[visp] 02/05: Imported Upstream version 3.0.0
Olivier Sallou
osallou at debian.org
Mon Jan 4 16:58:01 UTC 2016
This is an automated email from the git hooks/post-receive script.
osallou pushed a commit to branch master
in repository visp.
commit f95bb9f162200adcfc64176ae80a135509fbe091
Author: Fabien Spindler <Fabien.Spindler at inria.fr>
Date: Thu Dec 31 18:42:40 2015 +0100
Imported Upstream version 3.0.0
---
.gitignore | 5 +
.travis.yml | 56 +
CMakeHeaderFileList.cmake | 407 --
CMakeLists.txt | 3081 ++++----------
CMakeModules/AddExtraCompilationFlags.cmake | 166 -
CMakeModules/CPackConfig.cmake | 113 -
CMakeModules/CPackConfigDeb.cmake | 180 -
CMakeModules/CPackConfigNsis.cmake | 58 -
CMakeModules/CPackConfigRpm.cmake | 149 -
CMakeModules/FindARIA.cmake | 134 -
CMakeModules/FindBICLOPS.cmake | 151 -
CMakeModules/FindCALINUX.cmake | 91 -
CMakeModules/FindCMU1394.cmake | 109 -
CMakeModules/FindCPP11.cmake | 71 -
CMakeModules/FindDC1394.cmake | 141 -
CMakeModules/FindDIRECT3D.cmake | 146 -
CMakeModules/FindDIRECTSHOW.cmake | 283 --
CMakeModules/FindDL.cmake | 66 -
CMakeModules/FindDMTX.cmake | 71 -
CMakeModules/FindFFMPEG.cmake | 322 --
CMakeModules/FindGDI.cmake | 159 -
CMakeModules/FindGSL.cmake | 193 -
CMakeModules/FindICONV.cmake | 89 -
CMakeModules/FindIRISA.cmake | 91 -
CMakeModules/FindLAPACK_C.cmake | 165 -
CMakeModules/FindLIBFREENECT.cmake | 98 -
CMakeModules/FindLIBUSB-1.cmake | 86 -
CMakeModules/FindMyCoin3D.cmake | 231 --
CMakeModules/FindMyGTK2.cmake | 296 --
CMakeModules/FindMyJPEG.cmake | 130 -
CMakeModules/FindMyPNG.cmake | 192 -
CMakeModules/FindMyZLIB.cmake | 139 -
CMakeModules/FindNAS.cmake | 63 -
CMakeModules/FindPARPORT.cmake | 58 -
CMakeModules/FindPTHREAD.cmake | 104 -
CMakeModules/FindPTU46.cmake | 127 -
CMakeModules/FindRAW1394.cmake | 88 -
CMakeModules/FindRT.cmake | 73 -
CMakeModules/FindSOQT.cmake | 129 -
CMakeModules/FindSOWIN.cmake | 95 -
CMakeModules/FindSOXT.cmake | 79 -
CMakeModules/FindV4L2.cmake | 106 -
CMakeModules/FindXML2.cmake | 123 -
CMakeModules/FindZBAR.cmake | 71 -
CMakeModules/OgreTools.cmake | 174 -
CMakeModules/VISPCRTLinkage.cmake | 44 -
CMakeModules/VISPConfig.cmake | 149 -
CMakeModules/VISPConfig.cmake.in | 238 --
CMakeModules/VISPDetectPlatform.cmake | 41 -
CMakeModules/VISPExtraTargets.cmake | 74 -
CMakeModules/VISPGenerateConfig.cmake | 196 -
CMakeModules/VISPGeneratePkgConfigScript.cmake | 332 --
CMakeModules/VISPUse.cmake.in | 49 -
CMakeModules/cmake_uninstall.cmake.in | 64 -
CMakeModules/resources.cfg.in | 11 -
CMakeModules/visp-config.bat.in | 138 -
CMakeModules/visp-config.in | 99 -
CMakeModules/visp-config.install.in | 103 -
CMakeModules/visp.pc.in | 14 -
CMakeSourceFileList.cmake | 489 ---
CTestConfig.cmake | 238 +-
ChangeLog | 71 +
INSTALL.txt | 21 +-
README.md | 15 +
README.txt | 274 --
ViSP-third-party.txt.cmake | 60 -
cmake/AddExtraCompilationFlags.cmake | 201 +
cmake/CPackConfig.cmake | 110 +
cmake/CPackConfigDeb.cmake | 177 +
cmake/CPackConfigNsis.cmake | 55 +
cmake/CPackConfigRpm.cmake | 146 +
cmake/FindARIA.cmake | 133 +
cmake/FindBICLOPS.cmake | 148 +
cmake/FindCALINUX.cmake | 88 +
cmake/FindCMU1394.cmake | 106 +
cmake/FindCPP11.cmake | 68 +
cmake/FindDC1394.cmake | 138 +
cmake/FindDIRECT3D.cmake | 143 +
cmake/FindDIRECTSHOW.cmake | 280 ++
cmake/FindDL.cmake | 63 +
cmake/FindDMTX.cmake | 68 +
cmake/FindFFMPEG.cmake | 319 ++
cmake/FindGDI.cmake | 158 +
cmake/FindGSL.cmake | 180 +
cmake/FindICONV.cmake | 86 +
cmake/FindIRISA.cmake | 88 +
cmake/FindIsInf.cmake | 70 +
cmake/FindIsNaN.cmake | 70 +
cmake/FindLAPACK_C.cmake | 145 +
cmake/FindLIBFREENECT.cmake | 95 +
cmake/FindLIBUSB_1.cmake | 83 +
cmake/FindMyCoin3D.cmake | 228 +
cmake/FindMyGTK2.cmake | 293 ++
cmake/FindMyJPEG.cmake | 127 +
cmake/FindMyPNG.cmake | 189 +
cmake/FindMyZLIB.cmake | 130 +
cmake/FindNAS.cmake | 60 +
{CMakeModules => cmake}/FindOpenCV2.cmake | 0
cmake/FindPARPORT.cmake | 56 +
cmake/FindPTHREAD.cmake | 101 +
cmake/FindPTU46.cmake | 124 +
cmake/FindRAW1394.cmake | 85 +
cmake/FindRT.cmake | 70 +
cmake/FindRound.cmake | 65 +
cmake/FindSOQT.cmake | 126 +
cmake/FindSOWIN.cmake | 92 +
cmake/FindSOXT.cmake | 76 +
cmake/FindV4L2.cmake | 103 +
cmake/FindXML2.cmake | 122 +
cmake/FindZBAR.cmake | 68 +
{CMakeModules => cmake}/GNUInstallDirs.cmake | 0
cmake/OgreTools.cmake | 371 ++
cmake/VISPCRTLinkage.cmake | 44 +
cmake/VISPConfig.cmake | 146 +
cmake/VISPDetectPlatform.cmake | 79 +
cmake/VISPExtraTargets.cmake | 109 +
cmake/VISPGenerateConfig.cmake | 213 +
cmake/VISPGenerateHeaders.cmake | 77 +
cmake/VISPGeneratePkgConfigScript.cmake | 302 ++
cmake/VISPModule.cmake | 966 +++++
cmake/VISPUse.cmake.in | 46 +
cmake/VISPUtils.cmake | 457 ++
cmake/templates/VISPConfig.cmake.in | 320 ++
.../templates}/VISPConfigVersion.cmake.in | 0
cmake/templates/ViSP-third-party.txt.in | 59 +
cmake/templates/cmake_uninstall.cmake.in | 61 +
cmake/templates/resources.cfg.in | 7 +
cmake/templates/visp-config.bat.in | 134 +
cmake/templates/visp-config.in | 96 +
cmake/templates/visp-config.install.in | 100 +
cmake/templates/visp.pc.in | 14 +
cmake/templates/visp_modules.h.in | 8 +
cmake/templates/vpConfig.h.in | 411 ++
cmake/templates/vpHeader.h.in | 6 +
demo/CMakeLists.txt | 54 +-
demo/wireframe-simulator/CMakeLists.txt | 56 +-
demo/wireframe-simulator/servoSimu4Points.cpp | 63 +-
demo/wireframe-simulator/servoSimuCylinder.cpp | 69 +-
demo/wireframe-simulator/servoSimuSphere.cpp | 71 +-
doc/biblio/references.bib | 28 +
doc/config-doxygen.in | 71 +-
doc/image/img-ccmake-centos-all.png | Bin 78227 -> 54061 bytes
doc/image/img-ccmake-centos.png | Bin 65667 -> 0 bytes
doc/image/img-ccmake-opensuse-all.png | Bin 77995 -> 0 bytes
doc/image/img-ccmake-opensuse.png | Bin 64950 -> 0 bytes
doc/image/img-ccmake-raspberry-pi.png | Bin 91849 -> 0 bytes
doc/image/img-ccmake-ubuntu-all.png | Bin 106180 -> 186677 bytes
doc/image/img-ccmake-ubuntu.png | Bin 80266 -> 0 bytes
doc/image/img-cmake-win10-config-end.png | Bin 0 -> 52558 bytes
doc/image/img-cmake-win10-config.png | Bin 0 -> 48680 bytes
doc/image/img-cmake-win10-create-build-folder.png | Bin 0 -> 8509 bytes
doc/image/img-cmake-win10-msvc-generate.png | Bin 0 -> 51934 bytes
doc/image/img-cmake-win10-msvc-launch.png | Bin 0 -> 14031 bytes
doc/image/img-cmake-win10-msvc-version.png | Bin 0 -> 8252 bytes
doc/image/img-cmake-win7-create-build-folder.jpg | Bin 17691 -> 17902 bytes
doc/image/img-cmake-win7-generate.jpg | Bin 87045 -> 116340 bytes
doc/image/img-cmake-win7-msvc-config-end.jpg | Bin 76238 -> 120450 bytes
doc/image/img-cmake-win7-msvc-config.jpg | Bin 70074 -> 108107 bytes
doc/image/img-cmake-win7-msvc-launch.jpg | Bin 50796 -> 42966 bytes
doc/image/img-cmake-win7-msvc-version.jpg | Bin 31876 -> 31624 bytes
doc/image/img-cmake-win7-opencv-advanced.jpg | Bin 79725 -> 0 bytes
doc/image/img-cmake-win7-opencv.jpg | Bin 71487 -> 0 bytes
doc/image/img-cmake-win7-solution.jpg | Bin 52671 -> 35564 bytes
doc/image/img-cmake-win8.1-4.jpg | Bin 91594 -> 0 bytes
doc/image/img-cmake-win8.1-6.jpg | Bin 104457 -> 0 bytes
doc/image/img-cmake-win8.1-config-end.jpg | Bin 0 -> 129616 bytes
doc/image/img-cmake-win8.1-config.jpg | Bin 0 -> 107955 bytes
doc/image/img-cmake-win8.1-create-build-folder.jpg | Bin 18130 -> 16648 bytes
doc/image/img-cmake-win8.1-gdi.jpg | Bin 102805 -> 0 bytes
doc/image/img-cmake-win8.1-mingw-configure-end.jpg | Bin 69684 -> 129579 bytes
doc/image/img-cmake-win8.1-mingw-configure.jpg | Bin 63547 -> 108163 bytes
doc/image/img-cmake-win8.1-mingw-generate.jpg | Bin 91910 -> 129926 bytes
doc/image/img-cmake-win8.1-mingw-launch.jpg | Bin 40873 -> 0 bytes
...cmake-win8.1-mingw-opencv-detected-advanced.jpg | Bin 67753 -> 0 bytes
.../img-cmake-win8.1-mingw-opencv-detected.jpg | Bin 68353 -> 0 bytes
doc/image/img-cmake-win8.1-msvc-generate.jpg | Bin 81061 -> 129616 bytes
doc/image/img-cmake-win8.1-msvc-launch.jpg | Bin 41975 -> 40333 bytes
doc/image/img-cmake-win8.1-opencv-advanced.jpg | Bin 103997 -> 0 bytes
doc/image/img-cmake-win8.1-opencv.jpg | Bin 95124 -> 0 bytes
doc/image/img-logo-visp.png | Bin 0 -> 10741 bytes
doc/image/img-mingw64-installer-finished.jpg | Bin 54096 -> 0 bytes
doc/image/img-monkey-blured-default.png | Bin 0 -> 37902 bytes
doc/image/img-monkey-blured-var2.png | Bin 0 -> 29843 bytes
doc/image/img-monkey-canny.png | Bin 0 -> 11183 bytes
doc/image/img-monkey-dIxy.png | Bin 0 -> 99559 bytes
doc/image/img-monkey-gray.png | Bin 0 -> 46930 bytes
doc/image/img-monkey-pyr.png | Bin 0 -> 61023 bytes
doc/image/img-monkey-sobel.png | Bin 0 -> 46894 bytes
doc/image/img-monkey-win.jpg | Bin 0 -> 31617 bytes
doc/image/img-monkey.png | Bin 0 -> 55379 bytes
doc/image/img-opencv-issue-dshow.jpg | Bin 57556 -> 0 bytes
doc/image/img-started-win-run.jpg | Bin 23851 -> 23205 bytes
.../img-tracker-mb-visibility-ogre-advanced.png | Bin 0 -> 6141 bytes
doc/image/img-tracker-mb-visibility-ogre.png | Bin 0 -> 5337 bytes
doc/image/img-tracker-mb-visibility-scanline.png | Bin 0 -> 3241 bytes
doc/image/img-win10-cmake-error-sdk-missing.png | Bin 0 -> 34829 bytes
doc/image/img-win10-explorer-install-end.png | Bin 0 -> 44346 bytes
doc/image/img-win10-msvc-build-end.png | Bin 0 -> 48368 bytes
doc/image/img-win10-msvc-build.png | Bin 0 -> 144822 bytes
doc/image/img-win10-msvc-install-end.png | Bin 0 -> 44659 bytes
doc/image/img-win10-msvc-install.png | Bin 0 -> 250622 bytes
doc/image/img-win10-msvc-open.png | Bin 0 -> 27182 bytes
doc/image/img-win10-msvc-release.png | Bin 0 -> 372331 bytes
doc/image/img-win7-ViSP-images.jpg | Bin 35663 -> 27496 bytes
doc/image/img-win7-cmd-displayGDI.jpg | Bin 41868 -> 37151 bytes
doc/image/img-win7-cygwin-git.jpg | Bin 0 -> 65830 bytes
doc/image/img-win7-msvc-build-succeed.jpg | Bin 69012 -> 105132 bytes
doc/image/img-win7-msvc-build.jpg | Bin 68478 -> 89983 bytes
doc/image/img-win7-msvc-install-end.jpg | Bin 60953 -> 63294 bytes
doc/image/img-win7-msvc-install-succeed.jpg | Bin 68875 -> 115466 bytes
doc/image/img-win7-msvc-install.jpg | Bin 72183 -> 104028 bytes
doc/image/img-win7-msvc-open.jpg | Bin 56691 -> 53544 bytes
doc/image/img-win7-msvc-release.jpg | Bin 70813 -> 121226 bytes
doc/image/img-win8.1-cmake-ogre.jpg | Bin 0 -> 106809 bytes
doc/image/img-win8.1-cmake-ogredeps-issue.jpg | Bin 0 -> 86847 bytes
doc/image/img-win8.1-cmake-ogredeps.jpg | Bin 0 -> 117738 bytes
doc/image/img-win8.1-cmd-displayGDI.jpg | Bin 30915 -> 39227 bytes
doc/image/img-win8.1-cygwin-svn.jpg | Bin 55374 -> 0 bytes
doc/image/img-win8.1-explorer-install-end.jpg | Bin 30542 -> 72636 bytes
doc/image/img-win8.1-mingw-cmd-displayGDI.jpg | Bin 0 -> 30885 bytes
...win8.1-mingw-opencv-cmake-configure-install.jpg | Bin 73947 -> 82285 bytes
.../img-win8.1-mingw-opencv-cmake-configure.jpg | Bin 42180 -> 45901 bytes
doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg | Bin 0 -> 112644 bytes
doc/image/img-win8.1-msvc-build-end.jpg | Bin 77917 -> 108218 bytes
doc/image/img-win8.1-msvc-build-ogredeps.jpg | Bin 0 -> 26860 bytes
doc/image/img-win8.1-msvc-build.jpg | Bin 25908 -> 99119 bytes
doc/image/img-win8.1-msvc-install-end.jpg | Bin 80293 -> 109089 bytes
doc/image/img-win8.1-msvc-install.jpg | Bin 36139 -> 216804 bytes
doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg | Bin 0 -> 27406 bytes
.../img-win8.1-msvc-mbtracker-ogre-opengl.jpg | Bin 0 -> 45109 bytes
doc/image/img-win8.1-msvc-ogredeps-issue.jpg | Bin 0 -> 68389 bytes
doc/image/img-win8.1-msvc-open.jpg | Bin 51389 -> 57429 bytes
doc/image/img-win8.1-msvc-release.jpg | Bin 35398 -> 206534 bytes
doc/image/img-win8.1-msvc-solution.jpg | Bin 17582 -> 30423 bytes
doc/image/img-win8.1-ogre-explorer.jpg | Bin 0 -> 43758 bytes
doc/image/img-win8.1-ogre-tortoisehg.jpg | Bin 0 -> 48158 bytes
doc/image/img-win8.1-ogredeps-explorer.jpg | Bin 0 -> 20817 bytes
doc/image/img-win8.1-ogredeps-tortoisehg.jpg | Bin 0 -> 30825 bytes
doc/image/img-win8.1-visp-images.jpg | Bin 51522 -> 26536 bytes
doc/mainpage.doc.in | 591 +--
doc/tutorial-bridge-opencv.doc | 114 +
doc/tutorial-calibration.doc | 4 +-
doc/tutorial-detection-object.doc | 92 +-
doc/tutorial-getting-started-iOS.doc | 6 +-
doc/tutorial-getting-started.doc | 21 +-
doc/tutorial-grabber.doc | 3 +-
doc/tutorial-ibvs.doc | 17 +-
doc/tutorial-image-filtering.doc | 21 +-
doc/tutorial-install-centos.doc | 305 +-
doc/tutorial-install-fedora.doc | 334 +-
doc/tutorial-install-iOS.doc | 60 +-
doc/tutorial-install-opensuse.doc | 303 +-
doc/tutorial-install-osx-homebrew.doc | 365 ++
doc/tutorial-install-raspberry.doc | 255 +-
doc/tutorial-install-ubuntu-package.doc | 37 +-
doc/tutorial-install-ubuntu.doc | 333 +-
doc/tutorial-install-win10-msvc14.doc | 174 +
doc/tutorial-install-win7.doc | 158 +-
doc/tutorial-install-win8.1-mingw-w64.doc | 274 +-
doc/tutorial-install-win8.1-msvc.doc | 251 +-
doc/tutorial-plotter.doc | 2 +-
doc/tutorial-pose-estimation.doc | 4 +-
doc/tutorial-simu-robot-pioneer.doc | 1 +
doc/tutorial-trace.doc | 10 +-
doc/tutorial-tracking-blob.doc | 1 +
doc/tutorial-tracking-keypoint.doc | 5 +-
doc/tutorial-tracking-mb.doc | 681 +--
doc/tutorial-tracking-me.doc | 17 +-
doc/tutorial.doc | 129 +
example/CMakeLists.txt | 175 +-
example/calibration/CMakeLists.txt | 72 +-
example/calibration/calibrateTsai.cpp | 30 +-
example/calibration/camera_calibration.cpp | 40 +-
example/coin-simulator/CMakeLists.txt | 55 +-
.../coin-simulator/simulateCircle2DCamVelocity.cpp | 64 +-
.../simulateFourPoints2DCartesianCamVelocity.cpp | 65 +-
.../simulateFourPoints2DPolarCamVelocity.cpp | 65 +-
example/device/CMakeLists.txt | 54 -
example/device/display/CMakeLists.txt | 48 +-
example/device/display/displayD3D.cpp | 34 +-
example/device/display/displayGDI.cpp | 34 +-
example/device/display/displayGTK.cpp | 36 +-
example/device/display/displayOpenCV.cpp | 36 +-
example/device/display/displaySequence.cpp | 40 +-
example/device/display/displayX.cpp | 36 +-
example/device/display/displayXMulti.cpp | 34 +-
example/device/framegrabber/CMakeLists.txt | 48 +-
example/device/framegrabber/grab1394CMU.cpp | 36 +-
example/device/framegrabber/grab1394Two.cpp | 44 +-
example/device/framegrabber/grabDirectShow.cpp | 38 +-
.../device/framegrabber/grabDirectShowMulti.cpp | 37 +-
example/device/framegrabber/grabDisk.cpp | 40 +-
example/device/framegrabber/grabOpenCV-2.cpp | 32 +-
example/device/framegrabber/grabOpenCV.cpp | 36 +-
example/device/framegrabber/grabV4l2.cpp | 40 +-
example/device/kinect/CMakeLists.txt | 48 +-
example/device/kinect/kinectAcquisition.cpp | 30 +-
example/device/laserscanner/CMakeLists.txt | 48 +-
example/device/laserscanner/SickLDMRS-Acq.cpp | 26 +-
example/device/laserscanner/SickLDMRS-Process.cpp | 58 +-
example/device/light/CMakeLists.txt | 48 +-
example/device/light/ringLight.cpp | 30 +-
example/direct-visual-servoing/CMakeLists.txt | 48 +-
.../photometricVisualServoing.cpp | 72 +-
example/homography/CMakeLists.txt | 53 +-
example/homography/homographyHLM2DObject.cpp | 40 +-
example/homography/homographyHLM3DObject.cpp | 40 +-
.../homography/homographyHartleyDLT2DObject.cpp | 40 +-
example/homography/homographyRansac2DObject.cpp | 46 +-
example/image/CMakeLists.txt | 59 +-
example/image/imageDiskRW.cpp | 30 +-
example/key-point/CMakeLists.txt | 48 +-
example/key-point/fernClassifier.cpp | 48 +-
example/key-point/keyPointSurf.cpp | 42 +-
example/key-point/planarObjectDetector.cpp | 50 +-
example/manual/CMakeLists.txt | 140 +-
.../geometric-features/manGeometricFeatures.cpp | 38 +-
.../manual/hello-world/Autotools/HelloWorld.cpp | 6 +-
example/manual/hello-world/Autotools/have_visp.m4 | 19 +-
example/manual/hello-world/CMake/HelloWorld.cpp | 43 +-
example/manual/hello-world/Makefile/HelloWorld.cpp | 6 +-
example/manual/image-manipulation/manDisplay.cpp | 30 +-
.../manual/image-manipulation/manGrab1394-2.cpp | 26 +-
.../image-manipulation/manGrabDirectShow.cpp | 26 +-
example/manual/image-manipulation/manGrabDisk.cpp | 26 +-
.../manual/image-manipulation/manGrabOpenCV.cpp | 26 +-
example/manual/image-manipulation/manGrabV4l2.cpp | 24 +-
example/manual/moments/manServoMomentsSimple.cpp | 48 +-
example/manual/ogre/HelloWorldOgre.cpp | 39 +-
example/manual/ogre/HelloWorldOgreAdvanced.cpp | 39 +-
.../manual/simulation/manServo4PointsDisplay.cpp | 438 +-
example/manual/simulation/manSimu4Dots.cpp | 597 +--
example/manual/simulation/manSimu4Points.cpp | 498 +--
example/math/BSpline.cpp | 50 +-
example/math/CMakeLists.txt | 55 +-
example/math/Nurbs.cpp | 395 --
example/math/exponentialMap.cpp | 34 +-
example/moments/image/CMakeLists.txt | 42 +-
example/moments/image/servoMomentImage.cpp | 85 +-
example/moments/points/CMakeLists.txt | 42 +-
example/moments/points/servoMomentPoints.cpp | 63 +-
example/moments/polygon/CMakeLists.txt | 42 +-
example/moments/polygon/servoMomentPolygon.cpp | 63 +-
example/ogre-simulator/AROgre.cpp | 56 +-
example/ogre-simulator/AROgreBasic.cpp | 56 +-
example/ogre-simulator/CMakeLists.txt | 50 +-
example/parse-argv/CMakeLists.txt | 53 +-
example/parse-argv/parse-argv1.cpp | 24 +-
example/parse-argv/parse-argv2.cpp | 24 +-
example/pose-estimation/CMakeLists.txt | 60 +-
example/pose-estimation/poseVirtualVS.cpp | 48 +-
example/robot-simulator/afma6/CMakeLists.txt | 55 +-
.../servoSimuAfma6FourPoints2DCamVelocity.cpp | 54 +-
example/robot-simulator/camera/CMakeLists.txt | 48 +-
.../camera/servoSimu3D_cMcd_CamVelocity.cpp | 36 +-
.../servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp | 38 +-
.../camera/servoSimu3D_cdMc_CamVelocity.cpp | 36 +-
.../servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp | 34 +-
.../camera/servoSimuCircle2DCamVelocity.cpp | 37 +-
.../camera/servoSimuCircle2DCamVelocityDisplay.cpp | 56 +-
.../servoSimuCylinder2DCamVelocityDisplay.cpp | 55 +-
...muCylinder2DCamVelocityDisplaySecondaryTask.cpp | 54 +-
.../camera/servoSimuFourPoints2DCamVelocity.cpp | 36 +-
.../servoSimuFourPoints2DCamVelocityDisplay.cpp | 52 +-
...ervoSimuFourPoints2DPolarCamVelocityDisplay.cpp | 61 +-
.../camera/servoSimuLine2DCamVelocityDisplay.cpp | 55 +-
.../camera/servoSimuPoint2DCamVelocity1.cpp | 57 +-
.../camera/servoSimuPoint2DCamVelocity2.cpp | 57 +-
.../camera/servoSimuPoint2DCamVelocity3.cpp | 59 +-
.../camera/servoSimuPoint2DhalfCamVelocity1.cpp | 60 +-
.../camera/servoSimuPoint2DhalfCamVelocity2.cpp | 52 +-
.../camera/servoSimuPoint2DhalfCamVelocity3.cpp | 48 +-
.../camera/servoSimuPoint3DCamVelocity.cpp | 37 +-
.../camera/servoSimuSphere2DCamVelocity.cpp | 37 +-
.../servoSimuSphere2DCamVelocitySecondaryTask.cpp | 37 +-
.../servoSimuSquareLine2DCamVelocityDisplay.cpp | 56 +-
.../camera/servoSimuThetaUCamVelocity.cpp | 34 +-
example/robot-simulator/viper850/CMakeLists.txt | 55 +-
.../servoSimuViper850FourPoints2DCamVelocity.cpp | 54 +-
example/servo-afma4/CMakeLists.txt | 48 +-
example/servo-afma4/moveAfma4.cpp | 28 +-
.../servo-afma4/servoAfma4Point2DArtVelocity.cpp | 67 +-
.../servo-afma4/servoAfma4Point2DCamVelocity.cpp | 67 +-
.../servoAfma4Point2DCamVelocityKalman.cpp | 71 +-
example/servo-afma6/CMakeLists.txt | 48 +-
.../servo-afma6/servoAfma62DhalfCamVelocity.cpp | 81 +-
.../servoAfma6Cylinder2DCamVelocity.cpp | 67 +-
...ervoAfma6Cylinder2DCamVelocitySecondaryTask.cpp | 67 +-
.../servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp | 57 +-
.../servoAfma6FourPoints2DArtVelocity.cpp | 67 +-
...a6FourPoints2DCamVelocityInteractionCurrent.cpp | 75 +-
...a6FourPoints2DCamVelocityInteractionDesired.cpp | 73 +-
.../servo-afma6/servoAfma6Line2DCamVelocity.cpp | 59 +-
.../servo-afma6/servoAfma6Point2DArtVelocity.cpp | 67 +-
.../servo-afma6/servoAfma6Point2DCamVelocity.cpp | 65 +-
.../servoAfma6Points2DCamVelocityEyeToHand.cpp | 65 +-
.../servo-afma6/servoAfma6Segment2DCamVelocity.cpp | 65 +-
.../servoAfma6SquareLines2DCamVelocity.cpp | 69 +-
.../servoAfma6TwoLines2DCamVelocity.cpp | 67 +-
example/servo-biclops/CMakeLists.txt | 48 +-
example/servo-biclops/moveBiclops.cpp | 42 +-
.../servoBiclopsPoint2DArtVelocity.cpp | 84 +-
example/servo-pioneer/CMakeLists.txt | 49 +-
example/servo-pioneer/movePioneer.cpp | 20 +-
example/servo-pioneer/servoPioneerPanSegment3D.cpp | 56 +-
example/servo-pioneer/servoPioneerPoint2DDepth.cpp | 58 +-
.../servoPioneerPoint2DDepthWithoutVpServo.cpp | 62 +-
example/servo-pioneer/sonarPioneerReader.cpp | 52 +-
example/servo-ptu46/CMakeLists.txt | 48 +-
example/servo-ptu46/movePtu46.cpp | 26 +-
.../servo-ptu46/servoPtu46Point2DArtVelocity.cpp | 64 +-
example/servo-viper650/CMakeLists.txt | 49 +-
...50FourPoints2DArtVelocityInteractionCurrent.cpp | 62 +-
...50FourPoints2DCamVelocityInteractionCurrent.cpp | 62 +-
.../servoViper650Point2DCamVelocity.cpp | 61 +-
example/servo-viper850/CMakeLists.txt | 48 +-
...50FourPoints2DArtVelocityInteractionCurrent.cpp | 62 +-
...50FourPoints2DArtVelocityInteractionDesired.cpp | 62 +-
...50FourPoints2DCamVelocityInteractionCurrent.cpp | 62 +-
.../servoViper850FourPointsKinect.cpp | 60 +-
...r850Point2DArtVelocity-jointAvoidance-basic.cpp | 65 +-
...per850Point2DArtVelocity-jointAvoidance-gpa.cpp | 73 +-
...r850Point2DArtVelocity-jointAvoidance-large.cpp | 313 ++
.../servoViper850Point2DArtVelocity.cpp | 63 +-
.../servoViper850Point2DCamVelocity.cpp | 65 +-
.../servoViper850Point2DCamVelocityKalman.cpp | 69 +-
example/tools/CMakeLists.txt | 48 +-
example/tools/histogram.cpp | 32 +-
example/tools/keyboard.cpp | 26 +-
example/tools/parallelPort.cpp | 28 +-
example/tools/plot2d.cpp | 28 +-
example/tools/plot3d.cpp | 24 +-
example/tracking/CMakeLists.txt | 67 +-
example/tracking/mbtEdgeKltTracking.cpp | 115 +-
example/tracking/mbtEdgeTracking.cpp | 120 +-
example/tracking/mbtKltTracking.cpp | 112 +-
example/tracking/templateTracker.cpp | 179 +-
example/tracking/trackDot.cpp | 53 +-
example/tracking/trackDot2.cpp | 53 +-
example/tracking/trackDot2WithAutoDetection.cpp | 60 +-
example/tracking/trackKltOpencv.cpp | 52 +-
example/tracking/trackMeCircle.cpp | 70 +-
example/tracking/trackMeEllipse.cpp | 72 +-
example/tracking/trackMeLine.cpp | 59 +-
example/tracking/trackMeNurbs.cpp | 61 +-
example/video/CMakeLists.txt | 61 +-
example/video/imageSequenceReader.cpp | 40 +-
example/video/videoReader.cpp | 42 +-
example/wireframe-simulator/CMakeLists.txt | 59 +-
example/wireframe-simulator/wireframeSimulator.cpp | 46 +-
include/vpConfig.h.cmake | 368 --
macros/have_visp.m4 | 19 +-
modules/CMakeLists.txt | 43 +
modules/ar/CMakeLists.txt | 183 +
.../media/materials/scripts/Examples.material | 0
.../media/materials/textures/grass_1024.jpg | Bin
.../media/materials/textures/r2skin.jpg | Bin
.../data}/ogre-simulator/media/models/robot.mesh | Bin
.../ogre-simulator/media/models/robot.skeleton | Bin
modules/ar/include/visp3/ar/vpAR.h | 160 +
modules/ar/include/visp3/ar/vpAROgre.h | 392 ++
modules/ar/include/visp3/ar/vpSimulator.h | 305 ++
modules/ar/include/visp3/ar/vpSimulatorException.h | 89 +
modules/ar/include/visp3/ar/vpViewer.h | 166 +
modules/ar/src/coin-simulator/vpAR.cpp | 185 +
modules/ar/src/coin-simulator/vpSimulator.cpp | 1099 +++++
modules/ar/src/coin-simulator/vpViewer.cpp | 265 ++
modules/ar/src/ogre-simulator/vpAROgre.cpp | 1097 +++++
modules/ar/src/vpSimulatorException.cpp | 58 +
modules/core/CMakeLists.txt | 251 ++
modules/core/include/visp3/core/vpArray2D.h | 652 +++
modules/core/include/visp3/core/vpBSpline.h | 219 +
.../core/include/visp3/core/vpCameraParameters.h | 346 ++
modules/core/include/visp3/core/vpCircle.h | 118 +
modules/core/include/visp3/core/vpClient.h | 212 +
modules/core/include/visp3/core/vpColVector.h | 320 ++
modules/core/include/visp3/core/vpColor.h | 319 ++
modules/core/include/visp3/core/vpConvert.h | 112 +
modules/core/include/visp3/core/vpCylinder.h | 195 +
modules/core/include/visp3/core/vpDebug.h | 550 +++
modules/core/include/visp3/core/vpDisplay.h | 983 +++++
.../core/include/visp3/core/vpDisplayException.h | 108 +
modules/core/include/visp3/core/vpException.h | 133 +
modules/core/include/visp3/core/vpExponentialMap.h | 96 +
modules/core/include/visp3/core/vpFeatureDisplay.h | 116 +
.../core/include/visp3/core/vpForceTwistMatrix.h | 174 +
.../core/include/visp3/core/vpForwardProjection.h | 237 ++
modules/core/include/visp3/core/vpFrameGrabber.h | 136 +
.../include/visp3/core/vpFrameGrabberException.h | 95 +
modules/core/include/visp3/core/vpGDIRenderer.h | 146 +
modules/core/include/visp3/core/vpGEMM.h | 394 ++
modules/core/include/visp3/core/vpGaussRand.h | 171 +
modules/core/include/visp3/core/vpHinkley.h | 168 +
modules/core/include/visp3/core/vpHistogram.h | 323 ++
modules/core/include/visp3/core/vpHistogramPeak.h | 170 +
modules/core/include/visp3/core/vpHistogramValey.h | 167 +
.../core/include/visp3/core/vpHomogeneousMatrix.h | 202 +
modules/core/include/visp3/core/vpImage.h | 1595 +++++++
modules/core/include/visp3/core/vpImageConvert.h | 328 ++
modules/core/include/visp3/core/vpImageException.h | 99 +
modules/core/include/visp3/core/vpImageFilter.h | 445 ++
.../core/include/visp3/core/vpImageMorphology.h | 225 +
modules/core/include/visp3/core/vpImagePoint.h | 308 ++
modules/core/include/visp3/core/vpImageTools.h | 654 +++
modules/core/include/visp3/core/vpIoException.h | 99 +
modules/core/include/visp3/core/vpIoTools.h | 237 ++
modules/core/include/visp3/core/vpKalmanFilter.h | 222 +
modules/core/include/visp3/core/vpLine.h | 215 +
.../visp3/core/vpLinearKalmanFilterInstantiation.h | 176 +
modules/core/include/visp3/core/vpList.h | 844 ++++
modules/core/include/visp3/core/vpMath.h | 472 +++
modules/core/include/visp3/core/vpMatrix.h | 633 +++
.../core/include/visp3/core/vpMatrixException.h | 107 +
.../include/visp3/core/vpMeterPixelConversion.h | 243 ++
modules/core/include/visp3/core/vpMoment.h | 156 +
modules/core/include/visp3/core/vpMomentAlpha.h | 235 ++
modules/core/include/visp3/core/vpMomentArea.h | 65 +
.../include/visp3/core/vpMomentAreaNormalized.h | 152 +
modules/core/include/visp3/core/vpMomentBasic.h | 84 +
.../core/include/visp3/core/vpMomentCInvariant.h | 270 ++
modules/core/include/visp3/core/vpMomentCentered.h | 132 +
modules/core/include/visp3/core/vpMomentCommon.h | 130 +
modules/core/include/visp3/core/vpMomentDatabase.h | 151 +
.../include/visp3/core/vpMomentGravityCenter.h | 133 +
.../visp3/core/vpMomentGravityCenterNormalized.h | 72 +
modules/core/include/visp3/core/vpMomentObject.h | 294 ++
modules/core/include/visp3/core/vpMouseButton.h | 66 +
modules/core/include/visp3/core/vpMutex.h | 110 +
modules/core/include/visp3/core/vpNetwork.h | 495 +++
modules/core/include/visp3/core/vpNoise.h | 44 +
.../include/visp3/core/vpPixelMeterConversion.h | 251 ++
modules/core/include/visp3/core/vpPlane.h | 168 +
modules/core/include/visp3/core/vpPoint.h | 140 +
modules/core/include/visp3/core/vpPolygon.h | 188 +
modules/core/include/visp3/core/vpPolygon3D.h | 216 +
modules/core/include/visp3/core/vpPoseVector.h | 220 +
.../core/include/visp3/core/vpQuaternionVector.h | 118 +
modules/core/include/visp3/core/vpRGBa.h | 157 +
modules/core/include/visp3/core/vpRansac.h | 255 ++
modules/core/include/visp3/core/vpRect.h | 331 ++
modules/core/include/visp3/core/vpRequest.h | 237 ++
modules/core/include/visp3/core/vpRobust.h | 206 +
modules/core/include/visp3/core/vpRotationMatrix.h | 167 +
modules/core/include/visp3/core/vpRotationVector.h | 161 +
modules/core/include/visp3/core/vpRowVector.h | 271 ++
modules/core/include/visp3/core/vpRxyzVector.h | 181 +
modules/core/include/visp3/core/vpRzyxVector.h | 179 +
modules/core/include/visp3/core/vpRzyzVector.h | 180 +
modules/core/include/visp3/core/vpScale.h | 97 +
modules/core/include/visp3/core/vpServer.h | 221 +
modules/core/include/visp3/core/vpSphere.h | 110 +
modules/core/include/visp3/core/vpSubColVector.h | 96 +
modules/core/include/visp3/core/vpSubMatrix.h | 102 +
modules/core/include/visp3/core/vpSubRowVector.h | 93 +
modules/core/include/visp3/core/vpThetaUVector.h | 197 +
modules/core/include/visp3/core/vpTime.h | 88 +
modules/core/include/visp3/core/vpTracker.h | 106 +
.../core/include/visp3/core/vpTrackingException.h | 98 +
.../core/include/visp3/core/vpTranslationVector.h | 166 +
modules/core/include/visp3/core/vpTriangle.h | 113 +
modules/core/include/visp3/core/vpUniRand.h | 93 +
.../include/visp3/core/vpVelocityTwistMatrix.h | 184 +
modules/core/include/visp3/core/vpXmlParser.h | 307 ++
.../core/include/visp3/core/vpXmlParserCamera.h | 355 ++
.../visp3/core/vpXmlParserHomogeneousMatrix.h | 258 ++
modules/core/src/camera/vpCameraParameters.cpp | 596 +++
modules/core/src/camera/vpMeterPixelConversion.cpp | 112 +
modules/core/src/camera/vpPixelMeterConversion.cpp | 127 +
modules/core/src/camera/vpXmlParserCamera.cpp | 1100 +++++
modules/core/src/display/vpColor.cpp | 144 +
modules/core/src/display/vpDisplay.cpp | 3961 ++++++++++++++++++
modules/core/src/display/vpFeatureDisplay.cpp | 425 ++
modules/core/src/image/vpImageConvert.cpp | 3777 +++++++++++++++++
modules/core/src/image/vpImageFilter.cpp | 707 ++++
modules/core/src/image/vpImagePoint.cpp | 386 ++
modules/core/src/image/vpImageTools.cpp | 198 +
modules/core/src/image/vpRGBa.cpp | 255 ++
modules/core/src/math/kalman/vpKalmanFilter.cpp | 463 +++
.../kalman/vpLinearKalmanFilterInstantiation.cpp | 854 ++++
modules/core/src/math/matrix/vpColVector.cpp | 1274 ++++++
modules/core/src/math/matrix/vpMatrix.cpp | 3575 ++++++++++++++++
modules/core/src/math/matrix/vpMatrix_cholesky.cpp | 128 +
.../core/src/math/matrix/vpMatrix_covariance.cpp | 215 +
modules/core/src/math/matrix/vpMatrix_lu.cpp | 278 ++
modules/core/src/math/matrix/vpMatrix_qr.cpp | 238 ++
modules/core/src/math/matrix/vpMatrix_svd.cpp | 713 ++++
modules/core/src/math/matrix/vpRowVector.cpp | 1015 +++++
modules/core/src/math/matrix/vpSubColVector.cpp | 244 ++
modules/core/src/math/matrix/vpSubMatrix.cpp | 178 +
modules/core/src/math/matrix/vpSubRowVector.cpp | 186 +
modules/core/src/math/misc/vpHinkley.cpp | 446 ++
modules/core/src/math/misc/vpMath.cpp | 262 ++
.../core/src/math/random-generator/vpGaussRand.cpp | 73 +
.../core/src/math/random-generator/vpUniRand.cpp | 97 +
modules/core/src/math/robust/vpRobust.cpp | 1011 +++++
modules/core/src/math/robust/vpScale.cpp | 302 ++
modules/core/src/math/spline/vpBSpline.cpp | 529 +++
.../src/math/transformation/vpExponentialMap.cpp | 317 ++
.../src/math/transformation/vpForceTwistMatrix.cpp | 513 +++
.../math/transformation/vpHomogeneousMatrix.cpp | 862 ++++
.../core/src/math/transformation/vpPoseVector.cpp | 536 +++
.../src/math/transformation/vpQuaternionVector.cpp | 268 ++
.../src/math/transformation/vpRotationMatrix.cpp | 756 ++++
.../src/math/transformation/vpRotationVector.cpp | 112 +
.../core/src/math/transformation/vpRxyzVector.cpp | 177 +
.../core/src/math/transformation/vpRzyxVector.cpp | 187 +
.../core/src/math/transformation/vpRzyzVector.cpp | 178 +
.../src/math/transformation/vpThetaUVector.cpp | 320 ++
.../math/transformation/vpTranslationVector.cpp | 543 +++
.../math/transformation/vpVelocityTwistMatrix.cpp | 530 +++
.../vpXmlParserHomogeneousMatrix.cpp | 607 +++
modules/core/src/tools/convert/vpConvert.cpp | 434 ++
modules/core/src/tools/exceptions/vpException.cpp | 146 +
modules/core/src/tools/file/vpIoTools.cpp | 1495 +++++++
modules/core/src/tools/geometry/vpPlane.cpp | 392 ++
modules/core/src/tools/geometry/vpPolygon.cpp | 469 +++
modules/core/src/tools/geometry/vpPolygon3D.cpp | 650 +++
modules/core/src/tools/geometry/vpRect.cpp | 252 ++
modules/core/src/tools/geometry/vpTriangle.cpp | 179 +
modules/core/src/tools/histogram/vpHistogram.cpp | 976 +++++
.../core/src/tools/histogram/vpHistogramPeak.cpp | 115 +
.../core/src/tools/histogram/vpHistogramValey.cpp | 95 +
.../core/src/tools}/network/CMakeLists.txt | 0
modules/core/src/tools/network/vpClient.cpp | 223 +
modules/core/src/tools/network/vpNetwork.cpp | 789 ++++
modules/core/src/tools/network/vpRequest.cpp | 84 +
modules/core/src/tools/network/vpServer.cpp | 281 ++
modules/core/src/tools/time/vpTime.cpp | 247 ++
modules/core/src/tools/xml/vpXmlParser.cpp | 411 ++
.../src/tracking/forward-projection/vpCircle.cpp | 410 ++
.../src/tracking/forward-projection/vpCylinder.cpp | 425 ++
.../forward-projection/vpForwardProjection.cpp | 142 +
.../src/tracking/forward-projection/vpLine.cpp | 568 +++
.../src/tracking/forward-projection/vpPoint.cpp | 500 +++
.../src/tracking/forward-projection/vpSphere.cpp | 230 ++
modules/core/src/tracking/moments/vpMoment.cpp | 139 +
.../core/src/tracking/moments/vpMomentAlpha.cpp | 168 +
modules/core/src/tracking/moments/vpMomentArea.cpp | 100 +
.../tracking/moments/vpMomentAreaNormalized.cpp | 112 +
.../core/src/tracking/moments/vpMomentBasic.cpp | 94 +
.../src/tracking/moments/vpMomentCInvariant.cpp | 274 ++
.../core/src/tracking/moments/vpMomentCentered.cpp | 180 +
.../core/src/tracking/moments/vpMomentCommon.cpp | 213 +
.../core/src/tracking/moments/vpMomentDatabase.cpp | 95 +
.../src/tracking/moments/vpMomentGravityCenter.cpp | 81 +
.../moments/vpMomentGravityCenterNormalized.cpp | 96 +
.../core/src/tracking/moments/vpMomentObject.cpp | 656 +++
modules/core/src/tracking/vpTracker.cpp | 80 +
.../test/camera/testCameraParametersConversion.cpp | 112 +
modules/core/test/image/testConversion.cpp | 668 +++
modules/core/test/image/testCreateSubImage.cpp | 267 ++
modules/core/test/image/testImageBinarise.cpp | 179 +
modules/core/test/image/testImagePoint.cpp | 104 +
modules/core/test/image/testIoPGM.cpp | 273 ++
modules/core/test/image/testIoPPM.cpp | 314 ++
modules/core/test/image/testPerformanceLUT.cpp | 351 ++
modules/core/test/image/testReadImage.cpp | 241 ++
modules/core/test/image/testUndistortImage.cpp | 291 ++
modules/core/test/math/testArray2D.cpp | 163 +
modules/core/test/math/testColVector.cpp | 292 ++
modules/core/test/math/testKalmanAcceleration.cpp | 118 +
modules/core/test/math/testKalmanVelocity.cpp | 138 +
modules/core/test/math/testMath.cpp | 534 +++
modules/core/test/math/testMatrix.cpp | 280 ++
modules/core/test/math/testMatrixException.cpp | 73 +
modules/core/test/math/testMatrixInverse.cpp | 324 ++
modules/core/test/math/testPoseVector.cpp | 130 +
modules/core/test/math/testQuaternion.cpp | 155 +
modules/core/test/math/testRobust.cpp | 219 +
modules/core/test/math/testRotation.cpp | 359 ++
modules/core/test/math/testRowVector.cpp | 286 ++
modules/core/test/math/testSvd.cpp | 202 +
modules/core/test/math/testTranslationVector.cpp | 135 +
modules/core/test/math/testTwistMatrix.cpp | 115 +
modules/core/test/network/testClient.cpp | 77 +
modules/core/test/network/testServer.cpp | 80 +
modules/core/test/tools/convert/testConvert.cpp | 321 ++
modules/core/test/tools/geometry/testPolygon.cpp | 251 ++
modules/core/test/tools/io/testIoTools.cpp | 442 ++
modules/core/test/tools/time/testTime.cpp | 123 +
modules/core/test/tools/xml/testXmlParser.cpp | 388 ++
modules/detection/CMakeLists.txt | 55 +
.../include/visp3/detection/vpDetectorBase.h | 123 +
.../visp3/detection/vpDetectorDataMatrixCode.h | 119 +
.../include/visp3/detection/vpDetectorFace.h | 99 +
.../include/visp3/detection/vpDetectorQRCode.h | 123 +
.../src/barcode/vpDetectorDataMatrixCode.cpp | 127 +
modules/detection/src/barcode/vpDetectorQRCode.cpp | 98 +
modules/detection/src/face/vpDetectorFace.cpp | 130 +
modules/detection/src/vpDetectorBase.cpp | 107 +
modules/gui/CMakeLists.txt | 68 +
modules/gui/include/visp3/gui/vpD3DRenderer.h | 207 +
modules/gui/include/visp3/gui/vpDisplayD3D.h | 117 +
modules/gui/include/visp3/gui/vpDisplayGDI.h | 141 +
modules/gui/include/visp3/gui/vpDisplayGTK.h | 284 ++
modules/gui/include/visp3/gui/vpDisplayOpenCV.h | 304 ++
modules/gui/include/visp3/gui/vpDisplayWin32.h | 221 +
modules/gui/include/visp3/gui/vpDisplayX.h | 288 ++
modules/gui/include/visp3/gui/vpPlot.h | 201 +
modules/gui/include/visp3/gui/vpPlotCurve.h | 94 +
modules/gui/include/visp3/gui/vpPlotGraph.h | 217 +
.../gui/include/visp3/gui/vpProjectionDisplay.h | 148 +
modules/gui/include/visp3/gui/vpWin32API.h | 61 +
modules/gui/include/visp3/gui/vpWin32Renderer.h | 183 +
modules/gui/include/visp3/gui/vpWin32Window.h | 144 +
modules/gui/src/display/vpDisplayGTK.cpp | 1715 ++++++++
modules/gui/src/display/vpDisplayOpenCV.cpp | 2214 ++++++++++
modules/gui/src/display/vpDisplayX.cpp | 3402 +++++++++++++++
modules/gui/src/display/windows/vpD3DRenderer.cpp | 1216 ++++++
modules/gui/src/display/windows/vpDisplayD3D.cpp | 119 +
modules/gui/src/display/windows/vpDisplayGDI.cpp | 119 +
modules/gui/src/display/windows/vpDisplayWin32.cpp | 943 +++++
modules/gui/src/display/windows/vpGDIRenderer.cpp | 1163 ++++++
modules/gui/src/display/windows/vpWin32API.cpp | 140 +
modules/gui/src/display/windows/vpWin32Window.cpp | 336 ++
.../src/forward-projection/vpProjectionDisplay.cpp | 175 +
modules/gui/src/plot/vpPlot.cpp | 709 ++++
modules/gui/src/plot/vpPlotCurve.cpp | 116 +
modules/gui/src/plot/vpPlotGraph.cpp | 1424 +++++++
modules/gui/test/display/testClick.cpp | 439 ++
modules/gui/test/display/testDisplayRoi.cpp | 205 +
modules/gui/test/display/testDisplays.cpp | 404 ++
modules/gui/test/display/testMouseEvent.cpp | 589 +++
modules/gui/test/display/testVideoDevice.cpp | 481 +++
modules/gui/test/display/testVideoDeviceDual.cpp | 356 ++
modules/io/CMakeLists.txt | 68 +
modules/io/include/visp3/io/vpDiskGrabber.h | 156 +
modules/io/include/visp3/io/vpFFMPEG.h | 268 ++
modules/io/include/visp3/io/vpImageIo.h | 252 ++
modules/io/include/visp3/io/vpKeyboard.h | 104 +
modules/io/include/visp3/io/vpParallelPort.h | 103 +
.../io/include/visp3/io/vpParallelPortException.h | 103 +
modules/io/include/visp3/io/vpParseArgv.h | 194 +
modules/io/include/visp3/io/vpVideoReader.h | 323 ++
modules/io/include/visp3/io/vpVideoWriter.h | 294 ++
modules/io/src/image/vpImageIo.cpp | 3273 +++++++++++++++
modules/io/src/parallel-port/vpParallelPort.cpp | 185 +
modules/io/src/tools/vpKeyboard.cpp | 149 +
modules/io/src/tools/vpParseArgv.cpp | 576 +++
modules/io/src/video/vpDiskGrabber.cpp | 412 ++
modules/io/src/video/vpFFMPEG.cpp | 934 +++++
modules/io/src/video/vpVideoReader.cpp | 777 ++++
modules/io/src/video/vpVideoWriter.cpp | 431 ++
modules/robot/CMakeLists.txt | 102 +
modules/robot/include/visp3/robot/vpAfma4.h | 162 +
modules/robot/include/visp3/robot/vpAfma6.h | 205 +
modules/robot/include/visp3/robot/vpArit.h | 210 +
modules/robot/include/visp3/robot/vpBiclops.h | 197 +
modules/robot/include/visp3/robot/vpBound.h | 178 +
modules/robot/include/visp3/robot/vpCgiconstants.h | 185 +
.../robot/include/visp3/robot/vpImageSimulator.h | 313 ++
modules/robot/include/visp3/robot/vpImstack.h | 54 +
modules/robot/include/visp3/robot/vpKeyword.h | 50 +
modules/robot/include/visp3/robot/vpLex.h | 58 +
modules/robot/include/visp3/robot/vpMy.h | 90 +
modules/robot/include/visp3/robot/vpPioneer.h | 155 +
modules/robot/include/visp3/robot/vpPioneerPan.h | 229 ++
modules/robot/include/visp3/robot/vpPtu46.h | 118 +
modules/robot/include/visp3/robot/vpRfstack.h | 57 +
modules/robot/include/visp3/robot/vpRingLight.h | 131 +
modules/robot/include/visp3/robot/vpRobot.h | 163 +
modules/robot/include/visp3/robot/vpRobotAfma4.h | 292 ++
modules/robot/include/visp3/robot/vpRobotAfma6.h | 353 ++
modules/robot/include/visp3/robot/vpRobotBiclops.h | 183 +
.../include/visp3/robot/vpRobotBiclopsController.h | 165 +
modules/robot/include/visp3/robot/vpRobotCamera.h | 138 +
.../robot/include/visp3/robot/vpRobotException.h | 143 +
modules/robot/include/visp3/robot/vpRobotPioneer.h | 149 +
modules/robot/include/visp3/robot/vpRobotPtu46.h | 152 +
.../robot/include/visp3/robot/vpRobotSimulator.h | 98 +
.../robot/include/visp3/robot/vpRobotTemplate.h | 104 +
.../robot/include/visp3/robot/vpRobotViper650.h | 429 ++
.../robot/include/visp3/robot/vpRobotViper850.h | 436 ++
.../visp3/robot/vpRobotWireFrameSimulator.h | 473 +++
modules/robot/include/visp3/robot/vpServolens.h | 149 +
.../robot/include/visp3/robot/vpSimulatorAfma6.h | 282 ++
.../robot/include/visp3/robot/vpSimulatorCamera.h | 135 +
.../robot/include/visp3/robot/vpSimulatorPioneer.h | 138 +
.../include/visp3/robot/vpSimulatorPioneerPan.h | 140 +
.../include/visp3/robot/vpSimulatorViper850.h | 310 ++
modules/robot/include/visp3/robot/vpSkipio.h | 51 +
modules/robot/include/visp3/robot/vpTmstack.h | 62 +
modules/robot/include/visp3/robot/vpToken.h | 96 +
modules/robot/include/visp3/robot/vpUnicycle.h | 142 +
modules/robot/include/visp3/robot/vpView.h | 157 +
modules/robot/include/visp3/robot/vpViper.h | 164 +
modules/robot/include/visp3/robot/vpViper650.h | 148 +
modules/robot/include/visp3/robot/vpViper850.h | 151 +
modules/robot/include/visp3/robot/vpVwstack.h | 56 +
.../include/visp3/robot/vpWireFrameSimulator.h | 570 +++
.../robot/src/image-simulator/vpImageSimulator.cpp | 1651 ++++++++
modules/robot/src/light/vpRingLight.cpp | 228 +
modules/robot/src/real-robot/afma4/vpAfma4.cpp | 646 +++
.../robot/src/real-robot/afma4/vpRobotAfma4.cpp | 1860 +++++++++
modules/robot/src/real-robot/afma4/vpServolens.cpp | 895 ++++
modules/robot/src/real-robot/afma6/vpAfma6.cpp | 1581 +++++++
.../robot/src/real-robot/afma6/vpRobotAfma6.cpp | 2240 ++++++++++
modules/robot/src/real-robot/biclops/vpBiclops.cpp | 451 ++
.../src/real-robot/biclops/vpRobotBiclops.cpp | 1423 +++++++
.../biclops/vpRobotBiclopsController.cpp | 487 +++
.../src/real-robot/pioneer/vpRobotPioneer.cpp | 239 ++
modules/robot/src/real-robot/ptu46/vpPtu46.cpp | 321 ++
.../robot/src/real-robot/ptu46/vpRobotPtu46.cpp | 930 +++++
.../robot/src/real-robot/viper/vpRobotViper650.cpp | 2535 ++++++++++++
.../robot/src/real-robot/viper/vpRobotViper850.cpp | 2399 +++++++++++
modules/robot/src/real-robot/viper/vpViper.cpp | 1334 ++++++
modules/robot/src/real-robot/viper/vpViper650.cpp | 967 +++++
modules/robot/src/real-robot/viper/vpViper850.cpp | 881 ++++
.../robot/src/robot-simulator}/arms/afma6_arm1.bnd | 0
.../robot/src/robot-simulator}/arms/afma6_arm2.bnd | 0
.../robot/src/robot-simulator}/arms/afma6_arm3.bnd | 0
.../robot/src/robot-simulator}/arms/afma6_arm4.bnd | 0
.../robot/src/robot-simulator}/arms/afma6_gate.bnd | 0
.../src/robot-simulator}/arms/afma6_tool_ccmop.bnd | 0
.../robot-simulator}/arms/afma6_tool_gripper.bnd | 0
.../robot-simulator}/arms/afma6_tool_vacuum.bnd | 0
.../src/robot-simulator}/arms/viper850_arm1.bnd | 0
.../src/robot-simulator}/arms/viper850_arm2.bnd | 0
.../src/robot-simulator}/arms/viper850_arm3.bnd | 0
.../src/robot-simulator}/arms/viper850_arm4.bnd | 0
.../src/robot-simulator}/arms/viper850_arm5.bnd | 0
.../src/robot-simulator}/arms/viper850_arm6.bnd | 0
.../robot/src/robot-simulator/vpRobotCamera.cpp | 284 ++
.../robot/src/robot-simulator/vpRobotSimulator.cpp | 47 +
.../robot-simulator/vpRobotWireFrameSimulator.cpp | 372 ++
.../robot/src/robot-simulator/vpSimulatorAfma6.cpp | 2626 ++++++++++++
.../src/robot-simulator/vpSimulatorCamera.cpp | 273 ++
.../src/robot-simulator/vpSimulatorPioneer.cpp | 241 ++
.../src/robot-simulator/vpSimulatorPioneerPan.cpp | 261 ++
.../src/robot-simulator/vpSimulatorViper850.cpp | 2530 ++++++++++++
modules/robot/src/vpRobot.cpp | 279 ++
modules/robot/src/vpRobotTemplate.cpp | 172 +
.../robot/src/wireframe-simulator/core/vpArit.cpp | 810 ++++
.../src/wireframe-simulator/core/vpAritio.cpp | 148 +
.../robot/src/wireframe-simulator/core/vpBound.cpp | 227 +
.../src/wireframe-simulator/core/vpBoundio.cpp | 215 +
.../src/wireframe-simulator/core/vpClipping.cpp | 486 +++
.../src/wireframe-simulator/core/vpCoreDisplay.cpp | 237 ++
.../src/wireframe-simulator/core/vpKeyword.cpp | 244 ++
.../robot/src/wireframe-simulator/core/vpLex.cpp | 819 ++++
.../robot/src/wireframe-simulator/core/vpMyio.cpp | 130 +
.../src/wireframe-simulator/core/vpParser.cpp | 107 +
.../src/wireframe-simulator/core/vpProjection.cpp | 240 ++
.../src/wireframe-simulator/core/vpRfstack.cpp | 201 +
.../src/wireframe-simulator/core/vpSkipio.cpp | 102 +
.../src/wireframe-simulator/core/vpTmstack.cpp | 240 ++
.../robot/src/wireframe-simulator/core/vpToken.cpp | 74 +
.../src/wireframe-simulator/core/vpViewio.cpp | 154 +
.../src/wireframe-simulator/core/vpVwstack.cpp | 278 ++
.../robot/src}/wireframe-simulator/scene/3pts.bnd | 0
.../robot/src}/wireframe-simulator/scene/3pts.sld | 0
.../src}/wireframe-simulator/scene/camera.bnd | 0
.../src}/wireframe-simulator/scene/camera.sld | 0
.../src}/wireframe-simulator/scene/circle.bnd | 0
.../src}/wireframe-simulator/scene/circle.sld | 0
.../src}/wireframe-simulator/scene/circle_sq2.bnd | 0
.../src}/wireframe-simulator/scene/circle_sq2.sld | 0
.../src}/wireframe-simulator/scene/circles2.bnd | 0
.../src}/wireframe-simulator/scene/circles2.sld | 0
.../robot/src}/wireframe-simulator/scene/cube.bnd | 0
.../robot/src}/wireframe-simulator/scene/cube.sld | 0
.../src}/wireframe-simulator/scene/cylinder.bnd | 0
.../src}/wireframe-simulator/scene/cylinder.sld | 0
.../src}/wireframe-simulator/scene/diamond.bnd | 0
.../src}/wireframe-simulator/scene/diamond.sld | 0
.../robot/src}/wireframe-simulator/scene/line.bnd | 0
.../robot/src}/wireframe-simulator/scene/line.sld | 0
.../robot/src}/wireframe-simulator/scene/pipe.bnd | 0
.../robot/src}/wireframe-simulator/scene/pipe.sld | 0
.../robot/src}/wireframe-simulator/scene/plan.bnd | 0
.../robot/src}/wireframe-simulator/scene/plan.sld | 0
.../robot/src}/wireframe-simulator/scene/plate.bnd | 0
.../robot/src}/wireframe-simulator/scene/plate.sld | 0
.../src}/wireframe-simulator/scene/plate_6cm.bnd | 0
.../src}/wireframe-simulator/scene/plate_6cm.sld | 0
.../src}/wireframe-simulator/scene/plate_8cm.bnd | 0
.../src}/wireframe-simulator/scene/plate_8cm.sld | 0
.../robot/src}/wireframe-simulator/scene/point.bnd | 0
.../robot/src}/wireframe-simulator/scene/point.sld | 0
.../src}/wireframe-simulator/scene/point_cloud.bnd | 0
.../src}/wireframe-simulator/scene/point_cloud.sld | 0
.../src}/wireframe-simulator/scene/rectangle.bnd | 0
.../src}/wireframe-simulator/scene/rectangle.sld | 0
.../robot/src}/wireframe-simulator/scene/road.bnd | 0
.../robot/src}/wireframe-simulator/scene/road.sld | 0
.../src}/wireframe-simulator/scene/sphere.bnd | 0
.../src}/wireframe-simulator/scene/sphere.sld | 0
.../src}/wireframe-simulator/scene/square10cm.bnd | 0
.../src}/wireframe-simulator/scene/square10cm.sld | 0
.../robot/src}/wireframe-simulator/scene/tool.bnd | 0
.../robot/src}/wireframe-simulator/scene/tool.sld | 0
.../src}/wireframe-simulator/scene/trapezoid.bnd | 0
.../src}/wireframe-simulator/scene/trapezoid.sld | 0
.../wireframe-simulator/vpWireFrameSimulator.cpp | 1860 +++++++++
modules/robot/test/servo-afma4/testAfma4.cpp | 71 +
modules/robot/test/servo-afma4/testRobotAfma4.cpp | 76 +
modules/robot/test/servo-afma6/testAfma6.cpp | 108 +
modules/robot/test/servo-afma6/testRobotAfma6.cpp | 113 +
.../robot/test/servo-afma6/testRobotAfma6Pose.cpp | 236 ++
.../robot/test/servo-viper/testRobotViper850.cpp | 122 +
.../test/servo-viper/testRobotViper850Pose.cpp | 239 ++
modules/robot/test/servo-viper/testViper850.cpp | 124 +
modules/sensor/CMakeLists.txt | 74 +
.../sensor/include/visp3/sensor/vp1394CMUGrabber.h | 299 ++
.../sensor/include/visp3/sensor/vp1394TwoGrabber.h | 472 +++
.../include/visp3/sensor/vpDirectShowDevice.h | 80 +
.../include/visp3/sensor/vpDirectShowGrabber.h | 118 +
.../include/visp3/sensor/vpDirectShowGrabberImpl.h | 221 +
.../visp3/sensor/vpDirectShowSampleGrabberI.h | 102 +
modules/sensor/include/visp3/sensor/vpKinect.h | 185 +
modules/sensor/include/visp3/sensor/vpLaserScan.h | 150 +
.../sensor/include/visp3/sensor/vpLaserScanner.h | 87 +
.../sensor/include/visp3/sensor/vpOpenCVGrabber.h | 119 +
modules/sensor/include/visp3/sensor/vpScanPoint.h | 263 ++
modules/sensor/include/visp3/sensor/vpSickLDMRS.h | 157 +
.../sensor/include/visp3/sensor/vpV4l2Grabber.h | 372 ++
.../src/framegrabber/1394/vp1394CMUGrabber.cpp | 732 ++++
.../src/framegrabber/1394/vp1394TwoGrabber.cpp | 3667 +++++++++++++++++
.../src/framegrabber/OpenCV/vpOpenCVGrabber.cpp | 323 ++
.../framegrabber/directshow/vpDirectShowDevice.cpp | 128 +
.../directshow/vpDirectShowGrabber.cpp | 238 ++
.../directshow/vpDirectShowGrabberImpl.cpp | 1238 ++++++
.../directshow/vpDirectShowSampleGrabberI.cpp | 330 ++
.../sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp | 1367 ++++++
modules/sensor/src/kinect/vpKinect.cpp | 312 ++
.../sensor/src/laserscanner/sick/vpSickLDMRS.cpp | 288 ++
.../test/framegrabber/test1394TwoGrabber.cpp | 138 +
.../test/framegrabber/test1394TwoResetBus.cpp | 104 +
modules/tracker/blob/CMakeLists.txt | 42 +
modules/tracker/blob/include/visp3/blob/vpDot.h | 421 ++
modules/tracker/blob/include/visp3/blob/vpDot2.h | 555 +++
modules/tracker/blob/src/dots/vpDot.cpp | 968 +++++
modules/tracker/blob/src/dots/vpDot2.cpp | 2626 ++++++++++++
.../tracker/blob/test/tracking/testTrackDot.cpp | 321 ++
modules/tracker/klt/CMakeLists.txt | 42 +
.../tracker/klt/include/visp3/klt/vpKltOpencv.h | 412 ++
modules/tracker/klt/src/vpKltOpencv.cpp | 1355 ++++++
modules/tracker/mbt/CMakeLists.txt | 115 +
.../mbt/include/visp3/mbt/vpMbEdgeKltTracker.h | 378 ++
.../mbt/include/visp3/mbt/vpMbEdgeTracker.h | 493 +++
.../mbt/include/visp3/mbt/vpMbHiddenFaces.h | 892 ++++
.../tracker/mbt/include/visp3/mbt/vpMbKltTracker.h | 442 ++
.../tracker/mbt/include/visp3/mbt/vpMbScanLine.h | 214 +
.../tracker/mbt/include/visp3/mbt/vpMbTracker.h | 661 +++
.../tracker/mbt/include/visp3/mbt/vpMbXmlParser.h | 244 ++
.../mbt/include/visp3/mbt/vpMbtDistanceCircle.h | 250 ++
.../mbt/include/visp3/mbt/vpMbtDistanceCylinder.h | 271 ++
.../include/visp3/mbt/vpMbtDistanceKltCylinder.h | 212 +
.../mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h | 213 +
.../mbt/include/visp3/mbt/vpMbtDistanceLine.h | 254 ++
.../mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h | 94 +
.../mbt/include/visp3/mbt/vpMbtKltXmlParser.h | 229 ++
.../tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h | 242 ++
.../tracker/mbt/include/visp3/mbt/vpMbtMeLine.h | 127 +
.../tracker/mbt/include/visp3/mbt/vpMbtPolygon.h | 167 +
.../tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h | 120 +
modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp | 2862 +++++++++++++
.../tracker/mbt/src/edge/vpMbtDistanceCircle.cpp | 427 ++
.../tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp | 719 ++++
modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp | 849 ++++
modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp | 545 +++
modules/tracker/mbt/src/edge/vpMbtMeLine.cpp | 867 ++++
modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp | 522 +++
.../tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp | 1398 +++++++
.../mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp | 202 +
modules/tracker/mbt/src/klt/vpMbKltTracker.cpp | 1398 +++++++
.../mbt/src/klt/vpMbtDistanceKltCylinder.cpp | 623 +++
.../tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp | 591 +++
modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp | 294 ++
modules/tracker/mbt/src/vpMbScanLine.cpp | 836 ++++
modules/tracker/mbt/src/vpMbTracker.cpp | 2526 ++++++++++++
modules/tracker/mbt/src/vpMbXmlParser.cpp | 386 ++
modules/tracker/mbt/src/vpMbtPolygon.cpp | 285 ++
modules/tracker/me/CMakeLists.txt | 42 +
modules/tracker/me/include/visp3/me/vpMe.h | 301 ++
modules/tracker/me/include/visp3/me/vpMeEllipse.h | 334 ++
modules/tracker/me/include/visp3/me/vpMeLine.h | 260 ++
modules/tracker/me/include/visp3/me/vpMeNurbs.h | 225 +
modules/tracker/me/include/visp3/me/vpMeSite.h | 259 ++
modules/tracker/me/include/visp3/me/vpMeTracker.h | 176 +
modules/tracker/me/include/visp3/me/vpNurbs.h | 152 +
modules/tracker/me/src/moving-edges/vpMe.cpp | 476 +++
.../tracker/me/src/moving-edges/vpMeEllipse.cpp | 1240 ++++++
modules/tracker/me/src/moving-edges/vpMeLine.cpp | 1446 +++++++
modules/tracker/me/src/moving-edges/vpMeNurbs.cpp | 1368 ++++++
modules/tracker/me/src/moving-edges/vpMeSite.cpp | 734 ++++
.../tracker/me/src/moving-edges/vpMeTracker.cpp | 371 ++
modules/tracker/me/src/moving-edges/vpNurbs.cpp | 1082 +++++
modules/tracker/me/test/testNurbs.cpp | 385 ++
modules/tracker/tt/CMakeLists.txt | 41 +
.../tt/include/visp3/tt/vpTemplateTracker.h | 272 ++
.../tt/include/visp3/tt/vpTemplateTrackerBSpline.h | 61 +
.../tt/include/visp3/tt/vpTemplateTrackerHeader.h | 104 +
.../tt/include/visp3/tt/vpTemplateTrackerSSD.h | 83 +
.../tt/include/visp3/tt/vpTemplateTrackerSSDESM.h | 72 +
.../tt/vpTemplateTrackerSSDForwardAdditional.h | 81 +
.../tt/vpTemplateTrackerSSDForwardCompositional.h | 66 +
.../tt/vpTemplateTrackerSSDInverseCompositional.h | 83 +
.../include/visp3/tt/vpTemplateTrackerTriangle.h | 166 +
.../tt/include/visp3/tt/vpTemplateTrackerWarp.h | 246 ++
.../include/visp3/tt/vpTemplateTrackerWarpAffine.h | 175 +
.../visp3/tt/vpTemplateTrackerWarpHomography.h | 200 +
.../visp3/tt/vpTemplateTrackerWarpHomographySL3.h | 217 +
.../tt/include/visp3/tt/vpTemplateTrackerWarpRT.h | 175 +
.../tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h | 175 +
.../visp3/tt/vpTemplateTrackerWarpTranslation.h | 175 +
.../tt/include/visp3/tt/vpTemplateTrackerZNCC.h | 83 +
.../tt/vpTemplateTrackerZNCCForwardAdditional.h | 65 +
.../tt/vpTemplateTrackerZNCCInverseCompositional.h | 81 +
.../tt/include/visp3/tt/vpTemplateTrackerZone.h | 118 +
.../tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp | 129 +
.../tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp | 329 ++
.../ssd/vpTemplateTrackerSSDForwardAdditional.cpp | 210 +
.../vpTemplateTrackerSSDForwardCompositional.cpp | 175 +
.../vpTemplateTrackerSSDInverseCompositional.cpp | 239 ++
.../tt/src/tools/vpTemplateTrackerBSpline.cpp | 79 +
.../tt/src/tools/vpTemplateTrackerTriangle.cpp | 435 ++
.../tracker/tt/src/tools/vpTemplateTrackerZone.cpp | 616 +++
modules/tracker/tt/src/vpTemplateTracker.cpp | 978 +++++
.../tracker/tt/src/warp/vpTemplateTrackerWarp.cpp | 164 +
.../tt/src/warp/vpTemplateTrackerWarpAffine.cpp | 169 +
.../src/warp/vpTemplateTrackerWarpHomography.cpp | 218 +
.../warp/vpTemplateTrackerWarpHomographySL3.cpp | 399 ++
.../tt/src/warp/vpTemplateTrackerWarpRT.cpp | 177 +
.../tt/src/warp/vpTemplateTrackerWarpSRT.cpp | 185 +
.../src/warp/vpTemplateTrackerWarpTranslation.cpp | 126 +
.../tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp | 131 +
.../vpTemplateTrackerZNCCForwardAdditional.cpp | 320 ++
.../vpTemplateTrackerZNCCInverseCompositional.cpp | 436 ++
modules/tracker/tt_mi/CMakeLists.txt | 45 +
.../include/visp3/tt_mi/vpTemplateTrackerMI.h | 172 +
.../visp3/tt_mi/vpTemplateTrackerMIBSpline.h | 103 +
.../include/visp3/tt_mi/vpTemplateTrackerMIESM.h | 105 +
.../tt_mi/vpTemplateTrackerMIForwardAdditional.h | 111 +
.../vpTemplateTrackerMIForwardCompositional.h | 70 +
.../vpTemplateTrackerMIInverseCompositional.h | 121 +
.../tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp | 859 ++++
.../tt_mi/src/mi/vpTemplateTrackerMIESM.cpp | 492 +++
.../mi/vpTemplateTrackerMIForwardAdditional.cpp | 436 ++
.../mi/vpTemplateTrackerMIForwardCompositional.cpp | 323 ++
.../mi/vpTemplateTrackerMIInverseCompositional.cpp | 588 +++
.../tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp | 791 ++++
modules/vision/CMakeLists.txt | 51 +
.../vision/include/visp3/vision/vpBasicKeyPoint.h | 254 ++
.../vision/include/visp3/vision/vpCalibration.h | 210 +
.../include/visp3/vision/vpCalibrationException.h | 101 +
.../vision/include/visp3/vision/vpFernClassifier.h | 336 ++
modules/vision/include/visp3/vision/vpHomography.h | 403 ++
modules/vision/include/visp3/vision/vpKeyPoint.h | 1062 +++++
.../vision/include/visp3/vision/vpKeyPointSurf.h | 301 ++
.../include/visp3/vision/vpLevenbergMarquartd.h | 99 +
.../include/visp3/vision/vpPlanarObjectDetector.h | 283 ++
modules/vision/include/visp3/vision/vpPose.h | 230 ++
.../vision/include/visp3/vision/vpPoseException.h | 99 +
.../vision/include/visp3/vision/vpPoseFeatures.h | 736 ++++
.../visp3/vision/vpXmlConfigParserKeyPoint.h | 271 ++
modules/vision/src/calibration/vpCalibration.cpp | 831 ++++
.../vision/src/calibration/vpCalibrationTools.cpp | 1837 +++++++++
.../src/homography-estimation/vpHomography.cpp | 815 ++++
.../src/homography-estimation/vpHomographyDLT.cpp | 386 ++
.../homography-estimation/vpHomographyExtract.cpp | 1331 ++++++
.../homography-estimation/vpHomographyMalis.cpp | 720 ++++
.../homography-estimation/vpHomographyRansac.cpp | 540 +++
.../src/homography-estimation/vpHomographyVVS.cpp | 733 ++++
modules/vision/src/key-point/vpBasicKeyPoint.cpp | 49 +
modules/vision/src/key-point/vpFernClassifier.cpp | 583 +++
modules/vision/src/key-point/vpKeyPoint.cpp | 3928 ++++++++++++++++++
modules/vision/src/key-point/vpKeyPointSurf.cpp | 722 ++++
.../src/key-point/vpPlanarObjectDetector.cpp | 540 +++
.../src/key-point/vpXmlConfigParserKeyPoint.cpp | 429 ++
.../src/pose-estimation/vpLevenbergMarquartd.cpp | 1663 ++++++++
modules/vision/src/pose-estimation/vpPose.cpp | 743 ++++
.../vision/src/pose-estimation/vpPoseDementhon.cpp | 712 ++++
.../vision/src/pose-estimation/vpPoseFeatures.cpp | 575 +++
.../vision/src/pose-estimation/vpPoseLagrange.cpp | 603 +++
modules/vision/src/pose-estimation/vpPoseLowe.cpp | 370 ++
.../vision/src/pose-estimation/vpPoseRansac.cpp | 467 +++
.../vpPoseVirtualVisualServoing.cpp | 274 ++
.../vision/test/homography/testDisplacement.cpp | 193 +
modules/vision/test/key-point/testKeyPoint-2.cpp | 437 ++
modules/vision/test/key-point/testKeyPoint-3.cpp | 287 ++
modules/vision/test/key-point/testKeyPoint-4.cpp | 406 ++
modules/vision/test/key-point/testKeyPoint-5.cpp | 268 ++
modules/vision/test/key-point/testKeyPoint-6.cpp | 333 ++
modules/vision/test/key-point/testKeyPoint-7.cpp | 684 +++
modules/vision/test/key-point/testKeyPoint.cpp | 248 ++
modules/vision/test/key-point/testSurfKeyPoint.cpp | 376 ++
modules/vision/test/pose/testFindMatch.cpp | 121 +
modules/vision/test/pose/testPose.cpp | 207 +
modules/vision/test/pose/testPoseFeatures.cpp | 210 +
modules/vision/test/pose/testPoseRansac.cpp | 132 +
modules/visual_features/CMakeLists.txt | 42 +
.../include/visp3/visual_features/vpBasicFeature.h | 172 +
.../visp3/visual_features/vpFeatureBuilder.h | 185 +
.../include/visp3/visual_features/vpFeatureDepth.h | 238 ++
.../visp3/visual_features/vpFeatureEllipse.h | 152 +
.../visp3/visual_features/vpFeatureException.h | 99 +
.../include/visp3/visual_features/vpFeatureLine.h | 274 ++
.../visp3/visual_features/vpFeatureLuminance.h | 157 +
.../visp3/visual_features/vpFeatureMoment.h | 246 ++
.../visp3/visual_features/vpFeatureMomentAlpha.h | 172 +
.../visp3/visual_features/vpFeatureMomentArea.h | 81 +
.../vpFeatureMomentAreaNormalized.h | 180 +
.../visp3/visual_features/vpFeatureMomentBasic.h | 94 +
.../visual_features/vpFeatureMomentCInvariant.h | 292 ++
.../visual_features/vpFeatureMomentCentered.h | 110 +
.../visp3/visual_features/vpFeatureMomentCommon.h | 267 ++
.../visual_features/vpFeatureMomentDatabase.h | 176 +
.../visual_features/vpFeatureMomentGravityCenter.h | 239 ++
.../vpFeatureMomentGravityCenterNormalized.h | 251 ++
.../include/visp3/visual_features/vpFeaturePoint.h | 269 ++
.../visp3/visual_features/vpFeaturePoint3D.h | 287 ++
.../visp3/visual_features/vpFeaturePointPolar.h | 342 ++
.../visp3/visual_features/vpFeatureSegment.h | 302 ++
.../visp3/visual_features/vpFeatureThetaU.h | 329 ++
.../visp3/visual_features/vpFeatureTranslation.h | 364 ++
.../visual_features/vpFeatureVanishingPoint.h | 147 +
.../visp3/visual_features/vpGenericFeature.h | 238 ++
.../feature-builder/vpFeatureBuilderEllipse.cpp | 330 ++
.../src/feature-builder/vpFeatureBuilderLine.cpp | 280 ++
.../src/feature-builder/vpFeatureBuilderPoint.cpp | 334 ++
.../feature-builder/vpFeatureBuilderPoint3D.cpp | 89 +
.../feature-builder/vpFeatureBuilderPointPolar.cpp | 400 ++
.../feature-builder/vpFeatureBuilderSegment.cpp | 182 +
.../vpFeatureBuilderVanishingPoint.cpp | 151 +
.../src/visual-feature/vpBasicFeature.cpp | 197 +
.../src/visual-feature/vpFeatureDepth.cpp | 505 +++
.../src/visual-feature/vpFeatureEllipse.cpp | 473 +++
.../src/visual-feature/vpFeatureLine.cpp | 576 +++
.../src/visual-feature/vpFeatureLuminance.cpp | 422 ++
.../src/visual-feature/vpFeatureMoment.cpp | 271 ++
.../src/visual-feature/vpFeatureMomentAlpha.cpp | 160 +
.../src/visual-feature/vpFeatureMomentArea.cpp | 100 +
.../vpFeatureMomentAreaNormalized.cpp | 192 +
.../src/visual-feature/vpFeatureMomentBasic.cpp | 152 +
.../visual-feature/vpFeatureMomentCInvariant.cpp | 420 ++
.../src/visual-feature/vpFeatureMomentCentered.cpp | 310 ++
.../src/visual-feature/vpFeatureMomentCommon.cpp | 85 +
.../src/visual-feature/vpFeatureMomentDatabase.cpp | 106 +
.../vpFeatureMomentGravityCenter.cpp | 159 +
.../vpFeatureMomentGravityCenterNormalized.cpp | 217 +
.../src/visual-feature/vpFeaturePoint.cpp | 572 +++
.../src/visual-feature/vpFeaturePoint3D.cpp | 688 ++++
.../src/visual-feature/vpFeaturePointPolar.cpp | 676 +++
.../src/visual-feature/vpFeatureSegment.cpp | 696 ++++
.../src/visual-feature/vpFeatureThetaU.cpp | 810 ++++
.../src/visual-feature/vpFeatureTranslation.cpp | 784 ++++
.../src/visual-feature/vpFeatureVanishingPoint.cpp | 309 ++
.../src/visual-feature/vpGenericFeature.cpp | 754 ++++
modules/visual_features/test/feature/testPoint.cpp | 168 +
modules/vs/CMakeLists.txt | 43 +
modules/vs/include/visp3/vs/vpAdaptiveGain.h | 171 +
modules/vs/include/visp3/vs/vpServo.h | 612 +++
modules/vs/include/visp3/vs/vpServoData.h | 116 +
modules/vs/include/visp3/vs/vpServoDisplay.h | 79 +
modules/vs/include/visp3/vs/vpServoException.h | 96 +
modules/vs/src/vpAdaptiveGain.cpp | 362 ++
modules/vs/src/vpServo.cpp | 1817 ++++++++
modules/vs/src/vpServoData.cpp | 124 +
modules/vs/src/vpServoDisplay.cpp | 150 +
modules/vs/test/visual-feature/testFeature.cpp | 99 +
.../vs/test/visual-feature/testFeatureMoment.cpp | 220 +
.../vs/test/visual-feature/testFeatureSegment.cpp | 275 ++
src/CMakeLists.txt | 133 -
src/camera/calibration/vpCalibration.cpp | 1056 -----
src/camera/calibration/vpCalibration.h | 237 --
src/camera/calibration/vpCalibrationException.h | 108 -
src/camera/calibration/vpCalibrationTools.cpp | 2009 ---------
src/camera/vpCameraParameters.cpp | 632 ---
src/camera/vpCameraParameters.h | 367 --
src/camera/vpMeterPixelConversion.cpp | 116 -
src/camera/vpMeterPixelConversion.h | 247 --
src/camera/vpPixelMeterConversion.cpp | 131 -
src/camera/vpPixelMeterConversion.h | 255 --
src/camera/vpXmlParserCamera.cpp | 1101 -----
src/camera/vpXmlParserCamera.h | 359 --
.../homography-estimation/vpHomography.cpp | 768 ----
.../homography-estimation/vpHomography.h | 442 --
.../homography-estimation/vpHomographyDLT.cpp | 631 ---
.../homography-estimation/vpHomographyExtract.cpp | 1995 ---------
.../homography-estimation/vpHomographyMalis.cpp | 855 ----
.../homography-estimation/vpHomographyRansac.cpp | 618 ---
.../homography-estimation/vpHomographyVVS.cpp | 735 ----
.../pose-estimation/vpLevenbergMarquartd.cpp | 1667 --------
.../pose-estimation/vpLevenbergMarquartd.h | 103 -
src/computer-vision/pose-estimation/vpPose.cpp | 751 ----
src/computer-vision/pose-estimation/vpPose.h | 280 --
.../pose-estimation/vpPoseDementhon.cpp | 716 ----
.../pose-estimation/vpPoseException.h | 103 -
.../pose-estimation/vpPoseFeatures.cpp | 582 ---
.../pose-estimation/vpPoseFeatures.h | 735 ----
.../pose-estimation/vpPoseLagrange.cpp | 607 ---
src/computer-vision/pose-estimation/vpPoseLowe.cpp | 374 --
.../pose-estimation/vpPoseRansac.cpp | 857 ----
.../vpPoseVirtualVisualServoing.cpp | 278 --
src/data-structure/vpList.h | 839 ----
src/detection/barcode/vpDetectorDataMatrixCode.cpp | 127 -
src/detection/barcode/vpDetectorDataMatrixCode.h | 122 -
src/detection/barcode/vpDetectorQRCode.cpp | 99 -
src/detection/barcode/vpDetectorQRCode.h | 126 -
src/detection/face/vpDetectorFace.cpp | 129 -
src/detection/face/vpDetectorFace.h | 100 -
src/detection/vpDetectorBase.h | 162 -
src/device/display/vpDisplay.cpp | 3960 ------------------
src/device/display/vpDisplay.h | 936 -----
src/device/display/vpDisplayException.h | 114 -
src/device/display/vpDisplayGTK.cpp | 1722 --------
src/device/display/vpDisplayGTK.h | 288 --
src/device/display/vpDisplayOpenCV.cpp | 2220 ----------
src/device/display/vpDisplayOpenCV.h | 284 --
src/device/display/vpDisplayX.cpp | 3400 ---------------
src/device/display/vpDisplayX.h | 279 --
src/device/display/vpMouseButton.h | 70 -
src/device/display/windows/vpD3DRenderer.cpp | 1217 ------
src/device/display/windows/vpD3DRenderer.h | 211 -
src/device/display/windows/vpDisplayD3D.cpp | 120 -
src/device/display/windows/vpDisplayD3D.h | 121 -
src/device/display/windows/vpDisplayGDI.cpp | 121 -
src/device/display/windows/vpDisplayGDI.h | 145 -
src/device/display/windows/vpDisplayWin32.cpp | 964 -----
src/device/display/windows/vpDisplayWin32.h | 225 -
src/device/display/windows/vpGDIRenderer.cpp | 1163 ------
src/device/display/windows/vpGDIRenderer.h | 150 -
src/device/display/windows/vpWin32API.cpp | 141 -
src/device/display/windows/vpWin32API.h | 65 -
src/device/display/windows/vpWin32Renderer.h | 187 -
src/device/display/windows/vpWin32Window.cpp | 348 --
src/device/display/windows/vpWin32Window.h | 148 -
src/device/framegrabber/1394/vp1394CMUGrabber.cpp | 734 ----
src/device/framegrabber/1394/vp1394CMUGrabber.h | 303 --
src/device/framegrabber/1394/vp1394TwoGrabber.cpp | 3836 -----------------
src/device/framegrabber/1394/vp1394TwoGrabber.h | 473 ---
src/device/framegrabber/OpenCV/vpOpenCVGrabber.cpp | 323 --
src/device/framegrabber/OpenCV/vpOpenCVGrabber.h | 123 -
.../framegrabber/directshow/vpDirectShowDevice.cpp | 129 -
.../framegrabber/directshow/vpDirectShowDevice.h | 86 -
.../directshow/vpDirectShowGrabber.cpp | 241 --
.../framegrabber/directshow/vpDirectShowGrabber.h | 122 -
.../directshow/vpDirectShowGrabberImpl.cpp | 1242 ------
.../directshow/vpDirectShowGrabberImpl.h | 227 -
.../directshow/vpDirectShowSampleGrabberI.cpp | 289 --
.../directshow/vpDirectShowSampleGrabberI.h | 106 -
src/device/framegrabber/disk/vpDiskGrabber.cpp | 416 --
src/device/framegrabber/disk/vpDiskGrabber.h | 160 -
.../generic-framegrabber/vpFrameGrabber.h | 142 -
.../generic-framegrabber/vpFrameGrabberException.h | 102 -
src/device/framegrabber/v4l2/vpV4l2Grabber.cpp | 1366 ------
src/device/framegrabber/v4l2/vpV4l2Grabber.h | 360 --
src/device/kinect/vpKinect.cpp | 318 --
src/device/kinect/vpKinect.h | 188 -
src/device/laserscanner/sick/vpSickLDMRS.cpp | 292 --
src/device/laserscanner/sick/vpSickLDMRS.h | 161 -
src/device/laserscanner/vpLaserScan.h | 153 -
src/device/laserscanner/vpLaserScanner.h | 90 -
src/device/laserscanner/vpScanPoint.h | 266 --
src/device/light/vpRingLight.cpp | 229 --
src/device/light/vpRingLight.h | 134 -
src/exceptions/vpException.cpp | 150 -
src/exceptions/vpException.h | 136 -
src/image/vpColor.cpp | 145 -
src/image/vpColor.h | 244 --
src/image/vpImage.h | 1601 -------
src/image/vpImageConvert.cpp | 3554 ----------------
src/image/vpImageConvert.h | 322 --
src/image/vpImageException.h | 105 -
src/image/vpImageFilter.cpp | 691 ----
src/image/vpImageFilter.h | 448 --
src/image/vpImageIo.cpp | 3329 ---------------
src/image/vpImageIo.h | 256 --
src/image/vpImageMorphology.h | 229 --
src/image/vpImagePoint.cpp | 415 --
src/image/vpImagePoint.h | 315 --
src/image/vpImageTools.cpp | 202 -
src/image/vpImageTools.h | 611 ---
src/image/vpRGBa.cpp | 268 --
src/image/vpRGBa.h | 161 -
src/key-point/vpBasicKeyPoint.cpp | 53 -
src/key-point/vpBasicKeyPoint.h | 259 --
src/key-point/vpFernClassifier.cpp | 584 ---
src/key-point/vpFernClassifier.h | 339 --
src/key-point/vpKeyPoint.cpp | 3008 --------------
src/key-point/vpKeyPoint.h | 861 ----
src/key-point/vpKeyPointSurf.cpp | 783 ----
src/key-point/vpKeyPointSurf.h | 313 --
src/key-point/vpPlanarObjectDetector.cpp | 542 ---
src/key-point/vpPlanarObjectDetector.h | 286 --
src/key-point/vpXmlConfigParserKeyPoint.cpp | 426 --
src/key-point/vpXmlConfigParserKeyPoint.h | 269 --
src/math/kalman/vpKalmanFilter.cpp | 466 ---
src/math/kalman/vpKalmanFilter.h | 225 -
.../kalman/vpLinearKalmanFilterInstantiation.cpp | 857 ----
.../kalman/vpLinearKalmanFilterInstantiation.h | 179 -
src/math/matrix/vpColVector.cpp | 863 ----
src/math/matrix/vpColVector.h | 219 -
src/math/matrix/vpGEMM.h | 364 --
src/math/matrix/vpMatrix.cpp | 4349 --------------------
src/math/matrix/vpMatrix.h | 592 ---
src/math/matrix/vpMatrixException.h | 111 -
src/math/matrix/vpMatrix_cholesky.cpp | 136 -
src/math/matrix/vpMatrix_covariance.cpp | 106 -
src/math/matrix/vpMatrix_lu.cpp | 282 --
src/math/matrix/vpMatrix_qr.cpp | 242 --
src/math/matrix/vpMatrix_svd.cpp | 717 ----
src/math/matrix/vpRowVector.cpp | 392 --
src/math/matrix/vpRowVector.h | 162 -
src/math/matrix/vpSubColVector.cpp | 189 -
src/math/matrix/vpSubColVector.h | 104 -
src/math/matrix/vpSubMatrix.cpp | 184 -
src/math/matrix/vpSubMatrix.h | 106 -
src/math/matrix/vpSubRowVector.cpp | 186 -
src/math/matrix/vpSubRowVector.h | 106 -
src/math/misc/vpHinkley.cpp | 450 --
src/math/misc/vpHinkley.h | 173 -
src/math/misc/vpMath.cpp | 53 -
src/math/misc/vpMath.h | 380 --
src/math/misc/vpNoise.cpp | 147 -
src/math/misc/vpNoise.h | 219 -
src/math/robust/vpRansac.h | 259 --
src/math/robust/vpRobust.cpp | 1015 -----
src/math/robust/vpRobust.h | 210 -
src/math/robust/vpScale.cpp | 306 --
src/math/robust/vpScale.h | 104 -
src/math/spline/vpBSpline.cpp | 533 ---
src/math/spline/vpBSpline.h | 315 --
src/math/spline/vpNurbs.cpp | 1134 -----
src/math/spline/vpNurbs.h | 191 -
src/math/transformation/vpExponentialMap.cpp | 321 --
src/math/transformation/vpExponentialMap.h | 100 -
src/math/transformation/vpForceTwistMatrix.cpp | 437 --
src/math/transformation/vpForceTwistMatrix.h | 150 -
src/math/transformation/vpHomogeneousMatrix.cpp | 754 ----
src/math/transformation/vpHomogeneousMatrix.h | 220 -
src/math/transformation/vpPoseVector.cpp | 323 --
src/math/transformation/vpPoseVector.h | 186 -
src/math/transformation/vpQuaternionVector.cpp | 153 -
src/math/transformation/vpQuaternionVector.h | 118 -
src/math/transformation/vpRotationMatrix.cpp | 757 ----
src/math/transformation/vpRotationMatrix.h | 184 -
src/math/transformation/vpRotationVector.cpp | 125 -
src/math/transformation/vpRotationVector.h | 167 -
src/math/transformation/vpRxyzVector.cpp | 176 -
src/math/transformation/vpRxyzVector.h | 199 -
src/math/transformation/vpRzyxVector.cpp | 136 -
src/math/transformation/vpRzyxVector.h | 204 -
src/math/transformation/vpRzyzVector.cpp | 150 -
src/math/transformation/vpRzyzVector.h | 199 -
src/math/transformation/vpThetaUVector.cpp | 325 --
src/math/transformation/vpThetaUVector.h | 201 -
src/math/transformation/vpTranslationVector.cpp | 380 --
src/math/transformation/vpTranslationVector.h | 144 -
src/math/transformation/vpVelocityTwistMatrix.cpp | 460 ---
src/math/transformation/vpVelocityTwistMatrix.h | 158 -
.../vpXmlParserHomogeneousMatrix.cpp | 608 ---
.../transformation/vpXmlParserHomogeneousMatrix.h | 262 --
src/network/vpClient.cpp | 227 -
src/network/vpClient.h | 216 -
src/network/vpNetwork.cpp | 793 ----
src/network/vpNetwork.h | 499 ---
src/network/vpRequest.cpp | 88 -
src/network/vpRequest.h | 241 --
src/network/vpServer.cpp | 285 --
src/network/vpServer.h | 225 -
src/robot/real-robot/afma4/vpAfma4.cpp | 650 ---
src/robot/real-robot/afma4/vpAfma4.h | 166 -
src/robot/real-robot/afma4/vpRobotAfma4.cpp | 1868 ---------
src/robot/real-robot/afma4/vpRobotAfma4.h | 296 --
src/robot/real-robot/afma4/vpServolens.cpp | 899 ----
src/robot/real-robot/afma4/vpServolens.h | 153 -
src/robot/real-robot/afma6/vpAfma6.cpp | 1585 -------
src/robot/real-robot/afma6/vpAfma6.h | 209 -
src/robot/real-robot/afma6/vpRobotAfma6.cpp | 2245 ----------
src/robot/real-robot/afma6/vpRobotAfma6.h | 357 --
src/robot/real-robot/biclops/vpBiclops.cpp | 455 --
src/robot/real-robot/biclops/vpBiclops.h | 201 -
src/robot/real-robot/biclops/vpRobotBiclops.cpp | 1436 -------
src/robot/real-robot/biclops/vpRobotBiclops.h | 176 -
.../biclops/vpRobotBiclopsController.cpp | 494 ---
.../real-robot/biclops/vpRobotBiclopsController.h | 160 -
src/robot/real-robot/pioneer/vpPioneer.h | 159 -
src/robot/real-robot/pioneer/vpPioneerPan.h | 233 --
src/robot/real-robot/pioneer/vpRobotPioneer.cpp | 236 --
src/robot/real-robot/pioneer/vpRobotPioneer.h | 146 -
src/robot/real-robot/pioneer/vpUnicycle.h | 146 -
src/robot/real-robot/ptu46/vpPtu46.cpp | 325 --
src/robot/real-robot/ptu46/vpPtu46.h | 122 -
src/robot/real-robot/ptu46/vpRobotPtu46.cpp | 937 -----
src/robot/real-robot/ptu46/vpRobotPtu46.h | 156 -
src/robot/real-robot/viper/vpRobotViper650.cpp | 2348 -----------
src/robot/real-robot/viper/vpRobotViper650.h | 426 --
src/robot/real-robot/viper/vpRobotViper850.cpp | 2400 -----------
src/robot/real-robot/viper/vpRobotViper850.h | 440 --
src/robot/real-robot/viper/vpViper.cpp | 1302 ------
src/robot/real-robot/viper/vpViper.h | 165 -
src/robot/real-robot/viper/vpViper650.cpp | 876 ----
src/robot/real-robot/viper/vpViper650.h | 149 -
src/robot/real-robot/viper/vpViper850.cpp | 877 ----
src/robot/real-robot/viper/vpViper850.h | 155 -
src/robot/robot/vpRobot.cpp | 283 --
src/robot/robot/vpRobot.h | 165 -
src/robot/robot/vpRobotException.h | 147 -
src/robot/robot/vpRobotTemplate.cpp | 176 -
src/robot/robot/vpRobotTemplate.h | 107 -
.../simulator-robot/arms/CMakeRobotArmsList.cmake | 56 -
src/robot/simulator-robot/vpRobotCamera.cpp | 283 --
src/robot/simulator-robot/vpRobotCamera.h | 142 -
src/robot/simulator-robot/vpRobotSimulator.cpp | 51 -
src/robot/simulator-robot/vpRobotSimulator.h | 101 -
.../simulator-robot/vpRobotWireFrameSimulator.cpp | 375 --
.../simulator-robot/vpRobotWireFrameSimulator.h | 453 --
src/robot/simulator-robot/vpSimulatorAfma6.cpp | 2612 ------------
src/robot/simulator-robot/vpSimulatorAfma6.h | 287 --
src/robot/simulator-robot/vpSimulatorCamera.cpp | 269 --
src/robot/simulator-robot/vpSimulatorCamera.h | 138 -
src/robot/simulator-robot/vpSimulatorPioneer.cpp | 245 --
src/robot/simulator-robot/vpSimulatorPioneer.h | 142 -
.../simulator-robot/vpSimulatorPioneerPan.cpp | 265 --
src/robot/simulator-robot/vpSimulatorPioneerPan.h | 144 -
src/robot/simulator-robot/vpSimulatorViper850.cpp | 2516 -----------
src/robot/simulator-robot/vpSimulatorViper850.h | 314 --
src/servo/vpAdaptiveGain.cpp | 366 --
src/servo/vpAdaptiveGain.h | 175 -
src/servo/vpServo.cpp | 1595 -------
src/servo/vpServo.h | 568 ---
src/servo/vpServoData.cpp | 128 -
src/servo/vpServoData.h | 120 -
src/servo/vpServoDisplay.cpp | 154 -
src/servo/vpServoDisplay.h | 83 -
src/servo/vpServoException.h | 101 -
src/simulator/coin-simulator/vpAR.cpp | 192 -
src/simulator/coin-simulator/vpAR.h | 164 -
.../coin-simulator/vpProjectionDisplay.cpp | 176 -
src/simulator/coin-simulator/vpProjectionDisplay.h | 151 -
src/simulator/coin-simulator/vpSimulator.cpp | 1101 -----
src/simulator/coin-simulator/vpSimulator.h | 307 --
.../coin-simulator/vpSimulatorException.h | 106 -
src/simulator/coin-simulator/vpViewer.cpp | 266 --
src/simulator/coin-simulator/vpViewer.h | 169 -
src/simulator/image-simulator/vpImageSimulator.cpp | 2070 ----------
src/simulator/image-simulator/vpImageSimulator.h | 327 --
src/simulator/ogre-simulator/vpAROgre.cpp | 1072 -----
src/simulator/ogre-simulator/vpAROgre.h | 397 --
src/simulator/wireframe-simulator/core/vpArit.cpp | 827 ----
src/simulator/wireframe-simulator/core/vpArit.h | 214 -
.../wireframe-simulator/core/vpAritio.cpp | 151 -
src/simulator/wireframe-simulator/core/vpBound.cpp | 230 --
src/simulator/wireframe-simulator/core/vpBound.h | 182 -
.../wireframe-simulator/core/vpBoundio.cpp | 218 -
.../wireframe-simulator/core/vpCgiconstants.h | 189 -
src/simulator/wireframe-simulator/core/vpCgidefs.h | 525 ---
.../wireframe-simulator/core/vpClipping.cpp | 489 ---
.../wireframe-simulator/core/vpCoreDisplay.cpp | 240 --
src/simulator/wireframe-simulator/core/vpImstack.h | 58 -
.../wireframe-simulator/core/vpKeyword.cpp | 247 --
src/simulator/wireframe-simulator/core/vpKeyword.h | 54 -
src/simulator/wireframe-simulator/core/vpLex.cpp | 822 ----
src/simulator/wireframe-simulator/core/vpLex.h | 62 -
src/simulator/wireframe-simulator/core/vpMy.h | 94 -
src/simulator/wireframe-simulator/core/vpMyio.cpp | 133 -
.../wireframe-simulator/core/vpParser.cpp | 110 -
.../wireframe-simulator/core/vpProjection.cpp | 243 --
.../wireframe-simulator/core/vpRfstack.cpp | 204 -
src/simulator/wireframe-simulator/core/vpRfstack.h | 61 -
.../wireframe-simulator/core/vpSkipio.cpp | 105 -
src/simulator/wireframe-simulator/core/vpSkipio.h | 55 -
.../wireframe-simulator/core/vpTmstack.cpp | 243 --
src/simulator/wireframe-simulator/core/vpTmstack.h | 66 -
src/simulator/wireframe-simulator/core/vpToken.cpp | 77 -
src/simulator/wireframe-simulator/core/vpToken.h | 100 -
src/simulator/wireframe-simulator/core/vpView.h | 161 -
.../wireframe-simulator/core/vpViewio.cpp | 157 -
.../wireframe-simulator/core/vpVwstack.cpp | 281 --
src/simulator/wireframe-simulator/core/vpVwstack.h | 60 -
.../scene/CMakeWireframeScenesList.cmake | 86 -
.../wireframe-simulator/vpWireFrameSimulator.cpp | 2056 ---------
.../wireframe-simulator/vpWireFrameSimulator.h | 621 ---
src/tools/convert/vpConvert.cpp | 435 --
src/tools/convert/vpConvert.h | 112 -
src/tools/geometry/vpPlane.cpp | 396 --
src/tools/geometry/vpPlane.h | 177 -
src/tools/geometry/vpPolygon.cpp | 412 --
src/tools/geometry/vpPolygon.h | 184 -
src/tools/geometry/vpRect.cpp | 256 --
src/tools/geometry/vpRect.h | 335 --
src/tools/geometry/vpTriangle.cpp | 183 -
src/tools/geometry/vpTriangle.h | 116 -
src/tools/histogram/vpHistogram.cpp | 1126 -----
src/tools/histogram/vpHistogram.h | 307 --
src/tools/histogram/vpHistogramPeak.cpp | 119 -
src/tools/histogram/vpHistogramPeak.h | 174 -
src/tools/histogram/vpHistogramValey.cpp | 99 -
src/tools/histogram/vpHistogramValey.h | 171 -
src/tools/io/vpIoException.h | 103 -
src/tools/io/vpIoTools.cpp | 1407 -------
src/tools/io/vpIoTools.h | 255 --
src/tools/io/vpKeyboard.cpp | 153 -
src/tools/io/vpKeyboard.h | 107 -
src/tools/io/vpParallelPort.cpp | 187 -
src/tools/io/vpParallelPort.h | 107 -
src/tools/io/vpParallelPortException.h | 108 -
src/tools/io/vpParseArgv.cpp | 580 ---
src/tools/io/vpParseArgv.h | 196 -
src/tools/mutex/vpMutex.h | 113 -
src/tools/plot/vpPlot.cpp | 637 ---
src/tools/plot/vpPlot.h | 187 -
src/tools/plot/vpPlotCurve.cpp | 117 -
src/tools/plot/vpPlotCurve.h | 98 -
src/tools/plot/vpPlotGraph.cpp | 1412 -------
src/tools/plot/vpPlotGraph.h | 201 -
src/tools/time/vpTime.cpp | 245 --
src/tools/time/vpTime.h | 92 -
src/tools/trace/vpDebug.h | 554 ---
src/tools/xml/vpXmlParser.cpp | 419 --
src/tools/xml/vpXmlParser.h | 311 --
src/tracking/dots/vpDot.cpp | 972 -----
src/tracking/dots/vpDot.h | 425 --
src/tracking/dots/vpDot2.cpp | 2664 ------------
src/tracking/dots/vpDot2.h | 566 ---
src/tracking/feature-builder/vpFeatureBuilder.h | 181 -
.../feature-builder/vpFeatureBuilderEllipse.cpp | 342 --
.../feature-builder/vpFeatureBuilderLine.cpp | 288 --
.../feature-builder/vpFeatureBuilderPoint.cpp | 347 --
.../feature-builder/vpFeatureBuilderPoint3D.cpp | 93 -
.../feature-builder/vpFeatureBuilderPointPolar.cpp | 402 --
.../feature-builder/vpFeatureBuilderSegment.cpp | 184 -
.../vpFeatureBuilderVanishingPoint.cpp | 155 -
src/tracking/forward-projection/vpCircle.cpp | 359 --
src/tracking/forward-projection/vpCircle.h | 115 -
src/tracking/forward-projection/vpCylinder.cpp | 429 --
src/tracking/forward-projection/vpCylinder.h | 199 -
.../forward-projection/vpForwardProjection.cpp | 146 -
.../forward-projection/vpForwardProjection.h | 241 --
src/tracking/forward-projection/vpLine.cpp | 572 ---
src/tracking/forward-projection/vpLine.h | 219 -
src/tracking/forward-projection/vpPoint.cpp | 320 --
src/tracking/forward-projection/vpPoint.h | 211 -
src/tracking/forward-projection/vpSphere.cpp | 234 --
src/tracking/forward-projection/vpSphere.h | 114 -
src/tracking/general-tracking-issues/vpTracker.cpp | 84 -
src/tracking/general-tracking-issues/vpTracker.h | 111 -
.../general-tracking-issues/vpTrackingException.h | 102 -
src/tracking/klt/vpKltOpencv.cpp | 1289 ------
src/tracking/klt/vpKltOpencv.h | 417 --
src/tracking/mbt/edge/vpMbEdgeTracker.cpp | 2619 ------------
src/tracking/mbt/edge/vpMbEdgeTracker.h | 518 ---
src/tracking/mbt/edge/vpMbtDistanceCircle.cpp | 430 --
src/tracking/mbt/edge/vpMbtDistanceCircle.h | 222 -
src/tracking/mbt/edge/vpMbtDistanceCylinder.cpp | 775 ----
src/tracking/mbt/edge/vpMbtDistanceCylinder.h | 244 --
src/tracking/mbt/edge/vpMbtDistanceLine.cpp | 634 ---
src/tracking/mbt/edge/vpMbtDistanceLine.h | 220 -
src/tracking/mbt/edge/vpMbtMeEllipse.cpp | 359 --
src/tracking/mbt/edge/vpMbtMeEllipse.h | 245 --
src/tracking/mbt/edge/vpMbtMeLine.cpp | 687 ----
src/tracking/mbt/edge/vpMbtMeLine.h | 129 -
src/tracking/mbt/edge/vpMbtXmlParser.cpp | 528 ---
src/tracking/mbt/edge/vpMbtXmlParser.h | 162 -
src/tracking/mbt/hybrid/vpMbEdgeKltTracker.cpp | 1172 ------
src/tracking/mbt/hybrid/vpMbEdgeKltTracker.h | 362 --
src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.cpp | 198 -
src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.h | 98 -
src/tracking/mbt/klt/vpMbKltTracker.cpp | 1199 ------
src/tracking/mbt/klt/vpMbKltTracker.h | 422 --
src/tracking/mbt/klt/vpMbtDistanceKltPoints.cpp | 555 ---
src/tracking/mbt/klt/vpMbtDistanceKltPoints.h | 204 -
src/tracking/mbt/klt/vpMbtKltXmlParser.cpp | 284 --
src/tracking/mbt/klt/vpMbtKltXmlParser.h | 233 --
src/tracking/mbt/vpMbHiddenFaces.h | 746 ----
src/tracking/mbt/vpMbTracker.cpp | 2445 -----------
src/tracking/mbt/vpMbTracker.h | 577 ---
src/tracking/mbt/vpMbXmlParser.cpp | 323 --
src/tracking/mbt/vpMbXmlParser.h | 210 -
src/tracking/mbt/vpMbtPolygon.cpp | 844 ----
src/tracking/mbt/vpMbtPolygon.h | 299 --
src/tracking/moments/vpMoment.cpp | 143 -
src/tracking/moments/vpMoment.h | 140 -
src/tracking/moments/vpMomentAlpha.cpp | 172 -
src/tracking/moments/vpMomentAlpha.h | 239 --
src/tracking/moments/vpMomentArea.cpp | 104 -
src/tracking/moments/vpMomentArea.h | 69 -
src/tracking/moments/vpMomentAreaNormalized.cpp | 116 -
src/tracking/moments/vpMomentAreaNormalized.h | 156 -
src/tracking/moments/vpMomentBasic.cpp | 98 -
src/tracking/moments/vpMomentBasic.h | 88 -
src/tracking/moments/vpMomentCInvariant.cpp | 278 --
src/tracking/moments/vpMomentCInvariant.h | 274 --
src/tracking/moments/vpMomentCentered.cpp | 184 -
src/tracking/moments/vpMomentCentered.h | 136 -
src/tracking/moments/vpMomentCommon.cpp | 222 -
src/tracking/moments/vpMomentCommon.h | 118 -
src/tracking/moments/vpMomentDatabase.cpp | 99 -
src/tracking/moments/vpMomentDatabase.h | 155 -
src/tracking/moments/vpMomentGravityCenter.cpp | 85 -
src/tracking/moments/vpMomentGravityCenter.h | 137 -
.../moments/vpMomentGravityCenterNormalized.cpp | 100 -
.../moments/vpMomentGravityCenterNormalized.h | 76 -
src/tracking/moments/vpMomentObject.cpp | 660 ---
src/tracking/moments/vpMomentObject.h | 299 --
src/tracking/moving-edges/vpMe.cpp | 496 ---
src/tracking/moving-edges/vpMe.h | 346 --
src/tracking/moving-edges/vpMeEllipse.cpp | 1410 -------
src/tracking/moving-edges/vpMeEllipse.h | 393 --
src/tracking/moving-edges/vpMeLine.cpp | 1450 -------
src/tracking/moving-edges/vpMeLine.h | 264 --
src/tracking/moving-edges/vpMeNurbs.cpp | 1397 -------
src/tracking/moving-edges/vpMeNurbs.h | 237 --
src/tracking/moving-edges/vpMeSite.cpp | 738 ----
src/tracking/moving-edges/vpMeSite.h | 263 --
src/tracking/moving-edges/vpMeTracker.cpp | 375 --
src/tracking/moving-edges/vpMeTracker.h | 183 -
.../template-tracker/ssd/vpTemplateTrackerSSD.cpp | 132 -
.../template-tracker/ssd/vpTemplateTrackerSSD.h | 83 -
.../ssd/vpTemplateTrackerSSDESM.cpp | 332 --
.../template-tracker/ssd/vpTemplateTrackerSSDESM.h | 71 -
.../ssd/vpTemplateTrackerSSDForwardAdditional.cpp | 213 -
.../ssd/vpTemplateTrackerSSDForwardAdditional.h | 83 -
.../vpTemplateTrackerSSDForwardCompositional.cpp | 178 -
.../ssd/vpTemplateTrackerSSDForwardCompositional.h | 68 -
.../vpTemplateTrackerSSDInverseCompositional.cpp | 242 --
.../ssd/vpTemplateTrackerSSDInverseCompositional.h | 85 -
.../tools/vpTemplateTrackerBSpline.cpp | 82 -
.../tools/vpTemplateTrackerBSpline.h | 64 -
.../tools/vpTemplateTrackerHeader.h | 91 -
.../tools/vpTemplateTrackerTriangle.cpp | 438 --
.../tools/vpTemplateTrackerTriangle.h | 165 -
.../tools/vpTemplateTrackerZone.cpp | 619 ---
.../template-tracker/tools/vpTemplateTrackerZone.h | 119 -
.../template-tracker/vpTemplateTracker.cpp | 981 -----
src/tracking/template-tracker/vpTemplateTracker.h | 234 --
.../warp/vpTemplateTrackerWarp.cpp | 167 -
.../template-tracker/warp/vpTemplateTrackerWarp.h | 245 --
.../warp/vpTemplateTrackerWarpAffine.cpp | 172 -
.../warp/vpTemplateTrackerWarpAffine.h | 175 -
.../warp/vpTemplateTrackerWarpHomography.cpp | 221 -
.../warp/vpTemplateTrackerWarpHomography.h | 200 -
.../warp/vpTemplateTrackerWarpHomographySL3.cpp | 402 --
.../warp/vpTemplateTrackerWarpHomographySL3.h | 216 -
.../warp/vpTemplateTrackerWarpSRT.cpp | 188 -
.../warp/vpTemplateTrackerWarpSRT.h | 174 -
.../warp/vpTemplateTrackerWarpTranslation.cpp | 129 -
.../warp/vpTemplateTrackerWarpTranslation.h | 175 -
.../zncc/vpTemplateTrackerZNCC.cpp | 134 -
.../template-tracker/zncc/vpTemplateTrackerZNCC.h | 82 -
.../vpTemplateTrackerZNCCForwardAdditional.cpp | 323 --
.../zncc/vpTemplateTrackerZNCCForwardAdditional.h | 66 -
.../vpTemplateTrackerZNCCInverseCompositional.cpp | 439 --
.../vpTemplateTrackerZNCCInverseCompositional.h | 82 -
src/video/vpFFMPEG.cpp | 935 -----
src/video/vpFFMPEG.h | 272 --
src/video/vpVideoReader.cpp | 765 ----
src/video/vpVideoReader.h | 307 --
src/video/vpVideoWriter.cpp | 435 --
src/video/vpVideoWriter.h | 298 --
src/visual-feature/vpBasicFeature.cpp | 202 -
src/visual-feature/vpBasicFeature.h | 173 -
src/visual-feature/vpFeatureDepth.cpp | 510 ---
src/visual-feature/vpFeatureDepth.h | 242 --
src/visual-feature/vpFeatureDisplay.cpp | 429 --
src/visual-feature/vpFeatureDisplay.h | 120 -
src/visual-feature/vpFeatureEllipse.cpp | 489 ---
src/visual-feature/vpFeatureEllipse.h | 156 -
src/visual-feature/vpFeatureException.h | 103 -
src/visual-feature/vpFeatureLine.cpp | 566 ---
src/visual-feature/vpFeatureLine.h | 313 --
src/visual-feature/vpFeatureLuminance.cpp | 386 --
src/visual-feature/vpFeatureLuminance.h | 160 -
src/visual-feature/vpFeatureMoment.cpp | 276 --
src/visual-feature/vpFeatureMoment.h | 242 --
src/visual-feature/vpFeatureMomentAlpha.cpp | 164 -
src/visual-feature/vpFeatureMomentAlpha.h | 176 -
src/visual-feature/vpFeatureMomentArea.cpp | 104 -
src/visual-feature/vpFeatureMomentArea.h | 85 -
.../vpFeatureMomentAreaNormalized.cpp | 196 -
src/visual-feature/vpFeatureMomentAreaNormalized.h | 184 -
src/visual-feature/vpFeatureMomentBasic.cpp | 156 -
src/visual-feature/vpFeatureMomentBasic.h | 98 -
src/visual-feature/vpFeatureMomentCInvariant.cpp | 424 --
src/visual-feature/vpFeatureMomentCInvariant.h | 294 --
src/visual-feature/vpFeatureMomentCentered.cpp | 314 --
src/visual-feature/vpFeatureMomentCentered.h | 114 -
src/visual-feature/vpFeatureMomentCommon.cpp | 89 -
src/visual-feature/vpFeatureMomentCommon.h | 271 --
src/visual-feature/vpFeatureMomentDatabase.cpp | 110 -
src/visual-feature/vpFeatureMomentDatabase.h | 180 -
.../vpFeatureMomentGravityCenter.cpp | 163 -
src/visual-feature/vpFeatureMomentGravityCenter.h | 243 --
.../vpFeatureMomentGravityCenterNormalized.cpp | 221 -
.../vpFeatureMomentGravityCenterNormalized.h | 255 --
src/visual-feature/vpFeaturePoint.cpp | 557 ---
src/visual-feature/vpFeaturePoint.h | 306 --
src/visual-feature/vpFeaturePoint3D.cpp | 631 ---
src/visual-feature/vpFeaturePoint3D.h | 366 --
src/visual-feature/vpFeaturePointPolar.cpp | 646 ---
src/visual-feature/vpFeaturePointPolar.h | 394 --
src/visual-feature/vpFeatureSegment.cpp | 608 ---
src/visual-feature/vpFeatureSegment.h | 400 --
src/visual-feature/vpFeatureThetaU.cpp | 705 ----
src/visual-feature/vpFeatureThetaU.h | 401 --
src/visual-feature/vpFeatureTranslation.cpp | 617 ---
src/visual-feature/vpFeatureTranslation.h | 494 ---
src/visual-feature/vpFeatureVanishingPoint.cpp | 320 --
src/visual-feature/vpFeatureVanishingPoint.h | 151 -
src/visual-feature/vpGenericFeature.cpp | 777 ----
src/visual-feature/vpGenericFeature.h | 242 --
test/CMakeLists.txt | 64 -
test/camera/CMakeLists.txt | 66 -
test/camera/testCameraParametersConversion.cpp | 185 -
test/device/CMakeLists.txt | 51 -
test/device/display/CMakeLists.txt | 81 -
test/device/display/testClick.cpp | 443 --
test/device/display/testDisplayRoi.cpp | 209 -
test/device/display/testDisplays.cpp | 408 --
test/device/display/testMouseEvent.cpp | 593 ---
test/device/display/testVideoDevice.cpp | 485 ---
test/device/display/testVideoDeviceDual.cpp | 360 --
test/device/framegrabber/CMakeLists.txt | 64 -
test/device/framegrabber/test1394TwoGrabber.cpp | 143 -
test/device/framegrabber/test1394TwoResetBus.cpp | 108 -
test/feature/CMakeLists.txt | 66 -
test/feature/testPoint.cpp | 241 --
test/homography/CMakeLists.txt | 66 -
test/homography/testDisplacement.cpp | 237 --
test/image/CMakeLists.txt | 81 -
test/image/testConversion.cpp | 564 ---
test/image/testCreateSubImage.cpp | 267 --
test/image/testImagePoint.cpp | 108 -
test/image/testIoPGM.cpp | 273 --
test/image/testIoPPM.cpp | 314 --
test/image/testReadImage.cpp | 241 --
test/image/testUndistortImage.cpp | 291 --
test/key-point/CMakeLists.txt | 66 -
test/key-point/testSurfKeyPoint.cpp | 386 --
test/math/CMakeLists.txt | 75 -
test/math/testColvector.cpp | 145 -
test/math/testKalmanAcceleration.cpp | 122 -
test/math/testKalmanVelocity.cpp | 142 -
test/math/testMatrix.cpp | 286 --
test/math/testMatrixException.cpp | 160 -
test/math/testMatrixInverse.cpp | 323 --
test/math/testRobust.cpp | 220 -
test/math/testRotation.cpp | 216 -
test/math/testSvd.cpp | 270 --
test/math/testTwistMatrix.cpp | 186 -
test/network/CMakeLists.txt | 64 -
test/network/testClient.cpp | 81 -
test/network/testServer.cpp | 84 -
test/pose/CMakeLists.txt | 69 -
test/pose/testFindMatch.cpp | 127 -
test/pose/testPose.cpp | 280 --
test/pose/testPoseFeatures.cpp | 216 -
test/pose/testPoseRansac.cpp | 140 -
test/servo-afma4/CMakeLists.txt | 67 -
test/servo-afma4/testAfma4.cpp | 75 -
test/servo-afma4/testRobotAfma4.cpp | 80 -
test/servo-afma6/CMakeLists.txt | 73 -
test/servo-afma6/testAfma6.cpp | 112 -
test/servo-afma6/testRobotAfma6.cpp | 117 -
test/servo-afma6/testRobotAfma6Pose.cpp | 240 --
test/servo-viper/CMakeLists.txt | 73 -
test/servo-viper/testRobotViper850.cpp | 126 -
test/servo-viper/testRobotViper850Pose.cpp | 243 --
test/servo-viper/testViper850.cpp | 128 -
test/tools/CMakeLists.txt | 54 -
test/tools/convert/CMakeLists.txt | 68 -
test/tools/convert/testConvert.cpp | 325 --
test/tools/geometry/CMakeLists.txt | 66 -
test/tools/geometry/testPolygon.cpp | 255 --
test/tools/io/CMakeLists.txt | 68 -
test/tools/io/testIoTools.cpp | 446 --
test/tools/time/CMakeLists.txt | 66 -
test/tools/time/testTime.cpp | 195 -
test/tools/xml/CMakeLists.txt | 66 -
test/tools/xml/testXmlParser.cpp | 388 --
test/tracking/CMakeLists.txt | 66 -
test/tracking/testTrackDot.cpp | 319 --
test/visual-feature/CMakeLists.txt | 70 -
test/visual-feature/testFeature.cpp | 103 -
test/visual-feature/testFeatureMoment.cpp | 226 -
test/visual-feature/testFeatureSegment.cpp | 263 --
tutorial/CMakeLists.txt | 41 +-
tutorial/bridge/opencv/CMakeLists.txt | 22 +
tutorial/bridge/opencv/chessboard.pgm | 4 +
tutorial/bridge/opencv/tutorial-bridge-opencv.cpp | 53 +
tutorial/computer-vision/CMakeLists.txt | 2 +-
.../tutorial-homography-from-points.cpp | 14 +-
.../tutorial-pose-from-points-image.cpp | 24 +-
.../tutorial-pose-from-points-tracking.cpp | 35 +-
tutorial/detection/barcode/CMakeLists.txt | 2 +-
.../barcode/tutorial-barcode-detector-live.cpp | 17 +-
.../barcode/tutorial-barcode-detector.cpp | 12 +-
tutorial/detection/face/CMakeLists.txt | 2 +-
.../detection/face/tutorial-face-detector-live.cpp | 13 +-
tutorial/detection/face/tutorial-face-detector.cpp | 10 +-
tutorial/detection/matching/CMakeLists.txt | 2 +-
.../matching/tutorial-matching-keypoint-SIFT.cpp | 8 +-
.../tutorial-matching-keypoint-homography.cpp | 21 +-
.../matching/tutorial-matching-keypoint.cpp | 8 +-
.../matching/tutorial-matching-surf-deprecated.cpp | 6 +-
...utorial-matching-surf-homography-deprecated.cpp | 10 +-
tutorial/detection/object/CMakeLists.txt | 19 +-
tutorial/detection/object/cube.cao | 19 +
tutorial/detection/object/cube.init | 6 +
tutorial/detection/object/cube.mpeg | Bin 0 -> 901120 bytes
tutorial/detection/object/cube.wrl | 34 +
tutorial/detection/object/cube.xml | 47 +
tutorial/detection/object/cube0001.png | Bin 0 -> 74142 bytes
tutorial/detection/object/cube0150.png | Bin 0 -> 80911 bytes
tutorial/detection/object/cube0200.png | Bin 0 -> 84952 bytes
.../object/tutorial-detection-object-mbt.cpp | 20 +-
.../object/tutorial-detection-object-mbt2.cpp | 335 ++
tutorial/grabber/CMakeLists.txt | 2 +-
tutorial/grabber/tutorial-grabber-1394-writer.cpp | 10 +-
tutorial/grabber/tutorial-grabber-1394.cpp | 8 +-
tutorial/grabber/tutorial-grabber-CMU1394.cpp | 6 +-
tutorial/grabber/tutorial-grabber-opencv-bis.cpp | 4 +-
tutorial/grabber/tutorial-grabber-opencv.cpp | 4 +-
tutorial/grabber/tutorial-grabber-v4l2.cpp | 6 +-
tutorial/grabber/tutorial-video-reader.cpp | 10 +-
tutorial/grabber/tutorial-video-recorder.cpp | 14 +-
tutorial/image/CMakeLists.txt | 12 +-
tutorial/image/monkey.bmp | Bin 0 -> 110686 bytes
tutorial/image/monkey.jpeg | Bin 0 -> 33676 bytes
tutorial/image/monkey.pgm | 298 ++
tutorial/image/monkey.png | Bin 0 -> 55379 bytes
tutorial/image/monkey.ppm | 4 +
tutorial/image/tutorial-image-converter.cpp | 12 +-
tutorial/image/tutorial-image-filter.cpp | 14 +-
tutorial/image/tutorial-image-manipulation.cpp | 2 +-
tutorial/image/tutorial-image-reader.cpp | 6 +-
tutorial/image/tutorial-image-viewer.cpp | 18 +-
tutorial/image/tutorial-undistort.cpp | 6 +-
tutorial/image/tutorial-viewer.cpp | 16 +-
tutorial/robot/pioneer/CMakeLists.txt | 2 +-
tutorial/robot/pioneer/tutorial-pioneer-robot.cpp | 2 +-
...orial-simu-pioneer-continuous-gain-adaptive.cpp | 19 +-
...orial-simu-pioneer-continuous-gain-constant.cpp | 19 +-
.../robot/pioneer/tutorial-simu-pioneer-pan.cpp | 19 +-
tutorial/robot/pioneer/tutorial-simu-pioneer.cpp | 19 +-
tutorial/simulator/image/CMakeLists.txt | 2 +-
.../simulator/image/tutorial-image-simulator.cpp | 10 +-
tutorial/trace/CMakeLists.txt | 2 +-
tutorial/trace/tutorial-trace.cpp | 2 +-
tutorial/tracking/blob/CMakeLists.txt | 2 +-
.../tracking/blob/tutorial-blob-auto-tracker.cpp | 10 +-
.../blob/tutorial-blob-tracker-live-firewire.cpp | 21 +-
.../blob/tutorial-blob-tracker-live-v4l2.cpp | 15 +-
tutorial/tracking/keypoint/CMakeLists.txt | 2 +-
.../keypoint/tutorial-klt-tracker-live-v4l2.cpp | 13 +-
.../keypoint/tutorial-klt-tracker-with-reinit.cpp | 8 +-
.../tracking/keypoint/tutorial-klt-tracker.cpp | 8 +-
tutorial/tracking/model-based/edges/CMakeLists.txt | 2 +-
tutorial/tracking/model-based/edges/teabox.xml | 7 +-
.../model-based/edges/tutorial-mb-edge-tracker.cpp | 25 +-
.../tracking/model-based/generic/CMakeLists.txt | 29 +
tutorial/tracking/model-based/generic/teabox.cao | 27 +
tutorial/tracking/model-based/generic/teabox.init | 5 +
tutorial/tracking/model-based/generic/teabox.mpg | Bin 0 -> 239076 bytes
tutorial/tracking/model-based/generic/teabox.ppm | 5 +
tutorial/tracking/model-based/generic/teabox.wrl | 33 +
tutorial/tracking/model-based/generic/teabox.xml | 44 +
.../generic/tutorial-mb-tracker-full.cpp | 213 +
.../model-based/generic/tutorial-mb-tracker.cpp | 159 +
.../tracking/model-based/hybrid/CMakeLists.txt | 2 +-
tutorial/tracking/model-based/hybrid/teabox.xml | 7 +-
.../hybrid/tutorial-mb-hybrid-tracker.cpp | 27 +-
.../tracking/model-based/keypoint/CMakeLists.txt | 2 +-
.../keypoint/tutorial-mb-klt-tracker.cpp | 24 +-
tutorial/tracking/moving-edges/CMakeLists.txt | 2 +-
.../moving-edges/tutorial-me-ellipse-tracker.cpp | 40 +-
.../moving-edges/tutorial-me-line-tracker.cpp | 35 +-
tutorial/tracking/template-tracker/CMakeLists.txt | 2 +-
.../template-tracker/tutorial-template-tracker.cpp | 12 +-
tutorial/visual-servo/ibvs/CMakeLists.txt | 2 +-
.../ibvs/tutorial-ibvs-4pts-display.cpp | 26 +-
.../ibvs/tutorial-ibvs-4pts-image-tracking.cpp | 28 +-
.../ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp | 32 +-
.../visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp | 12 +-
...-ibvs-4pts-plotter-continuous-gain-adaptive.cpp | 8 +-
.../tutorial-ibvs-4pts-plotter-gain-adaptive.cpp | 8 +-
.../ibvs/tutorial-ibvs-4pts-plotter.cpp | 8 +-
.../ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp | 18 +-
.../tutorial-ibvs-4pts-wireframe-robot-afma6.cpp | 22 +-
.../tutorial-ibvs-4pts-wireframe-robot-viper.cpp | 22 +-
tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp | 6 +-
1883 files changed, 279427 insertions(+), 265542 deletions(-)
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..c433c8d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+*~
+*/CMakeFiles
+CMakeCache.txt
+.DS_Store
+CMakeLists.txt.user*
\ No newline at end of file
diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 0000000..2bb9c07
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,56 @@
+language: cpp
+
+os:
+ - linux
+ - osx
+
+env:
+ - VISP_INPUT_IMAGE_PATH=${TRAVIS_BUILD_DIR}
+
+compiler:
+ - gcc
+ - clang
+
+sudo: required
+dist: trusty
+
+before_install:
+ - cd ${TRAVIS_BUILD_DIR} && { curl -O http://visp-doc.inria.fr/download/dataset/ViSP-images-3.0.0.zip ; cd -; }
+ - unzip ${TRAVIS_BUILD_DIR}/ViSP-images-3.0.0.zip -d ${TRAVIS_BUILD_DIR}
+ # Get libs for OSX
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew update; fi"
+ #- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew tap homebrew/science; fi"
+ #- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew install opencv3; fi"
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew install libxml2 libdc1394 gsl; fi"
+
+ # Get libs for Linux
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get update -qq; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq cmake libopencv-dev libx11-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq liblapack-dev libgsl0-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libdc1394-22-dev libv4l-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libcoin80-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libxml2-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libjpeg-dev libpng12-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libswscale-dev libavutil-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libavformat-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libavcodec-dev libbz2-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libogre-1.9-dev libois-dev; fi"
+ - "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libzbar-dev libdmtx-dev; fi"
+
+before_script:
+ - mkdir build
+ - cd build
+ - cmake ..
+ # Show 3rd parties that are detected
+ - cat ViSP-third-party.txt
+
+script:
+ - make
+ - ctest
+
+notifications:
+ email:
+ - Fabien.Spindler at inria.fr
+ on_success: change
+ on_failure: always
+
diff --git a/CMakeHeaderFileList.cmake b/CMakeHeaderFileList.cmake
deleted file mode 100644
index 68a3429..0000000
--- a/CMakeHeaderFileList.cmake
+++ /dev/null
@@ -1,407 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeHeaderFileList.cmake 5255 2015-02-03 14:52:43Z strinh $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP header file list.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set HEADER_subdir variable to all the files we want to parse during
-# the build process.
-# Don't forget to update HEADER_ALL variable if you add/remove a
-# HEADER_subdir variable
-#
-# If you add/remove a directory, modify here
-
-SET (HEADER_CAMERA
- camera/vpCameraParameters.h
- camera/vpMeterPixelConversion.h
- camera/vpPixelMeterConversion.h
- camera/vpXmlParserCamera.h
- camera/calibration/vpCalibration.h
- camera/calibration/vpCalibrationException.h
- )
-
-SET (HEADER_COMPUTER_VISION
- computer-vision/homography-estimation/vpHomography.h
- computer-vision/pose-estimation/vpLevenbergMarquartd.h
- computer-vision/pose-estimation/vpPoseException.h
- computer-vision/pose-estimation/vpPose.h
- computer-vision/pose-estimation/vpPoseFeatures.h
- )
-
-SET (HEADER_DATA_STRUCTURE
- data-structure/vpList.h
- )
-
-set(HEADER_DETECTION
- detection/vpDetectorBase.h
- detection/barcode/vpDetectorDataMatrixCode.h
- detection/barcode/vpDetectorQRCode.h
- detection/face/vpDetectorFace.h
-)
-
-SET (HEADER_EXCEPTION
- exceptions/vpException.h
- )
-
-SET (HEADER_DEVICE_FRAMEGRABBER
- device/framegrabber/1394/vp1394TwoGrabber.h
- device/framegrabber/1394/vp1394CMUGrabber.h
- device/framegrabber/disk/vpDiskGrabber.h
- device/framegrabber/generic-framegrabber/vpFrameGrabberException.h
- device/framegrabber/generic-framegrabber/vpFrameGrabber.h
- device/framegrabber/v4l2/vpV4l2Grabber.h
- device/framegrabber/directshow/vpDirectShowGrabber.h
- device/framegrabber/directshow/vpDirectShowGrabberImpl.h
- device/framegrabber/directshow/vpDirectShowDevice.h
- device/framegrabber/directshow/vpDirectShowSampleGrabberI.h
- device/framegrabber/OpenCV/vpOpenCVGrabber.h
- )
-
-SET (HEADER_IMAGE
- image/vpColor.h
- image/vpImageConvert.h
- image/vpImageException.h
- image/vpImageFilter.h
- image/vpImage.h
- image/vpImageIo.h
- image/vpImageMorphology.h
- image/vpImageTools.h
- image/vpRGBa.h
- image/vpImagePoint.h
- )
-
-SET (HEADER_KEY_POINT
- key-point/vpBasicKeyPoint.h
- key-point/vpKeyPointSurf.h
- key-point/vpPlanarObjectDetector.h
- key-point/vpFernClassifier.h
- key-point/vpKeyPoint.h
- key-point/vpXmlConfigParserKeyPoint.h
- )
-
-SET (HEADER_DEVICE_KINECT
- device/kinect/vpKinect.h
- )
-
-SET (HEADER_DEVICE_LASERSCANNER
- device/laserscanner/vpScanPoint.h
- device/laserscanner/vpLaserScan.h
- device/laserscanner/vpLaserScanner.h
- device/laserscanner/sick/vpSickLDMRS.h
- )
-
-SET (HEADER_DEVICE_LIGHT
- device/light/vpRingLight.h
- )
-
-SET (HEADER_MATH
- math/kalman/vpKalmanFilter.h
- math/kalman/vpLinearKalmanFilterInstantiation.h
- math/matrix/vpColVector.h
- math/matrix/vpMatrixException.h
- math/matrix/vpMatrix.h
- math/matrix/vpRowVector.h
- math/matrix/vpSubMatrix.h
- math/matrix/vpSubColVector.h
- math/matrix/vpSubRowVector.h
- math/matrix/vpGEMM.h
- math/misc/vpMath.h
- math/misc/vpHinkley.h
- math/misc/vpNoise.h
- math/robust/vpRansac.h
- math/robust/vpRobust.h
- math/robust/vpScale.h
- math/spline/vpBSpline.h
- math/spline/vpNurbs.h
- math/transformation/vpExponentialMap.h
- math/transformation/vpForceTwistMatrix.h
- math/transformation/vpHomogeneousMatrix.h
- math/transformation/vpPoseVector.h
- math/transformation/vpQuaternionVector.h
- math/transformation/vpRotationMatrix.h
- math/transformation/vpRotationVector.h
- math/transformation/vpRxyzVector.h
- math/transformation/vpRzyxVector.h
- math/transformation/vpRzyzVector.h
- math/transformation/vpXmlParserHomogeneousMatrix.h
- math/transformation/vpThetaUVector.h
- math/transformation/vpTranslationVector.h
- math/transformation/vpVelocityTwistMatrix.h
- )
-
-SET (HEADER_ROBOT
- robot/robot/vpRobotException.h
- robot/robot/vpRobot.h
- robot/robot/vpRobotTemplate.h
- robot/real-robot/afma4/vpAfma4.h
- robot/real-robot/afma4/vpRobotAfma4.h
- robot/real-robot/afma4/vpServolens.h
- robot/real-robot/afma6/vpAfma6.h
- robot/real-robot/afma6/vpRobotAfma6.h
- robot/real-robot/biclops/vpBiclops.h
- robot/real-robot/biclops/vpRobotBiclopsController.h
- robot/real-robot/biclops/vpRobotBiclops.h
- robot/real-robot/pioneer/vpUnicycle.h
- robot/real-robot/pioneer/vpPioneer.h
- robot/real-robot/pioneer/vpPioneerPan.h
- robot/real-robot/pioneer/vpRobotPioneer.h
- robot/real-robot/ptu46/vpPtu46.h
- robot/real-robot/ptu46/vpRobotPtu46.h
- robot/real-robot/viper/vpViper.h
- robot/real-robot/viper/vpViper650.h
- robot/real-robot/viper/vpViper850.h
- robot/real-robot/viper/vpRobotViper650.h
- robot/real-robot/viper/vpRobotViper850.h
- robot/simulator-robot/vpRobotCamera.h
- robot/simulator-robot/vpRobotSimulator.h
- robot/simulator-robot/vpRobotWireFrameSimulator.h
- robot/simulator-robot/vpSimulatorAfma6.h
- robot/simulator-robot/vpSimulatorCamera.h
- robot/simulator-robot/vpSimulatorPioneer.h
- robot/simulator-robot/vpSimulatorPioneerPan.h
- robot/simulator-robot/vpSimulatorViper850.h
- )
-
-SET (HEADER_SERVO
- servo/vpAdaptiveGain.h
- servo/vpServoData.h
- servo/vpServoDisplay.h
- servo/vpServoException.h
- servo/vpServo.h
- )
-
-SET (HEADER_SIMULATOR
- simulator/coin-simulator/vpAR.h
- simulator/coin-simulator/vpProjectionDisplay.h
- simulator/coin-simulator/vpSimulatorException.h
- simulator/coin-simulator/vpSimulator.h
- simulator/coin-simulator/vpViewer.h
- simulator/ogre-simulator/vpAROgre.h
- simulator/image-simulator/vpImageSimulator.h
- simulator/wireframe-simulator/vpWireFrameSimulator.h
- simulator/wireframe-simulator/core/vpArit.h
- simulator/wireframe-simulator/core/vpBound.h
- simulator/wireframe-simulator/core/vpCgiconstants.h
- simulator/wireframe-simulator/core/vpImstack.h
- simulator/wireframe-simulator/core/vpKeyword.h
- simulator/wireframe-simulator/core/vpLex.h
- simulator/wireframe-simulator/core/vpMy.h
- simulator/wireframe-simulator/core/vpRfstack.h
- simulator/wireframe-simulator/core/vpSkipio.h
- simulator/wireframe-simulator/core/vpTmstack.h
- simulator/wireframe-simulator/core/vpToken.h
- simulator/wireframe-simulator/core/vpView.h
- simulator/wireframe-simulator/core/vpVwstack.h
- )
-
-SET (HEADER_TOOLS
- tools/convert/vpConvert.h
- tools/geometry/vpPlane.h
- tools/geometry/vpRect.h
- tools/geometry/vpTriangle.h
- tools/geometry/vpPolygon.h
- tools/histogram/vpHistogram.h
- tools/histogram/vpHistogramPeak.h
- tools/histogram/vpHistogramValey.h
- tools/io/vpIoException.h
- tools/io/vpIoTools.h
- tools/io/vpKeyboard.h
- tools/io/vpParallelPort.h
- tools/io/vpParallelPortException.h
- tools/io/vpParseArgv.h
- tools/mutex/vpMutex.h
- tools/plot/vpPlot.h
- tools/plot/vpPlotCurve.h
- tools/plot/vpPlotGraph.h
- tools/time/vpTime.h
- tools/trace/vpDebug.h
- tools/xml/vpXmlParser.h
- )
-
-SET (HEADER_TRACKING
- tracking/dots/vpDot2.h
- tracking/dots/vpDot.h
- tracking/feature-builder/vpFeatureBuilder.h
- tracking/forward-projection/vpCircle.h
- tracking/forward-projection/vpCylinder.h
- tracking/forward-projection/vpForwardProjection.h
- tracking/forward-projection/vpLine.h
- tracking/forward-projection/vpPoint.h
- tracking/forward-projection/vpSphere.h
- tracking/general-tracking-issues/vpTracker.h
- tracking/general-tracking-issues/vpTrackingException.h
- tracking/klt/vpKltOpencv.h
- tracking/moving-edges/vpMeEllipse.h
- tracking/moving-edges/vpMe.h
- tracking/moving-edges/vpMeLine.h
- tracking/moving-edges/vpMeSite.h
- tracking/moving-edges/vpMeTracker.h
- tracking/moving-edges/vpMeNurbs.h
-
- tracking/mbt/vpMbTracker.h
- tracking/mbt/vpMbHiddenFaces.h
- tracking/mbt/vpMbXmlParser.h
- tracking/mbt/vpMbtPolygon.h
- tracking/mbt/edge/vpMbtDistanceCircle.h
- tracking/mbt/edge/vpMbtDistanceCylinder.h
- tracking/mbt/edge/vpMbtDistanceLine.h
- tracking/mbt/edge/vpMbtMeEllipse.h
- tracking/mbt/edge/vpMbtMeLine.h
- tracking/mbt/edge/vpMbEdgeTracker.h
- tracking/mbt/edge/vpMbtXmlParser.h
- tracking/mbt/hybrid/vpMbEdgeKltTracker.h
- tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.h
- tracking/mbt/klt/vpMbtDistanceKltPoints.h
- tracking/mbt/klt/vpMbKltTracker.h
- tracking/mbt/klt/vpMbtKltXmlParser.h
-
- tracking/moments/vpMomentAlpha.h
- tracking/moments/vpMomentBasic.h
- tracking/moments/vpMomentCentered.h
- tracking/moments/vpMomentCInvariant.h
- tracking/moments/vpMomentCommon.h
- tracking/moments/vpMoment.h
- tracking/moments/vpMomentDatabase.h
- tracking/moments/vpMomentGravityCenter.h
- tracking/moments/vpMomentGravityCenterNormalized.h
- tracking/moments/vpMomentObject.h
- tracking/moments/vpMomentAreaNormalized.h
- tracking/moments/vpMomentArea.h
-
- tracking/template-tracker/vpTemplateTracker.h
- tracking/template-tracker/ssd/vpTemplateTrackerSSD.h
- tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.h
- tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.h
- tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.h
- tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.h
- tracking/template-tracker/zncc/vpTemplateTrackerZNCC.h
- tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.h
- tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.h
- tracking/template-tracker/tools/vpTemplateTrackerBSpline.h
- tracking/template-tracker/tools/vpTemplateTrackerHeader.h
- tracking/template-tracker/tools/vpTemplateTrackerZone.h
- tracking/template-tracker/tools/vpTemplateTrackerTriangle.h
- tracking/template-tracker/warp/vpTemplateTrackerWarp.h
- tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.h
- tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.h
- tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.h
- tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.h
- tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.h
- )
-
-SET (HEADER_VIDEO
- video/vpVideoReader.h
- video/vpVideoWriter.h
- video/vpFFMPEG.h
- )
-
-SET (HEADER_DEVICE_DISPLAY
- device/display/vpDisplayException.h
- device/display/vpDisplayGTK.h
- device/display/vpDisplayOpenCV.h
- device/display/vpDisplay.h
- device/display/vpDisplayX.h
- device/display/vpMouseButton.h
- device/display/windows/vpDisplayGDI.h
- device/display/windows/vpDisplayWin32.h
- device/display/windows/vpGDIRenderer.h
- device/display/windows/vpWin32Renderer.h
- device/display/windows/vpWin32Window.h
- device/display/windows/vpD3DRenderer.h
- device/display/windows/vpDisplayD3D.h
- device/display/windows/vpWin32API.h
- )
-
-SET (HEADER_VISUAL_FEATURE
- visual-feature/vpBasicFeature.h
- visual-feature/vpFeatureDepth.h
- visual-feature/vpFeatureDisplay.h
- visual-feature/vpFeatureEllipse.h
- visual-feature/vpFeatureException.h
- visual-feature/vpFeatureLine.h
- visual-feature/vpFeatureLuminance.h
- visual-feature/vpFeatureMoment.h
- visual-feature/vpFeatureMomentAlpha.h
- visual-feature/vpFeatureMomentArea.h
- visual-feature/vpFeatureMomentAreaNormalized.h
- visual-feature/vpFeatureMomentBasic.h
- visual-feature/vpFeatureMomentCentered.h
- visual-feature/vpFeatureMomentCInvariant.h
- visual-feature/vpFeatureMomentCommon.h
- visual-feature/vpFeatureMomentDatabase.h
- visual-feature/vpFeatureMomentGravityCenter.h
- visual-feature/vpFeatureMomentGravityCenterNormalized.h
- visual-feature/vpFeaturePoint3D.h
- visual-feature/vpFeaturePoint.h
- visual-feature/vpFeaturePointPolar.h
- visual-feature/vpFeatureThetaU.h
- visual-feature/vpFeatureTranslation.h
- visual-feature/vpFeatureVanishingPoint.h
- visual-feature/vpFeatureSegment.h
- visual-feature/vpGenericFeature.h
- )
-
-SET (HEADER_NETWORK
- network/vpNetwork.h
- network/vpServer.h
- network/vpClient.h
- network/vpRequest.h
- )
-
-SET (HEADER_ALL
- ${HEADER_CAMERA}
- ${HEADER_COMPUTER_VISION}
- ${HEADER_DATA_STRUCTURE}
- ${HEADER_DETECTION}
- ${HEADER_DEVICE_DISPLAY}
- ${HEADER_DEVICE_FRAMEGRABBER}
- ${HEADER_DEVICE_KINECT}
- ${HEADER_DEVICE_LASERSCANNER}
- ${HEADER_DEVICE_LIGHT}
- ${HEADER_EXCEPTION}
- ${HEADER_IMAGE}
- ${HEADER_KEY_POINT}
- ${HEADER_MATH}
- ${HEADER_ROBOT}
- ${HEADER_SIMULATOR}
- ${HEADER_SERVO}
- ${HEADER_TOOLS}
- ${HEADER_TRACKING}
- ${HEADER_VIDEO}
- ${HEADER_VISUAL_FEATURE}
- ${HEADER_NETWORK}
- )
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d10e231..c53d54a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,2359 +1,722 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 5287 2015-02-09 15:29:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file. Detect third party libraries (X11, GTK, ...)
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Need to be befor project(VISP) to work
-if(WIN32)
- set(CMAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/install" CACHE PATH "Installation Directory")
-endif()
-
-project(VISP C CXX)
-
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#-----------------------------------------------------------------------------
-# VISP version number. An even minor number corresponds to releases.
-SET(VISP_VERSION_MAJOR "2")
-SET(VISP_VERSION_MINOR "10")
-SET(VISP_VERSION_PATCH "0")
-SET(VISP_VERSION "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}")
-# Package revision number
-SET(VISP_REVISION "1")
-
-# where are user-specific cmake modules
-set(VISP_CMAKE_MODULE_PATH ${VISP_SOURCE_DIR}/CMakeModules)
-
-find_file(GNU_INSTALL_DIRS_FROM_CMAKE NAMES GNUInstallDirs.cmake PATHS ${CMAKE_ROOT}/Modules)
-mark_as_advanced(GNU_INSTALL_DIRS_FROM_CMAKE)
-if(GNU_INSTALL_DIRS_FROM_CMAKE)
- include(${CMAKE_ROOT}/Modules/GNUInstallDirs.cmake)
-else()
- include(${VISP_CMAKE_MODULE_PATH}/GNUInstallDirs.cmake)
-endif()
-
-list(APPEND CMAKE_MODULE_PATH ${VISP_CMAKE_MODULE_PATH})
-
-# the include directory we depend on
-SET(VISP_INTERN_INCLUDE_DIR ${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR})
-SET(VISP_INCLUDE_DIR "${VISP_INTERN_INCLUDE_DIR}/visp")
-SET(VISP_DOC_DIR "${VISP_BINARY_DIR}/doc")
-
-# The location in which to install VISP libraries.
-SET(LIBRARY_OUTPUT_PATH ${VISP_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
-# The location in which to install some VISP binaries.
-SET(BINARY_OUTPUT_PATH ${VISP_BINARY_DIR}/${CMAKE_INSTALL_BINDIR})
-
-# the third party include directories we depend on
-SET(VISP_EXTERN_INCLUDE_DIRS "")
-# the internal visp libs we depend on
-SET(VISP_INTERN_LIBRARY visp)
-# the third party libraries we depend on
-SET(VISP_EXTERN_LIBRARIES "")
-# the compiler definitions
-SET(VISP_DEFS "")
-
-if(WIN32)
- # Postfix of .lib and .dll
- set(VISP_DEBUG_POSTFIX "d")
- set(VISP_DLLVERSION "${VISP_VERSION_MAJOR}${VISP_VERSION_MINOR}${VISP_VERSION_PATCH}")
-else()
- set(VISP_DEBUG_POSTFIX "")
- set(VISP_DLLVERSION "")
-endif()
-
-# Get the OS
-SET(OS ${CMAKE_SYSTEM_NAME})
-
-SET(OGRE_HOME $ENV{OGRE_HOME})
-if(OGRE_HOME)
- # replace \ with / especially for windows
- STRING(REGEX REPLACE "\\\\" "/" OGRE_HOME ${OGRE_HOME})
-endif()
-
-# add the path to detect Ogre3D
-if(WIN32)
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
-endif(WIN32)
-
-if(UNIX)
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/cmake")
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
- list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/OGRE/cmake")
- list(APPEND CMAKE_MODULE_PATH "/usr/lib/OGRE/cmake")
- list(APPEND CMAKE_MODULE_PATH "/usr/local/lib64/OGRE/cmake")
- list(APPEND CMAKE_MODULE_PATH "/usr/lib64/OGRE/cmake")
- list(APPEND CMAKE_MODULE_PATH "/usr/share/OGRE/cmake/modules")
-endif(UNIX)
-
-# Create include directory which will contain all the headers
-MAKE_DIRECTORY(${VISP_INCLUDE_DIR})
-
-#-----------------------------------------------------------------------------
-# Add extra compilation flags under UNIX
-#-----------------------------------------------------------------------------
-include(${VISP_CMAKE_MODULE_PATH}/AddExtraCompilationFlags.cmake)
-ADD_EXTRA_COMPILATION_FLAGS()
-
-#--------------------------------------------------------------------
-# By default set release configuration
-#--------------------------------------------------------------------
-if(NOT CMAKE_BUILD_TYPE)
- set(CMAKE_BUILD_TYPE "Release" CACHE String "Choose the type of build, options are: None Debug Release" FORCE)
-endif()
-
-#--------------------------------------------------------------------
-# Option management
-#--------------------------------------------------------------------
-
-# Choose static or shared libraries.
-OPTION(BUILD_SHARED_LIBS "Build ViSP shared libraries (.dll/.so) instead of static ones (.lib/.a)." ON)
-# Build examples as an option.
-OPTION(BUILD_EXAMPLES "Build ViSP examples." ON)
-# Build examples as an option.
-OPTION(BUILD_TESTS "Build ViSP tests." ON)
-# Build demos as an option.
-OPTION(BUILD_DEMOS "Build ViSP demos." ON)
-# Build demos as an option.
-OPTION(BUILD_TUTORIALS "Build ViSP tutorials." ON)
-# Build deprecated functions as an option.
-OPTION(BUILD_DEPRECATED_FUNCTIONS "Build deprecated functionalities." ON)
-# Debug and trace cflags
-option(ACTIVATE_DEBUG_TRACE "Enable debug and trace printings" ON)
-
-if(ACTIVATE_DEBUG_TRACE)
- list(APPEND VISP_DEFS "-DVP_TRACE")
- list(APPEND VISP_DEFS "-DVP_DEBUG")
-else()
- string(REPLACE "-DVP_TRACE" "" " " VISP_DEFS "${VISP_DEFS}")
- string(REPLACE "-DVP_DEBUG" "" " " VISP_DEFS "${VISP_DEFS}")
-endif()
-
-if(MSVC)
- option(BUILD_WITH_STATIC_CRT "Enables use of staticaly linked CRT for staticaly linked ViSP" ON)
-endif()
-
-# Note that it is better to set MOMENTS_COMBINE_MATRICES to OFF
-OPTION(MOMENTS_COMBINE_MATRICES "Use linear combination of matrices instead of linear combination of moments to compute interaction matrices." OFF)
-MARK_AS_ADVANCED(MOMENTS_COMBINE_MATRICES)
-
-OPTION(ENABLE_TEST_WITHOUT_DISPLAY "Don't use display features when testing" ON)
-MARK_AS_ADVANCED(ENABLE_TEST_WITHOUT_DISPLAY)
-
-IF(ENABLE_TEST_WITHOUT_DISPLAY)
- SET(OPTION_TO_DESACTIVE_DISPLAY "-d")
-ENDIF()
-
-IF(BUILD_DEPRECATED_FUNCTIONS)
- SET(VISP_BUILD_DEPRECATED_FUNCTIONS TRUE) # for header vpConfig.h
-ENDIF()
-
-IF(MOMENTS_COMBINE_MATRICES)
- SET(VISP_MOMENTS_COMBINE_MATRICES TRUE) # for header vpConfig.h
-ENDIF()
-
-
-IF (UNIX)
- find_package(DC1394)
- if(DC1394_FOUND)
- option(USE_DC1394 "Compile ViSP with the libdc1394.2 library" ON)
- else()
- set(USE_DC1394 OFF)
- endif()
-
- FIND_PACKAGE(V4L2)
- IF(V4L2_FOUND)
- OPTION(USE_V4L2 "Compile ViSP with the v4l2 (video for linux 2) library" ON)
- ELSE(V4L2_FOUND)
- SET(USE_V4L2 OFF)
- ENDIF(V4L2_FOUND)
-
- IF(NOT RAW1394_FOUND)
- FIND_PACKAGE(RAW1394)
- ENDIF(NOT RAW1394_FOUND)
- FIND_PACKAGE(RT)
- FIND_PACKAGE(CALINUX)
- FIND_PACKAGE(IRISA)
- IF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- OPTION(USE_AFMA4 "Compile ViSP for Afma4 robot usage at Irisa" ON)
- OPTION(USE_AFMA6 "Compile ViSP for Afma6 robot usage at Irisa" ON)
- OPTION(USE_VIPER650 "Compile ViSP for Viper S650 robot usage at Irisa" ON)
- OPTION(USE_VIPER850 "Compile ViSP for Viper S850 robot usage at Irisa" ON)
- ELSE(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- SET(USE_AFMA4 OFF)
- SET(USE_AFMA6 OFF)
- SET(USE_VIPER650 OFF)
- SET(USE_VIPER850 OFF)
- ENDIF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
-
- FIND_PACKAGE(BICLOPS)
- IF(BICLOPS_FOUND)
- OPTION(USE_BICLOPS "Compile ViSP for Biclops head usage at Irisa" ON)
- ELSE(BICLOPS_FOUND)
- SET(USE_BICLOPS OFF)
- ENDIF(BICLOPS_FOUND)
-
- FIND_PACKAGE(PTU46)
- IF(PTU46_FOUND)
- OPTION(USE_PTU46 "Compile ViSP for Ptu-46 head usage at Irisa" ON)
- ELSE(PTU46_FOUND)
- SET(USE_PTU46 OFF)
- ENDIF(PTU46_FOUND)
-
-ENDIF(UNIX)
-
-IF(WIN32)
- FIND_PACKAGE(CMU1394)
- IF(CMU1394_FOUND)
- OPTION(USE_CMU1394 "Compile ViSP with the CMU 1394 Digital Camera SDK" ON)
- ELSE(CMU1394_FOUND)
- SET(USE_CMU1394 OFF)
- ENDIF(CMU1394_FOUND)
-
- FIND_PACKAGE(GDI)
- IF(GDI_FOUND)
- OPTION(USE_GDI "Manages the display on windows" ON)
- ELSE(GDI_FOUND)
- SET(USE_GDI OFF)
- ENDIF(GDI_FOUND)
-
- FIND_PACKAGE(DIRECT3D)
- IF(DIRECT3D_FOUND)
- OPTION(USE_DIRECT3D "Manages the display on windows" ON)
- ELSE(DIRECT3D_FOUND)
- SET(USE_DIRECT3D OFF)
- ENDIF(DIRECT3D_FOUND)
-
- FIND_PACKAGE(DIRECTSHOW)
- IF(DIRECTSHOW_FOUND)
- OPTION(USE_DIRECTSHOW "Manages the frame grabbing on windows" ON)
- ELSE(DIRECTSHOW_FOUND)
- SET(USE_DIRECTSHOW OFF)
- ENDIF(DIRECTSHOW_FOUND)
-
-ENDIF(WIN32)
-
-include(FindOpenMP)
-if(OPENMP_FOUND)
- OPTION(USE_OPENMP "Add C++ compiler flags for OpenMP parallization" ON)
-else(OPENMP_FOUND)
- SET(USE_OPENMP OFF)
-endif(OPENMP_FOUND)
-
-include(FindCPP11)
-if(CPP11_FOUND)
- OPTION(USE_CPP11 "Add C++ compiler flags for C++11 support" OFF)
-else()
- SET(USE_CPP11 OFF)
-endif()
-
-# Since the FindLAPACK.cmake provided with CMake is for Fortran language,
-# in CMakeModules we have added FindLAPACK_C.cmake for C language
-FIND_PACKAGE(LAPACK_C)
-IF(LAPACK_FOUND)
- OPTION(USE_LAPACK "Compile ViSP with the lapack/blas libraries" ON)
-ELSE(LAPACK_FOUND)
- SET(USE_LAPACK OFF)
-ENDIF(LAPACK_FOUND)
-
-#IF(NOT USE_LAPACK) # line removed since the template tracker needs gsl
- find_package(GSL)
- if(GSL_FOUND)
- option(USE_GSL "Compile ViSP with the GSL library" ON)
- else()
- set(USE_GSL OFF)
- endif()
-#ENDIF()
-
-FIND_PACKAGE(OpenGL)
-FIND_PACKAGE(Coin3D)
-if (NOT COIN3D_FOUND)
- FIND_PACKAGE(MyCoin3D)
-endif()
-IF (COIN3D_FOUND AND OPENGL_FOUND)
- OPTION(USE_COIN "Compile ViSP with Coin3D" ON)
-ELSE (COIN3D_FOUND AND OPENGL_FOUND)
- SET(USE_COIN OFF)
-ENDIF (COIN3D_FOUND AND OPENGL_FOUND)
-
-FIND_PACKAGE(YARP QUIET)
-MARK_AS_ADVANCED(YARP_DIR)
-IF (YARP_FOUND)
- OPTION(USE_YARP "Compile ViSP with YARP" ON)
-ELSE (YARP_FOUND)
- SET(USE_YARP OFF)
-ENDIF ()
-
-FIND_PACKAGE(OGRE QUIET)
-MARK_AS_ADVANCED(OGRE_DIR)
-IF (OGRE_FOUND)
- OPTION(USE_OGRE "Compile ViSP with Ogre3D engine and renderer" ON)
-ELSE ()
- SET(USE_OGRE OFF)
-ENDIF ()
-
-FIND_PACKAGE(OIS QUIET)
-MARK_AS_ADVANCED(OIS_DIR)
-IF (OGRE_FOUND AND OIS_FOUND)
- OPTION(USE_OIS "Compile ViSP with OIS (Object Oriented Input System library)" ON)
-ELSE ()
- SET(USE_OIS OFF)
-ENDIF ()
-
-FIND_PACKAGE(LIBFREENECT)
-IF(LIBFREENECT_FOUND)
- OPTION(USE_LIBFREENECT "Compile ViSP with the libfreenect library" ON)
-ELSE(LIBFREENECT_FOUND)
- SET(USE_LIBFREENECT OFF)
-ENDIF(LIBFREENECT_FOUND)
-
-FIND_PACKAGE(LIBUSB-1)
-IF(LIBUSB_1_FOUND)
- OPTION(USE_LIBUSB_1 "Compile ViSP with the libusb-1.0 library" ON)
-ELSE(LIBUSB_1_FOUND)
- SET(USE_LIBUSB_1 OFF)
-ENDIF(LIBUSB_1_FOUND)
-
-IF(WIN32 AND USE_COIN)
- FIND_PACKAGE(SOWIN)
- IF (SOWIN_FOUND)
- OPTION(USE_SOWIN "Compile ViSP with SoWin" OFF)
- ELSE (SOWIN_FOUND)
- SET(USE_SOWIN OFF)
- ENDIF (SOWIN_FOUND)
-ENDIF(WIN32 AND USE_COIN)
-
-IF(USE_COIN)
- FIND_PACKAGE(SOQT)
- IF (SOQT_FOUND)
- OPTION(USE_SOQT "Compile ViSP with SoQt" OFF)
- ELSE (SOQT_FOUND)
- SET(USE_SOQT OFF)
- ENDIF (SOQT_FOUND)
- IF(USE_SOQT)
- INCLUDE (${CMAKE_ROOT}/Modules/FindQt.cmake)
- IF (QT_FOUND)
- OPTION(USE_QT "Compile ViSP with Qt" ON)
- ELSE (QT_FOUND)
- SET(USE_QT OFF)
- ENDIF (QT_FOUND)
- ENDIF (USE_SOQT)
-ENDIF(USE_COIN)
-
-IF(USE_COIN)
- FIND_PACKAGE(SOXT)
- IF (SOXT_FOUND)
- OPTION(USE_SOXT "Compile ViSP with SoXt" OFF)
- ELSE (SOXT_FOUND)
- SET(USE_SOXT OFF)
- ENDIF (SOXT_FOUND)
-ENDIF(USE_COIN)
-
-FIND_PACKAGE(PTHREAD)
-IF (PTHREAD_FOUND)
- OPTION(USE_PTHREAD "Compile ViSP with pthread" ON)
-ELSE ()
- SET(USE_PTHREAD OFF)
-ENDIF ()
-
-FIND_PACKAGE(XML2)
-IF(XML2_FOUND)
- OPTION(USE_XML2 "Compile ViSP with the xml2 library" ON)
-ELSE(XML2_FOUND)
- SET(USE_XML2 OFF)
-ENDIF(XML2_FOUND)
-
-FIND_PACKAGE(OpenCV QUIET)
-MARK_AS_ADVANCED(OpenCV_DIR OpenCV_FOUND OPENCV_FOUND)
-IF(OpenCV_FOUND)
- OPTION(USE_OPENCV "Compile ViSP with OpenCV >= 2.1 library" ON)
-ELSE(OpenCV_FOUND)
- FIND_PACKAGE(OpenCV2 QUIET)
- IF(OpenCV_FOUND)
- OPTION(USE_OPENCV "Compile ViSP with OpenCV <= 2.0 library" ON)
- ELSE(OpenCV_FOUND)
- SET(USE_OPENCV OFF)
- ENDIF(OpenCV_FOUND)
-ENDIF(OpenCV_FOUND)
-
-FIND_PACKAGE(ZLIB)
-if(ZLIB_FOUND)
- OPTION(USE_ZLIB "Compile ViSP with zlib library" ON)
-else()
- FIND_PACKAGE(MyZLIB)
- if(ZLIB_FOUND)
- option(USE_ZLIB "Compile ViSP with zlib library" ON)
- else()
- set(USE_ZLIB OFF)
- endif()
-endif()
-
-FIND_PACKAGE(X11)
-IF(X11_FOUND)
- OPTION(USE_X11 "Compile ViSP with the X11 library" ON)
-ELSE(X11_FOUND)
- SET(USE_X11 OFF)
-ENDIF(X11_FOUND)
-
-# The native FindGTK2.cmake doesn't consider libgobject-2.0 that is
-# requested by ViSP. That's why we use our FindMyGTK2.cmake
-#FIND_PACKAGE(GTK2)
-#mark_as_advanced(GTK2_ATK_INCLUDE_DIR)
-#mark_as_advanced(GTK2_CAIRO_INCLUDE_DIR)
-#mark_as_advanced(GTK2_FONTCONFIG_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GDK_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GDK_PIXBUF_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GDKCONFIG_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GLIB_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GLIBCONFIG_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GOBJECT_INCLUDE_DIR)
-#mark_as_advanced(GTK2_GTK_INCLUDE_DIR)
-#mark_as_advanced(GTK2_PANGO_INCLUDE_DIR)
-#IF(GTK2_FOUND)
-# OPTION(USE_GTK2 "Compile ViSP with the gtk2 library" ON)
-#ELSE(GTK2_FOUND)
- FIND_PACKAGE(MyGTK2)
- if(GTK2_FOUND)
- OPTION(USE_GTK2 "Compile ViSP with the gtk2 library" OFF)
- else(GTK2_FOUND)
- SET(USE_GTK2 OFF)
- endif(GTK2_FOUND)
-#ENDIF(GTK2_FOUND)
-
-FIND_PACKAGE(JPEG)
-if(JPEG_FOUND)
- OPTION(USE_LIBJPEG "Compile ViSP with the libjpeg library" ON)
-else(JPEG_FOUND)
- FIND_PACKAGE(MyJPEG)
- if(JPEG_FOUND)
- OPTION(USE_LIBJPEG "Compile ViSP with the libjpeg library" ON)
- else(JPEG_FOUND)
- SET(USE_LIBJPEG OFF)
- endif()
-endif()
-
-FIND_PACKAGE(PNG)
-if(PNG_FOUND)
- option(USE_LIBPNG "Compile ViSP with the libpng library" ON)
-else(PNG_FOUND)
- FIND_PACKAGE(MyPNG)
- if(PNG_FOUND)
- OPTION(USE_LIBPNG "Compile ViSP with the libpng library" ON)
- else(PNG_FOUND)
- SET(USE_LIBPNG OFF)
- endif()
-endif()
-
-find_package(FFMPEG)
-if(FFMPEG_FOUND)
- if(USE_OPENCV)
- option(USE_FFMPEG "Compile ViSP with the ffmpeg library" OFF)
- else()
- option(USE_FFMPEG "Compile ViSP with the ffmpeg library" ON)
- endif()
-else()
- set(USE_FFMPEG OFF)
-endif()
-
-# To control Pioneer mobile robots, under UNIX we need Aria, pthread, rt and dl 3rd party libraries
-find_package(ARIA)
-if(ARIA_FOUND)
- OPTION(USE_ARIA "Compile ViSP with Aria library to control Pioneer mobile robots" ON)
-else()
- SET(USE_ARIA OFF)
-endif()
-
-find_package(RT)
-if(RT_FOUND)
- OPTION(USE_RT "Compile ViSP with rt library" ON)
-else()
- SET(USE_RT OFF)
-endif()
-find_package(DL)
-if(DL_FOUND)
- OPTION(USE_DL "Compile ViSP with dl library" ON)
-else()
- SET(USE_DL OFF)
-endif()
-
-find_package(ZBAR)
-if(ZBAR_FOUND)
- OPTION(USE_ZBAR "Compile ViSP with zbar library" ON)
-else()
- SET(USE_ZBAR OFF)
-endif()
-
-find_package(DMTX)
-if(DMTX_FOUND)
- OPTION(USE_DMTX "Compile ViSP with zbar library" ON)
-else()
- SET(USE_DMTX OFF)
-endif()
-
-# Set other options to default value
-#SET(USE_X11 ON) # For Linux/OSX display
-#SET(USE_XML2 ON) # For the Xml Parser (vpCameraParameters)
-#SET(USE_COIN ON) # For the simulator
-#SET(USE_SOWIN ON) # For the simulator viewer
-#SET(USE_SOQT ON) # For the simulator viewer
-#SET(USE_SOXT ON) # For the simulator viewer
-#SET(USE_PTHREAD ON) # For Biclops/Simulator
-#SET(USE_OPENCV ON) # For Intel opencv
-#SET(USE_GSL ON) # For matrix manipulations
-#SET(USE_GTK2 ON) # For Linux/OSX/Windows display with gtk-2.x
-#SET(USE_DIRECT3D ON) # For Windows display
-#SET(USE_GDI ON) # For Windows display
-#SET(USE_DC1394 ON) # For firewire grabber under Linux and OSX ?
-#SET(USE_V4L2 ON) # For Video 4 Linux 2 grabber under Linux
-#SET(USE_DIRECTSHOW ON) # For Windows direct show grabber
-#SET(USE_LIBJPEG ON) # For reading jpeg files
-#SET(USE_LIBPNG ON) # For reading png files
-#SET(USE_ZLIB ON) # For data compression
-#SET(USE_YARP ON) # For YARP
-#SET(USE_FFMPEG ON) # For video read and write
-
-IF(BUILD_SHARED_LIBS)
- SET(VISP_BUILD_SHARED_LIBS TRUE) # for header vpConfig.h
-ENDIF(BUILD_SHARED_LIBS)
-
-#----------------------------------------------------------------------
-# Try to find doxygen for documentation generation
-# Use "make visp_doc" target to generate the documentation
-#----------------------------------------------------------------------
-find_package(Doxygen)
-if(DOXYGEN_FOUND)
- set(VISP_HAVE_DOXYGEN "yes") # for header vpConfig.h
- set(VISP_HAVE_DOXYGEN_FOUND "yes") # for ViSP-third-party.txt
- if(DOXYGEN_DOT_EXECUTABLE)
- set(VISP_HAVE_DOT "yes") # for header vpConfig.h
- set(VISP_HAVE_DOT_FOUND "yes") # for ViSP-third-party.txt
- else()
- set(VISP_HAVE_DOT "no") # for header vpConfig.h
- set(VISP_HAVE_DOT_FOUND "no") # for ViSP-third-party.txt
- endif()
- ## we need latex for doxygen because of the formulas
- find_package(LATEX)
- if(NOT LATEX_COMPILER)
- message(STATUS "latex command LATEX_COMPILER not found but usually required. You will probably get warnings and user interaction on doxy run.")
- endif()
- if(NOT MAKEINDEX_COMPILER)
- message(STATUS "makeindex command MAKEINDEX_COMPILER not found but usually required.")
- endif()
- if(NOT DVIPS_CONVERTER)
- message(STATUS "dvips command DVIPS_CONVERTER not found but usually required.")
- endif()
-
- configure_file(${VISP_SOURCE_DIR}/doc/config-doxygen.in
- ${VISP_DOC_DIR}/config-doxygen
- @ONLY )
-
- configure_file(${VISP_SOURCE_DIR}/doc/mainpage.doc.in
- ${VISP_DOC_DIR}/mainpage.doc
- @ONLY )
-else()
- set(VISP_HAVE_DOXYGEN "no") # for header vpConfig.h
- set(VISP_HAVE_DOXYGEN_FOUND "no") # for ViSP-third-party.txt
- set(VISP_HAVE_DOT_FOUND "no") # for ViSP-third-party.txt
-endif()
-
-# ----------------------------------------------------------------------------
-# Solution folders:
-# ----------------------------------------------------------------------------
-if(MSVC_IDE OR CMAKE_GENERATOR MATCHES Xcode)
- option(ENABLE_SOLUTION_FOLDERS "Solution folder in Visual Studio or in other IDEs" ON)
-endif()
-
-if(ENABLE_SOLUTION_FOLDERS)
- set_property(GLOBAL PROPERTY USE_FOLDERS ON)
- set_property(GLOBAL PROPERTY PREDEFINED_TARGETS_FOLDER "CMakeTargets")
-endif()
-
-# Extra ViSP targets: uninstall, etc.
-include(${VISP_CMAKE_MODULE_PATH}/VISPExtraTargets.cmake)
-
-
-INCLUDE(CheckLibraryExists)
-
-#--------------------------------------------------------------------
-# yarp lib
-#--------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_YARP_FOUND "no") # for ViSP-third-party.txt
-
-IF(USE_YARP)
- IF(YARP_FOUND)
- MESSAGE(STATUS "yarp found")
- SET(VISP_HAVE_YARP TRUE) # for header vpConfig.h
- SET(VISP_HAVE_YARP_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${YARP_INCLUDE_DIRS})
-
- # Work around to add Yarp libraries and also third party libraries requested by Yarp
- LIST(REVERSE YARP_LIBRARIES) # to start with YARP_init, that depends on YARP_dev, YARP_sig and YARP_OS
- FOREACH(lib ${YARP_LIBRARIES})
- #MESSAGE("lib: ${lib}")
- GET_TARGET_PROPERTY(CONFIGURATIONS ${lib} IMPORTED_CONFIGURATIONS)
- FOREACH(CONFIGURATION ${CONFIGURATIONS})
- GET_TARGET_PROPERTY(YARP_LIB ${lib} "IMPORTED_LOCATION_${CONFIGURATION}") # Get Yarp full absolute library path and name
-
- IF(WIN32)
- #Work around when YARP is build as shared libraries
- STRING(REGEX REPLACE ".dll$" ".lib" YARP_LIB ${YARP_LIB})
-
- IF(${CONFIGURATION} STREQUAL "RELEASE")
- LIST(APPEND VISP_EXTERN_LIBRARIES optimized ${YARP_LIB}) # Append full absolute library path and name
- ELSEIF(${CONFIGURATION} STREQUAL "DEBUG")
- LIST(APPEND VISP_EXTERN_LIBRARIES debug ${YARP_LIB}) # Append full absolute library path and name
- ENDIF()
- ELSE()
- LIST(APPEND VISP_EXTERN_LIBRARIES ${YARP_LIB}) # Append full absolute library path and name
- ENDIF()
- # Get 3rd party libraries requested by Yarp
- GET_TARGET_PROPERTY(YARP_LINK_LIBS_ ${lib} "IMPORTED_LINK_INTERFACE_LIBRARIES_${CONFIGURATION}")
- LIST(APPEND YARP_LINK_LIBS ${YARP_LINK_LIBS_})
- ENDFOREACH()
- ENDFOREACH()
-
- # Remove Yarp libraries since they were added previously with full absolute library path and name
- if(YARP_LINK_LIBS)
- foreach(lib ${YARP_LIBRARIES})
- list(REMOVE_ITEM YARP_LINK_LIBS ${lib})
- endforeach()
- endif()
- # Add 3rd party libraries requested by Yarp
- LIST(APPEND VISP_EXTERN_LIBRARIES ${YARP_LINK_LIBS})
-
- LIST(APPEND VISP_DEFS ${YARP_DEFINES})
- ELSE()
- MESSAGE(STATUS "yarp not found")
- SET(VISP_HAVE_YARP_FOUND "no") # for ViSP-third-party.txt
- ENDIF()
-ENDIF(USE_YARP)
-
-#---------------------------------------------------------------------
-# Try to determine if Inria's NAS server hosting /udd/ is available
-#----------------------------------------------------------------------
-FIND_PACKAGE(NAS)
-IF(NAS_FOUND)
- SET(VISP_HAVE_ACCESS_TO_NAS TRUE) # for header vpConfig.h
-ENDIF(NAS_FOUND)
-
-#---------------------------------------------------------------------
-# Platform dependent
-#----------------------------------------------------------------------
-IF(WIN32 AND NOT CYGWIN)
- if(BUILD_DEPRECATED_FUNCTIONS)
- list(APPEND VISP_DEFS "-DWIN32") # only for compat with previous versions
- endif()
- CHECK_LIBRARY_EXISTS("winmm.lib" getch "" HAVE_LIBWINMM) # for timeGetTime()
- IF(HAVE_LIBWINMM)
- #MESSAGE("have winmm.lib")
- LIST(APPEND VISP_EXTERN_LIBRARIES "winmm.lib")
- ENDIF(HAVE_LIBWINMM)
- if(MSVC) #check if MSVC is the compiler and set VISP_USE_MSVC accordingly
- SET(VISP_USE_MSVC TRUE)
- endif()
- # Add library ws2_32.a or ws2_32.lib for vpNetwork class
- if(MINGW)
- CHECK_LIBRARY_EXISTS("ws2_32.a" getch "" HAVE_LIBWS2_32) # for inet_ntoa() and socket functionalities
- if(HAVE_LIBWS2_32)
- list(APPEND VISP_EXTERN_LIBRARIES "ws2_32.a")
- else()
- find_library(WS2_32_LIBRARY "libws2_32.a"
- "$ENV{MINGW_DIR}/lib"
- "$ENV{MINGW_DIR}/mingw/lib"
- C:/mingw/mingw/lib)
- mark_as_advanced(WS2_32_LIBRARY)
- if(WS2_32_LIBRARY)
- list(APPEND VISP_EXTERN_LIBRARIES "${WS2_32_LIBRARY}")
- endif()
- endif()
- else() # pure WIN32
- CHECK_LIBRARY_EXISTS("ws2_32.lib" getch "" HAVE_LIBWS2_32) # for inet_ntoa() and socket functionalities
- if(HAVE_LIBWS2_32)
- #message("have ws2_32.lib")
- list(APPEND VISP_EXTERN_LIBRARIES "ws2_32.lib")
- endif()
- endif()
-ENDIF(WIN32 AND NOT CYGWIN)
-if(UNIX)
- if(BUILD_DEPRECATED_FUNCTIONS)
- list(APPEND VISP_DEFS "-DUNIX")
- if(APPLE)
- list(APPEND VISP_DEFS "-DAPPLE")
- endif()
- if(${CMAKE_SYSTEM_NAME} MATCHES "SunOS")
- list(APPEND VISP_DEFS "-DSOLARIS")
- endif()
- endif()
-endif()
-
-#--------------------------------------------------------------------
-# OpenMP
-#--------------------------------------------------------------------
-if(USE_OPENMP)
- set(VISP_OPENMP_FLAGS "${OpenMP_CXX_FLAGS}")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-else()
- set(VISP_OPENMP_FLAGS "")
- if(OpenMP_CXX_FLAGS)
- string(REPLACE ${OpenMP_CXX_FLAGS} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
-endif()
-
-#--------------------------------------------------------------------
-# C++11
-#--------------------------------------------------------------------
-if(USE_CPP11)
- set(VISP_CPP11_FLAGS "${CPP11_CXX_FLAGS}")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CPP11_CXX_FLAGS}")
-else()
- set(VISP_CPP11_FLAGS "")
- if(CPP11_CXX_FLAGS)
- string(REPLACE ${CPP11_CXX_FLAGS} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
-endif()
-
-# Remove duplicates compilation flags
-separate_arguments(CMAKE_CXX_FLAGS)
-list(REMOVE_DUPLICATES CMAKE_CXX_FLAGS)
-string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}" CACHE STRING "common C++ build flags" FORCE)
-
-#--------------------------------------------------------------------
-# LAPACK lib
-#--------------------------------------------------------------------
-SET(VISP_HAVE_LAPACK_FOUND "no") # for ViSP-third-party.txt
-IF(USE_LAPACK)
- MESSAGE(STATUS "lapack/blas found")
- SET(VISP_HAVE_LAPACK TRUE) # for header vpConfig.h
- SET(VISP_HAVE_LAPACK_FOUND "yes") # for ViSP-third-party.txt
-
- #MESSAGE("LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}")
- LIST(APPEND VISP_EXTERN_LIBRARIES ${LAPACK_LIBRARIES})
-ENDIF(USE_LAPACK)
-
-#--------------------------------------------------------------------
-# X11 lib
-#--------------------------------------------------------------------
-set(VISP_HAVE_X11_FOUND "no") # for ViSP-third-party.txt
-if(USE_X11)
- message(STATUS "X11 found")
- set(VISP_HAVE_X11 TRUE) # for header vpConfig.h
- set(VISP_HAVE_X11_FOUND "yes") # for ViSP-third-party.txt
- # try to found -lm requested on some platforms to link with X11
- list(APPEND VISP_EXTERN_INCLUDE_DIRS ${X11_INCLUDE_DIR})
- list(APPEND VISP_EXTERN_LIBRARIES ${X11_LIBRARIES})
- find_library(M_LIBRARY NAMES m)
- mark_as_advanced(M_LIBRARY)
- if(M_LIBRARY)
- list(APPEND VISP_EXTERN_LIBRARIES ${M_LIBRARY})
- endif()
- #MESSAGE("X11: ${X11_LIBRARIES}")
-else()
- # try to found -lsocket -lnsl requested for vpNetwork and vpSickLDMRS
- find_library(SOCKET_LIBRARY NAMES socket)
- find_library(NSL_LIBRARY NAMES nsl)
- if (SOCKET_LIBRARY)
- list(APPEND VISP_EXTERN_LIBRARIES ${SOCKET_LIBRARY})
- endif()
- if (NSL_LIBRARY)
- list(APPEND VISP_EXTERN_LIBRARIES ${NSL_LIBRARY})
- endif()
- mark_as_advanced(SOCKET_LIBRARY NSL_LIBRARY)
-endif()
-
-
-#--------------------------------------------------------------------
-# OpenGL, Qt, SoQt and Coin libs
-#--------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_SIMULATOR_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_COIN_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_OGRE_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_OIS_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_SOWIN_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_SOXT_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_SOQT_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_QT4_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_QT3_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_OPENGL_FOUND "no") # for ViSP-third-party.txt
-
-IF(USE_OGRE)
- # Find Boost
- if (NOT OGRE_BUILD_PLATFORM_IPHONE)
- if (WIN32 OR APPLE)
- set(Boost_USE_STATIC_LIBS TRUE)
- else ()
- # Statically linking boost to a dynamic Ogre build doesn't work on Linux 64bit
- set(Boost_USE_STATIC_LIBS ${OGRE_STATIC})
- endif ()
- if (MINGW)
- # this is probably a bug in CMake: the boost find module tries to look for
- # boost libraries with name libboost_*, but CMake already prefixes library
- # search names with "lib". This is the workaround.
- set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} "")
- endif ()
- #set(Boost_ADDITIONAL_VERSIONS "1.53.0" "1.52.0" "1.51.0" "1.50.0" "1.49.0" "1.48.0" "1.47.0" "1.46.0" "1.45.0" "1.44.0" "1.44" "1.44.0" "1.42" "1.42.0" "1.41.0" "1.41" "1.40.0" "1.40" "1.39.0" "1.39" "1.38.0" "1.38" "1.37.0" "1.37" )
- # Components that need to be linked with. Since Ogre 1.9 we need not only boost_thread and boost_date_time, but also boost_system
- set(OGRE_BOOST_COMPONENTS thread system date_time)
- if(WIN32)
- list(APPEND OGRE_BOOST_COMPONENTS chrono)
- endif()
- find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
- mark_as_advanced(Boost_LIB_DIAGNOSTIC_DEFINITIONS Boost_DIR BOOST_THREAD_LIBRARY)
- if (NOT Boost_FOUND)
- set(OGRE_BOOST_COMPONENTS thread date_time)
- if(WIN32)
- list(APPEND OGRE_BOOST_COMPONENTS chrono)
- endif()
- find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
- endif()
- if (NOT Boost_FOUND)
- # Try again with the other type of libs
- set(Boost_USE_STATIC_LIBS NOT ${Boost_USE_STATIC_LIBS})
- find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
- endif()
-
- if (Boost_FOUND)
- # Set up referencing of Boost
- #LIST(APPEND VISP_DEFS "-DBOOST_ALL_NO_LIB")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${Boost_INCLUDE_DIR})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${Boost_LIBRARIES})
- endif()
- endif()
-
- MARK_AS_ADVANCED(OGRE_SAMPLES_INCLUDEPATH)
- #message("OGRE_SAMPLES_INCLUDEPATH: ${OGRE_SAMPLES_INCLUDEPATH}")
- if(OGRE_SAMPLES_INCLUDEPATH)
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${OGRE_SAMPLES_INCLUDEPATH})
- endif()
-
- # if OGRE_MEDIA_DIR is empty, try to find the path by searching for plugins.cfg
- # Introduced since Ubuntu 12.04
- if(NOT OGRE_MEDIA_DIR)
- FIND_PATH(OGRE_MEDIA_DIR ../plugins.cfg
- $ENV{OGRE_MEDIA_DIR}
- /usr/share/OGRE-1.7.4/media
- /usr/share/OGRE-1.8.0/media
- /usr/share/OGRE-1.8.1/media
- /usr/share/OGRE-1.9.0/media
- )
- endif()
-
- #message("OGRE_PLUGIN_DIR_REL: ${OGRE_PLUGIN_DIR_REL}")
- #message("OGRE_PLUGIN_DIR_DBG: ${OGRE_PLUGIN_DIR_DBG}")
- SET(VISP_HAVE_OGRE_FOUND "yes") # for ViSP-third-party.txt
- SET(VISP_HAVE_OGRE TRUE) # for header vpConfig.h
-
- # If Ogre media are not available we provide the minimal material to run the examples:
- # - resources.cfg
- # - plugins.cfg
- # - media/materials/...
- # - media/models/...
- #
- # We need to introduce OGRE_MEDIA_NOT_AVAILABLE to memorize when OGRE_MEDIA_DIR is not set.
- # Because in that case, OGRE_MEDIA_DIR should be set first to VISP_HAVE_OGRE_RESOURCES_PATH
- # (for the "make all" case) then to VISP_INSTALL_DIR_OGRE_RESOURCES (for the "make install" case)
- if(NOT OGRE_MEDIA_DIR)
- set(OGRE_MEDIA_NOT_AVAILABLE "TRUE")
- endif()
-
- # Try to search for an existing plugins.cfg file
- # Here we cannot use OGRE_PLUGIN_DIR_REL or OGRE_PLUGIN_DIR_DBG where
- # we may find an existing plugins.cfg file, since under Windows in these
- # files the PluginFolder is set to a relative path. We need an absolute
- # path to avoid recopy of the plugins in ViSP.
- # Under Linux or OSX, we may find plugins.cfg with a PluginFolder set
- # to an absolute path in OGRE_MEDIA_DIR/..
- FIND_PATH(VISP_HAVE_OGRE_PLUGINS_PATH
- NAMES plugins.cfg
- PATHS ${OGRE_MEDIA_DIR}/..
- NO_SYSTEM_ENVIRONMENT_PATH
- )
-
- # If no plugins.cfg file is found, we create one with absolute path
- IF(NOT VISP_HAVE_OGRE_PLUGINS_PATH)
- include(${VISP_CMAKE_MODULE_PATH}/OgreTools.cmake)
- # case 1: normal case
- #--------------
- CREATE_OGRE_PLUGIN_CONFIG_FILE()
-
- # case 2: install or packaging case
- #--------------
- # install rule for plugins.cfg:
- IF(UNIX)
- if(OGRE_PLUGIN_DIR_REL)
- INSTALL(FILES
- ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
- if(OGRE_PLUGIN_DIR_DBG)
- INSTALL(FILES
- ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
-
- ELSE()
- if(OGRE_PLUGIN_DIR_REL)
- INSTALL(FILES
- ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg
- DESTINATION data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
- if(OGRE_PLUGIN_DIR_DBG)
- INSTALL(FILES
- ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg
- DESTINATION data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
- ENDIF()
- ENDIF()
-
- # Try to search for an existing resources.cfg file
- FIND_PATH(VISP_HAVE_OGRE_RESOURCES_PATH
- NAMES resources.cfg
- PATHS ${OGRE_MEDIA_DIR}/..
- NO_SYSTEM_ENVIRONMENT_PATH
- )
-
- # Here we copy all the minimal media files
- # - media/materials/...
- # - media/models/...
- if(OGRE_MEDIA_NOT_AVAILABLE)
- file(COPY data/ogre-simulator/media DESTINATION ${VISP_BINARY_DIR}/data/ogre-simulator)
- endif()
-
-
- # Here we create a resources.cfg if it was not found
- IF(NOT VISP_HAVE_OGRE_RESOURCES_PATH)
- # we create a resources.cfg file for vpAROgre.cpp
- # case 1: normal case
- # If OGRE_MEDIA_DIR is not found, we set it to VISP_HAVE_OGRE_RESOURCES_PATH in order to use
- # the minimal requested media to run the examples
- #--------------
- SET(VISP_HAVE_OGRE_RESOURCES_PATH ${VISP_BINARY_DIR}/data/ogre-simulator)
- if(OGRE_MEDIA_NOT_AVAILABLE)
- set(OGRE_MEDIA_DIR ${VISP_HAVE_OGRE_RESOURCES_PATH}/media)
- endif()
-
- # On Fedora 20 when ogre-devel and ogre-samples packages are installed, AROgre and
- # AROgreBasic examples produce a segfault when the folowing line in resource.cf.in
- # is commented:
- # '#FileSystem=@OGRE_MEDIA_DIR@/materials/programs'
- # Here we check if @OGRE_MEDIA_DIR@/materials/programs forder is empty.
- # If empty, we comment the line to avoid lintian warnings.
- # If not empty we remove the comment to use the available programs.
- file(GLOB OGRE_PROGRAM_CONTENT ${OGRE_MEDIA_DIR}/materials/programs/*.*)
- if(OGRE_PROGRAM_CONTENT)
- set(OGRE_COMMENT_LINE "")
- else()
- set(OGRE_COMMENT_LINE "#")
- endif()
- CONFIGURE_FILE(
- ${VISP_CMAKE_MODULE_PATH}/resources.cfg.in
- ${VISP_HAVE_OGRE_RESOURCES_PATH}/resources.cfg
- IMMEDIATE @ONLY)
-
- # case 2: install or packaging case
- # If OGRE_MEDIA_DIR is not found, we set it to VISP_INSTALL_DIR_OGRE_RESOURCES in order to use
- # the minimal requested media to run the examples
- #--------------
- IF(UNIX)
- set(VISP_INSTALL_DIR_OGRE_RESOURCES "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator")
- ELSE()
- set(VISP_INSTALL_DIR_OGRE_RESOURCES "${CMAKE_INSTALL_PREFIX}/data/ogre-simulator")
- ENDIF()
-
- if(OGRE_MEDIA_NOT_AVAILABLE)
- set(OGRE_MEDIA_DIR ${VISP_INSTALL_DIR_OGRE_RESOURCES}/media)
- endif()
-
- # install rule for resources.cfg and Ogre media if they are not available:
- if(UNIX)
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/resources.cfg.in
- ${VISP_BINARY_DIR}/unix-install/resources.cfg
- IMMEDIATE @ONLY)
- install(FILES
- ${VISP_BINARY_DIR}/unix-install/resources.cfg
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- if(OGRE_MEDIA_NOT_AVAILABLE)
- install(DIRECTORY
- data/ogre-simulator/media
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator
- FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
- else()
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/resources.cfg.in
- ${VISP_BINARY_DIR}/win-install/resources.cfg
- IMMEDIATE @ONLY)
- install(FILES
- ${VISP_BINARY_DIR}/win-install/resources.cfg
- DESTINATION data/ogre-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- if(OGRE_MEDIA_NOT_AVAILABLE)
- install(DIRECTORY
- data/ogre-simulator/media
- DESTINATION data/ogre-simulator
- FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endif()
- endif()
- endif()
-
- MARK_AS_ADVANCED(VISP_HAVE_OGRE_PLUGINS_PATH)
- MARK_AS_ADVANCED(VISP_HAVE_OGRE_RESOURCES_PATH)
- #message("VISP_HAVE_OGRE_PLUGINS_PATH: ${VISP_HAVE_OGRE_PLUGINS_PATH}")
- #message("VISP_HAVE_OGRE_RESOURCES_PATH: ${VISP_HAVE_OGRE_RESOURCES_PATH}")
-
- # hack to fix possible presence of NOTFOUND in OGRE_INCLUDE_DIRS
- #message("OGRE_INCLUDE_DIRS: ${OGRE_INCLUDE_DIRS}")
- #LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${OGRE_INCLUDE_DIRS})
- foreach(inc_ ${OGRE_INCLUDE_DIRS})
- if(NOT ${inc_} MATCHES "NOTFOUND")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${inc_})
- endif()
- endforeach()
- LIST(APPEND VISP_EXTERN_LIBRARIES ${OGRE_LIBRARIES})
-ENDIF(USE_OGRE)
-
-IF(USE_OIS AND USE_OGRE)
- SET(VISP_HAVE_OIS_FOUND "yes") # for ViSP-third-party.txt
- SET(VISP_HAVE_OIS TRUE) # for header vpConfig.h
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${OIS_INCLUDE_DIR})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${OIS_LIBRARIES})
- IF(APPLE)
- # With Ogre 1.7.4 and 1.8.1 to be able to link with libOIS.a, Cocoa framework is requested.
- # This is a work around since it should come with FindOGRE.cmake
- LIST(APPEND VISP_EXTERN_LIBRARIES "-framework Cocoa")
- ENDIF(APPLE)
-
-ENDIF(USE_OIS AND USE_OGRE)
-
-IF(USE_COIN)
- IF (COIN3D_FOUND)
- MESSAGE(STATUS "Coin3D found")
- SET(VISP_HAVE_COIN_FOUND "yes") # for ViSP-third-party.txt
- SET(VISP_HAVE_COIN TRUE) # for header vpConfig.h
- SET(VISP_HAVE_OPENGL TRUE) # for header vpConfig.h
- SET(VISP_HAVE_OPENGL_FOUND "yes") # for ViSP-third-party.txt
- IF(WIN32)
- LIST(APPEND VISP_DEFS "-DCOIN_DLL")
- ENDIF(WIN32)
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${COIN3D_INCLUDE_DIRS})
- # On OSX cmake 2.8 found OpenGL but OPENGL_INCLUDE_DIR was set to NOT_FOUND
- # We add a test to be sure that the OPENGL vars exist.
- if(OPENGL_INCLUDE_DIR)
- list(APPEND VISP_EXTERN_INCLUDE_DIRS ${OPENGL_INCLUDE_DIR})
- endif()
- if(OPENGL_LIBRARIES)
- list(APPEND VISP_EXTERN_LIBRARIES ${OPENGL_LIBRARIES})
- endif()
-
- #MESSAGE("COIN3D_INCLUDE_DIRS = ${COIN3D_INCLUDE_DIRS}")
- #MESSAGE("COIN3D_LIBRARIES = ${COIN3D_LIBRARIES}")
- LIST(APPEND VISP_EXTERN_LIBRARIES ${COIN3D_LIBRARIES})
-
- IF (USE_SOWIN AND SOWIN_FOUND)
- # We manage SoWin and Coin
- SET(VISP_HAVE_SOWIN TRUE) # for header vpConfig.h
- SET(VISP_HAVE_SOWIN_FOUND "yes") # for ViSP-third-party.txt
- SET(VISP_HAVE_COIN_AND_GUI TRUE) # for header vpConfig.h
-
- #MESSAGE("SOWIN_INCLUDE_DIR = ${SOWIN_INCLUDE_DIRS}")
- #MESSAGE("SOWIN_LIBRARIES = ${SOWIN_LIBRARIES}")
-
- LIST(APPEND VISP_DEFS "-DSOWIN_DLL")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${SOWIN_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${SOWIN_LIBRARIES})
- ENDIF(USE_SOWIN AND SOWIN_FOUND)
-
- IF(USE_SOQT AND SOQT_FOUND)
- #MESSAGE("SoQt found")
-
- IF (USE_QT AND QT_FOUND)
- #MESSAGE("Qt found")
- # OpenGL, QT and SOQT are found
- # We first manage OpenGL
- SET(VISP_HAVE_SOQT TRUE) # for header vpConfig.h
- SET(VISP_HAVE_SOQT_FOUND "yes") # for ViSP-third-party.txt
- SET(VISP_HAVE_QT TRUE) # for header vpConfig.h
- SET(VISP_HAVE_COIN_AND_GUI TRUE) # for header vpConfig.h
-
- #MESSAGE(STATUS "SOQT_INCLUDE_DIRS = ${SOQT_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${SOQT_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${QT_INCLUDE_DIR})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${QT_INCLUDES})
-
- # Add SoQt libraries
- #MESSAGE("SOQT_LIBRARIES = ${SOQT_LIBRARIES}")
- LIST(APPEND VISP_EXTERN_LIBRARIES ${SOQT_LIBRARIES})
- IF(WIN32)
- LIST(APPEND VISP_DEFS "-DSOQT_DLL")
- ENDIF(WIN32)
-
- # We manage QT libraries
- IF (DESIRED_QT_VERSION MATCHES 3)
- #Add Qt3 libraries
- SET(VISP_HAVE_QT3_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("QT_QT_LIBRARY ${QT_QT_LIBRARY}")
- LIST(APPEND VISP_EXTERN_LIBRARIES ${QT_QT_LIBRARY})
- ELSEIF(DESIRED_QT_VERSION MATCHES 4)
- #Add Qt4 libraries
- SET(VISP_HAVE_QT4_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("QT_QTGUI_LIBRARY ${QT_QTGUI_LIBRARY}")
- #MESSAGE("QT_QTGUI_LIBRARY_RELEASE ${QT_QTGUI_LIBRARY_RELEASE}")
- #MESSAGE("QT_QTGUI_LIBRARY_DEBUG ${QT_QTGUI_LIBRARY_DEBUG}")
- IF (QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE AND QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
- LIST(APPEND VISP_EXTERN_LIBRARIES optimized ${QT_QTGUI_LIBRARY_RELEASE})
- LIST(APPEND VISP_EXTERN_LIBRARIES optimized ${QT_QTCORE_LIBRARY_RELEASE})
- LIST(APPEND VISP_EXTERN_LIBRARIES debug ${QT_QTGUI_LIBRARY_DEBUG})
- LIST(APPEND VISP_EXTERN_LIBRARIES debug ${QT_QTCORE_LIBRARY_DEBUG})
- ELSEIF (QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE)
- LIST(APPEND VISP_EXTERN_LIBRARIES ${QT_QTGUI_LIBRARY_RELEASE})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${QT_QTCORE_LIBRARY_RELEASE})
- ELSEIF (QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
- LIST(APPEND VISP_EXTERN_LIBRARIES ${QT_QTGUI_LIBRARY_DEBUG})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${QT_QTCORE_LIBRARY_DEBUG})
- ENDIF()
- ENDIF (DESIRED_QT_VERSION MATCHES 3)
-
- # Because in QT_DEFINITIONS defs are separated by ";", parse the
- # QT_DEFINITIONS in order to build a space separated string
- #SET(VISP_DEFS "${VISP_DEFS} ${QT_DEFINITIONS}") did not work
- # in third party projects which use visp-config shell script
- # (due to ";" # separate char)
- FOREACH(DEF ${QT_DEFINITIONS})
- #MESSAGE("DEF: ${DEF}")
- LIST(APPEND VISP_DEFS ${DEF})
- ENDFOREACH(DEF)
-
- LIST(APPEND VISP_DEFS "-DQT_DLL")
- ENDIF (USE_QT AND QT_FOUND)
- ENDIF(USE_SOQT AND SOQT_FOUND)
-
- IF (USE_SOXT AND SOXT_FOUND)
- # OPenGL and SoXt are found
- # We first manage OpenGL
- SET(VISP_HAVE_SOXT TRUE) # for header vpConfig.h
- SET(VISP_HAVE_COIN_AND_GUI TRUE) # for header vpConfig.h
- SET(VISP_HAVE_SOXT_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_LIBRARIES ${SOXT_LIBRARIES})
- ENDIF(USE_SOXT AND SOXT_FOUND)
-
- IF(VISP_HAVE_COIN_AND_GUI)
- SET(VISP_HAVE_COIN_AND_GUI_FOUND "yes") # for ViSP-third-party.txt
- MESSAGE(STATUS "Coin3D + GUI found")
- ELSE(VISP_HAVE_COIN_AND_GUI)
- MESSAGE(STATUS "Coin3D + GUI not found")
- SET(VISP_HAVE_COIN_AND_GUI_FOUND "no") # for ViSP-third-party.txt
- ENDIF(VISP_HAVE_COIN_AND_GUI)
- ENDIF (COIN3D_FOUND)
-ENDIF(USE_COIN)
-
-#---------------------------------------------------------------------
-# XML2
-#---------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_XML2_FOUND "no") # for ViSP-third-party.txt
-
-IF(USE_XML2)
- #FIND_PACKAGE(XML2)
- IF(XML2_FOUND)
- SET(VISP_HAVE_XML2 TRUE)
- SET(VISP_HAVE_XML2_FOUND "yes") # for ViSP-third-party.txt
- MESSAGE(STATUS "XML2 found")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${XML2_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${XML2_LIBRARIES})
- ELSE(XML2_FOUND)
- MESSAGE(STATUS "XML2 not found")
- SET(VISP_HAVE_XML2_FOUND "no") # for ViSP-third-party.txt
- ENDIF(XML2_FOUND)
-ENDIF(USE_XML2)
-
-
-#---------------------------------------------------------------------
-# libfreenect
-# Note that this library needs libusb-1.0 and libpthread
-#---------------------------------------------------------------------
-# default initialisation for ViSP-third-party.txt
-SET(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES_FOUND "no")
-SET(VISP_HAVE_LIBFREENECT_FOUND "no")
-SET(VISP_HAVE_LIBUSB_1_FOUND "no")
-
-IF(USE_LIBFREENECT)
- #FIND_PACKAGE(LIBFREENECT)
- IF(LIBFREENECT_FOUND)
- SET(VISP_HAVE_LIBFREENECT TRUE)
- SET(VISP_HAVE_LIBFREENECT_FOUND "yes") # for ViSP-third-party.txt
-
- MESSAGE(STATUS "libfreenect found")
-
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${LIBFREENECT_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${LIBFREENECT_LIBRARIES})
- ELSE()
- MESSAGE(STATUS "libfreenect not found")
- SET(VISP_HAVE_LIBFREENECT_FOUND "no") # for ViSP-third-party.txt
- ENDIF()
-ENDIF()
-
-IF(USE_LIBUSB_1)
- #FIND_PACKAGE(LIBUSB-1)
- IF(LIBUSB_1_FOUND)
- SET(VISP_HAVE_LIBUSB_1 TRUE)
- SET(VISP_HAVE_LIBUSB_1_FOUND "yes") # for ViSP-third-party.txt
-
- MESSAGE(STATUS "libusb-1.0 found")
-
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${LIBUSB_1_LIBRARIES})
- ELSE()
- MESSAGE(STATUS "libusb-1.0 not found")
- SET(VISP_HAVE_LIBUSB_1_FOUND "no") # for ViSP-third-party.txt
- ENDIF()
-ENDIF()
-
-# Check if libfreenect dependencies (ie libusb-1.0 and libpthread) are available
-IF(USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD)
- IF(LIBFREENECT_FOUND AND LIBUSB_1_FOUND AND PTHREAD_FOUND)
- SET(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES TRUE)
- SET(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES_FOUND "yes") # for ViSP-third-party.txt
-
- # The material is found. Check if libfreenect is an old version
- include(CheckCXXSourceCompiles)
- SET(CMAKE_REQUIRED_LIBRARIES ${LIBFREENECT_LIBRARIES} ${PTHREAD_LIBRARIES} ${LIBUSB_1_LIBRARIES})
- SET(CMAKE_REQUIRED_INCLUDES ${LIBFREENECT_INCLUDE_DIRS} ${PTHREAD_INCLUDE_DIRS} ${LIBUSB_1_INCLUDE_DIRS})
- CHECK_CXX_SOURCE_COMPILES("
- #include <libfreenect.hpp>
-
- class vpMyKinect : public Freenect::FreenectDevice
- {
- };
-
- int main()
- {
- Freenect::Freenect<vpMyKinect> freenect;
- }
- " LIBFREENECT_IS_OLD_VERSION)
- #MESSAGE("LIBFREENECT_IS_OLD_VERSION: ${LIBFREENECT_IS_OLD_VERSION}")
- IF(LIBFREENECT_IS_OLD_VERSION)
- SET(VISP_HAVE_LIBFREENECT_OLD TRUE)
- ELSE()
- SET(VISP_HAVE_LIBFREENECT_OLD FALSE)
- ENDIF()
-
- ENDIF()
-ENDIF()
-
-
-#--------------------------------------------------------------------
-# pthread lib
-#--------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_PTHREAD_FOUND "no") # for ViSP-third-party.txt
-
-IF(USE_PTHREAD)
- #FIND_PACKAGE(PTHREAD)
- IF(PTHREAD_FOUND)
- MESSAGE(STATUS "pthread found")
- SET(VISP_HAVE_PTHREAD TRUE) # for header vpConfig.h
- SET(VISP_HAVE_PTHREAD_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${PTHREAD_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${PTHREAD_LIBRARIES})
- #MESSAGE("PTHREAD_LIBRARIES = ${PTHREAD_LIBRARIES}")
- ELSE(PTHREAD_FOUND)
- MESSAGE(STATUS "pthread not found")
- SET(VISP_HAVE_PTHREAD_FOUND "no") # for ViSP-third-party.txt
- ENDIF(PTHREAD_FOUND)
-ENDIF(USE_PTHREAD)
-
-#--------------------------------------------------------------------
-# parallel port usage
-#--------------------------------------------------------------------
-FIND_PACKAGE(PARPORT)
-IF(PARPORT_FOUND)
- SET(VISP_HAVE_PARPORT TRUE) # for header vpConfig.h
-ENDIF(PARPORT_FOUND)
-
-#----------------------------------------------------------------------
-# Third party software :
-#----------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_OPENCV_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_GSL_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_GTK_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_D3D9_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_GDI_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_LIBJPEG_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_LIBPNG_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_ZLIB_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_FFMPEG_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_CMU1394_FOUND "no") # for ViSP-third-party.txt
-
-# OpenCV
-if(USE_OPENCV)
- #SET(OpenCV_FIND_QUIETLY TRUE)
- #FIND_PACKAGE(OpenCV)
- if(OpenCV_FOUND)
- message(STATUS "OpenCV found")
- set(VISP_HAVE_OPENCV TRUE) # for header vpConfig.h
- set(VISP_HAVE_OPENCV_FOUND "yes") # for ViSP-third-party.txt
- # On win32 since OpenCV 2.4.7 and on OSX with OpenCV 2.4.10 we cannot use OpenCV_LIBS to set ViSP 3rd party libraries.
- # Using OpenCV_LIBS works to build visp library, examples, demos and test thanks to the components,
- # but not tutorials that are stand alone Cmake project that use ViSP as a 3rd party.
- # To be clear OpenCV_LIBS contains opencv_core and not c:\...\opencv_core248.lib full path as requested
- # to use ViSP. This was not the case with OpenCV 2.4.6.
- # For the build of ViSP it works with OpenCV_LIBS: in that case thanks to opencv_core properties, CMake
- # is able to find the real name and location of the libraries.
- # But when ViSP is used as a 3rd party where it should import OpenCV libraries, it doesn't work with
- # OpenCV_LIBS.
- # The solution here is to get the real name of OpenCV libraries thanks to the properties and link
- # with these names.
- # An other way could be to include OpenCVConfig.cmake, but in that case, visp-config and visp.pc
- # will be not able to give the names of OpenCV libraries when used without CMake.
- #message("OpenCV_LIB_COMPONENTS: ${OpenCV_LIB_COMPONENTS}")
- #message("OpenCV_LIBS: ${OpenCV_LIBS}")
- #if(WIN32 AND OpenCV_LIB_COMPONENTS AND OpenCV_VERSION AND OpenCV_VERSION VERSION_GREATER 2.4.6.1)
- if(OpenCV_LIB_COMPONENTS AND OpenCV_VERSION AND OpenCV_VERSION VERSION_GREATER 2.4.6.1)
- set(config_ "NONE" "RELEASE" "DEBUG" "RELEASEWITHDEBINFO") # RelWithDebugInfo was requested under Fedora 20
-
- if(POLICY CMP0045)
- # Fix Error on non-existent target in get_target_property for 3rd party location extraction
- cmake_policy(PUSH)
- cmake_policy(SET CMP0045 OLD)
- endif()
-
- foreach(component_ ${OpenCV_LIB_COMPONENTS})
- foreach(imp_config_ ${config_})
- if(OpenCV_SHARED)
- get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_IMPLIB_${imp_config_})
- # particular case of opencv_ts that doesn't have an implib
- if(NOT EXISTS "${component_property_${imp_config_}_}")
- get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_LOCATION_${imp_config_})
- endif()
- else()
- get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_LOCATION_${imp_config_})
- endif()
- get_target_property(component_property_3rdparty_${imp_config_}_ ${component_} IMPORTED_LINK_INTERFACE_LIBRARIES_${imp_config_})
- #message("component_property_${imp_config_}_: ${component_property_${imp_config_}_}")
- #message("component_property_3rdparty_${imp_config_}_: ${component_property_3rdparty_${imp_config_}_}")
-
- # Under Unix, there is no specific suffix for OpenCV libraries. If one is found we add it
- # Under Windows, we add the "optimized", "debug" specific keywords
- if(WIN32 AND EXISTS "${component_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "RELEASE") # also valid for RELEASEWITHDEBINFO
- list(APPEND VISP_EXTERN_LIBRARIES optimized "${component_property_${imp_config_}_}")
- elseif(WIN32 AND EXISTS "${component_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "DEBUG")
- list(APPEND VISP_EXTERN_LIBRARIES debug "${component_property_${imp_config_}_}")
- elseif(EXISTS "${component_property_${imp_config_}_}")
- list(APPEND VISP_EXTERN_LIBRARIES "${component_property_${imp_config_}_}")
- endif()
-
- if(component_property_3rdparty_${imp_config_}_)
- foreach(3rdparty_ ${component_property_3rdparty_${imp_config_}_})
- #message("3rdparty_ ${3rdparty_}")
- list(FIND OpenCV_LIB_COMPONENTS ${3rdparty_} 3rdparty_is_opencv_component_)
- if(3rdparty_is_opencv_component_ LESS 0)
- #message("${3rdparty_} is not an opencv component")
- get_target_property(3rdparty_opt_location_ ${3rdparty_} IMPORTED_LOCATION_${imp_config_})
- if(NOT EXISTS "${3rdparty_opt_location_}")
- #message("3rdparty_: ${3rdparty_} location doesn't exist in ${imp_config_}")
- get_target_property(3rdparty_opt_location_ ${3rdparty_} IMPORTED_LOCATION)
- #message("3rdparty_: ${3rdparty_} location : ${3rdparty_opt_location_}")
- endif()
- if(EXISTS "${3rdparty_opt_location_}")
- #message("3rdparty_opt_location_: ${3rdparty_opt_location_} with config ${imp_config_}")
- if(WIN32 AND "${imp_config_}" MATCHES "RELEASE")
- #message("is release")
- list(APPEND VISP_EXTERN_LIBRARIES optimized ${3rdparty_opt_location_})
- elseif(WIN32 AND "${imp_config_}" MATCHES "DEBUG")
- list(APPEND VISP_EXTERN_LIBRARIES debug ${3rdparty_opt_location_})
- else()
- list(APPEND VISP_EXTERN_LIBRARIES ${3rdparty_opt_location_})
- endif()
- else()
- find_library(3rdparty_location_ NAMES ${3rdparty_})
- mark_as_advanced(3rdparty_location_)
- if(3rdparty_location_)
- #message(${3rdparty_location_})
- list(APPEND VISP_EXTERN_LIBRARIES ${3rdparty_location_}) # should be a system dependency
- else()
- list(APPEND VISP_EXTERN_LIBRARIES ${3rdparty_}) # should be a system dependency
- endif()
- endif()
- endif()
- endforeach()
- endif()
- endforeach()
- endforeach()
-
- if(POLICY CMP0045)
- # Fix Error on non-existent target in get_target_property for 3rd party location extraction
- cmake_policy(POP)
- endif()
-
- else()
- # this should be an old OpenCV version that doesn't have the previous behavior
- list(APPEND VISP_EXTERN_LIBRARIES ${OpenCV_LIBS})
- endif()
- list(APPEND VISP_EXTERN_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
- #LIST(APPEND VISP_EXTERN_LIBRARIES ${OpenCV_LIBS})
- #MESSAGE("OpenCV_INCLUDE_DIRS = ${OpenCV_INCLUDE_DIRS}")
- #MESSAGE("OpenCV_LIBS = ${OpenCV_LIBS}")
- #MESSAGE("OpenCV_LIB_DIR = ${OpenCV_LIB_DIR}")
- set(VISP_HAVE_OPENCV_VERSION "(${OpenCV_VERSION_MAJOR}*65536 + ${OpenCV_VERSION_MINOR}*256 + ${OpenCV_VERSION_PATCH})") # for ViSP-third-party.txt
- #MESSAGE("VISP_HAVE_OPENCV_VERSION = ${VISP_HAVE_OPENCV_VERSION}")
- #message("VISP_EXTERN_LIBRARIES = ${VISP_EXTERN_LIBRARIES}")
-
- # additional check to see if opencv_nonfree module introduced in OpenCV 2.4 is available
- #message("OpenCV_VERSION = ${OpenCV_VERSION}")
- #message("OpenCV_NONFREE_FOUND: ${OPENCV_NONFREE_FOUND}")
- if(OpenCV_VERSION)
- if(OpenCV_VERSION VERSION_LESS "2.4.0")
- message(STATUS "OpenCV nonfree module found")
- set(VISP_HAVE_OPENCV_NONFREE TRUE) # for header vpConfig.h
- else() # Version is greater or equal to 2.4.0
- if(OPENCV_NONFREE_FOUND) # OpenCV < 3.0.0
- message(STATUS "OpenCV xfeatures2d module found")
- set(VISP_HAVE_OPENCV_NONFREE TRUE) # for header vpConfig.h
- elseif(OPENCV_XFEATURES2D_FOUND) # OpenCV >= 3.0.0
- set(VISP_HAVE_OPENCV_XFEATURES2D TRUE) # for header vpConfig.h
- else()
- message(STATUS "OpenCV nonfree or xfeature2d module not found")
- endif()
- endif()
- else()
- message("OpenCV_VERSION is false")
- message(STATUS "OpenCV nonfree not found")
- endif()
- else(OpenCV_FOUND)
- message(STATUS "OpenCV not found")
- set(VISP_HAVE_OPENCV_FOUND "no") # for ViSP-third-party.txt
- set(VISP_HAVE_OPENCV_VERSION "(0)") # for ViSP-third-party.txt
- endif(OpenCV_FOUND)
-else(USE_OPENCV)
- set(OpenCV_VERSION_MAJOR 0)
- set(OpenCV_VERSION_MINOR 0)
- SET(OpenCV_VERSION_PATCH 0)
- set(VISP_HAVE_OPENCV_VERSION "(0)") # for ViSP-third-party.txt
-endif(USE_OPENCV)
-
-# gnu scientific library
-IF(USE_GSL)
- #FIND_PACKAGE(GSL)
- IF(GSL_FOUND)
- MESSAGE(STATUS "GSL found")
- SET(VISP_HAVE_GSL TRUE) # for header vpConfig.h
- SET(VISP_HAVE_GSL_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${GSL_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${GSL_LIBRARIES})
- #MESSAGE("GSL_INCLUDE_DIRS = ${GSL_INCLUDE_DIRS}")
- #MESSAGE("GSL_LIBRARIES = ${GSL_LIBRARIES}")
- ELSE(GSL_FOUND)
- MESSAGE(STATUS "GSL not found")
- SET(VISP_HAVE_GSL_FOUND "no") # for ViSP-third-party.txt
- ENDIF(GSL_FOUND)
-ENDIF(USE_GSL)
-
-# strategy for gtk: search first gtk2. If not found search gtk1
-# gtk-2 library:
-IF(USE_GTK2)
- #FIND_PACKAGE(GTK2)
- IF(GTK2_FOUND)
- MESSAGE(STATUS "GTK2 found")
- SET(VISP_HAVE_GTK TRUE) # for header vpConfig.h
- SET(VISP_HAVE_GTK_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${GTK2_INCLUDE_DIRS})
-
- IF(UNIX)
- ## ignore gtk pedantic warnings by gcc specific -isystem :
- SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${GTK2_gtk_INCLUDE_PATH}")
- ENDIF(UNIX)
-
- LIST(APPEND VISP_EXTERN_LIBRARIES ${GTK2_LIBRARIES})
- # MESSAGE(STATUS "DBG CMAKE_CXX_FLAGS=${CMAKE_CXX_FLAGS}")
- #MESSAGE("GTK2 includes: ${GTK2_INCLUDE_DIRS}")
- #MESSAGE("GTK2 Libs: ${GTK2_LIBRARIES}")
- SET(USE_GTK1 OFF) # no need to found gtk-1
- ELSE(GTK2_FOUND)
- MESSAGE(STATUS "GTK2 not found")
- SET(VISP_HAVE_GTK_FOUND "no") # for ViSP-third-party.txt
- #MESSAGE("GTK2 requested but not found.")
- ENDIF(GTK2_FOUND)
-ENDIF(USE_GTK2)
-
-# Under Windows, usage of Direct3D9
-IF(USE_DIRECT3D)
- #FIND_PACKAGE(DIRECT3D)
- IF(DIRECT3D_FOUND)
- MESSAGE(STATUS "Direct3D found")
- SET(VISP_HAVE_D3D9 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_D3D9_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_D3D9= ${VISP_HAVE_D3D9}")
- #MESSAGE("DBG DIRECT3D_INCLUDE_DIRS=${DIRECT3D_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${DIRECT3D_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${DIRECT3D_LIBRARIES})
- #MESSAGE("DIRECT3D_LIBRARIES = ${DIRECT3D_LIBRARIES}")
- ELSE(DIRECT3D_FOUND)
- MESSAGE(STATUS "Direct3D not found")
- SET(VISP_HAVE_D3D9_FOUND "no") # for ViSP-third-party.txt
- ENDIF(DIRECT3D_FOUND)
-ENDIF(USE_DIRECT3D)
-
-# Under Windows, usage of GDI (Graphics Device Interface)
-IF(USE_GDI)
- #FIND_PACKAGE(GDI)
- IF(GDI_FOUND)
- MESSAGE(STATUS "GDI found")
- SET(VISP_HAVE_GDI TRUE) # for header vpConfig.h
- SET(VISP_HAVE_GDI_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_GDI= ${VISP_HAVE_GDI}")
- LIST(APPEND VISP_EXTERN_LIBRARIES ${GDI_LIBRARIES})
- #MESSAGE("GDI_LIBRARIES = ${GDI_LIBRARIES}")
- ELSE(GDI_FOUND)
- MESSAGE(STATUS "GDI not found")
- SET(VISP_HAVE_GDI_FOUND "no") # for ViSP-third-party.txt
- ENDIF(GDI_FOUND)
-ENDIF(USE_GDI)
-
-# zlib
-IF(USE_ZLIB)
- #FIND_PACKAGE(ZLIB)
- IF(ZLIB_FOUND)
- MESSAGE(STATUS "zlib found")
- SET(VISP_HAVE_ZLIB TRUE) # for header vpConfig.h
- SET(VISP_HAVE_ZLIB_FOUND "yes") # for ViSP-third-party.txt
-
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${ZLIB_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${ZLIB_LIBRARIES})
- #MESSAGE(ZLIB_LIBRARIES: ${ZLIB_LIBRARIES})
- ELSE()
- MESSAGE(STATUS "zlib not found")
- SET(VISP_HAVE_FOUND "no") # for ViSP-third-party.txt
- ENDIF()
-ENDIF(USE_ZLIB)
-
-# Libjpeg
-IF(USE_LIBJPEG)
- #FIND_PACKAGE(JPEG)
- #MESSAGE("test ${JPEG_FOUND}")
- IF(JPEG_FOUND)
- MESSAGE(STATUS "libjpeg found")
- SET(VISP_HAVE_LIBJPEG TRUE) # for header vpConfig.h
- SET(VISP_HAVE_LIBJPEG_FOUND "yes") # for ViSP-third-party.txt
-
- #message("JPEG_INCLUDE_DIR: ${JPEG_INCLUDE_DIR}")
- # Here we use JPEG_INCLUDE_DIR that is the var name defined in CMake/Modules/FindJPEG.cmake
- # In FindMyJPEG.cmake, we define both JPEG_INCLUDE_DIR and JPEG_INCLUDE_DIRS
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${JPEG_INCLUDE_DIR})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${JPEG_LIBRARIES})
-
- # If Qt3 and libjpeg are used, we add here QT_CLEAN_NAMESPACE define to
- # handle a compilation error "conflicting declaration 'typedef long int
- # INT32'"
- IF(VISP_HAVE_QT3_FOUND)
- LIST(APPEND VISP_DEFS "-DQT_CLEAN_NAMESPACE")
- # MESSAGE("add -DQT_CLEAN_NAMESPACE")
- ENDIF(VISP_HAVE_QT3_FOUND)
-
- ELSE(JPEG_FOUND)
- MESSAGE(STATUS "libjpeg not found")
- SET(VISP_HAVE_LIBJPEG_FOUND "no") # for ViSP-third-party.txt
- ENDIF(JPEG_FOUND)
-ENDIF(USE_LIBJPEG)
-
-# Libpng
-IF(USE_LIBPNG)
- #FIND_PACKAGE(PNG)
- IF(PNG_FOUND)
- MESSAGE(STATUS "libpng found")
- SET(VISP_HAVE_LIBPNG TRUE) # for header vpConfig.h
- SET(VISP_HAVE_LIBPNG_FOUND "yes") # for ViSP-third-party.txt
-
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${PNG_LIBRARIES})
- LIST(APPEND VISP_DEFS ${PNG_DEFINITIONS})
- ELSE()
- MESSAGE(STATUS "libpng not found")
- SET(VISP_HAVE_LIBPNG_FOUND "no") # for ViSP-third-party.txt
- ENDIF()
-ENDIF(USE_LIBPNG)
-
-# FFMPEG
-IF(USE_FFMPEG)
- #FIND_PACKAGE(FFMPEG)
- IF(FFMPEG_FOUND)
- MESSAGE(STATUS "FFMPEG found")
- SET(VISP_HAVE_FFMPEG TRUE) # for header vpConfig.h
- SET(VISP_HAVE_FFMPEG_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${FFMPEG_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${FFMPEG_LIBRARIES})
- ELSE(LIBFFMPEG_FOUND)
- MESSAGE(STATUS "FFMPEG not found")
- SET(VISP_HAVE_FFMPEG_FOUND "no") # for ViSP-third-party.txt
- ENDIF(FFMPEG_FOUND)
-ENDIF(USE_FFMPEG)
-
-#----------------------------------------------------------------------
-# Specific hardware : framegrabbers and cameras
-#----------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_DC1394_2_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_V4L2_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_DIRECTSHOW_FOUND "no") # for ViSP-third-party.txt
-
-# library for firewire cameras
-IF(USE_DC1394)
- #FIND_PACKAGE(DC1394)
- if(DC1394_FOUND)
- set(VISP_HAVE_DC1394 TRUE) # for header vpConfig.h
-
- MESSAGE(STATUS "libdc1394-2.x found")
- SET(VISP_HAVE_DC1394_2 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_DC1394_2_FOUND "yes") # for ViSP-third-party.txt
- IF(DC1394_CAMERA_ENUMERATE_FOUND)
- SET(VISP_HAVE_DC1394_2_CAMERA_ENUMERATE TRUE) # for header vpConfig.h
- ENDIF(DC1394_CAMERA_ENUMERATE_FOUND)
- IF(DC1394_FIND_CAMERAS_FOUND)
- SET(VISP_HAVE_DC1394_2_FIND_CAMERAS TRUE) # for header vpConfig.h
- ENDIF(DC1394_FIND_CAMERAS_FOUND)
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${DC1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${DC1394_LIBRARY})
-
- #MESSAGE("DBG VISP_HAVE_DC1394_2= ${VISP_HAVE_DC1394_2}")
- #MESSAGE("DBG DC1394_INCLUDE_DIRS=${DC1394_INCLUDE_DIRS}")
-
- IF(APPLE)
- #SET(VISP_EXTERN_LIBRARIES ${VISP_EXTERN_LIBRARIES} IOkit "-framework Carbon")
- LIST(APPEND VISP_EXTERN_LIBRARIES "-framework IOkit -framework Carbon")
- ENDIF(APPLE)
- #MESSAGE("DC1394_LIBRARIES = ${DC1394_LIBRARIES}")
- ELSE(DC1394_FOUND)
- MESSAGE(STATUS "libdc1394-2.x not found")
- SET(VISP_HAVE_DC1394_2_FOUND "no") # for ViSP-third-party.txt
- #MESSAGE("Firewire dc1394 requested but not found. Turn off USE_DC1394!")
- # SET(USE_DC1394 OFF)
- ENDIF(DC1394_FOUND)
-ENDIF(USE_DC1394)
-
-# library for CMU1394 pan-tilt head
-IF(USE_CMU1394)
-# FIND_PACKAGE(CMU1394)
- IF(CMU1394_FOUND)
- MESSAGE(STATUS "CMU1394 found")
- SET(VISP_HAVE_CMU1394 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_CMU1394_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_CMU1394= ${VISP_HAVE_CMU1394}")
- #MESSAGE("DBG CMU1394_INCLUDE_DIRS=${CMU1394_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${CMU1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${CMU1394_LIBRARIES})
- #MESSAGE("CMU1394_LIBRARIES = ${CMU1394_LIBRARIES}")
- ELSE(CMU1394_FOUND)
- MESSAGE(STATUS "CMU1394 not found")
- SET(VISP_HAVE_CMU1394_FOUND "no") # for ViSP-third-party.txt
- # SET(CMU1394 OFF)
- ENDIF(CMU1394_FOUND)
-ENDIF(USE_CMU1394)
-
-# library for Video For Linux Two framegrabber devices
-IF(USE_V4L2)
- #FIND_PACKAGE(V4L2)
- IF(V4L2_FOUND)
- MESSAGE(STATUS "V4l2 found")
- SET(VISP_HAVE_V4L2 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_V4L2_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_V4L2= ${VISP_HAVE_V4L2}")
- #MESSAGE("DBG V4L2_INCLUDE_DIRS=${V4L2_INCLUDE_DIRS}")
- #MESSAGE("DBG V4L2_LIBRARIES=${V4L2_LIBRARIES}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${V4L2_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${V4L2_LIBRARIES})
- ELSE(V4L2_FOUND)
- MESSAGE(STATUS "V4l2 not found")
- SET(VISP_HAVE_V4L2_FOUND "no") # for ViSP-third-party.txt
- #MESSAGE("Video For Linux Two requested but not found.")
- # SET(USE_V4L2 OFF)
- ENDIF(V4L2_FOUND)
-ENDIF(USE_V4L2)
-
-# Under Windows, usage of Direct Show
-IF(USE_DIRECTSHOW)
- #FIND_PACKAGE(DIRECTSHOW)
- IF(DIRECTSHOW_FOUND)
- MESSAGE(STATUS "DirectShow found")
- SET(VISP_HAVE_DIRECTSHOW TRUE) # for header vpConfig.h
- SET(VISP_HAVE_DIRECTSHOW_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_DIRECTSHOW= ${VISP_HAVE_PTU46}")
- #MESSAGE("DBG DIRECTSHOW_INCLUDE_DIRS=${DIRECTSHOW_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${DIRECTSHOW_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${DIRECTSHOW_LIBRARIES})
- #MESSAGE("DIRECTSHOW_LIBRARIES = ${DIRECTSHOW_LIBRARIES}")
- ELSE(DIRECTSHOW_FOUND)
- MESSAGE(STATUS "DirectShow not found")
- SET(VISP_HAVE_DIRECTSHOW_FOUND "no") # for ViSP-third-party.txt
- ENDIF(DIRECTSHOW_FOUND)
-ENDIF(USE_DIRECTSHOW)
-
-#----------------------------------------------------------------------
-# Specific compiler options
-#----------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_OPENMP_FOUND "no")
-SET(VISP_HAVE_CPP11_COMPATIBILITY_FOUND "no")
-
-IF(USE_OPENMP)
- IF(OPENMP_FOUND)
- MESSAGE(STATUS "OpenMP found")
- SET(VISP_HAVE_OPENMP TRUE) # for header vpConfig.h
- SET(VISP_HAVE_OPENMP_FOUND "yes") # for ViSP-third-party.txt
- ELSE(OPENMP_FOUND)
- MESSAGE(STATUS "OpenMP not found")
- SET(VISP_HAVE_OPENMP_FOUND "no") # for ViSP-third-party.txt
- ENDIF(OPENMP_FOUND)
-ENDIF(USE_OPENMP)
-
-IF(USE_CPP11)
- IF(CPP11_FOUND)
- MESSAGE(STATUS "C++11 found")
- SET(VISP_HAVE_CPP11_COMPATIBILITY TRUE) # for header vpConfig.h
- SET(VISP_HAVE_CPP11_COMPATIBILITY_FOUND "yes") # for ViSP-third-party.txt
- ELSE(CPP11_FOUND)
- MESSAGE(STATUS "C++11 not found")
- SET(VISP_HAVE_CPP11_COMPATIBILITY_FOUND "no") # for ViSP-third-party.txt
- ENDIF(CPP11_FOUND)
-ENDIF()
-
-
-#----------------------------------------------------------------------
-# The wireframe simulator
-#----------------------------------------------------------------------
-# include all the wireframe scene files
-INCLUDE(${VISP_SOURCE_DIR}/src/simulator/wireframe-simulator/scene/CMakeWireframeScenesList.cmake)
-
-SET(WIREFRAME_SCENES_LIST "")
-foreach(SCENE ${WIREFRAME_SCENES})
- set(WIREFRAME_SCENES_SRC "${VISP_SOURCE_DIR}/src/simulator/wireframe-simulator/scene/${SCENE}")
- set(WIREFRAME_SCENES_DST "${VISP_BINARY_DIR}/data/wireframe-simulator/${SCENE}")
- IF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- ADD_CUSTOM_COMMAND(
- OUTPUT ${WIREFRAME_SCENES_DST}
- COMMAND ${CMAKE_COMMAND} -E copy ${WIREFRAME_SCENES_SRC} ${WIREFRAME_SCENES_DST}
- DEPENDS ${WIREFRAME_SCENES_SRC}
- )
- SET(WIREFRAME_SCENES_LIST ${WIREFRAME_SCENES_LIST} ${WIREFRAME_SCENES_DST})
- ELSE(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- file(COPY ${WIREFRAME_SCENES_SRC}
- DESTINATION data/wireframe-simulator
- FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE
- )
- ENDIF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
-endforeach(SCENE)
-
-IF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- ADD_CUSTOM_TARGET(SCENE ALL
- DEPENDS ${WIREFRAME_SCENES_LIST}
- )
-ENDIF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
-
-
-#----------------------------------------------------------------------
-# Robot simulators
-#----------------------------------------------------------------------
-INCLUDE(${VISP_SOURCE_DIR}/src/robot/simulator-robot/arms/CMakeRobotArmsList.cmake)
-
-SET(ROBOT_ARMS_LIST "")
-foreach(SCENE_ROBOT ${ROBOT_ARMS_SCENES})
- set(ROBOT_ARMS_SRC "${VISP_SOURCE_DIR}/src/robot/simulator-robot/arms/${SCENE_ROBOT}")
- set(ROBOT_ARMS_DST "${VISP_BINARY_DIR}/data/robot-simulator/${SCENE_ROBOT}")
- IF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- ADD_CUSTOM_COMMAND(
- OUTPUT ${ROBOT_ARMS_DST}
- COMMAND ${CMAKE_COMMAND} -E copy ${ROBOT_ARMS_SRC} ${ROBOT_ARMS_DST}
- DEPENDS ${ROBOT_ARMS_SRC}
- )
- SET(ROBOT_ARMS_LIST ${ROBOT_ARMS_LIST} ${ROBOT_ARMS_DST})
- ELSE(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- file(COPY ${ROBOT_ARMS_SRC}
- DESTINATION data/robot-simulator
- FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE
- )
- ENDIF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
-endforeach(SCENE_ROBOT)
-
-IF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
- ADD_CUSTOM_TARGET(SCENE_ROBOT ALL
- DEPENDS ${ROBOT_ARMS_LIST}
- )
-ENDIF(CMAKE_MINOR_VERSION LESS 8
- AND CMAKE_MAJOR_VERSION GREATER 1)
-
-#----------------------------------------------------------------------
-# Specific hardware : robots
-#----------------------------------------------------------------------
-# default initialisation
-SET(VISP_HAVE_AFMA4_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_AFMA6_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_BICLOPS_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_PTU46_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_PIONEER_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_VIPER650_FOUND "no") # for ViSP-third-party.txt
-SET(VISP_HAVE_VIPER850_FOUND "no") # for ViSP-third-party.txt
-
-# library for Irisa's Afma4 cylindrical robot
-IF(USE_AFMA4)
- # Low level API to control the robot:
- # libcalinux - librt - libraw1394 - libirisa
-# IF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RAW1394)
-# ENDIF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RT)
-# FIND_PACKAGE(CALINUX)
-# FIND_PACKAGE(IRISA)
- IF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Afma4 found")
- SET(VISP_HAVE_AFMA4 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_AFMA4_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${RAW1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${CALINUX_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${IRISA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${IRISA_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${CALINUX_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RAW1394_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RT_LIBRARIES})
- ELSE(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Afma4 not found")
- MESSAGE("Afma4 robot requested but not found. Turn off USE_AFMA4 option!")
- SET(VISP_HAVE_AFMA4_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_AFMA4 OFF)
- ENDIF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
-ENDIF(USE_AFMA4)
-
-# library for Irisa's Afma6 cartesian robot
-IF(USE_AFMA6)
- # Low level API to control the robot:
- # libcalinux - librt - libraw1394 - libirisa
-# IF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RAW1394)
-# ENDIF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RT)
-# FIND_PACKAGE(CALINUX)
-# FIND_PACKAGE(IRISA)
- IF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Afma6 found")
- SET(VISP_HAVE_AFMA6 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_AFMA6_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${RAW1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${CALINUX_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${IRISA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${IRISA_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${CALINUX_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RAW1394_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RT_LIBRARIES})
- ELSE(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Afma6 not found")
- MESSAGE("Afma6 robot requested but not found. Turn off USE_AFMA6 option!")
- SET(VISP_HAVE_AFMA6_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_AFMA6 OFF)
- ENDIF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
-ENDIF(USE_AFMA6)
-
-# library for Irisa's Viper850 arm robot
-IF(USE_VIPER850)
- # Low level API to control the robot:
- # libcalinux - librt - libraw1394 - libirisa
-# IF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RAW1394)
-# ENDIF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RT)
-# FIND_PACKAGE(CALINUX)
-# FIND_PACKAGE(IRISA)
- IF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Viper850 found")
- SET(VISP_HAVE_VIPER850 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_VIPER850_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${RAW1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${CALINUX_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${IRISA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${IRISA_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${CALINUX_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RAW1394_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RT_LIBRARIES})
- ELSE(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Viper S850 not found")
- MESSAGE("Viper S850 robot requested but not found. Turn off USE_VIPER850 option!")
- SET(VISP_HAVE_VIPER850_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_VIPER850 OFF)
- ENDIF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
-ENDIF(USE_VIPER850)
-
-# library for Irisa's Viper650 arm robot
-IF(USE_VIPER650)
- # Low level API to control the robot:
- # libcalinux - librt - libraw1394 - libirisa
-# IF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RAW1394)
-# ENDIF(NOT RAW1394_FOUND)
-# FIND_PACKAGE(RT)
-# FIND_PACKAGE(CALINUX)
-# FIND_PACKAGE(IRISA)
- IF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Viper650 found")
- SET(VISP_HAVE_VIPER650 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_VIPER650_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${RAW1394_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${CALINUX_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${IRISA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${IRISA_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${CALINUX_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RAW1394_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RT_LIBRARIES})
- ELSE(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
- MESSAGE(STATUS "Viper S650 not found")
- MESSAGE("Viper S650 robot requested but not found. Turn off USE_VIPER650 option!")
- SET(VISP_HAVE_VIPER650_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_VIPER650 OFF)
- ENDIF(RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND)
-ENDIF(USE_VIPER650)
-
-# library for Biclops pan-tilt head
-IF(USE_BICLOPS)
-# FIND_PACKAGE(BICLOPS)
- IF(BICLOPS_FOUND)
- MESSAGE(STATUS "Biclops found")
- SET(VISP_HAVE_BICLOPS TRUE) # for header vpConfig.h
- #message("BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION: ${BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION}")
- IF(BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION)
- SET(VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION TRUE) # for header vpConfig.h
- ENDIF()
- SET(VISP_HAVE_BICLOPS_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_BICLOPS= ${VISP_HAVE_BICLOPS}")
- #MESSAGE("DBG BICLOPS_INCLUDE_DIRS=${BICLOPS_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${BICLOPS_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${BICLOPS_LIBRARIES})
- #MESSAGE("BICLOPS_LIBRARIES = ${BICLOPS_LIBRARIES}")
- ELSE(BICLOPS_FOUND)
- MESSAGE(STATUS "Biclops not found")
- MESSAGE("Biclops requested but not found. Turn off USE_BICLOPS option!")
- SET(VISP_HAVE_BICLOPS_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_BICLOPS OFF)
- ENDIF(BICLOPS_FOUND)
-ENDIF(USE_BICLOPS)
-
-# library for Ptu-46 pan-tilt head
-IF(USE_PTU46)
-# FIND_PACKAGE(PTU46)
- IF(PTU46_FOUND)
- MESSAGE(STATUS "Ptu46 found")
- SET(VISP_HAVE_PTU46 TRUE) # for header vpConfig.h
- SET(VISP_HAVE_PTU46_FOUND "yes") # for ViSP-third-party.txt
- #MESSAGE("DBG VISP_HAVE_PTU46= ${VISP_HAVE_PTU46}")
- #MESSAGE("DBG PTU46_INCLUDE_DIRS=${PTU46_INCLUDE_DIRS}")
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${PTU46_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${PTU46_LIBRARIES})
- #MESSAGE("PTU46_LIBRARIES = ${PTU46_LIBRARIES}")
- ELSE(PTU46_FOUND)
- MESSAGE(STATUS "Ptu46 not found")
- MESSAGE("Ptu-46 requested but not found. Turn off USE_PTU46 option!")
- SET(VISP_HAVE_PTU46_FOUND "no") # for ViSP-third-party.txt
- # SET(USE_PTU46 OFF)
- ENDIF(PTU46_FOUND)
-ENDIF(USE_PTU46)
-
-# libraries for Pioneer mobile robots
-if(USE_ARIA)
- if(UNIX)
- # Under Unix we need Aria, pthread, dl and rt libraries
- if(ARIA_FOUND AND PTHREAD_FOUND AND RT_FOUND AND DL_FOUND)
- message(STATUS "Aria found")
- SET(VISP_HAVE_PIONEER TRUE) # for header vpConfig.h
- SET(VISP_HAVE_PIONEER_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${ARIA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${PTHREAD_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${ARIA_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${PTHREAD_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${RT_LIBRARIES})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${DL_LIBRARIES})
- endif()
- else(UNIX)
- if(ARIA_FOUND)
- message(STATUS "Aria found")
- SET(VISP_HAVE_PIONEER TRUE) # for header vpConfig.h
- SET(VISP_HAVE_PIONEER_FOUND "yes") # for ViSP-third-party.txt
- LIST(APPEND VISP_EXTERN_INCLUDE_DIRS ${ARIA_INCLUDE_DIRS})
- LIST(APPEND VISP_EXTERN_LIBRARIES ${ARIA_LIBRARIES})
- else()
- message(STATUS "Aria not found")
- endif()
- endif(UNIX)
-endif()
-
-if(USE_ZBAR)
- set(VISP_HAVE_ZBAR TRUE) # for header vpConfig.h
- set(VISP_HAVE_ZBAR_FOUND "yes") # for ViSP-third-party.txt
- list(APPEND VISP_EXTERN_INCLUDE_DIRS ${ZBAR_INCLUDE_DIRS})
- list(APPEND VISP_EXTERN_LIBRARIES ${ZBAR_LIBRARIES})
-else()
- set(VISP_HAVE_ZBAR_FOUND "no") # for ViSP-third-party.txt
-endif()
-
-if(USE_DMTX)
- set(VISP_HAVE_DMTX TRUE) # for header vpConfig.h
- set(VISP_HAVE_DMTX_FOUND "yes") # for ViSP-third-party.txt
- list(APPEND VISP_EXTERN_INCLUDE_DIRS ${DMTX_INCLUDE_DIRS})
- list(APPEND VISP_EXTERN_LIBRARIES ${DMTX_LIBRARIES})
-else()
- set(VISP_HAVE_DMTX_FOUND "no") # for ViSP-third-party.txt
-endif()
-
-#----------------------------------------------------------------------
-# Add definitions
-#----------------------------------------------------------------------
-# With Visual Studio 2005, Microsoft deprecates the standard C library, for
-# example fopen() and sprintf(), to non-portable functions fopen_s() and
-# sprintf_s(). These functions are considered by Microsoft more secure. This is
-# a worthwhile exercise ! The use of these deprecated functions causes a lot of
-# warnings. To suppress it, we add the _CRT_SECURE_NO_DEPRECATE preprocessor
-# definition
-if(WIN32 AND MSVC)
- add_definitions("-D_CRT_SECURE_NO_DEPRECATE")
-endif()
-
-#MESSAGE("Definitions: ${VISP_DEFS}")
-IF(VISP_DEFS)
- LIST(REMOVE_DUPLICATES VISP_DEFS)
-ENDIF(VISP_DEFS)
-#MESSAGE("Definitions without duplicates: ${VISP_DEFS}")
-add_definitions(${VISP_DEFS})
-
-#----------------------------------------------------------------------
-# Add intern and third party include dirs
-#----------------------------------------------------------------------
-#MESSAGE("VISP_EXTERN_INCLUDE_DIRS: ${VISP_EXTERN_INCLUDE_DIRS}")
-IF(VISP_EXTERN_INCLUDE_DIRS)
- LIST(REMOVE_DUPLICATES VISP_EXTERN_INCLUDE_DIRS)
-ENDIF(VISP_EXTERN_INCLUDE_DIRS)
-#MESSAGE("VISP_INTERN_INCLUDE_DIR: ${VISP_INTERN_INCLUDE_DIR}")
-#MESSAGE("VISP_EXTERN_INCLUDE_DIRS: ${VISP_EXTERN_INCLUDE_DIRS}")
-include_directories(BEFORE ${VISP_INTERN_INCLUDE_DIR})
-include_directories(${VISP_EXTERN_INCLUDE_DIRS})
-
-#----------------------------------------------------------------------
-# Use statically or dynamically linked CRT?
-# Default: dynamic
-#----------------------------------------------------------------------
-if(MSVC)
- include(${VISP_CMAKE_MODULE_PATH}/VISPCRTLinkage.cmake)
-endif(MSVC)
-
-#----------------------------------------------------------------------
-# Platform specific
-#----------------------------------------------------------------------
-include(${VISP_CMAKE_MODULE_PATH}/VISPDetectPlatform.cmake)
-
-# Set the path where to install the lib
-if(WIN32)
- if(DEFINED VISP_RUNTIME AND DEFINED VISP_ARCH)
- set(VISP_INSTALL_BINARIES_PREFIX "${VISP_ARCH}/${VISP_RUNTIME}/")
- else()
- message(STATUS "Can't detect runtime and/or arch")
- set(VISP_INSTALL_BINARIES_PREFIX "")
- endif()
-else()
- set(VISP_INSTALL_BINARIES_PREFIX "")
-endif()
-
-# where to install the library
-if(WIN32)
- if(VISP_STATIC)
- set(VISP_LIB_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}")
- else()
- set(VISP_LIB_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}")
- endif()
- set(VISP_BIN_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_BINDIR}")
-else()
- set(VISP_LIB_INSTALL_PATH ${CMAKE_INSTALL_LIBDIR})
- set(VISP_BIN_INSTALL_PATH ${CMAKE_INSTALL_BINDIR})
-endif()
-
-#----------------------------------------------------------------------
-# Cleanify the libraries list
-#----------------------------------------------------------------------
-IF(APPLE)
- LIST(REMOVE_DUPLICATES VISP_EXTERN_LIBRARIES)
-ENDIF()
-
-#----------------------------------------------------------------------
-# customize clean target
-#----------------------------------------------------------------------
-INCLUDE(${VISP_SOURCE_DIR}/CMakeHeaderFileList.cmake)
-
-SET(FILE_TO_CLEAN "core*;*~;gmon.out;DartTestfile.txt")
-FOREACH(header ${HEADER_ALL})
- GET_FILENAME_COMPONENT(headerName ${header} NAME)
- LIST(APPEND FILE_TO_CLEAN ${CMAKE_SOURCE_DIR}/include/visp/${headerName})
-ENDFOREACH(header)
-
-SET_DIRECTORY_PROPERTIES(PROPERTIES
- ADDITIONAL_MAKE_CLEAN_FILES "${FILE_TO_CLEAN}"
-)
-
-#----------------------------------------------------------------------
-# Propagation in src to build the library
-#----------------------------------------------------------------------
-add_subdirectory(src)
-
-#----------------------------------------------------------------------
-# Configure the files that depend on the build <binary dir> or
-# installation <install dir> usage. This is the case of:
-# vpConfig.h
-#----------------------------------------------------------------------
-
-# case 1: when ViSP is build with make; files are used in <binary dir>
-#--------------
-set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${VISP_BINARY_DIR}")
-
-SET(VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
-SET(VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
-
-# Generate the package dependent file include/visp/vpConfig.h
-CONFIGURE_FILE(${VISP_SOURCE_DIR}/include/vpConfig.h.cmake
- ${VISP_INCLUDE_DIR}/vpConfig.h
-)
-
-# case 2: when ViSP is build with make install; files are used in <install dir>
-#--------------
-IF(UNIX)
- set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}")
-ELSE()
- set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}")
-ENDIF()
-
-SET(VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
-SET(VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
-# Only if resources.cfg created by hand, we change the path to resources.cfg in install/vpConfig.h
-if(VISP_INSTALL_DIR_OGRE_RESOURCES)
- SET(VISP_HAVE_OGRE_RESOURCES_PATH ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/ogre-simulator)
-endif()
-# Only if plugins.cfg created by hand, we change the path to plugins.cfg in install/vpConfig.h
-if(VISP_INSTALL_DIR_OGRE_RESOURCES)
- SET(VISP_HAVE_OGRE_PLUGINS_PATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator)
-endif()
-
-#----------------------------------------------------------------------
-# Generate the ViSP-third-party.txt information file
-#----------------------------------------------------------------------
-CONFIGURE_FILE(${VISP_SOURCE_DIR}/ViSP-third-party.txt.cmake
- ${VISP_BINARY_DIR}/ViSP-third-party.txt
-)
-
-#----------------------------------------------------------------------
-# Create and install visp-config.1.gz man page
-#----------------------------------------------------------------------
-if(UNIX)
- FIND_PROGRAM(GZIP gzip)
- file(MAKE_DIRECTORY ${VISP_BINARY_DIR}/doc/man/man1)
- ADD_CUSTOM_COMMAND(
- OUTPUT ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
- COMMAND ${GZIP} --best -c ${CMAKE_CURRENT_SOURCE_DIR}/doc/man/man1/visp-config.1 > ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
- DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/doc/man/man1/visp-config.1
- )
- ADD_CUSTOM_TARGET(man ALL
- DEPENDS ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
- )
- INSTALL(FILES
- ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/man/man1
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- MARK_AS_ADVANCED(GZIP)
-endif()
-
-#----------------------------------------------------------------------
-# Export the library
-#----------------------------------------------------------------------
-# we need to adapt VISP_SCENES_DIR and VISP_ROBOT_ARMS_DIR
-# here set to /usr/share/visp/data
-if(UNIX)
- foreach(SCENE ${WIREFRAME_SCENES})
- install(FILES
- "${VISP_SOURCE_DIR}/src/simulator/wireframe-simulator/scene/${SCENE}"
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/wireframe-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endforeach()
- foreach(SCENE ${ROBOT_ARMS_SCENES})
- install(FILES
- "${VISP_SOURCE_DIR}/src/robot/simulator-robot/arms/${SCENE}"
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/robot-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endforeach()
-else()
- foreach(SCENE ${WIREFRAME_SCENES})
- install(FILES
- "${VISP_SOURCE_DIR}/src/simulator/wireframe-simulator/scene/${SCENE}"
- DESTINATION data/wireframe-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endforeach()
- foreach(SCENE ${ROBOT_ARMS_SCENES})
- install(FILES
- "${VISP_SOURCE_DIR}/src/robot/simulator-robot/arms/${SCENE}"
- DESTINATION data/robot-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries
- )
- endforeach()
-endif()
-
-#----------------------------------------------------------------------
-# For CPack packaging tool
-#----------------------------------------------------------------------
-
-option(BUILD_PACKAGE "Configure ViSP packaging" OFF)
-if (BUILD_PACKAGE)
- if(UNIX AND NOT APPLE AND NOT WIN32) # =linux
- option(BUILD_PACKAGE_DEBIAN "Build debian package" ON)
- option(BUILD_PACKAGE_RPM "Build rpm package" ON)
- endif()
-
- include (${VISP_SOURCE_DIR}/CMakeModules/CPackConfig.cmake)
-endif(BUILD_PACKAGE)
-
-#----------------------------------------------------------------------
-# Configure the file describing how to use ViSP. VISPConfig.cmake
-# is the main file configuring a CMake package.
-# . Exports build settings and dependencies for projects using ViSP as a
-# third party project.
-# . Create and install files for simple use of find_package(VISP)
-# by other cmakified "user" projects and libraries depending on ViSP.
-# (see "Mastering CMake", pp.72)
-# . To use ViSP in a third party project based on CMake:
-# find_package(VISP REQUIRED)
-# include_directories(${VISP_INCLUDE_DIRS})
-# target_link_libraries(<target> ${VISP_LIBRARIES})
-#----------------------------------------------------------------------
-include(${VISP_CMAKE_MODULE_PATH}/VISPGenerateConfig.cmake)
-
-#----------------------------------------------------------------------
-# Generate the package dependent visp-config shell script for projects which
-# are not using CMake:
-# Usage:
-# visp-config --cflags ...
-#----------------------------------------------------------------------
-include(${VISP_CMAKE_MODULE_PATH}/VISPGeneratePkgConfigScript.cmake)
-
-#----------------------------------------------------------------------
-# Propagation in sub dirs to build demo, example, test, tutorial
-#----------------------------------------------------------------------
-if(BUILD_TESTS)
- #----------------------------------------------------------------------
- # For Dart server and tests
- # We use CDash set through CTestConfig.cmake file
- # Dashboards are sent to http://cdash.irisa.fr/CDash/index.php?project=ViSP
- #----------------------------------------------------------------------
- enable_testing()
- include(Dart)
- mark_as_advanced(DART_ROOT)
- mark_as_advanced(BUILD_TESTING)
- #
- # Test coverage specific code
- #
- if(CMAKE_COMPILER_IS_GNUCXX AND NOT BUILD_SHARED_LIBS AND CMAKE_BUILD_TYPE MATCHES "Debug")
- option(BUILD_TEST_COVERAGE "Enable test coverage." OFF)
- endif()
-
- if(BUILD_TEST_COVERAGE)
- # Add build options for test coverage. Currently coverage is only supported
- # on gcc compiler
- # Because using -fprofile-arcs with shared lib can cause problems like:
- # hidden symbol `__bb_init_func', we add this option only for static
- # library build
- message(STATUS "Add -ftest-coverage -fprofile-arcs compiler options")
- set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ftest-coverage -fprofile-arcs")
- endif()
-
- add_subdirectory(test)
-endif()
-
-if(BUILD_DEMOS)
- add_subdirectory(demo)
-endif()
-if(BUILD_EXAMPLES)
- add_subdirectory(example)
-endif()
-if(BUILD_TUTORIALS)
- set(VISP_DIR ${PROJECT_BINARY_DIR})
- mark_as_advanced(VISP_DIR)
- mark_as_advanced(VISP_INCLUDE_DIRS)
- add_subdirectory(tutorial)
-endif()
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP overall configuration file. Detect third party libraries (X11, GTK, ...)
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Need to be befor project(VISP) to work
+if(WIN32)
+ set(CMAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/install" CACHE PATH "Installation Directory")
+endif()
+
+project(VISP C CXX)
+
+cmake_minimum_required(VERSION 2.8.3)
+
+list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
+
+include(cmake/VISPUtils.cmake)
+
+vp_clear_vars(VISPModules_TARGETS)
+
+#-----------------------------------------------------------------------------
+# VISP version number. An even minor number corresponds to releases.
+set(VISP_VERSION_MAJOR "3")
+set(VISP_VERSION_MINOR "0")
+set(VISP_VERSION_PATCH "0")
+set(VISP_VERSION "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}")
+# Package revision number
+set(VISP_REVISION "1")
+
+find_file(GNU_INSTALL_DIRS_FROM_CMAKE NAMES GNUInstallDirs.cmake PATHS ${CMAKE_ROOT}/Modules)
+mark_as_advanced(GNU_INSTALL_DIRS_FROM_CMAKE)
+if(GNU_INSTALL_DIRS_FROM_CMAKE)
+ include(${CMAKE_ROOT}/Modules/GNUInstallDirs.cmake)
+else()
+ include(cmake/GNUInstallDirs.cmake)
+endif()
+
+# the include directory we depend on
+set(VISP_INCLUDE_DIR ${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR})
+set(VISP_DOC_DIR "${VISP_BINARY_DIR}/doc")
+
+# The location in which to install VISP libraries.
+set(LIBRARY_OUTPUT_PATH ${VISP_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
+# The location in which to install some VISP binaries.
+set(BINARY_OUTPUT_PATH ${VISP_BINARY_DIR}/${CMAKE_INSTALL_BINDIR})
+
+include_directories(${VISP_BINARY_DIR}/include)
+
+if(WIN32)
+ # Postfix of .lib and .dll
+ set(VISP_DEBUG_POSTFIX "d")
+ set(VISP_DLLVERSION "${VISP_VERSION_MAJOR}${VISP_VERSION_MINOR}${VISP_VERSION_PATCH}")
+else()
+ set(VISP_DEBUG_POSTFIX "")
+ set(VISP_DLLVERSION "")
+endif()
+
+# ----------------------------------------------------------------------------
+# Handle always full RPATH
+# http://www.cmake.org/Wiki/CMake_RPATH_handling
+# ----------------------------------------------------------------------------
+
+# avoid CMP0042 warning
+if(APPLE)
+ set(CMAKE_MACOSX_RPATH ON)
+endif()
+
+# use, i.e. don't skip the full RPATH for the build tree
+set(CMAKE_SKIP_BUILD_RPATH FALSE)
+
+# when building, don't use the install RPATH already
+# (but later on when installing)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+
+# add the automatically determined parts of the RPATH
+# which point to directories outside the build tree to the install RPATH
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+# the RPATH to be used when installing, but only if it's not a system directory
+list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" isSystemDir)
+if("${isSystemDir}" STREQUAL "-1")
+ set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+endif("${isSystemDir}" STREQUAL "-1")
+
+# ----------------------------------------------------------------------------
+# Path for additional contrib modules
+# ----------------------------------------------------------------------------
+set(VISP_CONTRIB_MODULES_PATH "" CACHE PATH "Where to look for additional contrib ViSP modules")
+
+# Get the OS
+set(OS ${CMAKE_SYSTEM_NAME})
+
+set(OGRE_HOME $ENV{OGRE_HOME})
+if(OGRE_HOME)
+ # replace \ with / especially for windows
+ STRING(REGEX REPLACE "\\\\" "/" OGRE_HOME ${OGRE_HOME})
+endif()
+
+# add the path to detect Ogre3D
+if(WIN32)
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
+endif(WIN32)
+
+if(UNIX)
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/cmake")
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
+ list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/OGRE/cmake")
+ list(APPEND CMAKE_MODULE_PATH "/usr/lib/OGRE/cmake")
+ list(APPEND CMAKE_MODULE_PATH "/usr/local/lib64/OGRE/cmake")
+ list(APPEND CMAKE_MODULE_PATH "/usr/lib64/OGRE/cmake")
+ list(APPEND CMAKE_MODULE_PATH "/usr/share/OGRE/cmake/modules")
+endif(UNIX)
+
+#--------------------------------------------------------------------
+# Option management
+#--------------------------------------------------------------------
+
+# Choose static or shared libraries.
+VP_OPTION(BUILD_SHARED_LIBS "" "" "Build ViSP shared libraries (.dll/.so) instead of static ones (.lib/.a)" "" ON)
+# Build examples as an option.
+VP_OPTION(BUILD_EXAMPLES "" "" "Build ViSP examples" "" ON)
+# Build examples as an option.
+VP_OPTION(BUILD_TESTS "" "" "Build ViSP tests" "" ON)
+VP_OPTION(BUILD_COVERAGE "" "" "Enables test coverage" "" OFF IF (BUILD_TESTS AND CMAKE_COMPILER_IS_GNUCXX AND NOT BUILD_SHARED_LIBS AND CMAKE_BUILD_TYPE MATCHES "Debug"))
+
+# Build demos as an option.
+VP_OPTION(BUILD_DEMOS "" "" "Build ViSP demos" "" ON)
+# Build demos as an option.
+VP_OPTION(BUILD_TUTORIALS "" "" "Build ViSP tutorials" "" ON)
+# Build deprecated functions as an option.
+VP_OPTION(BUILD_DEPRECATED_FUNCTIONS "" "" "Build deprecated functionalities" "" ON)
+# Debug and trace cflags
+VP_OPTION(ACTIVATE_DEBUG_TRACE "" "" "Enable debug and trace printings" "" ON)
+
+VP_OPTION(BUILD_WITH_STATIC_CRT "" "" "Enables use of staticaly linked CRT for staticaly linked ViSP" "" ON IF MSVC)
+
+# Note that it is better to set MOMENTS_COMBINE_MATRICES to OFF
+VP_OPTION(MOMENTS_COMBINE_MATRICES "" "" "Use linear combination of matrices instead of linear combination of moments to compute interaction matrices." "MOMENTS_COMBINE_MATRICES" OFF)
+VP_OPTION(ENABLE_TEST_WITHOUT_DISPLAY "" "" "Don't use display features when testing" "ENABLE_TEST_WITHOUT_DISPLAY" ON)
+
+if(ENABLE_TEST_WITHOUT_DISPLAY)
+ set(OPTION_TO_DESACTIVE_DISPLAY "-d")
+endif()
+
+# Check for Inria's robot drivers
+VP_CHECK_PACKAGE(RAW1394)
+VP_CHECK_PACKAGE(RT)
+VP_CHECK_PACKAGE(CALINUX)
+VP_CHECK_PACKAGE(IRISA)
+# Try to determine if Inria's NAS server hosting /udd/ is available
+VP_CHECK_PACKAGE(NAS)
+# check for linux/parport.h
+VP_CHECK_PACKAGE(PARPORT)
+# Find IsNaN
+VP_CHECK_PACKAGE(IsNaN)
+# Find IsInf
+VP_CHECK_PACKAGE(IsInf)
+# Find Round
+VP_CHECK_PACKAGE(Round)
+# OpenGL
+VP_CHECK_PACKAGE(OpenGL)
+# for pioneer
+VP_CHECK_PACKAGE(RT)
+VP_CHECK_PACKAGE(DL)
+
+VP_OPTION(USE_AFMA4 "" "" "Include Afma4 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND))
+VP_OPTION(USE_AFMA6 "" "" "Include Afma6 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND))
+VP_OPTION(USE_VIPER650 "" "" "Include Viper s650 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND))
+VP_OPTION(USE_VIPER850 "" "" "Include Viper s850 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND))
+VP_OPTION(USE_DC1394 DC1394 "" "Include dc1394 support" "" ON IF UNIX)
+VP_OPTION(USE_V4L2 V4L2 "" "Include v4l2 support" "" ON IF UNIX)
+VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF UNIX)
+VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX)
+VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32)
+VP_OPTION(USE_GDI GDI "" "Include gdi support" "" ON IF WIN32)
+VP_OPTION(USE_DIRECT3D DIRECT3D "" "Include d3d support" "" ON IF WIN32)
+VP_OPTION(USE_DIRECTSHOW DIRECTSHOW "" "Include dshow support" "" ON IF WIN32)
+VP_OPTION(USE_OPENMP OpenMP "" "Include openmp support" "" ON)
+VP_OPTION(USE_CPP11 CPP11 "" "Include c++11 support" "" OFF)
+# Since the FindLAPACK.cmake provided with CMake is for Fortran language,
+# in CMakeModules we have added FindLAPACK_C.cmake for C language
+VP_OPTION(USE_LAPACK LAPACK_C "" "Include lapack support" "" ON)
+VP_OPTION(USE_GSL GSL "" "Include gsl support" "" ON)
+VP_OPTION(USE_COIN3D "Coin3D;MyCoin3D" "" "Include coin3d support" "" ON IF OPENGL_FOUND)
+VP_OPTION(USE_YARP YARP QUIET "Include yarp support" "YARP_DIR" ON)
+VP_OPTION(USE_OGRE OGRE QUIET "Include Ogre support" "OGRE_DIR" ON)
+VP_OPTION(USE_OIS OIS QUIET "Include Ogre/ois support" "OIS_DIR" ON IF USE_OGRE)
+VP_OPTION(USE_LIBFREENECT LIBFREENECT "" "Include libfreenect support" "" ON)
+VP_OPTION(USE_LIBUSB_1 LIBUSB_1 "" "Include libusb-1 support" "" ON)
+VP_OPTION(USE_SOWIN SOWIN "" "Include Coin/SoWin support" "" OFF IF (WIN32 AND USE_COIN3D))
+VP_OPTION(USE_SOQT SOQT "" "Include Coin/SoQt support" "" OFF IF USE_COIN3D)
+VP_OPTION(USE_QT Qt "" "Include Coin/SoQt/Qt support" "" ON IF USE_SOQT)
+VP_OPTION(USE_SOXT SOXT "" "Include Coin/SoXt support" "" OFF IF USE_COIN3D)
+VP_OPTION(USE_PTHREAD PTHREAD "" "Include pthread support" "" ON)
+VP_OPTION(USE_XML2 XML2 "" "Include xml support" "" ON)
+VP_OPTION(USE_OPENCV OpenCV QUIET "Include OpenCV support" "OpenCV_DIR;OpenCV_FOUND;OPENCV_FOUND" ON)
+VP_OPTION(USE_ZLIB "ZLIB;MyZLIB" "" "Include zlib support" "" ON)
+VP_OPTION(USE_X11 X11 "" "Include X11 support" "" ON)
+# The native FindGTK2.cmake doesn't consider libgobject-2.0 that is
+# requested by ViSP. That's why we use our FindMyGTK2.cmake
+VP_OPTION(USE_GTK2 MyGTK2 "" "Include gtk2 support" "" OFF)
+VP_OPTION(USE_JPEG "JPEG;MyJPEG" "" "Include jpeg support" "" ON)
+VP_OPTION(USE_PNG "PNG;MyPNG" "" "Include png support" "" ON)
+VP_OPTION(USE_FFMPEG FFMPEG "" "Include ffmpeg support" "" OFF)
+# To control Pioneer mobile robots, under UNIX we need Aria, pthread, rt and dl 3rd party libraries
+VP_OPTION(USE_ARIA ARIA "" "Include aria support" "" ON)
+#VP_OPTION(USE_RT RT "" "Include rt support" "" ON)
+#VP_OPTION(USE_DL DL "" "Include dl support" "" ON)
+# bar codes
+VP_OPTION(USE_ZBAR ZBAR "" "Include zbar support" "" ON)
+VP_OPTION(USE_DMTX DMTX "" "Include dmtx support" "" ON)
+
+#----------------------------------------------------------------------
+# For Dart server and tests
+# We use CDash set through CTestConfig.cmake file
+# Dashboards are sent to http://cdash.irisa.fr/CDash/index.php?project=ViSP
+#----------------------------------------------------------------------
+if(BUILD_TESTS)
+ enable_testing()
+ mark_as_advanced(DART_ROOT)
+ mark_as_advanced(BUILD_TESTING)
+endif()
+
+#----------------------------------------------------------------------
+# Try to find doxygen for documentation generation
+# Use "make visp_doc" target to generate the documentation
+#----------------------------------------------------------------------
+find_package(Doxygen)
+if(DOXYGEN_FOUND)
+ set(VISP_HAVE_DOXYGEN "yes") # for header vpConfig.h
+ set(VISP_HAVE_DOXYGEN_FOUND "yes") # for ViSP-third-party.txt
+ ## we need latex for doxygen because of the formulas
+ find_package(LATEX)
+ if(NOT LATEX_COMPILER)
+ message(STATUS "latex command LATEX_COMPILER not found but usually required. You will probably get warnings and user interaction on doxy run.")
+ endif()
+ if(NOT MAKEINDEX_COMPILER)
+ message(STATUS "makeindex command MAKEINDEX_COMPILER not found but usually required.")
+ endif()
+ if(NOT DVIPS_CONVERTER)
+ message(STATUS "dvips command DVIPS_CONVERTER not found but usually required.")
+ endif()
+
+ configure_file(${VISP_SOURCE_DIR}/doc/config-doxygen.in
+ ${VISP_DOC_DIR}/config-doxygen
+ @ONLY )
+
+ configure_file(${VISP_SOURCE_DIR}/doc/mainpage.doc.in
+ ${VISP_DOC_DIR}/mainpage.doc
+ @ONLY )
+else()
+ set(VISP_HAVE_DOXYGEN "no") # for header vpConfig.h
+ set(VISP_HAVE_DOXYGEN_FOUND "no") # for ViSP-third-party.txt
+endif()
+
+# ----------------------------------------------------------------------------
+# Solution folders:
+# ----------------------------------------------------------------------------
+if(MSVC_IDE OR CMAKE_GENERATOR MATCHES Xcode)
+ option(ENABLE_SOLUTION_FOLDERS "Solution folder in Visual Studio or in other IDEs" ON)
+endif()
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_property(GLOBAL PROPERTY USE_FOLDERS ON)
+ set_property(GLOBAL PROPERTY PREDEFINED_TARGETS_FOLDER "CMakeTargets")
+endif()
+
+# Extra ViSP targets: uninstall, etc.
+include(cmake/VISPExtraTargets.cmake)
+
+include(CheckLibraryExists)
+
+# ----------------------------------------------------------------------------
+# Check for system libs
+# ----------------------------------------------------------------------------
+if(UNIX)
+ # try to found -lm requested on some platforms to link with X11
+ find_library(M_LIBRARY NAMES m)
+ mark_as_advanced(M_LIBRARY)
+ if(M_LIBRARY)
+ list(APPEND VISP_LINKER_LIBS ${M_LIBRARY})
+ endif()
+ # try to found -lsocket -lnsl requested for vpNetwork and vpSickLDMRS
+ find_library(SOCKET_LIBRARY NAMES socket)
+ find_library(NSL_LIBRARY NAMES nsl)
+ mark_as_advanced(SOCKET_LIBRARY NSL_LIBRARY)
+ if (SOCKET_LIBRARY)
+ list(APPEND VISP_LINKER_LIBS ${SOCKET_LIBRARY})
+ endif()
+ if (NSL_LIBRARY)
+ list(APPEND VISP_LINKER_LIBS ${NSL_LIBRARY})
+ endif()
+endif()
+
+#----------------------------------------------------------------------
+# Add definitions
+#----------------------------------------------------------------------
+# With Visual Studio 2005, Microsoft deprecates the standard C library, for
+# example fopen() and sprintf(), to non-portable functions fopen_s() and
+# sprintf_s(). These functions are considered by Microsoft more secure. This is
+# a worthwhile exercise ! The use of these deprecated functions causes a lot of
+# warnings. To suppress it, we add the _CRT_SECURE_NO_DEPRECATE preprocessor
+# definition
+if(WIN32 AND MSVC)
+ add_definitions("-D_CRT_SECURE_NO_DEPRECATE")
+endif()
+
+#----------------------------------------------------------------------
+# Use statically or dynamically linked CRT?
+# Default: dynamic
+#----------------------------------------------------------------------
+if(MSVC)
+ include(cmake/VISPCRTLinkage.cmake)
+endif(MSVC)
+
+#----------------------------------------------------------------------
+# Platform specific
+#----------------------------------------------------------------------
+include(cmake/VISPDetectPlatform.cmake)
+
+# Set the path where to install the lib
+if(WIN32)
+ if(DEFINED VISP_RUNTIME AND DEFINED VISP_ARCH)
+ set(VISP_INSTALL_BINARIES_PREFIX "${VISP_ARCH}/${VISP_RUNTIME}/")
+ else()
+ message(STATUS "Can't detect runtime and/or arch")
+ set(VISP_INSTALL_BINARIES_PREFIX "")
+ endif()
+else()
+ set(VISP_INSTALL_BINARIES_PREFIX "")
+endif()
+
+# where to install the library
+if(WIN32)
+ if(VISP_STATIC)
+ set(VISP_LIB_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}")
+ else()
+ set(VISP_LIB_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}")
+ endif()
+ set(VISP_BIN_INSTALL_PATH "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_BINDIR}")
+else()
+ set(VISP_LIB_INSTALL_PATH ${CMAKE_INSTALL_LIBDIR})
+ set(VISP_BIN_INSTALL_PATH ${CMAKE_INSTALL_BINDIR})
+endif()
+
+
+#----------------------------------------------------------------------
+# Configure the files that depend on the build <binary dir> or
+# installation <install dir> usage. This is the case of:
+# vpConfig.h
+#----------------------------------------------------------------------
+
+# case 1: when ViSP is build with make; files are used in <binary dir>
+#--------------
+set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${VISP_BINARY_DIR}")
+
+set(VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
+set(VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
+
+# case 2: when ViSP is build with make install; files are used in <install dir>
+#--------------
+IF(UNIX)
+ set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}")
+ELSE()
+ set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}")
+ENDIF()
+
+list(APPEND VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
+list(APPEND VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
+# Only if resources.cfg created by hand, we change the path to resources.cfg in install/vpConfig.h
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ list(APPEND VISP_HAVE_OGRE_RESOURCES_PATH "${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/ogre-simulator")
+endif()
+# append to VISP_HAVE_OGRE_PLUGINS_PATH the path of the installed plugins.cfg file
+# to be able to use ViSP from build tree or install tree
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ list(APPEND VISP_HAVE_OGRE_PLUGINS_PATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator")
+endif()
+
+#----------------------------------------------------------------------
+# Create and install visp-config.1.gz man page
+#----------------------------------------------------------------------
+if(UNIX)
+ FIND_PROGRAM(GZIP gzip)
+ file(MAKE_DIRECTORY ${VISP_BINARY_DIR}/doc/man/man1)
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
+ COMMAND ${GZIP} --best -c ${CMAKE_CURRENT_SOURCE_DIR}/doc/man/man1/visp-config.1 > ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
+ DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/doc/man/man1/visp-config.1
+ )
+ ADD_CUSTOM_TARGET(man ALL
+ DEPENDS ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
+ )
+ INSTALL(FILES
+ ${VISP_BINARY_DIR}/doc/man/man1/visp-config.1.gz
+ DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/man/man1
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ MARK_AS_ADVANCED(GZIP)
+endif()
+
+#----------------------------------------------------------------------
+# Modules
+#----------------------------------------------------------------------
+include(cmake/VISPModule.cmake)
+
+# process subdirectories
+add_subdirectory(modules)
+
+#-----------------------------------------------------------------------------
+# Add extra compilation flags under UNIX
+#-----------------------------------------------------------------------------
+include(cmake/AddExtraCompilationFlags.cmake) # should be called after FindOpenMP
+vp_add_extra_compilation_flags()
+
+#-------------------------------------------------------------------------------
+# specific things that need to be updated in vpConfig.h and ViSP-third-party.txt
+#-------------------------------------------------------------------------------
+VP_SET(VISP_HAVE_OPENMP TRUE IF USE_OPENMP)
+VP_SET(VISP_HAVE_OPENCV TRUE IF (BUILD_MODULE_visp_core AND USE_OPENCV))
+VP_SET(VISP_HAVE_X11 TRUE IF (BUILD_MODULE_visp_core AND USE_X11))
+VP_SET(VISP_HAVE_GTK TRUE IF (BUILD_MODULE_visp_core AND USE_GTK2))
+VP_SET(VISP_HAVE_GDI TRUE IF (BUILD_MODULE_visp_core AND USE_GDI))
+VP_SET(VISP_HAVE_D3D9 TRUE IF (BUILD_MODULE_visp_core AND USE_DIRECT3D))
+VP_SET(VISP_HAVE_JPEG TRUE IF (BUILD_MODULE_visp_core AND USE_JPEG))
+VP_SET(VISP_HAVE_PNG TRUE IF (BUILD_MODULE_visp_core AND USE_PNG))
+VP_SET(VISP_HAVE_YARP TRUE IF (BUILD_MODULE_visp_core AND USE_YARP))
+VP_SET(VISP_HAVE_GSL TRUE IF (BUILD_MODULE_visp_core AND USE_GSL))
+VP_SET(VISP_HAVE_LAPACK_C TRUE IF (BUILD_MODULE_visp_core AND USE_LAPACK))
+VP_SET(VISP_HAVE_PTHREAD TRUE IF (BUILD_MODULE_visp_core AND USE_PTHREAD))
+VP_SET(VISP_HAVE_XML2 TRUE IF (BUILD_MODULE_visp_core AND USE_XML2))
+VP_SET(VISP_HAVE_FFMPEG TRUE IF (BUILD_MODULE_visp_core AND USE_FFMPEG))
+
+VP_SET(VISP_HAVE_OGRE TRUE IF (BUILD_MODULE_visp_ar AND USE_OGRE))
+VP_SET(VISP_HAVE_OIS TRUE IF (BUILD_MODULE_visp_ar AND USE_OIS))
+VP_SET(VISP_HAVE_COIN3D TRUE IF (BUILD_MODULE_visp_ar AND USE_COIN3D))
+VP_SET(VISP_HAVE_SOWIN TRUE IF (BUILD_MODULE_visp_ar AND USE_SOWIN))
+VP_SET(VISP_HAVE_SOXT TRUE IF (BUILD_MODULE_visp_ar AND USE_SOXT))
+VP_SET(VISP_HAVE_SOQT TRUE IF (BUILD_MODULE_visp_ar AND USE_SOQT))
+VP_SET(VISP_HAVE_QT TRUE IF (BUILD_MODULE_visp_ar AND USE_QT))
+
+VP_SET(VISP_HAVE_ZBAR TRUE IF (BUILD_MODULE_visp_detection AND USE_ZBAR))
+VP_SET(VISP_HAVE_DMTX TRUE IF (BUILD_MODULE_visp_detection AND USE_DMTX))
+
+VP_SET(VISP_HAVE_AFMA4 TRUE IF (BUILD_MODULE_visp_robot AND USE_AFMA4))
+VP_SET(VISP_HAVE_AFMA6 TRUE IF (BUILD_MODULE_visp_robot AND USE_AFMA6))
+VP_SET(VISP_HAVE_VIPER650 TRUE IF (BUILD_MODULE_visp_robot AND USE_VIPER650))
+VP_SET(VISP_HAVE_VIPER850 TRUE IF (BUILD_MODULE_visp_robot AND USE_VIPER850))
+VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS))
+VP_SET(VISP_HAVE_PTU46 TRUE IF (BUILD_MODULE_visp_robot AND USE_PTU46))
+#VP_SET(VISP_HAVE_PIONEER TRUE IF (BUILD_MODULE_visp_robot AND USE_ARIA))
+if(BUILD_MODULE_visp_robot AND USE_ARIA)
+ if(UNIX AND USE_PTHREAD AND RT_FOUND AND DL_FOUND)
+ set(VISP_HAVE_PIONEER TRUE)
+ elseif(NOT UNIX)
+ set(VISP_HAVE_PIONEER TRUE)
+ endif()
+endif()
+VP_SET(VISP_HAVE_COIN3D TRUE IF (BUILD_MODULE_visp_robot AND USE_COIN3D))
+
+VP_SET(VISP_HAVE_V4L2 TRUE IF (BUILD_MODULE_visp_sensor AND USE_V4L2))
+VP_SET(VISP_HAVE_DC1394 TRUE IF (BUILD_MODULE_visp_sensor AND USE_DC1394))
+VP_SET(VISP_HAVE_CMU1394 TRUE IF (BUILD_MODULE_visp_sensor AND USE_CMU1394))
+VP_SET(VISP_HAVE_DIRECTSHOW TRUE IF (BUILD_MODULE_visp_sensor AND USE_DIRECTSHOW))
+VP_SET(VISP_HAVE_LIBFREENECT TRUE IF (BUILD_MODULE_visp_sensor AND USE_LIBFREENECT))
+VP_SET(VISP_HAVE_LIBUSB_1 TRUE IF (BUILD_MODULE_visp_sensor AND USE_LIBUSB_1))
+
+#VP_SET(VISP_HAVE_COIN3D TRUE IF (BUILD_MODULE_visp_mbt AND USE_COIN3D))
+
+
+VP_SET(VISP_BUILD_SHARED_LIBS TRUE IF BUILD_SHARED_LIBS) # for header vpConfig.h
+VP_SET(VISP_HAVE_DC1394_CAMERA_ENUMERATE TRUE IF (USE_DC1394 AND DC1394_CAMERA_ENUMERATE_FOUND)) # for header vpConfig.h
+VP_SET(VISP_HAVE_DC1394_FIND_CAMERAS TRUE IF (USE_DC1394 AND DC1394_FIND_CAMERAS_FOUND)) # for header vpConfig.h
+VP_SET(VISP_HAVE_D3D9 TRUE IF USE_DIRECT3D) # for header vpConfig.h
+VP_SET(VISP_HAVE_GTK TRUE IF USE_GTK2) # for header vpConfig.h
+
+# Check if libfreenect dependencies (ie libusb-1.0 and libpthread) are available
+if(USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD)
+ if(LIBFREENECT_FOUND AND LIBUSB_1_FOUND AND PTHREAD_FOUND)
+ set(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES TRUE)
+ set(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES_FOUND "yes") # for ViSP-third-party.txt
+
+ # The material is found. Check if libfreenect is an old version
+ include(CheckCXXSourceCompiles)
+ set(CMAKE_REQUIRED_LIBRARIES ${LIBFREENECT_LIBRARIES} ${PTHREAD_LIBRARIES} ${LIBUSB_1_LIBRARIES})
+ set(CMAKE_REQUIRED_INCLUDES ${LIBFREENECT_INCLUDE_DIRS} ${PTHREAD_INCLUDE_DIRS} ${LIBUSB_1_INCLUDE_DIRS})
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <libfreenect.hpp>
+
+ class vpMyKinect : public Freenect::FreenectDevice
+ {
+ };
+
+ int main()
+ {
+ Freenect::Freenect<vpMyKinect> freenect;
+ }
+ " LIBFREENECT_IS_OLD_VERSION)
+ #MESSAGE("LIBFREENECT_IS_OLD_VERSION: ${LIBFREENECT_IS_OLD_VERSION}")
+ if(LIBFREENECT_IS_OLD_VERSION)
+ set(VISP_HAVE_LIBFREENECT_OLD TRUE) # for header vpConfig.h
+ else()
+ set(VISP_HAVE_LIBFREENECT_OLD FALSE) # for header vpConfig.h
+ endif()
+ endif()
+endif()
+
+# check OpenCV nonfree modules and version
+if(USE_OPENCV)
+ set(VISP_HAVE_OPENCV_VERSION "(${OpenCV_VERSION_MAJOR}*65536 + ${OpenCV_VERSION_MINOR}*256 + ${OpenCV_VERSION_PATCH})") # for vpConfig.h
+ if(OpenCV_VERSION)
+ if(OpenCV_VERSION VERSION_LESS "2.4.0")
+ message(STATUS "opencv nonfree module found")
+ set(VISP_HAVE_OPENCV_NONFREE TRUE) # for header vpConfig.h
+ elseif(OPENCV_NONFREE_FOUND) # OpenCV < 3.0.0
+ message(STATUS "opencv xfeatures2d module found")
+ set(VISP_HAVE_OPENCV_NONFREE TRUE) # for header vpConfig.h
+ elseif(OPENCV_XFEATURES2D_FOUND) # OpenCV >= 3.0.0
+ set(VISP_HAVE_OPENCV_XFEATURES2D TRUE) # for header vpConfig.h
+ else()
+ message(STATUS "opencv nonfree or xfeature2d module not found")
+ endif()
+ else()
+ message(STATUS "opencv nonfree not found")
+ set(VISP_HAVE_OPENCV_VERSION "(0)") # for vpConfig.h
+ endif()
+endif()
+
+# coin and gui
+set(VISP_HAVE_COIN3D_AND_GUI_FOUND "no") # for ViSP-third-party.txt
+set(VISP_HAVE_QT3_FOUND "no") # for ViSP-third-party.txt
+set(VISP_HAVE_QT4_FOUND "no") # for ViSP-third-party.txt
+if(USE_SOWIN)
+ set(VISP_HAVE_COIN3D_AND_GUI TRUE) # for header vpConfig.h
+ set(VISP_HAVE_COIN3D_AND_GUI_FOUND "yes") # for ViSP-third-party.txt
+elseif(USE_SOXT)
+ set(VISP_HAVE_COIN3D_AND_GUI TRUE) # for header vpConfig.h
+ set(VISP_HAVE_COIN3D_AND_GUI_FOUND "yes") # for ViSP-third-party.txt
+elseif(USE_SOQT AND USE_QT)
+ set(VISP_HAVE_COIN3D_AND_GUI TRUE) # for header vpConfig.h
+ set(VISP_HAVE_COIN3D_AND_GUI_FOUND "yes") # for ViSP-third-party.txt
+ if(DESIRED_QT_VERSION MATCHES 3)
+ set(VISP_HAVE_QT3_FOUND "yes") # for ViSP-third-party.txt
+ elseif(DESIRED_QT_VERSION MATCHES 4)
+ set(VISP_HAVE_QT4_FOUND "yes") # for ViSP-third-party.txt
+ endif()
+endif()
+
+# Ogre plugins and resources
+if(USE_OGRE)
+ include(cmake/OgreTools.cmake)
+ vp_set_ogre_media()
+endif()
+
+# Find isnan macro (C-style)
+VP_SET(VISP_HAVE_FUNC_ISNAN TRUE IF HAVE_FUNC_ISNAN) # for header vpConfig.h
+# Find std::isnan function (cmath)
+VP_SET(VISP_HAVE_FUNC_STD_ISNAN TRUE IF HAVE_FUNC_STD_ISNAN) # for header vpConfig.h
+# Find _isnan function for MSVC
+VP_SET(VISP_HAVE_FUNC__ISNAN TRUE IF HAVE_FUNC__ISNAN) # for header vpConfig.h
+# Find isinf macro (C-style)
+VP_SET(VISP_HAVE_FUNC_ISINF TRUE IF HAVE_FUNC_ISINF) # for header vpConfig.h
+# Find std::isinf function (cmath)
+VP_SET(VISP_HAVE_FUNC_STD_ISINF TRUE IF HAVE_FUNC_STD_ISINF) # for header vpConfig.h
+# Find _finite function for MSVC
+VP_SET(VISP_HAVE_FUNC__FINITE TRUE IF HAVE_FUNC__FINITE) # for header vpConfig.h
+# Find round function (math.h)
+VP_SET(VISP_HAVE_FUNC_ROUND TRUE IF HAVE_FUNC_ROUND) # for header vpConfig.h
+# Find std::round function (cmath)
+VP_SET(VISP_HAVE_FUNC_STD_ROUND TRUE IF HAVE_FUNC_STD_ROUND) # for header vpConfig.h
+
+VP_SET(VISP_HAVE_ACCESS_TO_NAS TRUE IF NAS_FOUND) # for header vpConfig.h
+VP_SET(VISP_BUILD_DEPRECATED_FUNCTIONS TRUE IF BUILD_DEPRECATED_FUNCTIONS) # for header vpConfig.h
+VP_SET(MOMENTS_COMBINE_MATRICES TRUE IF VISP_MOMENTS_COMBINE_MATRICES) # for header vpConfig.h
+VP_SET(VISP_USE_MSVC TRUE IF MSVC) # for header vpConfig.h
+VP_SET(VISP_HAVE_CPP11_COMPATIBILITY TRUE IF USE_CPP11) # for header vpConfig.h
+VP_SET(VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION TRUE IF (USE_BICLOPS AND BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION)) # for header vpConfig.h
+
+if(USE_CPP11)
+ set(VISP_HAVE_CPP11_COMPATIBILITY_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_CPP11_COMPATIBILITY_FOUND "no") # for ViSP-third-party.txt
+endif()
+
+# libraries for Pioneer mobile robots
+if(USE_ARIA AND UNIX)
+ if(ARIA_FOUND AND USE_PTHREAD AND RT_FOUND AND DL_FOUND)
+ set(VISP_HAVE_PIONEER TRUE) # for header vpConfig.h
+ set(VISP_HAVE_PIONEER_FOUND "yes") # for ViSP-third-party.txt
+ endif()
+elseif(USE_ARIA AND NOT UNIX)
+ set(VISP_HAVE_PIONEER TRUE) # for header vpConfig.h
+ set(VISP_HAVE_PIONEER_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_PIONEER_FOUND "no") # for ViSP-third-party.txt
+endif()
+
+if(USE_AFMA4)
+ set(VISP_HAVE_AFMA4_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_AFMA4_FOUND "no") # for ViSP-third-party.txt
+endif()
+if(USE_AFMA6)
+ set(VISP_HAVE_AFMA6_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_AFMA6_FOUND "no") # for ViSP-third-party.txt
+endif()
+if(USE_VIPER650)
+ set(VISP_HAVE_VIPER650_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_VIPER650_FOUND "no") # for ViSP-third-party.txt
+endif()
+if(USE_VIPER850)
+ set(VISP_HAVE_VIPER850_FOUND "yes") # for ViSP-third-party.txt
+else()
+ set(VISP_HAVE_VIPER850_FOUND "no") # for ViSP-third-party.txt
+endif()
+
+# ----------------------------------------------------------------------------
+# Finalization: generate configuration-based files
+# ----------------------------------------------------------------------------
+
+# Generate platform-dependent and configuration-dependent headers
+include(cmake/VISPGenerateHeaders.cmake)
+
+# Configure the file describing how to use ViSP. VISPConfig.cmake
+# is the main file configuring a CMake package.
+# . Exports build settings and dependencies for projects using ViSP as a
+# third party project.
+# . Create and install files for simple use of find_package(VISP)
+# by other cmakified "user" projects and libraries depending on ViSP.
+# (see "Mastering CMake", pp.72)
+# . To use ViSP in a third party project based on CMake:
+# find_package(VISP REQUIRED)
+# include_directories(${VISP_INCLUDE_DIRS})
+# target_link_libraries(<target> ${VISP_LIBRARIES})
+include(cmake/VISPGenerateConfig.cmake)
+
+#----------------------------------------------------------------------
+# For Dart server and tests
+# We use CDash set through CTestConfig.cmake file
+# Dashboards are sent to http://cdash.irisa.fr/CDash/index.php?project=ViSP
+#----------------------------------------------------------------------
+if(BUILD_TESTS)
+ include(CTest)
+endif()
+
+#----------------------------------------------------------------------
+# For CPack packaging tool
+#----------------------------------------------------------------------
+
+option(BUILD_PACKAGE "Configure ViSP packaging" OFF)
+if(BUILD_PACKAGE)
+ if(UNIX AND NOT APPLE AND NOT WIN32) # =linux
+ option(BUILD_PACKAGE_DEBIAN "Build debian package" ON)
+ option(BUILD_PACKAGE_RPM "Build rpm package" ON)
+ endif()
+
+ include(cmake/CPackConfig.cmake)
+endif(BUILD_PACKAGE)
+
+
+#----------------------------------------------------------------------
+# Generate the package dependent visp-config shell script for projects which
+# are not using CMake:
+# Usage:
+# visp-config --cflags ...
+#----------------------------------------------------------------------
+include(cmake/VISPGeneratePkgConfigScript.cmake)
+
+#----------------------------------------------------------------------
+# Propagation in sub dirs to build demo, example, test, tutorial
+#----------------------------------------------------------------------
+
+set(VISP_DIR ${PROJECT_BINARY_DIR})
+mark_as_advanced(VISP_DIR)
+mark_as_advanced(VISP_INCLUDE_DIRS)
+
+if(BUILD_DEMOS)
+ add_subdirectory(demo)
+endif()
+if(BUILD_EXAMPLES)
+ add_subdirectory(example)
+endif()
+if(BUILD_TUTORIALS)
+ add_subdirectory(tutorial)
+endif()
diff --git a/CMakeModules/AddExtraCompilationFlags.cmake b/CMakeModules/AddExtraCompilationFlags.cmake
deleted file mode 100644
index c75a694..0000000
--- a/CMakeModules/AddExtraCompilationFlags.cmake
+++ /dev/null
@@ -1,166 +0,0 @@
-#############################################################################
-#
-# $Id: AddExtraCompilationFlags.cmake 4608 2014-01-21 16:37:58Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file. Add extra compilation flags.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-MACRO(ADD_EXTRA_COMPILATION_FLAGS)
- include(CheckCXXCompilerFlag)
- if(CMAKE_COMPILER_IS_GNUCXX OR MINGW) #Not only UNIX but also WIN32 for MinGW
-
- set(WARNING_ALL "-Wall")
- CHECK_CXX_COMPILER_FLAG(${WARNING_ALL} WARNING_ALL_ALLOWED)
- if(WARNING_ALL_ALLOWED)
-# MESSAGE("Compiler flag ${WARNING_ALL} allowed")
- set(ACTIVATE_WARNING_ALL "ON" CACHE BOOL "activate -Wall flag")
- else()
-# MESSAGE("Compiler flag ${WARNING_ALL} not allowed")
- endif()
-
- set(WARNING_EXTRA "-Wextra")
- CHECK_CXX_COMPILER_FLAG(${WARNING_EXTRA} WARNING_EXTRA_ALLOWED)
- if(WARNING_EXTRA_ALLOWED)
-# MESSAGE("Compiler flag ${WARNING_EXTRA} allowed")
- set(ACTIVATE_WARNING_EXTRA "ON" CACHE BOOL "activate -Wextra flag")
- else()
-# MESSAGE("Compiler flag ${WARNING_EXTRA} not allowed")
- endif()
-
- set(WARNING_STRICT_OVERFLOW "-Wstrict-overflow=5")
- CHECK_CXX_COMPILER_FLAG(${WARNING_STRICT_OVERFLOW} WARNING_STRICT_OVERFLOW_ALLOWED)
- if(WARNING_STRICT_OVERFLOW_ALLOWED)
-# MESSAGE("Compiler flag ${WARNING_STRICT_OVERFLOW} allowed")
- set(ACTIVATE_WARNING_STRICT_OVERFLOW "OFF" CACHE BOOL "activate -Wstrict-overflow=5 flag")
- else()
-# MESSAGE("Compiler flag ${WARNING_STRICT_OVERFLOW} not allowed")
- endif()
-
- set(WARNING_FLOAT_EQUAL "-Wfloat-equal")
- CHECK_CXX_COMPILER_FLAG(${WARNING_FLOAT_EQUAL} WARNING_FLOAT_EQUAL_ALLOWED)
- if(WARNING_FLOAT_EQUAL_ALLOWED)
-# MESSAGE("Compiler flag ${WARNING_FLOAT_EQUAL} allowed")
- set(ACTIVATE_WARNING_FLOAT_EQUAL "OFF" CACHE BOOL "activate -Wfloat-equal flag")
- else()
-# MESSAGE("Compiler flag ${WARNING_FLOAT_EQUAL} not allowed")
- endif()
-
- set(WARNING_SIGN_CONVERSION "-Wsign-conversion")
- CHECK_CXX_COMPILER_FLAG(${WARNING_SIGN_CONVERSION} WARNING_SIGN_CONVERSION_ALLOWED)
- if(WARNING_SIGN_CONVERSION_ALLOWED)
-# MESSAGE("Compiler flag ${WARNING_SIGN_CONVERSION} allowed")
- set(ACTIVATE_WARNING_SIGN_CONVERSION "OFF" CACHE BOOL "activate -Wsign-conversion flag")
- else()
-# MESSAGE("Compiler flag ${WARNING_SIGN_CONVERSION} not allowed")
- endif()
-
- if(ACTIVATE_WARNING_ALL)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_ALL})
- else()
- string(REPLACE ${WARNING_ALL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
- if(ACTIVATE_WARNING_EXTRA)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_EXTRA})
- else()
- string(REPLACE ${WARNING_EXTRA} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
- if(ACTIVATE_WARNING_STRICT_OVERFLOW)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_STRICT_OVERFLOW})
- list(APPEND CMAKE_C_FLAGS ${WARNING_STRICT_OVERFLOW})
- else()
- string(REPLACE ${WARNING_STRICT_OVERFLOW} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- string(REPLACE ${WARNING_STRICT_OVERFLOW} "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
- endif()
- if(ACTIVATE_WARNING_FLOAT_EQUAL)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_FLOAT_EQUAL})
- else()
- string(REPLACE ${WARNING_FLOAT_EQUAL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
- if(ACTIVATE_WARNING_SIGN_CONVERSION)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_SIGN_CONVERSION})
- else()
- string(REPLACE ${WARNING_SIGN_CONVERSION} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
-
- elseif(MSVC)
- # Add specific compilation flags for Windows Visual
-
- set(WARNING_ALL "/Wall")
- CHECK_CXX_COMPILER_FLAG(${WARNING_ALL} WARNING_ALL_ALLOWED)
- if(WARNING_ALL_ALLOWED)
- #MESSAGE("Compiler flag ${WARNING_ALL} allowed")
- set(ACTIVATE_WARNING_ALL "OFF" CACHE BOOL "activate /Wall flag")
- else()
- #MESSAGE("Compiler flag ${WARNING_ALL} not allowed")
- endif()
-
- if(ACTIVATE_WARNING_ALL)
- list(APPEND CMAKE_CXX_FLAGS ${WARNING_ALL})
- else()
- string(REPLACE ${WARNING_ALL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- endif()
- if(MSVC80 OR MSVC90 OR MSVC10 OR MSVC11)
- # To avoid compiler warning (level 4) C4571, compile with /EHa if you still want
- # your catch(...) blocks to catch structured exceptions.
- list(APPEND CMAKE_CXX_FLAGS "/EHa")
- endif()
- endif()
-
- # If compiler support symbol visibility, enable it.
- include(CheckCCompilerFlag)
- check_c_compiler_flag(-fvisibility=hidden HAS_VISIBILITY)
- if (HAS_VISIBILITY)
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fvisibility=hidden")
- endif()
-
- if(UNIX)
- if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
- endif()
- endif()
-
- # Remove duplicates compilation flags
- separate_arguments(CMAKE_CXX_FLAGS)
- list(REMOVE_DUPLICATES CMAKE_CXX_FLAGS)
- string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}" CACHE STRING "common C++ build flags" FORCE)
- separate_arguments(CMAKE_C_FLAGS)
- list(REMOVE_DUPLICATES CMAKE_C_FLAGS)
- string(REPLACE ";" " " CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
- set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}" CACHE STRING "common C build flags" FORCE)
-
- #message("CMAKE_CXX_FLAGS : ${CMAKE_CXX_FLAGS}")
-ENDMACRO(ADD_EXTRA_COMPILATION_FLAGS)
diff --git a/CMakeModules/CPackConfig.cmake b/CMakeModules/CPackConfig.cmake
deleted file mode 100644
index cb4e00d..0000000
--- a/CMakeModules/CPackConfig.cmake
+++ /dev/null
@@ -1,113 +0,0 @@
-#############################################################################
-#
-# $Id: CPackConfig.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP packaging configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-message(STATUS "Configuring CPack")
-#mark_as_advanced(BUILD_PACKAGE)
-
-
-SET(CPACK_PACKAGE_NAME "libvisp")
-SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Visual tracking and visual servoing library in C++ (development files)")
-SET(CPACK_PACKAGE_CONTACT "Fabien Spindler <Fabien.Spindler at inria.fr>")
-SET(CPACK_PACKAGE_VENDOR "Inria, French National Institute for Research in Computer Science and Control")
-SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.txt")
-SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.txt")
-
-SET(CPACK_PACKAGE_VERSION ${VISP_VERSION})
-SET(CPACK_PACKAGE_VERSION_MAJOR ${VISP_VERSION_MAJOR})
-SET(CPACK_PACKAGE_VERSION_MINOR ${VISP_VERSION_MINOR})
-SET(CPACK_PACKAGE_VERSION_PATCH ${VISP_VERSION_PATCH})
-
-SET(CPACK_COMPONENTS_ALL libraries headers)
-SET(CPACK_COMPONENT_LIBRARIES_DISPLAY_NAME "Libraries")
-SET(CPACK_COMPONENT_HEADERS_DISPLAY_NAME "C++ Headers")
-SET(CPACK_COMPONENT_LIBRARIES_DESCRIPTION "ViSP ${VISP_VERSION} libraries")
-SET(CPACK_COMPONENT_HEADERS_DESCRIPTION "C/C++ header files for use with ViSP ${VISP_VERSION} libraries")
-SET(CPACK_COMPONENT_HEADERS_DEPENDS libraries)
-SET(CPACK_COMPONENT_LIBRARIES_GROUP "Development")
-SET(CPACK_COMPONENT_HEADERS_GROUP "Development")
-SET(CPACK_COMPONENT_GROUP_DEVELOPMENT_DESCRIPTION "All of the tools you'll ever need to develop software with ViSP")
-SET(CPACK_ALL_INSTALL_TYPES Full Developer)
-SET(CPACK_COMPONENT_LIBRARIES_INSTALL_TYPES Developer Full)
-SET(CPACK_COMPONENT_HEADERS_INSTALL_TYPES Developer Full)
-
-# The following components are regex's to match anywhere (unless anchored)
-# in absolute path + filename to find files or directories to be excluded
-# from source tarball.
-SET(CPACK_SOURCE_IGNORE_FILES
- "/\\\\.svn/"
- "/autom4te.cache/"
- "/build/"
- "/lib/"
- "~$"
-)
-
-SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME} ${VISP_VERSION}")
-
-# In ViSP packages we only want to have the libraries and the headers (nor the demo, example, and test)
-# That's why we turn off the demo, example and test building
-SET(BUILD_DEMOS OFF CACHE BOOL "Build ViSP demos." FORCE )
-SET(BUILD_EXAMPLES OFF CACHE BOOL "Build ViSP examples." FORCE)
-SET(BUILD_TESTING OFF CACHE BOOL "Build ViSP tests." FORCE)
-SET(BUILD_TUTORIAL OFF CACHE BOOL "Build ViSP tutorials." FORCE)
-
-
-if(UNIX)
- set(CMAKE_INSTALL_PREFIX "/usr" CACHE String "Package install prefix" FORCE)
-elseif(WIN32)
- set(CMAKE_INSTALL_PREFIX "C:/Program Files/ViSP ${VISP_VERSION}" CACHE String "Package install prefix" FORCE)
-endif()
-
-set(BUILD_SHARED_LIBS ON CACHE BOOL "Build ViSP with shared libraries." FORCE)
-
-
-if(WIN32 AND NOT UNIX)
- include(${VISP_SOURCE_DIR}/CMakeModules/CPackConfigNsis.cmake)
-elseif(APPLE)
- set(CPACK_GENERATOR "PackageMaker;TBZ2")
-elseif(UNIX)
- if(BUILD_PACKAGE_DEBIAN)
- include(${VISP_SOURCE_DIR}/CMakeModules/CPackConfigDeb.cmake)
- elseif(BUILD_PACKAGE_RPM)
- include(${VISP_SOURCE_DIR}/CMakeModules/CPackConfigRpm.cmake)
- endif()
-endif()
-
-INCLUDE(CPack)
diff --git a/CMakeModules/CPackConfigDeb.cmake b/CMakeModules/CPackConfigDeb.cmake
deleted file mode 100644
index b7aaa40..0000000
--- a/CMakeModules/CPackConfigDeb.cmake
+++ /dev/null
@@ -1,180 +0,0 @@
-#############################################################################
-#
-# $Id: CPackConfigDeb.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP packaging configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-# To install the generated debian package use the gdebi-gtk package
-# installer on Ubuntu:
-# gdebi-gtk libvisp-dev-2.6.1-1_i386.deb
-
-# $ dpkg --print-architecture
-FIND_PROGRAM(DPKG_CMD dpkg)
-MARK_AS_ADVANCED(DPKG_CMD)
-IF(NOT DPKG_CMD)
- MESSAGE(STATUS "Can not find dpkg in your path, default to i386.")
- SET(CPACK_SYSTEM_NAME i386)
-ELSE()
- EXECUTE_PROCESS(COMMAND "${DPKG_CMD}" --print-architecture
- OUTPUT_VARIABLE CPACK_SYSTEM_NAME
- OUTPUT_STRIP_TRAILING_WHITESPACE
- )
-ENDIF()
-
-# $ lsb_release -sr (ex: 12.04)
-FIND_PROGRAM(LSB_RELEASE_CMD lsb_release)
-MARK_AS_ADVANCED(LSB_RELEASE_CMD)
-IF(NOT LSB_RELEASE_CMD)
- MESSAGE(STATUS "Can not find lsb_release in your path.")
- SET(DEBIAN_DISTRO_RELEASE "")
-ELSE()
- EXECUTE_PROCESS(COMMAND "${LSB_RELEASE_CMD}" -sr
- OUTPUT_VARIABLE DEBIAN_DISTRO_RELEASE
- OUTPUT_STRIP_TRAILING_WHITESPACE
- )
- if(DEBIAN_DISTRO_RELEASE)
- #message("DEBIAN_DISTRO_RELEASE 1: ${DEBIAN_DISTRO_RELEASE}")
- STRING(REGEX REPLACE "[.]" "" DEBIAN_DISTRO_RELEASE ${DEBIAN_DISTRO_RELEASE})
- #message("DEBIAN_DISTRO_RELEASE 2: ${DEBIAN_DISTRO_RELEASE}")
- endif()
-ENDIF()
-
-
-list(APPEND CPACK_GENERATOR DEB)
-
-SET(CPACK_PACKAGE_VERSION ${VISP_VERSION}-${VISP_REVISION})
-SET(CPACK_PACKAGE_NAME "libvisp-dev")
-set(CPACK_PACKAGE_FILE_NAME "${CPACK_PACKAGE_NAME}_${VISP_VERSION}-${VISP_REVISION}_${CPACK_SYSTEM_NAME}")
-
-set(CPACK_DEBIAN_PACKAGE_DEPENDS "cmake (>=2.6)")
-IF(USE_X11)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libx11-dev (>=2:1.3.2)")
-ENDIF()
-IF(USE_GTK2)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libgtk2.0-dev (>=2.20.1), libglib2.0-dev (>=2.24.1), libpango1.0-dev (>=1.28.0), libatk1.0-dev (>=1.30.0), libcairo2-dev (>=1.8.10)")
-ENDIF()
-IF(USE_LAPACK)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, liblapack-dev(>=3.2)")
-ENDIF()
-IF(USE_GSL)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libgsl0-dev (>=1.13)")
-ENDIF()
-IF(USE_V4L2)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libv4l-dev (>=0.6.4)")
-ENDIF()
-IF(USE_DC1394_2)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libdc1394-22-dev (>=2.1.2)")
-ENDIF()
-IF(USE_XML2)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libxml2-dev (>=2.7.2)")
-ENDIF()
-IF(USE_LIBPNG)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libpng12-dev (>=1.2.42)")
-ENDIF()
-IF(USE_LIBJPEG)
- if(DEBIAN_DISTRO_RELEASE)
- if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg62-dev (>=6b-15)")
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg-dev (>=6b-15)")
- endif()
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg-dev (>=6b-15)")
- endif()
-ENDIF()
-IF(USE_FFMPEG)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libswscale-dev (>=4:0.5.1), libavutil-dev (>=4:0.5.1), libavformat-dev (>=4:0.5.1), libavcodec-dev (>=4:0.5.1), libbz2-dev (>=1.0.5-4), libbz2-1.0 (>=1.0.5-4)")
-ENDIF()
-IF(USE_COIN)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libcoin60-dev (>=3.1.2-1)")
-ENDIF()
-IF(USE_SOQT)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libsoqt4-dev (>=1.4.2~svn20090224-2)")
-ENDIF()
-IF(USE_OGRE)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libogre-dev (>=1.7.3)")
- # since ogre-samples-media is not available on ubuntu oneiric 11.10 for example, we need an extra test to add this dependency
- IF(OGRE_MEDIA_DIR)
- if(DEBIAN_DISTRO_RELEASE)
- if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples-media (>=1.7.3)")
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples (>=1.7.3)")
- endif()
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples-media (>=1.7.3)")
- endif()
- ENDIF()
-ENDIF()
-IF(USE_OIS)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libois-dev (>=1.3.0)")
-ENDIF()
-IF(USE_OPENCV)
- if(DEBIAN_DISTRO_RELEASE)
- if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libcv-dev (>=2.0), libcvaux-dev (>=2.0), libhighgui-dev (>=2.0)")
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libopencv-dev (>=2.3.1)")
- endif()
- else()
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libopencv-dev (>=2.3.1)")
- endif()
-ENDIF()
-IF(USE_LIBFREENECT)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, freenect (>=1:0.0.1)")
-ENDIF()
-if(USE_LIBUSB_1)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libusb-1.0-0-dev (>=2:1.0)")
-endif()
-
-set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libc6 (>=2.4)")
-
-SET(CPACK_DEBIAN_PACKAGE_DESCRIPTION "Visual Servoing Platform development files
- ViSP, standing for Visual Servoing Platform, is unique. This software
- is a complete cross-platform solution that allows prototyping and
- developing applications in visual tracking and visual servoing.
- .
- ViSP can be useful in robotics, computer vision, augmented reality and
- computer animation.
- .
- This package contains development files (headers and shared library
- symbolic link).")
-
-SET(CPACK_DEBIAN_PACKAGE_SECTION "libdevel")
-SET(CPACK_DEBIAN_PACKAGE_HOMEPAGE "http://www.irisa.fr/lagadic/visp")
-
diff --git a/CMakeModules/CPackConfigNsis.cmake b/CMakeModules/CPackConfigNsis.cmake
deleted file mode 100644
index 9ccb97c..0000000
--- a/CMakeModules/CPackConfigNsis.cmake
+++ /dev/null
@@ -1,58 +0,0 @@
-#############################################################################
-#
-# $Id: CPackConfigNsis.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP packaging configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-set(BUILD_SHARED_LIBS CACHE FORCE "Build ViSP with shared libraries." ON)
-
-# InstallRequiredSystemLibraries: Hack for Visual Studio support Search for
-# system runtime libraries based on the platform.
-INCLUDE(InstallRequiredSystemLibraries)
-
-set(CPACK_GENERATOR NSIS)
-set(CPACK_NSIS_CONTACT "visp at inria.fr")
-SET(CPACK_NSIS_HELP_LINK "http://www.irisa.fr/lagadic/visp/visp.html")
-set(CPACK_NSIS_URL_INFO_ABOUT "http://www.irisa.fr/lagadic/visp/visp.html")
-set(CPACK_NSIS_MODIFY_PATH "ON")
-set(CPACK_NSIS_DISPLAY_NAME "${CPACK_PACKAGE_INSTALL_DIRECTORY} SDK")
-# set(CPACK_PACKAGE_ICON ${VISP_SOURCE_DIR}/visp.bmp)
-# set(CPACK_NSIS_MUI_ICON ${VISP_SOURCE_DIR}/logoViSP.ico)
-# set(CPACK_NSIS_MUI_UNIICON ${VISP_SOURCE_DIR}/logoViSP.ico)
-
-SET(CPACK_PACKAGE_FILE_NAME "ViSP-${VISP_VERSION}-win32")
-SET(CPACK_SOURCE_PACKAGE_FILE_NAME "ViSP-${VISP_VERSION}-win32-src")
diff --git a/CMakeModules/CPackConfigRpm.cmake b/CMakeModules/CPackConfigRpm.cmake
deleted file mode 100644
index ec624e3..0000000
--- a/CMakeModules/CPackConfigRpm.cmake
+++ /dev/null
@@ -1,149 +0,0 @@
-#############################################################################
-#
-# $Id: CPackConfigRpm.cmake 4972 2014-11-16 13:03:24Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP packaging configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-# $ uname -i
-
-FIND_PROGRAM(UNAME_CMD uname)
-MARK_AS_ADVANCED(UNAME_CMD)
-IF(NOT UNAME_CMD)
- MESSAGE(STATUS "Can not find uname in your path, default to i386.")
- SET(CPACK_SYSTEM_NAME i386)
-ELSE()
- EXECUTE_PROCESS(COMMAND "${UNAME_CMD}" -m
- OUTPUT_VARIABLE CPACK_SYSTEM_NAME
- OUTPUT_STRIP_TRAILING_WHITESPACE
- )
-ENDIF()
-
-# create a fedora short name from cat /etc/fedora-release
-# for example, cat .. return "Fedora release 13 (Goddard)" we build "fc13"
-FIND_PROGRAM(CAT_CMD cat)
-MARK_AS_ADVANCED(CAT_CMD)
-IF(NOT CAT_CMD)
- MESSAGE(STATUS "Can not find cat in your path, default to empty.")
- SET(DIST_SHORT_NAME "")
-ELSE()
- SET(DIST_SHORT_NAME "")
- EXECUTE_PROCESS(COMMAND "${CAT_CMD}" /etc/fedora-release
- OUTPUT_VARIABLE FEDORA_FULL_RELEASE_NAME
- OUTPUT_STRIP_TRAILING_WHITESPACE
- )
- #message("fedora release found 1: ${FEDORA_FULL_RELEASE_NAME}")
- IF(FEDORA_FULL_RELEASE_NAME)
- # replace " " space separator by ";" separator
- STRING(REGEX REPLACE "[ ]" ";" FEDORA_FULL_RELEASE_NAME ${FEDORA_FULL_RELEASE_NAME})
- # message("fedora release found 2: ${FEDORA_FULL_RELEASE_NAME}")
- LIST(GET FEDORA_FULL_RELEASE_NAME 0 ITEM1) # here ITEM1 should contain "Fedora"
- # message("item1: ${ITEM1}")
- STRING(COMPARE EQUAL ${ITEM1} "Fedora" IS_FEDORA)
- IF(IS_FEDORA)
- LIST(GET FEDORA_FULL_RELEASE_NAME 2 ITEM3) # here ITEM3 should contain the dist number
- # message("dist number: ${ITEM3}")
- IF(ITEM3)
- SET(DIST_SHORT_NAME "fc${ITEM3}")
- ENDIF()
- ENDIF()
- ENDIF()
-ENDIF()
-
-
-list(APPEND CPACK_GENERATOR RPM)
-
-SET(CPACK_PACKAGE_NAME "libvisp-devel")
-set(CPACK_PACKAGE_FILE_NAME "${CPACK_PACKAGE_NAME}-${VISP_VERSION}-${VISP_REVISION}.${DIST_SHORT_NAME}.${CPACK_SYSTEM_NAME}")
-set(CPACK_RPM_PACKAGE_GROUP "Development/Libraries")
-set(CPACK_RPM_PACKAGE_ARCHITECTURE "${CPACK_SYSTEM_NAME}")
-set(CPACK_RPM_PACKAGE_LICENSE "GPL")
-set(CPACK_RPM_PACKAGE_RELEASE ${VISP_REVISION})
-
-set(CPACK_RPM_PACKAGE_DEPENDS "cmake >= 2.6")
-IF(USE_X11)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libX11-devel >= 1.3")
-ENDIF()
-IF(USE_GTK2)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, gtk2-devel, glib2-devel, pango-devel, atk-devel, cairo-devel")
-ENDIF()
-IF(USE_LAPACK)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, lapack-devel >= 3.2")
-ENDIF()
-IF(USE_GSL)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, gsl-devel >= 1.13")
-ENDIF()
-IF(USE_V4L2)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libv4l-devel >= 0.6.2")
-ENDIF()
-IF(USE_DC1394_2)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libdc1394-devel >= 2.1.2")
-ENDIF()
-IF(USE_XML2)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libxml2-devel >= 2.7.6")
-ENDIF()
-IF(USE_LIBPNG)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libpng-devel >= 2:1.2.44")
-ENDIF()
-IF(USE_LIBJPEG)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libjpeg-devel >= 6b-46")
-ENDIF()
-IF(USE_COIN)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, Coin2-devel >= 2.5.0")
-ENDIF()
-IF(USE_SOQT)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, SoQt-devel >= 1.5.0")
-ENDIF()
-IF(USE_OPENCV)
- set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, opencv-devel >= 2.0")
-ENDIF()
-#message("CPACK_RPM_PACKAGE_DEPENDS ${CPACK_RPM_PACKAGE_DEPENDS}")
-
-set(CPACK_RPM_PACKAGE_REQUIRES "${CPACK_RPM_PACKAGE_DEPENDS}")
-
-set(CPACK_RPM_PACKAGE_DESCRIPTION "Visual Servoing Platform development files
- ViSP, standing for Visual Servoing Platform, is unique. This software
- is a complete cross-platform solution that allows prototyping and
- developing applications in visual tracking and visual servoing.
- .
- ViSP can be useful in robotics, computer vision, augmented reality and
- computer animation.
- .
- This package contains development files (headers and shared library
- symbolic link).")
-
-set(CPACK_RPM_EXCLUDE_FROM_AUTO_FILELIST_ADDITION "/usr/share/man" "/usr/share/man/man1" "/usr/lib64/pkgconfig" "/usr/lib/pkgconfig" "/usr/lib64/cmake" "/usr/lib/cmake")
diff --git a/CMakeModules/FindARIA.cmake b/CMakeModules/FindARIA.cmake
deleted file mode 100644
index e3c32aa..0000000
--- a/CMakeModules/FindARIA.cmake
+++ /dev/null
@@ -1,134 +0,0 @@
-#############################################################################
-#
-# $Id: FindARIA.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Aria libraries and headers to control Pioneer mobile robots.
-# Once run this will define:
-#
-# ARIA_FOUND
-# ARIA_INCLUDE_DIRS
-# ARIA_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-FIND_PATH(ARIA_INCLUDE_DIR Aria.h
- $ENV{ARIA_HOME}/include
- /usr/local/Aria/include
- /usr/Aria/include
- /usr/include
- /usr/local/include
- "C:/Program Files/MobileRobots/Aria/include"
- )
-#MESSAGE("DBG ARIA_INCLUDE_DIR=${ARIA_INCLUDE_DIR}")
-
-if(UNIX)
- FIND_LIBRARY(ARIA_LIBRARY
- NAMES Aria
- PATHS
- $ENV{ARIA_HOME}/lib
- /usr/local/Aria/lib
- /usr/Aria/lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/MobileRobots/Aria/lib"
- )
- #MESSAGE("DBG ARIA_LIBRARY=${ARIA_LIBRARY}")
-else()
- if(MSVC11)
- set(ARIA_SUFFIX_NAME VC11)
- elseif(MSVC10)
- set(ARIA_SUFFIX_NAME VC10)
- elseif(MSVC90)
- set(ARIA_SUFFIX_NAME VC9)
- elseif(MSVC80)
- set(ARIA_SUFFIX_NAME VC8)
- elseif(MSVC71)
- set(ARIA_SUFFIX_NAME VC71)
- else()
- set(ARIA_SUFFIX_NAME "")
- endif()
-
- FIND_LIBRARY(ARIA_LIBRARY_DEBUG
- NAMES AriaDebug${ARIA_SUFFIX_NAME}
- PATHS
- $ENV{ARIA_HOME}/lib
- /usr/local/Aria/lib
- /usr/Aria/lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/MobileRobots/Aria/lib"
- )
- FIND_LIBRARY(ARIA_LIBRARY_RELEASE
- NAMES Aria${ARIA_SUFFIX_NAME}
- PATHS
- $ENV{ARIA_HOME}/lib
- /usr/local/Aria/lib
- /usr/Aria/lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/MobileRobots/Aria/lib"
- )
-endif()
-
-if(UNIX)
- IF(ARIA_LIBRARY AND ARIA_INCLUDE_DIR)
- SET(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
- SET(ARIA_LIBRARIES ${ARIA_LIBRARY})
- SET(ARIA_FOUND TRUE)
- ELSE()
- SET(ARIA_FOUND FALSE)
- ENDIF()
-else()
- set(ARIA_LIBRARIES "")
- IF(ARIA_LIBRARY_RELEASE AND ARIA_INCLUDE_DIR)
- SET(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
- LIST(APPEND ARIA_LIBRARIES optimized ${ARIA_LIBRARY_RELEASE})
- SET(ARIA_FOUND TRUE)
- ENDIF()
- IF(ARIA_LIBRARY_DEBUG AND ARIA_INCLUDE_DIR)
- SET(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
- LIST(APPEND ARIA_LIBRARIES debug ${ARIA_LIBRARY_DEBUG})
- SET(ARIA_FOUND TRUE)
- ENDIF()
-
-endif()
-
-MARK_AS_ADVANCED(
- ARIA_INCLUDE_DIR
- ARIA_LIBRARY
- ARIA_LIBRARY_DEBUG
- ARIA_LIBRARY_RELEASE
- )
-
diff --git a/CMakeModules/FindBICLOPS.cmake b/CMakeModules/FindBICLOPS.cmake
deleted file mode 100644
index c5fa981..0000000
--- a/CMakeModules/FindBICLOPS.cmake
+++ /dev/null
@@ -1,151 +0,0 @@
-#############################################################################
-#
-# $Id: FindBICLOPS.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libBiclops, libPMD and libUtils for Biclops head.
-# Once run this will define:
-#
-# BICLOPS_FOUND
-# BICLOPS_INCLUDE_DIRS
-# BICLOPS_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-IF(NOT UNIX AND NOT WIN32)
- # MESSAGE("FindBICLOPS.cmake: macro only for Unix and Windows for the moment.")
- SET(BICLOPS_FOUND FALSE)
-ELSE(NOT UNIX AND NOT WIN32)
-
- FIND_PATH(BICLOPS_INCLUDE_DIR Biclops.h
- $ENV{BICLOPS_HOME}/include
- /usr/include )
-
- #MESSAGE("DBG BICLOPS_INCLUDE_DIR=${BICLOPS_INCLUDE_DIR}")
-
- FIND_PATH(PMD_INCLUDE_DIR PMD.h
- $ENV{BICLOPS_HOME}/include
- $ENV{BICLOPS_PMD_HOME}/include
- /usr/include )
-
- FIND_PATH(UTILS_INCLUDE_DIR utility.h
- $ENV{BICLOPS_HOME}/include
- $ENV{BICLOPS_UTILS_HOME}/include
- /usr/include )
-
- FIND_LIBRARY(BICLOPS_LIBRARY
- NAMES Biclops libBiclops libBiclopsD
- PATHS
- $ENV{BICLOPS_HOME}/lib
- /usr/lib
- )
-
- FIND_LIBRARY(PMD_LIBRARY
- NAMES PMD libPMD libPMDD
- PATHS
- $ENV{BICLOPS_HOME}/lib
- $ENV{BICLOPS_PMD_HOME}/lib
- /usr/lib
- )
-
- FIND_LIBRARY(UTILS_LIBRARY
- NAMES Utils libUtils libUtilsD
- PATHS
- $ENV{BICLOPS_HOME}/lib
- $ENV{BICLOPS_UTILS_HOME}/lib
- /usr/lib
- )
- #MESSAGE("DBG BICLOPS_LIBRARY=${BICLOPS_LIBRARY}")
-
- ## --------------------------------
-
- IF(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
- SET(BICLOPS_LIBRARIES ${BICLOPS_LIBRARY} ${PMD_LIBRARY} ${UTILS_LIBRARY})
- ELSE(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
-# MESSAGE(SEND_ERROR "Biclops library not found. Set USE_BICLOPS option OFF")
- ENDIF(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
-
- IF(NOT BICLOPS_INCLUDE_DIR)
-# MESSAGE(SEND_ERROR "Biclops include dir not found. Set USE_BICLOPS option OFF")
- ENDIF(NOT BICLOPS_INCLUDE_DIR)
-
- IF(NOT PMD_INCLUDE_DIR)
-# MESSAGE(SEND_ERROR "PMD include dir not found. Set USE_BICLOPS option OFF")
- ENDIF(NOT PMD_INCLUDE_DIR)
-
- IF(NOT UTILS_INCLUDE_DIR)
-# MESSAGE(SEND_ERROR "Utils include dir not found. Set USE_BICLOPS option OFF ")
- ENDIF(NOT UTILS_INCLUDE_DIR)
-
- IF(BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
- SET(BICLOPS_INCLUDE_DIRS ${BICLOPS_INCLUDE_DIR} ${PMD_INCLUDE_DIR} ${UTILS_INCLUDE_DIR})
- ENDIF(BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
-
- IF(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
- #message("BICLOPS_LIBRARIES: ${BICLOPS_LIBRARIES}")
- #message("BICLOPS_INCLUDE_DIRS: ${BICLOPS_INCLUDE_DIRS}")
-
- # Try to compile a sample code using Biclops library to see if GetHomedState() is available
- #SET(BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION FALSE)
- include(CheckCXXSourceCompiles)
-
- SET(CMAKE_REQUIRED_LIBRARIES ${BICLOPS_LIBRARIES})
- SET(CMAKE_REQUIRED_INCLUDES ${BICLOPS_INCLUDE_DIRS})
- CHECK_CXX_SOURCE_COMPILES("
- #include <PMDUtils.h>
- int main(){
- PMDAxisControl *axis;
- axis->GetHomedState();
- return 0;
- }
- " BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION)
-
- #MESSAGE("DBG1 BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION ${BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION}")
-
- SET(BICLOPS_FOUND TRUE)
- ELSE(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
- SET(BICLOPS_FOUND FALSE)
- ENDIF(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- BICLOPS_INCLUDE_DIR
- BICLOPS_LIBRARIES
- BICLOPS_LIBRARY
- PMD_INCLUDE_DIR
- UTILS_INCLUDE_DIR
- PMD_LIBRARY
- UTILS_LIBRARY
- )
-ENDIF(NOT UNIX AND NOT WIN32)
diff --git a/CMakeModules/FindCALINUX.cmake b/CMakeModules/FindCALINUX.cmake
deleted file mode 100644
index 61571bc..0000000
--- a/CMakeModules/FindCALINUX.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#############################################################################
-#
-# $Id: FindCALINUX.cmake 5309 2015-02-11 11:08:15Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Cerebellum CA linux library material
-#
-# CALINUX_FOUND
-# CALINUX_INCLUDE_DIRS
-# CALINUX_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindCALINUX.cmake: only available for Unix.")
- SET(CALINUX_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_PATH(CALINUX_INCLUDE_DIR calinux.h
- $ENV{CALINUX_HOME}/include
- /local/soft/Cerebellum/CALinux/current/include
- /home/soft/Cerebellum/CALinux/current/include
- )
- #MESSAGE("DBG CALINUX_INCLUDE_DIR=${CALINUX_INCLUDE_DIR}")
-
- FIND_LIBRARY(CALINUX_LIBRARY
- NAMES calinux
- PATHS
- $ENV{CALINUX_HOME}/lib
- /local/soft/Cerebellum/CALinux/current/lib
- /home/soft/Cerebellum/CALinux/current/lib
- )
-
- #MESSAGE("DBG CALINUX_LIBRARY=${CALINUX_LIBRARY}")
-
- ## --------------------------------
-
- IF(CALINUX_LIBRARY)
- SET(CALINUX_LIBRARIES ${CALINUX_LIBRARY})
- ELSE(CALINUX_LIBRARY)
-# MESSAGE(SEND_ERROR "Calinux library not found.")
- ENDIF(CALINUX_LIBRARY)
-
- IF(NOT CALINUX_INCLUDE_DIR)
-# MESSAGE(SEND_ERROR "Calinux include dir not found.")
- ENDIF(NOT CALINUX_INCLUDE_DIR)
-
- IF(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
- SET(CALINUX_INCLUDE_DIRS ${CALINUX_INCLUDE_DIR})
- SET(CALINUX_FOUND TRUE)
- ELSE(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
- SET(CALINUX_FOUND FALSE)
- ENDIF(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- CALINUX_INCLUDE_DIR
- CALINUX_LIBRARIES
- CALINUX_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindCMU1394.cmake b/CMakeModules/FindCMU1394.cmake
deleted file mode 100644
index a01b813..0000000
--- a/CMakeModules/FindCMU1394.cmake
+++ /dev/null
@@ -1,109 +0,0 @@
-#############################################################################
-#
-# $Id: FindCMU1394.cmake 4603 2014-01-21 13:40:58Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find CMU 1394 Digital Camera SDK to acquire images from firewire
-# cameras on Windows.
-# Once run this will define:
-#
-# CMU1394_FOUND
-# CMU1394_INCLUDE_DIRS
-# CMU1394_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-if(WIN32)
- FIND_PATH(CMU1394_INCLUDE_DIR 1394Camera.h
- $ENV{CMU1394_HOME}/include
- "C:/Program Files (x86)/CMU/1394Camera/include"
- "C:/Program Files/CMU/1394Camera/include"
- )
- #MESSAGE("DBG CMU1394_INCLUDE_DIR=${CMU1394_INCLUDE_DIR}")
-
- if(CMAKE_CL_64)
- FIND_LIBRARY(CMU1394_LIBRARY_DEBUG
- NAMES 1394camerad
- PATHS
- $ENV{CMU1394_HOME}/lib64/x64
- "C:/Program Files (x86)/CMU/1394Camera/lib64/x64"
- "C:/Program Files/CMU/1394Camera/lib64/x64"
- )
-
- FIND_LIBRARY(CMU1394_LIBRARY_RELEASE
- NAMES 1394camera
- PATHS
- $ENV{CMU1394_HOME}/lib64/x64
- "C:/Program Files (x86)/CMU/1394Camera/lib64/x64"
- "C:/Program Files/CMU/1394Camera/lib64/x64"
- )
- else()
- FIND_LIBRARY(CMU1394_LIBRARY_DEBUG
- NAMES 1394camerad
- PATHS
- $ENV{CMU1394_HOME}/lib
- "C:/Program Files (x86)/CMU/1394Camera/lib"
- "C:/Program Files/CMU/1394Camera/lib"
- )
-
- FIND_LIBRARY(CMU1394_LIBRARY_RELEASE
- NAMES 1394camera
- PATHS
- $ENV{CMU1394_HOME}/lib
- "C:/Program Files (x86)/CMU/1394Camera/lib"
- "C:/Program Files/CMU/1394Camera/lib"
- )
- endif()
- #MESSAGE("DBG CMU1394_LIBRARY_DEBUG=${CMU1394_LIBRARY_DEBUG}")
- #MESSAGE("DBG CMU1394_LIBRARY_RELEASE=${CMU1394_LIBRARY_RELEASE}")
-
- set(CMU1394_LIBRARIES "")
- if(CMU1394_LIBRARY_RELEASE AND CMU1394_INCLUDE_DIR)
- set(CMU1394_INCLUDE_DIRS ${CMU1394_INCLUDE_DIR})
- list(APPEND CMU1394_LIBRARIES optimized ${CMU1394_LIBRARY_RELEASE})
- set(CMU1394_FOUND TRUE)
- endif()
- if(CMU1394_LIBRARY_DEBUG AND CMU1394_INCLUDE_DIR)
- set(CMU1394_INCLUDE_DIRS ${CMU1394_INCLUDE_DIR})
- list(APPEND CMU1394_LIBRARIES debug ${CMU1394_LIBRARY_DEBUG})
- set(CMU1394_FOUND TRUE)
- ENDIF()
-
- MARK_AS_ADVANCED(
- CMU1394_INCLUDE_DIR
- CMU1394_LIBRARY
- CMU1394_LIBRARY_DEBUG
- CMU1394_LIBRARY_RELEASE
- )
-endif()
diff --git a/CMakeModules/FindCPP11.cmake b/CMakeModules/FindCPP11.cmake
deleted file mode 100644
index 6900179..0000000
--- a/CMakeModules/FindCPP11.cmake
+++ /dev/null
@@ -1,71 +0,0 @@
-#############################################################################
-#
-# $Id: FindCPP11.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find C++11 flags.
-#
-# CPP11_FOUND - true if C++11 support is detected
-# CPP11_CXX_FLAGS - flags to add to the CXX compiler for C++11 support
-#
-# Authors:
-# Fabien Spindler
-
-include(CheckCXXCompilerFlag)
-include(FindPackageHandleStandardArgs)
-
-set(CPP11_CXX_FLAG_CANDIDATES
- "-std=c++11"
- "-std=c++0x"
-)
-
-# check CXX c++11 compiler flag
-foreach(FLAG ${CPP11_CXX_FLAG_CANDIDATES})
- set(SAFE_CMAKE_REQUIRED_FLAGS "${CMAKE_REQUIRED_FLAGS}")
- set(CMAKE_REQUIRED_FLAGS "${FLAG}")
- unset(CPP11_FLAG_DETECTED CACHE)
- message(STATUS "Try C++11 flag = [${FLAG}]")
- CHECK_CXX_COMPILER_FLAG(${FLAG} CPP11_FLAG_DETECTED)
- set(CMAKE_REQUIRED_FLAGS "${SAFE_CMAKE_REQUIRED_FLAGS}")
- if(CPP11_FLAG_DETECTED)
- set(CPP11_CXX_FLAGS_INTERNAL "${FLAG}")
- break()
- endif()
-endforeach()
-
-set(CPP11_CXX_FLAGS "${CPP11_CXX_FLAGS_INTERNAL}"
- CACHE STRING "C++ compiler flags for C++11 support")
-# handle the standard arguments for find_package
-find_package_handle_standard_args(CPP11 DEFAULT_MSG CPP11_CXX_FLAGS )
-
-mark_as_advanced(
- CPP11_CXX_FLAGS
-)
diff --git a/CMakeModules/FindDC1394.cmake b/CMakeModules/FindDC1394.cmake
deleted file mode 100644
index 01b4db7..0000000
--- a/CMakeModules/FindDC1394.cmake
+++ /dev/null
@@ -1,141 +0,0 @@
-#############################################################################
-#
-# $Id: FindDC1394.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libDC1394 for IEEE1394 camera. First search for libdc1394-2.x
-# and if not found, search for libdc1394-1.x
-# Once run this will define:
-#
-# DC1394_FOUND
-# DC1394_INCLUDE_DIRS
-# DC1394_LIBRARIES
-#
-# The two defines below are only useful to compile with libdc1394-2.x. In
-# that case DC1394_VERSION=2. Since the libdc1394-2.x API is not stable, we
-# need to determine if dc1394_find_cameras() or dc1394_enumerate_cameras()
-# functions are available. dc1394_enumerate_cameras() was introduced after
-# libdc1394-2.0.0-rc7. DC1394_CAMERA_ENUMERATE_FOUND is TRUE when
-# dc1394_camera_enumerate() function is found. DC1394_FIND_CAMERAS_FOUND is
-# TRUE when dc1394_find_cameras() is found.
-# DC1394_CAMERA_ENUMERATE_FOUND
-# DC1394_FIND_CAMERAS_FOUND
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindDC1394.cmake: libdc1394 only available for Unix.")
- SET(DC1394_FOUND FALSE)
-ELSE(NOT UNIX)
- # Search for libdc1394-2.x
- FIND_PATH(DC1394_INCLUDE_DIR dc1394/control.h
- $ENV{DC1394_HOME}/include
- $ENV{DC1394_DIR}/include
- /usr/include )
- #MESSAGE("DBG DC1394_INCLUDE_DIR=${DC1394_INCLUDE_DIR}")
-
- FIND_LIBRARY(DC1394_LIBRARY
- NAMES dc1394
- PATHS
- $ENV{DC1394_HOME}/lib
- $ENV{DC1394_DIR}/lib
- /usr/lib
- )
- #MESSAGE("DBG DC1394_LIBRARY=${DC1394_LIBRARY}")
-
- IF(DC1394_LIBRARY AND DC1394_INCLUDE_DIR)
-
- # Since the libdc1394-2.x API is not stable, try to compile a
- # sample code to determine if we have to use dc1394_find_cameras() or
- # dc1394_enumerate_cameras() functions. dc1394_enumerate_cameras() was
- # introduced after libdc1394-2.0.0-rc7
-
- include(CheckCXXSourceCompiles)
-
- SET(CMAKE_REQUIRED_LIBRARIES ${DC1394_LIBRARY})
- SET(CMAKE_REQUIRED_INCLUDES ${DC1394_INCLUDE_DIR})
-
- CHECK_CXX_SOURCE_COMPILES("
- #include <dc1394/control.h>
- #include <dc1394/utils.h>
-
- int main(){
- dc1394_t * d;
- dc1394camera_list_t * list;
- d = dc1394_new ();
- dc1394_camera_enumerate (d, &list);
- return 0;
- }
- " DC1394_CAMERA_ENUMERATE_FOUND)
- #MESSAGE("DC1394_CAMERA_ENUMERATE_FOUND: ${DC1394_CAMERA_ENUMERATE_FOUND}")
-
- IF(NOT DC1394_CAMERA_ENUMERATE_FOUND)
- # Try to test the compatibility to libdc1394-2.0.0-rc7
- CHECK_CXX_SOURCE_COMPILES("
- #include <dc1394/control.h>
- #include <dc1394/utils.h>
-
- int main(){
- dc1394camera_t **cameras;
- unsigned int num_cameras;
- dc1394_find_cameras(&cameras, &num_cameras);
- return 0;
- }
- " DC1394_FIND_CAMERAS_FOUND)
- #MESSAGE("DC1394_FIND_CAMERAS_FOUND: ${DC1394_FIND_CAMERAS_FOUND}")
- ENDIF(NOT DC1394_CAMERA_ENUMERATE_FOUND)
-
- IF(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
- SET(DC1394_FOUND FALSE)
- MESSAGE("libdc1394-2.x found but not compatible with ViSP...")
- ELSE(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
- SET(DC1394_FOUND TRUE)
- ENDIF(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
-
- SET(DC1394_LIBRARIES ${DC1394_LIBRARY})
- SET(DC1394_INCLUDE_DIRS ${DC1394_INCLUDE_DIR})
- ENDIF(DC1394_LIBRARY AND DC1394_INCLUDE_DIR)
-
-
-
- ## --------------------------------
-
- MARK_AS_ADVANCED(
- DC1394_LIBRARY
- DC1394_INCLUDE_DIR
- DC1394_INCLUDE_DIR
- DC1394_LIBRARIES
- DC1394_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindDIRECT3D.cmake b/CMakeModules/FindDIRECT3D.cmake
deleted file mode 100755
index 18eb6a1..0000000
--- a/CMakeModules/FindDIRECT3D.cmake
+++ /dev/null
@@ -1,146 +0,0 @@
-#############################################################################
-#
-# $Id: FindDIRECT3D.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Direct3D on Windows. This code comes originaly
-# from http://www.cmake.org/Wiki/CMakeUserFindDirectShow .
-# Once loaded this will define:
-#
-# DIRECT3D_FOUND - system has DirectShow
-# DIRECT3D_INCLUDE_DIRS - include directory for DirectShow
-# DIRECT3D_LIBRARIES - libraries you need to link to
-#
-# Authors:
-# Bruno Renier
-#
-#############################################################################
-
-SET(DIRECT3D_FOUND "NO")
-
-# Direct3D is only available on Windows platforms
-IF(WIN32 AND NOT MINGW)
- # Find Direct3D Include Directory
- FIND_PATH(DIRECT3D_INCLUDE_DIR d3dx9.h
- "$ENV{DXSDK_DIR}/Include"
- "C:/Program Files/Microsoft DirectX SDK/Include"
- "C:/DXSDK/Include"
- NO_DEFAULT_PATH
- DOC "What is the path where the file d3dx9.h can be found"
- )
- FIND_PATH(DIRECT3D_INCLUDE_DIR d3dx9.h
- NO_DEFAULT_PATH
- DOC "What is the path where the file d3dx9.h can be found"
- )
-
- # if Direct3D include dir found, then find Direct3D libraries
- IF(DIRECT3D_INCLUDE_DIR)
- IF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
- "$ENV{DXSDK_DIR}/Lib"
- "$ENV{DXSDK_DIR}/Lib/x64"
- "C:/Program Files/Microsoft DirectX SDK/Lib/x64"
- "C:/DXSDK/Include/Lib/x64"
- NO_DEFAULT_PATH
- DOC "Where can the Direct3D d3d9 library be found"
- )
- ELSE(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
- "$ENV{DXSDK_DIR}/Lib"
- "$ENV{DXSDK_DIR}/Lib/x86"
- "C:/Program Files/Microsoft DirectX SDK/Lib/x86"
- "C:/DXSDK/Include/Lib/x86"
- NO_DEFAULT_PATH
- DOC "Where can the Direct3D d3d9 library be found"
- )
- ENDIF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
- DOC "Where can the Direct3D d3d9 library be found"
- )
-
- IF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
- "$ENV{DXSDK_DIR}/Lib/"
- "$ENV{DXSDK_DIR}/Lib/x64"
- "C:/Program Files/Microsoft DirectX SDK/Lib/x64"
- "C:/DXSDK/Include/Lib/x64"
- NO_DEFAULT_PATH
- DOC "Where can the Direct3D d3dx9 library be found"
- )
- ELSE(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
- "$ENV{DXSDK_DIR}/Lib/"
- "$ENV{DXSDK_DIR}/Lib/x86"
- "C:/Program Files/Microsoft DirectX SDK/Lib/x86"
- "C:/DXSDK/Include/Lib/x86"
- NO_DEFAULT_PATH
- DOC "Where can the Direct3D d3dx9 library be found"
- )
- ENDIF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
- DOC "Where can the Direct3D d3dx9 library be found"
- )
-
- # if Direct3D libraries found, then we're ok
- IF(DIRECT3D_d3d9_LIBRARY)
- IF(DIRECT3D_d3dx9_LIBRARY)
- # everything found
- SET(DIRECT3D_FOUND "YES")
- ENDIF(DIRECT3D_d3dx9_LIBRARY)
- ENDIF(DIRECT3D_d3d9_LIBRARY)
- ENDIF(DIRECT3D_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- DIRECT3D_INCLUDE_DIR
- DIRECT3D_d3d9_LIBRARY
- DIRECT3D_d3dx9_LIBRARY
- )
-
-
-ENDIF(WIN32 AND NOT MINGW)
-
-
-#---------------------------------------------------------------------
-IF(DIRECT3D_FOUND)
- SET(DIRECT3D_INCLUDE_DIRS ${DIRECT3D_INCLUDE_DIR})
- SET(DIRECT3D_LIBRARIES
- "${DIRECT3D_d3d9_LIBRARY}"
- "${DIRECT3D_d3dx9_LIBRARY}"
- )
-ELSE(DIRECT3D_FOUND)
- # make FIND_PACKAGE friendly
- IF(NOT DIRECT3D_FIND_QUIETLY)
- IF(DIRECT3D_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR
- "Direct3D required, please specify it's location.")
- ENDIF(DIRECT3D_FIND_REQUIRED)
- ENDIF(NOT DIRECT3D_FIND_QUIETLY)
-ENDIF(DIRECT3D_FOUND)
diff --git a/CMakeModules/FindDIRECTSHOW.cmake b/CMakeModules/FindDIRECTSHOW.cmake
deleted file mode 100644
index 95ecea8..0000000
--- a/CMakeModules/FindDIRECTSHOW.cmake
+++ /dev/null
@@ -1,283 +0,0 @@
-#############################################################################
-#
-# $Id: FindDIRECTSHOW.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Direct Show on Windows. This code comes originaly
-# from http://www.cmake.org/Wiki/CMakeUserFindDirectShow .
-# Once loaded this will define:
-#
-# DIRECTSHOW_FOUND - system has DirectShow
-# DIRECTSHOW_INCLUDE_DIRS - include directory for DirectShow
-# DIRECTSHOW_LIBRARIES - libraries you need to link to
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-SET(DIRECTSHOW_FOUND "NO")
-
-# DirectShow is only available on Windows platforms
-IF(WIN32 AND NOT MINGW)
-# find DirectX
- FIND_PATH(DIRECTX_INCLUDE_PATH ddraw.h
- "$ENV{DXSDK_DIR}/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
- "C:/DXSDK/include"
- "C:/Program Files/Microsoft Platform SDK/Include"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
- NO_DEFAULT_PATH
- DOC "What is the path where the file ddraw.h can be found"
- )
- FIND_PATH(DIRECTX_INCLUDE_PATH ddraw.h
- DOC "What is the path where the file ddraw.h can be found"
- )
-
- # find DirectShow include directory
- FIND_PATH(DIRECTSHOW_dshow_INCLUDE_PATH dshow.h
- "$ENV{WINSDK_HOME}/Include"
- "$ENV{DXSDK_DIR}/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
- "C:/DXSDK/include"
- "C:/Program Files/Microsoft Platform SDK/Include"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
- NO_DEFAULT_PATH
- DOC "What is the path where the file dshow.h can be found"
- )
- FIND_PATH(DIRECTSHOW_dshow_INCLUDE_PATH dshow.h
- DOC "What is the path where the file dshow.h can be found"
- )
-
- FIND_PATH(DIRECTSHOW_qedit_INCLUDE_PATH qedit.h
- "$ENV{WINSDK_HOME}/Include"
- "$ENV{DXSDK_DIR}/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
- "C:/DXSDK/include"
- "C:/Program Files/Microsoft Platform SDK/Include"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
- NO_DEFAULT_PATH
- DOC "What is the path where the file qedit.h can be found"
- )
- FIND_PATH(DIRECTSHOW_qedit_INCLUDE_PATH qedit.h
- DOC "What is the path where the file qedit.h can be found"
- )
-
- FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
- DOC "What is the path where the file atlbase.h can be found"
- )
-# Specific path search for Visual Studio .NET 2003
- IF(MSVC71)
- IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/atlmfc/include"
- "$ENV{VS71COMNTOOLS}/../../Vc7/atlmfc/include"
- DOC "What is the path where the file atlbase.h can be found"
- )
- ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- ENDIF(MSVC71)
-
-# Specific path search for Visual Studio 2005
- IF(MSVC80)
- IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
- "C:/Program Files/Microsoft Visual Studio 8/VC/atlmfc/include"
- "$ENV{VS80COMNTOOLS}/../../VC/atlmfc/include"
- DOC "What is the path where the file atlbase.h can be found"
- )
- ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- ENDIF(MSVC80)
-
-# Specific path search for Visual Studio 2008
- IF(MSVC90)
- IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
- "C:/Program Files/Microsoft Visual Studio 9.0/VC/atlmfc/include"
- "$ENV{VS90COMNTOOLS}/../../VC/atlmfc/include"
- DOC "What is the path where the file atlbase.h can be found"
- )
- ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- ENDIF(MSVC90)
-# Specific path search for Visual Studio 2008
- IF(MSVC10)
- IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
- "C:/Program Files/Microsoft Visual Studio 10.0/VC/atlmfc/include"
- "$ENV{VS100COMNTOOLS}/../../VC/atlmfc/include"
- DOC "What is the path where the file atlbase.h can be found"
- )
- ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
- ENDIF(MSVC10)
- IF(DIRECTX_INCLUDE_PATH AND DIRECTSHOW_dshow_INCLUDE_PATH AND DIRECTSHOW_qedit_INCLUDE_PATH AND DIRECTSHOW_atlbase_INCLUDE_PATH)
- SET(DIRECTSHOW_INCLUDE_DIRS ${DIRECTX_INCLUDE_PATH}
- ${DIRECTSHOW_dshow_INCLUDE_PATH}
- ${DIRECTSHOW_qedit_INCLUDE_PATH}
- ${DIRECTSHOW_atlbase_INCLUDE_PATH})
- ENDIF(DIRECTX_INCLUDE_PATH AND DIRECTSHOW_dshow_INCLUDE_PATH AND DIRECTSHOW_qedit_INCLUDE_PATH AND DIRECTSHOW_atlbase_INCLUDE_PATH)
- # if DirectShow include dir found, then find DirectShow libraries
- IF(DIRECTSHOW_INCLUDE_DIRS)
- IF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
- NAMES strmiids
- PATHS
- "$ENV{WINSDK_HOME}/Lib/x64"
- "$ENV{DXSDK_DIR}/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
- "C:/DXSDK/lib/x64"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x64"
- "C:/Program Files/Microsoft Platform SDK/Lib/x64"
- NO_DEFAULT_PATH
- DOC "Where can the DirectShow strmiids library be found"
- )
- ELSE(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
- NAMES strmiids
- PATHS
- "$ENV{WINSDK_HOME}/Lib"
- "$ENV{WINSDK_HOME}/Lib/x86"
- "$ENV{DXSDK_DIR}/Lib"
- "$ENV{DXSDK_DIR}/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib"
- "C:/DXSDK/lib"
- "C:/DXSDK/lib/x64"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
- "C:/Program Files/Microsoft Platform SDK/Lib"
- NO_DEFAULT_PATH
- DOC "Where can the DirectShow strmiids library be found"
- )
- ENDIF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
- NAMES strmiids
- DOC "Where can the DirectShow strmiids library be found"
- )
-
-
- IF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
- NAMES quartz
- PATHS
- "$ENV{WINSDK_HOME}/Lib/x64"
- "$ENV{DXSDK_DIR}/Lib/x64"
- "C:/DXSDK/lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x64"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x64"
- "C:/Program Files/Microsoft Platform SDK/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
- NO_DEFAULT_PATH
- DOC "Where can the DirectShow quartz library be found"
- )
- ELSE(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
- NAMES quartz
- PATHS
- "$ENV{WINSDK_HOME}/Lib/x86"
- "$ENV{DXSDK_DIR}/Lib/x86"
- "C:/DXSDK/lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x86"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x86"
- "C:/Program Files/Microsoft Platform SDK/Lib/x86"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A"
- "$ENV{WINSDK_HOME}/Lib"
- "$ENV{DXSDK_DIR}/Lib"
- "C:/DXSDK/lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib"
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
- "C:/Program Files/Microsoft Platform SDK/Lib"
- NO_DEFAULT_PATH
- DOC "Where can the DirectShow quartz library be found"
- )
- ENDIF(CMAKE_CL_64)
- FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
- NAMES quartz
- DOC "Where can the DirectShow quartz library be found"
- )
-
- # if DirectShow libraries found, then we're ok
- IF(DIRECTSHOW_strmiids_LIBRARY)
- IF(DIRECTSHOW_quartz_LIBRARY)
- # everything found
- SET(DIRECTSHOW_FOUND "YES")
- ENDIF(DIRECTSHOW_quartz_LIBRARY)
- ENDIF(DIRECTSHOW_strmiids_LIBRARY)
- ENDIF(DIRECTSHOW_INCLUDE_DIRS)
-
- MARK_AS_ADVANCED(
- DIRECTSHOW_INCLUDE_DIRS
- DIRECTSHOW_strmiids_LIBRARY
- DIRECTSHOW_quartz_LIBRARY
- DIRECTX_INCLUDE_PATH
- DIRECTSHOW_dshow_INCLUDE_PATH
- DIRECTSHOW_qedit_INCLUDE_PATH
- DIRECTSHOW_atlbase_INCLUDE_PATH
- )
-
-ENDIF(WIN32 AND NOT MINGW)
-
-
-#---------------------------------------------------------------------
-IF(DIRECTSHOW_FOUND)
- SET(DIRECTSHOW_LIBRARIES
- ${DIRECTSHOW_strmiids_LIBRARY}
- ${DIRECTSHOW_quartz_LIBRARY}
- )
-ELSE(DIRECTSHOW_FOUND)
- # make FIND_PACKAGE friendly
- IF(NOT DIRECTSHOW_FIND_QUIETLY)
- IF(DIRECTSHOW_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR
- "DirectShow required, please specify it's location.")
- ENDIF(DIRECTSHOW_FIND_REQUIRED)
- ENDIF(NOT DIRECTSHOW_FIND_QUIETLY)
-ENDIF(DIRECTSHOW_FOUND)
diff --git a/CMakeModules/FindDL.cmake b/CMakeModules/FindDL.cmake
deleted file mode 100644
index f62ab6b..0000000
--- a/CMakeModules/FindDL.cmake
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: FindDL.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find dl library.
-# Once run this will define:
-#
-# DL_FOUND
-# DL_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-FIND_LIBRARY(DL_LIBRARY
- NAMES dl
- PATHS
- $ENV{DL_HOME}/lib
- /usr/lib
- /usr/local/lib
- )
-
-#MESSAGE("DBG DL_LIBRARY=${DL_LIBRARY}")
-
-IF(DL_LIBRARY)
- SET(DL_LIBRARIES ${DL_LIBRARY})
- SET(DL_FOUND TRUE)
-ELSE()
- SET(DL_FOUND FALSE)
-ENDIF()
-
-MARK_AS_ADVANCED(
- DL_LIBRARY
- )
-
diff --git a/CMakeModules/FindDMTX.cmake b/CMakeModules/FindDMTX.cmake
deleted file mode 100644
index db31d64..0000000
--- a/CMakeModules/FindDMTX.cmake
+++ /dev/null
@@ -1,71 +0,0 @@
-#############################################################################
-#
-# $Id: FindV4L2.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find dmtx library.
-# Once run this will define:
-#
-# DMTX_FOUND
-# DMTX_INCLUDE_DIRS
-# DMTX_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-find_path(DMTX_INCLUDE_DIRS dmtx.h
- $ENV{DMTX_DIR}/include
- /usr/include
- /usr/local/include
-)
-
-find_library(DMTX_LIBRARIES
- NAMES dmtx
- PATHS
- $ENV{DMTX_DIR}/lib
- /usr/lib
- /usr/local/lib
-)
-
-if(DMTX_INCLUDE_DIRS AND DMTX_LIBRARIES)
- set(DMTX_FOUND TRUE)
-else()
- set(DMTX_FOUND FALSE)
-endif()
-
-mark_as_advanced(
- DMTX_INCLUDE_DIRS
- DMTX_LIBRARIES
-)
-
diff --git a/CMakeModules/FindFFMPEG.cmake b/CMakeModules/FindFFMPEG.cmake
deleted file mode 100644
index ff8dea3..0000000
--- a/CMakeModules/FindFFMPEG.cmake
+++ /dev/null
@@ -1,322 +0,0 @@
-#############################################################################
-#
-# $Id: FindFFMPEG.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find FFMPEG. FFMpeg depend son Zlib.
-# Once run this will define:
-#
-# FFMPEG_FOUND - system has FFMPEG
-# FFMPEG_INCLUDE_DIRS - the FFMPEG include directory
-# FFMPEG_LIBRARIES - Link these to use FFMPEG
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# detection of the FFMPEG headers location
-if(MINGW)
- find_path(FFMPEG_INCLUDE_DIR_AVCODEC
- NAMES
- libavcodec/avcodec.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
- NAMES
- libavformat/avformat.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVUTIL
- NAMES
- libavutil/avutil.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_SWSCALE
- NAMES
- libswscale/swscale.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- libswscale
- ffmpeg
- )
-
- # Detection of the FFMPEG library on Unix
- find_library(FFMPEG_AVUTIL_LIBRARY
- NAMES
- avutil
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
- find_library(FFMPEG_AVCODEC_LIBRARY
- NAMES
- avcodec
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
- find_library(FFMPEG_AVFORMAT_LIBRARY
- NAMES
- avformat
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-
- find_library(FFMPEG_AVCORE_LIBRARY
- NAMES
- avcore
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-
- find_library(FFMPEG_SWSCALE_LIBRARY
- NAMES
- swscale
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-else(MINGW)
- find_path(FFMPEG_INCLUDE_DIR_AVCODEC
- NAMES
- libavcodec/avcodec.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
- NAMES
- libavformat/avformat.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVUTIL
- NAMES
- libavutil/avutil.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_SWSCALE
- NAMES
- libswscale/swscale.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- libswscale
- ffmpeg
- )
-
- # Detection of the FFMPEG library on Unix
- find_library(FFMPEG_AVUTIL_LIBRARY
- NAMES
- avutil
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
- find_library(FFMPEG_AVCODEC_LIBRARY
- NAMES
- avcodec
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
- find_library(FFMPEG_AVFORMAT_LIBRARY
- NAMES
- avformat
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-
- find_library(FFMPEG_AVCORE_LIBRARY
- NAMES
- avcore
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-
- find_library(FFMPEG_SWSCALE_LIBRARY
- NAMES
- swscale
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-endif(MINGW)
-
- # FFMpeg depend son Zlib
- FIND_PACKAGE(ZLIB)
- if(NOT ZLIB_FOUND)
- FIND_PACKAGE(MyZLIB)
- endif()
-
- # FFMpeg depend son BZip2
- # with CMake 2.6, the CMake bzip2 package material is named FindBZip2.cmake
- # while with CMake 2.8, the name is FindBZIP2.cmake
- # that is why we need to call FIND_PACKAGE(BZip2) and FIND_PACKAGE(BZIP2)
- FIND_PACKAGE(BZIP2 QUIET)
- # MESSAGE("BZIP2_FOUND: ${BZIP2_FOUND}")
- if(NOT BZIP2_FOUND)
- FIND_PACKAGE(BZip2 QUIET)
- # MESSAGE("BZIP2_FOUND: ${BZIP2_FOUND}")
- endif()
-
- # FFMpeg may depend also on iconv since probably version 1.1.3 where if detected,
- # iconv usage is enabled by default
- find_package(ICONV QUIET)
- #message("ICONV_FOUND: ${ICONV_FOUND}")
-
-IF(FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES AND BZIP2_LIBRARIES)
- SET(FFMPEG_FOUND TRUE)
- SET(FFMPEG_INCLUDE_DIRS
- ${FFMPEG_INCLUDE_DIR_AVCODEC}
- ${FFMPEG_INCLUDE_DIR_AVFORMAT}
- ${FFMPEG_INCLUDE_DIR_AVUTIL}
- ${FFMPEG_INCLUDE_DIR_SWSCALE}
- )
- SET(FFMPEG_LIBRARIES
- ${FFMPEG_SWSCALE_LIBRARY}
- ${FFMPEG_AVFORMAT_LIBRARY}
- ${FFMPEG_AVCODEC_LIBRARY}
- ${FFMPEG_AVUTIL_LIBRARY}
- )
- if(FFMPEG_AVCORE_LIBRARY)
- LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
- endif()
- list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES} ${BZIP2_LIBRARIES})
- if(ICONV_FOUND)
- list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
- endif()
-
-elseif(MINGW AND FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES)
- # Bzip2 is nor requested with mingw-w64
- SET(FFMPEG_FOUND TRUE)
- SET(FFMPEG_INCLUDE_DIRS
- ${FFMPEG_INCLUDE_DIR_AVCODEC}
- ${FFMPEG_INCLUDE_DIR_AVFORMAT}
- ${FFMPEG_INCLUDE_DIR_AVUTIL}
- ${FFMPEG_INCLUDE_DIR_SWSCALE}
- )
- SET(FFMPEG_LIBRARIES
- ${FFMPEG_SWSCALE_LIBRARY}
- ${FFMPEG_AVFORMAT_LIBRARY}
- ${FFMPEG_AVCODEC_LIBRARY}
- ${FFMPEG_AVUTIL_LIBRARY}
- )
- if(FFMPEG_AVCORE_LIBRARY)
- LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
- endif()
- list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES})
- if(ICONV_FOUND)
- list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
- endif()
-
-ELSE()
- SET(FFMPEG_FOUND FALSE)
-ENDIF ()
-
-MARK_AS_ADVANCED(
- BZIP2_DIR
- FFMPEG_INCLUDE_DIR_AVCODEC
- FFMPEG_INCLUDE_DIR_AVFORMAT
- FFMPEG_INCLUDE_DIR_AVUTIL
- FFMPEG_INCLUDE_DIR_SWSCALE
- FFMPEG_AVUTIL_LIBRARY
- FFMPEG_AVFORMAT_LIBRARY
- FFMPEG_AVCODEC_LIBRARY
- FFMPEG_SWSCALE_LIBRARY
- FFMPEG_AVCORE_LIBRARY
- FFMPEG_INCLUDE_DIRS
- FFMPEG_LIBRARIES
-)
-
diff --git a/CMakeModules/FindGDI.cmake b/CMakeModules/FindGDI.cmake
deleted file mode 100644
index 3db6543..0000000
--- a/CMakeModules/FindGDI.cmake
+++ /dev/null
@@ -1,159 +0,0 @@
-#############################################################################
-#
-# $Id: FindGDI.cmake 5286 2015-02-09 14:36:35Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find GDI (Graphics Device Interface) on Windows.
-# Once loaded this will define:
-#
-# GDI_FOUND - system has GDI
-# GDI_LIBRARIES - libraries you need to link to
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-SET(GDI_FOUND "NO")
-
-# GDI (Graphics Device Interface) is only available on Windows platforms
-IF(WIN32)
- IF(MINGW)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/MinGW/lib"
- "C:/mingw/mingw/lib"
- "$ENV{MINGW_DIR}/lib"
- "$ENV{MINGW_DIR}/mingw/lib"
- DOC "Where can the GDI (Graphics Device Interface) library be found"
- NO_DEFAULT_PATH
- )
- ELSE(MINGW)
- IF(CMAKE_CL_64)
- # Generic path seach
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "$ENV{WINSDK_DIR}/Lib/x64"
- "$ENV{WINSDK_HOME}/Lib/x64"
- "$ENV{DXSDK_DIR}/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0a/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v7.1/Lib/x64"
- "C:/Program Files/Microsoft SDKs/Windows/v7.1A/Lib/x64"
- "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.0A/Lib/x64"
- "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib/x64"
- "C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x64"
- "C:/Program Files/Microsoft Platform SDK/Lib/x64"
- "C:/DXSDK/Include/Lib/x64"
- DOC "Where can the GDI (Graphics Device Interface) library be found"
- )
-
- # Specific path search for Visual Studio .NET 2003
- IF(MSVC71)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
- )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC71)
-
- # Specific path search for Visual Studio 2005
- IF(MSVC80)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio 8/VC/PlatformSDK/Lib/AMD64" )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC80)
-
- # Specific path search for Visual Studio 2008
- IF(MSVC90)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio 9/VC/PlatformSDK/Lib/AMD64" )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC90)
-
- ELSE(CMAKE_CL_64)
- # Generic path seach
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "$ENV{WINSDK_DIR}/Lib"
- "$ENV{WINSDK_HOME}/Lib"
- "$ENV{DXSDK_DIR}/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0a/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
- "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib"
- "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.0A/Lib"
- "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1/Lib"
- "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib"
- "C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x86"
- "C:/Program Files/Microsoft Platform SDK/Lib"
- "C:/DXSDK/Include/Lib"
- DOC "Where can the GDI (Graphics Device Interface) library be found"
- )
-
- # Specific path search for Visual Studio .NET 2003
- IF(MSVC71)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
- )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC71)
-
- # Specific path search for Visual Studio 2005
- IF(MSVC80)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio 8/VC/PlatformSDK/Lib"
- )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC80)
-
- # Specific path search for Visual Studio 2008
- IF(MSVC90)
- IF(NOT GDI_LIBRARY)
- FIND_LIBRARY(GDI_LIBRARY gdi32
- "C:/Program Files/Microsoft Visual Studio 9/VC/PlatformSDK/Lib"
- )
- ENDIF(NOT GDI_LIBRARY)
- ENDIF(MSVC90)
- ENDIF(CMAKE_CL_64)
- ENDIF(MINGW)
- # if GDI libraries found, then we're ok
- IF(GDI_LIBRARY)
- SET(GDI_FOUND "YES")
- SET(GDI_LIBRARIES ${GDI_LIBRARY})
-
- ENDIF(GDI_LIBRARY)
-
- MARK_AS_ADVANCED(GDI_LIBRARY)
-ENDIF(WIN32)
diff --git a/CMakeModules/FindGSL.cmake b/CMakeModules/FindGSL.cmake
deleted file mode 100644
index 91a0801..0000000
--- a/CMakeModules/FindGSL.cmake
+++ /dev/null
@@ -1,193 +0,0 @@
-#############################################################################
-#
-# $Id: FindGSL.cmake 4815 2014-07-31 14:00:05Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find gnu scientific library GSL
-# (see http://www.gnu.org/software/gsl/)
-# Once run this will define:
-#
-# GSL_FOUND : system has GSL lib
-# GSL_LIBRARIES : full path to the libraries
-# GSL_INCLUDE_DIRS : where to find headers
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# it seems that a macro() can't accept a list as input. That's why we have
-# MY_LIBRARY1 and MY_LIBRARY2
-macro(CheckCompilation_gsl2 MY_INCLUDE_DIR MY_LIBRARY1 MY_LIBRARY2 MY_BUILD_SUCCEED2)
- # The material is found. Check if it works on the requested architecture
- include(CheckCXXSourceCompiles)
- #message("Test compilation: ${MY_INCLUDE_DIR} + ${MY_LIBRARY1} + ${MY_LIBRARY2}")
-
- set(CMAKE_REQUIRED_LIBRARIES ${MY_LIBRARY1} ${MY_LIBRARY2})
- set(CMAKE_REQUIRED_INCLUDES ${MY_INCLUDE_DIR})
- check_cxx_source_compiles("
- #include <gsl/gsl_linalg.h> // Contrib for GSL library
- #include <gsl/gsl_math.h>
- #include <gsl/gsl_eigen.h>
- int main()
- {
- gsl_matrix *A2 = gsl_matrix_alloc(6, 6) ;
- }
- " BUILD_SUCCEED2)
- #message("BUILD_SUCCEED: ${BUILD_SUCCEED}")
- set(${MY_BUILD_SUCCEED2} ${BUILD_SUCCEED2})
-endmacro()
-
-macro(CheckCompilation_gsl1 MY_INCLUDE_DIR MY_LIBRARY MY_BUILD_SUCCEED)
- # The material is found. Check if it works on the requested architecture
- include(CheckCXXSourceCompiles)
- #message("Test compilation: ${MY_INCLUDE_DIR} + ${MY_LIBRARY}")
-
- set(CMAKE_REQUIRED_LIBRARIES ${MY_LIBRARY})
- set(CMAKE_REQUIRED_INCLUDES ${MY_INCLUDE_DIR})
- check_cxx_source_compiles("
- #include <gsl/gsl_linalg.h> // Contrib for GSL library
- #include <gsl/gsl_math.h>
- #include <gsl/gsl_eigen.h>
- int main()
- {
- gsl_matrix *A1 = gsl_matrix_alloc(6, 6) ;
- }
- " BUILD_SUCCEED)
- #message("BUILD_SUCCEED: ${BUILD_SUCCEED}")
- set(${MY_BUILD_SUCCEED} ${BUILD_SUCCEED})
-endmacro()
-
-find_path(GSL_INCLUDE_DIR gsl/gsl_linalg.h
- $ENV{GSL_HOME}/include
- $ENV{GSL_DIR}/include
- /usr/include
- /usr/local/include
-)
-
-find_library(GSL_gsl_LIBRARY
- NAMES gsl
- PATHS
- "$ENV{GSL_HOME}/lib"
- "$ENV{GSL_DIR}/lib"
- /usr/lib
- /usr/local/lib
-)
-
-if(WIN32)
- find_library(GSL_cblas_LIBRARY
- NAMES cblas
- PATHS
- "$ENV{GSL_HOME}/lib"
- "$ENV{GSL_DIR}/lib"
- /usr/lib
- /usr/local/lib
- )
-
- find_library(GSL_gsl_LIBRARY_DEBUG
- NAMES gsl_d
- PATHS
- "$ENV{GSL_HOME}/lib"
- "$ENV{GSL_DIR}/lib"
- /usr/lib
- /usr/local/lib
- )
- find_library(GSL_cblas_LIBRARY_DEBUG
- NAMES cblas_d
- PATHS
- "$ENV{GSL_HOME}/lib"
- "$ENV{GSL_DIR}/lib"
- /usr/lib
- /usr/local/lib
- )
-
- if(GSL_INCLUDE_DIR AND GSL_gsl_LIBRARY AND GSL_cblas_LIBRARY
- AND GSL_gsl_LIBRARY_DEBUG AND GSL_cblas_LIBRARY_DEBUG)
- set(GSL_LIBRARIES "optimized" ${GSL_cblas_LIBRARY}
- "optimized" ${GSL_gsl_LIBRARY}
- "debug" ${GSL_cblas_LIBRARY_DEBUG}
- "debug" ${GSL_gsl_LIBRARY_DEBUG})
- set(GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR})
- set(GSL_FOUND TRUE)
- else()
- set(GSL_FOUND FALSE)
- endif()
-
- mark_as_advanced(
- GSL_gsl_LIBRARY_DEBUG
- GSL_cblas_LIBRARY_DEBUG
- )
-else()
- if(GSL_INCLUDE_DIR AND GSL_gsl_LIBRARY)
- # Check if gsl library is sufficient
- set(GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR})
- set(GSL_LIBRARIES ${GSL_gsl_LIBRARY})
-
- CheckCompilation_gsl1(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} BUILD_SUCCEED1)
- #message("BUILD_STATUS 1: ${BUILD_SUCCEED1}")
- if(BUILD_SUCCEED1)
- set(GSL_FOUND TRUE)
- else()
- # Try to add gslcblas library if requested
-
- find_library(GSL_cblas_LIBRARY
- NAMES gslcblas
- PATHS
- "$ENV{GSL_HOME}/lib"
- "$ENV{GSL_DIR}/lib"
- /usr/lib
- /usr/local/lib
- )
- if(GSL_cblas_LIBRARY)
- list(APPEND GSL_LIBRARIES ${GSL_cblas_LIBRARY})
- #message("add cblas to GSL_LIBRARIES: ${GSL_LIBRARIES}")
- CheckCompilation_gsl2(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} ${GSL_cblas_LIBRARY} BUILD_SUCCEED2)
- #message("BUILD_STATUS 2: ${BUILD_SUCCEED2}")
- if(BUILD_SUCCEED2)
- set(GSL_FOUND TRUE)
- else()
- set(GSL_FOUND FALSE)
- endif()
- else()
- set(GSL_FOUND FALSE)
- endif()
- endif()
- else()
- set(GSL_FOUND FALSE)
- endif()
-endif()
-
-mark_as_advanced(
- GSL_gsl_LIBRARY
- GSL_cblas_LIBRARY
- GSL_INCLUDE_DIR
-)
-
diff --git a/CMakeModules/FindICONV.cmake b/CMakeModules/FindICONV.cmake
deleted file mode 100755
index 2a5ac18..0000000
--- a/CMakeModules/FindICONV.cmake
+++ /dev/null
@@ -1,89 +0,0 @@
-#############################################################################
-#
-# $Id: FindICONV.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find ICONV library.
-# Once run this will define:
-#
-# ICONV_FOUND
-# ICONV_INCLUDE_DIRS
-# ICONV_LIBRARIES
-#
-# Authors:
-# Anthony Saunier
-#
-#############################################################################
-
-if(MINGW)
- find_path(ICONV_INCLUDE_DIR iconv.h
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- )
- find_library(ICONV_LIBRARY iconv
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-else()
- find_path(ICONV_INCLUDE_DIR iconv.h
- $ENV{ICONV_DIR}/include
- $ENV{ICONV_HOME}/include
- $ENV{XML2_DIR}/include
- $ENV{XML2_HOME}/include
- "c:/libxml2/include"
- "c:/iconv/include"
- )
- find_library(ICONV_LIBRARY iconv
- $ENV{ICONV_DIR}/lib
- $ENV{ICONV_HOME}/lib
- $ENV{XML2_DIR}/lib
- $ENV{XML2_HOME}/lib
- "c:/libxml2/lib"
- "c:/iconv/lib"
- )
-endif()
-
-if(ICONV_LIBRARY)
- SET(ICONV_LIBRARIES ${ICONV_LIBRARY})
-endif()
-
-if(ICONV_LIBRARIES AND ICONV_INCLUDE_DIR)
- SET(ICONV_INCLUDE_DIRS ${ICONV_INCLUDE_DIR})
- SET(ICONV_FOUND TRUE)
-else()
- SET(ICONV_FOUND FALSE)
-endif()
-
-mark_as_advanced(
- ICONV_INCLUDE_DIR
- ICONV_LIBRARIES
- ICONV_LIBRARY
-)
diff --git a/CMakeModules/FindIRISA.cmake b/CMakeModules/FindIRISA.cmake
deleted file mode 100644
index f7d9993..0000000
--- a/CMakeModules/FindIRISA.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#############################################################################
-#
-# $Id: FindIRISA.cmake 5309 2015-02-11 11:08:15Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Cerebellum Irisa library API to dial with the robot Afma6
-#
-# IRISA_FOUND
-# IRISA_INCLUDE_DIRS
-# IRISA_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindIRISA.cmake: only available for Unix.")
- SET(IRISA_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_PATH(IRISA_INCLUDE_DIR irisa_Afma6.h
- $ENV{IRISA_HOME}/include
- /local/soft/Cerebellum/Irisa/current/src
- /home/soft/Cerebellum/Irisa/current/src
- )
- #MESSAGE("DBG IRISA_INCLUDE_DIR=${IRISA_INCLUDE_DIR}")
-
- FIND_LIBRARY(IRISA_LIBRARY
- NAMES irisa
- PATHS
- $ENV{IRISA_HOME}/lib
- /local/soft/Cerebellum/Irisa/current/src
- /home/soft/Cerebellum/Irisa/current/src
- )
-
- #MESSAGE("DBG IRISA_LIBRARY=${IRISA_LIBRARY}")
-
- ## --------------------------------
-
- IF(IRISA_LIBRARY)
- SET(IRISA_LIBRARIES ${IRISA_LIBRARY})
- ELSE(IRISA_LIBRARY)
- #MESSAGE(SEND_ERROR "Irisa library not found.")
- ENDIF(IRISA_LIBRARY)
-
- IF(NOT IRISA_INCLUDE_DIR)
- #MESSAGE(SEND_ERROR "Irisa include dir not found.")
- ENDIF(NOT IRISA_INCLUDE_DIR)
-
- IF(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
- SET(IRISA_INCLUDE_DIRS ${IRISA_INCLUDE_DIR})
- SET(IRISA_FOUND TRUE)
- ELSE(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
- SET(IRISA_FOUND FALSE)
- ENDIF(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- IRISA_INCLUDE_DIR
- IRISA_LIBRARIES
- IRISA_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindLAPACK_C.cmake b/CMakeModules/FindLAPACK_C.cmake
deleted file mode 100644
index 577944d..0000000
--- a/CMakeModules/FindLAPACK_C.cmake
+++ /dev/null
@@ -1,165 +0,0 @@
-#############################################################################
-#
-# $Id: FindLAPACK_C.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find lapack, the Linear Algebra PACKage.
-# Since the FindLAPACK.cmake provided with CMake is for Fortran language,
-# this file is able to detect lapack for C language.
-# Once run this will define:
-#
-# LAPACK_FOUND
-# LAPACK_LIBRARIES
-#
-# Authors:
-# Filip Novotny
-# Fabien Spindler
-#
-#############################################################################
-
-SET(LAPACK_FOUND FALSE)
-SET(LAPACK_LIBRARIES "")
-IF(WIN32)
- FIND_LIBRARY(LAPACK_LIBRARY_LAPACK_RELEASE
- NAMES lapack
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_BLAS_RELEASE
- NAMES blas
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_F2C_RELEASE
- NAMES libf2c
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_LAPACK_DEBUG
- NAMES lapackd
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_BLAS_DEBUG
- NAMES blasd
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_F2C_DEBUG
- NAMES libf2cd
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- )
-
- IF((LAPACK_LIBRARY_LAPACK_RELEASE AND LAPACK_LIBRARY_BLAS_RELEASE AND LAPACK_LIBRARY_F2C_RELEASE))
- LIST(APPEND LAPACK_LIBRARIES optimized ${LAPACK_LIBRARY_LAPACK_RELEASE})
- LIST(APPEND LAPACK_LIBRARIES optimized ${LAPACK_LIBRARY_BLAS_RELEASE})
- LIST(APPEND LAPACK_LIBRARIES optimized ${LAPACK_LIBRARY_F2C_RELEASE})
-
- SET(LAPACK_FOUND TRUE)
- ENDIF()
- IF((LAPACK_LIBRARY_LAPACK_DEBUG AND LAPACK_LIBRARY_BLAS_DEBUG AND LAPACK_LIBRARY_F2C_DEBUG))
- LIST(APPEND LAPACK_LIBRARIES debug ${LAPACK_LIBRARY_LAPACK_DEBUG})
- LIST(APPEND LAPACK_LIBRARIES debug ${LAPACK_LIBRARY_BLAS_DEBUG})
- LIST(APPEND LAPACK_LIBRARIES debug ${LAPACK_LIBRARY_F2C_DEBUG})
- SET(LAPACK_FOUND TRUE)
- ENDIF()
-
-
-ELSE(WIN32)
- FIND_LIBRARY(LAPACK_LIBRARY_LAPACK
- NAMES lapack
- PATHS
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- /usr/lib
- /usr/lib64
- /usr/local/lib
- /usr/local/lib64
- )
-
- FIND_LIBRARY(LAPACK_LIBRARY_BLAS
- NAMES blas
- PATHS
- $ENV{LAPACK_DIR}
- $ENV{LAPACK_HOME}
- $ENV{LAPACK_HOME}/lib
- $ENV{LAPACK_DIR}/lib
- /usr/lib
- /usr/lib64
- /usr/local/lib
- /usr/local/lib64
- )
- IF((LAPACK_LIBRARY_LAPACK AND LAPACK_LIBRARY_BLAS))
- SET(LAPACK_LIBRARIES ${LAPACK_LIBRARY_LAPACK} ${LAPACK_LIBRARY_BLAS})
- SET(LAPACK_FOUND TRUE)
- ENDIF()
-ENDIF(WIN32)
-## --------------------------------
-
-
-MARK_AS_ADVANCED(
- LAPACK_LIBRARIES
- LAPACK_LIBRARY_LAPACK
- LAPACK_LIBRARY_BLAS
- LAPACK_LIBRARY_LAPACK_RELEASE
- LAPACK_LIBRARY_BLAS_RELEASE
- LAPACK_LIBRARY_LAPACK_DEBUG
- LAPACK_LIBRARY_BLAS_DEBUG
- LAPACK_LIBRARY_F2C_DEBUG
- LAPACK_LIBRARY_F2C_RELEASE
-)
-
diff --git a/CMakeModules/FindLIBFREENECT.cmake b/CMakeModules/FindLIBFREENECT.cmake
deleted file mode 100644
index a9325f8..0000000
--- a/CMakeModules/FindLIBFREENECT.cmake
+++ /dev/null
@@ -1,98 +0,0 @@
-#############################################################################
-#
-# $Id: FindLIBFREENECT.cmake 4695 2014-03-15 11:28:48Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Cerebellum Irisa library API to dial with the robot Afma6
-#
-# LIBFREENECT_FOUND
-# LIBFREENECT_INCLUDE_DIRS
-# LIBFREENECT_LIBRARIES
-#
-# Authors:
-# Celine Teuliere
-# Fabien Spindler
-
-IF(WIN32)
- FIND_PATH(LIBFREENECT_HPP_INCLUDE_DIR libfreenect.hpp
- $ENV{LIBFREENECT_HOME}/include
- $ENV{LIBFREENECT_HPP_INCLUDE_DIR}
- $ENV{LIBFREENECT_H_INCLUDE_DIR}
- )
- FIND_PATH(LIBFREENECT_H_INCLUDE_DIR libfreenect.h
- $ENV{LIBFREENECT_HOME}/include
- $ENV{LIBFREENECT_HPP_INCLUDE_DIR}
- $ENV{LIBFREENECT_H_INCLUDE_DIR}
- )
- FIND_LIBRARY(LIBFREENECT_LIBRARY freenect
- $ENV{LIBFREENECT_HOME}/lib
- $ENV{LIBFREENECT_LIBRARY_DIR}
- "c:/libfreenect/lib"
- )
-ELSE()
- FIND_PATH(LIBFREENECT_HPP_INCLUDE_DIR libfreenect.hpp
- $ENV{LIBFREENECT_HOME}/include
- /usr/include
- /usr/local/include
- /usr/include/libfreenect
- /usr/local/include/libfreenect
- )
- FIND_PATH(LIBFREENECT_H_INCLUDE_DIR libfreenect.h
- $ENV{LIBFREENECT_HOME}/include
- /usr/include
- /usr/local/include
- /usr/include/libfreenect
- /usr/local/include/libfreenect
- )
- FIND_LIBRARY(LIBFREENECT_LIBRARY freenect
- $ENV{LIBFREENECT_HOME}/lib
- $ENV{LIBFREENECT_HOME}/build/lib
- /usr/lib
- /usr/local/lib
- )
-ENDIF()
-
-## --------------------------------
-IF(LIBFREENECT_LIBRARY AND LIBFREENECT_HPP_INCLUDE_DIR AND LIBFREENECT_H_INCLUDE_DIR)
- SET(LIBFREENECT_INCLUDE_DIRS ${LIBFREENECT_HPP_INCLUDE_DIR} ${LIBFREENECT_H_INCLUDE_DIR})
- SET(LIBFREENECT_LIBRARIES ${LIBFREENECT_LIBRARY})
- SET(LIBFREENECT_FOUND TRUE)
-ELSE()
- SET(LIBFREENECT_FOUND FALSE)
-ENDIF()
-
-MARK_AS_ADVANCED(
- LIBFREENECT_INCLUDE_DIRS
- LIBFREENECT_HPP_INCLUDE_DIR
- LIBFREENECT_H_INCLUDE_DIR
- LIBFREENECT_LIBRARIES
- LIBFREENECT_LIBRARY
- )
diff --git a/CMakeModules/FindLIBUSB-1.cmake b/CMakeModules/FindLIBUSB-1.cmake
deleted file mode 100644
index 4a9432c..0000000
--- a/CMakeModules/FindLIBUSB-1.cmake
+++ /dev/null
@@ -1,86 +0,0 @@
-#############################################################################
-#
-# $Id: FindLIBUSB-1.cmake 4678 2014-02-19 09:52:35Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libusb-1.0 library. Headers include dir is not searched.
-#
-# LIBUSB_1_FOUND
-# LIBUSB_1_LIBRARIES
-# LIBUSB_1_INCLUDE_DIRS
-#
-# Authors:
-# Celine Teuliere
-# Fabien Spindler
-
-IF(WIN32)
- FIND_LIBRARY(LIBUSB_1_LIBRARY libusb
- $ENV{LIBUSB_1_HOME}
- $ENV{LIBUSB_1_HOME}/lib
- "c:/libusb/lib"
- )
-ELSE(WIN32)
- FIND_LIBRARY(LIBUSB_1_LIBRARY usb-1.0
- $ENV{LIBUSB_1_HOME}/lib
- $ENV{LIBUSB_1_HOME}/build/lib
- /lib/
- /lib64/
- /usr/lib
- /usr/local/lib
- )
- FIND_PATH(LIBUSB_1_INCLUDE_DIR libusb.h
- $ENV{LIBUSB_1_HOME}/include/libusb-1.0
- $ENV{LIBUSB_1_HOME}/build/include/libusb-1.0
- /usr/include/libusb-1.0
- /usr/local/include/libusb-1.0
- )
-ENDIF(WIN32)
-
-## --------------------------------
-
-IF(LIBUSB_1_LIBRARY AND LIBUSB_1_INCLUDE_DIR)
- SET(LIBUSB_1_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIR})
- SET(LIBUSB_1_LIBRARIES ${LIBUSB_1_LIBRARY})
- SET(LIBUSB_1_FOUND TRUE)
-ELSE()
- SET(LIBUSB_1_FOUND FALSE)
-ENDIF()
-
-MARK_AS_ADVANCED(
- LIBUSB_1_INCLUDE_DIRS
- LIBUSB_1_INCLUDE_DIR
- LIBUSB_1_LIBRARIES
- LIBUSB_1_LIBRARY
- )
-
-
-
-
diff --git a/CMakeModules/FindMyCoin3D.cmake b/CMakeModules/FindMyCoin3D.cmake
deleted file mode 100644
index 1cefd16..0000000
--- a/CMakeModules/FindMyCoin3D.cmake
+++ /dev/null
@@ -1,231 +0,0 @@
-#############################################################################
-#
-# $Id: FindMyCoin3D.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find Coin library. Try first to see if Coin3D-3 is available. If not,
-# check for Coin3D-2.
-# Once run this will define:
-#
-# COIN3D_FOUND
-# COIN3D_LIBRARIES
-# COIN3D_INCLUDE_DIRS
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-MACRO(TRY_COMPILE_WITH_COIN COIN3D_LIB COIN3D_INC)
-
- # Try to compile a sample code using Coin release library
- include(CheckCXXSourceCompiles)
-
- SET(CMAKE_REQUIRED_LIBRARIES "${COIN3D_LIB}")
- SET(CMAKE_REQUIRED_INCLUDES "${COIN3D_INC}")
- SET(CMAKE_REQUIRED_DEFINITIONS "-DCOIN_DLL")
- #MESSAGE("COIN3D_LIB: ${COIN3D_LIB}")
- #MESSAGE("COIN3D_INC: ${COIN3D_INC}")
- CHECK_CXX_SOURCE_COMPILES("
- #include <Inventor/nodes/SoSeparator.h>
- int main(){
- SoSeparator *scene = new SoSeparator;
- return 0;
- }
- " IS_COMPILER_COMPATIBLE)
- #MESSAGE("DBG1 IS_COMPILER_COMPATIBLE ${IS_COMPILER_COMPATIBLE}")
-
-ENDMACRO(TRY_COMPILE_WITH_COIN)
-
-IF(UNIX OR WIN32)
- FIND_PATH(COIN3D_INCLUDE_DIR Inventor/nodes/SoSeparator.h
- $ENV{COIN_DIR}/include
- $ENV{COINDIR}/include
- $ENV{COIN_DIR}
- $ENV{COINDIR}
- $ENV{COIN3D_DIR}/include
- $ENV{COIN3DDIR}/include
- /usr/include
- /usr/include/Coin2
- /Library/Frameworks/Inventor.framework/Headers
- )
- #MESSAGE("DBG COIN3D_INCLUDE_DIR=${COIN3D_INCLUDE_DIR}")
-
-
- IF(WIN32)
- # Try first to find Coin3D-3 and only if not found Coin3D-2
- FIND_LIBRARY(COIN3D_LIBRARY_RELEASE
- NAMES coin3 coin4 #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{COIN3DDIR}/lib"
- "$ENV{COIN3D_DIR}/lib"
- )
-
- FIND_LIBRARY(COIN3D_LIBRARY_DEBUG
- NAMES coin3d coin4d #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{COIN3DDIR}/lib"
- "$ENV{COIN3D_DIR}/lib"
- )
-
- #MESSAGE("DBG COIN3D_LIBRARY_RELEASE=${COIN3D_LIBRARY_RELEASE}")
- #MESSAGE("DBG COIN3D_LIBRARY_DEBUG=${COIN3D_LIBRARY_DEBUG}")
-
- ELSE(WIN32)
- FIND_LIBRARY(COIN3D_LIBRARY
- NAMES Coin
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{COIN3DDIR}/lib"
- "$ENV{COIN3D_DIR}/lib"
- /Library/Frameworks/Inventor.framework/Libraries
- )
-
- #MESSAGE("DBG COIN3D_LIBRARY=${COIN3D_LIBRARY}")
- ENDIF(WIN32)
-
- MARK_AS_ADVANCED(
- COIN3D_INCLUDE_DIR
- COIN3D_LIBRARY_RELEASE
- COIN3D_LIBRARY_DEBUG
- COIN3D_LIBRARY
- )
- ## --------------------------------
-
- IF(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
-
- IF(WIN32)
-
- IF(COIN3D_LIBRARY_RELEASE)
-
- TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_RELEASE} ${COIN3D_INCLUDE_DIR})
-
- IF(NOT IS_COMPILER_COMPATIBLE)
- SET(COIN3D_LIBRARY_RELEASE FALSE)
- ENDIF(NOT IS_COMPILER_COMPATIBLE)
- ENDIF(COIN3D_LIBRARY_RELEASE)
-
-
- # Try to found COIN3D-2
- IF(NOT COIN3D_LIBRARY_RELEASE)
- FIND_LIBRARY(COIN3D_LIBRARY_RELEASE
- NAMES coin2 #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{COIN3DDIR}/lib"
- "$ENV{COIN3D_DIR}/lib"
- )
- TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_RELEASE} ${COIN3D_INCLUDE_DIR})
-
- IF(NOT IS_COMPILER_COMPATIBLE)
- SET(COIN3D_LIBRARY_RELEASE FALSE)
- ENDIF(NOT IS_COMPILER_COMPATIBLE)
- ENDIF(NOT COIN3D_LIBRARY_RELEASE)
-
- IF(COIN3D_LIBRARY_DEBUG)
-
- TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_DEBUG} ${COIN3D_INCLUDE_DIR})
-
- IF(NOT IS_COMPILER_COMPATIBLE)
- SET(COIN3D_LIBRARY_DEBUG FALSE)
- ENDIF(NOT IS_COMPILER_COMPATIBLE)
- ENDIF(COIN3D_LIBRARY_DEBUG)
-
- # Try to found COIN3D-2
- IF(NOT COIN3D_LIBRARY_DEBUG)
- FIND_LIBRARY(COIN3D_LIBRARY_DEBUG
- NAMES coin2d #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{COIN3DDIR}/lib"
- "$ENV{COIN3D_DIR}/lib"
- )
- TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_DEBUG} ${COIN3D_INCLUDE_DIR})
-
- IF(NOT IS_COMPILER_COMPATIBLE)
- SET(COIN3D_LIBRARY_DEBUG FALSE)
- ENDIF(NOT IS_COMPILER_COMPATIBLE)
-
- ENDIF(NOT COIN3D_LIBRARY_DEBUG)
-
- IF(NOT COIN3D_LIBRARY_RELEASE AND NOT COIN3D_LIBRARY_DEBUG)
- SET(COIN3D_FOUND FALSE)
- ENDIF(NOT COIN3D_LIBRARY_RELEASE AND NOT COIN3D_LIBRARY_DEBUG)
-
- ELSE(WIN32)
-
- TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY} ${COIN3D_INCLUDE_DIR})
-
- IF(IS_COMPILER_COMPATIBLE)
- SET(COIN3D_LIBRARIES ${COIN3D_LIBRARY})
- SET(COIN3D_FOUND TRUE)
- MARK_AS_ADVANCED(
- COIN3D_LIBRARIES
- COIN3D_LIBRARY
- )
- ELSE(IS_COMPILER_COMPATIBLE)
- SET(COIN3D_FOUND FALSE)
- ENDIF(IS_COMPILER_COMPATIBLE)
- ENDIF(WIN32)
-
- SET(COIN3D_INCLUDE_DIRS ${COIN3D_INCLUDE_DIR})
- SET(COIN3D_FOUND TRUE)
- ELSE(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
- SET(COIN3D_FOUND FALSE)
- #MESSAGE("Coin library not found.")
- ENDIF(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
-
- #MESSAGE(STATUS "COIN3D_FOUND : ${COIN3D_FOUND}")
- IF (COIN3D_FOUND)
- IF (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
- SET(COIN3D_LIBRARIES optimized ${COIN3D_LIBRARY_RELEASE}
- debug ${COIN3D_LIBRARY_DEBUG})
- ELSE (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
- IF (COIN3D_LIBRARY_DEBUG)
- SET (COIN3D_LIBRARIES ${COIN3D_LIBRARY_DEBUG})
- ENDIF (COIN3D_LIBRARY_DEBUG)
- IF (COIN3D_LIBRARY_RELEASE)
- SET (COIN3D_LIBRARIES ${COIN3D_LIBRARY_RELEASE})
- ENDIF (COIN3D_LIBRARY_RELEASE)
- ENDIF (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
- ENDIF(COIN3D_FOUND)
-
-
-ELSE(UNIX OR WIN32)
- SET(COIN3D_FOUND FALSE)
-ENDIF(UNIX OR WIN32)
diff --git a/CMakeModules/FindMyGTK2.cmake b/CMakeModules/FindMyGTK2.cmake
deleted file mode 100644
index 674de22..0000000
--- a/CMakeModules/FindMyGTK2.cmake
+++ /dev/null
@@ -1,296 +0,0 @@
-#############################################################################
-#
-# $Id: FindMyGTK2.cmake 4637 2014-02-04 09:48:33Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find GTK (and glib).
-# Once run this will define:
-#
-# GTK2_INCLUDE_DIRS - Directories to include to use GTK
-# GTK2_LIBRARIES - Files to link against to use GTK
-# GTK2_FOUND - If false, don't try to use GTK
-# GTK2_GL_FOUND - If false, don't try to use GTK's GL features
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(UNIX OR WIN32)
-
- FIND_PATH( GTK2_gtk_INCLUDE_PATH gtk/gtk.h
- $ENV{GTK2_DIR}/include/gtk-2.0
- $ENV{GTK2_HOME}/include/gtk-2.0
- /usr/include/gtk-2.0
- /usr/local/include/gtk-2.0
- /opt/gnome/include/gtk-2.0
- "C:/GTK/include/gtk-2.0"
- /sw/include/gtk-2.0
- )
-
- # Some Linux distributions (e.g. Red Hat) have glibconfig.h
- # and glib.h in different directories, so we need to look
- # for both.
- # - Atanas Georgiev <atanas at cs.columbia.edu>
-
- FIND_PATH( GTK2_glibconfig_INCLUDE_PATH glibconfig.h
- $ENV{GTK2_DIR}/lib/glib-2.0/include
- $ENV{GTK2_HOME}/lib/glib-2.0/include
- /usr/lib/glib-2.0/include
- /usr/lib64/glib-2.0/include
- /usr/lib/i386-linux-gnu/glib-2.0/include
- /usr/lib/x86_64-linux-gnu/glib-2.0/include
- /opt/gnome/lib/glib-2.0/include
- C:/GTK/lib/glib-2.0/include
- /sw/lib/glib-2.0/include
- )
-
- FIND_PATH( GTK2_glib_INCLUDE_PATH glib.h
- $ENV{GTK2_DIR}/include/glib-2.0
- $ENV{GTK2_HOME}/include/glib-2.0
- /usr/include/glib-2.0
- /opt/gnome/include/glib-2.0
- C:/GTK/include/glib-2.0
- /sw/include/glib-2.0
- )
-
- FIND_PATH( GTK2_pango_INCLUDE_PATH pango/pango.h
- $ENV{GTK2_DIR}/include/pango-1.0
- $ENV{GTK2_HOME}/include/pango-1.0
- /usr/include/pango-1.0
- /opt/gnome/include/pango-1.0
- C:/GTK/include/pango-1.0
- /sw/include/pango-1.0
- )
-
- FIND_PATH( GTK2_cairo_INCLUDE_PATH cairo.h
- $ENV{GTK2_DIR}/include/cairo
- $ENV{GTK2_HOME}/include/cairo
- /usr/include/cairo
- /opt/gnome/include/cairo
- C:/GTK/include/cairo
- /sw/include/cairo
- )
-
- FIND_PATH( GTK2_gdkconfig_INCLUDE_PATH gdkconfig.h
- $ENV{GTK2_DIR}/lib/gtk-2.0/include
- $ENV{GTK2_HOME}/lib/gtk-2.0/include
- /usr/lib/gtk-2.0/include
- /usr/lib64/gtk-2.0/include
- /opt/gnome/lib/gtk-2.0/include
- C:/GTK/lib/gtk-2.0/include
- /sw/lib/gtk-2.0/include
- /usr/lib/i386-linux-gnu/gtk-2.0/include
- /usr/lib/x86_64-linux-gnu/gtk-2.0/include
- )
-
- FIND_PATH( GTK2_gdkpixbuf_INCLUDE_PATH gdk-pixbuf/gdk-pixbuf.h
- $ENV{GTK2_DIR}/gdk-pixbuf-2.0
- $ENV{GTK2_HOME}/gdk-pixbuf-2.0
- /usr/include/gdk-pixbuf-2.0
- /usr/include/gtk-2.0
- C:/GTK/include/gtk-2.0
- /sw/include/gtk-2.0
- )
-
- #MESSAGE("GTK2_gdkpixbuf_INCLUDE_PATH: ${GTK2_gdkpixbuf_INCLUDE_PATH}")
-
- FIND_PATH( GTK2_atk_INCLUDE_PATH atk/atk.h
- $ENV{GTK2_DIR}/include/atk-1.0
- $ENV{GTK2_HOME}/include/atk-1.0
- /usr/include/atk-1.0
- /opt/gnome/include/atk-1.0
- C:/GTK/include/atk-1.0
- /sw/include/atk-1.0
- )
-
- FIND_LIBRARY( GTK2_gtk_LIBRARY
- NAMES gtk-x11-2.0 gtk-win32-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /usr/local/lib
- /usr/openwin/lib
- /usr/X11R6/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- FIND_LIBRARY( GTK2_gdk_LIBRARY
- NAMES gdk-x11-2.0 gdk-win32-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /usr/local/lib
- /usr/openwin/lib
- /usr/X11R6/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- FIND_LIBRARY( GTK2_gmodule_LIBRARY
- NAMES gmodule-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /usr/local/lib
- /usr/openwin/lib
- /usr/X11R6/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- FIND_LIBRARY( GTK2_glib_LIBRARY
- NAMES glib-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /usr/local/lib
- /usr/openwin/lib
- /usr/X11R6/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- FIND_LIBRARY( GTK2_gthread_LIBRARY
- NAMES gthread-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /usr/local/lib
- /usr/openwin/lib
- /usr/X11R6/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- FIND_LIBRARY( GTK2_gobject_LIBRARY
- NAMES gobject-2.0
- PATHS $ENV{GTK2_DIR}/lib
- PATHS $ENV{GTK2_HOME}/lib
- /usr/lib
- /opt/gnome/lib
- C:/GTK/lib
- /sw/lib
- )
-
- IF(GTK2_gtk_INCLUDE_PATH)
- IF(GTK2_glib_INCLUDE_PATH)
- IF(GTK2_glibconfig_INCLUDE_PATH)
- IF(GTK2_gtk_LIBRARY)
- IF(GTK2_glib_LIBRARY)
- IF(GTK2_pango_INCLUDE_PATH)
- IF(GTK2_gdkconfig_INCLUDE_PATH)
- IF(GTK2_atk_INCLUDE_PATH)
- # Assume that if gtk and glib were found, the other
- # supporting libraries have also been found.
-
- SET( GTK2_FOUND "YES" )
- SET( GTK2_INCLUDE_DIRS ${GTK2_gtk_INCLUDE_PATH}
- ${GTK2_glib_INCLUDE_PATH}
- ${GTK2_glibconfig_INCLUDE_PATH}
- ${GTK2_pango_INCLUDE_PATH}
- ${GTK2_atk_INCLUDE_PATH}
- ${GTK2_gdkconfig_INCLUDE_PATH})
- IF(GTK2_cairo_INCLUDE_PATH)
- LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_cairo_INCLUDE_PATH} )
- ENDIF(GTK2_cairo_INCLUDE_PATH)
- IF(GTK2_gdkpixbuf_INCLUDE_PATH)
- LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_gdkpixbuf_INCLUDE_PATH} )
- ENDIF()
- IF(GTK2_gdkconfig_INCLUDE_PATH)
- LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_gdkconfig_INCLUDE_PATH} )
- ENDIF(GTK2_gdkconfig_INCLUDE_PATH)
-
- SET( GTK2_LIBRARIES ${GTK2_gtk_LIBRARY}
- ${GTK2_gdk_LIBRARY}
- ${GTK2_glib_LIBRARY}
- ${GTK2_gobject_LIBRARY})
-
- IF(GTK2_gmodule_LIBRARY)
- LIST(APPEND GTK2_LIBRARIES ${GTK2_gmodule_LIBRARY})
- ENDIF(GTK2_gmodule_LIBRARY)
- IF(GTK2_gthread_LIBRARY)
- LIST(APPEND GTK2_LIBRARIES ${GTK2_gthread_LIBRARY})
- ENDIF(GTK2_gthread_LIBRARY)
-
- ELSE(GTK2_atk_INCLUDE_PATH)
- MESSAGE("Can not find atk")
- ENDIF(GTK2_atk_INCLUDE_PATH)
- ELSE(GTK2_gdkconfig_INCLUDE_PATH)
- #MESSAGE("Can not find gdkconfig include")
- ENDIF(GTK2_gdkconfig_INCLUDE_PATH)
- ELSE(GTK2_pango_INCLUDE_PATH)
- #MESSAGE("Can not find pango includes")
- ENDIF(GTK2_pango_INCLUDE_PATH)
- ELSE(GTK2_glib_LIBRARY)
- #MESSAGE("Can not find glib lib")
- ENDIF(GTK2_glib_LIBRARY)
- ELSE(GTK2_gtk_LIBRARY)
- #MESSAGE("Can not find gtk lib")
- ENDIF(GTK2_gtk_LIBRARY)
- ELSE(GTK2_glibconfig_INCLUDE_PATH)
- #MESSAGE("Can not find glibconfig includes")
- ENDIF(GTK2_glibconfig_INCLUDE_PATH)
- ELSE(GTK2_glib_INCLUDE_PATH)
- #MESSAGE("Can not find glib includes")
- ENDIF(GTK2_glib_INCLUDE_PATH)
- ELSE(GTK2_gtk_INCLUDE_PATH)
- #MESSAGE("Can not find gtk includes")
- ENDIF(GTK2_gtk_INCLUDE_PATH)
-
- MARK_AS_ADVANCED(
- GTK2_gdk_LIBRARY
- GTK2_glib_INCLUDE_PATH
- GTK2_glib_LIBRARY
- GTK2_glibconfig_INCLUDE_PATH
- GTK2_gmodule_LIBRARY
- GTK2_gthread_LIBRARY
- GTK2_gtk_INCLUDE_PATH
- GTK2_gtk_LIBRARY
- GTK2_atk_INCLUDE_PATH
- GTK2_gdkconfig_INCLUDE_PATH
- GTK2_gobject_LIBRARY
- GTK2_pango_INCLUDE_PATH
- GTK2_cairo_INCLUDE_PATH
- GTK2_gdkpixbuf_INCLUDE_PATH
- )
-
-ELSE(UNIX OR WIN32)
- MESSAGE("FindGTK2 is working on UNIX/LINUX and Windows, only!")
-
-ENDIF(UNIX OR WIN32)
-
diff --git a/CMakeModules/FindMyJPEG.cmake b/CMakeModules/FindMyJPEG.cmake
deleted file mode 100644
index 8364005..0000000
--- a/CMakeModules/FindMyJPEG.cmake
+++ /dev/null
@@ -1,130 +0,0 @@
-#############################################################################
-#
-# $Id: FindMyJPEG.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libjpeg library.
-# Once run this will define:
-#
-# JPEG_FOUND
-# JPEG_INCLUDE_DIRS
-# JPEG_LIBRARIES
-#
-# Authors:
-# Nicolas Melchior
-#
-#############################################################################
-
-
-# detection of the Libjpeg headers location
-if(MINGW)
- find_path(JPEG_INCLUDE_DIR
- NAMES
- jpeglib.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- )
- # Detection of the Libjpeg library on Unix
- find_library(JPEG_LIBRARY
- NAMES
- jpeg libjpeg
- PATHS
- "$ENV{MINGW_DIR}/lib"
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-else()
- find_path(JPEG_INCLUDE_DIR
- NAMES
- jpeglib.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{LIBJPEG_DIR}/include
- $ENV{LIBJPEG_DIR}
- "C:/Program Files/GnuWin32/include"
- )
- # Detection of the Libjpeg library on Unix
- find_library(JPEG_LIBRARY
- NAMES
- jpeg libjpeg
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{LIBJPEG_DIR}/lib
- $ENV{LIBJPEG_DIR}/Release
- $ENV{LIBJPEG_DIR}
- "C:/Program Files/GnuWin32/lib"
- )
-endif()
- #MESSAGE("JPEG_INCLUDE_DIR=${JPEG_INCLUDE_DIR}")
- #MESSAGE("JPEG_LIBRARY=${JPEG_LIBRARY}")
-
-
- MARK_AS_ADVANCED(
- JPEG_LIBRARY
- JPEG_INCLUDE_DIR
- )
-
-## --------------------------------
-
-IF(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
- # The material is found. Check if it works on the requested architecture
- include(CheckCXXSourceCompiles)
-
- SET(CMAKE_REQUIRED_LIBRARIES ${JPEG_LIBRARY})
- SET(CMAKE_REQUIRED_INCLUDES ${JPEG_INCLUDE_DIR})
- CHECK_CXX_SOURCE_COMPILES("
- #include <stdio.h>
- #include <jpeglib.h> // Contrib for jpeg image io
- #include <jerror.h>
- int main()
- {
- struct jpeg_decompress_struct cinfo;
- struct jpeg_error_mgr jerr;
- cinfo.err = jpeg_std_error(&jerr);
- }
- " JPEG_BUILD_TEST)
- #MESSAGE("JPEG_BUILD_TEST: ${JPEG_BUILD_TEST}")
- IF(JPEG_BUILD_TEST)
- SET(JPEG_INCLUDE_DIRS ${JPEG_INCLUDE_DIR})
- SET(JPEG_LIBRARIES ${JPEG_LIBRARY})
- SET(JPEG_FOUND TRUE)
- ELSE()
- SET(JPEG_FOUND FALSE)
- #MESSAGE("libjpeg library found but not compatible with architecture.")
- ENDIF()
-
-ELSE(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
- SET(JPEG_FOUND FALSE)
-ENDIF(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
diff --git a/CMakeModules/FindMyPNG.cmake b/CMakeModules/FindMyPNG.cmake
deleted file mode 100644
index 1a70f44..0000000
--- a/CMakeModules/FindMyPNG.cmake
+++ /dev/null
@@ -1,192 +0,0 @@
-#############################################################################
-#
-# $Id: FindMyPNG.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libpng library.
-# To use the libpng library, the zlib library is required.
-# Once run this will define:
-#
-# PNG_FOUND
-# PNG_INCLUDE_DIR
-# PNG_LIBRARIES
-#
-# Authors:
-# Nicolas Melchior
-#
-#############################################################################
-
-
-# detection of the Libpng headers location
-if(MINGW)
- find_path(PNG_INCLUDE_DIR
- NAMES
- png.h
- PATHS
- "C:/mingw/include/libpng14"
- "$ENV{MINGW_DIR}/include/libpng14"
- )
-else()
- find_path(PNG_INCLUDE_DIR
- NAMES
- png.h
- PATHS
- "$ENV{LIBPNG_DIR}/include"
- "$ENV{LIBPNG_DIR}"
- "$ENV{LIBPNG_INCLUDE_DIR}"
- "/usr/include"
- "/usr/local/include"
- "C:/Program Files/libpng/include"
- )
-endif()
-#MESSAGE("PNG_INCLUDE_DIR=${PNG_INCLUDE_DIR}")
-
-if(UNIX)
- # Detection of the Libpng library on Unix
- find_library(PNG_LIBRARY
- NAMES
- png15 libpng15 libpng14 png12 libpng12 png libpng
- PATHS
- "$ENV{LIBPNG_DIR}/lib"
- "$ENV{LIBPNG_DIR}/Release"
- "$ENV{LIBPNG_DIR}"
- "$ENV{LIBPNG_LIBRARY_DIR}"
- /usr/lib
- /usr/local/lib
- /lib
- "C:/Program Files/libpng/lib"
- )
-elseif(MINGW)
- # Detection of the Libpng library on mingw
- find_library(PNG_LIBRARY
- NAMES
- png15 libpng15 libpng14 png12 libpng12 png libpng
- PATHS
- "C:/mingw/lib64"
- "$ENV{MINGW_DIR}/lib64"
- )
- #MESSAGE("PNG_LIBRARY=${PNG_LIBRARY}")
-else()
- FIND_LIBRARY(PNG_LIBRARY_RELEASE
- NAMES
- png15 libpng15 png12 libpng12 png libpng
- PATHS
- $ENV{LIBPNG_DIR}/lib
- $ENV{LIBPNG_DIR}/Release
- $ENV{LIBPNG_DIR}
- $ENV{LIBPNG_LIBRARY_RELEASE_DIR}
- /usr/lib
- /usr/local/lib
- /lib
- "C:/Program Files/libpng/lib"
- )
- FIND_LIBRARY(PNG_LIBRARY_DEBUG
- NAMES
- png15d libpng15d png12d libpng12d pngd libpngd
- PATHS
- $ENV{LIBPNG_DIR}/lib
- $ENV{LIBPNG_DIR}/Debug
- $ENV{LIBPNG_DIR}
- $ENV{LIBPNG_LIBRARY_DEBUG_DIR}
- /usr/lib
- /usr/local/lib
- /lib
- "C:/Program Files/libpng/lib"
- )
- #MESSAGE("PNG_LIBRARY_RELEASE=${PNG_LIBRARY_RELEASE}")
- #MESSAGE("PNG_LIBRARY_DEBUG=${PNG_LIBRARY_DEBUG}")
-ENDIF(UNIX)
-## --------------------------------
-
-SET(PNG_FOUND FALSE)
-
-FIND_PACKAGE(ZLIB)
-if(NOT ZLIB_FOUND)
- FIND_PACKAGE(MyZLIB)
-endif()
-
-IF(UNIX)
- IF(PNG_LIBRARY AND PNG_INCLUDE_DIR)
- SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
- SET(PNG_LIBRARIES ${PNG_LIBRARY})
- SET(PNG_FOUND TRUE)
- ENDIF()
-ELSE(UNIX)
- SET(PNG_LIBRARIES "")
- IF(PNG_LIBRARY_RELEASE AND PNG_INCLUDE_DIR)
- SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
- LIST(APPEND PNG_LIBRARIES optimized ${PNG_LIBRARY_RELEASE})
- SET(PNG_FOUND TRUE)
- ENDIF()
- IF(PNG_LIBRARY_DEBUG AND PNG_INCLUDE_DIR)
- SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
- LIST(APPEND PNG_LIBRARIES debug ${PNG_LIBRARY_DEBUG})
- SET(PNG_FOUND TRUE)
- ENDIF()
-ENDIF(UNIX)
-
-IF(ZLIB_FOUND)
- IF(PNG_FOUND)
- # The material is found. Check if it works on the requested architecture
- include(CheckCXXSourceCompiles)
-
- #MESSAGE(PNG_LIBRARIES: ${PNG_LIBRARIES})
- #MESSAGE(ZLIB_LIBRARIES: ${ZLIB_LIBRARIES})
-
- SET(CMAKE_REQUIRED_LIBRARIES ${PNG_LIBRARIES} ${ZLIB_LIBRARIES})
- SET(CMAKE_REQUIRED_INCLUDES ${PNG_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS})
- CHECK_CXX_SOURCE_COMPILES("
- #include <png.h> // Contrib for png image io
- int main()
- {
- /* create a png read struct */
- png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
- }
- " PNG_BUILD_TEST)
- #MESSAGE("PNG_BUILD_TEST: ${PNG_BUILD_TEST}")
- IF(PNG_BUILD_TEST)
- SET(PNG_FOUND TRUE)
- ELSE()
- SET(PNG_FOUND FALSE)
- MESSAGE("libpng library found but not compatible with architecture.")
- ENDIF()
- ELSEIF(PNG_FOUND)
- MESSAGE("To use libpng library, you should also install zlib library")
- ENDIF()
-ENDIF()
-
-
-MARK_AS_ADVANCED(
- PNG_LIBRARY
- PNG_LIBRARY_DEBUG
- PNG_LIBRARY_RELEASE
- PNG_INCLUDE_DIR
-)
diff --git a/CMakeModules/FindMyZLIB.cmake b/CMakeModules/FindMyZLIB.cmake
deleted file mode 100644
index 1d55ae6..0000000
--- a/CMakeModules/FindMyZLIB.cmake
+++ /dev/null
@@ -1,139 +0,0 @@
-#############################################################################
-#
-# $Id: FindMyZLIB.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find zlib library.
-# This file should be used only if FindZLIB.cmake provided with CMake
-# is not able to detect zlib.
-# The detection can be eased by setting ZLIB_HOME environment variable
-# especially under windows.
-#
-# ZLIB_FOUND
-# ZLIB_INCLUDE_DIRS
-# ZLIB_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-
-if(MINGW)
- find_path(ZLIB_INCLUDE_DIR zlib.h
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- )
-else()
- find_path(ZLIB_INCLUDE_DIR zlib.h
- $ENV{ZLIB_DIR}/include
- $ENV{ZLIB_INCLUDE_DIR}
- /usr/include
- /usr/local/include
- "C:/Program Files/zlib/include"
- )
-endif()
-
-if(UNIX)
- find_library(ZLIB_LIBRARY z zlib
- $ENV{ZLIB_DIR}/lib
- $ENV{ZLIB_LIBRARY_DIR}
- /lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/zlib/lib"
- )
- #MESSAGE("ZLIB_LIBRARY=${ZLIB_LIBRARY}")
- #MESSAGE("ZLIB_INCLUDE_DIRS=${ZLIB_INCLUDE_DIRS}")
-elseif(MINGW)
- find_library(ZLIB_LIBRARY z zlib
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-else()
- find_library(ZLIB_LIBRARY_RELEASE z zlib
- $ENV{ZLIB_DIR}/lib
- $ENV{ZLIB_LIBRARY_RELEASE_DIR}
- /lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/zlib/lib"
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-
- FIND_LIBRARY(ZLIB_LIBRARY_DEBUG zlibd
- $ENV{ZLIB_DIR}/lib
- $ENV{ZLIB_LIBRARY_DEBUG_DIR}
- /lib
- /usr/lib
- /usr/local/lib
- "C:/Program Files/zlib/lib"
- )
- #MESSAGE("ZLIB_LIBRARY_RELEASE=${ZLIB_LIBRARY_RELEASE}")
- #MESSAGE("ZLIB_LIBRARY_DEBUG=${ZLIB_LIBRARY_DEBUG}")
-endif()
-## --------------------------------
-
-
-IF(UNIX)
- IF(ZLIB_LIBRARY AND ZLIB_INCLUDE_DIR)
- SET(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
- SET(ZLIB_LIBRARIES ${ZLIB_LIBRARY})
- SET(ZLIB_FOUND TRUE)
- #MESSAGE("ZLIB_LIBRARIES=${ZLIB_LIBRARIES}")
- #MESSAGE("ZLIB_INCLUDE_DIRS=${ZLIB_INCLUDE_DIRS}")
- ELSE()
- SET(ZLIB_FOUND FALSE)
- ENDIF()
-ELSE(UNIX)
- SET(ZLIB_LIBRARIES "")
- IF(ZLIB_LIBRARY_RELEASE AND ZLIB_INCLUDE_DIR)
- SET(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
- LIST(APPEND ZLIB_LIBRARIES optimized)
- LIST(APPEND ZLIB_LIBRARIES ${ZLIB_LIBRARY_RELEASE})
- SET(ZLIB_FOUND TRUE)
- ENDIF()
- IF(ZLIB_LIBRARY_DEBUG AND ZLIB_INCLUDE_DIR)
- SET(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
- LIST(APPEND ZLIB_LIBRARIES debug)
- LIST(APPEND ZLIB_LIBRARIES ${ZLIB_LIBRARY_DEBUG})
- SET(ZLIB_FOUND TRUE)
- ENDIF()
-ENDIF(UNIX)
-
-MARK_AS_ADVANCED(
- ZLIB_INCLUDE_DIR
- ZLIB_LIBRARY
- ZLIB_LIBRARY_RELEASE
- ZLIB_LIBRARY_DEBUG
- )
-
-
-
-
diff --git a/CMakeModules/FindNAS.cmake b/CMakeModules/FindNAS.cmake
deleted file mode 100644
index 4e927af..0000000
--- a/CMakeModules/FindNAS.cmake
+++ /dev/null
@@ -1,63 +0,0 @@
-#############################################################################
-#
-# $Id: FindNAS.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to determine if Inria's NAS server hosting /udd/ is available
-#
-# NAS_FOUND
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-FIND_FILE(AFMA6_CONSTANT_FILE_ON_NAS
- NAMES const_Afma6.cnf
- PATHS
- /udd/fspindle/robot/Afma6/current/include
- Z:/robot/Afma6/current/include
- )
-
-#MESSAGE("DBG AFMA6_CONSTANT_FILE_ON_NAS=${AFMA6_CONSTANT_FILE_ON_NAS}")
-
-## --------------------------------
-
-IF(AFMA6_CONSTANT_FILE_ON_NAS)
- SET(NAS_FOUND TRUE)
-ELSE(AFMA6_CONSTANT_FILE_ON_NAS)
- SET(NAS_FOUND FALSE)
-ENDIF(AFMA6_CONSTANT_FILE_ON_NAS)
-
-MARK_AS_ADVANCED(
- AFMA6_CONSTANT_FILE_ON_NAS
-)
diff --git a/CMakeModules/FindPARPORT.cmake b/CMakeModules/FindPARPORT.cmake
deleted file mode 100644
index 8450d1e..0000000
--- a/CMakeModules/FindPARPORT.cmake
+++ /dev/null
@@ -1,58 +0,0 @@
-#############################################################################
-#
-# $Id: FindPARPORT.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find linux/parport.h.h for parallel port usage.
-# Once run this will define:
-#
-# PARPORT_FOUND
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-FIND_PATH(PARPORT_INCLUDE_DIR linux/parport.h
- /usr/include
- /usr/src/linux/include)
- #MESSAGE("DBG PARPORT_INCLUDE_DIR=${PARPORT_INCLUDE_DIR}")
-
-MARK_AS_ADVANCED(
- PARPORT_INCLUDE_DIR
-)
-
-IF(PARPORT_INCLUDE_DIR)
- SET(PARPORT_FOUND TRUE)
-ELSE(PARPORT_INCLUDE_DIR)
- SET(PARPORT_FOUND FALSE)
-ENDIF(PARPORT_INCLUDE_DIR)
-
diff --git a/CMakeModules/FindPTHREAD.cmake b/CMakeModules/FindPTHREAD.cmake
deleted file mode 100644
index 683d7bf..0000000
--- a/CMakeModules/FindPTHREAD.cmake
+++ /dev/null
@@ -1,104 +0,0 @@
-#############################################################################
-#
-# $Id: FindPTHREAD.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find pthread library.
-# Once run this will define:
-#
-# PTHREAD_FOUND
-# PTHREAD_INCLUDE_DIRS
-# PTHREAD_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-if(MINGW)
- find_path(PTHREAD_INCLUDE_DIR pthread.h
- "$ENV{MINGW_DIR}/include"
- "$ENV{MINGW_DIR}/mingw/include"
- C:/mingw/mingw/include
- )
-
- # pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2 are comming from web
- find_library(PTHREAD_LIBRARY
- NAMES pthread pthreadGC2 pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2
- PATHS
- "$ENV{MINGW_DIR}/lib"
- "$ENV{MINGW_DIR}/mingw/lib"
- C:/mingw/mingw/lib
- )
-else()
- find_path(PTHREAD_INCLUDE_DIR pthread.h
- "$ENV{PTHREAD_HOME}/include"
- "$ENV{PTHREAD_DIR}/include"
- /usr/include
- )
- # pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2 are comming from web
- find_library(PTHREAD_LIBRARY
- NAMES pthread pthreadGC2 pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2
- PATHS
- "$ENV{PTHREAD_HOME}/lib"
- "$ENV{PTHREAD_DIR}/lib"
- /usr/lib
- /usr/local/lib
- /lib
- )
-endif()
- #MESSAGE("DBG PTHREAD_INCLUDE_DIR=${PTHREAD_INCLUDE_DIR}")
- #MESSAGE(STATUS "DBG PTHREAD_LIBRARY=${PTHREAD_LIBRARY}")
-
- ## --------------------------------
-
- IF(PTHREAD_LIBRARY)
- SET(PTHREAD_LIBRARIES ${PTHREAD_LIBRARY})
- ELSE(PTHREAD_LIBRARY)
- #MESSAGE(SEND_ERROR "pthread library not found.")
- ENDIF(PTHREAD_LIBRARY)
-
- IF(NOT PTHREAD_INCLUDE_DIR)
- #MESSAGE(SEND_ERROR "pthread include dir not found.")
- ENDIF(NOT PTHREAD_INCLUDE_DIR)
-
- IF(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
- SET(PTHREAD_INCLUDE_DIRS ${PTHREAD_INCLUDE_DIR})
- SET(PTHREAD_FOUND TRUE)
- ELSE(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
- SET(PTHREAD_FOUND FALSE)
- ENDIF(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- PTHREAD_INCLUDE_DIR
- PTHREAD_LIBRARY
- )
- #MESSAGE(STATUS "PTHREAD_FOUND : ${PTHREAD_FOUND}")
diff --git a/CMakeModules/FindPTU46.cmake b/CMakeModules/FindPTU46.cmake
deleted file mode 100644
index 46fdae3..0000000
--- a/CMakeModules/FindPTU46.cmake
+++ /dev/null
@@ -1,127 +0,0 @@
-#############################################################################
-#
-# $Id: FindPTU46.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libraries for Irisa's Ptu-46 pan-tilt head.
-# Once run this will define:
-#
-# PTU46_FOUND
-# PTU46_INCLUDE_DIRS
-# PTU46_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindPTU46.cmake: Ptu-46 only available for Unix.")
- SET(PTU46_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_PATH(PTU46_INCLUDE_DIR ptu.h
- $ENV{PTU46_HOME}/include
- /udd/fspindle/robot/Ptu-Evi/current/include
- /local/soft/Ptu-Evi/current/include
- )
- #MESSAGE("DBG PTU46_INCLUDE_DIR=${PTU46_INCLUDE_DIR}")
-
- FIND_LIBRARY(PTUPTU46_LIBRARY
- NAMES ptu
- PATHS
- $ENV{PTU46_HOME}/lib
- /udd/fspindle/robot/Ptu-Evi/current/lib
- /local/soft/Ptu-Evi/current/lib
- )
- FIND_LIBRARY(EVIPTU46_LIBRARY
- NAMES evi
- PATHS
- $ENV{PTU46_HOME}/lib
- /udd/fspindle/robot/Ptu-Evi/current/lib
- /local/soft/Ptu-Evi/current/lib
- )
- FIND_LIBRARY(SERIALPTU46_LIBRARY
- NAMES serial
- PATHS
- $ENV{PTU46_HOME}/lib
- /udd/fspindle/robot/Ptu-Evi/current/lib
- /local/soft/Ptu-Evi/current/lib
- )
- #MESSAGE("DBG PTU46_LIBRARY=${PTU46_LIBRARY}")
-
- ## --------------------------------
-
- IF(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
- # The material is found. Check if it works on the requested architecture
- include(CheckCXXSourceCompiles)
-
- SET(CMAKE_REQUIRED_LIBRARIES ${PTUPTU46_LIBRARY} ${SERIALPTU46_LIBRARY})
- SET(CMAKE_REQUIRED_INCLUDES ${PTU46_INCLUDE_DIR})
- CHECK_CXX_SOURCE_COMPILES("
- #include <ptu.h> // Contrib for Ptu-46 robot
- int main()
- {
- Ptu ptu;
- return 0;
- }
- " PTU46_BUILD_TEST)
- #MESSAGE("PTU46_BUILD_TEST: ${PTU46_BUILD_TEST}")
- IF(PTU46_BUILD_TEST)
- SET(PTU46_LIBRARIES ${PTUPTU46_LIBRARY} ${EVIPTU46_LIBRARY}
- ${SERIALPTU46_LIBRARY})
-# ELSE()
-# MESSAGE("Ptu-46 library found but not compatible with architecture.")
- ENDIF()
-
- ELSE(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
-# MESSAGE(SEND_ERROR "Ptu-46 library not found.")
- ENDIF(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
-
- IF(NOT PTU46_INCLUDE_DIR)
-# MESSAGE(SEND_ERROR "Ptu-46 include dir not found.")
- ENDIF(NOT PTU46_INCLUDE_DIR)
-
- IF(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
- SET(PTU46_INCLUDE_DIRS ${PTU46_INCLUDE_DIR})
- SET(PTU46_FOUND TRUE)
- ELSE(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
- SET(PTU46_FOUND FALSE)
- ENDIF(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- PTU46_INCLUDE_DIR
- PTU46_LIBRARIES
- PTUPTU46_LIBRARY
- EVIPTU46_LIBRARY
- SERIALPTU46_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindRAW1394.cmake b/CMakeModules/FindRAW1394.cmake
deleted file mode 100644
index db33295..0000000
--- a/CMakeModules/FindRAW1394.cmake
+++ /dev/null
@@ -1,88 +0,0 @@
-#############################################################################
-#
-# $Id: FindRAW1394.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find libraw1394 for IEEE1394 camera under Linux.
-# Once run this will define:
-#
-# RAW1394_FOUND
-# RAW1394_INCLUDE_DIRS
-# RAW1394_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindRAW1394.cmake: libraw1394 only available for Unix.")
- SET(RAW1394_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_PATH(RAW1394_INCLUDE_DIR libraw1394/raw1394.h
- $ENV{RAW1394_HOME}/include
- /usr/include )
- #MESSAGE("DBG RAW1394_INCLUDE_DIR=${RAW1394_INCLUDE_DIR}")
-
- FIND_LIBRARY(RAW1394_LIBRARY
- NAMES raw1394
- PATHS
- $ENV{RAW1394_HOME}/lib
- /usr/lib
- )
- #MESSAGE("DBG RAW1394_LIBRARY=${RAW1394_LIBRARY}")
-
- ## --------------------------------
-
- IF(RAW1394_LIBRARY)
- SET(RAW1394_LIBRARIES ${RAW1394_LIBRARY})
- ELSE(RAW1394_LIBRARY)
- #MESSAGE("libraw1394 library not found.")
- ENDIF(RAW1394_LIBRARY)
-
- IF(NOT RAW1394_INCLUDE_DIR)
- #MESSAGE("libraw1394 include dir not found.")
- ENDIF(NOT RAW1394_INCLUDE_DIR)
-
- IF(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
- SET(RAW1394_INCLUDE_DIRS ${RAW1394_INCLUDE_DIR})
- SET(RAW1394_FOUND TRUE)
- ELSE(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
- SET(RAW1394_FOUND FALSE)
- ENDIF(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
-
- MARK_AS_ADVANCED(
- RAW1394_INCLUDE_DIR
- RAW1394_LIBRARIES
- RAW1394_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindRT.cmake b/CMakeModules/FindRT.cmake
deleted file mode 100644
index 4554ea7..0000000
--- a/CMakeModules/FindRT.cmake
+++ /dev/null
@@ -1,73 +0,0 @@
-#############################################################################
-#
-# $Id: FindRT.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find RT library material
-#
-# RT_FOUND
-# RT_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindRT.cmake: only available for Unix.")
- SET(RT_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_LIBRARY(RT_LIBRARY
- NAMES rt
- PATHS
- $ENV{RT_HOME}/lib
- /usr/lib
- /usr/local/lib
- )
-
- #MESSAGE("DBG RT_LIBRARY=${RT_LIBRARY}")
-
- ## --------------------------------
-
- IF(RT_LIBRARY)
- SET(RT_LIBRARIES ${RT_LIBRARY})
- SET(RT_FOUND TRUE)
- ELSE(RT_LIBRARY)
- SET(RT_FOUND FALSE)
- #MESSAGE(SEND_ERROR "rt library not found.")
- ENDIF(RT_LIBRARY)
-
- MARK_AS_ADVANCED(
- RT_LIBRARIES
- RT_LIBRARY
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindSOQT.cmake b/CMakeModules/FindSOQT.cmake
deleted file mode 100644
index ad7d73c..0000000
--- a/CMakeModules/FindSOQT.cmake
+++ /dev/null
@@ -1,129 +0,0 @@
-#############################################################################
-#
-# $Id: FindSOQT.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find SoQt library.
-# Once run this will define:
-#
-# SOQT_FOUND
-# SOQT_INCLUDE_DIRS
-# SOQT_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-set(SOQT_FOUND FALSE)
-
-# detection of the SoQt headers location
-FIND_PATH(SOQT_INCLUDE_DIR Inventor/Qt/SoQt.h
- "$ENV{COIN_DIR}/include"
- "$ENV{SOQT_DIR}/include"
- "$ENV{COIN_DIR}"
- "$ENV{SOQT_DIR}"
- "$ENV{COINDIR}/include"
- "$ENV{SOQTDIR}/include"
- /usr/include/Coin2
- /Library/Frameworks/SoQt.framework/Headers
- )
-#MESSAGE("DBG SOQT_INCLUDE_DIR=${SOQT_INCLUDE_DIR}")
-
-IF(WIN32)
- # Detection of the SoQt library on Windows
- FIND_LIBRARY(SOQT_LIBRARY_RELEASE
- NAMES soqt1 #only shared libraries under windows
- PATHS
- "$ENV{COIN_DIR}/lib"
- "$ENV{SOQT_DIR}/lib"
- "$ENV{COINDIR}/lib"
- "$ENV{SOQTDIR}/lib"
- )
- FIND_LIBRARY(SOQT_LIBRARY_DEBUG
- NAMES soqt1d #only shared libraries under windows
- PATHS
- "$ENV{COIN_DIR}/lib"
- "$ENV{SOQT_DIR}/lib"
- "$ENV{COINDIR}/lib"
- "$ENV{SOQTDIR}/lib"
- )
-
- if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY_RELEASE)
- set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
- list(APPEND SOQT_LIBRARIES optimized ${SOQT_LIBRARY_RELEASE})
- set(SOQT_FOUND TRUE)
- endif()
-
- if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY_DEBUG)
- set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
- list(APPEND SOQT_LIBRARIES debug ${SOQT_LIBRARY_DEBUG})
- set(SOQT_FOUND TRUE)
- endif()
-
- MARK_AS_ADVANCED(
- SOQT_LIBRARIES
- SOQT_LIBRARY_DEBUG
- SOQT_LIBRARY_RELEASE
- SOQT_INCLUDE_DIR
- )
-
-ELSE(WIN32)
- # Detection of the SoQt library on Unix
- FIND_LIBRARY(SOQT_LIBRARY
- NAMES SoQt
- PATHS
- "$ENV{COIN_DIR}/lib"
- "$ENV{SOQT_DIR}/lib"
- "$ENV{COINDIR}/lib"
- "$ENV{SOQTDIR}/lib"
- /usr/lib
- /usr/local/lib
- /lib
- /Library/Frameworks/SoQt.framework/Libraries
- )
-
- if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY)
- set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
- set(SOQT_LIBRARIES ${SOQT_LIBRARY})
- set(SOQT_FOUND TRUE)
- endif()
-
- MARK_AS_ADVANCED(
- SOQT_LIBRARIES
- SOQT_LIBRARY
- SOQT_INCLUDE_DIR
- )
- #MESSAGE(STATUS "DBG SOQT_LIBRARY=${SOQT_LIBRARY}")
-
-ENDIF(WIN32)
-
-#MESSAGE(STATUS "SOQT_FOUND : ${SOQT_FOUND}")
diff --git a/CMakeModules/FindSOWIN.cmake b/CMakeModules/FindSOWIN.cmake
deleted file mode 100644
index 632fb72..0000000
--- a/CMakeModules/FindSOWIN.cmake
+++ /dev/null
@@ -1,95 +0,0 @@
-#############################################################################
-#
-# $Id: FindSOWIN.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find SoQt library.
-# Once run this will define:
-#
-# SOWIN_FOUND
-# SOWIN_INCLUDE_DIRS
-# SOWIN_LIBRARIES
-# SOWIN_LIBRARY_RELEASE
-# SOWIN_LIBRARY_DEBUG
-#
-# Authors:
-# Fabien Spindler
-# Anthony Saunier
-#
-#############################################################################
-
-set(SOWIN_FOUND FALSE)
-
-if(WIN32)
- FIND_LIBRARY(SOWIN_LIBRARY_RELEASE
- NAMES sowin1 #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{SOWIN_DIR}/lib"
- )
- FIND_LIBRARY(SOWIN_LIBRARY_DEBUG
- NAMES sowin1d #only shared libraries under windows
- PATHS
- "$ENV{COINDIR}/lib"
- "$ENV{COIN_DIR}/lib"
- "$ENV{SOWIN_DIR}/lib"
- )
-
- FIND_PATH(SOWIN_INCLUDE_DIR Inventor/Win/SoWin.h
- "$ENV{COINDIR}/include"
- "$ENV{COIN_DIR}/include"
- "$ENV{SOWIN_DIR}/include"
- )
-
- ## --------------------------------
- set(SOWIN_LIBRARIES "")
-
- if(SOWIN_LIBRARY_DEBUG)
- list(APPEND SOWIN_LIBRARIES debug ${SOWIN_LIBRARY_DEBUG})
- endif()
- if(SOWIN_LIBRARY_RELEASE)
- list(APPEND SOWIN_LIBRARIES optimized ${SOWIN_LIBRARY_RELEASE})
- endif()
- IF(SOWIN_LIBRARIES AND SOWIN_INCLUDE_DIR)
- set(SOWIN_INCLUDE_DIRS ${SOWIN_INCLUDE_DIR})
- set(SOWIN_FOUND TRUE)
- else()
- set(SOWIN_FOUND FALSE)
- endif()
-
-endif(WIN32)
-
-MARK_AS_ADVANCED(
- SOWIN_LIBRARY_DEBUG
- SOWIN_LIBRARY_RELEASE
- SOWIN_INCLUDE_DIR
-)
diff --git a/CMakeModules/FindSOXT.cmake b/CMakeModules/FindSOXT.cmake
deleted file mode 100644
index 2d17830..0000000
--- a/CMakeModules/FindSOXT.cmake
+++ /dev/null
@@ -1,79 +0,0 @@
-#############################################################################
-#
-# $Id: FindSOXT.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find SoXt library.
-# Once run this will define:
-#
-# SOXT_FOUND
-# SOXT_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(UNIX)
-
-FIND_LIBRARY(SOXT_LIBRARY
- NAMES SoXt
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- "$ENV{COIN_DIR}/lib"
- "$ENV{COINDIR}/lib"
- )
-MARK_AS_ADVANCED(
- SOXT_LIBRARY
- )
- #MESSAGE(STATUS "DBG SOXT_LIBRARY=${SOXT_LIBRARY}")
-
- ## --------------------------------
-
- IF(SOXT_LIBRARY)
-
- SET(SOXT_LIBRARIES ${SOXT_LIBRARY})
- SET(SOXT_FOUND TRUE)
- MARK_AS_ADVANCED(
- SOXT_LIBRARIES
- )
- ELSE(SOXT_LIBRARY)
- SET(SOXT_FOUND FALSE)
- #MESSAGE("SoXt library not found.")
- ENDIF(SOXT_LIBRARY)
-
- #MESSAGE(STATUS "SOXT_FOUND : ${SOXT_FOUND}")
-
-ELSE(UNIX)
- SET(SOXT_FOUND FALSE)
-ENDIF(UNIX)
diff --git a/CMakeModules/FindV4L2.cmake b/CMakeModules/FindV4L2.cmake
deleted file mode 100644
index fe068c3..0000000
--- a/CMakeModules/FindV4L2.cmake
+++ /dev/null
@@ -1,106 +0,0 @@
-#############################################################################
-#
-# $Id: FindV4L2.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find linux/videodev.h for Video For Linux Two framegrabbing
-# capabilities.
-# Once run this will define:
-#
-# V4L2_FOUND
-# V4L2_INCLUDE_DIRS
-# V4L2_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT UNIX)
- # MESSAGE("FindV4L2.cmake: only available for Unix.")
- SET(V4L2_FOUND FALSE)
-ELSE(NOT UNIX)
-
- FIND_PATH(V4L2_INCLUDE_VIDEODEV2 linux/videodev2.h
- $ENV{V4L2_HOME}/include
- $ENV{V4L2_DIR}/include
- /usr/include
- /usr/local/include
- /usr/src/linux/include
- )
- #MESSAGE("DBG V4L2_INCLUDE_VIDEODEV2=${V4L2_INCLUDE_VIDEODEV2}")
-
- FIND_PATH(V4L2_INCLUDE_LIBV4L2 libv4l2.h
- $ENV{V4L2_HOME}/include
- $ENV{V4L2_DIR}/include
- /usr/include
- /usr/local/include
- )
- #MESSAGE("DBG V4L2_INCLUDE_LIBV4L2=${V4L2_INCLUDE_LIBV4L2}")
-
- FIND_LIBRARY(V4L2_LIBRARY_LIBV4L2
- NAMES v4l2
- PATHS
- $ENV{V4L2_HOME}/lib
- $ENV{V4L2_DIR}/lib
- /usr/lib
- /usr/local/lib
- )
-
- FIND_LIBRARY(V4L2_LIBRARY_LIBV4LCONVERT
- NAMES v4lconvert
- PATHS
- $ENV{V4L2_HOME}/lib
- $ENV{V4L2_DIR}/lib
- /usr/lib
- /usr/local/lib
- )
-
-
- ## --------------------------------
-
-
- IF(V4L2_INCLUDE_VIDEODEV2 AND V4L2_INCLUDE_LIBV4L2 AND V4L2_LIBRARY_LIBV4L2 AND V4L2_LIBRARY_LIBV4LCONVERT)
- SET(V4L2_INCLUDE_DIRS ${V4L2_INCLUDE_VIDEODEV2} ${V4L2_INCLUDE_LIBV4L2})
- SET(V4L2_LIBRARIES ${V4L2_LIBRARY_LIBV4L2} ${V4L2_LIBRARY_LIBV4LCONVERT})
- SET(V4L2_FOUND TRUE)
- ELSE()
- SET(V4L2_FOUND FALSE)
- ENDIF()
-
- MARK_AS_ADVANCED(
- V4L2_INCLUDE_DIRS
- V4L2_INCLUDE_VIDEODEV2
- V4L2_INCLUDE_LIBV4L2
- V4L2_LIBRARY_LIBV4L2
- V4L2_LIBRARY_LIBV4LCONVERT
- )
-ENDIF(NOT UNIX)
diff --git a/CMakeModules/FindXML2.cmake b/CMakeModules/FindXML2.cmake
deleted file mode 100755
index d03d955..0000000
--- a/CMakeModules/FindXML2.cmake
+++ /dev/null
@@ -1,123 +0,0 @@
-#############################################################################
-#
-# $Id: FindXML2.cmake 5316 2015-02-12 10:58:18Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find XML library.
-# Once run this will define:
-#
-# XML2_FOUND
-# XML2_INCLUDE_DIRS
-# XML2_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-if(WIN32)
- find_package(ICONV)
- if(MINGW)
- find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
- "$ENV{MINGW_DIR}/include/libxml2"
- C:/mingw/include/libxml2
- )
- find_library(XML2_LIBRARY libxml2
- "$ENV{MINGW_DIR}/lib"
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
- else()
- find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
- "$ENV{XML2_DIR}/include"
- "$ENV{XML2_DIR}/include/libxml2"
- "$ENV{XML2_HOME}/include"
- "$ENV{XML2_HOME}/include/libxml2"
- )
- find_library(XML2_LIBRARY libxml2
- "$ENV{XML2_DIR}/lib"
- "$ENV{XML2_HOME}/lib"
- /usr/lib
- /usr/local/lib
- "c:/libxml2/lib"
- )
- endif()
-else(WIN32)
- find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
- "$ENV{XML2_DIR}/include/libxml2"
- "$ENV{XML2_HOME}/include/libxml2"
- /usr/include/libxml2
- /usr/local/include/libxml2
- )
- find_library(XML2_LIBRARY xml2
- "$ENV{XML2_DIR}/lib"
- "$ENV{XML2_HOME}/lib"
- /usr/lib
- /usr/local/lib
- )
-endif(WIN32)
-#MESSAGE("DBG XML2_INCLUDE_DIR=${XML2_INCLUDE_DIR}")
-
-
-#MESSAGE("DBG XML2_LIBRARY=${XML2_LIBRARY}")
-
-## --------------------------------
-
-IF(NOT XML2_INCLUDE_DIR)
- #MESSAGE(SEND_ERROR "xml include dir not found.")
-ENDIF(NOT XML2_INCLUDE_DIR)
-
-IF(XML2_LIBRARY)
- SET(XML2_LIBRARIES ${XML2_LIBRARY})
-ELSE(XML2_LIBRARY)
- #MESSAGE(SEND_ERROR "xml library not found.")
-ENDIF(XML2_LIBRARY)
-
-
-IF(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
- SET(XML2_INCLUDE_DIRS ${XML2_INCLUDE_DIR})
- SET(XML2_FOUND TRUE)
-
- IF(WIN32 AND ICONV_FOUND)
- LIST(APPEND XML2_INCLUDE_DIRS ${ICONV_INCLUDE_DIRS})
- SET(XML2_LIBRARIES ${XML2_LIBRARIES} ${ICONV_LIBRARIES})
- ENDIF()
-
-ELSE(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
- SET(XML2_FOUND FALSE)
-ENDIF(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
-
-MARK_AS_ADVANCED(
- XML2_INCLUDE_DIR
- XML2_LIBRARIES
- XML2_LIBRARY
- )
diff --git a/CMakeModules/FindZBAR.cmake b/CMakeModules/FindZBAR.cmake
deleted file mode 100644
index 21c31be..0000000
--- a/CMakeModules/FindZBAR.cmake
+++ /dev/null
@@ -1,71 +0,0 @@
-#############################################################################
-#
-# $Id: FindV4L2.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Try to find zbar library.
-# Once run this will define:
-#
-# ZBAR_FOUND
-# ZBAR_INCLUDE_DIRS
-# ZBAR_LIBRARIES
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-find_path(ZBAR_INCLUDE_DIRS zbar.h
- $ENV{ZBAR_DIR}/include
- /usr/include
- /usr/local/include
-)
-
-find_library(ZBAR_LIBRARIES
- NAMES zbar
- PATHS
- $ENV{ZBAR_DIR}/lib
- /usr/lib
- /usr/local/lib
-)
-
-if(ZBAR_INCLUDE_DIRS AND ZBAR_LIBRARIES)
- set(ZBAR_FOUND TRUE)
-else()
- set(ZBAR_FOUND FALSE)
-endif()
-
-mark_as_advanced(
- ZBAR_INCLUDE_DIRS
- ZBAR_LIBRARIES
-)
-
diff --git a/CMakeModules/OgreTools.cmake b/CMakeModules/OgreTools.cmake
deleted file mode 100755
index fd714de..0000000
--- a/CMakeModules/OgreTools.cmake
+++ /dev/null
@@ -1,174 +0,0 @@
-#############################################################################
-#
-# $Id: OgreTools.cmake 4769 2014-07-08 20:25:50Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file. Some useful tools for Ogre3D.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-#########################################################
-# Find Ogre plugins
-#
-# This is a modified version of the macro provided with Ogre
-# except that it should be used only in a desperate way when the original
-# one doesn't detect anything
-#########################################################
-
-macro(ogre_find_plugin_lib_visp PLUGIN)
- # On Unix, the plugins might have no prefix
- if (CMAKE_FIND_LIBRARY_PREFIXES)
- set(TMP_CMAKE_LIB_PREFIX ${CMAKE_FIND_LIBRARY_PREFIXES})
- set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} "")
- endif()
-
- # strip RenderSystem_ or Plugin_ prefix from plugin name
- string(REPLACE "RenderSystem_" "" PLUGIN_TEMP ${PLUGIN})
- string(REPLACE "Plugin_" "" PLUGIN_NAME ${PLUGIN_TEMP})
-
- set(OGRE_PLUGIN_PATH_SUFFIXES
- PlugIns PlugIns/${PLUGIN_NAME} Plugins Plugins/${PLUGIN_NAME} ${PLUGIN}
- RenderSystems RenderSystems/${PLUGIN_NAME} ${ARGN})
- # find link libraries for plugins
- set(OGRE_${PLUGIN}_LIBRARY_NAMES "${PLUGIN}${OGRE_LIB_SUFFIX}")
- get_debug_names(OGRE_${PLUGIN}_LIBRARY_NAMES)
- find_library(OGRE_${PLUGIN}_LIBRARY_REL NAMES ${OGRE_${PLUGIN}_LIBRARY_NAMES}
- HINTS ${OGRE_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}/OGRE ${OGRE_LIBRARY_DIRS}/OGRE-${OGRE_VERSION_MAJOR}.${OGRE_VERSION_MINOR}.${OGRE_VERSION_PATCH}
- PATH_SUFFIXES "" OGRE opt release release/opt relwithdebinfo relwithdebinfo/opt minsizerel minsizerel/opt)
- find_library(OGRE_${PLUGIN}_LIBRARY_DBG NAMES ${OGRE_${PLUGIN}_LIBRARY_NAMES_DBG}
- HINTS ${OGRE_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}/OGRE ${OGRE_LIBRARY_DIRS}/OGRE-${OGRE_VERSION_MAJOR}.${OGRE_VERSION_MINOR}.${OGRE_VERSION_PATCH}
- PATH_SUFFIXES "" OGRE opt debug debug/opt)
- make_library_set(OGRE_${PLUGIN}_LIBRARY)
-
- if (OGRE_${PLUGIN}_LIBRARY)
- set(OGRE_${PLUGIN}_FOUND TRUE)
- endif ()
-
- mark_as_advanced(OGRE_${PLUGIN}_LIBRARY_REL OGRE_${PLUGIN}_LIBRARY_DBG OGRE_${PLUGIN}_LIBRARY_FWK)
-
-endmacro(ogre_find_plugin_lib_visp)
-
-MACRO(CREATE_OGRE_PLUGIN_CONFIG_FILE)
- SET(VISP_HAVE_OGRE_PLUGINS_PATH ${VISP_BINARY_DIR}/data/ogre-simulator)
-
- # If OGRE_PLUGIN_DIR_REL and OGRE_PLUGIN_DIR_DBG are not defined we
- # try to find them manually
- IF(NOT OGRE_PLUGIN_DIR_REL AND NOT OGRE_PLUGIN_DIR_DBG)
- ogre_find_plugin_lib_visp(RenderSystem_Direct3D9)
- ogre_find_plugin_lib_visp(RenderSystem_Direct3D10)
- ogre_find_plugin_lib_visp(RenderSystem_Direct3D11)
- ogre_find_plugin_lib_visp(RenderSystem_GL)
- ogre_find_plugin_lib_visp(RenderSystem_GLES)
- ogre_find_plugin_lib_visp(Plugin_ParticleFX)
- ogre_find_plugin_lib_visp(Plugin_BSPSceneManager)
- ogre_find_plugin_lib_visp(Plugin_CgProgramManager)
- ogre_find_plugin_lib_visp(Plugin_PCZSceneManager)
- ogre_find_plugin_lib_visp(Plugin_OctreeSceneManager)
- ogre_find_plugin_lib_visp(Plugin_OctreeZone)
-
-
- IF(OGRE_RenderSystem_GL_LIBRARY_REL)
- GET_FILENAME_COMPONENT(OGRE_PLUGIN_DIR_REL ${OGRE_RenderSystem_GL_LIBRARY_REL} PATH)
- #message("set manually OGRE_PLUGIN_DIR_REL to ${OGRE_PLUGIN_DIR_REL}")
- ELSEIF(OGRE_RenderSystem_GL_LIBRARY_DBG)
- GET_FILENAME_COMPONENT(OGRE_PLUGIN_DIR_DBG ${OGRE_RenderSystem_GL_LIBRARY_DBG} PATH)
- #message("set manually OGRE_PLUGIN_DIR_DBG to ${OGRE_PLUGIN_DIR_DBG}")
- ENDIF()
- ENDIF()
-
- IF(OGRE_PLUGIN_DIR_REL)
- LIST(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D9_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D10_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D11_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_RenderSystem_GL_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_RenderSystem_GLES_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_ParticleFX_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_BSPSceneManager_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_CgProgramManager_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_PCZSceneManager_LIBRARY_REL})
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_OctreeSceneManager_LIBRARY_REL})
- IF (NOT APPLE)
- # Since the plugin Plugin_Octree causes problems on OSX, we take
- # it only into account on non Apple platforms
- LIST(APPEND PLUGIN_REL ${OGRE_Plugin_OctreeZone_LIBRARY_REL})
- ENDIF()
-
- SET(PLUGINS_CONTENT_REL "# Defines plugins to load\n\n")
- LIST(APPEND PLUGINS_CONTENT_REL "# Define plugin folder\n")
-
- LIST(APPEND PLUGINS_CONTENT_REL "PluginFolder=${OGRE_PLUGIN_DIR_REL}/\n\n")
- LIST(APPEND PLUGINS_CONTENT_REL "# Define plugins\n")
- foreach(PLUGIN ${PLUGIN_REL})
- if(PLUGIN)
- GET_FILENAME_COMPONENT(PLUGIN_NAME ${PLUGIN} NAME_WE)
- LIST(APPEND PLUGINS_CONTENT_REL " Plugin=${PLUGIN_NAME}\n")
- endif()
- endforeach()
- #MESSAGE("PLUGINS_CONTENT_REL: ${PLUGINS_CONTENT_REL}")
- FILE(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg" ${PLUGINS_CONTENT_REL})
- ENDIF()
-
- IF(OGRE_PLUGIN_DIR_DBG)
- LIST(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D9_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D10_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D11_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_RenderSystem_GL_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_RenderSystem_GLES_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_ParticleFX_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_BSPSceneManager_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_CgProgramManager_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_PCZSceneManager_LIBRARY_DBG})
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_OctreeSceneManager_LIBRARY_DBG})
- IF (NOT APPLE)
- # Since the plugin Plugin_Octree causes problems on OSX, we take
- # it only into account on non Apple platforms
- LIST(APPEND PLUGIN_DBG ${OGRE_Plugin_OctreeZone_LIBRARY_DBG})
- ENDIF()
-
- SET(PLUGINS_CONTENT_DBG "# Defines plugins to load\n\n")
- LIST(APPEND PLUGINS_CONTENT_DBG "# Define plugin folder\n")
- LIST(APPEND PLUGINS_CONTENT_DBG "PluginFolder=${OGRE_PLUGIN_DIR_DBG}/\n\n")
- LIST(APPEND PLUGINS_CONTENT_DBG "# Define plugins\n")
- foreach(PLUGIN ${PLUGIN_DBG})
- if(PLUGIN)
- GET_FILENAME_COMPONENT(PLUGIN_NAME ${PLUGIN} NAME_WE)
- LIST(APPEND PLUGINS_CONTENT_DBG " Plugin=${PLUGIN_NAME}\n")
- endif()
- endforeach()
-
- #MESSAGE("PLUGINS_CONTENT_DBG: ${PLUGINS_CONTENT_DBG}")
- FILE(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg" ${PLUGINS_CONTENT_DBG})
- ENDIF()
-
-ENDMACRO()
diff --git a/CMakeModules/VISPCRTLinkage.cmake b/CMakeModules/VISPCRTLinkage.cmake
deleted file mode 100644
index 20675fa..0000000
--- a/CMakeModules/VISPCRTLinkage.cmake
+++ /dev/null
@@ -1,44 +0,0 @@
-if(NOT MSVC)
- message(FATAL_ERROR "CRT options are available only for MSVC")
-endif()
-
-if(NOT BUILD_SHARED_LIBS AND BUILD_WITH_STATIC_CRT)
- foreach(flag_var
- CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
- CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
- CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
- CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
- if(${flag_var} MATCHES "/MD")
- string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}")
- endif()
- if(${flag_var} MATCHES "/MDd")
- string(REGEX REPLACE "/MDd" "/MTd" ${flag_var} "${${flag_var}}")
- endif()
- endforeach(flag_var)
-
- set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /NODEFAULTLIB:atlthunk.lib /NODEFAULTLIB:msvcrt.lib /NODEFAULTLIB:msvcrtd.lib")
- set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_EXE_LINKER_FLAGS_DEBUG} /NODEFAULTLIB:libcmt.lib")
- set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /NODEFAULTLIB:libcmtd.lib")
-else()
- foreach(flag_var
- CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
- CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
- CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
- CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
- if(${flag_var} MATCHES "/MT")
- string(REGEX REPLACE "/MT" "/MD" ${flag_var} "${${flag_var}}")
- endif()
- if(${flag_var} MATCHES "/MTd")
- string(REGEX REPLACE "/MTd" "/MDd" ${flag_var} "${${flag_var}}")
- endif()
- endforeach(flag_var)
-endif()
-
-if(NOT ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} LESS 2.8 AND NOT ${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION} LESS 8.6)
- include(ProcessorCount)
- ProcessorCount(N)
- if(NOT N EQUAL 0)
- SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP${N} ")
- SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N} ")
- endif()
-endif()
diff --git a/CMakeModules/VISPConfig.cmake b/CMakeModules/VISPConfig.cmake
deleted file mode 100644
index f485751..0000000
--- a/CMakeModules/VISPConfig.cmake
+++ /dev/null
@@ -1,149 +0,0 @@
-#############################################################################
-#
-# $Id: VISPConfig.cmake.in 4806 2014-07-30 08:55:55Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# CMake package config file for ViSP.
-#
-# ** File generated automatically, do not modify **
-#
-# This file will define the following CMake variables:
-# - VISP_INCLUDE_DIRS : ViSP and third-party include directories
-# - VISP_LIBRARIES : ViSP library to link against. Third-party libraries are
-# linked automatically thanks to cmake export file VISPTargets.cmake
-# - VISP_VERSION_STRING : Full ViSP version that is build. Example: "2.10.0"
-# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION. Example: "2"
-# - VISP_VERSION_MINOR : Minor version part of VISP_VERSION. Example: "10"
-# - VISP_VERSION_PATCH : Patch version part of VISP_VERSION. Example: "0"
-#
-# Advanced variables:
-# - VISP_SHARED : Use ViSP as shared library
-# - VISP_CONFIG_PATH : Path to this VISPConfig.cmake
-# - VISP_FIND_QUIETLY : If set to TRUE turn off messages during configuration
-# - VISP_USE_FILE : File to include to use ViSP without specific cmake code
-#
-# Windows specific variables:
-# - VISP_STATIC : If set to TRUE uses ViSP static library (.lib) rather then dynamic (.dll)
-#
-# Typical usage in user project:
-#
-# find_package(VISP)
-# include_directories(${VISP_INCLUDE_DIRS})
-# target_link_libraries(MY_TARGET_NAME ${VISP_LIBRARIES})
-#
-# It is also possible to build your project using VISP_USE_FILE.
-#
-# find_package(VISP)
-# if(VISP_FOUND)
-# include(${VISP_USE_FILE})
-# endif()
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# similar code exist in VISPDetectPlatform.cmake
-if(MSVC)
- if(CMAKE_CL_64)
- set(VISP_ARCH x64)
- else()
- set(VISP_ARCH x86)
- endif()
- if(MSVC_VERSION EQUAL 1400)
- set(VISP_RUNTIME vc8)
- elseif(MSVC_VERSION EQUAL 1500)
- set(VISP_RUNTIME vc9)
- elseif(MSVC_VERSION EQUAL 1600)
- set(VISP_RUNTIME vc10)
- elseif(MSVC_VERSION EQUAL 1700)
- set(VISP_RUNTIME vc11)
- elseif(MSVC_VERSION EQUAL 1800)
- set(VISP_RUNTIME vc12)
- endif()
-elseif(MINGW)
- set(VISP_RUNTIME mingw)
-
- execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine
- OUTPUT_VARIABLE VISP_GCC_TARGET_MACHINE
- OUTPUT_STRIP_TRAILING_WHITESPACE)
- if(VISP_GCC_TARGET_MACHINE MATCHES "64")
- set(MINGW64 1)
- set(VISP_ARCH x64)
- else()
- set(VISP_ARCH x86)
- endif()
-endif()
-
-if(CMAKE_VERSION VERSION_GREATER 2.6.2)
- unset(VISP_CONFIG_PATH CACHE)
-endif()
-
-if(NOT VISP_FIND_QUIETLY)
- message(STATUS "ViSP ARCH: ${VISP_ARCH}")
- message(STATUS "ViSP RUNTIME: ${VISP_RUNTIME}")
-endif()
-
-get_filename_component(VISP_CONFIG_PATH "${CMAKE_CURRENT_LIST_FILE}" PATH CACHE)
-if(VISP_RUNTIME AND VISP_ARCH)
- if(NOT DEFINED VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
- set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib")
- elseif(NOT DEFINED VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib/VISPConfig.cmake")
- set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
- elseif(VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib/VISPConfig.cmake")
- set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
- elseif(VISP_STATIC EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
- set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib")
- endif()
-endif()
-
-if(VISP_LIB_PATH AND EXISTS "${VISP_LIB_PATH}/VISPConfig.cmake")
- include("${VISP_LIB_PATH}/VISPConfig.cmake")
-
- set(VISP_FOUND TRUE CACHE BOOL "" FORCE)
-
- if(NOT VISP_FIND_QUIETLY)
- message(STATUS "Found VISP ${VISP_VERSION} in ${VISP_LIB_PATH}")
- if(NOT VISP_LIB_PATH MATCHES "/staticlib")
- get_filename_component(_VISP_LIB_PATH "${VISP_LIB_PATH}/../bin" ABSOLUTE)
- file(TO_NATIVE_PATH "${_VISP_LIB_PATH}" _VISP_LIB_PATH)
- message(STATUS "You might need to add ${_VISP_LIB_PATH} to your PATH to be able to run your applications.")
- endif()
- endif()
-else()
- if(NOT VISP_FIND_QUIETLY)
- message(WARNING
-"Found ViSP for Windows but it has no binaries compatible with your configuration.
-You should manually point CMake variable VISP_DIR to your build of ViSP library."
- )
- endif()
- set(VISP_FOUND FALSE CACHE BOOL "" FORCE)
-endif()
diff --git a/CMakeModules/VISPConfig.cmake.in b/CMakeModules/VISPConfig.cmake.in
deleted file mode 100644
index f420843..0000000
--- a/CMakeModules/VISPConfig.cmake.in
+++ /dev/null
@@ -1,238 +0,0 @@
-#############################################################################
-#
-# $Id: VISPConfig.cmake.in 5011 2014-11-28 10:11:48Z ayol $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# CMake package config file for ViSP.
-#
-# ** File generated automatically, do not modify **
-#
-# This file will define the following CMake variables:
-# - VISP_INCLUDE_DIRS : ViSP and third-party include directories
-# - VISP_LIBRARIES : ViSP library to link against. Third-party libraries are
-# linked automatically thanks to cmake export file VISPTargets.cmake
-# - VISP_VERSION_STRING : Full ViSP version that is build, "@VISP_VERSION@"
-# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION: "@VISP_VERSION_MAJOR@"
-# - VISP_VERSION_MINOR : Minor version part of VISP_VERSION: "@VISP_VERSION_MINOR@"
-# - VISP_VERSION_PATCH : Patch version part of VISP_VERSION: "@VISP_VERSION_PATCH@"
-#
-# Advanced variables:
-# - VISP_SHARED : Use ViSP as shared library
-# - VISP_CONFIG_PATH : Path to this VISPConfig.cmake
-# - VISP_USE_FILE : File to include to use ViSP without specific cmake code
-#
-# Typical usage in user project:
-#
-# find_package(VISP)
-# include_directories(${VISP_INCLUDE_DIRS})
-# target_link_libraries(MY_TARGET_NAME ${VISP_LIBRARIES})
-#
-# It is also possible to build your project using VISP_USE_FILE.
-#
-# find_package(VISP)
-# if(VISP_FOUND)
-# include(${VISP_USE_FILE})
-# endif()
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-#if(VISP_FOUND)
-# return()
-#endif()
-
-#set(VISP_FOUND TRUE)
-
-# Set the version numbers
-set(VISP_VERSION_STRING "@VISP_VERSION@")
-set(VISP_VERSION_MAJOR "@VISP_VERSION_MAJOR@")
-set(VISP_VERSION_MINOR "@VISP_VERSION_MINOR@")
-set(VISP_VERSION_PATCH "@VISP_VERSION_PATCH@")
-
-# Some additional settings are required if ViSP is built as static libs
-set(VISP_SHARED @BUILD_SHARED_LIBS@)
-
-# Extract the directory where *this* file has been installed (determined at cmake run-time)
-get_filename_component(VISP_CONFIG_PATH "${CMAKE_CURRENT_LIST_FILE}" PATH CACHE)
-mark_as_advanced(VISP_CONFIG_PATH)
-
-if(NOT WIN32)
- # Since this file is installed in ./${VISP_INSTALL_LIBDIR}/cmake/visp
- # the first "../../" are here to go to ${VISP_INSTALL_LIBDIR}, and then
- # VISP_INSTALL_LIBDIR_TO_PARENT goes to the parent dir "."
- set(VISP_INSTALL_PATH "${VISP_CONFIG_PATH}/../../@VISP_INSTALL_LIBDIR_TO_PARENT@")
- # Get the absolute path with no ../.. relative marks, to eliminate implicit linker warnings
- if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_LESS 2.8)
- get_filename_component(VISP_INSTALL_PATH "${VISP_INSTALL_PATH}" ABSOLUTE)
- else()
- get_filename_component(VISP_INSTALL_PATH "${VISP_INSTALL_PATH}" REALPATH)
- endif()
-endif()
-
-# Tells the user project where to find ViSP headers
-set(VISP_INCLUDE_DIRS @VISP_INCLUDE_DIRS_CONFIGCMAKE@)
-
-# Tells the user project ViSP library name
-set(VISP_LIBRARIES "@VISP_INTERN_LIBRARY@")
-
-# need to be improved
-if(POLICY CMP0024)
- # Fix to prevent multiple includes
- if(NOT TARGET @VISP_INTERN_LIBRARY@)
- cmake_policy(PUSH)
- cmake_policy(SET CMP0024 OLD)
- # Our library dependencies (contains definitions for IMPORTED targets)
- include("${CMAKE_CURRENT_LIST_DIR}/VISPTargets.cmake")
- cmake_policy(POP)
- endif()
-else()
- # Fix for cmake 2.8.7 to prevent multiple includes
- if(NOT TARGET @VISP_INTERN_LIBRARY@)
- # Our library dependencies (contains definitions for IMPORTED targets)
- include("${CMAKE_CURRENT_LIST_DIR}/VISPTargets.cmake")
- endif()
-endif()
-
-# where to find the USE file to be used by user project
-set(VISP_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/VISPUse.cmake")
-
-if(BUILD_TEST_COVERAGE)
- # Add build options for test coverage. Currently coverage is only supported
- # on gcc compiler
- # Because using -fprofile-arcs with shared lib can cause problems like:
- # hidden symbol `__bb_init_func', we add this option only for static
- # library build
- set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ftest-coverage -fprofile-arcs")
-endif()
-
-#----------------------------------------------------------------------
-# Remember VISP' third party libs configuration:
-#----------------------------------------------------------------------
-SET(VISP_HAVE_X11 "@VISP_HAVE_X11@")
-SET(VISP_HAVE_PTHREAD "@VISP_HAVE_PTHREAD@")
-SET(VISP_HAVE_GTK "@VISP_HAVE_GTK@")
-SET(VISP_HAVE_GDI "@VISP_HAVE_GDI@")
-SET(VISP_HAVE_D3D9 "@VISP_HAVE_D3D9@")
-SET(VISP_HAVE_DISPLAY "@VISP_HAVE_DISPLAY@")
-SET(VISP_HAVE_GSL "@VISP_HAVE_GSL@")
-SET(VISP_HAVE_LAPACK "@VISP_HAVE_LAPACK@")
-SET(VISP_HAVE_LIBFREENECT "@VISP_HAVE_LIBFREENECT@")
-SET(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES "@VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES@")
-SET(VISP_HAVE_LIBFREENECT_OLD "@VISP_HAVE_LIBFREENECT_OLD@")
-SET(VISP_HAVE_LIBUSB_1 "@VISP_HAVE_LIBUSB_1@")
-SET(VISP_HAVE_OPENCV "@VISP_HAVE_OPENCV@")
-SET(VISP_HAVE_OPENCV_NONFREE "@VISP_HAVE_OPENCV_NONFREE@")
-SET(VISP_HAVE_OPENCV_VERSION "@VISP_HAVE_OPENCV_VERSION@")
-SET(VISP_HAVE_OGRE "@VISP_HAVE_OGRE@")
-SET(VISP_HAVE_OPENGL "@VISP_HAVE_OPENGL@")
-SET(VISP_HAVE_OIS "@VISP_HAVE_OIS@")
-SET(VISP_HAVE_COIN_AND_GUI "@VISP_HAVE_COIN_AND_GUI@")
-SET(VISP_HAVE_COIN "@VISP_HAVE_COIN@")
-SET(VISP_HAVE_QT "@VISP_HAVE_QT@")
-SET(VISP_HAVE_SOQT "@VISP_HAVE_SOQT@")
-SET(VISP_HAVE_SOWIN "@VISP_HAVE_SOWIN@")
-SET(VISP_HAVE_SOXT "@VISP_HAVE_SOXT@")
-SET(VISP_HAVE_DC1394_2 "@VISP_HAVE_DC1394_2@")
-SET(VISP_HAVE_CMU1394 "@VISP_HAVE_CMU1394@")
-SET(VISP_HAVE_V4L2 "@VISP_HAVE_V4L2@")
-SET(VISP_HAVE_DIRECTSHOW "@VISP_HAVE_DIRECTSHOW@")
-SET(VISP_HAVE_AFMA4 "@VISP_HAVE_AFMA4@")
-SET(VISP_HAVE_AFMA6 "@VISP_HAVE_AFMA6@")
-SET(VISP_HAVE_BICLOPS "@VISP_HAVE_BICLOPS@")
-SET(VISP_HAVE_PTU46 "@VISP_HAVE_PTU46@")
-SET(VISP_HAVE_VIPER650 "@VISP_HAVE_VIPER650@")
-SET(VISP_HAVE_VIPER850 "@VISP_HAVE_VIPER850@")
-SET(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@")
-SET(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@")
-SET(VISP_HAVE_XML2 "@VISP_HAVE_XML2@")
-SET(VISP_HAVE_LIBJPEG "@VISP_HAVE_LIBJPEG@")
-SET(VISP_HAVE_LIBPNG "@VISP_HAVE_LIBPNG@")
-SET(VISP_HAVE_FFMPEG "@VISP_HAVE_FFMPEG@")
-SET(VISP_HAVE_YARP "@VISP_HAVE_YARP@")
-SET(VISP_HAVE_OPENMP "@VISP_HAVE_OPENMP@")
-SET(VISP_HAVE_ACCESS_TO_NAS "@VISP_HAVE_ACCESS_TO_NAS@")
-SET(VISP_HAVE_CPP11_COMPATIBILITY "@VISP_HAVE_CPP11_COMPATIBILITY@")
-
-#----------------------------------------------------------------------
-# Some useful macro to be able to build the tutorials along side ViSP
-#----------------------------------------------------------------------
-# Create a target from the *.cpp file, link against ViSP libraries and add a dependency to ViSP library
-# tu ensure that the library is build before this target.
-macro(visp_add_target file_cpp)
- get_filename_component(target ${file_cpp} NAME_WE)
- include_directories(${VISP_INCLUDE_DIRS})
- add_executable(${target} ${file_cpp})
- target_link_libraries(${target} ${VISP_LIBRARIES})
- if(VISP_INTERN_LIBRARY)
- add_dependencies(${target} ${VISP_INTERN_LIBRARY})
- endif()
-endmacro()
-
-# Create a dependency to the target extracted from the *.cpp file and put the target in the solution dependency folder.
-macro(visp_add_dependency file_cpp dependency)
- get_filename_component(target ${file_cpp} NAME_WE)
- if(TARGET visp_${dependency})
- add_dependencies(visp_${dependency} ${target})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${target} PROPERTIES FOLDER "${dependency}")
- endif()
- endif()
-endmacro()
-
-# Copy the data files to the same location than the target associated to the cpp files
-# Since CMake 3.0.0 policy CMP0026 was introduced to disallow location property on target.
-# If CMake 3.0.0 is used, we use $<TARGET_FILE_DIR:tgt> to get the target location
-if (CMAKE_VERSION VERSION_GREATER 2.8.12)
- macro(visp_copy_data file_cpp file_data)
- get_filename_component(target ${file_cpp} NAME_WE)
- #get_target_property(target_location ${target} LOCATION)
- get_filename_component(target_location "${target_location}" PATH)
- add_custom_command(
- TARGET ${target}
- POST_BUILD
- #COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "${target_location}"
- COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "$<TARGET_FILE_DIR:${target}>"
- )
- endmacro()
-else()
- macro(visp_copy_data file_cpp file_data)
- get_filename_component(target ${file_cpp} NAME_WE)
- get_target_property(target_location ${target} LOCATION)
- get_filename_component(target_location "${target_location}" PATH)
- add_custom_command(
- TARGET ${target}
- POST_BUILD
- COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "${target_location}"
- )
- endmacro()
-endif()
diff --git a/CMakeModules/VISPDetectPlatform.cmake b/CMakeModules/VISPDetectPlatform.cmake
deleted file mode 100644
index 50ef61f..0000000
--- a/CMakeModules/VISPDetectPlatform.cmake
+++ /dev/null
@@ -1,41 +0,0 @@
-# Similar code exist in VISPConfig.cmake
-
-if(NOT DEFINED VISP_STATIC)
- # look for global setting
- if(NOT DEFINED BUILD_SHARED_LIBS OR BUILD_SHARED_LIBS)
- set(VISP_STATIC OFF)
- else()
- set(VISP_STATIC ON)
- endif()
-endif()
-
-if(MSVC)
- if(CMAKE_CL_64)
- set(VISP_ARCH x64)
- else()
- set(VISP_ARCH x86)
- endif()
- if(MSVC_VERSION EQUAL 1400)
- set(VISP_RUNTIME vc8)
- elseif(MSVC_VERSION EQUAL 1500)
- set(VISP_RUNTIME vc9)
- elseif(MSVC_VERSION EQUAL 1600)
- set(VISP_RUNTIME vc10)
- elseif(MSVC_VERSION EQUAL 1700)
- set(VISP_RUNTIME vc11)
- elseif(MSVC_VERSION EQUAL 1800)
- set(VISP_RUNTIME vc12)
- endif()
-elseif(MINGW)
- set(VISP_RUNTIME mingw)
-
- execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine
- OUTPUT_VARIABLE VISP_GCC_TARGET_MACHINE
- OUTPUT_STRIP_TRAILING_WHITESPACE)
- if(VISP_GCC_TARGET_MACHINE MATCHES "64")
- set(MINGW64 1)
- set(VISP_ARCH x64)
- else()
- set(VISP_ARCH x86)
- endif()
-endif()
diff --git a/CMakeModules/VISPExtraTargets.cmake b/CMakeModules/VISPExtraTargets.cmake
deleted file mode 100644
index b1ff045..0000000
--- a/CMakeModules/VISPExtraTargets.cmake
+++ /dev/null
@@ -1,74 +0,0 @@
-# ----------------------------------------------------------------------------
-# Uninstall target, for "make uninstall"
-# ----------------------------------------------------------------------------
-configure_file(
- "${VISP_CMAKE_MODULE_PATH}/cmake_uninstall.cmake.in"
- "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
- IMMEDIATE @ONLY)
-
-add_custom_target(uninstall
- "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
-
-if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(uninstall PROPERTIES FOLDER "CMakeTargets")
-endif()
-
-# ----------------------------------------------------------------------------
-# Doxygen documentation target, for "make visp_doc" and "make html-doc" (to keep compat with previous versions)
-# ----------------------------------------------------------------------------
-if(DOXYGEN_FOUND)
- add_custom_target(html-doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen) # for compat with previous versions
- add_custom_target(visp_doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen)
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_doc PROPERTIES FOLDER "extra")
- set_target_properties(html-doc PROPERTIES FOLDER "extra")
- endif()
-endif()
-
-# ----------------------------------------------------------------------------
-# Tests target, for make visp_library
-# ----------------------------------------------------------------------------
-add_custom_target(visp_library)
-if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_library PROPERTIES FOLDER "extra")
-endif()
-
-# ----------------------------------------------------------------------------
-# Tests target, for make visp_tests
-# ----------------------------------------------------------------------------
-if(BUILD_TESTS)
- add_custom_target(visp_tests)
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_tests PROPERTIES FOLDER "extra")
- endif()
-endif()
-
-# ----------------------------------------------------------------------------
-# Tests target, for make visp_examples
-# ----------------------------------------------------------------------------
-if(BUILD_EXAMPLES)
- add_custom_target(visp_examples)
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_examples PROPERTIES FOLDER "extra")
- endif()
-endif()
-
-# ----------------------------------------------------------------------------
-# Tests target, for make visp_demos
-# ----------------------------------------------------------------------------
-if(BUILD_DEMOS)
- add_custom_target(visp_demos)
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_demos PROPERTIES FOLDER "extra")
- endif()
-endif()
-
-# ----------------------------------------------------------------------------
-# Tests target, for make visp_tutorials
-# ----------------------------------------------------------------------------
-if(BUILD_TUTORIALS)
- add_custom_target(visp_tutorials)
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(visp_tutorials PROPERTIES FOLDER "extra")
- endif()
-endif()
diff --git a/CMakeModules/VISPGenerateConfig.cmake b/CMakeModules/VISPGenerateConfig.cmake
deleted file mode 100755
index d7d2ef5..0000000
--- a/CMakeModules/VISPGenerateConfig.cmake
+++ /dev/null
@@ -1,196 +0,0 @@
-#############################################################################
-#
-# $Id: GenerateConfigt.cmake 4676 2014-02-17 22:08:37Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# This file generates the ViSPConfig.cmake file:
-# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
-# Part 2/3: ${BIN_DIR}/unix-install/VISPConfig.cmake -> For use with "make install"
-# Part 3/3: ${BIN_DIR}/win-install/VISPConfig.cmake -> For use within binary installers/packages
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Macro that returns the relative path to go from a child folder to the parent folder
-# input: path_to_child
-# output: path_to_parent, the relative path to go from path_to_child to parent
-# example: if input =lib/x86_64-linux-gnu, then output=../..
-macro(get_path_to_parent path_to_child path_to_parent)
- set(${path_to_parent} "")
- set(input_ "${path_to_child}")
- while(input_)
- if(input_)
- set(${path_to_parent} "${${path_to_parent}}../")
- endif()
- get_filename_component(input_ "${input_}" PATH)
- endwhile(input_)
-endmacro()
-
-# Here we determine the relative path from ./${CMAKE_INSTALL_LIBDIR} to its parent folder
-# if CMAKE_INSTALL_LIBDIR=lib, then VISP_INSTALL_LIBDIR_TO_PARENT=../
-# if CMAKE_INSTALL_LIBDIR=lib/x86_64-linux-gnu, then VISP_INSTALL_LIBDIR_TO_PARENT=../..
-get_path_to_parent(${CMAKE_INSTALL_LIBDIR} VISP_INSTALL_LIBDIR_TO_PARENT)
-
-# -------------------------------------------------------------------------------------------
-# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
-# -------------------------------------------------------------------------------------------
-
-# Export the library
-export(TARGETS ${VISP_INTERN_LIBRARY} FILE "${PROJECT_BINARY_DIR}/VISPTargets.cmake")
-
-# Update include dirs
-set(VISP_INCLUDE_DIRS_CONFIGCMAKE_ "${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR}")
-list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE_ ${VISP_EXTERN_INCLUDE_DIRS})
-set(VISP_INCLUDE_DIRS_CONFIGCMAKE "")
-foreach(val ${VISP_INCLUDE_DIRS_CONFIGCMAKE_})
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE "${VISP_INCLUDE_DIRS_CONFIGCMAKE} \"${val}\"")
-endforeach()
-#message("VISP_INCLUDE_DIRS_CONFIGCMAKE: ${VISP_INCLUDE_DIRS_CONFIGCMAKE}")
-
-configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfig.cmake.in
- ${VISP_BINARY_DIR}/VISPConfig.cmake
- IMMEDIATE @ONLY)
-
-configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfigVersion.cmake.in
- ${VISP_BINARY_DIR}/VISPConfigVersion.cmake
- IMMEDIATE @ONLY)
-
-configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPUse.cmake.in
- ${VISP_BINARY_DIR}/VISPUse.cmake
- IMMEDIATE @ONLY)
-
-# --------------------------------------------------------------------------------------------
-# Part 2/3: ${BIN_DIR}/unix-install/VISPConfig.cmake -> For use *with* "make install"
-# -------------------------------------------------------------------------------------------
-
-if(UNIX)
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE_ "\${VISP_INSTALL_PATH}/${CMAKE_INSTALL_INCLUDEDIR}")
- list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE_ ${VISP_EXTERN_INCLUDE_DIRS})
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE "")
- foreach(val ${VISP_INCLUDE_DIRS_CONFIGCMAKE_})
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE "${VISP_INCLUDE_DIRS_CONFIGCMAKE} \"${val}\"")
- endforeach()
- #message("VISP_INCLUDE_DIRS_CONFIGCMAKE: ${VISP_INCLUDE_DIRS_CONFIGCMAKE}")
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfig.cmake.in
- ${VISP_BINARY_DIR}/unix-install/VISPConfig.cmake
- IMMEDIATE @ONLY)
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfigVersion.cmake.in
- ${VISP_BINARY_DIR}/unix-install/VISPConfigVersion.cmake
- IMMEDIATE @ONLY)
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPUse.cmake.in
- ${VISP_BINARY_DIR}/unix-install/VISPUse.cmake
- IMMEDIATE @ONLY)
-
- configure_file(${VISP_SOURCE_DIR}/include/vpConfig.h.cmake
- ${VISP_BINARY_DIR}/unix-install/vpConfig.h @ONLY)
-
- install(FILES
- ${VISP_BINARY_DIR}/unix-install/VISPConfig.cmake
- ${VISP_BINARY_DIR}/unix-install/VISPConfigVersion.cmake
- ${VISP_BINARY_DIR}/unix-install/VISPUse.cmake
- DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/visp"
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries)
-
- # Install the export set for use with the install-tree
- install(EXPORT VISPTargets
- FILE VISPTargets.cmake
- DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/visp"
- COMPONENT libraries)
-endif()
-
-# --------------------------------------------------------------------------------------------
-# Part 3/3: ${BIN_DIR}/win-install/VISPConfig.cmake -> For use within binary installers/packages
-# --------------------------------------------------------------------------------------------
-if(WIN32)
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE_ "\${VISP_CONFIG_PATH}/${CMAKE_INSTALL_INCLUDEDIR}")
- list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE_ ${VISP_EXTERN_INCLUDE_DIRS})
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE "")
- foreach(val ${VISP_INCLUDE_DIRS_CONFIGCMAKE_})
- set(VISP_INCLUDE_DIRS_CONFIGCMAKE "${VISP_INCLUDE_DIRS_CONFIGCMAKE} \"${val}\"")
- endforeach()
- #message("VISP_INCLUDE_DIRS_CONFIGCMAKE: ${VISP_INCLUDE_DIRS_CONFIGCMAKE}")
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfig.cmake.in
- ${VISP_BINARY_DIR}/win-install/VISPConfig.cmake
- IMMEDIATE @ONLY)
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPConfigVersion.cmake.in
- ${VISP_BINARY_DIR}/win-install/VISPConfigVersion.cmake
- IMMEDIATE @ONLY)
-
- configure_file(
- ${VISP_CMAKE_MODULE_PATH}/VISPUse.cmake.in
- ${VISP_BINARY_DIR}/win-install/VISPUse.cmake
- IMMEDIATE @ONLY)
-
- if(BUILD_SHARED_LIBS)
- install(FILES
- "${CMAKE_BINARY_DIR}/win-install/ViSPConfig.cmake"
- "${CMAKE_BINARY_DIR}/win-install/ViSPUse.cmake"
- DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}"
- COMPONENT libraries)
- install(EXPORT VISPTargets
- DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}"
- FILE VISPTargets.cmake
- COMPONENT libraries)
- else()
- install(FILES
- "${CMAKE_BINARY_DIR}/win-install/ViSPConfig.cmake"
- "${CMAKE_BINARY_DIR}/win-install/ViSPUse.cmake"
- DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}"
- COMPONENT libraries)
- install(EXPORT VISPTargets
- DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}"
- FILE VISPTargets.cmake
- COMPONENT libraries)
- endif()
-
- install(FILES
- "${VISP_CMAKE_MODULE_PATH}/VISPConfig.cmake"
- "${VISP_BINARY_DIR}/win-install/VISPConfigVersion.cmake"
- DESTINATION "${CMAKE_INSTALL_PREFIX}"
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
- COMPONENT libraries)
-endif()
diff --git a/CMakeModules/VISPGeneratePkgConfigScript.cmake b/CMakeModules/VISPGeneratePkgConfigScript.cmake
deleted file mode 100644
index f33c2b2..0000000
--- a/CMakeModules/VISPGeneratePkgConfigScript.cmake
+++ /dev/null
@@ -1,332 +0,0 @@
-#############################################################################
-#
-# $Id: VISPGeneratePkgConfigScript.cmake 5314 2015-02-12 08:32:30Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# This file generates the ViSP library config shell scripts:
-# - visp-config in <build dir>/bin from visp-config.in
-# - visp-config in <build dir>/install from visp-config.install.in
-# When make install, this file is copied in <install dir>/bin
-# - visp.pc in <build dir>/install from visp.pc.in
-# When make install, this file is copied in <install dir>/lib/pkgconfig
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-if (UNIX)
- #######################################################################
- #
- # for Unix platforms: Linux, OSX
- #
- #######################################################################
- set(FILE_VISP_CONFIG_SCRIPT_IN "${VISP_SOURCE_DIR}/CMakeModules/visp-config.in")
- set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config")
-
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "${VISP_SOURCE_DIR}/CMakeModules/visp-config.install.in")
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/unix-install/visp-config")
-
- set(FILE_VISP_CONFIG_PC_INSTALL_IN "${VISP_SOURCE_DIR}/CMakeModules/visp.pc.in")
- set(FILE_VISP_CONFIG_PC_INSTALL "${VISP_BINARY_DIR}/unix-install/visp.pc")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_PREFIX
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_CFLAGS
- #----------------------------------------------------------------------
- foreach(INCDIR ${VISP_EXTERN_INCLUDE_DIRS})
- list(APPEND VISP_CONFIG_CFLAGS "-I${INCDIR}")
- endforeach()
-
- # Suppress twins
- if(VISP_CONFIG_CFLAGS)
- list(REMOVE_DUPLICATES VISP_CONFIG_CFLAGS)
- endif()
-
- # Format the string to suppress CMake separators ";"
- set(VISP_CONFIG_CFLAGS_REFORMATED "")
- foreach(element ${VISP_CONFIG_CFLAGS})
- set(VISP_CONFIG_CFLAGS_REFORMATED "${VISP_CONFIG_CFLAGS_REFORMATED} ${element}")
- endforeach()
- set(VISP_CONFIG_CFLAGS ${VISP_CONFIG_CFLAGS_REFORMATED})
-# message(": ${VISP_CONFIG_CFLAGS}")
-
- if(BUILD_TEST_COVERAGE)
- # Add build options for test coverage. Currently coverage is only supported
- # on gcc compiler
- # Because using -fprofile-arcs with shared lib can cause problems like:
- # hidden symbol `__bb_init_func', we add this option only for static
- # library build
- set(VISP_CONFIG_CFLAGS "${VISP_CONFIG_CFLAGS} -ftest-coverage -fprofile-arcs")
- endif()
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_LIBS
- #
- # add "-l" to library names
- # skip *.so, *.a, *.dylib, -framework*, -l*
- #
- #----------------------------------------------------------------------
-
- # need to be improved
- if(POLICY CMP0026)
- cmake_policy(PUSH)
- cmake_policy(SET CMP0026 OLD)
- get_target_property(visp_libpath ${VISP_INTERN_LIBRARY} LOCATION_Release)
- cmake_policy(POP)
- else()
- get_target_property(visp_libpath ${VISP_INTERN_LIBRARY} LOCATION_Release)
- endif()
- #message("ViSP libpath: ${vip_libpath}")
- get_filename_component(visp_libname "${visp_libpath}" NAME)
-
- #message("ViSP libname: ${visp_libname}")
- # Manage the libs
- list(REMOVE_ITEM VISP_EXTERN_LIBRARIES "debug")
- list(REMOVE_ITEM VISP_EXTERN_LIBRARIES "optimized")
- set(TMP_LIBS)
- foreach(lib ${VISP_EXTERN_LIBRARIES})
- if("${lib}" MATCHES "[-][f][r][a][m][e][w][o][r][k]+.")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES ".[.][f][r][a][m][e][w][o][r][k]+$")
- # replace /path/name.framework by -framework name
- get_filename_component(FRAMEWORK ${lib} NAME_WE)
- #message("add -framework ${FRAMEWORK}")
- list(APPEND TMP_LIBS "-framework ${FRAMEWORK}")
- elseif("${lib}" MATCHES ".[.][s][o]+$" OR "${lib}" MATCHES ".[.][a]+$")
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES ".[.][s][o][.][0123456789]+$")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES ".[.][s][o][.][0123456789]*[.][0123456789]+$")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES ".[.][s][o][.][0123456789]*[.][0123456789]*[.][0123456789]+$")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES ".[.][d][y][l][i][b]+$")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- elseif("${lib}" MATCHES "^(-l)")
- # does nothing
- list(APPEND TMP_LIBS ${lib})
- else()
- # add -l prefix
- #MESSAGE("add -l${lib}")
- list(APPEND TMP_LIBS "${lib}")
- endif()
- endforeach()
-
- foreach(val ${TMP_LIBS})
- set(VISP_CONFIG_LIBS "${VISP_CONFIG_LIBS} ${val}")
- endforeach(val)
-
- #---------------------------------------------------------------------
- # Updates the <build dir>/bin/visp-config shell script
- # Updates VISP_CONFIG_LIBS_SCRIPT (for visp-config)
- # Updates VISP_CONFIG_CFLAGS_SCRIPT (for visp-config)
- #----------------------------------------------------------------------
-
- # prepend with ViSP own include dir
- set(VISP_CONFIG_CFLAGS_SCRIPT "-I$PREFIX/${CMAKE_INSTALL_INCLUDEDIR} ${VISP_CONFIG_CFLAGS}")
-
- # prepend with ViSP own lib
- set(VISP_CONFIG_LIBS_SCRIPT "$PREFIX/${CMAKE_INSTALL_LIBDIR}/${visp_libname} ${VISP_CONFIG_LIBS}")
-
- set(VISP_ECHO_NO_NEWLINE_CHARACTER "")
- set(VISP_ECHO_NO_NEWLINE_OPTION "")
- if(APPLE)
- set(VISP_ECHO_NO_NEWLINE_CHARACTER "\\c")
- else()
- set(VISP_ECHO_NO_NEWLINE_OPTION "-n")
- endif()
-
- configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
-
- #---------------------------------------------------------------------
- # Updates the <build dir>/install/visp-config shell script
- #----------------------------------------------------------------------
-
- configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
-
- #---------------------------------------------------------------------
- # Updates the <build dir>/install/visp.pc pkg-config file
- # Updates VISP_CONFIG_CFLAGS_PC (for libvisp.pc used by pkg-config)
- # Updates VISP_CONFIG_LIBS_PC (for libvisp.pc used by pkg-config)
- #----------------------------------------------------------------------
- set(exec_prefix "\${prefix}")
- set(includedir "\${prefix}/${CMAKE_INSTALL_INCLUDEDIR}")
- set(libdir "\${prefix}/${CMAKE_INSTALL_LIBDIR}")
-
- # prepend with ViSP own include dir
- set(VISP_CONFIG_CFLAGS_PC "-I\${includedir} ${VISP_CONFIG_CFLAGS}")
-
- # prepend with ViSP own lib dir and append -L<lib dir>
- set(VISP_CONFIG_LIBS_PC "\${libdir}/${visp_libname} ${VISP_CONFIG_LIBS}")
- configure_file(${FILE_VISP_CONFIG_PC_INSTALL_IN} ${FILE_VISP_CONFIG_PC_INSTALL})
-
-else(UNIX)
- #######################################################################
- #
- # for windows platforms
- #
- #######################################################################
- set(FILE_VISP_CONFIG_SCRIPT_IN "${VISP_SOURCE_DIR}/CMakeModules/visp-config.bat.in")
- set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config.bat")
-
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "${VISP_SOURCE_DIR}/CMakeModules/visp-config.bat.in")
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/win-install/visp-config-${VISP_ARCH}-${VISP_RUNTIME}.bat")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_PREFIX
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_DEF
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_DEFS "")
- set(VISP_OPENMP_SUPPORT "no")
- if(NOT ${VISP_OPENMP_FLAGS} STREQUAL "")
- set(VISP_CONFIG_SCRIPT_DEFS "${VISP_OPENMP_FLAGS}")
- set(VISP_OPENMP_SUPPORT "yes")
- endif()
- if(NOT ${VISP_CPP11_FLAGS} STREQUAL "")
- set(VISP_CONFIG_SCRIPT_DEFS "${VISP_CPP11_FLAGS}, ${VISP_CONFIG_SCRIPT_DEFS}")
- endif()
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_INCLUDE
- #----------------------------------------------------------------------
- list(APPEND VISP_EXTERN_INCLUDE_DIRS "%PREFIX%/${CMAKE_INSTALL_INCLUDEDIR}")
- list(REMOVE_DUPLICATES VISP_EXTERN_INCLUDE_DIRS)
-
- # Format the string
- set(VISP_CONFIG_SCRIPT_INC ${VISP_EXTERN_INCLUDE_DIRS})
- #message(VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC})
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_LIBDIR
- # 1/ For usage with the build tree
- # 2/ For usage with the install tree
- #
- # and updates VISP_CONFIG_SCRIPT_LIBS_${config}
- #----------------------------------------------------------------------
- set(TMP_SCRIPT_LIBS_DEBUG "${VISP_INTERN_LIBRARY}${VISP_DLLVERSION}${VISP_DEBUG_POSTFIX}.lib")
- set(TMP_SCRIPT_LIBS_OPTIMIZED "${VISP_INTERN_LIBRARY}${VISP_DLLVERSION}.lib")
-
- #MESSAGE(VISP_EXTERN_LIBRARIES: ${VISP_EXTERN_LIBRARIES})
- set(TMP_IS_DEBUG FALSE)
- set(TMP_IS_OPTIMIZED FALSE)
- foreach(lib ${VISP_EXTERN_LIBRARIES})
- if("${lib}" MATCHES "[d][e][b][u][g]")
- set(TMP_IS_DEBUG TRUE)
- elseif("${lib}" MATCHES "[o][p][t][i][m][i][z][e][d]")
- set(TMP_IS_OPTIMIZED TRUE)
- else()
- # Get the library name
- get_filename_component(libname ${lib} NAME)
- if("${libname}" MATCHES ".+[.][l][i][b]" OR "${libname}" MATCHES ".+[.][L][i][b]")
- #MESSAGE("${libname} matches .lib or .Lib")
- else()
- # We need to add .lib suffix
- #MESSAGE("For ${libname} we add .lib suffix")
- set(libname "${libname}.lib")
- endif()
-
- # Get the library path
- get_filename_component(libpath ${lib} PATH)
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR_ "${libpath}")
-
- if(TMP_IS_DEBUG)
- set(TMP_IS_DEBUG FALSE)
- list(APPEND TMP_SCRIPT_LIBS_DEBUG ${libname})
- elseif(TMP_IS_OPTIMIZED)
- set(TMP_IS_OPTIMIZED FALSE)
- list(APPEND TMP_SCRIPT_LIBS_OPTIMIZED ${libname})
- else()
- list(APPEND TMP_SCRIPT_LIBS_DEBUG ${libname})
- list(APPEND TMP_SCRIPT_LIBS_OPTIMIZED ${libname})
- endif()
- endif()
- endforeach(lib)
-
- # Format the string
- set(VISP_CONFIG_SCRIPT_LIBS_DEBUG "${TMP_SCRIPT_LIBS_DEBUG}")
- set(VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED "${TMP_SCRIPT_LIBS_OPTIMIZED}")
-
- # Format the string
- string(REGEX REPLACE "lib/Release" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
- string(REGEX REPLACE "lib/Debug" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
-
- # 1/ For usage with the build tree
- set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib/$(ConfigurationName)")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
- list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
- configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
-
- # 2/ For usage with the install tree
- set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/${VISP_ARCH}/${VISP_RUNTIME}/lib")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
- list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
- configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
-endif(UNIX)
-
-#----------------------------------------------------------------------
-# customize install target
-#----------------------------------------------------------------------
-# install rule for visp-config shell script
-install(FILES ${FILE_VISP_CONFIG_SCRIPT_INSTALL}
- DESTINATION ${CMAKE_INSTALL_BINDIR}
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE
- COMPONENT libraries)
-
-# install rule for visp.pc pkg-config file
-if(UNIX)
- install(FILES ${FILE_VISP_CONFIG_PC_INSTALL}
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_WRITE
- COMPONENT libraries)
-else()
- # not implemented yet
-endif()
-
-
diff --git a/CMakeModules/VISPUse.cmake.in b/CMakeModules/VISPUse.cmake.in
deleted file mode 100644
index 36ab38f..0000000
--- a/CMakeModules/VISPUse.cmake.in
+++ /dev/null
@@ -1,49 +0,0 @@
-#############################################################################
-#
-# $Id: VISPUse.cmake.in 4616 2014-01-24 18:43:25Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# cmake PackageConfig file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-# Tell the compiler where to find ViSP's header files
-# and the third party headers we depend on
-include_directories(${VISP_INCLUDE_DIRS})
-
-# Tell the compiler where to find ViSP's libraries
-# and the third party libraries we depend on
-link_libraries(${VISP_LIBRARIES})
-
diff --git a/CMakeModules/cmake_uninstall.cmake.in b/CMakeModules/cmake_uninstall.cmake.in
deleted file mode 100644
index fb9072b..0000000
--- a/CMakeModules/cmake_uninstall.cmake.in
+++ /dev/null
@@ -1,64 +0,0 @@
-#############################################################################
-#
-# $Id: cmake_uninstall.cmake.in 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Comes from http://www.cmake.org/Wiki/CMake_FAQ to create an uninstall target
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
- #MESSAGE("Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
- MESSAGE("There is no files to uninstall")
-ELSE(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
- FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
- STRING(REGEX REPLACE "\n" ";" files "${files}")
- FOREACH(file ${files})
- MESSAGE(STATUS "Uninstalling \"${file}\"")
- IF(EXISTS "${file}")
- EXEC_PROGRAM(
- "@CMAKE_COMMAND@" ARGS "-E remove \"${file}\""
- OUTPUT_VARIABLE rm_out
- RETURN_VALUE rm_retval
- )
- IF("${rm_retval}" STREQUAL 0)
- ELSE("${rm_retval}" STREQUAL 0)
- MESSAGE(FATAL_ERROR "Problem when removing \"${file}\"")
- ENDIF("${rm_retval}" STREQUAL 0)
- ELSE(EXISTS "${file}")
- MESSAGE(STATUS "File \"${file}\" does not exist.")
- ENDIF(EXISTS "${file}")
- ENDFOREACH(file)
-
-ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
diff --git a/CMakeModules/resources.cfg.in b/CMakeModules/resources.cfg.in
deleted file mode 100644
index dc38028..0000000
--- a/CMakeModules/resources.cfg.in
+++ /dev/null
@@ -1,11 +0,0 @@
-# Resources required by the sample browser and most samples.
-[Essential]
-#Zip=@OGRE_MEDIA_DIR@/packs/SdkTrays.zip
-
-# Resource locations to be added to the default path
-[General]
-FileSystem=@OGRE_MEDIA_DIR@
- at OGRE_COMMENT_LINE@FileSystem=@OGRE_MEDIA_DIR@/materials/programs
-FileSystem=@OGRE_MEDIA_DIR@/materials/scripts
-FileSystem=@OGRE_MEDIA_DIR@/materials/textures
-FileSystem=@OGRE_MEDIA_DIR@/models
diff --git a/CMakeModules/visp-config.bat.in b/CMakeModules/visp-config.bat.in
deleted file mode 100755
index 6c8167a..0000000
--- a/CMakeModules/visp-config.bat.in
+++ /dev/null
@@ -1,138 +0,0 @@
- at rem #############################################################################
- at rem #
- at rem # $Id: visp-config.bat.in 4961 2014-11-14 13:06:26Z fspindle $
- at rem #
- at rem # This file is part of the ViSP software.
- at rem # Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- at rem #
- at rem # This software is free software; you can redistribute it and/or
- at rem # modify it under the terms of the GNU General Public License
- at rem # ("GPL") version 2 as published by the Free Software Foundation.
- at rem # See the file LICENSE.txt at the root directory of this source
- at rem # distribution for additional information about the GNU GPL.
- at rem #
- at rem # For using ViSP with software that can not be combined with the GNU
- at rem # GPL, please contact INRIA about acquiring a ViSP Professional
- at rem # Edition License.
- at rem #
- at rem # See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- at rem #
- at rem # This software was developed at:
- at rem # INRIA Rennes - Bretagne Atlantique
- at rem # Campus Universitaire de Beaulieu
- at rem # 35042 Rennes Cedex
- at rem # France
- at rem # http://www.irisa.fr/lagadic
- at rem #
- at rem # If you have questions regarding the use of this file, please contact
- at rem # INRIA at visp at inria.fr
- at rem #
- at rem # This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
- at rem # WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
- at rem #
- at rem #
- at rem # Description:
- at rem # visp-config.bat script for Windows.
- at rem # Auto-generated from visp-config.bat.in by cmake.
- at rem #
- at rem # Authors:
- at rem # Fabien Spindler
- at rem #
- at rem #############################################################################
-
-
- at echo off
-
- at rem Get the dirname
- at rem for /F %%x in ('CHDIR') do set PREFIX=%%x\..
-set CURPATH=%CD /d %%~dp0%
-
- at rem echo CURPATH: %CURPATH%
-
-set PREFIX=%CURPATH%\..
-
-set VERSION=${VISP_VERSION}
-
-set DEFS=${VISP_CONFIG_SCRIPT_DEFS}
-
-set INCLUDE="${VISP_CONFIG_SCRIPT_INC}"
-
-set OPENMP_SUPPORT=${VISP_OPENMP_SUPPORT}
-
-set LIBDIR="${VISP_CONFIG_SCRIPT_LIBDIR}"
-
-set LIBS_DEBUG=${VISP_CONFIG_SCRIPT_LIBS_DEBUG}
-
-set LIBS_OPTIMIZED=${VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED}
-
- at rem Test if an argument is provided
-
-if "%1" == "" goto USAGE
-
- at rem Parse the argument list
-for %%a in (%*) do (
- at rem echo Read arg %%a
- if "%%a" == "--help" (
- goto USAGE
- )
- if "%%a" == "--prefix" (
- echo %PREFIX%
- goto END
- )
- if "%%a" == "--def" (
- echo %DEFS%
- goto END
- )
- if "%%a" == "--include" (
- echo %INCLUDE%
- goto END
- )
- if "%%a" == "--openmp" (
- echo %OPENMP_SUPPORT%
- goto END
- )
- if "%%a" == "--libpath" (
- echo %LIBDIR%
- goto END
- )
- if "%%a" == "--libs-debug" (
- echo %LIBS_DEBUG%
- goto END
- )
- if "%%a" == "--libs-optimized" (
- echo %LIBS_OPTIMIZED%
- goto END
- )
- if "%%a" == "--version" (
- echo ViSP %VERSION% Visual Servoing Platform
- echo.
- echo Copyright 2005 - 2014 Inria. All rights reserved.
- goto END
- )
- if "%%a" == "--dumpversion" (
- echo %VERSION%
- goto END
- )
-
-)
-
-:USAGE
-echo ViSP %VERSION% (Visual Servoing Platform)
-echo Copyright (C) 2005 - 2014 Inria. All rights reserved.
-echo.
-echo Usage: %0 [--prefix] [--def] [--include] [--openmp] [--libpath]
-echo [--libs-debug] [--libs-optimized] [--version] [--dumpversion] [--help]
-echo.
-echo --prefix Show ViSP installation prefix.
-echo --def Print pre-processor definitions.
-echo --include Print include directories.
-echo --openmp Indicates if OpenMP support has to be turned on/off.
-echo --libpath Print library directories.
-echo --libs-debug Print library dependencies for debug configuration.
-echo --libs-optimized Print library dependencies for optimized configuration.
-echo --dumpversion Output ViSP version information.
-echo --help Display this help and exit.
-echo.
-goto END
-
-:End
\ No newline at end of file
diff --git a/CMakeModules/visp-config.in b/CMakeModules/visp-config.in
deleted file mode 100755
index 8f11a5a..0000000
--- a/CMakeModules/visp-config.in
+++ /dev/null
@@ -1,99 +0,0 @@
-#!/bin/sh
-
-#############################################################################
-#
-# $Id: visp-config.in 5043 2014-12-08 16:14:15Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# visp-config shell script.
-# Auto-generated from visp-config.in by cmake.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-relpath=`dirname $0`
-relpath=`(cd $relpath/..; pwd)`
-
-PREFIX=$relpath
-
-CFLAGS="${VISP_CONFIG_CFLAGS_SCRIPT}"
-
-LIBS="${VISP_CONFIG_LIBS_SCRIPT}"
-
-VERSION="${VISP_VERSION}"
-
-NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
-NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
-
-usage()
-{
- cat <<EOF
-
-ViSP $VERSION (Visual Servoing Platform)
-Copyright (C) 2005 - 2014 Inria. All rights reserved.
-
-Usage: $0 [--prefix] [--cflags] [--libs] [--version] [--dumpversion] [--help]
-
- --prefix Show ViSP installation prefix.
- --cflags Print pre-processor and compiler flags including
- third party includes we depend on.
- --libs Print library linking information with ViSP
- including third party libraries we depend on.
- --version Output ViSP information.
- --dumpversion Output ViSP version information.
- --help Display this help and exit.
-
-EOF
- exit $1
-}
-
-if ! test "$1"; then
- usage;
- exit 0;
-fi;
-
-for arg in $@; do
- case $arg in
- --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
- --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
- --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
- --version)
- echo "ViSP $VERSION (Visual Servoing Platform)"
- echo ""
- echo "Copyright (C) 2005 - 2014 Inria. All rights reserved.";;
- --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
- *) usage; exit 0 ;;
- esac;
-done;
-echo ""
diff --git a/CMakeModules/visp-config.install.in b/CMakeModules/visp-config.install.in
deleted file mode 100755
index 6bd8a10..0000000
--- a/CMakeModules/visp-config.install.in
+++ /dev/null
@@ -1,103 +0,0 @@
-#!/bin/sh
-
-#############################################################################
-#
-# $Id: visp-config.in 4057 2013-01-05 13:10:29Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# visp-config shell script.
-# Auto-generated from visp-config.install.in by cmake. For backward
-# compatibility used visp.pc.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-
-relpath=`dirname $0`
-relpath=`(cd $relpath/..; pwd)`
-
-PREFIX=$relpath
-
-CFLAGS_CMD=`pkg-config --cflags visp`
-CFLAGS=$CFLAGS_CMD
-
-LIBS_CMD=`pkg-config --libs visp`
-LIBS=$LIBS_CMD
-
-VERSION_CMD=`pkg-config --modversion visp`
-VERSION=$VERSION_CMD
-
-NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
-NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
-
-usage()
-{
- cat <<EOF
-
-ViSP $VERSION (Visual Servoing Platform)
-Copyright (C) 2005 - 2014 Inria. All rights reserved.
-
-Usage: $0 [--prefix] [--cflags] [--libs] [--version] [--dumpversion] [--help]
-
- --prefix Show ViSP installation prefix.
- --cflags Print pre-processor and compiler flags including
- third party includes we depend on.
- --libs Print library linking information with ViSP
- including third party libraries we depend on.
- --version Output ViSP information.
- --dumpversion Output ViSP version information.
- --help Display this help and exit.
-
-EOF
- exit $1
-}
-
-if ! test "$1"; then
- usage;
- exit 0;
-fi;
-
-for arg in $@; do
- case $arg in
- --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
- --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
- --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
- --version)
- echo "ViSP $VERSION (Visual Servoing Platform)"
- echo ""
- echo "Copyright (C) 2005 - 2014 Inria. All rights reserved.";;
- --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
- *) usage; exit 0 ;;
- esac;
-done;
-echo ""
diff --git a/CMakeModules/visp.pc.in b/CMakeModules/visp.pc.in
deleted file mode 100644
index 4594265..0000000
--- a/CMakeModules/visp.pc.in
+++ /dev/null
@@ -1,14 +0,0 @@
-# Package Information for pkg-config
-
-prefix=${CMAKE_INSTALL_PREFIX}
-exec_prefix=@exec_prefix@
-libdir=@libdir@
-includedir=@includedir@
-
-Name: ViSP
-Description: Visual Servoing Platform
-URL: http://team.inria.fr/lagadic/visp
-Version: ${VISP_VERSION}
-Libs: ${VISP_CONFIG_LIBS_PC}
-
-Cflags: ${VISP_CONFIG_CFLAGS_PC}
diff --git a/CMakeSourceFileList.cmake b/CMakeSourceFileList.cmake
deleted file mode 100644
index ec9a60a..0000000
--- a/CMakeSourceFileList.cmake
+++ /dev/null
@@ -1,489 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeSourceFileList.cmake,v 1.29 2008-12-17 14:45:01 fspindle Exp $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP source file list.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_subdir variable to all the files we want to parse during
-# the build process.
-# Don't forget to update SRC_ALL variable if you add/remove a SRC_subdir
-# variable
-#
-# If you add/remove a directory, modify here
-SET (SRC_CAMERA
- camera/vpCameraParameters.cpp
- camera/vpMeterPixelConversion.cpp
- camera/vpPixelMeterConversion.cpp
- camera/calibration/vpCalibration.cpp
- camera/calibration/vpCalibrationTools.cpp
- )
-
-IF(VISP_HAVE_XML2)
- LIST(APPEND SRC_CAMERA camera/vpXmlParserCamera.cpp)
-ENDIF()
-
-SET (SRC_COMPUTER_VISION
- computer-vision/homography-estimation/vpHomography.cpp
- computer-vision/homography-estimation/vpHomographyDLT.cpp
- computer-vision/homography-estimation/vpHomographyVVS.cpp
- computer-vision/homography-estimation/vpHomographyExtract.cpp
- computer-vision/homography-estimation/vpHomographyMalis.cpp
- computer-vision/homography-estimation/vpHomographyRansac.cpp
- computer-vision/pose-estimation/vpLevenbergMarquartd.cpp
- computer-vision/pose-estimation/vpPose.cpp
- computer-vision/pose-estimation/vpPoseFeatures.cpp
- computer-vision/pose-estimation/vpPoseDementhon.cpp
- computer-vision/pose-estimation/vpPoseLagrange.cpp
- computer-vision/pose-estimation/vpPoseLowe.cpp
- computer-vision/pose-estimation/vpPoseRansac.cpp
- computer-vision/pose-estimation/vpPoseVirtualVisualServoing.cpp
- )
-
-if(VISP_HAVE_ZBAR)
- list(APPEND SRC_DETECTION detection/barcode/vpDetectorQRCode.cpp)
-endif()
-
-if(VISP_HAVE_DMTX)
- list(APPEND SRC_DETECTION detection/barcode/vpDetectorDataMatrixCode.cpp)
-endif()
-
-SET (SRC_EXCEPTION
- exceptions/vpException.cpp
- )
-
-SET (SRC_DEVICE_FRAMEGRABBER
- device/framegrabber/disk/vpDiskGrabber.cpp
- )
-
-IF(VISP_HAVE_DC1394_2)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/1394/vp1394TwoGrabber.cpp)
-ENDIF()
-IF(VISP_HAVE_CMU1394)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/1394/vp1394CMUGrabber.cpp)
-ENDIF()
-IF(VISP_HAVE_V4L2)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/v4l2/vpV4l2Grabber.cpp)
-ENDIF()
-IF(VISP_HAVE_DIRECTSHOW)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/directshow/vpDirectShowGrabber.cpp)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/directshow/vpDirectShowGrabberImpl.cpp)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/directshow/vpDirectShowDevice.cpp)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp)
-ENDIF()
-IF(VISP_HAVE_OPENCV)
- LIST(APPEND SRC_DEVICE_FRAMEGRABBER device/framegrabber/OpenCV/vpOpenCVGrabber.cpp)
-ENDIF()
-
-SET (SRC_IMAGE
- image/vpColor.cpp
- image/vpImageConvert.cpp
- image/vpImageFilter.cpp
- image/vpImageIo.cpp
- image/vpImageTools.cpp
- image/vpRGBa.cpp
- image/vpImagePoint.cpp
- )
-
-SET (SRC_KEY_POINT
- key-point/vpBasicKeyPoint.cpp
- )
-
-if(VISP_HAVE_OPENCV_NONFREE AND OpenCV_VERSION VERSION_LESS 3.0.0)
- list(APPEND SRC_KEY_POINT key-point/vpKeyPointSurf.cpp)
-endif()
-if(VISP_HAVE_OPENCV)
- list(APPEND SRC_KEY_POINT key-point/vpPlanarObjectDetector.cpp)
- list(APPEND SRC_KEY_POINT key-point/vpFernClassifier.cpp)
- list (APPEND SRC_KEY_POINT key-point/vpKeyPoint.cpp)
-endif()
-
-IF(VISP_HAVE_XML2)
- LIST(APPEND SRC_KEY_POINT key-point/vpXmlConfigParserKeyPoint.cpp)
-ENDIF()
-
-IF(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
- SET (SRC_DEVICE_KINECT
- device/kinect/vpKinect.cpp
- )
-ENDIF()
-
-SET (SRC_DEVICE_LASERSCANNER
- device/laserscanner/sick/vpSickLDMRS.cpp
- )
-
-IF(VISP_HAVE_PARPORT)
- SET (SRC_DEVICE_LIGHT
- device/light/vpRingLight.cpp
- )
-ENDIF()
-
-SET (SRC_MATH
- math/kalman/vpKalmanFilter.cpp
- math/kalman/vpLinearKalmanFilterInstantiation.cpp
- math/matrix/vpColVector.cpp
- math/matrix/vpMatrix.cpp
- math/matrix/vpMatrix_lu.cpp
- math/matrix/vpMatrix_qr.cpp
- math/matrix/vpMatrix_svd.cpp
- math/matrix/vpMatrix_covariance.cpp
- math/matrix/vpRowVector.cpp
- math/matrix/vpSubMatrix.cpp
- math/matrix/vpSubColVector.cpp
- math/matrix/vpSubRowVector.cpp
- math/misc/vpMath.cpp
- math/misc/vpNoise.cpp
- math/misc/vpHinkley.cpp
- math/robust/vpRobust.cpp
- math/robust/vpScale.cpp
- math/spline/vpBSpline.cpp
- math/spline/vpNurbs.cpp
- math/transformation/vpExponentialMap.cpp
- math/transformation/vpForceTwistMatrix.cpp
- math/transformation/vpHomogeneousMatrix.cpp
- math/transformation/vpPoseVector.cpp
- math/transformation/vpQuaternionVector.cpp
- math/transformation/vpRotationMatrix.cpp
- math/transformation/vpRotationVector.cpp
- math/transformation/vpRxyzVector.cpp
- math/transformation/vpRzyxVector.cpp
- math/transformation/vpRzyzVector.cpp
- math/transformation/vpThetaUVector.cpp
- math/transformation/vpTranslationVector.cpp
- math/transformation/vpVelocityTwistMatrix.cpp
- )
-
-IF(VISP_HAVE_LAPACK)
- LIST(APPEND SRC_MATH math/matrix/vpMatrix_cholesky.cpp)
-ENDIF()
-
-IF(VISP_HAVE_XML2)
- LIST(APPEND SRC_MATH math/transformation/vpXmlParserHomogeneousMatrix.cpp)
-ENDIF()
-
-SET (SRC_ROBOT
- robot/robot/vpRobot.cpp
- robot/robot/vpRobotTemplate.cpp
- robot/real-robot/afma4/vpAfma4.cpp
- robot/real-robot/afma6/vpAfma6.cpp
- robot/real-robot/biclops/vpBiclops.cpp
- robot/real-robot/ptu46/vpPtu46.cpp
- robot/real-robot/viper/vpViper.cpp
- robot/real-robot/viper/vpViper650.cpp
- robot/real-robot/viper/vpViper850.cpp
- robot/simulator-robot/vpRobotCamera.cpp
- robot/simulator-robot/vpRobotSimulator.cpp
- robot/simulator-robot/vpSimulatorCamera.cpp
- robot/simulator-robot/vpSimulatorPioneer.cpp
- robot/simulator-robot/vpSimulatorPioneerPan.cpp
- )
-
-IF(WIN32 OR VISP_HAVE_PTHREAD)
- list(APPEND SRC_ROBOT robot/simulator-robot/vpRobotWireFrameSimulator.cpp)
- list(APPEND SRC_ROBOT robot/simulator-robot/vpSimulatorAfma6.cpp)
- list(APPEND SRC_ROBOT robot/simulator-robot/vpSimulatorViper850.cpp)
-ENDIF()
-
-IF(VISP_HAVE_AFMA4)
- LIST(APPEND SRC_ROBOT robot/real-robot/afma4/vpRobotAfma4.cpp)
-ENDIF()
-IF(UNIX)
- LIST(APPEND SRC_ROBOT robot/real-robot/afma4/vpServolens.cpp)
-ENDIF()
-IF(VISP_HAVE_AFMA6)
- LIST(APPEND SRC_ROBOT robot/real-robot/afma6/vpRobotAfma6.cpp)
-ENDIF()
-IF(VISP_HAVE_BICLOPS)
- LIST(APPEND SRC_ROBOT robot/real-robot/biclops/vpRobotBiclopsController.cpp)
- LIST(APPEND SRC_ROBOT robot/real-robot/biclops/vpRobotBiclops.cpp)
-ENDIF()
-IF(VISP_HAVE_PTU46)
- LIST(APPEND SRC_ROBOT robot/real-robot/ptu46/vpRobotPtu46.cpp)
-ENDIF()
-IF(VISP_HAVE_PIONEER)
- LIST(APPEND SRC_ROBOT robot/real-robot/pioneer/vpRobotPioneer.cpp)
-ENDIF()
-IF(VISP_HAVE_VIPER650)
- LIST(APPEND SRC_ROBOT robot/real-robot/viper/vpRobotViper650.cpp)
-ENDIF()
-IF(VISP_HAVE_VIPER850)
- LIST(APPEND SRC_ROBOT robot/real-robot/viper/vpRobotViper850.cpp)
-ENDIF()
-
-SET (SRC_SERVO
- servo/vpAdaptiveGain.cpp
- servo/vpServo.cpp
- servo/vpServoData.cpp
- servo/vpServoDisplay.cpp
- )
-
-SET (SRC_SIMULATOR
- simulator/image-simulator/vpImageSimulator.cpp
- simulator/wireframe-simulator/vpWireFrameSimulator.cpp
- simulator/wireframe-simulator/core/vpArit.cpp
- simulator/wireframe-simulator/core/vpAritio.cpp
- simulator/wireframe-simulator/core/vpBound.cpp
- simulator/wireframe-simulator/core/vpBoundio.cpp
- simulator/wireframe-simulator/core/vpClipping.cpp
- simulator/wireframe-simulator/core/vpCoreDisplay.cpp
- simulator/wireframe-simulator/core/vpKeyword.cpp
- simulator/wireframe-simulator/core/vpLex.cpp
- simulator/wireframe-simulator/core/vpMyio.cpp
- simulator/wireframe-simulator/core/vpParser.cpp
- simulator/wireframe-simulator/core/vpProjection.cpp
- simulator/wireframe-simulator/core/vpRfstack.cpp
- simulator/wireframe-simulator/core/vpSkipio.cpp
- simulator/wireframe-simulator/core/vpTmstack.cpp
- simulator/wireframe-simulator/core/vpToken.cpp
- simulator/wireframe-simulator/core/vpViewio.cpp
- simulator/wireframe-simulator/core/vpVwstack.cpp
- )
-IF(VISP_HAVE_X11 OR VISP_HAVE_GDI OR VISP_HAVE_OPENCV OR VISP_HAVE_D3D9 OR VISP_HAVE_GTK)
- list(APPEND SRC_SIMULATOR simulator/coin-simulator/vpProjectionDisplay.cpp)
-ENDIF()
-IF(VISP_HAVE_COIN_AND_GUI)
- LIST(APPEND SRC_SIMULATOR simulator/coin-simulator/vpAR.cpp)
- LIST(APPEND SRC_SIMULATOR simulator/coin-simulator/vpSimulator.cpp)
- LIST(APPEND SRC_SIMULATOR simulator/coin-simulator/vpViewer.cpp)
-ENDIF()
-IF(VISP_HAVE_OGRE)
- LIST(APPEND SRC_SIMULATOR simulator/ogre-simulator/vpAROgre.cpp)
-ENDIF()
-
-SET (SRC_TOOLS
- tools/convert/vpConvert.cpp
- tools/geometry/vpPlane.cpp
- tools/geometry/vpRect.cpp
- tools/geometry/vpTriangle.cpp
- tools/geometry/vpPolygon.cpp
- tools/histogram/vpHistogram.cpp
- tools/histogram/vpHistogramPeak.cpp
- tools/histogram/vpHistogramValey.cpp
- tools/io/vpIoTools.cpp
- tools/io/vpKeyboard.cpp
- tools/io/vpParseArgv.cpp
- tools/time/vpTime.cpp
- )
-
-IF(VISP_HAVE_X11 OR VISP_HAVE_GDI OR VISP_HAVE_OPENCV OR VISP_HAVE_D3D9 OR VISP_HAVE_GTK)
- list(APPEND SRC_TOOLS tools/plot/vpPlot.cpp)
- list(APPEND SRC_TOOLS tools/plot/vpPlotCurve.cpp)
- list(APPEND SRC_TOOLS tools/plot/vpPlotGraph.cpp)
-ENDIF()
-IF(VISP_HAVE_XML2)
- LIST(APPEND SRC_TOOLS tools/xml/vpXmlParser.cpp)
-ENDIF()
-
-IF(VISP_HAVE_PARPORT)
- LIST(APPEND SRC_TOOLS tools/io/vpParallelPort.cpp)
-ENDIF()
-
-SET (SRC_TRACKING
- tracking/dots/vpDot2.cpp
- tracking/dots/vpDot.cpp
- tracking/feature-builder/vpFeatureBuilderEllipse.cpp
- tracking/feature-builder/vpFeatureBuilderLine.cpp
- tracking/feature-builder/vpFeatureBuilderPoint3D.cpp
- tracking/feature-builder/vpFeatureBuilderPoint.cpp
- tracking/feature-builder/vpFeatureBuilderPointPolar.cpp
- tracking/feature-builder/vpFeatureBuilderSegment.cpp
- tracking/feature-builder/vpFeatureBuilderVanishingPoint.cpp
- tracking/forward-projection/vpCircle.cpp
- tracking/forward-projection/vpCylinder.cpp
- tracking/forward-projection/vpForwardProjection.cpp
- tracking/forward-projection/vpLine.cpp
- tracking/forward-projection/vpPoint.cpp
- tracking/forward-projection/vpSphere.cpp
- tracking/general-tracking-issues/vpTracker.cpp
- tracking/moving-edges/vpMe.cpp
- tracking/moving-edges/vpMeEllipse.cpp
- tracking/moving-edges/vpMeLine.cpp
- tracking/moving-edges/vpMeSite.cpp
- tracking/moving-edges/vpMeTracker.cpp
- tracking/moving-edges/vpMeNurbs.cpp
-
- tracking/mbt/vpMbTracker.cpp
- tracking/mbt/vpMbtPolygon.cpp
- tracking/mbt/edge/vpMbEdgeTracker.cpp
- tracking/mbt/edge/vpMbtDistanceCircle.cpp
- tracking/mbt/edge/vpMbtDistanceCylinder.cpp
- tracking/mbt/edge/vpMbtDistanceLine.cpp
- tracking/mbt/edge/vpMbtMeEllipse.cpp
- tracking/mbt/edge/vpMbtMeLine.cpp
-
- tracking/moments/vpMoment.cpp
- tracking/moments/vpMomentAlpha.cpp
- tracking/moments/vpMomentArea.cpp
- tracking/moments/vpMomentAreaNormalized.cpp
- tracking/moments/vpMomentBasic.cpp
- tracking/moments/vpMomentCentered.cpp
- tracking/moments/vpMomentCInvariant.cpp
- tracking/moments/vpMomentCommon.cpp
- tracking/moments/vpMomentDatabase.cpp
- tracking/moments/vpMomentGravityCenter.cpp
- tracking/moments/vpMomentGravityCenterNormalized.cpp
- tracking/moments/vpMomentObject.cpp
-
- tracking/template-tracker/vpTemplateTracker.cpp
- tracking/template-tracker/ssd/vpTemplateTrackerSSD.cpp
- tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.cpp
- tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
- tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
- tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
- tracking/template-tracker/zncc/vpTemplateTrackerZNCC.cpp
- tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
- tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
- tracking/template-tracker/tools/vpTemplateTrackerBSpline.cpp
- tracking/template-tracker/tools/vpTemplateTrackerZone.cpp
- tracking/template-tracker/tools/vpTemplateTrackerTriangle.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarp.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.cpp
- tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.cpp
- )
-
-if(VISP_HAVE_XML2)
- list(APPEND SRC_TRACKING tracking/mbt/vpMbXmlParser.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/edge/vpMbtXmlParser.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/klt/vpMbtKltXmlParser.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.cpp)
-endif()
-
-if(VISP_HAVE_OPENCV)
- list(APPEND SRC_TRACKING detection/face/vpDetectorFace.cpp)
- list(APPEND SRC_TRACKING tracking/klt/vpKltOpencv.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/hybrid/vpMbEdgeKltTracker.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/klt/vpMbtDistanceKltPoints.cpp)
- list(APPEND SRC_TRACKING tracking/mbt/klt/vpMbKltTracker.cpp)
-endif()
-
-SET (SRC_VIDEO
- video/vpVideoReader.cpp
- video/vpVideoWriter.cpp
- )
-
-IF(VISP_HAVE_FFMPEG)
- LIST(APPEND SRC_VIDEO video/vpFFMPEG.cpp)
-ENDIF()
-
-SET (SRC_DEVICE_DISPLAY
- device/display/vpDisplay.cpp
- )
-
-IF(VISP_HAVE_GTK)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/vpDisplayGTK.cpp)
-ENDIF()
-IF(VISP_HAVE_OPENCV)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/vpDisplayOpenCV.cpp)
-ENDIF()
-IF(VISP_HAVE_X11)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/vpDisplayX.cpp)
-ENDIF()
-
-IF(WIN32)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpDisplayWin32.cpp)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpWin32Window.cpp)
-ENDIF()
-IF(VISP_HAVE_GDI)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpGDIRenderer.cpp)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpWin32API.cpp)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpDisplayGDI.cpp)
-ENDIF()
-IF(VISP_HAVE_D3D9)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpD3DRenderer.cpp)
- LIST(APPEND SRC_DEVICE_DISPLAY device/display/windows/vpDisplayD3D.cpp)
-ENDIF()
-
-SET (SRC_VISUAL_FEATURE
- visual-feature/vpBasicFeature.cpp
- visual-feature/vpFeatureDepth.cpp
- visual-feature/vpFeatureDisplay.cpp
- visual-feature/vpFeatureEllipse.cpp
- visual-feature/vpFeatureLine.cpp
- visual-feature/vpFeatureLuminance.cpp
- visual-feature/vpFeatureMoment.cpp
- visual-feature/vpFeatureMomentAlpha.cpp
- visual-feature/vpFeatureMomentArea.cpp
- visual-feature/vpFeatureMomentAreaNormalized.cpp
- visual-feature/vpFeatureMomentBasic.cpp
- visual-feature/vpFeatureMomentCentered.cpp
- visual-feature/vpFeatureMomentCInvariant.cpp
- visual-feature/vpFeatureMomentCommon.cpp
- visual-feature/vpFeatureMomentDatabase.cpp
- visual-feature/vpFeatureMomentGravityCenter.cpp
- visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
- visual-feature/vpFeaturePoint3D.cpp
- visual-feature/vpFeaturePoint.cpp
- visual-feature/vpFeaturePointPolar.cpp
- visual-feature/vpFeatureThetaU.cpp
- visual-feature/vpFeatureTranslation.cpp
- visual-feature/vpFeatureVanishingPoint.cpp
- visual-feature/vpFeatureSegment.cpp
- visual-feature/vpGenericFeature.cpp
- )
-
-SET (SRC_NETWORK
- network/vpNetwork.cpp
- network/vpServer.cpp
- network/vpClient.cpp
- network/vpRequest.cpp
- )
-
-SET (SRC_ALL
- ${SRC_CAMERA}
- ${SRC_COMPUTER_VISION}
- ${SRC_EXCEPTION}
- ${SRC_DETECTION}
- ${SRC_DEVICE_DISPLAY}
- ${SRC_DEVICE_FRAMEGRABBER}
- ${SRC_DEVICE_KINECT}
- ${SRC_DEVICE_LASERSCANNER}
- ${SRC_DEVICE_LIGHT}
- ${SRC_IMAGE}
- ${SRC_KEY_POINT}
- ${SRC_MATH}
- ${SRC_ROBOT}
- ${SRC_SERVO}
- ${SRC_SIMULATOR}
- ${SRC_TOOLS}
- ${SRC_TRACKING}
- ${SRC_VIDEO}
- ${SRC_VISUAL_FEATURE}
- ${SRC_NETWORK}
- )
-
diff --git a/CTestConfig.cmake b/CTestConfig.cmake
index e35b40d..4d3982d 100644
--- a/CTestConfig.cmake
+++ b/CTestConfig.cmake
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CTestConfig.cmake,v 1.9 2008-12-11 13:19:44 fspindle Exp $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -52,12 +49,12 @@ set(CTEST_DROP_SITE_CDASH TRUE)
# on the ViSP dashboard http://cdash.irisa.fr/CDash/
#--------------------------------------------------------------------
# Start with the short system name, e.g. "Linux", "FreeBSD" or "Windows"
-IF(BUILDNAME)
- SET(BUILDNAME "${BUILDNAME}-${CMAKE_SYSTEM_NAME}")
-ELSE(BUILDNAME)
+if(BUILDNAME)
+ set(BUILDNAME "${BUILDNAME}-${CMAKE_SYSTEM_NAME}")
+else(BUILDNAME)
# To suppress the first space if BUILDNAME is not set
- SET(BUILDNAME "${CMAKE_SYSTEM_NAME}")
-ENDIF(BUILDNAME)
+ set(BUILDNAME "${CMAKE_SYSTEM_NAME}")
+endif(BUILDNAME)
# Add i386 or amd64
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@@ -89,56 +86,56 @@ elseif(MINGW)
set(BUILDNAME "${BUILDNAME}-mingw")
else()
# g++
- SET(BUILDNAME "${BUILDNAME}-${CMAKE_BASE_NAME}")
+ set(BUILDNAME "${BUILDNAME}-${CMAKE_BASE_NAME}")
endif()
# Find out the version of gcc being used.
-IF(CMAKE_COMPILER_IS_GNUCC)
- EXEC_PROGRAM(${CMAKE_CXX_COMPILER}
+if(CMAKE_COMPILER_IS_GNUCC)
+ exec_program(${CMAKE_CXX_COMPILER}
ARGS -dumpversion
OUTPUT_VARIABLE COMPILER_VERSION
)
- #MESSAGE("COMPILER_VERSION 1: ${COMPILER_VERSION}")
- STRING(REGEX REPLACE ".* ([0-9])\\.([0-9])\\.[0-9].*" "\\1\\2"
+ #message("COMPILER_VERSION 1: ${COMPILER_VERSION}")
+ string(REGEX REPLACE ".* ([0-9])\\.([0-9])\\.[0-9].*" "\\1\\2"
COMPILER_VERSION ${COMPILER_VERSION})
- #MESSAGE("COMPILER_VERSION 2: ${COMPILER_VERSION}")
+ #message("COMPILER_VERSION 2: ${COMPILER_VERSION}")
- SET(BUILDNAME "${BUILDNAME}${COMPILER_VERSION}")
+ set(BUILDNAME "${BUILDNAME}${COMPILER_VERSION}")
-ENDIF(CMAKE_COMPILER_IS_GNUCC)
+endif(CMAKE_COMPILER_IS_GNUCC)
# Add the type of library generation, e.g. "Dynamic or Static"
-IF(BUILD_SHARED_LIBS)
- SET(BUILDNAME "${BUILDNAME}-Dyn")
-ELSE(BUILD_SHARED_LIBS)
- SET(BUILDNAME "${BUILDNAME}-Sta")
-ENDIF(BUILD_SHARED_LIBS)
+if(BUILD_SHARED_LIBS)
+ set(BUILDNAME "${BUILDNAME}-Dyn")
+else(BUILD_SHARED_LIBS)
+ set(BUILDNAME "${BUILDNAME}-Sta")
+endif(BUILD_SHARED_LIBS)
# Add the build type, e.g. "Debug, Release..."
-IF(CMAKE_BUILD_TYPE)
- SET(BUILDNAME "${BUILDNAME}-${CMAKE_BUILD_TYPE}")
-ENDIF(CMAKE_BUILD_TYPE)
+if(CMAKE_BUILD_TYPE)
+ set(BUILDNAME "${BUILDNAME}-${CMAKE_BUILD_TYPE}")
+endif(CMAKE_BUILD_TYPE)
#---- Robots ----
# Add specific Afma4 robots
-IF(VISP_HAVE_AFMA4)
- SET(BUILDNAME "${BUILDNAME}-Afma4")
-ENDIF(VISP_HAVE_AFMA4)
+if(VISP_HAVE_AFMA4)
+ set(BUILDNAME "${BUILDNAME}-Afma4")
+endif(VISP_HAVE_AFMA4)
# Add specific Afma6 robots
-IF(VISP_HAVE_AFMA6)
- SET(BUILDNAME "${BUILDNAME}-Afma6")
-ENDIF(VISP_HAVE_AFMA6)
+if(VISP_HAVE_AFMA6)
+ set(BUILDNAME "${BUILDNAME}-Afma6")
+endif(VISP_HAVE_AFMA6)
# Add specific Ptu46 robots
-IF(VISP_HAVE_PTU46)
- SET(BUILDNAME "${BUILDNAME}-Ptu46")
-ENDIF(VISP_HAVE_PTU46)
+if(VISP_HAVE_PTU46)
+ set(BUILDNAME "${BUILDNAME}-Ptu46")
+endif(VISP_HAVE_PTU46)
# Add specific Biclops robots
-IF(VISP_HAVE_BICLOPS)
- SET(BUILDNAME "${BUILDNAME}-Biclops")
-ENDIF(VISP_HAVE_BICLOPS)
+if(VISP_HAVE_BICLOPS)
+ set(BUILDNAME "${BUILDNAME}-Biclops")
+endif(VISP_HAVE_BICLOPS)
# Add specific Pioneer robots
if(VISP_HAVE_PIONEER)
@@ -147,44 +144,44 @@ endif()
#---- Framegrabers ----
# Firewire dc1394-2.x
-IF(VISP_HAVE_DC1394_2)
- SET(BUILDNAME "${BUILDNAME}-dc1394")
-ENDIF(VISP_HAVE_DC1394_2)
+if(VISP_HAVE_DC1394)
+ set(BUILDNAME "${BUILDNAME}-dc1394")
+endif(VISP_HAVE_DC1394)
# Video 4 linux 2 (V4L2)
-IF(VISP_HAVE_V4L2)
- SET(BUILDNAME "${BUILDNAME}-v4l2")
-ENDIF(VISP_HAVE_V4L2)
+if(VISP_HAVE_V4L2)
+ set(BUILDNAME "${BUILDNAME}-v4l2")
+endif(VISP_HAVE_V4L2)
# Directshow
-IF(VISP_HAVE_DIRECTSHOW)
- SET(BUILDNAME "${BUILDNAME}-dshow")
-ENDIF(VISP_HAVE_DIRECTSHOW)
-IF(VISP_HAVE_CMU1394)
- SET(BUILDNAME "${BUILDNAME}-CMU1394")
-ENDIF(VISP_HAVE_CMU1394)
-IF(VISP_HAVE_LIBFREENECT)
- SET(BUILDNAME "${BUILDNAME}-freenect")
-ENDIF()
-IF(VISP_HAVE_LIBUSB_1)
- SET(BUILDNAME "${BUILDNAME}-usb")
-ENDIF()
+if(VISP_HAVE_DIRECTSHOW)
+ set(BUILDNAME "${BUILDNAME}-dshow")
+endif(VISP_HAVE_DIRECTSHOW)
+if(VISP_HAVE_CMU1394)
+ set(BUILDNAME "${BUILDNAME}-CMU1394")
+endif(VISP_HAVE_CMU1394)
+if(VISP_HAVE_LIBFREENECT)
+ set(BUILDNAME "${BUILDNAME}-freenect")
+endif()
+if(VISP_HAVE_LIBUSB_1)
+ set(BUILDNAME "${BUILDNAME}-usb")
+endif()
#---- Video-devices ----
# X11
-IF(VISP_HAVE_X11)
- SET(BUILDNAME "${BUILDNAME}-X11")
-ENDIF(VISP_HAVE_X11)
+if(VISP_HAVE_X11)
+ set(BUILDNAME "${BUILDNAME}-X11")
+endif(VISP_HAVE_X11)
# GTK
-IF(VISP_HAVE_GTK)
- SET(BUILDNAME "${BUILDNAME}-gtk")
-ENDIF(VISP_HAVE_GTK)
+if(VISP_HAVE_GTK)
+ set(BUILDNAME "${BUILDNAME}-gtk")
+endif(VISP_HAVE_GTK)
# GDI (Windows Graphics Device Interface)
-IF(VISP_HAVE_GDI)
- SET(BUILDNAME "${BUILDNAME}-gdi")
-ENDIF(VISP_HAVE_GDI)
+if(VISP_HAVE_GDI)
+ set(BUILDNAME "${BUILDNAME}-gdi")
+endif(VISP_HAVE_GDI)
# D3D (Direct3D9)
-IF(VISP_HAVE_D3D9)
- SET(BUILDNAME "${BUILDNAME}-Direct3D")
-ENDIF(VISP_HAVE_D3D9)
+if(VISP_HAVE_D3D9)
+ set(BUILDNAME "${BUILDNAME}-Direct3D")
+endif(VISP_HAVE_D3D9)
# OpenCV
if(VISP_HAVE_OPENCV)
if(OpenCV_VERSION)
@@ -200,56 +197,56 @@ endif(VISP_HAVE_OPENCV)
#---- Mathematics ----
# Lapack (Linear Algebra PACKage)
-IF(VISP_HAVE_LAPACK)
- SET(BUILDNAME "${BUILDNAME}-lapack")
-ENDIF()
+if(VISP_HAVE_LAPACK_C)
+ set(BUILDNAME "${BUILDNAME}-lapack")
+endif()
# GSL (Gnu Scientific Library)
-IF(VISP_HAVE_GSL)
- SET(BUILDNAME "${BUILDNAME}-gsl")
-ENDIF(VISP_HAVE_GSL)
+if(VISP_HAVE_GSL)
+ set(BUILDNAME "${BUILDNAME}-gsl")
+endif(VISP_HAVE_GSL)
#---- Simulator ----
# Ogre
-IF(VISP_HAVE_OGRE)
- SET(BUILDNAME "${BUILDNAME}-Ogre")
-ENDIF()
-IF(VISP_HAVE_OIS)
- SET(BUILDNAME "${BUILDNAME}-OIS")
-ENDIF()
+if(VISP_HAVE_OGRE)
+ set(BUILDNAME "${BUILDNAME}-Ogre")
+endif()
+if(VISP_HAVE_OIS)
+ set(BUILDNAME "${BUILDNAME}-OIS")
+endif()
# Coin
-IF(VISP_HAVE_COIN)
- SET(BUILDNAME "${BUILDNAME}-Coin")
-ENDIF(VISP_HAVE_COIN)
+if(VISP_HAVE_COIN3D)
+ set(BUILDNAME "${BUILDNAME}-Coin")
+endif(VISP_HAVE_COIN3D)
# SoQt
-IF(VISP_HAVE_SOQT)
- SET(BUILDNAME "${BUILDNAME}-SoQt")
-ENDIF(VISP_HAVE_SOQT)
+if(VISP_HAVE_SOQT)
+ set(BUILDNAME "${BUILDNAME}-SoQt")
+endif(VISP_HAVE_SOQT)
# Qt
-IF(VISP_HAVE_QT)
- SET(BUILDNAME "${BUILDNAME}-Qt${DESIRED_QT_VERSION}")
-ENDIF(VISP_HAVE_QT)
+if(VISP_HAVE_QT)
+ set(BUILDNAME "${BUILDNAME}-Qt${DESIRED_QT_VERSION}")
+endif(VISP_HAVE_QT)
# SoWin
-IF(VISP_HAVE_SOWIN)
- SET(BUILDNAME "${BUILDNAME}-SoWin")
-ENDIF(VISP_HAVE_SOWIN)
+if(VISP_HAVE_SOWIN)
+ set(BUILDNAME "${BUILDNAME}-SoWin")
+endif(VISP_HAVE_SOWIN)
# SoXt
-IF(VISP_HAVE_SOXT)
- SET(BUILDNAME "${BUILDNAME}-SoXt")
-ENDIF(VISP_HAVE_SOXT)
+if(VISP_HAVE_SOXT)
+ set(BUILDNAME "${BUILDNAME}-SoXt")
+endif(VISP_HAVE_SOXT)
#---- Images ----
-IF(VISP_HAVE_FFMPEG)
- SET(BUILDNAME "${BUILDNAME}-ffmpeg")
-ENDIF(VISP_HAVE_FFMPEG)
-IF(VISP_HAVE_LIBJPEG)
- SET(BUILDNAME "${BUILDNAME}-jpeg")
-ENDIF(VISP_HAVE_LIBJPEG)
-IF(VISP_HAVE_LIBPNG)
- SET(BUILDNAME "${BUILDNAME}-png")
-ENDIF(VISP_HAVE_LIBPNG)
-IF(VISP_HAVE_ZLIB)
- SET(BUILDNAME "${BUILDNAME}-zlib")
-ENDIF()
+if(VISP_HAVE_FFMPEG)
+ set(BUILDNAME "${BUILDNAME}-ffmpeg")
+endif(VISP_HAVE_FFMPEG)
+if(VISP_HAVE_JPEG)
+ set(BUILDNAME "${BUILDNAME}-jpeg")
+endif(VISP_HAVE_JPEG)
+if(VISP_HAVE_PNG)
+ set(BUILDNAME "${BUILDNAME}-png")
+endif(VISP_HAVE_PNG)
+#if(VISP_HAVE_ZLIB)
+# set(BUILDNAME "${BUILDNAME}-zlib")
+#endif()
#---- Misc ----
# XML
@@ -274,16 +271,21 @@ endif()
#---- Special compiler flags ----
if(ACTIVATE_WARNING_STRICT_OVERFLOW)
- SET(BUILDNAME "${BUILDNAME}-Wov")
+ set(BUILDNAME "${BUILDNAME}-Wov")
endif()
if(ACTIVATE_WARNING_FLOAT_EQUAL)
- SET(BUILDNAME "${BUILDNAME}-Weq")
+ set(BUILDNAME "${BUILDNAME}-Weq")
endif()
if(USE_CPP11)
- SET(BUILDNAME "${BUILDNAME}-c11")
+ set(BUILDNAME "${BUILDNAME}-c11")
endif()
if(MOMENTS_COMBINE_MATRICES)
- SET(BUILDNAME "${BUILDNAME}-Moment")
+ set(BUILDNAME "${BUILDNAME}-Moment")
+endif()
+
+#---- Suffix contrib ----
+if(VISP_CONTRIB_MODULES_PATH)
+ set(BUILDNAME "${BUILDNAME}-contrib")
endif()
-#MESSAGE("BUILDNAME=${BUILDNAME}")
+#message("BUILDNAME=${BUILDNAME}")
diff --git a/ChangeLog b/ChangeLog
index 9d8a0a7..6e5a8eb 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -6,6 +6,77 @@
http://team.inria.fr/lagadic
+ViSP 3.0.0 (released December 18th, 2015)
+ - New features
+ . Compatibility with Windows 8.1 and 10
+ . Compatibility with Microsoft Visual C++ 2015 (MSCV 14)
+ . Compatibility with Odroid XU4
+ . Source code migrates to GitHub: http://github.org/lagadic/visp
+ . Introduce Travis continuous integration
+ . New website: http://visp.inria.fr
+ . Reorganize source in modules (core, vision, mbt, tt, …) with a library
+ per module named libvisp_<module>. Keep compat with previous releases.
+ . Allow to link contributions to ViSP source code using
+ VISP_CONTRIB_MODULES_PATH CMake var;
+ see http://github.org/lagadic/visp_contrib
+ . Model-based tracker:
+ + New visibility tests based on the scanline rendering algorithm.
+ + Improve Ogre3D visibility tests in order to use it in a probalistic
+ approach via the setNbRayCastingAttemptsForVisibility()
+ and setGoodNbRayCastingAttemptsRatio() functions.
+ + Introduction of cylinder tracking with the model-based trackers using
+ key points.
+ + Improve the setPose() functionnality to use the given pose as a
+ prediction of the key points position while using vpMbKltTracker and
+ vpMbEdgeKltTracker.
+ . Introduce vpArray2D template class that is inherited by all matrices and
+ vectors.
+ . vpKeyPoint class:
+ + Full code compatibility with OpenCV 3.0.
+ + Add support to the new Features introduce in OpenCV 3.0 and contrib
+ module releases.
+ + Add possibility to detect keypoints on an image pyramid with OpenCV 3.0.
+ + Add possibility to change the image type (jpeg, png, ppm, pgm) when
+ saving a learning file.
+ + Add OpenMP support for Affine SIFT.
+ + Add single matching filter (discard pairs of keypoints where multiple
+ query keypoints are matched to the same train keypoints).
+ + Add possibility to match train keypoints to query keypoints (could be
+ useful when we learn only on one image, it allows to avoid to match
+ background keypoints with train keypoints).
+ + Optimize the FLANN matching by learning once the train descriptors
+ instead of doing it at each call of the matching function (noticeable
+ with large train descriptors).
+ + Add possibility to use the FLANN matcher with binary descriptors.
+ + Fix displayMatching function and fix small memory leak in loadLearningData.
+ + Should be possible to read / write learning files in binary mode regardless
+ of the endianness of the host (need to be tested on a real case).
+ + Save learning files in XML mode with the full precision (according to
+ the data type).
+ . and a lot of improvements and fixes
+ - Tutorials
+ . New tutorial: Bridge over OpenCV
+ . New tutorial: Installation from source with Homebrew
+ . Update all the installation tutorials since GitHub migration and modules
+ introduction and make more explicit 3rd party installation
+ . Update tutorial: Markerless 3D model-based tracking
+ - Bug fixed
+ . [18748] Ogre and wireframe resources not found after ViSP installation
+ . [18823] vpMatrix::kernel method returns false result if matrix has
+ less rows than columns
+ . [18939] vpParseArgv::ARGV_CONSTANT segfault
+ . [19312] Model-based tracker unable to initialize the number of cylinder
+ or circle from cao file
+ . [19326] segfault when using vpDisplayX::init(unsigned int, unsigned int, ...)
+ . [19368] Invalid rotation parameters in vpSimulatorViper850::getPosition()
+ . [#3] Issue when comparing two vpRGBa
+ . [#5] Unable to get calibration matrix K when model with distorsion is
+ used
+ . [#6] Issue with vpMath::round()
+ . [#18] Issues with vpColVector::median() + with assignment operator of
+ vpRowVector + with vpRowVector::normalize() function
+
+----------------------------------------------
ViSP 2.10.0 (released February 13th, 2015)
- New features
. New object detection and localisation capabilities that allow to
diff --git a/INSTALL.txt b/INSTALL.txt
index f39d4f0..5c349f1 100644
--- a/INSTALL.txt
+++ b/INSTALL.txt
@@ -1,8 +1,8 @@
- ViSP-2.7.0
+ ViSP
Visual Servoing Platform
- Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- www: http://www.irisa.fr/lagadic
+ Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ http://visp.inria.fr
@@ -56,14 +56,14 @@ directly if you prefer command line instead of graphical interface.
(5) install:
> make install
-Copy headers in ${CMAKE_INSTALL_PREFIX}/include/visp
+Copy headers in ${CMAKE_INSTALL_PREFIX}/include
Copy library in ${CMAKE_INSTALL_PREFIX}/lib
Copy visp-config shell script in ${CMAKE_INSTALL_PREFIX}/bin
(6) uninstall:
> make uninstall
Dual from install.
-Remove headers in ${CMAKE_INSTALL_PREFIX}/include/visp
+Remove headers in ${CMAKE_INSTALL_PREFIX}/include
Remove library and cmake files in ${CMAKE_INSTALL_PREFIX}/lib
Remove visp-config shell script in ${CMAKE_INSTALL_PREFIX}/bin
@@ -71,17 +71,8 @@ Remove visp-config shell script in ${CMAKE_INSTALL_PREFIX}/bin
> make clean
The "make clean" target will remove files generated by the compiler and linker.
-(8) distclean:
-> sh ./distclean.sh
-CMake does not generate a "make distclean" target (see the cmake FAQ). CMake
-generates many files related to the build system, but since CMakeLists.txt
-files can run scripts and other arbitrary commands, there is no way it can keep
-track of exactly which files are generated as part of running CMake. So we
-have developped a distclean shell script (working under Unix platforms) that
-removes these files related to the build system.
-
(8) documentation
-> make html-doc
+> make visp_doc
Generate doxygen html documentation in doc/html.
See README.txt for further information.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..dd4973f
--- /dev/null
+++ b/README.md
@@ -0,0 +1,15 @@
+### ViSP: Open source Visual Servoing Platform
+
+[](https://travis-ci.org/lagadic/visp)
+
+#### Resources
+- Homepage: http://visp.inria.fr
+- Wiki: https://github.com/lagadic/visp/wiki
+- Code documentation: http://visp-doc.inria.fr/doxygen/visp-daily
+- Q&A forum: http://gforge.inria.fr/forum/?group_id=397
+- Issue tracking: https://github.com/lagadic/visp/issues
+
+#### Contributing
+
+Please read before starting work on a pull request: http://visp.inria.fr/contributing-code/
+
diff --git a/README.txt b/README.txt
deleted file mode 100644
index db3a910..0000000
--- a/README.txt
+++ /dev/null
@@ -1,274 +0,0 @@
- ViSP-2.10.0
- Visual Servoing Platform
-
- Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- www: http://www.irisa.fr/lagadic
-
-
-
-This project is using the CMake build system.
-
-CMake is a complete stand-alone platform-independant build-system
-replacing autotools (autoconf/autoheader/automake/libtools) completely.
-It depends just on installed cmake (tested with cmake cvs version). It
-needs a cmake 2.6.x or more recent version of cmake.
-See http://www.cmake.org for details.
-
-USAGE:
-=====
-
-1. Install the newest cmake from www.cmake.org
-
- See INSTALL file for further information.
-
-2. Check out/install ViSP source code
-
- Checkout, copy or unzip the ViSP tar ball in <visp source dir>.
-
- For building you have got the option of "in-source" or "out-of-source"
- builds. CMake developpers strongly recommend using an "out-of-source" build
- which never writes any files to the source tree. Using a separate source and
- build tree greatly reduces the need for "make clean" and "make distclean"
- targets.
-
-2.1 In-source build under Unix platforms:
-
- cd <visp source dir>;
- ccmake .
- or for defaults: cmake .
- make
- make install
-
- The make install step is needed if you want to use ViSP in your own
- projects. To do the installation, you don't need to be root.
-
-2.2 Out-of-source build under Unix platforms:
-
- The out-of-source build is very useful for different parallel build
- configurations:
-
- mkdir <visp build dir>
- cd <visp build dir>
- ccmake <visp source dir>
- or for defaults: cmake <source dir>
- make
- make install
-
- The make install step is needed if you want to use ViSP in your own
- projects. To do the installation, you don't need to be root.
-
-
-2.3 Build under Win32
-
- Use cmake or CMakeSetup Gui for in-source or out-of-source build.
- Set environment variables and proceed as described for Unix.
-
-
-
-Important Files:
-===============
-
-- CMakeLists.txt:
- Overall configuration file. Detect third party libraries (X11, GTK, ...).
-
-- CMakeHeaderFileList.cmake:
- The list of all the headers (*.h) or (*.t.cpp for templates) to copy
- in <visp source dir>/include/visp directory or to install in
- <visp install prefix>/include/visp
-
-- CMakeSourceFileList.cmake:
- The list of all the sources (*.cpp) used to build the ViSP library.
-
-- src/CMakeLists.txt:
- Build the library in <visp source dir>/lib.
-
-- include/vpConfig.h.in versus include/visp/vpConfig.h:
- vpConfig.h.in is the source for vpConfig.h.
- vpConfig.h is generated in CMakeList.txt by the command
- CONFIGURE_FILE(${VISP_SOURCE_DIR}/include/vpConfig.h.cmake
- ${VISP_INCLUDE_DIR}/vpConfig.h
- vpConfig.h contains preprocessor defines like VISP_HAVE_X11 and is .
- included in most header files (*.h) to allow system dependent code usage
-
-- CMakeModule/visp-config.in versus bin/visp-config:
- visp-config is a shell script for third party projects not using cmake to
- get hold of prefix, cflags, libs and version.
- It is generated from visp-config.in by the module
- CMakeModules/GenerateConfigScript.cmake in CMakeList.txt.
-
-- lib/VISPBuildSettings.cmake:
- SET commands for most important variables, i.e.
- PROJECT_NAME
- CMAKE_VERSION
- C(XX)_COMPILER
- C(XX)_FLAGS(_*)
- BUILD_TYPE
- BUILD_TOOL
- It can be included from third cmake projects.
-
-- CMakeModules/VISPConfig.cmake.in / lib/VISPConfig.cmake:
- VISPConfig.cmake contains SET command for some basic variables like
- VISP_INCLUDE_DIR, VISP_LINK_DIRECTORIES, VISP_BUILD_SETTINGS_FILE,
- VISP_SOURCE_DIR, VISP_LIBRARIES and VISP_USE_FILE
- VISPConfig.cmake can be used by by third cmake projects to make these
- variables available.
- VISPConfig.cmake.in is the source for VISPConfig.cmake
-
-
-HOWTO:
-======
-
-1. Change configuration:
- ---------------------
-
- You can change configuration easily with ccmake GUI.
- ccmake <source dir>
- type 't' to toggle display of advanced variables
- or
- cmake -LA <source dir>
- to have the list of all configuration variables.
-
-
-2. Do a optimized build:
- --------------------
- cmake -DCMAKE_BUILD_TYPE=Release <visp source dir>
-
- or use ccmake <visp source dir> to set CMAKE_BUILD_TYPE.
-
- Available build types are: Release, Debug, RelWithDebInfo, MinSizeRel.
- The build process uses specific build variable CMAKE_CXX_FLAGS_*.
- For example Release build uses CMAKE_CXX_FLAGS_RELEASE, while Debug
- build uses CMAKE_CXX_FLAGS_DEBUG.
-
- The flags according to the different build types can be seen with
- cmake -LA <source_dir>
-
-
-3. Build shared libraries:
- -----------------------
- cmake -DBUILD_SHARED_LIBS=ON <visp source dir>
-
- or use ccmake <visp source dir> to set BUILD_SHARED_LIBS.
-
- Produce shared libraries (.so).
-
-
-4. Build static libraries:
- -----------------------
- cmake -DBUILD_SHARED_LIBS=OFF <visp source dir>
-
- or use ccmake <visp source dir> to set BUILD_SHARED_LIBS.
-
- Produce static libraries (.a).
-
-
-5. Add more files to clean target:
- ------------------------------
- SET_DIRECTORY_PROPERTIES(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES
- "file_to_remove")
-
-
-
-6. Use ViSP with another cmake project:
- -----------------------------------
- Include the following into your CMakeList.txt:
-
- FIND_PACKAGE(VISP REQUIRED)
- IF(VISP_FOUND)
- INCLUDE(${VISP_USE_FILE})
- ENDIF(VISP_FOUND)
-
- With CMake GUI set the environment variable VISP_DIR to
- <visp install prefix>/lib
- or using a command line run:
- `cmake -DVISP_DIR=<visp install prefix>/lib <project source dir>`
-
-
-7. Use ViSP with autoconf (autotools):
- -----------------------------------
- Copy the M4 macro file `macro/have_visp.m4` in your project.
-
- cp macro/have_visp.m4 <your project home dir>/macro
-
- This macro check if <visp install prefix>/bin/visp-config shell script
- is available (see below how to use ViSP with a common Makefile).
-
- In your configure.ac project file add lines like:
-
- AC_HAVE_VISP_IFELSE(have_visp=yes,have_visp=no)
- if test "x$have_visp" = "xyes"; then
- CXXFLAGS="$CXXFLAGS $ac_visp_cflags "
- LIBS="$LIBS $ac_visp_libs "
- fi
-
- cd <your project home dir>
- aclocal -I macro # updates the aclocal.m4 file
- autoconf
- configure --with-visp-install-bin=<visp install prefix>/bin
-
-
-8. Use ViSP with a common Makefile:
- -------------------------------
- a: Install ViSP:
- - configure ViSP using cmake, particularly set the ViSP install prefix
- (cmake -DCMAKE_INSTALL_PREFIX:PATH=<visp install prefix>)
- - build ViSP:
- make
- - install ViSP by:
- make install
- b: Make sure, the visp-config shell script is found by the makefile:
- export PATH=$PATH:<visp install prefix>/bin
- c: Use `visp-config --prefix/--cflags/--libs/--version`
- to get the compiler/linker flags needed to use ViSP
-
-
-9. View compiler and linker options used:
- -------------------------------------
- cmake -DCMAKE_VERBOSE_MAKEFILE=ON <visp source dir>
-
- Generate more 'verbose' makefile including compiler calls instead of
- default "SILENT" .
-
-
-10. View all available options/variables:
- ------------------------------------
- ccmake <source dir>
- type 't' to toggle display of advanced variables
- or
- cmake -LA <source dir>
-
-
-11. Remove Cache:
- -------------
- cd <visp build dir>
- rm CMakeCache.txt
-
-
-12. Make a distclean:
- -----------------
- CMake does not generate a "make distclean" target (see the cmake FAQ). CMake
- generates many files related to the build system, but since CMakeLists.txt
- files can run scripts and other arbitrary commands, there is no way it can
- keep track of exactly which files are generated as part of running CMake.
-
-
- So, for in-source build under Unix platforms, we have developped a distclean
- shell script (working only under Unix platforms) that removes these files
- related to the build system. To remove some intermediate files related to
- the build system: sh ./distclean.sh
-
- For out-of-source build, just remove the <visp build dir> tree.
-
-13. Where to found documentation about CMake:
- ----------------------------------------
- Almost complete documentation of cmake commands:
- cmake --help-full and http://www.cmake.org
-
- The 250-page book Mastering CMake by Ken Martin and Bill Hoffman, ISBN
- 1-930934-16-5, published by Kitware, Inc.
-
-------------------------
-
-Fabien Spindler
-
-
diff --git a/ViSP-third-party.txt.cmake b/ViSP-third-party.txt.cmake
deleted file mode 100644
index f8c19ea..0000000
--- a/ViSP-third-party.txt.cmake
+++ /dev/null
@@ -1,60 +0,0 @@
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : ${VISP_HAVE_GSL_FOUND}
- Lapack/blas : ${VISP_HAVE_LAPACK_FOUND}
-Simulator:
- Ogre simulator : ${VISP_HAVE_OGRE_FOUND}
- \- Ogre3D : ${VISP_HAVE_OGRE_FOUND}
- \- OIS : ${VISP_HAVE_OIS_FOUND}
- Coin simulator : ${VISP_HAVE_COIN_AND_GUI_FOUND}
- \- Coin3D : ${VISP_HAVE_COIN_FOUND}
- \- SoWin : ${VISP_HAVE_SOWIN_FOUND}
- \- SoXt : ${VISP_HAVE_SOXT_FOUND}
- \- SoQt : ${VISP_HAVE_SOQT_FOUND}
- \- Qt4 : ${VISP_HAVE_QT4_FOUND}
- \- Qt3 : ${VISP_HAVE_QT3_FOUND}
-Robots
- Afma6 : ${VISP_HAVE_AFMA6_FOUND}
- Afma4 : ${VISP_HAVE_AFMA4_FOUND}
- Biclops : ${VISP_HAVE_BICLOPS_FOUND}
- Ptu46 : ${VISP_HAVE_PTU46_FOUND}
- Pioneer : ${VISP_HAVE_PIONEER_FOUND}
- Viper S650 : ${VISP_HAVE_VIPER650_FOUND}
- Viper S850 : ${VISP_HAVE_VIPER850_FOUND}
-Video devices (display)
- X11 : ${VISP_HAVE_X11_FOUND}
- GTK : ${VISP_HAVE_GTK_FOUND}
- OpenCV : ${VISP_HAVE_OPENCV_FOUND}
- GDI : ${VISP_HAVE_GDI_FOUND}
- Direct3D : ${VISP_HAVE_D3D9_FOUND}
-Framegrabbers
- Firewire libdc1394-2.x : ${VISP_HAVE_DC1394_2_FOUND}
- Video For Linux Two : ${VISP_HAVE_V4L2_FOUND}
- DirectShow : ${VISP_HAVE_DIRECTSHOW_FOUND}
- CMU 1394 Digital Camera SDK : ${VISP_HAVE_CMU1394_FOUND}
- OpenCV : ${VISP_HAVE_OPENCV_FOUND}
-Specific devices
- Yarp : ${VISP_HAVE_YARP_FOUND}
- Kinect : ${VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES_FOUND}
- \-libfreenect : ${VISP_HAVE_LIBFREENECT_FOUND}
- \-libusb-1.0 : ${VISP_HAVE_LIBUSB_1_FOUND}
- \-pthread : ${VISP_HAVE_PTHREAD_FOUND}
-Video and image Read/Write:
- FFMPEG : ${VISP_HAVE_FFMPEG_FOUND}
- libjpeg : ${VISP_HAVE_LIBJPEG_FOUND}
- libpng : ${VISP_HAVE_LIBPNG_FOUND}
-Misc:
- XML2 : ${VISP_HAVE_XML2_FOUND}
- pthread : ${VISP_HAVE_PTHREAD_FOUND}
- OpenMP : ${VISP_HAVE_OPENMP_FOUND}
- zbar : ${VISP_HAVE_ZBAR_FOUND}
- dmtx : ${VISP_HAVE_DMTX_FOUND}
-Documentation:
- Doxygen : ${VISP_HAVE_DOXYGEN_FOUND}
- Graphviz dot : ${VISP_HAVE_DOT_FOUND}
-ViSP built with C++11 features: ${VISP_HAVE_CPP11_COMPATIBILITY_FOUND}
-
diff --git a/cmake/AddExtraCompilationFlags.cmake b/cmake/AddExtraCompilationFlags.cmake
new file mode 100644
index 0000000..71e3474
--- /dev/null
+++ b/cmake/AddExtraCompilationFlags.cmake
@@ -0,0 +1,201 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP overall configuration file. Add extra compilation flags.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+macro(vp_add_extra_compilation_flags)
+ # By default set release configuration
+ if(NOT CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE "Release" CACHE String "Choose the type of build, options are: None Debug Release" FORCE)
+ endif()
+
+ include(CheckCXXCompilerFlag)
+ if(CMAKE_COMPILER_IS_GNUCXX OR MINGW OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") #Not only UNIX but also WIN32 for MinGW
+
+ set(WARNING_ALL "-Wall")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_ALL} WARNING_ALL_ALLOWED)
+ if(WARNING_ALL_ALLOWED)
+# MESSAGE("Compiler flag ${WARNING_ALL} allowed")
+ set(ACTIVATE_WARNING_ALL "ON" CACHE BOOL "activate -Wall flag")
+ else()
+# MESSAGE("Compiler flag ${WARNING_ALL} not allowed")
+ endif()
+
+ set(WARNING_EXTRA "-Wextra")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_EXTRA} WARNING_EXTRA_ALLOWED)
+ if(WARNING_EXTRA_ALLOWED)
+# MESSAGE("Compiler flag ${WARNING_EXTRA} allowed")
+ set(ACTIVATE_WARNING_EXTRA "ON" CACHE BOOL "activate -Wextra flag")
+ else()
+# MESSAGE("Compiler flag ${WARNING_EXTRA} not allowed")
+ endif()
+
+ set(WARNING_STRICT_OVERFLOW "-Wstrict-overflow=5")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_STRICT_OVERFLOW} WARNING_STRICT_OVERFLOW_ALLOWED)
+ if(WARNING_STRICT_OVERFLOW_ALLOWED)
+# MESSAGE("Compiler flag ${WARNING_STRICT_OVERFLOW} allowed")
+ set(ACTIVATE_WARNING_STRICT_OVERFLOW "OFF" CACHE BOOL "activate -Wstrict-overflow=5 flag")
+ else()
+# MESSAGE("Compiler flag ${WARNING_STRICT_OVERFLOW} not allowed")
+ endif()
+
+ set(WARNING_FLOAT_EQUAL "-Wfloat-equal")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_FLOAT_EQUAL} WARNING_FLOAT_EQUAL_ALLOWED)
+ if(WARNING_FLOAT_EQUAL_ALLOWED)
+# MESSAGE("Compiler flag ${WARNING_FLOAT_EQUAL} allowed")
+ set(ACTIVATE_WARNING_FLOAT_EQUAL "OFF" CACHE BOOL "activate -Wfloat-equal flag")
+ else()
+# MESSAGE("Compiler flag ${WARNING_FLOAT_EQUAL} not allowed")
+ endif()
+
+ set(WARNING_SIGN_CONVERSION "-Wsign-conversion")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_SIGN_CONVERSION} WARNING_SIGN_CONVERSION_ALLOWED)
+ if(WARNING_SIGN_CONVERSION_ALLOWED)
+# MESSAGE("Compiler flag ${WARNING_SIGN_CONVERSION} allowed")
+ set(ACTIVATE_WARNING_SIGN_CONVERSION "OFF" CACHE BOOL "activate -Wsign-conversion flag")
+ else()
+# MESSAGE("Compiler flag ${WARNING_SIGN_CONVERSION} not allowed")
+ endif()
+
+ if(ACTIVATE_WARNING_ALL)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_ALL})
+ else()
+ string(REPLACE ${WARNING_ALL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ if(ACTIVATE_WARNING_EXTRA)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_EXTRA})
+ else()
+ string(REPLACE ${WARNING_EXTRA} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ if(ACTIVATE_WARNING_STRICT_OVERFLOW)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_STRICT_OVERFLOW})
+ list(APPEND CMAKE_C_FLAGS ${WARNING_STRICT_OVERFLOW})
+ else()
+ string(REPLACE ${WARNING_STRICT_OVERFLOW} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ string(REPLACE ${WARNING_STRICT_OVERFLOW} "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
+ endif()
+ if(ACTIVATE_WARNING_FLOAT_EQUAL)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_FLOAT_EQUAL})
+ else()
+ string(REPLACE ${WARNING_FLOAT_EQUAL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ if(ACTIVATE_WARNING_SIGN_CONVERSION)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_SIGN_CONVERSION})
+ else()
+ string(REPLACE ${WARNING_SIGN_CONVERSION} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+
+ elseif(MSVC)
+ # Add specific compilation flags for Windows Visual
+
+ set(WARNING_ALL "/Wall")
+ CHECK_CXX_COMPILER_FLAG(${WARNING_ALL} WARNING_ALL_ALLOWED)
+ if(WARNING_ALL_ALLOWED)
+ #MESSAGE("Compiler flag ${WARNING_ALL} allowed")
+ set(ACTIVATE_WARNING_ALL "OFF" CACHE BOOL "activate /Wall flag")
+ else()
+ #MESSAGE("Compiler flag ${WARNING_ALL} not allowed")
+ endif()
+
+ if(ACTIVATE_WARNING_ALL)
+ list(APPEND CMAKE_CXX_FLAGS ${WARNING_ALL})
+ else()
+ string(REPLACE ${WARNING_ALL} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ if(MSVC80 OR MSVC90 OR MSVC10 OR MSVC11)
+ # To avoid compiler warning (level 4) C4571, compile with /EHa if you still want
+ # your catch(...) blocks to catch structured exceptions.
+ list(APPEND CMAKE_CXX_FLAGS "/EHa")
+ endif()
+ endif()
+
+ # If compiler support symbol visibility, enable it.
+ include(CheckCCompilerFlag)
+ check_c_compiler_flag(-fvisibility=hidden HAS_VISIBILITY)
+ if (HAS_VISIBILITY)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fvisibility=hidden")
+ endif()
+
+# FS no more needed since it is added by default by CMake when build as shared
+# if(UNIX)
+# if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
+# endif()
+# endif()
+
+ # OpenMP
+ if(USE_OPENMP)
+ set(VISP_OPENMP_FLAGS "${OpenMP_CXX_FLAGS}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+ else()
+ set(VISP_OPENMP_FLAGS "")
+ if(OpenMP_CXX_FLAGS)
+ string(REPLACE ${OpenMP_CXX_FLAGS} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ endif()
+
+ # C++11
+ if(USE_CPP11)
+ set(VISP_CPP11_FLAGS "${CPP11_CXX_FLAGS}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CPP11_CXX_FLAGS}")
+ else()
+ set(VISP_CPP11_FLAGS "")
+ if(CPP11_CXX_FLAGS)
+ string(REPLACE ${CPP11_CXX_FLAGS} "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ endif()
+ endif()
+
+ if(BUILD_COVERAGE)
+ # Add build options for test coverage. Currently coverage is only supported
+ # on gcc compiler
+ # Because using -fprofile-arcs with shared lib can cause problems like:
+ # hidden symbol `__bb_init_func', we add this option only for static
+ # library build
+ message(STATUS "Add -ftest-coverage -fprofile-arcs compiler options")
+ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ftest-coverage -fprofile-arcs")
+ endif()
+
+ # Remove duplicates compilation flags
+ separate_arguments(CMAKE_CXX_FLAGS)
+ list(REMOVE_DUPLICATES CMAKE_CXX_FLAGS)
+ string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}" CACHE STRING "common C++ build flags" FORCE)
+ separate_arguments(CMAKE_C_FLAGS)
+ list(REMOVE_DUPLICATES CMAKE_C_FLAGS)
+ string(REPLACE ";" " " CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}" CACHE STRING "common C build flags" FORCE)
+
+ #message("CMAKE_CXX_FLAGS : ${CMAKE_CXX_FLAGS}")
+endmacro(vp_add_extra_compilation_flags)
diff --git a/cmake/CPackConfig.cmake b/cmake/CPackConfig.cmake
new file mode 100644
index 0000000..b22f280
--- /dev/null
+++ b/cmake/CPackConfig.cmake
@@ -0,0 +1,110 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP packaging configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+message(STATUS "Configuring CPack")
+#mark_as_advanced(BUILD_PACKAGE)
+
+
+SET(CPACK_PACKAGE_NAME "libvisp")
+SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Visual tracking and visual servoing library in C++ (development files)")
+SET(CPACK_PACKAGE_CONTACT "Fabien Spindler <Fabien.Spindler at inria.fr>")
+SET(CPACK_PACKAGE_VENDOR "Inria, French National Institute for Research in Computer Science and Control")
+SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.txt")
+SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.txt")
+
+SET(CPACK_PACKAGE_VERSION ${VISP_VERSION})
+SET(CPACK_PACKAGE_VERSION_MAJOR ${VISP_VERSION_MAJOR})
+SET(CPACK_PACKAGE_VERSION_MINOR ${VISP_VERSION_MINOR})
+SET(CPACK_PACKAGE_VERSION_PATCH ${VISP_VERSION_PATCH})
+
+SET(CPACK_COMPONENTS_ALL libraries headers)
+SET(CPACK_COMPONENT_LIBRARIES_DISPLAY_NAME "Libraries")
+SET(CPACK_COMPONENT_HEADERS_DISPLAY_NAME "C++ Headers")
+SET(CPACK_COMPONENT_LIBRARIES_DESCRIPTION "ViSP ${VISP_VERSION} libraries")
+SET(CPACK_COMPONENT_HEADERS_DESCRIPTION "C/C++ header files for use with ViSP ${VISP_VERSION} libraries")
+SET(CPACK_COMPONENT_HEADERS_DEPENDS libraries)
+SET(CPACK_COMPONENT_LIBRARIES_GROUP "Development")
+SET(CPACK_COMPONENT_HEADERS_GROUP "Development")
+SET(CPACK_COMPONENT_GROUP_DEVELOPMENT_DESCRIPTION "All of the tools you'll ever need to develop software with ViSP")
+SET(CPACK_ALL_INSTALL_TYPES Full Developer)
+SET(CPACK_COMPONENT_LIBRARIES_INSTALL_TYPES Developer Full)
+SET(CPACK_COMPONENT_HEADERS_INSTALL_TYPES Developer Full)
+
+# The following components are regex's to match anywhere (unless anchored)
+# in absolute path + filename to find files or directories to be excluded
+# from source tarball.
+SET(CPACK_SOURCE_IGNORE_FILES
+ "/\\\\.svn/"
+ "/autom4te.cache/"
+ "/build/"
+ "/lib/"
+ "~$"
+)
+
+SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME} ${VISP_VERSION}")
+
+# In ViSP packages we only want to have the libraries and the headers (nor the demo, example, and test)
+# That's why we turn off the demo, example and test building
+SET(BUILD_DEMOS OFF CACHE BOOL "Build ViSP demos." FORCE )
+SET(BUILD_EXAMPLES OFF CACHE BOOL "Build ViSP examples." FORCE)
+SET(BUILD_TESTING OFF CACHE BOOL "Build ViSP tests." FORCE)
+SET(BUILD_TUTORIAL OFF CACHE BOOL "Build ViSP tutorials." FORCE)
+
+
+if(UNIX)
+ set(CMAKE_INSTALL_PREFIX "/usr" CACHE String "Package install prefix" FORCE)
+elseif(WIN32)
+ set(CMAKE_INSTALL_PREFIX "C:/Program Files/ViSP ${VISP_VERSION}" CACHE String "Package install prefix" FORCE)
+endif()
+
+set(BUILD_SHARED_LIBS ON CACHE BOOL "Build ViSP with shared libraries." FORCE)
+
+
+if(WIN32 AND NOT UNIX)
+ include(${VISP_CMAKE_MODULE_PATH}/CPackConfigNsis.cmake)
+elseif(APPLE)
+ set(CPACK_GENERATOR "PackageMaker;TBZ2")
+elseif(UNIX)
+ if(BUILD_PACKAGE_DEBIAN)
+ include(${VISP_CMAKE_MODULE_PATH}/CPackConfigDeb.cmake)
+ elseif(BUILD_PACKAGE_RPM)
+ include(${VISP_CMAKE_MODULE_PATH}/CPackConfigRpm.cmake)
+ endif()
+endif()
+
+INCLUDE(CPack)
diff --git a/cmake/CPackConfigDeb.cmake b/cmake/CPackConfigDeb.cmake
new file mode 100644
index 0000000..116e1d9
--- /dev/null
+++ b/cmake/CPackConfigDeb.cmake
@@ -0,0 +1,177 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP packaging configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+# To install the generated debian package use the gdebi-gtk package
+# installer on Ubuntu:
+# gdebi-gtk libvisp-dev-2.6.1-1_i386.deb
+
+# $ dpkg --print-architecture
+FIND_PROGRAM(DPKG_CMD dpkg)
+MARK_AS_ADVANCED(DPKG_CMD)
+IF(NOT DPKG_CMD)
+ MESSAGE(STATUS "Can not find dpkg in your path, default to i386.")
+ SET(CPACK_SYSTEM_NAME i386)
+ELSE()
+ EXECUTE_PROCESS(COMMAND "${DPKG_CMD}" --print-architecture
+ OUTPUT_VARIABLE CPACK_SYSTEM_NAME
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+ENDIF()
+
+# $ lsb_release -sr (ex: 12.04)
+FIND_PROGRAM(LSB_RELEASE_CMD lsb_release)
+MARK_AS_ADVANCED(LSB_RELEASE_CMD)
+IF(NOT LSB_RELEASE_CMD)
+ MESSAGE(STATUS "Can not find lsb_release in your path.")
+ SET(DEBIAN_DISTRO_RELEASE "")
+ELSE()
+ EXECUTE_PROCESS(COMMAND "${LSB_RELEASE_CMD}" -sr
+ OUTPUT_VARIABLE DEBIAN_DISTRO_RELEASE
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+ if(DEBIAN_DISTRO_RELEASE)
+ #message("DEBIAN_DISTRO_RELEASE 1: ${DEBIAN_DISTRO_RELEASE}")
+ STRING(REGEX REPLACE "[.]" "" DEBIAN_DISTRO_RELEASE ${DEBIAN_DISTRO_RELEASE})
+ #message("DEBIAN_DISTRO_RELEASE 2: ${DEBIAN_DISTRO_RELEASE}")
+ endif()
+ENDIF()
+
+
+list(APPEND CPACK_GENERATOR DEB)
+
+SET(CPACK_PACKAGE_VERSION ${VISP_VERSION}-${VISP_REVISION})
+SET(CPACK_PACKAGE_NAME "libvisp-dev")
+set(CPACK_PACKAGE_FILE_NAME "${CPACK_PACKAGE_NAME}_${VISP_VERSION}-${VISP_REVISION}_${CPACK_SYSTEM_NAME}")
+
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "cmake (>=2.6)")
+IF(USE_X11)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libx11-dev (>=2:1.3.2)")
+ENDIF()
+IF(USE_GTK2)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libgtk2.0-dev (>=2.20.1), libglib2.0-dev (>=2.24.1), libpango1.0-dev (>=1.28.0), libatk1.0-dev (>=1.30.0), libcairo2-dev (>=1.8.10)")
+ENDIF()
+IF(USE_LAPACK)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, liblapack-dev(>=3.2)")
+ENDIF()
+IF(USE_GSL)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libgsl0-dev (>=1.13)")
+ENDIF()
+IF(USE_V4L2)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libv4l-dev (>=0.6.4)")
+ENDIF()
+IF(USE_DC1394_2)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libdc1394-22-dev (>=2.1.2)")
+ENDIF()
+IF(USE_XML2)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libxml2-dev (>=2.7.2)")
+ENDIF()
+IF(USE_LIBPNG)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libpng12-dev (>=1.2.42)")
+ENDIF()
+IF(USE_LIBJPEG)
+ if(DEBIAN_DISTRO_RELEASE)
+ if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg62-dev (>=6b-15)")
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg-dev (>=6b-15)")
+ endif()
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg-dev (>=6b-15)")
+ endif()
+ENDIF()
+IF(USE_FFMPEG)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libswscale-dev (>=4:0.5.1), libavutil-dev (>=4:0.5.1), libavformat-dev (>=4:0.5.1), libavcodec-dev (>=4:0.5.1), libbz2-dev (>=1.0.5-4), libbz2-1.0 (>=1.0.5-4)")
+ENDIF()
+IF(USE_COIN)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libcoin60-dev (>=3.1.2-1)")
+ENDIF()
+IF(USE_SOQT)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libsoqt4-dev (>=1.4.2~svn20090224-2)")
+ENDIF()
+IF(USE_OGRE)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libogre-dev (>=1.7.3)")
+ # since ogre-samples-media is not available on ubuntu oneiric 11.10 for example, we need an extra test to add this dependency
+ IF(OGRE_MEDIA_DIR)
+ if(DEBIAN_DISTRO_RELEASE)
+ if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples-media (>=1.7.3)")
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples (>=1.7.3)")
+ endif()
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, ogre-samples-media (>=1.7.3)")
+ endif()
+ ENDIF()
+ENDIF()
+IF(USE_OIS)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libois-dev (>=1.3.0)")
+ENDIF()
+IF(USE_OPENCV)
+ if(DEBIAN_DISTRO_RELEASE)
+ if(DEBIAN_DISTRO_RELEASE VERSION_LESS "1204")
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libcv-dev (>=2.0), libcvaux-dev (>=2.0), libhighgui-dev (>=2.0)")
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libopencv-dev (>=2.3.1)")
+ endif()
+ else()
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libopencv-dev (>=2.3.1)")
+ endif()
+ENDIF()
+IF(USE_LIBFREENECT)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, freenect (>=1:0.0.1)")
+ENDIF()
+if(USE_LIBUSB_1)
+ set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libusb-1.0-0-dev (>=2:1.0)")
+endif()
+
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libc6 (>=2.4)")
+
+SET(CPACK_DEBIAN_PACKAGE_DESCRIPTION "Visual Servoing Platform development files
+ ViSP, standing for Visual Servoing Platform, is unique. This software
+ is a complete cross-platform solution that allows prototyping and
+ developing applications in visual tracking and visual servoing.
+ .
+ ViSP can be useful in robotics, computer vision, augmented reality and
+ computer animation.
+ .
+ This package contains development files (headers and shared library
+ symbolic link).")
+
+SET(CPACK_DEBIAN_PACKAGE_SECTION "libdevel")
+SET(CPACK_DEBIAN_PACKAGE_HOMEPAGE "http://www.irisa.fr/lagadic/visp")
+
diff --git a/cmake/CPackConfigNsis.cmake b/cmake/CPackConfigNsis.cmake
new file mode 100644
index 0000000..18adfec
--- /dev/null
+++ b/cmake/CPackConfigNsis.cmake
@@ -0,0 +1,55 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP packaging configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+set(BUILD_SHARED_LIBS CACHE FORCE "Build ViSP with shared libraries." ON)
+
+# InstallRequiredSystemLibraries: Hack for Visual Studio support Search for
+# system runtime libraries based on the platform.
+INCLUDE(InstallRequiredSystemLibraries)
+
+set(CPACK_GENERATOR NSIS)
+set(CPACK_NSIS_CONTACT "visp at inria.fr")
+SET(CPACK_NSIS_HELP_LINK "http://www.irisa.fr/lagadic/visp/visp.html")
+set(CPACK_NSIS_URL_INFO_ABOUT "http://www.irisa.fr/lagadic/visp/visp.html")
+set(CPACK_NSIS_MODIFY_PATH "ON")
+set(CPACK_NSIS_DISPLAY_NAME "${CPACK_PACKAGE_INSTALL_DIRECTORY} SDK")
+# set(CPACK_PACKAGE_ICON ${VISP_SOURCE_DIR}/visp.bmp)
+# set(CPACK_NSIS_MUI_ICON ${VISP_SOURCE_DIR}/logoViSP.ico)
+# set(CPACK_NSIS_MUI_UNIICON ${VISP_SOURCE_DIR}/logoViSP.ico)
+
+SET(CPACK_PACKAGE_FILE_NAME "ViSP-${VISP_VERSION}-win32")
+SET(CPACK_SOURCE_PACKAGE_FILE_NAME "ViSP-${VISP_VERSION}-win32-src")
diff --git a/cmake/CPackConfigRpm.cmake b/cmake/CPackConfigRpm.cmake
new file mode 100644
index 0000000..0b26055
--- /dev/null
+++ b/cmake/CPackConfigRpm.cmake
@@ -0,0 +1,146 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP packaging configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+# $ uname -i
+
+FIND_PROGRAM(UNAME_CMD uname)
+MARK_AS_ADVANCED(UNAME_CMD)
+IF(NOT UNAME_CMD)
+ MESSAGE(STATUS "Can not find uname in your path, default to i386.")
+ SET(CPACK_SYSTEM_NAME i386)
+ELSE()
+ EXECUTE_PROCESS(COMMAND "${UNAME_CMD}" -m
+ OUTPUT_VARIABLE CPACK_SYSTEM_NAME
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+ENDIF()
+
+# create a fedora short name from cat /etc/fedora-release
+# for example, cat .. return "Fedora release 13 (Goddard)" we build "fc13"
+FIND_PROGRAM(CAT_CMD cat)
+MARK_AS_ADVANCED(CAT_CMD)
+IF(NOT CAT_CMD)
+ MESSAGE(STATUS "Can not find cat in your path, default to empty.")
+ SET(DIST_SHORT_NAME "")
+ELSE()
+ SET(DIST_SHORT_NAME "")
+ EXECUTE_PROCESS(COMMAND "${CAT_CMD}" /etc/fedora-release
+ OUTPUT_VARIABLE FEDORA_FULL_RELEASE_NAME
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+ #message("fedora release found 1: ${FEDORA_FULL_RELEASE_NAME}")
+ IF(FEDORA_FULL_RELEASE_NAME)
+ # replace " " space separator by ";" separator
+ STRING(REGEX REPLACE "[ ]" ";" FEDORA_FULL_RELEASE_NAME ${FEDORA_FULL_RELEASE_NAME})
+ # message("fedora release found 2: ${FEDORA_FULL_RELEASE_NAME}")
+ LIST(GET FEDORA_FULL_RELEASE_NAME 0 ITEM1) # here ITEM1 should contain "Fedora"
+ # message("item1: ${ITEM1}")
+ STRING(COMPARE EQUAL ${ITEM1} "Fedora" IS_FEDORA)
+ IF(IS_FEDORA)
+ LIST(GET FEDORA_FULL_RELEASE_NAME 2 ITEM3) # here ITEM3 should contain the dist number
+ # message("dist number: ${ITEM3}")
+ IF(ITEM3)
+ SET(DIST_SHORT_NAME "fc${ITEM3}")
+ ENDIF()
+ ENDIF()
+ ENDIF()
+ENDIF()
+
+
+list(APPEND CPACK_GENERATOR RPM)
+
+SET(CPACK_PACKAGE_NAME "libvisp-devel")
+set(CPACK_PACKAGE_FILE_NAME "${CPACK_PACKAGE_NAME}-${VISP_VERSION}-${VISP_REVISION}.${DIST_SHORT_NAME}.${CPACK_SYSTEM_NAME}")
+set(CPACK_RPM_PACKAGE_GROUP "Development/Libraries")
+set(CPACK_RPM_PACKAGE_ARCHITECTURE "${CPACK_SYSTEM_NAME}")
+set(CPACK_RPM_PACKAGE_LICENSE "GPL")
+set(CPACK_RPM_PACKAGE_RELEASE ${VISP_REVISION})
+
+set(CPACK_RPM_PACKAGE_DEPENDS "cmake >= 2.6")
+IF(USE_X11)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libX11-devel >= 1.3")
+ENDIF()
+IF(USE_GTK2)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, gtk2-devel, glib2-devel, pango-devel, atk-devel, cairo-devel")
+ENDIF()
+IF(USE_LAPACK)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, lapack-devel >= 3.2")
+ENDIF()
+IF(USE_GSL)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, gsl-devel >= 1.13")
+ENDIF()
+IF(USE_V4L2)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libv4l-devel >= 0.6.2")
+ENDIF()
+IF(USE_DC1394_2)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libdc1394-devel >= 2.1.2")
+ENDIF()
+IF(USE_XML2)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libxml2-devel >= 2.7.6")
+ENDIF()
+IF(USE_LIBPNG)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libpng-devel >= 2:1.2.44")
+ENDIF()
+IF(USE_LIBJPEG)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, libjpeg-devel >= 6b-46")
+ENDIF()
+IF(USE_COIN)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, Coin2-devel >= 2.5.0")
+ENDIF()
+IF(USE_SOQT)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, SoQt-devel >= 1.5.0")
+ENDIF()
+IF(USE_OPENCV)
+ set(CPACK_RPM_PACKAGE_DEPENDS "${CPACK_RPM_PACKAGE_DEPENDS}, opencv-devel >= 2.0")
+ENDIF()
+#message("CPACK_RPM_PACKAGE_DEPENDS ${CPACK_RPM_PACKAGE_DEPENDS}")
+
+set(CPACK_RPM_PACKAGE_REQUIRES "${CPACK_RPM_PACKAGE_DEPENDS}")
+
+set(CPACK_RPM_PACKAGE_DESCRIPTION "Visual Servoing Platform development files
+ ViSP, standing for Visual Servoing Platform, is unique. This software
+ is a complete cross-platform solution that allows prototyping and
+ developing applications in visual tracking and visual servoing.
+ .
+ ViSP can be useful in robotics, computer vision, augmented reality and
+ computer animation.
+ .
+ This package contains development files (headers and shared library
+ symbolic link).")
+
+set(CPACK_RPM_EXCLUDE_FROM_AUTO_FILELIST_ADDITION "/usr/share/man" "/usr/share/man/man1" "/usr/lib64/pkgconfig" "/usr/lib/pkgconfig" "/usr/lib64/cmake" "/usr/lib/cmake")
diff --git a/cmake/FindARIA.cmake b/cmake/FindARIA.cmake
new file mode 100644
index 0000000..c066bc9
--- /dev/null
+++ b/cmake/FindARIA.cmake
@@ -0,0 +1,133 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Aria libraries and headers to control Pioneer mobile robots.
+# Once run this will define:
+#
+# ARIA_FOUND
+# ARIA_INCLUDE_DIRS
+# ARIA_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+find_path(ARIA_INCLUDE_DIR Aria.h
+ $ENV{ARIA_HOME}/include
+ /usr/local/Aria/include
+ /usr/Aria/include
+ /usr/include
+ /usr/include/Aria
+ /usr/local/include
+ /usr/local/include/Aria
+ "C:/Program Files/MobileRobots/Aria/include"
+)
+#MESSAGE("DBG ARIA_INCLUDE_DIR=${ARIA_INCLUDE_DIR}")
+
+if(UNIX)
+ find_library(ARIA_LIBRARY
+ NAMES Aria
+ PATHS
+ $ENV{ARIA_HOME}/lib
+ /usr/local/Aria/lib
+ /usr/Aria/lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/MobileRobots/Aria/lib"
+ )
+ #MESSAGE("DBG ARIA_LIBRARY=${ARIA_LIBRARY}")
+else()
+ if(MSVC11)
+ set(ARIA_SUFFIX_NAME VC11)
+ elseif(MSVC10)
+ set(ARIA_SUFFIX_NAME VC10)
+ elseif(MSVC90)
+ set(ARIA_SUFFIX_NAME VC9)
+ elseif(MSVC80)
+ set(ARIA_SUFFIX_NAME VC8)
+ elseif(MSVC71)
+ set(ARIA_SUFFIX_NAME VC71)
+ else()
+ set(ARIA_SUFFIX_NAME "")
+ endif()
+
+ find_library(ARIA_LIBRARY_DEBUG
+ NAMES AriaDebug${ARIA_SUFFIX_NAME}
+ PATHS
+ $ENV{ARIA_HOME}/lib
+ /usr/local/Aria/lib
+ /usr/Aria/lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/MobileRobots/Aria/lib"
+ )
+ find_library(ARIA_LIBRARY_RELEASE
+ NAMES Aria${ARIA_SUFFIX_NAME}
+ PATHS
+ $ENV{ARIA_HOME}/lib
+ /usr/local/Aria/lib
+ /usr/Aria/lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/MobileRobots/Aria/lib"
+ )
+endif()
+
+if(UNIX)
+ if(ARIA_LIBRARY AND ARIA_INCLUDE_DIR)
+ set(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
+ set(ARIA_LIBRARIES ${ARIA_LIBRARY})
+ set(ARIA_FOUND TRUE)
+ else()
+ set(ARIA_FOUND FALSE)
+ endif()
+else()
+ set(ARIA_LIBRARIES "")
+ if(ARIA_LIBRARY_RELEASE AND ARIA_INCLUDE_DIR)
+ set(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
+ list(APPEND ARIA_LIBRARIES optimized ${ARIA_LIBRARY_RELEASE})
+ set(ARIA_FOUND TRUE)
+ endif()
+ IF(ARIA_LIBRARY_DEBUG AND ARIA_INCLUDE_DIR)
+ set(ARIA_INCLUDE_DIRS ${ARIA_INCLUDE_DIR})
+ list(APPEND ARIA_LIBRARIES debug ${ARIA_LIBRARY_DEBUG})
+ set(ARIA_FOUND TRUE)
+ endif()
+
+endif()
+
+mark_as_advanced(
+ ARIA_INCLUDE_DIR
+ ARIA_LIBRARY
+ ARIA_LIBRARY_DEBUG
+ ARIA_LIBRARY_RELEASE
+)
+
diff --git a/cmake/FindBICLOPS.cmake b/cmake/FindBICLOPS.cmake
new file mode 100644
index 0000000..0a50f5b
--- /dev/null
+++ b/cmake/FindBICLOPS.cmake
@@ -0,0 +1,148 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libBiclops, libPMD and libUtils for Biclops head.
+# Once run this will define:
+#
+# BICLOPS_FOUND
+# BICLOPS_INCLUDE_DIRS
+# BICLOPS_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+IF(NOT UNIX AND NOT WIN32)
+ # MESSAGE("FindBICLOPS.cmake: macro only for Unix and Windows for the moment.")
+ SET(BICLOPS_FOUND FALSE)
+ELSE(NOT UNIX AND NOT WIN32)
+
+ FIND_PATH(BICLOPS_INCLUDE_DIR Biclops.h
+ $ENV{BICLOPS_HOME}/include
+ /usr/include )
+
+ #MESSAGE("DBG BICLOPS_INCLUDE_DIR=${BICLOPS_INCLUDE_DIR}")
+
+ FIND_PATH(PMD_INCLUDE_DIR PMD.h
+ $ENV{BICLOPS_HOME}/include
+ $ENV{BICLOPS_PMD_HOME}/include
+ /usr/include )
+
+ FIND_PATH(UTILS_INCLUDE_DIR utility.h
+ $ENV{BICLOPS_HOME}/include
+ $ENV{BICLOPS_UTILS_HOME}/include
+ /usr/include )
+
+ FIND_LIBRARY(BICLOPS_LIBRARY
+ NAMES Biclops libBiclops libBiclopsD
+ PATHS
+ $ENV{BICLOPS_HOME}/lib
+ /usr/lib
+ )
+
+ FIND_LIBRARY(PMD_LIBRARY
+ NAMES PMD libPMD libPMDD
+ PATHS
+ $ENV{BICLOPS_HOME}/lib
+ $ENV{BICLOPS_PMD_HOME}/lib
+ /usr/lib
+ )
+
+ FIND_LIBRARY(UTILS_LIBRARY
+ NAMES Utils libUtils libUtilsD
+ PATHS
+ $ENV{BICLOPS_HOME}/lib
+ $ENV{BICLOPS_UTILS_HOME}/lib
+ /usr/lib
+ )
+ #MESSAGE("DBG BICLOPS_LIBRARY=${BICLOPS_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
+ SET(BICLOPS_LIBRARIES ${BICLOPS_LIBRARY} ${PMD_LIBRARY} ${UTILS_LIBRARY})
+ ELSE(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
+# MESSAGE(SEND_ERROR "Biclops library not found. Set USE_BICLOPS option OFF")
+ ENDIF(BICLOPS_LIBRARY AND PMD_LIBRARY AND UTILS_LIBRARY)
+
+ IF(NOT BICLOPS_INCLUDE_DIR)
+# MESSAGE(SEND_ERROR "Biclops include dir not found. Set USE_BICLOPS option OFF")
+ ENDIF(NOT BICLOPS_INCLUDE_DIR)
+
+ IF(NOT PMD_INCLUDE_DIR)
+# MESSAGE(SEND_ERROR "PMD include dir not found. Set USE_BICLOPS option OFF")
+ ENDIF(NOT PMD_INCLUDE_DIR)
+
+ IF(NOT UTILS_INCLUDE_DIR)
+# MESSAGE(SEND_ERROR "Utils include dir not found. Set USE_BICLOPS option OFF ")
+ ENDIF(NOT UTILS_INCLUDE_DIR)
+
+ IF(BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
+ SET(BICLOPS_INCLUDE_DIRS ${BICLOPS_INCLUDE_DIR} ${PMD_INCLUDE_DIR} ${UTILS_INCLUDE_DIR})
+ ENDIF(BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
+
+ IF(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
+ #message("BICLOPS_LIBRARIES: ${BICLOPS_LIBRARIES}")
+ #message("BICLOPS_INCLUDE_DIRS: ${BICLOPS_INCLUDE_DIRS}")
+
+ # Try to compile a sample code using Biclops library to see if GetHomedState() is available
+ #SET(BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION FALSE)
+ include(CheckCXXSourceCompiles)
+
+ SET(CMAKE_REQUIRED_LIBRARIES ${BICLOPS_LIBRARIES})
+ SET(CMAKE_REQUIRED_INCLUDES ${BICLOPS_INCLUDE_DIRS})
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <PMDUtils.h>
+ int main(){
+ PMDAxisControl *axis;
+ axis->GetHomedState();
+ return 0;
+ }
+ " BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION)
+
+ #MESSAGE("DBG1 BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION ${BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION}")
+
+ SET(BICLOPS_FOUND TRUE)
+ ELSE(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
+ SET(BICLOPS_FOUND FALSE)
+ ENDIF(BICLOPS_LIBRARIES AND BICLOPS_INCLUDE_DIR AND PMD_INCLUDE_DIR AND UTILS_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ BICLOPS_INCLUDE_DIR
+ BICLOPS_LIBRARIES
+ BICLOPS_LIBRARY
+ PMD_INCLUDE_DIR
+ UTILS_INCLUDE_DIR
+ PMD_LIBRARY
+ UTILS_LIBRARY
+ )
+ENDIF(NOT UNIX AND NOT WIN32)
diff --git a/cmake/FindCALINUX.cmake b/cmake/FindCALINUX.cmake
new file mode 100644
index 0000000..b2844e7
--- /dev/null
+++ b/cmake/FindCALINUX.cmake
@@ -0,0 +1,88 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Cerebellum CA linux library material
+#
+# CALINUX_FOUND
+# CALINUX_INCLUDE_DIRS
+# CALINUX_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindCALINUX.cmake: only available for Unix.")
+ SET(CALINUX_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_PATH(CALINUX_INCLUDE_DIR calinux.h
+ $ENV{CALINUX_HOME}/include
+ /local/soft/Cerebellum/CALinux/current/include
+ /home/soft/Cerebellum/CALinux/current/include
+ )
+ #MESSAGE("DBG CALINUX_INCLUDE_DIR=${CALINUX_INCLUDE_DIR}")
+
+ FIND_LIBRARY(CALINUX_LIBRARY
+ NAMES calinux
+ PATHS
+ $ENV{CALINUX_HOME}/lib
+ /local/soft/Cerebellum/CALinux/current/lib
+ /home/soft/Cerebellum/CALinux/current/lib
+ )
+
+ #MESSAGE("DBG CALINUX_LIBRARY=${CALINUX_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(CALINUX_LIBRARY)
+ SET(CALINUX_LIBRARIES ${CALINUX_LIBRARY})
+ ELSE(CALINUX_LIBRARY)
+# MESSAGE(SEND_ERROR "Calinux library not found.")
+ ENDIF(CALINUX_LIBRARY)
+
+ IF(NOT CALINUX_INCLUDE_DIR)
+# MESSAGE(SEND_ERROR "Calinux include dir not found.")
+ ENDIF(NOT CALINUX_INCLUDE_DIR)
+
+ IF(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
+ SET(CALINUX_INCLUDE_DIRS ${CALINUX_INCLUDE_DIR})
+ SET(CALINUX_FOUND TRUE)
+ ELSE(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
+ SET(CALINUX_FOUND FALSE)
+ ENDIF(CALINUX_LIBRARIES AND CALINUX_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ CALINUX_INCLUDE_DIR
+ CALINUX_LIBRARIES
+ CALINUX_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindCMU1394.cmake b/cmake/FindCMU1394.cmake
new file mode 100644
index 0000000..1578026
--- /dev/null
+++ b/cmake/FindCMU1394.cmake
@@ -0,0 +1,106 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find CMU 1394 Digital Camera SDK to acquire images from firewire
+# cameras on Windows.
+# Once run this will define:
+#
+# CMU1394_FOUND
+# CMU1394_INCLUDE_DIRS
+# CMU1394_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+if(WIN32)
+ FIND_PATH(CMU1394_INCLUDE_DIR 1394Camera.h
+ $ENV{CMU1394_HOME}/include
+ "C:/Program Files (x86)/CMU/1394Camera/include"
+ "C:/Program Files/CMU/1394Camera/include"
+ )
+ #MESSAGE("DBG CMU1394_INCLUDE_DIR=${CMU1394_INCLUDE_DIR}")
+
+ if(CMAKE_CL_64)
+ FIND_LIBRARY(CMU1394_LIBRARY_DEBUG
+ NAMES 1394camerad
+ PATHS
+ $ENV{CMU1394_HOME}/lib64/x64
+ "C:/Program Files (x86)/CMU/1394Camera/lib64/x64"
+ "C:/Program Files/CMU/1394Camera/lib64/x64"
+ )
+
+ FIND_LIBRARY(CMU1394_LIBRARY_RELEASE
+ NAMES 1394camera
+ PATHS
+ $ENV{CMU1394_HOME}/lib64/x64
+ "C:/Program Files (x86)/CMU/1394Camera/lib64/x64"
+ "C:/Program Files/CMU/1394Camera/lib64/x64"
+ )
+ else()
+ FIND_LIBRARY(CMU1394_LIBRARY_DEBUG
+ NAMES 1394camerad
+ PATHS
+ $ENV{CMU1394_HOME}/lib
+ "C:/Program Files (x86)/CMU/1394Camera/lib"
+ "C:/Program Files/CMU/1394Camera/lib"
+ )
+
+ FIND_LIBRARY(CMU1394_LIBRARY_RELEASE
+ NAMES 1394camera
+ PATHS
+ $ENV{CMU1394_HOME}/lib
+ "C:/Program Files (x86)/CMU/1394Camera/lib"
+ "C:/Program Files/CMU/1394Camera/lib"
+ )
+ endif()
+ #MESSAGE("DBG CMU1394_LIBRARY_DEBUG=${CMU1394_LIBRARY_DEBUG}")
+ #MESSAGE("DBG CMU1394_LIBRARY_RELEASE=${CMU1394_LIBRARY_RELEASE}")
+
+ set(CMU1394_LIBRARIES "")
+ if(CMU1394_LIBRARY_RELEASE AND CMU1394_INCLUDE_DIR)
+ set(CMU1394_INCLUDE_DIRS ${CMU1394_INCLUDE_DIR})
+ list(APPEND CMU1394_LIBRARIES optimized ${CMU1394_LIBRARY_RELEASE})
+ set(CMU1394_FOUND TRUE)
+ endif()
+ if(CMU1394_LIBRARY_DEBUG AND CMU1394_INCLUDE_DIR)
+ set(CMU1394_INCLUDE_DIRS ${CMU1394_INCLUDE_DIR})
+ list(APPEND CMU1394_LIBRARIES debug ${CMU1394_LIBRARY_DEBUG})
+ set(CMU1394_FOUND TRUE)
+ ENDIF()
+
+ MARK_AS_ADVANCED(
+ CMU1394_INCLUDE_DIR
+ CMU1394_LIBRARY
+ CMU1394_LIBRARY_DEBUG
+ CMU1394_LIBRARY_RELEASE
+ )
+endif()
diff --git a/cmake/FindCPP11.cmake b/cmake/FindCPP11.cmake
new file mode 100644
index 0000000..917378b
--- /dev/null
+++ b/cmake/FindCPP11.cmake
@@ -0,0 +1,68 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find C++11 flags.
+#
+# CPP11_FOUND - true if C++11 support is detected
+# CPP11_CXX_FLAGS - flags to add to the CXX compiler for C++11 support
+#
+# Authors:
+# Fabien Spindler
+
+include(CheckCXXCompilerFlag)
+include(FindPackageHandleStandardArgs)
+
+set(CPP11_CXX_FLAG_CANDIDATES
+ "-std=c++11"
+ "-std=c++0x"
+)
+
+# check CXX c++11 compiler flag
+foreach(FLAG ${CPP11_CXX_FLAG_CANDIDATES})
+ set(SAFE_CMAKE_REQUIRED_FLAGS "${CMAKE_REQUIRED_FLAGS}")
+ set(CMAKE_REQUIRED_FLAGS "${FLAG}")
+ unset(CPP11_FLAG_DETECTED CACHE)
+ message(STATUS "Try C++11 flag = [${FLAG}]")
+ CHECK_CXX_COMPILER_FLAG(${FLAG} CPP11_FLAG_DETECTED)
+ set(CMAKE_REQUIRED_FLAGS "${SAFE_CMAKE_REQUIRED_FLAGS}")
+ if(CPP11_FLAG_DETECTED)
+ set(CPP11_CXX_FLAGS_INTERNAL "${FLAG}")
+ break()
+ endif()
+endforeach()
+
+set(CPP11_CXX_FLAGS "${CPP11_CXX_FLAGS_INTERNAL}"
+ CACHE STRING "C++ compiler flags for C++11 support")
+# handle the standard arguments for find_package
+find_package_handle_standard_args(CPP11 DEFAULT_MSG CPP11_CXX_FLAGS )
+
+mark_as_advanced(
+ CPP11_CXX_FLAGS
+)
diff --git a/cmake/FindDC1394.cmake b/cmake/FindDC1394.cmake
new file mode 100644
index 0000000..0c53afc
--- /dev/null
+++ b/cmake/FindDC1394.cmake
@@ -0,0 +1,138 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libDC1394 for IEEE1394 camera. First search for libdc1394-2.x
+# and if not found, search for libdc1394-1.x
+# Once run this will define:
+#
+# DC1394_FOUND
+# DC1394_INCLUDE_DIRS
+# DC1394_LIBRARIES
+#
+# The two defines below are only useful to compile with libdc1394-2.x. In
+# that case DC1394_VERSION=2. Since the libdc1394-2.x API is not stable, we
+# need to determine if dc1394_find_cameras() or dc1394_enumerate_cameras()
+# functions are available. dc1394_enumerate_cameras() was introduced after
+# libdc1394-2.0.0-rc7. DC1394_CAMERA_ENUMERATE_FOUND is TRUE when
+# dc1394_camera_enumerate() function is found. DC1394_FIND_CAMERAS_FOUND is
+# TRUE when dc1394_find_cameras() is found.
+# DC1394_CAMERA_ENUMERATE_FOUND
+# DC1394_FIND_CAMERAS_FOUND
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindDC1394.cmake: libdc1394 only available for Unix.")
+ SET(DC1394_FOUND FALSE)
+ELSE(NOT UNIX)
+ # Search for libdc1394-2.x
+ FIND_PATH(DC1394_INCLUDE_DIR dc1394/control.h
+ $ENV{DC1394_HOME}/include
+ $ENV{DC1394_DIR}/include
+ /usr/include )
+ #MESSAGE("DBG DC1394_INCLUDE_DIR=${DC1394_INCLUDE_DIR}")
+
+ FIND_LIBRARY(DC1394_LIBRARY
+ NAMES dc1394
+ PATHS
+ $ENV{DC1394_HOME}/lib
+ $ENV{DC1394_DIR}/lib
+ /usr/lib
+ )
+ #MESSAGE("DBG DC1394_LIBRARY=${DC1394_LIBRARY}")
+
+ IF(DC1394_LIBRARY AND DC1394_INCLUDE_DIR)
+
+ # Since the libdc1394-2.x API is not stable, try to compile a
+ # sample code to determine if we have to use dc1394_find_cameras() or
+ # dc1394_enumerate_cameras() functions. dc1394_enumerate_cameras() was
+ # introduced after libdc1394-2.0.0-rc7
+
+ include(CheckCXXSourceCompiles)
+
+ SET(CMAKE_REQUIRED_LIBRARIES ${DC1394_LIBRARY})
+ SET(CMAKE_REQUIRED_INCLUDES ${DC1394_INCLUDE_DIR})
+
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <dc1394/control.h>
+ #include <dc1394/utils.h>
+
+ int main(){
+ dc1394_t * d;
+ dc1394camera_list_t * list;
+ d = dc1394_new ();
+ dc1394_camera_enumerate (d, &list);
+ return 0;
+ }
+ " DC1394_CAMERA_ENUMERATE_FOUND)
+ #MESSAGE("DC1394_CAMERA_ENUMERATE_FOUND: ${DC1394_CAMERA_ENUMERATE_FOUND}")
+
+ IF(NOT DC1394_CAMERA_ENUMERATE_FOUND)
+ # Try to test the compatibility to libdc1394-2.0.0-rc7
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <dc1394/control.h>
+ #include <dc1394/utils.h>
+
+ int main(){
+ dc1394camera_t **cameras;
+ unsigned int num_cameras;
+ dc1394_find_cameras(&cameras, &num_cameras);
+ return 0;
+ }
+ " DC1394_FIND_CAMERAS_FOUND)
+ #MESSAGE("DC1394_FIND_CAMERAS_FOUND: ${DC1394_FIND_CAMERAS_FOUND}")
+ ENDIF(NOT DC1394_CAMERA_ENUMERATE_FOUND)
+
+ IF(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
+ SET(DC1394_FOUND FALSE)
+ MESSAGE("libdc1394-2.x found but not compatible with ViSP...")
+ ELSE(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
+ SET(DC1394_FOUND TRUE)
+ ENDIF(NOT DC1394_CAMERA_ENUMERATE_FOUND AND NOT DC1394_FIND_CAMERAS_FOUND)
+
+ SET(DC1394_LIBRARIES ${DC1394_LIBRARY})
+ SET(DC1394_INCLUDE_DIRS ${DC1394_INCLUDE_DIR})
+ ENDIF(DC1394_LIBRARY AND DC1394_INCLUDE_DIR)
+
+
+
+ ## --------------------------------
+
+ MARK_AS_ADVANCED(
+ DC1394_LIBRARY
+ DC1394_INCLUDE_DIR
+ DC1394_INCLUDE_DIR
+ DC1394_LIBRARIES
+ DC1394_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindDIRECT3D.cmake b/cmake/FindDIRECT3D.cmake
new file mode 100644
index 0000000..91b45ee
--- /dev/null
+++ b/cmake/FindDIRECT3D.cmake
@@ -0,0 +1,143 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Direct3D on Windows. This code comes originaly
+# from http://www.cmake.org/Wiki/CMakeUserFindDirectShow .
+# Once loaded this will define:
+#
+# DIRECT3D_FOUND - system has DirectShow
+# DIRECT3D_INCLUDE_DIRS - include directory for DirectShow
+# DIRECT3D_LIBRARIES - libraries you need to link to
+#
+# Authors:
+# Bruno Renier
+#
+#############################################################################
+
+SET(DIRECT3D_FOUND "NO")
+
+# Direct3D is only available on Windows platforms
+IF(WIN32 AND NOT MINGW)
+ # Find Direct3D Include Directory
+ FIND_PATH(DIRECT3D_INCLUDE_DIR d3dx9.h
+ "$ENV{DXSDK_DIR}/Include"
+ "C:/Program Files/Microsoft DirectX SDK/Include"
+ "C:/DXSDK/Include"
+ NO_DEFAULT_PATH
+ DOC "What is the path where the file d3dx9.h can be found"
+ )
+ FIND_PATH(DIRECT3D_INCLUDE_DIR d3dx9.h
+ NO_DEFAULT_PATH
+ DOC "What is the path where the file d3dx9.h can be found"
+ )
+
+ # if Direct3D include dir found, then find Direct3D libraries
+ IF(DIRECT3D_INCLUDE_DIR)
+ IF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
+ "$ENV{DXSDK_DIR}/Lib"
+ "$ENV{DXSDK_DIR}/Lib/x64"
+ "C:/Program Files/Microsoft DirectX SDK/Lib/x64"
+ "C:/DXSDK/Include/Lib/x64"
+ NO_DEFAULT_PATH
+ DOC "Where can the Direct3D d3d9 library be found"
+ )
+ ELSE(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
+ "$ENV{DXSDK_DIR}/Lib"
+ "$ENV{DXSDK_DIR}/Lib/x86"
+ "C:/Program Files/Microsoft DirectX SDK/Lib/x86"
+ "C:/DXSDK/Include/Lib/x86"
+ NO_DEFAULT_PATH
+ DOC "Where can the Direct3D d3d9 library be found"
+ )
+ ENDIF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3d9_LIBRARY d3d9
+ DOC "Where can the Direct3D d3d9 library be found"
+ )
+
+ IF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
+ "$ENV{DXSDK_DIR}/Lib/"
+ "$ENV{DXSDK_DIR}/Lib/x64"
+ "C:/Program Files/Microsoft DirectX SDK/Lib/x64"
+ "C:/DXSDK/Include/Lib/x64"
+ NO_DEFAULT_PATH
+ DOC "Where can the Direct3D d3dx9 library be found"
+ )
+ ELSE(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
+ "$ENV{DXSDK_DIR}/Lib/"
+ "$ENV{DXSDK_DIR}/Lib/x86"
+ "C:/Program Files/Microsoft DirectX SDK/Lib/x86"
+ "C:/DXSDK/Include/Lib/x86"
+ NO_DEFAULT_PATH
+ DOC "Where can the Direct3D d3dx9 library be found"
+ )
+ ENDIF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECT3D_d3dx9_LIBRARY d3dx9
+ DOC "Where can the Direct3D d3dx9 library be found"
+ )
+
+ # if Direct3D libraries found, then we're ok
+ IF(DIRECT3D_d3d9_LIBRARY)
+ IF(DIRECT3D_d3dx9_LIBRARY)
+ # everything found
+ SET(DIRECT3D_FOUND "YES")
+ ENDIF(DIRECT3D_d3dx9_LIBRARY)
+ ENDIF(DIRECT3D_d3d9_LIBRARY)
+ ENDIF(DIRECT3D_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ DIRECT3D_INCLUDE_DIR
+ DIRECT3D_d3d9_LIBRARY
+ DIRECT3D_d3dx9_LIBRARY
+ )
+
+
+ENDIF(WIN32 AND NOT MINGW)
+
+
+#---------------------------------------------------------------------
+IF(DIRECT3D_FOUND)
+ SET(DIRECT3D_INCLUDE_DIRS ${DIRECT3D_INCLUDE_DIR})
+ SET(DIRECT3D_LIBRARIES
+ "${DIRECT3D_d3d9_LIBRARY}"
+ "${DIRECT3D_d3dx9_LIBRARY}"
+ )
+ELSE(DIRECT3D_FOUND)
+ # make FIND_PACKAGE friendly
+ IF(NOT DIRECT3D_FIND_QUIETLY)
+ IF(DIRECT3D_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR
+ "Direct3D required, please specify it's location.")
+ ENDIF(DIRECT3D_FIND_REQUIRED)
+ ENDIF(NOT DIRECT3D_FIND_QUIETLY)
+ENDIF(DIRECT3D_FOUND)
diff --git a/cmake/FindDIRECTSHOW.cmake b/cmake/FindDIRECTSHOW.cmake
new file mode 100644
index 0000000..5d34ab3
--- /dev/null
+++ b/cmake/FindDIRECTSHOW.cmake
@@ -0,0 +1,280 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Direct Show on Windows. This code comes originaly
+# from http://www.cmake.org/Wiki/CMakeUserFindDirectShow .
+# Once loaded this will define:
+#
+# DIRECTSHOW_FOUND - system has DirectShow
+# DIRECTSHOW_INCLUDE_DIRS - include directory for DirectShow
+# DIRECTSHOW_LIBRARIES - libraries you need to link to
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+SET(DIRECTSHOW_FOUND "NO")
+
+# DirectShow is only available on Windows platforms
+IF(WIN32 AND NOT MINGW)
+# find DirectX
+ FIND_PATH(DIRECTX_INCLUDE_PATH ddraw.h
+ "$ENV{DXSDK_DIR}/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
+ "C:/DXSDK/include"
+ "C:/Program Files/Microsoft Platform SDK/Include"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
+ NO_DEFAULT_PATH
+ DOC "What is the path where the file ddraw.h can be found"
+ )
+ FIND_PATH(DIRECTX_INCLUDE_PATH ddraw.h
+ DOC "What is the path where the file ddraw.h can be found"
+ )
+
+ # find DirectShow include directory
+ FIND_PATH(DIRECTSHOW_dshow_INCLUDE_PATH dshow.h
+ "$ENV{WINSDK_HOME}/Include"
+ "$ENV{DXSDK_DIR}/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
+ "C:/DXSDK/include"
+ "C:/Program Files/Microsoft Platform SDK/Include"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
+ NO_DEFAULT_PATH
+ DOC "What is the path where the file dshow.h can be found"
+ )
+ FIND_PATH(DIRECTSHOW_dshow_INCLUDE_PATH dshow.h
+ DOC "What is the path where the file dshow.h can be found"
+ )
+
+ FIND_PATH(DIRECTSHOW_qedit_INCLUDE_PATH qedit.h
+ "$ENV{WINSDK_HOME}/Include"
+ "$ENV{DXSDK_DIR}/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Include"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Include"
+ "C:/DXSDK/include"
+ "C:/Program Files/Microsoft Platform SDK/Include"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Include"
+ NO_DEFAULT_PATH
+ DOC "What is the path where the file qedit.h can be found"
+ )
+ FIND_PATH(DIRECTSHOW_qedit_INCLUDE_PATH qedit.h
+ DOC "What is the path where the file qedit.h can be found"
+ )
+
+ FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
+ DOC "What is the path where the file atlbase.h can be found"
+ )
+# Specific path search for Visual Studio .NET 2003
+ IF(MSVC71)
+ IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/atlmfc/include"
+ "$ENV{VS71COMNTOOLS}/../../Vc7/atlmfc/include"
+ DOC "What is the path where the file atlbase.h can be found"
+ )
+ ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ ENDIF(MSVC71)
+
+# Specific path search for Visual Studio 2005
+ IF(MSVC80)
+ IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
+ "C:/Program Files/Microsoft Visual Studio 8/VC/atlmfc/include"
+ "$ENV{VS80COMNTOOLS}/../../VC/atlmfc/include"
+ DOC "What is the path where the file atlbase.h can be found"
+ )
+ ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ ENDIF(MSVC80)
+
+# Specific path search for Visual Studio 2008
+ IF(MSVC90)
+ IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
+ "C:/Program Files/Microsoft Visual Studio 9.0/VC/atlmfc/include"
+ "$ENV{VS90COMNTOOLS}/../../VC/atlmfc/include"
+ DOC "What is the path where the file atlbase.h can be found"
+ )
+ ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ ENDIF(MSVC90)
+# Specific path search for Visual Studio 2008
+ IF(MSVC10)
+ IF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ FIND_PATH(DIRECTSHOW_atlbase_INCLUDE_PATH atlbase.h
+ "C:/Program Files/Microsoft Visual Studio 10.0/VC/atlmfc/include"
+ "$ENV{VS100COMNTOOLS}/../../VC/atlmfc/include"
+ DOC "What is the path where the file atlbase.h can be found"
+ )
+ ENDIF(NOT DIRECTSHOW_atlbase_INCLUDE_PATH)
+ ENDIF(MSVC10)
+ IF(DIRECTX_INCLUDE_PATH AND DIRECTSHOW_dshow_INCLUDE_PATH AND DIRECTSHOW_qedit_INCLUDE_PATH AND DIRECTSHOW_atlbase_INCLUDE_PATH)
+ SET(DIRECTSHOW_INCLUDE_DIRS ${DIRECTX_INCLUDE_PATH}
+ ${DIRECTSHOW_dshow_INCLUDE_PATH}
+ ${DIRECTSHOW_qedit_INCLUDE_PATH}
+ ${DIRECTSHOW_atlbase_INCLUDE_PATH})
+ ENDIF(DIRECTX_INCLUDE_PATH AND DIRECTSHOW_dshow_INCLUDE_PATH AND DIRECTSHOW_qedit_INCLUDE_PATH AND DIRECTSHOW_atlbase_INCLUDE_PATH)
+ # if DirectShow include dir found, then find DirectShow libraries
+ IF(DIRECTSHOW_INCLUDE_DIRS)
+ IF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
+ NAMES strmiids
+ PATHS
+ "$ENV{WINSDK_HOME}/Lib/x64"
+ "$ENV{DXSDK_DIR}/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
+ "C:/DXSDK/lib/x64"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x64"
+ "C:/Program Files/Microsoft Platform SDK/Lib/x64"
+ NO_DEFAULT_PATH
+ DOC "Where can the DirectShow strmiids library be found"
+ )
+ ELSE(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
+ NAMES strmiids
+ PATHS
+ "$ENV{WINSDK_HOME}/Lib"
+ "$ENV{WINSDK_HOME}/Lib/x86"
+ "$ENV{DXSDK_DIR}/Lib"
+ "$ENV{DXSDK_DIR}/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib"
+ "C:/DXSDK/lib"
+ "C:/DXSDK/lib/x64"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
+ "C:/Program Files/Microsoft Platform SDK/Lib"
+ NO_DEFAULT_PATH
+ DOC "Where can the DirectShow strmiids library be found"
+ )
+ ENDIF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_strmiids_LIBRARY
+ NAMES strmiids
+ DOC "Where can the DirectShow strmiids library be found"
+ )
+
+
+ IF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
+ NAMES quartz
+ PATHS
+ "$ENV{WINSDK_HOME}/Lib/x64"
+ "$ENV{DXSDK_DIR}/Lib/x64"
+ "C:/DXSDK/lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x64"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x64"
+ "C:/Program Files/Microsoft Platform SDK/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
+ NO_DEFAULT_PATH
+ DOC "Where can the DirectShow quartz library be found"
+ )
+ ELSE(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
+ NAMES quartz
+ PATHS
+ "$ENV{WINSDK_HOME}/Lib/x86"
+ "$ENV{DXSDK_DIR}/Lib/x86"
+ "C:/DXSDK/lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib/x86"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib/x86"
+ "C:/Program Files/Microsoft Platform SDK/Lib/x86"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A"
+ "$ENV{WINSDK_HOME}/Lib"
+ "$ENV{DXSDK_DIR}/Lib"
+ "C:/DXSDK/lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0A/Lib"
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
+ "C:/Program Files/Microsoft Platform SDK/Lib"
+ NO_DEFAULT_PATH
+ DOC "Where can the DirectShow quartz library be found"
+ )
+ ENDIF(CMAKE_CL_64)
+ FIND_LIBRARY(DIRECTSHOW_quartz_LIBRARY
+ NAMES quartz
+ DOC "Where can the DirectShow quartz library be found"
+ )
+
+ # if DirectShow libraries found, then we're ok
+ IF(DIRECTSHOW_strmiids_LIBRARY)
+ IF(DIRECTSHOW_quartz_LIBRARY)
+ # everything found
+ SET(DIRECTSHOW_FOUND "YES")
+ ENDIF(DIRECTSHOW_quartz_LIBRARY)
+ ENDIF(DIRECTSHOW_strmiids_LIBRARY)
+ ENDIF(DIRECTSHOW_INCLUDE_DIRS)
+
+ MARK_AS_ADVANCED(
+ DIRECTSHOW_INCLUDE_DIRS
+ DIRECTSHOW_strmiids_LIBRARY
+ DIRECTSHOW_quartz_LIBRARY
+ DIRECTX_INCLUDE_PATH
+ DIRECTSHOW_dshow_INCLUDE_PATH
+ DIRECTSHOW_qedit_INCLUDE_PATH
+ DIRECTSHOW_atlbase_INCLUDE_PATH
+ )
+
+ENDIF(WIN32 AND NOT MINGW)
+
+
+#---------------------------------------------------------------------
+IF(DIRECTSHOW_FOUND)
+ SET(DIRECTSHOW_LIBRARIES
+ ${DIRECTSHOW_strmiids_LIBRARY}
+ ${DIRECTSHOW_quartz_LIBRARY}
+ )
+ELSE(DIRECTSHOW_FOUND)
+ # make FIND_PACKAGE friendly
+ IF(NOT DIRECTSHOW_FIND_QUIETLY)
+ IF(DIRECTSHOW_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR
+ "DirectShow required, please specify it's location.")
+ ENDIF(DIRECTSHOW_FIND_REQUIRED)
+ ENDIF(NOT DIRECTSHOW_FIND_QUIETLY)
+ENDIF(DIRECTSHOW_FOUND)
diff --git a/cmake/FindDL.cmake b/cmake/FindDL.cmake
new file mode 100644
index 0000000..3ca9e2f
--- /dev/null
+++ b/cmake/FindDL.cmake
@@ -0,0 +1,63 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find dl library.
+# Once run this will define:
+#
+# DL_FOUND
+# DL_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+FIND_LIBRARY(DL_LIBRARY
+ NAMES dl
+ PATHS
+ $ENV{DL_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ )
+
+#MESSAGE("DBG DL_LIBRARY=${DL_LIBRARY}")
+
+IF(DL_LIBRARY)
+ SET(DL_LIBRARIES ${DL_LIBRARY})
+ SET(DL_FOUND TRUE)
+ELSE()
+ SET(DL_FOUND FALSE)
+ENDIF()
+
+MARK_AS_ADVANCED(
+ DL_LIBRARY
+ )
+
diff --git a/cmake/FindDMTX.cmake b/cmake/FindDMTX.cmake
new file mode 100644
index 0000000..a46a160
--- /dev/null
+++ b/cmake/FindDMTX.cmake
@@ -0,0 +1,68 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find dmtx library.
+# Once run this will define:
+#
+# DMTX_FOUND
+# DMTX_INCLUDE_DIRS
+# DMTX_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+find_path(DMTX_INCLUDE_DIRS dmtx.h
+ $ENV{DMTX_DIR}/include
+ /usr/include
+ /usr/local/include
+)
+
+find_library(DMTX_LIBRARIES
+ NAMES dmtx
+ PATHS
+ $ENV{DMTX_DIR}/lib
+ /usr/lib
+ /usr/local/lib
+)
+
+if(DMTX_INCLUDE_DIRS AND DMTX_LIBRARIES)
+ set(DMTX_FOUND TRUE)
+else()
+ set(DMTX_FOUND FALSE)
+endif()
+
+mark_as_advanced(
+ DMTX_INCLUDE_DIRS
+ DMTX_LIBRARIES
+)
+
diff --git a/cmake/FindFFMPEG.cmake b/cmake/FindFFMPEG.cmake
new file mode 100644
index 0000000..86d20f3
--- /dev/null
+++ b/cmake/FindFFMPEG.cmake
@@ -0,0 +1,319 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find FFMPEG. FFMpeg depend son Zlib.
+# Once run this will define:
+#
+# FFMPEG_FOUND - system has FFMPEG
+# FFMPEG_INCLUDE_DIRS - the FFMPEG include directory
+# FFMPEG_LIBRARIES - Link these to use FFMPEG
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# detection of the FFMPEG headers location
+if(MINGW)
+ find_path(FFMPEG_INCLUDE_DIR_AVCODEC
+ NAMES
+ libavcodec/avcodec.h
+ PATHS
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
+ NAMES
+ libavformat/avformat.h
+ PATHS
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_AVUTIL
+ NAMES
+ libavutil/avutil.h
+ PATHS
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_SWSCALE
+ NAMES
+ libswscale/swscale.h
+ PATHS
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ PATH_SUFFIXES
+ libswscale
+ ffmpeg
+ )
+
+ # Detection of the FFMPEG library on Unix
+ find_library(FFMPEG_AVUTIL_LIBRARY
+ NAMES
+ avutil
+ PATHS
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+ find_library(FFMPEG_AVCODEC_LIBRARY
+ NAMES
+ avcodec
+ PATHS
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+ find_library(FFMPEG_AVFORMAT_LIBRARY
+ NAMES
+ avformat
+ PATHS
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+
+ find_library(FFMPEG_AVCORE_LIBRARY
+ NAMES
+ avcore
+ PATHS
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+
+ find_library(FFMPEG_SWSCALE_LIBRARY
+ NAMES
+ swscale
+ PATHS
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+else(MINGW)
+ find_path(FFMPEG_INCLUDE_DIR_AVCODEC
+ NAMES
+ libavcodec/avcodec.h
+ PATHS
+ "/usr/include"
+ "/usr/local/include"
+ $ENV{FFMPEG_DIR}/include
+ $ENV{FFMPEG_DIR}
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
+ NAMES
+ libavformat/avformat.h
+ PATHS
+ "/usr/include"
+ "/usr/local/include"
+ $ENV{FFMPEG_DIR}/include
+ $ENV{FFMPEG_DIR}
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_AVUTIL
+ NAMES
+ libavutil/avutil.h
+ PATHS
+ "/usr/include"
+ "/usr/local/include"
+ $ENV{FFMPEG_DIR}/include
+ $ENV{FFMPEG_DIR}
+ PATH_SUFFIXES
+ ffmpeg
+ )
+
+ find_path(FFMPEG_INCLUDE_DIR_SWSCALE
+ NAMES
+ libswscale/swscale.h
+ PATHS
+ "/usr/include"
+ "/usr/local/include"
+ $ENV{FFMPEG_DIR}/include
+ $ENV{FFMPEG_DIR}
+ PATH_SUFFIXES
+ libswscale
+ ffmpeg
+ )
+
+ # Detection of the FFMPEG library on Unix
+ find_library(FFMPEG_AVUTIL_LIBRARY
+ NAMES
+ avutil
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{FFMPEG_DIR}/lib
+ $ENV{FFMPEG_DIR}/Release
+ $ENV{FFMPEG_DIR}
+ )
+ find_library(FFMPEG_AVCODEC_LIBRARY
+ NAMES
+ avcodec
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{FFMPEG_DIR}/lib
+ $ENV{FFMPEG_DIR}/Release
+ $ENV{FFMPEG_DIR}
+ )
+ find_library(FFMPEG_AVFORMAT_LIBRARY
+ NAMES
+ avformat
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{FFMPEG_DIR}/lib
+ $ENV{FFMPEG_DIR}/Release
+ $ENV{FFMPEG_DIR}
+ )
+
+ find_library(FFMPEG_AVCORE_LIBRARY
+ NAMES
+ avcore
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{FFMPEG_DIR}/lib
+ $ENV{FFMPEG_DIR}/Release
+ $ENV{FFMPEG_DIR}
+ )
+
+ find_library(FFMPEG_SWSCALE_LIBRARY
+ NAMES
+ swscale
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{FFMPEG_DIR}/lib
+ $ENV{FFMPEG_DIR}/Release
+ $ENV{FFMPEG_DIR}
+ )
+endif(MINGW)
+
+ # FFMpeg depend son Zlib
+ FIND_PACKAGE(ZLIB)
+ if(NOT ZLIB_FOUND)
+ FIND_PACKAGE(MyZLIB)
+ endif()
+
+ # FFMpeg depend son BZip2
+ # with CMake 2.6, the CMake bzip2 package material is named FindBZip2.cmake
+ # while with CMake 2.8, the name is FindBZIP2.cmake
+ # that is why we need to call FIND_PACKAGE(BZip2) and FIND_PACKAGE(BZIP2)
+ FIND_PACKAGE(BZIP2 QUIET)
+ # MESSAGE("BZIP2_FOUND: ${BZIP2_FOUND}")
+ if(NOT BZIP2_FOUND)
+ FIND_PACKAGE(BZip2 QUIET)
+ # MESSAGE("BZIP2_FOUND: ${BZIP2_FOUND}")
+ endif()
+
+ # FFMpeg may depend also on iconv since probably version 1.1.3 where if detected,
+ # iconv usage is enabled by default
+ find_package(ICONV QUIET)
+ #message("ICONV_FOUND: ${ICONV_FOUND}")
+
+IF(FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES AND BZIP2_LIBRARIES)
+ SET(FFMPEG_FOUND TRUE)
+ SET(FFMPEG_INCLUDE_DIRS
+ ${FFMPEG_INCLUDE_DIR_AVCODEC}
+ ${FFMPEG_INCLUDE_DIR_AVFORMAT}
+ ${FFMPEG_INCLUDE_DIR_AVUTIL}
+ ${FFMPEG_INCLUDE_DIR_SWSCALE}
+ )
+ SET(FFMPEG_LIBRARIES
+ ${FFMPEG_SWSCALE_LIBRARY}
+ ${FFMPEG_AVFORMAT_LIBRARY}
+ ${FFMPEG_AVCODEC_LIBRARY}
+ ${FFMPEG_AVUTIL_LIBRARY}
+ )
+ if(FFMPEG_AVCORE_LIBRARY)
+ LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
+ endif()
+ list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES} ${BZIP2_LIBRARIES})
+ if(ICONV_FOUND)
+ list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
+ endif()
+
+elseif(MINGW AND FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES)
+ # Bzip2 is nor requested with mingw-w64
+ SET(FFMPEG_FOUND TRUE)
+ SET(FFMPEG_INCLUDE_DIRS
+ ${FFMPEG_INCLUDE_DIR_AVCODEC}
+ ${FFMPEG_INCLUDE_DIR_AVFORMAT}
+ ${FFMPEG_INCLUDE_DIR_AVUTIL}
+ ${FFMPEG_INCLUDE_DIR_SWSCALE}
+ )
+ SET(FFMPEG_LIBRARIES
+ ${FFMPEG_SWSCALE_LIBRARY}
+ ${FFMPEG_AVFORMAT_LIBRARY}
+ ${FFMPEG_AVCODEC_LIBRARY}
+ ${FFMPEG_AVUTIL_LIBRARY}
+ )
+ if(FFMPEG_AVCORE_LIBRARY)
+ LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
+ endif()
+ list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES})
+ if(ICONV_FOUND)
+ list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
+ endif()
+
+ELSE()
+ SET(FFMPEG_FOUND FALSE)
+ENDIF ()
+
+MARK_AS_ADVANCED(
+ BZIP2_DIR
+ FFMPEG_INCLUDE_DIR_AVCODEC
+ FFMPEG_INCLUDE_DIR_AVFORMAT
+ FFMPEG_INCLUDE_DIR_AVUTIL
+ FFMPEG_INCLUDE_DIR_SWSCALE
+ FFMPEG_AVUTIL_LIBRARY
+ FFMPEG_AVFORMAT_LIBRARY
+ FFMPEG_AVCODEC_LIBRARY
+ FFMPEG_SWSCALE_LIBRARY
+ FFMPEG_AVCORE_LIBRARY
+ FFMPEG_INCLUDE_DIRS
+ FFMPEG_LIBRARIES
+)
+
diff --git a/cmake/FindGDI.cmake b/cmake/FindGDI.cmake
new file mode 100644
index 0000000..0248e44
--- /dev/null
+++ b/cmake/FindGDI.cmake
@@ -0,0 +1,158 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find GDI (Graphics Device Interface) on Windows.
+# Once loaded this will define:
+#
+# GDI_FOUND - system has GDI
+# GDI_LIBRARIES - libraries you need to link to
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+SET(GDI_FOUND "NO")
+
+# GDI (Graphics Device Interface) is only available on Windows platforms
+IF(WIN32)
+ IF(MINGW)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/MinGW/lib"
+ "C:/mingw/mingw/lib"
+ "$ENV{MINGW_DIR}/lib"
+ "$ENV{MINGW_DIR}/mingw/lib"
+ DOC "Where can the GDI (Graphics Device Interface) library be found"
+ NO_DEFAULT_PATH
+ )
+ ELSE(MINGW)
+ IF(CMAKE_CL_64)
+ # Generic path search
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "$ENV{WINSDK_DIR}/Lib/x64"
+ "$ENV{WINSDK_HOME}/Lib/x64"
+ "$ENV{DXSDK_DIR}/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0a/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.1/Lib/x64"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.1A/Lib/x64"
+ "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.0A/Lib/x64"
+ "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib/x64"
+ "C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x64"
+ "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x64"
+ "C:/Program Files/Microsoft Platform SDK/Lib/x64"
+ "C:/DXSDK/Include/Lib/x64"
+ DOC "Where can the GDI (Graphics Device Interface) library be found"
+ )
+
+ # Specific path search for Visual Studio .NET 2003
+ IF(MSVC71)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
+ )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC71)
+
+ # Specific path search for Visual Studio 2005
+ IF(MSVC80)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio 8/VC/PlatformSDK/Lib/AMD64" )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC80)
+
+ # Specific path search for Visual Studio 2008
+ IF(MSVC90)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio 9/VC/PlatformSDK/Lib/AMD64" )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC90)
+
+ ELSE(CMAKE_CL_64)
+ # Generic path search
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "$ENV{WINSDK_DIR}/Lib"
+ "$ENV{WINSDK_HOME}/Lib"
+ "$ENV{DXSDK_DIR}/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0a/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.0/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v6.1/Lib"
+ "C:/Program Files/Microsoft SDKs/Windows/v7.0A/Lib"
+ "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.0A/Lib"
+ "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1/Lib"
+ "C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib"
+ "C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x86"
+ "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x86"
+ "C:/Program Files/Microsoft Platform SDK/Lib"
+ "C:/DXSDK/Include/Lib"
+ DOC "Where can the GDI (Graphics Device Interface) library be found"
+ )
+
+ # Specific path search for Visual Studio .NET 2003
+ IF(MSVC71)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio .NET 2003/Vc7/PlatformSDK/Lib"
+ )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC71)
+
+ # Specific path search for Visual Studio 2005
+ IF(MSVC80)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio 8/VC/PlatformSDK/Lib"
+ )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC80)
+
+ # Specific path search for Visual Studio 2008
+ IF(MSVC90)
+ IF(NOT GDI_LIBRARY)
+ FIND_LIBRARY(GDI_LIBRARY gdi32
+ "C:/Program Files/Microsoft Visual Studio 9/VC/PlatformSDK/Lib"
+ )
+ ENDIF(NOT GDI_LIBRARY)
+ ENDIF(MSVC90)
+ ENDIF(CMAKE_CL_64)
+ ENDIF(MINGW)
+ # if GDI libraries found, then we're ok
+ IF(GDI_LIBRARY)
+ SET(GDI_FOUND "YES")
+ SET(GDI_LIBRARIES ${GDI_LIBRARY})
+
+ ENDIF(GDI_LIBRARY)
+
+ MARK_AS_ADVANCED(GDI_LIBRARY)
+ENDIF(WIN32)
diff --git a/cmake/FindGSL.cmake b/cmake/FindGSL.cmake
new file mode 100644
index 0000000..9f84470
--- /dev/null
+++ b/cmake/FindGSL.cmake
@@ -0,0 +1,180 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find gnu scientific library GSL
+# (see http://www.gnu.org/software/gsl/)
+# Once run this will define:
+#
+# GSL_FOUND : system has GSL lib
+# GSL_LIBRARIES : full path to the libraries
+# GSL_INCLUDE_DIRS : where to find headers
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# it seems that a macro() can't accept a list as input. That's why we have
+# MY_LIBRARY1 and MY_LIBRARY2
+macro(CheckCompilation_gsl2 MY_INCLUDE_DIR MY_LIBRARY1 MY_LIBRARY2 MY_BUILD_SUCCEED2)
+ # The material is found. Check if it works on the requested architecture
+ include(CheckCXXSourceCompiles)
+ #message("Test compilation: ${MY_INCLUDE_DIR} + ${MY_LIBRARY1} + ${MY_LIBRARY2}")
+
+ set(CMAKE_REQUIRED_LIBRARIES ${MY_LIBRARY1} ${MY_LIBRARY2})
+ set(CMAKE_REQUIRED_INCLUDES ${MY_INCLUDE_DIR})
+ check_cxx_source_compiles("
+ #include <gsl/gsl_linalg.h> // Contrib for GSL library
+ #include <gsl/gsl_math.h>
+ #include <gsl/gsl_eigen.h>
+ int main()
+ {
+ gsl_matrix *A2 = gsl_matrix_alloc(6, 6) ;
+ }
+ " BUILD_SUCCEED2)
+ #message("BUILD_SUCCEED: ${BUILD_SUCCEED}")
+ set(${MY_BUILD_SUCCEED2} ${BUILD_SUCCEED2})
+endmacro()
+
+macro(CheckCompilation_gsl1 MY_INCLUDE_DIR MY_LIBRARY MY_BUILD_SUCCEED)
+ # The material is found. Check if it works on the requested architecture
+ include(CheckCXXSourceCompiles)
+ #message("Test compilation: ${MY_INCLUDE_DIR} + ${MY_LIBRARY}")
+
+ set(CMAKE_REQUIRED_LIBRARIES ${MY_LIBRARY})
+ set(CMAKE_REQUIRED_INCLUDES ${MY_INCLUDE_DIR})
+ check_cxx_source_compiles("
+ #include <gsl/gsl_linalg.h> // Contrib for GSL library
+ #include <gsl/gsl_math.h>
+ #include <gsl/gsl_eigen.h>
+ int main()
+ {
+ gsl_matrix *A1 = gsl_matrix_alloc(6, 6) ;
+ }
+ " BUILD_SUCCEED)
+ #message("BUILD_SUCCEED: ${BUILD_SUCCEED}")
+ set(${MY_BUILD_SUCCEED} ${BUILD_SUCCEED})
+endmacro()
+
+set(GSL_LIB_SEARCH_PATH
+ "$ENV{GSL_HOME}/lib"
+ "$ENV{GSL_DIR}/lib"
+ /usr/lib
+ /usr/local/lib
+)
+set(GSL_INC_SEARCH_PATH
+ $ENV{GSL_HOME}/include
+ $ENV{GSL_DIR}/include
+ /usr/include
+ /usr/local/include
+)
+
+find_path(GSL_INCLUDE_DIR gsl/gsl_linalg.h
+ ${GSL_INC_SEARCH_PATH}
+)
+
+find_library(GSL_gsl_LIBRARY
+ NAMES gsl
+ PATHS ${GSL_LIB_SEARCH_PATH}
+)
+
+if(WIN32)
+ find_library(GSL_cblas_LIBRARY
+ NAMES cblas
+ PATHS ${GSL_LIB_SEARCH_PATH}
+ )
+
+ find_library(GSL_gsl_LIBRARY_DEBUG
+ NAMES gsl_d
+ PATHS ${GSL_LIB_SEARCH_PATH}
+ )
+ find_library(GSL_cblas_LIBRARY_DEBUG
+ NAMES cblas_d
+ PATHS ${GSL_LIB_SEARCH_PATH}
+ )
+
+ if(GSL_INCLUDE_DIR AND GSL_gsl_LIBRARY AND GSL_cblas_LIBRARY
+ AND GSL_gsl_LIBRARY_DEBUG AND GSL_cblas_LIBRARY_DEBUG)
+ set(GSL_LIBRARIES "optimized" ${GSL_cblas_LIBRARY}
+ "optimized" ${GSL_gsl_LIBRARY}
+ "debug" ${GSL_cblas_LIBRARY_DEBUG}
+ "debug" ${GSL_gsl_LIBRARY_DEBUG})
+ set(GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR})
+ set(GSL_FOUND TRUE)
+ else()
+ set(GSL_FOUND FALSE)
+ endif()
+
+ mark_as_advanced(
+ GSL_gsl_LIBRARY_DEBUG
+ GSL_cblas_LIBRARY_DEBUG
+ )
+else()
+ if(GSL_INCLUDE_DIR AND GSL_gsl_LIBRARY)
+ # Check if gsl library is sufficient
+ set(GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR})
+ set(GSL_LIBRARIES ${GSL_gsl_LIBRARY})
+
+ CheckCompilation_gsl1(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} BUILD_SUCCEED1)
+ #message("BUILD_STATUS 1: ${BUILD_SUCCEED1}")
+ if(BUILD_SUCCEED1)
+ set(GSL_FOUND TRUE)
+ else()
+ # Try to add gslcblas library if requested
+
+ find_library(GSL_cblas_LIBRARY
+ NAMES gslcblas
+ PATHS ${GSL_LIB_SEARCH_PATH}
+ )
+ if(GSL_cblas_LIBRARY)
+ list(APPEND GSL_LIBRARIES ${GSL_cblas_LIBRARY})
+ #message("add cblas to GSL_LIBRARIES: ${GSL_LIBRARIES}")
+ CheckCompilation_gsl2(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} ${GSL_cblas_LIBRARY} BUILD_SUCCEED2)
+ #message("BUILD_STATUS 2: ${BUILD_SUCCEED2}")
+ if(BUILD_SUCCEED2)
+ set(GSL_FOUND TRUE)
+ else()
+ set(GSL_FOUND FALSE)
+ endif()
+ else()
+ set(GSL_FOUND FALSE)
+ endif()
+ endif()
+ else()
+ set(GSL_FOUND FALSE)
+ endif()
+endif()
+
+mark_as_advanced(
+ GSL_gsl_LIBRARY
+ GSL_cblas_LIBRARY
+ GSL_INCLUDE_DIR
+)
+
diff --git a/cmake/FindICONV.cmake b/cmake/FindICONV.cmake
new file mode 100644
index 0000000..6277c84
--- /dev/null
+++ b/cmake/FindICONV.cmake
@@ -0,0 +1,86 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find ICONV library.
+# Once run this will define:
+#
+# ICONV_FOUND
+# ICONV_INCLUDE_DIRS
+# ICONV_LIBRARIES
+#
+# Authors:
+# Anthony Saunier
+#
+#############################################################################
+
+if(MINGW)
+ find_path(ICONV_INCLUDE_DIR iconv.h
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ )
+ find_library(ICONV_LIBRARY iconv
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+else()
+ find_path(ICONV_INCLUDE_DIR iconv.h
+ $ENV{ICONV_DIR}/include
+ $ENV{ICONV_HOME}/include
+ $ENV{XML2_DIR}/include
+ $ENV{XML2_HOME}/include
+ "c:/libxml2/include"
+ "c:/iconv/include"
+ )
+ find_library(ICONV_LIBRARY iconv
+ $ENV{ICONV_DIR}/lib
+ $ENV{ICONV_HOME}/lib
+ $ENV{XML2_DIR}/lib
+ $ENV{XML2_HOME}/lib
+ "c:/libxml2/lib"
+ "c:/iconv/lib"
+ )
+endif()
+
+if(ICONV_LIBRARY)
+ SET(ICONV_LIBRARIES ${ICONV_LIBRARY})
+endif()
+
+if(ICONV_LIBRARIES AND ICONV_INCLUDE_DIR)
+ SET(ICONV_INCLUDE_DIRS ${ICONV_INCLUDE_DIR})
+ SET(ICONV_FOUND TRUE)
+else()
+ SET(ICONV_FOUND FALSE)
+endif()
+
+mark_as_advanced(
+ ICONV_INCLUDE_DIR
+ ICONV_LIBRARIES
+ ICONV_LIBRARY
+)
diff --git a/cmake/FindIRISA.cmake b/cmake/FindIRISA.cmake
new file mode 100644
index 0000000..42c9543
--- /dev/null
+++ b/cmake/FindIRISA.cmake
@@ -0,0 +1,88 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Cerebellum Irisa library API to dial with the robot Afma6
+#
+# IRISA_FOUND
+# IRISA_INCLUDE_DIRS
+# IRISA_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindIRISA.cmake: only available for Unix.")
+ SET(IRISA_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_PATH(IRISA_INCLUDE_DIR irisa_Afma6.h
+ $ENV{IRISA_HOME}/include
+ /local/soft/Cerebellum/Irisa/current/src
+ /home/soft/Cerebellum/Irisa/current/src
+ )
+ #MESSAGE("DBG IRISA_INCLUDE_DIR=${IRISA_INCLUDE_DIR}")
+
+ FIND_LIBRARY(IRISA_LIBRARY
+ NAMES irisa
+ PATHS
+ $ENV{IRISA_HOME}/lib
+ /local/soft/Cerebellum/Irisa/current/src
+ /home/soft/Cerebellum/Irisa/current/src
+ )
+
+ #MESSAGE("DBG IRISA_LIBRARY=${IRISA_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(IRISA_LIBRARY)
+ SET(IRISA_LIBRARIES ${IRISA_LIBRARY})
+ ELSE(IRISA_LIBRARY)
+ #MESSAGE(SEND_ERROR "Irisa library not found.")
+ ENDIF(IRISA_LIBRARY)
+
+ IF(NOT IRISA_INCLUDE_DIR)
+ #MESSAGE(SEND_ERROR "Irisa include dir not found.")
+ ENDIF(NOT IRISA_INCLUDE_DIR)
+
+ IF(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
+ SET(IRISA_INCLUDE_DIRS ${IRISA_INCLUDE_DIR})
+ SET(IRISA_FOUND TRUE)
+ ELSE(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
+ SET(IRISA_FOUND FALSE)
+ ENDIF(IRISA_LIBRARIES AND IRISA_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ IRISA_INCLUDE_DIR
+ IRISA_LIBRARIES
+ IRISA_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindIsInf.cmake b/cmake/FindIsInf.cmake
new file mode 100644
index 0000000..a1b4ce9
--- /dev/null
+++ b/cmake/FindIsInf.cmake
@@ -0,0 +1,70 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find isnan macro, std::isnan function or _isnan function.
+#
+# Authors:
+# Souriya Trinh
+#
+#############################################################################
+
+include(CheckIncludeFile)
+include(CheckIncludeFiles)
+include(CheckCXXSourceCompiles)
+
+macro(check_math_expr _expr _var)
+ check_cxx_source_compiles("
+#include <cmath>
+int main(int argc, char ** argv)
+{
+ (void)${_expr};
+ return 0;
+}
+" ${_var})
+endmacro()
+
+check_include_files("float.h" HAVE_FLOAT_H)
+check_math_expr("isinf(1.0)" HAVE_FUNC_ISINF)
+check_math_expr("std::isinf(1.0)" HAVE_FUNC_STD_ISINF)
+
+if(HAVE_FLOAT_H)
+ # The version that should work with MSVC
+ check_cxx_source_compiles("
+#include <float.h>
+int main(int argc, char ** argv)
+{
+ (void)_finite(1.0);
+ return 0;
+}
+" HAVE_FUNC__ISINF)
+else()
+ set(HAVE_FUNC__FINITE FALSE)
+endif()
+
diff --git a/cmake/FindIsNaN.cmake b/cmake/FindIsNaN.cmake
new file mode 100644
index 0000000..13325d6
--- /dev/null
+++ b/cmake/FindIsNaN.cmake
@@ -0,0 +1,70 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find isnan macro, std::isnan function or _isnan function.
+#
+# Authors:
+# Souriya Trinh
+#
+#############################################################################
+
+include(CheckIncludeFile)
+include(CheckIncludeFiles)
+include(CheckCXXSourceCompiles)
+
+macro(check_math_expr _expr _var)
+ check_cxx_source_compiles("
+#include <cmath>
+int main(int argc, char ** argv)
+{
+ (void)${_expr};
+ return 0;
+}
+" ${_var})
+endmacro()
+
+check_include_files("float.h" HAVE_FLOAT_H)
+check_math_expr("isnan(1.0)" HAVE_FUNC_ISNAN)
+check_math_expr("std::isnan(1.0)" HAVE_FUNC_STD_ISNAN)
+
+if(HAVE_FLOAT_H)
+ # The version that should work with MSVC
+ check_cxx_source_compiles("
+#include <float.h>
+int main(int argc, char ** argv)
+{
+ (void)_isnan(1.0);
+ return 0;
+}
+" HAVE_FUNC__ISNAN)
+else()
+ set(HAVE_FUNC__ISNAN FALSE)
+endif()
+
diff --git a/cmake/FindLAPACK_C.cmake b/cmake/FindLAPACK_C.cmake
new file mode 100644
index 0000000..0a2f3ac
--- /dev/null
+++ b/cmake/FindLAPACK_C.cmake
@@ -0,0 +1,145 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find lapack, the Linear Algebra PACKage.
+# Since the FindLAPACK.cmake provided with CMake is for Fortran language,
+# this file is able to detect lapack for C language.
+# Once run this will define:
+#
+# LAPACK_C_FOUND
+# LAPACK_C_LIBRARIES
+#
+# Authors:
+# Filip Novotny
+# Fabien Spindler
+#
+#############################################################################
+
+set(LAPACK_C_FOUND FALSE)
+set(LAPACK_C_LIBRARIES "")
+if(WIN32)
+ set(LAPACK_C_LIB_SEARCH_PATH
+ $ENV{LAPACK_C_HOME}
+ $ENV{LAPACK_C_DIR}
+ $ENV{LAPACK_C_HOME}/lib
+ $ENV{LAPACK_C_DIR}/lib
+ $ENV{LAPACK_HOME}
+ $ENV{LAPACK_DIR}
+ $ENV{LAPACK_HOME}/lib
+ $ENV{LAPACK_DIR}/lib
+ )
+
+ find_library(LAPACK_C_LIBRARY_LAPACK_C_RELEASE
+ NAMES lapack
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_BLAS_RELEASE
+ NAMES blas
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_F2C_RELEASE
+ NAMES libf2c
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_LAPACK_C_DEBUG
+ NAMES lapackd
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_BLAS_DEBUG
+ NAMES blasd
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_F2C_DEBUG
+ NAMES libf2cd
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ if((LAPACK_C_LIBRARY_LAPACK_C_RELEASE AND LAPACK_C_LIBRARY_BLAS_RELEASE AND LAPACK_C_LIBRARY_F2C_RELEASE))
+ list(APPEND LAPACK_C_LIBRARIES optimized ${LAPACK_C_LIBRARY_LAPACK_C_RELEASE})
+ list(APPEND LAPACK_C_LIBRARIES optimized ${LAPACK_C_LIBRARY_BLAS_RELEASE})
+ list(APPEND LAPACK_C_LIBRARIES optimized ${LAPACK_C_LIBRARY_F2C_RELEASE})
+ set(LAPACK_C_FOUND TRUE)
+ endif()
+ if((LAPACK_C_LIBRARY_LAPACK_C_DEBUG AND LAPACK_C_LIBRARY_BLAS_DEBUG AND LAPACK_C_LIBRARY_F2C_DEBUG))
+ list(APPEND LAPACK_C_LIBRARIES debug ${LAPACK_C_LIBRARY_LAPACK_C_DEBUG})
+ list(APPEND LAPACK_C_LIBRARIES debug ${LAPACK_C_LIBRARY_BLAS_DEBUG})
+ list(APPEND LAPACK_C_LIBRARIES debug ${LAPACK_C_LIBRARY_F2C_DEBUG})
+ set(LAPACK_C_FOUND TRUE)
+ endif()
+
+else(WIN32)
+
+ set(LAPACK_C_LIB_SEARCH_PATH
+ $ENV{LAPACK_C_HOME}
+ $ENV{LAPACK_C_DIR}
+ $ENV{LAPACK_C_HOME}/lib
+ $ENV{LAPACK_C_DIR}/lib
+ $ENV{LAPACK_HOME}
+ $ENV{LAPACK_DIR}
+ $ENV{LAPACK_HOME}/lib
+ $ENV{LAPACK_DIR}/lib
+ /usr/lib
+ /usr/lib64
+ /usr/local/lib
+ /usr/local/lib64
+ )
+ find_library(LAPACK_C_LIBRARY_LAPACK
+ NAMES lapack
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ find_library(LAPACK_C_LIBRARY_BLAS
+ NAMES blas
+ PATHS ${LAPACK_C_LIB_SEARCH_PATH}
+ )
+
+ if((LAPACK_C_LIBRARY_LAPACK AND LAPACK_C_LIBRARY_BLAS))
+ set(LAPACK_C_LIBRARIES ${LAPACK_C_LIBRARY_LAPACK} ${LAPACK_C_LIBRARY_BLAS})
+ set(LAPACK_C_FOUND TRUE)
+ endif()
+endif(WIN32)
+
+mark_as_advanced(
+ LAPACK_C_LIBRARIES
+ LAPACK_C_LIBRARY_LAPACK
+ LAPACK_C_LIBRARY_BLAS
+ LAPACK_C_LIBRARY_LAPACK_C_RELEASE
+ LAPACK_C_LIBRARY_BLAS_RELEASE
+ LAPACK_C_LIBRARY_LAPACK_C_DEBUG
+ LAPACK_C_LIBRARY_BLAS_DEBUG
+ LAPACK_C_LIBRARY_F2C_DEBUG
+ LAPACK_C_LIBRARY_F2C_RELEASE
+)
+
diff --git a/cmake/FindLIBFREENECT.cmake b/cmake/FindLIBFREENECT.cmake
new file mode 100644
index 0000000..3260836
--- /dev/null
+++ b/cmake/FindLIBFREENECT.cmake
@@ -0,0 +1,95 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Cerebellum Irisa library API to dial with the robot Afma6
+#
+# LIBFREENECT_FOUND
+# LIBFREENECT_INCLUDE_DIRS
+# LIBFREENECT_LIBRARIES
+#
+# Authors:
+# Celine Teuliere
+# Fabien Spindler
+
+IF(WIN32)
+ FIND_PATH(LIBFREENECT_HPP_INCLUDE_DIR libfreenect.hpp
+ $ENV{LIBFREENECT_HOME}/include
+ $ENV{LIBFREENECT_HPP_INCLUDE_DIR}
+ $ENV{LIBFREENECT_H_INCLUDE_DIR}
+ )
+ FIND_PATH(LIBFREENECT_H_INCLUDE_DIR libfreenect.h
+ $ENV{LIBFREENECT_HOME}/include
+ $ENV{LIBFREENECT_HPP_INCLUDE_DIR}
+ $ENV{LIBFREENECT_H_INCLUDE_DIR}
+ )
+ FIND_LIBRARY(LIBFREENECT_LIBRARY freenect
+ $ENV{LIBFREENECT_HOME}/lib
+ $ENV{LIBFREENECT_LIBRARY_DIR}
+ "c:/libfreenect/lib"
+ )
+ELSE()
+ FIND_PATH(LIBFREENECT_HPP_INCLUDE_DIR libfreenect.hpp
+ $ENV{LIBFREENECT_HOME}/include
+ /usr/include
+ /usr/local/include
+ /usr/include/libfreenect
+ /usr/local/include/libfreenect
+ )
+ FIND_PATH(LIBFREENECT_H_INCLUDE_DIR libfreenect.h
+ $ENV{LIBFREENECT_HOME}/include
+ /usr/include
+ /usr/local/include
+ /usr/include/libfreenect
+ /usr/local/include/libfreenect
+ )
+ FIND_LIBRARY(LIBFREENECT_LIBRARY freenect
+ $ENV{LIBFREENECT_HOME}/lib
+ $ENV{LIBFREENECT_HOME}/build/lib
+ /usr/lib
+ /usr/local/lib
+ )
+ENDIF()
+
+## --------------------------------
+IF(LIBFREENECT_LIBRARY AND LIBFREENECT_HPP_INCLUDE_DIR AND LIBFREENECT_H_INCLUDE_DIR)
+ SET(LIBFREENECT_INCLUDE_DIRS ${LIBFREENECT_HPP_INCLUDE_DIR} ${LIBFREENECT_H_INCLUDE_DIR})
+ SET(LIBFREENECT_LIBRARIES ${LIBFREENECT_LIBRARY})
+ SET(LIBFREENECT_FOUND TRUE)
+ELSE()
+ SET(LIBFREENECT_FOUND FALSE)
+ENDIF()
+
+MARK_AS_ADVANCED(
+ LIBFREENECT_INCLUDE_DIRS
+ LIBFREENECT_HPP_INCLUDE_DIR
+ LIBFREENECT_H_INCLUDE_DIR
+ LIBFREENECT_LIBRARIES
+ LIBFREENECT_LIBRARY
+ )
diff --git a/cmake/FindLIBUSB_1.cmake b/cmake/FindLIBUSB_1.cmake
new file mode 100644
index 0000000..05e9573
--- /dev/null
+++ b/cmake/FindLIBUSB_1.cmake
@@ -0,0 +1,83 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libusb-1.0 library. Headers include dir is not searched.
+#
+# LIBUSB_1_FOUND
+# LIBUSB_1_LIBRARIES
+# LIBUSB_1_INCLUDE_DIRS
+#
+# Authors:
+# Celine Teuliere
+# Fabien Spindler
+
+IF(WIN32)
+ FIND_LIBRARY(LIBUSB_1_LIBRARY libusb
+ $ENV{LIBUSB_1_HOME}
+ $ENV{LIBUSB_1_HOME}/lib
+ "c:/libusb/lib"
+ )
+ELSE(WIN32)
+ FIND_LIBRARY(LIBUSB_1_LIBRARY usb-1.0
+ $ENV{LIBUSB_1_HOME}/lib
+ $ENV{LIBUSB_1_HOME}/build/lib
+ /lib/
+ /lib64/
+ /usr/lib
+ /usr/local/lib
+ )
+ FIND_PATH(LIBUSB_1_INCLUDE_DIR libusb.h
+ $ENV{LIBUSB_1_HOME}/include/libusb-1.0
+ $ENV{LIBUSB_1_HOME}/build/include/libusb-1.0
+ /usr/include/libusb-1.0
+ /usr/local/include/libusb-1.0
+ )
+ENDIF(WIN32)
+
+## --------------------------------
+
+IF(LIBUSB_1_LIBRARY AND LIBUSB_1_INCLUDE_DIR)
+ SET(LIBUSB_1_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIR})
+ SET(LIBUSB_1_LIBRARIES ${LIBUSB_1_LIBRARY})
+ SET(LIBUSB_1_FOUND TRUE)
+ELSE()
+ SET(LIBUSB_1_FOUND FALSE)
+ENDIF()
+
+MARK_AS_ADVANCED(
+ LIBUSB_1_INCLUDE_DIRS
+ LIBUSB_1_INCLUDE_DIR
+ LIBUSB_1_LIBRARIES
+ LIBUSB_1_LIBRARY
+ )
+
+
+
+
diff --git a/cmake/FindMyCoin3D.cmake b/cmake/FindMyCoin3D.cmake
new file mode 100644
index 0000000..c7c211d
--- /dev/null
+++ b/cmake/FindMyCoin3D.cmake
@@ -0,0 +1,228 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find Coin library. Try first to see if Coin3D-3 is available. If not,
+# check for Coin3D-2.
+# Once run this will define:
+#
+# COIN3D_FOUND
+# COIN3D_LIBRARIES
+# COIN3D_INCLUDE_DIRS
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+MACRO(TRY_COMPILE_WITH_COIN COIN3D_LIB COIN3D_INC)
+
+ # Try to compile a sample code using Coin release library
+ include(CheckCXXSourceCompiles)
+
+ SET(CMAKE_REQUIRED_LIBRARIES "${COIN3D_LIB}")
+ SET(CMAKE_REQUIRED_INCLUDES "${COIN3D_INC}")
+ SET(CMAKE_REQUIRED_DEFINITIONS "-DCOIN_DLL")
+ #MESSAGE("COIN3D_LIB: ${COIN3D_LIB}")
+ #MESSAGE("COIN3D_INC: ${COIN3D_INC}")
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <Inventor/nodes/SoSeparator.h>
+ int main(){
+ SoSeparator *scene = new SoSeparator;
+ return 0;
+ }
+ " IS_COMPILER_COMPATIBLE)
+ #MESSAGE("DBG1 IS_COMPILER_COMPATIBLE ${IS_COMPILER_COMPATIBLE}")
+
+ENDMACRO(TRY_COMPILE_WITH_COIN)
+
+IF(UNIX OR WIN32)
+ FIND_PATH(COIN3D_INCLUDE_DIR Inventor/nodes/SoSeparator.h
+ $ENV{COIN_DIR}/include
+ $ENV{COINDIR}/include
+ $ENV{COIN_DIR}
+ $ENV{COINDIR}
+ $ENV{COIN3D_DIR}/include
+ $ENV{COIN3DDIR}/include
+ /usr/include
+ /usr/include/Coin2
+ /Library/Frameworks/Inventor.framework/Headers
+ )
+ #MESSAGE("DBG COIN3D_INCLUDE_DIR=${COIN3D_INCLUDE_DIR}")
+
+
+ IF(WIN32)
+ # Try first to find Coin3D-3 and only if not found Coin3D-2
+ FIND_LIBRARY(COIN3D_LIBRARY_RELEASE
+ NAMES coin3 coin4 #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COIN3DDIR}/lib"
+ "$ENV{COIN3D_DIR}/lib"
+ )
+
+ FIND_LIBRARY(COIN3D_LIBRARY_DEBUG
+ NAMES coin3d coin4d #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COIN3DDIR}/lib"
+ "$ENV{COIN3D_DIR}/lib"
+ )
+
+ #MESSAGE("DBG COIN3D_LIBRARY_RELEASE=${COIN3D_LIBRARY_RELEASE}")
+ #MESSAGE("DBG COIN3D_LIBRARY_DEBUG=${COIN3D_LIBRARY_DEBUG}")
+
+ ELSE(WIN32)
+ FIND_LIBRARY(COIN3D_LIBRARY
+ NAMES Coin
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COIN3DDIR}/lib"
+ "$ENV{COIN3D_DIR}/lib"
+ /Library/Frameworks/Inventor.framework/Libraries
+ )
+
+ #MESSAGE("DBG COIN3D_LIBRARY=${COIN3D_LIBRARY}")
+ ENDIF(WIN32)
+
+ MARK_AS_ADVANCED(
+ COIN3D_INCLUDE_DIR
+ COIN3D_LIBRARY_RELEASE
+ COIN3D_LIBRARY_DEBUG
+ COIN3D_LIBRARY
+ )
+ ## --------------------------------
+
+ IF(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
+
+ IF(WIN32)
+
+ IF(COIN3D_LIBRARY_RELEASE)
+
+ TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_RELEASE} ${COIN3D_INCLUDE_DIR})
+
+ IF(NOT IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_LIBRARY_RELEASE FALSE)
+ ENDIF(NOT IS_COMPILER_COMPATIBLE)
+ ENDIF(COIN3D_LIBRARY_RELEASE)
+
+
+ # Try to found COIN3D-2
+ IF(NOT COIN3D_LIBRARY_RELEASE)
+ FIND_LIBRARY(COIN3D_LIBRARY_RELEASE
+ NAMES coin2 #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COIN3DDIR}/lib"
+ "$ENV{COIN3D_DIR}/lib"
+ )
+ TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_RELEASE} ${COIN3D_INCLUDE_DIR})
+
+ IF(NOT IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_LIBRARY_RELEASE FALSE)
+ ENDIF(NOT IS_COMPILER_COMPATIBLE)
+ ENDIF(NOT COIN3D_LIBRARY_RELEASE)
+
+ IF(COIN3D_LIBRARY_DEBUG)
+
+ TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_DEBUG} ${COIN3D_INCLUDE_DIR})
+
+ IF(NOT IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_LIBRARY_DEBUG FALSE)
+ ENDIF(NOT IS_COMPILER_COMPATIBLE)
+ ENDIF(COIN3D_LIBRARY_DEBUG)
+
+ # Try to found COIN3D-2
+ IF(NOT COIN3D_LIBRARY_DEBUG)
+ FIND_LIBRARY(COIN3D_LIBRARY_DEBUG
+ NAMES coin2d #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COIN3DDIR}/lib"
+ "$ENV{COIN3D_DIR}/lib"
+ )
+ TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY_DEBUG} ${COIN3D_INCLUDE_DIR})
+
+ IF(NOT IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_LIBRARY_DEBUG FALSE)
+ ENDIF(NOT IS_COMPILER_COMPATIBLE)
+
+ ENDIF(NOT COIN3D_LIBRARY_DEBUG)
+
+ IF(NOT COIN3D_LIBRARY_RELEASE AND NOT COIN3D_LIBRARY_DEBUG)
+ SET(COIN3D_FOUND FALSE)
+ ENDIF(NOT COIN3D_LIBRARY_RELEASE AND NOT COIN3D_LIBRARY_DEBUG)
+
+ ELSE(WIN32)
+
+ TRY_COMPILE_WITH_COIN(${COIN3D_LIBRARY} ${COIN3D_INCLUDE_DIR})
+
+ IF(IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_LIBRARIES ${COIN3D_LIBRARY})
+ SET(COIN3D_FOUND TRUE)
+ MARK_AS_ADVANCED(
+ COIN3D_LIBRARIES
+ COIN3D_LIBRARY
+ )
+ ELSE(IS_COMPILER_COMPATIBLE)
+ SET(COIN3D_FOUND FALSE)
+ ENDIF(IS_COMPILER_COMPATIBLE)
+ ENDIF(WIN32)
+
+ SET(COIN3D_INCLUDE_DIRS ${COIN3D_INCLUDE_DIR})
+ SET(COIN3D_FOUND TRUE)
+ ELSE(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
+ SET(COIN3D_FOUND FALSE)
+ #MESSAGE("Coin library not found.")
+ ENDIF(COIN3D_LIBRARY OR COIN3D_LIBRARY_RELEASE OR COIN3D_LIBRARY_DEBUG AND COIN3D_INCLUDE_DIR)
+
+ #MESSAGE(STATUS "COIN3D_FOUND : ${COIN3D_FOUND}")
+ IF (COIN3D_FOUND)
+ IF (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
+ SET(COIN3D_LIBRARIES optimized ${COIN3D_LIBRARY_RELEASE}
+ debug ${COIN3D_LIBRARY_DEBUG})
+ ELSE (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
+ IF (COIN3D_LIBRARY_DEBUG)
+ SET (COIN3D_LIBRARIES ${COIN3D_LIBRARY_DEBUG})
+ ENDIF (COIN3D_LIBRARY_DEBUG)
+ IF (COIN3D_LIBRARY_RELEASE)
+ SET (COIN3D_LIBRARIES ${COIN3D_LIBRARY_RELEASE})
+ ENDIF (COIN3D_LIBRARY_RELEASE)
+ ENDIF (COIN3D_LIBRARY_DEBUG AND COIN3D_LIBRARY_RELEASE)
+ ENDIF(COIN3D_FOUND)
+
+
+ELSE(UNIX OR WIN32)
+ SET(COIN3D_FOUND FALSE)
+ENDIF(UNIX OR WIN32)
diff --git a/cmake/FindMyGTK2.cmake b/cmake/FindMyGTK2.cmake
new file mode 100644
index 0000000..0750694
--- /dev/null
+++ b/cmake/FindMyGTK2.cmake
@@ -0,0 +1,293 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find GTK (and glib).
+# Once run this will define:
+#
+# GTK2_INCLUDE_DIRS - Directories to include to use GTK
+# GTK2_LIBRARIES - Files to link against to use GTK
+# GTK2_FOUND - If false, don't try to use GTK
+# GTK2_GL_FOUND - If false, don't try to use GTK's GL features
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(UNIX OR WIN32)
+
+ FIND_PATH( GTK2_gtk_INCLUDE_PATH gtk/gtk.h
+ $ENV{GTK2_DIR}/include/gtk-2.0
+ $ENV{GTK2_HOME}/include/gtk-2.0
+ /usr/include/gtk-2.0
+ /usr/local/include/gtk-2.0
+ /opt/gnome/include/gtk-2.0
+ "C:/GTK/include/gtk-2.0"
+ /sw/include/gtk-2.0
+ )
+
+ # Some Linux distributions (e.g. Red Hat) have glibconfig.h
+ # and glib.h in different directories, so we need to look
+ # for both.
+ # - Atanas Georgiev <atanas at cs.columbia.edu>
+
+ FIND_PATH( GTK2_glibconfig_INCLUDE_PATH glibconfig.h
+ $ENV{GTK2_DIR}/lib/glib-2.0/include
+ $ENV{GTK2_HOME}/lib/glib-2.0/include
+ /usr/lib/glib-2.0/include
+ /usr/lib64/glib-2.0/include
+ /usr/lib/i386-linux-gnu/glib-2.0/include
+ /usr/lib/x86_64-linux-gnu/glib-2.0/include
+ /opt/gnome/lib/glib-2.0/include
+ C:/GTK/lib/glib-2.0/include
+ /sw/lib/glib-2.0/include
+ )
+
+ FIND_PATH( GTK2_glib_INCLUDE_PATH glib.h
+ $ENV{GTK2_DIR}/include/glib-2.0
+ $ENV{GTK2_HOME}/include/glib-2.0
+ /usr/include/glib-2.0
+ /opt/gnome/include/glib-2.0
+ C:/GTK/include/glib-2.0
+ /sw/include/glib-2.0
+ )
+
+ FIND_PATH( GTK2_pango_INCLUDE_PATH pango/pango.h
+ $ENV{GTK2_DIR}/include/pango-1.0
+ $ENV{GTK2_HOME}/include/pango-1.0
+ /usr/include/pango-1.0
+ /opt/gnome/include/pango-1.0
+ C:/GTK/include/pango-1.0
+ /sw/include/pango-1.0
+ )
+
+ FIND_PATH( GTK2_cairo_INCLUDE_PATH cairo.h
+ $ENV{GTK2_DIR}/include/cairo
+ $ENV{GTK2_HOME}/include/cairo
+ /usr/include/cairo
+ /opt/gnome/include/cairo
+ C:/GTK/include/cairo
+ /sw/include/cairo
+ )
+
+ FIND_PATH( GTK2_gdkconfig_INCLUDE_PATH gdkconfig.h
+ $ENV{GTK2_DIR}/lib/gtk-2.0/include
+ $ENV{GTK2_HOME}/lib/gtk-2.0/include
+ /usr/lib/gtk-2.0/include
+ /usr/lib64/gtk-2.0/include
+ /opt/gnome/lib/gtk-2.0/include
+ C:/GTK/lib/gtk-2.0/include
+ /sw/lib/gtk-2.0/include
+ /usr/lib/i386-linux-gnu/gtk-2.0/include
+ /usr/lib/x86_64-linux-gnu/gtk-2.0/include
+ )
+
+ FIND_PATH( GTK2_gdkpixbuf_INCLUDE_PATH gdk-pixbuf/gdk-pixbuf.h
+ $ENV{GTK2_DIR}/gdk-pixbuf-2.0
+ $ENV{GTK2_HOME}/gdk-pixbuf-2.0
+ /usr/include/gdk-pixbuf-2.0
+ /usr/include/gtk-2.0
+ C:/GTK/include/gtk-2.0
+ /sw/include/gtk-2.0
+ )
+
+ #MESSAGE("GTK2_gdkpixbuf_INCLUDE_PATH: ${GTK2_gdkpixbuf_INCLUDE_PATH}")
+
+ FIND_PATH( GTK2_atk_INCLUDE_PATH atk/atk.h
+ $ENV{GTK2_DIR}/include/atk-1.0
+ $ENV{GTK2_HOME}/include/atk-1.0
+ /usr/include/atk-1.0
+ /opt/gnome/include/atk-1.0
+ C:/GTK/include/atk-1.0
+ /sw/include/atk-1.0
+ )
+
+ FIND_LIBRARY( GTK2_gtk_LIBRARY
+ NAMES gtk-x11-2.0 gtk-win32-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY( GTK2_gdk_LIBRARY
+ NAMES gdk-x11-2.0 gdk-win32-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY( GTK2_gmodule_LIBRARY
+ NAMES gmodule-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY( GTK2_glib_LIBRARY
+ NAMES glib-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY( GTK2_gthread_LIBRARY
+ NAMES gthread-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY( GTK2_gobject_LIBRARY
+ NAMES gobject-2.0
+ PATHS $ENV{GTK2_DIR}/lib
+ PATHS $ENV{GTK2_HOME}/lib
+ /usr/lib
+ /opt/gnome/lib
+ C:/GTK/lib
+ /sw/lib
+ )
+
+ IF(GTK2_gtk_INCLUDE_PATH)
+ IF(GTK2_glib_INCLUDE_PATH)
+ IF(GTK2_glibconfig_INCLUDE_PATH)
+ IF(GTK2_gtk_LIBRARY)
+ IF(GTK2_glib_LIBRARY)
+ IF(GTK2_pango_INCLUDE_PATH)
+ IF(GTK2_gdkconfig_INCLUDE_PATH)
+ IF(GTK2_atk_INCLUDE_PATH)
+ # Assume that if gtk and glib were found, the other
+ # supporting libraries have also been found.
+
+ SET( GTK2_FOUND "YES" )
+ SET( GTK2_INCLUDE_DIRS ${GTK2_gtk_INCLUDE_PATH}
+ ${GTK2_glib_INCLUDE_PATH}
+ ${GTK2_glibconfig_INCLUDE_PATH}
+ ${GTK2_pango_INCLUDE_PATH}
+ ${GTK2_atk_INCLUDE_PATH}
+ ${GTK2_gdkconfig_INCLUDE_PATH})
+ IF(GTK2_cairo_INCLUDE_PATH)
+ LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_cairo_INCLUDE_PATH} )
+ ENDIF(GTK2_cairo_INCLUDE_PATH)
+ IF(GTK2_gdkpixbuf_INCLUDE_PATH)
+ LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_gdkpixbuf_INCLUDE_PATH} )
+ ENDIF()
+ IF(GTK2_gdkconfig_INCLUDE_PATH)
+ LIST(APPEND GTK2_INCLUDE_DIRS ${GTK2_gdkconfig_INCLUDE_PATH} )
+ ENDIF(GTK2_gdkconfig_INCLUDE_PATH)
+
+ SET( GTK2_LIBRARIES ${GTK2_gtk_LIBRARY}
+ ${GTK2_gdk_LIBRARY}
+ ${GTK2_glib_LIBRARY}
+ ${GTK2_gobject_LIBRARY})
+
+ IF(GTK2_gmodule_LIBRARY)
+ LIST(APPEND GTK2_LIBRARIES ${GTK2_gmodule_LIBRARY})
+ ENDIF(GTK2_gmodule_LIBRARY)
+ IF(GTK2_gthread_LIBRARY)
+ LIST(APPEND GTK2_LIBRARIES ${GTK2_gthread_LIBRARY})
+ ENDIF(GTK2_gthread_LIBRARY)
+
+ ELSE(GTK2_atk_INCLUDE_PATH)
+ MESSAGE("Can not find atk")
+ ENDIF(GTK2_atk_INCLUDE_PATH)
+ ELSE(GTK2_gdkconfig_INCLUDE_PATH)
+ #MESSAGE("Can not find gdkconfig include")
+ ENDIF(GTK2_gdkconfig_INCLUDE_PATH)
+ ELSE(GTK2_pango_INCLUDE_PATH)
+ #MESSAGE("Can not find pango includes")
+ ENDIF(GTK2_pango_INCLUDE_PATH)
+ ELSE(GTK2_glib_LIBRARY)
+ #MESSAGE("Can not find glib lib")
+ ENDIF(GTK2_glib_LIBRARY)
+ ELSE(GTK2_gtk_LIBRARY)
+ #MESSAGE("Can not find gtk lib")
+ ENDIF(GTK2_gtk_LIBRARY)
+ ELSE(GTK2_glibconfig_INCLUDE_PATH)
+ #MESSAGE("Can not find glibconfig includes")
+ ENDIF(GTK2_glibconfig_INCLUDE_PATH)
+ ELSE(GTK2_glib_INCLUDE_PATH)
+ #MESSAGE("Can not find glib includes")
+ ENDIF(GTK2_glib_INCLUDE_PATH)
+ ELSE(GTK2_gtk_INCLUDE_PATH)
+ #MESSAGE("Can not find gtk includes")
+ ENDIF(GTK2_gtk_INCLUDE_PATH)
+
+ MARK_AS_ADVANCED(
+ GTK2_gdk_LIBRARY
+ GTK2_glib_INCLUDE_PATH
+ GTK2_glib_LIBRARY
+ GTK2_glibconfig_INCLUDE_PATH
+ GTK2_gmodule_LIBRARY
+ GTK2_gthread_LIBRARY
+ GTK2_gtk_INCLUDE_PATH
+ GTK2_gtk_LIBRARY
+ GTK2_atk_INCLUDE_PATH
+ GTK2_gdkconfig_INCLUDE_PATH
+ GTK2_gobject_LIBRARY
+ GTK2_pango_INCLUDE_PATH
+ GTK2_cairo_INCLUDE_PATH
+ GTK2_gdkpixbuf_INCLUDE_PATH
+ )
+
+ELSE(UNIX OR WIN32)
+ MESSAGE("FindGTK2 is working on UNIX/LINUX and Windows, only!")
+
+ENDIF(UNIX OR WIN32)
+
diff --git a/cmake/FindMyJPEG.cmake b/cmake/FindMyJPEG.cmake
new file mode 100644
index 0000000..488fe9c
--- /dev/null
+++ b/cmake/FindMyJPEG.cmake
@@ -0,0 +1,127 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libjpeg library.
+# Once run this will define:
+#
+# JPEG_FOUND
+# JPEG_INCLUDE_DIRS
+# JPEG_LIBRARIES
+#
+# Authors:
+# Nicolas Melchior
+#
+#############################################################################
+
+
+# detection of the Libjpeg headers location
+if(MINGW)
+ find_path(JPEG_INCLUDE_DIR
+ NAMES
+ jpeglib.h
+ PATHS
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ )
+ # Detection of the Libjpeg library on Unix
+ find_library(JPEG_LIBRARY
+ NAMES
+ jpeg libjpeg
+ PATHS
+ "$ENV{MINGW_DIR}/lib"
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+else()
+ find_path(JPEG_INCLUDE_DIR
+ NAMES
+ jpeglib.h
+ PATHS
+ "/usr/include"
+ "/usr/local/include"
+ $ENV{LIBJPEG_DIR}/include
+ $ENV{LIBJPEG_DIR}
+ "C:/Program Files/GnuWin32/include"
+ )
+ # Detection of the Libjpeg library on Unix
+ find_library(JPEG_LIBRARY
+ NAMES
+ jpeg libjpeg
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ $ENV{LIBJPEG_DIR}/lib
+ $ENV{LIBJPEG_DIR}/Release
+ $ENV{LIBJPEG_DIR}
+ "C:/Program Files/GnuWin32/lib"
+ )
+endif()
+ #MESSAGE("JPEG_INCLUDE_DIR=${JPEG_INCLUDE_DIR}")
+ #MESSAGE("JPEG_LIBRARY=${JPEG_LIBRARY}")
+
+
+ MARK_AS_ADVANCED(
+ JPEG_LIBRARY
+ JPEG_INCLUDE_DIR
+ )
+
+## --------------------------------
+
+IF(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
+ # The material is found. Check if it works on the requested architecture
+ include(CheckCXXSourceCompiles)
+
+ SET(CMAKE_REQUIRED_LIBRARIES ${JPEG_LIBRARY})
+ SET(CMAKE_REQUIRED_INCLUDES ${JPEG_INCLUDE_DIR})
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <stdio.h>
+ #include <jpeglib.h> // Contrib for jpeg image io
+ #include <jerror.h>
+ int main()
+ {
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+ cinfo.err = jpeg_std_error(&jerr);
+ }
+ " JPEG_BUILD_TEST)
+ #MESSAGE("JPEG_BUILD_TEST: ${JPEG_BUILD_TEST}")
+ IF(JPEG_BUILD_TEST)
+ SET(JPEG_INCLUDE_DIRS ${JPEG_INCLUDE_DIR})
+ SET(JPEG_LIBRARIES ${JPEG_LIBRARY})
+ SET(JPEG_FOUND TRUE)
+ ELSE()
+ SET(JPEG_FOUND FALSE)
+ #MESSAGE("libjpeg library found but not compatible with architecture.")
+ ENDIF()
+
+ELSE(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
+ SET(JPEG_FOUND FALSE)
+ENDIF(JPEG_LIBRARY AND JPEG_INCLUDE_DIR)
diff --git a/cmake/FindMyPNG.cmake b/cmake/FindMyPNG.cmake
new file mode 100644
index 0000000..6e8b1f5
--- /dev/null
+++ b/cmake/FindMyPNG.cmake
@@ -0,0 +1,189 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libpng library.
+# To use the libpng library, the zlib library is required.
+# Once run this will define:
+#
+# PNG_FOUND
+# PNG_INCLUDE_DIR
+# PNG_LIBRARIES
+#
+# Authors:
+# Nicolas Melchior
+#
+#############################################################################
+
+
+# detection of the Libpng headers location
+if(MINGW)
+ find_path(PNG_INCLUDE_DIR
+ NAMES
+ png.h
+ PATHS
+ "C:/mingw/include/libpng14"
+ "$ENV{MINGW_DIR}/include/libpng14"
+ )
+else()
+ find_path(PNG_INCLUDE_DIR
+ NAMES
+ png.h
+ PATHS
+ "$ENV{LIBPNG_DIR}/include"
+ "$ENV{LIBPNG_DIR}"
+ "$ENV{LIBPNG_INCLUDE_DIR}"
+ "/usr/include"
+ "/usr/local/include"
+ "C:/Program Files/libpng/include"
+ )
+endif()
+#MESSAGE("PNG_INCLUDE_DIR=${PNG_INCLUDE_DIR}")
+
+if(UNIX)
+ # Detection of the Libpng library on Unix
+ find_library(PNG_LIBRARY
+ NAMES
+ png15 libpng15 libpng14 png12 libpng12 png libpng
+ PATHS
+ "$ENV{LIBPNG_DIR}/lib"
+ "$ENV{LIBPNG_DIR}/Release"
+ "$ENV{LIBPNG_DIR}"
+ "$ENV{LIBPNG_LIBRARY_DIR}"
+ /usr/lib
+ /usr/local/lib
+ /lib
+ "C:/Program Files/libpng/lib"
+ )
+elseif(MINGW)
+ # Detection of the Libpng library on mingw
+ find_library(PNG_LIBRARY
+ NAMES
+ png15 libpng15 libpng14 png12 libpng12 png libpng
+ PATHS
+ "C:/mingw/lib64"
+ "$ENV{MINGW_DIR}/lib64"
+ )
+ #MESSAGE("PNG_LIBRARY=${PNG_LIBRARY}")
+else()
+ FIND_LIBRARY(PNG_LIBRARY_RELEASE
+ NAMES
+ png15 libpng15 png12 libpng12 png libpng
+ PATHS
+ $ENV{LIBPNG_DIR}/lib
+ $ENV{LIBPNG_DIR}/Release
+ $ENV{LIBPNG_DIR}
+ $ENV{LIBPNG_LIBRARY_RELEASE_DIR}
+ /usr/lib
+ /usr/local/lib
+ /lib
+ "C:/Program Files/libpng/lib"
+ )
+ FIND_LIBRARY(PNG_LIBRARY_DEBUG
+ NAMES
+ png15d libpng15d png12d libpng12d pngd libpngd
+ PATHS
+ $ENV{LIBPNG_DIR}/lib
+ $ENV{LIBPNG_DIR}/Debug
+ $ENV{LIBPNG_DIR}
+ $ENV{LIBPNG_LIBRARY_DEBUG_DIR}
+ /usr/lib
+ /usr/local/lib
+ /lib
+ "C:/Program Files/libpng/lib"
+ )
+ #MESSAGE("PNG_LIBRARY_RELEASE=${PNG_LIBRARY_RELEASE}")
+ #MESSAGE("PNG_LIBRARY_DEBUG=${PNG_LIBRARY_DEBUG}")
+ENDIF(UNIX)
+## --------------------------------
+
+SET(PNG_FOUND FALSE)
+
+FIND_PACKAGE(ZLIB)
+if(NOT ZLIB_FOUND)
+ FIND_PACKAGE(MyZLIB)
+endif()
+
+IF(UNIX)
+ IF(PNG_LIBRARY AND PNG_INCLUDE_DIR)
+ SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
+ SET(PNG_LIBRARIES ${PNG_LIBRARY})
+ SET(PNG_FOUND TRUE)
+ ENDIF()
+ELSE(UNIX)
+ SET(PNG_LIBRARIES "")
+ IF(PNG_LIBRARY_RELEASE AND PNG_INCLUDE_DIR)
+ SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
+ LIST(APPEND PNG_LIBRARIES optimized ${PNG_LIBRARY_RELEASE})
+ SET(PNG_FOUND TRUE)
+ ENDIF()
+ IF(PNG_LIBRARY_DEBUG AND PNG_INCLUDE_DIR)
+ SET(PNG_INCLUDE_DIRS ${PNG_INCLUDE_DIR})
+ LIST(APPEND PNG_LIBRARIES debug ${PNG_LIBRARY_DEBUG})
+ SET(PNG_FOUND TRUE)
+ ENDIF()
+ENDIF(UNIX)
+
+IF(ZLIB_FOUND)
+ IF(PNG_FOUND)
+ # The material is found. Check if it works on the requested architecture
+ include(CheckCXXSourceCompiles)
+
+ #MESSAGE(PNG_LIBRARIES: ${PNG_LIBRARIES})
+ #MESSAGE(ZLIB_LIBRARIES: ${ZLIB_LIBRARIES})
+
+ SET(CMAKE_REQUIRED_LIBRARIES ${PNG_LIBRARIES} ${ZLIB_LIBRARIES})
+ SET(CMAKE_REQUIRED_INCLUDES ${PNG_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS})
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <png.h> // Contrib for png image io
+ int main()
+ {
+ /* create a png read struct */
+ png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ }
+ " PNG_BUILD_TEST)
+ #MESSAGE("PNG_BUILD_TEST: ${PNG_BUILD_TEST}")
+ IF(PNG_BUILD_TEST)
+ SET(PNG_FOUND TRUE)
+ ELSE()
+ SET(PNG_FOUND FALSE)
+ MESSAGE("libpng library found but not compatible with architecture.")
+ ENDIF()
+ ELSEIF(PNG_FOUND)
+ MESSAGE("To use libpng library, you should also install zlib library")
+ ENDIF()
+ENDIF()
+
+
+MARK_AS_ADVANCED(
+ PNG_LIBRARY
+ PNG_LIBRARY_DEBUG
+ PNG_LIBRARY_RELEASE
+ PNG_INCLUDE_DIR
+)
diff --git a/cmake/FindMyZLIB.cmake b/cmake/FindMyZLIB.cmake
new file mode 100644
index 0000000..7c6908e
--- /dev/null
+++ b/cmake/FindMyZLIB.cmake
@@ -0,0 +1,130 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find zlib library.
+# This file should be used only if FindZLIB.cmake provided with CMake
+# is not able to detect zlib.
+# The detection can be eased by setting ZLIB_HOME environment variable
+# especially under windows.
+#
+# ZLIB_FOUND
+# ZLIB_INCLUDE_DIRS
+# ZLIB_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+if(MINGW)
+ find_path(ZLIB_INCLUDE_DIR zlib.h
+ "$ENV{MINGW_DIR}/include"
+ C:/mingw/include
+ )
+else()
+ find_path(ZLIB_INCLUDE_DIR zlib.h
+ $ENV{ZLIB_DIR}/include
+ $ENV{ZLIB_INCLUDE_DIR}
+ /usr/include
+ /usr/local/include
+ "C:/Program Files/zlib/include"
+ )
+endif()
+
+if(UNIX)
+ find_library(ZLIB_LIBRARY z zlib
+ $ENV{ZLIB_DIR}/lib
+ $ENV{ZLIB_LIBRARY_DIR}
+ /lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/zlib/lib"
+ )
+elseif(MINGW)
+ find_library(ZLIB_LIBRARY z zlib
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+else()
+ find_library(ZLIB_LIBRARY_RELEASE z zlib
+ $ENV{ZLIB_DIR}/lib
+ $ENV{ZLIB_LIBRARY_RELEASE_DIR}
+ /lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/zlib/lib"
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+
+ FIND_LIBRARY(ZLIB_LIBRARY_DEBUG zlibd
+ $ENV{ZLIB_DIR}/lib
+ $ENV{ZLIB_LIBRARY_DEBUG_DIR}
+ /lib
+ /usr/lib
+ /usr/local/lib
+ "C:/Program Files/zlib/lib"
+ )
+endif()
+
+if(UNIX OR MINGW)
+ if(ZLIB_LIBRARY AND ZLIB_INCLUDE_DIR)
+ set(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
+ set(ZLIB_LIBRARIES ${ZLIB_LIBRARY})
+ set(ZLIB_FOUND TRUE)
+ else()
+ set(ZLIB_FOUND FALSE)
+ endif()
+else()
+ set(ZLIB_LIBRARIES "")
+ if(ZLIB_LIBRARY_RELEASE AND ZLIB_INCLUDE_DIR)
+ set(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
+ list(APPEND ZLIB_LIBRARIES optimized)
+ list(APPEND ZLIB_LIBRARIES ${ZLIB_LIBRARY_RELEASE})
+ set(ZLIB_FOUND TRUE)
+ endif()
+ if(ZLIB_LIBRARY_DEBUG AND ZLIB_INCLUDE_DIR)
+ set(ZLIB_INCLUDE_DIRS ${ZLIB_INCLUDE_DIR})
+ list(APPEND ZLIB_LIBRARIES debug)
+ list(APPEND ZLIB_LIBRARIES ${ZLIB_LIBRARY_DEBUG})
+ set(ZLIB_FOUND TRUE)
+ endif()
+endif()
+
+mark_as_advanced(
+ ZLIB_INCLUDE_DIR
+ ZLIB_LIBRARY
+ ZLIB_LIBRARY_RELEASE
+ ZLIB_LIBRARY_DEBUG
+ )
+
+
+
+
diff --git a/cmake/FindNAS.cmake b/cmake/FindNAS.cmake
new file mode 100644
index 0000000..a254545
--- /dev/null
+++ b/cmake/FindNAS.cmake
@@ -0,0 +1,60 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to determine if Inria's NAS server hosting /udd/ is available
+#
+# NAS_FOUND
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+FIND_FILE(AFMA6_CONSTANT_FILE_ON_NAS
+ NAMES const_Afma6.cnf
+ PATHS
+ /udd/fspindle/robot/Afma6/current/include
+ Z:/robot/Afma6/current/include
+ )
+
+#MESSAGE("DBG AFMA6_CONSTANT_FILE_ON_NAS=${AFMA6_CONSTANT_FILE_ON_NAS}")
+
+## --------------------------------
+
+IF(AFMA6_CONSTANT_FILE_ON_NAS)
+ SET(NAS_FOUND TRUE)
+ELSE(AFMA6_CONSTANT_FILE_ON_NAS)
+ SET(NAS_FOUND FALSE)
+ENDIF(AFMA6_CONSTANT_FILE_ON_NAS)
+
+MARK_AS_ADVANCED(
+ AFMA6_CONSTANT_FILE_ON_NAS
+)
diff --git a/CMakeModules/FindOpenCV2.cmake b/cmake/FindOpenCV2.cmake
similarity index 100%
rename from CMakeModules/FindOpenCV2.cmake
rename to cmake/FindOpenCV2.cmake
diff --git a/cmake/FindPARPORT.cmake b/cmake/FindPARPORT.cmake
new file mode 100644
index 0000000..80778b3
--- /dev/null
+++ b/cmake/FindPARPORT.cmake
@@ -0,0 +1,56 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find linux/parport.h.h for parallel port usage.
+# Once run this will define:
+#
+# PARPORT_FOUND
+# PARPORT_INCLUDE_DIRS
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+find_path(PARPORT_INCLUDE_DIRS linux/parport.h
+ /usr/include
+ /usr/src/linux/include
+)
+
+mark_as_advanced(
+ PARPORT_INCLUDE_DIRS
+)
+
+if(PARPORT_INCLUDE_DIRS)
+ set(PARPORT_FOUND TRUE)
+else()
+ set(PARPORT_FOUND FALSE)
+endif()
+
diff --git a/cmake/FindPTHREAD.cmake b/cmake/FindPTHREAD.cmake
new file mode 100644
index 0000000..acbeb98
--- /dev/null
+++ b/cmake/FindPTHREAD.cmake
@@ -0,0 +1,101 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find pthread library.
+# Once run this will define:
+#
+# PTHREAD_FOUND
+# PTHREAD_INCLUDE_DIRS
+# PTHREAD_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+if(MINGW)
+ find_path(PTHREAD_INCLUDE_DIR pthread.h
+ "$ENV{MINGW_DIR}/include"
+ "$ENV{MINGW_DIR}/mingw/include"
+ C:/mingw/mingw/include
+ )
+
+ # pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2 are comming from web
+ find_library(PTHREAD_LIBRARY
+ NAMES pthread pthreadGC2 pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2
+ PATHS
+ "$ENV{MINGW_DIR}/lib"
+ "$ENV{MINGW_DIR}/mingw/lib"
+ C:/mingw/mingw/lib
+ )
+else()
+ find_path(PTHREAD_INCLUDE_DIR pthread.h
+ "$ENV{PTHREAD_HOME}/include"
+ "$ENV{PTHREAD_DIR}/include"
+ /usr/include
+ )
+ # pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2 are comming from web
+ find_library(PTHREAD_LIBRARY
+ NAMES pthread pthreadGC2 pthreadVSE pthreadGCE pthreadGC pthreadVC1 pthreadVC2
+ PATHS
+ "$ENV{PTHREAD_HOME}/lib"
+ "$ENV{PTHREAD_DIR}/lib"
+ /usr/lib
+ /usr/local/lib
+ /lib
+ )
+endif()
+ #MESSAGE("DBG PTHREAD_INCLUDE_DIR=${PTHREAD_INCLUDE_DIR}")
+ #MESSAGE(STATUS "DBG PTHREAD_LIBRARY=${PTHREAD_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(PTHREAD_LIBRARY)
+ SET(PTHREAD_LIBRARIES ${PTHREAD_LIBRARY})
+ ELSE(PTHREAD_LIBRARY)
+ #MESSAGE(SEND_ERROR "pthread library not found.")
+ ENDIF(PTHREAD_LIBRARY)
+
+ IF(NOT PTHREAD_INCLUDE_DIR)
+ #MESSAGE(SEND_ERROR "pthread include dir not found.")
+ ENDIF(NOT PTHREAD_INCLUDE_DIR)
+
+ IF(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
+ SET(PTHREAD_INCLUDE_DIRS ${PTHREAD_INCLUDE_DIR})
+ SET(PTHREAD_FOUND TRUE)
+ ELSE(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
+ SET(PTHREAD_FOUND FALSE)
+ ENDIF(PTHREAD_LIBRARIES AND PTHREAD_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ PTHREAD_INCLUDE_DIR
+ PTHREAD_LIBRARY
+ )
+ #MESSAGE(STATUS "PTHREAD_FOUND : ${PTHREAD_FOUND}")
diff --git a/cmake/FindPTU46.cmake b/cmake/FindPTU46.cmake
new file mode 100644
index 0000000..bad98b1
--- /dev/null
+++ b/cmake/FindPTU46.cmake
@@ -0,0 +1,124 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libraries for Irisa's Ptu-46 pan-tilt head.
+# Once run this will define:
+#
+# PTU46_FOUND
+# PTU46_INCLUDE_DIRS
+# PTU46_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindPTU46.cmake: Ptu-46 only available for Unix.")
+ SET(PTU46_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_PATH(PTU46_INCLUDE_DIR ptu.h
+ $ENV{PTU46_HOME}/include
+ /udd/fspindle/robot/Ptu-Evi/current/include
+ /local/soft/Ptu-Evi/current/include
+ )
+ #MESSAGE("DBG PTU46_INCLUDE_DIR=${PTU46_INCLUDE_DIR}")
+
+ FIND_LIBRARY(PTUPTU46_LIBRARY
+ NAMES ptu
+ PATHS
+ $ENV{PTU46_HOME}/lib
+ /udd/fspindle/robot/Ptu-Evi/current/lib
+ /local/soft/Ptu-Evi/current/lib
+ )
+ FIND_LIBRARY(EVIPTU46_LIBRARY
+ NAMES evi
+ PATHS
+ $ENV{PTU46_HOME}/lib
+ /udd/fspindle/robot/Ptu-Evi/current/lib
+ /local/soft/Ptu-Evi/current/lib
+ )
+ FIND_LIBRARY(SERIALPTU46_LIBRARY
+ NAMES serial
+ PATHS
+ $ENV{PTU46_HOME}/lib
+ /udd/fspindle/robot/Ptu-Evi/current/lib
+ /local/soft/Ptu-Evi/current/lib
+ )
+ #MESSAGE("DBG PTU46_LIBRARY=${PTU46_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
+ # The material is found. Check if it works on the requested architecture
+ include(CheckCXXSourceCompiles)
+
+ SET(CMAKE_REQUIRED_LIBRARIES ${PTUPTU46_LIBRARY} ${SERIALPTU46_LIBRARY})
+ SET(CMAKE_REQUIRED_INCLUDES ${PTU46_INCLUDE_DIR})
+ CHECK_CXX_SOURCE_COMPILES("
+ #include <ptu.h> // Contrib for Ptu-46 robot
+ int main()
+ {
+ Ptu ptu;
+ return 0;
+ }
+ " PTU46_BUILD_TEST)
+ #MESSAGE("PTU46_BUILD_TEST: ${PTU46_BUILD_TEST}")
+ IF(PTU46_BUILD_TEST)
+ SET(PTU46_LIBRARIES ${PTUPTU46_LIBRARY} ${EVIPTU46_LIBRARY}
+ ${SERIALPTU46_LIBRARY})
+# ELSE()
+# MESSAGE("Ptu-46 library found but not compatible with architecture.")
+ ENDIF()
+
+ ELSE(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
+# MESSAGE(SEND_ERROR "Ptu-46 library not found.")
+ ENDIF(PTUPTU46_LIBRARY AND EVIPTU46_LIBRARY AND SERIALPTU46_LIBRARY)
+
+ IF(NOT PTU46_INCLUDE_DIR)
+# MESSAGE(SEND_ERROR "Ptu-46 include dir not found.")
+ ENDIF(NOT PTU46_INCLUDE_DIR)
+
+ IF(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
+ SET(PTU46_INCLUDE_DIRS ${PTU46_INCLUDE_DIR})
+ SET(PTU46_FOUND TRUE)
+ ELSE(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
+ SET(PTU46_FOUND FALSE)
+ ENDIF(PTU46_LIBRARIES AND PTU46_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ PTU46_INCLUDE_DIR
+ PTU46_LIBRARIES
+ PTUPTU46_LIBRARY
+ EVIPTU46_LIBRARY
+ SERIALPTU46_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindRAW1394.cmake b/cmake/FindRAW1394.cmake
new file mode 100644
index 0000000..cb8fb61
--- /dev/null
+++ b/cmake/FindRAW1394.cmake
@@ -0,0 +1,85 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find libraw1394 for IEEE1394 camera under Linux.
+# Once run this will define:
+#
+# RAW1394_FOUND
+# RAW1394_INCLUDE_DIRS
+# RAW1394_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindRAW1394.cmake: libraw1394 only available for Unix.")
+ SET(RAW1394_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_PATH(RAW1394_INCLUDE_DIR libraw1394/raw1394.h
+ $ENV{RAW1394_HOME}/include
+ /usr/include )
+ #MESSAGE("DBG RAW1394_INCLUDE_DIR=${RAW1394_INCLUDE_DIR}")
+
+ FIND_LIBRARY(RAW1394_LIBRARY
+ NAMES raw1394
+ PATHS
+ $ENV{RAW1394_HOME}/lib
+ /usr/lib
+ )
+ #MESSAGE("DBG RAW1394_LIBRARY=${RAW1394_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(RAW1394_LIBRARY)
+ SET(RAW1394_LIBRARIES ${RAW1394_LIBRARY})
+ ELSE(RAW1394_LIBRARY)
+ #MESSAGE("libraw1394 library not found.")
+ ENDIF(RAW1394_LIBRARY)
+
+ IF(NOT RAW1394_INCLUDE_DIR)
+ #MESSAGE("libraw1394 include dir not found.")
+ ENDIF(NOT RAW1394_INCLUDE_DIR)
+
+ IF(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
+ SET(RAW1394_INCLUDE_DIRS ${RAW1394_INCLUDE_DIR})
+ SET(RAW1394_FOUND TRUE)
+ ELSE(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
+ SET(RAW1394_FOUND FALSE)
+ ENDIF(RAW1394_LIBRARIES AND RAW1394_INCLUDE_DIR)
+
+ MARK_AS_ADVANCED(
+ RAW1394_INCLUDE_DIR
+ RAW1394_LIBRARIES
+ RAW1394_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindRT.cmake b/cmake/FindRT.cmake
new file mode 100644
index 0000000..0ef73aa
--- /dev/null
+++ b/cmake/FindRT.cmake
@@ -0,0 +1,70 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find RT library material
+#
+# RT_FOUND
+# RT_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindRT.cmake: only available for Unix.")
+ SET(RT_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_LIBRARY(RT_LIBRARY
+ NAMES rt
+ PATHS
+ $ENV{RT_HOME}/lib
+ /usr/lib
+ /usr/local/lib
+ )
+
+ #MESSAGE("DBG RT_LIBRARY=${RT_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(RT_LIBRARY)
+ SET(RT_LIBRARIES ${RT_LIBRARY})
+ SET(RT_FOUND TRUE)
+ ELSE(RT_LIBRARY)
+ SET(RT_FOUND FALSE)
+ #MESSAGE(SEND_ERROR "rt library not found.")
+ ENDIF(RT_LIBRARY)
+
+ MARK_AS_ADVANCED(
+ RT_LIBRARIES
+ RT_LIBRARY
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindRound.cmake b/cmake/FindRound.cmake
new file mode 100644
index 0000000..b96556d
--- /dev/null
+++ b/cmake/FindRound.cmake
@@ -0,0 +1,65 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find round function.
+#
+# Authors:
+# Souriya Trinh
+#
+#############################################################################
+
+include(CheckIncludeFile)
+include(CheckIncludeFiles)
+include(CheckCXXSourceCompiles)
+
+macro(check_math_expr1 _expr _var)
+ check_cxx_source_compiles("
+#include <math.h>
+int main(int argc, char ** argv)
+{
+ (void)${_expr};
+ return 0;
+}
+" ${_var})
+endmacro()
+
+macro(check_math_expr2 _expr _var)
+ check_cxx_source_compiles("
+#include <cmath>
+int main(int argc, char ** argv)
+{
+ (void)${_expr};
+ return 0;
+}
+" ${_var})
+endmacro()
+
+check_math_expr1("round(5.5)" HAVE_FUNC_ROUND)
+check_math_expr2("std::round(5.5)" HAVE_FUNC_STD_ROUND)
diff --git a/cmake/FindSOQT.cmake b/cmake/FindSOQT.cmake
new file mode 100644
index 0000000..4dfb2e4
--- /dev/null
+++ b/cmake/FindSOQT.cmake
@@ -0,0 +1,126 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find SoQt library.
+# Once run this will define:
+#
+# SOQT_FOUND
+# SOQT_INCLUDE_DIRS
+# SOQT_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+set(SOQT_FOUND FALSE)
+
+# detection of the SoQt headers location
+FIND_PATH(SOQT_INCLUDE_DIR Inventor/Qt/SoQt.h
+ "$ENV{COIN_DIR}/include"
+ "$ENV{SOQT_DIR}/include"
+ "$ENV{COIN_DIR}"
+ "$ENV{SOQT_DIR}"
+ "$ENV{COINDIR}/include"
+ "$ENV{SOQTDIR}/include"
+ /usr/include/Coin2
+ /Library/Frameworks/SoQt.framework/Headers
+ )
+#MESSAGE("DBG SOQT_INCLUDE_DIR=${SOQT_INCLUDE_DIR}")
+
+IF(WIN32)
+ # Detection of the SoQt library on Windows
+ FIND_LIBRARY(SOQT_LIBRARY_RELEASE
+ NAMES soqt1 #only shared libraries under windows
+ PATHS
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{SOQT_DIR}/lib"
+ "$ENV{COINDIR}/lib"
+ "$ENV{SOQTDIR}/lib"
+ )
+ FIND_LIBRARY(SOQT_LIBRARY_DEBUG
+ NAMES soqt1d #only shared libraries under windows
+ PATHS
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{SOQT_DIR}/lib"
+ "$ENV{COINDIR}/lib"
+ "$ENV{SOQTDIR}/lib"
+ )
+
+ if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY_RELEASE)
+ set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
+ list(APPEND SOQT_LIBRARIES optimized ${SOQT_LIBRARY_RELEASE})
+ set(SOQT_FOUND TRUE)
+ endif()
+
+ if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY_DEBUG)
+ set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
+ list(APPEND SOQT_LIBRARIES debug ${SOQT_LIBRARY_DEBUG})
+ set(SOQT_FOUND TRUE)
+ endif()
+
+ MARK_AS_ADVANCED(
+ SOQT_LIBRARIES
+ SOQT_LIBRARY_DEBUG
+ SOQT_LIBRARY_RELEASE
+ SOQT_INCLUDE_DIR
+ )
+
+ELSE(WIN32)
+ # Detection of the SoQt library on Unix
+ FIND_LIBRARY(SOQT_LIBRARY
+ NAMES SoQt
+ PATHS
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{SOQT_DIR}/lib"
+ "$ENV{COINDIR}/lib"
+ "$ENV{SOQTDIR}/lib"
+ /usr/lib
+ /usr/local/lib
+ /lib
+ /Library/Frameworks/SoQt.framework/Libraries
+ )
+
+ if(SOQT_INCLUDE_DIR AND SOQT_LIBRARY)
+ set(SOQT_INCLUDE_DIRS ${SOQT_INCLUDE_DIR})
+ set(SOQT_LIBRARIES ${SOQT_LIBRARY})
+ set(SOQT_FOUND TRUE)
+ endif()
+
+ MARK_AS_ADVANCED(
+ SOQT_LIBRARIES
+ SOQT_LIBRARY
+ SOQT_INCLUDE_DIR
+ )
+ #MESSAGE(STATUS "DBG SOQT_LIBRARY=${SOQT_LIBRARY}")
+
+ENDIF(WIN32)
+
+#MESSAGE(STATUS "SOQT_FOUND : ${SOQT_FOUND}")
diff --git a/cmake/FindSOWIN.cmake b/cmake/FindSOWIN.cmake
new file mode 100644
index 0000000..667e87b
--- /dev/null
+++ b/cmake/FindSOWIN.cmake
@@ -0,0 +1,92 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find SoQt library.
+# Once run this will define:
+#
+# SOWIN_FOUND
+# SOWIN_INCLUDE_DIRS
+# SOWIN_LIBRARIES
+# SOWIN_LIBRARY_RELEASE
+# SOWIN_LIBRARY_DEBUG
+#
+# Authors:
+# Fabien Spindler
+# Anthony Saunier
+#
+#############################################################################
+
+set(SOWIN_FOUND FALSE)
+
+if(WIN32)
+ FIND_LIBRARY(SOWIN_LIBRARY_RELEASE
+ NAMES sowin1 #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{SOWIN_DIR}/lib"
+ )
+ FIND_LIBRARY(SOWIN_LIBRARY_DEBUG
+ NAMES sowin1d #only shared libraries under windows
+ PATHS
+ "$ENV{COINDIR}/lib"
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{SOWIN_DIR}/lib"
+ )
+
+ FIND_PATH(SOWIN_INCLUDE_DIR Inventor/Win/SoWin.h
+ "$ENV{COINDIR}/include"
+ "$ENV{COIN_DIR}/include"
+ "$ENV{SOWIN_DIR}/include"
+ )
+
+ ## --------------------------------
+ set(SOWIN_LIBRARIES "")
+
+ if(SOWIN_LIBRARY_DEBUG)
+ list(APPEND SOWIN_LIBRARIES debug ${SOWIN_LIBRARY_DEBUG})
+ endif()
+ if(SOWIN_LIBRARY_RELEASE)
+ list(APPEND SOWIN_LIBRARIES optimized ${SOWIN_LIBRARY_RELEASE})
+ endif()
+ IF(SOWIN_LIBRARIES AND SOWIN_INCLUDE_DIR)
+ set(SOWIN_INCLUDE_DIRS ${SOWIN_INCLUDE_DIR})
+ set(SOWIN_FOUND TRUE)
+ else()
+ set(SOWIN_FOUND FALSE)
+ endif()
+
+endif(WIN32)
+
+MARK_AS_ADVANCED(
+ SOWIN_LIBRARY_DEBUG
+ SOWIN_LIBRARY_RELEASE
+ SOWIN_INCLUDE_DIR
+)
diff --git a/cmake/FindSOXT.cmake b/cmake/FindSOXT.cmake
new file mode 100644
index 0000000..1a5de4a
--- /dev/null
+++ b/cmake/FindSOXT.cmake
@@ -0,0 +1,76 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find SoXt library.
+# Once run this will define:
+#
+# SOXT_FOUND
+# SOXT_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(UNIX)
+
+FIND_LIBRARY(SOXT_LIBRARY
+ NAMES SoXt
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /lib
+ "$ENV{COIN_DIR}/lib"
+ "$ENV{COINDIR}/lib"
+ )
+MARK_AS_ADVANCED(
+ SOXT_LIBRARY
+ )
+ #MESSAGE(STATUS "DBG SOXT_LIBRARY=${SOXT_LIBRARY}")
+
+ ## --------------------------------
+
+ IF(SOXT_LIBRARY)
+
+ SET(SOXT_LIBRARIES ${SOXT_LIBRARY})
+ SET(SOXT_FOUND TRUE)
+ MARK_AS_ADVANCED(
+ SOXT_LIBRARIES
+ )
+ ELSE(SOXT_LIBRARY)
+ SET(SOXT_FOUND FALSE)
+ #MESSAGE("SoXt library not found.")
+ ENDIF(SOXT_LIBRARY)
+
+ #MESSAGE(STATUS "SOXT_FOUND : ${SOXT_FOUND}")
+
+ELSE(UNIX)
+ SET(SOXT_FOUND FALSE)
+ENDIF(UNIX)
diff --git a/cmake/FindV4L2.cmake b/cmake/FindV4L2.cmake
new file mode 100644
index 0000000..d2e92ae
--- /dev/null
+++ b/cmake/FindV4L2.cmake
@@ -0,0 +1,103 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find linux/videodev.h for Video For Linux Two framegrabbing
+# capabilities.
+# Once run this will define:
+#
+# V4L2_FOUND
+# V4L2_INCLUDE_DIRS
+# V4L2_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT UNIX)
+ # MESSAGE("FindV4L2.cmake: only available for Unix.")
+ SET(V4L2_FOUND FALSE)
+ELSE(NOT UNIX)
+
+ FIND_PATH(V4L2_INCLUDE_VIDEODEV2 linux/videodev2.h
+ $ENV{V4L2_HOME}/include
+ $ENV{V4L2_DIR}/include
+ /usr/include
+ /usr/local/include
+ /usr/src/linux/include
+ )
+ #MESSAGE("DBG V4L2_INCLUDE_VIDEODEV2=${V4L2_INCLUDE_VIDEODEV2}")
+
+ FIND_PATH(V4L2_INCLUDE_LIBV4L2 libv4l2.h
+ $ENV{V4L2_HOME}/include
+ $ENV{V4L2_DIR}/include
+ /usr/include
+ /usr/local/include
+ )
+ #MESSAGE("DBG V4L2_INCLUDE_LIBV4L2=${V4L2_INCLUDE_LIBV4L2}")
+
+ FIND_LIBRARY(V4L2_LIBRARY_LIBV4L2
+ NAMES v4l2
+ PATHS
+ $ENV{V4L2_HOME}/lib
+ $ENV{V4L2_DIR}/lib
+ /usr/lib
+ /usr/local/lib
+ )
+
+ FIND_LIBRARY(V4L2_LIBRARY_LIBV4LCONVERT
+ NAMES v4lconvert
+ PATHS
+ $ENV{V4L2_HOME}/lib
+ $ENV{V4L2_DIR}/lib
+ /usr/lib
+ /usr/local/lib
+ )
+
+
+ ## --------------------------------
+
+
+ IF(V4L2_INCLUDE_VIDEODEV2 AND V4L2_INCLUDE_LIBV4L2 AND V4L2_LIBRARY_LIBV4L2 AND V4L2_LIBRARY_LIBV4LCONVERT)
+ SET(V4L2_INCLUDE_DIRS ${V4L2_INCLUDE_VIDEODEV2} ${V4L2_INCLUDE_LIBV4L2})
+ SET(V4L2_LIBRARIES ${V4L2_LIBRARY_LIBV4L2} ${V4L2_LIBRARY_LIBV4LCONVERT})
+ SET(V4L2_FOUND TRUE)
+ ELSE()
+ SET(V4L2_FOUND FALSE)
+ ENDIF()
+
+ MARK_AS_ADVANCED(
+ V4L2_INCLUDE_DIRS
+ V4L2_INCLUDE_VIDEODEV2
+ V4L2_INCLUDE_LIBV4L2
+ V4L2_LIBRARY_LIBV4L2
+ V4L2_LIBRARY_LIBV4LCONVERT
+ )
+ENDIF(NOT UNIX)
diff --git a/cmake/FindXML2.cmake b/cmake/FindXML2.cmake
new file mode 100644
index 0000000..ba6ca5b
--- /dev/null
+++ b/cmake/FindXML2.cmake
@@ -0,0 +1,122 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find XML library.
+# Once run this will define:
+#
+# XML2_FOUND
+# XML2_INCLUDE_DIRS
+# XML2_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+if(WIN32)
+ find_package(ICONV)
+ if(MINGW)
+ find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
+ "$ENV{MINGW_DIR}/include/libxml2"
+ C:/mingw/include/libxml2
+ )
+ find_library(XML2_LIBRARY libxml2
+ "$ENV{MINGW_DIR}/lib"
+ "$ENV{MINGW_DIR}/lib64"
+ C:/mingw/lib64
+ )
+ else()
+ find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
+ "$ENV{XML2_DIR}/include"
+ "$ENV{XML2_DIR}/include/libxml2"
+ "$ENV{XML2_HOME}/include"
+ "$ENV{XML2_HOME}/include/libxml2"
+ )
+ find_library(XML2_LIBRARY libxml2
+ "$ENV{XML2_DIR}/lib"
+ "$ENV{XML2_HOME}/lib"
+ /usr/lib
+ /usr/local/lib
+ "c:/libxml2/lib"
+ )
+ endif()
+else(WIN32)
+ find_path(XML2_INCLUDE_DIR libxml/xmlmemory.h
+ "$ENV{XML2_DIR}/include/libxml2"
+ "$ENV{XML2_HOME}/include/libxml2"
+ /usr/include/libxml2
+ /usr/local/include/libxml2
+ /usr/local/opt/libxml2/include/libxml2
+ )
+ find_library(XML2_LIBRARY xml2
+ "$ENV{XML2_DIR}/lib"
+ "$ENV{XML2_HOME}/lib"
+ /usr/lib
+ /usr/local/lib
+ /usr/local/opt/libxml2/lib
+ )
+endif(WIN32)
+#MESSAGE("DBG XML2_INCLUDE_DIR=${XML2_INCLUDE_DIR}")
+
+
+#MESSAGE("DBG XML2_LIBRARY=${XML2_LIBRARY}")
+
+## --------------------------------
+
+IF(NOT XML2_INCLUDE_DIR)
+ #MESSAGE(SEND_ERROR "xml include dir not found.")
+ENDIF(NOT XML2_INCLUDE_DIR)
+
+IF(XML2_LIBRARY)
+ SET(XML2_LIBRARIES ${XML2_LIBRARY})
+ELSE(XML2_LIBRARY)
+ #MESSAGE(SEND_ERROR "xml library not found.")
+ENDIF(XML2_LIBRARY)
+
+
+IF(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
+ SET(XML2_INCLUDE_DIRS ${XML2_INCLUDE_DIR})
+ SET(XML2_FOUND TRUE)
+
+ IF(WIN32 AND ICONV_FOUND)
+ LIST(APPEND XML2_INCLUDE_DIRS ${ICONV_INCLUDE_DIRS})
+ SET(XML2_LIBRARIES ${XML2_LIBRARIES} ${ICONV_LIBRARIES})
+ ENDIF()
+
+ELSE(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
+ SET(XML2_FOUND FALSE)
+ENDIF(XML2_LIBRARIES AND XML2_INCLUDE_DIR)
+
+MARK_AS_ADVANCED(
+ XML2_INCLUDE_DIR
+ XML2_LIBRARIES
+ XML2_LIBRARY
+ )
diff --git a/cmake/FindZBAR.cmake b/cmake/FindZBAR.cmake
new file mode 100644
index 0000000..241779a
--- /dev/null
+++ b/cmake/FindZBAR.cmake
@@ -0,0 +1,68 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Try to find zbar library.
+# Once run this will define:
+#
+# ZBAR_FOUND
+# ZBAR_INCLUDE_DIRS
+# ZBAR_LIBRARIES
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+find_path(ZBAR_INCLUDE_DIRS zbar.h
+ $ENV{ZBAR_DIR}/include
+ /usr/include
+ /usr/local/include
+)
+
+find_library(ZBAR_LIBRARIES
+ NAMES zbar
+ PATHS
+ $ENV{ZBAR_DIR}/lib
+ /usr/lib
+ /usr/local/lib
+)
+
+if(ZBAR_INCLUDE_DIRS AND ZBAR_LIBRARIES)
+ set(ZBAR_FOUND TRUE)
+else()
+ set(ZBAR_FOUND FALSE)
+endif()
+
+mark_as_advanced(
+ ZBAR_INCLUDE_DIRS
+ ZBAR_LIBRARIES
+)
+
diff --git a/CMakeModules/GNUInstallDirs.cmake b/cmake/GNUInstallDirs.cmake
similarity index 100%
rename from CMakeModules/GNUInstallDirs.cmake
rename to cmake/GNUInstallDirs.cmake
diff --git a/cmake/OgreTools.cmake b/cmake/OgreTools.cmake
new file mode 100644
index 0000000..81951be
--- /dev/null
+++ b/cmake/OgreTools.cmake
@@ -0,0 +1,371 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP overall configuration file. Some useful tools for Ogre3D.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+#########################################################
+# Find Ogre plugins
+#
+# This is a modified version of the macro provided with Ogre
+# except that it should be used only in a desperate way when the original
+# one doesn't detect anything
+#########################################################
+
+
+macro(vp_ogre_find_plugin_lib_visp PLUGIN)
+ # On Unix, the plugins might have no prefix
+ if (CMAKE_FIND_LIBRARY_PREFIXES)
+ set(TMP_CMAKE_LIB_PREFIX ${CMAKE_FIND_LIBRARY_PREFIXES})
+ set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} "")
+ endif()
+
+ # strip RenderSystem_ or Plugin_ prefix from plugin name
+ string(REPLACE "RenderSystem_" "" PLUGIN_TEMP ${PLUGIN})
+ string(REPLACE "Plugin_" "" PLUGIN_NAME ${PLUGIN_TEMP})
+
+ set(OGRE_PLUGIN_PATH_SUFFIXES
+ PlugIns PlugIns/${PLUGIN_NAME} Plugins Plugins/${PLUGIN_NAME} ${PLUGIN}
+ RenderSystems RenderSystems/${PLUGIN_NAME} ${ARGN})
+ # find link libraries for plugins
+ set(OGRE_${PLUGIN}_LIBRARY_NAMES "${PLUGIN}${OGRE_LIB_SUFFIX}")
+ get_debug_names(OGRE_${PLUGIN}_LIBRARY_NAMES)
+ find_library(OGRE_${PLUGIN}_LIBRARY_REL NAMES ${OGRE_${PLUGIN}_LIBRARY_NAMES}
+ HINTS ${OGRE_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}/OGRE ${OGRE_LIBRARY_DIRS}/OGRE-${OGRE_VERSION_MAJOR}.${OGRE_VERSION_MINOR}.${OGRE_VERSION_PATCH}
+ PATH_SUFFIXES "" OGRE opt release release/opt relwithdebinfo relwithdebinfo/opt minsizerel minsizerel/opt)
+ find_library(OGRE_${PLUGIN}_LIBRARY_DBG NAMES ${OGRE_${PLUGIN}_LIBRARY_NAMES_DBG}
+ HINTS ${OGRE_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}/OGRE ${OGRE_LIBRARY_DIRS}/OGRE-${OGRE_VERSION_MAJOR}.${OGRE_VERSION_MINOR}.${OGRE_VERSION_PATCH}
+ PATH_SUFFIXES "" OGRE opt debug debug/opt)
+ make_library_set(OGRE_${PLUGIN}_LIBRARY)
+
+ if (OGRE_${PLUGIN}_LIBRARY)
+ set(OGRE_${PLUGIN}_FOUND TRUE)
+ endif ()
+
+ mark_as_advanced(OGRE_${PLUGIN}_LIBRARY_REL OGRE_${PLUGIN}_LIBRARY_DBG OGRE_${PLUGIN}_LIBRARY_FWK)
+endmacro()
+
+macro(vp_create_ogre_plugin_config_file)
+ set(VISP_HAVE_OGRE_PLUGINS_PATH "${VISP_BINARY_DIR}/data/ogre-simulator" CACHE INTERNAL "Ogre plugins location")
+
+ # If OGRE_PLUGIN_DIR_REL and OGRE_PLUGIN_DIR_DBG are not defined we
+ # try to find them manually
+ if(NOT OGRE_PLUGIN_DIR_REL AND NOT OGRE_PLUGIN_DIR_DBG)
+ vp_ogre_find_plugin_lib_visp(RenderSystem_Direct3D9)
+ vp_ogre_find_plugin_lib_visp(RenderSystem_Direct3D10)
+ vp_ogre_find_plugin_lib_visp(RenderSystem_Direct3D11)
+ vp_ogre_find_plugin_lib_visp(RenderSystem_GL)
+ vp_ogre_find_plugin_lib_visp(RenderSystem_GLES)
+ vp_ogre_find_plugin_lib_visp(Plugin_ParticleFX)
+ vp_ogre_find_plugin_lib_visp(Plugin_BSPSceneManager)
+ vp_ogre_find_plugin_lib_visp(Plugin_CgProgramManager)
+ vp_ogre_find_plugin_lib_visp(Plugin_PCZSceneManager)
+ vp_ogre_find_plugin_lib_visp(Plugin_OctreeSceneManager)
+ vp_ogre_find_plugin_lib_visp(Plugin_OctreeZone)
+
+ if(OGRE_RenderSystem_GL_LIBRARY_REL)
+ get_filename_component(OGRE_PLUGIN_DIR_REL ${OGRE_RenderSystem_GL_LIBRARY_REL} PATH)
+ #message("set manually OGRE_PLUGIN_DIR_REL to ${OGRE_PLUGIN_DIR_REL}")
+ elseif(OGRE_RenderSystem_GL_LIBRARY_DBG)
+ get_filename_component(OGRE_PLUGIN_DIR_DBG ${OGRE_RenderSystem_GL_LIBRARY_DBG} PATH)
+ #message("set manually OGRE_PLUGIN_DIR_DBG to ${OGRE_PLUGIN_DIR_DBG}")
+ endif()
+ endif()
+
+ if(OGRE_PLUGIN_DIR_REL)
+ list(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D9_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D10_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_RenderSystem_Direct3D11_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_RenderSystem_GL_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_RenderSystem_GLES_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_ParticleFX_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_BSPSceneManager_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_CgProgramManager_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_PCZSceneManager_LIBRARY_REL})
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_OctreeSceneManager_LIBRARY_REL})
+ if(NOT APPLE)
+ # Since the plugin Plugin_Octree causes problems on OSX, we take
+ # it only into account on non Apple platforms
+ list(APPEND PLUGIN_REL ${OGRE_Plugin_OctreeZone_LIBRARY_REL})
+ endif()
+
+ set(PLUGINS_CONTENT_REL "# Defines plugins to load\n\n")
+ list(APPEND PLUGINS_CONTENT_REL "# Define plugin folder\n")
+
+ list(APPEND PLUGINS_CONTENT_REL "PluginFolder=${OGRE_PLUGIN_DIR_REL}/\n\n")
+ list(APPEND PLUGINS_CONTENT_REL "# Define plugins\n")
+ foreach(PLUGIN ${PLUGIN_REL})
+ if(PLUGIN)
+ get_filename_component(PLUGIN_NAME ${PLUGIN} NAME_WE)
+ list(APPEND PLUGINS_CONTENT_REL " Plugin=${PLUGIN_NAME}\n")
+ endif()
+ endforeach()
+ #MESSAGE("PLUGINS_CONTENT_REL: ${PLUGINS_CONTENT_REL}")
+ file(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg" ${PLUGINS_CONTENT_REL})
+ endif()
+
+ if(OGRE_PLUGIN_DIR_DBG)
+ list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D9_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D10_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D11_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_GL_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_GLES_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_ParticleFX_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_BSPSceneManager_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_CgProgramManager_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_PCZSceneManager_LIBRARY_DBG})
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_OctreeSceneManager_LIBRARY_DBG})
+ if(NOT APPLE)
+ # Since the plugin Plugin_Octree causes problems on OSX, we take
+ # it only into account on non Apple platforms
+ list(APPEND PLUGIN_DBG ${OGRE_Plugin_OctreeZone_LIBRARY_DBG})
+ endif()
+
+ set(PLUGINS_CONTENT_DBG "# Defines plugins to load\n\n")
+ list(APPEND PLUGINS_CONTENT_DBG "# Define plugin folder\n")
+ list(APPEND PLUGINS_CONTENT_DBG "PluginFolder=${OGRE_PLUGIN_DIR_DBG}/\n\n")
+ list(APPEND PLUGINS_CONTENT_DBG "# Define plugins\n")
+ foreach(PLUGIN ${PLUGIN_DBG})
+ if(PLUGIN)
+ get_filename_component(PLUGIN_NAME ${PLUGIN} NAME_WE)
+ list(APPEND PLUGINS_CONTENT_DBG " Plugin=${PLUGIN_NAME}\n")
+ endif()
+ endforeach()
+
+ #MESSAGE("PLUGINS_CONTENT_DBG: ${PLUGINS_CONTENT_DBG}")
+ file(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg" ${PLUGINS_CONTENT_DBG})
+ endif()
+endmacro()
+
+function(vp_set_ogre_media)
+ # if OGRE_MEDIA_DIR is empty, try to find the path by searching for plugins.cfg
+ # Introduced since Ubuntu 12.04
+ if(NOT OGRE_MEDIA_DIR)
+ find_path(OGRE_MEDIA_DIR ../plugins.cfg
+ $ENV{OGRE_MEDIA_DIR}
+ /usr/share/OGRE-1.7.4/media
+ /usr/share/OGRE-1.8.0/media
+ /usr/share/OGRE-1.8.1/media
+ /usr/share/OGRE-1.9.0/media
+ )
+ endif()
+
+ # If Ogre media are not available we provide the minimal material to run the examples:
+ # - resources.cfg
+ # - plugins.cfg
+ # - media/materials/...
+ # - media/models/...
+ #
+ # We need to introduce OGRE_MEDIA_NOT_AVAILABLE to memorize when OGRE_MEDIA_DIR is not set.
+ # Because in that case, OGRE_MEDIA_DIR should be set first to VISP_HAVE_OGRE_RESOURCES_PATH
+ # (for the "make all" case) then to VISP_INSTALL_DIR_OGRE_RESOURCES (for the "make install" case)
+ if(NOT OGRE_MEDIA_DIR)
+ set(OGRE_MEDIA_NOT_AVAILABLE "TRUE")
+ endif()
+
+ # Try to search for an existing plugins.cfg file
+ # Here we cannot use OGRE_PLUGIN_DIR_REL or OGRE_PLUGIN_DIR_DBG where
+ # we may find an existing plugins.cfg file, since under Windows in these
+ # files the PluginFolder is set to a relative path. We need an absolute
+ # path to avoid recopy of the plugins in ViSP.
+ # Under Linux or OSX, we may find plugins.cfg with a PluginFolder set
+ # to an absolute path in OGRE_MEDIA_DIR/..
+ find_path(ogre_plugings_cfg_exists
+ NAMES plugins.cfg
+ PATHS ${OGRE_MEDIA_DIR}/..
+ NO_SYSTEM_ENVIRONMENT_PATH
+ )
+ mark_as_advanced(ogre_plugings_cfg_exists)
+
+ #message("OGRE_PLUGIN_DIR_REL: ${OGRE_PLUGIN_DIR_REL}")
+ #message("OGRE_PLUGIN_DIR_DBG: ${OGRE_PLUGIN_DIR_DBG}")
+ if(ogre_plugings_cfg_exists)
+ set(VISP_HAVE_OGRE_PLUGINS_PATH "${ogre_plugings_cfg_exists}" CACHE INTERNAL "Ogre plugins location")
+ else(NOT ogre_plugings_cfg_exists)
+ # If no plugins.cfg file is found, we create one with absolute path
+
+ # case 1: normal case
+ #--------------
+ vp_create_ogre_plugin_config_file()
+
+ # case 2: install or packaging case
+ #--------------
+ # install rule for plugins.cfg:
+ if(UNIX)
+ if(OGRE_PLUGIN_DIR_REL)
+ install(FILES
+ ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+ if(OGRE_PLUGIN_DIR_DBG)
+ install(FILES
+ ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/visp/data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+
+ else()
+ if(OGRE_PLUGIN_DIR_REL)
+ install(FILES
+ ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg
+ DESTINATION data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+ if(OGRE_PLUGIN_DIR_DBG)
+ install(FILES
+ ${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg
+ DESTINATION data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+ endif()
+ endif()
+
+ # Try to search for an existing resources.cfg file
+ find_path(ogre_resources_cfg_exists
+ NAMES resources.cfg
+ PATHS ${OGRE_MEDIA_DIR}/..
+ NO_SYSTEM_ENVIRONMENT_PATH
+ )
+ mark_as_advanced(ogre_resources_cfg_exists)
+ # Here we copy all the minimal media files
+ # - media/materials/...
+ # - media/models/...
+ if(OGRE_MEDIA_NOT_AVAILABLE)
+ file(COPY modules/ar/data/ogre-simulator/media DESTINATION ${VISP_BINARY_DIR}/data/ogre-simulator)
+ endif()
+
+ if(ogre_resources_cfg_exists)
+ set(VISP_HAVE_OGRE_RESOURCES_PATH "${ogre_resources_cfg_exists}" CACHE INTERNAL "Ogre resources location")
+ else()
+ # Here we create a resources.cfg if it was not found
+
+ # we create a resources.cfg file for vpAROgre.cpp
+ # case 1: normal case
+ # If OGRE_MEDIA_DIR is not found, we set it to VISP_HAVE_OGRE_RESOURCES_PATH in order to use
+ # the minimal requested media to run the examples
+ #--------------
+ set(VISP_HAVE_OGRE_RESOURCES_PATH "${VISP_BINARY_DIR}/data/ogre-simulator" CACHE INTERNAL "Ogre resources location")
+
+ if(OGRE_MEDIA_NOT_AVAILABLE)
+ set(OGRE_MEDIA_DIR ${VISP_HAVE_OGRE_RESOURCES_PATH}/media)
+ endif()
+
+ # Here we add all the subdirs in @OGRE_MEDIA_DIR@/* as resource location.
+ vp_get_relative_subdirs(media_subdirs ${OGRE_MEDIA_DIR})
+ set(OGRE_RESOURCES_FileSystem "FileSystem=${OGRE_MEDIA_DIR}\n")
+ foreach(m ${media_subdirs})
+ set(OGRE_RESOURCES_FileSystem "${OGRE_RESOURCES_FileSystem}FileSystem=${OGRE_MEDIA_DIR}/${m}\n")
+ endforeach()
+
+ configure_file(
+ ${VISP_SOURCE_DIR}/cmake/templates/resources.cfg.in
+ ${VISP_HAVE_OGRE_RESOURCES_PATH}/resources.cfg
+ IMMEDIATE @ONLY
+ )
+
+ # case 2: install or packaging case
+ # If OGRE_MEDIA_DIR is not found, we set it to VISP_INSTALL_DIR_OGRE_RESOURCES in order to use
+ # the minimal requested media to run the examples
+ #--------------
+ if(UNIX)
+ set(VISP_INSTALL_DIR_OGRE_RESOURCES "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator")
+ else()
+ set(VISP_INSTALL_DIR_OGRE_RESOURCES "${CMAKE_INSTALL_PREFIX}/data/ogre-simulator")
+ endif()
+ # make the var global
+ set(VISP_INSTALL_DIR_OGRE_RESOURCES ${VISP_INSTALL_DIR_OGRE_RESOURCES} CACHE INTERNAL "Ogre media install dir")
+
+ if(OGRE_MEDIA_NOT_AVAILABLE)
+ set(OGRE_MEDIA_DIR ${VISP_INSTALL_DIR_OGRE_RESOURCES}/media)
+ endif()
+
+ # Here we add all the subdirs in @OGRE_MEDIA_DIR@/* as resource location.
+ set(OGRE_RESOURCES_FileSystem "FileSystem=${OGRE_MEDIA_DIR}\n")
+ foreach(m ${media_subdirs})
+ set(OGRE_RESOURCES_FileSystem "${OGRE_RESOURCES_FileSystem}FileSystem=${OGRE_MEDIA_DIR}/${m}\n")
+ endforeach()
+
+ # install rule for resources.cfg and Ogre media if they are not available:
+ if(UNIX)
+ configure_file(
+ ${VISP_SOURCE_DIR}/cmake/templates/resources.cfg.in
+ ${VISP_BINARY_DIR}/unix-install/resources.cfg
+ IMMEDIATE @ONLY
+ )
+ install(FILES
+ ${VISP_BINARY_DIR}/unix-install/resources.cfg
+ DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ if(OGRE_MEDIA_NOT_AVAILABLE)
+ install(DIRECTORY
+ ${VISP_BINARY_DIR}/data/ogre-simulator/media
+ DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/data/ogre-simulator
+ FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+ else()
+ configure_file(
+ ${VISP_SOURCE_DIR}/cmake/templates/resources.cfg.in
+ ${VISP_BINARY_DIR}/win-install/resources.cfg
+ IMMEDIATE @ONLY
+ )
+ install(FILES
+ ${VISP_BINARY_DIR}/win-install/resources.cfg
+ DESTINATION data/ogre-simulator
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ if(OGRE_MEDIA_NOT_AVAILABLE)
+ install(DIRECTORY
+ ${VISP_BINARY_DIR}/data/ogre-simulator/media
+ DESTINATION data/ogre-simulator
+ FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+ endif()
+ endif()
+ endif()
+endfunction()
diff --git a/cmake/VISPCRTLinkage.cmake b/cmake/VISPCRTLinkage.cmake
new file mode 100644
index 0000000..a99b662
--- /dev/null
+++ b/cmake/VISPCRTLinkage.cmake
@@ -0,0 +1,44 @@
+if(NOT MSVC)
+ message(FATAL_ERROR "CRT options are available only for MSVC")
+endif()
+
+if(NOT BUILD_SHARED_LIBS AND BUILD_WITH_STATIC_CRT)
+ foreach(flag_var
+ CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
+ CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
+ CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
+ CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
+ if(${flag_var} MATCHES "/MD")
+ string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}")
+ endif()
+ if(${flag_var} MATCHES "/MDd")
+ string(REGEX REPLACE "/MDd" "/MTd" ${flag_var} "${${flag_var}}")
+ endif()
+ endforeach(flag_var)
+
+ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /NODEFAULTLIB:atlthunk.lib /NODEFAULTLIB:msvcrt.lib /NODEFAULTLIB:msvcrtd.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_EXE_LINKER_FLAGS_DEBUG} /NODEFAULTLIB:libcmt.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /NODEFAULTLIB:libcmtd.lib")
+else()
+ foreach(flag_var
+ CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
+ CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
+ CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
+ CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
+ if(${flag_var} MATCHES "/MT")
+ string(REGEX REPLACE "/MT" "/MD" ${flag_var} "${${flag_var}}")
+ endif()
+ if(${flag_var} MATCHES "/MTd")
+ string(REGEX REPLACE "/MTd" "/MDd" ${flag_var} "${${flag_var}}")
+ endif()
+ endforeach(flag_var)
+endif()
+
+if(NOT ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} LESS 2.8 AND NOT ${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION} LESS 8.6)
+ include(ProcessorCount)
+ ProcessorCount(N)
+ if(NOT N EQUAL 0)
+ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP${N} ")
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N} ")
+ endif()
+endif()
diff --git a/cmake/VISPConfig.cmake b/cmake/VISPConfig.cmake
new file mode 100644
index 0000000..b0e7dc9
--- /dev/null
+++ b/cmake/VISPConfig.cmake
@@ -0,0 +1,146 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# CMake package config file for ViSP.
+#
+# ** File generated automatically, do not modify **
+#
+# This file will define the following CMake variables:
+# - VISP_INCLUDE_DIRS : ViSP and third-party include directories
+# - VISP_LIBRARIES : ViSP library to link against. Third-party libraries are
+# linked automatically thanks to cmake export file VISPTargets.cmake
+# - VISP_VERSION_STRING : Full ViSP version that is build. Example: "2.10.0"
+# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION. Example: "2"
+# - VISP_VERSION_MINOR : Minor version part of VISP_VERSION. Example: "10"
+# - VISP_VERSION_PATCH : Patch version part of VISP_VERSION. Example: "0"
+#
+# Advanced variables:
+# - VISP_SHARED : Use ViSP as shared library
+# - VISP_CONFIG_PATH : Path to this VISPConfig.cmake
+# - VISP_FIND_QUIETLY : If set to TRUE turn off messages during configuration
+# - VISP_USE_FILE : File to include to use ViSP without specific cmake code
+#
+# Windows specific variables:
+# - VISP_STATIC : If set to TRUE uses ViSP static library (.lib) rather then dynamic (.dll)
+#
+# Typical usage in user project:
+#
+# find_package(VISP)
+# include_directories(${VISP_INCLUDE_DIRS})
+# target_link_libraries(MY_TARGET_NAME ${VISP_LIBRARIES})
+#
+# It is also possible to build your project using VISP_USE_FILE.
+#
+# find_package(VISP)
+# if(VISP_FOUND)
+# include(${VISP_USE_FILE})
+# endif()
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# similar code exist in VISPDetectPlatform.cmake
+if(MSVC)
+ if(CMAKE_CL_64)
+ set(VISP_ARCH x64)
+ else()
+ set(VISP_ARCH x86)
+ endif()
+ if(MSVC_VERSION EQUAL 1400)
+ set(VISP_RUNTIME vc8)
+ elseif(MSVC_VERSION EQUAL 1500)
+ set(VISP_RUNTIME vc9)
+ elseif(MSVC_VERSION EQUAL 1600)
+ set(VISP_RUNTIME vc10)
+ elseif(MSVC_VERSION EQUAL 1700)
+ set(VISP_RUNTIME vc11)
+ elseif(MSVC_VERSION EQUAL 1800)
+ set(VISP_RUNTIME vc12)
+ endif()
+elseif(MINGW)
+ set(VISP_RUNTIME mingw)
+
+ execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine
+ OUTPUT_VARIABLE VISP_GCC_TARGET_MACHINE
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ if(VISP_GCC_TARGET_MACHINE MATCHES "64")
+ set(MINGW64 1)
+ set(VISP_ARCH x64)
+ else()
+ set(VISP_ARCH x86)
+ endif()
+endif()
+
+if(CMAKE_VERSION VERSION_GREATER 2.6.2)
+ unset(VISP_CONFIG_PATH CACHE)
+endif()
+
+if(NOT VISP_FIND_QUIETLY)
+ message(STATUS "ViSP ARCH: ${VISP_ARCH}")
+ message(STATUS "ViSP RUNTIME: ${VISP_RUNTIME}")
+endif()
+
+get_filename_component(VISP_CONFIG_PATH "${CMAKE_CURRENT_LIST_FILE}" PATH CACHE)
+if(VISP_RUNTIME AND VISP_ARCH)
+ if(NOT DEFINED VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
+ set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib")
+ elseif(NOT DEFINED VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib/VISPConfig.cmake")
+ set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
+ elseif(VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib/VISPConfig.cmake")
+ set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
+ elseif(VISP_STATIC EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
+ set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib")
+ endif()
+endif()
+
+if(VISP_LIB_PATH AND EXISTS "${VISP_LIB_PATH}/VISPConfig.cmake")
+ include("${VISP_LIB_PATH}/VISPConfig.cmake")
+
+ set(VISP_FOUND TRUE CACHE BOOL "" FORCE)
+
+ if(NOT VISP_FIND_QUIETLY)
+ message(STATUS "Found VISP ${VISP_VERSION} in ${VISP_LIB_PATH}")
+ if(NOT VISP_LIB_PATH MATCHES "/staticlib")
+ get_filename_component(_VISP_LIB_PATH "${VISP_LIB_PATH}/../bin" ABSOLUTE)
+ file(TO_NATIVE_PATH "${_VISP_LIB_PATH}" _VISP_LIB_PATH)
+ message(STATUS "You might need to add ${_VISP_LIB_PATH} to your PATH to be able to run your applications.")
+ endif()
+ endif()
+else()
+ if(NOT VISP_FIND_QUIETLY)
+ message(WARNING
+"Found ViSP for Windows but it has no binaries compatible with your configuration.
+You should manually point CMake variable VISP_DIR to your build of ViSP library."
+ )
+ endif()
+ set(VISP_FOUND FALSE CACHE BOOL "" FORCE)
+endif()
diff --git a/cmake/VISPDetectPlatform.cmake b/cmake/VISPDetectPlatform.cmake
new file mode 100644
index 0000000..bf4e730
--- /dev/null
+++ b/cmake/VISPDetectPlatform.cmake
@@ -0,0 +1,79 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# ----------------------------------------------------------------------------
+# Similar code exist in VISPConfig.cmake
+# ----------------------------------------------------------------------------
+
+if(NOT DEFINED VISP_STATIC)
+ # look for global setting
+ if(NOT DEFINED BUILD_SHARED_LIBS OR BUILD_SHARED_LIBS)
+ set(VISP_STATIC OFF)
+ else()
+ set(VISP_STATIC ON)
+ endif()
+endif()
+
+if(MSVC)
+ if(CMAKE_CL_64)
+ set(VISP_ARCH x64)
+ else()
+ set(VISP_ARCH x86)
+ endif()
+ if(MSVC_VERSION EQUAL 1400)
+ set(VISP_RUNTIME vc8)
+ elseif(MSVC_VERSION EQUAL 1500)
+ set(VISP_RUNTIME vc9)
+ elseif(MSVC_VERSION EQUAL 1600)
+ set(VISP_RUNTIME vc10)
+ elseif(MSVC_VERSION EQUAL 1700)
+ set(VISP_RUNTIME vc11)
+ elseif(MSVC_VERSION EQUAL 1800)
+ set(VISP_RUNTIME vc12)
+ elseif(MSVC_VERSION EQUAL 1900)
+ set(VISP_RUNTIME vc14)
+ endif()
+elseif(MINGW)
+ set(VISP_RUNTIME mingw)
+
+ execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine
+ OUTPUT_VARIABLE VISP_GCC_TARGET_MACHINE
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ if(VISP_GCC_TARGET_MACHINE MATCHES "64")
+ set(MINGW64 1)
+ set(VISP_ARCH x64)
+ else()
+ set(VISP_ARCH x86)
+ endif()
+endif()
diff --git a/cmake/VISPExtraTargets.cmake b/cmake/VISPExtraTargets.cmake
new file mode 100644
index 0000000..dc68f74
--- /dev/null
+++ b/cmake/VISPExtraTargets.cmake
@@ -0,0 +1,109 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# ----------------------------------------------------------------------------
+# Uninstall target, for "make uninstall"
+# ----------------------------------------------------------------------------
+configure_file(
+ cmake/templates/cmake_uninstall.cmake.in
+ "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
+ IMMEDIATE @ONLY)
+
+add_custom_target(uninstall
+ "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(uninstall PROPERTIES FOLDER "CMakeTargets")
+endif()
+
+# ----------------------------------------------------------------------------
+# Doxygen documentation target, for "make visp_doc" and "make html-doc" (to keep compat with previous versions)
+# ----------------------------------------------------------------------------
+if(DOXYGEN_FOUND)
+ add_custom_target(html-doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen) # for compat with previous versions
+ add_custom_target(visp_doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen)
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_doc PROPERTIES FOLDER "extra")
+ set_target_properties(html-doc PROPERTIES FOLDER "extra")
+ endif()
+endif()
+
+# ----------------------------------------------------------------------------
+# Tests target, for make visp_tests
+# ----------------------------------------------------------------------------
+if(BUILD_TESTS)
+ add_custom_target(visp_tests)
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_tests PROPERTIES FOLDER "extra")
+ endif()
+endif()
+
+# ----------------------------------------------------------------------------
+# Tests target, for make visp_examples
+# ----------------------------------------------------------------------------
+if(BUILD_EXAMPLES)
+ add_custom_target(visp_examples)
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_examples PROPERTIES FOLDER "extra")
+ endif()
+endif()
+
+# ----------------------------------------------------------------------------
+# Tests target, for make visp_demos
+# ----------------------------------------------------------------------------
+if(BUILD_DEMOS)
+ add_custom_target(visp_demos)
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_demos PROPERTIES FOLDER "extra")
+ endif()
+endif()
+
+# ----------------------------------------------------------------------------
+# Tests target, for make visp_tutorials
+# ----------------------------------------------------------------------------
+if(BUILD_TUTORIALS)
+ add_custom_target(visp_tutorials)
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_tutorials PROPERTIES FOLDER "extra")
+ endif()
+endif()
+
+# ----------------------------------------------------------------------------
+# Target building all ViSP modules
+# ----------------------------------------------------------------------------
+add_custom_target(visp_modules)
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(visp_modules PROPERTIES FOLDER "extra")
+endif()
+
diff --git a/cmake/VISPGenerateConfig.cmake b/cmake/VISPGenerateConfig.cmake
new file mode 100644
index 0000000..256f0b3
--- /dev/null
+++ b/cmake/VISPGenerateConfig.cmake
@@ -0,0 +1,213 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# This file generates the ViSPConfig.cmake file:
+# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
+# Part 2/3: ${BIN_DIR}/unix-install/VISPConfig.cmake -> For use with "make install"
+# Part 3/3: ${BIN_DIR}/win-install/VISPConfig.cmake -> For use within binary installers/packages
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Macro that returns the relative path to go from a child folder to the parent folder
+# input: path_to_child
+# output: path_to_parent, the relative path to go from path_to_child to parent
+# example: if input =lib/x86_64-linux-gnu, then output=../..
+macro(get_path_to_parent path_to_child path_to_parent)
+ set(${path_to_parent} "")
+ set(input_ "${path_to_child}")
+ while(input_)
+ if(input_)
+ set(${path_to_parent} "${${path_to_parent}}../")
+ endif()
+ get_filename_component(input_ "${input_}" PATH)
+ endwhile(input_)
+endmacro()
+
+# Here we determine the relative path from ./${CMAKE_INSTALL_LIBDIR} to its parent folder
+# if CMAKE_INSTALL_LIBDIR=lib, then VISP_INSTALL_LIBDIR_TO_PARENT=../
+# if CMAKE_INSTALL_LIBDIR=lib/x86_64-linux-gnu, then VISP_INSTALL_LIBDIR_TO_PARENT=../..
+get_path_to_parent(${CMAKE_INSTALL_LIBDIR} VISP_INSTALL_LIBDIR_TO_PARENT)
+
+#build list of modules available for the ViSP user
+set(VISP_LIB_COMPONENTS "")
+foreach(m ${VISP_MODULES_PUBLIC})
+ list(INSERT VISP_LIB_COMPONENTS 0 ${${m}_MODULE_DEPS_OPT} ${m})
+endforeach()
+vp_list_unique(VISP_LIB_COMPONENTS)
+#message("VISP_LIB_COMPONENTS: ${VISP_LIB_COMPONENTS}")
+set(VISP_MODULES_CONFIGCMAKE ${VISP_LIB_COMPONENTS})
+vp_list_filterout(VISP_LIB_COMPONENTS "^visp_")
+if(VISP_LIB_COMPONENTS)
+ list(REMOVE_ITEM VISP_MODULES_CONFIGCMAKE ${VISP_LIB_COMPONENTS})
+endif()
+
+
+# -------------------------------------------------------------------------------------------
+# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
+# -------------------------------------------------------------------------------------------
+
+# Export the library
+export(TARGETS ${VISPModules_TARGETS} FILE "${PROJECT_BINARY_DIR}/VISPModules.cmake")
+
+## Update include dirs
+set(VISP_INCLUDE_DIRS_CONFIGCMAKE "${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR}")
+foreach(m ${VISP_MODULES_BUILD})
+ if(EXISTS "${VISP_MODULE_${m}_LOCATION}/include")
+ list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE "${VISP_MODULE_${m}_LOCATION}/include")
+ endif()
+ list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE ${VISP_MODULE_${m}_INC_DEPS})
+endforeach()
+vp_list_unique(VISP_INCLUDE_DIRS_CONFIGCMAKE)
+
+configure_file(
+ cmake/templates/VISPConfig.cmake.in
+ ${VISP_BINARY_DIR}/VISPConfig.cmake
+ IMMEDIATE @ONLY
+)
+
+configure_file(
+ cmake/templates/VISPConfigVersion.cmake.in
+ ${VISP_BINARY_DIR}/VISPConfigVersion.cmake
+ IMMEDIATE @ONLY
+)
+
+configure_file(
+ cmake/VISPUse.cmake.in
+ ${VISP_BINARY_DIR}/VISPUse.cmake
+ IMMEDIATE @ONLY
+)
+
+# --------------------------------------------------------------------------------------------
+# Part 2/3: ${BIN_DIR}/unix-install/VISPConfig.cmake -> For use *with* "make install"
+# -------------------------------------------------------------------------------------------
+
+if(UNIX)
+ set(VISP_INCLUDE_DIRS_CONFIGCMAKE "\${VISP_INSTALL_PATH}/${CMAKE_INSTALL_INCLUDEDIR}")
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE ${VISP_MODULE_${m}_INC_DEPS})
+ endforeach()
+ vp_list_unique(VISP_INCLUDE_DIRS_CONFIGCMAKE)
+
+ configure_file(
+ cmake/templates/VISPConfig.cmake.in
+ ${VISP_BINARY_DIR}/unix-install/VISPConfig.cmake
+ IMMEDIATE @ONLY
+ )
+
+ configure_file(
+ cmake/templates/VISPConfigVersion.cmake.in
+ ${VISP_BINARY_DIR}/unix-install/VISPConfigVersion.cmake
+ IMMEDIATE @ONLY
+ )
+
+ configure_file(
+ cmake/VISPUse.cmake.in
+ ${VISP_BINARY_DIR}/unix-install/VISPUse.cmake
+ IMMEDIATE @ONLY
+ )
+
+ install(FILES
+ ${VISP_BINARY_DIR}/unix-install/VISPConfig.cmake
+ ${VISP_BINARY_DIR}/unix-install/VISPConfigVersion.cmake
+ ${VISP_BINARY_DIR}/unix-install/VISPUse.cmake
+ DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/visp"
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+
+ # Install the export set for use with the install-tree
+ install(EXPORT VISPModules
+ FILE VISPModules.cmake
+ DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/visp"
+ COMPONENT dev
+ )
+endif()
+
+# --------------------------------------------------------------------------------------------
+# Part 3/3: ${BIN_DIR}/win-install/VISPConfig.cmake -> For use within binary installers/packages
+# --------------------------------------------------------------------------------------------
+if(WIN32)
+ set(VISP_INCLUDE_DIRS_CONFIGCMAKE "\${VISP_CONFIG_PATH}/${CMAKE_INSTALL_INCLUDEDIR}")
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_INCLUDE_DIRS_CONFIGCMAKE ${VISP_MODULE_${m}_INC_DEPS})
+ endforeach()
+ vp_list_unique(VISP_INCLUDE_DIRS_CONFIGCMAKE)
+
+ configure_file(
+ cmake/templates/VISPConfig.cmake.in
+ ${VISP_BINARY_DIR}/win-install/VISPConfig.cmake
+ IMMEDIATE @ONLY
+ )
+
+ configure_file(
+ cmake/templates/VISPConfigVersion.cmake.in
+ ${VISP_BINARY_DIR}/win-install/VISPConfigVersion.cmake
+ IMMEDIATE @ONLY
+ )
+
+ configure_file(
+ cmake/VISPUse.cmake.in
+ ${VISP_BINARY_DIR}/win-install/VISPUse.cmake
+ IMMEDIATE @ONLY
+ )
+
+ if(BUILD_SHARED_LIBS)
+ install(FILES
+ "${CMAKE_BINARY_DIR}/win-install/ViSPConfig.cmake"
+ "${CMAKE_BINARY_DIR}/win-install/ViSPUse.cmake"
+ DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}"
+ COMPONENT dev)
+ install(EXPORT VISPModules
+ DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}${CMAKE_INSTALL_LIBDIR}"
+ FILE VISPModules.cmake
+ COMPONENT dev)
+ else()
+ install(FILES
+ "${CMAKE_BINARY_DIR}/win-install/ViSPConfig.cmake"
+ "${CMAKE_BINARY_DIR}/win-install/ViSPUse.cmake"
+ DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}"
+ COMPONENT dev)
+ install(EXPORT VISPModules
+ DESTINATION "${VISP_INSTALL_BINARIES_PREFIX}static${CMAKE_INSTALL_LIBDIR}"
+ FILE VISPModules.cmake
+ COMPONENT dev)
+ endif()
+
+ install(FILES
+ "cmake/VISPConfig.cmake"
+ "${VISP_BINARY_DIR}/win-install/VISPConfigVersion.cmake"
+ DESTINATION "${CMAKE_INSTALL_PREFIX}"
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+ COMPONENT dev
+ )
+endif()
diff --git a/cmake/VISPGenerateHeaders.cmake b/cmake/VISPGenerateHeaders.cmake
new file mode 100644
index 0000000..2110e9b
--- /dev/null
+++ b/cmake/VISPGenerateHeaders.cmake
@@ -0,0 +1,77 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# platform-specific config file
+configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpConfig.h.in" "${VISP_INCLUDE_DIR}/visp3/core/vpConfig.h")
+install(FILES "${VISP_INCLUDE_DIR}/visp3/core/vpConfig.h"
+ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/visp3/core
+ COMPONENT dev
+)
+
+#----------------------------------------------------------------------
+# information file
+#----------------------------------------------------------------------
+configure_file(${VISP_SOURCE_DIR}/cmake/templates/ViSP-third-party.txt.in "${VISP_BINARY_DIR}/ViSP-third-party.txt")
+
+# ----------------------------------------------------------------------------
+# visp_modules.h based on actual modules list
+# ----------------------------------------------------------------------------
+set(VISP_MODULE_DEFINITIONS_CONFIGMAKE "#ifndef __visp_modules_h__\n#define __visp_modules_h__\n\n")
+
+set(VISP_MOD_LIST ${VISP_MODULES_PUBLIC})
+vp_list_sort(VISP_MOD_LIST)
+foreach(m ${VISP_MOD_LIST})
+ if(m MATCHES "^visp_")
+ string(REGEX REPLACE "^visp_" "" m "${m}")
+ endif()
+ string(TOUPPER "${m}" m)
+ set(VISP_MODULE_DEFINITIONS_CONFIGMAKE "${VISP_MODULE_DEFINITIONS_CONFIGMAKE}#define VISP_HAVE_MODULE_${m}\n")
+endforeach()
+
+set(VISP_MODULE_DEFINITIONS_CONFIGMAKE "${VISP_MODULE_DEFINITIONS_CONFIGMAKE}\n#endif\n")
+
+configure_file("${VISP_SOURCE_DIR}/cmake/templates/visp_modules.h.in" "${VISP_INCLUDE_DIR}/visp3/visp_modules.h")
+install(FILES "${VISP_INCLUDE_DIR}/visp3/visp_modules.h"
+ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/visp3
+ COMPONENT dev
+)
+
+# ----------------------------------------------------------------------------
+# install old headers
+# ----------------------------------------------------------------------------
+file(GLOB old_hdrs "${VISP_INCLUDE_DIR}/visp/*.h")
+install(FILES ${old_hdrs}
+ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/visp
+ COMPONENT dev
+)
diff --git a/cmake/VISPGeneratePkgConfigScript.cmake b/cmake/VISPGeneratePkgConfigScript.cmake
new file mode 100644
index 0000000..c8b41cc
--- /dev/null
+++ b/cmake/VISPGeneratePkgConfigScript.cmake
@@ -0,0 +1,302 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# This file generates the ViSP library config shell scripts:
+# - visp-config in <build dir>/bin from visp-config.in
+# - visp-config in <build dir>/install from visp-config.install.in
+# When make install, this file is copied in <install dir>/bin
+# - visp.pc in <build dir>/install from visp.pc.in
+# When make install, this file is copied in <install dir>/lib/pkgconfig
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+if (UNIX)
+ #######################################################################
+ #
+ # for Unix platforms: Linux, OSX
+ #
+ #######################################################################
+ set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.in")
+ set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config")
+
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.install.in")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/unix-install/visp-config")
+
+ set(FILE_VISP_CONFIG_PC_INSTALL_IN "cmake/templates/visp.pc.in")
+ set(FILE_VISP_CONFIG_PC_INSTALL "${VISP_BINARY_DIR}/unix-install/visp.pc")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_PREFIX
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_CFLAGS
+ #----------------------------------------------------------------------
+ foreach(m ${VISP_MODULES_BUILD})
+ foreach(inc ${VISP_MODULE_${m}_INC_DEPS})
+ list(APPEND VISP_CONFIG_CFLAGS "-I${inc}")
+ endforeach()
+ endforeach()
+ vp_list_unique(VISP_CONFIG_CFLAGS)
+
+ # Format the string to suppress CMake separators ";"
+ vp_list_remove_separator(VISP_CONFIG_CFLAGS)
+
+ if(BUILD_TEST_COVERAGE)
+ # Add build options for test coverage. Currently coverage is only supported
+ # on gcc compiler
+ # Because using -fprofile-arcs with shared lib can cause problems like:
+ # hidden symbol `__bb_init_func', we add this option only for static
+ # library build
+ set(VISP_CONFIG_CFLAGS "${VISP_CONFIG_CFLAGS} -ftest-coverage -fprofile-arcs")
+ endif()
+
+ #---------------------------------------------------------------------
+ # Updates the <build dir>/bin/visp-config shell script
+ # Updates VISP_CONFIG_LIBS_SCRIPT (for visp-config)
+ # Updates VISP_CONFIG_CFLAGS_SCRIPT (for visp-config)
+ #----------------------------------------------------------------------
+
+ # prepend with ViSP own include dir
+ set(VISP_CONFIG_CFLAGS_SCRIPT "-I$PREFIX/${CMAKE_INSTALL_INCLUDEDIR} ${VISP_CONFIG_CFLAGS}")
+
+ # prepend with ViSP own modules first
+ set(VISP_CONFIG_LIBS_SCRIPT "")
+ foreach(m ${VISP_MODULES_BUILD})
+ # need to be improved
+ if(POLICY CMP0026)
+ cmake_policy(PUSH)
+ cmake_policy(SET CMP0026 OLD)
+ get_target_property(m_libpath ${m} LOCATION_Release)
+ cmake_policy(POP)
+ else()
+ get_target_property(m_libpath ${m} LOCATION_Release)
+ endif()
+ get_filename_component(m_libname "${m_libpath}" NAME)
+ list(APPEND VISP_CONFIG_LIBS_SCRIPT "$PREFIX/${CMAKE_INSTALL_LIBDIR}/${m_libname}")
+ endforeach()
+ # append deps
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_CONFIG_LIBS_SCRIPT ${VISP_MODULE_${m}_LINK_DEPS})
+ endforeach()
+ vp_list_unique(VISP_CONFIG_LIBS_SCRIPT)
+# message("VISP_CONFIG_LIBS_SCRIPT: ${VISP_CONFIG_LIBS_SCRIPT}")
+
+ set(VISP_ECHO_NO_NEWLINE_CHARACTER "")
+ set(VISP_ECHO_NO_NEWLINE_OPTION "")
+ if(APPLE)
+ set(VISP_ECHO_NO_NEWLINE_CHARACTER "\\c")
+ else()
+ set(VISP_ECHO_NO_NEWLINE_OPTION "-n")
+ endif()
+
+ configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
+
+ #---------------------------------------------------------------------
+ # Updates the <build dir>/install/visp-config shell script
+ #----------------------------------------------------------------------
+
+ configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
+
+ #---------------------------------------------------------------------
+ # Updates the <build dir>/install/visp.pc pkg-config file
+ # Updates VISP_CONFIG_CFLAGS_PC (for libvisp.pc used by pkg-config)
+ # Updates VISP_CONFIG_LIBS_PC (for libvisp.pc used by pkg-config)
+ #----------------------------------------------------------------------
+ set(exec_prefix "\${prefix}")
+ set(includedir "\${prefix}/${CMAKE_INSTALL_INCLUDEDIR}")
+ set(libdir "\${prefix}/${CMAKE_INSTALL_LIBDIR}")
+
+ # prepend with ViSP own include dir
+ set(VISP_CONFIG_CFLAGS_PC "-I\${includedir} ${VISP_CONFIG_CFLAGS}")
+
+ # prepend with ViSP own modules first
+ set(VISP_CONFIG_LIBS_PC "")
+ foreach(m ${VISP_MODULES_BUILD})
+ # need to be improved
+ if(POLICY CMP0026)
+ cmake_policy(PUSH)
+ cmake_policy(SET CMP0026 OLD)
+ get_target_property(m_libpath ${m} LOCATION_Release)
+ cmake_policy(POP)
+ else()
+ get_target_property(m_libpath ${m} LOCATION_Release)
+ endif()
+ get_filename_component(m_libname "${m_libpath}" NAME)
+ list(APPEND VISP_CONFIG_LIBS_PC "\${libdir}/${m_libname}")
+ endforeach()
+ # append deps
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_CONFIG_LIBS_PC ${VISP_MODULE_${m}_LINK_DEPS})
+ endforeach()
+ vp_list_remove_separator(VISP_CONFIG_LIBS_PC)
+
+ configure_file(${FILE_VISP_CONFIG_PC_INSTALL_IN} ${FILE_VISP_CONFIG_PC_INSTALL})
+
+else(UNIX)
+ #######################################################################
+ #
+ # for windows platforms
+ #
+ #######################################################################
+ set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.bat.in")
+ set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config.bat")
+
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.bat.in")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/win-install/visp-config-${VISP_ARCH}-${VISP_RUNTIME}.bat")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_PREFIX
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_DEF
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_DEFS "")
+ set(VISP_OPENMP_SUPPORT "no")
+ if(NOT ${VISP_OPENMP_FLAGS} STREQUAL "")
+ set(VISP_CONFIG_SCRIPT_DEFS "${VISP_OPENMP_FLAGS}")
+ set(VISP_OPENMP_SUPPORT "yes")
+ endif()
+ if(NOT ${VISP_CPP11_FLAGS} STREQUAL "")
+ set(VISP_CONFIG_SCRIPT_DEFS "${VISP_CPP11_FLAGS}, ${VISP_CONFIG_SCRIPT_DEFS}")
+ endif()
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_INCL
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_INC "%PREFIX%/${CMAKE_INSTALL_INCLUDEDIR}")
+ foreach(m ${VISP_MODULES_BUILD})
+ foreach(inc ${VISP_MODULE_${m}_INC_DEPS})
+ list(APPEND VISP_CONFIG_SCRIPT_INC "${inc}")
+ endforeach()
+ endforeach()
+ vp_list_unique(VISP_CONFIG_SCRIPT_INC)
+ #message(VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC})
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_LIBDIR
+ # 1/ For usage with the build tree
+ # 2/ For usage with the install tree
+ #
+ # and updates VISP_CONFIG_SCRIPT_LIBS_${config}
+ #----------------------------------------------------------------------
+ set(TMP_SCRIPT_LIBS_DEBUG "${VISP_INTERN_LIBRARY}${VISP_DLLVERSION}${VISP_DEBUG_POSTFIX}.lib")
+ set(TMP_SCRIPT_LIBS_OPTIMIZED "${VISP_INTERN_LIBRARY}${VISP_DLLVERSION}.lib")
+
+ #MESSAGE(VISP_EXTERN_LIBRARIES: ${VISP_EXTERN_LIBRARIES})
+ set(TMP_IS_DEBUG FALSE)
+ set(TMP_IS_OPTIMIZED FALSE)
+ foreach(lib ${VISP_EXTERN_LIBRARIES})
+ if("${lib}" MATCHES "[d][e][b][u][g]")
+ set(TMP_IS_DEBUG TRUE)
+ elseif("${lib}" MATCHES "[o][p][t][i][m][i][z][e][d]")
+ set(TMP_IS_OPTIMIZED TRUE)
+ else()
+ # Get the library name
+ get_filename_component(libname ${lib} NAME)
+ if("${libname}" MATCHES ".+[.][l][i][b]" OR "${libname}" MATCHES ".+[.][L][i][b]")
+ #MESSAGE("${libname} matches .lib or .Lib")
+ else()
+ # We need to add .lib suffix
+ #MESSAGE("For ${libname} we add .lib suffix")
+ set(libname "${libname}.lib")
+ endif()
+
+ # Get the library path
+ get_filename_component(libpath ${lib} PATH)
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR_ "${libpath}")
+
+ if(TMP_IS_DEBUG)
+ set(TMP_IS_DEBUG FALSE)
+ list(APPEND TMP_SCRIPT_LIBS_DEBUG ${libname})
+ elseif(TMP_IS_OPTIMIZED)
+ set(TMP_IS_OPTIMIZED FALSE)
+ list(APPEND TMP_SCRIPT_LIBS_OPTIMIZED ${libname})
+ else()
+ list(APPEND TMP_SCRIPT_LIBS_DEBUG ${libname})
+ list(APPEND TMP_SCRIPT_LIBS_OPTIMIZED ${libname})
+ endif()
+ endif()
+ endforeach(lib)
+
+ # Format the string
+ set(VISP_CONFIG_SCRIPT_LIBS_DEBUG "${TMP_SCRIPT_LIBS_DEBUG}")
+ set(VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED "${TMP_SCRIPT_LIBS_OPTIMIZED}")
+
+ # Format the string
+ string(REGEX REPLACE "lib/Release" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
+ string(REGEX REPLACE "lib/Debug" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
+
+ # 1/ For usage with the build tree
+ set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib/$(ConfigurationName)")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
+ list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
+ configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
+
+ # 2/ For usage with the install tree
+ set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/${VISP_ARCH}/${VISP_RUNTIME}/lib")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
+ list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
+ configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
+endif(UNIX)
+
+#----------------------------------------------------------------------
+# customize install target
+#----------------------------------------------------------------------
+# install rule for visp-config shell script
+install(FILES ${FILE_VISP_CONFIG_SCRIPT_INSTALL}
+ DESTINATION ${CMAKE_INSTALL_BINDIR}
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
+ OWNER_WRITE
+ COMPONENT dev
+)
+
+# install rule for visp.pc pkg-config file
+if(UNIX)
+ install(FILES ${FILE_VISP_CONFIG_PC_INSTALL}
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_WRITE
+ COMPONENT dev
+ )
+else()
+ # not implemented yet
+endif()
+
+
diff --git a/cmake/VISPModule.cmake b/cmake/VISPModule.cmake
new file mode 100644
index 0000000..9e0d8ea
--- /dev/null
+++ b/cmake/VISPModule.cmake
@@ -0,0 +1,966 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Local variables (set for each module):
+#
+# name - short name in lower case i.e. core
+# the_module - full name in lower case i.e. visp_core
+
+# Global variables:
+#
+# VISP_MODULE_${the_module}_LOCATION
+# VISP_MODULE_${the_module}_BINARY_DIR
+# VISP_MODULE_${the_module}_DESCRIPTION
+# VISP_MODULE_${the_module}_CLASS - PUBLIC|INTERNAL|BINDINGS
+# VISP_MODULE_${the_module}_HEADERS
+# VISP_MODULE_${the_module}_SOURCES
+# VISP_MODULE_${the_module}_DEPS - final flattened set of module dependencies
+# VISP_MODULE_${the_module}_DEPS_TO_LINK - differs from above for world build only
+# VISP_MODULE_${the_module}_DEPS_EXT - non-module dependencies
+# VISP_MODULE_${the_module}_REQ_DEPS
+# VISP_MODULE_${the_module}_OPT_DEPS
+# VISP_MODULE_${the_module}_PRIVATE_REQ_DEPS
+# VISP_MODULE_${the_module}_PRIVATE_OPT_DEPS
+# VISP_MODULE_${the_module}_CHILDREN - list of submodules for compound modules (cmake >= 2.8.8)
+# HAVE_${the_module} - for fast check of module availability
+
+# To control the setup of the module you could also set:
+# the_description - text to be used as current module description
+# VISP_MODULE_TYPE - STATIC|SHARED - set to force override global settings for current module
+# BUILD_${the_module}_INIT - ON|OFF (default ON) - initial value for BUILD_${the_module}
+# VISP_MODULE_CHILDREN - list of submodules
+
+# The verbose template for ViSP module:
+#
+# vp_add_module(modname <dependencies>)
+# vp_glob_module_sources((<extra sources&headers>)
+# or glob them manually and vp_set_module_sources(...)
+# vp_module_include_directories(<extra include directories>)
+# vp_create_module()
+#
+# If module have no "extra" then you can define it in one line:
+#
+# vp_define_module(modname <dependencies>)
+
+# clean flags for modules enabled on previous cmake run
+# this is necessary to correctly handle modules removal
+foreach(mod ${VISP_MODULES_BUILD} ${VISP_MODULES_DISABLED_USER} ${VISP_MODULES_DISABLED_AUTO} ${VISP_MODULES_DISABLED_FORCE})
+ if(HAVE_${mod})
+ unset(HAVE_${mod} CACHE)
+ endif()
+ unset(VISP_MODULE_${mod}_REQ_DEPS CACHE)
+ unset(VISP_MODULE_${mod}_OPT_DEPS CACHE)
+ unset(VISP_MODULE_${mod}_PRIVATE_REQ_DEPS CACHE)
+ unset(VISP_MODULE_${mod}_PRIVATE_OPT_DEPS CACHE)
+ unset(VISP_MODULE_${mod}_LINK_DEPS CACHE)
+ unset(VISP_MODULE_${mod}_INC_DEPS CACHE)
+endforeach()
+
+# clean modules info which needs to be recalculated
+set(VISP_MODULES_PUBLIC "" CACHE INTERNAL "List of ViSP modules marked for export")
+set(VISP_MODULES_BUILD "" CACHE INTERNAL "List of ViSP modules included into the build")
+set(VISP_MODULES_DISABLED_USER "" CACHE INTERNAL "List of ViSP modules explicitly disabled by user")
+set(VISP_MODULES_DISABLED_AUTO "" CACHE INTERNAL "List of ViSP modules implicitly disabled due to dependencies")
+set(VISP_MODULES_DISABLED_FORCE "" CACHE INTERNAL "List of ViSP modules which can not be build in current configuration")
+
+# adds dependencies to ViSP module
+# Usage:
+# add_dependencies(visp_<name> [REQUIRED] [<list of dependencies>] [OPTIONAL <list of modules>])
+# Notes:
+# * <list of dependencies> - can include full names of modules or full pathes to shared/static libraries or cmake targets
+macro(vp_add_dependencies full_modname)
+ vp_debug_message("vp_add_dependencies(" ${full_modname} ${ARGN} ")")
+ #we don't clean the dependencies here to allow this macro several times for every module
+ foreach(d "REQUIRED" ${ARGN})
+ if(d STREQUAL "REQUIRED")
+ set(__depsvar VISP_MODULE_${full_modname}_REQ_DEPS)
+ elseif(d STREQUAL "OPTIONAL")
+ set(__depsvar VISP_MODULE_${full_modname}_OPT_DEPS)
+ elseif(d STREQUAL "PRIVATE_REQUIRED")
+ set(__depsvar VISP_MODULE_${full_modname}_PRIVATE_REQ_DEPS)
+ elseif(d STREQUAL "PRIVATE_OPTIONAL")
+ set(__depsvar VISP_MODULE_${full_modname}_PRIVATE_OPT_DEPS)
+ else()
+ list(APPEND ${__depsvar} "${d}")
+ endif()
+ endforeach()
+ unset(__depsvar)
+
+ vp_list_unique(VISP_MODULE_${full_modname}_REQ_DEPS)
+ vp_list_unique(VISP_MODULE_${full_modname}_OPT_DEPS)
+ vp_list_unique(VISP_MODULE_${full_modname}_PRIVATE_REQ_DEPS)
+ vp_list_unique(VISP_MODULE_${full_modname}_PRIVATE_OPT_DEPS)
+
+ set(VISP_MODULE_${full_modname}_REQ_DEPS ${VISP_MODULE_${full_modname}_REQ_DEPS}
+ CACHE INTERNAL "Required dependencies of ${full_modname} module")
+ set(VISP_MODULE_${full_modname}_OPT_DEPS ${VISP_MODULE_${full_modname}_OPT_DEPS}
+ CACHE INTERNAL "Optional dependencies of ${full_modname} module")
+ set(VISP_MODULE_${full_modname}_PRIVATE_REQ_DEPS ${VISP_MODULE_${full_modname}_PRIVATE_REQ_DEPS}
+ CACHE INTERNAL "Required private dependencies of ${full_modname} module")
+ set(VISP_MODULE_${full_modname}_PRIVATE_OPT_DEPS ${VISP_MODULE_${full_modname}_PRIVATE_OPT_DEPS}
+ CACHE INTERNAL "Optional private dependencies of ${full_modname} module")
+endmacro()
+
+# declare new ViSP module in current folder
+# Usage:
+# vp_add_module(<name> [INTERNAL|BINDINGS] [REQUIRED] [<list of dependencies>] [OPTIONAL <list of optional dependencies>])
+# Example:
+# vp_add_module(mymodule INTERNAL visp_core OPTIONAL visp_ar)
+macro(vp_add_module _name)
+ vp_debug_message("vp_add_module(" ${_name} ${ARGN} ")")
+ string(TOLOWER "${_name}" name)
+ set(the_module visp_${name})
+ #message("Found module: ${the_module}")
+
+ # the first pass - collect modules info, the second pass - create targets
+ if(VISP_INITIAL_PASS)
+ #guard agains redefinition
+ if(";${VISP_MODULES_BUILD};${VISP_MODULES_DISABLED_USER};" MATCHES ";${the_module};")
+ message(FATAL_ERROR "Redefinition of the ${the_module} module.
+ at: ${CMAKE_CURRENT_SOURCE_DIR}
+ previously defined at: ${VISP_MODULE_${the_module}_LOCATION}
+")
+ endif()
+
+ if(NOT DEFINED the_description)
+ set(the_description "The ViSP ${name} module")
+ endif()
+
+ if(NOT DEFINED BUILD_${the_module}_INIT)
+ set(BUILD_${the_module}_INIT ON)
+ endif()
+
+ # create option to enable/disable this module
+ option(BUILD_MODULE_${the_module} "Include ${the_module} module into ViSP build" ${BUILD_${the_module}_INIT})
+
+ # remember the module details
+ set(VISP_MODULE_${the_module}_DESCRIPTION "${the_description}" CACHE INTERNAL "Brief description of ${the_module} module")
+ set(VISP_MODULE_${the_module}_LOCATION "${CMAKE_CURRENT_SOURCE_DIR}" CACHE INTERNAL "Location of ${the_module} module sources")
+
+ set(VISP_MODULE_${the_module}_LINK_DEPS "" CACHE INTERNAL "")
+ set(VISP_MODULE_${the_module}_INC_DEPS "" CACHE INTERNAL "")
+
+ # parse list of dependencies
+ if("${ARGV1}" STREQUAL "INTERNAL" OR "${ARGV1}" STREQUAL "BINDINGS")
+ set(VISP_MODULE_${the_module}_CLASS "${ARGV1}" CACHE INTERNAL "The category of the module")
+ set(__vp_argn__ ${ARGN})
+ list(REMOVE_AT __vp_argn__ 0)
+ vp_add_dependencies(${the_module} ${__vp_argn__})
+ unset(__vp_argn__)
+ else()
+ set(VISP_MODULE_${the_module}_CLASS "PUBLIC" CACHE INTERNAL "The category of the module")
+ vp_add_dependencies(${the_module} ${ARGN})
+ if(BUILD_MODULE_${the_module})
+ set(VISP_MODULES_PUBLIC ${VISP_MODULES_PUBLIC} "${the_module}" CACHE INTERNAL "List of ViSP modules marked for export")
+ endif()
+ endif()
+
+ if(BUILD_MODULE_${the_module})
+ set(VISP_MODULES_BUILD ${VISP_MODULES_BUILD} "${the_module}" CACHE INTERNAL "List of ViSP modules included into the build")
+ else()
+ set(VISP_MODULES_DISABLED_USER ${VISP_MODULES_DISABLED_USER} "${the_module}" CACHE INTERNAL "List of ViSP modules explicitly disabled by user")
+ endif()
+
+ # add submodules if any
+ set(VISP_MODULE_${the_module}_CHILDREN "${VISP_MODULE_CHILDREN}" CACHE INTERNAL "List of ${the_module} submodules")
+
+ # stop processing of current file
+ return()
+ else()
+ set(VISP_MODULE_${the_module}_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE INTERNAL "")
+ if(NOT BUILD_MODULE_${the_module})
+ return() # extra protection from redefinition
+ endif()
+ endif()
+endmacro()
+
+# remove visp_ prefix from name
+macro(vp_short_module_name name)
+ if(${name} MATCHES "^visp_")
+ string(REGEX REPLACE "^visp_" "" ${name} "${${name}}")
+ endif()
+endmacro()
+
+# collect modules from specified directories
+# NB: must be called only once!
+macro(vp_glob_modules)
+ if(DEFINED VISP_INITIAL_PASS)
+ message(FATAL_ERROR "ViSP has already loaded its modules. Calling vp_glob_modules second time is not allowed.")
+ endif()
+ set(__directories_observed "")
+
+ # collect modules
+ set(VISP_INITIAL_PASS ON)
+ set(VISP_PROCESSING_EXTRA_MODULES 0)
+ foreach(__path ${ARGN})
+ if("${__path}" STREQUAL "EXTRA")
+ set(VISP_PROCESSING_EXTRA_MODULES 1)
+ endif()
+ get_filename_component(__path "${__path}" ABSOLUTE)
+
+ list(FIND __directories_observed "${__path}" __pathIdx)
+ if(__pathIdx GREATER -1)
+ message(FATAL_ERROR "The directory ${__path} is observed for ViSP modules second time.")
+ endif()
+ list(APPEND __directories_observed "${__path}")
+
+ file(GLOB __vpmodules RELATIVE "${__path}" "${__path}/*")
+
+ if(__vpmodules)
+ list(SORT __vpmodules)
+ foreach(mod ${__vpmodules})
+ get_filename_component(__modpath "${__path}/${mod}" ABSOLUTE)
+ if(EXISTS "${__modpath}/CMakeLists.txt")
+
+ list(FIND __directories_observed "${__modpath}" __pathIdx)
+ if(__pathIdx GREATER -1)
+ message(FATAL_ERROR "The module from ${__modpath} is already loaded.")
+ endif()
+ list(APPEND __directories_observed "${__modpath}")
+
+ add_subdirectory("${__modpath}" "${CMAKE_CURRENT_BINARY_DIR}/${mod}/.${mod}")
+ else()
+ # modules in tracker
+ get_filename_component(__subpath "${__path}/${mod}" ABSOLUTE)
+ file(GLOB __vpsubmodules RELATIVE "${__subpath}" "${__subpath}/*")
+ if(__vpsubmodules)
+ list(SORT __vpsubmodules)
+ foreach(submod ${__vpsubmodules})
+ get_filename_component(__submodpath "${__subpath}/${submod}" ABSOLUTE)
+ if(EXISTS "${__submodpath}/CMakeLists.txt")
+ list(FIND __directories_observed "${__submodpath}" __pathIdx)
+ if(__pathIdx GREATER -1)
+ message(FATAL_ERROR "The module from ${__submodpath} is already loaded.")
+ endif()
+ list(APPEND __directories_observed "${__submodpath}")
+ add_subdirectory("${__submodpath}" "${CMAKE_CURRENT_BINARY_DIR}/${submod}/.${submod}")
+ endif()
+ endforeach()
+ endif()
+ endif()
+ endforeach()
+ endif()
+ endforeach()
+ vp_clear_vars(__vpmodules __directories_observed __path __modpath __pathIdx __vpsubmodules __subpath __submodpath)
+
+ # resolve dependencies
+ __vp_resolve_dependencies()
+
+ # create modules
+ set(VISP_INITIAL_PASS OFF PARENT_SCOPE)
+ set(VISP_INITIAL_PASS OFF)
+
+ foreach(m ${VISP_MODULES_BUILD})
+ if(m MATCHES "^visp_")
+ string(REGEX REPLACE "^visp_" "" __shortname "${m}")
+ add_subdirectory("${VISP_MODULE_${m}_LOCATION}" "${CMAKE_CURRENT_BINARY_DIR}/${__shortname}")
+ else()
+ message(WARNING "Check module name: ${m}")
+ add_subdirectory("${VISP_MODULE_${m}_LOCATION}" "${CMAKE_CURRENT_BINARY_DIR}/${m}")
+ endif()
+ endforeach()
+
+ unset(__shortname)
+endmacro()
+
+# disables ViSP module with missing dependencies
+function(__vp_module_turn_off the_module)
+ list(REMOVE_ITEM VISP_MODULES_DISABLED_AUTO "${the_module}")
+ list(APPEND VISP_MODULES_DISABLED_AUTO "${the_module}")
+ list(REMOVE_ITEM VISP_MODULES_BUILD "${the_module}")
+ list(REMOVE_ITEM VISP_MODULES_PUBLIC "${the_module}")
+ set(HAVE_${the_module} OFF CACHE INTERNAL "Module ${the_module} can not be built in current configuration")
+
+ set(VISP_MODULES_DISABLED_AUTO "${VISP_MODULES_DISABLED_AUTO}" CACHE INTERNAL "")
+ set(VISP_MODULES_BUILD "${VISP_MODULES_BUILD}" CACHE INTERNAL "")
+ set(VISP_MODULES_PUBLIC "${VISP_MODULES_PUBLIC}" CACHE INTERNAL "")
+endfunction()
+
+# sort modules by dependencies
+function(__vp_sort_modules_by_deps __lst)
+ vp_list_sort(${__lst})
+ set(input ${${__lst}})
+ set(result "")
+ while(input)
+ list(LENGTH input length_before)
+ foreach (m ${input})
+ # check if module is in the result already
+ if (NOT ";${result};" MATCHES ";${m};")
+ # scan through module dependencies...
+ set(unresolved_deps_found FALSE)
+ foreach (d ${VISP_MODULE_${m}_CHILDREN} ${VISP_MODULE_${m}_DEPS})
+ # ... which are not already in the result and are enabled
+ if ((NOT ";${result};" MATCHES ";${d};") AND HAVE_${d})
+ set(unresolved_deps_found TRUE)
+ break()
+ endif()
+ endforeach()
+ # chek if all dependencies for this module has been resolved
+ if (NOT unresolved_deps_found)
+ list(APPEND result ${m})
+ list(REMOVE_ITEM input ${m})
+ endif()
+ endif()
+ endforeach()
+ list(LENGTH input length_after)
+ # check for infinite loop or unresolved dependencies
+ if (NOT length_after LESS length_before)
+ message(WARNING "Unresolved dependencies or loop in dependency graph (${length_after})\n"
+ "Processed ${__lst}: ${${__lst}}\n"
+ "Good modules: ${result}\n"
+ "Bad modules: ${input}"
+ )
+ list(APPEND result ${input})
+ break()
+ endif()
+ endwhile()
+ set(${__lst} "${result}" PARENT_SCOPE)
+endfunction()
+
+# resolve dependensies
+function(__vp_resolve_dependencies)
+ foreach(m ${VISP_MODULES_DISABLED_USER})
+ set(HAVE_${m} OFF CACHE INTERNAL "Module ${m} will not be built in current configuration")
+ endforeach()
+ foreach(m ${VISP_MODULES_BUILD})
+ set(HAVE_${m} ON CACHE INTERNAL "Module ${m} will be built in current configuration")
+ endforeach()
+
+ # disable MODULES with unresolved dependencies
+ set(has_changes ON)
+ while(has_changes)
+ set(has_changes OFF)
+ foreach(m ${VISP_MODULES_BUILD})
+ set(__deps ${VISP_MODULE_${m}_REQ_DEPS} ${VISP_MODULE_${m}_PRIVATE_REQ_DEPS})
+ while(__deps)
+ vp_list_pop_front(__deps d)
+ string(TOLOWER "${d}" upper_d)
+ if(NOT (HAVE_${d} OR HAVE_${upper_d} OR TARGET ${d} OR EXISTS ${d}))
+ if(d MATCHES "^visp_") # TODO Remove this condition in the future and use HAVE_ variables only
+ message(STATUS "Module ${m} disabled because ${d} dependency can't be resolved!")
+ __vp_module_turn_off(${m})
+ set(has_changes ON)
+ break()
+ else()
+ message(STATUS "Assume that non-module dependency is available: ${d} (for module ${m})")
+ endif()
+ endif()
+ endwhile()
+ endforeach()
+ endwhile()
+
+# message(STATUS "List of active modules: ${VISP_MODULES_BUILD}")
+
+ foreach(m ${VISP_MODULES_BUILD})
+ set(deps_${m} ${VISP_MODULE_${m}_REQ_DEPS})
+ foreach(d ${VISP_MODULE_${m}_OPT_DEPS})
+ if(NOT (";${deps_${m}};" MATCHES ";${d};"))
+ if(HAVE_${d} OR TARGET ${d})
+ list(APPEND deps_${m} ${d})
+ endif()
+ endif()
+ endforeach()
+# message(STATUS "Initial deps of ${m} (w/o private deps): ${deps_${m}}")
+ endforeach()
+
+ # propagate dependencies
+ set(has_changes ON)
+ while(has_changes)
+ set(has_changes OFF)
+ foreach(m2 ${VISP_MODULES_BUILD}) # transfer deps of m2 to m
+ foreach(m ${VISP_MODULES_BUILD})
+ if((NOT m STREQUAL m2) AND ";${deps_${m}};" MATCHES ";${m2};")
+ foreach(d ${deps_${m2}})
+ if(NOT (";${deps_${m}};" MATCHES ";${d};"))
+# message(STATUS " Transfer dependency ${d} from ${m2} to ${m}")
+ list(APPEND deps_${m} ${d})
+ set(has_changes ON)
+ endif()
+ endforeach()
+ endif()
+ endforeach()
+ endforeach()
+ endwhile()
+
+ # process private deps
+ foreach(m ${VISP_MODULES_BUILD})
+ foreach(d ${VISP_MODULE_${m}_PRIVATE_REQ_DEPS})
+ if(NOT (";${deps_${m}};" MATCHES ";${d};"))
+ list(APPEND deps_${m} ${d})
+ endif()
+ endforeach()
+ foreach(d ${VISP_MODULE_${m}_PRIVATE_OPT_DEPS})
+ if(NOT (";${deps_${m}};" MATCHES ";${d};"))
+ if(HAVE_${d} OR TARGET ${d})
+ list(APPEND deps_${m} ${d})
+ endif()
+ endif()
+ endforeach()
+ endforeach()
+
+ vp_list_sort(VISP_MODULES_BUILD)
+
+ foreach(m ${VISP_MODULES_BUILD})
+ #message(STATUS "FULL deps of ${m}: ${deps_${m}}")
+ set(VISP_MODULE_${m}_DEPS ${deps_${m}})
+ set(VISP_MODULE_${m}_DEPS_EXT ${deps_${m}})
+ vp_list_filterout(VISP_MODULE_${m}_DEPS_EXT "^visp_[^ ]+$")
+ if(VISP_MODULE_${m}_DEPS_EXT AND VISP_MODULE_${m}_DEPS)
+ list(REMOVE_ITEM VISP_MODULE_${m}_DEPS ${VISP_MODULE_${m}_DEPS_EXT})
+ endif()
+ endforeach()
+
+ # reorder dependencies
+ foreach(m ${VISP_MODULES_BUILD})
+ __vp_sort_modules_by_deps(VISP_MODULE_${m}_DEPS)
+ vp_list_sort(VISP_MODULE_${m}_DEPS_EXT)
+
+ set(LINK_DEPS ${VISP_MODULE_${m}_DEPS})
+
+ set(VISP_MODULE_${m}_DEPS ${VISP_MODULE_${m}_DEPS} CACHE INTERNAL "Flattened dependencies of ${m} module")
+ set(VISP_MODULE_${m}_DEPS_EXT ${VISP_MODULE_${m}_DEPS_EXT} CACHE INTERNAL "Extra dependencies of ${m} module")
+ set(VISP_MODULE_${m}_DEPS_TO_LINK ${LINK_DEPS} CACHE INTERNAL "Flattened dependencies of ${m} module (for linker)")
+
+# message(STATUS " module deps of ${m}: ${VISP_MODULE_${m}_DEPS}")
+# message(STATUS " module link deps of ${m}: ${VISP_MODULE_${m}_DEPS_TO_LINK}")
+# message(STATUS " extra deps of ${m}: ${VISP_MODULE_${m}_DEPS_EXT}")
+# message(STATUS "")
+ endforeach()
+
+ __vp_sort_modules_by_deps(VISP_MODULES_BUILD)
+
+ set(VISP_MODULES_PUBLIC ${VISP_MODULES_PUBLIC} CACHE INTERNAL "List of ViSP modules marked for export")
+ set(VISP_MODULES_BUILD ${VISP_MODULES_BUILD} CACHE INTERNAL "List of ViSP modules included into the build")
+ set(VISP_MODULES_DISABLED_AUTO ${VISP_MODULES_DISABLED_AUTO} CACHE INTERNAL "List of ViSP modules implicitly disabled due to dependencies")
+endfunction()
+
+# setup include paths for the list of passed modules
+macro(vp_target_include_modules target)
+ foreach(d ${ARGN})
+ if(d MATCHES "^visp_" AND HAVE_${d})
+ if (EXISTS "${VISP_MODULE_${d}_LOCATION}/include")
+ vp_target_include_directories(${target} "${VISP_MODULE_${d}_LOCATION}/include")
+ # Work arround to be able to build the modules without INTERFACE_INCLUDE_DIRECTORIES
+ # that was only introduces since CMake 2.8.12
+ if (CMAKE_VERSION VERSION_LESS 2.8.12)
+ vp_target_include_directories(${target} "${VISP_MODULE_${d}_INC_DEPS}")
+ endif()
+ endif()
+ elseif(EXISTS "${d}")
+ # FS keep external deps inc
+ set(VISP_MODULE_${the_module}_INC_DEPS "${VISP_MODULE_${the_module}_INC_DEPS};${d}" CACHE INTERNAL "")
+ vp_target_include_directories(${target} "${d}")
+ endif()
+ endforeach()
+ vp_list_unique(VISP_MODULE_${the_module}_INC_DEPS)
+endmacro()
+
+# setup include path for ViSP headers for specified module
+# vp_module_include_directories(<extra include directories/extra include modules>)
+macro(vp_module_include_directories)
+ vp_target_include_directories(${the_module}
+ "${VISP_MODULE_${the_module}_LOCATION}/include"
+ "${VISP_MODULE_${the_module}_LOCATION}/src"
+ )
+ vp_target_include_modules(${the_module} ${VISP_MODULE_${the_module}_DEPS} ${ARGN})
+endmacro()
+
+# sets header and source files for the current module
+# NB: all files specified as headers will be installed
+# Usage:
+# ocv_set_module_sources([HEADERS] <list of files> [SOURCES] <list of files>)
+macro(vp_set_module_sources)
+ vp_debug_message("vp_set_module_sources(" ${ARGN} ")")
+
+ set(VISP_MODULE_${the_module}_HEADERS "")
+ set(VISP_MODULE_${the_module}_SOURCES "")
+
+ foreach(f "HEADERS" ${ARGN})
+ if(f STREQUAL "HEADERS" OR f STREQUAL "SOURCES")
+ set(__filesvar "VISP_MODULE_${the_module}_${f}")
+ else()
+ list(APPEND ${__filesvar} "${f}")
+ endif()
+ endforeach()
+
+ # use full paths for module to be independent from the module location
+ vp_convert_to_full_paths(VISP_MODULE_${the_module}_HEADERS)
+
+ if(${the_module} MATCHES visp_core)
+ list(APPEND VISP_MODULE_${the_module}_HEADERS "${VISP_INCLUDE_DIR}/visp3/core/vpConfig.h")
+ list(APPEND VISP_MODULE_${the_module}_HEADERS "${VISP_INCLUDE_DIR}/visp3/visp_modules.h")
+ endif()
+
+
+ set(VISP_MODULE_${the_module}_HEADERS ${VISP_MODULE_${the_module}_HEADERS} CACHE INTERNAL "List of header files for ${the_module}")
+ set(VISP_MODULE_${the_module}_SOURCES ${VISP_MODULE_${the_module}_SOURCES} CACHE INTERNAL "List of source files for ${the_module}")
+endmacro()
+
+# finds and sets headers and sources for the standard ViSP module
+# Usage:
+# vp_glob_module_sources(<extra sources&headers in the same format as used in vp_set_module_sources>)
+macro(vp_glob_module_sources)
+ vp_debug_message("vp_glob_module_sources(" ${ARGN} ")")
+ set(_argn ${ARGN})
+
+ file(GLOB_RECURSE lib_srcs
+ "${CMAKE_CURRENT_LIST_DIR}/src/*.cpp"
+ )
+ file(GLOB_RECURSE lib_int_hdrs
+ "${CMAKE_CURRENT_LIST_DIR}/src/*.hpp"
+ "${CMAKE_CURRENT_LIST_DIR}/src/*.h"
+ )
+ file(GLOB lib_hdrs
+ "${CMAKE_CURRENT_LIST_DIR}/include/visp3/*.h"
+ "${CMAKE_CURRENT_LIST_DIR}/include/visp3/${name}/*.h"
+ )
+
+ vp_source_group("Src" DIRBASE "${CMAKE_CURRENT_LIST_DIR}/src" FILES ${lib_srcs} ${lib_int_hdrs})
+ vp_source_group("Include" DIRBASE "${CMAKE_CURRENT_LIST_DIR}/include" FILES ${lib_hdrs})
+
+ vp_set_module_sources(${_argn} HEADERS ${lib_hdrs}
+ SOURCES ${lib_srcs} ${lib_int_hdrs})
+endmacro()
+
+# finds and copy data from a source to a destination
+# Usage:
+# vp_glob_module_data(<source> <destination>)
+macro(vp_glob_module_copy_data src dst)
+ set(__data "")
+ file(GLOB_RECURSE __data
+ "${CMAKE_CURRENT_LIST_DIR}/${src}"
+ )
+
+ foreach(__d ${__data})
+ file(COPY ${__d}
+ DESTINATION "${VISP_BINARY_DIR}/${dst}"
+ FILE_PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_WRITE
+ )
+
+ # install
+ if(UNIX)
+ set(__install_dst "${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/${dst}")
+ else()
+ set(__install_dst "${dst}")
+ endif()
+
+ install(FILES ${__d}
+ DESTINATION "${__install_dst}"
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_WRITE
+ )
+ endforeach()
+endmacro()
+
+# creates old headers for compat with previous releases in include/visp
+# Usage:
+# vp_create_compat_headers(<list of new headers>)
+macro(vp_create_compat_headers)
+ vp_debug_message("vp_create_compat_headers(" ${ARGN} ")")
+
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "")
+
+ foreach(h ${ARGN})
+ get_filename_component(__h_name_we ${h} NAME_WE)
+ get_filename_component(__h_name ${h} NAME)
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "#ifndef __${__h_name_we}_h_\n#define __${__h_name_we}_h_\n\n#include <visp3/${name}/${__h_name}>\n\n#endif\n")
+ set(__compat_header_dst "${VISP_INCLUDE_DIR}/visp/${__h_name_we}.h")
+ configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpHeader.h.in" ${__compat_header_dst})
+ endforeach()
+
+ unset(__h_name_we)
+ unset(__h_name)
+ unset(__compat_header_dst)
+endmacro()
+
+# creates headers for modules include/visp3/<module>/vp<module>.h
+# Usage:
+# vp_create_global_module_header(<module>)
+macro(vp_create_global_module_header module)
+ vp_debug_message("vp_create_global_module_header(" ${module} ")")
+
+ set(__name ${module})
+ vp_short_module_name(__name)
+ set(__module_header_dst "${VISP_INCLUDE_DIR}/visp3/${__name}/${module}.h")
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "#ifndef __${module}_h_\n#define __${module}_h_\n")
+
+ # when core, include also vpConfig.h
+ if(__name MATCHES "core")
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "${VISP_HEADER_CONTENT_CONFIGMAKE}\n#include <visp3/${__name}/vpConfig.h>")
+ endif()
+
+ # include the modules we depend on
+ if(VISP_MODULE_${module}_REQ_DEPS)
+ foreach(dep ${VISP_MODULE_${module}_REQ_DEPS})
+ vp_short_module_name(dep)
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "${VISP_HEADER_CONTENT_CONFIGMAKE}\n#include <visp3/${dep}/vp${dep}.h>")
+ endforeach()
+ endif()
+
+ foreach(h ${VISP_MODULE_${module}_HEADERS})
+ get_filename_component(__h_name_we ${h} NAME_WE)
+ get_filename_component(__h_name ${h} NAME)
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "${VISP_HEADER_CONTENT_CONFIGMAKE}\n#include <visp3/${__name}/${__h_name}>")
+ endforeach()
+
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "${VISP_HEADER_CONTENT_CONFIGMAKE}\n\n#endif\n")
+ configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpHeader.h.in" ${__module_header_dst})
+
+ unset(__h_name_we)
+ unset(__h_name)
+ unset(__module_header_dst)
+endmacro()
+
+# creates ViSP module in current folder
+# creates new target, configures standard dependencies, compilers flags, install rules
+# Usage:
+# vp_create_module(<extra link dependencies>)
+# vp_create_module()
+macro(vp_create_module)
+ vp_debug_message("vp_create_module(" ${ARGN} ")")
+ set(VISP_MODULE_${the_module}_LINK_DEPS "${VISP_MODULE_${the_module}_LINK_DEPS};${ARGN}" CACHE INTERNAL "")
+ _vp_create_module(${ARGN})
+ set(the_module_target ${the_module})
+endmacro()
+
+macro(_vp_create_module)
+ vp_create_compat_headers(${VISP_MODULE_${the_module}_HEADERS})
+ vp_create_global_module_header(${the_module})
+
+ vp_add_library(${the_module} ${VISP_MODULE_TYPE} ${VISP_MODULE_${the_module}_HEADERS} ${VISP_MODULE_${the_module}_SOURCES})
+
+ vp_target_link_libraries(${the_module} ${VISP_MODULE_${the_module}_DEPS_TO_LINK})
+ #vp_target_link_libraries(${the_module} LINK_INTERFACE_LIBRARIES ${VISP_MODULE_${the_module}_DEPS_TO_LINK})
+ vp_target_link_libraries(${the_module} ${VISP_MODULE_${the_module}_DEPS_EXT} ${VISP_LINKER_LIBS} ${ARGN})
+
+ add_dependencies(visp_modules ${the_module})
+
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${the_module} PROPERTIES FOLDER "modules")
+ endif()
+
+ set_target_properties(${the_module} PROPERTIES
+ OUTPUT_NAME "${the_module}${VISP_DLLVERSION}"
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
+ LIBRARY_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
+ RUNTIME_OUTPUT_DIRECTORY ${BINARY_OUTPUT_PATH}
+ )
+
+ set_property(TARGET ${the_module} APPEND PROPERTY
+ INTERFACE_INCLUDE_DIRECTORIES ${VISP_MODULE_${the_module}_INC_DEPS}
+ )
+
+ # For dynamic link numbering convenions
+ if(NOT ANDROID)
+ # Android SDK build scripts can include only .so files into final .apk
+ # As result we should not set version properties for Android
+ set_target_properties(${the_module} PROPERTIES
+ VERSION ${VISP_VERSION}
+ SOVERSION ${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}
+ )
+ endif()
+
+ if((NOT DEFINED VISP_MODULE_TYPE AND BUILD_SHARED_LIBS)
+ OR (DEFINED VISP_MODULE_TYPE AND VISP_MODULE_TYPE STREQUAL SHARED))
+ set_target_properties(${the_module} PROPERTIES COMPILE_DEFINITIONS visp_EXPORTS)
+ set_target_properties(${the_module} PROPERTIES DEFINE_SYMBOL visp_EXPORTS)
+ endif()
+
+ if(MSVC)
+ if(CMAKE_CROSSCOMPILING)
+ set_target_properties(${the_module} PROPERTIES LINK_FLAGS "/NODEFAULTLIB:secchk")
+ endif()
+ set_target_properties(${the_module} PROPERTIES LINK_FLAGS "/NODEFAULTLIB:libc /DEBUG")
+ endif()
+
+ vp_install_target(${the_module} EXPORT VISPModules OPTIONAL
+ RUNTIME DESTINATION ${VISP_BIN_INSTALL_PATH} COMPONENT libs
+ LIBRARY DESTINATION ${VISP_LIB_INSTALL_PATH} COMPONENT libs
+ ARCHIVE DESTINATION ${VISP_LIB_INSTALL_PATH} COMPONENT dev
+ )
+
+ foreach(m ${VISP_MODULE_${the_module}_CHILDREN} ${the_module})
+ # only "public" headers need to be installed
+ if(VISP_MODULE_${m}_HEADERS AND ";${VISP_MODULES_PUBLIC};" MATCHES ";${m};")
+ foreach(hdr ${VISP_MODULE_${m}_HEADERS})
+ string(REGEX REPLACE "^.*visp3/" "visp3/" hdr2 "${hdr}")
+ if(NOT hdr2 MATCHES "visp3/${m}/private.*" AND hdr2 MATCHES "^(visp3/?.*)/[^/]+.h(..)?$" )
+ install(FILES ${hdr} OPTIONAL DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/${CMAKE_MATCH_1}" COMPONENT dev)
+ endif()
+ endforeach()
+ endif()
+ endforeach()
+
+endmacro()
+
+
+# short command for adding simple ViSP module
+# see vp_add_module for argument details
+# Usage:
+# vp_define_module(module_name [INTERNAL] [REQUIRED] [<list of dependencies>] [OPTIONAL <list of optional dependencies>])
+macro(vp_define_module module_name)
+ vp_debug_message("vp_define_module(" ${module_name} ${ARGN} ")")
+ set(_argn ${ARGN})
+
+ vp_add_module(${module_name} ${_argn})
+ vp_glob_module_sources()
+ vp_module_include_directories()
+ vp_create_module()
+endmacro()
+
+# ensures that all passed modules are available
+# sets VP_DEPENDENCIES_FOUND variable to TRUE/FALSE
+macro(vp_check_dependencies)
+ set(VP_DEPENDENCIES_FOUND TRUE)
+ foreach(d ${ARGN})
+ if(d MATCHES "^visp_[^ ]+$" AND NOT HAVE_${d})
+ set(VP_DEPENDENCIES_FOUND FALSE)
+ break()
+ endif()
+ endforeach()
+endmacro()
+
+# auxiliary macro to parse arguments of vp_add_tests commands
+macro(__vp_parse_test_sources tests_type)
+ set(VISP_${tests_type}_${the_module}_SOURCES "")
+ set(VISP_${tests_type}_${the_module}_SOURCES_EXCLUDE "")
+ set(VISP_${tests_type}_${the_module}_DEPS "")
+ set(VISP_${tests_type}_${the_module}_CTEST_EXCLUDE_FOLDER "")
+ set(__file_group_name "")
+ set(__file_group_sources "")
+ foreach(arg "DEPENDS_ON" ${ARGN} "FILES")
+ if(arg STREQUAL "FILES")
+ set(__currentvar "__file_group_sources")
+ if(__file_group_name AND __file_group_sources)
+ source_group("${__file_group_name}" FILES ${__file_group_sources})
+ list(APPEND VISP_${tests_type}_${the_module}_SOURCES ${__file_group_sources})
+ endif()
+ set(__file_group_name "")
+ set(__file_group_sources "")
+ elseif(arg STREQUAL "DEPENDS_ON")
+ set(__currentvar "VISP_${tests_type}_${the_module}_DEPS")
+ elseif(" ${__currentvar}" STREQUAL " __file_group_sources" AND NOT __file_group_name) # spaces to avoid CMP0054
+ set(__file_group_name "${arg}")
+ elseif(arg STREQUAL "CTEST_EXCLUDE_PATH")
+ set(__currentvar "VISP_${tests_type}_${the_module}_CTEST_EXCLUDE_FOLDER")
+ elseif(arg STREQUAL "SOURCES_EXCLUDE")
+ set(__currentvar "VISP_${tests_type}_${the_module}_SOURCES_EXCLUDE")
+ else()
+ list(APPEND ${__currentvar} "${arg}")
+ endif()
+ endforeach()
+ unset(__file_group_name)
+ unset(__file_group_sources)
+ unset(__currentvar)
+endmacro()
+
+# this is a command for adding ViSP tests to the module
+# vp_add_tests([FILES <source group name> <list of sources>]
+# [FILES_EXCLUDE <list of sources>]
+# [DEPENDS_ON] <list of extra dependencies>
+# [CTEST_EXCLUDE_FOLDER] <list of folder to exclude from ctest>)
+macro(vp_add_tests)
+ vp_debug_message("vp_add_tests(" ${ARGN} ")")
+
+ set(test_path "${CMAKE_CURRENT_LIST_DIR}/test")
+ if(BUILD_TESTS AND EXISTS "${test_path}")
+ __vp_parse_test_sources(TEST ${ARGN})
+
+ set(__exclude_ctest "")
+ foreach(__folder ${VISP_TEST_${the_module}_CTEST_EXCLUDE_FOLDER} )
+ file(GLOB_RECURSE __files "${CMAKE_CURRENT_LIST_DIR}/test/${__folder}/*.cpp")
+ list(APPEND __exclude_ctest ${__files})
+ endforeach()
+ set(__exclude_sources "")
+ foreach(__source ${VISP_TEST_${the_module}_SOURCES_EXCLUDE} )
+ file(GLOB __files "${CMAKE_CURRENT_LIST_DIR}/test/${__source}")
+ list(APPEND __exclude_sources ${__files})
+ endforeach()
+
+ set(test_deps ${the_module} ${VISP_MODULE_${the_module}_DEPS})
+
+ foreach(d ${VISP_TEST_${the_module}_DEPS})
+ list(APPEND test_deps ${d})
+ list(APPEND test_deps ${VISP_MODULE_${d}_DEPS})
+ # Work arround to be able to build the modules without INTERFACE_INCLUDE_DIRECTORIES
+ # that was only introduces since CMake 2.8.12
+ if(CMAKE_VERSION VERSION_LESS 2.8.12)
+ list(APPEND test_deps "${VISP_MODULE_${__m}_INC_DEPS}")
+ endif()
+ endforeach()
+
+ vp_check_dependencies(${test_deps})
+ if(VP_DEPENDENCIES_FOUND)
+ if(NOT VISP_TEST_${the_module}_SOURCES)
+ file(GLOB_RECURSE test_srcs "${test_path}/*.cpp")
+ vp_source_group("Src" DIRBASE "${test_path}" FILES ${test_srcs})
+ set(VISP_TEST_${the_module}_SOURCES ${test_srcs})
+ endif()
+
+ foreach(t ${VISP_TEST_${the_module}_SOURCES})
+ # check if source is not in exclude list
+ list(FIND __exclude_sources ${t} __to_exclude_from_sources)
+ if(${__to_exclude_from_sources} EQUAL -1)
+ # Compute the name of the binary to create
+ get_filename_component(the_target ${t} NAME_WE)
+ # From source compile the binary and add link rules
+ vp_add_executable(${the_target} ${t})
+ vp_target_include_modules(${the_target} ${test_deps})
+ vp_target_link_libraries(${the_target} ${test_deps} ${VISP_MODULE_${the_module}_DEPS} ${VISP_LINKER_LIBS})
+
+ # ctest only if not in the exclude list
+ list(FIND __exclude_ctest ${t} __to_exclude_from_ctest)
+ if(${__to_exclude_from_ctest} EQUAL -1)
+ add_test(${the_target} ${the_target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ endif()
+ # TODO FS add visp_test_${name} target to group all the tests
+ add_dependencies(visp_tests ${the_target})
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${the_target} PROPERTIES FOLDER "tests")
+ endif()
+ endif()
+ endforeach()
+
+ else(VP_DEPENDENCIES_FOUND)
+ # TODO: warn about unsatisfied dependencies
+ endif(VP_DEPENDENCIES_FOUND)
+
+ endif()
+endmacro()
+
+# setup include paths for the list of passed modules
+macro(vp_include_modules)
+ foreach(d ${ARGN})
+ if(d MATCHES "^visp_" AND HAVE_${d})
+ if (EXISTS "${VISP_MODULE_${d}_LOCATION}/include")
+ vp_include_directories("${VISP_MODULE_${d}_LOCATION}/include")
+ endif()
+ elseif(EXISTS "${d}")
+ vp_include_directories("${d}")
+ endif()
+ endforeach()
+endmacro()
+
+# same as previous but with dependencies
+macro(vp_include_modules_recurse)
+ vp_include_modules(${ARGN})
+ foreach(d ${ARGN})
+ if(d MATCHES "^visp_" AND HAVE_${d} AND DEFINED VISP_MODULE_${d}_DEPS)
+ foreach (sub ${VISP_MODULE_${d}_DEPS})
+ vp_include_modules(${sub})
+ endforeach()
+ endif()
+ endforeach()
+endmacro()
+
+# This is a command to configure files as include headers of the corresponding module.
+# vp_add_config_file(<list of header config files>)
+#
+# If the input config filename is suffixed by .in or .cmake the suffix is removed
+# in the configured file.
+#
+# Example:
+# vp_add_config_file(cmake/template/vpConfigModule.h.in)
+# creates include/visp3/module_name/vpConfigModule.h
+macro(vp_add_config_file)
+ foreach(d ${ARGN})
+ # Removes first "/" if it exists
+ string(FIND ${d} "/" FIRST_SEPARATOR_POS)
+ if(${FIRST_SEPARATOR_POS} EQUAL 0)
+ string(SUBSTRING ${d} 1 -1 d)
+ endif()
+
+ # Find start of file name
+ string(FIND ${d} "/" LAST_SEPARATOR_POS REVERSE)
+ if(${LAST_SEPARATOR_POS} EQUAL -1)
+ set(START 0)
+ else()
+ math(EXPR START "${LAST_SEPARATOR_POS}+1")
+ endif()
+
+ # Save entire path
+ set(FILENAME_CONFIG ${d})
+
+ # Find file name
+ string(FIND ${d} "." EXTENSION_POS REVERSE)
+
+ if(${EXTENSION_POS} EQUAL -1)
+ string(SUBSTRING ${d} ${START} -1 FILENAME_CONFIG_SHORT)
+ else()
+ string(SUBSTRING ${d} ${EXTENSION_POS} -1 EXT_CONFIG_FILE)
+ if(EXT_CONFIG_FILE MATCHES ".cmake" OR EXT_CONFIG_FILE MATCHES ".in")
+ math(EXPR LENGTH "${EXTENSION_POS} - ${START}")
+ string(SUBSTRING ${d} ${START} ${LENGTH} FILENAME_CONFIG_SHORT)
+ else()
+ string(SUBSTRING ${d} ${START} -1 FILENAME_CONFIG_SHORT)
+ endif()
+ endif()
+
+ set(MODULE_NAME ${the_module})
+ if(MODULE_NAME MATCHES "^visp_")
+ string(REGEX REPLACE "^visp_" "" MODULE_NAME "${MODULE_NAME}")
+ endif()
+
+ configure_file("${VISP_MODULE_${the_module}_LOCATION}/${FILENAME_CONFIG}" "${VISP_INCLUDE_DIR}/visp3/${MODULE_NAME}/${FILENAME_CONFIG_SHORT}")
+
+ vp_create_compat_headers("${VISP_INCLUDE_DIR}/visp3/${MODULE_NAME}/${FILENAME_CONFIG_SHORT}")
+
+ install(FILES "${VISP_INCLUDE_DIR}/visp3/${MODULE_NAME}/${FILENAME_CONFIG_SHORT}"
+ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/visp3/${MODULE_NAME}
+ COMPONENT dev
+ )
+
+ endforeach()
+endmacro()
+
+# This is a command to add a list of paths associated to the corresponding module
+# to the CMAKE_MODULE_PATH global var to find specific cmake material
+# vp_add_cmake_module_path(<list of cmake module paths>)
+# Example:
+# vp_add_cmake_module_path(cmake)
+# Appends the cmake full path to CMAKE_MODULE_PATH var.
+macro(vp_add_cmake_module_path)
+ foreach(d ${ARGN})
+ # Removes first "/" if it exists
+ string(FIND ${d} "/" FIRST_SEPARATOR_POS)
+ if(${FIRST_SEPARATOR_POS} EQUAL 0)
+ string(SUBSTRING ${d} 1 -1 d)
+ endif()
+ if(EXISTS "${VISP_MODULE_${the_module}_LOCATION}/${d}")
+ list(APPEND CMAKE_MODULE_PATH "${VISP_MODULE_${the_module}_LOCATION}/${d}")
+ endif()
+ endforeach()
+endmacro()
diff --git a/cmake/VISPUse.cmake.in b/cmake/VISPUse.cmake.in
new file mode 100644
index 0000000..016dae2
--- /dev/null
+++ b/cmake/VISPUse.cmake.in
@@ -0,0 +1,46 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# cmake PackageConfig file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+# Tell the compiler where to find ViSP's header files
+# and the third party headers we depend on
+include_directories(${VISP_INCLUDE_DIRS})
+
+# Tell the compiler where to find ViSP's libraries
+# and the third party libraries we depend on
+link_libraries(${VISP_LIBRARIES})
+
diff --git a/cmake/VISPUtils.cmake b/cmake/VISPUtils.cmake
new file mode 100644
index 0000000..eb817aa
--- /dev/null
+++ b/cmake/VISPUtils.cmake
@@ -0,0 +1,457 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# adds include directories in such way that directories from the ViSP source tree go first
+function(vp_include_directories)
+ vp_debug_message("vp_include_directories( ${ARGN} )")
+ set(__add_before "")
+ foreach(dir ${ARGN})
+ get_filename_component(__abs_dir "${dir}" ABSOLUTE)
+ string(REPLACE "+" "\\+" __VISP_BINARY_DIR_filtered ${VISP_BINARY_DIR})
+# if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}")
+ if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${__VISP_BINARY_DIR_filtered}")
+ list(APPEND __add_before "${dir}")
+ else()
+ include_directories(AFTER SYSTEM "${dir}")
+ endif()
+ endforeach()
+ include_directories(BEFORE ${__add_before})
+endfunction()
+
+# adds include directories in such way that directories from the ViSP source tree go first
+function(vp_target_include_directories target)
+ set(__params "")
+ foreach(dir ${ARGN})
+ get_filename_component(__abs_dir "${dir}" ABSOLUTE)
+ string(REPLACE "+" "\\+" __VISP_BINARY_DIR_filtered ${VISP_BINARY_DIR})
+# if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}")
+ if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${__VISP_BINARY_DIR_filtered}")
+ list(APPEND __params "${__abs_dir}")
+ else()
+ list(APPEND __params "${dir}")
+ endif()
+ endforeach()
+ if(__params)
+ if(CMAKE_VERSION VERSION_LESS 2.8.11)
+ include_directories(${__params})
+ else()
+ if(TARGET ${target})
+ target_include_directories(${target} PRIVATE ${__params})
+ else()
+ set(__new_inc "${VP_TARGET_INCLUDE_DIRS_${target}};${__params}")
+ set(VP_TARGET_INCLUDE_DIRS_${target} "${__new_inc}" CACHE INTERNAL "")
+ endif()
+ endif()
+ endif()
+endfunction()
+
+# clears all passed variables
+macro(vp_clear_vars)
+ foreach(_var ${ARGN})
+ unset(${_var} CACHE)
+ endforeach()
+endmacro()
+
+# print message
+macro(vp_debug_message)
+ #string(REPLACE ";" " " __msg "${ARGN}")
+ #message(STATUS "${__msg}")
+endmacro()
+
+# remove all matching elements from the list
+macro(vp_list_filterout lst regex)
+ foreach(item ${${lst}})
+ if(item MATCHES "${regex}")
+ list(REMOVE_ITEM ${lst} "${item}")
+ endif()
+ endforeach()
+endmacro()
+
+# stable & safe duplicates removal macro
+macro(vp_list_unique __lst)
+ if(${__lst})
+ list(REMOVE_DUPLICATES ${__lst})
+ endif()
+endmacro()
+
+# safe list sorting macro
+macro(vp_list_sort __lst)
+ if(${__lst})
+ list(SORT ${__lst})
+ endif()
+endmacro()
+
+# gets and removes the first element from list
+macro(vp_list_pop_front LST VAR)
+ if(${LST})
+ list(GET ${LST} 0 ${VAR})
+ list(REMOVE_AT ${LST} 0)
+ else()
+ set(${VAR} "")
+ endif()
+endmacro()
+
+# remove cmake ; list separator
+macro(vp_list_remove_separator __lst)
+ if(${__lst})
+ set(__lst_reformated "")
+ foreach(element ${${__lst}})
+ set(__lst_reformated "${__lst_reformated} ${element}")
+ endforeach()
+ set(${__lst} ${__lst_reformated})
+ endif()
+endmacro()
+
+# Provides an option that the user can optionally select.
+# Can accept condition to control when option is available for user.
+# Usage:
+# vp_option(<option_variable>
+# <package to find to activate the option>
+# <QUIET or "">
+# "help string describing the option"
+# "advanced list of vars separated by ;"
+# <initial value or boolean expression> [IF <condition>])
+macro(VP_OPTION variable package quiet description advanced value)
+ set(__option TRUE)
+ set(__value ${value})
+ set(__condition "")
+ set(__varname "__value")
+ set(__p ${package})
+
+ # get the first package considered as the main package from a list: ie "Zlib;MyZlib"
+ set(__first_package "")
+ foreach(p ${package})
+ if(${p} MATCHES "^My")
+ string(REGEX REPLACE "^My" "" p "${p}")
+ endif()
+ set(__first_package ${p})
+ break()
+ endforeach()
+
+ if(NOT ${__first_package} STREQUAL "")
+ # get the first package name from the list
+ #list(GET ${package} 0 ${__package})
+ string(TOLOWER "${__first_package}" __package_lower)
+ string(TOUPPER "${__first_package}" __package_upper) # useful for Qt -> QT_FOUND
+ endif()
+
+ # set VISP_HAVE_<package>_FOUND="no"
+ set(__alias_have_found_str VISP_HAVE_${__package_upper}_FOUND)
+ set(${__alias_have_found_str} "no")
+
+ foreach(arg ${ARGN})
+ if(arg STREQUAL "IF" OR arg STREQUAL "if")
+ set(__varname "__condition")
+ else()
+ list(APPEND ${__varname} ${arg})
+ endif()
+ endforeach()
+ unset(__varname)
+ if(__condition STREQUAL "")
+ set(__condition 2 GREATER 1)
+ endif()
+ if(${__condition})
+
+ if(NOT ${__first_package} STREQUAL "")
+ foreach(p ${package})
+ if("${quiet}" STREQUAL "")
+ find_package(${p})
+ else()
+ find_package(${p} ${quiet})
+ endif()
+ if(${__package_upper}_FOUND OR ${__first_package}_FOUND)
+ set(__option TRUE)
+ break()
+ else()
+ set(__option FALSE)
+ endif()
+ endforeach()
+ endif()
+ if(${__option})
+ if(__value MATCHES ";")
+ if(${__value})
+ option(${variable} "${description}" ON)
+ else()
+ option(${variable} "${description}" OFF)
+ endif()
+ elseif(DEFINED ${__value})
+ if(${__value})
+ option(${variable} "${description}" ON)
+ else()
+ option(${variable} "${description}" OFF)
+ endif()
+ else()
+ option(${variable} "${description}" ${__value})
+ endif()
+ else()
+ unset(${variable} CACHE)
+ endif()
+ unset(__condition)
+ unset(__value)
+
+ else()
+ set(${variable} OFF)
+ endif()
+ foreach(a ${advanced})
+ mark_as_advanced(${a})
+ endforeach()
+ if(${variable} AND NOT ${__first_package} STREQUAL "")
+ # set VISP_HAVE_<package>=TRUE and VISP_HAVE_<package>_FOUND="yes"
+ message(STATUS "${__package_lower} found")
+ set(${__alias_have_found_str} "yes") # for ViSP-third-party.txt
+ endif()
+ unset(__option)
+ unset(__alias_have)
+ unset(__alias_have_found_str)
+endmacro()
+
+# Provides a macro to set a var.
+# Can accept condition to set var.
+# Usage:
+# vp_set(<option_variable>
+# <initial value or boolean expression> [IF <condition>])
+macro(VP_SET variable value)
+ set(__value ${value})
+ set(__condition "")
+ set(__varname "__value")
+
+ foreach(arg ${ARGN})
+ if(arg STREQUAL "IF" OR arg STREQUAL "if")
+ set(__varname "__condition")
+ else()
+ list(APPEND ${__varname} ${arg})
+ endif()
+ endforeach()
+ unset(__varname)
+ if(__condition STREQUAL "")
+ set(__condition 2 GREATER 1)
+ endif()
+ if(${__condition})
+ if(__value MATCHES ";")
+ if(${__value})
+ set(${variable} TRUE)
+ else()
+ set(${variable} FALSE)
+ endif()
+ elseif(DEFINED ${__value})
+ if(${__value})
+ set(${variable} TRUE)
+ else()
+ set(${variable} FALSE)
+ endif()
+ else()
+ set(${variable} ${__value})
+ endif()
+ endif()
+ unset(__condition)
+ unset(__value)
+endmacro()
+
+# short command to setup source group
+function(vp_source_group group)
+ cmake_parse_arguments(SG "" "DIRBASE" "GLOB;GLOB_RECURSE;FILES" ${ARGN})
+ set(files "")
+ if(SG_FILES)
+ list(APPEND files ${SG_FILES})
+ endif()
+ if(SG_GLOB)
+ file(GLOB srcs ${SG_GLOB})
+ list(APPEND files ${srcs})
+ endif()
+ if(SG_GLOB_RECURSE)
+ file(GLOB_RECURSE srcs ${SG_GLOB_RECURSE})
+ list(APPEND files ${srcs})
+ endif()
+ if(SG_DIRBASE)
+ foreach(f ${files})
+ file(RELATIVE_PATH fpart "${SG_DIRBASE}" "${f}")
+ if(fpart MATCHES "^\\.\\.")
+ message(AUTHOR_WARNING "Can't detect subpath for source_group command: Group=${group} FILE=${f} DIRBASE=${SG_DIRBASE}")
+ set(fpart "")
+ else()
+ get_filename_component(fpart "${fpart}" PATH)
+ if(fpart)
+ set(fpart "/${fpart}") # add '/'
+ string(REPLACE "/" "\\" fpart "${fpart}")
+ endif()
+ endif()
+ source_group("${group}${fpart}" FILES ${f})
+ endforeach()
+ else()
+ source_group(${group} FILES ${files})
+ endif()
+endfunction()
+
+# convert list of paths to full paths
+macro(vp_convert_to_full_paths VAR)
+ if(${VAR})
+ set(__tmp "")
+ foreach(path ${${VAR}})
+ get_filename_component(${VAR} "${path}" ABSOLUTE)
+ list(APPEND __tmp "${${VAR}}")
+ endforeach()
+ set(${VAR} ${__tmp})
+ unset(__tmp)
+ endif()
+endmacro()
+
+# add install command
+function(vp_install_target)
+ install(TARGETS ${ARGN})
+
+ set(isPackage 0)
+ unset(__package)
+ unset(__target)
+ foreach(e ${ARGN})
+ if(NOT DEFINED __target)
+ set(__target "${e}")
+ endif()
+ if(isPackage EQUAL 1)
+ set(__package "${e}")
+ break()
+ endif()
+ if(e STREQUAL "EXPORT")
+ set(isPackage 1)
+ endif()
+ endforeach()
+
+ if(DEFINED __package)
+ list(APPEND ${__package}_TARGETS ${__target})
+ set(${__package}_TARGETS "${${__package}_TARGETS}" CACHE INTERNAL "List of ${__package} targets")
+ endif()
+
+ if(INSTALL_CREATE_DISTRIB)
+ if(MSVC AND NOT BUILD_SHARED_LIBS)
+ set(__target "${ARGV0}")
+
+ set(isArchive 0)
+ set(isDst 0)
+ unset(__dst)
+ foreach(e ${ARGN})
+ if(isDst EQUAL 1)
+ set(__dst "${e}")
+ break()
+ endif()
+ if(isArchive EQUAL 1 AND e STREQUAL "DESTINATION")
+ set(isDst 1)
+ endif()
+ if(e STREQUAL "ARCHIVE")
+ set(isArchive 1)
+ else()
+ set(isArchive 0)
+ endif()
+ endforeach()
+
+# message(STATUS "Process ${__target} dst=${__dst}...")
+ if(DEFINED __dst)
+ if(CMAKE_VERSION VERSION_LESS 2.8.12)
+ get_target_property(fname ${__target} LOCATION_DEBUG)
+ if(fname MATCHES "\\.lib$")
+ string(REGEX REPLACE "\\.lib$" ".pdb" fname "${fname}")
+ install(FILES ${fname} DESTINATION ${__dst} CONFIGURATIONS Debug)
+ endif()
+
+ get_target_property(fname ${__target} LOCATION_RELEASE)
+ if(fname MATCHES "\\.lib$")
+ string(REGEX REPLACE "\\.lib$" ".pdb" fname "${fname}")
+ install(FILES ${fname} DESTINATION ${__dst} CONFIGURATIONS Release)
+ endif()
+ else()
+ # CMake 2.8.12 brokes PDB support in STATIC libraries for MSVS
+ endif()
+ endif()
+ endif()
+ endif()
+endfunction()
+
+function(vp_target_link_libraries target)
+ set(LINK_DEPS ${ARGN})
+ target_link_libraries(${target} ${LINK_DEPS})
+endfunction()
+
+function(_vp_append_target_includes target)
+ if(DEFINED VP_TARGET_INCLUDE_DIRS_${target})
+ target_include_directories(${target} PRIVATE ${VP_TARGET_INCLUDE_DIRS_${target}})
+ unset(VP_TARGET_INCLUDE_DIRS_${target} CACHE)
+ endif()
+endfunction()
+
+function(vp_add_executable target)
+ add_executable(${target} ${ARGN})
+ _vp_append_target_includes(${target})
+endfunction()
+
+function(vp_add_library target)
+ add_library(${target} ${ARGN})
+
+ _vp_append_target_includes(${target})
+endfunction()
+
+# Macros that checks if package have been installed.
+# After it set vars:
+# <package>_FOUND
+# <package>_INCLUDE_DIRS
+# <package>_LIBRARIES
+# VISP_HAVE_<package>
+macro(VP_CHECK_PACKAGE package)
+ set(ALIAS ${package})
+ string(TOUPPER "${ALIAS}" ALIAS_UPPER) # useful for OpenGL
+ set(ALIAS_FOUND ${ALIAS}_FOUND)
+ set(ALIAS_UPPER_FOUND ${ALIAS_UPPER}_FOUND)
+ set(ALIAS_INCLUDE_DIRS ${ALIAS}_INCLUDE_DIRS)
+ set(ALIAS_LIBRARIES ${ALIAS}_LIBRARIES)
+ set(ALIAS_VISP_HAVE VISP_HAVE_${ALIAS})
+ set(ALIAS_UPPER_VISP_HAVE VISP_HAVE_${ALIAS_UPPER})
+
+ find_package(${ALIAS})
+
+ if(${ALIAS_FOUND} OR ${ALIAS_UPPER_FOUND})
+ set(${ALIAS_VISP_HAVE} 1)
+ set(${ALIAS_UPPER_VISP_HAVE} 1)
+ endif()
+endmacro()
+
+# Macro the get the list of subdirs from the path
+# var: returned variable name
+# path: path from witch relative subdirs are
+macro(vp_get_relative_subdirs var path)
+ set(ALIAS ${var})
+ file(GLOB_RECURSE rel_path_lst_ RELATIVE ${path} ${path}/*)
+ set(${ALIAS} "")
+ foreach(f ${rel_path_lst_})
+ get_filename_component(d ${f} PATH)
+ list(APPEND ${ALIAS} ${d})
+ endforeach()
+ list(REMOVE_DUPLICATES ${ALIAS})
+endmacro()
diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in
new file mode 100644
index 0000000..224df4e
--- /dev/null
+++ b/cmake/templates/VISPConfig.cmake.in
@@ -0,0 +1,320 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# CMake package config file for ViSP.
+#
+# ** File generated automatically, do not modify **
+#
+# This file will define the following CMake variables:
+# - VISP_LIBS : The list of all imported targets for VISP modules.
+# - VISP_INCLUDE_DIRS : ViSP and third-party include directories
+# - VISP_LIBRARIES : ViSP library to link against. Third-party libraries are
+# linked automatically thanks to cmake export file VISPModules.cmake
+# - VISP_VERSION_STRING : Full ViSP version that is build, "@VISP_VERSION@"
+# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION: "@VISP_VERSION_MAJOR@"
+# - VISP_VERSION_MINOR : Minor version part of VISP_VERSION: "@VISP_VERSION_MINOR@"
+# - VISP_VERSION_PATCH : Patch version part of VISP_VERSION: "@VISP_VERSION_PATCH@"
+#
+# Advanced variables:
+# - VISP_SHARED : Use ViSP as shared library
+# - VISP_CONFIG_PATH : Path to this VISPConfig.cmake
+# - VISP_USE_FILE : File to include to use ViSP without specific cmake code
+# - VISP_LIB_COMPONENTS: Present VISP modules list
+#
+# Typical usage in user project:
+#
+# find_package(VISP REQUIRED)
+# include_directories(${VISP_INCLUDE_DIRS})
+# target_link_libraries(MY_TARGET_NAME ${VISP_LIBRARIES})
+#
+# It is also possible to build your project using VISP_USE_FILE.
+#
+# find_package(VISP)
+# if(VISP_FOUND)
+# include(${VISP_USE_FILE})
+# endif()
+#
+# Or you can search for specific OpenCV modules:
+#
+# find_package(VISP REQUIRED core videoio)
+#
+# If the module is found then VISP_<MODULE>_FOUND is set to TRUE.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+#if(VISP_FOUND)
+# return()
+#endif()
+
+#set(VISP_FOUND TRUE)
+
+# Set the version numbers
+set(VISP_VERSION_STRING "@VISP_VERSION@")
+set(VISP_VERSION_MAJOR "@VISP_VERSION_MAJOR@")
+set(VISP_VERSION_MINOR "@VISP_VERSION_MINOR@")
+set(VISP_VERSION_PATCH "@VISP_VERSION_PATCH@")
+
+set(VISP_LIB_COMPONENTS @VISP_MODULES_CONFIGCMAKE@)
+
+# Form list of modules (components) to find
+if(NOT VISP_FIND_COMPONENTS)
+ set(VISP_FIND_COMPONENTS ${VISP_LIB_COMPONENTS})
+endif()
+
+# expand short module names and see if requested components exist
+set(VISP_FIND_COMPONENTS_ "")
+foreach(__vpcomponent ${VISP_FIND_COMPONENTS})
+ if(NOT __vpcomponent MATCHES "^visp_")
+ set(__vpcomponent visp_${__vpcomponent})
+ endif()
+ list(FIND VISP_LIB_COMPONENTS ${__vpcomponent} __vpcomponentIdx)
+ if(__vpcomponentIdx LESS 0)
+ #requested component is not found...
+ if(VISP_FIND_REQUIRED)
+ message(FATAL_ERROR "${__vpcomponent} is required but was not found")
+ elseif(NOT VISP_FIND_QUIETLY)
+ message(WARNING "${__vpcomponent} is required but was not found")
+ endif()
+ #indicate that module is NOT found
+ string(TOUPPER "${__vpcomponent}" __vpcomponentUP)
+ set(${__vpcomponentUP}_FOUND "${__vpcomponentUP}_FOUND-NOTFOUND")
+ else()
+ list(APPEND VISP_FIND_COMPONENTS_ ${__vpcomponent})
+ # Not using list(APPEND) here, because VISP_LIBS may not exist yet.
+ # Also not clearing VISP_LIBS anywhere, so that multiple calls
+ # to find_package(VISP) with different component lists add up.
+ set(VISP_LIBS ${VISP_LIBS} "${__vpcomponent}")
+ #indicate that module is found
+ string(TOUPPER "${__vpcomponent}" __vpcomponentUP)
+ set(${__vpcomponentUP}_FOUND 1)
+ endif()
+endforeach()
+set(VISP_FIND_COMPONENTS ${VISP_FIND_COMPONENTS_})
+#message("VISP_FIND_COMPONENTS: ${VISP_FIND_COMPONENTS}")
+
+# Resolve dependencies
+foreach(__opttype OPT DBG)
+ set(VISP_LIBS_${__opttype} "${VISP_LIBS}")
+endforeach()
+
+# Some additional settings are required if ViSP is built as static libs
+set(VISP_SHARED @BUILD_SHARED_LIBS@)
+
+# Extract the directory where *this* file has been installed (determined at cmake run-time)
+set(VISP_CONFIG_PATH ${VISP_DIR} CACHE PATH "ViSPConfig.cmake path location" FORCE)
+mark_as_advanced(VISP_CONFIG_PATH)
+
+if(NOT WIN32)
+ # Since this file is installed in ./${VISP_INSTALL_LIBDIR}/cmake/visp
+ # the first "../../" are here to go to ${VISP_INSTALL_LIBDIR}, and then
+ # VISP_INSTALL_LIBDIR_TO_PARENT goes to the parent dir "."
+ set(VISP_INSTALL_PATH "${VISP_CONFIG_PATH}/../../@VISP_INSTALL_LIBDIR_TO_PARENT@")
+ # Get the absolute path with no ../.. relative marks, to eliminate implicit linker warnings
+ if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_LESS 2.8)
+ get_filename_component(VISP_INSTALL_PATH "${VISP_INSTALL_PATH}" ABSOLUTE)
+ else()
+ get_filename_component(VISP_INSTALL_PATH "${VISP_INSTALL_PATH}" REALPATH)
+ endif()
+endif()
+
+# Tells the user project where to find ViSP headers
+set(VISP_INCLUDE_DIRS "@VISP_INCLUDE_DIRS_CONFIGCMAKE@")
+
+# Tells the user project ViSP library name
+set(VISP_LIBRARIES ${VISP_LIBS})
+
+# need to be improved
+if(POLICY CMP0024)
+ # Fix to prevent multiple includes
+ if(NOT TARGET visp_core)
+ cmake_policy(PUSH)
+ cmake_policy(SET CMP0024 OLD)
+ # Our library dependencies (contains definitions for IMPORTED targets)
+ include("${CMAKE_CURRENT_LIST_DIR}/VISPModules.cmake")
+ cmake_policy(POP)
+ endif()
+else()
+ # Fix for cmake 2.8.7 to prevent multiple includes
+ if(NOT TARGET visp_core)
+ # Our library dependencies (contains definitions for IMPORTED targets)
+ include("${CMAKE_CURRENT_LIST_DIR}/VISPModules.cmake")
+ endif()
+endif()
+
+# where to find the USE file to be used by user project
+set(VISP_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/VISPUse.cmake")
+
+if(BUILD_TEST_COVERAGE)
+ # Add build options for test coverage. Currently coverage is only supported
+ # on gcc compiler
+ # Because using -fprofile-arcs with shared lib can cause problems like:
+ # hidden symbol `__bb_init_func', we add this option only for static
+ # library build
+ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ftest-coverage -fprofile-arcs")
+endif()
+
+#----------------------------------------------------------------------
+# Remember VISP' third party libs configuration:
+#----------------------------------------------------------------------
+SET(VISP_HAVE_X11 "@VISP_HAVE_X11@")
+SET(VISP_HAVE_PTHREAD "@VISP_HAVE_PTHREAD@")
+SET(VISP_HAVE_GTK "@VISP_HAVE_GTK@")
+SET(VISP_HAVE_GDI "@VISP_HAVE_GDI@")
+SET(VISP_HAVE_D3D9 "@VISP_HAVE_D3D9@")
+SET(VISP_HAVE_DISPLAY "@VISP_HAVE_DISPLAY@")
+SET(VISP_HAVE_GSL "@VISP_HAVE_GSL@")
+SET(VISP_HAVE_LAPACK_C "@VISP_HAVE_LAPACK_C@")
+SET(VISP_HAVE_LIBFREENECT "@VISP_HAVE_LIBFREENECT@")
+SET(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES "@VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES@")
+SET(VISP_HAVE_LIBFREENECT_OLD "@VISP_HAVE_LIBFREENECT_OLD@")
+SET(VISP_HAVE_LIBUSB_1 "@VISP_HAVE_LIBUSB_1@")
+SET(VISP_HAVE_OPENCV "@VISP_HAVE_OPENCV@")
+SET(VISP_HAVE_OPENCV_NONFREE "@VISP_HAVE_OPENCV_NONFREE@")
+SET(VISP_HAVE_OPENCV_VERSION "@VISP_HAVE_OPENCV_VERSION@")
+SET(VISP_HAVE_OGRE "@VISP_HAVE_OGRE@")
+SET(VISP_HAVE_OPENGL "@VISP_HAVE_OPENGL@")
+SET(VISP_HAVE_OIS "@VISP_HAVE_OIS@")
+SET(VISP_HAVE_COIN3D_AND_GUI "@VISP_HAVE_COIN3D_AND_GUI@")
+SET(VISP_HAVE_COIN3D "@VISP_HAVE_COIN3D@")
+SET(VISP_HAVE_QT "@VISP_HAVE_QT@")
+SET(VISP_HAVE_SOQT "@VISP_HAVE_SOQT@")
+SET(VISP_HAVE_SOWIN "@VISP_HAVE_SOWIN@")
+SET(VISP_HAVE_SOXT "@VISP_HAVE_SOXT@")
+SET(VISP_HAVE_DC1394 "@VISP_HAVE_DC1394@")
+SET(VISP_HAVE_CMU1394 "@VISP_HAVE_CMU1394@")
+SET(VISP_HAVE_V4L2 "@VISP_HAVE_V4L2@")
+SET(VISP_HAVE_DIRECTSHOW "@VISP_HAVE_DIRECTSHOW@")
+SET(VISP_HAVE_AFMA4 "@VISP_HAVE_AFMA4@")
+SET(VISP_HAVE_AFMA6 "@VISP_HAVE_AFMA6@")
+SET(VISP_HAVE_BICLOPS "@VISP_HAVE_BICLOPS@")
+SET(VISP_HAVE_PTU46 "@VISP_HAVE_PTU46@")
+SET(VISP_HAVE_VIPER650 "@VISP_HAVE_VIPER650@")
+SET(VISP_HAVE_VIPER850 "@VISP_HAVE_VIPER850@")
+SET(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@")
+SET(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@")
+SET(VISP_HAVE_XML2 "@VISP_HAVE_XML2@")
+SET(VISP_HAVE_JPEG "@VISP_HAVE_JPEG@")
+SET(VISP_HAVE_PNG "@VISP_HAVE_PNG@")
+SET(VISP_HAVE_FFMPEG "@VISP_HAVE_FFMPEG@")
+SET(VISP_HAVE_YARP "@VISP_HAVE_YARP@")
+SET(VISP_HAVE_OPENMP "@VISP_HAVE_OPENMP@")
+SET(VISP_HAVE_ACCESS_TO_NAS "@VISP_HAVE_ACCESS_TO_NAS@")
+SET(VISP_HAVE_CPP11_COMPATIBILITY "@VISP_HAVE_CPP11_COMPATIBILITY@")
+
+#----------------------------------------------------------------------
+# Some useful macro to be able to build the tutorials along side ViSP
+#----------------------------------------------------------------------
+# Create a target from the *.cpp file, link against ViSP libraries and add a dependency to ViSP library
+# tu ensure that the library is build before this target.
+macro(visp_add_target file_cpp)
+ get_filename_component(target ${file_cpp} NAME_WE)
+ include_directories(${VISP_INCLUDE_DIRS})
+ add_executable(${target} ${file_cpp})
+ target_link_libraries(${target} ${VISP_LIBRARIES})
+endmacro()
+
+# Create a dependency to the target extracted from the *.cpp file and put the target in the solution dependency folder.
+macro(visp_add_dependency file_cpp dependency)
+ get_filename_component(target ${file_cpp} NAME_WE)
+ if(TARGET visp_${dependency})
+ add_dependencies(visp_${dependency} ${target})
+ if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${target} PROPERTIES FOLDER "${dependency}")
+ endif()
+ endif()
+endmacro()
+
+# Copy the data files to the same location than the target associated to the cpp files
+# Since CMake 3.0.0 policy CMP0026 was introduced to disallow location property on target.
+# If CMake 3.0.0 is used, we use $<TARGET_FILE_DIR:tgt> to get the target location
+if (CMAKE_VERSION VERSION_GREATER 2.8.12)
+ macro(visp_copy_data file_cpp file_data)
+ get_filename_component(target ${file_cpp} NAME_WE)
+ #get_target_property(target_location ${target} LOCATION)
+ get_filename_component(target_location "${target_location}" PATH)
+ add_custom_command(
+ TARGET ${target}
+ POST_BUILD
+ #COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "${target_location}"
+ COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "$<TARGET_FILE_DIR:${target}>"
+ )
+ endmacro()
+else()
+ macro(visp_copy_data file_cpp file_data)
+ get_filename_component(target ${file_cpp} NAME_WE)
+ get_target_property(target_location ${target} LOCATION)
+ get_filename_component(target_location "${target_location}" PATH)
+ add_custom_command(
+ TARGET ${target}
+ POST_BUILD
+ COMMAND ${CMAKE_COMMAND} -E copy "${file_data}" "${target_location}"
+ )
+ endmacro()
+endif()
+
+# Check dependencies for examples
+macro(visp_check_dependencies)
+ set(VP_DEPENDENCIES_FOUND TRUE)
+ foreach(d ${ARGN})
+ if(NOT TARGET ${d})
+ #message(WARNING "ViSP: Can't resolve dependency: ${d}")
+ set(VP_DEPENDENCIES_FOUND FALSE)
+ break()
+ endif()
+ endforeach()
+endmacro()
+
+# add subdirectory if dependencies are found
+# visp_add_subdirectory(<subdir> REQUIRED_DEPS <deps>)
+macro(visp_add_subdirectory subdir)
+ set(__deps "")
+
+ foreach(arg ${ARGN})
+ if(arg STREQUAL "REQUIRED_DEPS")
+ set(__varname "__deps")
+ else()
+ list(APPEND ${__varname} ${arg})
+ endif()
+ endforeach()
+ unset(__varname)
+
+ if(__deps STREQUAL "")
+ set(VP_DEPENDENCIES_FOUND TRUE)
+ else()
+ visp_check_dependencies(${__deps})
+ endif()
+
+ if(VP_DEPENDENCIES_FOUND)
+ add_subdirectory(${subdir})
+ endif()
+endmacro()
diff --git a/CMakeModules/VISPConfigVersion.cmake.in b/cmake/templates/VISPConfigVersion.cmake.in
similarity index 100%
rename from CMakeModules/VISPConfigVersion.cmake.in
rename to cmake/templates/VISPConfigVersion.cmake.in
diff --git a/cmake/templates/ViSP-third-party.txt.in b/cmake/templates/ViSP-third-party.txt.in
new file mode 100644
index 0000000..4c103b7
--- /dev/null
+++ b/cmake/templates/ViSP-third-party.txt.in
@@ -0,0 +1,59 @@
+ ViSP third-party libraries
+
+Below you will find the list of third party libraries used to
+build ViSP on your computer.
+
+Mathematics:
+ Gnu Scientific Library : ${VISP_HAVE_GSL_FOUND}
+ Lapack/blas : ${VISP_HAVE_LAPACK_C_FOUND}
+Simulator:
+ Ogre simulator : ${VISP_HAVE_OGRE_FOUND}
+ \- Ogre3D : ${VISP_HAVE_OGRE_FOUND}
+ \- OIS : ${VISP_HAVE_OIS_FOUND}
+ Coin simulator : ${VISP_HAVE_COIN3D_AND_GUI_FOUND}
+ \- Coin3D : ${VISP_HAVE_COIN3D_FOUND}
+ \- SoWin : ${VISP_HAVE_SOWIN_FOUND}
+ \- SoXt : ${VISP_HAVE_SOXT_FOUND}
+ \- SoQt : ${VISP_HAVE_SOQT_FOUND}
+ \- Qt4 : ${VISP_HAVE_QT4_FOUND}
+ \- Qt3 : ${VISP_HAVE_QT3_FOUND}
+Robots
+ Afma6 : ${VISP_HAVE_AFMA6_FOUND}
+ Afma4 : ${VISP_HAVE_AFMA4_FOUND}
+ Biclops : ${VISP_HAVE_BICLOPS_FOUND}
+ Ptu46 : ${VISP_HAVE_PTU46_FOUND}
+ Pioneer : ${VISP_HAVE_PIONEER_FOUND}
+ Viper S650 : ${VISP_HAVE_VIPER650_FOUND}
+ Viper S850 : ${VISP_HAVE_VIPER850_FOUND}
+Video devices (display)
+ X11 : ${VISP_HAVE_X11_FOUND}
+ GTK : ${VISP_HAVE_GTK2_FOUND}
+ OpenCV : ${VISP_HAVE_OPENCV_FOUND}
+ GDI : ${VISP_HAVE_GDI_FOUND}
+ Direct3D : ${VISP_HAVE_DIRECT3D_FOUND}
+Framegrabbers
+ Firewire libdc1394-2.x : ${VISP_HAVE_DC1394_FOUND}
+ Video For Linux Two : ${VISP_HAVE_V4L2_FOUND}
+ DirectShow : ${VISP_HAVE_DIRECTSHOW_FOUND}
+ CMU 1394 Digital Camera SDK : ${VISP_HAVE_CMU1394_FOUND}
+ OpenCV : ${VISP_HAVE_OPENCV_FOUND}
+Specific devices
+ Yarp : ${VISP_HAVE_YARP_FOUND}
+ Kinect : ${VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES_FOUND}
+ \-libfreenect : ${VISP_HAVE_LIBFREENECT_FOUND}
+ \-libusb-1.0 : ${VISP_HAVE_LIBUSB_1_FOUND}
+ \-pthread : ${VISP_HAVE_PTHREAD_FOUND}
+Video and image Read/Write:
+ FFMPEG : ${VISP_HAVE_FFMPEG_FOUND}
+ libjpeg : ${VISP_HAVE_JPEG_FOUND}
+ libpng : ${VISP_HAVE_PNG_FOUND}
+Misc:
+ XML2 : ${VISP_HAVE_XML2_FOUND}
+ pthread : ${VISP_HAVE_PTHREAD_FOUND}
+ OpenMP : ${VISP_HAVE_OPENMP_FOUND}
+ zbar : ${VISP_HAVE_ZBAR_FOUND}
+ dmtx : ${VISP_HAVE_DMTX_FOUND}
+Documentation:
+ Doxygen : ${VISP_HAVE_DOXYGEN_FOUND}
+ViSP built with C++11 features: ${VISP_HAVE_CPP11_COMPATIBILITY_FOUND}
+
diff --git a/cmake/templates/cmake_uninstall.cmake.in b/cmake/templates/cmake_uninstall.cmake.in
new file mode 100644
index 0000000..b32e083
--- /dev/null
+++ b/cmake/templates/cmake_uninstall.cmake.in
@@ -0,0 +1,61 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# Comes from http://www.cmake.org/Wiki/CMake_FAQ to create an uninstall target
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ #MESSAGE("Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
+ MESSAGE("There is no files to uninstall")
+ELSE(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+ STRING(REGEX REPLACE "\n" ";" files "${files}")
+ FOREACH(file ${files})
+ MESSAGE(STATUS "Uninstalling \"${file}\"")
+ IF(EXISTS "${file}")
+ EXEC_PROGRAM(
+ "@CMAKE_COMMAND@" ARGS "-E remove \"${file}\""
+ OUTPUT_VARIABLE rm_out
+ RETURN_VALUE rm_retval
+ )
+ IF("${rm_retval}" STREQUAL 0)
+ ELSE("${rm_retval}" STREQUAL 0)
+ MESSAGE(FATAL_ERROR "Problem when removing \"${file}\"")
+ ENDIF("${rm_retval}" STREQUAL 0)
+ ELSE(EXISTS "${file}")
+ MESSAGE(STATUS "File \"${file}\" does not exist.")
+ ENDIF(EXISTS "${file}")
+ ENDFOREACH(file)
+
+ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
diff --git a/cmake/templates/resources.cfg.in b/cmake/templates/resources.cfg.in
new file mode 100644
index 0000000..8160b53
--- /dev/null
+++ b/cmake/templates/resources.cfg.in
@@ -0,0 +1,7 @@
+# Resources required by the sample browser and most samples.
+[Essential]
+#Zip=@OGRE_MEDIA_DIR@/packs/SdkTrays.zip
+
+# Resource locations to be added to the default path
+[General]
+ at OGRE_RESOURCES_FileSystem@
diff --git a/cmake/templates/visp-config.bat.in b/cmake/templates/visp-config.bat.in
new file mode 100755
index 0000000..688a747
--- /dev/null
+++ b/cmake/templates/visp-config.bat.in
@@ -0,0 +1,134 @@
+ at rem #############################################################################
+ at rem #
+ at rem # This file is part of the ViSP software.
+ at rem # Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ at rem #
+ at rem # This software is free software; you can redistribute it and/or
+ at rem # modify it under the terms of the GNU General Public License
+ at rem # ("GPL") version 2 as published by the Free Software Foundation.
+ at rem # See the file LICENSE.txt at the root directory of this source
+ at rem # distribution for additional information about the GNU GPL.
+ at rem #
+ at rem # For using ViSP with software that can not be combined with the GNU
+ at rem # GPL, please contact Inria about acquiring a ViSP Professional
+ at rem # Edition License.
+ at rem #
+ at rem # See http://visp.inria.fr for more information.
+ at rem #
+ at rem # This software was developed at:
+ at rem # Inria Rennes - Bretagne Atlantique
+ at rem # Campus Universitaire de Beaulieu
+ at rem # 35042 Rennes Cedex
+ at rem # France
+ at rem #
+ at rem # If you have questions regarding the use of this file, please contact
+ at rem # Inria at visp at inria.fr
+ at rem #
+ at rem # This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ at rem # WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ at rem #
+ at rem # Description:
+ at rem # visp-config.bat script for Windows.
+ at rem # Auto-generated from visp-config.bat.in by cmake.
+ at rem #
+ at rem # Authors:
+ at rem # Fabien Spindler
+ at rem #
+ at rem #############################################################################
+
+
+ at echo off
+
+ at rem Get the dirname
+ at rem for /F %%x in ('CHDIR') do set PREFIX=%%x\..
+set CURPATH=%CD /d %%~dp0%
+
+ at rem echo CURPATH: %CURPATH%
+
+set PREFIX=%CURPATH%\..
+
+set VERSION=${VISP_VERSION}
+
+set DEFS=${VISP_CONFIG_SCRIPT_DEFS}
+
+set INCLUDE="${VISP_CONFIG_SCRIPT_INC}"
+
+set OPENMP_SUPPORT=${VISP_OPENMP_SUPPORT}
+
+set LIBDIR="${VISP_CONFIG_SCRIPT_LIBDIR}"
+
+set LIBS_DEBUG=${VISP_CONFIG_SCRIPT_LIBS_DEBUG}
+
+set LIBS_OPTIMIZED=${VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED}
+
+ at rem Test if an argument is provided
+
+if "%1" == "" goto USAGE
+
+ at rem Parse the argument list
+for %%a in (%*) do (
+ at rem echo Read arg %%a
+ if "%%a" == "--help" (
+ goto USAGE
+ )
+ if "%%a" == "--prefix" (
+ echo %PREFIX%
+ goto END
+ )
+ if "%%a" == "--def" (
+ echo %DEFS%
+ goto END
+ )
+ if "%%a" == "--include" (
+ echo %INCLUDE%
+ goto END
+ )
+ if "%%a" == "--openmp" (
+ echo %OPENMP_SUPPORT%
+ goto END
+ )
+ if "%%a" == "--libpath" (
+ echo %LIBDIR%
+ goto END
+ )
+ if "%%a" == "--libs-debug" (
+ echo %LIBS_DEBUG%
+ goto END
+ )
+ if "%%a" == "--libs-optimized" (
+ echo %LIBS_OPTIMIZED%
+ goto END
+ )
+ if "%%a" == "--version" (
+ echo ViSP %VERSION% Visual Servoing Platform
+ echo.
+ echo Copyright 2005 - 2015 Inria. All rights reserved.
+ goto END
+ )
+ if "%%a" == "--dumpversion" (
+ echo %VERSION%
+ goto END
+ )
+
+)
+
+:USAGE
+echo ViSP %VERSION% (Visual Servoing Platform)
+echo Copyright (C) 2005 - 2015 Inria. All rights reserved.
+echo.
+echo Usage: %0 [--prefix] [--def] [--include] [--openmp] [--libpath]
+echo [--libs-debug] [--libs-optimized] [--version] [--dumpversion] [--help]
+echo.
+echo --prefix Show ViSP installation prefix.
+echo --def Print pre-processor definitions.
+echo --include Print include directories.
+echo --openmp Indicates if OpenMP support has to be turned on/off.
+echo --libpath Print library directories.
+echo --libs-debug Print library dependencies for debug configuration.
+echo --libs-optimized Print library dependencies for optimized configuration.
+echo --dumpversion Output ViSP version information.
+echo --help Display this help and exit.
+echo.
+goto END
+
+:End
diff --git a/cmake/templates/visp-config.in b/cmake/templates/visp-config.in
new file mode 100755
index 0000000..319d526
--- /dev/null
+++ b/cmake/templates/visp-config.in
@@ -0,0 +1,96 @@
+#!/bin/sh
+
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# visp-config shell script.
+# Auto-generated from visp-config.in by cmake.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+relpath=`dirname $0`
+relpath=`(cd $relpath/..; pwd)`
+
+PREFIX=$relpath
+
+CFLAGS="${VISP_CONFIG_CFLAGS_SCRIPT}"
+
+LIBS="${VISP_CONFIG_LIBS_SCRIPT}"
+
+VERSION="${VISP_VERSION}"
+
+NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
+NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
+
+usage()
+{
+ cat <<EOF
+
+ViSP $VERSION (Visual Servoing Platform)
+Copyright (C) 2005 - 2015 Inria. All rights reserved.
+
+Usage: $0 [--prefix] [--cflags] [--libs] [--version] [--dumpversion] [--help]
+
+ --prefix Show ViSP installation prefix.
+ --cflags Print pre-processor and compiler flags including
+ third party includes we depend on.
+ --libs Print library linking information with ViSP
+ including third party libraries we depend on.
+ --version Output ViSP information.
+ --dumpversion Output ViSP version information.
+ --help Display this help and exit.
+
+EOF
+ exit $1
+}
+
+if ! test "$1"; then
+ usage;
+ exit 0;
+fi;
+
+for arg in $@; do
+ case $arg in
+ --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
+ --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
+ --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
+ --version)
+ echo "ViSP $VERSION (Visual Servoing Platform)"
+ echo ""
+ echo "Copyright (C) 2005 - 2015 Inria. All rights reserved.";;
+ --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
+ *) usage; exit 0 ;;
+ esac;
+done;
+echo ""
diff --git a/cmake/templates/visp-config.install.in b/cmake/templates/visp-config.install.in
new file mode 100755
index 0000000..e3f2cf4
--- /dev/null
+++ b/cmake/templates/visp-config.install.in
@@ -0,0 +1,100 @@
+#!/bin/sh
+
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# visp-config shell script.
+# Auto-generated from visp-config.install.in by cmake. For backward
+# compatibility used visp.pc.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+
+relpath=`dirname $0`
+relpath=`(cd $relpath/..; pwd)`
+
+PREFIX=$relpath
+
+CFLAGS_CMD=`pkg-config --cflags visp`
+CFLAGS=$CFLAGS_CMD
+
+LIBS_CMD=`pkg-config --libs visp`
+LIBS=$LIBS_CMD
+
+VERSION_CMD=`pkg-config --modversion visp`
+VERSION=$VERSION_CMD
+
+NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
+NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
+
+usage()
+{
+ cat <<EOF
+
+ViSP $VERSION (Visual Servoing Platform)
+Copyright (C) 2005 - 2015 Inria. All rights reserved.
+
+Usage: $0 [--prefix] [--cflags] [--libs] [--version] [--dumpversion] [--help]
+
+ --prefix Show ViSP installation prefix.
+ --cflags Print pre-processor and compiler flags including
+ third party includes we depend on.
+ --libs Print library linking information with ViSP
+ including third party libraries we depend on.
+ --version Output ViSP information.
+ --dumpversion Output ViSP version information.
+ --help Display this help and exit.
+
+EOF
+ exit $1
+}
+
+if ! test "$1"; then
+ usage;
+ exit 0;
+fi;
+
+for arg in $@; do
+ case $arg in
+ --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
+ --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
+ --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
+ --version)
+ echo "ViSP $VERSION (Visual Servoing Platform)"
+ echo ""
+ echo "Copyright (C) 2005 - 2015 Inria. All rights reserved.";;
+ --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
+ *) usage; exit 0 ;;
+ esac;
+done;
+echo ""
diff --git a/cmake/templates/visp.pc.in b/cmake/templates/visp.pc.in
new file mode 100644
index 0000000..4941a6a
--- /dev/null
+++ b/cmake/templates/visp.pc.in
@@ -0,0 +1,14 @@
+# Package Information for pkg-config
+
+prefix=${CMAKE_INSTALL_PREFIX}
+exec_prefix=@exec_prefix@
+libdir=@libdir@
+includedir=@includedir@
+
+Name: ViSP
+Description: Visual Servoing Platform
+URL: http://visp.inria.fr
+Version: ${VISP_VERSION}
+Libs: ${VISP_CONFIG_LIBS_PC}
+
+Cflags: ${VISP_CONFIG_CFLAGS_PC}
diff --git a/cmake/templates/visp_modules.h.in b/cmake/templates/visp_modules.h.in
new file mode 100644
index 0000000..408a36d
--- /dev/null
+++ b/cmake/templates/visp_modules.h.in
@@ -0,0 +1,8 @@
+/*
+ * ** File generated automatically, do not modify **
+ *
+ * This file defines the list of modules available in current build configuration
+ *
+*/
+
+ at VISP_MODULE_DEFINITIONS_CONFIGMAKE@
diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in
new file mode 100644
index 0000000..c1cf464
--- /dev/null
+++ b/cmake/templates/vpConfig.h.in
@@ -0,0 +1,411 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * ViSP configuration.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpConfig_h
+#define vpConfig_h
+
+#if defined _MSC_VER && _MSC_VER >= 1200
+ #pragma warning( disable: 4100 4127 4251 4275 4514 4668 4710 4820 )
+ #if _MSC_VER >= 1400 // 1400 = MSVC 8 2005
+ #pragma warning( disable: 4548 )
+ #endif
+ #if _MSC_VER > 1500 // 1500 = MSVC 9 2008
+ #pragma warning( disable: 4986 )
+ #endif
+
+ // 4100 : undocumented ("unreferenced formal parameter")
+ // 4127 : conditional expression is constant
+ // 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions
+ // 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier'
+ // 4514 : 'function' : unreferenced inline function has been removed
+ // 4548 : expression before comma has no effect
+ // 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives'
+ // 4710 : 'function' : function not inlined
+ // 4820 : 'bytes' bytes padding added after construct 'member_name'
+ // 4986 : undocumented
+#endif
+
+#if defined _MSC_VER && (_MSC_VER == 1500)
+// Visual Studio 9 2008 specific stuff
+// Fix running 64-bit OpenMP Debug Builds compiled with Visual Studio 2008 SP1
+// See discussion on https://gforge.inria.fr/forum/message.php?msg_id=149273&group_id=397
+// and the proposed fix: http://www.johanseland.com/2010/08/running-64-bit-openmp-debug-builds.html
+# define _BIND_TO_CURRENT_OPENMP_VERSION 1
+#endif
+
+#if defined(__MINGW__) || defined(__MINGW32__) || defined(__MINGW64__)
+// Work arround to fix build issues that may occur with Mingw:
+// error: 'DBL_EPSILON' was not declared in this scope
+// error: 'FLT_EPSILON' was not declared in this scope
+
+# include <float.h>
+
+# ifndef DBL_EPSILON
+# define DBL_EPSILON __DBL_EPSILON__
+# endif
+# ifndef FLT_EPSILON
+# define FLT_EPSILON __FLT_EPSILON__
+# endif
+#endif
+
+#include <visp3/visp_modules.h>
+
+// ViSP major version.
+#define VISP_VERSION_MAJOR ${VISP_VERSION_MAJOR}
+
+// ViSP minor version.
+#define VISP_VERSION_MINOR ${VISP_VERSION_MINOR}
+
+// ViSP patch version.
+#define VISP_VERSION_PATCH ${VISP_VERSION_PATCH}
+
+// ViSP version with dots "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}".
+#cmakedefine VISP_VERSION ${VISP_VERSION}
+
+// ViSP version as an integer
+#define VP_VERSION_INT(a, b, c) (a<<16 | b<<8 | c)
+#define VISP_VERSION_INT VP_VERSION_INT(VISP_VERSION_MAJOR, \
+ VISP_VERSION_MINOR, \
+ VISP_VERSION_PATCH)
+
+// ViSP library is either compiled static or shared
+// Used to set declspec(import, export) in headers if required under Windows
+#cmakedefine VISP_BUILD_SHARED_LIBS
+
+// Defined if deprecated functionalities are requested to build
+#cmakedefine VISP_BUILD_DEPRECATED_FUNCTIONS
+
+// Defined if MSVC is the compiler
+#cmakedefine VISP_USE_MSVC
+
+// Defined if X11 library available.
+#cmakedefine VISP_HAVE_X11
+
+// Defined if XML2 library available.
+#cmakedefine VISP_HAVE_XML2
+
+// Defined if pthread library available.
+#cmakedefine VISP_HAVE_PTHREAD
+
+// Defined if YARP available.
+#cmakedefine VISP_HAVE_YARP
+
+// Defined if OpenCV available.
+#cmakedefine VISP_HAVE_OPENCV
+
+// Defined if OpenCV nonfree module available. Only with OpenCV < 3.0.0
+#cmakedefine VISP_HAVE_OPENCV_NONFREE
+
+// Defined if OpenCV xfeatures2d module available. Only since OpenCV >= 3.0.0
+#cmakedefine VISP_HAVE_OPENCV_XFEATURES2D
+
+// OpenCV version in hexadecimal (for example 2.1.0 gives 0x020100).
+#ifdef VISP_HAVE_OPENCV
+# define VISP_HAVE_OPENCV_VERSION ${VISP_HAVE_OPENCV_VERSION}
+#endif
+
+// Defined if GTK2 library available
+#cmakedefine VISP_HAVE_GTK
+
+// Defined if GDI (Graphics Device Interface) library available
+#cmakedefine VISP_HAVE_GDI
+
+// Defined if Direct3D9 library available
+#cmakedefine VISP_HAVE_D3D9
+
+// Defined if one of the display device is available
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+# define VISP_HAVE_DISPLAY
+#endif
+
+// Defined if GSL library available (-lgsl -lgslcblas).
+#cmakedefine VISP_HAVE_GSL
+
+// Defined if lapack/blas libraries are available (-lblas -llapack).
+#cmakedefine VISP_HAVE_LAPACK_C
+
+// Defined the path to the basic scenes used by the simulator
+#cmakedefine VISP_SCENES_DIR "${VISP_SCENES_DIR}"
+
+// Defined the path to the robot's arms 3D model used by the robot simulators
+#cmakedefine VISP_ROBOT_ARMS_DIR "${VISP_ROBOT_ARMS_DIR}"
+
+// Defined if Ogre3d is available.
+#cmakedefine VISP_HAVE_OGRE
+
+// Defined if Ogre3d plugins.cfg is available.
+#cmakedefine VISP_HAVE_OGRE_PLUGINS_PATH "${VISP_HAVE_OGRE_PLUGINS_PATH}"
+
+// Defined if Ogre3d resources.cfg is available.
+#cmakedefine VISP_HAVE_OGRE_RESOURCES_PATH "${VISP_HAVE_OGRE_RESOURCES_PATH}"
+
+// Defined if OIS (Object Oriented Input System) library available.
+#cmakedefine VISP_HAVE_OIS
+
+// Defined if Coin3D and one of the GUI (SoXt, SoWin, SoQt + Qt)
+// libraries are available.
+#cmakedefine VISP_HAVE_COIN3D_AND_GUI
+// provided for compat with previous releases
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+# define VISP_HAVE_COIN_AND_GUI
+#endif
+
+// Defined if Coin3D library available.
+#cmakedefine VISP_HAVE_COIN3D
+// provided for compat with previous releases
+#ifdef VISP_HAVE_COIN3D
+# define VISP_HAVE_COIN
+#endif
+
+// Defined if OpenGL library available.
+#cmakedefine VISP_HAVE_OPENGL
+
+// Defined if Qt library available (either Qt-3 or Qt-4).
+#cmakedefine VISP_HAVE_QT
+
+// Defined if SoQt library available.
+#cmakedefine VISP_HAVE_SOQT
+
+// Defined if SoWin library available.
+#cmakedefine VISP_HAVE_SOWIN
+
+// Defined if SoXt library available.
+#cmakedefine VISP_HAVE_SOXT
+
+// Defined if libjpeg library available.
+#cmakedefine VISP_HAVE_JPEG
+#ifdef VISP_HAVE_JPEG
+# define VISP_HAVE_LIBJPEG
+#endif
+
+// Defined if libpng library available.
+#cmakedefine VISP_HAVE_PNG
+#ifdef VISP_HAVE_PNG
+# define VISP_HAVE_LIBPNG
+#endif
+
+// Defined if libfreenect, libusb-1.0 and libpthread libraries available.
+#cmakedefine VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES
+
+// Defined if libfreenect library available.
+#cmakedefine VISP_HAVE_LIBFREENECT
+
+// Defined if libfreenect library in an old version package for
+// ubuntu 10.04 lucid is available.
+// This is a workaround useful to initialise vpKinect depending on the
+// libfreenect version
+//#ifdef VISP_HAVE_LIBFREENECT_OLD
+// Freenect::Freenect<vpKinect> freenect;
+// vpKinect & kinect = freenect.createDevice(0);
+//#else
+// Freenect::Freenect freenect;
+// vpKinect & kinect = freenect.createDevice<vpKinect>(0);
+//#endif
+#cmakedefine VISP_HAVE_LIBFREENECT_OLD
+
+// Defined if libusb-1.0 library available.
+#cmakedefine VISP_HAVE_LIBUSB_1
+
+// Defined if ffmpeg library available.
+#cmakedefine VISP_HAVE_FFMPEG
+
+// Defined if raw1394 and dc1394-2.x libraries available.
+#cmakedefine VISP_HAVE_DC1394
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) && defined(VISP_HAVE_DC1394)
+# define VISP_HAVE_DC1394_2
+#endif
+
+// Defined if dc1394_camera_enumerate() is available in dc1394-2.x.
+// dc1394_camera_enumerate() was introduced after libdc1394-2.0.0-rc7.
+#cmakedefine VISP_HAVE_DC1394_CAMERA_ENUMERATE
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) && defined(VISP_HAVE_DC1394_CAMERA_ENUMERATE)
+# define VISP_HAVE_DC1394_2_CAMERA_ENUMERATE
+#endif
+
+// Defined if dc1394_find_cameras() is available in dc1394-2.x
+// dc1394_find_cameras() is still present until libdc1394-2.0.0-rc7.
+// This function was suppress and replace by dc1394_camera_enumerate()
+// in more recent releases.
+#cmakedefine VISP_HAVE_DC1394_FIND_CAMERAS
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) && defined(VISP_HAVE_DC1394_FIND_CAMERAS)
+# define VISP_HAVE_DC1394_2_FIND_CAMERAS
+#endif
+
+// Defined if CMU 1394 Digital Camera SDK available.
+#cmakedefine VISP_HAVE_CMU1394
+
+// Defined if Video For Linux Two available.
+#cmakedefine VISP_HAVE_V4L2
+
+// Defined if DirectShow library is available (only under Windows).
+#cmakedefine VISP_HAVE_DIRECTSHOW
+
+// Defined if Irisa's Afma4 robot available.
+#cmakedefine VISP_HAVE_AFMA4
+
+// Defined if Irisa's Afma6 robot available.
+#cmakedefine VISP_HAVE_AFMA6
+
+// Defined if Biclops pan-tilt head available.
+#cmakedefine VISP_HAVE_BICLOPS
+#cmakedefine VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION
+
+// Defined if Irisa's Ptu-46 pan-tilt head available.
+#cmakedefine VISP_HAVE_PTU46
+
+// Defined if Irisa's Viper S650 robot available.
+#cmakedefine VISP_HAVE_VIPER650
+
+// Defined if Irisa's Viper S850 robot available.
+#cmakedefine VISP_HAVE_VIPER850
+
+// Defined if the Aria library and (pthread, rt, dl libraries under Unix) is found.
+// These libraries are used to control Pioneer mobile robots.
+#cmakedefine VISP_HAVE_PIONEER
+
+// Defined if linux/parport.h is available for parallel port usage.
+#cmakedefine VISP_HAVE_PARPORT
+
+// Defined if libzbar is available for bar code detection
+#cmakedefine VISP_HAVE_ZBAR
+
+// Defined if libdmtx is available for bar code detection
+#cmakedefine VISP_HAVE_DMTX
+
+// Defined if Inria's NAS server hosting /udd/ is available
+// Used for the moment in vpAfma6 class to check if config files are
+// available in /udd/fspindle/robot/Afma6/current/include/
+#cmakedefine VISP_HAVE_ACCESS_TO_NAS
+
+// Defined if Doxygen documentation tool is found
+#cmakedefine VISP_HAVE_DOXYGEN
+
+// Defined if we want to compute interaction matrices by combining
+// other interaction matrices
+#cmakedefine VISP_MOMENTS_COMBINE_MATRICES
+
+//Defined if we want to use openmp
+#cmakedefine VISP_HAVE_OPENMP
+
+//Defined if we want to use c++ 11
+#cmakedefine VISP_HAVE_CPP11_COMPATIBILITY
+
+// Defined if isnan macro is available
+#cmakedefine VISP_HAVE_FUNC_ISNAN
+
+// Defined if std::isnan function is available
+#cmakedefine VISP_HAVE_FUNC_STD_ISNAN
+
+// Defined if _isnan (Microsoft version) is available
+#cmakedefine VISP_HAVE_FUNC__ISNAN
+
+// Defined if isinf macro is available
+#cmakedefine VISP_HAVE_FUNC_ISINF
+
+// Defined if std::isinf function is available
+#cmakedefine VISP_HAVE_FUNC_STD_ISINF
+
+// Defined if _finite (Microsoft version) function is available
+#cmakedefine VISP_HAVE_FUNC__FINITE
+
+// Defined if round function is available
+#cmakedefine VISP_HAVE_FUNC_ROUND
+
+// Defined if std::round function is available
+#cmakedefine VISP_HAVE_FUNC_STD_ROUND
+
+// Handle portable symbol export.
+// Defining manually which symbol should be exported is required
+// under Windows whether MinGW or MSVC is used.
+//
+// The headers then have to be able to work in two different modes:
+// - dllexport when one is building the library,
+// - dllimport for clients using the library.
+//
+// On Linux, set the visibility accordingly. If C++ symbol visibility
+// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
+# if defined(_WIN32) || defined(__CYGWIN__)
+// On Microsoft Windows, use dllimport and dllexport to tag symbols.
+# define VISP_DLLIMPORT __declspec(dllimport)
+# define VISP_DLLEXPORT __declspec(dllexport)
+# define VISP_DLLLOCAL
+# else
+// On Linux, for GCC >= 4, tag symbols using GCC extension.
+# if __GNUC__ >= 4
+# define VISP_DLLIMPORT __attribute__ ((visibility("default")))
+# define VISP_DLLEXPORT __attribute__ ((visibility("default")))
+# define VISP_DLLLOCAL __attribute__ ((visibility("hidden")))
+# else
+// Otherwise (GCC < 4 or another compiler is used), export everything.
+# define VISP_DLLIMPORT
+# define VISP_DLLEXPORT
+# define VISP_DLLLOCAL
+# endif // __GNUC__ >= 4
+# endif // defined(_WIN32) || defined(__CYGWIN__)
+
+// Under Windows, for shared libraries (DLL) we need to define export on
+// compilation or import on use (like a third party project).
+// We exploit here the fact that cmake auto set xxx_EXPORTS (with S) on
+// compilation.
+#if defined(VISP_BUILD_SHARED_LIBS)
+// Depending on whether one is building or using the
+// library define VISP_EXPORT to import or export.
+# ifdef visp_EXPORTS
+# define VISP_EXPORT VISP_DLLEXPORT
+# else
+# define VISP_EXPORT VISP_DLLIMPORT
+# endif
+# define VISP_LOCAL VISP_DLLLOCAL
+#else
+// If one is using the library statically, get rid of
+// extra information.
+# define VISP_EXPORT
+# define VISP_LOCAL
+#endif
+
+// Add the material to produce a warning when deprecated functions are used
+#ifndef vp_deprecated
+# if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# define vp_deprecated __attribute__((deprecated))
+# else
+# define vp_deprecated __declspec(deprecated)
+# endif
+#endif
+
+#endif
+
+
diff --git a/cmake/templates/vpHeader.h.in b/cmake/templates/vpHeader.h.in
new file mode 100644
index 0000000..128c4db
--- /dev/null
+++ b/cmake/templates/vpHeader.h.in
@@ -0,0 +1,6 @@
+/*
+ * ** File generated automatically, do not modify **
+ *
+*/
+
+ at VISP_HEADER_CONTENT_CONFIGMAKE@
diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt
index 3d98dfa..bd0b505 100644
--- a/demo/CMakeLists.txt
+++ b/demo/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,14 +35,31 @@
#
#############################################################################
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- wireframe-simulator
-)
+project(ViSP-demo)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP)
+
+if(MSVC)
+ if(NOT VISP_SHARED)
+ foreach(flag_var
+ CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
+ CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
+ CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
+ CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
+ if(${flag_var} MATCHES "/MD")
+ string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}")
+ endif()
+ if(${flag_var} MATCHES "/MDd")
+ string(REGEX REPLACE "/MDd" "/MTd" ${flag_var} "${${flag_var}}")
+ endif()
+ endforeach(flag_var)
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
+ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /NODEFAULTLIB:atlthunk.lib /NODEFAULTLIB:msvcrt.lib /NODEFAULTLIB:msvcrtd.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_EXE_LINKER_FLAGS_DEBUG} /NODEFAULTLIB:libcmt.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /NODEFAULTLIB:libcmtd.lib")
+ endif()
+endif()
+visp_add_subdirectory(wireframe-simulator REQUIRED_DEPS visp_core visp_robot visp_vs visp_io visp_gui)
diff --git a/demo/wireframe-simulator/CMakeLists.txt b/demo/wireframe-simulator/CMakeLists.txt
index ff71aa1..3337c23 100644
--- a/demo/wireframe-simulator/CMakeLists.txt
+++ b/demo/wireframe-simulator/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,42 +35,27 @@
#
#############################################################################
+project(demo-wireframe-simulator)
+
+cmake_minimum_required(VERSION 2.8)
+
+find_package(VISP REQUIRED visp_core visp_robot visp_vs visp_io visp_gui)
+
# SOURCE variable corresponds to the list of all the sources to build binaries.
# The generate binary comes by removing the .cpp extension to
# the source name.
#
# If you want to add/remove a source, modify here
-set (SOURCE
+set(demo_cpp
servoSimu4Points.cpp
servoSimuCylinder.cpp
servoSimuSphere.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
- add_dependencies(visp_demos ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "demos")
- endif()
-
- if(UNIX)
- install(TARGETS ${binary}
- DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}/demo/wireframe-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE)
- else()
- install(TARGETS ${binary}
- DESTINATION demo/wireframe-simulator
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE)
+foreach(cpp ${demo_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "demos")
endif()
endforeach()
diff --git a/demo/wireframe-simulator/servoSimu4Points.cpp b/demo/wireframe-simulator/servoSimu4Points.cpp
index 1e2fbbf..5b1408e 100644
--- a/demo/wireframe-simulator/servoSimu4Points.cpp
+++ b/demo/wireframe-simulator/servoSimu4Points.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimu4Points.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Demonstration of the wireframe simulator with a simple visual servoing
*
@@ -47,25 +43,25 @@
#include <stdlib.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpTime.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
#define GETOPTARGS "dh"
@@ -226,9 +222,8 @@ main(int argc, const char ** argv)
// Set initial position of the object in the world frame
vpHomogeneousMatrix wMo(0.0,0.0,0.2,0,0,0);
// Position of the camera in the world frame
- vpHomogeneousMatrix wMc, cMw;
+ vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
- cMw = wMc.inverse();
//The four point used as visual features
vpPoint point[4] ;
@@ -382,7 +377,7 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
- robot.getPosition(wMc) ;
+ wMc = robot.getPosition() ;
cMo = wMc.inverse() * wMo;
for (int i = 0 ; i < 4 ; i++)
{
diff --git a/demo/wireframe-simulator/servoSimuCylinder.cpp b/demo/wireframe-simulator/servoSimuCylinder.cpp
index 6e3fd35..b436098 100644
--- a/demo/wireframe-simulator/servoSimuCylinder.cpp
+++ b/demo/wireframe-simulator/servoSimuCylinder.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuCylinder.cpp 4659 2014-02-09 14:11:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Demonstration of the wireframe simulator with a simple visual servoing
*
@@ -48,25 +44,25 @@
#include <stdlib.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpServo.h>
-#include <visp/vpTime.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
#define GETOPTARGS "dh"
@@ -194,7 +190,7 @@ main(int argc, const char ** argv)
}
vpServo task;
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
@@ -205,9 +201,8 @@ main(int argc, const char ** argv)
// Set initial position of the object in the world frame
vpHomogeneousMatrix wMo(0.0,0.0,0,0,0,0);
// Position of the camera in the world frame
- vpHomogeneousMatrix wMc, cMw;
+ vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
- cMw = wMc.inverse();
// Create a cylinder
vpCylinder cylinder(0,0,1,0,0,0,0.1);
@@ -288,7 +283,7 @@ main(int argc, const char ** argv)
while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext,false)){};
}
- robot.setPosition( cMw );
+ robot.setPosition( wMc );
//Print the task
task.print();
@@ -319,8 +314,8 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
- robot.getPosition(cMw) ;
- cMo = cMw * wMo;
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
cylinder.track(cMo) ;
vpFeatureBuilder::create(l[0], cylinder, vpCylinder::line1);
diff --git a/demo/wireframe-simulator/servoSimuSphere.cpp b/demo/wireframe-simulator/servoSimuSphere.cpp
index b26af02..8e0b0fe 100644
--- a/demo/wireframe-simulator/servoSimuSphere.cpp
+++ b/demo/wireframe-simulator/servoSimuSphere.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuSphere.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Demonstration of the wireframe simulator with a simple visual servoing
*
@@ -49,26 +45,26 @@
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpServo.h>
-#include <visp/vpSphere.h>
-#include <visp/vpTime.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpSphere.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
#define GETOPTARGS "dh"
@@ -255,7 +251,7 @@ main(int argc, const char ** argv)
}
vpServo task;
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
@@ -270,11 +266,10 @@ main(int argc, const char ** argv)
// Set initial position of the object in the world frame
vpHomogeneousMatrix wMo(0.0,0.0,0,0,0,0);
// Position of the camera in the world frame
- vpHomogeneousMatrix wMc, cMw;
+ vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
- cMw = wMc.inverse();
- robot.setPosition( cMw );
+ robot.setPosition( wMc );
//The sphere
vpSphere sphere(0,0,0,0.15);
@@ -385,8 +380,8 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
- robot.getPosition(cMw) ;
- cMo = cMw * wMo;
+ wMc = robot.getPosition() ;
+ cMo = wMc.inverse() * wMo;
sphere.track(cMo);
diff --git a/doc/biblio/references.bib b/doc/biblio/references.bib
index e651b96..7afeccc 100644
--- a/doc/biblio/references.bib
+++ b/doc/biblio/references.bib
@@ -375,3 +375,31 @@ x-advisor = {Chaumette, F.},
address = {New York, NY, USA},
}
+ at inproceedings{Marey:2010,
+ TITLE = {{A new large projection operator for the redundancy framework}},
+ AUTHOR = {Marey, Mohammed and Chaumette, Francois},
+ URL = {https://hal.inria.fr/inria-00544789},
+ BOOKTITLE = {{IEEE Int. Conf. on Robotics and Automation, ICRA'10}},
+ ADDRESS = {Anchorage, Alaska, United States},
+ PAGES = {3727-3732},
+ YEAR = {2010},
+ KEYWORDS = {Visual Servoing},
+ PDF = {https://hal.inria.fr/inria-00544789/file/2010_icra_marey.pdf},
+ HAL_ID = {inria-00544789},
+ HAL_VERSION = {v1},
+}
+
+ at inproceedings{Marey:2010b,
+ TITLE = {{New strategies for avoiding robot joint limits: Application to visual servoing using a large projection operator}},
+ AUTHOR = {Marey, Mohammed and Chaumette, Fran{\c c}ois},
+ URL = {https://hal.inria.fr/inria-00544790},
+ BOOKTITLE = {{IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, IROS'10}},
+ ADDRESS = {Taipei, Taiwan, Taiwan},
+ PAGES = {6222-6227},
+ YEAR = {2010},
+ PDF = {https://hal.inria.fr/inria-00544790/file/2010_iros_marey.pdf},
+ HAL_ID = {inria-00544790},
+ HAL_VERSION = {v1},
+}
+
+
diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in
index 2a21e8e..fbd86bd 100644
--- a/doc/config-doxygen.in
+++ b/doc/config-doxygen.in
@@ -32,7 +32,7 @@ PROJECT_NAME = ViSP
# This could be handy for archiving the generated documentation or
# if some version control system is used.
-PROJECT_NUMBER =
+PROJECT_NUMBER = "@VISP_VERSION@"
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer
@@ -45,7 +45,7 @@ PROJECT_BRIEF =
# exceed 55 pixels and the maximum width should not exceed 200 pixels.
# Doxygen will copy the logo to the output directory.
-PROJECT_LOGO =
+PROJECT_LOGO = "@VISP_SOURCE_DIR@/doc/image/img-logo-visp.png"
# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute)
# base path where the generated documentation will be put.
@@ -61,7 +61,7 @@ OUTPUT_DIRECTORY = doc
# source files, where putting all generated files in the same directory would
# otherwise cause performance problems for the file system.
-CREATE_SUBDIRS = YES
+CREATE_SUBDIRS = NO
# The OUTPUT_LANGUAGE tag is used to specify the language in which all
# documentation generated by doxygen is written. Doxygen will use this
@@ -130,7 +130,7 @@ FULL_PATH_NAMES = YES
# relative paths, which will be relative from the directory where doxygen is
# started.
-STRIP_FROM_PATH =
+STRIP_FROM_PATH =
# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of
# the path mentioned in the documentation of a class, which tells
@@ -139,7 +139,22 @@ STRIP_FROM_PATH =
# definition is used. Otherwise one should specify the include paths that
# are normally passed to the compiler using the -I flag.
-STRIP_FROM_INC_PATH =
+STRIP_FROM_INC_PATH = "@VISP_SOURCE_DIR@/modules/ar/include" \
+ "@VISP_SOURCE_DIR@/modules/core/include" \
+ "@VISP_SOURCE_DIR@/modules/detection/include" \
+ "@VISP_SOURCE_DIR@/modules/gui/include" \
+ "@VISP_SOURCE_DIR@/modules/io/include" \
+ "@VISP_SOURCE_DIR@/modules/robot/include" \
+ "@VISP_SOURCE_DIR@/modules/sensor/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/blob/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/klt/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/mbt/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/me/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/tt/include" \
+ "@VISP_SOURCE_DIR@/modules/tracker/tt_mi/include" \
+ "@VISP_SOURCE_DIR@/modules/vision/include" \
+ "@VISP_SOURCE_DIR@/modules/visual_features/include"
+ "@VISP_SOURCE_DIR@/modules/vs/include"
# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter
# (but less readable) file names. This can be useful if your file system
@@ -667,13 +682,13 @@ WARN_LOGFILE = warning.log
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
-INPUT = "@VISP_SOURCE_DIR@/src" \
+INPUT = "@VISP_SOURCE_DIR@/modules" \
"@VISP_SOURCE_DIR@/example" \
"@VISP_SOURCE_DIR@/tutorial" \
"@VISP_SOURCE_DIR@/demo" \
- "@VISP_SOURCE_DIR@/test" \
"@VISP_SOURCE_DIR@/doc" \
- "@VISP_BINARY_DIR@/doc"
+ "@VISP_BINARY_DIR@/doc" \
+ "@VISP_CONTRIB_MODULES_PATH@"
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -738,7 +753,7 @@ EXCLUDE_SYMBOLS =
EXAMPLE_PATH = "@VISP_SOURCE_DIR@/example" \
"@VISP_SOURCE_DIR@/tutorial" \
"@VISP_SOURCE_DIR@/demo" \
- "@VISP_SOURCE_DIR@/test"
+ "@VISP_SOURCE_DIR@/modules"
# If the value of the EXAMPLE_PATH tag contains directories, you can use the
# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
@@ -972,7 +987,7 @@ HTML_COLORSTYLE_GAMMA = 80
# page will contain the date and time when the page was generated. Setting
# this to NO can help when comparing the output of multiple runs.
-HTML_TIMESTAMP = YES
+HTML_TIMESTAMP = NO
# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
# documentation will contain sections that can be hidden and shown after the
@@ -1550,7 +1565,7 @@ PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS \
VISP_HAVE_D3D9 \
VISP_HAVE_GSL \
VISP_HAVE_DISPLAY \
- VISP_HAVE_LAPACK \
+ VISP_HAVE_LAPACK_C \
VISP_HAVE_OPENCV \
VISP_HAVE_OPENCV_NONFREE \
VISP_HAVE_OPENCV_VERSION=0x020408 \
@@ -1558,14 +1573,14 @@ PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS \
VISP_HAVE_OGRE_PLUGINS_PATH \
VISP_HAVE_OGRE_RESOURCES_PATH \
VISP_HAVE_OIS \
- VISP_HAVE_COIN_AND_GUI \
- VISP_HAVE_COIN \
+ VISP_HAVE_COIN3D_AND_GUI \
+ VISP_HAVE_COIN3D \
VISP_HAVE_OPENGL \
VISP_HAVE_QT \
VISP_HAVE_SOQT \
VISP_HAVE_SOWIN \
VISP_HAVE_SOXT \
- VISP_HAVE_DC1394_2 \
+ VISP_HAVE_DC1394 \
VISP_HAVE_CMU1394 \
VISP_HAVE_V4L2 \
VISP_HAVE_DIRECTSHOW \
@@ -1578,8 +1593,8 @@ PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS \
VISP_HAVE_PIONEER \
VISP_HAVE_PARPORT \
VISP_HAVE_XML2 \
- VISP_HAVE_LIBJPEG \
- VISP_HAVE_LIBPNG \
+ VISP_HAVE_JPEG \
+ VISP_HAVE_PNG \
VISP_HAVE_FFMPEG \
VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES \
VISP_HAVE_LIBFREENECT \
@@ -1589,12 +1604,30 @@ PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS \
VISP_HAVE_DMTX \
VISP_HAVE_ACCESS_TO_NAS \
VISP_HAVE_CPP11_COMPATIBILITY \
+ VISP_HAVE_MODULE_AR \
+ VISP_HAVE_MODULE_CORE \
+ VISP_HAVE_MODULE_DETECTION \
+ VISP_HAVE_MODULE_GUI \
+ VISP_HAVE_MODULE_IO \
+ VISP_HAVE_MODULE_ROBOT \
+ VISP_HAVE_MODULE_SENSOR \
+ VISP_HAVE_MODULE_BLOB \
+ VISP_HAVE_MODULE_KLT \
+ VISP_HAVE_MODULE_MBT \
+ VISP_HAVE_MODULE_ME \
+ VISP_HAVE_MODULE_TT \
+ VISP_HAVE_MODULE_TT_MI \
+ VISP_HAVE_MODULE_VISION \
+ VISP_HAVE_MODULE_VISUAL_FEATURES \
+ VISP_HAVE_MODULE_VS \
VISP_BUILD_DEPRECATED_FUNCTIONS \
WIN32 \
APPLE \
UNIX \
VP_DEBUG \
- VP_TRACE
+ VP_TRACE \
+ __APPLE__ \
+ __MACH__
# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then
# this tag can be used to specify a list of macro names that should be expanded.
@@ -1684,7 +1717,7 @@ HIDE_UNDOC_RELATIONS = YES
# toolkit from AT&T and Lucent Bell Labs. The other options in this section
# have no effect if this option is set to NO (the default)
-HAVE_DOT = @VISP_HAVE_DOT@
+HAVE_DOT = NO
# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is
# allowed to run in parallel. When set to 0 (the default) doxygen will
@@ -1874,4 +1907,4 @@ GENERATE_LEGEND = YES
# remove the intermediate dot files that are used to generate
# the various graphs.
-DOT_CLEANUP = NO
+DOT_CLEANUP = YES
diff --git a/doc/image/img-ccmake-centos-all.png b/doc/image/img-ccmake-centos-all.png
index b9ecef6..56a9a7d 100644
Binary files a/doc/image/img-ccmake-centos-all.png and b/doc/image/img-ccmake-centos-all.png differ
diff --git a/doc/image/img-ccmake-centos.png b/doc/image/img-ccmake-centos.png
deleted file mode 100644
index 49c361c..0000000
Binary files a/doc/image/img-ccmake-centos.png and /dev/null differ
diff --git a/doc/image/img-ccmake-opensuse-all.png b/doc/image/img-ccmake-opensuse-all.png
deleted file mode 100644
index 6180b97..0000000
Binary files a/doc/image/img-ccmake-opensuse-all.png and /dev/null differ
diff --git a/doc/image/img-ccmake-opensuse.png b/doc/image/img-ccmake-opensuse.png
deleted file mode 100644
index 0b780d1..0000000
Binary files a/doc/image/img-ccmake-opensuse.png and /dev/null differ
diff --git a/doc/image/img-ccmake-raspberry-pi.png b/doc/image/img-ccmake-raspberry-pi.png
deleted file mode 100644
index 3854775..0000000
Binary files a/doc/image/img-ccmake-raspberry-pi.png and /dev/null differ
diff --git a/doc/image/img-ccmake-ubuntu-all.png b/doc/image/img-ccmake-ubuntu-all.png
index 258f782..f20fada 100644
Binary files a/doc/image/img-ccmake-ubuntu-all.png and b/doc/image/img-ccmake-ubuntu-all.png differ
diff --git a/doc/image/img-ccmake-ubuntu.png b/doc/image/img-ccmake-ubuntu.png
deleted file mode 100644
index 6f59981..0000000
Binary files a/doc/image/img-ccmake-ubuntu.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-config-end.png b/doc/image/img-cmake-win10-config-end.png
new file mode 100644
index 0000000..bb7cd97
Binary files /dev/null and b/doc/image/img-cmake-win10-config-end.png differ
diff --git a/doc/image/img-cmake-win10-config.png b/doc/image/img-cmake-win10-config.png
new file mode 100644
index 0000000..ba44f9b
Binary files /dev/null and b/doc/image/img-cmake-win10-config.png differ
diff --git a/doc/image/img-cmake-win10-create-build-folder.png b/doc/image/img-cmake-win10-create-build-folder.png
new file mode 100644
index 0000000..9c5cecf
Binary files /dev/null and b/doc/image/img-cmake-win10-create-build-folder.png differ
diff --git a/doc/image/img-cmake-win10-msvc-generate.png b/doc/image/img-cmake-win10-msvc-generate.png
new file mode 100644
index 0000000..5d99682
Binary files /dev/null and b/doc/image/img-cmake-win10-msvc-generate.png differ
diff --git a/doc/image/img-cmake-win10-msvc-launch.png b/doc/image/img-cmake-win10-msvc-launch.png
new file mode 100644
index 0000000..ef8ec9d
Binary files /dev/null and b/doc/image/img-cmake-win10-msvc-launch.png differ
diff --git a/doc/image/img-cmake-win10-msvc-version.png b/doc/image/img-cmake-win10-msvc-version.png
new file mode 100644
index 0000000..a56df1e
Binary files /dev/null and b/doc/image/img-cmake-win10-msvc-version.png differ
diff --git a/doc/image/img-cmake-win7-create-build-folder.jpg b/doc/image/img-cmake-win7-create-build-folder.jpg
index 2c07e4b..19606af 100644
Binary files a/doc/image/img-cmake-win7-create-build-folder.jpg and b/doc/image/img-cmake-win7-create-build-folder.jpg differ
diff --git a/doc/image/img-cmake-win7-generate.jpg b/doc/image/img-cmake-win7-generate.jpg
index 4beeaed..2c37483 100644
Binary files a/doc/image/img-cmake-win7-generate.jpg and b/doc/image/img-cmake-win7-generate.jpg differ
diff --git a/doc/image/img-cmake-win7-msvc-config-end.jpg b/doc/image/img-cmake-win7-msvc-config-end.jpg
index 54dee35..fade2b1 100644
Binary files a/doc/image/img-cmake-win7-msvc-config-end.jpg and b/doc/image/img-cmake-win7-msvc-config-end.jpg differ
diff --git a/doc/image/img-cmake-win7-msvc-config.jpg b/doc/image/img-cmake-win7-msvc-config.jpg
index acf85cd..26601bd 100644
Binary files a/doc/image/img-cmake-win7-msvc-config.jpg and b/doc/image/img-cmake-win7-msvc-config.jpg differ
diff --git a/doc/image/img-cmake-win7-msvc-launch.jpg b/doc/image/img-cmake-win7-msvc-launch.jpg
index 8a382d3..6a2e07b 100644
Binary files a/doc/image/img-cmake-win7-msvc-launch.jpg and b/doc/image/img-cmake-win7-msvc-launch.jpg differ
diff --git a/doc/image/img-cmake-win7-msvc-version.jpg b/doc/image/img-cmake-win7-msvc-version.jpg
index a480e0f..3fa3fcb 100644
Binary files a/doc/image/img-cmake-win7-msvc-version.jpg and b/doc/image/img-cmake-win7-msvc-version.jpg differ
diff --git a/doc/image/img-cmake-win7-opencv-advanced.jpg b/doc/image/img-cmake-win7-opencv-advanced.jpg
deleted file mode 100644
index 5c816a1..0000000
Binary files a/doc/image/img-cmake-win7-opencv-advanced.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win7-opencv.jpg b/doc/image/img-cmake-win7-opencv.jpg
deleted file mode 100644
index eb8f9bd..0000000
Binary files a/doc/image/img-cmake-win7-opencv.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win7-solution.jpg b/doc/image/img-cmake-win7-solution.jpg
index 2bb2ccd..34c2382 100644
Binary files a/doc/image/img-cmake-win7-solution.jpg and b/doc/image/img-cmake-win7-solution.jpg differ
diff --git a/doc/image/img-cmake-win8.1-4.jpg b/doc/image/img-cmake-win8.1-4.jpg
deleted file mode 100644
index 4c18a15..0000000
Binary files a/doc/image/img-cmake-win8.1-4.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-6.jpg b/doc/image/img-cmake-win8.1-6.jpg
deleted file mode 100644
index 8dbd4e7..0000000
Binary files a/doc/image/img-cmake-win8.1-6.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-config-end.jpg b/doc/image/img-cmake-win8.1-config-end.jpg
new file mode 100644
index 0000000..9e536f8
Binary files /dev/null and b/doc/image/img-cmake-win8.1-config-end.jpg differ
diff --git a/doc/image/img-cmake-win8.1-config.jpg b/doc/image/img-cmake-win8.1-config.jpg
new file mode 100644
index 0000000..777fa5e
Binary files /dev/null and b/doc/image/img-cmake-win8.1-config.jpg differ
diff --git a/doc/image/img-cmake-win8.1-create-build-folder.jpg b/doc/image/img-cmake-win8.1-create-build-folder.jpg
index 3de0224..2aa758b 100644
Binary files a/doc/image/img-cmake-win8.1-create-build-folder.jpg and b/doc/image/img-cmake-win8.1-create-build-folder.jpg differ
diff --git a/doc/image/img-cmake-win8.1-gdi.jpg b/doc/image/img-cmake-win8.1-gdi.jpg
deleted file mode 100644
index 03eb63c..0000000
Binary files a/doc/image/img-cmake-win8.1-gdi.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-configure-end.jpg b/doc/image/img-cmake-win8.1-mingw-configure-end.jpg
index 948f784..75cfaa8 100644
Binary files a/doc/image/img-cmake-win8.1-mingw-configure-end.jpg and b/doc/image/img-cmake-win8.1-mingw-configure-end.jpg differ
diff --git a/doc/image/img-cmake-win8.1-mingw-configure.jpg b/doc/image/img-cmake-win8.1-mingw-configure.jpg
index 925560a..846699b 100644
Binary files a/doc/image/img-cmake-win8.1-mingw-configure.jpg and b/doc/image/img-cmake-win8.1-mingw-configure.jpg differ
diff --git a/doc/image/img-cmake-win8.1-mingw-generate.jpg b/doc/image/img-cmake-win8.1-mingw-generate.jpg
index a6bf443..af6b7d0 100644
Binary files a/doc/image/img-cmake-win8.1-mingw-generate.jpg and b/doc/image/img-cmake-win8.1-mingw-generate.jpg differ
diff --git a/doc/image/img-cmake-win8.1-mingw-launch.jpg b/doc/image/img-cmake-win8.1-mingw-launch.jpg
deleted file mode 100644
index 277ae03..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-launch.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-opencv-detected-advanced.jpg b/doc/image/img-cmake-win8.1-mingw-opencv-detected-advanced.jpg
deleted file mode 100644
index 32ef3c2..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-opencv-detected-advanced.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-opencv-detected.jpg b/doc/image/img-cmake-win8.1-mingw-opencv-detected.jpg
deleted file mode 100644
index df0312f..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-opencv-detected.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-msvc-generate.jpg b/doc/image/img-cmake-win8.1-msvc-generate.jpg
index be57b42..83e03a0 100644
Binary files a/doc/image/img-cmake-win8.1-msvc-generate.jpg and b/doc/image/img-cmake-win8.1-msvc-generate.jpg differ
diff --git a/doc/image/img-cmake-win8.1-msvc-launch.jpg b/doc/image/img-cmake-win8.1-msvc-launch.jpg
index 59c8289..966d638 100644
Binary files a/doc/image/img-cmake-win8.1-msvc-launch.jpg and b/doc/image/img-cmake-win8.1-msvc-launch.jpg differ
diff --git a/doc/image/img-cmake-win8.1-opencv-advanced.jpg b/doc/image/img-cmake-win8.1-opencv-advanced.jpg
deleted file mode 100644
index c9fdbce..0000000
Binary files a/doc/image/img-cmake-win8.1-opencv-advanced.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-opencv.jpg b/doc/image/img-cmake-win8.1-opencv.jpg
deleted file mode 100644
index e49a416..0000000
Binary files a/doc/image/img-cmake-win8.1-opencv.jpg and /dev/null differ
diff --git a/doc/image/img-logo-visp.png b/doc/image/img-logo-visp.png
new file mode 100644
index 0000000..c5a3a96
Binary files /dev/null and b/doc/image/img-logo-visp.png differ
diff --git a/doc/image/img-mingw64-installer-finished.jpg b/doc/image/img-mingw64-installer-finished.jpg
deleted file mode 100644
index a38a755..0000000
Binary files a/doc/image/img-mingw64-installer-finished.jpg and /dev/null differ
diff --git a/doc/image/img-monkey-blured-default.png b/doc/image/img-monkey-blured-default.png
new file mode 100644
index 0000000..812d296
Binary files /dev/null and b/doc/image/img-monkey-blured-default.png differ
diff --git a/doc/image/img-monkey-blured-var2.png b/doc/image/img-monkey-blured-var2.png
new file mode 100644
index 0000000..efd67c4
Binary files /dev/null and b/doc/image/img-monkey-blured-var2.png differ
diff --git a/doc/image/img-monkey-canny.png b/doc/image/img-monkey-canny.png
new file mode 100644
index 0000000..9fef772
Binary files /dev/null and b/doc/image/img-monkey-canny.png differ
diff --git a/doc/image/img-monkey-dIxy.png b/doc/image/img-monkey-dIxy.png
new file mode 100644
index 0000000..ec00669
Binary files /dev/null and b/doc/image/img-monkey-dIxy.png differ
diff --git a/doc/image/img-monkey-gray.png b/doc/image/img-monkey-gray.png
new file mode 100644
index 0000000..349d78f
Binary files /dev/null and b/doc/image/img-monkey-gray.png differ
diff --git a/doc/image/img-monkey-pyr.png b/doc/image/img-monkey-pyr.png
new file mode 100644
index 0000000..dc6fd0e
Binary files /dev/null and b/doc/image/img-monkey-pyr.png differ
diff --git a/doc/image/img-monkey-sobel.png b/doc/image/img-monkey-sobel.png
new file mode 100644
index 0000000..c308ddb
Binary files /dev/null and b/doc/image/img-monkey-sobel.png differ
diff --git a/doc/image/img-monkey-win.jpg b/doc/image/img-monkey-win.jpg
new file mode 100755
index 0000000..2c8cd00
Binary files /dev/null and b/doc/image/img-monkey-win.jpg differ
diff --git a/doc/image/img-monkey.png b/doc/image/img-monkey.png
new file mode 100644
index 0000000..81bd2dc
Binary files /dev/null and b/doc/image/img-monkey.png differ
diff --git a/doc/image/img-opencv-issue-dshow.jpg b/doc/image/img-opencv-issue-dshow.jpg
deleted file mode 100644
index 4de4fd5..0000000
Binary files a/doc/image/img-opencv-issue-dshow.jpg and /dev/null differ
diff --git a/doc/image/img-started-win-run.jpg b/doc/image/img-started-win-run.jpg
index 018832e..6b93168 100644
Binary files a/doc/image/img-started-win-run.jpg and b/doc/image/img-started-win-run.jpg differ
diff --git a/doc/image/img-tracker-mb-visibility-ogre-advanced.png b/doc/image/img-tracker-mb-visibility-ogre-advanced.png
new file mode 100644
index 0000000..938796e
Binary files /dev/null and b/doc/image/img-tracker-mb-visibility-ogre-advanced.png differ
diff --git a/doc/image/img-tracker-mb-visibility-ogre.png b/doc/image/img-tracker-mb-visibility-ogre.png
new file mode 100644
index 0000000..d6c83b4
Binary files /dev/null and b/doc/image/img-tracker-mb-visibility-ogre.png differ
diff --git a/doc/image/img-tracker-mb-visibility-scanline.png b/doc/image/img-tracker-mb-visibility-scanline.png
new file mode 100644
index 0000000..a4e2ad2
Binary files /dev/null and b/doc/image/img-tracker-mb-visibility-scanline.png differ
diff --git a/doc/image/img-win10-cmake-error-sdk-missing.png b/doc/image/img-win10-cmake-error-sdk-missing.png
new file mode 100644
index 0000000..2bb185f
Binary files /dev/null and b/doc/image/img-win10-cmake-error-sdk-missing.png differ
diff --git a/doc/image/img-win10-explorer-install-end.png b/doc/image/img-win10-explorer-install-end.png
new file mode 100644
index 0000000..93164ce
Binary files /dev/null and b/doc/image/img-win10-explorer-install-end.png differ
diff --git a/doc/image/img-win10-msvc-build-end.png b/doc/image/img-win10-msvc-build-end.png
new file mode 100644
index 0000000..860d4cc
Binary files /dev/null and b/doc/image/img-win10-msvc-build-end.png differ
diff --git a/doc/image/img-win10-msvc-build.png b/doc/image/img-win10-msvc-build.png
new file mode 100644
index 0000000..9945b7b
Binary files /dev/null and b/doc/image/img-win10-msvc-build.png differ
diff --git a/doc/image/img-win10-msvc-install-end.png b/doc/image/img-win10-msvc-install-end.png
new file mode 100644
index 0000000..1e188be
Binary files /dev/null and b/doc/image/img-win10-msvc-install-end.png differ
diff --git a/doc/image/img-win10-msvc-install.png b/doc/image/img-win10-msvc-install.png
new file mode 100644
index 0000000..7220c34
Binary files /dev/null and b/doc/image/img-win10-msvc-install.png differ
diff --git a/doc/image/img-win10-msvc-open.png b/doc/image/img-win10-msvc-open.png
new file mode 100644
index 0000000..7c096c1
Binary files /dev/null and b/doc/image/img-win10-msvc-open.png differ
diff --git a/doc/image/img-win10-msvc-release.png b/doc/image/img-win10-msvc-release.png
new file mode 100644
index 0000000..d16c5c8
Binary files /dev/null and b/doc/image/img-win10-msvc-release.png differ
diff --git a/doc/image/img-win7-ViSP-images.jpg b/doc/image/img-win7-ViSP-images.jpg
index 7bc78d5..1917f62 100644
Binary files a/doc/image/img-win7-ViSP-images.jpg and b/doc/image/img-win7-ViSP-images.jpg differ
diff --git a/doc/image/img-win7-cmd-displayGDI.jpg b/doc/image/img-win7-cmd-displayGDI.jpg
index 0be1bb8..f98b235 100644
Binary files a/doc/image/img-win7-cmd-displayGDI.jpg and b/doc/image/img-win7-cmd-displayGDI.jpg differ
diff --git a/doc/image/img-win7-cygwin-git.jpg b/doc/image/img-win7-cygwin-git.jpg
new file mode 100644
index 0000000..58a6e31
Binary files /dev/null and b/doc/image/img-win7-cygwin-git.jpg differ
diff --git a/doc/image/img-win7-msvc-build-succeed.jpg b/doc/image/img-win7-msvc-build-succeed.jpg
index d894f88..7dfa5c8 100644
Binary files a/doc/image/img-win7-msvc-build-succeed.jpg and b/doc/image/img-win7-msvc-build-succeed.jpg differ
diff --git a/doc/image/img-win7-msvc-build.jpg b/doc/image/img-win7-msvc-build.jpg
index 0382d6d..066339a 100644
Binary files a/doc/image/img-win7-msvc-build.jpg and b/doc/image/img-win7-msvc-build.jpg differ
diff --git a/doc/image/img-win7-msvc-install-end.jpg b/doc/image/img-win7-msvc-install-end.jpg
index f11a3a1..3bfa47d 100644
Binary files a/doc/image/img-win7-msvc-install-end.jpg and b/doc/image/img-win7-msvc-install-end.jpg differ
diff --git a/doc/image/img-win7-msvc-install-succeed.jpg b/doc/image/img-win7-msvc-install-succeed.jpg
index 9701b64..84c74b2 100644
Binary files a/doc/image/img-win7-msvc-install-succeed.jpg and b/doc/image/img-win7-msvc-install-succeed.jpg differ
diff --git a/doc/image/img-win7-msvc-install.jpg b/doc/image/img-win7-msvc-install.jpg
index 5ded148..e40a5fc 100644
Binary files a/doc/image/img-win7-msvc-install.jpg and b/doc/image/img-win7-msvc-install.jpg differ
diff --git a/doc/image/img-win7-msvc-open.jpg b/doc/image/img-win7-msvc-open.jpg
index 957cf0c..bce38c2 100644
Binary files a/doc/image/img-win7-msvc-open.jpg and b/doc/image/img-win7-msvc-open.jpg differ
diff --git a/doc/image/img-win7-msvc-release.jpg b/doc/image/img-win7-msvc-release.jpg
index fec3a95..63b8cbd 100644
Binary files a/doc/image/img-win7-msvc-release.jpg and b/doc/image/img-win7-msvc-release.jpg differ
diff --git a/doc/image/img-win8.1-cmake-ogre.jpg b/doc/image/img-win8.1-cmake-ogre.jpg
new file mode 100644
index 0000000..79ffcc7
Binary files /dev/null and b/doc/image/img-win8.1-cmake-ogre.jpg differ
diff --git a/doc/image/img-win8.1-cmake-ogredeps-issue.jpg b/doc/image/img-win8.1-cmake-ogredeps-issue.jpg
new file mode 100644
index 0000000..501e8ab
Binary files /dev/null and b/doc/image/img-win8.1-cmake-ogredeps-issue.jpg differ
diff --git a/doc/image/img-win8.1-cmake-ogredeps.jpg b/doc/image/img-win8.1-cmake-ogredeps.jpg
new file mode 100644
index 0000000..21af130
Binary files /dev/null and b/doc/image/img-win8.1-cmake-ogredeps.jpg differ
diff --git a/doc/image/img-win8.1-cmd-displayGDI.jpg b/doc/image/img-win8.1-cmd-displayGDI.jpg
index a23766f..231862d 100644
Binary files a/doc/image/img-win8.1-cmd-displayGDI.jpg and b/doc/image/img-win8.1-cmd-displayGDI.jpg differ
diff --git a/doc/image/img-win8.1-cygwin-svn.jpg b/doc/image/img-win8.1-cygwin-svn.jpg
deleted file mode 100644
index 44ddfda..0000000
Binary files a/doc/image/img-win8.1-cygwin-svn.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-explorer-install-end.jpg b/doc/image/img-win8.1-explorer-install-end.jpg
index 95458c4..7133f83 100644
Binary files a/doc/image/img-win8.1-explorer-install-end.jpg and b/doc/image/img-win8.1-explorer-install-end.jpg differ
diff --git a/doc/image/img-win8.1-mingw-cmd-displayGDI.jpg b/doc/image/img-win8.1-mingw-cmd-displayGDI.jpg
new file mode 100644
index 0000000..a1d9626
Binary files /dev/null and b/doc/image/img-win8.1-mingw-cmd-displayGDI.jpg differ
diff --git a/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg b/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg
index 301047f..9868b95 100644
Binary files a/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg and b/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg differ
diff --git a/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg b/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg
index 3723af7..2aacbe5 100644
Binary files a/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg and b/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg differ
diff --git a/doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg b/doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg
new file mode 100644
index 0000000..d9aa073
Binary files /dev/null and b/doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg differ
diff --git a/doc/image/img-win8.1-msvc-build-end.jpg b/doc/image/img-win8.1-msvc-build-end.jpg
index 1cf3ef0..5772b34 100644
Binary files a/doc/image/img-win8.1-msvc-build-end.jpg and b/doc/image/img-win8.1-msvc-build-end.jpg differ
diff --git a/doc/image/img-win8.1-msvc-build-ogredeps.jpg b/doc/image/img-win8.1-msvc-build-ogredeps.jpg
new file mode 100644
index 0000000..cf3e409
Binary files /dev/null and b/doc/image/img-win8.1-msvc-build-ogredeps.jpg differ
diff --git a/doc/image/img-win8.1-msvc-build.jpg b/doc/image/img-win8.1-msvc-build.jpg
index e4ef2b2..398b848 100644
Binary files a/doc/image/img-win8.1-msvc-build.jpg and b/doc/image/img-win8.1-msvc-build.jpg differ
diff --git a/doc/image/img-win8.1-msvc-install-end.jpg b/doc/image/img-win8.1-msvc-install-end.jpg
index f26f698..208e0a0 100644
Binary files a/doc/image/img-win8.1-msvc-install-end.jpg and b/doc/image/img-win8.1-msvc-install-end.jpg differ
diff --git a/doc/image/img-win8.1-msvc-install.jpg b/doc/image/img-win8.1-msvc-install.jpg
index ed2f561..d2c3a8e 100644
Binary files a/doc/image/img-win8.1-msvc-install.jpg and b/doc/image/img-win8.1-msvc-install.jpg differ
diff --git a/doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg b/doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg
new file mode 100644
index 0000000..b5d51a9
Binary files /dev/null and b/doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg differ
diff --git a/doc/image/img-win8.1-msvc-mbtracker-ogre-opengl.jpg b/doc/image/img-win8.1-msvc-mbtracker-ogre-opengl.jpg
new file mode 100644
index 0000000..f182adc
Binary files /dev/null and b/doc/image/img-win8.1-msvc-mbtracker-ogre-opengl.jpg differ
diff --git a/doc/image/img-win8.1-msvc-ogredeps-issue.jpg b/doc/image/img-win8.1-msvc-ogredeps-issue.jpg
new file mode 100644
index 0000000..2c2e03b
Binary files /dev/null and b/doc/image/img-win8.1-msvc-ogredeps-issue.jpg differ
diff --git a/doc/image/img-win8.1-msvc-open.jpg b/doc/image/img-win8.1-msvc-open.jpg
index c7b8222..282605d 100644
Binary files a/doc/image/img-win8.1-msvc-open.jpg and b/doc/image/img-win8.1-msvc-open.jpg differ
diff --git a/doc/image/img-win8.1-msvc-release.jpg b/doc/image/img-win8.1-msvc-release.jpg
index 4a63729..476163a 100644
Binary files a/doc/image/img-win8.1-msvc-release.jpg and b/doc/image/img-win8.1-msvc-release.jpg differ
diff --git a/doc/image/img-win8.1-msvc-solution.jpg b/doc/image/img-win8.1-msvc-solution.jpg
index b09305b..ad854c3 100644
Binary files a/doc/image/img-win8.1-msvc-solution.jpg and b/doc/image/img-win8.1-msvc-solution.jpg differ
diff --git a/doc/image/img-win8.1-ogre-explorer.jpg b/doc/image/img-win8.1-ogre-explorer.jpg
new file mode 100644
index 0000000..efbf4ba
Binary files /dev/null and b/doc/image/img-win8.1-ogre-explorer.jpg differ
diff --git a/doc/image/img-win8.1-ogre-tortoisehg.jpg b/doc/image/img-win8.1-ogre-tortoisehg.jpg
new file mode 100644
index 0000000..6a5d82b
Binary files /dev/null and b/doc/image/img-win8.1-ogre-tortoisehg.jpg differ
diff --git a/doc/image/img-win8.1-ogredeps-explorer.jpg b/doc/image/img-win8.1-ogredeps-explorer.jpg
new file mode 100644
index 0000000..c77d60d
Binary files /dev/null and b/doc/image/img-win8.1-ogredeps-explorer.jpg differ
diff --git a/doc/image/img-win8.1-ogredeps-tortoisehg.jpg b/doc/image/img-win8.1-ogredeps-tortoisehg.jpg
new file mode 100644
index 0000000..e18955a
Binary files /dev/null and b/doc/image/img-win8.1-ogredeps-tortoisehg.jpg differ
diff --git a/doc/image/img-win8.1-visp-images.jpg b/doc/image/img-win8.1-visp-images.jpg
index ddccad8..b7596d9 100644
Binary files a/doc/image/img-win8.1-visp-images.jpg and b/doc/image/img-win8.1-visp-images.jpg differ
diff --git a/doc/mainpage.doc.in b/doc/mainpage.doc.in
index 83092b3..5605b5b 100644
--- a/doc/mainpage.doc.in
+++ b/doc/mainpage.doc.in
@@ -1,6 +1,6 @@
/*!
-\mainpage Online documentation for ViSP @VISP_VERSION@
+\mainpage ViSP @VISP_VERSION@ main page
\tableofcontents
@@ -11,20 +11,23 @@ servoing applications. ViSP is developed and maintained by the Inria
<a href="http://team.inria.fr/lagadic" target="_parent">Lagadic</a> team located at <a
href="http://www.inria.fr/en/centre/rennes" target="_parent">Inria Rennes</a>.</p>
-<p>ViSP official site is <a
-href="http://team.inria.fr/lagadic/visp" target="_parent">http://team.inria.fr/lagadic/visp</a></p>
+<p>ViSP official site is <a href="http://visp.inria.fr" target="_parent">http://visp.inria.fr</a></p>
+
+<p>ViSP wiki is <a href="https://github.com/lagadic/visp/wiki" target="_parent">https://github.com/lagadic/visp/wiki</a></p>
+
+<p>ViSP source code is now available on GitHub <a href="http://github.org/lagadic/visp" target="_parent">http://github.org/lagadic/visp</a></p>
-<p>If you have any problems or find any bugs, please report them at
-<a href="http://gforge.inria.fr/tracker/?group_id=397" target="_parent">http://gforge.inria.fr/tracker/?group_id=397</a>. If you may need help, please use the available forums <a href="http://gforge.inria.fr/forum/?group_id=397" target="_parent">http://gforge.inria.fr/forum/?group_id=397</a> or mailing lists <a href="http://gforge.inria.fr/mail/?group_id=397" target="_parent">http://gforge.inria.fr/mail/?group_id=397</a>.<p>
+<p>If you have any problems or find any bugs, please report them in
+<a href="https://github.com/lagadic/visp/issues">the bug tracker</a>. If you may need help, please use the available <a href="http://gforge.inria.fr/forum/?group_id=397" target="_parent">forums</a>.<p>
It is also possible to contact ViSP main developers using: <a href="mailto:visp at inria.fr?subject=[visp]">visp at inria.fr</a>
\section download_sec Download
<p>From <a
-href="http://team.inria.fr/lagadic/visp/download.html" target="_parent">http://team.inria.fr/lagadic/visp/download.html</a>
-you can either download the latest stable release or the current
-development distribution using Subversion.</p>
+href="http://visp.inria.fr/download" target="_parent">http://visp.inria.fr/download</a>
+you can either download the latest stable release, a daily snapshot or the current
+development distribution using git.</p>
\section install_sec Installation
@@ -35,10 +38,10 @@ href="http://www.cmake.org" target="_parent">http://www.cmake.org</a>. Furthermo
depending on your operation system and the capabilities (framegrabber,
display, simulation, ...) you need, prior to install ViSP you may
install third party libraries <a
-href="http://team.inria.fr/lagadic/visp/libraries.html" target="_parent">http://team.inria.fr/lagadic/visp/libraries.html</a>.</p>
+href="http://visp.inria.fr/software-architecture" target="_parent">http://visp.inria.fr/software-architecture</a>.</p>
<p>ViSP full installation procedure using CMake is detailed in the \ref tuto_install_sdk "Installation from prebuild SDK tutorials" and \ref tuto_install_src "Installation from source tutorials". \ref tuto_started "Getting started tutorials" and documents in pdf are also available from <a
-href="http://team.inria.fr/lagadic/visp/publication.html" target="_parent">http://team.inria.fr/lagadic/visp/publication.html</a>.</p>
+href="http://visp.inria.fr/publications" target="_parent">http://visp.inria.fr/publications</a>.</p>
\section tutorial Tutorials
@@ -50,14 +53,16 @@ href="http://team.inria.fr/lagadic/visp/publication.html" target="_parent">http:
\subsection tuto_install_src Installation from source
-- \ref tutorial-install-ubuntu <br>In this first tutorial you will learn how to install ViSP from source on Linux Ubuntu.
+- \ref tutorial-install-ubuntu <br>In this tutorial you will learn how to install ViSP from source on Linux Ubuntu.
- \ref tutorial-install-fedora <br>In this other tutorial you will learn how to install ViSP from source on Linux Fedora.
- \ref tutorial-install-centos <br>In this other tutorial you will learn how to install ViSP from source on Linux CentOS.
- \ref tutorial-install-opensuse <br>In this other tutorial you will learn how to install ViSP from source on Linux openSUSE.
- \ref tutorial-install-raspberry <br>In this tutorial you will learn how to install ViSP from source on Raspberry Pi.
- \ref tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012.
- \ref tutorial-install-win81-msvc <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++ 2013.
+- \ref tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015.
- \ref tutorial-install-win81-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64.
+- \ref tutorial-install-osx-homebrew <br>In this tutorial you will learn how to install ViSP from source on OSX with Homebrew.
- \ref tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
\subsection tuto_started Getting started
@@ -103,6 +108,10 @@ href="http://team.inria.fr/lagadic/visp/publication.html" target="_parent">http:
- \ref tutorial-simu-robot-pioneer <br>This tutorial focuses on visual servoing simulation on a unicycle robot. The study case is a Pioneer P3-DX mobile robot equipped with a camera.
- \ref tutorial-boost-vs <br>This tutorial explains how to speed up the time to convergence of a visual servo.
+\subsection tuto_bridge Bridges over other frameworks
+
+- \ref tutorial-bridge-opencv <br>This tutorial explicit how to convert OpenCV to/from ViSP structures such as camera parameters, images...
+
\subsection tuto_tools Other tools
- \ref tutorial-plotter <br>This tutorial explains how to plot curves in real-time during a visual servo.
@@ -122,423 +131,473 @@ scheme, ...</p>
<p>ViSP library is an open source C++ library which is developed at <a href="http://www.inria.fr/en/centre/rennes">Inria</a> by <a href="http://team.inria.fr/lagadic/welcome-eng.html">Lagadic team</a>. If you enjoy using ViSP, you may contribute to the project in different ways. This will motivate us to continue the efforts.
-- You can help VISP to be more widely known, by displaying a <a href="http://team.inria.fr/lagadic/visp/documentation/visp-flyers.pdf">ViSP flyer</a> at work, in your lab or school.
-- You can submit a <a href="https://gforge.inria.fr/tracker/?atid=1867&group_id=397&func=browse">bug report using the tracker</a>.
-- You can submit <a href="https://gforge.inria.fr/tracker/?func=browse&group_id=397&atid=1660">patches or new functionalities here</a>.
+- You can submit a <a href="https://github.com/lagadic/visp/issues">bug report using the tracker</a>.
+- You can submit patches or new functionalities using GitHub Pull Request mechanism.
- You can write new tutorials, new documentations or simply improve the existing documentation.
- If you just want to say you've been happy with the library, you can send us a postcard from your place, to the following address: <em>Inria Rennes Bretagne Atlantique, Lagadic team, Campus de Beaulieu, 35042 Rennes Cedex, FRANCE.</em>
-<p>You can also ask for help using either <a href="https://gforge.inria.fr/forum/?group_id=397">the french or the english forum</a>, or <a href="https://gforge.inria.fr/mail/?group_id=397">ViSP users mailing list</a>.
+<p>You can also ask for help using either <a href="https://gforge.inria.fr/forum/?group_id=397">the french or the english forum</a>.
*/
-/*
- ******************************************
- * ViSP capabilities
- ******************************************
- */
+
+/*******************************************
+ * Module core
+ *******************************************/
/*!
- \defgroup Capabilities ViSP capabilities
+ \defgroup module_core core: Core module
+ Core module content.
*/
-/*
- ******************************************
- * Data representation
- ******************************************
- */
/*!
- \ingroup Capabilities
- \defgroup DataRepresentation Data representation
+ \ingroup module_core
+ \defgroup group_core_camera Camera model
+ Camera model.
*/
/*!
- \ingroup DataRepresentation
- \defgroup ImageDataRepresentation Image
+ \ingroup module_core
+ \defgroup group_core_image Image manipulation
+ Image manipulation, including I/O from image files or videos.
*/
/*!
- \ingroup DataRepresentation
- \defgroup List List
+ \ingroup module_core
+ \defgroup group_core_math Mathematics
+ Mathematics.
*/
-/*
- ******************************************
- * Mathematics
- ******************************************
- */
/*!
- \ingroup Capabilities
- \defgroup Math Mathematics
+ \ingroup group_core_math
+ \defgroup group_core_robust Robust estimation
+ Robust estimation.
*/
/*!
- \ingroup Math
- \defgroup Matrix Matrices and vectors
+ \ingroup group_core_math
+ \defgroup group_core_random Random number generator
+ Random number generator.
*/
/*!
- \ingroup Math
- \defgroup Geometry Geometry
+ \ingroup group_core_math
+ \defgroup group_core_matrices Matrices and vectors
+ Matrices and vectors.
*/
/*!
- \ingroup Geometry
- \defgroup GeometryPlane Plane
+ \ingroup group_core_math
+ \defgroup group_core_transformations Tranformations
+ Transformations.
*/
/*!
- \ingroup Geometry
- \defgroup GeometryFeature Geometric feature (3D transformation and forward projection)
+ \ingroup group_core_math
+ \defgroup group_core_math_tools Other tools
+ Other tools.
*/
/*!
- \ingroup Math
- \defgroup Transformations Transformations
+ \ingroup group_core_math
+ \defgroup group_core_math_spline Spline
+ Spline.
*/
/*!
- \ingroup Math
- \defgroup Hinkley Hinkley's cumulative sum test
+ \ingroup group_core_math
+ \defgroup group_core_kalman Kalman filter
+ Kalman filter.
*/
/*!
- \ingroup Math
- \defgroup Robust Robust estimation
+ \ingroup module_core
+ \defgroup group_core_tools Tools
+ Tools.
*/
/*!
- \ingroup Math
- \defgroup Random Random number generator
+ \ingroup group_core_tools
+ \defgroup group_core_moments Image moments
+ Image moments.
*/
/*!
- \ingroup Math
- \defgroup MathTools Other tools
+ \ingroup group_core_tools
+ \defgroup group_core_gui Generic GUI
+ Generic graphical user interface tools.
*/
-
/*!
- \ingroup Transformations
- \defgroup Transformation 3D transformations
+ \ingroup group_core_tools
+ \defgroup group_core_trackers Generic tracker
+ Generic tracker tools.
*/
/*!
- \ingroup Transformation
- \defgroup RotTransformation Rotation and their representation
+ \ingroup group_core_tools
+ \defgroup group_core_geometry 2D and 3D geometry
+ 2D and 3D geometry.
*/
/*!
- \ingroup Transformation
- \defgroup TransTransformation Translation
+ \ingroup group_core_tools
+ \defgroup group_core_bridges Bridges over other frameworks
+ Conversion from/to OpenCV...
*/
/*!
- \ingroup Transformation
- \defgroup PoseTransformation Pose
+ \ingroup group_core_tools
+ \defgroup group_core_mutex Mutex
+ Mutex.
*/
/*!
- \ingroup Transformation
- \defgroup HomographyTransformation Homography
+ \ingroup group_core_tools
+ \defgroup group_core_debug Debug and exceptions
+ Debug and exceptions.
*/
/*!
- \ingroup Transformation
- \defgroup TwistTransformation Twist transformation matrix
+ \ingroup group_core_tools
+ \defgroup group_core_network Networking
+ Networking.
*/
/*!
- \ingroup Transformation
- \defgroup ExpMapTransformation Exponential map
+ \ingroup group_core_tools
+ \defgroup group_core_histogram Histogram
+ Histogram.
*/
/*!
- \ingroup Transformations
- \defgroup VisionTransformation Other related transformations
+ \ingroup group_core_tools
+ \defgroup group_core_files_io Files and directories
+ Files and directories.
*/
/*!
- \ingroup VisionTransformation
- \defgroup CameraModelTransformation Camera models
+ \ingroup group_core_tools
+ \defgroup group_core_time Time management
+ Time management.
*/
-/*
- ******************************************
- * Image
- ******************************************
- */
-/*!
- \ingroup Capabilities
- \defgroup Image Image
-*/
+/*******************************************
+ * Module io
+ *******************************************/
/*!
- \ingroup Image
- \defgroup ImageManipulation Image manipulation
+ \defgroup module_io io: I/O module
+ I/O module content.
*/
/*!
- \ingroup ImageManipulation
- \defgroup ImageContainer Image representation
+ \ingroup module_io
+ \defgroup group_io_image Image I/O
+ Image reading and writing.
*/
/*!
- \ingroup ImageManipulation
- \defgroup ImageRW Reading and writing images
+ \ingroup module_io
+ \defgroup group_io_video Video I/O
+ Video reading and writing.
*/
/*!
- \ingroup ImageManipulation
- \defgroup ImageGUI Graphical user interface
+ \ingroup module_io
+ \defgroup group_io_keyboard Keyboard I/O
+ Keyboard management.
*/
/*!
- \ingroup ImageManipulation
- \defgroup ImageConversion Image conversion
+ \ingroup module_io
+ \defgroup module_io_cmd_parser Command line parser I/O
+ Command line parser.
*/
/*!
- \ingroup ImageManipulation
- \defgroup ImageTool Image tools
-*/
-/*!
- \ingroup ImageManipulation
- \defgroup Framegrabber Image acquisition
+ \ingroup module_io
+ \defgroup group_io_parallel_port Parallel port I/O
+ Parallel port I/O.
*/
+/*******************************************
+ * Module visual_features
+ *******************************************/
/*!
- \ingroup Image
- \defgroup ImageProcessing Image processing
-*/
-/*!
- \ingroup ImageProcessing
- \defgroup ImageFiltering Image filtering
+ \defgroup module_visual_features visual_features: Visual features module
+ Visual features module content.
*/
/*!
- \ingroup ImageProcessing
- \defgroup Histogram Image histogram
+ \ingroup module_visual_features
+ \defgroup group_visual_features Visual features
+ Visual features.
*/
/*!
- \ingroup Image
- \defgroup ImageSimulator Image simulator
+ \ingroup module_visual_features
+ \defgroup group_visual_features_builder Visual features builder
+ Visual features builder.
*/
-
-/*
- ******************************************
- * Tracking
- ******************************************
- */
-/*!
- \ingroup Capabilities
- \defgroup Tracking Tracking
-*/
-/*!
- \ingroup Tracking
- \defgroup TrackingFeature Tracking features (3D transformation and forward projection)
-*/
-/*!
- \ingroup Tracking
- \defgroup TrackingMoments Tracking 2D image moments
-*/
-/*!
- \ingroup Tracking
- \defgroup TrackingImage Tracking in image sequences
-*/
+/*******************************************
+ * Module gui
+ *******************************************/
/*!
- \ingroup TrackingImage
- \defgroup TrackingImageBasic Basic algorithms
+ \defgroup module_gui gui: Graphical user interface module
+ Graphical user interface module content.
*/
/*!
- \ingroup TrackingImage
- \defgroup TrackingImagePoint Point tracker
+ \ingroup module_gui
+ \defgroup group_gui_display Display device
+ Display device interface.
*/
/*!
- \ingroup TrackingImage
- \defgroup TrackingImageME Moving edges tracker
+ \ingroup module_gui
+ \defgroup group_gui_plotter Drawing of 2D or 3D graphics
+ Drawing of 2D or 3D graphics.
*/
/*!
- \ingroup TrackingImage
- \defgroup ModelBasedTracking Model based tracker
+ \ingroup module_gui
+ \defgroup group_gui_projection Forward projection
+ Forward projection interface.
*/
-/*
- ******************************************
- * Computer vision
- ******************************************
- */
+
+/*******************************************
+ * Module ar
+ *******************************************/
/*!
- \ingroup Capabilities
- \defgroup ComputerVision Computer Vision
+ \defgroup module_ar ar: Augmented reality module
+ Augmented reality module content.
*/
/*!
- \ingroup ComputerVision
- \defgroup Calibration Calibration
+ \ingroup module_ar
+ \defgroup group_ar_renderer Renderer
+ Renderer interfaces.
*/
/*!
- \ingroup ComputerVision
- \defgroup CameraModel Camera models
+ \ingroup module_ar
+ \defgroup group_ar_simulator Simulator
+ Simulator interfaces.
*/
+
+/*******************************************
+ * Module detection
+ *******************************************/
/*!
- \ingroup ComputerVision
- \defgroup Homography Homography estimation
+ \defgroup module_detection detection: Detection module
+ Detection module content.
*/
/*!
- \ingroup ComputerVision
- \defgroup PlanarSurfaceDetector Planar surface detector
+ \ingroup module_detection
+ \defgroup group_detection_barcode Bar code detection
+ Bar code detection.
*/
/*!
- \ingroup ComputerVision
- \defgroup Pose Pose estimation
+ \ingroup module_detection
+ \defgroup group_detection_face Face detection
+ Face detection.
*/
-/*
- ******************************************
- * Building a visual feature task
- ******************************************
+
+/*******************************************
+ * Module tracker
+ *******************************************/
+ /*!
+ \defgroup module_tracker tracker: ViSP trackers
+ ViSP trackers.
*/
+
+/*******************************************
+ * Module mbt
+ *******************************************/
/*!
- \ingroup Capabilities
- \defgroup Vs Building a visual servoing task
+ \ingroup module_tracker
+ \defgroup module_mbt mbt: Model-based tracker module
+ Model-based tracker module content.
*/
/*!
- \ingroup Vs
- \defgroup VsFeature Visual features
+ \ingroup module_mbt
+ \defgroup group_mbt_trackers Trackers
+ Model-based trackers.
*/
/*!
- \ingroup VsFeature
- \defgroup VsFeature2 2D visual features
+ \ingroup module_mbt
+ \defgroup group_mbt_features Features
+ Model-based trackers features.
*/
/*!
- \ingroup VsFeature
- \defgroup VsFeature3 3D visual features
+ \ingroup module_mbt
+ \defgroup group_mbt_faces Faces management
+ Faces management including visibility.
*/
/*!
- \ingroup VsFeature
- \defgroup VsFeatureGeneric Generic visual features
+ \ingroup module_mbt
+ \defgroup group_mbt_xml_parser XML parsers
+ XML parsers dedicated to model-based trackers.
*/
+/*******************************************
+ * Module robot
+ *******************************************/
/*!
- \ingroup VsFeature
- \defgroup VsFeatureDisplay Display of visual features
+ \defgroup module_robot robot: Robot module
+ Robot module content.
*/
/*!
- \ingroup VsFeature
- \defgroup VsFeatureBuilder Visual features builder
+ \ingroup module_robot
+ \defgroup group_robot_real Real robots
+ Real robots interfaces.
*/
/*!
- \ingroup Vs
- \defgroup VsTask Task and control laws
+ \ingroup group_robot_real
+ \defgroup group_robot_real_gantry Gantry robot
+ Afma6 gantry robot interfaces.
*/
-
-/*
- ******************************************
- * Working with robots
- ******************************************
- */
/*!
- \ingroup Capabilities
- \defgroup Robot Working with robots
+ \ingroup group_robot_real
+ \defgroup group_robot_real_cylindrical Cylindrical robot
+ Afma4 cylindrical robot interfaces.
*/
/*!
- \ingroup Robot
- \defgroup RobotSimu Robot simulation
+ \ingroup group_robot_real
+ \defgroup group_robot_real_unicycle Unicycle robot
+ Pioneer P3DX unicycle robot interfaces.
*/
/*!
- \ingroup RobotSimu
- \defgroup RobotSimuWithVisu Simulation with 3D visualization
+ \ingroup group_robot_real
+ \defgroup group_robot_real_ptu Pan-tilt unit
+ Pan-tilt unit interfaces.
*/
/*!
- \ingroup RobotSimu
- \defgroup RobotSimuWithoutVisu Simulation without visualization.
+ \ingroup group_robot_real
+ \defgroup group_robot_real_arm Anthropomorphic arm
+ Anthropomorphic arm interfaces.
*/
-
/*!
- \ingroup Robot
- \defgroup RealRobot Real robots
+ \ingroup group_robot_real
+ \defgroup group_robot_real_template New robot interface (template)
+ Template for users that want to integrate their own real-robot.
*/
/*!
- \ingroup RealRobot
- \defgroup Afma4 Afma4 cylindrical robot
+ \ingroup module_robot
+ \defgroup group_robot_simu Robot simulation
+ Simulated robots interfaces.
*/
/*!
- \ingroup RealRobot
- \defgroup Afma6 Afma6 gantry robot
+ \ingroup group_robot_simu
+ \defgroup group_robot_simu_gantry Gantry robot simulation
+ Gantry robot interfaces.
*/
/*!
- \ingroup RealRobot
- \defgroup Biclops Biclops pan-tilt head
+ \ingroup group_robot_simu
+ \defgroup group_robot_simu_arm Anthropomorphic arm simulation
+ Anthropomorphic arm interfaces.
*/
/*!
- \ingroup RealRobot
- \defgroup Ptu46 Ptu-46 pan-tilt head
+ \ingroup group_robot_simu
+ \defgroup group_robot_simu_unicycle Unicycle robot simulation
+ Pioneer P3DX unicycle robot interfaces.
*/
/*!
- \ingroup RealRobot
- \defgroup Viper Adept Viper 6 dof robot
+ \ingroup group_robot_simu
+ \defgroup group_robot_simu_camera Free flying camera simulation
+ Free flying camera interfaces.
*/
/*!
- \ingroup RealRobot
- \defgroup Cycab Cycab car-like mobile robot
+ \ingroup module_robot
+ \defgroup group_robot_image_simu Image simulation
+ Image simulation.
*/
+
+/*******************************************
+ * Module sensor
+ *******************************************/
/*!
- \ingroup RealRobot
- \defgroup Pioneer Pioneer mobile robot
+ \defgroup module_sensor sensor: Sensor module
+ Sensor module content. It provides bridges over drivers to acquire data from some sensors.
*/
-/*
- ******************************************
- * Tools
- ******************************************
- */
/*!
- \ingroup Capabilities
- \defgroup Tool Tools
+ \ingroup module_sensor
+ \defgroup group_sensor_laserscanner Laser scanner
+ Laser scanner interfaces.
*/
-/*!
- \ingroup Tool
- \defgroup ArgumentParser Command line parsing
+/*!
+ \ingroup module_sensor
+ \defgroup group_sensor_camera Camera
+ Camera interfaces.
*/
-/*!
- \ingroup Tool
- \defgroup Mutex Mutex
+/*!
+ \ingroup module_sensor
+ \defgroup group_sensor_rgbd RGB-D sensor
+ RGB-D sensor interfaces.
*/
+/*******************************************
+ * Module tt
+ *******************************************/
/*!
- \ingroup Tool
- \defgroup Network Network
+ \ingroup module_tracker
+ \defgroup module_tt tt: Template tracker module
+ Template tracker module content.
*/
/*!
- \ingroup Tool
- \defgroup plot Drawing of 2D or 3D graphics
+ \ingroup module_tt
+ \defgroup group_tt_tracker Trackers
+ Classes dedicated to template tracking.
*/
/*!
- \ingroup Tool
- \defgroup System System
+ \ingroup module_tt
+ \defgroup group_tt_warp Warping function
+ Classes that implement warping functions that are estimated by the template trackers.
*/
/*!
- \ingroup System
- \defgroup Time Time management
+ \ingroup module_tt
+ \defgroup group_tt_tools Tools
+ Tools used by template trackers.
*/
-/*!
- \ingroup System
- \defgroup ParallelPort Parallel port
+/*******************************************
+ * Module tt_mi
+ *******************************************/
+ /*!
+ \ingroup module_tracker
+ \defgroup module_tt_mi tt_mi: Template tracker based on mutual information module
+ Template tracker based on mutual information module content. This module is an
+ extension of \ref module_tt where mutual information cost function is introduced.
*/
/*!
- \ingroup System
- \defgroup Keyboard Keyboard
+ \ingroup module_tt_mi
+ \defgroup group_tt_mi_tracker Trackers
+ Classes dedicates to template tracking with mutual information cost function.
*/
-/*!
- \ingroup Tool
- \defgroup IO Input/output
+/*******************************************
+ * Module me
+ *******************************************/
+ /*!
+ \ingroup module_tracker
+ \defgroup module_me me: Moving-edges tracker module
+ Moving-edges tracker module.
*/
-/*!
- \ingroup IO
- \defgroup FileDirectories File and directories
+/*******************************************
+ * Module blob
+ *******************************************/
+ /*!
+ \ingroup module_tracker
+ \defgroup module_blob blob: Blob tracker module
+ Blob tracker module.
*/
-/*!
- \ingroup Tool
- \defgroup DebugException Debug and exceptions
+/*******************************************
+ * Module klt
+ *******************************************/
+ /*!
+ \ingroup module_tracker
+ \defgroup module_klt klt: Kanade Lucas tracker module
+ Kanade Lucas tracker module.
*/
-/*!
- \ingroup DebugException
- \defgroup Debug Debug
+/*******************************************
+ * Module vision
+ *******************************************/
+ /*!
+ \defgroup module_vision vision: Computer vision module
+ Computer vision module content.
*/
/*!
- \ingroup DebugException
- \defgroup Exception Exceptions
+ \ingroup module_vision
+ \defgroup group_vision_calib Camera calibration
+ Camera calibration algorithms.
*/
-
-/*
- ******************************************
- * Drivers
- ******************************************
- */
/*!
- \ingroup Capabilities
- \defgroup Driver Drivers
+ \ingroup module_vision
+ \defgroup group_vision_homography Homography estimation
+ Homography estimation algorithms.
*/
/*!
- \ingroup Driver
- \defgroup RobotDriver Robot drivers
+ \ingroup module_vision
+ \defgroup group_vision_pose Pose estimation
+ Pose estimation algorithms.
*/
/*!
- \ingroup Driver
- \defgroup CameraDriver Camera drivers
+ \ingroup module_vision
+ \defgroup group_vision_keypoints Keypoints detection, matching and classifiers
+ Keypoints detection and matching algorithms but also classifiers.
*/
-/*!
- \ingroup Driver
- \defgroup LaserDriver Laser scanner drivers
+/*******************************************
+ * Module vs
+ *******************************************/
+ /*!
+ \defgroup module_vs vs: Visual servoing module
+ Visual servoing module content.
*/
/*!
- \ingroup Driver
- \defgroup KinectDriver Kinect driver
+ \ingroup module_vs
+ \defgroup group_task Task and control laws
+ Classes used to define task and control laws.
*/
diff --git a/doc/tutorial-bridge-opencv.doc b/doc/tutorial-bridge-opencv.doc
new file mode 100644
index 0000000..e7c4993
--- /dev/null
+++ b/doc/tutorial-bridge-opencv.doc
@@ -0,0 +1,114 @@
+/**
+
+\page tutorial-bridge-opencv Tutorial: Bridge over OpenCV
+\tableofcontents
+
+ViSP is interfaced with OpenCV third party. In this tutorial we explain how to convert data such as camera parameters or images from ViSP to OpenCV or \e vice \e versa.
+
+\section tutorial_bridge_opencv_cam Camera parameters conversions
+
+ViSP camera parameters are implemented in vpCameraParameters class. If you want to calibrate a camera with ViSP tools follow \ref tutorial-calibration.
+
+Let us recall the pinhole camera model implemented in ViSP. In this model, a scene view is formed by projecting 3D points into the image plane using a perspective transformation.
+
+
+\f[
+ \left[ \begin{array}{c}
+ u \\
+ v \\
+ 1
+ \end{array}\right] =
+ \left[ \begin{array}{ccc}
+ u_0 & 0 & p_x \\
+ 0 & v_0 & p_y \\
+ 0 & 0 & 1
+ \end{array}\right]
+ \left[ \begin{array}{c}
+ X_c \\
+ Y_c \\
+ Z_c
+ \end{array}\right]
+ \f]
+
+where:
+
+- \f$(X_c,Y_c,Z_c)\f$ are the coordinates of a 3D point in the camera frame
+- \f$(u,v)\f$ are the coordinates in pixels of the projected 3D point
+- \f$(u_0,v_0)\f$ is a principal point that is usually near the image center
+- \f$(p_x,p_y)\f$ are the focal lengths expressed in pixel units.
+
+When \f$Z_c \neq 0\f$, the previous equation si equivalent to the following:
+\f[
+ \begin{array}{lcl}
+ x &=& X_c / Z_c \\
+ y &=& Y_c / Z_c \\
+ u &=& u_0 + x \; p_x \\
+ v &=& v_0 + y \; p_y
+ \end{array}
+ \f]
+
+Real lenses usually have some radial distortion. So, the above model is extended as:
+
+\f[
+ \begin{array}{lcl}
+ x &=& X_c / Z_c \\
+ y &=& Y_c / Z_c \\
+ x^{'} &=& x (1 + k_{ud} r^2) \\
+ y^{'} &=& y (1 + k_{ud} r^2) \\
+ r^2 &=& x^2 + y^2 \\
+ u &=& u_0 + x^{'} \; p_x \\
+ v &=& v_0 + y^{'} \; p_y
+ \end{array}
+ \f]
+
+where \f$k_{ud}\f$ is the first order radial distorsion. Higher order distorsion coefficients are not considered in ViSP.
+
+\note In ViSP we introduce an extra parameter named \f$k_{du}\f$ which is the radial first order distorsion that allows to transform pixels in meters. If this parameter is unknown as in OpenCV a good approximation is to consider \f$k_{du} = - k_{ud}\f$
+
+Even if OpenCV notations are different, this model is exactly the same then the one used in OpenCV and described <a href="http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html">here</a> where higher order OpenCV distorsion parameters are turned to 0.
+
+The following table gives the correspondances between ViSP and OpenCV parameters:
+
+
+\f[
+ \begin{array}{l|l}
+ ViSP & OpenCV \\ \hline
+ u_0 & c_x\\
+ v_0 & c_y \\
+ p_x & f_x \\
+ p_y & f_y \\
+ k_{ud} & k_1 \\
+ k_{du} & -k_1 \\
+ & k_2 = 0 \\
+ & k_3 = 0 \\
+ & k_4 = 0 \\
+ & k_5 = 0 \\
+ & k_6 = 0 \\
+ & p_1 = 0 \\
+ & p_2 = 0
+ \end{array}
+ \f]
+
+From a coding point of view, let us consider the following code also available in tutorial-bridge-opencv.cpp where we initialize camera parameters using ViSP:
+
+\snippet tutorial-bridge-opencv.cpp Set ViSP camera parameters
+
+These parameters could be used to initialize OpenCV camera parameters:
+
+\snippet tutorial-bridge-opencv.cpp Set OpenCV camera parameters
+
+\section tutorial_bridge_opencv_image Image conversions
+
+ViSP image is implemented in vpImage class, while OpenCV images in cv::Mat class. All the functions that allow image conversion from ViSP to OpenCV or \e vice \e versa are implemented in vpImageConvert.
+
+Some examples are available in tutorial-bridge-opencv.cpp.
+
+For example, the following code allow to read an image with ViSP:
+
+\snippet tutorial-bridge-opencv.cpp Load ViSP image
+
+and then convert the image in OpenCV using:
+
+\snippet tutorial-bridge-opencv.cpp Convert ViSP 2 OpenCV image
+
+**/
\ No newline at end of file
diff --git a/doc/tutorial-calibration.doc b/doc/tutorial-calibration.doc
index f4d4bca..d9bb20a 100644
--- a/doc/tutorial-calibration.doc
+++ b/doc/tutorial-calibration.doc
@@ -33,7 +33,9 @@ In this model we consider the parameters \f$(u_0,v_0,p_x,p_y, k_{ud}, k_{du})\f$
- \f$(p_x, p_y)\f$ are the ratio between the focal length and the size of a pixel;
- \f$(k_{ud}, k_{du})\f$ are the parameters used to correct the distortion.
-Note that the container dedicated to camera parameters is implemented in the vpCameraParameters class. It allows to consider two kind of models; with or without distortion.
+\note It may be useful to make a tour in \ref tutorial-bridge-opencv that makes in relation the camera model used in ViSP with the one proposed by OpenCV.
+
+\note Note also that the container dedicated to camera parameters is implemented in the vpCameraParameters class. It allows to consider two kind of models; with or without distortion.
The calibration process allows to estimate the values of these parameters. To this end, one of the following calibration grid can be used:
- a black and white chessboard [<a href="http://www.irisa.fr/lagadic/visp/img/OpenCV_Chessboard.pdf">OpenCV_Chessboard.pdf</a>];
diff --git a/doc/tutorial-detection-object.doc b/doc/tutorial-detection-object.doc
index 553c952..08fc960 100644
--- a/doc/tutorial-detection-object.doc
+++ b/doc/tutorial-detection-object.doc
@@ -35,7 +35,12 @@ The next section presents an example of the detection and the pose estimation of
\subsection detection_object_teabox_example Teabox detection and pose estimation
-The following example comes from tutorial-detection-object-mbt.cpp and shows the different steps to detect and get the pose of a teabox.
+The following video shows the resulting detection and localization of a teabox that is learned on the first image of the video.
+
+\htmlonly<iframe width="420" height="315" src="https://www.youtube.com/embed/3rCvq5_UJvw" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+
+The corresponding code is available in tutorial-detection-object-mbt.cpp. It contains the different steps to learn the teabox object on one image (the first image of the video) and then detect and get the pose of the teabox in the rest of the video.
\include tutorial-detection-object-mbt.cpp
@@ -118,8 +123,91 @@ Now, for the Ransac pose estimation part :
- basically, a Ransac method is composed of two steps repeated a certain number of iterations: first we pick randomly 4 points and estimate the pose, the second step is to keep all points which sufficienly "agree" (the reprojection error is below a threshold) with the pose determinated in the first step. These points are inliers and form the consensus set, the other are outliers.
If enough points are in the consensus set (here 20 % of all the points), the pose is refined and returned, otherwise another iteration is made (here 200 iterations maximum).
-Below you will also find the content of detection-config.xml configuration file, also provided in this example. It allows to use FAST detector and ORB extractor.
+Below you will also find the content of detection-lconfig.xml configuration file, also provided in this example. It allows to use FAST detector and ORB extractor.
\include detection-config.xml
+\section detection_object_additional_functionalities Additional functionalities
+
+\subsection detection_object_multiple_learning How to learn keypoints from multiple images
+
+The following video shows an extension of the previous example where here we learn a cube from 3 images and then detect an localize the cube in all the images of the video.
+
+\htmlonly
+<iframe width="420" height="315" src="https://www.youtube.com/embed/7akuCtViznE" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+
+The corresponding source code is given in tutorial-detection-object-mbt2.cpp. If you have a look on this file you will find the following.
+
+Before starting with the keypoints detection and learning part, we have to set the correct pose for the tracker using a predefined pose:
+
+\snippet tutorial-detection-object-mbt2.cpp Set tracker pose
+
+One good thing to do is to refine the pose by running one iteration of the model-based tracker:
+
+\snippet tutorial-detection-object-mbt2.cpp Refine pose
+
+The vpKeyPoint::buildReference() allows to append the current detected keypoints with those already present by setting the function parameter append to true.
+
+But before that, the same learning procedure must be done in order to train on multiple images. We detect keypoints on the desired image:
+
+\snippet tutorial-detection-object-mbt2.cpp Keypoints reference detection
+
+Then, we keep only keypoints that are located on the object faces:
+
+\snippet tutorial-detection-object-mbt2.cpp Keypoints selection on faces
+
+And finally, we build the reference keypoints and we set the flag append to true to say that we want to keep the previously learned keypoints:
+
+\snippet tutorial-detection-object-mbt2.cpp Keypoints build reference
+
+\subsection detection_object_display_multiple_images How to display the matching when the learning is done on multiple images
+
+In this section we will explain how to display the matching between keypoints detected in the current image and their correspondances in the reference images that are used during the learning stage, as given in the next video:
+
+\htmlonly
+<iframe width="420" height="315" src="https://www.youtube.com/embed/q16lXMIVDbM" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+
+\warning If you want to load the learning data from a file, you have to use a learning file that contains training images (with the parameter saveTrainingImages vpKeyPoint::saveLearningData() set to true when saving the file, by default it is).
+
+Before showing how to display the matching for all the training images, we have to attribute an unique identifier (a positive integer) for the set of keypoints learned for a particular image during the training process:
+
+\snippet tutorial-detection-object-mbt2.cpp Keypoints build reference
+
+It permits to link the training keypoints with the correct corresponding training image.
+
+After that, the first thing to do is to create the image that will contain the keypoints matching with:
+
+\snippet tutorial-detection-object-mbt2.cpp Create image matching
+
+The previous line allows to allocate an image with the correct size according to the number of training images used.
+
+Then, we have to update for each new image the matching image with the current image:
+
+\snippet tutorial-detection-object-mbt2.cpp Insert image matching
+
+\note The current image will be inserted preferentially at the center of the matching image if it possible.
+
+And to display the matching we use:
+
+\snippet tutorial-detection-object-mbt2.cpp Display image matching
+
+We can also display the RANSAC inliers / outliers in the current image and in the matching image:
+
+\snippet tutorial-detection-object-mbt2.cpp Display RANSAC inliers
+
+\snippet tutorial-detection-object-mbt2.cpp Display RANSAC outliers
+
+The following code shows how to retrieve the RANSAC inliers and outliers:
+
+\snippet tutorial-detection-object-mbt2.cpp Get RANSAC inliers outliers
+
+Finally, we can also display the model in the matching image. For that, we have to modify the principal point offset of the intrinsic parameter.
+This is more or less an hack as you have to manually change the principal point coordinate to make it works.
+
+\snippet tutorial-detection-object-mbt2.cpp Display model image matching
+
+\note You can refer to the full code in the section \ref detection_object_multiple_learning to have an example of how to learn from multiple images and how to display all the matching.
+
*/
diff --git a/doc/tutorial-getting-started-iOS.doc b/doc/tutorial-getting-started-iOS.doc
index c36ea6c..cd1ff55 100644
--- a/doc/tutorial-getting-started-iOS.doc
+++ b/doc/tutorial-getting-started-iOS.doc
@@ -43,9 +43,9 @@ Here is the detailed explanation of the source, line by line :
\code
#include <vector>
-#include <visp/vpConfig.h>
-#include <visp/vpHomography.h>
-#include <visp/vpMeterPixelConversion.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMeterPixelConversion.h>
\endcode
Include all the headers for homography computation in ViewController.mm.
diff --git a/doc/tutorial-getting-started.doc b/doc/tutorial-getting-started.doc
index 2a51888..7d386ce 100644
--- a/doc/tutorial-getting-started.doc
+++ b/doc/tutorial-getting-started.doc
@@ -3,17 +3,7 @@
\page tutorial-getting-started Tutorial: How to create and build a CMake project that uses ViSP on Unix or Windows
\tableofcontents
-\note We assume in this tutorial that you have successfully installed ViSP. Information on ViSP installation is provided in:
-- \ref tutorial-install-ubuntu
-- \ref tutorial-install-fedora
-- \ref tutorial-install-opensuse
-- \ref tutorial-install-raspberry
-- \ref tutorial-install-win7
-- \ref tutorial-install-win81-msvc
-- \ref tutorial-install-win81-mingw64
-- \ref tutorial-install-iOS
-- \ref tutorial-install-ubuntu-package
-- or on <a href="http://www.irisa.fr/lagadic/visp/install.html">Install from source page</a>
+\note We assume in this tutorial that you have successfully installed ViSP either with an \ref tuto_install_sdk or with an \ref tuto_install_src.
In this tutorial you will learn how to use ViSP either on Unix-like systems (including OSX, Fedora, Ubuntu, Debian, ...) or on Windows.
@@ -148,17 +138,17 @@ make
By now you should have an executable called \c tutorial-viewer. You just have to run it giving an image location as an argument:
\code
-./tutorial_viewer lena.ppm
+./tutorial_viewer monkey.ppm
\endcode
Here is a screen shot of the resulting output window :
- \image html img-lena.png
+ \image html img-monkey.png
\section image_win On Windows
-We suppose from now, that you have created a folder (let say \c C:/ViSP/ViSP-started) that contains the following files: CMakeLists.txt, tutorial_viewer.cpp and lena.ppm. These files are also provider in ViSP source code, in \c tutorial/image folder.
+We suppose from now, that you have created a folder (let say \c C:/ViSP/ViSP-started) that contains the following files: CMakeLists.txt, tutorial_viewer.cpp and monkey.ppm. These files are also provider in ViSP source code, in \c tutorial/image folder.
\subsection image_win_config Configure your project
@@ -202,8 +192,9 @@ We suppose from now, that you have created a folder (let say \c C:/ViSP/ViSP-sta
- Here is a screen shot of the resulting output window :
- \image html img-lena-win.jpg
+ \image html img-monkey-win.jpg
+\subsection image_next Next tutorial
You are now ready to see the \ref tutorial-grabber or \ref tutorial-image-filtering.
*/
diff --git a/doc/tutorial-grabber.doc b/doc/tutorial-grabber.doc
index 3c2cf4d..eefc950 100644
--- a/doc/tutorial-grabber.doc
+++ b/doc/tutorial-grabber.doc
@@ -4,7 +4,7 @@
\tableofcontents
- \section grabber-camera-firewire-unix Images from firewire cameras
+\section grabber-camera-firewire-unix Images from firewire cameras
The next example also available in tutorial-grabber-1394.cpp shows how to use a framegrabber to acquire color images from a firewire camera under Unix. The following example suppose that libX11 and libdc1394-2 3rd party are available.
@@ -102,5 +102,6 @@ To synchronize the video decoding with the video framerate, we measure the begin
The synchronization is done by waiting from the beginning of the iteration the corresponding time expressed in milliseconds by using:
\snippet tutorial-video-reader.cpp vpVideoReader loop rate
+\section grabber-next Next tutorial
You are now ready to see the next \ref tutorial-tracking-blob.
*/
diff --git a/doc/tutorial-ibvs.doc b/doc/tutorial-ibvs.doc
index be371d3..0f5cc15 100644
--- a/doc/tutorial-ibvs.doc
+++ b/doc/tutorial-ibvs.doc
@@ -56,19 +56,19 @@ The following example available in tutorial-ibvs-4pts.cpp shows how to use ViSP
Now we give a line by line description of the source:
\code
-#include <visp/vpFeatureBuilder.h>
+#include <visp3/core/vpFeatureBuilder.h>
\endcode
Include a kind of common header for all the classes that implement visual features, especially in our case vpFeaturePoint that will allow us to handle \f${\bf x} = (x,y)\f$ described in the \ref ibvs_intro.
\code
-#include <visp/vpServo.h>
+#include <visp3/vs/vpServo.h>
\endcode
Include the header of the vpServo class that implements the control law \f[ {\bf v}_c = -\lambda {\bf L}_{\bf x}^{+} {\bf e} \f] described in the \ref ibvs_intro.
\code
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/robot/vpSimulatorCamera.h>
\endcode
Include the header of the vpSimulatorCamera class that allows to simulate a 6 dof free flying camera.
@@ -183,10 +183,10 @@ We explain now the new lines that were introduced.
First, we add the headers of the classes that are used to introduce the viewers and some display functionality. vpProjectionDisplay is the class that allows to handle the external view from a given camera position, while vpServoDisplay allows to display in overlay the position of the current and desired features in the internal camera view.
\code
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpDisplayX.h>
+#include <visp3/core/vpDisplayGDI.h>
+#include <visp3/core/vpProjectionDisplay.h>
+#include <visp3/vs/vpServoDisplay.h>
\endcode
Secondly, we introduce the function display_trajectory() that allows to display the trajectory of the current features in the image. From the 3D coordinates of the points given in the object frame, we compute their respective position in the camera frame, then we apply the perspective projection before retrieving their positions in sub-pixels in the image thanks to the camera parameters. The successive sub-pixel positions are stored in a vector named \c traj and displayed as a green trajectory.
@@ -279,7 +279,7 @@ We explain now the new lines that were introduced.
First we include the header of the wireframe simulator.
\code
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
\endcode
Then in the main(), we create an instance of the simulator. First we initialize the object in the scene. We recall that the target is a 20cm by 20cm square. This is exactly an object handled by the simulator and defined as a vpWireFrameSimulator::PLATE. By vpWireFrameSimulator::D_STANDARD we indicate that the object displayed at the desired position is also a PLATE. Secondly we initialize the camera position wrt to the object using \c cMo, and also the desired camera position, the one th [...]
@@ -387,5 +387,6 @@ depends on \f$(x,y)\f$ but also on \f$Z\f$ the depth of the feature, we need to
}
\endcode
+\section ibvs_next Next tutorial
You are now ready to see the \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs.
*/
diff --git a/doc/tutorial-image-filtering.doc b/doc/tutorial-image-filtering.doc
index df5b59e..44ddd72 100644
--- a/doc/tutorial-image-filtering.doc
+++ b/doc/tutorial-image-filtering.doc
@@ -13,21 +13,21 @@ Let us consider the following source code that comes from tutorial-image-filter.
\include tutorial-image-filter.cpp
-Once build, you should have \c tutorial-image-filter binary. It shows how to apply different filters on an input image. Here we will consider lena.pgm as input image.
+Once build, you should have \c tutorial-image-filter binary. It shows how to apply different filters on an input image. Here we will consider monkey.pgm as input image.
-\image html img-lena-gray.png
+\image html img-monkey-gray.png
To see the resulting filtered images, just run:
\code
-./tutorial-image-filter lena.pgm
+./tutorial-image-filter monkey.pgm
\endcode
The following sections give a line by line explanation of the source code dedicated to image filtering capabilities.
\section blur Gaussian blur
-Lena input image is read from disk and is stored in \c I which is a gray level image declared as
+Monkey input image is read from disk and is stored in \c I which is a gray level image declared as
\snippet tutorial-image-filter.cpp vpImage construction
@@ -37,7 +37,7 @@ To apply a Gaussian blur to this image we first have to declare a resulting floa
The resulting image is the following:
-\image html img-lena-blured-default.png
+\image html img-monkey-blured-default.png
It is also possible to specify the Gaussian filter kernel size and the Gaussian standard deviation (sigma) using:
@@ -47,7 +47,7 @@ vpImageFilter::gaussianBlur(I, F, 7, 2); // Kernel size: 7, sigma: 2
We thus obtain the following image:
-\image html img-lena-blured-var2.png
+\image html img-monkey-blured-var2.png
\section gradient Gradients computation
@@ -61,7 +61,7 @@ Gradients along Y could be obtained using:
The resulting floating-point images \c dIx, \c dIy are the following:
-\image html img-lena-dIxy.png
+\image html img-monkey-dIxy.png
\section canny Canny edge detector
@@ -77,7 +77,7 @@ Where:
The resulting image \c C is the following:
-\image html img-lena-canny.png
+\image html img-monkey-canny.png
\section convolution Convolution
@@ -103,7 +103,7 @@ After the declaration of a new floating-point image \c Gx, the convolution is ob
The content of the filtered image \c Gx is the following.
-\image html img-lena-sobel.png
+\image html img-monkey-sobel.png
\section pyramid Gaussian image pyramid
@@ -111,8 +111,9 @@ To construct a pyramid of Gaussian filtered images as a vector of images impleme
\snippet tutorial-image-filter.cpp Gaussian pyramid
The content of \c pyr[0], \c pyr[1], \c pyr[2] is the following:
-\image html img-lena-pyr.png
+\image html img-monkey-pyr.png
+\section img_filtering_next Next tutorial
You are now ready to see the next \ref tutorial-tracking-blob.
*/
diff --git a/doc/tutorial-install-centos.doc b/doc/tutorial-install-centos.doc
index 5f59d50..cc1ad3a 100644
--- a/doc/tutorial-install-centos.doc
+++ b/doc/tutorial-install-centos.doc
@@ -7,7 +7,7 @@ In this tutorial you will learn how to install ViSP from source on CentOS. These
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_centos_required Required packages
+\section install_centos_required Install prerequisities
- gcc 4.4.x or later. This can be installed with:
\code
@@ -18,60 +18,217 @@ sudo yum install gcc-c++
sudo yum install cmake
\endcode
-ViSP is interfaced with several optional <a href="http://www.irisa.fr/lagadic/visp/libraries.html">third-party libraries</a>. The installation of the corresponding packages is described in \ref install_centos_3rdparty section.
-\section install_centos_get_source Getting ViSP source code
+\section install_centos_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_centos_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+sudo yum install opencv-devel
+\endcode
+- libX11 to be able to open a window to display images
+\code
+sudo yum install libX11-devel
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+sudo yum install lapack-devel
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+sudo yum install libdc1394-devel
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+sudo yum install libv4l-devel
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+sudo yum install libxml2-devel
+\endcode
+
+\subsection install_centos_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- Coin, to be able to support vrml cad model used by the model-based trackers
+\code
+sudo yum install Coin2-devel
+\endcode
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+sudo yum install libjpeg-devel libpng-devel
+\endcode
+
+
+\section install_centos_install_visp Install ViSP from source code
+\subsection install_centos_get_source Getting ViSP source code
There are different ways to get ViSP source code:
-- You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
or
\code
-unzip ViSP-2.10.0.zip
+unzip visp-x.y.z.zip
\endcode
-- You can also download a more <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a>. Once downloaded, uncompress the file using
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
\code
-unzip ViSP-2.y.z-snapshot-2015.mm.dd.zip
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
\endcode
-- Or you get the cutting-edge ViSP from Subversion repository. To this end you have first to install subversion
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
\code
-sudo yum install subversion
+$ git clone https://github.com/lagadic/visp.git
\endcode
-and then use the following command
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
+
+\subsection install_centos_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
\code
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-trunk
+cd $HOME; mkdir visp-build
\endcode
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/ViSP-2.10.0
+- Enter the \<binary_dir\> and configure the build:
+\code
+cd $HOME/visp-build
+cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-centos-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
-\section install_centos_config Configuring ViSP from source
+\subsection install_centos_build Building ViSP from source
-- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+- To build ViSP proceed with:
\code
-cd $HOME; mkdir ViSP-build-release
+make -j4
\endcode
-- Enter the \<binary_dir\> and to configure the build type:
+- To install ViSP proceed with:
\code
-cmake [<optional parameters>] <source_dir>
+sudo make install
\endcode
-For example:
+
+- To build ViSP documentation, you have first to install Doxygen package:
\code
-cd $HOME/ViSP-build-release
-cmake -DCMAKE_BUILD_TYPE=RELEASE -DBUILD_SHARED_LIBS=ON ../ViSP-2.10.0
+sudo yum install doxygen graphviz
\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+Then you can proceed with:
+\code
+make -j4 visp_doc
+\endcode
+
+\section install_centos_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+\code
+cd $HOME
+unzip ViSP-images-x.y.z.zip
+\endcode
+
+We suppose now that the data are located in \c $HOME/ViSP-images.
+\code
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
+\endcode
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
+\code
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_centos_tips Tips and tricks
+
+\subsection install_centos_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_centos_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_centos_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_centos_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
\code
-ccmake ../ViSP-2.10.0
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_robot
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_mbt
+... visp_tt
+... visp_tt_mi
\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used.
-\image html img-ccmake-centos.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP.
-- There is an other way to see which are the 3rd parties that are found during the configuration stage and that will be used by ViSP during the build. To this end you can have a look to the text file named ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+\subsection install_centos_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
\code
ViSP third-party libraries
@@ -80,12 +237,12 @@ build ViSP on your computer.
Mathematics:
Gnu Scientific Library : no
- Lapack/blas : no
+ Lapack/blas : yes
Simulator:
Ogre simulator : no
\- Ogre3D : no
\- OIS : no
- Coin simulator :
+ Coin simulator : no
\- Coin3D : no
\- SoWin : no
\- SoXt : no
@@ -100,19 +257,18 @@ Robots
Pioneer : no
Viper S650 : no
Viper S850 : no
-Video devices (display)
- X11 : no
- GTK : no
- OpenCV : no
+Video devices (display)
+ X11 : yes
+ GTK : no
+ OpenCV : yes
GDI : no
Direct3D : no
Framegrabbers
- Firewire libdc1394-1.x : no
- Firewire libdc1394-2.x : no
- Video For Linux Two : no
+ Firewire libdc1394-2.x : yes
+ Video For Linux Two : yes
DirectShow : no
CMU 1394 Digital Camera SDK : no
- OpenCV : no
+ OpenCV : yes
Specific devices
Yarp : no
Kinect : no
@@ -124,7 +280,7 @@ Video and image Read/Write:
libjpeg : no
libpng : no
Misc:
- XML2 : no
+ XML2 : yes
pthread : yes
OpenMP : yes
zbar : no
@@ -134,56 +290,11 @@ Documentation:
Graphviz dot : no
ViSP built with C++11 features: no
\endcode
-In our case, only \c pthread an OpenMP 3rd parties are detected.
-\subsection install_centos_3rdparty Optional 3rd party packages
+\section install_centos_issues Known issues
+\subsection install_centos_issues_pthread libpthread may be hidden by files in //lib64
-As mentioned previously, ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. We recommend to install the following:
-
-- OpenCV
-\code
-sudo yum install opencv-devel
-\endcode
-- libX11 to be able to open a window to display images
-\code
-sudo yum install libX11-devel
-\endcode
-- lapack and gsl to benefit from optimized mathematical capabilities
-\code
-sudo yum install lapack-devel gsl-devel
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-sudo yum install libv4l-devel
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-sudo yum install libxml2-devel
-\endcode
-- libjpeg, libpng to support jpeg and png images
-\code
-sudo yum install libjpeg-devel libpng-devel
-\endcode
-
-Once installed, if you want that ViSP exploit the new 3rd parties, you have to configure ViSP again.
-
-\code
-ccmake ../ViSP-2.10.0
-\endcode
-The following image shows now that all the previous optional 3rd parties are detected.
-\image html img-ccmake-centos-all.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP after installation of optional 3rd party libraries.
-
-\note Other <a href="http://www.irisa.fr/lagadic/visp/libraries.html">3rd party libraries</a> are interfaced with ViSP. This is for example the case of <a href="http://www.irisa.fr/lagadic/visp/librarieslist.html#Ogre">Ogre</a> that can be used to enhance the visibility computation of the faces that are tracked with the model-based tracker (see \ref tutorial-tracking-mb), or <a href="http://www.irisa.fr/lagadic/visp/librarieslist.html#Coin">Coin</a> that can be used to parse vrml files t [...]
-
-\subsection install_centos_generate Generating Makefiles
-
-To generate the makefiles, just press [g] key in the ccmake gui.
-
-Now we can build ViSP.
-
-\subsection install_centos_issues Known issues
-
-- On CentOS 7.0 with cmake 2.8.11, during cmake configuration you may encounter the following issue:
+On CentOS 7.0 with cmake 2.8.11, during cmake configuration you may encounter the following issue:
\code
CMake Warning at src/CMakeLists.txt:80 (add_library):
Cannot generate a safe runtime search path for target visp because files in
@@ -214,32 +325,16 @@ $ ls -als libpthread*
0 lrwxrwxrwx. 1 root root 18 Feb 4 12:34 libpthread.so.0 -> libpthread-2.17.so
\endcode
-- On CentOS 7.0, vpVideoReader is not able to read mpeg videos. If ffmpeg is not installed (this is our case since ffmpeg is not packaged for CentOS 7.0), this class uses OpenCV to read and decode videos. Some examples or tutorials provided in ViSP hang during cv::Capture::open() call. The reason is that OpenCV 2.4.5 cv::Capture seams buggy. This is for example the case if you run:
+\subsection install_centos_issues_videoreader vpVideoReader is not able to read mpeg videos
+
+On CentOS 7.0, vpVideoReader is not able to read mpeg videos. If ffmpeg is not installed (this is our case since ffmpeg is not packaged for CentOS 7.0), this class uses OpenCV to read and decode videos. Some examples or tutorials provided in ViSP hang during cv::Capture::open() call. The reason is that OpenCV 2.4.5 cv::Capture seams buggy. This is for example the case if you run:
\code
$ ./example/video/videoReader
\endcode
A work arround consists in installing a more recent OpenCV version from source.
-\section install_centos_build Building ViSP from source
-- To build ViSP proceed with:
-\code
-make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-sudo make install
-\endcode
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-sudo yum install doxygen graphviz
-\endcode
-Then you can proceed with:
-\code
-make visp_doc
-\endcode
+\section install_centos_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-install-fedora.doc b/doc/tutorial-install-fedora.doc
index a9d4d85..7053d31 100644
--- a/doc/tutorial-install-fedora.doc
+++ b/doc/tutorial-install-fedora.doc
@@ -7,9 +7,9 @@ In this tutorial you will learn how to install ViSP from source on Linux Fedora.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_fedora_required Required packages
+\section install_fedora_required Install prerequisities
-- gcc 4.4.x or later. This can be installed with:
+- gcc 4.4.x or later. This can be istalled with:
\code
su -c "yum install gcc-c++"
\endcode
@@ -18,61 +18,233 @@ su -c "yum install gcc-c++"
su -c "yum install cmake"
\endcode
+\section install_fedora_3rdparty Install 3rd parties
-ViSP is interfaced with several optional <a href="http://www.irisa.fr/lagadic/visp/libraries.html">third-party libraries</a>. The installation of the corresponding packages is described in \ref install_fedora_3rdparty section.
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-\section install_fedora_get_source Getting ViSP source code
+\subsection install_fedora_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+su -c "yum install opencv-devel"
+\endcode
+- libX11 to be able to open a window to display images
+\code
+su -c "yum install libX11-devel"
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+su -c "yum install lapack-devel"
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+su -c "yum install libdc1394-devel"
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+su -c "yum install libv4l-devel"
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+su -c "yum install libxml2-devel"
+\endcode
+- QR code detection
+\code
+su -c "yum install zbar-devel"
+\endcode
+
+\subsection install_fedora_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- Coin, to be able to support vrml cad model used by the model-based trackers
+\code
+su -c "yum install Coin2-devel"
+\endcode
+- libjpeg, libpng to support jpeg and png images (only useful if OpenCV is not installed)
+\code
+su -c "yum install libjpeg-devel libpng-devel"
+\endcode
+- ffmpeg, to be able to read or encode compressed video streams (only useful if OpenCV is not installed)
+\code
+su -c "yum install ffmpeg-devel"
+\endcode
+- Ogre 3D if you want to do augmented reality or simulation
+\code
+su -c "yum install ogre-devel ogre-samples ois-devel"
+\endcode
+- Datamatrix code detection
+\code
+su -c "yum install libdmtx-devel"
+\endcode
+
+\section install_fedora_install_visp Install ViSP from source code
+\subsection install_fedora_get_source Getting ViSP source code
There are different ways to get ViSP source code:
-- You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
or
\code
-unzip ViSP-2.10.0.zip
+unzip visp-x.y.z.zip
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
\endcode
-- You can also download a more <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a>. Once downloaded, uncompress the file using
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
+
+\subsection install_fedora_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
\code
-unzip ViSP-2.y.z-snapshot-2015.mm.dd.zip
+cd $HOME; mkdir visp-build
\endcode
-- Or you get the cutting-edge ViSP from Subversion repository. To this end you have first to install subversion
+
+- Enter the \<binary_dir\> and configure the build:
\code
-su -c "yum install subversion"
+cd $HOME/visp-build
+cmake ../visp
\endcode
-and then use the following command
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
\code
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-trunk
+ccmake ../visp
\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/ViSP-2.10.0
+Now we can build ViSP.
-\section install_fedora_config Configuring ViSP from source
+\subsection install_fedora_build Building ViSP from source
-- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+- To build ViSP proceed with:
\code
-cd $HOME; mkdir ViSP-build-release
+make -j4
\endcode
-- Enter the \<binary_dir\> and to configure the build type:
+- To install ViSP proceed with:
\code
-cmake [<optional parameters>] <source_dir>
+sudo make install
\endcode
-For example:
+
+- To build ViSP documentation, you have first to install Doxygen package:
\code
-cd $HOME/ViSP-build-release
-cmake -DCMAKE_BUILD_TYPE=RELEASE -DBUILD_SHARED_LIBS=ON ../ViSP-2.10.0
+su -c "yum install doxygen graphviz"
\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+Then you can proceed with:
+\code
+make -j4 visp_doc
+\endcode
+
+\section install_fedora_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
\code
-ccmake ../ViSP-2.10.0
+cd $HOME
+unzip ViSP-images-x.y.z.zip
\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used.
-\image html img-ccmake-ubuntu.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP.
-- There is an other way to see which are the 3rd parties that are found during the configuration stage and that will be used by ViSP during the build. To this end you can have a look to the text file named ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+We suppose now that the data are located in \c $HOME/ViSP-images.
+\code
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
+\endcode
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
+\code
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_fedora_tips Tips and tricks
+
+\subsection install_fedora_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_fedora_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_fedora_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_fedora_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_robot
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_fedora_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
\code
ViSP third-party libraries
@@ -81,12 +253,12 @@ build ViSP on your computer.
Mathematics:
Gnu Scientific Library : no
- Lapack/blas : no
+ Lapack/blas : yes
Simulator:
Ogre simulator : no
\- Ogre3D : no
\- OIS : no
- Coin simulator :
+ Coin simulator : no
\- Coin3D : no
\- SoWin : no
\- SoXt : no
@@ -101,19 +273,18 @@ Robots
Pioneer : no
Viper S650 : no
Viper S850 : no
-Video devices (display)
- X11 : no
- GTK : no
- OpenCV : no
+Video devices (display)
+ X11 : yes
+ GTK : no
+ OpenCV : yes
GDI : no
Direct3D : no
Framegrabbers
- Firewire libdc1394-1.x : no
- Firewire libdc1394-2.x : no
- Video For Linux Two : no
+ Firewire libdc1394-2.x : yes
+ Video For Linux Two : yes
DirectShow : no
CMU 1394 Digital Camera SDK : no
- OpenCV : no
+ OpenCV : yes
Specific devices
Yarp : no
Kinect : no
@@ -125,101 +296,18 @@ Video and image Read/Write:
libjpeg : no
libpng : no
Misc:
- XML2 : no
+ XML2 : yes
pthread : yes
OpenMP : yes
- zbar : no
+ zbar : yes
dmtx : no
Documentation:
Doxygen : no
Graphviz dot : no
ViSP built with C++11 features: no
\endcode
-In our case, only \c pthread an OpenMP 3rd parties are detected.
-
-\subsection install_fedora_3rdparty Optional 3rd party packages
-
-As mentioned previously, ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. We recommend to install the following:
-
-- OpenCV
-\code
-su -c "yum install opencv-devel"
-\endcode
-- libX11 to be able to open a window to display images
-\code
-su -c "yum install libX11-devel"
-\endcode
-- lapack and gsl to benefit from optimized mathematical capabilities
-\code
-su -c "yum install lapack-devel gsl-devel"
-\endcode
-- libdc1394 to grab images from firewire cameras
-\code
-su -c "yum install libdc1394-devel"
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-su -c "yum install libv4l-devel"
-\endcode
-- Coin, to be able to support vrml cad model used by the model-based trackers
-\code
-su -c "yum install Coin2-devel"
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-su -c "yum install libxml2-devel"
-\endcode
-- libjpeg, libpng to support jpeg and png images
-\code
-su -c "yum install libjpeg-devel libpng-devel"
-\endcode
-- ffmpeg, to be able to read or encode compressed video streams
-\code
-su -c "yum install ffmpeg-devel"
-\endcode
-- Ogre 3D if you want to do augmented reality or simulation
-\code
-su -c "yum install ogre-devel ogre-samples ois-devel"
-\endcode
-- Bar code detection
-\code
-su -c "yum install zbar-devel libdmtx-devel"
-\endcode
-
-Once installed, if you want that ViSP exploit the new 3rd parties, you have to configure ViSP again.
-
-\code
-ccmake ../ViSP-2.10.0
-\endcode
-The following image shows now that all the previous optional 3rd parties are detected.
-\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP after installation of optional 3rd party libraries.
-
-\subsection install_fedora_generate Generating Makefiles
-To generate the makefiles, just press [g] key in the ccmake gui.
-
-Now we can build ViSP.
-
-\section install_fedora_build Building ViSP from source
-
-- To build ViSP proceed with:
-\code
-make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-sudo make install
-\endcode
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-su -c "yum install doxygen graphviz"
-\endcode
-Then you can proceed with:
-\code
-make visp_doc
-\endcode
+\section install_fedora_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-install-iOS.doc b/doc/tutorial-install-iOS.doc
index 040da89..cec266f 100644
--- a/doc/tutorial-install-iOS.doc
+++ b/doc/tutorial-install-iOS.doc
@@ -7,38 +7,45 @@ In this tutorial you will learn how to install ViSP from source on OSX in order
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_iOS_required Required software
+\section install_iOS_required Install required software
-- CMake gui 2.8 for OSX or higher that could be download at : http://www.cmake.org/cmake/resources/software.html
+- CMake gui 2.8 for OSX or higher that could be download at : http://www.cmake.org
+\section install_iOS_3rd_party Install optional packages
-ViSP is interfaced with several optional <a href="http://www.irisa.fr/lagadic/visp/libraries.html">third-party libraries</a>. The installation of the corresponding packages is described in \ref install_iOS_3rdparty section.
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install the following:
-\section install_iOS_get_source Getting ViSP source code
+- OpenCV: get OpenCV for iOS Version 2.4.3 or later in http://opencv.org/downloads.html
+
+- liblapack, libxml2, libjpeg, libpng, ffmpeg should be already included in your OS X package
+
+\section install_iOS_install_visp Install ViSP from source code
+\subsection install_iOS_get_source Getting ViSP source code
There are different ways to get ViSP source code:
-- You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
or
\code
-unzip ViSP-2.10.0.zip
+unzip visp-x.y.z.zip
\endcode
-- You can also download a more <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a>. Once downloaded, uncompress the file using
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
\code
-unzip ViSP-2.y.z-snapshot-2015.mm.dd.zip
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
\endcode
-- Or you get the cutting-edge ViSP from Subversion repository. To this end you have first to install subversion and then use the following command
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
\code
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-trunk
+$ git clone https://github.com/lagadic/visp.git
\endcode
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/ViSP-2.10.0
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
-\section install_iOS_config Configuring ViSP from source
+\subsection install_iOS_config Configuring ViSP from source
- Choose the right path for the \<source_dir\> and the \<binary_dir\> in c-make gui.
@@ -52,26 +59,11 @@ We suppose now that ViSP source is in a directory denoted \<source_dir\>, for e
\image html img-configure-options-iOS.png
-- Press Configure again.
-
-\subsection install_iOS_3rdparty Optional 3rd party packages
-
-As mentioned previously, ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. We recommend to install the following:
-
-- OpenCV: get OpenCV for iOS Version 2.4.3 or later in http://opencv.org/downloads.html
-
-- liblapack, libxml2, libjpeg, libpng, ffmpeg should be already included in your OS X package
-
-Once installed, if you want that ViSP exploit the new 3rd parties, you have to configure ViSP again.
+- Press Configure again. Now we can finish the configuration stage by generating the Xcode project.
+- To generate the Xcode project press Generate on c-make gui. From now, c-make gui can be closed.
-Now we can finish the configuration stage by generating the Xcode project.
-
-\subsection install_iOS_generate_xcode Generate Xcode project
-
-To generate the Xcode project press Generate on c-make gui. From now, c-make gui can be closed.
-
-\section install_iOS_build Building ViSP library
-\subsection getting-started-iOS-configure Open and configure ViSP Xcode project
+\subsection install_iOS_build Building ViSP library
+\subsubsection getting-started-iOS-configure Open and configure ViSP Xcode project
Launch Xcode (Version 4.6.2) and open ViSP.xcodeproj that should be in the \<binary_dir\> where you previously built ViSP sources.
@@ -86,12 +78,14 @@ In the Architectures group, edit the following information in this order :
\image html img-getting-started-iOS-architectures.png
-\subsection getting-started-iOS-generate Generate ViSP library
+\subsubsection getting-started-iOS-generate Generate ViSP library
Now that you have correctly configured your project, choose the visp target and press run.
\image html img-getting-started-iOS-run.png
+\section install_iOS_next Next tutorial
+
You are now ready to see the next \ref tutorial-getting-started-iOS that will show you how to use ViSP as a 3rd party to build your own project.
*/
diff --git a/doc/tutorial-install-opensuse.doc b/doc/tutorial-install-opensuse.doc
index e1f3340..0861c12 100644
--- a/doc/tutorial-install-opensuse.doc
+++ b/doc/tutorial-install-opensuse.doc
@@ -3,11 +3,11 @@
\page tutorial-install-opensuse Tutorial: Installation from source on Linux openSUSE
\tableofcontents
-In this tutorial you will learn how to install ViSP from source on openSUSE. These steps have been tested for openSUSE 13.2 (x86_64) distribution, but should work with any other distribution as well.
+In this tutorial you will learn how to install ViSP from source on openSUSE. These steps have been tested for openSUSE 14.1 (x86_64) distribution, but should work with any other distribution as well.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_opensuse_required Required packages
+\section install_opensuse_required Install prerequisities
- gcc 4.4.x or later. This can be installed with:
\code
@@ -17,61 +17,205 @@ sudo zypper install gcc-c++
\code
sudo zypper install cmake
\endcode
+- Git if you want to get the source code from http://github.com/lagadic/visp
+\code
+sudo zypper install git-core
+\endcode
+
+\section install_opensuse_3rdparty Install 3rd parties
-ViSP is interfaced with several optional <a href="http://www.irisa.fr/lagadic/visp/libraries.html">third-party libraries</a>. The installation of the corresponding packages is described in \ref install_opensuse_3rdparty section.
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-\section install_opensuse_get_source Getting ViSP source code
+\subsection install_opensuse_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+sudo zypper install opencv-devel
+\endcode
+- libX11 to be able to open a window to display images
+\code
+sudo zypper install libX11-devel
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+sudo zypper install lapack-devel
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+sudo zypper install libv4l-devel
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+sudo zypper install libxml2-devel
+\endcode
+
+\subsection install_opensuse_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- libjpeg and libpng to support jpeg and png images (only useful if OpenCV is not installed)
+\code
+sudo zypper install libjpeg8-devel libpng-devel
+\endcode
+
+\section install_opensuse_install_visp Install ViSP from source code
+\subsection install_opensuse_get_source Getting ViSP source code
There are different ways to get ViSP source code:
-- You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
or
\code
-unzip ViSP-2.10.0.zip
+unzip visp-x.y.z.zip
\endcode
-- You can also download a more <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a>. Once downloaded, uncompress the file using
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
\code
-unzip ViSP-2.y.z-snapshot-2015.mm.dd.zip
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
\endcode
-- Or you get the cutting-edge ViSP from Subversion repository. To this end you have first to install subversion
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
\code
-sudo zypper install subversion
+$ git clone https://github.com/lagadic/visp.git
\endcode
-and then use the following command
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
+
+\subsection install_opensuse_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
\code
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-trunk
+cd $HOME; mkdir visp-build
\endcode
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/ViSP-2.10.0
+- Enter the \<binary_dir\> and configure the build:
+\code
+cd $HOME/Vvisp-build
+cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
-\section install_opensuse_config Configuring ViSP from source
+\subsection install_opensuse_build Building ViSP from source
-- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+- To build ViSP proceed with:
\code
-cd $HOME; mkdir ViSP-build-release
+make -j4
\endcode
-- Enter the \<binary_dir\> and to configure the build type:
+- To install ViSP proceed with:
\code
-cmake [<optional parameters>] <source_dir>
+sudo make install
+\endcode
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+sudo zypper install doxygen graphviz
\endcode
-For example:
+Then you can proceed with:
\code
-cd $HOME/ViSP-build-release
-cmake -DCMAKE_BUILD_TYPE=RELEASE -DBUILD_SHARED_LIBS=ON ../ViSP-2.10.0
+make -j4 visp_doc
\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\section install_opensuse_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
\code
-ccmake ../ViSP-2.10.0
+cd $HOME
+unzip ViSP-images-x.y.z.zip
\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used.
-\image html img-ccmake-opensuse.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP.
-- There is an other way to see which are the 3rd parties that are found during the configuration stage and that will be used by ViSP during the build. To this end you can have a look to the text file named ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+We suppose now that the data are located in \c $HOME/ViSP-images.
+\code
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
+\endcode
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
+\code
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_opensuse_tips Tips and tricks
+
+\subsection install_opensuse_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_opensuse_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_opensuse_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_opensuse_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_robot
+... visp_sensor
+... visp_vision
+... visp_vs
+... visp_ar
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_opensuse_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
\code
ViSP third-party libraries
@@ -79,13 +223,13 @@ Below you will find the list of third party libraries used to
build ViSP on your computer.
Mathematics:
- Gnu Scientific Library : no
- Lapack/blas : no
+ Gnu Scientific Library : yes
+ Lapack/blas : yes
Simulator:
Ogre simulator : no
\- Ogre3D : no
\- OIS : no
- Coin simulator :
+ Coin simulator : no
\- Coin3D : no
\- SoWin : no
\- SoXt : no
@@ -100,115 +244,50 @@ Robots
Pioneer : no
Viper S650 : no
Viper S850 : no
-Video devices (display)
+Video devices (display)
X11 : no
- GTK : no
- OpenCV : no
+ GTK : no
+ OpenCV : yes
GDI : no
Direct3D : no
Framegrabbers
- Firewire libdc1394-1.x : no
- Firewire libdc1394-2.x : no
+ Firewire libdc1394-2.x : yes
Video For Linux Two : no
DirectShow : no
CMU 1394 Digital Camera SDK : no
- OpenCV : no
+ OpenCV : yes
Specific devices
Yarp : no
- Kinect : no
- \-libfreenect : no
- \-libusb-1.0 : no
+ Kinect : yes
+ \-libfreenect : yes
+ \-libusb-1.0 : yes
\-pthread : yes
Video and image Read/Write:
FFMPEG : no
- libjpeg : no
- libpng : no
+ libjpeg : yes
+ libpng : yes
Misc:
- XML2 : no
+ XML2 : yes
pthread : yes
- OpenMP : yes
+ OpenMP : no
zbar : no
- dmtx : no
+ dmtx : yes
Documentation:
Doxygen : no
Graphviz dot : no
ViSP built with C++11 features: no
\endcode
-In our case, only \c pthread an OpenMP 3rd parties are detected.
-
-\subsection install_opensuse_3rdparty Optional 3rd party packages
-
-As mentioned previously, ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. We recommend to install the following:
-
-- OpenCV
-\code
-sudo zypper install opencv-devel
-\endcode
-- libX11 to be able to open a window to display images
-\code
-sudo zypper install libX11-devel
-\endcode
-- lapack and gsl to benefit from optimized mathematical capabilities
-\code
-sudo zypper install lapack-devel gsl-devel
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-sudo zypper install libv4l-devel
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-sudo zypper install libxml2-devel
-\endcode
-- libjpeg and libpng to support jpeg and png images
-\code
-sudo zypper install libjpeg8-devel libpng-devel
-\endcode
-Once installed, if you want that ViSP exploit the new 3rd parties, you have to configure ViSP again.
+\section install_opensuse_issues Known issues
+\subsection install_opensuse_issues_libjpeg libjpeg.so.8 may conflict with libjpeg.so.62
-\code
-ccmake ../ViSP-2.10.0
-\endcode
-The following image shows now that all the previous optional 3rd parties are detected.
-\image html img-ccmake-opensuse-all.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP after installation of optional 3rd party libraries.
-
-\note Other <a href="http://www.irisa.fr/lagadic/visp/libraries.html">3rd party libraries</a> are interfaced with ViSP. This is for example the case of <a href="http://www.irisa.fr/lagadic/visp/librarieslist.html#Ogre">Ogre</a> that can be used to enhance the visibility computation of the faces that are tracked with the model-based tracker (see \ref tutorial-tracking-mb), or <a href="http://www.irisa.fr/lagadic/visp/librarieslist.html#Coin">Coin</a> that can be used to parse vrml files t [...]
-
-\subsection install_opensuse_generate Generating Makefiles
-
-To generate the makefiles, just press [g] key in the ccmake gui.
-
-Now we can build ViSP.
-
-\subsection install_opensuse_issues Known issues
-
-- Note that with openSUSE 12.04 but also with 13.02, libjpeg-devel package lead to libjpeg62.so installation, that may conflict with libjpeg8.so that is also installed. That's why we recommend to not install libjpeg-devel, but rather install libjpeg8-devel.
+Note that with openSUSE 12.04 but also with 13.02, libjpeg-devel package lead to libjpeg62.so installation, that may conflict with libjpeg8.so that is also installed. That's why we recommend to not install libjpeg-devel, but rather install libjpeg8-devel.
\code
Linking CXX executable HelloWorld
/usr/lib64/gcc/x86_64-suse-linux/4.8/../../../../x86_64-suse-linux/bin/ld: warning: libjpeg.so.8, needed by /usr/lib64/libopencv_highgui.so.2.4.9, may conflict with libjpeg.so.62
\endcode
-\section install_opensuse_build Building ViSP from source
-
-- To build ViSP proceed with:
-\code
-make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-sudo make install
-\endcode
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-sudo zypper install doxygen graphviz
-\endcode
-Then you can proceed with:
-\code
-make visp_doc
-\endcode
+\section install_opensuse_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-install-osx-homebrew.doc b/doc/tutorial-install-osx-homebrew.doc
new file mode 100644
index 0000000..a134e5f
--- /dev/null
+++ b/doc/tutorial-install-osx-homebrew.doc
@@ -0,0 +1,365 @@
+/**
+
+\page tutorial-install-osx-homebrew Tutorial: Installation from source on OSX with Homebrew
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on OSX with Homebrew. These steps have been tested with Mac OS X 10.9.5 Mavericks and with 10.10.3 Yosemite.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_osx_brew_required Install prerequisities
+
+First, go to http://brew.sh to install Homebrew.
+
+Next use homebrew to install additional software.
+
+\code
+$ brew update
+$ brew install cmake
+\endcode
+
+You will need to add the next line to your ~/.bashrc or ~/.bash_profile to have Homebrew be at the front of the PATH.
+\code
+export PATH=/usr/local/bin:$PATH
+\endcode
+
+In order for the above changes to take effect reopen the terminal or run this command:
+\code
+$ source ~/.bashrc
+\endcode
+
+
+\section install_osx_brew_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_osx_brew_3rdparty_recommended Recommended 3rd party
+
+We recommend to install the following packages.
+
+\subsubsection install_osx_brew_opencv OpenCV 3rd party
+
+\code
+$ brew install homebrew/science/opencv3
+\endcode
+
+You can find OpenCV at:
+
+\code
+/usr/local/opt/opencv3
+\endcode
+
+Now to indicate to CMake where OpenCV is installed you will need to add the following line in your ~/.bashrc or ~/.bash_profile
+\code
+export OpenCV_DIR=/usr/local/opt/opencv3/share/OpenCV
+\endcode
+Note that OpenCV_DIR var gives now the location of OpenCVConfig.cmake file.
+
+In order for the above changes to take effect reopen the terminal or run this command:
+\code
+$ source ~/.bashrc
+\endcode
+
+\subsubsection install_osx_brew_libxml2 libxml2 3rd party
+
+Just run the following instruction:
+\code
+$ brew install libxml2
+\endcode
+
+\subsubsection install_osx_brew_libdc1394 libdc1394 3rd party
+
+Just run the following instruction:
+\code
+$ brew install libdc1394
+\endcode
+
+\subsubsection install_osx_brew_zbar zbar 3rd party
+
+To install the zbar library used in detection module for QR code detection, run:
+\code
+$ brew install zbar
+\endcode
+
+\subsection install_osx_brew_3rdparty_other Other 3rd party
+
+\subsubsection install_osx_brew_gsl GSL 3rd party
+
+If lapack 3rd party is not detected during CMake configuration it may be useful to install the Gnu Scientific Library (GSL) to benefit from optimized mathematical capabilities. To this end run the following instruction:
+\code
+$ brew install gsl
+\endcode
+
+\section install_osx_brew_install_visp Install ViSP from source code
+\subsection install_osx_brew_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+tar xvzf visp-x.y.z.tar.gz
+\endcode
+or
+\code
+unzip visp-x.y.z.zip
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> repository using the following command
+\code
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
+
+\subsection install_osx_brew_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+\code
+cd $HOME; mkdir visp-build
+\endcode
+
+- Enter the \<binary_dir\> and configure the build:
+\code
+cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+ccmake ../visp
+\endcode
+The previous command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd party that will be used.
+Then to generate the makefiles, just press [g] key in the ccmake gui.
+Now we can build ViSP.
+
+\subsection install_osx_brew_build Building ViSP from source
+
+- To build ViSP proceed with:
+\code
+$ make -j4
+\endcode
+
+- To install ViSP proceed with:
+\code
+$ sudo make install
+\endcode
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ brew install doxygen
+\endcode
+Then you can proceed with:
+\code
+$ make -j4 visp_doc
+\endcode
+\section install_brew_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+\code
+cd $HOME
+unzip ViSP-images-x.y.z.zip
+\endcode
+
+We suppose now that the data are located in \c $HOME/ViSP-images.
+\code
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
+\endcode
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
+\code
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_brew_tips Tips and tricks
+
+\subsection install_brew_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_brew_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_brew_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_brew_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_robot
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_brew_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+\code
+ ViSP third-party libraries
+
+Below you will find the list of third party libraries used to
+build ViSP on your computer.
+
+Mathematics:
+ Gnu Scientific Library : yes
+ Lapack/blas : yes
+Simulator:
+ Ogre simulator : no
+ \- Ogre3D : no
+ \- OIS : no
+ Coin simulator : no
+ \- Coin3D : no
+ \- SoWin : no
+ \- SoXt : no
+ \- SoQt : no
+ \- Qt4 : no
+ \- Qt3 : no
+Robots
+ Afma6 : no
+ Afma4 : no
+ Biclops : no
+ Ptu46 : no
+ Pioneer : no
+ Viper S650 : no
+ Viper S850 : no
+Video devices (display)
+ X11 : yes
+ GTK : no
+ OpenCV : yes
+ GDI : no
+ Direct3D : no
+Framegrabbers
+ Firewire libdc1394-2.x : yes
+ Video For Linux Two : no
+ DirectShow : no
+ CMU 1394 Digital Camera SDK : no
+ OpenCV : yes
+Specific devices
+ Yarp : no
+ Kinect : yes
+ \-libfreenect : yes
+ \-libusb-1.0 : yes
+ \-pthread : yes
+Video and image Read/Write:
+ FFMPEG : no
+ libjpeg : yes
+ libpng : yes
+Misc:
+ XML2 : yes
+ pthread : yes
+ OpenMP : no
+ zbar : no
+ dmtx : yes
+Documentation:
+ Doxygen : no
+ Graphviz dot : no
+ViSP built with C++11 features: no
+\endcode
+
+\section install_osx_brew_issues Kwown issues
+\subsection install_osx_brew_issues_opencv3 /usr/local/lib/pkgconfig is not writable
+
+- During OpenCV installation, if you get the following errors:
+\code
+$ brew install opencv3
+Could not symlink lib/pkgconfig/isl.pc
+/usr/local/lib/pkgconfig is not writable.
+
+You can try again using:
+ brew link isl
+...
+\endcode
+it means maybe that you install other softwares without brew in /usr/local.
+A work arround is to change the owner of the corresponding folder like:
+\code
+$ sudo chown {your-user-name} /usr/local/lib/pkgconfig
+\endcode
+
+- If you enter into troubles with the packages you install with brew, a good stating is to run:
+\code
+$ brew doctor
+\endcode
+
+\subsection install_osx_brew_issues_libpng Application built with libpng-1.5.18 but running with 1.6.17
+- If you encounter the following issue
+\code
+$ ./modules/vision/testKeypoint-5
+libpng warning: Application built with libpng-1.5.18 but running with 1.6.17
+error: can't create a png read structure!
+error reading png file
+\endcode
+It means that apparently there is a conflict between libpng version installed by "brew install opencv3" (1.6.17), and the one used by X11/XQuartz (1.5.18).
+A work arround is to turn off libpng usage in ViSP. To configure and build again ViSP without png support:
+\code
+$ ccmake -DUSE_PNG=OFF ../ViSP
+$ make -j4
+\endcode
+An other work arround option is to turn off X11 usage in ViSP. Display capabilities will be then the one from OpenCV. To this end, configure and build again ViSP without X11 support:
+\code
+$ ccmake -DUSE_X11=OFF ../ViSP
+$ make -j4
+\endcode
+
+\section install_osx_brew_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial-install-raspberry.doc b/doc/tutorial-install-raspberry.doc
index 212b44b..1556de1 100644
--- a/doc/tutorial-install-raspberry.doc
+++ b/doc/tutorial-install-raspberry.doc
@@ -13,7 +13,7 @@ In a first section we give some useful instructions to start with a Raspberry PI
\section install_raspberry Raspberry Pi installation
-\subsection raspberry Setting up Raspberry Pi
+\subsection install_raspberry_setup Setting up Raspberry Pi
There are a lot of documentation and tutorial that explain different ways to setup a Raspberry Pi. A good reference is the official page <a href="http://www.raspberrypi.org/">http://www.raspberrypi.org</a>
@@ -28,7 +28,7 @@ We suggest to start with NOOBS (New Out Of the Box Software). Bellow we resume t
- Your Raspberry Pi will boot, and a window will appear with a list of different operating systems that you can install. Select the "recommended Raspbian" checkbox and click on "Install" button.
- When the install process has completed, the Raspberry Pi configuration menu (raspi-config) will load. Here you are able to set the time and date for your region and enable a Raspberry Pi camera board, or even create users. You can exit this menu by using Tab on your keyboard to move to Finish.
-\subsection login Logging in
+\subsection install_raspberry_login Logging in
The default login for Raspbian is username \c pi with the password \c raspberry.
@@ -40,7 +40,7 @@ sudo apt-get update
sudo apt-get upgrade
\endcode
-\subsection camera Setting up a Raspberry Pi camera
+\subsection install_raspberry_camera Setting up a Raspberry Pi camera
If you have a Raspberry Pi camera module see <a href="http://www.raspberrypi.org/help/camera-module-setup/">http://www.raspberrypi.org/help/camera-module-setup</a>. To resume, enable the camera using:
\code
@@ -80,68 +80,226 @@ bcm2835-v4l2
\endcode
-\subsection startx Start graphical user interface
+\subsection install_raspberry_startx Start graphical user interface
To load the graphical user interface, type \c startx and press Enter on your keyboard. This will later allow to use ViSP vpDisplayX or vpDisplayOpenCV classes useful to display images in a X11 window.
-\section install_visp ViSP installation
-\subsection prerequisities Prerequisities
+\section install_raspberry_visp ViSP installation
+\subsection install_raspberry_visp_prerequisities Install prerequisities
-First you need to install the following packagages (g++, CMake, Subversion) that are requested to get and build ViSP:
+First you need to install the following packagages (g++, CMake, Git) that are requested to get and build ViSP:
\code
-sudo apt-get install build-essential cmake-curses-gui subversion
+sudo apt-get install build-essential cmake-curses-gui git
\endcode
-Then you can install the following packages (X11, lapack, gsl, v4l, xml, png, jpeg, opencv), that correspond to the optional 3rd party libraries that are interfaced with ViSP:
+\subsection install_raspberry_visp_3rd_party Install 3rd parties
+
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsubsection install_raspberry_visp_3rd_party_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+sudo apt-get install libopencv-dev
+\endcode
+- libX11 to be able to open a window to display images
+\code
+sudo apt-get install libx11-dev
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+sudo apt-get install liblapack-dev
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+sudo apt-get install libv4l-dev
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+sudo apt-get install libxml2-dev
+\endcode
+- QR code detection
\code
-sudo apt-get install libx11-dev liblapack-dev libgsl0-dev libv4l-dev libxml2-dev libpng12-dev libjpeg-dev libopencv-dev
+sudo apt-get install libzbar-dev
\endcode
-The complete list of 3rd party libraries is detailed following <a href="http://team.inria.fr/lagadic/visp/libraries.html">http://team.inria.fr/lagadic/visp/libraries.html</a> link.
+\subsubsection install_raspberry_visp_3rd_party_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
-\subsection get_source Getting ViSP source code
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+sudo apt-get install libjpeg-dev libpng12-dev
+\endcode
+
+\subsection install_raspberry_visp_install Install ViSP from source code
+\subsubsection install_raspberry_visp_get_source Getting ViSP source code
There are different ways to get ViSP source code on Raspberry Pi:
-- You can download the <a href="http://team.inria.fr/lagadic/visp/download.html#latest">latest stable release</a> tarball. Once downloaded, uncompress the file using:
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
mkdir /home/pi/soft; cd /home/pi/soft
-wget http://gforge.inria.fr/frs/download.php/latestfile/475/ViSP-2.10.0.tar.gz
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
-
-- You can also download the cutting-edge version of the source code from Subversion repository using:
+or
\code
mkdir /home/pi/soft; cd /home/pi/soft
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-code
+unzip visp-x.y.z.zip
\endcode
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c /home/pi/soft/ViSP-2.10.0 or \c /home/pi/soft/ViSP-trunk
-
-\subsection config Configuring ViSP from source
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
+\endcode
-- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
\code
-cd /home/pi/soft; mkdir ViSP-build-release
+$ git clone https://github.com/lagadic/visp.git
\endcode
-- Enter the \<binary_dir\> and configure the build type:
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c /home/pi/soft/visp
+
+\subsubsection install_raspberry_visp_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
\code
-cmake [<optional parameters>] <source_dir>
+cd /home/pi/soft; mkdir visp-build
\endcode
-For example:
+
+- Enter \<binary_dir\> and configure the build:
\code
-cd /home/pi/soft/ViSP-build-release
-cmake -DCMAKE_BUILD_TYPE=RELEASE -DBUILD_SHARED_LIBS=ON ../ViSP-2.10.0
+cd /home/pi/soft/visp-build
+cmake ../visp
\endcode
A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
\code
-ccmake ../ViSP-2.10.0
+ccmake ../visp
\endcode
The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. Then to generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-raspberry-pi.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP.
+\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+
+\subsubsection install_raspberry_visp_build Building ViSP from source
+
+- To build ViSP proceed with:
+\code
+make
+\endcode
+
+- To install ViSP proceed with:
+\code
+sudo make install
+\endcode
-- There is an other way to see which are the 3rd parties that are found during the configuration stage and that will be used by ViSP during the build. To this end you can have a look to the text file named ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+sudo apt-get install doxygen graphviz texlive-latex-base
+\endcode
+Then you can proceed with:
+\code
+make -j4 visp_doc
+\endcode
+
+\subsection install_raspberry_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+\code
+cd $HOME
+unzip ViSP-images-x.y.z.zip
+\endcode
+
+We suppose now that the data are located in \c $HOME/ViSP-images.
+\code
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
+\endcode
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
+\code
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_raspberry_tips Tips and tricks
+
+\subsection install_raspberry_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_raspberry_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_raspberry_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_raspberry_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_robot
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_raspberry_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
\code
ViSP third-party libraries
@@ -149,13 +307,13 @@ Below you will find the list of third party libraries used to
build ViSP on your computer.
Mathematics:
- Gnu Scientific Library : yes
+ Gnu Scientific Library : no
Lapack/blas : yes
Simulator:
Ogre simulator : no
\- Ogre3D : no
\- OIS : no
- Coin simulator :
+ Coin simulator : no
\- Coin3D : no
\- SoWin : no
\- SoXt : no
@@ -173,7 +331,7 @@ Robots
Video devices (display)
X11 : yes
GTK : no
- OpenCV : yes
+ OpenCV : yes
GDI : no
Direct3D : no
Framegrabbers
@@ -181,13 +339,13 @@ Framegrabbers
Video For Linux Two : yes
DirectShow : no
CMU 1394 Digital Camera SDK : no
- OpenCV : yes
+ OpenCV : yes
Specific devices
Yarp : no
Kinect : no
\-libfreenect : no
\-libusb-1.0 : no
- \-pthread : yes
+ \-pthread : no
Video and image Read/Write:
FFMPEG : no
libjpeg : yes
@@ -195,8 +353,8 @@ Video and image Read/Write:
Misc:
XML2 : yes
pthread : yes
- OpenMP : yes
- zbar : no
+ OpenMP : no
+ zbar : yes
dmtx : no
Documentation:
Doxygen : no
@@ -204,26 +362,7 @@ Documentation:
ViSP built with C++11 features: no
\endcode
-\subsection build Building ViSP from source
-
-- To build ViSP proceed with:
-\code
-make
-\endcode
-
-- To install ViSP proceed with:
-\code
-sudo make install
-\endcode
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-sudo apt-get install doxygen graphviz texlive-latex-base
-\endcode
-Then you can proceed with:
-\code
-make visp_doc
-\endcode
+\section install_raspberry_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project on Raspberry Pi or on any other system. Then if you have a Raspberry Pi camera module, you can also follow the \ref tutorial-tracking-blob especially subsection Tracking form v4l2 live cameras.
diff --git a/doc/tutorial-install-ubuntu-package.doc b/doc/tutorial-install-ubuntu-package.doc
index 0e7eb6e..0925e76 100644
--- a/doc/tutorial-install-ubuntu-package.doc
+++ b/doc/tutorial-install-ubuntu-package.doc
@@ -7,60 +7,44 @@ In this tutorial you will learn how to install ViSP from prebuilt official packa
\note Concerning ViSP installation, we provide also other \ref tutorial.
+\section install_ubuntu_package_list ViSP official packages
+
Since Ubuntu 14.04 LTS, ViSP library can be installed from <a href="http://packages.ubuntu.com/search?keywords=visp&searchon=names&suite=trusty§ion=all">existing packages</a> on amd64 or i386 architectures. In Ubuntu 13.10, the packages are only available for amd64 architecture.
The table below shows which are the packages available given the distribution and architecture.
<table>
<tr>
- <th colspan="2">Ubuntu 13.10
- <th colspan="2">Ubuntu 14.04 LTS
- <th colspan="2">Ubuntu 14.10
+ <th>Ubuntu 14.04 LTS
+ <th>Ubuntu 14.10 and 15.04
+ <th>Ubuntu 15.10
</tr>
<tr>
- <th>i386</th>
- <th>amd64</th>
- <th>i386</th>
- <th>amd64</th>
- <th>i386</th>
- <th>amd64</th>
+ <th>trusty</th>
+ <th>utopic and vivid</th>
+ <th>wily</th>
</tr>
<tr>
- <td></td>
- <td>libvisp-dev</td>
- <td>libvisp-dev</td>
<td>libvisp-dev</td>
<td>libvisp-dev</td>
<td>libvisp-dev</td>
</tr>
<tr>
- <td></td>
- <td>libvisp2.8</td>
<td>libvisp2.8</td>
- <td>libvisp2.8</td>
- <td>libvisp2.9</td>
<td>libvisp2.9</td>
+ <td>libvisp2.10</td>
</tr>
<tr>
- <td></td>
- <td>libvisp2.8-dbg</td>
<td>libvisp2.8-dbg</td>
- <td>libvisp2.8-dbg</td>
- <td>libvisp2.9-dbg</td>
<td>libvisp2.9-dbg</td>
+ <td>libvisp2.10-dbg</td>
</tr>
<tr>
- <td></td>
- <td>libvisp-doc</td>
- <td>libvisp-doc</td>
<td>libvisp-doc</td>
<td>libvisp-doc</td>
<td>libvisp-doc</td>
</tr>
<tr>
- <td></td>
- <td>visp-images</td>
- <td>visp-images</td>
<td>visp-images</td>
<td>visp-images</td>
<td>visp-images</td>
@@ -83,6 +67,7 @@ To install ViSP html documentation you can run:
sudo apt-get install visp-doc
\endcode
+\section install_ubuntu_package_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-install-ubuntu.doc b/doc/tutorial-install-ubuntu.doc
index 9411877..b9e61e1 100644
--- a/doc/tutorial-install-ubuntu.doc
+++ b/doc/tutorial-install-ubuntu.doc
@@ -7,7 +7,7 @@ In this tutorial you will learn how to install ViSP from source on Linux Ubuntu.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_ubuntu_required Required packages
+\section install_ubuntu_required Install prerequisities
- gcc 4.4.x or later. This can be installed with:
\code
@@ -18,61 +18,234 @@ sudo apt-get install build-essential
sudo apt-get install cmake-curses-gui
\endcode
+\section install_ubuntu_3rdparty Install 3rd parties
-ViSP is interfaced with several optional <a href="http://www.irisa.fr/lagadic/visp/libraries.html">third-party libraries</a>. The installation of the corresponding packages is described in \ref install_ubuntu_3rdparty section.
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-\section install_ubuntu_get_source Getting ViSP source code
+\subsection install_ubuntu_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+sudo apt-get install libopencv-dev
+\endcode
+- libX11 to be able to open a window to display images
+\code
+sudo apt-get install libx11-dev
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+sudo apt-get install liblapack-dev
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+sudo apt-get install libdc1394-22-dev
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+sudo apt-get install libv4l-dev
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+sudo apt-get install libxml2-dev
+\endcode
+- QR code detection
+\code
+sudo apt-get install libzbar-dev
+\endcode
+
+\subsection install_ubuntu_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- Coin, to be able to support vrml cad model used by the model-based trackers
+\code
+sudo apt-get install libcoin80-dev
+\endcode
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+sudo apt-get install libjpeg-dev libpng12-dev
+\endcode
+- ffmpeg, to be able to read or encode compressed video streams (only useful if OpenCV is not installed)
+\code
+sudo apt-get install libswscale-dev libavutil-dev libavformat-dev libavcodec-dev libbz2-dev libbz2-1.0
+\endcode
+- Ogre 3D if you want to do augmented reality or simulation
+\code
+sudo apt-get install libogre-1.9-dev libois-dev
+\endcode
+- Datamatrix code detection
+\code
+sudo apt-get install libdmtx-dev
+\endcode
+
+\section install_ubuntu_visp Install ViSP from source code
+\subsection install_ubuntu_visp_get_source Getting ViSP source code
There are different ways to get ViSP source code:
-- You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
\code
-tar xvzf ViSP-2.10.0.tar.gz
+tar xvzf visp-x.y.z.tar.gz
\endcode
or
\code
-unzip ViSP-2.10.0.zip
+unzip visp-x.y.z.zip
\endcode
-- You can also download a more <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a>. Once downloaded, uncompress the file using
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
\code
-unzip ViSP-2.y.z-snapshot-2015.mm.dd.zip
+tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
\endcode
-- Or you get the cutting-edge ViSP from Subversion repository. To this end you have first to install subversion
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
\code
-sudo apt-get install subversion
+$ git clone https://github.com/lagadic/visp.git
\endcode
-and then use the following command
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/visp
+
+\subsection install_ubuntu_visp_config Configuring ViSP from source
+
+- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
\code
-svn checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-trunk
+cd $HOME; mkdir visp-build
\endcode
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c $HOME/ViSP-2.10.0
+- Enter the \<binary_dir\> and configure the build:
+\code
+cd $HOME/visp-build
+cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
-\section install_ubuntu_config Configuring ViSP from source
+Now we can build ViSP.
-- Create first a directory denoted \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
+\subsection install_ubuntu_visp_build Building ViSP from source
+
+- To build ViSP proceed with:
+\code
+make -j4
+\endcode
+
+- To install ViSP in \c /usr/local which is the default install location, proceed with:
+\code
+sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
+
+- To build ViSP documentation, you have first to install Doxygen package:
\code
-cd $HOME; mkdir ViSP-build-release
+sudo apt-get install doxygen graphviz texlive-latex-base
\endcode
+Then you can proceed with:
+\code
+make -j4 visp_doc
+\endcode
+
+\section install_ubuntu_dataset Install ViSP dataset
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-- Enter the \<binary_dir\> and to configure the build type:
\code
-cmake [<optional parameters>] <source_dir>
+cd $HOME
+unzip ViSP-images-x.y.z.zip
\endcode
-For example:
+
+We suppose now that the data are located in \c $HOME/ViSP-images.
\code
-cd $HOME/ViSP-build-release
-cmake -DCMAKE_BUILD_TYPE=RELEASE -DBUILD_SHARED_LIBS=ON ../ViSP-2.10.0
+$ ls $HOME/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+
+Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
+
\code
-ccmake ../ViSP-2.10.0
+echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
+source ~/.bashrc
\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used.
-\image html img-ccmake-ubuntu.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP.
-- There is an other way to see which are the 3rd parties that are found during the configuration stage and that will be used by ViSP during the build. To this end you can have a look to the text file named ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+
+\code
+$ cd $HOME/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_ubuntu_tips Tips and tricks
+
+\subsection install_ubuntu_tips_uninstall How to uninstall ViSP
+After ViSP installation, you can remove installed material using:
+\code
+$ sudo make uninstall
+\endcode
+
+\subsection install_ubuntu_tips_modules How to build only ViSP libraries
+
+If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_ubuntu_tips_module_once How to build a ViSP specific module
+
+If you want to build a given module and all the dependencies:
+\code
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_ubuntu_tips_target Which are the targets that could be run with make ?
+
+To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_demos
+... visp_tutorials
+... visp_examples
+... visp_modules
+... visp_doc
+... visp_core
+... visp_detection
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_robot
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_ubuntu_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \<binary_dir\>. We provide hereafter an example of a possible content of this file:
\code
ViSP third-party libraries
@@ -81,12 +254,12 @@ build ViSP on your computer.
Mathematics:
Gnu Scientific Library : no
- Lapack/blas : no
+ Lapack/blas : yes
Simulator:
Ogre simulator : no
\- Ogre3D : no
\- OIS : no
- Coin simulator :
+ Coin simulator : no
\- Coin3D : no
\- SoWin : no
\- SoXt : no
@@ -101,19 +274,18 @@ Robots
Pioneer : no
Viper S650 : no
Viper S850 : no
-Video devices (display)
- X11 : no
- GTK : no
- OpenCV : no
+Video devices (display)
+ X11 : yes
+ GTK : no
+ OpenCV : yes
GDI : no
Direct3D : no
Framegrabbers
- Firewire libdc1394-1.x : no
- Firewire libdc1394-2.x : no
- Video For Linux Two : no
+ Firewire libdc1394-2.x : yes
+ Video For Linux Two : yes
DirectShow : no
CMU 1394 Digital Camera SDK : no
- OpenCV : no
+ OpenCV : yes
Specific devices
Yarp : no
Kinect : no
@@ -125,101 +297,18 @@ Video and image Read/Write:
libjpeg : no
libpng : no
Misc:
- XML2 : no
+ XML2 : yes
pthread : yes
OpenMP : yes
- zbar : no
+ zbar : yes
dmtx : no
Documentation:
Doxygen : no
Graphviz dot : no
ViSP built with C++11 features: no
\endcode
-In our case, only \c pthread an OpenMP 3rd parties are detected.
-
-\subsection install_ubuntu_3rdparty Optional 3rd party packages
-
-As mentioned previously, ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. We recommend to install the following:
-
-- OpenCV
-\code
-sudo apt-get install libopencv-dev
-\endcode
-- libX11 to be able to open a window to display images
-\code
-sudo apt-get install libx11-dev
-\endcode
-- lapack and GSL to benefit from optimized mathematical capabilities
-\code
-sudo apt-get install liblapack-dev libgsl0-dev
-\endcode
-- libdc1394 to grab images from firewire cameras
-\code
-sudo apt-get install libdc1394-22-dev
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-sudo apt-get install libv4l-dev
-\endcode
-- Coin, to be able to support vrml cad model used by the model-based trackers
-\code
-sudo apt-get install libCoin80-dev
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-sudo apt-get install libxml2-dev
-\endcode
-- libjpeg, libpng to support jpeg and png images
-\code
-sudo apt-get install libjpeg-dev libpng12-dev
-\endcode
-- ffmpeg, to be able to read or encode compressed video streams
-\code
-sudo apt-get install libswscale-dev libavutil-dev libavformat-dev libavcodec-dev libbz2-dev libbz2-1.0
-\endcode
-- Ogre 3D if you want to do augmented reality or simulation
-\code
-sudo apt-get install libogre-1.9-dev libois-dev
-\endcode
-- Bar code detection
-\code
-sudo apt-get install libzbar-dev libdmtx-dev
-\endcode
-
-Once installed, if you want that ViSP exploit the new 3rd party, you have to configure ViSP again.
-
-\code
-ccmake ../ViSP-2.10.0
-\endcode
-The following image shows now that all the previous optional 3rd parties are detected.
-\image html img-ccmake-ubuntu-all.png Snapshot of the ccmake \c ../ViSP-2.10.0 command used to configure ViSP after installation of optional 3rd party libraries.
-\subsection install_ubuntu_generate Generating Makefiles
-
-To generate the makefiles, just press [g] key in the ccmake gui.
-
-Now we can build ViSP.
-
-\section install_ubuntu_build Building ViSP from source
-
-- To build ViSP proceed with:
-\code
-make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-sudo make install
-\endcode
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-sudo apt-get install doxygen graphviz texlive-latex-base
-\endcode
-Then you can proceed with:
-\code
-make visp_doc
-\endcode
+\section install_ubuntu_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-install-win10-msvc14.doc b/doc/tutorial-install-win10-msvc14.doc
new file mode 100644
index 0000000..1d0b4d5
--- /dev/null
+++ b/doc/tutorial-install-win10-msvc14.doc
@@ -0,0 +1,174 @@
+/**
+
+\page tutorial-install-win10-msvc14 Tutorial: Installation from source on Windows 10 with Visual C++ 2015
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++. These steps have been tested on Windows 10 (64 bit), with CMake 3.3.2 and Visual Studio Community 2015.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win10_msvc14_required Install prerequisities
+
+- Visual C++. Visual Studio Community 2015 that includes Visual C++ could be downloaded from https://www.visualstudio.com. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2015.
+- CMake that could be download at : http://www.cmake.org.
+\warning We notice that CMake 3.4.1 was not able to detect Visual C++ 2015. This issue is kwown by CMake developers and registered as a bug https://cmake.org/Bug/view.php?id=15831. That is why we suggest to install CMake 3.3.2 to work with Visual C++ 2015.
+
+To install CMake 3.3.2, download the previous release binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-3.3.2-win32-x86.exe you downloaded.
+- Windows Software Developement Kit (SDK) for Windows 10. This SDK could be downloaded from https://dev.windows.com/en-US/downloads/windows-10-sdk. This SDK is requested by CMake and allows also to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
+
+\section install_win10_msvc14_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_win10_msvc14_3rdparty_recommended Recommended 3rd parties
+We recommend to install OpenCV 3rd party. Other 3rd parties should be considered only by expert developers.
+
+\subsubsection install_win10_msvc10_3rdparty_opencv OpenCV 3rd party
+
+- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.1.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.1.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
+\note OpenCV 3.1.0 windows installer contains pre-build libraries compatible with Visual C++ 2015 (in \c C:\\OpenCV\\opencv\\build\\x64\\vc14) and also libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x64\\vc12). With any other compiler version you need to build yourself OpenCV from source.
+
+\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
+
+- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
+\code
+setx OpenCV_DIR C:\OpenCV\opencv\build
+\endcode
+where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win81_msvc_env_var.
+
+\section install_win10_msvc14_install_visp Install ViSP from source code
+\subsection install_win10_msvc14_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file.
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\ViSP> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\visp
+
+\subsection install_win10_msvc14_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \<binary_dir\>, for example \c C:/ViSP/visp-build.
+
+- Launch CMake (cmake-gui) and complete the \<source_dir\> and \<binary_dir\> locations as in the next image.
+
+\image html img-cmake-win10-msvc-launch.png
+
+- Click then on "Configure" button.
+
+\image html img-cmake-win10-create-build-folder.png
+
+- Click on "Yes" to create the \c C:/ViSP/visp-build folder.
+- Select then your compiler, for example here Visual Studio Express 2013 Win64, and click on "Finish" button.
+
+\image html img-cmake-win10-msvc-version.png
+
+- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV and OpenMP 3rd parties are automatically detected.
+
+\image html img-cmake-win10-config.png
+
+- As given in the previous image, note also that the installation folder is set to \c C:/ViSP/visp-build/install.
+\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win10-config-end.png
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+
+\image html img-cmake-win10-msvc-generate.png
+
+- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
+
+\image html img-win8.1-msvc-solution.jpg
+
+\subsection install_win10_msvc14_build Building ViSP from source
+
+- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+
+\image html img-win10-msvc-open.png
+
+- Enter menu "BUILD/Build Solution" to build ViSP.
+
+\image html img-win10-msvc-build.png
+
+- At the end of the build process you should have the following indicating that all the build succeeded.
+
+\image html img-win10-msvc-build-end.png
+
+- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
+
+\image html img-win10-msvc-install.png
+
+- At the end of the installation, you should have the following.
+
+\image html img-win10-msvc-install-end.png
+
+- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
+
+- This ends ViSP installation with \c Debug configuration.
+
+- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
+
+\image html img-win10-msvc-release.png
+
+- Now, as previously, build and install ViSP again.
+
+- At the end, in \c C:/ViSP/visp-build/install/x64/vc14/bin folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+
+\image html img-win10-explorer-install-end.png
+
+\subsection install_win10_msvc14_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc14\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc14\bin"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned.
+
+\code
+C:\> echo %PATH%
+\endcode
+
+\section install_win10_msvc14_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
+
+\image html img-win8.1-visp-images.jpg
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
+\code
+setx VISP_INPUT_IMAGE_PATH C:\ViSP
+\endcode
+
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+
+\image html img-win8.1-cmd-displayGDI.jpg
+
+\section install_win10_msvc14_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial-install-win7.doc b/doc/tutorial-install-win7.doc
index 97748a3..fe2fe36 100644
--- a/doc/tutorial-install-win7.doc
+++ b/doc/tutorial-install-win7.doc
@@ -3,58 +3,57 @@
\page tutorial-install-win7 Tutorial: Installation from source on Windows 7 with Visual C++ 2012
\tableofcontents
-In this tutorial you will learn how to install ViSP from source on Windows with Visual C++. These steps have been tested on Windows 7 (64 bit), with CMake 3.1 and Visual Studio 2012 but should work with any other version as well.
+In this tutorial you will learn how to install ViSP from source on Windows with Visual C++. These steps have been tested on Windows 7 (64 bit), with CMake 3.3.2 and Visual Studio 2012 but should work with any other version as well.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_win7_required Required packages
+\section install_win7_required Install prerequisities
- Visual Studio C++. In this tutorial we use Visual Studio 2012. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products
-- CMake 2.8 or higher that could be download at : http://www.cmake.org/cmake/resources/software.html
+\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
+- CMake 2.8.3 or higher that could be download at : http://www.cmake.org. To install CMake, download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-x.y.z-win32-x86.exe you downloaded.
-\section install_win7_get_source Getting ViSP source code
+\section install_win7_3rdparty Install recommended 3rd party
-There are different ways to get ViSP source code.
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install the following:
-\subsection install_win7_get_source_release Getting the latest release
+\subsection install_win7_3rdparty_opencv OpenCV 3rd party
-You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip.
+We recommend to install OpenCV.
-\subsection install_win7_get_source_snapshot Getting the latest snapshot
+- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
+\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc12 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc12 for the Win64 compiler) and also libraries compatible with Visual C++ 2012 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc11 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc11 for the Win64 compiler). With any other compiler version you need to build yourself OpenCV from source.
-When significant changes or bug fixes were introduced in the current developpement version of the source code, we provide snapshots.
+\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
-If available, you can download a <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a> following the link.
+- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
+\code
+setx OpenCV_DIR C:\OpenCV\opencv\build
+\endcode
+where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-\subsection install_win7_get_source_svn Getting the source from Subversion
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win7_env_var.
-You can also get the cutting-edge ViSP version from Subversion repository svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP.
+\section install_win7_install_visp Install ViSP from source code
+\subsection install_win7_get_source Getting ViSP source code
-One way to do that under Windows is to install Subversion that comes with Cygwin installer that is available on https://cygwin.com/install.html
+There are different ways to get ViSP source code:
-- From the previous link, download Cygwin installer for 32-bits or 64-bits version.
-- Double click on the setup binary to start installation. Keep the default settings and select a mirror.
-- As shown in the next image, in the window that allows to select the packages, search for "subversion" and select "subversion" package in "Devel":
-\image html img-win8.1-cygwin-svn.jpg
-- Click on Next button twice to start installation, and then on Finish button to exit installer.
-- Now you should be able to use subversion
-\code
-C:\Users\...> C:\cygwin64\bin\svn.exe --version
-svn, version 1.7.14
-\endcode
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-Once installed, to get ViSP source code run:
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
\code
-C:\Users\...> cd C:\ViSP
-C:\Users\...> C:\cygwin64\bin\svn.exe checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-code
+C:\ViSP> git clone https://github.com/lagadic/visp.git
\endcode
-\section install_win7_config Configuring ViSP from source
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\visp
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0
+\subsection install_win7_config Configuring ViSP from source
-The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \<binary_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0-build.
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
- Launch CMake (cmake-gui) and complete the \<source_dir\> and \<binary_dir\> locations as in the next image.
@@ -64,60 +63,33 @@ The goal of the configuration step is now to use CMake to produce a Visual Studi
\image html img-cmake-win7-create-build-folder.jpg
-- Click on "Yes" to create the \c C:\\ViSP\\ViSP-2.10.0-build folder.
+- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
- Select then your compiler, for example here Visual Studio 11 Win64, and click on "Finish" button.
\image html img-cmake-win7-msvc-version.jpg
-- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface) and OpenMP 3rd parties are automatically detected.
+- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV and OpenMP 3rd parties are automatically detected.
\image html img-cmake-win7-msvc-config.jpg
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\ViSP-2.10.0-build\\install.
+- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
- Click then on "Configure" button. All the red lines should disappear.
\image html img-cmake-win7-msvc-config-end.jpg
\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-\subsection install_win7_3rdparty Optional 3rd party packages
-
-ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>.
-
-\subsubsection install_win7_3rdparty_opencv OpenCV 3rd party
-
-- We recommend to install OpenCV. From http://opencv.org/downloads.html download the latest OpenCV for Windows version. There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help. In our case we install \c OpenCV-3.0.0-beta.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0-beta.exe copied all the material in \c C:\\OpenCV\\opencv.
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv-3.0.0-beta\build
-\endcode
-
-- Here the directory is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- Quit and restart CMake (cmake gui). <b>It is important to quit CMake</b> in order to take into account the new \c OpenCV_DIR environment variable. Click "Configure" button. As shown in the next image, a new red line indicating that OpenCV is found should appear.
-
-\image html img-cmake-win7-opencv.jpg
-
-- As shown in the next image, if you enable the advanced view, you can see that we will use OpenCV libraries located in \c C:/OpenCV/opencv/build/x64/vc11/lib folder.
-
-\image html img-cmake-win-opencv-advanced.jpg
-
-- Press "Configure" button again.
-
-\subsection install_win7_gene Ending the configuration
-
- To finish the configuration, click on "Generate" button.
\image html img-cmake-win7-generate.jpg
-- Once the generation is done, in \c C:/ViSP/ViSP-2.10.0-build folder you have the Visual Studio \c VISP.sln generated solution file.
+- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
\image html img-cmake-win7-solution.jpg
-\section install_win7_build Building ViSP from source
+\subsection install_win7_build Building ViSP from source
-- To build ViSP just double click on \c C:/ViSP/ViSP-2.10.0-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
\image html img-win7-msvc-open.jpg
@@ -137,7 +109,7 @@ setx OpenCV_DIR C:\OpenCV\opencv-3.0.0-beta\build
\image html img-win7-msvc-install-succeed.jpg
-- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/ViSP-install folder.
+- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
- This ends ViSP installation with \c Debug configuration.
@@ -147,55 +119,83 @@ setx OpenCV_DIR C:\OpenCV\opencv-3.0.0-beta\build
- Now, as previously, build and install ViSP again.
-- At the end, in \c C:/ViSP/ViSP-install/bin folder you have two versions of ViSP DLL library; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+- At the end, in \c C:/ViSP/visp-build/install/bin folder you have two versions of ViSP DLL libraries for each module; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
\image html img-win7-msvc-install-end.jpg
-\section install_win7_env_var Setting up PATH variable
+\subsection install_win7_env_var Setting up PATH variable
If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\ViSP\ViSP-2.10.0-build\install\x64\vc12\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc11\bin"
\endcode
Then to add the path to OpenCV 3rd party libraries, close and re-open a cmd-terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc12\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc11\bin"
\endcode
Then close and re-open a cmd terminal to check if the PATH var was well positioned
\code
-C:\Users\...> echo %PATH%
+C:\> echo %PATH%
\endcode
-\section install_win7_annex Appendix
-\subsection install_win7_annex_data Installing testing data
+\section install_win7_dataset Install ViSP dataset
-Some ViSP examples and tests require data set (images, models). These data set is provided as a compressed zip files.
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-- Download ViSP-images-2.10.0.zip from http://www.irisa.fr/lagadic/visp/download.html#dataDownloadForExample and uncompress it for example in \c C:/ViSP.
+Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
\image html img-win7-ViSP-images.jpg
-\subsection install_win7_annex_usage Using testing data
-- ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
+We suppose now that the data are located in \c C:/ViSP/ViSP-images.
\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
+C:\> dir ViSP/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
\endcode
-- It is also possible to run the examples and tests without positioning \c VISP_INPUT_IMAGE_PATH by using command line option \c -i \<\c path \c to \c the \c data \c set\>. If you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
\code
-displayGDI.exe -i C:\ViSP
+setx VISP_INPUT_IMAGE_PATH C:\ViSP
\endcode
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+
\image html img-win7-cmd-displayGDI.jpg
+\section install_win7_tip Tip and tricks
+
+\subsection install_win7_tip_git How to install Git
+
+One way to install Git as a command line tool under Windows is to install Git that comes with Cygwin installer that is available on https://cygwin.com/install.html
+
+- From the previous link, download Cygwin installer for 32-bits or 64-bits version.
+- Double click on the setup binary to start installation in C:/cygwin64 as Root Directory. Keep the default settings and select a mirror.
+- As shown in the next image, in the window that allows to select the packages, search for \c "git" and select \c "git" package in \c "Devel" category:
+\image html img-win7-cygwin-git.jpg
+- Click on Next button twice to start installation, and then on Finish button to exit installer.
+- To complete the installation, dont forget to add \c C:\\cygwin64\\bin to the path. Open a cmd terminal and run:
+\code
+C:\> setx PATH "%PATH%;C:\cygwin64\bin"
+\endcode
+- Close and re-open a cmd terminal to take changes done in PATH variable.
+- Now you should be able to use Git.
+\code
+C:\> git --version
+git version 2.5.3
+\endcode
+
+\section install_win7_next Next tutorial
+
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
*/
diff --git a/doc/tutorial-install-win8.1-mingw-w64.doc b/doc/tutorial-install-win8.1-mingw-w64.doc
index 4166013..98c7160 100644
--- a/doc/tutorial-install-win8.1-mingw-w64.doc
+++ b/doc/tutorial-install-win8.1-mingw-w64.doc
@@ -3,240 +3,203 @@
\page tutorial-install-win81-mingw64 Tutorial: Installation from source on Windows 8.1 with Mingw-w64
\tableofcontents
-In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.1.2 and Mingw-w64 - GCC for Windows 64 & 32 bits.
+In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.3.2 and Mingw-w64 - GCC for Windows 64 & 32 bits.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_win81_mingw64_required Required packages
+\section install_win81_mingw64_required Install prerequisities
\subsection install_win81_mingw64_required_mingw Mingw-w64
Mingw-w64 could be found following http://mingw-w64.sourceforge.net. From that page, click on download "Win-builds" that will bring you to the page http://win-builds.org/download.html Here simply download and run the package manager, in our case we downloaded win-builds-1.5.0.exe file.
- Once downloaded, double click on the the exe file. It will open an installation window.
-- Select x86_64 and Mingw installation folder C:\\mingw as in the following image:
+- Select x86_64 and Mingw installation folder \c C:\\mingw as in the following image:
\image html img-mingw64-installer-started.jpg
-\note It you want to install Mingw in an other folder, you have to set MINGW_DIR environment variable to your installation location in order to allow CMake to detect 3rd party libraries that come with Mingw installer. Fo example, if you install Mingw in C:\\folder\\mingw-w64, setting MINGW_DIR is simply done running in a cmd terminal:
+\note It you want to install Mingw in an other folder, you have to set MINGW_DIR environment variable to your installation location in order to allow CMake to detect 3rd party libraries that come with Mingw installer. Fo example, if you install Mingw in \c C:\\folder\\mingw-w64, setting \c MINGW_DIR is simply done running in a cmd terminal:
\code
-C:\Users\...> setx MINGW_DIR "C:\mingw"
+C:> setx MINGW_DIR "C:\folder\mingw-w64"
\endcode
- Click on "OK" button. It will open an other window:
\image html img-mingw64-installer-process.jpg
- Where you can click on "Process" button to start the installation
-- A window shows the progression of the installation. When all the packages are installed (in our case 92/92 packages, see next image) you can close all the installation windows.
-\image html img-mingw64-installer-finished.jpg
-- To finish the installation, just add C:\\mingw\\bin folder to the PATH variable. To this end open a cmd terminal and do the following:
+- A window shows the progression of the installation. When all the packages are installed you can close all the installation windows.
+- To finish the installation, just add \c C:\\mingw\\bin folder to the PATH variable. To this end open a cmd terminal and do the following:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\mingw\bin"
+C:> echo %PATH%
+C:> setx PATH "%PATH%;C:\mingw\bin"
\endcode
- Close and re-open a cmd terminal, then run again
\code
-C:\Users\...> echo %PATH%
+C:> echo %PATH%
\endcode
to check that C:\\mingw\\bin was added.
\subsection install_win81_mingw64_required_cmake CMake
-CMake 2.8 or higher that could be download at : http://www.cmake.org In our case we install CMake 3.1.2.
+CMake 2.8.3 or higher that could be download at : http://www.cmake.org.
+- Download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>.
+- Install CMake just by double clicking on the binary \c cmake-x.y.z-win32-x86.exe you downloaded.
-- Download the latest binary distribution installer for Windows from http://www.cmake.org/download. You will find it under "Windows (Win32 Installer)".
-- Install CMake just by double click on the binary cmake-3.1.2-win32-x86.exe you downloaded.
+\section install_win81_mingw64_3rdparty Install recommended 3rd parties
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. Some of them are automatically detected since they are part of MinGW distribution. Other may be installed from source. We recommend to install the following:
-\section install_win81_mingw64_get_source Getting ViSP source code
+\subsection install_win81_mingw64_3rdparty_opencv OpenCV 3rd party
-There are different ways to get ViSP source code.
+We recommend to install OpenCV.
+
+- Since OpenCV is not packaged for Mingw, you have to install OpenCV from source. From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the prebuilt binaries, <b>but also the source code</b> in \c C:\\OpenCV\\opencv.
-\subsection install_win81_mingw64_get_source_release Getting the latest release
+- Now we have to use CMake to configure OpenCV from source. As \<source_dir\> we will set \c C:\\OpenCV\\opencv\\sources and as \<binary_dir\> we set \c C:\\OpenCV\\opencv\\sources\\build-mingw.
+\image html img-win8.1-mingw-opencv-cmake-configure.jpg
+
+- Click then on "Configure" button.
+- Click on "Yes" to create \c C:\\OpenCV\\opencv\\sources\\build-mingw folder.
+- Select then "MinGW Makefiles" and click on "Finish" button.
+- Modify "CMAKE_INSTALL_PREFIX" to \c C:\\OpenCV\\opencv\\build folder. This is the location of the pre-build libraries that come with the installer. Doing that, allows to install OpenCV libraries in a same parent folder.
+\image html img-win8.1-mingw-opencv-cmake-configure-install.jpg
+- Click then on "Configure" button to remove the red lines.
+- Click on "Generate" button to generate the Makefiles for Mingw.
+- Open a cmd terminal, enter in the \<binary_dir\> folder and start mingw32-make
+\code
+C:\> cd C:\OpenCV\opencv\sources\build-mingw
+C:\OpenCV\opencv\sources\build-mingw> mingw32-make
+\endcode
+\note Depending on OpenCV version you may encounter the following issues, for which we propose work arrounds; \ref issue_win81_mingw_opencv_dtor, \ref issue_win81_mingw_opencv_tiff, or \ref issue_win81_mingw_opencv_ipp.
-You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip.
+- To install OpenCV run:
+\code
+C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
+\endcode
-\subsection install_win81_mingw64_get_source_snapshot Getting the latest snapshot
+- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up cmd terminal and enter:
+\code
+setx OpenCV_DIR C:\OpenCV\opencv\build
+\endcode
+where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-When significant changes or bug fixes were introduced in the current developpement version of the source code, we provide snapshots.
+- Here the directory is the one where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
-If available, you can download a <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a> following the link.
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win81_mingw64_env_var.
-\subsection install_win81_mingw64_get_source_svn Getting the source from Subversion
+\section install_win81_mingw64_install_visp Install ViSP from source code
+\subsection install_win81_mingw64_get_source Getting ViSP source code
-You can also get the cutting-edge ViSP version from Subversion repository svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP.
+There are different ways to get ViSP source code.
-One way to do that under Windows is to install Subversion that comes with Cygwin installer that is available on https://cygwin.com/install.html
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-- From the previous link, download Cygwin installer for 32-bits or 64-bits version.
-- Double click on the setup binary to start installation. Keep the default settings and select a mirror.
-- As shown in the next image, in the window that allows to select the packages, search for "subversion" and select "subversion" package in "Devel":
-\image html img-win8.1-cygwin-svn.jpg
-- Click on Next button twice to start installation, and then on Finish button to exit installer.
-- Now you should be able to use subversion
-\code
-C:\Users\...> C:\cygwin64\bin\svn.exe --version
-svn, version 1.7.14
-\endcode
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-Once installed, to get ViSP source code run:
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
\code
-C:\Users\...> cd C:\ViSP
-C:\Users\...> C:\cygwin64\bin\svn.exe checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-code
+C:\ViSP> git clone https://github.com/lagadic/visp.git
\endcode
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\visp
-\section install_win81_mingw64_config Configuring ViSP from source
+\subsection install_win81_mingw64_config Configuring ViSP from source
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0
-
-The goal of the configuration step is now to use CMake to produce all the material requested to build ViSP with Mingw. This material will be located in \<binary_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0-build.
+The goal of the configuration step is now to use CMake to produce all the material requested to build ViSP with Mingw. This material will be located in \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
- Launch CMake (cmake-gui) and complete the \<source_dir\> and \<binary_dir\> locations as in the next image.
-
-\image html img-cmake-win8.1-mingw-launch.jpg
+\image html img-cmake-win8.1-msvc-launch.jpg
- Click then on "Configure" button.
-
\image html img-cmake-win8.1-create-build-folder.jpg
-- Click on "Yes" to create the \c C:\\ViSP\\ViSP-2.10.0-build folder.
+- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
- Select then "MinGW Makefiles" and click on "Finish" button.
-
\image html img-cmake-win8.1-mingw-version.jpg
-- This will start CMake configuration. As shown in the next image, ffmpeg, gdi, libjpeg, libpng, pthread and libxml2 3rd party are detected.
-
+- This will start CMake configuration. As shown in the next image, OpenCV, ffmpeg, gdi, libjpeg, libpng, pthread and libxml2 3rd party are detected.
\image html img-cmake-win8.1-mingw-configure.jpg
+\note If OpenCV is not detected, you may encounter the following issue \ref issue_win81_mingw_opencv_not_detected.
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\ViSP-2.10.0-build\\install.
+- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
- Click then on "Configure" button. All the red lines should disappear.
\image html img-cmake-win8.1-mingw-configure-end.jpg
\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-\subsection install_win81_mingw64_3rdparty Optional 3rd party packages
-
-ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>. Some of them are automatically detected since they are part of MinGW distribution. Other may be installed from source.
-
-\subsubsection install_win81_mingw64_3rdparty_opencv OpenCV 3rd party
-
-- We recommend to install OpenCV. Since OpenCV is not package for Mingw, you have to install OpenCV from source. From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c OpenCV-2.4.10.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-2.4.10.exe copied all the prebuilt binaries, <b>but also the source code</b> in \c C:\\OpenCV\\opencv.
-\note You can also install OpenCV 3.0.0. The principle remains the same.
-
-- Now we have to use CMake to configure OpenCV from source. As \<source_dir\> we will set \c C:\\OpenCV\\opencv\\sources and as \<binary_dir\> we set \c C:\\OpenCV\\opencv\\sources\\build-mingw.
-\image html img-win8.1-mingw-opencv-cmake-configure.jpg
-
-- Click then on "Configure" button.
-- Click on "Yes" to create the \c C:\\OpenCV\\opencv\\sources\\build-mingw.
-- Select then "MinGW Makefiles" and click on "Finish" button.
-\note Here you should encounter a first issue \ref issue_win81_mingw_opencv_endianness.
-- Modify "CMAKE_INSTALL_PREFIX" to \c C:\\OpenCV\\opencv\\build folder. This is the location of the pre-build libraries that come with the installer. Doing that, allows to install OpenCV libraries in a same parent folder.
-\image html img-win8.1-mingw-opencv-cmake-configure-install.jpg
-- Click on "Generate" button to generate the Makefiles for Mingw.
-- Open a cmd terminal, enter in the \<binary_dir\> folder and start mingw32-make
-\code
-C:\Users\...> cd C:\OpenCV\opencv\sources\build-mingw
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make
-\endcode
-\note Here you may encounter an other issue \ref issue_win81_mingw_opencv_tiff and maybe the following if you try to build with OpenCV 3.0.0: \ref issue_win81_mingw_opencv_ipp and \ref issue_win81_mingw_opencv_dshow.
-
-- To install OpenCV run:
-\code
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
-\endcode
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up cmd terminal and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
-
-- Here the directory is the one where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- Quit and restart CMake Gui on ViSP. <b>It is important to quit CMake</b> in order to take into account the new \c OpenCV_DIR environment variable. Click "Configure" button. As shown in the next image, a new red line indicating that OpenCV is found should appear.
-\image html img-cmake-win8.1-mingw-opencv-detected.jpg
-\note If OpenCV is not detected, you may encounter the following issue \ref issue_win81_mingw_opencv_not_detected.
-
-- If you enable the advanced view, you can see that we will use OpenCV libraries located in \c C:/OpenCV/opencv/build/x64/mingw/lib folder.
-
-\image html img-cmake-win8.1-mingw-opencv-detected-advanced.jpg
-
-- Press "Configure" button again.
-
-\subsection install_win81_mingw64_gene Ending the configuration
-
- To finish the configuration, click on "Generate" button.
-
\image html img-cmake-win8.1-mingw-generate.jpg
-- Once the generation is done, in \c C:/ViSP/ViSP-2.10.0-build folder you have the Makefile file that will be used by Mingw to build the entire project.
+- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Makefile file that will be used by Mingw to build the entire project.
-\section install_win81_mingw64_build Building ViSP from source
+\subsection install_win81_mingw64_build Building ViSP from source
-- To build ViSP, open a cmd terminal, change to C:\\ViSP\ViSP-2.10.0-build folder and run mingw32-make:
+- To build ViSP, open a cmd terminal, change to \c C:\\ViSP\\visp-build folder and run mingw32-make:
\code
-C:\Users\...> cd C:\ViSP\ViSP-2.10.0-build
-C:\ViSP\ViSP-2.10.0-build> mingw32-make
+C:\> cd C:\ViSP\visp-build
+C:\ViSP\visp-build> mingw32-make
\endcode
-\note To \ref issue_win81_mingw_warning_pragma just follow the link.
- Now to install ViSP, in the same cmd terminal run:
\code
-C:\ViSP\ViSP-2.10.0-build> mingw32-make install
+C:\ViSP\visp-build> mingw32-make install
\endcode
-- At the end, in \c C:/ViSP/ViSP-2.10.0-build/install/x64/mingw/bin folder you have ViSP DLL library in libvisp-2100.dll file.
-\note When CMAKE\_BUILD\_TYPE is set to Debug, the library name is suffixed by "d".
+- At the end, in \c C:/ViSP/visp-build/install/x64/mingw/bin folder you will find ViSP DLL libraries corresponding to the build modules.
+\note When CMAKE\_BUILD\_TYPE is set to Debug, the library names are suffixed by "d".
-\section install_win81_mingw64_env_var Setting up PATH variable
+\subsection install_win81_mingw64_env_var Setting up PATH variable
If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\ViSP\ViSP-2.10.0-build\install\x64\mingw\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\mingw\bin"
\endcode
Then to add the path to OpenCV 3rd party library, close and re-open a cmd-terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\mingw\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\mingw\bin"
\endcode
Then close and re-open a cmd terminal to check if the PATH var was well positioned
\code
-C:\Users\...> echo %PATH%
+C:\> echo %PATH%
\endcode
-\section install_win81_mingw64_annex Appendix
-\subsection install_win81_mingw64_annex_data Installing testing data
+\section install_win81_mingw64_dataset Install ViSP dataset
-Some ViSP examples and tests require data set (images, models). These data set is provided as a compressed zip files.
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-- Download ViSP-images-2.10.0.zip from http://www.irisa.fr/lagadic/visp/download.html#dataDownloadForExample and uncompress it for example in \c C:/ViSP.
+Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
\image html img-win8.1-visp-images.jpg
-\subsection install_win81_mingw64_annex_usage Using testing data
-- ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
+We suppose now that the data are located in \c C:/ViSP/ViSP-images.
\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
+C:\> dir ViSP/ViSP-images
+Klimt README.md circle ellipse iv mbt mire-2
+LICENSE.txt calibration cube ellipse-1 line mire video
\endcode
-- It is also possible to run the examples and tests without positioning \c VISP_INPUT_IMAGE_PATH by using command line option \c -i \<\c path \c to \c the \c data \c set\>. If you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
\code
-displayGDI.exe -i C:\ViSP
+setx VISP_INPUT_IMAGE_PATH C:\ViSP
\endcode
-\image html img-win8.1-cmd-displayGDI.jpg
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+
+\image html img-win8.1-mingw-cmd-displayGDI.jpg
\section issue_win81_mingw Known issues
+
\subsection issue_win81_mingw_opencv_endianness OpenCV endianness failure during CMake configuration
-\note This issue occurs with OpenCV 2.4.10 and 2.3.0-beta.
+\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
If you encounter the following issue during CMake configuration
\image html img-opencv-issue-test-big-endian.jpg
@@ -247,12 +210,13 @@ test_big_endian(WORDS_BIGENDIAN)
\endcode
by:
\code
+#test_big_endian(WORDS_BIGENDIAN)
set(WORDS_BIGENDIAN 0)
\endcode
\subsection issue_win81_mingw_opencv_tiff OpenCV build error: cannot build with tiff support
-\note This issue occurs with OpenCV 2.4.10 and 2.3.0-beta.
+\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
If you encounter a build issue during libtiff build as given in the next image:
\image html img-opencv-issue-tiff.jpg
@@ -264,56 +228,48 @@ If you encounter a build issue during libtiff build as given in the next image:
C:\OpenCV\opencv\sources\build-mingw> mingw32-make
\endcode
-\subsection issue_win81_mingw_opencv_not_detected OpenCV not detected with Mingw build
-
-\note This issue occurs with OpenCV 2.4.10 and 2.3.0-beta.
-
-- To fix this issue, edit C:\\OpenCV\\opencv\\sources\\cmake\\OpenCVConfig.cmake, and line 89 replace:
-\code
- if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64")
-\endcode
-by:
-\code
- if(OPENCV_GCC_TARGET_MACHINE MATCHES "64")
-\endcode
-
-- Then open a new cmd terminal to build and install OpenCV again:
-\code
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
-\endcode
-
\subsection issue_win81_mingw_opencv_ipp OpenCV link error: cannot find -lRunTmChk
-\note This issue occurs with OpenCV 2.3.0-beta and with OpenCV master branch.
+\note This issue occurs with OpenCV 2.3.0-beta and 2.3.0 releases.
The following image shows the link issue that may appear when building OpenCV with mingw:
\image html img-opencv-issue-ipp.jpg
A work arround is to configure OpenCV without ipp support turning WITH_IPP=OFF and then trying to build again.
-\subsection issue_win81_mingw_opencv_dshow OpenCV build error: cannot build with dshow support
+\subsection issue_win81_mingw_opencv_dtor OpenCV build error: struct has virtual functions and accessible non-virtual destructor
-\note This issue occurs with OpenCV 2.3.0-beta and with OpenCV master branch.
+This error that occurs with OpenCV 3.0.0 during \c cap_dshow.cpp build is known and reported as an issue in https://github.com/Itseez/opencv/pull/5282/commits.
+\image html img-win8.1-mingw-opencv-issue-dtor.jpg
-The following image shows the link issue that may appear when building OpenCV with mingw:
-\image html img-opencv-issue-dshow.jpg
+- The fix consists in modifying \c modules/videoio/src/cap_dshow.cpp by adding near line 96:
+\code
+#ifdef __MINGW32__
+// MinGW does not understand COM interfaces
+#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
+#endif
+\endcode
-A work arround is to configure OpenCV without Direct Show support turning WITH_DSHOW=OFF and then trying to build again.
+\subsection issue_win81_mingw_opencv_not_detected OpenCV not detected with Mingw build
-\subsection issue_win81_mingw_warning_pragma Fix warning "ignoring #pragma comment(lib,"uuid.lib")" during build
+\note This issue occurs with OpenCV 2.4.10 and 2.3.0-beta.
-During the build of ViSP the following warning may occur:
+- To fix this issue, edit \c C:\\OpenCV\\opencv\\sources\\cmake\\OpenCVConfig.cmake, and line 89 replace:
\code
-C:/mingw/mingw/include/urlmon.h:301:0: warning: ignoring #pragma comment [-Wunknown-pragmas]
-#pragma comment(lib,"uuid.lib")
-^
+ if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64")
+\endcode
+by:
+\code
+ if(OPENCV_GCC_TARGET_MACHINE MATCHES "64")
\endcode
-To remove this warning, edit C:/mingw/mingw/include/urlmon.h and comment line 301:
+- Then open a new cmd terminal to build and install OpenCV again:
\code
-//#pragma comment(lib,"uuid.lib")
+C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
\endcode
+\section install_win81_mingw_next Next tutorial
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
*/
diff --git a/doc/tutorial-install-win8.1-msvc.doc b/doc/tutorial-install-win8.1-msvc.doc
index f4bb0e2..121fae5 100644
--- a/doc/tutorial-install-win8.1-msvc.doc
+++ b/doc/tutorial-install-win8.1-msvc.doc
@@ -3,132 +3,160 @@
\page tutorial-install-win81-msvc Tutorial: Installation from source on Windows 8.1 with Visual C++ 2013
\tableofcontents
-In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.1.1 and Visual Studio Express 2013 but should work with any other version as well.
+In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.3.2 and Visual Studio Express 2013 but should work with any other version as well.
\note Concerning ViSP installation, we provide also other \ref tutorial.
-\section install_win81_msvc_required Required packages
+\section install_win81_msvc_required Install prerequisities
-- Visual Studio C++. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products In this tutorial we use Visual Studio Express 2013.
+- Visual Studio C++. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products . In this tutorial we use Visual Studio Express 2013.
\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
-- CMake 2.8 or higher that could be download at : http://www.cmake.org/cmake/resources/software.html
+- CMake 2.8.3 or higher that could be download at : http://www.cmake.org. To install CMake, download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-x.y.z-win32-x86.exe you downloaded.
+\section install_win81_msvc_3rdparty Install 3rd parties
-\section install_win81_msvc_get_source Getting ViSP source code
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-There are different ways to get ViSP source code.
+\subsection install_win81_msvc_3rdparty_recommended Recommended 3rd parties
+We recommend to install the following:
-\subsection install_win81_msvc_get_source_release Getting the latest release
+\subsubsection install_win81_msvc_3rdparty_gdi Windows SDK 3rd party
-You can download the <a href="http://www.irisa.fr/lagadic/visp/download.html#latest">latest stable release</a> as a zip.
+We recommend to install the "Windows Software Development Kit (SDK) for windows 8.1" to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
-\subsection install_win81_msvc_get_source_snapshot Getting the latest snapshot
+- This SDK can be downloaded from https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx
-When significant changes or bug fixes were introduced in the current developpement version of the source code, we provide snapshots.
-If available, you can download a <a href="http://www.irisa.fr/lagadic/visp/download.html#snapshot">recent snapshot</a> following the link.
+\subsubsection install_win81_msvc_3rdparty_opencv OpenCV 3rd party
-\subsection install_win81_msvc_get_source_svn Getting the source from Subversion
+We recommend also to install OpenCV.
-You can also get the cutting-edge ViSP version from Subversion repository svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP.
+- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
+\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc12 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc12 for the Win64 compiler) and also libraries compatible with Visual C++ 2012 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc11 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc11 for the Win64 compiler). With any other compiler version you need to build yourself OpenCV from source.
-One way to do that under Windows is to install Subversion that comes with Cygwin installer that is available on https://cygwin.com/install.html
+\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
-- From the previous link, download Cygwin installer for 32-bits or 64-bits version.
-- Double click on the setup binary to start installation. Keep the default settings and select a mirror.
-- As shown in the next image, in the window that allows to select the packages, search for "subversion" and select "subversion" package in "Devel":
-\image html img-win8.1-cygwin-svn.jpg
-- Click on Next button twice to start installation, and then on Finish button to exit installer.
-- Now you should be able to use subversion
-\code
-C:\Users\...> C:\cygwin64\bin\svn.exe --version
-svn, version 1.7.14
-\endcode
-
-Once installed, to get ViSP source code run:
+- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
\code
-C:\Users\...> cd C:\ViSP
-C:\Users\...> C:\cygwin64\bin\svn.exe checkout svn://scm.gforge.inria.fr/svn/visp/trunk/ViSP ViSP-code
+setx OpenCV_DIR C:\OpenCV\opencv\build
\endcode
+where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-\section install_win81_msvc_config Configuring ViSP from source
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win81_msvc_env_var.
-We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0
+\subsection install_win81_msvc_3rdparty_other Other optional 3rd parties
-The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \<binary_dir\>, for example \c C:\\ViSP\\ViSP-2.10.0-build.
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+\subsubsection install_win81_msvc_3rdparty_ogre Ogre3D 3rd party
-- Launch CMake (cmake-gui) and complete the \<source_dir\> and \<binary_dir\> locations as in the next image.
-
-\image html img-cmake-win8.1-msvc-launch.jpg
+\warning Ogre3D installation from source code described in this section is reserved to expert users.
-- Click then on "Configure" button.
+If you are interested in augmented reality (see vpAROgre), or if you want to enable advanced visibility computation during model-based tracking (see vpMbTracker::setOgreVisibilityTest() and \ref tutorial-tracking-mb) we recommend to install Ogre3D. You can check if a <a href="http://www.ogre3d.org/download/sdk">prebuild SDK</A> matches your Visual C++ version, and follow the <a href="http://www.ogre3d.org/tikiwiki/Installing+the+Ogre+SDK">SDK installation instructions</a>. You can also i [...]
-\image html img-cmake-win8.1-create-build-folder.jpg
-
-- Click on "Yes" to create the \c C:\\ViSP\\ViSP-2.10.0-build folder.
-- Select then your compiler, for example here Visual Studio Express 2013 Win64, and click on "Finish" button.
+Ogre source code is hosted on <a href="https://bitbucket.org/sinbad/ogre">BitBucket</a>. On Ogre wiki you will find useful information to <a href="http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Building+Ogre">build and install Ogre from source</a>. Hereafter we give the main steps.
-\image html img-cmake-win8.1-msvc-version.jpg
+<b>Ogre prerequisities</b>
+- To build Ogre on Windows you will need to install the DirectX SDK. Since Windows 8, DirectX is no longer a standalone package, rather it is bundled in the Windows 8.x SDK. The latest version is the <a href="https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx">Windows 8.1 SDK</a>.
+- To get the Ogre source code but also the source code of all the dependencies hosted on BitBucket and available with Mercurial, you have to install <a href="http://tortoisehg.bitbucket.org">TortoiseHg</a>.
-- This will start CMake configuration. As shown in the next image, only OpenMP 3rd party is automatically detected.
+<b>Getting Ogre dependencies</b>
+- A repository containing Ogre dependencies source code is available <a href="https://bitbucket.org/cabalistic/ogredeps">here at BitBucket</a>.
+- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/cabalistic/ogredeps and as destination \c C:\\Ogre\\ogredeps as in the next image:
+\image html img-win8.1-ogredeps-tortoisehg.jpg
+- Click on Clone button to get dependencies source code in \c C:\\Ogre\\ogredeps
-\image html img-cmake-win8.1-4.jpg
+<b>Building Ogre dependencies</b>
+- To build the dependencies, open CMake GUI and set source code location pointing to \c C:\\Ogre\\ogredeps and build location pointing to \c C:\\Ogre\\ogredeps\\build. Select Visual Studio 12 2013 Win64 as generator, click on "Configure" button twice and then on "Generate" button. You should have something similar to the next image:
+\image html img-win8.1-cmake-ogredeps.jpg
+- Open \c C:\\Ogre\\ogredeps\\build\\OGREDEPS.sln solution file with Visual C++.
+- In Visual, select Debug configuration and build INSTALL project:
+\image html img-win8.1-msvc-build-ogredeps.jpg
+\note If the build fails due to the environment variable DXSDK_DIR that is not set, see \ref known_issues_win81_msvc_ogredeps for a work arround.
+- In Visual, select also Release configuration and build again INSTALL project.
+- Now in \c C:\\Ogre\\ogredeps\\build\\ogredeps\\bin you will find the Cg and OIS dependencies build in Debug and Release:
+\image html img-win8.1-ogredeps-explorer.jpg
+- To finish dependencies installation, set OGRE_DEPENDENCIES_DIR environment variable pointing to \c C:\\Ogre\\ogredeps\\build\\ogredeps
+\code
+setx OGRE_DEPENDENCIES_DIR C:\Ogre\ogredeps\build\ogredeps
+\endcode
+
+<b>Getting Ogre source</b>
+
+- From <a href="http://www.ogre3d.org/download/source">Ogre download page</a>, identify the last release. In our case Ogre 1.9 that is on <a href="https://bitbucket.org/sinbad/ogre/branches">BitBucket repository</a> under branch v1-9.
+- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/sinbad/ogre, as destination \c C:\\Ogre\\ogre and select revision v1-9 corresponding to the branch of the latest release as presented in the next image:
+\image html img-win8.1-ogre-tortoisehg.jpg
+- Click on Clone button to get Ogre source code in \c C:\\Ogre\\ogre
+
+<b>Building Ogre source</b>
+
+- Close and re-open CMake Gui. Set source code location pointing to \c C:\\Ogre\\ogre and build location pointing to \c C:\\Ogre\\ogre\\build. Select Visual Studio 12 2013 Win64 as generator, click on "Configure" button and then on "Generate" button. You should have something similar to the next image:
+\image html img-win8.1-cmake-ogre.jpg
+\note As shown in the previous image, Ogre installation folder is set to \c C:\\Ogre\\ogre\\build\\sdk.
+- Open \c C:\\Ogre\\ogre\\build\\OGRE.sln solution file with Visual C++.
+- In Visual, select Debug configuration and build INSTALL project.
+- In Visual, select also Release configuration and build again INSTALL project.
+- Now in \c C:\\Ogre\\ogre\\build\\sdk\\bin you will find all the libraries in Debug and Release configuration, including Cg and OIS dependencies there where build previously.
+\image html img-win8.1-ogre-explorer.jpg
+- To finish Ogre installation set OGRE_HOME environment variables to \c C:\\Ogre\\ogre\\build\\sdk location.
+\code
+setx OGRE_HOME C:\Ogre\ogre\build\sdk
+\endcode
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\ViSP-2.10.0-build\\install.
-\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
+\section install_win81_msvc_install_visp Install ViSP from source code
+\subsection install_win81_msvc_get_source Getting ViSP source code
-- Click then on "Configure" button. All the red lines should disappear.
-\image html img-cmake-win8.1-6.jpg
-\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+There are different ways to get ViSP source code.
-\subsection install_win81_msvc_3rdparty Optional 3rd party packages
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-ViSP is interfaced with some 3rd party libraries. The <a href="http://www.irisa.fr/lagadic/visp/libraries.html">complete list is provided here</a>.
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-\subsubsection install_win81_msvc_3rdparty_gdi Windows Software Development Kit
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\ViSP> git clone https://github.com/lagadic/visp.git
+\endcode
-- We recommend to install the "Windows Software Development Kit (SDK) for windows 8.1" to get the Graphical Device Interface (GDI) capabilities. The GDi is used in ViSP to display images in a window thanks to vpDisplayGDI class. This SDK can be downloaded from https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx
+We suppose now that ViSP source is in a directory denoted \<source_dir\>, for example \c C:\\ViSP\\visp
-- Once installed, return to CMake Gui and click on "Configure" button. As shown in the the next image, GDI should be detected.
+\subsection install_win81_msvc_config Configuring ViSP from source
-\image html img-cmake-win8.1-gdi.jpg
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
-- Click again on "Configure" button.
+- Launch CMake (cmake-gui) and complete the \<source_dir\> and \<binary_dir\> locations as in the next image.
-\subsubsection install_win81_msvc_3rdparty_opencv OpenCV 3rd party
+\image html img-cmake-win8.1-msvc-launch.jpg
-- We recommend to install OpenCV. From http://opencv.org/downloads.html download the latest OpenCV for Windows version. There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help. In our case we install \c OpenCV-2.4.10.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-2.4.10.exe copied all the material in \c C:\\OpenCV\\opencv.
+- Click then on "Configure" button.
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
+\image html img-cmake-win8.1-create-build-folder.jpg
-- Here the directory is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
+- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
+- Select then your compiler, for example here Visual Studio Express 2013 Win64, and click on "Finish" button.
-- Quit and restart CMake (cmake gui). <b>It is important to quit CMake</b> in order to take into account the new \c OpenCV_DIR environment variable. Click "Configure" button. As shown in the next image, a new red line indicating that OpenCV is found should appear.
+\image html img-cmake-win8.1-msvc-version.jpg
-\image html img-cmake-win8.1-opencv.jpg
+- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV, Ogre and OpenMP 3rd parties are automatically detected.
-- As shown in the next image, if you enable the advanced view, you can see that we will use OpenCV libraries located in \c C:/OpenCV/opencv/build/x64/vc12/lib folder.
-
-\image html img-cmake-win8.1-opencv-advanced.jpg
+\image html img-cmake-win8.1-config.jpg
-- Press "Configure" button again.
+- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
+\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
-\subsection install_win81_msvc_gene Ending the configuration
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win8.1-config-end.jpg
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
- To finish the configuration, click on "Generate" button.
\image html img-cmake-win8.1-msvc-generate.jpg
-- Once the generation is done, in \c C:/ViSP/ViSP-2.10.0-build folder you have the Visual Studio \c VISP.sln generated solution file.
+- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
\image html img-win8.1-msvc-solution.jpg
-\section install_win81_msvc_build Building ViSP from source
+\subsection install_win81_msvc_build Building ViSP from source
-- To build ViSP just double click on \c C:/ViSP/ViSP-2.10.0-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
\image html img-win8.1-msvc-open.jpg
@@ -148,7 +176,7 @@ setx OpenCV_DIR C:\OpenCV\opencv\build
\image html img-win8.1-msvc-install-end.jpg
-- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/ViSP-2.10.0-build/install folder.
+- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
- This ends ViSP installation with \c Debug configuration.
@@ -158,53 +186,96 @@ setx OpenCV_DIR C:\OpenCV\opencv\build
- Now, as previously, build and install ViSP again.
-- At the end, in \c C:/ViSP/ViSP-2.10.0-build/install/x64/vc12/bin folder you have two versions of ViSP DLL library; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+- At the end, in \c C:/ViSP/visp-build/install/x64/vc12/bin folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
\image html img-win8.1-explorer-install-end.jpg
-\section install_win81_msvc_env_var Setting up PATH variable
+\subsection install_win81_msvc_env_var Setting up PATH variable
If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\ViSP\ViSP-2.10.0-build\install\x64\vc12\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc12\bin"
\endcode
-Then to add the path to OpenCV 3rd party library, close and re-open a cmd-terminal and run:
+Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
\code
-C:\Users\...> echo %PATH%
-C:\Users\...> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc12\bin"
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc12\bin"
\endcode
-Then close and re-open a cmd terminal to check if the PATH var was well positioned
+If installed, you have also to add the location of Ogre 3rd party libraries to the path. Close and re-open a cmd terminal to run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\Ogre\ogre\build\sdk\bin\release"
+C:\> setx PATH "%PATH%;C:\Ogre\ogre\build\sdk\bin\debug"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned.
\code
-C:\Users\...> echo %PATH%
+C:\> echo %PATH%
\endcode
-\section install_win81_msvc_annex Appendix
-\subsection install_win81_msvc_annex_data Installing testing data
+\section install_win81_msvc_dataset Install ViSP dataset
-Some ViSP examples and tests require data set (images, models). These data set is provided as a compressed zip files.
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-- Download ViSP-images-2.10.0.zip from http://www.irisa.fr/lagadic/visp/download.html#dataDownloadForExample and uncompress it for example in \c C:/ViSP.
+Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
\image html img-win8.1-visp-images.jpg
-\subsection install_win81_msvc_annex_usage Using testing data
-- ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
\code
setx VISP_INPUT_IMAGE_PATH C:\ViSP
\endcode
-- It is also possible to run the examples and tests without positioning \c VISP_INPUT_IMAGE_PATH by using command line option \c -i \<\c path \c to \c the \c data \c set\>. If you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
+
+\image html img-win8.1-cmd-displayGDI.jpg
+
+\section known_issues_win81_msvc Known issues
+
+\subsection known_issues_win81_msvc_ogredeps Unable to build ogredeps: DXSDK_DIR missing
+
+During ogredeps build as described in \ref install_win81_msvc_3rdparty_ogre installation you may get in Visual C++ the following error when building SDL2 project:
+\code
+error: DIRECTX requires $DXSDK_DIR environment variable to be set
+\endcode
+\image html img-win8.1-msvc-ogredeps-issue.jpg
+
+To fix the issue:
+- Quit Visual C++ and start again CMake over ogredeps project
+- Disable SDL2 dependency build turning \c OGREDEPS_BUILD_SDL2 option off
+\image html img-win8.1-cmake-ogredeps-issue.jpg
+- Configure and generate again the project
+- Open \c C:\\Ogre\\ogredeps\\build\\OGREDEPS.sln solution file with Visual C++ and try a new build as described in \ref install_win81_msvc_3rdparty_ogre
+
+\subsection known_issues_win81_msvc_ogre_mbtracker Model-based trackers are not working with Ogre visibility ckeck
+
+If you run \c mbtEdgeTracking.exe, \c mbtKltTracking.exe or \c mbtEdgeKltTracking.exe enabling Ogre visibility check (using "-o" option), you may encounter the following issue:
\code
-displayGDI.exe -i C:\ViSP
+C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
+...
+OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
+...
+*** Initializing OIS ***
\endcode
+and then a wonderful runtime issue as in the next image:
+\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
-\image html img-win8.1-cmd-displayGDI.jpg
+It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
+\code
+C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
+\endcode
+Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+
+\section install_win81_msvc_next Next tutorial
You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
diff --git a/doc/tutorial-plotter.doc b/doc/tutorial-plotter.doc
index d9dca89..7ced52a 100644
--- a/doc/tutorial-plotter.doc
+++ b/doc/tutorial-plotter.doc
@@ -17,7 +17,7 @@ The last image of the drawing is the following:
Now we describe the new lines that were introduced:
\code
-#include <visp/vpPlot.h>
+#include <visp3/core/vpPlot.h>
\endcode
Include the header of the vpPlot class that allows curves drawing.
diff --git a/doc/tutorial-pose-estimation.doc b/doc/tutorial-pose-estimation.doc
index 3e189bd..06a48b2 100644
--- a/doc/tutorial-pose-estimation.doc
+++ b/doc/tutorial-pose-estimation.doc
@@ -26,8 +26,8 @@ And here is the detailed explanation of the new lines introduced in the source c
First we include the headers of the vpPixelMeterConversion class that contains static functions able to convert coordinates of a point expressed in pixels in the image into meter in the image plane thanks to the camera intrinsic parameters. We also include the header of the vpPose class that is able to estimate a pose from points.
\code
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpPose.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpPose.h>
\endcode
Here we introduce the computePose() function that does the pose estimation. This function uses as input two vectors. The first one is a vpPoint that contains the 3D coordinates in meters of a point, while the second one is a vpDot2 that contains the 2D coordinates in pixels of a blob center of gravity. The 3D and 2D coordinates of the points need to be matched. That means that \c point[i] and \c dot[i] should refer to the same physical point. Other inputs are \c cam that corresponds to t [...]
diff --git a/doc/tutorial-simu-robot-pioneer.doc b/doc/tutorial-simu-robot-pioneer.doc
index 2ffcdec..a2842b4 100644
--- a/doc/tutorial-simu-robot-pioneer.doc
+++ b/doc/tutorial-simu-robot-pioneer.doc
@@ -233,5 +233,6 @@ The following control law is used:
\end{array}\right]
= -0.2 \left( {\bf L_{s} {^c}V_e {^e}J_e}\right)^{+} ({\bf s} - {\bf s}^*) \f]
+\section simu_robot_pioneer_next Next tutorial
You are now ready to see the next \ref tutorial-boost-vs.
*/
diff --git a/doc/tutorial-trace.doc b/doc/tutorial-trace.doc
index 2945ae3..71138b4 100644
--- a/doc/tutorial-trace.doc
+++ b/doc/tutorial-trace.doc
@@ -64,7 +64,7 @@ If you develop a project that uses ViSP library as a 3rd party, there are differ
#define VP_TRACE
#define VP_DEBUG
-#include <visp/vpDebug.h>
+#include <visp3/core/vpDebug.h>
\endcode
either by modifying your \c CMakeLists.txt file by adding an option as in ViSP:
\code
@@ -84,15 +84,15 @@ The following example also available in tutorial-trace.cpp shows how to use the
\note In the previous example it is important to notice that the following lines have to be put prior to any other ViSP includes:
\code
#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-#include <visp/vpDebug.h>
+#include <visp3/core/vpDebug.h>
\endcode
-For example, if you modify the previous example just by including <visp/vpImage.h> on the top of the file, you will get the following warnings:
+For example, if you modify the previous example just by including <visp3/core/vpImage.h> on the top of the file, you will get the following warnings:
\code
Building CXX object tutorial/trace/CMakeFiles/tutorial-trace.dir/tutorial-trace.cpp.o
.../ViSP-code/tutorial/trace/tutorial-trace.cpp:5:1: warning: "VP_DEBUG_MODE" redefined
-In file included from .../ViSP-build-debug/include/visp/vpImage.h:52,
+In file included from .../ViSP-build-debug/include/visp3/core/vpImage.h:52,
from .../ViSP-code/tutorial/trace/tutorial-trace.cpp:2:
-.../ViSP-build-debug/include/visp/vpDebug.h:67:1: warning: this is the location of the previous definition
+.../ViSP-build-debug/include/visp3/core/vpDebug.h:67:1: warning: this is the location of the previous definition
\endcode
diff --git a/doc/tutorial-tracking-blob.doc b/doc/tutorial-tracking-blob.doc
index ed63c25..6d5dbf3 100644
--- a/doc/tutorial-tracking-blob.doc
+++ b/doc/tutorial-tracking-blob.doc
@@ -90,6 +90,7 @@ Here \c blob_list contains the list of the blobs that are detected in the image
Finally, when a new image is available we do the tracking of all the blobs:
\snippet tutorial-blob-auto-tracker.cpp Display
+\section tracking_blob_next Next tutorial
You are now ready to see the next \ref tutorial-tracking-keypoint.
*/
diff --git a/doc/tutorial-tracking-keypoint.doc b/doc/tutorial-tracking-keypoint.doc
index 25d041a..a8530b8 100644
--- a/doc/tutorial-tracking-keypoint.doc
+++ b/doc/tutorial-tracking-keypoint.doc
@@ -143,8 +143,9 @@ The example tutorial-klt-tracker-with-reinit.cpp shows how to do that. In that e
In this code we do the following:
- save the features that are tracked until now
- initialize the tracker to detect new features
-- parse all the saved features and compare them to the newly detected features. If a previous feature is close in terms of geometric distanceto a newly detected one, it is rejected (in our case less than 2 pixels). If not, it is added to the list of detected features.
-
+- parse all the saved features and compare them to the newly detected features. If a previous feature is close in terms of geometric distance to a newly detected one, it is rejected (in our case less than 2 pixels). If not, it is added to the list of detected features.
+
+\section tracking_keypoint_next Next tutorial
You are now ready to see the next \ref tutorial-tracking-me.
*/
diff --git a/doc/tutorial-tracking-mb.doc b/doc/tutorial-tracking-mb.doc
index 50d6c0d..125db5a 100644
--- a/doc/tutorial-tracking-mb.doc
+++ b/doc/tutorial-tracking-mb.doc
@@ -1,186 +1,119 @@
/**
-\page tutorial-tracking-mb Tutorial: Model-based tracking
+\page tutorial-tracking-mb Tutorial: Markerless model-based tracking
\tableofcontents
-With ViSP it is possible to track an object using its cad model. Considered objects should be modeled by lines, circles or cylinders. The model of the object could be defined in vrml format (except for circles), or in cao format.
+ViSP allows simultaneously the tracking of a markerless object using the knowledge of its CAD model while providing its 3D localization (i.e., the object pose expressed in the camera frame) when a calibrated camera is used. Considered objects should be modeled by lines, circles or cylinders. The CAD model of the object could be defined in vrml format (except for circles), or in cao format.
-Next section highlights the differents versions of the markerless model-based trackers that are implemented in ViSP.
+Next sections highlight how to use the differents versions of the markerless model-based trackers that are implemented in ViSP.
-\section tracking_mb Markerless model-based trackers
+\section mb_started Getting started
-The model-based tracker can consider moving-edges behind the lines of the model (see section \ref tracking_mb_edges). It can also consider keypoints that are detected and tracked on each visible face of the model (see section \ref tracking_mb_klt). The tracker can also handle moving-edges and keypoints in an hybrid scheme (see section \ref tracking_mb_hybrid).
+In ViSP, depending on the visual features that are used three trackers are available:
+- a tracker implemented in vpMbEdgeTracker that consider moving-edges behind the visible lines of the model. This tracker is appropriate to track untextured objects.
+- an other tracker implemented in vpMbKltTracker that consider KLT keypoints that are detected and tracked on each visible face of the model. This tracker is more designed to track textured objects with edges that are not really visible.
+- an hybrid version implemented in vpMbEdgeKltTracker that is able to consider moving-edges and KLT keypoints. This tracker is appropriate to track textured objects with visible edges.
-While the \ref tracking_mb_edges is appropriate to track untextured object, the \ref tracking_mb_klt is more designed to exploit textured objects with edges that are not really visible. The \ref tracking_mb_hybrid is appropriate to track textured objects with visible edges.
+\subsection mb_started_src Example source code
+The following example that comes from tutorial-mb-tracker.cpp allows to track a tea box modeled in cao format using one of the markerless model-based tracker implemented in ViSP.
-In the following sections, we consider the tracking of a tea box modeled in cao format.
+\include tutorial-mb-tracker.cpp
-\subsection tracking_mb_edges Model-based edges tracker
+\note An extension of the previous getting started example is proposed in tutorial-mb-tracker-full.cpp where advanced functionalities such as reading tracker settings from an XML file or visibility computation are implemented.
-The following example that comes from tutorial-mb-edge-tracker.cpp allows to track the tea box using vpMbEdgeTracker class.
+\note Other tutorials that are specific to a given tracker are provided in tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp.
-\include tutorial-mb-edge-tracker.cpp
+\subsection mb_started_input Example input data
-The video below shows the result of the tea box model-based edges tracking.
+The previous example uses the following data as input:
+- a video file; "teabox.mpg" is the default video.
+- a cad model that describes the object to track. In our case the file "teabox.cao" is the default one. See \ref mb_model section to learn how the teabox is modelled and section \ref mb_advanced_cao to learn how to model an other object.
+- a file with extension "*.init" that contains the 3D coordinates of some points used to compute an initial pose which serves to initialize the tracker. The user has than to click in the image on the corresponding 2D points. The default file is named "teabox.init". The content of this file is detailed in \ref mb_started_src_explained section.
+- an optional image with extension "*.ppm" that may help the user to remember the location of the corresponding 3D points specified in "*.init" file.
-\htmlonly
-<iframe width="420" height="315" src="http://www.youtube.com/embed/b__u_yGEbmc?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-Hereafter is the description of the new lines introduced in this example.
-
-\snippet tutorial-mb-edge-tracker.cpp Include
-
-Here we include the header of the vpMbEdgeTracker class that allows to track an object from its cad model using moving-edges. The tracker will use image \c I and the intrinsic camera parameters \c cam as input.
-
-\snippet tutorial-mb-edge-tracker.cpp Image
-
-As output, it will estimate \c cMo, the pose of the object in the camera frame.
-
-\snippet tutorial-mb-edge-tracker.cpp cMo
-
-Once the input image \c teabox.pgm is loaded in \c I, a window is created and initialized with image \c I. Then we create an instance of the tracker.
-\snippet tutorial-mb-edge-tracker.cpp Constructor
+\subsection mb_started_exe Running the example
-There are then two different ways to initialize the tracker.
-
-- The first one, if libxml2 is available, is to read the settings from \c teabox.xml input file if the file exists.
-\snippet tutorial-mb-edge-tracker.cpp Load xml
-The content of the xml file is the following:
+Once build, to see the options that are available in the previous source code, just run:
\code
-<?xml version="1.0"?>
-<conf>
- <ecm>
- <mask>
- <size>5</size>
- <nb_mask>180</nb_mask>
- </mask>
- <range>
- <tracking>8</tracking>
- </range>
- <contrast>
- <edge_threshold>10000</edge_threshold>
- <mu1>0.5</mu1>
- <mu2>0.5</mu2>
- </contrast>
- </ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
- <camera>
- <u0>325.66776</u0>
- <v0>243.69727</v0>
- <px>839.21470</px>
- <py>839.44555</py>
- </camera>
- <face>
- <angle_appear>70</angle_appear>
- <angle_disappear>80</angle_disappear>
- <near_clipping>0.1</near_clipping>
- <far_clipping>100</far_clipping>
- <fov_clipping>1</fov_clipping>
- </face>
-</conf>
+$ ./tutorial-mb-tracker --help
+Usage: ./tutorial-mb-tracker [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]
\endcode
-
-- The second one consists in initializing the parameters directly in the source code:
-\snippet tutorial-mb-edge-tracker.cpp Set parameters
-
-
-An important setting concerns the visibility test that is used to determine if a face is visible. Note that moving-edges are tracked only on visible faces. Two different visibility tests are implemented; with or without Ogre ray tracing. The default test is the one without Ogre. The function vpMbEdgeTracker::setOgreVisibilityTest() allow to select which test is to use.
-
-Let us now highlight how the visibility test works. As illustrated in the following figure, the angle \f$ \alpha \f$ between the normal of the face and the line going from the camera to the center of gravity of the face is used to determine if the face is visible. If we consider two parameters; the angle to determine if a face is appearing \f$ \alpha_{appear} \f$, and the angle to determine if the face is disappearing \f$ \alpha_{disappear} \f$, a face will be considered as visible if \ [...]
+By default, teabox.mpg video is used as input. Using "--tracker" option, you can specify which tracker has to be used:
+- Using vpMbEdgeTracker to track only moving-edges:
\code
-<conf>
- ...
- <face>
- <angle_appear>70</angle_appear>
- <angle_disappear>80</angle_disappear>
- </face>
+$ ./tutorial-mb-tracker --tracker 0
\endcode
-or in the code:
-\snippet tutorial-mb-edge-tracker.cpp Set angles
-
-\note When these two angle parameters are not set, their default values set to 89 degrees are used.
-
-\image html img-tracker-mb-visibility.jpg Principle of the visibility test used to determine if a face is visible.
-
-- When Ogre visibility test is disabled (we recall that this is the default behavior), the algorithm that computes the normal of the face is very simple. It makes the assumption that faces are convex and oriented counter clockwise. Here the face is considered as appearing if \f$ \alpha < 70\f$ degrees, and disappearing if \f$ \alpha > 80\f$ degrees. When only moving-edges are used (nor keypoints) and when the object to track is simple like a single box, we suggest as here to disable Ogre [...]
-\snippet tutorial-mb-edge-tracker.cpp Set ogre
-
-- When Ogre visibility test is enabled, the algorithm used to determine the visibility of a face is the same than previously except that once visible faces are detected thanks to their normal, we add an other test to reject faces that are partially occluded by an other one. This additional test is performed using Ogre ray-tracing capability.
+will produce results similar to:
+\htmlonly
+<br>
+<iframe width="420" height="315" src="http://www.youtube.com/embed/b__u_yGEbmc?rel=0" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+- Using vpMbKltTracker to track only keypoints:
\code
- tracker.setOgreVisibilityTest(true);
+$ ./tutorial-mb-tracker --tracker 1
\endcode
-
-
-Additionally to the visibility test described above, it is also possible to use clipping. Firstly, the algorithm removes the faces that are not visibles, according to the visibility test used, then it will also remove the faces or parts of the faces that are out of the clipping planes. As illustrated in the following figure, different clipping planes can be enabled.
-
-\image html img-fov.png Camera field of view and clipping planes.
-
-Let's consider two plane categories: the ones belonging to the field of view or FOV (Left, Right, Up and Down), and the Near and Far clipping planes. The FOV planes can be enabled by:
-
-\snippet tutorial-mb-edge-tracker.cpp Set clipping fov
-
-which is equivalent to:
-
+will produce results similar to:
+\htmlonly
+<br>
+<iframe width="420" height="315" src="http://www.youtube.com/embed/eZmUw9r6Idw?rel=0" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+- Using vpMbEdgeKltTracker to track moving-edges and keypoints in an hybrid scheme:
\code
- tracker.setClipping(vpMbtPolygon::LEFT_CLIPPING
- | vpMbtPolygon::RIGHT_CLIPPING
- | vpMbtPolygon::UP_CLIPPING
- | vpMbtPolygon::DOWN_CLIPPING);
+$ ./tutorial-mb-tracker --tracker 2
\endcode
+will produce results similar to:
+\htmlonly
+<br>
+<iframe width="420" height="315" src="http://www.youtube.com/embed/a-RX9NPF2k0?rel=0" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
-Of course, if the user just wants to activate Right and Up clipping, he will use:
-
+With this example it is also possible to work on an other data set using "--name" command line option. For example, if you run:
\code
- tracker.setClipping(vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING);
+$ ./tutorial-mb-tracker --name myobject
\endcode
+you have to put the following data in the same folder than the binary:
+- myobject.mpg: A video that contains the object to track.
+- myobject.init: The coordinates of at least four 3D points used for the initialization.
+- myobject.cao: The CAD model of the object to track.
-For the Near and Far clipping it is quite different. Indeed, thoses planes require clipping distances. Here there are two choices, either the user uses default values and activate them with:
+Since the video myobject.mpg was acquired by an other camera, you have to set the camera intrinsic parameters in the source code modifying the line:
+\snippet tutorial-mb-tracker.cpp Set camera parameters
-\code
- tracker.setClipping(vpMbtPolygon::NEAR_CLIPPING | vpMbtPolygon::FAR_CLIPPING);
-\endcode
+and build again before using "--name" command line option.
-or the user can specify the distances in meters, which will automatically activate the clipping for thoses planes:
+\subsection mb_started_src_explained Source code explained
-\snippet tutorial-mb-edge-tracker.cpp Set clipping distance
+Hereafter is the description of the some lines introduced in the previous example.
-It is also possible to enable them in the xml file. This is done with the following lines:
+First we include the header of the hybrid tracker that includes internally vpMbEdgeTracker and vpMbKltTracker classes.
+\snippet tutorial-mb-tracker.cpp Include
-\code
-<conf>
- ...
- <face>
- ...
- <near_clipping>0.1</near_clipping>
- <far_clipping>100.0</far_clipping>
- <fov_clipping>0</fov_clipping>
- </face>
-\endcode
+The tracker uses image \c I and the intrinsic camera parameters \c cam as input.
+\snippet tutorial-mb-tracker.cpp Image
-Here for simplicity, the user just has the possibility to either activate all the FOV clipping planes or none of them (fov_clipping requires a boolean).
+As output, it estimates \c cMo, the pose of the object in the camera frame.
+\snippet tutorial-mb-tracker.cpp cMo
-\note When clipping parameters are not set in the xml file, nor in the code, clipping is not used. Usually clipping is not helpful when the oject to track is simple.
+Once input image \c teabox.pgm is loaded in \c I, a window is created and initialized with image \c I. Then we create an instance of the tracker depending on "--tracker" command line option.
+\snippet tutorial-mb-tracker.cpp Constructor
-Now we are ready to load the cad model of the object. ViSP supports cad model in cao format or in vrml format. The cao format is a particular format only supported by ViSP. It doesn't require an additional 3rd party rather then vrml format that require Coin 3rd party. We load the cad model in cao format with:
-\snippet tutorial-mb-edge-tracker.cpp Load cao
-The file \c teabox.cao describes first the vertices of the box, then the edges that corresponds to the faces. A more complete description of this file is provided in \ref tracking_mb_cao_face. The next figure gives the index of the vertices that are defined in \c teabox.cao.
+Then the corresponding tracker settings are initialized. More details are given in \ref mb_settings section.
+\snippet tutorial-mb-tracker.cpp Set parameters
-To load the cad model in vrml the user has to replace the previous line by the following:
-\snippet tutorial-mb-edge-tracker.cpp Load wrl
-As for the cao format, \c teabox.wrl describes first the vertices of the box, then the edges that corresponds to the faces. A more complete description of this file is provided in \ref tracking_mb_vrml_face.
+Now we are ready to load the cad model of the object. ViSP supports cad model in cao format or in vrml format. The cao format is a particular format only supported by ViSP. It doesn't require an additional 3rd party rather then vrml format that require Coin 3rd party. We load the cad model in cao format from teabox.cao file which complete description is provided in \ref mb_teabox_cao with:
+\snippet tutorial-mb-tracker.cpp Load cao
-\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format.
+It is also possible to modify the code to load the cad model in vrml format from teabox.wrl file described in \ref mb_teabox_vrml. To this end modify the previous line with:
+\code
+tracker->loadModel(objectname + ".wrl");
+\endcode
Once the model of the object to track is loaded, with the next line the display in the image window of additional drawings in overlay such as the moving edges positions, is then enabled by:
-\snippet tutorial-mb-edge-tracker.cpp Set display
+\snippet tutorial-mb-tracker.cpp Set display
Now we have to initialize the tracker. With the next line we choose to use a user interaction.
-\snippet tutorial-mb-edge-tracker.cpp Init
+\snippet tutorial-mb-tracker.cpp Init
The user has to click in the image on four vertices with their 3D coordinates defined in the "teabox.init" file. The following image shows where the user has to click.
@@ -193,50 +126,117 @@ The content of \c teabox.init file that defines 3D coordinates of some points of
\includelineno teabox.init
We give now the signification of each line of this file:
-- line 1: Number of 3D points that should be defined in this file. At least 4 points are required. Four is the minimal number of points requested to compute a pose.
+- line 1: Number of 3D points that are defined in this file. At least 4 points are required. Four is the minimal number of points requested to compute a pose.
- line 2: Each point is defined by its 3D coordinates. Here we define the first point with coordinates (0,0,0). In the previous figure it corresponds to vertex 0 of the tea box. This point is also the origin of the frame in which all the points are defined.
- line 3: 3D coordinates of vertex 3.
- line 4: 3D coordinates of vertex 2.
- line 5: 3D coordinates of vertex 5.
-Here the user has to click on vertex 0, 3, 2 and 5 in the window that displays image \c I. From the 3D coordinates defined in \c teabox.init and the corresponding 2D coordinates of the vertices obtained by user interaction a pose is computed that is than used to initialize the tracker.
+Here the user has to click on vertex 0, 3, 2 and 5 in the window that displays image \c I. From the 3D coordinates defined in \c teabox.init and the corresponding 2D coordinates of the vertices obtained by user interaction a pose is computed that is then used to initialize the tracker.
Next, in the infinite while loop, after displaying the next image, we track the object on a new image \c I.
+\snippet tutorial-mb-tracker.cpp Track
-\snippet tutorial-mb-edge-tracker.cpp Track
+The result of the tracking is a pose \c cMo that can be obtained by:
+\snippet tutorial-mb-tracker.cpp Get pose
-The result of the tracking is a pose \c cMo that could be obtained by:
-\snippet tutorial-mb-edge-tracker.cpp Get pose
Next lines are used first to retrieve the camera parameters used by the tracker, then to display the visible part of the cad model using red lines with 2 as thickness, and finally to display the object frame at the estimated position \c cMo. Each axis of the frame are 0.025 meters long. Using vpColor::none indicates that x-axis is displayed in red, y-axis in green, while z-axis in blue. The thickness of the axis is 3.
+\snippet tutorial-mb-tracker.cpp Display
+
+The last lines are here to free the memory allocated for the display and tracker:
+\snippet tutorial-mb-tracker.cpp Cleanup
+
+\section mb_model Teabox CAD model
-\snippet tutorial-mb-edge-tracker.cpp Display
+ViSP supports two different ways to describe CAD models, either in cao or in vrml format.
+- cao format is specific to ViSP. It allows to describe the CAD model of an object using a text file with extension \c .cao.
+- vrml format is supported only if Coin 3rd party is installed. This format allows to describe the CAD model of an object using a text file with extension \c .wrl.
-The last lines are here to free the memory allocated by libxml2 or Coin 3rd party libraries:
-\snippet tutorial-mb-edge-tracker.cpp Cleanup
+\subsection mb_teabox_cao teabox.cao example
-\subsection tracking_mb_klt Model-based keypoint tracker
+The content of the file teabox.cao used in the getting started \ref mb_started_src but also in tutorial-mb-edge-tracker.cpp and in tutorial-mb-hybrid-tracker.cpp examples is given here:
+
+\includelineno teabox.cao
-The following example that comes from tutorial-mb-klt-tracker.cpp allows to track the tea box using vpMbKltTracker class.
+This file describes the model of the tea box corresponding to the next image:
-\include tutorial-mb-klt-tracker.cpp
+\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format.
-The video below shows the result of the tea box model-based KLT tracking where keypoints are used as input features.
-
-\htmlonly
-<iframe width="420" height="315" src="http://www.youtube.com/embed/eZmUw9r6Idw?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
+We make the choice to describe the faces of the box from the 3D points that correspond to the vertices. We provide now a line by line description of the file. Notice that the characters after the '#' are considered as comments.
+- line 1: Header of the \c .cao file
+- line 3: The model is defined by 8 3D points. Here the 8 points correspond to the 8 vertices of the tea box presented in the previous figure. Thus, next 8 lines define the 3D points coordinates.
+- line 4: 3D point with coordinate (0,0,0) corresponding to vertex 0 of the tea box. This point is also the origin of the frame in which all the 3D points are defined.
+- line 5: 3D point with coordinate (0,0,-0.08) corresponding to vertex 1.
+- line 6 to 11: The other 3D points corresponding to vertices 2 to 7 respectively.
+- line 13: Number of 3D lines defined from two 3D points. It is possible to introduce 3D lines and then use these lines to define faces from these 3D lines. This is particularly useful to define faces from non-closed polygons. For instance, it can be used to specify the tracking of only 3 edges of a rectangle. Notice also that a 3D line that doesn't belong to a face is always visible and consequently always tracked.
+- line 15: Number of faces defined from 3D lines. In our teabox example we decide to define all the faces from 3D points, that is why this value is set to 0.
+- line 17: The number of faces defined by a set of 3D points. Here our teabox has 6 faces. Thus, next 6 lines describe each face from the 3D points defined previously line 4 to 11. Notice here that all the faces defined from 3D points corresponds to closed polygons.
+- line 18: First face defined by 4 3D points, respectively vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
+- line 19: Second face also defined by 4 points, respectively vertices 1,6,5,2 to have a counter clockwise orientation.
+- line 20 to 23: The four other faces of the box.
+- line 25: Number of 3D cylinders describing the model. Since we model a simple box, the number of cylinders is 0.
+- line 27: Number of 3D circles describing the model. For the same reason, the number of circles is 0.
+
+\subsection mb_teabox_cao_triangle teabox-triangle.cao example
+
+The content of the file teabox-triangle.cao used in the tutorial-mb-klt-tracker.cpp example is given here:
+
+\includelineno teabox-triangle.cao
+
+This file describes the model of the tea box corresponding to the next image:
+
+\image html img-teabox-cao-triangle.jpg Index of the vertices used to model the tea box in cao format with triangles.
+
+Until line 15, the content of this file is similar to the one described in
+\ref mb_teabox_cao. Line 17 we specify that the model contains 12 faces. Each face is then described as a triangle.
+
+\note Since some lines of the model (for example the one between points 0 and 2, or 7 and 3...) don't correspond to teabox edges, this CAD model is not suited for moving-edges and hybrid trackers.
+
+\subsection mb_teabox_vrml teabox.wrl example
+
+The content of the teabox.wrl file used in tutorial-mb-tracker-full.cpp and tutorial-mb-edge-tracker.cpp when teabox.cao is missing is given hereafter. This content is to make into relation with teabox.cao described in \ref mb_teabox_cao. As for the cao format, teabox.wrl describes first the vertices of the box, then the edges that corresponds to the faces.
+
+\includelineno teabox.wrl
+
+This file describes the model of the tea box corresponding to the next image:
+
+\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in vrml format.
+
+We provide now a line by line description of the file where the faces of the box are defined from the vertices:
+- line 1 to 10: Header of the \c .wrl file.
+- line 13 to 20: 3D coordinates of the 8 tea box vertices.
+- line 34 to 29: Each line describe a face. In this example, a face is defined by 4 vertices. For example, the first face join vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
+
+
+\section mb_settings Tracker settings
-This example is very similar to the one presented in \ref tracking_mb_edges except that here we use vpMbKltTracker class to track the tea box.
+\subsection mb_settings_xml Settings from an XML file
-As previously, there are two different ways to initialize the tracker.
-- The first one, if libxml2 is available, consists in reading the settings from an xml file.
-\snippet tutorial-mb-klt-tracker.cpp Load xml
-The \c teabox.xml file used here contains the following:
+Instead of setting the tracker parameters from source code, if libxml2 is available it is possible to specify the settings from an XML file. As done in tutorial-mb-tracker-full.cpp example, to read the parameters from an XML file, simply modify the code like:
+\snippet tutorial-mb-tracker-full.cpp Load xml
+The content of the XML file teabox.xml that is considered by default is the following:
\code
<?xml version="1.0"?>
<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>8</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
<klt>
<mask_border>5</mask_border>
<max_features>300</max_features>
@@ -262,34 +262,19 @@ The \c teabox.xml file used here contains the following:
</face>
</conf>
\endcode
-- The second one consists in initializing the parameters directly in the source code:
-\snippet tutorial-mb-klt-tracker.cpp Set parameters
-
-Note also that in this example we can model the tea box with triangles:
-\code
- tracker.loadModel("teabox-triangle.cao");
-\endcode
-The file \c teabox-triangle.cao describes first the vertices of the box, then the triangular faces. A more complete description of this file is provided in \ref tracking_mb_cao_triangle).
-
-Note that this is the only tracker for which lines of the model are not necessary edges of the object.
-
-\subsection tracking_mb_hybrid Model-based hybrid tracker
-
-The following example that comes from tutorial-mb-hybrid-tracker.cpp allows to track the tea box using vpMbEdgeKltTracker class.
-\include tutorial-mb-hybrid-tracker.cpp
+Depending on the tracker all the XML tags are not useful:
+- \<ecm\> tag corresponds to the moving-edges settings. This tag is used by vpMbEdgeTracker and vpMbEdgeKltTracker.
+- \<klt\> tag corresponds to the keypoint settings. This tag is used by vpMbKltTracker and vpMbEdgeKltTracker.
+- \<camera\> and \<face\> tags are used by all the trackers.
-The video below shows the result of the tea box model-based hybrid tracking where moving-edges and keypoints are used as input features.
+\subsection mb_settings_ecm Moving-edges settings
+Moving-edges settings affect the way the visible edges of an object are tracked.
+These settings could be tuned either from XML using \<ecm\> tag as:
-\htmlonly
-<iframe width="420" height="315" src="http://www.youtube.com/embed/a-RX9NPF2k0?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-The source code is very similar to the one described in \ref tracking_mb_edges and \ref tracking_mb_klt. It doesn't require additional line by line explanation.
-We provide just hereafter the content of the \c teabox.xml file:
\code
-<?xml version="1.0"?>
<conf>
+ ...
<ecm>
<mask>
<size>5</size>
@@ -303,11 +288,36 @@ We provide just hereafter the content of the \c teabox.xml file:
<mu1>0.5</mu1>
<mu2>0.5</mu2>
</contrast>
+ <sample>
+ <step>4</step>
+ </sample>
</ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
+ ...
+</conf>
+\endcode
+
+of from source code using vpMbEdgeTracker::setMovingEdge() method:
+\snippet tutorial-mb-tracker-full.cpp Set moving-edges parameters
+
+With the previous parameters you can set:
+- mask_size: defines the size of the convolution mask used to detect an edge.
+- nb_mask: number of mask applied to determine the object contour. The number of mask determines the precision of the normal of the edge for every sample. If precision is 2deg, then there are 360/2 = 180 masks.
+- range_tracking: range on both sides of the reference pixel along the normal of the contour used to track a moving-edge. If the displacement of the tracked object in two successive images is large, you have to increase this parameter.
+- edge_threshold: likelihood threshold used to determined if the moving edge is valid or not.
+- mu1: minimum image contrast allowed to detect a contour.
+- mu2: maximum image contrast allowed to detect a contour.
+- sample_step: minimum distance in pixel between two discretized moving-edges. To increase the number of moving-edges you have to reduce this parameter.
+
+\note Most important parameters are \e range_tracking and \e sample_step.
+
+
+\subsection mb_settings_klt Keypoints settings
+Keypoint settings affect tracking of keypoints on visible faces using KLT.
+These settings could be tuned either from XML using \<klt\> tag as:
+
+\code
+<conf>
+ ...
<klt>
<mask_border>5</mask_border>
<max_features>300</max_features>
@@ -318,83 +328,163 @@ We provide just hereafter the content of the \c teabox.xml file:
<size_block>3</size_block>
<pyramid_lvl>3</pyramid_lvl>
</klt>
+ ...
+</conf>
+\endcode
+
+of from source code using vpMbKltTracker::setKltOpencv() and vpMbKltTracker::setMaskBorder() methods:
+\snippet tutorial-mb-tracker-full.cpp Set klt parameters
+
+With the previous parameters you can set:
+- mask_border: erosion number of the mask used on each face.
+- max_features: maximum number of keypoint features to track in the image.
+- window_size: window size used to refine the corner locations.
+- quality: parameter characterizing the minimal accepted quality of image corners. Corners with quality measure less than this parameter are rejected. This means that if you want to have more keypoints on a face, you have to reduce this parameter.
+- min_distance: minimal Euclidean distance between detected corners during keypoint detection stage used to initialize keypoint location.
+- harris: free parameter of the Harris detector.
+- size_block: size of the averaging block used to track the keypoint features.
+- pyramid_lvl: maximal pyramid level. If the level is zero, then no pyramid is computed for the optical flow.
+
+\note Most important parameters are \e min_distance and \e quality.
+
+\subsection mb_settings_cam Camera settings
+Camera settings correspond to the intrinsic camera parameters without distorsion. If images are acquired by a camera that has a large field of view that introduce distorsion, images need to be undistorded before processed by the tracker. The camera parameters are then the one obtained on undistorded images.
+
+Camera settings could be set from XML using \<camera\> tag as:
+\code
+<conf>
+ ...
<camera>
<u0>325.66776</u0>
<v0>243.69727</v0>
<px>839.21470</px>
<py>839.44555</py>
</camera>
+ ...
+</conf>
+\endcode
+of from source code using vpMbTracker::setCameraParameters() method:
+\snippet tutorial-mb-tracker-full.cpp Set camera parameters
+
+As described in vpCameraParameters class, these parameters correspond to \f$(p_x, p_y)\f$ the ratio between the focal length and the size of a pixel, and \f$(u_0, v_0)\f$ the coordinates of the principal point in pixel.
+
+\note The \ref tutorial-calibration explains how to obtain these parameters from a camera calibration stage.
+
+\subsection mb_settings_visibility Visibility settings
+
+An important setting concerns the visibility test that is used to determine if a face is visible or not. Note that moving-edges and keypoints are only tracked on visible faces. Three different visibility tests are implemented; with or without Ogre ray tracing and with or without scanline rendering. The default test is the one without Ogre and scanline. The functions vpMbTracker::setOgreVisibilityTest() and vpMbTracker::setScanLineVisibilityTest() allow to select which test to use.
+
+\subsubsection mb_settings_visibility_default Default visibility based on normals
+Let us now highlight how the default visibility test works. As illustrated in the following figure, the angle \f$ \alpha \f$ between the normal of the face and the line going from the camera to the center of gravity of the face is used to determine if the face is visible.
+
+\image html img-tracker-mb-visibility.jpg Principle of the visibility test used to determine if a face is visible.
+
+When no advanced visibility test is enable (we recall that this is the default behavior), the algorithm that computes the normal of the face is very simple. It makes the assumption that faces are convex and oriented counter clockwise. If we consider two parameters; the angle to determine if a face is appearing \f$ \alpha_{appear} \f$, and the angle to determine if the face is disappearing \f$ \alpha_{disappear} \f$, a face will be considered as visible if \f$ \alpha \leq \alpha_{disapp [...]
+\code
+<conf>
+ ...
<face>
<angle_appear>70</angle_appear>
<angle_disappear>80</angle_disappear>
- <near_clipping>0.1</near_clipping>
- <far_clipping>100</far_clipping>
- <fov_clipping>1</fov_clipping>
</face>
-</conf>
\endcode
+or in the code:
+\snippet tutorial-mb-tracker-full.cpp Set angles
-\section tracking_model How to model the objects to track
+Here the face is considered as appearing if \f$ \alpha \leq 70\f$ degrees, and disappearing if \f$ \alpha > 80\f$ degrees.
-ViSP supports two different ways to describe CAD models, either in cao or in vrml format.
-- cao format is specific to ViSP. It allows to describe the CAD model of an object using a text file with extension \c .cao.
-- vrml format is supported only if Coin 3rd party is installed. This format allows to describe the CAD model of an object using a text file with extension \c .wrl.
+\note When these two angle parameters are not set, their default values set to 89 degrees are used.
+\subsubsection mb_settings_visibility_ogre Advanced visibility via Ogre3D
-\subsection tracking_mb_cao_face teabox.cao example
+The Ogre3D visibility test approach is based on ray tracing. When this test is enabled, the algorithm used to determine the visibility of a face performs (<b>in addition to the previous test based on normals, i.e on the visible faces resulting from the previous test</b>) another test which sets the faces that are <b>partially occluded as non-visible</b>. It can be enabled via:
+\code
+ tracker->setOgreVisibilityTest(true);
+\endcode
+\image html img-tracker-mb-visibility-ogre.png "Ogre visibility test on both polygons."
-The content of the file teabox.cao used in the \ref tracking_mb_edges and \ref tracking_mb_hybrid examples is given here:
+When using the <b>classical version of the ogre visibility test</b> (which is the default behavior when activating this test), <b>only one ray</b> is used per polygon to test its visibility. As shown on the figure above, this only ray is sent from the camera to the center of gravity of the considered polygon. If the ray intersects another polygon before the considered one, it is set as non-visible. Intersections are computed <b>between the ray and the axis-aligned bounding-box (AABB)</b> [...]
-\includelineno teabox.cao
+Additionnaly, it is also possible to use a statistical approach during the ray tracing phase in order to improve the visibility results.
+\code
+ tracker->setNbRayCastingAttemptsForVisibility(4);
+ tracker->setGoodNbRayCastingAttemptsRatio(0.70);
+\endcode
+\image html img-tracker-mb-visibility-ogre-advanced.png "Ogre visibility test on the first polygon, using a statistical approach."
-This file describes the model of the tea box corresponding to the next image:
+Contrary to the classical version of this test, the <b>statistical approach uses multiple rays per polygons</b> (4 in the example above). Each ray is sent randomly toward the considered polygon. If a specified ratio of rays do not have intersected another polygon before the considered one, the polygon is set as visible. In the example above, three ray on four return the first polygon as visible. As the ratio of good matches is more than 70% (which corresponds to the chosen ratio in this [...]
-\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format.
+\note Since ViSP 3.0.0 we have introduced vpMbTracker::setOgreShowConfigDialog() method that allows to open the Ogre configuration pannel which can be used to select the renderer. To enable this feature, use:
-We make the choice to describe the faces of the box from the 3D points that correspond to the vertices. We provide now a line by line description of the file. Notice that the characters after the '#' are considered as comments.
-- line 1: Header of the \c .cao file
-- line 3: The model is defined by 8 3D points. Here the 8 points correspond to the 8 vertices of the tea box presented in the previous figure. Thus, next 8 lines define the 3D points coordinates.
-- line 4: 3D point with coordinate (0,0,0) corresponding to vertex 0 of the tea box. This point is also the origin of the frame in which all the 3D points are defined.
-- line 5: 3D point with coordinate (0,0,-0.08) corresponding to vertex 1.
-- line 6 to 11: The other 3D points corresponding to vertices 2 to 7 respectively.
-- line 13: Number of 3D lines defined from two 3D points. It is possible to introduce 3D lines and then use these lines to define faces from these 3D lines. This is particularly useful to define faces from non-closed polygons. For instance, it can be used to specify the tracking of only 3 edges of a rectangle. Notice also that a 3D line that doesn't belong to a face is always visible and consequently always tracked.
-- line 15: Number of faces defined from 3D lines. In our teabox example we decide to define all the faces from 3D points, that is why this value is set to 0.
-- line 17: The number of faces defined by a set of 3D points. Here our teabox has 6 faces. Thus, next 6 lines describe each face from the 3D points defined previously line 4 to 11. Notice here that all the faces defined from 3D points corresponds to closed polygons.
-- line 18: First face defined by 4 3D points, respectively vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
-- line 19: Second face also defined by 4 points, respectively vertices 1,6,5,2 to have a counter clockwise orientation.
-- line 20 to 23: The four other faces of the box.
-- line 25: Number of 3D cylinders describing the model. Since we model a simple box, the number of cylinders is 0.
-- line 27: Number of 3D circles describing the model. For the same reason, the number of circles is 0.
+\code
+ tracker->setOgreShowConfigDialog(true);
+\endcode
-\subsection tracking_mb_cao_triangle teabox-triangle.cao example
+\subsubsection mb_settings_visibility_scanline Advanced visibility via Scanline Rendering
-The content of the file teabox-triangle.cao used in the \ref tracking_mb_klt example is given here:
+Contrary to the visibility test using Ogre3D, this method <b>does not require any additional third-party library</b>. As for the advanced visibility using Ogre3D, <b>this test is applied in addition to the test based on normals (i.e on the faces set as visible during this test) and also in addition to the test with Ogre3D if it has been activated</b>. This test is based on the scanline rendering algorithm and can be enabled via:
+\code
+ tracker->setScanLineVisibilityTest(true);
+\endcode
+\image html img-tracker-mb-visibility-scanline.png "Scanline visibility test on both polygons."
-\includelineno teabox-triangle.cao
+Even if this approach requires a bit <b>more computing power</b>, it is a <b>pixel perfect visibility test</b>. According to the camera point of view, polygons will be <b>decomposed in order to consider only their visible parts</b>. As a result, if we consider the example above, dashed red lines won't be considered during the tracking phase and detected keypoints will be correctly matched with the closest (in term of depth from the camera position) polygon.
-This file describes the model of the tea box corresponding to the next image:
+\subsection mb_settings_clipping Clipping settings
-\image html img-teabox-cao-triangle.jpg Index of the vertices used to model the tea box in cao format with triangles.
+Additionally to the visibility test described above, it is also possible to use clipping. Firstly, the algorithm removes the faces that are not visibles, according to the visibility test used, then it will also remove the faces or parts of the faces that are out of the clipping planes. As illustrated in the following figure, different clipping planes can be enabled.
-Until line 15, the content of this file is similar to the one described in
-\ref tracking_mb_cao_face. Line 17 we specify that the model contains 12 faces. Each face is then described as a triangle.
+\image html img-fov.png Camera field of view and clipping planes.
-\subsection tracking_mb_vrml_face teabox.wrl example
+Let us consider two plane categories: the ones belonging to the field of view or FOV (Left, Right, Up and Down), and the Near and Far clipping planes. The FOV planes can be enabled by:
-The content of the teabox.wrl file used in the \ref tracking_mb_edges is given hereafter. This content is to make into relation with teabox.cao described in \ref tracking_mb_cao_face.
+\snippet tutorial-mb-tracker-full.cpp Set clipping fov
-\includelineno teabox.wrl
+which is equivalent to:
-This file describes the model of the tea box corresponding to the next image:
+\code
+ tracker->setClipping(vpMbtPolygon::LEFT_CLIPPING
+ | vpMbtPolygon::RIGHT_CLIPPING
+ | vpMbtPolygon::UP_CLIPPING
+ | vpMbtPolygon::DOWN_CLIPPING);
+\endcode
-\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in vrml format.
+Of course, if the user just wants to activate Right and Up clipping, he will use:
+
+\code
+ tracker->setClipping(vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING);
+\endcode
+
+For the Near and Far clipping it is quite different. Indeed, those planes require clipping distances. Here there are two choices, either the user uses default values and activate them with:
+
+\code
+ tracker->setClipping(vpMbtPolygon::NEAR_CLIPPING | vpMbtPolygon::FAR_CLIPPING);
+\endcode
+
+or the user can specify the distances in meters, which will automatically activate the clipping for those planes:
+
+\snippet tutorial-mb-tracker-full.cpp Set clipping distance
+
+It is also possible to enable them in the XML file. This is done with the following lines:
+
+\code
+<conf>
+ ...
+ <face>
+ ...
+ <near_clipping>0.1</near_clipping>
+ <far_clipping>100.0</far_clipping>
+ <fov_clipping>0</fov_clipping>
+ </face>
+\endcode
+
+Here for simplicity, the user just has the possibility to either activate all the FOV clipping planes or none of them (fov_clipping requires a boolean).
+
+\note When clipping parameters are not set in the XML file, nor in the code, clipping is not used. Usually clipping is not helpful when the oject to track is simple.
-We provide now a line by line description of the file where the faces of the box are defined from the vertices:
-- line 1 to 10: Header of the \c .wrl file.
-- line 13 to 20: 3D coordinates of the 8 tea box vertices.
-- line 34 to 29: Each line describe a face. In this example, a face is defined by 4 vertices. For example, the first face join vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
-\section advanced_model_sec Advanced: How to manipulate the model
+\section mb_advanced Advanced
+\subsection mb_advanced_model How to manipulate the model
The following code shows how to access to the CAD model
- to check if a face is visible,
@@ -428,7 +518,7 @@ The following code shows how to access to the CAD model
\endcode
-\section advanced_lod_sec Advanced: Level of detail (LOD)
+\subsection mb_advanced_lod Level of detail (LOD)
The level of detail (LOD) consists in introducing additional constraints to the visibility check to determine if the features of a face have to be tracked or not. Two parameters are used
- the line length (in pixel)
@@ -451,12 +541,12 @@ tracker.setMinLineLengthThresh(35.0, "Left line");
tracker.setMinPolygonAreaThresh(120.0, "Front face");
\endcode
-Furthermore, to set a name to a face see \ref advanced_cao_nam_sec.
+Furthermore, to set a name to a face see \ref mb_advanced_cao_nam.
-\section advanced_cao_sec Advanced: CAD model in cao format
+\subsection mb_advanced_cao CAD model in cao format
-\subsection advanced_cao_lin_sec How to model faces from lines
+\subsubsection mb_advanced_cao_lin How to model faces from lines
The first thing to do is to declare the differents points. Then you define each segment of the face with the index of the start point and with the index of the end point.
Finally, you define the face with the index of the segments which constitute the face.
@@ -493,7 +583,7 @@ V1
\endcode
-\subsection advanced_cao_cyl_sec How to model cylinders
+\subsubsection mb_advanced_cao_cyl How to model cylinders
The first thing to do is to declare the two points defining the cylinder axis of revolution. Then you declare the cylinder with the index of the points that define the cylinder axis of revolution and with the cylinder radius.
\note For the level of detail, in a case of a cylinder, this is taking into account by using the length of the axis of revolution to determine the visibility.
@@ -521,7 +611,7 @@ V1
\endcode
-\subsection advanced_cao_cir_sec How to model circles
+\subsubsection mb_advanced_cao_cir How to model circles
The first thing to do is to declare three points: one point for the center of the circle and two points on the circle plane (i.e. not necessary located on the perimeter of the circle but on the plane of the circle). Then you declare your circle with the radius and with index of the three points.
\note The way you declare the two points on the circle plane (clockwise or counter clockwise) will determine the direction of the normal of the circle and so will influe on the visibility of the circle.
@@ -551,9 +641,9 @@ V1
\endcode
-\subsection advanced_cao_hie_sec How to create an hierarchical model
+\subsubsection mb_advanced_cao_hie How to create a hierarchical model
It could be useful to define a complex model instead of using one big model file with all the declaration with different sub-models, each one representing a specific part of the complex model in a specific model file.
-To create an hierarchical model, the first step is to define all the elementary parts and then regroup them.
+To create a hierarchical model, the first step is to define all the elementary parts and then regroup them.
\image html img-plane-hierarchical-diagram.jpg Example of a possible hierarchical modelling of a plane.
@@ -586,10 +676,10 @@ load("tailplane.cao")
\note The path to include another model can be expressed as an absolute or a relative path (relative to the file which includes the model).
-\subsection advanced_cao_nam_sec How to set a name to a face
-To exploit the name of a face in the code, see \ref advanced_lod_sec.
+\subsubsection mb_advanced_cao_nam How to set a name to a face
+To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
-It could be useful to give a name for a face in a model in order to easily modify his LOD parameters for example, or just for debuging purpose.
+It could be useful to give a name for a face in a model in order to easily modify his LOD parameters for example, or to decide if you want to use this face or not during the tracking phase.
This is done directly in the model file :
\code
V1
@@ -622,9 +712,9 @@ load("tailplane.cao")
You can give a name to all the elements excepts for points.
-\subsection advanced_cao_lod_sec How to tune the level of detail
+\subsubsection mb_advanced_cao_lod How to tune the level of detail
-As explained in section \ref advanced_lod_sec the parameters of the lod can be set in the source code. They can also be set directly in the configuration file or in the CAD model in cao format.
+As explained in section \ref mb_advanced_lod the parameters of the lod can be set in the source code. They can also be set directly in the configuration file or in the CAD model in cao format.
The following lines show the content of the configuration file :
@@ -672,7 +762,106 @@ load("tailplane.cao")
Basically, the LOD settings expressed in configuration file will have effect on all the elements in the CAD model while the LOD settings expressed in CAD model will be specific to an element.
The natural order would be to load first the configuration file and after the CAD model.
+\subsection mb_advanced_wrml CAD model in wrml format
+\subsubsection mb_advanced_wrml_nam How to set a name to a face
+
+To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
+
+When using a wrml file, names are associated with shapes. In the example below (the model of a teabox), as only one shape is defined, all its faces will have the same name: "teabox_name".
+
+\note If you want to set different names for different faces, you have to define them in different shapes.
+
+\code
+#VRML V2.0 utf8
+
+DEF fst_0 Group {
+children [
+
+# Object "teabox"
+
+DEF teabox_name Shape {
+
+geometry DEF cube IndexedFaceSet {
+
+coord Coordinate {
+point [
+0 0 0 ,
+0 0 -0.08,
+0.165 0 -0.08,
+0.165 0 0 ,
+0.165 0.068 0 ,
+0.165 0.068 -0.08,
+0 0.068 -0.08,
+0 0.068 0 ]
+}
+
+coordIndex [
+ 0,1,2,3,-1,
+ 1,6,5,2,-1,
+ 4,5,6,7,-1,
+ 0,3,4,7,-1,
+ 5,4,3,2,-1,
+ 0,7,6,1,-1]}
+}
+
+]
+}
+\endcode
+
+\subsection mb_advanced_exclude How not to consider specific polygons
+
+When using model-based trackers, it is possible not to consider edge tracking or keypoint tracking for specific faces. To do so, <b>the faces you want to consider must have a name</b>.
+
+If you want to enable (default behavior) or disable the edge tracking on specific face it can be done via:
+
+\code
+vpMbEdgeTracker::setUseEdgeTracking("name of the face", boolean);
+\endcode
+
+If the boolean is set to False, the tracking of the edges of the faces that have the given name will be disable. If it is set to True (default behavior), it will be enable.
+
+As for the edge tracking, the same functionnality is also available when using keypoints via:
+
+\code
+vpMbKltTracker::setUseKltTracking("name of the face", boolean);
+\endcode
+
+\section mb_known_issues Known issues
+\subsection mb_known_issues_example_with_ogre Model-based trackers examples are not working with Ogre visibility ckeck
+
+If you run mbtEdgeTracking.cpp, mbtKltTracking.cpp or mbtEdgeKltTracking.cpp examples enabling Ogre visibility check (using "-o" option), you may encounter the following issue that was obtained following \ref tutorial-install-win81-msvc :
+\code
+C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
+...
+OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
+...
+*** Initializing OIS ***
+\endcode
+and then a wonderful runtime issue as in the next image:
+\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
+
+It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
+\code
+C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
+\endcode
+Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+
+\subsection mb_known_issues_tutorial_with_ogre Model-based trackers tutorials are not working with Ogre visibility ckeck
+
+This issue is similar to \ref mb_known_issues_example_with_ogre. It may occur with tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp. To make working the tutorials:
+
+- modify the code like:
+\code
+ tracker.setOgreVisibilityTest(true);
+ tracker.setOgreShowConfigDialog(true);
+\endcode
+- build the modified tutorial
+- run the binary. Now the binary should open the Ogre configuration window where you have to select an Ogre renderer that is working on your computer.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+- Press then OK to continue and start the tracking of the object.
+\subsection mb_next Next tutorial
You are now ready to see the next \ref tutorial-tracking-tt.
diff --git a/doc/tutorial-tracking-me.doc b/doc/tutorial-tracking-me.doc
index 33c35d0..89de133 100644
--- a/doc/tutorial-tracking-me.doc
+++ b/doc/tutorial-tracking-me.doc
@@ -20,16 +20,16 @@ Here after we explain line by line the program.
Images that are processed could be acquired from a firewire camera on Unix or Windows, of from an usb camera under Unix. That is allowed by including the grabber headers.
\code
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
\endcode
To display these images we then include the headers of the viewers.
\code
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/core/vpDisplayGDI.h>
+#include <visp3/core/vpDisplayX.h>
\endcode
The Graphical Display Interface (GDI) allows to display images under Windows, while X11 allows this feature under unix-like systems.
@@ -37,13 +37,13 @@ The Graphical Display Interface (GDI) allows to display images under Windows, wh
Finally, to track a line with the moving edges, we include the header of the vpMeLine class.
\code
-#include <visp/vpMeLine.h>
+#include <visp3/core/vpMeLine.h>
\endcode
In the main() function, The source code is build only if one of the grabbers and one of the viewers is available.
\code
-#if (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
\endcode
Here we create a gray level image container \c I that will contain the images acquired by our camera.
@@ -55,7 +55,7 @@ vpImage<unsigned char> I;
Then, we create a grabber instance, first for a firewire camera under Unix if libdc1394 3rd party is installed, secondly for a firewire camera under Windows if CMU1394 3rd party is installed, and lastly for an usb camera under Unix if none of the previous 3rd party are installed, and if libv4l is installed. The \ref tutorial-grabber gives more details concerning the framegrabbing.
\code
-#if defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g(false);
#elif defined(VISP_HAVE_CMU1394)
vp1394CMUGrabber g;
@@ -137,6 +137,7 @@ This example is very similar to the one presented in \ref tracking_me_line. It d
Note here that the user has to initialize the tracker on image \c I by clicking on five points located on the ellipse to track.
+\section tracking_me_next Next tutorial
You are now ready to see the next \ref tutorial-tracking-mb.
*/
diff --git a/doc/tutorial.doc b/doc/tutorial.doc
new file mode 100644
index 0000000..dd28425
--- /dev/null
+++ b/doc/tutorial.doc
@@ -0,0 +1,129 @@
+/*! \page tutorial_mainpage Tutorials
+This page references all the tutorials.
+
+- \subpage tutorial_install_pkg
+
+- \subpage tutorial_install_src
+
+- \subpage tutorial_started
+
+- \subpage tutorial_image
+
+- \subpage tutorial_calib
+
+- \subpage tutorial_tracking
+
+- \subpage tutorial_detection
+
+- \subpage tutorial_computer_vision
+
+- \subpage tutorial_vs
+
+- \subpage tutorial_bridge
+
+- \subpage tutorial_tools
+
+*/
+
+/*! \page tutorial_install_pkg Installation from packages
+This page introduces the user to the way to install ViSP from existing prebuild packages.
+
+- \subpage tutorial-install-ubuntu-package <br>In this first tutorial you will learn how to install ViSP prebuilt library from Ubuntu packages.
+
+*/
+
+/*! \page tutorial_install_src Installation from source code
+This page introduces the user to the way to install ViSP from source code.
+
+- \subpage tutorial-install-ubuntu <br>In this tutorial you will learn how to install ViSP from source on Linux Ubuntu.
+- \subpage tutorial-install-fedora <br>In this other tutorial you will learn how to install ViSP from source on Linux Fedora.
+- \subpage tutorial-install-centos <br>In this other tutorial you will learn how to install ViSP from source on Linux CentOS.
+- \subpage tutorial-install-opensuse <br>In this other tutorial you will learn how to install ViSP from source on Linux openSUSE.
+- \subpage tutorial-install-raspberry <br>In this tutorial you will learn how to install ViSP from source on Raspberry Pi.
+- \subpage tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012.
+- \subpage tutorial-install-win81-msvc <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++ 2013.
+- \subpage tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015.
+- \subpage tutorial-install-win81-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64.
+- \subpage tutorial-install-osx-homebrew <br>In this tutorial you will learn how to install ViSP from source on OSX with Homebrew.
+- \subpage tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
+
+*/
+
+/*! \page tutorial_started Getting started
+This page introduces the user to the way to start with ViSP.
+
+- \subpage tutorial-getting-started <br>This tutorial shows how to build a project that uses ViSP to read and display an image.
+- \subpage tutorial-getting-started-iOS <br>This tutorial shows how to build a project that uses ViSP on iOS devices.
+
+*/
+
+/*! \page tutorial_image Image manipulation
+This page introduces the user to the way to get, to filter or to render images.
+
+- \subpage tutorial-grabber <br>This tutorial shows how to acquire images from a camera.
+- \subpage tutorial-image-filtering <br>This tutorial shows how to filter an image with ViSP.
+- \subpage tutorial-simu-image <br>This tutorial shows how to project the image of a planar scene to a given camera position.
+
+*/
+
+/*! \page tutorial_calib Camera calibration
+This page introduces the user to the way to calibrate a camera.
+
+- \subpage tutorial-calibration <br>This tutorial explains how to calibrate a camera.
+
+*/
+
+/*! \page tutorial_tracking Tracking
+This page introduces the user to the way to track objects in images.
+
+- \subpage tutorial-tracking-blob <br>This tutorial introduces blob tracking and detection.
+- \subpage tutorial-tracking-keypoint <br>This tutorial focuses on keypoint tracking using Kanade-Lucas-Tomasi feature tracker.
+- \subpage tutorial-tracking-me <br>This tutorial focuses on line and ellipse tracking using moving-edges.
+- \subpage tutorial-tracking-mb <br>This tutorial focuses on model-based trackers using either edges, keypoints or and hybrid scheme that uses edges and keypoints.
+- \subpage tutorial-tracking-tt <br>This tutorial focuses on template trackers based on image registration approaches.
+
+*/
+
+/*! \page tutorial_detection Detection
+This page introduces the user to the way to detect features or objects in images.
+
+- \subpage tutorial-matching <br>This tutorial shows how to detect and match keypoints. New since ViSP 2.10.0.
+- \subpage tutorial-matching-deprecated <br>This tutorial shows how to detect and match SURF keypoints. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
+- \subpage tutorial-detection-barcode <br>This tutorial focuses on bar code (QR code, Data Matrix code) detection. New since ViSP 2.10.0.
+- \subpage tutorial-detection-face <br>This tutorial focuses on face detection thanks to OpenCV Haar cascade classifier. New since ViSP 2.10.0.
+- \subpage tutorial-detection-object <br>This tutorial shows how to learn keypoints detected on a known object and how to use the matched correspondences to detect and estimate the pose of the object. New since ViSP 2.10.0.
+
+*/
+
+/*! \page tutorial_computer_vision Computer vision
+This page introduces the user to the way to estimate a pose or an homography.
+
+- \subpage tutorial-pose-estimation <br>This tutorial focuses on pose estimation from planar or non planar points. New since ViSP 2.10.0.
+- \subpage tutorial-homography <br>Here we explain how to estimate an homography from couples of matched points.
+- \subpage tutorial-homography-deprecated <br>Here we explain how to estimate an homography from couples of matched points. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
+
+*/
+
+/*! \page tutorial_vs Visual servoing
+This page introduces the user to the way to achieve a visual servoing.
+
+- \subpage tutorial-ibvs <br>This tutorial explains how to simulate an IBVS.
+- \subpage tutorial-simu-robot-pioneer <br>This tutorial focuses on visual servoing simulation on a unicycle robot. The study case is a Pioneer P3-DX mobile robot equipped with a camera.
+- \subpage tutorial-boost-vs <br>This tutorial explains how to speed up the time to convergence of a visual servo.
+
+*/
+
+/*! \page tutorial_bridge Bridges over other frameworks
+This page introduces the user to the way to bridge other frameworks like OpenCV.
+
+- \subpage tutorial-bridge-opencv <br>This tutorial explicit how to convert OpenCV to/from ViSP structures such as camera parameters, images...
+
+*/
+
+/*! \page tutorial_tools Other tools
+This page introduces the user to other tools that may be useful.
+
+- \subpage tutorial-plotter <br>This tutorial explains how to plot curves in real-time during a visual servo.
+- \subpage tutorial-trace <br>This tutorial explains how to introduce trace in the code that could be enabled for debugging or disabled.
+
+*/
\ No newline at end of file
diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt
index adfe43d..6054dab 100644
--- a/example/CMakeLists.txt
+++ b/example/CMakeLists.txt
@@ -1,78 +1,97 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- calibration
- coin-simulator
- device
- direct-visual-servoing
- homography
- image
- key-point
- math
- manual
- moments/image
- moments/points
- moments/polygon
- ogre-simulator
- parse-argv
- pose-estimation
- robot-simulator/afma6
- robot-simulator/camera
- robot-simulator/viper850
- servo-afma4
- servo-afma6
- servo-biclops
- servo-pioneer
- servo-ptu46
- servo-viper650
- servo-viper850
- tools
- tracking
- video
- wireframe-simulator
-)
-
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP overall configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+project(ViSP-examples)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP)
+
+if(MSVC)
+ if(NOT VISP_SHARED)
+ foreach(flag_var
+ CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE
+ CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO
+ CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
+ CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
+ if(${flag_var} MATCHES "/MD")
+ string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}")
+ endif()
+ if(${flag_var} MATCHES "/MDd")
+ string(REGEX REPLACE "/MDd" "/MTd" ${flag_var} "${${flag_var}}")
+ endif()
+ endforeach(flag_var)
+
+ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /NODEFAULTLIB:atlthunk.lib /NODEFAULTLIB:msvcrt.lib /NODEFAULTLIB:msvcrtd.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_EXE_LINKER_FLAGS_DEBUG} /NODEFAULTLIB:libcmt.lib")
+ set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /NODEFAULTLIB:libcmtd.lib")
+ endif()
+endif()
+
+visp_add_subdirectory(calibration REQUIRED_DEPS visp_core visp_vision visp_io visp_gui)
+visp_add_subdirectory(coin-simulator REQUIRED_DEPS visp_core visp_robot visp_vs visp_ar visp_io)
+visp_add_subdirectory(device/display REQUIRED_DEPS visp_core visp_gui visp_io)
+visp_add_subdirectory(device/framegrabber REQUIRED_DEPS visp_core visp_sensor visp_io visp_gui)
+visp_add_subdirectory(device/kinect REQUIRED_DEPS visp_core visp_sensor visp_gui)
+visp_add_subdirectory(device/laserscanner REQUIRED_DEPS visp_core visp_sensor visp_io)
+visp_add_subdirectory(device/light REQUIRED_DEPS visp_core visp_robot visp_io)
+visp_add_subdirectory(direct-visual-servoing REQUIRED_DEPS visp_core visp_robot visp_visual_features visp_io visp_gui)
+visp_add_subdirectory(homography REQUIRED_DEPS visp_core visp_vision visp_io)
+visp_add_subdirectory(image REQUIRED_DEPS visp_core visp_io)
+visp_add_subdirectory(key-point REQUIRED_DEPS visp_core visp_vision visp_sensor visp_io visp_gui)
+visp_add_subdirectory(manual REQUIRED_DEPS visp_core visp_sensor visp_vs visp_robot visp_ar visp_vision visp_io visp_gui)
+visp_add_subdirectory(math REQUIRED_DEPS visp_core visp_vision visp_io)
+visp_add_subdirectory(moments/image REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui)
+visp_add_subdirectory(moments/points REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui)
+visp_add_subdirectory(moments/polygon REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui)
+visp_add_subdirectory(ogre-simulator REQUIRED_DEPS visp_core visp_vision visp_ar visp_blob visp_io visp_gui)
+visp_add_subdirectory(parse-argv REQUIRED_DEPS visp_core visp_io)
+visp_add_subdirectory(pose-estimation REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui)
+visp_add_subdirectory(robot-simulator/afma6 REQUIRED_DEPS visp_core visp_vs visp_robot visp_io visp_gui)
+visp_add_subdirectory(robot-simulator/camera REQUIRED_DEPS visp_core visp_vs visp_robot visp_io visp_gui)
+visp_add_subdirectory(robot-simulator/viper850 REQUIRED_DEPS visp_core visp_vs visp_robot visp_io visp_gui)
+visp_add_subdirectory(servo-afma4 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+visp_add_subdirectory(servo-afma6 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+visp_add_subdirectory(servo-biclops REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+visp_add_subdirectory(servo-pioneer REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+visp_add_subdirectory(servo-ptu46 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+visp_add_subdirectory(servo-viper650 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+visp_add_subdirectory(servo-viper850 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+visp_add_subdirectory(tools REQUIRED_DEPS visp_core visp_robot visp_io visp_gui)
+visp_add_subdirectory(tracking REQUIRED_DEPS visp_core visp_io visp_gui)
+visp_add_subdirectory(video REQUIRED_DEPS visp_core visp_io visp_gui)
+visp_add_subdirectory(wireframe-simulator REQUIRED_DEPS visp_core visp_robot visp_io visp_gui)
diff --git a/example/calibration/CMakeLists.txt b/example/calibration/CMakeLists.txt
index 8ca78d0..726d18f 100644
--- a/example/calibration/CMakeLists.txt
+++ b/example/calibration/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,21 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set (SOURCE
+project(example-calibration)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_vision visp_gui visp_io)
+
+set(example_cpp
camera_calibration.cpp
calibrateTsai.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
@@ -82,24 +73,7 @@ list(APPEND data2copy "${CMAKE_CURRENT_SOURCE_DIR}/circles-03.png" )
list(APPEND data2copy "${CMAKE_CURRENT_SOURCE_DIR}/circles-04.png" )
list(APPEND data2copy "${CMAKE_CURRENT_SOURCE_DIR}/circles-05.png" )
-# Since CMake 3.0.0 policy CMP0026 was introduced to disallow location property on target.
-# If CMake 3.0.0 is used, we use $<TARGET_FILE_DIR:tgt> to get the target location
-if (CMAKE_VERSION VERSION_GREATER 2.8.12)
- foreach(data ${data2copy})
- add_custom_command(
- TARGET camera_calibration
- POST_BUILD
- COMMAND ${CMAKE_COMMAND} -E copy "${data}" "$<TARGET_FILE_DIR:camera_calibration"
- )
- endforeach()
-else()
- get_target_property(target_location camera_calibration LOCATION)
- get_filename_component(target_location "${target_location}" PATH)
- foreach(data ${data2copy})
- add_custom_command(
- TARGET camera_calibration
- POST_BUILD
- COMMAND ${CMAKE_COMMAND} -E copy "${data}" "${target_location}"
- )
- endforeach()
-endif()
+# Copy the data files to the same location than the target
+foreach(data ${data2copy})
+ visp_copy_data(camera_calibration.cpp ${data})
+endforeach()
diff --git a/example/calibration/calibrateTsai.cpp b/example/calibration/calibrateTsai.cpp
index 77b299d..6a9764a 100644
--- a/example/calibration/calibrateTsai.cpp
+++ b/example/calibration/calibrateTsai.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: calibrateTsai.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tsai calibration example to estimate hand to eye transformation.
*
@@ -50,11 +46,11 @@
#include <iomanip>
#include <vector>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpCalibration.h>
-#include <visp/vpExponentialMap.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/vision/vpCalibration.h>
+#include <visp3/core/vpExponentialMap.h>
int main()
{
diff --git a/example/calibration/camera_calibration.cpp b/example/calibration/camera_calibration.cpp
index 0154607..9ac67a7 100644
--- a/example/calibration/camera_calibration.cpp
+++ b/example/calibration/camera_calibration.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: camera_calibration.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Camera calibration with chessboard or circle calibration grid.
*
@@ -40,7 +36,7 @@
*****************************************************************************/
#include <iostream>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
@@ -49,17 +45,17 @@
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
-#include <visp/vpCalibration.h>
+#include <visp3/vision/vpCalibration.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpPoint.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpXmlParserCamera.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpXmlParserCamera.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -176,9 +172,7 @@ int main(int argc, const char ** argv)
for (int i=0; i< s.boardSize.height; i++) {
for (int j=0; j< s.boardSize.width; j++) {
- vpPoint P;
- P.setWorldCoordinates(j*s.squareSize, i*s.squareSize, 0);
- model.push_back(P);
+ model.push_back( vpPoint(j*s.squareSize, i*s.squareSize, 0) );
}
}
diff --git a/example/coin-simulator/CMakeLists.txt b/example/coin-simulator/CMakeLists.txt
index 21a6189..b2b79c7 100644
--- a/example/coin-simulator/CMakeLists.txt
+++ b/example/coin-simulator/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,31 +35,25 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set (SOURCE
+project(example-coin-simulator)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_robot visp_vs visp_ar visp_io)
+
+set(example_cpp
simulateFourPoints2DCartesianCamVelocity.cpp
simulateFourPoints2DPolarCamVelocity.cpp
- simulateCircle2DCamVelocity.cpp
+ simulateCircle2DCamVelocity.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
# Add test
- add_test(${binary} ${binary} ${OPTION_TO_DESACTIVE_DISPLAY})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} ${OPTION_TO_DESACTIVE_DISPLAY})
endforeach()
diff --git a/example/coin-simulator/simulateCircle2DCamVelocity.cpp b/example/coin-simulator/simulateCircle2DCamVelocity.cpp
index b5b3c9e..3a2325e 100644
--- a/example/coin-simulator/simulateCircle2DCamVelocity.cpp
+++ b/example/coin-simulator/simulateCircle2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: simulateCircle2DCamVelocity.cpp 5263 2015-02-04 13:43:25Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a visual servoing with visualization.
*
@@ -54,24 +50,24 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#ifdef VISP_HAVE_COIN_AND_GUI
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpSimulator.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpCircle.h>
-#include <visp/vpServo.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/ar/vpSimulator.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#define GETOPTARGS "cdi:h"
#define SAVE 0
@@ -173,7 +169,9 @@ void *mainLoop (void *_simu)
vcMo[3] = 0 ;
vcMo[4] = vpMath::rad(45) ;
vcMo[5] = vpMath::rad(40) ;
- vpHomogeneousMatrix cMo(vcMo) ; ;
+ vpHomogeneousMatrix cMo(vcMo);
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Robot (=camera) location in the world frame
vpHomogeneousMatrix cMod ;
cMod[0][3] = 0 ;
@@ -185,14 +183,15 @@ void *mainLoop (void *_simu)
while (pos!=0)
{
vpServo task ;
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
robot.setMaxTranslationVelocity(4.);
// Sets the initial camera location
- robot.setPosition(cMo) ;
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
simu->setCameraPosition(cMo) ;
if (pos==1) cMod[2][3] = 0.32 ;
@@ -239,10 +238,11 @@ void *mainLoop (void *_simu)
double t = vpTime::measureTimeMs();
if (iter==1) std::cout << "get the robot position" << std::endl;
- robot.getPosition(cMo) ;
+ wMc = robot.getPosition() ;
if (iter==1) std::cout << "new circle position" << std::endl;
//retrieve x,y and Z of the vpCircle structure
+ cMo = wMc.inverse() * wMo;
circle.track(cMo) ;
vpFeatureBuilder::create(p,circle);
diff --git a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
index b1be43c..ed58895 100644
--- a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
+++ b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: simulateFourPoints2DCartesianCamVelocity.cpp 5263 2015-02-04 13:43:25Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a visual servoing with visualization.
*
@@ -52,24 +48,24 @@
from the camera and from an external view using vpSimulator.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#ifdef VISP_HAVE_COIN_AND_GUI
+#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpSimulator.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/ar/vpSimulator.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#define GETOPTARGS "di:h"
#define SAVE 0
@@ -162,7 +158,7 @@ void *mainLoop (void *_simu)
simu->initMainApplication() ;
vpServo task ;
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
@@ -186,12 +182,16 @@ void *mainLoop (void *_simu)
vcMo[4] = vpMath::rad(0) ;
vcMo[5] = vpMath::rad(40) ;
- vpHomogeneousMatrix cMo(vcMo) ;
- robot.setPosition(cMo) ;
+ vpHomogeneousMatrix cMo(vcMo);
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera location in world frame
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
simu->setCameraPosition(cMo) ;
simu->getCameraPosition(cMo) ;
- robot.setPosition(cMo) ;
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
robot.setMaxTranslationVelocity(4.);
vpCameraParameters cam ;
@@ -258,7 +258,8 @@ void *mainLoop (void *_simu)
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
- robot.getPosition(cMo) ;
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
for (int i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
diff --git a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
index 9ab7e76..6700ead 100644
--- a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
+++ b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: simulateFourPoints2DPolarCamVelocity.cpp 5263 2015-02-04 13:43:25Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a visual servoing with visualization.
*
@@ -50,24 +46,24 @@
of the four points.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#ifdef VISP_HAVE_COIN_AND_GUI
+#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpSimulator.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePointPolar.h>
-#include <visp/vpServo.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/ar/vpSimulator.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#define GETOPTARGS "di:h"
#define SAVE 0
@@ -160,7 +156,7 @@ void *mainLoop (void *_simu)
simu->initMainApplication() ;
vpServo task ;
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
@@ -176,12 +172,16 @@ void *mainLoop (void *_simu)
vcMo[4] = vpMath::rad(0) ;
vcMo[5] = vpMath::rad(90) ;
- vpHomogeneousMatrix cMo(vcMo) ;
- robot.setPosition(cMo) ;
+ vpHomogeneousMatrix cMo(vcMo);
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera location in world frame
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
simu->setCameraPosition(cMo) ;
simu->getCameraPosition(cMo) ;
- robot.setPosition(cMo) ;
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
vpCameraParameters cam ;
@@ -273,7 +273,8 @@ void *mainLoop (void *_simu)
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
- robot.getPosition(cMo) ;
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
for (int i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
diff --git a/example/device/CMakeLists.txt b/example/device/CMakeLists.txt
deleted file mode 100644
index 81f049d..0000000
--- a/example/device/CMakeLists.txt
+++ /dev/null
@@ -1,54 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- display
- framegrabber
- kinect
- laserscanner
- light
-)
-
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
diff --git a/example/device/display/CMakeLists.txt b/example/device/display/CMakeLists.txt
index befd33c..6dd20f9 100644
--- a/example/device/display/CMakeLists.txt
+++ b/example/device/display/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-device-display)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_gui visp_io)
+
+set(example_cpp
displaySequence.cpp
displayGDI.cpp
displayD3D.cpp
@@ -53,18 +51,10 @@ set(SOURCE
displayXMulti.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/device/display/displayD3D.cpp b/example/device/display/displayD3D.cpp
index ccf1ed2..d94f9fb 100755
--- a/example/device/display/displayD3D.cpp
+++ b/example/device/display/displayD3D.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayD3D.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Windows' D3D Display Test
*
@@ -48,17 +44,17 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if ( defined(VISP_HAVE_D3D9) )
-#include <visp/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayD3D.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
/*!
\example displayD3D.cpp
diff --git a/example/device/display/displayGDI.cpp b/example/device/display/displayGDI.cpp
index 41724ea..5497c95 100755
--- a/example/device/display/displayGDI.cpp
+++ b/example/device/display/displayGDI.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayGDI.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Windows' GDI Display Test
*
@@ -47,17 +43,17 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if ( defined(_WIN32) && defined(VISP_HAVE_GDI) )
-#include <visp/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGDI.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
/*!
\example displayGDI.cpp
diff --git a/example/device/display/displayGTK.cpp b/example/device/display/displayGTK.cpp
index ef7dbe9..a10b0ff 100644
--- a/example/device/display/displayGTK.cpp
+++ b/example/device/display/displayGTK.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayGTK.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image on the disk and display it using GTK.
*
@@ -49,20 +45,20 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_GTK
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
/*!
\example displayGTK.cpp
diff --git a/example/device/display/displayOpenCV.cpp b/example/device/display/displayOpenCV.cpp
index 97d4fe0..9d93b92 100644
--- a/example/device/display/displayOpenCV.cpp
+++ b/example/device/display/displayOpenCV.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayOpenCV.cpp 5005 2014-11-24 08:25:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image on the disk and display it using OpenCV.
*
@@ -47,19 +43,19 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#if defined(VISP_HAVE_OPENCV)
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
-#include <visp/vpTime.h>
+#include <visp3/core/vpTime.h>
/*!
\example displayOpenCV.cpp
diff --git a/example/device/display/displaySequence.cpp b/example/device/display/displaySequence.cpp
index a7dd3ee..a7f820f 100644
--- a/example/device/display/displaySequence.cpp
+++ b/example/device/display/displaySequence.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displaySequence.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image sequence from the disk and display it.
*
@@ -52,24 +48,24 @@
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#include <stdio.h>
#include <stdlib.h>
#include <sstream>
#include <iomanip>
#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
-#include <visp/vpTime.h>
+#include <visp3/core/vpTime.h>
/*!
\example displaySequence.cpp
diff --git a/example/device/display/displayX.cpp b/example/device/display/displayX.cpp
index b028054..c0813a1 100644
--- a/example/device/display/displayX.cpp
+++ b/example/device/display/displayX.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayX.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image on the disk and display it using X11.
*
@@ -48,18 +44,18 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#ifdef VISP_HAVE_X11
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
-#include <visp/vpTime.h>
+#include <visp3/core/vpTime.h>
/*!
\example displayX.cpp
diff --git a/example/device/display/displayXMulti.cpp b/example/device/display/displayXMulti.cpp
index 4bff550..7f9eddd 100644
--- a/example/device/display/displayXMulti.cpp
+++ b/example/device/display/displayXMulti.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: displayXMulti.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image on the disk and display it using X11.
*
@@ -48,20 +44,20 @@
the overlayed features in an image on the disk.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_X11
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
/*!
\example displayXMulti.cpp
diff --git a/example/device/framegrabber/CMakeLists.txt b/example/device/framegrabber/CMakeLists.txt
index acba5bf..997371b 100644
--- a/example/device/framegrabber/CMakeLists.txt
+++ b/example/device/framegrabber/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-device-framegrabber)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_sensor visp_io visp_gui)
+
+set(example_cpp
grab1394Two.cpp
grab1394CMU.cpp
grabDisk.cpp
@@ -54,18 +52,10 @@ set(SOURCE
grabOpenCV-2.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/device/framegrabber/grab1394CMU.cpp b/example/device/framegrabber/grab1394CMU.cpp
index fdca9c3..052790f 100644
--- a/example/device/framegrabber/grab1394CMU.cpp
+++ b/example/device/framegrabber/grab1394CMU.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grab1394CMU.cpp 4659 2014-02-09 14:11:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Video capture example based on CMU 1394 Digital Camera SDK.
*
@@ -50,14 +46,14 @@
#include <stdlib.h>
#include <iostream>
-#include <visp/vpConfig.h>
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
#define GRAB_COLOR
diff --git a/example/device/framegrabber/grab1394Two.cpp b/example/device/framegrabber/grab1394Two.cpp
index 401e31d..738c7ca 100644
--- a/example/device/framegrabber/grab1394Two.cpp
+++ b/example/device/framegrabber/grab1394Two.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grab1394Two.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Firewire cameras video capture.
*
@@ -55,23 +51,23 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <list>
-#if defined(VISP_HAVE_DC1394_2)
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpTime.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRGBa.h>
+#if defined(VISP_HAVE_DC1394)
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpRGBa.h>
#define GRAB_CxOLOR
diff --git a/example/device/framegrabber/grabDirectShow.cpp b/example/device/framegrabber/grabDirectShow.cpp
index cea7264..1a03f03 100644
--- a/example/device/framegrabber/grabDirectShow.cpp
+++ b/example/device/framegrabber/grabDirectShow.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabDirectShow.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Acquire images using DirectShow (under Windows only) and display it
* using GTK or GDI.
@@ -41,8 +37,8 @@
*
*****************************************************************************/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
/*!
\file grabDirectShow.cpp
@@ -55,13 +51,13 @@
#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-#include <visp/vpDirectShowGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
// List of allowed command line options
#define GETOPTARGS "dhn:o:"
diff --git a/example/device/framegrabber/grabDirectShowMulti.cpp b/example/device/framegrabber/grabDirectShowMulti.cpp
index 312c80a..aa922ba 100644
--- a/example/device/framegrabber/grabDirectShowMulti.cpp
+++ b/example/device/framegrabber/grabDirectShowMulti.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabDirectShowMulti.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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.
*
@@ -41,8 +38,8 @@
*
*****************************************************************************/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
/*!
\file grabDirectShowMulti.cpp
@@ -60,13 +57,13 @@
#if defined (VISP_HAVE_DIRECTSHOW)
#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-#include <visp/vpDirectShowGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
// List of allowed command line options
diff --git a/example/device/framegrabber/grabDisk.cpp b/example/device/framegrabber/grabDisk.cpp
index 3fe4981..dc2b6a0 100644
--- a/example/device/framegrabber/grabDisk.cpp
+++ b/example/device/framegrabber/grabDisk.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabDisk.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Read an image sequence from the disk and display it.
*
@@ -41,19 +37,19 @@
*****************************************************************************/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
-#include <visp/vpDiskGrabber.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpImage.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/io/vpDiskGrabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
/*!
\file grabDisk.cpp
diff --git a/example/device/framegrabber/grabOpenCV-2.cpp b/example/device/framegrabber/grabOpenCV-2.cpp
index a7f9c00..26eb174 100644
--- a/example/device/framegrabber/grabOpenCV-2.cpp
+++ b/example/device/framegrabber/grabOpenCV-2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabOpenCV-2.cpp 5005 2014-11-24 08:25:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Acquire images using OpenCV cv::VideoCapture.
*
@@ -39,7 +35,7 @@
*
*****************************************************************************/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
/*!
\example grabOpenCV-2.cpp
@@ -50,14 +46,14 @@
#include <iostream>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpOpenCVGrabber.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
// usage: binary <device name>
diff --git a/example/device/framegrabber/grabOpenCV.cpp b/example/device/framegrabber/grabOpenCV.cpp
index 82b278c..347fb54 100644
--- a/example/device/framegrabber/grabOpenCV.cpp
+++ b/example/device/framegrabber/grabOpenCV.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabOpenCV.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Acquire images using DirectShow (under Windows only) and display it
* using GTK or GDI.
@@ -40,8 +36,8 @@
*
*****************************************************************************/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
/*!
\file grabOpenCV.cpp
@@ -53,12 +49,12 @@
#if defined (VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
-#include <visp/vpOpenCVGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
// List of allowed command line options
#define GETOPTARGS "dhn:o:D:"
diff --git a/example/device/framegrabber/grabV4l2.cpp b/example/device/framegrabber/grabV4l2.cpp
index 8670fb8..a9f229d 100644
--- a/example/device/framegrabber/grabV4l2.cpp
+++ b/example/device/framegrabber/grabV4l2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: grabV4l2.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Acquire images using 1394 device with cfox (MAC OSX) and display it
* using GTK or GTK.
@@ -41,8 +37,8 @@
*****************************************************************************/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <stdlib.h>
/*!
\file grabV4l2.cpp
@@ -55,14 +51,14 @@
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpTime.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
// List of allowed command line options
#define GETOPTARGS "df:i:hn:o:p:s:t:v:x"
diff --git a/example/device/kinect/CMakeLists.txt b/example/device/kinect/CMakeLists.txt
index 4bc4708..ed54369 100644
--- a/example/device/kinect/CMakeLists.txt
+++ b/example/device/kinect/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,26 +35,19 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- kinectAcquisition.cpp
-)
+project(example-device-kinect)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_sensor visp_gui)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ kinectAcquisition.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/device/kinect/kinectAcquisition.cpp b/example/device/kinect/kinectAcquisition.cpp
index e043eef..436ae76 100644
--- a/example/device/kinect/kinectAcquisition.cpp
+++ b/example/device/kinect/kinectAcquisition.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: kinectAcquisition.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Kinect example.
*
@@ -48,20 +44,20 @@
*/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
#ifdef VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpKinect.h>
-#include <visp/vpTime.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/sensor/vpKinect.h>
+#include <visp3/core/vpTime.h>
int main() {
try {
diff --git a/example/device/laserscanner/CMakeLists.txt b/example/device/laserscanner/CMakeLists.txt
index 5ca5f10..28ea84b 100644
--- a/example/device/laserscanner/CMakeLists.txt
+++ b/example/device/laserscanner/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,20 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-device-laserscanner)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_sensor visp_io)
+
+set(example_cpp
SickLDMRS-Acq.cpp
SickLDMRS-Process.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/device/laserscanner/SickLDMRS-Acq.cpp b/example/device/laserscanner/SickLDMRS-Acq.cpp
index 940a7ac..01fba82 100644
--- a/example/device/laserscanner/SickLDMRS-Acq.cpp
+++ b/example/device/laserscanner/SickLDMRS-Acq.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: SickLDMRS-Acq.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Sick LD-MRS laser driver.
*
@@ -50,9 +46,9 @@
platforms since the Sick LD-MRS driver was not ported to Windows.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpSickLDMRS.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/sensor/vpSickLDMRS.h>
+#include <visp3/io/vpParseArgv.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
diff --git a/example/device/laserscanner/SickLDMRS-Process.cpp b/example/device/laserscanner/SickLDMRS-Process.cpp
index 9a2ad7b..f714d9c 100644
--- a/example/device/laserscanner/SickLDMRS-Process.cpp
+++ b/example/device/laserscanner/SickLDMRS-Process.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: SickLDMRS-Process.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Sick LD-MRS laser driver.
*
@@ -61,18 +57,20 @@
select the layers to proceed.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpSickLDMRS.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/sensor/vpSickLDMRS.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#ifdef VISP_HAVE_MODULE_GUI
+# include <visp3/gui/vpDisplayX.h>
+# include <visp3/gui/vpDisplayGDI.h>
+# include <visp3/gui/vpDisplayGTK.h>
+#endif
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpIoTools.h>
#if ( !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
@@ -113,7 +111,8 @@ void *laser_display_and_save_loop(void *)
}
}
- vpDisplay *display;
+ vpDisplay *display = NULL;
+#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
@@ -122,6 +121,7 @@ void *laser_display_and_save_loop(void *)
display = new vpDisplayGTK;
#endif
display->init (map, 10, 10, "Laser scan");
+#endif
unsigned int iter = 0;
for ( ; ; ) {
@@ -236,7 +236,7 @@ void *laser_acq_loop(void *)
void *camera_acq_and_display_loop(void *)
{
-#ifdef VISP_HAVE_DC1394_2
+#ifdef VISP_HAVE_DC1394
try {
// Initialize the firewire framegrabber
vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib
@@ -253,7 +253,8 @@ void *camera_acq_and_display_loop(void *)
g.acquire(I); // Acquire an image
I.quarterSizeImage(Q);
- vpDisplay *display;
+ vpDisplay *display = NULL;
+#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
@@ -262,7 +263,8 @@ void *camera_acq_and_display_loop(void *)
display = new vpDisplayGTK;
#endif
display->init (Q, 320, 10, "Camera");
-
+#endif
+
// Create a file with cameraimage time stamps
std::ofstream fdimage_ts;
if (save) {
diff --git a/example/device/light/CMakeLists.txt b/example/device/light/CMakeLists.txt
index 2628f47..a38c889 100644
--- a/example/device/light/CMakeLists.txt
+++ b/example/device/light/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,26 +35,19 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- ringLight.cpp
-)
+project(example-device-laserscanner)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_robot visp_io)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ ringLight.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/device/light/ringLight.cpp b/example/device/light/ringLight.cpp
index 324ea03..c6a02d7 100644
--- a/example/device/light/ringLight.cpp
+++ b/example/device/light/ringLight.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: ringLight.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of ring light control.
*
@@ -47,8 +43,8 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
#if defined VISP_HAVE_PARPORT
@@ -56,9 +52,9 @@
#include <stdio.h>
#include <iostream>
-#include <visp/vpRingLight.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpTime.h>
+#include <visp3/robot/vpRingLight.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpTime.h>
// List of allowed command line options
#define GETOPTARGS "d:hn:ot:"
diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt
index 288a5ab..6e33223 100644
--- a/example/direct-visual-servoing/CMakeLists.txt
+++ b/example/direct-visual-servoing/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,20 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- photometricVisualServoing.cpp
-)
+project(example-direct-vvs)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_robot visp_visual_features visp_io visp_gui)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ photometricVisualServoing.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/direct-visual-servoing/photometricVisualServoing.cpp b/example/direct-visual-servoing/photometricVisualServoing.cpp
index c9af07b..6e16ab3 100755
--- a/example/direct-visual-servoing/photometricVisualServoing.cpp
+++ b/example/direct-visual-servoing/photometricVisualServoing.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: photometricVisualServoing.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,26 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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.
*
- *
- *
* Authors:
* Eric Marchand
* Christophe Collewet
@@ -45,33 +40,33 @@
*/
-#include <visp/vpDebug.h>
+#include <visp3/core/vpDebug.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageTools.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpRobotCamera.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <visp/vpFeatureLuminance.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/visual_features/vpFeatureLuminance.h>
+#include <visp3/io/vpParseArgv.h>
-#include <visp/vpImageSimulator.h>
+#include <visp3/robot/vpImageSimulator.h>
#include <stdlib.h>
#define Z 1
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:n:h"
@@ -302,6 +297,8 @@ main(int argc, const char ** argv)
//camera desired position
vpHomogeneousMatrix cMo ;
cMo.buildFrom(0,0,1.2,vpMath::rad(15),vpMath::rad(-5),vpMath::rad(20));
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
//set the robot at the desired position
sim.setCameraPosition(cMo) ;
@@ -340,9 +337,10 @@ main(int argc, const char ** argv)
}
#endif
// create the robot (here a simulated free flying camera)
- vpRobotCamera robot ;
+ vpSimulatorCamera robot;
robot.setSamplingTime(0.04);
- robot.setPosition(cMo) ;
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc);
// ------------------------------------------------------
// Visual feature, interaction matrix, error
@@ -460,9 +458,9 @@ main(int argc, const char ** argv)
std::cout << " |Tc| = " << sqrt(v.sumSquare()) << std::endl;
// send the robot velocity
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.getPosition(cMo) ;
-
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
}
while(normeError > 10000 && iter < opt_niter);
diff --git a/example/homography/CMakeLists.txt b/example/homography/CMakeLists.txt
index 74aeeb3..b48423e 100644
--- a/example/homography/CMakeLists.txt
+++ b/example/homography/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,32 +35,26 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-homography)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_vision visp_io)
+
+set(example_cpp
homographyHartleyDLT2DObject.cpp
homographyHLM2DObject.cpp
homographyHLM3DObject.cpp
homographyRansac2DObject.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
# Add test
- ADD_TEST(${binary} ${binary})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target})
endforeach()
diff --git a/example/homography/homographyHLM2DObject.cpp b/example/homography/homographyHLM2DObject.cpp
index 3966301..e3e9329 100644
--- a/example/homography/homographyHLM2DObject.cpp
+++ b/example/homography/homographyHLM2DObject.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: homographyHLM2DObject.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of the HLM (Malis) homography estimation algorithm.
*
@@ -56,17 +52,17 @@
*/
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdlib.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/homography/homographyHLM3DObject.cpp b/example/homography/homographyHLM3DObject.cpp
index ffe94ae..91d3cdd 100644
--- a/example/homography/homographyHLM3DObject.cpp
+++ b/example/homography/homographyHLM3DObject.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: homographyHLM3DObject.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Test the HLM (Malis) homography estimation algorithm with a 3D object.
*
@@ -54,17 +50,17 @@
*/
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdlib.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/homography/homographyHartleyDLT2DObject.cpp b/example/homography/homographyHartleyDLT2DObject.cpp
index 86561fd..23959a8 100644
--- a/example/homography/homographyHartleyDLT2DObject.cpp
+++ b/example/homography/homographyHartleyDLT2DObject.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: homographyHartleyDLT2DObject.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of the HartleyDLT homography estimation algorithm.
*
@@ -55,17 +51,17 @@
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdlib.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/homography/homographyRansac2DObject.cpp b/example/homography/homographyRansac2DObject.cpp
index 7e092a2..1e85ca0 100644
--- a/example/homography/homographyRansac2DObject.cpp
+++ b/example/homography/homographyRansac2DObject.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: homographyRansac2DObject.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of the Ransac homography estimation algorithm.
*
@@ -57,19 +53,19 @@
*/
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDebug.h>
-#include <visp/vpRansac.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpRansac.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdlib.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -155,8 +151,8 @@ main(int argc, const char ** argv)
std::vector<vpPoint> P(nbpt); // Point to be tracked
std::vector<double> xa(nbpt), ya(nbpt), xb(nbpt), yb(nbpt);
- std::vector<vpPoint> aP(nbpt); // Point to be tracked
- std::vector<vpPoint> bP(nbpt); // Point to be tracked
+ std::vector<vpPoint> aP(nbpt); // Point to be tracked
+ std::vector<vpPoint> bP(nbpt); // Point to be tracked
P[0].setWorldCoordinates(-L,-L, 0 ) ; // inlier
P[1].setWorldCoordinates(2*L,-L, 0 ) ; // inlier
diff --git a/example/image/CMakeLists.txt b/example/image/CMakeLists.txt
index b95ae4c..53aa29d 100644
--- a/example/image/CMakeLists.txt
+++ b/example/image/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,33 +35,23 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- imageDiskRW.cpp
-)
+project(example-image)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_io)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ imageDiskRW.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
-endforeach()
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
-add_test(imageDiskRW imageDiskRW)
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target})
+endforeach()
diff --git a/example/image/imageDiskRW.cpp b/example/image/imageDiskRW.cpp
index 87db3f1..5aaeb1f 100644
--- a/example/image/imageDiskRW.cpp
+++ b/example/image/imageDiskRW.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: imageDiskRW.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Reading and writting images on the disk.
*
@@ -59,11 +55,11 @@
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
#include <stdlib.h>
#include <stdio.h>
// List of allowed command line options
diff --git a/example/key-point/CMakeLists.txt b/example/key-point/CMakeLists.txt
index fc8b2d3..297f3ff 100644
--- a/example/key-point/CMakeLists.txt
+++ b/example/key-point/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,29 +35,22 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-keypoint)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_vision visp_sensor visp_io visp_gui)
+
+set(example_cpp
keyPointSurf.cpp
planarObjectDetector.cpp
fernClassifier.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/key-point/fernClassifier.cpp b/example/key-point/fernClassifier.cpp
index cf3cb61..1023fcd 100644
--- a/example/key-point/fernClassifier.cpp
+++ b/example/key-point/fernClassifier.cpp
@@ -1,36 +1,32 @@
/****************************************************************************
*
- * $Id: fernClassifier.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.GPL at the root directory of this source
+ * 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Detection of points of interests and matching using the Ferns classifier.
*
@@ -50,24 +46,24 @@
Detection of points of interests and matching using a Fern classifier.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
#include <iostream>
#include <stdlib.h>
-#include <visp/vpFernClassifier.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpHomography.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpTime.h>
+#include <visp3/vision/vpFernClassifier.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpTime.h>
#include <iomanip>
#define GETOPTARGS "hlcdb:i:p"
diff --git a/example/key-point/keyPointSurf.cpp b/example/key-point/keyPointSurf.cpp
index 4eaacdb..72f73c0 100644
--- a/example/key-point/keyPointSurf.cpp
+++ b/example/key-point/keyPointSurf.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: keyPointSurf.cpp 5202 2015-01-24 09:29:06Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tracking of Surf key points.
*
@@ -53,25 +49,25 @@
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
-#include <visp/vpKeyPointSurf.h>
+#include <visp3/vision/vpKeyPointSurf.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:h"
diff --git a/example/key-point/planarObjectDetector.cpp b/example/key-point/planarObjectDetector.cpp
index fbdfea1..009bc7d 100644
--- a/example/key-point/planarObjectDetector.cpp
+++ b/example/key-point/planarObjectDetector.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: planarObjectDetector.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Detection of planar surface using Fern classifier.
*
@@ -50,27 +46,27 @@
Tracking of planar surface using Fern classifier.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
#include <iostream>
#include <stdlib.h>
-#include <visp/vpPlanarObjectDetector.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpHomography.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpTime.h>
+#include <visp3/vision/vpPlanarObjectDetector.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpTime.h>
#include <iomanip>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#define GETOPTARGS "hlcdb:i:p"
diff --git a/example/manual/CMakeLists.txt b/example/manual/CMakeLists.txt
index 0e35823..f50b75a 100644
--- a/example/manual/CMakeLists.txt
+++ b/example/manual/CMakeLists.txt
@@ -1,75 +1,65 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP manual examples build.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- geometric-features/manGeometricFeatures.cpp
- hello-world/CMake/HelloWorld.cpp
- image-manipulation/manDisplay.cpp
- image-manipulation/manGrab1394-2.cpp
- image-manipulation/manGrabDirectShow.cpp
- image-manipulation/manGrabDisk.cpp
- image-manipulation/manGrabV4l2.cpp
- ogre/HelloWorldOgre.cpp
- ogre/HelloWorldOgreAdvanced.cpp
- simulation/manServo4PointsDisplay.cpp
- simulation/manSimu4Dots.cpp
- simulation/manSimu4Points.cpp
- moments/manServoMomentsSimple.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
-endforeach()
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP manual examples build.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+project(example-manual)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_sensor visp_vs visp_robot visp_ar visp_vision visp_io visp_gui)
+
+set(example_cpp
+ geometric-features/manGeometricFeatures.cpp
+ hello-world/CMake/HelloWorld.cpp
+ image-manipulation/manDisplay.cpp
+ image-manipulation/manGrab1394-2.cpp
+ image-manipulation/manGrabDirectShow.cpp
+ image-manipulation/manGrabDisk.cpp
+ image-manipulation/manGrabV4l2.cpp
+ ogre/HelloWorldOgre.cpp
+ ogre/HelloWorldOgreAdvanced.cpp
+ simulation/manServo4PointsDisplay.cpp
+ simulation/manSimu4Dots.cpp
+ simulation/manSimu4Points.cpp
+ moments/manServoMomentsSimple.cpp
+)
+
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
+endforeach()
diff --git a/example/manual/geometric-features/manGeometricFeatures.cpp b/example/manual/geometric-features/manGeometricFeatures.cpp
index c3b11bd..e98a533 100644
--- a/example/manual/geometric-features/manGeometricFeatures.cpp
+++ b/example/manual/geometric-features/manGeometricFeatures.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: manGeometricFeatures.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Geometric features example.
*
@@ -53,24 +49,24 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpImageIo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpImageIo.h>
// For 2D image
-#include <visp/vpImage.h>
+#include <visp3/core/vpImage.h>
// Video device interface
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGTK.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
// For frame transformation and projection
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
// Needed geometric features
-#include <visp/vpPoint.h>
-#include <visp/vpLine.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpCircle.h>
-#include <visp/vpSphere.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpSphere.h>
#include <iostream>
diff --git a/example/manual/hello-world/Autotools/HelloWorld.cpp b/example/manual/hello-world/Autotools/HelloWorld.cpp
index 123a582..09062b9 100644
--- a/example/manual/hello-world/Autotools/HelloWorld.cpp
+++ b/example/manual/hello-world/Autotools/HelloWorld.cpp
@@ -1,8 +1,8 @@
#include <iostream>
-#include <visp/vpDebug.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
int main()
diff --git a/example/manual/hello-world/Autotools/have_visp.m4 b/example/manual/hello-world/Autotools/have_visp.m4
index 951eb3d..adf5d58 100644
--- a/example/manual/hello-world/Autotools/have_visp.m4
+++ b/example/manual/hello-world/Autotools/have_visp.m4
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: have_visp.m4 4574 2014-01-09 08:48:51Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
diff --git a/example/manual/hello-world/CMake/HelloWorld.cpp b/example/manual/hello-world/CMake/HelloWorld.cpp
index 86acb03..c3e7b6d 100644
--- a/example/manual/hello-world/CMake/HelloWorld.cpp
+++ b/example/manual/hello-world/CMake/HelloWorld.cpp
@@ -1,7 +1,44 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Hello world example.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
#include <iostream>
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpThetaUVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
#include <limits>
int main()
diff --git a/example/manual/hello-world/Makefile/HelloWorld.cpp b/example/manual/hello-world/Makefile/HelloWorld.cpp
index 123a582..09062b9 100644
--- a/example/manual/hello-world/Makefile/HelloWorld.cpp
+++ b/example/manual/hello-world/Makefile/HelloWorld.cpp
@@ -1,8 +1,8 @@
#include <iostream>
-#include <visp/vpDebug.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
int main()
diff --git a/example/manual/image-manipulation/manDisplay.cpp b/example/manual/image-manipulation/manDisplay.cpp
index 9ee2439..99c599a 100644
--- a/example/manual/image-manipulation/manDisplay.cpp
+++ b/example/manual/image-manipulation/manDisplay.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manDisplay.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Display example.
*
@@ -47,11 +43,11 @@
*/
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpColor.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpImagePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImagePoint.h>
int main()
{
diff --git a/example/manual/image-manipulation/manGrab1394-2.cpp b/example/manual/image-manipulation/manGrab1394-2.cpp
index 81d2ce5..2a1df9c 100644
--- a/example/manual/image-manipulation/manGrab1394-2.cpp
+++ b/example/manual/image-manipulation/manGrab1394-2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manGrab1394-2.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Images grabbing example.
*
@@ -54,14 +50,14 @@
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <stdlib.h>
#include <stdio.h>
int main()
{
-#ifdef VISP_HAVE_DC1394_2
+#ifdef VISP_HAVE_DC1394
try {
unsigned int ncameras; // Number of cameras on the bus
vp1394TwoGrabber g;
diff --git a/example/manual/image-manipulation/manGrabDirectShow.cpp b/example/manual/image-manipulation/manGrabDirectShow.cpp
index 43f925c..360a429 100644
--- a/example/manual/image-manipulation/manGrabDirectShow.cpp
+++ b/example/manual/image-manipulation/manGrabDirectShow.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manGrabDirectShow.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Images grabbing example.
*
@@ -52,10 +48,10 @@
*/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDirectShowGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
int main()
{
diff --git a/example/manual/image-manipulation/manGrabDisk.cpp b/example/manual/image-manipulation/manGrabDisk.cpp
index 6bafc46..03d1a2d 100644
--- a/example/manual/image-manipulation/manGrabDisk.cpp
+++ b/example/manual/image-manipulation/manGrabDisk.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manGrabDisk.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Images grabbing example.
*
@@ -52,10 +48,10 @@
*/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDiskGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpDiskGrabber.h>
int main()
{
diff --git a/example/manual/image-manipulation/manGrabOpenCV.cpp b/example/manual/image-manipulation/manGrabOpenCV.cpp
index c2928ac..9f6f6b1 100644
--- a/example/manual/image-manipulation/manGrabOpenCV.cpp
+++ b/example/manual/image-manipulation/manGrabOpenCV.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manGrabOpenCV.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Images grabbing example.
*
@@ -44,9 +40,9 @@
\brief Images grabbing example with the vpOpenCVGrabber class.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpOpenCVGrabber.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
int main(){
vpImage<unsigned char> I; // Grey level image
diff --git a/example/manual/image-manipulation/manGrabV4l2.cpp b/example/manual/image-manipulation/manGrabV4l2.cpp
index 21910d5..27df423 100644
--- a/example/manual/image-manipulation/manGrabV4l2.cpp
+++ b/example/manual/image-manipulation/manGrabV4l2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: manGrabV4l2.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Images grabbing example.
*
@@ -52,8 +48,8 @@
*/
-#include <visp/vpImage.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
int main()
{
diff --git a/example/manual/moments/manServoMomentsSimple.cpp b/example/manual/moments/manServoMomentsSimple.cpp
index 6cebf41..b44f45e 100644
--- a/example/manual/moments/manServoMomentsSimple.cpp
+++ b/example/manual/moments/manServoMomentsSimple.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoMomentPolygon.cpp 3323 2011-09-13 15:23:56Z fnovotny $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example of visual servoing with moments using a polygon as object container
*
@@ -45,16 +41,16 @@
*/
-#include <visp/vpPoint.h> //the basic tracker
+#include <visp3/core/vpPoint.h> //the basic tracker
#include <vector> //store the polygon
-#include <visp/vpMomentObject.h> //transmit the polygon to the object
-#include <visp/vpMomentCommon.h> //update the common database with the object
-#include <visp/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
-#include <visp/vpServo.h> //visual servoing task
-#include <visp/vpRobotCamera.h>
-#include <visp/vpPlane.h>
-#include <visp/vpException.h>
+#include <visp3/core/vpMomentObject.h> //transmit the polygon to the object
+#include <visp3/core/vpMomentCommon.h> //update the common database with the object
+#include <visp3/visual_features/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
+#include <visp3/vs/vpServo.h> //visual servoing task
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpException.h>
#include <limits>
#include <iostream> //some console output
//this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane form
@@ -90,14 +86,15 @@ int main()
vpHomogeneousMatrix cMo(0.1,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
cMoToABC(cMo,A,B,C);
cMoToABC(cdMo,Ad,Bd,Cd);
// Define source and destination polygons
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
- p.track(cMo) ;
+ vpPoint p(x[i],y[i],0.0);
+ p.track(cMo);
vec_p.push_back(p);
p.track(cdMo) ;
vec_p_d.push_back(p);
@@ -142,18 +139,19 @@ int main()
al->init();
al->error(*al);
//param robot
- vpRobotCamera robot ;
+ vpSimulatorCamera robot ;
float sampling_time = 0.010f; // Sampling period in seconds
robot.setSamplingTime(sampling_time);
- robot.setPosition(cMo);
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc);
do{
- robot.getPosition(cMo);
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
vec_p.clear();
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
+ vpPoint p(x[i],y[i],0.0);
p.track(cMo) ;
vec_p.push_back(p);
}
diff --git a/example/manual/ogre/HelloWorldOgre.cpp b/example/manual/ogre/HelloWorldOgre.cpp
index e9c91c8..17d83e7 100644
--- a/example/manual/ogre/HelloWorldOgre.cpp
+++ b/example/manual/ogre/HelloWorldOgre.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: HelloWorldOgre.cpp 5128 2015-01-06 11:46:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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.
*
@@ -46,19 +43,19 @@
#include <iostream>
-#include <visp/vpOpenCVGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpAROgre.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/ar/vpAROgre.h>
int main()
{
try {
#if defined(VISP_HAVE_OGRE)
-#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394_2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
// Image to stock gathered data
// Here we acquire a color image. The consequence will be that
@@ -71,7 +68,7 @@ int main()
vpV4l2Grabber grabber;
grabber.open(I);
grabber.acquire(I);
-#elif defined(VISP_HAVE_DC1394_2)
+#elif defined(VISP_HAVE_DC1394)
// libdc1394-2
vp1394TwoGrabber grabber;
grabber.open(I);
@@ -133,7 +130,7 @@ int main()
// Rendering loop, ended with on escape
while(ogre.continueRendering()){
// Acquire a new image
-#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394)
grabber.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
grabber >> frame;
diff --git a/example/manual/ogre/HelloWorldOgreAdvanced.cpp b/example/manual/ogre/HelloWorldOgreAdvanced.cpp
index 1033351..8aae0da 100644
--- a/example/manual/ogre/HelloWorldOgreAdvanced.cpp
+++ b/example/manual/ogre/HelloWorldOgreAdvanced.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: HelloWorldOgreAdvanced.cpp 5128 2015-01-06 11:46:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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.
*
@@ -46,13 +43,13 @@
#include <iostream>
-#include <visp/vpOpenCVGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpAROgre.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/ar/vpAROgre.h>
#if defined(VISP_HAVE_OGRE)
@@ -110,7 +107,7 @@ int main()
{
try {
#if defined(VISP_HAVE_OGRE)
-#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394_2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
// Image to store gathered data
// Here we acquire a grey level image. The consequence will be that
@@ -127,7 +124,7 @@ int main()
// the image size
grabber.open(I);
grabber.acquire(I);
-#elif defined(VISP_HAVE_DC1394_2)
+#elif defined(VISP_HAVE_DC1394)
// libdc1394-2
vp1394TwoGrabber grabber;
// Open frame grabber
@@ -165,7 +162,7 @@ int main()
// Rendering loop
while(ogre.continueRendering()){
// Acquire a new image
-#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394)
grabber.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
grabber >> frame;
diff --git a/example/manual/simulation/manServo4PointsDisplay.cpp b/example/manual/simulation/manServo4PointsDisplay.cpp
index 9028c79..3813a43 100755
--- a/example/manual/simulation/manServo4PointsDisplay.cpp
+++ b/example/manual/simulation/manServo4PointsDisplay.cpp
@@ -1,213 +1,225 @@
-/****************************************************************************
- *
- * $Id: manServo4PointsDisplay.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulation of a visual servoing with display.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file manServo4PointsDisplay.cpp
- \brief Visual servoing experiment on 4 points with a display.
-*/
-
-/*!
- \example manServo4PointsDisplay.cpp
- Visual servoing experiment on 4 points with a display.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#ifdef VISP_HAVE_GTK
-
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPose.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpIoTools.h>
-
-int main()
-{
- try {
- //////////////////////////////////////////
- // sets the initial camera location
- vpHomogeneousMatrix cMo(0.3,0.2,3,
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
-
- ///////////////////////////////////
- // initialize the robot
- vpRobotCamera robot ;
- robot.setSamplingTime(0.04); // 40ms
- robot.setPosition(cMo) ;
-
- //initialize the camera parameters
- vpCameraParameters cam(800,800,240,180);
-
- //Image definition
- unsigned int height = 360 ;
- unsigned int width = 480 ;
- vpImage<unsigned char> I(height,width);
-
- //Display initialization
- vpDisplayGTK disp;
- disp.init(I,100,100,"Simulation display");
-
- ////////////////////////////////////////
- // Desired visual features initialization
-
- // sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
-
- // sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
-
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
-
- // creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
-
-
- ///////////////////////////////////////
- // Current visual features initialization
-
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo) ;
-
- // creates the associated features
- vpFeaturePoint p[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],point[i]) ;
-
-
- /////////////////////////////////
- // Task defintion
- vpServo task ;
- // we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
-
- // Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
- // Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
- // Set the gain
- task.setLambda(1.0) ;
- // Print the current information about the task
- task.print();
-
-
- ////////////////////////////////////////////////
- // The control loop
- int k = 0;
- while(k++ < 200){
- double t = vpTime::measureTimeMs();
-
- // Display the image background
- vpDisplay::display(I);
-
- // Update the current features
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].project(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
- }
-
- // Display the task features (current and desired)
- vpServoDisplay::display(task,cam,I);
- vpDisplay::flush(I);
-
- // Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // Compute the control law
- vpColVector v = task.computeControlLaw() ;
-
- // Send the computed velocity to the robot and compute the new robot position
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
- robot.getPosition(cMo) ;
-
- // Print the current information about the task
- task.print();
-
- // Wait 40 ms
- vpTime::wait(t,40);
- }
- task.kill();
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-int
-main()
-{ vpTRACE("You should install GTK") ;
-
-}
-#endif
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulation of a visual servoing with display.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file manServo4PointsDisplay.cpp
+ \brief Visual servoing experiment on 4 points with a display.
+*/
+
+/*!
+ \example manServo4PointsDisplay.cpp
+ Visual servoing experiment on 4 points with a display.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ try {
+ //////////////////////////////////////////
+ // sets the initial camera location
+ vpHomogeneousMatrix cMo(0.3,0.2,3,
+ vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
+
+ ///////////////////////////////////
+ // initialize the robot
+ vpSimulatorCamera robot ;
+ robot.setSamplingTime(0.04); // 40ms
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
+
+ //initialize the camera parameters
+ vpCameraParameters cam(800,800,240,180);
+
+ //Image definition
+ unsigned int height = 360 ;
+ unsigned int width = 480 ;
+ vpImage<unsigned char> I(height,width);
+
+ //Display initialization
+#if defined(VISP_HAVE_X11)
+ vpDisplayX disp;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK disp;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI disp;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV disp;
+#endif
+ disp.init(I,100,100,"Simulation display");
+
+ ////////////////////////////////////////
+ // Desired visual features initialization
+
+ // sets the points coordinates in the object frame (in meter)
+ vpPoint point[4] ;
+ point[0].setWorldCoordinates(-0.1,-0.1,0) ;
+ point[1].setWorldCoordinates(0.1,-0.1,0) ;
+ point[2].setWorldCoordinates(0.1,0.1,0) ;
+ point[3].setWorldCoordinates(-0.1,0.1,0) ;
+
+ // sets the desired camera location
+ vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+
+ // computes the 3D point coordinates in the camera frame and its 2D coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].project(cMo_d) ;
+
+ // creates the associated features
+ vpFeaturePoint pd[4] ;
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(pd[i],point[i]) ;
+
+
+ ///////////////////////////////////////
+ // Current visual features initialization
+
+ // computes the 3D point coordinates in the camera frame and its 2D coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].project(cMo) ;
+
+ // creates the associated features
+ vpFeaturePoint p[4] ;
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(p[i],point[i]) ;
+
+
+ /////////////////////////////////
+ // Task defintion
+ vpServo task ;
+ // we want an eye-in-hand control law ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+
+ // Set the position of the camera in the end-effector frame
+ vpHomogeneousMatrix cMe ;
+ vpVelocityTwistMatrix cVe(cMe) ;
+ task.set_cVe(cVe) ;
+ // Set the Jacobian (expressed in the end-effector frame)
+ vpMatrix eJe ;
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // we want to see a point on a point
+ for (int i = 0 ; i < 4 ; i++)
+ task.addFeature(p[i],pd[i]) ;
+ // Set the gain
+ task.setLambda(1.0) ;
+ // Print the current information about the task
+ task.print();
+
+
+ ////////////////////////////////////////////////
+ // The control loop
+ int k = 0;
+ while(k++ < 200){
+ double t = vpTime::measureTimeMs();
+
+ // Display the image background
+ vpDisplay::display(I);
+
+ // Update the current features
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ point[i].project(cMo) ;
+ vpFeatureBuilder::create(p[i],point[i]) ;
+ }
+
+ // Display the task features (current and desired)
+ vpServoDisplay::display(task,cam,I);
+ vpDisplay::flush(I);
+
+ // Update the robot Jacobian
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // Compute the control law
+ vpColVector v = task.computeControlLaw() ;
+
+ // Send the computed velocity to the robot and compute the new robot position
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
+
+ // Print the current information about the task
+ task.print();
+
+ // Wait 40 ms
+ vpTime::wait(t,40);
+ }
+ task.kill();
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int
+main()
+{ vpTRACE("You should install GTK") ;
+
+}
+#endif
diff --git a/example/manual/simulation/manSimu4Dots.cpp b/example/manual/simulation/manSimu4Dots.cpp
index e915971..66ea177 100755
--- a/example/manual/simulation/manSimu4Dots.cpp
+++ b/example/manual/simulation/manSimu4Dots.cpp
@@ -1,295 +1,302 @@
-/****************************************************************************
- *
- * $Id: manSimu4Dots.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulation of a visual servoing with visualization and image generation.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file manSimu4Dots.cpp
- \brief Visual servoing experiment on 4 points with a visualization and image generation
- from the camera and from an external view using vpSimulator.
-*/
-
-/*!
- \example manSimu4Dots.cpp
- Visual servoing experiment on 4 points with a visualization and image generation
- from the camera and from an external view using vpSimulator.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-
-#if (defined(VISP_HAVE_COIN_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
-
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpSimulator.h>
-
-#if defined(VISP_HAVE_X11)
-# include <visp/vpDisplayX.h>
-#elif defined(VISP_HAVE_GDI)
-# include <visp/vpDisplayGDI.h>
-#elif defined(VISP_HAVE_GTK)
-# include <visp/vpDisplayGTK.h>
-#endif
-// You may have strange compiler issues using the simulator based on SoQt
-// and the vpDisplayGTK. In that case prefer to use another display like
-// vpDisplayX under linux or vpDisplayGDI under Windows
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPose.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpDot2.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpIoTools.h>
-
-static
-void *mainLoop (void *_simu)
-{
- // pointer copy of the vpSimulator instance
- vpSimulator *simu = (vpSimulator *)_simu ;
-
- // Simulation initialization
- simu->initMainApplication() ;
-
- ///////////////////////////////////
- // Set the initial camera location
- vpHomogeneousMatrix cMo(0.3,0.2,3,
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(40));
-
- ///////////////////////////////////
- // Initialize the robot
- vpRobotCamera robot ;
- robot.setSamplingTime(0.04); // 40ms
- robot.setPosition(cMo) ;
- // Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
- // Initialize the camera parameters
- vpCameraParameters cam ;
- simu->getCameraParameters(cam);
-
- ////////////////////////////////////////
- // Desired visual features initialization
-
- // sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
-
- // sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
-
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
-
- // creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
-
-
-
- ///////////////////////////////////////
- // Current visual features initialization
- unsigned int height = simu->getInternalHeight();
- unsigned int width = simu->getInternalWidth();
-
- // Create a greyscale image
- vpImage<unsigned char> I(height,width);
-
- //Display initialization
-#if defined(VISP_HAVE_X11)
- vpDisplayX disp;
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI disp;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK disp;
-#endif
- disp.init(I,100,100,"Simulation display");
- // disp(I);
- // Get the current image
- vpTime::wait(500); // wait to be sure the image is generated
- simu->getInternalImage(I);
-
- // Display the current image
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- // Initialize the four dots tracker
- std::cout << "A click in the four dots clockwise. " << std::endl;
- vpDot2 dot[4];
- vpFeaturePoint p[4];
- for (int i = 0 ; i < 4 ; i++)
- {
- dot[i].setGraphics(true);
- // Call for a click
- std::cout << "A click in the dot " << i << std::endl;
- dot[i].initTracking(I);
- // Create the associated feature
- vpFeatureBuilder::create(p[i],cam,dot[i]);
- // flush the display
- vpDisplay::flush(I);
- }
-
-
- /////////////////////////////////
- // Task defintion
- vpServo task ;
- // we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED) ;
-
- // Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
- // Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
- // Set the gain
- task.setLambda(1.0) ;
- // Print the current information about the task
- task.print();
-
- vpTime::wait(500);
-
- ////////////////////////////////////////////////
- // The control loop
- int k = 0;
- while(k++ < 200){
- double t = vpTime::measureTimeMs();
-
- // Get the current internal camera view and display it
- simu->getInternalImage(I);
- vpDisplay::display(I);
-
- // Track the four dots and update the associated visual features
- for (int i = 0 ; i < 4 ; i++)
- {
- dot[i].track(I) ;
- vpFeatureBuilder::create(p[i],cam,dot[i]) ;
- }
-
- // Display the desired and current visual features
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I);
-
- // Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // Compute the control law
- vpColVector v = task.computeControlLaw() ;
-
- // Send the computed velocity to the robot and compute the new robot position
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
- robot.getPosition(cMo) ;
-
- // Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
-
-
- // Wait 40 ms
- vpTime::wait(t,40);
- }
- // Print information about the task
- task.print();
- task.kill();
- simu->closeMainApplication() ;
-
- void *a=NULL ;
- return a ;
-}
-
-
-int
-main()
-{
- try {
- vpSimulator simu ;
-
- // Internal view initialization : view from the robot camera
- simu.initInternalViewer(480, 360) ;
- // External view initialization : view from an external camera
- simu.initExternalViewer(300, 300) ;
-
- // Inernal camera paramters initialization
- vpCameraParameters cam(800,800,240,180) ;
- simu.setInternalCameraParameters(cam) ;
-
- vpTime::wait(500) ;
- // Load the scene
- std::cout << "Load : ./4Points.iv" << std::endl
- << "This file should be in the working directory" << std::endl;
-
- simu.load("./4points.iv") ;
-
- // Run the main loop
- simu.initApplication(&mainLoop) ;
- // Run the simulator
- simu.mainLoop() ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-int
-main()
-{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
-
-}
-#endif
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulation of a visual servoing with visualization and image generation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file manSimu4Dots.cpp
+ \brief Visual servoing experiment on 4 points with a visualization and image generation
+ from the camera and from an external view using vpSimulator.
+*/
+
+/*!
+ \example manSimu4Dots.cpp
+ Visual servoing experiment on 4 points with a visualization and image generation
+ from the camera and from an external view using vpSimulator.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+
+#if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/ar/vpSimulator.h>
+
+#if defined(VISP_HAVE_X11)
+# include <visp3/gui/vpDisplayX.h>
+#elif defined(VISP_HAVE_GDI)
+# include <visp3/gui/vpDisplayGDI.h>
+#elif defined(VISP_HAVE_GTK)
+# include <visp3/gui/vpDisplayGTK.h>
+#endif
+// You may have strange compiler issues using the simulator based on SoQt
+// and the vpDisplayGTK. In that case prefer to use another display like
+// vpDisplayX under linux or vpDisplayGDI under Windows
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpIoTools.h>
+
+static
+void *mainLoop (void *_simu)
+{
+ // pointer copy of the vpSimulator instance
+ vpSimulator *simu = (vpSimulator *)_simu ;
+
+ // Simulation initialization
+ simu->initMainApplication() ;
+
+ ///////////////////////////////////
+ // Set the initial camera location
+ vpHomogeneousMatrix cMo(0.3,0.2,3,
+ vpMath::rad(0),vpMath::rad(0),vpMath::rad(40));
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
+
+ ///////////////////////////////////
+ // Initialize the robot
+ vpSimulatorCamera robot ;
+ robot.setSamplingTime(0.04); // 40ms
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
+ // Send the robot position to the visualizator
+ simu->setCameraPosition(cMo) ;
+ // Initialize the camera parameters
+ vpCameraParameters cam ;
+ simu->getCameraParameters(cam);
+
+ ////////////////////////////////////////
+ // Desired visual features initialization
+
+ // sets the points coordinates in the object frame (in meter)
+ vpPoint point[4] ;
+ point[0].setWorldCoordinates(-0.1,-0.1,0) ;
+ point[1].setWorldCoordinates(0.1,-0.1,0) ;
+ point[2].setWorldCoordinates(0.1,0.1,0) ;
+ point[3].setWorldCoordinates(-0.1,0.1,0) ;
+
+ // sets the desired camera location
+ vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+
+ // computes the 3D point coordinates in the camera frame and its 2D coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].project(cMo_d) ;
+
+ // creates the associated features
+ vpFeaturePoint pd[4] ;
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(pd[i],point[i]) ;
+
+ ///////////////////////////////////////
+ // Current visual features initialization
+ unsigned int height = simu->getInternalHeight();
+ unsigned int width = simu->getInternalWidth();
+
+ // Create a greyscale image
+ vpImage<unsigned char> I(height,width);
+
+ //Display initialization
+#if defined(VISP_HAVE_X11)
+ vpDisplayX disp;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI disp;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK disp;
+#endif
+ disp.init(I,100,100,"Simulation display");
+ // disp(I);
+ // Get the current image
+ vpTime::wait(500); // wait to be sure the image is generated
+ simu->getInternalImage(I);
+
+ // Display the current image
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ // Initialize the four dots tracker
+ std::cout << "A click in the four dots clockwise. " << std::endl;
+ vpDot2 dot[4];
+ vpFeaturePoint p[4];
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ dot[i].setGraphics(true);
+ // Call for a click
+ std::cout << "A click in the dot " << i << std::endl;
+ dot[i].initTracking(I);
+ // Create the associated feature
+ vpFeatureBuilder::create(p[i],cam,dot[i]);
+ // flush the display
+ vpDisplay::flush(I);
+ }
+
+
+ /////////////////////////////////
+ // Task defintion
+ vpServo task ;
+ // we want an eye-in-hand control law ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setInteractionMatrixType(vpServo::DESIRED) ;
+
+ // Set the position of the camera in the end-effector frame
+ vpHomogeneousMatrix cMe ;
+ vpVelocityTwistMatrix cVe(cMe) ;
+ task.set_cVe(cVe) ;
+ // Set the Jacobian (expressed in the end-effector frame)
+ vpMatrix eJe ;
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // we want to see a point on a point
+ for (int i = 0 ; i < 4 ; i++)
+ task.addFeature(p[i],pd[i]) ;
+ // Set the gain
+ task.setLambda(1.0) ;
+ // Print the current information about the task
+ task.print();
+
+ vpTime::wait(500);
+
+ ////////////////////////////////////////////////
+ // The control loop
+ int k = 0;
+ while(k++ < 200){
+ double t = vpTime::measureTimeMs();
+
+ // Get the current internal camera view and display it
+ simu->getInternalImage(I);
+ vpDisplay::display(I);
+
+ // Track the four dots and update the associated visual features
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ dot[i].track(I) ;
+ vpFeatureBuilder::create(p[i],cam,dot[i]) ;
+ }
+
+ // Display the desired and current visual features
+ vpServoDisplay::display(task,cam,I) ;
+ vpDisplay::flush(I);
+
+ // Update the robot Jacobian
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // Compute the control law
+ vpColVector v = task.computeControlLaw() ;
+
+ // Send the computed velocity to the robot and compute the new robot position
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
+
+ // Send the robot position to the visualizator
+ simu->setCameraPosition(cMo) ;
+
+ // Wait 40 ms
+ vpTime::wait(t,40);
+ }
+ // Print information about the task
+ task.print();
+ task.kill();
+ simu->closeMainApplication() ;
+
+ void *a=NULL ;
+ return a ;
+}
+
+
+int
+main()
+{
+ try {
+ vpSimulator simu ;
+
+ // Internal view initialization : view from the robot camera
+ simu.initInternalViewer(480, 360) ;
+ // External view initialization : view from an external camera
+ simu.initExternalViewer(300, 300) ;
+
+ // Inernal camera paramters initialization
+ vpCameraParameters cam(800,800,240,180) ;
+ simu.setInternalCameraParameters(cam) ;
+
+ vpTime::wait(500) ;
+ // Load the scene
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ std::string ipath = vpIoTools::getViSPImagesDataPath();
+ std::string filename = "./4points.iv";
+
+ // Set the default input path
+ if (! ipath.empty())
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
+
+ std::cout << "Load : " << filename << std::endl
+ << "This file should be in the working directory" << std::endl;
+
+ simu.load(filename.c_str());
+
+ // Run the main loop
+ simu.initApplication(&mainLoop) ;
+ // Run the simulator
+ simu.mainLoop() ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int
+main()
+{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
+
+}
+#endif
diff --git a/example/manual/simulation/manSimu4Points.cpp b/example/manual/simulation/manSimu4Points.cpp
index 4335ef8..01842b1 100755
--- a/example/manual/simulation/manSimu4Points.cpp
+++ b/example/manual/simulation/manSimu4Points.cpp
@@ -1,246 +1,252 @@
-/****************************************************************************
- *
- * $Id: manSimu4Points.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulation of a visual servoing with visualization.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file manSimu4Points.cpp
- \brief Visual servoing experiment on 4 points with a visualization
- from the camera and from an external view using vpSimulator.
-*/
-
-/*!
- \example manSimu4Points.cpp
- Visual servoing experiment on 4 points with a visualization
- from the camera and from an external view using vpSimulator.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-
-#if (defined(VISP_HAVE_COIN_AND_GUI))
-
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpTime.h>
-#include <visp/vpSimulator.h>
-
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-static
-void *mainLoop (void *_simu)
-{
- // pointer copy of the vpSimulator instance
- vpSimulator *simu = (vpSimulator *)_simu ;
-
- // Simulation initialization
- simu->initMainApplication() ;
-
-
- /////////////////////////////////////////
- // sets the initial camera location
- vpHomogeneousMatrix cMo(-0.3,-0.2,3,
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
-
- ///////////////////////////////////
- // Initialize the robot
- vpRobotCamera robot ;
- robot.setSamplingTime(0.04); // 40ms
- robot.setPosition(cMo) ;
- // Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
- // Initialize the camera parameters
- vpCameraParameters cam ;
- simu->getCameraParameters(cam);
-
- ////////////////////////////////////////
- // Desired visual features initialization
-
- // sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
-
- // sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
-
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
-
- // creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
-
-
-
- ///////////////////////////////////////
- // Current visual features initialization
-
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo) ;
-
- // creates the associated features
- vpFeaturePoint p[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],point[i]) ;
-
-
- /////////////////////////////////
- // Task defintion
- vpServo task ;
- // we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
-
- // Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
- // Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
- // Set the gain
- task.setLambda(1.0) ;
- // Print the current information about the task
- task.print();
-
- vpTime::wait(500);
- ////////////////////////////////////////////////
- // The control loop
- int k = 0;
- while(k++ < 200){
- double t = vpTime::measureTimeMs();
-
-
- // Update the current features
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].project(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
- }
-
- // Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
- // Compute the control law
- vpColVector v = task.computeControlLaw() ;
-
- // Send the computed velocity to the robot and compute the new robot position
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
- robot.getPosition(cMo) ;
-
- // Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
-
- // Print the current information about the task
- task.print();
-
- // Wait 40 ms
- vpTime::wait(t,40);
- }
- task.kill();
- simu->closeMainApplication() ;
-
-
- void *a=NULL ;
- return a ;
- // return (void *);
-}
-
-
-int
-main()
-{
- try {
- vpSimulator simu ;
-
- // Internal view initialization : view from the robot camera
- simu.initInternalViewer(480, 360) ;
- // External view initialization : view from an external camera
- simu.initExternalViewer(300, 300) ;
-
- // Inernal camera paramters initialization
- vpCameraParameters cam(800,800,240,180) ;
- simu.setInternalCameraParameters(cam) ;
-
- vpTime::wait(1000) ;
- // Load the scene
- std::cout << "Load : ./4Points.iv" << std::endl
- << "This file should be in the working directory" << std::endl;
- simu.load("./4points.iv") ;
-
- // Run the main loop
- simu.initApplication(&mainLoop) ;
- // Run the simulator
- simu.mainLoop() ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-int
-main()
-{
- vpTRACE("You should install Coin3D and/or GTK") ;
-}
-#endif
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulation of a visual servoing with visualization.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file manSimu4Points.cpp
+ \brief Visual servoing experiment on 4 points with a visualization
+ from the camera and from an external view using vpSimulator.
+*/
+
+/*!
+ \example manSimu4Points.cpp
+ Visual servoing experiment on 4 points with a visualization
+ from the camera and from an external view using vpSimulator.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+
+#if (defined(VISP_HAVE_COIN3D_AND_GUI))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/ar/vpSimulator.h>
+
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+
+static
+void *mainLoop (void *_simu)
+{
+ // pointer copy of the vpSimulator instance
+ vpSimulator *simu = (vpSimulator *)_simu ;
+
+ // Simulation initialization
+ simu->initMainApplication() ;
+
+ /////////////////////////////////////////
+ // sets the initial camera location
+ vpHomogeneousMatrix cMo(-0.3,-0.2,3,
+ vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
+
+ ///////////////////////////////////
+ // Initialize the robot
+ vpSimulatorCamera robot ;
+ robot.setSamplingTime(0.04); // 40ms
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc) ;
+ // Send the robot position to the visualizator
+ simu->setCameraPosition(cMo) ;
+ // Initialize the camera parameters
+ vpCameraParameters cam ;
+ simu->getCameraParameters(cam);
+
+ ////////////////////////////////////////
+ // Desired visual features initialization
+
+ // sets the points coordinates in the object frame (in meter)
+ vpPoint point[4] ;
+ point[0].setWorldCoordinates(-0.1,-0.1,0) ;
+ point[1].setWorldCoordinates(0.1,-0.1,0) ;
+ point[2].setWorldCoordinates(0.1,0.1,0) ;
+ point[3].setWorldCoordinates(-0.1,0.1,0) ;
+
+ // sets the desired camera location
+ vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+
+ // computes the 3D point coordinates in the camera frame and its 2D coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].project(cMo_d) ;
+
+ // creates the associated features
+ vpFeaturePoint pd[4] ;
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(pd[i],point[i]) ;
+
+ ///////////////////////////////////////
+ // Current visual features initialization
+
+ // computes the 3D point coordinates in the camera frame and its 2D coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].project(cMo) ;
+
+ // creates the associated features
+ vpFeaturePoint p[4] ;
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(p[i],point[i]) ;
+
+
+ /////////////////////////////////
+ // Task defintion
+ vpServo task ;
+ // we want an eye-in-hand control law ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
+
+ // Set the position of the camera in the end-effector frame
+ vpHomogeneousMatrix cMe ;
+ vpVelocityTwistMatrix cVe(cMe) ;
+ task.set_cVe(cVe) ;
+ // Set the Jacobian (expressed in the end-effector frame)
+ vpMatrix eJe ;
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // we want to see a point on a point
+ for (int i = 0 ; i < 4 ; i++)
+ task.addFeature(p[i],pd[i]) ;
+ // Set the gain
+ task.setLambda(1.0) ;
+ // Print the current information about the task
+ task.print();
+
+ vpTime::wait(500);
+ ////////////////////////////////////////////////
+ // The control loop
+ int k = 0;
+ while(k++ < 200){
+ double t = vpTime::measureTimeMs();
+
+
+ // Update the current features
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ point[i].project(cMo) ;
+ vpFeatureBuilder::create(p[i],point[i]) ;
+ }
+
+ // Update the robot Jacobian
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // Compute the control law
+ vpColVector v = task.computeControlLaw() ;
+
+ // Send the computed velocity to the robot and compute the new robot position
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
+
+ // Send the robot position to the visualizator
+ simu->setCameraPosition(cMo) ;
+
+ // Print the current information about the task
+ task.print();
+
+ // Wait 40 ms
+ vpTime::wait(t,40);
+ }
+ task.kill();
+ simu->closeMainApplication() ;
+
+
+ void *a=NULL ;
+ return a ;
+ // return (void *);
+}
+
+
+int
+main()
+{
+ try {
+ vpSimulator simu ;
+
+ // Internal view initialization : view from the robot camera
+ simu.initInternalViewer(480, 360) ;
+ // External view initialization : view from an external camera
+ simu.initExternalViewer(300, 300) ;
+
+ // Inernal camera paramters initialization
+ vpCameraParameters cam(800,800,240,180) ;
+ simu.setInternalCameraParameters(cam) ;
+
+ vpTime::wait(1000) ;
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ std::string ipath = vpIoTools::getViSPImagesDataPath();
+ std::string filename = "./4points.iv";
+
+ // Set the default input path
+ if (! ipath.empty())
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
+
+ std::cout << "Load : " << filename << std::endl
+ << "This file should be in the working directory" << std::endl;
+
+ simu.load(filename.c_str());
+
+ // Run the main loop
+ simu.initApplication(&mainLoop) ;
+ // Run the simulator
+ simu.mainLoop() ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int
+main()
+{
+ vpTRACE("You should install Coin3D and/or GTK") ;
+}
+#endif
diff --git a/example/math/BSpline.cpp b/example/math/BSpline.cpp
index 00379d2..13e388f 100644
--- a/example/math/BSpline.cpp
+++ b/example/math/BSpline.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: BSpline.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Exemple of a B-Spline curve.
*
@@ -52,21 +48,24 @@
-#include <visp/vpDebug.h>
+#include <visp3/core/vpDebug.h>
-#include <visp/vpBSpline.h>
+#include <visp3/core/vpBSpline.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayX.h> // Should be after #include <visp/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#ifdef VISP_HAVE_MODULE_GUI
+# include <visp3/gui/vpDisplayGTK.h>
+# include <visp3/gui/vpDisplayGDI.h>
+# include <visp3/gui/vpDisplayOpenCV.h>
+# include <visp3/gui/vpDisplayD3D.h>
+# include <visp3/gui/vpDisplayX.h> // Should be after #include <visp3/gui/vpDisplayOpenCV.h>
+#endif
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#include <cstdlib>
#include <stdlib.h>
@@ -171,6 +170,8 @@ main(int argc, const char ** argv)
vpImage<unsigned char> I(540,480);
// We open a window using either X11, GTK or GDI.
+
+#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -189,6 +190,7 @@ main(int argc, const char ** argv)
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
}
+#endif
vpBSpline bSpline;
std::list<double> knots;
diff --git a/example/math/CMakeLists.txt b/example/math/CMakeLists.txt
index d64b3e3..bddbbd9 100644
--- a/example/math/CMakeLists.txt
+++ b/example/math/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,32 +35,24 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-math)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io)
+
+set(example_cpp
exponentialMap.cpp
BSpline.cpp
- Nurbs.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
# Add test
- add_test(${binary} ${binary} -c ${OPTION_TO_DESACTIVE_DISPLAY})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
endforeach()
-
diff --git a/example/math/Nurbs.cpp b/example/math/Nurbs.cpp
deleted file mode 100644
index 1bd00c5..0000000
--- a/example/math/Nurbs.cpp
+++ /dev/null
@@ -1,395 +0,0 @@
-/****************************************************************************
- *
- * $Id: Nurbs.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exemple of a Nurbs curve.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-/*!
- \file Nurbs.cpp
-
- \brief Describe a curve thanks to a Nurbs.
-*/
-
-/*!
- \example Nurbs.cpp
-
- Describe a curve thanks to a BSpline.
-*/
-
-
-
-#include <visp/vpDebug.h>
-
-#include <visp/vpNurbs.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h> // Should be after #include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayX.h>
-
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <cstdlib>
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9)
-
-// List of allowed command line options
-#define GETOPTARGS "cdh"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Describe a curve thanks to a Nurbs.\n\
-\n\
-SYNOPSIS\n\
- %s [-c] [-d] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
- }
-
- // 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<unsigned char> I(540,480);
- vpImage<unsigned char> I2(540,480);
- vpImage<unsigned char> I3(540,480);
-
- // We open a window using either X11, GTK or GDI.
-#if defined VISP_HAVE_X11
- vpDisplayX display[3];
-#elif defined VISP_HAVE_GDI
- vpDisplayGDI display[3];
-#elif defined VISP_HAVE_GTK
- vpDisplayGTK display[3];
-#elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV display[3];
-#endif
-
- if (opt_display) {
- // Display size is automatically defined by the image (I) size
- display[0].init(I, 100, 100,"Points as control points") ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
-
- vpNurbs Nurbs;
- std::list<double> knots;
- knots.push_back(0);
- knots.push_back(0);
- knots.push_back(0);
- knots.push_back(1);
- knots.push_back(2);
- knots.push_back(3);
- knots.push_back(4);
- knots.push_back(4);
- knots.push_back(5);
- knots.push_back(5);
- knots.push_back(5);
-
- std::list<vpImagePoint> controlPoints;
- std::list<double> weights;
- vpImagePoint pt;
- pt.set_ij(50,300);
- controlPoints.push_back(pt);
- weights.push_back(1);
- pt.set_ij(100,130);
- controlPoints.push_back(pt);
- weights.push_back(5);
- pt.set_ij(150,400);
- controlPoints.push_back(pt);
- weights.push_back(0.2);
- pt.set_ij(200,370);
- controlPoints.push_back(pt);
- weights.push_back(10);
- pt.set_ij(250,120);
- controlPoints.push_back(pt);
- weights.push_back(1);
- pt.set_ij(300,250);
- controlPoints.push_back(pt);
- weights.push_back(2);
- pt.set_ij(350,200);
- controlPoints.push_back(pt);
- weights.push_back(3);
- pt.set_ij(400,300);
- controlPoints.push_back(pt);
- weights.push_back(1);
-
- Nurbs.set_p(2);
- Nurbs.set_knots(knots);
- Nurbs.set_controlPoints(controlPoints);
- Nurbs.set_weights(weights);
-
- std::cout << "The parameters are :" <<std::endl;
- std::cout << "p : " << Nurbs.get_p() <<std::endl;
- std::cout << "" <<std::endl;
- std::cout << "The knot vector :" <<std::endl;
- std::list<double> knots_cur;
- Nurbs.get_knots(knots_cur);
- unsigned int i_display=0;
- for(std::list<double>::const_iterator it=knots_cur.begin(); it!=knots_cur.end(); ++it, ++i_display){
- std::cout << i_display << " ---> " << *it << std::endl;
- }
- std::cout << "The control points are :" <<std::endl;
- std::list<vpImagePoint> controlPoints_cur;
- Nurbs.get_controlPoints(controlPoints_cur);
- i_display=0;
- for(std::list<vpImagePoint>::const_iterator it=controlPoints_cur.begin(); it!=controlPoints_cur.end(); ++it, ++i_display){
- std::cout << i_display << " ---> " << *it << std::endl;
- }
- std::cout << "The associated weights are :" <<std::endl;
- std::list<double> weights_cur;
- Nurbs.get_weights(weights_cur);
- i_display=0;
- for(std::list<double>::const_iterator it=weights_cur.begin(); it!=weights_cur.end(); ++it, ++i_display){
- std::cout << i_display << " ---> " << *it << std::endl;
- }
-
- unsigned int i = Nurbs.findSpan(5/2.0);
- std::cout << "The knot interval number for the value u = 5/2 is : " << i <<std::endl;
-
- vpBasisFunction *N = NULL;
- N = Nurbs.computeBasisFuns(5/2.0);
- std::cout << "The nonvanishing basis functions N(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
- std::cout << N[j].value << std::endl;
-
- vpBasisFunction **N2 = NULL;
- N2 = Nurbs.computeDersBasisFuns(5/2.0, 2);
- std::cout << "The first derivatives of the basis functions N'(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
- std::cout << N2[1][j].value << std::endl;
-
- std::cout << "The second derivatives of the basis functions N''(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
- std::cout << N2[2][j].value << std::endl;
-
- if (opt_display && opt_click_allowed)
- {
- double u = 0.0;
- while (u <= 5)
- {
- pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,vpColor::red);
- u+=0.01;
- }
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
- vpDisplay::displayCross(I, *it, 4, vpColor::green);
- }
-
- vpDisplay::flush(I) ;
- vpDisplay::getClick(I);
- }
-
- if (opt_display) {
- try{
- // Display size is automatically defined by the image (I) size
- display[1].init(I2, 100, 100,"Points interpolation") ;
- vpDisplay::display(I2) ;
- vpDisplay::flush(I2) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
- exit(-1);
- }
- }
-
- Nurbs.globalCurveInterp(controlPoints);
-
- if (opt_display && opt_click_allowed)
- {
- double u = 0.0;
- while (u <= 1)
- {
- pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I2,pt,4,vpColor::red);
- u+=0.01;
- }
-
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
- vpDisplay::displayCross(I2, *it, 4, vpColor::green);
- }
- vpDisplay::flush(I2) ;
- vpDisplay::getClick(I2);
- }
-
-
- if (opt_display) {
- try{
- // Display size is automatically defined by the image (I) size
- display[2].init(I3, 100, 100,"Points approximation") ;
- vpDisplay::display(I3) ;
- vpDisplay::flush(I3) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
- exit(-1);
- }
- }
-
- Nurbs.globalCurveApprox(controlPoints,5);
-
- if (opt_display && opt_click_allowed)
- {
- double u = 0.0;
- while (u <= 1)
- {
- pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I3,pt,4,vpColor::red);
- u+=0.01;
- }
-
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
- vpDisplay::displayCross(I3, *it, 4, vpColor::green);
- }
-
- vpDisplay::flush(I3) ;
- vpDisplay::getClick(I3);
- }
-
- if (N != NULL) delete[] N;
- if (N2 != NULL)
- {
- for (int j = 0; j <= 2; j++)
- delete[] N2[j];
- delete[] N2;
- }
-
- return 0;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-int main()
-{
- std::cout << "This example requires a video device. "
- << std::endl
- << "You should install X11, GTK, OpenCV, GDI or Direct3D"
- << std::endl
- << "to be able to execute this example."
- << std::endl;
- return 0;
-}
-#endif
diff --git a/example/math/exponentialMap.cpp b/example/math/exponentialMap.cpp
index 2e627bc..b7e9289 100644
--- a/example/math/exponentialMap.cpp
+++ b/example/math/exponentialMap.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: exponentialMap.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Test some vpColVector functionalities.
*
@@ -46,13 +42,13 @@
*/
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationVector.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpExponentialMap.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpExponentialMap.h>
int
diff --git a/example/moments/image/CMakeLists.txt b/example/moments/image/CMakeLists.txt
index 046dc9c..df5053c 100644
--- a/example/moments/image/CMakeLists.txt
+++ b/example/moments/image/CMakeLists.txt
@@ -1,9 +1,7 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
@@ -12,20 +10,19 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+# See http://visp.inria.fr for more information.
#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
+# Inria at visp at 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.
@@ -38,26 +35,19 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- servoMomentImage.cpp
-)
+project(example-moments-image)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+cmake_minimum_required(VERSION 2.6)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_gui)
+
+set(example_cpp
+ servoMomentImage.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/moments/image/servoMomentImage.cpp b/example/moments/image/servoMomentImage.cpp
index e6577d6..0208b4f 100644
--- a/example/moments/image/servoMomentImage.cpp
+++ b/example/moments/image/servoMomentImage.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoMomentImage.cpp 4670 2014-02-17 08:59:05Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example of visual servoing with moments using an image as object
* container
@@ -47,31 +43,31 @@
#define PRINT_CONDITION_NUMBER
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpImageSimulator.h>
-#include <visp/vpPlane.h>
-#include <visp/vpPoseVector.h>
-#include <visp/vpPlot.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/gui/vpPlot.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
@@ -146,7 +142,7 @@ vpDisplayGTK displayInt;
vpHomogeneousMatrix cMo;
vpHomogeneousMatrix cdMo;
-vpRobotCamera robot;//robot used in this simulation
+vpSimulatorCamera robot;//robot used in this simulation
vpImage<vpRGBa> Iint(480,640, 0);//internal image used for interface display
vpServo task; //servoing task
vpCameraParameters cam;//robot camera parameters
@@ -303,7 +299,11 @@ void execute(unsigned int nbIter){
vpDisplay::flush(Iint);
unsigned int iter=0;
double t=0;
- robot.setPosition(cMo);
+
+ vpHomogeneousMatrix wMo; // Set to identity
+ vpHomogeneousMatrix wMc; // Camera position in the world frame
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc);
float sampling_time = 0.010f; // Sampling period in seconds
robot.setSamplingTime(sampling_time);
@@ -317,7 +317,8 @@ void execute(unsigned int nbIter){
vpColVector v ;
t = vpTime::measureTimeMs();
//get the cMo
- robot.getPosition(cMo);
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
currentpose.buildFrom(cMo); // For plot
//setup the plane in A,B,C style
vpPlane pl;
@@ -349,19 +350,19 @@ void execute(unsigned int nbIter){
//robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
err_features = task.error;
- std::cout<<" || s - s* || = "<<task.error.sumSquare()<<std::endl;
+ std::cout<<" || s - s* || = "<<task.error.sumSquare()<<std::endl;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
ViSP_plot.plot(0,iter, v);
- ViSP_plot.plot(1,iter,currentpose); // Plot the velocities
- ViSP_plot.plot(2, iter,err_features); //cMo as translations and theta_u
+ ViSP_plot.plot(1,iter,currentpose); // Plot the velocities
+ ViSP_plot.plot(2, iter,err_features); //cMo as translations and theta_u
- _error = ( task.getError() ).sumSquare();
+ _error = ( task.getError() ).sumSquare();
- #if defined(PRINT_CONDITION_NUMBER)
- /*
+#if defined(PRINT_CONDITION_NUMBER)
+ /*
* Condition number of interaction matrix
*/
vpMatrix Linteraction = task.L;
diff --git a/example/moments/points/CMakeLists.txt b/example/moments/points/CMakeLists.txt
index e80fdb2..2fa569e 100644
--- a/example/moments/points/CMakeLists.txt
+++ b/example/moments/points/CMakeLists.txt
@@ -1,9 +1,7 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
@@ -12,20 +10,19 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+# See http://visp.inria.fr for more information.
#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
+# Inria at visp at 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.
@@ -38,26 +35,19 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- servoMomentPoints.cpp
-)
+project(example-moments-points)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+cmake_minimum_required(VERSION 2.6)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_gui)
+
+set(example_cpp
+ servoMomentPoints.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/moments/points/servoMomentPoints.cpp b/example/moments/points/servoMomentPoints.cpp
index b5e2bed..a7fe1d0 100644
--- a/example/moments/points/servoMomentPoints.cpp
+++ b/example/moments/points/servoMomentPoints.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoMomentPoints.cpp 4673 2014-02-17 09:06:49Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example of visual servoing with moments using discrete points as object
* container
@@ -45,27 +41,27 @@
Example of moment-based visual servoing with Images
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpPlane.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpPlane.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
@@ -165,8 +161,7 @@ void initScene(){
int nbpoints = 8;
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
+ vpPoint p(x[i]/20,y[i]/20,0.0);
p.track(cMo) ;
src_pts.push_back(p);
}
@@ -174,8 +169,7 @@ void initScene(){
src.setType(vpMomentObject::DISCRETE);
src.fromVector(src_pts);
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
+ vpPoint p(x[i]/20,y[i]/20,0.0);
p.track(cdMo) ;
dst_pts.push_back(p);
}
@@ -239,8 +233,7 @@ void refreshScene(vpMomentObject &obj){
std::vector<vpPoint> cur_pts;
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
+ vpPoint p(x[i]/20,y[i]/20,0.0);
p.track(cMo) ;
cur_pts.push_back(p);
}
diff --git a/example/moments/polygon/CMakeLists.txt b/example/moments/polygon/CMakeLists.txt
index 6b54b33..e2303a6 100644
--- a/example/moments/polygon/CMakeLists.txt
+++ b/example/moments/polygon/CMakeLists.txt
@@ -1,9 +1,7 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
@@ -12,20 +10,19 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+# See http://visp.inria.fr for more information.
#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
+# Inria at visp at 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.
@@ -38,26 +35,19 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- servoMomentPolygon.cpp
-)
+project(example-moments-polygon)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+cmake_minimum_required(VERSION 2.6)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_gui)
+
+set(example_cpp
+ servoMomentPolygon.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/moments/polygon/servoMomentPolygon.cpp b/example/moments/polygon/servoMomentPolygon.cpp
index 87cdede..78e3671 100644
--- a/example/moments/polygon/servoMomentPolygon.cpp
+++ b/example/moments/polygon/servoMomentPolygon.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoMomentPolygon.cpp 4670 2014-02-17 08:59:05Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example of visual servoing with moments using a polygon as object container
*
@@ -44,27 +40,27 @@
Example of moment-based visual servoing with Images
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpPlane.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpPlane.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
@@ -164,8 +160,7 @@ void initScene(){
int nbpoints = 4;
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
+ vpPoint p(x[i],y[i],0.0);
p.track(cMo) ;
src_pts.push_back(p);
}
@@ -173,8 +168,7 @@ void initScene(){
src.setType(vpMomentObject::DENSE_POLYGON);
src.fromVector(src_pts);
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
+ vpPoint p(x[i],y[i],0.0);
p.track(cdMo) ;
dst_pts.push_back(p);
}
@@ -190,8 +184,7 @@ void refreshScene(vpMomentObject &obj){
vector<vpPoint> cur_pts;
for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
+ vpPoint p(x[i],y[i],0.0);
p.track(cMo) ;
cur_pts.push_back(p);
}
diff --git a/example/ogre-simulator/AROgre.cpp b/example/ogre-simulator/AROgre.cpp
index 71995b4..230fd82 100644
--- a/example/ogre-simulator/AROgre.cpp
+++ b/example/ogre-simulator/AROgre.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: AROgre.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Implementation of a simple augmented reality application using the vpAROgre
* class.
@@ -47,7 +43,7 @@
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
//#if defined(VISP_HAVE_OGRE) && (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) || (defined(VISP_HAVE_X11) && ! defined(APPLE)))
@@ -59,22 +55,22 @@
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
// used on APPLE platforms
-# include <visp/vpDisplayX.h>
+# include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpAROgre.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/ar/vpAROgre.h>
// List of allowed command line options
#define GETOPTARGS "ci:p:h"
@@ -702,6 +698,10 @@ int main(int argc, const char **argv)
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ return 1;
+ }
catch(...) {
std::cout << "Catch an exception " << std::endl;
return 1;
diff --git a/example/ogre-simulator/AROgreBasic.cpp b/example/ogre-simulator/AROgreBasic.cpp
index 3236ecf..37bedea 100644
--- a/example/ogre-simulator/AROgreBasic.cpp
+++ b/example/ogre-simulator/AROgreBasic.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: AROgreBasic.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Implementation of a simple augmented reality application using the vpAROgre
* class.
@@ -47,7 +43,7 @@
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#include <iostream>
//#if defined(VISP_HAVE_OGRE) && (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) || (defined(VISP_HAVE_X11) && ! defined(APPLE)))
@@ -59,22 +55,22 @@
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
// used on APPLE platforms
-# include <visp/vpDisplayX.h>
+# include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpAROgre.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/ar/vpAROgre.h>
// List of allowed command line options
#define GETOPTARGS "ci:p:h"
@@ -584,6 +580,10 @@ int main(int argc, const char **argv)
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ return 1;
+ }
catch(...) {
std::cout << "Catch an exception " << std::endl;
return 1;
diff --git a/example/ogre-simulator/CMakeLists.txt b/example/ogre-simulator/CMakeLists.txt
index 9b19f17..466062b 100644
--- a/example/ogre-simulator/CMakeLists.txt
+++ b/example/ogre-simulator/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,20 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- AROgre.cpp
- AROgreBasic.cpp
-)
+project(example-ogre-simulator)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+cmake_minimum_required(VERSION 2.6)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+find_package(VISP REQUIRED visp_core visp_vision visp_ar visp_blob visp_io visp_gui)
+
+set(example_cpp
+ AROgre.cpp
+ AROgreBasic.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/parse-argv/CMakeLists.txt b/example/parse-argv/CMakeLists.txt
index ba94d4e..0ed1db5 100644
--- a/example/parse-argv/CMakeLists.txt
+++ b/example/parse-argv/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,30 +35,24 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-parse-argv)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io)
+
+set(example_cpp
parse-argv1.cpp
parse-argv2.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
# Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target})
endforeach()
diff --git a/example/parse-argv/parse-argv1.cpp b/example/parse-argv/parse-argv1.cpp
index 2634975..87c52f4 100644
--- a/example/parse-argv/parse-argv1.cpp
+++ b/example/parse-argv/parse-argv1.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: parse-argv1.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of command line parsing.
*
@@ -54,8 +50,8 @@
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
diff --git a/example/parse-argv/parse-argv2.cpp b/example/parse-argv/parse-argv2.cpp
index 420174d..449ccc7 100644
--- a/example/parse-argv/parse-argv2.cpp
+++ b/example/parse-argv/parse-argv2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: parse-argv2.cpp 5311 2015-02-11 17:42:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of command line parsing.
*
@@ -54,8 +50,8 @@
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpParseArgv.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
diff --git a/example/pose-estimation/CMakeLists.txt b/example/pose-estimation/CMakeLists.txt
index c6700ea..85949d1 100644
--- a/example/pose-estimation/CMakeLists.txt
+++ b/example/pose-estimation/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,34 +35,23 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-SET (SOURCE
- poseVirtualVS.cpp
-)
+project(example-pose-estimation)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+cmake_minimum_required(VERSION 2.6)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+find_package(VISP REQUIRED visp_core visp_blob visp_vision visp_io visp_gui)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
- endif()
-endforeach()
+set(example_cpp
+ poseVirtualVS.cpp
+)
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
-add_test(poseVirtualVS poseVirtualVS -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+endforeach()
diff --git a/example/pose-estimation/poseVirtualVS.cpp b/example/pose-estimation/poseVirtualVS.cpp
index 3bf6d19..e37fba1 100644
--- a/example/pose-estimation/poseVirtualVS.cpp
+++ b/example/pose-estimation/poseVirtualVS.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: poseVirtualVS.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Pose computation on an object made of dots.
* reading of PGM image
@@ -63,28 +59,28 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp/vpPose.h>
-#include <visp/vpDot.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
diff --git a/example/robot-simulator/afma6/CMakeLists.txt b/example/robot-simulator/afma6/CMakeLists.txt
index 43b97b4..318b76b 100644
--- a/example/robot-simulator/afma6/CMakeLists.txt
+++ b/example/robot-simulator/afma6/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 2457 2010-01-07 10:41:18Z nmelchio $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,29 +35,23 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- servoSimuAfma6FourPoints2DCamVelocity.cpp
-)
+project(example-robot-simulator-afma6)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_io visp_gui)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ servoSimuAfma6FourPoints2DCamVelocity.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
-endforeach()
-# Add test
-add_test(servoSimuAfma6FourPoints2DCamVelocity servoSimuAfma6FourPoints2DCamVelocity -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+endforeach()
diff --git a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
index f4544d1..505f197 100644
--- a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuFourPoints2DPolarCamVelocityDisplay.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing using 4 points with cartesian
* coordinates as visual feature.
@@ -57,8 +53,8 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined(_WIN32) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
@@ -69,21 +65,21 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorAfma6.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/CMakeLists.txt b/example/robot-simulator/camera/CMakeLists.txt
index b05b61b..d689596 100644
--- a/example/robot-simulator/camera/CMakeLists.txt
+++ b/example/robot-simulator/camera/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 2457 2010-01-07 10:41:18Z nmelchio $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-robot-simulator-camera)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_io visp_gui)
+
+set(example_cpp
servoSimu3D_cdMc_CamVelocity.cpp
servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
servoSimu3D_cMcd_CamVelocity.cpp
@@ -69,18 +67,10 @@ set(SOURCE
servoSimuThetaUCamVelocity.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
index 2ee501a..7f01c4e 100644
--- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimu3D_cMcd_CamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 3D visual servoing.
*
@@ -61,14 +57,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
index 8b50449..7af42ad 100644
--- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 3D visual servoing.
*
@@ -85,14 +81,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpMath.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -269,7 +265,7 @@ main(int argc, const char ** argv)
// Create the identity matrix
vpMatrix I(3,3);
- I.setIdentity();
+ I.eye();
// Compute the camera translational velocity
vpColVector v(3);
diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
index 0d2cd25..69b9731 100644
--- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimu3D_cdMc_CamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 3D visual servoing.
*
@@ -62,14 +58,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
index 0348c75..b736b95 100644
--- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 3D visual servoing.
*
@@ -80,13 +76,13 @@
#include <stdio.h>
#include <string>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpTranslationVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpTranslationVector.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
index 4b4b9c0..191a73c 100644
--- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuCircle2DCamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a circle.
*
@@ -61,14 +56,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCircle.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
index e3ca194..4000cb0 100644
--- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuCircle2DCamVelocityDisplay.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a circle.
*
@@ -51,30 +46,29 @@
- display the camera view.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpCircle.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp
index 091b86c..7b857d8 100644
--- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuCylinder2DCamVelocityDisplay.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a cylinder.
*
@@ -50,30 +46,29 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp
index efc02a5..d5efbaf 100644
--- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp
+++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a cylinder.
*
@@ -54,29 +50,29 @@
*/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp
index 7d750d9..9c6fe44 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuFourPoints2DCamVelocity.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing using 4 points as visual feature.
*
@@ -57,14 +53,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpConfig.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
index e3f31a0..d7a9507 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuFourPoints2DCamVelocityDisplay.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing using 4 points as visual feature.
*
@@ -56,28 +52,28 @@
*/
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
index 8e9d1b6..8c40e0c 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuFourPoints2DPolarCamVelocityDisplay.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing using 4 points with polar
* coordinates as visual feature.
@@ -57,32 +53,32 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePointPolar.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
@@ -335,7 +331,6 @@ main(int argc, const char ** argv)
point[2].setWorldCoordinates(0.25,0.25,0) ;
point[3].setWorldCoordinates(-0.25,0.25,0) ;
-
for (i = 0 ; i < 4 ; i++)
externalview.insert(point[i]) ;
diff --git a/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp
index 54c5351..672d7a2 100644
--- a/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuLine2DCamVelocityDisplay.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a line.
*
@@ -50,29 +45,29 @@
- display the camera view.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpLine.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp
index d1066f2..ee5ea2a 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuPoint2DCamVelocity1.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a point.
*
@@ -51,13 +47,13 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -143,35 +139,34 @@ main(int argc, const char ** argv)
vpSimulatorCamera robot ;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the point coordinates in the world frame
- vpPoint point ;
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its 2D coordinates
- point.track(cMo) ;
+ point.track(cMo);
// sets the current position of the visual feature
vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p,point); //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ; // buildFrom(x,y,Z) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0,0,1); // buildFrom(x,y,Z) ;
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a point on a point
std::cout << std::endl ;
diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
index 1524bb6..2646dda 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuPoint2DCamVelocity2.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a point.
*
@@ -59,13 +55,13 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -161,29 +157,28 @@ main(int argc, const char ** argv)
std::cout << std::endl ;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the point coordinates in the world frame
- vpPoint point ;
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its 2D coordinates
- point.track(cMo) ;
+ point.track(cMo);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, point); //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0,0,1);
// define the task
// - we want an eye-in-hand control law
diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
index 5806ace..66006ce 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuPoint2DCamVelocity3.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a point.
*
@@ -58,13 +54,13 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -157,32 +153,31 @@ main(int argc, const char ** argv)
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo a point " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ std::cout << std::endl;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the point coordinates in the world frame
- vpPoint point ;
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its 2D coordinates
- point.track(cMo) ;
+ point.track(cMo);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p,point); //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ; // buildFrom(x,y,Z) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0,0,1); // buildFrom(x,y,Z) ;
// define the task
// - we want an eye-in-hand control law
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
index 2788691..e75c628 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuPoint2DhalfCamVelocity1.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2 1/2 D visual servoing.
*
@@ -55,16 +51,16 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -172,25 +168,23 @@ main(int argc, const char ** argv)
// sets the desired camera location
vpPoseVector cd_r_o(0,0,1,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ vpHomogeneousMatrix cdMo(cd_r_o);
// sets the point coordinates in the world frame
- vpPoint point ;
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its 2D coordinates
- point.track(cMo) ;
+ point.track(cMo);
- vpPoint pointd ;
- pointd.setWorldCoordinates(0,0,0) ;
- pointd.track(cdMo) ;
+ vpPoint pointd(0, 0, 0);
+ pointd.track(cdMo);
//------------------------------------------------------------------
// 1st feature (x,y)
// want to it at (0,0)
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ;
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p,point);
- vpFeaturePoint pd ;
- vpFeatureBuilder::create(pd,pointd) ;
+ vpFeaturePoint pd;
+ vpFeatureBuilder::create(pd,pointd);
//------------------------------------------------------------------
// 2nd feature (Z)
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
index 5e98f98..22dd534 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuPoint2DhalfCamVelocity2.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2 1/2 D visual servoing using theta U visual features.
*
@@ -55,16 +50,16 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -213,19 +208,16 @@ main(int argc, const char ** argv)
//------------------------------------------------------------------
// sets the point coordinates in the world frame
- vpPoint point ;
- // defined point coordinates in the scene frame : oP
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its
// 2D coordinates cP and then p
// computes the point coordinates in the camera frame and its 2D coordinates" ) ;
- point.track(cMo) ;
+ point.track(cMo);
// We also defined (again by forward projection) the desired position
// of this point according to the desired camera position
- vpPoint pointd ;
- pointd.setWorldCoordinates(0,0,0) ;
- pointd.track(cdMo) ;
+ vpPoint pointd(0, 0, 0);
+ pointd.track(cdMo);
// Nevertheless, a vpPoint is not a feature, this is just a "tracker"
// from which the feature are built
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
index d42e493..aade620 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuPoint2DhalfCamVelocity3.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2 1/2 D visual servoing using theta U visual features.
*
@@ -55,16 +50,16 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -212,9 +207,7 @@ main(int argc, const char ** argv)
//------------------------------------------------------------------
// sets the point coordinates in the world frame
- vpPoint P ;
- // defined point coordinates in the scene frame : oP
- P.setWorldCoordinates(0,0,0) ;
+ vpPoint P(0, 0, 0) ;
// computes the P coordinates in the camera frame and its
// 2D coordinates cP and then p
// computes the point coordinates in the camera frame and its 2D coordinates
@@ -222,8 +215,7 @@ main(int argc, const char ** argv)
// We also defined (again by forward projection) the desired position
// of this point according to the desired camera position
- vpPoint Pd ;
- Pd.setWorldCoordinates(0,0,0) ;
+ vpPoint Pd(0, 0, 0) ;
Pd.track(cdMo) ;
// Nevertheless, a vpPoint is not a feature, this is just a "tracker"
diff --git a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
index b2361f4..69b1955 100644
--- a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuPoint3DCamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 3D visual servoing on a 3D point.
*
@@ -55,13 +51,13 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeaturePoint3D.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
@@ -166,8 +162,7 @@ main(int argc, const char ** argv)
wMo = wMc * cMo;
// sets the point coordinates in the world frame
- vpPoint point ;
- point.setWorldCoordinates(0,0,0) ;
+ vpPoint point(0,0,0) ;
// computes the point coordinates in the camera frame
point.track(cMo) ;
diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
index 9c39af7..da152cf 100644
--- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuSphere2DCamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a sphere.
*
@@ -54,14 +49,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSphere.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpSphere.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
index 340f2dc..bf1d4f2 100644
--- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
+++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuSphere2DCamVelocitySecondaryTask.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a sphere.
*
@@ -54,15 +50,14 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpServo.h>
-#include <visp/vpSphere.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpSphere.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
index 5d9f608..f6f9fc2 100644
--- a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
@@ -1,11 +1,8 @@
-
/****************************************************************************
*
- * $Id: servoSimuSquareLine2DCamVelocityDisplay.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing on a line.
*
@@ -50,30 +45,29 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpLine.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp b/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp
index b28ab52..84e253a 100644
--- a/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuThetaUCamVelocity.cpp 2457 2010-01-07 10:41:18Z nmelchio $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a visual servoing using theta U visual features.
* tests the control law
@@ -56,13 +52,13 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
#define GETOPTARGS "h"
diff --git a/example/robot-simulator/viper850/CMakeLists.txt b/example/robot-simulator/viper850/CMakeLists.txt
index f15684b..87c6771 100644
--- a/example/robot-simulator/viper850/CMakeLists.txt
+++ b/example/robot-simulator/viper850/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 2457 2010-01-07 10:41:18Z nmelchio $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,29 +35,23 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- servoSimuViper850FourPoints2DCamVelocity.cpp
-)
+project(example-robot-simulator-viper850)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_vs visp_robot visp_io visp_gui)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ servoSimuViper850FourPoints2DCamVelocity.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
-endforeach()
-# Add test
-add_test(servoSimuViper850FourPoints2DCamVelocity servoSimuViper850FourPoints2DCamVelocity -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+endforeach()
diff --git a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
index f00a476..eec9d55 100644
--- a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoSimuFourPoints2DPolarCamVelocityDisplay.cpp 2503 2010-02-16 18:55:01Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Simulation of a 2D visual servoing using 4 points with polar
* coordinates as visual feature.
@@ -57,8 +53,8 @@
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#if (defined(_WIN32) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI))
@@ -69,21 +65,21 @@
#include <stdlib.h>
#include <stdio.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorViper850.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorViper850.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
diff --git a/example/servo-afma4/CMakeLists.txt b/example/servo-afma4/CMakeLists.txt
index ef53ba4..3535431 100644
--- a/example/servo-afma4/CMakeLists.txt
+++ b/example/servo-afma4/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,29 +35,22 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-afma4)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+
+set(example_cpp
moveAfma4.cpp
servoAfma4Point2DArtVelocity.cpp
servoAfma4Point2DCamVelocity.cpp
servoAfma4Point2DCamVelocityKalman.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/servo-afma4/moveAfma4.cpp b/example/servo-afma4/moveAfma4.cpp
index f889ed7..b46b99a 100644
--- a/example/servo-afma4/moveAfma4.cpp
+++ b/example/servo-afma4/moveAfma4.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: moveAfma4.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Test for Afma 4 dof robot.
*
@@ -55,16 +51,16 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#ifdef VISP_HAVE_AFMA4
#include <unistd.h>
#include <stdlib.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRobotAfma4.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpRobotAfma4.h>
// List of allowed command line options
#define GETOPTARGS "mh"
diff --git a/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp b/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp
index 53763b0..ee40a18 100644
--- a/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp
+++ b/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma4Point2DArtVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -65,38 +61,37 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma4.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
int
main()
diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
index 5549e73..01f1272 100644
--- a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
+++ b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma4Point2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -66,34 +62,33 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma4.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
int
main()
diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
index 05a8ec8..6e583ce 100644
--- a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
+++ b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma4Point2DCamVelocityKalman.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -60,33 +56,32 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma4.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpDot2.h>
-#include <visp/vpAdaptiveGain.h>
-#include <visp/vpLinearKalmanFilterInstantiation.h>
+#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/vs/vpAdaptiveGain.h>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
// List of allowed command line options
diff --git a/example/servo-afma6/CMakeLists.txt b/example/servo-afma6/CMakeLists.txt
index 1cc2add..9a391d5 100644
--- a/example/servo-afma6/CMakeLists.txt
+++ b/example/servo-afma6/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-afma6)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+
+set(example_cpp
servoAfma6Ellipse2DCamVelocity.cpp
servoAfma6FourPoints2DArtVelocity.cpp
servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
@@ -60,17 +58,9 @@ set(SOURCE
servoAfma6Segment2DCamVelocity.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
index 9180438..53b40a9 100644
--- a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma62DhalfCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -60,43 +56,42 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpLine.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpPose.h>
-
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vision/vpPose.h>
+
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPoint.h>
-#include <visp/vpHomogeneousMatrix.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
int
diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
index a45fed9..d43d25c 100644
--- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Cylinder2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -56,35 +52,34 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
index 4442911..170b7ad 100644
--- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
+++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -60,35 +56,34 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
int
diff --git a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
index 1accd1a..23aff2d 100644
--- a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Ellipse2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -62,34 +58,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
diff --git a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
index 5e1c2cd..6fa5064 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6FourPoints2DArtVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -63,38 +59,37 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
index debf5dd..fae0200 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -67,37 +63,36 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpDot.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpPose.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
+#include <visp3/core/vpException.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
index d59457f..4441659 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -67,36 +63,35 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpDot.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
+#include <visp3/core/vpException.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
index 3acfdff..07d0191 100644
--- a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Line2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -59,32 +55,31 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpLine.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
index 7f0e31d..90beaf4 100644
--- a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Point2DArtVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -64,38 +60,37 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
index 6ed0b84..8d29c56 100644
--- a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Point2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -65,30 +61,29 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
index 1065231..8a24042 100644
--- a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
+++ b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Points2DCamVelocityEyeToHand.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-to-hand control
@@ -61,36 +57,35 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
#include <list>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
#define SAVE 0
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot.h>
-#include <visp/vpPose.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
#define L 0.006
#define D 0
diff --git a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
index c151bde..f1b55e5 100644
--- a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6Point2DCamVelocity.cpp 3616 2012-03-09 14:31:52Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -51,31 +47,30 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <vector>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureSegment.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
index 663fa0f..6b99783 100644
--- a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6SquareLines2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -60,37 +56,36 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpLine.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/blob/vpDot.h>
int
main()
diff --git a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
index 45f8bd8..679fdab 100644
--- a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoAfma6TwoLines2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -64,35 +60,34 @@
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpLine.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-
-#include <visp/vpRobotAfma6.h>
+#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#include <visp3/robot/vpRobotAfma6.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
int
main()
diff --git a/example/servo-biclops/CMakeLists.txt b/example/servo-biclops/CMakeLists.txt
index 183f15d..b9adb9e 100644
--- a/example/servo-biclops/CMakeLists.txt
+++ b/example/servo-biclops/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,20 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-biclops)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+
+set(example_cpp
moveBiclops.cpp
servoBiclopsPoint2DArtVelocity.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp
index c02ed03..49ccdf3 100644
--- a/example/servo-biclops/moveBiclops.cpp
+++ b/example/servo-biclops/moveBiclops.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: moveBiclops.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tests the control law
* Authors:
@@ -58,15 +54,15 @@
-#include <visp/vpParseArgv.h>
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTime.h>
#include <stdlib.h>
#ifdef VISP_HAVE_BICLOPS
-#include <visp/vpRobotBiclops.h>
+#include <visp3/robot/vpRobotBiclops.h>
// List of allowed command line options
#define GETOPTARGS "c:h"
@@ -114,16 +110,16 @@ Set the program options.
*/
bool getOptions(int argc, const char **argv, std::string& conf)
{
- const char *optarg;
+ const char *optarg_;
int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': conf = optarg; break;
+ case 'c': conf = optarg_; break;
case 'h': usage(argv[0], NULL, conf); return false; break;
default:
- usage(argv[0], optarg, conf); return false; break;
+ usage(argv[0], optarg_, conf); return false; break;
}
}
@@ -131,7 +127,7 @@ bool getOptions(int argc, const char **argv, std::string& conf)
// standalone param or error
usage(argv[0], NULL, conf);
std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg << std::endl << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
index 8450bd3..5f38321 100644
--- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
+++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoBiclopsPoint2DArtVelocity.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -64,41 +60,39 @@
-#include <visp/vpTime.h>
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <signal.h>
#include <stdlib.h>
-#if ( defined (VISP_HAVE_BICLOPS) && (defined (VISP_HAVE_DC1394_2) || defined(VISP_HAVE_DIRECTSHOW)) )
+#if ( defined (VISP_HAVE_BICLOPS) && (defined (VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)) )
#ifdef VISP_HAVE_PTHREAD
# include <pthread.h>
#endif
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDirectShowGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotBiclops.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
+#include <visp3/core/vpException.h>
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
@@ -169,17 +163,17 @@ Set the program options.
*/
bool getOptions(int argc, const char **argv, std::string& conf, std::string &debugdir, std::string& user)
{
- const char *optarg;
+ const char *optarg_;
int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': conf = optarg; break;
- case 'd': debugdir = optarg; break;
+ case 'c': conf = optarg_; break;
+ case 'd': debugdir = optarg_; break;
case 'h': usage(argv[0], NULL, conf, debugdir, user); return false; break;
default:
- usage(argv[0], optarg, conf, debugdir, user); return false; break;
+ usage(argv[0], optarg_, conf, debugdir, user); return false; break;
}
}
@@ -187,7 +181,7 @@ bool getOptions(int argc, const char **argv, std::string& conf, std::string &deb
// standalone param or error
usage(argv[0], NULL, conf, debugdir, user);
std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg << std::endl << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
@@ -276,7 +270,7 @@ main(int argc, const char ** argv)
vpImage<unsigned char> I ;
-#if defined VISP_HAVE_DC1394_2
+#if defined VISP_HAVE_DC1394
vp1394TwoGrabber g;
#elif defined VISP_HAVE_DIRECTSHOW
vpDirectShowGrabber g;
diff --git a/example/servo-pioneer/CMakeLists.txt b/example/servo-pioneer/CMakeLists.txt
index 76781f8..c6880b8 100644
--- a/example/servo-pioneer/CMakeLists.txt
+++ b/example/servo-pioneer/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-pioneer)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+
+set(example_cpp
movePioneer.cpp
servoPioneerPoint2DDepth.cpp
servoPioneerPoint2DDepthWithoutVpServo.cpp
@@ -51,18 +49,9 @@ set(SOURCE
sonarPioneerReader.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
-
diff --git a/example/servo-pioneer/movePioneer.cpp b/example/servo-pioneer/movePioneer.cpp
index 31bb006..bc81e91 100644
--- a/example/servo-pioneer/movePioneer.cpp
+++ b/example/servo-pioneer/movePioneer.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: movePioneer.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example that shows how to control a Pioneer mobile robot in ViSP.
*
@@ -41,9 +37,9 @@
#include <iostream>
-#include <visp/vpConfig.h>
-#include <visp/vpTime.h>
-#include <visp/vpRobotPioneer.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotPioneer.h>
#ifndef VISP_HAVE_PIONEER
int main()
diff --git a/example/servo-pioneer/servoPioneerPanSegment3D.cpp b/example/servo-pioneer/servoPioneerPanSegment3D.cpp
index 8b3296e..dd239e2 100644
--- a/example/servo-pioneer/servoPioneerPanSegment3D.cpp
+++ b/example/servo-pioneer/servoPioneerPanSegment3D.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoPioneerPanSegment3D.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* IBVS on Pioneer P3DX mobile platform
*
@@ -40,25 +36,25 @@
*****************************************************************************/
#include <iostream>
-#include <visp/vpConfig.h>
-
-#include <visp/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
-#include <visp/vpRobotBiclops.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureSegment.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpPioneerPan.h>
-#include <visp/vpPlot.h>
-#include <visp/vpServo.h>
-#include <visp/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/robot/vpPioneerPan.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#define USE_REAL_ROBOT
#define USE_PLOTTER
@@ -84,7 +80,7 @@
#if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
int main(int argc, char **argv)
{
-#if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
try {
vpImage<unsigned char> I; // Create a gray level image container
@@ -175,7 +171,7 @@ int main(int argc, char **argv)
g.open(I);
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
-#elif defined(VISP_HAVE_DC1394_2)
+#elif defined(VISP_HAVE_DC1394)
// Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
diff --git a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
index 73e89af..b1c5e1e 100644
--- a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
+++ b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoPioneerPoint2DDepth.cpp 5128 2015-01-06 11:46:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* IBVS on Pioneer P3DX mobile platform
*
@@ -40,27 +36,27 @@
*****************************************************************************/
#include <iostream>
-#include <visp/vpConfig.h>
-
-#include <visp/vpRobotPioneer.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpOpenCVGrabber.h>
-#include <visp/vpServo.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-#if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/robot/vpRobotPioneer.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
#if defined(VISP_HAVE_PIONEER)
# define TEST_COULD_BE_ACHIEVED
@@ -157,7 +153,7 @@ int main(int argc, char **argv)
g.open(I);
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
-#elif defined(VISP_HAVE_DC1394_2)
+#elif defined(VISP_HAVE_DC1394)
// Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
diff --git a/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp b/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp
index f43906e..99b771f 100644
--- a/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp
+++ b/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: servoPioneerPoint2DDepthWithoutVpServo.cpp 5128 2015-01-06 11:46:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* IBVS on Pioneer P3DX mobile platform
*
@@ -40,25 +36,25 @@
*****************************************************************************/
#include <iostream>
-#include <visp/vpConfig.h>
-#include <visp/vpRobotPioneer.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vpOpenCVGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-#if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotPioneer.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
#if defined(VISP_HAVE_PIONEER)
# define TEST_COULD_BE_ACHIEVED
@@ -155,7 +151,7 @@ int main(int argc, char **argv)
g.open(I);
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
-#elif defined(VISP_HAVE_DC1394_2)
+#elif defined(VISP_HAVE_DC1394)
// Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
@@ -226,8 +222,8 @@ int main(int argc, char **argv)
robot.get_eJe(eJe);
vpMatrix L; // Interaction matrix
- L.stackMatrices(L_x); // constant since build with the desired feature
- L.stackMatrices(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
+ L.stack(L_x); // constant since build with the desired feature
+ L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
vpColVector v; // vz, wx
@@ -259,8 +255,8 @@ int main(int argc, char **argv)
// Update the global interaction matrix
vpMatrix L;
- L.stackMatrices(L_x); // constant since build with the desired feature
- L.stackMatrices(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
+ L.stack(L_x); // constant since build with the desired feature
+ L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
// Update the global error s-s*
vpColVector error;
diff --git a/example/servo-pioneer/sonarPioneerReader.cpp b/example/servo-pioneer/sonarPioneerReader.cpp
index e40043d..144fa4f 100644
--- a/example/servo-pioneer/sonarPioneerReader.cpp
+++ b/example/servo-pioneer/sonarPioneerReader.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: sonarPioneerReader.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* Example that shows how to control a Pioneer mobile robot in ViSP.
*
@@ -41,15 +37,15 @@
#include <iostream>
-#include <visp/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
-#include <visp/vpConfig.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpTime.h>
+#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpTime.h>
#ifndef VISP_HAVE_PIONEER
int main()
@@ -112,11 +108,13 @@ void sonarPrinter(void)
double end_angle = 45;
range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
printf(" front quadrant: %5.0f ", range);
- if (range != sonar.getMaxRange())
+ //if (range != sonar.getMaxRange())
+ if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
- if (isInitialized && range != sonar.getMaxRange())
+ //if (isInitialized && range != sonar.getMaxRange())
+ if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
{
double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
double y = range * sin(vpMath::rad(angle));
@@ -135,19 +133,22 @@ void sonarPrinter(void)
range = sonar.currentReadingPolar(-135, -45, &angle);
printf(" right quadrant: %5.0f ", range);
- if (range != sonar.getMaxRange())
+ //if (range != sonar.getMaxRange())
+ if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(45, 135, &angle);
printf(" left quadrant: %5.0f ", range);
- if (range != sonar.getMaxRange())
+ //if (range != sonar.getMaxRange())
+ if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(-135, 135, &angle);
printf(" back quadrant: %5.0f ", range);
- if (range != sonar.getMaxRange())
+ //if (range != sonar.getMaxRange())
+ if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
@@ -179,7 +180,8 @@ void sonarPrinter(void)
// reading->getSensorY(), reading->getSensorTh(), reading->getRange());
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
- if (isInitialized && range != sonar.getMaxRange())
+ //if (isInitialized && range != sonar.getMaxRange())
+ if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
{
vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
@@ -238,7 +240,7 @@ int main(int argc, char **argv)
// Create a display to show sensor data
if (isInitialized == false)
{
- I.resize(half_size*2, half_size*2);
+ I.resize((unsigned int)half_size*2, (unsigned int)half_size*2);
I = 255;
#if defined(VISP_HAVE_X11)
diff --git a/example/servo-ptu46/CMakeLists.txt b/example/servo-ptu46/CMakeLists.txt
index 9135d34..854e0cd 100644
--- a/example/servo-ptu46/CMakeLists.txt
+++ b/example/servo-ptu46/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,27 +35,20 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-ptu46)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+
+set(example_cpp
movePtu46.cpp
servoPtu46Point2DArtVelocity.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/servo-ptu46/movePtu46.cpp b/example/servo-ptu46/movePtu46.cpp
index 920aed4..10cfbe4 100644
--- a/example/servo-ptu46/movePtu46.cpp
+++ b/example/servo-ptu46/movePtu46.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: movePtu46.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tests the control law
*
@@ -54,8 +50,8 @@
robot is controlled first in position, then in velocity.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
# include <unistd.h>
#endif
@@ -64,7 +60,7 @@
#ifdef VISP_HAVE_PTU46
-#include <visp/vpRobotPtu46.h>
+#include <visp3/robot/vpRobotPtu46.h>
int
main()
diff --git a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
index 3a2a365..7f38f89 100644
--- a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
+++ b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoPtu46Point2DArtVelocity.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -61,8 +57,8 @@
point.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
# include <unistd.h>
#endif
@@ -70,32 +66,31 @@
-#if (defined(VISP_HAVE_PTU46) & defined (VISP_HAVE_DC1394_2) )
+#if (defined(VISP_HAVE_PTU46) & defined (VISP_HAVE_DC1394) )
#ifdef VISP_HAVE_PTHREAD
# include <pthread.h>
#endif
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp/vpRobotPtu46.h>
+#include <visp3/robot/vpRobotPtu46.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot2.h>
+#include <visp3/blob/vpDot2.h>
#ifdef VISP_HAVE_PTHREAD
@@ -104,6 +99,7 @@ pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
void signalCtrC( int signumber )
{
+ (void)(signumber);
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock( &mutexEndLoop );
#endif
@@ -166,17 +162,15 @@ main()
return(-1) ;
}
-
vpServo task ;
vpDot2 dot ;
try{
vpERROR_TRACE("start dot.initTracking(I) ") ;
- int x,y;
- vpDisplay::getClick( I,y,x );
- dot.set_u( x ) ;
- dot.set_v( y ) ;
+ vpImagePoint germ;
+ vpDisplay::getClick( I, germ );
+ dot.setCog(germ);
vpDEBUG_TRACE(25,"Click!");
//dot.initTracking(I) ;
dot.track(I);
diff --git a/example/servo-viper650/CMakeLists.txt b/example/servo-viper650/CMakeLists.txt
index eefd0ef..0054ec8 100644
--- a/example/servo-viper650/CMakeLists.txt
+++ b/example/servo-viper650/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 3750 2012-06-01 09:39:38Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,29 +35,21 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-viper650)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+
+set(example_cpp
servoViper650Point2DCamVelocity.cpp
servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
-
diff --git a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
index 2f0ad87..00b70c3 100644
--- a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
+++ b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp 3870 2012-09-05 17:03:43Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -52,33 +48,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
+#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
index facd496..22c2f59 100644
--- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp 3870 2012-09-05 17:03:43Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -53,33 +49,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
+#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
index 60cab3e..eb51b7b 100644
--- a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
+++ b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper650Point2DCamVelocity.cpp 3616 2012-03-09 14:31:52Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -53,8 +49,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -62,25 +58,24 @@
#include <fstream>
#include <sstream>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394_2) && defined (VISP_HAVE_X11))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDot2.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
+#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394) && defined (VISP_HAVE_X11))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
int
main()
diff --git a/example/servo-viper850/CMakeLists.txt b/example/servo-viper850/CMakeLists.txt
index e93f230..8c48bb0 100644
--- a/example/servo-viper850/CMakeLists.txt
+++ b/example/servo-viper850/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,33 +35,28 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-viper850)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
+
+set(example_cpp
servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
servoViper850FourPointsKinect.cpp
servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
+ servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
servoViper850Point2DArtVelocity.cpp
servoViper850Point2DCamVelocity.cpp
servoViper850Point2DCamVelocityKalman.cpp
)
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
index 0a894bf..da2d787 100644
--- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -52,33 +48,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
index 0a9e968..638f038 100644
--- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DArtVelocityInteractionDesired.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -52,33 +48,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
int
main()
diff --git a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
index a10613a..cb17f8b 100644
--- a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -53,33 +49,33 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-viper850/servoViper850FourPointsKinect.cpp b/example/servo-viper850/servoViper850FourPointsKinect.cpp
index 4fc9b64..35170ac 100644
--- a/example/servo-viper850/servoViper850FourPointsKinect.cpp
+++ b/example/servo-viper850/servoViper850FourPointsKinect.cpp
@@ -1,10 +1,7 @@
-
/****************************************************************************
*
- * $Id: servoViper850FourPoints2DCamVelocityKinect.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -13,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -52,8 +47,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdio.h>
#include <iostream>
@@ -64,30 +59,29 @@
#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES) )
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpPose.h>
-#include <visp/vpKinect.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/sensor/vpKinect.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot2.h>
+#include <visp3/blob/vpDot2.h>
#define L 0.05 // to deal with a 10cm by 10cm square
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
index ea887eb..8c78c9e 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp 4698 2014-03-26 06:55:37Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -51,8 +47,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -62,27 +58,26 @@
#include <cmath> // std::fabs
#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2) && defined(VISP_HAVE_DISPLAY))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPlot.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/gui/vpPlot.h>
int
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
index da87690..8a71f3a 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp 4698 2014-03-26 06:55:37Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -50,8 +46,8 @@
Implemented from \cite Marchand96f and section II.B in \cite Chaumette01c.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -59,27 +55,26 @@
#include <fstream>
#include <sstream>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2) && defined(VISP_HAVE_DISPLAY))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPlot.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/gui/vpPlot.h>
int
@@ -193,7 +188,6 @@ main()
std::cin >> beta ;
vpDot2 dot ;
-
std::cout << "Click on a dot..." << std::endl;
dot.initTracking(I) ;
@@ -336,18 +330,17 @@ main()
task.kill();
return 0;
}
- catch (...)
+ catch (vpException &e)
{
- vpERROR_TRACE(" Test failed") ;
+ std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return 0;
}
}
-
#else
int
main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an Viper 850 robot or a firewire framegrabber connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
new file mode 100644
index 0000000..e05cf3b
--- /dev/null
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
@@ -0,0 +1,313 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * tests the control law
+ * eye-in-hand control
+ * velocity computed in articular
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ * Giovanni Claudio
+ *
+ *****************************************************************************/
+
+/*!
+ \example servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
+
+ Joint limits avoidance using a secondary task for joint limit avoidance \cite Marey:2010b using the
+ new large projection operator (see equation(24) in the paper \cite Marey:2010).
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2) && defined(VISP_HAVE_DISPLAY))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/gui/vpPlot.h>
+
+
+int
+main()
+{
+ try {
+ vpRobotViper850 robot ;
+
+ vpServo task ;
+
+ vpImage<unsigned char> I ;
+
+ bool reset = false;
+ vp1394TwoGrabber g(reset);
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
+ g.open(I) ;
+
+ g.acquire(I) ;
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX display(I,800,100,"Current image") ;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV display(I,800,100,"Current image") ;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK display(I,800,100,"Current image") ;
+#endif
+
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+
+ vpColVector jointMin(6), jointMax(6) ;
+ jointMin = robot.getJointMin();
+ jointMax = robot.getJointMax();
+
+ vpColVector Qmiddle(6);
+ vpColVector data(12) ;
+
+ Qmiddle = (jointMin + jointMax) /2.;
+ // double rho1 = 0.1 ;
+
+ double rho = 0.1;
+ double rho1 = 0.3;
+
+ vpColVector q(6) ;
+
+ // Create a window with two graphics
+ // - first graphic to plot q(t), Qmin, Qmax, Ql0min, Ql1min, Ql0max and Ql1max
+ vpPlot plot(2);
+
+ // The first graphic contains 12 data to plot: q(t), Low Limits, Upper Limits, ql0min, ql1min, ql0max and ql1max
+ plot.initGraph(0, 12);
+ // The second graphic contains the values of the secondaty task velocities
+ plot.initGraph(1, 6);
+
+ // For the first graphic :
+ // - along the x axis the expected values are between 0 and 200
+ // - along the y axis the expected values are between -1.2 and 1.2
+ plot.initRange(0, 0., 200., -1.2, 1.2);
+ plot.setTitle(0, "Joint behavior");
+
+ // For the second graphic :
+ plot.setTitle(1, "Q secondary task");
+
+ // For the first and second graphic, set the curves legend
+ char legend[10];
+ for (unsigned int i=0; i < 6; i++) {
+ sprintf(legend, "q%d", i+1);
+ plot.setLegend(0, i, legend);
+ plot.setLegend(1, i, legend);
+ }
+ plot.setLegend(0, 6, "Low Limit");
+ plot.setLegend(0, 7, "Upper Limit");
+ plot.setLegend(0, 8, "ql0 min");
+ plot.setLegend(0, 9, "ql0 max");
+ plot.setLegend(0, 10, "ql1 min");
+ plot.setLegend(0, 11, "ql1 max");
+
+ // Set the curves color
+ plot.setColor(0, 0, vpColor::red);
+ plot.setColor(0, 1, vpColor::green);
+ plot.setColor(0, 2, vpColor::blue);
+ plot.setColor(0, 3, vpColor::orange);
+ plot.setColor(0, 4, vpColor(0, 128, 0));
+ plot.setColor(0, 5, vpColor::cyan);
+ for (unsigned int i= 6; i < 12; i++)
+ plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
+
+ vpColVector sec_task(6) ;
+
+ vpDot2 dot ;
+
+ std::cout << "Click on a dot..." << std::endl;
+ dot.initTracking(I) ;
+ vpImagePoint cog = dot.getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::flush(I);
+
+ vpCameraParameters cam ;
+ // Update camera parameters
+ robot.getCameraParameters (cam, I);
+
+ // sets the current position of the visual feature
+ vpFeaturePoint p ;
+ vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
+
+ p.set_Z(1) ;
+ // sets the desired position of the visual feature
+ vpFeaturePoint pd ;
+ pd.buildFrom(0,0,1) ;
+
+ // Define the task
+ // - we want an eye-in-hand control law
+ // - articular velocity are computed
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+
+ vpVelocityTwistMatrix cVe ;
+ robot.get_cVe(cVe) ;
+ std::cout << cVe <<std::endl ;
+ task.set_cVe(cVe) ;
+
+ // - Set the Jacobian (expressed in the end-effector frame)") ;
+ vpMatrix eJe ;
+ robot.get_eJe(eJe) ;
+ task.set_eJe(eJe) ;
+
+ // - we want to see a point on a point..") ;
+ std::cout << std::endl ;
+ task.addFeature(p,pd) ;
+
+ // - set the gain
+ task.setLambda(0.8) ;
+
+ // Display task information " ) ;
+ task.print() ;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+
+ int iter = 0;
+ std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
+ for ( ; ; ) {
+ iter ++;
+ // Acquire a new image from the camera
+ g.acquire(I) ;
+
+ // Display this image
+ vpDisplay::display(I) ;
+
+ // Achieve the tracking of the dot in the image
+ dot.track(I) ;
+ cog = dot.getCog();
+
+ // Display a green cross at the center of gravity position in the image
+ vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+
+ // Get the measured joint positions of the robot
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ // Update the point feature from the dot location
+ vpFeatureBuilder::create(p, cam, dot);
+
+ // Get the jacobian of the robot
+ robot.get_eJe(eJe) ;
+ // Update this jacobian in the task structure. It will be used to compute
+ // the velocity skew (as an articular velocity)
+ // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe) ;
+
+ vpColVector prim_task ;
+ // Compute the visual servoing skew vector
+ prim_task = task.computeControlLaw() ;
+
+ // Compute the secondary task for the joint limit avoidance
+ sec_task = task.secondaryTaskJointLimitAvoidance(q, prim_task, jointMin, jointMax, rho, rho1);
+
+ vpColVector v ;
+ v = prim_task + sec_task;
+
+ // Display the current and desired feature points in the image display
+ vpServoDisplay::display(task, cam, I) ;
+
+ // Apply the computed joint velocities to the robot
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+
+ {
+ // Add the material to plot curves
+
+ // q normalized between (entre -1 et 1)
+ for (unsigned int i=0 ; i < 6 ; i++) {
+ data[i] = (q[i] - Qmiddle[i]) ;
+ data[i] /= (jointMax[i] - jointMin[i]) ;
+ data[i]*=2 ;
+ }
+
+ data[6] = -1.0;
+ data[7] = 1.0;
+
+ unsigned int joint = 2;
+ double tQmin_l0 = jointMin[joint] + rho *(jointMax[joint] - jointMin[joint]);
+ double tQmax_l0 = jointMax[joint] - rho *(jointMax[joint] - jointMin[joint]);
+
+ double tQmin_l1 = tQmin_l0 - rho * rho1 * (jointMax[joint] - jointMin[joint]);
+ double tQmax_l1 = tQmax_l0 + rho * rho1 * (jointMax[joint] - jointMin[joint]);
+
+ data[8] = 2*(tQmin_l0 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
+ data[9] = 2*(tQmax_l0 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
+ data[10] = 2*(tQmin_l1 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
+ data[11] = 2*(tQmax_l1 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
+ plot.plot(0, iter, data); // plot q(t), Low Limits, Upper Limits, ql0min, ql1min, ql0max and ql1max
+ plot.plot(1, iter, sec_task); //plot secondary task velocities
+ }
+
+ vpDisplay::flush(I) ;
+ }
+
+ // Display task information
+ task.print() ;
+ task.kill();
+ return 0;
+ }
+ catch (...)
+ {
+ vpERROR_TRACE(" Test failed") ;
+ return 0;
+ }
+}
+
+
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+}
+#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
index a790b56..bca8d42 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850Point2DArtVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -51,8 +47,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -60,29 +56,28 @@
#include <fstream>
#include <sstream>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpIoTools.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
-#include <visp/vpDot2.h>
+#include <visp3/blob/vpDot2.h>
int
main()
diff --git a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
index 7b72b9c..1a44c1f 100644
--- a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
+++ b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850Point2DCamVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -53,8 +49,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -62,27 +58,26 @@
#include <fstream>
#include <sstream>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDot2.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
int
diff --git a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
index e2926ad..1b03c00 100644
--- a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
+++ b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: servoViper850Point2DCamVelocityKalman.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* tests the control law
* eye-in-hand control
@@ -54,8 +50,8 @@
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h> // Debug trace
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <stdio.h>
@@ -63,29 +59,28 @@
#include <fstream>
#include <sstream>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDot2.h>
-#include <visp/vpAdaptiveGain.h>
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
+#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/vs/vpAdaptiveGain.h>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
int
main()
diff --git a/example/tools/CMakeLists.txt b/example/tools/CMakeLists.txt
index cad9940..eda73a2 100644
--- a/example/tools/CMakeLists.txt
+++ b/example/tools/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-servo-tools)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_robot visp_io visp_gui)
+
+set(example_cpp
keyboard.cpp
parallelPort.cpp
plot2d.cpp
@@ -51,18 +49,10 @@ set(SOURCE
histogram.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
diff --git a/example/tools/histogram.cpp b/example/tools/histogram.cpp
index d6d4fe9..0bec968 100644
--- a/example/tools/histogram.cpp
+++ b/example/tools/histogram.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: histogram.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of Histogram manipulation.
*
@@ -48,12 +44,12 @@
-#include <visp/vpDebug.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHistogram.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpHistogram.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
#include <stdio.h>
#include <stdlib.h>
diff --git a/example/tools/keyboard.cpp b/example/tools/keyboard.cpp
index ef97dd0..418f6d1 100644
--- a/example/tools/keyboard.cpp
+++ b/example/tools/keyboard.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: keyboard.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of keyboard management.
*
@@ -45,15 +41,15 @@
Keyboard example.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
#include <stdio.h>
#include <iostream>
#include <signal.h>
-#include <visp/vpKeyboard.h>
+#include <visp3/io/vpKeyboard.h>
int
diff --git a/example/tools/parallelPort.cpp b/example/tools/parallelPort.cpp
index 789f522..6889a11 100644
--- a/example/tools/parallelPort.cpp
+++ b/example/tools/parallelPort.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: parallelPort.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of keybord management.
*
@@ -45,8 +41,8 @@
Send a data to the parallel port.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if defined VISP_HAVE_PARPORT
#include <stdlib.h>
@@ -54,8 +50,8 @@
#include <iostream>
#include <signal.h>
-#include <visp/vpParallelPort.h>
-#include <visp/vpParseArgv.h>
+#include <visp3/io/vpParallelPort.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
#define GETOPTARGS "d:h"
diff --git a/example/tools/plot2d.cpp b/example/tools/plot2d.cpp
index 5c487c3..5215dd8 100644
--- a/example/tools/plot2d.cpp
+++ b/example/tools/plot2d.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: plot2d.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example which describes how to use the vpPlot class
*
@@ -46,11 +42,11 @@
Plot 2D curves example.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <visp/vpPlot.h>
-#include <visp/vpMath.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/core/vpMath.h>
int main ()
{
diff --git a/example/tools/plot3d.cpp b/example/tools/plot3d.cpp
index 39e7f72..659b5ca 100755
--- a/example/tools/plot3d.cpp
+++ b/example/tools/plot3d.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: plot.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example which describes how to use the vpPlot class
*
@@ -47,8 +43,8 @@
*/
#include <iostream>
-#include <visp/vpConfig.h>
-#include <visp/vpPlot.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpPlot.h>
int main ()
{
diff --git a/example/tracking/CMakeLists.txt b/example/tracking/CMakeLists.txt
index fad0953..00e492a 100644
--- a/example/tracking/CMakeLists.txt
+++ b/example/tracking/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,12 +35,13 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
+project(example-tracking)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui)
+
+set(example_cpp
mbtEdgeTracking.cpp
mbtEdgeKltTracking.cpp
mbtKltTracking.cpp
@@ -52,24 +50,16 @@ set(SOURCE
trackMeCircle.cpp
trackMeEllipse.cpp
trackMeLine.cpp
- trackMeNurbs.cpp
+# trackMeNurbs.cpp
trackDot.cpp
trackDot2.cpp
trackKltOpencv.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
endforeach()
@@ -89,7 +79,7 @@ add_test(trackDot2WithAutoDetection trackDot2WithAutoDetection -c ${OPTION_TO_DE
add_test(trackMeCircle trackMeCircle -c ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(trackMeEllipse trackMeEllipse -c ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(trackMeLine trackMeLine -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(trackMeNurbs trackMeNurbs -c ${OPTION_TO_DESACTIVE_DISPLAY})
+#add_test(trackMeNurbs trackMeNurbs -c ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(trackDot trackDot -c ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(trackDot2 trackDot2 -c ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -104,6 +94,7 @@ add_test(templateTrackerPyramidal-SSDESM-Homography templateTracker -c -l 2 -
add_test(templateTrackerPyramidal-SSDESM-HomographySL3 templateTracker -c -l 2 -t 0 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDESM-SRT templateTracker -c -l 2 -t 0 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDESM-Translation templateTracker -c -l 2 -t 0 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-SSDESM-RT templateTracker -c -l 2 -t 0 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDForwardAdditional-Affine templateTracker -c -l 2 -t 1 -w 0 ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDForwardAdditional-Homography templateTracker -c -l 2 -t 1 -w 1 ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -116,6 +107,7 @@ add_test(templateTrackerPyramidal-SSDForwardAdditional-Homography templateTra
add_test(templateTrackerPyramidal-SSDForwardAdditional-HomographySL3 templateTracker -c -l 2 -t 1 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDForwardAdditional-SRT templateTracker -c -l 2 -t 1 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDForwardAdditional-Translation templateTracker -c -l 2 -t 1 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-SSDForwardAdditional-RT templateTracker -c -l 2 -t 1 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDForwardCompositional-Affine templateTracker -c -l 2 -t 2 -w 0 ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDForwardCompositional-Homography templateTracker -c -l 2 -t 2 -w 1 ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -128,6 +120,7 @@ add_test(templateTrackerPyramidal-SSDForwardCompositional-Homography template
add_test(templateTrackerPyramidal-SSDForwardCompositional-HomographySL3 templateTracker -c -l 2 -t 2 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDForwardCompositional-SRT templateTracker -c -l 2 -t 2 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDForwardCompositional-Translation templateTracker -c -l 2 -t 2 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-SSDForwardCompositional-RT templateTracker -c -l 2 -t 2 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDInverseCompositional-Affine templateTracker -c -l 2 -t 3 -w 0 ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-SSDInverseCompositional-Homography templateTracker -c -l 2 -t 3 -w 1 ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -140,6 +133,7 @@ add_test(templateTrackerPyramidal-SSDInverseCompositional-Homography template
add_test(templateTrackerPyramidal-SSDInverseCompositional-HomographySL3 templateTracker -c -l 2 -t 3 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDInverseCompositional-SRT templateTracker -c -l 2 -t 3 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-SSDInverseCompositional-Translation templateTracker -c -l 2 -t 3 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-SSDInverseCompositional-RT templateTracker -c -l 2 -t 3 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-ZNCCForwardAdditional-Affine templateTracker -c -l 2 -t 4 -w 0 ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-ZNCCForwardAdditional-Homography templateTracker -c -l 2 -t 4 -w 1 ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -152,6 +146,7 @@ add_test(templateTrackerPyramidal-ZNCCForwardAdditional-Homography templateTr
add_test(templateTrackerPyramidal-ZNCCForwardAdditional-HomographySL3 templateTracker -c -l 2 -t 4 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-ZNCCForwardAdditional-SRT templateTracker -c -l 2 -t 4 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-ZNCCForwardAdditional-Translation templateTracker -c -l 2 -t 4 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-ZNCCForwardAdditional-RT templateTracker -c -l 2 -t 4 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-ZNCCInverseCompositional-Affine templateTracker -c -l 2 -t 5 -w 0 ${OPTION_TO_DESACTIVE_DISPLAY})
#add_test(templateTracker-ZNCCInverseCompositional-Homography templateTracker -c -l 2 -t 5 -w 1 ${OPTION_TO_DESACTIVE_DISPLAY})
@@ -164,3 +159,13 @@ add_test(templateTrackerPyramidal-ZNCCInverseCompositional-Homography templat
add_test(templateTrackerPyramidal-ZNCCInverseCompositional-HomographySL3 templateTracker -c -l 2 -t 5 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-ZNCCInverseCompositional-SRT templateTracker -c -l 2 -t 5 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(templateTrackerPyramidal-ZNCCInverseCompositional-Translation templateTracker -c -l 2 -t 5 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(templateTrackerPyramidal-ZNCCInverseCompositional-RT templateTracker -c -l 2 -t 5 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+
+if(BUILD_MODULE_visp_tt_mi)
+ add_test(templateTrackerPyramidal-MIInverseCompositional-Affine templateTracker -c -l 2 -t 9 -w 0 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+ add_test(templateTrackerPyramidal-MIInverseCompositional-Homography templateTracker -c -l 2 -t 9 -w 1 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+ add_test(templateTrackerPyramidal-MIInverseCompositional-HomographySL3 templateTracker -c -l 2 -t 9 -w 2 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+ add_test(templateTrackerPyramidal-MIInverseCompositional-SRT templateTracker -c -l 2 -t 9 -w 3 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+ add_test(templateTrackerPyramidal-MIInverseCompositional-Translation templateTracker -c -l 2 -t 9 -w 4 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+ add_test(templateTrackerPyramidal-MIInverseCompositional-RT templateTracker -c -l 2 -t 9 -w 5 -p ${OPTION_TO_DESACTIVE_DISPLAY})
+endif()
diff --git a/example/tracking/mbtEdgeKltTracking.cpp b/example/tracking/mbtEdgeKltTracking.cpp
index 84f3853..d890915 100644
--- a/example/tracking/mbtEdgeKltTracking.cpp
+++ b/example/tracking/mbtEdgeKltTracking.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: mbtTracking.cpp 3957 2012-11-07 15:22:30Z fnovotny $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of Hybrid Tracking of MBT and MBT KTL.
*
@@ -45,30 +41,31 @@
\brief Example of Hybrid Tracking of MBT and MBT KTL on an image sequence containing a cube.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
+#include <iostream>
+#include <visp3/core/vpConfig.h>
-#if defined (VISP_HAVE_OPENCV) && defined (VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#include <visp3/core/vpDebug.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfCo"
+#define GETOPTARGS "x:m:i:n:dchtfColw"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre);
+ bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &useScanline);
void usage(const char *name, const char *badparam)
{
@@ -78,7 +75,7 @@ Example of tracking based on the 3D model.\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
[-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C]",
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l]",
name );
fprintf(stdout, "\n\
@@ -130,6 +127,12 @@ OPTIONS: \n\
-o\n\
Use Ogre3D for visibility tests\n\
\n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog thatallows to set the renderer.\n\
+\n\
+ -l\n\
+ Use the scanline for visibility tests\n\
+\n\
-h \n\
Print the help.\n\n");
@@ -140,7 +143,7 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre)
+ bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline)
{
const char *optarg_;
int c;
@@ -156,7 +159,9 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
case 'c': click_allowed = false; break;
case 'd': display = false; break;
case 'C': trackCylinder = false; break;
- case 'o' : useOgre = true; break;
+ case 'o': useOgre = true; break;
+ case 'l': useScanline = true; break;
+ case 'w': showOgreConfigDialog = true; break;
case 'h': usage(argv[0], NULL); return false; break;
default:
@@ -195,6 +200,8 @@ main(int argc, const char ** argv)
bool cao3DModel = false;
bool trackCylinder = true;
bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
bool quit = false;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
@@ -205,7 +212,9 @@ main(int argc, const char ** argv)
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures, opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre)) {
+ if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
+ useScanline)) {
return (-1);
}
@@ -254,7 +263,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -267,7 +276,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -340,7 +349,6 @@ main(int argc, const char ** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
vpKltOpencv klt;
klt.setMaxFeatures(10000);
@@ -370,6 +378,11 @@ main(int argc, const char ** argv)
// Tells if the tracker has to use Ogre3D for visibility tests
tracker.setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker.setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker.setScanLineVisibilityTest(useScanline);
// Retrieve the camera parameters from the tracker
tracker.getCameraParameters(cam);
@@ -442,7 +455,6 @@ main(int argc, const char ** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
vpKltOpencv klt;
klt.setMaxFeatures(10000);
@@ -469,28 +481,29 @@ main(int argc, const char ** argv)
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam);
tracker.setOgreVisibilityTest(useOgre);
+ tracker.setScanLineVisibilityTest(useScanline);
tracker.initFromPose(I, cMo);
}
// Test to set an initial pose
if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
- cMo.buildFrom(0.04371844921, 0.08438820979, 0.5382029442, 2.200417277, 0.873535825, -0.3479076844);
+ cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
vpTRACE("Test set pose");
tracker.setPose(I, cMo);
- if (opt_display) {
- // display the 3D model
- tracker.display(I, cMo, cam, vpColor::darkRed);
- // display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
-// if (opt_click_allowed) {
-// vpDisplay::flush(I);
-// vpDisplay::getClick(I);
-// }
- }
+// if (opt_display) {
+// // display the 3D model
+// tracker.display(I, cMo, cam, vpColor::darkRed);
+// // display the frame
+// vpDisplay::displayFrame (I, cMo, cam, 0.05);
+//// if (opt_click_allowed) {
+//// vpDisplay::flush(I);
+//// vpDisplay::getClick(I);
+//// }
+// }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() > reader.getFirstFrameIndex() + 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
@@ -525,7 +538,7 @@ main(int argc, const char ** argv)
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
// Cleanup memory allocated by Coin library used to load a vrml model in vpMbEdgeKltTracker::loadModel()
// We clean only if Coin was used.
if(! cao3DModel)
@@ -544,7 +557,7 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "OpenCV and display are required." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
return 0;
}
diff --git a/example/tracking/mbtEdgeTracking.cpp b/example/tracking/mbtEdgeTracking.cpp
index 04ec00c..e471316 100644
--- a/example/tracking/mbtEdgeTracking.cpp
+++ b/example/tracking/mbtEdgeTracking.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: mbtEdgeTracking.cpp 5156 2015-01-13 07:07:08Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of model based tracking.
*
@@ -47,29 +43,31 @@
\brief Example of model based tracking on an image sequence containing a cube.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
-
-#if defined (VISP_HAVE_DISPLAY)
-
-#define GETOPTARGS "x:m:i:n:dchtfCo"
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+
+#define GETOPTARGS "x:m:i:n:dchtfColw"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre);
+ bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &useScanline);
void usage(const char *name, const char *badparam)
{
@@ -79,7 +77,7 @@ Example of tracking based on the 3D model.\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
[-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C]",
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l]",
name );
fprintf(stdout, "\n\
@@ -129,7 +127,13 @@ OPTIONS: \n\
execution of this program without humain intervention.\n\
\n\
-o\n\
- Use Ogre3D for visibility tests\n\
+ Use Ogre3D for visibility tests.\n\
+\n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog thatallows to set the renderer.\n\
+\n\
+ -l\n\
+ Use the scanline for visibility tests\n\
\n\
-h \n\
Print the help.\n\n");
@@ -141,7 +145,7 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre)
+ bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline)
{
const char *optarg_;
int c;
@@ -158,6 +162,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
case 'd': display = false; break;
case 'C': trackCylinder = false; break;
case 'o': useOgre = true; break;
+ case 'l': useScanline = true; break;
+ case 'w': showOgreConfigDialog = true; break;
case 'h': usage(argv[0], NULL); return false; break;
default:
@@ -196,6 +202,8 @@ main(int argc, const char ** argv)
bool cao3DModel = false;
bool trackCylinder = true;
bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
bool quit = false;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
@@ -207,7 +215,9 @@ main(int argc, const char ** argv)
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures, opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre)) {
+ if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
+ useScanline)) {
return (-1);
}
@@ -256,7 +266,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -269,7 +279,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -342,7 +352,6 @@ main(int argc, const char ** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
tracker.setCameraParameters(cam);
tracker.setMovingEdge(me);
@@ -359,6 +368,11 @@ main(int argc, const char ** argv)
// Tells if the tracker has to use Ogre3D for visibility tests
tracker.setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker.setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker.setScanLineVisibilityTest(useScanline);
// Retrieve the camera parameters from the tracker
tracker.getCameraParameters(cam);
@@ -431,7 +445,6 @@ main(int argc, const char ** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
tracker.setCameraParameters(cam);
tracker.setMovingEdge(me);
@@ -445,28 +458,29 @@ main(int argc, const char ** argv)
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam);
tracker.setOgreVisibilityTest(useOgre);
+ tracker.setScanLineVisibilityTest(useScanline);
tracker.initFromPose(I, cMo);
}
// Test to set an initial pose
if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
- cMo.buildFrom(0.04371844921, 0.08438820979, 0.5382029442, 2.200417277, 0.873535825, -0.3479076844);
+ cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
vpTRACE("Test set pose");
tracker.setPose(I, cMo);
- if (opt_display) {
- // display the 3D model
- tracker.display(I, cMo, cam, vpColor::darkRed);
- // display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
-// if (opt_click_allowed) {
-// vpDisplay::flush(I);
-// vpDisplay::getClick(I);
-// }
- }
+// if (opt_display) {
+// // display the 3D model
+// tracker.display(I, cMo, cam, vpColor::darkRed);
+// // display the frame
+// vpDisplay::displayFrame (I, cMo, cam, 0.05);
+//// if (opt_click_allowed) {
+//// vpDisplay::flush(I);
+//// vpDisplay::getClick(I);
+//// }
+// }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() > reader.getFirstFrameIndex() + 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
@@ -502,7 +516,7 @@ main(int argc, const char ** argv)
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
// Cleanup memory allocated by Coin library used to load a vrml model in vpMbEdgeTracker::loadModel()
// We clean only if Coin was used.
if(! cao3DModel)
@@ -521,7 +535,7 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "Display is required to run this example." << std::endl;
+ std::cout << "visp_mbt module is required to run this example." << std::endl;
return 0;
}
diff --git a/example/tracking/mbtKltTracking.cpp b/example/tracking/mbtKltTracking.cpp
index 080b67d..fffc508 100644
--- a/example/tracking/mbtKltTracking.cpp
+++ b/example/tracking/mbtKltTracking.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: mbtTracking.cpp 3957 2012-11-07 15:22:30Z fnovotny $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of MBT KLT Tracking.
*
@@ -45,25 +41,26 @@
\brief Example of MBT KLT Tracking on an image sequence containing a cube.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
+#include <iostream>
+#include <visp3/core/vpConfig.h>
-#if defined (VISP_HAVE_OPENCV) && defined (VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#include <visp3/core/vpDebug.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/mbt/vpMbKltTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfo"
+#define GETOPTARGS "x:m:i:n:dchtfolw"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
@@ -78,7 +75,7 @@ Example of tracking based on the 3D model.\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
[-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f]",
+ [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l]",
name );
fprintf(stdout, "\n\
@@ -125,6 +122,12 @@ OPTIONS: \n\
-o\n\
Use Ogre3D for visibility tests\n\
\n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog thatallows to set the renderer.\n\
+\n\
+ -l\n\
+ Use the scanline for visibility tests\n\
+\n\
-h \n\
Print the help.\n\n");
@@ -135,7 +138,7 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
std::string &initFile, bool &displayKltPoints, bool &click_allowed, bool &display,
- bool& cao3DModel, bool &useOgre)
+ bool& cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline)
{
const char *optarg_;
int c;
@@ -151,6 +154,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
case 'c': click_allowed = false; break;
case 'd': display = false; break;
case 'o': useOgre = true; break;
+ case 'l': useScanline = true; break;
+ case 'w': showOgreConfigDialog = true; break;
case 'h': usage(argv[0], NULL); return false; break;
default:
@@ -188,6 +193,8 @@ main(int argc, const char ** argv)
bool opt_display = true;
bool cao3DModel = false;
bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
bool quit = false;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
@@ -198,7 +205,8 @@ main(int argc, const char ** argv)
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayKltPoints, opt_click_allowed, opt_display, cao3DModel, useOgre)) {
+ if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayKltPoints,
+ opt_click_allowed, opt_display, cao3DModel, useOgre, showOgreConfigDialog, useScanline)) {
return (-1);
}
@@ -240,7 +248,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -253,7 +261,7 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
}
else{
-#ifdef VISP_HAVE_COIN
+#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
@@ -345,6 +353,11 @@ main(int argc, const char ** argv)
// Tells if the tracker has to use Ogre3D for visibility tests
tracker.setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker.setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker.setScanLineVisibilityTest(useScanline);
// Retrieve the camera parameters from the tracker
tracker.getCameraParameters(cam);
@@ -433,28 +446,29 @@ main(int argc, const char ** argv)
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam);
tracker.setOgreVisibilityTest(useOgre);
+ tracker.setScanLineVisibilityTest(useScanline);
tracker.initFromPose(I, cMo);
}
// Test to set an initial pose
if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
- cMo.buildFrom(0.04371844921, 0.08438820979, 0.5382029442, 2.200417277, 0.873535825, -0.3479076844);
+ cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
vpTRACE("Test set pose");
tracker.setPose(I, cMo);
- if (opt_display) {
- // display the 3D model
- tracker.display(I, cMo, cam, vpColor::darkRed);
- // display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
-// if (opt_click_allowed) {
-// vpDisplay::flush(I);
-// vpDisplay::getClick(I);
-// }
- }
+// if (opt_display) {
+// // display the 3D model
+// tracker.display(I, cMo, cam, vpColor::darkRed);
+// // display the frame
+// vpDisplay::displayFrame (I, cMo, cam, 0.05);
+//// if (opt_click_allowed) {
+//// vpDisplay::flush(I);
+//// vpDisplay::getClick(I);
+//// }
+// }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() > reader.getFirstFrameIndex() + 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
@@ -473,11 +487,11 @@ main(int argc, const char ** argv)
}
}
- // Uncomment if you want to print the covariance matrix.
+ // Uncomment if you want to print the covariance matrix.
// Make sure tracker.setCovarianceComputation(true) has been called (uncomment below).
// std::cout << tracker.getCovarianceMatrix() << std::endl << std::endl;
- vpDisplay::flush(I);
+ vpDisplay::flush(I) ;
}
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I);
@@ -490,7 +504,7 @@ main(int argc, const char ** argv)
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
// Cleanup memory allocated by Coin library used to load a vrml model in vpMbKltTracker::loadModel()
// We clean only if Coin was used.
if(! cao3DModel)
@@ -509,7 +523,7 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "OpenCV and display are required." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
return 0;
}
diff --git a/example/tracking/templateTracker.cpp b/example/tracking/templateTracker.cpp
index 26f3976..6993f2a 100644
--- a/example/tracking/templateTracker.cpp
+++ b/example/tracking/templateTracker.cpp
@@ -1,9 +1,7 @@
/****************************************************************************
*
- * $Id: templateTracker.cpp 5002 2014-11-24 08:18:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
@@ -12,20 +10,19 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See http://visp.inria.fr for more information.
*
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
+ * Inria at visp at 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.
@@ -46,36 +43,45 @@
\brief Example of template tracking.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpParseArgv.h>
-
-#include <visp/vpTemplateTrackerSSD.h>
-#include <visp/vpTemplateTrackerSSDForwardAdditional.h>
-#include <visp/vpTemplateTrackerSSDForwardCompositional.h>
-#include <visp/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp/vpTemplateTrackerSSDESM.h>
-#include <visp/vpTemplateTrackerZNCCForwardAdditional.h>
-#include <visp/vpTemplateTrackerZNCCInverseCompositional.h>
-
-#include <visp/vpTemplateTrackerWarpAffine.h>
-#include <visp/vpTemplateTrackerWarpHomography.h>
-#include <visp/vpTemplateTrackerWarpHomographySL3.h>
-#include <visp/vpTemplateTrackerWarpSRT.h>
-#include <visp/vpTemplateTrackerWarpTranslation.h>
-
-#if defined (VISP_HAVE_DISPLAY)
-
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_TT) && defined (VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpParseArgv.h>
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+#include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
+#include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
+#include <visp3/tt/vpTemplateTrackerSSDESM.h>
+#include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
+#include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
+
+#include <visp3/tt/vpTemplateTrackerWarpAffine.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
+#include <visp3/tt/vpTemplateTrackerWarpSRT.h>
+#include <visp3/tt/vpTemplateTrackerWarpTranslation.h>
+#include <visp3/tt/vpTemplateTrackerWarpRT.h>
+
+#ifdef VISP_HAVE_MODULE_TT_MI
+# include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
+# include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
+# include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
+# include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
+#endif
#define GETOPTARGS "cdhi:l:pt:w:"
@@ -85,7 +91,11 @@ typedef enum {
WARP_HOMOGRAPHY,
WARP_HOMOGRAPHY_SL3,
WARP_SRT,
- WARP_TRANSLATION
+ WARP_TRANSLATION,
+#ifdef VISP_HAVE_MODULE_TT_MI
+ WARP_RT,
+#endif
+ WARP_LAST
} WarpType;
typedef enum {
@@ -94,7 +104,14 @@ typedef enum {
TRACKER_SSD_FORWARD_COMPOSITIONAL,
TRACKER_SSD_INVERSE_COMPOSITIONAL, // The most efficient
TRACKER_ZNCC_FORWARD_ADDITIONEL,
- TRACKER_ZNCC_INVERSE_COMPOSITIONAL
+ TRACKER_ZNCC_INVERSE_COMPOSITIONAL,
+#ifdef VISP_HAVE_MODULE_TT_MI
+ TRACKER_MI_ESM,
+ TRACKER_MI_FORWARD_ADDITIONAL,
+ TRACKER_MI_FORWARD_COMPOSITIONAL,
+ TRACKER_MI_INVERSE_COMPOSITIONAL, // The most efficient
+#endif
+ TRACKER_LAST
} TrackerType;
#endif
@@ -136,7 +153,22 @@ OPTIONS: Default\n\
-c\n\
Disable the mouse click. Useful to automaze the \n\
execution of this program without humain intervention.\n\
- \n\
+ \n", last_frame);
+
+#ifdef VISP_HAVE_MODULE_TT_MI
+ fprintf(stdout, "\n\
+ -w <warp type=[0,1,2,3,4,5]> %d\n\
+ Set the model used to warp the template. \n\
+ Authorized values are:\n\
+ %d : Affine\n\
+ %d : Homography\n\
+ %d : Homography in SL3\n\
+ %d : SRT (scale, rotation, translation)\n\
+ %d : RT (rotation, translation)\n\
+ %d : Translation\n\n",
+ (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION, (int)WARP_RT);
+#else
+ fprintf(stdout, "\n\
-w <warp type=[0,1,2,3,4]> %d\n\
Set the model used to warp the template. \n\
Authorized values are:\n\
@@ -144,9 +176,13 @@ OPTIONS: Default\n\
%d : Homography\n\
%d : Homography in SL3\n\
%d : SRT (scale, rotation, translation)\n\
- %d : Translation\n\
- \n\
- -t <tracker type=[0,1,2,3,4,5]> %d\n\
+ %d : Translation\n\n",
+ (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION);
+#endif
+
+#ifdef VISP_HAVE_MODULE_TT_MI
+ fprintf(stdout, "\n\
+ -t <tracker type=[0,1,2,3,4,5,6,7,8,9]> %d\n\
Set the tracker used to track the template. \n\
Authorized values are:\n\
%d : SSD ESM\n\
@@ -155,18 +191,40 @@ OPTIONS: Default\n\
%d : SSD inverse compositional\n\
%d : ZNCC forward additional\n\
%d : ZNCC inverse compositional\n\
- \n\
- -p\n\
- Enable pyramidal tracking.\n\
- \n\
- -h \n\
- Print the help.\n\n",
- last_frame, (int)warp_type,
- (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION,
+ %d : MI ESM\n\
+ %d : MI forward additional\n\
+ %d : MI forward compositional\n\
+ %d : MI inverse compositional\n\n",
+ (int)tracker_type,
+ (int)TRACKER_SSD_ESM, (int)TRACKER_SSD_FORWARD_ADDITIONAL, (int)TRACKER_SSD_FORWARD_COMPOSITIONAL,
+ (int)TRACKER_SSD_INVERSE_COMPOSITIONAL, (int)TRACKER_ZNCC_FORWARD_ADDITIONEL,
+ (int)TRACKER_ZNCC_INVERSE_COMPOSITIONAL,
+ (int)TRACKER_MI_ESM, (int)TRACKER_MI_FORWARD_ADDITIONAL, (int)TRACKER_MI_FORWARD_COMPOSITIONAL,
+ (int)TRACKER_MI_INVERSE_COMPOSITIONAL);
+#else
+ fprintf(stdout, "\n\
+ -t <tracker type=[0,1,2,3,4,5]> %d\n\
+ Set the tracker used to track the template. \n\
+ Authorized values are:\n\
+ %d : SSD ESM\n\
+ %d : SSD forward additional\n\
+ %d : SSD forward compositional\n\
+ %d : SSD inverse compositional\n\
+ %d : ZNCC forward additional\n\
+ %d : ZNCC inverse compositional\n\n",
(int)tracker_type,
(int)TRACKER_SSD_ESM, (int)TRACKER_SSD_FORWARD_ADDITIONAL, (int)TRACKER_SSD_FORWARD_COMPOSITIONAL,
(int)TRACKER_SSD_INVERSE_COMPOSITIONAL, (int)TRACKER_ZNCC_FORWARD_ADDITIONEL,
(int)TRACKER_ZNCC_INVERSE_COMPOSITIONAL);
+
+#endif
+ fprintf(stdout, "\n\
+ -p\n\
+ Enable pyramidal tracking.\n\
+ \n\
+ -h \n\
+ Print the help.\n\n");
+
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
@@ -195,13 +253,13 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
}
}
- if (warp_type > WARP_TRANSLATION) {
+ if (warp_type >= WARP_LAST) {
usage(argv[0], NULL, warp_type, tracker_type, last_frame);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument -w <warp type> with \"warp type\"=" << (int)warp_type << std::endl << std::endl;
return false;
}
- if (tracker_type > TRACKER_ZNCC_INVERSE_COMPOSITIONAL) {
+ if (tracker_type >= TRACKER_LAST) {
usage(argv[0], NULL, warp_type, tracker_type, last_frame);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument -t <tracker type> with \"tracker type\"=" << (int)tracker_type << std::endl << std::endl;
@@ -310,6 +368,10 @@ main(int argc, const char ** argv)
case WARP_HOMOGRAPHY_SL3: warp = new vpTemplateTrackerWarpHomographySL3; break;
case WARP_SRT: warp = new vpTemplateTrackerWarpSRT; break;
case WARP_TRANSLATION: warp = new vpTemplateTrackerWarpTranslation; break;
+#ifdef VISP_HAVE_MODULE_TT_MI
+ case WARP_RT: warp = new vpTemplateTrackerWarpRT; break;
+#endif
+ default: return 0;
}
vpTemplateTracker *tracker = NULL;
@@ -320,6 +382,13 @@ main(int argc, const char ** argv)
case TRACKER_SSD_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerSSDInverseCompositional(warp); break;
case TRACKER_ZNCC_FORWARD_ADDITIONEL: tracker = new vpTemplateTrackerZNCCForwardAdditional(warp); break;
case TRACKER_ZNCC_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerZNCCInverseCompositional(warp); break;
+#ifdef VISP_HAVE_MODULE_TT_MI
+ case TRACKER_MI_ESM: tracker = new vpTemplateTrackerMIESM(warp); break;
+ case TRACKER_MI_FORWARD_ADDITIONAL: tracker = new vpTemplateTrackerMIForwardAdditional(warp); break;
+ case TRACKER_MI_FORWARD_COMPOSITIONAL: tracker = new vpTemplateTrackerMIForwardCompositional(warp); break;
+ case TRACKER_MI_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerMIInverseCompositional(warp); break;
+#endif
+ default: return 0;
}
tracker->setSampling(2,2);
@@ -421,7 +490,7 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "No display is available." << std::endl;
+ std::cout << "visp_tt module or display not available." << std::endl;
return 0;
}
diff --git a/example/tracking/trackDot.cpp b/example/tracking/trackDot.cpp
index 7d895e6..eeb269a 100644
--- a/example/tracking/trackDot.cpp
+++ b/example/tracking/trackDot.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackDot.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of dot tracking.
*
@@ -53,26 +49,26 @@
Example of dot tracking on an image sequence using vpDot.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_BLOB) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdf:i:n:p:s:h"
@@ -464,10 +460,11 @@ main(int argc, const char ** argv)
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_blob module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/tracking/trackDot2.cpp b/example/tracking/trackDot2.cpp
index a659837..e575288 100644
--- a/example/tracking/trackDot2.cpp
+++ b/example/tracking/trackDot2.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackDot2.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of dot tracking.
*
@@ -45,26 +41,26 @@
\brief Example of dot tracking on an image sequence using vpDot2.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_BLOB) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot2.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdf:i:n:p:s:h"
@@ -511,10 +507,11 @@ main(int argc, const char ** argv)
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/tracking/trackDot2WithAutoDetection.cpp b/example/tracking/trackDot2WithAutoDetection.cpp
index b05db3d..aaf7de4 100644
--- a/example/tracking/trackDot2WithAutoDetection.cpp
+++ b/example/tracking/trackDot2WithAutoDetection.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackDot2WithAutoDetection.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Test auto detection of dots.
*
@@ -50,26 +46,26 @@
Example of auto detection of dots using vpDot2.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_BLOB) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDot2.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:p:f:n:s:S:G:E:h"
@@ -510,17 +506,11 @@ main(int argc, const char ** argv)
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/example/tracking/trackKltOpencv.cpp b/example/tracking/trackKltOpencv.cpp
index 5b2dd80..d399556 100644
--- a/example/tracking/trackKltOpencv.cpp
+++ b/example/tracking/trackKltOpencv.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackKltOpencv.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Example of dot tracking.
*
@@ -45,27 +41,27 @@
\brief Example of KLT tracking using OpenCV library.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
#include <vector>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_KLT) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV))
#if defined (VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
-#include <visp/vpKltOpencv.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdf:i:n:p:s:h"
@@ -448,10 +444,12 @@ main()
}
#endif
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_klt module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
+
#endif
diff --git a/example/tracking/trackMeCircle.cpp b/example/tracking/trackMeCircle.cpp
index 43bc4e9..b2e4878 100644
--- a/example/tracking/trackMeCircle.cpp
+++ b/example/tracking/trackMeCircle.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackMeCircle.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tracking of an ellipse.
*
@@ -51,27 +47,26 @@
Tracking of an ellipse using vpMe.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_ME) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpColor.h>
-#include <visp/vpMeEllipse.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/me/vpMeEllipse.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:h"
@@ -286,7 +281,6 @@ main(int argc, const char ** argv)
me.setPointsToTrack(60) ;
me.setThreshold(15000) ;
- E1.setCircle(true) ;
E1.setMe(&me) ;
E1.setDisplay(vpMeSite::RANGE_RESULT) ;
// If click is allowed, wait for a mouse click to select the points
@@ -296,15 +290,14 @@ main(int argc, const char ** argv)
}
else {
// Create a list of points to automate the test
- unsigned int n=5 ;
- vpImagePoint *ip = new vpImagePoint [n];
- ip[0].set_i( 39 ); ip[0].set_j( 136 );
- ip[1].set_i( 42 ); ip[1].set_j( 83 );
- ip[2].set_i( 86 ); ip[2].set_j( 55 );
- ip[3].set_i( 132 ); ip[3].set_j( 72 );
- ip[4].set_i( 145 ); ip[4].set_j( 134 );
- E1.initTracking(I, n, ip) ;
- delete [] ip ;
+ std::vector<vpImagePoint> ip;
+ ip.push_back(vpImagePoint(39, 136));
+ ip.push_back(vpImagePoint(42, 83));
+ ip.push_back(vpImagePoint(86, 55));
+ ip.push_back(vpImagePoint(132, 72));
+ ip.push_back(vpImagePoint(145, 134));
+
+ E1.initTracking(I, ip) ;
}
if (opt_display) {
@@ -332,10 +325,11 @@ main(int argc, const char ** argv)
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/tracking/trackMeEllipse.cpp b/example/tracking/trackMeEllipse.cpp
index e352385..d082733 100644
--- a/example/tracking/trackMeEllipse.cpp
+++ b/example/tracking/trackMeEllipse.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackMeEllipse.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tracking of an ellipse.
*
@@ -51,8 +47,8 @@
Tracking of an ellipse using vpMe.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
@@ -60,20 +56,20 @@
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_ME) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpColor.h>
-#include <visp/vpMeEllipse.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/me/vpMeEllipse.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:h"
@@ -296,15 +292,14 @@ main(int argc, const char ** argv)
E1.initTracking(I) ;
else {
// Create a list of points to automate the test
- unsigned int n=5 ;
- vpImagePoint *ip = new vpImagePoint [n];
- ip[0].set_i( 33 ); ip[0].set_j( 276 );
- ip[1].set_i( 83 ); ip[1].set_j( 126 );
- ip[2].set_i( 201 ); ip[2].set_j( 36 );
- ip[3].set_i( 243 ); ip[3].set_j( 164 );
- ip[4].set_i( 195 ); ip[4].set_j( 329 );
- E1.initTracking(I, n, ip) ;
- delete [] ip ;
+ std::vector<vpImagePoint> ip;
+ ip.push_back(vpImagePoint(33, 276));
+ ip.push_back(vpImagePoint(83, 126));
+ ip.push_back(vpImagePoint(201, 36));
+ ip.push_back(vpImagePoint(243, 164));
+ ip.push_back(vpImagePoint(195, 329));
+
+ E1.initTracking(I, ip) ;
}
if (opt_display) {
E1.display(I, vpColor::green) ;
@@ -352,10 +347,11 @@ main(int argc, const char ** argv)
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/tracking/trackMeLine.cpp b/example/tracking/trackMeLine.cpp
index c91695c..8f9dfdb 100644
--- a/example/tracking/trackMeLine.cpp
+++ b/example/tracking/trackMeLine.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackMeLine.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tracking of a line.
*
@@ -51,32 +47,32 @@
Tracking of a line using vpMe.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_ME) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpColor.h>
-#include <visp/vpMeLine.h>
+#include <visp3/me/vpMeLine.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:h"
@@ -364,10 +360,11 @@ main(int argc, const char ** argv)
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/tracking/trackMeNurbs.cpp b/example/tracking/trackMeNurbs.cpp
index eda500c..6d2b6ac 100644
--- a/example/tracking/trackMeNurbs.cpp
+++ b/example/tracking/trackMeNurbs.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: trackMeNurbs.cpp 5108 2015-01-05 07:48:58Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Tracking of a nurbs.
*
@@ -52,29 +48,29 @@
Tracking of a nurbs using vpMe.
*/
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if defined(VISP_HAVE_MODULE_ME) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpColor.h>
-#include <visp/vpMeNurbs.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/me/vpMeNurbs.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpVideoReader.h>
// List of allowed command line options
#define GETOPTARGS "cdi:h"
@@ -324,15 +320,16 @@ main(int argc, const char ** argv)
return 0;
}
catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
+ std::cout << "Catch an exception: " << e.getMessage() << std::endl;
+ return 0;
}
}
#else
-int
-main()
+#include <iostream>
+
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities...");
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
}
#endif
diff --git a/example/video/CMakeLists.txt b/example/video/CMakeLists.txt
index 1b2a73e..1f54cd3 100644
--- a/example/video/CMakeLists.txt
+++ b/example/video/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 2158 2009-05-07 07:24:51Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,37 +35,25 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-SET (SOURCE
+project(example-video)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui)
+
+set(example_cpp
videoReader.cpp
imageSequenceReader.cpp
)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
-endforeach()
-
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
-add_test(videoReader videoReader -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(imageSequenceReader imageSequenceReader -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+endforeach()
diff --git a/example/video/imageSequenceReader.cpp b/example/video/imageSequenceReader.cpp
index 5880542..3d8b03a 100644
--- a/example/video/imageSequenceReader.cpp
+++ b/example/video/imageSequenceReader.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: imageDiskRW.cpp 2158 2009-05-07 07:24:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Reading an image sequence.
*
@@ -49,16 +45,16 @@
Reading an image sequence using vpVideoReader class.
*/
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)
diff --git a/example/video/videoReader.cpp b/example/video/videoReader.cpp
index 4d6b365..dfbac7a 100644
--- a/example/video/videoReader.cpp
+++ b/example/video/videoReader.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: imageDiskRW.cpp 2158 2009-05-07 07:24:51Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Reading a video file.
*
@@ -49,17 +45,17 @@
Reading a video file using vpVideoReader class.
*/
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)
diff --git a/example/wireframe-simulator/CMakeLists.txt b/example/wireframe-simulator/CMakeLists.txt
index 8678ea0..ac38e2f 100644
--- a/example/wireframe-simulator/CMakeLists.txt
+++ b/example/wireframe-simulator/CMakeLists.txt
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
@@ -38,33 +35,23 @@
#
#############################################################################
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- wireframeSimulator.cpp
-)
+project(example-wireframe-simulator)
+
+cmake_minimum_required(VERSION 2.6)
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
+find_package(VISP REQUIRED visp_core visp_robot visp_io visp_gui)
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
+set(example_cpp
+ wireframeSimulator.cpp
+)
- add_dependencies(visp_examples ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "examples")
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
endif()
-endforeach()
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
-add_test(wireframeSimulator wireframeSimulator -c ${OPTION_TO_DESACTIVE_DISPLAY})
+ # Add test
+ get_filename_component(target ${cpp} NAME_WE)
+ add_test(${target} ${target} -c ${OPTION_TO_DESACTIVE_DISPLAY})
+endforeach()
diff --git a/example/wireframe-simulator/wireframeSimulator.cpp b/example/wireframe-simulator/wireframeSimulator.cpp
index 46600a0..2a26d54 100644
--- a/example/wireframe-simulator/wireframeSimulator.cpp
+++ b/example/wireframe-simulator/wireframeSimulator.cpp
@@ -1,10 +1,8 @@
/****************************************************************************
*
- * $Id: wireframeSimulator.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,25 +10,23 @@
* 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
+ * GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See http://visp.inria.fr for more information.
+ *
* This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
+ * Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
- * http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
+ * Inria at visp at 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:
* Demonstration of the wireframe simulator
*
@@ -47,19 +43,19 @@
#include <stdlib.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
#define GETOPTARGS "cdh"
diff --git a/include/vpConfig.h.cmake b/include/vpConfig.h.cmake
deleted file mode 100644
index 192b846..0000000
--- a/include/vpConfig.h.cmake
+++ /dev/null
@@ -1,368 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpConfig.h.cmake 5324 2015-02-13 16:11:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * ViSP configuration.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpConfig_h
-#define vpConfig_h
-
-#if defined _MSC_VER && _MSC_VER >= 1200
- #pragma warning( disable: 4100 4127 4251 4275 4514 4668 4710 4820 )
- #if _MSC_VER >= 1400 // 1400 = MSVC 8 2005
- #pragma warning( disable: 4548 )
- #endif
- #if _MSC_VER > 1500 // 1500 = MSVC 9 2008
- #pragma warning( disable: 4986 )
- #endif
-
- // 4100 : undocumented ("unreferenced formal parameter")
- // 4127 : conditional expression is constant
- // 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions
- // 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier'
- // 4514 : 'function' : unreferenced inline function has been removed
- // 4548 : expression before comma has no effect
- // 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives'
- // 4710 : 'function' : function not inlined
- // 4820 : 'bytes' bytes padding added after construct 'member_name'
- // 4986 : undocumented
-#endif
-
-#if defined _MSC_VER && (_MSC_VER == 1500)
-// Visual Studio 9 2008 specific stuff
-// Fix running 64-bit OpenMP Debug Builds compiled with Visual Studio 2008 SP1
-// See discussion on https://gforge.inria.fr/forum/message.php?msg_id=149273&group_id=397
-// and the proposed fix: http://www.johanseland.com/2010/08/running-64-bit-openmp-debug-builds.html
-# define _BIND_TO_CURRENT_OPENMP_VERSION 1
-#endif
-
-#if defined(__MINGW__) || defined(__MINGW32__) || defined(__MINGW64__)
-// Work arround to fix build issues that may occur with Mingw:
-// error: 'DBL_EPSILON' was not declared in this scope
-// error: 'FLT_EPSILON' was not declared in this scope
-
-# include <float.h>
-
-# ifndef DBL_EPSILON
-# define DBL_EPSILON __DBL_EPSILON__
-# endif
-# ifndef FLT_EPSILON
-# define FLT_EPSILON __FLT_EPSILON__
-# endif
-#endif
-
-// ViSP major version.
-#define VISP_VERSION_MAJOR ${VISP_VERSION_MAJOR}
-
-// ViSP minor version.
-#define VISP_VERSION_MINOR ${VISP_VERSION_MINOR}
-
-// ViSP patch version.
-#define VISP_VERSION_PATCH ${VISP_VERSION_PATCH}
-
-// ViSP version with dots "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}".
-#cmakedefine VISP_VERSION ${VISP_VERSION}
-
-// ViSP version as an integer
-#define VP_VERSION_INT(a, b, c) (a<<16 | b<<8 | c)
-#define VISP_VERSION_INT VP_VERSION_INT(VISP_VERSION_MAJOR, \
- VISP_VERSION_MINOR, \
- VISP_VERSION_PATCH)
-
-// ViSP library is either compiled static or shared
-// Used to set declspec(import, export) in headers if required under Windows
-#cmakedefine VISP_BUILD_SHARED_LIBS
-
-// Defined if deprecated functionalities are requested to build
-#cmakedefine VISP_BUILD_DEPRECATED_FUNCTIONS
-
-// Defined if MSVC is the compiler
-#cmakedefine VISP_USE_MSVC
-
-// Defined if X11 library available.
-#cmakedefine VISP_HAVE_X11
-
-// Defined if XML2 library available.
-#cmakedefine VISP_HAVE_XML2
-
-// Defined if pthread library available.
-#cmakedefine VISP_HAVE_PTHREAD
-
-// Defined if YARP available.
-#cmakedefine VISP_HAVE_YARP
-
-// Defined if OpenCV available.
-#cmakedefine VISP_HAVE_OPENCV
-
-// Defined if OpenCV nonfree module available. Only with OpenCV < 3.0.0
-#cmakedefine VISP_HAVE_OPENCV_NONFREE
-
-// Defined if OpenCV xfeatures2d module available. Only since OpenCV >= 3.0.0
-#cmakedefine VISP_HAVE_OPENCV_XFEATURES2D
-
-// OpenCV version in hexadecimal (for example 2.1.0 gives 0x020100).
-#ifdef VISP_HAVE_OPENCV
-# define VISP_HAVE_OPENCV_VERSION ${VISP_HAVE_OPENCV_VERSION}
-#endif
-
-// Defined if GTK2 library available
-#cmakedefine VISP_HAVE_GTK
-
-// Defined if GDI (Graphics Device Interface) library available
-#cmakedefine VISP_HAVE_GDI
-
-// Defined if Direct3D9 library available
-#cmakedefine VISP_HAVE_D3D9
-
-// Defined if one of the display device is available
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
-# define VISP_HAVE_DISPLAY
-#endif
-
-// Defined if GSL library available (-lgsl -lgslcblas).
-#cmakedefine VISP_HAVE_GSL
-
-// Defined if lapack/blas libraries are available (-lblas -llapack).
-#cmakedefine VISP_HAVE_LAPACK
-
-// Defined the path to the basic scenes used by the simulator
-#cmakedefine VISP_SCENES_DIR "${VISP_SCENES_DIR}"
-
-// Defined the path to the robot's arms 3D model used by the robot simulators
-#cmakedefine VISP_ROBOT_ARMS_DIR "${VISP_ROBOT_ARMS_DIR}"
-
-// Defined if Ogre3d is available.
-#cmakedefine VISP_HAVE_OGRE
-
-// Defined if Ogre3d plugins.cfg is available.
-#cmakedefine VISP_HAVE_OGRE_PLUGINS_PATH "${VISP_HAVE_OGRE_PLUGINS_PATH}"
-
-// Defined if Ogre3d resources.cfg is available.
-#cmakedefine VISP_HAVE_OGRE_RESOURCES_PATH "${VISP_HAVE_OGRE_RESOURCES_PATH}"
-
-// Defined if OIS (Object Oriented Input System) library available.
-#cmakedefine VISP_HAVE_OIS
-
-// Defined if Coin3D and one of the GUI (SoXt, SoWin, SoQt + Qt)
-// libraries are available.
-#cmakedefine VISP_HAVE_COIN_AND_GUI
-
-// Defined if Coin3D library available.
-#cmakedefine VISP_HAVE_COIN
-
-// Defined if OpenGL library available.
-#cmakedefine VISP_HAVE_OPENGL
-
-// Defined if Qt library available (either Qt-3 or Qt-4).
-#cmakedefine VISP_HAVE_QT
-
-// Defined if SoQt library available.
-#cmakedefine VISP_HAVE_SOQT
-
-// Defined if SoWin library available.
-#cmakedefine VISP_HAVE_SOWIN
-
-// Defined if SoXt library available.
-#cmakedefine VISP_HAVE_SOXT
-
-// Defined if libjpeg library available.
-#cmakedefine VISP_HAVE_LIBJPEG
-
-// Defined if libpng library available.
-#cmakedefine VISP_HAVE_LIBPNG
-
-// Defined if libfreenect, libusb-1.0 and libpthread libraries available.
-#cmakedefine VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES
-
-// Defined if libfreenect library available.
-#cmakedefine VISP_HAVE_LIBFREENECT
-
-// Defined if libfreenect library in an old version package for
-// ubuntu 10.04 lucid is available.
-// This is a workaround useful to initialise vpKinect depending on the
-// libfreenect version
-//#ifdef VISP_HAVE_LIBFREENECT_OLD
-// Freenect::Freenect<vpKinect> freenect;
-// vpKinect & kinect = freenect.createDevice(0);
-//#else
-// Freenect::Freenect freenect;
-// vpKinect & kinect = freenect.createDevice<vpKinect>(0);
-//#endif
-#cmakedefine VISP_HAVE_LIBFREENECT_OLD
-
-// Defined if libusb-1.0 library available.
-#cmakedefine VISP_HAVE_LIBUSB_1
-
-// Defined if ffmpeg library available.
-#cmakedefine VISP_HAVE_FFMPEG
-
-// Defined if raw1394 and dc1394-2.x libraries available.
-#cmakedefine VISP_HAVE_DC1394_2
-
-// Defined if dc1394_camera_enumerate() is available in dc1394-2.x.
-// dc1394_camera_enumerate() was introduced after libdc1394-2.0.0-rc7.
-#cmakedefine VISP_HAVE_DC1394_2_CAMERA_ENUMERATE
-
-// Defined if dc1394_find_cameras() is available in dc1394-2.x
-// dc1394_find_cameras() is still present until libdc1394-2.0.0-rc7.
-// This function was suppress and replace by dc1394_camera_enumerate()
-// in more recent releases.
-#cmakedefine VISP_HAVE_DC1394_2_FIND_CAMERAS
-
-// Defined if CMU 1394 Digital Camera SDK available.
-#cmakedefine VISP_HAVE_CMU1394
-
-// Defined if Video For Linux Two available.
-#cmakedefine VISP_HAVE_V4L2
-
-// Defined if DirectShow library is available (only under Windows).
-#cmakedefine VISP_HAVE_DIRECTSHOW
-
-// Defined if Irisa's Afma4 robot available.
-#cmakedefine VISP_HAVE_AFMA4
-
-// Defined if Irisa's Afma6 robot available.
-#cmakedefine VISP_HAVE_AFMA6
-
-// Defined if Biclops pan-tilt head available.
-#cmakedefine VISP_HAVE_BICLOPS
-#cmakedefine VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION
-
-// Defined if Irisa's Ptu-46 pan-tilt head available.
-#cmakedefine VISP_HAVE_PTU46
-
-// Defined if Irisa's Viper S650 robot available.
-#cmakedefine VISP_HAVE_VIPER650
-
-// Defined if Irisa's Viper S850 robot available.
-#cmakedefine VISP_HAVE_VIPER850
-
-// Defined if the Aria library and (pthread, rt, dl libraries under Unix) is found.
-// These libraries are used to control Pioneer mobile robots.
-#cmakedefine VISP_HAVE_PIONEER
-
-// Defined if linux/parport.h is available for parallel port usage.
-#cmakedefine VISP_HAVE_PARPORT
-
-// Defined if libzbar is available for bar code detection
-#cmakedefine VISP_HAVE_ZBAR
-
-// Defined if libdmtx is available for bar code detection
-#cmakedefine VISP_HAVE_DMTX
-
-// Defined if Inria's NAS server hosting /udd/ is available
-// Used for the moment in vpAfma6 class to check if config files are
-// available in /udd/fspindle/robot/Afma6/current/include/
-#cmakedefine VISP_HAVE_ACCESS_TO_NAS
-
-// Defined if Doxygen documentation tool is found
-#cmakedefine VISP_HAVE_DOXYGEN
-
-// Defined if dot documentation tool used by Doxygen is found
-#cmakedefine VISP_HAVE_DOT
-
-// Defined if we want to compute interaction matrices by combining
-// other interaction matrices
-#cmakedefine VISP_MOMENTS_COMBINE_MATRICES
-
-//Defined if we want to use openmp
-#cmakedefine VISP_HAVE_OPENMP
-
-//Defined if we want to use c++ 11
-#cmakedefine VISP_HAVE_CPP11_COMPATIBILITY
-
-// Handle portable symbol export.
-// Defining manually which symbol should be exported is required
-// under Windows whether MinGW or MSVC is used.
-//
-// The headers then have to be able to work in two different modes:
-// - dllexport when one is building the library,
-// - dllimport for clients using the library.
-//
-// On Linux, set the visibility accordingly. If C++ symbol visibility
-// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
-# if defined(_WIN32) || defined(__CYGWIN__)
-// On Microsoft Windows, use dllimport and dllexport to tag symbols.
-# define VISP_DLLIMPORT __declspec(dllimport)
-# define VISP_DLLEXPORT __declspec(dllexport)
-# define VISP_DLLLOCAL
-# else
-// On Linux, for GCC >= 4, tag symbols using GCC extension.
-# if __GNUC__ >= 4
-# define VISP_DLLIMPORT __attribute__ ((visibility("default")))
-# define VISP_DLLEXPORT __attribute__ ((visibility("default")))
-# define VISP_DLLLOCAL __attribute__ ((visibility("hidden")))
-# else
-// Otherwise (GCC < 4 or another compiler is used), export everything.
-# define VISP_DLLIMPORT
-# define VISP_DLLEXPORT
-# define VISP_DLLLOCAL
-# endif // __GNUC__ >= 4
-# endif // defined(_WIN32) || defined(__CYGWIN__)
-
-// Under Windows, for shared libraries (DLL) we need to define export on
-// compilation or import on use (like a third party project).
-// We exploit here the fact that cmake auto set xxx_EXPORTS (with S) on
-// compilation.
-#if defined(VISP_BUILD_SHARED_LIBS)
-// Depending on whether one is building or using the
-// library define VISP_EXPORT to import or export.
-# ifdef visp_EXPORTS
-# define VISP_EXPORT VISP_DLLEXPORT
-# else
-# define VISP_EXPORT VISP_DLLIMPORT
-# endif
-# define VISP_LOCAL VISP_DLLLOCAL
-#else
-// If one is using the library statically, get rid of
-// extra information.
-# define VISP_EXPORT
-# define VISP_LOCAL
-#endif
-
-// Add the material to produce a warning when deprecated functions are used
-#ifndef vp_deprecated
-# if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# define vp_deprecated __attribute__((deprecated))
-# else
-# define vp_deprecated __declspec(deprecated)
-# endif
-#endif
-
-#endif
-
-
diff --git a/macros/have_visp.m4 b/macros/have_visp.m4
index 951eb3d..adf5d58 100644
--- a/macros/have_visp.m4
+++ b/macros/have_visp.m4
@@ -1,10 +1,8 @@
#############################################################################
#
-# $Id: have_visp.m4 4574 2014-01-09 08:48:51Z fspindle $
-#
# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
# This software is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# ("GPL") version 2 as published by the Free Software Foundation.
@@ -12,21 +10,20 @@
# 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
+# GPL, please contact Inria about acquiring a ViSP Professional
# Edition License.
#
-# See http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
+# See http://visp.inria.fr for more information.
+#
# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
+# Inria Rennes - Bretagne Atlantique
# Campus Universitaire de Beaulieu
# 35042 Rennes Cedex
# France
-# http://www.irisa.fr/lagadic
#
# If you have questions regarding the use of this file, please contact
-# INRIA at visp at inria.fr
-#
+# Inria at visp at 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.
#
diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt
new file mode 100644
index 0000000..1d88d57
--- /dev/null
+++ b/modules/CMakeLists.txt
@@ -0,0 +1,43 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+if(NOT VISP_MODULES_PATH)
+ set(VISP_MODULES_PATH "${CMAKE_CURRENT_SOURCE_DIR}")
+endif()
+
+# extra modules are outside ViSP. Useful for contrib or specific additional modules
+vp_glob_modules(${VISP_MODULES_PATH} EXTRA ${VISP_CONTRIB_MODULES_PATH})
diff --git a/modules/ar/CMakeLists.txt b/modules/ar/CMakeLists.txt
new file mode 100644
index 0000000..6106061
--- /dev/null
+++ b/modules/ar/CMakeLists.txt
@@ -0,0 +1,183 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+if(USE_OGRE)
+ if(NOT OGRE_BUILD_PLATFORM_IPHONE)
+ if(WIN32 OR APPLE)
+ set(Boost_USE_STATIC_LIBS TRUE)
+ else()
+ # Statically linking boost to a dynamic Ogre build doesn't work on Linux 64bit
+ set(Boost_USE_STATIC_LIBS ${OGRE_STATIC})
+ endif()
+ if(MINGW)
+ # this is probably a bug in CMake: the boost find module tries to look for
+ # boost libraries with name libboost_*, but CMake already prefixes library
+ # search names with "lib". This is the workaround.
+ set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} "")
+ endif()
+ #set(Boost_ADDITIONAL_VERSIONS "1.53.0" "1.52.0" "1.51.0" "1.50.0" "1.49.0" "1.48.0" "1.47.0" "1.46.0" "1.45.0" "1.44.0" "1.44" "1.44.0" "1.42" "1.42.0" "1.41.0" "1.41" "1.40.0" "1.40" "1.39.0" "1.39" "1.38.0" "1.38" "1.37.0" "1.37" )
+ # Components that need to be linked with. Since Ogre 1.9 we need not only boost_thread and boost_date_time, but also boost_system
+ set(OGRE_BOOST_COMPONENTS thread system date_time)
+ if(WIN32)
+ list(APPEND OGRE_BOOST_COMPONENTS chrono)
+ endif()
+ find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
+ mark_as_advanced(Boost_LIB_DIAGNOSTIC_DEFINITIONS Boost_DIR BOOST_THREAD_LIBRARY)
+ if(NOT Boost_FOUND)
+ set(OGRE_BOOST_COMPONENTS thread date_time)
+ if(WIN32)
+ list(APPEND OGRE_BOOST_COMPONENTS chrono)
+ endif()
+ find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
+ endif()
+ if(NOT Boost_FOUND)
+ # Try again with the other type of libs
+ set(Boost_USE_STATIC_LIBS NOT ${Boost_USE_STATIC_LIBS})
+ find_package(Boost COMPONENTS ${OGRE_BOOST_COMPONENTS} QUIET)
+ endif()
+
+ if(Boost_FOUND)
+ # Set up referencing of Boost
+ #LIST(APPEND VISP_DEFS "-DBOOST_ALL_NO_LIB")
+ list(APPEND opt_incs ${Boost_INCLUDE_DIR})
+ list(APPEND opt_libs ${Boost_LIBRARIES})
+ endif()
+ endif()
+
+ mark_as_advanced(OGRE_SAMPLES_INCLUDEPATH)
+ #message("OGRE_SAMPLES_INCLUDEPATH: ${OGRE_SAMPLES_INCLUDEPATH}")
+ if(OGRE_SAMPLES_INCLUDEPATH)
+ list(APPEND opt_incs ${OGRE_SAMPLES_INCLUDEPATH})
+ endif()
+
+ # hack to fix possible presence of NOTFOUND in OGRE_INCLUDE_DIRS
+ #message("OGRE_INCLUDE_DIRS: ${OGRE_INCLUDE_DIRS}")
+ foreach(inc_ ${OGRE_INCLUDE_DIRS})
+ if(NOT ${inc_} MATCHES "NOTFOUND")
+ list(APPEND opt_incs ${inc_})
+ endif()
+ endforeach()
+ list(APPEND opt_libs ${OGRE_LIBRARIES})
+endif(USE_OGRE)
+
+if(USE_OIS AND USE_OGRE)
+ list(APPEND opt_incs ${OIS_INCLUDE_DIR})
+ list(APPEND opt_libs ${OIS_LIBRARIES})
+ if(APPLE)
+ # With Ogre 1.7.4 and 1.8.1 to be able to link with libOIS.a, Cocoa framework is requested.
+ # This is a work around since it should come with FindOGRE.cmake
+ list(APPEND opt_libs "-framework Cocoa")
+ endif()
+endif()
+
+if(USE_COIN3D)
+ if(WIN32)
+ add_definitions("-DCOIN_DLL")
+ endif()
+ list(APPEND opt_incs ${COIN3D_INCLUDE_DIRS})
+ # On OSX cmake 2.8 found OpenGL but OPENGL_INCLUDE_DIR was set to NOT_FOUND
+ # We add a test to be sure that the OPENGL vars exist.
+ if(OPENGL_INCLUDE_DIR)
+ list(APPEND opt_incs ${OPENGL_INCLUDE_DIR})
+ endif()
+ if(OPENGL_LIBRARIES)
+ list(APPEND opt_libs ${OPENGL_LIBRARIES})
+ endif()
+
+ list(APPEND opt_libs ${COIN3D_LIBRARIES})
+ if(USE_SOWIN)
+ add_definitions("-DSOWIN_DLL")
+ list(APPEND opt_incs ${SOWIN_INCLUDE_DIRS})
+ list(APPEND opt_libs ${SOWIN_LIBRARIES})
+ endif()
+
+ if(USE_SOQT AND USE_QT)
+ list(APPEND opt_incs ${SOQT_INCLUDE_DIRS})
+ list(APPEND opt_incs ${QT_INCLUDE_DIR})
+ list(APPEND opt_incs ${QT_INCLUDES})
+ list(APPEND opt_libs ${SOQT_LIBRARIES})
+ if(WIN32)
+ add_definitions("-DSOQT_DLL")
+ endif()
+
+ # We manage QT libraries
+ if(DESIRED_QT_VERSION MATCHES 3)
+ #Add Qt3 libraries
+ set(VISP_HAVE_QT3_FOUND "yes") # for ViSP-third-party.txt
+ #message("QT_QT_LIBRARY ${QT_QT_LIBRARY}")
+ list(APPEND opt_libs ${QT_QT_LIBRARY})
+ elseif(DESIRED_QT_VERSION MATCHES 4)
+ #Add Qt4 libraries
+ set(VISP_HAVE_QT4_FOUND "yes") # for ViSP-third-party.txt
+ #message("QT_QTGUI_LIBRARY ${QT_QTGUI_LIBRARY}")
+ #message("QT_QTGUI_LIBRARY_RELEASE ${QT_QTGUI_LIBRARY_RELEASE}")
+ #message("QT_QTGUI_LIBRARY_DEBUG ${QT_QTGUI_LIBRARY_DEBUG}")
+ if(QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE AND QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
+ list(APPEND opt_libs optimized ${QT_QTGUI_LIBRARY_RELEASE})
+ list(APPEND opt_libs optimized ${QT_QTCORE_LIBRARY_RELEASE})
+ list(APPEND opt_libs debug ${QT_QTGUI_LIBRARY_DEBUG})
+ list(APPEND opt_libs debug ${QT_QTCORE_LIBRARY_DEBUG})
+ elseif(QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE)
+ list(APPEND opt_libs ${QT_QTGUI_LIBRARY_RELEASE})
+ list(APPEND opt_libs ${QT_QTCORE_LIBRARY_RELEASE})
+ elseif(QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
+ list(APPEND opt_libs ${QT_QTGUI_LIBRARY_DEBUG})
+ list(APPEND opt_libs ${QT_QTCORE_LIBRARY_DEBUG})
+ endif()
+ endif()
+
+ # Because in QT_DEFINITIONS defs are separated by ";", parse the
+ # QT_DEFINITIONS in order to build a space separated string
+ vp_list_remove_separator(QT_DEFINITIONS)
+ add_definitions(${QT_DEFINITIONS})
+ add_definitions("-DQT_DLL")
+ endif(USE_SOQT AND USE_QT)
+
+ if(USE_SOXT)
+ # OpenGL and SoXt are found
+ list(APPEND opt_libs ${SOXT_LIBRARIES})
+ endif()
+endif(USE_COIN3D)
+
+vp_add_module(ar visp_core OPTIONAL visp_io)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
diff --git a/data/ogre-simulator/media/materials/scripts/Examples.material b/modules/ar/data/ogre-simulator/media/materials/scripts/Examples.material
similarity index 100%
rename from data/ogre-simulator/media/materials/scripts/Examples.material
rename to modules/ar/data/ogre-simulator/media/materials/scripts/Examples.material
diff --git a/data/ogre-simulator/media/materials/textures/grass_1024.jpg b/modules/ar/data/ogre-simulator/media/materials/textures/grass_1024.jpg
similarity index 100%
rename from data/ogre-simulator/media/materials/textures/grass_1024.jpg
rename to modules/ar/data/ogre-simulator/media/materials/textures/grass_1024.jpg
diff --git a/data/ogre-simulator/media/materials/textures/r2skin.jpg b/modules/ar/data/ogre-simulator/media/materials/textures/r2skin.jpg
similarity index 100%
rename from data/ogre-simulator/media/materials/textures/r2skin.jpg
rename to modules/ar/data/ogre-simulator/media/materials/textures/r2skin.jpg
diff --git a/data/ogre-simulator/media/models/robot.mesh b/modules/ar/data/ogre-simulator/media/models/robot.mesh
similarity index 100%
rename from data/ogre-simulator/media/models/robot.mesh
rename to modules/ar/data/ogre-simulator/media/models/robot.mesh
diff --git a/data/ogre-simulator/media/models/robot.skeleton b/modules/ar/data/ogre-simulator/media/models/robot.skeleton
similarity index 100%
rename from data/ogre-simulator/media/models/robot.skeleton
rename to modules/ar/data/ogre-simulator/media/models/robot.skeleton
diff --git a/modules/ar/include/visp3/ar/vpAR.h b/modules/ar/include/visp3/ar/vpAR.h
new file mode 100644
index 0000000..4618b65
--- /dev/null
+++ b/modules/ar/include/visp3/ar/vpAR.h
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Use to display an image behind the internal view of the simulator
+ * used for augmented reality application
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpAR.h
+
+ \brief Class used to display an image behind the internal view of
+ the simulator. Used for augmented reality applications.
+
+ \warning The content of this file is only available if Coin3D and
+ one of the GUI (SoWin, SoXT, SoQt) are installed.
+
+*/
+
+
+#ifndef vpAR_HH
+#define vpAR_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+
+// visp
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <visp3/ar/vpSimulator.h>
+
+#include <visp3/ar/vpViewer.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpTime.h>
+
+/*!
+ \class vpAR
+
+ \ingroup group_ar_renderer
+
+ \brief Implementation of an augmented reality viewer.
+
+ This class can be used to display an image behind the internal view
+ of the simulator used for augmented reality application.
+
+ \warning This class is only available if Coin3D and one of the GUI
+ (SoWin, SoXT, SoQt) are installed.
+
+ The code below shows how to use the class.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/ar/vpAR.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+static void *mainloopfunction(void *_simu)
+{
+ vpAR *simu = (vpAR *)_simu ;
+ simu->initMainApplication() ;
+
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo;
+
+ //Your code to compute the pose cMo.
+
+ //Set the image to use as background.
+ simu->setImage(I) ;
+ //Set the camera position thanks to the pose cMo computed before.
+ simu->setCameraPosition(cMo) ;
+
+ simu->closeMainApplication();
+}
+#endif
+
+int main()
+{
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+ vpAR simu;
+ //Camera parameters.
+ vpCameraParameters cam(600,600,160,120);
+
+ //Initialize the internal view of the simulator.
+ simu.initInternalViewer(640,480, vpSimulator::grayImage);
+
+ vpTime::wait(300);
+
+ //Load the cad model.
+ simu.load("./4points.iv"); //4points.iv can be downloaded on the website with the image package
+
+ //Initialize the internal camera parameters.
+ simu.setInternalCameraParameters(cam);
+
+ simu.initApplication(&mainloopfunction);
+
+ simu.mainLoop();
+#endif
+ return 0;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpAR : public vpSimulator
+{
+
+private:
+
+ bool background;
+
+ public:
+
+
+ virtual ~vpAR() ;
+ void initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type = grayImage) ;
+ void setImage(vpImage<unsigned char> &I) ;
+ void setImage(vpImage<vpRGBa> &I) ;
+
+} ;
+
+
+#endif
+#endif
diff --git a/modules/ar/include/visp3/ar/vpAROgre.h b/modules/ar/include/visp3/ar/vpAROgre.h
new file mode 100644
index 0000000..edda44a
--- /dev/null
+++ b/modules/ar/include/visp3/ar/vpAROgre.h
@@ -0,0 +1,392 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Augmented Reality viewer using Ogre3D.
+ *
+ * Authors:
+ * Bertrand Delabarre
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpAROgre.h
+
+ \brief Class that implements augmented reality viewer based on Ogre3D.
+
+ \warning The content of this file is only available if Ogre3D and
+ one of the renderer (OpenGL or DirectX) are installed.
+
+*/
+
+#ifndef __VP_AROGRE__
+#define __VP_AROGRE__
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_OGRE
+#include <list>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+
+#include <Ogre.h>
+#include <OgreFrameListener.h>
+
+#ifdef VISP_HAVE_OIS
+# include <OIS.h>
+#endif
+
+/*!
+ \class vpAROgre
+
+ \ingroup group_ar_renderer
+
+ \brief Implementation of an augmented reality viewer.
+
+ This class provides methods to show a 3D scene in a real world. To do that
+ you will need to initialise it with the parameters of your camera, then each
+ frame you will need to compute a pose for your camera and give it to the
+ application. With that information and the image to be shown in background
+ it will set up the 3D scene correspondingly.
+
+*/
+class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEventListener
+#ifdef VISP_HAVE_OIS
+ , public OIS::KeyListener
+#endif
+{
+ public:
+ vpAROgre(const vpCameraParameters &cam = vpCameraParameters(),
+ unsigned int width = 0, unsigned int height = 0,
+ const char* resourcePath =
+#ifdef VISP_HAVE_OGRE_RESOURCES_PATH
+ VISP_HAVE_OGRE_RESOURCES_PATH,
+#else
+ ".",
+#endif
+ const char* pluginsPath =
+#ifdef VISP_HAVE_OGRE_PLUGINS_PATH
+ VISP_HAVE_OGRE_PLUGINS_PATH
+#else
+ "."
+#endif
+ );
+
+ virtual ~vpAROgre(void);
+
+ /*!
+ Add optional resource location. Since a resource file cannot be always
+ sufficient to manage multiple location media (depending on the computer and
+ the executable path), this method may be used to add such paths.
+
+ \warning To be effective, this method must be called before the init() one.
+
+ \param resourceLocation : The resource location (it may be a folder or a zip
+ file).
+ */
+ inline void addResource(const std::string& resourceLocation){
+ mOptionnalResourceLocation.push_back(resourceLocation);
+ }
+
+ void addRotation(const std::string &sceneName, const vpRotationMatrix &wRo);
+
+ bool continueRendering(void);
+
+ virtual bool customframeStarted(const Ogre::FrameEvent& evt);
+
+ virtual bool customframeEnded(const Ogre::FrameEvent& evt);
+
+ virtual void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMw);
+
+ virtual void display(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMw);
+
+ inline Ogre::Camera* getCamera(){ return mCamera; }
+
+ /*!
+ Get the far distance for clipping.
+
+ \return Far clipping value.
+ */
+ inline double getFarClippingDistance() const { return mFarClipping; }
+
+ /*!
+ Get the near distance for clipping.
+
+ \return Near clipping value.
+ */
+ inline double getNearClippingDistance() const { return mNearClipping; }
+
+ vpTranslationVector getPosition(const std::string &sceneName)const;
+
+ void getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo);
+
+ inline Ogre::SceneManager* getSceneManager(){ return mSceneMgr; }
+
+ virtual void init(vpImage<unsigned char> &I, bool bufferedKeys=false, bool hidden=false);
+ virtual void init(vpImage<vpRGBa> &I, bool bufferedKeys=false, bool hidden=false);
+
+ /*!
+ Test if the window is hidden or not.
+
+ \warning True if the window is hidden, false otherwise.
+ */
+ bool isWindowHidden(){ return windowHidden; }
+
+#ifdef VISP_HAVE_OIS
+ /**
+ * Default event handler
+ */
+ virtual bool keyPressed( const OIS::KeyEvent & /*e*/) { return true; }
+ /**
+ * Default event handler
+ */
+ virtual bool keyReleased( const OIS::KeyEvent & /*e*/) { return true; }
+#endif
+
+ void load(const std::string &entityName, const std::string &model);
+
+ bool renderOneFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw);
+
+ bool renderOneFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw);
+
+ void setCameraParameters(const vpCameraParameters &cameraP);
+
+ /*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+ */
+ void setFarClippingDistance(const double &dist){
+ mFarClipping = dist;
+ updateCameraProjection();
+ }
+
+ /*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+ */
+ void setNearClippingDistance(const double &dist){
+ mNearClipping = dist;
+ updateCameraProjection();
+ }
+
+ /*!
+ Set the plugins path used to locate the plugins.cfg file.
+
+ By default, this path is set to VISP_HAVE_OGRE_PLUGINS_PATH defined in vpConfig.h
+
+ \warning To be effective, this method must be called before the init() one.
+
+ \param pluginsPath : The new plugins path (must not have a terminate /).
+ */
+ inline void setPluginsPath(const char* pluginsPath){
+ mPluginsPath = pluginsPath;
+ }
+
+ void setPosition(const std::string &sceneName, const vpTranslationVector &wTo);
+ void setPosition(const std::string &sceneName, const vpHomogeneousMatrix &wMo);
+
+ /*!
+ Set the resource path used to locate the resource.cfg file.
+
+ By default, this path is set to VISP_HAVE_OGRE_RESOURCES_PATH defined in vpConfig.h
+
+ \warning To be effective, this method must be called before the init() one.
+
+ \param resourcePath : The new resource path (must not have a terminate /).
+ */
+ inline void setResourcePath(const char* resourcePath){
+ mResourcePath = resourcePath;
+ }
+
+ void setRotation(const std::string &sceneName, const vpRotationMatrix &wRo);
+
+ void setScale(const std::string &sceneName, const float factorx, const float factory, const float factorz);
+
+ /*!
+ Enable/Disable the appearance of the config dialog on startup.
+
+ \warning To be effective, this method must be called before the init() one.
+
+ \param showConfigDialog : if true, shows the dialog window (used to set the
+ display options)
+ */
+ inline void setShowConfigDialog(const bool showConfigDialog){
+ mshowConfigDialog = showConfigDialog;
+ }
+
+ void setVisibility(const std::string &sceneName, bool isVisible);
+
+ /*!
+ Set the name of the window.
+
+ \warning Has to be called before initialisation.
+
+ \param n : Name of the window.
+ */
+ inline void setWindowName(const Ogre::String &n){
+ name = n;
+ }
+
+ /*!
+ Set the window position in the screen.
+
+ \throw vpException::notInitialised if the window has not been created (using
+ the init() method).
+
+ \param win_x : x coordinate of the new top left corner of the window in the
+ screen.
+ \param win_y : y coordinate of the new top left corner of the window in the
+ screen.
+ */
+ inline void setWindowPosition(const unsigned int win_x, const unsigned int win_y){
+ if(mWindow == NULL){
+ throw vpException(vpException::notInitialized, "Window not initialised, cannot set its position");
+ }
+ mWindow->reposition(static_cast<int>(win_x), static_cast<int>(win_y));
+ }
+
+ virtual void windowClosed(Ogre::RenderWindow* rw);
+
+ protected:
+
+ virtual void init(bool bufferedKeys=false, bool hidden=false);
+ virtual void createCamera(void);
+
+ /**
+ * Build the 3D scene
+ * Override this to show what you want
+ */
+ virtual void createScene(void){};
+
+ virtual void closeOIS(void);
+
+ /*!
+ Update the 3D scene
+
+ \return Always true.
+ */
+ virtual bool updateScene(const Ogre::FrameEvent & /*evt*/) {return true;};
+
+ /*!
+ Check for keyboard, mouse and joystick inputs.
+
+ \return Always true.
+ */
+ virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) {return true;};
+
+ /*!
+ Clean up the 3D scene.
+
+ \return Always true.
+ */
+ virtual bool destroyScene(void) {
+ if(!mSceneMgr)
+ return false;
+
+ mSceneMgr->destroyAllCameras ();
+ mSceneMgr->clearScene();
+ mRoot->destroySceneManager(mSceneMgr);
+ return true;
+ }
+
+ virtual void updateCameraParameters (const vpHomogeneousMatrix &cMo);
+
+ virtual void updateCameraProjection(void);
+
+ virtual void updateBackgroundTexture(const vpImage<unsigned char> &I);
+
+ virtual void updateBackgroundTexture(const vpImage<vpRGBa> &I);
+
+
+ private:
+
+ void createBackground(vpImage<unsigned char> &I);
+ void createBackground(vpImage<vpRGBa> &I);
+
+ bool frameStarted(const Ogre::FrameEvent& evt);
+
+ bool frameEnded( const Ogre::FrameEvent& evt);
+
+ bool stopTest(const Ogre::FrameEvent& evt);
+
+ protected:
+ // Attributes
+ Ogre::String name; /**Name of th Window*/
+
+ // OGRE 3D System
+ Ogre::Root* mRoot; /** Application's root */
+ Ogre::Camera* mCamera; /** Camera */
+ Ogre::SceneManager* mSceneMgr; /** Scene manager */
+ Ogre::RenderWindow* mWindow; /** Display window */
+ Ogre::String mResourcePath; /** Path to resources.cfg */
+ Ogre::String mPluginsPath; /** Path to plugins.cfg */
+
+#ifdef VISP_HAVE_OIS
+ // OIS Input manager and devices
+ OIS::InputManager* mInputManager;
+ OIS::Keyboard* mKeyboard;
+#endif
+
+ // ViSP AR System
+ bool keepOn; /** Has the application received a signal to stop(false) or not (true) */
+ vpImage<vpRGBa> mImageRGBA; /** vpImage to store grabbed image */
+ vpImage<unsigned char> mImage; /** vpImage to store grabbed image */
+ Ogre::HardwarePixelBufferSharedPtr mPixelBuffer; /** Pointer to the pixel buffer */
+ Ogre::Rectangle2D* mBackground; /** Background image */
+ unsigned int mBackgroundHeight; /** Height of the acquired image */
+ unsigned int mBackgroundWidth; /** Width of the acquired image */
+ unsigned int mWindowHeight; /** Height of the window */
+ unsigned int mWindowWidth; /** Width of the window */
+ bool windowHidden; /** Is window hidden */
+
+ // Camera calculations
+ double mNearClipping; /** Near Clipping Distance **/
+ double mFarClipping; /** Far Clipping Distance **/
+ vpCameraParameters mcam; /** The intrinsic camera parameters */
+
+ bool mshowConfigDialog; /** if true, shows the dialog window (used to set the display options) */
+
+ std::list<std::string> mOptionnalResourceLocation; /** Optional resource location (used to load mesh and material) */
+
+};
+
+#endif // VISP_HAVE_OGRE
+
+#endif
diff --git a/modules/ar/include/visp3/ar/vpSimulator.h b/modules/ar/include/visp3/ar/vpSimulator.h
new file mode 100644
index 0000000..6a16b72
--- /dev/null
+++ b/modules/ar/include/visp3/ar/vpSimulator.h
@@ -0,0 +1,305 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulator based on Coin3d.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#ifndef vpSimulator_HH
+#define vpSimulator_HH
+/*!
+ \file vpSimulator.h
+ \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
+ The simulator uses the vpViewer class.
+
+ \warning The content of this file is only available if Coin3D and
+ one of the GUI (SoWin, SoXT, SoQt) are installed.
+*/
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+
+#include <visp3/ar/vpViewer.h>
+
+/* KNOWN ISSUE DEALING WITH X11 and QT
+ If you get a strange compiler error on the line with None,
+ it's probably because you're also including X11 headers,
+ which #define the symbol None. Put the X11 includes after
+ the Qt includes to solve this problem.
+ */
+
+#include <Inventor/nodes/SoBaseColor.h>
+#include <Inventor/nodes/SoTransform.h>
+#include <Inventor/nodes/SoCone.h>
+#include <Inventor/nodes/SoCube.h>
+#include <Inventor/nodes/SoImage.h>
+#include <Inventor/nodes/SoLightModel.h>
+#include <Inventor/nodes/SoPerspectiveCamera.h>
+#include <Inventor/nodes/SoRotationXYZ.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/SoOffscreenRenderer.h>
+#include <Inventor/fields/SoSFTime.h>
+#include <Inventor/threads/SbThread.h>
+
+// visp
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpImageConvert.h>
+
+/*!
+ \class vpSimulator
+
+ \ingroup group_ar_simulator
+
+ \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
+
+ The simulator uses the vpViewer class.
+
+ \warning This class is only available if Coin3D and one of the GUI
+ (SoWin, SoXT, SoQt) are installed.
+*/
+
+class VISP_EXPORT vpSimulator
+{
+protected:
+ //! perform some initialization
+ void init() ;
+ //! perform some destruction
+ void kill() ;
+
+public:
+ //! constructor
+ vpSimulator() ;
+ virtual ~vpSimulator() ;
+
+protected:
+ //! main Widget
+#if defined(VISP_HAVE_SOWIN)
+ HWND mainWindow ;
+#elif defined(VISP_HAVE_SOQT)
+ QWidget * mainWindow ;
+#elif defined(VISP_HAVE_SOXT)
+ Widget mainWindow ;
+#endif
+
+ bool mainWindowInitialized ;
+
+ //! open the SoGui application
+ void initSoApplication() ;
+
+public:
+ typedef enum {
+ grayImage,
+ colorImage
+ } vpImageType ;
+ vpImageType typeImage ;
+
+ GLubyte * image_background;
+ //! activate the mainloop
+ virtual void mainLoop() ;
+
+protected:
+ //! view from the camera
+ vpViewer *internalView ;
+ //! view from an external camera
+ vpViewer *externalView ;
+
+public:
+ //! initialize the camera view
+ virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol) ;
+ //! initialize the external view
+ void initExternalViewer(const unsigned int nlig, const unsigned int ncol) ;
+
+protected:
+ //! thread with the main program
+ SbThread * mainThread;
+ //pthread_t mainThread;
+
+public:
+ //! begin the main program
+ void initApplication(void *(*start_routine)(void *)) ;
+ void initApplication(void *(*start_routine)(void *), void* data);
+ //! perform some initialization in the main program thread
+ void initMainApplication() ;
+ void closeMainApplication() ;
+
+ //----------------------------------------------------
+ // scene description
+protected:
+ unsigned int internal_width;
+ unsigned int internal_height;
+ unsigned int external_width;
+ unsigned int external_height;
+
+public:
+ /*!
+ Return the width of the internal view.
+
+ \return The width of the internal view.
+ */
+ unsigned int getInternalWidth() const { return internal_width; }
+ /*!
+ Return the height of the internal view.
+
+ \return The height of the internal view.
+ */
+ unsigned int getInternalHeight() const { return internal_height; }
+
+protected:
+ //! root node of the scene : contains everything except stuff specific to
+ //! each view
+ SoSeparator *scene;
+ //! root node of the internal view
+ SoSeparator *internalRoot;
+ //! root node of the external view
+ SoSeparator *externalRoot;
+
+ //! internal camera
+ SoPerspectiveCamera *internalCamera ;
+ //! external camera
+ SoPerspectiveCamera *externalCamera ;
+
+ //! internal camera position
+ SoTransform *internalCameraPosition ;
+
+ //! external camera position
+ SoTransform *extrenalCameraPosition ;
+
+ //! representation of the camera in the external view
+ SoSeparator *internalCameraObject;
+
+ //! initialize the scene graph
+ void initSceneGraph() ;
+
+ //!Add a new object in the scene graph ad a given location
+ void addObject(SoSeparator * object, const vpHomogeneousMatrix &fMo,
+ SoSeparator * root) ;
+
+public :
+ //!Add a new object in the scene graph ad a given location
+ void addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo) ;
+
+public:
+ //! display the scene (handle with care)
+ void redraw() ;
+ //! load an iv file
+ void load(const char *file_name) ;
+ //! load an iv file, set the location of this scene
+ void load(const char *iv_filename,const vpHomogeneousMatrix &fMo) ;
+ //! save the scene in an iv file
+ void save(const char *name,bool binary=false) ;
+
+ //! Add the representation of the absolute frame
+ void addAbsoluteFrame (float zoom=1) ;
+ //! Add the representation of a frame
+ void addFrame (const vpHomogeneousMatrix &fMo, float zoom=1) ;
+ //! set the size of the camera/frame
+ void setZoomFactor (const float zoom) ;
+
+protected:
+ float zoomFactor ;
+ //---------------------------------------------------
+ // camera description
+protected:
+ bool cameraPositionInitialized ;
+ //! internal camera position
+ vpHomogeneousMatrix cMf ;
+ //! internal camera parameters
+ vpCameraParameters internalCameraParameters ;
+ //! internal camera parameters
+ vpCameraParameters externalCameraParameters ;
+
+public:
+ //! set the camera position (from an homogeneous matrix)
+ void setCameraPosition(vpHomogeneousMatrix &cMf) ;
+ //! get the camera position (from an homogeneous matrix)
+ void getCameraPosition(vpHomogeneousMatrix &_cMf) { _cMf = cMf ; }
+ //! modify the position of the camera in the scene graph
+ void moveInternalCamera(vpHomogeneousMatrix &cMf) ;
+ //! set internal camera parameters
+ void setInternalCameraParameters(vpCameraParameters &cam) ;
+ //! set external camera parameters
+ void setExternalCameraParameters(vpCameraParameters &cam) ;
+ //! get the external camera position
+ void getExternalCameraPosition(vpHomogeneousMatrix &cMf) ;
+
+ //! get an Image of the internal view
+ void getInternalImage(vpImage<unsigned char> &I) ;
+ //! get an Image of the internal view
+ void getInternalImage(vpImage<vpRGBa> &I) ;
+ /* --- Off screen rendering --- */
+
+ void changeZoomFactor(const float zoom, const int index);
+public:
+ typedef enum { INTERNAL, EXTERNAL } vpSimulatorViewType ;
+#ifdef VISP_HAVE_MODULE_IO
+ void write(const char * fileName);
+#endif
+protected:
+ SbTime * realtime ;
+ SoOffscreenRenderer * offScreenRenderer ;
+ void offScreenRendering (vpSimulatorViewType view=vpSimulator::EXTERNAL,
+ int * width = NULL,
+ int * height = NULL);
+public:
+ //! image of the internal view
+ unsigned char * bufferView;
+
+ //! Flag to protect the read and write of the framebuffer (between the simulator and the viewer).
+ int get ;
+public:
+ //! get the image corresponding to the internal view
+ unsigned char* getBufferedOffScreenRenderer()
+ {
+ return bufferView;
+ }
+
+ //! get the size of the internal view
+ void getSizeInternalView(int& width, int& height);
+
+ //! get the intrinsic parameters of the camera
+ void getCameraParameters(vpCameraParameters &cam)
+ {
+ cam = internalCameraParameters;
+ }
+} ;
+
+
+#endif
+#endif
diff --git a/modules/ar/include/visp3/ar/vpSimulatorException.h b/modules/ar/include/visp3/ar/vpSimulatorException.h
new file mode 100644
index 0000000..d391bfc
--- /dev/null
+++ b/modules/ar/include/visp3/ar/vpSimulatorException.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the simulator classes.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpSimulatorException_H
+#define __vpSimulatorException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpSimulatorException.h
+ \brief error that can be emited by the vpSimulator class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+
+ \class vpSimulatorException
+ \brief Error that can be emited by the vpSimulator class and its derivates.
+ */
+class VISP_EXPORT vpSimulatorException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpSimulator member
+ */
+ enum errorSimulatorCodeEnum
+ {
+ ioError,
+ noFileNameError,
+ notInitializedError,
+ windowSizeNotInitializedError,
+ badInitializationError
+ } ;
+
+ public:
+ vpSimulatorException (const int id, const char* format, ...);
+ vpSimulatorException (const int id, const std::string & msg);
+ vpSimulatorException (const int id);
+};
+
+#endif
diff --git a/modules/ar/include/visp3/ar/vpViewer.h b/modules/ar/include/visp3/ar/vpViewer.h
new file mode 100644
index 0000000..2dc64c9
--- /dev/null
+++ b/modules/ar/include/visp3/ar/vpViewer.h
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulator based on Coin3d.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpViewer_HH
+#define vpViewer_HH
+/*!
+ \file vpViewer.h
+
+ Viewer used by the simulator. Under Windows, the viewer is based
+ either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or
+ SoXt.
+
+ \warning The content of this file is only available if Coin3D and
+ one of the GUI (SoWin, SoXT, SoQt) are installed.
+
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+
+#if defined(VISP_HAVE_SOWIN)
+
+ #include <Inventor/Win/SoWin.h>
+ #include <Inventor/Win/viewers/SoWinExaminerViewer.h>
+
+#elif defined(VISP_HAVE_SOQT)
+
+ #include <Inventor/Qt/SoQt.h>
+ #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+
+#elif defined(VISP_HAVE_SOXT)
+
+ #include <Inventor/Xt/SoXt.h>
+ #include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
+
+#endif
+
+// Coin stuff
+
+#include <Inventor/nodes/SoBaseColor.h>
+#include <Inventor/nodes/SoCone.h>
+#include <Inventor/nodes/SoCube.h>
+#include <Inventor/nodes/SoImage.h>
+#include <Inventor/nodes/SoLightModel.h>
+#include <Inventor/nodes/SoPerspectiveCamera.h>
+#include <Inventor/nodes/SoRotationXYZ.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/sensors/SoTimerSensor.h>
+
+#if defined(VISP_HAVE_SOWIN)
+#include <GL/gl.h>
+#elif defined(VISP_HAVE_SOQT)
+#include <qgl.h>
+#elif defined(VISP_HAVE_SOXT)
+#include <GL/gl.h>
+#endif
+
+// visp
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+
+/*!
+ \class vpViewer
+ \ingroup group_ar_simulator
+
+ \brief Viewer used by the simulator.
+
+ Under Windows, the viewer is based either on SoWin or SoQt. Under
+ Unix, the viewer is based on SoQt or SoXt.
+
+ \warning This class is only available if Coin3D and one of the GUI
+ (SoWin, SoXT, SoQt) are installed.
+
+*/
+
+class vpSimulator;
+
+#if defined(VISP_HAVE_SOWIN)
+class VISP_EXPORT vpViewer : public SoWinExaminerViewer
+#elif defined(VISP_HAVE_SOQT)
+class VISP_EXPORT vpViewer : public SoQtExaminerViewer
+#elif defined(VISP_HAVE_SOXT)
+class VISP_EXPORT vpViewer : public SoXtExaminerViewer
+#endif
+{
+
+ friend class vpSimulator ;
+
+public:
+ typedef enum{
+ internalView,
+ externalView
+ } vpViewerType ;
+#if defined(VISP_HAVE_SOWIN)
+ vpViewer(HWND parent, vpSimulator *simu,vpViewerType viewerType);
+#elif defined(VISP_HAVE_SOQT)
+ vpViewer(QWidget * parent, vpSimulator *simu,vpViewerType viewerType);
+#elif defined(VISP_HAVE_SOXT)
+ vpViewer(Widget parent, vpSimulator *simu,vpViewerType viewerType);
+#endif
+
+ virtual ~vpViewer();
+ void resize(int x, int y, bool fixed = false) ;
+ virtual void actualRedraw(void);
+
+private:
+
+ vpViewerType viewerType;
+ vpSimulator *simu ;
+ SbBool processSoEvent(const SoEvent * const event) ;
+#if defined(VISP_HAVE_SOWIN)
+ static HWND init(const char * appname) {return SoWin::init(appname);};
+ static void mainLoop() {SoWin::mainLoop();};
+ static void exitMainLoop() {SoWin::exitMainLoop();};
+#elif defined(VISP_HAVE_SOQT)
+ static QWidget * init(const char * appname) {return SoQt::init(appname);};
+ static void mainLoop() { SoQt::mainLoop();};
+ static void exitMainLoop() {SoQt::exitMainLoop();};
+#elif defined(VISP_HAVE_SOXT)
+ static Widget init(const char * appname) {return SoXt::init(appname);};
+ static void mainLoop() { SoXt::mainLoop();};
+ static void exitMainLoop() {SoXt::exitMainLoop();};
+#endif
+};
+
+#endif //VISP_HAVE_COIN3D_AND_GUI
+
+#endif
diff --git a/modules/ar/src/coin-simulator/vpAR.cpp b/modules/ar/src/coin-simulator/vpAR.cpp
new file mode 100644
index 0000000..9241332
--- /dev/null
+++ b/modules/ar/src/coin-simulator/vpAR.cpp
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Use to display an image behind the internal view of the simulator
+ * used for augmented reality application
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpAR.cpp
+ \brief class used to display an image behind the internal view of the simulator. Used for augmented reality applications.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+
+#include <visp3/ar/vpAR.h>
+#include <visp3/core/vpTime.h>
+
+
+/* Objets OIV. */
+#include <Inventor/nodes/SoCone.h> /* Objet cone. */
+#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
+#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
+#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
+#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
+#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
+#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+
+#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
+#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
+#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
+#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
+#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
+#include <Inventor/actions/SoWriteAction.h>
+
+
+
+/*!
+ Basic Destructor that calls the kill() method of the vpSimulator class.
+*/
+vpAR::~vpAR()
+{
+ kill() ;
+}
+
+
+/*!
+ Initialisation of the internal view of the simulator.
+
+ \param width : Width of the internal view.
+ \param height : Height of the internal view.
+ \param type : Type of background image ie gray scaled or color.
+*/
+void
+vpAR::initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type)
+{
+
+ vpSimulator::initInternalViewer(width,height) ;
+
+
+ // no image is loaded
+ background = false ;
+
+ if ( image_background != NULL)
+ {
+ free(image_background) ;
+ image_background = NULL ;
+ }
+
+ typeImage = type;
+ if (typeImage == grayImage)
+ image_background =(GLubyte *)
+ malloc(internal_width*internal_height*sizeof(GLubyte)) ;
+ else
+ image_background =(GLubyte *)
+ malloc(3*internal_width*internal_height*sizeof(GLubyte)) ;
+
+}
+
+
+
+/*!
+ Set the background image and turn it to deal with the frame of OpenGL.
+
+ \param I : Gray scaled image for the background.
+*/
+// Grey pictures SetBackGroundImage
+void
+vpAR::setImage(vpImage<unsigned char> &I)
+{
+
+ if ((internal_width != I.getWidth()) ||
+ (internal_height != I.getHeight()))
+ {
+ vpERROR_TRACE("The image size is different from the view size ");
+ throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
+ }
+
+
+ background = true ;
+
+
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ //le repere image open GL est en bas a gauche donc l'image serait inverse
+ image_background[i*I.getWidth()+j] = I[I.getHeight()-i-1][j] ;
+
+}
+
+
+
+/*!
+ Set the background image and turn it to deal with the frame of OpenGL.
+
+ \param I : Color image for the background.
+*/
+// Color pictures SetBackGroundImage
+void
+vpAR::setImage(vpImage<vpRGBa> &I)
+{
+
+ if ((internal_width != I.getWidth()) ||
+ (internal_height != I.getHeight()))
+ {
+ vpERROR_TRACE("The image size is different from the view size ");
+ throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
+ }
+
+
+ background = true ;
+
+ unsigned int k =0 ;
+ for (unsigned int i=0 ; i <I.getHeight() ; i++)
+ {
+ k=0;
+ for (unsigned int j=0 ; j <I.getWidth() ; j++)
+ //le repere image open GL est en bas a gauche donc l'image serait inverse
+ {
+ image_background[i*I.getWidth()*3+k+0]=I[I.getHeight()-i-1][j].R ;
+ image_background[i*I.getWidth()*3+k+1]=I[I.getHeight()-i-1][j].G ;
+ image_background[i*I.getWidth()*3+k+2]=I[I.getHeight()-i-1][j].B ;
+ k+=3;
+ }
+ }
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_ar.a(vpAR.cpp.o) has no symbols
+void dummy_vpAR() {};
+#endif
diff --git a/modules/ar/src/coin-simulator/vpSimulator.cpp b/modules/ar/src/coin-simulator/vpSimulator.cpp
new file mode 100644
index 0000000..776252b
--- /dev/null
+++ b/modules/ar/src/coin-simulator/vpSimulator.cpp
@@ -0,0 +1,1099 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulator based on Coin3d.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+/*!
+ \file vpSimulator.cpp
+ \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
+ The simulator uses the vpViewer class.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+
+#include <visp3/ar/vpSimulator.h>
+#include <visp3/core/vpTime.h>
+
+#include <visp3/core/vpImage.h>
+
+#ifdef VISP_HAVE_MODULE_IO
+# include <visp3/io/vpImageIo.h>
+#endif
+
+/* Objets OIV. */
+#include <Inventor/nodes/SoCone.h> /* Objet cone. */
+#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
+#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
+#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
+#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
+#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
+#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+
+#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
+#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
+#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
+#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
+#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
+#include <Inventor/actions/SoWriteAction.h>
+
+
+
+
+// Positions of all of the vertices:
+//
+static float pyramidVertexes [5][3] =
+ {
+ {0.33f, 0.33f, 0.f},
+ {-0.33f, 0.33f, 0.f},
+ {-0.33f, -0.33f, 0.f},
+ {0.33f, -0.33f, 0.f},
+
+ {0.f, 0.f, -1.0f}
+ };
+
+
+static int32_t pyramidFaces[] =
+ {
+ 0, 1, 2, 3, SO_END_FACE_INDEX, // top face
+
+ 0, 1, 4, SO_END_FACE_INDEX, // 4 faces about top
+ 1, 2, 4, SO_END_FACE_INDEX,
+ 2, 3, 4, SO_END_FACE_INDEX,
+ 3, 0, 4, SO_END_FACE_INDEX,
+ };
+
+
+// Routine to create a scene graph representing a dodecahedron
+SoSeparator *
+makePyramide()
+{
+ SoSeparator *result = new SoSeparator;
+ result->ref();
+
+ // Define coordinates for vertices
+ SoCoordinate3 *myCoords = new SoCoordinate3;
+ myCoords->point.setValues(0, 5, pyramidVertexes);
+ result->addChild(myCoords);
+
+ // Define the IndexedFaceSet, with indices into the vertices:
+ SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
+ myFaceSet->coordIndex.setValues (0, 21, (const int32_t*)pyramidFaces);
+ result->addChild (myFaceSet);
+
+ result->unrefNoDelete();
+ return result;
+}
+
+/* Cree une fleche composee d'un cylindre et d'un cone.
+ * La fleche a une hauteur total de <longueur>, dont
+ * <proportionFleche>% pour la fleche. Le rayon du cylindre
+ * est <radius>, et celui de la fleche <radius> * 5.
+ * La fleche est oriente selon l'axe Y.
+ */
+static SoSeparator *
+createArrow (float longueur,
+ float proportionFleche,
+ float radius)
+{
+ SoSeparator *fleche = new SoSeparator;
+ fleche->ref();
+
+ SoTranslation *poseCylindre = new SoTranslation;
+ SoCylinder *line = new SoCylinder;
+ SoTranslation *posePointe = new SoTranslation;
+ SoCone *pointe = new SoCone;
+
+ float l_cylindre = longueur * ( 1 - proportionFleche);
+ float l_cone = longueur * proportionFleche;
+ float radius_cylindre = radius;
+ float radius_cone = radius * 5;
+
+ line->radius.setValue (radius_cylindre);
+ line->height.setValue (l_cylindre);
+
+ poseCylindre->translation.setValue (0, l_cylindre / 2, 0);
+ posePointe->translation.setValue (0.0, l_cylindre / 2 + l_cone / 2, 0);
+
+ pointe->bottomRadius.setValue (radius_cone);
+ pointe->height.setValue (l_cone);
+
+
+ fleche->addChild (poseCylindre);
+ fleche->addChild (line);
+ fleche->addChild (posePointe);
+ fleche->addChild (pointe);
+
+ return fleche;
+}
+
+
+/*
+ Cree un objet repere dans un noeud separator, et le renvoie.
+ \return : code d'erreur, SIMU_CODE_OK si tout s'est bien passe.
+*/
+#define LONGUEUR_FLECHE 1.0f
+#define RAYON_FLECHE 0.002f
+#define PROPORTION_FLECHE 0.1f
+
+SoSeparator *
+createFrame (float longueurFleche = LONGUEUR_FLECHE ,
+ float proportionFleche = PROPORTION_FLECHE,
+ float radiusFleche = RAYON_FLECHE)
+{
+ vpDEBUG_TRACE (15, "# Entree.");
+
+ SoSeparator *frame = new SoSeparator;
+ frame-> ref ();
+
+ SoRotationXYZ *rotationY_X = new SoRotationXYZ;
+ rotationY_X->axis = SoRotationXYZ::Z;
+ rotationY_X->angle.setValue ((float)(- M_PI / 2));
+
+ SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
+ rotationX_Y->axis = SoRotationXYZ::Z;
+ rotationX_Y->angle.setValue ((float)(M_PI / 2));
+
+ SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
+ rotationY_Z->axis = SoRotationXYZ::X;
+ rotationY_Z->angle.setValue ((float)(M_PI / 2));
+
+ SoMaterial *rouge = new SoMaterial;
+ rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
+ rouge->emissiveColor.setValue(0.5, 0.0, 0.0);
+
+ SoMaterial *vert = new SoMaterial;
+ vert->diffuseColor.setValue(0.0, 1.0, 0.0);
+ vert->emissiveColor.setValue(0.0, 0.5, 0.0);
+
+ SoMaterial *bleu = new SoMaterial;
+ bleu->diffuseColor.setValue(0.0, 0.0, 1.0);
+ bleu->emissiveColor.setValue(0.0, 0.0, 0.5);
+
+ SoSeparator *fleche = createArrow(longueurFleche,
+ proportionFleche,
+ radiusFleche);
+
+ frame->addChild (rouge);
+ frame->addChild (rotationY_X);
+ frame->addChild (fleche);
+ frame->addChild (vert);
+ frame->addChild (rotationX_Y);
+ frame->addChild (fleche);
+ frame->addChild (bleu);
+ frame->addChild (rotationY_Z);
+ frame->addChild (fleche);
+
+ frame-> unrefNoDelete ();
+
+ vpDEBUG_TRACE (15, "# Sortie.");
+ return frame;
+}
+
+SoSeparator *
+createCameraObject (const float zoomFactor = 1.0)
+{
+ vpDEBUG_TRACE (15, "# Entree.");
+
+ SoSeparator * cam = new SoSeparator;
+ cam->ref ();
+
+ SoMaterial *myMaterial = new SoMaterial;
+ myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
+ myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
+
+ SoScale *taille = new SoScale;
+ {
+ float zoom = 0.1f * zoomFactor;
+ taille->scaleFactor.setValue (zoom, zoom, zoom);
+ }
+
+ SoMaterial *couleurBlanc = new SoMaterial;
+ couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
+ couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
+ SoDrawStyle * filDeFer = new SoDrawStyle;
+ filDeFer->style.setValue (SoDrawStyle::LINES);
+ filDeFer->lineWidth.setValue (1);
+
+ SoSeparator * cone = new SoSeparator;
+ cone->ref();
+ cone->addChild (makePyramide());
+ cone->addChild (couleurBlanc);
+ cone->addChild (filDeFer);
+ cone->addChild (makePyramide());
+ cone->unrefNoDelete();
+
+ cam->addChild(myMaterial);
+ cam->addChild(taille);
+ cam->addChild(cone);
+ cam->addChild(createFrame(2.0f,0.1f,0.01f));
+
+ // cam->unref() ;
+ vpDEBUG_TRACE (15, "# Sortie.");
+ return cam;
+}
+
+
+//--------------------------------------------------------------
+void
+vpSimulator::init()
+{
+ internal_width = 200;
+ internal_height= 200;
+ external_width = 200;
+ external_height= 200;
+
+ mainWindowInitialized = false ;
+ internalView = NULL ;
+ externalView = NULL ;
+ image_background = NULL ;
+
+ zoomFactor = 1 ;
+ cameraPositionInitialized = false ;
+
+ // write image process
+ realtime=NULL ;
+ offScreenRenderer = NULL ;
+ bufferView = NULL;
+ get = 1 ;
+ typeImage = grayImage;
+ mainThread = NULL;
+ scene = NULL;
+ internalRoot = NULL;
+ externalRoot = NULL;
+ internalCamera = NULL;
+ externalCamera = NULL;
+ internalCameraPosition = NULL;
+ extrenalCameraPosition = NULL;
+ internalCameraObject = NULL;
+#if defined(VISP_HAVE_SOWIN)
+ // mainWindow = ?;
+#elif defined(VISP_HAVE_SOQT)
+ mainWindow = NULL;
+#elif defined(VISP_HAVE_SOXT)
+ // mainWindow = ?;
+#endif
+
+}
+void
+vpSimulator::kill()
+{
+ if (internalView !=NULL) {delete internalView ; internalView = NULL;}
+ if (externalView !=NULL) {delete externalView ; externalView = NULL;}
+ if (bufferView!=NULL) {delete[] bufferView ; bufferView = NULL;}
+ if (image_background != NULL) {
+ free (image_background);
+ image_background = NULL;
+ }
+
+}
+
+vpSimulator::vpSimulator()
+ :
+#if defined(VISP_HAVE_SOWIN)
+ mainWindow(),
+#elif defined(VISP_HAVE_SOQT)
+ mainWindow(NULL),
+#elif defined(VISP_HAVE_SOXT)
+ mainWindow(),
+#endif
+ mainWindowInitialized(false), typeImage(vpSimulator::grayImage),
+ image_background(NULL), internalView(NULL), externalView(NULL),
+ mainThread(NULL), internal_width(0), internal_height(0),
+ external_width(0), external_height(0), scene(NULL), internalRoot(NULL),
+ externalRoot(NULL), internalCamera(NULL), externalCamera(NULL),
+ internalCameraPosition(NULL), extrenalCameraPosition(NULL), internalCameraObject(NULL),
+ zoomFactor(0.), cameraPositionInitialized(false), cMf(), internalCameraParameters(),
+ externalCameraParameters(), realtime(NULL), offScreenRenderer(NULL), bufferView(NULL),
+ get(0)
+{
+ vpSimulator::init() ;
+}
+
+vpSimulator::~vpSimulator()
+{
+ vpSimulator::kill() ;
+}
+
+void
+vpSimulator::initSoApplication()
+{
+ mainWindow = vpViewer::init("");
+ mainWindowInitialized = true ;
+}
+
+void
+vpSimulator::initSceneGraph()
+{
+ this->scene = new SoSeparator;
+ this->internalRoot = new SoSeparator;
+ this->externalRoot = new SoSeparator;
+
+ this->scene->ref();
+ this->internalRoot->ref();
+ this->externalRoot->ref();
+
+ // define the camera SoPerspectiveCamera
+ this->internalCamera = new SoPerspectiveCamera ;
+ this->externalCamera = new SoPerspectiveCamera ;
+
+
+ this->internalCameraPosition = new SoTransform;
+ this->internalCameraObject = createCameraObject(zoomFactor);
+
+ internalCamera->farDistance.setValue(100);
+ internalCamera->nearDistance.setValue(0.0001f);
+
+
+
+ // link between camera and internal root
+ this->internalRoot->addChild (this->internalCamera);
+ this->internalRoot->addChild (this->scene);
+
+ this->externalRoot->addChild (this->externalCamera);
+ this->externalRoot->addChild (this->scene);
+
+
+ SoSeparator * camera = new SoSeparator;
+ camera->ref();
+ camera->addChild (this->internalCameraPosition);
+ camera->addChild (this->internalCameraObject);
+ this->externalRoot->addChild (camera);
+
+
+ //this->externalRoot->addChild (internalCameraPosition);
+ // this->externalRoot->addChild (internalCameraObject);
+ SoCube *cube = new SoCube ;
+ cube->width=0.01f ;
+ cube->depth=0.01f ;
+ cube->height=0.01f ;
+
+ this->externalRoot->addChild (cube);
+
+ if (realtime==NULL)
+ {
+
+ SoDB::enableRealTimeSensor(FALSE);
+ SoSceneManager::enableRealTimeUpdate(FALSE);
+ realtime = (SbTime *) SoDB::getGlobalField("realTime");
+ realtime->setValue(0.0);
+
+ }
+
+
+}
+
+
+
+/*!
+ \brief Define the zoom factor used to define the size of the objects (frame,
+ camera, ...)
+
+ \param zoom: zoom factor of the objects. By default, 1.
+*/
+void
+vpSimulator::setZoomFactor (const float zoom)
+{
+ zoomFactor = zoom;
+ static bool firstTime = true;
+ if(firstTime){
+ SoScale *taille = new SoScale;
+ taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+ this->scene->addChild(taille);
+ firstTime = false;
+ }
+ else{
+ SoScale * taille = (SoScale*)this->scene->getChild(0);
+ taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+ }
+}
+
+/*!
+ \brief Change the zoom factor associated to the child given by index.
+ In order to create multiple zoom factor for multiple object to display,
+ objects loaded the load() function, you have to know the index of the scale
+ object associated to.
+
+ Usually, if you define the main zoom factor (for example for the frames) and
+ then load two differents objects, You can change the zoom factor of all the
+ objects using: changeZoomFactor(newZoom, 0)
+
+ If you want to change the zoom factor of the first object, use changeZoomFactor(newZoom, 1)
+
+ And for the second object, use changeZoomFactor(newZoom, 3)
+
+ \param zoomFactor : the new zoom use to specify the apparent size of the object
+ \param index : the index of the Scale object to modify (see comments)
+*/
+void
+vpSimulator::changeZoomFactor(const float zoomFactor, const int index)
+{
+ SoScale * taille = (SoScale*)this->scene->getChild(index);
+ taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+// this->setZoomFactor(zoomFactor);
+}
+
+void
+vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height)
+{
+ internal_width = width;
+ internal_height = height;
+
+ if (mainWindowInitialized==false)
+ {
+ initSoApplication() ;
+ initSceneGraph() ;
+ }
+
+ internalView = new vpViewer(mainWindow, this,vpViewer::internalView);
+
+ // set the scene to render from this view
+ internalView->setSceneGraph(internalRoot);
+
+ // set the title
+ internalView->setTitle("Internal camera view") ;
+
+ //If the view mode is on, user events will be caught and used to influence
+ //the camera position / orientation. in this viewer we do not want that,
+ //we set it to false
+ internalView->setViewing(false);
+
+ // Turn the viewer decorations
+ internalView->setDecoration(false) ;
+
+ internalView->resize((int)width, (int)height, true) ;
+
+ // open the window
+ internalView->show();
+
+ bufferView = new unsigned char[3*width*height] ;
+
+}
+
+void
+vpSimulator::initExternalViewer(const unsigned int width, const unsigned int height)
+{
+
+ external_width = width;
+ external_height = height;
+
+ if (mainWindowInitialized==false)
+ {
+ initSoApplication() ;
+ initSceneGraph() ;
+ }
+
+ externalView = new vpViewer(mainWindow,this, vpViewer::externalView);
+
+ // set the scene to render this view
+ externalView->setSceneGraph(externalRoot);
+
+ // set the title
+ externalView->setTitle("External View") ;
+ externalView->resize((int)width, (int)height, false) ;
+ // the goal here is to see all the scene and not to determine
+ // a manual viewpoint
+ externalView->viewAll ();
+
+ // open the window
+ externalView->show();
+}
+
+void
+vpSimulator::setInternalCameraParameters(vpCameraParameters &_cam)
+{
+ internalCameraParameters = _cam ;
+
+
+ float px = (float)_cam.get_px();
+ float py = (float)_cam.get_py();
+ float v = internal_height/(2.f*py);
+
+ internalCamera->ref() ;
+ internalCamera->heightAngle = 2*atan(v);
+ internalCamera->aspectRatio=(internal_width/internal_height)*(px/py);
+ internalCamera->nearDistance = 0.001f ;
+
+ internalCamera->farDistance = 1000;
+ internalCamera->unrefNoDelete() ;
+}
+
+void
+vpSimulator::setExternalCameraParameters(vpCameraParameters &_cam)
+{
+// SoPerspectiveCamera *camera ;
+// camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
+ externalCameraParameters = _cam ;
+
+ float px = (float)_cam.get_px();
+ float py = (float)_cam.get_py();
+ float v = external_height/(2*py);
+
+ externalCamera->ref() ;
+ externalCamera->heightAngle = 2*atan(v);
+ externalCamera->aspectRatio=(external_width/external_height)*(px/py);
+ externalCamera->nearDistance = 0.001f ;
+ externalCamera->farDistance = 1000;
+ externalCamera->unrefNoDelete() ;
+
+}
+
+void
+vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf)
+{
+/* SoCamera *camera ;
+ camera = this->externalView->getCamera() ;*/
+ SoSFVec3f position = externalCamera->position ;
+
+ // get the rotation
+ SoSFRotation orientation = externalCamera->orientation;
+ SbVec3f axis ; float angle ;
+ orientation.getValue(axis,angle) ;
+ SbRotation rotation(axis,angle) ;
+
+ // get the translation
+ SbVec3f t ;
+ t = position.getValue() ;
+
+ SbMatrix matrix ;
+ matrix.setRotate(rotation) ;
+
+ vpHomogeneousMatrix fMc ;
+ SbMatrix rotX;
+ rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
+ matrix.multLeft (rotX);
+ for(unsigned int i=0;i<4;i++)
+ for(unsigned int j=0;j<4;j++)
+ fMc[j][i]=matrix[(int)i][(int)j];
+ fMc[0][3] = t[0] ;
+ fMc[1][3] = t[1] ;
+ fMc[2][3] = t[2] ;
+
+ cMf = fMc.inverse() ;
+}
+
+
+void
+vpSimulator::setCameraPosition(vpHomogeneousMatrix &_cMf)
+{
+ cameraPositionInitialized = true ;
+ cMf = _cMf ;
+}
+void
+vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf)
+{
+
+ SbMatrix matrix;
+ SbRotation rotCam;
+ SbMatrix rotX;
+ rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
+ for(unsigned int i=0;i<4;i++)
+ for(unsigned int j=0;j<4;j++)
+ matrix[(int)j][(int)i]=(float)cMf[i][j];
+
+ matrix= matrix.inverse();
+ matrix.multLeft (rotX);
+ rotCam.setValue(matrix);
+
+
+ internalCamera->ref() ;
+ internalCamera->orientation.setValue(rotCam);
+ internalCamera->position.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
+ internalCamera->unref() ;
+
+ rotX.setRotate (SbRotation (SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
+ matrix.multLeft (rotX);
+ rotCam.setValue(matrix);
+ internalCameraPosition->ref() ;
+ internalCameraPosition->rotation.setValue(rotCam);
+ internalCameraPosition->translation.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
+ internalCameraPosition->unref() ;
+}
+
+
+/*! this function MUST NOT be called from a thread where the vpSimulator and
+ its mainloop are not
+*/
+void
+vpSimulator::redraw()
+{
+
+ // if (this->cameraPositionInitialized==true)
+ {
+ if (this->externalView != NULL)
+ {
+ this->externalView->render() ; //call actualRedraw()
+ // vpHomogeneousMatrix c ;
+ // getExternalCameraPosition(c) ;
+ }
+ if (this->internalView != NULL)
+ {
+ this->moveInternalCamera(this->cMf) ;
+ this->internalView->render() ; //call actualRedraw()
+ }
+ }
+}
+
+// This function is called 20 times each second.
+static void
+timerSensorCallback(void *data , SoSensor *)
+{
+ vpSimulator * simulator = (vpSimulator *)data ;
+
+ simulator->redraw() ;
+
+}
+
+
+void
+vpSimulator::mainLoop()
+{
+ if (mainWindowInitialized==false)
+ {
+ vpERROR_TRACE("main window is not opened ") ;
+ }
+
+ vpTime::wait(1000) ;
+
+ // Timer sensor
+ SoTimerSensor * timer = new SoTimerSensor(timerSensorCallback, (void *)this);
+ timer->setInterval(0.01);
+ timer->schedule();
+ vpViewer::mainLoop() ;
+}
+
+
+//-----------------------------------------------------------------
+// scene stuff
+//-----------------------------------------------------------------
+
+//!loading the virtual scene
+void
+vpSimulator::load(const char *file_name)
+{
+
+ SoInput input;
+ if (!input.openFile(file_name))
+ {
+ vpERROR_TRACE("Erreur cannot open file %s",file_name);
+ }
+
+ SoSeparator *newscene=SoDB::readAll(&input);
+ newscene->ref() ;
+ if (newscene==NULL)
+ {
+ vpERROR_TRACE("Error while reading %s",file_name);
+ }
+
+ SoScale *taille = new SoScale;
+ taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+
+// newscene->addChild(taille);
+
+// std::cout << "this->scene->getNumChildren() = " << this->scene->getNumChildren() << std::endl;
+
+ this->scene->addChild(taille);
+ this->scene->addChild(newscene);
+ newscene->unref() ;
+
+}
+
+
+void
+vpSimulator::save(const char *name,bool binary)
+{
+ // get a pointer to the object "name"
+ SoOutput output ;
+ output.openFile(name) ;
+
+ if (binary==true) output.setBinary(TRUE) ;
+
+ SoWriteAction writeAction(&output) ;
+ writeAction.apply(scene) ;
+ output.closeFile() ;
+
+}
+
+/*!
+ Add the representation of a frame.
+ \param fMo : desired position of the frame
+ \param zoom : Zoom factor.
+*/
+void
+vpSimulator::addFrame (const vpHomogeneousMatrix &fMo, float zoom)
+{
+
+ SoScale *taille = new SoScale;
+ taille->scaleFactor.setValue (zoom, zoom, zoom);
+
+ SoSeparator * frame = new SoSeparator;
+ frame->ref();
+ frame->addChild(taille);
+ frame->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom));
+ this->addObject(frame, fMo, externalRoot) ;
+ // frame->unref();
+}
+
+/*!
+ \brief Add the representation of the absolute frame
+
+ \param zoom : Zoom factor.
+*/
+void
+vpSimulator::addAbsoluteFrame (float zoom)
+{
+ scene->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom)) ;
+}
+
+/*!
+ \brief Add a new object in the scene graph
+ \param iv_filename : name of.iv file to load
+ \param fMo : position of the object wrt the reference frame
+*/
+void
+vpSimulator::load(const char * iv_filename,const vpHomogeneousMatrix &fMo)
+{
+
+ SoInput in;
+ SoSeparator * newObject;
+
+ if (! in.openFile (iv_filename))
+ {
+ vpERROR_TRACE ("Erreur lors de la lecture du fichier %s.", iv_filename);
+ }
+
+ newObject = SoDB::readAll (&in);
+ if (NULL == newObject)
+ {
+ vpERROR_TRACE ("Problem reading data for file <%s>.", iv_filename);
+ }
+
+ try
+ {
+ this->addObject (newObject, fMo) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error adding object from file <%s> ",iv_filename) ;
+ throw ;
+ }
+
+}
+
+
+/*!
+ \brief Add a new object in the scene graph
+ \param newObject : pointer toward the new object
+ \param fMo : position of the object wrt the reference frame
+*/
+void
+vpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo)
+{
+ try
+ {
+ this->addObject(newObject, fMo , scene);
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error adding object in scene graph ") ;
+ throw ;
+ }
+}
+
+
+
+
+
+/*!
+ \brief Add an object in a sub scene graph
+ \param object : pointer toward the new object
+ \param fMo : position of the object wrt the reference frame
+ \param root : pointer toward the subscene graph
+*/
+
+void
+vpSimulator::addObject(SoSeparator * object,
+ const vpHomogeneousMatrix &fMo,
+ SoSeparator * root)
+{
+
+ bool identity = true ;
+ for (unsigned int i=0 ; i <4 ;i++){
+ for (unsigned int j=0 ; j < 4 ; j++){
+ if (i==j){
+ if (fabs(fMo[i][j] -1) > 1e-6) identity=false ;
+ }
+ else{
+ if (fabs(fMo[i][j]) > 1e-6) identity=false ;
+ }
+ }
+ }
+
+ if (identity==true)
+ {
+ root->addChild (object);
+ }
+ else
+ {
+ SbMatrix matrix;
+ SbRotation rotation;
+ for(unsigned int i=0;i<4;i++)
+ for(unsigned int j=0;j<4;j++)
+ matrix[(int)j][(int)i]=(float)fMo[i][j];
+
+ // matrix= matrix.inverse();
+ rotation.setValue(matrix);
+
+ SoTransform *displacement = new SoTransform;
+ SoSeparator *newNode = new SoSeparator;
+
+ displacement->rotation.setValue(rotation);
+ displacement->translation.setValue(matrix[3][0],
+ matrix[3][1],
+ matrix[3][2]);
+
+ root->addChild (newNode);
+ newNode->addChild (displacement);
+ newNode->addChild (object);
+
+ }
+}
+
+
+//! init the main program thread
+void
+vpSimulator::initApplication(void *(*start_routine)(void *))
+{
+ //pthread_create (&mainThread, NULL, start_routine, (void *)this);
+ mainThread = SbThread::create (start_routine, (void *)this);
+}
+
+/*!
+ Set the function used for the simulation loop and the data to pass to this
+ function. As the data are represented using a generic pointer, care should be
+ taken to ensure there is no memory corruption.
+
+ \param start_routine : A pointer to the function used as a main simulation
+ loop for the simulation.
+ \param data : The data to pass to the main loop.
+*/
+void
+vpSimulator::initApplication(void *(*start_routine)(void *), void* data)
+{
+ mainThread = SbThread::create (start_routine, (void *)data);
+}
+
+//! performed some initialization in the main program thread
+//! should be locate at the beginning of the main program
+void
+vpSimulator::initMainApplication()
+{
+ //pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
+ //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
+ vpTime::wait(1000) ;
+}
+//! performed some thread destruction in the main program thread
+//! should be locate at the end of the main program
+void
+vpSimulator::closeMainApplication()
+{
+ vpViewer::exitMainLoop() ;
+ //pthread_exit (NULL);
+}
+
+
+
+
+
+/* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
+ * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
+ * enregistre ou passe a l'utilisateur.
+ * INPUT:
+ * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
+ * il faut rendre la vue externe.
+ * OUTPUT:
+ * - width : largeur de l'image dans le buffer.
+ * - height : hauteur de l'image dans le buffer.
+ */
+void
+vpSimulator::offScreenRendering(vpSimulatorViewType view, int * width, int * height)
+{
+
+ SbVec2s size(320,200);
+ SoNode * thisroot;
+
+ {
+ if (view==vpSimulator::INTERNAL)
+ {
+ size = this ->internalView ->getViewportRegion().getWindowSize();
+ thisroot = this ->internalView->getSceneManager()->getSceneGraph() ;
+ }
+ else
+ {
+ size = this ->externalView ->getViewportRegion().getWindowSize();
+ thisroot = this ->externalView->getSceneManager()->getSceneGraph() ;
+ }
+ }
+ SbViewportRegion myViewPort(size);
+
+ // Creation du rendu si necessaire.
+ if (NULL == this ->offScreenRenderer)
+ {
+ //Init du SoOffscreenRenderer
+ this ->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
+ }
+ else
+ {
+ // Redefini le view port
+ this ->offScreenRenderer ->setViewportRegion (myViewPort);
+ }
+
+ // Rendu offscreen
+ if (! this ->offScreenRenderer ->render(thisroot))
+ {
+ vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
+ delete this ->offScreenRenderer;
+ this ->offScreenRenderer = NULL;
+ }
+ else
+ {
+
+
+ /*
+ if (view==vpSimulator::INTERNAL)
+ {
+ //Recopie du buffer contenant l'image, dans bufferView
+ int length = 3*size [0]*size[1];
+ delete [] bufferView;
+ bufferView = new unsigned char [length];
+ for(int i=0; i<length; i++)
+ {
+ bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
+ }
+ }*/
+
+ }
+
+ // exit(1) ;
+ if (NULL != width) { * width = size [0]; }
+ if (NULL != height) { * height = size [1]; }
+
+
+}
+
+
+/* Enregistre l'image de vue interne ou externe dans un fichier RGB.
+ * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
+ * ce buffer au format PS (copie directe).
+ * INPUT
+ * - fileName: nom du fichier dans lequel placer le resultat.
+ * OUTPUT
+ * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
+ */
+
+#ifdef VISP_HAVE_MODULE_IO
+void
+vpSimulator::write (const char * fileName)
+{
+
+ while (get==0) { vpTRACE("%d ",get); }
+ get =2 ;
+ /* FILE *fp = fopen(fileName, "w");
+ fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
+ fwrite(bufferView, sizeof(unsigned char), internal_width*internal_height*3, fp) ;*/
+ vpImage<vpRGBa> I(internal_height, internal_width) ;
+
+
+ for(unsigned int i=0 ; i < internal_height ; i++)
+ for(unsigned int j=0 ; j < internal_width ; j++)
+ {
+ unsigned char r,g,b ;
+ unsigned int index = 3*((internal_height-i-1)* internal_width + j );
+ r = *(bufferView+index);
+ g = *(bufferView+index+1);
+ b = *(bufferView+index+2);
+ I[i][j].R =r ;
+ I[i][j].G =g ;
+ I[i][j].B =b ;
+ }
+ vpImageIo::write(I,fileName) ;
+ // fclose (fp);
+ get =1 ;
+}
+#endif
+
+void
+vpSimulator::getSizeInternalView(int& width, int& height)
+{
+ SbVec2s size = this ->internalView ->getViewportRegion().getWindowSize();
+ width = size [0];
+ height = size[1];
+}
+
+/*!
+ Make a copy of the current internal view
+ \param I : destination image
+ */
+
+void
+vpSimulator::getInternalImage(vpImage<vpRGBa> &I)
+{
+ //while (get==0) {;}
+ get =2 ;
+ I.resize(internal_height,internal_width) ;
+ vpImageConvert::RGBToRGBa(bufferView,(unsigned char*)I.bitmap,internal_width,internal_height,true);
+ get =1 ;
+}
+
+/*!
+ Make a copy of the current internal view
+ \param I : destination image
+ */
+void
+vpSimulator::getInternalImage(vpImage<unsigned char> &I)
+{
+ //while (get==0) {;}
+ get =2 ;
+ I.resize(internal_height,internal_width) ;
+ vpImageConvert::RGBToGrey(bufferView,I.bitmap,internal_width,internal_height,true);
+ get =1 ;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no symbols
+void dummy_vpSimulator() {};
+#endif
diff --git a/modules/ar/src/coin-simulator/vpViewer.cpp b/modules/ar/src/coin-simulator/vpViewer.cpp
new file mode 100644
index 0000000..0cb5c68
--- /dev/null
+++ b/modules/ar/src/coin-simulator/vpViewer.cpp
@@ -0,0 +1,265 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simulator based on Coin3d.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+/*!
+ \file vpViewer.cpp
+ Viewer used by the simulator. Under Windows, the viewer is
+ based either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or SoXt.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_COIN3D_AND_GUI
+#include <visp3/ar/vpViewer.h>
+#include <visp3/ar/vpSimulator.h>
+
+
+#include <Inventor/events/SoKeyboardEvent.h>
+#include <Inventor/nodes/SoEventCallback.h>
+
+
+#if defined(VISP_HAVE_SOWIN)
+vpViewer::vpViewer(HWND parent, vpSimulator *_simu, vpViewerType viewerType):
+ SoWinExaminerViewer(parent,(char *)NULL,false)
+#elif defined(VISP_HAVE_SOQT)
+vpViewer::vpViewer(QWidget * parent, vpSimulator *_simu, vpViewerType viewerType) :
+ SoQtExaminerViewer(parent,(char *)NULL,false)
+#elif defined(VISP_HAVE_SOXT)
+vpViewer::vpViewer(Widget parent, vpSimulator *_simu, vpViewerType viewerType):
+ SoXtExaminerViewer(parent,(char *)NULL,false)
+#endif
+{
+ this->simu = _simu ;
+ this->viewerType = viewerType;
+ // Coin should not clear the pixel-buffer, so the background image
+ // is not removed.
+
+ this->setClearBeforeRender(FALSE, TRUE);
+ // this->setAntialiasing(true, 2) ;
+ setAutoRedraw(false);
+}
+
+vpViewer::~vpViewer()
+{
+
+}
+
+void
+vpViewer::actualRedraw(void)
+{
+
+ {
+ const SbViewportRegion vp = this->getViewportRegion();
+ SbVec2s origin = vp.getViewportOriginPixels();
+ SbVec2s size = vp.getViewportSizePixels();
+ glViewport(origin[0], origin[1], size[0], size[1]);
+
+ const SbColor col = this->getBackgroundColor();
+ glClearColor(col[0], col[1], col[2], 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ // this should be used only with the vpAR:vpSimulator
+ // to diplay an image background
+ if (simu->image_background != NULL)
+ {
+ glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+ if (simu->typeImage == vpSimulator::grayImage)
+ glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
+ (GLenum)GL_LUMINANCE,
+ GL_UNSIGNED_BYTE,
+ simu->image_background );
+ else
+ glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
+ (GLenum)GL_RGB,
+ GL_UNSIGNED_BYTE,
+ simu->image_background );
+
+ glEnable(GL_DEPTH_TEST);
+ glClear(GL_DEPTH_BUFFER_BIT); // clear the z-buffer
+ glClearDepth(100.0); // Profondeur du Z-Buf
+ }
+ // Render normal scenegraph.
+#if defined(VISP_HAVE_SOWIN)
+ SoWinExaminerViewer::actualRedraw();
+#elif defined(VISP_HAVE_SOQT)
+ SoQtExaminerViewer::actualRedraw();
+#elif defined(VISP_HAVE_SOXT)
+ SoXtExaminerViewer::actualRedraw();
+#endif
+ glSwapBuffers() ;
+ if(viewerType == vpViewer::internalView){
+ simu->get = 0 ;
+ glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
+ (GLenum)GL_RGB,
+ GL_UNSIGNED_BYTE,
+ simu->bufferView ) ;
+ simu->get =1 ;
+ }
+ }
+
+}
+
+/*!
+
+ \param x : width
+ \param y : height
+ \param fixed : set as true to disable mouse window resizing
+
+*/
+void
+#if defined(VISP_HAVE_SOWIN) || defined (VISP_HAVE_SOQT)
+vpViewer::resize(int x, int y, bool fixed)
+#else
+vpViewer::resize(int x, int y, bool /*fixed*/)
+#endif
+{
+ SbVec2s size(x,y) ;
+ setSize(size);
+ setGLSize(size) ;
+
+#if defined(VISP_HAVE_SOWIN)
+ HWND parent = getParentWidget();
+
+ RECT rcClient, rcWindow;
+ POINT ptDiff;
+ GetClientRect(parent, &rcClient);
+ GetWindowRect(parent, &rcWindow);
+ ptDiff.x = (rcWindow.right - rcWindow.left) - rcClient.right;
+ ptDiff.y = (rcWindow.bottom - rcWindow.top) - rcClient.bottom;
+ MoveWindow(parent,rcWindow.left, rcWindow.top,
+ x + ptDiff.x, y + ptDiff.y, TRUE);
+ if(fixed){
+ DWORD dwStyle = GetWindowLong(parent, GWL_STYLE);
+ dwStyle &= ~(WS_SIZEBOX);
+ SetWindowLong(parent, GWL_STYLE,dwStyle);
+ }
+#elif defined(VISP_HAVE_SOQT)
+ if(fixed){
+ QWidget * parent = getParentWidget();
+ parent->setFixedSize(x, y);
+ }
+#endif
+}
+
+/*!
+\param left : left coordinate
+\param bottom : bottom coordinate
+\param x : width
+\param y : height
+
+*/
+
+SbBool
+vpViewer::processSoEvent(const SoEvent * const event)
+{
+ if ( this->isViewing() &&
+ event->getTypeId() == SoKeyboardEvent::getClassTypeId() )
+ {
+ SoKeyboardEvent * kbevent = (SoKeyboardEvent *) event;
+ switch ( kbevent->getKey() ) {
+ case SoKeyboardEvent::H:
+ if ( kbevent->getState() == SoButtonEvent::DOWN )
+ {
+ std::cout << "H : this help "<<std::endl ;
+ std::cout << "M : get and save the external camera location (matrix)"<<std::endl;
+ std::cout << "V : get and save the external camera location (vector)"<<std::endl;
+ std::cout << "M : load camera location (vector)"<<std::endl;
+ std::cout << "P : get external camera location and set the internal one"<<std::endl;
+ }
+ return TRUE;
+
+ case SoKeyboardEvent::M:
+ if ( kbevent->getState() == SoButtonEvent::DOWN )
+ {
+ vpHomogeneousMatrix cMf ;
+ simu->getExternalCameraPosition(cMf) ;
+ std::ofstream f("cMf.dat") ;
+ cMf.save(f) ;
+ f.close() ;
+ }
+ return TRUE;
+ case SoKeyboardEvent::V:
+ if ( kbevent->getState() == SoButtonEvent::DOWN )
+ {
+ vpHomogeneousMatrix cMf ;
+ simu->getExternalCameraPosition(cMf) ;
+ vpPoseVector vcMf(cMf) ;
+ std::ofstream f("vcMf.dat") ;
+ vcMf.save(f) ;
+ f.close() ;
+ }
+ return TRUE;
+ case SoKeyboardEvent::L:
+ if ( kbevent->getState() == SoButtonEvent::DOWN )
+ {
+ vpPoseVector vcMf;
+ std::ifstream f("vcMf.dat") ;
+ vcMf.load(f) ;
+ f.close() ;
+ vpHomogeneousMatrix cMf(vcMf) ;
+ simu->setCameraPosition(cMf) ;
+ simu->moveInternalCamera(cMf) ;
+ }
+ return TRUE;
+ case SoKeyboardEvent::P:
+ if ( kbevent->getState() == SoButtonEvent::DOWN )
+ {
+ vpHomogeneousMatrix cMf ;
+ simu->getExternalCameraPosition(cMf) ;
+ vpPoseVector vcMf(cMf) ;
+ vcMf.print() ;
+ simu->setCameraPosition(cMf) ;
+ simu->moveInternalCamera(cMf) ;
+ }
+ return TRUE;
+ default:
+ break;
+ }
+ }
+#if defined(VISP_HAVE_SOWIN)
+ return SoWinExaminerViewer::processSoEvent(event);
+#elif defined(VISP_HAVE_SOQT)
+ return SoQtExaminerViewer::processSoEvent(event);
+#elif defined(VISP_HAVE_SOXT)
+ return SoXtExaminerViewer::processSoEvent(event);
+#endif
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_ar.a(vpViewer.cpp.o) has no symbols
+void dummy_vpViewer() {};
+#endif
diff --git a/modules/ar/src/ogre-simulator/vpAROgre.cpp b/modules/ar/src/ogre-simulator/vpAROgre.cpp
new file mode 100644
index 0000000..8559c49
--- /dev/null
+++ b/modules/ar/src/ogre-simulator/vpAROgre.cpp
@@ -0,0 +1,1097 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Augmented Reality viewer using Ogre3D.
+ *
+ * Authors:
+ * Bertrand Delabarre
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpAROgre.cpp
+
+ \brief Class that implements augmented reality viewer based on Ogre3D.
+
+ \warning The content of this file is only available if Ogre3D and
+ one of the renderer (OpenGL or Direct3Dt) are installed.
+
+*/
+
+#include "visp3/core/vpConfig.h"
+
+#ifdef VISP_HAVE_OGRE
+
+#include "visp3/ar/vpAROgre.h"
+#include "visp3/core/vpIoTools.h"
+
+
+/*!
+ Constructor.
+
+ \param cam : Camera parameters.
+
+ \param width : Width of the window.
+ \param height : Height of the window.
+
+ \param resourcePath : Path to the resources.cfg file telling Ogre where to
+ look for resources.
+
+ \param pluginsPath : Path to the plugins.cfg of plugins_d.cfg files
+ telling Ogre where to look for renderer plugins.
+
+*/
+vpAROgre::vpAROgre(const vpCameraParameters &cam,
+ unsigned int width, unsigned int height,
+ const char *resourcePath, const char *pluginsPath)
+ : name("ViSP - Augmented Reality"),mRoot(0), mCamera(0), mSceneMgr(0), mWindow(0),
+ mResourcePath(resourcePath), mPluginsPath(pluginsPath),
+#ifdef VISP_HAVE_OIS
+ mInputManager(0), mKeyboard(0),
+#endif
+ keepOn(true), // When created no reason to stop displaying
+ mImageRGBA(), mImage(), mPixelBuffer(NULL), mBackground(NULL), mBackgroundHeight(0),
+ mBackgroundWidth(0), mWindowHeight(height), mWindowWidth(width), windowHidden(false),
+ mNearClipping(0.001), mFarClipping(200), mcam(cam), mshowConfigDialog(true),
+ mOptionnalResourceLocation()
+{
+}
+
+/*!
+ Initialisation of Ogre with a grey level background.
+
+ Load the plugins that are specified in the plugins.cfg or
+ plugins_d.cfg files. These files are located in
+ VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
+ Note that plugins.cfg file is always considered under Unix
+ platforms. The file plugins_d.cfg is only considered under Windows
+ when the build type is Debug.
+
+ Load also the resources that are defined in the resources.cfg
+ file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
+ that is defined in vpConfig.h.
+
+ Create also the grey level background used to display the image.
+
+ \param I : Image that is displayed in the background.
+
+ \param bufferedKeys : If true, use of buffered input for the keyboard (see
+ Ogre documentation). Note that this parameter is only useful if OIS is used.
+
+ \param hidden : If true, the created window will be hidden.
+ Note that this functionnality requires Ogre3D 1.8.1 at least.
+
+ \exception vpException::ioError : If the required plugins.cfg /
+ plugins_d.cfg or resources.cfg files are not accessible.
+
+*/
+void vpAROgre::init(vpImage<unsigned char> &I,
+ bool
+#ifdef VISP_HAVE_OIS
+ bufferedKeys
+#endif
+ ,bool hidden
+ )
+{
+ mBackgroundWidth = I.getWidth();
+ mBackgroundHeight = I.getHeight();
+
+ init(
+#ifdef VISP_HAVE_OIS
+ bufferedKeys,
+#else
+ false,
+#endif
+ hidden
+ );
+ // Create the background image which will come from the grabber
+ createBackground(I);
+}
+
+/*!
+ Initialisation of Ogre with a color background.
+
+ Load the plugins that are specified in the plugins.cfg or
+ plugins_d.cfg files. These files are located in
+ VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
+ Note that plugins.cfg file is always considered under Unix
+ platforms. The file plugins_d.cfg is only considered under Windows
+ when the build type is Debug.
+
+ Load also the resources that are defined in the resources.cfg
+ file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
+ that is defined in vpConfig.h.
+
+ Create also a color background used to display the image.
+
+ \param I : Image that is displayed in the background.
+
+ \param bufferedKeys : If true, use of buffered input for the keyboard (see
+ Ogre documentation). Note that this parameter is only useful if OIS is used.
+
+ \param hidden : If true, the created window will be hidden.
+ Note that this functionnality requires Ogre3D 1.8.1 at least.
+
+ \exception vpException::ioError : If the required plugins.cfg /
+ plugins_d.cfg or resources.cfg files are not accessible.
+
+*/
+void vpAROgre::init(vpImage<vpRGBa> &I,
+ bool
+#ifdef VISP_HAVE_OIS
+ bufferedKeys
+#endif
+ ,bool hidden
+ )
+{
+ mBackgroundWidth = I.getWidth();
+ mBackgroundHeight = I.getHeight();
+
+ init(
+#ifdef VISP_HAVE_OIS
+ bufferedKeys,
+#else
+ false,
+#endif
+ hidden
+ );
+ // Create the background image which will come from the grabber
+ createBackground(I);
+}
+
+/*!
+ Initialisation of Ogre.
+
+ Load the plugins that are specified in the plugins.cfg or
+ plugins_d.cfg files. These files are located in
+ VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
+ Note that plugins.cfg file is always considered under Unix
+ platforms. The file plugins_d.cfg is only considered under Windows
+ when the build type is Debug.
+
+ Load also the resources that are defined in the resources.cfg
+ file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
+ that is defined in vpConfig.h.
+
+ \param bufferedKeys : If true, use of buffered input for the keyboard (see
+ Ogre documentation). Note that this parameter is only useful if OIS is used.
+
+ \param hidden : If true, the created window will be hidden.
+ Note that this functionnality requires Ogre3D 1.8.1 at least.
+
+ \exception vpException::ioError : If the required plugins.cfg /
+ plugins_d.cfg or resources.cfg files are not accessible.
+
+*/
+void vpAROgre::init(bool
+#ifdef VISP_HAVE_OIS
+ bufferedKeys
+#endif
+ ,bool hidden
+ )
+{
+ // Create the root
+ // mPluginsPath may contain more than one folder location separated by ";"
+ bool pluginsFileExists = false;
+ std::string pluginFile;
+ std::vector<std::string> plugingsPaths = vpIoTools::splitChain(std::string(mPluginsPath), std::string(";"));
+ for (size_t i=0; i<plugingsPaths.size(); i++) {
+#if defined(NDEBUG) || !defined(_WIN32)
+ pluginFile = plugingsPaths[i]+"/plugins.cfg";
+#else
+ pluginFile = plugingsPaths[i]+"/plugins_d.cfg";
+#endif
+
+ if(vpIoTools::checkFilename(pluginFile)) {
+ pluginsFileExists = true;
+ break;
+ }
+ }
+ if (! pluginsFileExists) {
+ std::string errorMsg = std::string("Error: the requested plugins file \"")
+#if defined(NDEBUG) || !defined(_WIN32)
+ + std::string("plugins.cfg")
+#else
+ + std::string("plugins_d.cfg")
+#endif
+ + std::string("\" doesn't exist in ")
+ + std::string(mPluginsPath);
+ std::cout << errorMsg << std::endl;
+
+ throw (vpException(vpException::ioError, errorMsg));
+ }
+ std::cout << "######################### Load plugin file: " << pluginFile << std::endl;
+
+ if(Ogre::Root::getSingletonPtr() == NULL)
+ mRoot = new Ogre::Root(pluginFile, "ogre.cfg", "Ogre.log");
+ else
+ mRoot = Ogre::Root::getSingletonPtr();
+
+ // Load resource paths from config file
+
+ // File format is:
+ // [ResourceGroupName]
+ // ArchiveType=Path
+ // .. repeat
+ // For example:
+ // [General]
+ // FileSystem=media/
+ // Zip=packages/level1.zip
+
+ // mResourcePath may contain more than one folder location separated by ";"
+ bool resourcesFileExists = false;
+ std::string resourceFile;
+ std::vector<std::string> resourcesPaths = vpIoTools::splitChain(std::string(mResourcePath), std::string(";"));
+ for (size_t i=0; i<resourcesPaths.size(); i++) {
+ resourceFile = resourcesPaths[i]+"/resources.cfg";
+ if(vpIoTools::checkFilename(resourceFile)) {
+ resourcesFileExists = true;
+ break;
+ }
+ }
+ if (! resourcesFileExists) {
+ std::string errorMsg = std::string("Error: the requested resource file \"resources.cfg\"")
+ + std::string("doesn't exist in ")
+ + std::string(mResourcePath);
+
+ std::cout << errorMsg << std::endl;
+
+ throw (vpException(vpException::ioError, errorMsg));
+ }
+ std::cout << "######################### Load resource file: " << resourceFile << std::endl;
+ Ogre::ConfigFile cf;
+ cf.load(resourceFile);
+
+ // Go through all sections & settings in the file
+ Ogre::ConfigFile::SectionIterator seci = cf.getSectionIterator();
+
+ Ogre::String secName, typeName, archName;
+ while (seci.hasMoreElements())
+ {
+ secName = seci.peekNextKey();
+ Ogre::ConfigFile::SettingsMultiMap *settings = seci.getNext();
+ Ogre::ConfigFile::SettingsMultiMap::iterator i;
+ for (i = settings->begin(); i != settings->end(); ++i)
+ {
+ typeName = i->first;
+ archName = i->second;
+ Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+ archName, typeName, secName);
+ }
+ }
+ std::cout << "##################### add resources" << std::endl;
+ //Add optionnal resources (given by the user).
+ for(std::list<std::string>::const_iterator iter = mOptionnalResourceLocation.begin(); iter != mOptionnalResourceLocation.end(); ++iter){
+ Ogre::ResourceGroupManager::getSingleton().addResourceLocation(*iter, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ }
+
+ // Create the window
+ bool canInit = true;
+ if(mshowConfigDialog){
+ mRoot->restoreConfig();
+ if(!mRoot->showConfigDialog())
+ canInit = false;
+ }
+ else{
+ if(!mRoot->restoreConfig())
+ canInit = false;
+ }
+
+ if(!mRoot->isInitialised()){
+ if(!canInit){ //We set the default renderer system
+ const Ogre::RenderSystemList& lRenderSystemList = mRoot->getAvailableRenderers();
+ if( lRenderSystemList.size() == 0 )
+ throw "ConfigDialog aborted"; // Exit the application on cancel
+
+ Ogre::RenderSystem *lRenderSystem = lRenderSystemList.at(0);
+ std::cout << "Using " << lRenderSystem->getName() << " as renderer." << std::endl;
+ mRoot->setRenderSystem(lRenderSystem);
+ }
+
+ mRoot->initialise(false);
+ }
+
+ bool fullscreen = false;
+ Ogre::NameValuePairList misc;
+ Ogre::ConfigOptionMap config = mRoot->getRenderSystem()->getConfigOptions();
+ Ogre::ConfigOptionMap::const_iterator it = config.begin();
+
+ while( it != config.end() ){
+ Ogre::String leftconf = (*it).first;
+ Ogre::String rightconf = (*it).second.currentValue;
+
+ if(leftconf == "Video Mode"){
+ if(canInit) {
+ int ret = sscanf(rightconf.c_str(), "%d %*s %d", &mWindowWidth, &mWindowHeight);
+ if (ret == 0)
+ std::cout << "Cannot read Ogre video mode" << std::endl;
+ }
+ else{
+ if(mWindowWidth == 0 && mWindowHeight == 0){
+ mWindowWidth = mBackgroundWidth;
+ mWindowHeight = mBackgroundHeight;
+ }
+ }
+ }
+ else if( leftconf == "Full Screen" ){
+ if(canInit){
+ if(rightconf == "Yes") fullscreen = true;
+ }
+ }
+ else
+ misc[leftconf] = rightconf;
+
+ it++;
+ }
+
+ // With Ogre version >= 1.8.1 we hide the window
+ if( hidden ){
+#if ( OGRE_VERSION >= (1 << 16 | 8 << 8 | 1) )
+ misc["hidden"] = "true";
+ windowHidden = true;
+#endif
+ }
+ mWindow = mRoot->createRenderWindow(name, mWindowWidth, mWindowHeight, fullscreen, &misc);
+
+ // Initialise resources
+ Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
+ //-----------------------------------------------------
+ // 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
+ //-----------------------------------------------------
+
+ mSceneMgr = mRoot->createSceneManager(Ogre::ST_GENERIC);
+
+ // Create the camera
+ createCamera();
+
+ // Create a viewport
+ Ogre::Viewport* viewPort = mWindow->addViewport(mCamera);
+// Ogre::Viewport* viewPort = mCamera->getViewport();
+ viewPort->setClearEveryFrame(true);
+ // Set the projection parameters to match the camera intrinsic parameters
+ updateCameraProjection();
+
+ // Create the 3D scene
+ createScene();
+
+ // Initialise and register event handlers
+ mRoot->addFrameListener(this);
+
+ // Register as a Window listener
+ Ogre::WindowEventUtilities::addWindowEventListener(mWindow, this);
+
+#ifdef VISP_HAVE_OIS
+ // Initialise OIS
+ Ogre::LogManager::getSingletonPtr()->logMessage("*** Initializing OIS ***");
+ OIS::ParamList pl;
+
+ size_t windowHnd = 0;
+ std::ostringstream windowHndStr;
+ // Initialise window
+ mWindow->getCustomAttribute("WINDOW", &windowHnd);
+ windowHndStr << windowHnd;
+ pl.insert(std::make_pair(std::string("WINDOW"), windowHndStr.str()));
+ // Let the user use the keyboard elsewhere
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ pl.insert(std::make_pair(std::string("x11_keyboard_grab"), std::string("false")));
+#endif
+
+ mInputManager = OIS::InputManager::createInputSystem( pl );
+
+ //Create all devices
+ // Here we only consider the keyboard input
+ mKeyboard = static_cast<OIS::Keyboard*>(mInputManager->createInputObject( OIS::OISKeyboard, bufferedKeys ));
+ if ( !bufferedKeys ) mKeyboard->setEventCallback ( this);
+#endif
+
+ // Initialise a render to texture to be able to retrieve a screenshot
+ Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
+ mWindow->getWidth(),mWindow->getHeight(), 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
+
+
+
+// Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
+// 640,480, 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
+ Ogre::RenderTexture* RTarget = Texture->getBuffer()->getRenderTarget();
+ /*Ogre::Viewport* Viewport =*/ RTarget->addViewport(mCamera);
+ RTarget->getViewport(0)->setClearEveryFrame(true);
+ RTarget->getViewport(0)->setOverlaysEnabled(false);
+}
+
+/*!
+ Destructor.
+*/
+vpAROgre::~vpAROgre(void)
+{
+ // Destroy 3D scene
+ destroyScene();
+ // Close OIS
+ closeOIS();
+
+ if ( mWindow) {
+ Ogre::WindowEventUtilities::removeWindowEventListener(mWindow, this);
+ windowClosed(mWindow);
+ }
+ // Delete root
+ if (mRoot) delete mRoot;
+}
+
+/*!
+ Function testing if the program must stop rendering or not.
+ \param evt : Frame event to process.
+ \return False if the program must be stopped.
+*/
+bool vpAROgre::stopTest(const Ogre::FrameEvent& evt)
+{
+ // Always keep this part
+ if(keepOn){
+ return updateScene(evt);
+ }
+ else
+ return keepOn;
+}
+
+/*!
+ Function telling what to do before each frame.
+ Calls customframeStarted() and see if the program has to be stopped.
+
+ This function is called before every frame is rendered by Ogre.
+
+ \param evt : Frame event to process.
+ \return True if everything went well.
+*/
+bool vpAROgre::frameStarted(const Ogre::FrameEvent& evt)
+{
+ // custom method telling what to do at the beginning of each frame
+ bool result = customframeStarted(evt);
+
+ // Listen to the window
+ Ogre::WindowEventUtilities::messagePump();
+ processInputEvent(evt);
+
+ // See if we have to stop rendering
+ if(result) return stopTest(evt);
+ else return result;
+}
+
+
+/*!
+ Function telling what to do after each frame.
+ Calls customframeEnded() and see if the program is to be stopped.
+ \param evt : Frame event to process.
+ \return True if everything went well.
+*/
+bool vpAROgre::frameEnded(const Ogre::FrameEvent& evt)
+{
+ // custom method telling what to do at the end of each frame
+ bool result = customframeEnded(evt);
+
+ // See if we have to stop rendering
+ if(result) return stopTest(evt);
+ else return result;
+}
+
+/*!
+
+ Function telling what to do before each frame. This method is called
+ before every frame rendered by Ogre.
+
+ \param evt : Frame event to process
+ \return True if everything went well
+*/
+bool vpAROgre::customframeStarted(const Ogre::FrameEvent& /*evt*/)
+{
+ // See if window was closed
+ if(mWindow->isClosed()) return false;
+
+#ifdef VISP_HAVE_OIS
+ // Get keyboard input
+ mKeyboard->capture();
+ if(mKeyboard->isKeyDown(OIS::KC_ESCAPE))
+ return false;
+#endif
+ return true;
+}
+
+
+/*!
+ Function telling what to do after each frame.
+ \param evt : Frame event to process.
+ \return True if everything went well.
+*/
+bool vpAROgre::customframeEnded(const Ogre::FrameEvent& /*evt*/){return true;}
+
+/*!
+
+ Unattach OIS (Object-oriented Input System library) before window shutdown
+ (very important under Linux).
+
+ \warning This method is only available if OIS is installed and detected
+ during ViSP configuration stage.
+
+ \param rw : RenderWindow to close.
+*/
+void vpAROgre::windowClosed(Ogre::RenderWindow* rw)
+{
+ //Only close for window that created OIS (the main window in these demos)
+ if( rw == mWindow ) closeOIS();
+}
+
+/*!
+ Render one frame.
+ \param I : Grey level image to show in background.
+ \param cMw : Camera pose as an homogeneous matrix.
+*/
+bool vpAROgre::renderOneFrame(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMw)
+{
+ // Update the background to match the situation
+ updateBackgroundTexture(I);
+
+ // Update the camera parameters to match the grabbed image
+ updateCameraParameters(cMw);
+
+ // Display on Ogre Window
+ return mRoot->renderOneFrame();
+}
+
+/*!
+ Render one frame.
+ \param I : RGBa image to show in background.
+ \param cMw : Camera pose as an homogeneous matrix.
+*/
+bool vpAROgre::renderOneFrame(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMw)
+{
+ // Update the background to match the situation
+ updateBackgroundTexture(I);
+
+ // Update the camera parameters to match the grabbed image
+ updateCameraParameters(cMw);
+
+ // Display on Ogre Window
+ return mRoot->renderOneFrame();
+}
+
+/*!
+ Display a frame.
+ \param I : Grey level image to show in background.
+ \param cMw : Camera pose as an homogeneous matrix.
+*/
+void vpAROgre::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMw)
+{
+ // Display on Ogre Window
+ if(renderOneFrame(I,cMw)){
+ mWindow->update();
+ keepOn = true;
+ }
+ else
+ keepOn = false;
+}
+
+/*!
+ Display a frame.
+ \param I : RGBa image to show in background.
+ \param cMw : Camera pose as an homogeneous matrix.
+*/
+void vpAROgre::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw)
+{
+ // Display on Ogre Window
+ if(renderOneFrame(I,cMw)){
+ mWindow->update();
+ keepOn = true;
+ }
+ else
+ keepOn = false;
+}
+
+/*!
+ Ask the program if we can continue to render.
+ \return True if nothing stopped the rendering loop.
+*/
+bool vpAROgre::continueRendering(void)
+{
+ return keepOn;
+}
+
+/*!
+ Set the camera intrinsic parameters
+*/
+void vpAROgre::setCameraParameters(const vpCameraParameters &cameraP )
+{
+ mcam = cameraP;
+}
+
+/*!
+ Load a mesh in the 3D world.
+ \param entityName : Name of the Entity and SceneNode to create.
+ \param model : 3D model to load.
+*/
+void vpAROgre::load(const std::string &entityName, const std::string &model)
+{
+ Ogre::Entity *newEntity = mSceneMgr->createEntity(entityName, model);
+ Ogre::SceneNode *newNode = mSceneMgr->getRootSceneNode()->createChildSceneNode(entityName);
+ newNode->attachObject(newEntity);
+}
+
+/*!
+ Change position of a ScneneNode.
+ \param sceneName : Name of the SceneNode to move.
+ \param wTo : New position of the node (translation between object frame and
+ world frame).
+*/
+void vpAROgre::setPosition(const std::string &sceneName,
+ const vpTranslationVector &wTo)
+{
+ // Reset the position
+ Ogre::SceneNode *node = mSceneMgr->getSceneNode(sceneName);
+ node->setPosition((Ogre::Real)wTo[0], (Ogre::Real)wTo[1], (Ogre::Real)wTo[2]);
+}
+
+/*!
+ Get position of a SceneNode.
+ \param sceneName : Name of the SceneNode in the scene graph.
+ \return The position of the node.
+*/
+vpTranslationVector vpAROgre::getPosition(const std::string &sceneName)const
+{
+ Ogre::Vector3 translation = mSceneMgr->getSceneNode(sceneName)->getPosition();
+ return vpTranslationVector((Ogre::Real)translation[0], (Ogre::Real)translation[1], (Ogre::Real)translation[2]);
+}
+
+/*!
+ Set the orientation of a SceneNode.
+ \param sceneName : Name of the SceneNode to rotate.
+ \param wRo : The rotation matrix representing the rotation to apply.
+*/
+void vpAROgre::setRotation(const std::string &sceneName, const vpRotationMatrix &wRo)
+{
+ // Get the node in its original position
+ mSceneMgr->getSceneNode(sceneName)->resetOrientation();
+ // Apply the new rotation
+ Ogre::Matrix3 rotationOgre
+ = Ogre::Matrix3( (Ogre::Real)wRo[0][0], (Ogre::Real)wRo[0][1], (Ogre::Real)wRo[0][2],
+ (Ogre::Real)wRo[1][0], (Ogre::Real)wRo[1][1], (Ogre::Real)wRo[1][2],
+ (Ogre::Real)wRo[2][0], (Ogre::Real)wRo[2][1], (Ogre::Real)wRo[2][2]);
+ Ogre::Quaternion q(rotationOgre);
+ mSceneMgr->getSceneNode(sceneName)->rotate(q);
+}
+
+/*!
+ Add a rotation to a SceneNode.
+ \param sceneName : Name of the SceneNode to rotate.
+ \param wRo : The rotation matrix representing the rotation to apply.
+*/
+void vpAROgre::addRotation(const std::string &sceneName, const vpRotationMatrix &wRo)
+{
+ // Apply the new rotation
+ Ogre::Matrix3 rotationOgre
+ = Ogre::Matrix3( (Ogre::Real)wRo[0][0], (Ogre::Real)wRo[0][1], (Ogre::Real)wRo[0][2],
+ (Ogre::Real)wRo[1][0], (Ogre::Real)wRo[1][1], (Ogre::Real)wRo[1][2],
+ (Ogre::Real)wRo[2][0], (Ogre::Real)wRo[2][1], (Ogre::Real)wRo[2][2]);
+ Ogre::Quaternion q(rotationOgre);
+ mSceneMgr->getSceneNode(sceneName)->rotate(q);
+}
+
+/*!
+ Set the position and the orientation of a SceneNode.
+ \param sceneName : Name of the SceneNode to rotate.
+
+ \param wMo : The homogeneous matrix representing the rotation and
+ translation to apply.
+
+*/
+void vpAROgre::setPosition(const std::string &sceneName, const vpHomogeneousMatrix &wMo)
+{
+ // Extract the position and orientation data
+ vpRotationMatrix rotations;
+ vpTranslationVector translation;
+ wMo.extract(rotations);
+ wMo.extract(translation);
+ // Apply them to the node
+ setPosition(sceneName, translation);
+ setRotation(sceneName, rotations);
+}
+
+/*!
+ Tell if a SceneNode is shown on the screen or not.
+ \param sceneName : Name of the SceneNode.
+ \param isVisible : If true we show the node, if false we don't.
+*/
+void vpAROgre::setVisibility(const std::string &sceneName, bool isVisible)
+{
+ mSceneMgr->getSceneNode(sceneName)->setVisible(isVisible);
+}
+
+/*!
+ Scale a SceneNode.
+ \param sceneName : Name of the SceneNode.
+ \param factorx : Scale factor along the x-axis.
+ \param factory : Scale factor along the x-axis.
+ \param factorz : Scale factor along the x-axis.
+*/
+void vpAROgre::setScale(const std::string &sceneName, const float factorx, const float factory, const float factorz)
+{
+ // Reset the scale to its original value
+ mSceneMgr->getSceneNode(sceneName)->scale(Ogre::Vector3(1,1,1)/mSceneMgr->getSceneNode(sceneName)->getScale());
+ // Apply the new scale
+ mSceneMgr->getSceneNode(sceneName)->scale(Ogre::Vector3(factorx, factory, factorz));
+}
+
+/*!
+ Create the Ogre camera.
+*/
+void vpAROgre::createCamera(void)
+{
+ mCamera = mSceneMgr->createCamera("Camera");
+}
+
+/*!
+ Create a greylevel background to show the real scene.
+
+ \param I : This parameter is here only used to initialize a grey level background.
+*/
+void vpAROgre::createBackground(vpImage<unsigned char> & /* I */)
+{
+ // Create a rectangle to show the incoming images from the camera
+ mBackground = new Ogre::Rectangle2D(true); // true = textured
+ mBackground->setCorners(-1.0, 1.0, 1.0, -1.0); // Spread all over the window
+ mBackground->setBoundingBox(Ogre::AxisAlignedBox(-100000.0*Ogre::Vector3::UNIT_SCALE, 100000.0*Ogre::Vector3::UNIT_SCALE)); // To be shown everywhere
+
+ // Texture options
+ Ogre::MaterialManager::getSingleton().setDefaultTextureFiltering(Ogre::TFO_NONE);
+ Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1);
+
+ // Dynamic texture
+ // If we are using opengl we can boost a little bit performances with a dynamic texture
+ if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
+ Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ mBackgroundWidth,//width
+ mBackgroundHeight,//height
+ 0, // num of mip maps
+ Ogre::PF_BYTE_L,
+ Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
+ }
+ else{
+ Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ mBackgroundWidth,//width
+ mBackgroundHeight,//height
+ 0, // num of mip maps
+ Ogre::PF_BYTE_L,
+ Ogre::TU_DEFAULT);
+ }
+
+ // Pointer to the dynamic texture
+ Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
+//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
+//#else
+// ;
+//#endif
+ mPixelBuffer = dynTexPtr->getBuffer();
+
+ // Material to apply the texture to the background
+ Ogre::MaterialPtr Backgroundmaterial
+ = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+// .dynamicCast<Ogre::Material>();
+//#else
+// ;
+//#endif
+ Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
+ Backgroundtechnique->createPass();
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
+ Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
+ mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
+ mBackground->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND); // To be rendered in Background
+
+ // Add the background to the Scene Graph so it will be rendered
+ Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode");
+ BackgroundNode->attachObject(mBackground);
+}
+
+/*!
+ Create a color background to show the real scene.
+
+ \param I : This parameter is here only used to initialize a color background.
+*/
+void vpAROgre::createBackground(vpImage<vpRGBa> & /* I */)
+{
+ // Create a rectangle to show the incoming images from the camera
+ mBackground = new Ogre::Rectangle2D(true); // true = textured
+ mBackground->setCorners(-1.0, 1.0, 1.0, -1.0); // Spread all over the window
+ mBackground->setBoundingBox(Ogre::AxisAlignedBox(-100000.0*Ogre::Vector3::UNIT_SCALE, 100000.0*Ogre::Vector3::UNIT_SCALE)); // To be shown everywhere
+
+ // Texture options
+ Ogre::MaterialManager::getSingleton().setDefaultTextureFiltering(Ogre::TFO_NONE);
+ Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1);
+
+ // Dynamic texture
+ // If we are using opengl we can boost a little bit performances with a dynamic texture
+ if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
+ Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ mBackgroundWidth,//width
+ mBackgroundHeight,//height
+ 0, // num of mip maps
+ //Ogre::PF_BYTE_RGBA,
+ Ogre::PF_BYTE_BGRA,
+ Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
+ }
+ else{ // As that texture does not seem to work properly with direct3D we use a default texture
+ Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ mBackgroundWidth,//width
+ mBackgroundHeight,//height
+ 0, // num of mip maps
+ //Ogre::PF_BYTE_RGBA,
+ Ogre::PF_BYTE_BGRA,
+ Ogre::TU_DEFAULT);
+ }
+
+
+ // Pointer to the dynamic texture
+ Ogre::TexturePtr dynTexPtr =
+ Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
+//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
+//#else
+// ;
+//#endif
+
+ // Get the pixel buffer
+ mPixelBuffer = dynTexPtr->getBuffer();
+
+ // Material to apply the texture to the background
+ Ogre::MaterialPtr Backgroundmaterial
+ = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+// .dynamicCast<Ogre::Material>();
+//#else
+// ;
+//#endif
+ Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
+ Backgroundtechnique->createPass();
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
+ Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
+ Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
+ mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
+ mBackground->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND); // To be rendered in Background
+
+ // Add the background to the Scene Graph so it will be rendered
+ Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode");
+ BackgroundNode->attachObject(mBackground);
+}
+
+/*!
+ Close the OIS based input manager.
+
+ \warning This method is only to call if OIS is installed and detected during
+ ViSP configuration stage.
+
+*/
+void vpAROgre::closeOIS(void)
+{
+#ifdef VISP_HAVE_OIS
+ if( mInputManager )
+ {
+ mInputManager->destroyInputObject( mKeyboard );
+
+ OIS::InputManager::destroyInputSystem(mInputManager);
+ mInputManager = 0;
+ }
+#endif
+}
+
+/*!
+ Update the projection parameters of the camera.
+*/
+// Note: equation taken from:
+// http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
+void vpAROgre::updateCameraProjection(void)
+{
+ if(mCamera != 0){
+ Ogre::Real f,n,f_m_n,f_p_n,px,py,u0,v0;
+ f = (Ogre::Real)(mFarClipping); // Far clip distance
+ n = (Ogre::Real)(mNearClipping); // Near clip distance
+ f_m_n = (Ogre::Real)(f-n);
+ f_p_n = (Ogre::Real)(f+n);
+ px = (Ogre::Real)mcam.get_px();
+ py = (Ogre::Real)mcam.get_py();
+ u0 = (Ogre::Real)mcam.get_u0();
+ v0 = (Ogre::Real)mcam.get_v0();
+ Ogre::Matrix4 Projection
+ = Ogre::Matrix4( (Ogre::Real)(2.0*px/mBackgroundWidth), 0, (Ogre::Real)(1.0 - 2.0*(u0/mBackgroundWidth)), 0,
+ 0, (Ogre::Real)(2.0*py/mBackgroundHeight), (Ogre::Real)(-1.0 + 2.0*(v0/mBackgroundHeight)),0,
+ 0, 0, (Ogre::Real)(-1.0*f_p_n/f_m_n), (Ogre::Real)(-2.0*f*n/f_m_n),
+ 0, 0, -1.0, 0);
+ mCamera->setCustomProjectionMatrix(true, Projection);
+ }
+}
+
+/*!
+ Update the texture we see in background with a grey level image.
+*/
+void vpAROgre::updateBackgroundTexture(const vpImage<unsigned char> &I)
+{
+ // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
+ // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
+ // performance than HBL_NORMAL
+ mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
+ const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
+ // Buffer data
+ Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
+ // Fill in the data in the grey level texture
+ memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth);
+
+ // Unlock the pixel buffer
+ mPixelBuffer->unlock();
+}
+
+/*!
+ Update the texture we see in background with a RGBa image.
+*/
+void vpAROgre::updateBackgroundTexture(const vpImage<vpRGBa> &I)
+{
+ // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
+ // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
+ // performance than HBL_NORMAL
+ mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
+ const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
+ // Buffer data
+ Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
+ // Fill in the data in the grey level texture
+#if 1 // if texture in BGRa format
+ for(unsigned int i=0; i<mBackgroundHeight; i++){
+ for(unsigned int j=0; j<mBackgroundWidth; j++){
+ // Color Image
+// *pDest++=I[i][mBackgroundWidth-j].B; // Blue component
+// *pDest++=I[i][mBackgroundWidth-j].G; // Green component
+// *pDest++=I[i][mBackgroundWidth-j].R; // Red component
+
+ *pDest++=I[i][j].B; // Blue component
+ *pDest++=I[i][j].G; // Green component
+ *pDest++=I[i][j].R; // Red component
+
+ *pDest++ = 255; // Alpha component
+ }
+ }
+#else // if texture in RGBa format which is the format of the input image
+ memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth*sizeof(vpRGBa));
+#endif
+
+ // Unlock the pixel buffer
+ mPixelBuffer->unlock();
+}
+
+/*!
+ Update Camera parameters from a pose calculation.
+*/
+void vpAROgre::updateCameraParameters (const vpHomogeneousMatrix &cMw)
+{
+ // The matrix is given to Ogre with some changes to fit with the world projection
+ Ogre::Matrix4 ModelView
+// = Ogre::Matrix4( (Ogre::Real)-cMo[0][0], (Ogre::Real)-cMo[0][1], (Ogre::Real)-cMo[0][2], (Ogre::Real)-cMo[0][3],
+ = Ogre::Matrix4( (Ogre::Real)cMw[0][0], (Ogre::Real)cMw[0][1], (Ogre::Real)cMw[0][2], (Ogre::Real)cMw[0][3],
+ (Ogre::Real)-cMw[1][0], (Ogre::Real)-cMw[1][1], (Ogre::Real)-cMw[1][2], (Ogre::Real)-cMw[1][3],
+ (Ogre::Real)-cMw[2][0], (Ogre::Real)-cMw[2][1], (Ogre::Real)-cMw[2][2], (Ogre::Real)-cMw[2][3],
+ (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)1);
+ mCamera->setCustomViewMatrix(true, ModelView);
+}
+
+/*!
+ Get the result of the rendering loop.
+
+ \param I : The image on which to copy the result of the rendering loop.
+ \param cMo : The desired camera pose.
+*/
+void vpAROgre::getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo)
+{
+ updateCameraParameters(cMo);
+ Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("rtf");
+//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+// .dynamicCast<Ogre::Texture>();
+//#else
+// ;
+//#endif
+ Ogre::RenderTexture* RTarget = dynTexPtr->getBuffer()->getRenderTarget();
+ mWindow->update();
+ RTarget->update();
+ if(I.getHeight() != mWindow->getHeight() || I.getWidth() != mWindow->getWidth()){
+ I.resize(mWindow->getHeight(), mWindow->getWidth());
+ }
+ Ogre::HardwarePixelBufferSharedPtr mPixelBuffer = dynTexPtr->getBuffer();
+ mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
+ const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
+ dynTexPtr->getBuffer()->blitToMemory(pixelBox);
+ Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
+#if 1 // if texture in BGRa format
+ for(unsigned int i=0; i<I.getHeight(); i++){
+ for(unsigned int j=0; j<I.getWidth(); j++){
+ // Color Image
+ I[i][j].B = *pDest++; // Blue component
+ I[i][j].G = *pDest++; // Green component
+ I[i][j].R = *pDest++; // Red component
+ I[i][j].A = *pDest++; // Alpha component
+ }
+ }
+#else // if texture in RGBa format which is the format of the input image
+ memcpy(I.bitmap, pDest, I.getHeight()*I.getWidth()*sizeof(vpRGBa));
+#endif
+
+ // Unlock the pixel buffer
+ mPixelBuffer->unlock();
+
+}
+
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_ar.a(vpAROgre.cpp.o) has no symbols
+void dummy_vpAROgre() {};
+#endif
+
diff --git a/modules/ar/src/vpSimulatorException.cpp b/modules/ar/src/vpSimulatorException.cpp
new file mode 100644
index 0000000..c236fd1
--- /dev/null
+++ b/modules/ar/src/vpSimulatorException.cpp
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the simulator classes.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/* \file vpSimulatorException.h
+ \brief error that can be emited by the vpSimulator class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/ar/vpSimulatorException.h>
+
+vpSimulatorException::vpSimulatorException (const int id, const char* format, ...)
+{
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+}
+
+vpSimulatorException::vpSimulatorException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+
+vpSimulatorException::vpSimulatorException (const int id)
+ : vpException(id){ ; }
diff --git a/modules/core/CMakeLists.txt b/modules/core/CMakeLists.txt
new file mode 100644
index 0000000..5d8c0f6
--- /dev/null
+++ b/modules/core/CMakeLists.txt
@@ -0,0 +1,251 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+#vp_define_module(core)
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+# winmm.lib for timeGetTime() under windows
+CHECK_LIBRARY_EXISTS("winmm.lib" getch "" HAVE_LIBWINMM) # for timeGetTime()
+if(HAVE_LIBWINMM)
+ list(APPEND opt_libs "winmm.lib")
+endif()
+
+# Add library ws2_32.a or ws2_32.lib for vpNetwork class
+if(WIN32 AND NOT CYGWIN)
+ if(MINGW)
+ CHECK_LIBRARY_EXISTS("ws2_32.a" getch "" HAVE_LIBWS2_32) # for inet_ntoa() and socket functionalities
+ if(HAVE_LIBWS2_32)
+ list(APPEND opt_libs "ws2_32.a")
+ else()
+ find_library(WS2_32_LIBRARY "libws2_32.a"
+ "$ENV{MINGW_DIR}/lib"
+ "$ENV{MINGW_DIR}/mingw/lib"
+ C:/mingw/mingw/lib)
+ mark_as_advanced(WS2_32_LIBRARY)
+ if(WS2_32_LIBRARY)
+ list(APPEND opt_libs "${WS2_32_LIBRARY}")
+ endif()
+ endif()
+ else() # pure WIN32
+ CHECK_LIBRARY_EXISTS("ws2_32.lib" getch "" HAVE_LIBWS2_32) # for inet_ntoa() and socket functionalities
+ if(HAVE_LIBWS2_32)
+ #message("have ws2_32.lib")
+ list(APPEND opt_libs "ws2_32.lib")
+ endif()
+ endif()
+endif()
+
+# OpenCV
+if(USE_OPENCV)
+ # On win32 since OpenCV 2.4.7 and on OSX with OpenCV 2.4.10 we cannot use OpenCV_LIBS to set ViSP 3rd party libraries.
+ # Using OpenCV_LIBS works to build visp library, examples, demos and test thanks to the components,
+ # but not tutorials that are stand alone Cmake project that use ViSP as a 3rd party.
+ # To be clear OpenCV_LIBS contains opencv_core and not c:\...\opencv_core248.lib full path as requested
+ # to use ViSP. This was not the case with OpenCV 2.4.6.
+ # For the build of ViSP it works with OpenCV_LIBS: in that case thanks to opencv_core properties, CMake
+ # is able to find the real name and location of the libraries.
+ # But when ViSP is used as a 3rd party where it should import OpenCV libraries, it doesn't work with
+ # OpenCV_LIBS.
+ # The solution here is to get the real name of OpenCV libraries thanks to the properties and link
+ # with these names.
+ # An other way could be to include OpenCVConfig.cmake, but in that case, visp-config and visp.pc
+ # will be not able to give the names of OpenCV libraries when used without CMake.
+ #message("OpenCV_LIB_COMPONENTS: ${OpenCV_LIB_COMPONENTS}")
+ #message("OpenCV_LIBS: ${OpenCV_LIBS}")
+ #if(WIN32 AND OpenCV_LIB_COMPONENTS AND OpenCV_VERSION AND OpenCV_VERSION VERSION_GREATER 2.4.6.1)
+ if(OpenCV_LIB_COMPONENTS AND OpenCV_VERSION AND OpenCV_VERSION VERSION_GREATER 2.4.6.1)
+ # ReleaseWithDebugInfo was requested to detect libopencv-devel under Fedora 20
+ # RelWithDebugInfo was requested to detect ros-hydro-opencv2 2.4.9 under Ubuntu 12.04 LTS with ROS hydro
+ set(config_ "NONE" "RELEASE" "DEBUG" "RELEASEWITHDEBINFO" "RELWITHDEBINFO")
+ if(POLICY CMP0045)
+ # Fix Error on non-existent target in get_target_property for 3rd party location extraction
+ cmake_policy(PUSH)
+ cmake_policy(SET CMP0045 OLD)
+ endif()
+
+ foreach(component_ ${OpenCV_LIB_COMPONENTS})
+ foreach(imp_config_ ${config_})
+ if(OpenCV_SHARED)
+ get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_IMPLIB_${imp_config_})
+ # particular case of opencv_ts that doesn't have an implib
+ if(NOT EXISTS "${component_property_${imp_config_}_}")
+ get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_LOCATION_${imp_config_})
+ endif()
+ else()
+ get_target_property(component_property_${imp_config_}_ ${component_} IMPORTED_LOCATION_${imp_config_})
+ endif()
+ get_target_property(component_property_3rdparty_${imp_config_}_ ${component_} IMPORTED_LINK_INTERFACE_LIBRARIES_${imp_config_})
+ #message("component_property_${imp_config_}_: ${component_property_${imp_config_}_}")
+ #message("component_property_3rdparty_${imp_config_}_: ${component_property_3rdparty_${imp_config_}_}")
+ # Under Unix, there is no specific suffix for OpenCV libraries. If one is found we add it
+ # Under Windows, we add the "optimized", "debug" specific keywords
+ if(WIN32 AND EXISTS "${component_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "RELEASE") # also valid for RELEASEWITHDEBINFO
+ list(APPEND opt_libs optimized "${component_property_${imp_config_}_}")
+ elseif(WIN32 AND EXISTS "${component_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "DEBUG")
+ list(APPEND opt_libs debug "${component_property_${imp_config_}_}")
+ elseif(EXISTS "${component_property_${imp_config_}_}")
+ list(APPEND opt_libs "${component_property_${imp_config_}_}")
+ endif()
+
+ if(component_property_3rdparty_${imp_config_}_)
+ foreach(3rdparty_ ${component_property_3rdparty_${imp_config_}_})
+ #message("3rdparty_ ${3rdparty_}")
+ list(FIND OpenCV_LIB_COMPONENTS ${3rdparty_} 3rdparty_is_opencv_component_)
+ if(3rdparty_is_opencv_component_ LESS 0)
+ #message("${3rdparty_} is not an opencv component")
+ get_target_property(3rdparty_opt_location_ ${3rdparty_} IMPORTED_LOCATION_${imp_config_})
+ if(NOT EXISTS "${3rdparty_opt_location_}")
+ #message("3rdparty_: ${3rdparty_} location doesn't exist in ${imp_config_}")
+ get_target_property(3rdparty_opt_location_ ${3rdparty_} IMPORTED_LOCATION)
+ #message("3rdparty_: ${3rdparty_} location : ${3rdparty_opt_location_}")
+ endif()
+ if(EXISTS "${3rdparty_opt_location_}")
+ #message("3rdparty_opt_location_: ${3rdparty_opt_location_} with config ${imp_config_}")
+ if(WIN32 AND "${imp_config_}" MATCHES "RELEASE")
+ #message("is release")
+ list(APPEND opt_libs optimized ${3rdparty_opt_location_})
+ elseif(WIN32 AND "${imp_config_}" MATCHES "DEBUG")
+ list(APPEND opt_libs debug ${3rdparty_opt_location_})
+ else()
+ list(APPEND opt_libs ${3rdparty_opt_location_})
+ endif()
+ else()
+ find_library(3rdparty_location_ NAMES ${3rdparty_})
+ mark_as_advanced(3rdparty_location_)
+ if(3rdparty_location_)
+ #message(${3rdparty_location_})
+ list(APPEND opt_libs ${3rdparty_location_}) # should be a system dependency
+ else()
+ list(APPEND opt_libs ${3rdparty_}) # should be a system dependency
+ endif()
+ endif()
+ endif()
+ endforeach()
+ endif()
+ endforeach()
+ endforeach()
+
+ if(POLICY CMP0045)
+ # Fix Error on non-existent target in get_target_property for 3rd party location extraction
+ cmake_policy(POP)
+ endif()
+
+ else()
+ # this should be an old OpenCV version that doesn't have the previous behavior
+ list(APPEND opt_libs ${OpenCV_LIBS})
+ endif()
+ list(APPEND opt_incs ${OpenCV_INCLUDE_DIRS})
+ #list(APPEND opt_libs ${OpenCV_LIBS})
+ #MESSAGE("OpenCV_INCLUDE_DIRS = ${OpenCV_INCLUDE_DIRS}")
+ #MESSAGE("OpenCV_LIBS = ${OpenCV_LIBS}")
+ #MESSAGE("OpenCV_LIB_DIR = ${OpenCV_LIB_DIR}")
+ #message("OpenCV_NONFREE_FOUND: ${OPENCV_NONFREE_FOUND}")
+endif(USE_OPENCV)
+
+
+
+if(USE_YARP)
+ list(APPEND opt_incs ${YARP_INCLUDE_DIRS})
+
+ # Work around to add Yarp libraries and also third party libraries requested by Yarp
+ list(REVERSE YARP_LIBRARIES) # to start with YARP_init, that depends on YARP_dev, YARP_sig and YARP_OS
+ foreach(lib ${YARP_LIBRARIES})
+ get_target_property(CONFIGURATIONS ${lib} IMPORTED_CONFIGURATIONS)
+ foreach(CONFIGURATION ${CONFIGURATIONS})
+ get_target_property(YARP_LIB ${lib} "IMPORTED_LOCATION_${CONFIGURATION}") # Get Yarp full absolute library path and name
+
+ if(WIN32)
+ #Work around when YARP is build as shared libraries
+ string(REGEX REPLACE ".dll$" ".lib" YARP_LIB ${YARP_LIB})
+
+ if(${CONFIGURATION} STREQUAL "RELEASE")
+ list(APPEND VISP_EXTERN_LIBRARIES optimized ${YARP_LIB}) # Append full absolute library path and name
+ elseif(${CONFIGURATION} STREQUAL "DEBUG")
+ list(APPEND VISP_EXTERN_LIBRARIES debug ${YARP_LIB}) # Append full absolute library path and name
+ endif()
+ else()
+ list(APPEND opt_libs ${YARP_LIB}) # Append full absolute library path and name
+ endif()
+ # Get 3rd party libraries requested by Yarp
+ get_target_property(YARP_LINK_LIBS_ ${lib} "IMPORTED_LINK_INTERFACE_LIBRARIES_${CONFIGURATION}")
+ list(APPEND YARP_LINK_LIBS ${YARP_LINK_LIBS_})
+ endforeach()
+ endforeach()
+
+ # Remove Yarp libraries since they were added previously with full absolute library path and name
+ if(YARP_LINK_LIBS)
+ foreach(lib ${YARP_LIBRARIES})
+ list(REMOVE_ITEM YARP_LINK_LIBS ${lib})
+ endforeach()
+ endif()
+ # Add 3rd party libraries requested by Yarp
+ list(APPEND opt_libs ${YARP_LINK_LIBS})
+ add_definitions(${YARP_DEFINES})
+endif(USE_YARP)
+
+# Math: gsl, lapack, OpenCV
+if(USE_GSL)
+ list(APPEND opt_incs ${GSL_INCLUDE_DIRS})
+ list(APPEND opt_libs ${GSL_LIBRARIES})
+endif()
+if(USE_LAPACK)
+ list(APPEND opt_libs ${LAPACK_C_LIBRARIES})
+endif()
+
+# Misc: xml, pthread, zlib
+if(USE_XML2)
+ list(APPEND opt_incs ${XML2_INCLUDE_DIRS})
+ list(APPEND opt_libs ${XML2_LIBRARIES})
+endif()
+if(USE_PTHREAD)
+ list(APPEND opt_incs ${PTHREAD_INCLUDE_DIRS})
+ list(APPEND opt_libs ${PTHREAD_LIBRARIES})
+endif()
+if(USE_ZLIB)
+ list(APPEND opt_incs ${ZLIB_INCLUDE_DIRS})
+ list(APPEND opt_libs ${ZLIB_LIBRARIES})
+endif()
+
+vp_add_module(core)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+vp_create_compat_headers("include/visp3/core/vpConfig.h")
+vp_add_tests(CTEST_EXCLUDE_PATH network DEPENDS_ON visp_io visp_gui)
diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h
new file mode 100644
index 0000000..8aa1fc1
--- /dev/null
+++ b/modules/core/include/visp3/core/vpArray2D.h
@@ -0,0 +1,652 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * This class implements an 2D array as a template class.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef __vpArray2D_h_
+#define __vpArray2D_h_
+
+#include <iostream>
+#include <ostream>
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fstream>
+#include <sstream>
+#include <limits>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+/*!
+ \class vpArray2D
+ \ingroup group_core_matrices
+
+ \brief Implementation of a generic 2D array used as vase class of matrices and vectors.
+
+ This class implements a 2D array as a template class and all the basic functionalities
+ common to matrices and vectors.
+ More precisely:
+ - concerning matrices, vpMatrix but also specific containers such as twist
+ (vpVelocityTwistMatrix and vpForceTwistMatrix), homogeneous (vpHomogeneousMatrix),
+ rotation (vpRotationMatrix) and homography (vpHomography) matrices inherit from
+ vpArray2D<double>.
+ - concerning vectors, vpColVector, vpRowVector but also specific containers describing
+ the pose (vpPoseVector) and the rotation (vpRotationVector) inherit also from
+ vpArray2D<double>.
+*/
+template<class Type>
+class vpArray2D
+{
+protected:
+ //! Number of rows in the array
+ unsigned int rowNum;
+ //! Number of columns in the array
+ unsigned int colNum;
+ //! Address of the first element of each rows
+ Type **rowPtrs;
+ //! Current array size (rowNum * colNum)
+ unsigned int dsize;
+
+public:
+ //! Address of the first element of the data array
+ Type *data;
+
+public:
+ /*!
+ Basic constructor of a 2D array.
+ Number of columns and rows are set to zero.
+ */
+ vpArray2D<Type>()
+ : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ {}
+ /*!
+ Copy constructor of a 2D array.
+ */
+ vpArray2D<Type>(const vpArray2D<Type> & A)
+ : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ {
+ resize(A.rowNum, A.colNum);
+ memcpy(data, A.data, rowNum*colNum*sizeof(Type));
+ }
+ /*!
+ Constructor that initializes a 2D array with 0.
+
+ \param r : Array number of rows.
+ \param c : Array number of columns.
+ */
+ vpArray2D<Type>(unsigned int r, unsigned int c)
+ : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ {
+ resize(r, c);
+ }
+ /*!
+ Constructor that initialize a 2D array with \e val.
+
+ \param r : Array number of rows.
+ \param c : Array number of columns.
+ \param val : Each element of the array is set to \e val.
+ */
+ vpArray2D<Type>(unsigned int r, unsigned int c, Type val)
+ : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ {
+ resize(r, c);
+ *this = val;
+ }
+ /*!
+ Destructor that desallocate memory.
+ */
+ virtual ~vpArray2D<Type>()
+ {
+ if (data != NULL ) {
+ free(data);
+ data=NULL;
+ }
+
+ if (rowPtrs!=NULL) {
+ free(rowPtrs);
+ rowPtrs=NULL ;
+ }
+ rowNum = colNum = dsize = 0;
+ }
+
+ /** @name Inherited functionalities from vpArray2D */
+ //@{
+
+ Type getMinValue() const;
+
+ Type getMaxValue() const;
+
+ //! Return the number of rows of the 2D array
+ inline unsigned int getRows() const { return rowNum ;}
+ //! Return the number of columns of the 2D array
+ inline unsigned int getCols() const { return colNum; }
+ //! Return the number of elements of the 2D array.
+ inline unsigned int size() const { return colNum*rowNum; }
+ /*!
+ Set the size of the array and initialize all the values to zero.
+
+ \param nrows : number of rows.
+ \param ncols : number of column.
+ \param flagNullify : if true, then the array is re-initialized to 0
+ after resize. If false, the initial values from the common part of the
+ array (common part between old and new version of the array) are kept.
+ Default value is true.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ if ((nrows == rowNum) && (ncols == colNum)) {
+ if (flagNullify && this->data != NULL) {
+ memset(this->data, 0, this->dsize*sizeof(Type));
+ }
+ }
+ else {
+ const bool recopyNeeded = (ncols != this ->colNum);
+ Type * copyTmp = NULL;
+ unsigned int rowTmp = 0, colTmp=0;
+
+ // Recopy case per case is required if number of cols has changed;
+ // structure of Type array is not the same in this case.
+ if (recopyNeeded && this->data != NULL) {
+ copyTmp = new Type[this->dsize];
+ memcpy (copyTmp, this ->data, sizeof(Type)*this->dsize);
+ rowTmp=this->rowNum; colTmp=this->colNum;
+ }
+
+ // Reallocation of this->data array
+ this->dsize = nrows*ncols;
+ this->data = (Type*)realloc(this->data, this->dsize*sizeof(Type));
+ if ((NULL == this->data) && (0 != this->dsize)) {
+ if (copyTmp != NULL) delete [] copyTmp;
+ throw(vpException(vpException::memoryAllocationError,
+ "Memory allocation error when allocating 2D array data")) ;
+ }
+
+ this->rowPtrs = (Type**)realloc (this->rowPtrs, nrows*sizeof(Type*));
+ if ((NULL == this->rowPtrs) && (0 != this->dsize)) {
+ if (copyTmp != NULL) delete [] copyTmp;
+ throw(vpException(vpException::memoryAllocationError,
+ "Memory allocation error when allocating 2D array rowPtrs")) ;
+ }
+
+ // Update rowPtrs
+ {
+ Type **t_= rowPtrs;
+ for (unsigned int i=0; i<dsize; i+=ncols) {
+ *t_++ = this->data + i;
+ }
+ }
+
+ this->rowNum = nrows; this->colNum = ncols;
+
+ // Recopy of this->data array values or nullify
+ if (flagNullify) {
+ memset(this->data,0,this->dsize*sizeof(Type));
+ }
+ else if (recopyNeeded && this->rowPtrs != NULL) {
+ // Recopy...
+ const unsigned int minRow = (this->rowNum<rowTmp)?this->rowNum:rowTmp;
+ const unsigned int minCol = (this->colNum<colTmp)?this->colNum:colTmp;
+ for (unsigned int i=0; i<this->rowNum; ++i) {
+ for (unsigned int j=0; j<this->colNum; ++j) {
+ if ((minRow > i) && (minCol > j)) {
+ (*this)[i][j] = copyTmp [i*colTmp+j];
+ }
+ else {
+ (*this)[i][j] = 0;
+ }
+ }
+ }
+ }
+
+ if (copyTmp != NULL)
+ delete [] copyTmp;
+ }
+ }
+ //! Set all the elements of the array to \e x.
+ vpArray2D<Type> & operator=(Type x)
+ {
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] = x;
+
+ return *this;
+ }
+
+ /*!
+ Copy operator of a 2D array.
+ */
+ vpArray2D<Type> & operator=(const vpArray2D<Type> & A)
+ {
+ resize(A.rowNum, A.colNum);
+ memcpy(data, A.data, rowNum*colNum*sizeof(Type));
+ return *this;
+ }
+
+ //! Set element \f$A_{ij} = x\f$ using A[i][j] = x
+ inline Type *operator[](unsigned int i) { return rowPtrs[i]; }
+ //! Get element \f$x = A_{ij}\f$ using x = A[i][j]
+ inline Type *operator[](unsigned int i) const {return rowPtrs[i];}
+
+ /*!
+ \relates vpArray2D
+ Writes the given array to the output stream and returns a reference to the output stream.
+ */
+ friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+ {
+ if (A.data == NULL)
+ return s;
+ std::ios_base::fmtflags original_flags = s.flags();
+
+ s.precision(10) ;
+ for (unsigned int i=0;i<A.getRows();i++) {
+ for (unsigned int j=0;j<A.getCols() -1;j++){
+ s << A[i][j] << " ";
+ }
+ // We don't add " " after the last row element
+ s << A[i][A.getCols() -1];
+ // We don't add a \n char on the end of the last array line
+ if (i < A.getRows()-1)
+ s << std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return s;
+ }
+ //@}
+
+ //---------------------------------
+ // Inherited array I/O Static Public Member Functions
+ //---------------------------------
+ /** @name Inherited I/O from vpArray2D with Static Public Member Functions */
+ //@{
+ /*!
+ Load a matrix from a file.
+
+ \param filename : Absolute file name.
+ \param A : Array to be loaded
+ \param binary : If true the matrix is loaded from a binary file, else from a text file.
+ \param header : Header of the file is loaded in this parameter.
+
+ \return Returns true if success.
+
+ \sa save()
+ */
+ static bool load(const std::string &filename, vpArray2D<Type> &A,
+ const bool binary = false, char *header = NULL)
+ {
+ std::fstream file;
+
+ if (!binary)
+ file.open(filename.c_str(), std::fstream::in);
+ else
+ file.open(filename.c_str(), std::fstream::in|std::fstream::binary);
+
+ if(!file) {
+ file.close();
+ return false;
+ }
+
+ else {
+ if (!binary) {
+ char c='0';
+ std::string h;
+ while ((c != '\0') && (c != '\n')) {
+ file.read(&c,1);
+ h+=c;
+ }
+ if (header != NULL)
+ strncpy(header, h.c_str(), h.size() + 1);
+
+ unsigned int rows, cols;
+ file >> rows;
+ file >> cols;
+
+ if (rows > (std::numeric_limits<unsigned int>::max)()
+ || cols > (std::numeric_limits<unsigned int>::max)())
+ throw vpException(vpException::badValue, "Array exceed the max size.");
+
+ A.resize(rows,cols);
+
+ Type value;
+ for(unsigned int i = 0; i < rows; i++) {
+ for(unsigned int j = 0; j < cols; j++) {
+ file >> value;
+ A[i][j] = value;
+ }
+ }
+ }
+ else {
+ char c='0';
+ std::string h;
+ while ((c != '\0') && (c != '\n')) {
+ file.read(&c,1);
+ h+=c;
+ }
+ if (header != NULL)
+ strncpy(header, h.c_str(), h.size() + 1);
+
+ unsigned int rows, cols;
+ file.read((char*)&rows,sizeof(unsigned int));
+ file.read((char*)&cols,sizeof(unsigned int));
+ A.resize(rows,cols);
+
+ Type value;
+ for(unsigned int i = 0; i < rows; i++) {
+ for(unsigned int j = 0; j < cols; j++) {
+ file.read((char*)&value,sizeof(Type));
+ A[i][j] = value;
+ }
+ }
+ }
+ }
+
+ file.close();
+ return true;
+ }
+ /*!
+ Load an array from a YAML-formatted file.
+
+ \param filename : absolute file name.
+ \param A : array to be loaded from the file.
+ \param header : header of the file is loaded in this parameter.
+
+ \return Returns true on success.
+
+ \sa saveYAML()
+
+ */
+ static bool loadYAML(const std::string &filename, vpArray2D<Type> &A, char *header = NULL)
+ {
+ std::fstream file;
+
+ file.open(filename.c_str(), std::fstream::in);
+
+ if(!file) {
+ file.close();
+ return false;
+ }
+
+ unsigned int rows = 0,cols = 0;
+ std::string h;
+ std::string line,subs;
+ bool inheader = true;
+ unsigned int i=0, j;
+ unsigned int lineStart = 0;
+
+ while ( getline (file,line) ) {
+ if(inheader) {
+ if(rows == 0 && line.compare(0,5,"rows:") == 0) {
+ std::stringstream ss(line);
+ ss >> subs;
+ ss >> rows;
+ }
+ else if (cols == 0 && line.compare(0,5,"cols:") == 0) {
+ std::stringstream ss(line);
+ ss >> subs;
+ ss >> cols;
+ }
+ else if (line.compare(0,5,"data:") == 0)
+ inheader = false;
+ else
+ h += line + "\n";
+ }
+ else {
+ // if i == 0, we just got out of the header: initialize matrix dimensions
+ if(i == 0) {
+ if(rows == 0 || cols == 0) {
+ file.close();
+ return false;
+ }
+ A.resize(rows, cols);
+ // get indentation level which is common to all lines
+ lineStart = (unsigned int)line.find("[") + 1;
+ }
+ std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
+ j = 0;
+ while(getline(ss, subs, ','))
+ A[i][j++] = atof(subs.c_str());
+ i++;
+ }
+ }
+
+ if (header != NULL)
+ strncpy(header, h.substr(0,h.length()-1).c_str(), h.size());
+
+ file.close();
+ return true;
+ }
+
+ /*!
+ Save a matrix to a file.
+
+ \param filename : Absolute file name.
+ \param A : Array to be saved.
+ \param binary : If true the matrix is saved in a binary file, else a text file.
+ \param header : Optional line that will be saved at the beginning of the file.
+
+ \return Returns true if success.
+
+ Warning : If you save the matrix as in a text file the precision is
+ less than if you save it in a binary file.
+
+ \sa load()
+ */
+ static bool save(const std::string &filename, const vpArray2D<Type> &A,
+ const bool binary = false, const char *header = "")
+ {
+ std::fstream file;
+
+ if (!binary)
+ file.open(filename.c_str(), std::fstream::out);
+ else
+ file.open(filename.c_str(), std::fstream::out|std::fstream::binary);
+
+ if(!file) {
+ file.close();
+ return false;
+ }
+
+ else {
+ if (!binary) {
+ unsigned int i = 0;
+ file << "# ";
+ while (header[i] != '\0') {
+ file << header[i];
+ if (header[i] == '\n')
+ file << "# ";
+ i++;
+ }
+ file << '\0';
+ file << std::endl;
+ file << A.getRows() << "\t" << A.getCols() << std::endl;
+ file << A << std::endl;
+ }
+ else {
+ int headerSize = 0;
+ while (header[headerSize] != '\0') headerSize++;
+ file.write(header,headerSize+1);
+ unsigned int matrixSize;
+ matrixSize = A.getRows();
+ file.write((char*)&matrixSize,sizeof(int));
+ matrixSize = A.getCols();
+ file.write((char*)&matrixSize,sizeof(int));
+ Type value;
+ for(unsigned int i = 0; i < A.getRows(); i++) {
+ for(unsigned int j = 0; j < A.getCols(); j++) {
+ value = A[i][j];
+ file.write((char*)&value,sizeof(Type));
+ }
+ }
+ }
+ }
+
+ file.close();
+ return true;
+ }
+ /*!
+ Save an array in a YAML-formatted file.
+
+ \param filename : absolute file name.
+ \param A : array to be saved in the file.
+ \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
+
+ \return Returns true if success.
+
+ Here is an example of outputs.
+ \code
+ vpArray2D<double> M(3,4);
+ vpArray2D::saveYAML("matrix.yml", M, "example: a YAML-formatted header");
+ vpArray2D::saveYAML("matrixIndent.yml", M, "example:\n - a YAML-formatted header\n - with inner indentation");
+ \endcode
+ Content of matrix.yml:
+ \code
+ example: a YAML-formatted header
+ rows: 3
+ cols: 4
+ data:
+ - [0, 0, 0, 0]
+ - [0, 0, 0, 0]
+ - [0, 0, 0, 0]
+ \endcode
+ Content of matrixIndent.yml:
+ \code
+ example:
+ - a YAML-formatted header
+ - with inner indentation
+ rows: 3
+ cols: 4
+ data:
+ - [0, 0, 0, 0]
+ - [0, 0, 0, 0]
+ - [0, 0, 0, 0]
+ \endcode
+
+ \sa loadYAML()
+ */
+ static bool saveYAML(const std::string &filename, const vpArray2D<Type> &A, const char *header = "")
+ {
+ std::fstream file;
+
+ file.open(filename.c_str(), std::fstream::out);
+
+ if(!file) {
+ file.close();
+ return false;
+ }
+
+ unsigned int i = 0;
+ bool inIndent = false;
+ std::string indent = "";
+ bool checkIndent = true;
+ while (header[i] != '\0') {
+ file << header[i];
+ if(checkIndent) {
+ if (inIndent) {
+ if(header[i] == ' ')
+ indent += " ";
+ else if (indent.length() > 0)
+ checkIndent = false;
+ }
+ if (header[i] == '\n' || (inIndent && header[i] == ' '))
+ inIndent = true;
+ else
+ inIndent = false;
+ }
+ i++;
+ }
+
+ if(i != 0)
+ file << std::endl;
+ file << "rows: " << A.getRows() << std::endl;
+ file << "cols: " << A.getCols() << std::endl;
+
+ if(indent.length() == 0)
+ indent = " ";
+
+ file << "data: " << std::endl;
+ unsigned int j;
+ for(i=0;i<A.getRows();++i)
+ {
+ file << indent << "- [";
+ for(j=0;j<A.getCols()-1;++j)
+ file << A[i][j] << ", ";
+ file << A[i][j] << "]" << std::endl;
+ }
+
+ file.close();
+ return true;
+ }
+ //@}
+};
+
+/*!
+ Return the array min value.
+ */
+template<class Type>
+Type
+vpArray2D<Type>::getMinValue() const
+{
+ Type *dataptr = data;
+ Type min = *dataptr;
+ dataptr++;
+ for (unsigned int i = 0; i < dsize-1; i++)
+ {
+ if (*dataptr < min) min = *dataptr;
+ dataptr++;
+ }
+ return min;
+}
+
+/*!
+ Return the array max value.
+ */
+template<class Type>
+Type
+vpArray2D<Type>::getMaxValue() const
+{
+ Type *dataptr = data;
+ Type max = *dataptr;
+ dataptr++;
+ for (unsigned int i = 0; i < dsize-1; i++)
+ {
+ if (*dataptr > max) max = *dataptr;
+ dataptr++;
+ }
+ return max;
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpBSpline.h b/modules/core/include/visp3/core/vpBSpline.h
new file mode 100644
index 0000000..1329e51
--- /dev/null
+++ b/modules/core/include/visp3/core/vpBSpline.h
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * This class implements the B-Spline
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpBSpline_H
+#define vpBSpline_H
+
+/*!
+ \file vpBSpline.h
+ \brief Class that provides tools to compute and manipulate a B-Spline curve.
+*/
+
+#include <visp3/core/vpImagePoint.h>
+
+#include <vector>
+#include <list>
+#include <visp3/core/vpMatrix.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ Structure that defines a B-Spline basis function \f$ N_{i,p}^k(u) \f$.
+
+ - i is the number of the knot interval in which the basis function is computed.
+ - p is the degree of the B-Spline basis function.
+ - u is the "point" point of the curve where the basis function is computed.
+ - k indicates which kth derivative is computed.
+ - value is the numerical value of \f$ N_{i,p}^k(u) \f$.
+*/
+typedef struct vpBasisFunction{
+ unsigned int i;
+ unsigned int p;
+ double u;
+ unsigned int k;
+ double value;
+} vpBasisFunction;
+#endif
+
+/*!
+ \class vpBSpline
+ \ingroup group_core_math_spline
+
+ \brief Class that provides tools to compute and manipulate a B-Spline curve.
+
+ The different parameters are :
+
+ - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1}, i = 0, ...,m \f$.
+ To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
+
+ - The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
+ \f[ N_{i,0}(u) = \left\{\begin{array}{cc}
+ 1 & \mbox{if } u_i \leq u \leq u_{i+1} \\ 0 & else
+ \end{array}\right.\f]
+
+ \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
+
+ where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
+
+ - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
+
+ It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
+ \f[ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)\f]
+
+ You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
+*/
+
+class VISP_EXPORT vpBSpline
+{
+ public/*protected*/:
+ //!Vector wich contains the control points
+ std::vector<vpImagePoint> controlPoints;
+ //! Vector which contain the knots \f$ {u0, ..., um} \f$
+ std::vector<double> knots;
+ //! Degree of the B-Spline basis functions.
+ unsigned int p;
+ //! Vector wich contains the points used during the interpolation method.
+ std::vector<vpImagePoint> crossingPoints;
+
+ public:
+
+ vpBSpline();
+ vpBSpline(const vpBSpline &bspline);
+ virtual ~vpBSpline();
+
+ /*!
+ Gets the degree of the B-Spline.
+
+ \return the degree of the B-Spline.
+ */
+ inline unsigned int get_p() const {return p;}
+
+ /*!
+ Gets all the control points.
+
+ \param list : A std::list containing the coordinates of the control points.
+ */
+ inline void get_controlPoints(std::list<vpImagePoint> &list) const {
+ list.clear();
+ for (unsigned int i = 0; i < controlPoints.size(); i++)
+ list.push_back(*(&(controlPoints[0])+i));
+ }
+
+ /*!
+ Gets all the knots.
+
+ \param list : A std::list containing the value of the knots.
+ */
+ inline void get_knots(std::list<double> &list) const {
+ list.clear();
+ for (unsigned int i = 0; i < knots.size(); i++)
+ list.push_back(*(&(knots[0])+i));
+ }
+
+ /*!
+ Gets all the crossing points (used in the interpolation method)
+
+ \param list : A std::list containing the coordinates of the crossing points.
+ */
+ inline void get_crossingPoints(std::list<vpImagePoint> &list) const {
+ list.clear();
+ for (unsigned int i = 0; i < crossingPoints.size(); i++)
+ list.push_back(*(&(crossingPoints[0])+i));
+ }
+
+
+ /*!
+ Sets the degree of the B-Spline.
+
+ \param degree : the degree of the B-Spline.
+ */
+ inline void set_p(unsigned int degree) {this->p = degree;}
+
+
+ /*!
+ Sets all the control points.
+
+ \param list : A std::list containing the coordinates of the control points
+ */
+ inline void set_controlPoints(const std::list<vpImagePoint> &list) {
+ controlPoints.clear();
+ for(std::list<vpImagePoint>::const_iterator it = list.begin(); it!=list.end(); ++it){
+ controlPoints.push_back(*it);
+ }
+ }
+
+ /*!
+ Sets all the knots.
+
+ \param list : A std::list containing the value of the knots.
+ */
+ inline void set_knots(const std::list<double> &list) {
+ knots.clear();
+ for(std::list<double>::const_iterator it = list.begin(); it!=list.end(); ++it){
+ knots.push_back(*it);
+ }
+ }
+
+ /*!
+ Sets all the crossing points (used in the interpolation method)
+
+ \param list : A std::list containing the coordinates of the crossing points
+ */
+ inline void set_crossingPoints(const std::list<vpImagePoint> &list) {
+ crossingPoints.clear();
+ for(std::list<vpImagePoint>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ crossingPoints.push_back(*it);
+ }
+ }
+
+ static unsigned int findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots);
+ unsigned int findSpan(double u);
+
+ static vpBasisFunction* computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots);
+ vpBasisFunction* computeBasisFuns(double u);
+
+ static vpBasisFunction** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots);
+ vpBasisFunction** computeDersBasisFuns(double u, unsigned int der);
+
+ static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
+ vpImagePoint computeCurvePoint(double u);
+
+ static vpImagePoint* computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
+ vpImagePoint* computeCurveDers(double u, unsigned int der);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpCameraParameters.h b/modules/core/include/visp3/core/vpCameraParameters.h
new file mode 100644
index 0000000..fec82cf
--- /dev/null
+++ b/modules/core/include/visp3/core/vpCameraParameters.h
@@ -0,0 +1,346 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Camera intrinsic parameters.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpCameraParameters.h
+ \brief Declaration of the vpCameraParameters class.
+ Class vpCameraParameters define the camera intrinsic parameters
+
+*/
+
+#ifndef vpCameraParameters_H
+#define vpCameraParameters_H
+
+#include <vector>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+
+/*!
+ \class vpCameraParameters
+
+ \ingroup group_core_camera
+
+ \brief Generic class defining intrinsic camera parameters.
+
+ Let us define the pinhole camera model implemented in ViSP. In this model, a scene view is formed
+ by projecting 3D points into the image plane using a perspective transformation.
+
+ \f[
+ \left[ \begin{array}{c}
+ u \\
+ v \\
+ 1
+ \end{array}\right] =
+ \left[ \begin{array}{ccc}
+ u_0 & 0 & p_x \\
+ 0 & v_0 & p_y \\
+ 0 & 0 & 1
+ \end{array}\right]
+ \left[ \begin{array}{c}
+ X_c \\
+ Y_c \\
+ Z_c
+ \end{array}\right]
+ \f]
+
+ where:
+
+ - \f$(X_c,Y_c,Z_c)\f$ are the coordinates of a 3D point in the camera frame
+ - \f$(u,v)\f$ are the coordinates in pixels of the projected 3D point
+ - \f$(u_0,v_0)\f$ is a principal point that is usually near the image center
+ - \f$(p_x,p_y)\f$ are the focal lengths expressed in pixel units.
+
+ When \f$Z_c \neq 0\f$, the previous equation si equivalent to the following:
+ \f[
+ \begin{array}{lcl}
+ x &=& X_c / Z_c \\
+ y &=& Y_c / Z_c \\
+ u &=& u_0 + x \; p_x \\
+ v &=& v_0 + y \; p_y
+ \end{array}
+ \f]
+
+ Real lenses usually have some radial distortion. So, the above model is extended as:
+
+ \f[
+ \begin{array}{lcl}
+ x &=& X_c / Z_c \\
+ y &=& Y_c / Z_c \\
+ x^{'} &=& x (1 + k_{ud} r^2) \\
+ y^{'} &=& y (1 + k_{ud} r^2) \\
+ r^2 &=& x^2 + y^2 \\
+ u &=& u_0 + x^{'} \; p_x \\
+ v &=& v_0 + y^{'} \; p_y
+ \end{array}
+ \f]
+
+ where \f$k_{ud}\f$ is the first order radial distorsion. Higher order distorsion coefficients are not considered in ViSP.
+
+ Now in ViSP we consider also the inverse transformation, where from pixel coordinates we want to compute their
+ normalized coordinates in the image plane. Previous equations could be written like:
+
+ \f[
+ \begin{array}{lcl}
+ x &=& (u - u_0) / p_x \\
+ y &=& (v - v_0) / p_y
+ \end{array}
+ \f]
+
+ Considering radial distortion, the above model is extended as:
+ \f[
+ \begin{array}{lcl}
+ (u-u_0)^{'} &=& (u-u_0) (1 + k_{du} r^2) \\
+ (v-v_0)^{'} &=& (v-v_0) (1 + k_{du} r^2) \\
+ r^2 &=& ((u-u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \\
+ x &=& (u - u_0)^{'} / p_x \\
+ y &=& (v - v_0)^{'} / p_y
+ \end{array}
+ \f]
+
+ Finally, in ViSP the main intrinsic camera parameters are \f$(p_x, p_y)\f$ the ratio
+ between the focal length and the size of a pixel, and \f$(u_0,
+ v_0)\f$ the coordinates of the principal point in pixel. The lens
+ distortion can also be considered by two additional parameters
+ \f$(k_{ud}, k_{du})\f$.
+
+ \note \ref tutorial-calibration shows how to calibrate a camera
+ to estimate the parameters corresponding to the model implemented in this class.
+
+ \note Note also that \ref tutorial-bridge-opencv gives the correspondance between
+ ViSP and OpenCV camera modelization.
+
+ \note The conversion from pixel coordinates \f$(u,v)\f$ in the normalized
+ space \f$(x,y)\f$ is implemented in vpPixelMeterConversion, whereas
+ the conversion from normalized coordinates into pixel is implemented
+ in vpMeterPixelConversion.
+
+ From a practical point of view, two kinds of camera modelisation are implemented in this class:
+
+ <b>1. Camera parameters for a perspective projection without distortion model</b>
+
+ In this modelisation, only \f$u_0,v_0,p_x,p_y\f$ parameters are considered.
+
+ Initialization of such a model can be done using:
+ - initPersProjWithoutDistortion() that allows to set \f$u_0,v_0,p_x,p_y\f$ parameters;
+ - initFromFov() that computes the parameters from an image size and a camera field of view.
+
+ <b>2. Camera parameters for a perspective projection with distortion model</b>
+
+ In this modelisation, all the parameters \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$
+ are considered. Initialization of such a model can be done using:
+ - initPersProjWithDistortion() that allows to set \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$ parameters;
+
+ The selection of the camera model (without or with distorsion) is done during
+ vpCameraParameters initialisation.
+
+ Here an example of camera initialisation, for a model without distortion. A complete example is given in initPersProjWithoutDistortion().
+ \code
+ double px = 600;
+ double py = 600;
+ double u0 = 320;
+ double v0 = 240;
+
+ // Create a camera parameter container
+ vpCameraParameters cam;
+ // Camera initialization with a perspective projection without distortion model
+ cam.initPersProjWithoutDistortion(px,py,u0,v0);
+ // It is also possible to print the current camera parameters
+ std::cout << cam << std::endl;
+ \endcode
+
+ Here an example of camera initialisation, for a model with distortion. A complete example is given in initPersProjWithDistortion().
+ \code
+ double px = 600;
+ double py = 600;
+ double u0 = 320;
+ double v0 = 240;
+ double kud = -0.19;
+ double kdu = 0.20;
+
+ // Create a camera parameter container
+ vpCameraParameters cam;
+
+ // Camera initialization with a perspective projection without distortion model
+ cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
+ \endcode
+
+ The code below shows how to know the currently used projection model:
+ \code
+ vpCameraParameters cam;
+ ...
+ vpCameraParameters::vpCameraParametersProjType projModel;
+ projModel = cam.get_projModel(); // Get the projection model type
+ \endcode
+
+ An XML parser for camera parameters is also provided in vpXmlParserCamera.
+
+*/
+class VISP_EXPORT vpCameraParameters
+{
+ friend class vpMeterPixelConversion;
+ friend class vpPixelMeterConversion;
+public :
+ typedef enum{
+ perspectiveProjWithoutDistortion, //!< Perspective projection without distortion model
+ perspectiveProjWithDistortion //!< Perspective projection with distortion model
+ } vpCameraParametersProjType ;
+
+
+ //generic functions
+ vpCameraParameters() ;
+ vpCameraParameters(const vpCameraParameters &c) ;
+ vpCameraParameters(const double px, const double py,
+ const double u0, const double v0) ;
+ vpCameraParameters(const double px, const double py,
+ const double u0, const double v0,
+ const double kud, const double kdu) ;
+
+ vpCameraParameters& operator =(const vpCameraParameters &c) ;
+ virtual ~vpCameraParameters() ;
+
+ void init() ;
+ void init(const vpCameraParameters &c) ;
+ void initFromCalibrationMatrix(const vpMatrix& _K);
+ void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov);
+ void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0);
+ void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0,
+ const double kud,const double kdu) ;
+
+ /*!
+ Specify if the fov has been computed.
+
+ \sa computeFov()
+
+ \return True if the fov has been computed, False otherwise.
+ */
+ inline bool isFovComputed() const { return isFov; }
+
+ void computeFov(const unsigned int &w, const unsigned int &h);
+
+ /*!
+ Get the horizontal angle in radian of the field of view.
+
+ \return FOV horizontal angle computed with px and width.
+
+ \sa computeFov(), getVerticalFovAngle()
+ */
+ inline double getHorizontalFovAngle() const {
+ if(!isFov) vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() won't be significant.");
+ return m_hFovAngle;
+ }
+
+ /*!
+ Get the vertical angle in radian of the field of view.
+
+ \return FOV vertical angle computed with py and height.
+
+ \sa computeFov(), getHorizontalFovAngle()
+ */
+ inline double getVerticalFovAngle() const {
+ if(!isFov) vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't be significant.");
+ return m_vFovAngle;
+ }
+
+ /*!
+ Get the list of the normals corresponding to planes describing the field of view.
+ - vector[0] : Left Normal.
+ - vector[1] : Right Normal.
+ - vector[2] : Up Normal.
+ - vector[3] : Down Normal.
+
+ \sa computeFov()
+
+ \return List of the normals.
+ */
+ inline std::vector<vpColVector> getFovNormals() const {
+ if(!isFov) vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be significant.");
+ return fovNormals;
+ }
+
+ inline double get_px() const { return px; }
+ inline double get_px_inverse() const {return inv_px; }
+ inline double get_py_inverse() const {return inv_py; }
+ inline double get_py() const { return py; }
+ inline double get_u0() const { return u0; }
+ inline double get_v0() const { return v0; }
+ inline double get_kud() const { return kud; }
+ inline double get_kdu() const { return kdu; }
+
+ inline vpCameraParametersProjType get_projModel() const {
+ return projModel;
+ }
+
+ vpMatrix get_K() const;
+ vpMatrix get_K_inverse() const;
+
+ void printParameters() ;
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpCameraParameters &cam);
+
+private:
+ static const double DEFAULT_U0_PARAMETER;
+ static const double DEFAULT_V0_PARAMETER;
+ static const double DEFAULT_PX_PARAMETER;
+ static const double DEFAULT_PY_PARAMETER;
+ static const double DEFAULT_KUD_PARAMETER;
+ static const double DEFAULT_KDU_PARAMETER;
+ static const vpCameraParametersProjType DEFAULT_PROJ_TYPE;
+
+
+ double px, py ; //!< pixel size
+ double u0, v0 ; //!< principal point
+ double kud ; //!< radial distortion (from undistorted to distorted)
+ double kdu ; //!< radial distortion (from distorted to undistorted)
+
+ unsigned int width ; //!< Width of the image used for the fov computation
+ unsigned int height ; //!< Height of the image used for the fov computation
+ bool isFov ; //!< Boolean to specify if the fov has been computed
+ double m_hFovAngle ; //!< Field of view horizontal angle
+ double m_vFovAngle ; //!< Field of view vertical angle
+ std::vector<vpColVector> fovNormals ; //!< Normals of the planes describing the fov
+
+ double inv_px, inv_py;
+
+ vpCameraParametersProjType projModel ; //!< used projection model
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpCircle.h b/modules/core/include/visp3/core/vpCircle.h
new file mode 100644
index 0000000..aa5cca3
--- /dev/null
+++ b/modules/core/include/visp3/core/vpCircle.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature circle.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpCircle.h
+ \brief class that defines what is a circle
+*/
+
+#ifndef vpCircle_hh
+#define vpCircle_hh
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <math.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpForwardProjection.h>
+
+/*!
+ \class vpCircle
+ \ingroup group_core_geometry
+ \brief Class that defines what is a circle.
+*/
+class VISP_EXPORT vpCircle : public vpForwardProjection
+{
+public:
+ void init() ;
+ vpCircle() ;
+ vpCircle(const vpColVector& oP) ;
+ vpCircle(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+ virtual ~vpCircle() ;
+
+
+ void setWorldCoordinates(const vpColVector& oP) ;
+ void setWorldCoordinates(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+
+
+ double getA() const { return cP[0] ; }
+ double getB() const{ return cP[1] ; }
+ double getC() const { return cP[2] ; }
+
+ double getX() const { return cP[3] ; }
+ double getY() const { return cP[4] ; }
+ double getZ() const{ return cP[5] ; }
+
+ double getR() const { return cP[6] ; }
+
+
+
+ void projection() ;
+ void projection(const vpColVector &cP, vpColVector &p) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo) ;
+
+
+ void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ vpCircle *duplicate() const ;
+
+ //###################
+ // Static Functions
+ //###################
+
+ public:
+ static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j);
+
+} ;
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpClient.h b/modules/core/include/visp3/core/vpClient.h
new file mode 100644
index 0000000..f492f68
--- /dev/null
+++ b/modules/core/include/visp3/core/vpClient.h
@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Client
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpClient_H
+#define vpClient_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpRequest.h>
+#include <visp3/core/vpNetwork.h>
+#include <visp3/core/vpTime.h>
+
+
+/*!
+ \class vpClient
+
+ \ingroup group_core_network
+
+ \brief This class represents a Transmission Control Protocol (TCP) client.
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
+ on one computer to another program on another computer.
+
+ Exemple of client's code, receiving and sending basic message
+ It corresponds to the client used in the first exemple of vpServer class' documentation:
+
+ \code
+#include <visp3/core/vpClient.h>
+#include <iostream>
+
+int main(int argc, char **argv)
+{
+ std::string servername = "localhost";
+ unsigned int port = 35000;
+
+ vpClient client;
+ client.connectToHostname(servername, port);
+ //client.connectToIP("127.0.0.1",port);
+
+ int val = 0;
+
+ while(1)
+ {
+ if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
+ std::cout << "Error while sending" << std::endl;
+ else
+ std::cout << "Sending : " << val << std::endl;
+
+ if(client.receive(&val) != sizeof(int)) //Receiving a value from the first client
+ std::cout << "Error while receiving" << std::endl;
+ else
+ std::cout << "Received : " << val << std::endl;
+ }
+
+ return 0;
+}
+ \endcode
+
+ Exemple of client's code, sending a vpImage on request form.
+ It correspond to the server used in the second exemple of vpServer class' documentation.
+
+ \code
+#include <visp3/core/vpClient.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <iostream>
+
+#include "vpRequestImage.h" //See vpRequest class documentation
+
+int main(int argc, char **argv)
+{
+#if defined(VISP_HAVE_V4L2)
+ std::string servername = "localhost";
+ unsigned int port = 35000;
+
+ vpImage<unsigned char> I; // Create a gray level image container
+
+ // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
+ vpV4l2Grabber g;
+ g.setScale(1);
+ g.setInput(0);
+ g.open(I);
+
+ // Create an image viewer
+#if defined(VISP_HAVE_X11)
+ vpDisplayX d(I, -1, -1, "Camera frame");
+#elif defined(VISP_HAVE_GDI) //Win32
+ vpDisplayGDI d(I, -1, -1, "Camera frame");
+#endif
+
+ vpClient client;
+ client.connectToHostname(servername, port);
+ //client.connectToIP("127.0.0.1",port);
+
+ vpRequestImage reqImage(&I);
+
+ while(1)
+ {
+ double t = vpTime::measureTimeMs();
+ // Acquire a new image
+ g.acquire(I);
+
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ client.sendAndEncodeRequest(reqImage);
+
+ // A click in the viewer to exit
+ if ( vpDisplay::getClick(I, false) )
+ break;
+ }
+
+ return 0;
+#endif
+}
+ \endcode
+
+ \sa vpClient
+ \sa vpRequest
+ \sa vpNetwork
+*/
+class VISP_EXPORT vpClient : public vpNetwork
+{
+private:
+ //######## PARAMETERS ########
+ //# #
+ //############################
+
+ unsigned int numberOfAttempts;
+
+ //######## Private Functions ########
+ //# #
+ //###################################
+
+ bool connectServer(vpNetwork::vpReceptor &serv);
+
+public:
+ vpClient();
+ virtual ~vpClient();
+
+ bool connectToHostname(const std::string &hostname, const unsigned int &port_serv);
+ bool connectToIP(const std::string &ip, const unsigned int &port_serv);
+
+ void deconnect(const unsigned int &index = 0);
+ /*!
+ Get the actual number of attempts to connect to the server.
+
+ \sa vpClient::setNumberOfAttempts()
+
+ \return Actual number of attempts.
+ */
+ unsigned int getNumberOfAttempts(){ return numberOfAttempts; }
+
+ /*!
+ Get the number of server that the client is connected on.
+
+ \return Number of servers.
+ */
+ unsigned int getNumberOfServers(){ return (unsigned int)receptor_list.size(); }
+
+ void print();
+
+ /*!
+ Set the number of attempts to connect to the server.
+
+ \sa vpClient::getNumberOfAttempts()
+
+ \param nb : Number of attempts.
+ */
+ void setNumberOfAttempts(const unsigned int &nb){ numberOfAttempts = nb; }
+
+ void stop();
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h
new file mode 100644
index 0000000..2a88c93
--- /dev/null
+++ b/modules/core/include/visp3/core/vpColVector.h
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Provide some simple operation on column vectors.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpColVector_H
+#define vpColVector_H
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpRowVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpPoseVector.h>
+
+class vpMatrix;
+class vpRowVector;
+class vpRotationVector;
+class vpTranslationVector;
+class vpPoseVector;
+
+/*!
+ \file vpColVector.h
+ \brief definition of column vector class as well
+ as a set of operations on these vector
+*/
+
+/*!
+ \class vpColVector
+ \ingroup group_core_matrices
+
+ \brief Implementation of column vector and the associated operations.
+
+ This class provides a data structure for a column vector that contains values of double.
+ It contains also some functions to achieve a set of operations on these vectors.
+
+ The vpColVector class is derived from vpArray2D<double>.
+*/
+class VISP_EXPORT vpColVector : public vpArray2D<double>
+{
+ friend class vpMatrix;
+public:
+
+ //! Basic constructor that creates an empty 0-size column vector.
+ vpColVector() : vpArray2D<double>() {};
+ //! Construct a column vector of size n. All the elements are initialized to zero.
+ vpColVector(unsigned int n) : vpArray2D<double>(n,1){};
+ //! Construct a column vector of size n. Each element is set to \e val.
+ vpColVector(unsigned int n, double val) : vpArray2D<double>(n, 1, val){};
+ //! Copy constructor that allows to construct a column vector from an other one.
+ vpColVector(const vpColVector &v) : vpArray2D<double>(v) {};
+ vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) ;
+ //! Constructor that initialize a column vector from a 3-dim (Euler or \f$\theta {\bf u}\f$)
+ //! or 4-dim (quaternion) rotation vector.
+ vpColVector(const vpRotationVector &v);
+ //! Constructor that initialize a column vector from a 6-dim pose vector.
+ vpColVector(const vpPoseVector &p);
+ //! Constructor that initialize a column vector from a 3-dim translation vector.
+ vpColVector(const vpTranslationVector &t);
+ vpColVector(const vpMatrix &M);
+ vpColVector(const vpMatrix &M, unsigned int j);
+ vpColVector(const std::vector<double> &v);
+ vpColVector(const std::vector<float> &v);
+ /*!
+ Destructor.
+ */
+ virtual ~vpColVector() {};
+
+ /*!
+ Removes all elements from the vector (which are destroyed),
+ leaving the container with a size of 0.
+ */
+ void clear()
+ {
+ if (data != NULL ) {
+ free(data);
+ data=NULL;
+ }
+
+ if (rowPtrs!=NULL) {
+ free(rowPtrs);
+ rowPtrs=NULL ;
+ }
+ rowNum = colNum = dsize = 0;
+ }
+
+ /*!
+ Convert a column vector containing angles in degrees into radians.
+ \sa rad2deg()
+ */
+ inline void deg2rad() {
+ double d2r = M_PI/180.0;
+
+ (*this) *= d2r;
+ }
+
+ double euclideanNorm() const;
+ /*!
+ Extract a sub-column vector from a column vector.
+ \param r : Index of the row corresponding to the first element of the vector to extract.
+ \param colsize : Size of the vector to extract.
+ \exception vpException::fatalError If the vector to extract is not contained in the original one.
+
+ \code
+ vpColVector v1;
+ for (unsigned int i=0; i<4; i++)
+ v1.stack(i);
+ // v1 is equal to [0 1 2 3]^T
+ vpColVector v2 = v1.extract(1, 3);
+ // v2 is equal to [1 2 3]^T
+ \endcode
+ */
+ vpColVector extract(unsigned int r, unsigned int colsize) const
+ {
+ if (r >= rowNum || r+colsize > rowNum) {
+ throw(vpException(vpException::fatalError,
+ "Cannot extract a (%dx1) column vector from a (%dx1) column vector starting at index %d",
+ colsize, rowNum, r));
+ }
+
+ return vpColVector(*this, r, colsize);
+ }
+
+ double infinityNorm() const;
+ void init(const vpColVector &v, unsigned int r, unsigned int nrows);
+ void insert(unsigned int i, const vpColVector &v);
+
+ vpColVector &normalize() ;
+ vpColVector &normalize(vpColVector &x) const ;
+
+ //! Operator that allows to set a value of an element \f$v_i\f$: v[i] = x
+ inline double &operator[](unsigned int n) { return *(data + n); }
+ //! Operator that allows to get the value of an element \f$v_i\f$: x = v[i]
+ inline const double &operator[](unsigned int n) const { return *(data+n); }
+ //! Copy operator. Allow operation such as A = v
+ vpColVector &operator=(const vpColVector &v);
+ vpColVector &operator=(const vpPoseVector &p);
+ vpColVector &operator=(const vpRotationVector &rv);
+ vpColVector &operator=(const vpTranslationVector &tv);
+ vpColVector &operator=(const vpMatrix &M);
+ vpColVector &operator=(const std::vector<double> &v);
+ vpColVector &operator=(const std::vector<float> &v);
+ vpColVector &operator=(double x);
+
+ double operator*(const vpColVector &x) const;
+ vpMatrix operator*(const vpRowVector &v) const;
+ vpColVector operator*(const double x) const;
+ vpColVector &operator*=(double x);
+
+ vpColVector operator/(const double x) const;
+ vpColVector &operator/=(double x);
+
+ vpColVector operator+(const vpColVector &v) const;
+ vpColVector &operator+=(vpColVector v);
+
+ vpColVector operator-(const vpColVector &v) const;
+ vpColVector &operator-=(vpColVector v);
+ vpColVector operator-() const;
+
+ vpColVector &operator<<(const vpColVector &v);
+ vpColVector &operator<<(double *);
+
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+
+ /*!
+ Convert a column vector containing angles in radians into degrees.
+ \sa deg2rad()
+ */
+ inline void rad2deg() {
+ double r2d = 180.0/M_PI;
+
+ (*this) *= r2d;
+ }
+
+ void reshape(vpMatrix & M, const unsigned int &nrows, const unsigned int &ncols);
+ vpMatrix reshape(const unsigned int &nrows, const unsigned int &ncols);
+
+ /*! Modify the size of the column vector.
+ \param i : Size of the vector. This value corresponds to the vector number
+ of rows.
+ \param flagNullify : If true, set the data to zero.
+ \exception vpException::fatalError When \e ncols is not equal to 1.
+ */
+ void resize(const unsigned int i, const bool flagNullify = true)
+ {
+ vpArray2D<double>::resize(i, 1, flagNullify);
+ }
+ /*!
+ Resize the column vector to a \e nrows-dimension vector.
+ This function can only be used with \e ncols = 1.
+ \param nrows : Vector number of rows. This value corresponds
+ to the size of the vector.
+ \param ncols : Vector number of columns. This value should be set to 1.
+ \param flagNullify : If true, set the data to zero.
+ \exception vpException::fatalError When \e ncols is not equal to 1.
+
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify)
+ {
+ if (ncols != 1)
+ throw(vpException(vpException::fatalError,
+ "Cannot resize a column vector to a (%dx%d) dimension vector that has more than one column",
+ nrows, ncols));
+ vpArray2D<double>::resize(nrows, ncols, flagNullify);
+ };
+
+ void stack(const double &d);
+ void stack(const vpColVector &v);
+
+ double sumSquare() const;
+ vpRowVector t() const;
+ vpRowVector transpose() const;
+ void transpose(vpRowVector &v) const;
+
+ /*!
+ Compute and return the cross product of two 3-dimension vectors: \f$a \times b\f$.
+ \param a : 3-dimension column vector.
+ \param b : 3-dimension column vector.
+ \return The cross product \f$a \times b\f$.
+
+ \exception vpException::dimensionError If the vectors dimension is not equal to 3.
+
+ */
+ inline static vpColVector cross(const vpColVector &a, const vpColVector &b)
+ {
+ return crossProd(a,b);
+ }
+ static vpColVector crossProd(const vpColVector &a, const vpColVector &b) ;
+
+ static double dotProd(const vpColVector &a, const vpColVector &b) ;
+ static vpColVector invSort(const vpColVector &v) ;
+ static double median(const vpColVector &v) ;
+ static double mean(const vpColVector &v) ;
+ // Compute the skew matrix [v]x
+ static vpMatrix skew(const vpColVector &v);
+
+ static vpColVector sort(const vpColVector &v) ;
+
+ static vpColVector stack(const vpColVector &A, const vpColVector &B);
+ static void stack(const vpColVector &A, const vpColVector &B, vpColVector &C);
+
+ static double stdev(const vpColVector &v, const bool useBesselCorrection=false);
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use extract().
+ */
+ vp_deprecated vpColVector rows(unsigned int first_row, unsigned int last_row) const
+ {
+ return vpColVector(*this, first_row-1, last_row-first_row+1);
+ }
+ /*!
+ \deprecated You should rather use eye()
+ */
+ vp_deprecated void setIdentity(const double & val=1.0) ;
+ /*!
+ \deprecated You should rather use stack(const vpColVector &)
+ */
+ vp_deprecated void stackMatrices(const vpColVector &r) { stack(r); };
+ /*!
+ \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B)
+ */
+ vp_deprecated static vpColVector stackMatrices(const vpColVector &A, const vpColVector &B) { return stack(A, B); };
+ /*!
+ \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
+ */
+ vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { stack(A, B, C); };
+
+ //@}
+#endif
+};
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+VISP_EXPORT
+#endif
+vpColVector operator*(const double &x, const vpColVector &v) ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpColor.h b/modules/core/include/visp3/core/vpColor.h
new file mode 100644
index 0000000..4c25b4c
--- /dev/null
+++ b/modules/core/include/visp3/core/vpColor.h
@@ -0,0 +1,319 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Color definition.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpColor_hh
+#define vpColor_hh
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+
+ \class vpColor
+
+ \ingroup group_core_gui
+
+ \brief Class to define colors available for display functionnalities.
+
+ You may use predefined colors (black, white, red, green, blue, cyan
+ and orange) or specify your own color by settings its R,G,B values.
+
+ An identifier vpColorIdentifier is associated to each color. This
+ identifier is useful to determine if a color is predefined or
+ specified by it R,G,B values. In that last case, the identifier is
+ always set to vpColor::id_unknown.
+
+ The example below shows how to display geometric features in a
+ display overlay using predefined colors (here the blue color to draw
+ a circle) and a specific brown color (used to draw a rectangle).
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320); // Create a black grey level image
+
+ vpDisplay *d;
+
+ // Depending on the detected third party libraries, we instantiate here the
+ // first video device which is available
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a filled circle with the predefined blue color
+ vpDisplay::displayCircle(I, 100, 200, 30, vpColor::blue, true);
+
+ // Creation of a new brown color with its RGB values
+ vpColor color(128, 100, 50);
+
+ // Draw a brown rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, color, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ delete d;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpColor : public vpRGBa
+{
+ public:
+ /*! Predefined colors identifier. */
+ typedef enum {
+ id_black=0, /*!< Identifier associated to the predefined vpColor::black color. */
+ id_white, /*!< Identifier associated to the predefined vpColor::white color. */
+ id_lightGray, /*!< Identifier associated to the predefined vpColor::lightGray color. */
+ id_gray, /*!< Identifier associated to the predefined vpColor::gray color. */
+ id_darkGray, /*!< Identifier associated to the predefined vpColor::darkGray color. */
+ id_lightRed, /*!< Identifier associated to the predefined vpColor::lightRed color. */
+ id_red, /*!< Identifier associated to the predefined vpColor::red color. */
+ id_darkRed, /*!< Identifier associated to the predefined vpColor::darkRed color. */
+ id_lightGreen, /*!< Identifier associated to the predefined vpColor::lightGreen color. */
+ id_green, /*!< Identifier associated to the predefined vpColor::green color. */
+ id_darkGreen, /*!< Identifier associated to the predefined vpColor::darkGreen color. */
+ id_lightBlue, /*!< Identifier associated to the predefined vpColor::lightBlue color. */
+ id_blue, /*!< Identifier associated to the predefined vpColor::blue color. */
+ id_darkBlue, /*!< Identifier associated to the predefined vpColor::darkBlue color. */
+ id_yellow, /*!< Identifier associated to the predefined vpColor::yellow color. */
+ id_cyan, /*!< Identifier associated to the predefined vpColor::cyan color. */
+ id_orange, /*!< Identifier associated to the predefined vpColor::orange color. */
+ id_purple, /*!< Identifier associated to the predefined vpColor::purple color. */
+
+ id_unknown /*!< Identifier associated to unknowned
+ colors. By unknowned, we mean not a predefined
+ color. This identifier can also be used to know
+ the number of predefined colors. */
+
+ } vpColorIdentifier;
+
+ vpColorIdentifier id; /*!< Color identifier to indicate if a color
+ is predefined or set by the user using its
+ RGB values. */
+
+ /* Predefined colors. */
+ static const vpColor black ;
+ static const vpColor white ;
+ static const vpColor lightGray;
+ static const vpColor gray;
+ static const vpColor darkGray;
+ static const vpColor lightRed;
+ static const vpColor red ;
+ static const vpColor darkRed;
+ static const vpColor lightGreen;
+ static const vpColor green;
+ static const vpColor darkGreen;
+ static const vpColor lightBlue;
+ static const vpColor blue ;
+ static const vpColor darkBlue;
+ static const vpColor yellow ;
+ static const vpColor cyan;
+ static const vpColor orange;
+ static const vpColor purple;
+ static const vpColor none;
+
+ static const unsigned int nbColors;
+ static const vpColor allColors[];
+
+ /*!
+ Default constructor. All the colors components are set to zero.
+
+ The color identifier is set to vpColor::id_unknown to indicate
+ that this color is not a predefined one.
+
+ */
+ inline vpColor()
+ : vpRGBa(), id(id_unknown)
+ {};
+ /*!
+ Construct a color from its RGB values.
+
+ \param r : Red component.
+ \param g : Green component.
+ \param b : Blue component.
+
+ \param cid : The color identifier to indicate if this color is or
+ not a predefined one.
+ */
+ inline vpColor(unsigned char r, unsigned char g, unsigned char b,
+ vpColor::vpColorIdentifier cid=vpColor::id_unknown)
+ : vpRGBa(r, g, b), id(cid)
+ {};
+ /*! Default destructor. */
+ inline virtual ~vpColor() {};
+
+ friend VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 );
+ friend VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 );
+ /*!
+ Set a color from its RGB values.
+
+ \param r : Red component.
+ \param g : Green component.
+ \param b : Blue component.
+
+ The color identifier is set to vpColor::id_unknown to indicate
+ that this color is not a predefined one.
+
+ */
+ inline void setColor(unsigned char r, unsigned char g, unsigned char b) {
+ this->R = r;
+ this->G = g;
+ this->B = b;
+ this->A = 0;
+ id = id_unknown;
+ };
+
+ /*!
+
+ Get a predefined color
+
+ \param i : color indice
+ */
+ static inline vpColor getColor(const unsigned int &i) {
+ return vpColor::allColors[i % vpColor::nbColors];
+ };
+
+};
+
+// In this file if windows
+#if defined(VISP_USE_MSVC) && defined(visp_EXPORTS)
+/*!< Predefined black color with R=G=B=0 and identifier vpColor::id_black. */
+vpColor const __declspec(selectany) vpColor::black = vpColor(0, 0, 0, id_black);
+/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white. */
+vpColor const __declspec(selectany) vpColor::white = vpColor(255, 255, 255, id_white);
+/*!< Predefined light gray color with R=G=B=64 and identifier vpColor::id_lightGray. */
+vpColor const __declspec(selectany) vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
+/*!< Predefined gray color with R=G=B=128 and identifier vpColor::id_gray. */
+vpColor const __declspec(selectany) vpColor::gray = vpColor(128, 128, 128, id_gray);
+/*!< Predefined dark gray color with R=G=B=192 and identifier vpColor::id_darkGray. */
+vpColor const __declspec(selectany) vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
+/*!< Predefined light red color with R= 255 and G=B=140 and identifier
+ vpColor::id_lightRed. */
+vpColor const __declspec(selectany) vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
+/*!< Predefined red color with R=255 and G=B=0 and identifier
+ vpColor::id_red. */
+vpColor const __declspec(selectany) vpColor::red = vpColor(255, 0, 0, id_red);
+/*!< Predefined dark red color with R= 128 and G=B=0 and identifier
+ vpColor::id_darkRed. */
+vpColor const __declspec(selectany) vpColor::darkRed = vpColor(128, 0, 0, id_darkRed);
+/*!< Predefined light green color with G= 255 and R=B=140 and identifier
+ vpColor::id_lightGreen. */
+vpColor const __declspec(selectany) vpColor::lightGreen = vpColor(140, 255, 140, id_lightGreen);
+/*!< Predefined green color with G=255 and R=B=0 and identifier
+ vpColor::id_green. */
+vpColor const __declspec(selectany) vpColor::green = vpColor(0, 255, 0, id_green);
+/*!< Predefined dark green color with G= 128 and R=B=0 and identifier
+ vpColor::id_darkGreen. */
+vpColor const __declspec(selectany) vpColor::darkGreen = vpColor(0, 128, 0, id_darkGreen);
+/*!< Predefined light blue color with B= 255 and R=G=140 and identifier
+ vpColor::id_lightBlue. */
+vpColor const __declspec(selectany) vpColor::lightBlue = vpColor(140, 140, 255, id_lightBlue);
+/*!< Predefined blue color with R=G=0 and B=255 and identifier
+ vpColor::id_blue. */
+vpColor const __declspec(selectany) vpColor::blue = vpColor(0, 0, 255, id_blue);
+/*!< Predefined dark blue color with B= 128 and R=G=0 and identifier
+ vpColor::id_darkBlue. */
+vpColor const __declspec(selectany) vpColor::darkBlue = vpColor(0, 0, 128, id_darkBlue);
+/*!< Predefined yellow color with R=G=255 and B=0 and identifier
+ vpColor::id_yellow. */
+vpColor const __declspec(selectany) vpColor::yellow = vpColor(255, 255, 0, id_yellow);
+/*!< Predefined cyan color with R=0 and G=B=255 and identifier
+ vpColor::id_cyan. */
+vpColor const __declspec(selectany) vpColor::cyan = vpColor(0, 255, 255, id_cyan);
+/*!< Predefined orange color with R=255, G=165 and B=0 and identifier
+ vpColor::id_orange. */
+vpColor const __declspec(selectany) vpColor::orange = vpColor(255, 165, 0, id_orange);
+/*!< Predefined purple color with R=128, G=0 and B=128 and identifier
+ vpColor::id_purple. */
+vpColor const __declspec(selectany) vpColor::purple = vpColor(128, 0, 128, id_purple);
+/*!< Predefined none color with R=G=B=0 and identifier vpColor::id_unknown. */
+vpColor const __declspec(selectany) vpColor::none = vpColor(0, 0, 0, id_unknown);
+
+const __declspec(selectany) unsigned int vpColor::nbColors = 18;
+
+/*!< Array of available colors. */
+vpColor const __declspec(selectany) vpColor::allColors[vpColor::nbColors] = {
+ vpColor::blue , // 12
+ vpColor::green , // 9
+ vpColor::red , // 6
+ vpColor::cyan , // 15
+ vpColor::purple , // 4
+ vpColor::yellow , // 14
+ vpColor::orange , // 16
+ vpColor::lightBlue ,// 11
+ vpColor::lightGreen,// 8
+ vpColor::lightRed , // 5
+ vpColor::darkBlue , // 13
+ vpColor::darkGreen ,// 10
+ vpColor::darkRed , // 7
+ vpColor::lightGray ,// 2
+ vpColor::gray , // 3
+ vpColor::darkGray , // 4
+ vpColor::black , // 0
+ vpColor::white}; // 17
+
+#endif
+
+#endif
diff --git a/modules/core/include/visp3/core/vpConvert.h b/modules/core/include/visp3/core/vpConvert.h
new file mode 100644
index 0000000..8153259
--- /dev/null
+++ b/modules/core/include/visp3/core/vpConvert.h
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Directory management.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpConvert_h__
+#define __vpConvert_h__
+
+/*!
+ \file vpConvert.h
+ \brief Tools for type or general conversion.
+ */
+
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ #include <opencv2/core/core.hpp>
+ #include <opencv2/features2d/features2d.hpp>
+#endif
+
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpPoint.h>
+
+/*!
+ \class vpConvert
+ \ingroup group_core_bridges
+ Bridges over other frameworks like OpenCV.
+ */
+class VISP_EXPORT vpConvert
+{
+
+public:
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ static void convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to);
+ static void convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to);
+ static void convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to);
+ static void convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame=false);
+ static void convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame=false);
+
+ static void convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to);
+ static void convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to);
+ static void convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to);
+ static void convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
+ static void convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
+ static void convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to);
+
+ static void convertToOpenCV(const vpImagePoint &from, cv::Point2f &to);
+ static void convertToOpenCV(const vpImagePoint &from, cv::Point2d &to);
+ static void convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame=false);
+ static void convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame=false);
+
+ static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to);
+ static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to);
+ static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame=false);
+ static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame=false);
+
+private:
+ static vpImagePoint keyPointToVpImagePoint(const cv::KeyPoint &keypoint);
+ static vpImagePoint point2fToVpImagePoint(const cv::Point2f &point);
+ static vpImagePoint point2dToVpImagePoint(const cv::Point2d &point);
+ static vpPoint point3fToVpObjectPoint(const cv::Point3f &point3f);
+ static vpPoint point3dToVpObjectPoint(const cv::Point3d &point3d);
+ static vpPoint point3fToVpCamPoint(const cv::Point3f &point3f);
+ static vpPoint point3dToVpCamPoint(const cv::Point3d &point3d);
+ static int dMatchToTrainIndex(const cv::DMatch &match);
+
+ static cv::Point2f vpImagePointToPoint2f(const vpImagePoint &point);
+ static cv::Point2d vpImagePointToPoint2d(const vpImagePoint &point);
+ static cv::Point3f vpCamPointToPoint3f(const vpPoint &point);
+ static cv::Point3d vpCamPointToPoint3d(const vpPoint &point);
+ static cv::Point3f vpObjectPointToPoint3f(const vpPoint &point);
+ static cv::Point3d vpObjectPointToPoint3d(const vpPoint &point);
+
+#endif
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpCylinder.h b/modules/core/include/visp3/core/vpCylinder.h
new file mode 100644
index 0000000..6bda1c6
--- /dev/null
+++ b/modules/core/include/visp3/core/vpCylinder.h
@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Cylinder feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpCylinder.h
+ \brief class that defines what is a cylinder
+*/
+
+#ifndef vpCylinder_hh
+#define vpCylinder_hh
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <math.h>
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpForwardProjection.h>
+
+/*!
+ \class vpCylinder
+ \ingroup group_core_geometry
+ \brief Class that defines what is a cylinder.
+
+ A cylinder may be represented by the equation:
+ \f$ (X - X_0)^2 + (Y - Y_0)^2 + (Z - Z_0)^2 - (A \; X + B \; Y + C \; Z)^2 - R^2 = 0 \f$
+ with
+
+ \f$
+ \left\{ \begin{array}{l}
+ A^2 + B^2 + C^2 = 1 \\
+ A \; X_0 + B \; Y_0 + C \; Z_0 = 0
+ \end{array} \right.
+ \f$
+
+ where \f$R\f$ is the radius of the cylinder, \f$A, B, C\f$ are the coordinates of
+ its direction vector and \f$X_0, Y_0, Z_0\f$ are the coordinates
+ of the nearest point belonging to the cylinder axis from the projection center.
+
+ Setting the cylinder parameters is achieved throw the constructors with parameters
+ or the setWorldCoordinates() methods.
+
+ Considering the set of parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
+ expressed in the world frame, cylinder coordinates expressed in the camera
+ frame are obtained using changeFrame().
+
+ The projection of a cylinder on the image plane is (for
+ non-degenerated cases) a set of two straight lines with equation:
+
+ \f$
+ \left\{ \begin{array}{lll}
+ x \;\cos\theta_1 + x \;\sin\theta_1 - \rho_1 = 0 \\
+ y \;\cos\theta_2 + y \;\sin\theta_2 - \rho_2 = 0
+ \end{array} \right.
+ \f$
+
+ The projection is achieved using projection() methods. The methods getRho1(), getTheta1() and
+ getRho2(), getTheta2() allow to access to the projected line parameters.
+*/
+class VISP_EXPORT vpCylinder : public vpForwardProjection
+{
+public:
+ typedef enum
+ {
+ line1, /*!< First limb of the cylinder. */
+ line2 /*!< Second limb of the cylinder. */
+ } vpLineCylinderType;
+
+ vpCylinder() ;
+ vpCylinder(const vpColVector& oP) ;
+ vpCylinder(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+ virtual ~vpCylinder() ;
+
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo) ;
+
+ void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+
+ vpCylinder *duplicate() const ;
+
+ /*!
+ Return the \f$\rho_1\f$ parameter of the line corresponding to the
+ projection of the cylinder in the image plane.
+ \sa getTheta1()
+ */
+ double getRho1() const { return p[0] ; }
+ /*!
+ Return the \f$\theta_1\f$ parameter of the line corresponding to the
+ projection of the cylinder in the image plane.
+ \sa getRho1()
+ */
+ double getTheta1() const { return p[1] ; }
+
+ /*!
+ Return the \f$\rho_2\f$ parameter of the line corresponding to the
+ projection of the cylinder in the image plane.
+ \sa getTheta2()
+ */
+ double getRho2() const { return p[2] ; }
+ /*!
+ Return the \f$\theta_2\f$ parameter of the line corresponding to the
+ projection of the cylinder in the image plane.
+ \sa getRho2()
+ */
+ double getTheta2() const { return p[3] ; }
+
+ /*!
+ Return cylinder \f$A\f$ parameter expressed in the camera frame.
+ */
+ double getA() const { return cP[0] ; }
+ /*!
+ Return cylinder \f$B\f$ parameter expressed in the camera frame.
+ */
+ double getB() const { return cP[1] ; }
+ /*!
+ Return cylinder \f$C\f$ parameter expressed in the camera frame.
+ */
+ double getC() const { return cP[2] ; }
+ /*!
+ Return cylinder \f$X_0\f$ parameter expressed in the camera frame.
+ */
+ double getX() const { return cP[3] ; }
+ /*!
+ Return cylinder \f$Y_0\f$ parameter expressed in the camera frame.
+ */
+ double getY() const { return cP[4] ; }
+ /*!
+ Return cylinder \f$Z_0\f$ parameter expressed in the camera frame.
+ */
+ double getZ() const { return cP[5] ; }
+ /*!
+ Return cylinder \f$R\f$ parameter corresponding to the cylinder radius.
+ */
+ double getR() const { return cP[6] ; }
+
+ void init() ;
+
+ void projection() ;
+ void projection(const vpColVector &cP, vpColVector &p) ;
+
+ void setWorldCoordinates(const vpColVector& oP) ;
+ void setWorldCoordinates(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+} ;
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpDebug.h b/modules/core/include/visp3/core/vpDebug.h
new file mode 100644
index 0000000..bf3f3e3
--- /dev/null
+++ b/modules/core/include/visp3/core/vpDebug.h
@@ -0,0 +1,550 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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.
+ * - 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
+ * 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
+ *
+ *****************************************************************************/
+
+#ifndef __VP_DEBUG_HH
+#define __VP_DEBUG_HH
+
+#include <visp3/core/vpConfig.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <iostream>
+
+
+#if defined(_WIN32)
+# ifndef __FUNCTION__
+# define __FUNCTION__ " "
+# endif
+#endif
+
+#ifndef VP_DEBUG_MODE
+# define VP_DEBUG_MODE 0
+#endif
+
+
+/*!
+ \class vpTraceOutput
+
+ \ingroup group_core_debug
+
+ \brief This class is used to display debug or error messages.
+
+ It needs to be initialized with the file name, function name and
+ line, of the place where it is created. It is best used by first
+ instanciating the object and directly calling the () operator. This
+ is used to mimic variadic macros.
+
+ This class is used to define the following macros:
+
+ - Macros for tracing: vpTRACE(), vpERROR_TRACE(), vpIN_FCT() and
+ vpOUT_FCT() work like printf
+ with carreer return at the end of the string, while vpCTRACE() and
+ vpCERROR() work like the C++ output streams std::cout and
+ std::cerr.
+
+ - Macros for debuging: vpDEBUG_TRACE(level) and vpDERROR_TRACE(level)
+ work like printf, but print only if the tracing level \e level is
+ greater than the debug level VP_DEBUG_MODE macro. vpCDEBUG(level)
+ work like the C++ output stream std::cout. vpDEBUG_ENABLE(level) is
+ equal to 1 if the debug level level is greater than the debug mode
+ VP_DEBUG_MODE, 0 else.
+
+ The example below shows how to use these macros.
+
+ \code
+#define VP_TRACE // Activate the trace mode
+#define VP_DEBUG // Activate the debug mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ vpIN_FCT("main()");
+
+ // Check the active debug levels
+ std::cout << "Debug level 1 active: " << vpDEBUG_ENABLE(1) << std::endl;
+ std::cout << "Debug level 2 active: " << vpDEBUG_ENABLE(2) << std::endl;
+ std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
+
+ // C-like debug printings
+ vpTRACE("C-like trace"); // stdout
+
+ // Printing depend only VP_DEBUG_MODE value is >= 1
+ vpTRACE(1, "C-like trace level 1"); // stdout
+ vpERROR_TRACE(1, "C-like error trace level 1"); // stderr
+
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
+ vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
+
+ // C++-like debug printings
+ vpCTRACE << "C++-like trace" << std::endl; // stdout
+ vpCERROR << "C++-like error trace" << std::endl; // stderr
+
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
+
+ vpOUT_FCT("main()");
+}
+ \endcode
+
+*/
+class vpTraceOutput
+{
+ private:
+ const char* currentFile; //Name of the file to use in the displays
+ const char* currentFunc; //Name of the function to use in the displays
+ int currentLine; //Line to use in the displays
+
+ //if true, output to std::cerr/stderr else std::cout/stdout
+ bool err;
+ //string to display before anything else
+ const char* header;
+
+ public:
+ /*!
+ Constructor.
+ \param file Should be the name of the file where this constructor was called.
+ \param line Should be the line in file where this constructor was called.
+ \param func Should be the name of the function where this constructor was called.
+ \param error If true, writes to the error stream.
+ \param s String to print before any other message (acts like a header).
+ \note Call the constructor with something like vpTraceOutput(__FILE__,__LINE__, __FUNCTION__).
+ */
+ vpTraceOutput(const char* file, int line, const char* func, bool error=false, const char * s=NULL) :
+ currentFile(file),
+ currentFunc(func),
+ currentLine(line),
+ err(error),
+ header(s)
+ {}
+
+ /*!
+ Displays a string if the debug level is inferior to VP_DEBUG_MODE.
+ \param level Level of this message.
+ \param format Formating string.
+ */
+ void operator()(int level, const char* format, ...)
+ {
+ //if the level is inferior to VP_DEBUG_MODE
+ if(VP_DEBUG_MODE >= level)
+ {
+ //gets the variable list of arguments
+ va_list args;
+ va_start(args, format);
+
+ if (err)
+ std::cerr << "(L" << level << ") " ;
+ else
+ std::cout << "(L" << level << ") " ;
+
+ //calls display with it
+ display(format, args);
+
+ va_end(args);
+ }
+ }
+
+ /*!
+ Displays a string.
+ \param format Formating string.
+ */
+ void operator()(const char* format, ...)
+ {
+ //gets the variable list of arguments
+ va_list args;
+ va_start(args, format);
+
+#ifdef VP_DEBUG
+ std::cout<<"(L0) ";
+#endif
+
+ //calls display with it
+ display(format, args);
+
+ va_end(args);
+ }
+
+ /*!
+
+ Displays a message to either stdout or
+ stderr (based on error boolean).
+
+ \param format Formating string.
+ \param args List of arguments.
+
+ */
+ void display(const char* format, va_list args)
+ {
+ //if we want to write to std::cerr/stderr
+ if(err)
+ {
+ //first writes the header if there is one
+ if(header != NULL) std::cerr<<header;
+ //then writes the recorded namefile, function and line
+ std::cerr << "!!\t" << currentFile << ": " <<currentFunc << "(#" << currentLine << ") : " ;
+ //and finally writes the message passed to () operator.
+ vfprintf (stderr, format, args);
+ fprintf (stderr, "\n");
+ //flushes the buffer
+ fflush (stderr);
+ }
+ else
+ {
+ //first writes the header if there is one
+ if(header != NULL) std::cout<<header;
+ //then writes the recorded namefile, function and line
+ std::cout <<currentFile << ": " << currentFunc << "(#" << currentLine << ") : " ;
+ //and finally writes the message passed to () operator.
+ vprintf (format, args);
+ printf ("\n");
+ //flushes the buffer
+ fflush (stdout);
+ }
+ }
+
+};
+
+
+/* ------------------------------------------------------------------------- */
+/* --- vpTRACE IN/OUT FONCTION --------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+#ifdef VP_TRACE // Activate the trace mode
+
+/*!
+ \ingroup group_core_debug
+ Works like vpTRACE() and should be used at the beginning of a function.
+
+ \code
+#define VP_TRACE // To activate the trace mode
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ vpIN_FCT("main()");
+ // the body of the main() function
+ vpOUT_FCT("main()");
+}
+ \endcode
+
+ \sa vpOUT_FCT
+*/
+#define vpIN_FCT (vpTraceOutput(__FILE__,__LINE__, __FUNCTION__, false, "begin "))
+
+
+/*!
+ \ingroup group_core_debug
+ Works like vpTRACE() and should be used at the end of a function.
+
+ \code
+#define VP_TRACE // To activate the trace mode
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ vpIN_FCT("main()");
+ // the body of the main() function
+ vpOUT_FCT("main()");
+}
+ \endcode
+
+ \sa vpIN_FCT
+*/
+#define vpOUT_FCT (vpTraceOutput(__FILE__,__LINE__, __FUNCTION__, false, "end "))
+
+#else // #ifdef VP_TRACE
+
+inline void vpIN_FCT (const char * /* a */, ...){}
+inline void vpOUT_FCT (const char * /* a */, ...){}
+
+#endif // #ifdef VP_TRACE
+
+/* -------------------------------------------------------------------------- */
+/* --- vpTRACE -------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#ifdef VP_TRACE
+
+/*!
+ \ingroup group_core_debug
+ Used to display trace messages on the standard stream (C++).
+ Use like this : vpCTRACE<<"my message"<<std::endl;
+
+ \code
+#define VP_TRACE // To activate trace mode
+#define VP_DEBUG // To activate the debug mode
+#define VP_DEBUG_MODE 2 // To activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // C++-like debug printings
+ vpCTRACE << "C++-like trace" << std::endl; // stdout
+ vpCERROR << "C++-like error trace" << std::endl; // stderr
+
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
+}
+ \endcode
+
+ \sa vpTRACE(), vpCERROR(), vpCDEBUG()
+*/
+#define vpCTRACE std::cout << "(L0) " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
+
+
+/*!
+ \ingroup group_core_debug
+ Used to display error messages on the error stream (C++).
+ Use like this : vpCERROR<<"my message"<<std::endl;
+
+ \code
+#define VP_TRACE // To activate trace mode
+#define VP_DEBUG // To activate the debug mode
+#define VP_DEBUG_MODE 2 // To activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // C++-like debug printings
+ vpCTRACE << "C++-like trace" << std::endl; // stdout
+ vpCERROR << "C++-like error trace" << std::endl; // stderr
+
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
+}
+ \endcode
+
+ \sa vpCTRACE(), vpCDEBUG()
+*/
+#define vpCERROR std::cerr << "(L0) " << "!!\t" << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
+
+/*!
+ \ingroup group_core_debug
+ Used to display error messages on the error stream.
+ Prints the name of the file, the function name and the line where
+ it was used.
+ Use like this : vpERROR_TRACE("my error message number %d", i);
+ with any "printf" string.
+
+ \code
+#define VP_TRACE // To activate trace mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // Printing depend only VP_DEBUG_MODE value is >= 1
+ vpTRACE(1, "C-like trace level 1"); // stdout
+ vpERROR_TRACE(1, "C-like error trace level 1"); // stderr
+}
+ \endcode
+
+ \sa vpTRACE()
+*/
+#define vpERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
+
+/*!
+ \ingroup group_core_debug
+ Used to display trace messages on the standard stream.
+ Prints the name of the file, the function name and the line where
+ it was used.
+ Use like this : vpTRACE("my debug message number %d", i);
+ with any "printf" string.
+
+ \code
+#define VP_TRACE // To activate trace mode
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // C-like debug printings
+ vpTRACE("C-like trace"); // stdout
+}
+ \endcode
+
+ \sa vpCTRACE(), vpERROR_TRACE()
+*/
+#define vpTRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
+
+#else // #ifdef VP_TRACE
+
+#define vpCTRACE if(false) std::cout // Warning C4127
+#define vpCERROR if(false) std::cerr // Warning C4127
+
+inline void vpERROR_TRACE (const char * /* a */, ...){}
+inline void vpERROR_TRACE (int /* level */, const char * /* a */, ...){}
+inline void vpTRACE (const char * /* a */, ...){}
+inline void vpTRACE (int /* level */, const char * /* a */, ...){}
+
+#endif // #ifdef VP_TRACE
+
+/* ------------------------------------------------------------------------- */
+/* --- VP_DEBUG ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------- */
+
+#ifdef VP_DEBUG
+
+/*!
+ \ingroup group_core_debug
+ vpDERROR_TRACE works like printf, but prints only if the
+ tracing level is smaller than the debug level VP_DEBUG_MODE.
+
+ \code
+#define VP_DEBUG // Activate the debug mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
+ vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
+}
+ \endcode
+
+ \sa vpDEBUG_TRACE()
+*/
+#define vpDERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
+
+/*!
+ \ingroup group_core_debug
+ vpDEBUG_TRACE works like printf, but prints only if the
+ tracing level level is greater than the debug level VP_DEBUG_MODE.
+
+ \code
+#define VP_DEBUG // Activate the debug mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
+ vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
+}
+ \endcode
+
+ \sa vpDERROR_TRACE()
+*/
+#define vpDEBUG_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
+
+/*!
+ \ingroup group_core_debug
+ vpCDEBUG(level) work like the C++ output stream std::cout.
+ \code
+#define VP_DEBUG // Activate the debug mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // C++-like debug printings
+ vpCTRACE << "C++-like trace" << std::endl; // stdout
+ vpCERROR << "C++-like error trace" << std::endl; // stderr
+
+ // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
+ vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
+}
+ \endcode
+
+ \sa vpCTRACE(), vpCERROR()
+*/
+#define vpCDEBUG(level) if (VP_DEBUG_MODE < level) ; else \
+ std::cout << "(L" << level << ") "<< __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
+
+/*!
+ \ingroup group_core_debug
+
+ vpDEBUG_ENABLE(level) is equal to 1 if the debug level \e level is greater than
+ the debug mode VP_DEBUG_MODE, 0 else.
+
+ \code
+#define VP_DEBUG // Activate the debug mode
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+int main()
+{
+ // Check the active debug levels
+ std::cout << "Debug level 1 active: " << vpDEBUG_ENABLE(1) << std::endl;
+ std::cout << "Debug level 2 active: " << vpDEBUG_ENABLE(2) << std::endl;
+ std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
+}
+ \endcode
+*/
+#define vpDEBUG_ENABLE(level) (VP_DEBUG_MODE >= level)
+
+#else // #ifdef VP_DEBUG
+
+inline void vpDERROR_TRACE(const char * /* a */, ...){}
+inline void vpDEBUG_TRACE(const char * /* a */, ...){}
+inline void vpDERROR_TRACE(int /* level */, const char * /* a */, ...){}
+inline void vpDEBUG_TRACE(int /* level */, const char * /* a */, ...){}
+
+#define vpCDEBUG(level) if(false) std::cout // Warning C4127
+#define vpDEBUG_ENABLE(level) (false) // Warning C4127
+
+#endif // #ifdef VP_DEBUG
+
+/* -------------------------------------------------------------------------- */
+/* --- DEFENSIF ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#ifdef VP_DEFENSIF
+#define DEFENSIF(a) (a)
+#else
+#define DEFENSIF(a) (0)
+#endif /*#ifdef DEFENSIF*/
+
+
+#endif /* #ifdef __DEBUG_HH */
diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h
new file mode 100644
index 0000000..9b626ee
--- /dev/null
+++ b/modules/core/include/visp3/core/vpDisplay.h
@@ -0,0 +1,983 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpDisplay_h
+#define vpDisplay_h
+
+#include <string>
+#include <sstream>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpMouseButton.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImagePoint.h>
+
+/*!
+ \file vpDisplay.h
+ \brief Generic class for image display, also provide the interface
+ with the image.
+*/
+
+/*!
+
+ \class vpDisplay
+
+ \ingroup group_core_gui
+
+ \brief Class that defines generic functionnalities for display.
+
+ The \ref tutorial-getting-started is a good starting point to know
+ how to use this class to display an image in a window.
+
+ The example below shows how to use this class.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+int main()
+{
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+#ifdef _WIN32
+ vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
+#else
+ vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
+#endif
+
+ vpDisplay *d;
+
+ // Depending on the detected third party libraries, we instantiate here the
+ // first video device which is available
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My image");
+
+ // To initialize the video device, it is also possible to replace
+ // the 3 previous lines by:
+ // d->init(I, 400, 100, "My image");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(50);
+ topLeftCorner.set_j(10);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Get non blocking keyboard events
+ std::cout << "Check keyboard events..." << std::endl;
+ char key[10]; sprintf(key, "\0");
+ bool ret;
+ for (int i=0; i< 200; i++) {
+ bool ret = vpDisplay::getKeyboardEvent(I, key, false);
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ vpTime::wait(40);
+ }
+
+ // Get a blocking keyboard event
+ std::cout << "Wait for a keyboard event..." << std::endl;
+ ret = vpDisplay::getKeyboardEvent(I, key, true);
+ std::cout << "keyboard event: " << ret << std::endl;
+ if (ret)
+ std::cout << "key: " << "\"" << key << "\"" << std::endl;
+
+ // Wait for a click in the display window
+ std::cout << "Wait for a button click..." << std::endl;
+ vpDisplay::getClick(I);
+
+ delete d;
+}
+ \endcode
+
+ Other examples are available in tutorial-image-viewer.cpp and
+ tutorial-viewer.cpp.
+*/
+class VISP_EXPORT vpDisplay
+{
+ protected :
+ //! display has been initialized
+ bool displayHasBeenInitialized ;
+ //! display position
+ int windowXPosition ;
+ //! display position
+ int windowYPosition ;
+ unsigned int width ;
+ unsigned int height ;
+ std::string title_;
+
+ /*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+ */
+ virtual void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4, unsigned int h=2,
+ unsigned int thickness=1) =0;
+ /*!
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param ip : Upper left image point location of the string in the display.
+ \param text : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+ */
+ virtual void displayCharString(const vpImagePoint &ip, const char *text,
+ const vpColor &color=vpColor::green) =0;
+ /*!
+ Display a circle.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+ */
+ virtual void displayCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1) =0;
+ /*!
+ Display a cross at the image point \e ip location.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+ */
+ virtual void displayCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness=1) =0;
+ /*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Dashed line thickness.
+ */
+ virtual void displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) =0;
+ /*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+ */
+ virtual void displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) =0;
+
+ /*!
+ Display a point at the image point \e ip location.
+ \param ip : Point location.
+ \param color : Point color.
+ */
+ virtual void displayPoint(const vpImagePoint &ip, const vpColor &color) =0;
+
+ /*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+ */
+ virtual void displayRectangle(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1)=0 ;
+ /*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+ */
+ virtual void displayRectangle(const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1 )=0;
+ /*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+ */
+ virtual void displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1)=0 ;
+
+ public:
+ vpDisplay() ;
+ vpDisplay(const vpDisplay& d) ;
+ virtual ~vpDisplay();
+
+ /*!
+ Set the window backgroud to \e color.
+ \param color : Background color.
+ */
+ virtual void clearDisplay(const vpColor &color=vpColor::white) =0 ;
+ /*!
+ Close the window.
+ */
+ virtual void closeDisplay() =0;
+
+ /*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+ */
+ virtual void displayImage(const vpImage<unsigned char> &I) =0 ;
+ /*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+ */
+ virtual void displayImage(const vpImage<vpRGBa> &I) =0 ;
+
+ virtual void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
+ virtual void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
+
+ /*!
+ Return the position (along the horizontal axis) on the screen of the display window.
+ \sa getWindowYPosition()
+ */
+ int getWindowXPosition() const {return windowXPosition;};
+ /*!
+ Return the position (along the vertical axis) on the screen of the display window.
+ \sa getWindowXPosition()
+ */
+ int getWindowYPosition() const {return windowYPosition;};
+ /*!
+ Flushes the display.
+ It's necessary to use this function to see the results of any drawing.
+ */
+ virtual void flushDisplay() =0;
+
+ /*!
+ Flushes the display.
+ It's necessary to use this function to see the results of any drawing.
+ */
+ virtual void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0;
+
+
+ /* Simple interface with the mouse event */
+
+ /*!
+ Wait for a click from one of the mouse button.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+ */
+ virtual bool getClick(bool blocking=true) =0;
+
+ /*!
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+ */
+ virtual bool getClick(vpImagePoint &ip,
+ bool blocking=true) =0;
+ /*!
+ Wait for a mouse button click and get the position of the clicked
+ pixel. The button used to click is also set.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+ */
+ virtual bool getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) =0 ;
+ /*!
+ Wait for a mouse button click release and get the position of the
+ image point were the click release occurs. The button used to click is
+ also set. Same method as getClick(unsigned int&, unsigned int&,
+ vpMouseButton::vpMouseButtonType &, bool).
+
+ \param ip [out] : Position of the clicked image point.
+
+ \param button [in] : Button used to click.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+ \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
+
+ */
+ virtual bool getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) =0;
+
+ /*!
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+ */
+ virtual bool getKeyboardEvent(bool blocking=true) =0;
+ /*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+ */
+ virtual bool getKeyboardEvent(char *string, bool blocking=true) =0;
+ /*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+ */
+ virtual bool getPointerMotionEvent (vpImagePoint &ip) =0;
+
+ /*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+ */
+ virtual bool getPointerPosition (vpImagePoint &ip) =0;
+
+ /*!
+ Return the display height.
+ \sa getWidth()
+ */
+ inline unsigned int getHeight() const { return height ; }
+ /*!
+ Return the display width.
+ \sa getHeight()
+ */
+ inline unsigned int getWidth() const { return width ; }
+
+ /*!
+ Initialize the display (size, position and title) of a gray level image.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+ */
+ virtual void init(vpImage<unsigned char> &I,
+ int x=-1, int y=-1,
+ const char *title=NULL) =0 ;
+ /*!
+ Initialize the display (size, position and title) of a color
+ image in RGBa format.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+ */
+ virtual void init(vpImage<vpRGBa> &I,
+ int x=-1, int y=-1,
+ const char *title=NULL) =0 ;
+
+ /*!
+ Initialize the display size, position and title.
+
+ \param width, height : Width and height of the window.
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+ The following example shows how to use this function
+ \code
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#ifdef VISP_HAVE_DISPLAY
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "lena.pgm");
+
+ vpDisplay *d;
+
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#else
+ std::cout << "Sorry, no video device is available" << std::endl;
+ return -1;
+#endif
+
+ d->init(I.getWidth(), I.getHeight(), 10, 20, "viewer");
+
+ // Now associate the display to the image
+ I.display = d;
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // wait for a mouse clink in the display to exit
+ vpDisplay::getClick(I);
+
+ delete d;
+#endif
+}
+ \endcode
+ */
+ virtual void init(unsigned int width, unsigned int height,
+ int x=-1, int y=-1 ,
+ const char *title=NULL) =0;
+
+ /*!
+ Check if the display has been initialised
+
+ \return True if the display has been initialised, otherwise False
+ */
+ inline bool isInitialised() { return displayHasBeenInitialized; }
+
+ /*!
+ Set the font used to display a text in overlay. The display is
+ performed using displayCharString().
+
+ \param font : The expected font name. The available fonts are given by
+ the "xlsfonts" binary. To choose a font you can also use the
+ "xfontsel" binary.
+
+ \note Under UNIX, to know all the available fonts, use the
+ "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
+
+ \sa displayCharString()
+ */
+ virtual void setFont(const char *font) =0;
+ /*!
+ Set the window title.
+ \param title : Window title.
+ */
+ virtual void setTitle(const char *title) =0;
+ /*!
+ Set the window position in the screen.
+
+ \param winx, winy : Position of the upper-left window's border in
+ the screen.
+
+ */
+ virtual void setWindowPosition(int winx, int winy) = 0 ;
+
+ /*!
+ @name Display functionalities on gray level images.
+ */
+ static void close(vpImage<unsigned char> &I) ;
+ static void display(const vpImage<unsigned char> &I) ;
+ static void displayArrow(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4, unsigned int h=2,
+ unsigned int thickness=1) ;
+ static void displayArrow(const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4, unsigned int h=2,
+ unsigned int thickness=1) ;
+ static void displayCamera(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness) ;
+ static void displayCharString(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, const char *string,
+ const vpColor &color) ;
+ static void displayCharString(const vpImage<unsigned char> &I,
+ int i, int j, const char *string,
+ const vpColor &color) ;
+ static void displayCircle(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ static void displayCircle(const vpImage<unsigned char> &I,
+ int i, int j, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ static void displayCross(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayCross(const vpImage<unsigned char> &I,
+ int i, int j, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayDotLine(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayDotLine(const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayEllipse(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayEllipse(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ const double &theta1, const double &theta2, bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayFrame(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color=vpColor::none,
+ unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0,0)) ;
+ static void displayLine(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayLine(const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPoint(const vpImage<unsigned char> &I,
+ int i, int j,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPolygon(const vpImage<unsigned char> &I,
+ const std::vector<vpImagePoint> &vip,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ int i, int j,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<unsigned char> &I,
+ unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
+ static void displayText(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, const std::string &s,
+ const vpColor &color) ;
+ static void displayText(const vpImage<unsigned char> &I,
+ int i, int j, const std::string &s,
+ const vpColor &color) ;
+ static void flush(const vpImage<unsigned char> &I) ;
+ static void flushROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
+
+ static bool getClick(const vpImage<unsigned char> &I, bool blocking=true) ;
+ static bool getClick(const vpImage<unsigned char> &I,
+ vpImagePoint &ip, bool blocking=true) ;
+ static bool getClick(const vpImage<unsigned char> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClick(const vpImage<unsigned char> &I,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClickUp(const vpImage<unsigned char> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClickUp(const vpImage<unsigned char> &I,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static void getImage(const vpImage<unsigned char> &Is, vpImage<vpRGBa> &Id) ;
+
+ static bool getKeyboardEvent(const vpImage<unsigned char> &I,
+ bool blocking=true);
+ static bool getKeyboardEvent(const vpImage<unsigned char> &I,
+ char *string, bool blocking=true);
+ static bool getPointerMotionEvent (const vpImage<unsigned char> &I,
+ vpImagePoint &ip);
+ static bool getPointerPosition (const vpImage<unsigned char> &I,
+ vpImagePoint &ip);
+ static void setBackground(const vpImage<unsigned char> &I, const vpColor &color);
+ static void setFont(const vpImage<unsigned char> &I, const char *font);
+ static void setTitle(const vpImage<unsigned char> &I,
+ const char *windowtitle);
+ static void setWindowPosition(const vpImage<unsigned char> &I,
+ int winx, int winy);
+
+ /*!
+ @name Display functionalities on color images.
+ */
+ static void close(vpImage<vpRGBa> &I) ;
+
+ static void display(const vpImage<vpRGBa> &I) ;
+ static void displayArrow(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4, unsigned int h=2,
+ unsigned int thickness=1) ;
+ static void displayArrow(const vpImage<vpRGBa> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4, unsigned int h=2,
+ unsigned int thickness=1) ;
+ static void displayCamera(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness) ;
+ static void displayCharString(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, const char *string,
+ const vpColor &color) ;
+ static void displayCharString(const vpImage<vpRGBa> &I,
+ int i, int j, const char *string,
+ const vpColor &color) ;
+ static void displayCircle(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ static void displayCircle(const vpImage<vpRGBa> &I,
+ int i, int j, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ static void displayCross(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayCross(const vpImage<vpRGBa> &I,
+ int i, int j, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayDotLine(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayDotLine(const vpImage<vpRGBa> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayFrame(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color=vpColor::none,
+ unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0,0));
+ static void displayEllipse(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayEllipse(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ const double &angle1, const double &angle2, bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayLine(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayLine(const vpImage<vpRGBa> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPoint(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPoint(const vpImage<vpRGBa> &I,
+ int i, int j,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayPolygon(const vpImage<vpRGBa> &I,
+ const std::vector<vpImagePoint> &vip,
+ const vpColor &color,
+ unsigned int thickness=1) ;
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ int i, int j,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ static void displayRectangle(const vpImage<vpRGBa> &I,
+ unsigned int i, unsigned int j,
+ float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness=1);
+ static void displayROI(const vpImage<vpRGBa> &I, const vpRect &roi) ;
+ static void displayText(const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, const std::string &s,
+ const vpColor &color) ;
+ static void displayText(const vpImage<vpRGBa> &I,
+ int i, int j, const std::string &s,
+ const vpColor &color) ;
+
+ static void flush(const vpImage<vpRGBa> &I) ;
+ static void flushROI(const vpImage<vpRGBa> &I, const vpRect &roi) ;
+ static bool getClick(const vpImage<vpRGBa> &I, bool blocking=true) ;
+ static bool getClick(const vpImage<vpRGBa> &I,
+ vpImagePoint &ip, bool blocking=true) ;
+ static bool getClick(const vpImage<vpRGBa> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClick(const vpImage<vpRGBa> &I,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClickUp(const vpImage<vpRGBa> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static bool getClickUp(const vpImage<vpRGBa> &I,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking=true) ;
+ static void getImage(const vpImage<vpRGBa> &Is, vpImage<vpRGBa> &Id) ;
+
+ static bool getKeyboardEvent(const vpImage<vpRGBa> &I,
+ bool blocking=true);
+ static bool getKeyboardEvent(const vpImage<vpRGBa> &I,
+ char *string, bool blocking=true);
+ static bool getPointerMotionEvent (const vpImage<vpRGBa> &I, vpImagePoint &ip);
+ static bool getPointerPosition (const vpImage<vpRGBa> &I, vpImagePoint &ip);
+
+ static void setBackground(const vpImage<vpRGBa> &I, const vpColor &color);
+ static void setFont(const vpImage<vpRGBa> &I, const char *font);
+ static void setTitle(const vpImage<vpRGBa> &I, const char *windowtitle);
+ static void setWindowPosition(const vpImage<vpRGBa> &I, int winx, int winy);
+
+ private:
+ //! get the window pixmap and put it in vpRGBa image
+ virtual void getImage(vpImage<vpRGBa> &I) = 0;
+
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpDisplayException.h b/modules/core/include/visp3/core/vpDisplayException.h
new file mode 100644
index 0000000..74bde04
--- /dev/null
+++ b/modules/core/include/visp3/core/vpDisplayException.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception that can be emited by the vpDisplay class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef __vpDisplayException_H
+#define __vpDisplayException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpDisplayException.h
+ \brief error that can be emited by the vpDisplay class and its derivates
+ */
+/* Classes standards. */
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpDisplayException
+ \ingroup group_core_debug
+ \brief Error that can be emited by the vpDisplay class and its derivates.
+ */
+class VISP_EXPORT vpDisplayException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpDisplay member
+ */
+ enum errorDisplayCodeEnum
+ {
+ notInitializedError,
+ cannotOpenWindowError,
+ connexionError,
+ XWindowsError,
+ GTKWindowsError,
+ colorAllocError,
+ depthNotSupportedError
+ } ;
+
+ public:
+ vpDisplayException(const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+
+ vpDisplayException (const int id, const std::string & msg)
+ : vpException(id, msg)
+ {
+ }
+
+ vpDisplayException (const int id)
+ : vpException(id)
+ {
+ }
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h
new file mode 100644
index 0000000..9402fee
--- /dev/null
+++ b/modules/core/include/visp3/core/vpException.h
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception handling.
+ *
+ * Authors:
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+
+
+/* \file vpException.h
+ \brief error that can be emited by the vp class and its derivates
+ */
+
+#ifndef __vpException_H
+#define __vpException_H
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <visp3/core/vpConfig.h>
+
+/* Classes standards. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+#include <stdarg.h>
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/*!
+ \class vpException
+ \ingroup group_core_debug
+ \brief error that can be emited 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 VISP_EXPORT vpException : public std::exception
+{
+ protected:
+
+ //! Contains the error code, see the errorCodeEnum table for details.
+ int code;
+
+ //! Contains an error message (can be empty)
+ std::string message;
+
+ //! Set the message container
+ void setMessage(const char* format, va_list args);
+
+ //! forbid the empty constructor (protected)
+ vpException(): code(notInitialized), message("") { };
+
+ public:
+
+ enum generalExceptionEnum
+ {
+ memoryAllocationError,
+ memoryFreeError,
+ functionNotImplementedError,
+ ioError,
+ cannotUseConstructorError,
+ notImplementedError,
+ divideByZeroError,
+ dimensionError,
+ fatalError,
+ badValue, /*!< Used to indicate that a value is not in the allowed range. */
+ notInitialized /*!< Used to indicate that a parameter is not initialized. */
+ } ;
+
+ vpException (const int code, const char* format, va_list args);
+ vpException (const int code, const char* format, ...);
+ vpException (const int code, const std::string & msg);
+ vpException (const int code);
+
+ /*!
+ Basic destructor. Do nothing but implemented to fit the inheritance from
+ std::exception
+ */
+ virtual ~vpException() throw() {}
+
+ //! send the object code
+ int getCode (void);
+
+ //! send a reference (constant) related the error message (can be empty)
+ const std::string &getStringMessage (void);
+ //! send a pointer on the array of \e char related to the error string.
+ //!Cannot be \e NULL.
+ const char *getMessage (void);
+
+ //! 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 */
diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h
new file mode 100644
index 0000000..cc07c77
--- /dev/null
+++ b/modules/core/include/visp3/core/vpExponentialMap.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exponential map.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpExponentialMap.h
+ \brief Provides exponential map computation
+*/
+
+
+
+#ifndef vpExponentialMap_h
+#define vpExponentialMap_h
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+/*!
+ \class vpExponentialMap
+
+ \ingroup group_core_transformations
+
+ \brief Direct or inverse exponential map computation.
+
+ - The direct exponential map allows to determine a displacement from a
+ velocity vector applied during a sampling time. With direct() the sampling
+ time is set to 1 second. With direct(const vpColVector &, const float &)
+ the sampling time can be set to an other value.
+
+ - The inverse exponential map allows to compute a velocity vector from a
+ displacement measured during a time interval. With inverse() the time
+ interval also called sampling time is set to 1 second. With
+ inverse(const vpHomogeneousMatrix &, const float &) the sampling time
+ can be set to an other value.
+
+ The displacement is represented as an homogeneous matrix
+ (vpHomogeneousMatrix). Velocities are represented as a \f$ [{\bf t}, {\bf
+ \theta u} ]^t \f$ 6 dimension vector where \f$ t \f$ is a translation vector
+ (see vpTranslationVector) and \f$ \theta u \f$ a rotation vector (see
+ vpThetaUVector).
+
+*/
+class VISP_EXPORT vpExponentialMap
+{
+
+public:
+ static vpHomogeneousMatrix direct(const vpColVector &v);
+ static vpHomogeneousMatrix direct(const vpColVector &v,
+ const double &delta_t);
+ static vpColVector inverse(const vpHomogeneousMatrix &M);
+ static vpColVector inverse(const vpHomogeneousMatrix &M,
+ const double &delta_t);
+
+};
+#endif
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpFeatureDisplay.h b/modules/core/include/visp3/core/vpFeatureDisplay.h
new file mode 100644
index 0000000..5ce0bb7
--- /dev/null
+++ b/modules/core/include/visp3/core/vpFeatureDisplay.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureDisplay_H
+#define vpFeatureDisplay_H
+
+/*!
+ \file vpFeatureDisplay.h
+ \brief interface with the image for feature display
+*/
+
+//Color / image / display
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpRGBa.h>
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+
+/*!
+ \class vpFeatureDisplay
+
+ \ingroup group_core_gui
+ \brief Interface with the image for feature display.
+*/
+class VISP_EXPORT vpFeatureDisplay
+{
+
+public:
+ static void displayCylinder(double rho1,double theta1,
+ double rho2,double theta2,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+ static void displayCylinder(double rho1,double theta1,
+ double rho2,double theta2,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+
+ static void displayEllipse(double x,double y,
+ double mu20, double mu11, double m02,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+
+ static void displayEllipse(double x,double y,
+ double mu20, double mu11, double m02,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+
+ static void displayLine(double rho,double theta,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+ static void displayLine(double rho,double theta,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+
+ static void displayPoint(double x,double y,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+ static void displayPoint(double x,double y,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green,
+ unsigned int thickness=1) ;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpForceTwistMatrix.h b/modules/core/include/visp3/core/vpForceTwistMatrix.h
new file mode 100644
index 0000000..7edea4a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpForceTwistMatrix.h
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Twist transformation matrix that allows to transform forces from one
+ * frame to an other.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpForceTwistMatrix_h
+#define vpForceTwistMatrix_h
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+
+/*!
+ \class vpForceTwistMatrix
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a force/torque twist matrix and operations on such kind of matrices.
+
+ Class that consider the particular case of twist
+ transformation matrix that allows to transform a force/troque vector
+ from one frame to an other.
+
+ The vpForceTwistMatrix class is derived from vpArray2D<double>.
+
+ The twist transformation matrix that allows to transform the
+ force/torque vector expressed at frame \f${\cal F}_b\f$ into the
+ frame \f${\cal F}_a\f$ is a 6 by 6 matrix defined as
+
+ \f[
+ ^a{\bf F}_b = \left[ \begin{array}{cc}
+ ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
+ {[^a{\bf t}_b]}_{\times} \; ^a{\bf R}_b & ^a{\bf R}_b
+ \end{array}
+ \right]
+ \f]
+
+ \f$ ^a{\bf R}_b \f$ is a rotation matrix and
+ \f$ ^a{\bf t}_b \f$ is a translation vector.
+
+ The code belows shows for example how to convert a force/torque skew
+ from probe frame to a sensor frame.
+
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpForceTwistMatrix.h>
+
+int main()
+{
+ vpForceTwistMatrix sFp; // Twist transformation matrix from sensor to probe frame
+
+ vpHomogeneousMatrix sMp; // Force/torque sensor frame to probe frame transformation
+ // ... sMp need here to be initialized
+
+ sFp.buildFrom(sMp);
+
+ vpColVector p_H(6); // Force/torque skew in the probe frame: fx,fy,fz,tx,ty,tz
+ // ... p_H should here have an initial value
+
+ vpColVector s_H(6); // Force/torque skew in the sensor frame: fx,fy,fz,tx,ty,tz
+
+ // Compute the value of the force/torque in the sensor frame
+ s_H = sFp * p_H;
+}
+ \endcode
+*/
+class VISP_EXPORT vpForceTwistMatrix : public vpArray2D<double>
+{
+ public:
+ // basic constructor
+ vpForceTwistMatrix();
+ // copy constructor
+ vpForceTwistMatrix(const vpForceTwistMatrix &F) ;
+ // constructor from an homogeneous transformation
+ vpForceTwistMatrix(const vpHomogeneousMatrix &M) ;
+
+ // Construction from Translation and rotation (ThetaU parameterization)
+ vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
+ // Construction from Translation and rotation (matrix parameterization)
+ vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
+ vpForceTwistMatrix(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz) ;
+ /*!
+ Destructor.
+ */
+ virtual ~vpForceTwistMatrix() {};
+
+ vpForceTwistMatrix buildFrom(const vpTranslationVector &t,
+ const vpRotationMatrix &R);
+ vpForceTwistMatrix buildFrom(const vpTranslationVector &t,
+ const vpThetaUVector &thetau);
+ vpForceTwistMatrix buildFrom(const vpHomogeneousMatrix &M) ;
+
+ // Basic initialisation (identity)
+ void eye() ;
+
+ vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const ;
+ vpMatrix operator*(const vpMatrix &M) const ;
+
+ vpColVector operator*(const vpColVector &H) const ;
+
+ // copy operator from vpMatrix (handle with care)
+ vpForceTwistMatrix &operator=(const vpForceTwistMatrix &H);
+
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+
+ /*!
+ This function is not applicable to a velocity twist matrix that is always a
+ 6-by-6 matrix.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize a velocity twist matrix"));
+ };
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use eye().
+ */
+ vp_deprecated void setIdentity();
+ //@}
+#endif
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpForwardProjection.h b/modules/core/include/visp3/core/vpForwardProjection.h
new file mode 100644
index 0000000..cd6cb94
--- /dev/null
+++ b/modules/core/include/visp3/core/vpForwardProjection.h
@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Forward projection.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpForwardProjection_H
+#define vpForwardProjection_H
+
+/*!
+ \file vpForwardProjection.h
+ \brief class that defines what is a generic geometric feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTracker.h>
+#include <visp3/core/vpColor.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+
+/*!
+ \class vpForwardProjection
+ \brief Class that defines what is a generic geometric feature.
+
+ Each geometric feature has parameters expressed:
+
+ - in the object frame \e oP. These parameters are located in the public
+ attribute vpForwardProjection::oP.
+ - in the camera frame \e cP. These parameters are located in the public
+ attribute vpTracker::cP.
+ - in the image plane \e p. These parameters are located in the public
+ attribute vpTracker::p. They correspond to normalized coordinates
+ of the feature expressed in meters.
+*/
+class VISP_EXPORT vpForwardProjection : public vpTracker
+{
+public:
+ /*!
+ Feature coordinates expressed in the object frame, also called world frame.
+ */
+ vpColVector oP ;
+
+public:
+ /*!
+ Default initialisation of the feature parameters:
+ - in the object frame: \e oP
+ - in the camera frame: \e cP
+ - in the image plane: \e p.
+ */
+ virtual void init() = 0;
+
+ //! Destructor.
+ virtual ~vpForwardProjection() { ; }
+
+public:
+ /*!
+ Sets the parameters which define the feature in the object frame.
+
+ \param oP : Feature parameters expressed in the object frame used
+ to set the vpForwardProjection::oP public attribute.
+
+ */
+ virtual void setWorldCoordinates(const vpColVector &oP) = 0;
+
+ /*!
+
+ Computes the features parameters in the camera frame (\e cP) thanks
+ to the parameters given in the object frame
+ (vpForwardProjection::oP) and the homogeneous matrix relative to
+ the pose (\e cMo) between the object frame and the camera frame.
+
+ To set the parameters in the object frame you need to call
+ setWorldCoordinates().
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+ \param cP : The vector which will contain the feature parameters
+ expressed in the camera frame.
+
+ With this method, the vpTracker::cP public attribute is not updated.
+
+ */
+ virtual void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)=0;
+ /*!
+
+ Computes the features parameters in the camera frame (\e cP) thanks
+ to the parameters given in the object frame
+ (vpForwardProjection::oP) and the homogeneous matrix relative to
+ the pose (\e cMo) between the object frame and the camera frame.
+
+ To set the parameters in the object frame you need to call
+ setWorldCoordinates().
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+ The features parameters in the camera frame (cP) are updated in
+ the vpTracker::cP public attribute.
+ */
+ virtual void changeFrame(const vpHomogeneousMatrix &cMo) = 0 ;
+ /*!
+
+ Computes the feature parameters in the image plane from the
+ parameters expressed in the camera frame.
+
+ \param cP [input] : Feature parameters expressed in the camera frame.
+
+ \param p [output] : Feature parameters expressed in the image plane.
+ */
+ virtual void projection(const vpColVector &cP, vpColVector &p) =0 ;
+
+ /*!
+ Computes the feature parameters in the image plane. These
+ parameters are than updated in the vpTracker::p public attribute.
+
+ \warning To compute these parameters, the method exploit the
+ feature parameters in the camera frame. Thus, vpTracker::cP need
+ to be updated before the call of this method. For that, a call to
+ changeFrame(const vpHomogeneousMatrix &) is requested.
+ */
+ virtual void projection() =0 ;
+
+ void project() ;
+ void project(const vpHomogeneousMatrix &cMo) ;
+ void track(const vpHomogeneousMatrix &cMo) ;
+
+ /*!
+
+ Displays the feature in the image \e I thanks to the 2D feature
+ parameters in the image plane (vpTracker::p) and the camera
+ parameters which enable to convert the features from meter to pixel.
+
+ \param I : The image where the feature must be displayed in overlay.
+
+ \param cam : The camera parameters to enable the conversion from
+ meter to pixel.
+
+ \param color : The desired color to display the line in the image.
+ \param thickness : Thickness of the feature representation.
+ */
+ virtual void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) =0;
+
+ /*!
+
+ Displays the feature in the image \e I thanks to the features in
+ the object frame (vpForwardProjection::oP), the homogeneous matrix relative
+ to the pose between the object frame and the camera frame and the
+ camera parameters which enable to convert the features from meter
+ to pixel.
+
+ \param I : The image where the line must be displayed in overlay.
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+ \param cam : The camera parameters to enable the conversion from
+ meter to pixel.
+
+ \param color : The desired color to display the line in the image.
+ \param thickness : Thickness of the feature representation.
+ */
+ virtual void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) =0;
+
+ virtual void print() const ;
+
+ /*!
+ Create an object with the same type.
+ */
+ virtual vpForwardProjection *duplicate() const = 0 ;
+
+public:
+ /*!
+ Used for memory issue especially in the vpServo class.
+ */
+ typedef enum
+ {
+ user,
+ vpDisplayForwardProjection
+ } vpForwardProjectionDeallocatorType;
+
+private:
+ vpForwardProjectionDeallocatorType deallocate ;
+public:
+ vpForwardProjection() : oP(), deallocate(user) {}
+
+ void setDeallocate(vpForwardProjectionDeallocatorType d) { deallocate = d ; }
+ vpForwardProjectionDeallocatorType getDeallocate() { return deallocate ; }
+} ;
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpFrameGrabber.h b/modules/core/include/visp3/core/vpFrameGrabber.h
new file mode 100644
index 0000000..a8b2036
--- /dev/null
+++ b/modules/core/include/visp3/core/vpFrameGrabber.h
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Frame grabbing.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpFrameGrabber_hh
+#define vpFrameGrabber_hh
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+ \file vpFrameGrabber.h
+ \brief Base class for all video devices. It is
+ designed to provide a generic front end to video sources.
+*/
+
+/*!
+ \class vpFrameGrabber
+
+ \brief Base class for all video devices. It is designed to provide a front
+ end to video sources.
+
+ This class should provide a virtual function that allows the acquisition
+ of an image.
+
+ The example below shows how to use this class.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vpFrameGrabber *g; // Generic framegrabber
+
+#if defined( VISP_HAVE_DC1394 )
+ vp1394TwoGrabber *g_1394_2 = new vp1394TwoGrabber;
+ // specific settings for firewire grabber based on libdc1394-2.x version
+ g_1394_2->setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_320x240_YUV422);
+ g_1394_2->setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
+ g = g_1394_2;
+#elif defined( VISP_HAVE_V4L2 )
+ vpV4l2Grabber *g_v4l2 = new vpV4l2Grabber;
+ // specific settings for Video For Linux Two grabber
+ g_v4l2->setInput(2); // Input 2 on the board
+ g_v4l2->setFramerate(vpV4l2Grabber::framerate_50fps); // 50 fps
+ g_v4l2->setWidth(384); // Acquired images are 768 width
+ g_v4l2->setHeight(288); // Acquired images are 576 height
+ g_v4l2->setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
+ g = g_v4l2;
+#endif
+
+ g->open(I); // Open the framegrabber
+ g->acquire(I); // Acquire an image
+ vpImageIo::write(I, "image.pgm"); // Write image on the disk
+#endif
+}
+ \endcode
+
+
+ \author Eric Marchand (Eric.Marchand at irisa.fr), Irisa / Inria Rennes
+*/
+class VISP_EXPORT vpFrameGrabber
+{
+public :
+ bool init ; //!< Set to true if the frame grabber has been initialized.
+protected:
+ unsigned int height ; //!< Number of rows in the image.
+ unsigned int width ; //!< Number of columns in the image.
+
+
+public:
+ //! Return the number of rows in the image.
+ inline unsigned int getHeight() const { return height ; }
+ //! Return the number of columns in the image.
+ inline unsigned int getWidth() const { return width ; }
+
+public:
+ vpFrameGrabber() : init(false), height(0), width(0) {};
+ virtual ~vpFrameGrabber() { ; }
+
+ virtual void open(vpImage<unsigned char> &I) =0 ;
+ virtual void open(vpImage<vpRGBa> &I) =0 ;
+
+ virtual void acquire(vpImage<unsigned char> &I) =0 ;
+ virtual void acquire(vpImage<vpRGBa> &I) =0 ;
+
+
+ /*!
+ This virtual function is used to de-allocate
+ the memory used by a specific frame grabber
+ */
+ virtual void close() =0 ;
+
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpFrameGrabberException.h b/modules/core/include/visp3/core/vpFrameGrabberException.h
new file mode 100644
index 0000000..94d9087
--- /dev/null
+++ b/modules/core/include/visp3/core/vpFrameGrabberException.h
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpFrameGrabber class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpFrameGrabberException_H
+#define __vpFrameGrabberException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpFrameGrabberException.h
+ \brief error that can be emited by the vpFrameGrabber class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \brief Error that can be emited by the vpFrameGrabber class and its derivates
+ */
+class VISP_EXPORT vpFrameGrabberException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpFrameGrabber member
+ */
+ enum errorFrameGrabberCodeEnum
+ {
+ settingError,
+ initializationError,
+ otherError
+ } ;
+
+ public:
+ vpFrameGrabberException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpFrameGrabberException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpFrameGrabberException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif /* #ifndef __vpFrameGrabberException_H */
diff --git a/modules/core/include/visp3/core/vpGDIRenderer.h b/modules/core/include/visp3/core/vpGDIRenderer.h
new file mode 100644
index 0000000..f1d2a92
--- /dev/null
+++ b/modules/core/include/visp3/core/vpGDIRenderer.h
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * GDI renderer for windows 32 display
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) )
+#ifndef vpGDIRenderer_HH
+#define vpGDIRenderer_HH
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <windows.h>
+
+#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpDisplayException.h>
+
+#include <visp3/core/vpMath.h>
+
+class VISP_EXPORT vpGDIRenderer : public vpWin32Renderer
+{
+ //the handle of the associated window
+ HWND hWnd;
+
+ //the bitmap object to display
+ HBITMAP bmp;
+
+ //colors for overlay
+ COLORREF colors[vpColor::id_unknown];
+
+ //font used to draw text
+ HFONT hFont;
+
+ //used to ensure that only one thread at a time is accessing bmp
+ CRITICAL_SECTION CriticalSection;
+
+ public:
+ double timelost;
+ vpGDIRenderer();
+ virtual ~vpGDIRenderer();
+
+ bool init(HWND hWnd, unsigned int width, unsigned int height);
+
+ bool render();
+
+ // gets the image's width.
+ unsigned int getImageWidth(){ return nbCols; }
+
+ // gets the image's height.
+ unsigned int getImageHeight(){ return nbRows; }
+
+ void setImg(const vpImage<vpRGBa>& I);
+ void setImg(const vpImage<unsigned char>& I);
+ void setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+ void setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+
+ void setPixel(const vpImagePoint &iP, const vpColor &color);
+
+ void drawLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness, int style=PS_SOLID);
+
+ void drawRect(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill=false,
+ unsigned int thickness=1);
+
+ void clear(const vpColor &color);
+
+ void drawCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill=false, unsigned int thickness=1);
+
+ void drawText(const vpImagePoint &ip, const char * text,
+ const vpColor &color);
+
+ void drawCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1);
+
+ void drawArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness=1);
+
+ void getImage(vpImage<vpRGBa> &I);
+
+ private:
+
+ //updates the renderer hbitmaps.
+ bool updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
+ unsigned int w, unsigned int h);
+ //updates the renderer hbitmaps.
+ bool updateBitmapROI(unsigned char * imBuffer, const vpImagePoint &iP,
+ unsigned int w, unsigned int h);
+
+
+ //converts a vpImage<vpRGBa> into a HBITMAP .
+ void convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp);
+
+ //converst a vpImage<unsigned char> into a HBITMAP .
+ void convert(const vpImage<unsigned char> &I, HBITMAP& hBmp);
+
+ //converts a vpImage<vpRGBa> into a HBITMAP .
+ void convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ //converst a vpImage<unsigned char> into a HBITMAP .
+ void convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+};
+#endif
+#endif
+#endif
diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h
new file mode 100644
index 0000000..02c1769
--- /dev/null
+++ b/modules/core/include/visp3/core/vpGEMM.h
@@ -0,0 +1,394 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix generalized multiplication.
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+
+#ifndef __VP_GEMM__
+#define __VP_GEMM__
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpException.h>
+
+const vpArray2D<double> null(0,0);
+
+/*!
+ Enumeration of the operations applied on matrices in vpGEMM() function.
+
+ Operations are :
+ - VP_GEMM_A_T to use the transpose matrix of A instead of the matrix A
+ - VP_GEMM_B_T to use the transpose matrix of B instead of the matrix B
+ - VP_GEMM_C_T to use the transpose matrix of C instead of the matrix C
+
+ \relates vpArray2D
+ */
+typedef enum {
+ VP_GEMM_A_T=1, //! Use A^T instead of A
+ VP_GEMM_B_T=2, //! Use B^T instead of B
+ VP_GEMM_C_T=4, //! Use C^T instead of C
+} vpGEMMmethod;
+
+template<unsigned int>
+inline void GEMMsize(const vpArray2D<double> & /*A*/,const vpArray2D<double> & /*B*/, unsigned int &/*Arows*/, unsigned int &/*Acols*/, unsigned int &/*Brows*/, unsigned int &/*Bcols*/)
+{}
+
+template<>
+void inline GEMMsize<0>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getRows();
+ Acols= A.getCols();
+ Brows= B.getRows();
+ Bcols= B.getCols();
+}
+
+template<>
+inline void GEMMsize<1>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getCols();
+ Acols= A.getRows();
+ Brows= B.getRows();
+ Bcols= B.getCols();
+}
+template<>
+inline void GEMMsize<2>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getRows();
+ Acols= A.getCols();
+ Brows= B.getCols();
+ Bcols= B.getRows();
+}
+template<>
+inline void GEMMsize<3>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getCols();
+ Acols= A.getRows();
+ Brows= B.getCols();
+ Bcols= B.getRows();
+}
+
+template<>
+inline void GEMMsize<4>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getRows();
+ Acols= A.getCols();
+ Brows= B.getRows();
+ Bcols= B.getCols();
+}
+
+template<>
+inline void GEMMsize<5>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getCols();
+ Acols= A.getRows();
+ Brows= B.getRows();
+ Bcols= B.getCols();
+}
+
+template<>
+inline void GEMMsize<6>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getRows();
+ Acols= A.getCols();
+ Brows= B.getCols();
+ Bcols= B.getRows();
+}
+
+template<>
+inline void GEMMsize<7>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+{
+ Arows= A.getCols();
+ Acols= A.getRows();
+ Brows= B.getCols();
+ Bcols= B.getRows();
+}
+
+template<unsigned int>
+inline void GEMM1(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,vpArray2D<double> &/*D*/){}
+
+template<>
+inline void GEMM1<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[n][c]*alpha;
+ D[r][c]=sum;
+ }
+}
+
+template<>
+inline void GEMM1<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[n][c]*alpha;
+ D[r][c]=sum;
+ }
+}
+
+template<>
+inline void GEMM1<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[c][n]*alpha;
+ D[r][c]=sum;
+ }
+}
+
+template<>
+inline void GEMM1<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[c][n]*alpha;
+ D[r][c]=sum;
+ }
+}
+
+template<unsigned int>
+inline void GEMM2(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpArray2D<double> & /*A*/,const vpArray2D<double> & /*B*/, const double & /*alpha*/, const vpArray2D<double> & /*C*/ , const double &/*beta*/, vpArray2D<double> &/*D*/)
+{}
+
+template<>
+inline void GEMM2<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[n][c]*alpha;
+ D[r][c]=sum+C[r][c]*beta;
+ }
+}
+
+template<>
+inline void GEMM2<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[n][c]*alpha;
+ D[r][c]=sum+C[r][c]*beta;
+ }
+}
+
+template<>
+inline void GEMM2<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[c][n]*alpha;
+ D[r][c]=sum+C[r][c]*beta;
+ }
+}
+
+template<>
+inline void GEMM2<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[c][n]*alpha;
+ D[r][c]=sum+C[r][c]*beta;
+ }
+}
+
+
+template<>
+inline void GEMM2<4>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[n][c]*alpha;
+ D[r][c]=sum+C[c][r]*beta;
+ }
+}
+
+template<>
+inline void GEMM2<5>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[n][c]*alpha;
+ D[r][c]=sum+C[c][r]*beta;
+ }
+
+}
+
+template<>
+inline void GEMM2<6>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[r][n]*B[c][n]*alpha;
+ D[r][c]=sum+C[c][r]*beta;
+ }
+}
+
+template<>
+inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+{
+ for(unsigned int r=0;r<Arows;r++)
+ for(unsigned int c=0;c<Bcols;c++){
+ double sum=0;
+ for(unsigned int n=0;n<Brows;n++)
+ sum+=A[n][r]*B[c][n]*alpha;
+ D[r][c]=sum+C[c][r]*beta;
+ }
+}
+
+template<unsigned int T>
+inline void vpTGEMM(const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha ,const vpArray2D<double> & C, const double & beta, vpArray2D<double> & D)
+{
+ unsigned int Arows;
+ unsigned int Acols;
+ unsigned int Brows;
+ unsigned int Bcols;
+
+ GEMMsize<T>(A,B,Arows,Acols,Brows,Bcols);
+
+ try {
+ if ((Arows != D.getRows()) || (Bcols != D.getCols())) D.resize(Arows,Bcols);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if (Acols != Brows) {
+ throw(vpException(vpException::dimensionError,
+ "In vpGEMM, cannot multiply (%dx%d) matrix by (%dx%d) matrix",
+ Arows, Acols, Brows, Bcols)) ;
+ }
+
+ if(C.getRows()!=0 && C.getCols()!=0){
+ if ((Arows != C.getRows()) || (Bcols != C.getCols())) {
+ throw(vpException(vpException::dimensionError,
+ "In vpGEMM, cannot add resulting (%dx%d) matrix to (%dx%d) matrix",
+ Arows, Bcols, C.getRows(), C.getCols())) ;
+ }
+
+ GEMM2<T>(Arows,Brows,Bcols,A,B,alpha,C,beta,D);
+ }else{
+ GEMM1<T>(Arows,Brows,Bcols,A,B,alpha,D);
+ }
+
+}
+
+/*!
+ This function performs generalized matrix multiplication:
+ D = alpha*op(A)*op(B) + beta*op(C), where op(X) is X or X^T.
+ Operation on A, B and C matrices is described by enumeration vpGEMMmethod().
+
+ For example, to compute D = alpha*A^T*B^T+beta*C we need to call :
+ \code
+ vpGEMM(A, B, alpha, C, beta, D, VP_GEMM_A_T + VP_GEMM_B_T);
+ \endcode
+
+ If C is not used, vpGEMM must be called using an empty array \e null.
+ Thus to compute D = alpha*A^T*B, we have to call:
+ \code
+ vpGEMM(A, B, alpha, null, 0, D, VP_GEMM_B_T);
+ \endcode
+
+ \exception vpException::incorrectMatrixSizeError if the sizes of the matrices
+ do not allow the operations.
+
+ \param A : An array that could be a vpMatrix.
+ \param B : An array that could be a vpMatrix.
+ \param alpha : A scalar.
+ \param C : An array that could be a vpMatrix.
+ \param beta : A scalar.
+ \param D : The resulting array that could be a vpMatrix.
+ \param ops : A scalar describing operation applied on the matrices. Possible values
+ are the one defined in vpGEMMmethod(): VP_GEMM_A_T, VP_GEMM_B_T, VP_GEMM_C_T.
+
+ \relates vpArray2D
+
+*/
+inline void vpGEMM(const vpArray2D<double> & A, const vpArray2D<double> & B,
+ const double & alpha, const vpArray2D<double> & C,
+ const double & beta, vpArray2D<double> & D, const unsigned int &ops=0)
+{
+ switch(ops){
+ case 0 :
+ vpTGEMM<0>( A, B, alpha , C, beta, D);
+ break;
+ case 1 :
+ vpTGEMM<1>( A, B, alpha , C, beta, D);
+ break;
+ case 2 :
+ vpTGEMM<2>( A, B, alpha , C, beta, D);
+ break;
+ case 3 :
+ vpTGEMM<3>( A, B, alpha , C, beta, D);
+ break;
+ case 4 :
+ vpTGEMM<4>( A, B, alpha , C, beta, D);
+ break;
+ case 5 :
+ vpTGEMM<5>( A, B, alpha , C, beta, D);
+ break;
+ case 6 :
+ vpTGEMM<6>( A, B, alpha , C, beta, D);
+ break;
+ case 7 :
+ vpTGEMM<7>( A, B, alpha , C, beta, D);
+ break;
+ default:
+ throw(vpException(vpException::functionNotImplementedError,
+ "Operation on vpGEMM not implemented")) ;
+ break;
+ }
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpGaussRand.h b/modules/core/include/visp3/core/vpGaussRand.h
new file mode 100644
index 0000000..34b086a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpGaussRand.h
@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generation of random number with uniform and normal probability density.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpGaussRand_hh
+#define vpGaussRand_hh
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpUniRand.h>
+
+/*!
+ \class vpGaussRand
+ \ingroup group_core_random
+ \brief Class for generating random number with normal probability density.
+
+ The algorithms and notations used are described in \cite Gentle:2004.
+
+ The code below shows how to use the random generator to get values that have their mean equal to
+ 10 with a standart deviation equal to 0.5.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpGaussRand.h>
+
+int main()
+{
+ vpGaussRand noise(0.5, 10);
+ for(int i=0; i< 10; i++) {
+ std::cout << "noise " << i << ": " << noise() << std::endl;
+ }
+ return 0;
+}
+ \endcode
+
+ The previous example produces the following printings:
+\code
+noise 0: 9.43873
+noise 1: 10.1977
+noise 2: 10.8145
+noise 3: 9.13729
+noise 4: 8.86476
+noise 5: 9.83382
+noise 6: 9.43609
+noise 7: 9.34311
+noise 8: 9.62742
+noise 9: 9.37701
+\endcode
+
+ Note that the previous example produces always the same "random" results. To produce real random
+ values, you need to initialize the random generator with different values using seed(). For example,
+ this could be done using the current time. The code becomes:
+
+\code
+#include <iostream>
+#include <visp3/core/vpGaussRand.h>
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+ vpGaussRand noise(0.5, 10);
+ long seed = (long)vpTime::measureTimeMs();
+
+ noise.seed(seed);
+ for(int i=0; i< 10; i++) {
+ std::cout << "noise " << i << ": " << noise() << std::endl;
+ }
+ return 0;
+}
+\endcode
+
+ Now if you run the previous example you will always get different values:
+ \code
+noise 0: 10.5982
+noise 1: 9.19111
+noise 2: 9.82498
+noise 3: 9.07857
+noise 4: 9.9285
+noise 5: 10.3688
+noise 6: 9.75621
+noise 7: 10.3259
+noise 8: 10.4238
+noise 9: 10.2391
+ \endcode
+ */
+class VISP_EXPORT vpGaussRand : public vpUniRand
+{
+private :
+ double mean;
+ double sigma;
+ double gaussianDraw();
+
+public:
+
+ /*!
+ Default noise generator constructor.
+ */
+ vpGaussRand() : vpUniRand(), mean(0), sigma(0) {}
+
+ /*!
+ Gaussian noise random generator constructor.
+
+ \param sigma_val : Standard deviation.
+ \param mean_val : Mean value.
+ \param noise_seed : Seed of the noise
+ */
+ vpGaussRand(const double sigma_val, const double mean_val, const long noise_seed = 0)
+ : vpUniRand(noise_seed), mean(mean_val), sigma(sigma_val) {}
+
+ /*!
+ Set the standard deviation and mean for gaussian noise.
+
+ \param sigma_val : New standard deviation sigma.
+ \param mean_val : New mean value.
+ */
+ void setSigmaMean(const double sigma_val, const double mean_val) {
+ this->mean = mean_val;
+ this->sigma = sigma_val;
+ }
+
+ /*!
+ Set the seed of the noise.
+
+ \param seed_val : New seed.
+ */
+ void seed(const long seed_val) {
+ x=seed_val;
+ }
+
+ /*!
+ Return a random value from the Gaussian noise generator.
+ */
+ double operator()() {
+ return sigma*gaussianDraw()+mean;
+ }
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpHinkley.h b/modules/core/include/visp3/core/vpHinkley.h
new file mode 100644
index 0000000..24d579b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpHinkley.h
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Hinkley's cumulative sum test implementation.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpHinkley_H
+#define vpHinkley_H
+
+/*!
+ \file vpHinkley.h
+ \brief class for Hinkley's cumulative test computation.
+*/
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \class vpHinkley
+
+ \ingroup group_core_math_tools
+ \brief This class implements the Hinkley's cumulative sum test.
+
+ \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
+
+ The Hinkley's cumulative sum test is designed to detect jump in mean
+ of an observed signal \f$ s(t) \f$. It is known to be robust (by
+ taking into account all the past of the observed quantity),
+ efficient, and inducing a very low computational load. The other
+ attractive features of this test are two-fold. First, it can
+ straightforwardly and accurately provide the jump instant. Secondly,
+ due to its formulation (cumulative sum test), it can simultaneously
+ handle both very abrupt and important changes, and gradual smaller
+ ones without adapting the involved thresholds.
+
+ Two tests are performed in parallel to look for downwards or upwards
+ jumps in \f$ s(t) \f$, respectively defined by:
+
+ \f[ S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2}) \f]
+ \f[ M_k = \max_{0 \leq i \leq k} S_i\f]
+ \f[ T_k = \sum_{t=0}^{k} (s(t) - m_0 - \frac{\delta}{2}) \f]
+ \f[ N_k = \max_{0 \leq i \leq k} T_i\f]
+
+ In which \f$m_o\f$ is computed on-line and corresponds to the mean
+ of the signal \f$ s(t) \f$ we want to detect a jump. \f$m_o\f$ is
+ re-initialized at zero after each jump detection. \f$\delta\f$
+ denotes the jump minimal magnitude that we want to detect and
+ \f$\alpha\f$ is a predefined threshold. These values are set by
+ default to 0.2 in the default constructor vpHinkley(). To modify the
+ default values use setAlpha() and setDelta() or the
+ vpHinkley(double alpha, double delta) constructor.
+
+ A downward jump is detected if \f$ M_k - S_k > \alpha \f$.
+ A upward jump is detected if \f$ T_k - N_k > \alpha \f$.
+
+ To detect only downward jumps in \f$ s(t) \f$ use
+ testDownwardJump().To detect only upward jumps in \f$ s(t) \f$ use
+ testUpwardJump(). To detect both, downard and upward jumps use
+ testDownUpwardJump().
+
+ If a jump is detected, the jump location is given by the last instant
+ \f$k^{'}\f$ when \f$ M_{k^{'}} - S_{k^{'}} = 0 \f$, or \f$ T_{k^{'}} -
+ N_{k^{'}} = 0 \f$.
+
+*/
+class VISP_EXPORT vpHinkley
+{
+ public:
+ /*! \enum vpHinkleyJumpType
+ Indicates if a jump is detected by the Hinkley test.
+ */
+ typedef enum {
+ noJump, /*!< No jump is detected by the Hinkley test. */
+ downwardJump, /*!< A downward jump is detected by the Hinkley test. */
+ upwardJump /*!< An upward jump is detected by the Hinkley test. */
+ } vpHinkleyJumpType;
+
+ public:
+ vpHinkley();
+ ~vpHinkley();
+ vpHinkley(double alpha, double delta);
+
+ void init();
+ void init(double alpha, double delta) ;
+
+ void setDelta(double delta);
+ void setAlpha(double alpha);
+ vpHinkleyJumpType testDownwardJump(double signal);
+ vpHinkleyJumpType testUpwardJump(double signal);
+ vpHinkleyJumpType testDownUpwardJump(double signal);
+
+ static void print(vpHinkleyJumpType jump) ;
+
+ /*!
+ \return The mean value \f$m_0\f$ of the signal \f$ s(t) \f$.
+
+ */
+ inline double getMean() {return mean;}
+ /*!
+ \return The value of \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2})\f$
+
+ */
+ inline double getSk() {return Sk;}
+ /*!
+ \return The value of \f$M_k\f$, the maximum value of \f$S_k\f$.
+
+ */
+ inline double getMk() {return Mk;}
+ /*!
+
+ \return The value of \f$T_k = \sum_{t=0}^{k} (s(t) - m_0 -
+ \frac{\delta}{2})\f$
+
+ */
+ inline double getTk() {return Tk;}
+ /*!
+ \return The value of \f$N_k\f$, the maximum value of \f$T_k\f$.
+
+ */
+ inline double getNk() {return Nk;}
+ private:
+ void computeMean(double signal);
+ void computeSk(double signal);
+ void computeMk();
+ void computeTk(double signal);
+ void computeNk();
+
+ private:
+ double dmin2;
+ double alpha;
+ int nsignal; // Signal length
+ double mean; // Signal mean value
+ double Sk;
+ double Mk;
+ double Tk;
+ double Nk;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h
new file mode 100644
index 0000000..c1d5020
--- /dev/null
+++ b/modules/core/include/visp3/core/vpHistogram.h
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpHistogram.h
+ \brief Declaration of the vpHistogram class.
+ Class vpHistogram defines gray level image histograms
+
+*/
+
+#ifndef vpHistogram_h
+#define vpHistogram_h
+
+#include <sstream>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpHistogramPeak.h>
+#include <visp3/core/vpHistogramValey.h>
+#include <visp3/core/vpColor.h>
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+# include <visp3/core/vpList.h>
+#endif
+
+#include <list>
+
+/*!
+ \class vpHistogram
+ \ingroup group_core_histogram
+ \brief Class to compute a gray level image histogram.
+
+ Here are two examples showing how to use this class to determine the
+ threshold which can be used to segment two objects.
+
+ The code below:
+
+ \code
+ vpImage<unsigned char> I;
+ ...
+ unsigned char dist = 60;
+ vpHistogramValey valey;
+ vpHistogram h(I);
+ h.smooth(); // Filter the histogram values
+ vpHistogramPeak peakl, peakr; // Two highest peaks in the histogram
+ // - peakl: Peak on the left
+ // - peakr: Peak on the right
+
+ h.getPeaks(dist, peakl, peakr, valey);
+ unsigned char threshold; // Position of the valey between the two peaks
+ threshold = valey.getLevel();
+ \endcode
+
+ has the same behaviour than this one:
+
+ \code
+ vpImage<unsigned char> I;
+ ...
+ unsigned char dist = 60;
+ vpHistogram h(I);
+ h.smooth(); // Filter the histogram values
+ vpHistogramPeak peak1, peak2; // Two highest peaks in the histogram
+ // - peak1: Highest peak
+ // - peakr: Second highest peak
+
+ // Get the two highest peaks
+ h.getPeaks(dist, peak1, peak2);
+
+ // Get the valey between the two highest peaks
+ vpHistogramValey valey;
+ h.getValey(peak1, peak2, valey);
+
+ unsigned char threshold; // Position of the valey between the two peaks
+ threshold = valey.getLevel();
+ \endcode
+
+*/
+class VISP_EXPORT vpHistogram
+{
+public:
+ vpHistogram();
+ vpHistogram(const vpHistogram &h);
+ vpHistogram(const vpImage<unsigned char> &I);
+ virtual ~vpHistogram();
+
+ vpHistogram & operator=(const vpHistogram &h);
+
+ /*!
+
+ Return the number of pixels having the gray \e level.
+
+ \param level : Gray level in the histogram.
+
+ \return Number of pixels having the gray level.
+
+ \code
+ vpImage<unsigned char> I; // A gray level image
+
+ vpHistogram h;
+ h.calculate(I); // Histogram of the gray level image
+
+ // Print the histogram values
+ for (int i=0; i < h.getSize(); i ++)
+ printf("%d: %d\n", i, h[i]);
+ \endcode
+
+ */
+ inline unsigned operator[](const unsigned char level) const
+ {
+ if (level < size) {
+ return histogram[level];
+ }
+
+ std::stringstream ss;
+ ss << "Level is > to size (" << size << ") !";
+ throw vpException(vpException::dimensionError, ss.str().c_str());
+ };
+ /*!
+
+ Return the number of pixels having the gray \e level.
+
+ \param level : Gray level in the histogram.
+
+ \return Number of pixels having the gray level.
+
+ \code
+ vpImage<unsigned char> I; // A gray level image
+
+ vpHistogram h;
+ h.calculate(I); // Histogram of the gray level image
+
+ // Print the histogram values
+ for (int i=0; i < h.getSize(); i ++)
+ printf("%d: %d\n", i, h(i));
+ \endcode
+
+ */
+ inline unsigned operator()(const unsigned char level) const
+ {
+ if(level < size) {
+ return histogram[level];
+ }
+
+ std::stringstream ss;
+ ss << "Level is > to size (" << size << ") !";
+ throw vpException(vpException::dimensionError, ss.str().c_str());
+ };
+ /*!
+
+ Return the number of pixels having the gray \e level.
+
+ \param level : Gray level in the histogram.
+
+ \return Number of pixels having the gray level.
+
+ \code
+ vpImage<unsigned char> I; // A gray level image
+
+ vpHistogram h;
+ h.calculate(I); // Histogram of the gray level image
+
+ // Print the histogram values
+ for (int i=0; i < h.getSize(); i ++)
+ printf("%d: %d\n", i, h.get(i));
+ \endcode
+
+ */
+ inline unsigned get(const unsigned char level) const
+ {
+ if(level < size) {
+ return histogram[level];
+ }
+
+ std::stringstream ss;
+ ss << "Level is > to size (" << size << ") !";
+ throw vpException(vpException::dimensionError, ss.str().c_str());
+ };
+
+ /*!
+
+ Set the number of pixels having the gray \e level.
+
+ \param level : Gray level in the histogram. Level is in [0:255]
+
+ \param value : Number of pixels having the gray level.
+
+ \code
+ vpHistogram h;
+
+ // Set histogram values
+ for (int i=0; i < h.getSize(); i ++)
+ h.set(i, i*2); // for each level i, set a value of 2*i
+ \endcode
+
+ */
+ inline void set(const unsigned char level, unsigned int value)
+ {
+ if(level < size) {
+ histogram[level] = value;
+ } else {
+ std::stringstream ss;
+ ss << "Level is > to size (" << size << ") !";
+ throw vpException(vpException::dimensionError, ss.str().c_str());
+ }
+ };
+
+ void calculate(const vpImage<unsigned char> &I, const unsigned int nbins=256);
+
+ void display(const vpImage<unsigned char> &I, const vpColor &color=vpColor::white, const unsigned int thickness=2,
+ const unsigned int maxValue_=0);
+
+ void smooth(const unsigned int fsize = 3);
+ unsigned getPeaks(std::list<vpHistogramPeak> & peaks);
+ unsigned getPeaks(unsigned char dist,
+ vpHistogramPeak & peak1,
+ vpHistogramPeak & peak2);
+ bool getPeaks(unsigned char dist,
+ vpHistogramPeak & peakl,
+ vpHistogramPeak & peakr,
+ vpHistogramValey & valey);
+ unsigned getValey(std::list<vpHistogramValey> & valey);
+ bool getValey(const vpHistogramPeak & peak1,
+ const vpHistogramPeak & peak2,
+ vpHistogramValey & valey);
+ unsigned getValey(unsigned char dist,
+ const vpHistogramPeak & peak,
+ vpHistogramValey & valeyl,
+ vpHistogramValey & valeyr);
+ unsigned sort(std::list<vpHistogramPeak> & peaks);
+
+ bool write(const std::string &filename);
+ bool write(const char *filename);
+
+ /*!
+ Get the histogram size.
+
+ \return The size of the histogram, or the image maximum gray
+ levels numbers.
+
+ \sa getValues()
+ */
+ inline unsigned getSize() const
+ {
+ return size;
+ };
+
+ /*!
+
+ Get the histogram values.
+
+ \return A pointer to the array of histogram values. The size of
+ this array is given by getSize().
+
+ \code
+ vpImage<unsigned char> I; // A gray level image
+
+ vpHistogram h;
+ h.calculate(I); // Histogram of the gray level image
+
+ // Print the histogram values
+ unsigned char *values = h.getValues();
+ for (int i=0; i < h.getSize(); i ++)
+ printf("%d: %d\n", i, values[i]);
+ \endcode
+
+ \sa getSize()
+ */
+ inline unsigned * getValues()
+ {
+ return histogram;
+ };
+
+private:
+ void init(unsigned size = 256);
+
+ unsigned int *histogram;
+ unsigned size; // Histogram size (max allowed 256)
+};
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpHistogramPeak.h b/modules/core/include/visp3/core/vpHistogramPeak.h
new file mode 100644
index 0000000..aa1af95
--- /dev/null
+++ b/modules/core/include/visp3/core/vpHistogramPeak.h
@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpHistogramPeak.h
+ \brief Declaration of the vpHistogramPeak class.
+ Class vpHistogramPeak defines a gray level histogram peak.
+
+*/
+
+#ifndef vpHistogramPeak_h
+#define vpHistogramPeak_h
+
+#include <visp3/core/vpConfig.h>
+
+#include <ostream>
+
+/*!
+ \class vpHistogramPeak
+
+ \ingroup group_core_histogram
+ \brief Declaration of the peak (maximum value) in a gray level image
+ histogram.
+
+ A peak is internally represented as a gray \e level and a \e value.
+ The \e value represents the number of pixels having the gray \e level.
+
+*/
+
+class VISP_EXPORT vpHistogramPeak
+{
+public :
+ vpHistogramPeak();
+ vpHistogramPeak(unsigned char level, unsigned value);
+ vpHistogramPeak(const vpHistogramPeak & p);
+
+ /*! Destructor that does nothing. */
+ virtual ~vpHistogramPeak() {}
+
+ vpHistogramPeak & operator=(const vpHistogramPeak &p);
+ bool operator==(const vpHistogramPeak &p) const;
+
+ /*!
+
+ Set the peak gray \e level. To set the number of pixels having this
+ gray level use setValue().
+
+ \param lvl : Location of the peak or gray \e level.
+
+ \sa setValue(), set()
+
+ */
+ inline void setLevel(unsigned char lvl)
+ {
+ this->level = lvl;
+ };
+ /*!
+
+ Set the peak number of pixels having a same gray level. To set the
+ gray level of this peak use setLevel().
+
+ \param val : Number of pixels having the same location or gray level.
+
+ \sa setLevel(), set()
+
+ */
+ inline void setValue(unsigned val)
+ {
+ this->value = val;
+ };
+ /*!
+
+ Set the peak gray \e level and number of pixels at this gray level.
+
+ \param lvl : Location of the peak or gray level.
+ \param val : Number of pixels having the same location or gray level.
+
+ \sa setLevel(), setValue()
+
+ */
+ inline void set(unsigned char lvl, unsigned val)
+ {
+ this->level = lvl;
+ this->value = val;
+ };
+
+ /*!
+
+ Get the peak gray \e level. The number of pixels having this
+ gray level is available throw getValue().
+
+ \return Location of the peak or gray level.
+
+ \sa getValue()
+
+ */
+ inline unsigned char getLevel() const
+ {
+ return level;
+ };
+ /*!
+
+ Get the peak number of pixels having a same gray level. The
+ corresponding gray level is available throw getLevel().
+
+ \return : Number of pixels having the same location or gray level.
+
+ \sa getLevel()
+
+ */
+ inline unsigned getValue() const
+ {
+ return value;
+ };
+
+ //---------------------------------
+ // Printing
+ //---------------------------------
+ friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpHistogramPeak &p);
+
+
+protected :
+ unsigned char level; //! Gray level ot the peak.
+ unsigned value; //! Number of pixels on the gray level.
+};
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
diff --git a/modules/core/include/visp3/core/vpHistogramValey.h b/modules/core/include/visp3/core/vpHistogramValey.h
new file mode 100644
index 0000000..d141a19
--- /dev/null
+++ b/modules/core/include/visp3/core/vpHistogramValey.h
@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpHistogramValey.h
+ \brief Declaration of the vpHistogramValey class.
+ Class vpHistogramValey defines a gray level histogram valey.
+
+*/
+
+#ifndef vpHistogramValey_h
+#define vpHistogramValey_h
+
+#include <visp3/core/vpHistogramPeak.h>
+
+/*!
+ \class vpHistogramValey
+
+ \ingroup group_core_histogram
+ \brief Declaration of the valey (minimum value) in a gray level image
+ histogram.
+
+ A valey is internally represented as a gray \e level and a \e value.
+ The \e value represents the number of pixels having the gray \e level.
+
+*/
+
+class VISP_EXPORT vpHistogramValey : vpHistogramPeak
+{
+ public:
+ vpHistogramValey() :
+ vpHistogramPeak() {};
+
+ vpHistogramValey(unsigned char lvl, unsigned val) :
+ vpHistogramPeak(lvl, val) {};
+
+ vpHistogramValey(const vpHistogramValey & v) :
+ vpHistogramPeak(v) {};
+
+ /*! Destructor that does nothing. */
+ virtual ~vpHistogramValey() {}
+
+ vpHistogramValey & operator=(const vpHistogramValey &v);
+ bool operator==(const vpHistogramValey &v) const;
+
+ /*!
+
+ Set the valey gray \e level. To set the number of pixels having this
+ gray level use setValue().
+
+ \param lvl : Location of the valey or gray level.
+
+ \sa setValue(), set()
+
+ */
+ inline void setLevel(unsigned char lvl)
+ {
+ this->level = lvl;
+ };
+ /*!
+
+ Set the valey number of pixels having a same gray level. To set the
+ gray level of this valey use setLevel().
+
+ \param val : Number of pixels having the same location or gray level.
+
+ \sa setPosition(), set()
+
+ */
+ inline void setValue(unsigned val)
+ {
+ this->value = val;
+ };
+ /*!
+
+ Set the valey gray \e level and number of pixels at this location.
+
+ \param lvl : Location of the valey or gray level.
+ \param val : Number of pixels having the same location or gray level.
+
+ \sa setLevel(), setValue()
+
+ */
+ inline void set(unsigned char lvl, unsigned val)
+ {
+ this->level = lvl;
+ this->value = val;
+ };
+
+ /*!
+
+ Get the valey gray \e level. The number of pixels having this
+ gray level is available through getValue().
+
+ \return Location of the valey or gray level.
+
+ \sa getValue()
+
+ */
+ inline unsigned char getLevel() const
+ {
+ return level;
+ };
+ /*!
+
+ Get the valey number of pixels having a same gray level. The
+ corresponding gray level is available through getLevel().
+
+ \return Number of pixels having the same location or gray level.
+
+ \sa getLevel()
+
+ */
+ inline unsigned getValue() const
+ {
+ return value;
+ };
+
+ //---------------------------------
+ // Printing
+ //---------------------------------
+ friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpHistogramValey &v);
+};
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
diff --git a/modules/core/include/visp3/core/vpHomogeneousMatrix.h b/modules/core/include/visp3/core/vpHomogeneousMatrix.h
new file mode 100644
index 0000000..3314daa
--- /dev/null
+++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h
@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homogeneous matrix.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpHomogeneousMatrix.h
+ \brief Definition and computation on the homogeneous matrices
+*/
+
+
+#ifndef VPHOMOGENEOUSMATRIX_HH
+#define VPHOMOGENEOUSMATRIX_HH
+
+class vpTranslationVector;
+class vpPoseVector;
+class vpMatrix;
+class vpRotationMatrix;
+class vpPoseVector;
+class vpThetaUVector;
+class vpQuaternionVector;
+class vpPoint;
+
+#include <vector>
+#include <fstream>
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+//#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpPoseVector.h>
+
+/*!
+ \class vpHomogeneousMatrix
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of an homogeneous matrix and operations on such kind of matrices.
+
+ The class provides a data structure for the homogeneous matrices
+ as well as a set of operations on these matrices.
+
+ The vpHomogeneousMatrix class is derived from vpArray2D<double>.
+
+ An homogeneous matrix is 4x4 matrix defines as
+ \f[
+ ^a{\bf M}_b = \left(\begin{array}{cc}
+ ^a{\bf R}_b & ^a{\bf t}_b \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right)
+ \f]
+ that defines the position of frame <em>b</em> in frame <em>a</em>
+
+ \f$ ^a{\bf R}_b \f$ is a rotation matrix and
+ \f$ ^a{\bf t}_b \f$ is a translation vector.
+
+*/
+class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
+{
+ public:
+ vpHomogeneousMatrix();
+ vpHomogeneousMatrix(const vpHomogeneousMatrix &M) ;
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu) ;
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) ;
+ vpHomogeneousMatrix(const vpPoseVector &p) ;
+ vpHomogeneousMatrix(const std::vector<float> &v);
+ vpHomogeneousMatrix(const std::vector<double> &v);
+ vpHomogeneousMatrix(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz) ;
+ /*!
+ Destructor.
+ */
+ virtual ~vpHomogeneousMatrix() {};
+
+ void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) ;
+ void buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu) ;
+ void buildFrom(const vpTranslationVector &t, const vpQuaternionVector& q) ;
+ void buildFrom(const vpPoseVector &p) ;
+ void buildFrom(const std::vector<float> &v) ;
+ void buildFrom(const std::vector<double> &v) ;
+ void buildFrom(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz) ;
+
+ void convert(std::vector<float> &M);
+ void convert(std::vector<double> &M);
+
+ // Set to identity
+ void eye();
+
+ vpColVector getCol(const unsigned int j) const;
+ vpRotationMatrix getRotationMatrix();
+ vpThetaUVector getThetaUVector();
+ vpTranslationVector getTranslationVector();
+
+ // Invert the homogeneous matrix.
+ vpHomogeneousMatrix inverse() const ;
+ // Invert the homogeneous matrix.
+ void inverse(vpHomogeneousMatrix &Mi) const;
+
+ // Test if the rotational part of the matrix is a rotation matrix.
+ bool isAnHomogeneousMatrix() const ;
+
+ void insert(const vpRotationMatrix &R) ;
+ void insert(const vpThetaUVector &tu) ;
+ void insert(const vpTranslationVector &t) ;
+ void insert(const vpQuaternionVector &t) ;
+
+ void extract(vpRotationMatrix &R) const;
+ void extract(vpThetaUVector &tu) const;
+ void extract(vpTranslationVector &t) const;
+ void extract(vpQuaternionVector& q) const;
+
+ // Load an homogeneous matrix from a file
+ void load(std::ifstream &f) ;
+ // Save an homogeneous matrix in a file
+ void save(std::ofstream &f) const ;
+
+ vpHomogeneousMatrix &operator=(const vpHomogeneousMatrix &M);
+ vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const;
+
+ vpColVector operator*(const vpColVector &v) const;
+ // Multiply by a point
+ vpPoint operator*(const vpPoint &bP) const;
+
+ void print() const;
+
+ /*!
+ This function is not applicable to an homogeneous matrix that is always a
+ 4-by-4 matrix.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize an homogeneous matrix"));
+ };
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use eye().
+ */
+ vp_deprecated void setIdentity();
+ //@}
+#endif
+
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h
new file mode 100644
index 0000000..a4ace00
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImage.h
@@ -0,0 +1,1595 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image handling.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpImage.h
+ \brief Image handling.
+*/
+
+#ifndef vpImage_H
+#define vpImage_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImageException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRGBa.h>
+
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <string.h>
+
+class vpDisplay;
+
+/*!
+ \class vpImage
+
+ \ingroup group_core_image
+
+ \brief Definition of the vpImage class member functions.
+
+ This is a template class, therefore the type of each element of the
+ array is not a priori defined.
+
+ <h3> Data structure </h3>
+
+ Each image is build using two structure (an array bitmap which size
+ is [width*height]) and an array of pointer row (which size is [nrow])
+ the ith element in the row array row[i] is pointer toward the ith
+ "line" of the image (ie, bitmap +i*width )
+
+ \image html image-data-structure.gif
+ \image latex image-data-structure.ps width=10cm
+
+ Such a structure allows a fast acces to each element of the image.
+ if i is the ith rows and j the jth columns the value of this pixel
+ is given by I[i][j] (that is equivalent to row[i][j]).
+
+ <h3>Example</h3>
+ The following example available in tutorial-image-manipulation.cpp shows how
+ to create gray level and color images and how to access to the pixels.
+ \include tutorial-image-manipulation.cpp
+
+ <h3>Important remark</h3> To provide high-performance access there
+ is no verification to ensure that 0 \f$\le\f$ i < height and 0
+ \f$\le\f$ j < width. Since the memory allocated in the bitmap array
+ is continuous, that means that if (i, j) is outside the image you
+ will manipulate a pixel that is not as expected. To highlight this
+ remark, we provide hereafter an example where the considered pixel
+ is outside the image:
+
+\code
+unsigned int width = 320;
+unsigned int height = 240;
+vpImage<unsigned char> I(height, width); // Create an 320x240 image
+// Set pixel coordinates that is outside the image
+unsigned int i = 100;
+unsigned int j = 400;
+unsigned char value;
+value = I[i][j]; // Here we will get the pixel value at position (101, 80)
+\endcode
+
+*/
+template<class Type>
+class vpImage
+{
+ friend class vpImageConvert;
+
+public:
+ Type *bitmap ; //!< points toward the bitmap
+ vpDisplay *display ;
+
+ //! constructor
+ vpImage() ;
+ //! copy constructor
+ vpImage(const vpImage<Type>&);
+ //! constructor set the size of the image
+ vpImage(unsigned int height, unsigned int width) ;
+ //! constructor set the size of the image and init all the pixel
+ vpImage(unsigned int height, unsigned int width, Type value) ;
+ //! constructor from an image stored as a continuous array in memory
+ vpImage(Type * const array, const unsigned int height, const unsigned int width, const bool copyData=false) ;
+ //! destructor
+ virtual ~vpImage() ;
+ //! set the size of the image
+ void init(unsigned int height, unsigned int width) ;
+ //! set the size of the image
+ void init(unsigned int height, unsigned int width, Type value) ;
+ //! init from an image stored as a continuous array in memory
+ void init(Type * const array, const unsigned int height, const unsigned int width, const bool copyData=false);
+ //! set the size of the image without initializing it.
+ void resize(const unsigned int h, const unsigned int w) ;
+ //! set the size of the image and initialize it.
+ void resize(const unsigned int h, const unsigned int w, const Type val) ;
+ //! destructor
+ void destroy() ;
+
+ /*!
+ Get the image height.
+
+ \return The image height.
+
+ \sa getWidth()
+
+ */
+ inline unsigned int getHeight() const { return height; }
+ /*!
+ Get the image width.
+
+ \return The image width.
+
+ \sa getHeight()
+
+ */
+ inline unsigned int getWidth() const { return width; }
+
+ /*!
+
+ Get the number of rows in the image.
+
+ \return The image number of rows, or image height.
+
+ \sa getHeight()
+ */
+ inline unsigned int getRows() const { return height ; }
+
+ /*!
+ Get the number of columns in the image.
+
+ \return The image number of column, or image width.
+
+ \sa getWidth()
+ */
+ inline unsigned int getCols() const { return width ; }
+
+ /*!
+ Get the image size.
+
+ \return The image size = width * height.
+
+ \sa getWidth(), getHeight()
+ */
+ inline unsigned int getSize() const { return width*height ; }
+
+
+ // Return the maximum value within the bitmap
+ Type getMaxValue() const ;
+ // Return the minumum value within the bitmap
+ Type getMinValue() const ;
+ //Look for the minumum and the maximum value within the bitmap
+ void getMinMaxValue(Type &min, Type &max) const;
+
+ // Gets the value of a pixel at a location with bilinear interpolation.
+ Type getValue(double i, double j) const;
+ // Gets the value of a pixel at a location with bilinear interpolation.
+ Type getValue(vpImagePoint &ip) const;
+
+ /*!
+
+ Get the image number of pixels which corresponds to the image
+ width multiplied by the image height.
+
+ \return The image number of pixels or image size.
+
+
+ \sa getWidth(), getHeight()
+ */
+ inline unsigned int getNumberOfPixel() const{ return npixels; }
+
+ //------------------------------------------------------------------
+ // Acces to the image
+
+
+ //! operator[] allows operation like I[i] = x.
+ inline Type *operator[]( const unsigned int i) { return row[i];}
+ inline Type *operator[]( const int i) { return row[i];}
+
+ //! operator[] allows operation like x = I[i]
+ inline const Type *operator[](unsigned int i) const { return row[i];}
+ inline const Type *operator[](int i) const { return row[i];}
+
+ /*!
+ Get the value of an image point.
+
+ \param i,j : Image point coordinates; i for the row position, j for the
+ column position.
+
+ \return Value of the image point (i, j).
+
+ */
+ inline Type operator()(const unsigned int i, const unsigned int j) const
+ {
+ return bitmap[i*width+j] ;
+ }
+ /*!
+ Set the value of an image point.
+
+ \param i, j: Image point coordinates; i for the row position, j for the
+ column position.
+
+ \param v : Value to set for image point (i, j).
+
+ */
+ inline void operator()(const unsigned int i, const unsigned int j,
+ const Type &v)
+ {
+ bitmap[i*width+j] = v ;
+ }
+ /*!
+ Get the value of an image point.
+
+ \param ip : An image point with sub-pixel coordinates. Sub-pixel
+ coordinates are roughly transformed to insigned int coordinates by cast.
+
+ \return Value of the image point \e ip.
+
+ \sa getValue(const vpImagePoint &)
+
+ */
+ inline Type operator()(const vpImagePoint &ip) const
+ {
+ unsigned int i = (unsigned int) ip.get_i();
+ unsigned int j = (unsigned int) ip.get_j();
+
+ return bitmap[i*width+j] ;
+ }
+ /*!
+ Set the value of an image point.
+
+ \param ip : An image point with sub-pixel coordinates. Sub-pixel
+ coordinates are roughly transformed to insigned int coordinates by cast.
+
+ \param v : Value to set for the image point.
+
+ */
+ inline void operator()(const vpImagePoint &ip,
+ const Type &v)
+ {
+ unsigned int i = (unsigned int) ip.get_i();
+ unsigned int j = (unsigned int) ip.get_j();
+
+ bitmap[i*width+j] = v ;
+ }
+
+ vpImage<Type> operator-(const vpImage<Type> &B);
+
+ //! Copy operator
+ vpImage<Type>& operator=(const vpImage<Type> &I) ;
+
+ vpImage<Type>& operator=(const Type &v);
+ bool operator==(const vpImage<Type> &I);
+ bool operator!=(const vpImage<Type> &I);
+
+ void insert(const vpImage<Type> &src, const vpImagePoint topLeft);
+
+
+ // Returns a new image that's half size of the current image
+ void halfSizeImage(vpImage<Type> &res);
+
+ // Returns a new image that's a quarter size of the current image
+ void quarterSizeImage(vpImage<Type> &res);
+
+ // Returns a new image that's double size of the current image
+ void doubleSizeImage(vpImage<Type> &res);
+
+ void sub(const vpImage<Type> &B, vpImage<Type> &C);
+ void sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C);
+
+ // Perform a look-up table transformation
+ void performLut(const Type (&lut)[256]);
+
+private:
+ unsigned int npixels ; //<! number of pixel in the image
+ unsigned int width ; //<! number of columns
+ unsigned int height ; //<! number of rows
+ Type **row ; //!< points the row pointer array
+ } ;
+
+
+/*!
+ \brief Image initialisation
+
+ Allocate memory for an [h x w] image.
+
+ \param w : Image width.
+ \param h : Image height.
+ \param value : Set all the element of the bitmap to \e value.
+
+ \exception vpException::memoryAllocationError
+
+ \sa vpImage::init(h, w)
+*/
+template<class Type>
+void
+vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
+{
+ try
+ {
+ init(h,w) ;
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+
+ for (unsigned int i=0 ; i < npixels ; i++)
+ bitmap[i] = value ;
+}
+
+
+/*!
+ \brief Image initialization
+
+ Allocate memory for an [h x w] image.
+
+ \param w : Image width.
+ \param h : Image height.
+
+ Element of the bitmap are not initialized
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \exception vpException::memoryAllocationError
+
+*/
+template<class Type>
+void
+vpImage<Type>::init(unsigned int h, unsigned int w)
+{
+ if (h != this->height) {
+ if (row != NULL) {
+ vpDEBUG_TRACE(10,"Destruction row[]");
+ delete [] row;
+ row = NULL;
+ }
+ }
+
+ if ((h != this->height) || (w != this->width))
+ {
+ if (bitmap != NULL) {
+ vpDEBUG_TRACE(10,"Destruction bitmap[]") ;
+ delete [] bitmap;
+ bitmap = NULL;
+ }
+ }
+
+ this->width = w ;
+ this->height = h;
+
+ npixels=width*height;
+
+ if (bitmap == NULL) bitmap = new Type[npixels] ;
+
+ // vpERROR_TRACE("Allocate bitmap %p",bitmap) ;
+ if (bitmap == NULL)
+ {
+ vpERROR_TRACE("cannot allocate bitmap ") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate bitmap ")) ;
+ }
+
+ if (row == NULL) row = new Type*[height] ;
+// vpERROR_TRACE("Allocate row %p",row) ;
+ if (row == NULL)
+ {
+ vpERROR_TRACE("cannot allocate row ") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate row ")) ;
+ }
+
+ unsigned int i ;
+ for ( i =0 ; i < height ; i++)
+ row[i] = bitmap + i*width ;
+}
+
+/*!
+ \brief Image initialization
+
+ Init from image data stored as a continuous array in memory.
+
+ \param array : Image data stored as a continuous array in memory
+ \param h : Image height.
+ \param w : Image width.
+ \param copyData : If false (by default) only the memory address is copied, otherwise the data are copied.
+
+ \exception vpException::memoryAllocationError
+*/
+template<class Type>
+void
+vpImage<Type>::init(Type * const array, const unsigned int h, const unsigned int w, const bool copyData)
+{
+ if (h != this->height) {
+ if (row != NULL) {
+ delete [] row;
+ row = NULL;
+ }
+ }
+
+ //Delete bitmap if copyData==false, otherwise only if the dimension differs
+ if ( (copyData && ((h != this->height) || (w != this->width))) || !copyData ) {
+ if (bitmap != NULL) {
+ delete [] bitmap;
+ bitmap = NULL;
+ }
+ }
+
+ this->width = w ;
+ this->height = h;
+
+ npixels = width*height;
+
+ if(copyData) {
+ if (bitmap == NULL) bitmap = new Type[npixels];
+
+ if (bitmap == NULL) {
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate bitmap ")) ;
+ }
+
+ //Copy the image data
+ memcpy(bitmap, array, (size_t) (npixels * sizeof(Type)));
+ } else {
+ //Copy the address of the array in the bitmap
+ bitmap = array;
+ }
+
+ if (row == NULL) row = new Type*[height];
+ if (row == NULL) {
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate row ")) ;
+ }
+
+ for (unsigned int i = 0 ; i < height ; i++) {
+ row[i] = bitmap + i*width;
+ }
+}
+
+/*!
+ \brief Constructor
+
+ Allocate memory for an [h x w] image.
+
+ \param w : Image width.
+ \param h : Image height.
+
+ Element of the bitmap are set to zero.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \exception vpException::memoryAllocationError
+
+ \sa vpImage::init(height, width)
+*/
+template<class Type>
+vpImage<Type>::vpImage(unsigned int h, unsigned int w)
+ : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+{
+ try
+ {
+ init(h,w,0) ;
+ }
+ catch(...)
+ {
+ throw ;
+ }
+}
+
+/*!
+ \brief Constructor
+
+ Allocate memory for an [height x width] image.
+
+ \param w : Image width.
+ \param h : Image height.
+ \param value : Set all the element of the bitmap to value.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \return MEMORY_FAULT if memory allocation is impossible, else OK
+
+ \sa vpImage::init(height, width, value)
+*/
+template<class Type>
+vpImage<Type>::vpImage (unsigned int h, unsigned int w, Type value)
+ : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+{
+ try
+ {
+ init(h,w,value) ;
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+/*!
+ \brief Constructor
+
+ Construct a vpImage from a continuous array in memory.
+
+ \param array : Image data stored as a continuous array in memory.
+ \param h : Image height.
+ \param w : Image width.
+ \param copyData : If false (by default) only the memory address is copied, otherwise the data are copied.
+
+ \return MEMORY_FAULT if memory allocation is impossible, else OK
+
+ \sa vpImage::init(array, height, width)
+*/
+template<class Type>
+vpImage<Type>::vpImage (Type * const array, const unsigned int h, const unsigned int w, const bool copyData)
+ : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+{
+ try
+ {
+ init(array, h, w, copyData);
+ }
+ catch(vpException &)
+ {
+ throw ;
+ }
+}
+
+/*!
+ \brief Constructor
+
+ No memory allocation is done
+
+ set all the element of the bitmap to value
+
+ \sa vpImage::resize(height, width) for memory allocation
+*/
+template<class Type>
+vpImage<Type>::vpImage()
+ : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+{
+}
+
+/*!
+ \brief resize the image : Image initialization
+
+ Allocate memory for an [height x width] image.
+
+ \warning The image is not initialized.
+
+ \param w : Image width.
+ \param h : Image height.
+
+ Element of the bitmap are not initialized
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \exception vpException::memoryAllocationError
+
+ \sa init(unsigned int, unsigned int)
+*/
+template<class Type>
+void
+vpImage<Type>::resize(unsigned int h, unsigned int w)
+{
+ try
+ {
+ init(h, w) ;
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+/*!
+ \brief resize the image : Image initialization
+
+ Allocate memory for an [height x width] image and initialize the image.
+
+ \param w : Image width.
+ \param h : Image height.
+ \param val : Pixels value.
+
+ Element of the bitmap are not initialized
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \exception vpException::memoryAllocationError
+
+ \sa init(unsigned int, unsigned int)
+*/
+template<class Type>
+void
+vpImage<Type>::resize(unsigned int h, unsigned int w, const Type val)
+{
+ try
+ {
+ init(h, w, val) ;
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+
+/*!
+ \brief Destructor : Memory de-allocation
+
+ \warning does not deallocate memory for display and video
+
+*/
+template<class Type>
+void
+vpImage<Type>::destroy()
+{
+ // vpERROR_TRACE("Deallocate ") ;
+
+
+ if (bitmap!=NULL)
+ {
+ // vpERROR_TRACE("Deallocate bitmap memory %p",bitmap) ;
+// vpDEBUG_TRACE(20,"Deallocate bitmap memory %p",bitmap) ;
+ delete [] bitmap ;
+ bitmap = NULL;
+ }
+
+
+ if (row!=NULL)
+ {
+ // vpERROR_TRACE("Deallocate row memory %p",row) ;
+// vpDEBUG_TRACE(20,"Deallocate row memory %p",row) ;
+ delete [] row ;
+ row = NULL;
+ }
+
+}
+
+/*!
+ \brief Destructor : Memory de-allocation
+
+ \warning does not deallocate memory for display and video
+
+*/
+template<class Type>
+vpImage<Type>::~vpImage()
+{
+ destroy() ;
+}
+
+
+
+/*!
+ Copy constructor
+*/
+template<class Type>
+vpImage<Type>::vpImage(const vpImage<Type>& I)
+ : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+{
+ try
+ {
+ resize(I.getHeight(),I.getWidth());
+ memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
+ for (unsigned int i =0 ; i < this->height ; i++) row[i] = bitmap + i*this->width ;
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+/*!
+ \brief Return the maximum value within the bitmap
+
+ \sa getMinValue()
+*/
+template<class Type>
+Type vpImage<Type>::getMaxValue() const
+{
+ Type m = bitmap[0] ;
+ for (unsigned int i=0 ; i < npixels ; i++)
+ {
+ if (bitmap[i]>m) m = bitmap[i] ;
+ }
+ return m ;
+}
+
+/*!
+ \brief Return the minimum value within the bitmap
+
+ \sa getMaxValue()
+*/
+template<class Type>
+Type vpImage<Type>::getMinValue() const
+{
+ Type m = bitmap[0];
+ for (unsigned int i=0 ; i < npixels ; i++)
+ if (bitmap[i]<m) m = bitmap[i] ;
+ return m ;
+}
+
+
+/*!
+ \brief Look for the minimum and the maximum value within the bitmap
+
+ \sa getMaxValue()
+ \sa getMinValue()
+*/
+template<class Type>
+void vpImage<Type>::getMinMaxValue(Type &min, Type &max) const
+{
+ min = max = bitmap[0];
+ for (unsigned int i=0 ; i < npixels ; i++)
+ {
+ if (bitmap[i]<min) min = bitmap[i] ;
+ if (bitmap[i]>max) max = bitmap[i] ;
+ }
+}
+
+/*!
+ \brief Copy operator
+*/
+template<class Type>
+vpImage<Type> & vpImage<Type>::operator=(const vpImage<Type> &I)
+{
+ /* we first have to set the initial values of the image because resize function calls init function that test the actual size of the image */
+ if(bitmap != NULL){
+ delete[] bitmap;
+ bitmap = NULL ;
+ }
+
+ if(row != NULL){
+ delete[] row;
+ row = NULL ;
+ }
+ this->width = I.width;
+ this->height = I.height;
+ this->npixels = I.npixels;
+ try
+ {
+ if(I.npixels != 0)
+ {
+ if (bitmap == NULL){
+ bitmap = new Type[npixels] ;
+ }
+
+ if (bitmap == NULL){
+ vpERROR_TRACE("cannot allocate bitmap ") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate bitmap ")) ;
+ }
+
+ if (row == NULL){
+ row = new Type*[height] ;
+ }
+ if (row == NULL){
+ vpERROR_TRACE("cannot allocate row ") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "cannot allocate row ")) ;
+ }
+
+ memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
+
+ for (unsigned int i=0; i<this->height; i++){
+ row[i] = bitmap + i*this->width;
+ }
+ }
+ }
+ catch(vpException &)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ return (* this);
+}
+
+
+/*!
+ \brief = operator : Set all the element of the bitmap to a given value \e v.
+ \f$ A = v <=> A[i][j] = v \f$
+
+ \warning = must be defined for \f$ <\f$ Type \f$ > \f$
+*/
+template<class Type>
+vpImage<Type>& vpImage<Type>::operator=(const Type &v)
+{
+ for (unsigned int i=0 ; i < npixels ; i++)
+ bitmap[i] = v ;
+
+ return *this;
+}
+
+/*!
+ Compare two images.
+
+ \return true if the images are the same, false otherwise.
+*/
+template<class Type>
+bool vpImage<Type>::operator==(const vpImage<Type> &I)
+{
+ if (this->width != I.getWidth())
+ return false;
+ if (this->height != I.getHeight())
+ return false;
+
+ for (unsigned int i=0 ; i < npixels ; i++)
+ {
+ if (bitmap[i] != I.bitmap[i])
+ return false;
+ }
+ return true ;
+}
+/*!
+ Compare two images.
+
+ \return true if the images are different, false if they are the same.
+*/
+template<class Type>
+bool vpImage<Type>::operator!=(const vpImage<Type> &I)
+{
+ if (this->width != I.getWidth())
+ return true;
+ if (this->height != I.getHeight())
+ return true;
+
+ for (unsigned int i=0 ; i < npixels ; i++)
+ {
+ if (bitmap[i] == I.bitmap[i])
+ return false;
+ }
+ return true ;
+}
+
+/*!
+ Operation A - B (A is unchanged).
+
+ \code
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpImage<unsigned char> A(288, 384);
+ vpImage<unsigned char> B(288, 384);
+ vpImage<unsigned char> C;
+
+ A = 128;
+ B = 120;
+
+ // operator-() : C = A - B
+ C = A - B;
+
+ return 0;
+}
+ \endcode
+
+ \sa sub(const vpImage<Type> &, const vpImage<Type> &, vpImage<Type> &) to
+ avoid matrix allocation for each use.
+*/
+template<class Type>
+vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
+{
+ vpImage<Type> C;
+ sub(*this,B,C);
+ return C;
+}
+
+/*!
+ Insert an image into another one.
+
+ It is possible to insert the image \f$ src \f$ into the calling vpImage.
+ You can set the point in the destination image where the top left corner of the \f$ src \f$ image will belocated.
+
+ \param src : Image to insert
+ \param topLeft : Coordinates of the \f$ src \f$ image's top left corner in the destination image.
+*/
+template<class Type>
+void vpImage<Type>::insert(const vpImage<Type> &src,
+ const vpImagePoint topLeft)
+{
+ Type* srcBitmap;
+ Type* destBitmap;
+
+ int itl = (int)topLeft.get_i();
+ int jtl = (int)topLeft.get_j();
+
+ int dest_ibegin = 0;
+ int dest_jbegin = 0;
+ int src_ibegin = 0;
+ int src_jbegin = 0;
+ int dest_w = (int)this->getWidth();
+ int dest_h = (int)this->getHeight();
+ int src_w = (int)src.getWidth();
+ int src_h = (int)src.getHeight();
+ int wsize = (int)src.getWidth();
+ int hsize = (int)src.getHeight();
+
+ if (itl >= dest_h || jtl >= dest_w)
+ return;
+
+ if (itl < 0)
+ src_ibegin = -itl;
+ else
+ dest_ibegin = itl;
+
+ if (jtl < 0)
+ src_jbegin = -jtl;
+ else
+ dest_jbegin = jtl;
+
+ if (src_w - src_jbegin > dest_w - dest_jbegin)
+ wsize = dest_w - dest_jbegin;
+ else
+ wsize = src_w - src_jbegin;
+
+ if (src_h - src_ibegin > dest_h - dest_ibegin)
+ hsize = dest_h - dest_ibegin;
+ else
+ hsize = src_h - src_ibegin;
+
+ for (int i = 0; i < hsize; i++)
+ {
+ srcBitmap = src.bitmap + ((src_ibegin+i)*src_w+src_jbegin);
+ destBitmap = this->bitmap + ((dest_ibegin+i)*dest_w+dest_jbegin);
+
+ memcpy(destBitmap,srcBitmap,wsize*sizeof(Type));
+ }
+}
+
+/*!
+ Returns a new image that's half size of the current image.
+ No filtering is used during the sub sampling.
+
+ Used for building pyramid of the image.
+ \warning Operator = must be defined for Type.
+
+ \param res [out] : Subsampled image that is half size of the current image.
+
+ The example below shows how to use this method:
+ \code
+ vpImage<unsigned char> I; // original image
+ vpImageIo::read(I, "myImage.pgm");
+ vpImage<unsigned char> I2; // half size image
+ I.halfSizeImage(I2);
+ vpImageIo::write(I2, "myHalfSizeImage.pgm");
+ \endcode
+
+ This other example shows how to construct a pyramid of the image:
+ \code
+ vpImage<unsigned char> I[4]; // pyramid with 4 levels
+ vpImageIo::read(I[1], "myImage.pgm"); // Original image at level 1
+ // compute the other levels
+ I5[1].doubleSizeImage(I5[0]); // double size image at level 0
+ I5[1].halfSizeImage(I5[2]); // half size image at level 2
+ I5[1].quarterSizeImage(I5[3]); // quarter size image at level 3
+ \endcode
+
+*/
+template<class Type>
+void
+vpImage<Type>::halfSizeImage(vpImage<Type> &res)
+{
+ unsigned int h = height/2;
+ unsigned int w = width/2;
+ res.resize(h, w);
+ for(unsigned int i = 0; i < h; i++)
+ for(unsigned int j = 0; j < w; j++)
+ res[i][j] = (*this)[i<<1][j<<1];
+}
+
+/*!
+ Returns a new image that's a quarter size of the current image.
+ No filtering is used during the sub sampling.
+ Used for building a quarter of the image.
+ \warning Operator = must be defined for Type.
+
+ \param res [out] : Subsampled image that is quarter size of the
+ current image.
+
+ The example below shows how to use this method:
+ \code
+ vpImage<unsigned char> I; // original image
+ vpImageIo::read(I, "myImage.pgm");
+ vpImage<unsigned char> I4; // quarter size image
+ I.halfSizeImage(I4);
+ vpImageIo::write(I4, "myQuarterSizeImage.pgm");
+ \endcode
+
+ See halfSizeImage(vpImage<Type> &) for an example of pyramid construction.
+
+*/
+
+template<class Type>
+void
+vpImage<Type>::quarterSizeImage(vpImage<Type> &res)
+{
+ unsigned int h = height/4;
+ unsigned int w = width/4;
+ res.resize(h, w);
+ for(unsigned int i = 0; i < h; i++)
+ for(unsigned int j = 0; j < w; j++)
+ res[i][j] = (*this)[i<<2][j<<2];
+}
+
+/*!
+ Returns a new image that's double size of the current image.
+ Used (eg. in case of keypoints extraction, we might
+ double size of the image in order to have more keypoints).
+ The double size image is computed by nearest-neighbour interpolation:
+
+ \code
+ A B C
+ E F G
+ H I J
+
+ where
+ A C H J are pixels from original image
+ B E G I are interpolated pixels
+ \endcode
+
+
+ \warning Operator = must be defined for Type.
+
+ \param res [out] : Image that is double size of the current image.
+
+ The example below shows how to use this method:
+ \code
+ vpImage<unsigned char> I; // original image
+ vpImageIo::read(I, "myImage.pgm");
+ vpImage<unsigned char> I2; // double size image
+ I.doubleSizeImage(I2);
+ vpImageIo::write(I2, "myDoubleSizeImage.pgm");
+ \endcode
+
+ See halfSizeImage(vpImage<Type> &) for an example of pyramid construction.
+
+*/
+template<class Type>
+void
+vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
+{
+ int h = height*2;
+ int w = width*2;
+
+ res.resize(h, w);
+
+ for(int i = 0; i < h; i++)
+ for(int j = 0; j < w; j++)
+ res[i][j] = (*this)[i>>1][j>>1];
+
+ /*
+ A B C
+ E F G
+ H I J
+ A C H J are pixels from original image
+ B E G I are interpolated pixels
+ */
+
+ //interpolate pixels B and I
+ for(int i = 0; i < h; i += 2)
+ for(int j = 1; j < w - 1; j += 2)
+ res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
+ + (*this)[i>>1][(j>>1) + 1]));
+
+ //interpolate pixels E and G
+ for(int i = 1; i < h - 1; i += 2)
+ for(int j = 0; j < w; j += 2)
+ res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
+ + (*this)[(i>>1)+1][j>>1]));
+
+ //interpolate pixel F
+ for(int i = 1; i < h - 1; i += 2)
+ for(int j = 1; j < w - 1; j += 2)
+ res[i][j] = (Type)(0.25 * ((*this)[i>>1][j>>1]
+ + (*this)[i>>1][(j>>1)+1]
+ + (*this)[(i>>1)+1][j>>1]
+ + (*this)[(i>>1)+1][(j>>1)+1]));
+}
+
+/*!
+
+ \warning This generic method is not implemented. You should rather use the
+ instantiated methods for double, unsigned char and vpRGBa images.
+
+ \sa vpImage<double>::getValue(double, double)
+ \sa vpImage<unsigned char>::getValue(double, double)
+ \sa vpImage<vpRGBa>::getValue(double, double)
+
+*/
+template<class Type>
+Type vpImage<Type>::getValue(double /* i */, double /* j */) const
+{
+ vpTRACE("Not implemented");
+}
+
+/*!
+
+ Retrieves pixel value from an image of unsigned char with sub-pixel accuracy.
+
+ Gets the value of a sub-pixel with coordinates (i,j) with bilinear
+ interpolation. If location is out of bounds, then return value of
+ closest pixel.
+
+ \param i : Sub-pixel coordinate along the rows.
+ \param j : Sub-pixel coordinate along the columns.
+
+ \return Interpolated sub-pixel value from the four neighbours.
+
+ \exception vpImageException::notInTheImage : If (i,j) is out
+ of the image.
+
+*/
+template<>
+inline unsigned char vpImage<unsigned char>::getValue(double i, double j) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(i);
+ jround = (unsigned int)floor(j);
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (i > height - 1)
+ i = (double)(height - 1);
+
+ if (j > width - 1)
+ j = (double)(width - 1);
+
+ double rratio = i - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = j - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
+ + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ return (unsigned char)vpMath::round(value);
+}
+
+
+/*!
+
+ Retrieves pixel value from an image of double with sub-pixel accuracy.
+
+ Gets the value of a sub-pixel with coordinates (i,j) with bilinear
+ interpolation. If location is out of bounds, then return value of
+ closest pixel.
+
+ \param i : Sub-pixel coordinate along the rows.
+ \param j : Sub-pixel coordinate along the columns.
+
+ \return Interpolated sub-pixel value from the four neighbours.
+
+ \exception vpImageException::notInTheImage : If (i,j) is out
+ of the image.
+
+*/
+template<>
+inline double vpImage<double>::getValue(double i, double j) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(i);
+ jround = (unsigned int)floor(j);
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (i > height - 1)
+ i = (double)(height - 1);
+
+ if (j > width - 1)
+ j = (double)(width - 1);
+
+ double rratio = i - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = j - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
+ + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ return value;
+}
+
+template<>
+inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(i);
+ jround = (unsigned int)floor(j);
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (i > height - 1)
+ i = (double)(height - 1);
+
+ if (j > width - 1)
+ j = (double)(width - 1);
+
+ double rratio = i - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = j - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+ double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
+ + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
+ double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
+ + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
+ double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
+ + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
+ return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
+}
+
+/*!
+
+ \warning This generic method is not implemented. You should rather use the
+ instantiated methods for double, unsigned char and vpRGBa images.
+
+ \sa vpImage<double>::getValue(vpImagePoint &)
+ \sa vpImage<unsigned char>::getValue(vpImagePoint &)
+ \sa vpImage<vpRGBa>::getValue(vpImagePoint &)
+
+*/
+template<class Type>
+Type vpImage<Type>::getValue(vpImagePoint & /* ip */) const
+{
+ vpTRACE("Not implemented");
+}
+
+template<>
+inline unsigned char vpImage<unsigned char>::getValue(vpImagePoint &ip) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(ip.get_i());
+ jround = (unsigned int)floor(ip.get_j());
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (ip.get_i() > height - 1)
+ ip.set_i((double)(height - 1));
+
+ if (ip.get_j() > width - 1)
+ ip.set_j((double)(width - 1));
+
+ double rratio = ip.get_i() - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = ip.get_j() - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
+ + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ return (unsigned char)vpMath::round(value);
+}
+
+
+template<>
+inline double vpImage<double>::getValue(vpImagePoint &ip) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(ip.get_i());
+ jround = (unsigned int)floor(ip.get_j());
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (ip.get_i() > height - 1)
+ ip.set_i((double)(height - 1));
+
+ if (ip.get_j() > width - 1)
+ ip.set_j((double)(width - 1));
+
+ double rratio = ip.get_i() - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = ip.get_j() - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
+ + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ return value;
+}
+
+template<>
+inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
+{
+ unsigned int iround, jround;
+ double rfrac, cfrac;
+
+ iround = (unsigned int)floor(ip.get_i());
+ jround = (unsigned int)floor(ip.get_j());
+
+ if (iround >= height || jround >= width) {
+ vpERROR_TRACE("Pixel outside the image") ;
+ throw(vpException(vpImageException::notInTheImage,
+ "Pixel outside the image"));
+ }
+
+ if (ip.get_i() > height - 1)
+ ip.set_i((double)(height - 1));
+
+ if (ip.get_j() > width - 1)
+ ip.set_j((double)(width - 1));
+
+ double rratio = ip.get_i() - (double) iround;
+ if(rratio < 0)
+ rratio=-rratio;
+ double cratio = ip.get_j() - (double) jround;
+ if(cratio < 0)
+ cratio=-cratio;
+
+ rfrac = 1.0f - rratio;
+ cfrac = 1.0f - cratio;
+
+ double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
+ + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
+ double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
+ + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
+ double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
+ + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
+ return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
+}
+
+/*!
+ Operation C = *this - B.
+
+ \code
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpImage<unsigned char> A(288, 384);
+ vpImage<unsigned char> B(288, 384);
+ vpImage<unsigned char> C;
+
+ A = 128;
+ B = 120;
+
+ A.sub(B, C); // C = A - B
+
+ return 0;
+}
+ \endcode
+
+ The result is placed in the third parameter C and not returned.
+ A new image won't be allocated for every use of the function
+ (Speed gain if used many times with the same result matrix size).
+
+ \exception vpException::memoryAllocationError If the images size differ.
+
+ \sa operator-()
+*/
+template<class Type>
+void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
+{
+
+ try
+ {
+ if ((this->getHeight() != C.getHeight())
+ || (this->getWidth() != C.getWidth()))
+ C.resize(this->getHeight(), this->getWidth());
+ }
+ catch(vpException &me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ std::cout << me << std::endl ;
+ throw ;
+ }
+
+ if ( (this->getWidth() != B.getWidth())||(this->getHeight() != B.getHeight()))
+ {
+ vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "vpImage mismatch in vpImage/vpImage substraction ")) ;
+ }
+
+ for (unsigned int i=0;i<this->getWidth()*this->getHeight();i++)
+ {
+ *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i) ;
+ }
+}
+
+/*!
+ Operation C = A - B.
+
+ The result is placed in the third parameter C and not returned.
+ A new image won't be allocated for every use of the function
+ (Speed gain if used many times with the same result matrix size).
+
+ \exception vpException::memoryAllocationError If the images size differ.
+
+ \sa operator-()
+*/
+template<class Type>
+void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B,
+ vpImage<Type> &C)
+{
+
+ try
+ {
+ if ((A.getHeight() != C.getHeight())
+ || (A.getWidth() != C.getWidth()))
+ C.resize(A.getHeight(), A.getWidth());
+ }
+ catch(vpException &me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ std::cout << me << std::endl ;
+ throw ;
+ }
+
+ if ( (A.getWidth() != B.getWidth())||(A.getHeight() != B.getHeight()))
+ {
+ vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
+ throw(vpException(vpException::memoryAllocationError,
+ "vpImage mismatch in vpImage/vpImage substraction ")) ;
+ }
+
+ for (unsigned int i=0;i<A.getWidth()*A.getHeight();i++)
+ {
+ *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i) ;
+ }
+}
+
+/*!
+
+ \warning This generic method is not implemented. You should rather use the
+ instantiated methods for unsigned char and vpRGBa images.
+
+ \sa vpImage<unsigned char>::performLut(const unsigned char (&)[256])
+ \sa vpImage<vpRGBa char>::performLut(const vpRGBa (&)[256])
+
+*/
+template<class Type>
+void vpImage<Type>::performLut(const Type (&)[256])
+{
+// vpTRACE("Not implemented");
+ std::cerr << "Not implemented !" << std::endl;
+}
+
+/*!
+ Modify the intensities of a grayscale image using the look-up table passed in parameter.
+
+ \param lut : Look-up table (unsigned char array of size=256) which maps each intensity to his new value.
+*/
+template<>
+inline void vpImage<unsigned char>::performLut(const unsigned char (&lut)[256]) {
+ unsigned int size = getWidth()*getHeight();
+ unsigned char *ptrStart = (unsigned char*) bitmap;
+ unsigned char *ptrEnd = ptrStart + size;
+ unsigned char *ptrCurrent = ptrStart;
+
+ while(ptrCurrent != ptrEnd) {
+ *ptrCurrent = lut[*ptrCurrent];
+ ++ptrCurrent;
+ }
+}
+
+/*!
+ Modify the intensities of a color image using the look-up table passed in parameter.
+
+ \param lut : Look-up table (vpRGBa array of size=256) which maps each intensity to his new value.
+*/
+template<>
+inline void vpImage<vpRGBa>::performLut(const vpRGBa (&lut)[256]) {
+ unsigned int size = getWidth()*getHeight();
+ unsigned char *ptrStart = (unsigned char*) bitmap;
+ unsigned char *ptrEnd = ptrStart + size*4;
+ unsigned char *ptrCurrent = ptrStart;
+
+ while(ptrCurrent != ptrEnd) {
+ *ptrCurrent = lut[*ptrCurrent].R;
+ ++ptrCurrent;
+
+ *ptrCurrent = lut[*ptrCurrent].G;
+ ++ptrCurrent;
+
+ *ptrCurrent = lut[*ptrCurrent].B;
+ ++ptrCurrent;
+
+ *ptrCurrent = lut[*ptrCurrent].A;
+ ++ptrCurrent;
+ }
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h
new file mode 100644
index 0000000..e9d794b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImageConvert.h
@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Convert image types.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpImageConvert.h
+ \brief Convert image types
+*/
+
+#ifndef vpIMAGECONVERT_H
+#define vpIMAGECONVERT_H
+
+// image
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDebug.h>
+// color
+#include <visp3/core/vpRGBa.h>
+
+#ifdef VISP_HAVE_OPENCV
+# if (VISP_HAVE_OPENCV_VERSION >= 0x030000) // Require opencv >= 3.0.0
+# include <opencv2/core/core.hpp>
+# include <opencv2/highgui/highgui.hpp>
+# include <opencv2/imgproc/imgproc_c.h>
+# elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) // Require opencv >= 2.4.8
+# include <opencv2/core/core.hpp>
+# include <opencv2/highgui/highgui.hpp>
+# elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/core/core.hpp>
+# include <opencv2/legacy/legacy.hpp>
+# include <opencv2/highgui/highgui.hpp>
+# include <opencv2/highgui/highgui_c.h>
+# else
+# include <highgui.h>
+# endif
+#endif
+
+#ifdef VISP_HAVE_YARP
+# include <yarp/sig/Image.h>
+#endif
+
+#if defined(_WIN32)
+# include <windows.h>
+#endif
+
+/*!
+ \class vpImageConvert
+
+ \ingroup group_core_image
+
+ Convert image types.
+
+ The following example available in tutorial-image-converter.cpp shows how to
+ convert an OpenCV cv::Mat image into a vpImage:
+
+ \include tutorial-image-converter.cpp
+
+*/
+class VISP_EXPORT vpImageConvert
+{
+
+public:
+ static void convert(const vpImage<unsigned char> &src,
+ vpImage<vpRGBa> & dest) ;
+ static void convert(const vpImage<vpRGBa> &src,
+ vpImage<unsigned char> & dest) ;
+
+ static void convert(const vpImage<float> &src,
+ vpImage<unsigned char> &dest);
+ static void convert(const vpImage<unsigned char> &src,
+ vpImage<float> &dest);
+
+ static void convert(const vpImage<double> &src,
+ vpImage<unsigned char> &dest);
+ static void convert(const vpImage<unsigned char> &src,
+ vpImage<double> &dest);
+
+#ifdef VISP_HAVE_OPENCV
+ // Deprecated: will be removed with OpenCV transcient from C to C++ api
+ static void convert(const IplImage* src,
+ vpImage<vpRGBa> & dest, bool flip = false) ;
+ static void convert(const IplImage* src,
+ vpImage<unsigned char> & dest, bool flip = false) ;
+ static void convert(const vpImage<vpRGBa> & src,
+ IplImage *&dest) ;
+ static void convert(const vpImage<unsigned char> & src,
+ IplImage* &dest) ;
+# if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ static void convert(const cv::Mat& src,
+ vpImage<vpRGBa>& dest, const bool flip = false);
+ static void convert(const cv::Mat& src,
+ vpImage<unsigned char>& dest, const bool flip = false);
+ static void convert(const vpImage<vpRGBa> & src,
+ cv::Mat& dest) ;
+ static void convert(const vpImage<unsigned char> & src,
+ cv::Mat& dest, const bool copyData = true) ;
+# endif
+#endif
+
+#ifdef VISP_HAVE_YARP
+ static void convert(const vpImage<unsigned char> & src,
+ yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData = true) ;
+ static void convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
+ vpImage<unsigned char> & dest,const bool copyData = true ) ;
+
+
+ static void convert(const vpImage<vpRGBa> & src,
+ yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData = true) ;
+ static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
+ vpImage<vpRGBa> & dest,const bool copyData = true) ;
+
+ static void convert(const vpImage<vpRGBa> & src,
+ yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest) ;
+ static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src,
+ vpImage<vpRGBa> & dest) ;
+#endif
+
+ static void split(const vpImage<vpRGBa> &src,
+ vpImage<unsigned char>* pR,
+ vpImage<unsigned char>* pG,
+ vpImage<unsigned char>* pB,
+ vpImage<unsigned char>* pa = NULL);
+
+ static void merge(const vpImage<unsigned char> *R,
+ const vpImage<unsigned char> *G,
+ const vpImage<unsigned char> *B,
+ const vpImage<unsigned char> *a,
+ vpImage<vpRGBa> &RGBa);
+
+ /*!
+ Converts a yuv pixel value in rgb format.
+
+ \param y Y component of a pixel.
+ \param u U component of a pixel.
+ \param v V component of a pixel.
+ \param r Red component from the YUV coding format. This value is computed
+ using:
+ \f[ r = 0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128) \f]
+ \param g Green component from the YUV coding format. This value is computed
+ using:
+ \f[g = 0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128) \f]
+ \param b Blue component from the YUV coding format. This value is computed
+ using:
+ \f[b = 0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128) \f]
+
+ */
+ static inline void YUVToRGB(unsigned char y,
+ unsigned char u,
+ unsigned char v,
+ unsigned char &r,
+ unsigned char &g,
+ unsigned char &b)
+ {
+ double dr, dg, db;
+ dr = floor(0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128));
+ dg = floor(0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128));
+ db = floor(0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128));
+
+ dr = dr < 0. ? 0. : dr;
+ dg = dg < 0. ? 0. : dg;
+ db = db < 0. ? 0. : db;
+ dr = dr > 255. ? 255. : dr;
+ dg = dg > 255. ? 255. : dg;
+ db = db > 255. ? 255. : db;
+
+ r = (unsigned char) dr;
+ g = (unsigned char) dg;
+ b = (unsigned char) db;
+ };
+ static void YUYVToRGBa(unsigned char* yuyv, unsigned char* rgba,
+ unsigned int width, unsigned int height);
+ static void YUYVToRGB(unsigned char* yuyv, unsigned char* rgb,
+ unsigned int width, unsigned int height);
+ static void YUYVToGrey(unsigned char* yuyv, unsigned char* grey,
+ unsigned int size);
+ static void YUV411ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int size);
+ static void YUV411ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int size);
+ static void YUV411ToGrey(unsigned char* yuv,
+ unsigned char* grey, unsigned int size);
+ static void YUV422ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int size);
+ static void YUV422ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int size);
+ static void YUV422ToGrey(unsigned char* yuv,
+ unsigned char* grey, unsigned int size);
+ static void YUV420ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int width, unsigned int height);
+ static void YUV420ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int width, unsigned int height);
+ static void YUV420ToGrey(unsigned char* yuv,
+ unsigned char* grey, unsigned int size);
+
+ static void YUV444ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int size);
+ static void YUV444ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int size);
+ static void YUV444ToGrey(unsigned char* yuv,
+ unsigned char* grey, unsigned int size);
+
+ static void YV12ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int width, unsigned int height);
+ static void YV12ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int width, unsigned int height);
+ static void YVU9ToRGBa(unsigned char* yuv,
+ unsigned char* rgba, unsigned int width, unsigned int height);
+ static void YVU9ToRGB(unsigned char* yuv,
+ unsigned char* rgb, unsigned int width, unsigned int height);
+ static void RGBToRGBa(unsigned char* rgb,
+ unsigned char* rgba, unsigned int size);
+ static void RGBaToRGB(unsigned char* rgba,
+ unsigned char* rgb, unsigned int size);
+
+ static void RGBToGrey(unsigned char* rgb,
+ unsigned char* grey, unsigned int size);
+ static void RGBaToGrey(unsigned char* rgba,
+ unsigned char* grey, unsigned int size);
+
+ static void RGBToRGBa(unsigned char * bgr, unsigned char * rgba,
+ unsigned int width, unsigned int height, bool flip = false);
+ static void RGBToGrey(unsigned char * bgr, unsigned char * grey,
+ unsigned int width, unsigned int height, bool flip = false);
+
+ static void GreyToRGBa(unsigned char* grey,
+ unsigned char* rgba, unsigned int size);
+ static void GreyToRGB(unsigned char* grey,
+ unsigned char* rgb, unsigned int size);
+
+ static void BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
+ unsigned int width, unsigned int height, bool flip=false);
+
+ static void BGRToGrey(unsigned char * bgr, unsigned char * grey,
+ unsigned int width, unsigned int height, bool flip=false);
+
+ static void YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb,
+ unsigned int size);
+ static void YCbCrToRGBa (unsigned char *ycbcr, unsigned char *rgb,
+ unsigned int size);
+ static void YCrCbToRGB(unsigned char *ycbcr, unsigned char *rgb,
+ unsigned int size);
+ static void YCrCbToRGBa(unsigned char *ycbcr, unsigned char *rgb,
+ unsigned int size);
+ static void YCbCrToGrey(unsigned char *ycbcr, unsigned char *grey,
+ unsigned int size);
+ static void MONO16ToGrey(unsigned char *grey16, unsigned char *grey,
+ unsigned int size);
+ static void MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba,
+ unsigned int size);
+
+ static void HSVToRGBa(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
+ const unsigned int size);
+ static void HSVToRGBa(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
+ unsigned char *rgba, const unsigned int size);
+ static void RGBaToHSV(const unsigned char *rgba, double *hue, double *saturation, double *value,
+ const unsigned int size);
+ static void RGBaToHSV(const unsigned char *rgba, unsigned char *hue, unsigned char *saturation, unsigned char *value,
+ const unsigned int size);
+
+ static void HSVToRGB(const double *hue, const double *saturation, const double *value, unsigned char *rgb,
+ const unsigned int size);
+ static void HSVToRGB(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
+ unsigned char *rgb, const unsigned int size);
+ static void RGBToHSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
+ const unsigned int size);
+ static void RGBToHSV(const unsigned char *rgb, unsigned char *hue, unsigned char *saturation, unsigned char *value,
+ const unsigned int size);
+
+private:
+ static void computeYCbCrLUT();
+
+ static void HSV2RGB(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
+ const unsigned int size, const unsigned int step);
+ static void RGB2HSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
+ const unsigned int size, const unsigned int step);
+
+private:
+ static bool YCbCrLUTcomputed;
+ static int vpCrr[256];
+ static int vpCgb[256];
+ static int vpCgr[256];
+ static int vpCbb[256];
+
+} ;
+
+#endif
+
+
+/*
+* Local variables:
+* c-basic-offset: 2
+* End:
+*/
diff --git a/modules/core/include/visp3/core/vpImageException.h b/modules/core/include/visp3/core/vpImageException.h
new file mode 100644
index 0000000..2ab9bc7
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImageException.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpImage class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef __vpImageException_H
+#define __vpImageException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpImageException.h
+ \brief error that can be emited by the vpImage class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+
+ \class vpImageException
+ \ingroup group_core_debug
+ \brief Error that can be emited by the vpImage class and its derivates.
+ */
+class VISP_EXPORT vpImageException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpImage member
+ */
+ enum errorImageCodeEnum
+ {
+ ioError,
+ noFileNameError,
+ notInitializedError,
+ incorrectInitializationError,
+ notInTheImage
+ } ;
+
+ public:
+ vpImageException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpImageException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpImageException (const int id)
+ : vpException(id){ ; }
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h
new file mode 100644
index 0000000..b50bf80
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImageFilter.h
@@ -0,0 +1,445 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Various image tools, convolution, ...
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpImageFilter_H
+#define vpImageFilter_H
+
+/*!
+ \file vpImageFilter.h
+ \brief Various image filter, convolution, etc...
+
+*/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageException.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <string.h>
+
+/*!
+ \class vpImageFilter
+
+ \ingroup group_core_image
+
+ \brief Various image filter, convolution, etc...
+
+*/
+class VISP_EXPORT vpImageFilter
+{
+
+public:
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ static void canny(const vpImage<unsigned char>& I,
+ vpImage<unsigned char>& Ic,
+ const unsigned int gaussianFilterSize,
+ const double thresholdCanny,
+ const unsigned int apertureSobel);
+#endif
+
+ /*!
+ Apply a 1x3 derivative filter to an image pixel.
+
+ \param I : Image to filter
+ \param r: coordinates (row) of the pixel
+ \param c : coordinates (column) of the pixel
+ */
+ template<class T>
+ static double derivativeFilterX(const vpImage<T> &I,
+ const unsigned int r, const unsigned int c)
+ {
+ return (2047.0 *(I[r][c+1] - I[r][c-1])
+ +913.0 *(I[r][c+2] - I[r][c-2])
+ +112.0 *(I[r][c+3] - I[r][c-3]))/8418.0;
+ }
+
+ /*!
+ Apply a 3x1 derivative filter to an image pixel.
+
+ \param I : Image to filter
+ \param r : coordinates (row) of the pixel
+ \param c : coordinates (column) of the pixel
+ */
+ template<class T>
+ static double derivativeFilterY(const vpImage<T> &I,
+ const unsigned int r, const unsigned int c)
+ {
+ return (2047.0 *(I[r+1][c] - I[r-1][c])
+ +913.0 *(I[r+2][c] - I[r-2][c])
+ +112.0 *(I[r+3][c] - I[r-3][c]))/8418.0;
+ }
+
+ /*!
+ Apply a 1 x size Derivative Filter in X to an image pixel.
+
+ \param I : Image to filter
+ \param r : coordinates (row) of the pixel
+ \param c : coordinates (column) of the pixel
+ \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
+ \param size : size of the filter
+
+ \sa vpImageFilter::getGaussianDerivativeKernel()
+ */
+
+ template<class T>
+ static double derivativeFilterX(const vpImage<T> &I,
+ const unsigned int r, const unsigned int c,
+ const double *filter, const unsigned int size)
+ {
+ unsigned int i;
+ double result;
+
+ result = 0;
+
+ for(i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r][c+i] - I[r][c-i]) ;
+ }
+ return result;
+ }
+
+
+
+ /*!
+ Apply a size x 1 Derivative Filter in Y to an image pixel.
+
+ \param I : Image to filter
+ \param r : coordinates (row) of the pixel
+ \param c : coordinates (column) of the pixel
+ \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
+ \param size : size of the filter
+
+ \sa vpImageFilter::getGaussianDerivativeKernel()
+ */
+ template<class T>
+ static double derivativeFilterY(const vpImage<T> &I,
+ const unsigned int r, const unsigned int c,
+ const double *filter, const unsigned int size)
+ {
+ unsigned int i;
+ double result;
+
+ result = 0;
+
+ for(i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r+i][c] - I[r-i][c]) ;
+ }
+ return result;
+ }
+
+ static void filter(const vpImage<double> &I,
+ vpImage<double>& Iu,
+ vpImage<double>& Iv,
+ const vpMatrix& M) ;
+
+
+ static void filter(const vpImage<unsigned char> &I,
+ vpImage<double>& If,
+ const vpMatrix& M) ;
+
+
+ static void filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size);
+ static void filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size);
+
+ static inline unsigned char filterGaussXPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
+ {
+ return (unsigned char)((1.*I[i][j-2]+4.*I[i][j-1]+6.*I[i][j]+4.*I[i][j+1]+1.*I[i][j+2])/16.);
+ }
+ static inline unsigned char filterGaussYPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
+ {
+ return (unsigned char)((1.*I[i-2][j]+4.*I[i-1][j]+6.*I[i][j]+4.*I[i+1][j]+1.*I[i+2][j])/16.);
+ }
+
+ static void filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
+ static void filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
+
+ static inline double filterX(const vpImage<unsigned char> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+
+ static inline double filterXLeftBorder(const vpImage<unsigned char> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(c>i)
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ else
+ result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterXRightBorder(const vpImage<unsigned char> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(c+i<I.getWidth())
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ else
+ result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterX(const vpImage<double> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterXLeftBorder(const vpImage<double> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(c>i)
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ else
+ result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterXRightBorder(const vpImage<double> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(c+i<I.getWidth())
+ result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ else
+ result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static void filterY(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
+ static void filterY(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
+ static inline double filterY(const vpImage<unsigned char> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ double static inline filterYTopBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(r>i)
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ else
+ result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ double static inline filterYBottomBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(r+i<I.getHeight())
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ else
+ result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterYTopBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(r>i)
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ else
+ result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterYBottomBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ if(r+i<I.getHeight())
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ else
+ result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static inline double filterY(const vpImage<double> &I,
+ unsigned int r, unsigned int c,
+ const double *filter,unsigned int size)
+ {
+ double result;
+
+ result = 0;
+
+ for(unsigned int i=1; i<=(size-1)/2; i++)
+ {
+ result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ }
+ return result+filter[0]*I[r][c];
+ }
+
+ static void gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size=7, double sigma=0., bool normalize=true);
+ static void gaussianBlur(const vpImage<double> &I, vpImage<double>& GI, unsigned int size=7, double sigma=0., bool normalize=true);
+ /*!
+ Apply a 5x5 Gaussian filter to an image pixel.
+
+ \param fr : Image to filter
+ \param r : coordinates (row) of the pixel
+ \param c : coordinates (column) of the pixel
+ */
+ template<class T>
+ static double gaussianFilter(const vpImage<T> & fr,
+ const unsigned int r, const unsigned int c)
+ {
+ //filter Gaussien
+ return (
+ 15.0 * fr[r][c]
+ + 12.0 * ( fr[r-1][c] + fr[r][c-1] + fr[r+1][c] + fr[r][c+1] )
+ + 9.0 * ( fr[r-1][c-1] + fr[r+1][c-1] + fr[r-1][c+1] + fr[r+1][c+1])
+ + 5.0 * ( fr[r-2][c] + fr[r][c-2] + fr[r+2][c] + fr[r][c+2] )
+ + 4.0 * ( fr[r-2][c+1] + fr[r-2][c-1] + fr[r-1][c-2] + fr[r+1][c-2] +
+ fr[r+2][c-1] + fr[r+2][c+1] + fr[r-1][c+2] + fr[r+1][c+2] )
+ + 2.0 * ( fr[r-2][c-2] + fr[r+2][c-2] + fr[r-2][c+2] + fr[r+2][c+2] )
+ )
+ /159.0;
+ }
+ //operation pour pyramide gaussienne
+ static void getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
+ static void getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
+ static void getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
+
+ static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
+ static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
+
+ //fonction renvoyant le gradient en X de l'image I pour traitement pyramidal => dimension /2
+ static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx);
+ static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
+ static void getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
+ static void getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel, unsigned int size);
+
+ //fonction renvoyant le gradient en Y de l'image I
+ static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy);
+ static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
+ static void getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
+ static void getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel,unsigned int size);
+
+} ;
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h
new file mode 100644
index 0000000..898c819
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImageMorphology.h
@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Morphology tools.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpImageMorphology_H
+#define vpImageMorphology_H
+
+/*!
+ \file vpImageMorphology.h
+ \brief Various mathematical morphology tools, erosion, dilatation...
+
+*/
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageException.h>
+#include <visp3/core/vpMatrix.h>
+
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <string.h>
+
+/*!
+ \class vpImageMorphology
+
+ \ingroup group_core_image
+
+ \brief Various mathematical morphology tools, erosion, dilatation...
+
+ \author Fabien Spindler (Fabien.Spindler at irisa.fr) Irisa / Inria Rennes
+
+
+*/
+class vpImageMorphology
+{
+public:
+ /*! \enum vpConnexityType
+ Type of connexity 4, or 8.
+ */
+ typedef enum {
+ CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
+ right, up, down) */
+ CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
+ right, up, down, and the 4 pixels located on the diagonal) */
+ } vpConnexityType;
+
+public:
+ template<class Type>
+ static void erosion(vpImage<Type> &I, Type value, Type value_out,
+ vpConnexityType connexity = CONNEXITY_4);
+
+ template<class Type>
+ static void dilatation(vpImage<Type> &I, Type value, Type value_out,
+ vpConnexityType connexity = CONNEXITY_4);
+
+} ;
+
+/*!
+
+ Erode a binary image using a structuring element of size one.
+
+ \param I : Image to process.
+ \param value : Values of the pixels to erode.
+ \param value_out : Value to set if erosion is done.
+ \param connexity : Type of connexity: 4 or 8.
+
+ To erode a black area in an unsigned char image, set \e value to
+ 0 and \e value_out to 255.
+
+ To erode a white area in an unsigned char image with one element mask, set
+ \e value to 255 and \e value_out to 0.
+
+ \sa dilatation()
+*/
+template<class Type>
+void vpImageMorphology::erosion(vpImage<Type> &I,
+ Type value,
+ Type value_out,
+ vpConnexityType connexity)
+{
+ vpImage<Type> J(I.getHeight(), I.getWidth()) ;
+ J = I;
+
+ if (connexity == CONNEXITY_4) {
+ for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
+ for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
+ {
+ if (I[i][j] == value)
+ {
+ // Consider 4 neighbors
+ if ((I[i-1][j] == value_out) ||
+ (I[i+1][j] == value_out) ||
+ (I[i][j-1] == value_out) ||
+ (I[i][j+1] == value_out))
+ J[i][j] = value_out;
+ }
+ }
+ }
+ else {
+ for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
+ for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
+ {
+ if (I[i][j] == value)
+ {
+ // Consider 8 neighbors
+ if ((I[i-1][j-1] == value_out) ||
+ (I[i-1][j] == value_out) ||
+ (I[i-1][j+1] == value_out) ||
+ (I[i][j-1] == value_out) ||
+ (I[i][j+1] == value_out) ||
+ (I[i+1][j+1] == value_out) ||
+ (I[i+1][j+1] == value_out) ||
+ (I[i+1][j+1] == value_out) )
+ J[i][j] = value_out ;
+ }
+ }
+
+
+ }
+ I = J ;
+}
+
+/*!
+
+ Dilate a binary image using a structuring element of size one.
+
+ \param I : Image to process.
+ \param value : Values of the pixels to dilate.
+ \param value_out : Value to set if dilatation is done.
+ \param connexity : Type of connexity: 4 or 8.
+
+ To dilate a black area in an unsigned char image with one element mask, set
+ \e value to 0 and \e value_out to 255.
+
+ To dilate a white area in an unsigned char image with one element mask, set
+ \e value to 255 and \e value_out to 0.
+
+ \sa erosion()
+*/
+template<class Type>
+void vpImageMorphology::dilatation(vpImage<Type> &I,
+ Type value,
+ Type value_out,
+ vpConnexityType connexity)
+{
+ vpImage<Type> J(I.getHeight(), I.getWidth()) ;
+ J = I;
+ if (connexity == CONNEXITY_4) {
+ for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
+ for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
+ {
+ if (I[i][j] == value_out)
+ {
+ // Consider 4 neighbors
+ if ((I[i-1][j] == value) ||
+ (I[i+1][j] == value) ||
+ (I[i][j-1] == value) ||
+ (I[i][j+1] == value))
+ J[i][j] = value ;
+ }
+ }
+ }
+ else {
+ for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
+ for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
+ {
+ if (I[i][j] == value_out)
+ {
+ // Consider 8 neighbors
+ if ((I[i-1][j-1] == value) ||
+ (I[i-1][j] == value) ||
+ (I[i-1][j+1] == value) ||
+ (I[i][j-1] == value) ||
+ (I[i][j+1] == value) ||
+ (I[i+1][j+1] == value) ||
+ (I[i+1][j+1] == value) ||
+ (I[i+1][j+1] == value) )
+ J[i][j] = value ;
+ }
+ }
+ }
+
+ I = J ;
+}
+#endif
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpImagePoint.h b/modules/core/include/visp3/core/vpImagePoint.h
new file mode 100644
index 0000000..2d60965
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImagePoint.h
@@ -0,0 +1,308 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point useful for image processing
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpImagePoint_H
+#define vpImagePoint_H
+
+/*!
+ \file vpImagePoint.h
+ \brief Class that defines a 2D point in an image. This class is useful
+ for image processing
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMath.h>
+
+#include <ostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <vector>
+
+class vpRect;
+
+/*!
+ \class vpImagePoint
+ \ingroup group_core_image
+
+ \brief Class that defines a 2D point in an image. This class is
+ useful for image processing and stores only the <B>2D coordinates
+ given in sub-pixel</B>.
+
+ \warning If you want to define a point thanks to its coordinates
+ given in meter in the object frame, the camera frame or the image
+ plane, you have to use the class vpPoint.
+
+ In this class, the 2D coordinates are not necessary integer
+ values. It is easy to manipulate the given coordinates in the two
+ frames used in ViSP : the (i,j) coordinates and the (u,v)
+ coordinates. The two following images illustrate the two coordinate
+ systems.
+
+ \image html vpImagePoint.gif
+ \image latex vpImagePoint.ps width=10cm
+
+ \warning <B>An instance of the vpImagePoint class corresponds to a
+ particular point. Thus, if you change the point coordinate using the
+ method set_i(const double i), it produces the same effect than if
+ you used the method set_v(const double v). These two methods change
+ the same private attribute. It is also true for the two methods
+ set_j(const double j) and set_u(const double u).</B>
+*/
+
+
+class VISP_EXPORT vpImagePoint
+{
+ public:
+ /*!
+ Default constructor that initialize the coordinates of the image
+ point to zero.
+ */
+ inline vpImagePoint() : i(0), j(0) {}
+ /*!
+ Default constructor that initialize the coordinates of the image
+ thanks to the parameters \f$ ii \f$ and \f$ jj \f$.
+ */
+ inline vpImagePoint(double ii, double jj) : i(ii), j(jj) {}
+ /*!
+ Copy constructor.
+
+ Initialize the coordinates of the image point with \e ip.
+
+ \param ip : An image point.
+ */
+ inline vpImagePoint(const vpImagePoint &ip) : i(ip.i), j(ip.j) {}
+ //! Destructor.
+ inline virtual ~vpImagePoint() { ; }
+
+ /*!
+
+ Copy operator.
+
+ */
+ inline const vpImagePoint& operator=(const vpImagePoint &ip) {
+ this->i = ip.i;
+ this->j = ip.j;
+ return *this;
+ }
+ vpImagePoint& operator+=(const vpImagePoint &ip);
+
+ /*!
+
+ Operator -=.
+
+ */
+ inline vpImagePoint& operator-=(const vpImagePoint &ip) {
+ this->i -= ip.i;
+ this->j -= ip.j;
+ return *this;
+ }
+ vpImagePoint& operator/=(const double scale);
+ /*!
+
+ Operator *=.
+*/
+ inline vpImagePoint& operator*=(const double scale) {
+ this->i *= scale;
+ this->j *= scale;
+ return *this;
+ }
+
+ /*!
+
+ Sets the point coordinate corresponding to the \f$ i \f$ axes in
+ the frame (i,j).
+
+ \param ii : The desired value for the coordinate along the \f$ i \f$ axes.
+
+ \sa set_j(), set_u(), set_v()
+ */
+ inline void set_i(const double ii) { this->i = ii ; }
+
+ /*!
+
+ Sets the point coordinate corresponding to the \f$ j \f$ axes in
+ the frame (i,j).
+
+ \param jj : The desired value for the coordinate along the \f$ j \f$ axes.
+
+ \sa set_i(), set_u(), set_v()
+ */
+ inline void set_j(const double jj) { this->j = jj ; }
+
+ /*!
+
+ Sets the point coordinates in the frame (i,j).
+
+ \param ii : The desired value for the coordinate along the \f$ i \f$ axes.
+ \param jj : The desired value for the coordinate along the \f$ j \f$ axes.
+
+ \sa set_i(), set_j(), set_u(), set_v()
+ */
+ inline void set_ij(const double ii, const double jj) {
+ this->i = ii ;
+ this->j = jj ;
+ }
+
+ /*!
+
+ Gets the point coordinate corresponding to the \f$ i \f$ axes in
+ the frame (i,j).
+
+ \return The value of the coordinate along the \f$ i \f$ axes.
+
+ \sa get_j(), get_u(), get_v()
+ */
+ inline double get_i() const { return i ; }
+
+ /*!
+
+ Gets the point coordinate corresponding to the \f$ j \f$ axes in
+ the frame (i,j).
+
+ \return The value of the coordinate along the \f$ j \f$ axes.
+
+ \sa get_i(), get_u(), get_v()
+ */
+ inline double get_j() const { return j ; }
+
+ /*!
+
+ Sets the point coordinate corresponding to the \f$ u \f$ axes in
+ the frame (u,v).
+
+ \param u : The desired value for the coordinate along the \f$ u \f$ axes.
+
+ \sa set_i(), set_j(), set_v()
+ */
+ inline void set_u(const double u) { j = u ; }
+
+ /*!
+
+ Sets the point coordinate corresponding to the \f$ v \f$ axes in
+ the frame (u,v).
+
+ \param v : The desired value for the coordinate along the \f$ v \f$ axes.
+
+ \sa set_i(), set_j(), set_u()
+ */
+ inline void set_v(const double v) { i = v ; }
+
+ /*!
+
+ Sets the point coordinates in the frame (u,v).
+
+ \param u : The desired value for the coordinate along the \f$ u \f$ axes.
+ \param v : The desired value for the coordinate along the \f$ v \f$ axes.
+
+ \sa set_i(), set_j(), set_u(), set_v()
+ */
+ inline void set_uv(const double u, const double v) {
+ this->i = v ;
+ this->j = u ;
+ }
+
+ /*!
+
+ Gets the point coordinate corresponding to the \f$ u \f$ axes in
+ the frame (u,v).
+
+ \return The value of the coordinate along the \f$ u \f$ axes.
+
+ \sa get_i(), get_j(), get_v()
+ */
+ inline double get_u() const { return j ; }
+
+ /*!
+
+ Gets the point coordinate corresponding to the \f$ v \f$ axes in
+ the frame (u,v).
+
+ \return The value of the coordinate along the \f$ v \f$ axes.
+
+ \sa get_i(), get_j(), get_u()
+ */
+ inline double get_v() const { return i; }
+
+ /*!
+
+ Compute the distance \f$ |iP1 - iP2| = \sqrt{(i_1-i_2)^2+(j_1-j_2)^2} \f$
+
+ \param iP1 : First point
+ \param iP2 : Second point
+
+ \return the distance between the two points.
+ */
+ static double distance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
+ return(sqrt(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j())));}
+
+
+ static vpRect getBBox(const std::vector<vpImagePoint>& ipVec);
+
+ /*!
+
+ Compute the distance \f$ |iP1 - iP2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
+
+ \param iP1 : First point
+ \param iP2 : Second point
+
+ \return the distance between the two points.
+ */
+ static double sqrDistance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
+ return(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j()));}
+
+
+ bool inRectangle( const vpRect &rect ) const;
+
+ friend VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 );
+ friend VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
+ friend VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
+ friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint &ip2 );
+ friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset );
+ friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const double offset );
+ friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const vpImagePoint &ip2 );
+ friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const int offset );
+ friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const double offset );
+ friend VISP_EXPORT vpImagePoint operator*( const vpImagePoint &ip1, const double scale );
+ friend VISP_EXPORT vpImagePoint operator/( const vpImagePoint &ip1, const double scale );
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip);
+
+ private:
+ double i,j;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h
new file mode 100644
index 0000000..0d744dd
--- /dev/null
+++ b/modules/core/include/visp3/core/vpImageTools.h
@@ -0,0 +1,654 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image tools.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpImageTools_H
+#define vpImageTools_H
+
+/*!
+ \file vpImageTools.h
+
+ \brief Various image tools; sub-image extraction, modification of
+ the look up table, binarisation...
+
+*/
+
+#include <visp3/core/vpImage.h>
+
+#ifdef VISP_HAVE_PTHREAD
+# include <pthread.h>
+#endif
+
+#include <visp3/core/vpImageException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <string.h>
+
+/*!
+ \class vpImageTools
+
+ \ingroup group_core_image
+
+ \brief Various image tools; sub-image extraction, modification of
+ the look up table, binarisation...
+
+*/
+class VISP_EXPORT vpImageTools
+{
+
+public:
+ template<class Type>
+ static void createSubImage(const vpImage<Type> &I,
+ unsigned int i_sub, unsigned int j_sub,
+ unsigned int nrow_sub, unsigned int ncol_sub,
+ vpImage<Type> &S);
+
+ template<class Type>
+ static void createSubImage(const vpImage<Type> &I,
+ const vpRect &rect,
+ vpImage<Type> &S);
+
+ template<class Type>
+ static inline void binarise(vpImage<Type> &I,
+ Type threshold1, Type threshold2,
+ Type value1, Type value2, Type value3, const bool useLUT=false);
+
+ static void changeLUT(vpImage<unsigned char>& I,
+ unsigned char A,
+ unsigned char newA,
+ unsigned char B,
+ unsigned char newB);
+
+ template<class Type>
+ static void undistort(const vpImage<Type> &I,
+ const vpCameraParameters &cam,
+ vpImage<Type> &newI);
+
+ template<class Type>
+ static void flip(const vpImage<Type> &I,
+ vpImage<Type> &newI);
+
+ template<class Type>
+ static void flip(vpImage<Type> &I);
+
+
+ static void imageDifference(const vpImage<unsigned char> &I1,
+ const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Idiff) ;
+
+ static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
+ const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Idiff);
+} ;
+
+/*!
+ Extract a sub part of an image
+
+ \param I : Input image from which a sub image will be extracted.
+ \param i_sub, j_sub : coordinates of the upper left point of the sub image
+ \param nrow_sub, ncol_sub : number of row, column of the sub image
+ \param S : Sub-image.
+*/
+template<class Type>
+void vpImageTools::createSubImage(const vpImage<Type> &I,
+ unsigned int i_sub, unsigned int j_sub,
+ unsigned int nrow_sub, unsigned int ncol_sub,
+ vpImage<Type> &S)
+{
+ unsigned int i,j ;
+ unsigned int imax = i_sub + nrow_sub ;
+ unsigned int jmax = j_sub + ncol_sub ;
+
+ if (imax > I.getHeight())
+ {
+ imax = I.getHeight() -1 ;
+ nrow_sub = imax-i_sub ;
+ }
+ if (jmax > I.getWidth())
+ {
+ jmax = I.getWidth() -1 ;
+ ncol_sub = jmax -j_sub ;
+ }
+
+ S.resize(nrow_sub, ncol_sub) ;
+ for (i=i_sub ; i < imax ; i++)
+ for (j=j_sub ; j < jmax ; j++)
+ {
+ S[i-i_sub][j-j_sub] = I[i][j] ;
+ }
+}
+
+/*!
+ Extract a sub part of an image
+
+ \param I : Input image from which a sub image will be extracted.
+
+ \param rect : Rectangle area in the image \e I corresponding to the
+ sub part of the image to extract.
+
+ \param S : Sub-image.
+*/
+template<class Type>
+void vpImageTools::createSubImage(const vpImage<Type> &I,
+ const vpRect &rect,
+ vpImage<Type> &S)
+{
+ double dleft = rect.getLeft();
+ double dtop = rect.getTop();
+ double dright = ceil( rect.getRight() );
+ double dbottom = ceil( rect.getBottom() );
+
+ if (dleft < 0.0) dleft = 0.0;
+ else if (dleft >= I.getWidth()) dleft = I.getWidth() - 1;
+
+ if (dright < 0.0) dright = 0.0;
+ else if (dright >= I.getWidth()) dright = I.getWidth() - 1;
+
+ if (dtop < 0.0) dtop = 0.0;
+ else if (dtop >= I.getHeight()) dtop = I.getHeight() - 1;
+
+ if (dbottom < 0.0) dbottom = 0.0;
+ else if (dbottom >= I.getHeight()) dbottom = I.getHeight() - 1;
+
+ // Convert the double-precision rectangle coordinates into integer positions
+ unsigned int left = (unsigned int) dleft;
+ unsigned int top = (unsigned int) dtop;
+ unsigned int bottom = (unsigned int) dbottom;
+ unsigned int right = (unsigned int) dright;
+
+ unsigned int width = right - left + 1;;
+ unsigned int height = bottom - top + 1;
+
+ S.resize(height, width) ;
+ for (unsigned int i=top ; i <= bottom ; i++) {
+ for (unsigned int j=left ; j <= right ; j++) {
+ S[i-top][j-left] = I[i][j] ;
+ }
+ }
+}
+
+/*!
+
+ Binarise an image.
+
+ - Pixels whose values are less than \e threshold1 are set to \e value1
+
+ - Pixels whose values are greater then or equal to \e threshold1 and
+ less then or equal to \e threshold2 are set to \e value2
+
+ - Pixels whose values are greater than \e threshold2 are set to \e value3
+
+*/
+template<class Type>
+inline void vpImageTools::binarise(vpImage<Type> &I,
+ Type threshold1, Type threshold2,
+ Type value1, Type value2, Type value3, const bool useLUT)
+{
+ if(useLUT) {
+ std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
+ }
+
+ Type v;
+ Type *p = I.bitmap;
+ Type *pend = I.bitmap + I.getWidth()*I.getHeight();
+ for (; p < pend; p ++) {
+ v = *p;
+ if (v < threshold1) *p = value1;
+ else if (v > threshold2) *p = value3;
+ else *p = value2;
+ }
+}
+
+/*!
+
+ Binarise an image.
+
+ - Pixels whose values are less than \e threshold1 are set to \e value1
+
+ - Pixels whose values are greater then or equal to \e threshold1 and
+ less then or equal to \e threshold2 are set to \e value2
+
+ - Pixels whose values are greater than \e threshold2 are set to \e value3
+
+*/
+template<>
+inline void vpImageTools::binarise(vpImage<unsigned char> &I,
+ unsigned char threshold1, unsigned char threshold2,
+ unsigned char value1, unsigned char value2, unsigned char value3, const bool useLUT)
+{
+ if(useLUT) {
+ //Construct the LUT
+ unsigned char lut[256];
+ for(unsigned int i = 0; i < 256; i++) {
+ lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
+ }
+
+ I.performLut(lut);
+ } else {
+ unsigned char v;
+ unsigned char *p = I.bitmap;
+ unsigned char *pend = I.bitmap + I.getWidth()*I.getHeight();
+ for (; p < pend; p ++) {
+ v = *p;
+ if (v < threshold1) *p = value1;
+ else if (v > threshold2) *p = value3;
+ else *p = value2;
+ }
+ }
+}
+
+#ifdef VISP_HAVE_PTHREAD
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+template<class Type>
+class vpUndistortInternalType
+{
+public:
+ Type *src;
+ Type *dst;
+ unsigned int width;
+ unsigned int height;
+ vpCameraParameters cam;
+ unsigned int nthreads;
+ unsigned int threadid;
+public:
+ vpUndistortInternalType()
+ : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0)
+ {};
+
+ vpUndistortInternalType(const vpUndistortInternalType<Type> &u) {
+ *this = u;
+ };
+ vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u) {
+ src = u.src;
+ dst = u.dst;
+ width = u.width;
+ height = u.height;
+ cam = u.cam;
+ nthreads = u.nthreads;
+ threadid = u.threadid;
+ }
+
+ static void *vpUndistort_threaded(void *arg);
+};
+
+
+template<class Type>
+void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
+{
+ vpUndistortInternalType<Type> *undistortSharedData = (vpUndistortInternalType<Type>*)arg;
+ int offset = (int)undistortSharedData->threadid;
+ int width = (int)undistortSharedData->width;
+ int height = (int)undistortSharedData->height;
+ int nthreads = (int)undistortSharedData->nthreads;
+
+ double u0 = undistortSharedData->cam.get_u0();
+ double v0 = undistortSharedData->cam.get_v0();
+ double px = undistortSharedData->cam.get_px();
+ double py = undistortSharedData->cam.get_py();
+ double kud = undistortSharedData->cam.get_kud();
+
+ double invpx = 1.0/px;
+ double invpy = 1.0/py;
+
+ double kud_px2 = kud * invpx * invpx;
+ double kud_py2 = kud * invpy * invpy;
+
+ Type *dst = undistortSharedData->dst+(height/nthreads*offset)*width;
+ Type *src = undistortSharedData->src;
+
+ for (double v = height/nthreads*offset;v < height/nthreads*(offset+1) ; v++) {
+ double deltav = v - v0;
+ //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
+ double fr1 = 1.0 + kud_py2 * deltav * deltav;
+
+ for (double u = 0 ; u < width ; u++) {
+ //computation of u,v : corresponding pixel coordinates in I.
+ double deltau = u - u0;
+ //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
+ double fr2 = fr1 + kud_px2 * deltau * deltau;
+
+ double u_double = deltau * fr2 + u0;
+ double v_double = deltav * fr2 + v0;
+
+ //computation of the bilinear interpolation
+
+ //declarations
+ int u_round = (int) (u_double);
+ int v_round = (int) (v_double);
+ if (u_round < 0.f) u_round = -1;
+ if (v_round < 0.f) v_round = -1;
+ double du_double = (u_double) - (double) u_round;
+ double dv_double = (v_double) - (double) v_round;
+ Type v01;
+ Type v23;
+ if ( (0 <= u_round) && (0 <= v_round) &&
+ (u_round < ((width) - 1)) && (v_round < ((height) - 1)) ) {
+ //process interpolation
+ const Type* _mp = &src[v_round*width+u_round];
+ v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
+ _mp += width;
+ v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
+ *dst = (Type)(v01 + ((v23 - v01) * dv_double));
+ }
+ else {
+ *dst = 0;
+ }
+ dst++;
+ }
+ }
+
+ pthread_exit((void*) 0);
+ return NULL;
+}
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+#endif // VISP_HAVE_PTHREAD
+
+/*!
+ Undistort an image
+
+ \param I : Input image to undistort.
+
+ \param cam : Parameters of the camera causing distortion.
+
+ \param undistI : Undistorted output image. The size of this image
+ will be the same than the input image \e I. If the distortion
+ parameter \f$K_d\f$ is null (see cam.get_kd_mp()), \e undistI is
+ just a copy of \e I.
+
+ \warning This function works only with Types authorizing "+,-,
+ multiplication by a scalar" operators.
+
+ \warning This function is time consuming :
+ - On "Rhea"(Intel Core 2 Extreme X6800 2.93GHz, 2Go RAM)
+ or "Charon"(Intel Xeon 3 GHz, 2Go RAM) : ~8 ms for a 640x480 image.
+*/
+template<class Type>
+void vpImageTools::undistort(const vpImage<Type> &I,
+ const vpCameraParameters &cam,
+ vpImage<Type> &undistI)
+{
+#ifdef VISP_HAVE_PTHREAD
+ //
+ // Optimized version using pthreads
+ //
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ undistI.resize(height, width);
+
+ double kud = cam.get_kud();
+
+ //if (kud == 0) {
+ if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
+ // There is no need to undistort the image
+ undistI = I;
+ return;
+ }
+
+ unsigned int nthreads = 2;
+ pthread_attr_t attr;
+ pthread_t *callThd = new pthread_t [nthreads];
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+ vpUndistortInternalType<Type> *undistortSharedData;
+ undistortSharedData = new vpUndistortInternalType<Type> [nthreads];
+
+ for(unsigned int i=0;i<nthreads;i++) {
+ // Each thread works on a different set of data.
+ // vpTRACE("create thread %d", i);
+ undistortSharedData[i].src = I.bitmap;
+ undistortSharedData[i].dst = undistI.bitmap;
+ undistortSharedData[i].width = I.getWidth();
+ undistortSharedData[i].height = I.getHeight();
+ undistortSharedData[i].cam = cam;
+ undistortSharedData[i].nthreads = nthreads;
+ undistortSharedData[i].threadid = i;
+ pthread_create( &callThd[i], &attr,
+ &vpUndistortInternalType<Type>::vpUndistort_threaded,
+ &undistortSharedData[i]);
+ }
+ pthread_attr_destroy(&attr);
+ /* Wait on the other threads */
+
+ for(unsigned int i=0;i<nthreads;i++) {
+ // vpTRACE("join thread %d", i);
+ pthread_join( callThd[i], NULL);
+ }
+
+ delete [] callThd;
+ delete [] undistortSharedData;
+#else // VISP_HAVE_PTHREAD
+ //
+ // optimized version without pthreads
+ //
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ undistI.resize(height, width);
+
+ double u0 = cam.get_u0();
+ double v0 = cam.get_v0();
+ double px = cam.get_px();
+ double py = cam.get_py();
+ double kud = cam.get_kud();
+
+ //if (kud == 0) {
+ if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
+ // There is no need to undistort the image
+ undistI = I;
+ return;
+ }
+
+ double invpx = 1.0/px;
+ double invpy = 1.0/py;
+
+ double kud_px2 = kud * invpx * invpx;
+ double kud_py2 = kud * invpy * invpy;
+
+ Type *dst = undistI.bitmap;
+ for (double v = 0;v < height ; v++) {
+ double deltav = v - v0;
+ //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
+ double fr1 = 1.0 + kud_py2 * deltav * deltav;
+
+ for (double u = 0 ; u < width ; u++) {
+ //computation of u,v : corresponding pixel coordinates in I.
+ double deltau = u - u0;
+ //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
+ double fr2 = fr1 + kud_px2 * deltau * deltau;
+
+ double u_double = deltau * fr2 + u0;
+ double v_double = deltav * fr2 + v0;
+
+ //printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
+
+ //computation of the bilinear interpolation
+
+ //declarations
+ int u_round = (int) (u_double);
+ int v_round = (int) (v_double);
+ if (u_round < 0.f) u_round = -1;
+ if (v_round < 0.f) v_round = -1;
+ double du_double = (u_double) - (double) u_round;
+ double dv_double = (v_double) - (double) v_round;
+ Type v01;
+ Type v23;
+ if ( (0 <= u_round) && (0 <= v_round) &&
+ (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1)) ) {
+ //process interpolation
+ const Type* _mp = &I[(unsigned int)v_round][(unsigned int)u_round];
+ v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
+ _mp += width;
+ v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
+ *dst = (Type)(v01 + ((v23 - v01) * dv_double));
+ //printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
+ }
+ else {
+ *dst = 0;
+ }
+ dst++;
+ }
+ }
+#endif // VISP_HAVE_PTHREAD
+
+
+
+
+#if 0
+ // non optimized version
+ int width = I.getWidth();
+ int height = I.getHeight();
+
+ undistI.resize(height,width);
+
+ double u0 = cam.get_u0();
+ double v0 = cam.get_v0();
+ double px = cam.get_px();
+ double py = cam.get_py();
+ double kd = cam.get_kud();
+
+ if (kd == 0) {
+ // There is no need to undistort the image
+ undistI = I;
+ return;
+ }
+
+ for(int v = 0 ; v < height; v++){
+ for(int u = 0; u < height; u++){
+ double r2 = vpMath::sqr(((double)u - u0)/px) +
+ vpMath::sqr(((double)v-v0)/py);
+ double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
+ double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
+ undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double);
+ }
+ }
+#endif
+}
+
+/*!
+ Flip vertically the input image and give the result in the output image.
+
+ \param I : Input image to flip.
+ \param newI : Output image which is the flipped input image.
+*/
+
+template<class Type>
+void vpImageTools::flip(const vpImage<Type> &I,
+ vpImage<Type> &newI)
+{
+ unsigned int height = 0, width = 0;
+
+ height = I.getHeight();
+ width = I.getWidth();
+ newI.resize(height, width);
+
+ for (unsigned int i = 0; i < height; i++)
+ {
+ memcpy(newI.bitmap+i*width, I.bitmap+(height-1-i)*width,
+ width*sizeof(Type));
+ }
+}
+
+
+/*!
+ Flip vertically the input image.
+
+ \param I : Input image which is flipped and modified in output.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+ vpImage<vpRGBa> I;
+#ifdef _WIN32
+ std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
+#else
+ std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
+#endif
+
+ // Read an image from the disk
+ vpImageIo::read(I, filename);
+
+ // Flip the image
+ vpImageTools::flip(I);
+
+ vpImageIo::write(I, "Klimt-flip.ppm"); // Write the image in a PGM P5 image file format
+}
+ \endcode
+*/
+template<class Type>
+void vpImageTools::flip(vpImage<Type> &I)
+{
+ unsigned int height = 0, width = 0;
+ unsigned int i = 0;
+ vpImage<Type> Ibuf;
+
+ height = I.getHeight();
+ width = I.getWidth();
+ Ibuf.resize(1, width);
+
+ for ( i = 0; i < height/2; i++)
+ {
+ memcpy(Ibuf.bitmap, I.bitmap+i*width,
+ width*sizeof(Type));
+
+ memcpy(I.bitmap+i*width, I.bitmap+(height-1-i)*width,
+ width*sizeof(Type));
+ memcpy(I.bitmap+(height-1-i)*width, Ibuf.bitmap,
+ width*sizeof(Type));
+ }
+}
+
+#endif
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpIoException.h b/modules/core/include/visp3/core/vpIoException.h
new file mode 100644
index 0000000..9e52677
--- /dev/null
+++ b/modules/core/include/visp3/core/vpIoException.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpIo class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpIoException_H
+#define __vpIoException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/*!
+ \file vpIoException.h
+ \brief Error that can be emited by the vpIoTools class and its derivates.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+#include <iostream>
+#include <string>
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpIoException
+ \ingroup group_core_debug
+ \brief Error that can be emited by the vpIoTools class and its derivates.
+ */
+class VISP_EXPORT vpIoException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited 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:
+ vpIoException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpIoException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpIoException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h
new file mode 100644
index 0000000..f46ecd1
--- /dev/null
+++ b/modules/core/include/visp3/core/vpIoTools.h
@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Directory management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpIoTools_HH
+#define vpIoTools_HH
+
+/*!
+ \file vpIoTools.h
+ \brief File and directories basic tools.
+ */
+
+#include <visp3/core/vpConfig.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <stdlib.h>
+#include <vector>
+#include <visp3/core/vpColor.h>
+
+/*!
+ \class vpIoTools
+ \ingroup group_core_files_io
+ \brief File and directories basic tools.
+
+ The example below shows how to manipulate the functions of this
+ class to create first a directory which name corresponds to the user
+ name and then create a file in this directory.
+
+ \code
+#include <iostream>
+#include <string>
+#include <fstream>
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ std::string username;
+ vpIoTools::getUserName(username);
+
+ // Test if a username directory exist. If no try to create it
+ if (vpIoTools::checkDirectory(username) == false) {
+ try {
+ // Create a directory with name "username"
+ vpIoTools::makeDirectory(username);
+ }
+ catch (...) {
+ std::cout << "Cannot create " << username << " directory" << std::endl;
+ return false;
+ }
+ }
+ // Create a empty filename with name "username/file.txt"
+ std::ofstream f;
+ std::string filename = username + "/file.txt";
+ filename = vpIoTools::path(filename); // Under Windows converts the filename string into "username\\file.txt"
+
+ std::cout << "Create: " << filename << std::endl;
+ f.open(filename.c_str());
+ f.close();
+
+ // Rename the file
+ std::string newfilename = username + "/newfile.txt";
+ std::cout << "Rename: " << filename << " in: " << newfilename << std::endl;
+ if (vpIoTools::rename(filename, newfilename) == false)
+ std::cout << "Unable to rename: " << filename << std::endl;
+
+ // Remove the file
+ std::cout << "Remove: " << newfilename << std::endl;
+ if (vpIoTools::remove(newfilename) == false)
+ std::cout << "Unable to remove: " << newfilename << std::endl;
+}
+ \endcode
+
+ The example below shows how to read a configuration file and how to create a name for experiment files.
+ We assume the following file "/home/user/demo/config.txt" :
+ \code
+expNumber 2
+save 0
+lambda 0.4
+use2D 0
+use3D 1
+ \endcode
+
+ \code
+#include <iostream>
+#include <string>
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ // reading configuration file
+ vpIoTools::loadConfigFile("/home/user/demo/config.txt");
+ std::string nExp;vpIoTools::readConfigVar("expNumber", nExp); // nExp <- "2"
+ double lambda;vpIoTools::readConfigVar("lambda", lambda); // lambda <- 0.4
+ bool use2D;vpIoTools::readConfigVar("use2D", use2D); // use2D <- false
+ bool use3D;vpIoTools::readConfigVar("use3D", use3D); // use3D <- true
+ bool doSave;vpIoTools::readConfigVar("save", doSave); // doSave <- false
+
+ // creating name for experiment files
+ vpIoTools::setBaseDir("/home/user/data");
+ vpIoTools::setBaseName("exp" + nExp); // full name <- "/home/user/data/exp2"
+ vpIoTools::addNameElement("2D", use2D); // full name <- "/home/user/data/exp2" since use2D==false
+ vpIoTools::addNameElement("3D", use3D); // full name <- "/home/user/data/exp2_3D"
+ vpIoTools::addNameElement("lambda", lambda); // full name <- "/home/user/data/exp2_3D_lambda0.4"
+
+ // saving file
+ vpIoTools::saveConfigFile(doSave); // would copy "/home/user/demo/config.txt" to "/home/user/data/exp2_3D_lambda0.4_config.txt" if doSave was true
+
+ // create sub directory
+ vpIoTools::createBaseNamePath(); // creates "/home/user/data/exp2_3D_lambda0.4/"
+}
+ \endcode
+
+ */
+
+class VISP_EXPORT vpIoTools
+{
+
+public:
+ static void getUserName(std::string &username);
+ static std::string getUserName();
+ static std::string getenv(const char *env);
+ static std::string getenv(const std::string &env);
+ static std::string getViSPImagesDataPath();
+ static void getVersion(const std::string &version, unsigned int &major, unsigned int &minor, unsigned int &patch);
+ static bool checkDirectory(const char *dirname);
+ static bool checkDirectory(const std::string &dirname);
+ static bool checkFilename(const char *filename);
+ static bool checkFilename(const std::string &filename);
+ static bool copy(const char *src, const char *dst);
+ static bool copy(const std::string &src, const std::string &dst);
+ static void makeDirectory(const char *dirname);
+ static void makeDirectory(const std::string &dirname);
+ static bool remove(const char *filename);
+ static bool remove(const std::string &filename);
+ static bool rename(const char *oldfilename, const char *newfilename);
+ static bool rename(const std::string &oldfilename, const std::string &newfilename);
+
+ static std::string path(const char * pathname);
+ static std::string path(const std::string &pathname);
+
+ /*!
+ Define the directory separator character, backslash ('\') for windows platform or slash ('/') otherwise.
+ */
+ static const char separator =
+ #if defined(_WIN32)
+ '\\';
+ #else
+ '/';
+ #endif
+
+ static std::string getFileExtension(const std::string &pathname, const bool checkFile=false);
+ static std::string getName(const std::string &pathname);
+ static std::string getNameWE(const std::string &pathname);
+ static std::string getParent(const std::string& pathname);
+ static std::string createFilePath(const std::string& parent, const std::string child);
+ static bool isAbsolutePathname(const std::string& pathname);
+ static std::pair<std::string, std::string> splitDrive(const std::string& pathname);
+ static std::vector<std::string> splitChain(const std::string & chain, const std::string & sep);
+
+ /*!
+ @name Configuration file parsing
+ */
+ //@{
+ // read configuration file
+ static bool loadConfigFile(const std::string &confFile);
+ static bool readConfigVar(const std::string &var, float &value);
+ static bool readConfigVar(const std::string &var, double &value);
+ static bool readConfigVar(const std::string &var, int &value);
+ static bool readConfigVar(const std::string &var, unsigned int &value);
+ static bool readConfigVar(const std::string &var, bool &value);
+ static bool readConfigVar(const std::string &var, std::string &value);
+ static bool readConfigVar(const std::string &var, vpColor &value);
+ static bool readConfigVar(const std::string &var, vpArray2D<double> &value,
+ const unsigned int &nCols = 0,
+ const unsigned int &nRows = 0);
+
+ // construct experiment filename & path
+ static void setBaseName(const std::string &s);
+ static void setBaseDir(const std::string &dir);
+ static void addNameElement(const std::string &strTrue,
+ const bool &cond=true,
+ const std::string &strFalse="");
+ static void addNameElement(const std::string &strTrue, const double &val);
+ static std::string getBaseName();
+ static std::string getFullName();
+
+ // write files
+ static void saveConfigFile(const bool &actuallySave = true);
+ static void createBaseNamePath(const bool &empty = false);
+ //@}
+
+ protected:
+ static std::string baseName;
+ static std::string baseDir;
+ static std::string configFile;
+ static std::vector<std::string> configVars;
+ static std::vector<std::string> configValues;
+} ;
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpKalmanFilter.h b/modules/core/include/visp3/core/vpKalmanFilter.h
new file mode 100644
index 0000000..5a88164
--- /dev/null
+++ b/modules/core/include/visp3/core/vpKalmanFilter.h
@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Kalman filtering.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpKalmanFilter_h
+#define vpKalmanFilter_h
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+#include <math.h>
+
+/*!
+ \file vpKalmanFilter.h
+ \brief Generic kalman filtering implementation
+*/
+
+/*!
+ \class vpKalmanFilter
+ \ingroup group_core_kalman
+ \brief This class provides a generic Kalman filtering algorithm along with
+ some specific state model (constant velocity, constant acceleration)
+ which are implemented in the vpLinearKalmanFilterInstantiation class.
+
+ The state evolution equation is given by:
+ \f[
+ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
+ \f]
+ where \f${\bf x}_{k}\f$ is the unknown state at iteration \f$k\f$.
+
+ The measurement equation is given by:
+ \f[
+ {\bf z}_k = {\bf H} {\bf x}_k + {\bf r}_k
+ \f]
+ where \f${\bf z}_{k}\f$ is the measure (also named observation) at iteration \f$k\f$.
+
+ The predicted state is obtained by:
+ \f[
+ {\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}
+ \f]
+ \f[
+ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
+ + {\bf Q}_k \f]
+ where
+ <ul>
+ <li> \f$ {\bf x}_{k|k-1}\f$ is the prediction of the state,
+ <li> \f$ {\bf P}_{k \mid k-1}\f$ is the state prediction covariance matrix.
+ </ul>
+ Filtering equation are:
+ \f[
+ {\bf W}_k = {\bf P}_{k \mid k-1} {\bf H}^T
+ \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}
+ \f]
+ \f[
+ {\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k -
+ {\bf H x}_{k \mid k-1} \right]
+ \f]
+ \f[
+ {\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H} \right) {\bf P}_{k \mid
+ k-1} \f]
+
+ where \f$ {\bf W}_k \f$ is the filter gain.
+
+ Notice that there is a recursion for the inverse covariance
+ \f[
+ {\bf P}_{k \mid k}^{-1}= {\bf P}_{k \mid k-1}^{-1} + {\bf H}^T {\bf
+ R}^{-1} {\bf H}
+ \f]
+ where \f${\bf P}_{k \mid k}^{-1}\f$ is the inverse of the covariance matrix.
+
+ ViSP provides different state evolution models implemented in the
+ vpLinearKalmanFilterInstantiation class.
+*/
+class VISP_EXPORT vpKalmanFilter
+{
+protected :
+ //bool init_done ;
+
+ //! Filter step or iteration. When set to zero, initialize the filter.
+ long iter ;
+
+ //! Size of the state vector \f${\bf x}_k\f$.
+ unsigned int size_state ;
+ //! Size of the measure vector \f${\bf z}_k\f$.
+ unsigned int size_measure ;
+ //! Number of signal to filter.
+ unsigned int nsignal ;
+
+ //! When set to true, print the content of internal variables during filtering() and prediction().
+ bool verbose_mode;
+
+public:
+ vpKalmanFilter() ;
+ vpKalmanFilter(unsigned int n_signal) ;
+ vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
+ /*! Destructor that does noting. */
+ virtual ~vpKalmanFilter() {};
+ /*!
+ Set the number of signal to filter.
+ */
+ void setNumberOfSignal(unsigned int n_signal)
+ {
+ this->nsignal = n_signal;
+ }
+
+ // int init() { return init_done ; }
+ void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
+ void prediction() ;
+ void filtering(const vpColVector &z) ;
+ /*!
+ Return the size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
+ */
+ unsigned int getStateSize() { return size_state; };
+ /*!
+ Return the size of the measure vector \f${\bf z}_{(k)}\f$ for one signal.
+ */
+ unsigned int getMeasureSize() { return size_measure; };
+ /*!
+ Return the number of signal to filter.
+ */
+ unsigned int getNumberOfSignal() { return nsignal; };
+ /*!
+ Return the iteration number.
+ */
+ int getIteration() { return iter ; }
+ /*!
+ Sets the verbose mode.
+ \param on : If true, activates the verbose mode which consists in printing the Kalman
+ filter internal values.
+ */
+ void verbose(bool on) { verbose_mode = on;};
+
+public:
+ /*!
+ The updated state estimate \f${\bf x}_{k \mid k} \f$ where
+ \f${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k
+ \left[ {\bf z}_k - {\bf H x}_{k \mid k-1} \right]\f$.
+ */
+ vpColVector Xest ;
+ /*!
+ The predicted state \f${\bf x}_{k \mid k-1} \f$ where
+ \f$ {\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}\f$.
+ */
+ vpColVector Xpre ;
+ //! Transition matrix \f${\bf F}\f$ that describes the evolution of the state.
+ vpMatrix F ;
+
+ //! Matrix \f${\bf H}\f$ that describes the evolution of the measurements.
+ vpMatrix H ;
+
+ //! Measurement noise covariance matrix \f${\bf R}\f$.
+ vpMatrix R ;
+ //! Process noise covariance matrix \f${\bf Q}\f$.
+ vpMatrix Q ;
+ /*! Sampling time \f$\Delta t\f$ in second between two succesive
+ iterations. Only used in some specific state models implemented
+ in vpLinearKalmanFilterInstantiation.*/
+ double dt ;
+ /*!
+ The state prediction covariance \f${\bf P}_{k \mid k-1} \f$ where
+ \f$ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
+ + {\bf Q}_k\f$.
+ */
+ vpMatrix Ppre ;
+
+ /*!
+ The updated covariance of the state \f${\bf P}_{k \mid k}\f$
+ where \f${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H}
+ \right) {\bf P}_{k \mid k-1}\f$.
+ */
+ vpMatrix Pest ;
+
+protected:
+
+ /*!
+ Filter gain \f${\bf W}_k\f$ where \f$ {\bf W}_k = {\bf P}_{k
+ \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf
+ R}_k \right]^{-1}\f$.
+ */
+ vpMatrix W ;
+
+ //! Identity matrix \f$ \bf I\f$.
+ vpMatrix I ;
+} ;
+
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpLine.h b/modules/core/include/visp3/core/vpLine.h
new file mode 100644
index 0000000..3ac5d75
--- /dev/null
+++ b/modules/core/include/visp3/core/vpLine.h
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Line feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpLine_H
+#define vpLine_H
+
+/*!
+ \file vpLine.h
+ \brief class that defines what is a line
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+#include <visp3/core/vpForwardProjection.h>
+
+/*!
+ \class vpLine
+ \ingroup group_core_geometry
+
+ \brief Class that defines a line in the object frame, the
+ camera frame and the image plane. All the parameters
+ must be set in meter.
+
+ \par Object and camera frame parametrization:
+ In the 3D frames, the object frame parameters (\e oP) and the camera
+ frame parameters (\e cP), the line is defined as the intersection
+ between two plans. Thus, the parameters which define the line are
+ the parameters which define the two equations of the two plans. Each
+ point which belongs to the line is a solution of those two
+ equations:
+
+ \par
+ \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
+ \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
+ Here \f$ (X, Y, Z) \f$ are the 3D coordinates in one of the two 3D frames.
+
+ \par
+ In this class it is easily possible to compute the parameters (\e cP) of the
+ line in the camera frame thanks to its parameters (\e oP) in the
+ object frame. But you have to notes that four constraints are
+ added in the planes equations.
+
+ \par
+ \f[ D1 = 0 \f]
+ \f[ D2 > 0 \f]
+ \f[ A1 A2 + B1 B2 + C1 C2 = 0 \f]
+ \f[ || A2 || = 1 \f]
+
+
+ \par
+ The line parameters \e oP corresponding to the object frame are
+ located in the vpForwardProjection::oP public attribute, where \e oP
+ is a vector defined as: \f[ oP = \left[\begin{array}{c}A1_o \\ B1_o
+ \\ C1_o \\ D1_o \\ A2_o \\ B2_o \\ C2_o \\ D2_o \end{array}\right]
+ \f]
+
+ \par
+ The line parameters corresponding to the camera frame are located
+ in the vpTracker::cP public attribute, where \e cP is a vector
+ defined as: \f[ cP = \left[\begin{array}{c}A1_c \\ B1_c \\ C1_c \\
+ D1_c \\ A2_c \\ B2_c \\ C2_c \\ D2_c \end{array}\right] \f]
+
+ \par Image plane parametrization:
+ In the image plane, the line is defined thanks to its 2D equation.
+ \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f] Here \f$ x
+ \f$ and \f$ y \f$ are the coordinates of a point belonging to the
+ line in the image plane while \f$ \rho \f$ and \f$ \theta \f$ are
+ the parameters used to define the line. The value of \f$ \theta
+ \f$ is between \f$ -\pi/2 \f$ and \f$ \pi/2 \f$ and the value of
+ \f$ \rho \f$ can be positive or negative. The conventions used to
+ choose the sign of \f$ \rho \f$ and the value of \f$ \theta \f$
+ are illustrated by the following image.
+
+ \par
+ \image html vpFeatureLine.gif
+ \image latex vpFeatureLine.ps width=10cm
+
+ \par
+ The line parameters corresponding to the image frame are located
+ in the vpTracker::p public attribute, where \e p is a vector defined
+ as: \f[ p = \left[\begin{array}{c} \rho \\ \theta \end{array}\right]
+ \f]
+*/
+class VISP_EXPORT vpLine : public vpForwardProjection
+{
+
+public:
+
+ void init() ;
+
+ vpLine() ;
+ //! Destructor
+ virtual ~vpLine() { ; }
+
+ /*!
+
+ Sets the \f$ \rho \f$ parameter used to define the line in the
+ image plane.
+
+ \param rho : The desired value for \f$ \rho \f$.
+
+ \sa setTheta()
+ */
+ void setRho(const double rho) { p[0] = rho ; };
+
+ /*!
+ Sets the \f$ \theta \f$ angle value used to define the line in the
+ image plane.
+
+ \param theta : The desired value for \f$ \theta \f$ angle.
+
+ \sa setRho()
+ */
+ void setTheta(const double theta) { p[1] = theta ;};
+
+ /*!
+
+ Gets the \f$ \theta \f$ angle value corresponding to one of the
+ two parameters used to define the line parametrization in the
+ image plane.
+
+ \return Returns the current value of \f$ \theta \f$.
+
+ \sa getRho()
+ */
+ double getTheta() const { return p[1] ; }
+
+ /*!
+ Gets the \f$ \rho \f$ value corresponding to one of the
+ two parameters used to define the line parametrization in the
+ image plane.
+
+ \return Returns the current value of \f$ \rho \f$.
+
+ \sa getTheta()
+ */
+ double getRho() const { return p[0] ; }
+
+
+
+ void setWorldCoordinates(const double &A1, const double &B1,
+ const double &C1, const double &D1,
+ const double &A2, const double &B2,
+ const double &C2, const double &D2) ;
+
+
+ void setWorldCoordinates(const vpColVector &oP1,
+ const vpColVector &oP2) ;
+
+
+ void setWorldCoordinates(const vpColVector &oP) ;
+
+
+ void projection() ;
+ void projection(const vpColVector &cP, vpColVector &p) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo) ;
+
+ void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+
+ vpLine *duplicate() const ;
+} ;
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h
new file mode 100644
index 0000000..3ae9c1b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Kalman filtering.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpLinearKalmanFilterInstantiation_h
+#define vpLinearKalmanFilterInstantiation_h
+
+#include <visp3/core/vpKalmanFilter.h>
+
+#include <math.h>
+
+/*!
+ \file vpLinearKalmanFilterInstantiation.h
+ \brief Implementation of some specific linear Kalman filters.
+*/
+
+/*!
+ \class vpLinearKalmanFilterInstantiation
+ \ingroup group_core_kalman
+ \brief This class provides an implementation of some specific linear Kalman filters.
+*/
+class VISP_EXPORT vpLinearKalmanFilterInstantiation : public vpKalmanFilter
+{
+ public:
+ /*!
+ Selector used to set the Kalman filter state model.
+ */
+ typedef enum {
+ /*! Consider the state as a constant velocity model with white
+ noise. Measures available are the succesive positions of the
+ target. To know more about this state model, see
+ initStateConstVel_MeasurePos(). */
+ stateConstVel_MeasurePos,
+ /*! Consider the state as a constant velocity model with colored noise
+ measurements as acceleration terms. Measured available are the
+ velocities of the target. To know more about this state model,
+ see initStateConstVelWithColoredNoise_MeasureVel(). */
+ stateConstVelWithColoredNoise_MeasureVel,
+ /*! Consider the state as a constant acceleration model with colored noise
+ measurements as acceleration terms. Measured available are the
+ velocities of the target. To know more about this state model,
+ see initStateConstAccWithColoredNoise_MeasureVel(). */
+ stateConstAccWithColoredNoise_MeasureVel,
+ /*! Used to indicate that the state model is not initialized. */
+ unknown
+ } vpStateModel;
+
+ /*!
+ Default linear Kalman filter.
+
+ By default the state model is unknown and set to
+ vpLinearKalmanFilterInstantiation::unknown.
+ */
+ vpLinearKalmanFilterInstantiation() : model(unknown)
+ {
+ };
+
+ /*! Destructor that does nothng. */
+ virtual ~vpLinearKalmanFilterInstantiation() {};
+ /*!
+ Return the current state model.
+ */
+ inline vpStateModel getStateModel() {
+ return model;
+ }
+ void filter(vpColVector &z);
+
+ /*! @name Generic linear filter initializer */
+ //@{
+ inline void setStateModel(vpStateModel model);
+
+ void initFilter(unsigned int nsignal, vpColVector &sigma_state,
+ vpColVector &sigma_measure, double rho, double dt);
+ //@}
+
+ /*! @name Linear filter initializer with constant velocity models */
+ //@{
+ void initStateConstVel_MeasurePos(unsigned int nsignal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double dt);
+ void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double rho);
+ //@}
+
+ /*! @name Linear filter initializer with constant acceleration models */
+ //@{
+ void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double rho,
+ double dt);
+ //@}
+
+ protected:
+ vpStateModel model;
+
+} ;
+
+/*!
+ Set the Kalman state model. Depending on the state model, we set
+ the state vector size and the measure vector size.
+
+ The example below shows how to use this method and then to get the
+ size of the state and measure vectors.
+
+ \code
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+
+int main()
+{
+ vpLinearKalmanFilterInstantiation kalman;
+
+ kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
+ std::cout << "State vector size: " << kalman.getStateSize() << std::endl; // Value is 2
+ std::cout << "Measure vector size: " << kalman.getMeasureSize() << std::endl; // Value is 1
+}
+ \endcode
+*/
+void vpLinearKalmanFilterInstantiation::setStateModel(vpStateModel mdl) {
+ this->model = mdl;
+ switch(model) {
+ case stateConstVel_MeasurePos:
+ case stateConstVelWithColoredNoise_MeasureVel:
+ size_state = 2;
+ size_measure = 1;
+ break;
+ case stateConstAccWithColoredNoise_MeasureVel:
+ size_state = 3;
+ size_measure = 1;
+ break;
+ case unknown:
+ size_state = 0;
+ size_measure = 0;
+ break;
+ }
+}
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpList.h b/modules/core/include/visp3/core/vpList.h
new file mode 100644
index 0000000..094968b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpList.h
@@ -0,0 +1,844 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * List data structure.
+ *
+ * Authors:
+ * Eric Marchand
+ * Nicolas Mansard : Toward const-specification respect
+ *
+ *****************************************************************************/
+
+
+#ifndef VP_LIST_H
+#define VP_LIST_H
+
+
+/*!
+ \file vpList.h
+ \brief Definition of the list managment class
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+
+#include <stdio.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ \class vpListElement
+ \brief Each element of a list
+*/
+template <class type>
+class vpListElement
+{
+//private:
+// vpListElement(const vpListElement &)
+// : prev(NULL), next(NULL), val()
+// {
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// }
+// vpListElement &operator=(const vpListElement &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+
+public:
+ vpListElement() : prev(NULL), next(NULL), val() {};
+ vpListElement<type> *prev; //<! pointer to the previous element in the list
+ vpListElement<type> *next; //<! pointer to the next element in the list
+ type val; //<! value of the element
+} ;
+
+#endif /* DOXYGEN_SHOULD_SKIP_THIS */
+
+
+/*!
+ \class vpList
+ \brief Provide simple list management
+
+ \verbatim
+ Data structure:
+ each object vpListElement contains its own value and
+ two pointers to the next element of the list and to the previous one
+
+ A list is mainly a pointer to three of these elements
+ - one (cur) points the current element
+ - one (first) points to a virtual element located at the
+ beginning of the list
+ - one (last) points to a virtual element located at the
+ end of the list
+
+ first, last and cur are used internally and are not to be considered
+ by "basic" user.
+
+ Notation:
+ In this documentation we note such a list as
+ [*, a, b, c, *]
+ f ^ l
+ where
+ - the * are the two virtual elements (first (f) and last (l))
+ - the ^ denotes the position of the current element
+
+ \endverbatim
+*/
+
+
+
+template <class type>
+class vpList
+{
+ private:
+ void init() ;
+ public:
+
+ unsigned int nb; //<! number of items in the List
+ /*!
+ \brief the first virtual item in the list
+ \verbatim
+ [*, a, b, c, *]
+ f
+ \endverbatim
+ */
+ vpListElement<type> *first;
+ /*!
+ \brief the last virtualitem in the list
+ \verbatim
+ [*, a, b, c, *]
+ l
+ \endverbatim
+ */
+ vpListElement<type> *last;
+ /*!
+ \brief the current item in the list
+ \verbatim
+ [*, a, b, c, *]
+ ^
+ \endverbatim
+ */
+ vpListElement<type> *cur; // the current element
+ public:
+ vpList() ; // constr.
+ vpList(const vpList& l); // cloning
+ virtual ~vpList(); // destr.
+
+ inline void next(void) ; // current element's successor ( cur = cur->next )
+ inline void previous(void) ; // current element's predecessor ( cur = cur->pred )
+ inline void front(void) ; // go to the front of the List (cur = first)
+ inline void end(void) ; // go back to the end of the List ( cur = last )
+ inline bool outside(void) const; // test whether we are outside the List
+
+ bool empty(void) const; // tests whether the List is empty
+
+ inline type& value(void); // returns the current element value
+ inline const type& value(void) const; // returns the current element value
+
+ void suppress(void); // deletes the current item
+ void kill(); // empties the List
+
+ void display() ; // displays the content of the list
+ void print() {display() ;} // displays the content of the list
+
+
+ inline void addRight(const type& el); // inserts an element on the right
+ inline void addLeft(const type& el); // inserts an element on the left
+ inline void modify(const type& el); // modifies thevalue field of the curr. el.
+ inline void addRight(type& el); // inserts an element on the right
+ inline void addLeft(type& el); // inserts an element on the left
+ inline void swapLeft(); // Switch the current element with the element on the left
+ inline void swapRight(); // Switch the current element with the element on the right
+ inline unsigned int nbElement(void); // returns the number of items currently in the list
+ inline unsigned int nbElements(void); // returns the number of items currently in the list
+
+ vpList<type>& operator=(const vpList<type>& l);
+ inline void operator+=(vpList<type>& l);
+ inline void operator+=(const type& l);
+
+ // Other non fundamental member (may be somehow useful)
+ bool nextOutside(void) const; // test whether we are outside the List
+ bool previousOutside(void) const;// test whether we are outside the List
+
+
+ type& previousValue(void); // returns the previous element value
+ type& nextValue(void); // returns the next element value
+ type& firstValue(void) ;
+ type& lastValue(void) ;
+
+
+};
+
+
+/*!
+ \brief initialization, Create an empty list
+ \verbatim
+ init() --> [*, *]
+ \endverbatim */
+template<class type>
+void vpList<type>::init()
+{
+ vpListElement<type> *x=new vpListElement<type>;
+ vpListElement<type> *y=new vpListElement<type> ;
+
+ first = x ;
+ last = y ;
+
+ x->prev = NULL ;
+ x->next = y ;
+ y->prev = x ;
+ y->next =NULL ;
+
+ cur = x ;
+ nb = 0 ;
+}
+
+/*!
+ \brief Basic constructor, initialization, Create an empty list
+ \verbatim
+ init() --> [*, *]
+ \endverbatim
+ \sa init()
+ */
+template<class type>
+vpList<type>::vpList() : nb(0), first(NULL), last(NULL), cur(NULL)
+{
+ init() ;
+}
+
+
+
+/*!
+ \brief vpList destructor
+ \sa kill()
+ */
+template<class type>
+vpList<type>::~vpList()
+{
+ kill() ;
+
+ /*if (first != NULL) */ delete first ;
+ /*if (last != NULL) */ delete last ;
+}
+
+/*!
+ \brief return the number of element in the list
+ */
+template<class type>
+unsigned int vpList<type>::nbElement(void)
+{
+ return(nb) ;
+}
+
+/*!
+ \brief return the number of element in the list
+ */
+template<class type>
+unsigned int vpList<type>::nbElements(void)
+{
+ return(nb) ;
+}
+
+
+/*!
+ \brief position the current element on the next one
+ \verbatim
+ [*, a, b, c, d, *] --> next() --> [*, a, b, c, d, *]
+ ^ ^
+ \endverbatim
+*/
+template<class type>
+void vpList<type>::next(void)
+{
+ cur = cur->next ;
+}
+
+
+/*!
+ \brief position the current element on the previous one
+ \verbatim
+ [*, a, b, c, d, *] --> previous() --> [*, a, b, c, d, *]
+ ^ ^
+ \endverbatim
+*/
+template<class type>
+void vpList<type>::previous(void)
+{
+ cur = cur->prev ;
+}
+
+/*!
+ \brief return the value of the current element
+
+ \verbatim
+ [*, a, b, c, *] --> value() return b
+ ^
+ \endverbatim
+ */
+template<class type>
+type& vpList<type>::value(void)
+{
+ return(cur->val) ;
+}
+
+/*!
+ \brief return the value of the current element using a const ref.
+
+ \verbatim
+ [*, a, b, c, *] --> value() return b
+ ^
+ \endverbatim
+ */
+template<class type>
+const type& vpList<type>::value(void) const
+{
+ return(cur->val) ;
+}
+
+/*!
+ \brief return the value of the previous element
+
+ \verbatim
+ [*, a, b, c, *] --> previousValue() return a
+ ^
+ \endverbatim
+*/
+template<class type>
+type& vpList<type>::previousValue(void)
+{
+ return(cur->prev->val) ;
+}
+
+/*!
+ \brief return the value of the next element
+ \verbatim
+ [*, a, b, c, d, *] --> nextValue() return c
+ ^
+ \endverbatim
+*/
+template<class type>
+type& vpList<type>::nextValue(void)
+{
+ return(cur->next->val) ;
+}
+
+
+
+/*!
+ \brief return the first element of the list
+ \verbatim
+ [*, a, b, c, d, *] --> firstValue() return a
+ \endverbatim
+ */
+template<class type>
+type& vpList<type>::firstValue(void)
+{
+ return(first->next->val) ;
+}
+
+
+
+/*!\brief return the last element of the list
+ \verbatim
+ [*, a, b, c, d, *] --> lastValue() return d
+ \endverbatim
+ */
+template<class type>
+type& vpList<type>::lastValue(void)
+{
+ return(last->prev->val) ;
+}
+
+
+/*!
+ \brief Position the current element on the first element of the list
+
+ \verbatim
+ [*, a, b, c, d, *] --> front() --> [*, a, b, c, d, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::front(void)
+{
+ cur = first->next ;
+}
+
+/*!
+ \brief Position the current element on the last element of the list
+
+ \verbatim
+ [*, a, b, c, d, *] --> end() --> [*, a, b, c, d, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::end(void)
+{
+ cur = last->prev ;
+}
+
+/*!
+ \brief Test if the list is empty
+
+ \verbatim
+ [*, a, b, c, d, *] --> empty return false
+ [*, *] --> empty return true
+ \endverbatim
+ */
+template<class type>
+bool vpList<type>::empty(void) const
+{
+ return((first->next == last) &&( first == last->prev)) ;
+}
+
+/*!
+ \brief Test if the current element is outside the list
+ (on the virtual element)
+
+ \verbatim
+ [*, a, b, c, d, *] --> outside return false
+ ^
+ [*, a, b, c, d, *] --> outside return true
+ ^ or ^
+ \endverbatim
+ */
+template<class type>
+bool vpList<type>::outside(void) const
+{
+
+ return((cur==first)||(cur==last)) ;
+}
+
+/*!
+ \brief Test if the next element is outside the list
+ (ie if the current element is the last one)
+
+ \verbatim
+ [*, a, b, c, d, *] --> nextOutside return true
+ ^
+ \endverbatim
+ */
+template<class type>
+bool vpList<type>::nextOutside(void) const
+{
+ return((cur->next==first)||(cur->next==last)) ;
+}
+
+
+/*!
+ \brief Test if the previous element is outside the list
+ (ie if the current element is the firts one)
+
+ \verbatim
+ [*, a, b, c, d, *] --> nextOutside return true
+ ^
+ \endverbatim
+ */
+template<class type>
+bool vpList<type>::previousOutside(void) const
+{
+ return((cur->prev==first)||(cur->prev==last)) ;
+}
+
+
+/*!
+ \brief add a new element in the list, at the right of the current one
+
+ \warning the new element becomes the current one
+
+ \verbatim
+ [*, a, b, c, *] --> addRight(i) --> [*, a, b, i, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::addRight(const type& v)
+{
+ vpListElement<type> *x=new vpListElement<type>;
+
+ x->val = v ;
+ if (empty())
+ {
+ cur = first ;
+ }
+ else
+ {
+ if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
+ }
+ cur->next->prev = x ;
+ x->next = cur->next ;
+ x->prev = cur ;
+ cur->next = x ;
+ cur = x ;
+ nb++ ;
+}
+
+
+/*!
+ \brief add a new element in the list, at the left of the current one
+
+ \warning the new element becomes the current one
+
+ \verbatim
+ [*, a, b, c, *] --> addLeft(i) --> [*, a, i, b, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::addLeft(const type& v)
+{
+ vpListElement<type> *x=new vpListElement<type>;
+
+ x->val = v ;
+
+ if (empty())
+ {
+ cur = last ;
+ }
+ else
+ {
+ if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
+ }
+ x->next = cur ;
+ x->prev = cur->prev ;
+ cur->prev->next = x ;
+ cur->prev = x ;
+ cur = x ;
+ nb++ ;
+
+}
+
+/*!
+ \brief add a new element in the list, at the right of the current one
+
+ \warning the new element becomes the current one
+
+ \verbatim
+ [*, a, b, c, *] --> addRight(i) --> [*, a, b, i, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::addRight(type& v)
+{
+ vpListElement<type> *x=new vpListElement<type>;
+
+ x->val = v ;
+ if (empty())
+ {
+ cur = first ;
+ }
+ else
+ {
+ if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
+ }
+ cur->next->prev = x ;
+ x->next = cur->next ;
+ x->prev = cur ;
+ cur->next = x ;
+ cur = x ;
+ nb++ ;
+}
+
+
+/*!
+ \brief add a new element in the list, at the left of the current one
+
+ \warning the new element becomes the current one
+
+ \verbatim
+ [*, a, b, c, *] --> addLeft(i) --> [*, a, i, b, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::addLeft(type& v)
+{
+ vpListElement<type> *x=new vpListElement<type>;
+
+ x->val = v ;
+
+ if (empty())
+ {
+ cur = last ;
+ }
+ else
+ {
+ if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
+ }
+ x->next = cur ;
+ x->prev = cur->prev ;
+ cur->prev->next = x ;
+ cur->prev = x ;
+ cur = x ;
+ nb++ ;
+
+}
+
+/*!
+ \brief Modify the value of the current element
+
+ \verbatim
+ [*, a, b, c, *] --> modify(i) --> [*, a, i, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::modify(const type& v)
+{
+ cur->val = v ;
+}
+
+/*!
+ \brief Switch the current element with the element on the left
+
+ \verbatim
+ [*, a, b, c, *] --> swapLeft --> [*, b, a, c, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::swapLeft()
+{
+ if (cur->prev != first)
+ {
+ cur->prev->prev->next = cur;
+ cur->next->prev = cur->prev;
+
+ vpListElement<type> *nextTmp;
+ vpListElement<type> *prevTmp;
+
+ nextTmp = cur->next;
+ prevTmp = cur->prev;
+
+ cur->next = cur->prev;
+ cur->prev = cur->prev->prev;
+
+ prevTmp->prev = cur;
+ prevTmp->next = nextTmp;
+ }
+ else
+ {
+ std::cout << "vpList: previous element is outside (swapLeft) " << std::endl ;
+ }
+}
+
+/*!
+ \brief Switch the current element with the element on the right
+
+ \verbatim
+ [*, a, b, c, *] --> swapRight --> [*, a, c, b, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::swapRight()
+{
+ if (cur->next != last)
+ {
+ cur->prev->next = cur->next;
+ cur->next->next->prev = cur;
+
+ vpListElement<type> *nextTmp;
+ vpListElement<type> *prevTmp;
+
+ nextTmp = cur->next;
+ prevTmp = cur->prev;
+
+ cur->next = nextTmp->next;
+ cur->prev = nextTmp;
+
+ nextTmp->prev = prevTmp;
+ nextTmp->next = cur;
+ }
+ else
+ {
+ std::cout << "vpList: next element is outside (swapRight) " << std::endl ;
+ }
+}
+
+/*!
+ \brief Destroy the list
+
+ \verbatim
+ [*, a, b, c, *] --> kill --> [*, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::kill()
+{
+
+ front() ;
+ while (!empty())
+ {
+ suppress() ;
+ }
+
+}
+
+
+
+/*!
+ \brief suppress the current element
+
+ \warning new current element is on the next one
+
+ \verbatim
+ [*, a, b, c, d, *] --> suppress --> [*, a, c, d, *]
+ ^ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::suppress(void)
+{
+ vpListElement<type> *x ;
+
+ cur->prev->next = cur->next ;
+ cur->next->prev = cur->prev ;
+ x = cur ;
+ cur = cur->next ;
+
+ if (x!=NULL) delete x ;
+
+ nb-- ;
+
+
+}
+
+
+
+
+/*!
+ \brief Copy constructor const
+
+ \param l : the list to copy
+ */
+
+template<class type>
+vpList<type>& vpList<type>::operator=(const vpList<type>& l)
+{
+ type x ;
+ vpListElement<type> *e ;
+
+ kill() ;
+ e = l.first->next ;
+ front() ;
+ while (e!=l.last)
+ {
+ x = e->val ;
+ addRight(x) ;
+ e = e->next ;
+ }
+
+ nb = l.nb ;
+ cur = first->next ;
+
+ return *this;
+}
+
+/*!
+ \brief Append two lists
+
+ \verbatim
+ [*, a, b, *] += [*, c, d, *] --> [*, a, b, c, d, *]
+ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::operator+=(vpList<type>& l)
+{
+ type x ;
+
+ l.front() ;
+ end() ;
+ while (!l.outside())
+ {
+ x = l.value() ;
+ addRight(x) ;
+ l.next() ;
+ }
+}
+
+/*!
+ \brief Append an element to a list
+
+ \verbatim
+ [*, a, b, *] += c --> [*, a, b, c, *]
+ ^
+ \endverbatim
+ */
+template<class type>
+void vpList<type>::operator += (const type& l)
+{
+ end() ;
+ addRight(l) ;
+}
+
+
+/*!
+ \brief copy constructor
+
+ \param l : the list to copy
+*/
+template<class type>
+vpList<type>::vpList(const vpList<type>& l)
+ : nb(0), first(NULL), last(NULL), cur(NULL)
+{
+ init() ;
+ *this = l;
+}
+
+/*!
+ \brief Print (std::cout) all the element of the list
+ */
+template<class type>
+void vpList<type>::display()
+{
+ unsigned int k = 1 ;
+ front() ;
+ while(!outside()) {
+ std::cout<<k<<" ---> "<<value()<<std::endl ;
+ next() ;
+ k++ ;
+ }
+ std::cout<< std::endl << std::endl ;
+}
+
+#endif /* #ifndef VP_LIST_H */
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h
new file mode 100644
index 0000000..1de5277
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMath.h
@@ -0,0 +1,472 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simple mathematical function not available in the C math library (math.h).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMath.h
+ \brief Provides simple Math computation that are not available in
+ the C mathematics library (math.h)
+*/
+
+
+
+#ifndef vpMATH_HH
+#define vpMATH_HH
+
+#include <visp3/core/vpConfig.h>
+
+#include <math.h>
+#include <limits>
+#include <climits>
+#include <algorithm>
+#include <vector>
+
+#if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
+# include <cmath>
+#endif
+
+#if defined(_WIN32) // Not defined in Microsoft math.h
+
+# ifndef M_PI
+# define M_PI 3.14159265358979323846
+# endif
+
+# ifndef M_PI_2
+# define M_PI_2 (M_PI/2.0)
+# endif
+
+# ifndef M_PI_4
+# define M_PI_4 (M_PI/4.0)
+# endif
+
+#endif
+
+
+/*!
+ \class vpMath
+ \ingroup group_core_math_tools
+ \brief Provides simple mathematics computation tools that are not
+ available in the C mathematics library (math.h)
+
+ \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
+*/
+
+class VISP_EXPORT vpMath
+{
+ public:
+
+ /*!
+ Convert an angle in radians into degrees.
+
+ \param rad : Angle in radians.
+ \return Angle converted in degrees.
+ */
+ static inline double deg(double rad) { return (rad*180.0)/M_PI ; }
+
+ /*!
+ Convert an angle in degrees into radian.
+ \param deg : Angle in degrees.
+ \return Angle converted in radian.
+ */
+ static inline double rad(double deg) { return (deg*M_PI)/180.0 ; }
+
+ /*!
+ Compute x square value.
+ \return \f$ x^2 \f$.
+ */
+ static inline double sqr(double x) { return x*x ; }
+
+ // factorial of x
+ static inline double fact(unsigned int x) ;
+
+ // combinaison
+ static inline long double comb(unsigned int n, unsigned int p) ;
+
+ // round x to the nearest integer
+ static inline int round(const double x) ;
+
+ // return the sign of x (+-1)
+ static inline int sign(double x) ;
+
+
+ // test if a number equals 0 (with threshold value)
+ static inline bool nul(double x, double s=0.001);
+
+ // test if two numbers are equals (with a user defined threshold)
+ static inline bool equal(double x, double y, double s=0.001);
+
+ // test if a number is greater than another (with a user defined threshold)
+ static inline bool greater(double x, double y, double s=0.001);
+
+
+ /*!
+ Find the maximum between two numbers (or other).
+ \param a : First number.
+ \param b : Second number.
+ \return The maximum of the two numbers.
+ */
+ template <class Type> static Type maximum(const Type& a, const Type& b)
+ {
+ return (a > b) ? a : b;
+ }
+
+ /*!
+ Find the minimum between two numbers (or other).
+ \param a : First number.
+ \param b : Second number.
+ \return The minimum of the two numbers.
+ */
+ template <class Type> static Type minimum(const Type& a, const Type& b)
+ {
+ return (a < b) ? a : b;
+ }
+
+ /*!
+ Find the absolute value of a number (or other).
+ \param x : The number.
+ \return The absolute value of x
+ */
+ template <class Type> static Type abs(const Type& x)
+ {
+ return (x < 0) ? -x : x;
+ }
+
+
+ // sinus cardinal
+ static double sinc(double x) ;
+ static double sinc(double sinx, double x) ;
+ static double mcosc(double cosx, double x) ;
+ static double msinc(double sinx, double x) ;
+
+ // sigmoid
+ static inline double sigmoid(double x, double x0=0.,double x1=1., double n=12.);
+
+ /*!
+ Exchange two numbers.
+
+ \param a First number to exchange.
+ \param b Second number to exchange
+ */
+ template <class Type> static void swap(Type& a, Type& b)
+ {
+ Type tmp = b;
+ b = a;
+ a = tmp;
+ }
+
+ static bool isNaN(const double value);
+ static bool isInf(const double value);
+
+ template<typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
+ template<typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
+
+ static double getMean(const std::vector<double> &v);
+ static double getMedian(const std::vector<double> &v);
+ static double getStdev(const std::vector<double> &v, const bool useBesselCorrection=false);
+
+ private:
+ static const double ang_min_sinc;
+ static const double ang_min_mc;
+};
+
+
+
+//Begining of the inline functions definition
+
+/*!
+ Computes and returns x!
+ \param x : parameter of factorial function.
+*/
+double vpMath::fact(unsigned int x)
+{
+ if ( (x == 1) || (x == 0)) return 1;
+ return x * fact(x-1);
+}
+
+/*!
+ Computes the number of combination of p elements inside n elements.
+
+ \param n : total number of elements.
+ \param p : requested number of elements.
+
+ \return \f$ n! / ((n-p)! p!) \f$
+*/
+long double vpMath::comb(unsigned int n, unsigned int p)
+{
+ if (n == p) return 1;
+ return fact(n)/ (fact(n-p) * fact(p));
+}
+
+
+/*!
+ Round x to the nearest integer.
+
+ \param x : Value to round.
+
+ \return Nearest integer of x.
+
+*/
+int vpMath::round(const double x)
+{
+#if defined(VISP_HAVE_FUNC_ROUND)
+ //:: to design the global namespace and avoid to call recursively vpMath::round
+ return (int)::round(x);
+#elif defined(VISP_HAVE_FUNC_STD_ROUND)
+ return (int)std::round(x)
+#else
+ return (x > 0.0) ? ((int) floor(x + 0.5)) : ((int) ceil(x - 0.5));
+#endif
+}
+
+/*!
+ Return the sign of x.
+
+ \return -1 if x is negative, +1 if positive and 0 if zero.
+
+*/
+int vpMath::sign(double x)
+{
+ if (fabs(x) < std::numeric_limits<double>::epsilon())
+ return 0;
+ else {
+ if (x < 0)
+ return -1;
+ else
+ return 1;
+ }
+}
+
+/*!
+ Compares \f$ | x | \f$ to \f$ s \f$.
+ \param x : Value to test.
+ \param s : Tolerance threshold
+ \return true if \f$ | x | < s \f$.
+
+*/
+bool vpMath::nul(double x, double s)
+{
+ return(fabs(x)<s);
+}
+
+/*!
+ Compares \f$ | x - y | \f$ to \f$ s \f$.
+ \param x : x value.
+ \param y : y value.
+ \param s : Tolerance threshold.
+ \return true if \f$ | x - y | < s \f$.
+*/
+bool vpMath::equal(double x, double y, double s)
+{
+ return( nul(x-y, s) );
+}
+
+/*!
+ Compares \f$ x \f$ to \f$ y - s \f$.
+ \param x : x value.
+ \param y : y value.
+ \param s : Tolerance threshold.
+ \return true if \f$ x > y - s \f$.
+*/
+bool vpMath::greater(double x, double y, double s)
+{
+ return(x>(y-s));
+}
+
+/*!
+
+ Sigmoid function between [x0,x1] with \f$ s(x)=0 if x\le x0\f$ and \f$ s(x)=1 if x \ge x1 \f$
+ \param x : Value of x.
+ \param x0 : Lower bound (default 0).
+ \param x1 : Upper bound (default 1).
+ \param n : Degree of the exponential (default 12).
+
+\return \f$1/(1+exp(-n*((x-x0)/(x1-x0)-0.5)))\f$
+ */
+double vpMath::sigmoid(double x, double x0,double x1, double n)
+{
+ if(x < x0)
+ return 0.;
+ else if(x > x1)
+ return 1.;
+ double l0 = 1./(1.+exp(0.5*n));
+ double l1 = 1./(1.+exp(-0.5*n));
+ return (1./(1.+exp(-n*((x-x0)/(x1-x0)-0.5)))-l0)/(l1-l0);
+}
+
+//unsigned char
+template<> inline unsigned char vpMath::saturate<unsigned char>(char v) {
+ return (unsigned char) (std::max)((int) v, 0);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v) {
+ return (unsigned char) (std::min)((unsigned int) v, (unsigned int) UCHAR_MAX);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(int v) {
+ return (unsigned char) ((unsigned int) v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(short v) {
+ return saturate<unsigned char> ((int) v);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v) {
+ return (unsigned char) (std::min)(v, (unsigned int) UCHAR_MAX);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(float v) {
+ int iv = vpMath::round(v);
+ return saturate<unsigned char> (iv);
+}
+
+template<> inline unsigned char vpMath::saturate<unsigned char>(double v) {
+ int iv = vpMath::round(v);
+ return saturate<unsigned char> (iv);
+}
+
+//char
+template<> inline char vpMath::saturate<char>(unsigned char v) {
+ return (char) (std::min)((int) v, SCHAR_MAX);
+}
+
+template<> inline char vpMath::saturate<char>(unsigned short v) {
+ return (char) (std::min)((unsigned int) v, (unsigned int) SCHAR_MAX);
+}
+
+template<> inline char vpMath::saturate<char>(int v) {
+ return (char) ((unsigned int) (v - SCHAR_MIN) <= (unsigned int) UCHAR_MAX ? v :
+ v > 0 ? SCHAR_MAX : SCHAR_MIN);
+}
+
+template<> inline char vpMath::saturate<char>(short v) {
+ return saturate<char>((int) v);
+}
+
+template<> inline char vpMath::saturate<char>(unsigned int v) {
+ return (char) (std::min)(v, (unsigned int) SCHAR_MAX);
+}
+
+template<> inline char vpMath::saturate<char>(float v) {
+ int iv = vpMath::round(v);
+ return saturate<char>(iv);
+}
+
+template<> inline char vpMath::saturate<char>(double v) {
+ int iv = vpMath::round(v);
+ return saturate<char>(iv);
+}
+
+//unsigned short
+template<> inline unsigned short vpMath::saturate<unsigned short>(char v) {
+ return (unsigned short) (std::max)((int) v, 0);
+}
+
+template<> inline unsigned short vpMath::saturate<unsigned short>(short v) {
+ return (unsigned short) (std::max)((int) v, 0);
+}
+
+template<> inline unsigned short vpMath::saturate<unsigned short>(int v) {
+ return (unsigned short) ((unsigned int) v <= (unsigned int) USHRT_MAX ? v :
+ v > 0 ? USHRT_MAX : 0);
+}
+
+template<> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v) {
+ return (unsigned short) (std::min)(v, (unsigned int) USHRT_MAX);
+}
+
+template<> inline unsigned short vpMath::saturate<unsigned short>(float v) {
+ int iv = vpMath::round(v);
+ return vpMath::saturate<unsigned short>(iv);
+}
+
+template<> inline unsigned short vpMath::saturate<unsigned short>(double v) {
+ int iv = vpMath::round(v);
+ return vpMath::saturate<unsigned short>(iv);
+}
+
+//short
+template<> inline short vpMath::saturate<short>(unsigned short v) {
+ return (short) (std::min)((int) v, SHRT_MAX);
+}
+template<> inline short vpMath::saturate<short>(int v) {
+ return (short) ((unsigned int) (v - SHRT_MIN) <= (unsigned int) USHRT_MAX ? v :
+ v > 0 ? SHRT_MAX : SHRT_MIN);
+}
+template<> inline short vpMath::saturate<short>(unsigned int v) {
+ return (short) (std::min)(v, (unsigned int) SHRT_MAX);
+}
+template<> inline short vpMath::saturate<short>(float v) {
+ int iv = vpMath::round(v);
+ return vpMath::saturate<short>(iv);
+}
+template<> inline short vpMath::saturate<short>(double v) {
+ int iv = vpMath::round(v);
+ return vpMath::saturate<short>(iv);
+}
+
+//int
+template<> inline int vpMath::saturate<int>(float v) {
+ return vpMath::round(v);
+}
+
+template<> inline int vpMath::saturate<int>(double v) {
+ return vpMath::round(v);
+}
+
+//unsigned int
+// (Comment from OpenCV) we intentionally do not clip negative numbers, to make -1 become 0xffffffff etc.
+template<> inline unsigned int vpMath::saturate<unsigned int>(float v) {
+ return (unsigned int) vpMath::round(v);
+}
+
+template<> inline unsigned int vpMath::saturate<unsigned int>(double v) {
+ return (unsigned int) vpMath::round(v);
+}
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h
new file mode 100644
index 0000000..a35b1e0
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMatrix.h
@@ -0,0 +1,633 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix manipulation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpMatrix_H
+#define vpMatrix_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpForceTwistMatrix.h>
+
+#ifdef VISP_HAVE_GSL
+# include <gsl/gsl_math.h>
+# include <gsl/gsl_eigen.h>
+#endif
+
+#include <iostream>
+#include <math.h>
+
+class vpRowVector;
+class vpColVector;
+class vpTranslationVector;
+class vpHomogeneousMatrix;
+class vpVelocityTwistMatrix;
+class vpForceTwistMatrix;
+
+/*!
+ \file vpMatrix.h
+
+ \brief Definition of matrix class as well as a set of operations on
+ these matrices.
+*/
+
+
+
+/*!
+ \class vpMatrix
+ \ingroup group_core_matrices
+
+ \brief Implementation of a matrix and operations on matrices.
+
+ vpMatrix class provides a data structure for the matrices as well
+ as a set of operations on these matrices.
+
+ The vpMatrix class is derived from vpArray2D<double>.
+
+ \warning Note the matrix in the class (*this) will be noted A in the comment
+
+ \ingroup libmath
+
+ \sa vpRowVector, vpColVector, vpHomogeneousMatrix, vpRotationMatrix,
+ vpTwistMatrix, vpHomography
+*/
+class VISP_EXPORT vpMatrix : public vpArray2D<double>
+{
+ public:
+ /*!
+ Method used to compute the determinant of a square matrix.
+ \sa det()
+ */
+ typedef enum {
+ LU_DECOMPOSITION /*!< LU decomposition method. */
+ } vpDetMethod;
+
+ public:
+ /*!
+ Basic constructor of a matrix of double. Number of columns and rows are zero.
+ */
+ vpMatrix() : vpArray2D<double>(0, 0) {};
+ /*!
+ Constructor that initialize a matrix of double with 0.
+
+ \param r : Matrix number of rows.
+ \param c : Matrix number of columns.
+ */
+ vpMatrix(unsigned int r, unsigned int c) : vpArray2D<double>(r, c) {};
+ /*!
+ Constructor that initialize a matrix of double with \e val.
+
+ \param r : Matrix number of rows.
+ \param c : Matrix number of columns.
+ \param val : Each element of the matrix is set to \e val.
+ */
+ vpMatrix(unsigned int r, unsigned int c, double val) : vpArray2D<double>(r, c, val) {};
+ vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c,
+ unsigned int nrows, unsigned int ncols) ;
+ /*!
+ Create a matrix from a 2D array that could be one of the following container that
+ inherit from vpArray2D such as vpMatrix, vpRotationMatrix, vpHomogeneousMatrix,
+ vpPoseVector, vpColVector, vpRowVector...
+
+ The following example shows how to create a matrix from an homogeneous matrix:
+ \code
+ vpRotationMatrix R;
+ vpMatrix M(R);
+ \endcode
+ */
+ vpMatrix(const vpArray2D<double>& A) : vpArray2D<double>(A) {};
+
+ //! Destructor (Memory de-allocation)
+ virtual ~vpMatrix() {};
+
+ /*!
+ Removes all elements from the matrix (which are destroyed),
+ leaving the container with a size of 0.
+ */
+ void clear()
+ {
+ if (data != NULL ) {
+ free(data);
+ data=NULL;
+ }
+
+ if (rowPtrs!=NULL) {
+ free(rowPtrs);
+ rowPtrs=NULL ;
+ }
+ rowNum = colNum = dsize = 0;
+ }
+
+ //-------------------------------------------------
+ // Setting a diagonal matrix
+ //-------------------------------------------------
+ /** @name Setting a diagonal matrix */
+ //@{
+ void diag(const double &val = 1.0);
+ void diag(const vpColVector &A);
+ // Initialize an identity matrix n-by-n
+ void eye();
+ void eye(unsigned int n) ;
+ // Initialize an identity matrix m-by-n
+ void eye(unsigned int m, unsigned int n) ;
+ //@}
+
+ //---------------------------------
+ // Assignment
+ //---------------------------------
+ /** @name Assignment operators */
+ //@{
+ vpMatrix &operator<<(double*);
+ vpMatrix &operator=(const vpArray2D<double> &A);
+ vpMatrix &operator=(const double x);
+ //@}
+
+ //-------------------------------------------------
+ // Stacking
+ //-------------------------------------------------
+ /** @name Stacking */
+ //@{
+ // Stack the matrix A below the current one, copy if not initialized this = [ this A ]^T
+ void stack(const vpMatrix &A);
+ void stack(const vpRowVector &r);
+ // Stacks columns of a matrix in a vector
+ void stackColumns(vpColVector &out );
+
+ // Stacks columns of a matrix in a vector
+ vpColVector stackColumns();
+
+ // Stacks columns of a matrix in a vector
+ void stackRows(vpRowVector &out );
+
+ // Stacks columns of a matrix in a vector
+ vpRowVector stackRows();
+ //@}
+
+ //---------------------------------
+ // Matrix insertion with Static Public Member Functions
+ //---------------------------------
+ /** @name Matrix insertion */
+ //@{
+ // Insert matrix A in the current matrix at the given position (r, c).
+ void insert(const vpMatrix&A, const unsigned int r, const unsigned int c);
+ //@}
+
+ //-------------------------------------------------
+ // Columns, Rows extraction, SubMatrix
+ //-------------------------------------------------
+ /** @name Columns, rows, sub-matrices extraction */
+ //@{
+ vpRowVector getRow(const unsigned int i) const;
+ vpRowVector getRow(const unsigned int i, const unsigned int j_begin, const unsigned int size) const;
+ vpColVector getCol(const unsigned int j) const;
+ vpColVector getCol(const unsigned int j, const unsigned int i_begin, const unsigned int size) const;
+ void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols);
+ //@}
+
+ //---------------------------------
+ // Matrix operations.
+ //---------------------------------
+ /** @name Matrix operations */
+ //@{
+ // operation A = A + B
+ vpMatrix &operator+=(const vpMatrix &B);
+ // operation A = A - B
+ vpMatrix &operator-=(const vpMatrix &B);
+ vpMatrix operator*(const vpMatrix &B) const;
+ vpMatrix operator*(const vpRotationMatrix &R) const;
+ vpMatrix operator*(const vpVelocityTwistMatrix &V) const;
+ vpMatrix operator*(const vpForceTwistMatrix &V) const;
+ // operation t_out = A * t (A is unchanged, t and t_out are translation vectors)
+ vpTranslationVector operator*(const vpTranslationVector &tv) const;
+ vpColVector operator*(const vpColVector &v) const;
+ vpMatrix operator+(const vpMatrix &B) const;
+ vpMatrix operator-(const vpMatrix &B) const;
+ vpMatrix operator-() const;
+
+ //! Add x to all the element of the matrix : Aij = Aij + x
+ vpMatrix &operator+=(const double x);
+ //! Substract x to all the element of the matrix : Aij = Aij - x
+ vpMatrix &operator-=(const double x);
+ //! Multiply all the element of the matrix by x : Aij = Aij * x
+ vpMatrix &operator*=(const double x);
+ //! Divide all the element of the matrix by x : Aij = Aij / x
+ vpMatrix &operator/=(double x);
+
+ // Cij = Aij * x (A is unchanged)
+ vpMatrix operator*(const double x) const;
+ // Cij = Aij / x (A is unchanged)
+ vpMatrix operator/(const double x) const;
+
+ /*!
+ Return the sum of all the \f$a_{ij}\f$ elements of the matrix.
+
+ \return \f$\sum a_{ij}\f$
+ */
+ double sum() const;
+ double sumSquare() const;
+ // return the determinant of the matrix.
+ double det(vpDetMethod method = LU_DECOMPOSITION) const;
+
+ //Compute the exponential matrix of a square matrix
+ vpMatrix expm() const;
+
+ //-------------------------------------------------
+ // Kronecker product
+ //-------------------------------------------------
+ /** @name Kronecker product */
+ //@{
+ // Compute Kronecker product matrix
+ void kron(const vpMatrix &m1, vpMatrix &out) const;
+
+ // Compute Kronecker product matrix
+ vpMatrix kron(const vpMatrix &m1) const;
+ //@}
+
+ //-------------------------------------------------
+ // Transpose
+ //-------------------------------------------------
+ /** @name Transpose */
+ //@{
+ // Compute the transpose C = A^T
+ vpMatrix t() const;
+
+ // Compute the transpose C = A^T
+ vpMatrix transpose()const;
+ void transpose(vpMatrix & C )const;
+
+ vpMatrix AAt() const;
+ void AAt(vpMatrix &B) const;
+
+ vpMatrix AtA() const;
+ void AtA(vpMatrix &B) const;
+ //@}
+
+ //-------------------------------------------------
+ // Matrix inversion
+ //-------------------------------------------------
+ /** @name Matrix inversion */
+ //@{
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ //! LU Decomposition
+ void LUDcmp(unsigned int* perm, int& d);
+ //! solve AX = B using the LU Decomposition
+ void LUBksb(unsigned int* perm, vpColVector& b);
+
+#endif // doxygen should skip this
+ // inverse matrix A using the LU decomposition
+ vpMatrix inverseByLU() const;
+#if defined(VISP_HAVE_LAPACK_C)
+ // inverse matrix A using the Cholesky decomposition (only for real symmetric matrices)
+ vpMatrix inverseByCholesky() const;
+ //lapack implementation of inverse by Cholesky
+ vpMatrix inverseByCholeskyLapack() const;
+ // inverse matrix A using the QR decomposition
+ vpMatrix inverseByQR() const;
+ //lapack implementation of inverse by QR
+ vpMatrix inverseByQRLapack() const;
+#endif
+ //! Compute the pseudo inverse of the matrix using the SVD.
+ vpMatrix pseudoInverse(double svThreshold=1e-6) const;
+ //! Compute the pseudo inverse of the matrix using the SVD.
+ //! return the rank
+ unsigned int pseudoInverse(vpMatrix &Ap, double svThreshold=1e-6) const;
+ //! Compute the pseudo inverse of the matrix using the SVD.
+ //! return the rank and the singular value
+ unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold=1e-6) const ;
+ //! Compute the pseudo inverse of the matrix using the SVD.
+ //! return the rank and the singular value, image
+ unsigned int pseudoInverse(vpMatrix &Ap,
+ vpColVector &sv, double svThreshold,
+ vpMatrix &ImA,
+ vpMatrix &ImAt) const ;
+ //! Compute the pseudo inverse of the matrix using the SVD.
+ //! return the rank and the singular value, image, kernel.
+ unsigned int pseudoInverse(vpMatrix &Ap,
+ vpColVector &sv, double svThreshold,
+ vpMatrix &ImA,
+ vpMatrix &ImAt,
+ vpMatrix &kerA) const ;
+ //@}
+
+ //-------------------------------------------------
+ // SVD decomposition
+ //-------------------------------------------------
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void svdFlake(vpColVector& w, vpMatrix& v);
+ void svdNr(vpColVector& w, vpMatrix& v);
+#ifdef VISP_HAVE_GSL
+ void svdGsl(vpColVector& w, vpMatrix& v);
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ void svdOpenCV(vpColVector& w, vpMatrix& v);
+#endif
+#ifdef VISP_HAVE_LAPACK_C
+ void svdLapack(vpColVector& W, vpMatrix& V);
+#endif
+ //! solve AX=B using the SVD decomposition
+ void SVBksb(const vpColVector& w,
+ const vpMatrix& v,
+ const vpColVector& b, vpColVector& x);
+#endif
+
+ /** @name SVD decomposition */
+ //@{
+ // singular value decomposition SVD
+
+ void svd(vpColVector& w, vpMatrix& v);
+
+ // solve Ax=B using the SVD decomposition (usage A = solveBySVD(B,x) )
+ void solveBySVD(const vpColVector &B, vpColVector &x) const ;
+ // solve Ax=B using the SVD decomposition (usage x=A.solveBySVD(B))
+ vpColVector solveBySVD(const vpColVector &B) const ;
+
+ unsigned int kernel(vpMatrix &KerA, double svThreshold=1e-6) const;
+ double cond() const;
+ //@}
+
+ //-------------------------------------------------
+ // Eigen values and vectors
+ //-------------------------------------------------
+ /** @name Eigen values */
+
+ //@{
+ // compute the eigen values using the Gnu Scientific library
+ vpColVector eigenValues() const;
+ void eigenValues(vpColVector &evalue, vpMatrix &evector) const;
+ //@}
+
+ //-------------------------------------------------
+ // Norms
+ //-------------------------------------------------
+ /** @name Norms */
+ //@{
+ double euclideanNorm() const;
+ double infinityNorm() const;
+ //@}
+
+ //---------------------------------
+ // Printing
+ //---------------------------------
+ /** @name Printing */
+ //@{
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+ std::ostream & matlabPrint(std::ostream & os) const;
+ std::ostream & maplePrint(std::ostream & os) const;
+ std::ostream & csvPrint(std::ostream & os) const;
+ std::ostream & cppPrint(std::ostream & os, const char * matrixName = NULL, bool octet = false) const;
+ void printSize() const { std::cout << getRows() <<" x " << getCols() <<" " ; }
+ //@}
+
+ //------------------------------------------------------------------
+ // Static functionalities
+ //------------------------------------------------------------------
+
+ //---------------------------------
+ // Setting a diagonal matrix with Static Public Member Functions
+ //---------------------------------
+ /** @name Setting a diagonal matrix with Static Public Member Functions */
+ //@{
+ // Create a diagonal matrix with the element of a vector DAii = Ai
+ static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA) ;
+ //@}
+
+ //---------------------------------
+ // Matrix insertion with Static Public Member Functions
+ //---------------------------------
+ /** @name Matrix insertion with Static Public Member Functions */
+ //@{
+ // Insert matrix B in matrix A at the given position (r, c).
+ static vpMatrix insert(const vpMatrix &A,const vpMatrix &B, const unsigned int r, const unsigned int c) ;
+ // Insert matrix B in matrix A (not modified) at the given position (r, c), the result is given in matrix C.
+ static void insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c) ;
+
+ //---------------------------------
+ // Stacking with Static Public Member Functions
+ //---------------------------------
+ /** @name Stacking with Static Public Member Functions */
+ //@{
+ // Juxtapose to matrices C = [ A B ]
+ static vpMatrix juxtaposeMatrices(const vpMatrix &A,const vpMatrix &B) ;
+ // Juxtapose to matrices C = [ A B ]
+ static void juxtaposeMatrices(const vpMatrix &A,const vpMatrix &B, vpMatrix &C) ;
+ // Stack two matrices C = [ A B ]^T
+ static vpMatrix stack(const vpMatrix &A, const vpMatrix &B) ;
+ static vpMatrix stack(const vpMatrix &A, const vpRowVector &r) ;
+
+ // Stack two matrices C = [ A B ]^T
+ static void stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
+ static void stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C);
+ //@}
+
+ //---------------------------------
+ // Matrix operations Static Public Member Functions
+ //---------------------------------
+ /** @name Matrix operations with Static Public Member Functions */
+ //@{
+ static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
+ static void add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C);
+ static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C);
+ static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM);
+ static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
+ static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMatrix &C);
+ static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneousMatrix &C);
+ static void mult2Matrices(const vpMatrix &A, const vpColVector &B, vpColVector &C);
+ static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w);
+ static void negateMatrix(const vpMatrix &A, vpMatrix &C);
+ static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
+ static void sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C);
+ //@}
+
+ //---------------------------------
+ // Kronecker product Static Public Member Functions
+ //---------------------------------
+ /** @name Kronecker product with Static Public Member Functions */
+ //@{
+ // Compute Kronecker product matrix
+ static void kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out);
+
+ // Compute Kronecker product matrix
+ static vpMatrix kron(const vpMatrix &m1, const vpMatrix &m2 );
+ //@}
+
+ //---------------------------------
+ // Covariance computation Static Public Member Functions
+ //---------------------------------
+ /** @name Covariance computation with Static Public Member Functions */
+ //@{
+ static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b);
+ static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &w);
+ static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W);
+ static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls);
+ //@}
+
+ //---------------------------------
+ // Matrix I/O Static Public Member Functions
+ //---------------------------------
+ /** @name Matrix I/O with Static Public Member Functions */
+ //@{
+ /*!
+ Load a matrix from a file. This function overloads vpArray2D::load().
+
+ \param filename : absolute file name.
+ \param M : matrix to be loaded.
+ \param binary :If true the matrix is loaded from a binary file, else from a text file.
+ \param header : Header of the file is loaded in this parameter
+
+ \return Returns true if no problem appends.
+ */
+ static inline bool loadMatrix(const std::string &filename, vpArray2D<double> &M,
+ const bool binary = false, char *header = NULL)
+ {
+ return vpArray2D<double>::load(filename, M, binary, header);
+ }
+
+ /*!
+ Load a matrix from a YAML-formatted file. This function overloads vpArray2D::loadYAML().
+
+ \param filename : absolute file name.
+ \param M : matrix to be loaded from the file.
+ \param header : Header of the file is loaded in this parameter.
+
+ \return Returns true if no problem appends.
+ */
+ static inline bool loadMatrixYAML(const std::string &filename, vpArray2D<double> &M, char *header = NULL)
+ {
+ return vpArray2D<double>::loadYAML(filename, M, header);
+ }
+
+ /*!
+ Save a matrix to a file. This function overloads vpArray2D::load().
+
+ \param filename : absolute file name.
+ \param M : matrix to be saved.
+ \param binary : If true the matrix is save in a binary file, else a text file.
+ \param header : optional line that will be saved at the beginning of the file.
+
+ \return Returns true if no problem appends.
+
+ Warning : If you save the matrix as in a text file the precision is less than if you save it in a binary file.
+ */
+ static inline bool saveMatrix(const std::string &filename, const vpArray2D<double> &M,
+ const bool binary = false,
+ const char *header = "")
+ {
+ return vpArray2D<double>::save(filename, M, binary, header);
+ }
+
+ /*!
+ Save a matrix in a YAML-formatted file. This function overloads vpArray2D::saveYAML().
+
+ \param filename : absolute file name.
+ \param M : matrix to be saved in the file.
+ \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
+
+ \return Returns true if success.
+
+ */
+ static inline bool saveMatrixYAML(const std::string &filename, const vpArray2D<double> &M, const char *header = "")
+ {
+ return vpArray2D<double>::saveYAML(filename, M, header);
+ }
+ //@}
+
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Only provided for compatibilty with ViSP previous releases. This function does nothing.
+ */
+ vp_deprecated void init() { };
+ /*!
+ \deprecated You should rather use stack(const vpMatrix &A)
+ */
+ vp_deprecated void stackMatrices(const vpMatrix &A) { stack(A); };
+ /*!
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix &B)
+ */
+ vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B) { return vpMatrix::stack(A, B); };
+ /*!
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+ */
+ vp_deprecated static void stackMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { vpMatrix::stack(A, B, C); };
+ /*!
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix &B)
+ */
+ vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, const vpRowVector &B);
+ /*!
+ \deprecated You should rather use stack(const vpMatrix &A, const vpRowVector &B, vpMatrix &C)
+ */
+ vp_deprecated static void stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C);
+ /*!
+ \deprecated You should rather use vpColVector::stack(const vpColVector &A, const vpColVector &B)
+ */
+ vp_deprecated static vpMatrix stackMatrices(const vpColVector &A, const vpColVector &B);
+ /*!
+ \deprecated You should rather use vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
+ */
+ vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C);
+
+ /*!
+ \deprecated You should rather use diag(const double &)
+ */
+ vp_deprecated void setIdentity(const double & val=1.0) ;
+ //@}
+#endif
+
+ private:
+ double detByLU() const;
+ static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, vpMatrix &Js, vpColVector &deltaP);
+
+};
+
+
+//////////////////////////////////////////////////////////////////////////
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+VISP_EXPORT
+#endif
+vpMatrix operator*(const double &x, const vpMatrix &A) ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMatrixException.h b/modules/core/include/visp3/core/vpMatrixException.h
new file mode 100644
index 0000000..91698e7
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMatrixException.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpMatrix class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef __vpMatrixException_H
+#define __vpMatrixException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* Classes standards. */
+//
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpMatrixException
+ \ingroup group_core_debug
+ \brief error that can be emited by the vpMatrix class and its derivates
+ */
+class VISP_EXPORT vpMatrixException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpMatrix member
+ */
+ enum errorCodeEnum
+ {
+ //! error returns by a constructor
+ constructionError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedError,
+ //! index out of range
+ outOfRangeError,
+ //! iterative algorithm doesn't converge (ex SVD)
+ convergencyError,
+ incorrectMatrixSizeError,
+ forbiddenOperatorError,
+ subMatrixError,
+ matrixError,
+ rankDeficient
+ } ;
+
+ public:
+ vpMatrixException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpMatrixException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpMatrixException (const 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
new file mode 100644
index 0000000..7b51e2a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMeterPixelConversion.h
@@ -0,0 +1,243 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Meter to pixel conversion.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+#ifndef vpMeterPixelConversion_H
+#define vpMeterPixelConversion_H
+
+
+/*!
+ \file vpMeterPixelConversion.h
+ \brief meter to pixel conversion
+
+*/
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+
+/*!
+ \class vpMeterPixelConversion
+
+ \ingroup group_core_camera
+
+ \brief Conversion from normalized coordinates \f$(x,y)\f$ in meter
+ to pixel coordinates \f$(u,v)\f$.
+
+ This class relates to vpCameraParameters.
+
+*/
+class VISP_EXPORT vpMeterPixelConversion
+{
+public:
+ static void convertEllipse(const vpCameraParameters &cam,
+ const vpCircle &circle, vpImagePoint ¢er,
+ double &mu20_p, double &mu11_p, double &mu02_p);
+
+ static void convertLine(const vpCameraParameters &cam,
+ const double &rho_m, const double &theta_m,
+ double &rho_p, double &theta_p) ;
+
+/*!
+
+ \brief Point coordinates conversion from normalized coordinates
+ \f$(x,y)\f$ in meter to pixel coordinates \f$(u,v)\f$.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param x,y : input coordinates in meter.
+ \param u,v : output coordinates in pixels.
+
+ \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
+ with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
+ distortion.
+*/
+
+ inline static void
+ convertPoint(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ double &u, double &v)
+ {
+ switch(cam.projModel){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ convertPointWithoutDistortion(cam,x,y,u,v);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ convertPointWithDistortion(cam,x,y,u,v);
+ break;
+ }
+ }
+
+/*!
+
+ \brief Point coordinates conversion from normalized coordinates
+ \f$(x,y)\f$ in meter to pixel coordinates.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param x,y : input coordinates in meter.
+ \param iP : output coordinates in pixels.
+
+ In the frame (u,v) the result is given by:
+
+ \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
+ with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
+ distortion.
+*/
+
+ inline static void
+ convertPoint(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ vpImagePoint &iP)
+ {
+ switch(cam.projModel){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ convertPointWithoutDistortion(cam,x,y,iP);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ convertPointWithDistortion(cam,x,y,iP);
+ break;
+ }
+ }
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ /*!
+
+ \brief Point coordinates conversion without distortion from
+ normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates
+ \f$(u,v)\f$.
+
+ \f$ u = x*p_x+u_0 \f$ and \f$ v = y*p_y+v_0 \f$
+ */
+
+ inline static void
+ convertPointWithoutDistortion(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ double &u, double &v)
+ {
+ u = x * cam.px + cam.u0 ;
+ v = y * cam.py + cam.v0 ;
+ }
+
+ /*!
+
+ \brief Point coordinates conversion without distortion from
+ normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates.
+
+ In the frame (u,v) the result is given by:
+
+ \f$ u = x*p_x+u_0 \f$ and \f$ v = y*p_y+v_0 \f$
+ */
+
+ inline static void
+ convertPointWithoutDistortion(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ vpImagePoint &iP)
+ {
+ iP.set_u( x * cam.px + cam.u0 );
+ iP.set_v( y * cam.py + cam.v0 );
+ }
+
+ /*!
+
+ \brief Point coordinates conversion with distortion from
+ normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates
+ \f$(u,v)\f$.
+
+ \param cam : camera parameters.
+ \param x,y : input coordinates in meter.
+ \param u,v : output coordinates in pixels.
+
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and
+ \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
+ with \f$ r^2 = x^2+y^2 \f$
+ */
+ inline static void
+ convertPointWithDistortion(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ double &u, double &v)
+ {
+ double r2 = 1.+cam.kud*(x*x+y*y);
+ u = cam.u0 + cam.px*x*r2;
+ v = cam.v0 + cam.py*y*r2;
+ }
+
+ /*!
+
+ \brief Point coordinates conversion with distortion from
+ normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates.
+
+ \param cam : camera parameters.
+ \param x,y : input coordinates in meter.
+ \param iP : output coordinates in pixels.
+
+ In the frame (u,v) the result is given by:
+
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and
+ \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
+ with \f$ r^2 = x^2+y^2 \f$
+ */
+ inline static void
+ convertPointWithDistortion(const vpCameraParameters &cam,
+ const double &x, const double &y,
+ vpImagePoint &iP)
+ {
+ double r2 = 1.+cam.kud*(x*x+y*y);
+ iP.set_u( cam.u0 + cam.px*x*r2 );
+ iP.set_v( cam.v0 + cam.py*y*r2 );
+ }
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+} ;
+
+#endif
+
+
diff --git a/modules/core/include/visp3/core/vpMoment.h b/modules/core/include/visp3/core/vpMoment.h
new file mode 100644
index 0000000..00243a0
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMoment.h
@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base for 2D moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMoment.h
+ \brief Base class for all 2D moments.
+*/
+
+#ifndef __MOMENT_H__
+#define __MOMENT_H__
+
+#include <vector>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+
+class vpMomentDatabase;
+class vpMomentObject;
+
+
+/*!
+ \class vpMoment
+
+ \ingroup group_core_moments
+
+ \brief This class defines shared methods/attributes for 2D moments.
+
+ All moments or combination of moments in the moments module are based on this class.
+ A moment uses a vpMomentObject object to access all useful information.
+ Moment values are obtained by a 4-step process common for all moment types:
+ - Declaration.
+ \code
+ vpMoment moment;
+ \endcode
+ - Update with object.
+ \code
+ moment.update(object);
+ \endcode
+ - Compute the moment value
+ \code
+ moment.compute();
+ \endcode
+ - Access the values:
+ \code
+ std::vector<double> values = moment.get();
+ \endcode
+
+ A moment may also be linked to a vpMomentDatabase. Moments linked to a database are able to access each others values.
+ Some moments can be computed only if they are linked to a a database containing their dependencies.
+ Linking to a database is done using the vpMoment::linkTo(...) method.
+
+ There are no constraints about format of the array returned by vpMoment::get(); any implementation is fine.
+
+ Each moment must have a string name by implementing the char* vpMoment::name() method which allows to identify the moment in the database.
+ Each moment must also implement a compute method describing how to obtain its values from the object.
+
+ \attention Order of moment computation DOES matter: when you compute a moment using vpMoment::compute(),
+ all moment dependencies must be computed.
+ We recall that implemented moments are:
+ - vpMomentAlpha
+ - vpMomentArea
+ - vpMomentAreaNormalized
+ - vpMomentBasic
+ - vpMomentCentered
+ - vpMomentCInvariant
+ - vpMomentGravityCenter
+ - vpMomentGravityCenterNormalized
+
+*/
+class VISP_EXPORT vpMoment
+{
+private:
+ vpMomentObject* object;
+ vpMomentDatabase* moments;
+ char _name[255];
+
+protected:
+ std::vector<double> values;
+ /*!
+ Returns the linked moment database.
+ \return the moment database
+ */
+ inline vpMomentDatabase& getMoments() const { return *moments; }
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMoment(const vpMoment &)
+// : object(NULL), moments(NULL), values()
+// {
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// }
+// vpMoment &operator=(const vpMoment &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpMoment();
+
+ /*!
+ Virtual destructor.
+ */
+ virtual ~vpMoment() {}
+
+ virtual void compute()=0;
+ inline const vpMomentObject& getObject() const { return *object;}
+ /*!
+ Returns all values computed by the moment.
+ \return vector of values
+ */
+ const std::vector<double>& get() const { return values;}
+ void linkTo(vpMomentDatabase& moments);
+ virtual const char* name() const = 0;
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMoment& m);
+ virtual void printDependencies(std::ostream& os) const;
+ void update(vpMomentObject& object);
+
+};
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentAlpha.h b/modules/core/include/visp3/core/vpMomentAlpha.h
new file mode 100644
index 0000000..edf0dd2
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentAlpha.h
@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Alpha moment descriptor for in-plane orientation.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMomentAlpha.h
+ \brief Alpha moment descriptor for in-plane orientation.
+*/
+
+#ifndef __MOMENTALPHA_H__
+#define __MOMENTALPHA_H__
+
+#include <visp3/core/vpMoment.h>
+
+/*!
+ \class vpMomentAlpha
+
+ \ingroup group_core_moments
+
+ \brief This class defines the orientation of the object inside the plane parallel to the object.
+
+ The value of the moment is computed in radians in a \f$ [-\pi/2 .. \pi/2] \f$ interval at most by the formula \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$.
+
+ To obtain the \f$ [-\pi .. \pi] \f$ precision, you have to specify reference information. This reference information
+ describes an object with 0 rad orientation or at least an orientation between \f$ -\pi/2 \f$ and \f$ \pi/2 \f$ rad.
+ The nature of this information are third-order centered moments and a reference value of alpha.
+ Reference centered moments are obtained with vpMomentCentered and reference alpha is obtained with vpMomentAlpha using the reference constructor.
+
+ Therefore there are two modes for vpMomentAlpha and one constructor per mode:
+ - Reference mode (using the empty constructor):
+ The vpMomentAlpha doesn't need any additionnal information, it will compute its values from a reference object.
+ - Relative mode (using non-empty constructor):
+ The vpMomentAlpha is computed relative a reference alpha.
+ By knowing the reference, it may distinguish in-plane rotations of \f$ \alpha \f$ from rotations of \f$ \alpha + \pi \f$.
+
+ The following code demonstrates a calculation of a reference alpha and then uses this alpha to estimate the orientation
+ of the same object after performing a 180 degrees rotation.
+ Therefore the first and second alpha should have opposite values.
+
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentAlpha.h>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+
+//generic function for printing
+void print (double i) { std::cout << i << "\t";}
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-3); p.set_y(-1); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+
+ //////////////////////////////REFERENCE VALUES////////////////////////////////
+ vpMomentObject objRef(3); // Reference object. Must be of order 3
+ // because we will need the 3rd order
+ // centered moments
+ objRef.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
+ objRef.fromVector(vec_p); // Init the dense object with the polygon
+
+ vpMomentDatabase dbRef; //reference database
+ vpMomentGravityCenter gRef; // declaration of gravity center
+ vpMomentCentered mcRef; // centered moments
+ vpMomentAlpha alphaRef; //declare alpha as reference
+
+ gRef.linkTo(dbRef); //add gravity center to database
+ mcRef.linkTo(dbRef); //add centered moments
+ alphaRef.linkTo(dbRef); //add alpha depending on centered moments
+
+ dbRef.updateAll(objRef); // All of the moments must be updated, not just alpha
+
+ gRef.compute(); // compute the moment
+ mcRef.compute(); //compute centered moments AFTER gravity center
+ alphaRef.compute(); //compute alpha AFTER centered moments.
+
+ //the order of values in the vector must be as follows:
+ //mu30 mu21 mu12 mu03
+ std::vector<double> mu3ref(4);
+ mu3ref[0] = mcRef.get(3,0);
+ mu3ref[1] = mcRef.get(2,1);
+ mu3ref[2] = mcRef.get(1,2);
+ mu3ref[3] = mcRef.get(0,3);
+
+
+ std::cout << "--- Reference object ---" << std::endl;
+ std::cout << "alphaRef=" << alphaRef << std::endl << "mu3="; // print reference alpha
+ std::for_each (mu3ref.begin(), mu3ref.end(),print);
+ std::cout << std::endl;
+
+ ////////////CURRENT VALUES (same object rotated 180deg - must be
+ ////////////entered in reverse order)////////////////
+ vec_p.clear();
+
+ p.set_x(-3); p.set_y(1); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+ p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(-2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(1); p.set_y(-1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+
+
+ vpMomentObject obj(3); // second object. Order 3 is also required
+ // because of the Alpha will compare
+ // third-order centered moments to given reference.
+
+ obj.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
+ obj.fromVector(vec_p); // Init the dense object with the polygon
+
+ vpMomentDatabase db; // database
+ vpMomentGravityCenter g; // declaration of gravity center
+ vpMomentCentered mc; // mc containts centered moments
+ vpMomentAlpha alpha(mu3ref,alphaRef.get()); //declare alpha as relative to a reference
+
+ g.linkTo(db); //add gravity center to database
+ mc.linkTo(db); //add centered moments
+ alpha.linkTo(db); //add alpha depending on centered moments
+
+ db.updateAll(obj); // All of the moments must be updated
+
+ g.compute(); // compute the moment
+ mc.compute(); //compute centered moments AFTER gravity center
+ alpha.compute(); //compute alpha AFTER centered moments.
+
+ std::cout << "--- current object ---" << std::endl;
+ std::cout << "alpha=" << alpha.get() << std::endl;
+
+ return 0;
+}
+
+ \endcode
+This program outputs:
+\code
+--- Reference object ---
+alphaRef=0.441601
+mu3=1.80552 0.921882 0.385828 0.122449
+--- current object ---
+alpha=-0.441601
+
+\endcode
+
+Shortcuts for quickly getting those references exist in vpMomentCommon.
+
+This moment depends on vpMomentCentered.
+*/
+class VISP_EXPORT vpMomentAlpha : public vpMoment {
+ private:
+ bool isRef;
+ bool symmetric;
+ std::vector<double> ref;
+ double alphaRef;
+ public:
+
+ vpMomentAlpha();
+ vpMomentAlpha(std::vector<double>& ref,double alphaRef);
+
+ void compute();
+ /*!
+ Retrieve the orientation of the object as a single double value.
+ */
+ double get() const { return values[0]; }
+ /*!
+ Moment name.
+ */
+ const char* name() const {return "vpMomentAlpha";}
+
+ inline bool is_ref() const
+ {
+ if (isRef)
+ return true;
+ else
+ return false;
+ }
+
+ inline bool is_symmetric() const
+ {
+ if (symmetric)
+ return true;
+ else
+ return false;
+ }
+
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& v);
+ void printDependencies(std::ostream& os) const;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentArea.h b/modules/core/include/visp3/core/vpMomentArea.h
new file mode 100644
index 0000000..2c8a22b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentArea.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Just the area m00 = mu00
+ *
+ * Authors:
+ * Manikandan Bakthavatchalam
+ *
+ *****************************************************************************/
+#ifndef __MOMENTAREA_H__
+#define __MOMENTAREA_H__
+
+#include <visp3/core/vpMoment.h>
+
+class vpMomentObject;
+class vpMomentCentered; // Required for discrete case of vpMomentObject
+
+/*!
+ \class vpMomentArea
+
+ \ingroup group_core_moments
+
+ \brief Class handling the surface moment.
+
+*/
+class VISP_EXPORT vpMomentArea : public vpMoment {
+ public:
+ vpMomentArea();
+ void compute();
+ /*!
+ Moment name.
+ */
+ const char* name() const {return "vpMomentArea";}
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentArea& m);
+ void printDependencies(std::ostream& os) const;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentAreaNormalized.h b/modules/core/include/visp3/core/vpMomentAreaNormalized.h
new file mode 100644
index 0000000..146e4c3
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentAreaNormalized.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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).
+*/
+#ifndef __MOMENTSURFACENORMALIZED_H__
+#define __MOMENTSURFACENORMALIZED_H__
+
+#include <visp3/core/vpMoment.h>
+
+class vpMomentObject;
+class vpMomentCentered;
+
+/*!
+ \class vpMomentAreaNormalized
+
+ \ingroup group_core_moments
+
+ \brief Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
+
+ This moment depends on vpMomentCentered.
+
+ The idea behind vpMomentAreaNormalized is described in \cite Tahri05z.
+
+ During a visual servoing process, a vpMomentAreaNormalized will converge towards the desired depth when the current surface will converge to the destination surface.
+ It is defined as follows: \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$ where \e a is the current surface and \e a* the destination surface.
+ Consequently, the vpMomentAreaNormalized needs to have information about the desired depth \e Z* and the desired surface \e a*.
+
+ \warning About the order of the object.
+ The surface (refered to as \e a in the above paragraph) depends of the nature of the object.
+ - In case of a continuous object (when vpMomentObject::getType() is vpMomentObject::DENSE_FULL_OBJECT or vpMomentObject::DENSE_POLYGON) \f$a=m_{00}\f$.
+ - In case of a discrete object (when vpMomentObject::getType() is vpMomentObject::DISCRETE) \f$a=\mu_{20}+\mu_{02}\f$.
+
+ Therefore, a vpMomentObject has to be of minimum order 2 in order to compute a vpMomentAreaNormalized moment in the discrete case
+ and of minimum order 0 in continous cases.
+
+ This example shows a computation in the discrete case.
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains object points
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane of point 1
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane of point 2
+ vec_p.push_back(p);
+
+ //////////////////////////////REFERENCE VALUES////////////////////////////////
+ vpMomentObject obj(2); // Object where all the moment defined with
+ // i+j <= 2 will be computed below. Order is
+ // 2 because in discrete mode, the surface
+ // computation is a=mu02+mu02
+
+
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
+ obj.fromVector(vec_p); // initialize the object with the points coordinates
+
+ vpMomentDatabase db; //reference database
+ vpMomentGravityCenter g; // declaration of gravity center
+ vpMomentCentered mc; // centered moments
+ vpMomentAreaNormalized an(2,1); //declare normalized surface with
+ //destination depth of 1 meter and
+ //destination surface of 2 m2
+
+ g.linkTo(db); //add gravity center to database
+ mc.linkTo(db); //add centered moments
+ an.linkTo(db); //add alpha depending on centered moments
+
+ db.updateAll(obj); // All of the moments must be updated, not just an
+
+ g.compute(); // compute the moment
+ mc.compute(); //compute centered moments AFTER gravity center
+ an.compute(); //compute alpha AFTER centered moments.
+
+ std::cout << an << std::endl;
+
+ return 0;
+}
+ \endcode
+ This code produces the following output:
+ \code
+An:1.41421
+ \endcode
+*/
+class VISP_EXPORT vpMomentAreaNormalized : public vpMoment {
+ private:
+ double desiredSurface;
+ double desiredDepth;
+ public:
+ vpMomentAreaNormalized(double desiredSurface, double desiredDepth);
+ void compute();
+ /*!
+ Retrieves the desired depth \e Z* as specified in the constructor.
+ */
+ double getDesiredDepth() const { return desiredDepth; }
+ /*!
+ Retrieves the desired surface \e a* as specified in the constructor.
+ */
+ double getDesiredSurface() const { return desiredSurface; }
+
+ /*!
+ Moment name.
+ */
+ const char* name() const {return "vpMomentAreaNormalized";}
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& v);
+ void printDependencies(std::ostream& os) const;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentBasic.h b/modules/core/include/visp3/core/vpMomentBasic.h
new file mode 100644
index 0000000..c433e06
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentBasic.h
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMomentBasic.h
+ \brief Basic moment descriptor (also called m).
+*/
+
+#ifndef __MOMENTBASIC_H__
+#define __MOMENTBASIC_H__
+
+#include <visp3/core/vpMoment.h>
+
+/*!
+ \class vpMomentBasic
+
+ \ingroup group_core_moments
+
+ \brief This class defines the 2D basic moment \f$m_{ij}\f$.
+ This class is a wrapper for vpMomentObject wich allows to use it as a moment.
+ This enables all common operations such as adding to a vpMomentDatabase.
+
+ In the case of a dense object \e O, 2D basic moments are defined by:
+ \f[m_{ij} = \int \int_{O} x^j y^j dx dy\f]
+
+ In the case of a discrete set of \e n points, the basic moments are defined by
+ \f[m_{ij} = \sum_{k=1}^{n} x_k^j y_k^j \f]
+
+ The selection of a dense or discrete object is done in vpMomentObject.
+ The maximum order of vpMomentBasic is the same as that of vpMomentObject (ie: \f$i+j \leq \f$ vpMomentObject::getOrder).
+
+ Note that in the continuous case, the basic moment \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
+ In the discrete case, it is the number of discrete points \f$n\f$.
+*/
+
+class VISP_EXPORT vpMomentBasic : public vpMoment {
+ public:
+ vpMomentBasic();
+
+ void compute();
+ const std::vector<double>& get() const;
+ double get (unsigned int i,unsigned int j) const;
+ /*!
+ Moment name.
+ */
+ const char* name() const { return "vpMomentBasic";}
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentBasic& v);
+ void printDependencies(std::ostream& os) const;
+};
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentCInvariant.h b/modules/core/include/visp3/core/vpMomentCInvariant.h
new file mode 100644
index 0000000..6a887a5
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentCInvariant.h
@@ -0,0 +1,270 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Descriptor for various invariants used to drive space roations around X and Y axis.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpMomentCInvariant.h
+ \brief Descriptor for various invariants used to drive space roations around X and Y axis.
+*/
+#ifndef __MOMENTCINVARIANT_H__
+#define __MOMENTCINVARIANT_H__
+
+#include <visp3/core/vpMoment.h>
+#include <visp3/core/vpMomentDatabase.h>
+
+class vpMomentCentered;
+class vpMomentBasic;
+
+/*!
+ \class vpMomentCInvariant
+
+ \ingroup group_core_moments
+
+ This class defines several 2D (translation+rotation+scale) invariants for both symmetric and non-symmetric objects.
+ These moment-based invariants are described in the following papers \cite Chaumette04a, \cite Tahri05z.
+
+ The descriptions for the invariants \f$C_1\f$ to \f$C_{10}\f$ can be found in \cite Chaumette04a and for invariants \f$P_x\f$,\f$P_y\f$,\f$S_x\f$,\f$S_y\f$ in \cite Tahri05z.
+
+ These invariants are classicaly used in visual servoing to control the out-of-plane rotations.
+ The C-type or P-type invariants are used for non-symmetric objects whereas the S-type invariants are used for symmetric objects.
+
+ For most cases of non-symmetric objects, (\f$C_4\f$,\f$C_6\f$) or (\f$P_x\f$,\f$P_y\f$) couples are widely used to control x and y rotations.
+ For symmetric objects \f$S_x\f$ and \f$S_y\f$ are the only choice.
+
+ There are 14 translation+rotation+scale invariants (10 C-type, 2 P-type and 2 S-type) that can be accessed from by vpMomentCInvariant::get
+ or any of the get shortcuts.
+
+ The example below shows how to retrieve the \f$C_2\f$ invariant:
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <iostream>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p;
+
+ p.set_x(6); p.set_y(-1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(3); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(0); p.set_y(1.2); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(-7); p.set_y(-4); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+
+ vpMomentObject obj(5); // Create an image moment object with 5 as maximum order
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
+ obj.fromVector(vec_p);
+
+ //initialisation with default values
+ vpMomentCommon db(vpMomentCommon::getSurface(obj),vpMomentCommon::getMu3(obj),vpMomentCommon::getAlpha(obj),1.);
+ bool success;
+
+ db.updateAll(obj); // Update AND compute all moments
+
+ //get C-invariant
+ const vpMomentCInvariant& C = static_cast<const vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
+ if(success)
+ std::cout << C.get(1) << std:: endl; // print C2 invariant
+ else
+ std::cout << "vpMomentCInvariant not found." << std::endl;
+
+ return 0;
+}
+\endcode
+
+vpMomentCInvariant depends on vpMomentCentered (see vpMomentDatabase and vpMomentCommon).
+*/
+class VISP_EXPORT vpMomentCInvariant : public vpMoment {
+ private:
+ std::vector<double> I;
+ std::vector<double> II;
+ std::vector<double> c;
+ std::vector<double> s;
+ double K;
+ void computeI(const vpMomentCentered& momentCentered, std::vector<double>& I);
+
+ /* To calculate Sx and Sy from normalized moments */
+ void calcSxSy(double& sx, double& sy) const;
+ void calcSxSyNormalized(double& sx, double& sy) const;
+ std::vector<double> cn; // same as s above but calculated from normalized moments
+ std::vector<double> sn; // same as c above but calculated from normalized moments
+ double In1; // same as I1 in Sx,Sy formulae but calculated from normalized moments
+ bool flg_sxsynormalization_;
+
+ public:
+
+ vpMomentCInvariant(bool flg_sxsynormalization = false);
+
+ /*!
+ Shorcut for getting the value of \f$C_1\f$.
+ */
+ double C1() const { return values[0]; }
+ /*!
+ Shorcut for getting the value of \f$C_2\f$.
+ */
+ double C2() const { return values[1]; }
+ /*!
+ Shorcut for getting the value of \f$C_3\f$.
+ */
+ double C3() const { return values[2]; }
+ /*!
+ Shorcut for getting the value of \f$C_4\f$.
+ */
+ double C4() const { return values[3]; }
+ /*!
+ Shorcut for getting the value of \f$C_5\f$.
+ */
+ double C5() const { return values[4]; }
+ /*!
+ Shorcut for getting the value of \f$C_6\f$.
+ */
+ double C6() const { return values[5]; }
+ /*!
+ Shorcut for getting the value of \f$C_7\f$.
+ */
+ double C7() const { return values[6]; }
+ /*!
+ Shorcut for getting the value of \f$C_8\f$.
+ */
+ double C8() const { return values[7]; }
+ /*!
+ Shorcut for getting the value of \f$C_9\f$.
+ */
+ double C9() const { return values[8]; }
+ /*!
+ Shorcut for getting the value of \f$C_{10}\f$.
+ */
+ double C10() const { return values[9]; }
+
+ void compute();
+
+ /*!
+ Gets the desired invariant.
+ \param i given index. For invariants from C1 to C10 the corresponding index is from 0 to 9. For \f$S_x\f$,\f$S_y\f$ the indexes are 10,11 and for \f$P_x\f$,\f$P_y\f$ they are 12,13.
+ */
+ double get(unsigned int i) const { return values[i]; }
+
+ /*!
+ Access to partial invariant c (see [2]).
+ */
+ double getC(unsigned int i) const {return c[i];}
+ /*!
+ Access to partial invariants. The index convention is the same as in [1].
+ */
+ double getI(unsigned int index) const {return I[index];}
+
+ /*!
+ Print the moment invariants used to obtain the actual visual features
+ */
+ void printInvariants(std::ostream& os) const;
+
+ /*!
+ Access to partial invariant I (see [2]).
+ */
+ double getII(unsigned int i) const {return II[i];}
+ /*!
+ Access to partial invariant K (see [2]).
+ */
+ double getK() const {return K;}
+
+ /*!
+ Access to partial invariant S (see [2]).
+ */
+ double getS(unsigned int i) const {return s[i];}
+
+ /*!
+ Moment name.
+ */
+ const char* name() const {return "vpMomentCInvariant";}
+
+ /*!
+ Print partial invariant.
+ */
+ void printI(unsigned int index);
+
+ /*!
+ Shorcut for getting the value of \f$P_x\f$.
+ */
+ double Px(){ return values[12]; }
+ /*!
+ Shorcut for getting the value of\f$P_y\f$.
+ */
+ double Py(){ return values[13]; }
+
+ /*!
+ Shorcut for getting the value of \f$S_x\f$.
+ */
+ double Sx() const { return values[10]; }
+ /*!
+ Shorcut for getting the value of \f$S_y\f$.
+ */
+ double Sy() const { return values[11]; }
+
+ /*!
+ * Getters for I
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getIn1() const {return In1;}
+
+ /*!
+ * Getter for c
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getCN(unsigned int i) const {return cn[i];}
+
+ /*!
+ * Getter for s
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getSN(unsigned int i) const {return sn[i];}
+
+ /*!
+ * To know if Sx and Sy were calculated from normalized moments or not
+ */
+ bool isSxSyfromNormalizedMoments() const {return flg_sxsynormalization_;};
+
+ /*!
+ * To get all the invariant values as a whole.
+ */
+ inline const std::vector<double>& getMomentVector() const { return values; }
+
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCInvariant& v);
+};
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentCentered.h b/modules/core/include/visp3/core/vpMomentCentered.h
new file mode 100644
index 0000000..8fcd14f
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentCentered.h
@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Centered moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#ifndef __MOMENTCENTERED_H__
+#define __MOMENTCENTERED_H__
+
+#include <visp3/core/vpMoment.h>
+/*!
+ \file vpMomentCentered.h
+ \brief Centered moment descriptor (also refered to as \f$\mu_{ij}\f$).
+
+*/
+
+class vpMomentObject;
+
+/*!
+ \class vpMomentCentered
+
+ \ingroup group_core_moments
+
+ \brief This class defines the double-indexed centered moment descriptor \f$\mu_{ij}\f$.
+
+ In the case of a dense object \e O, centered moments are defined by:
+ \f[\mu_{ij}= \int \int_{O} (x_k-x_g)^j (y_k-y_g)^j\f]
+
+ In the case of a discrete set of \e n points, centered moments are defined by:
+ \f[\mu_{ij}= \sum_{k=1}^{n} (x_k-x_g)^j (y_k-y_g)^j\f]
+
+ where \f$(x_g,y_g)\f$ are the coordinates of the center of gravity.
+
+ The centered moments are computed from the object at the highest possible order.
+ For example if the vpMomentObject is defined up to order 5, vpMomentCentered will be too.
+
+ Values of vpMomentCentered may be accessed by one of the two vpMomentCentered::get methods.
+ When using vpMomentCentered::get (), the format of the return vector is the following:
+ \f$ \mu_{ij} \f$ is stored at vpMomentCentered::get ()[j* (vpMomentObject::getOrder () +1)+i]
+
+ vpMomentCentered depends on vpMomentGravityCenter.
+
+*/
+class VISP_EXPORT vpMomentCentered : public vpMoment {
+public:
+
+ vpMomentCentered();
+
+ void compute();
+ double get(unsigned int i,unsigned int j) const;
+
+ inline const std::vector<double>& get() const;
+ /*!
+ Moment name.
+ */
+ inline const char* name() const {return "vpMomentCentered";}
+
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& v);
+ void printWithIndices(std::ostream& os) const;
+ void printDependencies(std::ostream& os) const;
+
+protected:
+
+ void set(unsigned int i, unsigned int j, double value);
+
+};
+
+/*!
+ Returns all centered moment values \f$\mu_{ij}\f$ with \f$i+j \leq order\f$ where order is the object's order.
+
+ \return Vector of moment values. To access \f$\mu_{ij}\f$, you have to read vpMomentObject::get()[j*order+i].
+
+ For example, if the maximal order is 3, the following values are provided:
+ \code
+u00 u10 u20 u01 u11 u21 u02 u12 u12 u30 u03
+ \endcode
+ To have a better reading of the moments you can picture them as a triangular matrix:
+ \code
+u00 u10 u20 u30
+u01 u11 u21 x
+u02 u12 x x
+u30 x x x
+ \endcode
+ The moments of the same order are on each of the matrix reverse diagonals.
+ To access for example to the centered moment \f$\mu_{12}\f$, you should use this kind of code:
+ \code
+vpMomentCentered mc;
+//[...]
+mc.compute();
+double mu12;
+mu12 = mc.get()[2*(obj.getOrder()+1)+1]; // i=1 and j=2
+mu12 = mc.get(1,2); // the same
+ \endcode
+*/
+inline const std::vector<double>& vpMomentCentered::get() const
+{
+ return vpMoment::get();
+}
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentCommon.h b/modules/core/include/visp3/core/vpMomentCommon.h
new file mode 100644
index 0000000..dbd5f03
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentCommon.h
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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.
+*/
+#ifndef VPCOMMONMOMENTS_H
+#define VPCOMMONMOMENTS_H
+
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentGravityCenterNormalized.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentAlpha.h>
+#include <visp3/core/vpMomentArea.h>
+
+#include <vector>
+
+class vpMomentObject;
+
+/*!
+ \class vpMomentCommon
+
+ \ingroup group_core_moments
+
+ \brief This class initializes and allows access to commonly used moments.
+
+ It is a vpMomentDatabase filled with the following moments:
+ - vpMomentBasic
+ - vpMomentGravityCenter
+ - vpMomentCentered
+ - vpMomentGravityCenterNormalized
+ - vpMomentAreaNormalized
+ - vpMomentCInvariant
+ - vpMomentAlpha
+ - vpMomentArea
+
+ There is no need to do the linkTo operations manually nor is it necessary to care about the order of moment computation.
+
+ This class carries an vpMomentCommon::updateAll() method capable of updating AND computing moments from an object (see 4-step process in vpMoment).
+ The moments computed by this class are classical moments used in moment-based visual servoing.
+ For more information see \cite Tahri05z.
+
+ To initialize this moment set the user needs to compute the following things:
+ - the Mu3 value set: set of third-order centered moments computed for a reference object. (\f$\mu_{ij}$ with $i+j = 3\f$ ).
+ These values allow the system to save the reference angular position and to perform planar rotations of more than 180 degrees if needed.
+ - the destination depth.
+ - the surface of the destination object in the end of the visual servoing process.
+ - the reference alpha: angular position of the object used to obtain the Mu3 set.
+
+ Shortcuts for each of these prerequisites are provided by this class except depth (methods
+ vpMomentCommon::getMu3(), vpMomentCommon::getSurface(), vpMomentCommon::getAlpha()).
+
+ \attention Make sure your object is at least of order 5 when using this pre-filled database.
+
+*/
+class VISP_EXPORT vpMomentCommon : public vpMomentDatabase
+{
+private:
+ vpMomentBasic momentBasic;
+ vpMomentGravityCenter momentGravity;
+ vpMomentCentered momentCentered;
+ vpMomentGravityCenterNormalized momentGravityNormalized;
+ vpMomentAreaNormalized momentSurfaceNormalized;
+ vpMomentCInvariant* momentCInvariant;
+ vpMomentAlpha momentAlpha;
+ vpMomentArea momentArea;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMomentCommon(const vpMomentCommon &)
+// : vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), momentGravityNormalized(),
+// momentSurfaceNormalized(0, 0), momentCInvariant(), momentAlpha(), momentArea()
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMomentCommon &operator=(const vpMomentCommon &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ=1.0, bool flg_sxsyfromnormalized=false);
+
+ static double getAlpha(vpMomentObject& object);
+ static std::vector<double> getMu3(vpMomentObject& object);
+ static double getSurface(vpMomentObject& object);
+
+ void updateAll(vpMomentObject& object);
+ ~vpMomentCommon();
+};
+#endif // VPCOMMONMOMENTS_H
diff --git a/modules/core/include/visp3/core/vpMomentDatabase.h b/modules/core/include/visp3/core/vpMomentDatabase.h
new file mode 100644
index 0000000..f85eb30
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentDatabase.h
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pseudo-database used to handle dependencies between moments
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpMomentDatabase.h
+ \brief Pseudo-database used to handle dependencies between moments.
+*/
+#ifndef __MOMENTDATABASE_H__
+#define __MOMENTDATABASE_H__
+
+#include <visp3/core/vpImage.h>
+
+#include <map>
+#include <iostream>
+#include <cstring>
+
+class vpMoment;
+class vpMomentObject;
+
+/*!
+ \class vpMomentDatabase
+
+ \ingroup group_core_moments
+
+ \brief This class allows to register all vpMoments so they can access each other according to their dependencies.
+
+ Sometimes, a moment needs to have access to other moment's values to be computed.
+ For example vpMomentCentered needs additionnal information about the gravity center vpMomentGravityCenter in order to compute the moment's value from a vpMomentObject.
+ This gravity center should be stored in a vpMomentDatabase where it can be accessed.
+
+ All moments in a database can access each other freely at any time. They can also verify if a moment is present in the database or not.
+ Here is a example of a dependency between two moments using a vpMomentDatabase:
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <iostream>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ vpMomentObject obj(1); // Create an image moment object with 1 as
+ // maximum order (sufficient for gravity center)
+ obj.setType(vpMomentObject::DISCRETE); // The object is defined by
+ // two discrete points
+ obj.fromVector(vec_p); // Init the dense object with the polygon
+
+ vpMomentDatabase db;
+ vpMomentGravityCenter g; // declaration of gravity center
+ vpMomentCentered mc; // mc containts centered moments
+
+ g.linkTo(db); //add gravity center to database
+ mc.linkTo(db); //centered moments depend on gravity, add them to the
+ //database to grant access
+
+ db.updateAll(obj); // All of the moments must be updated, not just mc
+
+ //There is no global compute method since the order of compute calls
+ //depends on the implementation
+ g.compute(); // compute the moment
+ mc.compute(); //compute centered moments AFTER gravity center
+
+ std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment
+ std::cout << "Centered moments: " << mc << std:: endl; // print centered moment
+
+ return 0;
+}
+ \endcode
+
+ The following code outputs:
+ \code
+Gravity center:
+Xg=1.5, Yg=1.5
+Centered moments:
+2 0
+0 x
+ \endcode
+
+ A moment is identified in the database by it's vpMoment::name method. Consequently, a database can contain at most one moment of each type.
+ Often it is useful to update all moments with the same object. Shortcuts (vpMomentDatabase::updateAll) are provided for that matter.
+*/
+class VISP_EXPORT vpMomentDatabase{
+ private:
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ struct cmp_str {
+ bool operator()(char const *a, char const *b) const{
+ return std::strcmp(a, b) < 0;
+ }
+ };
+#endif
+ std::map<const char*,vpMoment*,cmp_str> moments;
+ void add(vpMoment& moment, const char* name);
+ public:
+ vpMomentDatabase() : moments() {}
+ virtual ~vpMomentDatabase() {}
+
+ const vpMoment& get(const char* type, bool& found) const;
+ /*!
+ Get the first element in the database.
+ May be useful in case an unnamed object is present but is the only element in the database.
+ \return the first element in the database.
+ */
+ vpMoment& get_first(){return *(moments.begin()->second);}
+
+ virtual void updateAll(vpMomentObject& object);
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& v);
+ friend class vpMoment;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentGravityCenter.h b/modules/core/include/visp3/core/vpMomentGravityCenter.h
new file mode 100644
index 0000000..857d1c8
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentGravityCenter.h
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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 Xg,Yg).
+*/
+#ifndef __MOMENTGRAVITYCENTER_H__
+#define __MOMENTGRAVITYCENTER_H__
+
+#include <visp3/core/vpMoment.h>
+#include <visp3/core/vpMomentDatabase.h>
+class vpMomentObject;
+
+/*!
+ \class vpMomentGravityCenter
+
+ \ingroup group_core_moments
+
+ \brief Class describing 2D gravity center moment.
+
+ This moment can be computed from scratch (no need to compute any different moments before computing this).
+ It gives access to both coordinates of the gravity center \f$x_g\f$ and \f$y_g\f$.
+
+ These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}} \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <iostream>
+
+int main()
+{
+ // Define the contour of an object by a 5 clockwise vertices on a plane
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+
+ p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
+ vec_p.push_back(p);
+
+ vpMomentObject obj(1); // Create an image moment object with 1 as
+ // maximum order (because only m00,m01,m10
+ // are needed to compute the gravity center primitive.
+ obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
+ obj.fromVector(vec_p); // Init the dense object with the polygon
+
+ vpMomentGravityCenter g; // declaration of gravity center
+ g.update(obj); // specify the object
+ g.compute(); // compute the moment
+
+ std::cout << "Xg=" << g.getXg() << std::endl; // access to Xg
+ std::cout << "Yg=" << g.getYg() << std::endl; // access to Yg
+
+ std::cout << g << std:: endl; // print gravity center
+
+ return 0;
+}
+\endcode
+
+This example produces the following results:
+\code
+Xg=0.0166667
+Yg=-0.00833333
+Xg=0.0166667, Yg=-0.00833333
+\endcode
+*/
+
+class VISP_EXPORT vpMomentGravityCenter : public vpMoment {
+ public:
+
+ vpMomentGravityCenter();
+ void compute();
+ const std::vector<double>& get() const;
+ /*!
+ Shortcut function to retrieve \f$x_g\f$.
+ \return The first gravity center coordinate.
+ */
+ double getXg() const { return values[0]; }
+ /*!
+ Shortcut function to retrieve \f$y_g\f$.
+ \return The second gravity center coordinate.
+ */
+ double getYg() const { return values[1]; }
+ /*!
+ The class's string name.
+ */
+ const char* name() const {return "vpMomentGravityCenter";}
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenter& v);
+ void printDependencies(std::ostream& os) const;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h
new file mode 100644
index 0000000..67a02d0
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpMomentGravityCenterNormalized.h
+ \brief 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+*/
+#ifndef __MOMENTGRAVITYNORMALIZED_H__
+#define __MOMENTGRAVITYNORMALIZED_H__
+
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+class vpMomentObject;
+
+/*!
+ \class vpMomentGravityCenterNormalized
+
+ \ingroup group_core_moments
+
+ \brief Class describing 2D normalized gravity center moment.
+
+ Centered and normalized gravity center moment is defined as follows: \f$(x_n,y_n)\f$ where
+ \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$.
+
+ vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get access to \f$a_n\f$ and on vpMomentGravityCenter to get access to \f$(x_g,y_g)\f$ .
+*/
+class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter {
+ public:
+ vpMomentGravityCenterNormalized();
+ void compute();
+ /*!
+ Moment name.
+ */
+ const char* name() const {return "vpMomentGravityCenterNormalized";}
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenterNormalized& v);
+ void printDependencies(std::ostream& os) const;
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMomentObject.h b/modules/core/include/visp3/core/vpMomentObject.h
new file mode 100644
index 0000000..14b8b72
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMomentObject.h
@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Object input structure used by moments.
+ *
+ * Authors:
+ * Filip Novotny
+ * Manikandan Bakthavatchalam
+ *****************************************************************************/
+/*!
+ \file vpMomentObject.h
+ \brief Object input structure used by moments.
+*/
+#ifndef __MOMENTOBJECT_H__
+#define __MOMENTOBJECT_H__
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMoment.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <cstdlib>
+#include <utility>
+
+class vpCameraParameters;
+
+/*!
+ \class vpMomentObject
+
+ \ingroup group_core_moments
+
+ \brief Class for generic objects.
+
+ It contains all basic moments often described by \f$m_{ij}\f$ of order \f$i+j\f$ going from \f$m_{00}\f$ to the order used as parameter in vpMomentObject() constructor.
+ All other moments implemented in ViSP (gravity center, alpha orientation, centered moments...) use this moment object as a combination of its different values.
+
+ When constructing a vpMomentObject() you need first to specify the maximum used moment order as parameter.
+
+ Then there are three ways to initialize a vpMomentObject. Firstly using fromImage() you can considerer
+ a dense object \e O defined by a binary image. Secondly, as described in fromVector() you can also define a dense object \e O by a closed contour.
+ In these two cases, 2D basic moments are defined by:
+ \f[m_{ij} = \int \int_{O} x^i y^j dx dy\f]
+
+ Lastly, as presented in fromVector() you can consider a discrete set of \e n points. In that last case, the basic moments are defined by
+ \f[m_{ij} = \sum_{k=1}^{n} x_k^i y_k^j \f]
+
+ With setType() method you can specify the object type.
+
+
+ \attention Be careful with the object order. When you specify a maximum order in the vpMomentObject::vpMomentObject constructor (see its detailed description),
+ it will compute all moment orders up to the order you specified. If you want to access the values \f$ m_{ij} \f$ with the vpMomentObject::get method, you can
+ do object.get()[j*(order+1)+i].
+
+ A few tips about which orders to use in different situations:
+ - moment based visual servoing: use vpMomentObject(6). This will compute moment values up to order 6 which will enable vpFeatureMoments up to order 5 which is the maximum order required for common moments.
+ - computing gravity center: use vpMomentObject(1). You only need \f$ m_{00},m_{01},m_{10} \f$. You should compute moments up to order 1.
+ - computing gravity center interaction matrix (vpFeatureMomentGravityCenter): use vpMomentObject(2). This will compute moment values till order 2 since they are needed for the interaction matrix of vpFeatureMoments of order 1.
+
+
+ The following example shows how to create a moment object from 4 discrete points
+ locate on a plane one meter in front of the camera. It shows also how to get the basic
+ moments that are computed and how to compute other classical moments such as the gravity
+ center or the centered moments.
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpPoint.h>
+
+int main()
+{
+ // Define an object as 4 clockwise points on a plane (Z=0)
+ std::vector<vpPoint> vec_p; // vector that contains the 4 points
+
+ vec_p.push_back( vpPoint(-0.2, 0.1, 0.0) ); // values in meters
+ vec_p.push_back( vpPoint(+0.3, 0.1, 0.0) ); // values in meters
+ vec_p.push_back( vpPoint(+0.2,-0.1, 0.0) ); // values in meters
+ vec_p.push_back( vpPoint(-0.2,-0.15, 0.0) ); // values in meters
+
+ // These points are observed by a camera
+ vpHomogeneousMatrix cMo(0, 0, 1, 0, 0, 0); // We set the camera to be 1m far the object
+ // ... update cMo from an image processing
+
+ // Apply the perspective projection to update the points coordinates in the camera plane
+ for(unsigned int i=0; i<vec_p.size(); ++i)
+ vec_p[i].project(cMo);
+
+ std::cout << "Considered points: " << std::endl;
+ for(unsigned int i=0; i<vec_p.size(); ++i)
+ std::cout << "point " << i << ": " << vec_p[i].get_x() << ", " << vec_p[i].get_y() << std::endl;
+
+ // Define an image moment object from the previous points
+ vpMomentObject obj(5); // use moments up to order 5
+ obj.setType(vpMomentObject::DISCRETE); // initialize the object as constituted by discrete points
+ obj.fromVector(vec_p); // init the object from the points
+
+ // --- Access the computed moments by querying the moment object
+
+ // 1. Getting a vector of doubles
+ std::vector<double> moment = obj.get();
+ std::cout << std::endl << "Basic moment available (from vector of doubles) " << std::endl;
+ for(unsigned int k=0; k<=obj.getOrder(); k++) {
+ for(unsigned int l=0; l<(obj.getOrder()+1)-k; l++){
+ std::cout << "m" << l << k << "=" << moment[k*(momobj.getOrder()+1)+ l] << "\t";
+ }
+ std::cout<<std::endl;
+ }
+
+ // 2. Print the contents of moment object directly
+ std::cout << std::endl << "Basic moment available: ";
+ std::cout << obj << std::endl;
+
+ // 3. Directly indexing the moment object
+ std::cout << std::endl << "Direct acces to some basic moments: " << std::endl;
+ std::cout << "m00: " << obj.get(0, 0) << std::endl;
+ std::cout << "m10: " << obj.get(1, 0) << std::endl;
+ std::cout << "m01: " << obj.get(0, 1) << std::endl;
+ std::cout << "m22: " << obj.get(2, 2) << std::endl;
+ std::cout << "m20: " << obj.get(2, 0) << std::endl;
+ std::cout << "m02: " << obj.get(0, 2) << std::endl;
+
+ // Get common moments computed using basic moments
+ double m00 = vpMomentCommon::getSurface(obj); // surface = m00
+ double alpha = vpMomentCommon::getAlpha(obj); // orientation
+ std::vector<double> mu_3 = vpMomentCommon::getMu3(obj); // centered moment up to 3rd order
+
+ std::cout << std::endl << "Common moments computed using basic moments:" << std::endl;
+ std::cout << "Surface: " << m00 << std::endl;
+ std::cout << "Alpha: " << alpha << std::endl;
+ std::cout << "Centered moments (mu03, mu12, mu21, mu30): ";
+ for(unsigned int i=0; i<mu_3.size(); ++i)
+ std::cout << mu_3[i] << " ";
+ std::cout << std::endl;
+
+ return 0;
+}
+ \endcode
+
+ This example produces the following results:
+ \code
+Considered points:
+point 0: -0.2, 0.1
+point 1: 0.3, 0.1
+point 2: 0.2, -0.1
+point 3: -0.2, -0.15
+
+Basic moment available (from vector of doubles):
+m00=4 m10=0.1 m20=0.21 m30=0.019 m40=0.0129 m50=0.00211
+m01=-0.05 m11=0.02 m21=0.003 m31=0.0023 m41=0.00057
+m02=0.0525 m12=-0.0015 m22=0.0026 m32=9e-05
+m03=-0.002375 m13=0.000575 m23=-4.5e-05
+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
+
+Direct acces to some basic moments:
+m00: 4
+m10: 0.1
+m01: -0.05
+m22: 0.0026
+m20: 0.21
+m02: 0.0525
+
+Common moments computed using basic moments:
+Surface: 0.259375
+Alpha: 0.133296
+Centered moments (mu03, mu12, mu21, mu30): 0.003375 0.0045625 -0.00228125 -0.000421875
+ \endcode
+
+ Note that in the continuous case, the moment object \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
+ In the discrete case, it is the number of discrete points \f$n\f$.
+*/
+class VISP_EXPORT vpMomentObject
+{
+public:
+
+ /*!
+ Type of object that will be considered.
+ */
+ 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) describing a polygon. It will be treated as dense. */
+ DISCRETE = 2, /*!< A cloud of points. Treated as discrete. */
+ } vpObjectType;
+
+ /*!
+ Type of camera image background.
+ */
+ typedef enum{
+ BLACK = 0, /*! Black background */
+ WHITE = 1, /*! No functionality as of now */
+ } vpCameraImgBckGrndType;
+
+ bool flg_normalize_intensity; // To scale the intensity of each individual pixel in the image by the maximum intensity value present in it
+
+ // Constructors
+ vpMomentObject(unsigned int order);
+ vpMomentObject(const vpMomentObject& srcobj);
+ /*!
+ Virtual destructor to allow polymorphic usage.
+ For instance,
+ \code
+ vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
+ \endcode
+ */
+ virtual ~vpMomentObject();
+
+ void fromImage(const vpImage<unsigned char>& image,unsigned char threshold, const vpCameraParameters& cam); // Binary version
+ void fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam, vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size = true); // Photometric version
+
+ void fromVector(std::vector<vpPoint>& points);
+ const std::vector<double>& get() const;
+ double get(unsigned int i,unsigned int j) const;
+
+ /*!
+ \return The type of object that is considered.
+ */
+ vpObjectType getType() const {return type;}
+
+ /*!
+ \return The maximal order. The basic moments \f$m_{ij}\f$ that will be computed
+ are for \f$i+j \in [0:\mbox{order}]\f$.
+ */
+ unsigned int getOrder() const {return order-1;}
+
+ // Constructor helpers
+ void init(unsigned int orderinp);
+ void init(const vpMomentObject& objin);
+
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& v);
+ /*!
+ Outputs raw moments in indexed form like m[1,1] = value of moment m11
+ \param momobj : A vpMomentObject
+ \param os : Output stream.
+ */
+ static void printWithIndices(const vpMomentObject& momobj, std::ostream& os);
+ /*!
+ Specifies the type of the input data.
+ \param input_type : An input type.
+ */
+ void setType(vpObjectType input_type){this->type=input_type;}
+
+ /*!
+ Converts the raw moments contained in vpMomentObject to a vpMatrix
+ \param momobj : A vpMomentObject
+ */
+ static vpMatrix convertTovpMatrix(const vpMomentObject& momobj);
+
+protected:
+ unsigned int order;
+ vpObjectType type;
+ std::vector<double> values;
+ void set(unsigned int i, unsigned int j, const double& value_ij);
+ void cacheValues(std::vector<double>& cache,double x, double y);
+
+private:
+ void cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized);
+ double calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points);
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpMouseButton.h b/modules/core/include/visp3/core/vpMouseButton.h
new file mode 100644
index 0000000..650f0d1
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMouseButton.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Color definition.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpMouseButton_h
+#define vpMouseButton_h
+
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \class vpMouseButton
+ \ingroup group_gui_display
+ \brief Class that defines mouse button identifiers.
+*/
+class VISP_EXPORT vpMouseButton
+{
+public:
+ typedef enum {
+ button1 = 1, /*!< Mouse left button. */
+ button2 = 2, /*!< Mouse middle button, or roll. */
+ button3 = 3, /*!< Mouse right button. */
+ none = 0 /*!< No button. */
+ } vpMouseButtonType ;
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpMutex.h b/modules/core/include/visp3/core/vpMutex.h
new file mode 100644
index 0000000..d6ad112
--- /dev/null
+++ b/modules/core/include/visp3/core/vpMutex.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mutex protection.
+ *
+ * Authors:
+ * Celine Teuliere
+ *
+ *****************************************************************************/
+
+
+#ifndef __VP_MUTEX__
+#define __VP_MUTEX__
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+#ifdef VISP_HAVE_PTHREAD
+
+#include <pthread.h>
+
+/*!
+
+ \class vpMutex
+
+ \ingroup group_core_mutex
+
+ \brief Class that allows protection by mutex.
+
+ \warning This class needs the pthread third-party library.
+*/
+class VISP_EXPORT vpMutex {
+public:
+ vpMutex() : m_mutex() {
+ pthread_mutex_init( &m_mutex, NULL );
+ }
+ void lock() {
+ pthread_mutex_lock( &m_mutex );
+ }
+ void unlock() {
+ pthread_mutex_unlock( &m_mutex );
+ }
+
+ /*!
+
+ \class vpScopedLock
+
+ \ingroup group_core_mutex
+
+ \brief Class that allows protection by mutex.
+
+ \warning This class needs the pthread third-party library.
+ */
+ class VISP_EXPORT vpScopedLock
+ {
+ private:
+ vpMutex & _mutex;
+
+// private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpScopedLock &operator=(const vpScopedLock &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpScopedLock(vpMutex & mutex)
+ : _mutex(mutex)
+ {
+ _mutex.lock();
+ }
+ ~vpScopedLock()
+ {
+ _mutex.unlock();
+ }
+ };
+private:
+ pthread_mutex_t m_mutex;
+};
+
+#endif
+
+#endif
diff --git a/modules/core/include/visp3/core/vpNetwork.h b/modules/core/include/visp3/core/vpNetwork.h
new file mode 100644
index 0000000..eb19164
--- /dev/null
+++ b/modules/core/include/visp3/core/vpNetwork.h
@@ -0,0 +1,495 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Network
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpNetwork_H
+#define vpNetwork_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpRequest.h>
+
+#include <vector>
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# include <unistd.h>
+# include <sys/socket.h>
+# include <netinet/in.h>
+# include <arpa/inet.h>
+# include <netdb.h>
+#else
+# include<io.h>
+//# include<winsock.h>
+# include<winsock2.h>
+//# pragma comment(lib, "ws2_32.lib") // Done by CMake in main CMakeLists.txt
+#endif
+
+
+/*!
+ \class vpNetwork
+
+ \ingroup group_core_network
+
+ \brief This class represents a Transmission Control Protocol (TCP) network.
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
+ on one computer to another program on another computer.
+
+ \warning This class shouldn't be used directly. You better use vpClient and
+ vpServer to simulate your network. Some exemples are provided in these classes.
+
+ \sa vpServer
+ \sa vpNetwork
+*/
+class VISP_EXPORT vpNetwork
+{
+protected:
+
+ struct vpReceptor{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int socketFileDescriptorReceptor;
+ socklen_t receptorAddressSize;
+#else
+ SOCKET socketFileDescriptorReceptor;
+ int receptorAddressSize;
+#endif
+ struct sockaddr_in receptorAddress;
+ std::string receptorIP;
+
+ vpReceptor() : socketFileDescriptorReceptor(0), receptorAddressSize(), receptorAddress(), receptorIP() {}
+ };
+
+ struct vpEmitter{
+ struct sockaddr_in emitterAddress;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int socketFileDescriptorEmitter;
+#else
+ SOCKET socketFileDescriptorEmitter;
+#endif
+ vpEmitter() : emitterAddress(), socketFileDescriptorEmitter(0)
+ {
+ emitterAddress.sin_family = AF_INET;
+ emitterAddress.sin_addr.s_addr = INADDR_ANY;
+ emitterAddress.sin_port = 0;
+ socketFileDescriptorEmitter = 0;
+ }
+ };
+
+ //######## PARAMETERS ########
+ //# #
+ //############################
+
+ vpEmitter emitter;
+ std::vector<vpReceptor> receptor_list;
+ fd_set readFileDescriptor;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int socketMax;
+#else
+ SOCKET socketMax;
+#endif
+
+ //Message Handling
+ std::vector<vpRequest*> request_list;
+
+ unsigned int max_size_message;
+ std::string separator;
+ std::string beginning;
+ std::string end;
+ std::string param_sep;
+
+ std::string currentMessageReceived;
+
+ struct timeval tv;
+ long tv_sec;
+ long tv_usec;
+
+ bool verboseMode;
+
+private:
+
+ std::vector<int> _handleRequests();
+ int _handleFirstRequest();
+
+ void _receiveRequest();
+ void _receiveRequestFrom(const unsigned int &receptorEmitting);
+ int _receiveRequestOnce();
+ int _receiveRequestOnceFrom(const unsigned int &receptorEmitting);
+
+public:
+
+ vpNetwork();
+ virtual ~vpNetwork();
+
+ void addDecodingRequest(vpRequest *);
+
+ int getReceptorIndex(const char *name);
+
+ /*!
+ Get the Id of the request at the index ind.
+
+ \param ind : Index of the request.
+
+ \return Id of the request.
+ */
+ std::string getRequestIdFromIndex(const int &ind){
+ if(ind >= (int)request_list.size() || ind < 0)
+ return "";
+ return request_list[(unsigned)ind]->getId();
+ }
+
+ /*!
+ Get the maximum size that the emitter can receive (in request mode).
+
+ \sa vpNetwork::setMaxSizeReceivedMessage()
+
+ \return Acutal max size value.
+ */
+ unsigned int getMaxSizeReceivedMessage(){ return max_size_message; }
+
+ void print(const char *id = "");
+
+ template<typename T>
+ int receive(T* object, const unsigned int &sizeOfObject = sizeof(T));
+ template<typename T>
+ int receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject = sizeof(T));
+
+ std::vector<int> receiveRequest();
+ std::vector<int> receiveRequestFrom(const unsigned int &receptorEmitting);
+ int receiveRequestOnce();
+ int receiveRequestOnceFrom(const unsigned int &receptorEmitting);
+
+ std::vector<int> receiveAndDecodeRequest();
+ std::vector<int> receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting);
+ int receiveAndDecodeRequestOnce();
+ int receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting);
+
+ void removeDecodingRequest(const char *);
+
+ template<typename T>
+ int send(T* object, const int unsigned &sizeOfObject = sizeof(T));
+ template<typename T>
+ int sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject = sizeof(T));
+
+ int sendRequest(vpRequest &req);
+ int sendRequestTo(vpRequest &req, const unsigned int &dest);
+
+ int sendAndEncodeRequest(vpRequest &req);
+ int sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest);
+
+ /*!
+ Change the maximum size that the emitter can receive (in request mode).
+
+ \sa vpNetwork::getMaxSizeReceivedMessage()
+
+ \param s : new maximum size value.
+ */
+ void setMaxSizeReceivedMessage(const unsigned int &s){ max_size_message = s;}
+
+ /*!
+ Change the time the emitter spend to check if he receives a message from a receptor.
+ Initially this value is set to 10usec.
+
+ \sa vpNetwork::setTimeoutUSec()
+
+ \param sec : new value in second.
+ */
+ void setTimeoutSec(const long &sec){ tv_sec = sec; }
+
+ /*!
+ Change the time the emitter spend to check if he receives a message from a receptor.
+ Initially this value is set to 10usec.
+
+ \sa vpNetwork::setTimeoutSec()
+
+ \param usec : new value in micro second.
+ */
+ void setTimeoutUSec(const long &usec){ tv_usec = usec; }
+
+ /*!
+ Set the verbose mode.
+
+ \param mode : Change the verbose mode. True to turn on, False to turn off.
+ */
+ void setVerbose(const bool &mode){ verboseMode = mode; }
+};
+
+//######## Definition of Template Functions ########
+//# #
+//##################################################
+
+/*!
+ Receives a object. The size of the received object is suppose to be the size of the type of the object.
+ Note that a received message can correspond to a deconnection signal.
+
+ \warning Using this function means that you know what kind of object you are suppose to receive,
+ and when you are suppose to receive.
+ If the emitter has several receptors. It might be a problem, and in that case you better use the
+ "request" option.
+
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+
+ \param object : Received object.
+ \param sizeOfObject : Size of the received object.
+
+ \return the number of bytes received, or -1 if an error occured.
+*/
+template<typename T>
+int vpNetwork::receive(T* object, const unsigned int &sizeOfObject)
+{
+ if(receptor_list.size() == 0)
+ {
+ if(verboseMode)
+ vpTRACE( "No receptor" );
+ return -1;
+ }
+
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+ FD_ZERO(&readFileDescriptor);
+
+ for(unsigned int i=0; i<receptor_list.size(); i++){
+ FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
+
+ if(i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
+ }
+
+ int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ int numbytes = 0;
+
+ if(value == -1){
+ if(verboseMode)
+ vpERROR_TRACE( "Select error" );
+ return -1;
+ }
+ else if(value == 0){
+ //Timeout
+ return 0;
+ }
+ else{
+ for(unsigned int i=0; i<receptor_list.size(); i++){
+ if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ numbytes = recv(receptor_list[i].socketFileDescriptorReceptor, (char*)(void*)object, sizeOfObject, 0);
+#else
+ numbytes = recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, (char*)(void*)object, (int)sizeOfObject, 0);
+#endif
+ if(numbytes <= 0)
+ {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
+ receptor_list.erase(receptor_list.begin()+(int)i);
+ return numbytes;
+ }
+
+ break;
+ }
+ }
+ }
+
+ return numbytes;
+}
+
+/*!
+ Receives a object from a receptor, by specifying its size or not.
+ Note that a received message can correspond to a deconnection signal.
+
+ \warning Using this function means that you know what kind of object you are suppose to receive,
+ and when you are suppose to receive.
+ If the emitter has several receptors. It might be a problem, and in that case you better use the
+ "request" mode.
+
+ \sa vpNetwork::getReceptorIndex()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param object : Received object.
+ \param receptorEmitting : Index of the receptor emitting the message.
+ \param sizeOfObject : Size of the received object.
+
+ \return the number of bytes received, or -1 if an error occured.
+*/
+template<typename T>
+int vpNetwork::receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject)
+{
+ if(receptor_list.size() == 0 || receptorEmitting > (unsigned int)receptor_list.size()-1 )
+ {
+ if(verboseMode)
+ vpTRACE( "No receptor at the specified index" );
+ return -1;
+ }
+
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+ FD_ZERO(&readFileDescriptor);
+
+ socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;
+ FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);
+
+ int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ int numbytes = 0;
+
+ if(value == -1){
+ if(verboseMode)
+ vpERROR_TRACE( "Select error" );
+ return -1;
+ }
+ else if(value == 0){
+ //timeout
+ return 0;
+ }
+ else{
+ if(FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor)){
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ numbytes = recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, sizeOfObject, 0);
+#else
+ numbytes = recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, (int)sizeOfObject, 0);
+#endif
+ if(numbytes <= 0)
+ {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;
+ receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
+ return numbytes;
+ }
+ }
+ }
+
+ return numbytes;
+}
+
+/*!
+ Send an object. The size of the received object is suppose to be the size of its type.
+ Note that sending object containing pointers, virtual methods, etc, won't probably work.
+
+ \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
+ and when it is suppose to receive.
+ If the emitter has several receptors. It might be a problem, and in that case you better use the
+ "request" option.
+
+ \sa vpNetwork::sendTo()
+ \sa vpNetwork::sendRequest()
+ \sa vpNetwork::sendRequestTo()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+
+ \param object : Received object.
+ \param sizeOfObject : Size of the object
+
+ \return The number of bytes sent, or -1 if an error happened.
+*/
+template<typename T>
+int vpNetwork::send(T* object, const unsigned int &sizeOfObject)
+{
+ if(receptor_list.size() == 0)
+ {
+ if(verboseMode)
+ vpTRACE( "No receptor !" );
+ return 0;
+ }
+
+ int flags = 0;
+//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
+#if defined(__linux__)
+ flags = MSG_NOSIGNAL; // Only for Linux
+#endif
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ return sendto(receptor_list[0].socketFileDescriptorReceptor, (const char*)(void*)object, sizeOfObject,
+ flags, (sockaddr*) &receptor_list[0].receptorAddress,receptor_list[0].receptorAddressSize);
+#else
+ return sendto(receptor_list[0].socketFileDescriptorReceptor, (const char*)(void*)object, (int)sizeOfObject,
+ flags, (sockaddr*) &receptor_list[0].receptorAddress,receptor_list[0].receptorAddressSize);
+#endif
+
+}
+
+/*!
+ Send an object. The size has to be specified.
+
+ \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
+ and when it is suppose to receive.
+ If the emitter has several receptors. It might be a problem, and in that case you better use the
+ "request" option.
+
+ \sa vpNetwork::getReceptorIndex()
+ \sa vpNetwork::send()
+ \sa vpNetwork::sendRequest()
+ \sa vpNetwork::sendRequestTo()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+
+ \param object : Object to send.
+ \param dest : Index of the receptor that you are sending the object.
+ \param sizeOfObject : Size of the object.
+
+ \return The number of bytes sent, or -1 if an error happened.
+*/
+template<typename T>
+int vpNetwork::sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject)
+{
+ if(receptor_list.size() == 0 || dest > (unsigned int)receptor_list.size()-1 )
+ {
+ if(verboseMode)
+ vpTRACE( "No receptor at the specified index." );
+ return 0;
+ }
+
+ int flags = 0;
+//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
+#if defined(__linux__)
+ flags = MSG_NOSIGNAL; // Only for Linux
+#endif
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ return sendto(receptor_list[dest].socketFileDescriptorReceptor, (const char*)(void*)object, sizeOfObject,
+ flags, (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
+#else
+ return sendto(receptor_list[dest].socketFileDescriptorReceptor, (const char*)(void*)object, (int)sizeOfObject,
+ flags, (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
+#endif
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpNoise.h b/modules/core/include/visp3/core/vpNoise.h
new file mode 100644
index 0000000..584e87a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpNoise.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generation of random number with uniform and normal probability density.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpNoise_hh
+#define vpNoise_hh
+
+#include <visp3/core/vpGaussRand.h>
+
+#endif
diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h
new file mode 100644
index 0000000..8e9d98e
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h
@@ -0,0 +1,251 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pixel to meter conversion.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#ifndef vpPixelMeterConversion_H
+#define vpPixelMeterConversion_H
+
+
+/*!
+ \file vpPixelMeterConversion.h
+ \brief pixel to meter conversion
+
+*/
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImagePoint.h>
+
+/*!
+ \class vpPixelMeterConversion
+
+ \ingroup group_core_camera
+
+ \brief Conversion from pixel coordinates \f$(u,v)\f$ to normalized
+ coordinates \f$(x,y)\f$ in meter.
+
+ This class relates to vpCameraParameters.
+
+*/
+class VISP_EXPORT vpPixelMeterConversion
+{
+public:
+
+/*!
+ \brief Point coordinates conversion from pixel coordinates
+ \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param u,v : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
+ projection with distortion.
+*/
+inline static void
+convertPoint(const vpCameraParameters &cam,
+ const double &u, const double &v,
+ double &x, double &y)
+{
+ switch(cam.projModel){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ convertPointWithoutDistortion(cam,u,v,x,y);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ convertPointWithDistortion(cam,u,v,x,y);
+ break;
+ }
+}
+
+/*!
+ \brief Point coordinates conversion from pixel coordinates
+ Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
+ projection with distortion.
+*/
+inline static void
+convertPoint(const vpCameraParameters &cam,
+ const vpImagePoint &iP,
+ double &x, double &y)
+{
+ switch(cam.projModel){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ convertPointWithoutDistortion(cam,iP,x,y);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ convertPointWithDistortion(cam,iP,x,y);
+ break;
+ }
+}
+
+/*!
+ \brief Point coordinates conversion without distortion from pixel coordinates
+ \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param u,v : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
+*/
+inline static void
+convertPointWithoutDistortion(
+ const vpCameraParameters &cam,
+ const double &u, const double &v,
+ double &x, double &y)
+{
+ x = (u - cam.u0)*cam.inv_px ;
+ y = (v - cam.v0)*cam.inv_py ;
+}
+
+/*!
+ \brief Point coordinates conversion without distortion from pixel coordinates
+ Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
+*/
+inline static void
+convertPointWithoutDistortion(
+ const vpCameraParameters &cam,
+ const vpImagePoint &iP,
+ double &x, double &y)
+{
+ x = (iP.get_u() - cam.u0)*cam.inv_px ;
+ y = (iP.get_v() - cam.v0)*cam.inv_py ;
+}
+
+/*!
+ \brief Point coordinates conversion with distortion from pixel coordinates
+ \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param u,v : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
+*/
+inline static void
+convertPointWithDistortion(
+ const vpCameraParameters &cam,
+ const double &u, const double &v,
+ double &x, double &y)
+{
+ double r2 = 1.+cam.kdu*(vpMath::sqr((u - cam.u0)*cam.inv_px) +
+ vpMath::sqr((v-cam.v0)*cam.inv_py));
+ x = (u - cam.u0)*r2*cam.inv_px ;
+ y = (v - cam.v0)*r2*cam.inv_py ;
+}
+
+/*!
+ \brief Point coordinates conversion with distortion from pixel coordinates
+ Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x,y : output coordinates in meter.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
+*/
+inline static void
+convertPointWithDistortion(
+ const vpCameraParameters &cam,
+ const vpImagePoint &iP,
+ double &x, double &y)
+{
+ double r2 = 1.+cam.kdu*(vpMath::sqr((iP.get_u() - cam.u0)*cam.inv_px) +
+ vpMath::sqr((iP.get_v()-cam.v0)*cam.inv_py));
+ x = (iP.get_u() - cam.u0)*r2*cam.inv_px ;
+ y = (iP.get_v() - cam.v0)*r2*cam.inv_py ;
+}
+
+ //! line coordinates conversion (rho,theta)
+ static void convertLine(const vpCameraParameters &cam,
+ const double &rho_p, const double &theta_p,
+ double &rho_m, double &theta_m) ;
+
+
+ static void convertMoment(const vpCameraParameters &cam,
+ unsigned int order,
+ const vpMatrix &moment_pixel,
+ vpMatrix &moment_meter) ;
+} ;
+
+#endif
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h
new file mode 100644
index 0000000..919042f
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPlane.h
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Plane geometrical structure.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpPlane_hh
+#define vpPlane_hh
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+/*!
+ \class vpPlane
+
+ \ingroup group_core_geometry
+
+ \brief This class defines the container for a plane geometrical structure.
+
+ A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
+ (x,y,z) are the coordinates of a point and where \f$[A,B,C]^T\f$ is a normal
+ vector of the plane.
+
+*/
+class VISP_EXPORT vpPlane
+{
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ // for backward compatibility
+ public:
+#else
+ private:
+#endif
+ double A,B,C,D ;
+
+public:
+ typedef enum {
+ object_frame,
+ camera_frame
+ } vpPlaneFrame;
+ vpPlane() ;
+ vpPlane(const vpPlane& P) ;
+ vpPlane(const double A, const double B,const double C,const double D) ;
+ vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame=camera_frame) ;
+ vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame) ;
+ void init(const vpPoint& P, const vpPoint& Q, const vpPoint& R, vpPlaneFrame frame=camera_frame) ;
+ void init(const vpColVector& P, const vpColVector &n) ;
+ void init(const vpPlane& P) ;
+
+ // SET the parameter
+ /*! Set plane parameter A. */
+ inline void setA(const double a) { this->A = a ; }
+ /*! Set plane parameter B. */
+ inline void setB(const double b) { this->B = b ; }
+ /*! Set plane parameter C. */
+ inline void setC(const double c) { this->C = c ; }
+ /*! Set plane parameter D. */
+ inline void setD(const double d) { this->D = d ; }
+ /*! Set plane parameters A, B, C, D. */
+ inline void setABCD(const double a, const double b,
+ const double c, const double d)
+ {
+ this->A = a;
+ this->B = b;
+ this->C = c;
+ this->D = d;
+ }
+
+ vpPlane& operator =(const vpPlane& f) ;
+
+ // GET information
+ /*! \return The value of the plane parameter A. */
+ double getA() const { return A ; }
+ /*! \return The value of the plane parameter B. */
+ double getB() const { return B ; }
+ /*! \return The value of the plane parameter C. */
+ double getC() const { return C ; }
+ /*! \return The value of the plane parameter D. */
+ double getD() const { return D ; }
+
+ /*!
+
+ \return Return the four dimension vector \f$[A,B,C,D]^T\f$
+ corresponding to the plane parameters.
+
+ */
+ inline vpColVector getABCD() const {
+ vpColVector n(4);
+ n[0]=A;
+ n[1]=B;
+ n[2]=C;
+ n[3]=D;
+
+ return n;
+ }
+ /*!
+
+ \warning This method is provided for compatibility with the
+ previous versions. Users should now use getABCD().
+
+ \return Return the four dimension vector \f$[A,B,C,D]^T\f$
+ corresponding to the plane parameters.
+
+ \sa getABCD()
+ */
+ inline vpColVector abcd() const {
+ vpColVector n(4);
+ n[0]=A;
+ n[1]=B;
+ n[2]=C;
+ n[3]=D;
+
+ return n;
+ }
+
+ vpColVector getNormal() const;
+ void getNormal(vpColVector &n) const;
+
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p);
+
+ // Operation with Plane
+ void projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const ;
+
+ double rayIntersection(const vpPoint &M0,
+ const vpPoint &M1,
+ vpColVector &H )const ;
+
+ double getIntersection(const vpColVector &M1,vpColVector &H )const ;
+ void changeFrame(const vpHomogeneousMatrix &cMo);
+
+} ;
+
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpPoint.h b/modules/core/include/visp3/core/vpPoint.h
new file mode 100644
index 0000000..5908f2b
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPoint.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Point feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpPoint_H
+#define vpPoint_H
+
+/*!
+ \file vpPoint.h
+ \brief class that defines what is a point
+*/
+
+class vpHomogeneousMatrix;
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpColor.h>
+
+
+/*!
+ \class vpPoint
+ \ingroup group_core_geometry
+ \brief Class that defines what is a point.
+*/
+class VISP_EXPORT vpPoint : public vpForwardProjection
+{
+
+public:
+ //! Basic constructor.
+ vpPoint();
+ vpPoint(double oX, double oY, double oZ);
+ vpPoint(const vpColVector &P);
+ vpPoint(const std::vector<double> &P);
+ //! Destructor.
+ virtual ~vpPoint() {}
+
+public:
+
+ // Compute the 3D coordinates _cP (camera frame)
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ vpPoint *duplicate() const ;
+
+ // Get coordinates
+ double get_X() const;
+ double get_Y() const;
+ double get_Z() const;
+ double get_W() const;
+ double get_oX() const;
+ double get_oY() const;
+ double get_oZ() const;
+ double get_oW() const;
+ double get_x() const;
+ double get_y() const;
+ double get_w() const;
+
+ void getWorldCoordinates(double& oX, double& oY, double& oZ);
+ void getWorldCoordinates(vpColVector &P);
+ vpColVector getWorldCoordinates(void);
+ void getWorldCoordinates(std::vector<double> &P);
+
+ //! Basic construction.
+ void init() ;
+
+ friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, const vpPoint& vpp);
+ vpPoint& operator=(const vpPoint& vpp);
+
+ //! Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP, output : the 2D coordinates _p.
+ void projection(const vpColVector &_cP, vpColVector &_p);
+
+ void projection();
+
+ // Set coordinates
+ void set_X(const double X);
+ void set_Y(const double Y);
+ void set_Z(const double Z);
+ void set_W(const double W);
+ void set_oX(const double oX);
+ void set_oY(const double oY);
+ void set_oZ(const double oZ);
+ void set_oW(const double oW);
+ void set_x(const double x);
+ void set_y(const double y);
+ void set_w(const double w);
+
+ void setWorldCoordinates(const double oX, const double oY, const double oZ);
+ void setWorldCoordinates(const vpColVector &P);
+ void setWorldCoordinates(const std::vector<double> &P);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpPolygon.h b/modules/core/include/visp3/core/vpPolygon.h
new file mode 100644
index 0000000..5e42c78
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPolygon.h
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a generic 2D polygon.
+ *
+ * Author:
+ * Amaury Dame
+ * Nicolas Melchior
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#ifndef vpPolygon_h
+#define vpPolygon_h
+
+#include <vector>
+
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpCameraParameters.h>
+
+/*!
+ \class vpPolygon
+ \ingroup group_core_geometry
+ \brief Defines a generic 2D polygon.
+
+ A polygon is internally represented by N 2D points.
+
+ By default three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint
+ class documentation for more details about the frame) are used \f$ (0,0) \f$,
+ \f$ (1,0) \f$ and \f$ (0,1) \f$.
+
+ The code bellow shows how to manipulate a polygon.
+\code
+#include <iostream>
+
+#include <visp3/core/vpPolygon.h>
+
+int main()
+{
+ std::vector<vpImagePoint> corners;
+
+ // Initialize the corners vector with 4 points
+ corners.push_back( vpImagePoint( 50, 100) );
+ corners.push_back( vpImagePoint( 50, 300) );
+ corners.push_back( vpImagePoint(200, 300) );
+ corners.push_back( vpImagePoint(200, 100) );
+
+ // Initialize a polygon with the corners
+ vpPolygon polygon(corners);
+
+ // Get the polygon bounding box
+ vpRect bbox = polygon.getBoundingBox();
+ std::cout << "Bounding box: " << bbox.getTopLeft() << " to " << bbox.getBottomRight() << std::endl;
+
+ // Get the polygon surface and center
+ std::cout << "Area: " << polygon.getArea() << std::endl;
+ std::cout << "Center: " << polygon.getCenter() << std::endl;
+
+ // Check if a point is inside the polygon
+ vpImagePoint ip(550, 200);
+ std::cout << "The point " << ip << " is " << (polygon.isInside(ip) ? "inside":"outside") << " the polygon" << std::endl;
+
+ return 0;
+}
+\endcode
+*/
+class VISP_EXPORT vpPolygon
+{
+ protected:
+ //! Collection of image points containing the corners.
+ std::vector<vpImagePoint> _corners;
+ //! Center of the polygon. It is automatically computed when the corners are set.
+ vpImagePoint _center;
+ //! Area of the polygon.
+ double _area;
+ //! Flag to indicate whether the polygon is a good polygon (ie. it has more than two corners, ...)
+ bool _goodPoly;
+ //! Boumding box containing the polygon.
+ vpRect _bbox;
+
+ public:
+ vpPolygon();
+ vpPolygon(const std::vector<vpImagePoint>& corners);
+ vpPolygon(const vpPolygon &poly);
+ virtual ~vpPolygon();
+
+ vpPolygon &operator=(const vpPolygon& poly);
+
+ void buildFrom(const std::vector<vpImagePoint>& corners);
+ void buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam);
+ void initClick(const vpImage<unsigned char>& I);
+
+ bool isInside(const vpImagePoint &iP);
+
+ void display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness=1) const;
+
+ /*!
+ Get the corners of the polygon.
+
+ \return A reference to the corners.
+ */
+ const std::vector<vpImagePoint>& getCorners() const {
+ return _corners;
+ }
+
+
+ /*!
+ Return the area of the polygon.
+ The area is computed when the polygon is built from the corners.
+
+ \return The area of the polygon.
+ */
+ inline double getArea() const{
+ return this->_area;
+ }
+
+ /*!
+ Return the center of the polygon.
+ The center is computed when the polygon is built from the corners.
+
+ \return The area of the polygon.
+ */
+ inline vpImagePoint getCenter() const{
+ return this->_center;
+ }
+
+ /*!
+ Return the bounding box. The bounding box is the smallest rectangle
+ containing all the polygon.
+
+ \return The bounding box of the polygon.
+ */
+ inline vpRect getBoundingBox() const {
+ return _bbox;
+ }
+
+ protected:
+ void init(const std::vector<vpImagePoint>& corners);
+ void updateArea();
+ void updateCenter();
+ void updateBoundingBox();
+
+ private:
+ bool testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4);
+
+ //###################
+ // Static Functions
+ //###################
+
+ public:
+ static bool isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j);
+ private:
+ static bool intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i, const double &j, const double &i_test, const double &j_test);
+};
+
+#endif
+
diff --git a/modules/core/include/visp3/core/vpPolygon3D.h b/modules/core/include/visp3/core/vpPolygon3D.h
new file mode 100644
index 0000000..3d8848c
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPolygon3D.h
@@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implements a polygon of the model used by the model-based tracker.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPolygon3D.h
+ \brief Implements a 3D polygon with render functionnalities like clipping.
+*/
+
+#ifndef vpPolygon3D_HH
+#define vpPolygon3D_HH
+
+#include <vector>
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+
+/*!
+ \class vpPolygon3D
+ \ingroup group_core_geometry
+ \brief Implements a 3D polygon with render functionnalities like clipping.
+
+*/
+class VISP_EXPORT vpPolygon3D
+{
+public:
+ typedef enum
+ {
+ NO_CLIPPING = 0,
+ NEAR_CLIPPING = 1,
+ FAR_CLIPPING = 2,
+ LEFT_CLIPPING = 4,
+ RIGHT_CLIPPING = 8,
+ UP_CLIPPING = 16,
+ DOWN_CLIPPING = 32,
+ FOV_CLIPPING = 60,
+ ALL_CLIPPING = 63
+ } vpPolygon3DClippingType;
+
+public:
+ //! Number of points used to define the polygon.
+ unsigned int nbpt;
+ //! Number of corners inside the image during the last call to getNbCornerInsideImage
+ unsigned int nbCornersInsidePrev;
+ //! corners in the object frame
+ vpPoint *p;
+ //! Region of interest clipped
+ std::vector<std::pair<vpPoint,unsigned int> > polyClipped;
+ //! Clipping flag
+ unsigned int clippingFlag;
+ //! Distance for near clipping
+ double distNearClip;
+ //! Distance for near clipping
+ double distFarClip;
+
+private:
+ bool getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
+ vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
+ const vpColVector &normal,
+ const unsigned int &flag);
+
+ bool getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
+ vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
+ const unsigned int &flag, const double &distance);
+
+public:
+ vpPolygon3D() ;
+ vpPolygon3D(const vpPolygon3D& mbtp) ;
+ virtual ~vpPolygon3D() ;
+
+ void addPoint(const unsigned int n, const vpPoint &P) ;
+
+ void changeFrame(const vpHomogeneousMatrix &cMo) ;
+
+ void computePolygonClipped(const vpCameraParameters &cam = vpCameraParameters());
+
+ /*!
+ Get the clipping used.
+
+ \sa vpPolygon3DClipping
+
+ \return Clipping flags.
+ */
+ inline unsigned int getClipping() const { return clippingFlag; }
+
+ /*!
+ Get the far distance for clipping.
+
+ \return Far clipping value.
+ */
+ inline double getFarClippingDistance() const { return distFarClip; }
+
+ /*!
+ Return the number of corners.
+
+ \return number of corner of the face
+ */
+ inline unsigned int getNbPoint() const {return nbpt ;}
+
+ /*!
+ Return the number of corners at the previous computation.
+
+ \return number of corner of the face at the previous computation
+ */
+ inline unsigned int getNbCornerInsidePrevImage() const { return nbCornersInsidePrev; }
+
+ unsigned int getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam);
+
+ /*!
+ Get the near distance for clipping.
+
+ \return Near clipping value.
+ */
+ inline double getNearClippingDistance() const { return distNearClip; }
+
+ vpPoint & getPoint(const unsigned int _index);
+
+ std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam);
+
+ std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo);
+
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi);
+
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi, const vpHomogeneousMatrix &cMo);
+
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi);
+
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo);
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ vp_deprecated void getRoiClipped(std::vector<vpPoint> &points);
+ //@}
+#endif
+
+ void getPolygonClipped(std::vector<std::pair<vpPoint,unsigned int> > &poly);
+
+ void getPolygonClipped(std::vector<vpPoint> &poly);
+
+ vpPolygon3D& operator=(const vpPolygon3D& mbtp) ;
+
+ /*!
+ Specify which clipping to use.
+
+ \sa vpPolygon3DClipping
+
+ \param flags : New clipping flags.
+ */
+ inline void setClipping(const unsigned int &flags) { clippingFlag = flags; }
+
+ /*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+ */
+ inline void setFarClippingDistance(const double &dist) { distFarClip = dist; clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);}
+
+ virtual void setNbPoint(const unsigned int nb) ;
+
+ /*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+ */
+ inline void setNearClippingDistance(const double &dist) { distNearClip = dist; clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);}
+
+public:
+ static void getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo,
+ const unsigned int &clippingFlags, const vpCameraParameters &cam = vpCameraParameters(),
+ const double &znear = 0.001, const double &zfar = 100 );
+ static void getMinMaxRoi(const std::vector<vpImagePoint> &roi, int & i_min, int &i_max, int &j_min, int &j_max);
+ static bool roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners);
+};
+
+#endif
+
diff --git a/modules/core/include/visp3/core/vpPoseVector.h b/modules/core/include/visp3/core/vpPoseVector.h
new file mode 100644
index 0000000..f25de5c
--- /dev/null
+++ b/modules/core/include/visp3/core/vpPoseVector.h
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpPOSEVECTOR_H
+#define vpPOSEVECTOR_H
+
+/*!
+ \file vpPoseVector.h
+
+ \brief Pose representation. A pose is a 6 dimension vector [t,tu]^T
+ where tu is a rotation vector (theta u representation) and t is a
+ translation vector.
+*/
+
+class vpRotationMatrix;
+class vpHomogeneousMatrix;
+class vpTranslationVector;
+class vpThetaUVector;
+class vpRowVector;
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+
+/*!
+ \class vpPoseVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a pose vector and operations on poses.
+
+ The vpPose class implements a complete representation of every rigid motion in the
+ euclidian space.
+
+ The vpPose class is derived from vpArray2D<double>.
+
+ The pose is composed of a translation and a rotation
+ minimaly represented by a 6 dimension pose vector as: \f[ ^{a}{\bf
+ r}_b = [^{a}{\bf t}_{b},\theta {\bf u}]^\top \in R^6\f]
+
+ where \f$ ^{a}{\bf r}_b \f$ is the pose from frame \f$ a \f$ to
+ frame \f$ b \f$, with \f$ ^{a}{\bf t}_{b} \f$ being the translation
+ vector between these frames along the x,y,z
+ axis and \f$\theta \bf u \f$, the axis-angle representation of the
+ rotation \f$^{a}\bf{R}_{b}\f$ between these frames.
+
+ Translations are expressed in meters, while the angles in the \f$\theta {\bf u}\f$
+ axis-angle representation are expressed in radians.
+
+ To know more about the \f$\theta \bf u\f$ rotation representation,
+ see vpThetaUVector documentation.
+
+*/
+class VISP_EXPORT vpPoseVector : public vpArray2D<double>
+{
+public:
+ // constructor
+ vpPoseVector() ;
+ // constructor from 3 angles (in radian)
+ vpPoseVector(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz) ;
+ // constructor convert an homogeneous matrix in a pose
+ vpPoseVector(const vpHomogeneousMatrix& M) ;
+ // constructor convert a translation and a "thetau" vector into a pose
+ vpPoseVector(const vpTranslationVector& tv,
+ const vpThetaUVector& tu) ;
+ // constructor convert a translation and a rotation matrix into a pose
+ vpPoseVector(const vpTranslationVector& tv,
+ const vpRotationMatrix& R) ;
+ /*!
+ Destructor.
+ */
+ virtual ~vpPoseVector() {};
+
+ vpPoseVector buildFrom(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz) ;
+ // convert an homogeneous matrix in a pose
+ vpPoseVector buildFrom(const vpHomogeneousMatrix& M) ;
+ // convert a translation and a "thetau" vector into a pose
+ vpPoseVector buildFrom(const vpTranslationVector& tv,
+ const vpThetaUVector& tu) ;
+ // convert a translation and a rotation matrix into a pose
+ vpPoseVector buildFrom(const vpTranslationVector& tv,
+ const vpRotationMatrix& R) ;
+
+ void extract(vpRotationMatrix &R) const;
+ void extract(vpThetaUVector &tu) const;
+ void extract(vpTranslationVector &tv) const;
+ void extract(vpQuaternionVector& q) const;
+
+ vpRotationMatrix getRotationMatrix() const;
+ vpThetaUVector getThetaUVector() const;
+ vpTranslationVector getTranslationVector() const;
+
+ // Load an homogeneous matrix from a file
+ void load(std::ifstream &f) ;
+
+ /*!
+ Set the value of an element of the pose vector: r[i] = x.
+
+ \param i : Pose vector element index
+
+ \code
+ // Create a pose vector with translation and rotation set to zero
+ vpPoseVector r;
+
+ // Initialize the pose vector
+ r[0] = 1;
+ r[1] = 2;
+ r[2] = 3;
+ r[3] = M_PI;
+ r[4] = -M_PI;
+ r[5] = 0;
+ \endcode
+
+ This code produces the same effect:
+ \code
+ vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
+ \endcode
+
+ */
+ inline double &operator [](unsigned int i) { return *(data + i); }
+ /*!
+ Get the value of an element of the pose vector: x = r[i].
+
+ \param i : Pose vector element index
+
+ \code
+ vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
+
+ double tx,ty,tz; // Translation
+ double tux, tuy,tuz; // Theta u rotation
+ tx = r[0];
+ ty = r[1];
+ tz = r[2];
+ tux = r[3];
+ tuy = r[4];
+ tuz = r[5];
+ \endcode
+ */
+ inline const double &operator [](unsigned int i) const { return *(data+i); }
+
+ // Print a vector [T thetaU] thetaU in degree
+ void print() const;
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+
+ /*!
+ This function is not applicable to a pose vector that is always a
+ 6-by-1 column vector.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize a pose vector"));
+ };
+
+ // Save an homogeneous matrix in a file
+ void save(std::ofstream &f) const ;
+ void set(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz);
+ vpRowVector t() const;
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ //@}
+#endif
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpQuaternionVector.h b/modules/core/include/visp3/core/vpQuaternionVector.h
new file mode 100644
index 0000000..5247a9f
--- /dev/null
+++ b/modules/core/include/visp3/core/vpQuaternionVector.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Quaternion definition.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+
+
+#ifndef __QUATERNIONVECTOR_H__
+#define __QUATERNIONVECTOR_H__
+
+/*!
+ \file vpQuaternionVector.h
+
+ \brief Class that consider the case of a quaternion and basic
+ operations on it.
+
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpThetaUVector.h>
+
+
+/*!
+ \class vpQuaternionVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation vector as quaternion angle
+ minimal representation.
+
+ Defines a quaternion and its basic operations.
+
+ The vpQuaternionVector class is derived from vpRotationVector.
+
+ A quaternion is defined by four values: \f${\bf q} = (x, y, z, w)\f$.
+
+ This class allows to compute a quaternion from a rotation matrix
+ using either vpQuaternionVector(const vpRotationMatrix &) constructor
+ or buildFrom() method.
+
+ It also defines common operations on a quaternion such as:
+ - multiplication (scalar and quaternion)
+ - addition
+ - substraction.
+
+ */
+class VISP_EXPORT vpQuaternionVector : public vpRotationVector
+{
+private:
+ static const double minimum;
+public:
+ vpQuaternionVector();
+ vpQuaternionVector(const vpQuaternionVector &q);
+ vpQuaternionVector(const double qx, const double qy, const double qz,const double qw) ;
+ vpQuaternionVector(const vpRotationMatrix &R);
+ vpQuaternionVector(const vpThetaUVector& tu) ;
+
+ //! Destructor.
+ virtual ~vpQuaternionVector() {};
+
+ vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw) ;
+ vpQuaternionVector buildFrom(const vpRotationMatrix& R);
+ vpQuaternionVector buildFrom(const vpThetaUVector& tu);
+ void set(const double x, const double y, const double z, const double w) ;
+
+ double x() const;
+ double y() const;
+ double z() const;
+ double w() const;
+
+ vpQuaternionVector operator+(const vpQuaternionVector &q) const;
+ vpQuaternionVector operator-(const vpQuaternionVector &q) const;
+ vpQuaternionVector operator-() const;
+ vpQuaternionVector operator*(const double l) const;
+ vpQuaternionVector operator*(const vpQuaternionVector &rq) const;
+ vpQuaternionVector operator/(const double l) const;
+
+ vpQuaternionVector conjugate() const;
+ vpQuaternionVector inverse() const;
+ double magnitude() const;
+ void normalize();
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h
new file mode 100644
index 0000000..54d3f35
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRGBa.h
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * RGBA pixel.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRGBa_h
+#define vpRGBa_h
+
+/*!
+ \file vpRGBa.h
+ \brief Define the object vpRGBa that is used to build color
+ images (it define a RGB 32 bits structure, fourth byte is not used)
+*/
+
+#include <visp3/core/vpColVector.h>
+
+
+/*!
+ \class vpRGBa
+
+ \ingroup group_core_image
+
+ \brief Class that defines a RGB 32 bits structure.
+
+ Define the object vpRGBa that is used to build color
+ images (it define a RGB 32 bits structure, fourth byte is not used)
+
+ \sa vpImage
+*/
+class VISP_EXPORT vpRGBa
+{
+public:
+ /*!
+ Basic constructor.
+
+ Build a black value.
+
+ */
+ inline vpRGBa() : R(0), G(0), B(0), A(0) {};
+
+ /*!
+ Constructor.
+
+ Initialize the color with R, G, B, A values.
+
+ \param r : Red value.
+ \param g : Green value.
+ \param b : Blue value.
+ \param a : Additional value.
+
+ */
+ inline vpRGBa(const unsigned char &r, const unsigned char &g,
+ const unsigned char &b, const unsigned char &a=0)
+ : R(r), G(g), B(b), A(a) {};
+
+
+ /*!
+ Constructor.
+
+ Initialize all the R, G, B, A components to \e v.
+
+ \param v : Value to set.
+
+ */
+ inline vpRGBa(const unsigned char &v) : R(v), G(v), B(v), A(v) {};
+
+
+ /*!
+ Copy constructor.
+ */
+ inline vpRGBa(const vpRGBa &v) : R(v.R), G(v.G), B(v.B), A(v.A) {};
+
+ /*!
+ Create a RGBa value from a 4 dimension column vector.
+
+ R=v[0]
+ G=v[1]
+ B=v[2]
+ A=v[3]
+
+ */
+ inline vpRGBa(const vpColVector &v) : R(0), G(0), B(0), A(0)
+ {
+ *this = v;
+ };
+
+ // We cannot add here the following destructor without changing the hypothesis that the size of this class is 4.
+ // With the destructor it becomes 16 that does break a lot of things arround image conversions
+ // virtual ~vpRGBa() {}; // Not to implement
+
+ vpRGBa & operator=(const unsigned char &v) ;
+ vpRGBa & operator=(const vpRGBa &v) ;
+ vpRGBa & operator=(const vpColVector &v) ;
+ bool operator==(const vpRGBa &v);
+ bool operator!=(const vpRGBa &v);
+
+ vpColVector operator-(const vpRGBa &v) const;
+ vpRGBa operator+(const vpRGBa &v) const;
+ vpColVector operator-(const vpColVector &v) const;
+ vpColVector operator+(const vpColVector &v) const;
+ vpColVector operator*(const float &v) const;
+ vpColVector operator*(const double &v) const;
+
+ bool operator<(const vpRGBa &v) const;
+ bool operator>(const vpRGBa &v) const;
+
+ public:
+ unsigned char R ; //!< Red component.
+ unsigned char G ; //!< Green component.
+ unsigned char B ; //!< Blue component.
+ unsigned char A ; //!< Additionnal component.
+
+ friend VISP_EXPORT vpRGBa operator*(const double &x, const vpRGBa &rgb);
+
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpRansac.h b/modules/core/include/visp3/core/vpRansac.h
new file mode 100644
index 0000000..fdceab7
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRansac.h
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Ransac robust algorithm.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpRansac.h
+
+ template class for
+*/
+
+#ifndef vpRANSAC_HH
+#define vpRANSAC_HH
+
+
+
+#include <visp3/core/vpUniRand.h> // random number generation
+#include <visp3/core/vpDebug.h> // debug and trace
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <ctime>
+/*!
+ \class vpRansac
+ \ingroup group_core_robust
+
+ \brief This class is a generic implementation of the Ransac algorithm. It
+ cannot be used alone.
+
+ RANSAC is described in \cite Fischler81 and \cite Hartley01a.
+
+ The code of this class is inspired by :
+ Peter Kovesi
+ School of Computer Science & Software Engineering
+ The University of Western Australia
+ pk at csse uwa edu au
+ http://www.csse.uwa.edu.au/~pk
+
+ \sa vpHomography
+
+ */
+template <class vpTransformation>
+class vpRansac
+{
+public:
+ static bool ransac(unsigned int npts,
+ vpColVector &x,
+ unsigned int s, double t,
+ vpColVector &model,
+ vpColVector &inliers,
+ int consensus = 1000,
+ double areaThreshold = 0.0,
+ const int maxNbumbersOfTrials = 10000);
+};
+
+/*!
+ \brief
+ RANSAC - Robustly fits a model to data with the RANSAC algorithm
+
+ \param npts : The number of data points.
+
+ \param x : Data sets to which we are seeking to fit a model M It is assumed
+ that x is of size [d x Npts] where d is the dimensionality of the data and
+ npts is the number of data points.
+
+ \param s : The minimum number of samples from x required by fitting fn to
+ fit a model. Value should be greater or equal to 4.
+
+ \param t : The distance threshold between data point and the model used to
+ decide whether a point is an inlier or not.
+
+ \param M : The model having the greatest number of inliers.
+
+ \param inliers : An array of indices of the elements of x that were the
+ inliers for the best model.
+
+ \param consensus : Consensus
+
+ \param areaThreshold : Not used.
+
+ \param maxNbumbersOfTrials : Maximum number of trials. Even if a solution is
+ not found, the method is stopped.
+
+*/
+
+template <class vpTransformation>
+bool
+vpRansac<vpTransformation>::ransac(unsigned int npts, vpColVector &x,
+ unsigned int s, double t,
+ vpColVector &M,
+ vpColVector &inliers,
+ int consensus,
+ double /* areaThreshold */,
+ const int maxNbumbersOfTrials)
+{
+ /* bool isplanar; */
+ /* if (s == 4) isplanar = true; */
+ /* else isplanar = false; */
+
+ double eps = 1e-6 ;
+ double p = 0.99; // Desired probability of choosing at least one sample
+ // free from outliers
+
+ int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
+ int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
+ // data set.
+
+ if (s<4)
+ s = 4;
+
+ // Sentinel value allowing detection of solution failure.
+ bool solutionFind = false ;
+ vpColVector bestM ;
+ int trialcount = 0;
+ int bestscore = -1;
+ double N = 1; // Dummy initialisation for number of trials.
+
+ vpUniRand random((const long)time(NULL)) ;
+ vpColVector bestinliers ;
+ unsigned int *ind = new unsigned int [s] ;
+ int numiter = 0;
+ int ninliers = 0;
+ double residual = 0.0;
+ while(( N > trialcount) && (consensus > bestscore))
+ {
+ // Select at random s datapoints to form a trial model, M.
+ // In selecting these points we have to check that they are not in
+ // a degenerate configuration.
+
+ bool degenerate = true;
+ int count = 1;
+
+ while ( degenerate == true)
+ {
+ // Generate s random indicies in the range 1..npts
+ for (unsigned int i=0 ; i < s ; i++)
+ ind[i] = (unsigned int)ceil(random()*npts) -1;
+
+ // Test that these points are not a degenerate configuration.
+ degenerate = vpTransformation::degenerateConfiguration(x,ind) ;
+ // degenerate = feval(degenfn, x(:,ind));
+
+ // Safeguard against being stuck in this loop forever
+ count = count + 1;
+
+ if (count > maxDataTrials) {
+ delete [] ind;
+ vpERROR_TRACE("Unable to select a nondegenerate data set");
+ throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
+ //return false; //Useless after a throw() function
+ }
+ }
+ // Fit model to this random selection of data points.
+ vpTransformation::computeTransformation(x,ind, M);
+
+ vpColVector d ;
+ // Evaluate distances between points and model.
+ vpTransformation::computeResidual(x, M, d) ;
+
+ // Find the indices of points that are inliers to this model.
+ residual = 0.0;
+ ninliers =0 ;
+ for (unsigned int i=0 ; i < npts ; i++)
+ {
+ double resid = fabs(d[i]);
+ if (resid < t)
+ {
+ inliers[i] = 1 ;
+ ninliers++ ;
+ residual += fabs(d[i]);
+ }
+ else inliers[i] = 0;
+ }
+
+ if (ninliers > bestscore) // Largest set of inliers so far...
+ {
+ bestscore = ninliers; // Record data for this model
+ bestinliers = inliers;
+ bestM = M;
+ solutionFind = true ;
+
+ // Update estimate of N, the number of trials to ensure we pick,
+ // with probability p, a data set with no outliers.
+
+ double fracinliers = (double)ninliers / (double)npts;
+
+ double pNoOutliers = 1 - pow(fracinliers,static_cast<int>(s));
+
+ pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
+ pNoOutliers = vpMath::minimum(1-eps, pNoOutliers);// Avoid division by 0.
+ N = (log(1-p)/log(pNoOutliers));
+ }
+
+ trialcount = trialcount+1;
+ // Safeguard against being stuck in this loop forever
+ if (trialcount > maxTrials)
+ {
+ vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
+ break ;
+ }
+ numiter++;
+ }
+
+ if (solutionFind==true) // We got a solution
+ {
+ M = bestM;
+ inliers = bestinliers;
+ }
+ else
+ {
+ vpTRACE("ransac was unable to find a useful solution");
+ M = 0;
+ }
+
+ if(ninliers > 0)
+ residual /= ninliers;
+ delete [] ind;
+
+ return true;
+}
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h
new file mode 100644
index 0000000..4f6f622
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRect.h
@@ -0,0 +1,331 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a rectangle in the plane.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRect_h
+#define vpRect_h
+
+
+
+
+/*!
+ \class vpRect
+ \ingroup group_core_geometry
+ \brief Defines a rectangle in the plane.
+
+ A rectangle is internally represented as an upper-left corner and a
+ width and height, but it is normally expressed as an upper-left
+ corner and bottom-right corner.
+
+ Note that the size (width and height) of a rectangle might be
+ different from what you are used to. If the top-left corner and the
+ bottom-right corner are the same, the height and the width of the
+ rectangle will both be 1.
+
+ Generally, width = right - left + 1 and height = bottom - top +
+ 1. We designed it this way to make it correspond to rectangular
+ spaces used by drawing functions in which the width and height
+ denote a number of pixels. For example, drawing a rectangle with
+ width and height 1 draws a single pixel.
+
+ The default coordinate system has origin (0, 0) in the top-left
+ corner. The positive direction of the y axis is down, and the
+ positive x axis is from left to right.
+
+ A vpRect can be constructed with a set of left, top, width and
+ height double. After creation the dimensions can be changed,
+ e.g. with setLeft(), setRight(), setTop() and setBottom(), or by
+ setting sizes, e.g. setWidth(), setHeight()
+
+*/
+
+#include <vector>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImagePoint.h>
+
+
+class VISP_EXPORT vpRect
+{
+public:
+
+ vpRect();
+ vpRect(double left, double top, double width, double height);
+ vpRect(const vpImagePoint &topLeft, double width, double height);
+ vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight);
+ vpRect(const vpRect& r);
+ vpRect(const std::vector<vpImagePoint> &ip);
+
+
+ vpRect &operator=(const vpRect& r);
+
+ /*!
+ Returns the bottom coordinate of the rectangle.
+ \sa getRight()
+ */
+ inline double getBottom() const { return (this->top + this->height - 1.0); };
+ /*!
+ Returns the bottom-right coordinate of the rectangle.
+ \sa getTopLeft(), getBottom(), getRight()
+ */
+ inline vpImagePoint getBottomRight() const {
+ vpImagePoint bottomRight;
+ bottomRight.set_u( getRight() );
+ bottomRight.set_v( getBottom() );
+
+ return bottomRight;
+ };
+ /*!
+
+ Returns the center point of the rectangle. The center point
+ coordinates are (\e x, \e y)
+
+ The default coordinate system has origin (0, 0) in the top-left
+ corner. The positive direction of the y axis is down, and the
+ positive x axis is from left to right.
+
+ \sa moveCenter()
+ */
+ inline void getCenter(double & x, double & y) const {
+ x = this->left + this->width / 2.0 - 0.5;
+ y = this->top + this->height / 2.0 - 0.5;
+ };
+ /*!
+
+ Returns the center point of the rectangle. The center point
+ coordinates are (\e x, \e y)
+
+ The default coordinate system has origin (0, 0) in the top-left
+ corner. The positive direction of the y axis is down, and the
+ positive x axis is from left to right.
+
+ \sa moveCenter()
+ */
+ inline vpImagePoint getCenter() const {
+ vpImagePoint center;
+ center.set_u( this->left + this->width / 2.0 - 0.5 );
+ center.set_v( this->top + this->height / 2.0 - 0.5 );
+ return center;
+ };
+ /*!
+
+ Returns the height of the rectangle. The height includes both the
+ top and bottom edges, i.e. height = bottom - top + 1.
+
+ \sa getWidth()
+
+ */
+ inline double getHeight() const { return this->height; };
+ /*!
+ Returns the left coordinate of the rectangle.
+
+ \sa getTopLeft(), getRight()
+ */
+ inline double getLeft() const { return this->left; };
+
+ /*!
+ Returns the right coordinate of the rectangle.
+ \sa getLeft()
+ */
+ inline double getRight() const { return (this->left + this->width - 1.0); };
+
+ /*!
+ Returns the size of the rectangle.
+ \sa getWidth(), getHeight()
+ */
+ inline double getSize() const { return (this->width * this->height); };
+
+ /*!
+ Returns the top coordinate of the rectangle.
+
+ \sa getTopLeft(), getBottom()
+ */
+ inline double getTop() const { return this->top; };
+ /*!
+ Returns the top-left position of the rectangle.
+
+ \sa getBottomRight(), getTop(), getLeft()
+ */
+ inline vpImagePoint getTopLeft() const {
+ vpImagePoint topLeft;
+ topLeft.set_u( this->left );
+ topLeft.set_v( this->top );
+ return topLeft;
+ };
+ /*!
+ Returns the width of the rectangle. The width includes both the
+ left and right edges, i.e. width = right - left + 1.
+
+ \sa getHeight()
+
+ */
+ inline double getWidth() const { return this->width; };
+
+ friend VISP_EXPORT bool inRectangle( const vpImagePoint &ip, const vpRect &rect );
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRect& r);
+ void set(double left, double top, double width, double height);
+ void set(const vpImagePoint &topLeft, double width, double height);
+ void set(const vpImagePoint &topLeft, const vpImagePoint &bottomRight);
+ void set(const vpRect& r);
+ void set(const std::vector<vpImagePoint> &ip);
+
+ /*!
+
+ Sets the bottom edge position of the rectangle to pos. May change
+ the height of the rectangle, but will never change the top edge
+ position the rectangle.
+
+ \sa setTop()
+ */
+ inline void setBottom(double pos) { this->height = pos - this->top + 1.0; };
+ /*!
+
+ Sets the bottom-right position of the rectangle. Will never change
+ the top-left position the rectangle.
+
+ \sa setTopLeft()
+ */
+ inline void setBottomRight(const vpImagePoint &bottomRight) {
+ this->height = bottomRight.get_v() - this->top + 1.0;
+ this->width = bottomRight.get_u() - this->left + 1.0;
+ };
+ /*!
+
+ Sets the height of the rectangle to \e h. The top edge is not moved,
+ but the bottom edge may be moved.
+
+ \sa setWidth()
+ */
+ inline void setHeight(double h) { this->height = h; };
+ /*!
+
+ Sets the left edge position of the rectangle to pos. May change the right
+ edge position of the rectangle, but will never change the width of the
+ rectangle.
+
+ \sa setRight()
+
+ */
+ inline void setLeft(double pos) { this->left = pos; };
+ /*!
+
+ Sets the coordinates of the rectangle's top left corner to
+ (left, top), and its size to (width, height).
+
+ \param l,t : (left, top) corner position.
+ \param w,h : (width, height) rectangle size.
+
+ */
+ inline void setRect(double l, double t, double w, double h) {
+ this->left = l;
+ this->top = t;
+ this->width = w;
+ this->height = h;
+ };
+ /*!
+
+ Sets the right edge position of the rectangle to pos. May change
+ the width of the rectangle, but will never change the left edge
+ position of the rectangle.
+
+ \sa setLeft()
+
+ */
+ inline void setRight(double pos) { this->width = pos - this->left + 1.0; };
+ /*!
+
+ Sets the top edge position of the rectangle to pos. May change the bottom
+ edge position of the rectangle, but will never change the height of the
+ rectangle.
+
+ \sa setBottom()
+ */
+ inline void setTop(double pos) { this->top = pos; };
+ /*!
+
+ Sets the top-left position of the rectangle. May change the bottom
+ edge position of the rectangle, but will never change the height of the
+ rectangle.
+
+ \sa setBottomRight()
+ */
+ inline void setTopLeft(const vpImagePoint &topLeft) {
+ this->left = topLeft.get_u();
+ this->top = topLeft.get_v();
+ };
+ /*!
+
+ Sets the width of the rectangle to \e w. The right edge is changed,
+ but not the left edge.
+
+ \sa setHeight()
+ */
+ inline void setWidth(double w) { this->width = w; }
+
+ /*!
+
+ Sets the center point of the rectangle to (\e x, \e y), leaving
+ the size unchanged.
+
+ \sa getCenter()
+
+ */
+ inline void moveCenter(double x, double y) {
+ this->left = x - this->width/2 + 0.5;
+ this->top = y - this->height/2 + 0.5;
+ };
+
+ /*!
+
+ Center the rectangle to the image point given as parameter, leaving
+ the size unchanged.
+
+ \sa getCenter()
+
+ */
+ inline void moveCenter(const vpImagePoint ¢er) {
+ this->left = center.get_u() - this->width/2 + 0.5;
+ this->top = center.get_v() - this->height/2 + 0.5;
+ };
+
+private:
+ double left; // Upper left corner position along the columns axis
+ double top; // Upper left corner position along the rows axis
+ double width; // Rectangle width
+ double height; // Rectangle height
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRequest.h b/modules/core/include/visp3/core/vpRequest.h
new file mode 100644
index 0000000..52ea270
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRequest.h
@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Network Request.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpRequest_H
+#define vpRequest_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImageException.h>
+
+#include <string.h>
+#include <vector>
+
+/*!
+ \class vpRequest
+
+ \ingroup group_core_network
+
+ \brief This the request that will transit on the network
+
+ Exemple request decoding an image on a specific form.
+ First parameter : Height of the image.
+ Second parameter : Width of the image.
+ Thirs parameter : Bitmap of the image (not compress).
+
+ Here is the header of the vpRequestImage class.
+
+ \code
+#ifndef vpRequestImage_H
+#define vpRequestImage_H
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRequest.h>
+
+class vpRequestImage : public vpRequest
+{
+private:
+ vpImage<unsigned char> *I;
+
+public:
+ vpRequestImage();
+ vpRequestImage(vpImage<unsigned char> *);
+ ~vpRequestImage();
+
+ virtual void encode();
+ virtual void decode();
+};
+
+#endif
+ \endcode
+
+ Here is the definition of the vpRequestImage class.
+
+ \code
+#include <vpRequestImage.h>
+
+vpRequestImage::vpRequestImage(){
+ request_id = "image";
+}
+
+vpRequestImage::vpRequestImage(vpImage<unsigned char> *Im){
+ request_id = "image";
+ I = Im;
+}
+
+vpRequestImage::~vpRequestImage(){}
+
+void vpRequestImage::encode(){
+ clear();
+
+ unsigned int h = I->getHeight();
+ unsigned int w = I->getWidth();
+
+ addParameterObject(&h);
+ addParameterObject(&w);
+ addParameterObject(I->bitmap,h*w*sizeof(unsigned char));
+}
+
+void vpRequestImage::decode(){
+ if(listOfParams.size() == 3){
+ unsigned int w, h;
+ memcpy((void*)&h, (void*)listOfParams[0].c_str(), sizeof(unsigned int));
+ memcpy((void*)&w, (void*)listOfParams[1].c_str(), sizeof(unsigned int));
+
+ I->resize(h,w);
+ memcpy((void*)I->bitmap,(void*)listOfParams[2].c_str(),w*h*sizeof(unsigned char));
+ }
+}
+ \endcode
+
+ \sa vpClient
+ \sa vpServer
+ \sa vpNetwork
+*/
+class VISP_EXPORT vpRequest
+{
+protected:
+ std::string request_id;
+ std::vector<std::string> listOfParams;
+
+public:
+ vpRequest();
+ virtual ~vpRequest();
+
+ void addParameter(char *params);
+ void addParameter(std::string ¶ms);
+ void addParameter(std::vector<std::string> &listOfparams);
+ template<typename T>
+ void addParameterObject(T * params, const int &sizeOfObject = sizeof(T));
+
+ /*!
+ Decode the parameters of the request (Funtion that has to be redifined).
+
+ \sa vpRequest::encode()
+ */
+ virtual void decode() = 0;
+
+ /*!
+ Clear the parameters of the request.
+ */
+ void clear(){ listOfParams.clear(); }
+
+ /*!
+ Encode the parameters of the request (Funtion that has to be redifined).
+
+ \sa vpRequest::decode()
+ */
+ virtual void encode() = 0;
+
+ /*!
+ Accessor on the parameters.
+
+ \return Parameter at the index i.
+ */
+ inline std::string& operator[](const unsigned int &i) { return listOfParams[i];}
+
+ /*!
+ Accessor on the parameters (const).
+
+ \return Parameter at the index i (const).
+ */
+ inline const std::string& operator[](const unsigned int &i) const { return listOfParams[i];}
+
+ /*!
+ Get the ID of the request.
+
+ \sa vpRequest::setId()
+
+ \return ID of the request.
+ */
+ std::string getId(){ return request_id; }
+
+ /*!
+ Change the ID of the request.
+
+ \sa vpRequest::getId()
+
+ \param id : new ID.
+ */
+ void setId(const char *id){ request_id = id; }
+
+ /*!
+ Get the number of parameters.
+
+ \return Number of parameters.
+ */
+ unsigned int size(){ return (unsigned int)listOfParams.size(); }
+};
+
+
+//######## Definition of Template Functions ########
+//# #
+//##################################################
+
+
+/*!
+ Add an object as parameter of the request.
+
+ \warning Only simple object can be sent unless you know its size.
+ Sending object containing pointers, virtual methods, etc, won't probably work.
+ Unless the size is well defined...
+
+ \sa vpRequest::addParameter()
+
+ \param params : Object to add.
+ \param sizeOfObject : Size of the object.
+*/
+template<typename T>
+void vpRequest::addParameterObject(T * params, const int &sizeOfObject)
+{
+ if(sizeOfObject != 0){
+ char *tempS = new char [sizeOfObject];
+ memcpy((void*)tempS, (void*)params, sizeOfObject);
+ std::string returnVal(tempS, (size_t)sizeOfObject);
+
+ listOfParams.push_back(returnVal);
+
+ delete [] tempS;
+ }
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRobust.h b/modules/core/include/visp3/core/vpRobust.h
new file mode 100644
index 0000000..8d0dae7
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRobust.h
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * M-Estimator and various influence function.
+ *
+ * Authors:
+ * Andrew Comport
+ * Jean Laneurit
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpRobust.h
+*/
+
+
+
+#ifndef CROBUST_HH
+#define CROBUST_HH
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+
+/*!
+ \class vpRobust
+ \ingroup group_core_robust
+ \brief Contains an M-Estimator and various influence function.
+
+ Supported methods: M-estimation, Tukey, Cauchy and Huber
+*/
+class VISP_EXPORT vpRobust
+{
+
+public:
+ //! Enumeration of influence functions
+ typedef enum
+ {
+ TUKEY,
+ CAUCHY,
+ // MCLURE,
+ HUBER
+ } vpRobustEstimatorType;
+
+private:
+
+ //!Normalized residue
+ vpColVector normres;
+ //!Sorted normalized Residues
+ vpColVector sorted_normres;
+ //!Sorted residues
+ vpColVector sorted_residues;
+
+ //!Noise threshold
+ double NoiseThreshold;
+ //!
+ double sig_prev;
+ //!
+ unsigned int it;
+ //! Vairiable used in swap method
+ double swap;
+ //! Size of the containers
+ unsigned int size;
+
+public:
+
+ //!Default Constructor
+ vpRobust(unsigned int n_data);
+
+ //!Destructor
+ virtual ~vpRobust(){};
+
+ //!Resize containers for sort methods
+ void resize(unsigned int n_data);
+
+ //! Compute the weights according a residue vector and a PsiFunction
+ void MEstimator(const vpRobustEstimatorType method,
+ const vpColVector &residues,
+ vpColVector &weights);
+
+ //! Compute the weights according a residue vector and a PsiFunction
+ void MEstimator(const vpRobustEstimatorType method,
+ const vpColVector &residues,
+ const vpColVector& all_residues,
+ vpColVector &weights);
+
+ //! Simult Mestimator
+ vpColVector simultMEstimator(vpColVector &residues);
+
+ //! Set iteration
+ void setIteration(const unsigned int iter){it=iter;}
+
+ /*!
+ Set maximal noise threshold.
+ \param noise_threshold : Maximal noise threshold.
+ */
+ inline void setThreshold(const double noise_threshold) {
+ NoiseThreshold=noise_threshold;
+ }
+
+//public :
+//double residualMedian ;
+//double normalizedResidualMedian ;
+// private:
+// double median(const vpColVector &x);
+// double median(const vpColVector &x, vpColVector &weights);
+
+ private:
+ //!Compute normalized median
+ double computeNormalizedMedian(vpColVector &all_normres,
+ const vpColVector &residues,
+ const vpColVector &all_residues,
+ const vpColVector &weights
+ );
+
+
+ //! Calculate various scale estimates
+ double scale(vpRobustEstimatorType method, vpColVector &x);
+ double simultscale(vpColVector &x);
+
+
+ //---------------------------------
+ // Partial derivative of loss function with respect to the residue
+ //---------------------------------
+ /** @name PsiFunctions */
+ //@{
+ //! Tuckey influence function
+ void psiTukey(double sigma, vpColVector &x, vpColVector &w);
+ //! Caucht influence function
+ void psiCauchy(double sigma, vpColVector &x, vpColVector &w);
+ //! McLure influence function
+ void psiMcLure(double sigma, vpColVector &x, vpColVector &w);
+ //! Huber influence function
+ void psiHuber(double sigma, vpColVector &x, vpColVector &w);
+ //@}
+
+ //! Partial derivative of loss function
+ //! with respect to the scale
+ double simult_chi_huber(double x);
+
+ //---------------------------------
+ // Constrained Partial derivative of loss function with respect to the scale
+ //---------------------------------
+ /** @name Constrained Chi Functions */
+ //@{
+ //! Constrained Chi Function
+ double constrainedChi(vpRobustEstimatorType method, double x);
+ //! Constrained Chi Tukey Function
+ double constrainedChiTukey(double x);
+ //! Constrained Chi Cauchy Function
+ double constrainedChiCauchy(double x);
+ //! Constrained Chi Huber Function
+ double constrainedChiHuber(double x);
+ //@}
+
+ //---------------------------------
+ // Mathematic functions used to calculate the Expectation
+ //---------------------------------
+ /** @name Some math function */
+ //@{
+ double erf(double x);
+ double gammp(double a, double x);
+ void gser(double *gamser, double a, double x, double *gln);
+ void gcf(double *gammcf, double a, double x, double *gln);
+ double gammln(double xx);
+ //@}
+
+ /** @name Sort function */
+ //@{
+ //! Swap two value
+ void exch(double &A, double &B){swap = A; A = B; B = swap;}
+ //! Sort function using partition method
+ int partition(vpColVector &a, int l, int r);
+ //! Sort the vector and select a value in the sorted vector
+ double select(vpColVector &a, int l, int r, int k);
+ //@}
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h
new file mode 100644
index 0000000..f55e94d
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRotationMatrix.h
@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rotation matrix.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpROTATIONMATRIX_H
+#define vpROTATIONMATRIX_H
+
+/*!
+ \file vpRotationMatrix.h
+ \brief Class that consider the particular case of rotation matrix
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRzyxVector.h>
+#include <visp3/core/vpRzyzVector.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpPoseVector.h>
+
+/*!
+ \class vpRotationMatrix
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation matrix and operations on such kind of matrices.
+
+ The vpRotationMatrix considers the particular case of
+ a rotation matrix.
+
+ The vpRotationMatrix class is derived from vpArray2D<double>.
+
+*/
+class VISP_EXPORT vpRotationMatrix : public vpArray2D<double>
+{
+public:
+ vpRotationMatrix();
+ vpRotationMatrix(const vpRotationMatrix &R);
+ vpRotationMatrix(const vpHomogeneousMatrix &M);
+ vpRotationMatrix(const vpThetaUVector &r);
+ vpRotationMatrix(const vpPoseVector &p);
+ vpRotationMatrix(const vpRzyzVector &r);
+ vpRotationMatrix(const vpRxyzVector &r);
+ vpRotationMatrix(const vpRzyxVector &r);
+ vpRotationMatrix(const vpQuaternionVector& q);
+ vpRotationMatrix(const double tux, const double tuy, const double tuz);
+ /*!
+ Destructor.
+ */
+ virtual ~vpRotationMatrix() {};
+
+ vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M);
+ vpRotationMatrix buildFrom(const vpThetaUVector &v) ;
+ vpRotationMatrix buildFrom(const vpPoseVector &p);
+ vpRotationMatrix buildFrom(const vpRzyzVector &v);
+ vpRotationMatrix buildFrom(const vpRxyzVector &v);
+ vpRotationMatrix buildFrom(const vpRzyxVector &v);
+ vpRotationMatrix buildFrom(const vpQuaternionVector& q);
+ vpRotationMatrix buildFrom(const double tux, const double tuy, const double tuz);
+
+ void eye();
+
+ vpColVector getCol(const unsigned int j) const;
+ vpThetaUVector getThetaUVector();
+
+ vpRotationMatrix inverse() const;
+ void inverse(vpRotationMatrix &R) const;
+
+ bool isARotationMatrix() const ;
+
+ // copy operator from vpRotationMatrix
+ vpRotationMatrix &operator=(const vpRotationMatrix &R);
+ // copy operator from vpMatrix (handle with care)
+ vpRotationMatrix &operator=(const vpMatrix &M);
+ // operation c = A * b (A is unchanged)
+ vpTranslationVector operator*(const vpTranslationVector &tv) const;
+ // operation C = A * B (A is unchanged)
+ vpRotationMatrix operator*(const vpRotationMatrix &R) const;
+ // operation C = A * B (A is unchanged)
+ vpMatrix operator*(const vpMatrix &M) const;
+ // operation v2 = A * v1 (A is unchanged)
+ vpColVector operator*(const vpColVector &v) const;
+ vpRotationMatrix operator*(const double x) const;
+ vpRotationMatrix &operator*=(const double x);
+
+ void printVector() ;
+
+ /*!
+ This function is not applicable to a rotation matrix that is always a
+ 3-by-3 matrix.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize a rotation matrix"));
+ };
+
+ // transpose
+ vpRotationMatrix t() const;
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use eye().
+ */
+ vp_deprecated void setIdentity();
+ //@}
+#endif
+
+private:
+ static const double threshold;
+};
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+VISP_EXPORT
+#endif
+vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R) ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h
new file mode 100644
index 0000000..15ee560
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRotationVector.h
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic rotation vector (cannot be used as is !).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpRotationVECTOR_H
+#define vpRotationVECTOR_H
+
+/*!
+ \file vpRotationVector.h
+ \brief class that consider the case of a generic rotation vector
+ (cannot be used as is !)
+*/
+
+#include <iostream>
+#include <math.h>
+#include <stdio.h>
+
+#include <visp3/core/vpArray2D.h>
+
+class vpRowVector;
+class vpColVector;
+
+/*!
+ \class vpRotationVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a generic rotation vector.
+
+ Class that consider the case of a generic rotation vector
+ (cannot be used as is !) consisting in three or four angles.
+
+ The vpRotationVector class is derived from vpArray2D<double>.
+ The vpRotationVector class is also the base class of specific rotations vectors such as
+ vpThetaUVector, vpRxyzVector, vpRzyxVector, vpRzyzVector and vpQuaternionVector.
+
+ The code below shows how this class can be used to manipulate a vpRxyzVector.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpRxyzVector r; // By default initialized to zero
+ r[0] = vpMath::rad(45); // Rotation around x set to 45 degres converted in radians
+ r[1] = M_PI; // Rotation around y set to PI radians
+ r[2] = 0; // Rotation around z set to 0 radians
+
+ std::cout << "Rxyz rotation vector: " << r << std::endl;
+
+ double rx = r[0]; // Get the value of the angle around x axis
+ double ry = r[1]; // Get the value of the angle around y axis
+ double rz = r[2]; // Get the value of the angle around z axis
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpRotationVector : public vpArray2D<double>
+{
+public:
+ //! Constructor that constructs a 0-size rotation vector.
+ vpRotationVector()
+ : vpArray2D<double>()
+ {}
+
+ //! Constructor that constructs a vector of size n and initialize all values to zero.
+ vpRotationVector(const unsigned int n)
+ : vpArray2D<double>(n, 1)
+ {}
+
+ /*!
+ Copy operator.
+ */
+ vpRotationVector(const vpRotationVector &v)
+ : vpArray2D<double>(v)
+ {}
+
+ /*!
+ Destructor.
+ */
+ virtual ~vpRotationVector() {};
+
+ /** @name Inherited functionalities from vpRotationVector */
+ //@{
+
+ /*!
+ Operator that allows to set the value of an element of the rotation
+ vector: r[i] = value
+ */
+ inline double &operator [](unsigned int i) { return *(data + i); }
+ /*!
+ Operator that allows to get the value of an element of the rotation
+ vector: value = r[i]
+ */
+ inline const double &operator [](unsigned int i) const { return *(data+i); }
+
+ /*!
+ Affectation of two vectors.
+ */
+ vpRotationVector &operator=(const vpRotationVector &v)
+ {
+ resize(v.size(), 1);
+ for (unsigned int i=0; i<v.size(); i++)
+ {
+ data[i] = v.data[i] ;
+ }
+ return *this;
+ }
+ vpColVector operator*(double x) const;
+
+ double sumSquare() const;
+
+ // Transpose of the rotation vector.
+ vpRowVector t() const;
+
+ //@}
+} ;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+VISP_EXPORT
+#endif
+vpColVector operator*(const double &x, const vpRotationVector &v) ;
+
+#endif
+
diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h
new file mode 100644
index 0000000..cd07b74
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRowVector.h
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Operation on row vectors.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpRowVector_H
+#define vpRowVector_H
+
+#include <vector>
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+
+class vpMatrix;
+class vpColVector;
+
+/*!
+ \file vpRowVector.h
+ \brief Definition of row vector class as well
+ as a set of operations on these vectors.
+
+*/
+
+/*!
+ \class vpRowVector
+
+ \ingroup group_core_matrices
+
+ \brief Implementation of row vector and the associated operations.
+
+ This class provides a data structure for a row vector that contains values of double.
+ It contains also some functions to achieve a set of operations on these vectors.
+
+ The vpRowVector class is derived from vpArray2D<double>.
+*/
+class VISP_EXPORT vpRowVector : public vpArray2D<double>
+{
+public:
+ //! Basic constructor that creates an empty 0-size row vector.
+ vpRowVector() : vpArray2D<double>() {};
+ //! Construct a row vector of size n. All the elements are initialized to zero.
+ vpRowVector(unsigned int n) : vpArray2D<double>(1, n){};
+ //! Construct a row vector of size n. Each element is set to \e val.
+ vpRowVector(unsigned int n, double val) : vpArray2D<double>(1, n, val){};
+ //! Copy constructor that allows to construct a row vector from an other one.
+ vpRowVector(const vpRowVector &v) : vpArray2D<double>(v) {};
+ vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) ;
+ vpRowVector(const vpMatrix &M);
+ vpRowVector(const vpMatrix &M, unsigned int i);
+ vpRowVector(const std::vector<double> &v);
+ vpRowVector(const std::vector<float> &v);
+ /*!
+ Destructor.
+ */
+ virtual ~vpRowVector() {};
+
+ /*!
+ Removes all elements from the vector (which are destroyed),
+ leaving the container with a size of 0.
+ */
+ void clear()
+ {
+ if (data != NULL ) {
+ free(data);
+ data=NULL;
+ }
+
+ if (rowPtrs!=NULL) {
+ free(rowPtrs);
+ rowPtrs=NULL ;
+ }
+ rowNum = colNum = dsize = 0;
+ }
+
+ /*!
+ Convert a column vector containing angles in degrees into radians.
+ \sa rad2deg()
+ */
+ inline void deg2rad() {
+ double d2r = M_PI/180.0;
+
+ (*this) *= d2r;
+ }
+
+ double euclideanNorm() const;
+ /*!
+ Extract a sub-row vector from a row vector.
+ \param c : Index of the column corresponding to the first element of the vector to extract.
+ \param rowsize : Size of the vector to extract.
+ \exception vpException::fatalError If the vector to extract is not contained in the original one.
+
+ \code
+ vpRowVector r1;
+ for (unsigned int i=0; i<4; i++)
+ r1.stack(i);
+ // r1 is equal to [0 1 2 3]
+ vpRowVector r2 = r1.extract(1, 3);
+ // r2 is equal to [1 2 3]
+ \endcode
+ */
+ vpRowVector extract(unsigned int c, unsigned int rowsize) const
+ {
+ if (c >= colNum || c+rowsize > colNum) {
+ throw(vpException(vpException::fatalError,
+ "Cannot extract a (1x%d) row vector from a (1x%d) row vector starting at index %d",
+ rowsize, colNum, c));
+ }
+
+ return vpRowVector(*this, c, rowsize);
+ }
+
+ void init(const vpRowVector &v, unsigned int c, unsigned int ncols);
+ void insert(unsigned int i, const vpRowVector &v);
+
+ vpRowVector &normalize() ;
+ vpRowVector &normalize(vpRowVector &x) const ;
+
+ //! Operator that allows to set a value of an element \f$v_i\f$: v[i] = x
+ inline double &operator[](unsigned int n) { return *(data+n); }
+ //! Operator that allows to get the value of an element \f$v_i\f$: x = v[i]
+ inline const double &operator[](unsigned int n) const { return *(data+n) ; }
+
+ //! Copy operator. Allow operation such as A = v
+ vpRowVector &operator=(const vpRowVector &v);
+ vpRowVector &operator=(const vpMatrix &M);
+ vpRowVector &operator=(const std::vector<double> &v);
+ vpRowVector &operator=(const std::vector<float> &v);
+ vpRowVector &operator=(const double x);
+
+ double operator*(const vpColVector &x) const;
+ vpRowVector operator*(const vpMatrix &M) const;
+ vpRowVector operator*(const double x) const;
+ vpRowVector &operator*=(double x);
+
+ vpRowVector operator/(const double x) const;
+ vpRowVector &operator/=(double x);
+
+ vpRowVector operator+(const vpRowVector &v) const;
+ vpRowVector &operator+=(vpRowVector v);
+
+ vpRowVector operator-(const vpRowVector &v) const;
+ vpRowVector &operator-=(vpRowVector v);
+ vpRowVector operator-() const;
+
+ vpRowVector &operator<<(const vpRowVector &v);
+
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+ /*!
+ Convert a column vector containing angles in radians into degrees.
+ \sa deg2rad()
+ */
+ inline void rad2deg() {
+ double r2d = 180.0/M_PI;
+
+ (*this) *= r2d;
+ }
+ void reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols);
+ vpMatrix reshape(const unsigned int &nrows,const unsigned int &ncols);
+
+ /*! Modify the size of the row vector.
+ \param i : Size of the vector. This value corresponds to the vector number
+ of columns.
+ \param flagNullify : If true, set the data to zero.
+ */
+ inline void resize(const unsigned int i, const bool flagNullify = true)
+ {
+ vpArray2D<double>::resize(1, i, flagNullify);
+ }
+
+ /*!
+ Resize the row vector to a \e ncols-dimension vector.
+ This function can only be used with \e nrows = 1.
+ \param nrows : Vector number of rows. This value should be set to 1.
+ \param ncols : Vector number of columns. This value corresponds
+ to the size of the vector.
+ \param flagNullify : If true, set the data to zero.
+ \exception vpException::fatalError When \e nrows is not equal to 1.
+
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify)
+ {
+ if (nrows != 1)
+ throw(vpException(vpException::fatalError,
+ "Cannot resize a row vector to a (%dx%d) dimension vector that has more than one row",
+ nrows, ncols));
+ vpArray2D<double>::resize(nrows, ncols, flagNullify);
+ };
+
+ void stack(const double &d);
+ void stack(const vpRowVector &v);
+
+ double sumSquare() const;
+ vpColVector t() const;
+ vpColVector transpose() const;
+ void transpose(vpColVector &v) const;
+
+ static double mean(const vpRowVector &v) ;
+ static double median(const vpRowVector &v) ;
+ static vpRowVector stack(const vpRowVector &A, const vpRowVector &B);
+ static void stack(const vpRowVector &A, const vpRowVector &B, vpRowVector &C);
+ static double stdev(const vpRowVector &v, const bool useBesselCorrection=false);
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use stack(const vpRowVector &)
+ */
+ vp_deprecated void stackMatrices(const vpRowVector &r) { stack(r); };
+ /*!
+ \deprecated You should rather use stack(const vpRowVector &A, const vpRowVector &B)
+ */
+ vp_deprecated static vpRowVector stackMatrices(const vpRowVector &A, const vpRowVector &B) { return stack(A, B); };
+ /*!
+ \deprecated You should rather use stack(const vpRowVector &A, const vpRowVector &B, vpRowVector &C)
+ */
+ vp_deprecated static void stackMatrices(const vpRowVector &A, const vpRowVector &B, vpRowVector &C) { stack(A, B, C); };
+
+ /*!
+ \deprecated You should rather use eye()
+ */
+ vp_deprecated void setIdentity(const double & val=1.0) ;
+ //@}
+#endif
+
+};
+
+VISP_EXPORT vpRowVector operator*(const double &x, const vpRowVector &v) ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRxyzVector.h b/modules/core/include/visp3/core/vpRxyzVector.h
new file mode 100644
index 0000000..184667f
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRxyzVector.h
@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rxyz angle parameterization for the rotation.
+ * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRxyzVECTOR_H
+#define vpRxyzVECTOR_H
+
+/*!
+ \file vpRxyzVector.h
+
+ \brief Class that consider the case of the Rxyz angle
+ parameterization for the rotation.
+
+ Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
+*/
+
+
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+class vpRotationVector;
+class vpRotationMatrix;
+class vpThetaUVector;
+
+/*!
+ \class vpRxyzVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation vector as \f$R(x,y,z)\f$ Euler angle
+ minimal representation.
+
+ Class that consider the case of the Euler
+ \f$(\varphi,\theta,\psi)\f$ angle using the x-y-z convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
+ rotation angles around the \f$x\f$, \f$y\f$ and \f$z\f$ axis.
+
+ \f[R_{xyz}(\varphi,\theta,\psi) = R_x(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
+
+ with
+
+ \f[R_{x}(\varphi) = \left(
+ \begin{array}{ccc}
+ 1 & 0 & 0 \\
+ 0 &\cos \varphi & -\sin\varphi \\
+ 0 &\sin \varphi & \cos\varphi \\
+ \end{array}
+ \right) \;
+ R_{y}(\theta) = \left(
+ \begin{array}{ccc}
+ \cos \theta & 0 & \sin\theta\\
+ 0 & 1 & 0 \\
+ -\sin\theta & 0 &\cos \theta
+ \end{array}
+ \right) \;
+ R_{z}(\psi) = \left(
+ \begin{array}{ccc}
+ \cos \psi & -\sin\psi & 0\\
+ \sin\psi &\cos \psi& 0 \\
+ 0 & 0 & 1
+ \end{array}
+ \right)\f]
+
+ The rotation matrix corresponding to the x-y-z convention is given by:
+
+ \f[
+ R_{xyz}(\varphi,\theta,\psi) = \left(
+ \begin{array}{ccc}
+ \cos\theta \cos\psi & -\cos\theta \sin\psi & \sin\theta \\
+ \sin\varphi \sin\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \sin\theta \sin\psi +\cos\varphi\cos\psi & -\sin\varphi \cos\theta \\
+ -\cos\varphi \sin\theta \cos\psi + \sin\varphi\sin\psi & \cos\varphi \sin\theta \sin\psi +\sin\varphi\cos\psi & \cos\varphi \cos\theta
+ \end{array}
+ \right)
+ \f]
+
+ The vpRxyzVector class is derived from vpRotationVector.
+
+ The code below shows first how to initialize this representation of
+ Euler angles, than how to contruct a rotation matrix from a
+ vpRxyzVector and finaly how to extract the vpRxyzVector Euler angles
+ from the build rotation matrix.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+
+int main()
+{
+ vpRxyzVector rxyz;
+
+ // Initialise the Euler angles
+ rxyz[0] = vpMath::rad( 45.f); // phi angle in rad around x axis
+ rxyz[1] = vpMath::rad(-30.f); // theta angle in rad around y axis
+ rxyz[2] = vpMath::rad( 90.f); // psi angle in rad around z axis
+
+ // Construct a rotation matrix from the Euler angles
+ vpRotationMatrix R(rxyz);
+
+ // Extract the Euler angles around x,y,z axis from a rotation matrix
+ rxyz.buildFrom(R);
+
+ // Print the extracted Euler angles. Values are the same than the
+ // one used for initialization
+ std::cout << rxyz;
+
+ // Since the rotation vector is 3 values column vector, the
+ // transpose operation produce a row vector.
+ vpRowVector rxyz_t = rxyz.t();
+
+ // Print the transpose row vector
+ std::cout << rxyz_t << std::endl;
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpRxyzVector : public vpRotationVector
+{
+ public:
+ vpRxyzVector();
+ vpRxyzVector(const vpRxyzVector &rxyz);
+ vpRxyzVector(const double phi, const double theta, const double psi);
+
+ // initialize a Rxyz vector from a rotation matrix
+ vpRxyzVector(const vpRotationMatrix& R) ;
+
+ // initialize a Rxyz vector from a ThetaU vector
+ vpRxyzVector(const vpThetaUVector& tu) ;
+
+ //! Destructor.
+ virtual ~vpRxyzVector() {};
+
+ // convert a rotation matrix into Rxyz vector
+ vpRxyzVector buildFrom(const vpRotationMatrix& R) ;
+
+ // convert a ThetaU vector into a Rxyz vector
+ vpRxyzVector buildFrom(const vpThetaUVector& tu) ;
+
+ void buildFrom(const double phi, const double theta, const double psi);
+
+ vpRxyzVector &operator=(double x) ;
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRzyxVector.h b/modules/core/include/visp3/core/vpRzyxVector.h
new file mode 100644
index 0000000..f3f6c84
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRzyxVector.h
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rzyx angle parameterization for the rotation.
+ * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpRzyxVector_h
+#define vpRzyxVector_h
+
+/*!
+ \file vpRzyxVector.h
+
+ \brief class that consider the case of the Rzyx angle
+ parameterization for the rotation.
+
+ Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
+*/
+
+class vpRotationMatrix;
+class vpThetaUVector;
+
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRotationVector.h>
+
+/*!
+ \class vpRzyxVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation vector as \f$R(z,y,x)\f$ Euler angle
+ minimal representation.
+
+ Class that consider the case of the Euler
+ \f$(\varphi,\theta,\psi)\f$ angle using the z-y-x convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
+ rotation angles around the \f$z\f$, \f$y\f$ and \f$x\f$ axis.
+
+ \f[R_{zyx}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_x(\psi)\f]
+
+ with
+
+ \f[
+ R_{z}(\varphi) = \left(
+ \begin{array}{ccc}
+ \cos \varphi & -\sin\varphi & 0\\
+ \sin\varphi &\cos \varphi& 0 \\
+ 0 & 0 & 1
+ \end{array}
+ \right) \;
+ R_{y}(\theta) = \left(
+ \begin{array}{ccc}
+ \cos \theta & 0 & \sin\theta\\
+ 0 & 1 & 0 \\
+ -\sin\theta & 0 &\cos \theta
+ \end{array}
+ \right) \;
+ R_{x}(\psi) = \left(
+ \begin{array}{ccc}
+ 1 & 0 & 0 \\
+ 0 &\cos \psi & -\sin\psi \\
+ 0 &\sin \psi & \cos\psi \\
+ \end{array}
+ \right)
+ \f]
+
+ The rotation matrix corresponding to the z-y-x convention is given by:
+
+ \f[
+ R_{zyx}(\varphi,\theta,\psi) = \left(
+ \begin{array}{ccc}
+ \cos\varphi \cos\theta & -\sin\varphi \cos\psi + \cos\varphi\sin\theta\sin\psi & \sin\varphi \sin\psi +\cos\varphi\sin\theta\cos\psi \\
+ \sin\varphi \cos\theta & \cos\varphi\cos\psi + \sin\varphi\sin\theta \sin\psi & -\cos\varphi \sin\psi +\sin\varphi\sin\theta\cos\psi \\
+ -\sin\theta & \cos\theta \sin\psi & \cos\theta \cos\psi
+ \end{array}
+ \right)
+ \f]
+
+ The vpRzyxVector class is derived from vpRotationVector.
+
+ The code below shows first how to initialize this representation of
+ Euler angles, than how to contruct a rotation matrix from a
+ vpRzyxVector and finaly how to extract the vpRzyxVector Euler angles
+ from the build rotation matrix.
+
+ \code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRzyxVector.h>
+
+int main()
+{
+ vpRzyxVector rzyx;
+
+ // Initialise the Euler angles
+ rzyx[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
+ rzyx[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
+ rzyx[2] = vpMath::rad( 90.f); // psi angle in rad/s around x axis
+
+ // Construct a rotation matrix from the Euler angles
+ vpRotationMatrix R(rzyx);
+
+ // Extract the Euler angles around z,y,x axis from a rotation matrix
+ rzyx.buildFrom(R);
+
+ // Print the extracted Euler angles. Values are the same than the
+ // one used for initialization
+ std::cout << rzyx;
+
+ // Since the rotation vector is 3 values column vector, the
+ // transpose operation produce a row vector.
+ vpRowVector rzyx_t = rzyx.t();
+
+ // Print the transpose row vector
+ std::cout << rzyx_t << std::endl;
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpRzyxVector : public vpRotationVector
+{
+public:
+ vpRzyxVector();
+ vpRzyxVector(const vpRzyxVector &rzyx);
+ vpRzyxVector(const double phi, const double theta, const double psi);
+
+ // initialize a Rzyx vector from a rotation matrix
+ vpRzyxVector(const vpRotationMatrix& R) ;
+
+ // initialize a Rzyx vector from a ThetaU vector
+ vpRzyxVector(const vpThetaUVector& tu) ;
+
+ //! Destructor.
+ virtual ~vpRzyxVector() {};
+
+ // convert a rotation matrix into Rzyx vector
+ vpRzyxVector buildFrom(const vpRotationMatrix& R) ;
+
+ // convert a ThetaU vector into a Rzyx vector
+ vpRzyxVector buildFrom(const vpThetaUVector& R) ;
+
+ void buildFrom(const double phi, const double theta, const double psi);
+
+ vpRzyxVector &operator=(double x) ;
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpRzyzVector.h b/modules/core/include/visp3/core/vpRzyzVector.h
new file mode 100644
index 0000000..1b27c3d
--- /dev/null
+++ b/modules/core/include/visp3/core/vpRzyzVector.h
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Euler angles parameterization for the rotation.
+ * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRzyzVector_h
+#define vpRzyzVector_h
+
+/*!
+ \file vpRzyzVector.h
+ \brief class that consider the case of the Rzyz angles parameterization
+ for the rotation
+
+ Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
+*/
+
+class vpRotationMatrix;
+class vpThetaUVector;
+
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+/*!
+ \class vpRzyzVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation vector as \f$R(z,y,z)\f$ Euler angle
+ minimal representation.
+
+ Class that consider the case of the Euler
+ \f$(\varphi,\theta,\psi)\f$ angles using the z-y-z convention, where
+ \f$(\varphi,\theta,\psi)\f$ are respectively the rotation angles
+ around the \f$z\f$, \f$y\f$ and \f$z\f$ axis.
+
+ \f[R_{zyz}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
+
+ with
+
+ \f[
+ R_{z}(\varphi) = \left(
+ \begin{array}{ccc}
+ \cos \varphi & -\sin\varphi & 0\\
+ \sin\varphi &\cos \varphi& 0 \\
+ 0 & 0 & 1
+ \end{array}
+ \right) \;
+ R_{y}(\theta) = \left(
+ \begin{array}{ccc}
+ \cos \theta & 0 & \sin\theta\\
+ 0 & 1 & 0 \\
+ -\sin\theta & 0 &\cos \theta
+ \end{array}
+ \right) \;
+ R_{z}(\psi) = \left(
+ \begin{array}{ccc}
+ \cos \psi & -\sin\psi & 0\\
+ \sin\psi &\cos \psi& 0 \\
+ 0 & 0 & 1
+ \end{array}
+ \right)
+ \f]
+
+ The rotation matrix corresponding to the z-y-z convention is given by:
+
+ \f[
+ R_{zyz}(\varphi,\theta,\psi) = \left(
+ \begin{array}{ccc}
+ \cos\varphi \cos\theta \cos\psi - \sin\varphi\sin\psi & -\cos\varphi \cos\theta \sin\psi -\sin\varphi\cos\psi & \cos\varphi \sin\theta \\
+ \sin\varphi \cos\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \cos\theta \sin\psi +\cos\varphi\cos\psi & \sin\varphi \sin\theta \\
+ -\sin\theta \cos\psi & \sin\theta \sin\psi & \cos\theta
+ \end{array}
+ \right)
+ \f]
+
+ The vpRzyzVector class is derived from vpRotationVector.
+
+ The code below shows first how to initialize this representation of
+ Euler angles, than how to contruct a rotation matrix from a
+ vpRzyzVector and finaly how to extract the vpRzyzVector Euler angles
+ from the build rotation matrix.
+
+ \code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRzyzVector.h>
+
+int main()
+{
+ vpRzyzVector rzyz;
+
+ // Initialise the Euler angles
+ rzyz[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
+ rzyz[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
+ rzyz[2] = vpMath::rad( 90.f); // psi angle in rad/s around z axis
+
+ // Construct a rotation matrix from the Euler angles
+ vpRotationMatrix R(rzyz);
+
+ // Extract the Euler angles around z,y,z axis from a rotation matrix
+ rzyz.buildFrom(R);
+
+ // Print the extracted Euler angles. Values are the same than the
+ // one used for initialization
+ std::cout << rzyz;
+
+ // Since the rotation vector is 3 values column vector, the
+ // transpose operation produce a row vector.
+ vpRowVector rzyz_t = rzyz.t();
+
+ // Print the transpose row vector
+ std::cout << rzyz_t << std::endl;
+}
+ \endcode
+*/
+class VISP_EXPORT vpRzyzVector : public vpRotationVector
+{
+ public:
+ vpRzyzVector();
+ vpRzyzVector(const vpRzyzVector &rzyz);
+
+ // initialize a Rzyz vector from a rotation matrix
+ vpRzyzVector(const vpRotationMatrix& R);
+
+ // initialize a Rzyz vector from a ThetaU vector
+ vpRzyzVector(const vpThetaUVector& tu);
+
+ vpRzyzVector(const double phi, const double theta, const double psi);
+
+ //! Destructor.
+ virtual ~vpRzyzVector() {};
+
+ // convert a rotation matrix into Rzyz vector
+ vpRzyzVector buildFrom(const vpRotationMatrix& R) ;
+
+ // convert a ThetaU vector into a Rzyz vector
+ vpRzyzVector buildFrom(const vpThetaUVector& R) ;
+
+ void buildFrom(const double phi, const double theta, const double psi);
+
+ vpRzyzVector &operator=(double x) ;
+
+} ;
+#endif
+
diff --git a/modules/core/include/visp3/core/vpScale.h b/modules/core/include/visp3/core/vpScale.h
new file mode 100644
index 0000000..182b503
--- /dev/null
+++ b/modules/core/include/visp3/core/vpScale.h
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
+ *
+ * Authors:
+ * Andrew Comport
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpScale.h
+*/
+
+// =========================================================
+/*!
+ * \brief Contains various estimators for scale.
+ * \n Methods : Median Absolute Deviation (MAD),
+ * MPDE, Mean shift kernel density estimation.
+ * \author Andrew Comport
+ * \date 24/10/03
+ */
+// ==========================================================
+
+
+#ifndef VPSCALE_HH
+#define VPSCALE_HH
+
+#include <visp3/core/vpColVector.h>
+#include <math.h>
+
+
+/*!
+ \class vpScale
+ \ingroup group_core_robust
+
+ \brief Contains different methods for estimating the robust scale of
+ an error distribution.
+
+ Methods of Median Absolute Deviation and Density Gradient estimation
+ using the Mean Shift method.
+
+ \author Andrew Comport
+ \date 24/10/03
+ */
+class VISP_EXPORT vpScale
+{
+
+private:
+ double bandwidth;
+ unsigned int dimension;
+
+public:
+
+ //! Constructor
+ vpScale();
+ vpScale(double kernel_bandwidth, unsigned int dim=1);
+ //! Destructor
+ virtual ~vpScale(void);
+
+ double MeanShift(vpColVector &error);
+ double KernelDensity(vpColVector &error, unsigned int position);
+ double KernelDensityGradient(vpColVector &error, unsigned int position);
+
+ double KernelDensity_EPANECHNIKOV(vpColVector &X);
+ double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n);
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpServer.h b/modules/core/include/visp3/core/vpServer.h
new file mode 100644
index 0000000..5d4e1ce
--- /dev/null
+++ b/modules/core/include/visp3/core/vpServer.h
@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Server
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpServer_H
+#define vpServer_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpNetwork.h>
+
+
+/*!
+ \class vpServer
+
+ \ingroup group_core_network
+
+ \brief This class represents a Transmission Control Protocol (TCP) server.
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
+ on one computer to another program on another computer.
+
+ Exemple of server's code, receiving and sending basic message.
+ It corresponds to the client used in the first exemple of vpClient class'
+ documentation.
+
+ \code
+#include <visp3/core/vpServer.h>
+#include <iostream>
+
+int main(int argc,const char** argv)
+{
+ int port = 35000;
+ vpServer serv(port); //Launch the server on localhost
+ serv.start();
+
+ bool run = true;
+ int val;
+
+ while(run){
+ serv.checkForConnections();
+
+ if(serv.getNumberOfClients() > 0)
+ {
+ if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
+ std::cout << "Error while receiving" << std::endl;
+ else
+ std::cout << "Received : " << val << std::endl;
+
+ val = val+1;
+ if(serv.send(&val) != sizeof(int)) //Sending the new value to the first client
+ std::cout << "Error while sending" << std::endl;
+ else
+ std::cout << "Sending : " << val << std::endl;
+ }
+ }
+
+ return 0;
+}
+ \endcode
+
+ Exemple of server's code, receiving a vpImage on request form.
+ It correspond to the client used in the second exemple of vpClient class' documentation.
+
+ \code
+#include <visp3/core/vpServer.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+
+#include "vpRequestImage.h" //See vpRequest class documentation
+
+int main(int argc,const char** argv)
+{
+ int port = 35000;
+
+ std::cout << "Port: " << port << std::endl;
+ vpServer serv(port);
+ serv.start();
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX display;
+#elif defined(VISP_HAVE_GDI) //Win32
+ vpDisplayGDI display;
+#endif
+
+ vpImage<unsigned char> I;
+
+ vpRequestImage reqImage(&I);
+ serv.addDecodingRequest(&reqImage);
+
+ bool run = true;
+
+ while(run){
+ serv.checkForConnections();
+
+ if(serv.getNumberOfClients() > 0)
+ {
+ int index = serv.receiveAndDecodeRequestOnce();
+ std::string id = serv.getRequestIdFromIndex(index);
+
+ if(id == reqImage.getId())
+ {
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
+ if (! display.isInitialised() )
+ display.init(I, -1, -1, "Remote display");
+#endif
+
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I);
+
+ // A click in the viewer to exit
+ if ( vpDisplay::getClick(I, false) )
+ run = false;
+ }
+ }
+ }
+
+ return 0;
+}
+ \endcode
+
+ \sa vpClient
+ \sa vpRequest
+ \sa vpNetwork
+*/
+class VISP_EXPORT vpServer : public vpNetwork
+{
+private:
+
+ //######## PARAMETERS ########
+ //# #
+ //############################
+ std::string adress;
+ int port;
+ bool started;
+ unsigned int max_clients;
+
+public:
+
+ vpServer();
+ vpServer(const int &port);
+ vpServer(const std::string &adress_serv,const int &port_serv);
+
+ virtual ~vpServer();
+
+ bool checkForConnections();
+
+ /*!
+ Check if the server is started.
+
+ \sa vpServer::start()
+
+ \return True if the server is started, false otherwise.
+ */
+ bool isStarted(){ return started; }
+
+ /*!
+ Get the maximum number of clients that can be connected to the server.
+
+ \sa vpServer::setMaxNumberOfClients()
+
+ \return Maximum number of clients.
+ */
+ unsigned int getMaxNumberOfClients(){ return max_clients; }
+
+ /*!
+ Get the number of clients connected to the server.
+
+ \return Number of clients connected.
+ */
+ unsigned int getNumberOfClients(){ return (unsigned int)receptor_list.size(); }
+
+ void print();
+
+ bool start();
+
+ /*!
+ Set the maximum number of clients that can be connected to the server.
+
+ \sa vpServer::getMaxNumberOfClients()
+
+ \param l : Maximum number of clients.
+ */
+ void setMaxNumberOfClients(unsigned int &l){ max_clients = l; }
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpSphere.h b/modules/core/include/visp3/core/vpSphere.h
new file mode 100644
index 0000000..e4d87b3
--- /dev/null
+++ b/modules/core/include/visp3/core/vpSphere.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Sphere feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpSphere.h
+ \brief forward projection of a sphere
+*/
+
+#ifndef vpSphere_hh
+#define vpSphere_hh
+
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+#include <visp3/core/vpForwardProjection.h>
+#include <math.h>
+/*!
+ \class vpSphere
+ \ingroup group_core_geometry
+ \brief Class that defines what is a sphere.
+
+ Forward projection of a sphere.
+*/
+class VISP_EXPORT vpSphere : public vpForwardProjection
+{
+public:
+ void init() ;
+ vpSphere() ;
+ virtual ~vpSphere() ;
+
+public:
+ vpSphere(const vpColVector& oP) ;
+ vpSphere(const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+
+
+ void setWorldCoordinates(const vpColVector& oP) ;
+ void setWorldCoordinates(const double X0, const double Y0,
+ const double Z0,
+ const double R) ;
+
+ double get_x() const { return p[0] ; }
+ double get_y() const { return p[1] ; }
+ double get_mu20() const { return p[2] ; }
+ double get_mu11() const { return p[3] ; }
+ double get_mu02() const { return p[4] ; }
+
+ double getX() const { return cP[0] ; }
+ double getY() const { return cP[1] ; }
+ double getZ() const { return cP[2] ; }
+
+ double getR() const { return cP[3] ; }
+
+ void projection() ;
+ void projection(const vpColVector &cP, vpColVector &p) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo) ;
+
+ void display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color=vpColor::green,
+ const unsigned int thickness=1) ;
+
+ vpSphere *duplicate() const ;
+} ;
+
+
+#endif
diff --git a/modules/core/include/visp3/core/vpSubColVector.h b/modules/core/include/visp3/core/vpSubColVector.h
new file mode 100644
index 0000000..99a5b70
--- /dev/null
+++ b/modules/core/include/visp3/core/vpSubColVector.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpColVector .
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#ifndef __VP_SUB_COL_VECTOR__
+#define __VP_SUB_COL_VECTOR__
+
+#include <visp3/core/vpColVector.h>
+
+/*!
+ \file vpSubColVector.h
+
+ \brief Definition of the vpSubColVector class
+*/
+
+/*!
+ \class vpSubColVector
+ \ingroup group_core_matrices
+ This class provides a mask on a vpColVector. It has internally a
+ pointer to the parent vpColVector.
+ All properties of vpColVector are available with
+ a vpSubColVector.
+
+ \author Jean Laneurit (IRISA - INRIA Rennes)
+
+ \sa vpMatrix vpColvector vpRowVector
+*/
+class VISP_EXPORT vpSubColVector : public vpColVector
+{
+
+private :
+ //! Copy constructor unavaible
+ vpSubColVector(const vpSubColVector& /* m */);
+
+protected :
+
+ //! Number of row of parent vpColvector at initialization
+ unsigned int pRowNum;
+ //! Parent vpColvector
+ vpColVector *parent;
+
+public:
+
+ vpSubColVector();
+ vpSubColVector(vpColVector &v, const unsigned int & offset,const unsigned int & nrows);
+ virtual ~vpSubColVector();
+
+ void checkParentStatus() const;
+
+ void init(vpColVector &v, const unsigned int & offset,const unsigned int & nrows);
+
+ vpSubColVector &operator=(const vpSubColVector &B);
+
+ vpSubColVector &operator=(const vpPoseVector &p);
+ vpSubColVector &operator=(const vpRotationVector &rv);
+ vpSubColVector &operator=(const vpTranslationVector &tv);
+
+ vpSubColVector &operator=(const vpColVector &B);
+ vpSubColVector &operator=(const vpMatrix &B);
+ vpSubColVector &operator=(const double &x);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpSubMatrix.h b/modules/core/include/visp3/core/vpSubMatrix.h
new file mode 100644
index 0000000..499a012
--- /dev/null
+++ b/modules/core/include/visp3/core/vpSubMatrix.h
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpMatrix .
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#ifndef __VP_SUB_MATRIX__
+#define __VP_SUB_MATRIX__
+
+#include <visp3/core/vpMatrix.h>
+
+
+/*!
+ \file vpSubMatrix.h
+
+ \brief Definition of the vpSubMatrix class
+*/
+
+/*!
+ \class vpSubMatrix
+ \ingroup group_core_matrices
+ \brief Definition of the vpSubMatrix
+ vpSubMatrix class provides a mask on a vpMatrix
+ all properties of vpMatrix are available with
+ a vpSubMatrix
+
+ \author Jean Laneurit (IRISA - INRIA Rennes)
+
+ \sa vpMatrix vpColvector vpRowVector
+*/
+class VISP_EXPORT vpSubMatrix : public vpMatrix{
+
+ private :
+ //! Eye method unavailable
+ void eye(unsigned int n);
+ //! Eye method unavailable
+ void eye(unsigned int m, unsigned int n);
+ //! Copy constructor unavailable
+ vpSubMatrix(const vpSubMatrix& /* m */);
+
+ protected :
+
+ unsigned int pRowNum;
+ unsigned int pColNum;
+ vpMatrix *parent;
+
+ public:
+
+ //!Default constructor
+ vpSubMatrix();
+ //!Constructor
+ vpSubMatrix(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
+ //!Destructor
+ ~vpSubMatrix();
+
+ //! Initialisation of vpMatrix
+ void init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
+
+ //!Check is parent vpRowVector has changed since initialization
+ void checkParentStatus() const;
+
+ //! Operation such as subA = subB
+ vpSubMatrix & operator=(const vpSubMatrix &B);
+ //! Operation such as subA = B
+ vpSubMatrix & operator=(const vpMatrix &B);
+ //! Operation such as subA = x
+ vpSubMatrix & operator=(const double &x);
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpSubRowVector.h b/modules/core/include/visp3/core/vpSubRowVector.h
new file mode 100644
index 0000000..15db691
--- /dev/null
+++ b/modules/core/include/visp3/core/vpSubRowVector.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpRowVector .
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#ifndef __VP_SUB_ROW_VECTOR__
+#define __VP_SUB_ROW_VECTOR__
+
+#include <visp3/core/vpRowVector.h>
+
+
+/*!
+ \file vpSubRowVector.h
+
+ \brief Definition of the vpSubRowVector class
+*/
+
+/*!
+ \class vpSubRowVector
+ \ingroup group_core_matrices
+ This class provides a mask on a vpRowVector. It has internally a
+ pointer to the parent vpRowVector.
+ All properties of vpRowVector are available with
+ a vpSubRowVector.
+
+ \author Jean Laneurit (IRISA - INRIA Rennes)
+
+ \sa vpMatrix vpColvector vpRowVector
+*/
+
+class VISP_EXPORT vpSubRowVector : public vpRowVector
+{
+
+private :
+ //! Copy constructor unavaible
+ vpSubRowVector(const vpSubRowVector& /* m */);
+
+protected :
+
+ //! Number of row of parent vpColvector at initialization
+ unsigned int pColNum;
+ //! Parent vpColvector
+ vpRowVector *parent;
+
+public:
+
+ vpSubRowVector();
+ vpSubRowVector(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols);
+ virtual ~vpSubRowVector();
+
+ void checkParentStatus() const;
+
+ void init(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols);
+
+ vpSubRowVector & operator=(const vpSubRowVector &B);
+ vpSubRowVector & operator=(const vpRowVector &B);
+ vpSubRowVector & operator=(const vpMatrix &B);
+ vpSubRowVector & operator=(const double &x);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpThetaUVector.h b/modules/core/include/visp3/core/vpThetaUVector.h
new file mode 100644
index 0000000..d44a8f2
--- /dev/null
+++ b/modules/core/include/visp3/core/vpThetaUVector.h
@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Theta U parameterization for the rotation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpTHETAUVECTOR_H
+#define vpTHETAUVECTOR_H
+
+/*!
+ \file vpThetaUVector.h
+ \brief class that consider the case of the Theta U parameterization for the
+ rotation
+*/
+
+class vpHomogeneousMatrix;
+class vpRotationMatrix;
+class vpPoseVector;
+class vpRzyxVector;
+class vpRxyzVector;
+class vpRzyzVector;
+class vpColVector;
+class vpRotationVector;
+class vpQuaternionVector;
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRzyxVector.h>
+#include <visp3/core/vpQuaternionVector.h>
+
+
+/*!
+ \class vpThetaUVector
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a rotation vector as \f$\theta {\bf u}\f$ axis-angle
+ minimal representation.
+
+ Class that consider the case of the \f$\theta {\bf u}\f$
+ parameterization for the rotation.
+
+ The vpThetaUVector class is derived from vpRotationVector.
+
+ The \f$\theta {\bf u}\f$ representation is one of the minimal
+ representation of a rotation matrix, where
+ \f${\bf u} = (u_{x} \; u_{y} \; u_{z})^{\top}\f$
+ is a unit vector representing the rotation
+ axis and \f$\theta\f$ is the rotation angle.
+
+ From the \f$\theta {\bf u}\f$ representation it is possible to build the
+ rotation matrix \f${\bf R}\f$ using the Rodrigues formula:
+
+ \f[
+ {\bf R} = {\bf I}_{3} + (1 - \cos{ \theta}) \; {\bf u u}^{\top} + \sin{ \theta} \; [{\bf u}]_{\times}
+ \f]
+
+ with \f${\bf I}_{3}\f$ the identity matrix of dimension
+ \f$3\times3\f$ and \f$[{\bf u}]_{\times}\f$ the skew matrix:
+
+ \f[
+ [{\bf u}]_{\times} = \left(
+ \begin{array}{ccc}
+ 0 & -u_{z} & u_{y} \\
+ u_{z} & 0 & -u_{x} \\
+ -u_{y} & u_{x} & 0
+ \end{array}
+ \right)
+ \f]
+ From the implementation point of view, it is nothing more than an
+ array of three floats.
+
+ The code below shows first how to initialize a \f$\theta {\bf u}\f$
+ vector, than how to contruct a rotation matrix from a vpThetaUVector
+ and finaly how to extract the theta U angles from the build rotation
+ matrix.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+
+int main()
+{
+ vpThetaUVector tu;
+
+ // Initialise the theta U rotation vector
+ tu[0] = vpMath::rad( 45.f);
+ tu[1] = vpMath::rad(-30.f);
+ tu[2] = vpMath::rad( 90.f);
+
+ // Construct a rotation matrix from the theta U angles
+ vpRotationMatrix R(tu);
+
+ // Extract the theta U angles from a rotation matrix
+ tu.buildFrom(R);
+
+ // Print the extracted theta U angles. Values are the same than the
+ // one used for initialization
+ std::cout << tu;
+
+ // Since the rotation vector is 3 values column vector, the
+ // transpose operation produce a row vector.
+ vpRowVector tu_t = tu.t();
+
+ // Print the transpose row vector
+ std::cout << tu_t << std::endl;
+}
+ \endcode
+*/
+class VISP_EXPORT vpThetaUVector : public vpRotationVector
+{
+private:
+
+ static const double minimum;
+
+public:
+ vpThetaUVector();
+ vpThetaUVector(const vpThetaUVector &tu);
+
+ // constructor initialize a Theta U vector from a homogeneous matrix
+ vpThetaUVector(const vpHomogeneousMatrix & M) ;
+ // constructor initialize a Theta U vector from a pose vector
+ vpThetaUVector(const vpPoseVector & p) ;
+ // constructor initialize a Theta U vector from a rotation matrix
+ vpThetaUVector(const vpRotationMatrix& R) ;
+ // constructor initialize a Theta U vector from a RzyxVector
+ vpThetaUVector(const vpRzyxVector& rzyx) ;
+ // constructor initialize a Theta U vector from a RzyzVector
+ vpThetaUVector(const vpRzyzVector& rzyz) ;
+ // constructor initialize a Theta U vector from a RxyzVector
+ vpThetaUVector(const vpRxyzVector& rxyz) ;
+ vpThetaUVector(const vpQuaternionVector& q) ;
+
+ vpThetaUVector(const double tux, const double tuy, const double tuz);
+ //! Destructor.
+ virtual ~vpThetaUVector() {};
+
+ // convert an homogeneous matrix into Theta U vector
+ vpThetaUVector buildFrom(const vpHomogeneousMatrix& M) ;
+ // convert a pose vector into Theta U vector
+ vpThetaUVector buildFrom(const vpPoseVector& p) ;
+ // convert a rotation matrix into Theta U vector
+ vpThetaUVector buildFrom(const vpRotationMatrix& R) ;
+ // convert an Rzyx vector into Theta U vector
+ vpThetaUVector buildFrom(const vpRzyxVector &rzyx) ;
+ // convert an Rzyz vector into Theta U vector
+ vpThetaUVector buildFrom(const vpRzyzVector &zyz) ;
+ // convert an Rxyz vector into Theta U vector
+ vpThetaUVector buildFrom(const vpRxyzVector &xyz) ;
+ vpThetaUVector buildFrom(const vpQuaternionVector &q) ;
+
+ void buildFrom(const double tux, const double tuy, const double tuz);
+
+ vpThetaUVector &operator=(double x) ;
+
+ // extract the angle and the axis from the ThetaU representation
+ void extract( double &theta, vpColVector &u) const;
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpTime.h b/modules/core/include/visp3/core/vpTime.h
new file mode 100644
index 0000000..a61def4
--- /dev/null
+++ b/modules/core/include/visp3/core/vpTime.h
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Time management and measurement.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpTime_h
+#define vpTime_h
+
+/*!
+ \file vpTime.h
+ \brief Time management and measurement
+
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+/*!
+ \class vpTime
+ \ingroup group_core_time
+ \brief Time management and measurement.
+
+ The example below shows how to synchronize a loop to a given framerate.
+
+ \code
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+ double t;
+ for ( ; ; ) {
+ t = vpTime::measureTimeMs();
+ ...
+ vpTime::wait(t, 40); // Loop time is set to 40 ms, ie 25 Hz
+ }
+}
+ \endcode
+
+*/
+
+namespace vpTime
+{
+
+VISP_EXPORT double getMinTimeForUsleepCall();
+VISP_EXPORT double measureTimeSecond() ;
+VISP_EXPORT double measureTimeMs() ;
+VISP_EXPORT double measureTimeMicros() ;
+VISP_EXPORT void sleepMs(double t);
+VISP_EXPORT int wait(double t0, double t) ;
+VISP_EXPORT void wait(double t) ;
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpTracker.h b/modules/core/include/visp3/core/vpTracker.h
new file mode 100644
index 0000000..56f0701
--- /dev/null
+++ b/modules/core/include/visp3/core/vpTracker.h
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic tracker.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpTracker_H
+#define vpTracker_H
+
+/*!
+ \file vpTracker.h
+ \brief Class that defines what is a generic tracker.
+*/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+
+/*!
+ \class vpTracker
+ \ingroup group_core_trackers
+ \brief Class that defines what is a feature generic tracker.
+
+ A tracker is able to track features with parameters expressed in:
+ - in the camera frame \e cP. These parameters are located in the public
+ attribute vpTracker::cP.
+ - in the image plane \e p. These parameters are located in the public
+ attribute vpTracker::p. They correspond to normalized coordinates
+ of the feature expressed in meters.
+
+*/
+class VISP_EXPORT vpTracker
+{
+public:
+ /*!
+ Feature coordinates expressed in the image plane \e p. They correspond
+ to 2D normalized coordinates expressed in meters.
+ */
+ vpColVector p ;
+ /*!
+ Feature coordinates expressed in the camera frame \e cP.
+ */
+ vpColVector cP ;
+
+ /*!
+ Flag used to indicate if the feature parameters \e cP expressed
+ in the camera frame are available.
+ */
+ bool cPAvailable ;
+
+public:
+ //! Default initialization.
+ void init() ;
+ //! Default constructor.
+ vpTracker() ;
+ //! Copy constructor.
+ vpTracker(const vpTracker &tracker) ;
+ //! Copy operator.
+ vpTracker &operator=(const vpTracker &tracker);
+
+ //! Destructor.
+ virtual ~vpTracker() { ; }
+} ;
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpTrackingException.h b/modules/core/include/visp3/core/vpTrackingException.h
new file mode 100644
index 0000000..bf8f51a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpTrackingException.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpTracking class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpTrackingException_H
+#define __vpTrackingException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpTrackingException.h
+ \brief error that can be emited by the vpTracker class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpException.h>
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpTrackingException
+ \ingroup group_core_debug
+ \brief Error that can be emited by the vpTracker class and its derivates.
+ */
+class VISP_EXPORT vpTrackingException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpTracking member
+ */
+ enum errorTrackingCodeEnum
+ {
+ featureLostError,
+
+ // Moving edges
+ notEnoughPointError,
+ initializationError,
+ fatalError
+ } ;
+
+ public:
+ vpTrackingException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpTrackingException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpTrackingException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpTranslationVector.h b/modules/core/include/visp3/core/vpTranslationVector.h
new file mode 100644
index 0000000..ad5a3b9
--- /dev/null
+++ b/modules/core/include/visp3/core/vpTranslationVector.h
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Translation vector.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpTRANSLATIONVECTOR_H
+#define vpTRANSLATIONVECTOR_H
+
+/*!
+ \file vpTranslationVector.h
+ \brief Class that consider the case of a translation vector.
+*/
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoseVector.h>
+
+
+/*!
+ \class vpTranslationVector
+
+ \ingroup group_core_transformations
+
+ \brief Class that consider the case of a translation vector.
+
+ Let us denote \f$^{a}{\bf t}_{b} = [t_x,t_y,t_z]^\top\f$ the translation
+ from frame \f$ a \f$ to frame \f$ b \f$. The representation of a
+ translation is a column vector of dimension 3.
+
+ Translations are expressed in meters.
+
+ The code below shows how to use a translation vector to build an
+ homogeneous matrix.
+
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpTranslationVector.h>
+
+int main()
+{
+ vpTranslationVector t; // Translation vector
+
+ // Initialization of the translation vector
+ t[0] = 0.2; // tx = 0.2 meters
+ t[1] = -0.1; // ty = -0.1 meters
+ t[2] = 1.0; // tz = 1 meters
+
+ // Construction of a rotation matrix
+ vpRotationMatrix R; // Set to identity by default
+
+ // Construction of an homogeneous matrix
+ vpHomogeneousMatrix M(t, R);
+}
+ \endcode
+*/
+class VISP_EXPORT vpTranslationVector : public vpArray2D<double>
+{
+public:
+
+ /*!
+ Default constructor.
+ The translation vector is initialized to zero.
+ */
+ vpTranslationVector() : vpArray2D<double>(3, 1) {};
+ vpTranslationVector(const double tx, const double ty, const double tz) ;
+ vpTranslationVector(const vpTranslationVector &tv);
+ vpTranslationVector(const vpHomogeneousMatrix &M);
+ vpTranslationVector(const vpPoseVector &p);
+
+ vpTranslationVector buildFrom(const double tx, const double ty, const double tz) ;
+ vpTranslationVector buildFrom(const vpHomogeneousMatrix& M) ;
+ vpTranslationVector buildFrom(const vpPoseVector& p) ;
+
+ double euclideanNorm() const;
+
+ // operators
+
+ // translation vectors additions c = a + b (a, b unchanged)
+ vpTranslationVector operator+(const vpTranslationVector &tv) const ;
+ // translation vectors substraction c = a - b (a, b unchanged)
+ vpTranslationVector operator-(const vpTranslationVector &tv) const ;
+ // negate t = -a (t is unchanged)
+ vpTranslationVector operator-() const ;
+ vpMatrix operator*(const vpRowVector &v) const;
+ // b = x * a (x=scalar)
+ vpTranslationVector operator*(const double x) const;
+ vpTranslationVector & operator*=(double x);
+ vpTranslationVector operator/(const double x) const;
+ vpTranslationVector & operator/=(double x);
+ // Copy operator. Allow operation such as A = v
+ vpTranslationVector &operator=(const vpTranslationVector &tv);
+
+ vpTranslationVector &operator=(double x) ;
+
+ //! Operator that allows to set a value of an element \f$t_i\f$: t[i] = x
+ inline double &operator [](unsigned int n) { return *(data + n); }
+ //! Operator that allows to get the value of an element \f$t_i\f$: x = t[i]
+ inline const double &operator [](unsigned int n) const { return *(data+n); }
+
+ /*!
+ This function is not applicable to a translation vector that is always a
+ 3-by-1 column vector.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize a translation vector"));
+ };
+
+ void set(const double tx, const double ty, const double tz) ;
+
+ // Skew Symmetric matrix
+ vpMatrix skew() const ;
+
+ double sumSquare() const;
+
+ vpRowVector t() const;
+
+ static vpTranslationVector cross(const vpTranslationVector &a,
+ const vpTranslationVector &b) ;
+ static vpMatrix skew(const vpTranslationVector &tv) ;
+ static void skew(const vpTranslationVector &tv, vpMatrix &M) ;
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpTriangle.h b/modules/core/include/visp3/core/vpTriangle.h
new file mode 100644
index 0000000..64de1db
--- /dev/null
+++ b/modules/core/include/visp3/core/vpTriangle.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a 2D triangle.
+ *
+ * Author:
+ * Amaury Dame
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpTriangle_h
+#define vpTriangle_h
+
+/*!
+ \class vpTriangle
+ \ingroup group_core_geometry
+ \brief Defines a 2D triangle.
+
+ A triangle is internally represented by three 2D points.
+
+ By default the three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint class documentation for more details about the frame.) are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
+*/
+
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMatrix.h>
+
+class VISP_EXPORT vpTriangle
+{
+ private:
+ bool goodTriange;
+ vpImagePoint S1;
+ double uvinv00;
+ double uvinv01;
+ double uvinv10;
+ double uvinv11;
+ double ptempo0;
+ double ptempo1;
+ double area;
+ vpImagePoint apex1;
+ vpImagePoint apex2;
+ vpImagePoint apex3;
+
+ public:
+ vpTriangle();
+ vpTriangle(const vpTriangle &tri);
+ virtual ~vpTriangle();
+
+ vpTriangle &operator=(const vpTriangle& tri);
+
+ vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
+
+ void buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
+
+ bool inTriangle(const vpImagePoint &iP, double threshold = 0.00001);
+
+ /*!
+ Get the apexes of the triangle.
+
+ \param iP1 : first apex.
+ \param iP2 : second apex.
+ \param iP3 : third apex.
+ */
+ void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, vpImagePoint &iP3) const {
+ iP1 = apex1;
+ iP2 = apex2;
+ iP3 = apex3;}
+
+
+ /*!
+ Return the area of the triangle.
+ The area is computed when the triangle is built from three image points.
+
+ \return The area of the triangle.
+ */
+ inline double getArea() const{
+ return this->area;
+ }
+
+ private:
+ void init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
+
+};
+
+#endif
+
diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h
new file mode 100644
index 0000000..b595a72
--- /dev/null
+++ b/modules/core/include/visp3/core/vpUniRand.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generation of random number with uniform and normal probability density.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpUniRand_hh
+#define vpUniRand_hh
+
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \class vpUniRand
+
+ \ingroup group_core_random
+ \brief Class for generating random numbers with uniform probability density.
+
+ The algorithms and notations used are described in \cite Gentle:2004.
+
+ The following example shows how to use this class to generate 10 numbers between 0 and 5.
+\code
+#include <iostream>
+#include <visp3/core/vpUniRand.h>
+
+int main()
+{
+ vpUniRand r;
+ for(unsigned int i=0;i<10;++i)
+ std::cout << 5*r() << std::endl;
+}
+\endcode
+ */
+class VISP_EXPORT vpUniRand
+{
+ long a;
+ long m; //2^31-1
+ long q; //integer part of m/a
+ long r;//r=m mod a
+ double normalizer; //we use a normalizer > m to ensure ans will never be 1 (it is the case if x = 739806647)
+
+private:
+ void draw0();
+protected:
+ long x;
+ double draw1();
+
+public:
+ //! Default constructor.
+ vpUniRand(const long seed = 0)
+ : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), x((seed)? seed : 739806647)
+ {}
+
+ //! Default destructor.
+ virtual ~vpUniRand() {};
+
+ //! Operator that allows to get a random value.
+ double operator()() {
+ return draw1();
+ }
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h
new file mode 100644
index 0000000..afe4c6d
--- /dev/null
+++ b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Velocity twist transformation matrix.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpVelocityRwistMatrix_h
+#define vpVelocityRwistMatrix_h
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+class vpHomogeneousMatrix;
+class vpColVector;
+
+/*!
+ \class vpVelocityTwistMatrix
+
+ \ingroup group_core_transformations
+
+ \brief Implementation of a velocity twist matrix and operations on such kind of matrices.
+
+ Class that consider the particular case of twist
+ transformation matrix that allows to transform a velocity skew from
+ one frame to an other.
+
+ The vpVelocityTwistMatrix class is derived from vpArray2D<double>.
+
+ A twist transformation matrix is a 6x6 matrix that express a velocity in frame <em>a</em> knowing
+ velocity in <em>b</em>. This matrix is defined as:
+ \f[
+ ^a{\bf V}_b = \left[\begin{array}{cc}
+ ^a{\bf R}_b & [^a{\bf t}_b]_\times \; ^a{\bf R}_b\\
+ {\bf 0}_{3\times 3} & ^a{\bf R}_b
+ \end{array}
+ \right]
+ \f]
+
+ where \f$ ^a{\bf R}_b \f$ is a rotation matrix and
+ \f$ ^a{\bf t}_b \f$ is a translation vector.
+
+ The vpVelocityTwistMatrix is derived from vpArray2D.
+
+ The code belows shows for example how to convert a velocity skew
+ from camera frame to a fix frame.
+
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+int main()
+{
+ vpVelocityTwistMatrix fVc; // Twist transformation matrix from fix to camera frame
+
+ vpHomogeneousMatrix fMc; // Fix to camera frame transformation
+ // ... fMc need here to be initialized
+
+ fVc.buildFrom(fMc);
+
+ vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
+ // ... c_v should here have an initial value
+
+ vpColVector f_v(6); // Velocity in the fix frame: vx,vy,vz,wx,wy,wz
+
+ // Compute the velocity in the fix frame
+ f_v = fVc * c_v;
+}
+ \endcode
+*/
+class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D<double>
+{
+ friend class vpMatrix;
+
+ public:
+ // basic constructor
+ vpVelocityTwistMatrix();
+ // copy constructor
+ vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V);
+ // constructor from an homogeneous transformation
+ vpVelocityTwistMatrix(const vpHomogeneousMatrix &M);
+
+ // Construction from Translation and rotation (ThetaU parameterization)
+ vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
+ // Construction from Translation and rotation (matrix parameterization)
+ vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R);
+ vpVelocityTwistMatrix(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz);
+ /*!
+ Destructor.
+ */
+ virtual ~vpVelocityTwistMatrix() {};
+
+
+ vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
+ const vpRotationMatrix &R);
+ vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
+ const vpThetaUVector &thetau);
+ vpVelocityTwistMatrix buildFrom(const vpHomogeneousMatrix &M) ;
+
+ void extract( vpRotationMatrix &R) const;
+ void extract(vpTranslationVector &t) const;
+
+ // Basic initialisation (identity)
+ void eye() ;
+
+ vpVelocityTwistMatrix inverse() const ;
+ void inverse(vpVelocityTwistMatrix &V) const;
+
+ vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const ;
+ vpMatrix operator*(const vpMatrix &M) const ;
+ vpColVector operator*(const vpColVector &v) const ;
+
+ vpVelocityTwistMatrix &operator=(const vpVelocityTwistMatrix &V);
+
+ int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+
+ /*!
+ This function is not applicable to a velocity twist matrix that is always a
+ 6-by-6 matrix.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize a velocity twist matrix"));
+ };
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /*!
+ \deprecated Provided only for compat with previous releases.
+ This function does nothing.
+ */
+ vp_deprecated void init() {};
+ /*!
+ \deprecated You should rather use eye().
+ */
+ vp_deprecated void setIdentity();
+ //@}
+#endif
+} ;
+
+#endif
diff --git a/modules/core/include/visp3/core/vpXmlParser.h b/modules/core/include/visp3/core/vpXmlParser.h
new file mode 100644
index 0000000..e3323a0
--- /dev/null
+++ b/modules/core/include/visp3/core/vpXmlParser.h
@@ -0,0 +1,307 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tools to automatise the creation of xml parser based on the libXML2
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#ifndef vpXmlParser_HH
+#define vpXmlParser_HH
+
+/*!
+ \file vpXmlParser.h
+ \brief Tools to simplify the creation of xml parser based on the libXML2
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <visp3/core/vpException.h>
+
+#include <libxml/parser.h>
+
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <typeinfo>
+#include <map>
+#include <string.h>
+
+
+/*!
+ \class vpXmlParser
+
+ \brief This class intends to simplify the creation of xml parser based on the
+ libxml2 third party library.
+
+ This class can be useful to manage external data parameters (for example for
+ configuration of an experiment, ...).
+
+ \warning This class is only available if libxml2 is installed and detected by ViSP.
+
+ In order to use this class, you have to create a new class inheriting from this one.
+ In the child class, you have to implement the methods:
+ - writeMainClass()
+ - readMainClass()
+
+ These two methods depends on the data to parse, and must not be directly called
+ (they are called from the parse() and the save() methods).
+
+ Following is an example of implementation for the document:
+
+ \code
+ <config>
+ <range>5</range>
+ <step>7</step>
+ <size_filter>3</size_filter>
+ </config>
+ \endcode
+
+ A class to parse this document is declared as follows:
+
+ \code
+ class vpDataParser: public vpXmlParser
+ {
+ private:
+ int m_range;
+ int m_step;
+ int m_size_filter
+ public:
+ typedef enum{
+ config,
+ range,
+ step,
+ size_filter
+ }dataToParse
+
+ vpDataParser(){
+ nodeMap["config"] = config;
+ nodeMap["range"] = range;
+ nodeMap["step"] = step;
+ nodeMap["size_filter"] = size_filter;
+ }
+
+ virtual void writeMainClass(xmlNodePtr node);
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+
+ // additionals methods specific to the data to parse
+ // such as: accessors
+ }
+ \endcode
+
+ The readMainClass function implementation is:
+
+ \code
+ void
+ vpDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+ {
+ for (xmlNodePtr tmpNode = node->xmlChildrenNode; tmpNode != NULL; tmpNode = tmpNode->next) {
+ if(tmpNode->type == XML_ELEMENT_NODE){
+
+ std::map<std::string, int>::iterator iter= this->nodeMap.find((char*)tmpNode->name);
+ if(iter == nodeMap.end()){
+ continue;
+ }
+
+ switch (iter->second){
+ case range:
+ this->m_range = xmlReadIntChild(doc, tmpNode);
+ break;
+ case step:
+ this->m_step = xmlReadIntChild(doc, tmpNode);
+ break;
+ case size_filter:
+ this->m_size_filter = xmlReadIntChild(doc, tmpNode);
+ break;
+ default:
+ std::cout << "problem in the readMainClass (" << iter->second << " , " << iter->first << " )" << std::endl;
+ break;
+ }
+ }
+ }
+ }
+ \endcode
+
+ Data can now be accessed through the internal variables of the class vpDataParser.
+
+ To store the data in a xml file, the function save has to be called. This
+ function needs the implementation of the writeMainClass function.
+
+ For example,
+
+ \code
+ void
+ vpDataParser::writeMainClass(xmlNodePtr node)
+ {
+ xmlWriteIntChild(node, "range", m_range);
+ xmlWriteIntChild(node, "step", m_step);
+ xmlWriteIntChild(node, "size_filter", m_size_filter);
+ }
+ \endcode
+
+*/
+class VISP_EXPORT vpXmlParser
+{
+protected:
+ std::string xmlReadStringChild (xmlDocPtr doc, xmlNodePtr node);
+ char* xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node);
+ int xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node);
+ unsigned int xmlReadUnsignedIntChild (xmlDocPtr doc, xmlNodePtr node);
+ double xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node);
+
+
+ void xmlWriteStringChild(xmlNodePtr node, const char* label, const std::string& value);
+ void xmlWriteCharChild(xmlNodePtr node, const char* label, const char* value);
+ void xmlWriteIntChild(xmlNodePtr node, const char* label, const int value);
+ void xmlWriteUnsignedIntChild(xmlNodePtr node, const char* label, const unsigned int value);
+ void xmlWriteDoubleChild(xmlNodePtr node, const char* label, const double value);
+
+ /*!
+ The map describing the data to parse
+ */
+ std::map<std::string, int> nodeMap;
+
+ /*!
+ The name of the main tag for the file to parse
+ */
+ std::string main_tag;
+
+public:
+
+ vpXmlParser();
+ vpXmlParser(const vpXmlParser& _twin);
+ virtual ~vpXmlParser();
+
+ /*!
+ As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
+ to clean up memory allocated by the xml2 library itself, the user should call
+ xmlCleanupParser() only when the process has finished using the xml2 library.
+ In case of doubt abstain from calling this function or do it just before calling exit()
+ to avoid leak reports from valgrind ! That's why
+ in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
+ function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
+ just before exit().
+ */
+ static void cleanup()
+ {
+ xmlCleanupParser();
+ }
+
+ /* virtual */ void parse(const std::string& filename);
+ /* virtual */ void save(const std::string& filename, const bool append=false);
+
+ /*!
+ Set the map describing the data to parse. This map stores the name of each
+ node and an associated key used to simplify the parsing of the file.
+
+ If the following file want to be parsed:
+
+ \code
+ <config>
+ <range>5</range>
+ <step>7</step>
+ <size_filter>3</size_filter>
+ </config>
+ \endcode
+
+ The following map has to be declared:
+
+ \code
+ std::map dataToParse;
+ dataToParse["config"] = 0;
+ dataToParse["range"] = 1;
+ dataToParse["step"] = 2;
+ dataToParse["size_filter"] = 3;
+ \endcode
+
+ Or, you can use keyzord instead of number as key but it implies to declare
+ in the child class an enumeration type of the name. For example:
+
+ \code
+ typedef enum{
+ config,
+ range,
+ step,
+ size_filter} data_enum;
+
+ std::map dataToParse;
+ dataToParse["config"] = config;
+ dataToParse["range"] = range;
+ dataToParse["step"] = step;
+ dataToParse["size_filter"] = size_filter;
+ \endcode
+
+ \param _map : the map describing the data to parse
+ */
+ void setMap(const std::map<std::string, int>& _map){ nodeMap = _map;}
+
+ /*!
+ set the name of the main tag
+
+ The main tag corresponds to the name of the root node
+
+ \param tag : name of the root node of the document
+ */
+ inline void setMainTag(const std::string& tag){ main_tag = tag;}
+
+protected:
+ /*!
+ pure virtual method used to read the document.
+
+ As the content of the function depends on the structure of the file to read,
+ data name, data types and data values, it has to be reimplemented for every
+ type of filenam
+
+ \param doc : a pointer representing the document
+ \param node : the root node of the document
+ */
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node)=0;
+
+ /*!
+ pure virtual method used to write the document.
+
+ As the content of the function depends on the structure of the file to read,
+ data name and data types, it has to be reimplemented for every
+ type of file to parse.
+
+ \param node : the root node of the document
+ */
+ virtual void writeMainClass(xmlNodePtr node)=0;
+
+};
+
+
+#endif /* VISP_HAVE_XML2 */
+
+#endif
diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h
new file mode 100644
index 0000000..ccd6a2a
--- /dev/null
+++ b/modules/core/include/visp3/core/vpXmlParserCamera.h
@@ -0,0 +1,355 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load and save camera intrinsic parameters.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpXmlParserCamera.h
+ \brief Declaration of the vpXmlParserCamera class.
+ Class vpXmlParserCamera allowed to load and save intrinsic camera parameters
+
+*/
+
+
+#ifndef vpXMLPARSERCAMERA_H
+#define vpXMLPARSERCAMERA_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <string>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpXmlParser.h>
+#include <libxml/xmlmemory.h> /* Functions of libxml. */
+
+/*!
+ \class vpXmlParserCamera
+
+ \ingroup group_core_camera
+
+ \brief XML parser to load and save intrinsic camera parameters.
+
+ To have a complete description of the camera parameters and the
+ corresponding projection model implemented in ViSP, see
+ vpCameraParameters.
+
+ Example of an XML file "myXmlFile.xml" containing intrinsic camera
+ parameters:
+
+ \code
+ <?xml version="1.0"?>
+ <root>
+ <camera>
+ <name>myCamera</name>
+ <image_width>640</image_width>
+ <image_height>480</image_height>
+ <model>
+ <type>perspectiveProjWithoutDistortion</type>
+ <px>1129.0</px>
+ <py>1130.6</py>
+ <u0>317.9</u0>
+ <v0>229.1</v0>
+ </model>
+ <model>
+ <type>perspectiveProjWithDistortion</type>
+ <px>1089.9</px>
+ <py>1090.1</py>
+ <u0>326.1</u0>
+ <v0>230.5</v0>
+ <kud>-0.196</kud>
+ <kdu>0.204</kdu>
+ </model>
+ </camera>
+ </root>
+ \endcode
+
+ Example of loading existing camera parameters from an XML file:
+ \code
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpXmlParserCamera.h>
+
+int main()
+{
+ vpCameraParameters cam; // Create a camera parameter container
+
+#ifdef VISP_HAVE_XML2
+ vpXmlParserCamera p; // Create a XML parser
+ vpCameraParameters::vpCameraParametersProjType projModel; // Projection model
+ // Use a perspective projection model without distortion
+ projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
+ // Parse the xml file "myXmlFile.xml" to find the intrinsic camera
+ // parameters of the camera named "myCamera" for the image sizes 640x480,
+ // for the projection model projModel. The size of the image is optional
+ // if camera parameters are given only for one image size.
+ if (p.parse(cam, "myXmlFile.xml", "myCamera", projModel,640,480) != vpXmlParserCamera::SEQUENCE_OK) {
+ std::cout << "Cannot found myCamera" << std::endl;
+ }
+
+ // cout the parameters
+ cam.printParameters();
+
+ // Get the camera parameters for the model without distortion
+ double px = cam.get_px();
+ double py = cam.get_py();
+ double u0 = cam.get_u0();
+ double v0 = cam.get_v0();
+
+ // Now we modify the principal point (u0,v0) for example to add noise
+ u0 *= 0.9;
+ v0 *= 0.8;
+
+ // Set the new camera parameters
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+
+ // Save the parameters in a new file "myXmlFileWithNoise.xml"
+ p.save(cam,"myXmlFileWithNoise.xml",p.getCameraName(),p.getWidth(),p.getHeight());
+
+ // Clean up memory allocated by the xml library
+ vpXmlParser::cleanup();
+#endif
+}
+ \endcode
+
+ Example of writing an XML file containing intrinsic camera parameters:
+ \code
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpXmlParserCamera.h>
+
+int main()
+{
+ // Create a camera parameter container. We want to set these parameters
+ // for a 320x240 image, and we want to use the perspective projection
+ // modelisation without distortion.
+ vpCameraParameters cam;
+
+ // Set the principal point coordinates (u0,v0)
+ double u0 = 162.3;
+ double v0 = 122.4;
+ // Set the pixel ratio (px, py)
+ double px = 563.2;
+ double py = 564.1;
+
+ // Set the camera parameters for a model without distortion
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+
+#ifdef VISP_HAVE_XML2
+ // Create a XML parser
+ vpXmlParserCamera p;
+ // Save the camera parameters in an XML file.
+ if (p.save(cam, "myNewXmlFile.xml", "myNewCamera", 320, 240) != vpXmlParserCamera::SEQUENCE_OK) {
+ std::cout << "Cannot save camera parameters" << std::endl;
+ }
+
+ // Clean up memory allocated by the xml library
+ vpXmlParser::cleanup();
+#endif
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpXmlParserCamera: public vpXmlParser
+{
+
+public:
+
+ /* --- XML Code------------------------------------------------------------ */
+ typedef enum
+ {
+ CODE_XML_BAD = -1,
+ CODE_XML_OTHER,
+ CODE_XML_CAMERA,
+ CODE_XML_CAMERA_NAME,
+ CODE_XML_HEIGHT,
+ CODE_XML_WIDTH,
+ CODE_XML_SUBSAMPLING_WIDTH,
+ CODE_XML_SUBSAMPLING_HEIGHT,
+ CODE_XML_FULL_HEIGHT,
+ CODE_XML_FULL_WIDTH,
+ CODE_XML_MODEL,
+ CODE_XML_MODEL_TYPE,
+ CODE_XML_U0,
+ CODE_XML_V0,
+ CODE_XML_PX,
+ CODE_XML_PY,
+ CODE_XML_KUD,
+ CODE_XML_KDU
+ } vpXmlCodeType;
+
+ typedef enum
+ {
+ SEQUENCE_OK ,
+ SEQUENCE_ERROR
+ } vpXmlCodeSequenceType;
+
+private :
+
+ vpCameraParameters camera;
+ std::string camera_name;
+ unsigned int image_width;
+ unsigned int image_height;
+ unsigned int subsampling_width;
+ unsigned int subsampling_height;
+ unsigned int full_width;
+ unsigned int full_height;
+
+ //! Allowed size difference between input image and data from the xml parser to handle minor differences (ex. FORMAT7 can creates 648*488 images).
+ static const int allowedPixelDiffOnImageSize = 15;
+
+public:
+
+ vpXmlParserCamera();
+ vpXmlParserCamera(vpXmlParserCamera& twinParser);
+ vpXmlParserCamera& operator =(const vpXmlParserCamera& twinparser);
+ ~vpXmlParserCamera(){}
+
+ int parse(vpCameraParameters &cam, const std::string &filename,
+ const std::string &camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int image_width = 0, const unsigned int image_height = 0);
+ int save(const vpCameraParameters &cam, const std::string &filename,
+ const std::string &camera_name,
+ const unsigned int image_width = 0, const unsigned int image_height = 0);
+
+ // get/set functions
+ std::string getCameraName(){return this->camera_name;}
+ unsigned int getWidth(){ return this->image_width; }
+ unsigned int getHeight(){ return this->image_height; }
+ unsigned int getSubsampling_width(){return this->subsampling_width;}
+ unsigned int getSubsampling_height(){return this->subsampling_height;}
+ vpCameraParameters getCameraParameters(){return this->camera;}
+
+ void setCameraName(const std::string& name){
+ this->camera_name = name;
+ }
+ void setWidth(const unsigned int width){ this->image_width = width ; }
+ void setHeight(const unsigned int height){ this->image_height = height ; }
+ void setSubsampling_width(const unsigned int subsampling){
+ this->subsampling_width = subsampling ;
+ }
+ void setSubsampling_height(const unsigned int subsampling){
+ this->subsampling_height = subsampling ;
+ }
+
+private:
+ int read (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ int count (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ int read_camera (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ xmlNodePtr find_camera (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& camera_name,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ vpXmlCodeSequenceType read_camera_model (xmlDocPtr doc, xmlNodePtr node,
+ vpCameraParameters &camera);
+
+ int read_camera_header (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& camera_name,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
+ void myXmlReadIntChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ int &res,
+ vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadDoubleChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ double &res,
+ vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadCharChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ char **res);
+ int write (xmlNodePtr node, const std::string& camera_name,
+ const unsigned int image_width = 0,
+ const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+ int write_camera(xmlNodePtr node_camera);
+
+private:
+
+ /*!
+
+ \param 2doc : a pointer representing the document
+ \param node : the root node of the document
+ */
+ virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
+
+ /*!
+
+
+ \param node2 : the root node of the document
+ */
+ virtual void writeMainClass(xmlNodePtr ){};
+
+};
+#endif //VISP_HAVE_XML2
+#endif
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h
new file mode 100644
index 0000000..7a83c29
--- /dev/null
+++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h
@@ -0,0 +1,258 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load and save Homogeneous Matrix in a XML file
+ *
+ * Authors:
+ * Giovanni Claudio
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpXmlParserHomogeneousMatrix.h
+ \brief Declaration of the vpXmlParserHomogeneousMatrix class.
+ Class vpXmlParserHomogeneousMatrix allowed to load and save Homogeneous Matrixes in a file XML
+
+*/
+
+#ifndef vpXMLPARSERHOMOGENEOUSMATRIX_H
+#define vpXMLPARSERHOMOGENEOUSMATRIX_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <string>
+#include <visp3/core/vpXmlParser.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <libxml/xmlmemory.h> /* Functions of libxml. */
+
+/*!
+ \class vpXmlParserHomogeneousMatrix
+
+ \ingroup group_core_transformations
+
+ \brief XML parser to load and save an homogeneous matrix in a file.
+
+ To have a complete description of the homogeneous matrix implemented in ViSP, see
+ vpHomogeneousMatrix.
+
+ Example of an XML file "homogeneous_matrixes.xml" containing a Pose vector
+ that will be converted in an homogeneous matrix:
+
+ \code
+<?xml version="1.0"?>
+<root>
+ <homogeneous_transformation>
+ <!--Name of the homogeneous matrix-->
+ <name>eMc</name>
+ <values>
+ <!--Translation vector with values in meters -->
+ <tx>1.00</tx>
+ <ty>1.30</ty>
+ <tz>3.50</tz>
+ <!--Rotational vector expressed in angle axis representation with values in radians -->
+ <theta_ux>0.20</theta_ux>
+ <theta_uy>0.30</theta_uy>
+ <theta_uz>0.50</theta_uz>
+ </values>
+ </homogeneous_transformation>
+</root>
+ \endcode
+
+ Example of loading an existing homogeneous matrix from an XML file.
+ \code
+
+#include <iostream>
+#include <string>
+
+#include <visp3/core/vpXmlParserHomogeneousMatrix.h>
+
+int main(int argc, char* argv[])
+{
+#ifdef VISP_HAVE_XML2
+ vpHomogeneousMatrix eMc;
+
+ // Create a XML parser
+ vpXmlParserHomogeneousMatrix p;
+
+ // Define the name of the matrix to load
+ std::string name = "eMc";
+
+ if (p.parse(eMc,"homogeneous_matrixes.xml", name) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
+ std::cout << "Cannot found the Homogeneous matrix named " << name << "." << std::endl;
+ }
+ else
+ std::cout << "Homogeneous matrix " << name <<": " << std::endl << eMc << std::endl;
+#endif
+
+ return 0;
+}
+ \endcode
+
+ Example of writing an homogenoeus matrix in a XML file.
+ \note Before writing an homogeneous matrix check if there
+ is already in the xml file a matrix with the same name.
+ If you are sure to overwrite it please delete it manually
+ from the file before.
+
+ \code
+#include <iostream>
+#include <string>
+
+#include <visp3/core/vpXmlParserHomogeneousMatrix.h>
+
+int main(int argc, char* argv[])
+{
+#ifdef VISP_HAVE_XML2
+ // Create Pose Vector and convert to homogeneous matrix
+ vpPoseVector r(1.0,1.3,3.5,0.2,0.3,0.5);
+ vpHomogeneousMatrix M(r);
+
+ // Create a XML parser
+ vpXmlParserHomogeneousMatrix p;
+
+ // Define the name of the matrix
+ std::string name_M = "eMe";
+
+ // Define name of the file xml to fill
+ char filename[FILENAME_MAX];
+ sprintf(filename, "%s", "homogeneous_matrixes.xml");
+
+ if (p.save(M, filename, name_M) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
+ std::cout << "Cannot save the Homogeneous matrix" << std::endl;
+ }
+
+ vpXmlParser::cleanup();
+#endif
+ return 0;
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpXmlParserHomogeneousMatrix: public vpXmlParser
+{
+
+public:
+
+ /* --- XML Code------------------------------------------------------------ */
+ typedef enum
+ {
+ CODE_XML_BAD = -1,
+ CODE_XML_OTHER,
+ CODE_XML_M,
+ CODE_XML_M_NAME,
+ CODE_XML_VALUE,
+ CODE_XML_TX,
+ CODE_XML_TY,
+ CODE_XML_TZ,
+ CODE_XML_TUX,
+ CODE_XML_TUY,
+ CODE_XML_TUZ
+ } vpXmlCodeType;
+
+ typedef enum
+ {
+ SEQUENCE_OK,
+ SEQUENCE_ERROR
+ } vpXmlCodeSequenceType;
+
+private :
+
+ vpHomogeneousMatrix m_M;
+ std::string m_name;
+
+public:
+
+ vpXmlParserHomogeneousMatrix();
+ vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix& twinParser);
+ ~vpXmlParserHomogeneousMatrix(){}
+
+ // get/set functions
+ vpHomogeneousMatrix getHomogeneousMatrix() const {return this->m_M;}
+ std::string getHomogeneousMatrixName() const {return this->m_name;}
+
+ vpXmlParserHomogeneousMatrix& operator =(const vpXmlParserHomogeneousMatrix& twinparser);
+ int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
+
+ int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
+
+ void setHomogeneousMatrixName(const std::string& name){
+ this->m_name = name;
+ }
+
+private:
+ int read (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name);
+
+ int count (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name);
+
+ int read_matrix (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name);
+
+ vpXmlCodeSequenceType read_values (xmlDocPtr doc, xmlNodePtr node,
+ vpHomogeneousMatrix &M);
+
+ static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
+ void myXmlReadIntChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ int &res,
+ vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadDoubleChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ double &res,
+ vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadCharChild (xmlDocPtr doc,
+ xmlNodePtr node,
+ char **res);
+ int write (xmlNodePtr node, const std::string& name);
+
+private:
+
+ /*!
+
+ \param 2doc : a pointer representing the document
+ \param node : the root node of the document
+ */
+ virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
+
+ /*!
+
+
+ \param node2 : the root node of the document
+ */
+ virtual void writeMainClass(xmlNodePtr ){};
+
+};
+#endif //VISP_HAVE_XML2
+#endif
diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp
new file mode 100644
index 0000000..c17d6bb
--- /dev/null
+++ b/modules/core/src/camera/vpCameraParameters.cpp
@@ -0,0 +1,596 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Camera intrinsic parameters.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpCameraParameters.cpp
+ \brief Definition of the vpCameraParameters class member functions.
+ Class vpCameraParameters define the camera intrinsic parameters
+
+*/
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <cmath>
+#include <limits>
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
+const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
+const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
+const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
+const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
+const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
+const vpCameraParameters::vpCameraParametersProjType
+ vpCameraParameters::DEFAULT_PROJ_TYPE =
+ vpCameraParameters::perspectiveProjWithoutDistortion;
+
+/*!
+ Default constructor.
+ By default, a perspective projection without distortion model is set.
+
+ \sa init()
+*/
+vpCameraParameters::vpCameraParameters()
+ :
+ px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
+ u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
+ kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
+ width(0), height(0),
+ isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
+ inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
+ projModel(DEFAULT_PROJ_TYPE)
+{
+ init() ;
+}
+
+/*!
+ Copy constructor
+ */
+vpCameraParameters::vpCameraParameters(const vpCameraParameters &c)
+ :
+ px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
+ u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
+ kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
+ width(0), height(0),
+ isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
+ inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
+ projModel(DEFAULT_PROJ_TYPE)
+{
+ init(c) ;
+}
+
+/*!
+ Constructor for perspective projection without distortion model
+
+ \param cam_px,cam_py : pixel size
+ \param cam_u0,cam_v0 : principal points
+
+ */
+vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
+ const double cam_u0, const double cam_v0)
+ :
+ px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
+ u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
+ kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
+ width(0), height(0),
+ isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
+ inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
+ projModel(DEFAULT_PROJ_TYPE)
+{
+ initPersProjWithoutDistortion(cam_px,cam_py,cam_u0,cam_v0) ;
+}
+
+/*!
+ Constructor for perspective projection with distortion model
+
+ \param cam_px,cam_py : pixel size
+ \param cam_u0,cam_v0 : principal points
+ \param cam_kud : undistorted to distorted radial distortion
+ \param cam_kdu : distorted to undistorted radial distortion
+
+ */
+vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
+ const double cam_u0, const double cam_v0,
+ const double cam_kud, const double cam_kdu)
+ :
+ px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
+ u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
+ kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
+ width(0), height(0),
+ isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
+ inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
+ projModel(DEFAULT_PROJ_TYPE)
+{
+ initPersProjWithDistortion(cam_px,cam_py,cam_u0,cam_v0,cam_kud,cam_kdu) ;
+}
+
+/*!
+ \brief basic initialization with the default parameters
+*/
+void
+vpCameraParameters::init()
+{
+ if (fabs(this->px)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ if (fabs(this->py)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ this->inv_px = 1./this->px;
+ this->inv_py = 1./this->py;
+}
+
+/*!
+ Initialization with specific parameters using perpective projection without
+ distortion model.
+ \param cam_px,cam_py : the ratio between the focal length and the size of a pixel.
+ \param cam_u0,cam_v0 : principal point coordinates in pixels.
+
+ The following sample code shows how to use this function:
+ \code
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+ double u0 = I.getWidth() / 2.;
+ double v0 = I.getHeight() / 2.;
+ double px = 600;
+ double py = 600;
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+ cam.computeFov(I.getWidth(), I.getHeight());
+ std::cout << cam << std::endl;
+ std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
+ << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
+}
+ \endcode
+ It produces the following output:
+ \code
+Camera parameters for perspective projection without distortion:
+ px = 600 py = 600
+ u0 = 320 v0 = 240
+
+Field of view (horizontal: 56.145 and vertical: 43.6028 degrees)
+ \endcode
+
+ */
+void
+vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py,
+ const double cam_u0, const double cam_v0)
+{
+ this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion ;
+
+ this->px = cam_px ;
+ this->py = cam_py ;
+ this->u0 = cam_u0 ;
+ this->v0 = cam_v0 ;
+ this->kud = 0 ;
+ this->kdu = 0 ;
+
+ if (fabs(px)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ if (fabs(py)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ this->inv_px = 1./px;
+ this->inv_py = 1./py;
+}
+
+/*!
+ Initialization with specific parameters using perpective projection with
+ distortion model.
+ \param cam_px,cam_py : the ratio between the focal length and the size of a pixel.
+ \param cam_u0,cam_v0 : principal points coordinates in pixels.
+ \param cam_kud : undistorted to distorted radial distortion.
+ \param cam_kdu : distorted to undistorted radial distortion.
+
+ The following sample code shows how to use this function:
+ \code
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+ double u0 = I.getWidth() / 2.;
+ double v0 = I.getHeight() / 2.;
+ double px = 600;
+ double py = 600;
+ double kud = -0.19;
+ double kdu = 0.20;
+ vpCameraParameters cam;
+ cam.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
+ cam.computeFov(I.getWidth(), I.getHeight());
+ std::cout << cam << std::endl;
+ std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
+ << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
+}
+ \endcode
+ It produces the following output:
+ \code
+Camera parameters for perspective projection with distortion:
+ px = 600 py = 600
+ u0 = 320 v0 = 240
+ kud = -0.19
+ kdu = 0.2
+
+Field of view (horizontal: 56.14497387 and vertical: 43.60281897 degrees) \endcode
+*/
+void
+vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double cam_py,
+ const double cam_u0, const double cam_v0,
+ const double cam_kud, const double cam_kdu)
+{
+ this->projModel = vpCameraParameters::perspectiveProjWithDistortion ;
+
+ this->px = cam_px ;
+ this->py = cam_py ;
+ this->u0 = cam_u0 ;
+ this->v0 = cam_v0 ;
+ this->kud = cam_kud ;
+ this->kdu = cam_kdu ;
+
+ if (fabs(px)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ if (fabs(py)<1e-6)
+ {
+ vpERROR_TRACE("Camera parameter px = 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "Camera parameter px = 0")) ;
+ }
+ this->inv_px = 1./px;
+ this->inv_py = 1./py;
+}
+
+/*!
+ destructor
+
+ nothing much to destroy...
+*/
+vpCameraParameters::~vpCameraParameters()
+{
+}
+
+/*!
+ initialization from another vpCameraParameters object
+*/
+void
+vpCameraParameters::init(const vpCameraParameters &c)
+{
+ *this = c ;
+}
+
+
+/*!
+ initialise the camera from a calibration matrix.
+ Using a calibration matrix leads to a camera without distortion
+
+ The K matrix in parameters must be like:
+
+ \f$ K = \left(\begin{array}{ccc}
+ p_x & 0 & u_0 \\
+ 0 & p_y & v_0 \\
+ 0 & 0 & 1
+ \end{array} \right) \f$
+
+ \param _K : the 3by3 calibration matrix
+*/
+void
+vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
+{
+ if(_K.getRows() != 3 || _K.getCols() != 3 ){
+ throw vpException(vpException::dimensionError, "bad size for calibration matrix");
+ }
+ if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
+ throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
+ }
+ initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
+}
+
+/*!
+ Initialize the camera model without distorsion from the image dimension and the camera field of view.
+ \param w : Image width.
+ \param h : Image height.
+ \param hfov : Camera horizontal field of view angle expressed in radians.
+ \param vfov : Camera vertical field of view angle expressed in radians.
+
+ The following sample code shows how to use this function:
+ \code
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+ vpCameraParameters cam;
+ double hfov = vpMath::rad(56);
+ double vfov = vpMath::rad(43);
+ cam.initFromFov(I.getWidth(), I.getHeight(), hfov, vfov);
+
+ std::cout << cam << std::endl;
+ std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
+ << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
+}
+ \endcode
+ It produces the following output:
+ \code
+Camera parameters for perspective projection without distortion:
+ px = 601.832 py = 609.275
+ u0 = 320 v0 = 240
+
+Field of view (horizontal: 56 and vertical: 43 degrees)
+ \endcode
+ */
+void
+vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
+{
+ projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
+ u0 = (double)w/2.;
+ v0 = (double)h/2.;
+ px = u0 / tan(hfov/2);
+ py = v0 / tan(vfov/2);
+ kud = 0;
+ kdu = 0;
+ inv_px = 1./px;
+ inv_py = 1./py;
+ computeFov(w, h);
+}
+
+/*!
+ copy operator
+ */
+vpCameraParameters&
+ vpCameraParameters::operator=(const vpCameraParameters& cam)
+{
+ projModel = cam.projModel ;
+ px = cam.px ;
+ py = cam.py ;
+ u0 = cam.u0 ;
+ v0 = cam.v0 ;
+ kud = cam.kud ;
+ kdu = cam.kdu ;
+
+ inv_px = cam.inv_px;
+ inv_py = cam.inv_py;
+
+ isFov = cam.isFov;
+ m_hFovAngle = cam.m_hFovAngle;
+ m_vFovAngle = cam.m_vFovAngle;
+ width = cam.width;
+ height = cam.height;
+ fovNormals = cam.fovNormals;
+
+ return *this ;
+}
+
+/*!
+ Compute angles and normals of the FOV.
+
+ \param w : Width of the image
+ \param h : Height of the image.
+*/
+void
+vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
+{
+ if( !isFov && w != width && h != height && w != 0 && h != 0){
+ fovNormals = std::vector<vpColVector>(4);
+
+ isFov = true;
+
+ double hFovAngle = atan(((double)w - u0) * ( 1.0 / px ));
+ double vFovAngle = atan(( v0 ) * ( 1.0 / py ));
+ double minushFovAngle = atan(( u0 ) * ( 1.0 / px ));
+ double minusvFovAngle = atan(((double)h - v0) * ( 1.0 / py ));
+
+ width = w;
+ height = h;
+
+ vpColVector n(3);
+ n = 0;
+ n[0] = 1.0;
+
+ vpRotationMatrix Rleft(0,-minushFovAngle,0);
+ vpRotationMatrix Rright(0,hFovAngle,0);
+
+ vpColVector nLeft, nRight;
+
+ nLeft = Rleft * (-n);
+ fovNormals[0] = nLeft.normalize();
+
+ nRight = Rright * n;
+ fovNormals[1] = nRight.normalize();
+
+ n = 0;
+ n[1] = 1.0;
+
+ vpRotationMatrix Rup(vFovAngle,0,0);
+ vpRotationMatrix Rdown(-minusvFovAngle,0,0);
+
+ vpColVector nUp, nDown;
+
+ nUp = Rup * (-n);
+ fovNormals[2] = nUp.normalize();
+
+ nDown = Rdown * n;
+ fovNormals[3] = nDown.normalize();
+
+ m_hFovAngle = hFovAngle + minushFovAngle;
+ m_vFovAngle = vFovAngle + minusvFovAngle;
+ }
+}
+
+
+/*!
+ Return the camera matrix \f$K\f$ given by:
+
+ \f$ K = \left[\begin{array}{ccc}
+ p_x & 0 & u_0 \\
+ 0 & p_y & v_0 \\
+ 0 & 0 & 1
+ \end{array} \right] \f$
+
+ \sa get_K_inverse()
+*/
+vpMatrix
+vpCameraParameters::get_K() const
+{
+ vpMatrix K(3, 3, 0.);
+ K[0][0] = px ;
+ K[1][1] = py ;
+ K[0][2] = u0 ;
+ K[1][2] = v0 ;
+ K[2][2] = 1.0 ;
+
+ return K;
+}
+/*!
+ Return the inverted camera matrix \f$K^{-1}\f$ given by:
+
+ \f$ K^{-1} = \left[\begin{array}{ccc}
+ 1/p_x & 0 & -u_0/p_x \\
+ 0 & 1/p_y & -v_0/p_y \\
+ 0 & 0 & 1
+ \end{array} \right] \f$
+
+ \sa get_K()
+*/
+vpMatrix
+vpCameraParameters::get_K_inverse() const
+{
+ vpMatrix K_inv(3, 3, 0.);
+ K_inv[0][0] = inv_px ;
+ K_inv[1][1] = inv_py ;
+ K_inv[0][2] = -u0*inv_px ;
+ K_inv[1][2] = -v0*inv_py ;
+ K_inv[2][2] = 1.0 ;
+
+ return K_inv;
+}
+
+
+/*!
+ Print the camera parameters on the standard output
+
+ \sa operator <<
+*/
+void
+vpCameraParameters::printParameters()
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ switch(projModel){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ std::cout.precision(10);
+ std::cout << "Camera parameters for perspective projection without distortion:"
+ << std::endl ;
+ std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
+ std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ std::cout.precision(10);
+ std::cout << "Camera parameters for perspective projection with distortion:"
+ << std::endl ;
+ std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
+ std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
+ std::cout << " kud = " << kud << std::endl ;
+ std::cout << " kdu = " << kdu << std::endl ;
+ break;
+ }
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+/*!
+
+ Print on the output stream \e os the camera parameters.
+
+ \param os : Output stream.
+ \param cam : Camera parameters.
+*/
+VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpCameraParameters &cam)
+{
+ switch(cam.get_projModel()){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ os << "Camera parameters for perspective projection without distortion:"
+ << std::endl ;
+ os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
+ << std::endl ;
+ os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
+ << std::endl ;
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ std::ios_base::fmtflags original_flags = os.flags();
+ os.precision(10);
+ os << "Camera parameters for perspective projection with distortion:"
+ << std::endl ;
+ os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
+ << std::endl ;
+ os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
+ << std::endl ;
+ os << " kud = " << cam.get_kud() << std::endl ;
+ os << " kdu = " << cam.get_kdu() << std::endl ;
+
+ os.flags(original_flags); // restore os to standard state
+ break;
+ }
+ return os;
+}
+
+
diff --git a/modules/core/src/camera/vpMeterPixelConversion.cpp b/modules/core/src/camera/vpMeterPixelConversion.cpp
new file mode 100644
index 0000000..f0969fe
--- /dev/null
+++ b/modules/core/src/camera/vpMeterPixelConversion.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Meter to pixel conversion.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeterPixelConversion.cpp
+ \brief meter to pixel conversion
+*/
+
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+
+//! Line coordinates conversion (rho,theta).
+void
+vpMeterPixelConversion::convertLine(const vpCameraParameters &cam,
+ const double &rho_m, const double &theta_m,
+ double &rho_p, double &theta_p)
+{
+ double co = cos(theta_m) ;
+ double si = sin(theta_m) ;
+ double d = sqrt(vpMath::sqr(cam.py*co) + vpMath::sqr(cam.px*si)) ;
+
+ if (fabs(d)<1e-6)
+ {
+ vpERROR_TRACE("division by zero") ;
+ throw(vpException(vpException::divideByZeroError,
+ "division by zero")) ;
+ }
+
+ theta_p = atan2(cam.px*si, cam.py*co) ;
+ rho_p = (cam.px*cam.py*rho_m + cam.u0*cam.py*co + cam.v0*cam.px*si) ;
+ rho_p /= d ;
+}
+
+/*!
+ Converts an ellipse with parameters expressed in meter in the image
+ plane in parameters expressed in the image in pixels.
+
+ The ellipse is here represented by its parameters \f$x_c,y_c,\mu_{20}, \mu_{11}, \mu_{02}\f$.
+
+ \param cam [in]: Intrinsic camera parameters.
+ \param circle [in]: 3D circle with parameters circle.p[] expressed in meters in the image plane.
+ \param center [out]: Center of the corresponding ellipse in the image with coordinates
+ expressed in pixels.
+ \param mu20_p,mu11_p,mu02_p [out]: Centered moments expressed in pixels.
+
+ The following code shows how to use this function:
+ \code
+ vpCircle circle;
+ double mu20_p, mu11_p, mu02_p;
+ circle.changeFrame(cMo);
+ circle.projection();
+ vpMeterPixelConversion::convertEllipse(cam, circle, center_p, mu20_p, mu11_p, mu02_p);
+ vpDisplay::displayEllipse(I, center_p, mu20_p, mu11_p, mu02_p);
+ \endcode
+ */
+void
+vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam,
+ const vpCircle &circle, vpImagePoint ¢er,
+ double &mu20_p, double &mu11_p, double &mu02_p)
+{
+ // Get the parameters of the ellipse in the image plane
+ double xc_m = circle.p[0];
+ double yc_m = circle.p[1];
+ double mu20_m = circle.p[2];
+ double mu11_m = circle.p[3];
+ double mu02_m = circle.p[4];
+
+ // Convert from meter to pixels
+ vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center);
+ mu20_p = mu20_m*vpMath::sqr(cam.get_px());
+ mu11_p = mu11_m*cam.get_px()*cam.get_py();
+ mu02_p = mu02_m*vpMath::sqr(cam.get_py());
+}
+
diff --git a/modules/core/src/camera/vpPixelMeterConversion.cpp b/modules/core/src/camera/vpPixelMeterConversion.cpp
new file mode 100644
index 0000000..17b1c1b
--- /dev/null
+++ b/modules/core/src/camera/vpPixelMeterConversion.cpp
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pixel to meter conversion.
+ *
+ * Authors:
+ * Eric Marchand
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPixelMeterConversion.cpp
+ \brief Pixel to meter conversion.
+*/
+#include<visp3/core/vpPixelMeterConversion.h>
+#include<visp3/core/vpCameraParameters.h>
+#include<visp3/core/vpException.h>
+#include<visp3/core/vpMath.h>
+#include<visp3/core/vpDebug.h>
+
+
+//! line coordinates conversion (rho,theta)
+void
+vpPixelMeterConversion::convertLine(const vpCameraParameters &cam,
+ const double &rho_p, const double &theta_p,
+ double &rho_m, double &theta_m)
+{
+ double co = cos(theta_p) ;
+ double si = sin(theta_p) ;
+ double d = vpMath::sqr(cam.px*co)+vpMath::sqr(cam.py*si) ;
+
+ if (fabs(d)<1e-6)
+ {
+ vpERROR_TRACE("division by zero") ;
+ throw(vpException(vpException::divideByZeroError,
+ "division by zero")) ;
+ }
+ theta_m = atan2(si*cam.py, co*cam.px) ;
+ rho_m = (rho_p - cam.u0*co-cam.v0*si)/sqrt(d) ;
+}
+
+
+void
+vpPixelMeterConversion::convertMoment(const vpCameraParameters &cam,
+ unsigned int order,
+ const vpMatrix &moment_pixel,
+ vpMatrix &moment_meter)
+{
+
+ vpMatrix m(order, order);
+ double yc = -cam.v0 ;
+ double xc = -cam.u0 ;
+
+ for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
+ {
+ for (unsigned int p=0 ; p<order; p++) // iteration en X
+ for (unsigned int q=0; q<order; q++) // iteration en Y
+ if (p+q==k) // on est bien dans la matrice triangulaire superieure
+ {
+ m[p][q] = 0; // initialisation e 0
+ for(unsigned int r=0; r<=p; r++) // somme externe
+ for(unsigned int t=0; t<=q; t++) // somme interne
+ {
+ m[p][q] +=
+ static_cast<double>(vpMath::comb(p, r))
+ * static_cast<double>(vpMath::comb(q, t))
+ * pow(xc, (int)(p-r)) * pow(yc, (int)(q-t))
+ * moment_pixel[r][t];
+
+ }
+ }
+
+ }
+
+ for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
+ for (unsigned int p=0 ; p<order; p++)
+ for (unsigned int q=0; q<order; q++)
+ if (p+q==k)
+ {
+ m[p][q] *= pow(cam.inv_px,(int)(1+p)) * pow(cam.inv_py,(int)(1+q));
+ }
+
+ for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
+ for (unsigned int p=0 ; p<order; p++)
+ for (unsigned int q=0; q<order; q++)
+ if (p+q==k)
+ {
+ moment_meter[p][q] = m[p][q];
+ }
+
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp
new file mode 100644
index 0000000..5016b5c
--- /dev/null
+++ b/modules/core/src/camera/vpXmlParserCamera.cpp
@@ -0,0 +1,1100 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load and save camera intrinsic parameters.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpXmlParserCamera.cpp
+ \brief Definition of the vpXmlParserCamera class member functions.
+ Class vpXmlParserCamera allowed to load and save intrinsic camera parameters
+
+*/
+#include <visp3/core/vpXmlParserCamera.h>
+#ifdef VISP_HAVE_XML2
+
+#include <stdlib.h>
+#include <string.h>
+
+#include <visp3/core/vpDebug.h>
+/* -------------------------------------------------------------------------- */
+/* --- LABEL XML ------------------------------------------------------------ */
+/* -------------------------------------------------------------------------- */
+
+#define LABEL_XML_ROOT "root"
+#define LABEL_XML_CAMERA "camera"
+#define LABEL_XML_CAMERA_NAME "name"
+#define LABEL_XML_WIDTH "image_width"
+#define LABEL_XML_HEIGHT "image_height"
+#define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
+#define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
+#define LABEL_XML_FULL_WIDTH "full_width"
+#define LABEL_XML_FULL_HEIGHT "full_height"
+#define LABEL_XML_MODEL "model"
+#define LABEL_XML_MODEL_TYPE "type"
+#define LABEL_XML_U0 "u0"
+#define LABEL_XML_V0 "v0"
+#define LABEL_XML_PX "px"
+#define LABEL_XML_PY "py"
+#define LABEL_XML_KUD "kud"
+#define LABEL_XML_KDU "kdu"
+
+#define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
+#define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
+/*!
+ Default constructor
+*/
+vpXmlParserCamera::vpXmlParserCamera()
+ : vpXmlParser(),
+ camera(), camera_name(), image_width(0), image_height(0),
+ subsampling_width(0), subsampling_height(0), full_width(0), full_height(0)
+{
+}
+/*!
+ Copy constructor
+ \param twinParser : parser object to copy
+*/
+vpXmlParserCamera::vpXmlParserCamera(vpXmlParserCamera& twinParser)
+ : vpXmlParser(twinParser),
+ camera(), camera_name(), image_width(0), image_height(0),
+ subsampling_width(0), subsampling_height(0), full_width(0), full_height(0)
+
+{
+ this->camera = twinParser.camera;
+ this->camera_name = twinParser.camera_name;
+ this->image_width = twinParser.image_width;
+ this->image_height = twinParser.image_height;
+ this->subsampling_width = twinParser.subsampling_width;
+ this->subsampling_height = twinParser.subsampling_height;
+ this->full_width = twinParser.full_width;
+ this->full_height = twinParser.full_height;
+}
+
+/*!
+ Copy operator
+ \param twinParser : parser object to copy
+ \return a copy of the input.
+*/
+vpXmlParserCamera&
+vpXmlParserCamera::operator =(const vpXmlParserCamera& twinParser) {
+ this->camera = twinParser.camera;
+ this->camera_name = twinParser.camera_name;
+ this->image_width = twinParser.image_width;
+ this->image_height = twinParser.image_height;
+ this->subsampling_width = twinParser.subsampling_width;
+ this->subsampling_height = twinParser.subsampling_height;
+ this->full_width = twinParser.full_width;
+ this->full_height = twinParser.full_height;
+ return *this ;
+}
+
+/*!
+ Parse an xml file to load camera parameters.
+ \param cam : camera parameters to fill.
+ \param filename : name of the xml file to parse
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param projModel : camera projection model needed.
+ \param im_width : image width on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : image height on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+*/
+int
+vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename,
+ const std::string& cam_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int im_width,
+ const unsigned int im_height)
+{
+ xmlDocPtr doc;
+ xmlNodePtr node;
+
+ doc = xmlParseFile(filename.c_str());
+ if (doc == NULL)
+ {
+ return SEQUENCE_ERROR;
+ }
+
+ node = xmlDocGetRootElement(doc);
+ if (node == NULL)
+ {
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ int ret = this ->read (doc, node, cam_name, projModel, im_width, im_height);
+
+ cam = camera ;
+
+ xmlFreeDoc(doc);
+
+ return ret;
+}
+
+/*!
+ Save camera parameters in an xml file.
+ \param cam : camera parameters to save.
+ \param filename : name of the xml file to fill.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+*/
+int
+vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename,
+ const std::string& cam_name,
+ const unsigned int im_width,
+ const unsigned int im_height)
+{
+ xmlDocPtr doc;
+ xmlNodePtr node;
+ xmlNodePtr nodeCamera = NULL;
+
+ doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR
+ + XML_PARSE_NOBLANKS);
+ if (doc == NULL){
+ doc = xmlNewDoc ((xmlChar*)"1.0");
+ node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
+ xmlDocSetRootElement(doc,node);
+ xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
+ "This file stores intrinsic camera parameters used\n"
+ " in the vpCameraParameters Class of ViSP available\n"
+ " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
+ " It can be read with the parse method of\n"
+ " the vpXmlParserCamera class.");
+ xmlAddChild(node,node_tmp);
+ }
+
+ node = xmlDocGetRootElement(doc);
+ if (node == NULL)
+ {
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ this->camera = cam;
+
+ int nbCamera = count(doc, node, cam_name,cam.get_projModel(),
+ im_width, im_height);
+ if( nbCamera > 0){
+// vpCERROR << nbCamera
+// << " set(s) of camera parameters is(are) already "<< std::endl
+// << "available in the file with your specifications : "<< std::endl
+// << "precise the grabber parameters or delete manually"<< std::endl
+// << "the previous one."<<std::endl;
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ nodeCamera = find_camera(doc, node, cam_name, im_width, im_height);
+ if(nodeCamera == NULL){
+ write(node, cam_name, im_width, im_height);
+ }
+ else{
+ write_camera(nodeCamera);
+ }
+
+ xmlSaveFormatFile(filename.c_str(), doc, 1);
+ xmlFreeDoc(doc);
+
+ return SEQUENCE_OK;
+}
+
+
+
+/*!
+ Read camera parameters from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : subsampling of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : subsampling of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+ */
+int
+vpXmlParserCamera::read (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& cam_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int im_width,
+ const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ // char * val_char;
+ vpXmlCodeType prop;
+
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+ unsigned int nbCamera = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+ /*
+ switch (prop)
+ {
+ case CODE_XML_CAMERA:
+ if (SEQUENCE_OK == this->read_camera (doc, node, camera_name, projModel,
+ image_width, image_height, subsampling_width, subsampling_height)){
+ nbCamera++;
+ }
+ break;
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+ */
+ if (prop == CODE_XML_CAMERA){
+ if (SEQUENCE_OK == this->read_camera (doc, node, cam_name, projModel,
+ im_width, im_height, subsampl_width, subsampl_height))
+ nbCamera++;
+ }
+ else back = SEQUENCE_ERROR;
+ }
+
+ if (nbCamera == 0){
+ back = SEQUENCE_ERROR;
+ vpCERROR << "No camera parameters is available" << std::endl
+ << "with your specifications" << std::endl;
+ }
+ else if(nbCamera > 1){
+ back = SEQUENCE_ERROR;
+ vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
+ << "with your specifications : " << std::endl
+ << "precise your choice..." << std::endl;
+ }
+
+ return back;
+}
+/*!
+ Read camera parameters from a XML file and count the number of available
+ sets of camera parameters corresponding with inputs.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : subsampling of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : subsampling of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return number of available camera parameters corresponding with inputs.
+ */
+int
+vpXmlParserCamera::count (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& cam_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int im_width,
+ const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ // char * val_char;
+ vpXmlCodeType prop;
+ int nbCamera = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ }
+ /*
+ switch (prop)
+ {
+ case CODE_XML_CAMERA:
+ if (SEQUENCE_OK == this->read_camera (doc, node, camera_name, projModel,
+ image_width, image_height,
+ subsampling_width, subsampling_height)){
+ nbCamera++;
+ }
+ break;
+ default:
+ break;
+ }
+ */
+ if (prop== CODE_XML_CAMERA) {
+ if (SEQUENCE_OK == this->read_camera (doc, node, cam_name, projModel,
+ im_width, im_height,
+ subsampl_width, subsampl_height))
+ nbCamera++;
+ }
+ }
+
+ return nbCamera;
+}
+/*!
+ Read camera headers from a XML file and return the last available
+ node pointeur in the xml tree corresponding with inputs.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : subsampling of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : subsampling of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return number of available camera parameters corresponding with inputs.
+ */
+xmlNodePtr
+vpXmlParserCamera::find_camera (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& cam_name,
+ const unsigned int im_width,
+ const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ // char * val_char;
+ vpXmlCodeType prop;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ }
+ /*
+ switch (prop)
+ {
+ case CODE_XML_CAMERA:
+ if (SEQUENCE_OK == this->read_camera_header(doc, node, camera_name,
+ image_width, image_height,
+ subsampling_width, subsampling_height)){
+ return node;
+ }
+ break;
+ default:
+ break;
+ }
+ */
+ if(prop == CODE_XML_CAMERA){
+ if (SEQUENCE_OK == this->read_camera_header(doc, node, cam_name,
+ im_width, im_height,
+ subsampl_width, subsampl_height))
+ return node;
+ }
+ }
+ return NULL;
+}
+
+/*!
+ Read camera fields from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : scale of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : scale of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+
+ */
+int
+vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& cam_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel,
+ const unsigned int im_width,
+ const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ vpXmlCodeType prop;
+ /* read value in the XML file. */
+ std::string camera_name_tmp = "";
+ unsigned int image_height_tmp = 0 ;
+ unsigned int image_width_tmp = 0 ;
+ unsigned int subsampling_width_tmp = 0;
+ unsigned int subsampling_height_tmp = 0;
+ // unsigned int full_width_tmp = 0;
+ // unsigned int full_height_tmp = 0;
+ vpCameraParameters cam_tmp;
+ vpCameraParameters cam_tmp_model;
+ bool projModelFound = false;
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+
+ switch (prop)
+ {
+ case CODE_XML_CAMERA_NAME: {
+ char * val_char = xmlReadCharChild(doc, node);
+ camera_name_tmp = val_char;
+ std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
+ xmlFree(val_char);
+ break;
+ }
+ case CODE_XML_WIDTH:
+ image_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+
+ case CODE_XML_HEIGHT:
+ image_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ case CODE_XML_SUBSAMPLING_WIDTH:
+ subsampling_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ case CODE_XML_SUBSAMPLING_HEIGHT:
+ subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ // case CODE_XML_FULL_WIDTH:
+ // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
+
+ // case CODE_XML_FULL_HEIGHT:
+ // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
+
+ case CODE_XML_MODEL:
+ back = read_camera_model(doc, node, cam_tmp_model);
+ if(cam_tmp_model.get_projModel() == projModel){
+ cam_tmp = cam_tmp_model;
+ projModelFound = true;
+ }
+ break;
+
+ case CODE_XML_BAD:
+ case CODE_XML_OTHER:
+ case CODE_XML_CAMERA:
+ case CODE_XML_FULL_HEIGHT:
+ case CODE_XML_FULL_WIDTH:
+ case CODE_XML_MODEL_TYPE:
+ case CODE_XML_U0:
+ case CODE_XML_V0:
+ case CODE_XML_PX:
+ case CODE_XML_PY:
+ case CODE_XML_KUD:
+ case CODE_XML_KDU:
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+
+ }
+ // Create a specific test for subsampling_width and subsampling_height to ensure that division by zero is not possible in the next test
+ bool test_subsampling_width = true;
+ bool test_subsampling_height = true;
+
+ if (subsampling_width) {
+ test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) < (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
+ }
+ if (subsampling_height) {
+ test_subsampling_height = (abs((int)subsampl_height - (int)subsampling_height_tmp) < (allowedPixelDiffOnImageSize * (int)(subsampling_height_tmp / subsampling_height)));
+ }
+ if( !((projModelFound == true) && (cam_name == camera_name_tmp) &&
+ (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
+ (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
+ (test_subsampling_width)&&
+ (test_subsampling_height))){
+ back = SEQUENCE_ERROR;
+ }
+ else{
+ this->camera = cam_tmp;
+ this->camera_name = camera_name_tmp;
+ this->image_width = image_width_tmp;
+ this->image_height = image_height_tmp;
+ this->subsampling_width = subsampling_width_tmp;
+ this->subsampling_height = subsampling_height_tmp;
+ this->full_width = subsampling_width_tmp * image_width_tmp;
+ this->full_height = subsampling_height_tmp * image_height_tmp;
+ }
+ return back;
+}
+/*!
+ Read camera header fields from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : scale of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : scale of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+
+ */
+int
+vpXmlParserCamera::
+read_camera_header (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& cam_name,
+ const unsigned int im_width,
+ const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ vpXmlCodeType prop;
+ /* read value in the XML file. */
+ std::string camera_name_tmp = "";
+ unsigned int image_height_tmp = 0 ;
+ unsigned int image_width_tmp = 0 ;
+ unsigned int subsampling_width_tmp = 0;
+ unsigned int subsampling_height_tmp = 0;
+ // unsigned int full_width_tmp = 0;
+ // unsigned int full_height_tmp = 0;
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+
+ switch (prop)
+ {
+ case CODE_XML_CAMERA_NAME:{
+ char * val_char = xmlReadCharChild(doc, node);
+ camera_name_tmp = val_char;
+ xmlFree(val_char);
+ }break;
+
+ case CODE_XML_WIDTH:
+ image_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+
+ case CODE_XML_HEIGHT:
+ image_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ case CODE_XML_SUBSAMPLING_WIDTH:
+ subsampling_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ case CODE_XML_SUBSAMPLING_HEIGHT:
+ subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ break;
+ // case CODE_XML_FULL_WIDTH:
+ // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
+
+ // case CODE_XML_FULL_HEIGHT:
+ // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
+
+ case CODE_XML_MODEL:
+ break;
+
+ case CODE_XML_BAD:
+ case CODE_XML_OTHER:
+ case CODE_XML_CAMERA:
+ case CODE_XML_FULL_HEIGHT:
+ case CODE_XML_FULL_WIDTH:
+ case CODE_XML_MODEL_TYPE:
+ case CODE_XML_U0:
+ case CODE_XML_V0:
+ case CODE_XML_PX:
+ case CODE_XML_PY:
+ case CODE_XML_KUD:
+ case CODE_XML_KDU:
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+ }
+ if( !((cam_name == camera_name_tmp) &&
+ (im_width == image_width_tmp || im_width == 0) &&
+ (im_height == image_height_tmp || im_height == 0) &&
+ (subsampl_width == subsampling_width_tmp ||
+ subsampl_width == 0)&&
+ (subsampl_height == subsampling_height_tmp ||
+ subsampl_height == 0))){
+ back = SEQUENCE_ERROR;
+ }
+ return back;
+}
+
+/*!
+ Read camera model fields from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_tmp : camera parameters to fill with read data (output).
+
+ \return error code.
+
+ */
+vpXmlParserCamera::vpXmlCodeSequenceType
+vpXmlParserCamera::read_camera_model (xmlDocPtr doc, xmlNodePtr node,
+ vpCameraParameters &cam_tmp)
+{
+ // counter of the number of read parameters
+ int nb = 0;
+ vpXmlCodeType prop;
+ /* read value in the XML file. */
+
+ char* model_type = NULL;
+ double u0 = cam_tmp.get_u0();
+ double v0 = cam_tmp.get_v0();
+ double px = cam_tmp.get_px();
+ double py = cam_tmp.get_py();
+ double kud = cam_tmp.get_kud();
+ double kdu = cam_tmp.get_kdu();
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+ int validation = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+ switch (prop)
+ {
+ case CODE_XML_MODEL_TYPE:{
+ if(model_type != NULL){
+ xmlFree(model_type);
+ }
+ model_type = xmlReadCharChild(doc, node);
+ nb++;
+ validation = validation | 0x01;
+ }break;
+ case CODE_XML_U0:
+ u0 = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x02;
+ break;
+ case CODE_XML_V0:
+ v0 = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x04;
+ break;
+ case CODE_XML_PX:
+ px = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x08;
+ break;
+ case CODE_XML_PY:
+ py = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x10;
+ break;
+ case CODE_XML_KUD:
+ kud = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x20;
+ break;
+ case CODE_XML_KDU:
+ kdu = xmlReadDoubleChild(doc, node);
+ nb++;
+ validation = validation | 0x40;
+ break;
+ case CODE_XML_BAD:
+ case CODE_XML_OTHER:
+ case CODE_XML_CAMERA:
+ case CODE_XML_CAMERA_NAME:
+ case CODE_XML_HEIGHT:
+ case CODE_XML_WIDTH:
+ case CODE_XML_SUBSAMPLING_WIDTH:
+ case CODE_XML_SUBSAMPLING_HEIGHT:
+ case CODE_XML_FULL_HEIGHT:
+ case CODE_XML_FULL_WIDTH:
+ case CODE_XML_MODEL:
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+ }
+
+ if(model_type == NULL) {
+ vpERROR_TRACE("projection model type doesn't match with any known model !");
+ return SEQUENCE_ERROR;
+ }
+
+ if( !strcmp(model_type,LABEL_XML_MODEL_WITHOUT_DISTORTION)){
+ if (nb != 5 || validation != 0x1F)
+ {
+ vpCERROR <<"ERROR in 'model' field:\n";
+ vpCERROR << "it must contain 5 parameters\n";
+ xmlFree(model_type);
+
+ return SEQUENCE_ERROR;
+ }
+ cam_tmp.initPersProjWithoutDistortion(px,py,u0,v0) ;
+ }
+ else if( !strcmp(model_type,LABEL_XML_MODEL_WITH_DISTORTION)){
+ if (nb != 7 || validation != 0x7F)
+ {
+ vpCERROR <<"ERROR in 'model' field:\n";
+ vpCERROR << "it must contain 7 parameters\n";
+ xmlFree(model_type);
+
+ return SEQUENCE_ERROR;
+ }
+ cam_tmp.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
+ }
+ else{
+ vpERROR_TRACE("projection model type doesn't match with any known model !");
+ xmlFree(model_type);
+
+ return SEQUENCE_ERROR;
+ }
+ xmlFree(model_type);
+
+ return back;
+}
+
+/*!
+ Write camera parameters in an XML Tree.
+
+ \param node : XML tree, pointing on a marker equipement.
+ \param cam_name : name of the camera : useful if the xml file has multiple
+ camera parameters. Set as "" if the camera name is not ambiguous.
+ \param im_width : width of image on which camera calibration was performed.
+ Set as 0 if not ambiguous.
+ \param im_height : height of the image on which camera calibration
+ was performed. Set as 0 if not ambiguous.
+ \param subsampl_width : subsampling of the image width sent by the camera.
+ Set as 0 if not ambiguous.
+ \param subsampl_height : subsampling of the image height sent by the camera.
+ Set as 0 if not ambiguous.
+
+ \return error code.
+ */
+int vpXmlParserCamera::
+write (xmlNodePtr node, const std::string& cam_name,
+ const unsigned int im_width, const unsigned int im_height,
+ const unsigned int subsampl_width,
+ const unsigned int subsampl_height)
+{
+ int back = SEQUENCE_OK;
+
+ xmlNodePtr node_tmp;
+ xmlNodePtr node_camera;
+
+ // <camera>
+ node_camera = xmlNewNode(NULL,(xmlChar*)LABEL_XML_CAMERA);
+ xmlAddChild(node,node_camera);
+ {
+ //<name>
+
+ if(!cam_name.empty()){
+ node_tmp = xmlNewComment((xmlChar*)"Name of the camera");
+ xmlAddChild(node_camera,node_tmp);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_CAMERA_NAME,
+ (xmlChar*)cam_name.c_str());
+ }
+
+ if(im_width != 0 || im_height != 0){
+ char str[11];
+ //<image_width>
+ node_tmp = xmlNewComment((xmlChar*)"Size of the image on which camera calibration was performed");
+ xmlAddChild(node_camera,node_tmp);
+
+ sprintf(str,"%u",im_width);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_WIDTH,(xmlChar*)str);
+ //<image_height>
+
+ sprintf(str,"%u",im_height);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_HEIGHT,(xmlChar*)str);
+ if(subsampling_width != 0 || subsampling_height != 0){
+ node_tmp = xmlNewComment((xmlChar*)"Subsampling used to obtain the current size of the image.");
+ xmlAddChild(node_camera,node_tmp);
+
+ //<subsampling_width>
+ sprintf(str,"%u",subsampl_width);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_WIDTH,
+ (xmlChar*)str);
+ //<subsampling_height>
+ sprintf(str,"%u",subsampl_height);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_HEIGHT,
+ (xmlChar*)str);
+ node_tmp = xmlNewComment((xmlChar*)"The full size is the sensor size actually used to grab the image. full_width = subsampling_width * image_width");
+ xmlAddChild(node_camera,node_tmp);
+
+ //<full_width>
+ sprintf(str,"%u",im_width*subsampl_width);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_WIDTH,
+ (xmlChar*)str);
+ //<full_height>
+ sprintf(str,"%u",im_height*subsampl_height);
+ xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_HEIGHT,
+ (xmlChar*)str);
+ }
+ }
+
+ node_tmp = xmlNewComment((xmlChar*)"Intrinsic camera parameters computed for each projection model");
+
+ xmlAddChild(node_camera,node_tmp);
+
+ back = write_camera(node_camera);
+ }
+ return back;
+}
+/*!
+ Write camera parameters in an XML Tree.
+
+ \param node_camera : XML pointer node, pointing on a camera node.
+
+ \return error code.
+ */
+int vpXmlParserCamera::
+write_camera(xmlNodePtr node_camera){
+ xmlNodePtr node_model;
+ xmlNodePtr node_tmp;
+
+ int back = SEQUENCE_OK;
+ switch(camera.get_projModel()){
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ //<model>
+ node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
+ xmlAddChild(node_camera,node_model);
+ {
+ char str[21];
+ node_tmp = xmlNewComment((xmlChar*)"Projection model type");
+ xmlAddChild(node_model,node_tmp);
+
+ //<type>without_distortion</type>
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
+ (xmlChar*)LABEL_XML_MODEL_WITHOUT_DISTORTION);
+
+ node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
+ xmlAddChild(node_model,node_tmp);
+ //<px>
+ sprintf(str,"%.10f",camera.get_px());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
+ //<py>
+ sprintf(str,"%.10f",camera.get_py());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
+
+ node_tmp = xmlNewComment((xmlChar*)"Principal point");
+ xmlAddChild(node_model,node_tmp);
+
+ //<u0>
+ sprintf(str,"%.10f",camera.get_u0());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
+ //<v0>
+ sprintf(str,"%.10f",camera.get_v0());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
+ }
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ //<model>
+ node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
+ xmlAddChild(node_camera,node_model);
+ {
+ char str[21];
+ node_tmp = xmlNewComment((xmlChar*)"Projection model type");
+ xmlAddChild(node_model,node_tmp);
+ //<type>with_distortion</type>
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
+ (xmlChar*)LABEL_XML_MODEL_WITH_DISTORTION);
+
+ node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
+ xmlAddChild(node_model,node_tmp);
+ //<px>
+ sprintf(str,"%.10f",camera.get_px());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
+ //<py>
+ sprintf(str,"%.10f",camera.get_py());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
+
+ node_tmp = xmlNewComment((xmlChar*)"Principal point");
+ xmlAddChild(node_model,node_tmp);
+ //<u0>
+ sprintf(str,"%.10f",camera.get_u0());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
+ //<v0>
+ sprintf(str,"%.10f",camera.get_v0());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
+
+ //<kud>
+ node_tmp = xmlNewComment((xmlChar*)"Undistorted to distorted distortion parameter");
+ xmlAddChild(node_model,node_tmp);
+ sprintf(str,"%.10f",camera.get_kud());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KUD,(xmlChar*)str);
+
+ //<kud>
+ node_tmp = xmlNewComment((xmlChar*)"Distorted to undistorted distortion parameter");
+ xmlAddChild(node_model,node_tmp);
+ sprintf(str,"%.10f",camera.get_kdu());
+ xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KDU,(xmlChar*)str);
+ }
+ break;
+ }
+ return back;
+}
+
+/*!
+ Translate a string (label) to a xml code.
+ \param str : string to translate.
+ \param res : resulting code.
+
+ \return error code.
+*/
+
+vpXmlParserCamera::vpXmlCodeSequenceType
+vpXmlParserCamera::str2xmlcode (char * str, vpXmlCodeType & res)
+{
+ vpXmlCodeType val_int = CODE_XML_BAD;
+ vpXmlCodeSequenceType back = vpXmlParserCamera::SEQUENCE_OK;
+
+ // DEBUG_TRACE (9, "# Entree :str=%s.", str);
+
+ if (! strcmp (str, LABEL_XML_CAMERA))
+ {
+ val_int = CODE_XML_CAMERA;
+ }
+ else if (! strcmp (str, LABEL_XML_CAMERA_NAME))
+ {
+ val_int = CODE_XML_CAMERA_NAME;
+ }
+ else if (! strcmp (str, LABEL_XML_MODEL))
+ {
+ val_int = CODE_XML_MODEL;
+ }
+ else if (! strcmp (str, LABEL_XML_MODEL_TYPE))
+ {
+ val_int = CODE_XML_MODEL_TYPE;
+ }
+ else if (! strcmp (str, LABEL_XML_WIDTH))
+ {
+ val_int = CODE_XML_WIDTH;
+ }
+ else if (! strcmp (str, LABEL_XML_HEIGHT))
+ {
+ val_int = CODE_XML_HEIGHT;
+ }
+ else if (! strcmp (str, LABEL_XML_SUBSAMPLING_WIDTH))
+ {
+ val_int = CODE_XML_SUBSAMPLING_WIDTH;
+ }
+ else if (! strcmp (str, LABEL_XML_SUBSAMPLING_HEIGHT))
+ {
+ val_int = CODE_XML_SUBSAMPLING_HEIGHT;
+ }
+ else if (! strcmp (str, LABEL_XML_FULL_WIDTH))
+ {
+ val_int = CODE_XML_FULL_WIDTH;
+ }
+ else if (! strcmp (str, LABEL_XML_FULL_HEIGHT))
+ {
+ val_int = CODE_XML_FULL_HEIGHT;
+ }
+ else if (! strcmp (str, LABEL_XML_U0))
+ {
+ val_int = CODE_XML_U0;
+ }
+ else if (! strcmp (str, LABEL_XML_V0))
+ {
+ val_int = CODE_XML_V0;
+ }
+ else if (! strcmp (str, LABEL_XML_PX))
+ {
+ val_int = CODE_XML_PX;
+ }
+ else if (! strcmp (str, LABEL_XML_PY))
+ {
+ val_int = CODE_XML_PY;
+ }
+ else if (! strcmp (str, LABEL_XML_KUD))
+ {
+ val_int = CODE_XML_KUD;
+ }
+ else if (! strcmp (str, LABEL_XML_KDU))
+ {
+ val_int = CODE_XML_KDU;
+ }
+ else
+ {
+ val_int = CODE_XML_OTHER;
+ }
+ res = val_int;
+
+ return back;
+}
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has no symbols
+void dummy_vpXmlParserCamera() {};
+#endif //VISP_HAVE_XML2
diff --git a/modules/core/src/display/vpColor.cpp b/modules/core/src/display/vpColor.cpp
new file mode 100644
index 0000000..de27764
--- /dev/null
+++ b/modules/core/src/display/vpColor.cpp
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Color definition.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpColor.h>
+
+// FS: Sould be improved to avoid the #if preprocessor line. Not a good idea to define colors in static.
+// See also vpColor.h where things need to be improved.
+//#if !defined(visp_EXPORTS)
+#if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
+/*!< Predefined black color with R=G=B=0 and identifier vpColor::id_black. */
+vpColor const vpColor::black = vpColor(0, 0, 0, id_black);
+/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white. */
+vpColor const vpColor::white = vpColor(255, 255, 255, id_white);
+/*!< Predefined light gray color with R=G=B=64 and identifier vpColor::id_lightGray. */
+vpColor const vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
+/*!< Predefined gray color with R=G=B=128 and identifier vpColor::id_gray. */
+vpColor const vpColor::gray = vpColor(128, 128, 128, id_gray);
+/*!< Predefined dark gray color with R=G=B=192 and identifier vpColor::id_darkGray. */
+vpColor const vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
+/*!< Predefined light red color with R= 255 and G=B=140 and identifier
+ vpColor::id_lightRed. */
+vpColor const vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
+/*!< Predefined red color with R=255 and G=B=0 and identifier
+ vpColor::id_red. */
+vpColor const vpColor::red = vpColor(255, 0, 0, id_red);
+/*!< Predefined dark red color with R= 128 and G=B=0 and identifier
+ vpColor::id_darkRed. */
+vpColor const vpColor::darkRed = vpColor(128, 0, 0, id_darkRed);
+/*!< Predefined light green color with G= 255 and R=B=140 and identifier
+ vpColor::id_lightGreen. */
+vpColor const vpColor::lightGreen = vpColor(140, 255, 140, id_lightGreen);
+/*!< Predefined green color with G=255 and R=B=0 and identifier
+ vpColor::id_green. */
+vpColor const vpColor::green = vpColor(0, 255, 0, id_green);
+/*!< Predefined dark green color with G= 128 and R=B=0 and identifier
+ vpColor::id_darkGreen. */
+vpColor const vpColor::darkGreen = vpColor(0, 128, 0, id_darkGreen);
+/*!< Predefined light blue color with B= 255 and R=G=140 and identifier
+ vpColor::id_lightBlue. */
+vpColor const vpColor::lightBlue = vpColor(140, 140, 255, id_lightBlue);
+/*!< Predefined blue color with R=G=0 and B=255 and identifier
+ vpColor::id_blue. */
+vpColor const vpColor::blue = vpColor(0, 0, 255, id_blue);
+/*!< Predefined dark blue color with B= 128 and R=G=0 and identifier
+ vpColor::id_darkBlue. */
+vpColor const vpColor::darkBlue = vpColor(0, 0, 128, id_darkBlue);
+/*!< Predefined yellow color with R=G=255 and B=0 and identifier
+ vpColor::id_yellow. */
+vpColor const vpColor::yellow = vpColor(255, 255, 0, id_yellow);
+/*!< Predefined cyan color with R=0 and G=B=255 and identifier
+ vpColor::id_cyan. */
+vpColor const vpColor::cyan = vpColor(0, 255, 255, id_cyan);
+/*!< Predefined orange color with R=255, G=165 and B=0 and identifier
+ vpColor::id_orange. */
+vpColor const vpColor::orange = vpColor(255, 165, 0, id_orange);
+/*!< Predefined purple color with R=128, G=0 and B=128 and identifier
+ vpColor::id_purple. */
+vpColor const vpColor::purple = vpColor(128, 0, 128, id_purple);
+/*!< Predefined none color with R=G=B=0 and identifier vpColor::id_unknown. */
+vpColor const vpColor::none = vpColor(0, 0, 0, id_unknown);
+
+const unsigned int vpColor::nbColors = 18;
+
+/*!< Array of available colors. */
+vpColor const vpColor::allColors[vpColor::nbColors] = {
+ vpColor::blue , // 12
+ vpColor::green , // 9
+ vpColor::red , // 6
+ vpColor::cyan , // 15
+ vpColor::purple , // 4
+ vpColor::yellow , // 14
+ vpColor::orange , // 16
+ vpColor::lightBlue ,// 11
+ vpColor::lightGreen,// 8
+ vpColor::lightRed , // 5
+ vpColor::darkBlue , // 13
+ vpColor::darkGreen ,// 10
+ vpColor::darkRed , // 7
+ vpColor::lightGray ,// 2
+ vpColor::gray , // 3
+ vpColor::darkGray , // 4
+ vpColor::black , // 0
+ vpColor::white}; // 17
+#endif
+
+vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpColor::orange,vpColor::purple};
+
+/*!
+ Compare two colors.
+
+ Return true if the R,G,B components are the same.
+
+ \param c1,c2 : Color to compare.
+*/
+VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 ) {
+ return ( ( c1.R == c2.R ) && ( c1.G == c2.G ) && ( c1.B == c2.B) );
+}
+
+/*!
+
+ Compare two colors.
+
+ Return true if the R,G,B components are different.
+
+ \param c1,c2 : Color to compare.
+*/
+VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 ) {
+ return ( ( c1.R != c2.R ) || ( c1.G != c2.G ) || ( c1.B == c2.B) );
+}
diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp
new file mode 100644
index 0000000..908df2f
--- /dev/null
+++ b/modules/core/src/display/vpDisplay.cpp
@@ -0,0 +1,3961 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <limits>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpDisplayException.h>
+#include <visp3/core/vpImageConvert.h>
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpMath.h>
+
+
+/*!
+ \file vpDisplay.cpp
+ \brief Generic class for image display.
+*/
+
+/*!
+ Default constructor.
+*/
+vpDisplay::vpDisplay()
+ : displayHasBeenInitialized(false), windowXPosition(0), windowYPosition(0), width(0), height(0), title_() {}
+
+/*!
+ Copy constructor.
+*/
+vpDisplay::vpDisplay(const vpDisplay &d)
+ : displayHasBeenInitialized(false), windowXPosition(0), windowYPosition(0), width(0), height(0), title_()
+{
+ displayHasBeenInitialized = d.displayHasBeenInitialized;
+ windowXPosition = d.windowXPosition;
+ windowYPosition = d.windowYPosition;
+
+ width = d.width;
+ height = d.height;
+}
+
+/*!
+ Destructor that desallocates memory.
+*/
+vpDisplay::~vpDisplay()
+{
+ displayHasBeenInitialized = false ;
+}
+
+/*!
+ Set the font of a text printed in the display overlay. To print a
+ text you may use displayCharString().
+
+ \param I : Image associated to the display window.
+ \param fontname : The expected font name.
+
+ \note Under UNIX, the available fonts are given by
+ the "xlsfonts" binary. To choose a font you can also use the
+ "xfontsel" binary.
+
+ \sa displayCharString()
+*/
+void
+vpDisplay::setFont ( const vpImage<unsigned char> &I,
+ const char *fontname )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setFont ( fontname ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Set the windows title.
+ \note This functionality is not implemented when vpDisplayOpenCV is used.
+
+ \param I : Image associated to the display window.
+ \param windowtitle : Window title.
+*/
+void
+vpDisplay::setTitle ( const vpImage<unsigned char> &I,
+ const char *windowtitle )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setTitle ( windowtitle ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Set the window position in the screen.
+
+ \param I : Image associated to the display window.
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void
+vpDisplay::setWindowPosition ( const vpImage<unsigned char> &I,
+ int winx, int winy )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setWindowPosition ( winx, winy ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Set the window background.
+
+ \param I : Image associated to the display window.
+ \param color: Background color.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void
+vpDisplay::setBackground(const vpImage<unsigned char> &I, const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->clearDisplay ( color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), close()
+*/
+void
+vpDisplay::display ( const vpImage<unsigned char> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayImage ( I ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+void
+vpDisplay::displayROI(const vpImage<unsigned char> &I, const vpRect &roi)
+{
+ vpImagePoint topLeft;
+ double top = floor(roi.getTop());
+ double left = floor(roi.getLeft());
+ double roiheight = floor(roi.getHeight());
+ double roiwidth = floor(roi.getWidth());
+ double iheight = (double)(I.getHeight());
+ double iwidth = (double)(I.getWidth());
+
+ if (top < 0 || top > iheight || left < 0 || left > iwidth || top+roiheight > iheight || left+roiwidth > iwidth)
+ {
+ vpERROR_TRACE ( "Region of interest outside of the image" ) ;
+ throw ( vpException ( vpException::dimensionError,"Region of interest outside of the image" ) ) ;
+ }
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayImageROI ( I , vpImagePoint(top,left), (unsigned int)roiwidth,(unsigned int)roiheight ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+
+/*!
+ Get the window pixmap and put it in vpRGBa image.
+
+ The code below shows how to use this method.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320); // Create a black grey level image
+ vpImage<vpRGBa> Ioverlay;
+
+ vpDisplay *d;
+
+ // Depending on the detected third party libraries, we instantiate here the
+ // first video device which is available
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Updates the color image with the original loaded image and the overlay
+ vpDisplay::getImage(I, Ioverlay) ;
+
+ // Write the color image on the disk
+ std::string ofilename("overlay.ppm");
+ vpImageIo::write(Ioverlay, ofilename) ;
+
+ // Wait for a click in the display window
+ vpDisplay::getClick(I);
+
+#ifdef VISP_HAVE_DISPLAY
+ delete d;
+#endif
+}
+ \endcode
+
+*/
+void
+vpDisplay::getImage ( const vpImage<unsigned char> &Isrc,
+ vpImage<vpRGBa> &Idest )
+{
+
+ try
+ {
+ if ( Isrc.display != NULL )
+ {
+ ( Isrc.display )->getImage ( Idest ) ;
+ }
+ else
+ {
+ vpImageConvert::convert(Isrc, Idest);
+// vpERROR_TRACE ( "Display not initialized" ) ;
+// throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+// "Display not initialized" ) ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Display the projection of an object frame represented by 3 arrows in
+ the image.
+
+ \param I : The image associated to the display.
+
+ \param cMo : Homogeneous matrix that gives the transformation
+ between the camera frame and the object frame to project in the
+ image.
+
+ \param cam : Camera intrinsic parameters.
+
+ \param size : Size of the object frame.
+
+ \param color : Color used to display the frame in the image.
+
+ \param thickness : the thickness of the line.
+
+ \param offset : Offset in pixels applied to the frame origin location in the image.
+
+*/
+void
+vpDisplay::displayFrame ( const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness,
+ vpImagePoint offset)
+{
+ // used by display
+ vpPoint o( 0.0, 0.0, 0.0);
+ vpPoint x(size, 0.0, 0.0);
+ vpPoint y( 0.0, size, 0.0);
+ vpPoint z( 0.0, 0.0, size);
+
+ o.track ( cMo ) ;
+ x.track ( cMo ) ;
+ y.track ( cMo ) ;
+ z.track ( cMo ) ;
+
+ vpImagePoint ipo, ip1;
+
+ if ( color == vpColor::none )
+ {
+ vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::red, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::green, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::blue, 4*thickness, 2*thickness, thickness) ;
+ }
+ else
+ {
+ vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ }
+}
+
+
+/*!
+
+ Display the projection of an object frame represented by 3 arrows in
+ the image.
+
+ \param I : The image associated to the display.
+
+ \param cMo : Homogeneous matrix that gives the transformation
+ between the camera frame and the object frame to project in the
+ image.
+
+ \param cam : Camera intrinsic parameters.
+
+ \param size : Size of the object frame.
+
+ \param color : Color used to display the frame in the image.
+
+ \param thickness : the thickness of the line
+
+ \param offset : Offset in pixels applied to the frame origin location in the image.
+*/
+void
+vpDisplay::displayFrame ( const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness,
+ vpImagePoint offset)
+{
+ // used by display
+ vpPoint o( 0.0, 0.0, 0.0);
+ vpPoint x(size, 0.0, 0.0);
+ vpPoint y( 0.0, size, 0.0);
+ vpPoint z( 0.0, 0.0, size);
+
+ o.track ( cMo ) ;
+ x.track ( cMo ) ;
+ y.track ( cMo ) ;
+ z.track ( cMo ) ;
+
+ vpImagePoint ipo, ip1;
+ if ( color == vpColor::none )
+ {
+ vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::red, 4, 2, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::green, 4, 2, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
+ vpDisplay::displayArrow ( I,ipo+offset, ip1+offset, vpColor::blue, 4, 2, thickness) ;
+ }
+ else
+ {
+ vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
+ vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+
+ }
+}
+
+/*!
+
+ Display the projection of an object camera represented by a cone in
+ the image.
+
+ \param I : The image associated to the display.
+
+ \param cMo : Homogeneous matrix that gives the transformation
+ between the camera frame and the object frame to project in the
+ image.
+
+ \param cam : Camera intrinsic parameters.
+
+ \param size : Size of the object camera.
+
+ \param color : Color used to display the camera in the image.
+
+ \param thickness : Thickness of the graphics drawing.
+
+*/
+void
+vpDisplay::displayCamera ( const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness)
+{
+ // used by display
+ double halfSize = size/2.0;
+ vpPoint pt[5];
+ pt[0].setWorldCoordinates ( -halfSize,-halfSize,0.0 );
+ pt[1].setWorldCoordinates ( halfSize,-halfSize,0.0 );
+ pt[2].setWorldCoordinates ( halfSize,halfSize,0.0 );
+ pt[3].setWorldCoordinates ( -halfSize,halfSize,0.0 );
+ pt[4].setWorldCoordinates ( 0.0,0.0,-size );
+
+ for (int i = 0; i < 5; i++)
+ pt[i].track ( cMo ) ;
+
+ vpImagePoint ip, ip_1, ip0;
+ vpMeterPixelConversion::convertPoint ( cam, pt[4].p[0], pt[4].p[1], ip0);
+
+ for (int i = 0; i < 4; i++)
+ {
+ vpMeterPixelConversion::convertPoint ( cam, pt[i].p[0], pt[i].p[1], ip_1);
+ vpMeterPixelConversion::convertPoint ( cam, pt[(i+1)%4].p[0], pt[(i+1)%4].p[1], ip);
+ vpDisplay::displayLine ( I, ip_1, ip, color, thickness);
+ vpDisplay::displayLine ( I, ip0, ip_1, color, thickness);
+ }
+}
+
+
+/*!
+
+ Display the projection of an object camera represented by a cone in
+ the image.
+
+ \param I : The image associated to the display.
+
+ \param cMo : Homogeneous matrix that gives the transformation
+ between the camera frame and the object frame to project in the
+ image.
+
+ \param cam : Camera intrinsic parameters.
+
+ \param size : Size of the object camera.
+
+ \param color : Color used to display the camera in the image.
+
+ \param thickness : Thickness of the graphics drawing.
+
+*/
+void
+vpDisplay::displayCamera( const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ double size, const vpColor &color,
+ unsigned int thickness)
+{
+ // used by display
+ double halfSize = size/2.0;
+ vpPoint pt[5];
+ pt[0].setWorldCoordinates ( -halfSize,-halfSize,0.0 );
+ pt[1].setWorldCoordinates ( halfSize,-halfSize,0.0 );
+ pt[2].setWorldCoordinates ( halfSize,halfSize,0.0 );
+ pt[3].setWorldCoordinates ( -halfSize,halfSize,0.0 );
+ pt[4].setWorldCoordinates ( 0.0,0.0,-size );
+
+ for (int i = 0; i < 5; i++)
+ pt[i].track ( cMo ) ;
+
+ vpImagePoint ip, ip_1, ip0;
+ vpMeterPixelConversion::convertPoint ( cam, pt[4].p[0], pt[4].p[1], ip0);
+
+ for (int i = 0; i < 4; i++)
+ {
+ vpMeterPixelConversion::convertPoint ( cam, pt[i].p[0], pt[i].p[1], ip_1);
+ vpMeterPixelConversion::convertPoint ( cam, pt[(i+1)%4].p[0], pt[(i+1)%4].p[1], ip);
+ vpDisplay::displayLine ( I, ip_1, ip, color, thickness);
+ vpDisplay::displayLine ( I, ip0, ip_1, color, thickness);
+ }
+}
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void
+vpDisplay::displayArrow ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w,unsigned int h,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+
+void
+vpDisplay::displayArrow ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w,unsigned int h,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display an arrow from image point (i1,j1) to image point (i2,j2).
+
+ \param I : The image associated to the display.
+ \param i1,j1 : Initial image point.
+ \param i2,j2 : Final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void
+vpDisplay::displayArrow ( const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int w, unsigned int h,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
+ ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display an arrow from image point (i1,j1) to image point (i2,j2).
+
+ \param I : The image associated to the display.
+ \param i1,j1 : Initial image point.
+ \param i2,j2 : Final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+
+*/
+void
+vpDisplay::displayArrow ( const vpImage<vpRGBa> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int w, unsigned int h,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
+
+ ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point \e ip location.
+ Use rather displayText() that does the same.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param ip : Upper left image point location of the string in the display.
+ \param string : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont(), displayText()
+
+*/
+void
+vpDisplay::displayCharString ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, const char *string,
+ const vpColor &color )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCharString ( ip, string, color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point \e ip location.
+ Use rather displayText() that does the same.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param ip : Upper left image point location of the string in the display.
+ \param string : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont(), displayText()
+
+*/
+void
+vpDisplay::displayCharString ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, const char *string,
+ const vpColor &color )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCharString ( ip, string, color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point (i,j) location.
+ Use rather displayText() that does the same.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param i,j : Upper left image point location of the string in the display.
+ \param string : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont(), displayText()
+
+*/
+void
+vpDisplay::displayCharString ( const vpImage<unsigned char> &I,
+ int i, int j, const char *string,
+ const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+
+ ( I.display )->displayCharString ( ip, string, color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point (i,j) location.
+ Use rather displayText() that does the same.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param i,j : Upper left image point location of the string in the display.
+ \param string : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont(), displayText()
+
+*/
+void
+vpDisplay::displayCharString ( const vpImage<vpRGBa> &I,
+ int i, int j, const char *string,
+ const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ ( I.display )->displayCharString ( ip, string, color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param ip : Upper left image point location of the string in the display.
+ \param s : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+
+*/
+void
+vpDisplay::displayText ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, const std::string &s,
+ const vpColor &color )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param ip : Upper left image point location of the string in the display.
+ \param s : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+
+*/
+void
+vpDisplay::displayText ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, const std::string &s,
+ const vpColor &color )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point (i,j) location.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param i,j : Upper left image point location of the string in the display.
+ \param s : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+
+*/
+void
+vpDisplay::displayText ( const vpImage<unsigned char> &I,
+ int i, int j, const std::string &s,
+ const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+
+ ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a string at the image point (i,j) location.
+
+ To select the font used to display the string, use setFont().
+
+ \param I : Image associated to the display.
+ \param i,j : Upper left image point location of the string in the display.
+ \param s : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+
+*/
+void
+vpDisplay::displayText ( const vpImage<vpRGBa> &I,
+ int i, int j, const std::string &s,
+ const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a circle.
+ \param I : The image associated to the display.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void
+vpDisplay::displayCircle ( const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCircle ( center, radius, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a circle.
+ \param I : The image associated to the display.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void
+vpDisplay::displayCircle ( const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCircle ( center, radius, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+
+ Display a circle.
+ \param I : The image associated to the display.
+ \param i,j : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+
+*/
+void
+vpDisplay::displayCircle ( const vpImage<unsigned char> &I,
+ int i, int j, unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+
+ ( I.display )->displayCircle ( ip, radius, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Display a circle.
+ \param I : The image associated to the display.
+ \param i,j : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+
+*/
+void
+vpDisplay::displayCircle ( const vpImage<vpRGBa> &I,
+ int i, int j, unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ ( I.display )->displayCircle ( ip, radius, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a cross at the image point \e ip location.
+ \param I : The image associated to the display.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplay::displayCross ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a cross at the image point \e ip location.
+ \param I : The image associated to the display.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplay::displayCross ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip, unsigned int size,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a cross at the image point (i,j) location.
+ \param I : The image associated to the display.
+ \param i,j : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplay::displayCross ( const vpImage<unsigned char> &I,
+ int i, int j,
+ unsigned int size, const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+
+ ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a cross at the image point (i,j) location.
+ \param I : The image associated to the display.
+ \param i,j : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplay::displayCross ( const vpImage<vpRGBa> &I,
+ int i, int j,
+ unsigned int size, const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Dashed line thickness.
+*/
+void vpDisplay::displayDotLine ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Dashed line thickness.
+*/
+void vpDisplay::displayDotLine ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a dashed line from image point (i1,j1) to image point (i2,j2).
+ \param I : The image associated to the display.
+ \param i1,j1: Initial image point.
+ \param i2,j2: Final image point.
+ \param color : Line color.
+ \param thickness : Dashed line thickness.
+
+*/
+void vpDisplay::displayDotLine ( const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a dashed line from image point (i1,j1) to image point (i2,j2).
+ \param I : The image associated to the display.
+ \param i1,j1: Initial image point.
+ \param i2,j2: Final image point.
+ \param color : Line color.
+ \param thickness : Dashed line thickness.
+*/
+void vpDisplay::displayDotLine ( const vpImage<vpRGBa> &I,
+ int i1, int j1,
+ int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplay::displayLine ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a line from image point (i1,j1) to image point (i2,j2).
+ \param I : The image associated to the display.
+ \param i1,j1: Initial image point.
+ \param i2,j2: Final image point.
+ \param color : Line color.
+ \param thickness : Line thickness.
+
+*/
+void vpDisplay::displayLine ( const vpImage<unsigned char> &I,
+ int i1, int j1, int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Display a line from image point (i1,j1) to image point (i2,j2).
+ \param I : The image associated to the display.
+ \param i1,j1: Initial image point.
+ \param i2,j2: Final image point.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplay::displayLine ( const vpImage<vpRGBa> &I,
+ int i1, int j1,
+ int i2, int j2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip1, ip2;
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param I : The image associated to the display.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplay::displayLine ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a point at the image point \e ip location.
+ \param I : The image associated to the display.
+ \param ip : Point location.
+ \param color : Point color.
+ \param thickness : Thickness of the point
+*/
+void vpDisplay::displayPoint ( const vpImage<unsigned char> &I,
+ const vpImagePoint &ip,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ if (thickness == 1)
+ ( I.display )->displayPoint ( ip, color ) ;
+ else {
+ vpRect rect(0, 0, thickness, thickness);
+ rect.moveCenter(ip);
+ ( I.display )->displayRectangle ( rect, color, true ) ;
+ }
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a point at the image point \e ip location.
+ \param I : The image associated to the display.
+ \param ip : Point location.
+ \param color : Point color.
+ \param thickness : Thickness of the point
+*/
+void vpDisplay::displayPoint ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &ip,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ if (thickness == 1)
+ ( I.display )->displayPoint ( ip, color ) ;
+ else {
+ vpRect rect(0, 0, thickness, thickness);
+ rect.moveCenter(ip);
+ ( I.display )->displayRectangle ( rect, color, true ) ;
+ }
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a point at the image point (i,j) location.
+ \param I : The image associated to the display.
+ \param i,j : Point location.
+ \param color : Point color.
+ \param thickness : Thickness of the point
+*/
+void vpDisplay::displayPoint ( const vpImage<unsigned char> &I,
+ int i, int j,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ if (thickness == 1)
+ ( I.display )->displayPoint ( ip, color ) ;
+ else {
+ vpRect rect(0, 0, thickness, thickness);
+ rect.moveCenter(ip);
+ ( I.display )->displayRectangle ( rect, color, true ) ;
+ }
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+ Display a point at the image point (i,j) location.
+ \param I : The image associated to the display.
+ \param i,j : Point location.
+ \param color : Point color.
+ \param thickness : Thickness of the point
+
+*/
+void vpDisplay::displayPoint ( const vpImage<vpRGBa> &I,
+ int i, int j,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint ip;
+ ip.set_i( i );
+ ip.set_j( j );
+ if (thickness == 1)
+ ( I.display )->displayPoint ( ip, color ) ;
+ else {
+ vpRect rect(0, 0, thickness, thickness);
+ rect.moveCenter(ip);
+ ( I.display )->displayRectangle ( rect, color, true ) ;
+ }
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a polygon defined by a vector of image points.
+ \param I : The image associated to the display.
+ \param vip : Vector of image point that define the vertexes of the polygon.
+ \param color : Line color.
+ \param thickness : Line thickness.
+
+*/
+void
+vpDisplay::displayPolygon(const vpImage<unsigned char> &I,
+ const std::vector<vpImagePoint> &vip,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ for (unsigned int i=0; i< vip.size(); i++)
+ ( I.display )->displayLine ( vip[i], vip[(i+1)%vip.size()], color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a polygon defined by a vector of image points.
+ \param I : The image associated to the display.
+ \param vip : Vector of image point that define the vertexes of the polygon.
+ \param color : Line color.
+ \param thickness : Line thickness.
+
+*/
+void
+vpDisplay::displayPolygon(const vpImage<vpRGBa> &I,
+ const std::vector<vpImagePoint> &vip,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ for (unsigned int i=0; i< vip.size(); i++)
+ ( I.display )->displayLine ( vip[i], vip[(i+1)%vip.size()], color, thickness );
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param topLeft : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
+ const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( topLeft, width, height, color,
+ fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
+ const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( topLeft, bottomRight, color,
+ fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
+ const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( rectangle, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Display a rectangle defined by its center, its orientation (angle)
+ and its size.
+
+ \param I : Image associated to the display.
+ \param center : Rectangle center point.
+ \param angle : Angle in radians width an horizontal axis oriented from left
+ to right.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+*/
+void
+vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if (I.display != NULL)
+ {
+ double i = center.get_i();
+ double j = center.get_j();
+
+ //A, B, C, D, corners of the rectangle clockwise
+ vpImagePoint ipa, ipb, ipc, ipd;
+ double cosinus = cos(angle);
+ double sinus = sin(angle);
+ ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
+ ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
+ ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
+ ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
+ ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
+ ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
+ ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
+ ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+
+ ( I.display )->displayLine(I, ipa, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipa, ipd, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught in displayRectangle") ;
+ throw ;
+ }
+}
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param topLeft : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( topLeft, width, height, color,
+ fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
+ const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( topLeft, bottomRight, color,
+ fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
+ const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayRectangle ( rectangle, color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Display a rectangle defined by its center, its orientation (angle)
+ and its size.
+
+ \param I : Image associated to the display.
+ \param center : Rectangle center point.
+ \param angle : Angle in radians width an horizontal axis oriented from left
+ to right.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+*/
+void
+vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if (I.display != NULL)
+ {
+ double i = center.get_i();
+ double j = center.get_j();
+
+ //A, B, C, D, corners of the rectangle clockwise
+ vpImagePoint ipa, ipb, ipc, ipd;
+ double cosinus = cos(angle);
+ double sinus = sin(angle);
+ ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
+ ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
+ ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
+ ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
+ ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
+ ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
+ ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
+ ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+
+ ( I.display )->displayLine(I, ipa, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipa, ipd, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught in displayRectangle") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a rectangle with (i,j) as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param i,j : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
+ int i, int j,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint topLeft;
+ topLeft.set_i( i );
+ topLeft.set_j( j );
+
+ ( I.display )->displayRectangle ( topLeft, width, height,
+ color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+
+ Display a rectangle defined by its center, its orientation (angle)
+ and its size.
+
+ \param I : Image associated to the display.
+ \param i,j : Rectangle center point.
+ \param angle : Angle in radians width an horizontal axis oriented from left
+ to right.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+*/
+void
+vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
+ unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color,unsigned int thickness)
+{
+ try
+ {
+ if (I.display != NULL)
+ {
+ //A, B, C, D, corners of the rectangle clockwise
+ vpImagePoint ipa, ipb, ipc, ipd;
+ float cosinus = cos(angle);
+ float sinus = sin(angle);
+ ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
+ ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
+ ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
+ ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
+ ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
+ ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
+ ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
+ ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+
+ ( I.display )->displayLine(I, ipa, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipa, ipd, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught in displayRectangle") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display a rectangle with (i,j) as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param I : The image associated to the display.
+ \param i,j : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
+ int i, int j,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ vpImagePoint topLeft;
+ topLeft.set_i( i );
+ topLeft.set_j( j );
+
+ ( I.display )->displayRectangle ( topLeft, width, height,
+ color, fill, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+/*!
+
+ Display a rectangle defined by its center, its orientation (angle)
+ and its size.
+
+ \param I : Image associated to the display.
+ \param i,j : Rectangle center point.
+ \param angle : Angle in radians width an horizontal axis oriented from left
+ to right.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+*/
+void
+vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
+ unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height,
+ const vpColor &color, unsigned int thickness)
+{
+ try
+ {
+ if (I.display != NULL)
+ {
+ //A, B, C, D, corners of the rectangle clockwise
+ vpImagePoint ipa, ipb, ipc, ipd;
+ float cosinus = cos(angle);
+ float sinus = sin(angle);
+ ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
+ ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
+ ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
+ ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
+ ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
+ ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
+ ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
+ ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+
+ ( I.display )->displayLine(I, ipa, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipa, ipd, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipb, color, thickness);
+ ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught in displayRectangle") ;
+ throw ;
+ }
+}
+
+/*!
+ Flushes the output buffer associated to image \e I display.
+ It's necessary to use this function to see the results of any drawing.
+
+ \warning This function is particular and must be called
+ to show the overlay. Because it's time spending, use it parcimoniously.
+
+ \code
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main() {
+ vpImage<unsigned char> I(240, 380);
+ vpDisplayGDI d;
+ d.init(I);
+ vpDisplay::display(I); // display the image
+ vpImagePoint center;
+ unsigned int radius = 100;
+ vpDisplay::displayCircle(I, center, radius, vpColor::red);
+
+ vpDisplay::flush(I); // Mendatory to display the requested features.
+}
+ \endcode
+
+*/
+void vpDisplay::flush ( const vpImage<unsigned char> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->flushDisplay() ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+void vpDisplay::flushROI ( const vpImage<unsigned char> &I, const vpRect &roi )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->flushDisplayROI(roi.getTopLeft(),(unsigned int)roi.getWidth(),(unsigned int)roi.getHeight()) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Close the display attached to I.
+*/
+void vpDisplay::close ( vpImage<unsigned char> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->closeDisplay() ;
+ I.display = NULL;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+ Set the windows title.
+ \note This functionality is not implemented when vpDisplayOpenCV is used.
+
+ \param I : Image associated to the display window.
+ \param windowtitle : Window title.
+*/
+void
+vpDisplay::setTitle ( const vpImage<vpRGBa> &I, const char *windowtitle )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setTitle ( windowtitle ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Set the window position in the screen.
+
+ \param I : Image associated to the display window.
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void
+vpDisplay::setWindowPosition ( const vpImage<vpRGBa> &I, int winx, int winy )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setWindowPosition ( winx, winy ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Set the font of a text printed in the display overlay. To print a
+ text you may use displayCharString().
+
+ \param I : Image associated to the display window.
+ \param fontname : The expected font name.
+
+ \note Under UNIX, the available fonts are given by
+ the "xlsfonts" binary. To choose a font you can also use the
+ "xfontsel" binary.
+
+ \sa displayCharString()
+*/
+void
+vpDisplay::setFont ( const vpImage<vpRGBa> &I, const char *fontname )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->setFont ( fontname ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Set the window background.
+
+ \param I : Image associated to the display window.
+ \param color: Background color.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void
+vpDisplay::setBackground(const vpImage<vpRGBa> &I, const vpColor &color)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->clearDisplay ( color ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), close()
+*/
+void
+vpDisplay::display ( const vpImage<vpRGBa> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayImage ( I ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+void
+vpDisplay::displayROI(const vpImage<vpRGBa> &I, const vpRect &roi)
+{
+ vpImagePoint topLeft;
+ double top = floor(roi.getTop());
+ double left = floor(roi.getLeft());
+ double roiheight = floor(roi.getHeight());
+ double roiwidth = floor(roi.getWidth());
+ double iheight = (double)(I.getHeight());
+ double iwidth = (double)(I.getWidth());
+
+ if (top < 0 || top >= iheight || left < 0 || left >= iwidth || top+roiheight >= iheight || left+roiwidth >= iwidth)
+ {
+ vpERROR_TRACE ( "Region of interest outside of the image" ) ;
+ throw ( vpException ( vpException::dimensionError,
+ "Region of interest outside of the image" ) ) ;
+ }
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->displayImageROI ( I , vpImagePoint(top,left), (unsigned int)roiwidth,(unsigned int)roiheight ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+
+/*!
+ Get the window pixmap and put it in vpRGBa image.
+
+ The code below shows how to use this method.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+int main()
+{
+ vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
+ vpImage<vpRGBa> Ioverlay;
+
+ vpDisplay *d;
+
+ // Depending on the detected third party libraries, we instantiate here the
+ // first video device which is available
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Updates the color image with the original loaded image and the overlay
+ vpDisplay::getImage(I, Ioverlay) ;
+
+ // Write the color image on the disk
+ std::string ofilename("overlay.ppm");
+ vpImageIo::write(Ioverlay, ofilename) ;
+
+ // Wait for a click in the display window
+ vpDisplay::getClick(I);
+
+#ifdef VISP_HAVE_DISPLAY
+ delete d;
+#endif
+}
+ \endcode
+
+*/
+void
+vpDisplay::getImage ( const vpImage<vpRGBa> &Isrc, vpImage<vpRGBa> &Idest )
+{
+
+ try
+ {
+ if ( Isrc.display != NULL )
+ {
+ ( Isrc.display )->getImage ( Idest ) ;
+ }
+ else {
+ Idest = Isrc;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Flushes the output buffer associated to image \e I display.
+ It's necessary to use this function to see the results of any drawing.
+
+ \warning This function is particular and must be called
+ to show the overlay. Because it's time spending, use it parcimoniously.
+
+ \code
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRGBa.h>
+
+int main() {
+ vpImage<vpRGBa> I(240, 380);
+ vpDisplayGDI d;
+ d.init(I);
+ vpDisplay::display(I); // display the image
+ vpImagePoint center;
+ unsigned int radius = 100;
+ vpDisplay::displayCircle(I, center, radius, vpColor::red);
+
+ vpDisplay::flush(I); // Mendatory to display the requested features.
+}
+ \endcode
+
+*/
+void vpDisplay::flush ( const vpImage<vpRGBa> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->flushDisplay() ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+void vpDisplay::flushROI ( const vpImage<vpRGBa> &I, const vpRect &roi )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->flushDisplayROI(roi.getTopLeft(),(unsigned int)roi.getWidth(),(unsigned int)roi.getHeight()) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Close the display attached to I.
+*/
+void vpDisplay::close ( vpImage<vpRGBa> &I )
+{
+
+ try
+ {
+ if ( I.display != NULL )
+ {
+ ( I.display )->closeDisplay() ;
+ I.display = NULL;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+}
+
+/*!
+ Wait for a click from one of the mouse button.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool vpDisplay::getClick ( const vpImage<unsigned char> &I, bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick(blocking) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
+ vpImagePoint &ip, bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick ( ip, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Wait for a mouse button click and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick ( ip, button, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Wait for a mouse button click and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise.
+
+*/
+bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ vpImagePoint ip;
+ return vpDisplay::getClick(I, ip, button, blocking);
+}
+
+/*!
+ Wait for a mouse button click and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise.
+
+*/
+bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ vpImagePoint ip;
+ return vpDisplay::getClick(I, ip, button, blocking);
+}
+
+/*!
+ Wait for a mouse button click release and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The clicked button.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ released and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ released, otherwise returns false.
+
+ \return true if a mouse button is released, false otherwise. If a
+ button is released, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool
+vpDisplay::getClickUp ( const vpImage<unsigned char> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClickUp ( ip, button, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Wait for a mouse button click release and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param button [out] : The clicked button.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ released and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ released, otherwise returns false.
+
+ \return true if a mouse button is released, false otherwise.
+
+*/
+bool vpDisplay::getClickUp ( const vpImage<unsigned char> &I,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ vpImagePoint ip;
+ return vpDisplay::getClickUp(I, ip, button, blocking);
+}
+
+/*!
+ Wait for a mouse button click release and get the position of the clicked
+ image point. The button used to click is also set.
+
+ \param I [in] : The displayed image.
+
+ \param button [out] : The clicked button.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ released and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ released, otherwise returns false.
+
+ \return true if a mouse button is released, false otherwise.
+
+*/
+bool vpDisplay::getClickUp ( const vpImage<vpRGBa> &I,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ vpImagePoint ip;
+ return vpDisplay::getClickUp(I, ip, button, blocking);
+}
+
+/*!
+ Get a keyboard event.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ Below you will find an example showing how to use this method.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320); // Create a black image
+
+ vpDisplay *d;
+
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#else
+ std::cout << "Sorry, no video device is available" << std::endl;
+ return -1;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Wait for keyboard event
+ std::cout << "Waiting a keyboard event..." << std::endl;
+ vpDisplay::getKeyboardEvent(I, true);
+ std::cout << "A keyboard event was detected" << std::endl;
+
+ // Non blocking keyboard event loop
+ int cpt_event = 0;
+ bool event;
+ char key[10];
+ std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
+ do {
+ event = vpDisplay::getKeyboardEvent(I, &key[0], false);
+ if (event) {
+ std::cout << "Key detected: " << key << std::endl;
+ cpt_event ++;
+ }
+
+ vpTime::wait(5); // wait 5 ms
+ } while(cpt_event < 5);
+
+#ifdef VISP_HAVE_DISPLAY
+ delete d;
+#endif
+}
+\endcode
+*/
+bool
+vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getKeyboardEvent ( blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+
+ Get a keyboard event.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ Below you will find an example showing how to use this method.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320); // Create a black image
+
+ vpDisplay *d;
+
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#else
+ std::cout << "Sorry, no video device is available" << std::endl;
+ return -1;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Wait for keyboard event
+ std::cout << "Waiting a keyboard event..." << std::endl;
+ vpDisplay::getKeyboardEvent(I, true);
+ std::cout << "A keyboard event was detected" << std::endl;
+
+ // Non blocking keyboard event loop
+ int cpt_event = 0;
+ bool event;
+ char key[10];
+ std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
+ do {
+ event = vpDisplay::getKeyboardEvent(I, &key[0], false);
+ if (event) {
+ std::cout << "Key detected: " << key << std::endl;
+ cpt_event ++;
+ }
+
+ vpTime::wait(5); // wait 5 ms
+ } while(cpt_event < 5);
+
+#ifdef VISP_HAVE_DISPLAY
+ delete d;
+#endif
+}
+\endcode
+*/
+bool
+vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I,
+ char *string, bool blocking)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getKeyboardEvent ( string, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+*/
+bool
+vpDisplay::getPointerMotionEvent (const vpImage<unsigned char> &I,
+ vpImagePoint &ip)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getPointerMotionEvent ( ip ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+*/
+bool
+vpDisplay::getPointerPosition (const vpImage<unsigned char> &I,
+ vpImagePoint &ip)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getPointerPosition ( ip ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false;
+}
+
+/*!
+ Wait for a click.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise.
+*/
+bool vpDisplay::getClick ( const vpImage<vpRGBa> &I, bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick(blocking) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false;
+}
+
+
+/*!
+ Return true when a mouse button is pressed.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
+ vpImagePoint &ip, bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick ( ip, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Return true when a mouse button is pressed.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The clicked button.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClick ( ip, button, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Return true when a mouse button is released.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The clicked button.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ released and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ released, otherwise returns false.
+
+ \return true if a mouse button is released, false otherwise. If a
+ button is released, the location of the mouse pointer is updated in
+ \e ip.
+
+*/
+bool
+vpDisplay::getClickUp ( const vpImage<vpRGBa> &I,
+ vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking )
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getClickUp ( ip, button, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+ Get a keyboard event.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ Below you will find an example showing how to use this method.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+int main()
+{
+ vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
+
+ vpDisplay *d;
+
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#else
+ std::cout << "Sorry, no video device is available" << std::endl;
+ return -1;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Wait for keyboard event
+ std::cout << "Waiting a keyboard event..." << std::endl;
+ vpDisplay::getKeyboardEvent(I, true);
+ std::cout << "A keyboard event was detected" << std::endl;
+
+ // Non blocking keyboard event loop
+ int cpt_event = 0;
+ bool event;
+ char key[10];
+ std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
+ do {
+ event = vpDisplay::getKeyboardEvent(I, &key[0], false);
+ if (event) {
+ std::cout << "Key detected: " << key << std::endl;
+ cpt_event ++;
+ }
+ vpTime::wait(5); // wait 5 ms
+ } while(cpt_event < 5);
+
+#ifdef VISP_HAVE_DISPLAY
+ delete d;
+#endif
+}
+\endcode
+*/
+bool
+vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getKeyboardEvent ( blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+
+/*!
+
+ Get a keyboard event.
+
+ \param I [in] : The displayed image.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ Below you will find an example showing how to use this method.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+int main()
+{
+ vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
+
+ vpDisplay *d;
+
+#if defined(VISP_HAVE_X11)
+ d = new vpDisplayX;
+#elif defined(VISP_HAVE_GTK)
+ d = new vpDisplayGTK;
+#elif defined(VISP_HAVE_GDI)
+ d = new vpDisplayGDI;
+#elif defined(VISP_HAVE_D3D9)
+ d = new vpDisplayD3D;
+#elif defined(VISP_HAVE_OPENCV)
+ d = new vpDisplayOpenCV;
+#else
+ std::cout << "Sorry, no video device is available" << std::endl;
+ return -1;
+#endif
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+#ifdef VISP_HAVE_DISPLAY
+ d->init(I);
+#endif
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Wait for keyboard event
+ std::cout << "Waiting a keyboard event..." << std::endl;
+ vpDisplay::getKeyboardEvent(I, true);
+ std::cout << "A keyboard event was detected" << std::endl;
+
+ // Non blocking keyboard event loop
+ int cpt_event = 0;
+ bool event;
+ char key[10];
+ std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
+ do {
+ event = vpDisplay::getKeyboardEvent(I, &key[0], false);
+ if (event) {
+ std::cout << "Key detected: " << key << std::endl;
+ cpt_event ++;
+ }
+
+ vpTime::wait(5); // wait 5 ms
+ } while(cpt_event < 5);
+
+ delete d;
+}
+\endcode
+*/
+bool
+vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I,
+ char *string, bool blocking)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getKeyboardEvent ( string, blocking ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false ;
+}
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+*/
+bool
+vpDisplay::getPointerMotionEvent (const vpImage<vpRGBa> &I, vpImagePoint &ip)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getPointerMotionEvent ( ip ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param I [in] : The displayed image.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+*/
+bool
+vpDisplay::getPointerPosition (const vpImage<vpRGBa> &I, vpImagePoint &ip)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ return ( I.display )->getPointerPosition ( ip ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ return false;
+}
+
+/*!
+ Display an ellipse from its parameters expressed in pixels.
+ \param I : Image to consider.
+ \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
+ \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
+ are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
+ \param use_centered_moments : When false, the parameters coef1, coef2, coef3
+ are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
+ from the centered moments.
+ \param color : Drawings color.
+ \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+
+ \f[ \left(\begin{array}{c}
+ u_\theta \\
+ v_\theta
+ \end{array} \right) = \left(\begin{array}{c}
+ u_c \\
+ v_c
+ \end{array} \right) + \left(\begin{array}{cc}
+ \cos(e) & -\sin(e) \\
+ \sin(e) & \cos(e)
+ \end{array} \right) \left(\begin{array}{c}
+ a \cos(\theta) \\
+ b \sin(\theta)
+ \end{array} \right) \f]
+
+ with \f$0 \leq \theta \leq 2\pi\f$.
+
+ The following example shows how to use for example this function to display the result of a tracking.
+ \code
+ vpMeEllipse ellipse;
+ ...
+ vpDisplay::display(I);
+ ellipse.track(I) ;
+
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
+ true, vpColor::orange, 1);
+ vpDisplay::flush(I);
+ \endcode
+*/
+void vpDisplay::displayEllipse(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
+}
+
+/*!
+ Display an ellipse from its parameters expressed in pixels.
+ \param I : Image to consider.
+ \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
+ \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
+ are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
+ \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
+ If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
+ \param use_centered_moments : When false, the parameters coef1, coef2, coef3
+ are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
+ from the centered moments.
+ \param color : Drawings color.
+ \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+
+ \f[ \left(\begin{array}{c}
+ u_\theta \\
+ v_\theta
+ \end{array} \right) = \left(\begin{array}{c}
+ u_c \\
+ v_c
+ \end{array} \right) + \left(\begin{array}{cc}
+ \cos(e) & -\sin(e) \\
+ \sin(e) & \cos(e)
+ \end{array} \right) \left(\begin{array}{c}
+ a \cos(\theta) \\
+ b \sin(\theta)
+ \end{array} \right) \f]
+
+ with \f$\theta_1 \leq \theta \leq \theta_2\f$.
+
+ The following example shows how to use for example this function to display the result of a tracking.
+ \code
+ vpMeEllipse ellipse;
+ ...
+ vpDisplay::display(I);
+ ellipse.track(I) ;
+
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
+ ellipse.get_mu11(), ellipse.get_mu02(),
+ ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
+ true, vpColor::orange, 1);
+ vpDisplay::flush(I);
+ \endcode
+*/
+void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ const double &theta1, const double &theta2, bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ double j1, i1;
+ vpImagePoint iP11;
+ double j2, i2;
+ vpImagePoint iP22;
+ j1 = j2 = i1 = i2 = 0 ;
+ double a=0., b=0., e=0.;
+
+ double mu20_p = coef1;
+ double mu11_p = coef2;
+ double mu02_p = coef3;
+
+ if (use_centered_moments) {
+ if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
+
+ double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p)/2);
+ b = sqrt((mu20_p + mu02_p - val_p)/2);
+
+ e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
+ e = atan(e);
+ }
+ else {
+ a = sqrt(mu20_p);
+ b = sqrt(mu02_p);
+ e = 0.;
+ }
+ }
+ else {
+ a = coef1;
+ b = coef2;
+ e = coef3;
+ }
+
+ // Approximation of the circumference of an ellipse:
+ // [Ramanujan, S., "Modular Equations and Approximations to ,"
+ // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
+ double t = (a-b)/(a+b);
+ double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
+
+ int nbpoints = (int)(floor(circumference/5));
+ if (nbpoints < 10)
+ nbpoints = 10;
+ double incr = 2*M_PI / nbpoints ; // angle increment
+
+ double smallalpha = theta1;
+ double highalpha = theta2;
+ double ce = cos(e);
+ double se = sin(e);
+
+ double k = smallalpha ;
+ j1 = a *cos(k) ; // equation of an ellipse
+ i1 = b *sin(k) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + ce *j1 - se *i1 );
+ iP11.set_i ( center.get_i() + se *j1 + ce *i1 );
+
+ while (k+incr<highalpha+incr)
+ {
+ j2 = a *cos(k+incr) ; // equation of an ellipse
+ i2 = b *sin(k+incr) ; // equation of an ellipse
+
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + ce *j2 - se *i2 );
+ iP22.set_i ( center.get_i() + se *j2 + ce *i2 );
+
+ ( I.display )->displayLine(iP11, iP22, color, thickness) ;
+
+ i1 = i2;
+ j1 = j2;
+ iP11 = iP22;
+
+ k += incr ;
+ }
+ }
+ }
+ catch ( vpException &e )
+ {
+ throw(e) ;
+ }
+}
+
+/*!
+ Display an ellipse from its parameters expressed in pixels.
+ \param I : Image to consider.
+ \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
+ \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
+ are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
+ \param use_centered_moments : When false, the parameters coef1, coef2, coef3
+ are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
+ from the centered moments.
+ \param color : Drawings color.
+ \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+
+ \f[ \left(\begin{array}{c}
+ u_\theta \\
+ v_\theta
+ \end{array} \right) = \left(\begin{array}{c}
+ u_c \\
+ v_c
+ \end{array} \right) + \left(\begin{array}{cc}
+ \cos(e) & -\sin(e) \\
+ \sin(e) & \cos(e)
+ \end{array} \right) \left(\begin{array}{c}
+ a \cos(\theta) \\
+ b \sin(\theta)
+ \end{array} \right) \f]
+
+ with \f$0 \leq \theta \leq 2\pi\f$.
+
+ The following example shows how to use for example this function to display the result of a tracking.
+ \code
+ vpMeEllipse ellipse;
+ ...
+ vpDisplay::display(I);
+ ellipse.track(I) ;
+
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
+ true, vpColor::orange, 1);
+ vpDisplay::flush(I);
+ \endcode
+*/
+void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
+}
+
+/*!
+ Display an ellipse from its parameters expressed in pixels.
+ \param I : Image to consider.
+ \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
+ \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
+ are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
+ \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
+ If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
+ \param use_centered_moments : When false, the parameters coef1, coef2, coef3
+ are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
+ from the centered moments.
+ \param color : Drawings color.
+ \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+
+ \f[ \left(\begin{array}{c}
+ u_\theta \\
+ v_\theta
+ \end{array} \right) = \left(\begin{array}{c}
+ u_c \\
+ v_c
+ \end{array} \right) + \left(\begin{array}{cc}
+ \cos(e) & -\sin(e) \\
+ \sin(e) & \cos(e)
+ \end{array} \right) \left(\begin{array}{c}
+ a \cos(\theta) \\
+ b \sin(\theta)
+ \end{array} \right) \f]
+
+ with \f$\theta_1 \leq \theta \leq \theta_2\f$.
+
+ The following example shows how to use for example this function to display the result of a tracking.
+ \code
+ vpMeEllipse ellipse;
+ ...
+ vpDisplay::display(I);
+ ellipse.track(I) ;
+
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
+ ellipse.get_mu11(), ellipse.get_mu02(),
+ ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
+ true, vpColor::orange, 1);
+ vpDisplay::flush(I);
+ \endcode
+*/
+void vpDisplay::displayEllipse(const vpImage<unsigned char> &I,
+ const vpImagePoint ¢er,
+ const double &coef1, const double &coef2, const double &coef3,
+ const double &theta1, const double &theta2, bool use_centered_moments,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ if ( I.display != NULL )
+ {
+ double j1, i1;
+ vpImagePoint iP11;
+ double j2, i2;
+ vpImagePoint iP22;
+ j1 = j2 = i1 = i2 = 0 ;
+ double a=0., b=0., e=0.;
+
+ double mu20_p = coef1;
+ double mu11_p = coef2;
+ double mu02_p = coef3;
+
+ if (use_centered_moments) {
+ if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
+
+ double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p)/2);
+ b = sqrt((mu20_p + mu02_p - val_p)/2);
+
+ e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
+ e = atan(e);
+ }
+ else {
+ a = sqrt(mu20_p);
+ b = sqrt(mu02_p);
+ e = 0.;
+ }
+ }
+ else {
+ a = coef1;
+ b = coef2;
+ e = coef3;
+ }
+
+ // Approximation of the circumference of an ellipse:
+ // [Ramanujan, S., "Modular Equations and Approximations to ,"
+ // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
+ double t = (a-b)/(a+b);
+ double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
+
+ int nbpoints = (int)(floor(circumference/5));
+ if (nbpoints < 10)
+ nbpoints = 10;
+ double incr = 2*M_PI / nbpoints ; // angle increment
+
+ double smallalpha = theta1;
+ double highalpha = theta2;
+ double ce = cos(e);
+ double se = sin(e);
+
+ double k = smallalpha ;
+ j1 = a *cos(k) ; // equation of an ellipse
+ i1 = b *sin(k) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + ce *j1 - se *i1 );
+ iP11.set_i ( center.get_i() + se *j1 + ce *i1 );
+
+ while (k+incr<highalpha+incr)
+ {
+ j2 = a *cos(k+incr) ; // equation of an ellipse
+ i2 = b *sin(k+incr) ; // equation of an ellipse
+
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + ce *j2 - se *i2 );
+ iP22.set_i ( center.get_i() + se *j2 + ce *i2 );
+
+ ( I.display )->displayLine(iP11, iP22, color, thickness) ;
+
+ i1 = i2;
+ j1 = j2;
+ iP11 = iP22;
+
+ k += incr ;
+ }
+ }
+ }
+ catch ( vpException &e )
+ {
+ throw(e) ;
+ }
+}
+
diff --git a/modules/core/src/display/vpFeatureDisplay.cpp b/modules/core/src/display/vpFeatureDisplay.cpp
new file mode 100644
index 0000000..6ab13e0
--- /dev/null
+++ b/modules/core/src/display/vpFeatureDisplay.cpp
@@ -0,0 +1,425 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+// Meter/pixel conversion
+#include <visp3/core/vpMeterPixelConversion.h>
+
+// display
+#include <visp3/core/vpDisplay.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpImagePoint.h>
+
+
+
+/*!
+ \param x,y : Point coordinates in meters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayPoint(double x,double y,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try{
+ vpImagePoint ip; // pixel coordinates in float
+ vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
+
+ vpDisplay::displayCross(I, ip, 15, color, thickness) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+}
+/*!
+ \param rho, theta : Line parameters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayLine(double rho,double theta,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+
+ try{
+ // x cos(theta) + y sin(theta) - rho = 0
+
+ double rhop,thetap ;
+ vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
+
+ // u cos(thetap) + v sin(thetap) - rhop = 0
+
+ double co = cos(thetap) ;
+ double si = sin(thetap) ;
+ double c = -rhop ;
+
+ // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
+ // double u1,v1,u2,v2 ;
+
+ double a = si ;
+ double b = co ;
+ vpImagePoint ip1, ip2;
+
+ if (fabs(a) < fabs(b)) {
+ ip1.set_ij(0, (-c)/b);
+ double h = I.getHeight() - 1;
+ ip2.set_ij(h, (-c - a*h)/b);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ }
+ else {
+ ip1.set_ij((-c)/a, 0);
+ double w = I.getWidth()-1;
+ ip2.set_ij((-c - b*w)/a, w);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+/*!
+ \param rho1, theta1 : Cylinder limb1 parameters.
+ \param rho2, theta2 : Cylinder limb2 parameters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayCylinder(double rho1,double theta1,
+ double rho2,double theta2,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ displayLine(rho1, theta1, cam, I, color, thickness) ;
+ displayLine(rho2, theta2, cam, I, color, thickness) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+/*!
+ \param x, y, mu20, mu11, mu02 : Ellipse parameters where:
+ - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
+ along the horizontal and vertical axis in the image.
+ - \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+
+ \sa vpDisplay::displayEllipse()
+*/
+void vpFeatureDisplay::displayEllipse(double x,double y,
+ double mu20, double mu11, double mu02,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try {
+ unsigned int number_of_points = 45 ;
+ const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
+ unsigned int i = 0 ;
+
+ double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
+ double a, b, e ;
+
+ //if (fabs(mu11)<1e-6) e =0 ;
+ if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
+ e = 0 ;
+ a = sqrt(mu20);
+ b = sqrt(mu02);
+ }
+ else {
+ e = (mu02-mu20+s)/(2*mu11) ;
+ a = sqrt( (mu02+mu20+s)/2.0) ;
+ b = sqrt( (mu02+mu20-s)/2.0) ;
+ }
+
+ double e1 = atan(e) ;
+
+ double k = 0.0 ;
+
+ double ce = cos(e1) ;
+ double se = sin(e1) ;
+
+ double x2 = 0;
+ double y2 =0;
+ vpImagePoint ip1, ip2;
+
+ for( i = 0; i < number_of_points+2 ; i++)
+ {
+ double x1 = a *cos(k) ; // equation of an ellipse
+ double y1 = b *sin(k) ; // equation of an ellipse
+ double x11 = x + ce *x1 - se *y1 ;
+ double y11 = y + se *x1 + ce *y1 ;
+
+ vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
+
+ if (i > 1) {
+ ip2.set_u( x2 );
+ ip2.set_v( y2 );
+
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
+ }
+
+ ip2 = ip1;
+ y2 = y1 ;
+ x2 = x1 ;
+ k += incr ;
+ } // end for loop
+ }
+ catch(vpException &e)
+ {
+ throw(e);
+ }
+}
+
+/*!
+ \param x, y : Point coordinates in meters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayPoint(double x,double y,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try{
+ vpImagePoint ip; // pixel coordinates in float
+ vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
+
+ vpDisplay::displayCross(I, ip, 15, color, thickness) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+}
+
+/*!
+ \param rho, theta : Line parameters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayLine(double rho,double theta,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+
+ try{
+ // x cos(theta) + y sin(theta) - rho = 0
+
+ double rhop,thetap ;
+ vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
+
+ // u cos(thetap) + v sin(thetap) - rhop = 0
+
+ double co = cos(thetap) ;
+ double si = sin(thetap) ;
+ double c = -rhop ;
+
+ // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
+ // double u1,v1,u2,v2 ;
+
+ double a = si ;
+ double b = co ;
+ vpImagePoint ip1, ip2;
+
+ if (fabs(a) < fabs(b)) {
+ ip1.set_ij(0, (-c)/b);
+ double h = I.getHeight() - 1;
+ ip2.set_ij(h, (-c - a*h)/b);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ }
+ else {
+ ip1.set_ij((-c)/a, 0);
+ double w = I.getWidth()-1;
+ ip2.set_ij((-c - b*w)/a, w);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+/*!
+ \param rho1, theta1 : Cylinder limb1 parameters.
+ \param rho2, theta2 : Cylinder limb2 parameters.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+*/
+void vpFeatureDisplay::displayCylinder(double rho1, double theta1,
+ double rho2, double theta2,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try
+ {
+ displayLine(rho1, theta1, cam, I, color, thickness) ;
+ displayLine(rho2, theta2, cam, I, color, thickness) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+ \param x, y, mu20, mu11, mu02 : Ellipse parameters where:
+ - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
+ along the horizontal and vertical axis in the image.
+ - \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
+ \param cam : Camera intrinsic parameters.
+ \param I : Image.
+ \param color : Color to use to display the feature
+ \param thickness : Thickness of the feature representation.
+
+ \sa vpDisplay::displayEllipse()
+*/
+void vpFeatureDisplay::displayEllipse(double x, double y,
+ double mu20, double mu11, double mu02,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ try {
+ unsigned int number_of_points = 45 ;
+ const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
+ unsigned int i = 0 ;
+
+ double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
+ double a, b, e ;
+
+ //if (fabs(mu11)<1e-6) e =0 ;
+ if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
+ e = 0 ;
+ a = sqrt(mu20);
+ b = sqrt(mu02);
+ }
+ else {
+ e = (mu02-mu20+s)/(2*mu11) ;
+ a = sqrt( (mu02+mu20+s)/2.0) ;
+ b = sqrt( (mu02+mu20-s)/2.0) ;
+ }
+
+ double e1 = atan(e) ;
+
+ double k = 0.0 ;
+
+ double ce = cos(e1) ;
+ double se = sin(e1) ;
+
+ double x2 = 0;
+ double y2 =0;
+ vpImagePoint ip1, ip2;
+
+ for( i = 0; i < number_of_points+2 ; i++)
+ {
+ double x1 = a *cos(k) ; // equation of an ellipse
+ double y1 = b *sin(k) ; // equation of an ellipse
+ double x11 = x + ce *x1 - se *y1 ;
+ double y11 = y + se *x1 + ce *y1 ;
+
+ vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
+
+ if (i > 1) {
+ ip2.set_u( x2 );
+ ip2.set_v( y2 );
+
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
+ }
+
+ ip2 = ip1;
+ y2 = y1 ;
+ x2 = x1 ;
+ k += incr ;
+ } // end for loop
+ }
+ catch(vpException &e)
+ {
+ throw(e);
+ }
+}
diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp
new file mode 100644
index 0000000..3780ce8
--- /dev/null
+++ b/modules/core/src/image/vpImageConvert.cpp
@@ -0,0 +1,3777 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Convert image types.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpImageConvert.cpp
+ \brief Convert image types
+*/
+
+
+#include <sstream>
+#include <map>
+
+// image
+#include <visp3/core/vpImageConvert.h>
+
+bool vpImageConvert::YCbCrLUTcomputed = false;
+int vpImageConvert::vpCrr[256];
+int vpImageConvert::vpCgb[256];
+int vpImageConvert::vpCgr[256];
+int vpImageConvert::vpCbb[256];
+
+
+/*!
+Convert a vpImage\<vpRGBa\> to a vpImage\<unsigned char\>
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> & dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+
+ GreyToRGBa(src.bitmap, (unsigned char *)dest.bitmap,
+ src.getHeight() * src.getWidth() );
+}
+
+/*!
+Convert a vpImage\<unsigned char\> to a vpImage\<vpRGBa\>
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> & dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+
+ RGBaToGrey((unsigned char *)src.bitmap, dest.bitmap,
+ src.getHeight() * src.getWidth() );
+}
+
+
+/*!
+Convert a vpImage\<float\> to a vpImage\<unsigend char\> by renormalizing between 0 and 255.
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<float> &src, vpImage<unsigned char> &dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+ unsigned int max_xy = src.getWidth()*src.getHeight();
+ float min, max;
+
+ src.getMinMaxValue(min,max);
+
+ for (unsigned int i = 0; i < max_xy; i++) {
+ float val = 255.f * (src.bitmap[i] - min) / (max - min);
+ if(val < 0)
+ dest.bitmap[i] = 0;
+ else if(val > 255)
+ dest.bitmap[i] = 255;
+ else
+ dest.bitmap[i] = (unsigned char)val;
+ }
+}
+
+/*!
+Convert a vpImage\<unsigned char\> to a vpImage\<float\> by basic casting.
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<float> &dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+ for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
+ dest.bitmap[i] = (float)src.bitmap[i];
+}
+
+/*!
+Convert a vpImage\<double\> to a vpImage\<unsigend char\> by renormalizing between 0 and 255.
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<double> &src, vpImage<unsigned char> &dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+ unsigned int max_xy = src.getWidth()*src.getHeight();
+ double min, max;
+
+ src.getMinMaxValue(min,max);
+
+ for (unsigned int i = 0; i < max_xy; i++) {
+ double val = 255. * (src.bitmap[i] - min) / (max - min);
+ if(val < 0)
+ dest.bitmap[i] = 0;
+ else if(val > 255)
+ dest.bitmap[i] = 255;
+ else
+ dest.bitmap[i] = (unsigned char)val;
+ }
+}
+
+/*!
+Convert a vpImage\<unsigned char\> to a vpImage\<double\> by basic casting.
+\param src : source image
+\param dest : destination image
+*/
+void
+vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<double> &dest)
+{
+ dest.resize(src.getHeight(), src.getWidth()) ;
+ for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
+ dest.bitmap[i] = (double)src.bitmap[i];
+}
+
+#ifdef VISP_HAVE_OPENCV
+// Deprecated: will be removed with OpenCV transcient from C to C++ api
+/*!
+ \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
+ IplImage structure will be removed with OpenCV transcient from C to C++ api.
+
+ Convert an IplImage to a vpImage\<vpRGBa\>.
+
+ An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
+ image structure. See http://opencvlibrary.sourceforge.net/ for general
+ OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
+ for the specific IplImage structure documentation.
+
+ \warning This function is only available if OpenCV was detected during
+ the configuration step.
+
+ \param src : Source image in OpenCV format.
+ \param dest : Destination image in ViSP format.
+ \param flip : Set to true to vertically flip the converted image.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ vpImage<vpRGBa> Ic; // A color image
+ IplImage* Ip;
+
+ // Read an image on a disk with openCV library
+ Ip = cvLoadImage("image.ppm", CV_LOAD_IMAGE_COLOR);
+ // Convert the grayscale IplImage into vpImage<vpRGBa>
+ vpImageConvert::convert(Ip, Ic);
+
+ // ...
+
+ // Release Ip header and data
+ cvReleaseImage(&Ip);
+#endif
+}
+ \endcode
+*/
+void
+vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
+{
+ int nChannel = src->nChannels;
+ int depth = src->depth;
+ int height = src->height;
+ int width = src->width;
+ int widthStep = src->widthStep;
+ int lineStep = (flip) ? 1 : 0;
+
+ if(nChannel == 3 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width);
+
+ //starting source address
+ unsigned char* input = (unsigned char*)src->imageData;
+ unsigned char* line;
+ unsigned char* beginOutput = (unsigned char*)dest.bitmap;
+ unsigned char* output = NULL;
+
+ for(int i=0 ; i < height ; i++)
+ {
+ line = input;
+ output = beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) + (1-lineStep) * 4 * width * i;
+ for(int j=0 ; j < width ; j++)
+ {
+ *(output++) = *(line+2);
+ *(output++) = *(line+1);
+ *(output++) = *(line);
+ *(output++) = 0;
+
+ line+=3;
+ }
+ //go to the next line
+ input+=widthStep;
+ }
+ }
+ else if(nChannel == 1 && depth == 8 ){
+ dest.resize((unsigned int)height, (unsigned int)width);
+ //starting source address
+ unsigned char * input = (unsigned char*)src->imageData;
+ unsigned char * line;
+ unsigned char* beginOutput = (unsigned char*)dest.bitmap;
+ unsigned char* output = NULL;
+
+ for(int i=0 ; i < height ; i++)
+ {
+ line = input;
+ output = beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) + (1-lineStep) * 4 * width * i;
+ for(int j=0 ; j < width ; j++)
+ {
+ *output++ = *(line);
+ *output++ = *(line);
+ *output++ = *(line);
+ *output++ = *(line);;
+
+ line++;
+ }
+ //go to the next line
+ input+=widthStep;
+ }
+ }
+}
+
+/*!
+ \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
+ IplImage structure will be removed with OpenCV transcient from C to C++ api.
+
+ Convert an IplImage to a vpImage\<unsigned char\>.
+
+ An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
+ image structure. See http://opencvlibrary.sourceforge.net/ for general
+ OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
+ for the specific IplImage structure documentation.
+
+ \warning This function is only available if OpenCV was detected during
+ the configuration step.
+
+ \param src : Source image in OpenCV format.
+ \param dest : Destination image in ViSP format.
+ \param flip : Set to true to vertically flip the converted image.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ vpImage<unsigned char> Ig; // A grayscale image
+ IplImage* Ip;
+
+ // Read an image on a disk with openCV library
+ Ip = cvLoadImage("image.pgm", CV_LOAD_IMAGE_GRAYSCALE);
+ // Convert the grayscale IplImage into vpImage<unsigned char>
+ vpImageConvert::convert(Ip, Ig);
+
+ // ...
+
+ // Release Ip header and data
+ cvReleaseImage(&Ip);
+#endif
+}
+ \endcode
+*/
+void
+vpImageConvert::convert(const IplImage* src, vpImage<unsigned char> &dest, bool flip)
+{
+ int nChannel = src->nChannels;
+ int depth = src->depth;
+ int height = src->height;
+ int width = src->width;
+ int widthStep = src->widthStep;
+ int lineStep = (flip) ? 1 : 0;
+
+ if (flip == false)
+ {
+ if(widthStep == width){
+ if(nChannel == 1 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ memcpy(dest.bitmap, src->imageData,
+ (size_t)(height*width));
+ }
+ if(nChannel == 3 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ BGRToGrey((unsigned char*)src->imageData,dest.bitmap, (unsigned int)width, (unsigned int)height,false);
+ }
+ }
+ else{
+ if(nChannel == 1 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ for (int i =0 ; i < height ; i++){
+ memcpy(dest.bitmap+i*width, src->imageData + i*widthStep,
+ (size_t)width);
+ }
+ }
+ if(nChannel == 3 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ for (int i = 0 ; i < height ; i++){
+ BGRToGrey((unsigned char*)src->imageData + i*widthStep,
+ dest.bitmap + i*width, (unsigned int)width, 1, false);
+ }
+ }
+ }
+ }
+ else
+ {
+ if(nChannel == 1 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ unsigned char* beginOutput = (unsigned char*)dest.bitmap;
+ for (int i =0 ; i < height ; i++){
+ memcpy(beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) , src->imageData + i*widthStep,
+ (size_t)width);
+ }
+ }
+ if(nChannel == 3 && depth == 8){
+ dest.resize((unsigned int)height, (unsigned int)width) ;
+ //for (int i = 0 ; i < height ; i++){
+ BGRToGrey((unsigned char*)src->imageData /*+ i*widthStep*/,
+ dest.bitmap /*+ i*width*/, (unsigned int)width, (unsigned int)height/*1*/, true);
+ //}
+ }
+ }
+}
+
+/*!
+ \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
+ IplImage structure will be removed with OpenCV transcient from C to C++ api.
+
+ Convert a vpImage\<vpRGBa\> to a IplImage.
+
+ An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
+ image structure. See http://opencvlibrary.sourceforge.net/ for general
+ OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
+ for the specific IplImage structure documentation.
+
+ \warning This function is only available if OpenCV was detected during
+ the configuration step.
+
+ \param src : source image
+ \param dest : destination image
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ vpImage<vpRGBa> Ic; // A color image
+ IplImage* Ip = NULL;
+
+ // Read an image on a disk
+ vpImageIo::read(Ic, "image.ppm");
+ // Convert the vpImage<vpRGBa> in to color IplImage
+ vpImageConvert::convert(Ic, Ip);
+ // Treatments on IplImage
+ //...
+ // Save the IplImage on the disk
+ cvSaveImage("Ipl.ppm", Ip);
+
+ //Release Ip header and data
+ cvReleaseImage(&Ip);
+#endif
+}
+ \endcode
+*/
+void
+vpImageConvert::convert(const vpImage<vpRGBa> & src, IplImage *&dest)
+{
+ int height = (int)src.getHeight();
+ int width = (int)src.getWidth();
+ CvSize size = cvSize(width, height);
+ int depth = 8;
+ int channels = 3;
+ if (dest != NULL){
+ if(dest->nChannels != channels || dest->depth != depth
+ || dest->height != height || dest->width != width){
+ if(dest->nChannels != 0) cvReleaseImage(&dest);
+ dest = cvCreateImage( size, depth, channels );
+ }
+ }
+ else dest = cvCreateImage( size, depth, channels );
+
+
+ //starting source address
+ unsigned char * input = (unsigned char*)src.bitmap;//rgba image
+ unsigned char * line;
+ unsigned char * output = (unsigned char*)dest->imageData;//bgr image
+
+ int j=0;
+ int i=0;
+ int widthStep = dest->widthStep;
+
+ for(i=0 ; i < height ; i++)
+ {
+ output = (unsigned char*)dest->imageData + i*widthStep;
+ line = input;
+ for( j=0 ; j < width ; j++)
+ {
+ *output++ = *(line+2); //B
+ *output++ = *(line+1); //G
+ *output++ = *(line); //R
+
+ line+=4;
+ }
+ //go to the next line
+ input+=4*width;
+ }
+}
+
+/*!
+ \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
+ IplImage structure will be removed with OpenCV transcient from C to C++ api.
+
+ Convert a vpImage\<unsigned char\> to a IplImage.
+
+ An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
+ image structure. See http://opencvlibrary.sourceforge.net/ for general
+ OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
+ for the specific IplImage structure documentation.
+
+ \warning This function is only available if OpenCV was detected during
+ the configuration step.
+
+ \param src : source image
+ \param dest : destination image
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ vpImage<unsigned char> Ig; // A greyscale image
+ IplImage* Ip = NULL;
+
+ // Read an image on a disk
+ vpImageIo::read(Ig, "image.pgm");
+ // Convert the vpImage<unsigned char> in to greyscale IplImage
+ vpImageConvert::convert(Ig, Ip);
+ // Treatments on IplImage Ip
+ //...
+ // Save the IplImage on the disk
+ cvSaveImage("Ipl.pgm", Ip);
+
+ //Release Ip header and data
+ cvReleaseImage(&Ip);
+#endif
+}
+ \endcode
+*/
+void
+vpImageConvert::convert(const vpImage<unsigned char> & src, IplImage* &dest)
+{
+ unsigned int height = src.getHeight();
+ unsigned int width = src.getWidth();
+ CvSize size = cvSize((int)width, (int)height);
+ int depth = 8;
+ int channels = 1;
+ if (dest != NULL){
+ if(dest->nChannels != channels || dest->depth != depth
+ || dest->height != (int) height || dest->width != (int) width){
+ if(dest->nChannels != 0) cvReleaseImage(&dest);
+ dest = cvCreateImage( size, depth, channels );
+ }
+ }
+ else dest = cvCreateImage( size, depth, channels );
+
+ unsigned int widthStep = (unsigned int)dest->widthStep;
+
+ if ( width == widthStep){
+ memcpy(dest->imageData,src.bitmap, width*height);
+ }
+ else{
+ //copying each line taking account of the widthStep
+ for (unsigned int i =0 ; i < height ; i++){
+ memcpy(dest->imageData + i*widthStep, src.bitmap + i*width,
+ width);
+ }
+ }
+}
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+/*!
+ Convert a cv::Mat to a vpImage\<vpRGBa\>.
+
+ A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
+ the general OpenCV documentation, or
+ http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
+ for the specific Mat structure documentation.
+
+ Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
+ method, only Mat with a depth equal to 8 and a channel between 1 and 3 are
+ converted.
+
+ \warning This function is only available if OpenCV (version 2.1.0 or greater)
+ was detected during the configuration step.
+
+ \param src : Source image in OpenCV format.
+ \param dest : Destination image in ViSP format.
+ \param flip : Set to true to vertically flip the converted image.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ vpImage<vpRGBa> Ic; // A color image
+ cv::Mat Ip;
+
+ // Read an image on a disk with openCV library
+ Ip = cv::imread("image.pgm", 1);// second parameter > 0 for a RGB encoding.
+ // Convert the grayscale cv::Mat into vpImage<vpRGBa>
+ vpImageConvert::convert(Ip, Ic);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void
+vpImageConvert::convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool flip)
+{
+ if(src.type() == CV_8UC4){
+ dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
+ vpRGBa rgbaVal;
+ for(unsigned int i=0; i<dest.getRows(); ++i)
+ for(unsigned int j=0; j<dest.getCols(); ++j){
+ cv::Vec4b tmp = src.at<cv::Vec4b>((int)i, (int)j);
+ rgbaVal.R = tmp[2];
+ rgbaVal.G = tmp[1];
+ rgbaVal.B = tmp[0];
+ rgbaVal.A = tmp[3];
+ if(flip)
+ dest[dest.getRows()-i-1][j] = rgbaVal;
+ else
+ dest[i][j] = rgbaVal;
+ }
+ }else if(src.type() == CV_8UC3){
+ dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
+ vpRGBa rgbaVal;
+ rgbaVal.A = 0;
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ for(unsigned int j=0; j<dest.getCols(); ++j){
+ cv::Vec3b tmp = src.at<cv::Vec3b>((int)i, (int)j);
+ rgbaVal.R = tmp[2];
+ rgbaVal.G = tmp[1];
+ rgbaVal.B = tmp[0];
+ if(flip){
+ dest[dest.getRows()-i-1][j] = rgbaVal;
+ }else{
+ dest[i][j] = rgbaVal;
+ }
+ }
+ }
+ }else if(src.type() == CV_8UC1){
+ dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
+ vpRGBa rgbaVal;
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ for(unsigned int j=0; j<dest.getCols(); ++j){
+ rgbaVal = src.at<unsigned char>((int)i, (int)j);
+ if(flip){
+ dest[dest.getRows()-i-1][j] = rgbaVal;
+ }else{
+ dest[i][j] = rgbaVal;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Convert a cv::Mat to a vpImage\<unsigned char\>.
+
+ A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
+ the general OpenCV documentation, or
+ http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
+ for the specific Mat structure documentation.
+
+ Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
+ method, only Mat with a depth equal to 8 and a channel between 1 and 3 are
+ converted.
+
+ \warning This function is only available if OpenCV was detected during
+ the configuration step.
+
+ \param src : Source image in OpenCV format.
+ \param dest : Destination image in ViSP format.
+ \param flip : Set to true to vertically flip the converted image.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ vpImage<unsigned char> Ig; // A grayscale image
+ cv::Mat Ip;
+
+ // Read an image on a disk with openCV library
+ Ip = cv::imread("image.pgm", 0);// second parameter = 0 for a gray level.
+ // Convert the grayscale cv::Mat into vpImage<unsigned char>
+ vpImageConvert::convert(Ip, Ig);
+
+ // ...
+#endif
+}
+
+ \endcode
+*/
+void
+vpImageConvert::convert(const cv::Mat& src, vpImage<unsigned char>& dest, const bool flip)
+{
+ if(src.type() == CV_8UC1){
+ dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
+ if(src.isContinuous() && !flip){
+ memcpy(dest.bitmap, src.data, (size_t)(src.rows*src.cols));
+ }
+ else{
+ if(flip){
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ memcpy(dest.bitmap+i*dest.getCols(), src.data+(dest.getRows()-i-1)*src.step1(), (size_t)src.step);
+ }
+ }else{
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ memcpy(dest.bitmap+i*dest.getCols(), src.data+i*src.step1(), (size_t)src.step);
+ }
+ }
+ }
+ }else if(src.type() == CV_8UC3){
+ dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
+ if(src.isContinuous() && !flip){
+ BGRToGrey((unsigned char*)src.data, (unsigned char*)dest.bitmap, (unsigned int)src.cols, (unsigned int)src.rows, flip);
+ }
+ else{
+ if(flip){
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ BGRToGrey((unsigned char*)src.data+i*src.step1(),
+ (unsigned char*)dest.bitmap+(dest.getRows()-i-1)*dest.getCols(),
+ (unsigned int)dest.getCols(), 1, false);
+ }
+ }else{
+ for(unsigned int i=0; i<dest.getRows(); ++i){
+ BGRToGrey((unsigned char*)src.data+i*src.step1(),
+ (unsigned char*)dest.bitmap+i*dest.getCols(),
+ (unsigned int)dest.getCols(), 1, false);
+ }
+ }
+ }
+ }
+}
+
+
+/*!
+ Convert a vpImage\<unsigned char\> to a cv::Mat.
+
+ A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
+ the general OpenCV documentation, or
+ http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
+ for the specific Mat structure documentation.
+
+ \warning This function is only available if OpenCV version 2.1.0 or greater
+ was detected during the configuration step.
+
+ \param src : source image (vpRGBa format)
+ \param dest : destination image (BGR format)
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ vpImage<unsigned char> Ig; // A greyscale image
+ cv::Mat Ip;
+
+ // Read an image on a disk
+ vpImageIo::read(Ig, "image.pgm");
+ // Convert the vpImage<unsigned char> in to color cv::Mat.
+ vpImageConvert::convert(Ig, Ip);
+ // Treatments on cv::Mat Ip
+ //...
+ // Save the cv::Mat on the disk
+ cv::imwrite("image.pgm", Ip);
+#endif
+}
+
+ \endcode
+*/
+void
+vpImageConvert::convert(const vpImage<vpRGBa> & src, cv::Mat& dest)
+{
+ cv::Mat vpToMat((int)src.getRows(), (int)src.getCols(), CV_8UC4, (void*)src.bitmap);
+
+ dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC3);
+ cv::Mat alpha((int)src.getRows(), (int)src.getCols(), CV_8UC1);
+
+ cv::Mat out[] = {dest, alpha};
+ int from_to[] = { 0,2, 1,1, 2,0, 3,3 };
+ cv::mixChannels(&vpToMat, 1, out, 2, from_to, 4);
+}
+
+/*!
+ Convert a vpImage\<unsigned char\> to a cv::Mat.
+
+ A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
+ the general OpenCV documentation, or
+ http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
+ for the specific Mat structure documentation.
+
+ \warning This function is only available if OpenCV version 2.1.0 or greater
+ was detected during the configuration step.
+
+ \param src : source image
+ \param dest : destination image
+ \param copyData : if true, the image is copied and modification in one object
+ will not modified the other.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ vpImage<unsigned char> Ig; // A greyscale image
+ cv::Mat Ip;
+
+ // Read an image on a disk
+ vpImageIo::read(Ig, "image.pgm");
+ // Convert the vpImage<unsigned char> in to greyscale cv::Mat
+ vpImageConvert::convert(Ig, Ip);
+ // Treatments on cv::Mat Ip
+ //...
+ // Save the cv::Mat on the disk
+ cv::imwrite("image-cv.pgm", Ip);
+#endif
+}
+
+ \endcode
+*/
+void
+vpImageConvert::convert(const vpImage<unsigned char> & src, cv::Mat& dest, const bool copyData)
+{
+ if(copyData){
+ cv::Mat tmpMap((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
+ dest = tmpMap.clone();
+ }else{
+ dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
+ }
+}
+
+#endif
+#endif
+
+#ifdef VISP_HAVE_YARP
+/*!
+ Convert a vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in ViSP format.
+ \param dest : Destination image in YARP format.
+ \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ vpImage<unsigned char> I; // A mocochrome image
+ // Read an image on a disk
+ vpImageIo::read(I, "image.pgm");
+
+ yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelMono >();
+ // Convert the vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
+ vpImageConvert::convert(I, Iyarp);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const vpImage<unsigned char> & src,
+ yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData)
+{
+ if(copyData)
+ {
+ dest->resize(src.getWidth(),src.getHeight());
+ memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth());
+ }
+ else
+ dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
+}
+
+/*!
+ Convert a yarp::sig::ImageOf\<yarp::sig::PixelMono\> to a vpImage\<unsigned char\>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in YARP format.
+ \param dest : Destination image in ViSP format.
+ \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+#if defined(VISP_HAVE_YARP)
+ #include <yarp/sig/ImageFile.h>
+#endif
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelMono >();
+ // Read an image on a disk
+ yarp::sig::file::read(*Iyarp, "image.pgm");
+
+ // Convert the yarp::sig::ImageOf<yarp::sig::PixelMono> to a vpImage<unsigned char>
+ vpImage<unsigned char> I;
+ vpImageConvert::convert(Iyarp, I);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
+ vpImage<unsigned char> & dest,const bool copyData)
+{
+ dest.resize(src->height(),src->width());
+ if(copyData)
+ memcpy(dest.bitmap, src->getRawImage(), src->height()*src->width()*sizeof(yarp::sig::PixelMono));
+ else
+ dest.bitmap = src->getRawImage();
+}
+
+/*!
+ Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgba>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in ViSP format.
+ \param dest : Destination image in YARP format.
+ \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ vpImage<vpRGBa> I; // A color image
+ // Read an image on a disk
+ vpImageIo::read(I,"image.jpg");
+
+ yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgba >();
+ // Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgba>
+ vpImageConvert::convert(I,Iyarp);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const vpImage<vpRGBa> & src,
+ yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData)
+{
+ if(copyData){
+ dest->resize(src.getWidth(),src.getHeight());
+ memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth()*sizeof(vpRGBa));
+ }
+ else
+ dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
+}
+
+/*!
+ Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgba> to a vpImage\<vpRGBa\>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in YARP format.
+ \param dest : Destination image in ViSP format.
+ \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+#if defined(VISP_HAVE_YARP)
+ #include <yarp/sig/ImageFile.h>
+#endif
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgba >();
+ // Read an image on a disk
+ yarp::sig::file::read(*Iyarp,"image.pgm");
+
+ // Convert the yarp::sig::ImageOf<yarp::sig::PixelRgba> to a vpImage<vpRGBa>
+ vpImage<vpRGBa> I;
+ vpImageConvert::convert(Iyarp,I);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
+ vpImage<vpRGBa> & dest,const bool copyData)
+{
+ dest.resize(src->height(),src->width());
+ if(copyData)
+ memcpy(dest.bitmap, src->getRawImage(),src->height()*src->width()*sizeof(yarp::sig::PixelRgba));
+ else
+ dest.bitmap = (vpRGBa*)src->getRawImage();
+}
+
+/*!
+ Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgb>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in ViSP format.
+ \param dest : Destination image in YARP format.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ vpImage<vpRGBa> I; // A color image
+ // Read an image on a disk
+ vpImageIo::read(I,"image.jpg");
+
+ yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgb >();
+ // Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgb>
+ vpImageConvert::convert(I,Iyarp);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest)
+{
+ dest->resize(src.getWidth(),src.getHeight());
+ for(unsigned int i = 0 ; i < src.getRows() ; i++){
+ for(unsigned int j = 0 ; j < src.getWidth() ; j++){
+ dest->pixel(j,i).r = src[i][j].R;
+ dest->pixel(j,i).g = src[i][j].G;
+ dest->pixel(j,i).b = src[i][j].B;
+ }
+ }
+}
+
+/*!
+ Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgb> to a vpImage\<vpRGBa\>
+
+ A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
+ the YARP image class documentation.
+
+ \param src : Source image in YARP format.
+ \param dest : Destination image in ViSP format.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
+
+#if defined(VISP_HAVE_YARP)
+ #include <yarp/sig/ImageFile.h>
+#endif
+
+int main()
+{
+#if defined(VISP_HAVE_YARP)
+ yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgb >();
+ // Read an image on a disk
+ yarp::sig::file::read(*Iyarp,"image.pgm");
+
+ // Convert the yarp::sig::ImageOf<yarp::sig::PixelRgb> to a vpImage<vpRGBa>
+ vpImage<vpRGBa> I;
+ vpImageConvert::convert(Iyarp,I);
+
+ // ...
+#endif
+}
+ \endcode
+*/
+void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src, vpImage<vpRGBa> & dest)
+{
+ dest.resize(src->height(),src->width());
+ for(int i = 0 ; i < src->height() ; i++){
+ for(int j = 0 ; j < src->width() ; j++){
+ dest[i][j].R = src->pixel(j,i).r;
+ dest[i][j].G = src->pixel(j,i).g;
+ dest[i][j].B = src->pixel(j,i).b;
+ dest[i][j].A = 0;
+ }
+ }
+}
+
+#endif
+
+#define vpSAT(c) \
+ if (c & (~255)) { if (c < 0) c = 0; else c = 255; }
+/*!
+ Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32.
+ Destination rgba memory area has to be allocated before.
+
+ \sa YUV422ToRGBa()
+*/
+void vpImageConvert::YUYVToRGBa(unsigned char* yuyv, unsigned char* rgba,
+ unsigned int width, unsigned int height)
+{
+ unsigned char *s;
+ unsigned char *d;
+ int w, h, c;
+ int r, g, b, cr, cg, cb, y1, y2;
+
+ h = (int)height;
+ w = (int)width;
+ s = yuyv;
+ d = rgba;
+ while (h--) {
+ c = w >> 1;
+ while (c--) {
+ y1 = *s++;
+ cb = ((*s - 128) * 454) >> 8;
+ cg = (*s++ - 128) * 88;
+ y2 = *s++;
+ cr = ((*s - 128) * 359) >> 8;
+ cg = (cg + (*s++ - 128) * 183) >> 8;
+
+ r = y1 + cr;
+ b = y1 + cb;
+ g = y1 - cg;
+ vpSAT(r);
+ vpSAT(g);
+ vpSAT(b);
+
+ *d++ = static_cast<unsigned char>(r);
+ *d++ = static_cast<unsigned char>(g);
+ *d++ = static_cast<unsigned char>(b);
+ *d++ = 0;
+
+ r = y2 + cr;
+ b = y2 + cb;
+ g = y2 - cg;
+ vpSAT(r);
+ vpSAT(g);
+ vpSAT(b);
+
+ *d++ = static_cast<unsigned char>(r);
+ *d++ = static_cast<unsigned char>(g);
+ *d++ = static_cast<unsigned char>(b);
+ *d++ = 0;
+
+ }
+ }
+}
+/*!
+
+ Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...)
+ to RGB24. Destination rgb memory area has to be allocated before.
+
+ \sa YUV422ToRGB()
+*/
+void vpImageConvert::YUYVToRGB(unsigned char* yuyv, unsigned char* rgb,
+ unsigned int width, unsigned int height)
+{
+ unsigned char *s;
+ unsigned char *d;
+ int h, w, c;
+ int r, g, b, cr, cg, cb, y1, y2;
+
+ h = (int)height;
+ w = (int)width;
+ s = yuyv;
+ d = rgb;
+ while (h--) {
+ c = w >> 1;
+ while (c--) {
+ y1 = *s++;
+ cb = ((*s - 128) * 454) >> 8;
+ cg = (*s++ - 128) * 88;
+ y2 = *s++;
+ cr = ((*s - 128) * 359) >> 8;
+ cg = (cg + (*s++ - 128) * 183) >> 8;
+
+ r = y1 + cr;
+ b = y1 + cb;
+ g = y1 - cg;
+ vpSAT(r);
+ vpSAT(g);
+ vpSAT(b);
+
+ *d++ = static_cast<unsigned char>(r);
+ *d++ = static_cast<unsigned char>(g);
+ *d++ = static_cast<unsigned char>(b);
+
+ r = y2 + cr;
+ b = y2 + cb;
+ g = y2 - cg;
+ vpSAT(r);
+ vpSAT(g);
+ vpSAT(b);
+
+ *d++ = static_cast<unsigned char>(r);
+ *d++ = static_cast<unsigned char>(g);
+ *d++ = static_cast<unsigned char>(b);
+ }
+ }
+}
+/*!
+
+ Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...)
+ to grey. Destination rgb memory area has to be allocated before.
+
+ \sa YUV422ToGrey()
+*/
+void vpImageConvert::YUYVToGrey(unsigned char* yuyv, unsigned char* grey, unsigned int size)
+{
+ unsigned int i=0,j=0;
+
+ while( j < size*2)
+ {
+ grey[i++] = yuyv[j];
+ grey[i++] = yuyv[j+2];
+ j+=4;
+ }
+}
+
+
+/*!
+
+Convert YUV411 into RGB32
+yuv411 : u y1 y2 v y3 y4
+
+*/
+void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
+{
+#if 1
+ // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ for(unsigned int i = size / 4; i; i--) {
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ Y2 = *yuv++;
+ Y3 = *yuv++;
+ UV = - U - V;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+ }
+#else
+ // tres tres lent ....
+ unsigned int i=0,j=0;
+ unsigned char r, g, b;
+ while( j < numpixels*3/2)
+ {
+
+ YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+
+ YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+
+ YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+
+ YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+
+ j+=6;
+ }
+#endif
+
+}
+
+/*!
+ Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB32 images.
+ Destination rgba memory area has to be allocated before.
+
+ \sa YUYVToRGBa()
+*/
+void vpImageConvert::YUV422ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
+{
+
+#if 1
+ // std::cout << "call optimized convertYUV422ToRGBa()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1;
+ for( unsigned int i = size / 2; i; i-- ) {
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y0 = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ Y1 = *yuv++;
+ UV = - U - V;
+
+ //---
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ rgba++;
+ }
+
+#else
+ // tres tres lent ....
+ unsigned int i=0,j=0;
+ unsigned char r, g, b;
+
+ while( j < size*2)
+ {
+
+ YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+
+ YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
+ rgba[i] = r;
+ rgba[i+1] = g;
+ rgba[i+2] = b;
+ rgba[i+3] = 0;
+ i+=4;
+ j+=4;
+
+ }
+#endif
+}
+
+/*!
+
+Convert YUV411 into Grey
+yuv411 : u y1 y2 v y3 y4
+
+*/
+void vpImageConvert::YUV411ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
+{
+ unsigned int i=0,j=0;
+ while( j < size*3/2)
+ {
+
+ grey[i ] = yuv[j+1];
+ grey[i+1] = yuv[j+2];
+ grey[i+2] = yuv[j+4];
+ grey[i+3] = yuv[j+5];
+
+ i+=4;
+
+ j+=6;
+ }
+}
+
+/*!
+
+ Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB images.
+ Destination rgb memory area has to be allocated before.
+
+ \sa YUYVToRGB()
+
+*/
+void vpImageConvert::YUV422ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
+{
+#if 1
+ // std::cout << "call optimized convertYUV422ToRGB()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1;
+ for( unsigned int i = size / 2; i; i-- ) {
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y0 = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ Y1 = *yuv++;
+ UV = - U - V;
+
+ //---
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ }
+
+#else
+ // tres tres lent ....
+ unsigned int i=0,j=0;
+ unsigned char r, g, b;
+
+ while( j < size*2)
+ {
+
+ YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+
+ YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+ j+=4;
+
+ }
+#endif
+}
+
+/*!
+
+ Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into Grey.
+ Destination grey memory area has to be allocated before.
+
+ \sa YUYVToGrey()
+
+*/
+void vpImageConvert::YUV422ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
+{
+ unsigned int i=0,j=0;
+
+ while( j < size*2)
+ {
+ grey[i++] = yuv[j+1];
+ grey[i++] = yuv[j+3];
+ j+=4;
+ }
+}
+
+/*!
+
+Convert YUV411 into RGB
+yuv411 : u y1 y2 v y3 y4
+
+*/
+void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
+{
+#if 1
+ // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ for(unsigned int i = size / 4; i; i--) {
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ Y2 = *yuv++;
+ Y3 = *yuv++;
+ UV = - U - V;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+ }
+#else
+ // tres tres lent ....
+
+ unsigned int i=0,j=0;
+ unsigned char r, g, b;
+
+ while( j < size*3/2)
+ {
+ YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+
+ YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+
+ YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+
+ YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
+ rgb[i] = r;
+ rgb[i+1] = g;
+ rgb[i+2] = b;
+ i+=3;
+ //TRACE("r= %d g=%d b=%d", r, g, b);
+
+ j+=6;
+ }
+#endif
+
+}
+
+
+
+/*!
+
+ Convert YUV420 into RGBa
+ yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
+
+*/
+void vpImageConvert::YUV420ToRGBa(unsigned char* yuv, unsigned char* rgba,
+ unsigned int width, unsigned int height)
+{
+ // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ unsigned int size = width*height;
+ unsigned char* iU = yuv + size;
+ unsigned char* iV = yuv + 5*size/4;
+ for(unsigned int i = 0; i<height/2; i++)
+ {
+ for(unsigned int j = 0; j < width/2 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv+width-1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv-width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba + 4*width-7;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba -4*width+1;
+ }
+ yuv+=width;
+ rgba+=4*width;
+ }
+}
+/*!
+
+ Convert YUV420 into RGB
+ yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
+
+*/
+void vpImageConvert::YUV420ToRGB(unsigned char* yuv,
+ unsigned char* rgb,
+ unsigned int width, unsigned int height)
+{
+ // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ unsigned int size = width*height;
+ unsigned char* iU = yuv + size;
+ unsigned char* iV = yuv + 5*size/4;
+ for(unsigned int i = 0; i<height/2; i++)
+ {
+ for(unsigned int j = 0; j < width/2 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv+width-1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv-width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb + 3*width-5;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb -3*width+1;
+ }
+ yuv+=width;
+ rgb+=3*width;
+ }
+}
+
+/*!
+
+ Convert YUV420 into Grey
+ yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
+
+*/
+void vpImageConvert::YUV420ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
+{
+ for(unsigned int i=0 ; i < size ; i++)
+ {
+ *grey++ = *yuv++;
+ }
+
+}
+/*!
+
+ Convert YUV444 into RGBa
+ yuv444 : u y v
+
+*/
+void vpImageConvert::YUV444ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
+{
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y;
+ for(unsigned int i = 0; i<size; i++)
+ {
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+ }
+}
+/*!
+
+ Convert YUV444 into RGB
+ yuv444 : u y v
+
+*/
+void vpImageConvert::YUV444ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
+{
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y;
+ for(unsigned int i = 0; i<size; i++)
+ {
+
+ U = (int)((*yuv++ - 128) * 0.354);
+ U5 = 5*U;
+ Y = *yuv++;
+ V = (int)((*yuv++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+ }
+}
+
+/*!
+
+ Convert YUV444 into Grey
+ yuv444 : u y v
+
+*/
+void vpImageConvert::YUV444ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
+{
+ yuv++;
+ for(unsigned int i=0 ; i < size ; i++)
+ {
+ *grey++ = *yuv;
+ yuv = yuv + 3;
+ }
+}
+
+/*!
+
+ Convert YV12 into RGBa
+ yuv420 : Y(NxM), V(N/2xM/2), U(N/2xM/2)
+
+*/
+void vpImageConvert::YV12ToRGBa(unsigned char* yuv, unsigned char* rgba,
+ unsigned int width, unsigned int height)
+{
+ // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ unsigned int size = width*height;
+ unsigned char* iV = yuv + size;
+ unsigned char* iU = yuv + 5*size/4;
+ for(unsigned int i = 0; i<height/2; i++)
+ {
+ for(unsigned int j = 0; j < width/2 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv+width-1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv-width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba + 4*width-7;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba -4*width+1;
+ }
+ yuv+=width;
+ rgba+=4*width;
+ }
+}
+/*!
+
+ Convert YV12 into RGB
+ yuv420 : Y(NxM), V(N/2xM/2), U(N/2xM/2)
+
+*/
+void vpImageConvert::YV12ToRGB(unsigned char* yuv, unsigned char* rgb,
+ unsigned int height, unsigned int width)
+{
+ // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3;
+ unsigned int size = width*height;
+ unsigned char* iV = yuv + size;
+ unsigned char* iU = yuv + 5*size/4;
+ for(unsigned int i = 0; i<height/2; i++)
+ {
+ for(unsigned int j = 0; j < width/2 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv+width-1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv-width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb + 3*width-5;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb -3*width+1;
+ }
+ yuv+=width;
+ rgb+=3*width;
+ }
+}
+
+/*!
+
+ Convert YVU9 into RGBa
+ yuv420 : Y(NxM), V(N/4xM/4), U(N/4xM/4)
+
+*/
+void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
+ unsigned int width, unsigned int height)
+{
+ // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3,Y4, Y5, Y6, Y7,Y8, Y9, Y10, Y11,Y12, Y13, Y14, Y15;
+ unsigned int size = width*height;
+ unsigned char* iV = yuv + size;
+ unsigned char* iU = yuv + 17*size/16;
+ for(unsigned int i = 0; i<height/4; i++)
+ {
+ for(unsigned int j = 0; j < width/4 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv+width-3;
+ Y4 = *yuv++;
+ Y5 = *yuv++;
+ Y6 = *yuv++;
+ Y7 = *yuv;
+ yuv = yuv+width-3;
+ Y8 = *yuv++;
+ Y9 = *yuv++;
+ Y10 = *yuv++;
+ Y11 = *yuv;
+ yuv = yuv+width-3;
+ Y12 = *yuv++;
+ Y13 = *yuv++;
+ Y14 = *yuv++;
+ Y15 = *yuv;
+ yuv = yuv-3*width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba + 4*width-15;
+
+ R = Y4 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y4 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y4 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y5 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y5 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y5 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y6 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y6 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y6 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y7 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y7 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y7 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba + 4*width-15;
+
+ R = Y8 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y8 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y8 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y9 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y9 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y9 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y10 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y10 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y10 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y11 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y11 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y11 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba + 4*width-15;
+
+ R = Y12 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y12 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y12 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y13 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y13 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y13 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y14 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y14 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y14 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba++ = 0;
+
+ //---
+ R = Y15 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y15 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y15 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgba++ = (unsigned char)R;
+ *rgba++ = (unsigned char)G;
+ *rgba++ = (unsigned char)B;
+ *rgba = 0;
+ rgba = rgba -12*width+1;
+ }
+ yuv+=3*width;
+ rgba+=12*width;
+ }
+}
+/*!
+
+ Convert YV12 into RGB
+ yuv420 : Y(NxM), V(N/4xM/4), U(N/4xM/4)
+
+*/
+void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
+ unsigned int height, unsigned int width)
+{
+ // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl;
+ register int U, V, R, G, B, V2, U5, UV;
+ register int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15;
+ unsigned int size = width*height;
+ unsigned char* iV = yuv + size;
+ unsigned char* iU = yuv + 17*size/16;
+ for(unsigned int i = 0; i<height/4; i++)
+ {
+ for(unsigned int j = 0; j < width/4 ; j++)
+ {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5*U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2*V;
+ UV = - U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv+width-3;
+ Y4 = *yuv++;
+ Y5 = *yuv++;
+ Y6 = *yuv++;
+ Y7 = *yuv;
+ yuv = yuv+width-3;
+ Y8 = *yuv++;
+ Y9 = *yuv++;
+ Y10 = *yuv++;
+ Y11 = *yuv;
+ yuv = yuv+width-3;
+ Y12 = *yuv++;
+ Y13 = *yuv++;
+ Y14 = *yuv++;
+ Y15 = *yuv;
+ yuv = yuv-3*width+1;
+
+ // Original equations
+ // R = Y + 1.402 V
+ // G = Y - 0.344 U - 0.714 V
+ // B = Y + 1.772 U
+ R = Y0 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y0 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y0 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y1 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y1 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y1 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y2 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y2 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y2 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y3 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y3 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y3 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb + 3*width-11;
+
+ R = Y4 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y4 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y4 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y5 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y5 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y5 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y6 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y6 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y6 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y7 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y7 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y7 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb + 3*width-11;
+
+ R = Y8 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y8 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y8 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y9 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y9 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y9 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y10 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y10 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y10 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y11 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y11 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y11 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb = (unsigned char)B;
+ rgb = rgb + 3*width-11;
+
+ R = Y12 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y12 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y12 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y13 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y13 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y13 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y14 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y14 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y14 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+
+ //---
+ R = Y15 + V2;
+ if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+
+ G = Y15 + UV;
+ if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+
+ B = Y15 + U5;
+ if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+
+ *rgb++ = (unsigned char)R;
+ *rgb++ = (unsigned char)G;
+ *rgb++ = (unsigned char)B;
+ rgb = rgb -9*width+1;
+ }
+ yuv+=3*width;
+ rgb+=9*width;
+ }
+}
+
+/*!
+
+ Convert RGB into RGBa
+
+*/
+void vpImageConvert::RGBToRGBa(unsigned char* rgb, unsigned char* rgba, unsigned int size)
+{
+ unsigned char *pt_input = rgb;
+ unsigned char *pt_end = rgb + 3*size;
+ unsigned char *pt_output = rgba;
+
+ while(pt_input != pt_end) {
+ *(pt_output++) = *(pt_input++) ; // R
+ *(pt_output++) = *(pt_input++) ; // G
+ *(pt_output++) = *(pt_input++) ; // B
+ *(pt_output++) = 0 ; // A
+ }
+}
+
+/*!
+
+ Convert RGB into RGBa
+
+*/
+void vpImageConvert::RGBaToRGB(unsigned char* rgba, unsigned char* rgb, unsigned int size)
+{
+ unsigned char *pt_input = rgba;
+ unsigned char *pt_end = rgba + 4*size;
+ unsigned char *pt_output = rgb;
+
+ while(pt_input != pt_end) {
+ *(pt_output++) = *(pt_input++) ; // R
+ *(pt_output++) = *(pt_input++) ; // G
+ *(pt_output++) = *(pt_input++) ; // B
+ pt_input++ ;
+ }
+}
+/*!
+ Weights convert from linear RGB to CIE luminance assuming a
+ modern monitor. See Charles Pontyon's Colour FAQ
+ http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html
+
+*/
+void vpImageConvert::RGBToGrey(unsigned char* rgb, unsigned char* grey, unsigned int size)
+{
+ unsigned char *pt_input = rgb;
+ unsigned char* pt_end = rgb + size*3;
+ unsigned char *pt_output = grey;
+ while(pt_input != pt_end) {
+ *pt_output = (unsigned char) (0.2126 * (*pt_input)
+ + 0.7152 * (*(pt_input + 1))
+ + 0.0722 * (*(pt_input + 2)) );
+ pt_input += 3;
+ pt_output ++;
+ }
+}
+/*!
+
+ Weights convert from linear RGBa to CIE luminance assuming a
+ modern monitor. See Charles Pontyon's Colour FAQ
+ http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html
+
+*/
+void vpImageConvert::RGBaToGrey(unsigned char* rgba, unsigned char* grey, unsigned int size)
+{
+ unsigned char *pt_input = rgba;
+ unsigned char* pt_end = rgba + size*4;
+ unsigned char *pt_output = grey;
+
+ while(pt_input != pt_end) {
+ *pt_output = (unsigned char) (0.2126 * (*pt_input)
+ + 0.7152 * (*(pt_input + 1))
+ + 0.0722 * (*(pt_input + 2)) );
+ pt_input += 4;
+ pt_output ++;
+ }
+}
+
+/*!
+ Convert from grey to linear RGBa.
+
+*/
+void
+vpImageConvert::GreyToRGBa(unsigned char* grey, unsigned char* rgba, unsigned int size)
+{
+ unsigned char *pt_input = grey;
+ unsigned char *pt_end = grey + size;
+ unsigned char *pt_output = rgba;
+
+ while(pt_input != pt_end) {
+ unsigned char p = *pt_input ;
+ *(pt_output ) = p ; // R
+ *(pt_output + 1) = p ; // G
+ *(pt_output + 2) = p ; // B
+ *(pt_output + 3) = p ; // A
+
+ pt_input ++;
+ pt_output += 4;
+ }
+}
+
+/*!
+ Convert from grey to linear RGBa.
+
+*/
+void
+vpImageConvert::GreyToRGB(unsigned char* grey, unsigned char* rgb, unsigned int size)
+{
+ unsigned char *pt_input = grey;
+ unsigned char* pt_end = grey + size;
+ unsigned char *pt_output = rgb;
+
+ while(pt_input != pt_end) {
+ unsigned char p = *pt_input ;
+ *(pt_output ) = p ; // R
+ *(pt_output + 1) = p ; // G
+ *(pt_output + 2) = p ; // B
+
+ pt_input ++;
+ pt_output += 3;
+ }
+}
+
+
+/*!
+ Converts a BGR image to RGBa
+ Flips the image verticaly if needed
+ assumes that rgba is already resized
+*/
+void
+vpImageConvert::BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
+ unsigned int width, unsigned int height, bool flip)
+{
+ //if we have to flip the image, we start from the end last scanline so the
+ //step is negative
+ int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+
+ //starting source address = last line if we need to flip the image
+ unsigned char * src = (flip) ? (bgr+(width*height*3)+lineStep) : bgr;
+ unsigned char * line;
+
+ unsigned int j=0;
+ unsigned int i=0;
+
+ for(i=0 ; i < height ; i++)
+ {
+ line = src;
+ for( j=0 ; j < width ; j++)
+ {
+ *rgba++ = *(line+2);
+ *rgba++ = *(line+1);
+ *rgba++ = *(line+0);
+ *rgba++ = 0;
+
+ line+=3;
+ }
+ //go to the next line
+ src+=lineStep;
+ }
+
+}
+
+/*!
+ Converts a BGR image to greyscale
+ Flips the image verticaly if needed
+ assumes that grey is already resized
+*/
+void
+vpImageConvert::BGRToGrey(unsigned char * bgr, unsigned char * grey,
+ unsigned int width, unsigned int height, bool flip)
+{
+ //if we have to flip the image, we start from the end last scanline so the
+ //step is negative
+ int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+
+ //starting source address = last line if we need to flip the image
+ unsigned char * src = (flip) ? bgr+(width*height*3)+lineStep : bgr;
+ unsigned char * line;
+
+ unsigned int j=0;
+ unsigned int i=0;
+
+ for(i=0 ; i < height ; i++)
+ {
+ line = src;
+ for( j=0 ; j < width ; j++)
+ {
+ *grey++ = (unsigned char)( 0.2126 * *(line+2)
+ + 0.7152 * *(line+1)
+ + 0.0722 * *(line+0)) ;
+ line+=3;
+ }
+
+ //go to the next line
+ src+=lineStep;
+ }
+}
+/*!
+ Converts a RGB image to RGBa
+ Flips the image verticaly if needed
+ assumes that rgba is already resized
+*/
+void
+vpImageConvert::RGBToRGBa(unsigned char * rgb, unsigned char * rgba,
+ unsigned int width, unsigned int height, bool flip)
+{
+ //if we have to flip the image, we start from the end last scanline so the
+ //step is negative
+ int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+
+ //starting source address = last line if we need to flip the image
+ unsigned char * src = (flip) ? (rgb+(width*height*3)+lineStep) : rgb;
+ unsigned char * line;
+
+ unsigned int j=0;
+ unsigned int i=0;
+
+ for(i=0 ; i < height ; i++)
+ {
+ line = src;
+ for( j=0 ; j < width ; j++)
+ {
+ *rgba++ = *(line++);
+ *rgba++ = *(line++);
+ *rgba++ = *(line++);
+ *rgba++ = 0;
+ }
+ //go to the next line
+ src+=lineStep;
+ }
+}
+
+/*!
+ Converts a RGB image to greyscale
+ Flips the image verticaly if needed
+ assumes that grey is already resized
+*/
+void
+vpImageConvert::RGBToGrey(unsigned char * rgb, unsigned char * grey,
+ unsigned int width, unsigned int height, bool flip)
+{
+ //if we have to flip the image, we start from the end last scanline so the
+ //step is negative
+ int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+
+ //starting source address = last line if we need to flip the image
+ unsigned char * src = (flip) ? rgb+(width*height*3)+lineStep : rgb;
+ unsigned char * line;
+
+ unsigned int j=0;
+ unsigned int i=0;
+
+ unsigned r,g,b;
+
+ for(i=0 ; i < height ; i++)
+ {
+ line = src;
+ for( j=0 ; j < width ; j++)
+ {
+ r = *(line++);
+ g = *(line++);
+ b = *(line++);
+ *grey++ = (unsigned char)( 0.2126 * r + 0.7152 * g + 0.0722 * b) ;
+ }
+
+ //go to the next line
+ src+=lineStep;
+ }
+}
+
+/*!
+
+ Compute the look up table useful for YCbCr conversions.
+
+*/
+void vpImageConvert::computeYCbCrLUT()
+{
+ if (YCbCrLUTcomputed == false) {
+ int index = 256, aux;
+
+ while (index-- ) {
+
+ aux = index - 128;
+ vpImageConvert::vpCrr[index] = (int)( 364.6610 * aux) >> 8;
+ vpImageConvert::vpCgb[index] = (int)( -89.8779 * aux) >> 8;
+ vpImageConvert::vpCgr[index] = (int)(-185.8154 * aux) >> 8;
+ vpImageConvert::vpCbb[index] = (int)( 460.5724 * aux) >> 8;
+ }
+
+ YCbCrLUTcomputed = true;
+ }
+}
+
+
+/*!
+
+ Convert an image from YCbCr 4:2:2 (Y0 Cb01 Y1 Cr01 Y2 Cb23 Y3 ...) to RGB
+ format. Destination rgb memory area has to be allocated before.
+
+ - In YCbCr (4:2:2) format each pixel is coded using 16 bytes.
+ Byte 0: YO (Luma for Pixel 0)
+ Byte 1: Chroma Blue Cb (Blue Chroma for Pixel 0 and 1)
+ Byte 2: Y1 (Luma for Pixel 1)
+ Byte 3: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
+ Byte 4: Y2 (Luma for Pixel 2)
+
+ - In RGB format, each pixel is coded using 24 bytes.
+ Byte 0: Red
+ Byte 1: Green
+ Byte 2: Blue
+
+
+*/
+void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
+{
+ unsigned char *cbv;
+ unsigned char *crv;
+ unsigned char *pt_ycbcr = ycbcr;
+ unsigned char *pt_rgb = rgb;
+ cbv = pt_ycbcr + 1;
+ crv = pt_ycbcr + 3;
+
+ vpImageConvert::computeYCbCrLUT();
+
+ int col = 0;
+
+ while (size--) {
+ register int val_r, val_g, val_b;
+ if (!(col++ % 2)) {
+ cbv = pt_ycbcr + 1;
+ crv = pt_ycbcr + 3;
+ }
+
+ val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
+ val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
+ val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
+
+ vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
+
+ *pt_rgb++ = (val_r < 0) ? 0u :
+ ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
+ *pt_rgb++ = (val_g < 0) ? 0u :
+ ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
+ *pt_rgb++ = (val_b < 0) ? 0u :
+ ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+
+ pt_ycbcr += 2;
+ }
+}
+
+/*!
+
+ Convert an image from YCbCr 4:2:2 (Y0 Cb01 Y1 Cr01 Y2 Cb23 Y3...) to
+ RGBa format. Destination rgba memory area has to be allocated
+ before.
+
+ - In YCbCr (4:2:2) format each pixel is coded using 16 bytes.
+ Byte 0: YO (Luma for Pixel 0)
+ Byte 1: Chroma Blue Cb (Blue Chroma for Pixel 0 and 1)
+ Byte 2: Y1 (Luma for Pixel 1)
+ Byte 3: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
+ Byte 4: Y2 (Luma for Pixel 2)
+
+ - In RGBa format, each pixel is coded using 24 bytes.
+ Byte 0: Red
+ Byte 1: Green
+ Byte 2: Blue
+ Byte 3: -
+
+
+*/
+void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsigned int size)
+{
+ unsigned char *cbv;
+ unsigned char *crv;
+ unsigned char *pt_ycbcr = ycbcr;
+ unsigned char *pt_rgba = rgba;
+ cbv = pt_ycbcr + 1;
+ crv = pt_ycbcr + 3;
+
+ vpImageConvert::computeYCbCrLUT();
+
+ int col = 0;
+
+ while (size--) {
+ register int val_r, val_g, val_b;
+ if (!(col++ % 2)) {
+ cbv = pt_ycbcr + 1;
+ crv = pt_ycbcr + 3;
+ }
+
+ val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
+ val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
+ val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
+
+ vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
+
+ *pt_rgba++ = (val_r < 0) ? 0u :
+ ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
+ *pt_rgba++ = (val_g < 0) ? 0u :
+ ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
+ *pt_rgba++ = (val_b < 0) ? 0u :
+ ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgba++ = 0;
+
+ pt_ycbcr += 2;
+ }
+}
+
+
+/*!
+
+ Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to grey
+ format. Destination grey image memory area has to be allocated
+ before.
+
+ - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
+ Byte 0: YO (Luma for Pixel 0)
+ Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
+ Byte 2: Y1 (Luma for Pixel 1)
+ Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
+ Byte 4: Y2 (Luma for Pixel 2)
+
+ - In grey format, each pixel is coded using 8 bytes.
+
+*/
+void vpImageConvert::YCbCrToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
+{
+ unsigned int i=0,j=0;
+
+ while( j < size*2)
+ {
+ grey[i++] = yuv[j];
+ grey[i++] = yuv[j+2];
+ j+=4;
+ }
+}
+
+/*!
+
+ Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to RGB
+ format. Destination rgb memory area has to be allocated before.
+
+ - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
+ Byte 0: YO (Luma for Pixel 0)
+ Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
+ Byte 2: Y1 (Luma for Pixel 1)
+ Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
+ Byte 4: Y2 (Luma for Pixel 2)
+
+ - In RGB format, each pixel is coded using 24 bytes.
+ Byte 0: Red
+ Byte 1: Green
+ Byte 2: Blue
+
+*/
+void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsigned int size)
+{
+ unsigned char *cbv;
+ unsigned char *crv;
+ unsigned char *pt_ycbcr = ycrcb;
+ unsigned char *pt_rgb = rgb;
+ crv = pt_ycbcr + 1;
+ cbv = pt_ycbcr + 3;
+
+ vpImageConvert::computeYCbCrLUT();
+
+ int col = 0;
+
+ while (size--) {
+ register int val_r, val_g, val_b;
+ if (!(col++ % 2)) {
+ crv = pt_ycbcr + 1;
+ cbv = pt_ycbcr + 3;
+ }
+
+ val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
+ val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
+ val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
+
+ vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
+
+ *pt_rgb++ = (val_r < 0) ? 0u :
+ ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
+ *pt_rgb++ = (val_g < 0) ? 0u :
+ ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
+ *pt_rgb++ = (val_b < 0) ? 0u :
+ ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+
+ pt_ycbcr += 2;
+ }
+}
+/*!
+
+ Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to RGBa
+ format. Destination rgba memory area has to be allocated before.
+
+ - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
+ Byte 0: YO (Luma for Pixel 0)
+ Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
+ Byte 2: Y1 (Luma for Pixel 1)
+ Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
+ Byte 4: Y2 (Luma for Pixel 2)
+
+ - In RGBa format, each pixel is coded using 24 bytes.
+ Byte 0: Red
+ Byte 1: Green
+ Byte 2: Blue
+ Byte 3: -
+
+
+*/
+void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsigned int size)
+{
+ unsigned char *cbv;
+ unsigned char *crv;
+ unsigned char *pt_ycbcr = ycrcb;
+ unsigned char *pt_rgba = rgba;
+ crv = pt_ycbcr + 1;
+ cbv = pt_ycbcr + 3;
+
+ vpImageConvert::computeYCbCrLUT();
+
+ int col = 0;
+
+ while (size--) {
+ register int val_r, val_g, val_b;
+ if (!(col++ % 2)) {
+ crv = pt_ycbcr + 1;
+ cbv = pt_ycbcr + 3;
+ }
+
+ val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
+ val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
+ val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
+
+ vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
+
+ *pt_rgba++ = (val_r < 0) ? 0u :
+ ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
+ *pt_rgba++ = (val_g < 0) ? 0u :
+ ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
+ *pt_rgba++ = (val_b < 0) ? 0u :
+ ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgba++ = 0;
+
+ pt_ycbcr += 2;
+ }
+}
+
+/*!
+
+ Split an image from vpRGBa format to monochrome channels.
+ \param src : source image.
+ \param pR : red channel. Set as NULL if not needed.
+ \param pG : green channel. Set as NULL if not needed.
+ \param pB : blue channel. Set as NULL if not needed.
+ \param pa : alpha channel. Set as NULL if not needed.
+
+ Example code using split :
+
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+int main()
+{
+ vpImage<vpRGBa> Ic; // A color image
+
+ // Load a color image from the disk
+ vpImageIo::read(Ic,"image.ppm");
+
+ // Only R and B Channels are desired.
+ vpImage<unsigned char> R, B;
+
+ // Split Ic color image
+ // R and B will be resized in split function if needed
+ vpImageConvert::split(Ic, &R, NULL, &B, NULL);
+
+ // Save the the R Channel.
+ vpImageIo::write(R, "RChannel.pgm");
+}
+ \endcode
+*/
+void vpImageConvert::split(const vpImage<vpRGBa> &src,
+ vpImage<unsigned char>* pR,
+ vpImage<unsigned char>* pG,
+ vpImage<unsigned char>* pB,
+ vpImage<unsigned char>* pa)
+{
+ register size_t n = src.getNumberOfPixel();
+ unsigned int height = src.getHeight();
+ unsigned int width = src.getWidth();
+ unsigned char* input;
+ unsigned char* dst ;
+
+ vpImage<unsigned char>* tabChannel[4];
+
+/* incrsrc[0] = 0; //init
+ incrsrc[1] = 0; //step after the first used channel
+ incrsrc[2] = 0; //step after the second used channel
+ incrsrc[3] = 0;
+ incrsrc[4] = 0;
+ */
+ tabChannel[0] = pR;
+ tabChannel[1] = pG;
+ tabChannel[2] = pB;
+ tabChannel[3] = pa;
+
+ register size_t i; /* ordre */
+ for(unsigned int j = 0;j < 4;j++){
+ if(tabChannel[j]!=NULL){
+ if(tabChannel[j]->getHeight() != height ||
+ tabChannel[j]->getWidth() != width){
+ tabChannel[j]->resize(height,width);
+ }
+ dst = (unsigned char*)tabChannel[j]->bitmap;
+
+ input = (unsigned char*)src.bitmap+j;
+ i = 0;
+#if 1 //optimization
+ if (n >= 4) { /* boucle deroulee lsize fois */
+ n -= 3;
+ for (; i < n; i += 4) {
+ *dst = *input; input += 4; dst++;
+ *dst = *input; input += 4; dst++;
+ *dst = *input; input += 4; dst++;
+ *dst = *input; input += 4; dst++;
+ }
+ n += 3;
+ }
+#endif
+ for (; i < n; i++) {
+ *dst = *input; input += 4; dst ++;
+ }
+ }
+ }
+}
+
+/*!
+ Merge 4 channels into an RGBa image.
+ \param R : Red channel.
+ \param G : Green channel.
+ \param B : Blue channel.
+ \param a : Alpha channel.
+ \param RGBa : Destination RGBa image.
+*/
+void vpImageConvert::merge(const vpImage<unsigned char> *R,
+ const vpImage<unsigned char> *G,
+ const vpImage<unsigned char> *B,
+ const vpImage<unsigned char> *a,
+ vpImage<vpRGBa> &RGBa) {
+ //Check if the input channels have all the same dimensions
+ std::map<unsigned int, unsigned int> mapOfWidths, mapOfHeights;
+ if(R != NULL) {
+ mapOfWidths[R->getWidth()]++;
+ mapOfHeights[R->getHeight()]++;
+ }
+
+ if(G != NULL) {
+ mapOfWidths[G->getWidth()]++;
+ mapOfHeights[G->getHeight()]++;
+ }
+
+ if(B != NULL) {
+ mapOfWidths[B->getWidth()]++;
+ mapOfHeights[B->getHeight()]++;
+ }
+
+ if(a != NULL) {
+ mapOfWidths[a->getWidth()]++;
+ mapOfHeights[a->getHeight()]++;
+ }
+
+ if(mapOfWidths.size() == 1 && mapOfHeights.size() == 1) {
+ unsigned int width = mapOfWidths.begin()->first;
+ unsigned int height = mapOfHeights.begin()->first;
+
+ RGBa.resize(height, width);
+
+ unsigned int size = width*height;
+ for(unsigned int i = 0; i < size; i++) {
+ if(R != NULL) {
+ RGBa.bitmap[i].R = R->bitmap[i];
+ }
+
+ if(G != NULL) {
+ RGBa.bitmap[i].G = G->bitmap[i];
+ }
+
+ if(B != NULL) {
+ RGBa.bitmap[i].B = B->bitmap[i];
+ }
+
+ if(a != NULL) {
+ RGBa.bitmap[i].A = a->bitmap[i];
+ }
+ }
+ } else {
+ throw vpException(vpException::dimensionError, "Mismatch dimensions !");
+ }
+}
+
+/*!
+
+ Converts a MONO16 grey scale image (each pixel is coded by two bytes) into a
+ grey image where each pixels are coded on one byte.
+
+ \param grey16 : Input image to convert (two bytes per pixel).
+ \param grey : Output image (one byte per pixel)
+ \param size : The image size or the number of pixels.
+
+*/
+void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size)
+{
+ register int i = (((int)size)<<1)-1;
+ register int j = (int)size-1;
+ register int y;
+
+ while (i >= 0) {
+ y = grey16[i--];
+ grey[j--] = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
+ }
+}
+
+/*!
+
+ Converts a MONO16 grey scale image (each pixel is coded by two bytes) into a
+ grey image where each pixels are coded on one byte.
+
+ \param grey16 : Input image to convert (two bytes per pixel).
+ \param rgba : Output image.
+ \param size : The image size or the number of pixels.
+
+*/
+void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size)
+{
+ register int i = (((int)size)<<1)-1;
+ register int j = (int)(size*4-1);
+ register int y;
+ register unsigned char v;
+
+ while (i >= 0) {
+ y = grey16[i--];
+ v = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
+ rgba[j--] = 0;
+ rgba[j--] = v;
+ rgba[j--] = v;
+ rgba[j--] = v;
+ }
+}
+
+void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, const double *value_, unsigned char *rgb,
+ const unsigned int size, const unsigned int step) {
+ for(unsigned int i = 0; i < size; i++) {
+ double h, s, v;
+ double f, p, q, t;
+ double hue = hue_[i], saturation = saturation_[i], value = value_[i];
+
+ if (vpMath::equal(saturation, 0.0, std::numeric_limits<double>::epsilon())) {
+ hue = value;
+ saturation = value;
+ } else {
+ h = hue * 6.0;
+ s = saturation;
+ v = value;
+
+ if (vpMath::equal(h, 6.0, std::numeric_limits<double>::epsilon())) {
+ h = 0.0;
+ }
+
+ f = h - (int) h;
+ p = v * (1.0 - s);
+ q = v * (1.0 - s * f);
+ t = v * (1.0 - s * (1.0 - f));
+
+ switch ((int) h) {
+ case 0:
+ hue = v;
+ saturation = t;
+ value = p;
+ break;
+
+ case 1:
+ hue = q;
+ saturation = v;
+ value = p;
+ break;
+
+ case 2:
+ hue = p;
+ saturation = v;
+ value = t;
+ break;
+
+ case 3:
+ hue = p;
+ saturation = q;
+ value = v;
+ break;
+
+ case 4:
+ hue = t;
+ saturation = p;
+ value = v;
+ break;
+
+ default: //case 5:
+ hue = v;
+ saturation = p;
+ value = q;
+ break;
+ }
+ }
+
+ rgb[i*step] = (unsigned char) vpMath::round(hue * 255.0);
+ rgb[i*step + 1] = (unsigned char) vpMath::round(saturation * 255.0);
+ rgb[i*step + 2] = (unsigned char) vpMath::round(value * 255.0);
+ }
+}
+
+void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
+ const unsigned int size, const unsigned int step) {
+ for(unsigned int i = 0; i < size; i++) {
+ double red, green, blue;
+ double h, s, v;
+ double min, max;
+ double delta;
+
+ red = rgb[i*step] / 255.0;
+ green = rgb[i*step + 1] / 255.0;
+ blue = rgb[i*step + 2] / 255.0;
+
+ if (red > green) {
+ max = (std::max)(red, blue);
+ min = (std::min)(green, blue);
+ } else {
+ max = (std::max)(green, blue);
+ min = (std::min)(red, blue);
+ }
+
+ v = max;
+
+ if (!vpMath::equal(max, 0.0, std::numeric_limits<double>::epsilon())) {
+ s = (max - min) / max;
+ } else {
+ s = 0.0;
+ }
+
+ if (vpMath::equal(s, 0.0, std::numeric_limits<double>::epsilon())) {
+ h = 0.0;
+ } else {
+ delta = max - min;
+ if (vpMath::equal(delta, 0.0, std::numeric_limits<double>::epsilon())) {
+ delta = 1.0;
+ }
+
+ if (vpMath::equal(red, max, std::numeric_limits<double>::epsilon())) {
+ h = (green - blue) / delta;
+ } else if (vpMath::equal(green, max, std::numeric_limits<double>::epsilon())) {
+ h = 2 + (blue - red) / delta;
+ } else {
+ h = 4 + (red - green) / delta;
+ }
+
+ h /= 6.0;
+ if (h < 0.0) {
+ h += 1.0;
+ } else if (h > 1.0) {
+ h -= 1.0;
+ }
+ }
+
+ hue[i] = h;
+ saturation[i] = s;
+ value[i] = v;
+ }
+}
+
+/*!
+ Converts an array of hue, saturation and value to an array of RGBa values.
+
+ \param hue : Array of hue values (range between [0 - 1]).
+ \param saturation : Array of saturation values (range between [0 - 1]).
+ \param value : Array of value values (range between [0 - 1]).
+ \param rgba : RGBa array values (with alpha channel set to zero) converted from HSV color space.
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::HSVToRGBa(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
+ const unsigned int size) {
+ vpImageConvert::HSV2RGB(hue, saturation, value, rgba, size, 4);
+}
+
+/*!
+ Converts an array of hue, saturation and value to an array of RGBa values.
+
+ \param hue : Array of hue values (range between [0 - 255]).
+ \param saturation : Array of saturation values (range between [0 - 255]).
+ \param value : Array of value values (range between [0 - 255]).
+ \param rgba : RGBa array values (with alpha channel set to zero) converted from HSV color space.
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::HSVToRGBa(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
+ unsigned char *rgba, const unsigned int size) {
+ for(unsigned int i = 0; i < size; i++) {
+ double h = hue[i] / 255.0, s = saturation[i] / 255.0, v = value[i] / 255.0;
+
+ vpImageConvert::HSVToRGBa(&h, &s, &v, (rgba + i*4), 1);
+ }
+}
+
+/*!
+ Converts an array of RGBa to an array of hue, saturation, value values.
+ The alpha channel is not used.
+
+ \param rgba : RGBa array values.
+ \param hue : Array of hue values converted from RGB color space (range between [0 - 1]).
+ \param saturation : Array of saturation values converted from RGB color space (range between [0 - 1]).
+ \param value : Array of value values converted from RGB color space (range between [0 - 1]).
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::RGBaToHSV(const unsigned char *rgba, double *hue, double *saturation, double *value,
+ const unsigned int size) {
+ vpImageConvert::RGB2HSV(rgba, hue, saturation, value, size, 4);
+}
+
+/*!
+ Converts an array of RGBa to an array of hue, saturation, value values.
+ The alpha channel is not used.
+
+ \param rgba : RGBa array values.
+ \param hue : Array of hue values converted from RGB color space (range between [0 - 255]).
+ \param saturation : Array of saturation values converted from RGB color space (range between [0 - 255]).
+ \param value : Array of value values converted from RGB color space (range between [0 - 255]).
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::RGBaToHSV(const unsigned char *rgba, unsigned char *hue, unsigned char *saturation,
+ unsigned char *value, const unsigned int size) {
+ for(unsigned int i = 0; i < size; i++) {
+ double h, s, v;
+ vpImageConvert::RGBaToHSV((rgba + i*4), &h, &s, &v, 1);
+
+ hue[i] = (unsigned char) (255.0 * h);
+ saturation[i] = (unsigned char) (255.0 * s);
+ value[i] = (unsigned char) (255.0 * v);
+ }
+}
+
+/*!
+ Converts an array of hue, saturation and value to an array of RGB values.
+
+ \param hue : Array of hue values (range between [0 - 1]).
+ \param saturation : Array of saturation values (range between [0 - 1]).
+ \param value : Array of value values (range between [0 - 1]).
+ \param rgb : RGB array values converted from RGB color space.
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::HSVToRGB(const double *hue, const double *saturation, const double *value, unsigned char *rgb,
+ const unsigned int size) {
+ vpImageConvert::HSV2RGB(hue, saturation, value, rgb, size, 3);
+}
+
+/*!
+ Converts an array of hue, saturation and value to an array of RGB values.
+
+ \param hue : Array of hue values (range between [0 - 255]).
+ \param saturation : Array of saturation values (range between [0 - 255]).
+ \param value : Array of value values (range between [0 - 255]).
+ \param rgb : RGB array values converted from HSV color space.
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::HSVToRGB(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
+ unsigned char *rgb, const unsigned int size) {
+ for(unsigned int i = 0; i < size; i++) {
+ double h = hue[i] / 255.0, s = saturation[i] / 255.0, v = value[i] / 255.0;
+
+ vpImageConvert::HSVToRGB(&h, &s, &v, (rgb + i*3), 1);
+ }
+}
+
+/*!
+ Converts an array of RGB to an array of hue, saturation, value values.
+
+ \param rgb : RGB array values.
+ \param hue : Array of hue values converted from RGB color space (range between [0 - 1]).
+ \param saturation : Array of saturation values converted from RGB color space (range between [0 - 1]).
+ \param value : Array of value values converted from RGB color space (range between [0 - 1]).
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::RGBToHSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
+ const unsigned int size) {
+ vpImageConvert::RGB2HSV(rgb, hue, saturation, value, size, 3);
+}
+
+/*!
+ Converts an array of RGB to an array of hue, saturation, value values.
+
+ \param rgb : RGB array values.
+ \param hue : Array of hue values converted from RGB color space (range between [0 - 255]).
+ \param saturation : Array of saturation values converted from RGB color space (range between [0 - 255]).
+ \param value : Array of value values converted from RGB color space (range between [0 - 255]).
+ \param size : The total image size or the number of pixels.
+*/
+void vpImageConvert::RGBToHSV(const unsigned char *rgb, unsigned char *hue, unsigned char *saturation, unsigned char *value,
+ const unsigned int size) {
+ for(unsigned int i = 0; i < size; i++) {
+ double h, s, v;
+
+ vpImageConvert::RGBToHSV((rgb + i*3), &h, &s, &v, 1);
+
+ hue[i] = (unsigned char) (255.0 * h);
+ saturation[i] = (unsigned char) (255.0 * s);
+ value[i] = (unsigned char) (255.0 * v);
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp
new file mode 100644
index 0000000..8088d2a
--- /dev/null
+++ b/modules/core/src/image/vpImageFilter.cpp
@@ -0,0 +1,707 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Various image tools, convolution, ...
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpImageConvert.h>
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+# include <opencv2/imgproc/imgproc.hpp>
+#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+# include <opencv2/imgproc/imgproc_c.h>
+#elif defined(VISP_HAVE_OPENCV)
+# include <cv.h>
+#endif
+
+/*!
+ Apply a filter to an image.
+
+ \param I : Image to filter
+ \param If : Filtered image.
+ \param M : Filter coefficients.
+
+*/
+void
+vpImageFilter::filter(const vpImage<unsigned char> &I,
+ vpImage<double>& If,
+ const vpMatrix& M)
+{
+
+ unsigned int size = M.getRows() ;
+ unsigned int half_size = size/2 ;
+
+ If.resize(I.getHeight(),I.getWidth()) ;
+
+ If = 0 ;
+
+ for (unsigned int i=half_size ; i < I.getHeight()-half_size ; i++)
+ {
+ for (unsigned int j=half_size ; j < I.getWidth()-half_size ; j++)
+ {
+ double conv_x = 0 ;
+
+ for(unsigned int a = 0 ; a < size ; a++ )
+ for(unsigned int b = 0 ; b < size ; b++ )
+ {
+ double val = I[i-half_size+a][j-half_size+b] ;
+ conv_x += M[a][b] * val ;
+ }
+ If[i][j] = conv_x ;
+ }
+ }
+
+}
+
+/*!
+ Apply a filter to an image.
+
+ \param I : Image to filter
+ \param Iu : Filtered image along the horizontal axis (u = columns).
+ \param Iv : Filtered image along the vertical axis (v = rows).
+ \param M : Separate filter coefficients
+
+*/
+void
+vpImageFilter::filter(const vpImage<double> &I,
+ vpImage<double>& Iu,
+ vpImage<double>& Iv,
+ const vpMatrix& M)
+{
+
+ unsigned int size = M.getRows() ;
+ unsigned int half_size = size/2 ;
+
+ Iu.resize(I.getHeight(),I.getWidth()) ;
+ Iv.resize(I.getHeight(),I.getWidth()) ;
+
+ Iu = 0 ;
+ Iv = 0 ;
+ for (unsigned int v=half_size ; v < I.getHeight()-half_size ; v++)
+ {
+ for (unsigned int u=half_size ; u < I.getWidth()-half_size ; u++)
+ {
+ double conv_u = 0 ;
+ double conv_v = 0 ;
+
+ for(unsigned int a = 0 ; a < size ; a++ )
+ for(unsigned int b = 0 ; b < size ; b++ )
+ {
+ double val = I[v-half_size+a][u-half_size+b] ;
+ conv_u += M[a][b] * val ;
+ conv_v += M[b][a] * val ;
+ }
+ Iu[v][u] = conv_u ;
+ Iv[v][u] = conv_v ;
+ }
+ }
+
+}
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+/*!
+ Apply the Canny edge operator on the image \e Isrc and return the resulting
+ image \e Ires.
+
+ The following example shows how to use the method:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageFilter.h>
+
+int main()
+{
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100 // Canny uses OpenCV >=2.1.0
+ // Constants for the Canny operator.
+ const unsigned int gaussianFilterSize = 5;
+ const double thresholdCanny = 15;
+ const unsigned int apertureSobel = 3;
+
+ // Image for the Canny edge operator
+ vpImage<unsigned char> Isrc;
+ vpImage<unsigned char> Icanny;
+
+ //First grab the source image Isrc.
+
+ //Apply the Canny edge operator and set the Icanny image.
+ vpImageFilter::canny(Isrc, Icanny, gaussianFilterSize, thresholdCanny, apertureSobel);
+#endif
+ return (0);
+}
+ \endcode
+
+ \param Isrc : Image to apply the Canny edge detector to.
+ \param Ires : Filtered image (255 means an edge, 0 otherwise).
+ \param gaussianFilterSize : The size of the mask of the Gaussian filter to
+ apply (an odd number).
+ \param thresholdCanny : The threshold for the Canny operator. Only value
+ greater than this value are marked as an edge).
+ \param apertureSobel : Size of the mask for the Sobel operator (odd number).
+*/
+void
+vpImageFilter:: canny(const vpImage<unsigned char>& Isrc,
+ vpImage<unsigned char>& Ires,
+ const unsigned int gaussianFilterSize,
+ const double thresholdCanny,
+ const unsigned int apertureSobel)
+{
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ IplImage* img_ipl = NULL;
+ vpImageConvert::convert(Isrc, img_ipl);
+ IplImage* edges_ipl;
+ edges_ipl = cvCreateImage(cvSize(img_ipl->width, img_ipl->height), img_ipl->depth, img_ipl->nChannels);
+
+ cvSmooth(img_ipl, img_ipl, CV_GAUSSIAN, (int)gaussianFilterSize, (int)gaussianFilterSize, 0, 0);
+ cvCanny(img_ipl, edges_ipl, thresholdCanny, thresholdCanny, (int)apertureSobel);
+
+ vpImageConvert::convert(edges_ipl, Ires);
+ cvReleaseImage(&img_ipl);
+ cvReleaseImage(&edges_ipl);
+#else
+ cv::Mat img_cvmat, edges_cvmat;
+ vpImageConvert::convert(Isrc, img_cvmat);
+ cv::GaussianBlur(img_cvmat, img_cvmat, cv::Size((int)gaussianFilterSize, (int)gaussianFilterSize), 0, 0);
+ cv::Canny(img_cvmat, edges_cvmat, thresholdCanny, thresholdCanny, (int)apertureSobel);
+ vpImageConvert::convert(edges_cvmat, Ires);
+#endif
+}
+#endif
+
+/*!
+ Apply a separable filter.
+ */
+void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size)
+{
+ vpImage<double> GIx ;
+ filterX(I, GIx,filter,size);
+ filterY(GIx, GI,filter,size);
+ GIx.destroy();
+}
+
+/*!
+ Apply a separable filter.
+ */
+void vpImageFilter::filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size)
+{
+ vpImage<double> GIx ;
+ filterX(I, GIx,filter,size);
+ filterY(GIx, GI,filter,size);
+ GIx.destroy();
+}
+
+void vpImageFilter::filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+{
+ dIx.resize(I.getHeight(),I.getWidth()) ;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < (size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
+ //dIx[i][j]=0;
+ }
+ for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
+ }
+ for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
+ //dIx[i][j]=0;
+ }
+ }
+}
+void vpImageFilter::filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+{
+ dIx.resize(I.getHeight(),I.getWidth()) ;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < (size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
+ //dIx[i][j]=0;
+ }
+ for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
+ }
+ for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
+ {
+ dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
+ //dIx[i][j]=0;
+ }
+ }
+}
+void vpImageFilter::filterY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+{
+ dIy.resize(I.getHeight(),I.getWidth()) ;
+ for (unsigned int i=0 ; i < (size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
+ }
+ }
+ for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
+ }
+ }
+ for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
+ }
+ }
+}
+void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+{
+ dIy.resize(I.getHeight(),I.getWidth()) ;
+ for (unsigned int i=0 ; i < (size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
+ }
+ }
+ for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
+ }
+ }
+ for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
+ }
+ }
+}
+
+/*!
+ Apply a Gaussian blur to an image.
+ \param I : Input image.
+ \param GI : Filtered image.
+ \param size : Filter size. This value should be odd.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
+ \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+
+ */
+void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size, double sigma, bool normalize)
+{
+ double *fg=new double[(size+1)/2] ;
+ vpImageFilter::getGaussianKernel(fg, size, sigma, normalize) ;
+ vpImage<double> GIx ;
+ vpImageFilter::filterX(I, GIx,fg,size);
+ vpImageFilter::filterY(GIx, GI,fg,size);
+ GIx.destroy();
+ delete[] fg;
+}
+
+/*!
+ Apply a Gaussian blur to a double image.
+ \param I : Input double image.
+ \param GI : Filtered image.
+ \param size : Filter size. This value should be odd.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
+ \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+
+ */
+void vpImageFilter::gaussianBlur(const vpImage<double> &I, vpImage<double>& GI, unsigned int size, double sigma, bool normalize)
+{
+ double *fg=new double[(size+1)/2] ;
+ vpImageFilter::getGaussianKernel(fg, size, sigma, normalize) ;
+ vpImage<double> GIx ;
+ vpImageFilter::filterX(I, GIx,fg,size);
+ vpImageFilter::filterY(GIx, GI,fg,size);
+ GIx.destroy();
+ delete[] fg;
+}
+
+/*!
+ Return the coefficients of a Gaussian filter.
+
+ \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
+ The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
+ \param size : Filter size. This value should be odd.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
+ \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+*/
+void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, double sigma, bool normalize)
+{
+ if (size%2 != 1)
+ throw (vpImageException(vpImageException::incorrectInitializationError,
+ "Bad Gaussian filter size"));
+
+ if (sigma<= 0)
+ sigma = (size-1)/6.0;
+
+ int middle = (int)(size-1)/2;
+ double sigma2 = vpMath::sqr(sigma);
+ for( int i=0; i<= middle; i++)
+ {
+ filter[i] = (1./(sigma*sqrt(2.*M_PI)))*exp(-(i*i)/(2.*sigma2));
+ }
+ if (normalize) {
+ //renormalization
+ double sum=0;
+ for(int i=1; i<=middle; i++)
+ {
+ sum += 2*filter[i] ;
+ }
+ sum += filter[0];
+
+ for(int i=0; i<=middle; i++)
+ {
+ filter[i] = filter[i]/sum;
+ }
+ }
+}
+
+/*!
+ Return the coefficients of a Gaussian derivative filter that may be used to compute spatial image derivatives after applying a Gaussian blur.
+
+ \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
+ The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
+ \param size : Filter size. This value should be odd.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
+ \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+*/
+void vpImageFilter::getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma, bool normalize)
+{
+ if (size%2 != 1)
+ throw (vpImageException(vpImageException::incorrectInitializationError,
+ "Bad Gaussian filter size"));
+
+ if (sigma<= 0)
+ sigma = (size-1)/6.0;
+
+ int middle = (int)(size-1)/2;
+ double sigma2 = vpMath::sqr(sigma);
+ filter[0] = 0.;
+ for(int i=1; i<= middle; i++)
+ {
+ filter[i] = -(1./(sigma*sqrt(2.*M_PI)))*(exp(-((i+1)*(i+1))/(2.*sigma2))-exp(-((i-1)*(i-1))/(2.*sigma2)))/2.;
+ }
+
+ if (normalize) {
+ double sum=0;
+ for(int i=1; i<=middle; i++)
+ {
+ sum += 2.*(1./(sigma*sqrt(2.*M_PI)))*exp(-(i*i)/(2.*sigma2));
+ }
+ sum += (1./(sigma*sqrt(2.*M_PI))) ;
+
+ for(int i=1; i<=middle; i++)
+ {
+ filter[i] = filter[i]/sum;
+ }
+ }
+}
+
+
+void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx)
+{
+ dIx.resize(I.getHeight(),I.getWidth()) ;
+ //dIx=0;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < 3 ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ for (unsigned int j=3 ; j < I.getWidth()-3 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j);
+ }
+ for (unsigned int j=I.getWidth()-3 ; j < I.getWidth() ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ }
+}
+
+void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy)
+{
+ dIy.resize(I.getHeight(),I.getWidth()) ;
+ //dIy=0;
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+ for (unsigned int i=3 ; i < I.getHeight()-3 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j);
+ }
+ }
+ for (unsigned int i=I.getHeight()-3 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+}
+
+void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+{
+ dIx.resize(I.getHeight(),I.getWidth()) ;
+ //#pragma omp parallel for
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < (size-1)/2 ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
+ }
+ for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ }
+}
+void vpImageFilter::getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+{
+ dIx.resize(I.getHeight(),I.getWidth()) ;
+ //dIx=0;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < (size-1)/2 ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
+ {
+ dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
+ }
+ for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
+ {
+ dIx[i][j]=0;
+ }
+ }
+}
+
+void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+{
+ dIy.resize(I.getHeight(),I.getWidth()) ;
+ //#pragma omp parallel for
+ for (unsigned int i=0 ; i < (size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+ //#pragma omp parallel for
+ for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
+ }
+ }
+ //#pragma omp parallel for
+ for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+}
+
+void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+{
+ dIy.resize(I.getHeight(),I.getWidth()) ;
+ //dIy=0;
+ for (unsigned int i=0 ; i < (size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+ for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
+ }
+ }
+ for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
+ {
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ dIy[i][j]=0;
+ }
+ }
+}
+
+/*!
+ Compute the gradient along X after applying a gaussian filter along Y.
+ \param I : Input image
+ \param dIx : Gradient along X.
+ \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
+ \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
+ \param size : Size of the Gaussian and Gaussian derivative kernels.
+ */
+void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
+{
+ vpImage<double> GIy;
+ vpImageFilter::filterY(I, GIy, gaussianKernel, size);
+ vpImageFilter::getGradX(GIy, dIx, gaussianDerivativeKernel, size);
+}
+
+/*!
+ Compute the gradient along Y after applying a gaussian filter along X.
+ \param I : Input image
+ \param dIy : Gradient along Y.
+ \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
+ \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
+ \param size : Size of the Gaussian and Gaussian derivative kernels.
+ */
+void vpImageFilter::getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel,unsigned int size)
+{
+ vpImage<double> GIx;
+ vpImageFilter::filterX(I, GIx, gaussianKernel, size);
+ vpImageFilter::getGradY(GIx, dIy, gaussianDerivativeKernel, size);
+}
+
+//operation pour pyramide gaussienne
+void vpImageFilter::getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+{
+ vpImage<unsigned char> GIx;
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat imgsrc, imgdest;
+ vpImageConvert::convert(I, imgsrc);
+ cv::pyrDown( imgsrc, imgdest, cv::Size((int)I.getWidth()/2,(int)I.getHeight()/2));
+ vpImageConvert::convert(imgdest, GI);
+#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat imgsrc, imgdest;
+ vpImageConvert::convert(I, imgsrc);
+ cv::pyrDown( imgsrc, imgdest, cvSize((int)I.getWidth()/2,(int)I.getHeight()/2));
+ vpImageConvert::convert(imgdest, GI);
+#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ IplImage* imgsrc = NULL;//cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
+ IplImage* imgdest = NULL;//cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
+ imgsrc = cvCreateImage(cvSize((int)I.getWidth(),(int)I.getHeight()), IPL_DEPTH_8U, 1);
+ imgdest = cvCreateImage(cvSize((int)I.getWidth()/2,(int)I.getHeight()/2), IPL_DEPTH_8U, 1);
+ vpImageConvert::convert(I,imgsrc);
+ cvPyrDown( imgsrc, imgdest);
+ vpImageConvert::convert(imgdest,GI);
+
+ cvReleaseImage(&imgsrc);
+ cvReleaseImage(&imgdest);
+ //vpImage<unsigned char> sGI;sGI=GI;
+
+#else
+ vpImageFilter::getGaussXPyramidal(I,GIx);
+ vpImageFilter::getGaussYPyramidal(GIx,GI);
+#endif
+}
+
+void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+{
+#if 0
+ GI.resize(I.getHeight(),(int)((I.getWidth()+1.)/2.)) ;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ GI[i][0]=I[i][0];
+ for (unsigned int j=1 ; j < ((I.getWidth()+1.)/2.)-1 ; j++)
+ {
+ GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
+ }
+ GI[i][(int)((I.getWidth()+1.)/2.)-1]=I[i][2*((int)((I.getWidth()+1.)/2.)-1)];
+ }
+#else
+ unsigned int w = I.getWidth()/2;
+
+ GI.resize(I.getHeight(), w) ;
+ for (unsigned int i=0 ; i < I.getHeight() ; i++)
+ {
+ GI[i][0]=I[i][0];
+ for (unsigned int j=1 ; j < w-1 ; j++)
+ {
+ GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
+ }
+ GI[i][w-1]=I[i][2*w-1];
+ }
+
+#endif
+}
+void vpImageFilter::getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+{
+
+#ifdef ORIG
+ GI.resize((int)((I.getHeight()+1.)/2.),I.getWidth()) ;
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ GI[0][j]=I[0][j];
+ for (unsigned int i=1 ; i < ((I.getHeight()+1.)/2.)-1 ; i++)
+ {
+ GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
+ }
+ GI[(int)((I.getHeight()+1.)/2.)-1][j]=I[2*((int)((I.getHeight()+1.)/2.)-1)][j];
+ }
+
+#else
+ unsigned int h = I.getHeight()/2;
+
+ GI.resize(h, I.getWidth()) ;
+ for (unsigned int j=0 ; j < I.getWidth() ; j++)
+ {
+ GI[0][j]=I[0][j];
+ for (unsigned int i=1 ; i < h-1 ; i++)
+ {
+ GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
+ }
+ GI[h-1][j]=I[2*h-1][j];
+ }
+#endif
+}
+
+
diff --git a/modules/core/src/image/vpImagePoint.cpp b/modules/core/src/image/vpImagePoint.cpp
new file mode 100644
index 0000000..fa44a4d
--- /dev/null
+++ b/modules/core/src/image/vpImagePoint.cpp
@@ -0,0 +1,386 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point useful for image processing
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRect.h>
+
+/*!
+
+ Check if an image point belongs to a rectangle.
+
+ \param rect : the rectangle.
+
+ \return Returns true if the point belongs to the rectangle.
+
+*/
+bool vpImagePoint::inRectangle( const vpRect &rect ) const
+{
+ return ( this->i <= rect.getBottom() &&
+ this->i >= rect.getTop() &&
+ this->j <= rect.getRight() &&
+ this->j >= rect.getLeft());
+}
+
+/*!
+
+ Operator +=.
+
+ This operator can be used to compute the center of gravity of a set of image points.
+
+ \code
+#include <iostream>
+#include <vector>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+std::vector<vpImagePoint> ip(2);
+
+ip[0].set_ij(100, 200);
+ip[1].set_ij(300, 400);
+
+vpImagePoint cog(0,0);
+for(unsigned int i=0; i<ip.size(); i++)
+ cog += ip[i];
+cog /= ip.size();
+std::cout << "cog: " << cog << std::endl;
+}
+ \endcode
+
+*/
+vpImagePoint& vpImagePoint::operator+=(const vpImagePoint &ip) {
+ this->i += ip.i;
+ this->j += ip.j;
+ return *this;
+}
+
+/*!
+
+ Operator /=.
+
+ This operator can be used to compute the center of gravity of a set of image points.
+ \code
+#include <iostream>
+#include <vector>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+std::vector<vpImagePoint> ip(2);
+
+ip[0].set_ij(100, 200);
+ip[1].set_ij(300, 400);
+
+vpImagePoint cog(0,0);
+for(unsigned int i=0; i<ip.size(); i++)
+ cog += ip[i];
+cog /= ip.size();
+std::cout << "cog: " << cog << std::endl;
+}
+ \endcode
+
+*/
+vpImagePoint& vpImagePoint::operator/=(const double scale) {
+ this->i /= scale;
+ this->j /= scale;
+ return *this;
+}
+
+/*!
+ \relates vpImagePoint
+
+ Returns true if ip1 and ip2 are equal; otherwire returns false.
+
+*/
+VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
+ //return ( ( ip1.get_i() == ip2.get_i() ) && ( ip1.get_j() == ip2.get_j() ) );
+
+ double i1 = ip1.get_i();
+ double j1 = ip1.get_j();
+ double i2 = ip2.get_i();
+ double j2 = ip2.get_j();
+
+ return (
+ ( std::fabs(i1-i2) <= std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
+ &&
+ ( std::fabs(j1-j2) <= std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
+ );
+}
+
+/*!
+
+ \relates vpImagePoint
+
+ Returns true if ip1 and ip2 are different; otherwire returns true.
+
+*/
+VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
+ //return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() ) );
+ double i1 = ip1.get_i();
+ double j1 = ip1.get_j();
+ double i2 = ip2.get_i();
+ double j2 = ip2.get_j();
+
+ return (
+ ( std::fabs(i1-i2) > std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
+ ||
+ ( std::fabs(j1-j2) > std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
+ );
+}
+
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint wich is the sum of \f$ ip1 \f$ and \f$ ip2 \f$.
+
+*/
+VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
+ return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint wich is the sum of \f$ ip1 \f$ and \f$ ip2 \f$.
+
+*/
+VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
+ return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with an offset added to the two coordinates.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip+10: " << ip+10 << std::endl; // new coordinates (110, 210)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset ) {
+ return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with an offset added to the two coordinates.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip+12.34: " << ip+12.34 << std::endl; // new coordinates (112.34, 212.34)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const double offset ) {
+ return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
+}
+
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint wich is the difference between \f$ ip1 \f$ and \f$ ip2 \f$.
+
+*/
+VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
+ return ( vpImagePoint(ip1.get_i()-ip2.get_i(), ip1.get_j()-ip2.get_j()));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with an offset substracted to the two coordinates.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip-10: " << ip-10 << std::endl; // new coordinates (90, 190)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const int offset ) {
+ return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with an offset substracted to the two coordinates.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip-12.34: " << ip-12.34 << std::endl; // new coordinates (87.66, 187.66)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const double offset ) {
+ return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with coordinates multiplied by a scale factor.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip*2: " << ip*2 << std::endl; // new coordinates (200, 400)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator*( const vpImagePoint &ip1, const double scale ) {
+ return ( vpImagePoint(ip1.get_i()*scale, ip1.get_j()*scale));
+}
+/*!
+
+ \relates vpImagePoint
+
+ Returns a vpImagePoint with coordinates divided by a scale factor.
+
+ \code
+#include <iostream>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
+ std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
+ std::cout << "ip/2: " << ip/2 << std::endl; // new coordinates (50, 100)
+
+ return 0;
+}
+ \endcode
+*/
+VISP_EXPORT vpImagePoint operator/( const vpImagePoint &ip1, const double scale ) {
+ return ( vpImagePoint(ip1.get_i()/scale, ip1.get_j()/scale));
+}
+
+/*!
+
+ \relates vpImagePoint
+
+ Writes the image point coordinates \e ip to the stream \e os, and
+ returns a reference to the stream. Writes the first coordinate along
+ the \e i axis and then the second one along the \e j axis. The
+ coordinates are separated by a comma.
+
+ The following code
+ \code
+#include <iostream>
+
+#include <visp3/core/vpImagePoint.h>
+int main()
+{
+ vpImagePoint ip;
+
+ ip.set_i(10);
+ ip.set_j(11.1);
+
+ std::cout << "Image point with coordinates: " << ip << std::endl;
+
+ return 0;
+}
+ \endcode
+
+ The previous sample code produces the output:
+ \verbatim
+Image point with coordinates: 10, 11.1
+ \endverbatim
+*/
+VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip)
+{
+ os << ip.get_i() << ", " << ip.get_j();
+ return os;
+}
+
+/**
+ * Computes and returns the bounding box.
+ * @param ipVec : Vector of input image points.
+ * @return Bounding box of the points.
+ */
+vpRect vpImagePoint::getBBox(const std::vector<vpImagePoint>& ipVec)
+{
+ vpRect rec(ipVec);
+
+ return rec;
+}
diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp
new file mode 100644
index 0000000..ae1f2b9
--- /dev/null
+++ b/modules/core/src/image/vpImageTools.cpp
@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image tools.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImageTools.h>
+
+
+/*!
+
+ Change the look up table (LUT) of an image. Considering pixel gray
+ level values \f$ l \f$ in the range \f$[A, B]\f$, this method allows
+ to rescale these values in \f$[A^*, B^*]\f$ by linear interpolation:
+
+ \f$
+ \left\{ \begin{array}{ll}
+ l \in ]-\infty, A] \mbox{, } & l = A^* \\
+ l \in [B, \infty[ \mbox{, } & l = B^* \\
+ l \in ]A, B[ \mbox{, } & l = A^* + (l-A) * \frac{B^*-A^*}{B-A}
+ \end{array}
+ \right.
+ \f$
+
+ \param I : Image to process.
+ \param A : Low gray level value of the range to consider.
+ \param A_star : New gray level value \f$ A^*\f$ to attribute to pixel
+ who's value was A
+ \param B : Height gray level value of the range to consider.
+ \param B_star : New gray level value \f$ B^*\f$ to attribute to pixel
+ who's value was B
+ \return The modified image.
+
+ \exception vpImageException::incorrectInitializationError If \f$B \leq A\f$.
+
+ As shown in the example below, this method can be used to binarize
+ an image. For an unsigned char image (in the range 0-255),
+ thresholding this image at level 127 can be done by:
+
+ \code
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+ vpImage<unsigned char> I;
+#ifdef _WIN32
+ std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
+#else
+ std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
+#endif
+
+ // Read an image from the disk
+ vpImageIo::read(I, filename);
+
+ // Binarize image I:
+ // - gray level values less than or equal to 127 are set to 0,
+ // - gray level values greater than 128 are set to 255
+ vpImageTools::changeLUT(I, 127, 0, 128, 255);
+
+ vpImageIo::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format
+}
+ \endcode
+
+*/
+void vpImageTools::changeLUT(vpImage<unsigned char>& I,
+ unsigned char A,
+ unsigned char A_star,
+ unsigned char B,
+ unsigned char B_star)
+{
+ // Test if input values are valid
+ if (B <= A) {
+ vpERROR_TRACE("Bad gray levels") ;
+ throw (vpImageException(vpImageException::incorrectInitializationError ,
+ "Bad gray levels")) ;
+ }
+ unsigned char v;
+
+ double factor = (double)(B_star - A_star)/(double)(B - A);
+
+ for (unsigned int i=0 ; i < I.getHeight(); i++)
+ for (unsigned int j=0 ; j < I.getWidth(); j++) {
+ v = I[i][j];
+
+ if (v <= A)
+ I[i][j] = A_star;
+ else if (v >= B)
+ I[i][j] = B_star;
+ else
+ I[i][j] = (unsigned char)(A_star + factor*(v-A));
+ }
+}
+
+/*!
+ Compute the signed difference between the two images I1 and I2 for
+ visualization issue : Idiff = I1-I2
+
+ - pixels with a null difference are set to 128.
+ - A negative difference implies a pixel value < 128
+ - A positive difference implies a pixel value > 128
+
+ \param I1 : The first image.
+ \param I2 : The second image.
+ \param Idiff : The result of the difference.
+*/
+void vpImageTools::imageDifference(const vpImage<unsigned char> &I1,
+ const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Idiff)
+{
+ if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
+ {
+ throw (vpException(vpException::dimensionError, "The two images have not the same size"));
+ }
+
+ if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
+ Idiff.resize(I1.getHeight(), I1.getWidth());
+
+ unsigned int n = I1.getHeight() * I1.getWidth() ;
+ int diff ;
+ for (unsigned int b = 0; b < n ; b++)
+ {
+ diff = I1.bitmap[b] - I2.bitmap[b] + 128;
+ Idiff.bitmap[b] = (unsigned char)
+ (vpMath::maximum(vpMath::minimum(diff, 255), 0));
+ }
+}
+
+/*!
+ Compute the difference between the two images I1 and I2
+ \warning : This is NOT for visualization
+ If you want to visualize difference images during servo, please use
+ vpImageTools::imageDifference(..,..,..) function.
+
+ \param I1 : The first image.
+ \param I2 : The second image.
+ \param Idiff : The result of the difference.
+*/
+
+void
+vpImageTools::imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
+ const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Idiff)
+{
+ if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
+ {
+ throw (vpException(vpException::dimensionError, "The two images do not have the same size"));
+ }
+
+ if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
+ Idiff.resize(I1.getHeight(), I1.getWidth());
+
+ unsigned int n = I1.getHeight() * I1.getWidth() ;
+ int diff ;
+ for (unsigned int b = 0; b < n ; b++)
+ {
+ diff = I1.bitmap[b] - I2.bitmap[b];
+ Idiff.bitmap[b] = diff;
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/image/vpRGBa.cpp b/modules/core/src/image/vpRGBa.cpp
new file mode 100644
index 0000000..368c188
--- /dev/null
+++ b/modules/core/src/image/vpRGBa.cpp
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * RGBA pixel.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpRGBa.cpp
+ \brief Define the object vpRGBa that is used to build color
+ images (it define a RGB 32 bits structure, fourth byte is not used - yet -)
+*/
+
+
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+
+
+/*!
+ Copy operator (from an unsigned char value)
+
+ \param v : Input color ( R = G = B = v )
+*/
+vpRGBa &
+vpRGBa::operator=(const unsigned char &v)
+{
+ this->R = v;
+ this->G = v;
+ this->B = v;
+ this->A = v;
+ return *this;
+}
+
+/*!
+ Copy operator.
+*/
+vpRGBa &
+vpRGBa::operator=(const vpRGBa &v)
+{
+ this->R = v.R;
+ this->G = v.G;
+ this->B = v.B;
+ this->A = v.A;
+ return *this;
+}
+
+/*!
+ Cast a vpColVector in a vpRGBa
+
+ \param v : Input vector. v[0], v[1], v[2], v[3] are to make into
+ relation with respectively R, G, B and A.
+
+ \exception vpException::dimensionError : If v is not a 4 four
+ dimention vector.
+*/
+vpRGBa &
+vpRGBa::operator=(const vpColVector &v)
+{
+ if (v.getRows() != 4) {
+ vpERROR_TRACE("Bad vector dimension ") ;
+ throw(vpException(vpException::dimensionError, "Bad vector dimension "));
+ }
+ R = (unsigned char)v[0];
+ G = (unsigned char)v[1];
+ B = (unsigned char)v[2];
+ A = (unsigned char)v[3];
+ return *this;
+}
+
+/*!
+ Compare two RGBa values.
+
+ \return true if the values are the same, false otherwise.
+*/
+bool vpRGBa::operator==(const vpRGBa &v)
+{
+ if (R != v.R)
+ return false;
+ if (G != v.G)
+ return false;
+ if (B != v.B)
+ return false;
+ if (A != v.A)
+ return false;
+
+ return true ;
+}
+/*!
+ Compare two color pixels.
+
+ \return true if the images are different, false if they are the same.
+*/
+bool vpRGBa::operator!=(const vpRGBa &v)
+{
+ return (R != v.R || G != v.G || B != v.B || A != v.A);
+}
+
+/*!
+ Substraction operator : "this" - v.
+ \param v : Color to substract to the current object "this".
+ \return "this" - v
+*/
+vpColVector
+vpRGBa::operator-(const vpRGBa &v) const
+{
+ vpColVector n(4); // new color
+ n[0] = (double)R - (double)v.R;
+ n[1] = (double)G - (double)v.G;
+ n[2] = (double)B - (double)v.B;
+ n[3] = (double)A - (double)v.A;
+ return n;
+}
+
+/*!
+ Addition operator : "this" + v.
+ \param v : Color to add to the current object "this".
+ \return "this" + v
+ \warning in case of overflow : e.g. 128+128 returns 0 for all 4 channels
+*/
+vpRGBa
+vpRGBa::operator+(const vpRGBa &v) const
+{
+ vpRGBa n; // new color
+ n.R = static_cast<unsigned char>( R + v.R );
+ n.G = static_cast<unsigned char>( G + v.G );
+ n.B = static_cast<unsigned char>( B + v.B );
+ n.A = static_cast<unsigned char>( A + v.A );
+ return n;
+}
+
+/*!
+ Substraction operator : "this" - v.
+ \param v : Color to substract to the current object "this".
+ \return "this" - v
+*/
+vpColVector
+vpRGBa::operator-(const vpColVector &v) const
+{
+ vpColVector n(4); // new color
+ n[0] = R - v[0];
+ n[1] = G - v[1];
+ n[2] = B - v[2];
+ n[3] = A - v[3];
+ return n;
+}
+
+/*!
+ Addition operator : "this" + v.
+ \param v : Color to add to the current object "this".
+ \return "this" + v
+*/
+vpColVector
+vpRGBa::operator+(const vpColVector &v) const
+{
+ vpColVector n(4); // new color
+ n[0] = R + v[0];
+ n[1] = G + v[1];
+ n[2] = B + v[2];
+ n[3] = A + v[3];
+ return n;
+}
+
+/*!
+ Multiplication operator : v * "this".
+ \param v : Value to multiply.
+ \return v * "this"
+*/
+vpColVector
+vpRGBa::operator*(const float &v) const
+{
+ vpColVector n(4);
+ n[0] = R * v;
+ n[1] = G * v;
+ n[2] = B * v;
+ n[3] = A * v;
+ return n;
+}
+
+/*!
+ Multiplication operator : v * "this".
+ \param v : Value to multiply.
+ \return v * "this"
+*/
+vpColVector
+vpRGBa::operator*(const double &v) const
+{
+ vpColVector n(4);
+ n[0] = R * v;
+ n[1] = G * v;
+ n[2] = B * v;
+ n[3] = A * v;
+ return n;
+}
+
+bool
+vpRGBa::operator<(const vpRGBa &v) const
+{
+ double gray1 = 0.2126*R+0.7152*G+0.0722*B;
+ double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
+
+ return (gray1 < gray2);
+}
+
+bool
+vpRGBa::operator>(const vpRGBa &v) const
+{
+ double gray1 = 0.2126*R+0.7152*G+0.0722*B;
+ double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
+
+ return (gray1 > gray2);
+}
+
+vpRGBa operator*(const double &x, const vpRGBa &rgb)
+{
+ return rgb*x;
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/math/kalman/vpKalmanFilter.cpp b/modules/core/src/math/kalman/vpKalmanFilter.cpp
new file mode 100644
index 0000000..b787573
--- /dev/null
+++ b/modules/core/src/math/kalman/vpKalmanFilter.cpp
@@ -0,0 +1,463 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Kalman filtering.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpKalmanFilter.cpp
+ \brief Generic kalman filtering implementation.
+*/
+
+#include <visp3/core/vpKalmanFilter.h>
+
+#include <math.h>
+#include <stdlib.h>
+
+/*!
+ Initialize the Kalman filter.
+
+ \param size_state_vector : Size of the state vector \f${\bf x}_{k}\f$ for one signal.
+
+ \param size_measure_vector : Size of the measure vector \f${\bf z}_{k}\f$
+ for one signal.
+
+ \param n_signal : Number of signal to filter.
+*/
+void
+vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector,
+ unsigned int n_signal)
+{
+ this->size_state = size_state_vector;
+ this->size_measure = size_measure_vector ;
+ this->nsignal = n_signal ;
+ F.resize(size_state*nsignal, size_state*nsignal) ;
+ H.resize(size_measure*nsignal, size_state*nsignal) ;
+
+ R.resize(size_measure*nsignal, size_measure*nsignal) ;
+ Q.resize(size_state*nsignal, size_state*nsignal) ;
+
+ Xest.resize(size_state*nsignal) ; Xest = 0;
+ Xpre.resize(size_state*nsignal) ; Xpre = 0 ;
+
+ Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;
+
+ I.resize(size_state*nsignal, size_state*nsignal) ;
+ // init_done = false ;
+ iter = 0 ;
+ dt = -1 ;
+}
+
+/*!
+ Construct a default Kalman filter.
+
+ The verbose mode is by default desactivated.
+
+*/
+vpKalmanFilter::vpKalmanFilter()
+ : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
+ Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
+{
+}
+
+/*!
+ Construct a default Kalman filter by setting the number of signal to filter.
+
+ The verbose mode is by default desactivated.
+
+ \param n_signal : Number of signal to filter.
+*/
+vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
+ : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
+ Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
+{
+}
+
+/*!
+ Construct a Kalman filter.
+
+ The verbose mode is by default desactivated.
+
+ \param size_state_vector : Size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
+
+ \param size_measure_vector : Size of the measure vector \f${\bf z}_{(k)}\f$
+ for one signal.
+
+ \param n_signal : Number of signal to filter.
+*/
+vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
+ : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
+ Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
+{
+ init( size_state_vector, size_measure_vector, n_signal) ;
+}
+
+/*!
+ Update the Kalman filter by applying the prediction equations.
+
+ The predicted state is given by
+ \f[
+ {{\bf x}}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}
+ \f]
+ and the state prediction covariance by
+ \f[
+ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
+ + {\bf Q}_k
+ \f]
+
+*/
+
+void
+vpKalmanFilter::prediction()
+{
+ if (Xest.getRows() != size_state*nsignal) {
+ std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
+ <<" " << size_state*nsignal<< std::endl ;
+ std::cout << " Error : Filter non initialized " << std::endl;
+ exit(1) ;
+ }
+
+// if (!init_done) {
+// std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<< std::endl ;
+// std::cout << " Error : Filter non initialized " << std::endl;
+// exit(1) ;
+// return;
+// }
+
+ if (verbose_mode) {
+ std::cout << "F = " << std::endl << F << std::endl ;
+ std::cout << "Xest = "<< std::endl << Xest << std::endl ;
+ }
+ // Prediction
+ // Bar-Shalom 5.2.3.2
+ Xpre = F*Xest ;
+ if (verbose_mode) {
+ std::cout << "Xpre = "<< std::endl << Xpre << std::endl ;
+ std::cout << "Q = "<< std::endl << Q << std::endl ;
+ std::cout << "Pest " << std::endl << Pest << std::endl ;
+ }
+ // Bar-Shalom 5.2.3.5
+ Ppre = F*Pest*F.t() + Q ;
+
+ // Matrice de covariance de l'erreur de prediction
+ if (verbose_mode)
+ std::cout << "Ppre " << std::endl << Ppre << std::endl ;
+}
+
+/*!
+
+ Update the Kalman filter by applying the filtering equations and
+ increment the filter iteration (vpKalmanFilter::iter).
+
+ \param z : Measure (or observation) \f${\bf z}_k\f$ provided at iteration \f$k\f$.
+
+ The filtering equation is given by:
+ \f[
+ {\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k -
+ {\bf H} {\bf x}_{k \mid k-1} \right]
+ \f]
+ where \f${\bf W_k}\f$ is the filter gain computed using the formula:
+ \f[
+ {\bf W_k} = {\bf P}_{k \mid k-1} {\bf H}^T
+ \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}
+ \f]
+ and where the updated covariance of the state is given by
+ \f[
+ {\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k \mid k-1}
+ \f]
+ or in a symetric form
+ \f[
+ {\bf P}_{k \mid k} = {\bf P}_{k \mid k-1} - {\bf W}_k {\bf S}_k {\bf W}^T_k
+ \f]
+ with
+ \f[
+ {\bf S}_k = {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k
+ \f]
+
+*/
+void
+vpKalmanFilter::filtering(const vpColVector &z)
+{
+ if (verbose_mode)
+ std::cout << "z " << std::endl << z << std::endl ;
+ // Bar-Shalom 5.2.3.11
+ vpMatrix S = H*Ppre*H.t() + R ;
+ if (verbose_mode)
+ std::cout << "S " << std::endl << S << std::endl ;
+
+ W = (Ppre * H.t())* (S).inverseByLU() ;
+ if (verbose_mode)
+ std::cout << "W " << std::endl << W << std::endl ;
+ // Bar-Shalom 5.2.3.15
+ Pest = Ppre - W*S*W.t() ;
+ if (verbose_mode)
+ std::cout << "Pest " << std::endl << Pest << std::endl ;
+
+ if (0) {
+ // Bar-Shalom 5.2.3.16
+ // numeriquement plus stable que 5.2.3.15
+ vpMatrix Pestinv ;
+ Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU()*H ;
+ Pest = Pestinv.inverseByLU() ;
+ }
+ // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
+ Xest = Xpre + (W*(z - (H*Xpre))) ;
+ if (verbose_mode)
+ std::cout << "Xest " << std::endl << Xest << std::endl ;
+
+ iter++ ;
+}
+
+
+#if 0
+
+
+/*!
+ \brief Filter initialization for a constant velocity model
+
+ \param dt : time between two measures
+ \param Vn : Variance of measure noise
+ \param Vw : Variance of the state noise
+
+ State model:
+ \f[ S = \left[\; y \quad \frac{\partial y}{\partial t}\;\right]^T = \left[\; y \quad \dot{ y}\;\right]^T
+ \f]
+
+ Filter model:
+ \f[
+ \begin{array}{rclll} \\
+ S(t+1) &= & F S(t) + W(t)&~~~~~~~~~~~& S(t+1) \mbox{ est un vecteur} \left[\;
+ y\quad \dot y\;\right]^T \\
+ X(t) &=& C S(t) + N(t)&& X(t) \mbox{ est un scalaire}
+ \end{array}
+ \f]
+
+
+ Matrix F describes the evolution of the state. This matrix is given by:
+ \f[
+ F= \left( \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right)
+ \f]
+
+ The noise \f$W = \left( \begin{array}{c} \;W_1 \quad W_2\; \end{array} \right)^T\f$
+ take into account the variations of the constant velocity model due to the accelerations.
+
+ Thus we have:
+ \f[
+ \left\{
+ \begin{array}{rcl}
+ y(t+1)& =& y(t) + \Delta(t) \dot y(t) + \underbrace{\frac{\Delta t^2}{2} \ddot y(t)}_{W_1} \\
+ \dot y(t+1) &=& \dot y(t) + \underbrace{\Delta(t) \ddot y(t)}_{W_2}
+ \end{array}
+ \right.
+ \f]
+ et donc
+ \f[
+ \left\{
+ \begin{array}{rcccccc}
+ y(t+1)& =& y(t) &+& \Delta(t) \dot y(t) &+& {W_1} \\
+ \dot y(t+1) &=& & &\dot y(t) &+& {W_2}
+ \end{array}
+ \right. \qquad \Leftrightarrow \qquad S(t+1) = F S(t) + W
+ \f]
+*/
+void
+vpKalmanFilter::initFilterCteAcceleration(double dt,
+ vpColVector &Z0,
+ vpColVector &Z1,
+ vpColVector &Z2,
+ vpColVector &sigma_noise,
+ vpColVector &sigma_state )
+{
+ this->dt = dt ;
+
+ double dt2 = dt*dt ;
+ double dt3 = dt2*dt ;
+ double dt4 = dt3*dt ;
+ double dt5 = dt4*dt ;
+
+ //init_done = true ;
+
+ Pest =0 ;
+ // initialise les matrices decrivant les modeles
+ for (int i=0; i < size_measure ; i++ )
+ {
+ // modele sur l'etat
+
+ // | 1 dt dt2/2 |
+ // F = | 0 1 dt |
+ // | 0 0 1 |
+
+ F[3*i][3*i] = 1 ;
+ F[3*i][3*i+1] = dt ;
+ F[3*i][3*i+2] = dt*dt/2 ;
+ F[3*i+1][3*i+1] = 1 ;
+ F[3*i+1][3*i+2] = dt ;
+ F[3*i+2][3*i+2] = 1 ;
+
+
+ // modele sur la mesure
+ H[i][3*i] = 1 ;
+ H[i][3*i+1] = 0 ;
+ H[i][3*i+2] = 0 ;
+
+ double sR = sigma_noise[i] ;
+ double sQ = sigma_state[i] ;
+
+ // bruit de mesure
+ R[i][i] = (sR) ;
+
+ // bruit d'etat 6.2.3.9
+ Q[3*i ][3*i ] = sQ * dt5/20;
+ Q[3*i ][3*i+1] = sQ * dt4/8;
+ Q[3*i ][3*i+2] = sQ * dt3/6 ;
+
+ Q[3*i+1][3*i ] = sQ * dt4/8 ;
+ Q[3*i+1][3*i+1] = sQ * dt3/3 ;
+ Q[3*i+1][3*i+2] = sQ * dt2/2 ;
+
+ Q[3*i+2][3*i ] = sQ * dt3/6 ;
+ Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
+ Q[3*i+2][3*i+2] = sQ * dt ;
+
+
+ // Initialisation pour la matrice de covariance sur l'etat
+
+ Pest[3*i ][3*i ] = sR ;
+ Pest[3*i ][3*i+1] = 1.5/dt*sR ;
+ Pest[3*i ][3*i+2] = sR/(dt2) ;
+
+ Pest[3*i+1][3*i ] = 1.5/dt*sR ;
+ Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
+ Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
+
+ Pest[3*i+2][3*i ] = sR/(dt2) ;
+ Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
+ Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
+
+
+ // Initialisation pour l'etat
+
+ Xest[3*i] = Z2[i] ;
+ Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
+ Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
+
+ }
+}
+
+void
+vpKalmanFilter::initFilterSinger(double dt,
+ double a,
+ vpColVector &Z0,
+ vpColVector &Z1,
+ vpColVector &sigma_noise,
+ vpColVector &sigma_state )
+{
+ this->dt = dt ;
+
+ double dt2 = dt*dt ;
+ double dt3 = dt2*dt ;
+
+ double a2 = a*a ;
+ double a3 = a2*a ;
+ double a4 = a3*a ;
+
+ //init_done = true ;
+
+ Pest =0 ;
+ // initialise les matrices decrivant les modeles
+ for (int i=0; i < size_measure ; i++ )
+ {
+ // modele sur l'etat
+
+ // | 1 dt dt2/2 |
+ // F = | 0 1 dt |
+ // | 0 0 1 |
+
+ F[3*i][3*i] = 1 ;
+ F[3*i][3*i+1] = dt ;
+ F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
+ F[3*i+1][3*i+1] = 1 ;
+ F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
+ F[3*i+2][3*i+2] = exp(-a*dt) ;
+
+
+ // modele sur la mesure
+ H[i][3*i] = 1 ;
+ H[i][3*i+1] = 0 ;
+ H[i][3*i+2] = 0 ;
+
+ double sR = sigma_noise[i] ;
+ double sQ = sigma_state[i] ;
+
+ R[i][i] = (sR) ; // bruit de mesure 1.5mm
+
+ Q[3*i ][3*i ] = sQ/a4*(1-exp(-2*a*dt)+2*a*dt+2*a3/3*dt3-2*a2*dt2-4*a*dt*exp(-a*dt) ) ;
+ Q[3*i ][3*i+1] = sQ/a3*(1+exp(-2*a*dt)-2*exp(-a*dt)+2*a*dt*exp(-a*dt)-2*a*dt+a2*dt2 ) ;
+ Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
+
+ Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
+ Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
+ Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
+
+ Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
+ Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
+ Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
+
+
+ // Initialisation pour la matrice de covariance sur l'etat
+ Pest[3*i ][3*i ] = sR ;
+ Pest[3*i ][3*i+1] = 1/dt*sR ;
+ Pest[3*i ][3*i+2] = 0 ;
+
+ Pest[3*i+1][3*i ] = 1/dt*sR ;
+ Pest[3*i+1][3*i+1] = 2*sR/dt2 + sQ/(a4*dt2)*(2-a2*dt2+2*a3*dt3/3.0 -2*exp(-a*dt)-2*a*dt*exp(-a*dt));
+ Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
+
+ Pest[3*i+2][3*i ] = 0 ;
+ Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
+ Pest[3*i+2][3*i+2] = 0 ;
+
+
+ // Initialisation pour l'etat
+
+ Xest[3*i] = Z1[i] ;
+ Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
+ Xest[3*i+2] = 0 ;
+
+ }
+}
+
+#endif
diff --git a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
new file mode 100644
index 0000000..9f65574
--- /dev/null
+++ b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
@@ -0,0 +1,854 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Kalman filtering.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpLinearKalmanFilterInstantiation.cpp
+ \brief Implementation of some specific Kalman filters.
+*/
+
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+
+#include <math.h>
+#include <stdlib.h>
+
+/*!
+
+ Initialize the Kalman filter material depending on the selected
+ state model set with setStateModel(). This function is provided as a
+ wrapper over all the other initializer functions like
+ initStateConstVel_MeasurePos(),
+ initStateConstVelWithColoredNoise_MeasureVel(),
+ initStateConstAccWithColoredNoise_MeasureVel().
+
+ \warning It is requiered to set the state model before using this method.
+
+ \param n_signal : Number of signal to filter.
+
+ \param sigma_state : Vector that contains the variance of the state
+ noise. The dimension of this vector is equal to the state vector
+ \f${\bf x}\f$ size multiplied by the number of signal to
+ filter. Values are used to initialize the \f${\bf Q}\f$ state
+ covariance matrix.
+
+ \param sigma_measure : Vector that contains the variance of the
+ measurement noise. The dimension of this vector is equal to the
+ measure vector \f${\bf x}\f$ size multiplied by the number of signal
+ to filter. Values are used to initialize the \f${\bf R}\f$ measure
+ covariance matrix.
+
+ \param rho : Degree of correlation between successive accelerations. Values
+ are in [0:1[.
+
+ \param delta_t : Sampling time \f$\Delta t\f$ expressed is
+ second. Depending on the filter modelization, this value may not be
+ used. This is for example the case for the
+ vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel model
+ implemented in initStateConstVelWithColoredNoise_MeasureVel().
+
+ \exception vpException::badValue : Bad rho value wich is not in [0:1[.
+
+ \exception vpException::notInitialized : If the state model is not
+ initialized. To initialize it you need to call setStateModel().
+
+ The example below shows how to initialize the filter for a one
+ dimensional signal.
+
+ \code
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+
+int main()
+{
+ vpLinearKalmanFilterInstantiation kalman;
+ // Select a constant velocity state model with colored noise
+ // Measures are velocities
+ kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
+
+ // Initialise the filter for a one dimension signal
+ int signal = 1;
+ vpColVector sigma_state(2); // State vector size is 2
+ vpColVector sigma_measure(1); // Measure vector size is 1
+ double rho = 0.9;
+ double dummy = 0; // non used parameter for the selected state model
+
+ kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy);
+}
+ \endcode
+
+ The example below shows a more complete example to filter a two
+ dimensional target trajectory with an estimation of the target
+ velocities from velocity measures.
+
+ \code
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+
+int main()
+{
+ vpLinearKalmanFilterInstantiation kalman;
+
+ // Set the constant velocity state model used for the filtering
+ vpLinearKalmanFilterInstantiation::vpStateModel model;
+ model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
+ kalman.setStateModel(model);
+
+ // We are now able to retrieve the size of the state vector and measure vector
+ int state_size = kalman.getStateSize();
+
+ // Filter the x and y velocities of a target (2 signals are to consider)
+ int nsignal = 2;
+
+ // Initialize the filter parameters:
+ // - Firstly, the state variance
+ int size = state_size*nsignal;
+ vpColVector sigma_state(size);
+ sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
+ sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
+ // - Secondly, the measures variance
+ vpColVector sigma_measure(nsignal); // 2 velocity measures available
+ sigma_measure[0] = 0.03; // Variance on the x velocity measure
+ sigma_measure[1] = 0.06; // Variance on the y velocity measure
+ // - Thirdly, the correlation between succesive accelerations
+ double rho = 0.9;
+
+ double dummy = 0; // non used parameter for the selected state model
+
+ // Initialize the filter
+ // The state model was set before
+ kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
+
+ // Does the filtering
+ vpColVector vm(2); // Measured velocities
+ for ( ; ; ) {
+ // Get the two dimentional velocity measures
+ // vm[0] = ...;
+ // vm[1] = ...;
+
+ // Compute the filtering and the prediction
+ kalman.filter(vm);
+ // Print the estimation of the velocities (1st value of the state vector)
+ std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
+ std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
+ << std::endl;
+ // The one step prediction is available in kalman.Xpre variable
+ }
+}
+ \endcode
+ */
+void
+vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double rho, double delta_t)
+{
+ switch (model) {
+ case stateConstVelWithColoredNoise_MeasureVel:
+ initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state,
+ sigma_measure, rho);
+ break;
+ case stateConstVel_MeasurePos:
+ initStateConstVel_MeasurePos(n_signal, sigma_state,
+ sigma_measure, delta_t);
+ break;
+ case stateConstAccWithColoredNoise_MeasureVel:
+ initStateConstAccWithColoredNoise_MeasureVel(n_signal, sigma_state,
+ sigma_measure, rho, delta_t);
+ break;
+ case unknown:
+ vpERROR_TRACE("Kalman state model is not set") ;
+ throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
+ }
+}
+
+/*!
+ Modelisation of a constant speed state model with white noise. The
+ measure is assumed to be the position of the target.
+
+ The considered state model is the following
+
+ \f[
+ \left\{
+ \begin{array}{rlrl}
+ x_{(k+1)} & = x_{(k)} & + \Delta t \; {\dot{x}}_{(k)} & + {w_1}_{(k)} \\
+ {\dot{x}}_{(k+1)} & = & {\dot{x}}_{(k)} &+{w_2}_{(k)}
+ \end{array}
+ \right.
+ \f]
+
+ The terms \f${w_1}_{(k)}\f$ and \f${w_2}_{(k)}\f$ account for deviations from the assumed
+ constant velocity trajectory. They are assumed zero-mean, white,
+ mutually uncorrelated, stationary random variable with variance
+ \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$.
+
+ We recall that the recursive state evolution equation is given by
+ \f[
+ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
+ \f]
+
+ From this state model, the transition matrix \f${\bf F}\f$ and the
+ state covariance matrix \f${\bf Q}\f$ are given by:
+
+ \f[
+ {\bf F} =
+ \left[
+ \begin{array}{cc}
+ 1 & \Delta t\\
+ 0 & 1
+ \end{array}
+ \right]
+ \f]
+
+ and
+
+ \f[
+ {\bf Q} = \sigma^2_Q
+ \left[
+ \begin{array}{cc}
+ \frac{1}{3}\Delta t^3 & \frac{1}{2}\Delta t^2\\
+ \frac{1}{2}\Delta t^2 & \Delta t
+ \end{array}
+ \right]
+ \f]
+
+ The initial value of the state vector at iteration 0 is set to:
+ \f[
+ {\bf x_{(0)}} =
+ \left[
+ \begin{array}{c}
+ z_{(0)}\\
+ 0
+ \end{array}
+ \right]
+ \f]
+
+ The value at iteration 1 is set to:
+ \f[
+ {\bf x_{(1)}} =
+ \left[
+ \begin{array}{c}
+ z_{(1)}\\
+ (z_{(1)} - z_{(0)})/ \Delta t
+ \end{array}
+ \right]
+ \f]
+
+ The initial value \f$P_{(0|0)}\f$ of the prediction covariance
+ matrix is given by:
+
+ \f[
+ {\bf P_{(0|0)}} =
+ \left[ \begin{array}{cc}
+ \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\
+ \frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t + \frac{\sigma^2_R}{2 \Delta t^2}
+ \end{array}
+ \right]
+ \f]
+
+ \param n_signal : Number of signal to filter.
+
+ \param sigma_state : Vector that fix the variance of the state covariance matrix
+ \f$[\sigma^2_Q \; 0]^T\f$. The dimension of
+ this vector is 2 multiplied by the number of signal to filter.
+
+ \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
+ noise. The dimension of this vector is equal to the number of signal
+ to filter.
+
+ \param delta_t : Sampling time \f$\Delta t\f$ expressed is second.
+
+*/
+void
+vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_signal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double delta_t )
+{
+ // init_done = true ;
+ setStateModel(stateConstVel_MeasurePos);
+
+ init(size_state, size_measure, n_signal);
+
+ iter = 0;
+ Pest = 0;
+ Xest = 0;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
+ this->dt = delta_t ;
+
+ double dt2 = dt*dt ;
+ double dt3 = dt2*dt ;
+
+ for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
+ // State model
+ // | 1 dt |
+ // F = | |
+ // | 0 1 |
+
+ F[2*i][2*i] = 1 ;
+ F[2*i][2*i+1] = dt ;
+ F[2*i+1][2*i+1] = 1 ;
+
+ // Measure model
+ H[i][2*i] = 1 ;
+ H[i][2*i+1] = 0 ;
+
+ double sR = sigma_measure[i] ;
+ double sQ = sigma_state[2*i] ; // sigma_state[2*i+1] is not used
+
+ // Measure noise
+ R[i][i] = sR ;
+
+ // State covariance matrix 6.2.2.12
+ Q[2*i][2*i] = sQ * dt3/3;
+ Q[2*i][2*i+1] = sQ * dt2/2;
+ Q[2*i+1][2*i] = sQ * dt2/2;
+ Q[2*i+1][2*i+1] = sQ * dt;
+
+ Pest[2*i][2*i] = sR ;
+ Pest[2*i][2*i+1] = sR/(2*dt) ;
+ Pest[2*i+1][2*i] = sR/(2*dt) ;
+ Pest[2*i+1][2*i+1] = sQ*2*dt/3.0+ sR/(2*dt2) ;
+ }
+}
+
+/*!
+
+ Modelisation of a constant speed state model with colored noise. The
+ measure is assumed to be the velocity of the target.
+
+ This state model assume that there is some memory associated with
+ noise measurements as acceleration terms. They can be represented as
+ remaining correlated (or colored) over succesive time intervals,
+ leading to the following state model:
+
+ \f[
+ \left\{
+ \begin{array}{rll}
+ x_{(k+1)} & = x_{(k)} + \nu_{(k)} &\\
+ \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{(k)}
+ \end{array}
+ \right.
+ \f]
+
+ The term \f$w_{(k)}\f$ account for deviations from the assumed
+ constant velocity trajectory. It is assumed zero-mean, white,
+ mutually uncorrelated, stationary random variable with variance
+ \f$\sigma^2_Q\f$. The term \f$\rho\f$ is the degree of correlation
+ between successive accelerations. Values can range from 0 to 1.
+
+ We recall that the recursive state evolution equation is given by
+ \f[
+ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
+ \f]
+
+ From this state model, the transition matrix \f${\bf F}\f$ and the
+ state covariance matrix \f${\bf Q}\f$ are given by:
+
+ \f[
+ {\bf F} =
+ \left[
+ \begin{array}{cc}
+ 1 & 1\\
+ 0 & \rho
+ \end{array}
+ \right]
+ \f]
+
+ and
+
+ \f[
+ {\bf Q} =
+ \left[
+ \begin{array}{cc}
+ 0 & 0\\
+ 0 & \sigma^2_Q
+ \end{array}
+ \right]
+ \f]
+
+ The measurement model is given by:
+ \f[
+ z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)}
+ \f]
+
+ where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the
+ velocity and \f$r_{(k)}\f$ is the measurement noise, assumed
+ zero-mean, white mutually uncorrelated stationary random variables
+ with variance \f$\sigma^2_R\f$, giving the covariance matrix:
+
+ \f[
+ {\bf R} = \left[\sigma^2_R\right]
+ \f]
+
+ The initial value of the state vector is set to:
+ \f[
+ {\bf x_{(0)}} =
+ \left[
+ \begin{array}{c}
+ z_{(0)}\\
+ 0
+ \end{array}
+ \right]
+ \f]
+
+ The initial value \f$P_{(0|0)}\f$ of the prediction covariance
+ matrix is given by:
+
+ \f[
+ {\bf P_{(0|0)}} =
+ \left[ \begin{array}{cc}
+ \sigma^2_R & 0\\
+ 0 & \sigma^2_Q/(1-\rho^2)
+ \end{array}
+ \right]
+ \f]
+
+ \param n_signal : Number of signal to filter.
+
+ \param sigma_state : Vector that fix the variance of the state covariance matrix
+ \f$[0 \; \sigma^2_Q]^T\f$. The dimension of
+ this vector is 2 multiplied by the number of signal to filter.
+
+ \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
+ noise. The dimension of this vector is equal to the number of signal
+ to filter.
+
+ \param rho : Degree of correlation between successive accelerations. Values
+ are in [0:1[.
+
+ \exception vpException::badValue : Bad rho value wich is not in [0:1[.
+
+ The example below shows how to filter a two dimensional target
+ trajectory with an estimation of the target velocity from velocity
+ measures.
+
+ \code
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+
+int main()
+{
+ vpLinearKalmanFilterInstantiation kalman;
+ // Filter the x and y velocities of a target (2 signals are to consider)
+ int nsignal = 2;
+
+ // Initialize the filter parameters:
+ // - Firstly, the state variance
+ vpColVector sigma_state(4); // 4 = 2 for the state size x 2 signal
+ sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
+ sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
+ // - Secondly, the measures variance
+ vpColVector sigma_measure(nsignal); // 2 velocity measures available
+ sigma_measure[0] = 0.03; // Variance on the x velocity measure
+ sigma_measure[1] = 0.06; // Variance on the y velocity measure
+ // - Thirdly, the correlation between succesive accelerations
+ double rho = 0.9;
+
+ // Initialize the filter
+ kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho);
+
+ // Does the filtering
+ vpColVector vm(2); // Measured velocities
+ for ( ; ; ) {
+ // Get the two dimentional velocity measures
+ // vm[0] = ...;
+ // vm[1] = ...;
+
+ // Compute the filtering and the prediction
+ kalman.filter(vm);
+ // Print the estimation of the velocities (1st value of the state vector)
+ std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
+ std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
+ << std::endl;
+ // The one step prediction is available in kalman.Xpre variable
+ }
+}
+ \endcode
+*/
+void
+vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(unsigned int n_signal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double rho)
+{
+ if ((rho < 0) || (rho >= 1)) {
+ vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
+ throw(vpException(vpException::badValue, "Bad rho value")) ;
+ }
+
+ setStateModel(stateConstVelWithColoredNoise_MeasureVel);
+
+ init(size_state, size_measure, n_signal);
+
+ iter = 0;
+ Pest = 0;
+ Xest = 0;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
+
+ for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
+ // State model
+ // | 1 1 |
+ // F = | |
+ // | 0 rho |
+
+ F[2*i][2*i] = 1 ;
+ F[2*i][2*i+1] = 1 ;
+ F[2*i+1][2*i+1] = rho ;
+
+ // Measure model
+ H[i][2*i] = 1 ;
+ H[i][2*i+1] = 0 ;
+
+ double sR = sigma_measure[i] ;
+ double sQ = sigma_state[2*i+1] ; // sigma_state[2*i] is not used
+
+ // Measure noise
+ R[i][i] = sR ;
+
+ // State covariance matrix
+ Q[2*i][2*i] = 0 ;
+ Q[2*i][2*i+1] = 0;
+ Q[2*i+1][2*i] = 0;
+ Q[2*i+1][2*i+1] = sQ ;
+
+ Pest[2*i][2*i] = sR ;
+ Pest[2*i][2*i+1] = 0. ;
+ Pest[2*i+1][2*i] = 0 ;
+ Pest[2*i+1][2*i+1] = sQ/(1-rho*rho) ;
+ }
+}
+
+/*!
+
+ Modelisation of a constant acceleration state model with colored noise. The
+ measure is assumed to be the velocity of the target.
+
+ This state model assume that there is some memory associated with
+ noise measurements as acceleration terms. They can be represented as
+ remaining correlated (or colored) over succesive time intervals,
+ leading to the following state model:
+
+ \f[
+ \left\{
+ \begin{array}{rll}
+ x_{(k+1)} & = x_{(k)} + \Delta t \; \dot{x}_{(k)} + \nu_{(k)} &\\
+ \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{1(k)} \\
+ \dot{x}_{(k+1)} & = \dot{x}_{(k)} &+w_{2(k)}\\
+ \end{array}
+ \right.
+ \f]
+
+ The terms \f$w_{1(k)}\f$ and \f$w_{2(k)}\f$ account for deviations
+ from the assumed constant acceleration trajectory. They are assumed
+ zero-mean, white, mutually uncorrelated, stationary random variable
+ with variance \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$. The term
+ \f$\rho\f$ is the degree of correlation between successive
+ accelerations. Values can range from 0 to 1.
+
+ We recall that the recursive state evolution equation is given by
+ \f[
+ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
+ \f]
+
+ From this state model, the transition matrix \f${\bf F}\f$ and the
+ state covariance matrix \f${\bf Q}\f$ are given by:
+
+ \f[
+ {\bf F} =
+ \left[
+ \begin{array}{ccc}
+ 1 & 1 & \Delta t\\
+ 0 & \rho & 0 \\
+ 0 & 0 & 1
+ \end{array}
+ \right]
+ \f]
+
+ and
+
+ \f[
+ {\bf Q} =
+ \left[
+ \begin{array}{ccc}
+ 0 & 0 & 0\\
+ 0 & \sigma^2_{Q_1} & 0\\
+ 0 & 0& \sigma^2_{Q_2} \\
+ \end{array}
+ \right]
+ \f]
+
+ The measurement model is given by:
+ \f[
+ z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)}
+ \f]
+
+ where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the velocity and \f$r_{(k)}\f$
+ is the measurement noise, assumed zero-mean, white mutually
+ uncorrelated stationary random variables with variance
+ \f$\sigma^2_R\f$, giving the covariance matrix:
+
+ \f[
+ {\bf R} = \left[\sigma^2_R\right]
+ \f]
+
+ The initial value of the state vector is set to:
+ \f[
+ {\bf x_{(0)}} =
+ \left[
+ \begin{array}{c}
+ z_{(0)}\\
+ 0 \\
+ 0
+ \end{array}
+ \right]
+ \f]
+
+ The initial value \f$P_{(0|0)}\f$ of the prediction covariance
+ matrix is given by:
+
+ \f[
+ {\bf P_{(0|0)}} =
+ \left[ \begin{array}{ccc}
+ \sigma^2_R & 0 & \sigma^2_R / \Delta t\\
+ 0 & \sigma^2_{Q_1}/(1-\rho^2) & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t \\
+ \sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t & (2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2
+ \end{array}
+ \right]
+ \f]
+
+ \param n_signal : Number of signal to filter.
+
+ \param sigma_state : Vector that fix the variance of the state
+ covariance matrix \f$[0 \; \sigma^2_{Q_1} \;
+ \sigma^2_{Q_2}]^T\f$. The dimension of this vector is 3 multiplied
+ by the number of signal to filter.
+
+ \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
+ noise. The dimension of this vector is equal to the number of signal
+ to filter.
+
+ \param rho : Degree of correlation between successive accelerations. Values
+ are in [0:1[.
+
+ \param delta_t : Sampling time \f$\Delta t\f$ expressed is second.
+
+ \exception vpException::badValue : Bad rho value wich is not in [0:1[.
+
+ The example below shows how to filter a two dimensional target
+ trajectory with an estimation of the target velocity from velocity
+ measures.
+
+ \code
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+
+int main()
+{
+ vpLinearKalmanFilterInstantiation kalman;
+ // Filter the x and y velocities of a target (2 signals are to consider)
+ int nsignal = 2;
+
+ // Initialize the filter parameters:
+ // - Firstly, the state variance
+ vpColVector sigma_state(6); // 6 = 3 for the state size x 2 signal
+ sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
+ sigma_state[2] = 0.001; // Variance on the acceleration for the 1st signal (x)
+ sigma_state[4] = 0.002; // Variance on the acceleration for the 2nd signal (y)
+ sigma_state[5] = 0.002; // Variance on the acceleration for the 2nd signal (y)
+ // - Secondly, the measures variance
+ vpColVector sigma_measure(nsignal); // 2 velocity measures available
+ sigma_measure[0] = 0.03; // Variance on the x velocity measure
+ sigma_measure[1] = 0.06; // Variance on the y velocity measure
+ // - Thirdly, the correlation between succesive accelerations
+ double rho = 0.9;
+ // - Lastly, the sampling time
+ double dt = 0.020; // 20 ms
+
+ // Initialize the filter
+ kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho, dt);
+
+ // Does the filtering
+ vpColVector vm(2); // Measured velocities
+ for ( ; ; ) {
+ // Get the two dimentional velocity measures
+ // vm[0] = ...;
+ // vm[1] = ...;
+
+ // Compute the filtering and the prediction
+ kalman.filter(vm);
+ // Print the estimation of the velocities (1st value of the state vector)
+ std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
+ std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
+ << std::endl;
+ // The one step prediction is available in kalman.Xpre variable
+ }
+}
+ \endcode
+
+*/
+void
+vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(unsigned int n_signal,
+ vpColVector &sigma_state,
+ vpColVector &sigma_measure,
+ double rho,
+ double delta_t)
+{
+ if ((rho < 0) || (rho >= 1)) {
+ vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
+ throw(vpException(vpException::badValue, "Bad rho value")) ;
+ }
+ setStateModel(stateConstAccWithColoredNoise_MeasureVel);
+
+ init(size_state, size_measure, n_signal);
+
+ iter = 0;
+ Pest = 0;
+ Xest = 0;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
+ this->dt = delta_t;
+ // initialise les matrices decrivant les modeles
+ for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
+ // State model
+ // | 1 1 dt |
+ // F = | o rho 0 |
+ // | 0 0 1 |
+
+ F[3*i][3*i] = 1 ;
+ F[3*i][3*i+1] = 1 ;
+ F[3*i][3*i+2] = dt ;
+ F[3*i+1][3*i+1] = rho ;
+ F[3*i+2][3*i+2] = 1 ;
+
+ // Measure model
+ H[i][3*i] = 1 ;
+ H[i][3*i+1] = 0 ;
+ H[i][3*i+2] = 0 ;
+
+ double sR = sigma_measure[i] ;
+ double sQ1 = sigma_state[3*i+1] ;
+ double sQ2 = sigma_state[3*i+2] ;
+
+ // Measure noise
+ R[i][i] = sR ;
+
+ // State covariance matrix
+ Q[3*i+1][3*i+1] = sQ1;
+ Q[3*i+2][3*i+2] = sQ2;
+
+ Pest[3*i][3*i] = sR ;
+ Pest[3*i][3*i+1] = 0. ;
+ Pest[3*i][3*i+2] = sR/dt ;
+ Pest[3*i+1][3*i+1] = sQ1/(1-rho*rho) ;
+ Pest[3*i+1][3*i+2] = -rho*sQ1/((1-rho*rho)*dt) ;
+ Pest[3*i+2][3*i+2] = (2*sR+sQ1/(1-rho*rho))/(dt*dt) ;
+ // complete the lower triangle
+ Pest[3*i+1][3*i] = Pest[3*i][3*i+1];
+ Pest[3*i+2][3*i] = Pest[3*i][3*i+2];
+ Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2];
+ }
+}
+
+/*!
+
+ Do the filtering and prediction of the measure signal.
+
+ \param z : Measures \f${\bf z}_k\f$ used to initialise the filter. The dimension of
+ this vector is equal to the number of signal to filter (given by
+ getNumberOfSignal()) multiplied by the size of the measure vector
+ (given by getMeasureSize()) .
+
+ \exception vpException::notInitialized : If the filter is not
+ initialized. To initialize the filter see initFilter().
+
+*/
+void
+vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
+{
+ if (nsignal < 1) {
+ vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter") ;
+ throw(vpException(vpException::notInitialized,
+ "Bad signal number")) ;
+ }
+
+ // Specific initialization of the filter that depends on the state model
+ if (iter == 0) {
+ Xest = 0;
+ switch (model) {
+ case stateConstVel_MeasurePos:
+ case stateConstVelWithColoredNoise_MeasureVel:
+ case stateConstAccWithColoredNoise_MeasureVel:
+ for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
+ Xest[size_state*i] = z[i] ;
+ }
+ prediction();
+ // init_done = true;
+ break;
+ case unknown:
+ vpERROR_TRACE("Kalman state model is not set") ;
+ throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
+ break;
+ }
+ iter ++;
+
+ return;
+ }
+ else if (iter == 1) {
+ 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 ;
+ }
+ prediction();
+ iter ++;
+
+ return;
+ }
+ }
+
+ filtering(z);
+ prediction();
+
+}
diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp
new file mode 100644
index 0000000..cb2d9e5
--- /dev/null
+++ b/modules/core/src/math/matrix/vpColVector.cpp
@@ -0,0 +1,1274 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Provide some simple operation on column vectors.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpColVector.cpp
+ \brief Class that provides a data structure for the column vectors as well
+ as a set of operations on these vectors
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <string.h>
+#include <math.h>
+#include <sstream>
+#include <assert.h>
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRotationVector.h>
+
+
+//! Operator that allows to add two column vectors.
+vpColVector
+vpColVector::operator+(const vpColVector &v) const
+{
+ if (getRows() != v.getRows() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx1) column vector to (%dx1) column vector",
+ getRows(), v.getRows())) ;
+ }
+ vpColVector r(rowNum);
+
+ for (unsigned int i=0;i<rowNum;i++)
+ r[i] = (*this)[i] + v[i];
+ return r;
+}
+
+//! Operator that allows to add two column vectors.
+vpColVector &
+vpColVector::operator+=(vpColVector v)
+{
+ if (getRows() != v.getRows() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx1) column vector to (%dx1) column vector",
+ getRows(), v.getRows())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] += v[i];
+ return (*this);
+}
+//! Operator that allows to substract two column vectors.
+vpColVector &
+vpColVector::operator-=(vpColVector v)
+{
+ if (getRows() != v.getRows() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (%dx1) column vector to (%dx1) column vector",
+ getRows(), v.getRows())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] -= v[i];
+ return (*this);
+}
+
+
+/*!
+ Operator that performs the dot product between two column vectors.
+
+ \exception vpException::dimensionError If the vector dimension differ.
+
+ \sa dotProd()
+ */
+double
+vpColVector::operator*(const vpColVector &v) const
+{
+ if (size() != v.size()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
+ size(), v.size()));
+ }
+ double r = 0 ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ r += (*this)[i] * v[i];
+ return r;
+}
+
+/*!
+
+ Multiply a column vector by a row vector.
+
+ \param v : Row vector.
+
+ \return The resulting matrix.
+
+*/
+vpMatrix vpColVector::operator*(const vpRowVector &v) const
+{
+ vpMatrix M(rowNum, v.getCols());
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<v.getCols(); j++) {
+ M[i][j] = (*this)[i] * v[j];
+ }
+ }
+ return M;
+}
+
+//! operator substraction of two vectors V = A-v
+vpColVector vpColVector::operator-(const vpColVector &m) const
+{
+ if (getRows() != m.getRows() ) {
+ throw(vpException(vpException::dimensionError,
+ "Bad size during vpColVector (%dx1) and vpColVector (%dx1) substraction",
+ getRows(), m.getRows())) ;
+ }
+ vpColVector v(rowNum);
+
+ for (unsigned int i=0;i<rowNum;i++)
+ v[i] = (*this)[i] - m[i];
+ return v;
+}
+
+/*!
+ Construct a column vector from a part of an input column vector \e v.
+
+ \param v : Input column vector used for initialization.
+ \param r : row index in \e v that corresponds to the first element of the column vector to contruct.
+ \param nrows : Number of rows of the constructed column vector.
+
+ The sub-vector starting from v[r] element and ending on v[r+nrows-1] element
+ is used to initialize the contructed column vector.
+
+ \sa init()
+*/
+vpColVector::vpColVector (const vpColVector &v, unsigned int r, unsigned int nrows)
+ : vpArray2D<double>(nrows, 1)
+{
+ init(v, r, nrows);
+}
+
+/*!
+ Initialize the column vector from a part of an input column vector \e v.
+
+ \param v : Input column vector used for initialization.
+ \param r : row index in \e v that corresponds to the first element of the column vector to contruct.
+ \param nrows : Number of rows of the constructed column vector.
+
+ The sub-vector starting from v[r] element and ending on v[r+nrows-1] element
+ is used to initialize the contructed column vector.
+
+ The following code shows how to use this function:
+\code
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector v(4);
+ int val = 0;
+ for(size_t i=0; i<v.getRows(); i++) {
+ v[i] = val++;
+ }
+ std::cout << "v: " << v.t() << std::endl;
+
+ vpColVector w;
+ w.init(v, 0, 2);
+ std::cout << "w: " << w.t() << std::endl;
+
+}
+\endcode
+ It produces the following output:
+ \code
+v: 0 1 2 3
+w: 1 2
+ \endcode
+ */
+void
+vpColVector::init(const vpColVector &v, unsigned int r, unsigned int nrows)
+{
+ unsigned int rnrows = r+nrows ;
+
+ if (rnrows > v.getRows())
+ throw(vpException(vpException::dimensionError,
+ "Bad row dimension (%d > %d) used to initialize vpColVector",
+ rnrows, v.getRows()));
+ resize(nrows);
+
+ if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
+ return; // Noting to do
+ for (unsigned int i=r ; i < rnrows; i++)
+ (*this)[i-r] = v[i];
+}
+
+vpColVector::vpColVector (const vpRotationVector &v)
+ : vpArray2D<double>(v.size(), 1)
+{
+ for (unsigned int i=0; i< v.size(); i++)
+ (*this)[i] = v[i];
+}
+
+vpColVector::vpColVector (const vpPoseVector &p)
+ : vpArray2D<double>(p.size(), 1)
+{
+ for (unsigned int i=0; i< p.size(); i++)
+ (*this)[i] = p[i];
+}
+
+vpColVector::vpColVector (const vpTranslationVector &v)
+ : vpArray2D<double>(v.size(), 1)
+{
+ for (unsigned int i=0; i< v.size(); i++)
+ (*this)[i] = v[i];
+}
+
+//! Constructor that take column j of matrix M.
+vpColVector::vpColVector (const vpMatrix &M, unsigned int j)
+ : vpArray2D<double>(M.getRows(), 1)
+{
+ for(unsigned int i=0; i< M.getCols(); i++)
+ (*this)[i] = M[i][j];
+}
+
+/*!
+ Constructor that creates a column vector from a m-by-1 matrix \e M.
+
+ \exception vpException::dimensionError If the matrix is not a m-by-1 matrix.
+ */
+vpColVector::vpColVector (const vpMatrix &M)
+ : vpArray2D<double>(M.getRows(), 1)
+{
+ if(M.getCols()!=1) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
+ M.getRows(), M.getRows(), M.getCols())) ;
+ }
+
+ for(unsigned int i=0; i< M.getRows(); i++)
+ (*this)[i] = M[i][0];
+}
+
+/*!
+ Constructor that creates a column vector from a std vector of double.
+ */
+vpColVector::vpColVector (const std::vector<double> &v)
+ : vpArray2D<double>(1, (unsigned int)v.size())
+{
+ for(unsigned int i=0; i< v.size(); i++)
+ (*this)[i] = v[i];
+}
+/*!
+ Constructor that creates a column vector from a std vector of float.
+ */
+vpColVector::vpColVector (const std::vector<float> &v)
+ : vpArray2D<double>(1, (unsigned int)v.size())
+{
+ for(unsigned int i=0; i< v.size(); i++)
+ (*this)[i] = (double)(v[i]);
+}
+
+/*!
+ Operator that allows to negate all the column vector elements.
+
+ \code
+ vpColVector r(3, 1);
+ // r contains [1 1 1]^T
+ vpColVector v = -r;
+ // v contains [-1 -1 -1]^T
+ \endcode
+ */
+vpColVector vpColVector::operator-() const
+{
+ vpColVector A ;
+ try {
+ A.resize(rowNum) ;
+ }
+ catch(vpException &/*e*/)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+ double *vd = A.data ; double *d = data ;
+
+ for (unsigned int i=0; i<rowNum; i++)
+ *(vd++)= - (*d++);
+
+ return A;
+}
+
+/*!
+ Operator that allows to multiply each element of a column vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The column vector multiplied by the scalar. The current
+ column vector (*this) is unchanged.
+
+ \code
+ vpColVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+
+ vpColVector w = v * 3;
+ // v is unchanged
+ // w is now equal to : [3, 6, 9]
+ \endcode
+*/
+vpColVector vpColVector::operator*(double x) const
+{
+ vpColVector v(rowNum);
+
+ double *vd = v.data ; double *d = data ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ *(vd++) = (*d++) * x;
+ return v;
+}
+
+/*!
+ Operator that allows to multiply each element of a column vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The column vector multiplied by the scalar.
+
+ \code
+ vpColVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+
+ v *= 3;
+ // v is now equal to : [3, 6, 9]
+ \endcode
+*/
+vpColVector &vpColVector::operator*=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] *= x;
+ return (*this);
+}
+
+/*!
+ Operator that allows to divide each element of a column vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The column vector divided by the scalar.
+
+ \code
+ vpColVector v(3);
+ v[0] = 8;
+ v[1] = 4;
+ v[2] = 2;
+
+ v /= 2;
+ // v is now equal to : [4, 2, 1]
+ \endcode
+*/
+vpColVector &vpColVector::operator/=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] /= x;
+ return (*this);
+}
+
+/*!
+ Operator that allows to divide each element of a column vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The column vector divided by the scalar. The current
+ column vector (*this) is unchanged.
+
+ \code
+ vpColVector v(3);
+ v[0] = 8;
+ v[1] = 4;
+ v[2] = 2;
+
+ vpColVector w = v / 2;
+ // v is unchanged
+ // w is now equal to : [4, 2, 1]
+ \endcode
+*/
+vpColVector vpColVector::operator/(double x) const
+{
+ vpColVector v(rowNum);
+
+ double *vd = v.data ; double *d = data ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ *(vd++) = (*d++) / x;
+ return v;
+}
+
+/*!
+ Transform a m-by-1 matrix into a column vector.
+ \warning Handled with care; M should be a 1 column matrix.
+ \exception vpException::dimensionError If the matrix has more than 1 column.
+*/
+vpColVector &vpColVector::operator=(const vpMatrix &M)
+{
+ if (M.getCols() !=1) {
+ throw (vpException(vpException::dimensionError,
+ "Cannot transform a (%dx%d) matrix into a column vector",
+ M.getRows(), M.getCols()));
+ }
+
+ try {
+ resize(M.getRows());
+ }
+ catch(...) {
+ throw ;
+ }
+
+ memcpy(data, M.data, rowNum*sizeof(double)) ;
+
+ return (*this);
+}
+
+/*!
+ Initialize a row vector from a standard vector of double.
+*/
+vpColVector & vpColVector::operator=(const std::vector<double> &v)
+{
+ resize((unsigned int)v.size());
+ for(unsigned int i=0; i<v.size(); i++)
+ (*this)[i] = v[i];
+ return *this;
+}
+/*!
+ Initialize a row vector from a standard vector of double.
+*/
+vpColVector & vpColVector::operator=(const std::vector<float> &v)
+{
+ resize((unsigned int)v.size());
+ for(unsigned int i=0; i<v.size(); i++)
+ (*this)[i] = (float)v[i];
+ return *this;
+}
+
+vpColVector &vpColVector::operator=(const vpColVector &v)
+{
+ unsigned int k = v.rowNum ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, v.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+
+/*!
+ Operator that allows to convert a translation vector into a column vector.
+ */
+vpColVector &vpColVector::operator=(const vpTranslationVector &tv)
+{
+ unsigned int k = tv.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+/*!
+ Operator that allows to convert a rotation vector into a column vector.
+ */
+vpColVector &vpColVector::operator=(const vpRotationVector &rv)
+{
+ unsigned int k = rv.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, rv.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+/*!
+ Operator that allows to convert a pose vector into a column vector.
+ */
+vpColVector &vpColVector::operator=(const vpPoseVector &p)
+{
+ unsigned int k = p.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, p.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+
+/*!
+ Copy operator.
+ Allows operation such as A << v
+ \code
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector A, B(5);
+ for (unsigned int i=0; i<B.size(); i++)
+ B[i] = i;
+ A << B;
+ std::cout << "A: " << A.t() << std::endl;
+}
+ \endcode
+ In column vector A we get:
+ \code
+A: 0 1 2 3 4
+ \endcode
+ */
+vpColVector & vpColVector::operator<<(const vpColVector &v)
+{
+ *this = v;
+ return *this;
+}
+
+/*!
+ Assigment operator. Allow operation such as A = *v
+
+ The following example shows how to use this operator.
+ \code
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ size_t n = 5;
+ vpColVector A(n);
+ double *B = new double [n];
+ for (unsigned int i = 0; i < n; i++)
+ B[i] = i;
+ A << B;
+ std::cout << "A: " << A.t() << std::endl;
+ delete [] B;
+}
+ \endcode
+ It produces the following output:
+ \code
+A: 0 1 2 3 4
+ \endcode
+ */
+vpColVector & vpColVector::operator<<( double *x )
+{
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<colNum; j++) {
+ rowPtrs[i][j] = *x++;
+ }
+ }
+ return *this;
+}
+
+//! Set each element of the column vector to x.
+vpColVector & vpColVector::operator=(double x)
+{
+ double *d = data ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ *(d++)= x ;
+ return *this;
+}
+
+/*!
+ Transpose the column vector. The resulting vector becomes a row vector.
+*/
+vpRowVector vpColVector::t() const
+{
+ vpRowVector v(rowNum);
+ memcpy(v.data, data, rowNum*sizeof(double)) ;
+ return v;
+}
+
+/*!
+ Transpose the column vector. The resulting vector becomes a row vector.
+ \sa t()
+*/
+vpRowVector vpColVector::transpose() const
+{
+ return t();
+}
+
+/*!
+ Transpose the column vector. The resulting vector \e v becomes a row vector.
+ \sa t()
+*/
+void vpColVector::transpose(vpRowVector &v) const
+{
+ v = t();
+}
+
+
+/*!
+ \relates vpColVector
+ Allows to multiply a scalar by a column vector.
+*/
+vpColVector operator*(const double &x, const vpColVector &v)
+{
+ vpColVector vout ;
+ vout = v*x ;
+ return vout ;
+}
+
+/*!
+ Compute end return the dot product of two column vectors:
+ \f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of both vectors.
+
+ \exception vpException::dimensionError If the vector dimension differ.
+*/
+double
+vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
+{
+ if (a.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute the dot product: first vector empty")) ;
+ }
+ if (b.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute the dot product: second vector empty")) ;
+ }
+ if (a.size() != b.size()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
+ a.size(), b.size()));
+ }
+
+ double *ad = a.data ; double *bd = b.data ;
+
+ double c = 0 ;
+ for (unsigned int i=0 ; i < a.getRows() ; i++)
+ c += *(ad++)* *(bd++) ;
+ // vpMatrix c = (a.t() * b);
+ // return c[0][0];
+ return c ;
+}
+
+/*!
+ Normalise the vector:
+
+ \f[
+ {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
+ \f]
+*/
+ vpColVector &vpColVector::normalize(vpColVector &x) const
+ {
+ x = x/sqrt(x.sumSquare());
+
+ return x;
+ }
+
+
+/*!
+ Normalise the vector:
+
+ \f[
+ {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
+ \f]
+*/
+vpColVector &vpColVector::normalize()
+{
+
+ double sum_square = sumSquare();
+
+ //if (sum != 0.0)
+ if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
+ *this /= sqrt(sum_square) ;
+
+ // If sum = 0, we have a nul vector. So we return just.
+ return *this;
+}
+
+/*!
+ Return a column vector with elements of \e v that are reverse sorted.
+ \sa sort()
+ */
+vpColVector
+vpColVector::invSort(const vpColVector &v)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot sort content of column vector: vector empty")) ;
+ }
+ vpColVector tab ;
+ tab = v ;
+ unsigned int nb_permutation = 1 ;
+ unsigned int i = 0 ;
+ while (nb_permutation !=0 )
+ {
+ nb_permutation = 0 ;
+ for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
+ {
+ if ((tab[j]>tab[j-1]))
+ {
+ double tmp = tab[j] ;
+ tab[j] = tab[j-1] ;
+ tab[j-1] = tmp ;
+ nb_permutation++ ;
+ }
+ }
+ i++ ;
+ }
+
+ return tab ;
+}
+
+/*!
+ Return a column vector with elements of \e v that are sorted.
+ \sa invSort()
+ */
+vpColVector
+vpColVector::sort(const vpColVector &v)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot sort content of column vector: vector empty")) ;
+ }
+ vpColVector tab ;
+ tab = v ;
+ unsigned int nb_permutation = 1 ;
+ unsigned int i = 0 ;
+ while (nb_permutation !=0 )
+ {
+ nb_permutation = 0 ;
+ for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
+ {
+ if ((tab[j]<tab[j-1]))
+ {
+ double tmp = tab[j] ;
+ tab[j] = tab[j-1] ;
+ tab[j-1] = tmp ;
+ nb_permutation++ ;
+ }
+ }
+ i++ ;
+ }
+
+ return tab ;
+}
+
+/*!
+ Stack column vector with a new element at the end of the vector.
+
+ \param d : Element to stack to the existing vector.
+
+ \code
+ vpColVector v(3, 1);
+ // v is equal to [1 1 1]^T
+ v.stack(-2);
+ // v is equal to [1 1 1 -2]^T
+ \endcode
+
+ \sa stack(const vpColVector &, const vpColVector &)
+ \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
+
+*/
+void vpColVector::stack(const double &d)
+{
+ this->resize(rowNum+1,false);
+ (*this)[rowNum-1] = d;
+}
+
+/*!
+ Stack column vectors.
+
+ \param v : Vector to stack to the existing one.
+
+ \code
+ vpColVector v1(3, 1);
+ // v1 is equal to [1 1 1]^T
+ vpColVector v2(2, 3);
+ // v2 is equal to [3 3]^T
+ v1.stack(v2);
+ // v1 is equal to [1 1 1 3 3]^T
+ \endcode
+
+ \sa stack(const vpColVector &, const double &)
+ \sa stack(const vpColVector &, const vpColVector &)
+ \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
+
+*/
+void vpColVector::stack(const vpColVector &v)
+{
+ *this = vpColVector::stack(*this, v);
+}
+
+/*!
+ Stack column vectors.
+
+ \param A : Initial vector.
+ \param B : Vector to stack at the end of A.
+ \return Stacked vector \f$[A B]^T\f$.
+
+ \code
+ vpColVector A(3);
+ vpColVector B(5);
+ vpColVector C;
+ C = vpColVector::stack(A, B); // C = [A B]T
+ // C is now an 8 dimension column vector
+ \endcode
+
+ \sa stack(const vpColVector &)
+ \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
+*/
+vpColVector vpColVector::stack(const vpColVector &A, const vpColVector &B)
+{
+ vpColVector C;
+ vpColVector::stack(A, B, C);
+ return C;
+}
+
+/*!
+ Stack column vectors.
+
+ \param A : Initial vector.
+ \param B : Vector to stack at the end of A.
+ \param C : Resulting stacked vector \f$C = [A B]^T\f$.
+
+ \code
+ vpColVector A(3);
+ vpColVector B(5);
+ vpColVector C;
+ vpColVector::stack(A, B, C); // C = [A B]T
+ // C is now an 8 dimension column vector
+ \endcode
+
+ \sa stack(const vpColVector &)
+ \sa stack(const vpColVector &, const vpColVector &)
+*/
+void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
+{
+ unsigned int nrA = A.getRows();
+ unsigned int nrB = B.getRows();
+
+ if (nrA == 0 && nrB == 0) {
+ C.resize(0);
+ return;
+ }
+
+ if (nrB == 0) {
+ C = A;
+ return;
+ }
+
+ if (nrA == 0) {
+ C = B;
+ return;
+ }
+
+ // General case
+ C.resize(nrA + nrB);
+
+ for (unsigned int i=0; i<nrA; i++)
+ C[i] = A[i];
+
+ for (unsigned int i=0; i<nrB; i++)
+ C[nrA+i] = B[i];
+}
+
+/*!
+ Compute the mean value of all the elements of the vector.
+*/
+double vpColVector::mean(const vpColVector &v)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute column vector mean: vector empty")) ;
+ }
+ double mean = 0 ;
+ double *vd = v.data ;
+ for (unsigned int i=0 ; i < v.getRows() ; i++)
+ mean += *(vd++) ;
+
+ return mean/v.getRows();
+}
+
+/*!
+ Compute the median value of all the elements of the vector.
+*/
+double
+vpColVector::median(const vpColVector &v)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute column vector median: vector empty")) ;
+ }
+
+ std::vector<double> vectorOfDoubles(v.size());
+ for(unsigned int i = 0; i < v.size(); i++) {
+ vectorOfDoubles[i] = v[i];
+ }
+
+ return vpMath::getMedian(vectorOfDoubles);
+}
+
+/*!
+ Compute the standard deviation value of all the elements of the vector.
+*/
+double
+vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute column vector stdev: vector empty")) ;
+ }
+
+ double mean_value = mean(v);
+ double sum_squared_diff = 0.0;
+ for(unsigned int i = 0; i < v.size(); i++) {
+ sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
+ }
+
+ double divisor = (double) v.size();
+ if(useBesselCorrection && v.size() > 1) {
+ divisor = divisor-1;
+ }
+
+ return std::sqrt(sum_squared_diff / divisor);
+}
+
+/*!
+ Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v.
+
+ \f[ \mbox{if} \quad {\bf V} = \left( \begin{array}{c} x \\ y \\ z
+ \end{array}\right), \quad \mbox{then} \qquad
+ [{\bf v}]_\times = \left( \begin{array}{ccc}
+ 0 & -z & y \\
+ z & 0 & -x \\
+ -y & x & 0
+ \end{array}\right)
+ \f]
+
+ \param v : Input vector used to compute the skew symmetric matrix.
+*/
+vpMatrix
+vpColVector::skew(const vpColVector &v)
+{
+ vpMatrix M ;
+ if (v.getRows() != 3) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute skew vector of a non 3-dimention vector (%d)",
+ v.getRows())) ;
+ }
+
+ M.resize(3,3) ;
+ M[0][0] = 0 ; M[0][1] = -v[2] ; M[0][2] = v[1] ;
+ M[1][0] = v[2] ; M[1][1] = 0 ; M[1][2] = -v[0] ;
+ M[2][0] = -v[1] ; M[2][1] = v[0] ; M[2][2] = 0 ;
+
+ return M ;
+}
+
+/*!
+ Compute and return the cross product of two vectors \f$a \times b\f$.
+
+ \param a : 3-dimension column vector.
+ \param b : 3-dimension column vector.
+ \return The cross product \f$a \times b\f$.
+
+ \exception vpException::dimensionError If the vectors dimension is not equal to 3.
+*/
+vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
+{
+ if (a.getRows() != 3 || b.getRows() != 3) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute the cross product between column vector with dimension %d and %d",
+ a.getRows(), b.getRows()));
+ }
+
+ return vpColVector::skew(a) * b;
+}
+
+
+/*!
+ Reshape the column vector in a matrix.
+ \param nrows : number of rows of the matrix
+ \param ncols : number of columns of the matrix
+ \return The reshaped matrix.
+*/
+vpMatrix vpColVector::reshape(const unsigned int &nrows,const unsigned int &ncols){
+ vpMatrix M(nrows, ncols);
+ reshape(M, nrows, ncols);
+ return M;
+}
+
+/*!
+ Reshape the column vector in a matrix.
+ \param M : the reshaped matrix.
+ \param nrows : number of rows of the matrix.
+ \param ncols : number of columns of the matrix.
+*/
+void vpColVector::reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols){
+ if(dsize!=nrows*ncols) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot reshape (%dx1) column vector in (%dx%d) matrix",
+ rowNum, M.getRows(), M.getCols())) ;
+ }
+ try {
+ if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ for(unsigned int j =0; j< ncols; j++)
+ for(unsigned int i =0; i< nrows; i++)
+ M[i][j]=data[j*ncols+i];
+}
+
+/*!
+ Insert a column vector.
+ \param i : Index of the first element to introduce. This index starts from 0.
+ \param v : Column vector to insert.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector v(4);
+ for (unsigned int i=0; i < v.size(); i++)
+ v[i] = i;
+ std::cout << "v: " << v.t() << std::endl;
+
+ vpColVector w(2);
+ for (unsigned int i=0; i < w.size(); i++)
+ w[i] = i+10;
+ std::cout << "w: " << w.t() << std::endl;
+
+ v.insert(1, w);
+ std::cout << "v: " << v.t() << std::endl;
+}
+ \endcode
+ It produces the following output:
+ \code
+v: 0 1 2 3
+w: 10 11
+v: 0 10 11 3
+ \endcode
+ */
+void vpColVector::insert(unsigned int i, const vpColVector &v)
+{
+ if (i+v.size() > this->size())
+ throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
+ for (unsigned int j=0; j < v.size(); j++)
+ (*this)[i+j] = v[j];
+}
+
+/*!
+
+ Pretty print a column vector. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each vector element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the vector.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all vector elements.
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+int
+vpColVector::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = getRows();
+ unsigned int n = 1;
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int i=0;i<m;++i) {
+ oss.str("");
+ oss << (*this)[i];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[i];
+ oss.str(ossFixed.str());
+ }
+
+ values[i]=oss.str();
+ size_type thislen=values[i].size();
+ size_type p=values[i].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+
+ }
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ for (unsigned int i=0;i<m;i++) {
+ s <<" ";
+ size_type p=values[i].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[i].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[i].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+
+ s <<std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
+
+/*!
+ Return the sum square of all the elements \f$v_{i}\f$ of the column vector v(m).
+
+ \return The value \f[\sum{i=0}^{m} v_i^{2}\f].
+ */
+double vpColVector::sumSquare() const
+{
+ double sum_square=0.0;
+ double x ;
+
+ for (unsigned int i=0;i<rowNum;i++) {
+ x=rowPtrs[i][0];
+ sum_square += x*x;
+ }
+
+ return sum_square;
+}
+
+/*!
+ Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{v_{i}^2}} \f$.
+
+ \return The Euclidean norm if the vector is initialized, 0 otherwise.
+
+*/
+double vpColVector::euclideanNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<dsize;i++) {
+ x = *(data +i); norm += x*x;
+ }
+
+ return sqrt(norm);
+}
+
+/*!
+
+ Compute and return the infinity norm \f$ {||x||}_{\infty} =
+ max\left({\mid x_{i} \mid}\right) \f$ with \f$i \in
+ \{0, ..., m-1\}\f$ where \e m is the vector size and \f$x_i\f$ an element of the vector.
+
+ \return The infinity norm if the matrix is initialized, 0 otherwise.
+
+ \sa euclideanNorm()
+*/
+double vpColVector::infinityNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<rowNum;i++){
+ x = fabs ( (*this)[i] ) ;
+ if (x > norm) {
+ norm = x;
+ }
+ }
+ return norm;
+}
diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp
new file mode 100644
index 0000000..4a1374e
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix.cpp
@@ -0,0 +1,3575 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix manipulation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+/*!
+\file vpMatrix.cpp
+\brief Definition of the vpMatrix class
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <vector>
+#include <sstream>
+#include <algorithm>
+#include <assert.h>
+#include <fstream>
+#include <string>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_GSL
+#include <gsl/gsl_linalg.h>
+#endif
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpDebug.h>
+
+//Prototypes of specific functions
+vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
+
+
+/*!
+ Construct a matrix as a sub-matrix of the input matrix \e M.
+ \sa init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
+*/
+vpMatrix::vpMatrix(const vpMatrix &M,
+ unsigned int r, unsigned int c,
+ unsigned int nrows, unsigned int ncols)
+ : vpArray2D<double>()
+{
+ if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot construct a sub matrix (%dx%d) starting at position (%d,%d) that is not contained in the original matrix (%dx%d)",
+ nrows, ncols, r, c, M.rowNum, M.colNum)) ;
+ }
+
+ init(M, r, c, nrows, ncols);
+}
+
+/*!
+ Initialize the matrix from a part of an input matrix \e M.
+
+ \param M : Input matrix used for initialization.
+ \param r : row index in matrix M.
+ \param c : column index in matrix M.
+ \param nrows : Number of rows of the matrix that should be initialized.
+ \param ncols : Number of columns of the matrix that should be initialized.
+
+ The sub-matrix starting from M[r][c] element and ending on M[r+nrows-1][c+ncols-1] element
+ is used to initialize the matrix.
+
+ The following code shows how to use this function:
+\code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix M(4,5);
+ int val = 0;
+ for(size_t i=0; i<M.getRows(); i++) {
+ for(size_t j=0; j<M.getCols(); j++) {
+ M[i][j] = val++;
+ }
+ }
+ M.print (std::cout, 4, "M ");
+
+ vpMatrix N;
+ N.init(M, 0, 1, 2, 3);
+ N.print (std::cout, 4, "N ");
+}
+\endcode
+ It produces the following output:
+ \code
+M [4,5]=
+ 0 1 2 3 4
+ 5 6 7 8 9
+ 10 11 12 13 14
+ 15 16 17 18 19
+N [2,3]=
+ 1 2 3
+ 6 7 8
+ \endcode
+ */
+void
+vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
+{
+ unsigned int rnrows = r+nrows ;
+ unsigned int cncols = c+ncols ;
+
+ if (rnrows > M.getRows())
+ throw(vpException(vpException::dimensionError,
+ "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, M.getRows()));
+ if (cncols > M.getCols())
+ throw(vpException(vpException::dimensionError,
+ "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, M.getCols()));
+ resize(nrows, ncols);
+
+ if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
+ return; // Noting to do
+ for (unsigned int i=r ; i < rnrows; i++)
+ for (unsigned int j=c ; j < cncols; j++)
+ (*this)[i-r][j-c] = M[i][j] ;
+}
+
+/*!
+ Set an n-by-n matrix to identity with ones on the diagonal and zeros
+ else where.
+*/
+void
+vpMatrix::eye(unsigned int n)
+{
+ try {
+ eye(n, n);
+ }
+ catch(...) {
+ throw ;
+ }
+}
+
+/*!
+ Set an m-by-n matrix to identity with ones on the diagonal and zeros
+ else where.
+*/
+void
+vpMatrix::eye(unsigned int m, unsigned int n)
+{
+ try {
+ resize(m,n) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ eye();
+}
+
+/*!
+ Set an m-by-n matrix to identity with ones on the diagonal and zeros
+ else where.
+*/
+void
+vpMatrix::eye()
+{
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<colNum; j++) {
+ if (i == j) (*this)[i][j] = 1.0;
+ else (*this)[i][j] = 0;
+ }
+ }
+}
+
+/*!
+ Set the matrix diagonal elements to \e val.
+ More generally set M[i][i] = val.
+*/
+void
+vpMatrix::setIdentity(const double & val)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int j=0;j<colNum;j++)
+ if (i==j) (*this)[i][j] = val ;
+ else (*this)[i][j] = 0;
+}
+
+
+/*!
+ Compute and return the transpose of the matrix.
+*/
+vpMatrix vpMatrix::t() const
+{
+ vpMatrix At ;
+
+ try {
+ At.resize(colNum, rowNum);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++) {
+ double *coli = (*this)[i] ;
+ for (unsigned int j=0;j<colNum;j++)
+ At[j][i] = coli[j];
+ }
+ return At;
+}
+
+
+/*!
+ Compute and return the transpose of the matrix.
+
+ \sa t()
+*/
+vpMatrix vpMatrix::transpose()const
+{
+ vpMatrix At ;
+ transpose(At);
+ return At;
+}
+
+/*!
+ Compute \e At the transpose of the matrix.
+ \param At (output) : Resulting transpose matrix.
+ \sa t()
+*/
+void vpMatrix::transpose(vpMatrix & At ) const
+{
+ try {
+ At.resize(colNum,rowNum);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+
+ size_t A_step = colNum;
+ double ** AtRowPtrs = At.rowPtrs;
+
+ for( unsigned int i = 0; i < colNum; i++ ) {
+ double * row_ = AtRowPtrs[i];
+ double * col = rowPtrs[0]+i;
+ for( unsigned int j = 0; j < rowNum; j++, col+=A_step )
+ *(row_++)=*col;
+ }
+}
+
+
+/*!
+ Computes the \f$AA^T\f$ operation \f$B = A*A^T\f$
+ \return \f$A*A^T\f$
+ \sa AAt(vpMatrix &) const
+*/
+vpMatrix vpMatrix::AAt() const
+{
+ vpMatrix B;
+
+ AAt(B);
+
+ return B;
+}
+
+/*!
+ Compute the AAt operation such as \f$B = A*A^T\f$.
+
+ The result is placed in the parameter \e B and not returned.
+
+ A new matrix won't be allocated for every use of the function. This
+ results in a speed gain if used many times with the same result
+ matrix size.
+
+ \sa AAt()
+*/
+void vpMatrix::AAt(vpMatrix &B) const
+{
+ try {
+ if ((B.rowNum != rowNum) || (B.colNum != rowNum)) B.resize(rowNum,rowNum);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+
+ // compute A*A^T
+ for(unsigned int i=0;i<rowNum;i++){
+ for(unsigned int j=i;j<rowNum;j++){
+ double *pi = rowPtrs[i];// row i
+ double *pj = rowPtrs[j];// row j
+
+ // sum (row i .* row j)
+ double ssum=0;
+ for(unsigned int k=0; k < colNum ;k++)
+ ssum += *(pi++)* *(pj++);
+
+ B[i][j]=ssum; //upper triangle
+ if(i!=j)
+ B[j][i]=ssum; //lower triangle
+ }
+ }
+}
+
+/*!
+ Compute the AtA operation such as \f$B = A^T*A\f$.
+
+ The result is placed in the parameter \e B and not returned.
+
+ A new matrix won't be allocated for every use of the function. This
+ results in a speed gain if used many times with the same result matrix
+ size.
+
+ \sa AtA()
+*/
+void vpMatrix::AtA(vpMatrix &B) const
+{
+ try {
+ if ((B.rowNum != colNum) || (B.colNum != colNum)) B.resize(colNum,colNum);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+
+ unsigned int i,j,k;
+ double s;
+ double *ptr;
+ double *Bi;
+ for (i=0;i<colNum;i++)
+ {
+ Bi = B[i] ;
+ for (j=0;j<i;j++)
+ {
+ ptr=data;
+ s = 0 ;
+ for (k=0;k<rowNum;k++)
+ {
+ s +=(*(ptr+i)) * (*(ptr+j));
+ ptr+=colNum;
+ }
+ *Bi++ = s ;
+ B[j][i] = s;
+ }
+ ptr=data;
+ s = 0 ;
+ for (k=0;k<rowNum;k++)
+ {
+ s +=(*(ptr+i)) * (*(ptr+i));
+ ptr+=colNum;
+ }
+ *Bi = s;
+ }
+}
+
+
+/*!
+ Compute the AtA operation such as \f$B = A^T*A\f$
+ \return \f$A^T*A\f$
+ \sa AtA(vpMatrix &) const
+*/
+vpMatrix vpMatrix::AtA() const
+{
+ vpMatrix B;
+
+ AtA(B);
+
+ return B;
+}
+
+/*!
+ Copy operator that allows to convert on of the following container that
+ inherit from vpArray2D such as vpMatrix, vpRotationMatrix, vpHomogeneousMatrix,
+ vpPoseVector, vpColVector, vpRowVector... into a vpMatrix.
+
+ \param A : 2D array to be copied.
+
+ The following example shows how to create a matrix from an homogeneous matrix:
+ \code
+ vpRotationMatrix R;
+ vpMatrix M = R;
+ \endcode
+
+*/
+vpMatrix &
+vpMatrix::operator=(const vpArray2D<double> &A)
+{
+ try {
+ resize(A.getRows(), A.getCols()) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ memcpy(data, A.data, dsize*sizeof(double));
+
+ return *this;
+}
+
+//! Set all the element of the matrix A to \e x.
+vpMatrix &
+vpMatrix::operator=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] = x;
+
+ return *this;
+}
+
+
+/*!
+ Assigment from an array of double. This methos has to be used carefully since
+ the array allocated behind \e x pointer should have the same dimension than the matrix.
+*/
+vpMatrix &
+vpMatrix::operator<<( double *x )
+{
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<colNum; j++) {
+ rowPtrs[i][j] = *x++;
+ }
+ }
+ return *this;
+}
+
+/*!
+
+ Create a diagonal matrix with the element of a vector.
+
+ \param A : Vector which element will be put in the diagonal.
+
+ \sa createDiagonalMatrix()
+
+\code
+#include <iostream>
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A;
+ vpColVector v(3);
+
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+
+ A.diag(v);
+
+ std::cout << "A:\n" << A << std::endl;
+}
+\endcode
+
+ Matrix A is now equal to:
+\code
+1 0 0
+0 2 0
+0 0 3
+\endcode
+*/
+void
+vpMatrix::diag(const vpColVector &A)
+{
+ unsigned int rows = A.getRows() ;
+ try {
+ this->resize(rows,rows) ;
+ }
+ catch(...) {
+ throw ;
+ }
+ (*this) = 0 ;
+ for (unsigned int i=0 ; i< rows ; i++ )
+ (* this)[i][i] = A[i] ;
+}
+
+/*!
+
+ Set the matrix as a diagonal matrix where each element on the diagonal is set to \e val.
+ Elements that are not on the diagonal are set to 0.
+
+ \param val : Value to set.
+
+ \sa eye()
+
+\code
+#include <iostream>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3, 4);
+
+ A.diag(2);
+
+ std::cout << "A:\n" << A << std::endl;
+}
+\endcode
+
+ Matrix A is now equal to:
+\code
+2 0 0 0
+0 2 0 0
+0 0 2 0
+\endcode
+*/
+void
+vpMatrix::diag(const double &val)
+{
+ (*this) = 0;
+ unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
+ for (unsigned int i=0 ; i< min_ ; i++ )
+ (* this)[i][i] = val;
+}
+
+
+/*!
+
+ Create a diagonal matrix with the element of a vector \f$ DA_{ii} = A_i \f$.
+
+ \param A : Vector which element will be put in the diagonal.
+
+ \param DA : Diagonal matrix DA[i][i] = A[i]
+
+\sa diag()
+*/
+
+void
+vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
+{
+ unsigned int rows = A.getRows() ;
+ try {
+ DA.resize(rows,rows) ;
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ DA =0 ;
+ for (unsigned int i=0 ; i< rows ; i++ )
+ DA[i][i] = A[i] ;
+}
+
+/*!
+ Operator that allows to multiply a matrix by a translation vector.
+ The matrix should be of dimension (3x3)
+ */
+vpTranslationVector
+vpMatrix::operator*(const vpTranslationVector &tv) const
+{
+ vpTranslationVector t_out;
+
+ if (rowNum != 3 || colNum != 3) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
+ rowNum, colNum, tv.getRows(), tv.getCols())) ;
+ }
+
+ for (unsigned int j=0;j<3;j++) t_out[j]=0 ;
+
+ for (unsigned int j=0;j<3;j++) {
+ double tj = tv[j] ; // optimization em 5/12/2006
+ for (unsigned int i=0;i<3;i++) {
+ t_out[i]+=rowPtrs[i][j] * tj;
+ }
+ }
+ return t_out;
+}
+
+/*!
+ Operation w = A * v (matrix A is unchanged, v and w are column vectors).
+ \sa multMatrixVector() to avoid matrix allocation for each use.
+*/
+vpColVector
+vpMatrix::operator*(const vpColVector &v) const
+{
+ vpColVector v_out;
+ vpMatrix::multMatrixVector(*this, v, v_out);
+ return v_out;
+}
+
+/*!
+ Operation w = A * v (v and w are vectors).
+
+ A new matrix won't be allocated for every use of the function
+ (Speed gain if used many times with the same result matrix size).
+
+ \sa operator*(const vpColVector &v) const
+*/
+void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
+{
+ if (A.colNum != v.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
+ A.getRows(), A.getCols(), v.getRows())) ;
+ }
+
+ try {
+ if (A.rowNum != w.rowNum) w.resize(A.rowNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ w = 0.0;
+ for (unsigned int j=0;j<A.colNum;j++) {
+ double vj = v[j] ; // optimization em 5/12/2006
+ for (unsigned int i=0;i<A.rowNum;i++) {
+ w[i]+=A.rowPtrs[i][j] * vj;
+ }
+ }
+}
+
+//---------------------------------
+// Matrix operations.
+//---------------------------------
+
+/*!
+ Operation C = A * B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \sa operator*()
+*/
+void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if (A.colNum != B.rowNum) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ // 5/12/06 some "very" simple optimization to avoid indexation
+ unsigned int BcolNum = B.colNum;
+ unsigned int BrowNum = B.rowNum;
+ unsigned int i,j,k;
+ double **BrowPtrs = B.rowPtrs;
+ for (i=0;i<A.rowNum;i++)
+ {
+ double *rowptri = A.rowPtrs[i];
+ double *ci = C[i];
+ for (j=0;j<BcolNum;j++)
+ {
+ double s = 0;
+ for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ ci[j] = s;
+ }
+ }
+}
+
+/*!
+ \warning This function is provided for compat with previous releases. You should
+ rather use the functionalities provided in vpRotationMatrix class.
+
+ Operation C = A * B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \exception vpException::dimensionError If matrices are not 3-by-3 dimension.
+
+*/
+void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMatrix &C)
+{
+ if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ // 5/12/06 some "very" simple optimization to avoid indexation
+ unsigned int BcolNum = B.colNum;
+ unsigned int BrowNum = B.rowNum;
+ unsigned int i,j,k;
+ double **BrowPtrs = B.rowPtrs;
+ for (i=0;i<A.rowNum;i++)
+ {
+ double *rowptri = A.rowPtrs[i];
+ double *ci = C[i];
+ for (j=0;j<BcolNum;j++)
+ {
+ double s = 0;
+ for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ ci[j] = s;
+ }
+ }
+}
+
+/*!
+ \warning This function is provided for compat with previous releases. You should
+ rather use the functionalities provided in vpHomogeneousMatrix class.
+
+ Operation C = A * B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \exception vpException::dimensionError If matrices are not 4-by-4 dimension.
+
+*/
+void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneousMatrix &C)
+{
+ if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ // 5/12/06 some "very" simple optimization to avoid indexation
+ unsigned int BcolNum = B.colNum;
+ unsigned int BrowNum = B.rowNum;
+ unsigned int i,j,k;
+ double **BrowPtrs = B.rowPtrs;
+ for (i=0;i<A.rowNum;i++)
+ {
+ double *rowptri = A.rowPtrs[i];
+ double *ci = C[i];
+ for (j=0;j<BcolNum;j++)
+ {
+ double s = 0;
+ for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ ci[j] = s;
+ }
+ }
+}
+
+/*!
+ \warning This function is provided for compat with previous releases. You should
+ rather use multMatrixVector() that is more explicit.
+
+ Operation C = A * B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \sa multMatrixVector()
+*/
+void vpMatrix::mult2Matrices(const vpMatrix &A, const vpColVector &B, vpColVector &C)
+{
+ vpMatrix::multMatrixVector(A, B, C);
+}
+
+/*!
+ Operation C = A * B (A is unchanged).
+ \sa mult2Matrices() to avoid matrix allocation for each use.
+*/
+vpMatrix vpMatrix::operator*(const vpMatrix &B) const
+{
+ vpMatrix C;
+
+ vpMatrix::mult2Matrices(*this,B,C);
+
+ return C;
+}
+
+/*!
+ Operator that allow to multiply a matrix by a rotation matrix.
+ The matrix should be of dimension m-by-3.
+*/
+vpMatrix vpMatrix::operator*(const vpRotationMatrix &R) const
+{
+ if (colNum != R.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix",
+ rowNum, colNum)) ;
+ }
+ vpMatrix C(rowNum, 3);
+
+ unsigned int RcolNum = R.getCols();
+ unsigned int RrowNum = R.getRows();
+ for (unsigned int i=0;i<rowNum;i++)
+ {
+ double *rowptri = rowPtrs[i];
+ double *ci = C[i];
+ for (unsigned int j=0;j<RcolNum;j++)
+ {
+ double s = 0;
+ for (unsigned int k=0;k<RrowNum;k++) s += rowptri[k] * R[k][j];
+ ci[j] = s;
+ }
+ }
+
+ return C;
+}
+/*!
+ Operator that allow to multiply a matrix by a velocity twist matrix.
+ The matrix should be of dimension m-by-6.
+*/
+vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const
+{
+ if (colNum != V.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (3x3) velocity twist matrix",
+ rowNum, colNum)) ;
+ }
+ vpMatrix M(rowNum, 6);
+
+ unsigned int VcolNum = V.getCols();
+ unsigned int VrowNum = V.getRows();
+ for (unsigned int i=0;i<rowNum;i++)
+ {
+ double *rowptri = rowPtrs[i];
+ double *ci = M[i];
+ for (unsigned int j=0;j<VcolNum;j++)
+ {
+ double s = 0;
+ for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
+ ci[j] = s;
+ }
+ }
+
+ return M;
+}
+/*!
+ Operator that allow to multiply a matrix by a force/torque twist matrix.
+ The matrix should be of dimension m-by-6.
+*/
+vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
+{
+ if (colNum != V.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (%dx%d) matrix by (3x3) force/torque twist matrix",
+ rowNum, colNum)) ;
+ }
+ vpMatrix M(rowNum, 6);
+
+ unsigned int VcolNum = V.getCols();
+ unsigned int VrowNum = V.getRows();
+ for (unsigned int i=0;i<rowNum;i++)
+ {
+ double *rowptri = rowPtrs[i];
+ double *ci = M[i];
+ for (unsigned int j=0;j<VcolNum;j++)
+ {
+ double s = 0;
+ for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
+ ci[j] = s;
+ }
+ }
+
+ return M;
+}
+
+/*!
+Operation C = A*wA + B*wB
+
+The result is placed in the third parameter C and not returned.
+A new matrix won't be allocated for every use of the function
+(Speed gain if used many times with the same result matrix size).
+
+\sa operator+()
+*/
+
+void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C){
+ try
+ {
+ if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx%d) matrix with (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ double ** ArowPtrs=A.rowPtrs;
+ double ** BrowPtrs=B.rowPtrs;
+ double ** CrowPtrs=C.rowPtrs;
+
+ for (unsigned int i=0;i<A.rowNum;i++)
+ for(unsigned int j=0;j<A.colNum;j++)
+ CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];
+}
+
+/*!
+ Operation C = A + B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \sa operator+()
+*/
+void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx%d) matrix with (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ double ** ArowPtrs=A.rowPtrs;
+ double ** BrowPtrs=B.rowPtrs;
+ double ** CrowPtrs=C.rowPtrs;
+
+ for (unsigned int i=0;i<A.rowNum;i++) {
+ for(unsigned int j=0;j<A.colNum;j++) {
+ CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
+ }
+ }
+}
+
+/*!
+ \warning This function is provided for compat with previous releases. You should
+ rather use the functionalities provided in vpColVector class.
+
+ Operation C = A + B.
+
+ The result is placed in the third parameter C and not returned.
+ A new vector won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \sa vpColVector::operator+()
+*/
+void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx%d) matrix with (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ double ** ArowPtrs=A.rowPtrs;
+ double ** BrowPtrs=B.rowPtrs;
+ double ** CrowPtrs=C.rowPtrs;
+
+ for (unsigned int i=0;i<A.rowNum;i++) {
+ for(unsigned int j=0;j<A.colNum;j++) {
+ CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
+ }
+ }
+}
+
+/*!
+ Operation C = A + B (A is unchanged).
+ \sa add2Matrices() to avoid matrix allocation for each use.
+*/
+vpMatrix vpMatrix::operator+(const vpMatrix &B) const
+{
+ vpMatrix C;
+ vpMatrix::add2Matrices(*this,B,C);
+ return C;
+}
+
+
+/*!
+ \warning This function is provided for compat with previous releases. You should
+ rather use the functionalities provided in vpColVector class.
+
+ Operation C = A - B on column vectors.
+
+ The result is placed in the third parameter C and not returned.
+ A new vector won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \exception vpException::dimensionError If A and B vectors have not the same size.
+
+ \sa vpColVector::operator-()
+*/
+void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ double ** ArowPtrs=A.rowPtrs;
+ double ** BrowPtrs=B.rowPtrs;
+ double ** CrowPtrs=C.rowPtrs;
+
+ for (unsigned int i=0;i<A.rowNum;i++) {
+ for(unsigned int j=0;j<A.colNum;j++) {
+ CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
+ }
+ }
+}
+
+/*!
+ Operation C = A - B.
+
+ The result is placed in the third parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (speed gain if used many times with the same result matrix size).
+
+ \exception vpException::dimensionError If A and B matrices have not the same size.
+
+ \sa operator-()
+*/
+void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ double ** ArowPtrs=A.rowPtrs;
+ double ** BrowPtrs=B.rowPtrs;
+ double ** CrowPtrs=C.rowPtrs;
+
+ for (unsigned int i=0;i<A.rowNum;i++) {
+ for(unsigned int j=0;j<A.colNum;j++) {
+ CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
+ }
+ }
+}
+
+/*!
+ Operation C = A - B (A is unchanged).
+ \sa sub2Matrices() to avoid matrix allocation for each use.
+*/
+vpMatrix vpMatrix::operator-(const vpMatrix &B) const
+{
+ vpMatrix C;
+ vpMatrix::sub2Matrices(*this,B,C);
+ return C;
+}
+
+//! Operation A = A + B
+
+vpMatrix &vpMatrix::operator+=(const vpMatrix &B)
+{
+ if ( (colNum != B.getCols())||(rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (%dx%d) matrix to (%dx%d) matrix",
+ rowNum, colNum, B.getRows(), B.getCols())) ;
+ }
+
+ double ** BrowPtrs=B.rowPtrs;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] += BrowPtrs[i][j];
+
+ return *this;
+}
+
+//! Operation A = A - B
+vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
+{
+ if ( (colNum != B.getCols())||(rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
+ rowNum, colNum, B.getRows(), B.getCols())) ;
+ }
+
+ double ** BrowPtrs=B.rowPtrs;
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] -= BrowPtrs[i][j];
+
+ return *this;
+}
+
+/*!
+ Operation C = -A.
+
+ The result is placed in the second parameter C and not returned.
+ A new matrix won't be allocated for every use of the function
+ (Speed gain if used many times with the same result matrix size).
+
+ \sa operator-(void)
+*/
+void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C)
+{
+ try {
+ if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ 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];
+}
+
+/*!
+ Operation C = -A (A is unchanged).
+ \sa negateMatrix() to avoid matrix allocation for each use.
+*/
+vpMatrix vpMatrix::operator-() const //negate
+{
+ vpMatrix C;
+ vpMatrix::negateMatrix(*this,C);
+ return C;
+}
+
+
+double
+vpMatrix::sum() const
+{
+ double s=0.0;
+ for (unsigned int i=0;i<rowNum;i++)
+ {
+ for(unsigned int j=0;j<colNum;j++)
+ {
+ s += rowPtrs[i][j];
+ }
+ }
+
+ return s;
+}
+
+
+//---------------------------------
+// Matrix/vector operations.
+//---------------------------------
+
+
+
+
+//---------------------------------
+// Matrix/real operations.
+//---------------------------------
+
+/*!
+ \relates vpMatrix
+ Allow to multiply a scalar by a matrix.
+*/
+vpMatrix operator*(const double &x,const vpMatrix &B)
+{
+ vpMatrix C(B.getRows(), B.getCols());
+
+ unsigned int Brow = B.getRows() ;
+ unsigned int Bcol = B.getCols() ;
+
+ for (unsigned int i=0;i<Brow;i++)
+ for(unsigned int j=0;j<Bcol;j++)
+ C[i][j]= B[i][j]*x;
+
+ return C ;
+}
+
+/*!
+ Operator that allows to multiply all the elements of a matrix
+ by a scalar.
+ */
+vpMatrix vpMatrix::operator*(double x) const
+{
+ vpMatrix M(rowNum,colNum);
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ M[i][j]= rowPtrs[i][j]*x;
+
+ return M;
+}
+
+//! Cij = Aij / x (A is unchanged)
+vpMatrix vpMatrix::operator/(double x) const
+{
+ vpMatrix C;
+
+ try {
+ C.resize(rowNum,colNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ //if (x == 0) {
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
+ throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
+ }
+
+ double xinv = 1/x ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ C[i][j]=rowPtrs[i][j]*xinv;
+
+ return C;
+}
+
+
+//! Add x to all the element of the matrix : Aij = Aij + x
+vpMatrix & vpMatrix::operator+=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j]+=x;
+
+ return *this;
+}
+
+
+//! Substract x to all the element of the matrix : Aij = Aij - x
+vpMatrix & vpMatrix::operator-=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j]-=x;
+
+ return *this;
+}
+
+/*!
+ Operator that allows to multiply all the elements of a matrix
+ by a scalar.
+ */
+vpMatrix & vpMatrix::operator*=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j]*=x;
+
+ return *this;
+}
+
+//! Divide all the element of the matrix by x : Aij = Aij / x
+vpMatrix & vpMatrix::operator/=(double x)
+{
+ //if (x == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
+ throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
+
+ double xinv = 1/x ;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j]*=xinv;
+
+ return *this;
+}
+
+//----------------------------------------------------------------
+// Matrix Operation
+//----------------------------------------------------------------
+
+
+
+
+
+
+
+/*!
+ Stacks columns of a matrix in a vector.
+ \param out : a vpColVector.
+*/
+void vpMatrix::stackColumns(vpColVector &out ){
+
+ try {
+ if ((out.rowNum != colNum*rowNum) || (out.colNum != 1)) out.resize(rowNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ double *optr=out.data;
+ for(unsigned int j =0;j<colNum ; j++){
+ for(unsigned int i =0;i<rowNum ; i++){
+ *(optr++)=rowPtrs[i][j];
+ }
+ }
+}
+
+/*!
+ Stacks columns of a matrix in a vector.
+ \return a vpColVector.
+*/
+vpColVector vpMatrix::stackColumns()
+{
+ vpColVector out(colNum*rowNum);
+ stackColumns(out);
+ return out;
+}
+
+/*!
+ Stacks rows of a matrix in a vector
+ \param out : a vpRowVector.
+*/
+void vpMatrix::stackRows(vpRowVector &out)
+{
+ try {
+ if ((out.getRows() != 1) || (out.getCols() != colNum*rowNum)) out.resize(rowNum);
+ }
+ catch(...) {
+ throw ;
+ }
+
+ double *mdata=data;
+ double *optr=out.data;
+ for(unsigned int i =0;i<dsize ; i++){
+ *(optr++)=*(mdata++);
+ }
+}
+/*!
+ Stacks rows of a matrix in a vector.
+ \return a vpRowVector.
+*/
+vpRowVector vpMatrix::stackRows()
+{
+ vpRowVector out(colNum*rowNum);
+ stackRows(out );
+ return out;
+}
+
+/*!
+ Compute Kronecker product matrix.
+ \param m1 : vpMatrix;
+ \param m2 : vpMatrix;
+ \param out : The kronecker product : \f$ m1 \otimes m2 \f$
+*/
+void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out)
+{
+ unsigned int r1= m1.getRows();
+ unsigned int c1= m1.getCols();
+ unsigned int r2= m2.getRows();
+ unsigned int c2= m2.getCols();
+
+ if (r1*r2 !=out.rowNum || c1*c2!= out.colNum )
+ {
+ vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix") ;
+ throw(vpException(vpException::dimensionError,
+ "In Kronecker product bad dimension of output matrix"));
+ }
+
+ for(unsigned int r =0;r<r1 ; r++){
+ for(unsigned int c =0;c<c1 ; c++){
+ double alpha = m1[r][c];
+ double *m2ptr = m2[0];
+ unsigned int roffset= r*r2;
+ unsigned int coffset= c*c2;
+ for(unsigned int rr =0;rr<r2 ; rr++){
+ for(unsigned int cc =0;cc<c2 ;cc++){
+ out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
+ }
+ }
+ }
+ }
+
+}
+
+/*!
+ Compute Kronecker product matrix.
+ \param m : vpMatrix.
+ \param out : If m1.kron(m2) out contains the kronecker product's result : \f$ m1 \otimes m2 \f$.
+*/
+void vpMatrix::kron(const vpMatrix &m , vpMatrix &out) const
+{
+ kron(*this,m,out);
+}
+
+/*!
+ Compute Kronecker product matrix.
+ \param m1 : vpMatrix;
+ \param m2 : vpMatrix;
+ \return The kronecker product : \f$ m1 \otimes m2 \f$
+*/
+vpMatrix vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2)
+{
+ unsigned int r1= m1.getRows();
+ unsigned int c1= m1.getCols();
+ unsigned int r2= m2.getRows();
+ unsigned int c2= m2.getCols();
+
+ vpMatrix out(r1*r2,c1*c2);
+
+ for(unsigned int r =0;r<r1 ; r++){
+ for(unsigned int c =0;c<c1 ; c++){
+ double alpha = m1[r][c];
+ double *m2ptr = m2[0];
+ unsigned int roffset= r*r2;
+ unsigned int coffset= c*c2;
+ for(unsigned int rr =0;rr<r2 ; rr++){
+ for(unsigned int cc =0;cc<c2 ;cc++){
+ out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
+ }
+ }
+ }
+ }
+ return out;
+}
+
+
+/*!
+ Compute Kronecker product matrix.
+ \param m : vpMatrix;
+ \return m1.kron(m2) The kronecker product : \f$ m1 \otimes m2 \f$
+*/
+vpMatrix vpMatrix::kron(const vpMatrix &m) const
+{
+ return kron(*this,m);
+}
+
+/*!
+
+ Solve a linear system \f$ A X = B \f$ using Singular Value
+ Decomposition (SVD).
+
+ Non destructive wrt. A and B.
+
+ \param b : Vector\f$ B \f$.
+
+ \param x : Vector \f$ X \f$.
+
+ Here an example:
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+vpMatrix A(3,3);
+
+A[0][0] = 4.64;
+A[0][1] = 0.288;
+A[0][2] = -0.384;
+
+A[1][0] = 0.288;
+A[1][1] = 7.3296;
+A[1][2] = 2.2272;
+
+A[2][0] = -0.384;
+A[2][1] = 2.2272;
+A[2][2] = 6.0304;
+
+vpColVector X(3), B(3);
+B[0] = 1;
+B[1] = 2;
+B[2] = 3;
+
+A.solveBySVD(B, X);
+
+// Obtained values of X
+// X[0] = 0.2468;
+// X[1] = 0.120782;
+// X[2] = 0.468587;
+
+std::cout << "X:\n" << X << std::endl;
+}
+\endcode
+
+\sa solveBySVD(const vpColVector &)
+*/
+void
+vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const
+{
+ x = pseudoInverse(1e-6)*b ;
+}
+
+
+/*!
+
+ Solve a linear system \f$ A X = B \f$ using Singular Value
+ Decomposition (SVD).
+
+ Non destructive wrt. A and B.
+
+ \param B : Vector\f$ B \f$.
+
+ \return Vector \f$ X \f$.
+
+ Here an example:
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+vpMatrix A(3,3);
+
+A[0][0] = 4.64;
+A[0][1] = 0.288;
+A[0][2] = -0.384;
+
+A[1][0] = 0.288;
+A[1][1] = 7.3296;
+A[1][2] = 2.2272;
+
+A[2][0] = -0.384;
+A[2][1] = 2.2272;
+A[2][2] = 6.0304;
+
+vpColVector X(3), B(3);
+B[0] = 1;
+B[1] = 2;
+B[2] = 3;
+
+X = A.solveBySVD(B);
+// Obtained values of X
+// X[0] = 0.2468;
+// X[1] = 0.120782;
+// X[2] = 0.468587;
+
+std::cout << "X:\n" << X << std::endl;
+}
+\endcode
+
+\sa solveBySVD(const vpColVector &, vpColVector &)
+*/
+vpColVector vpMatrix::solveBySVD(const vpColVector &B) const
+{
+ vpColVector X(colNum);
+
+ solveBySVD(B, X);
+ return X;
+}
+
+
+/*!
+
+ Singular value decomposition (SVD).
+
+ \f[ M = U \Sigma V^{\top} \f]
+
+ \warning Destructive method wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed not to
+ CHANGE.
+
+ \param w : Vector of singular values. \f$ \Sigma = diag(w) \f$.
+
+ \param v : Matrix \f$ V \f$.
+
+ \return Matrix \f$ U \f$.
+
+ \warning If the GNU Scientific Library (GSL) third party library is used to compute the SVD
+ decomposition, the singular values \f$ \Sigma_{i,i} \f$ are ordered in decreasing
+ fashion in \e w. This is not the case, if the GSL is not detected by ViSP.
+
+ Here an example of SVD decomposition of a non square Matrix M.
+
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix M(3,2);
+ M[0][0] = 1;
+ M[1][0] = 2;
+ M[2][0] = 0.5;
+
+ M[0][1] = 6;
+ M[1][1] = 8 ;
+ M[2][1] = 9 ;
+
+ vpMatrix v;
+ vpColVector w;
+ vpMatrix Mrec;
+ vpMatrix Sigma;
+
+ M.svd(w, v);
+ // Here M is modified and is now equal to U
+
+ // Construct the diagonal matrix from the singular values
+ Sigma.diag(w);
+
+ // Reconstruct the initial matrix M using the decomposition
+ Mrec = M * Sigma * v.t();
+
+ // Here, Mrec is obtained equal to the initial value of M
+ // Mrec[0][0] = 1;
+ // Mrec[1][0] = 2;
+ // Mrec[2][0] = 0.5;
+ // Mrec[0][1] = 6;
+ // Mrec[1][1] = 8 ;
+ // Mrec[2][1] = 9 ;
+
+ std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
+}
+\endcode
+
+*/
+void
+vpMatrix::svd(vpColVector& w, vpMatrix& v)
+{
+#if 1 /* no verification */
+ {
+ w.resize( this->getCols() );
+ v.resize( this->getCols(), this->getCols() );
+
+#if defined (VISP_HAVE_LAPACK_C)
+ svdLapack(w,v);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ svdOpenCV(w,v);
+#elif defined (VISP_HAVE_GSL) /* be careful of the copy below */
+ svdGsl(w,v) ;
+#else
+ svdNr(w,v) ;
+#endif
+
+ //svdNr(w,v) ;
+ }
+#else /* verification of the SVD */
+ {
+ int pb = 0;
+ unsigned int i,j,k,nrows,ncols;
+ vpMatrix A, Asvd;
+
+ A = (*this); /* copy because svd is destructive */
+
+ w.resize( this->getCols() );
+ v.resize( this->getCols(), this->getCols() );
+#ifdef VISP_HAVE_GSL /* be careful of the copy above */
+ svdGsl(w,v) ;
+#else
+ svdNr(w,v) ;
+#endif
+ //svdNr(w,v) ;
+
+ nrows = A.getRows();
+ ncols = A.getCols();
+ Asvd.resize(nrows,ncols);
+
+ for (i = 0 ; i < nrows ; i++)
+ {
+ for (j = 0 ; j < ncols ; j++)
+ {
+ Asvd[i][j] = 0.0;
+ for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*this)[i][k]*w[k]*v[j][k];
+ }
+ }
+ for (i=0;i<nrows;i++)
+ {
+ for (j=0;j<ncols;j++) if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
+ }
+ if (pb == 1)
+ {
+ printf("pb in SVD\n");
+ std::cout << " A : " << std::endl << A << std::endl;
+ std::cout << " Asvd : " << std::endl << Asvd << std::endl;
+ }
+ // else printf("SVD ok ;-)\n"); /* It's so good... */
+ }
+#endif
+}
+/*!
+ Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
+ \param Ap : The pseudo inverse \f$ A^+ \f$.
+ \param svThreshold : Threshold used to test the singular values.
+ \return Return the rank of the matrix A
+*/
+
+unsigned int
+vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
+{
+ vpColVector sv ;
+ return pseudoInverse(Ap, sv, svThreshold) ;
+}
+
+/*!
+ Compute and return the pseudo inverse of a n-by-m matrix : \f$ A^+ \f$
+ \param svThreshold : Threshold used to test the singular values.
+
+ \return Pseudo inverse of the matrix.
+
+ Here an example to compute the inverse of a n-by-n matrix. If the
+ matrix is n-by-n it is also possible to use inverseByLU().
+
+\code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.pseudoInverse();
+ std::cout << "Inverse by pseudo inverse: \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+\endcode
+
+ \sa inverseByLU()
+
+*/
+vpMatrix
+vpMatrix::pseudoInverse(double svThreshold) const
+{
+ vpMatrix Ap ;
+ vpColVector sv ;
+ pseudoInverse(Ap, sv, svThreshold) ;
+ return Ap ;
+}
+
+/*!
+ Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
+ \param Ap : The pseudo inverse \f$ A^+ \f$.
+ \param sv : Singular values.
+ \param svThreshold : Threshold used to test the singular values.
+ \return Return the rank of the matrix A
+*/
+unsigned int
+vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+{
+ vpMatrix imA, imAt ;
+ return pseudoInverse(Ap, sv, svThreshold, imA, imAt) ;
+}
+
+/*!
+ Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
+
+ Pseudo inverse, kernel and image are computed using the SVD decomposition.
+
+ A is an m x n matrix,
+ if m >=n the svd works on A other wise it works on \f$A^T\f$.
+
+ Therefore if m>=n we have
+
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
+ \f]
+ \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
+ \mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
+ \left[
+ \begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
+ \f]
+ where
+ Im(A) is an m x r matrix (r is the rank of A) and
+ Im(A^T) is an r x n matrix
+
+ \param Ap : The pseudo inverse \f$ A^+ \f$.
+ \param sv : Singular values.
+ \param svThreshold : Threshold used to test the singular values.
+ \param imAt : Image A^T
+ \param imA: Image A
+ \return Return the rank of the matrix A
+
+*/
+unsigned int
+vpMatrix::pseudoInverse(vpMatrix &Ap,
+ vpColVector &sv, double svThreshold,
+ vpMatrix &imA,
+ vpMatrix &imAt) const
+{
+
+ unsigned int i, j, k ;
+
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows() ;
+ unsigned int ncols_orig = getCols() ;
+ Ap.resize(ncols_orig,nrows_orig) ;
+
+ if (nrows_orig >= ncols_orig)
+ {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ }
+ else
+ {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix a(nrows,ncols) ;
+ vpMatrix a1(ncols,nrows);
+ vpMatrix v(ncols,ncols) ;
+ sv.resize(ncols) ;
+
+ if (nrows_orig >= ncols_orig) a = *this;
+ else a = (*this).t();
+
+ a.svd(sv,v);
+
+ // compute the highest singular value and the rank of h
+ double maxsv = 0 ;
+ for (i=0 ; i < ncols ; i++)
+ if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+
+ unsigned int rank = 0 ;
+ for (i=0 ; i < ncols ; i++)
+ if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+
+ /*------------------------------------------------------- */
+ for (i = 0 ; i < ncols ; i++)
+ {
+ for (j = 0 ; j < nrows ; j++)
+ {
+ a1[i][j] = 0.0;
+
+ for (k=0 ; k < ncols ; k++)
+ if (fabs(sv[k]) > maxsv*svThreshold)
+ {
+ a1[i][j] += v[i][k]*a[j][k]/sv[k];
+ }
+ }
+ }
+ if (nrows_orig >= ncols_orig) Ap = a1;
+ else Ap = a1.t();
+
+ if (nrows_orig >= ncols_orig)
+ {
+ // compute dim At
+ imAt.resize(ncols_orig,rank) ;
+ for (i=0 ; i < ncols_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imAt[i][j] = v[i][j] ;
+
+ // compute dim A
+ imA.resize(nrows_orig,rank) ;
+ for (i=0 ; i < nrows_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imA[i][j] = a[i][j] ;
+ }
+ else
+ {
+ // compute dim At
+ imAt.resize(ncols_orig,rank) ;
+ for (i=0 ; i < ncols_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imAt[i][j] = a[i][j] ;
+
+ imA.resize(nrows_orig,rank) ;
+ for (i=0 ; i < nrows_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imA[i][j] = v[i][j] ;
+
+ }
+
+#if 0 // debug
+ {
+ int pb = 0;
+ vpMatrix A, ApA, AAp, AApA, ApAAp ;
+
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+
+ A.resize(nrows,ncols) ;
+ A = *this ;
+
+ ApA = Ap * A;
+ AApA = A * ApA;
+ ApAAp = ApA * Ap;
+ AAp = A * Ap;
+
+ for (i=0;i<nrows;i++)
+ {
+ for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<ncols;i++)
+ {
+ for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<nrows;i++)
+ {
+ for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<ncols;i++)
+ {
+ for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
+ }
+ if (pb == 1)
+ {
+ printf("pb in pseudo inverse\n");
+ std::cout << " A : " << std::endl << A << std::endl;
+ std::cout << " Ap : " << std::endl << Ap << std::endl;
+ std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
+ std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
+ std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
+ std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
+ }
+ // else printf("Ap OK ;-) \n");
+
+ }
+#endif
+
+ // std::cout << v << std::endl ;
+ return rank ;
+}
+
+
+
+/*!
+ Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
+
+ Pseudo inverse, kernel and image are computed using the SVD decomposition.
+
+ A is an m x n matrix,
+ if m >=n the svd works on A other wise it works on \f$A^T\f$.
+
+ Therefore if m>=n we have
+
+\f[
+{\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
+\f]
+\f[
+{\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
+\mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
+\left[
+\begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
+\f]
+where
+Im(A) is an m x r matrix (r is the rank of A) and
+Im(A^T) is an r x n matrix
+
+
+ \param Ap : The pseudo inverse \f$ A^+ \f$.
+ \param sv : Singular values.
+ \param svThreshold : Threshold used to test the singular values.
+ \param imA: Image A
+ \param imAt : Image A^T
+ \param kerA : null space of A
+ \return Return the rank of the matrix A
+
+*/
+unsigned int
+vpMatrix::pseudoInverse(vpMatrix &Ap,
+ vpColVector &sv, double svThreshold,
+ vpMatrix &imA,
+ vpMatrix &imAt,
+ vpMatrix &kerA) const
+{
+
+ unsigned int i, j, k ;
+
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows() ;
+ unsigned int ncols_orig = getCols() ;
+ Ap.resize(ncols_orig,nrows_orig) ;
+
+ if (nrows_orig >= ncols_orig)
+ {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ }
+ else
+ {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix a(nrows,ncols) ;
+ vpMatrix a1(ncols,nrows);
+ vpMatrix v(ncols,ncols) ;
+ sv.resize(ncols) ;
+
+ if (nrows_orig >= ncols_orig) a = *this;
+ else a = (*this).t();
+
+ a.svd(sv,v);
+
+ // compute the highest singular value and the rank of h
+ double maxsv = 0 ;
+ for (i=0 ; i < ncols ; i++)
+ if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+
+ unsigned int rank = 0 ;
+ for (i=0 ; i < ncols ; i++)
+ if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+
+
+
+ /*------------------------------------------------------- */
+ for (i = 0 ; i < ncols ; i++)
+ {
+ for (j = 0 ; j < nrows ; j++)
+ {
+ a1[i][j] = 0.0;
+
+ for (k=0 ; k < ncols ; k++)
+ if (fabs(sv[k]) > maxsv*svThreshold)
+ {
+ a1[i][j] += v[i][k]*a[j][k]/sv[k];
+ }
+ }
+ }
+ if (nrows_orig >= ncols_orig) Ap = a1;
+ else Ap = a1.t();
+
+ if (nrows_orig >= ncols_orig)
+ {
+ // compute dim At
+ imAt.resize(ncols_orig,rank) ;
+ for (i=0 ; i < ncols_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imAt[i][j] = v[i][j] ;
+
+ // compute dim A
+ imA.resize(nrows_orig,rank) ;
+ for (i=0 ; i < nrows_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imA[i][j] = a[i][j] ;
+ }
+ else
+ {
+ // compute dim At
+ imAt.resize(ncols_orig,rank) ;
+ for (i=0 ; i < ncols_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imAt[i][j] = a[i][j] ;
+
+ imA.resize(nrows_orig,rank) ;
+ for (i=0 ; i < nrows_orig ; i++)
+ for (j=0 ; j < rank ; j++)
+ imA[i][j] = v[i][j] ;
+
+ }
+
+ vpMatrix cons(ncols_orig, ncols_orig);
+ cons = 0;
+
+ for (j = 0; j < ncols_orig; j++)
+ {
+ for (i = 0; i < ncols_orig; i++)
+ {
+ if (fabs(sv[i]) <= maxsv*svThreshold)
+ {
+ cons[i][j] = v[j][i];
+ }
+ }
+ }
+
+ vpMatrix Ker (ncols_orig-rank, ncols_orig);
+ k = 0;
+ for (j = 0; j < ncols_orig ; j++)
+ {
+ //if ( cons.row(j+1).sumSquare() != 0)
+ if ( std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
+ {
+ for (i = 0; i < cons.getCols(); i++)
+ Ker[k][i] = cons[j][i];
+
+ k++;
+ }
+ }
+ kerA = Ker;
+
+#if 0 // debug
+ {
+ int pb = 0;
+ vpMatrix A, ApA, AAp, AApA, ApAAp ;
+
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+
+ A.resize(nrows,ncols) ;
+ A = *this ;
+
+ ApA = Ap * A;
+ AApA = A * ApA;
+ ApAAp = ApA * Ap;
+ AAp = A * Ap;
+
+ for (i=0;i<nrows;i++)
+ {
+ for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<ncols;i++)
+ {
+ for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<nrows;i++)
+ {
+ for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
+ }
+ for (i=0;i<ncols;i++)
+ {
+ for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
+ }
+ if (pb == 1)
+ {
+ printf("pb in pseudo inverse\n");
+ std::cout << " A : " << std::endl << A << std::endl;
+ std::cout << " Ap : " << std::endl << Ap << std::endl;
+ std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
+ std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
+ std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
+ std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
+ std::cout << " KerA : " << std::endl << kerA << std::endl;
+ }
+ // else printf("Ap OK ;-) \n");
+
+ }
+#endif
+
+ // std::cout << v << std::endl ;
+ return rank ;
+}
+
+/*!
+ Extract a column vector from a matrix.
+ \warning All the indexes start from 0 in this function.
+ \param j : Index of the column to extract. If col=0, the first column is extracted.
+ \param i_begin : Index of the row that gives the location of the first element of the column vector to extract.
+ \param column_size : Size of the column vector to extract.
+ \return The extracted column vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ for(unsigned int i=0; i < A.getRows(); i++)
+ for(unsigned int j=0; j < A.getCols(); j++)
+ A[i][j] = i*A.getCols()+j;
+
+ A.print(std::cout, 4);
+
+ vpColVector cv = A.getCol(1, 1, 3);
+ std::cout << "Column vector: \n" << cv << std::endl;
+}
+ \endcode
+It produces the following output:
+ \code
+[4,4]=
+ 0 1 2 3
+ 4 5 6 7
+ 8 9 10 11
+ 12 13 14 15
+column vector:
+5
+9
+13
+ \endcode
+ */
+vpColVector
+vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
+{
+ if (i_begin + column_size > getRows() || j >= getCols())
+ throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
+ vpColVector c(column_size);
+ for (unsigned int i=0 ; i < column_size ; i++)
+ c[i] = (*this)[i_begin+i][j];
+ return c;
+}
+
+/*!
+ Extract a column vector from a matrix.
+ \warning All the indexes start from 0 in this function.
+ \param j : Index of the column to extract. If j=0, the first column is extracted.
+ \return The extracted column vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ for(unsigned int i=0; i < A.getRows(); i++)
+ for(unsigned int j=0; j < A.getCols(); j++)
+ A[i][j] = i*A.getCols()+j;
+
+ A.print(std::cout, 4);
+
+ vpColVector cv = A.getCol(1);
+ std::cout << "Column vector: \n" << cv << std::endl;
+}
+ \endcode
+It produces the following output:
+ \code
+[4,4]=
+ 0 1 2 3
+ 4 5 6 7
+ 8 9 10 11
+ 12 13 14 15
+column vector:
+1
+5
+9
+13
+ \endcode
+ */
+vpColVector
+vpMatrix::getCol(const unsigned int j) const
+{
+ if (j >= getCols())
+ throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
+ unsigned int nb_rows = getRows();
+ vpColVector c(nb_rows);
+ for (unsigned int i=0 ; i < nb_rows ; i++)
+ c[i] = (*this)[i][j];
+ return c;
+}
+
+/*!
+ Extract a row vector from a matrix.
+ \warning All the indexes start from 0 in this function.
+ \param i : Index of the row to extract. If i=0, the first row is extracted.
+ \return The extracted row vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRowVector.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ for(unsigned int i=0; i < A.getRows(); i++)
+ for(unsigned int j=0; j < A.getCols(); j++)
+ A[i][j] = i*A.getCols()+j;
+
+ A.print(std::cout, 4);
+
+ vpRowVector rv = A.getRow(1);
+ std::cout << "Row vector: \n" << rv << std::endl;
+} \endcode
+It produces the following output:
+ \code
+[4,4]=
+ 0 1 2 3
+ 4 5 6 7
+ 8 9 10 11
+ 12 13 14 15
+Row vector:
+4 5 6 7
+ \endcode
+ */
+vpRowVector
+vpMatrix::getRow(const unsigned int i) const
+{
+ if (i >= getRows())
+ throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
+ unsigned int nb_cols = getCols();
+ vpRowVector r( nb_cols );
+ for (unsigned int j=0 ; j < nb_cols ; j++)
+ r[j] = (*this)[i][j];
+ return r;
+}
+
+/*!
+ Extract a row vector from a matrix.
+ \warning All the indexes start from 0 in this function.
+ \param i : Index of the row to extract. If i=0, the first row is extracted.
+ \param j_begin : Index of the column that gives the location of the first element of the row vector to extract.
+ \param row_size : Size of the row vector to extract.
+ \return The extracted row vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRowVector.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ for(unsigned int i=0; i < A.getRows(); i++)
+ for(unsigned int j=0; j < A.getCols(); j++)
+ A[i][j] = i*A.getCols()+j;
+
+ A.print(std::cout, 4);
+
+ vpRowVector rv = A.getRow(1, 1, 3);
+ std::cout << "Row vector: \n" << rv << std::endl;
+} \endcode
+It produces the following output:
+ \code
+[4,4]=
+ 0 1 2 3
+ 4 5 6 7
+ 8 9 10 11
+ 12 13 14 15
+Row vector:
+5 6 7
+ \endcode
+ */
+vpRowVector
+vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
+{
+ if (j_begin + row_size > getCols() || i >= getRows())
+ throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
+ vpRowVector r(row_size);
+ for (unsigned int j=0 ; j < row_size ; j++)
+ r[j] = (*this)[i][j_begin+i];
+ return r;
+}
+
+/*!
+ Stack matrix \e B to the end of matrix \e A and return the resulting matrix [ A B ]^T
+
+ \param A : Upper matrix.
+ \param B : Lower matrix.
+ \return Stacked matrix [ A B ]^T
+
+ \warning A and B must have the same number of columns.
+*/
+vpMatrix
+vpMatrix::stack(const vpMatrix &A, const vpMatrix &B)
+{
+ vpMatrix C ;
+
+ try{
+ vpMatrix::stack(A, B, C) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ return C ;
+}
+
+/*!
+ Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T
+
+ \param A : Upper matrix.
+ \param r : Lower matrix.
+ \return Stacked matrix [ A r ]^T
+
+ \warning \e A and \e r must have the same number of columns.
+*/
+vpMatrix
+vpMatrix::stack(const vpMatrix &A, const vpRowVector &r)
+{
+ vpMatrix C ;
+
+ try{
+ vpMatrix::stack(A, r, C) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ return C ;
+}
+
+/*!
+ Stack matrix \e B to the end of matrix \e A and return the resulting matrix in \e C.
+
+ \param A : Upper matrix.
+ \param B : Lower matrix.
+ \param C : Stacked matrix C = [ A B ]^T
+
+ \warning A and B must have the same number of columns.
+*/
+void
+vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+{
+ unsigned int nra = A.getRows() ;
+ unsigned int nrb = B.getRows() ;
+
+ if (nra !=0)
+ if (A.getCols() != B.getCols()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot stack (%dx%d) matrix with (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ try {
+ C.resize(nra+nrb,B.getCols() ) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ unsigned int i,j ;
+ for (i=0 ; i < nra ; i++) {
+ for (j=0 ; j < A.getCols() ; j++)
+ C[i][j] = A[i][j] ;
+ }
+
+ for (i=0 ; i < nrb ; i++) {
+ for (j=0 ; j < B.getCols() ; j++) {
+ C[i+nra][j] = B[i][j] ;
+ }
+ }
+}
+
+/*!
+ Stack row vector \e v to the end of matrix \e A and return the resulting matrix in \e C.
+
+ \param A : Upper matrix.
+ \param r : Lower row vector.
+ \param C : Stacked matrix C = [ A r ]^T
+
+ \warning A and r must have the same number of columns.
+*/
+void
+vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
+{
+ unsigned int nra = A.getRows() ;
+
+ if (nra !=0)
+ if (A.getCols() != r.getCols()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot stack (%dx%d) matrix with (1x%d) row vector",
+ A.getRows(), A.getCols(), r.getCols())) ;
+ }
+
+ try {
+ C.resize(nra+1,r.getCols() ) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ unsigned int i,j ;
+ for (i=0 ; i < nra ; i++) {
+ for (j=0 ; j < A.getCols() ; j++)
+ C[i][j] = A[i][j] ;
+ }
+
+ for (j=0 ; j < r.getCols() ; j++) {
+ C[nra][j] = r[j] ;
+ }
+}
+
+/*!
+ Insert matrix B in matrix A at the given position.
+
+ \param A : Main matrix.
+ \param B : Matrix to insert.
+ \param r : Index of the row where to add the matrix.
+ \param c : Index of the column where to add the matrix.
+ \return Matrix with B insert in A.
+
+ \warning Throw exception if the sizes of the matrices do not allow the insertion.
+*/
+vpMatrix
+vpMatrix::insert(const vpMatrix &A, const vpMatrix &B,
+ const unsigned int r, const unsigned int c)
+{
+ vpMatrix C ;
+
+ try{
+ insert(A,B, C, r, c) ;
+ }
+ catch(...) {
+ throw;
+ }
+
+ return C ;
+}
+
+/*!
+ \relates vpMatrix
+ Insert matrix B in matrix A at the given position.
+
+ \param A : Main matrix.
+ \param B : Matrix to insert.
+ \param C : Result matrix.
+ \param r : Index of the row where to insert matrix B.
+ \param c : Index of the column where to insert matrix B.
+
+ \warning Throw exception if the sizes of the matrices do not
+ allow the insertion.
+*/
+void
+vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C,
+ const unsigned int r, const unsigned int c)
+{
+ if( ( (r + B.getRows()) <= A.getRows() ) &&
+ ( (c + B.getCols()) <= A.getCols() ) ){
+ try {
+ C.resize(A.getRows(),A.getCols() ) ;
+ }
+ catch(...) {
+ throw ;
+ }
+ for(unsigned int i=0; i<A.getRows(); i++){
+ for(unsigned int j=0; j<A.getCols(); j++){
+ if(i >= r && i < (r + B.getRows()) && j >= c && j < (c+B.getCols())){
+ C[i][j] = B[i-r][j-c];
+ }
+ else{
+ C[i][j] = A[i][j];
+ }
+ }
+ }
+ }
+ else{
+ throw vpException(vpException::dimensionError,
+ "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
+ B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
+ }
+}
+
+/*!
+ Juxtapose to matrices C = [ A B ].
+
+ \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$
+
+ \param A : Left matrix.
+ \param B : Right matrix.
+ \return Juxtaposed matrix C = [ A B ]
+
+ \warning A and B must have the same number of column
+*/
+vpMatrix
+vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
+{
+ vpMatrix C ;
+
+ try{
+ juxtaposeMatrices(A,B, C) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ return C ;
+}
+
+/*!
+ \relates vpMatrix
+ Juxtapose to matrices C = [ A B ].
+
+ \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$
+
+ \param A : Left matrix.
+ \param B : Right matrix.
+ \param C : Juxtaposed matrix C = [ A B ]
+
+ \warning A and B must have the same number of rows.
+*/
+void
+vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+{
+ unsigned int nca = A.getCols() ;
+ unsigned int ncb = B.getCols() ;
+
+ if (nca !=0)
+ if (A.getRows() != B.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ }
+
+ try {
+ C.resize(B.getRows(),nca+ncb) ;
+ }
+ catch(...) {
+ throw ;
+ }
+
+ unsigned int i,j ;
+ for (i=0 ; i < C.getRows(); i++)
+ for (j=0 ; j < nca ; j++)
+ C[i][j] = A[i][j] ;
+
+ for (i=0 ; i < C.getRows() ; i++)
+ for (j=0 ; j < ncb ; j++){
+ C[i][nca+j] = B[i][j] ;
+ }
+}
+
+
+//--------------------------------------------------------------------
+// Output
+//--------------------------------------------------------------------
+
+/*!
+
+ Pretty print a matrix. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each matrix element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the matrix.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all matrix elements
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+int
+vpMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = getRows();
+ unsigned int n = getCols();
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int i=0;i<m;++i) {
+ for (unsigned int j=0;j<n;++j){
+ oss.str("");
+ oss << (*this)[i][j];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[i][j];
+ oss.str(ossFixed.str());
+ }
+
+ values[i*n+j]=oss.str();
+ size_type thislen=values[i*n+j].size();
+ size_type p=values[i*n+j].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+ }
+ }
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ for (unsigned int i=0;i<m;i++) {
+ s <<" ";
+ for (unsigned int j=0;j<n;j++){
+ size_type p=values[i*n+j].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[i*n+j].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[i*n+j].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+ }
+ s <<std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
+
+
+/*!
+ Print using matlab syntax, to be put in matlab later.
+
+ Print using the following form:
+ \code
+[ a,b,c;
+d,e,f;
+g,h,i]
+ \endcode
+*/
+std::ostream & vpMatrix::matlabPrint(std::ostream & os) const
+{
+ os << "[ ";
+ for (unsigned int i=0; i < this->getRows(); ++ i) {
+ for (unsigned int j=0; j < this ->getCols(); ++ j) {
+ os << (*this)[i][j] << ", ";
+ }
+ if (this ->getRows() != i+1) { os << ";" << std::endl; }
+ else { os << "]" << std::endl; }
+ }
+ return os;
+};
+
+/*!
+ Print using MAPLE matrix input format.
+
+Print using the following way so that this output can be directly copied into MAPLE:
+ \code
+([
+[0.939846, 0.0300754, 0.340272, ],
+[0.0300788, 0.984961, -0.170136, ],
+[-0.340272, 0.170136, 0.924807, ],
+])
+ \endcode
+*/
+std::ostream & vpMatrix::maplePrint(std::ostream & os) const
+{
+ os << "([ " << std::endl;
+ for (unsigned int i=0; i < this->getRows(); ++ i) {
+ os << "[";
+ for (unsigned int j=0; j < this->getCols(); ++ j) {
+ os << (*this)[i][j] << ", ";
+ }
+ os << "]," << std::endl;
+ }
+ os << "])" << std::endl;
+ return os;
+};
+
+/*!
+ Print matrix in csv format.
+
+ Print as comma separated values so that this output can be imported into any program which has a csv data import option:
+ \code
+0.939846, 0.0300754, 0.340272
+0.0300788, 0.984961, -0.170136
+-0.340272, 0.170136, 0.924807
+ \endcode
+*/
+std::ostream & vpMatrix::csvPrint(std::ostream & os) const
+{
+ for (unsigned int i=0; i < this->getRows(); ++ i) {
+ for (unsigned int j=0; j < this->getCols(); ++ j) {
+ os << (*this)[i][j];
+ if (!(j==(this->getCols()-1)))
+ os << ", ";
+ }
+ os << std::endl;
+ }
+ return os;
+};
+
+
+/*!
+ Print to be used as part of a C++ code later.
+
+ Print under the following form:
+ \code
+vpMatrix A(6,4);
+A[0][0] = 1.4;
+A[0][1] = 0.6; ...
+ \endcode
+
+ \param os: the stream to be printed in.
+ \param matrixName: name of the matrix, "A" by default, to be used for
+ the line vpMatrix A(6,7) (see example).
+ \param octet: if false, print using double, if true, print byte per byte
+ each bytes of the double array.
+*/
+std::ostream & vpMatrix::cppPrint(std::ostream & os, const char * matrixName, bool octet) const
+{
+ const char defaultName [] = "A";
+ if (NULL == matrixName)
+ {
+ matrixName = defaultName;
+ }
+ os << "vpMatrix " << defaultName
+ << " (" << this ->getRows ()
+ << ", " << this ->getCols () << "); " <<std::endl;
+
+ for (unsigned int i=0; i < this->getRows(); ++ i)
+ {
+ for (unsigned int j=0; j < this ->getCols(); ++ j)
+ {
+ if (! octet)
+ {
+ os << defaultName << "[" << i << "][" << j
+ << "] = " << (*this)[i][j] << "; " << std::endl;
+ }
+ else
+ {
+ for (unsigned int k = 0; k < sizeof(double); ++ k)
+ {
+ os << "((unsigned char*)&(" << defaultName
+ << "[" << i << "][" << j << "]) )[" << k
+ <<"] = 0x" <<std::hex<<
+ (unsigned int)((unsigned char*)& ((*this)[i][j])) [k]
+ << "; " << std::endl;
+ }
+ }
+ }
+ os << std::endl;
+ }
+ return os;
+};
+
+/*!
+ Compute the determinant of the matrix using the LU Decomposition.
+
+ \return The determinant of the matrix if the matrix is square, 0 otherwise.
+
+ See the Numerical Recipes in C page 43 for further explanations.
+*/
+
+double vpMatrix::detByLU() const
+{
+ double det_ = 0;
+
+ // Test wether the matrix is squred
+ if (rowNum == colNum)
+ {
+ // create a temporary matrix that will be modified by LUDcmp
+ vpMatrix tmp(*this);
+
+ // using th LUdcmp based on NR codes
+ // it modified the tmp matrix in a special structure of type :
+ // b11 b12 b13 b14
+ // a21 b22 b23 b24
+ // a21 a32 b33 b34
+ // a31 a42 a43 b44
+
+ unsigned int * perm = new unsigned int[rowNum]; // stores the permutations
+ int d; // +- 1 fi the number of column interchange is even or odd
+ tmp.LUDcmp(perm, d);
+ delete[]perm;
+
+ // compute the determinant that is the product of the eigen values
+ det_ = (double) d;
+ for(unsigned int i=0;i<rowNum;i++)
+ {
+ det_*=tmp[i][i];
+ }
+ }
+ else {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute LU decomposition on a non square matrix (%dx%d)",
+ rowNum, colNum)) ;
+ }
+ return det_ ;
+}
+
+
+
+/*!
+ Stack A at the end of the current matrix, or copy if the matrix has no dimensions : this = [ this A ]^T.
+
+ Here an example for a robot velocity log :
+\code
+vpMatrix A;
+vpColVector v(6);
+for(unsigned int i = 0;i<100;i++)
+{
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, v);
+ Velocities.stackMatrices(v.t());
+}
+\endcode
+*/
+void vpMatrix::stack(const vpMatrix &A)
+{
+ if(rowNum == 0)
+ *this = A;
+ else
+ *this = vpMatrix::stack(*this, A);
+}
+
+/*!
+ Stack row vector \e r at the end of the current matrix, or copy if the matrix has no dimensions : this = [ this r ]^T.
+*/
+void vpMatrix::stack(const vpRowVector &r)
+{
+ if(rowNum == 0)
+ *this = r;
+ else
+ *this = vpMatrix::stack(*this, r);
+}
+
+
+/*!
+ Insert matrix A at the given position in the current matrix.
+
+ \warning Throw vpException::dimensionError if the
+ dimensions of the matrices do not allow the operation.
+
+ \param A : The matrix to insert.
+ \param r : The index of the row to begin to insert data.
+ \param c : The index of the column to begin to insert data.
+*/
+void vpMatrix::insert(const vpMatrix&A, const unsigned int r,
+ const unsigned int c)
+{
+ if( (r + A.getRows() ) <= rowNum && (c + A.getCols() ) <= colNum ){
+ // recopy matrix A in the current one, does not call static function to avoid initialisation and recopy of matrix
+ for(unsigned int i=r; i<(r+A.getRows()); i++){
+ for(unsigned int j=c; j<(c+A.getCols()); j++){
+ (*this)[i][j] = A[i-r][j-c];
+ }
+ }
+ }
+ else{
+ throw vpException(vpException::dimensionError,
+ "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
+ A.getRows(), A.getCols(), rowNum, colNum, r, c);
+ }
+}
+
+
+/*!
+ Compute the eigenvalues of a n-by-n real symmetric matrix.
+
+ \return The eigenvalues of a n-by-n real symmetric matrix.
+
+ \warning This method is only available if the Gnu Scientific Library
+ (GSL) is detected as a third party library.
+
+ \exception vpException::dimensionError If the matrix is not square.
+ \exception vpException::fatalError If the matrix is not symmetric.
+ \exception vpException::functionNotImplementedError If the GSL library is not detected.
+
+ Here an example:
+\code
+#include <iostream>
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3); // A is a symmetric matrix
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.;
+ A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.;
+ std::cout << "Initial symmetric matrix: \n" << A << std::endl;
+
+ // Compute the eigen values
+ vpColVector evalue; // Eigenvalues
+ evalue = A.eigenValues();
+ std::cout << "Eigen values: \n" << evalue << std::endl;
+}
+\endcode
+
+ \sa eigenValues(vpColVector &, vpMatrix &)
+
+*/
+vpColVector vpMatrix::eigenValues() const
+{
+ if (rowNum != colNum) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute eigen values on a non square matrix (%dx%d)",
+ rowNum, colNum)) ;
+ }
+
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
+ {
+ // Check if the matrix is symetric: At - A = 0
+ vpMatrix At_A = (*this).t() - (*this);
+ for (unsigned int i=0; i < rowNum; i++) {
+ for (unsigned int j=0; j < rowNum; j++) {
+ //if (At_A[i][j] != 0) {
+ if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute eigen values on a non symetric matrix")) ;
+ }
+ }
+ }
+
+ vpColVector evalue(rowNum); // Eigen values
+
+ gsl_vector *eval = gsl_vector_alloc (rowNum);
+ gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
+
+ gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
+ gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
+
+ unsigned int Atda = m->tda ;
+ for (unsigned int i=0 ; i < rowNum ; i++){
+ unsigned int k = i*Atda ;
+ for (unsigned int j=0 ; j < colNum ; j++)
+ m->data[k+j] = (*this)[i][j] ;
+ }
+ gsl_eigen_symmv (m, eval, evec, w);
+
+ gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
+
+ for (unsigned int i=0; i < rowNum; i++) {
+ evalue[i] = gsl_vector_get (eval, i);
+ }
+
+ gsl_eigen_symmv_free (w);
+ gsl_vector_free (eval);
+ gsl_matrix_free (m);
+ gsl_matrix_free (evec);
+
+ return evalue;
+ }
+#else
+ {
+ throw(vpException(vpException::functionNotImplementedError,
+ "Eigen values computation is not implemented. You should install GSL rd party")) ;
+ }
+#endif
+}
+
+/*!
+ Compute the eigenvalues of a n-by-n real symmetric matrix.
+ \return The eigenvalues of a n-by-n real symmetric matrix.
+
+ \warning This method is only available if the Gnu Scientific Library
+ (GSL) is detected as a third party library.
+
+ \param evalue : Eigenvalues of the matrix.
+
+ \param evector : Eigenvector of the matrix.
+
+ \exception vpException::dimensionError If the matrix is not square.
+ \exception vpException::fatalError If the matrix is not symmetric.
+ \exception vpException::functionNotImplementedError If the GSL library is not detected.
+
+ Here an example:
+\code
+#include <iostream>
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4); // A is a symmetric matrix
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.;
+ A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.;
+ A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+ std::cout << "Initial symmetric matrix: \n" << A << std::endl;
+
+ vpColVector d; // Eigenvalues
+ vpMatrix V; // Eigenvectors
+
+ // Compute the eigenvalues and eigenvectors
+ A.eigenValues(d, V);
+ std::cout << "Eigen values: \n" << d << std::endl;
+ std::cout << "Eigen vectors: \n" << V << std::endl;
+
+ vpMatrix D;
+ D.diag(d); // Eigenvalues are on the diagonal
+
+ std::cout << "D: " << D << std::endl;
+
+ // Verification: A * V = V * D
+ std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl;
+}
+\endcode
+
+\sa eigenValues()
+
+*/
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
+void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
+#else
+void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
+#endif
+{
+ if (rowNum != colNum) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute eigen values on a non square matrix (%dx%d)",
+ rowNum, colNum)) ;
+ }
+
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
+ {
+ // Check if the matrix is symetric: At - A = 0
+ vpMatrix At_A = (*this).t() - (*this);
+ for (unsigned int i=0; i < rowNum; i++) {
+ for (unsigned int j=0; j < rowNum; j++) {
+ //if (At_A[i][j] != 0) {
+ if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute eigen values on a non symetric matrix")) ;
+ }
+ }
+ }
+
+ // Resize the output matrices
+ evalue.resize(rowNum);
+ evector.resize(rowNum, colNum);
+
+ gsl_vector *eval = gsl_vector_alloc (rowNum);
+ gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
+
+ gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
+ gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
+
+ unsigned int Atda = m->tda ;
+ for (unsigned int i=0 ; i < rowNum ; i++){
+ unsigned int k = i*Atda ;
+ for (unsigned int j=0 ; j < colNum ; j++)
+ m->data[k+j] = (*this)[i][j] ;
+ }
+ gsl_eigen_symmv (m, eval, evec, w);
+
+ gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
+
+ for (unsigned int i=0; i < rowNum; i++) {
+ evalue[i] = gsl_vector_get (eval, i);
+ }
+ Atda = evec->tda ;
+ for (unsigned int i=0; i < rowNum; i++) {
+ unsigned int k = i*Atda ;
+ for (unsigned int j=0; j < rowNum; j++) {
+ evector[i][j] = evec->data[k+j];
+ }
+ }
+
+ gsl_eigen_symmv_free (w);
+ gsl_vector_free (eval);
+ gsl_matrix_free (m);
+ gsl_matrix_free (evec);
+ }
+#else
+ {
+ throw(vpException(vpException::functionNotImplementedError,
+ "Eigen values computation is not implemented. You should install GSL rd party")) ;
+ }
+#endif
+}
+
+
+/*!
+ Function to compute the null space (the kernel) of the interaction matrix A which is not full rank.
+ The null space ( the kernel ) of a matrix A is defined as Null(A) = Ker(A) = {X : A*X =0}.
+
+ \param kerA : The matrix to contain the null space (kernel) of A defined by the row vectors (A*KerA.t()=0)
+ \param svThreshold : Specify the used threshold in the svd(...) function (a function to compute the singular value decomposition)
+
+ \return the rank of the matrix.
+*/
+
+unsigned int
+vpMatrix::kernel(vpMatrix &kerA, double svThreshold) const
+{
+ unsigned int i, j ;
+ unsigned int nbline = getRows() ;
+ unsigned int nbcol = getCols() ;
+
+ if ( (nbline <= 0) || (nbcol <= 0) ) {
+ throw( vpException(vpException::dimensionError, "Cannot compute kernel of a zero-size matrix") );
+ }
+
+ vpMatrix A ; // Copy of the matrix, SVD function is destructive
+ vpColVector sv(nbcol) ; // singular values
+ vpMatrix v(nbcol,nbcol) ; // V matrix of singular value decomposition
+
+ // Copy and resize matrix to have at least as many rows as columns
+ // kernel is computed in svd method only if the matrix has more rows than columns
+
+ if (nbline < nbcol) A.resize(nbcol,nbcol) ;
+ else A.resize(nbline,nbcol) ;
+
+ for (i=0 ; i < nbline ; i++)
+ {
+ for (j=0 ; j < nbcol ; j++)
+ {
+ A[i][j] = (*this)[i][j] ;
+ }
+ }
+
+ A.svd(sv,v);
+
+ // Compute the highest singular value and rank of the matrix
+ double maxsv = 0 ;
+ for (i=0 ; i < nbcol ; i++)
+ if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+
+ unsigned int rank = 0 ;
+ for (i=0 ; i < nbcol ; i++)
+ if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+
+ if (rank != nbcol)
+ {
+ vpMatrix Ker(nbcol-rank,nbcol) ;
+ unsigned int k = 0 ;
+ for (j = 0 ; j < nbcol ; j++)
+ {
+ //if( v.col(j) in kernel and non zero )
+ if ( (fabs(sv[j]) <= maxsv*svThreshold) && (std::fabs(v.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon()))
+ {
+ // Ker.Row(k) = v.Col(j) ;
+ for (i=0 ; i < v.getRows() ; i++)
+ {
+ Ker[k][i] = v[i][j];
+ }
+ k++;
+ }
+ }
+ kerA = Ker ;
+ }
+ else
+ {
+ kerA.resize(0,0);
+ }
+
+ return rank ;
+}
+
+/*!
+ Compute the determinant of a n-by-n matrix.
+
+ \param method : Method used to compute the determinant. Default LU
+ decomposition methos is faster than the method based on Gaussian
+ elimination.
+
+ \return Determinant of the matrix.
+
+ \code
+#include <iostream>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by default method : " <<
+ A.det() << std::endl;
+ std:: cout << "Determinant by LU decomposition: " <<
+ A.det(vpMatrix::LU_DECOMPOSITION ) << std::endl;
+}
+\endcode
+*/
+double vpMatrix::det(vpDetMethod method) const
+{
+ double det_ = 0;
+
+ if ( method == LU_DECOMPOSITION )
+ {
+ det_ = this->detByLU();
+ }
+
+ return (det_);
+}
+
+/*!
+
+ Compute the exponential matrix of a square matrix.
+
+ \return Return the exponential matrix.
+
+*/
+vpMatrix
+vpMatrix::expm() const
+{
+ if(colNum != rowNum) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute the exponential of a non square (%dx%d) matrix",
+ rowNum, colNum ));
+ }
+ else
+ {
+#ifdef VISP_HAVE_GSL
+ size_t size_ = rowNum * colNum;
+ double *b = new double [size_];
+ for (size_t i=0; i< size_; i++)
+ b[i] = 0.;
+ gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
+ gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
+ gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
+ //gsl_matrix_fprintf(stdout, &em.matrix, "%g");
+ vpMatrix expA(rowNum, colNum);
+ memcpy(expA.data, b, size_ * sizeof(double));
+
+ delete [] b;
+ return expA;
+#else
+ vpMatrix _expE(rowNum, colNum);
+ vpMatrix _expD(rowNum, colNum);
+ vpMatrix _expX(rowNum, colNum);
+ vpMatrix _expcX(rowNum, colNum);
+ vpMatrix _eye(rowNum, colNum);
+
+ _eye.eye();
+ vpMatrix exp(*this);
+
+ // double f;
+ int e;
+ double c = 0.5;
+ int q = 6;
+ int p = 1;
+
+ double nA = 0;
+ for (unsigned int i = 0; i < rowNum;i++)
+ {
+ double sum = 0;
+ for (unsigned int j=0; j < colNum; j++)
+ {
+ sum += fabs((*this)[i][j]);
+ }
+ if (sum>nA||i==0)
+ {
+ nA=sum;
+ }
+ }
+
+ /* f = */ frexp(nA, &e);
+ //double s = (0 > e+1)?0:e+1;
+ double s = e+1;
+
+ double sca = 1.0 / pow(2.0,s);
+ exp=sca*exp;
+ _expX=*this;
+ _expE=c*exp+_eye;
+ _expD=-c*exp+_eye;
+ for (int k=2;k<=q;k++)
+ {
+ c = c * ((double)(q-k+1)) / ((double)(k*(2*q-k+1)));
+ _expcX=exp*_expX;
+ _expX=_expcX;
+ _expcX=c*_expX;
+ _expE=_expE+_expcX;
+ if (p) _expD=_expD+_expcX;
+ else _expD=_expD- _expcX;
+ p = !p;
+ }
+ _expX=_expD.inverseByLU();
+ exp=_expX*_expE;
+ for (int k=1;k<=s;k++)
+ {
+ _expE=exp*exp;
+ exp=_expE;
+ }
+ return exp;
+#endif
+ }
+}
+
+/**************************************************************************************************************/
+/**************************************************************************************************************/
+
+
+//Specific functions
+
+/*
+input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
+
+output:: the complement matrix of the element (rowNo,colNo).
+This is the matrix obtained from M after elimenating the row rowNo and column colNo
+
+example:
+1 2 3
+M = 4 5 6
+7 8 9
+1 3
+subblock(M, 1, 1) give the matrix 7 9
+*/
+vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
+{
+ vpMatrix M_comp(M.getRows()-1,M.getCols()-1);
+
+ for ( unsigned int i = 0 ; i < col ; i++)
+ {
+ for ( unsigned int j = 0 ; j < row ; j++)
+ M_comp[i][j]=M[i][j];
+ for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
+ M_comp[i][j-1]=M[i][j];
+ }
+ for ( unsigned int i = col+1 ; i < M.getCols(); i++)
+ {
+ for ( unsigned int j = 0 ; j < row ; j++)
+ M_comp[i-1][j]=M[i][j];
+ for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
+ M_comp[i-1][j-1]=M[i][j];
+ }
+ return M_comp;
+}
+
+/*!
+ \return The condition number, the ratio of the largest singular value of the matrix to the smallest.
+ */
+double vpMatrix::cond() const
+{
+ vpMatrix v;
+ vpColVector w;
+
+ vpMatrix M;
+ M = *this;
+
+ M.svd(w, v);
+ double min=w[0];
+ double max=w[0];
+ for(unsigned int i=0;i<M.getCols();i++)
+ {
+ if(min>w[i])min=w[i];
+ if(max<w[i])max=w[i];
+ }
+ return max/min;
+}
+
+/*!
+ Compute \f${\bf H} + \alpha * diag({\bf H})\f$
+ \param H : input Matrix \f${\bf H}\f$. This matrix should be square.
+ \param alpha : Scalar \f$\alpha\f$
+ \param HLM : Resulting operation.
+ */
+void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
+{
+ if(H.getCols() != H.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot compute HLM on a non square matrix (%dx%d)",
+ H.getRows(), H.getCols() ));
+ }
+ HLM.resize(H.getRows(), H.getCols());
+
+ for(unsigned int i=0;i<H.getCols();i++)
+ {
+ for(unsigned int j=0;j<H.getCols();j++)
+ {
+ HLM[i][j]=H[i][j];
+ if(i==j)
+ HLM[i][j]+= alpha*H[i][j];
+ }
+ }
+}
+
+/*!
+ Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{A_{ij}^2}} \f$.
+
+ \return The Euclidean norm if the matrix is initialized, 0 otherwise.
+
+ \sa infinityNorm()
+*/
+double vpMatrix::euclideanNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<dsize;i++) {
+ x = *(data +i); norm += x*x;
+ }
+
+ return sqrt(norm);
+}
+
+/*!
+
+ Compute and return the infinity norm \f$ {||x||}_{\infty} =
+ max\left(\sum_{j=0}^{n}{\mid x_{ij} \mid}\right) \f$ with \f$i \in
+ \{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size.
+
+ \return The infinity norm if the matrix is initialized, 0 otherwise.
+
+ \sa euclideanNorm()
+*/
+double vpMatrix::infinityNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<rowNum;i++){
+ x = 0;
+ for (unsigned int j=0; j<colNum;j++){
+ x += fabs (*(*(rowPtrs + i)+j)) ;
+ }
+ if (x > norm) {
+ norm = x;
+ }
+ }
+ return norm;
+}
+
+/*!
+ Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, n)\f$.
+
+ \return The value \f$\sum A_{ij}^{2}\f$.
+ */
+double vpMatrix::sumSquare() const
+{
+ double sum_square=0.0;
+ double x ;
+
+ for (unsigned int i=0;i<rowNum;i++) {
+ for(unsigned int j=0;j<colNum;j++) {
+ x=rowPtrs[i][j];
+ sum_square += x*x;
+ }
+ }
+
+ return sum_square;
+}
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B)
+{
+ return (vpMatrix)(vpColVector::stack(A, B));
+}
+
+void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
+{
+ vpColVector::stack(A, B, C);
+}
+
+vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B)
+{
+ return vpMatrix::stack(A, B);
+};
+
+void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C)
+{
+ vpMatrix::stack(A, B, C);
+};
+#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+
diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp
new file mode 100644
index 0000000..cec10f8
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix Cholesky decomposition.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+#ifdef VISP_HAVE_LAPACK_C
+extern "C" void dpotrf_ (char *uplo, int *n, double *a, int *lda, int *info);
+extern "C" int dpotri_(char *uplo, int *n, double *a, int *lda, int *info);
+
+vpMatrix vpMatrix::inverseByCholeskyLapack() const{
+ int rowNum_ = (int)this->getRows();
+ int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
+ int info;
+
+ vpMatrix A = *this;
+ dpotrf_((char*)"L",&rowNum_,A.data,&lda,&info);
+
+ if(info!=0)
+ std::cout << "cholesky:dpotrf_:error" << std::endl;
+
+ dpotri_((char*)"L",&rowNum_,A.data,&lda,&info);
+ if(info!=0){
+ std::cout << "cholesky:dpotri_:error" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+
+ for(unsigned int i=0;i<A.getRows();i++)
+ for(unsigned int j=0;j<A.getCols();j++)
+ if(i>j) A[i][j] = A[j][i];
+
+ return A;
+}
+
+/*!
+ Compute the inverse of a n-by-n matrix using the Cholesky decomposition.
+ The matrix must be real and symmetric.
+ Only available if lapack is installed.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ // Symmetric matrix
+ A[0][0] = 1/1.; A[0][1] = 1/5.; A[0][2] = 1/6.; A[0][3] = 1/7.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/3.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByCholesky();
+ std::cout << "Inverse by Cholesky: \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse()
+*/
+
+vpMatrix
+vpMatrix::inverseByCholesky() const
+{
+
+ if ( rowNum != colNum)
+ {
+ vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
+ throw(vpMatrixException(vpMatrixException::matrixError,
+ "Cannot invert a non-square vpMatrix")) ;
+ }
+ return inverseByCholeskyLapack();
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpMatrixLapack.cpp.o) has no symbols
+void dummy_vpMatrixLapack() {};
+#endif
diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp
new file mode 100644
index 0000000..81106cd
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Covariance matrix computation.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <limits> // numeric_limits
+#include <cmath> // std::fabs()
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpMatrixException.h>
+
+
+/*!
+ Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
+ Ax = b
+
+ \param A : Matrix A from Ax = b.
+
+ \param x : Vector x from Ax = b corresponding to the parameters to estimate.
+
+ \param b : Vector b from Ax = b.
+*/
+vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
+{
+// double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To consider OLS Estimate for sigma
+ double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma
+
+ if(denom <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
+
+// double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be equivalent to line bellow.
+ double sigma2 = (b - (A * x)).t() * (b - (A * x));
+
+ sigma2 /= denom;
+
+ return (A.t()*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
+}
+
+/*!
+ Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
+ WAx = Wb
+
+ \param A : Matrix A from WAx = Wb.
+
+ \param x : Vector x from WAx = Wb corresponding to the parameters to estimate.
+
+ \param b : Vector b from WAx = Wb.
+
+ \param W : Diagonal weigths matrix from WAx = Wb.
+*/
+vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &W)
+{
+ double denom = 0.0;
+ vpMatrix W2(W.getCols(),W.getCols());
+ for(unsigned int i = 0 ; i < W.getCols() ; i++){
+ denom += W[i][i];
+ W2[i][i] = W[i][i]*W[i][i];
+ }
+
+ if(denom <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
+
+// double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should be equivalent to line bellow.
+ double sigma2 = (W * b - (W * A * x)).t() * (W*b - (W * A * x));
+ sigma2 /= denom;
+
+ return (A.t()*(W2)*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
+}
+
+/*!
+ Compute the covariance matrix of an image-based virtual visual servoing.
+ This assumes the optimization has been done via v = Ls.pseudoInverse() * DeltaS.
+
+ \param cMo : Pose matrix that has been computed with the v.
+
+ \param deltaS : Error vector used in v = Ls.pseudoInverse() * DeltaS
+
+ \param Ls : interaction matrix used in v = Ls.pseudoInverse() * DeltaS
+*/
+vpMatrix
+vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls)
+{
+ vpMatrix Js;
+ vpColVector deltaP;
+ vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
+
+ return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS);
+}
+
+/*!
+ Compute the covariance matrix of an image-based virtual visual servoing.
+ This assumes the optimization has been done via v = (W * Ls).pseudoInverse() * W * DeltaS.
+
+ \param cMo : Pose matrix that has been computed with the v.
+
+ \param deltaS : Error vector used in v = (W * Ls).pseudoInverse() * W * DeltaS.
+
+ \param Ls : interaction matrix used in v = (W * Ls).pseudoInverse() * W * DeltaS.
+
+ \param W : Weight matrix used in v = (W * Ls).pseudoInverse() * W * DeltaS.
+*/
+vpMatrix
+vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
+{
+ vpMatrix Js;
+ vpColVector deltaP;
+ vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
+
+ return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS,W);
+}
+
+void
+vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, vpMatrix &Js, vpColVector &deltaP)
+{
+ //building Lp
+ vpMatrix LpInv(6,6);
+ LpInv = 0;
+ LpInv[0][0] = -1.0;
+ LpInv[1][1] = -1.0;
+ LpInv[2][2] = -1.0;
+
+ vpTranslationVector ctoInit;
+
+ cMo.extract(ctoInit);
+ vpMatrix ctoInitSkew = ctoInit.skew();
+
+ vpThetaUVector thetau;
+ cMo.extract(thetau);
+
+ vpColVector tu(3);
+ for(unsigned int i = 0 ; i < 3 ; i++)
+ tu[i] = thetau[i];
+
+ double theta = sqrt(tu.sumSquare()) ;
+
+// vpMatrix Lthetau(3,3);
+ vpMatrix LthetauInvAnalytic(3,3);
+ vpMatrix I3(3,3);
+ I3.eye();
+// Lthetau = -I3;
+ LthetauInvAnalytic = -I3;
+
+ if(theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon())
+ {
+ // Computing [theta/2 u]_x
+ vpColVector theta2u(3) ;
+ for (unsigned int i=0 ; i < 3 ; i++) {
+ theta2u[i] = tu[i]/2.0 ;
+ }
+ vpMatrix theta2u_skew = vpColVector::skew(theta2u);
+
+ vpColVector u(3) ;
+ for (unsigned int i=0 ; i < 3 ; i++) {
+ u[i] = tu[i]/theta ;
+ }
+ vpMatrix u_skew = vpColVector::skew(u);
+
+// Lthetau += (theta2u_skew - (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
+ LthetauInvAnalytic += -(vpMath::sqr(vpMath::sinc(theta/2.0)) * theta2u_skew - (1.0-vpMath::sinc(theta))*u_skew*u_skew);
+ }
+
+// vpMatrix LthetauInv = Lthetau.inverseByLU();
+
+ ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
+
+ for(unsigned int a = 0 ; a < 3 ; a++)
+ for(unsigned int b = 0 ; b < 3 ; b++)
+ LpInv[a][b+3] = ctoInitSkew[a][b];
+
+ for(unsigned int a = 0 ; a < 3 ; a++)
+ for(unsigned int b = 0 ; b < 3 ; b++)
+ LpInv[a+3][b+3] = LthetauInvAnalytic[a][b];
+
+ // Building Js
+ Js = Ls * LpInv;
+
+ // building deltaP
+ deltaP = (Js).pseudoInverse(Js.getRows()*std::numeric_limits<double>::epsilon()) * deltaS;
+}
diff --git a/modules/core/src/math/matrix/vpMatrix_lu.cpp b/modules/core/src/math/matrix/vpMatrix_lu.cpp
new file mode 100644
index 0000000..72d7b60
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_lu.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix LU decomposition.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#define TINY 1.0e-20;
+
+
+/*--------------------------------------------------------------------
+ LU Decomposition related functions
+-------------------------------------------------------------------- */
+
+/*!
+ \brief Performed the LU decomposition
+
+ Given a matrix A (n x n), this routine replaces it by the LU decomposition of
+ a rowwise permutation of itself. A is output, arranged as in equation
+ (2.3.14) of the NRC ; perm (n) is an output vector that records the row
+ permutation effected by the partial pivoting; d is output as 1 depending on
+ whether the number of row interchanges was even or odd, respectively.
+
+ \warning Destructive wrt. A
+
+ \sa This routine is used in combination with LUDksb to solve linear equations
+ or invert a matrix.
+
+ This function is extracted from the NRC
+
+ */
+
+void
+vpMatrix::LUDcmp(unsigned int *perm, int& d)
+{
+ unsigned int n = rowNum;
+
+ unsigned int i,imax=0,j,k;
+ double big,dum,sum_,temp;
+ vpColVector vv(n);
+
+ d=1;
+ for (i=0;i<n;i++) {
+ big=0.0;
+ for (j=0;j<n;j++)
+ if ((temp=fabs(rowPtrs[i][j])) > big) big=temp;
+ //if (big == 0.0)
+ if (std::fabs(big) <= std::numeric_limits<double>::epsilon())
+ {
+ //vpERROR_TRACE("Singular vpMatrix in LUDcmp") ;
+ throw(vpMatrixException(vpMatrixException::matrixError,
+ "Singular vpMatrix in LUDcmp")) ;
+ }
+ vv[i]=1.0/big;
+ }
+ for (j=0;j<n;j++) {
+ for (i=0;i<j;i++) {
+ sum_=rowPtrs[i][j];
+ for (k=0;k<i;k++) sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
+ rowPtrs[i][j]=sum_;
+ }
+ big=0.0;
+ for (i=j;i<n;i++) {
+ sum_=rowPtrs[i][j];
+ for (k=0;k<j;k++)
+ sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
+ rowPtrs[i][j]=sum_;
+ if ( (dum=vv[i]*fabs(sum_)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<n;k++) {
+ dum=rowPtrs[imax][k];
+ rowPtrs[imax][k]=rowPtrs[j][k];
+ rowPtrs[j][k]=dum;
+ }
+ d *= -1;
+ vv[imax]=vv[j];
+ }
+ perm[j]=imax;
+ //if (rowPtrs[j][j] == 0.0)
+ if (std::fabs(rowPtrs[j][j]) <= std::numeric_limits<double>::epsilon())
+ rowPtrs[j][j]=TINY;
+ if (j != n) {
+ dum=1.0/(rowPtrs[j][j]);
+ for (i=j+1;i<n;i++) rowPtrs[i][j] *= dum;
+ }
+ }
+}
+
+#undef TINY
+
+/*!
+ \brief Solve linear system AX = B using LU decomposition
+
+ Solves the set of n linear equations AX = B. Here A (n x n) is input, not
+ as the matrix A but rather as its LU decomposition, determined by the routine
+ ludcmp. perm (n) is input as the permutation vector returned by
+ ludcmp. b (n) is input as the right-hand side vector B, and returns with
+ the solution vector X. A and perm are not modified by this routine and can
+ be left in place for successive calls with different right-hand sides b. This
+ routine takes into account the possibility that b will begin with many zero
+ elements, so it is efficient for use in matrix inversion.
+
+ \sa This function must be used with LUDcmp
+
+ \sa LUDsolve and solveByLUD are more intuitive and direct to use
+
+ This function is extracted from the NRC
+
+*/
+void vpMatrix::LUBksb(unsigned int *perm, vpColVector& b)
+{
+ unsigned int n = rowNum;
+
+ unsigned int ii=0;
+ unsigned int ip;
+ double sum_;
+ bool flag = false;
+ unsigned int i;
+
+ for (i=0;i<n;i++) {
+ ip=perm[i];
+ sum_=b[ip];
+ b[ip]=b[i];
+ if (flag) {
+ for (unsigned int j=ii;j<=i-1;j++) sum_ -= rowPtrs[i][j]*b[j];
+ }
+ //else if (sum_) {
+ else if (std::fabs(sum_) > std::numeric_limits<double>::epsilon()) {
+ ii=i;
+ flag = true;
+ }
+ b[i]=sum_;
+ }
+ // for (int i=n-1;i>=0;i--) {
+ // sum_=b[i];
+ // for (int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
+ // b[i]=sum_/rowPtrs[i][i];
+ // }
+ i=n;
+ do {
+ i --;
+
+ sum_=b[i];
+ for (unsigned int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
+ b[i]=sum_/rowPtrs[i][i];
+ } while(i != 0);
+}
+#endif // doxygen should skip this
+
+/*!
+ Compute the inverse of a n-by-n matrix using the LU decomposition.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByLU();
+ std::cout << "Inverse by LU: \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse()
+*/
+vpMatrix
+vpMatrix::inverseByLU() const
+{
+ unsigned int i,j;
+
+ if ( rowNum != colNum)
+ {
+ vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
+ throw(vpMatrixException(vpMatrixException::matrixError,
+ "Cannot invert a non-square vpMatrix")) ;
+ }
+
+ vpMatrix B(rowNum, rowNum), X(rowNum, rowNum);
+ vpMatrix V(rowNum, rowNum);
+ vpColVector W(rowNum);
+
+ for (i=0; i<rowNum; i++) {
+ for (j=0; j<rowNum; j++) {
+ B[i][j] = (i == j) ? 1 : 0;
+ }
+ }
+
+ vpMatrix A(rowNum, rowNum);
+ A = *this;
+
+ unsigned int *perm = new unsigned int[rowNum];
+ int p;
+
+ try {
+ A.LUDcmp(perm, p);
+ }
+ catch(vpException e) {
+ delete [] perm;
+ throw(e);
+ }
+
+ vpColVector c_tmp(rowNum) ;
+ for (j=1; j<=rowNum; j++)
+ {
+ c_tmp =0 ; c_tmp[j-1] = 1 ;
+ A.LUBksb(perm, c_tmp);
+ for (unsigned int k=0 ; k < c_tmp.getRows() ; k++)
+ B[k][j-1] = c_tmp[k] ;
+ }
+ delete [] perm;
+ return B;
+}
+
diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp
new file mode 100644
index 0000000..a353923
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix QR decomposition.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <algorithm> // for std::min and std::max
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+int allocate_work(double** work);
+
+#ifdef VISP_HAVE_LAPACK_C
+extern "C" int dgeqrf_(int *m, int *n, double*a, int *lda, double *tau, double *work, int *lwork, int *info);
+extern "C" int dormqr_(char *side, char *trans, int *m, int *n,
+ int *k, double *a, int *lda, double *tau, double *c__,
+ int *ldc, double *work, int *lwork, int *info);
+extern "C" int dorgqr_(int *, int *, int *, double *, int *,
+ double *, double *, int *, int *);
+extern "C" int dtrtri_(char *uplo, char *diag, int *n, double *a, int *lda, int *info);
+#endif
+
+int allocate_work(double** work)
+{
+ unsigned int dimWork = (unsigned int)((*work)[0]);
+ delete[] *work;
+ *work = new double[dimWork];
+ return (int)dimWork;
+}
+#ifdef VISP_HAVE_LAPACK_C
+vpMatrix vpMatrix::inverseByQRLapack() const{
+ int rowNum_ = (int)this->getRows();
+ int colNum_ = (int)this->getCols();
+ int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
+ int dimTau = std::min(rowNum_,colNum_);
+ int dimWork = -1;
+ double *tau = new double[dimTau];
+ double *work = new double[1];
+ int info;
+ vpMatrix C;
+ vpMatrix A = *this;
+
+ try{
+ //1) Extract householder reflections (useful to compute Q) and R
+ dgeqrf_(
+ &rowNum_, //The number of rows of the matrix A. M >= 0.
+ &colNum_, //The number of columns of the matrix A. N >= 0.
+ A.data, /*On entry, the M-by-N matrix A.
+ On exit, the elements on and above the diagonal of the array
+ contain the min(M,N)-by-N upper trapezoidal matrix R (R is
+ upper triangular if m >= n); the elements below the diagonal,
+ with the array TAU, represent the orthogonal matrix Q as a
+ product of min(m,n) elementary reflectors.
+ */
+ &lda, //The leading dimension of the array A. LDA >= max(1,M).
+ tau, /*Dimension (min(M,N))
+ The scalar factors of the elementary reflectors
+ */
+ work, //Internal working array. dimension (MAX(1,LWORK))
+ &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
+ &info //status
+ );
+
+ if(info != 0){
+ std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+ dimWork = allocate_work(&work);
+
+ dgeqrf_(
+ &rowNum_, //The number of rows of the matrix A. M >= 0.
+ &colNum_, //The number of columns of the matrix A. N >= 0.
+ A.data, /*On entry, the M-by-N matrix A.
+ On exit, the elements on and above the diagonal of the array
+ contain the min(M,N)-by-N upper trapezoidal matrix R (R is
+ upper triangular if m >= n); the elements below the diagonal,
+ with the array TAU, represent the orthogonal matrix Q as a
+ product of min(m,n) elementary reflectors.
+ */
+ &lda, //The leading dimension of the array A. LDA >= max(1,M).
+ tau, /*Dimension (min(M,N))
+ The scalar factors of the elementary reflectors
+ */
+ work, //Internal working array. dimension (MAX(1,LWORK))
+ &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
+ &info //status
+ );
+
+
+ if(info != 0){
+ std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+
+ //A now contains the R matrix in its upper triangular (in lapack convention)
+ C = A;
+
+ //2) Invert R
+ dtrtri_((char*)"U",(char*)"N",&dimTau,C.data,&lda,&info);
+ if(info!=0){
+ if(info < 0)
+ std::cout << "dtrtri_:"<< -info << "th element had an illegal value" << std::endl;
+ else if(info > 0){
+ std::cout << "dtrtri_:R("<< info << "," <<info << ")"<< " is exactly zero. The triangular matrix is singular and its inverse can not be computed." << std::endl;
+ std::cout << "R=" << std::endl << C << std::endl;
+ }
+ throw vpMatrixException::badValue;
+ }
+
+ //3) Zero-fill R^-1
+ //the matrix is upper triangular for lapack but lower triangular for visp
+ //we fill it with zeros above the diagonal (where we don't need the values)
+ for(unsigned int i=0;i<C.getRows();i++)
+ for(unsigned int j=0;j<C.getRows();j++)
+ if(j>i) C[i][j] = 0.;
+
+ dimWork = -1;
+ int ldc = lda;
+
+ //4) Transpose Q and left-multiply it by R^-1
+ //get R^-1*tQ
+ //C contains R^-1
+ //A contains Q
+ dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
+ if(info != 0){
+ std::cout << "dormqr_:Preparation"<< -info << "th element had an illegal value" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+ dimWork = allocate_work(&work);
+
+ dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
+
+ if(info != 0){
+ std::cout << "dormqr_:"<< -info << "th element had an illegal value" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+ delete[] tau;
+ delete[] work;
+ }catch(vpMatrixException&){
+ delete[] tau;
+ delete[] work;
+ throw;
+ }
+
+ return C;
+
+}
+#endif
+
+/*!
+ Compute the inverse of a n-by-n matrix using the QR decomposition.
+ Only available if lapack is installed.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByQR();
+ std::cout << "Inverse by QR: \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse()
+*/
+
+#if defined(VISP_HAVE_LAPACK_C)
+vpMatrix
+vpMatrix::inverseByQR() const
+{
+
+ if ( rowNum != colNum)
+ {
+ vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
+ throw(vpMatrixException(vpMatrixException::matrixError,
+ "Cannot invert a non-square vpMatrix")) ;
+ }
+#ifdef VISP_HAVE_LAPACK_C
+ return inverseByQRLapack();
+#endif
+}
+
+#endif
diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp
new file mode 100644
index 0000000..81dbf4d
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp
@@ -0,0 +1,713 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Matrix SVD decomposition.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpDebug.h>
+
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <iostream>
+
+/*---------------------------------------------------------------------
+
+SVD related functions
+
+---------------------------------------------------------------------*/
+
+
+static double pythag(double a, double b)
+{
+ double absa, absb;
+ absa = fabs(a);
+ absb = fabs(b);
+ if (absa > absb) return absa*sqrt(1.0+vpMath::sqr(absb/absa));
+ //else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
+ else return (std::fabs(absb) <= std::numeric_limits<double>::epsilon() ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
+}
+
+#ifdef SIGN
+#undef SIGN
+#endif
+#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
+
+/*!
+ \brief Singular value decomposition
+
+ Given a matrix A (m x n) this routine compute its sngular value decomposition
+ A = U W V^T. The matrice U replace A on output. the diagonal matrix of
+ singular value is output as a vector W (n). The matrix V (not the transpose
+ V^T) is output as V (n x n)
+
+
+ \warning Destructive wrt A
+ \warning
+
+
+ \sa SVD for a more intuitive use
+
+
+ This function is extracted from the NRC
+
+*/
+
+#define MAX_ITER_SVD 50
+void vpMatrix::svdNr(vpColVector& W, vpMatrix& V)
+{
+
+ unsigned int m = rowNum;
+ unsigned int n = colNum;
+ double epsilon = 10*std::numeric_limits<double>::epsilon();
+
+ unsigned int flag,i,its,j,jj,k,l=0,nm=0;
+ double c,f,h,s,x,y,z;
+ double anorm=0.0,g=0.0,scale=0.0;
+
+ // So that the original NRC code (using 1..n indexing) can be used
+ // This should be considered as a temporary fix.
+ double **a = new double*[m+1];
+ double **v = new double*[n+1];
+ // double **w = W.rowPtrs;
+ // w--;
+
+ double *w = new double[n+1];
+ for (i=0;i<n;i++) w[i+1] = 0.0;
+
+ for (i=1;i<=m;i++) {
+ a[i] = this->rowPtrs[i-1]-1;
+ }
+ for (i=1;i<=n;i++) {
+ v[i] = V.rowPtrs[i-1]-1;
+ }
+
+ if (m < n)
+ {
+ delete[] w;
+ delete[] a;
+ delete[] v;
+ vpERROR_TRACE("\n\t\tSVDcmp: You must augment A with extra zero rows") ;
+ throw(vpMatrixException(vpMatrixException::matrixError,
+ "\n\t\tSVDcmp: You must augment A with "
+ "extra zero rows")) ;
+ }
+ double* rv1=new double[n+1];
+
+ for (i=1;i<=n;i++) {
+ l=i+1;
+ rv1[i]=scale*g;
+ g=s=scale=0.0;
+ if (i <= m) {
+ for (k=i;k<=m;k++) scale += fabs(a[k][i]);
+ //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
+ if ((std::fabs(scale) > epsilon)/* || (fabs(scale) > EPS_SVD)*/) {
+ for (k=i;k<=m;k++) {
+ a[k][i] /= scale;
+ s += a[k][i]*a[k][i];
+ }
+ f=a[i][i];
+ g = -SIGN(sqrt(s),f);
+ h=f*g-s;
+ a[i][i]=f-g;
+ if (i != n) {
+ for (j=l;j<=n;j++) {
+ for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
+ f=s/h;
+ for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+ }
+ }
+ for (k=i;k<=m;k++) a[k][i] *= scale;
+ }
+ }
+ w[i]=scale*g;
+ g=s=scale=0.0;
+ if (i <= m && i != n) {
+ for (k=l;k<=n;k++) scale += fabs(a[i][k]);
+ //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
+ if ((std::fabs(scale) > epsilon) /*|| (fabs(scale) > EPS_SVD)*/) {
+ for (k=l;k<=n;k++) {
+ a[i][k] /= scale;
+ s += a[i][k]*a[i][k];
+ }
+ f=a[i][l];
+ g = -SIGN(sqrt(s),f);
+ h=f*g-s;
+ a[i][l]=f-g;
+ for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
+ if (i != m) {
+ for (j=l;j<=m;j++) {
+ for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
+ for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
+ }
+ }
+ for (k=l;k<=n;k++) a[i][k] *= scale;
+ }
+ }
+ anorm=vpMath::maximum(anorm,(fabs(w[i])+fabs(rv1[i])));
+ }
+ for (i=n;i>=1;i--) {
+ if (i < n) {
+ //if ((g) || (fabs(g) > EPS_SVD)) {
+ if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
+ for (j=l;j<=n;j++)
+ v[j][i]=(a[i][j]/a[i][l])/g;
+ for (j=l;j<=n;j++) {
+ for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
+ for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
+ }
+ }
+ for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
+ }
+ v[i][i]=1.0;
+ g=rv1[i];
+ l=i;
+ }
+ for (i=n;i>=1;i--) {
+ l=i+1;
+ g=w[i];
+ if (i < n)
+ for (j=l;j<=n;j++) a[i][j]=0.0;
+ //if ((g != 0.0) || (fabs(g) > EPS_SVD)) {
+ if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
+ g=1.0/g;
+ if (i != n) {
+ for (j=l;j<=n;j++) {
+ for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
+ f=(s/a[i][i])*g;
+ for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+ }
+ }
+ for (j=i;j<=m;j++) a[j][i] *= g;
+ } else {
+ for (j=i;j<=m;j++) a[j][i]=0.0;
+ }
+ ++a[i][i];
+ }
+ for (k=n;k>=1;k--) {
+ for (its=1;its<=MAX_ITER_SVD;its++) {
+ flag=1;
+ for (l=k;l>=1;l--) {
+ nm=l-1;
+ //if ((fabs(rv1[l])+anorm == anorm) || (fabs(rv1[l]) <= EPS_SVD)) {
+ if ((std::fabs(rv1[l]) <= epsilon) /*|| (fabs(rv1[l]) <= EPS_SVD)*/) {
+ flag=0;
+ break;
+ }
+ //if ((fabs(w[nm])+anorm == anorm) || (fabs(w[nm]) <= EPS_SVD)) break;
+ if ((std::fabs(w[nm]) <= epsilon) /*|| (fabs(w[nm]) <= EPS_SVD)*/) break;
+ }
+ if (flag) {
+ c=0.0;
+ s=1.0;
+ for (i=l;i<=k;i++) {
+ f=s*rv1[i];
+ //if ((fabs(f)+anorm != anorm) || (fabs(f) <= EPS_SVD)) {
+ if ((std::fabs(f) > epsilon) /*|| (fabs(f) <= EPS_SVD)*/) {
+ g=w[i];
+ h=pythag(f,g);
+ w[i]=h;
+ h=1.0/h;
+ c=g*h;
+ s=(-f*h);
+ for (j=1;j<=m;j++) {
+ y=a[j][nm];
+ z=a[j][i];
+ a[j][nm]=y*c+z*s;
+ a[j][i]=z*c-y*s;
+ }
+ }
+ }
+ }
+ z=w[k];
+ if (l == k) {
+ if (z < 0.0) {
+ w[k] = -z;
+ for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
+ }
+ break;
+ }
+ if (its == MAX_ITER_SVD)
+ {
+ for (i=0;i<n;i++) W[i] = w[i+1];
+
+ vpERROR_TRACE("\n\t\t No convergence in SVDcmp ") ;
+ std::cout << *this <<std::endl ;
+ // throw(vpMatrixException(vpMatrixException::matrixError,
+ // "\n\t\t No convergence in SVDcmp ")) ;
+ }
+ x=w[l];
+ nm=k-1;
+ y=w[nm];
+ g=rv1[nm];
+ h=rv1[k];
+ f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
+ g=pythag(f,1.0);
+ f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
+ c=s=1.0;
+ for (j=l;j<=nm;j++) {
+ i=j+1;
+ g=rv1[i];
+ y=w[i];
+ h=s*g;
+ g=c*g;
+ z=pythag(f,h);
+ rv1[j]=z;
+ if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
+ c=f/z;
+ s=h/z;
+ }
+ f=x*c+g*s;
+ g=g*c-x*s;
+ h=y*s;
+ y=y*c;
+ for (jj=1;jj<=n;jj++) {
+ x=v[jj][j];
+ z=v[jj][i];
+ v[jj][j]=x*c+z*s;
+ v[jj][i]=z*c-x*s;
+ }
+ z=pythag(f,h);
+ w[j]=z;
+ //if ((z != 0.0) || (fabs(z) > EPS_SVD)) {
+ if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
+ z=1.0/z;
+ c=f*z;
+ s=h*z;
+ }
+ f=(c*g)+(s*y);
+ x=(c*y)-(s*g);
+ for (jj=1;jj<=m;jj++) {
+ y=a[jj][j];
+ z=a[jj][i];
+ a[jj][j]=y*c+z*s;
+ a[jj][i]=z*c-y*s;
+ }
+ }
+ rv1[l]=0.0;
+ rv1[k]=f;
+ w[k]=x;
+ }
+ }
+ for (i=0;i<n;i++) W[i] = w[i+1];
+
+
+ delete[] w;
+ delete[] rv1;
+ delete[] a;
+ delete[] v;
+
+}
+
+#undef SIGN
+#undef PYTHAG
+
+/*!
+ \brief solve a linear system AX = B using an SVD decomposition
+
+ Solves AX = B for a vector X, where A is am matrix m x n, w a vector (n) and
+ v a matrix (n x n) as returned by SVDcmp. m and n are the dimensions of A,
+ and will be equal for square matrices. b (m) is the input right-hand
+ side. x (n) is the output solution vector. No input quantities are
+ destroyed, so the routine may be called sequentially with different b's.
+
+ \warning not to be used directly
+
+ \sa to be used with svd first
+
+ \sa solveBySVD and SVDsolve for a more intuitive solution of AX=B problem
+*/
+
+
+void vpMatrix::SVBksb( const vpColVector& w,
+ const vpMatrix& v,
+ const vpColVector& b, vpColVector& x)
+{
+ unsigned int m = this->rowNum;
+ unsigned int n = this->colNum;
+ double** u = rowPtrs;
+
+ unsigned int jj,j,i;
+ double s,*tmp;
+
+ tmp=new double[n];
+ for (j=0;j<n;j++) {
+ s=0.0;
+ //if (w[j])
+ if (std::fabs(w[j]) > std::numeric_limits<double>::epsilon())
+ {
+ for (i=0;i<m;i++) s += u[i][j]*b[i];
+ s /= w[j];
+ }
+ tmp[j]=s;
+ }
+ for (j=0;j<n;j++) {
+ s=0.0;
+ for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
+ x[j]=s;
+ }
+ delete [] tmp;
+}
+
+#define TOL 1.0e-5
+
+/*!
+ \brief Compute the SVD decomposition
+
+ Computes the singular value decomposition of the matrix, U.
+ The contents of U are replaced such that A = U*S*V' where A represents
+ the initial value of U.
+ S is understood to have only room for ncol elements.
+ The matrix V may be NULL, in which case, no values are returned for V.
+
+ This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
+ for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
+
+ Gary William Flake
+ http://research.yahoo.com/~flakeg/nodelib/html/
+ http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html (not valid)
+
+ \sa SVDcmp and SVDksb
+*/
+
+#define TOLERANCE 1.0e-7
+
+static
+void svd_internal_use(double *U, double *S, double *V,
+ unsigned int nRow, unsigned int nCol)
+{
+ unsigned int i, j, k, EstColRank, RotCount, SweepCount, slimit;
+ double eps, e2, tol, vt, p, x0, y0, q, r, c0, s0, d1, d2;
+
+ eps = TOLERANCE;
+ slimit = nCol / 4;
+ if (slimit < 6.0)
+ slimit = 6;
+ SweepCount = 0;
+ e2 = 10.0 * nRow * eps * eps;
+ tol = eps * .1;
+ EstColRank = nCol;
+ if(V)
+ for (i = 0; i < nCol; i++)
+ for (j = 0; j < nCol; j++) {
+ V[nCol * i + j] = 0.0;
+ V[nCol * i + i] = 1.0;
+ }
+ RotCount = EstColRank * (EstColRank - 1) / 2;
+ while (RotCount != 0 && SweepCount <= slimit) {
+ RotCount = EstColRank * (EstColRank - 1) / 2;
+ SweepCount++;
+ for (j = 0; j < EstColRank - 1; j++) {
+ for (k = j + 1; k < EstColRank; k++) {
+ p = q = r = 0.0;
+ for (i = 0; i < nRow; i++) {
+ x0 = U[nCol * i + j];
+ y0 = U[nCol * i + k];
+ p += x0 * y0;
+ q += x0 * x0;
+ r += y0 * y0;
+ }
+ S[j] = q;
+ S[k] = r;
+ if (q >= r) {
+ if (q <= e2 * S[0] || fabs(p) <= tol * q)
+ RotCount--;
+ else {
+ p /= q;
+ r = 1 - r / q;
+ vt = sqrt(4 * p * p + r * r);
+ c0 = sqrt(fabs(.5 * (1 + r / vt)));
+ s0 = p / (vt * c0);
+ for (i = 0; i < nRow; i++) {
+ d1 = U[nCol * i + j];
+ d2 = U[nCol * i + k];
+ U[nCol * i + j] = d1 * c0 + d2 * s0;
+ U[nCol * i + k] = -d1 * s0 + d2 * c0;
+ }
+ if(V)
+ for (i = 0; i < nCol; i++) {
+ d1 = V[nCol * i + j];
+ d2 = V[nCol * i + k];
+ V[nCol * i + j] = d1 * c0 + d2 * s0;
+ V[nCol * i + k] = -d1 * s0 + d2 * c0;
+ }
+ }
+ }
+ else {
+ p /= r;
+ q = q / r - 1;
+ vt = sqrt(4 * p * p + q * q);
+ s0 = sqrt(fabs(.5 * (1 - q / vt)));
+ if (p < 0)
+ s0 = -s0;
+ c0 = p / (vt * s0);
+ for (i = 0; i < nRow; i++) {
+ d1 = U[nCol * i + j];
+ d2 = U[nCol * i + k];
+ U[nCol * i + j] = d1 * c0 + d2 * s0;
+ U[nCol * i + k] = -d1 * s0 + d2 * c0;
+ }
+ if(V)
+ for (i = 0; i < nCol; i++) {
+ d1 = V[nCol * i + j];
+ d2 = V[nCol * i + k];
+ V[nCol * i + j] = d1 * c0 + d2 * s0;
+ V[nCol * i + k] = -d1 * s0 + d2 * c0;
+ }
+ }
+ }
+ }
+ while (EstColRank >= 3 && S[(EstColRank - 1)] <= S[0] * tol + tol * tol)
+ EstColRank--;
+ }
+ for(i = 0; i < nCol; i++)
+ S[i] = sqrt(S[i]);
+ for(i = 0; i < nCol; i++)
+ for(j = 0; j < nRow; j++)
+ U[nCol * j + i] = U[nCol * j + i] / S[i];
+}
+
+/*!
+ \brief Singular value decomposition (other function)
+
+ Given a matrix A (m x n) this routine compute its singular value decomposition
+ A = U W V^T. The matrice U replace A on output. the diagonal matrix of
+ singular value is output as a vector W (n). The matrix V (not the transpose
+ V^T) is output as V (n x n)
+
+
+ \warning Destructive wrt A
+ \warning
+
+
+ \sa SVD for a more intuitive use
+
+ This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
+ for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
+
+ http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html
+ http://labs.yahoo.com/~flakeg/nodelib/html/svd.html
+
+ \sa SVDcmp and SVDksb
+
+*/
+
+void vpMatrix::svdFlake(vpColVector &W, vpMatrix &V)
+{
+
+
+ svd_internal_use(data, W.data, V.data, getRows(), getCols());
+}
+
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/core/core.hpp>
+void vpMatrix::svdOpenCV(vpColVector& w, vpMatrix& v){
+ int rows = (int)this->getRows();
+ int cols = (int)this->getCols();
+ cv::Mat m(rows, cols, CV_64F, this->data);
+ cv::SVD opencvSVD(m);
+ cv::Mat opencvV = opencvSVD.vt;
+ cv::Mat opencvW = opencvSVD.w;
+ v.resize((unsigned int)opencvV.rows, (unsigned int)opencvV.cols);
+ w.resize((unsigned int)(opencvW.rows*opencvW.cols));
+
+ memcpy(v.data, opencvV.data, (size_t)(8*opencvV.rows*opencvV.cols));
+ v=v.transpose();
+ memcpy(w.data, opencvW.data, (size_t)(8*opencvW.rows*opencvW.cols));
+ this->resize((unsigned int)opencvSVD.u.rows, (unsigned int)opencvSVD.u.cols);
+ memcpy(this->data,opencvSVD.u.data, (size_t)(8*opencvSVD.u.rows*opencvSVD.u.cols));
+}
+
+#endif
+
+#ifdef VISP_HAVE_LAPACK_C
+extern "C" int dgesdd_(char *jobz, int *m, int *n, double *a, int *lda, double *s, double *u, int *ldu, double *vt, int *ldvt, double *work, int *lwork, int *iwork, int *info);
+#include <stdio.h>
+#include <string.h>
+
+void vpMatrix::svdLapack(vpColVector& W, vpMatrix& V){
+ /* unsigned */ int m = static_cast<int>(this->getCols()), n = static_cast<int>(this->getRows()), lda = m, ldu = m, ldvt = std::min(m,n);
+ int info, lwork;
+
+ double wkopt;
+ double* work;
+
+ int* iwork = new int[8*static_cast<unsigned int>(std::min(n,m))];
+
+ double *s = W.data;
+ double* a = new double[static_cast<unsigned int>(lda*n)];
+ memcpy(a,this->data,this->getRows()*this->getCols()*sizeof(double));
+ double* u = V.data;
+ double* vt = this->data;
+
+
+
+ lwork = -1;
+ dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info );
+ lwork = (int)wkopt;
+ work = new double[static_cast<unsigned int>(lwork)];
+
+ dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info );
+
+ if( info > 0 ) {
+ vpTRACE("The algorithm computing SVD failed to converge.");
+ throw(vpMatrixException(vpMatrixException::fatalError,
+ "The algorithm computing SVD failed to converge.")) ;
+
+ }
+
+ V=V.transpose();
+ delete[] work;
+ delete[] iwork;
+ delete[] a;
+}
+#endif
+
+#ifdef VISP_HAVE_GSL
+#include <gsl/gsl_linalg.h>
+
+void
+vpMatrix::svdGsl(vpColVector& w, vpMatrix& v)
+{
+
+#if 0
+ // premier test avec la gsl 1. on recopie...
+ int i,j ;
+
+ int nc = getCols() ;
+ int nr = getRows() ;
+ gsl_matrix *A = gsl_matrix_alloc(nr, nc) ;
+
+ int Atda = A->tda ;
+ for (i=0 ; i < nr ; i++)
+ {
+ int k = i*Atda ;
+ for (j=0 ; j < nc ; j++)
+ A->data[k+j] = (*this)[i][j] ;
+ }
+ // gsl_matrix_set(A,i,j,(*this)[i][j]) ;
+
+ gsl_matrix *V = gsl_matrix_alloc(nc, nc) ;
+ gsl_vector *S = gsl_vector_alloc(nc) ;
+ gsl_vector *work = gsl_vector_alloc(nc) ;
+
+ gsl_linalg_SV_decomp(A,V,S, work) ;
+// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
+
+
+ //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
+ // autremement (surement !)
+
+ Atda = A->tda ;
+ for (i=0 ; i < nr ; i++)
+ for (j=0 ; j < nc ; j++)
+ (*this)[i][j] = gsl_matrix_get(A,i,j) ;
+
+ int Vtda = V->tda ;
+ for (i=0 ; i < nc ; i++)
+ {
+ int k = i*Vtda ;
+ for (j=0 ; j < nc ; j++)
+ v[i][j] = V->data[k+j] ;
+ }
+
+ for (j=0 ; j < nc ; j++)
+ w[j] = gsl_vector_get(S,j) ;
+
+
+ gsl_matrix_free(V) ;
+ gsl_matrix_free(A) ;
+ gsl_vector_free(S) ;
+ gsl_vector_free(work) ;
+
+#else //optimisation Anthony 20/03/2008
+
+ unsigned int nc = getCols() ;
+ unsigned int nr = getRows() ;
+ gsl_vector *work = gsl_vector_alloc(nc) ;
+
+// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
+
+
+ //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
+ // autremement (surement !)
+
+ gsl_matrix A;
+ A.size1 = nr;
+ A.size2 = nc;
+ A.tda = A.size2;
+ A.data = this->data;
+ A.owner = 0;
+ A.block = 0;
+
+ gsl_matrix V;
+ V.size1 = nc;
+ V.size2 = nc;
+ V.tda = V.size2;
+ V.data = v.data;
+ V.owner = 0;
+ V.block = 0;
+
+ gsl_vector S;
+ S.size = nc;
+ S.stride = 1;
+ S.data = w.data;
+ S.owner = 0;
+ S.block = 0;
+
+ gsl_linalg_SV_decomp(&A,&V,&S, work) ;
+
+ gsl_vector_free(work) ;
+
+#endif
+}
+#endif // # #GSL
+
+
+#undef TOL
+#undef TOLERANCE
+
+#undef MAX_ITER_SVD
+
+#endif // doxygen should skip this
diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp
new file mode 100644
index 0000000..5412ea0
--- /dev/null
+++ b/modules/core/src/math/matrix/vpRowVector.cpp
@@ -0,0 +1,1015 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Operation on row vectors.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpRowVector.cpp
+ \brief Definition of vpRowVector class member
+*/
+
+#include <string.h>
+#include <stdlib.h>
+#include <sstream>
+#include <assert.h>
+
+#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpRowVector.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+
+//! Copy operator. Allow operation such as A = v
+vpRowVector & vpRowVector::operator=(const vpRowVector &v)
+{
+ unsigned int k = v.colNum ;
+ if (colNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw;
+ }
+ }
+
+ memcpy(data, v.data, colNum*sizeof(double)) ;
+
+ return *this;
+}
+
+/*!
+ Initialize a row vector from a 1-by-n size matrix.
+ \warning Handled with care m should be a 1 column matrix.
+
+ \exception vpException::dimensionError If the matrix is not a 1-by-n dimension matrix.
+*/
+vpRowVector & vpRowVector::operator=(const vpMatrix &M)
+{
+ if (M.getRows() != 1 ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
+ M.getCols(), M.getRows(), M.getCols())) ;
+ }
+
+ if (M.getCols() != colNum)
+ resize(M.getCols());
+
+ memcpy(data, M.data, colNum*sizeof(double)) ;
+ return *this;
+}
+
+/*!
+ Initialize a row vector from a standard vector of double.
+*/
+vpRowVector & vpRowVector::operator=(const std::vector<double> &v)
+{
+ resize((unsigned int)v.size());
+ for(unsigned int i=0; i<v.size(); i++)
+ (*this)[i] = v[i];
+ return *this;
+}
+/*!
+ Initialize a row vector from a standard vector of double.
+*/
+vpRowVector & vpRowVector::operator=(const std::vector<float> &v)
+{
+ resize((unsigned int)v.size());
+ for(unsigned int i=0; i<v.size(); i++)
+ (*this)[i] = (float)v[i];
+ return *this;
+}
+
+//! Initialize each element of the vector with \e x.
+vpRowVector & vpRowVector::operator=(double x)
+{
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<colNum; j++) {
+ rowPtrs[i][j] = x;
+ }
+ }
+ return *this;
+}
+
+/*!
+
+ Multiply a row vector by a column vector.
+
+ \param x : Column vector.
+
+ \warning The number of elements of the two vectors must be equal.
+
+ \exception vpException::dimensionError : If the number of elements of the
+ two vectors is not the same.
+
+ \return A scalar.
+
+*/
+double vpRowVector::operator*(const vpColVector &x) const
+{
+ unsigned int nelements = x.getRows();
+ if (getCols() != nelements ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (1x%d) row vector by (%dx1) column vector",
+ colNum, x.getRows())) ;
+ }
+
+ double scalar = 0.0;
+
+ for (unsigned int i=0; i<nelements; i++) {
+ scalar += (*this)[i] * x[i];
+ }
+ return scalar;
+}
+/*!
+
+ Multiply a row vector by a matrix.
+
+ \param M : Matrix.
+
+ \warning The number of elements of the row vector must be equal to the number
+ of rows of the matrix.
+
+ \exception vpException::dimensionError If the number of elements of the
+ row vector is not equal to the number of rows of the matrix.
+
+ \return The resulting row vector.
+
+*/
+vpRowVector vpRowVector::operator*(const vpMatrix &M) const
+{
+ vpRowVector c(M.getCols());
+
+ if (colNum != M.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (1x%d) row vector by (%dx%d) matrix",
+ colNum, M.getRows(), M.getCols())) ;
+ }
+
+ c = 0.0;
+
+ for (unsigned int i=0;i<colNum;i++) {
+ double bi = data[i] ; // optimization em 5/12/2006
+ for (unsigned int j=0;j<M.getCols();j++) {
+ c[j]+=bi*M[i][j];
+ }
+ }
+
+ return c ;
+}
+
+/*!
+ Operator that allows to multiply each element of a row vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The row vector multiplied by the scalar. The current
+ row vector (*this) is unchanged.
+
+ \code
+ vpRowVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+
+ vpRowVector w = v * 3;
+ // v is unchanged
+ // w is now equal to : [3 6 9]
+ \endcode
+*/
+vpRowVector vpRowVector::operator*(double x) const
+{
+ vpRowVector v(colNum);
+
+ double *vd = v.data ; double *d = data ;
+
+ for (unsigned int i=0;i<colNum;i++)
+ *(vd++) = (*d++) * x;
+ return v;
+}
+
+/*!
+ Operator that allows to multiply each element of a row vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The row vector multiplied by the scalar.
+
+ \code
+ vpRowVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+
+ v *= 3;
+ // v is now equal to : [3 6 9]
+ \endcode
+*/
+vpRowVector &vpRowVector::operator*=(double x)
+{
+ for (unsigned int i=0;i<colNum;i++)
+ (*this)[i] *= x;
+ return (*this);
+}
+
+/*!
+ Operator that allows to divide each element of a row vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The row vector divided by the scalar. The current
+ row vector (*this) is unchanged.
+
+ \code
+ vpRowVector v(3);
+ v[0] = 8;
+ v[1] = 4;
+ v[2] = 2;
+
+ vpRowVector w = v / 2;
+ // v is equal to : [8 4 2]
+ // w is equal to : [4 2 1]
+ \endcode
+*/
+vpRowVector vpRowVector::operator/(double x) const
+{
+ vpRowVector v(colNum);
+
+ double *vd = v.data ; double *d = data ;
+
+ for (unsigned int i=0;i<colNum;i++)
+ *(vd++) = (*d++) / x;
+ return v;
+}
+
+/*!
+ Operator that allows to divide each element of a row vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The row vector divided by the scalar.
+
+ \code
+ vpRowVector v(3);
+ v[0] = 8;
+ v[1] = 4;
+ v[2] = 2;
+ // v is equal to : [8 4 2]
+
+ v /= 2;
+ // v is equal to : [4 2 1]
+ \endcode
+*/
+vpRowVector &vpRowVector::operator/=(double x)
+{
+ for (unsigned int i=0;i<colNum;i++)
+ (*this)[i] /= x;
+ return (*this);
+}
+
+/*!
+ Operator that allows to negate all the row vector elements.
+
+ \code
+ vpRowVector r(3, 1);
+ // r contains [1 1 1]
+ vpRowVector v = -r;
+ // v contains [-1 -1 -1]
+ \endcode
+ */
+vpRowVector vpRowVector::operator-() const
+{
+ vpRowVector A(colNum);
+
+ double *vd = A.data ; double *d = data ;
+
+ for (unsigned int i=0; i<colNum; i++)
+ *(vd++)= - (*d++);
+
+ return A;
+}
+
+/*!
+ Operator that allows to substract to row vectors that have the same size.
+ \exception vpException::dimensionError If the vectors size differ.
+ */
+vpRowVector vpRowVector::operator-(const vpRowVector &m) const
+{
+ if (getCols() != m.getCols() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (1x%d) row vector to (1x%d) row vector",
+ getCols(), m.getCols())) ;
+ }
+
+ vpRowVector v(colNum) ;
+
+ for (unsigned int i=0;i<colNum;i++)
+ v[i] = (*this)[i] - m[i];
+ return v;
+}
+
+/*!
+ Operator that allows to add to row vectors that have the same size.
+ \exception vpException::dimensionError If the vectors size differ.
+ */
+vpRowVector vpRowVector::operator+(const vpRowVector &v) const
+{
+ if (getCols() != v.getCols() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (1x%d) row vector to (1x%d) row vector",
+ getCols(), v.getCols())) ;
+ }
+
+ vpRowVector r(colNum) ;
+
+ for (unsigned int i=0;i<colNum;i++)
+ r[i] = (*this)[i] + v[i];
+ return r;
+}
+
+/*!
+ Operator that allows to add two row vectors that have the same size.
+ \exception vpException::dimensionError If the size of the two vectors differ.
+ */
+vpRowVector &
+vpRowVector::operator+=(vpRowVector v)
+{
+ if (getCols() != v.getCols() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot add (1x%d) row vector to (1x%d) row vector",
+ getCols(), v.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<colNum;i++)
+ (*this)[i] += v[i];
+ return (*this);
+}
+
+/*!
+ Operator that allows to substract two row vectors that have the same size.
+ \exception vpException::dimensionError If the size of the two vectors differ.
+ */
+vpRowVector &
+vpRowVector::operator-=(vpRowVector v)
+{
+ if (getCols() != v.getCols() ) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot substract (1x%d) row vector to (1x%d) row vector",
+ getCols(), v.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<colNum;i++)
+ (*this)[i] -= v[i];
+ return (*this);
+}
+
+/*!
+ Copy operator.
+ Allows operation such as A << v
+ \code
+#include <visp3/core/vpRowVector.h>
+
+int main()
+{
+ vpRowVector A, B(5);
+ for (unsigned int i=0; i<B.size(); i++)
+ B[i] = i;
+ A << B;
+ std::cout << "A: " << A << std::endl;
+}
+ \endcode
+ In row vector A we get:
+ \code
+A: 0 1 2 3 4
+ \endcode
+
+ */
+vpRowVector & vpRowVector::operator<<(const vpRowVector &v)
+{
+ *this = v;
+ return *this;
+}
+
+/*!
+ Transpose the row vector. The resulting vector becomes a column vector.
+*/
+vpColVector vpRowVector::t() const
+{
+ vpColVector v(colNum);
+ memcpy(v.data, data, colNum*sizeof(double)) ;
+ return v;
+}
+
+/*!
+ Transpose the row vector. The resulting vector becomes a column vector.
+ \sa t()
+*/
+vpColVector vpRowVector::transpose() const
+{
+ return t();
+}
+/*!
+ Transpose the row vector. The resulting vector \e v becomes a column vector.
+ \sa t()
+*/
+void vpRowVector::transpose(vpColVector &v) const
+{
+ v = t();
+}
+
+/*!
+ Constructor that creates a row vector corresponding to row \e i
+ of matrix \e M.
+ */
+vpRowVector::vpRowVector (const vpMatrix &M, unsigned int i)
+ : vpArray2D<double>(1, M.getCols())
+{
+ for(unsigned int j=0; j< M.getCols(); j++)
+ (*this)[j] = M[i][j];
+}
+/*!
+ Constructor that creates a row vector from a 1-by-n matrix \e M.
+
+ \exception vpException::dimensionError If the matrix is not a 1-by-n matrix.
+ */
+vpRowVector::vpRowVector (const vpMatrix &M)
+ : vpArray2D<double>(1, M.getCols())
+{
+ if(M.getRows()!=1) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
+ M.getCols(), M.getRows(), M.getCols())) ;
+ }
+
+ for(unsigned int j=0; j< M.getCols(); j++)
+ (*this)[j] = M[0][j];
+}
+
+/*!
+ Constructor that creates a row vector from a std vector of double.
+ */
+vpRowVector::vpRowVector (const std::vector<double> &v)
+ : vpArray2D<double>(1, (unsigned int)v.size())
+{
+ for(unsigned int j=0; j< v.size(); j++)
+ (*this)[j] = v[j];
+}
+/*!
+ Constructor that creates a row vector from a std vector of float.
+ */
+vpRowVector::vpRowVector (const std::vector<float> &v)
+ : vpArray2D<double>(1, (unsigned int)v.size())
+{
+ for(unsigned int j=0; j< v.size(); j++)
+ (*this)[j] = (double)(v[j]);
+}
+
+/*!
+ Construct a row vector from a part of an input row vector \e v.
+
+ \param v : Input row vector used for initialization.
+ \param c : column index in \e v that corresponds to the first element of the row vector to contruct.
+ \param ncols : Number of columns of the constructed row vector.
+
+ The sub-vector starting from v[c] element and ending on v[c+ncols-1] element
+ is used to initialize the contructed row vector.
+
+ \sa init()
+*/
+vpRowVector::vpRowVector (const vpRowVector &v, unsigned int c, unsigned int ncols)
+ : vpArray2D<double>(1, ncols)
+{
+ init(v, c, ncols);
+}
+
+/*!
+ Normalise the vector given as input parameter and return the normalized vector:
+
+ \f[
+ {\bf x} = \frac{{\bf x}}{\sqrt{\sum_{i=1}^{n}x^2_i}}
+ \f]
+ where \f$x_i\f$ is an element of the row vector \f$\bf x\f$.
+*/
+vpRowVector &vpRowVector::normalize(vpRowVector &x) const
+{
+ x = x/sqrt(x.sumSquare());
+
+ return x;
+}
+
+
+/*!
+ Normalise the vector modifying the vector as:
+
+ \f[
+ {\bf x} = \frac{{\bf x}}{\sqrt{\sum_{i=1}^{n}x^2_i}}
+ \f]
+ where \f$x_i\f$ is an element of the row vector \f$\bf x\f$.
+*/
+vpRowVector &vpRowVector::normalize()
+{
+ double sum_square = sumSquare();
+ if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
+ *this /= sqrt(sum_square) ;
+ }
+
+ // If sum = 0, we have a nul vector. So we return just.
+ return *this;
+}
+
+/*!
+ Reshape the row vector in a matrix.
+ \param nrows : number of rows of the matrix.
+ \param ncols : number of columns of the matrix.
+ \return The resulting matrix.
+
+ \exception vpException::dimensionError If the matrix and the row vector have not the same size.
+*/
+vpMatrix vpRowVector::reshape(const unsigned int &nrows,const unsigned int &ncols)
+{
+ vpMatrix M(nrows,ncols);
+ reshape(M, nrows, ncols);
+ return M;
+}
+
+/*!
+ Reshape the row vector in a matrix.
+ \param M : the reshaped matrix.
+ \param nrows : number of rows of the matrix.
+ \param ncols : number of columns of the matrix.
+
+ \exception vpException::dimensionError If the matrix and the row vector have not the same size.
+*/
+void vpRowVector::reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols){
+ if(dsize!=nrows*ncols) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot reshape (1x%d) row vector in (%dx%d) matrix",
+ colNum, M.getRows(), M.getCols())) ;
+ }
+ try {
+ if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
+ }
+ catch(...) {
+ throw ;
+ }
+ for(unsigned int i =0; i< nrows; i++)
+ for(unsigned int j =0; j< ncols; j++)
+ M[i][j]=data[i*nrows+j];
+}
+
+/*!
+ Insert a row vector.
+ \param i : Index of the first element to introduce. This index starts from 0.
+ \param v : Row vector to insert.
+
+ The following example shows how to use this function:
+ \code
+#include <visp/vpRowVector.h>
+
+int main()
+{
+ vpRowVector v(4);
+ for (unsigned int i=0; i < v.size(); i++)
+ v[i] = i;
+ std::cout << "v: " << v << std::endl;
+
+ vpRowVector w(2);
+ for (unsigned int i=0; i < w.size(); i++)
+ w[i] = i+10;
+ std::cout << "w: " << w << std::endl;
+
+ v.insert(1, w);
+ std::cout << "v: " << v << std::endl;
+} \endcode
+ It produces the following output:
+ \code
+v: 0 1 2 3
+w: 10 11
+v: 0 10 11 3
+ \endcode
+ */
+void vpRowVector::insert(unsigned int i, const vpRowVector &v)
+{
+ if (i+v.size() > this->size())
+ throw(vpException(vpException::dimensionError,
+ "Unable to insert (1x%d) row vector in (1x%d) row vector at position (%d)",
+ v.getCols(), colNum, i));
+ for (unsigned int j=0; j < v.size(); j++)
+ (*this)[i+j] = v[j];
+}
+
+/*!
+ Stack row vector with a new element at the end of the vector.
+
+ \param d : Element to stack to the existing one.
+
+ \code
+ vpRowVector v(3, 1);
+ // v is equal to [1 1 1]
+ v.stack(-2);
+ // v is equal to [1 1 1 -2]
+ \endcode
+
+ \sa stack(const vpRowVector &, const vpRowVector &)
+ \sa stack(const vpRowVector &, const vpRowVector &, vpRowVector &)
+
+*/
+void vpRowVector::stack(const double &d)
+{
+ this->resize(colNum+1,false);
+ (*this)[colNum-1] = d;
+}
+
+/*!
+ Stack row vectors.
+
+ \param v : Vector to stack to the existing one.
+
+ \code
+ vpRowVector v1(3, 1);
+ // v1 is equal to [1 1 1]
+ vpRowVector v2(2, 3);
+ // v2 is equal to [3 3]
+ v1.stack(v2);
+ // v1 is equal to [1 1 1 3 3]
+ \endcode
+
+ \sa stack(const vpRowVector &, const double &)
+ \sa stack(const vpRowVector &, const vpRowVector &)
+ \sa stack(const vpRowVector &, const vpRowVector &, vpRowVector &)
+
+*/
+void vpRowVector::stack(const vpRowVector &v)
+{
+ *this = vpRowVector::stack(*this, v);
+}
+
+/*!
+ Stack row vectors.
+
+ \param A : Initial vector.
+ \param B : Vector to stack at the end of A.
+ \return Stacked vector \f$[A B]\f$.
+
+ \code
+ vpRowVector r1(3, 1);
+ // r1 is equal to [1 1 1]
+ vpRowVector r2(2, 3);
+ // r2 is equal to [3 3]
+ vpRowVector v;
+ v = vpRowVector::stack(r1, r2);
+ // v is equal to [1 1 1 3 3]
+ \endcode
+
+ \sa stack(const vpRowVector &)
+ \sa stack(const vpRowVector &, const vpRowVector &, vpRowVector &)
+*/
+vpRowVector vpRowVector::stack(const vpRowVector &A, const vpRowVector &B)
+{
+ vpRowVector C;
+ vpRowVector::stack(A, B, C);
+ return C;
+}
+
+/*!
+ Stack row vectors.
+
+ \param A : Initial vector.
+ \param B : Vector to stack at the end of A.
+ \param C : Resulting stacked vector \f$C = [A B]\f$.
+
+ \code
+ vpRowVector r1(3, 1);
+ // r1 is equal to [1 1 1]
+ vpRowVector r2(2, 3);
+ // r2 is equal to [3 3]
+ vpRowVector v;
+ vpRowVector::stack(r1, r2, v);
+ // v is equal to [1 1 1 3 3]
+ \endcode
+
+ \sa stack(const vpRowVector &)
+ \sa stack(const vpRowVector &, const vpRowVector &)
+*/
+void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector &C)
+{
+ unsigned int nrA = A.getCols();
+ unsigned int nrB = B.getCols();
+
+ if (nrA == 0 && nrB == 0) {
+ C.resize(0);
+ return;
+ }
+
+ if (nrB == 0) {
+ C = A;
+ return;
+ }
+
+ if (nrA == 0) {
+ C = B;
+ return;
+ }
+
+ // General case
+ C.resize(nrA + nrB);
+
+ for (unsigned int i=0; i<nrA; i++)
+ C[i] = A[i];
+
+ for (unsigned int i=0; i<nrB; i++)
+ C[nrA+i] = B[i];
+}
+
+/*!
+ Compute the mean value of all the elements of the vector.
+*/
+double vpRowVector::mean(const vpRowVector &v)
+{
+ if (v.data == NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute mean value of an empty row vector"));
+ }
+
+ double mean = 0;
+ double *vd = v.data;
+ for (unsigned int i = 0; i < v.getCols(); i++)
+ mean += *(vd++);
+
+ return mean / v.getCols();
+}
+
+/*!
+ Compute the median value of all the elements of the vector.
+*/
+double
+vpRowVector::median(const vpRowVector &v)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute mean value of an empty row vector"));
+ }
+
+ std::vector<double> vectorOfDoubles(v.size());
+ for(unsigned int i = 0; i < v.size(); i++) {
+ vectorOfDoubles[i] = v[i];
+ }
+
+ return vpMath::getMedian(vectorOfDoubles);
+}
+
+/*!
+ Compute the standard deviation value of all the elements of the vector.
+*/
+double
+vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
+{
+ if (v.data==NULL) {
+ throw(vpException(vpException::fatalError,
+ "Cannot compute mean value of an empty row vector"));
+ }
+
+ double mean_value = mean(v);
+ double sum_squared_diff = 0.0;
+ for(unsigned int i = 0; i < v.size(); i++) {
+ sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
+ }
+
+ double divisor = (double) v.size();
+ if(useBesselCorrection && v.size() > 1) {
+ divisor = divisor-1;
+ }
+
+ return std::sqrt(sum_squared_diff / divisor);
+}
+
+/*!
+
+ Pretty print a row vector. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each row vector element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the vector.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all vector elements.
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+int
+vpRowVector::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = 1;
+ unsigned int n = getCols();
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int j=0;j<n;++j){
+ oss.str("");
+ oss << (*this)[j];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[j];
+ oss.str(ossFixed.str());
+ }
+
+ values[j]=oss.str();
+ size_type thislen=values[j].size();
+ size_type p=values[j].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+ }
+
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ s <<" ";
+ for (unsigned int j=0;j<n;j++){
+ size_type p=values[j].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[j].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[j].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+ }
+ s <<std::endl;
+
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
+
+/*!
+ Allows to multiply a scalar by row vector.
+*/
+vpRowVector operator*(const double &x,const vpRowVector &v)
+{
+ vpRowVector vout ;
+ vout = v*x ;
+ return vout ;
+}
+
+/*!
+ Return the sum square of all the elements \f$v_{i}\f$ of the row vector v(n).
+
+ \return The sum square value: \f$\sum_{j=0}^{n} v_j^{2}\f$.
+ */
+double vpRowVector::sumSquare() const
+{
+ double sum_square=0.0;
+ double x ;
+
+ for (unsigned int j=0;j<colNum;j++) {
+ x=rowPtrs[0][j];
+ sum_square += x*x;
+ }
+
+ return sum_square;
+}
+
+/*!
+ Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{v_{i}^2}} \f$.
+
+ \return The Euclidean norm if the vector is initialized, 0 otherwise.
+*/
+double vpRowVector::euclideanNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<dsize;i++) {
+ x = *(data +i); norm += x*x;
+ }
+
+ return sqrt(norm);
+}
+
+/*!
+ Initialize the row vector from a part of an input row vector \e v.
+
+ \param v : Input row vector used for initialization.
+ \param c : column index in \e v that corresponds to the first element of the row vector to contruct.
+ \param ncols : Number of columns of the constructed row vector.
+
+ The sub-vector starting from v[c] element and ending on v[c+ncols-1] element
+ is used to initialize the contructed row vector.
+
+ The following code shows how to use this function:
+\code
+#include <visp3/core/vpRowVector.h>
+
+int main()
+{
+ vpRowVector v(4);
+ int val = 0;
+ for(size_t i=0; i<v.getCols(); i++) {
+ v[i] = val++;
+ }
+ std::cout << "v: " << v << std::endl;
+
+ vpRowVector w;
+ w.init(v, 1, 2);
+ std::cout << "w: " << w << std::endl;
+}
+\endcode
+ It produces the following output:
+ \code
+v: 0 1 2 3
+w: 1 2
+ \endcode
+ */
+void
+vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
+{
+ unsigned int cncols = c+ncols ;
+
+ if (cncols > v.getCols())
+ throw(vpException(vpException::dimensionError,
+ "Bad column dimension (%d > %d) used to initialize vpRowVector",
+ cncols, v.getCols()));
+ resize(ncols);
+ if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
+ return; // Noting to do
+ for (unsigned int i=0 ; i < ncols; i++)
+ (*this)[i] = v[i+c];
+}
diff --git a/modules/core/src/math/matrix/vpSubColVector.cpp b/modules/core/src/math/matrix/vpSubColVector.cpp
new file mode 100644
index 0000000..92fe2e3
--- /dev/null
+++ b/modules/core/src/math/matrix/vpSubColVector.cpp
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpColVector.
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#include <stdlib.h>
+
+#include <visp3/core/vpSubColVector.h>
+#include <visp3/core/vpException.h>
+
+//! Default constructor that creates an empty vector.
+vpSubColVector::vpSubColVector()
+ : vpColVector(), pRowNum(0), parent(NULL)
+{
+}
+
+/*!
+ Construct a sub-column vector from a parent column vector.
+ \param v : parent column vector.
+ \param offset : offset where the sub-column vector starts in the parent column vector.
+ \param nrows : size of the sub-column vector.
+*/
+vpSubColVector::vpSubColVector(vpColVector &v, const unsigned int & offset, const unsigned int & nrows)
+ : vpColVector(), pRowNum(0), parent(NULL)
+{
+ init(v, offset, nrows);
+}
+
+/*!
+ Initialize a sub-column vector from a parent column vector.
+ \param v : parent column vector.
+ \param offset : offset where the sub-column vector starts in the parent column vector.
+ \param nrows : size of the sub-column vector.
+*/
+void vpSubColVector::init(vpColVector &v,
+ const unsigned int & offset,
+ const unsigned int & nrows)
+{
+ if (!v.data) {
+ throw(vpException(vpException::fatalError,
+ "Cannot initialize a sub-column vector from an empty parent column vector")) ;
+ }
+
+ if(offset+nrows<=v.getRows()){
+ data=v.data+offset;
+
+ rowNum=nrows;
+ colNum = 1;
+
+ pRowNum=v.getRows();
+ parent=&v;
+
+ if(rowPtrs){
+ free(rowPtrs);
+ }
+
+ rowPtrs=(double**)malloc( parent->getRows() * sizeof(double*));
+ for(unsigned int i=0;i<nrows;i++)
+ rowPtrs[i]=v.data+i+offset;
+
+ dsize = rowNum ;
+ } else {
+ throw(vpException(vpException::dimensionError,
+ "Cannot create a sub-column vector that is not completely containt in the parrent column vector")) ;
+ }
+}
+
+//! Destructor that set the pointer to the parrent column vector to NULL.
+vpSubColVector::~vpSubColVector(){
+ data=NULL ;
+}
+
+/*!
+ This method can be used to detect if the parent column vector
+ always exits or its size have not changed.
+ If this not the case an exception is thrown.
+*/
+void vpSubColVector::checkParentStatus() const{
+ if (!data) {
+ throw(vpException(vpException::fatalError,
+ "The parent of the current sub-column vector has been destroyed")) ;
+ }
+ if(pRowNum!=parent->getRows()){
+ throw(vpException(vpException::dimensionError,
+ "The size of the parent sub-column vector has changed")) ;
+ }
+}
+
+/*!
+ Allow to initialize a sub-column vector from an other one using operation A = B.
+ Notice that the sub-column vector is not resized to the dimension of \e B.
+
+ \param B : a sub-column vector.
+*/
+vpSubColVector & vpSubColVector::operator=(const vpSubColVector &B)
+{
+ if ( rowNum != B.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (%dx1) sub-column vector from (%dx1) sub-column vector",
+ rowNum, B.getRows())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i];
+ return *this;
+}
+
+/*!
+ Allow to initialize a sub-column vector from a column vector using operation A = B.
+ Notice that the sub-column vector is not resized to the dimension of \e B.
+ \param B : a column vector.
+*/
+vpSubColVector & vpSubColVector::operator=(const vpColVector &B)
+{
+ if ( rowNum != B.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (%dx1) sub-column vector from (%dx1) column vector",
+ rowNum, B.getRows())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i];
+
+ return *this;
+}
+
+/*!
+ Allow to initialize a sub-column vector from a m-by-1 matrix using operation A = B.
+ Notice that the sub-column vector is not resized to the dimension of \e B.
+ \param B : a matrix of size m-by-1.
+*/
+vpSubColVector & vpSubColVector::operator=(const vpMatrix &B)
+{
+ if ((B.getCols()!=1)||(rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (%dx1) sub-column vector from (%dx%d) matrix",
+ rowNum, B.getRows(), B.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i][1];
+ return *this;
+}
+
+/*!
+ Set all the elements of the sub-column vector to \e x.
+ \param x : a scalar value.
+*/
+vpSubColVector & vpSubColVector::operator=(const double &x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = x;
+ return *this;
+}
+
+/*!
+ Operator that allows to convert a translation vector into a column vector.
+ */
+vpSubColVector &vpSubColVector::operator=(const vpTranslationVector &tv)
+{
+ unsigned int k = tv.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+/*!
+ Operator that allows to convert a rotation vector into a column vector.
+ */
+vpSubColVector &vpSubColVector::operator=(const vpRotationVector &rv)
+{
+ unsigned int k = rv.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, rv.data, rowNum*sizeof(double)) ;
+ return *this;
+}
+/*!
+ Operator that allows to convert a pose vector into a column vector.
+ */
+vpSubColVector &vpSubColVector::operator=(const vpPoseVector &p)
+{
+ unsigned int k = p.getRows() ;
+ if (rowNum != k){
+ try {
+ resize(k);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, p.data, rowNum*sizeof(double)) ;
+ return *this;
+}
diff --git a/modules/core/src/math/matrix/vpSubMatrix.cpp b/modules/core/src/math/matrix/vpSubMatrix.cpp
new file mode 100644
index 0000000..19a1468
--- /dev/null
+++ b/modules/core/src/math/matrix/vpSubMatrix.cpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpMatrix .
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpSubMatrix.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpDebug.h>
+#include <stdlib.h>
+
+vpSubMatrix::vpSubMatrix()
+ : pRowNum(0), pColNum(0), parent(NULL)
+{
+}
+
+/*!
+ \brief Constructor
+ \param m : parent matrix
+ \param row_offset : row offset
+ \param col_offset : col offset
+ \param nrows : number of rows of the sub matrix
+ \param ncols : number of columns of the sub matrix
+*/
+vpSubMatrix::vpSubMatrix(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset,
+ const unsigned int & nrows, const unsigned int & ncols)
+ : pRowNum(0), pColNum(0), parent(NULL)
+{
+ init(m,row_offset,col_offset,nrows,ncols);
+}
+
+/*!
+ \brief Initialisation of a sub matrix
+ \param m : parent matrix
+ \param row_offset : row offset
+ \param col_offset : col offset
+ \param nrows : number of rows of the sub matrix
+ \param ncols : number of columns of the sub matrix
+*/
+void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset , const unsigned int & nrows , const unsigned int & ncols){
+
+ if(! m.data){
+ vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ;
+ throw(vpMatrixException(vpMatrixException::subMatrixError,
+ "\n\t\t SubMatrix parent matrix is not allocated")) ;
+ }
+
+ if(row_offset+nrows <= m.getRows() && col_offset+ncols <= m.getCols()){
+ data=m.data;
+ parent =&m;
+ rowNum = nrows;
+ colNum = ncols;
+ pRowNum=m.getRows();
+ pColNum=m.getCols();
+
+ if(rowPtrs)
+ free(rowPtrs);
+
+ rowPtrs=(double**) malloc(nrows * sizeof(double*));
+ for(unsigned int r=0;r<nrows;r++)
+ rowPtrs[r]= m.data+col_offset+(r+row_offset)*pColNum;
+
+ dsize = pRowNum*pColNum ;
+ }else{
+ vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ;
+ throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ;
+ }
+}
+
+/*!
+ \brief This method can be used to detect if the parent matrix
+ always exits or its size have not changed and throw an exception is not
+*/
+void vpSubMatrix::checkParentStatus() const {
+ if(!data){
+ vpERROR_TRACE("\n\t\t vpSubMatrix parent vpMatrix has been destroyed");
+ throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
+ "\n\t\t \n\t\t vpSubMatrix parent vpMatrix has been destroyed")) ;
+ }
+ if(pRowNum!=parent->getRows() || pColNum!=parent->getCols()){
+ vpERROR_TRACE("\n\t\t vpSubMatrix size of parent vpMatrix has been changed");
+ throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
+ "\n\t\t \n\t\t vpSubMatrix size of parent vpMatrix has been changed")) ;
+ }
+}
+
+/*!
+ \brief Operation A = B
+ \param B : a matrix
+*/
+vpSubMatrix & vpSubMatrix::operator=(const vpMatrix &B){
+
+ if ((colNum != B.getCols())||(rowNum != B.getRows()))
+ {
+ vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
+ throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
+ "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] = B[i][j];
+
+ return *this;
+}
+
+/*!
+ \brief Operation A = B
+ \param B : a subMatrix
+*/
+vpSubMatrix & vpSubMatrix::operator=(const vpSubMatrix &B){
+
+ if ((colNum != B.getCols())||(rowNum != B.getRows()))
+ {
+ vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
+ throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
+ "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
+ }
+
+
+ double ** BrowPtrs=B.rowPtrs;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] = BrowPtrs[i][j];
+
+ return *this;
+}
+
+/*!
+ \brief Operation A = x
+ \param x : a scalar
+*/
+vpSubMatrix & vpSubMatrix::operator=(const double &x){
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j] = x;
+
+ return *this;
+}
+
+
+vpSubMatrix::~vpSubMatrix()
+{
+ data=NULL;
+}
diff --git a/modules/core/src/math/matrix/vpSubRowVector.cpp b/modules/core/src/math/matrix/vpSubRowVector.cpp
new file mode 100644
index 0000000..84c2469
--- /dev/null
+++ b/modules/core/src/math/matrix/vpSubRowVector.cpp
@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Mask on a vpRowVector .
+ *
+ * Authors:
+ * Laneurit Jean
+ *
+ *****************************************************************************/
+
+#include <stdlib.h>
+
+#include <visp3/core/vpSubRowVector.h>
+#include <visp3/core/vpException.h>
+
+//! Default constructor that creates an empty vector.
+vpSubRowVector::vpSubRowVector()
+ : vpRowVector(), pColNum(0), parent(NULL)
+{
+}
+
+/*!
+ Construct a sub-row vector from a parent row vector.
+ \param v : parent row vector.
+ \param offset : offset where the sub-row vector starts in the parent row vector.
+ \param ncols : size of the sub-row vector.
+*/
+vpSubRowVector::vpSubRowVector(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols)
+ : vpRowVector(), pColNum(0), parent(NULL)
+{
+ init(v, offset, ncols);
+}
+
+/*!
+ Initialize a sub-row vector from a parent row vector.
+ \param v : parent row vector.
+ \param offset : offset where the sub-row vector starts in the parent row vector.
+ \param ncols : size of the sub-row vector.
+*/
+void vpSubRowVector::init(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols)
+{
+ if (!v.data) {
+ throw(vpException(vpException::fatalError,
+ "Cannot initialize a sub-row vector from an empty parent row vector")) ;
+ }
+
+ if(offset+ncols<=v.getCols()){
+ data=v.data+offset;
+
+ rowNum=1;
+ colNum = ncols;
+
+ pColNum=v.getCols();
+ parent=&v;
+
+ if(rowPtrs)
+ free(rowPtrs);
+
+ rowPtrs=(double**) malloc(1 * sizeof(double*));
+ for(unsigned int i=0;i<1;i++)
+ rowPtrs[i]=v.data+i+offset;
+
+ dsize = colNum ;
+ } else {
+ throw(vpException(vpException::dimensionError,
+ "Cannot create a sub-row vector that is not completely containt in the parrent row vector")) ;
+ }
+}
+
+//! Destructor that set the pointer to the parrent row vector to NULL.
+vpSubRowVector::~vpSubRowVector(){
+ data=NULL ;
+}
+
+/*!
+ This method can be used to detect if the parent row vector
+ always exits or its size have not changed.
+ If this not the case an exception is thrown.
+*/
+void vpSubRowVector::checkParentStatus() const
+{
+ if(!data){
+ throw(vpException(vpException::fatalError,
+ "The parent of the current sub-row vector has been destroyed")) ;
+ }
+ if(pColNum!=parent->getCols()){
+ throw(vpException(vpException::dimensionError,
+ "The size of the parent sub-row vector has changed")) ;
+ }
+}
+
+/*!
+ Allow to initialize a sub-row vector from an other one using operation A = B.
+ Notice that the sub-row vector is not resized to the dimension of \e B.
+
+ \param B : a sub-row vector.
+*/
+vpSubRowVector & vpSubRowVector::operator=(const vpSubRowVector &B)
+{
+ if ( colNum != B.getCols()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (1x%d) sub-row vector from (1x%d) sub-row vector",
+ colNum, B.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i];
+
+ return *this;
+}
+
+/*!
+ Allow to initialize a sub-row vector from a row vector using operation A = B.
+ Notice that the sub-row vector is not resized to the dimension of \e B.
+
+ \param B : a row vector.
+*/
+vpSubRowVector & vpSubRowVector::operator=(const vpRowVector &B)
+{
+ if ( colNum != B.getCols()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (1x%d) sub-row vector from (1x%d) row vector",
+ colNum, B.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i];
+
+ return *this;
+}
+
+/*!
+ Allow to initialize a sub-row vector from a matrix using operation A = B.
+ Notice that the sub-row vector is not resized to the dimension of \e B.
+
+ \param B : a matrix of size 1-by-n.
+*/
+vpSubRowVector & vpSubRowVector::operator=(const vpMatrix &B)
+{
+ if ((B.getRows()!=1)||(colNum != B.getCols())) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize (1x%d) sub-column vector from (%dx%d) matrix",
+ colNum, B.getRows(), B.getCols())) ;
+ }
+
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = B[i][1];
+ return *this;
+}
+/*!
+ Set all the elements of the sub-row vector to \e x.
+ \param x : a scalar value.
+*/
+vpSubRowVector & vpSubRowVector::operator=(const double &x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ data[i] = x;
+ return *this;
+}
diff --git a/modules/core/src/math/misc/vpHinkley.cpp b/modules/core/src/math/misc/vpHinkley.cpp
new file mode 100644
index 0000000..97facf6
--- /dev/null
+++ b/modules/core/src/math/misc/vpHinkley.cpp
@@ -0,0 +1,446 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Hinkley's cumulative sum test implementation.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpHinkley.cpp
+
+ \brief Definition of the vpHinkley class corresponding to the Hinkley's
+ cumulative sum test.
+
+*/
+
+#include <visp3/core/vpHinkley.h>
+#include <visp3/core/vpDebug.h>
+//#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+/* VP_DEBUG_MODE fixed by configure:
+ 1:
+ 2:
+ 3: Print data
+*/
+
+
+/*!
+
+ Constructor.
+
+ Call init() to initialise the Hinkley's test and set \f$\alpha\f$
+ and \f$\delta\f$ to default values.
+
+ By default \f$ \delta = 0.2 \f$ and \f$ \alpha = 0.2\f$. Use
+ setDelta() and setAlpha() to modify these values.
+
+*/
+vpHinkley::vpHinkley()
+ : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
+{
+}
+
+/*!
+
+ Constructor.
+
+ Call init() to initialise the Hinkley's test and set \f$\alpha\f$
+ and \f$\delta\f$ thresholds.
+
+ \param alpha_val : \f$\alpha\f$ is a predefined threshold.
+
+ \param delta_val : \f$\delta\f$ denotes the jump minimal magnitude that
+ we want to detect.
+
+ \sa setAlpha(), setDelta()
+
+*/
+
+vpHinkley::vpHinkley(double alpha_val, double delta_val)
+ : dmin2(delta_val/2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
+{
+}
+
+/*!
+
+ Call init() to initialise the Hinkley's test and set \f$\alpha\f$
+ and \f$\delta\f$ thresholds.
+
+ \param alpha_val : \f$\alpha\f$ is a predefined threshold.
+
+ \param delta_val : \f$\delta\f$ denotes the jump minimal magnitude that
+ we want to detect.
+
+ \sa setAlpha(), setDelta()
+
+*/
+void
+vpHinkley::init(double alpha_val, double delta_val)
+{
+ init();
+
+ setAlpha(alpha_val);
+ setDelta(delta_val);
+}
+
+/*!
+
+ Destructor.
+
+*/
+vpHinkley::~vpHinkley()
+{
+}
+
+/*!
+
+ Initialise the Hinkley's test by setting the mean signal value
+ \f$m_0\f$ to zero as well as \f$S_k, M_k, T_k, N_k\f$.
+
+*/
+void vpHinkley::init()
+{
+ nsignal = 0;
+ mean = 0.0;
+
+ Sk = 0;
+ Mk = 0;
+
+ Tk = 0;
+ Nk = 0;
+}
+
+/*!
+
+ Set the value of \f$\delta\f$, the jump minimal magnetude that we want to
+ detect.
+
+ \sa setAlpha()
+
+*/
+void vpHinkley::setDelta(double delta)
+{
+ dmin2 = delta / 2;
+}
+
+/*!
+
+ Set the value of \f$\alpha\f$, a predefined threshold.
+
+ \sa setDelta()
+
+*/
+void vpHinkley::setAlpha(double alpha_val)
+{
+ this->alpha = alpha_val;
+}
+
+/*!
+
+ Perform the Hinkley test. A downward jump is detected if
+ \f$ M_k - S_k > \alpha \f$.
+
+ \param signal : Observed signal \f$ s(t) \f$.
+
+ \sa setDelta(), setAlpha(), testUpwardJump()
+
+*/
+vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal)
+{
+
+ vpHinkleyJumpType jump = noJump;
+
+ nsignal ++; // Signal length
+
+ if (nsignal == 1) mean = signal;
+
+ // Calcul des variables cumulees
+ computeSk(signal);
+
+ computeMk();
+
+ vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
+ << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;
+
+ // teste si les variables cumulees excedent le seuil
+ if ((Mk - Sk) > alpha)
+ jump = downwardJump;
+
+#ifdef VP_DEBUG
+ if (VP_DEBUG_MODE >=2) {
+ switch(jump) {
+ case noJump:
+ std::cout << "noJump " << std::endl;
+ break;
+ case downwardJump:
+ std::cout << "downWardJump " << std::endl;
+ break;
+ case upwardJump:
+ std::cout << "upwardJump " << std::endl;
+ break;
+ }
+ }
+#endif
+
+ computeMean(signal);
+
+ if (jump == downwardJump) {
+ vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
+
+ Sk = 0; Mk = 0; nsignal = 0;
+ }
+
+ return (jump);
+}
+
+/*!
+
+ Perform the Hinkley test. An upward jump is detected if \f$ T_k - N_k >
+ \alpha \f$.
+
+ \param signal : Observed signal \f$ s(t) \f$.
+
+ \sa setDelta(), setAlpha(), testDownwardJump()
+
+*/
+vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
+{
+
+ vpHinkleyJumpType jump = noJump;
+
+ nsignal ++; // Signal length
+
+ if (nsignal == 1) mean = signal;
+
+ // Calcul des variables cumulees
+ computeTk(signal);
+
+ computeNk();
+
+ vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
+ << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;
+
+ // teste si les variables cumulees excedent le seuil
+ if ((Tk - Nk) > alpha)
+ jump = upwardJump;
+
+#ifdef VP_DEBUG
+ if (VP_DEBUG_MODE >= 2) {
+ switch(jump) {
+ case noJump:
+ std::cout << "noJump " << std::endl;
+ break;
+ case downwardJump:
+ std::cout << "downWardJump " << std::endl;
+ break;
+ case upwardJump:
+ std::cout << "upWardJump " << std::endl;
+ break;
+ }
+ }
+#endif
+ computeMean(signal);
+
+ if (jump == upwardJump) {
+ vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
+
+ Tk = 0; Nk = 0; nsignal = 0;
+ }
+
+ return (jump);
+}
+
+/*!
+
+ Perform the Hinkley test. A downward jump is detected if \f$ M_k - S_k >
+ \alpha \f$. An upward jump is detected if \f$ T_k - N_k > \alpha \f$.
+
+ \param signal : Observed signal \f$ s(t) \f$.
+
+ \sa setDelta(), setAlpha(), testDownwardJump(), testUpwardJump()
+
+*/
+vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
+{
+
+ vpHinkleyJumpType jump = noJump;
+
+ nsignal ++; // Signal length
+
+ if (nsignal == 1) mean = signal;
+
+ // Calcul des variables cumulees
+ computeSk(signal);
+ computeTk(signal);
+
+ computeMk();
+ computeNk();
+
+ vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
+ << " signal: " << signal
+ << " Sk: " << Sk << " Mk: " << Mk
+ << " Tk: " << Tk << " Nk: " << Nk << std::endl;
+
+ // teste si les variables cumulees excedent le seuil
+ if ((Mk - Sk) > alpha)
+ jump = downwardJump;
+ else if ((Tk - Nk) > alpha)
+ jump = upwardJump;
+
+#ifdef VP_DEBUG
+ if (VP_DEBUG_MODE >= 2) {
+ switch(jump) {
+ case noJump:
+ std::cout << "noJump " << std::endl;
+ break;
+ case downwardJump:
+ std::cout << "downWardJump " << std::endl;
+ break;
+ case upwardJump:
+ std::cout << "upwardJump " << std::endl;
+ break;
+ }
+ }
+#endif
+
+ computeMean(signal);
+
+ if ((jump == upwardJump) || (jump == downwardJump)) {
+ vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
+
+ Sk = 0; Mk = 0; Tk = 0; Nk = 0; nsignal = 0;
+ // Debut modif FS le 03/09/2003
+ mean = signal;
+ // Fin modif FS le 03/09/2003
+ }
+
+ return (jump);
+}
+
+/*!
+
+ Compute the mean value \f$m_0\f$ of the signal. The mean value must be
+ computed before the jump is estimated on-line.
+
+ \param signal : Observed signal \f$ s(t) \f$.
+
+*/
+void vpHinkley::computeMean(double signal)
+{
+ // Debut modif FS le 03/09/2003
+ // Lors d'une chute ou d'une remontee lente du signal, pariculierement
+ // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
+ // derives de la moyenne, elle n'est remise a jour avec la valeur
+ // courante du signal que si un debut de saut potentiel n'est pas detecte.
+ //if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
+ if ( ( std::fabs(Mk-Sk) <= std::fabs(vpMath::maximum(Mk,Sk))*std::numeric_limits<double>::epsilon() )
+ &&
+ ( std::fabs(Tk-Nk) <= std::fabs(vpMath::maximum(Tk,Nk))*std::numeric_limits<double>::epsilon() ) )
+ // Fin modif FS le 03/09/2003
+
+ // Mise a jour de la moyenne.
+ mean = (mean * (nsignal - 1) + signal) / (nsignal);
+
+}
+/*!
+
+ Compute \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2})\f$
+
+ \param signal : Observed signal \f$ s(t) \f$.
+
+*/
+void vpHinkley::computeSk(double signal)
+{
+
+ // Calcul des variables cumulees
+ Sk += signal - mean + dmin2;
+}
+/*!
+
+ Compute \f$M_k\f$, the maximum value of \f$S_k\f$.
+
+*/
+void vpHinkley::computeMk()
+{
+ if (Sk > Mk) Mk = Sk;
+}
+/*!
+
+ Compute \f$T_k = \sum_{t=0}^{k} (s(t) - m_0 - \frac{\delta}{2})\f$
+
+ \param signal : Observed signal \f$ s(t) \f$.
+*/
+void vpHinkley::computeTk(double signal)
+{
+
+ // Calcul des variables cumulees
+ Tk += signal - mean - dmin2;
+}
+/*!
+
+ Compute \f$N_k\f$, the minimum value of \f$T_k\f$.
+
+*/
+void vpHinkley::computeNk()
+{
+ if (Tk < Nk) Nk = Tk;
+}
+
+void vpHinkley::print(vpHinkley::vpHinkleyJumpType jump)
+{
+ switch(jump)
+ {
+ case noJump :
+ std::cout << " No jump detected " << std::endl ;
+ break ;
+ case downwardJump :
+ std::cout << " Jump downward detected " << std::endl ;
+ break ;
+ case upwardJump :
+ std::cout << " Jump upward detected " << std::endl ;
+ break ;
+ default:
+ std::cout << " Jump detected " << std::endl ;
+ break ;
+
+ }
+}
+
diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp
new file mode 100644
index 0000000..d8a6ff5
--- /dev/null
+++ b/modules/core/src/math/misc/vpMath.cpp
@@ -0,0 +1,262 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Simple mathematical function not available in the C math library (math.h).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMath.cpp
+ \brief Provides simple Math computation that are not available in
+ the C mathematics library (math.h)
+*/
+
+#include <stdint.h>
+#include <numeric>
+#include <functional>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpException.h>
+
+
+#if defined(VISP_HAVE_FUNC__ISNAN)
+# include <float.h>
+#endif
+
+#if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
+# if defined _MSC_VER || defined __BORLANDC__
+typedef __int64 int64;
+typedef unsigned __int64 uint64;
+# else
+typedef int64_t int64;
+typedef uint64_t uint64;
+# endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+typedef union Cv64suf
+{
+ int64 i;
+ uint64 u;
+ double f;
+}
+Cv64suf;
+#endif
+#endif
+
+const double vpMath::ang_min_sinc = 1.0e-8;
+const double vpMath::ang_min_mc = 2.5e-4;
+
+
+/*!
+ Check whether a double number is not a number (NaN) or not.
+ \param value : Double number to check.
+ \return Return true if value is not a number.
+ */
+bool vpMath::isNaN(const double value)
+{
+#if defined(VISP_HAVE_FUNC_ISNAN)
+ return isnan(value);
+#elif defined(VISP_HAVE_FUNC_STD_ISNAN)
+ return std::isnan(value);
+#elif defined(VISP_HAVE_FUNC__ISNAN)
+ return (_isnan(value) != 0);
+#else
+ #if 0
+ //This trick should work for any compiler which claims to use IEEE floating point.
+ //Do not work with g++ and -ffast-math option.
+ return (value != value);
+ #else
+ //Taken from OpenCV source code CvIsNan()
+ Cv64suf ieee754;
+ ieee754.f = value;
+ return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) +
+ ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
+ #endif
+#endif
+}
+/*!
+ Returns whether a double is an infinity value (either positive infinity or negative infinity).
+ \param value : Double number to check.
+ \return Return true if value is infinity.
+ */
+bool vpMath::isInf(const double value)
+{
+#if defined(VISP_HAVE_FUNC_ISINF)
+ return isinf(value);
+#elif defined(VISP_HAVE_FUNC_STD_ISINF)
+ return std::isinf(value);
+#elif defined(VISP_HAVE_FUNC__FINITE)
+ return !_finite(value);
+#else
+ //Taken from OpenCV source code CvIsInf()
+ Cv64suf ieee754;
+ ieee754.f = value;
+ return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 &&
+ (unsigned)ieee754.u == 0;
+#endif
+}
+
+/*!
+ Compute \f$ (1-cos(x))/x^2 \f$
+
+ \param cosx : Value of cos(x).
+ \param x : Value of x.
+
+ \return \f$ (1-cosx)/x^2 \f$
+
+*/
+double vpMath::mcosc(double cosx, double x)
+{
+ if (fabs(x) < ang_min_mc) return 0.5 ;
+ else return ((1.0-cosx)/x/x) ;
+}
+
+/*!
+
+ Compute \f$ (1-sinc(x))/x^2 \f$ with \f$ sinc(x) = sinx / x \f$.
+
+ \param sinx : value of sin(x).
+ \param x : Value of x.
+
+ \return \f$ (1-sinc(x))/x^2 \f$
+
+*/
+double vpMath::msinc(double sinx, double x)
+{
+ if (fabs(x) < ang_min_mc) return (1./6.0) ;
+ else return ((1.0-sinx/x)/x/x) ;
+}
+
+/*!
+
+ Compute sinus cardinal \f$ \frac{sin(x)}{x} \f$.
+
+ \param x : Value of x.
+
+ \return Sinus cardinal.
+
+*/
+double vpMath::sinc(double x)
+{
+ if (fabs(x) < ang_min_sinc) return 1.0 ;
+ else return sin(x)/x ;
+}
+/*!
+
+ Compute sinus cardinal \f$ \frac{sin(x)}{x}\f$.
+
+ \param sinx : Value of sin(x).
+ \param x : Value of x.
+
+ \return Sinus cardinal.
+
+*/
+double vpMath::sinc(double sinx, double x)
+{
+ if (fabs(x) < ang_min_sinc) return 1.0 ;
+ else return (sinx/x) ;
+}
+
+/*!
+ Compute the mean value for a vector of double.
+
+ \param v : Vector of double values.
+
+ \return The mean value.
+*/
+double vpMath::getMean(const std::vector<double> &v)
+{
+ if(v.empty()) {
+ throw vpException(vpException::notInitialized, "Empty vector !");
+ }
+
+ double sum = 0.0;
+ size_t size = v.size();
+
+ sum = std::accumulate(v.begin(), v.end(), 0.0);
+
+ return sum / (double) size;
+}
+
+/*!
+ Compute the median value for a vector of double.
+
+ \param v : Vector of double values.
+
+ \return The median value.
+*/
+double vpMath::getMedian(const std::vector<double> &v) {
+ if(v.empty()) {
+ throw vpException(vpException::notInitialized, "Empty vector !");
+ }
+
+ double median = 0.0;
+ std::vector<double> v_copy = v;
+ size_t size = v_copy.size();
+
+ if(size % 2 == 0) {
+ sort(v_copy.begin(), v_copy.end());
+ median = (v_copy[size / 2 - 1] + v_copy[size / 2]) / 2.0;
+ } else {
+ std::nth_element(v_copy.begin(), v_copy.begin() + (int) (size/2), v_copy.end());
+ median = v_copy[size/2];
+ }
+
+ return median;
+}
+
+/*!
+ Compute the standard deviation value for a vector of double.
+
+ \param v : Vector of double values.
+ \param useBesselCorrection : If true, the Bessel correction is used (normalize by N-1).
+
+ \return The standard deviation value.
+*/
+double vpMath::getStdev(const std::vector<double> &v, const bool useBesselCorrection) {
+ if(v.empty()) {
+ throw vpException(vpException::notInitialized, "Empty vector !");
+ }
+
+ double mean = getMean(v);
+
+ std::vector<double> diff(v.size());
+ std::transform(v.begin(), v.end(), diff.begin(),
+ std::bind2nd(std::minus<double>(), mean));
+ double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
+ double divisor = (double) v.size();
+ if(useBesselCorrection && v.size() > 1) {
+ divisor = divisor-1;
+ }
+
+ return std::sqrt(sq_sum / divisor);
+}
diff --git a/modules/core/src/math/random-generator/vpGaussRand.cpp b/modules/core/src/math/random-generator/vpGaussRand.cpp
new file mode 100644
index 0000000..3712e71
--- /dev/null
+++ b/modules/core/src/math/random-generator/vpGaussRand.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generation of random number with uniform and normal probability density.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <math.h>
+#include <visp3/core/vpGaussRand.h>
+
+/*!
+ Generate a normal random variable using the Box-Muller generator.
+
+ Generate a normal random variable with mean 0 and standard deviation of 1.
+ To adjust to some other distribution, multiply by the standard deviation and
+ add the mean. Box-Muller method
+*/
+double
+vpGaussRand::gaussianDraw()
+{
+ double v1, v2, rsq;
+ static bool AlreadyDone = false;
+ static double x2;
+
+ if (AlreadyDone) {
+ AlreadyDone=false;
+ return x2;
+ }
+
+ else {
+
+ do {
+ v1=2*draw1()-1;
+ v2=2*draw1()-1;
+ rsq=v1*v1+v2*v2;
+ } while (rsq >= 1);
+
+ double fac=sqrt(-2*log(rsq)/rsq);
+ x2=v2*fac;
+ AlreadyDone=true;
+ return v1*fac;
+ }
+}
diff --git a/modules/core/src/math/random-generator/vpUniRand.cpp b/modules/core/src/math/random-generator/vpUniRand.cpp
new file mode 100644
index 0000000..6e4a401
--- /dev/null
+++ b/modules/core/src/math/random-generator/vpUniRand.cpp
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generation of random number with uniform and normal probability density.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <math.h>
+#include <visp3/core/vpUniRand.h>
+
+/*!
+ Minimal random number generator of Park and Miller \cite Park:1988. Returns a
+ uniform random deviate between 0.0 and 1.0.
+
+*/
+inline void
+vpUniRand::draw0()
+{
+ long k= x/q;//temp value for computing without overflow
+ x = a*(x-k*q)-k*r;
+ if (x < 0) x += m; //compute x without overflow
+}
+
+/*!
+ Bays-Durham Shuffling of Park-Miller generator.
+
+ Minimal random number generator of Park and Miller with Bays-Durham
+ shuffle. Returns a uniform random deviate between 0.0 and 1.0 (exclusive of
+ the endpoint values).
+*/
+double
+vpUniRand::draw1()
+{
+ const long ntab = 33; //we work on a 32 elements array.
+ //the 33rd one is actually the first value of y.
+ const long modulo = ntab-2;
+
+ static long y = 0;
+ static long T[ntab];
+
+ long j; //index of T
+
+ //step 0
+ if (!y) { //first time
+ for(j = 0; j < ntab; j++) {
+ draw0();
+ T[j]=x;
+ } //compute table T
+ y=T[ntab-1];
+ }
+
+ //step 1
+ j = y & modulo; //compute modulo ntab+1 (the first element is the 0th)
+
+ //step 3
+ y=T[j];
+ double ans = (double)y/normalizer;
+
+ //step 4
+ //generate x(k+i) and set y=x(k+i)
+ draw0();
+
+ //refresh T[j];
+ T[j]=x;
+
+ return ans;
+}
diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp
new file mode 100644
index 0000000..d0b3e0f
--- /dev/null
+++ b/modules/core/src/math/robust/vpRobust.cpp
@@ -0,0 +1,1011 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * M-Estimator and various influence function.
+ *
+ * Authors:
+ * Andrew Comport
+ * Jean Laneurit
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpRobust.cpp
+*/
+
+
+
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpRobust.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#define vpITMAX 100
+#define vpEPS 3.0e-7
+#define vpCST 1
+
+
+// ===================================================================
+/*!
+ \brief Constructor.
+ \param n_data : Size of the data vector.
+
+*/
+vpRobust::vpRobust(unsigned int n_data)
+ : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(n_data)
+{
+ vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
+
+ normres.resize(n_data);
+ sorted_normres.resize(n_data);
+ sorted_residues.resize(n_data);
+ // NoiseThreshold=0.0017; //Can not be more accurate than 1 pixel
+}
+
+/*!
+ \brief Resize containers.
+ \param n_data : size of input data vector.
+
+*/
+void vpRobust::resize(unsigned int n_data){
+
+ if(n_data!=size){
+ normres.resize(n_data);
+ sorted_normres.resize(n_data);
+ sorted_residues.resize(n_data);
+ size=n_data;
+ }
+
+}
+
+// ===================================================================
+/*!
+
+ \brief Calculate an Mestimate given a particular loss function using MAD
+ (Median Absolute Deviation) as a scale estimate at each iteration.
+
+ \pre Requires a column vector of residues.
+
+ \post Keeps a copy of the weights so that rejected points are kept at zero
+ weight.
+
+ \param method : Type of M-Estimator \f$\rho(r_i)\f$:
+
+ - TUKEY : \f$ \rho(r_i, C) = \left\{
+ \begin{array}{ll}
+ \frac{r_i^6}{6} - \frac{C^2r_i^4}{2} +\frac{C^4r_i^2}{2} & \mbox{if} |r_i| < C \\
+ \frac{1}{6} C^6 & \mbox{else} \end{array}
+ \right.
+ \f$
+ with influence function \f$ \psi(r_i, C) = \left\{
+ \begin{array}{ll} r_i(r_i^2-C^2)^2 & \mbox{if} |r_i| < C \\ 0 & \mbox{else} \end{array} \right. \f$ where \f$C=4.7 \hat{\sigma} \f$ and with \f$ \hat{\sigma} = 1.48{Med}_i(|r_i - {Med}_j(r_j)|) \f$
+
+ - CAUCHY :
+
+ - HUBER :
+
+ \param residues : Residues \f$ r_i \f$ used in the previous formula.
+
+ \param weights : Vector of weights \f$w_i =
+ \frac{\psi(r_i)}{r_i}\f$. Values are in [0, 1]. A value near zero
+ means that the data is an outlier. This vector must have the same size
+ residue vector.
+
+ \return Returns a Column Vector of weights associated to each residue.
+ */
+
+// ===================================================================
+void vpRobust::MEstimator(const vpRobustEstimatorType method,
+ const vpColVector &residues,
+ vpColVector &weights)
+{
+
+ double med=0; // median
+ double normmedian=0; // Normalized median
+ double sigma=0;// Standard Deviation
+
+ // resize vector only if the size of residue vector has changed
+ unsigned int n_data = residues.getRows();
+ resize(n_data);
+
+ sorted_residues = residues;
+
+ unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
+
+ // Calculate median
+ med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+ //residualMedian = med ;
+
+ // Normalize residues
+ for(unsigned int i=0; i<n_data; i++)
+ {
+ normres[i] = (fabs(residues[i]- med));
+ sorted_normres[i] = (fabs(sorted_residues[i]- med));
+
+ }
+
+ // Calculate MAD
+ normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+ //normalizedResidualMedian = normmedian ;
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ sigma = 1.4826*normmedian; // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if(sigma < NoiseThreshold)
+ {
+ sigma= NoiseThreshold;
+ }
+
+ switch (method)
+ {
+ case TUKEY :
+ {
+ psiTukey(sigma, normres,weights);
+
+ vpCDEBUG(2) << "Tukey's function computed" << std::endl;
+ break ;
+
+ }
+ case CAUCHY :
+ {
+ psiCauchy(sigma, normres,weights);
+ break ;
+ }
+ /* case MCLURE :
+ {
+ psiMcLure(sigma, normres);
+ break ;
+ }*/
+ case HUBER :
+ {
+ psiHuber(sigma, normres,weights);
+ break ;
+ }
+ }
+}
+
+
+
+void vpRobust::MEstimator(const vpRobustEstimatorType method,
+ const vpColVector &residues,
+ const vpColVector& all_residues,
+ vpColVector &weights)
+{
+
+
+ double normmedian=0; // Normalized median
+ double sigma=0;// Standard Deviation
+
+ unsigned int n_all_data = all_residues.getRows();
+ vpColVector all_normres(n_all_data);
+
+ // compute median with the residues vector, return all_normres which are the normalized all_residues vector.
+ normmedian = computeNormalizedMedian(all_normres,residues,all_residues,weights);
+
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ sigma = 1.4826*normmedian; // Median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if(sigma < NoiseThreshold)
+ {
+ sigma= NoiseThreshold;
+ }
+
+
+ switch (method)
+ {
+ case TUKEY :
+ {
+ psiTukey(sigma, all_normres,weights);
+
+ vpCDEBUG(2) << "Tukey's function computed" << std::endl;
+ break ;
+
+ }
+ case CAUCHY :
+ {
+ psiCauchy(sigma, all_normres,weights);
+ break ;
+ }
+ /* case MCLURE :
+ {
+ psiMcLure(sigma, all_normres);
+ break ;
+ }*/
+ case HUBER :
+ {
+ psiHuber(sigma, all_normres,weights);
+ break ;
+ }
+
+
+ };
+}
+
+
+
+double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
+ const vpColVector &residues,
+ const vpColVector &all_residues,
+ const vpColVector & weights
+ )
+{
+ double med=0;
+ double normmedian=0;
+
+ unsigned int n_all_data = all_residues.getRows();
+ unsigned int n_data = residues.getRows();
+
+ // resize vector only if the size of residue vector has changed
+ resize(n_data);
+
+ sorted_residues = residues;
+ vpColVector no_null_weight_residues;
+ no_null_weight_residues.resize(n_data);
+
+ //all_normres.resize(n_all_data); // Normalized Residue
+ //vpColVector sorted_normres(n_data); // Normalized Residue
+ //vpColVector sorted_residues = residues;
+ //vpColVector sorted_residues;
+
+
+ unsigned int index =0;
+ for(unsigned int j=0;j<n_data;j++)
+ {
+ //if(weights[j]!=0)
+ if(std::fabs(weights[j]) > std::numeric_limits<double>::epsilon())
+ {
+ no_null_weight_residues[index]=residues[j];
+ index++;
+ }
+ }
+ sorted_residues.resize(index);
+ memcpy(sorted_residues.data,no_null_weight_residues.data,index*sizeof(double));
+ n_data=index;
+
+ vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
+ << std::endl;
+
+ // Calculate Median
+ // Be careful to not use the rejected residues for the
+ // calculation.
+
+ unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
+ med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+
+ unsigned int i;
+ // Normalize residues
+ for(i=0; i<n_all_data; i++)
+ {
+ all_normres[i] = (fabs(all_residues[i]- med));
+ }
+
+ for(i=0; i<n_data; i++)
+ {
+ sorted_normres[i] = (fabs(sorted_residues[i]- med));
+ }
+ // MAD calculated only on first iteration
+
+ //normmedian = Median(normres, weights);
+ //normmedian = Median(normres);
+ normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+
+ return normmedian;
+}
+
+// ===================================================================
+/*!
+ * \brief Calculate an Mestimate with a simultaneous scale estimate
+ * using HUBER's influence function
+ * \pre Requires a column vector of residues
+ * \post None
+ * \return Returns a Column Vector of weights associated to each residue
+ */
+// ===================================================================
+vpColVector
+vpRobust::simultMEstimator(vpColVector &residues)
+{
+
+ double med=0; // Median
+ double normmedian=0; // Normalized Median
+ double sigma=0; // Standard Deviation
+
+ unsigned int n_data = residues.getRows();
+ vpColVector norm_res(n_data); // Normalized Residue
+ vpColVector w(n_data);
+
+ vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
+ << std::endl;
+
+ // Calculate Median
+ unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
+ med = select(residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+
+ // Normalize residues
+ for(unsigned int i=0; i<n_data; i++)
+ norm_res[i] = (fabs(residues[i]- med));
+
+ // Check for various methods.
+ // For Huber compute Simultaneous scale estimate
+ // For Others use MAD calculated on first iteration
+ if(it==0)
+ {
+ normmedian = select(norm_res, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ sigma = 1.4826*normmedian; // Median Absolute Deviation
+ }
+ else
+ {
+ // compute simultaneous scale estimate
+ sigma = simultscale(residues);
+ }
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if(sigma < NoiseThreshold)
+ {
+ sigma= NoiseThreshold;
+ }
+
+ vpCDEBUG(2) << "MAD and C computed" << std::endl;
+
+ psiHuber(sigma, norm_res,w);
+
+ sig_prev = sigma;
+
+ return w;
+}
+
+double
+vpRobust::scale(vpRobustEstimatorType method, vpColVector &x)
+{
+ unsigned int p = 6; //Number of parameters to be estimated.
+ unsigned int n = x.getRows();
+ double sigma2=0;
+ /* long */ double Expectation=0;
+ /* long */ double Sum_chi=0;
+ /* long*/ double chiTmp =0;
+
+ for(unsigned int i=0; i<n; i++)
+ {
+
+ chiTmp = constrainedChi(method, x[i]);
+ Expectation += chiTmp*(1-erf(chiTmp));
+ Sum_chi += chiTmp;
+
+#ifdef VP_DEBUG
+#if VP_DEBUG_MODE == 3
+ {
+ std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
+ std::cout << "x[i] = " << x[i] <<std::endl;
+ std::cout << "chi = " << chiTmp << std::endl;
+ std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
+ std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
+ //getchar();
+ }
+#endif
+#endif
+ }
+
+
+ sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
+
+#ifdef VP_DEBUG
+#if VP_DEBUG_MODE == 3
+ {
+ std::cout << "Expectation = " << Expectation << std::endl;
+ std::cout << "Sum chi = " << Sum_chi << std::endl;
+ std::cout << "sig_prev" << sig_prev << std::endl;
+ std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
+ }
+#endif
+#endif
+
+ return sqrt(fabs(sigma2));
+
+}
+
+
+double
+vpRobust::simultscale(vpColVector &x)
+{
+ unsigned int p = 6; //Number of parameters to be estimated.
+ unsigned int n = x.getRows();
+ double sigma2=0;
+ /* long */ double Expectation=0;
+ /* long */ double Sum_chi=0;
+ /* long */ double chiTmp =0;
+
+ for(unsigned int i=0; i<n; i++)
+ {
+
+ chiTmp = simult_chi_huber(x[i]);
+ Expectation += chiTmp*(1-erf(chiTmp));
+ Sum_chi += chiTmp;
+
+#ifdef VP_DEBUG
+#if VP_DEBUG_MODE == 3
+ {
+ std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
+ std::cout << "x[i] = " << x[i] <<std::endl;
+ std::cout << "chi = " << chiTmp << std::endl;
+ std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
+ std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
+ //getchar();
+ }
+#endif
+#endif
+ }
+
+
+ sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
+
+#ifdef VP_DEBUG
+#if VP_DEBUG_MODE == 3
+ {
+ std::cout << "Expectation = " << Expectation << std::endl;
+ std::cout << "Sum chi = " << Sum_chi << std::endl;
+ std::cout << "sig_prev" << sig_prev << std::endl;
+ std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
+ }
+#endif
+#endif
+
+ return sqrt(fabs(sigma2));
+
+}
+
+double
+vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
+{
+ switch (method)
+ {
+ case TUKEY :
+ return constrainedChiTukey(x);
+ case CAUCHY :
+ return constrainedChiCauchy(x);
+ case HUBER :
+ return constrainedChiHuber(x);
+ };
+
+ return -1;
+
+}
+
+
+double
+vpRobust::constrainedChiTukey(double x)
+{
+ double sct=0;
+ double a=4.7;
+ double s=sig_prev;
+ //double epsillon=0.5;
+
+ if(fabs(x) <= 4.7*sig_prev)
+ {
+ //sct = (vpMath::sqr(s*a-x)*vpMath::sqr(s*a+x)*vpMath::sqr(x))/(s*vpMath::sqr(vpMath::sqr(a*vpMath::sqr(s))));
+ sct = (vpMath::sqr(s*a)*x-s*vpMath::sqr(s*a)-x*vpMath::sqr(x))*(vpMath::sqr(s*a)*x+s*vpMath::sqr(s*a)-x*vpMath::sqr(x))/s*vpMath::sqr(vpMath::sqr(vpMath::sqr(s)))/vpMath::sqr(vpMath::sqr(a));
+ }
+ else
+ sct = -1/s;
+
+ return sct;
+}
+
+
+double
+vpRobust::constrainedChiCauchy(double x)
+{
+ double sct = 0;
+ //double u = x/sig_prev;
+ double s = sig_prev;
+ double b = 2.3849;
+
+ sct = -1*(vpMath::sqr(x)*b)/(s*(vpMath::sqr(s*b)+vpMath::sqr(x)));
+
+ return sct;
+}
+
+double
+vpRobust::constrainedChiHuber(double x)
+{
+ double sct=0;
+ double u = x/sig_prev;
+ double c = 1.2107; //1.345;
+
+ if(fabs(u) <= c)
+ sct = vpMath::sqr(u);
+ else
+ sct = vpMath::sqr(c);
+
+ return sct;
+}
+
+double
+vpRobust::simult_chi_huber(double x)
+{
+ double sct=0;
+ double u = x/sig_prev;
+ double c = 1.2107; //1.345;
+
+ if(fabs(u) <= c)
+ {
+ //sct = 0.5*vpMath::sqr(u);
+ sct = vpMath::sqr(u);
+ }
+ else
+ {
+ //sct = 0.5*vpMath::sqr(c);
+ sct = vpMath::sqr(c);
+ }
+
+ return sct;
+}
+
+/*!
+ \brief calculation of Tukey's influence function
+
+ \param sigma : sigma parameters
+ \param x : normalized residue vector
+ \param weights : weight vector
+*/
+
+void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
+{
+
+ unsigned int n_data = x.getRows();
+ double cst_const = vpCST*4.6851;
+
+ for(unsigned int i=0; i<n_data; i++)
+ {
+ //if(sig==0 && weights[i]!=0)
+ if(std::fabs(sig) <= std::numeric_limits<double>::epsilon() && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
+ {
+ weights[i]=1;
+ continue;
+ }
+
+ double xi_sig = x[i]/sig;
+
+ //if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
+ if((std::fabs(xi_sig)<=(cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
+ {
+ weights[i] = vpMath::sqr(1-vpMath::sqr(xi_sig/cst_const));
+ //w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
+ }
+ else
+ {
+ //Outlier - could resize list of points tracked here?
+ weights[i] = 0;
+ }
+ }
+}
+
+/*!
+ \brief calculation of Tukey's influence function
+
+ \param sigma : sigma parameters
+ \param x : normalized residue vector
+ \param weights : weight vector
+*/
+void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
+{
+ double c = 1.2107; //1.345;
+ unsigned int n_data = x.getRows();
+
+ for(unsigned int i=0; i<n_data; i++)
+ {
+ //if(weights[i]!=0)
+ if(std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
+ {
+ double xi_sig = x[i]/sig;
+ if(fabs(xi_sig)<=c)
+ weights[i] = 1;
+ else
+ weights[i] = c/fabs(xi_sig);
+ }
+ }
+}
+
+/*!
+ \brief calculation of Cauchy's influence function
+
+ \param sigma : sigma parameters
+ \param x : normalized residue vector
+ \param weights : weight vector
+*/
+
+void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
+{
+ unsigned int n_data = x.getRows();
+ double const_sig = 2.3849*sig;
+
+ //Calculate Cauchy's equation
+ for(unsigned int i=0; i<n_data; i++)
+ {
+ weights[i] = 1/(1+vpMath::sqr(x[i]/(const_sig)));
+
+ // If one coordinate is an outlier the other is too!
+ // w[i] < 0.01 is a threshold to be set
+ /*if(w[i] < 0.01)
+ {
+ if(i%2 == 0)
+ {
+ w[i+1] = w[i];
+ i++;
+ }
+ else
+ w[i-1] = w[i];
+ }*/
+ }
+}
+
+
+/*!
+ \brief calculation of McLure's influence function
+
+ \param sigma : sigma parameters
+ \param x : normalized residue vector
+ \param weights : weight vector
+*/
+void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
+{
+ unsigned int n_data = r.getRows();
+
+ //McLure's function
+ for(unsigned int i=0; i<n_data; i++)
+ {
+ weights[i] = 1/(vpMath::sqr(1+vpMath::sqr(r[i]/sig)));
+ //w[i] = 2*mad/vpMath::sqr((mad+r[i]*r[i]));//odobez
+
+ // If one coordinate is an outlier the other is too!
+ // w[i] < 0.01 is a threshold to be set
+ /*if(w[i] < 0.01)
+ {
+ if(i%2 == 0)
+ {
+ w[i+1] = w[i];
+ i++;
+ }
+ else
+ w[i-1] = w[i];
+ }*/
+ }
+}
+
+
+/*!
+ \brief partition function
+ \param a : vector to be sorted
+ \param l : first value to be considered
+ \param r : last value to be considered
+*/
+int
+vpRobust::partition(vpColVector &a, int l, int r)
+{
+ int i = l-1;
+ int j = r;
+ double v = a[(unsigned int)r];
+
+ for (;;)
+ {
+ while (a[(unsigned int)++i] < v) ;
+ while (v < a[(unsigned int)--j]) if (j == l) break;
+ if (i >= j) break;
+ exch(a[(unsigned int)i], a[(unsigned int)j]);
+ }
+ exch(a[(unsigned int)i], a[(unsigned int)r]);
+ return i;
+}
+
+/*!
+ \brief sort a part of a vector and select a value of this new vector
+ \param a : vector to be sorted
+ \param l : first value to be considered
+ \param r : last value to be considered
+ \param k : value to be selected
+*/
+double
+vpRobust::select(vpColVector &a, int l, int r, int k)
+{
+ while (r > l)
+ {
+ int i = partition(a, l, r);
+ if (i >= k) r = i-1;
+ if (i <= k) l = i+1;
+ }
+ return a[(unsigned int)k];
+}
+
+
+double
+vpRobust::erf(double x)
+{
+ return x < 0.0 ? -gammp(0.5,x*x) : gammp(0.5,x*x);
+}
+
+double
+vpRobust::gammp(double a, double x)
+{
+ double gamser=0.,gammcf=0.,gln;
+
+ if (x < 0.0 || a <= 0.0)
+ std::cout << "Invalid arguments in routine GAMMP";
+ if (x < (a+1.0))
+ {
+ gser(&gamser,a,x,&gln);
+ return gamser;
+ }
+ else
+ {
+ gcf(&gammcf,a,x,&gln);
+ return 1.0-gammcf;
+ }
+}
+
+void
+vpRobust::gser(double *gamser, double a, double x, double *gln)
+{
+ double sum,del,ap;
+
+ *gln=gammln(a);
+ if (x <= 0.0)
+ {
+ if (x < 0.0)
+ std::cout << "x less than 0 in routine GSER";
+ *gamser=0.0;
+ return;
+ }
+ else
+ {
+ ap=a;
+ del=sum=1.0/a;
+ for (int n=1; n<=vpITMAX; n++)
+ {
+ ap += 1.0;
+ del *= x/ap;
+ sum += del;
+ if (fabs(del) < fabs(sum)*vpEPS)
+ {
+ *gamser=sum*exp(-x+a*log(x)-(*gln));
+ return;
+ }
+ }
+ std::cout << "a too large, vpITMAX too small in routine GSER";
+ return;
+ }
+}
+
+void
+vpRobust::gcf(double *gammcf, double a, double x, double *gln)
+{
+ double gold=0.0,g,fac=1.0,b1=1.0;
+ double b0=0.0,anf,ana,an,a1,a0=1.0;
+
+ *gln=gammln(a);
+ a1=x;
+ for (int n=1; n<=vpITMAX; n++)
+ {
+ an=(double) n;
+ ana=an-a;
+ a0=(a1+a0*ana)*fac;
+ b0=(b1+b0*ana)*fac;
+ anf=an*fac;
+ a1=x*a0+anf*a1;
+ b1=x*b0+anf*b1;
+ //if (a1)
+ if (std::fabs(a1) > std::numeric_limits<double>::epsilon())
+ {
+ fac=1.0/a1;
+ g=b1*fac;
+ if (fabs((g-gold)/g) < vpEPS)
+ {
+ *gammcf=exp(-x+a*log(x)-(*gln))*g;
+ return;
+ }
+ gold=g;
+ }
+ }
+ std::cout << "a too large, vpITMAX too small in routine GCF";
+}
+
+double
+vpRobust::gammln(double xx)
+{
+ double x,tmp,ser;
+ static double cof[6]={76.18009173,-86.50532033,24.01409822,
+ -1.231739516,0.120858003e-2,-0.536382e-5};
+
+ x=xx-1.0;
+ tmp=x+5.5;
+ tmp -= (x+0.5)*log(tmp);
+ ser=1.0;
+ for (int j=0; j<=5; j++)
+ {
+ x += 1.0;
+ ser += cof[j]/x;
+ }
+ return -tmp+log(2.50662827465*ser);
+}
+
+
+
+// double
+// vpRobust::median(const vpColVector &v)
+// {
+// int i,j;
+// int inf, sup;
+// int n = v.getRows() ;
+// vpColVector infsup(n) ;
+// vpColVector eq(n) ;
+//
+// for (i=0;i<n;i++)
+// {
+// // We compute the number of elements superior to the current value (sup)
+// // the number of elements inferior (inf) to the current value and
+// // the number of elements equal to the current value (eq)
+// inf = sup = 0;
+// for (j=0;j<n;j++)
+// {
+// if (i != j)
+// {
+// if (v[i] <= v[j]) inf++;
+// if (v[i] >= v[j]) sup++;
+// if (v[i] == v[j]) eq[i]++;
+// }
+// }
+// // We compute then difference between inf and sup
+// // the median should be for |inf-sup| = 0 (1 if an even number of element)
+// // which means that there are the same number of element in the array
+// // that are greater and smaller that this value.
+// infsup[i] = abs(inf-sup);
+// }
+//
+// // seek for the smaller value of |inf-sup| (should be 0 or 1)
+// int imin = 0 ; // index of the median in the array
+// //double eqmax = 0 ; // count of equal values
+// // min cannot be greater than the number of element
+// double min = n;
+//
+// // number of medians
+// int mediancount = 0;
+// // array of medians
+// int *medianindex = new int[n];
+//
+// for (i=0; i<n; i++)
+// {
+// if(infsup[i] < min)
+// {
+// min = infsup[i];
+// imin = i ;
+//
+// //reset count of median values
+// mediancount=0;
+// medianindex[mediancount]=i;
+// }
+// else if(infsup[i]==min) //If there is another median
+// {
+// mediancount++;
+// medianindex[mediancount]=i;
+// }
+// }
+//
+// // Choose smalest data to be the median
+// /*for(i=0; i<mediancount+1; i++)
+// {
+// //Choose the value with the greatest count
+// if(eq[medianindex[i]] > eqmax)
+// {
+// eqmax = eq[medianindex[i]];
+// imin = medianindex[i];
+// }
+// //If we have identical counts
+// // Choose smalest data to be the median
+// //if(v[medianindex[i]] < v[imin])
+// // imin = medianindex[i];
+// }*/
+//
+// // return the median
+// delete []medianindex;
+// return(v[imin]);
+// }
+//
+// // Calculate median only for the residues which have
+// // not be rejected. i.e. weight=0
+// double
+// vpRobust::median(const vpColVector &v, vpColVector &weights)
+// {
+// int i,j;
+// int inf, sup;
+// int n = v.getRows() ;
+// vpColVector infsup(n) ;
+// vpColVector eq(n) ;
+//
+// for (i=0;i<n;i++)
+// {
+// if(weights[i]!=0)
+// {
+// // We compute the number of elements superior to the current value (sup)
+// // the number of elements inferior (inf) to the current value and
+// // the number of elements equal to the current value (eq)
+// inf = sup = 0;
+// for (j=0;j<n;j++)
+// {
+// if (weights[j]!=0 && i!=j)
+// {
+// if (v[i] <= v[j]) inf++;
+// if (v[i] >= v[j]) sup++;
+// if (v[i] == v[j]) eq[i]++;
+// }
+// }
+// // We compute then difference between inf and sup
+// // the median should be for |inf-sup| = 0 (1 if an even number of element)
+// // which means that there are the same number of element in the array
+// // that are greater and smaller that this value.
+// infsup[i] = abs(inf-sup);
+// }
+// }
+//
+// // seek for the smaller value of |inf-sup| (should be 0 or 1)
+// int imin = 0 ; // index of the median in the array
+// //double eqmax = 0 ; // count of equal values
+// // min cannot be greater than the number of element
+// double min = n;
+//
+// for (i=0; i<n; i++)
+// {
+// if(weights[i]!=0)
+// {
+// if(infsup[i] < min)
+// {
+// min = infsup[i];
+// imin = i ;
+// }
+// }
+// }
+//
+// // return the median
+// return(v[imin]);
+// }
+
+
+#undef vpITMAX
+#undef vpEPS
+#undef vpCST
diff --git a/modules/core/src/math/robust/vpScale.cpp b/modules/core/src/math/robust/vpScale.cpp
new file mode 100644
index 0000000..f098e85
--- /dev/null
+++ b/modules/core/src/math/robust/vpScale.cpp
@@ -0,0 +1,302 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
+ *
+ * Authors:
+ * Andrew Comport
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpScale.cpp
+*/
+
+
+
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpScale.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#define DEBUG_LEVEL2 0
+
+
+
+
+//! Constructor
+vpScale::vpScale()
+ : bandwidth(0.02), dimension(1)
+{
+#if (DEBUG_LEVEL2)
+ std::cout << "vpScale constructor reached" << std::endl;
+#endif
+#if (DEBUG_LEVEL2)
+ std::cout << "vpScale constructor finished" << std::endl;
+#endif
+
+}
+
+//! Constructor
+vpScale::vpScale(double kernel_bandwidth, unsigned int dim)
+ : bandwidth(kernel_bandwidth), dimension(dim)
+
+{
+#if (DEBUG_LEVEL2)
+ std::cout << "vpScale constructor reached" << std::endl;
+#endif
+#if (DEBUG_LEVEL2)
+ std::cout << "vpScale constructor finished" << std::endl;
+#endif
+}
+
+//! Destructor
+vpScale::~vpScale()
+{
+}
+
+// Calculate the modes of the density for the distribution
+// and their associated errors
+double
+vpScale::MeanShift(vpColVector &error)
+{
+
+ unsigned int n = error.getRows()/dimension;
+ vpColVector density(n);
+ vpColVector density_gradient(n);
+ vpColVector mean_shift(n);
+
+ unsigned int increment=1;
+
+ // choose smallest error as start point
+ unsigned int i=0;
+ while(error[i]<0 && error[i]<error[i+1])
+ i++;
+
+ // Do mean shift until no shift
+ while(increment >= 1 && i<n)
+ {
+ increment=0;
+ density[i] = KernelDensity(error, i);
+ density_gradient[i] = KernelDensityGradient(error, i);
+ mean_shift[i]=vpMath::sqr(bandwidth)*density_gradient[i]/((dimension+2)*density[i]);
+
+ double tmp_shift = mean_shift[i];
+
+ // Do mean shift
+ while(tmp_shift>0 && tmp_shift>error[i]-error[i+1])
+ {
+ i++;
+ increment++;
+ tmp_shift-=(error[i]-error[i-1]);
+ }
+ }
+
+ return error[i];
+
+}
+
+// Calculate the density of each point in the error vector
+// Requires ordered set of errors
+double
+vpScale::KernelDensity(vpColVector &error, unsigned int position)
+{
+
+ unsigned int n = error.getRows()/dimension;
+ double density=0;
+ double Ke = 1;
+ unsigned int j=position;
+
+ vpColVector X(dimension);
+
+
+ // Use each error in the bandwidth to calculate
+ // the local density of error i
+ // First treat larger errors
+ //while(Ke !=0 && j<=n)
+ while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j<=n)
+ {
+ //Create vector of errors corresponding to each dimension of a feature
+ for(unsigned int i=0; i<dimension; i++)
+ {
+ X[i]=(error[position]-error[j])/bandwidth;
+ position++;
+ j++;
+ }
+ position-=dimension; // reset position
+
+ Ke = KernelDensity_EPANECHNIKOV(X);
+ density+=Ke;
+ }
+
+ Ke = 1;
+ j=position;
+ // Then treat smaller errors
+ //while(Ke !=0 && j>=dimension)
+ while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j>=dimension)
+ {
+ //Create vector of errors corresponding to each dimension of a feature
+ for(unsigned int i=0; i<dimension; i++)
+ {
+ X[i]=(error[position]-error[j])/bandwidth;
+ position++;
+ j--;
+ }
+ position-=dimension; // reset position
+
+ Ke = KernelDensity_EPANECHNIKOV(X);
+ density+=Ke;
+ }
+
+ density*=1/(n*bandwidth);
+
+ return density;
+
+}
+
+double
+vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
+{
+
+ unsigned int n = error.getRows()/dimension;
+ double density_gradient=0;
+ double sum_delta=0;
+ double delta=0;
+ int nx=0;
+
+ double inside_kernel = 1;
+ unsigned int j=position;
+ // Use each error in the bandwidth to calculate
+ // the local density gradient
+ // First treat larger errors than current
+ //while(inside_kernel !=0 && j<=n)
+ while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j<=n)
+ {
+ delta = error[position]-error[j];
+ if(vpMath::sqr(delta/bandwidth)<1)
+ {
+ inside_kernel = 1;
+ sum_delta+=error[j]-error[position];
+ j++;
+ nx++;
+ }
+ else
+ inside_kernel = 0;
+ }
+
+ inside_kernel = 1;
+ j=position;
+ // Then treat smaller errors than current
+ //while(inside_kernel !=0 && j>=dimension)
+ while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j>=dimension)
+ {
+ delta = error[position]-error[j];
+ if(vpMath::sqr(delta/bandwidth)<1)
+ {
+ inside_kernel = 1;
+ sum_delta+=error[j]-error[position];
+ j--;
+ nx++;
+ }
+ else
+ inside_kernel = 0;
+ }
+
+ density_gradient = KernelDensityGradient_EPANECHNIKOV(sum_delta, n);
+
+
+ return density_gradient;
+
+}
+
+
+//Epanechnikov_kernel for an d dimensional Euclidian space R^d
+double
+vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X)
+{
+
+ double XtX= X*X;
+ double c; // Volume of an n dimensional unit sphere
+
+ switch (dimension)
+ {
+ case 1:
+ c = 2;
+ break;
+ case 2:
+ c = M_PI;
+ break;
+ case 3:
+ c = 4*M_PI/3;
+ break;
+ default:
+ std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
+ exit(1);
+ }
+
+ if(XtX < 1)
+ return 1/(2*c)*(dimension+2)*(1-XtX);
+ else
+ return 0;
+
+}
+
+
+//Epanechnikov_kernel for an d dimensional Euclidian space R^d
+double
+vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
+{
+
+ double c; // Volume of an n dimensional unit sphere
+
+ switch (dimension)
+ {
+ case 1:
+ c = 2;
+ break;
+ case 2:
+ c = M_PI;
+ break;
+ case 3:
+ c = 4*M_PI/3;
+ break;
+ default:
+ std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
+ exit(1);
+ }
+
+ //return sumX*(dimension+2)/(n*pow(bandwidth, (double)dimension)*c*vpMath::sqr(bandwidth));
+ return sumX*(dimension+2)/(n*bandwidth*c*vpMath::sqr(bandwidth));
+
+}
+
diff --git a/modules/core/src/math/spline/vpBSpline.cpp b/modules/core/src/math/spline/vpBSpline.cpp
new file mode 100644
index 0000000..e6e832e
--- /dev/null
+++ b/modules/core/src/math/spline/vpBSpline.cpp
@@ -0,0 +1,529 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * This class implements the B-Spline
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpBSpline.h>
+#include <visp3/core/vpDebug.h>
+
+/*!
+ Basic constructor.
+
+ The degree \f$ p \f$ of the B-Spline basis functions is set to 3 to
+ compute cubic B-Spline.
+*/
+vpBSpline::vpBSpline()
+ : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
+ crossingPoints()
+{
+}
+
+/*!
+ Copy constructor.
+
+*/
+vpBSpline::vpBSpline(const vpBSpline &bspline)
+ : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
+ crossingPoints()
+{
+ controlPoints = bspline.controlPoints;
+ knots = bspline.knots;
+ p = bspline.p;
+ crossingPoints = bspline.crossingPoints;
+}
+/*!
+ Basic destructor.
+*/
+vpBSpline::~vpBSpline()
+{
+}
+
+/*!
+ Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$ l_u \in [u_i, u_{i+1}[ \f$
+
+ 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.
+
+ \param l_u : The knot whose knot interval is seeked.
+ \param l_p : Degree of the B-Spline basis functions.
+ \param l_knots : The knot vector
+
+ \return the number of the knot interval in which \f$ l_u \f$ lies.
+*/
+unsigned int
+vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
+{
+ unsigned int m = (unsigned int)l_knots.size()-1;
+
+ if(l_u > l_knots.back())
+ {
+ //vpTRACE("l_u higher than the maximum value in the knot vector : %lf",l_u);
+ return((unsigned int)(m-l_p-1));
+ }
+
+ //if (l_u == l_knots.back())
+ if (std::fabs(l_u - l_knots.back()) <= std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
+ return((unsigned int)(m-l_p-1));
+
+ double low = l_p;
+ double high = m-l_p;
+ double middle = (low+high)/2.0;
+
+ while (l_u < l_knots[(unsigned int)vpMath::round(middle)] || l_u >= l_knots[(unsigned int)vpMath::round(middle+1)])
+ {
+ if(l_u < l_knots[(unsigned int)vpMath::round(middle)]) high = middle;
+ else low = middle;
+ middle = (low+high)/2.0;
+ }
+
+ return (unsigned int)middle;
+}
+
+
+/*!
+ Find the knot interval in which the parameter \f$ u \f$ lies. Indeed \f$ u \in [u_i, u_{i+1}[ \f$
+
+ 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$ u \f$ equal to 0.5 the method will retun 1.
+ - For \f$ u \f$ equal to 2.5 the method will retun 3.
+ - For \f$ u \f$ equal to 3 the method will retun 3.
+
+ \param u : The knot whose knot interval is seeked.
+
+ \return the number of the knot interval in which \f$ u \f$ lies.
+*/
+unsigned int
+vpBSpline::findSpan(double u)
+{
+ return findSpan( u, p, knots);
+}
+
+
+/*!
+ Compute the nonvanishing basis functions at \f$ l_u \f$ which is in the \f$ l_i \f$ th knot interval. All the basis functions are stored in an array such as :
+
+ N = \f$ N_{l_i,0}(l_u) \f$, \f$ N_{l_i-1,1}(l_u) \f$, \f$ N_{l_i,1}(l_u) \f$, ... , \f$ N_{l_i-k,k}(l_u) \f$, ..., \f$ N_{l_i,k}(l_u) \f$, ... , \f$ N_{l_i-p,p}(l_u) \f$, ... , \f$ N_{l_i,p}(l_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 B-Spline basis functions.
+ \param l_knots : The knot vector
+
+ \return An array containing the nonvanishing basis functions at \f$ l_u \f$. The size of the array is \f$ l_p +1 \f$.
+*/
+vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots)
+{
+ vpBasisFunction* N = new vpBasisFunction[l_p+1];
+
+ N[0].value = 1.0;
+
+ double *left = new double[l_p+1];
+ double *right = new double[l_p+1];
+ double saved = 0.0;
+ double temp = 0.0;
+
+ for(unsigned int j = 1; j <= l_p; j++)
+ {
+ left[j] = l_u - l_knots[l_i+1-j];
+ right[j] = l_knots[l_i+j] - l_u;
+ saved = 0.0;
+
+ for (unsigned int r = 0; r < j; r++)
+ {
+ temp = N[r].value / (right[r+1]+left[j-r]);
+ N[r].value = saved +right[r+1]*temp;
+ saved = left[j-r]*temp;
+ }
+ N[j].value = saved;
+ }
+ for(unsigned int j = 0; j < l_p+1; j++)
+ {
+ N[j].i = l_i-l_p+j;
+ N[j].p = l_p;
+ N[j].u = l_u;
+ N[j].k = 0;
+ }
+
+ delete[] left;
+ delete[] right;
+
+ return N;
+}
+
+
+/*!
+ Compute the nonvanishing basis functions at \f$ u \f$. All the basis functions are stored in an array such as :
+
+ N = \f$ N_{i,0}(u) \f$, \f$ N_{i-1,1}(u) \f$, \f$ N_{i,1}(u) \f$, ... , \f$ N_{i-k,k}(u) \f$, ..., \f$ N_{i,k}(u) \f$, ... , \f$ N_{i-p,p}(u) \f$, ... , \f$ N_{i,p}(u) \f$
+
+ where i the number of the knot interval in which \f$ u \f$ lies.
+
+ \param u : A real number which is between the extrimities of the knot vector
+
+ \return An array containing the nonvanishing basis functions at \f$ u \f$. The size of the array is \f$ p +1 \f$.
+*/
+vpBasisFunction* vpBSpline::computeBasisFuns(double u)
+{
+ unsigned int i = findSpan(u);
+ return computeBasisFuns(u, i, p ,knots);
+}
+
+
+/*!
+ Compute the nonzero basis functions and their derivatives until the \f$ l_der \f$ th derivative. All the functions are computed at l_u.
+
+ \warning \f$ l_der \f$ must be under or equal \f$ l_p \f$.
+
+ The result is given as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
+
+ The formula to compute the kth derivative at \f$ u \f$ is :
+
+ \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
+
+ \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 B-Spline basis functions.
+ \param l_der : The last derivative to be computed.
+ \param l_knots : The knot vector
+
+ \return the basis functions and their derivatives as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
+
+ Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
+*/
+vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots)
+{
+ vpBasisFunction** N;
+ N = new vpBasisFunction*[l_der+1];
+ for(unsigned int j = 0; j <= l_der; j++)
+ N[j] = new vpBasisFunction[l_p+1];
+
+ vpMatrix a(2,l_p+1);
+ vpMatrix ndu(l_p+1,l_p+1);
+ ndu[0][0] = 1.0;
+
+ double *left = new double[l_p+1];
+ double *right = new double[l_p+1];
+ double saved = 0.0;
+ double temp = 0.0;
+
+ for(unsigned int j = 1; j <= l_p; j++)
+ {
+ left[j] = l_u - l_knots[l_i+1-j];
+ right[j] = l_knots[l_i+j] - l_u;
+ saved = 0.0;
+
+ for (unsigned int r = 0; r < j; r++)
+ {
+ ndu[j][r] = right[r+1]+left[j-r];
+ temp = ndu[r][j-1]/ndu[j][r];
+ ndu[r][j] = saved + right[r+1]*temp;
+ saved = left[j-r]*temp;
+ }
+ ndu[j][j] = saved;
+ }
+
+ for(unsigned int j = 0; j <= l_p; j++)
+ {
+ N[0][j].value = ndu[j][l_p];
+ N[0][j].i = l_i-l_p+j;
+ N[0][j].p = l_p;
+ N[0][j].u = l_u;
+ N[0][j].k = 0;
+ }
+
+ if( l_der > l_p)
+ {
+ vpTRACE("l_der must be under or equal to l_p");
+ l_der = l_p;
+ }
+
+ unsigned int s1,s2;
+ double d;
+ int rk;
+ unsigned int pk;
+ unsigned int j1,j2;
+
+ for (unsigned int r = 0; r <= l_p; r++)
+ {
+ s1 = 0;
+ s2 = 1;
+ a[0][0] = 1.0;
+ for(unsigned int k = 1; k <= l_der; k++)
+ {
+ d = 0.0;
+ rk = (int)(r-k);
+ pk = l_p-k;
+ if(r >= k)
+ {
+ a[s2][0] = a[s1][0]/ndu[pk+1][rk];
+ d = a[s2][0]*ndu[(unsigned int)rk][pk];
+ }
+
+ if(rk >= -1)
+ j1 = 1;
+ else
+ j1 = (unsigned int)(-rk);
+
+ if(r-1 <= pk)
+ j2 = k-1;
+ else
+ j2 = l_p-r;
+
+ for(unsigned int j =j1; j<= j2; j++)
+ {
+ a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][(unsigned int)rk+j];
+ d += a[s2][j]*ndu[(unsigned int)rk+j][pk];
+ }
+
+ if(r <= pk)
+ {
+ a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
+ d += a[s2][k]*ndu[r][pk];
+ }
+ N[k][r].value = d;
+ N[k][r].i = l_i-l_p+r;
+ N[k][r].p = l_p;
+ N[k][r].u = l_u;
+ N[k][r].k = k;
+
+ s1 = (s1+1)%2;
+ s2 = (s2+1)%2;
+ }
+ }
+
+ double r = l_p;
+ for ( unsigned int k = 1; k <= l_der; k++ )
+ {
+ for (unsigned int j = 0; j <= l_p; j++)
+ N[k][j].value *= r;
+ r *= (l_p-k);
+ }
+
+ delete[] left;
+ delete[] right;
+
+ return N;
+}
+
+
+/*!
+ Compute the nonzero basis functions and their derivatives until the \f$ der \f$ th derivative. All the functions are computed at u.
+
+ \warning \f$ der \f$ must be under or equal \f$ p \f$.
+
+ The result is given as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
+
+ The formula to compute the kth derivative at \f$ u \f$ is :
+
+ \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
+
+ \param u : A real number which is between the extrimities of the knot vector
+ \param der : The last derivative to be computed.
+
+ \return the basis functions and their derivatives as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
+
+ Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
+*/
+vpBasisFunction** vpBSpline::computeDersBasisFuns(double u, unsigned int der)
+{
+ unsigned int i = findSpan(u);
+ return computeDersBasisFuns(u, i, p , der, knots);
+}
+
+
+/*!
+ Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_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 B-Spline basis functions.
+ \param l_knots : The knot vector
+ \param l_controlPoints : the list of control points.
+
+ return the coordinates of a point corresponding to the knot \f$ u \f$.
+*/
+vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
+{
+ vpBasisFunction* N = computeBasisFuns(l_u, l_i, l_p, l_knots);
+ vpImagePoint pt;
+
+ double ic = 0;
+ double jc = 0;
+ for(unsigned int j = 0; j <= l_p; j++)
+ {
+ ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i();
+ jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j();
+ }
+
+ pt.set_i(ic);
+ pt.set_j(jc);
+
+ delete[] N;
+
+ return pt;
+}
+
+
+/*!
+ Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_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 vpBSpline::computeCurvePoint(double u)
+{
+ vpBasisFunction* N = computeBasisFuns(u);
+ vpImagePoint pt;
+
+ double ic = 0;
+ double jc = 0;
+ for(unsigned int j = 0; j <= p; j++)
+ {
+ ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i();
+ jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j();
+ }
+
+ pt.set_i(ic);
+ pt.set_j(jc);
+
+ delete[] N;
+
+ return pt;
+}
+
+
+/*!
+ 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)P_i) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
+
+ \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 B-Spline 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.
+
+ \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* vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
+{
+ vpImagePoint *derivate = new vpImagePoint[l_der+1];
+ vpBasisFunction** N;
+ N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
+
+ unsigned int du;
+ if (l_p < l_der)
+ {
+ vpTRACE("l_der must be under or equal to l_p");
+ du = l_p;
+ }
+ else du = l_der;
+
+ for(unsigned int k = 0; k <= du; k++)
+ {
+ derivate[k].set_ij(0.0,0.0);
+ for(unsigned int j = 0; j<= l_p; j++)
+ {
+ derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i());
+ derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j());
+ }
+ }
+
+
+ for(unsigned int j = 0; j <= l_der; j++)
+ delete[] N[j];
+ delete[] N;
+
+
+ return derivate;
+}
+
+
+/*!
+ 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)P_i) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
+
+ \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 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* vpBSpline::computeCurveDers(double u, unsigned int der)
+{
+ vpImagePoint *derivate = new vpImagePoint[der+1];
+ vpBasisFunction** N;
+ N = computeDersBasisFuns(u, der);
+
+ unsigned int du;
+ if (p < der)
+ {
+ vpTRACE("der must be under or equal to p");
+ du = p;
+ }
+ else du = der;
+
+ for(unsigned int k = 0; k <= du; k++)
+ {
+ derivate[k].set_ij(0.0,0.0);
+ for(unsigned int j = 0; j<= p; j++)
+ {
+ derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i());
+ derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j());
+ }
+ }
+
+ for(unsigned int j = 0; j <= der; j++)
+ delete[] N[j];
+ delete[] N;
+
+ return derivate;
+}
diff --git a/modules/core/src/math/transformation/vpExponentialMap.cpp b/modules/core/src/math/transformation/vpExponentialMap.cpp
new file mode 100644
index 0000000..9159b07
--- /dev/null
+++ b/modules/core/src/math/transformation/vpExponentialMap.cpp
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exponential map.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpExponentialMap.h>
+
+
+/*!
+
+ Compute the exponential map. The inverse function is inverse(). The sampling
+ time is here set to 1 second. To use an other value you should use
+ direct(const vpColVector &, const float &).
+
+ \param v : Instantaneous velocity represented by a 6 dimension
+ vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf t} \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector). \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
+ {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ \return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
+ velocity \f$ \bf v \f$. If \f$ \bf v \f$ is expressed in frame c, \f$ {\bf M}
+ = {^c}{\bf M}_{c_{new}} \f$.
+
+ \sa inverse(const vpHomogeneousMatrix &)
+*/
+vpHomogeneousMatrix
+vpExponentialMap::direct(const vpColVector &v)
+{
+ return vpExponentialMap::direct(v, 1.0);
+}
+
+/*!
+
+ Compute the exponential map. The inverse function is inverse().
+
+ \param v : Instantaneous velocity represented by a 6 dimension
+ vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf t} \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector). \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
+ {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ \param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
+ velocity v is applied.
+
+ \return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
+ velocity \f$ \bf v \f$. If \f$ \bf v \f$ is expressed in frame c, \f$ {\bf M}
+ = {^c}{\bf M}_{c_{new}} \f$.
+
+ \sa inverse(const vpHomogeneousMatrix &, const float &)
+*/
+vpHomogeneousMatrix
+vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
+{
+ double theta,si,co,sinc,mcosc,msinc;
+ vpThetaUVector u ;
+ vpRotationMatrix rd ;
+ vpTranslationVector dt ;
+
+ vpColVector v_dt = v * delta_t;
+
+ u[0] = v_dt[3];
+ u[1] = v_dt[4];
+ u[2] = v_dt[5];
+ rd.buildFrom(u);
+
+ theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
+ si = sin(theta);
+ co = cos(theta);
+ sinc = vpMath::sinc(si,theta);
+ mcosc = vpMath::mcosc(co,theta);
+ msinc = vpMath::msinc(si,theta);
+
+ dt[0] = v_dt[0]*(sinc + u[0]*u[0]*msinc)
+ + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
+ + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
+
+ dt[1] = v_dt[0]*(u[0]*u[1]*msinc + u[2]*mcosc)
+ + v_dt[1]*(sinc + u[1]*u[1]*msinc)
+ + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
+
+ dt[2] = v_dt[0]*(u[0]*u[2]*msinc - u[1]*mcosc)
+ + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
+ + v_dt[2]*(sinc + u[2]*u[2]*msinc);
+
+ vpHomogeneousMatrix Delta ;
+ Delta.insert(rd) ;
+ Delta.insert(dt) ;
+
+
+
+ if (0) // test new version wrt old version
+ {
+ // old version
+ unsigned int i,j;
+
+ double sinu,cosi,mcosi,s;
+ // double u[3];
+ // vpRotationMatrix rd ;
+ // vpTranslationVector dt ;
+
+ s = sqrt(v_dt[3]*v_dt[3] + v_dt[4]*v_dt[4] + v_dt[5]*v_dt[5]);
+ if (s > 1.e-15)
+ {
+ for (i=0;i<3;i++) u[i] = v_dt[3+i]/s;
+ sinu = sin(s);
+ cosi = cos(s);
+ mcosi = 1-cosi;
+ rd[0][0] = cosi + mcosi*u[0]*u[0];
+ rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
+ rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
+ rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
+ rd[1][1] = cosi + mcosi*u[1]*u[1];
+ rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
+ rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
+ rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
+ rd[2][2] = cosi + mcosi*u[2]*u[2];
+
+ dt[0] = v_dt[0]*(sinu/s + u[0]*u[0]*(1-sinu/s))
+ + v_dt[1]*(u[0]*u[1]*(1-sinu/s)-u[2]*mcosi/s)
+ + v_dt[2]*(u[0]*u[2]*(1-sinu/s)+u[1]*mcosi/s);
+
+ dt[1] = v_dt[0]*(u[0]*u[1]*(1-sinu/s)+u[2]*mcosi/s)
+ + v_dt[1]*(sinu/s + u[1]*u[1]*(1-sinu/s))
+ + v_dt[2]*(u[1]*u[2]*(1-sinu/s)-u[0]*mcosi/s);
+
+ dt[2] = v_dt[0]*(u[0]*u[2]*(1-sinu/s)-u[1]*mcosi/s)
+ + v_dt[1]*(u[1]*u[2]*(1-sinu/s)+u[0]*mcosi/s)
+ + v_dt[2]*(sinu/s + u[2]*u[2]*(1-sinu/s));
+ }
+ else
+ {
+ for (i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++) rd[i][j] = 0.0;
+ rd[i][i] = 1.0;
+ dt[i] = v_dt[i];
+ }
+ }
+ // end old version
+
+ // Test of the new version
+ vpHomogeneousMatrix Delta_old ;
+ Delta_old.insert(rd) ;
+ Delta_old.insert(dt) ;
+
+ int pb = 0;
+ for (i=0;i<4;i++)
+ {
+ for(j=0;j<4;j++)
+ if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) pb = 1;
+ }
+ if (pb == 1)
+ {
+ printf("pb vpHomogeneousMatrix::expMap\n");
+ std::cout << " Delta : " << std::endl << Delta << std::endl;
+ std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
+ }
+ // end of the test
+ }
+
+ return Delta ;
+}
+
+/*!
+
+ Compute an instantaneous velocity from an homogeneous matrix. The inverse
+ function is the exponential map, see direct().
+
+ \param M : An homogeneous matrix corresponding to a displacement.
+
+ \return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
+ vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
+ rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
+ vector: \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
+ u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ \sa direct(const vpColVector &)
+*/
+vpColVector
+vpExponentialMap::inverse(const vpHomogeneousMatrix &M)
+{
+ return vpExponentialMap::inverse(M, 1.0);
+}
+
+/*!
+
+ Compute an instantaneous velocity from an homogeneous matrix. The inverse
+ function is the exponential map, see direct().
+
+ \param M : An homogeneous matrix corresponding to a displacement.
+
+ \param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
+ displacement is applied.
+
+ \return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
+ vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
+ rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
+ vector: \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
+ u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ \sa direct(const vpColVector &, const float &)
+*/
+vpColVector
+vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t)
+{
+ vpColVector v(6);
+ unsigned int i;
+ double theta,si,co,sinc,mcosc,msinc,det;
+ vpThetaUVector u ;
+ vpRotationMatrix Rd,a;
+ vpTranslationVector dt ;
+
+ M.extract(Rd);
+ u.buildFrom(Rd);
+ for (i=0;i<3;i++) v[3+i] = u[i];
+
+ theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
+ si = sin(theta);
+ co = cos(theta);
+ sinc = vpMath::sinc(si,theta);
+ mcosc = vpMath::mcosc(co,theta);
+ msinc = vpMath::msinc(si,theta);
+
+ // a below is not a pure rotation matrix, even if not so far from
+ // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
+ // with V = t.U
+
+ a[0][0] = sinc + u[0]*u[0]*msinc;
+ a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
+ a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
+
+ a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
+ a[1][1] = sinc + u[1]*u[1]*msinc;
+ a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
+
+ a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
+ a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
+ a[2][2] = sinc + u[2]*u[2]*msinc;
+
+ det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
+ + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
+ - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
+
+ if (fabs(det) > 1.e-5)
+ {
+ v[0] = (M[0][3]*a[1][1]*a[2][2]
+ + M[1][3]*a[2][1]*a[0][2]
+ + M[2][3]*a[0][1]*a[1][2]
+ - M[2][3]*a[1][1]*a[0][2]
+ - M[1][3]*a[0][1]*a[2][2]
+ - M[0][3]*a[2][1]*a[1][2])/det;
+ v[1] = (a[0][0]*M[1][3]*a[2][2]
+ + a[1][0]*M[2][3]*a[0][2]
+ + M[0][3]*a[1][2]*a[2][0]
+ - a[2][0]*M[1][3]*a[0][2]
+ - a[1][0]*M[0][3]*a[2][2]
+ - a[0][0]*M[2][3]*a[1][2])/det;
+ v[2] = (a[0][0]*a[1][1]*M[2][3]
+ + a[1][0]*a[2][1]*M[0][3]
+ + a[0][1]*M[1][3]*a[2][0]
+ - a[2][0]*a[1][1]*M[0][3]
+ - a[1][0]*a[0][1]*M[2][3]
+ - a[0][0]*a[2][1]*M[1][3])/det;
+ }
+ else
+ {
+ v[0] = M[0][3];
+ v[1] = M[1][3];
+ v[2] = M[2][3];
+ }
+
+ // Apply the sampling time to the computed velocity
+ v /= delta_t;
+
+ return(v);
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp
new file mode 100644
index 0000000..f8c7d20
--- /dev/null
+++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp
@@ -0,0 +1,513 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Twist transformation matrix that allows to transform forces from one
+ * frame to an other.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <sstream>
+#include <assert.h>
+
+#include <visp3/core/vpForceTwistMatrix.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpForceTwistMatrix.cpp
+
+ \brief Definition of the vpForceTwistMatrix. Class that consider the
+ particular case of twist transformation matrix that allows to
+ transform a force/torque skew from one frame to an other.
+*/
+
+
+/*!
+ Copy operator.
+
+ \param M : Force/torque twist matrix to copy.
+*/
+vpForceTwistMatrix &
+vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
+{
+ for (int i=0; i<6; i++) {
+ for (int j=0; j<6; j++) {
+ rowPtrs[i][j] = M.rowPtrs[i][j];
+ }
+ }
+
+ return *this;
+}
+
+
+/*!
+ Initialize the force/torque 6 by 6 twist matrix to identity.
+*/
+void
+vpForceTwistMatrix::eye()
+{
+ for (unsigned int i=0 ; i < 6 ; i++) {
+ for (unsigned int j=0 ; j < 6; j++) {
+ if (i==j)
+ (*this)[i][j] = 1.0 ;
+ else
+ (*this)[i][j] = 0.0;
+ }
+ }
+}
+
+/*!
+ Initialize a force/torque twist transformation matrix to identity.
+*/
+vpForceTwistMatrix::vpForceTwistMatrix()
+ : vpArray2D<double>(6,6)
+{
+ eye() ;
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from another
+ force/torque twist matrix.
+
+ \param F : Force/torque twist matrix used as initializer.
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F)
+ : vpArray2D<double>(6,6)
+{
+ *this = F ;
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from an
+ homogeneous matrix. Given the homogeneous transformation
+
+ \f[ ^a{\bf M}_b = \left(\begin{array}{cc}
+ ^a{\bf R}_b & ^a{\bf t}_b\\
+ 0 & 1
+ \end{array} \right) \f]
+
+ the force/torque twist matrix is given by :
+
+ \f[
+ ^a{\bf F}_b = \left[\begin{array}{cc}
+ ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
+ {[^a{\bf t}_b]}_{\times} \; ^a{\bf R}_b & ^a{\bf R}_b
+ \end{array}
+ \right]
+ \f]
+
+ \param M : Homogeneous matrix \f$^a{\bf M}_b\f$ used to initialize the twist
+ transformation matrix.
+
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M)
+ : vpArray2D<double>(6,6)
+{
+ buildFrom(M);
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation vector
+ \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \param t : Translation vector.
+
+ \param thetau : \f$\theta u\f$ rotation vector.
+
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t,
+ const vpThetaUVector &thetau)
+ : vpArray2D<double>(6,6)
+{
+ buildFrom(t, thetau) ;
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation vector
+ \e t and a rotation matrix R.
+
+ \param t : Translation vector.
+
+ \param R : Rotation matrix.
+
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+ : vpArray2D<double>(6,6)
+{
+ buildFrom(t, R) ;
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation vector
+ \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
+
+ \param tx,ty,tz : Translation vector in meters.
+
+ \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz)
+ : vpArray2D<double>(6,6)
+{
+ vpTranslationVector T(tx,ty,tz) ;
+ vpThetaUVector tu(tux,tuy,tuz) ;
+ buildFrom(T,tu) ;
+}
+
+/*!
+ Set the twist transformation matrix to identity.
+ \sa eye()
+*/
+void
+vpForceTwistMatrix::setIdentity()
+{
+ eye() ;
+}
+
+
+/*!
+
+ Operator that allows to multiply a skew transformation matrix by an
+ other skew transformation matrix.
+
+\code
+#include <visp3/core/vpForceTwistMatrix.h>
+
+int main()
+{
+ vpForceTwistMatrix aFb, bFc;
+ // ... initialize the force/torque twist transformations aFb and bFc
+ // Compute the force/torque transformation from frame a to c
+ vpForceTwistMatrix aFc = aFb * bFc;
+}
+\endcode
+
+*/
+vpForceTwistMatrix
+vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const
+{
+ vpForceTwistMatrix Fout ;
+
+ for (unsigned int i=0;i<6;i++) {
+ for (unsigned int j=0;j<6;j++) {
+ double s =0 ;
+ for (unsigned int k=0;k<6;k++)
+ s +=rowPtrs[i][k] * F.rowPtrs[k][j];
+ Fout[i][j] = s ;
+ }
+ }
+ return Fout;
+}
+
+/*!
+ Operator that allows to multiply a skew transformation matrix by a matrix.
+
+ \exception vpException::dimensionError If M is not a 6 rows
+ dimension matrix.
+*/
+vpMatrix
+vpForceTwistMatrix::operator*(const vpMatrix &M) const
+{
+
+ if (6 != M.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix",
+ M.getRows(), M.getCols()));
+
+ }
+
+ vpMatrix p(6, M.getCols()) ;
+ for (unsigned int i=0;i<6;i++) {
+ for (unsigned int j=0;j<M.getCols();j++) {
+ double s =0 ;
+ for (unsigned int k=0;k<6;k++)
+ s += rowPtrs[i][k] * M[k][j];
+ p[i][j] = s ;
+ }
+ }
+ return p;
+}
+
+/*!
+
+ Operator that allows to multiply a skew transformation matrix by a
+ column vector.
+
+ \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$.
+
+ For example, this operator can be used to convert a force/torque skew from sensor
+ frame into the probe frame :
+
+ \f$^p{\bf H} = ^p{\bf F}_s \; ^s{\bf H}\f$
+
+ The example below shows how to handle that transformation.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpForceTwistMatrix.h>
+
+int main()
+{
+ // Get the force/torque measures from the sensor
+ vpColVector sH(6); // Force/torque measures given by the sensor
+
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+ robot.getForceTorque(sH); // Get the force/torque measures
+#endif
+
+ // Set the transformation from sensor frame to the probe frame
+ vpHomogeneousMatrix pMs;
+ pMs[2][3] = -0.262; // tz only
+
+ // Set the force/torque twist transformation
+ vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame
+
+ // Compute the resulting force/torque in the probe frame
+ vpColVector pH(6); // Force/torque in the probe frame
+ pH = pFs * sH;
+
+ return 0;
+}
+ \endcode
+
+ \exception vpException::dimensionError If \f$ \bf H \f$is not a 6
+ dimension vector.
+
+*/
+vpColVector
+vpForceTwistMatrix::operator*(const vpColVector &H) const
+{
+ vpColVector Hout(6);
+
+ if (6 != H.getRows()) {
+ throw (vpException(vpException::dimensionError,
+ "Cannot multiply a (6x6) force/torque twist matrix by a %d dimension column vector",
+ H.getRows()));
+ }
+
+ Hout = 0.0;
+
+ for (unsigned int i=0;i<6;i++) {
+ for (unsigned int j=0;j<6;j++) {
+ Hout[i]+=rowPtrs[i][j] * H[j];
+ }
+ }
+
+ return Hout ;
+}
+
+
+/*!
+
+ Build a force/torque twist transformation matrix from a translation vector
+ \e t and a rotation matrix R.
+
+ \param t : Translation vector.
+
+ \param R : Rotation matrix.
+
+*/
+vpForceTwistMatrix
+vpForceTwistMatrix::buildFrom(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+{
+ unsigned int i, j;
+ vpMatrix skewaR = t.skew(t)*R ;
+
+ for (i=0 ; i < 3 ; i++) {
+ for (j=0 ; j < 3 ; j++) {
+ (*this)[i][j] = R[i][j] ;
+ (*this)[i+3][j+3] = R[i][j] ;
+ (*this)[i+3][j] = skewaR[i][j] ;
+ }
+ }
+ return (*this) ;
+}
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation vector
+ \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \param tv : Translation vector.
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector.
+
+*/
+vpForceTwistMatrix
+vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
+ const vpThetaUVector &thetau)
+{
+ vpRotationMatrix R ;
+ R.buildFrom(thetau) ;
+ buildFrom(tv,R) ;
+ return (*this) ;
+}
+
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from an homogeneous matrix
+ \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
+ \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
+
+ \param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
+ transformation matrix.
+
+*/
+vpForceTwistMatrix
+vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
+{
+ vpTranslationVector tv ;
+ vpRotationMatrix R ;
+ M.extract(R) ;
+ M.extract(tv) ;
+
+ buildFrom(tv, R) ;
+ return (*this) ;
+}
+
+/*!
+
+ Pretty print a velocity twist matrix. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each matrix element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the matrix.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all matrix elements
+
+ \sa std::ostream &operator <<(ostream &s,const vpMatrix &m)
+*/
+int
+vpForceTwistMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = getRows();
+ unsigned int n = getCols();
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int i=0;i<m;++i) {
+ for (unsigned int j=0;j<n;++j){
+ oss.str("");
+ oss << (*this)[i][j];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[i][j];
+ oss.str(ossFixed.str());
+ }
+
+ values[i*n+j]=oss.str();
+ size_type thislen=values[i*n+j].size();
+ size_type p=values[i*n+j].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+ }
+ }
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ for (unsigned int i=0;i<m;i++) {
+ s <<" ";
+ for (unsigned int j=0;j<n;j++){
+ size_type p=values[i*n+j].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[i*n+j].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[i*n+j].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+ }
+ s <<std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp
new file mode 100644
index 0000000..c0d3eee
--- /dev/null
+++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp
@@ -0,0 +1,862 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homogeneous matrix.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpHomogeneousMatrix.cpp
+ \brief Defines vpHomogeneousMatrix class. Class that consider
+ the particular case of an homogeneous matrix.
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpException.h>
+
+/*!
+ Construct an homogeneous matrix from a translation vector and quaternion rotation vector.
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
+ const vpQuaternionVector &q)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(t,q);
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Default constructor that initialize an homogeneous matrix as identity.
+*/
+vpHomogeneousMatrix::vpHomogeneousMatrix()
+ : vpArray2D<double>(4, 4)
+{
+ eye() ;
+}
+
+/*!
+ Copy constructor that initialize an homogeneous matrix from another homogeneous matrix.
+*/
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M)
+ : vpArray2D<double>(4, 4)
+{
+ *this = M;
+}
+
+/*!
+ Construct an homogeneous matrix from a translation vector and \f$\theta {\bf u}\f$ rotation vector.
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
+ const vpThetaUVector &tu)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(t, tu);
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Construct an homogeneous matrix from a translation vector and a rotation matrix.
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+ : vpArray2D<double>(4, 4)
+{
+ insert(R);
+ insert(t);
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Construct an homogeneous matrix from a pose vector.
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]) ;
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Construct an homogeneous matrix from a vector of float.
+ \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ std::vector<float> v(12, 0);
+ v[1] = -1.; // ry=-90
+ v[4] = 1.; // rx=90
+ v[10] = -1.; // rz=-90
+ v[3] = 0.3; // tx
+ v[7] = 0.4; // ty
+ v[11] = 0.5; // tz
+
+ std::cout << "v: ";
+ for(unsigned int i=0; i<v.size(); i++)
+ std::cout << v[i] << " ";
+ std::cout << std::endl;
+
+ vpHomogeneousMatrix M(v);
+ std::cout << "M:\n" << M << std::endl;
+}
+ \endcode
+
+ It produces the following printings:
+ \code
+v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
+M:
+0 -1 0 0.3000000119
+1 0 0 0.400000006
+0 0 -1 0.5
+0 0 0 1
+ \endcode
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(v) ;
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Construct an homogeneous matrix from a vector of double.
+ \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ std::vector<double> v(12, 0);
+ v[1] = -1.; // ry=-90
+ v[4] = 1.; // rx=90
+ v[10] = -1.; // rz=-90
+ v[3] = 0.3; // tx
+ v[7] = 0.4; // ty
+ v[11] = 0.5; // tz
+
+ std::cout << "v: ";
+ for(unsigned int i=0; i<v.size(); i++)
+ std::cout << v[i] << " ";
+ std::cout << std::endl;
+
+ vpHomogeneousMatrix M(v);
+ std::cout << "M:\n" << M << std::endl;
+}
+ \endcode
+
+ It produces the following printings:
+ \code
+v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
+M:
+0 -1 0 0.3
+1 0 0 0.4
+0 0 -1 0.5
+0 0 0 1
+ \endcode
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(v) ;
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Construct an homogeneous matrix from a translation vector \f${\bf t}=(t_x, t_y, t_z)^T\f$
+ and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ rotation vector.
+ */
+vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
+ const double ty,
+ const double tz,
+ const double tux,
+ const double tuy,
+ const double tuz)
+ : vpArray2D<double>(4, 4)
+{
+ buildFrom(tx, ty, tz, tux, tuy, tuz);
+ (*this)[3][3] = 1.;
+}
+
+/*!
+ Build an homogeneous matrix from a translation vector
+ and a \f$\theta {\bf u}\f$ rotation vector.
+ */
+void
+vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
+ const vpThetaUVector &tu)
+{
+ insert(tu) ;
+ insert(t) ;
+}
+
+/*!
+ Build an homogeneous matrix from a translation vector
+ and a rotation matrix.
+ */
+void
+vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+{
+ insert(R) ;
+ insert(t) ;
+}
+
+/*!
+ Build an homogeneous matrix from a pose vector.
+ */
+void
+vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
+{
+ vpTranslationVector tv(p[0], p[1], p[2]);
+ vpThetaUVector tu(p[3], p[4], p[5]);
+
+ insert(tu) ;
+ insert(tv) ;
+}
+
+/*!
+ Build an homogeneous matrix from a translation vector
+ and a quaternion rotation vector.
+ */
+void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
+ const vpQuaternionVector &q)
+{
+ insert(t);
+ insert(q);
+}
+
+/*!
+ Build an homogeneous matrix from a translation vector \f${\bf t}=(t_x, t_y, t_z)^T\f$
+ and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ rotation vector.
+ */
+void
+vpHomogeneousMatrix::buildFrom(const double tx,
+ const double ty,
+ const double tz,
+ const double tux,
+ const double tuy,
+ const double tuz)
+{
+ vpRotationMatrix R(tux, tuy, tuz) ;
+ vpTranslationVector t(tx, ty, tz) ;
+
+ insert(R) ;
+ insert(t) ;
+}
+
+/*!
+ Build an homogeneous matrix from a vector of float.
+ \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ std::vector<float> v(12, 0);
+ v[1] = -1.; // ry=-90
+ v[4] = 1.; // rx=90
+ v[10] = -1.; // rz=-90
+ v[3] = 0.3; // tx
+ v[7] = 0.4; // ty
+ v[11] = 0.5; // tz
+
+ std::cout << "v: ";
+ for(unsigned int i=0; i<v.size(); i++)
+ std::cout << v[i] << " ";
+ std::cout << std::endl;
+
+ vpHomogeneousMatrix M;
+ M.buildFrom(v);
+ std::cout << "M:\n" << M << std::endl;
+}
+ \endcode
+
+ It produces the following printings:
+ \code
+v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
+M:
+0 -1 0 0.3000000119
+1 0 0 0.400000006
+0 0 -1 0.5
+0 0 0 1
+ \endcode
+ */
+void
+vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
+{
+ if (v.size() != 12 && v.size() != 16) {
+ throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
+ }
+
+ for (unsigned int i=0; i < 12; i++)
+ this->data[i] = (double)v[i];
+}
+
+/*!
+ Build an homogeneous matrix from a vector of double.
+ \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ std::vector<double> v(12, 0);
+ v[1] = -1.; // ry=-90
+ v[4] = 1.; // rx=90
+ v[10] = -1.; // rz=-90
+ v[3] = 0.3; // tx
+ v[7] = 0.4; // ty
+ v[11] = 0.5; // tz
+
+ std::cout << "v: ";
+ for(unsigned int i=0; i<v.size(); i++)
+ std::cout << v[i] << " ";
+ std::cout << std::endl;
+
+ vpHomogeneousMatrix M;
+ M.buildFrom(v);
+ std::cout << "M:\n" << M << std::endl;
+}
+ \endcode
+
+ It produces the following printings:
+ \code
+v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
+M:
+0 -1 0 0.3
+1 0 0 0.4
+0 0 -1 0.5
+0 0 0 1
+ \endcode
+ */
+void
+vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
+{
+ if (v.size() != 12 && v.size() != 16) {
+ throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
+ }
+
+ for (unsigned int i=0; i < 12; i++)
+ this->data[i] = v[i];
+}
+
+/*!
+ Copy operator that allows to set an homogeneous matrix from an other one.
+
+ \param M : Matrix to copy.
+*/
+vpHomogeneousMatrix &
+vpHomogeneousMatrix::operator=(const vpHomogeneousMatrix &M)
+{
+ for (int i=0; i<4; i++) {
+ for (int j=0; j<4; j++) {
+ rowPtrs[i][j] = M.rowPtrs[i][j];
+ }
+ }
+ return *this;
+}
+
+/*!
+ Operator that allow to multiply an homogeneous matrix by an other one.
+
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ vpHomogeneousMatrix aMb, bMc;
+ // Initialize aMb and bMc...
+
+ // Compute aMc * bMc
+ vpHomogeneousMatrix aMc = aMb * bMc;
+}
+ \endcode
+
+*/
+vpHomogeneousMatrix
+vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
+{
+ vpHomogeneousMatrix p,p1 ;
+
+ vpRotationMatrix R1, R2, R ;
+ vpTranslationVector T1, T2 , T;
+
+ extract(T1) ;
+ M.extract(T2) ;
+
+ extract (R1) ;
+ M.extract (R2) ;
+
+ R = R1*R2 ;
+
+ T = R1*T2 + T1 ;
+
+ p.insert(T) ;
+ p.insert(R) ;
+
+ return p;
+}
+
+/*!
+ Operator that allow to multiply an homogeneous matrix by a 4-dimension column vector.
+
+ \exception vpException::dimensionError : If the vector \e v is not a 4-dimension vector.
+*/
+vpColVector
+vpHomogeneousMatrix::operator*(const vpColVector &v) const
+{
+ if (v.getRows() != 4) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (4x4) homogeneous matrix by a (%dx1) column vector",
+ v.getRows()));
+ }
+ vpColVector p(rowNum);
+
+ p = 0.0;
+
+ for (unsigned int j=0;j<4;j++) {
+ for (unsigned int i=0;i<4;i++) {
+ p[i]+=rowPtrs[i][j] * v[j];
+ }
+ }
+
+ return p;
+}
+
+/*!
+ From the coordinates of the point in camera frame b and the transformation between
+ camera frame a and camera frame b computes the coordinates of the point in camera frame a.
+
+ \param bP : 3D coordinates of the point in camera frame bP.
+
+ \return A point with 3D coordinates in the camera frame a. The coordinates in the world or object
+ frame are set to the same coordinates than the one in the camera frame.
+*/
+vpPoint vpHomogeneousMatrix::operator*(const vpPoint& bP) const
+{
+ vpPoint aP ;
+
+ vpColVector v(4),v1(4) ;
+
+ v[0] = bP.get_X() ;
+ v[1] = bP.get_Y() ;
+ v[2] = bP.get_Z() ;
+ v[3] = bP.get_W() ;
+
+ v1[0] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2]+ (*this)[0][3]*v[3] ;
+ v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2]+ (*this)[1][3]*v[3] ;
+ v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2]+ (*this)[2][3]*v[3] ;
+ v1[3] = (*this)[3][0]*v[0] + (*this)[3][1]*v[1]+ (*this)[3][2]*v[2]+ (*this)[3][3]*v[3] ;
+
+ v1 /= v1[3] ;
+
+ // v1 = M*v ;
+ aP.set_X(v1[0]) ;
+ aP.set_Y(v1[1]) ;
+ aP.set_Z(v1[2]) ;
+ aP.set_W(v1[3]) ;
+
+ aP.set_oX(v1[0]) ;
+ aP.set_oY(v1[1]) ;
+ aP.set_oZ(v1[2]) ;
+ aP.set_oW(v1[3]) ;
+
+ return aP ;
+}
+
+/*********************************************************************/
+
+/*!
+ Test if the 3x3 rotational part of the homogeneous matrix is really
+ a rotation matrix.
+*/
+bool
+vpHomogeneousMatrix::isAnHomogeneousMatrix() const
+{
+ vpRotationMatrix R ;
+ extract(R) ;
+
+ return R.isARotationMatrix() ;
+}
+
+/*!
+ Extract the rotational matrix from the homogeneous matrix.
+ \param R : rotational component as a rotation matrix.
+*/
+void
+vpHomogeneousMatrix::extract(vpRotationMatrix &R) const
+{
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3; j++)
+ R[i][j] = (*this)[i][j] ;
+}
+
+/*!
+ Extract the translation vector from the homogeneous matrix.
+*/
+void
+vpHomogeneousMatrix::extract(vpTranslationVector &t) const
+{
+ t[0] = (*this)[0][3] ;
+ t[1] = (*this)[1][3] ;
+ t[2] = (*this)[2][3] ;
+}
+/*!
+ Extract the rotation as a \f$\theta \bf u\f$ vector.
+*/
+void
+vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
+{
+ vpRotationMatrix R;
+ (*this).extract(R);
+ tu.buildFrom(R);
+}
+
+/*!
+ Extract the rotation as a quaternion.
+*/
+void
+vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
+{
+ vpRotationMatrix R;
+ (*this).extract(R);
+ q.buildFrom(R);
+}
+
+/*!
+ Insert the rotational component of the homogeneous matrix.
+*/
+void
+vpHomogeneousMatrix::insert(const vpRotationMatrix &R)
+{
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3; j++)
+ (*this)[i][j] = R[i][j] ;
+}
+
+/*!
+
+ Insert the rotational component of the homogeneous matrix from a
+ \f$theta {\bf u}\f$ rotation vector.
+
+*/
+void
+vpHomogeneousMatrix::insert(const vpThetaUVector &tu)
+{
+ vpRotationMatrix R(tu) ;
+ insert(R) ;
+}
+
+/*!
+ Insert the translational component in a homogeneous matrix.
+*/
+void
+vpHomogeneousMatrix::insert(const vpTranslationVector &t)
+{
+ (*this)[0][3] = t[0] ;
+ (*this)[1][3] = t[1] ;
+ (*this)[2][3] = t[2] ;
+}
+
+/*!
+
+ Insert the rotational component of the homogeneous matrix from a
+ quaternion rotation vector.
+
+*/
+void
+vpHomogeneousMatrix::insert(const vpQuaternionVector &q){
+ insert(vpRotationMatrix(q));
+}
+
+/*!
+ Invert the homogeneous matrix
+
+ \return \f$\left[\begin{array}{cc}
+ {\bf R} & {\bf t} \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right]^{-1} = \left[\begin{array}{cc}
+ {\bf R}^T & -{\bf R}^T {\bf t} \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right]\f$
+
+*/
+vpHomogeneousMatrix
+vpHomogeneousMatrix::inverse() const
+{
+ vpHomogeneousMatrix Mi ;
+
+ vpRotationMatrix R ; extract(R) ;
+ vpTranslationVector T ; extract(T) ;
+
+ vpTranslationVector RtT ; RtT = -(R.t()*T) ;
+
+
+ Mi.insert(R.t()) ;
+ Mi.insert(RtT) ;
+
+ return Mi ;
+}
+
+/*!
+ Set transformation to identity.
+*/
+void vpHomogeneousMatrix::eye()
+{
+ (*this)[0][0] = 1 ;
+ (*this)[1][1] = 1 ;
+ (*this)[2][2] = 1 ;
+ (*this)[3][3] = 1 ;
+
+ (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0 ;
+ (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0 ;
+ (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0 ;
+ (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0 ;
+}
+
+/*!
+ Invert the homogeneous matrix.
+
+ \param M : The inverted homogeneous matrix: \f$\left[\begin{array}{cc}
+ {\bf R} & {\bf t} \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right]^{-1} = \left[\begin{array}{cc}
+ {\bf R}^T & -{\bf R}^T {\bf t} \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right]\f$
+
+*/
+void
+vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const
+{
+ M = inverse() ;
+}
+
+
+/*!
+ Write an homogeneous matrix in an output file stream.
+
+ \param f : Output file stream. The homogeneous matrix is saved as a
+ 4 by 4 matrix.
+
+ The code below shows how to save an homogeneous matrix in a file.
+
+ \code
+ // Contruct an homogeneous matrix
+ vpTranslationVector t(1,2,3);
+ vpRxyzVector r(M_PI, 0, -M_PI/4.);
+ vpRotationMatrix R(r);
+ vpHomogeneousMatrix M(t, R);
+
+ // Save the content of the matrix in "homogeneous.dat"
+ std::ofstream f("homogeneous.dat");
+ M.save(f);
+ \endcode
+
+ \sa load()
+*/
+void
+vpHomogeneousMatrix::save(std::ofstream &f) const
+{
+ if (! f.fail()) {
+ f << *this ;
+ }
+ else {
+ throw(vpException(vpException::ioError,
+ "Cannot save homogeneous matrix: ostream not open")) ;
+ }
+}
+
+
+/*!
+
+ Read an homogeneous matrix from an input file stream. The
+ homogeneous matrix is considered as a 4 by 4 matrix.
+
+ \param f : Input file stream.
+
+ The code below shows how to get an homogeneous matrix from a file.
+
+ \code
+ vpHomogeneousMatrix M;
+
+ std::ifstream f("homogeneous.dat");
+ M.load(f);
+ \endcode
+
+ \sa save()
+*/
+void
+vpHomogeneousMatrix::load(std::ifstream &f)
+{
+ if (! f.fail()) {
+ for (unsigned int i=0 ; i < 4 ; i++) {
+ for (unsigned int j=0 ; j < 4 ; j++) {
+ f >> (*this)[i][j] ;
+ }
+ }
+ }
+ else {
+ throw(vpException(vpException::ioError,
+ "Cannot laad homogeneous matrix: ifstream not open")) ;
+ }
+}
+
+//! Print the matrix as a pose vector \f$({\bf t}^T \theta {\bf u}^T)\f$
+void
+vpHomogeneousMatrix::print() const
+{
+ vpPoseVector r(*this) ;
+ std::cout << r.t() ;
+}
+/*!
+ Set homogeneous matrix to identity.
+ \sa eye()
+ */
+void
+vpHomogeneousMatrix::setIdentity()
+{
+ eye() ;
+}
+
+/*!
+ Converts an homogeneous matrix to a vector of 12 floats.
+ \param M : Converted matrix.
+ */
+void vpHomogeneousMatrix::convert(std::vector<float> &M)
+{
+ M.resize(12);
+ for(unsigned int i=0; i < 12; i++)
+ M[i] = (float)(this->data[i]);
+}
+
+/*!
+ Converts an homogeneous matrix to a vector of 12 doubles.
+ \param M : Converted matrix.
+ */
+void vpHomogeneousMatrix::convert(std::vector<double> &M)
+{
+ M.resize(12);
+ for(unsigned int i=0; i < 12; i++)
+ M[i] = this->data[i];
+}
+
+/*!
+ Return the translation vector from the homogeneous transformation matrix.
+ */
+vpTranslationVector vpHomogeneousMatrix::getTranslationVector()
+{
+ vpTranslationVector tr;
+ this->extract(tr);
+ return tr;
+}
+
+/*!
+ Return the rotation matrix from the homogeneous transformation matrix.
+ */
+vpRotationMatrix vpHomogeneousMatrix::getRotationMatrix()
+{
+ vpRotationMatrix R;
+ this->extract(R);
+ return R;
+}
+
+/*!
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part of the
+ homogeneous transformation.
+ */
+vpThetaUVector vpHomogeneousMatrix::getThetaUVector()
+{
+ vpThetaUVector tu;
+ this->extract(tu);
+ return tu;
+}
+
+/*!
+ Extract a column vector from an homogeneous matrix.
+ \warning All the indexes start from 0 in this function.
+ \param j : Index of the column to extract. If j=0, the first column is extracted.
+ \return The extracted column vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ vpHomogeneousMatrix M;
+
+ vpColVector t = M.getCol(3);
+ std::cout << "Last column: \n" << t << std::endl;
+}
+ \endcode
+It produces the following output:
+ \code
+Last column:
+0
+0
+1
+0
+ \endcode
+ */
+vpColVector
+vpHomogeneousMatrix::getCol(const unsigned int j) const
+{
+ if (j >= getCols())
+ throw(vpException(vpException::dimensionError,
+ "Unable to extract a column vector from the homogeneous matrix"));
+ unsigned int nb_rows = getRows();
+ vpColVector c(nb_rows);
+ for (unsigned int i=0 ; i < nb_rows ; i++)
+ c[i] = (*this)[i][j];
+ return c;
+}
diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp
new file mode 100644
index 0000000..9314c85
--- /dev/null
+++ b/modules/core/src/math/transformation/vpPoseVector.cpp
@@ -0,0 +1,536 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpPoseVector.cpp
+ \brief Pose vector.
+
+*/
+
+#include <sstream>
+#include <assert.h>
+
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpException.h>
+
+
+/*!
+
+ Default constructor that construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
+ u]^\top\f$ where \f$ \theta \bf u\f$ is a rotation vector \f$[\theta
+ u_x, \theta u_y, \theta u_z]^\top\f$ and \f$ \bf t \f$ is a
+ translation vector \f$[t_x, t_y, t_z]^\top\f$.
+
+ The pose vector is initialized to zero.
+
+*/
+vpPoseVector::vpPoseVector()
+ : vpArray2D<double>(6, 1)
+{}
+
+/*!
+
+ Construct a 6 dimension pose vector \f$ [\bf{t}, \theta
+ \bf{u}]^\top\f$ from 3 translations and 3 \f$ \theta \bf{u}\f$
+ angles.
+
+ Translations are expressed in meters, while rotations in radians.
+
+ \param tx,ty,tz : Translations \f$[t_x, t_y, t_z]^\top\f$
+ respectively along the x, y and z axis (in meters).
+
+ \param tux,tuy,tuz : Rotations \f$[\theta u_x, \theta u_y, \theta
+ u_z]^\top\f$ respectively around the x, y and z axis (in radians).
+
+*/
+vpPoseVector::vpPoseVector(const double tx,
+ const double ty,
+ const double tz,
+ const double tux,
+ const double tuy,
+ const double tuz)
+ : vpArray2D<double>(6, 1)
+{
+ (*this)[0] = tx;
+ (*this)[1] = ty;
+ (*this)[2] = tz;
+
+ (*this)[3] = tux;
+ (*this)[4] = tuy;
+ (*this)[5] = tuz;
+}
+
+/*!
+
+ Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
+ u]^\top\f$ from a translation vector \f$ \bf tv \f$ and a \f$\theta
+ \bf u\f$ vector.
+
+ \param tv : Translation vector \f$ \bf t \f$.
+ \param tu : \f$\theta \bf u\f$ rotation vector.
+
+*/
+vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
+ const vpThetaUVector& tu)
+ : vpArray2D<double>(6, 1)
+{
+ buildFrom(tv, tu) ;
+}
+
+/*!
+
+ Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
+ u]^\top\f$ from a translation vector \f$ \bf t \f$ and a rotation
+ matrix \f$ \bf R \f$.
+
+ \param tv : Translation vector \f$ \bf t \f$.
+
+ \param R : Rotation matrix \f$ \bf R \f$ from which \f$\theta \bf
+ u\f$ vector is extracted to initialise the pose vector.
+
+*/
+vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
+ const vpRotationMatrix& R)
+ : vpArray2D<double>(6, 1)
+{
+ buildFrom(tv, R) ;
+}
+
+/*!
+
+ Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
+ u]^\top\f$ from an homogeneous matrix \f$ \bf M \f$.
+
+ \param M : Homogeneous matrix \f$ \bf M \f$ from which translation
+ \f$ \bf t \f$ and \f$\theta \bf u \f$ vectors are extracted to
+ initialize the pose vector.
+
+*/
+vpPoseVector::vpPoseVector(const vpHomogeneousMatrix& M)
+ : vpArray2D<double>(6, 1)
+{
+ buildFrom(M) ;
+}
+
+/*!
+
+ Set the 6 dimension pose vector \f$ [\bf{t}, \theta
+ \bf{u}]^\top\f$ from 3 translations and 3 \f$ \theta \bf{u}\f$
+ angles.
+
+ Translations are expressed in meters, while rotations in radians.
+
+ \param tx,ty,tz : Translations \f$[t_x, t_y, t_z]^\top\f$
+ respectively along the x, y and z axis (in meters).
+
+ \param tux,tuy,tuz : Rotations \f$[\theta u_x, \theta u_y, \theta
+ u_z]^\top\f$ respectively around the x, y and z axis (in radians).
+
+*/
+void
+vpPoseVector::set(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz)
+{
+ (*this)[0] = tx;
+ (*this)[1] = ty;
+ (*this)[2] = tz;
+
+ (*this)[3] = tux;
+ (*this)[4] = tuy;
+ (*this)[5] = tuz;
+}
+
+/*!
+ Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$
+ from 3 translations and 3 \f$ \theta \bf{u}\f$ angles.
+
+ Translations are expressed in meters, while rotations in radians.
+
+ \param tx,ty,tz : Translations \f$[t_x, t_y, t_z]^\top\f$
+ respectively along the x, y and z axis (in meters).
+
+ \param tux,tuy,tuz : Rotations \f$[\theta u_x, \theta u_y, \theta
+ u_z]^\top\f$ respectively around the x, y and z axis (in radians).
+
+ \return The build pose vector.
+
+ \sa set()
+*/
+vpPoseVector
+vpPoseVector::buildFrom(const double tx, const double ty, const double tz,
+ const double tux, const double tuy, const double tuz)
+{
+ (*this)[0] = tx;
+ (*this)[1] = ty;
+ (*this)[2] = tz;
+
+ (*this)[3] = tux;
+ (*this)[4] = tuy;
+ (*this)[5] = tuz;
+ return *this ;
+}
+
+/*!
+ Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$ from
+ an homogeneous matrix \f$ \bf M \f$.
+
+ \param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
+ \bf t \f$ and \f$\theta \bf u \f$ vectors are extracted to initialize
+ the pose vector.
+
+ \return The build pose vector.
+
+*/
+vpPoseVector
+vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
+{
+ vpRotationMatrix R ; M.extract(R) ;
+ vpTranslationVector tv ; M.extract(tv) ;
+ buildFrom(tv,R) ;
+ return *this ;
+}
+
+/*!
+
+ Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$
+ from a translation vector \f$ \bf t \f$ and a \f$\theta \bf u\f$
+ vector.
+
+ \param tv : Translation vector \f$ \bf t \f$.
+ \param tu : \f$\theta \bf u\f$ rotation vector.
+
+ \return The build pose vector.
+*/
+vpPoseVector
+vpPoseVector::buildFrom(const vpTranslationVector& tv,
+ const vpThetaUVector& tu)
+{
+ for (unsigned int i =0 ; i < 3 ; i++) {
+ (*this)[i] = tv[i] ;
+ (*this)[i+3] = tu[i] ;
+ }
+ return *this ;
+}
+
+/*!
+
+ Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$
+ from a translation vector \f$ \bf t \f$ and a rotation matrix \f$
+ \bf R \f$.
+
+ \param tv : Translation vector \f$ \bf t \f$.
+
+ \param R : Rotation matrix \f$ \bf R \f$ from which \f$\theta \bf
+ u\f$ vector is extracted to initialise the pose vector.
+
+ \return The build pose vector.
+*/
+vpPoseVector
+vpPoseVector::buildFrom(const vpTranslationVector& tv,
+ const vpRotationMatrix& R)
+{
+ vpThetaUVector tu ;
+ tu.buildFrom(R) ;
+
+ buildFrom(tv, tu) ;
+ return *this ;
+}
+
+/*!
+ Extract the translation vector from the homogeneous matrix.
+*/
+void
+vpPoseVector::extract(vpTranslationVector &tv) const
+{
+ tv[0] = (*this)[0];
+ tv[1] = (*this)[1];
+ tv[2] = (*this)[2];
+}
+
+/*!
+ Extract the rotation as a \f$\theta \bf u\f$ vector.
+*/
+void
+vpPoseVector::extract(vpThetaUVector &tu) const
+{
+ tu[0] = (*this)[3];
+ tu[1] = (*this)[4];
+ tu[2] = (*this)[5];
+}
+/*!
+ Extract the rotation as a quaternion vector.
+*/
+void
+vpPoseVector::extract(vpQuaternionVector &q) const
+{
+ vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
+ q.buildFrom(R);
+}
+/*!
+ Extract the rotation as a rotation matrix.
+*/
+void
+vpPoseVector::extract(vpRotationMatrix &R) const
+{
+ R.buildFrom((*this)[3], (*this)[4], (*this)[5]);
+}
+/*!
+ Return the translation vector that corresponds to the translation part of the
+ pose vector.
+ */
+vpTranslationVector vpPoseVector::getTranslationVector() const
+{
+ vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]);
+ return tr;
+}
+
+/*!
+ Return the rotation matrix that corresponds to the rotation part of the
+ pose vector.
+ */
+vpRotationMatrix vpPoseVector::getRotationMatrix() const
+{
+ vpRotationMatrix R((*this)[0], (*this)[1], (*this)[2]);
+ return R;
+}
+
+/*!
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part of the
+ pose vector.
+ */
+vpThetaUVector vpPoseVector::getThetaUVector() const
+{
+ vpThetaUVector tu((*this)[0], (*this)[1], (*this)[2]);
+ return tu;
+}
+
+/*!
+
+ Prints to the standart stream the pose vector.
+
+ \warning Values concerning the \f$ \theta {\bf u}\f$ rotation are
+ converted in degrees.
+
+ The following code
+ \code
+ // Create a pose vector
+ vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
+ r.print();
+ \endcode
+ produces the output:
+
+ \code
+ 1 2 3 180 -180 0
+ \endcode
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+void
+vpPoseVector::print() const
+{
+ for (unsigned int i =0 ; i < 6 ; i++)
+ if (i<3) std::cout << (*this)[i] <<" " ;
+ else std::cout << vpMath::deg((*this)[i]) <<" " ;
+ std::cout <<std::endl ;
+}
+
+/*!
+
+ Save the pose vector in the output file stream.
+
+ \param f : Output file stream. Should be open before entering in this method.
+
+ \exception vpException::ioError : If the output stream is not open.
+
+ \sa load()
+*/
+void
+vpPoseVector::save(std::ofstream &f) const
+{
+ if (! f.fail()) {
+ f << *this ;
+ }
+ else {
+ throw(vpException(vpException::ioError,
+ "Cannot save the pose vector: ofstream not openned")) ;
+ }
+}
+
+
+/*!
+ Read a pose vector from an input file stream.
+
+ \param f : The input file stream..Should be open before entering in
+ this method.
+
+ \exception vpException::ioError : If the input file stream is not open.
+
+ \sa save()
+*/
+void
+vpPoseVector::load(std::ifstream &f)
+{
+ if (! f.fail()) {
+ for (unsigned int i=0 ; i < 6 ; i++) {
+ f >> (*this)[i] ;
+ }
+ }
+ else {
+ throw(vpException(vpException::ioError,
+ "Cannot read pose vector: ifstream not openned")) ;
+ }
+}
+
+/*
+ Transpose the pose vector. The resulting vector becomes a row vector.
+
+*/
+vpRowVector vpPoseVector::t() const
+{
+ vpRowVector v(rowNum);
+ memcpy(v.data, data, rowNum*sizeof(double)) ;
+ return v;
+}
+
+/*!
+
+ Pretty print a pose vector. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each vector element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the vector.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all vector elements.
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+int
+vpPoseVector::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = getRows();
+ unsigned int n = 1;
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int i=0;i<m;++i) {
+ oss.str("");
+ oss << (*this)[i];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[i];
+ oss.str(ossFixed.str());
+ }
+
+ values[i]=oss.str();
+ size_type thislen=values[i].size();
+ size_type p=values[i].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+
+ }
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ for (unsigned int i=0;i<m;i++) {
+ s <<" ";
+ size_type p=values[i].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[i].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[i].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+
+ s <<std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
diff --git a/modules/core/src/math/transformation/vpQuaternionVector.cpp b/modules/core/src/math/transformation/vpQuaternionVector.cpp
new file mode 100644
index 0000000..0f15a40
--- /dev/null
+++ b/modules/core/src/math/transformation/vpQuaternionVector.cpp
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Quaternion vector.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpMath.h>
+#include <stdio.h>
+#include <string.h>
+#include <algorithm>
+
+
+// minimum value of sine
+const double vpQuaternionVector::minimum = 0.0001;
+
+/*!
+ \file vpQuaternionVector.cpp
+ \brief Defines a quaternion and common operations on it.
+*/
+
+/*! Default constructor that initialize all the 4 angles to zero. */
+vpQuaternionVector::vpQuaternionVector()
+ : vpRotationVector(4)
+{}
+
+/*! Copy constructor. */
+vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q)
+ : vpRotationVector(q)
+{}
+
+//! Constructor from doubles.
+vpQuaternionVector::vpQuaternionVector(const double x_, const double y_,
+ const double z_,const double w_)
+ : vpRotationVector(4)
+{
+ set(x_, y_, z_, w_);
+}
+
+/*!
+ Constructs a quaternion from a rotation matrix.
+
+ \param R : Matrix containing a rotation.
+*/
+vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R)
+ : vpRotationVector(4)
+{
+ buildFrom(R);
+}
+
+/*!
+ Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector from a \f$\theta {\bf u}\f$ vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input to initialize the Euler angles.
+*/
+vpQuaternionVector::vpQuaternionVector(const vpThetaUVector& tu)
+ : vpRotationVector (4)
+{
+ buildFrom(tu) ;
+}
+
+
+/*!
+ Manually change values of a quaternion.
+ \param qx : x quaternion parameter.
+ \param qy : y quaternion parameter.
+ \param qz : z quaternion parameter.
+ \param qw : w quaternion parameter.
+*/
+void vpQuaternionVector::set(const double qx, const double qy,
+ const double qz, const double qw)
+{
+ data[0]=qx;
+ data[1]=qy;
+ data[2]=qz;
+ data[3]=qw;
+}
+/*!
+ Manually change values of a quaternion.
+ \param qx : x quaternion parameter.
+ \param qy : y quaternion parameter.
+ \param qz : z quaternion parameter.
+ \param qw : w quaternion parameter.
+
+ \sa set()
+*/
+vpQuaternionVector
+vpQuaternionVector::buildFrom(const double qx, const double qy,
+ const double qz, const double qw)
+{
+ set(qx, qy, qz, qw);
+ return *this;
+}
+
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a quaternion.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input.
+ \return Quaternion vector.
+*/
+vpQuaternionVector
+vpQuaternionVector::buildFrom(const vpThetaUVector& tu)
+{
+ vpRotationMatrix R(tu) ;
+ buildFrom(R) ;
+
+ return *this ;
+}
+/*!
+ Quaternion addition.
+
+ Adds two quaternions. Addition is component-wise.
+
+ \param q : quaternion to add.
+*/
+vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) const
+{
+ return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
+}
+/*!
+ Quaternion substraction.
+
+ Substracts a quaternion from another. Substraction is component-wise.
+
+ \param q : quaternion to substract.
+*/
+vpQuaternionVector vpQuaternionVector::operator-(const vpQuaternionVector &q) const
+{
+ return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
+}
+
+//! Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
+vpQuaternionVector vpQuaternionVector::operator-() const
+{
+ return vpQuaternionVector(-x(), -y(), -z(), -w());
+}
+
+//! Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
+vpQuaternionVector vpQuaternionVector::operator*(const double l) const
+{
+ return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
+}
+
+//! Multiply two quaternions.
+vpQuaternionVector vpQuaternionVector::operator* (const vpQuaternionVector &rq) const {
+ return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
+ w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
+ w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
+ w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
+}
+
+//! Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
+vpQuaternionVector vpQuaternionVector::operator/(const double l) const
+{
+ if(vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
+ throw vpException(vpException::fatalError, "Division by scalar l==0 !");
+ }
+
+ return vpQuaternionVector(x()/l,y()/l,z()/l,w()/l);
+}
+
+/*!
+ Constructs a quaternion from a rotation matrix.
+
+ \param R : Rotation matrix.
+*/
+vpQuaternionVector
+vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
+{
+ vpThetaUVector tu(R);
+ vpColVector u;
+ double theta;
+ tu.extract(theta, u);
+
+ theta *= 0.5;
+
+ double sinTheta_2 = sin(theta);
+ set( u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta) );
+ return *this;
+}
+
+/*!
+ Quaternion conjugate.
+
+ \return The conjugate quaternion.
+*/
+vpQuaternionVector vpQuaternionVector::conjugate() const {
+ return vpQuaternionVector( -x(), -y(), -z(), w() );
+}
+
+/*!
+ Quaternion inverse.
+
+ \return The inverse quaternion.
+*/
+vpQuaternionVector vpQuaternionVector::inverse() const {
+ vpQuaternionVector q_inv;
+
+ double mag_square = w()*w() + x()*x() + y()*y() + z()*z();
+ if(!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
+ q_inv = this->conjugate() / mag_square;
+ } else {
+ std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
+ }
+
+ return q_inv;
+}
+
+/*!
+ Quaternion magnitude or norm.
+
+ \return The magnitude or norm of the quaternion.
+*/
+double vpQuaternionVector::magnitude() const {
+ return sqrt( w()*w() + x()*x() + y()*y() + z()*z() );
+}
+
+/*!
+ Normalize the quaternion.
+*/
+void vpQuaternionVector::normalize() {
+ double mag = magnitude();
+ if(!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
+ set( x()/mag, y()/mag, z()/mag, w()/mag );
+ }
+}
+
+//! Returns x-component of the quaternion.
+double vpQuaternionVector::x() const {return data[0];}
+//! Returns y-component of the quaternion.
+double vpQuaternionVector::y() const {return data[1];}
+//! Returns z-component of the quaternion.
+double vpQuaternionVector::z() const {return data[2];}
+//! Returns w-component of the quaternion.
+double vpQuaternionVector::w() const {return data[3];}
diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp
new file mode 100644
index 0000000..5ad5104
--- /dev/null
+++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp
@@ -0,0 +1,756 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rotation matrix.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpRotationMatrix.cpp
+ \brief Class that consider
+ the particular case of rotation matrix
+*/
+
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+
+// Rotation classes
+#include <visp3/core/vpRotationMatrix.h>
+
+
+// Exception
+#include <visp3/core/vpException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+#include <math.h>
+const double vpRotationMatrix::threshold = 1e-6;
+
+
+/*!
+ Initializes the rotation matrix as identity.
+
+ \sa eye()
+*/
+void
+vpRotationMatrix::setIdentity()
+{
+ eye();
+}
+/*!
+ Initialize the rotation matrix as identity.
+
+ \sa setIdentity()
+*/
+void
+vpRotationMatrix::eye()
+{
+ for (unsigned int i=0 ; i < 3 ; i++) {
+ for (unsigned int j=0 ; j < 3; j++) {
+ if (i==j)
+ (*this)[i][j] = 1.0;
+ else
+ (*this)[i][j] = 0.0;
+ }
+ }
+}
+
+/*!
+ Set the current rotation matrix from a rotation matrix \e R.
+
+ \param R : Rotation matrix.
+ \code
+ vpRotationMatrix R1(vpMath::rad(10, vpMath::rad(20), vpMath::rad(30));
+ vpRotationMatrix R2 = R1;
+ \endcode
+*/
+vpRotationMatrix &
+vpRotationMatrix::operator=(const vpRotationMatrix &R)
+{
+ for (unsigned int i=0; i<3; i++) {
+ for (unsigned int j=0; j<3; j++) {
+ rowPtrs[i][j] = R.rowPtrs[i][j];
+ }
+ }
+
+ return *this;
+}
+
+/*!
+ Converts a 3-by-3 matrix into a rotation matrix.
+
+ \param M : Input matrix.
+
+ \code
+ vpMatrix M;
+ M.eye()
+ vpRotationMatrix R = M;
+ \endcode
+
+ \exception vpException::fatalError If the input matrix is not a rotation matrix.
+
+ \sa isARotationMatrix()
+*/
+vpRotationMatrix &
+vpRotationMatrix::operator=(const vpMatrix &M)
+{
+ if ((M.getCols() !=3) &&(M.getRows() !=3)) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
+ M.getRows(), M.getCols()));
+ }
+
+ for (unsigned int i=0; i<3; i++) {
+ for (unsigned int j=0; j<3; j++) {
+ (*this)[i][j] = M[i][j];
+ }
+ }
+
+ if (isARotationMatrix() == false) {
+ throw(vpException(vpException::fatalError,
+ "Cannot set a rotation matrix from a matrix that is not a rotation matrix"));
+ }
+
+ return *this;
+}
+
+/*!
+ Compute the product between two rotation matrices.
+ */
+vpRotationMatrix
+vpRotationMatrix::operator*(const vpRotationMatrix &R) const
+{
+ vpRotationMatrix p ;
+
+ for (unsigned int i=0;i<3;i++) {
+ for (unsigned int j=0;j<3;j++) {
+ double s =0 ;
+ for (unsigned int k=0;k<3;k++)
+ s +=rowPtrs[i][k] * R.rowPtrs[k][j];
+ p[i][j] = s ;
+ }
+ }
+ return p;
+}
+/*!
+ Operator that allows to multiply a rotation matrix by a 3-by-3 matrix.
+ Allows for example to multiply a rotation matrix by a skew matrix.
+ \code
+ vpRotationMatrix R;
+ vpTranslationVector t;
+ vpMatrix M = t.skew();
+ vpMatrix RM = R * M;
+ \endcode
+
+ \exception vpException::dimensionError : If \e M is not
+ a 3-by-3 dimension matrix.
+
+*/
+vpMatrix
+vpRotationMatrix::operator*(const vpMatrix &M) const
+{
+ if (M.getRows() != 3 || M.getCols() != 3) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
+ M.getRows(), M.getCols()));
+ }
+ vpMatrix p(3, 3) ;
+
+ for (unsigned int i=0;i<3;i++) {
+ for (unsigned int j=0;j<3;j++) {
+ double s =0 ;
+ for (unsigned int k=0;k<3;k++)
+ s +=(*this)[i][k] * M[k][j];
+ p[i][j] = s ;
+ }
+ }
+ return p;
+}
+
+/*!
+
+ Operator that allows to multiply a rotation matrix by a 3 dimension
+ column vector.
+
+ \param v : Three dimension column vector.
+
+ \return The product of the rotation matrix by the column vector
+
+ \exception vpException::dimensionError If the column
+ vector \e v is not a 3 dimension vector.
+
+ The code below shows how to use this operator.
+\code
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector p1(3), p2(3);
+ vpRotationMatrix R;
+
+ p2 = R * p1;
+
+ return 0;
+}
+\endcode
+
+*/
+vpColVector
+vpRotationMatrix::operator*(const vpColVector &v) const
+{
+ if (v.getRows() != 3) {
+ throw (vpException(vpException::dimensionError,
+ "Cannot multiply a (3x3) rotation matrix by a %d dimension column vector",
+ v.getRows()));
+ }
+ vpColVector v_out(3);
+
+ for (unsigned int j=0;j<colNum;j++) {
+ double vj = v[j] ; // optimization em 5/12/2006
+ for (unsigned int i=0;i<rowNum;i++) {
+ v_out[i] += rowPtrs[i][j] * vj;
+ }
+ }
+
+ return v_out;
+}
+
+
+/*!
+ Multiply a rotation matrix by a translation vector and return the resulting translation vector.
+ */
+vpTranslationVector
+vpRotationMatrix::operator*(const vpTranslationVector &tv) const
+{
+ vpTranslationVector p ;
+
+ for (unsigned int j=0;j<3;j++)
+ p[j] = 0;
+
+ for (unsigned int j=0;j<3;j++) {
+ for (unsigned int i=0;i<3;i++) {
+ p[i] += rowPtrs[i][j] * tv[j];
+ }
+ }
+
+ return p;
+}
+
+/*!
+ Operator that allows to multiply all the elements of a rotation matrix
+ by a scalar.
+ */
+vpRotationMatrix vpRotationMatrix::operator*(double x) const
+{
+ vpRotationMatrix R;
+
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ R[i][j]= rowPtrs[i][j]*x;
+
+ return R;
+}
+
+/*!
+ Operator that allows to multiply all the elements of a rotation matrix
+ by a scalar.
+ */
+vpRotationMatrix & vpRotationMatrix::operator*=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ for(unsigned int j=0;j<colNum;j++)
+ rowPtrs[i][j]*=x;
+
+ return *this;
+}
+
+/*********************************************************************/
+
+/*!
+ Test if the rotation matrix is really a rotation matrix.
+*/
+bool
+vpRotationMatrix::isARotationMatrix() const
+{
+ unsigned int i,j ;
+ bool isRotation = true ;
+
+ // test R^TR = Id ;
+ vpRotationMatrix RtR = (*this).t()*(*this) ;
+ for (i=0 ; i < 3 ; i++) {
+ for (j=0 ; j < 3 ; j++) {
+ if (i==j) {
+ if (fabs(RtR[i][j]-1) > threshold) isRotation = false ;
+ }
+ else {
+ if (fabs(RtR[i][j]) > threshold) isRotation = false ;
+ }
+ }
+ }
+ // test if it is a basis
+ // test || Ci || = 1
+ for (i=0 ; i < 3 ; i++) {
+ if ((sqrt(vpMath::sqr(RtR[0][i]) +
+ vpMath::sqr(RtR[1][i]) +
+ vpMath::sqr(RtR[2][i])) - 1) > threshold) isRotation = false ;
+ }
+
+ // test || Ri || = 1
+ for (i=0 ; i < 3 ; i++) {
+ if ((sqrt(vpMath::sqr(RtR[i][0]) +
+ vpMath::sqr(RtR[i][1]) +
+ vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false ;
+ }
+
+ // test if the basis is orthogonal
+ return isRotation ;
+}
+
+
+/*!
+ Default constructor that initialise a 3-by-3 rotation matrix to identity.
+*/
+vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3,3)
+{
+ eye();
+}
+
+
+/*!
+ Copy contructor that construct a 3-by-3 rotation matrix from another rotation matrix.
+*/
+vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3,3)
+{
+ (*this) = M ;
+}
+/*!
+ Construct a 3-by-3 rotation matrix from an homogeneous matrix.
+*/
+vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3,3)
+{
+ buildFrom(M);
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}\f$ angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3,3)
+{
+ buildFrom(tu) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from a pose vector.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3,3)
+{
+ buildFrom(p) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ R(z,y,z) \f$ Euler angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3,3)
+{
+ buildFrom(euler) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ R(x,y,z) \f$ Euler angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3,3)
+{
+ buildFrom(Rxyz) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ R(z,y,x) \f$ Euler angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3,3)
+{
+ buildFrom(Rzyx) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3,3)
+{
+ buildFrom(tux, tuy, tuz) ;
+}
+
+/*!
+ Construct a 3-by-3 rotation matrix from quaternion angle representation.
+ */
+vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector& q) : vpArray2D<double>(3,3)
+{
+ buildFrom(q);
+}
+
+/*!
+ Return the rotation matrix transpose which is also the inverse of the rotation matrix.
+
+ \sa inverse()
+*/
+vpRotationMatrix
+vpRotationMatrix::t() const
+{
+ vpRotationMatrix Rt ;
+
+ unsigned int i,j;
+ for (i=0;i<3;i++)
+ for (j=0;j<3;j++)
+ Rt[j][i] = (*this)[i][j];
+
+ return Rt;
+}
+
+/*!
+ Return the rotation matrix inverse which is also the transpose of the rotation matrix.
+
+ \sa t()
+*/
+vpRotationMatrix vpRotationMatrix::inverse() const
+{
+ vpRotationMatrix Ri = (*this).t() ;
+
+ return Ri ;
+}
+
+/*!
+ Inverse the rotation matrix.
+
+ \param R (output): Inverted rotation matrix.
+
+ \code
+#include <visp3/core/vpRotationMatrix.h>
+
+int main()
+{
+ vpRotationMatrix R, Rinv;
+ // ... Update rotation matrix R
+ // Compute the inverse in Rinv
+ R.inverse(Rinv);
+}
+ \endcode
+*/
+void
+vpRotationMatrix::inverse(vpRotationMatrix &R) const
+{
+ R = inverse();
+}
+
+/*!
+ Print to std::cout the rotation matrix as a \f$ \theta {\bf u} \f$ angle representation vector.
+ */
+void
+vpRotationMatrix::printVector()
+{
+ vpThetaUVector tu(*this) ;
+
+ for (unsigned int i=0; i<3; i++)
+ std::cout << tu[i] << " " ;
+
+ std::cout << std::endl ;
+}
+
+
+/*!
+ Transform a \f$ \theta {\bf u}\f$ angle representation into a rotation matrix.
+
+ The rotation is computed using :
+ \f[
+ R = \cos{ \theta} \; {I}_{3} + (1 - \cos{ \theta}) \; u u^{T} + \sin{ \theta} \; [u]_\times
+ \f]
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpThetaUVector &v)
+{
+ unsigned int i,j;
+ double theta, si, co, sinc, mcosc;
+ vpRotationMatrix R;
+
+ theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
+ si = sin(theta);
+ co = cos(theta);
+ sinc = vpMath::sinc(si,theta);
+ mcosc = vpMath::mcosc(co,theta);
+
+ R[0][0] = co + mcosc*v[0]*v[0];
+ R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
+ R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
+ R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
+ R[1][1] = co + mcosc*v[1]*v[1];
+ R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
+ R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
+ R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
+ R[2][2] = co + mcosc*v[2]*v[2];
+
+ for (i=0;i<3;i++) for (j=0;j<3;j++) (*this)[i][j] = R[i][j];
+
+ return *this ;
+}
+
+/*!
+ Build a rotation matrix from an homogeneous matrix.
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
+{
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3; j++)
+ (*this)[i][j] = M[i][j] ;
+
+ return *this ;
+}
+
+/*!
+ Build a rotation matrix from a pose vector.
+
+ \sa buildFrom(const vpThetaUVector &)
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpPoseVector &p)
+{
+ vpThetaUVector tu(p);
+ return buildFrom(tu);
+}
+
+/*!
+ Transform a vector representing the Euler angle
+ into a rotation matrix.
+ Rzyz(\f$ \phi, \theta , \psi \f$) = Rot(\f$ z,\phi \f$) Rot(\f$ y,\theta \f$) Rot(\f$ z,\psi \f$)
+
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpRzyzVector &v)
+{
+ double c0,c1,c2,s0,s1,s2;
+
+ c0 = cos(v[0]);
+ c1 = cos(v[1]);
+ c2 = cos(v[2]);
+ s0 = sin(v[0]);
+ s1 = sin(v[1]);
+ s2 = sin(v[2]);
+
+ (*this)[0][0] = c0*c1*c2 - s0*s2;
+ (*this)[0][1] = -c0*c1*s2 - s0*c2;
+ (*this)[0][2] = c0*s1;
+ (*this)[1][0] = s0*c1*c2+c0*s2 ;
+ (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
+ (*this)[1][2] = s0*s1;
+ (*this)[2][0] = -s1*c2;
+ (*this)[2][1] = s1*s2;
+ (*this)[2][2] = c1;
+
+ return (*this) ;
+}
+
+
+/*!
+
+ Transform a vector representing the Rxyz angle into a rotation
+ matrix.
+ Rxyz(\f$ \phi,\theta, \psi \f$) = Rot(\f$ x, \psi \f$) Rot(\f$ y, \theta \f$
+ ) Rot(\f$ z,\phi \f$)
+
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpRxyzVector &v)
+{
+ double c0,c1,c2,s0,s1,s2;
+
+ c0 = cos(v[0]);
+ c1 = cos(v[1]);
+ c2 = cos(v[2]);
+ s0 = sin(v[0]);
+ s1 = sin(v[1]);
+ s2 = sin(v[2]);
+
+ (*this)[0][0] = c1*c2;
+ (*this)[0][1] = -c1*s2;
+ (*this)[0][2] = s1;
+ (*this)[1][0] = c0*s2+s0*s1*c2;
+ (*this)[1][1] = c0*c2-s0*s1*s2;
+ (*this)[1][2] = -s0*c1;
+ (*this)[2][0] = -c0*s1*c2+s0*s2;
+ (*this)[2][1] = c0*s1*s2+c2*s0;
+ (*this)[2][2] = c0*c1;
+
+ return (*this) ;
+}
+
+
+
+/*!
+ Transform a vector representing the Rzyx angle
+ into a rotation matrix.
+ Rxyz(\f$ \phi, \theta , \psi \f$) =
+ Rot(\f$ z, \psi \f$) Rot(\f$ y, \theta \f$)Rot(\f$ x, \phi \f$)
+*/
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpRzyxVector &v)
+{
+ double c0,c1,c2,s0,s1,s2;
+
+ c0 = cos(v[0]);
+ c1 = cos(v[1]);
+ c2 = cos(v[2]);
+ s0 = sin(v[0]);
+ s1 = sin(v[1]);
+ s2 = sin(v[2]);
+
+ (*this)[0][0] = c0*c1 ;
+ (*this)[0][1] = c0*s1*s2 - s0*c2 ;
+ (*this)[0][2] = c0*s1*c2 + s0*s2 ;
+
+ (*this)[1][0] = s0*c1 ;
+ (*this)[1][1] = s0*s1*s2 + c0*c2 ;
+ (*this)[1][2] = s0*s1*c2 - c0*s2 ;
+
+ (*this)[2][0] = -s1 ;
+ (*this)[2][1] = c1*s2 ;
+ (*this)[2][2] = c1*c2 ;
+
+ return (*this) ;
+}
+
+
+
+/*!
+ Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ angle representation.
+ */
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const double tux,
+ const double tuy,
+ const double tuz)
+{
+ vpThetaUVector tu(tux, tuy, tuz) ;
+ buildFrom(tu) ;
+ return *this ;
+}
+
+
+/*!
+ Construct a 3-by-3 rotation matrix from a quaternion representation.
+ */
+vpRotationMatrix
+vpRotationMatrix::buildFrom(const vpQuaternionVector& q){
+ double a = q.w();
+ double b = q.x();
+ double c = q.y();
+ double d = q.z();
+ (*this)[0][0] = a*a+b*b-c*c-d*d;
+ (*this)[0][1] = 2*b*c-2*a*d;
+ (*this)[0][2] = 2*a*c+2*b*d;
+
+ (*this)[1][0] = 2*a*d+2*b*c;
+ (*this)[1][1] = a*a-b*b+c*c-d*d;
+ (*this)[1][2] = 2*c*d-2*a*b;
+
+ (*this)[2][0] = 2*b*d-2*a*c;
+ (*this)[2][1] = 2*a*b+2*c*d;
+ (*this)[2][2] = a*a-b*b-c*c+d*d;
+ return *this;
+}
+
+/*!
+ Allow to multiply a scalar by a rotation matrix.
+*/
+vpRotationMatrix operator*(const double &x,const vpRotationMatrix &R)
+{
+ vpRotationMatrix C;
+
+ unsigned int Rrow = R.getRows() ;
+ unsigned int Rcol = R.getCols() ;
+
+ for (unsigned int i=0;i<Rrow;i++)
+ for(unsigned int j=0;j<Rcol;j++)
+ C[i][j]= R[i][j]*x;
+
+ return C ;
+}
+
+/*!
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation matrix.
+ */
+vpThetaUVector vpRotationMatrix::getThetaUVector()
+{
+ vpThetaUVector tu;
+ tu.buildFrom(*this);
+ return tu;
+}
+
+/*!
+ Extract a column vector from a rotation matrix.
+ \warning All the indexes start from 0 in this function.
+ \param j : Index of the column to extract. If j=0, the first column is extracted.
+ \return The extracted column vector.
+
+ The following example shows how to use this function:
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+int main()
+{
+ vpRotationMatrix R;
+
+ vpColVector r = R.getCol(2);
+ std::cout << "Last column: \n" << r << std::endl;
+}
+ \endcode
+It produces the following output:
+ \code
+Last column:
+0
+0
+1
+ \endcode
+ */
+vpColVector
+vpRotationMatrix::getCol(const unsigned int j) const
+{
+ if (j >= getCols())
+ throw(vpException(vpException::dimensionError,
+ "Unable to extract a column vector from the homogeneous matrix"));
+ unsigned int nb_rows = getRows();
+ vpColVector c(nb_rows);
+ for (unsigned int i=0 ; i < nb_rows ; i++)
+ c[i] = (*this)[i][j];
+ return c;
+}
+
diff --git a/modules/core/src/math/transformation/vpRotationVector.cpp b/modules/core/src/math/transformation/vpRotationVector.cpp
new file mode 100644
index 0000000..85bdb41
--- /dev/null
+++ b/modules/core/src/math/transformation/vpRotationVector.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic rotation vector (cannot be used as is !).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <algorithm>
+#include <math.h>
+
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRowVector.h>
+
+/*!
+ \file vpRotationVector.cpp
+ \brief Class that consider the case of a generic rotation vector
+ (cannot be used as is !).
+*/
+
+
+
+/*!
+ Return the transpose of the rotation vector.
+
+*/
+vpRowVector vpRotationVector::t() const
+{
+ vpRowVector v(dsize);
+
+ for (unsigned int i=0; i< dsize; i++)
+ v[i] = data[i];
+
+ return v;
+}
+
+/*!
+ Operator that allows to multiply each element of a rotation vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The rotation vector multiplied by the scalar as a column vector. The current
+ rotation vector (*this) is unchanged.
+
+*/
+vpColVector vpRotationVector::operator*(double x) const
+{
+ vpColVector v(dsize);
+
+ for (unsigned int i=0;i<dsize;i++)
+ v[i] = (*this)[i] * x;
+ return v;
+}
+
+/*!
+ \relates vpRotationVector
+ Allows to multiply a scalar by rotaion vector.
+*/
+vpColVector operator*(const double &x,const vpRotationVector &v)
+{
+ vpColVector vout ;
+ vout = v*x ;
+ return vout ;
+}
+
+/*!
+ Return the sum square of all the elements \f$r_{i}\f$ of the rotation vector r(m).
+
+ \return The value \f[\sum{i=0}^{m} r_i^{2}\f].
+ */
+double vpRotationVector::sumSquare() const
+{
+ double sum_square=0.0;
+ double x ;
+
+ for (unsigned int i=0;i<rowNum;i++) {
+ x=rowPtrs[i][0];
+ sum_square += x*x;
+ }
+
+ return sum_square;
+}
diff --git a/modules/core/src/math/transformation/vpRxyzVector.cpp b/modules/core/src/math/transformation/vpRxyzVector.cpp
new file mode 100644
index 0000000..c85a11a
--- /dev/null
+++ b/modules/core/src/math/transformation/vpRxyzVector.cpp
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rxyz angle parameterization for the rotation.
+ * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <math.h>
+
+#include <visp3/core/vpRxyzVector.h>
+
+/*!
+ \file vpRxyzVector.cpp
+ \brief class that consider the case of the Rxyz angle parameterization for the rotation :
+ Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
+*/
+
+/*! Default constructor that initialize all the 3 angles to zero. */
+vpRxyzVector::vpRxyzVector()
+ : vpRotationVector(3)
+{}
+
+/*! Copy constructor. */
+vpRxyzVector::vpRxyzVector(const vpRxyzVector &rxyz)
+ : vpRotationVector(rxyz)
+{}
+
+/*!
+ Constructor from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$x\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
+*/
+vpRxyzVector::vpRxyzVector(const double phi, const double theta, const double psi)
+ : vpRotationVector (3)
+{
+ buildFrom(phi, theta, psi);
+}
+
+/*!
+ Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles from a rotation matrix.
+ \param R : Rotation matrix used to initialize the Euler angles.
+*/
+vpRxyzVector::vpRxyzVector(const vpRotationMatrix& R)
+ : vpRotationVector (3)
+{
+ buildFrom(R) ;
+}
+
+/*!
+ Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector from a \f$\theta {\bf u}\f$ vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input to initialize the Euler angles.
+*/
+vpRxyzVector::vpRxyzVector(const vpThetaUVector& tu)
+ : vpRotationVector (3)
+{
+ buildFrom(tu) ;
+}
+
+/*!
+ Convert a rotation matrix into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector.
+
+ \param R : Rotation matrix used as input.
+ \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRxyzVector
+vpRxyzVector::buildFrom(const vpRotationMatrix& R)
+{
+ double COEF_MIN_ROT = 1e-6;
+ double phi ;
+
+ if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
+ else phi = atan2(-R[1][2], R[2][2]) ;
+
+ double si = sin(phi) ;
+ double co = cos(phi) ;
+ double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
+ double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);
+
+ buildFrom(phi, theta, psi);
+
+ return *this ;
+}
+
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$
+ Euler angles vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input.
+ \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRxyzVector
+vpRxyzVector::buildFrom(const vpThetaUVector& tu)
+{
+ vpRotationMatrix R ;
+ R.buildFrom(tu) ;
+ buildFrom(R) ;
+
+ return *this ;
+}
+
+/*!
+ Construction from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$x\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
+*/
+void
+vpRxyzVector::buildFrom(const double phi, const double theta, const double psi)
+{
+ data[0] = phi ;
+ data[1] = theta ;
+ data[2] = psi ;
+}
+
+/*!
+
+ Initialize each element of the vector to the same angle value \e v.
+
+ \param v : Angle value to set for each element of the vector.
+
+\code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRxyzVector.h>
+
+int main()
+{
+ vpRxyzVector v;
+
+ // Initialise the rotation vector
+ v = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
+}
+\endcode
+*/
+vpRxyzVector &vpRxyzVector::operator=(double v)
+{
+ for (unsigned int i=0; i< dsize; i++)
+ data[i] = v;
+
+ return *this;
+}
diff --git a/modules/core/src/math/transformation/vpRzyxVector.cpp b/modules/core/src/math/transformation/vpRzyxVector.cpp
new file mode 100644
index 0000000..0a629d1
--- /dev/null
+++ b/modules/core/src/math/transformation/vpRzyxVector.cpp
@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Rzyx angle parameterization for the rotation.
+ * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpRzyxVector.h>
+#include <math.h>
+/*!
+ \file vpRzyxVector.cpp
+ \brief class that consider the case of the Rzyx angle parameterization for the rotation :
+ Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta,Rot(x,psi)
+
+*/
+
+/*! Default constructor that initialize all the 3 angles to zero. */
+vpRzyxVector::vpRzyxVector()
+ : vpRotationVector(3)
+{}
+
+/*! Copy constructor. */
+vpRzyxVector::vpRzyxVector(const vpRzyxVector &rzyx)
+ : vpRotationVector(rzyx)
+{}
+
+/*!
+ Constructor from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$x\f$ axis.
+*/
+vpRzyxVector::vpRzyxVector(const double phi, const double theta, const double psi)
+ : vpRotationVector (3)
+{
+ buildFrom(phi, theta, psi);
+}
+
+/*!
+ Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
+ angles from a rotation matrix.
+ \param R : Rotation matrix used to initialize the Euler angles.
+*/
+vpRzyxVector::vpRzyxVector(const vpRotationMatrix& R)
+ : vpRotationVector (3)
+{
+ buildFrom(R) ;
+}
+
+/*!
+ Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector from a \f$\theta {\bf u}\f$ vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input to initialize the Euler angles.
+*/
+vpRzyxVector::vpRzyxVector(const vpThetaUVector& tu)
+ : vpRotationVector (3)
+{
+ buildFrom(tu) ;
+}
+
+/*!
+ Convert a rotation matrix into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector.
+
+ Source: R. Paul, Robot Manipulators: Mathematics, Programming, and Control.
+ MIT Press, 1981, p. 71
+
+ \param R : Rotation matrix used as input.
+ \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRzyxVector
+vpRzyxVector::buildFrom(const vpRotationMatrix& R)
+{
+ double nx = R[0][0];
+ double ny = R[1][0];
+
+ double phi = atan2(ny,nx) ;
+ double si = sin(phi) ;
+ double co = cos(phi) ;
+
+ double nz = R[2][0];
+ double theta = atan2(-nz, co*nx+si*ny) ;
+
+ double ax = R[0][2];
+ double ay = R[1][2];
+ double ox = R[0][1];
+ double oy = R[1][1];
+
+ double psi = atan2(si*ax-co*ay,-si*ox+co*oy);
+
+ buildFrom(phi, theta, psi);
+
+ return *this ;
+}
+
+
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$
+ Euler angles vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input.
+ \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRzyxVector
+vpRzyxVector::buildFrom(const vpThetaUVector& tu)
+{
+ vpRotationMatrix R ;
+ R.buildFrom(tu) ;
+ buildFrom(R) ;
+
+ return *this ;
+}
+
+/*!
+ Construction from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$x\f$ axis.
+*/
+void
+vpRzyxVector::buildFrom(const double phi, const double theta, const double psi)
+{
+ data[0] = phi ;
+ data[1] = theta ;
+ data[2] = psi ;
+}
+
+/*!
+
+ Initialize each element of the vector to the same angle value \e v.
+
+ \param v : Angle value to set for each element of the vector.
+
+\code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRzyxVector.h>
+
+int main()
+{
+ vpRzyxVector v;
+
+ // Initialise the rotation vector
+ v = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
+}
+\endcode
+*/
+vpRzyxVector &vpRzyxVector::operator=(double v)
+{
+ for (unsigned int i=0; i< dsize; i++)
+ data[i] = v;
+
+ return *this;
+}
diff --git a/modules/core/src/math/transformation/vpRzyzVector.cpp b/modules/core/src/math/transformation/vpRzyzVector.cpp
new file mode 100644
index 0000000..a57623f
--- /dev/null
+++ b/modules/core/src/math/transformation/vpRzyzVector.cpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Euler angles parameterization for the rotation.
+ * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpRzyzVector.h>
+#include <math.h>
+
+/*!
+ \file vpRzyzVector.cpp
+ \brief class that consider the case of the Rzyz angle parameterization for the rotation :
+ Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
+*/
+
+/*! Default constructor that initialize all the 3 angles to zero. */
+vpRzyzVector::vpRzyzVector()
+ : vpRotationVector (3)
+{}
+/*! Copy constructor. */
+vpRzyzVector::vpRzyzVector(const vpRzyzVector &rzyz)
+ : vpRotationVector(rzyz)
+{}
+
+/*!
+ Constructor from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
+*/
+vpRzyzVector::vpRzyzVector(const double phi, const double theta, const double psi)
+ : vpRotationVector (3)
+{
+ buildFrom(phi, theta, psi);
+}
+
+/*!
+ Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles from a rotation matrix.
+ \param R : Rotation matrix used to initialize the Euler angles.
+*/
+vpRzyzVector::vpRzyzVector(const vpRotationMatrix& R)
+ : vpRotationVector (3)
+{
+ buildFrom(R) ;
+}
+
+/*!
+ Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector from a \f$\theta {\bf u}\f$ vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input to initialize the Euler angles.
+*/
+vpRzyzVector::vpRzyzVector(const vpThetaUVector& tu)
+ : vpRotationVector (3)
+{
+ buildFrom(tu) ;
+}
+
+/*!
+ Convert a rotation matrix into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
+ angles vector.
+
+ \param R : Rotation matrix used as input.
+ \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRzyzVector
+vpRzyzVector::buildFrom(const vpRotationMatrix& R)
+{
+ double phi ;
+ if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
+ phi = 0 ;
+ else
+ phi = atan2(R[1][2],R[0][2]) ;
+ double cphi = cos(phi) ;
+ double sphi = sin(phi) ;
+
+ double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);
+
+ double psi = atan2(-sphi*R[0][0]+cphi*R[1][0],-sphi*R[0][1]+cphi*R[1][1]) ;
+
+ buildFrom(phi, theta, psi);
+
+ return *this ;
+}
+
+
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
+ Euler angles vector.
+ \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
+ input.
+ \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+*/
+vpRzyzVector
+vpRzyzVector::buildFrom(const vpThetaUVector& tu)
+{
+ vpRotationMatrix R ;
+ R.buildFrom(tu) ;
+ buildFrom(R) ;
+
+ return *this ;
+}
+
+/*!
+
+ Initialize each element of the vector to the same angle value \e v.
+
+ \param v : Angle value to set for each element of the vector.
+
+\code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRzyzVector.h>
+
+int main()
+{
+ vpRzyzVector r;
+
+ // Initialise the rotation vector
+ r = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
+}
+\endcode
+*/
+vpRzyzVector &vpRzyzVector::operator=(double v)
+{
+ for (unsigned int i=0; i< dsize; i++)
+ data[i] = v;
+
+ return *this;
+}
+
+/*!
+ Construction from 3 angles (in radian).
+ \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
+ \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
+ \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
+*/
+void
+vpRzyzVector::buildFrom(const double phi, const double theta, const double psi)
+{
+ data[0] = phi ;
+ data[1] = theta ;
+ data[2] = psi ;
+}
diff --git a/modules/core/src/math/transformation/vpThetaUVector.cpp b/modules/core/src/math/transformation/vpThetaUVector.cpp
new file mode 100644
index 0000000..3a78e3b
--- /dev/null
+++ b/modules/core/src/math/transformation/vpThetaUVector.cpp
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Theta U parameterization for the rotation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+\file vpThetaUVector.cpp
+\brief class that consider the case of the Theta U parameterization for the
+rotation
+*/
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#include <visp3/core/vpThetaUVector.h>
+
+const double vpThetaUVector::minimum = 0.0001;
+
+/*! Default constructor that initialize all the 3 angles to zero. */
+vpThetaUVector::vpThetaUVector()
+ : vpRotationVector(3)
+{}
+/*! Copy constructor. */
+vpThetaUVector::vpThetaUVector(const vpThetaUVector &tu)
+ : vpRotationVector(tu)
+{}
+
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from an homogeneous matrix.
+*/
+vpThetaUVector::vpThetaUVector(const vpHomogeneousMatrix& M)
+ : vpRotationVector(3)
+{
+ buildFrom(M) ;
+}
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from a pose vector.
+*/
+vpThetaUVector::vpThetaUVector(const vpPoseVector& p)
+ : vpRotationVector(3)
+{
+ buildFrom(p) ;
+}
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from a rotation matrix.
+*/
+vpThetaUVector::vpThetaUVector(const vpRotationMatrix& R)
+ : vpRotationVector(3)
+{
+ buildFrom(R) ;
+}
+
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
+representation vector.
+*/
+vpThetaUVector::vpThetaUVector(const vpRzyxVector& rzyx)
+ : vpRotationVector(3)
+{
+ buildFrom(rzyx) ;
+}
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from an Euler z-y-z
+representation vector.
+*/
+vpThetaUVector::vpThetaUVector(const vpRzyzVector& rzyz)
+ : vpRotationVector(3)
+{
+ buildFrom(rzyz) ;
+}
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
+representation vector.
+*/
+vpThetaUVector::vpThetaUVector(const vpRxyzVector& rxyz)
+ : vpRotationVector(3)
+{
+ buildFrom(rxyz) ;
+}
+/*!
+Initialize a \f$\theta {\bf u}\f$ vector from a quaternion
+representation vector.
+*/
+vpThetaUVector::vpThetaUVector(const vpQuaternionVector& q)
+ : vpRotationVector(4)
+{
+ buildFrom(q) ;
+}
+
+/*!
+ Build a \f$\theta {\bf u}\f$ vector from 3 angles in radian.
+*/
+vpThetaUVector::vpThetaUVector(const double tux, const double tuy, const double tuz)
+ : vpRotationVector (3)
+{
+ buildFrom(tux, tuy, tuz);
+}
+
+/*!
+Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
+{
+ vpRotationMatrix R;
+
+ M.extract(R);
+ buildFrom(R);
+
+ return *this ;
+}
+/*!
+Converts a pose vector into a \f$\theta {\bf u}\f$ vector copying
+the \f$\theta {\bf u}\f$ values contained in the pose vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpPoseVector& p)
+{
+ for(unsigned int i=0; i<3; i++)
+ data[i] = p[i+3];
+
+ return *this ;
+}
+
+/*!
+Converts a rotation matrix into a \f$\theta {\bf u}\f$ vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpRotationMatrix& R)
+{
+ double s,c,theta,sinc;
+
+ s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
+ + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
+ + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
+ s = sqrt(s)/2.0;
+ c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
+ theta=atan2(s,c); /* theta in [0, PI] since s > 0 */
+
+ // General case when theta != pi. If theta=pi, c=-1
+ if ( (1+c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
+ {
+ sinc = vpMath::sinc(s,theta);
+
+ data[0] = (R[2][1]-R[1][2])/(2*sinc);
+ data[1] = (R[0][2]-R[2][0])/(2*sinc);
+ data[2] = (R[1][0]-R[0][1])/(2*sinc);
+ }
+ else /* theta near PI */
+ {
+ if ( (R[0][0]-c) < std::numeric_limits<double>::epsilon() )
+ data[0] = 0.;
+ else
+ data[0] = theta*(sqrt((R[0][0]-c)/(1-c)));
+ if ((R[2][1]-R[1][2]) < 0) data[0] = -data[0];
+
+ if ( (R[1][1]-c) < std::numeric_limits<double>::epsilon() )
+ data[1] = 0.;
+ else
+ data[1] = theta*(sqrt((R[1][1]-c)/(1-c)));
+
+ if ((R[0][2]-R[2][0]) < 0) data[1] = -data[1];
+
+ if ( (R[2][2]-c) < std::numeric_limits<double>::epsilon() )
+ data[2] = 0.;
+ else
+ data[2] = theta*(sqrt((R[2][2]-c)/(1-c)));
+
+ if ((R[1][0]-R[0][1]) < 0) data[2] = -data[2];
+ }
+
+ return *this ;
+}
+/*!
+Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
+representation vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpRzyxVector& rzyx)
+{
+ vpRotationMatrix R(rzyx) ;
+
+ buildFrom(R) ;
+ return *this ;
+}
+/*!
+Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-z
+representation vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpRzyzVector& rzyz)
+{
+ vpRotationMatrix R(rzyz) ;
+
+ buildFrom(R) ;
+ return *this ;
+}
+/*!
+Build a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
+representation vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpRxyzVector& rxyz)
+{
+ vpRotationMatrix R(rxyz) ;
+
+ buildFrom(R) ;
+ return *this ;
+}
+
+/*!
+Build a \f$\theta {\bf u}\f$ vector from a quaternion
+representation vector.
+*/
+vpThetaUVector
+vpThetaUVector::buildFrom(const vpQuaternionVector& q)
+{
+ vpRotationMatrix R(q) ;
+
+ buildFrom(R) ;
+ return *this ;
+}
+
+/*!
+
+ Initialize each element of the \f$\theta {\bf u}\f$ vector to the
+ same angle value \e v.
+
+ \param v : Angle value to set for each element of the \f$\theta {\bf
+ u}\f$ vector.
+
+\code
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpThetaUVector.h>
+
+int main()
+{
+ vpThetaUVector tu;
+
+ // Initialise the theta U rotation vector
+ tu = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
+}
+\endcode
+*/
+vpThetaUVector &vpThetaUVector::operator=(double v)
+{
+ for (unsigned int i=0; i< dsize; i++)
+ data[i] = v;
+
+ return *this;
+}
+
+/*!
+
+Extract the rotation angle \f$ \theta \f$ and the unit vector
+\f$\bf u \f$ from the \f$ \theta {\bf u} \f$ representation.
+
+\param theta : Rotation angle \f$ \theta \f$.
+
+\param u : Unit vector \f${\bf u} = (u_{x},u_{y},u_{z})^{\top} \f$
+representing the rotation axis.
+
+*/
+void
+vpThetaUVector::extract(double &theta, vpColVector &u) const
+{
+ u.resize(3);
+
+ theta = sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]);
+ //if (theta == 0) {
+ if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
+ u = 0;
+ return;
+ }
+ for (unsigned int i=0 ; i < 3 ; i++)
+ u[i] = data[i] / theta ;
+}
+
+/*!
+ Build a \f$\theta {\bf u}\f$ vector from 3 angles in radian.
+*/
+void
+vpThetaUVector::buildFrom(const double tux, const double tuy, const double tuz)
+{
+ data[0] = tux;
+ data[1] = tuy;
+ data[2] = tuz;
+}
diff --git a/modules/core/src/math/transformation/vpTranslationVector.cpp b/modules/core/src/math/transformation/vpTranslationVector.cpp
new file mode 100644
index 0000000..234283c
--- /dev/null
+++ b/modules/core/src/math/transformation/vpTranslationVector.cpp
@@ -0,0 +1,543 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Translation vector.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <stdio.h>
+#include <string.h>
+
+#include <visp3/core/vpTranslationVector.h>
+
+/*!
+ \file vpTranslationVector.cpp
+ \brief Class that consider the case of a translation vector.
+*/
+
+/*!
+ Construct a translation vector \f$ \bf t \f$ from 3 doubles.
+
+ \param tx,ty,tz : Translation respectively along x, y and z axis. Values are in meters.
+
+*/
+vpTranslationVector::vpTranslationVector(const double tx, const double ty, const double tz)
+ : vpArray2D<double>(3, 1)
+{
+ (*this)[0] = tx;
+ (*this)[1] = ty;
+ (*this)[2] = tz;
+}
+
+/*!
+ Construct a translation vector \f$ \bf t \f$ from the translation contained in
+ an homogeneous matrix.
+
+ \param M : Homogeneous matrix where translations are in meters.
+
+*/
+vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M)
+ : vpArray2D<double>(3, 1)
+{
+ M.extract( *this );
+}
+
+/*!
+ Construct a translation vector \f$ \bf t \f$ from the translation contained in
+ a pose vector.
+
+ \param p : Pose vector where translations are in meters.
+
+*/
+vpTranslationVector::vpTranslationVector(const vpPoseVector &p)
+ : vpArray2D<double>(3, 1)
+{
+ (*this)[0] = p[0];
+ (*this)[1] = p[1];
+ (*this)[2] = p[2];
+}
+
+/*!
+ Copy constructor.
+
+ \param tv : Translation vector to copy.
+
+ \code
+ vpTranslationVector t1(1,2,3); // Create and initialize a translation vector
+
+ vpTranslationVector t2(t1); // t2 is now a copy of t1
+ \endcode
+
+*/
+vpTranslationVector::vpTranslationVector (const vpTranslationVector &tv)
+ : vpArray2D<double>(tv)
+{
+}
+
+/*!
+ Build a 3 dimension translation vector \f$ \bf t\f$ from
+ an homogeneous matrix \f$ \bf M \f$.
+
+ \param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
+ \bf t \f$ and \f$\theta \bf u \f$ vectors are extracted to initialize
+ the pose vector.
+
+ \return The build pose vector.
+
+*/
+vpTranslationVector
+vpTranslationVector::buildFrom(const vpHomogeneousMatrix& M)
+{
+ M.extract( *this );
+ return *this ;
+}
+
+/*!
+ Build a 3 dimension translation vector \f$ \bf t\f$ from
+ the translation contained in a pose vector.
+
+ \param p : Pose vector where translations are in meters.
+
+ \return The build pose vector.
+
+*/
+vpTranslationVector
+vpTranslationVector::buildFrom(const vpPoseVector& p)
+{
+ (*this)[0] = p[0];
+ (*this)[1] = p[1];
+ (*this)[2] = p[2];
+ return *this ;
+}
+
+/*!
+ Build a 3 dimension translation vector \f$ \bf t\f$ from 3 doubles.
+
+ \param tx,ty,tz : Translation respectively along x, y and z axis in meter.
+
+ \return The build pose vector.
+ \sa set()
+*/
+vpTranslationVector
+vpTranslationVector::buildFrom(const double tx, const double ty, const double tz)
+{
+ set(tx, ty, tz);
+ return *this ;
+}
+
+/*!
+ Initialize a translation vector from 3 doubles.
+
+ \param tx,ty,tz : Translation respectively along x, y and z axis in meter.
+
+*/
+void
+vpTranslationVector::set(const double tx, const double ty, const double tz)
+{
+ (*this)[0] = tx ;
+ (*this)[1] = ty ;
+ (*this)[2] = tz ;
+}
+
+/*!
+ Operator that allows to add two translation vectors.
+
+ \param tv : Translation vector to add.
+
+ \return The sum of the current translation vector (*this) and the one to add.
+ \code
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2(4,5,6);
+ vpTranslationVector t3;
+
+ t3 = t2 + t1;
+ // t1 and t2 leave unchanged
+ // t3 is now equal to : 5, 7, 9
+ \endcode
+
+*/
+vpTranslationVector
+vpTranslationVector::operator+(const vpTranslationVector &tv) const
+{
+ vpTranslationVector s;
+
+ for (unsigned int i=0;i<3;i++) s[i] = (*this)[i]+tv[i] ;
+
+ return s;
+}
+
+/*!
+ Operator that allows to substract two translation vectors.
+
+ \param tv : Translation vector to substract.
+
+ \return The substraction of the current translation vector (*this) and the one to substract.
+ \code
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2(4,5,6);
+ vpTranslationVector t3;
+
+ t3 = t2 - t1;
+ // t1 and t2 leave unchanged
+ // t3 is now equal to : 3, 3, 3
+ \endcode
+
+*/
+vpTranslationVector
+vpTranslationVector::operator-(const vpTranslationVector &tv) const
+{
+ vpTranslationVector sub ;
+
+ for (unsigned int i=0;i<3;i++) sub[i] = (*this)[i]-tv[i] ;
+
+ return sub;
+}
+
+
+/*!
+ Operator that allows to negate a translation vector.
+
+ \return The negate translation. The current translation vector
+ (*this) is unchanged.
+
+ \code
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2;
+ t2 = -t1;
+ // t1 is unchanged
+ // t2 is now equal to : [-1, -2, -3]
+ \endcode
+*/
+vpTranslationVector vpTranslationVector::operator-() const //negate
+{
+ vpTranslationVector tv;
+ for (unsigned int i=0;i<dsize;i++)
+ {
+ *(tv.data + i) = -*(data + i) ;
+ }
+
+ return tv;
+}
+
+/*!
+ Operator that allows to multiply each element of a translation vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The translation vector multiplied by the scalar. The current
+ translation vector (*this) is unchanged.
+
+ \code
+ vpTranslationVector t1(1,2,3);
+ t2 = t1 * 3;
+ // t1 is unchanged
+ // t2 is now equal to : [3, 6, 9]
+ \endcode
+*/
+vpTranslationVector vpTranslationVector::operator*(const double x) const
+{
+ vpTranslationVector tv;
+ for (unsigned int i=0;i<dsize;i++) {
+ *(tv.data + i) = (*(data + i)) * x ;
+ }
+
+ return tv;
+}
+
+/*!
+
+ Multiply a 3-by-1 dimension translation vector by a 1-by-n row vector.
+
+ \param v : Row vector.
+
+ \return The resulting matrix that is 3-by-n dimension.
+
+*/
+vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const
+{
+ vpMatrix M(rowNum, v.getCols());
+ for (unsigned int i=0; i<rowNum; i++) {
+ for (unsigned int j=0; j<v.getCols(); j++) {
+ M[i][j] = (*this)[i] * v[j];
+ }
+ }
+ return M;
+}
+
+/*!
+ Operator that allows to multiply each element of a translation vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The translation vector multiplied by the scalar.
+*/
+vpTranslationVector &vpTranslationVector::operator*=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] *= x;
+ return (*this);
+}
+/*!
+ Operator that allows to divide each element of a translation vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The column vector divided by the scalar.
+*/
+vpTranslationVector &vpTranslationVector::operator/=(double x)
+{
+ for (unsigned int i=0;i<rowNum;i++)
+ (*this)[i] /= x;
+ return (*this);
+}
+
+/*!
+ Operator that allows to divide each element of a translation vector by a scalar.
+
+ \param x : The scalar.
+
+ \return The translation vector divided by the scalar. The current
+ translation vector (*this) is unchanged.
+
+ \code
+ vpTranslationVector t1(8,4,2);
+ t2 = t1 / 2;
+ // t1 is unchanged
+ // t2 is now equal to : [4, 2, 1]
+ \endcode
+*/
+vpTranslationVector vpTranslationVector::operator/(const double x) const
+{
+ vpTranslationVector tv;
+ for (unsigned int i=0;i<dsize;i++) {
+ *(tv.data + i) = (*(data + i)) / x ;
+ }
+
+ return tv;
+}
+
+/*!
+ Copy operator.
+ \param tv : Translation vector to copy
+ \return A copy of \e tv.
+
+ \code
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2;
+ t2 = t1;
+ // t1 is unchanged
+ // t2 is now equal to t1 : 1, 2, 3
+ \endcode
+*/
+vpTranslationVector &vpTranslationVector::operator=(const vpTranslationVector &tv)
+{
+ unsigned int k = tv.rowNum ;
+ if (rowNum != k){
+ try {
+ resize(k, 1);
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ }
+
+ memcpy(data, tv.data, rowNum*sizeof(double)) ;
+
+ return *this;
+}
+
+/*!
+ Initialize each element of a translation vector to the same value x.
+
+ \param x : Value to set for each element of the translation vector.
+
+ \code
+ vpTranslationVector t;
+ t = 3;
+ // Here t is set to 3,3,3
+ \endcode
+*/
+vpTranslationVector & vpTranslationVector::operator=(double x)
+{
+ double *d = data ;
+
+ for (int i=0;i<3;i++)
+ *(d++)= x ;
+
+ return *this;
+}
+
+/*!
+ Compute the skew symmetric matrix \f$M\f$ of translation vector \e tv.
+
+ \f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
+ \end{array}\right), \quad \mbox{then} \qquad
+ M = \left( \begin{array}{ccc}
+ 0 & -t_z & t_y \\
+ t_z & 0 & -t_x \\
+ -t_y & t_x & 0
+ \end{array}\right)
+ \f]
+
+ \param tv : Translation vector in input used to compute the skew symmetric
+ matrix M.
+
+ \param M : Skew symmetric matrix of translation vector \f$t\f$.
+*/
+void
+vpTranslationVector::skew(const vpTranslationVector &tv, vpMatrix &M)
+{
+ M.resize(3,3) ;
+ M[0][0] = 0 ; M[0][1] = -tv[2] ; M[0][2] = tv[1] ;
+ M[1][0] = tv[2] ; M[1][1] = 0 ; M[1][2] = -tv[0] ;
+ M[2][0] = -tv[1]; M[2][1] = tv[0] ; M[2][2] = 0 ;
+}
+
+/*!
+
+ Compute the skew symmetric matrix \f$M\f$ of translation vector \e tv.
+
+ \f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
+ \end{array}\right), \quad \mbox{then} \qquad
+ M = \left( \begin{array}{ccc}
+ 0 & -t_z & t_y \\
+ t_z & 0 & -t_x \\
+ -t_y & t_x & 0
+ \end{array}\right)
+ \f]
+
+ \param tv : Translation vector in input.
+
+ \return Skew symmetric matrix \f$M\f$ of translation vector \f$t\f$.
+
+*/
+vpMatrix
+vpTranslationVector::skew(const vpTranslationVector &tv)
+{
+ vpMatrix M(3, 3);
+ skew(tv,M);
+ return M;
+}
+
+/*!
+
+ Compute the skew symmetric matrix \f$M\f$ of the translation vector (matrice
+ de pre-produit vectoriel), where
+
+ \f[ M = \left( \begin{array}{ccc}
+ 0 & -t_z & t_y \\
+ t_z & 0 & -t_x \\
+ -t_y & t_x & 0
+ \end{array}\right)
+ \f]
+
+ and where \f$(t_x,t_y,t_z)\f$ are the coordinates of the translation
+ vector.
+
+ \return Skew symmetric matrix \f$M\f$ of the translation vector.
+
+*/
+vpMatrix
+vpTranslationVector::skew() const
+{
+ vpMatrix M(3, 3);
+ skew(*this,M);
+ return M;
+}
+
+
+/*!
+
+ Return the cross product of two translation vectors \f$a \times b\f$.
+
+ \param a,b : Translation vectors in input.
+
+ \return The cross product of two translation vectors \f$a \times
+ b\f$.
+*/
+vpTranslationVector
+vpTranslationVector::cross(const vpTranslationVector &a,
+ const vpTranslationVector &b)
+{
+ vpMatrix skew_a = vpTranslationVector::skew(a) ;
+ return (vpTranslationVector)(skew_a * b);
+}
+
+/*!
+ Transpose the translation vector. The resulting vector becomes a row vector.
+*/
+vpRowVector vpTranslationVector::t() const
+{
+ vpRowVector v(rowNum);
+ memcpy(v.data, data, rowNum*sizeof(double)) ;
+ return v;
+}
+
+/*!
+ Compute and return the Euclidean norm of the translation vector
+ \f$ ||x|| = \sqrt{ \sum{t_{i}^2}} \f$.
+
+ \return The Euclidean norm if the vector is initialized, 0 otherwise.
+
+*/
+double vpTranslationVector::euclideanNorm() const
+{
+ double norm=0.0;
+ double x ;
+ for (unsigned int i=0;i<dsize;i++) {
+ x = *(data +i); norm += x*x;
+ }
+
+ return sqrt(norm);
+}
+
+/*!
+ Return the sum square of all the elements \f$t_{i}\f$ of the translation vector t(m).
+
+ \return The value \f[\sum{i=0}^{m} t_i^{2}\f].
+ */
+double vpTranslationVector::sumSquare() const
+{
+ double sum_square=0.0;
+ double x ;
+
+ for (unsigned int i=0;i<rowNum;i++) {
+ x=rowPtrs[i][0];
+ sum_square += x*x;
+ }
+
+ return sum_square;
+}
diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp
new file mode 100644
index 0000000..f12951e
--- /dev/null
+++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp
@@ -0,0 +1,530 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Velocity twist transformation matrix.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <sstream>
+#include <assert.h>
+
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpException.h>
+
+
+/*!
+ \file vpVelocityTwistMatrix.cpp
+
+ \brief Definition of the vpVelocityTwistMatrix. Class that consider the
+ particular case of twist transformation matrix that allows to
+ transform a velocity skew from one frame to an other.
+*/
+
+
+/*!
+ Copy operator that allow to set a velocity twist matrix from an other one.
+
+ \param V : Velocity twist matrix to copy.
+*/
+vpVelocityTwistMatrix &
+vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
+{
+ for (int i=0; i<6; i++)
+ {
+ for (int j=0; j<6; j++)
+ {
+ rowPtrs[i][j] = V.rowPtrs[i][j];
+ }
+ }
+
+ return *this;
+}
+
+
+/*!
+ Initialize a 6x6 velocity twist matrix as identity.
+*/
+void
+vpVelocityTwistMatrix::eye()
+{
+ for (unsigned int i=0 ; i < 6 ; i++)
+ for (unsigned int j=0 ; j < 6; j++)
+ if (i==j)
+ (*this)[i][j] = 1.0 ;
+ else
+ (*this)[i][j] = 0.0;
+}
+
+/*!
+ Initialize a velocity twist transformation matrix as identity.
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix()
+ : vpArray2D<double>(6, 6)
+{
+ eye() ;
+}
+
+/*!
+ Initialize a velocity twist transformation matrix from another velocity twist matrix.
+
+ \param V : Velocity twist matrix used as initializer.
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V)
+ : vpArray2D<double>(6, 6)
+{
+ *this = V;
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from an homogeneous matrix
+ \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
+ \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
+
+ \param M : Homogeneous matrix \f$M\f$ used to initialize the velocity twist
+ transformation matrix.
+
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M)
+ : vpArray2D<double>(6, 6)
+{
+ buildFrom(M);
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from a translation vector
+ \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \param t : Translation vector.
+
+ \param thetau : \f$\theta u\f$ rotation vector.
+
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
+ const vpThetaUVector &thetau)
+ : vpArray2D<double>(6, 6)
+{
+ buildFrom(t, thetau) ;
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from a translation vector
+ \e t and a rotation matrix M.
+
+ \param t : Translation vector.
+
+ \param R : Rotation matrix.
+
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+ : vpArray2D<double>(6, 6)
+{
+ buildFrom(t,R) ;
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from a translation vector
+ \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$
+ parametrization.
+
+ \param tx,ty,tz : Translation vector in meters.
+
+ \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx,
+ const double ty,
+ const double tz,
+ const double tux,
+ const double tuy,
+ const double tuz)
+ : vpArray2D<double>(6, 6)
+{
+ vpTranslationVector T(tx,ty,tz) ;
+ vpThetaUVector tu(tux,tuy,tuz) ;
+ buildFrom(T,tu) ;
+}
+
+/*!
+
+ Set the velocity twist transformation matrix to identity.
+
+*/
+void
+vpVelocityTwistMatrix::setIdentity()
+{
+ eye() ;
+}
+
+
+/*!
+
+ Operator that allows to multiply a velocity twist transformation matrix by an
+ other velocity twist transformation matrix.
+
+*/
+vpVelocityTwistMatrix
+vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const
+{
+ vpVelocityTwistMatrix p ;
+
+ for (unsigned int i=0;i<6;i++)
+ for (unsigned int j=0;j<6;j++)
+ {
+ double s =0 ;
+ for (int k=0;k<6;k++)
+ s +=rowPtrs[i][k] * V.rowPtrs[k][j];
+ p[i][j] = s ;
+ }
+ return p;
+}
+
+/*!
+ Operator that allows to multiply a velocity twist transformation matrix by a matrix.
+
+ As shown in the example below, this operator can be used to compute the corresponding
+ camera velocity skew from the joint velocities knowing the robot jacobian.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+int main()
+{
+ vpSimulatorCamera robot;
+
+ vpColVector q_vel(6); // Joint velocity on the 6 joints
+ // ... q_vel need here to be initialized
+
+ vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
+
+ vpVelocityTwistMatrix cVe; // Velocity skew transformation from camera frame to end-effector
+ robot.get_cVe(cVe);
+
+ vpMatrix eJe; // Robot jacobian
+ robot.get_eJe(eJe);
+
+ // Compute the velocity in the camera frame
+ c_v = cVe * eJe * q_vel;
+
+ return 0;
+}
+ \endcode
+
+ \exception vpException::dimensionError If M is not a 6 rows dimension matrix.
+*/
+vpMatrix
+vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
+{
+ if (6 != M.getRows()) {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
+ M.getRows(), M.getCols()));
+ }
+
+ vpMatrix p(6, M.getCols()) ;
+ for (unsigned int i=0;i<6;i++)
+ for (unsigned int j=0;j<M.getCols();j++)
+ {
+ double s =0 ;
+ for (unsigned int k=0;k<6;k++)
+ s += rowPtrs[i][k] * M[k][j];
+ p[i][j] = s ;
+ }
+ return p;
+}
+
+/*!
+
+ Operator that allows to multiply a twist transformation matrix by a
+ 6-dimension column vector.
+
+ \param v : Velocity skew vector.
+
+ \exception vpException::dimensionError If v is not a 6 dimension column vector.
+
+*/
+vpColVector
+vpVelocityTwistMatrix::operator*(const vpColVector &v) const
+{
+ vpColVector c(6);
+
+ if (6 != v.getRows())
+ {
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (6x6) velocity twist matrix by a (%d) column vector",
+ v.getRows()));
+ }
+
+ c = 0.0;
+
+ for (unsigned int i=0;i<6;i++) {
+ for (unsigned int j=0;j<6;j++) {
+ {
+ c[i]+=rowPtrs[i][j] * v[j];
+ }
+ }
+ }
+
+ return c ;
+}
+
+
+/*!
+
+ Build a velocity twist transformation matrix from a translation vector
+ \e t and a rotation matrix M.
+
+ \param t : Translation vector.
+
+ \param R : Rotation matrix.
+
+*/
+vpVelocityTwistMatrix
+vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
+ const vpRotationMatrix &R)
+{
+ unsigned int i, j;
+ vpMatrix skewaR = t.skew(t)*R ;
+
+ for (i=0 ; i < 3 ; i++)
+ for (j=0 ; j < 3 ; j++)
+ {
+ (*this)[i][j] = R[i][j] ;
+ (*this)[i+3][j+3] = R[i][j] ;
+ (*this)[i][j+3] = skewaR[i][j] ;
+
+ }
+ return (*this) ;
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from a translation vector
+ \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \param t : Translation vector.
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector.
+
+*/
+vpVelocityTwistMatrix
+vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
+ const vpThetaUVector &thetau)
+{
+ vpRotationMatrix R ;
+ R.buildFrom(thetau) ;
+ buildFrom(t,R) ;
+ return (*this) ;
+}
+
+
+/*!
+
+ Initialize a velocity twist transformation matrix from an homogeneous matrix
+ \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
+ \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
+
+ \param M : Homogeneous matrix \f$M\f$ used to initialize the twist
+ transformation matrix.
+
+*/
+vpVelocityTwistMatrix
+vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
+{
+ vpTranslationVector tv ;
+ vpRotationMatrix R ;
+ M.extract(R) ;
+ M.extract(tv) ;
+
+ buildFrom(tv, R) ;
+ return (*this) ;
+}
+
+
+//! Invert the velocity twist matrix.
+vpVelocityTwistMatrix
+vpVelocityTwistMatrix::inverse() const
+{
+ vpVelocityTwistMatrix Wi;
+ vpRotationMatrix R;extract(R);
+ vpTranslationVector T;extract(T);
+ vpTranslationVector RtT ; RtT = -(R.t()*T) ;
+
+ Wi.buildFrom(RtT,R.t());
+
+ return Wi ;
+}
+
+
+//! Invert the velocity twist matrix.
+void
+vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const
+{
+ V = inverse();
+}
+
+//! Extract the rotation matrix from the velocity twist matrix.
+void
+vpVelocityTwistMatrix::extract( vpRotationMatrix &R) const
+{
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3; j++)
+ R[i][j] = (*this)[i][j];
+}
+
+//! Extract the translation vector from the velocity twist matrix.
+void
+vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
+{
+ vpRotationMatrix R;extract(R);
+ vpMatrix skTR(3,3);
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3; j++)
+ skTR[i][j] = (*this)[i][j+3];
+
+ vpMatrix skT = skTR*R.t();
+ tv[0] = skT[2][1];
+ tv[1] = skT[0][2];
+ tv[2] = skT[1][0];
+}
+
+/*!
+
+ Pretty print a velocity twist matrix. The data are tabulated.
+ The common widths before and after the decimal point
+ are set with respect to the parameter maxlen.
+
+ \param s Stream used for the printing.
+
+ \param length The suggested width of each matrix element.
+ The actual width grows in order to accomodate the whole integral part,
+ and shrinks if the whole extent is not needed for all the numbers.
+ \param intro The introduction which is printed before the matrix.
+ Can be set to zero (or omitted), in which case
+ the introduction is not printed.
+
+ \return Returns the common total width for all matrix elements
+
+ \sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
+*/
+int
+vpVelocityTwistMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
+{
+ typedef std::string::size_type size_type;
+
+ unsigned int m = getRows();
+ unsigned int n = getCols();
+
+ std::vector<std::string> values(m*n);
+ std::ostringstream oss;
+ std::ostringstream ossFixed;
+ std::ios_base::fmtflags original_flags = oss.flags();
+
+ // ossFixed <<std::fixed;
+ ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+
+ size_type maxBefore=0; // the length of the integral part
+ size_type maxAfter=0; // number of decimals plus
+ // one place for the decimal point
+ for (unsigned int i=0;i<m;++i) {
+ for (unsigned int j=0;j<n;++j){
+ oss.str("");
+ oss << (*this)[i][j];
+ if (oss.str().find("e")!=std::string::npos){
+ ossFixed.str("");
+ ossFixed << (*this)[i][j];
+ oss.str(ossFixed.str());
+ }
+
+ values[i*n+j]=oss.str();
+ size_type thislen=values[i*n+j].size();
+ size_type p=values[i*n+j].find('.');
+
+ if (p==std::string::npos){
+ maxBefore=vpMath::maximum(maxBefore, thislen);
+ // maxAfter remains the same
+ } else{
+ maxBefore=vpMath::maximum(maxBefore, p);
+ maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ }
+ }
+ }
+
+ size_type totalLength=length;
+ // increase totalLength according to maxBefore
+ totalLength=vpMath::maximum(totalLength,maxBefore);
+ // decrease maxAfter according to totalLength
+ maxAfter=std::min(maxAfter, totalLength-maxBefore);
+ if (maxAfter==1) maxAfter=0;
+
+ // the following line is useful for debugging
+ //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+
+ if (intro) s <<intro;
+ s <<"["<<m<<","<<n<<"]=\n";
+
+ for (unsigned int i=0;i<m;i++) {
+ s <<" ";
+ for (unsigned int j=0;j<n;j++){
+ size_type p=values[i*n+j].find('.');
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s.width((std::streamsize)maxBefore);
+ s <<values[i*n+j].substr(0,p).c_str();
+
+ if (maxAfter>0){
+ s.setf(std::ios::left, std::ios::adjustfield);
+ if (p!=std::string::npos){
+ s.width((std::streamsize)maxAfter);
+ s <<values[i*n+j].substr(p,maxAfter).c_str();
+ } else{
+ assert(maxAfter>1);
+ s.width((std::streamsize)maxAfter);
+ s <<".0";
+ }
+ }
+
+ s <<' ';
+ }
+ s <<std::endl;
+ }
+
+ s.flags(original_flags); // restore s to standard state
+
+ return (int)(maxBefore+maxAfter);
+}
diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
new file mode 100644
index 0000000..ffe2871
--- /dev/null
+++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
@@ -0,0 +1,607 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load and save Homogeneous Matrix in a XML file
+ *
+ * Authors:
+ * Giovanni Claudio
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpXmlParserHomogeneousMatrix.cpp
+ \brief Definition of the vpXmlParserHomogeneousMatrix class member functions.
+ Class vpXmlParserHomogeneousMatrix allowed to load and save an homogeneous matrix in a XML file.
+
+*/
+#include <visp3/core/vpXmlParserHomogeneousMatrix.h>
+#ifdef VISP_HAVE_XML2
+
+#include <stdlib.h>
+#include <string.h>
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
+/* -------------------------------------------------------------------------- */
+/* --- LABEL XML ------------------------------------------------------------ */
+/* -------------------------------------------------------------------------- */
+
+#define LABEL_XML_ROOT "root"
+#define LABEL_XML_M "homogeneous_transformation"
+#define LABEL_XML_M_NAME "name"
+#define LABEL_XML_VALUE "values"
+#define LABEL_XML_TRANSLATION "translation"
+#define LABEL_XML_TX "tx"
+#define LABEL_XML_TY "ty"
+#define LABEL_XML_TZ "tz"
+#define LABEL_XML_ROTATION "rotation"
+#define LABEL_XML_TUX "theta_ux"
+#define LABEL_XML_TUY "theta_uy"
+#define LABEL_XML_TUZ "theta_uz"
+
+/*!
+ Default constructor
+*/
+vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix()
+ : vpXmlParser(), m_M(), m_name()
+{
+}
+/*!
+ Copy constructor
+ \param twinParser : parser object to copy
+*/
+vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix& twinParser)
+ : vpXmlParser(twinParser), m_M(), m_name()
+{
+ *this = twinParser;
+}
+
+/*!
+ Copy operator
+ \param twinParser : parser object to copy
+ \return a copy of the input.
+*/
+vpXmlParserHomogeneousMatrix&
+vpXmlParserHomogeneousMatrix::operator =(const vpXmlParserHomogeneousMatrix& twinParser)
+{
+ this->m_M = twinParser.m_M;
+ this->m_name = twinParser.m_name;
+
+ return *this ;
+}
+
+/*!
+ Parse an xml file to load an homogeneous matrix
+ \param M : homogeneous matrix to fill.
+ \param filename : name of the xml file to parse.
+ \param name : name of the homogeneous matrix to find in the xml file.
+
+ \return error code.
+*/
+int
+vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename,
+ const std::string &name)
+{
+ xmlDocPtr doc;
+ xmlNodePtr node;
+
+ doc = xmlParseFile(filename.c_str());
+ if (doc == NULL)
+ {
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " I cannot open the file "<< filename << std::endl;
+
+ return SEQUENCE_ERROR;
+ }
+
+ node = xmlDocGetRootElement(doc);
+ if (node == NULL)
+ {
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ int ret = this ->read (doc, node, name);
+
+ M = m_M ;
+
+ xmlFreeDoc(doc);
+
+ return ret;
+}
+
+/*!
+ Save an homogeneous matrix in an xml file.
+ \param M : homogeneous matrix to save.
+ \param filename : name of the xml file to fill.
+ \param name : name of the homogeneous matrix.
+
+ \return error code.
+*/
+int
+vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
+ const std::string &name)
+{
+ xmlDocPtr doc;
+ xmlNodePtr node;
+
+ doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR
+ + XML_PARSE_NOBLANKS);
+ if (doc == NULL){
+ doc = xmlNewDoc ((xmlChar*)"1.0");
+ node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
+ xmlDocSetRootElement(doc,node);
+ xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
+ "This file stores homogeneous matrix used\n"
+ " in the vpHomogeneousMatrix Class of ViSP available\n"
+ " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
+ " It can be read with the parse method of\n"
+ " the vpXmlParserHomogeneousMatrix class.");
+ xmlAddChild(node,node_tmp);
+ }
+
+ node = xmlDocGetRootElement(doc);
+ if (node == NULL)
+ {
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ this->m_M = M;
+
+ int M_isFound = count(doc, node, name);
+
+ if( M_isFound > 0){
+ //vpCERROR
+ std::cout << "There is already an homogeneous matrix "<< std::endl
+ << "available in the file with the input name: "<< name << "."<< std::endl
+ << "Please delete it manually from the xml file."<< std::endl;
+ xmlFreeDoc(doc);
+ return SEQUENCE_ERROR;
+ }
+
+ write(node, name);
+
+ xmlSaveFormatFile(filename.c_str(), doc, 1);
+ xmlFreeDoc(doc);
+// std::cout << "Homogeneous matrix '"<< name << "' saved in the file named "<< filename << " correctly." << std::endl;
+
+ return SEQUENCE_OK;
+}
+
+
+
+/*!
+ Read Homogeneous matrix values from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param name : name of the Homogeneous Matrix
+ \return error code.
+ */
+int
+vpXmlParserHomogeneousMatrix::read (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name)
+{
+ // char * val_char;
+ vpXmlCodeType prop;
+
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+ unsigned int nbM = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+ if (prop == CODE_XML_M){
+ if (SEQUENCE_OK == this->read_matrix (doc, node, name))
+ nbM++;
+ }
+ else back = SEQUENCE_ERROR;
+ }
+
+ if (nbM == 0){
+ back = SEQUENCE_ERROR;
+ vpCERROR << "No Homogeneous matrix is available" << std::endl
+ << "with name: " << name << std::endl;
+ }
+ else if(nbM > 1){
+ back = SEQUENCE_ERROR;
+ vpCERROR << nbM << " There are more Homogeneous matrix" << std::endl
+ << "with the same name : " << std::endl
+ << "precise your choice..." << std::endl;
+ }
+
+ return back;
+}
+/*!
+ Read homogeneous matrix names from a XML file and read if there is already a homogeneous matrix
+ with the same name.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param name : name of the homogeneous matrix.
+
+ \return 1 if there is an homogeneous matrix corresponding with the input name, 0 otherwise.
+ */
+int
+vpXmlParserHomogeneousMatrix::count (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name)
+{
+ // char * val_char;
+ vpXmlCodeType prop;
+ int nbM = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ }
+ if (prop== CODE_XML_M) {
+ if (SEQUENCE_OK == this->read_matrix (doc, node, name))
+ nbM++;
+ }
+ }
+
+ return nbM;
+}
+
+/*!
+ Read Homogeneous Matrix fields from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param name : name of the Homogeneous matrix
+
+ \return error code.
+
+ */
+int
+vpXmlParserHomogeneousMatrix::read_matrix (xmlDocPtr doc, xmlNodePtr node,
+ const std::string& name)
+{
+ vpXmlCodeType prop;
+ /* read value in the XML file. */
+ std::string M_name_tmp = "";
+ vpHomogeneousMatrix M_tmp;
+
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+
+ switch (prop)
+ {
+ case CODE_XML_M_NAME: {
+ char * val_char = xmlReadCharChild(doc, node);
+ M_name_tmp = val_char;
+ xmlFree(val_char);
+ break;
+ }
+
+ case CODE_XML_VALUE: //VALUE
+ if (name == M_name_tmp)
+ {
+ std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
+ back = read_values(doc, node, M_tmp);
+ }
+ break;
+
+ case CODE_XML_BAD:
+ case CODE_XML_OTHER:
+ case CODE_XML_M:
+ case CODE_XML_TX:
+ case CODE_XML_TY:
+ case CODE_XML_TZ:
+ case CODE_XML_TUX:
+ case CODE_XML_TUY:
+ case CODE_XML_TUZ:
+
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+
+ }
+
+ if( !(name == M_name_tmp)){
+ back = SEQUENCE_ERROR;
+ }
+ else{
+ this-> m_M = M_tmp;
+ //std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
+ //std::cout << this-> M << std::endl;
+ this-> m_name = M_name_tmp;
+
+ }
+ return back;
+}
+
+
+/*!
+ Read homogeneous matrix fields from a XML file.
+
+ \param doc : XML file.
+ \param node : XML tree, pointing on a marker equipement.
+ \param M_tmp : homogeneous matrix to fill with read data (output).
+
+ \return error code.
+
+ */
+vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType
+vpXmlParserHomogeneousMatrix::read_values (xmlDocPtr doc, xmlNodePtr node,
+ vpHomogeneousMatrix &M)
+{
+ // counter of the number of read parameters
+ int nb = 0;
+ vpXmlCodeType prop;
+ /* read value in the XML file. */
+
+ double tx_=0.;
+ double ty_=0.;
+ double tz_=0.;
+ double tux_=0.;
+ double tuy_=0.;
+ double tuz_=0.;
+
+ vpXmlCodeSequenceType back = SEQUENCE_OK;
+ //int validation = 0;
+
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next)
+ {
+ // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
+ if (node->type != XML_ELEMENT_NODE) continue;
+ if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
+ {
+ prop = CODE_XML_OTHER;
+ back = SEQUENCE_ERROR;
+ }
+
+ switch (prop)
+ {
+
+ case CODE_XML_TX:
+ tx_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+ case CODE_XML_TY:
+ ty_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+ case CODE_XML_TZ:
+ tz_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+ case CODE_XML_TUX:
+ tux_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+ case CODE_XML_TUY:
+ tuy_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+ case CODE_XML_TUZ:
+ tuz_ = xmlReadDoubleChild(doc, node);
+ nb++;
+ break;
+
+ case CODE_XML_BAD:
+ case CODE_XML_OTHER:
+ case CODE_XML_M:
+ case CODE_XML_M_NAME:
+ case CODE_XML_VALUE:
+
+ default:
+ back = SEQUENCE_ERROR;
+ break;
+ }
+ }
+
+ if (nb != 6)
+ {
+ vpCERROR <<"ERROR in 'model' field:\n";
+ vpCERROR << "it must contain 6 parameters\n";
+
+ return SEQUENCE_ERROR;
+ }
+
+ // Create the Homogeneous matrix
+ M.buildFrom(tx_,ty_,tz_,tux_,tuy_,tuz_);
+
+ // std::cout << "Read values from file:" << std::endl;
+ // std::cout << "tx:" << tx_<< std::endl;
+ // std::cout << "ty:" << ty_<< std::endl;
+ // std::cout << "tz:" << tz_<< std::endl;
+ // std::cout << "tux:" << tux_<< std::endl;
+ // std::cout << "tuy:" << tuy_<< std::endl;
+ // std::cout << "tuz:" << tuz_<< std::endl;
+
+ return back;
+}
+
+/*!
+ Write Homogeneous Matrix in an XML Tree.
+
+ \param node : XML tree, pointing on a marker equipement.
+ \param name : name of the Homogeneous Matrix.
+
+
+ \return error code.
+ */
+int vpXmlParserHomogeneousMatrix::
+write (xmlNodePtr node, const std::string& name)
+{
+ int back = SEQUENCE_OK;
+
+ xmlNodePtr node_tmp;
+ xmlNodePtr node_matrix;
+ xmlNodePtr node_values;
+ char str[11];
+
+ // Convert from Rotational matrix to Theta-U vector
+ vpRotationMatrix R;
+ m_M.extract(R);
+
+ vpThetaUVector tu(R);
+
+ // <homogeneous_transformation>
+ node_tmp = xmlNewComment((xmlChar*)"Homogeneous Matrix");
+ xmlAddChild(node,node_tmp);
+ node_matrix = xmlNewNode(NULL,(xmlChar*)LABEL_XML_M);
+ xmlAddChild(node,node_matrix);
+ {
+ //<name>
+
+ if(!name.empty()){
+ node_tmp = xmlNewComment((xmlChar*)"Name of the homogeneous matrix");
+ xmlAddChild(node_matrix,node_tmp);
+ xmlNewTextChild(node_matrix,NULL,(xmlChar*)LABEL_XML_M_NAME, (xmlChar*)name.c_str());
+ }
+
+ //<values>
+
+ node_values = xmlNewNode(NULL,(xmlChar*)LABEL_XML_VALUE);
+ xmlAddChild(node_matrix,node_values);
+ {
+ node_tmp = xmlNewComment((xmlChar*)"Translation vector with values in meters");
+ xmlAddChild(node_values,node_tmp);
+
+ //<tx>
+ sprintf(str,"%f", m_M[0][3]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TX,(xmlChar*)str);
+
+ //<ty>
+ sprintf(str,"%f", m_M[1][3]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TY,(xmlChar*)str);
+
+ //<tz>
+ sprintf(str,"%f", m_M[2][3]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TZ,(xmlChar*)str);
+
+ node_tmp = xmlNewComment((xmlChar*)"Rotational vector expressed in angle axis representation with values in radians");
+ xmlAddChild(node_values,node_tmp);
+
+ //<tux>
+ sprintf(str,"%f", tu[0]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUX,(xmlChar*)str);
+
+ //<tuy>
+ sprintf(str,"%f", tu[1]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUY,(xmlChar*)str);
+
+ //<tuz>
+ sprintf(str,"%f", tu[2]);
+ xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUZ,(xmlChar*)str);
+ }
+ }
+ return back;
+}
+
+/*!
+ Translate a string (label) to a xml code.
+ \param str : string to translate.
+ \param res : resulting code.
+
+ \return error code.
+*/
+
+vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType
+vpXmlParserHomogeneousMatrix::str2xmlcode (char * str, vpXmlCodeType & res)
+{
+ vpXmlCodeType val_int = CODE_XML_BAD;
+ vpXmlCodeSequenceType back = vpXmlParserHomogeneousMatrix::SEQUENCE_OK;
+
+ // DEBUG_TRACE (9, "# Entree :str=%s.", str);
+
+ if (! strcmp (str, LABEL_XML_M))
+ {
+ val_int = CODE_XML_M;
+ }
+ else if (! strcmp (str, LABEL_XML_M_NAME))
+ {
+ val_int = CODE_XML_M_NAME;
+ }
+ else if (! strcmp (str, LABEL_XML_VALUE))
+ {
+ val_int = CODE_XML_VALUE;
+ }
+ else if (! strcmp (str, LABEL_XML_TX))
+ {
+ val_int = CODE_XML_TX;
+ }
+ else if (! strcmp (str, LABEL_XML_TY))
+ {
+ val_int = CODE_XML_TY;
+ }
+ else if (! strcmp (str, LABEL_XML_TZ))
+ {
+ val_int = CODE_XML_TZ;
+ }
+ else if (! strcmp (str, LABEL_XML_TUX))
+ {
+ val_int = CODE_XML_TUX;
+ }
+ else if (! strcmp (str, LABEL_XML_TUY))
+ {
+ val_int = CODE_XML_TUY;
+ }
+ else if (! strcmp (str, LABEL_XML_TUZ))
+ {
+ val_int = CODE_XML_TUZ;
+ }
+ else
+ {
+ val_int = CODE_XML_OTHER;
+ }
+ res = val_int;
+
+ return back;
+}
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
+void dummy_vpXmlParserHomogeneousMatrix() {};
+#endif //VISP_HAVE_XML2
diff --git a/modules/core/src/tools/convert/vpConvert.cpp b/modules/core/src/tools/convert/vpConvert.cpp
new file mode 100644
index 0000000..cd8bdf8
--- /dev/null
+++ b/modules/core/src/tools/convert/vpConvert.cpp
@@ -0,0 +1,434 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Directory management.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpConvert.cpp
+ \brief Tools for type or general conversion.
+*/
+
+#include <vector> // std::vector
+#include <algorithm> // std::transform
+
+#include <visp3/core/vpConvert.h>
+
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ /**!
+ Unary function used to transform a cv::KeyPoint to a vpImagePoint.
+ \param keypoint : KeyPoint to convert.
+
+ \return A vpImagePoint with the 2D coordinates corresponding to the location of the KeyPoint.
+ */
+ vpImagePoint vpConvert::keyPointToVpImagePoint(const cv::KeyPoint &keypoint) {
+ return vpImagePoint(keypoint.pt.y, keypoint.pt.x);
+ }
+
+ /*!
+ Unary function to convert a cv::Point2f to a vpImagePoint.
+ \param point : Point to convert.
+
+ \return A vpImagePoint with the 2D coordinates stored in cv::Point2f.
+ */
+ vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) {
+ return vpImagePoint(point.y, point.x);
+ }
+
+ /*!
+ Unary function to convert a cv::Point2d to a vpImagePoint.
+ \param point : Point to convert.
+
+ \return A vpImagePoint with the 2D coordinates stored in cv::Point2d.
+ */
+ vpImagePoint vpConvert::point2dToVpImagePoint(const cv::Point2d &point) {
+ return vpImagePoint(point.y, point.x);
+ }
+
+ /*!
+ Unary function to convert a cv::Point3f to a vpPoint (object frame).
+ \param point3f : Point to convert.
+ \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3f.
+ */
+ vpPoint vpConvert::point3fToVpObjectPoint(const cv::Point3f &point3f) {
+ vpPoint pt;
+ pt.set_oX(point3f.x);
+ pt.set_oY(point3f.y);
+ pt.set_oZ(point3f.z);
+ pt.set_oW(1.0);
+ return pt;
+ }
+
+ /*!
+ Unary function to convert a cv::Point3f to a vpPoint (camera frame).
+ \param point3f : Point to convert.
+ \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3f.
+ */
+ vpPoint vpConvert::point3fToVpCamPoint(const cv::Point3f &point3f) {
+ vpPoint pt;
+ pt.set_X(point3f.x);
+ pt.set_Y(point3f.y);
+ pt.set_Z(point3f.z);
+ pt.set_W(1.0);
+ return pt;
+ }
+
+ /*!
+ Unary function to convert a cv::Point3d to a vpPoint (object frame).
+ \param point3d : Point to convert.
+ \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3d.
+ */
+ vpPoint vpConvert::point3dToVpObjectPoint(const cv::Point3d &point3d) {
+ vpPoint pt;
+ pt.set_oX(point3d.x);
+ pt.set_oY(point3d.y);
+ pt.set_oZ(point3d.z);
+ pt.set_oW(1.0);
+ return pt;
+ }
+
+ /*!
+ Unary function to convert a cv::Point3d to a vpPoint (camera frame).
+ \param point3d : Point to convert.
+ \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3d.
+ */
+ vpPoint vpConvert::point3dToVpCamPoint(const cv::Point3d &point3d) {
+ vpPoint pt;
+ pt.set_X(point3d.x);
+ pt.set_Y(point3d.y);
+ pt.set_Z(point3d.z);
+ pt.set_W(1.0);
+ return pt;
+ }
+
+ /*!
+ Unary function to convert a vpImagePoint to a cv::Point2f.
+ \param point : Image point to convert.
+
+ \return A cv::Point2f with the 2D coordinates stored in vpImagePoint.
+ */
+ cv::Point2f vpConvert::vpImagePointToPoint2f(const vpImagePoint &point) {
+ return cv::Point2f((float) point.get_u(), (float) point.get_v());
+ }
+
+ /*!
+ Unary function to convert a vpImagePoint to a cv::Point2d.
+ \param point : Image point to convert.
+
+ \return A cv::Point2d with the 2D coordinates stored in vpImagePoint.
+ */
+ cv::Point2d vpConvert::vpImagePointToPoint2d(const vpImagePoint &point) {
+ return cv::Point2d(point.get_u(), point.get_v());
+ }
+
+ /*!
+ Unary function to convert the 3D coordinates in the camera frame to a cv::Point3f.
+ \param point : Point to convert.
+
+ \return A cv::Point3f with the 3D coordinates stored in vpPoint in the camera frame.
+ */
+ cv::Point3f vpConvert::vpCamPointToPoint3f(const vpPoint &point) {
+ return cv::Point3f((float) point.get_X(), (float) point.get_Y(), (float) point.get_Z());
+ }
+
+ /*!
+ Unary function to convert the 3D coordinates in the camera frame to a cv::Point3d.
+ \param point : Point to convert.
+
+ \return A cv::Point3d with the 3D coordinates stored in vpPoint in the camera frame.
+ */
+ cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point) {
+ return cv::Point3d(point.get_X(), point.get_Y(), point.get_Z());
+ }
+
+ /*!
+ Unary function to convert the 3D coordinates in the object frame to a cv::Point3f.
+ \param point : Point to convert.
+
+ \return A cv::Point3f with the 3D coordinates stored in vpPoint in the object frame.
+ */
+ cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point) {
+ return cv::Point3f((float) point.get_oX(), (float) point.get_oY(), (float) point.get_oZ());
+ }
+
+ /*!
+ Unary function to convert the 3D coordinates in the object frame to a cv::Point3d.
+ \param point : Point to convert.
+
+ \return A cv::Point3d with the 3D coordinates stored in vpPoint in the object frame.
+ */
+ cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) {
+ return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
+ }
+
+ /*!
+ Unary function to return the train index stored in a cv::DMatch.
+ \param match : The cv::DMatch we want to get the train index.
+
+ \return The train index stored in a cv::DMatch.
+ */
+ int vpConvert::dMatchToTrainIndex(const cv::DMatch &match) {
+ return match.trainIdx;
+ }
+
+
+ /*!
+ Convert a cv::KeyPoint to a vpImagePoint.
+ \param from : cv::KeyPoint to convert.
+ \param to : vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to) {
+ to = keyPointToVpImagePoint(from);
+ }
+
+ /*!
+ Convert a cv::Point2f to a vpImagePoint.
+ \param from : cv::Point2f to convert.
+ \param to : vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to) {
+ to = point2fToVpImagePoint(from);
+ }
+
+ /*!
+ Convert a cv::Point2d to a vpImagePoint.
+ \param from : cv::Point2d to convert.
+ \param to : vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) {
+ to = point2dToVpImagePoint(from);
+ }
+
+ /*!
+ Convert a cv::Point3f to a vpPoint.
+ \param from : cv::Point3f to convert.
+ \param to : vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame) {
+ if(cameraFrame) {
+ to = point3fToVpCamPoint(from);
+ } else {
+ to = point3fToVpObjectPoint(from);
+ }
+ }
+
+ /*!
+ Convert a cv::Point3d to a vpPoint.
+ \param from : cv::Point3d to convert.
+ \param to : vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame) {
+ if(cameraFrame) {
+ to = point3dToVpCamPoint(from);
+ } else {
+ to = point3dToVpObjectPoint(from);
+ }
+ }
+
+ /*!
+ Convert a vector of cv::KeyPoint to a vector of vpImagePoint.
+ \param from : Vector of cv::KeyPoint to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), keyPointToVpImagePoint);
+ }
+
+ /*!
+ Convert a vector of cv::Point2f to a vector of vpImagePoint.
+ \param from : Vector of cv::Point2f to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), point2fToVpImagePoint);
+ }
+
+ /*!
+ Convert a vector of cv::Point2d to a vector of vpImagePoint.
+ \param from : Vector of cv::Point2d to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), point2dToVpImagePoint);
+ }
+
+ /*!
+ Convert a vector of cv::Point3f to a vector of vpPoint.
+ \param from : Vector of cv::Point3f to convert.
+ \param to : Vector of vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
+ to.resize(from.size());
+ if(cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), point3fToVpCamPoint);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), point3fToVpObjectPoint);
+ }
+ }
+
+ /*!
+ Convert a vector of cv::Point3d to a vector of vpPoint.
+ \param from : Vector of cv::Point3d to convert.
+ \param to : Vector of vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
+ to.resize(from.size());
+ if(cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), point3dToVpCamPoint);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), point3dToVpObjectPoint);
+ }
+ }
+
+ /*!
+ Convert a vector of cv::DMatch to a vector of unsigned int (for a query index 0, to[0] ==> train index).
+
+ \warning The list of query indexes in DMatch must be ordered in a way that from[i].queryIdx == i.
+
+ \param from : Vector of cv::DMatch to convert.
+ \param to : Vector of unsigned int converted.
+ */
+ void vpConvert::convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), dMatchToTrainIndex);
+ }
+
+ /*!
+ Convert a vpImagePoint to a cv::Point2f.
+ \param from : vpImagePoint to convert.
+ \param to : cv::Point2f converted.
+ */
+ void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2f &to) {
+ to = vpImagePointToPoint2f(from);
+ }
+
+ /*!
+ Convert a vpImagePoint to a cv::Point2d.
+ \param from : vpImagePoint to convert.
+ \param to : cv::Point2d converted.
+ */
+ void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) {
+ to = vpImagePointToPoint2d(from);
+ }
+
+ /*!
+ Convert a vpPoint to a cv::Point3f.
+ \param from : vpPoint to convert.
+ \param to : cv::Point3f converted.
+ \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame) {
+ if(cameraFrame) {
+ to = vpCamPointToPoint3f(from);
+ } else {
+ to = vpObjectPointToPoint3f(from);
+ }
+ }
+
+ /*!
+ Convert a vpPoint to a cv::Point3d.
+ \param from : vpPoint to convert.
+ \param to : cv::Point3d converted.
+ \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
+ */
+ void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame) {
+ if(cameraFrame) {
+ to = vpCamPointToPoint3d(from);
+ } else {
+ to = vpObjectPointToPoint3d(from);
+ }
+ }
+
+ /*!
+ Convert a vector of vpImagePoint to a vector of cv::Point2f.
+ \param from : Vector of vpImagePoint to convert.
+ \param to : Vector of cv::Point2f converted.
+ */
+ void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2f);
+ }
+
+ /*!
+ Convert a vector of vpImagePoint to a vector of cv::Point2d.
+ \param from : Vector of vpImagePoint to convert.
+ \param to : Vector of cv::Point2d converted.
+ */
+ void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to) {
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2d);
+ }
+
+ /*!
+ Convert a vector of vpPoint to a vector of cv::Point3f.
+ \param from : Vector of vpPoint to convert.
+ \param to : Vector of cv::Point3f converted.
+ \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
+ */
+ void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame) {
+ to.resize(from.size());
+ if(cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3f);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3f);
+ }
+ }
+
+ /*!
+ Convert a vector of vpPoint to a vector of cv::Point3d.
+ \param from : Vector of vpPoint to convert.
+ \param to : Vector of cv::Point3d converted.
+ \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
+ */
+ void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame) {
+ to.resize(from.size());
+ if(cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3d);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3d);
+ }
+ }
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpConvert.cpp.o) has no symbols
+void dummy_vpConvert() {};
+#endif
diff --git a/modules/core/src/tools/exceptions/vpException.cpp b/modules/core/src/tools/exceptions/vpException.cpp
new file mode 100644
index 0000000..16bb557
--- /dev/null
+++ b/modules/core/src/tools/exceptions/vpException.cpp
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception handling.
+ *
+ * Authors:
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+
+
+/* \file vpException.cpp
+ \brief error that can be emited by the vp class and its derivates
+ */
+
+#include <stdio.h>
+#include "visp3/core/vpException.h"
+
+
+/* ------------------------------------------------------------------------- */
+/* --- CONSTRUCTORS -------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+vpException::vpException (int id)
+ : code (id), message ()
+{
+}
+
+vpException::vpException (int id, const std::string & msg)
+ : code (id), message (msg)
+{
+}
+
+vpException::vpException (int id, const char* format, ...)
+ : code (id), message ()
+{
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+}
+
+vpException::vpException (const 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)
+{
+ char buffer[1024];
+ vsnprintf (buffer, 1024, format, args);
+ std::string msg(buffer);
+ message = msg;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- ACCESSORS ---------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+const char *vpException::getMessage (void)
+{
+ return (this->message) .c_str();
+}
+
+const std::string &vpException::getStringMessage (void)
+{
+ return this->message;
+}
+
+int
+vpException::getCode (void)
+{
+ 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
new file mode 100644
index 0000000..5b6d2c7
--- /dev/null
+++ b/modules/core/src/tools/file/vpIoTools.cpp
@@ -0,0 +1,1495 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Directory management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpIoTools.cpp
+ \brief File and directories basic tools.
+*/
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpIoException.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fstream>
+#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <limits>
+#include <cmath>
+#include <algorithm>
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# include <unistd.h>
+#elif defined(_WIN32)
+# include <windows.h>
+# include <direct.h>
+#endif
+#if !defined(_WIN32)
+# include <wordexp.h>
+#endif
+
+std::string vpIoTools::baseName = "";
+std::string vpIoTools::baseDir = "";
+std::string vpIoTools::configFile = "";
+std::vector<std::string> vpIoTools::configVars = std::vector<std::string>();
+std::vector<std::string> vpIoTools::configValues = std::vector<std::string>();
+
+/*!
+ Sets the base name (prefix) of the experiment files.
+
+ \param s : Prefix of the experiment files.
+*/
+void vpIoTools::setBaseName(const std::string &s) {baseName = s;}
+/*!
+ Sets the base directory of the experiment files.
+
+ \param dir : Directory where the data will be saved.
+*/
+void vpIoTools::setBaseDir(const std::string &dir) {baseDir = dir + "/";}
+/*!
+ Gets the base name (prefix) of the experiment files.
+
+ \return the base name of the experiment files.
+*/
+std::string vpIoTools::getBaseName() {return baseName;}
+/*!
+ Gets the full path of the experiment files : baseDir/baseName
+
+ \return the full path of the experiment files.
+*/
+std::string vpIoTools::getFullName() {return baseDir + baseName;}
+
+/*!
+ Get the user name.
+
+ - Under unix, get the content of the LOGNAME environment variable. For most
+ purposes (especially in conjunction with crontab), it is more useful to use
+ the environment variable LOGNAME to find out who the user is, rather than
+ the getlogin() function. This is more flexible precisely because the user
+ can set LOGNAME arbitrarily.
+ - Under windows, uses the GetUserName() function.
+
+ \param username : The user name.
+
+ \exception vpIoException::cantGetUserName : If this method cannot get the
+ user name.
+
+ \sa getUserName()
+*/
+void
+vpIoTools::getUserName(std::string &username)
+{
+ // With MinGW, UNIX and _WIN32 are defined
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ // Get the user name.
+ char *_username = NULL;
+ _username = ::getenv("LOGNAME");
+ if (_username == NULL) {
+ vpERROR_TRACE( "Cannot get the username. Check your LOGNAME environment variable" );
+ throw(vpIoException(vpIoException::cantGetUserName,
+ "Cannot get the username")) ;
+ }
+ username = _username;
+#elif defined(_WIN32)
+ unsigned int info_buffer_size = 1024;
+ TCHAR *infoBuf = new TCHAR [info_buffer_size];
+ DWORD bufCharCount = (DWORD) info_buffer_size;
+ // Get the user name.
+ if( ! GetUserName( infoBuf, &bufCharCount ) ) {
+ delete [] infoBuf;
+ vpERROR_TRACE( "Cannot get the username" );
+ throw(vpIoException(vpIoException::cantGetUserName,
+ "Cannot get the username")) ;
+
+ }
+ username = infoBuf;
+ delete [] infoBuf;
+#endif
+}
+/*!
+ Get the user name.
+
+ - Under unix, get the content of the LOGNAME environment variable. For most
+ purposes (especially in conjunction with crontab), it is more useful to use
+ the environment variable LOGNAME to find out who the user is, rather than
+ the getlogin() function. This is more flexible precisely because the user
+ can set LOGNAME arbitrarily.
+ - Under windows, uses the GetUserName() function.
+
+ \return The user name.
+
+ \exception vpIoException::cantGetUserName : If this method cannot get the
+ user name.
+
+ \sa getUserName(std::string &)
+*/
+std::string
+vpIoTools::getUserName()
+{
+ std::string username;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ // Get the user name.
+ char *_username = NULL;
+ _username = ::getenv("LOGNAME");
+ if (_username == NULL) {
+ vpERROR_TRACE( "Cannot get the username. Check your LOGNAME environment variable" );
+ throw(vpIoException(vpIoException::cantGetUserName,
+ "Cannot get the username")) ;
+ }
+ username = _username;
+#elif defined(_WIN32)
+ unsigned int info_buffer_size = 1024;
+ TCHAR *infoBuf = new TCHAR [info_buffer_size];
+ DWORD bufCharCount = (DWORD) info_buffer_size;
+ // Get the user name.
+ if( ! GetUserName( infoBuf, &bufCharCount ) ) {
+ delete [] infoBuf;
+ vpERROR_TRACE( "Cannot get the username" );
+ throw(vpIoException(vpIoException::cantGetUserName,
+ "Cannot get the username")) ;
+
+ }
+ username = infoBuf;
+ delete [] infoBuf;
+#endif
+ return username;
+}
+
+/*!
+ Get the content of an environment variable.
+
+ \param env : Environment variable name (HOME, LOGNAME...).
+ \return Value of the environment variable.
+
+ \exception vpIoException::cantGetenv : If an error occur while
+ getting the environment variable value.
+
+ \code
+#include <iostream>
+#include <string>
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ std::string envvalue;
+ try {
+ envvalue = vpIoTools::getenv("HOME");
+ std::cout << "$HOME = \"" << envvalue << "\"" << std::endl;
+ }
+ catch (vpException &e) {
+ std::cout << e.getMessage() << std::endl;
+ return -1;
+ }
+ return 0;
+}
+ \endcode
+
+ \sa getenv(std::string &)
+*/
+std::string
+vpIoTools::getenv(const char *env)
+{
+ std::string value;
+ // Get the environment variable value.
+ char *_value = NULL;
+ _value = ::getenv(env);
+ if (_value == NULL) {
+ vpERROR_TRACE( "Cannot get the environment variable value" );
+ throw(vpIoException(vpIoException::cantGetenv,
+ "Cannot get the environment variable value")) ;
+ }
+ value = _value;
+
+ return value;
+}
+
+/*!
+ Get the content of an environment variable.
+
+ \param env : Environment variable name (HOME, LOGNAME...).
+ \return Value of the environment variable
+
+ \exception vpIoException::cantGetenv : If an error occur while
+ getting the environment variable value.
+
+ \code
+#include <iostream>
+#include <string>
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ std::string envvalue;
+ try {
+ std::string env = "HOME";
+ envvalue = vpIoTools::getenv(env);
+ std::cout << "$HOME = \"" << envvalue << "\"" << std::endl;
+ }
+ catch (vpException &e) {
+ std::cout << e.getMessage() << std::endl;
+ return -1;
+ }
+ return 0;
+}
+ \endcode
+
+ \sa getenv(const char *)
+*/
+std::string
+vpIoTools::getenv(const std::string &env)
+{
+ return (vpIoTools::getenv(env.c_str()));
+}
+
+/*!
+ Extract major, minor and patch from a version given as "x.x.x".
+ Ex: If version is "1.2.1", major will be 1, minor 2 and patch 1.
+
+ \param version : String to extract the values.
+ \param major : Extracted major.
+ \param minor : Extracted minor.
+ \param patch : Extracted patch.
+*/
+void
+vpIoTools::getVersion(const std::string &version, unsigned int &major, unsigned int &minor, unsigned int &patch)
+{
+ if(version.size() == 0){
+ major = 0;
+ minor = 0;
+ patch = 0;
+ }
+ else{
+ size_t major_pos = version.find('.');
+ std::string major_str = version.substr(0, major_pos);
+ major = (unsigned)atoi(major_str.c_str());
+
+ if(major_pos != std::string::npos){
+ size_t minor_pos = version.find('.', major_pos+1);
+ std::string minor_str = version.substr(major_pos+1, (minor_pos - (major_pos+1)));
+ minor = (unsigned)atoi(minor_str.c_str());
+
+ if(minor_pos != std::string::npos){
+ std::string patch_str = version.substr(minor_pos+1);
+ patch = (unsigned)atoi(patch_str.c_str());
+ }
+ else{
+ patch = 0;
+ }
+ }
+ else{
+ minor = 0;
+ patch = 0;
+ }
+ }
+}
+
+/*!
+
+ Check if a directory exists.
+
+ \param dirname : Directory to test if it exists. The directory name
+ is converted to the current system's format; see path().
+
+ \return true : If the directory exists and is accessible with write access.
+
+ \return false : If dirname string is null, or is not a directory, or
+ has no write access.
+
+ \sa checkDirectory(const std::string &)
+*/
+bool
+vpIoTools::checkDirectory(const char *dirname )
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ struct stat stbuf;
+#elif defined(_WIN32)
+ struct _stat stbuf;
+#endif
+
+ if ( dirname == NULL || dirname[0] == '\0' ) {
+ return false;
+ }
+
+ std::string _dirname = path(dirname);
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( stat( _dirname.c_str(), &stbuf ) != 0 )
+#elif defined(_WIN32)
+ if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
+#endif
+ {
+ return false;
+ }
+ if ( (stbuf.st_mode & S_IFDIR) == 0 ) {
+ return false;
+ }
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( (stbuf.st_mode & S_IWUSR) == 0 )
+#elif defined(_WIN32)
+ if ( (stbuf.st_mode & S_IWRITE) == 0 )
+#endif
+ {
+ return false;
+ }
+ return true;
+}
+
+/*!
+ Check if a directory exists.
+
+ \param dirname : Directory to test if it exists. The directory name
+ is converted to the current system's format; see path().
+
+ \return true : If the directory exists and is accessible with write access.
+
+ \return false : If dirname string is null, or is not a directory, or
+ has no write access.
+
+ \sa checkDirectory(const char *)
+*/
+bool
+vpIoTools::checkDirectory(const std::string &dirname )
+{
+ return vpIoTools::checkDirectory(dirname.c_str());
+}
+/*!
+
+ Create a new directory.
+
+ \param dirname : Directory to create. The directory name
+ is converted to the current system's format; see path().
+
+ \exception vpIoException::invalidDirectoryName : The \e dirname is invalid.
+
+ \exception vpIoException::cantCreateDirectory : If the directory cannot be
+ created.
+
+ \sa makeDirectory(const std::string &)
+*/
+void
+vpIoTools::makeDirectory(const char *dirname )
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ struct stat stbuf;
+#elif defined(_WIN32)
+ struct _stat stbuf;
+#endif
+
+ if ( dirname == NULL || dirname[0] == '\0' ) {
+ vpERROR_TRACE( "invalid directory name\n");
+ throw(vpIoException(vpIoException::invalidDirectoryName,
+ "invalid directory name")) ;
+ }
+
+ std::string _dirname = path(dirname);
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( stat( _dirname.c_str(), &stbuf ) != 0 )
+#elif defined(_WIN32)
+ if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
+#endif
+ {
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+ if ( mkdir( _dirname.c_str(), (mode_t)0755 ) != 0 )
+#elif defined(_WIN32)
+ if ( _mkdir( _dirname.c_str()) != 0 )
+#endif
+ {
+ vpERROR_TRACE("unable to create directory '%s'\n", dirname );
+ throw(vpIoException(vpIoException::cantCreateDirectory,
+ "unable to create directory")) ;
+ }
+ vpDEBUG_TRACE(2,"has created directory '%s'\n", dirname );
+ }
+
+ if ( checkDirectory( dirname ) == false) {
+ vpERROR_TRACE("unable to create directory '%s'\n", dirname );
+ throw(vpIoException(vpIoException::cantCreateDirectory,
+ "unable to create directory")) ;
+ }
+}
+
+/*!
+
+ Create a new directory.
+
+ \param dirname : Directory to create. The directory name
+ is converted to the current system's format; see path().
+
+ \exception vpIoException::cantCreateDirectory : If the directory cannot be
+ created.
+
+ \sa makeDirectory(const char *)
+*/
+void
+vpIoTools::makeDirectory(const std::string &dirname )
+{
+ try {
+ vpIoTools::makeDirectory(dirname.c_str());
+ }
+ catch (...) {
+ vpERROR_TRACE("unable to create directory '%s'\n",dirname.c_str());
+ throw(vpIoException(vpIoException::cantCreateDirectory,
+ "unable to create directory")) ;
+ }
+}
+
+/*!
+
+ Check if a file exists.
+
+ \param filename : Filename to test if it exists.
+
+ \return true : If the filename exists and is accessible with read access.
+
+ \return false : If filename string is null, or is not a filename, or
+ has no read access.
+
+ \sa checkFilename(const std::string &)
+*/
+bool
+vpIoTools::checkFilename(const char *filename)
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ struct stat stbuf;
+#elif defined(_WIN32)
+ struct _stat stbuf;
+#endif
+
+ if ( filename == NULL || filename[0] == '\0' ) {
+ return false;
+ }
+
+ std::string _filename = path(filename);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( stat( _filename.c_str(), &stbuf ) != 0 )
+#elif defined(_WIN32)
+ if ( _stat( _filename.c_str(), &stbuf ) != 0 )
+#endif
+ {
+ return false;
+ }
+ if ( (stbuf.st_mode & S_IFREG) == 0 ) {
+ return false;
+ }
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( (stbuf.st_mode & S_IRUSR) == 0 )
+#elif defined(_WIN32)
+ if ( (stbuf.st_mode & S_IREAD) == 0 )
+#endif
+ {
+ return false;
+ }
+ return true;
+}
+
+/*!
+ Check if a file exists.
+
+ \param filename : Filename to test if it exists.
+
+ \return true : If the filename exists and is accessible with read access.
+
+ \return false : If filename string is null, or is not a filename, or
+ has no read access.
+
+ \sa checkFilename(const char *)
+*/
+bool
+vpIoTools::checkFilename(const std::string &filename)
+{
+ return vpIoTools::checkFilename(filename.c_str());
+}
+
+/*!
+
+ Copy a \e src file or directory in \e dst.
+
+ \param src : Existing file or directory to copy.
+ \param dst : New copied file or directory.
+
+ \return true if the file or the directory was copied, false otherwise.
+
+ \sa copy(const std::string &, const std::string &)
+
+*/
+bool
+vpIoTools::copy(const char *src, const char *dst)
+{
+ char cmd[FILENAME_MAX];
+ int ret;
+ // Check if we have to consider a file or a directory
+ if ( vpIoTools::checkFilename(src) ) {
+ //std::cout << "copy file: " << src << " in " << dst << std::endl;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ sprintf(cmd, "cp -p %s %s", src, dst);
+#elif defined(_WIN32)
+ std::string src_ = vpIoTools::path(src);
+ std::string dst_ = vpIoTools::path(dst);
+ sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
+#endif
+ ret = system( cmd );
+ //std::cout << cmd << " return value: " << ret << std::endl;
+ return true;
+ }
+ else if ( vpIoTools::checkDirectory(src) ) {
+ //std::cout << "copy directory: " << src << " in " << dst << std::endl;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ sprintf(cmd, "cp -p -r %s %s", src, dst);
+#elif defined(_WIN32)
+ std::string src_ = vpIoTools::path(src);
+ std::string dst_ = vpIoTools::path(dst);
+ sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
+#endif
+ ret = system( cmd );
+ if(ret) {}; // to avoid a warning
+ //std::cout << cmd << " return value: " << ret << std::endl;
+ return true;
+ }
+ else {
+ std::cout << "Cannot copy: " << src << " in " << dst << std::endl;
+ return false;
+ }
+}
+/*!
+
+ Copy a \e src file or directory in \e dst.
+
+ \param src : Existing file or directory to copy.
+ \param dst : New copied file or directory.
+
+ \return true if the file or the directory was copied, false otherwise.
+
+ \sa copy(const char *, const char *)
+
+*/
+bool
+vpIoTools::copy(const std::string &src, const std::string &dst)
+{
+ return vpIoTools::copy(src.c_str(), dst.c_str());
+}
+
+/*!
+
+ Remove a file or a directory.
+
+ \param file_or_dir : File or directory to remove.
+
+ \return true if the file or the directory was removed, false otherwise.
+
+ \sa remove(const std::string &)
+*/
+bool
+vpIoTools::remove(const char *file_or_dir)
+{
+ // Check if we have to consider a file or a directory
+ if ( vpIoTools::checkFilename(file_or_dir) ) {
+ //std::cout << "remove file: " << file_or_dir << std::endl;
+ if (::remove(file_or_dir) != 0)
+ return false;
+ else
+ return true;
+ }
+ else if ( vpIoTools::checkDirectory(file_or_dir) ) {
+ //std::cout << "remove directory: " << file_or_dir << std::endl;
+ char cmd[FILENAME_MAX];
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ sprintf(cmd, "rm -rf %s", file_or_dir);
+#elif defined(_WIN32)
+ std::string file_or_dir_ = vpIoTools::path(file_or_dir);
+ sprintf(cmd, "rmdir /S /Q %s", file_or_dir_.c_str());
+#endif
+ int ret = system( cmd );
+ if(ret) {}; // to avoid a warning
+ //std::cout << cmd << " return value: " << ret << std::endl;
+ return true;
+ }
+ else {
+ std::cout << "Cannot remove: " << file_or_dir << std::endl;
+ return false;
+ }
+}
+/*!
+
+ Remove a file or a directory.
+
+ \param file_or_dir : File or directory to remove.
+
+ \return true if the file or the directory was removed, false otherwise.
+
+ \sa remove(const char *)
+
+*/
+bool
+vpIoTools::remove(const std::string &file_or_dir)
+{
+ return vpIoTools::remove(file_or_dir.c_str());
+}
+
+/*!
+
+ Rename an existing file \e oldfilename in \e newfilename.
+
+ \param oldfilename : File to rename.
+ \param newfilename : New file name.
+
+ \return true if the file was renamed, false otherwise.
+
+ \sa rename(const std::string &, const std::string &)
+*/
+bool
+vpIoTools::rename(const char *oldfilename, const char *newfilename)
+{
+ if (::rename(oldfilename, newfilename) != 0)
+ return false;
+ else
+ return true;
+}
+
+/*!
+
+ Rename an existing file \e oldfilename in \e newfilename.
+
+ \param oldfilename : File to rename.
+ \param newfilename : New file name.
+
+ \return true if the file was renamed, false otherwise.
+
+ \sa rename(const char *, const char *)
+*/
+bool
+vpIoTools::rename(const std::string &oldfilename, const std::string &newfilename)
+{
+ return vpIoTools::rename(oldfilename.c_str(), newfilename.c_str());
+}
+
+
+
+/*!
+ Converts a path name to the current system's format.
+
+ \param pathname : Path name to convert. Path name to convert. Under
+ windows, converts all the "/" characters in the \e pathname string
+ into "\\" characters. Under Unix systems converts all the "\\"
+ characters in the \e pathname string into "/" characters.
+
+ \return The converted path name.
+
+ \sa path(const std::string &)
+*/
+std::string
+vpIoTools::path(const char *pathname)
+{
+ std::string path(pathname);
+
+#if defined(_WIN32)
+ for(unsigned int i=0 ; i<path.length() ; i++)
+ if( path[i] == '/') path[i] = '\\';
+#else
+ for(unsigned int i=0 ; i<path.length() ; i++)
+ if( path[i] == '\\') path[i] = '/';
+ wordexp_t exp_result;
+ wordexp(path.c_str(), &exp_result, 0);
+ path = std::string(exp_result.we_wordv[0]);
+ wordfree(&exp_result);
+#endif
+
+ return path;
+}
+
+/*!
+ Converts a path name to the current system's format.
+
+ \param pathname : Path name to convert. Under windows, converts all
+ the "/" characters in the \e pathname string into "\\"
+ characters. Under Unix systems converts all the "\\" characters in
+ the \e pathname string into "/" characters.
+
+ \return The converted path name.
+
+ \sa path(const char *)
+*/
+std::string
+vpIoTools::path(const std::string &pathname)
+{
+ return path(pathname.c_str());
+}
+
+
+/*!
+ Reads the configuration file and parses it.
+
+ \param confFile : path to the file containing the configuration parameters to parse.
+
+ \return true if succeed, false otherwise.
+ */
+bool vpIoTools::loadConfigFile(const std::string &confFile)
+{
+ configFile = path(confFile);
+ configVars.clear();configValues.clear();
+ std::ifstream confContent(configFile.c_str(), std::ios::in);
+
+ if(confContent.is_open())
+ {
+ std::string line,var,val;
+ long unsigned int k;
+ int c;
+ std::string stop[3] = {" ", "\t", "#"};
+ while(std::getline(confContent, line))
+ {
+ if((line.find("#",0,1) != 0) && (line.size() > 2))
+ {
+ try
+ {
+ // name of the variable
+ k = (unsigned long)line.find(" ");
+ var = line.substr(0,k);
+ // look for the end of the actual value
+ c = 200;
+ for(unsigned i=0;i<3;++i)
+ c = vpMath::minimum(c,(int)line.find(stop[i],k+1));
+ if(c==-1)
+ c = (int)line.size();
+ long unsigned int c_ = (long unsigned int) c;
+ val = line.substr(k+1,c_-k-1);
+ configVars.push_back(var);
+ configValues.push_back(val);
+ }
+ catch(...){}
+ }
+ }
+ confContent.close();
+ }
+ else {
+ return false;
+ }
+ return true;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e float.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, float &value)
+{
+ bool found = false;
+ for(unsigned int k=0;k<configVars.size() && found==false;++k)
+ {
+ if(configVars[k] == var)
+ {
+ if(configValues[k].compare("PI") == 0)
+ value = (float) M_PI;
+ else if(configValues[k].compare("PI/2") == 0)
+ value = (float) (M_PI/2.0);
+ else if(configValues[k].compare("-PI/2") == 0)
+ value = (float) (-M_PI/2.0);
+ else
+ value = (float) atof(configValues[k].c_str());
+ found = true;
+ }
+ }
+ if(found == false)
+ std::cout << var << " not found in config file" << std::endl;
+ return found;
+}
+/*!
+ Tries to read the parameter named \e var as a \e double.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, double &value)
+{
+ bool found = false;
+ for(unsigned int k=0;k<configVars.size() && found==false;++k)
+ {
+ if(configVars[k] == var)
+ {
+ if(configValues[k].compare("PI") == 0)
+ value = M_PI;
+ else if(configValues[k].compare("PI/2") == 0)
+ value = M_PI/2;
+ else if(configValues[k].compare("-PI/2") == 0)
+ value = -M_PI/2;
+ else
+ value = atof(configValues[k].c_str());
+ found = true;
+ }
+ }
+ if(found == false)
+ std::cout << var << " not found in config file" << std::endl;
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e int.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, int &value)
+{
+ bool found = false;
+ for(unsigned int k=0;k<configVars.size() && found==false;++k)
+ {
+ if(configVars[k] == var)
+ {
+ value = atoi(configValues[k].c_str());
+ found = true;
+ }
+ }
+ if(found == false)
+ std::cout << var << " not found in config file" << std::endl;
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e unsigned int.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, unsigned int &value)
+{
+ int v = 0;
+ bool found = readConfigVar(var,v);
+ value = (unsigned int) v;
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e bool.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, bool &value)
+{
+ int v = 0;
+ bool found = readConfigVar(var,v);
+ value = (v!=0);
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e vpColor.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read. See vpColor.cpp for the color number.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, vpColor &value)
+{
+ unsigned int v = 0;
+ bool found = readConfigVar(var,v);
+ value = vpColor::getColor(v);
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e std::string.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, std::string &value)
+{
+ bool found = false;
+ for(unsigned int k=0;k<configVars.size() && found==false;++k)
+ {
+ if(configVars[k] == var)
+ {
+ value = configValues[k];
+ found = true;
+ }
+ }
+ if(found == false)
+ std::cout << var << " not found in config file" << std::endl;
+ return found;
+}
+
+/*!
+ Tries to read the parameter named \e var as a \e vpMatrix.
+ If \e nCols and \e nRows are indicated, will resize the matrix.
+ Otherwise, will try to read as many values as indicated by the dimension of \e value.
+
+ \param var : Name of the parameter in the configuration file.
+ \param value : Value to be read.
+ \param nCols : Column dimension if resized.
+ \param nRows : Row dimension if resized
+
+ \return true if the parameter could be read.
+*/
+bool vpIoTools::readConfigVar(const std::string &var, vpArray2D<double> &value, const unsigned int &nCols, const unsigned int &nRows)
+{
+ bool found = false;
+ std::string nb;
+ for(unsigned int k=0;k<configVars.size() && found==false;++k)
+ {
+ if(configVars[k] == var)
+ {
+ found = true;
+ // resize or not
+ if(nCols != 0 && nRows != 0)
+ value.resize(nRows, nCols);
+ size_t ind=0,ind2;
+ for(unsigned int i=0;i<value.getRows();++i)
+ for(unsigned int j=0;j<value.getCols();++j)
+ {
+ ind2 = configValues[k].find(",",ind);
+ nb = configValues[k].substr(ind,ind2-ind);
+ if(nb.compare("PI") == 0)
+ value[i][j] = M_PI;
+ else if(nb.compare("PI/2") == 0)
+ value[i][j] = M_PI/2;
+ else if(nb.compare("-PI/2") == 0)
+ value[i][j] = -M_PI/2;
+ else
+ value[i][j] = atof(nb.c_str());
+ ind = ind2+1;
+ }
+ }
+ }
+ if(found == false)
+ std::cout << var << " not found in config file" << std::endl;
+ return found;
+}
+
+// construct experiment filename & path
+
+/*!
+ Augments the prefix of the experiment files by \e strTrue if \e cond is verified, and by \e strFalse otherwise.
+
+ \param strTrue : String to add if \e cond is true
+ \param cond : Condition managing the file name
+ \param strFalse : String to add if \e cond is false (default "")
+*/
+void vpIoTools::addNameElement(const std::string &strTrue, const bool &cond, const std::string &strFalse)
+{
+ if(cond)
+ baseName += "_" + strTrue;
+ else if(strFalse != "")
+ baseName += "_" + strFalse;
+}
+
+/*!
+ Augments the prefix of the experiment files by \e strTrue followed by \e val.
+
+ \param strTrue : String to add
+ \param val : Value to add
+
+*/
+void vpIoTools::addNameElement(const std::string &strTrue, const double &val)
+{
+ //if(val != 0.)
+ if(std::fabs(val) < std::numeric_limits<double>::epsilon())
+ {
+ char valC[256];
+ sprintf(valC, "%.3f", val);
+ std::string valS(valC);
+ baseName += "_" + strTrue + valS;
+ }
+}
+
+/*!
+ Creates the directory \e baseDir/baseName. If already exists, empties
+ it if \e empty is true.
+ Useful to save the images corresponding to a particular experiment.
+
+ \param empty : Indicates if the new directory has to be emptied
+
+*/
+void vpIoTools::createBaseNamePath(const bool &empty)
+{
+ if(vpIoTools::checkDirectory(baseDir + baseName) == false) {
+ vpIoTools::makeDirectory(baseDir + baseName);
+ std::cout << "creating directory " + baseDir + baseName << std::endl;
+ }
+ else {
+ if(empty) {
+ std::cout << "emptying directory " + baseDir + baseName << std::endl;
+ vpIoTools::remove(baseDir + baseName + "/*");
+ }
+ }
+}
+
+/*!
+ Copy the initial configuration file to the experiment directory.
+
+ \param actuallySave : If false, do not copy the file.
+
+*/
+void vpIoTools::saveConfigFile(const bool &actuallySave)
+{
+ if(actuallySave) {
+ std::string dest = baseDir + "/" + baseName + "_config.txt";
+ // file copy
+ vpIoTools::copy(configFile, dest);
+ }
+}
+
+/*!
+ Get ViSP images data path. ViSP images data can be installed from Debian or Ubuntu \e visp-images-data package.
+ It can be also installed from ViSP-images.zip that can be found on http://visp.inria.fr/download page.
+
+ This function returns the path to the folder that contains the data.
+ - It checks first if \e visp-images-data package is installed. In that case returns then \e /usr/share/visp-images-data".
+ - Then it checks if VISP_INPUT_IMAGE_PATH environment variable that gives the location of the data is set. In that
+ case returns the content of this environment var.
+
+ If the path is not found, returns an empty string.
+ */
+std::string vpIoTools::getViSPImagesDataPath()
+{
+ std::string data_path;
+ std::string file_to_test("ViSP-images/mbt/cube.cao");
+ std::string filename;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ // Test if visp-images-data package is u-installed (Ubuntu and Debian)
+ data_path = "/usr/share/visp-images-data";
+ filename = data_path + "/" + file_to_test;
+ if (vpIoTools::checkFilename(filename))
+ return data_path;
+#endif
+ // Test if VISP_INPUT_IMAGE_PATH env var is set
+ try {
+ data_path = vpIoTools::getenv("VISP_INPUT_IMAGE_PATH");
+ filename = data_path + "/" + file_to_test;
+ if (vpIoTools::checkFilename(filename))
+ return data_path;
+ }
+ catch(...) {
+ }
+ data_path = "";
+ return data_path;
+}
+
+/*!
+ Returns the extension of the file or an empty string if the file has no extension. If checkFile flag is set,
+ it will check first if the pathname denotes a directory and so return an empty string and second it will check
+ if the file denoted by the pathanme exists. If so, it will return the extension if present.
+ \param pathname : The pathname of the file we want to get the extension.
+ \param checkFile : If true, the file must exist otherwise an empty string will be returned.
+ \return The extension of the file or an empty string if the file has no extension.
+ or if the pathname is empty.
+ */
+std::string vpIoTools::getFileExtension(const std::string& pathname, const bool checkFile)
+{
+ if(checkFile && (vpIoTools::checkDirectory(pathname) || !vpIoTools::checkFilename(pathname))) {
+ return "";
+ }
+
+ //On Unix, or on the Mac
+ std::string sep = "/";
+ std::string altsep = "";
+ std::string extsep = ".";
+
+#if defined(_WIN32)
+ sep = "\\";
+ altsep = "/";
+ extsep = ".";
+#endif
+
+ //Python 2.7.8 module.
+//# Split a path in root and extension.
+//# The extension is everything starting at the last dot in the last
+//# pathname component; the root is everything before that.
+//# It is always true that root + ext == p.
+//
+//# Generic implementation of splitext, to be parametrized with
+//# the separators
+//def _splitext(p, sep, altsep, extsep):
+// """Split the extension from a pathname.
+//
+// Extension is everything from the last dot to the end, ignoring
+// leading dots. Returns "(root, ext)"; ext may be empty."""
+//
+// sepIndex = p.rfind(sep)
+// if altsep:
+// altsepIndex = p.rfind(altsep)
+// sepIndex = max(sepIndex, altsepIndex)
+//
+// dotIndex = p.rfind(extsep)
+// if dotIndex > sepIndex:
+// # skip all leading dots
+// filenameIndex = sepIndex + 1
+// while filenameIndex < dotIndex:
+// if p[filenameIndex] != extsep:
+// return p[:dotIndex], p[dotIndex:]
+// filenameIndex += 1
+//
+// return p, ''
+
+ int sepIndex = (int)pathname.rfind(sep);
+ if(!altsep.empty()) {
+ int altsepIndex = (int)pathname.rfind(altsep);
+ sepIndex = (std::max)(sepIndex, altsepIndex);
+ }
+
+ size_t dotIndex = pathname.rfind(extsep);
+ if(dotIndex != std::string::npos) {
+ //The extsep character exists
+ if((sepIndex != (int)std::string::npos && (int)dotIndex > sepIndex) || sepIndex == (int)std::string::npos) {
+ if(sepIndex == (int)std::string::npos) {
+ sepIndex = -1;
+ }
+ size_t filenameIndex = (size_t)(sepIndex + 1);
+
+ while(filenameIndex < dotIndex) {
+ if(pathname.compare(filenameIndex, 1, extsep) != 0) {
+ return pathname.substr(dotIndex);
+ }
+ filenameIndex++;
+ }
+ }
+ }
+
+
+ return "";
+}
+
+/*!
+ Returns the name of the file or directory denoted by this pathname.
+ \return The name of the file or directory denoted by this pathname, or an
+ empty string if this pathname's name sequence is empty.
+ */
+std::string vpIoTools::getName(const std::string& pathname)
+{
+ if(pathname.size() > 0)
+ {
+ std::string convertedPathname = vpIoTools::path(pathname);
+
+ size_t index = convertedPathname.find_last_of(vpIoTools::separator);
+ if(index != std::string::npos) {
+ return convertedPathname.substr(index + 1);
+ }
+
+ return convertedPathname;
+ }
+
+ return "";
+}
+
+/*!
+ Returns the name of the file without extension or directory denoted by this pathname.
+ \return The name of the file without extension or directory denoted by this pathname, or an
+ empty string if this pathname's name sequence is empty.
+ */
+std::string vpIoTools::getNameWE(const std::string& pathname)
+{
+ std::string name = vpIoTools::getName(pathname);
+ size_t found = name.find_last_of(".");
+ std::string name_we = name.substr(0, found);
+ return name_we;
+}
+
+/*!
+ Returns the pathname string of this pathname's parent.
+ \return The pathname string of this pathname's parent, or
+ an empty string if this pathname does not name a parent directory.
+ */
+std::string vpIoTools::getParent(const std::string& pathname)
+{
+ if(pathname.size() > 0)
+ {
+ std::string convertedPathname = vpIoTools::path(pathname);
+
+ size_t index = convertedPathname.find_last_of(vpIoTools::separator);
+ if(index != std::string::npos) {
+ return convertedPathname.substr(0, index);
+ }
+ }
+
+ return "";
+}
+
+/*!
+ Return the file path that corresponds to the concatenated
+ \e parent and \e child string files
+ by adding the corresponding separator for unix or windows.
+
+ The corresponding path is also converted. Under
+ windows, all the "/" characters are converted
+ into "\\" characters. Under Unix systems all the "\\"
+ characters are converted into "/" characters.
+ */
+std::string vpIoTools::createFilePath(const std::string& parent, const std::string child)
+{
+ if(child.size() == 0 && parent.size() == 0)
+ {
+ return "";
+ }
+
+ if(child.size() == 0)
+ {
+ return vpIoTools::path(parent);
+ }
+
+ if(parent.size() == 0)
+ {
+ return vpIoTools::path(child);
+ }
+
+ std::string convertedParent = vpIoTools::path(parent);
+ std::string convertedChild = vpIoTools::path(child);
+
+ std::stringstream ss;
+ ss << vpIoTools::separator;
+ std::string stringSeparator;
+ ss >> stringSeparator;
+
+ std::string lastConvertedParentChar = convertedParent.substr(convertedParent.size() - 1);
+ std::string firstConvertedChildChar = convertedChild.substr(0, 1);
+
+ if(lastConvertedParentChar == stringSeparator)
+ {
+ convertedParent = convertedParent.substr(0, convertedParent.size() - 1);
+ }
+
+ if(firstConvertedChildChar == stringSeparator)
+ {
+ convertedChild = convertedChild.substr(1);
+ }
+
+ return std::string(convertedParent + vpIoTools::separator + convertedChild);
+}
+
+/*!
+ Return whether a path is absolute.
+
+ \return true if the pathname is absolute, false otherwise.
+ */
+bool vpIoTools::isAbsolutePathname(const std::string& pathname)
+{
+ //# Inspired by the Python 2.7.8 module.
+ //# Return whether a path is absolute.
+ //# Trivial in Posix, harder on the Mac or MS-DOS.
+ //# For DOS it is absolute if it starts with a slash or backslash (current
+ //# volume), or if a pathname after the volume letter and colon / UNC resource
+ //# starts with a slash or backslash.
+ //
+ //def isabs(s):
+ // """Test whether a path is absolute"""
+ // s = splitdrive(s)[1]
+ // return s != '' and s[:1] in '/\\'
+ std::string path = splitDrive(pathname).second;
+ return path.size() > 0 && (path.substr(0, 1) == "/" || path.substr(0, 1) == "\\");
+}
+
+/*!
+ Split a path in a drive specification (a drive letter followed by a colon) and the path specification.
+ It is always true that drivespec + pathspec == p
+ Inspired by the Python 2.7.8 module.
+ \return a pair whose the first element is the drive specification and the second element
+ the path specification
+ */
+std::pair<std::string, std::string> vpIoTools::splitDrive(const std::string& pathname)
+{
+//# Split a path in a drive specification (a drive letter followed by a
+//# colon) and the path specification.
+//# It is always true that drivespec + pathspec == p
+//def splitdrive(p):
+// """Split a pathname into drive/UNC sharepoint and relative path specifiers.
+// Returns a 2-tuple (drive_or_unc, path); either part may be empty.
+//
+// If you assign
+// result = splitdrive(p)
+// It is always true that:
+// result[0] + result[1] == p
+//
+// If the path contained a drive letter, drive_or_unc will contain everything
+// up to and including the colon. e.g. splitdrive("c:/dir") returns ("c:", "/dir")
+//
+// If the path contained a UNC path, the drive_or_unc will contain the host name
+// and share up to but not including the fourth directory separator character.
+// e.g. splitdrive("//host/computer/dir") returns ("//host/computer", "/dir")
+//
+// Paths cannot contain both a drive letter and a UNC path.
+//
+// """
+// if len(p) > 1:
+// normp = p.replace(altsep, sep)
+// if (normp[0:2] == sep*2) and (normp[2] != sep):
+// # is a UNC path:
+// # vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
+// # \\machine\mountpoint\directory\etc\...
+// # directory ^^^^^^^^^^^^^^^
+// index = normp.find(sep, 2)
+// if index == -1:
+// return '', p
+// index2 = normp.find(sep, index + 1)
+// # a UNC path can't have two slashes in a row
+// # (after the initial two)
+// if index2 == index + 1:
+// return '', p
+// if index2 == -1:
+// index2 = len(p)
+// return p[:index2], p[index2:]
+// if normp[1] == ':':
+// return p[:2], p[2:]
+// return '', p
+
+
+ //On Unix, the drive is always empty.
+ //On the Mac, the drive is always empty (don't use the volume name -- it doesn't have the same
+ //syntactic and semantic oddities as DOS drive letters, such as there being a separate current directory per drive).
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+ return std::pair<std::string, std::string>("", pathname);
+#else
+ const std::string sep = "\\";
+ const std::string sepsep = "\\\\";
+ const std::string altsep = "/";
+
+ if(pathname.size() > 1) {
+ std::string normPathname = pathname;
+ std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), *sep.c_str());
+
+ if(normPathname.substr(0, 2) == sepsep && normPathname.substr(2, 1) != sep) {
+ // is a UNC path:
+ // vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
+ // \\machine\mountpoint\directory\etc\...
+ // directory ^^^^^^^^^^^^^^^
+ size_t index = normPathname.find(sep, 2);
+ if(index == std::string::npos) {
+ return std::pair<std::string, std::string>("", pathname);
+ }
+
+ size_t index2 = normPathname.find(sep, index + 1);
+ //# a UNC path can't have two slashes in a row
+ //# (after the initial two)
+ if(index2 == index + 1) {
+ return std::pair<std::string, std::string>("", pathname);
+ }
+
+ if(index2 == std::string::npos) {
+ index2 = pathname.size();
+ }
+
+ return std::pair<std::string, std::string>(pathname.substr(0, index2), pathname.substr(index2));
+ }
+
+ if(normPathname[1] == ':') {
+ return std::pair<std::string, std::string>(pathname.substr(0, 2), pathname.substr(2));
+ }
+ }
+
+ return std::pair<std::string, std::string>("", pathname);
+#endif
+}
+
+/*!
+ Split a chain.
+ \param chain : Input chain to split.
+ \param sep : Character separator.
+ \return A vector that contains all the subchains.
+
+ The following code shows how to use this function:
+ \code
+#include <visp3/core/vpIoTools.h>
+
+int main()
+{
+ {
+ std::string chain("/home/user;/usr/local/include;/usr/include");
+ std::string sep = ";";
+
+ std::vector<std::string> subChain = vpIoTools::splitChain(chain, sep);
+ std::cout << "Found the following subchains: " << std::endl;
+ for (size_t i=0; i < subChain.size(); i++)
+ std::cout << subChain[i] << std::endl;
+ }
+
+ {
+ std::string chain("This is an other example");
+ std::string sep = " ";
+
+ std::vector<std::string> subChain = vpIoTools::splitChain(chain, sep);
+ std::cout << "Found the following subchains: " << std::endl;
+ for (size_t i=0; i < subChain.size(); i++)
+ std::cout << subChain[i] << std::endl;
+ }
+}
+ \endcode
+
+ It produces the following output:
+ \code
+Found the following subchains:
+/home/user
+/usr/local/include
+/usr/include
+Found the following subchains:
+This
+is
+an
+other
+example
+ \endcode
+ */
+std::vector<std::string> vpIoTools::splitChain(const std::string & chain, const std::string & sep)
+{
+ size_t startIndex = 0;
+
+ std::string chainToSplit = chain;
+ std::vector<std::string> subChain;
+ size_t sepIndex = chainToSplit.find(sep);
+
+ while(sepIndex != std::string::npos) {
+
+ subChain.push_back( chainToSplit.substr(startIndex, sepIndex) );
+ chainToSplit = chainToSplit.substr(sepIndex+1, chain.size()-1);
+
+ sepIndex = chainToSplit.find(sep);
+ }
+ subChain.push_back(chainToSplit);
+
+ return subChain;
+}
diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp
new file mode 100644
index 0000000..045138b
--- /dev/null
+++ b/modules/core/src/tools/geometry/vpPlane.cpp
@@ -0,0 +1,392 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Plane geometrical structure.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpPlane.cpp
+ \brief definition of the vpPlane class member functions
+ \ingroup libtools
+*/
+
+#include <visp3/core/vpPlane.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+/*!
+ Copy operator.
+*/
+vpPlane&
+vpPlane::operator =(const vpPlane& p)
+{
+ A = p.A ;
+ B = p.B ;
+ C = p.C ;
+ D = p.D ;
+
+ return *this ;
+}
+
+/*!
+ Basic constructor that set the plane parameters A, B, C, D to zero.
+*/
+vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
+
+/*!
+ Plane constructor from A, B, C, D parameters.
+
+ A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
+ (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal
+ vector of the plane.
+
+ \param a, b, c, d : Parameters of the plane.
+
+*/
+vpPlane::vpPlane(const double a,const double b,const double c, const double d)
+ : A(a), B(b), C(c), D(d) {}
+
+/*!
+ Copy constructor.
+*/
+vpPlane::vpPlane(const vpPlane& P)
+ : A(0), B(0), C(0), D(0)
+{
+ setA(P.getA()) ;
+ setB(P.getB()) ;
+ setC(P.getC()) ;
+ setD(P.getD()) ;
+}
+
+/*!
+
+ Plane constructor from a point \e P on the plane and the normal
+ \e n to the plane.
+
+ A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
+ (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal
+ vector of the plane.
+
+ \param P : A point with coordinates (x,y,z) on the plane. The \e frame parameter indicates
+ if the coordinates of this points that are used are expressed in the camera of object frame.
+
+ \param n : The normal to the plane.
+
+ \param frame: Indicates if the plane should be initialized from the point P
+ coordinates expressed in the camera or object frame.
+
+*/
+vpPlane::vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame)
+ : A(0), B(0), C(0), D(0)
+{
+ //Equation of the plane is given by:
+ A = n[0];
+ B = n[1];
+ C = n[2];
+
+ if (frame == vpPlane::camera_frame)
+ D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
+ else
+ D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
+
+}
+
+/*!
+ Initialize the plane with the parameters of an other plane \e P.
+
+ \param P : Plane used as initializer.
+*/
+void vpPlane::init(const vpPlane& P)
+{
+ setA(P.getA()) ;
+ setB(P.getB()) ;
+ setC(P.getC()) ;
+ setD(P.getD()) ;
+}
+
+/*!
+ Initialize the plane from a point \e P on the plane and the normal
+ \e n to the plane.
+
+ \param P : A point with coordinates (x,y,z) on the plane.
+ The size of the vector should be 3, with P[0]=x, with P[1]=y, with P[2]=z.
+
+ \param n : The normal to the plane.
+
+ \sa vpPlane(const vpPoint&, const vpColVector &)
+*/
+void vpPlane::init(const vpColVector & P, const vpColVector &n)
+{
+ //Equation of the plane is given by:
+ A = n[0];
+ B = n[1];
+ C = n[2];
+
+ D=-(A*P[0]+B*P[1]+C*P[2]);
+}
+
+/*!
+ Compute the equation of a plane given three point P, Q, R.
+
+ The normal to the plane is given by:
+ n = PQ x PR
+
+ \param P,Q,R: Three points on the plane.
+ \param frame: Indicates if the plane should be initialized from the points
+ coordinates expressed in the camera or object frame.
+
+*/
+void
+vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
+{
+ vpColVector a(3);
+ vpColVector b(3);
+ vpColVector n(3);
+ if (frame == vpPlane::camera_frame) {
+ //Calculate vector corresponding to PQ
+ a[0]=P.get_X()-Q.get_X();
+ a[1]=P.get_Y()-Q.get_Y();
+ a[2]=P.get_Z()-Q.get_Z();
+
+ //Calculate vector corresponding to PR
+ b[0]=P.get_X()-R.get_X();
+ b[1]=P.get_Y()-R.get_Y();
+ b[2]=P.get_Z()-R.get_Z();
+ }
+ else {
+ //Calculate vector corresponding to PQ
+ a[0]=P.get_oX()-Q.get_oX();
+ a[1]=P.get_oY()-Q.get_oY();
+ a[2]=P.get_oZ()-Q.get_oZ();
+
+ //Calculate vector corresponding to PR
+ b[0]=P.get_oX()-R.get_oX();
+ b[1]=P.get_oY()-R.get_oY();
+ b[2]=P.get_oZ()-R.get_oZ();
+ }
+ //Calculate normal vector to plane PQ x PR
+ n=vpColVector::cross(a,b);
+
+ //Equation of the plane is given by:
+ A = n[0];
+ B = n[1];
+ C = n[2];
+ if (frame == vpPlane::camera_frame)
+ D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
+ else
+ D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
+
+ double norm = sqrt(A*A+B*B+C*C) ;
+ A /= norm ;
+ B /= norm ;
+ C /= norm ;
+ D /= norm ;
+}
+
+
+/*!
+ Compute the equation of a plane given three point P, Q, R.
+
+ The normal to the plane is given by:
+ n = PQ x PR
+
+ \param P,Q,R: Three points on the plane.
+ \param frame: Indicates if the plane should be initialized from the points
+ coordinates expressed in the camera or object frame.
+
+ \sa init(const vpPoint &, const vpPoint &, const vpPoint &)
+*/
+vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
+ : A(0), B(0), C(0), D(0)
+{
+ init(P, Q, R, frame) ;
+}
+
+/*!
+ Return the normal to the plane.
+
+ A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
+ (x,y,z) is a point of R^3 and (A,B,C) are the coordinates of the normal.
+
+ \sa getNormal(vpColVector &n)
+*/
+vpColVector vpPlane::getNormal() const
+{
+ vpColVector n(3);
+ n[0] = A ;
+ n[1] = B ;
+ n[2] = C ;
+
+ return n ;
+}
+
+/*!
+ Return the normal to the plane.
+
+ A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
+ (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is normal
+ vector of the plane.
+
+ \sa getNormal()
+
+*/
+void vpPlane::getNormal(vpColVector &n) const
+{
+ n.resize(3) ;
+ n[0] = A ;
+ n[1] = B ;
+ n[2] = C ;
+}
+
+/*!
+ Compute the coordinates of the projection of a point on the plane.
+
+ \param P : point to be projected on the plane
+ \param Pproj : result of the projection (pproj belongs to the plane)
+*/
+void
+vpPlane::projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const
+{
+ double x0,y0,z0 ;
+ double rho ;
+
+ x0 = P.get_X()/P.get_W() ;
+ y0 = P.get_Y()/P.get_W() ;
+ z0 = P.get_Z()/P.get_W() ;
+
+ rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
+
+ Pproj.set_X(x0+A*rho) ;
+ Pproj.set_Y(y0+B*rho) ;
+ Pproj.set_Z(z0+C*rho) ;
+ Pproj.set_W(1) ;
+}
+
+
+double
+vpPlane::rayIntersection(const vpPoint &M0,
+ const vpPoint &M1,
+ vpColVector &H ) const
+{
+
+ double k,scal;
+ double R[3];
+
+ // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
+ if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
+ || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
+ || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
+ {
+ R[0]= M1.get_X() - M0.get_X();
+ R[1]= M1.get_Y() - M0.get_Y();
+ R[2]= M1.get_Z() - M0.get_Z();
+
+ scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
+ //if (scal != 0)
+ if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
+ k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
+ else
+ k = 0;
+
+ H[0] = M0.get_X()+ k*R[0];
+ H[1] = M0.get_Y()+ k*R[1];
+ H[2] = M0.get_Z()+ k*R[2];
+ }
+ else
+ {
+ scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
+ //if (scal != 0)
+ if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
+ k = -getD()/scal;
+ else
+ k=0;
+ H[0] = k*M1.get_X();
+ H[1] = k*M1.get_Y();
+ H[2] = k*M1.get_Z();
+ }
+
+ return k;
+
+}
+
+double vpPlane::getIntersection(const vpColVector &M1,vpColVector &H )const
+{
+
+ double k,scal;
+
+ scal = A*M1[0] + B*M1[1] + C*M1[2];
+ //if (scal != 0)
+ if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
+ k = -getD()/scal;
+ else
+ k=0;
+ H[0] = k*M1[0];
+ H[1] = k*M1[1];
+ H[2] = k*M1[2];
+
+ return k;
+
+}
+
+/*!
+
+ Considering the plane in the Ro frame computes the equation of the
+ plane in the Rc frame.
+
+ \param cMo : Homogeneous transformation from Rc to Ro frames.
+
+*/
+void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ // Save current plane parameters
+ double Ao = A, Bo = B, Co = C, Do =D ;
+ A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
+ B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
+ C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
+ D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
+}
+
+/*!
+
+ Print the plane parameters as a stream like "(A,B,C,D) " where
+ A,B,C and D correspond to the parameters of the plane.
+
+*/
+VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
+{
+ return (os << "("<<p.getA() << ","<<p.getB()
+ << ","<<p.getC()<< ","<<p.getD() <<") ") ;
+} ;
diff --git a/modules/core/src/tools/geometry/vpPolygon.cpp b/modules/core/src/tools/geometry/vpPolygon.cpp
new file mode 100644
index 0000000..3a8ff33
--- /dev/null
+++ b/modules/core/src/tools/geometry/vpPolygon.cpp
@@ -0,0 +1,469 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a generic 2D polygon.
+ *
+ * Author:
+ * Amaury Dame
+ * Nicolas Melchior
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpUniRand.h>
+#include <set>
+#include <limits>
+/*!
+ Basic constructor.
+
+ By default, it defines a triangle with the three 2D points coordinates :
+ \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
+*/
+vpPolygon::vpPolygon()
+ : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
+{
+ std::vector<vpImagePoint> corners;
+ corners.push_back(vpImagePoint(0,0));
+ corners.push_back(vpImagePoint(1,0));
+ corners.push_back(vpImagePoint(0,1));
+ init(corners);
+}
+
+/*!
+ Constructor which initialise the polygon thanks to the given corners.
+
+ \warning the corners must be ordered (either clockwise or counter clockwise).
+
+ \param corners : The Points defining the corners.
+*/
+vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
+ : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
+{
+ if(corners.size() < 3){
+ _goodPoly = false;
+ }
+ init(corners);
+}
+
+/*!
+ Copy constructor
+
+ \param poly : The polygon used for the initialisation.
+*/
+vpPolygon::vpPolygon(const vpPolygon &poly)
+ : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
+{
+ _corners = poly._corners;
+ _center = poly._center;
+ _area = poly._area;
+ _goodPoly = poly._goodPoly;
+ _bbox = poly._bbox;
+}
+
+/*!
+ Basic destructor
+*/
+vpPolygon::~vpPolygon()
+{
+}
+
+/*!
+ Equal operator.
+
+ Assign \e poly to this polygon and return a reference to it.
+*/
+vpPolygon &
+vpPolygon::operator=(const vpPolygon& poly)
+{
+ _corners = poly._corners;
+ _center = poly._center;
+ _area = poly._area;
+ _goodPoly = poly._goodPoly;
+ return *this;
+};
+
+/*!
+ Initialise the triangle thanks to the collection of 2D points (in pixel).
+
+ \warning the corners must be ordered (either clockwise or counter clockwise).
+
+ \param corners : The corners of the polyon.
+*/
+void
+vpPolygon::buildFrom(const std::vector<vpImagePoint>& corners)
+{
+ init(corners);
+}
+
+/*!
+ Initialise the triangle thanks to the collection of 2D points (in meter). The
+ fields \e x and \e y are used to compute the corresponding coordinates in
+ pixel thanks to the camera parameters \e cam.
+
+ \warning the corners must be ordered (either clockwise or counter clockwise).
+
+ \param corners : The corners of the polyon.
+ \param cam : The camera parameters used to convert the coordinates from meter
+ to pixel.
+*/
+void
+vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
+{
+ std::vector<vpImagePoint> ipCorners(corners.size());
+ for(unsigned int i=0; i<corners.size(); ++i){
+ vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
+ }
+ buildFrom(ipCorners);
+}
+
+/*!
+ Initialise the polygon by (left-)clicking to add a corners to the polygon.
+ A right click is used to stop the addition of new corner.
+
+ \param I : The image where to click to initialise the corners.
+*/
+void
+vpPolygon::initClick(const vpImage<unsigned char>& I)
+{
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
+ vpImagePoint ip;
+
+ std::vector<vpImagePoint> cornersClick;
+
+ while(button == vpMouseButton::button1){
+ bool ret = vpDisplay::getClick(I, ip, button, true);
+ if(ret && button == vpMouseButton::button1){
+ vpDisplay::displayCross(I, ip, 5, vpColor::red);
+ cornersClick.push_back(ip);
+ vpDisplay::flush(I);
+ }
+ }
+
+ buildFrom(cornersClick);
+}
+
+
+/*!
+ Intialise the polygon using the collection of image points. This method
+ compute come internal variables such as center, area, ...
+
+ \warning the corners must be ordered (either clockwise or counter clockwise).
+
+ \param corners : The corners of the polyon.
+*/
+void
+vpPolygon::init(const std::vector<vpImagePoint>& corners)
+{
+ _corners = corners;
+
+ updateBoundingBox();
+ updateArea();
+ updateCenter();
+}
+
+
+
+/*!
+ Test if two segments are intersecting.
+
+ \throw vpException::divideByZeroError if the two lines are aligned (
+ denominator equal to zero).
+
+ \param ip1 : The first image point of the first segment.
+ \param ip2 : The second image point of the first segment.
+ \param ip3 : The first image point of the second segment.
+ \param ip4 : The second image point of the second segment.
+*/
+bool
+vpPolygon::testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4)
+{
+ double di1 = ip2.get_i() - ip1.get_i();
+ double dj1 = ip2.get_j() - ip1.get_j();
+
+ double di2 = ip4.get_i() - ip3.get_i();
+ double dj2 = ip4.get_j() - ip3.get_j();
+
+ double denominator = di1 * dj2 - dj1 * di2;
+
+ if(fabs(denominator) < std::numeric_limits<double>::epsilon()){
+ throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
+ }
+
+ double alpha = - ( ( ip1.get_i() - ip3.get_i() ) * dj2 + di2 * ( ip3.get_j() - ip1.get_j())) / denominator;
+ if(alpha < 0 || alpha >= 1){
+ return false;
+ }
+
+ double beta = - (di1 * (ip3.get_j() - ip1.get_j() ) + dj1 * (ip1.get_i() - ip3.get_i()) ) / denominator;
+ if(beta < 0 || beta >= 1){
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ Check if the 2D point \f$ iP \f$ is inside the polygon.
+
+ \param ip : The point which have to be tested.
+
+ \return Returns true if the point is inside the triangle, false otherwise.
+*/
+bool
+vpPolygon::isInside(const vpImagePoint& ip)
+{
+ vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
+ vpUniRand generator;
+ infPoint.set_i( infPoint.get_i() + 1000 * generator());
+ infPoint.set_j( infPoint.get_j() + 1000 * generator());// we add random since it appears that sometimes infPoint may cause a degenerated case (so realucnch and hope that result will be different).
+
+ unsigned int nbinterscetion = 0;
+ for(unsigned int i=0; i<_corners.size(); ++i){
+ vpImagePoint ip1 = _corners[i];
+ vpImagePoint ip2 = _corners[(i+1)%_corners.size()];
+ bool intersection = false;
+
+ // If the points are the same we continue without trying to found
+ // an intersection
+ if (ip1 == ip2)
+ continue;
+
+ try{
+ intersection = testIntersectionSegments(ip1, ip2, ip, infPoint );
+ }catch(vpException e){
+ return isInside(ip);
+ }
+
+ if(intersection){
+ ++nbinterscetion;
+ }
+ }
+
+ return ((nbinterscetion%2)==1);
+}
+
+
+
+/*!
+ Update the _area attribute of the polygon using the corners.
+
+ The area is computed using the formula:
+ \f[
+ A = \frac{1}{2} \sum_{i=0}^{n-1} (x_1 y_{i+1} - x_{i+1} y_{i})
+ \f]
+
+*/
+void
+vpPolygon::updateArea()
+{
+ if(_corners.size() == 0){
+ _area = 0;
+ _goodPoly = false;
+ return;
+ }
+ _area = 0;
+
+ for(unsigned int i=0; i<_corners.size(); ++i){
+ unsigned int i_p_1 = ( i+1 ) % _corners.size();
+ _area += _corners[i].get_j() * _corners[i_p_1].get_i()
+ - _corners[i_p_1].get_j() * _corners[i].get_i();
+ }
+
+ _area /= 2;
+ if(_area < 0){
+ _area = - _area;
+ }
+}
+
+
+/*!
+ Update the _center attribute of the polygon using the corners.
+
+ The i coordinates is computed using:
+
+ \f[
+ i = \frac{1}{6{area}} \sum_{i=0}^{n-1} (i_i + i_{i+1})(i_{i+1} j_i - j_{i+1} i_i)
+ \f]
+
+ The computation of the j coordinate is similar.
+*/
+void
+vpPolygon::updateCenter()
+{
+ if(_corners.size() == 0){
+ _center = vpImagePoint(0, 0);
+ _goodPoly = false;
+ return;
+ }
+ double i_tmp = 0;
+ double j_tmp = 0;
+#if 0
+ for(unsigned int i=0; i<(_corners.size()-1); ++i){
+ i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
+ (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
+
+ j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
+ (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
+ }
+#else
+ for(unsigned int i=0; i<_corners.size(); ++i){
+ unsigned int i_p_1 = ( i+1 ) % _corners.size();
+ i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
+ (_corners[i_p_1].get_i() * _corners[i].get_j()
+ - _corners[i_p_1].get_j() * _corners[i].get_i());
+
+ j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
+ (_corners[i_p_1].get_i() * _corners[i].get_j()
+ - _corners[i_p_1].get_j() * _corners[i].get_i());
+ }
+#endif
+
+ if(_area > 0){
+ _center.set_i(fabs(i_tmp / (6 * _area)));
+ _center.set_j(fabs(j_tmp / (6 * _area)));
+ }else{
+ _center = _corners[0];
+ _goodPoly = false;
+ }
+}
+
+void
+vpPolygon::updateBoundingBox()
+{
+ if(_corners.size() == 0){
+ _bbox.setBottomRight(vpImagePoint(0, 0));
+ _bbox.setTopLeft(vpImagePoint(0, 0));
+ _goodPoly = false;
+ return;
+ }
+
+ std::set<double> setI;
+ std::set<double> setJ;
+ for(unsigned int i=0; i<_corners.size(); ++i){
+ setI.insert(_corners[i].get_i());
+ setJ.insert(_corners[i].get_j());
+ }
+ vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
+ std::set<double>::const_iterator iterI = setI.end();
+ std::set<double>::const_iterator iterJ = setJ.end();
+ --iterI;
+ --iterJ;
+ vpImagePoint br(*iterI, *iterJ);
+
+ if(tl == br){
+ _goodPoly = false;
+ }
+ _bbox.setTopLeft(tl);
+ _bbox.setBottomRight(br);
+}
+
+/*!
+ Display the polygon in the image (overlay, so the image is not modified).
+ A call to the flush() method is necessary.
+
+ \param I : The image where is displayed the polygon.
+ \param color : The color of the lines of the polygon.
+ \param thickness : The thickness of the lines used to display the polygon.
+*/
+void
+vpPolygon::display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness) const
+{
+ const unsigned int N = (unsigned int)_corners.size();
+ for(unsigned int i=0; i<N; ++i){
+ vpDisplay::displayLine(I, _corners[i], _corners[(i+1)%N], color, thickness);
+ }
+}
+
+bool
+vpPolygon::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i_test, const double &j_test, const double &i, const double &j)
+{
+ double dx = p2.get_j() - p1.get_j();
+ double dy = p2.get_i() - p1.get_i();
+ double ex = j - j_test;
+ double ey = i - i_test;
+
+ double den = dx * ey - dy * ex;
+ double t = 0, u = 0;
+ //if(den != 0){
+ if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
+ t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
+ u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
+ }
+ else{
+ throw vpException(vpException::divideByZeroError, "denominator null");
+ }
+ return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
+}
+
+/*!
+ Test if an image point is inside a 2D polygon.
+
+ \param roi : List of the polygon corners.
+ \param i : i-coordinate of the image point to test.
+ \param j : j-coordinate of the image point to test.
+
+ \return True if the point defined by (i,j) is inside the polygon, False otherwise.
+*/
+bool
+vpPolygon::isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j)
+{
+ double i_test = 100000.;
+ double j_test = 100000.;
+ unsigned int nbInter = 0;
+ bool computeAgain = true;
+
+ if(computeAgain){
+ computeAgain = false;
+ for(unsigned int k=0; k< roi.size(); k++){
+ try{
+ if(vpPolygon::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
+ nbInter++;
+ }
+ }
+ catch(...){
+ computeAgain = true;
+ break;
+ }
+ }
+
+ if(computeAgain){
+ i_test += 100;
+ j_test -= 100;
+ nbInter = 0;
+ }
+ }
+ return ((nbInter%2) == 1);
+}
diff --git a/modules/core/src/tools/geometry/vpPolygon3D.cpp b/modules/core/src/tools/geometry/vpPolygon3D.cpp
new file mode 100644
index 0000000..a2e0bc5
--- /dev/null
+++ b/modules/core/src/tools/geometry/vpPolygon3D.cpp
@@ -0,0 +1,650 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <limits.h>
+
+#include <visp3/core/vpConfig.h>
+/*!
+ \file vpPolygon3D.cpp
+ \brief Implements a polygon of the model used by the model-based tracker.
+*/
+
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/core/vpPolygon.h>
+
+/*!
+ Basic constructor.
+*/
+vpPolygon3D::vpPolygon3D()
+ : nbpt(0), nbCornersInsidePrev(0),
+ p(NULL), polyClipped(), clippingFlag(vpPolygon3D::NO_CLIPPING),
+ distNearClip(0.001), distFarClip(100.)
+{
+}
+
+vpPolygon3D::vpPolygon3D(const vpPolygon3D& mbtp)
+ : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev),
+ p(NULL), polyClipped(mbtp.polyClipped), clippingFlag(mbtp.clippingFlag),
+ distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip)
+{
+ if (p) delete [] p;
+ p = new vpPoint [nbpt];
+ for(unsigned int i = 0; i < nbpt; i++)
+ p[i] = mbtp.p[i];
+}
+
+vpPolygon3D& vpPolygon3D::operator=(const vpPolygon3D& mbtp)
+{
+ nbpt = mbtp.nbpt;
+ nbCornersInsidePrev = mbtp.nbCornersInsidePrev;
+ polyClipped = mbtp.polyClipped;
+ clippingFlag = mbtp.clippingFlag;
+ distNearClip = mbtp.distNearClip;
+ distFarClip = mbtp.distFarClip;
+
+ if (p) delete [] p;
+ p = new vpPoint [nbpt];
+ for(unsigned int i = 0; i < nbpt; i++)
+ p[i] = mbtp.p[i];
+
+ return (*this);
+}
+
+/*!
+ Basic destructor.
+*/
+vpPolygon3D::~vpPolygon3D()
+{
+ if (p !=NULL)
+ {
+ delete[] p;
+ p = NULL;
+ }
+}
+
+/*!
+ Get a reference to a corner.
+
+ \throw vpException::dimensionError if the _index is out of range.
+
+ \param _index : the index of the corner
+*/
+vpPoint &
+vpPolygon3D::getPoint(const unsigned int _index)
+{
+ if(_index >= nbpt){
+ throw vpException(vpException::dimensionError, "index out of range");
+ }
+ return p[_index];
+}
+
+/*!
+ Set the number of points which are the corners of the polygon.
+
+ \param nb : The number of corners.
+*/
+void
+vpPolygon3D::setNbPoint(const unsigned int nb)
+{
+ nbpt = nb ;
+ if (p != NULL)
+ delete[] p;
+ p = new vpPoint[nb] ;
+}
+
+/*!
+ Add a corner point to the list of polygon's corners.
+
+ \param n : The index of the corner.
+ \param P : The point to add.
+*/
+void
+vpPolygon3D::addPoint(const unsigned int n, const vpPoint &P)
+{
+ //if( p!NULL && n < nbpt )
+ p[n] = P ;
+}
+
+/*!
+ Project the 3D corner points into the image thanks to the pose of the camera.
+
+ \param cMo : The pose of the camera.
+*/
+void
+vpPolygon3D::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ for (unsigned int i = 0 ; i < nbpt ; i++)
+ {
+ p[i].changeFrame(cMo) ;
+ p[i].projection() ;
+ }
+}
+
+/*!
+ Compute the region of interest in the image according to the used clipping.
+
+ \warning If the FOV clipping is used, camera normals have to be precomputed.
+
+ \param cam : camera parameters used to compute the field of view.
+*/
+void
+vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam)
+{
+ polyClipped.clear();
+ std::vector<vpColVector> fovNormals;
+ std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp;
+ std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp2;
+
+ if(cam.isFovComputed() && clippingFlag > 3)
+ fovNormals = cam.getFovNormals();
+
+ for(unsigned int i = 0 ; i < nbpt ; i++){
+ p[i%nbpt].projection();
+ polyClippedTemp.push_back(std::make_pair(p[i%nbpt],vpPolygon3D::NO_CLIPPING));
+ }
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ for(unsigned int i = 1 ; i < 64 ; i=i*2)
+ {
+ if(((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i==1)))
+ {
+ for(unsigned int j = 0 ; j < polyClippedTemp.size() ; j++)
+ {
+ vpPoint p1Clipped = polyClippedTemp[j].first;
+ vpPoint p2Clipped = polyClippedTemp[(j+1)%polyClippedTemp.size()].first;
+
+ unsigned int p2ClippedInfoBefore = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
+ unsigned int p1ClippedInfo = polyClippedTemp[j].second;
+ unsigned int p2ClippedInfo = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
+
+ bool problem = true;
+
+ switch(i){
+ case 1:
+ problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ i, distNearClip));
+ break;
+ case 2:
+ problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ i, distFarClip));
+ break;
+ case 4:
+ problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
+ break;
+ case 8:
+ problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
+ break;
+ case 16:
+ problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ fovNormals[2], vpPolygon3D::UP_CLIPPING));
+ break;
+ case 32:
+ problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
+ fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
+ break;
+ }
+
+ if(!problem)
+ {
+ p1Clipped.projection();
+ polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
+
+ if(p2ClippedInfo != p2ClippedInfoBefore)
+ {
+ p2Clipped.projection();
+ polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
+ }
+
+ if(nbpt == 2){
+ if(p2ClippedInfo == p2ClippedInfoBefore)
+ {
+ p2Clipped.projection();
+ polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
+ }
+ break;
+ }
+ }
+ }
+
+ polyClippedTemp = polyClippedTemp2;
+ polyClippedTemp2.clear();
+ }
+ }
+
+ polyClipped = polyClippedTemp;
+}
+
+/*!
+ Get the clipped points according to a plane equation.
+
+ \param cam : camera parameters
+ \param p1 : First extremity of the line.
+ \param p2 : Second extremity of the line.
+ \param p1Clipped : Resulting p1.
+ \param p2Clipped : Resulting p2.
+ \param p1ClippedInfo : Resulting clipping flag for p1.
+ \param p2ClippedInfo : Resulting clipping flag for p2.
+ \param A : Param A from plane equation.
+ \param B : Param B from plane equation.
+ \param C : Param C from plane equation.
+ \param D : Param D from plane equation.
+ \param flag : flag specifying the clipping used when calling this function.
+
+ \return True if the points have been clipped, False otherwise
+*/
+bool
+vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
+ vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
+ const vpColVector &normal, const unsigned int &flag)
+{
+ vpRowVector p1Vec(3);
+ p1Vec[0] = p1.get_X(); p1Vec[1] = p1.get_Y(); p1Vec[2] = p1.get_Z();
+ p1Vec = p1Vec.normalize();
+
+ vpRowVector p2Vec(3);
+ p2Vec[0] = p2.get_X(); p2Vec[1] = p2.get_Y(); p2Vec[2] = p2.get_Z();
+ p2Vec = p2Vec.normalize();
+
+ if((clippingFlag & flag) == flag){
+ double beta1 = acos( p1Vec * normal );
+ double beta2 = acos( p2Vec * normal );
+
+// std::cout << beta1 << " && " << beta2 << std::endl;
+
+ // if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
+ if(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
+ return false;
+ else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0){
+ vpPoint pClipped;
+ double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
+ t = t / ( normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) + normal[2] * (p2.get_Z() - p1.get_Z()) );
+
+ pClipped.set_X((p2.get_X() - p1.get_X())*t + p1.get_X());
+ pClipped.set_Y((p2.get_Y() - p1.get_Y())*t + p1.get_Y());
+ pClipped.set_Z((p2.get_Z() - p1.get_Z())*t + p1.get_Z());
+
+ if(beta1 < M_PI / 2.0){
+ p1ClippedInfo = p1ClippedInfo | flag;
+ p1Clipped = pClipped;
+ }
+ else{
+ p2ClippedInfo = p2ClippedInfo | flag;
+ p2Clipped = pClipped;
+ }
+ }
+ }
+
+ return true;
+}
+
+bool
+vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
+ vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
+ const unsigned int &flag, const double &distance)
+{
+ // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
+ // to avoid a valgrind "Source and destination overlap in memcpy" error,
+ // we introduce a two temporary points.
+ vpPoint p1Clipped_, p2Clipped_;
+ p1Clipped_ = p1;
+ p2Clipped_ = p2;
+
+ p1Clipped = p1Clipped_;
+ p2Clipped = p2Clipped_;
+
+
+ bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
+ if(flag == vpPolygon3D::FAR_CLIPPING)
+ test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
+
+ bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
+ if(flag == vpPolygon3D::FAR_CLIPPING)
+ test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
+
+ bool test3 = (p1Clipped.get_Z() < distance);
+ if(flag == vpPolygon3D::FAR_CLIPPING)
+ test3 = (p1Clipped.get_Z() > distance);
+
+ if(test1)
+ return false;
+
+ else if(test2){
+ vpPoint pClippedNear;
+ double t;
+ t = (p2Clipped.get_Z() - p1Clipped.get_Z());
+ t = (distance - p1Clipped.get_Z()) / t;
+
+ pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X())*t + p1Clipped.get_X());
+ pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y())*t + p1Clipped.get_Y());
+ pClippedNear.set_Z(distance);
+
+ if(test3){
+ p1Clipped = pClippedNear;
+ if(flag == vpPolygon3D::FAR_CLIPPING)
+ p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
+ else
+ p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
+ }
+ else{
+ p2Clipped = pClippedNear;
+ if(flag == vpPolygon3D::FAR_CLIPPING)
+ p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
+ else
+ p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
+ }
+ }
+
+ return true;
+}
+
+/*!
+ Get the region of interest in the image.
+
+ \warning Suppose that changeFrame() has already been called.
+
+ \param cam : camera parameters.
+
+ \return Image point corresponding to the region of interest.
+*/
+std::vector<vpImagePoint>
+vpPolygon3D::getRoi(const vpCameraParameters &cam)
+{
+ std::vector<vpImagePoint> roi;
+ for (unsigned int i = 0; i < nbpt; i ++){
+ vpImagePoint ip;
+ vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
+ roi.push_back(ip);
+ }
+
+ return roi;
+}
+
+/*!
+ Get the region of interest in the image.
+
+ \param cam : camera parameters.
+ \param cMo : pose.
+
+ \return Image point corresponding to the region of interest.
+*/
+std::vector<vpImagePoint>
+vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo);
+ return getRoi(cam);
+}
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+/*!
+ Get the 3D points of the clipped region of interest.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+
+ \param points : resulting points.
+*/
+void
+vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
+{
+ for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ points.push_back(polyClipped[i].first);
+ }
+}
+#endif
+
+/*!
+ Get the 3D clipped points and their clipping information.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+
+ \param poly : resulting points plus clipping information.
+*/
+void
+vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint,unsigned int> > &poly)
+{
+ poly = polyClipped;
+}
+
+/*!
+ Get the 3D clipped points.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+
+ \param poly : resulting points.
+*/
+void
+vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
+{
+ for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ poly.push_back(polyClipped[i].first);
+ }
+}
+
+/*!
+ Get the region of interest clipped in the image.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+
+ \param cam : camera parameters.
+ \param roi : image point corresponding to the region of interest.
+*/
+void
+vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
+{
+ for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ vpImagePoint ip;
+ vpMeterPixelConversion::convertPoint(cam,polyClipped[i].first.get_x(),polyClipped[i].first.get_y(),ip);
+// std::cout << "## " << ip.get_j() << " - " << ip.get_i() << std::endl;
+ roi.push_back(ip);
+ }
+}
+
+/*!
+ Get the region of interest clipped in the image.
+
+ \param cam : camera parameters.
+ \param cMo : pose.
+ \param roi : image point corresponding to the region of interest.
+*/
+void
+vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi, const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo);
+ computePolygonClipped(cam);
+ getRoiClipped(cam, roi);
+}
+
+/*!
+ Get the region of interest clipped in the image and the information to know if it's a clipped point.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+
+ \param cam : camera parameters.
+ \param roi : image point corresponding to the region of interest with clipping information.
+*/
+void
+vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi)
+{
+ for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ vpImagePoint ip;
+ polyClipped[i].first.projection();
+ vpMeterPixelConversion::convertPoint(cam,polyClipped[i].first.get_x(),polyClipped[i].first.get_y(),ip);
+ roi.push_back(std::make_pair(ip, polyClipped[i].second));
+ }
+}
+
+/*!
+ Get the region of interest clipped in the image and the information to know if it's a clipped point.
+
+ \param cam : camera parameters.
+ \param roi : image point corresponding to the region of interest with clipping information.
+ \param cMo : pose.
+*/
+void
+vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo);
+ computePolygonClipped(cam);
+ getRoiClipped(cam, roi);
+}
+
+
+
+/*!
+ Static method to check the number of points of a region defined by the vector of image point that are inside the image.
+
+ \param I : The image used for its size.
+ \param cam : The camera parameters.
+*/
+unsigned int
+vpPolygon3D::getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam)
+{
+ unsigned int nbPolyIn = 0;
+ for (unsigned int i = 0; i < nbpt; i ++){
+ if(p[i].get_Z() > 0){
+ vpImagePoint ip;
+ vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
+ if((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
+ nbPolyIn++;
+ }
+ }
+
+ nbCornersInsidePrev = nbPolyIn;
+
+ return nbPolyIn;
+}
+
+//###################################
+// Static functions
+//###################################
+
+/*!
+ Static method to compute the clipped points from a set of initial points.
+
+ \warning When using FOV clipping and personnal camera parameters, camera normals have to be computed before (see vpCameraParameters::computeFov())
+
+ \param ptIn : Input points
+ \param ptOut : Output points (result of the clipping).
+ \param cMo : Pose considered for the clipping.
+ \param clippingFlags: Clipping flag (see vpPolygon3D::vpPolygon3DClippingType).
+ \param cam : Camera parameters (Only used if clipping flags contain FOV clipping).
+ \param znear : Near clipping distance value (Only used if clipping flags contain Near clipping).
+ \param zfar : Far clipping distance value (Only used if clipping flags contain Far clipping).
+*/
+void
+vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
+ const vpCameraParameters &cam, const double &znear, const double &zfar)
+{
+ ptOut.clear();
+ vpPolygon3D poly;
+ poly.setNbPoint((unsigned int)ptIn.size());
+ poly.setClipping(clippingFlags);
+
+ if((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ poly.setNearClippingDistance(znear);
+
+ if((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ poly.setFarClippingDistance(zfar);
+
+ for(unsigned int i = 0; i < ptIn.size(); i++)
+ poly.addPoint(i,ptIn[i]);
+
+ poly.changeFrame(cMo);
+ poly.computePolygonClipped(cam);
+ poly.getPolygonClipped(ptOut);
+}
+
+void
+vpPolygon3D::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int & i_min, int &i_max, int &j_min, int &j_max)
+{
+ // i_min_d = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ double i_min_d = (double) INT_MAX;
+ double i_max_d = 0;
+ double j_min_d = (double) INT_MAX;
+ double j_max_d = 0;
+
+ for (unsigned int i = 0; i < iroi.size(); i += 1){
+ if(i_min_d > iroi[i].get_i())
+ i_min_d = iroi[i].get_i();
+
+ if(iroi[i].get_i() < 0)
+ i_min_d = 1;
+
+ if((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
+ i_max_d = iroi[i].get_i();
+
+ if(j_min_d > iroi[i].get_j())
+ j_min_d = iroi[i].get_j();
+
+ if(iroi[i].get_j() < 0)
+ j_min_d = 1;//border
+
+ if((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
+ j_max_d = iroi[i].get_j();
+ }
+ i_min = static_cast<int> (i_min_d);
+ i_max = static_cast<int> (i_max_d);
+ j_min = static_cast<int> (j_min_d);
+ j_max = static_cast<int> (j_max_d);
+}
+
+/*!
+ Static method to check whether the region defined by the vector of image point
+ is contained entirely in the image.
+
+ \param I : The image used for its size.
+ \param corners : The vector of points defining a region
+*/
+bool
+vpPolygon3D::roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners)
+{
+ double nbPolyIn = 0;
+ for(unsigned int i=0; i<corners.size(); ++i){
+ if((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) &&
+ (corners[i].get_i() < I.getHeight()) && (corners[i].get_j() < I.getWidth())){
+ nbPolyIn++;
+ }
+ }
+
+ if(nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
+ return false;
+
+ return true;
+}
diff --git a/modules/core/src/tools/geometry/vpRect.cpp b/modules/core/src/tools/geometry/vpRect.cpp
new file mode 100644
index 0000000..2b7ea31
--- /dev/null
+++ b/modules/core/src/tools/geometry/vpRect.cpp
@@ -0,0 +1,252 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a rectangle in the plane.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpRect.cpp
+ \brief Defines a rectangle in the plane.
+ \ingroup libtools
+*/
+
+
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpDebug.h>
+
+/*!
+
+ Constructs a default rectangle with the \e top, \e left corner set to (0,0)
+ and \e width and \e height set to 1.
+
+*/
+
+vpRect::vpRect() : left(0), top(0), width(0), height(0) {};
+
+/*!
+
+ Constructs a rectangle with the \e top, \e left corner and \e width
+ and \e height.
+
+ \param l : coordinate of the top/left corner along the horizontal axis.
+ \param t : coordinate of the top/left corner along the vertical axis.
+ \param w : rectangle width.
+ \param h : rectangle height.
+
+*/
+vpRect::vpRect(double l, double t, double w, double h)
+ : left(l), top(t), width(w), height(h)
+{
+};
+
+/*!
+
+ Constructs a rectangle with \e topLeft the top-left corner location
+ and \e width and \e height the rectangle size.
+
+ \param topLeft : coordinates of the top/left corner.
+ \param w : rectangle width.
+ \param h : rectangle height.
+
+*/
+vpRect::vpRect(const vpImagePoint &topLeft, double w, double h)
+ : left(topLeft.get_u()), top(topLeft.get_v()), width(w), height(h)
+{
+};
+
+/*!
+
+ Constructs a rectangle with \e topLeft the top-left corner location
+ and \e bottomRight the bottom-right corner.
+
+*/
+vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
+ : left(topLeft.get_u()), top(topLeft.get_v()), width(0), height(0)
+{
+ this->left = topLeft.get_u();
+ this->top = topLeft.get_v();
+
+ setBottom( bottomRight.get_v() );
+ setRight( bottomRight.get_u() );
+};
+
+/*!
+
+ Constructs a rectangle that is a copy of \e r.
+
+*/
+vpRect::vpRect(const vpRect& r)
+ : left(0), top(0), width(0), height(0)
+{
+ *this = r;
+};
+
+/*!
+
+ Assign \e r to this rectangle and return a reference to this rectangle.
+*/
+vpRect &vpRect::operator=(const vpRect& r)
+{
+ this->left = r.left;
+ this->top = r.top;
+ this->width = r.width;
+ this->height = r.height;
+ return *this;
+};
+
+/*!
+
+ Create a rectangle as the bounding box of a vector of image points.
+ \param ip : Vector of image points. At least 1 points is mandatory,
+ otherwise an exception is thrown.
+*/
+vpRect::vpRect(const std::vector<vpImagePoint> &ip)
+ : left(0), top(0), width(0), height(0)
+{
+ set(ip);
+}
+
+/*!
+
+ Set the rectangle with the \e top, \e left corner and \e width
+ and \e height.
+ \param l : coordinate of the top/left corner along the horizontal axis.
+ \param t : coordinate of the top/left corner along the vertical axis.
+ \param w : rectangle width.
+ \param h : rectangle height.
+
+*/
+void vpRect::set(double l, double t, double w, double h)
+{
+ left = l;
+ top = t;
+ width = w;
+ height = h;
+};
+
+/*!
+
+ Set the rectangle with \e topLeft the top-left corner location
+ and \e width and \e height the rectangle size.
+
+ \param topLeft : coordinates of the top-left corner.
+ \param w : rectangle width.
+ \param h : rectangle height.
+*/
+void vpRect::set(const vpImagePoint &topLeft, double w, double h)
+{
+ left = topLeft.get_u();
+ top = topLeft.get_v();
+ width = w;
+ height = h;
+};
+
+/*!
+
+ Set the rectangle as the bounding box of a vector of image points.
+ \param ip : Vector of image points. At least 1 points is mandatory,
+ otherwise an exception is thrown.
+*/
+void vpRect::set(const std::vector<vpImagePoint> &ip)
+{
+ if (ip.size() < 1)
+ throw (vpException(vpException::dimensionError,
+ "At least 1 point is requested to build a rectangle"));
+ double minu, maxu;
+ double minv, maxv;
+ minu = maxu = ip[0].get_u();
+ minv = maxv = ip[0].get_v();
+
+ for(size_t i=1; i<ip.size(); i++) {
+ double u = ip[i].get_u();
+ double v = ip[i].get_v();
+ if ( u < minu ) minu = u;
+ else if (u > maxu) maxu = u;
+ if ( v < minv ) minv = v;
+ else if (v > maxv) maxv = v;
+ }
+
+ setLeft (minu);
+ setTop (minv);
+ setRight (maxu);
+ setBottom(maxv);
+};
+
+/*!
+
+ Set the rectangle with \e topLeft the top-left corner location
+ and \e bottomRight the bottom-right corner.
+
+ \param topLeft : coordinates of the top-left corner.
+ \param bottomRight : coordinates of the bottom-right corner.
+
+*/
+void vpRect::set(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
+{
+ this->left = topLeft.get_u();
+ this->top = topLeft.get_v();
+
+ setBottom( bottomRight.get_v() );
+ setRight( bottomRight.get_u() );
+};
+
+/*!
+
+ Set the rectangle from \e r.
+
+*/
+void vpRect::set(const vpRect& r)
+{
+ *this = r;
+};
+
+/*!
+
+ Check if an image point belongs to a rectangle.
+
+ \param ip : the image point.
+ \param rect : the rectangle.
+
+ \return Returns true if the point belongs to the rectangle.
+
+*/
+VISP_EXPORT bool inRectangle( const vpImagePoint &ip, const vpRect &rect ) {
+ return ( ip.get_i() <= rect.getBottom() && ip.get_i() >= rect.getTop() && ip.get_j() <= rect.getRight() && ip.get_j() >= rect.getLeft());
+}
+
+VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRect& r)
+ {
+ os << r.getLeft() << ", " << r.getTop() << ", " << r.getWidth() << ", " << r.getHeight();
+ return os;
+}
diff --git a/modules/core/src/tools/geometry/vpTriangle.cpp b/modules/core/src/tools/geometry/vpTriangle.cpp
new file mode 100644
index 0000000..03026ce
--- /dev/null
+++ b/modules/core/src/tools/geometry/vpTriangle.cpp
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a 2D triangle.
+ *
+ * Author:
+ * Amaury Dame
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpTriangle.h>
+#include <visp3/core/vpDebug.h>
+
+/*!
+ Basic constructor.
+
+ By default, the three 2D points coordinates which define the triangle are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
+*/
+vpTriangle::vpTriangle()
+ : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
+ ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+{
+ init (vpImagePoint(0,0),vpImagePoint(1,0),vpImagePoint(0,1));
+}
+
+/*!
+ Constructor which initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
+
+ \param iP1 : The first apex of the triangle.
+ \param iP2 : The first apex of the triangle.
+ \param iP3 : The first apex of the triangle.
+*/
+vpTriangle::vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
+ : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
+ ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+{
+ init(iP1,iP2,iP3);
+}
+
+/*!
+ Copy constructor
+
+ \param tri : The triangle used for the initialisation.
+*/
+vpTriangle::vpTriangle(const vpTriangle &tri)
+ : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
+ ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+{
+ *this = tri;
+}
+
+/*!
+ Basic destructor
+*/
+vpTriangle::~vpTriangle()
+{
+}
+
+/*!
+ Assign \e tri to this triangle and return a reference to this triangle.
+*/
+vpTriangle &
+vpTriangle::operator=(const vpTriangle& tri)
+{
+ goodTriange = tri.goodTriange;
+ S1 = tri.S1;
+ uvinv00 = tri.uvinv00;
+ uvinv01 = tri.uvinv01;
+ uvinv10 = tri.uvinv10;
+ uvinv11 = tri.uvinv11;
+ ptempo0 = tri.ptempo0;
+ ptempo1 = tri.ptempo1;
+ area = tri.area;
+ return *this;
+};
+
+/*!
+ Initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
+
+ \param iP1 : The first apex of the triangle.
+ \param iP2 : The first apex of the triangle.
+ \param iP3 : The first apex of the triangle.
+*/
+void
+vpTriangle::buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
+{
+ init(iP1,iP2,iP3);
+}
+
+
+void
+vpTriangle::init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
+{
+ ptempo0 = ptempo1 = 0.;
+ apex1 = iP1;
+ apex2 = iP2;
+ apex3 = iP3;
+
+ vpMatrix uv(2,2);
+ vpMatrix uvinv(2,2);
+
+ uv[0][0] = iP2.get_i() - iP1.get_i();
+ uv[1][0] = iP3.get_i() - iP1.get_i();
+ uv[0][1] = iP2.get_j() - iP1.get_j();
+ uv[1][1] = iP3.get_j() - iP1.get_j();
+ try
+ {
+ uvinv=uv.inverseByLU();
+ goodTriange = true;
+ }
+ catch(...)
+ {
+ goodTriange = false;
+ std::cout<<"Empty triangle"<<std::endl;
+ }
+
+ uvinv00=uvinv[0][0];
+ uvinv01=uvinv[0][1];
+ uvinv10=uvinv[1][0];
+ uvinv11=uvinv[1][1];
+ S1 = iP1;
+ area = 0.5 * fabs(uv.det());
+}
+
+
+/*!
+ Check if the 2D point \f$ iP \f$ is inside the triangle.
+
+ \param iP : The point which coulb be inside the triangle.
+ \param threshold : A threshold used to define the accuracy of the computation when the point is very near from the edges of the triangle. 0 is the smallest value.
+
+ \return Returns true if the point is inside the triangle. Returns false otherwise.
+*/
+bool
+vpTriangle::inTriangle(const vpImagePoint &iP, double threshold)
+{
+ if(!goodTriange)
+ return false;
+
+ if(threshold < 0)
+ threshold = 0;
+
+ ptempo0 = iP.get_i() - S1.get_i();
+ ptempo1 = iP.get_j() - S1.get_j();
+
+ double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
+ double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
+
+ return (p_ds_uv0+p_ds_uv1<1.+threshold && p_ds_uv0>-threshold && p_ds_uv1>-threshold);
+}
+
diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp
new file mode 100644
index 0000000..aacdc33
--- /dev/null
+++ b/modules/core/src/tools/histogram/vpHistogram.cpp
@@ -0,0 +1,976 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpHistogram.cpp
+ \brief Definition of the vpHistogram class member functions.
+
+ Class vpHistogram allows to calculate and manipulate gray level
+ image histograms.
+
+*/
+
+#include <stdlib.h>
+#include <visp3/core/vpHistogram.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpDisplay.h>
+
+
+bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second);
+
+// comparison,
+bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second)
+{
+ if (first.getValue() > second.getValue()) return true;
+ else return false;
+}
+
+
+
+/*!
+ Defaut constructor for a gray level histogram.
+*/
+vpHistogram::vpHistogram() : histogram(NULL), size(256)
+{
+ init();
+}
+
+/*!
+ Copy constructor of a gray level histogram.
+*/
+vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256)
+{
+ init(h.size);
+ memcpy(histogram, h.histogram, size * sizeof(unsigned));
+}
+
+/*!
+ Calculates the histrogram from a gray level image.
+
+ \param I : Gray level image.
+
+ \sa calculate()
+*/
+vpHistogram::vpHistogram(const vpImage<unsigned char> &I)
+ : histogram(NULL), size(256)
+{
+ init();
+
+ calculate(I);
+}
+
+/*!
+ Destructor.
+*/
+vpHistogram::~vpHistogram()
+{
+ if (histogram != NULL) {
+ // vpTRACE("free: %p", &histogram);
+ delete [] histogram;
+ histogram = NULL;
+ size = 0;
+ }
+}
+
+/*!
+
+ Copy operator.
+ \param h : Histogram to copy.
+
+ \code
+ vpImage<unsigned char> I;
+
+ vpHistogram h(I);
+ vpHistogram hcopied = h;
+
+ \endcode
+*/
+vpHistogram &
+vpHistogram::operator=(const vpHistogram &h)
+{
+ init(h.size);
+ memcpy(histogram, h.histogram, size * sizeof(unsigned));
+
+ return *this;
+}
+
+/*!
+ Initialise the histogram structure.
+ - Allocates the array
+ - Initialise all the values to zero.
+*/
+void
+vpHistogram::init(unsigned size_)
+{
+ if (histogram != NULL) {
+ delete [] histogram;
+ histogram = NULL;
+ }
+
+ this->size = size_;
+ histogram = new unsigned [size];
+
+ memset(histogram, 0, size * sizeof(unsigned));
+
+ // vpTRACE("alloc: %p", &histogram);
+}
+
+
+/*!
+
+ Calculate the histogram from a gray level image.
+
+ \param I : Gray level image.
+ \param nbins : Number of bins to compute the histogram.
+*/
+void vpHistogram::calculate(const vpImage<unsigned char> &I, const unsigned int nbins)
+{
+ if(size != nbins) {
+ if (histogram != NULL) {
+ delete [] histogram;
+ histogram = NULL;
+ }
+
+ size = nbins > 256 ? 256 : (nbins > 0 ? nbins : 256);
+ if(nbins > 256 || nbins == 0) {
+ std::cerr << "nbins=" << nbins << " , nbins should be between ]0 ; 256] ; use by default nbins=256" << std::endl;
+ }
+ histogram = new unsigned int[size];
+ }
+
+ memset(histogram, 0, size * sizeof(unsigned));
+
+ unsigned int lut[256];
+ for(unsigned int i = 0; i < 256; i++) {
+ lut[i] = (unsigned int) (i * size / 256.0);
+ }
+
+ unsigned int size_ = I.getWidth()*I.getHeight();
+ unsigned char *ptrStart = (unsigned char*) I.bitmap;
+ unsigned char *ptrEnd = ptrStart + size_;
+ unsigned char *ptrCurrent = ptrStart;
+
+ while(ptrCurrent != ptrEnd) {
+ histogram[ lut[ *ptrCurrent ] ] ++;
+ ++ptrCurrent;
+ }
+}
+
+/*!
+ Display the histogram distribution in an image, the minimal image size is 36x36 px.
+
+ \param I : Image with the histogram distribution displayed.
+ \param color : Color used for the display.
+ \param thickness : Thickness of the line.
+ \param maxValue_ : Maximum value in the histogram, if 0 it will be computed from the current histogram.
+ Useful to plot a 3 channels histogram for a RGB image for example to keep a coherent vertical scale between the channels.
+*/
+void vpHistogram::display(const vpImage<unsigned char> &I, const vpColor &color, const unsigned int thickness,
+ const unsigned int maxValue_) {
+ unsigned int width = I.getWidth(), height = I.getHeight();
+ //Minimal width and height are 36 px
+ if(width < 36 || height < 36) {
+ std::cerr << "Image I must have at least width >= 36 && height >= 36 !" << std::endl;
+ return;
+ }
+
+ unsigned int maxValue = maxValue_;
+ if(maxValue == 0) {
+ for(unsigned int i = 0; i < size; i++) {
+ if(histogram[i] > maxValue) {
+ maxValue = histogram[i];
+ }
+ }
+ }
+
+ if (maxValue == 0) {
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot display histogram; max value=0"));
+ }
+ //Keep 12 free pixels at the top
+ unsigned int max_height = height-12;
+ double ratio_height = max_height / (double) maxValue;
+ double ratio_width = (width-1) / (double) (size-1.0);
+
+ for(unsigned int i = 1; i < size; i++) {
+ unsigned int value1 = histogram[i-1];
+ unsigned int value2 = histogram[i];
+
+ vpImagePoint startPt((height-1)-(value1*ratio_height), (i-1)*ratio_width);
+ vpImagePoint endPt((height-1)-(value2*ratio_height), (i*ratio_width));
+ vpDisplay::displayLine(I, startPt, endPt, color, thickness);
+ }
+}
+
+/*!
+
+ Smoothes the histogram and returns it.
+
+ A simple average scheme is used where each value \f$h(i)\f$ in the
+ histogram is replaced by the average of itself and the neighbours.
+
+ \f[h(i) = \sum_{j=i-\frac{fsize}{2}}^{i+\frac{fsize}{2}} h(j) \f]
+
+ \param fsize : Filter size. Corresponds to the number of values
+ around each point used to compute the mean value.
+
+ \return Smoothed histogram.
+
+ \exception vpImageException::notInitializedError : Histogram array
+ not initialsed. Means that the histogram was not calculated before.
+
+ \sa calculate()
+
+*/
+void
+vpHistogram::smooth(const unsigned int fsize)
+{
+ if (histogram == NULL) {
+ vpERROR_TRACE("Histogram array not initialised\n");
+ throw (vpImageException(vpImageException::notInitializedError,
+ "Histogram array not initialised")) ;
+ }
+
+ vpHistogram h;
+ h = *this;
+
+ int hsize = (int)fsize / 2; // half filter size
+ unsigned int sum;
+ unsigned int nb;
+
+ for (unsigned i=0; i < size; i ++) {
+ sum = 0;
+ nb = 0;
+ for (int j=-hsize; j <= hsize; j ++) {
+ // exploitation of the overflow to detect negative value...
+ if ( /*(i + j) >= 0 &&*/ (i + (unsigned int)j) < size ) {
+ sum += h.histogram[i + (unsigned int)j];
+ nb ++;
+ }
+ }
+ histogram[i] = sum / nb;
+ }
+
+}
+
+/*!
+
+ Build a list of all histogram peaks. This peak list is gray level
+ sorted from 0 to 255. That mean that the first peak has a gray level
+ less than the second one, etc.
+
+ To sort this list from highest peak value to the lowest one, you can
+ use sort().
+
+ \param peaks : List of peaks.
+ \return The number of peaks in the histogram.
+
+ \sa sort()
+*/
+unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & peaks)
+{
+ if (histogram == NULL) {
+ vpERROR_TRACE("Histogram array not initialised\n");
+ throw (vpImageException(vpImageException::notInitializedError,
+ "Histogram array not initialised")) ;
+ }
+
+ int prev_slope; // Previous histogram inclination
+ int next_slope; // Next histogram inclination
+ vpHistogramPeak p; // An histogram peak
+ unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
+
+ peaks.clear();
+
+ // Parse the histogram to get the local maxima
+ unsigned cpt = 0;
+ unsigned sum_level = 0;
+ nbpeaks = 0;
+ prev_slope = 1;
+
+ for (unsigned i = 0; i < size-1; i++) {
+ next_slope = (int)histogram[i+1] - (int)histogram[i];
+
+// if ((prev_slope < 0) && (next_slope > 0) ) {
+// sum_level += i;
+// cpt ++;
+// continue;
+// }
+
+ if ((prev_slope > 0) && (next_slope == 0) ) {
+ sum_level += i + 1;
+ cpt ++;
+ continue;
+ }
+
+
+ // Peak detection
+ if ( (prev_slope > 0) && (next_slope < 0) ) {
+ sum_level += i;
+ cpt ++;
+
+ unsigned int level = sum_level / cpt;
+ p.set((unsigned char)level, histogram[level]);
+ // vpTRACE("add %d %d", p.getLevel(), p.getValue());
+ peaks.push_back(p);
+
+ nbpeaks ++;
+
+ }
+
+ prev_slope = next_slope;
+ sum_level = 0;
+ cpt = 0;
+ }
+ if (prev_slope > 0) {
+ p.set((unsigned char)size-1u, histogram[size-1]);
+ // vpTRACE("add %d %d", p.getLevel(), p.getValue());
+ peaks.push_back(p);
+ nbpeaks ++;
+ }
+
+ return nbpeaks;
+}
+
+/*!
+
+ Find the two highest peaks in the histogram. Usually, the first
+ one correspond to the image background, the second one to
+ the object.
+
+ \param dist : Minimal distance between two significative histogram peaks.
+
+ \param peak1 : Highest peak in the histogram.
+ \param peak2 : Second highest peak in the histogram.
+
+ \return The number of highest peaks:
+ - 2: If the histogram is bimodal
+ - 1: If no second highest peak was found.
+ - 0: if no peaks were found.
+
+*/
+unsigned vpHistogram::getPeaks(unsigned char dist,
+ vpHistogramPeak & peak1,
+ vpHistogramPeak & peak2)
+{
+ std::list<vpHistogramPeak> peaks;
+ unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
+
+ nbpeaks = getPeaks(peaks);
+ sort(peaks);
+
+ if (nbpeaks == 0) {
+ peak1.set(0, 0);
+ peak2.set(0, 0);
+ return 0;
+ }
+
+ if (nbpeaks == 1) {
+ peak1 = peaks.front();
+ peak2.set(0, 0);
+ return 1;
+ }
+
+ // Parse the peaks list to get the peak with a distance greater
+ // than dist to the highest
+ peak1 = peaks.front();
+ for(std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++ it)
+ {
+ vpHistogramPeak p = *it;
+ if (abs(p.getLevel() - peak1.getLevel()) > dist) {
+ // The second peak is found
+ peak2 = p;
+ return 2;
+ }
+ }
+
+ // No second peak found
+ peak2.set(0, 0);
+ return 1;
+}
+
+
+/*!
+
+ Determine the two highest peaks in the histogram and compute a
+ threshold to separate the two objects. Here we dont know which is the
+ highest peak. It could be \e peakl or \e peakr.
+
+ \param dist : Distance between two significative histogram maxima
+ \param peakl : Position of the left histogram peak.
+ \param peakr : Position of the right histogram peak.
+
+ \param valey : Valey between the two peaks \e peakl and \e peakr.
+
+ \return true if the histogram is bimodal, false otherwise.
+*/
+
+bool
+vpHistogram::getPeaks(unsigned char dist,
+ vpHistogramPeak & peakl,
+ vpHistogramPeak & peakr,
+ vpHistogramValey & valey)
+{
+ unsigned char *peak; // Local maxima values
+ int prev_slope; // Previous histogram inclination
+ int next_slope; // Next histogram inclination
+ unsigned index_highest_peak; // Index in peak[] array of the highest peak
+ unsigned index_second_peak; // Index in peak[] array of the second peak
+
+ unsigned int prof;
+ unsigned int maxprof; // Nb pixels difference between 2 maxi peaks
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
+ unsigned int nbpeaks; // Number of peaks in the histogram (ie local maxima)
+
+ // Init the valey
+ valey.set(0, 0);
+
+ // Allocation for the
+ peak = new unsigned char [size];
+
+ // Parse the histogram to get the local maxima
+ nbpeaks = 0;
+ prev_slope = 1;
+ for (unsigned i = 0; i < size-1; i++) {
+ next_slope = (int)histogram[i+1] - (int)histogram[i];
+ if (next_slope == 0)
+ continue;
+ // Peak detection
+ if ( (prev_slope > 0) && (next_slope < 0) )
+ peak[nbpeaks ++] = (unsigned char)i;
+
+ prev_slope = next_slope;
+ }
+ 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++) {
+ if (histogram[ peak[i] ] > histogram[ peak[index_highest_peak] ]) {
+ index_highest_peak = i;
+ }
+ }
+
+// 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;
+
+ // Search second local maximum on the left of the global maximum
+ for (unsigned i = 0; i < index_highest_peak; i++) {
+ if (peak[index_highest_peak] - peak[i] > dist) {
+ prof=0;
+ for (int j=peak[i]; j <= peak[index_highest_peak]-dist; j++)
+ if((histogram[peak[i]] - histogram[j]) > prof)
+ prof = histogram[peak[i]] - histogram[j];
+
+ if (prof > maxprof) {
+ maxprof = prof;
+ index_second_peak = i;
+ }
+ }
+ }
+
+ // Search second local maximum on the right of the global maximum
+ for (unsigned i = index_highest_peak+1; i < nbpeaks; i++) {
+ if (peak[i] - peak[index_highest_peak] > dist) {
+ prof=0;
+ for (int j=peak[index_highest_peak]+dist; j <= peak[i]; j++)
+ if((histogram[peak[i]] - histogram[j]) > prof)
+ prof = histogram[peak[i]] - histogram[j];
+
+ if (prof > maxprof) {
+ maxprof = prof;
+ index_second_peak = i;
+ }
+ }
+ }
+// 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
+ {
+ peakl.set(peak[index_second_peak], histogram[ peak[index_second_peak] ]);
+ peakr.set(peak[index_highest_peak], histogram[ peak[index_highest_peak]]);
+ }
+
+ if (peakl == peakr) {
+
+ delete [] peak;
+
+ return (false); // Not a bimodal histogram
+ }
+
+ // Search the valey
+ mini = peakl.getValue();
+ sumindmini=0;
+ nbmini=0;
+ for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini=histogram[i];
+ nbmini=1;
+ sumindmini=i;
+ continue;
+ }
+ if (histogram[i] == mini) {
+ sumindmini += i;
+ nbmini ++;
+ }
+ }
+ // vpTRACE("nbmini %d", nbmini);
+ // vpTRACE("sumindmini %d", sumindmini);
+ // vpTRACE("mini: indmini: %d", sumindmini/nbmini);
+
+ if (nbmini == 0) {
+ // no valey found
+ valey.set(0, 0);
+
+ delete [] peak;
+
+ return false;
+ }
+ else {
+ mini = sumindmini/nbmini; // mean
+ valey.set((unsigned char)mini, histogram[mini]);
+
+ delete [] peak;
+
+ return (true);
+ }
+}
+
+/*!
+
+ Build a list of all histogram valey. This valey list is gray level
+ sorted from 0 to 255. That mean that the first valey has a gray level
+ less than the second one, etc.
+
+ \param valey : List of valey.
+ \return The number of valey in the histogram.
+
+ \sa sort()
+*/
+unsigned vpHistogram::getValey(std::list<vpHistogramValey> & valey)
+{
+ if (histogram == NULL) {
+ vpERROR_TRACE("Histogram array not initialised\n");
+ throw (vpImageException(vpImageException::notInitializedError,
+ "Histogram array not initialised")) ;
+ }
+
+ int prev_slope; // Previous histogram inclination
+ int next_slope; // Next histogram inclination
+ vpHistogramValey p; // An histogram valey
+ unsigned nbvaley; // Number of valey in the histogram (ie local minima)
+
+ valey.clear();
+
+ // Parse the histogram to get the local minima
+ unsigned cpt = 0;
+ unsigned sum_level = 0;
+ nbvaley = 0;
+ prev_slope = -1;
+
+ for (unsigned i = 0; i < size-1; i++) {
+ next_slope = (int)histogram[i+1] - (int)histogram[i];
+
+ if ((prev_slope < 0) && (next_slope == 0) ) {
+ sum_level += i + 1;
+ cpt ++;
+ continue;
+ }
+
+ // Valey detection
+ if ( (prev_slope < 0) && (next_slope > 0) ) {
+ sum_level += i;
+ cpt ++;
+
+ unsigned int level = sum_level / cpt;
+ p.set((unsigned char)level, histogram[level]);
+ // vpTRACE("add %d %d", p.getLevel(), p.getValue());
+ valey.push_back(p);
+
+ nbvaley ++;
+
+ }
+
+ prev_slope = next_slope;
+ sum_level = 0;
+ cpt = 0;
+ }
+ if (prev_slope < 0) {
+ p.set((unsigned char)size-1u, histogram[size-1]);
+ // vpTRACE("add %d %d", p.getLevel(), p.getValue());
+ valey.push_back(p);
+ nbvaley ++;
+ }
+
+ return nbvaley;
+}
+
+/*!
+
+ Find the valey between two peaks. It starts at the lowest peak and
+ works its way up to the highest peak. Along the way, it looks at
+ each point in the histogram to find the location of the smallest
+ histogram point which is the valey point.
+
+ \param peak1 : A peak in the histogram.
+ \param peak2 : A other peak in the histogram.
+
+ \param valey : Low point of the valey between two peaks in a histogram.
+
+ \return true if a valey was found, false otherwise.
+*/
+bool
+vpHistogram::getValey(const vpHistogramPeak & peak1,
+ const vpHistogramPeak & peak2,
+ vpHistogramValey & valey)
+{
+
+ // Set the left and right peaks
+ vpHistogramPeak peakl, peakr;
+ if (peak1.getLevel() > peak2.getLevel()) {
+ peakl = peak2;
+ peakr = peak1;
+ }
+ else {
+ peakl = peak1;
+ peakr = peak2;
+ }
+
+ // Search the valey
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
+
+ mini = peakl.getValue();
+ sumindmini=0;
+ nbmini=0;
+ for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini=histogram[i];
+ nbmini=1;
+ sumindmini=i;
+ continue;
+ }
+ if (histogram[i] == mini) {
+ sumindmini += i;
+ nbmini ++;
+ }
+ }
+
+ if (nbmini == 0) {
+ // no valey found
+ valey.set(0, 0);
+
+ return false;
+ }
+ else {
+ unsigned int minipos = sumindmini/nbmini; // position of the minimum
+
+ valey.set((unsigned char)minipos, histogram[minipos]);
+ return true;
+ }
+}
+/*!
+
+ Find the two valey around an histogram peak. It starts at the peak
+ position and works its way down and up to find the left and right
+ valey around the peak.
+
+
+ \param dist : Minimal distance between two significative histogram peaks.
+ \param peak : A peak in the histogram.
+ \param valeyl : The valey on the left of the peak.
+ \param valeyr : The valey on the right of the peak.
+
+ \return 0x00 : If no valey was found.
+ \return 0x01 : If only the right valey was found.
+ \return 0x10 : If only the left valey was found.
+ \return 0x11 : If two valeys around the peak were found.
+
+*/
+unsigned
+vpHistogram::getValey(unsigned char dist,
+ const vpHistogramPeak & peak,
+ vpHistogramValey & valeyl,
+ vpHistogramValey & valeyr)
+{
+ unsigned int ret = 0x11;
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
+ vpHistogramPeak peakr, peakl; // Left and right peaks of peak
+ std::list<vpHistogramPeak> peaks; // list of histogram peaks
+ // unsigned int nbpeaks=0; // Number of peaks in the histogram (ie local maxima)
+
+ if (peak.getLevel() == 0) {
+ valeyl.set(0, 0);
+ ret &= 0x01;
+ }
+ if (peak.getLevel() == size -1) {
+ valeyr.set((unsigned char)(size-1), 0);
+ ret &= 0x10;
+ }
+
+ if (ret >> 1) // consider the left part of the requested peak
+ {
+ // If the list of peaks is empty, compute it
+ if (peaks.empty())
+ {
+ /* 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<vpHistogramPeak>::const_iterator it;
+ unsigned index = 0;
+ for (it = peaks.begin(); it != peaks.end(); ++ it)
+ {
+ if (peak == *it) {
+ // we are on the peak.
+ break;
+ }
+ index ++;
+ }
+
+ bool found = false;
+ if (index == 0) {
+ // 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
+ std::list<vpHistogramPeak>::const_iterator lit; // left iterator
+ for(lit = peaks.begin(); lit != it; ++ lit)
+ {
+ if (abs((*lit).getLevel() - peak.getLevel()) > dist) {
+ // peakl found
+ peakl = *lit;
+ found = true; // we cannot stop here, since the other peaks on the right may exist
+ }
+ }
+ }
+ if ( ! found)
+ peakl.set(0,0);
+
+ // Search the valey on the left
+ mini = peak.getValue();
+ sumindmini=0;
+ nbmini=0;
+ for (unsigned i=peakl.getLevel(); i < peak.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini=histogram[i];
+ nbmini=1;
+ sumindmini=i;
+ continue;
+ }
+ if (histogram[i] == mini) {
+ sumindmini += i;
+ nbmini ++;
+ }
+ }
+ if (nbmini == 0) {
+ valeyl.set(0, 0);
+ ret &= 0x01;
+ }
+ else {
+ unsigned int minipos = sumindmini/nbmini; // position of the minimum
+ valeyl.set((unsigned char)minipos, histogram[minipos]);
+ ret &= 0x11;
+ }
+ }
+
+ if (ret << 1) {
+ // If the list of peaks is empty, compute it
+ if (peaks.empty()) {
+ /* nbpeaks = */ getPeaks(peaks);
+ }
+ // Go to the requested peak in the list
+ std::list<vpHistogramPeak>::const_iterator it;
+ unsigned index = 0;
+ for (it = peaks.begin(); it != peaks.end(); ++ it)
+ {
+ if (peak == *it) {
+ // we are on the peak.
+ break;
+ }
+ index ++;
+ }
+
+ bool found = false;
+ // search for the nearest peak on the right that matches the distance
+ std::list<vpHistogramPeak>::const_iterator rit; // right iterator
+ for(rit = it; rit != peaks.end(); ++ rit)
+
+ if (abs((*rit).getLevel() - peak.getLevel()) > dist) {
+ // peakr found
+ peakr = *rit;
+ found = true;
+ break; // we can stop here
+ }
+
+ if ( ! found)
+ peakr.set((unsigned char)(size-1),0);
+
+ // Search the valey on the right
+ mini = peak.getValue();
+ sumindmini=0;
+ nbmini=0;
+ for (unsigned i=(unsigned int)peak.getLevel()+1; i <= (unsigned int)peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini=histogram[i];
+ nbmini=1;
+ sumindmini=i;
+ continue;
+ }
+ if (histogram[i] == mini) {
+ sumindmini += i;
+ nbmini ++;
+ }
+ }
+ if (nbmini == 0) {
+ valeyr.set((unsigned char)(size-1), 0);
+ ret &= 0x10;
+ }
+ else {
+ unsigned int minipos = sumindmini/nbmini; // position of the minimum
+ valeyr.set((unsigned char)minipos, histogram[minipos]);
+ ret &= 0x11;
+ }
+ }
+
+ return ret;
+}
+
+/*!
+ Sort a list of histogram peaks from highest to the lowest.
+
+ \param peaks : List of histogram peaks.
+ \return The number of peaks in the histogram.
+
+ \sa getPeak()
+*/
+unsigned vpHistogram::sort(std::list<vpHistogramPeak> & peaks)
+{
+
+ if ( peaks.empty() ) {
+ return 0;
+ }
+
+ peaks.sort(compare_vpHistogramPeak);
+
+ return (unsigned int)peaks.size();
+}
+
+/*!
+
+ Write the histogram values in a file.
+
+ In the file, on each line you will find first the gray level and
+ than the number of pixels which have this gray level.
+
+ \param filename : File name to write with histogram values.
+
+ \return true if the file could be written, false otherwise.
+*/
+
+bool
+vpHistogram::write(const std::string &filename)
+{
+ return ( this->write(filename.c_str()) );
+}
+
+/*!
+
+ Write the histogram values in a file.
+
+ In the file, on each line you will find first the gray level and
+ than the number of pixels which have this gray level.
+
+ \param filename : File name to write with histogram values.
+
+ \return true if the file could be written, false otherwise.
+*/
+
+bool
+vpHistogram::write(const char *filename)
+{
+ std::string opath;
+
+ FILE *fd = fopen(filename, "w");
+ if (fd == NULL)
+ return false;
+ for (unsigned i=0; i < size; i ++)
+ fprintf(fd, "%d %d\n", i, histogram[i]);
+ fclose(fd);
+
+ return true;
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/tools/histogram/vpHistogramPeak.cpp b/modules/core/src/tools/histogram/vpHistogramPeak.cpp
new file mode 100644
index 0000000..1305000
--- /dev/null
+++ b/modules/core/src/tools/histogram/vpHistogramPeak.cpp
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpHistogramPeak.cpp
+ \brief Declaration of the vpHistogramPeak class.
+ Class vpHistogramPeak defines a gray level histogram peak.
+
+*/
+
+#include <visp3/core/vpHistogramPeak.h>
+
+
+/*!
+ Defaut constructor for a gray level histogram peak.
+*/
+vpHistogramPeak::vpHistogramPeak() : level(0), value(0) {}
+
+/*!
+ Defaut constructor for a gray level histogram peak.
+*/
+vpHistogramPeak::vpHistogramPeak(unsigned char lvl, unsigned val)
+ : level(lvl), value(val) {}
+
+/*!
+ Copy constructor of a gray level histogram peak.
+*/
+vpHistogramPeak::vpHistogramPeak(const vpHistogramPeak &p)
+ : level(0), value(0)
+{
+ *this = p;
+}
+
+/*!
+
+ Copy operator.
+ \param p : Histogram peak to copy.
+
+ \code
+ vpHistogramPeak p1(0, 255);
+ vpHistogramPeak p2 = p1; // Peak p2 is set to 0, 255
+ \endcode
+*/
+vpHistogramPeak &
+vpHistogramPeak::operator=(const vpHistogramPeak &p)
+{
+ setLevel(p.level);
+ setValue(p.value);
+
+ return *this;
+}
+
+/*!
+
+ Comparison operator.
+
+ \param p : Gray level histogram peak to compar.
+
+*/
+bool
+vpHistogramPeak::operator==(const vpHistogramPeak &p) const
+{
+ return ( (level == p.level) && (value == p.value) );
+}
+
+/*!
+ \brief std::cout a peak
+*/
+VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHistogramPeak &p)
+{
+
+ s << (int)p.getLevel() << " " << p.getValue();
+
+ return s;
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/tools/histogram/vpHistogramValey.cpp b/modules/core/src/tools/histogram/vpHistogramValey.cpp
new file mode 100644
index 0000000..78fd3f8
--- /dev/null
+++ b/modules/core/src/tools/histogram/vpHistogramValey.cpp
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Gray level histogram manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpHistogramValey.cpp
+ \brief Declaration of the vpHistogramValey class.
+ Class vpHistogramValey defines a gray level histogram valey.
+
+*/
+
+#include <visp3/core/vpHistogramValey.h>
+
+/*!
+
+ Copy operator.
+ \param v : Histogram valey to copy.
+
+ \code
+ vpHistogramValey v1(0, 255);
+ vpHistogramValey v2 = v1; // Valey p2 is set to 0, 255
+ \endcode
+*/
+vpHistogramValey &
+vpHistogramValey::operator=(const vpHistogramValey &v)
+{
+ setLevel(v.level);
+ setValue(v.value);
+
+ return *this;
+}
+
+/*!
+
+ Comparison operator.
+
+ \param v : Gray level histogram valey to compar.
+
+*/
+bool
+vpHistogramValey::operator==(const vpHistogramValey &v) const
+{
+ return ( (level == v.level) && (value == v.value) );
+}
+
+/*!
+ \brief std::cout a valey
+*/
+VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHistogramValey &v)
+{
+
+ s << (int)v.getLevel() << " " << v.getValue();
+
+ return s;
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/src/network/CMakeLists.txt b/modules/core/src/tools/network/CMakeLists.txt
similarity index 100%
rename from src/network/CMakeLists.txt
rename to modules/core/src/tools/network/CMakeLists.txt
diff --git a/modules/core/src/tools/network/vpClient.cpp b/modules/core/src/tools/network/vpClient.cpp
new file mode 100644
index 0000000..9ca5a87
--- /dev/null
+++ b/modules/core/src/tools/network/vpClient.cpp
@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Client
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpClient.h>
+
+
+vpClient::vpClient() : vpNetwork(), numberOfAttempts(0)
+{
+}
+
+/*!
+ Disconnect the client from all the servers, and close the sockets.
+*/
+vpClient::~vpClient()
+{
+ stop();
+}
+
+/*!
+ Connect to the server represented by the given hostname, and at a given port.
+
+ \sa vpClient::connectToIP();
+
+ \param hostname : Hostname of the server.
+ \param port_serv : Port used for the connection.
+
+ \return True if the connection has been etablished, false otherwise.
+*/
+bool vpClient::connectToHostname(const std::string &hostname, const unsigned int &port_serv)
+{
+ // get server host information from hostname
+ struct hostent *server = gethostbyname( hostname.c_str() );
+
+ if ( server == NULL )
+ {
+ std::string noSuchHostMessage( "ERROR, " );
+ noSuchHostMessage.append( hostname );
+ noSuchHostMessage.append( ": no such host\n" );
+ vpERROR_TRACE( noSuchHostMessage.c_str(),
+ "vpClient::connectToHostname(const std::string &hostname, const int &port_serv)" );
+ return false;
+ }
+
+ vpNetwork::vpReceptor serv;
+
+ serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( serv.socketFileDescriptorReceptor < 0){
+#else
+ if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
+#endif
+ vpERROR_TRACE( "ERROR opening socket",
+ "vpClient::connectToHostname()" );
+ return false;
+ }
+
+ memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
+ serv.receptorAddress.sin_family = AF_INET;
+ memmove( (char *) &serv.receptorAddress.sin_addr.s_addr, (char *) server->h_addr, (unsigned)server->h_length );
+ serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
+ serv.receptorIP = inet_ntoa(*(in_addr *)server->h_addr);
+
+ return connectServer(serv);
+}
+
+/*!
+ Connect to the server represented by the given ip, and at a given port.
+
+ \sa vpClient::connectToHostname()
+
+ \param ip : IP of the server.
+ \param port_serv : Port used for the connection.
+
+ \return True if the connection has been etablished, false otherwise.
+*/
+bool vpClient::connectToIP(const std::string &ip, const unsigned int &port_serv)
+{
+ vpNetwork::vpReceptor serv;
+
+ serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if ( serv.socketFileDescriptorReceptor < 0){
+#else
+ if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
+#endif
+ vpERROR_TRACE( "ERROR opening socket",
+ "vpClient::connectToIP()" );
+ return false;
+ }
+
+ memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
+ serv.receptorAddress.sin_family = AF_INET;
+ serv.receptorAddress.sin_addr.s_addr = inet_addr(ip.c_str());
+ serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
+
+ return connectServer(serv);
+}
+
+/*!
+ Deconnect from the server at a specific index.
+
+ \param index : Index of the server.
+*/
+void vpClient::deconnect(const unsigned int &index)
+{
+ if(index < receptor_list.size())
+ {
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ shutdown( receptor_list[index].socketFileDescriptorReceptor, SHUT_RDWR );
+#else // _WIN32
+ shutdown( receptor_list[index].socketFileDescriptorReceptor, SD_BOTH );
+#endif
+ receptor_list.erase(receptor_list.begin()+(int)index);
+ }
+}
+
+/*!
+ Stops the server and close the sockets.
+*/
+void vpClient::stop()
+{
+ for(unsigned int i = 0 ; i < receptor_list.size() ; i++){
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ shutdown( receptor_list[i].socketFileDescriptorReceptor, SHUT_RDWR );
+#else // _WIN32
+ shutdown( receptor_list[i].socketFileDescriptorReceptor, SD_BOTH );
+#endif
+ receptor_list.erase(receptor_list.begin()+(int)i);
+ i--;
+ }
+}
+
+/*!
+ Print the servers.
+*/
+void vpClient::print()
+{
+ vpNetwork::print("Server");
+}
+
+//Private function
+bool vpClient::connectServer(vpNetwork::vpReceptor &serv)
+{
+ serv.receptorAddressSize = sizeof( serv.receptorAddress );
+
+ numberOfAttempts = 15;
+ unsigned int ind = 1;
+ int connectionResult=-1;
+
+ while(ind <= numberOfAttempts){
+ std::cout << "Attempt number " << ind << "..." << std::endl;
+
+ connectionResult = connect( serv.socketFileDescriptorReceptor,
+ (sockaddr*) &serv.receptorAddress,
+ serv.receptorAddressSize );
+ if(connectionResult >= 0)
+ break;
+
+ ind++;
+ vpTime::wait(1000);
+ }
+
+ if( connectionResult< 0 )
+ {
+ vpERROR_TRACE( "ERROR connecting, the server may not be waiting for connection at this port.",
+ "vpClient::connectServer()");
+
+ return false;
+ }
+
+ receptor_list.push_back(serv);
+
+#ifdef SO_NOSIGPIPE
+ // Mac OS X does not have the MSG_NOSIGNAL flag. It does have this
+ // connections based version, however.
+ if (serv.socketFileDescriptorReceptor > 0) {
+ int set_option = 1;
+ if (0 == setsockopt(serv.socketFileDescriptorReceptor, SOL_SOCKET, SO_NOSIGPIPE, &set_option, sizeof(set_option))) {
+ } else {
+ std::cout << "Failed to set socket signal option" << std::endl;
+ }
+ }
+#endif // SO_NOSIGPIPE
+
+ std::cout << "Connected!" << std::endl;
+ return true;
+}
diff --git a/modules/core/src/tools/network/vpNetwork.cpp b/modules/core/src/tools/network/vpNetwork.cpp
new file mode 100644
index 0000000..98685eb
--- /dev/null
+++ b/modules/core/src/tools/network/vpNetwork.cpp
@@ -0,0 +1,789 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Network
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpNetwork.h>
+
+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)
+{
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+#if defined(_WIN32)
+ //Enable the sockets to be used
+ //Note that: if we were using "winsock.h" instead of "winsock2.h" we would had to use:
+ //WSAStartup(MAKEWORD(1,0), &WSAData);
+ WSADATA WSAData;
+ WSAStartup(MAKEWORD(2,0), &WSAData);
+#endif
+}
+
+vpNetwork::~vpNetwork()
+{
+#if defined(_WIN32)
+ WSACleanup();
+#endif
+}
+
+/*!
+ Add a decoding request to the emitter. This request will be used to decode the received messages.
+ Each request must have a different id.
+
+ \warning vpRequest is a virtual pure class. It has to be implemented according to the way how you want
+ to decode the message received.
+
+ \sa vpNetwork::removeDecodingRequest()
+
+ \param req : Request to add.
+*/
+void vpNetwork::addDecodingRequest(vpRequest *req)
+{
+ bool alreadyHas = false;
+
+ for(unsigned int i = 0 ; i < request_list.size() ; i++)
+ if(request_list[i]->getId() == req->getId()){
+ alreadyHas = true;
+ break;
+ }
+
+ if(alreadyHas)
+ std::cout << "Server already has one request with the similar ID. Request hasn't been added." << std::endl;
+ else
+ request_list.push_back(req);
+}
+
+/*!
+ Delete a decoding request from the emitter.
+
+ \sa vpNetwork::addDecodingRequest()
+
+ \param id : Id of the request to delete.
+*/
+void vpNetwork::removeDecodingRequest(const char *id)
+{
+ for(unsigned int i = 0 ; i < request_list.size() ; i++)
+ {
+ if(request_list[i]->getId() == id)
+ {
+ request_list.erase(request_list.begin()+(int)i);
+ break;
+ }
+ }
+}
+
+/*!
+ Print the receptors.
+
+ \param id : Message to display before the receptor's index.
+*/
+void vpNetwork::print(const char *id)
+{
+ for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
+ {
+ std::cout << id << i << " : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
+ }
+}
+
+/*!
+ Get the receptor index from its name. The name can be either the IP, or its name on the network.
+
+ \param name : Name of the receptor.
+
+ \return Index of the receptor, or -1 if an error occurs.
+*/
+int vpNetwork::getReceptorIndex(const char *name)
+{
+ struct hostent *server = gethostbyname(name);
+
+ if ( server == NULL )
+ {
+ std::string noSuchHostMessage( "ERROR, " );
+ noSuchHostMessage.append( name );
+ noSuchHostMessage.append( ": no such host\n" );
+ vpERROR_TRACE( noSuchHostMessage.c_str(), "vpNetwork::getReceptorIndex()" );
+ return -1;
+ }
+
+ std::string ip = inet_ntoa(*(in_addr *)server->h_addr);
+
+ for(int i = 0 ; i < (int)receptor_list.size() ; i++)
+ {
+ if(receptor_list[(unsigned)i].receptorIP == ip)
+ return i;
+ }
+
+ return -1;
+}
+
+/*!
+ Send a request to the first receptor in the list.
+
+ \sa vpNetwork::sendRequestTo()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+ \sa vpNetwork::send()
+ \sa vpNetwork::sendTo()
+
+ \param req : Request to send.
+
+ \return The number of bytes that have been sent, -1 if an error occured.
+*/
+int vpNetwork::sendRequest(vpRequest &req)
+{
+ return sendRequestTo(req,0);
+}
+
+/*!
+ Send a request to a specific receptor.
+
+ \sa vpNetwork::sendRequest()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+ \sa vpNetwork::send()
+ \sa vpNetwork::sendTo()
+
+ \param req : Request to send.
+ \param dest : Index of the receptor receiving the request.
+
+ \return The number of bytes that have been sent, -1 if an error occured.
+*/
+int vpNetwork::sendRequestTo(vpRequest &req, const unsigned int &dest)
+{
+ int size = (int)receptor_list.size();
+ int sizeMinusOne = (int)receptor_list.size()-1;
+ if(size == 0 || dest > (unsigned)sizeMinusOne)
+ {
+ if(verboseMode)
+ vpTRACE( "Cannot Send Request! Bad Index" );
+ return 0;
+ }
+
+ std::string message = beginning + req.getId() + separator;
+
+ if(req.size() != 0){
+ message += req[0];
+
+ for(unsigned int i = 1 ; i < req.size() ; i++){
+ message += param_sep + req[i];
+ }
+ }
+
+ message += end;
+
+ int flags = 0;
+//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
+#if defined(__linux__)
+ flags = MSG_NOSIGNAL; // Only for Linux
+#endif
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int value = sendto(receptor_list[dest].socketFileDescriptorReceptor, message.c_str(), message.size(), flags,
+ (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
+#else
+ int value = sendto((unsigned)receptor_list[dest].socketFileDescriptorReceptor, message.c_str(), (int)message.size(), flags,
+ (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
+#endif
+
+ return value;
+}
+
+/*!
+ Send and encode a request to the first receptor in the list.
+
+ \sa vpNetwork::sendRequestTo()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+ \sa vpNetwork::send()
+ \sa vpNetwork::sendTo()
+
+ \param req : Request to send.
+
+ \return The number of bytes that have been sent, -1 if an error occured.
+*/
+int vpNetwork::sendAndEncodeRequest(vpRequest &req)
+{
+ req.encode();
+ return sendRequest(req);
+}
+
+/*!
+ Send and encode a request to a specific receptor.
+
+ \sa vpNetwork::sendRequest()
+ \sa vpNetwork::sendAndEncodeRequest()
+ \sa vpNetwork::sendAndEncodeRequestTo()
+ \sa vpNetwork::send()
+ \sa vpNetwork::sendTo()
+
+ \param req : Request to send.
+ \param dest : Index of the receptor receiving the request.
+
+ \return The number of bytes that have been sent, -1 if an error occured.
+*/
+int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest)
+{
+ req.encode();
+ return sendRequestTo(req,dest);
+}
+
+/*!
+ Receive requests untils there is requests to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+*/
+std::vector<int> vpNetwork::receiveRequest()
+{
+ _receiveRequest();
+ return _handleRequests();
+}
+
+/*!
+ Receives requests, from a specific emitter, untils there is request to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message
+*/
+std::vector<int> vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting)
+{
+ _receiveRequestFrom(receptorEmitting);
+ return _handleRequests();
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value).
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::receiveRequestOnce()
+{
+ _receiveRequestOnce();
+ return _handleFirstRequest();
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message.
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting)
+{
+ _receiveRequestOnceFrom(receptorEmitting);
+ return _handleFirstRequest();
+}
+
+/*!
+ Receives and decode requests untils there is requests to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+*/
+std::vector<int> vpNetwork::receiveAndDecodeRequest()
+{
+ std::vector<int> res = receiveRequest();
+ for(unsigned int i = 0 ; i < res.size() ; i++)
+ if(res[i] != -1)
+ request_list[(unsigned)res[i]]->decode();
+
+ return res;
+}
+
+/*!
+ Receives and decode requests, from a specific emitter, untils there is request to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message
+*/
+std::vector<int> vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting)
+{
+ std::vector<int> res = receiveRequestFrom(receptorEmitting);
+ for(unsigned int i = 0 ; i < res.size() ; i++)
+ if(res[i] != -1)
+ request_list[(unsigned)res[i]]->decode();
+
+ return res;
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value).
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+ If it represents an entire request, it decodes the request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::receiveAndDecodeRequestOnce()
+{
+ int res = receiveRequestOnce();
+ if(res != -1)
+ request_list[(unsigned)res]->decode();
+
+ return res;
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+ If it represents an entire request, it decodes the request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message.
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting)
+{
+ int res = receiveRequestOnceFrom(receptorEmitting);
+ if(res != -1)
+ request_list[(unsigned)res]->decode();
+
+ return res;
+}
+
+
+//######## Definition of Template Functions ########
+//# #
+//##################################################
+
+
+/*!
+ Handle requests until there are requests to handle.
+
+ \warning : This function doesn't decode the requests. If it does handle a request that hasn't been ran yet,
+ The request's parameters will be replace.
+
+ \sa vpNetwork::handleFirstRequest()
+
+ \return : The list of index corresponding to the requests that have been handled.
+*/
+std::vector<int> vpNetwork::_handleRequests()
+{
+ std::vector<int> resIndex;
+ int index = _handleFirstRequest();
+
+ while(index != -1)
+ {
+ resIndex.push_back(index);
+ index = _handleFirstRequest();
+ }
+
+ return resIndex;
+}
+
+/*!
+ Handle the first request in the queue.
+
+ \warning : This function doesn't run the request. If it does handle a request that hasn't been ran yet,
+ The request's parameters will be replace.
+
+ \sa vpNetwork::handleRequests()
+
+ \return : The index of the request that has been handled.
+*/
+int vpNetwork::_handleFirstRequest()
+{
+ size_t indStart = currentMessageReceived.find(beginning);
+ size_t indSep = currentMessageReceived.find(separator);
+ size_t indEnd = currentMessageReceived.find(end);
+
+ if (indStart == std::string::npos && indSep == std::string::npos && indEnd == std::string::npos)
+ {
+ if(currentMessageReceived.size() != 0)
+ currentMessageReceived.clear();
+
+ if(verboseMode)
+ vpTRACE("Incorrect message");
+
+ return -1;
+ }
+
+ if(indStart == std::string::npos || indSep == std::string::npos || indEnd == std::string::npos)
+ return -1;
+
+ if(indEnd < indStart)
+ {
+ if(verboseMode)
+ vpTRACE("Incorrect message");
+ currentMessageReceived.erase((unsigned)indStart,indEnd+end.size());
+ return -1;
+ }
+
+ size_t indStart2 = currentMessageReceived.find(beginning,indStart+1);
+ if(indStart2 != std::string::npos && indStart2 < indEnd)
+ {
+ if(verboseMode)
+ vpTRACE("Incorrect message");
+ currentMessageReceived.erase((unsigned)indStart,(unsigned)indStart2);
+ return -1;
+ }
+
+ size_t deb = indStart + beginning.size();
+ std::string id = currentMessageReceived.substr((unsigned)deb, indSep - deb);
+
+ deb = indSep+separator.size();
+ std::string params = currentMessageReceived.substr((unsigned)deb, (unsigned)(indEnd - deb));
+
+// std::cout << "Handling : " << currentMessageReceived.substr(indStart, indEnd+end.size() - indStart) << std::endl;
+
+ int indRequest = 0;
+ bool hasBeenFound = false;
+ for(unsigned int i = 0 ; i < request_list.size() ; i++)
+ {
+ if(id == request_list[i]->getId()){
+ hasBeenFound = true;
+ request_list[i]->clear();
+ indRequest = (int)i;
+ break;
+ }
+ }
+
+ if(!hasBeenFound){
+ //currentMessageReceived.erase(indStart,indEnd+end.size());
+ if(verboseMode)
+ vpTRACE("No request corresponds to the received message");
+ return -1;
+ }
+
+ size_t indDebParam = indSep + separator.size();
+ size_t indEndParam = currentMessageReceived.find(param_sep,indDebParam);
+
+ std::string param;
+ while(indEndParam != std::string::npos || indEndParam < indEnd)
+ {
+ param = currentMessageReceived.substr((unsigned)indDebParam,(unsigned)(indEndParam - indDebParam));
+ request_list[(unsigned)indRequest]->addParameter(param);
+ indDebParam = indEndParam+param_sep.size();
+ indEndParam = currentMessageReceived.find(param_sep,indDebParam);
+ }
+
+ param = currentMessageReceived.substr((unsigned)indDebParam, indEnd - indDebParam);
+ request_list[(unsigned)indRequest]->addParameter(param);
+ currentMessageReceived.erase(indStart,indEnd+end.size());
+
+ return indRequest;
+}
+
+/*!
+ Receive requests untils there is requests to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+*/
+void vpNetwork::_receiveRequest()
+{
+ while(_receiveRequestOnce() > 0) {};
+}
+
+/*!
+ Receives requests, from a specific emitter, untils there is request to receive.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message
+*/
+void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting)
+{
+ while(_receiveRequestOnceFrom(receptorEmitting) > 0) {};
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value).
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::_receiveRequestOnce()
+{
+ if(receptor_list.size() == 0)
+ {
+ if(verboseMode)
+ vpTRACE( "No Receptor!" );
+ return -1;
+ }
+
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+ FD_ZERO(&readFileDescriptor);
+
+ for(unsigned int i=0; i<receptor_list.size(); i++){
+ if(i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
+ if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
+ }
+
+ int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ int numbytes = 0;
+
+ if(value == -1){
+ if(verboseMode)
+ vpERROR_TRACE( "Select error" );
+ return -1;
+ }
+ else if(value == 0){
+ //Timeout
+ return 0;
+ }
+ 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];
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ numbytes=recv(receptor_list[i].socketFileDescriptorReceptor, buf, max_size_message, 0);
+#else
+ numbytes=recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, buf, (int)max_size_message, 0);
+#endif
+
+ if(numbytes <= 0)
+ {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
+ receptor_list.erase(receptor_list.begin()+(int)i);
+ delete [] buf;
+ return numbytes;
+ }
+ else if(numbytes > 0){
+ std::string returnVal(buf, (unsigned int)numbytes);
+ currentMessageReceived.append(returnVal);
+ }
+ delete [] buf;
+ break;
+ }
+ }
+ }
+
+ return numbytes;
+}
+
+/*!
+ Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
+ This message can represent an entire request or not. Several calls to this function
+ might be necessary to get the entire request.
+
+ \warning Requests will be received but not decoded.
+
+ \sa vpNetwork::receive()
+ \sa vpNetwork::receiveRequestFrom()
+ \sa vpNetwork::receiveRequest()
+ \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequest()
+ \sa vpNetwork::receiveAndDecodeRequestFrom()
+ \sa vpNetwork::receiveAndDecodeRequestOnce()
+ \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
+
+ \param receptorEmitting : Index of the receptor emitting the message.
+
+ \return The number of bytes received, -1 if an error occured.
+*/
+int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting)
+{
+ int size = (int)receptor_list.size();
+ int sizeMinusOne = (int)receptor_list.size()-1;
+ if(size == 0 || receptorEmitting > (unsigned)sizeMinusOne )
+ {
+ if(verboseMode)
+ vpTRACE( "No receptor at the specified index!" );
+ return -1;
+ }
+
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+ FD_ZERO(&readFileDescriptor);
+
+ socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;
+ FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);
+
+ int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ int numbytes = 0;
+ if(value == -1){
+ if(verboseMode)
+ vpERROR_TRACE( "Select error" );
+ return -1;
+ }
+ else if(value == 0){
+ //Timeout
+ return 0;
+ }
+ 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
+ numbytes=recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, max_size_message, 0);
+#else
+ numbytes=recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, (int)max_size_message, 0);
+#endif
+ if(numbytes <= 0)
+ {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;
+ receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
+ delete [] buf;
+ return numbytes;
+ }
+ else if(numbytes > 0){
+ std::string returnVal(buf, (unsigned int)numbytes);
+ currentMessageReceived.append(returnVal);
+ }
+ delete [] buf;
+ }
+ }
+
+ return numbytes;
+}
+
+
+
diff --git a/modules/core/src/tools/network/vpRequest.cpp b/modules/core/src/tools/network/vpRequest.cpp
new file mode 100644
index 0000000..cd4ea88
--- /dev/null
+++ b/modules/core/src/tools/network/vpRequest.cpp
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Network request.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpRequest.h>
+
+vpRequest::vpRequest()
+ : request_id(""), listOfParams()
+{}
+
+vpRequest::~vpRequest()
+{}
+
+/*!
+ Add a message as parameter of the request.
+
+ \sa vpNetwork::addParameterObject()
+
+ \param params : Array of characters representing the message to add.
+*/
+void vpRequest::addParameter(char *params)
+{
+ std::string val = params;
+ listOfParams.push_back(val);
+}
+
+/*!
+ Add a message as parameter of the request.
+
+ \sa vpNetwork::addParameterObject()
+
+ \param params : std::string representing the message to add.
+*/
+void vpRequest::addParameter(std::string ¶ms)
+{
+ listOfParams.push_back(params);
+}
+
+/*!
+ Add messages as parameters of the request.
+ Each message corresponds to one parameter.
+
+ \sa vpNetwork::addParameterObject()
+
+ \param listOfparams : Array of std::string representing the messages to add.
+*/
+void vpRequest::addParameter(std::vector< std::string > &listOfparams)
+{
+ for(unsigned int i = 0; i < listOfparams.size() ; i++)
+ listOfparams.push_back(listOfparams[i]);
+}
diff --git a/modules/core/src/tools/network/vpServer.cpp b/modules/core/src/tools/network/vpServer.cpp
new file mode 100644
index 0000000..f96e5ae
--- /dev/null
+++ b/modules/core/src/tools/network/vpServer.cpp
@@ -0,0 +1,281 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * TCP Server
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpServer.h>
+
+
+/*!
+ Construct a server on the machine launching it.
+*/
+vpServer::vpServer( ) : adress(), port(0), started(false), max_clients(10)
+{
+ int protocol = 0;
+ emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if (emitter.socketFileDescriptorEmitter < 0)
+#else
+ if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
+#endif
+ {
+ vpERROR_TRACE( "vpServer::vpServer(), cannot open socket." );
+ }
+ emitter.emitterAddress.sin_family = AF_INET;
+ emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY;
+ emitter.emitterAddress.sin_port = 0;
+
+ adress = inet_ntoa(emitter.emitterAddress.sin_addr);
+ port = emitter.emitterAddress.sin_port;
+}
+
+/*!
+ Construct a server on the machine launching it, with a specified port.
+
+ \param port_serv : server's port.
+*/
+vpServer::vpServer( const int &port_serv ) : adress(), port(0), started(false), max_clients(10)
+{
+ int protocol = 0;
+ emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if (emitter.socketFileDescriptorEmitter < 0)
+#else
+ if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
+#endif
+ {
+ vpERROR_TRACE( "vpServer::vpServer(const int &port_serv), cannot open socket." );
+ }
+ emitter.emitterAddress.sin_family = AF_INET;
+ emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY; //inet_addr("127.0.0.1");;
+ emitter.emitterAddress.sin_port = htons( (unsigned short)port_serv );
+
+ adress = inet_ntoa(emitter.emitterAddress.sin_addr);
+ port = port_serv;
+}
+
+/*!
+ Construct a server on the machine at a given adress, with a specified port.
+
+ \param adress_serv : server's adress.
+ \param port_serv : server's port.
+*/
+vpServer::vpServer( const std::string &adress_serv,const int &port_serv )
+ : adress(), port(0), started(false), max_clients(10)
+{
+ int protocol = 0;
+ emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if (emitter.socketFileDescriptorEmitter < 0)
+#else
+ if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
+#endif
+ {
+ vpERROR_TRACE( "vpServer::vpServer(const std::string &adress_serv,const int &port_serv), cannot open socket." );
+ }
+ emitter.emitterAddress.sin_family = AF_INET;
+ emitter.emitterAddress.sin_addr.s_addr = inet_addr(adress_serv.c_str());
+ emitter.emitterAddress.sin_port = htons( (unsigned short)port_serv );
+
+ adress = adress_serv;
+ port = port_serv;
+}
+
+/*!
+ Shutdown the server.
+*/
+vpServer::~vpServer()
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ close( emitter.socketFileDescriptorEmitter );
+#else //Win32
+ closesocket( (unsigned)emitter.socketFileDescriptorEmitter );
+#endif
+
+ for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ close( receptor_list[i].socketFileDescriptorReceptor );
+#else //Win32
+ closesocket( (unsigned)receptor_list[i].socketFileDescriptorReceptor );
+#endif
+
+}
+
+/*!
+ Enable the server to wait for clients (on the limit of the maximum limit).
+
+ \return True if the server has started, false otherwise.
+*/
+bool vpServer::start()
+{
+ int serverStructLength = sizeof(emitter.emitterAddress);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int bindResult = bind( emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, (unsigned)serverStructLength );
+#else //Win32
+ int bindResult = bind( (unsigned)emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, serverStructLength );
+#endif
+
+
+ if( bindResult < 0 )
+ {
+ std::cout << "Error id : " << bindResult << std::endl;
+ std::string errorMessage( "vpServer::vpServer(), cannot bind to port" );
+ char posrtNumberString[32];
+ sprintf( posrtNumberString, "%d", port );
+ errorMessage += " ";
+ errorMessage += posrtNumberString;
+ errorMessage += " The port may be already used.";
+ vpERROR_TRACE( errorMessage.c_str() );
+ return false;
+ }
+
+#ifdef SO_NOSIGPIPE
+ // Mac OS X does not have the MSG_NOSIGNAL flag. It does have this
+ // connections based version, however.
+ if (emitter.socketFileDescriptorEmitter > 0) {
+ int set_option = 1;
+ if (0 == setsockopt(emitter.socketFileDescriptorEmitter, SOL_SOCKET, SO_NOSIGPIPE, &set_option, sizeof(set_option))) {
+ } else {
+ std::cout << "Failed to set socket signal option" << std::endl;
+ }
+ }
+#endif // SO_NOSIGPIPE
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ listen( emitter.socketFileDescriptorEmitter, (int)max_clients );
+#else //Win32
+ listen( (unsigned)emitter.socketFileDescriptorEmitter, (int)max_clients );
+#endif
+
+ std::cout << "Server ready" << std::endl;
+
+ started = true;
+
+ return true;
+}
+
+/*!
+ Check if a client has connected or deconnected the server
+
+ \return True if a client connected or deconnected, false otherwise OR server not started yet.
+*/
+bool vpServer::checkForConnections()
+{
+ if(!started)
+ if(!start()){
+ return false;
+ }
+
+ tv.tv_sec = tv_sec;
+ tv.tv_usec = tv_usec;
+
+ FD_ZERO(&readFileDescriptor);
+
+ socketMax = emitter.socketFileDescriptorEmitter;
+ FD_SET((unsigned)emitter.socketFileDescriptorEmitter,&readFileDescriptor);
+
+ for(unsigned int i=0; i<receptor_list.size(); i++){
+ FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
+
+ if(i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
+ }
+
+ int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ if(value == -1){
+ //vpERROR_TRACE( "vpServer::run(), select()" );
+ return false;
+ }
+ else if(value == 0){
+ return false;
+ }
+ else{
+ if(FD_ISSET((unsigned int)emitter.socketFileDescriptorEmitter,&readFileDescriptor)){
+ vpNetwork::vpReceptor client;
+ client.receptorAddressSize = sizeof(client.receptorAddress);
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ client.socketFileDescriptorReceptor = accept(emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
+#else //Win32
+ client.socketFileDescriptorReceptor = accept((unsigned int)emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
+#endif
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if((client.socketFileDescriptorReceptor) == -1)
+#else
+ if((client.socketFileDescriptorReceptor) == INVALID_SOCKET)
+#endif
+ vpERROR_TRACE( "vpServer::run(), accept()" );
+
+ client.receptorIP = inet_ntoa(client.receptorAddress.sin_addr);
+ printf("New client connected : %s\n", inet_ntoa(client.receptorAddress.sin_addr));
+ receptor_list.push_back(client);
+
+ return true;
+ }
+ else{
+ for(unsigned int i=0; i<receptor_list.size(); i++){
+ if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
+ char deco;
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ int numbytes = recv(receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
+#else //Win32
+ int numbytes = recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
+#endif
+
+
+ if(numbytes == 0)
+ {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
+ receptor_list.erase(receptor_list.begin()+(int)i);
+ return 0;
+ }
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
+/*!
+ Print the connected clients.
+*/
+void vpServer::print()
+{
+ vpNetwork::print("Client");
+}
+
diff --git a/modules/core/src/tools/time/vpTime.cpp b/modules/core/src/tools/time/vpTime.cpp
new file mode 100644
index 0000000..0abc2c6
--- /dev/null
+++ b/modules/core/src/tools/time/vpTime.cpp
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Time management and measurement.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpDebug.h>
+#include <iostream>
+
+
+/*!
+ \file vpTime.cpp
+ \brief Time management and measurement
+
+*/
+
+
+// Unix depend version
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# include <sys/time.h>
+# include <unistd.h>
+#elif defined(_WIN32)
+# include <windows.h>
+# include <winbase.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+namespace vpTime
+{
+#endif
+
+/*!
+ This time is in milli-seconds. Threshold to activate usleep() in waiting
+ methods (see wait()). This threshold is needed, because usleep() is not
+ accurate on many machines. Call sleep() functions during the time to wait
+ minus vpTime::minTimeForUsleepCall. The rest of the time to wait is managed
+ by a while loop.
+*/
+double minTimeForUsleepCall = 4;
+
+/*!
+ \return The time during which a while loop is used to handle the time
+ wainting functions. When the time to wait is greater than that value, we use
+ non blocking functions like usleep() instead.
+ */
+double getMinTimeForUsleepCall()
+{
+ return minTimeForUsleepCall;
+}
+
+/*!
+ \fn vpTime::measureTimeMs()
+ Return the time in milliseconds since January 1st 1970.
+
+ \sa measureTimeMicros(), measureTimeSecond()
+*/
+double measureTimeMs()
+{
+#if defined(_WIN32)
+ LARGE_INTEGER time, frequency;
+ QueryPerformanceFrequency(&frequency);
+ if(frequency.QuadPart == 0){
+ return(timeGetTime());
+ }
+ else{
+ QueryPerformanceCounter(&time);
+ return (double)(1000.0*time.QuadPart/frequency.QuadPart);
+ }
+#elif !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ struct timeval tp;
+ gettimeofday(&tp,0);
+ return(1000.0*tp.tv_sec + tp.tv_usec/1000.0);
+#endif
+}
+
+/*!
+ Return the time in microseconds since January 1st 1970.
+
+ \sa measureTimeMs(), measureTimeSecond()
+*/
+double measureTimeMicros()
+{
+#if defined(_WIN32)
+ LARGE_INTEGER time, frequency;
+ QueryPerformanceFrequency(&frequency);
+ if(frequency.QuadPart == 0){
+ return(timeGetTime());
+ }
+ else{
+ QueryPerformanceCounter(&time);
+ return (double)(1000000.0*time.QuadPart/frequency.QuadPart);
+ }
+#else
+
+ struct timeval tp;
+ gettimeofday(&tp,0);
+ return(1000000.0*tp.tv_sec + tp.tv_usec);
+#endif
+}
+
+/*!
+
+ Wait t miliseconds after t0 (in ms).
+
+ The waiting is done by a call to usleep() if the time to wait is greater than
+ vpTime::minTimeForUsleepCall.
+
+ \param t0 : reference time (in ms)
+ \param t : time to wait (in ms)
+
+ \return 0 : The function did wait.
+ \return 1 : The time was already over, no need to wait.
+*/
+int wait(double t0, double t)
+{
+ double timeCurrent, timeToWait;
+ timeCurrent = measureTimeMs();
+
+ timeToWait = t0 + t - timeCurrent;
+
+ if ( timeToWait <= 0. ) // no need to wait
+ return(1);
+ else {
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if (timeToWait > vpTime::minTimeForUsleepCall) {
+ usleep((unsigned long )((timeToWait-vpTime::minTimeForUsleepCall)*1000));
+ }
+#elif defined(_WIN32)
+ if (timeToWait > vpTime::minTimeForUsleepCall) {
+ Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
+ }
+#endif
+ // Blocking loop to have an accurate waiting
+ do {
+ timeCurrent = measureTimeMs();
+ timeToWait = t0 + t - timeCurrent;
+
+ } while (timeToWait > 0.);
+
+ return 0;
+ }
+}
+
+/*!
+ Wait t miliseconds from now.
+
+ The waiting is done by a call to usleep() if the time to wait is greater than
+ vpTime::minTimeForUsleepCall.
+
+ \param t : Time to wait in ms.
+
+*/
+void wait(double t)
+{
+ double t0, timeCurrent, timeToWait;
+ t0 = timeCurrent = measureTimeMs();
+
+ timeToWait = t;
+
+ if ( timeToWait <= 0. ) // no need to wait
+ return;
+ else {
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ if (timeToWait > vpTime::minTimeForUsleepCall) {
+ usleep((unsigned long )((timeToWait-vpTime::minTimeForUsleepCall)*1000));
+ }
+#elif defined(_WIN32)
+ if (timeToWait > vpTime::minTimeForUsleepCall) {
+ Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
+ }
+#endif
+ // Blocking loop to have an accurate waiting
+ do {
+ timeCurrent = measureTimeMs();
+ timeToWait = t0 + t - timeCurrent;
+
+ } while (timeToWait > 0.);
+
+ return;
+ }
+}
+
+/*!
+
+ Return the measured time in seconds since January 1st 1970.
+
+ \sa measureTimeMs()
+*/
+double measureTimeSecond()
+{
+ return vpTime::measureTimeMs()/1000.0 ;
+}
+
+/*!
+ Sleep t miliseconds from now.
+
+ \param t : Time to sleep in ms.
+
+*/
+void sleepMs(double t)
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ usleep((unsigned long )(t*1000));
+#elif defined(_WIN32)
+ Sleep((DWORD)(t));
+#endif
+}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+};
+#endif
diff --git a/modules/core/src/tools/xml/vpXmlParser.cpp b/modules/core/src/tools/xml/vpXmlParser.cpp
new file mode 100644
index 0000000..599c2ad
--- /dev/null
+++ b/modules/core/src/tools/xml/vpXmlParser.cpp
@@ -0,0 +1,411 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tool to automatise the creation of xml parser based on the libXML2
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpXmlParser.h>
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpDebug.h>
+#include <libxml/parser.h>
+
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <typeinfo>
+
+/*!
+ Basic constructor.
+
+ Initialise the main tag with default value.
+*/
+vpXmlParser::vpXmlParser() : nodeMap(), main_tag("config")
+{
+}
+
+/*!
+ Basic destructor that does nothing.
+
+ \warning As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
+ to clean up memory allocated by the xml2 library itself, the user should call
+ xmlCleanupParser() only when the process has finished using the xml2 library.
+ In case of doubt abstain from calling this function or do it just before calling exit()
+ to avoid leak reports from valgrind ! That's why
+ in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
+ function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
+ just before exit().
+
+ \sa cleanup()
+*/
+vpXmlParser::~vpXmlParser()
+{
+ //xmlCleanupParser();
+}
+
+/*!
+ Cpoy constructor.
+
+ \param _twin : The parser to copy.
+*/
+vpXmlParser::vpXmlParser(const vpXmlParser& _twin)
+ : nodeMap(), main_tag("config")
+{
+ main_tag = _twin.main_tag;
+ nodeMap = _twin.nodeMap;
+}
+
+/* utilities functions to read/write data from an xml document */
+
+/*!
+ Read an array of character.
+
+ \warning The array of characters is allocated and must be explicitly freed to
+ avoid memory leak.
+
+ \param doc : The main xml document
+ \param node : a pointer to the node to read value
+
+ \return pointer to an allocated array of character.
+*/
+char*
+vpXmlParser::xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node)
+{
+ if(node ->xmlChildrenNode == NULL){
+ std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read char*";
+ std::cerr << errorMsg << std::endl;
+ throw vpException(vpException::fatalError, errorMsg);
+ }
+ return (char *) xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+}
+
+/*!
+ Read an array of character.
+
+
+ \param doc : The main xml document
+ \param node : a pointer to the node to read value
+
+ \return std::string representing the value.
+*/
+
+std::string
+vpXmlParser::xmlReadStringChild (xmlDocPtr doc, xmlNodePtr node)
+{
+ if(node ->xmlChildrenNode == NULL){
+ std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read std::string";
+ std::cerr << errorMsg << std::endl;
+ throw vpException(vpException::fatalError, errorMsg);
+ }
+ char* dataCh = (char*)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ std::string dataStr = dataCh;
+ xmlFree(dataCh);
+ return dataStr;
+}
+
+/*!
+ read an int
+
+ \warning throw a vpException::ioError if the value cannot be parsed to an integer
+
+ \param doc : The main xml document
+ \param node : a pointer to the node to read value
+
+ \return the integer value in the node
+*/
+int
+vpXmlParser::xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node)
+{
+ if(node ->xmlChildrenNode == NULL){
+ std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read int";
+ std::cerr << errorMsg << std::endl;
+ throw vpException(vpException::fatalError, errorMsg);
+ }
+ char * val_char;
+ char * control_convert;
+ int val_int;
+
+ val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
+ val_int = strtol ((char *)val_char, &control_convert, 10);
+
+ if (val_char == control_convert){
+ xmlFree((xmlChar*) val_char);
+ throw vpException(vpException::ioError, "cannot parse entry to int");
+ }
+ xmlFree((xmlChar*) val_char);
+
+ return val_int;
+}
+
+/*!
+ read an int
+
+ \warning throw a vpException::ioError if the value cannot be parsed to an integer
+
+ \param doc : The main xml document
+ \param node : a pointer to the node to read value
+
+ \return the unsigned integer value in the node
+*/
+unsigned int
+vpXmlParser::xmlReadUnsignedIntChild (xmlDocPtr doc, xmlNodePtr node)
+{
+ if(node ->xmlChildrenNode == NULL){
+ std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read unsigned int";
+ std::cerr << errorMsg << std::endl;
+ throw vpException(vpException::fatalError, errorMsg);
+ }
+ char * val_char;
+ char * control_convert;
+ unsigned int val_uint;
+
+ val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
+ val_uint = strtoul ((char *)val_char, &control_convert, 10);
+
+ if (val_char == control_convert){
+ xmlFree((xmlChar*) val_char);
+ throw vpException(vpException::ioError, "cannot parse entry to int");
+ }
+ xmlFree((xmlChar*) val_char);
+
+ return val_uint;
+}
+
+
+/*!
+ read a double
+
+ \warning throw a vpException::ioError if the value cannot be parsed to an integer
+
+ \param doc : The main xml document
+ \param node : a pointer to the node to read value
+
+ \return the double value in the node
+*/
+double
+vpXmlParser::xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node)
+{
+ if(node ->xmlChildrenNode == NULL){
+ std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read double";
+ std::cerr << errorMsg << std::endl;
+ throw vpException(vpException::fatalError, errorMsg);
+ }
+ char * val_char;
+ char * control_convert;
+ double val_double;
+
+ val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
+ val_double = strtod ((char *)val_char, &control_convert);
+
+ if (val_char == control_convert){
+ xmlFree((xmlChar*) val_char);
+ throw vpException(vpException::ioError, "cannot parse entry to double");
+ }
+ xmlFree((xmlChar*) val_char);
+ return val_double;
+}
+
+/*!
+ write an array of character.
+
+ \param node : a pointer to the node to read value
+ \param label : label (name of the data) of the node
+ \param value : pointer to the array of character to write
+*/
+void
+vpXmlParser::xmlWriteCharChild(xmlNodePtr node, const char* label, const char* value)
+{
+ xmlNodePtr tmp;
+ tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)value);
+ xmlAddChild(node, tmp);
+}
+
+/*!
+ write an array of character.
+
+ \param node : a pointer to the node to read value
+ \param label : label (name of the data) of the node
+ \param value : std::string to write;
+*/
+void
+vpXmlParser::xmlWriteStringChild(xmlNodePtr node, const char* label, const std::string& value)
+{
+ xmlNodePtr tmp;
+ tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)value.c_str());
+ xmlAddChild(node, tmp);
+}
+
+
+/*!
+ write an integer.
+
+ \param node : a pointer to the node to read value
+ \param label : label (name of the data) of the node
+ \param value : integer to write
+*/
+void
+vpXmlParser::xmlWriteIntChild(xmlNodePtr node, const char* label, const int value)
+{
+ char str[100];
+ sprintf(str, "%d", value);
+ xmlNodePtr tmp;
+ tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
+ xmlAddChild(node, tmp);
+}
+
+/*!
+ write an unsigned integer.
+
+ \param node : a pointer to the node to read value
+ \param label : label (name of the data) of the node
+ \param value : unsigned integer to write
+*/
+void
+vpXmlParser::xmlWriteUnsignedIntChild(xmlNodePtr node, const char* label, const unsigned int value)
+{
+ char str[100];
+ sprintf(str, "%u", value);
+ xmlNodePtr tmp;
+ tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
+ xmlAddChild(node, tmp);
+}
+
+
+/*!
+ write a double.
+
+ \param node : a pointer to the node to read value
+ \param label : label (name of the data) of the node
+ \param value : double to write
+*/
+void
+vpXmlParser::xmlWriteDoubleChild(xmlNodePtr node, const char* label, const double value)
+{
+ char str[100];
+ sprintf(str, "%lf", value);
+ xmlNodePtr tmp;
+ tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
+ xmlAddChild(node, tmp);
+}
+
+
+/* -------------------------------------------------------------------------- */
+/* MAIN METHODS */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ parse the document.
+ The data in the file are stored in the attributes of the child class. This
+ method calls the readMainClass method which has to be implemented for every
+ child class depending on the content to parse.
+
+ \param filename : name of the file to parse
+*/
+void
+vpXmlParser::parse(const std::string& filename)
+{
+ xmlDocPtr doc;
+ xmlNodePtr root_node;
+
+ doc = xmlParseFile(filename.c_str());
+ if(doc == NULL){
+ vpERROR_TRACE("cannot open file");
+ throw vpException(vpException::ioError, "cannot open file");
+ }
+
+ root_node = xmlDocGetRootElement(doc);
+ if(root_node == NULL){
+ vpERROR_TRACE("cannot get root element");
+ throw vpException(vpException::ioError, "cannot get root element");
+ }
+
+ readMainClass(doc, root_node);
+
+ xmlFreeDoc(doc);
+}
+
+/*!
+ Save the content of the class in the file given in parameters.
+ The data of the class are in the child class.
+ This method calls the write_main_class method which has to be implemented for
+ every class depending on the data to save.
+
+ \param filename : the name of the file used to record the data
+ \param append : if true and if the file exists, the data will be added to the
+ data already in the file
+*/
+void
+vpXmlParser::save(const std::string& filename, const bool append)
+{
+ xmlDocPtr doc;
+ xmlNodePtr root_node;
+
+ doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR + XML_PARSE_NOBLANKS);
+ if (doc == NULL){
+ doc = xmlNewDoc ((xmlChar*)"1.0");
+ root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
+ xmlDocSetRootElement(doc, root_node);
+ }
+ else{
+ if(!append){
+ xmlFreeDoc(doc);
+ if (remove(filename.c_str()) != 0)
+ throw vpException(vpException::ioError, "Cannot remove existing xml file");
+
+ doc = xmlNewDoc ((xmlChar*)"1.0");
+ root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
+ xmlDocSetRootElement(doc, root_node);
+ }
+ }
+
+ root_node = xmlDocGetRootElement(doc);
+ if (root_node == NULL)
+ {
+ vpERROR_TRACE("problem to get the root node");
+ throw vpException(vpException::ioError, "problem to get the root node");
+ }
+
+ writeMainClass(root_node);
+
+ xmlSaveFormatFile(filename.c_str(), doc, 1);
+ xmlFreeDoc(doc);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpXmlParser.cpp.o) has no symbols
+void dummy_vpXmlParser() {};
+#endif
diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp
new file mode 100644
index 0000000..d91ac41
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp
@@ -0,0 +1,410 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature circle.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpCircle.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+void
+vpCircle::init()
+{
+
+ oP.resize(7) ;
+ cP.resize(7) ;
+
+ p.resize(5) ;
+}
+
+/*!
+ Set the world coordinates of the circle from the intersection of a plane and a sphere.
+ We mean here the coordinates of the circle in the object frame
+
+ \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
+ oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
+ oP[6] corresponds to the radius of the sphere.
+*/
+void
+vpCircle::setWorldCoordinates(const vpColVector& oP_)
+{
+ this->oP = oP_ ;
+}
+
+/*!
+ Set the world coordinates of the circle from the intersection of a plane and a sphere.
+ We mean here the coordinates of the circle in the object frame
+
+ \param A : A from the plane equation Ax + By + Cz = 0.
+ \param B : B from the plane equation Ax + By + Cz = 0.
+ \param C : C from the plane equation Ax + By + Cz = 0.
+ \param X0 : X Coordinate of the center of the sphere.
+ \param Y0 : Y Coordinate of the center of the sphere.
+ \param Z0 : Z Coordinate of the center of the sphere.
+ \param R : Radius of the sphere.
+*/
+void
+vpCircle::setWorldCoordinates(const double A, const double B, const double C,
+ const double X0, const double Y0, const double Z0,
+ const double R)
+{
+ oP[0] = A ;
+ oP[1] = B ;
+ oP[2] = C ;
+ oP[3] = X0 ;
+ oP[4] = Y0 ;
+ oP[5] = Z0 ;
+ oP[6] = R ;
+}
+
+
+
+vpCircle::vpCircle()
+{
+ init() ;
+}
+
+/*!
+ Construct the circle from the intersection of a plane and a sphere.
+
+ \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
+ oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
+ oP[6] corresponds to the radius of the sphere.
+
+ \sa setWorldCoordinates()
+*/
+vpCircle::vpCircle(const vpColVector& oP_)
+{
+ init() ;
+ setWorldCoordinates(oP_) ;
+}
+
+/*!
+ Construct the circle from the intersection of a plane and a sphere.
+
+ \param A : A from the plane equation Ax + By + Cz = 0.
+ \param B : B from the plane equation Ax + By + Cz = 0.
+ \param C : C from the plane equation Ax + By + Cz = 0.
+ \param X0 : X Coordinate of the center of the sphere.
+ \param Y0 : Y Coordinate of the center of the sphere.
+ \param Z0 : Z Coordinate of the center of the sphere.
+ \param R : Radius of the sphere.
+
+ \sa setWorldCoordinates()
+*/
+vpCircle::vpCircle(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R)
+{
+ init() ;
+ setWorldCoordinates(A, B, C,
+ X0, Y0, Z0,
+ R) ;
+}
+
+vpCircle::~vpCircle()
+{
+}
+
+
+
+
+/*!
+ Perspective projection of the circle.
+
+ From the 3D parameters of the circle in the camera frame available in cP, computes the 2D parameters of the ellipse resulting from the perspective projection in the image plane. Those 2D parameters are available in p vector.
+
+ See vpCircle::projection(const vpColVector &, vpColVector &) for a more detailed description of the parameters.
+ */
+void
+vpCircle::projection()
+{
+ projection(cP,p) ;
+}
+
+/*!
+ Perspective projection of the circle.
+ \param cP_: 3D cercle input parameters. This vector is of dimension 7. It contains the following parameters: A, B, C, X0, Y0, Z0, r where
+ - A,B,C are the parameters of the plane with equation Ax+By+Cz+D=0 containing the circle
+ - X0,Y0,Z0 are the 3D coordinates of the cercle in the camera frame
+ - r is the circle radius.
+
+ \param p_: 2D circle output parameters. This is a 5 dimension vector. It contains the following parameters: xc, yc, m20, m11, m02 where:
+ - xc,yc are the normalized coordinates of the center of the ellipse (ie the perspective projection of a 3D
+ circle becomes a 2D ellipse in the image) in the image plane.
+ - mu20,mu11,mu02 are the second order centered moments of the ellipse.
+ */
+void
+vpCircle::projection(const vpColVector &cP_, vpColVector &p_)
+{
+ vpColVector K(6) ;
+ {
+ double A = cP_[0] ;
+ double B = cP_[1] ;
+ double C = cP_[2] ;
+
+ double X0 = cP_[3] ;
+ double Y0 = cP_[4] ;
+ double Z0 = cP_[5] ;
+
+ double r = cP_[6];
+
+ // projection
+ double s = X0*X0 + Y0*Y0 + Z0*Z0 - r*r ;
+ double det = A*X0+B*Y0+C*Z0;
+ A = A/det ;
+ B = B/det ;
+ C = C/det ;
+
+ K[0] = 1 - 2*A*X0 + A*A*s;
+ K[1] = 1 - 2*B*Y0 + B*B*s;
+ K[2] = -A*Y0 - B*X0 + A*B*s;
+ K[3] = -C*X0 - A*Z0 + A*C*s;
+ K[4] = -C*Y0 - B*Z0 + B*C*s;
+ K[5] = 1 - 2*C*Z0 + C*C*s;
+
+ }
+
+// {
+// std::cout << "K dans vpCircle::projection(): " << std::endl;
+// for (unsigned int i=1; i<6; i++)
+// std::cout << K[i]/K[0] << std::endl;
+// }
+ double det = K[2]*K[2] -K[0]*K[1];
+ if (fabs(det) < 1e-8)
+ {
+ vpERROR_TRACE("division par 0") ;
+ throw(vpException(vpException::divideByZeroError,
+ "division par 0")) ;
+ }
+
+ double xc = (K[1]*K[3]-K[2]*K[4])/det;
+ double yc = (K[0]*K[4]-K[2]*K[3])/det;
+
+ double c = sqrt( (K[0]-K[1])*(K[0]-K[1]) + 4*K[2]*K[2] );
+ double s = 2*(K[0]*xc*xc + 2*K[2]*xc*yc + K[1]*yc*yc - K[5]);
+
+ double A,B,E ;
+
+ //if (fabs(K[2])<1e-6)
+ if (fabs(K[2])<std::numeric_limits<double>::epsilon())
+ {
+ E = 0.0;
+ if (K[0] > K[1])
+ {
+ A = sqrt(s/(K[0] + K[1] + c));
+ B = sqrt(s/(K[0] + K[1] - c));
+ }
+ else
+ {
+ A = sqrt(s/(K[0] + K[1] - c));
+ B = sqrt(s/(K[0] + K[1] + c));
+ }
+ }
+ else
+ {
+ E = (K[1] - K[0] + c)/(2*K[2]);
+ if ( fabs(E) > 1.0)
+ {
+ A = sqrt(s/(K[0] + K[1] + c));
+ B = sqrt(s/(K[0] + K[1] - c));
+ }
+ else
+ {
+ A = sqrt(s/(K[0] + K[1] - c));
+ B = sqrt(s/(K[0] + K[1] + c));
+ E = -1.0/E;
+ }
+ }
+
+ det = (1.0 + vpMath::sqr(E));
+ double mu20 = (vpMath::sqr(A) + vpMath::sqr(B*E)) /det ;
+ double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) *E / det ;
+ double mu02 = (vpMath::sqr(B) + vpMath::sqr(A*E)) / det ;
+
+ p_[0] = xc ;
+ p_[1] = yc ;
+ p_[2] = mu20 ;
+ p_[3] = mu11 ;
+ p_[4] = mu02 ;
+}
+
+//! perspective projection of the circle
+void
+vpCircle::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+{
+
+ double A,B,C ;
+ A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
+ B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
+ C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
+
+ double X0,Y0,Z0 ;
+ X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
+ Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
+ Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
+ double R = oP[6] ;
+
+ cP_[0] = A ;
+ cP_[1] = B ;
+ cP_[2] = C ;
+
+ cP_[3] = X0 ;
+ cP_[4] = Y0 ;
+ cP_[5] = Z0 ;
+
+ cP_[6] = R ;
+
+ // vpTRACE("_cP :") ; std::cout << _cP.t() ;
+
+}
+
+//! perspective projection of the circle
+void
+vpCircle::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+
+ double A,B,C ;
+ A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
+ B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
+ C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
+
+ double X0,Y0,Z0 ;
+ X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
+ Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
+ Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
+ double R = oP[6] ;
+
+ cP[0] = A ;
+ cP[1] = B ;
+ cP[2] = C ;
+
+ cP[3] = X0 ;
+ cP[4] = Y0 ;
+ cP[5] = Z0 ;
+
+ cP[6] = R ;
+
+ // vpTRACE("_cP :") ; std::cout << _cP.t() ;
+
+}
+
+void vpCircle::display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
+ cam, I, color, thickness) ;
+}
+
+// non destructive wrt. cP and p
+void vpCircle::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpColVector _cP, _p ;
+ changeFrame(cMo,_cP) ;
+ projection(_cP,_p) ;
+ vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
+ cam, I, color, thickness) ;
+
+}
+//! for memory issue (used by the vpServo class only)
+vpCircle *vpCircle::duplicate() const
+{
+ vpCircle *feature = new vpCircle(*this) ;
+ return feature ;
+}
+
+/*!
+ Computes the coordinates of the point corresponding to the intersection
+ between a circle and a line.
+
+ \warning This functions assumes changeFrame() and projection() have already been called.
+
+ \sa changeFrame(), projection()
+
+ \param circle : Circle to consider for the intersection.
+ \param cam : Camera parameters that have to be used for the intersection computation.
+ \param rho : The rho parameter of the line.
+ \param theta : The theta parameter of the line.
+ \param i : resulting i-coordinate of the intersection point.
+ \param j : resulting j-coordinate of the intersection point.
+*/
+void
+vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
+{
+ // This was taken from the code of art-v1. (from the artCylinder class)
+ double px = cam.get_px() ;
+ double py = cam.get_py() ;
+ double u0 = cam.get_u0() ;
+ double v0 = cam.get_v0() ;
+
+ double mu11 = circle.p[3];
+ double mu02 = circle.p[4];
+ double mu20 = circle.p[2];
+ double Xg = u0 + circle.p[0]*px;
+ double Yg = v0 + circle.p[1]*py;
+
+ // Find Intersection between line and ellipse in the image.
+
+ // Optimised calculation for X
+ double stheta = sin(theta);
+ double ctheta = cos(theta);
+ double sctheta = stheta*ctheta;
+ double m11yg = mu11*Yg;
+ double ctheta2 = vpMath::sqr(ctheta);
+ double m02xg = mu02*Xg;
+ double m11stheta = mu11*stheta;
+ j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
+ -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
+ m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
+ +mu02-mu02*ctheta2));
+ //Optimised calculation for Y
+ double rhom02 = rho*mu02;
+ double sctheta2 = stheta*ctheta2;
+ double ctheta3 = ctheta2*ctheta;
+ i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
+ +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
+ +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
+ (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
+ mu02*ctheta2)/stheta);
+}
diff --git a/modules/core/src/tracking/forward-projection/vpCylinder.cpp b/modules/core/src/tracking/forward-projection/vpCylinder.cpp
new file mode 100644
index 0000000..ab2ecaa
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpCylinder.cpp
@@ -0,0 +1,425 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Cylinder feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+void
+vpCylinder::init()
+{
+
+ oP.resize(7) ;
+ cP.resize(7) ;
+
+ p.resize(4) ;
+}
+
+/*!
+ Set the cylinder parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
+ expressed in the world frame.
+
+ \param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
+
+ \code
+ vpCylinder cylinder;
+ double A, B, C, X0, Y0, Z0, R;
+ ...
+ vpColVector oP(7);
+ oP[0] = A;
+ ...
+ oP[3] = X0;
+ ...
+ oP[6] = R;
+ cylinder.setWorldCoordinates(oP);
+ \endcode
+*/
+void
+vpCylinder::setWorldCoordinates(const vpColVector& o_P)
+{
+ this->oP = o_P ;
+}
+
+/*!
+ Set the cylinder parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
+ expressed in the world frame.
+
+ \param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
+
+*/
+void
+vpCylinder::setWorldCoordinates(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R)
+{
+ oP[0] = A ;
+ oP[1] = B ;
+ oP[2] = C ;
+ oP[3] = X0 ;
+ oP[4] = Y0 ;
+ oP[5] = Z0 ;
+ oP[6] = R ;
+}
+
+
+/*!
+ Default constructor.
+*/
+vpCylinder::vpCylinder()
+{
+ init() ;
+}
+
+/*!
+ Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
+ expressed in the world frame.
+
+ \param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
+
+ \code
+ vpCylinder cylinder;
+ double A, B, C, X0, Y0, Z0, R;
+ ...
+ vpColVector oP(7);
+ oP[0] = A;
+ ...
+ oP[3] = X0;
+ ...
+ oP[6] = R;
+ vpCylinder cylinder(oP);
+ \endcode
+ \sa setWorldCoordinates(const vpColVector&)
+*/
+vpCylinder::vpCylinder(const vpColVector& o_P)
+{
+ init() ;
+ setWorldCoordinates(o_P) ;
+}
+
+/*!
+ Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
+ expressed in the world frame.
+
+ \param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
+
+ \sa setWorldCoordinates(const double,const double,const double,const double,const double,const double,const double)
+*/
+vpCylinder::vpCylinder(const double A, const double B,
+ const double C,
+ const double X0, const double Y0,
+ const double Z0,
+ const double R)
+{
+ init() ;
+ setWorldCoordinates(A, B, C,
+ X0, Y0, Z0,
+ R) ;
+}
+
+/*!
+ Default constructor.
+ */
+vpCylinder::~vpCylinder()
+{
+}
+
+
+/*!
+ Perspective projection of the cylinder.
+
+ From the parameters of the cylinder in the camera frame, compute the perspective
+ projection of the cylinder in the image plane.
+
+ \code
+ vpCylinder cylinder;
+ vpColVector oP(7);
+ // Initialize oP[] with A,B,C,X0,X0,Z0,R parameters
+ cylinder.setWorldCoordinates(oP); // Set the cylinder world frame parameters
+
+ vpHomogeneousMatrix cMo; // Camera to world frame transformation
+ cylinder.changeFrame(cMo); // Update internal cP parameters
+
+ cylinder.projection(); // Compute the perspective projection
+ \endcode
+
+ \sa projection(const vpColVector &, vpColVector &)
+ */
+void
+vpCylinder::projection()
+{
+ projection(cP,p) ;
+}
+
+
+/*!
+ Perspective projection of the cylinder.
+
+ From the parameters of the cylinder in the camera frame \f$c{\bf P}\f$, compute the perspective
+ projection of the cylinder in the image plane.
+
+ \param cP_ [in] : Cylinder parameters in the camera frame.
+ \param p_ [out] : Parameters of the cylinder in the image plane obtained by perspective projection.
+
+ \exception vpException::fatalError : The camera is inside the cylinder.
+
+ \code
+ vpCylinder cylinder;
+ vpColVector oP(7);
+ // Initialize oP[] with A,B,C,X0,X0,Z0,R parameters
+ cylinder.setWorldCoordinates(oP); // Set the cylinder world frame parameters
+
+ vpHomogeneousMatrix cMo; // Camera to world frame transformation
+ vpColVector cP(7); // Parameters of the cylinder in the camera frame
+ cylinder.changeFrame(cMo, cP); // Update cP parameters
+
+ vpColVector p(4); // Parameters of the cylinder in the image plane
+ cylinder.projection(cP, p); // Compute the perspective projection and update p
+ \endcode
+
+ \sa projection()
+ */
+void
+vpCylinder::projection(const vpColVector &cP_, vpColVector &p_)
+{
+ //calcul de la scene 2-D
+
+ double co, si, e, x0, y0, z0;
+ double A,B,C, X0, Y0, Z0, R ;
+ double s, a, b, c, zero;
+
+ A = cP_[0] ;
+ B = cP_[1] ;
+ C = cP_[2] ;
+ X0 = cP_[3] ;
+ Y0 = cP_[4] ;
+ Z0 = cP_[5] ;
+ R= cP_[6] ;
+ zero = A*X0 + B*Y0 + C*Z0; // should be zero for a good reprensetation of the cylinder
+
+ s = X0*X0 + Y0*Y0 + Z0*Z0 - R*R - zero*zero;
+ if (s < 0)
+ {
+ printf("The camera is inside the cylinder with s=%f !\n", s);
+ throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
+ }
+ s = 1.0/sqrt(s);
+ a = X0 - A*zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
+ b = Y0 - B*zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
+ c = Z0 - C*zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
+ x0 = C*Y0 - B*Z0;
+ y0 = A*Z0 - C*X0;
+ z0 = B*X0 - A*Y0;
+
+ // rho1 / theta1
+ co = R*a*s-x0;
+ si = R*b*s-y0;
+ e = sqrt(co*co + si*si);
+ p_[0] = -(R*c*s-z0)/e ; // rho1
+ p_[1] = atan2(si,co) ; // theta 1
+
+ // while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
+ // while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
+
+ // rho2 / theta2
+ co = R*a*s+x0;
+ si = R*b*s+y0;
+ e = sqrt(co*co + si*si);
+ p_[2] = -( R*c*s+z0 )/e ; //rho2
+ p_[3] = atan2( si,co ) ; //theta2
+
+
+ // while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
+ // while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
+
+ // std::cout << p.t() << std::endl ;
+}
+
+/*!
+ From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
+ the cylinder internal parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+
+ \param cMo : Camera to world frame transformation.
+
+ \sa changeFrame(const vpHomogeneousMatrix &, vpColVector &)
+ */
+void
+vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo,cP) ;
+}
+
+/*!
+ From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
+ the cylinder parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+
+ \param cMo : Camera to world frame transformation.
+ \param cP_ [out] : Parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+
+ \sa changeFrame(const vpHomogeneousMatrix &)
+*/
+void
+vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+{
+ double X1, Y1, Z1;
+ double X2, Y2, Z2;
+ double s, a, b, c;
+
+ double oA,oB,oC, oX0, oY0, oZ0 ;
+ oA = oP[0] ;
+ oB = oP[1] ;
+ oC = oP[2] ;
+ oX0 = oP[3] ;
+ oY0 = oP[4] ;
+ oZ0 = oP[5] ;
+
+ X1 = cMo[0][0]*oA + cMo[0][1]*oB + cMo[0][2]*oC ;
+ Y1 = cMo[1][0]*oA + cMo[1][1]*oB + cMo[1][2]*oC ;
+ Z1 = cMo[2][0]*oA + cMo[2][1]*oB + cMo[2][2]*oC ;
+ s = sqrt ( X1*X1 + Y1*Y1 + Z1*Z1 );
+ a = X1 / s;
+ b = Y1 / s;
+ c = Z1 / s;
+
+ // set axis coordinates in camera frame
+ cP_[0] = a ;
+ cP_[1] = b ;
+ cP_[2] = c ;
+
+ X2 = cMo[0][3] + cMo[0][0]*oX0 + cMo[0][1]*oY0 + cMo[0][2]*oZ0;
+ Y2 = cMo[1][3] + cMo[1][0]*oX0 + cMo[1][1]*oY0 + cMo[1][2]*oZ0;
+ Z2 = cMo[2][3] + cMo[2][0]*oX0 + cMo[2][1]*oY0 + cMo[2][2]*oZ0;
+
+ // adding the constraint X0 is the nearest point to the origin (A^T . X0 = 0)
+ // using the projection operator (I - AA^T) orthogonal to A
+ cP_[3] = (1-a*a)*X2 - a*b*Y2 - a*c*Z2;
+ cP_[4] = -a*b*X2 + (1-b*b)*Y2 - b*c*Z2;
+ cP_[5] = -a*c*X2 - b*c*Y2 + (1-c*c)*Z2;
+
+ /* old version for the same onstraint
+ if ( fabs(a) > 0.25 )
+ {
+ double xx, yy, zz, xx1, yy1;
+
+ xx1 = a*Y2 - b*X2;
+ yy1 = a*Z2 - c*X2;
+ xx = -( b*xx1 + c*yy1);
+ yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
+ zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
+
+ // set point coordinates in camera frame
+ _cP[3] = xx ;
+ _cP[4] = yy ;
+ _cP[5] = zz ;
+ }
+ else if ( fabs(b) >0.25 )
+ {
+ double xx, yy, zz, xx1, yy1;
+
+ xx1 = a*Y2 - b*X2;
+ yy1 = c*Y2 - b*Z2;
+ xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
+ yy = a*xx1 + c*yy1;
+ zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
+
+
+ // set point coordinates in camera frame
+ _cP[3] = xx ;
+ _cP[4] = yy ;
+ _cP[5] = zz ;
+ }
+ else
+ {
+ double xx, yy, zz, xx1, yy1;
+
+ xx1 = a*Z2 - c*X2;
+ yy1 = b*Z2 - c*Y2;
+ xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
+ yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
+ zz = a*xx1 + b*yy1;
+
+ // set point coordinates in camera frame
+ _cP[3] = xx ;
+ _cP[4] = yy ;
+ _cP[5] = zz ;
+ }
+ */
+ //radius
+ cP_[6] = oP[6] ;
+
+}
+
+//! for memory issue (used by the vpServo class only)
+vpCylinder *vpCylinder::duplicate() const
+{
+ vpCylinder *feature = new vpCylinder(*this) ;
+ return feature ;
+}
+
+/*!
+ Display the projection of the cylinder in the image as two lines.
+*/
+void
+vpCylinder::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+
+ vpColVector _cP(7), _p(4) ;
+ changeFrame(cMo,_cP) ;
+ projection(_cP,_p) ;
+ vpFeatureDisplay::displayCylinder(_p[0],_p[1], _p[2], _p[3],
+ cam, I, color, thickness) ;
+
+}
+
+/*!
+ Display the projection of the cylinder in the image as two lines.
+*/
+void
+vpCylinder::display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3],
+ cam, I, color, thickness) ;
+}
diff --git a/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp
new file mode 100644
index 0000000..027bbcb
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp
@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Forward projection.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpForwardProjection.cpp
+ \brief class that defines what is a point
+*/
+
+
+
+
+
+/*!
+ Print to stdout the feature parameters in:
+ - the object frame
+ - the camera frame
+ - the image plane.
+*/
+void
+vpForwardProjection::print() const
+{
+ std::cout << "oP : " << oP.t() ;
+ std::cout << "cP : " << cP.t() ;
+ std::cout << "p : " << p.t() ;
+}
+
+/*!
+
+ Compute the feature parameters in the image plane (vpTracker::p)
+ from the parameters in the camera frame (vpTracker::cP).
+
+ \warning Be careful to update vpTracker::p and vpTracker::cP public
+ attributes before the call of this method.
+*/
+void
+vpForwardProjection::project()
+{
+ projection(cP, p) ;
+}
+
+/*!
+
+ Compute the feature parameters in the camera frame (vpTracker::cP)
+ and than compute the projection of these parameters in the image
+ plane (vpTracker::p).
+
+ \warning The feature parameters in the object frame
+ (vpForwardProjection:oP) need to be set prior the use of this
+ method. To initialize these parameters see setWorldCoordinates().
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+*/
+void
+vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
+{
+ try{
+ changeFrame(cMo) ;
+ projection() ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Track the feature parameters in the camera frame (vpTracker::cP)
+ and than compute the projection of these parameters in the image
+ plane (vpTracker::p).
+
+ This method is similar to project(const vpHomogeneousMatrix &).
+
+ \warning The feature parameters in the object frame
+ (vpForwardProjection:oP) need to be set prior the use of this
+ method. To initialize these parameters see setWorldCoordinates().
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+*/
+void
+vpForwardProjection::track(const vpHomogeneousMatrix &cMo)
+{
+ try{
+ project(cMo) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/tracking/forward-projection/vpLine.cpp b/modules/core/src/tracking/forward-projection/vpLine.cpp
new file mode 100644
index 0000000..8e25142
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpLine.cpp
@@ -0,0 +1,568 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Line feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpLine.h>
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+/*!
+ \file vpLine.cpp
+ \brief class that defines what is a line
+*/
+
+
+
+/*!
+
+ Initialize the memory space requested for the 2D line parameters (\e
+ p) in the image plane and for 3D line parameters (\e oP and \e cP)
+ respectively in the object frame and the camera frame.
+*/
+void
+vpLine::init()
+{
+ oP.resize(8) ;
+ cP.resize(8) ;
+ p.resize(2) ;
+}
+
+/*!
+ Default constructor.
+*/
+vpLine::vpLine()
+{
+ init() ;
+}
+
+
+
+/*!
+
+ Sets the parameters \e oP which define the line in the object
+ frame. As said in the class description, the line is defined as the
+ intersection of two planes. The different parameters here define the
+ equations of the two planes in the object frame. They are used to set
+ the vpForwardProjection::oP public attribute.
+
+ \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
+ \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
+ Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
+
+ \param A1, B1, C1, D1 : The parameters used to define the first plane.
+ \param A2, B2, C2, D2 : The parameters used to define the second plane.
+
+*/
+void
+vpLine::setWorldCoordinates(const double &A1, const double &B1,
+ const double &C1, const double &D1,
+ const double &A2, const double &B2,
+ const double &C2, const double &D2)
+{
+ oP[0] = A1 ;
+ oP[1] = B1 ;
+ oP[2] = C1 ;
+ oP[3] = D1 ;
+
+ oP[4] = A2 ;
+ oP[5] = B2 ;
+ oP[6] = C2 ;
+ oP[7] = D2 ;
+}
+
+
+/*!
+
+ Sets the parameters \e oP which define the line in the object frame. As
+ said in the class description, the line is defined as the
+ intersection of two planes. Eight parameters are required to define
+ the equations of the two planes in the object frame. They are used to set
+ the vpForwardProjection::oP public attribute.
+
+ \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
+ \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
+ Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
+
+ \param oP_ : The column vector which contains the eight parameters
+ needed to define the equations of the two planes in the object
+ frame. \f[ oP = \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \\ A2 \\
+ B2 \\ C2 \\ D2 \end{array}\right] \f]
+
+*/
+void
+vpLine::setWorldCoordinates(const vpColVector &oP_)
+{
+ if (oP_.getRows() != 8)
+ throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
+
+ this->oP = oP_ ;
+}
+
+
+/*!
+
+ Sets the parameters \e oP which define the line in the object frame. As
+ said in the class description, the line is defined as the
+ intersection of two planes. Eight parameters are required to define
+ the equations of the two planes in the object frame. They are used to set
+ the vpForwardProjection::oP public attribute.
+
+ \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
+ \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
+ Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
+
+ \param oP1 : The column vector which contains the four parameters
+ needed to define the equations of the first plane. \f[ oP1 =
+ \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \end{array}\right] \f]
+
+ \param oP2 : The column vector which contains the four parameters
+ needed to define the equations of the second plane. \f[ oP2 =
+ \left[\begin{array}{c} A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
+
+*/
+void
+vpLine::setWorldCoordinates(const vpColVector &oP1,
+ const vpColVector &oP2)
+{
+ if (oP1.getRows() != 4)
+ throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
+
+ if (oP2.getRows() != 4)
+ throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
+
+ for (unsigned int i=0 ; i < 4 ; i++)
+ {
+ oP[i] = oP1[i] ;
+ oP[i+4] = oP2[i] ;
+ }
+
+}
+
+
+/*!
+
+ Computes the 2D parameters \e p of the line in the image plane thanks to
+ the 3D line parameters \e cP in the camera frame located in the
+ vpTracker::cP public attribute. The parameters \f$p=(\rho,
+ \theta)\f$ are updated in the vpTracker::p public attribute.
+
+ \warning To compute these parameters \e p, the method exploit the feature
+ parameters \e cP in the camera frame. Thus, vpTracker::cP need to be
+ updated before the call of this method. For that, a call to
+ changeFrame(const vpHomogeneousMatrix &) is requested.
+
+ The code below shows how to use this method.
+ \code
+ //Create the line
+ vpLine line;
+
+ //Set the coordinates of the line in the object frame in meter.
+ line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
+ //Here the line is define by the intersection between the plane X = 0.5m and Z = 0.5m
+
+ //Create the homogeneous matrix
+ vpHomogeneousMatrix cMo;
+ //Computes or set here the homogeneous matrix
+
+ //Computes the equations of the two planes in the camera frame
+ line.changeFrame(cMo);
+
+ //Computes the line features in the camera frame( rho and theta)
+ line.projection();
+ \endcode
+*/
+void
+vpLine::projection()
+{
+ projection(cP,p) ;
+}
+
+
+/*!
+
+ Computes the 2D parameters \e p of the line in the image plane thanks
+ to the 3D line features \e cP expressed in the camera frame. The
+ image plane parameters \f$p=(\rho , \theta)\f$ are updated in
+ output.
+
+ \param cP_ : The vector containing the line features relative to the
+ camera frame. \f[ cP = \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1
+ \\ A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
+
+ \param p_ : The vector which contains the 2D line features expressed
+ in the image plane. \f[ p = \left[\begin{array}{c} \rho \\ \theta
+ \end{array}\right] \f]
+
+ \exception vpException::fatalError : Degenerate case, the image of the straight line is a point.
+*/
+void
+vpLine::projection(const vpColVector &cP_, vpColVector &p_)
+{
+ //projection
+
+ if (cP.getRows() != 8)
+ throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
+
+ double A1, A2, B1, B2, C1, C2, D1, D2;
+
+ A1=cP_[0] ;
+ B1=cP_[1] ;
+ C1=cP_[2] ;
+ D1=cP_[3] ;
+
+ A2=cP_[4] ;
+ B2=cP_[5] ;
+ C2=cP_[6] ;
+ D2=cP_[7] ;
+
+ double a, b, c, s;
+ a = A2*D1 - A1*D2;
+ b = B2*D1 - B1*D2;
+ c = C2*D1 - C1*D2;
+ s = a*a+b*b;
+ if (s <= 1e-8) // seuil pas terrible
+ {
+ printf("Degenerate case: the image of the straight line is a point!\n");
+ throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
+ }
+ s = 1.0/sqrt(s);
+
+ double rho = -c*s ;
+ double theta = atan2( b, a);
+
+ if (p.getRows() != 2)
+ p.resize(2);
+
+ p_[0] = rho ;
+ p_[1] = theta ;
+}
+
+
+/*!
+
+ Computes the line parameters \e cP in the camera frame thanks to the
+ line parameters \e oP given in the object frame and the homogeneous matrix
+ relative to the pose \e cMo between the object frame and the camera
+ frame. Thus the computation gives the equations of the two planes
+ needed to define the line in the camera frame thanks to the
+ equations of the same two planes in the object frame.
+
+ In input of this method, the line parameters \e oP in the object
+ frame are those from the vpForwardProjection::oP public attribute.
+
+ As a result of this method, line parameters \e cP in the camera
+ frame are updated in the vpTracker::cP public attribute.
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+ The code below shows how to use this method.
+ \code
+ //Create the line
+ vpLine line;
+
+ //Set the coordinates of the line in the object frame in meter.
+ line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
+ //The line is define by the intersection between the plane X = 0.5m and Z = 0.5m
+
+ //Create the homogeneous matrix
+ vpHomogeneousMatrix cMo;
+ //Computes or set here the homogeneous matrix
+
+ //Computes the equations of the two planes in the camera frame
+ line.changeFrame(cMo);
+ \endcode
+*/
+void
+vpLine::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo,cP) ;
+}
+
+
+/*!
+
+ Computes the line parameters \e cP in the camera frame thanks to the
+ line parameters \e oP given in the object frame and the homogeneous
+ matrix relative to the pose between the camera frame and the
+ object frame. Thus the computation gives the equations of the two
+ planes needed to define the line in the desired frame thanks to the
+ equations of the same two planes in the object frame.
+
+ In input of this method, the line parameters \e oP in the object
+ frame are those from the vpForwardProjection::oP public attribute.
+
+ \param cMo : The homogeneous matrix relative to the pose
+ between the desired frame and the object frame.
+
+ \param cP_ : The vector which will contain the parameters of the two
+ planes in the camera frame. \f[ cP =
+ \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \\ A2 \\ B2 \\ C2 \\ D2
+ \end{array}\right] \f]
+
+ The code below shows how to use this method.
+ \code
+ //Create the line
+ vpLine line;
+
+ //Set the coordinates of the line in the object frame in meter.
+ line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
+ //The line is define by the intersection between the plane X = 0.5m and Z = 0.5m
+
+ //Create the homogeneous matrix
+ vpHomogeneousMatrix cMo;
+ //Computes or set here the homogeneous matrix
+
+ //Creates the vector which will contain the line features
+ vpColVector cP(8);
+
+ //Computes the equations of the two planes in the camera frame
+ line.changeFrame(cMo, cP);
+ \endcode
+*/
+
+void
+vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+{
+
+ double a1, a2, b1, b2, c1, c2, d1, d2;
+ double A1, A2, B1, B2, C1, C2, D1, D2;
+
+ // in case of verification
+ // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
+
+ if (cP.getRows() != 8)
+ cP.resize(8);
+
+ a1=oP[0] ;
+ b1=oP[1] ;
+ c1=oP[2] ;
+ d1=oP[3] ;
+
+ a2=oP[4] ;
+ b2=oP[5] ;
+ c2=oP[6] ;
+ d2=oP[7] ;
+
+ A1 = cMo[0][0]*a1 + cMo[0][1]*b1 + cMo[0][2]*c1;
+ B1 = cMo[1][0]*a1 + cMo[1][1]*b1 + cMo[1][2]*c1;
+ C1 = cMo[2][0]*a1 + cMo[2][1]*b1 + cMo[2][2]*c1;
+ D1 = d1 - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
+
+ A2 = cMo[0][0]*a2 + cMo[0][1]*b2 + cMo[0][2]*c2;
+ B2 = cMo[1][0]*a2 + cMo[1][1]*b2 + cMo[1][2]*c2;
+ C2 = cMo[2][0]*a2 + cMo[2][1]*b2 + cMo[2][2]*c2;
+ D2 = d2 - (cMo[0][3]*A2 + cMo[1][3]*B2 + cMo[2][3]*C2);
+
+ // in case of verification
+ // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
+ // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2;
+
+ // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
+ // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
+
+ // Adding constraints on the straight line to have a unique representation
+
+ // direction of the straight line = N1 x N2
+ a2 = B1*C2 - C1*B2;
+ b2 = C1*A2 - A1*C2;
+ c2 = A1*B2 - B1*A2;
+
+ // Constraint D1 = 0 (the origin belongs to P1)
+ a1 = A2*D1 - A1*D2;
+ b1 = B2*D1 - B1*D2;
+ c1 = C2*D1 - C1*D2;
+
+ if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
+ {
+ A2 = A1;
+ B2 = B1;
+ C2 = C1;
+ D2 = D1;
+ }
+
+ // Constraint A1^2 + B1^2 + C1^2 = 1
+ d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
+ cP_[0] = A1 = a1*d1 ;
+ cP_[1] = B1 = b1*d1 ;
+ cP_[2] = C1 = c1*d1 ;
+ cP_[3] = D1 = 0 ;
+
+ // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
+ // N2_new = (N1 x N2) x N1_new
+ a1 = b2*C1 - c2*B1;
+ b1 = c2*A1 - a2*C1;
+ c1 = a2*B1 - b2*A1;
+
+ // Constraint A2^2 + B2^2 + C2^2 = 1
+ d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
+ a1 *= d1 ;
+ b1 *= d1 ;
+ c1 *= d1 ;
+
+ // D2_new = D2 / (N2^T . N2_new)
+ D2 /= (A2*a1 + B2*b1 + C2*c1);
+ A2 = a1;
+ B2 = b1;
+ C2 = c1;
+
+ // Constraint D2 < 0
+ if (D2 > 0)
+ {
+ A2 = -A2;
+ B2 = -B2;
+ C2 = -C2;
+ D2 = -D2;
+ }
+ // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
+ // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
+
+ cP_[4] = A2;
+ cP_[5] = B2;
+ cP_[6] = C2;
+ cP_[7] = D2;
+
+ // in case of verification
+ /*
+ x = -A2*D2;
+ y = -B2*D2;
+ z = -C2*D2;
+ d1 = ap1*x+bp1*y+cp1*z+dp1;
+ d2 = ap2*x+bp2*y+cp2*z+dp2;
+ if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
+ {
+ printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2);
+ exit(-1);
+ }
+ d1 = A1*x+B1*y+C1*z+D1;
+ d2 = A2*x+B2*y+C2*z+D2;
+ if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
+ {
+ printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
+ exit(-1);
+ }
+ */
+
+}
+
+
+
+/*!
+
+ Displays the line in the image \e I thanks to the 2D parameters of
+ the line \e p in the image plane (vpTracker::p) and the camera
+ parameters which enable to convert the parameters from meter to
+ pixel.
+
+ \param I : The image where the line must be displayed.
+
+ \param cam : The camera parameters to enable the conversion from
+ meter to pixel.
+
+ \param color : The desired color to display the line in the image.
+
+ \param thickness : Thickness of the feature representation.
+*/
+void vpLine::display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness) ;
+}
+
+
+/*!
+
+ Displays the line in the image \e I thanks to the parameters in the
+ object frame (vpForwardProjection::oP), the homogeneous matrix
+ relative to the pose between the camera frame and the object frame
+ and the camera parameters which enable to convert the features from
+ meter to pixel.
+
+ \param I : The image where the line must be displayed in overlay.
+
+ \param cMo : The homogeneous matrix corresponding to the pose
+ between the camera frame and the object frame.
+
+ \param cam : The camera parameters to enable the conversion from
+ meter to pixel.
+
+ \param color : The desired color to display the line in the image.
+
+ \param thickness : Thickness of the feature representation.
+*/
+// non destructive wrt. cP and p
+void vpLine::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpColVector _cP, _p ;
+ changeFrame(cMo,_cP) ;
+ projection(_cP,_p) ;
+ vpFeatureDisplay::displayLine(_p[0],_p[1],
+ cam, I, color, thickness) ;
+
+}
+
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpForwardProjection *fp;
+ vpLine line;
+ fp = line.duplicate(); // fp is now a vpLine
+ \endcode
+
+*/
+vpLine *vpLine::duplicate() const
+{
+ vpLine *feature = new vpLine(*this) ;
+ return feature ;
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/src/tracking/forward-projection/vpPoint.cpp b/modules/core/src/tracking/forward-projection/vpPoint.cpp
new file mode 100644
index 0000000..7f4cda3
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpPoint.cpp
@@ -0,0 +1,500 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Point feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpFeatureDisplay.h>
+
+/*!
+ \file vpPoint.cpp
+ \brief class that defines what is a point
+*/
+
+
+
+void
+vpPoint::init()
+{
+ p.resize(3) ; p = 0 ; p[2] = 1 ;
+ oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
+ cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
+
+ //default value Z (1 meters)
+ set_Z(1) ;
+}
+
+vpPoint::vpPoint()
+{
+ init();
+}
+
+/*!
+ Construction from 3D coordinates in the object frame.
+ \param oX, oY, oZ: Coordinates of a 3D point in the object frame.
+*/
+vpPoint::vpPoint(double oX, double oY, double oZ)
+{
+ init();
+ setWorldCoordinates(oX, oY, oZ);
+}
+
+/*!
+ Construction from 3D point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
+ of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
+ where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
+ where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+*/
+vpPoint::vpPoint(const vpColVector &P)
+{
+ init();
+ setWorldCoordinates(P);
+}
+
+/*!
+ Construction from 3D point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
+ of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
+ where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
+ where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+*/
+vpPoint::vpPoint(const std::vector<double> &P)
+{
+ init();
+ setWorldCoordinates(P);
+}
+
+/*!
+ Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param oX, oY, oZ: Coordinates of a 3D point in the object frame.
+*/
+void
+vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
+{
+ oP[0] = oX;
+ oP[1] = oY;
+ oP[2] = oZ;
+ oP[3] = 1;
+}
+
+/*!
+ Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
+ of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
+ where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
+ where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+*/
+void
+vpPoint::setWorldCoordinates(const vpColVector &P)
+{
+ if (P.size() == 3) {
+ oP[0] = P[0];
+ oP[1] = P[1];
+ oP[2] = P[2];
+ oP[3] = 1.;
+ }
+ else if (P.size() == 4) {
+ oP[0] = P[0];
+ oP[1] = P[1];
+ oP[2] = P[2];
+ oP[3] = P[3];
+ oP /= oP[3] ;
+ }
+ else {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize vpPoint from vector with size %d", P.size()));
+ }
+}
+
+/*!
+ Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
+ of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
+ where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
+ where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+*/
+void
+vpPoint::setWorldCoordinates(const std::vector<double> &P)
+{
+ if (P.size() == 3) {
+ oP[0] = P[0];
+ oP[1] = P[1];
+ oP[2] = P[2];
+ oP[3] = 1.;
+ }
+ else if (P.size() == 4) {
+ oP[0] = P[0];
+ oP[1] = P[1];
+ oP[2] = P[2];
+ oP[3] = P[3];
+ oP /= oP[3] ;
+ }
+ else {
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize vpPoint from vector with size %d", P.size()));
+ }
+}
+
+//! Get the point world coordinates. We mean here the coordinates of the point in the object frame.
+void
+vpPoint::getWorldCoordinates(double& oX, double& oY, double& oZ)
+{
+ oX = oP[0];
+ oY = oP[1];
+ oZ = oP[2];
+}
+
+
+/*!
+ Get the point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ */
+void
+vpPoint::getWorldCoordinates(vpColVector &P)
+{
+ P = oP;
+}
+/*!
+ Get the point world coordinates. We mean here the coordinates of the point in the object frame.
+ \param P: Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ */
+void
+vpPoint::getWorldCoordinates(std::vector<double> &P)
+{
+ P.resize(oP.size());
+ for(unsigned int i = 0; i < oP.size(); i++)
+ P[i] = oP[i];
+}
+
+/*!
+ Return the point world coordinates. We mean here the coordinates of the point in the object frame.
+ \return Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ */
+vpColVector
+vpPoint::getWorldCoordinates(void)
+{
+ return this->oP;
+}
+
+/*!
+ Compute the perspective projection of a point _cP.
+
+ \param _cP : Three dimension vector that corresponds to the coordinates of the point in the camera frame.
+ \param _p : Coordinates of the point in the image plane obtained by perspective projection.
+*/
+void
+vpPoint::projection(const vpColVector &_cP, vpColVector &_p)
+{
+ _p.resize(3) ;
+
+ _p[0] = _cP[0]/_cP[2] ;
+ _p[1] = _cP[1]/_cP[2] ;
+ _p[2] = 1 ;
+}
+
+/*!
+ From the 3D coordinates of the point in the object frame set using for example
+ setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
+ of the point in the camera frame.
+
+ \param cMo : Transformation from camera to object frame.
+ \param _cP : 3D coordinates of the point in the camera frame.
+*/
+void
+vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
+{
+
+ _cP.resize(4) ;
+
+ _cP[0] = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
+ _cP[1] = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
+ _cP[2] = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
+ _cP[3] = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
+
+ double d = 1/_cP[3] ;
+ _cP[0] *=d ;
+ _cP[1] *=d ;
+ _cP[2] *=d ;
+ _cP[3] *=d ; ;
+}
+
+/*!
+ From the 3D coordinates of the point in the object frame set using for example
+ setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
+ of the point in the camera frame.
+
+ \param cMo : Transformation from camera to object frame.
+
+*/
+void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo) {
+ double X = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
+ double Y = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
+ double Z = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
+ double W = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
+
+ double d = 1/W ;
+ cP[0] =X*d ;
+ cP[1] =Y*d ;
+ cP[2] =Z*d ;
+ cP[3] =1 ;
+}
+
+#if 0
+/*!
+ From the coordinates of the point in camera frame b and the transformation between
+ camera frame a and camera frame b computes the coordinates of the point in camera frame a.
+
+ \param aMb : 3D transformation between camera frame a and b.
+ \param bP : 3D coordinates of the point in camera frame bP.
+
+ \return A point with 3D coordinates in the camera frame a. The coordinates in the world or object
+ frame are set to the same coordinates than the one in the camera frame.
+*/
+const vpPoint
+operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
+{
+ vpPoint aP ;
+
+ vpColVector v(4),v1(4) ;
+
+ v[0] = bP.get_X() ;
+ v[1] = bP.get_Y() ;
+ v[2] = bP.get_Z() ;
+ v[3] = bP.get_W() ;
+
+ v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
+ v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
+ v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
+ v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
+
+ v1 /= v1[3] ;
+
+ // v1 = M*v ;
+ aP.set_X(v1[0]) ;
+ aP.set_Y(v1[1]) ;
+ aP.set_Z(v1[2]) ;
+ aP.set_W(v1[3]) ;
+
+ aP.set_oX(v1[0]) ;
+ aP.set_oY(v1[1]) ;
+ aP.set_oZ(v1[2]) ;
+ aP.set_oW(v1[3]) ;
+
+ return aP ;
+}
+
+/*!
+ From the coordinates of the point in image plane b and the homography between image
+ a and b computes the coordinates of the point in image plane a.
+
+ \param aHb : Homography between image a and b.
+ \param bP : 2D coordinates of the point in the image plane b.
+
+ \return A point with 2D coordinates in the image plane a.
+*/
+const vpPoint
+operator*(const vpHomography &aHb, const vpPoint& bP)
+{
+ vpPoint aP ;
+ vpColVector v(3),v1(3) ;
+
+ v[0] = bP.get_x() ;
+ v[1] = bP.get_y() ;
+ v[2] = bP.get_w() ;
+
+ v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
+ v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
+ v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
+
+ // v1 = M*v ;
+ aP.set_x(v1[0]) ;
+ aP.set_y(v1[1]) ;
+ aP.set_w(v1[2]) ;
+
+ return aP ;
+}
+#endif
+//! For memory issue (used by the vpServo class only).
+vpPoint *vpPoint::duplicate() const
+{
+ vpPoint *feature = new vpPoint(*this) ;
+ return feature ;
+}
+
+/*!
+ Display the point in the image.
+*/
+void
+vpPoint::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+
+ vpColVector _cP, _p ;
+ changeFrame(cMo,_cP) ;
+
+ if(_cP[2] < 0) // no display if point is behind the camera
+ return;
+
+ vpPoint::projection(_cP,_p) ;
+ vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
+}
+
+/*!
+ Display the point in the image.
+*/
+void
+vpPoint::display(const vpImage<vpRGBa> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpColVector _cP, _p ;
+ changeFrame(cMo,_cP) ;
+
+ if(_cP[2] < 0) // no display if point is behind the camera
+ return;
+
+ vpPoint::projection(_cP,_p) ;
+ vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
+}
+
+VISP_EXPORT std::ostream& operator<<(std::ostream& os, const vpPoint& /* vpp */)
+{
+ return( os<<"vpPoint" );
+}
+
+vpPoint&
+vpPoint::operator=(const vpPoint& vpp )
+{
+ p = vpp.p;
+ cP = vpp.cP;
+ oP = vpp.oP;
+ cPAvailable = vpp.cPAvailable;
+
+ return *this;
+}
+
+/*!
+ Display the point in the image.
+*/
+void
+vpPoint::display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness) ;
+}
+
+// Get coordinates
+//! Get the point X coordinate in the camera frame.
+double vpPoint::get_X() const { return cP[0] ; }
+//! Get the point Y coordinate in the camera frame.
+double vpPoint::get_Y() const { return cP[1] ; }
+//! Get the point Z coordinate in the camera frame.
+double vpPoint::get_Z() const { return cP[2] ; }
+//! Get the point W coordinate in the camera frame.
+double vpPoint::get_W() const { return cP[3] ; }
+
+//! Get the point X coordinate in the object frame.
+double vpPoint::get_oX() const { return oP[0] ; }
+//! Get the point Y coordinate in the object frame.
+double vpPoint::get_oY() const { return oP[1] ; }
+//! Get the point Z coordinate in the object frame.
+double vpPoint::get_oZ() const { return oP[2] ; }
+//! Get the point W coordinate in the object frame.
+double vpPoint::get_oW() const { return oP[3] ; }
+
+//! Get the point x coordinate in the image plane.
+double vpPoint::get_x() const { return p[0] ; }
+//! Get the point y coordinate in the image plane.
+double vpPoint::get_y() const { return p[1] ; }
+//! Get the point w coordinate in the image plane.
+double vpPoint::get_w() const { return p[2] ; }
+
+/*!
+ Perspective projection of the point.
+
+ Projection onto the //image plane of the point. Update the object
+ attribute p (2D //homogeneous coordinates) according to object
+ attribute cP (current //3D coordinates in the camera frame).
+
+*/
+void vpPoint::projection() {
+ double d = 1/cP[2] ;
+ p[0] = cP[0]*d ;
+ p[1] = cP[1]*d ;
+ p[2] = 1 ;
+}
+
+//! Set the point X coordinate in the camera frame.
+void vpPoint::set_X(const double X) { cP[0] = X ; }
+//! Set the point Y coordinate in the camera frame.
+void vpPoint::set_Y(const double Y) { cP[1] = Y ; }
+//! Set the point Z coordinate in the camera frame.
+void vpPoint::set_Z(const double Z) { cP[2] = Z ; }
+//! Set the point W coordinate in the camera frame.
+void vpPoint::set_W(const double W) { cP[3] = W ; }
+
+//! Set the point X coordinate in the object frame.
+void vpPoint::set_oX(const double oX) { oP[0] = oX ; }
+//! Set the point Y coordinate in the object frame.
+void vpPoint::set_oY(const double oY) { oP[1] = oY ; }
+//! Set the point Z coordinate in the object frame.
+void vpPoint::set_oZ(const double oZ) { oP[2] = oZ ; }
+//! Set the point W coordinate in the object frame.
+void vpPoint::set_oW(const double oW) { oP[3] = oW ; }
+
+//! Set the point x coordinate in the image plane.
+void vpPoint::set_x(const double x) { p[0] = x ; }
+//! Set the point y coordinate in the image plane.
+void vpPoint::set_y(const double y) { p[1] = y ; }
+//! Set the point w coordinate in the image plane.
+void vpPoint::set_w(const double w) { p[2] = w ; }
diff --git a/modules/core/src/tracking/forward-projection/vpSphere.cpp b/modules/core/src/tracking/forward-projection/vpSphere.cpp
new file mode 100644
index 0000000..81f2172
--- /dev/null
+++ b/modules/core/src/tracking/forward-projection/vpSphere.cpp
@@ -0,0 +1,230 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Sphere feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpSphere.h>
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+void
+vpSphere::init()
+{
+
+ oP.resize(4) ;
+ cP.resize(4) ;
+
+ p.resize(5) ;
+}
+
+void
+vpSphere::setWorldCoordinates(const vpColVector& oP_)
+{
+ this->oP = oP_ ;
+}
+
+void
+vpSphere::setWorldCoordinates(const double X0, const double Y0,
+ const double Z0, const double R)
+{
+ oP[0] = X0 ;
+ oP[1] = Y0 ;
+ oP[2] = Z0 ;
+ oP[3] = R ;
+}
+
+
+
+vpSphere::vpSphere()
+{
+ init() ;
+}
+
+
+vpSphere::vpSphere(const vpColVector& oP_)
+{
+ init() ;
+ setWorldCoordinates(oP_) ;
+}
+
+vpSphere::vpSphere(const double X0, const double Y0,
+ const double Z0, const double R)
+{
+ init() ;
+ setWorldCoordinates(X0, Y0, Z0, R) ;
+}
+
+vpSphere::~vpSphere()
+{
+}
+
+
+//! perspective projection of the sphere
+void
+vpSphere::projection()
+{
+ projection(cP,p) ;
+}
+
+//! Perspective projection of the circle.
+void
+vpSphere::projection(const vpColVector &cP_, vpColVector &p_)
+{
+ double x0, y0, z0; //variables intermediaires
+// double k0, k1, k2, k3, k4; //variables intermediaires
+ double E, A, B; //variables intermediaires
+
+ //calcul des parametres M20, M11, M02 de l'ellipse
+ double s, a, b, r, e; //variables intermediaires
+ r = cP_[3];
+
+ x0 = cP_[0] ;
+ y0 = cP_[1] ;
+ z0 = cP_[2] ;
+
+ s = r*r - y0*y0 -z0*z0;
+
+// k0 = (r*r - x0*x0 -z0*z0)/s;
+// k1 = (x0*y0)/s;
+// k2 = (x0*z0)/s;
+// k3 = (y0*z0)/s;
+// k4 = (r*r - x0*x0 -y0*y0)/s;
+
+ if ((s = z0*z0 - r*r) < 0.0)
+ {
+ vpERROR_TRACE("sphere derriere le plan image\n");
+ }
+
+ p_[0] = x0*z0/s ; //x
+ p_[1] = y0*z0/s ; //y
+
+ if (fabs(x0) > 1e-6)
+ {
+ // vpERROR_TRACE(" %f",r) ;
+ e = y0/x0;
+ b = r/sqrt(s);
+ if ((a = x0*x0 + y0*y0 + z0*z0 - r*r) < 0.0)
+ {
+ vpERROR_TRACE("sphere derriere le plan image\n");
+ }
+ a = r*sqrt(a)/s;
+ if (fabs(e) <= 1.0)
+ {
+ E = e;
+ A = a;
+ B = b;
+ }
+ else
+ {
+ E = -1.0/e;
+ A = b;
+ B = a;
+ }
+ }
+ else
+ {
+ // vpERROR_TRACE(" %f",r) ;
+ E = 0.0;
+ A = r/sqrt(s);
+ B = r*sqrt(y0*y0+z0*z0-r*r)/s;
+ }
+
+ p_[2] = ( A*A + B*B * E*E) / (1.0 + E*E); // mu20
+ p_[3] = ( A*A - B*B) * E / (1.0 + E*E); // mu11
+ p_[4] = ( B*B + A*A * E*E) / (1.0 + E*E); // mu02
+
+ // vpERROR_TRACE(" %f",r) ;
+
+ // std::cout << p.t() ;
+}
+//! perspective projection of the circle
+void
+vpSphere::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ changeFrame(cMo,cP) ;
+}
+
+//! Perspective projection of the circle.
+void
+vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+{
+ double x0, y0, z0; //variables intermediaires
+
+ x0 = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2] + cMo[0][3];
+ y0 = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2] + cMo[1][3];
+ z0 = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2] + cMo[2][3];
+
+ cP_[3] = oP[3];
+
+ cP_[0] = x0 ;
+ cP_[1] = y0 ;
+ cP_[2] = z0 ;
+}
+
+//! for memory issue (used by the vpServo class only)
+vpSphere *vpSphere::duplicate() const
+{
+ vpSphere *feature = new vpSphere(*this) ;
+ return feature ;
+}
+
+
+
+// non destructive wrt. cP and p
+void vpSphere::display(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpColVector _cP, _p ;
+ changeFrame(cMo,_cP) ;
+ projection(_cP,_p) ;
+ vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
+ cam, I, color, thickness) ;
+
+}
+
+
+
+void vpSphere::display(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const unsigned int thickness)
+{
+ vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
+ cam, I, color, thickness) ;
+}
diff --git a/modules/core/src/tracking/moments/vpMoment.cpp b/modules/core/src/tracking/moments/vpMoment.cpp
new file mode 100644
index 0000000..ee1c9a4
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMoment.cpp
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base for 2D moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMoment.cpp
+ \brief Base class for all 2D moments.
+*/
+
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMoment.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <cstring>
+/*!
+ Default constructor
+*/
+vpMoment::vpMoment(): object(NULL), moments(NULL), values() {}
+
+
+/*!
+ Links the moment to a database of moment primitives.
+ If the moment depends on other moments, these moments must be linked to the same database.
+ \attention Two moments of the same class cannot be stored in the same database
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCentered.h>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p;
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+
+ vpMomentObject obj(2);
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode.
+ obj.fromVector(vec_p); // Init the dense object with the polygon
+
+ vpMomentDatabase db;
+ vpMomentGravityCenter G; // declaration of gravity center
+ vpMomentCentered mc; // mc containts centered moments
+
+ G.linkTo(db); //add gravity center to database
+ mc.linkTo(db); //centered moments depend on gravity, add them to the
+ //database to grant access
+
+ G.update(obj); // specify the object for gravity center
+ mc.update(obj); // and for centered moments
+
+ G.compute(); // compute the moment
+ mc.compute(); //compute centered moments AFTER gravity center
+
+ return 0;
+}
+
+ \endcode
+
+ \param data_base : database of moment primitives.
+*/
+void vpMoment::linkTo(vpMomentDatabase& data_base){
+ if (strlen( name() ) >= 255) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the moment name"));
+ }
+
+ std::strcpy(_name,name());
+ this->moments=&data_base;
+
+ data_base.add(*this,_name);
+}
+
+
+/*!
+ Updates the moment with the current object. This does not compute any values.
+ \param moment_object : object descriptor of the current camera vision.
+*/
+void vpMoment::update(vpMomentObject& moment_object){
+ this->object=&moment_object;
+}
+
+/*!
+ Prints the moment contents to a stream
+ \param os : a std::stream.
+ \param m : a moment instance.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMoment& m){
+ for(std::vector<double>::const_iterator i = m.values.begin();i!=m.values.end();i++)
+ os << *i << ",";
+
+ return os;
+}
+
+/*!
+Prints values of all dependent moments required to calculate a specific vpMoment.
+Not made pure to maintain compatibility
+Recommended : Types inheriting from vpMoment should implement this function
+*/
+void vpMoment::printDependencies(std::ostream& os) const{
+ os << " WARNING : Falling back to base class version of printDependencies(). To prevent that, this has to be implemented in the derived classes!" << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentAlpha.cpp b/modules/core/src/tracking/moments/vpMomentAlpha.cpp
new file mode 100644
index 0000000..4be9276
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentAlpha.cpp
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Alpha moment descriptor for in-plane orientation.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentAlpha.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <cmath>
+
+/*!
+ Empty constructor. Initializes alpha moment as a reference alpha. A default-constructed alpha moment may be used as a reference for other alphas.
+ A reference alpha is class harbouring an alpha value computed for a \f$[-\pi/2..\pi/2]\f$ portion of the circle. Not setting a reference alpha will prevent you
+ from doing more than 180deg rotation with moments.
+ */
+vpMomentAlpha::vpMomentAlpha() : isRef(true), symmetric(false), ref(), alphaRef(0.) {
+ values.resize(1);
+}
+
+/*!
+ Common constructor. Initializes alpha moment as a non-reference alpha. A default-constructed alpha moment must be used as a reference for other this alpha.
+ A reference alpha is class harbouring an alpha value computed for a \f$[0..\pi]\f$ portion of the circle. Not having a reference alpha will prevent you
+ from doing more than 180deg rotation with moments.
+ \param ref_ : vector of 3rd order centered moments corresponding to the reference alpha in the following order: \f$\mu_{03},\mu_{12},\mu_{21},\mu_{30}\f$.
+ \param alpha_ref : value of the reference alpha.
+*/
+vpMomentAlpha::vpMomentAlpha(std::vector<double>& ref_, double alpha_ref)
+ : vpMoment(),isRef(false),symmetric(false),ref(ref_),alphaRef(alpha_ref)
+{
+ for (std::vector<double>::iterator it = ref_.begin(); it!=ref_.end(); it++)
+ if (*it<=1e-4)
+ symmetric = true;
+
+ values.resize(1);
+}
+
+/*!
+ Compute the value of the alpha-moment.
+ Depends on vpMomentCentered.
+ */
+void vpMomentAlpha::compute(){
+ //symmetric = symmetric | this->getObject().isSymmetric();
+ bool found_moment_centered;
+
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
+ found_moment_centered)));
+
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+
+ double alpha = 0.5 * atan2(2.0 * momentCentered.get(1, 1), (momentCentered.get(2, 0) - momentCentered.get(0, 2)));
+
+ unsigned int order = 4;
+ std::vector<double> rotMu(4);
+ std::vector<double> realMu(4);
+
+ if (isRef)
+ {
+ alphaRef = alpha;
+ }
+ else
+ {
+ if (!symmetric)
+ {
+ double r11 = cos(alpha - alphaRef);
+ double r12 = sin(alpha - alphaRef);
+ double r21 = -sin(alpha - alphaRef);
+ double r22 = cos(alpha - alphaRef);
+ unsigned int idx = 0;
+ for (register unsigned int c = 0; c < (order) * (order); c++)
+ {
+ unsigned int i = c % order;
+ unsigned int j = c / order;
+
+ if (i + j == 3)
+ {
+ double r11_k = 1.;
+ for (register unsigned int k = 0; k <= i; k++)
+ {
+ double r12_i_k = pow(r12, (int)(i - k));
+ double comb_i_k = static_cast<double> (vpMath::comb(i, k));
+ for (register unsigned int l = 0; l <= j; l++)
+ {
+ rotMu[idx] += static_cast<double> (comb_i_k * vpMath::comb(j, l) * r11_k * pow(r21, (int)l) * r12_i_k
+ * pow(r22, (int)(j - l)) * momentCentered.get(k + l, (unsigned int)(int)(i + j - k - l)));
+ }
+ r11_k *= r11;
+ }
+ realMu[idx] = momentCentered.get(i, j);
+ idx++;
+ }
+ }
+
+ double sum = 0.;
+ bool signChange = true;
+ for (register unsigned int i = 0; i < 4; i++)
+ {
+ if (std::abs(rotMu[i]) > 1e10 * std::numeric_limits<double>::epsilon() && std::abs(ref[i]) > 1e10
+ * std::numeric_limits<double>::epsilon() && rotMu[i] * ref[i] > 0)
+ signChange = false;
+ sum += std::abs(rotMu[i] * ref[i]);
+ }
+
+ if (sum < 1e4 * std::numeric_limits<double>::epsilon())
+ signChange = false;
+ if (signChange)
+ alpha = alpha + M_PI;
+ }
+ }
+ values[0] = alpha;
+}
+
+/*!
+ Prints the value of the major-axis orientation in degrees and rad
+ */
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& c){
+ os << (__FILE__) << std::endl;
+ os << "Alpha = " << c.values[0] << "rad = " << vpMath::deg(c.values[0]) << "deg " << std::endl;
+ return os;
+}
+
+/*!
+Prints the dependencies of alpha, namely centered moments mu11, mu20 ad mu02
+*/
+void vpMomentAlpha::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+ bool found_moment_centered;
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
+ found_moment_centered)));
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+
+ os << "mu11 = " << momentCentered.get(1, 1) << "\t";
+ os << "mu20 = " << momentCentered.get(2, 0) << "\t";
+ os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentArea.cpp b/modules/core/src/tracking/moments/vpMomentArea.cpp
new file mode 100644
index 0000000..80ca2e6
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentArea.cpp
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D area of the object
+ *
+ * Authors:
+ * Manikandan Bakthavatchalam
+ *
+ *****************************************************************************/
+#include <visp3/core/vpMomentArea.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <cmath>
+
+/*!
+ Has the area \f$ a = m_{00} = \mu_{00} \f$.
+ Gets the value of m00 from vpMomentCentered.
+*/
+void vpMomentArea::compute(){
+ /* getObject() returns a reference to a vpMomentObject. This is public member of vpMoment */
+ if(getObject().getType()==vpMomentObject::DISCRETE) {
+ bool found_moment_centered;
+ /* getMoments() returns a reference to a vpMomentDatabase. It is a protected member of and is 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<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ values[0] = momentCentered.get(2,0) + momentCentered.get(0,2);
+ }
+ else {
+ values[0] = getObject().get(0,0);
+ }
+}
+
+/*!
+ Default constructor.
+*/
+vpMomentArea::vpMomentArea() : vpMoment(){
+ values.resize(1);
+}
+
+/*!
+ Outputs the moment's values to a stream.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentArea& m){
+ os << (__FILE__) << std::endl;
+ os << "a(m00) = " << m.values[0] << std::endl;
+ return os;
+}
+
+/*!
+If the vpMomentObject type is
+1. DISCRETE(set of discrete points), uses mu20+mu02
+2. DENSE_FULL_OBJECT(from image) used mu00
+*/
+void vpMomentArea::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+
+ bool found_moment_centered;
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+
+ if(getObject().getType()==vpMomentObject::DISCRETE)
+ {
+ os << "mu20 = " << momentCentered.get(2, 0) << "\t";
+ os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
+ }
+ else
+ {
+ os << "mu00 = " << momentCentered.get(0, 0) << std::endl;
+ }
+}
diff --git a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp
new file mode 100644
index 0000000..bf3db21
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D normalized surface moment descriptor (usually described as An)
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <cmath>
+
+
+/*!
+ Computes the normalized area \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$.
+ Depends on vpMomentCentered.
+*/
+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*/
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+
+ double a;
+ /* getObject() returns a reference to the vpMomentObject from which
+ * the moment values are calculated. (public member of vpMoment)*/
+ if(getObject().getType()==vpMomentObject::DISCRETE)
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ else
+ a = getObject().get(0,0);
+
+ values[0] = desiredDepth*sqrt(desiredSurface/a);
+}
+
+/*!
+ Default constructor.
+ \param desired_surface : desired area \e a* when the visual servoing converges.
+ \param desired_depth : desired depth \e Z* when the visual servoing converges.
+*/
+vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, double desired_depth)
+ : vpMoment(),desiredSurface(desired_surface),desiredDepth(desired_depth)
+{
+ values.resize(1);
+}
+
+/*!
+ Outputs the moment's values to a stream.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& m){
+ os << (__FILE__) << std::endl;
+ os << "An = " << m.values[0] << std::endl ;
+ return os;
+}
+
+/*!
+Prints dependencies namely,
+1. Depth at desired pose Z*
+2. Area moment at desired pose
+ m00* if DENSE moment object, (mu20* + mu02*) if DISCRETE moment object
+3. Area moment at current pose
+ m00 if DENSE moment object, (mu20 + mu02) if DISCRETE moment object
+*/
+void vpMomentAreaNormalized::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+ os << "Desired depth Z* = " << desiredDepth << std::endl;
+ os << "Desired area m00* = " << desiredSurface << std::endl;
+
+ bool found_moment_centered;
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
+ if(!found_moment_centered)
+ throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+
+ double a;
+ if(getObject().getType()==vpMomentObject::DISCRETE)
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ else
+ a = getObject().get(0,0);
+ os << "a = " << a << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentBasic.cpp b/modules/core/src/tracking/moments/vpMomentBasic.cpp
new file mode 100644
index 0000000..d9011b3
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentBasic.cpp
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentObject.h>
+/*!
+ Default constructor.
+*/
+vpMomentBasic::vpMomentBasic() : vpMoment(){
+
+}
+
+/*!
+ Retrieve all moments of all orders computed. vpMomentBasic::get()[j*order+i] refers to moment \f$m_{ij}\f$.
+ \return all computed moments.
+
+ Same behaviour as vpMomentObject.
+*/
+const std::vector<double>& vpMomentBasic::get() const{
+ return getObject().get();
+}
+
+/*!
+ Gets the desired moment using indexes.
+ \param i : first index of the 2D moment.
+ \param j : second index of the 2D moment.
+ \return \f$m_{ij}\f$ moment.
+
+ Same behaviour as vpMomentObject.
+*/
+double vpMomentBasic::get(unsigned int i,unsigned int j) const{
+ return getObject().get(i,j);
+}
+
+/*!
+ Dummy function. Everything is already done in object.
+*/
+void vpMomentBasic::compute(){
+
+}
+
+/*!
+ Outputs the moment's values to a stream.
+ Same output as in vpMomentObject.
+*/
+VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentBasic& m){
+ os << (__FILE__) << std::endl;
+ vpMomentObject::printWithIndices(m.getObject(), os);
+ return os;
+}
+
+/*!
+No dependencies on other vpMoments, since basic moments are computed in vpMomentObject
+Just prints the basic moments in vpMomentObject with indices
+*/
+void vpMomentBasic::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+ os << "No dependencies on other vpMoments, since basic moments are computed in vpMomentObject" << std::endl;
+ vpMomentObject::printWithIndices(getObject(), os);
+}
diff --git a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp
new file mode 100644
index 0000000..b02b49a
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp
@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Descriptor for various invariants used to drive space roations around X and Y axis.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentObject.h>
+
+/*!
+ Default constructor.
+ (option to use a different calculation mode for sx and sy)
+*/
+vpMomentCInvariant::vpMomentCInvariant(bool flg_sxsynormalization)
+ : I(16),II(4),c(4),s(4), K(0.0), cn(4),sn(4), In1(0.0), flg_sxsynormalization_(flg_sxsynormalization)
+{
+ values.resize(14);
+}
+
+/*!
+ Computes some temporary invariants which are used internally to compute the final invariants.
+ \param momentCentered : centered moments
+ \param I : invariant output values
+*/
+void vpMomentCInvariant::computeI(const vpMomentCentered& momentCentered, std::vector<double>& I_val){
+
+ double mu30 = momentCentered.get(3,0);
+ double mu30_2 = mu30*mu30;
+ double mu30_3 = mu30_2*mu30;
+
+ double mu03 = momentCentered.get(0,3);
+ double mu03_2 = mu03*mu03;
+ double mu03_3 = mu03*mu03_2;
+
+ double mu20 = momentCentered.get(2,0);
+ double mu02 = momentCentered.get(0,2);
+ double mu50 = momentCentered.get(5,0);
+ double mu32 = momentCentered.get(3,2);
+ double mu14 = momentCentered.get(1,4);
+ double mu05 = momentCentered.get(0,5);
+ double mu23 = momentCentered.get(2,3);
+ double mu41 = momentCentered.get(4,1);
+ double mu40 = momentCentered.get(4,0);
+ double mu04 = momentCentered.get(0,4);
+ double mu31 = momentCentered.get(3,1);
+ double mu13 = momentCentered.get(1,3);
+ double mu22 = momentCentered.get(2,2);
+ double mu21 = momentCentered.get(2,1);
+ double mu12 = momentCentered.get(1,2);
+ double mu11 = momentCentered.get(1,1);
+
+ double mu11_2 = mu11*mu11;
+ double mu12_2 = mu12*mu12;
+ double mu21_2 = mu21*mu21;
+ double mu22_2 = mu22*mu22;
+ double mu13_2 = mu13*mu13;
+ double mu31_2 = mu31*mu31;
+ double mu04_2 = mu04*mu04;
+ double mu40_2 = mu40*mu40;
+ double mu21_3 = mu21*mu21_2;
+ double mu12_3 = mu12_2*mu12;
+ double mu12_4 = mu12_3*mu12;
+ double mu21_4 = mu21_2*mu21_2;
+
+ //double kappa = mu30_2+mu03_2-3*mu21_2+6*mu21*mu03; //Used in I8 calculation but simplified with MAPLE and found it to be wrong
+ double zeta = mu20-mu02;
+ double zeta_2 = zeta * zeta;
+ double omicron = (mu03_2+3*mu03*mu21+mu30*(mu30+3*mu12));
+ double omega = mu50+2*mu32+mu14;
+ double nu = mu05+2*mu23+mu41;
+ double ro = mu50-2*mu32-3*mu14;
+ double gamma = mu05-2*mu23-3*mu41;
+
+ double delta = mu50-10*mu32+5*mu14;
+ double phi = mu05-10*mu23+5*mu41;
+ double omega_2 = omega*omega;
+ double nu_2 = nu*nu;
+ double ro_2 = ro*ro;
+ double gamma_2 = gamma*gamma;
+ double delta_2 = delta*delta;
+ double phi_2 = phi*phi;
+
+ I_val[1]=-mu20*mu02+mu11_2;
+ I_val[2]=zeta_2+4*mu11_2;
+ I_val[3]=(mu30-3*mu12)*(mu30-3*mu12)+(mu03-3*mu21)*(mu03-3*mu21);
+ I_val[4]=(mu30+mu12)*(mu30+mu12)+(mu21+mu03)*(mu21+mu03);
+ I_val[5]=-mu30_2*mu03_2+(-4*mu12_3+6*mu21*mu12*mu03)*mu30-4*mu21_3*mu03+3*mu21_2*mu12_2;
+ I_val[6]=3*mu12_4+2*mu30*mu12_3+(3*mu30_2-6*mu03*mu21)*mu12_2-6*mu30*mu21*(mu21+mu03)*mu12+2*mu30_2*mu03_2+2*mu21_3*mu03+3*mu21_2*mu03_2+3*mu21_4;
+ I_val[7]=(3*mu21+2*mu03)*mu12_3+3*mu30*(mu03+2*mu21)*mu12_2-3*mu21*(mu30+mu03+mu21)*(-mu30+mu03+mu21)*mu12+mu30*(-mu30_2*mu03-2*mu21_3-3*mu03*mu21_2+mu03_3);
+ //I_val[8]=3*mu21_4-3*mu21_3*mu03+(3*mu03_2+kappa-6*mu12_2)*mu21_2-mu03*(-15*mu12_2+kappa)*mu21-(-3*mu12_2*mu30+(2*kappa-3*mu03_2)*mu12+kappa*mu30)*mu12;
+ I_val[8] = 3*mu03*mu21_3-2*mu03_2*mu21_2+mu21_2*mu30_2+3*mu12_2*mu03*mu21-mu03*mu21*mu30_2-mu03_3*mu21+3*mu12_3*mu30-2*mu12_2*mu30_2+mu12_2*mu03_2-mu12*mu30_3-mu12*mu30*mu03_2+3*mu12*mu30*mu21_2-6*mu12*mu30*mu03*mu21;
+ I_val[9]=omicron*omicron;
+
+ I_val[10]=mu40*mu04-4*mu31*mu13+3*mu22_2;
+ I_val[11]=3*mu13_2+2*mu31*mu13+(-3*mu40-3*mu04)*mu22-2*mu40*mu04+3*mu31_2;
+ I_val[12]=3*mu04_2+(2*mu40+12*mu22)*mu04+3*mu40_2+12*mu40*mu22+16*mu31*mu13;
+ I_val[13]=omega_2+nu_2;
+ I_val[14]=ro_2+gamma_2;
+ I_val[15]=delta_2+phi_2;
+
+ double a;
+ if(getObject().getType()==vpMomentObject::DISCRETE)
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ else
+ a = getObject().get(0,0);
+
+ c[1]=momentCentered.get(2,0)-momentCentered.get(0,2);
+ s[1]=2*momentCentered.get(1,1);
+ c[2]=momentCentered.get(0,3)-3*momentCentered.get(2,1);
+ s[2]=momentCentered.get(3,0)-3*momentCentered.get(1,2);
+ c[3]=c[1]*c[1]-s[1]*s[1];
+ s[3]=2*s[1]*c[1];
+
+ II[1]=c[1]*c[1]+s[1]*s[1];
+ II[2]=c[2]*c[2]+s[2]*s[2];
+ II[3]=momentCentered.get(2,0)+momentCentered.get(0,2);
+
+ K=(II[1]*(II[3]*sqrt(std::abs(II[3]))))/sqrt(std::abs(a));
+
+ /*
+ * Intermediate quantities required for calculation of normalized version of Sx and Sy
+ * The pij doubles below are the respective centered moment values mu_ij scaled by mu20 + mu02
+ */
+ double p20 = momentCentered.get(2,0)/II[3]; // II[3] is the normalization factor for the 2nd order moments
+ double p11 = momentCentered.get(1,1)/II[3];
+ double p02 = momentCentered.get(0,2)/II[3];
+
+ double d = sqrt(std::abs(a))/(II[3]*sqrt(std::abs(II[3]))); // d is the normalization factor for 3rd order moments
+ double p30 = momentCentered.get(3,0)*d;
+ double p21 = momentCentered.get(2,1)*d;
+ double p12 = momentCentered.get(1,2)*d;
+ double p03 = momentCentered.get(0,3)*d;
+
+ cn[1] = p20 - p02;
+ sn[1] = 2.0*p11;
+ sn[2] = p30 - 3.0*p12;
+ cn[2] = p03 - 3.0*p21;
+
+ cn[3] = cn[1]*cn[1]-sn[1]*sn[1];
+ sn[3] = 2.0*sn[1]*cn[1];
+
+ In1 = cn[1]*cn[1]+sn[1]*sn[1];
+}
+
+/*!
+ Computes translation-plane-rotation-scale invariants.
+ Depends on vpMomentCentered.
+ All possible invariants are computed here. The selection of the invariant is done afterwards.
+*/
+void vpMomentCInvariant::compute(){
+ if(getObject().getOrder()<5) throw vpException(vpException::notInitialized,"Order is not high enough for vpMomentCInvariant. Specify at least order 5.");
+ bool found_moment_centered;
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered)));
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+
+ computeI(momentCentered,I);
+ double II3_2 = II[3]*II[3];
+ double II3_3 = II3_2*II[3];
+
+ double a;
+ if(getObject().getType()==vpMomentObject::DISCRETE)
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ else
+ a = getObject().get(0,0);
+
+ values[0] = I[1]/I[2];
+ values[1] = I[3]/I[4];
+
+ values[2] = I[5]/I[6];
+
+ values[3] = I[7]/I[6];
+
+ values[4] = I[8]/I[6];
+
+ values[5] = I[9]/I[6];
+
+ values[6] = I[11]/I[10];
+
+ values[7] = I[12]/I[10];
+
+ values[8] = I[13]/I[15];
+
+ values[9] = I[14]/I[15];
+
+ if (flg_sxsynormalization_)
+ calcSxSyNormalized(values[10], values[11]);
+ else
+ calcSxSy(values[10], values[11]);
+
+ values[12] = II[1]/(II3_2); // Px
+ values[13] = a*II[2]/(II3_3); // Py
+}
+
+/*!
+ Sx and Sy as it was inside compute()
+ */
+void vpMomentCInvariant::calcSxSy(double& sx, double& sy) const{
+ sx = (c[2]*c[3]+s[2]*s[3])/K;
+ sy = (s[2]*c[3]-c[2]*s[3])/K;
+}
+
+/*!
+ * Sx and Sy from normalized 2nd and 3rd order moments
+ * Numerically better (than in the usual Sx,Sy when K appears in the denominator)
+ */
+void vpMomentCInvariant::calcSxSyNormalized(double& sx, double& sy) const{
+ sx = (cn[2]*cn[3] + sn[2]*sn[3]) / In1;
+ sy = (sn[2]*cn[3] - cn[2]*sn[3]) / In1;
+}
+
+/*!
+ Prints the temporary invariants. Used for debug purposes only
+ \param index : index of the temporary invariant
+*/
+void vpMomentCInvariant::printI(unsigned int index){
+ std::cout << "I("<< index << ")=" << I[index] << std::endl;
+}
+
+/*!
+ Print out all invariants that were computed
+ There are 15 of them, as in [Point-based and region based.ITRO05]
+ \cite Tahri05z
+ */
+void vpMomentCInvariant::printInvariants(std::ostream& os) const{
+ for (unsigned int i = 1; i < I.size(); ++i){ // i = 1 since vector I has been indexed from 1 in vpMomentCinvariant
+ os << "I[" << i << "]=" << I[i] << std::endl;
+ }
+ os << std::endl;
+
+}
+
+/*!
+ Outputs the moment's values to a stream.
+*/
+VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentCInvariant& c){
+ for(unsigned int i = 0;i<c.values.size();i++){
+ os << c.values[i] << "," << std::endl;
+ }
+ return os;
+}
+
diff --git a/modules/core/src/tracking/moments/vpMomentCentered.cpp b/modules/core/src/tracking/moments/vpMomentCentered.cpp
new file mode 100644
index 0000000..7a34536
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentCentered.cpp
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Centered moment descriptor
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <exception>
+#include <cassert>
+
+/*!
+ To set the values of centred moments. Required when normalizing the moment values.
+ @param i : first index of the 2D moment.
+ @param j : second index of the 2D moment.
+ @param value : value of the moment.
+*/
+void vpMomentCentered::set(unsigned int i, unsigned int j, double value){
+ vpMomentObject mobj = getObject();
+ assert(i+j<=mobj.getOrder());
+ if(i+j>mobj.getOrder()) throw vpException(vpException::badValue,"You cannot set that value.");
+ values[j*(mobj.getOrder()+1)+i] = value;
+}
+
+/*!
+ Computes centered moments of all available orders.
+ Depends on vpMomentGravityCenter.
+*/
+void vpMomentCentered::compute(){
+ bool found_moment_gravity;
+ values.resize((getObject().getOrder()+1)*(getObject().getOrder()+1));
+
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ unsigned int order = getObject().getOrder()+1;
+ for(register unsigned int j=0;j<(order);j++){
+ for(register unsigned int i=0;i<order-j;i++){
+ unsigned int c = order*j+i;
+ values[c]=0;
+ for(register unsigned int k=0;k<=i;k++){
+ double Xg_i_k = pow(-momentGravity.get()[0],(int)(i-k));
+ double comb_i_k = static_cast<double>( vpMath::comb(i,k) );
+ for(register unsigned int l=0;l<=j;l++){
+ values[c]+= static_cast<double>( comb_i_k*vpMath::comb(j,l)
+ *Xg_i_k
+ *pow(-momentGravity.get()[1],(int)(j-l))*getObject().get(k,l) );
+ }
+ }
+ }
+ }
+
+}
+
+/*!
+ Default constructor.
+*/
+vpMomentCentered::vpMomentCentered() : vpMoment(){
+
+}
+
+/*!
+ Gets the desired moment using indexes.
+ \param i : first index of the centered moment.
+ \param j : second index of the centered moment.
+ \return \f$\mu_{ij}\f$ moment.
+*/
+double vpMomentCentered::get(unsigned int i,unsigned int j) const {
+ unsigned int order = getObject().getOrder();
+ assert(i+j<=order);
+ if(i+j>order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
+
+ return values[j*(order+1)+i];
+}
+
+/*!
+ Outputs the centered moment's values \f$\mu_{ij}\f$ to a stream presented as a matrix.
+ The first line corresponds to \f$\mu_{0[0:order]}\f$, the second one to \f$\mu_{1[0:order]}\f$
+ Values in table corresponding to a higher order are marked with an "x" and not computed.
+
+ For example, if the maximal order is 3, the following values are provided:
+
+ \code
+u00 u10 u20 u30
+u01 u11 u21 x
+u02 u12 x x
+u30 x x x
+ \endcode
+
+ This output will be followed by an output with indexes as produced by printWithIndices() function
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& m){
+ for(unsigned int i = 0;i<m.values.size();i++){
+ if(i%(m.getObject().getOrder()+1)==0)
+ os << std::endl;
+
+ if((i%(m.getObject().getOrder()+1)+i/(m.getObject().getOrder()+1))<m.getObject().getOrder()+1)
+ os << m.values[i] ;
+ else
+ os << "x";
+
+ os << "\t";
+ }
+ os << std::endl;
+ m.printWithIndices(os);
+ return os;
+}
+
+/*!
+Print in a readable form which looks better than output from << operator
+*/
+void
+vpMomentCentered::printWithIndices(std::ostream& os) const {
+ unsigned int orderp1 = getObject().getOrder()+1;
+ for(unsigned int k=0; k<orderp1; k++) {
+ for(unsigned int l=0; l<orderp1-k; l++)
+ {
+ os << "mu[" << k << "," << l << "] = " << this->get(k,l) << "\t";
+ }
+ os << std::endl;
+ }
+ os << std::endl;
+}
+
+/*!
+Prints moments required for calculation of vpMomentCentered,
+which are
+1. Raw geometric moments (vpMomentObject) and
+2. Centre of gravity (vpMomentGravityCentered)
+*/
+void
+vpMomentCentered::printDependencies(std::ostream& os) const {
+ os << (__FILE__) << std::endl;
+ /*
+ Retreive the raw moments
+ */
+ const vpMomentObject objt = getObject();
+ vpMomentObject::printWithIndices(objt, os);
+
+ /*
+ Get xg,yg
+ */
+ bool found_moment_gravity;
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+ if(!found_moment_gravity)
+ throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ os << "Xg = " << momentGravity.getXg() << "\t" << "Yg = " << momentGravity.getYg() << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentCommon.cpp b/modules/core/src/tracking/moments/vpMomentCommon.cpp
new file mode 100644
index 0000000..06450f8
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentCommon.cpp
@@ -0,0 +1,213 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pre-filled moment database with all commonly used moments.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentObject.h>
+
+/*!
+ Default constructor. Initializes the common database with the following moments:
+ basic, gravity,centered,centered+normalized,normalized gravity,normalized surface, scale-plane-rotation-translation invariant, alpha, symmetric invariant.
+ \param dstSurface : destination surface. You may use vpMomentCommon::getSurface().
+ \param ref : reference 3rd order moments (see vpMomentAlpha). You may use vpMomentCommon::getMu3().
+ \param refAlpha : reference alpha (see vpMomentAlpha). You may use vpMomentCommon::getAlpha().
+ \param dstZ : destination depth.
+ \param flg_sxsyfromnormalized : flag to enable calculation of sx,sy from normalized moments.
+*/
+vpMomentCommon::vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ, bool flg_sxsyfromnormalized)
+ : vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), momentGravityNormalized(),
+ momentSurfaceNormalized(dstSurface,dstZ), momentCInvariant(), momentAlpha(ref,refAlpha),
+ momentArea()
+{
+ momentCInvariant = new vpMomentCInvariant(flg_sxsyfromnormalized);
+
+ momentBasic.linkTo(*this);
+ momentGravity.linkTo(*this);
+ momentCentered.linkTo(*this);
+ momentGravityNormalized.linkTo(*this);
+ momentSurfaceNormalized.linkTo(*this);
+ momentCInvariant->linkTo(*this);
+ momentAlpha.linkTo(*this);
+ momentArea.linkTo(*this);
+}
+
+/*!
+Updates all moments in the database with the object and computes all their values.
+This is possible because this particular database knows the link between the moments it contains.
+The order of computation is as follows:
+vpMomentGravityCenter,vpMomentCentered,vpMomentAlpha,vpMomentCInvariant,vpMomentSInvariant,vpMomentAreaNormalized,vpMomentGravityCenterNormalized
+\param object : Moment object.
+
+Example of using a preconfigured database to compute one of the C-invariants:
+\code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <iostream>
+
+int main()
+{
+ // Define two discrete points
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // std::vector that contains the vertices of the contour polygon
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-3); p.set_y(1); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+
+ vpMomentObject obj(5); // Object initialized up to order 5 to handle
+ // all computations required by vpMomentCInvariant
+ obj.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
+ obj.fromstd::vector(vec_p); // Init the discrete object with two points
+
+ //initialisation with default values
+ vpMomentCommon db(vpMomentCommon::getSurface(obj),vpMomentCommon::getMu3(obj),vpMomentCommon::getAlpha(obj),1.);
+ bool success;
+
+ db.updateAll(obj); // Update AND compute all moments
+
+ //get C-invariant
+ vpMomentCInvariant& C = static_cast<vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
+ if(success){
+ std::cout << C.get(0) << std:: std::endl;
+ }else
+ std::cout << "vpMomentCInvariant not found." << std::endl;
+
+ return 0;
+}
+
+\endcode
+*/
+void vpMomentCommon::updateAll(vpMomentObject& object){
+ try {
+ vpMomentDatabase::updateAll(object);
+
+ momentGravity.compute();
+ momentCentered.compute();
+ momentAlpha.compute();
+ momentCInvariant->compute();
+
+ momentSurfaceNormalized.compute();
+ momentGravityNormalized.compute();
+ momentArea.compute();
+
+ } catch(const char* ex){
+ std::cout << "exception:" << ex <<std::endl;
+
+ }
+}
+
+/*!
+Gets the surface of an object
+\param object : moment object
+*/
+double vpMomentCommon::getSurface(vpMomentObject& object) {
+ vpMomentDatabase moments;
+
+ vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;momentCentered.linkTo(moments);
+
+ moments.updateAll(object);
+
+ momentGravity.compute();
+ momentCentered.compute();
+
+ double a;
+ if (object.getType()==vpMomentObject::DISCRETE)
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ else
+ a = object.get(0,0);
+
+ return a;
+}
+
+/*!
+Gets a reference alpha of an object.
+\param object : Moment object.
+*/
+double vpMomentCommon::getAlpha(vpMomentObject& object) {
+ vpMomentDatabase moments;
+
+ vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;momentCentered.linkTo(moments);
+ vpMomentAlpha momentAlpha; momentAlpha.linkTo(moments);
+
+ moments.updateAll(object);
+ momentGravity.compute();
+ momentCentered.compute();
+ momentAlpha.compute();
+
+ return momentAlpha.get();
+}
+
+/*!
+Gets the reference 3rd order moments of an object.
+\param object : Moment object.
+*/
+std::vector<double> vpMomentCommon::getMu3(vpMomentObject& object) {
+ vpMomentDatabase moments;
+
+ vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;momentCentered.linkTo(moments);
+
+ moments.updateAll(object);
+
+ momentGravity.compute();
+ momentCentered.compute();
+
+ std::vector<double> mu(4);
+ unsigned int idx=0;
+ for (unsigned int j=0; j<4; j++) {
+ for (unsigned int i=0; i<4; i++) {
+ if (i+j==3){
+ mu[idx] = momentCentered.get(i,j);
+ idx++;
+ }
+ }
+ }
+ return mu;
+}
+
+vpMomentCommon::~vpMomentCommon(){
+ if (momentCInvariant)
+ delete momentCInvariant;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentDatabase.cpp b/modules/core/src/tracking/moments/vpMomentDatabase.cpp
new file mode 100644
index 0000000..6fbbda7
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentDatabase.cpp
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pseudo-database used to handle dependencies between moments
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMoment.h>
+#include <typeinfo>
+#include <iostream>
+#include <visp3/core/vpMomentObject.h>
+
+/*!
+ Adds a moment to the database.
+ \param moment : moment to add
+ \param : name of the moment's class
+
+ \attention You cannot add two moments with the same name. The rules for insersion are the same as those of std::map.
+*/
+void vpMomentDatabase::add(vpMoment& moment,const char* name){
+ moments.insert(std::pair<const char*,vpMoment*>((const char*)name,&moment));
+}
+
+/*!
+ Retrieves a moment from the database.
+ \param type : Name of the moment's class.
+ \param found : true if the moment's type exists in the database, false otherwise.
+ \return Moment corresponding to \e type.
+*/
+const vpMoment& vpMomentDatabase::get(const char* type, bool& found) const {
+ std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator it = moments.find(type);
+
+ found = (it!=moments.end());
+ return *(it->second);
+}
+
+/*!
+ Updates the moment object for all moments in the database
+ \param object : Moment object for which all the moments in the database should be updated.
+
+ Sometimes, it might be useful to update the whole database when computing only one moment when this moment depends on other moments.
+ The example provided in the header of this class gives an example that shows how to compute gravity center moment and the centered moment using a mass update.
+*/
+void vpMomentDatabase::updateAll(vpMomentObject& object){
+ std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
+ for(itr = moments.begin(); itr != moments.end(); itr++){
+ (*itr).second->update(object);
+ }
+}
+
+/*!
+ Outputs all the moments values in the database to a stream.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& m){
+ std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
+ os << "{";
+
+ for(itr = m.moments.begin(); itr != m.moments.end(); itr++){
+ os << (*itr).first << ": [" << *((*itr).second) << "],";
+ }
+ os << "}";
+
+ return os;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp
new file mode 100644
index 0000000..d5d14af
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D Gravity Center moment descriptor (usually described by the pair Xg,Yg)
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
+/*!
+ Computes the two gravity center coordinates commonly called \f$x_g\f$ and \f$y_g\f$.
+*/
+void vpMomentGravityCenter::compute(){
+ values[0] = getObject().get(1,0)/getObject().get(0,0);
+ values[1] = getObject().get(0,1)/getObject().get(0,0);
+}
+
+/*!
+ Default constructor.
+*/
+vpMomentGravityCenter::vpMomentGravityCenter() : vpMoment(){
+ values.resize(2);
+}
+
+/*!
+ Returns a vector of the two gravity center coordinates.
+ \return Coordinates in the following moment: \f$(x_g,y_g)\f$.
+*/
+const std::vector<double>& vpMomentGravityCenter::get() const{
+ return values;
+}
+
+/*!
+ Outputs the moment's values to a stream.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenter& m){
+ os << (__FILE__) << std::endl;
+ os << "(Xg,Yg) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
+ return os;
+}
+
+/*!
+Prints its dependencies
+Basic moments m10, m01 and m00 from vpMomentObject
+*/
+void vpMomentGravityCenter::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+ os << "m10 = " << getObject().get(1,0) << "\t";
+ os << "m00 = " << getObject().get(0,1) << "\t";
+ os << "m00 = " << getObject().get(0,0) << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
new file mode 100644
index 0000000..971eeab
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentGravityCenterNormalized.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+
+/*!
+ Computes normalized gravity center moment.
+ Depends on vpMomentAreaNormalized and on vpMomentGravityCenter.
+*/
+void vpMomentGravityCenterNormalized::compute(){
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
+
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+
+ if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ double Xn = momentGravity.get()[0]*momentSurfaceNormalized.get()[0];
+ double Yn = momentGravity.get()[1]*momentSurfaceNormalized.get()[0];
+
+ values[0] = Xn;
+ values[1] = Yn;
+
+}
+
+/*!
+ Default constructor.
+*/
+vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() : vpMomentGravityCenter(){}
+
+/*!
+ Outputs the moment's values to a stream.
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenterNormalized& m){
+ os << (__FILE__) << std::endl;
+ os << "(Xn,Yn) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
+ return os;
+}
+
+/*!
+Prints the dependent moments,
+1. centre of gravity
+2. normalized area moment
+*/
+void vpMomentGravityCenterNormalized::printDependencies(std::ostream& os) const{
+ os << (__FILE__) << std::endl;
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
+
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+
+ if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ os << "Xg = " << momentGravity.get()[0] << "\t" << "Yg = " << momentGravity.get()[1] << std::endl;
+ os << "An = " << momentSurfaceNormalized.get()[0] << std::endl;
+}
diff --git a/modules/core/src/tracking/moments/vpMomentObject.cpp b/modules/core/src/tracking/moments/vpMomentObject.cpp
new file mode 100644
index 0000000..ee2dfb2
--- /dev/null
+++ b/modules/core/src/tracking/moments/vpMomentObject.cpp
@@ -0,0 +1,656 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Object input structure used by moments.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpConfig.h>
+#include <stdexcept>
+
+#include <cmath>
+#include <limits>
+
+#ifdef VISP_HAVE_OPENMP
+#include <omp.h>
+#endif
+#include <cassert>
+
+/*!
+ Computes moments from a vector of points describing a polygon.
+ The points must be stored in a clockwise order. Used internally.
+ \param p : moment order (first index)
+ \param q : moment order (second index)
+ \param points : vector of points in a clockwise order
+
+ \return moment value
+*/
+double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points){
+ unsigned int i,k,l;
+ double den,mom,s;
+ double x_k;
+ double x_p_k;
+ double y_l;
+ double y_q_l;
+
+ den = static_cast<double>( (p+q+2)*(p+q+1)*vpMath::comb(p+q,p) );
+
+ mom = 0.0;
+ for (i=1;i<=points.size()-1;i++)
+ {
+ s = 0.0;
+ x_k=1;
+ for (k=0;k<=p;k++)
+ {
+ y_l=1;
+ x_p_k = pow(points[i-1].get_x(), (int)(p-k));
+ for (l=0;l<=q;l++)
+ {
+ y_q_l=pow(points[i-1].get_y(), (int)(q-l));
+
+ s += static_cast<double>(
+ vpMath::comb(k+l,l)
+ *vpMath::comb(p+q-k-l,q-l)
+ *x_k
+ *x_p_k
+ *y_l
+ *y_q_l );
+
+ y_l*=points[i].get_y();
+
+ }
+ x_k*=points[i].get_x();
+
+ }
+
+ s *= ((points[i-1].get_x())*(points[i].get_y())-(points[i].get_x())*(points[i-1].get_y()));
+ mom += s;
+ }
+ mom /= den;
+ return(mom);
+}
+
+/*!
+ Caching to avoid redundant multiplications.
+
+ \param cache : Lookup table that contains the order by order values. For example,
+ if the order is 3, cache will contain:
+ \code
+ 1 x x^2
+ y x*y x^2*y
+ y^2 x*y^2 x^2*y^2
+ \endcode
+
+ \param x, y : Coordinates of a point.
+*/
+void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y){
+ cache[0]=1;
+
+ for(register unsigned int i=1;i<order;i++)
+ cache[i]=cache[i-1]*x;
+
+ for(register unsigned int j=order;j<order*order;j+=order)
+ cache[j]=cache[j-order]*y;
+
+ for(register unsigned int j=1;j<order;j++){
+ for(register unsigned int i=1;i<order-j;i++){
+ cache[j*order+i] = cache[j*order]*cache[i];
+ }
+ }
+}
+
+/*!
+ * Manikandan.B
+ * Need to cache intensity along with the coordinates for photometric moments
+ */
+void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized) {
+
+ cache[0]=IntensityNormalized;
+
+ double invIntensityNormalized = 0.;
+ if (std::fabs(IntensityNormalized)>=std::numeric_limits<double>::epsilon())
+ invIntensityNormalized = 1.0/IntensityNormalized;
+
+ for(register unsigned int i=1;i<order;i++)
+ cache[i]=cache[i-1]*x;
+
+ for(register unsigned int j=order;j<order*order;j+=order)
+ cache[j]=cache[j-order]*y;
+
+ for(register unsigned int j=1;j<order;j++){
+ for(register unsigned int i=1;i<order-j;i++){
+ cache[j*order+i] = cache[j*order]*cache[i]*invIntensityNormalized;
+ }
+ }
+}
+
+
+/*!
+ Computes basic moments from a vector of points.
+ There are two cases:
+ -# Dense case specified by setType(vpMomentObject::DENSE_POLYGON):
+ - The points are the vertices describing a closed contour polygon.
+ - They must be stored in a clockwise order.
+ - The first and the last points should be the same to close the contour.
+ -# Discrete case specified by setType(vpMomentObject::DISCRETE)
+ - The points will be interpreted as a discrete point cloud.
+
+ \param points : Vector of points.
+
+ The code below shows how to use this function to consider a dense object defined by a closed contour.
+
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+
+int main()
+{
+ // Define the contour of an object by a 5 clockwise vertices on a plane
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+
+ p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
+ vec_p.push_back(p);
+
+ vpMomentObject obj(4); // Create an image moment object with 4 as maximum order
+ obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
+ obj.fromVector(vec_p); // Init the dense object with the polygon
+
+ return 0;
+}
+ \endcode
+
+ This other example shows how to consider an object as a discrete set of four points.
+
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
+
+int main()
+{
+ // Define 4 discrete points on a plane
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the 4 points
+
+ p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (point 1)
+ vec_p.push_back(p);
+ p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (point 2)
+ vec_p.push_back(p);
+ p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (point 3)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (point 4)
+ vec_p.push_back(p);
+
+ vpMomentObject obj(4); // Create an image moment object with 4 as maximum order
+ obj.setType(vpMomentObject::DISCRETE); // The object is constituted by discrete points
+ obj.fromVector(vec_p); // Init the dense object with the points
+
+ return 0;
+}
+ \endcode
+*/
+void vpMomentObject::fromVector(std::vector<vpPoint>& points){
+ if(type==vpMomentObject::DENSE_POLYGON){
+ if(
+ std::abs(points.rbegin()->get_x()-points.begin()->get_x())>std::numeric_limits<double>::epsilon() ||
+ std::abs(points.rbegin()->get_y()-points.begin()->get_y())>std::numeric_limits<double>::epsilon()
+ ){
+ points.resize(points.size()+1);
+ points[points.size()-1] = points[0];
+ }
+ for(register unsigned int j=0;j<order*order;j++){
+ values[j]=calc_mom_polygon(j%order,j/order,points);
+ }
+ }else{
+ std::vector<double> cache(order*order,0.);
+ values.assign(order*order,0);
+ for(register unsigned int i=0;i<points.size();i++){
+ cacheValues(cache,points[i].get_x(),points[i].get_y());
+ for(register unsigned int j=0;j<order;j++){
+ for(register unsigned int k=0;k<order-j;k++){
+ values[j*order+k]+=cache[j*order+k];
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Computes basic moments from an image.
+ There is no assumption made about whether the input is dense or discrete but it's more common to use vpMomentObject::DENSE_FULL_OBJECT with this method.
+
+ \param image : Image to consider.
+ \param threshold : Pixels with a luminance lower than this threshold will be considered.
+ \param cam : Camera parameters used to convert pixels coordinates in meters in the image plane.
+
+ The code below shows how to use this function.
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpCameraParameters cam; // Camera parameters used for pixel to meter conversion
+ vpImage<unsigned char> I(288, 384); // Image used to define the object
+ // ... Initialize the image
+
+ unsigned char threshold = 128; // Gray level used to define which part of the image belong to the dense object
+
+ vpMomentObject obj(3); // Create an image moment object with 3 as maximum order
+ obj.fromImage(I, threshold, cam); // Initialize the object from the image
+
+ return 0;
+}
+ \endcode
+*/
+
+void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned char threshold, const vpCameraParameters& cam){
+#ifdef VISP_HAVE_OPENMP
+ #pragma omp parallel shared(threshold)
+ {
+ std::vector<double> curvals(order*order);
+ curvals.assign(order*order,0.);
+ unsigned int i_, j_;
+
+ #pragma omp for nowait//automatically organize loop counter between threads
+ for(int i=0;i<(int)image.getCols();i++){
+ for(int j=0;j<(int)image.getRows();j++){
+ i_ = static_cast<unsigned int>(i);
+ j_ = static_cast<unsigned int>(j);
+ if(image[j_][i_]>threshold){
+ double x=0;
+ double y=0;
+ vpPixelMeterConversion::convertPoint(cam,i_,j_,x,y);
+
+ double xval=1.;
+ double yval=1.;
+ for(register unsigned int k=0;k<order;k++){
+ xval=1.;
+ for(register unsigned int l=0;l<order-k;l++){
+ curvals[(k*order+l)]+=(xval*yval);
+ xval*=x;
+ }
+ yval*=y;
+ }
+ }
+ }
+ }
+
+ #pragma omp master //only set this variable in master thread
+ {
+ values.assign(order*order, 0.);
+ }
+
+ #pragma omp barrier
+
+ for(register unsigned int k=0;k<order;k++){
+ for(register unsigned int l=0;l<order-k;l++){
+ //#pragma omp atomic // Removed since causes a build issue with msvc14 2015
+ values[k*order+l]+= curvals[k*order+l];
+ }
+ }
+
+ }
+#else
+ std::vector<double> cache(order*order,0.);
+ values.assign(order*order,0);
+ for(register unsigned int i=0;i<image.getCols();i++){
+ for(register unsigned int j=0;j<image.getRows();j++){
+ if(image[j][i]>threshold){
+ double x=0;
+ double y=0;
+ vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
+ cacheValues(cache,x,y);
+ for(register unsigned int k=0;k<order;k++){
+ for(register unsigned int l=0;l<order-k;l++){
+ values[k*order+l]+=cache[k*order+l];
+ }
+ }
+ }
+ }
+ }
+#endif
+
+ //Normalisation equivalent to sampling interval/pixel size delX x delY
+ double norm_factor = 1./(cam.get_px()*cam.get_py());
+ for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
+ *it = (*it) * norm_factor;
+ }
+}
+
+/*!
+ * Manikandan. B
+ * Photometric moments v2
+ * Intended to be used by 'vpMomentObject's of type DENSE_FULL_OBJECT
+ * @param image : Grayscale image
+ * @param cam : Camera parameters (to change to )
+ * @param bg_type : White/Black background surrounding the image
+ * @param normalize_with_pix_size : This flag if SET, the moments, after calculation are normalized w.r.t pixel size
+ * available from camera parameters
+ */
+void vpMomentObject::fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam,
+ vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
+{
+ std::vector<double> cache(order*order,0.);
+ values.assign(order*order,0);
+
+ // (x,y) - Pixel co-ordinates in metres
+ double x=0;
+ double y=0;
+ //for indexing into cache[] and values[]
+ unsigned int idx = 0;
+ unsigned int kidx = 0;
+
+ double intensity = 0;
+ double intensity_white = 0;
+
+ //double Imax = static_cast<double>(image.getMaxValue());
+ double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
+
+ double iscale = 1.0;
+ if (flg_normalize_intensity) // This makes the image a probability density function
+ iscale = 1.0/Imax;
+
+ if (bg_type == vpMomentObject::WHITE) {
+ /////////// WHITE BACKGROUND ///////////
+ for(register unsigned int j=0;j<image.getRows();j++){
+ for(register unsigned int i=0;i<image.getCols();i++){
+ x = 0;
+ y = 0;
+ intensity = (double)(image[j][i])*iscale;
+ intensity_white = 1. - intensity;
+
+ vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
+ cacheValues(cache,x,y, intensity_white); // Modify 'cache' which has x^p*y^q to x^p*y^q*(1 - I(x,y))
+
+ // Copy to "values"
+ for(register unsigned int k=0;k<order;k++){
+ kidx = k*order;
+ for(register unsigned int l=0;l<order-k;l++){
+ idx = kidx+l;
+ values[idx]+= cache[idx];
+ }
+ }
+ }
+ }
+ }
+ else {
+ /////////// BLACK BACKGROUND ///////////
+ for(register unsigned int j=0;j<image.getRows();j++){
+ for(register unsigned int i=0;i<image.getCols();i++){
+ x = 0;
+ y = 0;
+ intensity = (double)(image[j][i])*iscale;
+ vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
+
+ // Cache values for fast moment calculation
+ cacheValues(cache,x,y, intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
+
+ // Copy to moments array 'values'
+ for(register unsigned int k=0;k<order;k++){
+ kidx = k*order;
+ for(register unsigned int l=0;l<order-k;l++){
+ idx = kidx+l;
+ values[idx]+= cache[idx];
+ }
+ }
+
+ }
+ }
+ }
+
+ if (normalize_with_pix_size){
+ // Normalisation equivalent to sampling interval/pixel size delX x delY
+ double norm_factor = 1./(cam.get_px()*cam.get_py());
+ for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
+ *it = (*it) * norm_factor;
+ }
+ }
+}
+
+/*!
+ Does exactly the work of the default constructor as it existed in the very
+ first version of vpMomentObject
+ */
+void
+vpMomentObject::init(unsigned int orderinp) {
+ order = orderinp + 1;
+ type = vpMomentObject::DENSE_FULL_OBJECT;
+ flg_normalize_intensity = true; // By default, the intensity values are normalized
+ values.resize((order+1)*(order+1));
+ values.assign((order+1)*(order+1),0);
+}
+
+/*!
+ Helper to copy constructor
+ */
+void
+vpMomentObject::init(const vpMomentObject& objin){
+ order = objin.getOrder()+1;
+ type = objin.getType();
+ flg_normalize_intensity = objin.flg_normalize_intensity;
+ values.resize(objin.values.size());
+ values = objin.values;
+}
+
+/*!
+ Default constructor.
+ Initializes the object with the maximum used order. You cannot use higher order moments than the order of the moment object.
+ The parameter specified is the highest desired included order.
+ All orders up to this values will be computed. In other words, a vpMomentObject will compute all \f$ m_{ij} \f$ moments with \f$ i+j \in [0..order] \f$.
+
+ \param max_order : Maximum reached order (i+j) to be used. All
+ considered i+j will be of order smaller or equal than this
+ parameter. For example if this parameter is 5, all moment values of
+ order 0 to 5 included will be computed.
+
+ Mani : outsourced the constructor work to void init (unsigned int orderinp);
+*/
+vpMomentObject::vpMomentObject(unsigned int max_order)
+ : flg_normalize_intensity(true), order(max_order+1), type(vpMomentObject::DENSE_FULL_OBJECT),
+ values()
+{
+ init(max_order);
+}
+
+/*!
+ Copy constructor
+ */
+vpMomentObject::vpMomentObject(const vpMomentObject& srcobj)
+ : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT),
+ values()
+{
+ init(srcobj);
+}
+
+/*!
+ Returns all basic moment values \f$m_{ij}\f$ with \f$i \in [0:\mbox{order}]\f$ and \f$j \in [0:\mbox{order}]\f$.
+
+ \return Vector of moment values. To access \f$m_{ij}\f$, you have to read vpMomentObject::get()[j*(order+1)+i].
+
+ For example, if the maximal order is 3, the following values are provided:
+ \code
+m00 m10 m20 m01 m11 m21 m02 m12 m12 m30 m03
+ \endcode
+
+ To access for example to the basic moment m12, you should use this kind of code:
+ \code
+vpMomentObject obj(3);
+// ... initialise the object using fromVector() or fromImage()
+std::vector mij = obj.get();
+double m12;
+m12 = mij[2*(obj.getOrder()+1)+1]; // i=1 and j=2
+ \endcode
+*/
+const std::vector<double>& vpMomentObject::get() const {
+ return values;
+
+}
+
+/*!
+ Returns the basic moment value \f$m_{ij}\f$ corresponding to i,j indexes
+
+ \param i : First moment index, with \f$i+j \leq order\f$.
+ \param j : Second moment index, with \f$i+j \leq order\f$.
+*/
+double vpMomentObject::get(unsigned int i, unsigned int j) const {
+ assert(i+j<=getOrder());
+ if(i+j>=order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
+
+ return values[j*order+i];
+}
+
+/*!
+ Sets the basic moment value \f$m_{ij}\f$ corresponding to i,j indexes
+ \param i : First moment index, with \f$i+j \leq order\f$.
+ \param j : Second moment index, with \f$i+j \leq order\f$.
+ \param value_ij : Moment value.
+*/
+void vpMomentObject::set(unsigned int i, unsigned int j, const double& value_ij){
+ assert(i+j<=getOrder());
+ if(i+j>=order) throw vpException(vpException::badValue,"The requested value cannot be set, you should specify a higher order for the moment object.");
+ values[j*order+i] = value_ij;
+}
+
+/*!
+ Outputs the basic moment's values \f$m_{ij}\f$ to a stream presented as a matrix.
+ The first line corresponds to \f$m_{0[0:order]}\f$, the second one to \f$m_{1[0:order]}\f$
+ Values in table corresponding to a higher order are marked with an "x" and not computed.
+
+ For example, if the maximal order is 3, the following values are provided:
+
+ \code
+ m00 m10 m20 m30
+ m01 m11 m21 x
+ m02 m12 x x
+ m03 x x x
+ \endcode
+
+*/
+VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& m){
+ for(unsigned int i = 0;i<m.values.size();i++){
+
+ if(i%(m.order)==0)
+ os << std::endl;
+
+ if((i%(m.order)+i/(m.order))<m.order)
+ os << m.values[i] ;
+ else
+ os << "x";
+
+ os << "\t";
+
+ }
+
+ return os;
+}
+
+/*!
+ Outputs the raw moment values \f$m_{ij}\f$ in indexed form.
+ The moment values are same as provided by the operator << which outputs x for uncalculated moments.
+ */
+void
+vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os) {
+ std::vector<double> moment = momobj.get();
+ os << std::endl <<"Order of vpMomentObject: "<<momobj.getOrder()<<std::endl;
+ // Print out values. This is same as printing using operator <<
+ for(unsigned int k=0; k<=momobj.getOrder(); k++) {
+ for(unsigned int l=0; l<(momobj.getOrder()+1)-k; l++){
+ os << "m[" << l << "," << k << "] = " << moment[k*(momobj.getOrder()+1)+ l] << "\t";
+ }
+ os << std::endl;
+ }
+ os <<std::endl;
+}
+
+/*!
+ This function returns a vpMatrix of size (order+1, order+1).
+\code
+ vpMomentObject obj(8);
+ obj.setType(vpMomentObject::DENSE_FULL_OBJECT);
+ obj.fromImageWeighted(I, cam, vpMomentObject::BLACK); // cam should have the camera parameters
+ vpMatrix Mpq = vpMomentObject::convertTovpMatrix(obj);
+\endcode
+ Instead of accessing the moment m21 as obj.get(2,1), you can now do Mpq[2][1].
+ This is useful when you want to use the functions available in vpMatrix.
+ One use case i see now is to copy the contents of the matrix to a file or std::cout.
+ For instance, like
+ \code
+ // Print to console
+ Mpq.maplePrint(std::cout);
+ // Or write to a file
+ std::ofstream fileMpq("Mpq.csv");
+ Mpq.maplePrint(fileMpq);
+\endcode
+
+The output can be copied and pasted to MAPLE as a matrix.
+
+\warning
+The moments that are not calculated have zeros. For instance, for a vpMomentObject of order 8,
+the moment m[7,2] is not calculated. It will have 0 by default. User discretion is advised.
+*/
+vpMatrix
+vpMomentObject::convertTovpMatrix(const vpMomentObject& momobj) {
+ std::vector<double> moment = momobj.get();
+ unsigned int order = momobj.getOrder();
+ vpMatrix M(order+1, order+1);
+ for(unsigned int k=0; k<=order; k++) {
+ for(unsigned int l=0; l<(order+1)-k; l++){
+ M[l][k] = moment[k*(order+1)+ l];
+ }
+ }
+ return M;
+}
+
+/*!
+ Nothing to destruct. This will allow for a polymorphic usage
+ For instance,
+ \code
+ vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
+ \endcode
+ */
+vpMomentObject::~vpMomentObject(){
+// deliberate empty
+}
diff --git a/modules/core/src/tracking/vpTracker.cpp b/modules/core/src/tracking/vpTracker.cpp
new file mode 100644
index 0000000..5b61acc
--- /dev/null
+++ b/modules/core/src/tracking/vpTracker.cpp
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic tracker.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpTracker.h>
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpTracker.cpp
+ \brief Class that defines what is a generic tracker.
+*/
+
+
+void
+vpTracker::init()
+{
+ cPAvailable = false ;
+}
+
+
+
+vpTracker::vpTracker() : p(), cP(), cPAvailable(false) {}
+
+vpTracker::vpTracker(const vpTracker &tracker) : p(), cP(), cPAvailable(false)
+{
+ *this = tracker;
+}
+
+
+vpTracker &vpTracker::operator=(const vpTracker &tracker)
+{
+ p = tracker.p;
+ cP = tracker.cP;
+ cPAvailable = tracker.cPAvailable;
+
+ return *this;
+}
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/test/camera/testCameraParametersConversion.cpp b/modules/core/test/camera/testCameraParametersConversion.cpp
new file mode 100644
index 0000000..f86716b
--- /dev/null
+++ b/modules/core/test/camera/testCameraParametersConversion.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Performs various tests on the vpPixelMeterConversion and
+ * vpPixelMeterConversion class.
+ *
+ * Authors:
+ * Anthony saunier
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file testCameraParametersConversion.cpp
+
+ Performs various tests on the vpPixelMeterConversion and
+ vpPixelMeterConversion class.
+*/
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpMath.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+int main()
+{
+ try {
+ vpCameraParameters cam;
+ double px,py,u0,v0;
+ px = 1657.429131;
+ py = 1658.818598;
+ u0 = 322.2437833;
+ v0 = 230.8012737;
+ vpCameraParameters camDist;
+ double px_dist,py_dist,u0_dist,v0_dist,kud_dist,kdu_dist;
+ px_dist = 1624.824731;
+ py_dist = 1625.263641;
+ u0_dist = 324.0923411;
+ v0_dist = 245.2421388;
+ kud_dist = -0.1741532338;
+ kdu_dist = 0.1771165148;
+
+ cam.initPersProjWithoutDistortion(px,py,u0,v0);
+ camDist.initPersProjWithDistortion(px_dist,py_dist,u0_dist,v0_dist,
+ kud_dist, kdu_dist);
+
+ double u1 = 320;
+ double v1 = 240;
+ double x1 = 0, y1 = 0;
+ double u2 = 0, v2 = 0;
+ vpPixelMeterConversion::convertPoint(cam,u1,v1,x1,y1);
+ vpMeterPixelConversion::convertPoint(cam,x1,y1,u2,v2);
+ if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
+ vpTRACE("Error in convertPoint without distortion:\n"
+ "u1 = %f, u2 = %f\n"
+ "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
+ return -1;
+ }
+ vpTRACE("convertPoint without distortion :\n"
+ "u1 - u2 = %.20f\n"
+ "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
+
+ vpPixelMeterConversion::convertPoint(camDist,u1,v1,x1,y1);
+ vpMeterPixelConversion::convertPoint(camDist,x1,y1,u2,v2);
+ if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
+ vpTRACE("Error in convertPoint with distortion :\n"
+ "u1 = %f, u2 = %f\n"
+ "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
+ return -1;
+ }
+ vpTRACE("convertPoint with distortion :\n"
+ "u1 - u2 = %.20f\n"
+ "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/image/testConversion.cpp b/modules/core/test/image/testConversion.cpp
new file mode 100644
index 0000000..df01757
--- /dev/null
+++ b/modules/core/test/image/testConversion.cpp
@@ -0,0 +1,668 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image conversions.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <stdlib.h>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTime.h>
+
+
+/*!
+ \example testConversion.cpp
+
+ \brief Manipulation of image conversions.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test image conversions.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_grey.pgm and Klimt_color.ppm output images\n\
+ are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (opt_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ vpImage<unsigned char> Ig ; // Grey image
+ vpImage<vpRGBa> Ic ; // Color image
+
+ //-------------------- .pgm -> .ppm
+ vpTRACE("Convert a grey image (.pgm) to a color image (.ppm)");
+ // Load a grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ig, filename) ;
+ // Create a color image from the grey
+ vpImageConvert::convert(Ig, Ic);
+ filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
+ vpCTRACE << "Write " << filename << std::endl;
+ vpImageIo::write(Ic, filename) ;
+
+ //-------------------- .ppm -> .pgm
+ vpTRACE("Convert a color image (.ppm) to a grey image (.pgm)");
+ // Load a color image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ic, filename) ;
+ // Create a grey image from the color
+ vpImageConvert::convert(Ic, Ig);
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
+ vpCTRACE << "Write " << filename << std::endl;
+ vpImageIo::write(Ig, filename) ;
+
+ //-------------------- YUV -> RGB
+ unsigned char y=187, u=10, v=30;
+ unsigned char r, g, b;
+
+ // Convert a YUV pixel value to a RGB value
+ vpImageConvert::YUVToRGB(y, u, v, r, g, b);
+ vpTRACE("y(%d) u(%d) v(%d) = r(%d) g(%d) b(%d)", y, u, v, r, g, b);
+
+#ifdef VISP_HAVE_OPENCV
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ double t0 = vpTime::measureTimeMs();
+ /////////////////////////
+ // Convert a IplImage to a vpImage<vpRGBa>
+ ////////////////////////
+ IplImage* image = NULL; /*!< The image read / acquired */
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ vpCTRACE << "Reading the color image with opencv: "<< std::endl
+ << filename << std::endl;
+ if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) {
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpImageConvert::convert(image, Ic);
+ filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.ppm");
+ /* Save the the current image */
+ vpImageIo::write(Ic, filename) ;
+
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+
+ /* Read the pgm image */
+
+ vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
+ << filename << std::endl;
+ if(image!=NULL) cvReleaseImage( &image );
+ if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpImageConvert::convert(image, Ic);
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.ppm");
+ /* Save the the current image */
+ vpImageIo::write(Ic, filename) ;
+
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ///////////////////////////
+ // Convert a IplImage to a vpImage<unsigned char>
+ ////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ vpCTRACE << "Reading the color image with opencv: "<< std::endl
+ << filename << std::endl;
+ if(image!=NULL) cvReleaseImage( &image );
+ if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) {
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpImageConvert::convert(image, Ig);
+ filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.pgm");
+ /* Save the the current image */
+ vpImageIo::write(Ig, filename) ;
+
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+
+ /* Read the pgm image */
+
+ vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
+ << filename << std::endl;
+ if(image!=NULL) cvReleaseImage( &image );
+ if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+
+ return (-1);
+ }
+ vpImageConvert::convert(image, Ig);
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.pgm");
+ /* Save the the current image */
+ vpImageIo::write(Ig, filename) ;
+
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ////////////////////////////////////
+ // Convert a vpImage<vpRGBa> to a IplImage
+ ////////////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ // Load a color image from the disk
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ic, filename) ;
+ vpImageConvert::convert(Ic, image);
+ filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cv.ppm");
+ /* Save the the current image */
+ vpCTRACE << "Write " << filename << std::endl;
+ if((cvSaveImage(filename.c_str(), image)) == 0) {
+ vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
+ if(image!=NULL) cvReleaseImage( &image );
+ return (-1);
+ }
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ////////////////////////////////////////
+ // Convert a IplImage to a vpImage<unsigned char>
+ ////////////////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+
+ /* Read the grey image */
+
+ // Load a color image from the disk
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ig, filename) ;
+ vpImageConvert::convert(Ig, image);
+ filename = vpIoTools::createFilePath(opath, "Klimt_ipl_grey_cv.pgm");
+ /* Save the the current image */
+
+ vpCTRACE << "Write " << filename << std::endl;
+ if((cvSaveImage(filename.c_str(), image)) == 0) {
+ vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
+ if(image!=NULL) cvReleaseImage( &image );
+ return (-1);
+ }
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ if(image!=NULL) cvReleaseImage( &image );
+ double t1 = vpTime::measureTimeMs();
+ std::cout << "Conversion c interface : " << t1 - t0 << " ms" << std::endl;
+#endif
+
+ /* ------------------------------------------------------------------------ */
+ /* conversion for the new c++ interface */
+ /* ------------------------------------------------------------------------ */
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ double t2 = vpTime::measureTimeMs();
+ /////////////////////////
+ // Convert a cv::Mat to a vpImage<vpRGBa>
+ ////////////////////////
+ cv::Mat imageMat;
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ vpCTRACE << "Reading the color image with c++ interface of opencv: "<< std::endl
+ << filename << std::endl;
+ imageMat = cv::imread(filename, 1);// force to a three channel color image.
+ if(imageMat.data == NULL){
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return -1;
+ }
+ vpImageConvert::convert(imageMat, Ic);
+ filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.ppm");
+ /* Save the the current image */
+ vpImageIo::write(Ic, filename) ;
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ /* Read the pgm image */
+
+ vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
+ << filename << std::endl;
+ imageMat = cv::imread(filename, 0);// forced to grayscale.
+ if(imageMat.data == NULL) {
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpImageConvert::convert(imageMat, Ic);
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.ppm");
+ /* Save the the current image */
+ vpImageIo::write(Ic, filename) ;
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ///////////////////////////
+ // Convert a cv::Mat to a vpImage<unsigned char>
+ ////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ vpCTRACE << "Reading the color image with opencv: "<< std::endl
+ << filename << std::endl;
+ imageMat = cv::imread(filename, 1);// force to a three channel color image.
+ if(imageMat.data == NULL){
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return -1;
+ }
+ vpImageConvert::convert(imageMat, Ig);
+ filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.pgm");
+ /* Save the the current image */
+ vpImageIo::write(Ig, filename) ;
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+
+ /* Read the pgm image */
+
+ vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
+ << filename << std::endl;
+ imageMat = cv::imread(filename, 0);
+ if(imageMat.data == NULL){
+ vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpImageConvert::convert(imageMat, Ig);
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.pgm");
+ /* Save the the current image */
+ vpImageIo::write(Ig, filename) ;
+
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ////////////////////////////////////
+ // Convert a vpImage<vpRGBa> to a cv::Mat
+ ////////////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ // Load a color image from the disk
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ic, filename) ;
+ vpImageConvert::convert(Ic, imageMat);
+ filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cvMat.ppm");
+ /* Save the the current image */
+ vpCTRACE << "Write " << filename << std::endl;
+ if(!cv::imwrite(filename, imageMat)){
+ vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ////////////////////////////////////////
+ // Convert a IplImage to a vpImage<unsigned char>
+ ////////////////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+
+ /* Read the grey image */
+
+ // Load a color image from the disk
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ig, filename);
+ vpImageConvert::convert(Ig, imageMat);
+ filename = vpIoTools::createFilePath(opath, "Klimt_ipl_grey_cvMat.pgm");
+ /* Save the the current image */
+
+ vpCTRACE << "Write " << filename << std::endl;
+ if(!cv::imwrite(filename, imageMat)){
+ vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
+ return (-1);
+ }
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+ double t3 = vpTime::measureTimeMs();
+ std::cout << "Conversion c++ interface : " << t3 - t2 << " ms" << std::endl;
+#endif
+#endif
+
+ ////////////////////////////////////
+ // Split a vpImage<vpRGBa> to vpImage<unsigned char>
+ ////////////////////////////////////
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+
+ /* Read the color image */
+
+ // Load a color image from the disk
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Ic, filename) ;
+ vpImage<unsigned char> R,G,B,a;
+ vpImageConvert::split(Ic, &R,NULL,&B);
+ double begintime = vpTime::measureTimeMs();
+ for(int i=0; i<1000;i++){
+ vpImageConvert::split(Ic, &R,NULL,&B);
+ }
+ double endtime = vpTime::measureTimeMs();
+
+ std::cout<<"Time for 1000 split (ms): "<< endtime - begintime <<std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_RChannel.pgm");
+ /* Save the the current image */
+ vpCTRACE << "Write " << filename << std::endl;
+ vpImageIo::write(R, filename) ;
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_BChannel.pgm");
+ /* Save the the current image */
+ vpCTRACE << "Write " << filename << std::endl;
+ vpImageIo::write(B, filename) ;
+ vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
+
+ ////////////////////////////////////
+ // Merge 4 vpImage<unsigned char> (RGBa) to vpImage<vpRGBa>
+ ////////////////////////////////////
+ vpImageConvert::split(Ic, &R, &G, &B, &a);
+ begintime = vpTime::measureTimeMs();
+ vpImage<vpRGBa> I_merge;
+ for(int i=0; i<1000; i++){
+ vpImageConvert::merge(&R, &G, &B, &a, I_merge);
+ }
+ endtime = vpTime::measureTimeMs();
+
+ std::cout<<"Time for 1000 merge (ms): "<< endtime - begintime <<std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_merge.ppm");
+ /* Save the the current image */
+ vpImageIo::write(I_merge, filename) ;
+
+ ////////////////////////////////////
+ // Convert a vpImage<vpRGBa> in RGB color space to a vpImage<vpRGBa> in HSV color
+ ////////////////////////////////////
+ unsigned int size = Ic.getWidth()*Ic.getHeight();
+ unsigned int w = Ic.getWidth(), h = Ic.getHeight();
+ unsigned char *hue = new unsigned char[size];
+ unsigned char *saturation = new unsigned char[size];
+ unsigned char *value = new unsigned char[size];
+
+ vpImageConvert::RGBaToHSV((unsigned char *) Ic.bitmap, hue, saturation, value, size);
+ vpImage<unsigned char> I_hue(hue, h, w);
+ vpImage<unsigned char> I_saturation(saturation, h, w);
+ vpImage<unsigned char> I_value(value, h, w);
+ vpImage<vpRGBa> I_HSV;
+ vpImageConvert::merge(&I_hue, &I_saturation, &I_value, NULL, I_HSV);
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_HSV.ppm");
+ /* Save the the current image */
+ vpImageIo::write(I_HSV, filename);
+
+ //Check the conversion RGBa <==> HSV
+ double *hue2 = new double[size];
+ double *saturation2 = new double[size];
+ double *value2 = new double[size];
+ vpImageConvert::RGBaToHSV((unsigned char *) Ic.bitmap, hue2, saturation2, value2, size);
+
+ unsigned char *rgba = new unsigned char[size*4];
+ vpImageConvert::HSVToRGBa(hue2, saturation2, value2, rgba, size);
+
+ if(hue2 != NULL) {
+ delete[] hue2;
+ hue2 = NULL;
+ }
+
+ if(saturation2 != NULL) {
+ delete[] saturation2;
+ saturation2 = NULL;
+ }
+
+ if(value2 != NULL) {
+ delete[] value2;
+ value2 = NULL;
+ }
+
+ vpImage<vpRGBa> I_HSV2RGBa((vpRGBa *) rgba, h, w);
+ filename = vpIoTools::createFilePath(opath, "Klimt_HSV2RGBa.ppm");
+ /* Save the the current image */
+ vpImageIo::write(I_HSV2RGBa, filename);
+
+ for(unsigned int i = 0; i < Ic.getHeight(); i++) {
+ for(unsigned int j = 0; j < Ic.getWidth(); j++) {
+ if(Ic[i][j].R != I_HSV2RGBa[i][j].R ||
+ Ic[i][j].G != I_HSV2RGBa[i][j].G ||
+ Ic[i][j].B != I_HSV2RGBa[i][j].B) {
+ std::cerr << "Ic[i][j].R=" << static_cast<unsigned>(Ic[i][j].R)
+ << " ; I_HSV2RGBa[i][j].R=" << static_cast<unsigned>(I_HSV2RGBa[i][j].R) << std::endl;
+ std::cerr << "Ic[i][j].G=" << static_cast<unsigned>(Ic[i][j].G)
+ << " ; I_HSV2RGBa[i][j].G=" << static_cast<unsigned>(I_HSV2RGBa[i][j].G) << std::endl;
+ std::cerr << "Ic[i][j].B=" << static_cast<unsigned>(Ic[i][j].B)
+ << " ; I_HSV2RGBa[i][j].B=" << static_cast<unsigned>(I_HSV2RGBa[i][j].B) << std::endl;
+ throw vpException(vpException::fatalError, "Problem with conversion between RGB <==> HSV");
+ }
+ }
+ }
+
+ ////////////////////////////////////
+ // Test construction of vpImage from an array with copyData==true
+ ////////////////////////////////////
+ unsigned char *rgba2 = new unsigned char[size*4];
+ memset(rgba2, 127, size*4);
+ vpImage<vpRGBa> I_copyData((vpRGBa *) rgba2, h, w, true);
+
+ //Delete the array
+ if(rgba2 != NULL) {
+ delete[] rgba2;
+ rgba2 = NULL;
+ }
+
+ filename = vpIoTools::createFilePath(opath, "I_copyData.ppm");
+ /* Save the the current image */
+ vpImageIo::write(I_copyData, filename);
+
+ if(I_copyData.getSize() > 0) {
+ I_copyData[0][0].R = 10;
+ }
+
+ return 0;
+ }
+ catch(vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/image/testCreateSubImage.cpp b/modules/core/test/image/testCreateSubImage.cpp
new file mode 100644
index 0000000..1c0e8d0
--- /dev/null
+++ b/modules/core/test/image/testCreateSubImage.cpp
@@ -0,0 +1,267 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for sub-image extraction.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+
+/*!
+ \example testCreateSubImage.cpp
+
+ \brief Create a sub-image from an image by cropping a rectangular area.
+
+ Read an image from the disk, crop a rectangular area and save the
+ cropped image on the disk.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Read an image from the disk (Klimt.pgm), crop a rectangular area\n\
+and save the cropped image on the disk (Klimt_cropped.pgm).\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_cropped.pgm output image is written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (opt_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ //
+ // Here starts really the test
+ //
+ vpImage<unsigned char> I; // Input image
+ vpImage<unsigned char> C; // Cropped output image
+
+ // Read the input grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+
+ // Specify the cropping area
+ vpRect crop;
+ crop.setLeft(-10.2);
+ crop.setTop(10.0);
+ crop.setWidth(I.getWidth());
+ crop.setHeight(20.0);
+
+ // Create the cropped image
+ vpImageTools::createSubImage(I, crop, C);
+
+ // Write the cropped image on the disk
+ filename = vpIoTools::createFilePath(opath, "Klimt_crop.pgm");
+ std::cout << "Write cropped image: " << filename << std::endl;
+ vpImageIo::write(C, filename) ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/image/testImageBinarise.cpp b/modules/core/test/image/testImageBinarise.cpp
new file mode 100644
index 0000000..a863698
--- /dev/null
+++ b/modules/core/test/image/testImageBinarise.cpp
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for vpImageTools::binarise() function.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/*!
+ \example testImageBinarise.cpp
+
+ \brief Test vpImageTools::binarise() function.
+
+*/
+
+#include <visp3/core/vpImageTools.h>
+
+
+int main()
+{
+ std::cout << "Test vpImageTools::binarise() with different data types." << std::endl;
+
+ unsigned int width = 5, height = 4;
+ unsigned char *uchar_array = new unsigned char[width*height];
+ double *double_array = new double[width*height];
+ vpRGBa *rgba_array = new vpRGBa[width*height];
+ for(unsigned char i = 0; i < width*height; i++) {
+ uchar_array[i] = i;
+ double_array[i] = i;
+ rgba_array[i] = vpRGBa(i, i, i, i);
+ }
+
+ vpImage<unsigned char> I(uchar_array, height, width);
+ vpImage<double> I_double(double_array, height, width);
+ vpImage<vpRGBa> I_rgba(rgba_array, height, width);
+
+ std::cout << "I:" << std::endl;
+ for(unsigned int i = 0; i < I.getHeight(); i++) {
+ for(unsigned int j = 0; j < I.getWidth(); j++) {
+ std::cout << static_cast<unsigned>(I[i][j]) << " ";
+ }
+ std::cout << std::endl;
+ }
+
+ std::cout << "\nI_double:" << std::endl;
+ for(unsigned int i = 0; i < I_double.getHeight(); i++) {
+ for(unsigned int j = 0; j < I_double.getWidth(); j++) {
+ std::cout << I_double[i][j] << " ";
+ }
+ std::cout << std::endl;
+ }
+
+ std::cout << "\nI_rgba:" << std::endl;
+ for(unsigned int i = 0; i < I_rgba.getHeight(); i++) {
+ for(unsigned int j = 0; j < I_rgba.getWidth(); j++) {
+ std::cout << static_cast<unsigned>(I_rgba[i][j].R) << " ; " << static_cast<unsigned>(I_rgba[i][j].G) << " ; "
+ << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned int>(I_rgba[i][j].A) << std::endl;
+ }
+ std::cout << std::endl;
+ }
+
+ vpImageTools::binarise(I, (unsigned char) 5, (unsigned char) 12, (unsigned char) 0, (unsigned char) 127, (unsigned char) 255);
+ vpImageTools::binarise(I_double, 5.0, 12.0, 0.0, 127.0, 255.0);
+ vpImageTools::binarise(I_rgba, vpRGBa(5), vpRGBa(12), vpRGBa(0), vpRGBa(127), vpRGBa(255));
+
+ std::cout << "\nI binarise:" << std::endl;
+ for(unsigned int i = 0; i < I.getHeight(); i++) {
+ for(unsigned int j = 0; j < I.getWidth(); j++) {
+ std::cout << static_cast<unsigned>(I[i][j]) << " ";
+ }
+ std::cout << std::endl;
+ }
+
+ std::cout << "\nI_double binarise:" << std::endl;
+ for(unsigned int i = 0; i < I_double.getHeight(); i++) {
+ for(unsigned int j = 0; j < I_double.getWidth(); j++) {
+ std::cout << I_double[i][j] << " ";
+ }
+ std::cout << std::endl;
+ }
+
+ std::cout << "\nI_rgba binarise:" << std::endl;
+ for(unsigned int i = 0; i < I_rgba.getHeight(); i++) {
+ for(unsigned int j = 0; j < I_rgba.getWidth(); j++) {
+ std::cout << static_cast<unsigned>(I_rgba[i][j].R) << " ; " << static_cast<unsigned>(I_rgba[i][j].G) << " ; "
+ << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned>(I_rgba[i][j].A) << std::endl;
+ }
+ std::cout << std::endl;
+ }
+
+
+ //Check if results are the same between iterate and LUT methods
+ width = 32;
+ height = 8;
+ unsigned char *uchar_array1 = new unsigned char[width*height];
+ unsigned char *uchar_array2 = new unsigned char[width*height];
+ for(unsigned int i = 0; i < 256; i++) {
+ uchar_array1[i] = (unsigned char) i;
+ uchar_array2[i] = (unsigned char) i;
+ }
+
+ vpImage<unsigned char> I_uchar1(uchar_array1, height, width);
+ vpImage<unsigned char> I_uchar2(uchar_array2, height, width);
+
+ unsigned char threshold1 = 50, threshold2 = 200;
+ unsigned char value1 = 4, value2 = 127, value3 = 250;
+ vpImageTools::binarise(I_uchar1, threshold1, threshold2, value1, value2, value3, false);
+ vpImageTools::binarise(I_uchar2, threshold1, threshold2, value1, value2, value3, true);
+
+ for(unsigned int i = 0; i < height; i++) {
+ for(unsigned int j = 0; j < width; j++) {
+ if(I_uchar1[i][j] != I_uchar2[i][j]) {
+ std::cerr << "Results are different between iterate and LUT methods !" << std::endl;
+ return -1;
+ }
+ }
+ }
+
+
+ //Test performance between iterate and LUT methods
+ width = 640;
+ height = 480;
+ unsigned char *uchar_array_perf_lut = new unsigned char[width*height];
+ unsigned char *uchar_array_perf_iterate = new unsigned char[width*height];
+ for(unsigned int i = 0; i < width*height; i++) {
+ uchar_array_perf_lut[i] = (unsigned char) i;
+ uchar_array_perf_iterate[i] = (unsigned char) i;
+ }
+
+ vpImage<unsigned char> I_perf_lut(uchar_array_perf_lut, height, width);
+ vpImage<unsigned char> I_perf_iterate(uchar_array_perf_iterate, height, width);
+
+ unsigned int nbIterations = 100;
+ double t1 = vpTime::measureTimeMs();
+ for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ vpImageTools::binarise(I_perf_iterate, threshold1, threshold2, value1, value2, value3, false);
+ }
+ t1 = vpTime::measureTimeMs() - t1;
+ std::cout << "Iterate: " << t1 << " ms for " << nbIterations << " iterations." << std::endl;
+
+ double t2 = vpTime::measureTimeMs();
+ for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ vpImageTools::binarise(I_perf_lut, threshold1, threshold2, value1, value2, value3, true);
+ }
+ t2 = vpTime::measureTimeMs() - t2;
+ std::cout << "LUT: " << t2 << " ms for " << nbIterations << " iterations." << std::endl;
+
+
+ std::cout << "\ntestImageBinarise ok !" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/image/testImagePoint.cpp b/modules/core/test/image/testImagePoint.cpp
new file mode 100644
index 0000000..d5e8b0d
--- /dev/null
+++ b/modules/core/test/image/testImagePoint.cpp
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for vpImagePoint class.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \example testImagePoint.cpp
+
+ \brief Test vpImagePoint functionalities.
+
+*/
+
+#include <visp3/core/vpImagePoint.h>
+#include <iostream>
+
+int main()
+{
+ vpImagePoint ip1, ip2, ip3;
+
+ ip1.set_u(-11.1);
+ ip1.set_v(10);
+
+ ip2.set_j(-11.1);
+ ip2.set_i(10);
+
+ ip3.set_j(11.10001);
+ ip3.set_i(10.1);
+
+ std::cout << "We define ip1 with coordinates: "
+ << ip1 << std::endl;
+
+
+ std::cout << "We define ip2 with coordinates: "
+ << ip2 << std::endl;
+
+ std::cout << "We define ip3 with coordinates: "
+ << ip3 << std::endl;
+
+ if (ip1 == ip2) {
+ std::cout << "ip1 == ip2" << std::endl;
+ }
+ else {
+ std::cout << "ip1 != ip2 (bad result)" << std::endl;
+ return -1;
+ }
+
+ if (ip1 != ip2) {
+ std::cout << "ip1 != ip2 (bad result)" << std::endl;
+ return -1;
+ }
+ else {
+ std::cout << "ip1 == ip2" << std::endl;
+ }
+
+ if (ip1 == ip3) {
+ std::cout << "ip1 == ip3 (bad result)" << std::endl;
+ return -1;
+ }
+ else {
+ std::cout << "ip1 != ip3" << std::endl;
+ }
+
+ if (ip1 != ip3) {
+ std::cout << "ip1 != ip3" << std::endl;
+ }
+ else {
+ std::cout << "ip1 == ip3 (bad result)" << std::endl;
+ return -1;
+ }
+
+
+ return 0;
+}
diff --git a/modules/core/test/image/testIoPGM.cpp b/modules/core/test/image/testIoPGM.cpp
new file mode 100644
index 0000000..aef19f0
--- /dev/null
+++ b/modules/core/test/image/testIoPGM.cpp
@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read and write PGM images on the disk.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+/*!
+ \example testIoPGM.cpp
+
+ \brief Read and write PGM images on the disk. Also test exceptions.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Read and write PGM images on the disk. Also test exceptions.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_grey.pgm output image is written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ // Create a grey level image
+ vpImage<unsigned char> I ;
+
+ // Load a grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename);
+ // Write the content of the image on the disk
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename) ;
+
+ try {
+ // Try to load a non existing image (test for exceptions)
+ // Load a non existing grey image
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.pgm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
+ }
+
+ try {
+ // Try to write an image to a non existing directory
+ filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.pgm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename) ;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/image/testIoPPM.cpp b/modules/core/test/image/testIoPPM.cpp
new file mode 100644
index 0000000..f99a78f
--- /dev/null
+++ b/modules/core/test/image/testIoPPM.cpp
@@ -0,0 +1,314 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read and write PGM images on the disk.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+/*!
+ \example testIoPPM.cpp
+
+ \brief Read and write PPM images on the disk. Also test exceptions.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Read and write PPM images on the disk. Also test exceptions.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_grey.ppm and Klimt_color.ppm output image\n\
+ are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ /////////////////////////////////////////////////////////////////////
+ // Create a grey level image
+ vpImage<unsigned char> I ;
+
+ // Load a color image from the disk and convert it to a grey level one
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename);
+ // Write the content of the image on the disk
+ filename = vpIoTools::createFilePath(opath, "Klimt_grey.ppm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename) ;
+
+ // Try to load a non existing image (test for exceptions)
+ try
+ {
+ // Load a non existing grey image
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+ }
+ catch(vpImageException e)
+ {
+ vpERROR_TRACE("at main level");
+ std::cout << e << std::endl ;
+ }
+
+ // Try to write an image to a non existing directory
+ try {
+ filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename) ;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
+ }
+
+ /////////////////////////////////////////////////////////////////////
+ // Create a color image
+ vpImage<vpRGBa> Irgba ;
+
+ // Load a color image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(Irgba, filename);
+ // Write the content of the color image on the disk
+ filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(Irgba, filename) ;
+
+ try {
+ // Try to load a non existing image (test for exceptions)
+ // Load a non existing color image
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(Irgba, filename) ;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
+ }
+
+ try {
+ // Try to write a color image to a non existing directory
+ filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(Irgba, filename) ;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/core/test/image/testPerformanceLUT.cpp b/modules/core/test/image/testPerformanceLUT.cpp
new file mode 100644
index 0000000..557e49e
--- /dev/null
+++ b/modules/core/test/image/testPerformanceLUT.cpp
@@ -0,0 +1,351 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test performance between iteration and LUT.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+/*!
+ \example testPerformanceLUT.cpp
+
+ \brief Test performance between iteration and LUT.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test performance between methods to iterate over pixel image.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_grey.pgm output image is written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ Iterate over pixels using raw pointer and adjust the pixel intensities with the formula:
+ new_intensity = old_intensitie * alpha + beta.
+
+ \param I : Input color image.
+ \param alpha : Gain.
+ \param beta: Offset.
+*/
+void iterate_method1(vpImage<vpRGBa> &I, const double alpha, const double beta) {
+ unsigned int size = I.getWidth() * I.getHeight();
+ unsigned char *ptrStart = (unsigned char*) I.bitmap;
+ unsigned char *ptrEnd = ptrStart + size*4;
+ unsigned char *ptrCurrent = ptrStart;
+
+ while(ptrCurrent != ptrEnd) {
+ *ptrCurrent = vpMath::saturate<unsigned char>((*ptrCurrent) * alpha + beta);
+ ++ptrCurrent;
+ }
+}
+
+/*!
+ Iterate over pixels using a double for loop and adjust the pixel intensities with the formula:
+ new_intensity = old_intensitie * alpha + beta.
+
+ \param I : Input color image.
+ \param alpha : Gain.
+ \param beta: Offset.
+*/
+void iterate_method2(vpImage<vpRGBa> &I, const double alpha, const double beta) {
+ for(unsigned int i = 0; i < I.getHeight(); i++) {
+ for(unsigned int j = 0; j < I.getWidth(); j++) {
+ I[i][j].R = vpMath::saturate<unsigned char>(I[i][j].R * alpha + beta);
+ I[i][j].G = vpMath::saturate<unsigned char>(I[i][j].G * alpha + beta);
+ I[i][j].B = vpMath::saturate<unsigned char>(I[i][j].B * alpha + beta);
+ I[i][j].A = vpMath::saturate<unsigned char>(I[i][j].A * alpha + beta);
+ }
+ }
+}
+
+/*!
+ Using a look-up table, adjust the pixel intensities.
+
+ \param I : Input color image.
+ \param lut : Look-up table mapping for each intensity the new corresponding value.
+*/
+void lut_method(vpImage<vpRGBa> &I, const vpRGBa (&lut)[256]) {
+ I.performLut(lut);
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+
+ //
+ // Here starts really the test
+ //
+
+ // Create a grey level image
+ vpImage<vpRGBa> I_iterate1, I_iterate2, I_lut;
+
+ // Load a grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I_iterate1, filename);
+ vpImageIo::read(I_iterate2, filename);
+ vpImageIo::read(I_lut, filename);
+
+ std::cout << "I=" << I_iterate1.getWidth() << "x" << I_iterate1.getHeight() << std::endl;
+
+ double alpha = 1.5, beta = -30.0;
+ unsigned int nbIterations = 10;
+
+ //Iterate method 1
+ double t_iterate1 = vpTime::measureTimeMs();
+ for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ iterate_method1(I_iterate1, alpha, beta);
+ }
+ t_iterate1 = vpTime::measureTimeMs() - t_iterate1;
+ std::cout << "t_iterate1=" << t_iterate1 << " ms ; t_iterate1/" << nbIterations << "="
+ << (t_iterate1/nbIterations) << " ms" << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate1.ppm");
+ vpImageIo::write(I_iterate1, filename);
+
+
+ //Iterate method 2
+ double t_iterate2 = vpTime::measureTimeMs();
+ for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ iterate_method2(I_iterate2, alpha, beta);
+ }
+ t_iterate2 = vpTime::measureTimeMs() - t_iterate2;
+ std::cout << "t_iterate2=" << t_iterate2 << " ms ; t_iterate2/" << nbIterations << "="
+ << (t_iterate2/nbIterations) << " ms" << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate2.ppm");
+ vpImageIo::write(I_iterate2, filename);
+
+
+ //LUT method
+ double t_lut = vpTime::measureTimeMs();
+ for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ //Construct the LUT
+ vpRGBa lut[256];
+ for(unsigned int i = 0; i < 256; i++) {
+ lut[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].A = vpMath::saturate<unsigned char>(alpha * i + beta);
+ }
+
+ lut_method(I_lut, lut);
+ }
+ t_lut = vpTime::measureTimeMs() - t_lut;
+ std::cout << "t_lut=" << t_lut << " ms ; t_lut/" << nbIterations << "="
+ << (t_lut/nbIterations) << " ms" << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_performance_lut.ppm");
+ vpImageIo::write(I_lut, filename);
+ return 0;
+ }
+ catch(vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/image/testReadImage.cpp b/modules/core/test/image/testReadImage.cpp
new file mode 100644
index 0000000..553880f
--- /dev/null
+++ b/modules/core/test/image/testReadImage.cpp
@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read images on the disk.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+
+/*!
+ \example testReadImage.cpp
+
+ \brief Read images on the disk.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:p:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string ppath);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Personal image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string ppath)
+{
+ fprintf(stdout, "\n\
+Read images on the disk.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-p <personal image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm,\n\
+ .ppm, .jpeg and .png images.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -p <personal image path> %s\n\
+ Set personal image path.\n\
+ From this path read the image \"%s\".\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), ppath.c_str(), ppath.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param ppath : Personal image path.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'p': ppath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, ppath); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, ppath); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, ppath);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_ppath;
+ std::string ipath;
+ std::string ppath;
+ std::string filename;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_ppath) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_ppath.empty())
+ ppath = opt_ppath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+
+ //
+ // Here starts really the test
+ //
+
+ /////////////////////////////////////////////////////////////////////
+ // Create a grey level image
+ //vpImage<vpRGBa> I;
+ vpImage<unsigned char> I;
+ vpImage<vpRGBa> Irgb;
+
+ if (opt_ppath.empty())
+ {
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ vpImageIo::read(I,filename);
+ printf("Read ppm ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ vpImageIo::read(I,filename);
+ printf("Read pgm ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
+ vpImageIo::read(I,filename);
+ printf("Read jpeg ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpg");
+ vpImageIo::read(I,filename);
+ printf("Read jpg ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
+ vpImageIo::read(I,filename);
+ printf("Read png ok\n");
+
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ vpImageIo::read(Irgb,filename);
+ printf("Read ppm ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ vpImageIo::read(Irgb,filename);
+ printf("Read pgm ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
+ vpImageIo::read(Irgb,filename);
+ printf("Read jpeg ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpg");
+ vpImageIo::read(Irgb,filename);
+ printf("Read jpg ok\n");
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
+ vpImageIo::read(Irgb,filename);
+ printf("Read png ok\n");
+ }
+ else
+ {
+ filename = opt_ppath;
+ vpImageIo::read(I,filename);
+ vpTRACE("image read without problem");
+ }
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
diff --git a/modules/core/test/image/testUndistortImage.cpp b/modules/core/test/image/testUndistortImage.cpp
new file mode 100644
index 0000000..2c79331
--- /dev/null
+++ b/modules/core/test/image/testUndistortImage.cpp
@@ -0,0 +1,291 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image undistortion.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTime.h>
+#include <stdlib.h>
+/*!
+ \example testUndistortImage.cpp
+
+ \brief Undistort an image.
+
+ Read an image from the disk, undistort it and save the
+ undistorted image on the disk.
+
+ */
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+//#define COLOR
+#define BW
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Read an image from the disk, undistort it \n\
+and save the undistorted image on the disk.\n\
+(Klimt_undistorted.pgm).\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n\
+ ", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+ \n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_undistorted.pgm output image is written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+ */
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Set the default output path
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ opt_opath = "/tmp";
+#elif defined(_WIN32)
+ opt_opath = "C:\\temp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (opt_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ //
+ // Here starts really the test
+ //
+#if defined BW
+ vpImage<unsigned char> I; // Input image
+ vpImage<unsigned char> U; // undistorted output image
+#elif defined COLOR
+ vpImage<vpRGBa> I; // Input image
+ vpImage<vpRGBa> U; // undistorted output image
+#endif
+ vpCameraParameters cam;
+ cam.initPersProjWithDistortion(600,600,192,144,-0.17,0.17);
+ // Read the input grey image from the disk
+#if defined BW
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm") ;
+#elif defined COLOR
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+#endif
+ std::cout << "Read image: " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+
+ std::cout << "Undistortion in process... " << std::endl;
+ vpImageTools::undistort(I, cam, U);
+
+ double begintime = vpTime::measureTimeMs();
+
+ // For the test, to have a significant time measure we repeat the
+ // undistortion 100 times
+ for(unsigned int i=0;i<100;i++)
+ // Create the undistorted image
+ vpImageTools::undistort(I, cam, U);
+
+ double endtime = vpTime::measureTimeMs();
+
+ std::cout<<"Time for 100 undistortion (ms): "<< endtime - begintime
+ << std::endl;
+
+ // Write the undistorted image on the disk
+#if defined BW
+ filename = vpIoTools::path( vpIoTools::createFilePath(opath, "Klimt_undistorted.pgm") );
+#elif defined COLOR
+ filename = vpIoTools::path( vpIoTools::createFilePath(opath, "Klimt_undistorted.ppm") );
+#endif
+ std::cout << "Write undistorted image: " << filename << std::endl;
+ vpImageIo::write(U, filename) ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/math/testArray2D.cpp b/modules/core/test/math/testArray2D.cpp
new file mode 100644
index 0000000..5d84d35
--- /dev/null
+++ b/modules/core/test/math/testArray2D.cpp
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \example testArray2D.cpp
+
+ Test some vpArray2D functionalities.
+*/
+
+#include <cmath>
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpTranslationVector.h>
+
+template<typename Type>
+bool test(const std::string &s, const vpArray2D<Type> &A, const std::vector<Type> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") = \n" << A << std::endl;
+ if(bench.size() != A.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<A.size(); i++) {
+ if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+int main()
+{
+ int err = 1;
+ {
+ // test default constructor
+ vpArray2D<double> A;
+ std::vector<double> bench;
+ if (test("A", A, bench) == false)
+ return err;
+ }
+ {
+ // test copy constructor
+ vpArray2D<double> A(3, 4);
+
+ std::vector<double> bench(12);
+ for(unsigned int i=0; i<3; i++) {
+ for(unsigned int j=0; j<4; j++) {
+ A[i][j] = (double)(i+j);
+ bench[i*4+j] = (double)(i+j);
+ }
+ }
+ if (test("A", A, bench) == false)
+ return err;
+
+ vpArray2D<double> B(A);
+ if (test("B", B, bench) == false)
+ return err;
+ std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() << std::endl;
+ }
+ {
+ // test constructor with initial value
+ vpArray2D<double> A(3, 4, 2.);
+ std::vector<double> bench1(12, 2);
+ if (test("A", A, bench1) == false)
+ return err;
+
+ A.resize(5, 6);
+ std::vector<double> bench2(30, 0);
+ if (test("A", A, bench2) == false)
+ return err;
+
+ A = -2.;
+ std::vector<double> bench3(30, -2);
+ if (test("A", A, bench3) == false)
+ return err;
+ }
+
+ // Test with float
+ {
+ // test default constructor
+ vpArray2D<float> A;
+ std::vector<float> bench;
+ if (test("A", A, bench) == false)
+ return err;
+ }
+ {
+ // test copy constructor
+ vpArray2D<float> A(3, 4);
+
+ std::vector<float> bench(12);
+ for(unsigned int i=0; i<3; i++) {
+ for(unsigned int j=0; j<4; j++) {
+ A[i][j] = (float)(i+j);
+ bench[i*4+j] = (float)(i+j);
+ }
+ }
+ if (test("A", A, bench) == false)
+ return err;
+
+ vpArray2D<float> B(A);
+ if (test("B", B, bench) == false)
+ return err;
+ std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() << std::endl;
+ }
+ {
+ // test constructor with initial value
+ vpArray2D<float> A(3, 4, 2.);
+ std::vector<float> bench1(12, 2);
+ if (test("A", A, bench1) == false)
+ return err;
+
+ A.resize(5, 6);
+ std::vector<float> bench2(30, 0);
+ if (test("A", A, bench2) == false)
+ return err;
+
+ A = -2.;
+ std::vector<float> bench3(30, -2);
+ if (test("A", A, bench3) == false)
+ return err;
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testColVector.cpp b/modules/core/test/math/testColVector.cpp
new file mode 100644
index 0000000..4e44aab
--- /dev/null
+++ b/modules/core/test/math/testColVector.cpp
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \example testColVector.cpp
+
+ Test some vpColVector functionalities.
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+
+
+bool test(const std::string &s, const vpColVector &v, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v.t() << "]^T" << std::endl;
+ if(bench.size() != v.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<v.size(); i++) {
+ if (std::fabs(v[i]-bench[i]) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+
+
+int main()
+{
+ int err = 1;
+
+ {
+ vpColVector v;
+
+ v.resize(4);
+ v = 3;
+ std::vector<double> bench1(4, 3);
+ if (test("v", v, bench1) == false)
+ return err;
+ std::vector<double> bench2(4, 3./6);
+ v.normalize();
+ if (test("v", v, bench2) == false)
+ return err;
+
+ v.resize(5, 1, true);
+ std::vector<double> bench3(5, 0);
+ if (test("v", v, bench3) == false)
+ return err;
+ }
+
+ {
+ vpColVector v(4);
+ std::vector<double> bench1(4);
+ for(unsigned int i=0; i<v.size(); i++) {
+ v[i] = (double)i;
+ bench1[i] = (double)i;
+ }
+ if (test("v", v, bench1) == false)
+ return err;
+
+ vpColVector w;
+ w.init(v, 0, 2);
+ std::vector<double> bench2;
+ bench2.push_back(0);
+ bench2.push_back(1);
+ if (test("w", w, bench2) == false)
+ return err;
+
+ std::vector<double> bench3;
+ bench3.push_back(1);
+ bench3.push_back(2);
+ bench3.push_back(3);
+
+ vpColVector r1;
+ for(size_t i=0; i<4; i++)
+ r1.stack((double)i);
+
+ vpColVector r2 = r1.extract(1, 3);
+ if (test("r2", r2, bench3) == false)
+ return err;
+ }
+ {
+ vpMatrix M(4, 1);
+ std::vector<double> bench(4);
+ for(unsigned int i=0; i<M.getRows(); i++) {
+ M[i][0] = i;
+ bench[i] = i;
+ }
+ if (test("M", M, bench) == false)
+ return err;
+ vpColVector v;
+ v = M;
+ if (test("v", v, bench) == false)
+ return err;
+ vpColVector w(M);
+ if (test("w", w, bench) == false)
+ return err;
+ vpColVector z1(bench);
+ if (test("z1", z1, bench) == false)
+ return err;
+ vpColVector z2 = bench;
+ if (test("z2", z2, bench) == false)
+ return err;
+ }
+ {
+ vpColVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+ std::vector<double> bench1;
+ bench1.push_back(3);
+ bench1.push_back(6);
+ bench1.push_back(9);
+
+ vpColVector w = v * 3;
+ // v is unchanged
+ // w is now equal to : [3 6 9]
+ if (test("w", w, bench1) == false)
+ return err;
+
+ vpColVector x(w);
+ if (test("x", x, bench1) == false)
+ return err;
+
+ std::vector<float> bench2;
+ bench2.push_back(3);
+ bench2.push_back(6);
+ bench2.push_back(9);
+ vpColVector y1(bench2);
+ if (test("y1", y1, bench1) == false)
+ return err;
+ vpColVector y2 = bench2;
+ if (test("y2", y2, bench1) == false)
+ return err;
+ }
+ {
+ vpColVector r1(3, 1);
+ vpColVector r2 = -r1;
+ std::vector<double> bench(3,-1);
+ // v contains [-1 -1 -1]
+ if (test("r2", r2, bench) == false)
+ return err;
+ r2.stack(-2);
+ bench.push_back(-2);
+ if (test("r2", r2, bench) == false)
+ return err;
+ vpColVector r3 = vpColVector::stack(r1, r2);
+ std::vector<double> bench3(7, 1);
+ bench3[3] = bench3[4] = bench3[5] = -1;
+ bench3[6] = -2;
+ if (test("r3", r3, bench3) == false)
+ return err;
+
+ r1.stack(r2);
+ if (test("r1", r1, bench3) == false)
+ return err;
+ }
+ {
+ vpColVector r1(3, 2);
+ vpColVector r2(3, 4);
+ std::cout << "test r1: " << r1 << std::endl;
+ std::cout << "test r2: " << r2 << std::endl;
+ vpColVector r = r1 + r2;
+ std::cout << "test r1+r2: " << r1+r2 << std::endl;
+ std::cout << "test r: " << r << std::endl;
+ std::vector<double> bench(3, 6);
+ if (test("r", r, bench) == false)
+ return err;
+ r1 += r2;
+ if (test("r1", r1, bench) == false)
+ return err;
+ }
+ {
+ vpColVector r1(3, 2);
+ vpColVector r2(3, 4);
+ vpColVector r = r1 - r2;
+ std::vector<double> bench(3, -2);
+ if (test("r", r, bench) == false)
+ return err;
+ r1 -= r2;
+ if (test("r1", r1, bench) == false)
+ return err;
+ }
+ {
+ vpColVector r(5, 1);
+ r.clear();
+ r.resize(5);
+ r = 5;
+ std::vector<double> bench(5, 5);
+ if (test("r", r, bench) == false)
+ return err;
+ }
+ {
+ // Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ vpColVector r(10);
+ r[0] = 8.1472;
+ r[1] = 9.0579;
+ r[2] = 1.2699;
+ r[3] = 9.1338;
+ r[4] = 6.3236;
+ r[5] = 0.9754;
+ r[6] = 2.7850;
+ r[7] = 5.4688;
+ r[8] = 9.5751;
+ r[9] = 9.6489;
+
+ std::cout << "** Test mean" << std::endl;
+ double res = vpColVector::mean(r);
+ if(!vpMath::equal(res, 6.2386, 0.001)) {
+ std::cout << "Test fails: bad mean " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test stdev" << std::endl;
+ res = vpColVector::stdev(r);
+ if(!vpMath::equal(res, 3.2810, 0.001)) {
+ std::cout << "Test fails: bad stdev " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test stdev(bessel)" << std::endl;
+ res = vpColVector::stdev(r, true);
+ if(!vpMath::equal(res, 3.4585, 0.001)) {
+ std::cout << "Test fails: bad stdev(bessel) " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test median" << std::endl;
+ res = vpColVector::median(r);
+ if(!vpMath::equal(res, 7.2354, 0.001)) {
+ std::cout << "Test fails: bad median " << res << std::endl;
+ return err;
+ }
+
+ // Test median with odd number of elements
+ std::cout << "** Test median (odd)" << std::endl;
+ r.stack(1.5761);
+ res = vpColVector::median(r);
+ if(!vpMath::equal(res, 6.3236, 0.001)) {
+ std::cout << "Test fails: bad median (odd) " << res << std::endl;
+ return err;
+ }
+ std::cout << "r: [" << r << "]^T" << std::endl;
+ r.print(std::cout, 8, "r");
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testKalmanAcceleration.cpp b/modules/core/test/math/testKalmanAcceleration.cpp
new file mode 100644
index 0000000..1385029
--- /dev/null
+++ b/modules/core/test/math/testKalmanAcceleration.cpp
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests some vpLinearKalmanFilterInstantiation functionalities.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testKalmanAcceleration.cpp
+
+ \brief Test some vpLinearKalmanFilterInstantiation functionalities
+ with constant acceleration state model.
+*/
+
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+#include <iostream>
+#include <fstream>
+
+int
+main()
+{
+ try {
+ unsigned int nsignal = 1; // Number of signal to filter
+ unsigned int niter = 100;
+
+ std::string filename = "/tmp/log.dat";
+ std::ofstream flog(filename.c_str());
+
+ vpLinearKalmanFilterInstantiation kalman;
+
+ vpLinearKalmanFilterInstantiation::vpStateModel model;
+ model = vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel;
+ kalman.setStateModel(model);
+
+ unsigned int size_state_vector = kalman.getStateSize()*nsignal;
+ unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
+
+ vpColVector sigma_measure(size_measure_vector);
+ for (unsigned int signal=0; signal < nsignal; signal ++)
+ sigma_measure = 0.0001;
+ vpColVector sigma_state(size_state_vector);
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ sigma_state[3*signal] = 0.; // not used
+ sigma_state[3*signal+1] = 0.000001;
+ sigma_state[3*signal+2] = 0.000001;
+ }
+
+ vpColVector velocity_measure(size_measure_vector);
+
+ double rho = 0.9; // correlation
+ double dt = 0.2; // sampling period
+
+ for (unsigned int signal=0; signal < nsignal; signal ++)
+ velocity_measure[signal] = 3+2*signal;
+
+ kalman.verbose(false);
+ kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
+
+
+ for (unsigned int iter=0; iter <= niter; iter++) {
+ std::cout << "-------- iter " << iter << " ------------" << std::endl;
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ velocity_measure[signal] = 3+2*signal
+ + 0.3*sin(vpMath::rad(360./niter*iter));
+ }
+ std::cout << "measure : " << velocity_measure.t() << std::endl;
+
+ flog << velocity_measure.t();
+
+ // kalman.prediction();
+ kalman.filter(velocity_measure);
+ flog << kalman.Xest.t();
+ flog << kalman.Xpre.t();
+
+ std::cout << "Xest: " << kalman.Xest.t() << std::endl;
+ std::cout << "Xpre: " << kalman.Xpre.t() << std::endl;
+
+ flog << std::endl;
+ }
+
+ flog.close();
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 0;
+ }
+}
diff --git a/modules/core/test/math/testKalmanVelocity.cpp b/modules/core/test/math/testKalmanVelocity.cpp
new file mode 100644
index 0000000..b4bae5a
--- /dev/null
+++ b/modules/core/test/math/testKalmanVelocity.cpp
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests some vpLinearKalmanFilterInstantiation functionalities.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testKalmanVelocity.cpp
+
+ \brief Test some vpLinearKalmanFilterInstantiation functionalities
+ with constant velocity state model.
+*/
+
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+#include <iostream>
+#include <fstream>
+
+typedef enum {
+ Position, // Considered measures are the succesive positions of the target
+ Velocity // Considered measures are the succesive velocities of the target
+} vpMeasureType;
+
+int
+main()
+{
+ try {
+ unsigned int nsignal = 2; // Number of signal to filter
+ unsigned int niter = 200;
+ unsigned int size_state_vector = 2*nsignal;
+ unsigned int size_measure_vector = 1*nsignal;
+ //vpMeasureType measure_t = Velocity;
+ vpMeasureType measure_t = Position;
+
+ std::string filename = "/tmp/log.dat";
+ std::ofstream flog(filename.c_str());
+
+ vpLinearKalmanFilterInstantiation kalman;
+
+ vpColVector sigma_measure(size_measure_vector);
+ for (unsigned int signal=0; signal < nsignal; signal ++)
+ sigma_measure = 0.000001;
+ vpColVector sigma_state(size_state_vector);
+
+ switch (measure_t) {
+ case Velocity:
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ sigma_state[2*signal] = 0.; // not used
+ sigma_state[2*signal+1] = 0.000001;
+ }
+ break;
+ case Position:
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ sigma_state[2*signal] = 0.000001;
+ sigma_state[2*signal+1] = 0; // not used
+ }
+ break;
+ }
+
+ vpColVector measure(size_measure_vector);
+
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ measure[signal] = 3+2*signal;
+ }
+
+ kalman.verbose(true);
+
+ vpLinearKalmanFilterInstantiation::vpStateModel model;
+ double dt = 0.04; // Sampling period
+ double rho = 0.5;
+ double dummy = 0; // non used parameter
+ switch (measure_t) {
+ case Velocity:
+ model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
+ kalman.setStateModel(model);
+ kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
+ break;
+ case Position:
+ model = vpLinearKalmanFilterInstantiation::stateConstVel_MeasurePos;
+ kalman.setStateModel(model);
+ kalman.initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
+ break;
+ }
+
+ for (unsigned int iter=0; iter <= niter; iter++) {
+ std::cout << "-------- iter " << iter << " ------------" << std::endl;
+ for (unsigned int signal=0; signal < nsignal; signal ++) {
+ measure[signal] = 3+2*signal + 0.3*sin(vpMath::rad(360./niter*iter));
+ }
+ std::cout << "measure : " << measure.t() << std::endl;
+
+ flog << measure.t();
+
+ // kalman.prediction();
+ kalman.filter(measure);
+ flog << kalman.Xest.t() << std::endl;
+
+ std::cout << "Xest: " << kalman.Xest.t() << std::endl;
+ }
+
+ flog.close();
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/math/testMath.cpp b/modules/core/test/math/testMath.cpp
new file mode 100644
index 0000000..fc3ecb7
--- /dev/null
+++ b/modules/core/test/math/testMath.cpp
@@ -0,0 +1,534 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \example testMath.cpp
+
+ Test some vpMath functionalities.
+*/
+
+#include <iostream>
+#include <limits>
+#include <cfloat>
+
+#include <visp3/core/vpMath.h>
+
+#if defined _MSC_VER && _MSC_VER >= 1200
+ #pragma warning( disable: 4723 )
+
+// 4723 : potential divide by 0
+#endif
+
+#ifdef WIN32
+ #ifndef NAN
+ //https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
+ //http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
+ static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
+ #define NAN (*(const float *) __nan)
+ #endif
+#endif
+
+int main() {
+ //Test isNaN
+ if(vpMath::isNaN(0.0)) {
+ std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) << " / should be false" << std::endl;
+ return -1;
+ }
+
+ double num = 1.0, den = 0.0;
+ if(vpMath::isNaN(num/den)) {
+ std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num/den) << " / should be false" << std::endl;
+ return -1;
+ }
+
+ if(!vpMath::isNaN(NAN)) {
+ std::cerr << "Fail: IsNaN(NAN)=" << vpMath::isNaN(NAN) << " / should be true" << std::endl;
+ return -1;
+ }
+
+ num = 0.0;
+ if(!vpMath::isNaN(num/den)) {
+ std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num/den) << " / should be true" << std::endl;
+ return -1;
+ }
+
+ if(!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
+ std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
+ << " / should be true" << std::endl;
+ return -1;
+ }
+
+ if(!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
+ std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
+ << " / should be true" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::isNaN(std::numeric_limits<double>::infinity())) {
+ std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<double>::infinity())
+ << " / should be false" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
+ std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0/std::numeric_limits<double>::epsilon())
+ << " / should be false" << std::endl;
+ return -1;
+ }
+
+ if(!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
+ std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0/std::numeric_limits<double>::epsilon())
+ << " / should be true" << std::endl;
+ return -1;
+ }
+
+ float a = 0.0f, b = 0.0f;
+ if(!vpMath::isNaN(a/b)) {
+ std::cerr << "Fail: IsNaN(0.0f/0.0f)=" << vpMath::isNaN(a/b) << " / should be true" << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::isNaN is Ok !" << std::endl;
+
+
+ //Test isInf
+#if !defined(VISP_HAVE_FUNC__FINITE)
+ //Disable this test if using _finite as (!_finite(NAN)) returns true whereas isinf(NAN) returns false
+ if(vpMath::isInf(NAN)) {
+ std::cerr << "Fail: vpMath::isInf(NAN)=" << vpMath::isInf(NAN) << " / should be false" << std::endl;
+ return -1;
+ }
+#endif
+
+ if(!vpMath::isInf(1.0/a)) {
+ std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0/a) << " / should be true" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::isInf(0.0)) {
+ std::cerr << "Fail: vpMath::isInf(0.0)=" << vpMath::isInf(0.0) << " / should be false" << std::endl;
+ return -1;
+ }
+
+ if(!vpMath::isInf(exp(800))) {
+ std::cerr << "Fail: vpMath::isInf(exp(800))=" << vpMath::isInf(exp(800)) << " / should be true" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::isInf(DBL_MIN/2.0)) {
+ std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN/2.0) << " / should be false" << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::isInf is Ok !" << std::endl;
+
+
+ //Test round
+ if(vpMath::round(2.3) != 2) {
+ std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) << " / should be 2" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(3.8) != 4) {
+ std::cerr << "Fail: vpMath::round(3.8)=" << vpMath::round(3.8) << " / should be 4" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(5.5) != 6) {
+ std::cerr << "Fail: vpMath::round(5.5)=" << vpMath::round(5.5) << " / should be 6" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(-2.3) != -2) {
+ std::cerr << "Fail: vpMath::round(-2.3)=" << vpMath::round(-2.3) << " / should be -2" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(-3.8) != -4) {
+ std::cerr << "Fail: vpMath::round(-3.8)=" << vpMath::round(-3.8) << " / should be -4" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(-5.5) != -6) {
+ std::cerr << "Fail: vpMath::round(-5.5)=" << vpMath::round(-5.5) << " / should be -6" << std::endl;
+ return -1;
+ }
+
+ if(vpMath::round(0.0) != 0) {
+ std::cerr << "Fail: vpMath::round(0.0)=" << vpMath::round(0.0) << " / should be 0" << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::round is Ok !" << std::endl;
+
+
+ //Test saturate functions
+ //unsigned char
+ char char_value = -127;
+ unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
+ if(uchar_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ unsigned short ushort_value = 60000;
+ uchar_value = vpMath::saturate<unsigned char>(ushort_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ int int_value = 70000;
+ uchar_value = vpMath::saturate<unsigned char>(int_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = -70000;
+ uchar_value = vpMath::saturate<unsigned char>(int_value);
+ if(uchar_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ short short_value = 30000;
+ uchar_value = vpMath::saturate<unsigned char>(short_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ short_value = -30000;
+ uchar_value = vpMath::saturate<unsigned char>(short_value);
+ if(uchar_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ unsigned int uint_value = 10000;
+ uchar_value = vpMath::saturate<unsigned char>(uint_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ float float_value = 10000.1f;
+ uchar_value = vpMath::saturate<unsigned char>(float_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = -10000.1f;
+ uchar_value = vpMath::saturate<unsigned char>(float_value);
+ if(uchar_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ double double_value = 10000.1;
+ uchar_value = vpMath::saturate<unsigned char>(double_value);
+ if(uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ double_value = -10000.1;
+ uchar_value = vpMath::saturate<unsigned char>(double_value);
+ if(uchar_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value << " / should be 0" << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::saturate<unsigned char>() is Ok !" << std::endl;
+
+
+ //char
+ uchar_value = 255;
+ char_value = vpMath::saturate<char>(uchar_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ ushort_value = 60000;
+ char_value = vpMath::saturate<char>(ushort_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = 70000;
+ char_value = vpMath::saturate<char>(int_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = -70000;
+ char_value = vpMath::saturate<char>(int_value);
+ if(char_value != SCHAR_MIN) {
+ std::cerr << "Fail: vpMath::saturate<char>(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
+ return -1;
+ }
+
+ short_value = 30000;
+ char_value = vpMath::saturate<char>(short_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ short_value = -30000;
+ char_value = vpMath::saturate<char>(short_value);
+ if(char_value != SCHAR_MIN) {
+ std::cerr << "Fail: vpMath::saturate<char>(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
+ return -1;
+ }
+
+ uint_value = 10000;
+ char_value = vpMath::saturate<char>(uint_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = 10000.1f;
+ char_value = vpMath::saturate<char>(float_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = -10000.1f;
+ char_value = vpMath::saturate<char>(float_value);
+ if(char_value != SCHAR_MIN) {
+ std::cerr << "Fail: vpMath::saturate<char>(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
+ return -1;
+ }
+
+ double_value = 10000.1;
+ char_value = vpMath::saturate<char>(double_value);
+ if(char_value != SCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<char>(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
+ return -1;
+ }
+
+ double_value = -10000.1;
+ char_value = vpMath::saturate<char>(double_value);
+ if(char_value != SCHAR_MIN) {
+ std::cerr << "Fail: vpMath::saturate<char>(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::saturate<char>() is Ok !" << std::endl;
+
+
+ //unsigned short
+ char_value = -127;
+ ushort_value = vpMath::saturate<unsigned short>(char_value);
+ if(ushort_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(127)=" << ushort_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ short_value = -30000;
+ ushort_value = vpMath::saturate<unsigned short>(short_value);
+ if(ushort_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ int_value = 70000;
+ ushort_value = vpMath::saturate<unsigned short>(int_value);
+ if(ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = -70000;
+ ushort_value = vpMath::saturate<unsigned short>(int_value);
+ if(ushort_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ uint_value = 70000;
+ ushort_value = vpMath::saturate<unsigned short>(uint_value);
+ if(ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = 70000.1f;
+ ushort_value = vpMath::saturate<unsigned short>(float_value);
+ if(ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = -10000.1f;
+ ushort_value = vpMath::saturate<unsigned short>(float_value);
+ if(ushort_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value << " / should be 0" << std::endl;
+ return -1;
+ }
+
+ double_value = 70000.1;
+ ushort_value = vpMath::saturate<unsigned short>(double_value);
+ if(ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ return -1;
+ }
+
+ double_value = -10000.1;
+ ushort_value = vpMath::saturate<unsigned short>(double_value);
+ if(ushort_value != 0) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value << " / should be 0" << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::saturate<unsigned short>() is Ok !" << std::endl;
+
+
+ //short
+ ushort_value = 60000;
+ short_value = vpMath::saturate<short>(ushort_value);
+ if(short_value != SHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<short>(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = 70000;
+ short_value = vpMath::saturate<short>(int_value);
+ if(short_value != SHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
+ return -1;
+ }
+
+ int_value = -70000;
+ short_value = vpMath::saturate<short>(int_value);
+ if(short_value != SHRT_MIN) {
+ std::cerr << "Fail: vpMath::saturate<short>(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl;
+ return -1;
+ }
+
+ uint_value = 70000;
+ short_value = vpMath::saturate<short>(uint_value);
+ if(short_value != SHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = 70000.1f;
+ short_value = vpMath::saturate<short>(float_value);
+ if(short_value != SHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<short>(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl;
+ return -1;
+ }
+
+ float_value = -70000.1f;
+ short_value = vpMath::saturate<short>(float_value);
+ if(short_value != SHRT_MIN) {
+ std::cerr << "Fail: vpMath::saturate<short>(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl;
+ return -1;
+ }
+
+ double_value = 70000.1;
+ short_value = vpMath::saturate<short>(double_value);
+ if(short_value != SHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl;
+ return -1;
+ }
+
+ double_value = -70000.1;
+ short_value = vpMath::saturate<short>(double_value);
+ if(short_value != SHRT_MIN) {
+ std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::saturate<short>() is Ok !" << std::endl;
+
+
+ //Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ std::vector<double> vectorOfDoubles(10);
+ vectorOfDoubles[0] = 8.1472;
+ vectorOfDoubles[1] = 9.0579;
+ vectorOfDoubles[2] = 1.2699;
+ vectorOfDoubles[3] = 9.1338;
+ vectorOfDoubles[4] = 6.3236;
+ vectorOfDoubles[5] = 0.9754;
+ vectorOfDoubles[6] = 2.7850;
+ vectorOfDoubles[7] = 5.4688;
+ vectorOfDoubles[8] = 9.5751;
+ vectorOfDoubles[9] = 9.6489;
+
+
+ double res = vpMath::getMean(vectorOfDoubles);
+ if(!vpMath::equal(res, 6.2386, 0.001)) {
+ std::cerr << "Problem with vpMath::getMean()=" << res << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::getMean() is Ok !" << std::endl;
+
+ res = vpMath::getStdev(vectorOfDoubles);
+ if(!vpMath::equal(res, 3.2810, 0.001)) {
+ std::cerr << "Problem with vpMath::getStdev()=" << res << std::endl;
+ return -1;
+ }
+
+ res = vpMath::getStdev(vectorOfDoubles, true);
+ if(!vpMath::equal(res, 3.4585, 0.001)) {
+ std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::getStdev() is Ok !" << std::endl;
+
+ res = vpMath::getMedian(vectorOfDoubles);
+ if(!vpMath::equal(res, 7.2354, 0.001)) {
+ std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
+ return -1;
+ }
+
+ //Test median with odd number of elements
+ vectorOfDoubles.push_back(1.5761);
+ res = vpMath::getMedian(vectorOfDoubles);
+ if(!vpMath::equal(res, 6.3236, 0.001)) {
+ std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
+ return -1;
+ }
+ std::cout << "vpMath::getMedian() is Ok !" << std::endl;
+
+
+ std::cout << "OK !" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testMatrix.cpp b/modules/core/test/math/testMatrix.cpp
new file mode 100644
index 0000000..42679cd
--- /dev/null
+++ b/modules/core/test/math/testMatrix.cpp
@@ -0,0 +1,280 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpMatrix functionalities.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testMatrix.cpp
+
+ Test some vpMatrix functionalities.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpGEMM.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
+ if(bench.size() != M.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<M.size(); i++) {
+ if (std::fabs(M.data[i]-bench[i]) > std::fabs(M.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+int
+main()
+{
+ try {
+ int err = 1;
+ {
+ vpColVector c(6, 1);
+ vpRowVector r(6, 1);
+ std::vector<double> bench(6, 1);
+ vpMatrix M1(c);
+ if (test("M1", M1, bench) == false)
+ return err;
+ vpMatrix M2(r);
+ if (test("M2", M2, bench) == false)
+ return err;
+ }
+ {
+ vpMatrix M(4,5);
+ int val = 0;
+ for(unsigned int i=0; i<M.getRows(); i++) {
+ for(unsigned int j=0; j<M.getCols(); j++) {
+ M[i][j] = val++;
+ }
+ }
+ std::cout <<"M ";
+ M.print (std::cout, 4);
+
+ vpMatrix N;
+ N.init(M, 0, 1, 2, 3);
+ std::cout <<"N ";
+ N.print (std::cout, 4);
+
+ if (vpMatrix::saveMatrix("matrix.mat", M, false, "My 4-by-5 matrix"))
+ std::cout << "Matrix saved in matrix.mat file" << std::endl;
+ else
+ return err;
+
+ vpMatrix M1;
+ if (vpMatrix::loadMatrix("matrix.mat", M1, false))
+ std::cout << "Matrix loaded from matrix.mat file: \n" << M1 << std::endl;
+ else
+ return err;
+
+ if (vpMatrix::saveMatrixYAML("matrix.yml", M, "My 4-by-5 matrix"))
+ std::cout << "Matrix saved in matrix.yml file" << std::endl;
+ else
+ return err;
+
+ vpMatrix M2;
+ if (vpMatrix::loadMatrixYAML("matrix.yml", M2))
+ std::cout << "Matrix loaded from matrix.yml file: \n" << M2 << std::endl;
+ else
+ return err;
+ }
+ {
+ vpRotationMatrix R(vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
+ std::cout << "R: \n" << R << std::endl;
+ vpMatrix M1(R);
+ std::cout << "M1: \n" << M1 << std::endl;
+ vpMatrix M2(M1);
+ std::cout << "M2: \n" << M2 << std::endl;
+ vpMatrix M3 = R;
+ std::cout << "M3: \n" << M3 << std::endl;
+ vpMatrix M4 = M1;
+ std::cout << "M4: \n" << M4 << std::endl;
+ }
+ {
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST PRETTY PRINT---" << std::endl;
+ std::cout << "------------------------" << std::endl;
+ vpMatrix M ;
+ M.eye(4);
+
+ std::cout << "call std::cout << M;" << std::endl;
+ std::cout << M << std::endl;
+
+ std::cout << "call M.print (std::cout, 4);" << std::endl;
+ M.print (std::cout, 4);
+
+ std::cout << "------------------------" << std::endl;
+ M.resize(3,3) ;
+ M.eye(3);
+ M[1][0]=1.235;
+ M[1][1]=12.345;
+ M[1][2]=.12345;
+ std::cout << "call std::cout << M;" << std::endl;
+ std::cout << M;
+ std::cout << "call M.print (std::cout, 6);" << std::endl;
+ M.print (std::cout, 6);
+ std::cout << std::endl;
+
+ std::cout << "------------------------" << std::endl;
+ M[0][0]=-1.235;
+ M[1][0]=-12.235;
+
+ std::cout << "call std::cout << M;" << std::endl;
+ std::cout << M << std::endl;
+
+ std::cout << "call M.print (std::cout, 10);" << std::endl;
+ M.print (std::cout, 10);
+ std::cout << std::endl;
+
+ std::cout << "call M.print (std::cout, 2);" << std::endl;
+ M.print (std::cout, 2);
+ std::cout << std::endl;
+
+ std::cout << "------------------------" << std::endl;
+ M.resize(3,3) ;
+ M.eye(3);
+ M[0][2]=-0.0000000876;
+ std::cout << "call std::cout << M;" << std::endl;
+ std::cout << M << std::endl;
+
+ std::cout << "call M.print (std::cout, 4);" << std::endl;
+ M.print (std::cout, 4);
+ std::cout << std::endl;
+ std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
+ M.print (std::cout, 10, "M");
+ std::cout << std::endl;
+ std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
+ M.print (std::cout, 20, "M");
+ std::cout << std::endl;
+
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST RESIZE --------" << std::endl;
+ std::cout << "------------------------" << std::endl;
+ std::cout << "5x5" << std::endl;
+ M.resize(5,5,false);
+ std::cout << M << std::endl;
+ std::cout << "3x2" << std::endl;
+ M.resize(3,2,false);
+ std::cout << M << std::endl;
+ std::cout << "2x2" << std::endl;
+ M.resize(2,2,false);
+ std::cout << M << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ vpVelocityTwistMatrix vMe;
+ vpMatrix A(1,6),B;
+
+ A=1.0;
+ //vMe=1.0;
+ B=A*vMe;
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
+ std::cout << "------------------------" << std::endl;
+ vpRowVector r(3);
+ r[0] = 2;
+ r[1] = 3;
+ r[2] = 4;
+
+ vpColVector c(3);
+ c[0] = 1;
+ c[1] = 2;
+ c[2] = -1;
+
+ double rc = r * c;
+
+ r.print(std::cout, 2, "r");
+ c.print(std::cout, 2, "c");
+ std::cout << "r * c = " << rc << std::endl;
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
+ std::cout << "------------------------" << std::endl;
+ M.resize(3,3) ;
+ M.eye(3);
+
+ M[1][0] = 1.5;
+ M[2][0] = 2.3;
+
+ vpRowVector rM = r * M;
+
+ r.print(std::cout, 2, "r");
+ M.print(std::cout, 10, "M");
+ std::cout << "r * M = " << rM << std::endl;
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpGEMM " << std::endl;
+ std::cout << "------------------------" << std::endl;
+ M.resize(3,3) ;
+ M.eye(3);
+ vpMatrix N(3, 3);
+ N[0][0] = 2;
+ N[1][0] = 1.2;
+ N[1][2] = 0.6;
+ N[2][2] = 0.25;
+
+ vpMatrix C(3, 3);
+ C.eye(3);
+
+ vpMatrix D;
+
+ //realise the operation D = 2 * M^T * N + 3 C
+ vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
+ std::cout << D << std::endl;
+ return 0;
+ }
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/core/test/math/testMatrixException.cpp b/modules/core/test/math/testMatrixException.cpp
new file mode 100644
index 0000000..fee8ca7
--- /dev/null
+++ b/modules/core/test/math/testMatrixException.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test matrix exceptions.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testMatrixException.cpp
+ \brief Tests matrix exception
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix M ;
+ vpMatrix M1(2,3) ;
+ vpMatrix M2(3,3) ;
+ vpMatrix M3(2,2) ;
+
+ std::cout << "** test matrix exception during multiplication" << std::endl;
+
+ try {
+ M = M1*M3 ;
+ }
+ catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+
+ std::cout << "** test matrix exception during addition" << std::endl;
+
+ try {
+ M = M1+M3 ;
+ }
+ catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+}
diff --git a/modules/core/test/math/testMatrixInverse.cpp b/modules/core/test/math/testMatrixInverse.cpp
new file mode 100644
index 0000000..36d839e
--- /dev/null
+++ b/modules/core/test/math/testMatrixInverse.cpp
@@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test various inversions.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+
+/*!
+ \example testMatrixInverse.cpp
+ \brief Test various matrix inversions.
+*/
+
+
+
+#include <visp3/core/vpTime.h>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/io/vpParseArgv.h>
+#include <vector>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fstream>
+#include <iostream>
+#include <cmath>
+// List of allowed command line options
+#define GETOPTARGS "cdn:i:pf:R:C:vh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv,
+ unsigned int& nb_matrices, unsigned int& nb_iterations,
+ bool& use_plot_file, std::string& plotfile,
+ unsigned int& nbrows, unsigned int& nbcols, bool& verbose);
+void writeTime(const char *name, double time);
+vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test matrix inversions\n\
+using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
+Outputs a comparison of these methods.\n\
+\n\
+SYNOPSIS\n\
+ %s [-n <number of matrices>] [-f <plot filename>]\n\
+ [-R <number of rows>] [-C <number of columns>]\n\
+ [-i <number of iterations>] [-p] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -n <number of matrices> \n\
+ Number of matrices inverted during each test loop.\n\
+\n\
+ -i <number of iterations> \n\
+ Number of iterations of the test.\n\
+\n\
+ -f <plot filename> \n\
+ Set output path for plot output.\n\
+ The plot logs the times of \n\
+ the different inversion methods: \n\
+ QR,LU,Cholesky and Pseudo-inverse.\n\
+\n\
+ -R <number of rows>\n\
+ Number of rows of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -C <number of columns>\n\
+ Number of colums of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -p \n\
+ Plot into filename in the gnuplot format. \n\
+ If this option is used, tests results will be logged \n\
+ into a filename specified with -f.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+
+}
+/*!
+
+ Set the program options.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv,
+ unsigned int& nb_matrices, unsigned int& nb_iterations,
+ bool& use_plot_file, std::string& plotfile,
+ unsigned int& nbrows, unsigned int& nbcols, bool& verbose)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'h': usage(argv[0], NULL); return false; break;
+ case 'n':
+ nb_matrices = (unsigned int)atoi(optarg_);
+ break;
+ case 'i':
+ nb_iterations = (unsigned int)atoi(optarg_);
+ break;
+ case 'f':
+ plotfile = optarg_;
+ use_plot_file = true;
+ break;
+ case 'p':
+ use_plot_file = true;
+ break;
+ case 'R':
+ nbrows = (unsigned int)atoi(optarg_);
+ break;
+ case 'C':
+ nbcols = (unsigned int)atoi(optarg_);
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ // add default options -c -d
+ case 'c':
+ break;
+ case 'd':
+ break;
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+void writeTime(const char *name, double time)
+{
+ std::cout << name << " time=" << time << " ms." << std::endl;
+}
+
+vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols)
+{
+ vpMatrix A;
+ A.resize(nbrows,nbcols);
+
+ for (unsigned int i=0 ; i < A.getRows() ; i++)
+ for (unsigned int j=0 ; j < A.getCols() ; j++)
+ A[i][j] = (double)rand()/(double)RAND_MAX;
+ return A;
+}
+
+
+int
+main(int argc, const char ** argv)
+{
+#ifdef VISP_HAVE_LAPACK_C
+ try {
+ unsigned int nb_matrices=1000;
+ unsigned int nb_iterations=10;
+ unsigned int nb_rows = 6;
+ unsigned int nb_cols = 6;
+ bool verbose = false;
+ std::string plotfile("plot.txt");
+ bool use_plot_file=false;
+ std::ofstream of;
+
+ double t, qr_time, lu_time,pi_time,chol_time;
+ // Read the command line options
+ if (getOptions(argc, argv, nb_matrices,nb_iterations,use_plot_file,plotfile,nb_rows,nb_cols,verbose) == false) {
+ exit (-1);
+ }
+
+ if(use_plot_file){
+ of.open(plotfile.c_str());
+ }
+
+ for(unsigned int iter=0;iter<nb_iterations;iter++){
+ std::vector<vpMatrix> benchQR;
+ std::vector<vpMatrix> benchLU;
+ std::vector<vpMatrix> benchCholesky;
+ std::vector<vpMatrix> benchPseudoInverse;
+ if(verbose)
+ std::cout << "********* generating matrices for iteration " << iter << "." << std::endl;
+ for(unsigned int i=0;i<nb_matrices;i++){
+ vpMatrix cur;
+ double det=0.;
+ //don't put singular matrices in the benchmark
+ for(cur=makeRandomMatrix(nb_rows,nb_cols);std::abs(det=cur.AtA().det())<.01;cur = makeRandomMatrix(nb_rows,nb_cols))
+ if(verbose){
+ std::cout << "Generated random matrix A*tA=" << std::endl << cur.AtA() << std::endl;
+ std::cout << "generated random matrix not invertibleL: det="<<det<< ". Retrying..." << std::endl;
+ }
+ benchCholesky.push_back(cur);
+ benchQR.push_back(cur);
+ benchLU.push_back(cur);
+ benchPseudoInverse.push_back(cur);
+ }
+
+ if(verbose)
+ std::cout << "\t Inverting " << benchCholesky[0].AtA().getRows() << "x" << benchCholesky[0].AtA().getCols() << " matrix using cholesky decomposition." << std::endl;
+ t = vpTime::measureTimeMs() ;
+ for(unsigned int i=0;i<nb_matrices;i++){
+ benchCholesky[i]=benchCholesky[i].AtA().inverseByCholesky()*benchCholesky[i].transpose();
+ }
+ chol_time = vpTime::measureTimeMs() - t ;
+
+ if(verbose)
+ std::cout << "\t Inverting " << benchLU[0].AtA().getRows() << "x" << benchLU[0].AtA().getCols() << " matrix using LU decomposition." << std::endl;
+ t = vpTime::measureTimeMs() ;
+ for(unsigned int i=0;i<nb_matrices;i++)
+ benchLU[i] = benchLU[i].AtA().inverseByLU()*benchLU[i].transpose();
+ lu_time = vpTime::measureTimeMs() -t ;
+
+ if(verbose)
+ std::cout << "\t Inverting " << benchQR[0].AtA().getRows() << "x" << benchQR[0].AtA().getCols() << " matrix using QR decomposition." << std::endl;
+ t = vpTime::measureTimeMs() ;
+ for(unsigned int i=0;i<nb_matrices;i++){
+ benchQR[i]=benchQR[i].AtA().inverseByQR()*benchQR[i].transpose();
+ }
+ qr_time = vpTime::measureTimeMs() - t ;
+
+ if(verbose)
+ std::cout << "\t Inverting " << benchPseudoInverse[0].AtA().getRows() << "x" << benchPseudoInverse[0].AtA().getCols() << " matrix while computing pseudo-inverse." << std::endl;
+ t = vpTime::measureTimeMs() ;
+ for(unsigned int i=0;i<nb_matrices;i++){
+ benchPseudoInverse[i]=benchPseudoInverse[i].pseudoInverse();
+ }
+ pi_time = vpTime::measureTimeMs() - t ;
+
+ double avg_err_lu_qr=0.;
+ double avg_err_lu_pi=0.;
+ double avg_err_lu_chol=0.;
+ double avg_err_qr_pi=0.;
+ double avg_err_qr_chol=0.;
+ double avg_err_pi_chol=0.;
+
+ for(unsigned int i=0;i<nb_matrices;i++){
+ avg_err_lu_qr+= (benchQR[i]-benchLU[i]).euclideanNorm();
+ avg_err_lu_pi+= (benchPseudoInverse[i]-benchLU[i]).euclideanNorm();
+ avg_err_qr_pi+= (benchPseudoInverse[i]-benchQR[i]).euclideanNorm();
+ avg_err_qr_chol+= (benchCholesky[i]-benchQR[i]).euclideanNorm();
+ avg_err_lu_chol+= (benchCholesky[i]-benchLU[i]).euclideanNorm();
+ avg_err_pi_chol+= (benchCholesky[i]-benchPseudoInverse[i]).euclideanNorm();
+ }
+
+ avg_err_lu_qr/=nb_matrices;
+ avg_err_lu_pi/=nb_matrices;
+ avg_err_qr_pi/=nb_matrices;
+
+ if(use_plot_file){
+ of << iter << "\t" << lu_time << "\t" << qr_time << "\t" << pi_time << "\t" << chol_time << "\t" << avg_err_lu_qr << "\t" << avg_err_qr_pi << "\t" << avg_err_lu_pi << "\t" << avg_err_qr_chol << "\t" << avg_err_lu_chol << "\t" << avg_err_pi_chol << std::endl;
+ }
+ if(verbose || !use_plot_file){
+ writeTime("LU",lu_time);
+ writeTime("QR",qr_time);
+ writeTime("Pseudo-inverse",pi_time);
+ writeTime("Cholesky",chol_time);
+ }
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "You don't have lapack installed" << std::endl;
+ return 0;
+#endif
+}
+
diff --git a/modules/core/test/math/testPoseVector.cpp b/modules/core/test/math/testPoseVector.cpp
new file mode 100644
index 0000000..8c532c5
--- /dev/null
+++ b/modules/core/test/math/testPoseVector.cpp
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \example testPoseVector.cpp
+
+ Test some vpPoseVector functionalities.
+*/
+
+#include <cmath>
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpPoseVector.h>
+
+bool test(const std::string &s, const vpArray2D<double> &A, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A << std::endl;
+ if(bench.size() != A.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<A.size(); i++) {
+ if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+int main()
+{
+ int err = 1;
+ {
+ vpPoseVector p;
+ std::vector<double> bench(6,0);
+ if (test("p", p, bench) == false)
+ return err;
+ p[0] = bench[0] = 0.1;
+ p[1] = bench[1] = 0.2;
+ p[2] = bench[2] = 0.3;
+ p[3] = bench[3] = vpMath::rad(10);
+ p[4] = bench[4] = vpMath::rad(20);
+ p[5] = bench[5] = vpMath::rad(30);
+
+ if (test("p", p, bench) == false)
+ return err;
+
+ vpPoseVector p1(p[0], p[1], p[2], p[3], p[4], p[5]);
+ if (test("p1", p1, bench) == false)
+ return err;
+ vpPoseVector p2(p1);
+ if (test("p2", p2, bench) == false)
+ return err;
+ vpPoseVector p3 = p1;
+ if (test("p3", p3, bench) == false)
+ return err;
+ vpPoseVector p4;
+ p4.set(p[0], p[1], p[2], p[3], p[4], p[5]);
+ if (test("p4", p4, bench) == false)
+ return err;
+
+ vpTranslationVector t(p[0], p[1], p[2]);
+ vpThetaUVector tu(p[3], p[4], p[5]);
+ vpPoseVector p5(t, tu);
+ if (test("p5", p5, bench) == false)
+ return err;
+ vpPoseVector p6;
+ p6.buildFrom(t, tu);
+ if (test("p6", p6, bench) == false)
+ return err;
+
+ vpHomogeneousMatrix M(t, tu);
+ vpPoseVector p7(M);
+ if (test("p7", p7, bench) == false)
+ return err;
+ vpPoseVector p8;
+ p8.buildFrom(M);
+ if (test("p8", p8, bench) == false)
+ return err;
+
+ vpRotationMatrix R(tu);
+ vpPoseVector p9(t, R);
+ if (test("p9", p9, bench) == false)
+ return err;
+ vpPoseVector p10;
+ p10.buildFrom(t, R);
+ if (test("p10", p10, bench) == false)
+ return err;
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testQuaternion.cpp b/modules/core/test/math/testQuaternion.cpp
new file mode 100644
index 0000000..7dfd70f
--- /dev/null
+++ b/modules/core/test/math/testQuaternion.cpp
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests quaternion operations.
+ *
+ * Author:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file testQuaternion.cpp
+ \brief Tests quaternion operations.
+*/
+
+
+#include <limits>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpException.h>
+
+
+int main()
+{
+ try {
+ //Test addition of two quaternions
+ vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
+ vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
+ vpQuaternionVector q3 = q1 + q2;
+ std::cout << "q3=" << q3 << std::endl;
+ if(!vpMath::equal(q3.x(), 2.6, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.y(), 0.4, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.z(), -3.0, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.w(), 4.0, std::numeric_limits<double>::epsilon())) {
+ throw vpException(vpException::fatalError, "Problem with addition of two quaternions !");
+ }
+
+
+ //Test subtraction of two quaternions
+ vpQuaternionVector q4 = q3-q1;
+ std::cout << "q4=" << q4 << std::endl;
+ if(!vpMath::equal(q4.x(), q2.x(), std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q4.y(), q2.y(), std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q4.z(), q2.z(), std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q4.w(), q2.w(), std::numeric_limits<double>::epsilon()*1e4)) {
+ throw vpException(vpException::fatalError, "Problem with subtraction of two quaternions !");
+ }
+
+
+ //Test multiplication of two quaternions
+ //https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
+ vpQuaternionVector q5(3.0, 4.0, 3.0, -sin(M_PI));
+ vpQuaternionVector q6(3.9, -1.0, -3.0, 4.0);
+ vpQuaternionVector q7 = q5 * q6;
+ std::cout << "q7=" << q7 << std::endl;
+ if(!vpMath::equal(q7.x(), 3.0, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7.y(), 36.7, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7.z(), -6.6, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7.w(), 1.3, std::numeric_limits<double>::epsilon()*1e4)) {
+ throw vpException(vpException::fatalError, "Problem with multiplication of two quaternions !");
+ }
+
+
+ //Test quaternion conjugate
+ vpQuaternionVector q7_conj = q7.conjugate();
+ std::cout << "q7_conj=" << q7_conj << std::endl;
+ if(!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits<double>::epsilon()*1e4) ||
+ !vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits<double>::epsilon()*1e4)) {
+ throw vpException(vpException::fatalError, "Problem with quaternion conjugate !");
+ }
+
+
+ //Test quaternion inverse
+ vpQuaternionVector q7_inv = q7.inverse();
+ std::cout << "q7_inv=" << q7_inv << std::endl;
+ if(!vpMath::equal(q7_inv.x(), -0.00214111, 0.000001) ||
+ !vpMath::equal(q7_inv.y(), -0.026193, 0.000001) ||
+ !vpMath::equal(q7_inv.z(), 0.00471045, 0.000001) ||
+ !vpMath::equal(q7_inv.w(), 0.000927816, 0.000001)) {
+ throw vpException(vpException::fatalError, "Problem with quaternion inverse !");
+ }
+
+
+ //Test quaternion norm
+ double q7_norm = q7.magnitude();
+ std::cout << "q7_norm=" << q7_norm << std::endl;
+ if(!vpMath::equal(q7_norm, 37.4318, 0.0001)) {
+ throw vpException(vpException::fatalError, "Problem with quaternion magnitude !");
+ }
+
+
+ //Test quaternion normalization
+ q7.normalize();
+ std::cout << "q7_unit=" << q7 << std::endl;
+ if(!vpMath::equal(q7.x(), 0.0801457, 0.00001) ||
+ !vpMath::equal(q7.y(), 0.98045, 0.00001) ||
+ !vpMath::equal(q7.z(), -0.176321, 0.00001) ||
+ !vpMath::equal(q7.w(), 0.0347298, 0.00001)) {
+ throw vpException(vpException::fatalError, "Problem with quaternion normalization !");
+ }
+
+
+ //Test copy constructor
+ vpQuaternionVector q_copy1 = vpQuaternionVector(0, 0, 1, 1);
+ std::cout << "q_copy1=" << q_copy1 << std::endl;
+ vpQuaternionVector q_copy2 = q_copy1;
+ q_copy1.set(1, 0, 1, 10);
+ std::cout << "q_copy1 after set=" << q_copy1 << std::endl;
+ std::cout << "q_copy2=" << q_copy2 << std::endl;
+
+
+ //Test assignment operator
+ vpQuaternionVector q_copy3(10, 10, 10, 10);
+ q_copy3 = q_copy1;
+ std::cout << "q_copy3=" << q_copy3 << std::endl;
+
+
+ std::cout << "vpQuaternion operations are ok !" << std::endl;
+ return 0;
+ }
+ catch(vpException &e) {
+ std::cerr << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/math/testRobust.cpp b/modules/core/test/math/testRobust.cpp
new file mode 100644
index 0000000..68fa671
--- /dev/null
+++ b/modules/core/test/math/testRobust.cpp
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpMath functionalities.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobust.cpp
+
+ Test some vpMath functionalities.
+*/
+
+
+#include <visp3/core/vpRobust.h>
+#include <string>
+#include <fstream>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+// List of allowed command line options
+#define GETOPTARGS "cdho:"
+
+void usage(const char *name, const char *badparam, std::string ofilename);
+bool getOptions(int argc, const char **argv, std::string &ofilename);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ofilename : Output filename.
+*/
+void usage(const char *name, const char *badparam, std::string ofilename)
+{
+ fprintf(stdout, "\n\
+Test some vpMath functionalities. Compute weights and print\n\
+them in an output file.\n\
+\n\
+Using gnuplot the content of the output file can be printed by:\n\
+set style data line\n\
+set ylabel \"weight\"\n\
+set yr [0:1.19]\n\
+set xlabel \"Normalized residuals\"\n\
+plot '%s' title \"Tukey Estimator\" lw 2, 1 title \"Least-Squares\" lw 2\n\
+\n\
+\n\
+SYNOPSIS\n\
+ %s [-o <output filename>] [-h]\n", ofilename.c_str(), name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -o <output filename> %s\n\
+ Name and path of the file containing computed \n\
+ weights.\n\
+\n\
+ -h\n\
+ Print the help.\n",
+ ofilename.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ofilename : Output filename.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ofilename)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'o': ofilename = optarg_; break;
+ case 'h': usage(argv[0], NULL, ofilename); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+ default:
+ usage(argv[0], optarg_, ofilename);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ofilename);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string ofilename;
+ std::string username;
+
+ // Set the default output filename
+#if defined(_WIN32)
+ ofilename = "C:/temp";
+#else
+ ofilename = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Append to the output filename string, the login name of the user
+ ofilename = ofilename + "/" + username;
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(ofilename) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(ofilename);
+ }
+ catch (...) {
+ usage(argv[0], NULL, ofilename);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << ofilename << std::endl;
+ std::cerr << " Check your -o " << ofilename << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ // Append to the output filename string, the name of the file
+ ofilename = ofilename + "/w.dat";
+
+ // Read the command line options
+ if (getOptions(argc, argv, ofilename) == false) {
+ exit (-1);
+ }
+
+ double sig = 1 ;
+
+ double w ;
+ std::ofstream f;
+ std::cout << "Create file: " << ofilename << std::endl;
+ f.open(ofilename.c_str());
+ if (f.fail()) {
+ usage(argv[0], NULL, ofilename);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create the file: " << ofilename << std::endl;
+ std::cerr << " Check your -o " << ofilename << " option " << std::endl;
+ exit(-1);
+
+ }
+ double x = -10 ;
+ while (x<10)
+ {
+ if (fabs(x/sig)<=(4.6851))
+ {
+ w = vpMath::sqr(1-vpMath::sqr(x/(sig*4.6851)));
+ }
+ else
+ {
+ w = 0;
+ }
+ f << x <<" "<<w <<std::endl ;
+ x+= 0.01 ;
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp
new file mode 100644
index 0000000..eb0e0a8
--- /dev/null
+++ b/modules/core/test/math/testRotation.cpp
@@ -0,0 +1,359 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests transformation from various representations of rotation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file testRotation.cpp
+ \brief Tests transformation within various representations of rotation.
+*/
+
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpQuaternionVector.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <cassert>
+#include <limits>
+
+static unsigned int cpt = 0;
+
+bool test(const std::string &s, const vpArray2D<double> &v, const std::vector<double> &bench)
+{
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
+ if(bench.size() != v.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<v.size(); i++) {
+ if (std::fabs(v.data[i]-bench[i]) > std::fabs(v.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool test(const std::string &s, const vpRotationVector &v, const double &bench)
+{
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
+ for (unsigned int i=0; i<v.size(); i++) {
+ if (std::fabs(v[i]-bench) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+int main()
+{
+ try {
+ int err = 1;
+ {
+ vpThetaUVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ std::vector<double> bench1(3, vpMath::rad(10));
+ if (test("r1", r1, bench1) == false)
+ return err;
+
+ vpThetaUVector r2 = r1;
+ if (test("r2", r2, bench1) == false)
+ return err;
+
+ if (test("r2", r2, vpMath::rad(10)) == false)
+ return err;
+
+ vpThetaUVector r3;
+ r3 = vpMath::rad(10);
+ if (test("r3", r3, bench1) == false)
+ return err;
+
+ std::cout << "** Test " << ++cpt << std::endl;
+ for(unsigned int i=0; i<r3.size(); i++) {
+ if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ vpColVector r4 = 0.5 * r1;
+ std::vector<double> bench2(3, vpMath::rad(5));
+ if (test("r4", r4, bench2) == false)
+ return err;
+
+ vpThetaUVector r5(r3);
+ if (test("r5", r5, bench1) == false)
+ return err;
+ }
+ {
+ vpRxyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ std::vector<double> bench1(3, vpMath::rad(10));
+ if (test("r1", r1, bench1) == false)
+ return err;
+
+ vpRxyzVector r2 = r1;
+ if (test("r2", r2, bench1) == false)
+ return err;
+
+ if (test("r2", r2, vpMath::rad(10)) == false)
+ return err;
+
+ vpRxyzVector r3;
+ r3 = vpMath::rad(10);
+ if (test("r3", r3, bench1) == false)
+ return err;
+
+ std::cout << "** Test " << ++cpt << std::endl;
+ for(unsigned int i=0; i<r3.size(); i++) {
+ if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ vpColVector r4 = 0.5 * r1;
+ std::vector<double> bench2(3, vpMath::rad(5));
+ if (test("r4", r4, bench2) == false)
+ return err;
+
+ vpRxyzVector r5(r3);
+ if (test("r5", r5, bench1) == false)
+ return err;
+ }
+ {
+ vpRzyxVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ std::vector<double> bench1(3, vpMath::rad(10));
+ if (test("r1", r1, bench1) == false)
+ return err;
+
+ vpRzyxVector r2 = r1;
+ if (test("r2", r2, bench1) == false)
+ return err;
+
+ if (test("r2", r2, vpMath::rad(10)) == false)
+ return err;
+
+ vpRzyxVector r3;
+ r3 = vpMath::rad(10);
+ if (test("r3", r3, bench1) == false)
+ return err;
+
+ std::cout << "** Test " << ++cpt << std::endl;
+ for(unsigned int i=0; i<r3.size(); i++) {
+ if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ vpColVector r4 = 0.5 * r1;
+ std::vector<double> bench2(3, vpMath::rad(5));
+ if (test("r4", r4, bench2) == false)
+ return err;
+
+ vpRzyxVector r5(r3);
+ if (test("r5", r5, bench1) == false)
+ return err;
+ }
+ {
+ vpRzyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ std::vector<double> bench1(3, vpMath::rad(10));
+ if (test("r1", r1, bench1) == false)
+ return err;
+
+ vpRzyzVector r2 = r1;
+ if (test("r2", r2, bench1) == false)
+ return err;
+
+ if (test("r2", r2, vpMath::rad(10)) == false)
+ return err;
+
+ vpRzyzVector r3;
+ r3 = vpMath::rad(10);
+ if (test("r3", r3, bench1) == false)
+ return err;
+
+ std::cout << "** Test " << ++cpt << std::endl;
+ for(unsigned int i=0; i<r3.size(); i++) {
+ if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ vpColVector r4 = 0.5 * r1;
+ std::vector<double> bench2(3, vpMath::rad(5));
+ if (test("r4", r4, bench2) == false)
+ return err;
+
+ vpRzyzVector r5(r3);
+ if (test("r5", r5, bench1) == false)
+ return err;
+ }
+ {
+ vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ std::vector<double> bench1(4, vpMath::rad(10));
+ if (test("r1", r1, bench1) == false)
+ return err;
+
+ vpQuaternionVector r2 = r1;
+ if (test("r2", r2, bench1) == false)
+ return err;
+
+ if (test("r2", r2, vpMath::rad(10)) == false)
+ return err;
+
+ vpQuaternionVector r3;
+ r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
+ if (test("r3", r3, bench1) == false)
+ return err;
+
+ std::cout << "** Test " << ++cpt << std::endl;
+ for(unsigned int i=0; i<r3.size(); i++) {
+ if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ vpColVector r4 = 0.5 * r1;
+ std::vector<double> bench2(4, vpMath::rad(5));
+ if (test("r4", r4, bench2) == false)
+ return err;
+
+ vpQuaternionVector r5(r3);
+ if (test("r5", r5, bench1) == false)
+ return err;
+ }
+ {
+ vpRotationMatrix R;
+ for(int i=-10;i<10;i++){
+ for(int j=-10;j<10;j++){
+ vpThetaUVector tu(vpMath::rad(90+i), vpMath::rad(170+j), vpMath::rad(45)) ;
+ tu.buildFrom(vpRotationMatrix(tu)); //put some coherence into rotation convention
+
+ std::cout << "Initialization " <<std::endl ;
+
+ double theta;
+ vpColVector u;
+ tu.extract(theta, u);
+
+ std::cout << "theta=" << vpMath::deg(theta) << std::endl ;
+ std::cout << "u=" << u << std::endl ;
+
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
+ R.buildFrom(tu) ;
+
+ std::cout << "Matrix R" ;
+ if (R.isARotationMatrix()==1) std::cout <<" is a rotation matrix " << std::endl ;
+ else std::cout <<" is not a rotation matrix " << std::endl ;
+
+ std::cout << R << std::endl ;
+
+ std::cout << "From vpRotationMatrix to vpQuaternionVector " << std::endl ;
+ vpQuaternionVector q(R);
+ std::cout << q <<std::endl ;
+
+ R.buildFrom(q);
+ std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl ;
+
+ std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl ;
+ vpRxyzVector RxyzBuildFromR(R) ;
+ std::cout << RxyzBuildFromR <<std::endl ;
+
+
+ std::cout << "From vpRxyzVector to vpThetaUVector " << std::endl ;
+ std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl ;
+ std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl ;
+
+
+ vpThetaUVector tuBuildFromEu ;
+ tuBuildFromEu.buildFrom(R) ;
+
+ std::cout << std::endl ;
+ std::cout << "result : should equivalent to the first one " << std::endl ;
+
+
+ double theta2;
+ vpColVector u2;
+
+ tuBuildFromEu.extract(theta2, u2);
+ std::cout << "theta=" << vpMath::deg(theta2) << std::endl ;
+ std::cout << "u=" << u2 << std::endl ;
+
+ assert(vpMath::abs(theta2-theta)<std::numeric_limits<double>::epsilon()*1e10);
+ assert(vpMath::abs(u[0]-u2[0])<std::numeric_limits<double>::epsilon()*1e10);
+ assert(vpMath::abs(u[1]-u2[1])<std::numeric_limits<double>::epsilon()*1e10);
+ assert(vpMath::abs(u[2]-u2[2])<std::numeric_limits<double>::epsilon()*1e10);
+ }
+ vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
+ std::cout << "Initialization vpRzyzVector " <<std::endl ;
+ std::cout << rzyz << std::endl ;
+ std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl ;
+ R.buildFrom(rzyz) ;
+ std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl ;
+ vpRzyzVector rzyz_final ;
+ rzyz_final.buildFrom(R) ;
+ std::cout << rzyz_final << std::endl ;
+
+ vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
+ std::cout << "Initialization vpRzyxVector " <<std::endl ;
+ std::cout << rzyx << std::endl ;
+ std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl ;
+ R.buildFrom(rzyx) ;
+ std::cout << R << std::endl ;
+ std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl ;
+ vpRzyxVector rzyx_final ;
+ rzyx_final.buildFrom(R) ;
+ std::cout << rzyx_final << std::endl ;
+ }
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/math/testRowVector.cpp b/modules/core/test/math/testRowVector.cpp
new file mode 100644
index 0000000..29810ff
--- /dev/null
+++ b/modules/core/test/math/testRowVector.cpp
@@ -0,0 +1,286 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRowVector.cpp
+
+ Test some vpRowVector functionalities.
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRowVector.h>
+
+
+bool test(const std::string &s, const vpRowVector &v, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
+ if(bench.size() != v.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<v.size(); i++) {
+ if (std::fabs(v[i]-bench[i]) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+int main()
+{
+ int err = 1;
+
+ {
+ vpRowVector v;
+
+ v.resize(4);
+ v = 3;
+ std::vector<double> bench1(4, 3);
+ if (test("v", v, bench1) == false)
+ return err;
+ std::vector<double> bench2(4, 3./6);
+ v.normalize();
+ if (test("v", v, bench2) == false)
+ return err;
+
+ v.resize(1, 5, true);
+ std::vector<double> bench3(5, 0);
+ if (test("v", v, bench3) == false)
+ return err;
+ }
+
+ {
+ vpRowVector v(4);
+ std::vector<double> bench1(4);
+ for(unsigned int i=0; i<v.size(); i++) {
+ v[i] = (double)i;
+ bench1[i] = (double)i;
+ }
+ if (test("v", v, bench1) == false)
+ return err;
+
+ vpRowVector w;
+ w.init(v, 0, 2);
+ std::vector<double> bench2;
+ bench2.push_back(0);
+ bench2.push_back(1);
+ if (test("w", w, bench2) == false)
+ return err;
+
+ std::vector<double> bench3;
+ bench3.push_back(1);
+ bench3.push_back(2);
+ bench3.push_back(3);
+
+ vpRowVector r1;
+ for(size_t i=0; i<4; i++)
+ r1.stack((double)i);
+
+ vpRowVector r2 = r1.extract(1, 3);
+ if (test("r2", r2, bench3) == false)
+ return err;
+ }
+ {
+ vpMatrix M(1, 4);
+ std::vector<double> bench(4);
+ for(unsigned int i=0; i<M.getCols(); i++) {
+ M[0][i] = i;
+ bench[i] = i;
+ }
+ if (test("M", M, bench) == false)
+ return err;
+ vpRowVector v;
+ v = M;
+ if (test("v", v, bench) == false)
+ return err;
+ vpRowVector w(M);
+ if (test("w", w, bench) == false)
+ return err;
+ vpRowVector z1(bench);
+ if (test("z1", z1, bench) == false)
+ return err;
+ vpRowVector z2 = bench;
+ if (test("z2", z2, bench) == false)
+ return err;
+ }
+ {
+ vpRowVector v(3);
+ v[0] = 1;
+ v[1] = 2;
+ v[2] = 3;
+ std::vector<double> bench1;
+ bench1.push_back(3);
+ bench1.push_back(6);
+ bench1.push_back(9);
+
+ vpRowVector w = v * 3;
+ // v is unchanged
+ // w is now equal to : [3 6 9]
+ if (test("w", w, bench1) == false)
+ return err;
+
+ vpRowVector x(w);
+ if (test("x", x, bench1) == false)
+ return err;
+
+ std::vector<float> bench2;
+ bench2.push_back(3);
+ bench2.push_back(6);
+ bench2.push_back(9);
+ vpRowVector y1(bench2);
+ if (test("y1", y1, bench1) == false)
+ return err;
+ vpRowVector y2 = bench2;
+ if (test("y2", y2, bench1) == false)
+ return err;
+ }
+ {
+ vpRowVector r1(3, 1);
+ vpRowVector r2 = -r1;
+ std::vector<double> bench(3,-1);
+ // v contains [-1 -1 -1]
+ if (test("r2", r2, bench) == false)
+ return err;
+ r2.stack(-2);
+ bench.push_back(-2);
+ if (test("r2", r2, bench) == false)
+ return err;
+ vpRowVector r3 = vpRowVector::stack(r1, r2);
+ std::vector<double> bench3(7, 1);
+ bench3[3] = bench3[4] = bench3[5] = -1;
+ bench3[6] = -2;
+ if (test("r3", r3, bench3) == false)
+ return err;
+
+ r1.stack(r2);
+ if (test("r1", r1, bench3) == false)
+ return err;
+ }
+ {
+ vpRowVector r1(3, 2);
+ vpRowVector r2(3, 4);
+ vpRowVector r = r1 + r2;
+ std::vector<double> bench(3, 6);
+ if (test("r", r, bench) == false)
+ return err;
+ r1 += r2;
+ if (test("r1", r1, bench) == false)
+ return err;
+ }
+ {
+ vpRowVector r1(3, 2);
+ vpRowVector r2(3, 4);
+ vpRowVector r = r1 - r2;
+ std::vector<double> bench(3, -2);
+ if (test("r", r, bench) == false)
+ return err;
+ r1 -= r2;
+ if (test("r1", r1, bench) == false)
+ return err;
+ }
+ {
+ vpRowVector r(5, 1);
+ r.clear();
+ r.resize(5);
+ r = 5;
+ std::vector<double> bench(5, 5);
+ if (test("r", r, bench) == false)
+ return err;
+ }
+ {
+ // Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ vpRowVector r(10);
+ r[0] = 8.1472;
+ r[1] = 9.0579;
+ r[2] = 1.2699;
+ r[3] = 9.1338;
+ r[4] = 6.3236;
+ r[5] = 0.9754;
+ r[6] = 2.7850;
+ r[7] = 5.4688;
+ r[8] = 9.5751;
+ r[9] = 9.6489;
+
+ std::cout << "** Test mean" << std::endl;
+ double res = vpRowVector::mean(r);
+ if(!vpMath::equal(res, 6.2386, 0.001)) {
+ std::cout << "Test fails: bad mean " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test stdev" << std::endl;
+ res = vpRowVector::stdev(r);
+ if(!vpMath::equal(res, 3.2810, 0.001)) {
+ std::cout << "Test fails: bad stdev " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test stdev(bessel)" << std::endl;
+ res = vpRowVector::stdev(r, true);
+ if(!vpMath::equal(res, 3.4585, 0.001)) {
+ std::cout << "Test fails: bad stdev(bessel) " << res << std::endl;
+ return err;
+ }
+
+ std::cout << "** Test median" << std::endl;
+ res = vpRowVector::median(r);
+ if(!vpMath::equal(res, 7.2354, 0.001)) {
+ std::cout << "Test fails: bad median " << res << std::endl;
+ return err;
+ }
+
+ // Test median with odd number of elements
+ std::cout << "** Test median (odd)" << std::endl;
+ r.stack(1.5761);
+ res = vpRowVector::median(r);
+ if(!vpMath::equal(res, 6.3236, 0.001)) {
+ std::cout << "Test fails: bad median (odd) " << res << std::endl;
+ return err;
+ }
+ std::cout << "r: [" << r << "]" << std::endl;
+ r.print(std::cout, 8, "r");
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp
new file mode 100644
index 0000000..503856b
--- /dev/null
+++ b/modules/core/test/math/testSvd.cpp
@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test various svd decompositions.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \example testSvd.cpp
+ \brief Test various svd decompositions.
+*/
+
+#include <visp3/core/vpTime.h>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <vector>
+#include <algorithm>
+#include <stdlib.h>
+#include <stdio.h>
+
+bool testSvdOpenCvGSLCoherence(double epsilon);
+#ifdef VISP_HAVE_GSL
+bool testRandom(double epsilon);
+#endif
+
+
+#define abs(x) ((x) < 0 ? - (x) : (x))
+#ifdef VISP_HAVE_GSL
+
+bool testRandom(double epsilon)
+{
+ vpMatrix L0(6,6);
+ vpMatrix L1(6,6);
+
+ for (unsigned int i=0 ; i < L0.getRows() ; i++)
+ for (unsigned int j=0 ; j < L0.getCols() ; j++)
+ L1[i][j] = L0[i][j] = (double)rand()/(double)RAND_MAX;
+
+ vpColVector W0(L0.getCols()) ;
+ vpMatrix V0(L0.getCols(), L0.getCols()) ;
+ vpColVector W1(L1.getCols()) ;
+ vpMatrix V1(L1.getCols(), L1.getCols()) ;
+
+ L0.svdNr(W0,V0);
+ L1.svdGsl(W1,V1);
+
+ vpColVector _W0 = vpColVector::sort(W0);
+ vpColVector _W1 = vpColVector::sort(W1);
+
+ vpColVector diff = _W0-_W1;
+ double error=-1.0;
+
+ for(unsigned int i=0;i<6;i++)
+ error=std::max(abs(diff[i]),error);
+
+ return error<epsilon;
+
+}
+
+#endif
+
+bool testSvdOpenCvGSLCoherence(double epsilon)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && defined (VISP_HAVE_GSL) // Require opencv >= 2.1.1
+ vpMatrix A;
+ vpMatrix vA;
+ vpColVector wA;
+ vpMatrix B;
+ vpMatrix vB;
+ vpColVector wB;
+ A.resize(6,6);
+ B.resize(6,6);
+ vA.resize(6,6);
+ vB.resize(6,6);
+ wA.resize(6);
+ wB.resize(6);
+
+ for (unsigned int i=0 ; i < A.getRows() ; i++)
+ for (unsigned int j=0 ; j < A.getCols() ; j++)
+ B[i][j] = A[i][j] = (double)rand()/(double)RAND_MAX;
+
+ A.svdOpenCV(wA,vA);
+ B.svdGsl(wB,vB);
+
+ bool error=false;
+ for (unsigned int i=0 ; i < A.getRows() ; i++){
+ error = error | (abs(wA[i]-wB[i])>epsilon);
+ }
+
+ return !error;
+#else
+ (void)epsilon;
+ return true;
+#endif
+}
+
+int
+main()
+{
+ try {
+ vpMatrix L(60000,6), Ls ;
+ for (unsigned int i=0 ; i < L.getRows() ; i++)
+ for (unsigned int j=0 ; j < L.getCols() ; j++)
+ L[i][j] = 2*i+j + cos((double)(i+j))+((double)(i)) ;
+ // std::cout << L << std::endl ;
+ Ls = L ;
+ std::cout << "--------------------------------------"<<std::endl ;
+
+ vpColVector W(L.getCols()) ;
+ vpMatrix V(L.getCols(), L.getCols()) ;
+
+ double t = vpTime::measureTimeMs() ;
+ L.svdNr(W,V) ;
+ t = vpTime::measureTimeMs() -t ;
+
+ std::cout <<"svdNr Numerical recipes \n time " <<t << std::endl;
+ std::cout << W.t() ;
+ std::cout << "--------------------------------------"<<std::endl ;
+
+
+#ifdef VISP_HAVE_GSL
+ L = Ls ;
+ t = vpTime::measureTimeMs() ;
+ L.svdGsl(W,V) ;
+ t = vpTime::measureTimeMs() -t ;
+ std::cout <<"svdGsl_mod \n time " <<t << std::endl;
+ std::cout << W.t() ;
+
+ std::cout << "--------------------------------------"<<std::endl ;
+ std::cout << "TESTING RANDOM MATRICES:" ;
+
+ bool ret = true;
+ for(unsigned int i=0;i<2000;i++)
+ ret = ret & testRandom(0.00001);
+ if(ret)
+ std:: cout << "Success"<< std:: endl;
+ else
+ std:: cout << "Fail"<< std:: endl;
+
+ std::cout << "--------------------------------------"<<std::endl ;
+#endif
+
+ std::cout << "--------------------------------------"<<std::endl ;
+ std::cout << "TESTING OPENCV-GSL coherence:" ;
+
+ bool ret2 = true;
+ for(unsigned int i=0;i<1;i++)
+ ret2 = ret2 & testSvdOpenCvGSLCoherence(0.00001);
+ if(ret2)
+ std:: cout << "Success"<< std:: endl;
+ else
+ std:: cout << "Fail"<< std:: endl;
+
+ std::cout << "--------------------------------------"<<std::endl ;
+
+ L = Ls ;
+ t = vpTime::measureTimeMs() ;
+ L.svdFlake(W,V) ;
+ t = vpTime::measureTimeMs() -t ;
+ std::cout <<"svdFlake\n time " <<t << std::endl;
+ std::cout << W.t() ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/core/test/math/testTranslationVector.cpp b/modules/core/test/math/testTranslationVector.cpp
new file mode 100644
index 0000000..bb314d4
--- /dev/null
+++ b/modules/core/test/math/testTranslationVector.cpp
@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test some vpColVector functionalities.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \example testTranslationVector.cpp
+
+ Test some vpTranslationVector functionalities.
+*/
+
+#include <cmath>
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpTranslationVector.h>
+
+bool test(const std::string &s, const vpArray2D<double> &A, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A << std::endl;
+ if(bench.size() != A.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<A.size(); i++) {
+ if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+int main()
+{
+ int err = 1;
+ {
+ vpTranslationVector t;
+ std::vector<double> bench(3,0);
+ if (test("t", t, bench) == false)
+ return err;
+ }
+ {
+ vpTranslationVector t1(1, 2, 3);
+ std::vector<double> bench(3);
+ bench[0] = 1;
+ bench[1] = 2;
+ bench[2] = 3;
+ if (test("t1", t1, bench) == false)
+ return err;
+
+ vpTranslationVector t2(4, 5, 6);
+ bench[0] = 4;
+ bench[1] = 5;
+ bench[2] = 6;
+ if (test("t2", t2, bench) == false)
+ return err;
+
+ vpTranslationVector t3 = t1 + t2;
+ bench[0] = 5;
+ bench[1] = 7;
+ bench[2] = 9;
+ if (test("t3", t3, bench) == false)
+ return err;
+
+ vpMatrix skew = t3.skew();
+ std::vector<double> bench1(9, 0);
+ bench1[1] = -9;
+ bench1[2] = 7;
+ bench1[3] = 9;
+ bench1[5] = -5;
+ bench1[6] = -7;
+ bench1[7] = 5;
+ if (test("skew", skew, bench1) == false)
+ return err;
+
+ vpTranslationVector t4;
+ t4.set(-1, -2, -3);
+ bench[0] = -1;
+ bench[1] = -2;
+ bench[2] = -3;
+ if (test("t4", t4, bench) == false)
+ return err;
+
+ vpTranslationVector t5 = t4 * 2;
+ bench[0] = -2;
+ bench[1] = -4;
+ bench[2] = -6;
+ if (test("t5", t5, bench) == false)
+ return err;
+
+ vpTranslationVector t6 = vpTranslationVector::cross(t4, t5);
+ bench[0] = 0;
+ bench[1] = 0;
+ bench[2] = 0;
+ if (test("t6", t6, bench) == false)
+ return err;
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+}
diff --git a/modules/core/test/math/testTwistMatrix.cpp b/modules/core/test/math/testTwistMatrix.cpp
new file mode 100644
index 0000000..7f0f148
--- /dev/null
+++ b/modules/core/test/math/testTwistMatrix.cpp
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests some vpMatrix functionalities.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testTwistMatrix.cpp
+
+ \brief Test some vpMatrix functionalities.
+*/
+
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+int
+main()
+{
+ try {
+ vpTRACE("--------------------------");
+ vpTRACE("--- TEST vpVelocityTwistMatrix ---");
+ vpTRACE("--------------------------");
+
+ // Set the translation
+ vpTranslationVector cte;
+ cte[0] = 1.;
+ cte[1] = 0.5;
+ cte[2] = -1.;
+
+ // Set the rotation
+ vpRxyzVector cre;
+ cre[0] = M_PI/2.;
+ cre[1] = -M_PI/2.;
+ cre[2] = -M_PI/4.;
+
+ // Build rotation matrix
+ vpRotationMatrix cRe(cre);
+
+ // Build the twist matrix
+ vpVelocityTwistMatrix cVe(cte, cRe);
+
+ vpTRACE("cVe twist matrix:");
+ cVe.print (std::cout, 6);
+
+ // Set a speed skew
+ vpColVector ev(6);
+
+ ev[0] = 1.;
+ ev[1] = 0.1;
+ ev[2] = -0.5;
+ ev[3] = M_PI/180.;
+ ev[4] = M_PI/18.;
+ ev[5] = M_PI/10.;
+
+ vpTRACE("ev colvector:");
+ ev.print (std::cout, 6);
+
+ // Set a speed skew
+ vpColVector cv;
+
+ cv = cVe * ev;
+
+ vpTRACE("cv = cVe * ev:");
+ cv.print (std::cout, 6);
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/network/testClient.cpp b/modules/core/test/network/testClient.cpp
new file mode 100644
index 0000000..ff6a0f3
--- /dev/null
+++ b/modules/core/test/network/testClient.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for TCP client/Server.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \example testClient.cpp
+
+ Example of a TCP client/server.
+*/
+
+#include <iostream>
+#include <visp3/core/vpClient.h>
+
+int main()
+{
+ try {
+ std::string servername = "localhost";
+ unsigned int port = 35000;
+
+ vpClient client;
+ client.connectToHostname(servername, port);
+ //client.connectToIP("127.0.0.1",port);
+
+ int val = 0;
+
+ while(1)
+ {
+ if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
+ std::cout << "Error while sending" << std::endl;
+ else
+ std::cout << "Sending : " << val << std::endl;
+
+ if(client.receive(&val) != sizeof(int)) //Receiving a value from the first client
+ std::cout << "Error while receiving" << std::endl;
+ else
+ std::cout << "Received : " << val << std::endl;
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/network/testServer.cpp b/modules/core/test/network/testServer.cpp
new file mode 100644
index 0000000..c8644c2
--- /dev/null
+++ b/modules/core/test/network/testServer.cpp
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for TCP client/Server.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \example testServer.cpp
+
+ Example of a TCP client/server.
+*/
+
+#include <iostream>
+#include <visp3/core/vpServer.h>
+
+int main()
+{
+ try {
+ int port = 35000;
+ vpServer serv(port); //Launch the server on localhost
+ serv.start();
+
+ bool run = true;
+ int val;
+
+ while(run){
+ serv.checkForConnections();
+
+ if(serv.getNumberOfClients() > 0)
+ {
+ if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
+ std::cout << "Error while receiving" << std::endl;
+ else
+ std::cout << "Received : " << val << std::endl;
+
+ val = val+1;
+ if(serv.send(&val) != sizeof(int)) //Sending the new value to the first client
+ std::cout << "Error while sending" << std::endl;
+ else
+ std::cout << "Sending : " << val << std::endl;
+ }
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/tools/convert/testConvert.cpp b/modules/core/test/tools/convert/testConvert.cpp
new file mode 100644
index 0000000..d74683a
--- /dev/null
+++ b/modules/core/test/tools/convert/testConvert.cpp
@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test functions in vpIoTools.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+
+ \example testConvert.cpp
+
+ \brief Test functions in Convert.
+
+*/
+
+#include <iostream> // std::cout
+#include <limits> // std::numeric_limits
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpConvert.h>
+
+
+bool areSame(double a, double b) {
+ return fabs(a - b) < std::numeric_limits<double>::epsilon();
+}
+
+void testConvertFromImagePointToPoint2f() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpImagePoint imPt1(12.5f, .85f);
+ vpImagePoint imPt2(-44.26f, 125.11f);
+ vpImagePoint imPt3(0.0f, -1.756e-10f);
+
+ cv::Point2f pt1, pt2, pt3;
+ vpConvert::convertToOpenCV(imPt1, pt1);
+ vpConvert::convertToOpenCV(imPt2, pt2);
+ vpConvert::convertToOpenCV(imPt3, pt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+
+ std::vector<vpImagePoint> listOfImPts(3);
+ listOfImPts[0] = imPt1;
+ listOfImPts[1] = imPt2;
+ listOfImPts[2] = imPt3;
+
+ std::vector<cv::Point2f> listOfPts;
+ vpConvert::convertToOpenCV(listOfImPts, listOfPts);
+
+ if(listOfImPts.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromImagePointToPoint2f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromPoint2fToImagePoint() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpImagePoint imPt1, imPt2, imPt3;
+
+ cv::Point2f pt1(12.5f, .85f), pt2(-44.26f, 125.11f), pt3(0.0f, -1.756e-10f);
+ vpConvert::convertFromOpenCV(pt1, imPt1);
+ vpConvert::convertFromOpenCV(pt2, imPt2);
+ vpConvert::convertFromOpenCV(pt3, imPt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+
+ std::vector<vpImagePoint> listOfImPts;
+
+ std::vector<cv::Point2f> listOfPts(3);
+ listOfPts[0] = pt1;
+ listOfPts[1] = pt2;
+ listOfPts[2] = pt3;
+
+ vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
+
+ if(listOfImPts.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromPoint2fToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromImagePointToPoint2d() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpImagePoint imPt1(12.5, .85);
+ vpImagePoint imPt2(-44.26, 125.11);
+ vpImagePoint imPt3(0, -1.756e-10);
+
+ cv::Point2d pt1, pt2, pt3;
+ vpConvert::convertToOpenCV(imPt1, pt1);
+ vpConvert::convertToOpenCV(imPt2, pt2);
+ vpConvert::convertToOpenCV(imPt3, pt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+
+ std::vector<vpImagePoint> listOfImPts(3);
+ listOfImPts[0] = imPt1;
+ listOfImPts[1] = imPt2;
+ listOfImPts[2] = imPt3;
+
+ std::vector<cv::Point2d> listOfPts;
+ vpConvert::convertToOpenCV(listOfImPts, listOfPts);
+
+ if(listOfImPts.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromImagePointToPoint2d=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromPoint2dToImagePoint() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpImagePoint imPt1, imPt2, imPt3;
+
+ cv::Point2d pt1(12.5, .85), pt2(-44.26, 125.11), pt3(0, -1.756e-10);
+ vpConvert::convertFromOpenCV(pt1, imPt1);
+ vpConvert::convertFromOpenCV(pt2, imPt2);
+ vpConvert::convertFromOpenCV(pt3, imPt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+
+ std::vector<vpImagePoint> listOfImPts;
+
+ std::vector<cv::Point2d> listOfPts(3);
+ listOfPts[0] = pt1;
+ listOfPts[1] = pt2;
+ listOfPts[2] = pt3;
+
+ vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
+
+ if(listOfImPts.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromPoint2dToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromKeyPointToImagePoint() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ cv::KeyPoint kp1(12.5f, .85f, 0), kp2(-44.26f, 125.11f, 0), kp3(0.0f, -1.756e-10f, 0);
+ vpImagePoint imPt1, imPt2, imPt3;
+
+ vpConvert::convertFromOpenCV(kp1, imPt1);
+ vpConvert::convertFromOpenCV(kp2, imPt2);
+ vpConvert::convertFromOpenCV(kp3, imPt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(imPt1.get_u(), kp1.pt.x) && areSame(imPt1.get_v(), kp1.pt.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt2.get_u(), kp2.pt.x) && areSame(imPt2.get_v(), kp2.pt.y)) nbOk++; else nbNOk++;
+ if(areSame(imPt3.get_u(), kp3.pt.x) && areSame(imPt3.get_v(), kp3.pt.y)) nbOk++; else nbNOk++;
+
+ std::vector<cv::KeyPoint> listOfKeyPoints(3);
+ listOfKeyPoints[0] = kp1;
+ listOfKeyPoints[1] = kp2;
+ listOfKeyPoints[2] = kp3;
+
+ std::vector<vpImagePoint> listOfImPts;
+ vpConvert::convertFromOpenCV(listOfKeyPoints, listOfImPts);
+
+ if(listOfImPts.size() == listOfKeyPoints.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfImPts[i].get_u(), listOfKeyPoints[i].pt.x) && areSame(listOfImPts[i].get_v(), listOfKeyPoints[i].pt.y)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromKeyPointToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromPoint3fToPoint() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ cv::Point3f pt1(12.5f, .85f, 110.0f), pt2(-44.26f, 125.11f, -98e2f), pt3(0.0f, -1.756e-10f, 0.00015f);
+ vpPoint point1, point2, point3;
+
+ vpConvert::convertFromOpenCV(pt1, point1);
+ vpConvert::convertFromOpenCV(pt2, point2);
+ vpConvert::convertFromOpenCV(pt3, point3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ())) nbOk++; else nbNOk++;
+ if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
+ if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
+
+ std::vector<cv::Point3f> listOfPoints3f(3);
+ listOfPoints3f[0] = pt1;
+ listOfPoints3f[1] = pt2;
+ listOfPoints3f[2] = pt3;
+
+ std::vector<vpPoint> listOfPts;
+ vpConvert::convertFromOpenCV(listOfPoints3f, listOfPts);
+
+ if(listOfPoints3f.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromPoint3fToPoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+void testConvertFromPointToPoint3f() {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ cv::Point3f pt1, pt2, pt3;
+ vpPoint point1, point2, point3;
+ point1.set_oX(12.5f);
+ point1.set_oY(.85f);
+ point1.set_oZ(110.0f);
+
+ point2.set_oX(-44.26f);
+ point2.set_oY(125.11f);
+ point2.set_oZ(-98e2f);
+
+ point3.set_oX(0.0f);
+ point3.set_oY(-1.756e-10f);
+ point3.set_oZ(0.00015f);
+
+ vpConvert::convertToOpenCV(point1, pt1);
+ vpConvert::convertToOpenCV(point2, pt2);
+ vpConvert::convertToOpenCV(point3, pt3);
+
+ int nbOk = 0, nbNOk = 0;
+ if(areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ())) nbOk++; else nbNOk++;
+ if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
+ if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
+
+ std::vector<cv::Point3f> listOfPoints3f;
+ std::vector<vpPoint> listOfPts(3);
+ listOfPts[0] = point1;
+ listOfPts[1] = point2;
+ listOfPts[2] = point3;
+
+ vpConvert::convertToOpenCV(listOfPts, listOfPoints3f);
+
+ if(listOfPoints3f.size() == listOfPts.size()) {
+ for(size_t i = 0; i < 3; i++) {
+ if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
+ }
+ } else {
+ nbNOk += 3;
+ }
+
+ std::cout << "testConvertFromPointToPoint3f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
+#endif
+}
+
+int main() {
+ testConvertFromImagePointToPoint2f();
+ testConvertFromPoint2fToImagePoint();
+ testConvertFromImagePointToPoint2d();
+ testConvertFromPoint2dToImagePoint();
+
+ testConvertFromKeyPointToImagePoint();
+ testConvertFromPoint3fToPoint();
+ testConvertFromPointToPoint3f();
+ return 0;
+}
diff --git a/modules/core/test/tools/geometry/testPolygon.cpp b/modules/core/test/tools/geometry/testPolygon.cpp
new file mode 100644
index 0000000..21457b8
--- /dev/null
+++ b/modules/core/test/tools/geometry/testPolygon.cpp
@@ -0,0 +1,251 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example which test the polygon.
+ *
+ * Author:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpPolygon.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+
+
+#include <math.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+//! List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool& opt_display, bool& opt_click);
+
+/*!
+
+Print the program options.
+
+\param name : Program name.
+\param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+test the generic 2D polygons.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+ -c \n\
+ Disable mouse click.\n\
+\n\
+ -d \n\
+ Turn off display.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param opt_display : optionnal flag to turn off the display.
+ \param opt_click : activates the mouse click.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, bool& opt_display, bool& opt_click)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': opt_click = false; break;
+ case 'd': opt_display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+
+/* -------------------------------------------------------------------------- */
+/* MAIN FUNCTION */
+/* -------------------------------------------------------------------------- */
+
+int
+main(int argc, const char** argv)
+{
+ try {
+ bool opt_display = true;
+ bool opt_click = true;
+ vpImage<unsigned char> I(480, 640, 255);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_display, opt_click) == false) {
+ return (-1);
+ }
+
+ std::vector <vpImagePoint> vec1;
+ vec1.push_back(vpImagePoint(200, 200));
+ vec1.push_back(vpImagePoint(200, 400));
+ vec1.push_back(vpImagePoint(320, 400));
+ vec1.push_back(vpImagePoint(380, 300));
+ vec1.push_back(vpImagePoint(280, 280));
+ vpPolygon p1;
+ p1.buildFrom(vec1);
+
+ std::vector <vpImagePoint> vec2;
+ vec2.push_back(vpImagePoint(20, 20));
+ vec2.push_back(vpImagePoint(100, 20));
+ vec2.push_back(vpImagePoint(100, 100));
+ vec2.push_back(vpImagePoint(20, 100));
+ vpPolygon p2(vec2);
+
+
+ std::vector <vpImagePoint> vec3;
+ vpPolygon p3(vec3);
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ opt_display = false;
+#endif
+
+ std::cout << " Polygon 1 : " << std::endl;
+ std::cout << " area : " << p1.getArea() << std::endl;
+ std::cout << " center : " << p1.getCenter() << std::endl << std::endl;
+
+ std::cout << " Polygon 2 : " << std::endl;
+ std::cout << " area : " << p2.getArea() << std::endl;
+ std::cout << " center : " << p2.getCenter() << std::endl << std::endl;
+
+ std::cout << " Polygon 3 : " << std::endl;
+ std::cout << " area : " << p3.getArea() << std::endl;
+ std::cout << " center : " << p3.getCenter() << std::endl;
+
+
+ if(opt_display){
+#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || (defined VISP_HAVE_GDI)
+ display.init(I, 10, 10, "Test vpPolygon");
+#endif
+ vpDisplay::display(I);
+ p1.display(I, vpColor::green, 1);
+ vpDisplay::displayCross(I, p1.getCenter(), 5, vpColor::green);
+ p2.display(I, vpColor::red, 1);
+ vpDisplay::displayCross(I, p2.getCenter(), 5, vpColor::red);
+ p3.display(I, vpColor::blue, 1);
+ vpDisplay::displayCross(I, p3.getCenter(), 5, vpColor::lightBlue);
+ vpDisplay::displayText(I, vpImagePoint(10, 10), "Click to finish", vpColor::red);
+ vpDisplay::flush(I);
+
+ if (opt_click)
+ vpDisplay::getClick(I);
+
+
+ vpDisplay::display(I);
+ vpDisplay::displayText(I, vpImagePoint(10, 10), "Left click to add a point", vpColor::red);
+ vpDisplay::displayText(I, vpImagePoint(20, 10), "Right click to build the polygon", vpColor::red);
+ vpDisplay::flush(I);
+ if (opt_click) {
+ vpPolygon p4;
+ p4.initClick(I);
+ p4.display(I, vpColor::green, 1);
+ std::cout << std::endl;
+ std::cout << " Polygon 4 : " << std::endl;
+ std::cout << " area : " << p4.getArea() << std::endl;
+ std::cout << " center : " << p4.getCenter() << std::endl;
+ std::cout << "Click to continue." << std::endl;
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I);
+
+ vpRect bbox = p4.getBoundingBox();
+ for(unsigned int i= (unsigned int)floor(bbox.getTop()); i<(unsigned int)ceil(bbox.getBottom()); ++i){
+ for(unsigned int j=(unsigned int)floor(bbox.getLeft()); j<(unsigned int)ceil(bbox.getRight()); ++j){
+ if(p4.isInside(vpImagePoint(i, j))){
+ vpDisplay::displayPoint(I, vpImagePoint(i, j), vpColor::orange);
+ }
+ }
+ }
+ vpDisplay::flush(I);
+ std::cout << "Click to finish." << std::endl;
+
+ vpDisplay::getClick(I);
+ }
+ }
+
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+
diff --git a/modules/core/test/tools/io/testIoTools.cpp b/modules/core/test/tools/io/testIoTools.cpp
new file mode 100644
index 0000000..9021fe3
--- /dev/null
+++ b/modules/core/test/tools/io/testIoTools.cpp
@@ -0,0 +1,442 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test functions in vpIoTools.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+
+ \example testIoTools.cpp
+
+ \brief Test functions in IoTools.
+
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+#include <visp3/core/vpIoTools.h>
+
+
+int
+main(int argc, const char ** argv)
+{
+ const char c = vpIoTools::separator;
+ if(c == '\\')
+ {
+ std::cout << "The directory separator character is '" << c << "' (Windows platform)." << std::endl;
+ }
+ else {
+ std::cout << "The directory separator character is '" << c << "' (Unix like platform)." << std::endl;
+ }
+
+
+ std::string pathname = "";
+#if defined(_WIN32)
+ pathname = "C:\\Program Files (x86)\\Java\\jre7";
+#else
+ pathname = "/usr/bin/java";
+#endif
+
+ std::cout << "Parent of " << pathname << " is " << vpIoTools::getParent(pathname) << std::endl;
+ std::cout << "Name of " << pathname << " is " << vpIoTools::getName(pathname) << std::endl;
+
+
+ if(argc == 3 && std::string(argv[1]) == std::string("-i"))
+ {
+ std::cout << "Parent of " << argv[2] << " is " << vpIoTools::getParent(argv[2]) << std::endl;
+ std::cout << "Name of " << argv[2] << " is " << vpIoTools::getName(argv[2]) << std::endl;
+ }
+
+ std::string windowsPathnameStyle = "\\usr\\bin\\java";
+ std::cout << "Parent of " << windowsPathnameStyle << " is " << vpIoTools::getParent(windowsPathnameStyle) << std::endl;
+ std::cout << "Name of " << windowsPathnameStyle << " is " << vpIoTools::getName(windowsPathnameStyle) << std::endl;
+
+ std::string parent = "/usr/toto/", child = "\\blabla\\java";
+ std::cout << "parent=" << vpIoTools::path(parent) << " ; child=" << vpIoTools::path(child) << std::endl;
+ std::cout << "Create file path from parent=" << parent << " and child=" << child << " is "
+ << vpIoTools::createFilePath(parent, child) << std::endl;
+
+ std::string expandPath = "~/Documents/fictional directory/fictional file";
+ std::cout << "Path for " << expandPath << " is " << vpIoTools::path(expandPath) << std::endl;
+
+ std::cout << "Test get name with an empty pathname=" << vpIoTools::getName("") << std::endl;
+ std::cout << "Get parent with an empty pathname=" << vpIoTools::getParent("") << std::endl;
+ std::cout << "Get parent with a filename=" << vpIoTools::getParent("my_file.txt") << std::endl;
+ expandPath = "~/Documents/fictional dir/fictional file.txt";
+ std::cout << "Get name with a unix expand pathname " << expandPath << "=" << vpIoTools::getName(expandPath) << std::endl;
+ std::cout << "Get parent with a unix expand pathname " << expandPath << "=" << vpIoTools::getParent(expandPath) << std::endl;
+
+
+ pathname = "c:/dir";
+ std::cout << "pathname=" << vpIoTools::splitDrive(pathname).first << " ; " << vpIoTools::splitDrive(pathname).second << std::endl;
+
+ std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
+
+ pathname = "c:/dir/fictional directory/fictional file.txt";
+ std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
+
+ pathname = "/home/user/Documents/fictional directory/fictional file.txt";
+ std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
+
+ pathname = "~/Documents/fictional directory/fictional file.txt";
+ std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
+
+ pathname = "fictional directory/fictional file.txt";
+ std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
+
+
+ //Test vpIoTools::splitDrive
+ unsigned int nbFail = 0, nbOk = 0;
+#if defined(_WIN32)
+ if(strcmp(vpIoTools::splitDrive("c:\\foo\\bar").first.c_str(), "c:") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").first << " should be=c:" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("c:\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").second << " should be=\\foo\\bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("c:/foo/bar").first.c_str(), "c:") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").first << " should be=c:" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("c:/foo/bar").second.c_str(), "/foo/bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").second << " should be=/foo/bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "\\\\conky\\mountpoint") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first << " should be=\\\\conky\\mountpoint" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").second << " should be=\\foo\\bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("//conky/mountpoint/foo/bar").first.c_str(), "//conky/mountpoint") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("//conky/mountpoint/foo/bar").first << " should be=//conky/mountpoint" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("//conky/mountpoint/foo/bar").second.c_str(), "/foo/bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("//conky/mountpoint/foo/bar").second << " should be=/foo/bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").first << " should be=" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").second.c_str(),
+ "\\\\\\conky\\mountpoint\\foo\\bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").second << " should be=\\\\\\conky\\mountpoint\\foo\\bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").first.c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("///conky/mountpoint/foo/bar").first << " should be=" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second.c_str(), "///conky/mountpoint/foo/bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second << " should be=///conky/mountpoint/foo/bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").first.c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").first << " should be=" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").second.c_str(),
+ "\\\\conky\\\\mountpoint\\foo\\bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").second << " should be=\\\\conky\\\\mountpoint\\foo\\bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").first.c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("//conky//mountpoint/foo/bar").first << " should be=" << std::endl;
+ }
+ if(strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second.c_str(), "//conky//mountpoint/foo/bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second << " should be=//conky//mountpoint/foo/bar" << std::endl;
+ }
+
+ std::cout << "Test vpIoTools::splitDrive (Win32) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
+#endif
+
+
+ //Test vpIoTools::getFileExtension
+#if defined(_WIN32)
+ nbFail = 0;
+ nbOk = 0;
+
+ if(strcmp(vpIoTools::getFileExtension("foo.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("/foo/foo.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("/foo/foo.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension(".ext").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension(".ext") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("\\foo.ext\\foo").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("\\foo.ext\\foo") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("foo.ext\\").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext\\") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("foo.bar.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("xx/foo.bar.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("xx/foo.bar.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("xx\\foo.bar.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("xx\\foo.bar.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("c:a/b\\c.d").c_str(), ".d") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("c:a/b\\c.d") << " should be=.d" << std::endl;
+ }
+
+ std::cout << "Test vpIoTools::getFileExtension (WIN32 platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
+#else
+ nbFail = 0;
+ nbOk = 0;
+
+ if(strcmp(vpIoTools::getFileExtension("foo.bar").c_str(), ".bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar") << " should be=.bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("foo.boo.bar").c_str(), ".bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.bar") << " should be=.bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("foo.boo.biff.bar").c_str(), ".bar") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.biff.bar") << " should be=.bar" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension(".csh.rc").c_str(), ".rc") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension(".csh.rc") << " should be=.rc" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("nodots").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("nodots") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension(".cshrc").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension(".cshrc") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("...manydots").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("...manydots.ext").c_str(), ".ext") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots.ext") << " should be=.ext" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension(".").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension(".") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("..").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("..") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("........").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("........") << " should be=" << std::endl;
+ }
+
+ if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
+ nbOk++;
+ }
+ else {
+ nbFail++;
+ std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl;
+ }
+
+ std::cout << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
+#endif
+
+
+ std::cout << std::endl << "End" << std::endl;
+
+ return 0;
+}
diff --git a/modules/core/test/tools/time/testTime.cpp b/modules/core/test/tools/time/testTime.cpp
new file mode 100644
index 0000000..98c2273
--- /dev/null
+++ b/modules/core/test/tools/time/testTime.cpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Time management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \example testTime.cpp
+
+ \brief Time management.
+
+*/
+#include <visp3/core/vpConfig.h>
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# include <unistd.h>
+#elif defined(_WIN32)
+# include <windows.h>
+# include <mmsystem.h>
+# include <winbase.h>
+#endif
+#include <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <time.h>
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+ try {
+ double v = 0;
+
+ double t0 = vpTime::measureTimeMs();
+ for (int i =0 ; i < 100000; i ++)
+ for (int j =0 ; j < 100; j ++)
+ v = i * 2 / 3. + j;
+ std::cout << "Computed dummy value: " << v << std::endl;
+
+ double t1 = vpTime::measureTimeMs();
+ vpTime::wait(t1, 40);
+
+ double t2 = vpTime::measureTimeMs();
+
+ // Sleep 10ms
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ usleep(10*1000);
+#elif defined(_WIN32)
+ Sleep(10);
+#endif
+
+ double t3 = vpTime::measureTimeMs();
+
+ // Sleep 2ms
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ usleep(2*1000);
+#elif defined(_WIN32)
+ Sleep(2);
+#endif
+ double t4 = vpTime::measureTimeMs();
+
+ vpTime::wait(t4, 19);
+
+ double t5 = vpTime::measureTimeMs();
+
+ vpTime::wait(5);
+
+ double t6 = vpTime::measureTimeMs();
+
+ vpTime::wait(21);
+
+ double t7 = vpTime::measureTimeMs();
+
+ vpTime::wait(2);
+
+ double t8 = vpTime::measureTimeMs();
+
+ std::cout << "t1-t0: computation time: " << t1 - t0 << std::endl;
+ std::cout << "t2-t1: wait(t1, 40 ms): " << t2 - t1 << std::endl;
+ std::cout << "t3-t2: sleep(10 ms): " << t3 - t2 << std::endl;
+ std::cout << "t4-t3: sleep(2 ms): " << t4 - t3 << std::endl;
+ std::cout << "t5-t4: wait(t, 19 ms): " << t5 - t4 << std::endl;
+ std::cout << "t6-t5: wait(5 ms): " << t6 - t5 << std::endl;
+ std::cout << "t7-t6: wait(21 ms): " << t7 - t6 << std::endl;
+ std::cout << "t8-t7: wait(2 ms): " << t8 - t7 << std::endl;
+
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/core/test/tools/xml/testXmlParser.cpp b/modules/core/test/tools/xml/testXmlParser.cpp
new file mode 100644
index 0000000..c7b2b3d
--- /dev/null
+++ b/modules/core/test/tools/xml/testXmlParser.cpp
@@ -0,0 +1,388 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example which describes how to use the xml parser class.
+ *
+ * Author:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+
+/*!
+ \example testXmlParser.cpp
+
+ XML parser example.
+
+ This example contains the declaration of a class used to read and write data
+ in a xml file like:
+ \code
+ <config>
+ <range>5.5</range>
+ <step>7</step>
+ <size_filter>3</size_filter>
+ <name>Object</name>
+ </config>
+ \endcode
+
+*/
+
+
+#include <visp3/core/vpConfig.h>
+
+#include <iostream>
+#if defined(VISP_HAVE_XML2)
+
+#include <visp3/core/vpXmlParser.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+
+#include <string>
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/* -------------------------------------------------------------------------- */
+/* CLASS EXAMPLE */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ \class vpExampleDataParser
+ \brief Class example used to show how to implement a xml parser based on the
+ vpXmlParser
+*/
+class vpExampleDataParser: public vpXmlParser
+{
+protected:
+ double m_range;
+ int m_step;
+ int m_size_filter;
+ std::string m_name;
+
+ typedef enum{
+ config,
+ range,
+ step,
+ size_filter,
+ name
+ }dataToParse;
+
+
+public:
+ vpExampleDataParser();
+ virtual ~vpExampleDataParser();
+
+ // Data accessors.
+ double getRange() const {return m_range;}
+ int getStep() const {return m_step;}
+ int getSizeFilter() const {return m_size_filter;}
+ std::string getName() const {return m_name;}
+
+ void setRange(const double _range) {m_range = _range;}
+ void setStep(const int _step) {m_step = _step;}
+ void setSizeFilter(const int _size_filter) {m_size_filter = _size_filter;}
+ void setName(const std::string& _name) { m_name = _name;}
+
+protected:
+ virtual void readMainClass (xmlDocPtr doc, xmlNodePtr node);
+ virtual void writeMainClass (xmlNodePtr node);
+};
+
+/*!
+ Constructor.
+ Initialise the map according to the data to parse, and initialise data to
+ default values.
+
+*/
+vpExampleDataParser::vpExampleDataParser()
+ : m_range(0.), m_step(0), m_size_filter(0), m_name("")
+{
+ nodeMap["config"] = config;
+ nodeMap["range"] = range;
+ nodeMap["step"] = step;
+ nodeMap["size_filter"] = size_filter;
+ nodeMap["name"] = name;
+}
+
+/*!
+ Destructor.
+
+*/
+vpExampleDataParser::~vpExampleDataParser()
+{
+
+}
+
+/*!
+ Read the main class. This method corresponds to the parsing of the main
+ document (which contains the whole data in the class). At this point, the
+ document exists and is open.
+
+ \param doc : Pointer to the document to parse.
+ \param node : Pointer to the root node of the document.
+*/
+void
+vpExampleDataParser::readMainClass (xmlDocPtr doc, xmlNodePtr node)
+{
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case range:
+ this->m_range = xmlReadDoubleChild(doc, dataNode);
+ break;
+ case step:
+ this->m_step = xmlReadIntChild(doc, dataNode);
+ break;
+ case size_filter:
+ this->m_size_filter = xmlReadIntChild(doc, dataNode);
+ break;
+ case name:{
+ this->m_name = xmlReadStringChild(doc, dataNode);
+ }break;
+ default:
+ vpTRACE("unknown tag in readConfigNode : %d, %s", iter_data->second, (iter_data->first).c_str());
+ break;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Write the data in the file.
+ The file has already been opened or created in the save() method. And the root
+ node (corresponding to the main tag) has already been writen.
+
+ \param node : Pointer to the root node.
+*/
+void
+vpExampleDataParser::writeMainClass(xmlNodePtr node)
+{
+ xmlWriteDoubleChild(node, (const char*)"range", m_range);
+ xmlWriteIntChild(node, (const char*)"step", m_step);
+ xmlWriteIntChild(node, (const char*)"size_filter", m_size_filter);
+ xmlWriteCharChild(node, (const char*)"name", m_name.c_str());
+}
+
+
+#endif // doxygen
+
+/* -------------------------------------------------------------------------- */
+/* COMMAND LINE OPTIONS */
+/* -------------------------------------------------------------------------- */
+
+// List of allowed command line options
+#define GETOPTARGS "cdo:h"
+
+void usage(const char *name, const char *badparam, const std::string& opath, const std::string& user);
+bool getOptions(int argc, const char **argv, std::string &opath, const std::string& user);
+
+/*!
+
+Print the program options.
+
+\param name : Program name.
+\param badparam : Bad parameter name.
+\param opath : Output image path.
+\param user : Username.
+
+ */
+void usage(const char *name, const char *badparam, const std::string& opath, const std::string& user)
+{
+ fprintf(stdout, "\n\
+Write and read data in a xml file.\n\
+ \n\
+SYNOPSIS\n\
+ %s [-o <output image path>] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -o <output data path> %s\n\
+ Set data output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ dataTestXml.xml file is written.\n\
+ \n\
+ -h\n\
+ Print the help.\n\n", opath.c_str(), user.c_str());
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param opath : Output data path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, std::string &opath, const std::string& user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, opath, user); return false; break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, opath, user); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+
+/* -------------------------------------------------------------------------- */
+/* MAIN FUNCTION */
+/* -------------------------------------------------------------------------- */
+
+int main(int argc, const char** argv)
+{
+ try {
+ std::string opt_opath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ std::cout << "-------------------------------------------------------" << std::endl ;
+ std::cout << " testXmlParser.cpp" <<std::endl << std::endl ;
+ std::cout << " writing and readind data using a xml parser" << std::endl ;
+ std::cout << "-------------------------------------------------------" << std::endl ;
+ std::cout << std::endl ;
+
+ // Set the default output path
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ opt_opath = "/tmp";
+#elif defined(_WIN32)
+ opt_opath = "C:\\temp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_opath, username) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ std::string dirname = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(dirname) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(dirname);
+ }
+ catch (...) {
+ usage(argv[0], NULL, opath, username);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << dirname << std::endl;
+ std::cerr << " Check your -o " << opath << " option " << std::endl;
+ exit(-1);
+ }
+ }
+
+ filename = dirname + vpIoTools::path("/") + "dataTestXml.xml";
+
+ // Write data using a parser.
+ {
+ vpExampleDataParser parser1;
+
+ // Acquire data from measurments or tests.
+ parser1.setRange(3.5);
+ parser1.setStep(2);
+ parser1.setSizeFilter(5);
+ parser1.setName("cube");
+
+ std::cout << "Write data to " << filename << std::endl;
+ parser1.save(filename);
+ }
+
+ // Read data using another parser.
+ {
+ vpExampleDataParser parser2;
+
+ parser2.parse(filename);
+
+ std::cout << "Read from " << filename << std::endl ;
+ std::cout << "Range : " << parser2.getRange() << std::endl;
+ std::cout << "Step : " << parser2.getStep() << std::endl;
+ std::cout << "Filter size : " << parser2.getSizeFilter() << std::endl;
+ std::cout << "name : " << parser2.getName() << std::endl;
+ }
+
+ // Clean up memory allocated by the xml library
+ vpXmlParser::cleanup();
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+
+int main()
+{
+ std::cout << "Xml parser requires libxml2." << std::endl;
+ return 0;
+}
+#endif
diff --git a/modules/detection/CMakeLists.txt b/modules/detection/CMakeLists.txt
new file mode 100644
index 0000000..ee541e1
--- /dev/null
+++ b/modules/detection/CMakeLists.txt
@@ -0,0 +1,55 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+if(USE_ZBAR)
+ list(APPEND opt_incs ${ZBAR_INCLUDE_DIRS})
+ list(APPEND opt_libs ${ZBAR_LIBRARIES})
+endif()
+if(USE_DMTX)
+ list(APPEND opt_incs ${DMTX_INCLUDE_DIRS})
+ list(APPEND opt_libs ${DMTX_LIBRARIES})
+endif()
+
+vp_add_module(detection visp_core)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+
diff --git a/modules/detection/include/visp3/detection/vpDetectorBase.h b/modules/detection/include/visp3/detection/vpDetectorBase.h
new file mode 100644
index 0000000..299287c
--- /dev/null
+++ b/modules/detection/include/visp3/detection/vpDetectorBase.h
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for object detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpDetectorBase_h__
+#define __vpDetectorBase_h__
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <assert.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRect.h>
+
+/*!
+ \class vpDetectorBase
+ \ingroup group_detection_barcode group_detection_face
+ Base class for object detection.
+
+ This class is a generic class that can be used to detect:
+ - bar codes like QRcodes of Data matrices. The example given in tutorial-barcode-detector.cpp shows
+ how to detect one or more bar codes in an image. In tutorial-barcode-detector-live.cpp you will find
+ an other example that shows how to use this class to detect bar codes in images acquired by a camera.
+ - faces. An example is provided in tutorial-face-detector-live.cpp.
+ */
+class VISP_EXPORT vpDetectorBase
+{
+protected:
+ std::vector< std::vector<vpImagePoint> > m_polygon; //!< For each object, defines the polygon that contains the object.
+ std::vector< std::string > m_message; //!< Message attached to each object.
+ size_t m_nb_objects; //!< Number of detected objects.
+
+public:
+ /*!
+ Default constructor.
+ */
+ vpDetectorBase();
+ /*!
+ Default destructor.
+ */
+ virtual ~vpDetectorBase() {};
+
+ /*!
+ Detect objects in an image.
+ \param I : Image where to detect objects.
+ \return true if one or multiple objects are detected, false otherwise.
+ */
+ virtual bool detect(const vpImage<unsigned char> &I) = 0;
+ /*!
+ Returns object container box as a vector of points.
+ */
+ std::vector< std::vector<vpImagePoint> > & getPolygon()
+ {
+ return m_polygon;
+ }
+
+ /*!
+ Returns ith object container box as a vector of points.
+ */
+ std::vector<vpImagePoint> & getPolygon(size_t i);
+
+ /*!
+ Returns the contained message of the ith object if there is one.
+ */
+ std::string & getMessage(size_t i);
+
+ /*!
+ Returns the contained message of the ith object if there is one.
+ */
+ std::vector< std::string > & getMessage() { return m_message; }
+
+ /*!
+ Return the number of objects that are detected.
+ */
+ size_t getNbObjects() const {return m_nb_objects; }
+
+ /*!
+ Return the center of gravity location of the ith object.
+ */
+ vpImagePoint getCog(size_t i) const ;
+
+ /*!
+ Return the bounding box of the ith object.
+ */
+ vpRect getBBox(size_t i) const ;
+};
+
+#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h
new file mode 100644
index 0000000..3cf691f
--- /dev/null
+++ b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for bar code detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpDetectorDataMatrixCode_h__
+#define __vpDetectorDataMatrixCode_h__
+
+#include <vector>
+#include <utility>
+#include <string>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_DMTX
+
+#include <visp3/detection/vpDetectorBase.h>
+#include <visp3/core/vpImage.h>
+
+/*!
+ \class vpDetectorDataMatrixCode
+ \ingroup group_detection_barcode
+ Base class for bar code detector. This class is a wrapper over libdmtx
+ available from http://www.libdmtx.org/
+
+ The detect() function allows to detect multiple QR codes in an image. Once detected,
+ for each QR code it is possible to retrieve the location of the corners using getPolygon(),
+ the encoded message using getMessage(), the bounding box using getBBox() and the center
+ of gravity using getCog().
+
+ The following sample code shows how to use this class to detect QR codes in an image.
+ \code
+#include <visp3/detection/vpDetectorDataMatrixCode.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#ifdef VISP_HAVE_DMTX
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "bar-code.pgm");
+
+ vpDetectorDataMatrixCode detector;
+
+ bool status = detector.detect(I);
+ if (status) {
+ for(size_t i=0; i < detector.getNbObjects(); i++) {
+ std::cout << "Bar code " << i << ":" << std::endl;
+ std::vector<vpImagePoint> p = detector.getPolygon(i);
+ for(size_t j=0; j < p.size(); j++)
+ std::cout << " Point " << j << ": " << p[j] << std::endl;
+ std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl;
+ }
+ }
+#endif
+}
+ \endcode
+
+ The previous example may produce results like:
+ \code
+Bar code 0:
+ Point 0: 273.21, 78.9799
+ Point 1: 390.016, 85.1014
+ Point 2: 388.024, 199.185
+ Point 3: 269.23, 192.96
+ Message: "datamatrix 1"
+Bar code 1:
+ Point 0: 262.23, 396.404
+ Point 1: 381.041, 402.631
+ Point 2: 378.92, 524.188
+ Point 3: 257.916, 519.962
+ Message: "datamatrix 2"
+ \endcode
+
+ Other examples are also provided in tutorial-barcode-detector.cpp and
+ tutorial-barcode-detector-live.cpp
+
+ */
+class VISP_EXPORT vpDetectorDataMatrixCode : public vpDetectorBase
+{
+public:
+ vpDetectorDataMatrixCode();
+ virtual ~vpDetectorDataMatrixCode() {};
+ bool detect(const vpImage<unsigned char> &I);
+};
+
+#endif
+#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorFace.h b/modules/detection/include/visp3/detection/vpDetectorFace.h
new file mode 100644
index 0000000..49abc3d
--- /dev/null
+++ b/modules/detection/include/visp3/detection/vpDetectorFace.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Detect faces.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpDetectorFace_h__
+#define __vpDetectorFace_h__
+
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
+
+#include <opencv2/objdetect/objdetect.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <visp3/detection/vpDetectorBase.h>
+
+/*!
+ \class vpDetectorFace
+ \ingroup group_detection_face
+ The vpDetectorFace class is a wrapper over OpenCV Haar cascade face detection capabilities.
+ To use this class ViSP should be build against OpenCV 2.2.0 or a more recent version.
+
+ The following sample code shows how to use this class to detect the largest face in the image.
+ The cascade classifier file "haarcascade_frontalface_alt.xml" can be found in ViSP source code or in OpenCV.
+ \code
+#include <visp3/detection/vpDetectorFace.h>
+
+int main()
+{
+ vpImage<unsigned char> I;
+ vpDetectorFace face_detector;
+ face_detector.setCascadeClassifierFile("haarcascade_frontalface_alt.xml");
+
+ while(1) {
+ // acquire a new image in I
+ bool face_found = face_detector.track(I);
+ if (face_found) {
+ vpRect face_bbox = face_detector.getBoundingBox(0); // largest face has index 0
+ }
+ }
+}
+ \endcode
+
+ A more complete example that works with images acquired from a camera is provided in tutorial-face-detector-live.cpp.
+ */
+class VISP_EXPORT vpDetectorFace : public vpDetectorBase
+{
+protected:
+ std::vector<cv::Rect> m_faces; //!< Bounding box of each detected face.
+ cv::CascadeClassifier m_face_cascade; //!< Haar cascade classifier file name.
+ cv::Mat m_frame_gray; //!< OpenCV image used as input for the face detection.
+
+public:
+ vpDetectorFace();
+ /*!
+ Default destructor.
+ */
+ virtual ~vpDetectorFace() {};
+ void setCascadeClassifierFile(const std::string &filename);
+
+ bool detect(const vpImage<unsigned char> &I);
+};
+
+#endif
+#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorQRCode.h b/modules/detection/include/visp3/detection/vpDetectorQRCode.h
new file mode 100644
index 0000000..9302e14
--- /dev/null
+++ b/modules/detection/include/visp3/detection/vpDetectorQRCode.h
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for bar code detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpDetectorQRCode_h__
+#define __vpDetectorQRCode_h__
+
+#include <vector>
+#include <utility>
+#include <string>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_ZBAR
+
+#include <zbar.h>
+
+#include <visp3/detection/vpDetectorBase.h>
+#include <visp3/core/vpImage.h>
+
+/*!
+ \class vpDetectorQRCode
+ \ingroup group_detection_barcode
+ Base class for bar code detector. This class is a wrapper over libzbar
+ available from http://zbar.sourceforge.net/
+
+ The detect() function allows to detect multiple QR codes in an image. Once detected,
+ for each QR code it is possible to retrieve the location of the corners using getPolygon(),
+ the encoded message using getMessage(), the bounding box using getBBox() and the center
+ of gravity using getCog().
+
+ The following sample code shows how to use this class to detect QR codes in an image.
+ \code
+#include <visp3/detection/vpDetectorQRCode.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#ifdef VISP_HAVE_ZBAR
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "bar-code.pgm");
+
+ vpDetectorQRCode detector;
+
+ bool status = detector.detect(I);
+ if (status) {
+ for(size_t i=0; i < detector.getNbObjects(); i++) {
+ std::cout << "Bar code " << i << ":" << std::endl;
+ std::vector<vpImagePoint> p = detector.getPolygon(i);
+ for(size_t j=0; j < p.size(); j++)
+ std::cout << " Point " << j << ": " << p[j] << std::endl;
+ std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl;
+ }
+ }
+#endif
+}
+ \endcode
+
+ The previous example may produce results like:
+ \code
+Bar code 0:
+ Point 0: 48, 212
+ Point 1: 57, 84
+ Point 2: 188, 92
+ Point 3: 183, 220
+ Message: "qrcode 2"
+Bar code 1:
+ Point 0: 26, 550
+ Point 1: 35, 409
+ Point 2: 174, 414
+ Point 3: 167, 555
+ Message: "qrcode 1"
+ \endcode
+
+ Other examples are also provided in tutorial-barcode-detector.cpp and
+ tutorial-barcode-detector-live.cpp
+ */
+class VISP_EXPORT vpDetectorQRCode : public vpDetectorBase
+{
+protected:
+ zbar::ImageScanner m_scanner; //!< QR code detector.
+
+public:
+ vpDetectorQRCode();
+ virtual ~vpDetectorQRCode() {};
+ bool detect(const vpImage<unsigned char> &I);
+};
+
+#endif
+#endif
diff --git a/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp
new file mode 100644
index 0000000..a17def7
--- /dev/null
+++ b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for bar code detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <assert.h>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_DMTX
+
+#include <dmtx.h>
+
+#include <visp3/detection/vpDetectorDataMatrixCode.h>
+
+/*!
+ Default constructor that does nothing.
+ */
+vpDetectorDataMatrixCode::vpDetectorDataMatrixCode()
+{
+}
+
+/*!
+ Detect datamatrix bar codes in the image. Return true if a bar code is detected, false otherwise.
+
+ \param I : Input image.
+ */
+bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
+{
+ bool detected = false;
+ m_message.clear();
+ m_polygon.clear();
+ m_nb_objects = 0;
+ DmtxRegion *reg;
+ DmtxDecode *dec;
+ DmtxImage *img;
+ DmtxMessage *msg;
+
+ img = dmtxImageCreate(I.bitmap, (int) I.getWidth(), (int) I.getHeight(), DmtxPack8bppK);
+ assert(img != NULL);
+
+ dec = dmtxDecodeCreate(img, 1);
+ assert(dec != NULL);
+
+ bool end = false;
+ do {
+ reg = dmtxRegionFindNext(dec, 0);
+
+ if(reg != NULL) {
+ msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
+ if(msg != NULL) {
+
+ std::vector<vpImagePoint> polygon;
+
+ DmtxVector2 p00, p10, p11, p01;
+
+ p00.X = p00.Y = p10.Y = p01.X = 0.0;
+ p10.X = p01.Y = p11.X = p11.Y = 1.0;
+ dmtxMatrix3VMultiplyBy(&p00, reg->fit2raw);
+ dmtxMatrix3VMultiplyBy(&p10, reg->fit2raw);
+ dmtxMatrix3VMultiplyBy(&p11, reg->fit2raw);
+ dmtxMatrix3VMultiplyBy(&p01, reg->fit2raw);
+
+ polygon.push_back(vpImagePoint(I.getHeight()-p00.Y, p00.X));
+ polygon.push_back(vpImagePoint(I.getHeight()-p10.Y, p10.X));
+ polygon.push_back(vpImagePoint(I.getHeight()-p11.Y, p11.X));
+ polygon.push_back(vpImagePoint(I.getHeight()-p01.Y, p01.X));
+
+ m_polygon.push_back(polygon);
+ detected = true;
+ m_message.push_back( (const char *)msg->output);
+
+ m_nb_objects ++;
+ }
+ else {
+ end = true;
+ }
+ dmtxMessageDestroy(&msg);
+ }
+ else {
+ end = true;
+ }
+ dmtxRegionDestroy(®);
+
+ } while (!end);
+
+ dmtxDecodeDestroy(&dec);
+ dmtxImageDestroy(&img);
+ return detected;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDetectorDataMatrixCode.cpp.o) has no symbols
+void dummy_vpDetectorDataMatrixCode() {};
+#endif
diff --git a/modules/detection/src/barcode/vpDetectorQRCode.cpp b/modules/detection/src/barcode/vpDetectorQRCode.cpp
new file mode 100644
index 0000000..fa3148f
--- /dev/null
+++ b/modules/detection/src/barcode/vpDetectorQRCode.cpp
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for bar code detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_ZBAR
+
+#include <visp3/detection/vpDetectorQRCode.h>
+
+
+/*!
+ Default constructor.
+ */
+vpDetectorQRCode::vpDetectorQRCode() : m_scanner()
+{
+ // configure the reader
+ m_scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+}
+
+/*!
+ Detect QR codes in the image. Return true if a code is detected, false otherwise.
+
+ \param I : Input image.
+ */
+bool vpDetectorQRCode::detect(const vpImage<unsigned char> &I)
+{
+ bool detected = false;
+ m_message.clear();
+ m_polygon.clear();
+ m_nb_objects = 0;
+
+ m_scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ // wrap image data
+ zbar::Image img(width, height, "Y800", I.bitmap, (unsigned long)(width * height));
+
+ // scan the image for barcodes
+ m_nb_objects = (size_t) m_scanner.scan(img);
+
+ // extract results
+ for(zbar::Image::SymbolIterator symbol = img.symbol_begin();
+ symbol != img.symbol_end();
+ ++symbol) {
+ m_message.push_back( symbol->get_data() );
+ detected = true;
+
+ std::vector<vpImagePoint> polygon;
+ for(unsigned int i=0; i < (unsigned int)symbol->get_location_size(); i++){
+ polygon.push_back(vpImagePoint(symbol->get_location_y(i), symbol->get_location_x(i)));
+ }
+ m_polygon.push_back(polygon);
+ }
+
+ // clean up
+ img.set_data(NULL, 0);
+
+ return detected;
+}
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDetectorQRCode.cpp.o) has no symbols
+void dummy_vpDetectorQRCode() {};
+#endif
diff --git a/modules/detection/src/face/vpDetectorFace.cpp b/modules/detection/src/face/vpDetectorFace.cpp
new file mode 100644
index 0000000..b4b6f26
--- /dev/null
+++ b/modules/detection/src/face/vpDetectorFace.cpp
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Detect faces.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
+
+#include <algorithm>
+
+#include <visp3/detection/vpDetectorFace.h>
+#include <visp3/core/vpImageConvert.h>
+
+bool vpSortLargestFace(cv::Rect rect1, cv::Rect rect2)
+{
+ return (rect1.area() > rect2.area());
+}
+
+/*!
+ Default constructor.
+ */
+vpDetectorFace::vpDetectorFace() :
+ m_faces(), m_face_cascade(), m_frame_gray()
+{
+}
+
+/*!
+ Set the name of the OpenCV cascade classifier file used for face detection.
+ \param filename : Full path to access to the file. Such a file can be found in OpenCV. Within the last versions
+ it was name "haarcascade_frontalface_alt.xml".
+ */
+void vpDetectorFace::setCascadeClassifierFile(const std::string &filename)
+{
+ if( ! m_face_cascade.load( filename ) ) {
+ throw vpException(vpException::ioError, "Cannot read haar file: %s", filename.c_str());
+ }
+}
+
+/*!
+ Allows to detect a face in the image. When more than one face is detected, faces are sorted from largest to smallest.
+
+ \param I : Input image to process.
+ \return true if one or more faces are found, false otherwise.
+
+ The number of detected faces is returned using getNbObjects().
+ If a face is found the functions getBBox(), getCog() return some information about the location of the face.
+
+ The largest face is always available using getBBox(0) or getCog(0).
+ */
+bool vpDetectorFace::detect(const vpImage<unsigned char> &I)
+{
+ vpImageConvert::convert(I, m_frame_gray);
+
+ bool detected = false;
+ m_message.clear();
+ m_polygon.clear();
+ m_nb_objects = 0;
+
+ m_faces.clear();
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ m_face_cascade.detectMultiScale( m_frame_gray, m_faces, 1.1, 2, 0, cv::Size(30, 30) );
+#else
+ m_face_cascade.detectMultiScale( m_frame_gray, m_faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
+#endif
+
+ m_nb_objects = m_faces.size();
+
+ std::sort(m_faces.begin(), m_faces.end(), vpSortLargestFace);
+
+ if (m_faces.size())
+ for( size_t i = 0; i < m_faces.size(); i++ ) {
+ std::ostringstream message;
+ message << "Face " << i;
+ m_message.push_back( message.str() );
+
+ detected = true;
+
+ std::vector<vpImagePoint> polygon;
+ double x = m_faces[i].tl().x;
+ double y = m_faces[i].tl().y;
+ double w = m_faces[i].size().width;
+ double h = m_faces[i].size().height;
+
+ polygon.push_back(vpImagePoint(y , x ));
+ polygon.push_back(vpImagePoint(y+h, x ));
+ polygon.push_back(vpImagePoint(y+h, x+w));
+ polygon.push_back(vpImagePoint(y , x+w));
+
+ m_polygon.push_back(polygon);
+ }
+
+ return detected;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDetectorFace.cpp.o) has no symbols
+void dummy_vpDetectorFace() {};
+#endif
diff --git a/modules/detection/src/vpDetectorBase.cpp b/modules/detection/src/vpDetectorBase.cpp
new file mode 100644
index 0000000..bbc43ef
--- /dev/null
+++ b/modules/detection/src/vpDetectorBase.cpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base class for object detection.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/detection/vpDetectorBase.h>
+
+ /*!
+ Default constructor.
+ */
+vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0)
+{}
+
+/*!
+ Returns ith object container box as a vector of points.
+*/
+std::vector<vpImagePoint> &
+vpDetectorBase::getPolygon(size_t i)
+{
+ if (i < m_polygon.size())
+ return m_polygon[i];
+ else
+ throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i, m_polygon.size()));
+}
+
+/*!
+ Returns the contained message of the ith object if there is one.
+*/
+std::string &
+vpDetectorBase::getMessage(size_t i)
+{
+ if (i < m_polygon.size())
+ return m_message[i];
+ else
+ throw(vpException(vpException::badValue, "Bad index to retrieve object %d . Only %d objects are detected.", i, m_polygon.size()));
+}
+
+/*!
+ Return the center of gravity location of the ith object.
+*/
+vpImagePoint
+vpDetectorBase::getCog(size_t i) const
+{
+ vpImagePoint cog(0, 0);
+ for (size_t j = 0; j < m_polygon[i].size(); j++) {
+ cog += m_polygon[i][j];
+ }
+ cog /= (double)m_polygon[i].size();
+ return cog;
+}
+
+/*!
+Return the bounding box of the ith object.
+*/
+vpRect
+vpDetectorBase::getBBox(size_t i) const
+{
+ assert(m_polygon[i].size() > 2);
+
+ double left, right;
+ double top, bottom;
+ left = right = m_polygon[i][0].get_u();
+ top = bottom = m_polygon[i][0].get_v();
+ for (size_t j = 0; j < m_polygon[i].size(); j++) {
+ double u = m_polygon[i][j].get_u();
+ double v = m_polygon[i][j].get_v();
+ if (u < left) left = u;
+ if (u > right) right = u;
+ if (v < top) top = v;
+ if (v > bottom) bottom = v;
+ }
+ vpRect roi(vpImagePoint(top, left), vpImagePoint(bottom, right));
+ return roi;
+}
diff --git a/modules/gui/CMakeLists.txt b/modules/gui/CMakeLists.txt
new file mode 100644
index 0000000..8a47c7c
--- /dev/null
+++ b/modules/gui/CMakeLists.txt
@@ -0,0 +1,68 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+# Display X11, GTK, GDI, D3D9. OpenCV already handled by core module
+if(USE_X11)
+ list(APPEND opt_incs ${X11_INCLUDE_DIR})
+ list(APPEND opt_libs ${X11_LIBRARIES})
+endif()
+if(USE_GTK2)
+ list(APPEND opt_incs ${GTK2_INCLUDE_DIRS})
+ list(APPEND opt_libs ${GTK2_LIBRARIES})
+ # FS next lines removed until they proved to be useful
+# if(UNIX)
+# ## ignore gtk pedantic warnings by gcc specific -isystem :
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${GTK2_gtk_INCLUDE_PATH}")
+# endif()
+endif()
+if(USE_GDI)
+ list(APPEND opt_libs ${GDI_LIBRARIES})
+endif()
+if(USE_DIRECT3D)
+ list(APPEND opt_incs ${DIRECT3D_INCLUDE_DIRS})
+ list(APPEND opt_libs ${DIRECT3D_LIBRARIES})
+endif()
+
+vp_add_module(gui visp_core)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+vp_add_tests(DEPENDS_ON visp_io)
diff --git a/modules/gui/include/visp3/gui/vpD3DRenderer.h b/modules/gui/include/visp3/gui/vpD3DRenderer.h
new file mode 100644
index 0000000..ff7998e
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpD3DRenderer.h
@@ -0,0 +1,207 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * D3D renderer for windows 32 display
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_D3D9) )
+
+#ifndef VPD3DRENDERER_HH
+#define VPD3DRENDERER_HH
+
+#include <windows.h>
+#include <d3dx9.h>
+#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/core/vpDisplayException.h>
+
+
+#include <iostream>
+
+/*!
+ \class vpD3DRenderer.h
+
+ \brief Display under windows using Direct3D9.
+ Is used by vpD3DDisplay to do the drawing.
+
+*/
+class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer
+{
+
+ IDirect3D9 * pD3D;
+
+ //The d3d device we will be working with.
+ IDirect3DDevice9 * pd3dDevice;
+
+ //Sprite used to render the texture.
+ ID3DXSprite * pSprite;
+
+ //The system memory texture :
+ //The one we will be drawing on.
+ IDirect3DTexture9 * pd3dText;
+
+ //The video memory texture :
+ //The one we will use for display.
+ IDirect3DTexture9 * pd3dVideoText;
+
+ //The texture's width.
+ unsigned int textWidth;
+
+ //The window's handle.
+ HWND hWnd;
+
+ //Colors for overlay drawn with d3d directly.
+ unsigned long colors[vpColor::id_unknown];
+
+ //Colors for overlay drawn with GDI.
+ COLORREF colorsGDI[vpColor::id_unknown];
+
+ //Font used for text drawing.
+ HFONT hFont;
+
+ public:
+
+ bool init(HWND hwnd, unsigned int width, unsigned int height);
+ bool render();
+
+ vpD3DRenderer();
+ virtual ~vpD3DRenderer();
+
+ void setImg(const vpImage<vpRGBa>& im);
+ void setImg(const vpImage<unsigned char>& im);
+ void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+ void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+
+ void setPixel(const vpImagePoint &iP, const vpColor &color);
+
+ void drawLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness, int style=PS_SOLID);
+
+ void drawRect(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill=false,
+ unsigned int thickness=1);
+
+ void clear(const vpColor &color);
+
+ void drawCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill=false, unsigned int thickness=1);
+
+ void drawText(const vpImagePoint &ip, const char * text,
+ const vpColor &color);
+
+ void drawCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1);
+
+ void drawArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness=1);
+
+ void getImage(vpImage<vpRGBa> &I);
+
+
+
+ private:
+
+ void initView(float, float);
+
+ /*!
+ Sub function for circle drawing.
+ Circle drawing is based on Bresenham 's circle algorithm.
+ */
+ void subDrawCircle(int i, int j,
+ int x, int y,
+ vpColor col, unsigned char* buf,
+ unsigned int pitch, unsigned int maxX, unsigned int maxY);
+
+
+ /*!
+ Safe useful inline function to set a pixel in a texture buffer.
+ \param buf The texture's buffer.
+ \param pitch The image pitch.
+ \param x The x-coordinate of the pixel (in the locked rectangle base)
+ \param y The y-coordinate of the pixel (in the locked rectangle base)
+ \param color The color of the pixel.
+ \param maxX The maximum x value (equals to the width of the locked rectangle).
+ \param maxY The maximum y value (equals to the height of the locked rectangle).
+
+ */
+ inline void setBufferPixel(unsigned char* buf, unsigned int pitch,
+ int x, int y,
+ const vpColor &color,
+ unsigned int maxX, unsigned int maxY)
+ {
+ unsigned long c;
+ if (color.id < vpColor::id_unknown)
+ c = colors[color.id];
+ else {
+ c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
+ }
+
+ if(x>=0 && y>=0 && x<= (int)maxX && y<= (int)maxY)
+ *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
+ }
+ /*!
+ Unsafe useful inline function to set a pixel in a texture buffer.
+ \param buf The texture's buffer.
+ \param pitch The image pitch.
+ \param x The x-coordinate of the pixel (in the locked rectangle base)
+ \param y The y-coordinate of the pixel (in the locked rectangle base)
+ \param color The color of the pixel.
+
+ */
+ inline void setBufferPixel(unsigned char* buf, unsigned int pitch,
+ int x, int y,
+ const vpColor &color)
+ {
+ unsigned long c;
+ if (color.id < vpColor::id_unknown)
+ c = colors[color.id];
+ else {
+ c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
+ }
+
+ *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
+ }
+
+ unsigned int supPowerOf2(unsigned int n);
+
+};
+#endif
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayD3D.h b/modules/gui/include/visp3/gui/vpDisplayD3D.h
new file mode 100644
index 0000000..dd74303
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayD3D.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display using D3D
+ *
+ * Authors:
+ * Bruno Renier
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDisplay.h>
+#if ( defined(VISP_HAVE_D3D9) )
+
+#ifndef VPDISPLAYD3D_HH
+#define VPDISPLAYD3D_HH
+
+
+#include <visp3/gui/vpDisplayWin32.h>
+
+/*!
+ \class vpDisplayD3D
+
+ \ingroup group_gui_display
+
+ \brief Display for windows using Direct3D.
+
+ Direct3D is part of the DirectX API available under Windows
+ operating systems.
+
+ \warning Requires DirectX9 SDK to compile and DirectX9 DLLs to run.
+
+ The example below shows how to display an image with this video device.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+int main()
+{
+#if defined(VISP_HAVE_D3D9)
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+ vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
+
+ vpDisplayD3D d;
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+ d.init(I);
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My Direct 3D display");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(10);
+ topLeftCorner.set_j(20);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::red, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Wait for a click in the display window
+ vpDisplay::getClick(I);
+#endif
+}
+ \endcode
+*/
+class VISP_EXPORT vpDisplayD3D : public vpDisplayWin32
+{
+public:
+ vpDisplayD3D();
+ vpDisplayD3D(int winx, int winy, const char *title=NULL);
+ vpDisplayD3D(vpImage<vpRGBa> &I,int winx=-1, int winy=-1, const char *title=NULL);
+ vpDisplayD3D(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const char *title=NULL);
+
+ virtual ~vpDisplayD3D();
+
+};
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayGDI.h b/modules/gui/include/visp3/gui/vpDisplayGDI.h
new file mode 100644
index 0000000..75cfad2
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayGDI.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display using GDI
+ *
+ * Authors:
+ * Bruno Renier
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDisplay.h>
+
+#if ( defined(VISP_HAVE_GDI) )
+
+#ifndef vpDisplayGDI_HH
+#define vpDisplayGDI_HH
+
+#include <visp3/gui/vpDisplayWin32.h>
+
+/*!
+ \class vpDisplayGDI
+
+ \ingroup group_gui_display
+
+ \brief Display for windows using GDI (available on any windows 32 platform).
+
+ GDI stands for Graphics Device Interface and is a core component of Microsoft
+ Windows operating systems used for displaying graphics in a window.
+
+ The example below shows how to display an image with this video device.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGDI.h>
+
+int main()
+{
+#if defined(VISP_HAVE_GDI)
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+#ifdef _WIN32
+ vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
+#else
+ vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
+#endif
+
+ vpDisplayGDI d;
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+ d.init(I);
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My GDI display");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(50);
+ topLeftCorner.set_j(10);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Get non blocking keyboard events
+ std::cout << "Check keyboard events..." << std::endl;
+ char key[10];
+ bool ret;
+ for (int i=0; i< 200; i++) {
+ bool ret = vpDisplay::getKeyboardEvent(I, key, false);
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ vpTime::wait(40);
+ }
+
+ // Get a blocking keyboard event
+ std::cout << "Wait for a keyboard event..." << std::endl;
+ ret = vpDisplay::getKeyboardEvent(I, key, true);
+ std::cout << "keyboard event: " << ret << std::endl;
+ if (ret)
+ std::cout << "key: " << "\"" << key << "\"" << std::endl;
+
+ // Wait for a click in the display window
+ std::cout << "Wait for a button click..." << std::endl;
+ vpDisplay::getClick(I);
+#endif
+}
+ \endcode
+*/
+class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32
+{
+public:
+ vpDisplayGDI();
+ vpDisplayGDI(int winx, int winy, const char *title=NULL);
+ vpDisplayGDI(vpImage<vpRGBa> &I,int winx=-1, int winy=-1, const char *title=NULL);
+ vpDisplayGDI(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const char *title=NULL);
+
+ virtual ~vpDisplayGDI();
+};
+
+#endif
+#endif
+
diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h
new file mode 100644
index 0000000..f292edf
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h
@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Christophe Collewet
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpDisplayGTK_h
+#define vpDisplayGTK_h
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_GTK) )
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+
+#include <gtk/gtk.h>
+#include <gdk/gdkrgb.h>
+
+
+/*!
+ \file vpDisplayGTK.h
+ \brief Define the GTK console to display images.
+*/
+
+
+/*!
+
+ \class vpDisplayGTK
+
+ \ingroup group_gui_display
+
+ \brief The vpDisplayGTK allows to display image using the GTK+ library
+ version 1.2.
+
+ The GTK+ 1.2 library has to be available on the system.
+
+ The example below shows how to display an image with this video device.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+#if defined(VISP_HAVE_GTK)
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+#ifdef _WIN32
+ vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
+#else
+ vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
+#endif
+
+ vpDisplayGTK d;
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+ d.init(I);
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My GTK display");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(50);
+ topLeftCorner.set_j(10);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Get non blocking keyboard events
+ std::cout << "Check keyboard events..." << std::endl;
+ char key[10];
+ bool ret;
+ for (int i=0; i< 200; i++) {
+ bool ret = vpDisplay::getKeyboardEvent(I, key, false);
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ vpTime::wait(40);
+ }
+
+ // Get a blocking keyboard event
+ std::cout << "Wait for a keyboard event..." << std::endl;
+ ret = vpDisplay::getKeyboardEvent(I, key, true);
+ std::cout << "keyboard event: " << ret << std::endl;
+ if (ret)
+ std::cout << "key: " << "\"" << key << "\"" << std::endl;
+
+ // Wait for a click in the display window
+ std::cout << "Wait for a button click..." << std::endl;
+ vpDisplay::getClick(I);
+#endif
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpDisplayGTK: public vpDisplay
+{
+private:
+ //! true if GTK display is ready to use
+ GtkWidget *widget;
+ GdkPixmap *background;
+ GdkGC *gc;
+ GdkColor blue,red,yellow,green,cyan,orange,white, black, gdkcolor,
+ lightBlue, darkBlue, lightRed, darkRed,lightGreen, darkGreen,
+ purple, lightGray, gray, darkGray;
+ GdkColormap *colormap;
+
+ GdkFont *font;
+ guchar *vectgtk;
+ GdkColor **col ;
+ int ncol, nrow ;
+
+ typedef enum {
+ id_black=0,
+ id_white,
+ id_lightGray,
+ id_gray,
+ id_darkGray,
+ id_lightRed,
+ id_red,
+ id_darkRed,
+ id_lightGreen,
+ id_green,
+ id_darkGreen,
+ id_lightBlue,
+ id_blue,
+ id_darkBlue,
+ id_yellow,
+ id_cyan,
+ id_orange,
+ id_purple,
+ id_npredefined // Number of predefined colors
+ } vpColorIdentifier;
+
+public:
+ vpDisplayGTK() ;
+ vpDisplayGTK(int winx, int winy, const char *title=NULL) ;
+ vpDisplayGTK(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ vpDisplayGTK(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ virtual ~vpDisplayGTK() ;
+
+ void init(vpImage<unsigned char> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ void init(vpImage<vpRGBa> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ void init(unsigned int width, unsigned int height,
+ int winx=-1, int winy=-1 ,
+ const char *title=NULL) ;
+
+ unsigned int getScreenDepth();
+ void getScreenSize(unsigned int &width, unsigned int &height);
+ void getImage(vpImage<vpRGBa> &I) ;
+
+protected:
+
+ void setFont( const char *fontname );
+ void setTitle(const char *title) ;
+ void setWindowPosition(int winx, int winy);
+
+ void clearDisplay(const vpColor &color=vpColor::white) ;
+
+ void closeDisplay() ;
+
+ void displayArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4,unsigned int h=2,
+ unsigned int thickness=1) ;
+ void displayCharString(const vpImagePoint &ip, const char *text,
+ const vpColor &color=vpColor::green) ;
+
+ void displayCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ void displayCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1) ;
+ void displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayImage(const vpImage<vpRGBa> &I) ;
+ void displayImage(const vpImage<unsigned char> &I) ;
+ void displayImage(const unsigned char *I) ;
+
+ void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
+ void displayRectangle(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+
+ void flushDisplay() ;
+ void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) ;
+
+ bool getClick(bool blocking=true) ;
+ bool getClick(vpImagePoint &ip, bool blocking=true) ;
+ bool getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) ;
+ bool getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) ;
+ bool getKeyboardEvent(bool blocking=true);
+ bool getKeyboardEvent(char *string, bool blocking=true);
+ bool getPointerMotionEvent (vpImagePoint &ip);
+ bool getPointerPosition (vpImagePoint &ip);
+
+ inline unsigned int getWidth() const { return width ; }
+ inline unsigned int getHeight() const { return height ; }
+} ;
+
+#endif
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h
new file mode 100644
index 0000000..c706947
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h
@@ -0,0 +1,304 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Christophe Collewet
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpDisplayOpenCV_h
+#define vpDisplayOpenCV_h
+
+#include <visp3/core/vpConfig.h>
+#if defined(VISP_HAVE_OPENCV)
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020101
+# include <opencv2/core/core.hpp>
+# include <opencv2/highgui/highgui.hpp>
+#else
+# include <cv.h>
+# include <highgui.h>
+# include <cxcore.h>
+#endif
+
+/*!
+ \file vpDisplayOpenCV.h
+ \brief Define the OpenCV console to display images.
+*/
+
+
+/*!
+
+ \class vpDisplayOpenCV
+
+ \ingroup group_gui_display
+
+ \brief The vpDisplayOpenCV allows to display image using the opencv library.
+
+ The example below shows how to display an image with this video device.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+#if defined(VISP_HAVE_OPENCV)
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+ vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
+
+ vpDisplayOpenCV d;
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+ d.init(I);
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My OpenCV display");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(10);
+ topLeftCorner.set_j(50);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Get non blocking keyboard events
+ std::cout << "Check keyboard events..." << std::endl;
+ char key[10];
+ bool ret;
+ for (int i=0; i< 200; i++) {
+ bool ret = vpDisplay::getKeyboardEvent(I, key, false);
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ vpTime::wait(40);
+ }
+
+ // Get a blocking keyboard event
+ std::cout << "Wait for a keyboard event..." << std::endl;
+ ret = vpDisplay::getKeyboardEvent(I, key, true);
+ std::cout << "keyboard event: " << ret << std::endl;
+ if (ret)
+ std::cout << "key: " << "\"" << key << "\"" << std::endl;
+
+ // Wait for a click in the display window
+ std::cout << "Wait for a button click..." << std::endl;
+ vpDisplay::getClick(I);
+#endif
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpDisplayOpenCV: public vpDisplay
+{
+private:
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ //! true if OpenCV display is ready to use
+ IplImage* background;
+ CvScalar *col ;
+ CvScalar cvcolor;
+ CvFont *font;
+#else
+ cv::Mat background;
+ cv::Scalar *col ;
+ cv::Scalar cvcolor;
+ int font;
+ float fontScale;
+#endif
+ static std::vector<std::string> m_listTitles;
+ static unsigned int m_nbWindows ;
+ int fontHeight;
+ int x_move;
+ int y_move;
+ bool move;
+ int x_lbuttondown ;
+ int y_lbuttondown ;
+ bool lbuttondown;
+ int x_mbuttondown ;
+ int y_mbuttondown ;
+ bool mbuttondown;
+ int x_rbuttondown ;
+ int y_rbuttondown ;
+ bool rbuttondown;
+ int x_lbuttonup ;
+ int y_lbuttonup ;
+ bool lbuttonup;
+ int x_mbuttonup ;
+ int y_mbuttonup ;
+ bool mbuttonup;
+ int x_rbuttonup ;
+ int y_rbuttonup ;
+ bool rbuttonup;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpDisplayOpenCV(const vpDisplayOpenCV &)
+// : vpDisplay(),
+// #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+// background(NULL), col(NULL), cvcolor(), font(NULL),
+// #else
+// background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
+// #endif
+// fontHeight(10), x_move(0), y_move(0) , move(false),
+// x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+// x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+// x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+// x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+// x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+// x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpDisplayOpenCV &operator=(const vpDisplayOpenCV &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpDisplayOpenCV() ;
+ vpDisplayOpenCV(int winx, int winy, const char *title=NULL) ;
+ vpDisplayOpenCV(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ vpDisplayOpenCV(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ virtual ~vpDisplayOpenCV() ;
+
+ void init(vpImage<unsigned char> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ void init(vpImage<vpRGBa> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ void init(unsigned int width, unsigned int height,
+ int winx=-1, int winy=-1 ,
+ const char *title=NULL) ;
+ void getImage(vpImage<vpRGBa> &I) ;
+
+protected:
+ void setFont( const char *font );
+ void setTitle(const char *title) ;
+ void setWindowPosition(int winx, int winy);
+
+ void clearDisplay(const vpColor &color=vpColor::white) ;
+
+ void closeDisplay() ;
+
+ void displayArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4,unsigned int h=2,
+ unsigned int thickness=1) ;
+
+ void displayCharString(const vpImagePoint &ip, const char *text,
+ const vpColor &color=vpColor::green) ;
+
+ void displayCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ void displayCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1) ;
+ void displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayImage(const vpImage<vpRGBa> &I) ;
+ void displayImage(const vpImage<unsigned char> &I) ;
+ void displayImage(const unsigned char *I) ;
+
+ void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+ void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
+
+ void displayRectangle(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+
+ void flushDisplay() ;
+ void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ bool getClick(bool blocking=true) ;
+ bool getClick(vpImagePoint &ip, bool blocking=true) ;
+ bool getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) ;
+ bool getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) ;
+
+ inline unsigned int getWidth() const { return width ; }
+ inline unsigned int getHeight() const { return height ; }
+
+ bool getKeyboardEvent(bool blocking=true);
+ bool getKeyboardEvent(char *string, bool blocking=true);
+ bool getPointerMotionEvent (vpImagePoint &ip);
+ bool getPointerPosition (vpImagePoint &ip);
+
+ static void on_mouse( int event, int x, int y, int flags, void* param );
+} ;
+
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayWin32.h b/modules/gui/include/visp3/gui/vpDisplayWin32.h
new file mode 100644
index 0000000..7ac76f5
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayWin32.h
@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display base class
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+
+#ifndef vpDisplayWin32_hh
+#define vpDisplayWin32_hh
+
+#include <string>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <windows.h>
+#include <visp3/gui/vpWin32Window.h>
+#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImagePoint.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+/*!
+ Used to pass parameters to the window's thread.
+*/
+struct threadParam
+{
+ //! Pointer to the display associated with the window.
+ vpDisplayWin32 * vpDisp;
+
+ //! X position of the window.
+ int x;
+
+ //! Y position of the window.
+ int y;
+
+ //! Width of the window's client area.
+ unsigned int w;
+
+ //! Height of the window's client area.
+ unsigned int h;
+
+ //! Title of the window.
+ std::string title;
+};
+#endif /* DOXYGEN_SHOULD_SKIP_THIS */
+
+/*!
+ \class vpDisplayWin32
+
+ \brief Base abstract class for Windows 32 displays.
+ Implements the window creation in a separate thread
+ and the associated event handling functions for
+ Windows 32 displays.
+ Uses calls to a renderer to do some display.
+ (i.e. all display methods are implemented in the renderer)
+
+ \author Bruno Renier
+*/
+class VISP_EXPORT vpDisplayWin32 : public vpDisplay
+{
+ protected:
+ //! Maximum delay for window initialization
+ static const int MAX_INIT_DELAY;
+
+ //! Handle of the window's thread.
+ HANDLE hThread;
+
+ //! Id of the window's thread.
+ DWORD threadId;
+
+ //! Initialization status.
+ bool iStatus;
+
+ //! The window.
+ vpWin32Window window;
+
+ //!
+ RECT roi;
+
+ //! Function used to launch the window in a thread.
+ friend void vpCreateWindow(threadParam * param);
+
+ public:
+
+ vpDisplayWin32(vpWin32Renderer * rend = NULL);
+
+ vpDisplayWin32(vpImage<vpRGBa> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL);
+
+ vpDisplayWin32(vpImage<unsigned char> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL);
+
+ virtual ~vpDisplayWin32();
+
+ void setTitle(const char *windowtitle);
+ void setFont( const char *fontname );
+ void setWindowPosition(int winx, int winy);
+ void clearDisplay(const vpColor &color=vpColor::white);
+ void closeDisplay();
+ void displayImage(const vpImage<vpRGBa> &I);
+ void displayImage(const vpImage<unsigned char> &I);
+
+ void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void flushDisplay();
+ void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void init(vpImage<unsigned char> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ void init(vpImage<vpRGBa> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ void init(unsigned int width, unsigned int height,
+ int winx=-1, int winy=-1 ,
+ const char *title=NULL) ;
+
+ void getImage(vpImage<vpRGBa> &I);
+
+ protected:
+
+ void displayArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4,unsigned int h=2,
+ unsigned int thickness=1) ;
+
+ void displayCharString(const vpImagePoint &ip, const char *text,
+ const vpColor &color=vpColor::green) ;
+
+ void displayCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+
+ void displayCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1);
+
+
+ void displayPoint(const vpImagePoint &ip, const vpColor &color);
+
+ void displayRectangle(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1);
+ void displayRectangle(const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+
+ bool getClick( bool blocking=true);
+
+ bool getClick(vpImagePoint &ip, bool blocking=true);
+
+ bool getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button, bool blocking=true);
+
+ bool getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true);
+ bool getKeyboardEvent( bool blocking=true);
+ bool getKeyboardEvent(char *string, bool blocking);
+ bool getPointerMotionEvent (vpImagePoint &ip);
+ bool getPointerPosition (vpImagePoint &ip);
+
+ void waitForInit();
+
+};
+#endif
+#endif
+
diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h
new file mode 100644
index 0000000..e9e97c1
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpDisplayX.h
@@ -0,0 +1,288 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpDisplayX_h
+#define vpDisplayX_h
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDisplay.h>
+#ifdef VISP_HAVE_X11
+
+//namespace X11name
+//{
+#include <X11/Xlib.h>
+#include <X11/Xutil.h>
+//#include <X11/Xatom.h>
+//#include <X11/cursorfont.h>
+//} ;
+
+//using namespace X11name ;
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRect.h>
+
+
+
+/*!
+ \file vpDisplayX.h
+ \brief Define the X11 console to display images.
+*/
+
+
+/*!
+ \class vpDisplayX
+
+ \ingroup group_gui_display
+
+ \brief Define the X11 console to display images.
+
+ \note Ready to use with a Unix System (tested for Linux and SunOS)
+
+ This class define the X11 console to display images
+ It also define method to display some geometric feature (point, line, circle)
+ in the image.
+
+ The example below shows how to display an image with this video device.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImage<unsigned char> I; // Grey level image
+
+ // Read an image in PGM P5 format
+ vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX d;
+
+ // Initialize the display with the image I. Display and image are
+ // now link together.
+ d.init(I);
+#endif
+
+ // Specify the window location
+ vpDisplay::setWindowPosition(I, 400, 100);
+
+ // Set the display window title
+ vpDisplay::setTitle(I, "My X11 display");
+
+ // Set the display background with image I content
+ vpDisplay::display(I);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
+
+ // Draw a red rectangle in the display overlay (foreground)
+ vpImagePoint topLeftCorner;
+ topLeftCorner.set_i(50);
+ topLeftCorner.set_j(10);
+ vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
+
+ // Flush the foreground and background display
+ vpDisplay::flush(I);
+
+ // Get non blocking keyboard events
+ std::cout << "Check keyboard events..." << std::endl;
+ char key[10];
+ bool ret;
+ for (int i=0; i< 200; i++) {
+ bool ret = vpDisplay::getKeyboardEvent(I, key, false);
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ vpTime::wait(40);
+ }
+
+ // Get a blocking keyboard event
+ std::cout << "Wait for a keyboard event..." << std::endl;
+ ret = vpDisplay::getKeyboardEvent(I, key, true);
+ std::cout << "keyboard event: " << ret << std::endl;
+ if (ret)
+ std::cout << "key: " << "\"" << key << "\"" << std::endl;
+
+ // Wait for a click in the display window
+ std::cout << "Wait for a button click..." << std::endl;
+ vpDisplay::getClick(I);
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpDisplayX: public vpDisplay
+{
+private:
+ Display *display ;
+ Window window ;
+ XImage *Ximage ;
+ Colormap lut ;
+ GC context ;
+ int screen ;
+ XEvent event;
+ Pixmap pixmap;
+ unsigned long *x_color; // Array of predefined colors
+ unsigned int screen_depth ;
+ unsigned short colortable[256];
+ XColor xcolor;
+ XGCValues values;
+ bool ximage_data_init;
+ unsigned int RMask, GMask, BMask;
+ int RShift, GShift, BShift;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpDisplayX(const vpDisplayX &)
+// : vpDisplay(), display(NULL), window(), Ximage(NULL), lut(), context(), screen(), event(), pixmap(),
+// x_color(NULL), screen_depth(8), xcolor(), values(), ximage_data_init(false),
+// RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpDisplayX &operator=(const vpDisplayX &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpDisplayX() ;
+ vpDisplayX(int winx, int winy, const char *title=NULL) ;
+ vpDisplayX(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ vpDisplayX(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ virtual ~vpDisplayX() ;
+
+ void init(vpImage<unsigned char> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+ void init(vpImage<vpRGBa> &I,
+ int winx=-1, int winy=-1,
+ const char *title=NULL) ;
+
+ void init(unsigned int width, unsigned int height,
+ int winx=-1, int winy=-1 ,
+ const char *title=NULL) ;
+
+ unsigned int getScreenDepth();
+ void getScreenSize(unsigned int &width, unsigned int &height);
+ void getImage(vpImage<vpRGBa> &I) ;
+
+protected:
+
+ void setFont( const char *font );
+ void setTitle(const char *title) ;
+ void setWindowPosition(int winx, int winy);
+
+ void clearDisplay(const vpColor &color=vpColor::white) ;
+
+ void closeDisplay() ;
+
+ void displayArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color=vpColor::white,
+ unsigned int w=4,unsigned int h=2,
+ unsigned int thickness=1) ;
+
+ void displayCharString(const vpImagePoint &ip, const char *text,
+ const vpColor &color=vpColor::green) ;
+
+ void displayCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color,
+ bool fill = false,
+ unsigned int thickness=1);
+ void displayCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1) ;
+ void displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+
+ void displayImage(const vpImage<vpRGBa> &I) ;
+ void displayImage(const vpImage<unsigned char> &I) ;
+ void displayImage(const unsigned char *I) ;
+
+ void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness=1) ;
+ void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
+
+ void displayRectangle(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+ void displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill = false,
+ unsigned int thickness=1) ;
+
+ void flushDisplay() ;
+ void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ bool getClick(bool blocking=true) ;
+ bool getClick(vpImagePoint &ip, bool blocking=true);
+ bool getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true) ;
+ bool getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking=true);
+
+ bool getKeyboardEvent(bool blocking=true);
+ bool getKeyboardEvent(char *string, bool blocking=true);
+
+ int getMsb(unsigned int u32val);
+ bool getPointerMotionEvent (vpImagePoint &ip);
+ bool getPointerPosition (vpImagePoint &ip);
+
+ inline unsigned int getWidth() const { return width ; }
+ inline unsigned int getHeight() const { return height ; }
+
+
+} ;
+
+
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpPlot.h b/modules/gui/include/visp3/gui/vpPlot.h
new file mode 100644
index 0000000..8733a5f
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpPlot.h
@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Plot curves.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPlot.h
+ \brief Plot curves.
+*/
+
+#ifndef vpPlot_H
+#define vpPlot_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpPlotGraph.h>
+
+/*!
+ \class vpPlot
+ \ingroup group_gui_plotter
+
+ \brief This class enables real time drawing of 2D or 3D graphics. An
+ instance of the class open a window which contains between 1 and 4
+ graphics. Each one contains a desired number of curves.
+
+ \warning This class is only available if one of the display functionalities (X11, GDI, GTK, OpenCV or Direct3D)
+ is available. In visp3/core/vpConfig.h header file, you should have VISP_HAVE_DISPLAY define.
+
+ The example below shows how to use the vpPlot class. An other example provided in tutoral-ibvs-plotter.cpp
+ and described in \ref tutorial-plotter shows how to use this class to plot in real-time some curves during
+ an image-based visual servo.
+
+ \code
+#include <visp3/gui/vpPlot.h>
+
+int main ()
+{
+#if defined(VISP_HAVE_DISPLAY)
+ // Create a window (700 by 700) at position (100, 200) with two graphics
+ vpPlot A(2, 700, 700, 100, 200, "Curves...");
+
+ // The first graphic contains 1 curve and the second graphic contains 2 curves
+ A.initGraph(0,1);
+ A.initGraph(1,2);
+
+ // The color of the curve in the first graphic is red
+ A.setColor(0,0,vpColor::red);
+ // The first curve in the second graphic is green
+ A.setColor(1,0,vpColor::green);
+ // The second curve in the second graphic is blue
+ A.setColor(1,1,vpColor::blue);
+
+ // Add the point (0,0) in the first graphic
+ A.plot(0,0,0,0);
+
+ // Add the point (0,1) to the first curve of the second graphic
+ A.plot(1,0,0,1);
+
+ // Add the point (0,2) to the second curve of the second graphic
+ A.plot(1,1,0,2);
+
+ for (int i = 0; i < 50; i++) {
+ // Add the point (i,sin(i*pi/10) in the first graphic
+ A.plot(0,0,i,sin(i*M_PI/10));
+
+ // Add the point (i,1) to the first curve of the second graphic
+ A.plot(1,0,i,1);
+
+ // Add the point (i,2) to the second curve of the second graphic
+ A.plot(1,1,i,2);
+ }
+
+ return 0;
+#endif
+}
+ \endcode
+*/
+
+#if defined(VISP_HAVE_DISPLAY)
+
+class VISP_EXPORT vpPlot
+{
+ public:
+ vpImage<unsigned char> I;
+
+ private:
+ vpDisplay *display;
+
+ unsigned int graphNbr;
+ vpPlotGraph* graphList;
+
+ unsigned int margei;
+ unsigned int margej;
+
+ float factori;
+ float factorj;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpPlot(const vpPlot &)
+// : I(), display(NULL), graphNbr(0), graphList(NULL), margei(0), margej(0),
+// factori(0), factorj(0)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpPlot &operator=(const vpPlot &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpPlot();
+ vpPlot(const unsigned int nbGraph,
+ const unsigned int height=700,
+ const unsigned int width=700,
+ const int x=-1, const int y=-1, const char *title=NULL);
+ ~vpPlot();
+ void getPixelValue(const bool block);
+ void init(const unsigned int nbGraph,
+ const unsigned int height=700,
+ const unsigned int width=700,
+ const int x=-1, const int y=-1, const char *title=NULL);
+ void initGraph (unsigned int graphNum, unsigned int curveNbr);
+
+ void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax);
+ void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax);
+ void navigate (void);
+
+ void plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y);
+ void plot(const unsigned int graphNum, const double x, const vpColVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpRowVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpPoseVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpTranslationVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpRotationVector &v_y);
+ vpMouseButton::vpMouseButtonType plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z);
+ vpMouseButton::vpMouseButtonType plot(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z);
+
+ void resetPointList (const unsigned int graphNum);
+ void resetPointList (const unsigned int graphNum, const unsigned int curveNum);
+
+ void saveData(const unsigned int graphNum, const char* dataFile);
+ void setColor (const unsigned int graphNum, const unsigned int curveNum, vpColor color);
+ void setGraphThickness (const unsigned int graphNum, const unsigned int thickness);
+ void setGridThickness (const unsigned int graphNum, const unsigned int thickness);
+ /*!
+ Set the font of the characters. The display should be initialized before.
+
+ To know which font are available, on Unix you can use xfontsel or xlsfonts utilities.
+ */
+ void setFont(const char *font)
+ {
+ if (display->isInitialised())
+ vpDisplay::setFont(I, font);
+ }
+ void setLegend (const unsigned int graphNum, const unsigned int curveNum, const char *legend);
+ void setTitle (const unsigned int graphNum, const char *title);
+ void setUnitX (const unsigned int graphNum, const char *unitx);
+ void setUnitY (const unsigned int graphNum, const char *unity);
+ void setUnitZ (const unsigned int graphNum, const char *unitz);
+ void setThickness (const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness);
+
+ private:
+ void initNbGraph (unsigned int nbGraph);
+ void displayGrid();
+};
+#endif
+
+#endif
diff --git a/modules/gui/include/visp3/gui/vpPlotCurve.h b/modules/gui/include/visp3/gui/vpPlotCurve.h
new file mode 100644
index 0000000..be877d7
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpPlotCurve.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Define a curve for the vpPlot class.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#ifndef vpPlotCurve_H
+#define vpPlotCurve_H
+
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRect.h>
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPoint.h>
+
+#include <list>
+
+#if defined(VISP_HAVE_DISPLAY)
+
+//! Different styles to plot the curve.
+typedef enum
+{
+ point,
+ line,
+ dashed_line,
+ marker
+} vpCurveStyle;
+
+class vpPlotCurve
+{
+ public:
+ vpColor color;
+ vpCurveStyle curveStyle;
+ unsigned int thickness ;
+ //vpMarkerStyle markerStyle;
+ //char lineStyle[20];
+ //vpList<vpImagePoint> pointList;
+ unsigned int nbPoint;
+ vpImagePoint lastPoint;
+ std::list<double> pointListx;
+ std::list<double> pointListy;
+ std::list<double> pointListz;
+ char legend[256];
+ double xmin;
+ double xmax;
+ double ymin;
+ double ymax;
+
+ public:
+ vpPlotCurve();
+ ~vpPlotCurve();
+ void plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y);
+ void plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy);
+};
+
+#endif
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpPlotGraph.h b/modules/gui/include/visp3/gui/vpPlotGraph.h
new file mode 100644
index 0000000..30e3268
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpPlotGraph.h
@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Define a graph for the vpPlot class.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#ifndef vpPlotGraph_H
+#define vpPlotGraph_H
+
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+#include <visp3/gui/vpPlotCurve.h>
+#include <visp3/core/vpMouseButton.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRect.h>
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPoint.h>
+
+#if defined(VISP_HAVE_DISPLAY)
+
+class vpPlotGraph
+{
+ public:
+ double xorg;
+ double yorg;
+ double zoomx;
+ double zoomy;
+ double xmax;
+ double ymax;
+ double xmin;
+ double ymin;
+ double xdelt;
+ double ydelt;
+ bool gridx;
+ bool gridy;
+ vpColor gridColor;
+ char title[256];
+ char unitx[256];
+ char unity[256];
+ unsigned int curveNbr;
+ vpPlotCurve* curveList;
+ bool scaleInitialized;
+ bool firstPoint;
+
+ int nbDivisionx;
+ int nbDivisiony;
+
+ //Graph complet
+ vpImagePoint topLeft;
+ unsigned int width;
+ unsigned int height;
+ vpRect graphZone;
+
+ //Zone d'affichage
+ vpImagePoint dTopLeft;
+ unsigned int dWidth;
+ unsigned int dHeight;
+ vpRect dGraphZone;
+
+ //Zone d'affichage
+ vpImagePoint dTopLeft3D;
+// int dWidth;
+// int dHeight;
+ vpRect dGraphZone3D;
+
+ //3D part
+ vpCameraParameters cam;
+ vpHomogeneousMatrix cMo;
+ vpHomogeneousMatrix cMf;
+ double w_xval;
+ double w_xsize;
+ double w_yval;
+ double w_ysize;
+ double w_zval;
+ double w_zsize;
+ double ptXorg;
+ double ptYorg;
+ double ptZorg;
+ double zoomx_3D;
+ double zoomy_3D;
+ double zoomz_3D;
+
+ int nbDivisionz;
+
+ double zorg;
+ double zoomz;
+ double zmax;
+ double zmin;
+ double zdelt;
+ char unitz[256];
+ vpImagePoint old_iPr;
+ vpImagePoint old_iPz;
+ bool blockedr;
+ bool blockedz;
+ bool blocked;
+
+ unsigned int epsi;
+ unsigned int epsj;
+
+ bool dispUnit;
+ bool dispTitle;
+ bool dispLegend;
+
+ unsigned int gridThickness;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpPlotGraph(const vpPlotGraph &)
+// : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10),
+// xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(), curveNbr(1), curveList(NULL),
+// scaleInitialized(false), firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
+// width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
+// dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
+// w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.),
+// nbDivisionz(10), zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(),
+// blockedr(false), blockedz(false), blocked(false), epsi(5), epsj(6),
+// dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpPlotGraph &operator=(const vpPlotGraph &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpPlotGraph();
+ ~vpPlotGraph();
+
+ bool check3Dline(vpImagePoint &iP1, vpImagePoint &iP2);
+ bool check3Dpoint(vpImagePoint &iP);
+ void clearGraphZone(vpImage<unsigned char> &I);
+ void computeGraphParameters();
+ void computeGraphParameters3D();
+
+ void initGraph (unsigned int nbCurve);
+ void initSize (vpImagePoint topLeft, unsigned int width, unsigned int height, unsigned int margei, unsigned int margej);
+ void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const bool gx, const bool gy);
+ void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const double zmin, const double zmax, const int nbDivz, const bool gx, const bool gy);
+
+ void displayGrid (vpImage<unsigned char> &I);
+ void displayUnit (vpImage<unsigned char> &I);
+ void displayTitle (vpImage<unsigned char> &I);
+ void displayLegend (vpImage<unsigned char> &I);
+ void displayGrid3D (vpImage<unsigned char> &I);
+
+ void findPose();
+
+ bool getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP);
+
+ bool move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button);
+ vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b);
+
+ void plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y);
+ vpMouseButton::vpMouseButtonType plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z);
+ void replot (vpImage<unsigned char> &I);
+ void replot3D (vpImage<unsigned char> &I);
+
+ void rescalex(unsigned int side, double extremity);
+ void rescaley(unsigned int side, double extremity);
+ void rescalez(unsigned int side, double extremity);
+ //void rescale(double &min, double &max, double &delta, const int nbDiv, int side);
+ void resetPointList(const unsigned int curveNum);
+
+ void setCurveColor(const unsigned int curveNum, const vpColor color);
+ void setCurveThickness(const unsigned int curveNum, const unsigned int thickness);
+ void setGridThickness (const unsigned int thickness) {
+ this->gridThickness = thickness;
+ };
+ void setLegend (const unsigned int curveNum, const char *legend);
+ void setTitle (const char *title);
+ void setUnitX (const char *unitx);
+ void setUnitY (const char *unity);
+ void setUnitZ (const char *unitz);
+};
+
+#endif
+#endif
+#endif
+
diff --git a/modules/gui/include/visp3/gui/vpProjectionDisplay.h b/modules/gui/include/visp3/gui/vpProjectionDisplay.h
new file mode 100644
index 0000000..3177dcf
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpProjectionDisplay.h
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpProjectionDisplay_H
+#define vpProjectionDisplay_H
+
+/*!
+ \file vpProjectionDisplay.h
+ \brief interface with the image for feature display
+*/
+
+#include <visp3/core/vpConfig.h>
+#if defined (VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/core/vpForwardProjection.h>
+
+#include <list>
+
+/*!
+ \class vpProjectionDisplay
+ \ingroup group_gui_projection
+ \brief interface with the image for feature display
+*/
+class VISP_EXPORT vpProjectionDisplay
+{
+private:
+ vpImage<unsigned char> Icam ;
+ vpImage<unsigned char> Iext ;
+
+#if defined VISP_HAVE_X11
+ vpDisplayX dIcam ;
+ vpDisplayX dIext ;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK dIcam ;
+ vpDisplayGTK dIext ;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI dIcam ;
+ vpDisplayGDI dIext ;
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV dIcam ;
+ vpDisplayOpenCV dIext ;
+#elif defined(VISP_HAVE_D3D9)
+ vpDisplayD3D dIcam ;
+ vpDisplayD3D dIext ;
+#endif
+public:
+ void init() ;
+ void init(int select) ;
+ void close() ;
+ static int internalView() { return 0x01 ; }
+ static int externalView() { return 0x02 ; }
+
+ /*! Default constructor. */
+ vpProjectionDisplay()
+ : Icam(), Iext(),
+#if defined (VISP_HAVE_DISPLAY)
+ dIcam(), dIext(),
+#endif
+ listFp(), o(), x(), y(), z(), traj()
+ {
+ init();
+ }
+ vpProjectionDisplay(int select)
+ : Icam(), Iext(),
+#if defined (VISP_HAVE_DISPLAY)
+ dIcam(), dIext(),
+#endif
+ listFp(), o(), x(), y(), z(), traj()
+ {
+ init(select) ;
+ }
+
+ void insert( vpForwardProjection &fp) ;
+ void display(vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const bool &displayTraj = false,
+ const unsigned int thickness=1);
+ void displayCamera(vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const unsigned int thickness=1) ;
+
+private:
+ std::list<vpForwardProjection *> listFp ;
+ vpPoint o ;
+ vpPoint x ;
+ vpPoint y ;
+ vpPoint z ;
+ vpMatrix traj;
+} ;
+
+
+
+#endif
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/gui/include/visp3/gui/vpWin32API.h b/modules/gui/include/visp3/gui/vpWin32API.h
new file mode 100644
index 0000000..545829f
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpWin32API.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * GDI renderer for windows 32 display
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#ifndef vpWin32API_HH
+#define vpWin32API_HH
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+
+#include <windows.h>
+#include <string>
+
+DWORD vpProcessErrors(const std::string &api_name);
+void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h);
+void vpPrepareImageWithPen(CRITICAL_SECTION* CriticalSection, HWND hWnd,HBITMAP bmp,COLORREF color,unsigned int thickness, int style, HDC& hDCScreen,HDC& hDCMem,HPEN& hPen);
+void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection);
+void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection);
+BOOL vpReleaseSemaphore(HANDLE hSemaphore,LONG IReleaseCount,LPLONG lpPreviousCount);
+BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd);
+BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint);
+BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop);
+BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase);
+COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor);
+HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, UINT cBitsPerPel, const VOID *lpvBits);
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpWin32Renderer.h b/modules/gui/include/visp3/gui/vpWin32Renderer.h
new file mode 100644
index 0000000..b8268f4
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpWin32Renderer.h
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 renderer base class
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+
+#ifndef vpWin32Renderer_HH
+#define vpWin32Renderer_HH
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
+#include <windows.h>
+#include <visp3/core/vpDebug.h>
+
+class VISP_EXPORT vpWin32Renderer
+{
+
+ protected:
+ //the size of the display
+ unsigned int nbCols;
+ unsigned int nbRows;
+
+ public:
+ //! Destructor.
+ virtual ~vpWin32Renderer() {};
+
+ //! Returns the image dimensions.
+ unsigned int getImageWidth(){ return nbCols; }
+ unsigned int getImageHeight(){ return nbRows; }
+
+
+
+ //! Inits the display .
+ virtual bool init(HWND hWnd, unsigned int w, unsigned int h) =0;
+
+ //! Renders the image.
+ virtual bool render() =0;
+
+
+ /*!
+ Sets the image to display.
+ \param im The image to display.
+ */
+ virtual void setImg(const vpImage<vpRGBa>& im) =0;
+ virtual void setImg(const vpImage<unsigned char>& im) =0;
+ virtual void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
+ virtual void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
+
+ /*!
+ Sets the pixel at (x,y).
+ \param y The y coordinate of the pixel.
+ \param x The x coordinate of the pixel.
+ \param color The color of the pixel.
+ */
+ virtual void setPixel(const vpImagePoint &iP, const vpColor &color) =0;
+
+ /*!
+ Draws a line.
+ \param i1 its starting point's first coordinate
+ \param j1 its starting point's second coordinate
+ \param i2 its ending point's first coordinate
+ \param j2 its ending point's second coordinate
+ \param e line thickness
+ \param col the line's color
+ \param style style of the line
+ */
+ virtual void drawLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness, int style=PS_SOLID) =0;
+
+ /*!
+ Draws a rectangle.
+ \param i its top left point's first coordinate
+ \param j its top left point's second coordinate
+ \param width width of the rectangle
+ \param height height of the rectangle
+ \param col The rectangle's color
+ \param fill True if it is a filled rectangle
+ \param e line thickness
+ */
+ virtual void drawRect(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill=false,
+ unsigned int thickness=1) =0;
+
+ /*!
+ Clears the image to color c.
+ \param c The color used to fill the image.
+ */
+ virtual void clear(const vpColor &color) =0;
+
+ /*!
+ Draws a circle.
+ \param i its center point's first coordinate
+ \param j its center point's second coordinate
+ \param r The circle's radius
+ \param col The circle's color
+ */
+ virtual void drawCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill, unsigned int thickness=1) =0;
+
+ /*!
+ Draws some text.
+ \param i its top left point's first coordinate
+ \param j its top left point's second coordinate
+ \param s The string to display
+ \param col The text's color
+ */
+ virtual void drawText(const vpImagePoint &ip, const char * text,
+ const vpColor &color) =0;
+
+ /*!
+ Draws a cross.
+ \param i its center point's first coordinate
+ \param j its center point's second coordinate
+ \param size Size of the cross
+ \param col The cross' color
+ \param e width of the cross
+ */
+ virtual void drawCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness=1) =0;
+
+ /*!
+ Draws an arrow.
+ \param i1 its starting point's first coordinate
+ \param j1 its starting point's second coordinate
+ \param i2 its ending point's first coordinate
+ \param j2 its ending point's second coordinate
+ \param color The line's color
+ \param L ...
+ \param l ...
+ */
+ virtual void drawArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness) =0;
+
+ /*!
+ Gets the currently displayed image.
+ \param I Image returned.
+ */
+ virtual void getImage(vpImage<vpRGBa> &I) =0;
+};
+
+#endif
+#endif
+#endif
diff --git a/modules/gui/include/visp3/gui/vpWin32Window.h b/modules/gui/include/visp3/gui/vpWin32Window.h
new file mode 100644
index 0000000..e65996d
--- /dev/null
+++ b/modules/gui/include/visp3/gui/vpWin32Window.h
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display's window class
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#ifndef vpWin32Window_HH
+#define vpWin32Window_HH
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <windows.h>
+#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/core/vpGDIRenderer.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpDisplayException.h>
+
+
+//ViSP-defined messages for window's callback function
+#define vpWM_GETCLICK WM_USER+1
+#define vpWM_DISPLAY WM_USER+2
+#define vpWM_GETCLICKUP WM_USER+3
+#define vpWM_CLOSEDISPLAY WM_USER+4
+#define vpWM_GETPOINTERMOTIONEVENT WM_USER+5
+#define vpWM_DISPLAY_ROI WM_USER+6
+
+//No specific mouse button query
+#define vpNO_BUTTON_QUERY -1
+
+class vpDisplayWin32;
+
+class VISP_EXPORT vpWin32Window
+{
+private :
+
+ HINSTANCE hInst;
+
+ //! Window's handle
+ HWND hWnd;
+
+ //! Window is initialized
+ bool initialized;
+ //! Handle for the initialization semaphore
+ HANDLE semaInit;
+
+ //! Handle for the getClick semaphore
+ HANDLE semaClick;
+ //! Handle for the getClickUp semaphore
+ HANDLE semaClickUp;
+ //! Handle for the keyborad event semaphore
+ HANDLE semaKey;
+ //! Handle for the mouse event semaphore
+ HANDLE semaMove;
+
+ //! X coordinate of the click
+ int clickX;
+ int clickXUp;
+ //! Y coordinate of the click
+ int clickY;
+ int clickYUp;
+ //! X coordinate of the mouse
+ int coordX;
+ //! Y coordinate of the mouse
+ int coordY;
+ // Keyboard key
+ char lpString[10];
+ //! Button used for the click
+ vpMouseButton::vpMouseButtonType clickButton;
+ vpMouseButton::vpMouseButtonType clickButtonUp;
+
+ //! True if the window's class has already been registered
+ static bool registered;
+
+ //! The renderer used by the window
+ vpWin32Renderer * renderer;
+
+
+ public:
+
+ vpWin32Window(vpWin32Renderer * rend = NULL);
+ virtual ~vpWin32Window();
+
+ //! Returns the displayed image's width
+ unsigned int getImageWidth(){ return renderer->getImageWidth(); }
+ //! Returns the displayed image's height
+ unsigned int getImageHeight(){ return renderer->getImageHeight(); }
+ //! Returns the window's handle
+ HWND getHWnd(){ return hWnd;}
+
+ //! Returns true if the window is initialized
+ bool isInitialized(){ return initialized; }
+
+ //! Initialize the window
+ void initWindow(const char* title, int posx, int posy, unsigned int w, unsigned int h);
+
+ // Friend classes
+ friend class vpDisplayWin32;
+ friend class vpDisplayD3D;
+ friend class vpDisplayGDI;
+
+ //! The message loop
+ friend LRESULT CALLBACK WndProc(HWND hWnd, UINT message,
+ WPARAM wParam, LPARAM lParam);
+};
+
+#endif
+#endif
+#endif
diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp
new file mode 100644
index 0000000..9832970
--- /dev/null
+++ b/modules/gui/src/display/vpDisplayGTK.cpp
@@ -0,0 +1,1715 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Christophe Collewet
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpDisplayGTK.cpp
+ \brief Define the GTK console to display images.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GTK) )
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+// Display stuff
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
+
+//debug / exception
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpDisplayException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpImageTools.h>
+
+/*!
+
+ Constructor : initialize a display to visualize a gray level image
+ (8 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized).
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title)
+ : widget(NULL), background(NULL), gc(NULL),
+ blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
+ lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
+ purple(), lightGray(), gray(), darkGray(),
+ colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
+{
+ init(I, x, y, title) ;
+}
+
+
+/*!
+ Constructor : initialize a display to visualize a RGBa level image
+ (32 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title)
+ : widget(NULL), background(NULL), gc(NULL),
+ blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
+ lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
+ purple(), lightGray(), gray(), darkGray(),
+ colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
+{
+ init(I, x, y, title) ;
+}
+
+
+
+/*!
+
+ Constructor that just initialize the display position in the screen
+ and the display title.
+
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+ To initialize the display size, you need to call init().
+
+ \code
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayGTK d(100, 200, "My display");
+ vpImage<unsigned char> I(240,384);
+ d.init(I);
+}
+ \endcode
+*/
+vpDisplayGTK::vpDisplayGTK(int x, int y, const char *title)
+ : widget(NULL), background(NULL), gc(NULL),
+ blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
+ lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
+ purple(), lightGray(), gray(), darkGray(),
+ colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
+{
+ windowXPosition = x ;
+ windowYPosition = y ;
+
+ if(title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+}
+
+/*!
+ Basic constructor.
+
+ To initialize the window position, title and size you may call
+ init(vpImage<unsigned char> &, int, int, const char *) or
+ init(vpImage<vpRGBa> &, int, int, const char *).
+
+ \code
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayGTK d;
+ vpImage<unsigned char> I(240,384);
+ d.init(I, 100, 200, "My display");
+}
+ \endcode
+*/
+vpDisplayGTK::vpDisplayGTK()
+ : vpDisplay(), widget(NULL), background(NULL), gc(NULL),
+ blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
+ lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
+ purple(), lightGray(), gray(), darkGray(),
+ colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
+
+{
+}
+
+/*!
+ Destructor.
+*/
+vpDisplayGTK::~vpDisplayGTK()
+{
+ closeDisplay() ;
+}
+
+/*!
+ Initialize the display (size, position and title) of a gray level image.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayGTK::init(vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+
+ init (I.getWidth(), I.getHeight(), windowXPosition, windowYPosition, title) ;
+ I.display = this ;
+ displayHasBeenInitialized = true ;
+}
+
+/*!
+ Initialize the display (size, position and title) of a color
+ image in RGBa format.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayGTK::init(vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+
+ init (I.getWidth(), I.getHeight(), windowXPosition, windowYPosition, title) ;
+ I.display = this ;
+ displayHasBeenInitialized = true ;
+}
+/*!
+ Initialize the display size, position and title.
+
+ \param w, h : Width and height of the window.
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayGTK::init(unsigned int w, unsigned int h,
+ int x, int y,
+ const char *title)
+{
+ /* Initialisation of thegdk et gdk_rgb library */
+ int *argc=NULL ;
+ char **argv ;
+ gtk_init(argc,&argv);
+
+ this->width = w;
+ this->height = h;
+
+ /* Create the window*/
+ widget = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+
+ gtk_widget_add_events(widget, GDK_BUTTON_PRESS_MASK | GDK_BUTTON_RELEASE_MASK
+ | GDK_POINTER_MOTION_MASK);
+
+ gtk_window_set_default_size(GTK_WINDOW(widget), (gint)width, (gint)height);
+
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+
+ gtk_window_move(GTK_WINDOW(widget), windowXPosition, windowYPosition);
+
+ gtk_widget_show(widget);
+
+ gdk_rgb_init();
+
+ /* Create background pixmap */
+ background = gdk_pixmap_new(widget->window, (gint)width, (gint)height, -1);
+
+ /* Create graphic context */
+ gc = gdk_gc_new(widget->window);
+
+ /* get the colormap */
+ colormap = gdk_window_get_colormap(widget->window);
+
+ col = new GdkColor *[vpColor::id_unknown] ; // id_unknown = number of predefined colors
+
+ /* Create color */
+ gdk_color_parse("light blue",&lightBlue);
+ gdk_colormap_alloc_color(colormap,&lightBlue,FALSE,TRUE);
+ col[vpColor::id_lightBlue] = &lightBlue ;
+
+ gdk_color_parse("blue",&blue);
+ gdk_colormap_alloc_color(colormap,&blue,FALSE,TRUE);
+ col[vpColor::id_blue] = &blue ;
+
+ gdk_color_parse("dark blue",&darkBlue);
+ gdk_colormap_alloc_color(colormap,&darkBlue,FALSE,TRUE);
+ col[vpColor::id_darkBlue] = &darkBlue ;
+
+ gdk_color_parse("#FF8C8C",&lightRed);
+ gdk_colormap_alloc_color(colormap,&lightRed,FALSE,TRUE);
+ col[vpColor::id_lightRed] = &lightRed ;
+
+ gdk_color_parse("red",&red);
+ gdk_colormap_alloc_color(colormap,&red,FALSE,TRUE);
+ col[vpColor::id_red] = &red ;
+
+ gdk_color_parse("dark red",&darkRed);
+ gdk_colormap_alloc_color(colormap,&darkRed,FALSE,TRUE);
+ col[vpColor::id_darkRed] = &darkRed ;
+
+ gdk_color_parse("light green",&lightGreen);
+ gdk_colormap_alloc_color(colormap,&lightGreen,FALSE,TRUE);
+ col[vpColor::id_lightGreen] = &lightGreen ;
+
+ gdk_color_parse("green",&green);
+ gdk_colormap_alloc_color(colormap,&green,FALSE,TRUE);
+ col[vpColor::id_green] = &green ;
+
+ gdk_color_parse("dark green",&darkGreen);
+ gdk_colormap_alloc_color(colormap,&darkGreen,FALSE,TRUE);
+ col[vpColor::id_darkGreen] = &darkGreen ;
+
+ gdk_color_parse("yellow",&yellow);
+ gdk_colormap_alloc_color(colormap,&yellow,FALSE,TRUE);
+ col[vpColor::id_yellow] = &yellow ;
+
+ gdk_color_parse("cyan",&cyan);
+ gdk_colormap_alloc_color(colormap,&cyan,FALSE,TRUE);
+ col[vpColor::id_cyan] = &cyan ;
+
+ gdk_color_parse("orange",&orange);
+ gdk_colormap_alloc_color(colormap,&orange,FALSE,TRUE);
+ col[vpColor::id_orange] = &orange ;
+
+ gdk_color_parse("purple",&purple);
+ gdk_colormap_alloc_color(colormap,&purple,FALSE,TRUE);
+ col[vpColor::id_purple] = &purple ;
+
+ gdk_color_parse("white",&white);
+ gdk_colormap_alloc_color(colormap,&white,FALSE,TRUE);
+ col[vpColor::id_white] = &white ;
+
+ gdk_color_parse("black",&black);
+ gdk_colormap_alloc_color(colormap,&black,FALSE,TRUE);
+ col[vpColor::id_black] = &black ;
+
+ gdk_color_parse("#C0C0C0",&lightGray);
+ gdk_colormap_alloc_color(colormap,&lightGray,FALSE,TRUE);
+ col[vpColor::id_lightGray] = &lightGray ;
+
+ gdk_color_parse("#808080",&gray);
+ gdk_colormap_alloc_color(colormap,&gray,FALSE,TRUE);
+ col[vpColor::id_gray] = &gray ;
+
+ gdk_color_parse("#404040",&darkGray);
+ gdk_colormap_alloc_color(colormap,&darkGray,FALSE,TRUE);
+ col[vpColor::id_darkGray] = &darkGray ;
+
+ // Try to load a default font
+ font = gdk_font_load("-*-times-medium-r-normal-*-16-*-*-*-*-*-*-*");
+ if (font == NULL)
+ font = gdk_font_load("-*-courier-bold-r-normal-*-*-140-*-*-*-*-*-*");
+ if (font == NULL)
+ font = gdk_font_load("-*-courier 10 pitch-medium-r-normal-*-16-*-*-*-*-*-*-*");
+
+ if(title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+
+ displayHasBeenInitialized = true ;
+ gdk_window_set_title(widget->window, title_.c_str());
+}
+
+
+/*!
+
+ \warning Not implemented yet.
+
+ Set the font used to display a text in overlay. The display is
+ performed using displayCharString().
+
+ \param fontname : The expected font name.
+
+ \note Under UNIX, to know all the available fonts, use the
+ "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
+
+ \sa displayCharString()
+*/
+void
+vpDisplayGTK::setFont(const char *fontname)
+{
+ font = gdk_font_load((const gchar*)fontname);
+}
+
+/*!
+ Set the window title.
+ \param title : Window title.
+*/
+void
+vpDisplayGTK::setTitle(const char *title)
+{
+ if (displayHasBeenInitialized)
+ {
+ if(title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+ gdk_window_set_title(widget->window, title_.c_str());
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Set the window position in the screen.
+
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void vpDisplayGTK::setWindowPosition(int winx, int winy)
+{
+
+ if (displayHasBeenInitialized) {
+ gtk_window_move(GTK_WINDOW(widget), winx, winy);
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayGTK::displayImage(const vpImage<unsigned char> &I)
+{
+
+ if (displayHasBeenInitialized)
+ {
+ /* Copie de l'image dans le pixmap fond */
+ gdk_draw_gray_image(background,
+ gc, 0, 0, (gint)width, (gint)height,
+ GDK_RGB_DITHER_NONE,
+ I.bitmap,
+ (gint)width);
+
+ /* Le pixmap background devient le fond de la zone de dessin */
+ gdk_window_set_back_pixmap(widget->window, background, FALSE);
+
+ /* Affichage */
+ //gdk_window_clear(GTK_WINDOW(widget));
+ //gdk_flush();
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Display a selection of the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w : Width of the region of interest
+
+ \param h : Height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayGTK::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImage<unsigned char> Itemp;
+ vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(), h, w,Itemp);
+ /* Copie de l'image dans le pixmap fond */
+ gdk_draw_gray_image(background,
+ gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
+ GDK_RGB_DITHER_NONE,
+ I.bitmap,
+ (gint)w);
+
+ /* Le pixmap background devient le fond de la zone de dessin */
+ gdk_window_set_back_pixmap(widget->window, background, FALSE);
+
+ /* Affichage */
+ //gdk_window_clear(GTK_WINDOW(widget));
+ //gdk_flush();
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayGTK::displayImage(const vpImage<vpRGBa> &I)
+{
+
+ if (displayHasBeenInitialized)
+ {
+
+ /* Copie de l'image dans le pixmap fond */
+ gdk_draw_rgb_32_image(background,
+ gc, 0, 0, (gint)width, (gint)height,
+ GDK_RGB_DITHER_NONE,
+ (unsigned char *)I.bitmap,
+ (gint)(4*width));
+
+ /* Permet de fermer la fenetre si besoin (cas des sequences d'images) */
+ //while (g_main_iteration(FALSE));
+
+ /* Le pixmap background devient le fond de la zone de dessin */
+ gdk_window_set_back_pixmap(widget->window, background, FALSE);
+
+ /* Affichage */
+ //gdk_window_clear(GTK_WINDOW(widget));
+ //flushDisplay() ;
+
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Display a selection of the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w : Width of the region of interest
+
+ \param h : Height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayGTK::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImage<vpRGBa> Itemp;
+ vpImageTools::createSubImage(I,(unsigned int)iP.get_i(), (unsigned int)iP.get_j(), h, w, Itemp);
+ /* Copie de l'image dans le pixmap fond */
+ gdk_draw_rgb_32_image(background,
+ gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
+ GDK_RGB_DITHER_NONE,
+ (unsigned char *)Itemp.bitmap,
+ (gint)(4*w));
+
+ /* Permet de fermer la fenetre si besoin (cas des sequences d'images) */
+ //while (g_main_iteration(FALSE));
+
+ /* Le pixmap background devient le fond de la zone de dessin */
+ gdk_window_set_back_pixmap(widget->window, background, FALSE);
+
+ /* Affichage */
+ //gdk_window_clear(GTK_WINDOW(widget));
+ //flushDisplay() ;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ \warning Not implemented yet.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayGTK::displayImage(const unsigned char * /* I */)
+{
+ vpTRACE(" not implemented ") ;
+}
+
+/*!
+ Close the window.
+
+ \sa init()
+*/
+void vpDisplayGTK::closeDisplay()
+{
+ if (col != NULL)
+ {
+ delete [] col ; col = NULL ;
+ }
+
+ if (widget != NULL)
+ {
+ gdk_window_hide (widget->window);
+ gdk_window_destroy(widget->window);
+ widget = NULL;
+ }
+ displayHasBeenInitialized = false;
+}
+
+
+/*!
+ Flushes the display buffer.
+ It's necessary to use this function to see the results of any drawing.
+*/
+void vpDisplayGTK::flushDisplay()
+{
+ if (displayHasBeenInitialized)
+ {
+ gdk_window_clear(widget->window);
+ gdk_flush();
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Flushes the display buffer.
+ It's necessary to use this function to see the results of any drawing.
+*/
+void vpDisplayGTK::flushDisplayROI(const vpImagePoint &/*iP*/, const unsigned int /*width*/, const unsigned int /*height*/)
+{
+ if (displayHasBeenInitialized)
+ {
+ gdk_window_clear(widget->window);
+ gdk_flush();
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ \warning Not implemented yet.
+*/
+void vpDisplayGTK::clearDisplay(const vpColor & /* color */)
+{
+ vpTRACE("Not implemented") ;
+}
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpDisplayGTK::displayArrow ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w, unsigned int h,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ try{
+ double a = ip2.get_i() - ip1.get_i() ;
+ double b = ip2.get_j() - ip1.get_j() ;
+ double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+
+ //if ((a==0)&&(b==0))
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
+ {
+ // DisplayCrossLarge(i1,j1,3,col) ;
+ }
+ else
+ {
+ a /= lg ;
+ b /= lg ;
+
+ vpImagePoint ip3;
+ ip3.set_i(ip2.get_i() - w*a);
+ ip3.set_j(ip2.get_j() - w*b);
+
+ vpImagePoint ip4;
+ ip4.set_i( ip3.get_i() - b*h );
+ ip4.set_j( ip3.get_j() + a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ ip4.set_i( ip3.get_i() + b*h );
+ ip4.set_j( ip3.get_j() - a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ displayLine ( ip1, ip2, color, thickness ) ;
+ }
+ }
+ catch (...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param ip : Upper left image point location of the string in the display.
+ \param text : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+*/
+void vpDisplayGTK::displayCharString ( const vpImagePoint &ip,
+ const char *text,
+ const vpColor &color )
+{
+ if (displayHasBeenInitialized)
+ {
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+ if (font != NULL)
+ gdk_draw_string(background, font, gc,
+ vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v() ),
+ (const gchar *)text);
+ else
+ std::cout << "Cannot draw string: no font is selected" << std::endl;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+/*!
+ Display a circle.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the circle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void vpDisplayGTK::displayCircle ( const vpImagePoint ¢er,
+ unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ gdk_gc_set_line_attributes(gc, (gint)thickness,
+ GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+
+ if (fill == false)
+ gdk_draw_arc(background, gc, FALSE,
+ vpMath::round( center.get_u()-radius ),
+ vpMath::round( center.get_v()-radius ),
+ (gint)(2*radius), (gint)(2*radius), 23040, 23040) ; /* 23040 = 360*64 */
+ else
+ gdk_draw_arc(background, gc, TRUE,
+ vpMath::round( center.get_u()-radius ),
+ vpMath::round( center.get_v()-radius ),
+ (gint)(2*radius), (gint)(2*radius), 23040, 23040) ; /* 23040 = 360*64 */
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+/*!
+ Display a cross at the image point \e ip location.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplayGTK::displayCross ( const vpImagePoint &ip,
+ unsigned int size,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ try{
+ double i = ip.get_i();
+ double j = ip.get_j();
+ vpImagePoint ip1, ip2;
+
+ ip1.set_i( i-size/2 );
+ ip1.set_j( j );
+ ip2.set_i( i+size/2 );
+ ip2.set_j( j );
+ displayLine ( ip1, ip2, color, thickness ) ;
+
+ ip1.set_i( i );
+ ip1.set_j( j-size/2 );
+ ip2.set_i( i );
+ ip2.set_j( j+size/2 );
+
+ displayLine ( ip1, ip2, color, thickness ) ;
+ }
+ catch (...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayGTK::displayDotLine ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ if (displayHasBeenInitialized)
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ gdk_gc_set_line_attributes(gc, (gint)thickness,
+ GDK_LINE_ON_OFF_DASH, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ gdk_draw_line(background, gc,
+ vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ),
+ vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) );
+ gdk_gc_set_line_attributes(gc, 0,
+ GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayGTK::displayLine ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ gdk_gc_set_line_attributes(gc, (gint)thickness,
+ GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ gdk_draw_line(background, gc,
+ vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ),
+ vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) );
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Display a point at the image point \e ip location.
+ \param ip : Point location.
+ \param color : Point color.
+*/
+void vpDisplayGTK::displayPoint ( const vpImagePoint &ip,
+ const vpColor &color )
+{
+ if (displayHasBeenInitialized)
+ {
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ gdk_draw_point(background,gc,
+ vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v() ) );
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param w,h : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayGTK::displayRectangle ( const vpImagePoint &topLeft,
+ unsigned int w, unsigned int h,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+ gdk_gc_set_line_attributes(gc, (gint)thickness,
+ GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+
+ if (fill == false)
+ gdk_draw_rectangle(background, gc, FALSE,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ (gint)w-1, (gint)h-1);
+ else
+ gdk_draw_rectangle(background, gc, TRUE,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ (gint)w, (gint)h);
+
+ if (thickness > 1)
+ gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Display a rectangle.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayGTK::displayRectangle ( const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ gdk_gc_set_line_attributes(gc, (gint)thickness,
+ GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+
+ int w = vpMath::round( bottomRight.get_u() - topLeft.get_u() );
+ int h = vpMath::round( bottomRight.get_v() - topLeft.get_v() );
+
+ if (fill == false)
+ gdk_draw_rectangle(background, gc, FALSE,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ w-1,h-1);
+ else
+ gdk_draw_rectangle(background, gc, TRUE,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ w, h);
+
+ if (thickness > 1)
+ gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+/*!
+ Display a rectangle.
+
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplayGTK::displayRectangle ( const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if (color.id < vpColor::id_unknown)
+ gdk_gc_set_foreground(gc, col[color.id]);
+ else {
+ gdkcolor.red = 256 * color.R;
+ gdkcolor.green = 256 * color.G;
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdk_gc_set_foreground(gc, &gdkcolor);
+ }
+
+ if ( thickness == 1 ) thickness = 0;
+
+ gdk_gc_set_line_attributes(gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+
+ if (fill == false)
+ gdk_draw_rectangle(background, gc, FALSE,
+ vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getTop() ),
+ vpMath::round( rectangle.getWidth()-1 ),
+ vpMath::round( rectangle.getHeight()-1 ) );
+
+ else
+ gdk_draw_rectangle(background, gc, TRUE,
+ vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getTop() ),
+ vpMath::round( rectangle.getWidth()-1 ),
+ vpMath::round( rectangle.getHeight()-1 ) );
+
+ if (thickness > 1)
+ gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
+ GDK_JOIN_BEVEL) ;
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+}
+
+
+/*!
+ Wait for a click from one of the mouse button.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayGTK::getClick(bool blocking)
+{
+ bool ret = false;
+
+ int cpt =0;
+ if (displayHasBeenInitialized) {
+
+// flushDisplay() ;
+ GdkEvent *ev = NULL;
+ do {
+ 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 == widget->window && ev->type == GDK_BUTTON_PRESS){
+ ret = true ;
+ //printf("Click detection\n");
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+*/
+bool
+vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking)
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+
+ GdkEvent *ev = NULL;
+ double u, v ;
+ do {
+ while ((ev = gdk_event_get())!=NULL){
+ if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
+ u = ((GdkEventButton *)ev)->x ;
+ v = ((GdkEventButton *)ev)->y ;
+ ip.set_u( u );
+ ip.set_v( v );
+ ret = true ;
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret ;
+}
+
+
+/*!
+
+ Wait for a mouse button click and get the position of the clicked
+ pixel. The button used to click is also set.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+*/
+bool
+vpDisplayGTK::getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+ GdkEvent *ev = NULL;
+ double u, v ;
+ do {
+ while ((ev = gdk_event_get())){
+ if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS){
+ u = ((GdkEventButton *)ev)->x ;
+ v = ((GdkEventButton *)ev)->y ;
+ ip.set_u( u );
+ ip.set_v( v );
+
+ switch ((int)((GdkEventButton *)ev)->button) {
+ case 1:
+ button = vpMouseButton::button1; break;
+ case 2:
+ button = vpMouseButton::button2; break;
+ case 3:
+ button = vpMouseButton::button3; break;
+ }
+ ret = true ;
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Wait for a mouse button click release and get the position of the
+ image point were the click release occurs. The button used to click is
+ also set. Same method as getClick(unsigned int&, unsigned int&,
+ vpMouseButton::vpMouseButtonType &, bool).
+
+ \param ip [out] : Position of the clicked image point.
+
+ \param button [in] : Button used to click.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+ \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
+
+*/
+bool
+vpDisplayGTK::getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ bool ret = false;
+
+ if ( displayHasBeenInitialized ) {
+
+ //flushDisplay() ;
+ GdkEvent *ev = NULL;
+ double u, v ;
+ do {
+ while ((ev = gdk_event_get())!=NULL){
+ if ( ev->any.window == widget->window
+ && ev->type == GDK_BUTTON_RELEASE) {
+ u = ((GdkEventButton *)ev)->x ;
+ v = ((GdkEventButton *)ev)->y ;
+ ip.set_u( u );
+ ip.set_v( v );
+
+ switch ( ( int ) ( ( GdkEventButton * ) ev )->button ) {
+ case 1:
+ button = vpMouseButton::button1; break;
+ case 2:
+ button = vpMouseButton::button2; break;
+ case 3:
+ button = vpMouseButton::button3; break;
+ }
+ ret = true ;
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE ( "GTK not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "GTK not initialized" ) ) ;
+ }
+ return ret;
+}
+
+/*
+ \brief gets the displayed image (including the overlay plane)
+ and returns an RGBa image
+*/
+void vpDisplayGTK::getImage(vpImage<vpRGBa> &I)
+{
+
+
+ // shoudl certainly be optimized.
+ // doesn't work
+ if (displayHasBeenInitialized)
+ {
+
+ GdkImage *ImageGtk;
+ /*
+ */
+
+ ImageGtk = gdk_image_get(background, 0, 0, (gint)width, (gint)height);
+
+
+ I.resize(height,width) ;
+ guchar *pos;
+ guint32 pixel;
+ gint x,y;
+ guchar OctetRouge,OctetVert,OctetBleu,mask;
+ mask = 0x000000FF;
+
+ pos = (unsigned char *)I.bitmap;
+ for (y=0;y<(gint)height;y++)
+ {
+ for (x=0;x<(gint)width;x++)
+ {
+ pixel = gdk_image_get_pixel(ImageGtk,x,y);
+ OctetBleu = (guchar)pixel & mask;
+ OctetVert = (guchar)(pixel>>8) & mask;
+ OctetRouge = (guchar)(pixel>>16) & mask;
+ *pos++ = OctetRouge;
+ *pos++ = OctetVert;
+ *pos++ = OctetBleu;
+ *pos++ = 0;
+ }
+ }
+
+
+ }
+ else
+ {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+
+}
+
+/*!
+ \brief get the window depth (8,16,24,32)
+
+ usualy it 24 bits now...
+*/
+unsigned int vpDisplayGTK::getScreenDepth()
+{
+
+ unsigned int depth;
+
+ depth = (unsigned int)gdk_window_get_visual(widget->window)->depth ;
+
+ return (depth);
+}
+
+/*!
+ \brief get the window size
+
+ \warning Not implemented
+*/
+void vpDisplayGTK::getScreenSize(unsigned int &w, unsigned int &h)
+{
+ vpTRACE("Not implemented") ;
+ w = 0;
+ h = 0;
+}
+
+/*!
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayGTK::getKeyboardEvent(bool blocking)
+{
+ bool ret = false;
+
+ int cpt =0;
+ if (displayHasBeenInitialized) {
+
+ GdkEvent *ev = NULL;
+ do {
+ 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 == widget->window && ev->type == GDK_KEY_PRESS){
+ ret = true ;
+ //printf("Key press detection\n");
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayGTK::getKeyboardEvent(char *string, bool blocking)
+{
+ bool ret = false;
+
+ int cpt =0;
+ if (displayHasBeenInitialized) {
+
+ GdkEvent *ev = NULL;
+ do {
+ 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 == widget->window && ev->type == GDK_KEY_PRESS){
+ //std::cout << "Key val: \"" << gdk_keyval_name (ev->key.keyval) /*ev->key.string*/ << "\"" << std::endl;
+ sprintf(string, "%s", gdk_keyval_name (ev->key.keyval));
+ ret = true ;
+ //printf("Key press detection\n");
+ }
+ gdk_event_free(ev) ;
+ }
+ if (blocking){
+ flushDisplay();
+ vpTime::wait(100);
+ }
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+
+*/
+bool
+vpDisplayGTK::getPointerMotionEvent ( vpImagePoint &ip)
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+ GdkEvent *ev = NULL;
+ double u, v ;
+ if ((ev = gdk_event_get())){
+ if (ev->any.window == widget->window && ev->type == GDK_MOTION_NOTIFY){
+ u = ((GdkEventMotion *)ev)->x ;
+ v = ((GdkEventMotion *)ev)->y ;
+ ip.set_u( u );
+ ip.set_v( v );
+
+ ret = true ;
+ }
+ gdk_event_free(ev) ;
+ }
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayGTK::getPointerPosition ( vpImagePoint &ip)
+{
+ if (displayHasBeenInitialized)
+ {
+ int u,v;
+ gdk_window_get_pointer(widget->window, &u, &v, NULL);
+ ip.set_u( u );
+ ip.set_v( v );
+ }
+ else {
+ vpERROR_TRACE("GTK not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "GTK not initialized")) ;
+ }
+
+ return true;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayGTK.cpp.o) has no symbols
+void dummy_vpDisplayGTK() {};
+#endif
diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp
new file mode 100644
index 0000000..2c76e9f
--- /dev/null
+++ b/modules/gui/src/display/vpDisplayOpenCV.cpp
@@ -0,0 +1,2214 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Christophe Collewet
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpDisplayOpenCV.cpp
+ \brief Define the OpenCV console to display images.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV)
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+// Display stuff
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpImageTools.h>
+
+//debug / exception
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpDisplayException.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+
+# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/core/core_c.h> // for CV_FILLED versus cv::FILLED
+
+# ifndef CV_RGB
+# define CV_RGB( r, g, b ) cv::Scalar( (b), (g), (r), 0 )
+# endif
+#endif
+
+std::vector<std::string> vpDisplayOpenCV::m_listTitles = std::vector<std::string>();
+unsigned int vpDisplayOpenCV::m_nbWindows = 0;
+
+/*!
+
+ Constructor. Initialize a display to visualize a gray level image
+ (8 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title)
+ :
+ vpDisplay(),
+ #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ background(NULL), col(NULL), cvcolor(), font(NULL),
+ #else
+ background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
+ #endif
+ fontHeight(10), x_move(0), y_move(0) , move(false),
+ x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+ x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+ x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+ x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+ x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+ x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+{
+ init(I, x, y, title) ;
+}
+
+
+/*!
+ Constructor. Initialize a display to visualize a RGBa level image
+ (32 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title)
+ :
+ #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ background(NULL), col(NULL), cvcolor(), font(NULL),
+ #else
+ background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
+ #endif
+ fontHeight(10), x_move(0), y_move(0) , move(false),
+ x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+ x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+ x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+ x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+ x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+ x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+{
+ init(I, x, y, title) ;
+}
+
+/*!
+
+ Constructor that just initialize the display position in the screen
+ and the display title.
+
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+ To initialize the display size, you need to call init().
+
+ \code
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayOpenCV d(100, 200, "My display");
+ vpImage<unsigned char> I(240, 384);
+ d.init(I);
+}
+ \endcode
+*/
+vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const char *title )
+ :
+ #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ background(NULL), col(NULL), cvcolor(), font(NULL),
+ #else
+ background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
+ #endif
+ fontHeight(10), x_move(0), y_move(0) , move(false),
+ x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+ x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+ x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+ x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+ x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+ x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+{
+ windowXPosition = x;
+ windowYPosition = y;
+
+ if(title != NULL){
+ title_ = std::string(title);
+ }
+ else{
+ std::ostringstream s;
+ s << m_nbWindows++;
+ title_ = std::string("Window ") + s.str();
+ }
+
+ bool isInList;
+ do{
+ isInList = false;
+ for(size_t i = 0 ; i < m_listTitles.size() ; i++){
+ if(m_listTitles[i] == title_){
+ std::ostringstream s;
+ s << m_nbWindows++;
+ title_ = std::string("Window ") + s.str();
+ isInList = true;
+ break;
+ }
+ }
+ }
+ while(isInList);
+
+ m_listTitles.push_back(title_);
+}
+
+/*!
+ Basic constructor.
+
+ To initialize the window position, title and size you may call
+ init(vpImage<unsigned char> &, int, int, const char *) or
+ init(vpImage<vpRGBa> &, int, int, const char *).
+
+ \code
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayOpenCV d;
+ vpImage<unsigned char> I(240, 384);
+ d.init(I, 100, 200, "My display");
+}
+ \endcode
+*/
+vpDisplayOpenCV::vpDisplayOpenCV()
+ :
+ #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ background(NULL), col(NULL), cvcolor(), font(NULL),
+ #else
+ background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
+ #endif
+ fontHeight(10), x_move(0), y_move(0) , move(false),
+ x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+ x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+ x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+ x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+ x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+ x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+{
+}
+
+/*!
+ Destructor.
+*/
+vpDisplayOpenCV::~vpDisplayOpenCV()
+{
+ closeDisplay() ;
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvReleaseImage(&background);
+#endif
+}
+
+/*!
+ Initialize the display (size, position and title) of a gray level image.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayOpenCV::init(vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title)
+{
+
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+ init (I.getWidth(), I.getHeight(), x, y, title) ;
+ I.display = this ;
+ displayHasBeenInitialized = true ;
+}
+
+/*!
+ Initialize the display (size, position and title) of a color
+ image in RGBa format.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayOpenCV::init(vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+
+ init (I.getWidth(), I.getHeight(), x, y, title) ;
+ I.display = this ;
+ displayHasBeenInitialized = true ;
+}
+
+/*!
+ Initialize the display size, position and title.
+
+ \param w, h : Width and height of the window.
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+ \exception vpDisplayException::notInitializedError If OpenCV was not build
+ with an available display device suach as Gtk, Cocoa, Carbon, Qt.
+*/
+void
+vpDisplayOpenCV::init(unsigned int w, unsigned int h,
+ int x, int y,
+ const char *title)
+{
+ this->width = w;
+ this->height = h;
+
+ if (x != -1)
+ this->windowXPosition = x;
+ if (y != -1)
+ this->windowYPosition = y;
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ int flags = CV_WINDOW_AUTOSIZE;
+#else
+ int flags = cv::WINDOW_AUTOSIZE;
+#endif
+
+ if(title_.empty()){
+ if(title != NULL){
+ title_ = std::string(title);
+ }
+ else{
+
+ std::ostringstream s;
+ s << m_nbWindows++;
+ title_ = std::string("Window ") + s.str();
+ }
+
+ bool isInList;
+ do{
+ isInList = false;
+ for(size_t i = 0 ; i < m_listTitles.size() ; i++){
+ if(m_listTitles[i] == title_){
+ std::ostringstream s;
+ s << m_nbWindows++;
+ title_ = std::string("Window ") + s.str();
+ isInList = true;
+ break;
+ }
+ }
+ }
+ while(isInList);
+
+ m_listTitles.push_back(title_);
+ }
+
+ /* Create the window*/
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if (cvNamedWindow( this->title_.c_str(), flags ) < 0) {
+ vpERROR_TRACE("OpenCV was not built with a display device");
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV was not built with a display device")) ;
+ }
+#else
+ cv::namedWindow( this->title_, flags );
+#endif
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvMoveWindow( this->title_.c_str(), this->windowXPosition, this->windowYPosition );
+#else
+ cv::moveWindow( this->title_.c_str(), this->windowXPosition, this->windowYPosition );
+#endif
+ move = false;
+ lbuttondown = false;
+ mbuttondown = false;
+ rbuttondown = false;
+ lbuttonup = false;
+ mbuttonup = false;
+ rbuttonup = false;
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvSetMouseCallback( this->title_.c_str(), on_mouse, this );
+ col = new CvScalar[vpColor::id_unknown] ;
+#else
+ cv::setMouseCallback( this->title_, on_mouse, this );
+ col = new cv::Scalar[vpColor::id_unknown] ;
+#endif
+
+ /* Create color */
+ vpColor pcolor; // Predefined colors
+ pcolor = vpColor::lightBlue;
+ col[vpColor::id_lightBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::blue;
+ col[vpColor::id_blue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::darkBlue;
+ col[vpColor::id_darkBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::lightRed;
+ col[vpColor::id_lightRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::red;
+ col[vpColor::id_red] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::darkRed;
+ col[vpColor::id_darkRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::lightGreen;
+ col[vpColor::id_lightGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::green;
+ col[vpColor::id_green] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::darkGreen;
+ col[vpColor::id_darkGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::yellow;
+ col[vpColor::id_yellow] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::cyan;
+ col[vpColor::id_cyan] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::orange;
+ col[vpColor::id_orange] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::purple;
+ col[vpColor::id_purple] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::white;
+ col[vpColor::id_white] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::black;
+ col[vpColor::id_black] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::lightGray;
+ col[vpColor::id_lightGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::gray;
+ col[vpColor::id_gray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ pcolor = vpColor::darkGray;
+ col[vpColor::id_darkGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ font = new CvFont;
+ cvInitFont( font, CV_FONT_HERSHEY_PLAIN, 0.70f,0.70f);
+ CvSize fontSize;
+ int baseline;
+ cvGetTextSize( "A", font, &fontSize, &baseline );
+#else
+ int thickness = 1;
+ cv::Size fontSize;
+ int baseline;
+ fontSize = cv::getTextSize( "A", font, fontScale, thickness, &baseline );
+#endif
+
+ fontHeight = fontSize.height + baseline;
+ displayHasBeenInitialized = true ;
+}
+
+
+/*!
+ \warning This method is not yet implemented.
+
+ Set the font used to display a text in overlay. The display is
+ performed using displayCharString().
+
+ \param font : The expected font name. The available fonts are given by
+ the "xlsfonts" binary. To choose a font you can also use the
+ "xfontsel" binary.
+
+ \note Under UNIX, to know all the available fonts, use the
+ "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
+
+ \sa displayCharString()
+*/
+void
+vpDisplayOpenCV::setFont(const char * /* font */)
+{
+ vpERROR_TRACE("Not yet implemented" ) ;
+}
+
+/*!
+ Set the window title.
+
+ \warning This method is not implemented yet.
+
+ \param title : Window title.
+ */
+void
+vpDisplayOpenCV::setTitle(const char * /* title */)
+{
+// static bool warn_displayed = false;
+// if (! warn_displayed) {
+// vpTRACE("Not implemented");
+// warn_displayed = true;
+// }
+}
+
+
+/*!
+ Set the window position in the screen.
+
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void vpDisplayOpenCV::setWindowPosition(int winx, int winy)
+{
+ if (displayHasBeenInitialized) {
+ this->windowXPosition = winx;
+ this->windowYPosition = winy;
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvMoveWindow( this->title_.c_str(), winx, winy );
+#else
+ cv::moveWindow( this->title_.c_str(), winx, winy );
+#endif
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+/*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning suppres the overlay drawing
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &I)
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImage<vpRGBa> Ic;
+ vpImageConvert::convert(I,Ic);
+ vpImageConvert::convert(Ic,background);
+ /* Copie de l'image dans le pixmap fond */
+ width = I.getWidth();
+ height = I.getHeight();
+ /* Le pixmap background devient le fond de la zone de dessin */
+ }
+ else
+ {
+ vpERROR_TRACE("openCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a selection of the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w, h : Width and height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP,
+ const unsigned int w, const unsigned int h )
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImage<unsigned char> Itemp;
+ vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(),h,w,Itemp);
+ vpImage<vpRGBa> Ic;
+ vpImageConvert::convert(Itemp,Ic);
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ int depth = 8;
+ int channels = 3;
+ CvSize size = cvSize((int)this->width, (int)this->height);
+ if (background != NULL){
+ if(background->nChannels != channels || background->depth != depth
+ || background->height != (int) I.getHeight() || background->width != (int) I.getWidth()){
+ if(background->nChannels != 0) cvReleaseImage(&background);
+ background = cvCreateImage( size, depth, channels );
+ }
+ }
+ else background = cvCreateImage( size, depth, channels );
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(Ic, Ip);
+ unsigned char * input = (unsigned char*)Ip->imageData;
+ unsigned char * output = (unsigned char*)background->imageData;
+
+ unsigned int iwidth = Ic.getWidth();
+
+ output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ *(output+3*j) = *(input+j*3);
+ *(output+3*j+1) = *(input+j*3+1);
+ *(output+3*j+2) = *(input+j*3+2);
+ j++;
+ }
+ input = input + 3*iwidth;
+ output = output + 3*this->width;
+ i++;
+ }
+
+ cvReleaseImage(&Ip);
+#else
+ int depth = CV_8U;
+ int channels = 3;
+ cv::Size size((int)this->width, (int)this->height);
+ if(background.channels() != channels || background.depth() != depth
+ || background.rows != (int) I.getHeight() || background.cols != (int) I.getWidth()){
+ background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ }
+
+ cv::Mat Ip;
+ vpImageConvert::convert(Ic, Ip);
+
+ unsigned char * input = (unsigned char*)Ip.data;
+ unsigned char * output = (unsigned char*)background.data;
+
+ unsigned int iwidth = Ic.getWidth();
+
+ output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ *(output+3*j) = *(input+j*3);
+ *(output+3*j+1) = *(input+j*3+1);
+ *(output+3*j+2) = *(input+j*3+2);
+ j++;
+ }
+ input = input + 3*iwidth;
+ output = output + 3*this->width;
+ i++;
+ }
+#endif
+ }
+ else
+ {
+ vpERROR_TRACE("openCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+
+/*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning suppres the overlay drawing
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &I)
+{
+
+ if (displayHasBeenInitialized)
+ {
+ /* Copie de l'image dans le pixmap fond */
+
+ vpImageConvert::convert(I,background);
+ /* Copie de l'image dans le pixmap fond */
+ width = I.getWidth();
+ height = I.getHeight();
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a selection of the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w, h : Width and height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayOpenCV::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
+ const unsigned int w, const unsigned int h )
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImage<vpRGBa> Ic;
+ vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(),h,w,Ic);
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ CvSize size = cvSize((int)this->width, (int)this->height);
+ int depth = 8;
+ int channels = 3;
+ if (background != NULL){
+ if(background->nChannels != channels || background->depth != depth
+ || background->height != (int) I.getHeight() || background->width != (int) I.getWidth()){
+ if(background->nChannels != 0) cvReleaseImage(&background);
+ background = cvCreateImage( size, depth, channels );
+ }
+ }
+ else background = cvCreateImage( size, depth, channels );
+
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(Ic, Ip);
+
+ unsigned char * input = (unsigned char*)Ip->imageData;
+ unsigned char * output = (unsigned char*)background->imageData;
+
+ unsigned int iwidth = Ic.getWidth();
+
+ output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ *(output+3*j) = *(input+j*3);
+ *(output+3*j+1) = *(input+j*3+1);
+ *(output+3*j+2) = *(input+j*3+2);
+ j++;
+ }
+ input = input + 3*iwidth;
+ output = output + 3*this->width;
+ i++;
+ }
+
+ cvReleaseImage(&Ip);
+#else
+ int depth = CV_8U;
+ int channels = 3;
+ cv::Size size((int)this->width, (int)this->height);
+ if(background.channels() != channels || background.depth() != depth
+ || background.rows != (int) I.getHeight() || background.cols != (int) I.getWidth()){
+ background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ }
+ cv::Mat Ip;
+ vpImageConvert::convert(Ic, Ip);
+
+ unsigned char * input = (unsigned char*)Ip.data;
+ unsigned char * output = (unsigned char*)background.data;
+
+ unsigned int iwidth = Ic.getWidth();
+
+ output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ *(output+3*j) = *(input+j*3);
+ *(output+3*j+1) = *(input+j*3+1);
+ *(output+3*j+2) = *(input+j*3+2);
+ j++;
+ }
+ input = input + 3*iwidth;
+ output = output + 3*this->width;
+ i++;
+ }
+#endif
+ }
+ else
+ {
+ vpERROR_TRACE("openCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+
+/*!
+ \warning ot implemented yet
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayOpenCV::displayImage(const unsigned char * /* I */)
+{
+ vpTRACE(" not implemented ") ;
+}
+
+/*!
+
+ Close the window.
+
+ \sa init()
+
+*/
+void vpDisplayOpenCV::closeDisplay()
+{
+ if (col != NULL)
+ {
+ delete [] col ; col = NULL ;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if (font != NULL)
+ {
+ delete font ;
+ font = NULL ;
+ }
+#endif
+ if (displayHasBeenInitialized) {
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvDestroyWindow( this->title_.c_str() );
+#else
+ cv::destroyWindow( this->title_ );
+#endif
+
+ for(size_t i = 0 ; i < m_listTitles.size() ; i++){
+ if(title_ == m_listTitles[i]){
+ m_listTitles.erase(m_listTitles.begin()+(long int)i);
+ break;
+ }
+ }
+
+ title_.clear();
+
+ displayHasBeenInitialized= false;
+ }
+}
+
+
+/*!
+ Flushes the OpenCV buffer.
+ It's necessary to use this function to see the results of any drawing.
+
+*/
+void vpDisplayOpenCV::flushDisplay()
+{
+ if (displayHasBeenInitialized)
+ {
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvShowImage(this->title_.c_str(), background );
+ cvWaitKey(5);
+#else
+ cv::imshow(this->title_, background );
+ cv::waitKey(5);
+#endif
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Flushes the OpenCV buffer.
+ It's necessary to use this function to see the results of any drawing.
+
+*/
+void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint &/*iP*/, const unsigned int /*width*/, const unsigned int /*height*/)
+{
+ if (displayHasBeenInitialized)
+ {
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvShowImage(this->title_.c_str(), background );
+ cvWaitKey(5);
+#else
+ cv::imshow(this->title_.c_str(), background );
+ cv::waitKey(5);
+#endif
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+
+/*!
+ \warning Not implemented yet.
+*/
+void vpDisplayOpenCV::clearDisplay(const vpColor & /* color */)
+{
+ static bool warn_displayed = false;
+ if (! warn_displayed) {
+ vpTRACE("Not implemented");
+ warn_displayed = true;
+ }
+}
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpDisplayOpenCV::displayArrow ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w, unsigned int h,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ try{
+ double a = ip2.get_i() - ip1.get_i() ;
+ double b = ip2.get_j() - ip1.get_j() ;
+ double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+
+ //if ((a==0)&&(b==0))
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon())
+ &&(std::fabs(b)<= std::numeric_limits<double>::epsilon()))
+ {
+ // DisplayCrossLarge(i1,j1,3,col) ;
+ }
+ else
+ {
+ a /= lg ;
+ b /= lg ;
+
+ vpImagePoint ip3;
+ ip3.set_i(ip2.get_i() - w*a);
+ ip3.set_j(ip2.get_j() - w*b);
+
+ vpImagePoint ip4;
+ ip4.set_i( ip3.get_i() - b*h );
+ ip4.set_j( ip3.get_j() + a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ ip4.set_i( ip3.get_i() + b*h );
+ ip4.set_j( ip3.get_j() - a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ displayLine ( ip1, ip2, color, thickness ) ;
+ }
+ }
+ catch (...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param ip : Upper left image point location of the string in the display.
+ \param text : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+*/
+void vpDisplayOpenCV::displayCharString( const vpImagePoint &ip,
+ const char *text,
+ const vpColor &color )
+{
+ if (displayHasBeenInitialized)
+ {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvPutText( background, text,
+ cvPoint( vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v()+fontHeight ) ),
+ font, col[color.id] );
+#else
+ cv::putText( background, text,
+ cv::Point( vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v()+fontHeight ) ),
+ font, fontScale, col[color.id] );
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvPutText( background, text,
+ cvPoint( vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v()+fontHeight ) ),
+ font, cvcolor );
+#else
+ cv::putText( background, text,
+ cv::Point( vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v()+fontHeight ) ),
+ font, fontScale, cvcolor );
+#endif
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+/*!
+ Display a circle.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the circle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er,
+ unsigned int radius,
+ const vpColor &color,
+ bool fill ,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ if (fill == false) {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvCircle( background,
+ cvPoint( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, col[color.id], (int)thickness);
+#else
+ cv::circle( background,
+ cv::Point( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, col[color.id], (int)thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvCircle( background,
+ cvPoint( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, cvcolor, (int)thickness);
+#else
+ cv::circle( background,
+ cv::Point( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, cvcolor, (int)thickness);
+#endif
+ }
+ }
+ else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ int filled = cv::FILLED;
+#else
+ int filled = CV_FILLED;
+#endif
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvCircle( background,
+ cvPoint( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, col[color.id], filled);
+#else
+ cv::circle( background,
+ cv::Point( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, col[color.id], filled);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvCircle( background,
+ cvPoint( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, cvcolor, filled);
+#else
+ cv::circle( background,
+ cv::Point( vpMath::round( center.get_u() ),
+ vpMath::round( center.get_v() ) ),
+ (int)radius, cvcolor, filled);
+#endif
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a cross at the image point \e ip location.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void
+vpDisplayOpenCV::displayCross(const vpImagePoint &ip,
+ unsigned int size,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ vpImagePoint top,bottom,left,right;
+ top.set_i(ip.get_i()-size/2);
+ top.set_j(ip.get_j());
+ bottom.set_i(ip.get_i()+size/2);
+ bottom.set_j(ip.get_j());
+ left.set_i(ip.get_i());
+ left.set_j(ip.get_j()-size/2);
+ right.set_i(ip.get_i());
+ right.set_j(ip.get_j()+size/2);
+ try{
+ displayLine(top, bottom, color, thickness) ;
+ displayLine(left, right, color, thickness) ;
+ }
+ catch (...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+
+}
+
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+
+ \warning Dot lines are not yet implemented in OpenCV. We display a
+ normal line instead.
+
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void
+vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness)
+{
+
+ if (displayHasBeenInitialized)
+ {
+ //vpTRACE("Dot lines are not yet implemented");
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvLine( background,
+ cvPoint( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cvPoint( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ col[color.id], (int) thickness);
+#else
+ cv::line( background,
+ cv::Point( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cv::Point( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ col[color.id], (int) thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvLine( background,
+ cvPoint( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cvPoint( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ cvcolor, (int) thickness);
+#else
+ cv::line( background,
+ cv::Point( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cv::Point( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ cvcolor, (int) thickness);
+#endif
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void
+vpDisplayOpenCV::displayLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvLine( background,
+ cvPoint( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cvPoint( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ col[color.id], (int) thickness);
+#else
+ cv::line( background,
+ cv::Point( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cv::Point( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ col[color.id], (int) thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvLine( background,
+ cvPoint( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cvPoint( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ cvcolor, (int) thickness);
+#else
+ cv::line( background,
+ cv::Point( vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ) ),
+ cv::Point( vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) ),
+ cvcolor, (int) thickness);
+#endif
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a point at the image point \e ip location.
+ \param ip : Point location.
+ \param color : Point color.
+*/
+void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip,
+ const vpColor &color)
+{
+ if (displayHasBeenInitialized)
+ {
+ displayLine(ip,ip,color,1);
+// if (color.id < vpColor::id_unknown) {
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3 )] = (uchar)col[color.id].val[0];
+//
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3+1 )] = (uchar)col[color.id].val[1];
+//
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3+2 )] = (uchar)col[color.id].val[2];
+// }
+// else {
+// cvcolor = CV_RGB(color.R, color.G, color.B) ;
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3 )] = (uchar)cvcolor.val[0];
+//
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3+1 )] = (uchar)cvcolor.val[1];
+//
+// ((uchar*)(background->imageData
+// + background->widthStep*vpMath::round( ip.get_i() )))
+// [vpMath::round( ip.get_j()*3+2 )] = (uchar)cvcolor.val[2];
+//
+// }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param w,h : Rectangle size in terms of width and height.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft,
+ unsigned int w, unsigned int h,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ if (fill == false) {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ col[color.id], (int)thickness);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ col[color.id], (int)thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ cvcolor, (int)thickness);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ cvcolor, (int)thickness);
+#endif
+ }
+ }
+ else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ int filled = cv::FILLED;
+#else
+ int filled = CV_FILLED;
+#endif
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ col[color.id], filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ col[color.id], filled);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ cvcolor, filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( topLeft.get_u()+w ),
+ vpMath::round( topLeft.get_v()+h ) ),
+ cvcolor, filled);
+#endif
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+/*!
+ Display a rectangle.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayOpenCV::displayRectangle ( const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if (displayHasBeenInitialized)
+ {
+ if (fill == false) {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ col[color.id], (int)thickness);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ col[color.id], (int)thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ cvcolor, (int)thickness);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ cvcolor, (int)thickness);
+#endif
+ }
+ }
+ else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ int filled = cv::FILLED;
+#else
+ int filled = CV_FILLED;
+#endif
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ col[color.id], filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ col[color.id], filled);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cvPoint( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ cvcolor, filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ) ),
+ cv::Point( vpMath::round( bottomRight.get_u() ),
+ vpMath::round( bottomRight.get_v() ) ),
+ cvcolor, filled);
+#endif
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+/*!
+ Display a rectangle.
+
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplayOpenCV::displayRectangle(const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ if (displayHasBeenInitialized)
+ {
+ if (fill == false) {
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cvPoint( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ col[color.id], (int)thickness);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cv::Point( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ col[color.id], (int)thickness);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cvPoint( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ cvcolor, (int)thickness);
+
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cv::Point( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ cvcolor, (int)thickness);
+
+#endif
+ }
+ }
+ else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ int filled = cv::FILLED;
+#else
+ int filled = CV_FILLED;
+#endif
+ if (color.id < vpColor::id_unknown) {
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cvPoint( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ col[color.id], filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cv::Point( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ col[color.id], filled);
+#endif
+ }
+ else {
+ cvcolor = CV_RGB(color.R, color.G, color.B) ;
+#if VISP_HAVE_OPENCV_VERSION < 0x020408
+ cvRectangle( background,
+ cvPoint( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cvPoint( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ cvcolor, filled);
+#else
+ cv::rectangle( background,
+ cv::Point( vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getBottom() ) ),
+ cv::Point( vpMath::round( rectangle.getRight() ),
+ vpMath::round( rectangle.getTop() ) ),
+ cvcolor, filled);
+#endif
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+}
+
+
+
+/*!
+ Wait for a click from one of the mouse button.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayOpenCV::getClick(bool blocking)
+{
+ bool ret = false;
+ if (displayHasBeenInitialized) {
+ flushDisplay() ;
+ if (blocking){
+ lbuttondown = false;
+ mbuttondown = false;
+ rbuttondown = false;
+ }
+ do {
+ if (lbuttondown){
+ ret = true ;
+ lbuttondown = false;
+ }
+ if (mbuttondown){
+ ret = true ;
+ mbuttondown = false;
+ }
+ if (rbuttondown){
+ ret = true ;
+ rbuttondown = false;
+ }
+ if (blocking)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvWaitKey(10);
+#else
+ cv::waitKey(10);
+#endif
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+*/
+bool
+vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+ flushDisplay() ;
+
+ double u, v;
+
+ if (blocking){
+ lbuttondown = false;
+ mbuttondown = false;
+ rbuttondown = false;
+ }
+ do {
+ if (lbuttondown){
+ ret = true ;
+ u = (unsigned int)x_lbuttondown;
+ v = (unsigned int)y_lbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ lbuttondown = false;
+ }
+ if (mbuttondown){
+ ret = true ;
+ u = (unsigned int)x_mbuttondown;
+ v = (unsigned int)y_mbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ mbuttondown = false;
+ }
+ if (rbuttondown){
+ ret = true ;
+ u = (unsigned int)x_rbuttondown;
+ v = (unsigned int)y_rbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ rbuttondown = false;
+ }
+ if (blocking)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvWaitKey(10);
+#else
+ cv::waitKey(10);
+#endif
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ return ret ;
+}
+
+
+/*!
+
+ Wait for a mouse button click and get the position of the clicked
+ pixel. The button used to click is also set.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+*/
+bool
+vpDisplayOpenCV::getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+ //flushDisplay() ;
+ double u, v;
+ if (blocking){
+ lbuttondown = false;
+ mbuttondown = false;
+ rbuttondown = false;
+ }
+ do {
+ if (lbuttondown){
+ ret = true ;
+ u = (unsigned int)x_lbuttondown;
+ v = (unsigned int)y_lbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button1;
+ lbuttondown = false;
+ }
+ if (mbuttondown){
+ ret = true ;
+ u = (unsigned int)x_mbuttondown;
+ v = (unsigned int)y_mbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button2;
+ mbuttondown = false;
+ }
+ if (rbuttondown){
+ ret = true ;
+ u = (unsigned int)x_rbuttondown;
+ v = (unsigned int)y_rbuttondown;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button3;
+ rbuttondown = false;
+ }
+ if (blocking)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvWaitKey(10);
+#else
+ cv::waitKey(10);
+#endif
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Wait for a mouse button click release and get the position of the
+ image point were the click release occurs. The button used to click is
+ also set. Same method as getClick(unsigned int&, unsigned int&,
+ vpMouseButton::vpMouseButtonType &, bool).
+
+ \param ip [out] : Position of the clicked image point.
+
+ \param button [in] : Button used to click.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+ \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
+
+*/
+bool
+vpDisplayOpenCV::getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ bool ret = false;
+ if (displayHasBeenInitialized) {
+ //flushDisplay() ;
+ double u, v;
+ if (blocking){
+ lbuttonup = false;
+ mbuttonup = false;
+ rbuttonup = false;
+ }
+ do {
+ if (lbuttonup){
+ ret = true ;
+ u = (unsigned int)x_lbuttonup;
+ v = (unsigned int)y_lbuttonup;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button1;
+ lbuttonup = false;
+ }
+ if (mbuttonup){
+ ret = true ;
+ u = (unsigned int)x_mbuttonup;
+ v = (unsigned int)y_mbuttonup;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button2;
+ mbuttonup = false;
+ }
+ if (rbuttonup){
+ ret = true ;
+ u = (unsigned int)x_rbuttonup;
+ v = (unsigned int)y_rbuttonup;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = vpMouseButton::button3;
+ rbuttonup = false;
+ }
+ if (blocking)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvWaitKey(10);
+#else
+ cv::waitKey(10);
+#endif
+ } while ( ret == false && blocking == true);
+ }
+ else {
+ vpERROR_TRACE ( "OpenCV not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "OpenCV not initialized" ) ) ;
+ }
+ return ret;
+}
+
+/*
+ \brief gets the displayed image (including the overlay plane)
+ and returns an RGBa image
+*/
+void vpDisplayOpenCV::getImage(vpImage<vpRGBa> &I)
+{
+ vpImageConvert::convert(background,I);
+ // should certainly be optimized.
+}
+
+void vpDisplayOpenCV::on_mouse( int event, int x, int y, int /*flags*/, void* display )
+{
+ vpDisplayOpenCV* disp = (vpDisplayOpenCV*)display;
+ switch ( event )
+ {
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_MOUSEMOVE:
+#else
+ case cv::EVENT_MOUSEMOVE:
+#endif
+ {
+ disp->move = true;
+ disp->x_move = x;
+ disp->y_move = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_LBUTTONDOWN:
+#else
+ case cv::EVENT_LBUTTONDOWN:
+#endif
+ {
+ disp->lbuttondown = true;
+ disp->x_lbuttondown = x;
+ disp->y_lbuttondown = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_MBUTTONDOWN:
+#else
+ case cv::EVENT_MBUTTONDOWN:
+#endif
+ {
+ disp->mbuttondown = true;
+ disp->x_mbuttondown = x;
+ disp->y_mbuttondown = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_RBUTTONDOWN:
+#else
+ case cv::EVENT_RBUTTONDOWN:
+#endif
+ {
+ disp->rbuttondown = true;
+ disp->x_rbuttondown = x;
+ disp->y_rbuttondown = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_LBUTTONUP:
+#else
+ case cv::EVENT_LBUTTONUP:
+#endif
+ {
+ disp->lbuttonup = true;
+ disp->x_lbuttonup = x;
+ disp->y_lbuttonup = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_MBUTTONUP:
+#else
+ case cv::EVENT_MBUTTONUP:
+#endif
+ {
+ disp->mbuttonup = true;
+ disp->x_mbuttonup = x;
+ disp->y_mbuttonup = y;
+ break;
+ }
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ case CV_EVENT_RBUTTONUP:
+#else
+ case cv::EVENT_RBUTTONUP:
+#endif
+ {
+ disp->rbuttonup = true;
+ disp->x_rbuttonup = x;
+ disp->y_rbuttonup = y;
+ break;
+ }
+
+ default :
+ break;
+ }
+}
+
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayOpenCV::getKeyboardEvent(bool blocking)
+{
+ int key_pressed;
+ int delay;
+ if (displayHasBeenInitialized) {
+ flushDisplay() ;
+ if (blocking)
+ delay = 0;
+ else
+ delay = 10;
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ key_pressed = cvWaitKey(delay);
+#else
+ key_pressed = cv::waitKey(delay);
+#endif
+
+ if (key_pressed == -1)
+ return false;
+ return true;
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ //return false; // Never reached after throw()
+}
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayOpenCV::getKeyboardEvent(char *string, bool blocking)
+{
+ int key_pressed;
+ int delay;
+ if (displayHasBeenInitialized) {
+ flushDisplay() ;
+ if (blocking)
+ delay = 0;
+ else
+ delay = 10;
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ key_pressed = cvWaitKey(delay);
+#else
+ key_pressed = cv::waitKey(delay);
+#endif
+ if (key_pressed == -1)
+ return false;
+ else {
+ //std::cout << "Key pressed: \"" << key_pressed << "\"" << std::endl;
+ sprintf(string, "%c", key_pressed);
+ }
+ return true;
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ //return false; // Never reached after throw()
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \warning Not implemented yet.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayOpenCV::getPointerMotionEvent (vpImagePoint &ip )
+{
+ bool ret = false;
+
+ if (displayHasBeenInitialized) {
+ //flushDisplay() ;
+ double u, v;
+ if (move){
+ ret = true ;
+ u = (unsigned int)x_move;
+ v = (unsigned int)y_move;
+ ip.set_u( u );
+ ip.set_v( v );
+ move = false;
+ }
+ }
+
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ return ret;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayOpenCV::getPointerPosition ( vpImagePoint &ip)
+{
+ if (displayHasBeenInitialized) {
+ //vpTRACE("Not implemented yet");
+ bool moved = getPointerMotionEvent(ip);
+ if (!moved)
+ {
+ double u, v;
+ u = (unsigned int)x_move;
+ v = (unsigned int)y_move;
+ ip.set_u( u );
+ ip.set_v( v );
+ }
+ return false;
+ }
+ else {
+ vpERROR_TRACE("OpenCV not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "OpenCV not initialized")) ;
+ }
+ //return false; // Never reached after throw()
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayOpenCV.cpp.o) has no symbols
+void dummy_vpDisplayOpenCV() {};
+#endif
diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp
new file mode 100644
index 0000000..b8ea636
--- /dev/null
+++ b/modules/gui/src/display/vpDisplayX.cpp
@@ -0,0 +1,3402 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Image display.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpDisplayX.cpp
+ \brief Define the X11 console to display images.
+*/
+
+
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_X11
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+// Display stuff
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayX.h>
+
+//debug / exception
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpDisplayException.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+/*!
+
+ Constructor : initialize a display to visualize a gray level image
+ (8 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+vpDisplayX::vpDisplayX ( vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title )
+ : display(NULL), window(), Ximage(NULL), lut(), context(),
+ screen(0), event(), pixmap(), x_color(NULL),
+ screen_depth(8), xcolor(), values(), ximage_data_init(false),
+ RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+{
+ init ( I, x, y, title ) ;
+}
+
+
+
+/*!
+ Constructor : initialize a display to visualize a RGBa level image
+ (32 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized).
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title )
+ : display(NULL), window(), Ximage(NULL), lut(), context(),
+ screen(0), event(), pixmap(), x_color(NULL),
+ screen_depth(8), xcolor(), values(), ximage_data_init(false),
+ RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+{
+ init ( I, x, y, title ) ;
+}
+
+/*!
+
+ Constructor that just initialize the display position in the screen
+ and the display title.
+
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+ To initialize the display size, you need to call init().
+
+ \code
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayX d(100, 200, "My display");
+ vpImage<unsigned char> I(240, 384);
+ d.init(I);
+}
+ \endcode
+*/
+vpDisplayX::vpDisplayX ( int x, int y, const char *title )
+ : display(NULL), window(), Ximage(NULL), lut(), context(),
+ screen(0), event(), pixmap(), x_color(NULL),
+ screen_depth(8), xcolor(), values(), ximage_data_init(false),
+ RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+{
+ windowXPosition = x ;
+ windowYPosition = y ;
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+}
+
+/*!
+ Basic constructor.
+
+ To initialize the window position, title and size you may call
+ init(vpImage<unsigned char> &, int, int, const char *) or
+ init(vpImage<vpRGBa> &, int, int, const char *).
+
+ \code
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+ vpDisplayX d;
+ vpImage<unsigned char> I(240, 384);
+ d.init(I, 100, 200, "My display");
+}
+ \endcode
+*/
+vpDisplayX::vpDisplayX()
+ : display(NULL), window(), Ximage(NULL), lut(), context(),
+ screen(0), event(), pixmap(), x_color(NULL),
+ screen_depth(8), xcolor(), values(), ximage_data_init(false),
+ RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+{
+}
+
+/*!
+ Destructor.
+*/
+vpDisplayX::~vpDisplayX()
+{
+ closeDisplay() ;
+}
+
+/*!
+ Initialize the display (size, position and title) of a gray level image.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const char *title )
+{
+
+ if (x_color == NULL) {
+ // id_unknown = number of predefined colors
+ x_color= new unsigned long [vpColor::id_unknown];
+ }
+
+ XSizeHints hints;
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+
+ // Positionnement de la fenetre dans l'ecran.
+ if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
+ {
+ hints.flags = 0;
+ }
+ else
+ {
+ hints.flags = USPosition;
+ hints.x = windowXPosition;
+ hints.y = windowYPosition;
+ }
+
+ // setup X11 --------------------------------------------------
+ width = I.getWidth();
+ height = I.getHeight();
+ display = XOpenDisplay ( NULL );
+ if ( display == NULL )
+ {
+ vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
+ throw ( vpDisplayException ( vpDisplayException::connexionError,
+ "Can't connect display on server." ) ) ;
+ }
+
+ screen = DefaultScreen ( display );
+ lut = DefaultColormap ( display, screen );
+ screen_depth = (unsigned int)DefaultDepth ( display, screen );
+
+ if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
+ windowXPosition, windowYPosition, width, height, 1,
+ BlackPixel ( display, screen ),
+ WhitePixel ( display, screen ) ) ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't create window." );
+ throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
+ "Can't create window." ) ) ;
+ }
+
+ //
+ // Create color table for 8 and 16 bits screen
+ //
+ if ( screen_depth == 8 )
+ {
+ lut = XCreateColormap ( display, window,
+ DefaultVisual ( display, screen ), AllocAll ) ;
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ for ( unsigned int i = 0 ; i < 256 ; i++ )
+ {
+ xcolor.pixel = i ;
+ xcolor.red = 256 * i;
+ xcolor.green = 256 * i;
+ xcolor.blue = 256 * i;
+ XStoreColor ( display, lut, &xcolor );
+ }
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+ }
+
+ else if ( screen_depth == 16 )
+ {
+ for ( unsigned int i = 0; i < 256; i ++ )
+ {
+ xcolor.pad = 0;
+ xcolor.red = xcolor.green = xcolor.blue = 256 * i;
+ if ( XAllocColor ( display, lut, &xcolor ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
+ throw ( vpDisplayException ( vpDisplayException::colorAllocError,
+ "Can't allocate 256 colors." ) ) ;
+ }
+ colortable[i] = xcolor.pixel;
+ }
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+
+ Visual *visual = DefaultVisual (display, screen);
+ RMask = visual->red_mask;
+ GMask = visual->green_mask;
+ BMask = visual->blue_mask;
+
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ GShift = 15 - getMsb(GMask);
+ BShift = 15 - getMsb(BMask);
+ }
+
+ //
+ // Create colors for overlay
+ //
+ switch ( screen_depth )
+ {
+ case 8:
+ // Color BLACK and WHITE are set properly by default.
+
+ // Color LIGHT GRAY.
+ x_color[vpColor::id_lightGray] = 254;
+ xcolor.pixel = x_color[vpColor::id_lightGray] ;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GRAY.
+ x_color[vpColor::id_gray] = 253;
+ xcolor.pixel = x_color[vpColor::id_gray] ;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GRAY.
+ x_color[vpColor::id_darkGray] = 252;
+ xcolor.pixel = x_color[vpColor::id_darkGray] ;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT RED.
+ x_color[vpColor::id_lightRed] = 251;
+ xcolor.pixel = x_color[vpColor::id_lightRed] ;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color RED.
+ x_color[vpColor::id_red] = 250;
+ xcolor.pixel = x_color[vpColor::id_red] ;
+ xcolor.red = 256 * 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK RED.
+ x_color[vpColor::id_darkRed] = 249;
+ xcolor.pixel = x_color[vpColor::id_darkRed] ;
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT GREEN.
+ x_color[vpColor::id_lightGreen] = 248;
+ xcolor.pixel = x_color[vpColor::id_lightGreen] ;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GREEN.
+ x_color[vpColor::id_green] = 247;
+ xcolor.pixel = x_color[vpColor::id_green];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GREEN.
+ x_color[vpColor::id_darkGreen] = 246;
+ xcolor.pixel = x_color[vpColor::id_darkGreen] ;
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT BLUE.
+ x_color[vpColor::id_lightBlue] = 245;
+ xcolor.pixel = x_color[vpColor::id_lightBlue] ;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color BLUE.
+ x_color[vpColor::id_blue] = 244;
+ xcolor.pixel = x_color[vpColor::id_blue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK BLUE.
+ x_color[vpColor::id_darkBlue] = 243;
+ xcolor.pixel = x_color[vpColor::id_darkBlue] ;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color YELLOW.
+ x_color[vpColor::id_yellow] = 242;
+ xcolor.pixel = x_color[vpColor::id_yellow];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color ORANGE.
+ x_color[vpColor::id_orange] = 241;
+ xcolor.pixel = x_color[vpColor::id_orange];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color CYAN.
+ x_color[vpColor::id_cyan] = 240;
+ xcolor.pixel = x_color[vpColor::id_cyan];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color PURPLE.
+ x_color[vpColor::id_purple] = 239;
+ xcolor.pixel = x_color[vpColor::id_purple];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ break;
+
+ case 16:
+ case 24:
+ {
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ // Couleur BLACK.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_black] = xcolor.pixel;
+
+ // Couleur WHITE.
+ xcolor.pad = 0;
+ xcolor.red = 256* 255;
+ xcolor.green = 256* 255;
+ xcolor.blue = 256* 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_white] = xcolor.pixel;
+
+ // Couleur LIGHT GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGray] = xcolor.pixel;
+
+ // Couleur GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_gray] = xcolor.pixel;
+
+ // Couleur DARK GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGray] = xcolor.pixel;
+
+ // Couleur LIGHT RED.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightRed] = xcolor.pixel;
+
+ // Couleur RED.
+ xcolor.pad = 0;
+ xcolor.red = 256* 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_red] = xcolor.pixel;
+
+ // Couleur DARK RED.
+ xcolor.pad = 0;
+ xcolor.red = 256* 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkRed] = xcolor.pixel;
+
+ // Couleur LIGHT GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGreen] = xcolor.pixel;
+
+ // Couleur GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256*255;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_green] = xcolor.pixel;
+
+ // Couleur DARK GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256* 128;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGreen] = xcolor.pixel;
+
+ // Couleur LIGHT Blue.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightBlue] = xcolor.pixel;
+
+ // Couleur BLUE.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256* 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_blue] = xcolor.pixel;
+
+ // Couleur DARK BLUE.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256* 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkBlue] = xcolor.pixel;
+
+ // Couleur YELLOW.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_yellow] = xcolor.pixel;
+
+ // Couleur ORANGE.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_orange] = xcolor.pixel;
+
+ // Couleur CYAN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_cyan] = xcolor.pixel;
+
+ // Couleur PURPLE.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_purple] = xcolor.pixel;
+ break;
+ }
+ }
+
+ XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
+ XMapWindow ( display, window ) ;
+ // Selection des evenements.
+ XSelectInput ( display, window,
+ ExposureMask |
+ ButtonPressMask | ButtonReleaseMask |
+ KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask |
+ PointerMotionMask);
+
+ // graphic context creation
+ values.plane_mask = AllPlanes;
+ values.fill_style = FillSolid;
+ values.foreground = WhitePixel ( display, screen );
+ values.background = BlackPixel ( display, screen );
+ context = XCreateGC ( display, window,
+ GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
+ &values );
+
+ if ( context == NULL )
+ {
+ vpERROR_TRACE ( "Can't create graphics context." );
+ throw ( vpDisplayException ( vpDisplayException::XWindowsError,
+ "Can't create graphics context" ) ) ;
+
+ }
+
+ // Pixmap creation.
+ pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
+
+ // Hangs when forward X11 is used to send the display to an other computer
+// do
+// XNextEvent ( display, &event );
+// while ( event.xany.type != Expose );
+
+ {
+ Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
+ screen_depth, ZPixmap, 0, NULL,
+ I.getWidth() , I.getHeight(), XBitmapPad ( display ), 0 );
+
+ Ximage->data = ( char * ) malloc ( I.getHeight() * (unsigned int)Ximage->bytes_per_line );
+ ximage_data_init = true;
+
+ }
+ displayHasBeenInitialized = true ;
+
+ XStoreName ( display, window, title_.c_str() );
+
+ XSync ( display, 1 );
+
+ I.display = this ;
+}
+
+/*!
+ Initialize the display (size, position and title) of a color
+ image in RGBa format.
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void
+vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const char *title )
+{
+
+ XSizeHints hints;
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+
+ if (x_color == NULL) {
+ // id_unknown = number of predefined colors
+ x_color= new unsigned long [vpColor::id_unknown];
+ }
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+
+ // Positionnement de la fenetre dans l'ecran.
+ if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
+ {
+ hints.flags = 0;
+ }
+ else
+ {
+ hints.flags = USPosition;
+ hints.x = windowXPosition;
+ hints.y = windowYPosition;
+ }
+
+ // setup X11 --------------------------------------------------
+ width = I.getWidth();
+ height = I.getHeight();
+
+ if ( ( display = XOpenDisplay ( NULL ) ) == NULL )
+ {
+ vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
+ throw ( vpDisplayException ( vpDisplayException::connexionError,
+ "Can't connect display on server." ) ) ;
+ }
+
+ screen = DefaultScreen ( display );
+ lut = DefaultColormap ( display, screen );
+ screen_depth = (unsigned int)DefaultDepth ( display, screen );
+
+ vpDEBUG_TRACE ( 1, "Screen depth: %d\n", screen_depth );
+
+ if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
+ windowXPosition, windowYPosition,
+ width, height, 1,
+ BlackPixel ( display, screen ),
+ WhitePixel ( display, screen ) ) ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't create window." );
+ throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
+ "Can't create window." ) ) ;
+ }
+
+ //
+ // Create color table for 8 and 16 bits screen
+ //
+ if ( screen_depth == 8 )
+ {
+ lut = XCreateColormap ( display, window,
+ DefaultVisual ( display, screen ), AllocAll ) ;
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ for ( unsigned int i = 0 ; i < 256 ; i++ )
+ {
+ xcolor.pixel = i ;
+ xcolor.red = 256 * i;
+ xcolor.green = 256 * i;
+ xcolor.blue = 256 * i;
+ XStoreColor ( display, lut, &xcolor );
+ }
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+ }
+
+ else if ( screen_depth == 16 )
+ {
+ for ( unsigned int i = 0; i < 256; i ++ )
+ {
+ xcolor.pad = 0;
+ xcolor.red = xcolor.green = xcolor.blue = 256 * i;
+ if ( XAllocColor ( display, lut, &xcolor ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
+ throw ( vpDisplayException ( vpDisplayException::colorAllocError,
+ "Can't allocate 256 colors." ) ) ;
+ }
+ colortable[i] = xcolor.pixel;
+ }
+
+ Visual *visual = DefaultVisual (display, screen);
+ RMask = visual->red_mask;
+ GMask = visual->green_mask;
+ BMask = visual->blue_mask;
+
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ GShift = 15 - getMsb(GMask);
+ BShift = 15 - getMsb(BMask);
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+ }
+
+
+ //
+ // Create colors for overlay
+ //
+ switch ( screen_depth )
+ {
+
+ case 8:
+ // Color BLACK and WHITE are set properly.
+
+ // Color LIGHT GRAY.
+ x_color[vpColor::id_lightGray] = 254;
+ xcolor.pixel = x_color[vpColor::id_lightGray] ;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GRAY.
+ x_color[vpColor::id_gray] = 253;
+ xcolor.pixel = x_color[vpColor::id_gray] ;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GRAY.
+ x_color[vpColor::id_darkGray] = 252;
+ xcolor.pixel = x_color[vpColor::id_darkGray] ;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT RED.
+ x_color[vpColor::id_lightRed] = 251;
+ xcolor.pixel = x_color[vpColor::id_lightRed] ;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color RED.
+ x_color[vpColor::id_red] = 250;
+ xcolor.pixel = x_color[vpColor::id_red] ;
+ xcolor.red = 256 * 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK RED.
+ x_color[vpColor::id_darkRed] = 249;
+ xcolor.pixel = x_color[vpColor::id_darkRed] ;
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT GREEN.
+ x_color[vpColor::id_lightGreen] = 248;
+ xcolor.pixel = x_color[vpColor::id_lightGreen] ;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GREEN.
+ x_color[vpColor::id_green] = 247;
+ xcolor.pixel = x_color[vpColor::id_green];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GREEN.
+ x_color[vpColor::id_darkGreen] = 246;
+ xcolor.pixel = x_color[vpColor::id_darkGreen] ;
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT BLUE.
+ x_color[vpColor::id_lightBlue] = 245;
+ xcolor.pixel = x_color[vpColor::id_lightBlue] ;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color BLUE.
+ x_color[vpColor::id_blue] = 244;
+ xcolor.pixel = x_color[vpColor::id_blue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK BLUE.
+ x_color[vpColor::id_darkBlue] = 243;
+ xcolor.pixel = x_color[vpColor::id_darkBlue] ;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color YELLOW.
+ x_color[vpColor::id_yellow] = 242;
+ xcolor.pixel = x_color[vpColor::id_yellow];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color ORANGE.
+ x_color[vpColor::id_orange] = 241;
+ xcolor.pixel = x_color[vpColor::id_orange];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color CYAN.
+ x_color[vpColor::id_cyan] = 240;
+ xcolor.pixel = x_color[vpColor::id_cyan];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color PURPLE.
+ x_color[vpColor::id_purple] = 239;
+ xcolor.pixel = x_color[vpColor::id_purple];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor ( display, lut, &xcolor );
+
+ break;
+
+ case 16:
+ case 24:
+ {
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ // Couleur BLACK.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_black] = xcolor.pixel;
+
+ // Couleur WHITE.
+ xcolor.pad = 0;
+ xcolor.red = 256* 255;
+ xcolor.green = 256* 255;
+ xcolor.blue = 256* 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_white] = xcolor.pixel;
+
+ // Couleur LIGHT GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGray] = xcolor.pixel;
+
+ // Couleur GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_gray] = xcolor.pixel;
+
+ // Couleur DARK GRAY.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGray] = xcolor.pixel;
+
+ // Couleur LIGHT RED.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightRed] = xcolor.pixel;
+
+ // Couleur RED.
+ xcolor.pad = 0;
+ xcolor.red = 256* 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_red] = xcolor.pixel;
+
+ // Couleur DARK RED.
+ xcolor.pad = 0;
+ xcolor.red = 256* 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkRed] = xcolor.pixel;
+
+ // Couleur LIGHT GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGreen] = xcolor.pixel;
+
+ // Couleur GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256*255;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_green] = xcolor.pixel;
+
+ // Couleur DARK GREEN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256* 128;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGreen] = xcolor.pixel;
+
+ // Couleur LIGHT Blue.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightBlue] = xcolor.pixel;
+
+ // Couleur BLUE.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256* 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_blue] = xcolor.pixel;
+
+ // Couleur DARK BLUE.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256* 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkBlue] = xcolor.pixel;
+
+ // Couleur YELLOW.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_yellow] = xcolor.pixel;
+
+ // Couleur ORANGE.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_orange] = xcolor.pixel;
+
+ // Couleur CYAN.
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_cyan] = xcolor.pixel;
+
+ // Couleur PURPLE.
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_purple] = xcolor.pixel;
+ break;
+ }
+ }
+
+ XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
+ XMapWindow ( display, window ) ;
+ // Selection des evenements.
+ XSelectInput ( display, window,
+ ExposureMask |
+ ButtonPressMask | ButtonReleaseMask |
+ KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask |
+ PointerMotionMask);
+
+ // Creation du contexte graphique
+ values.plane_mask = AllPlanes;
+ values.fill_style = FillSolid;
+ values.foreground = WhitePixel ( display, screen );
+ values.background = BlackPixel ( display, screen );
+ context = XCreateGC ( display, window,
+ GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
+ &values );
+
+ if ( context == NULL )
+ {
+ vpERROR_TRACE ( "Can't create graphics context." );
+ throw ( vpDisplayException ( vpDisplayException::XWindowsError,
+ "Can't create graphics context" ) ) ;
+ }
+
+ // Pixmap creation.
+ pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
+
+ // Hangs when forward X11 is used to send the display to an other computer
+// do
+// XNextEvent ( display, &event );
+// while ( event.xany.type != Expose );
+
+ {
+ Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
+ screen_depth, ZPixmap, 0, NULL,
+ I.getWidth() , I.getHeight(), XBitmapPad ( display ), 0 );
+
+
+ Ximage->data = ( char * ) malloc ( I.getHeight() * (unsigned int)Ximage->bytes_per_line );
+ ximage_data_init = true;
+
+ }
+ displayHasBeenInitialized = true ;
+
+ XSync ( display, true );
+
+ XStoreName ( display, window, title_.c_str() );
+
+ I.display = this ;
+}
+
+
+/*!
+ Initialize the display size, position and title.
+
+ \param w, h : Width and height of the window.
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const char *title )
+{
+ if (x_color == NULL) {
+ // id_unknown = number of predefined colors
+ x_color= new unsigned long [vpColor::id_unknown];
+ }
+ /* setup X11 ------------------------------------------------------------- */
+ this->width = w;
+ this->height = h;
+
+ XSizeHints hints;
+
+ if (x != -1)
+ windowXPosition = x ;
+ if (y != -1)
+ windowYPosition = y ;
+ // Positionnement de la fenetre dans l'ecran.
+ if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
+ {
+ hints.flags = 0;
+ }
+ else
+ {
+ hints.flags = USPosition;
+ hints.x = windowXPosition;
+ hints.y = windowYPosition;
+ }
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+
+ if ( ( display = XOpenDisplay ( NULL ) ) == NULL )
+ {
+ vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
+ throw ( vpDisplayException ( vpDisplayException::connexionError,
+ "Can't connect display on server." ) ) ;
+ }
+
+ screen = DefaultScreen ( display );
+ lut = DefaultColormap ( display, screen );
+ screen_depth = (unsigned int)DefaultDepth ( display, screen );
+
+ vpTRACE ( "Screen depth: %d\n", screen_depth );
+
+ if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
+ windowXPosition, windowYPosition,
+ width, height, 1,
+ BlackPixel ( display, screen ),
+ WhitePixel ( display, screen ) ) ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't create window." );
+ throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
+ "Can't create window." ) ) ;
+ }
+
+
+ //
+ // Create color table for 8 and 16 bits screen
+ //
+ if ( screen_depth == 8 )
+ {
+ lut = XCreateColormap ( display, window,
+ DefaultVisual ( display, screen ), AllocAll ) ;
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ for ( unsigned int i = 0 ; i < 256 ; i++ )
+ {
+ xcolor.pixel = i ;
+ xcolor.red = 256 * i;
+ xcolor.green = 256 * i;
+ xcolor.blue = 256 * i;
+ XStoreColor ( display, lut, &xcolor );
+ }
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+ }
+
+ else if ( screen_depth == 16 )
+ {
+ for ( unsigned int i = 0; i < 256; i ++ )
+ {
+ xcolor.pad = 0;
+ xcolor.red = xcolor.green = xcolor.blue = 256 * i;
+ if ( XAllocColor ( display, lut, &xcolor ) == 0 )
+ {
+ vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
+ throw ( vpDisplayException ( vpDisplayException::colorAllocError,
+ "Can't allocate 256 colors." ) ) ;
+ }
+ colortable[i] = xcolor.pixel;
+ }
+
+ XSetWindowColormap ( display, window, lut ) ;
+ XInstallColormap ( display, lut ) ;
+
+ Visual *visual = DefaultVisual (display, screen);
+ RMask = visual->red_mask;
+ GMask = visual->green_mask;
+ BMask = visual->blue_mask;
+
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ GShift = 15 - getMsb(GMask);
+ BShift = 15 - getMsb(BMask);
+ }
+
+ vpColor pcolor; // predefined colors
+
+ //
+ // Create colors for overlay
+ //
+ switch ( screen_depth )
+ {
+
+ case 8:
+ // Color BLACK: default set to 0
+
+ // Color WHITE: default set to 255
+
+ // Color LIGHT GRAY.
+ pcolor = vpColor::lightGray;
+ xcolor.pixel = 254 ; // affected to 254
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GRAY.
+ pcolor = vpColor::gray;
+ xcolor.pixel = 253 ; // affected to 253
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GRAY.
+ pcolor = vpColor::darkGray;
+ xcolor.pixel = 252 ; // affected to 252
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT RED.
+ pcolor = vpColor::lightRed;
+ xcolor.pixel = 251 ; // affected to 251
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color RED.
+ pcolor = vpColor::red;
+ xcolor.pixel = 250 ; // affected to 250
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK RED.
+ pcolor = vpColor::darkRed;
+ xcolor.pixel = 249 ; // affected to 249
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT GREEN.
+ pcolor = vpColor::lightGreen;
+ xcolor.pixel = 248 ; // affected to 248
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color GREEN.
+ pcolor = vpColor::green;
+ xcolor.pixel = 247; // affected to 247
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK GREEN.
+ pcolor = vpColor::darkGreen;
+ xcolor.pixel = 246 ; // affected to 246
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color LIGHT BLUE.
+ pcolor = vpColor::lightBlue;
+ xcolor.pixel = 245 ; // affected to 245
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color BLUE.
+ pcolor = vpColor::blue;
+ xcolor.pixel = 244; // affected to 244
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color DARK BLUE.
+ pcolor = vpColor::darkBlue;
+ xcolor.pixel = 243 ; // affected to 243
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color YELLOW.
+ pcolor = vpColor::yellow;
+ xcolor.pixel = 242; // affected to 242
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color ORANGE.
+ pcolor = vpColor::orange;
+ xcolor.pixel = 241; // affected to 241
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color CYAN.
+ pcolor = vpColor::cyan;
+ xcolor.pixel = 240; // affected to 240
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ // Color PURPLE.
+ pcolor = vpColor::purple;
+ xcolor.pixel = 239; // affected to 239
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor ( display, lut, &xcolor );
+
+ break;
+
+ case 16:
+ case 24:
+ {
+ xcolor.flags = DoRed | DoGreen | DoBlue ;
+
+ // Couleur BLACK.
+ pcolor = vpColor::black;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_black] = xcolor.pixel;
+
+ // Color WHITE.
+ pcolor = vpColor::white;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_white] = xcolor.pixel;
+
+ // Color LIGHT GRAY.
+ pcolor = vpColor::lightGray;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGray] = xcolor.pixel;
+
+ // Color GRAY.
+ pcolor = vpColor::gray;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_gray] = xcolor.pixel;
+
+ // Color DARK GRAY.
+ pcolor = vpColor::darkGray;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGray] = xcolor.pixel;
+
+ // Color LIGHT RED.
+ pcolor = vpColor::lightRed;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightRed] = xcolor.pixel;
+
+ // Color RED.
+ pcolor = vpColor::red;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_red] = xcolor.pixel;
+
+ // Color DARK RED.
+ pcolor = vpColor::darkRed;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkRed] = xcolor.pixel;
+
+ // Color LIGHT GREEN.
+ pcolor = vpColor::lightGreen;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightGreen] = xcolor.pixel;
+
+ // Color GREEN.
+ pcolor = vpColor::green;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_green] = xcolor.pixel;
+
+ // Color DARK GREEN.
+ pcolor = vpColor::darkGreen;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkGreen] = xcolor.pixel;
+
+ // Color LIGHT BLUE.
+ pcolor = vpColor::lightBlue;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_lightBlue] = xcolor.pixel;
+
+ // Color BLUE.
+ pcolor = vpColor::blue;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_blue] = xcolor.pixel;
+
+ // Color DARK BLUE.
+ pcolor = vpColor::darkBlue;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_darkBlue] = xcolor.pixel;
+
+ // Color YELLOW.
+ pcolor = vpColor::yellow;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_yellow] = xcolor.pixel;
+
+ // Color ORANGE.
+ pcolor = vpColor::orange;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_orange] = xcolor.pixel;
+
+ // Color CYAN.
+ pcolor = vpColor::cyan;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_cyan] = xcolor.pixel;
+
+ // Color PURPLE.
+ pcolor = vpColor::purple;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor ( display, lut, &xcolor );
+ x_color[vpColor::id_purple] = xcolor.pixel;
+ break;
+ }
+ }
+
+ XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
+ XMapWindow ( display, window ) ;
+ // Selection des evenements.
+ XSelectInput ( display, window,
+ ExposureMask |
+ ButtonPressMask | ButtonReleaseMask |
+ KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask |
+ PointerMotionMask);
+
+ /* Creation du contexte graphique */
+ values.plane_mask = AllPlanes;
+ values.fill_style = FillSolid;
+ values.foreground = WhitePixel ( display, screen );
+ values.background = BlackPixel ( display, screen );
+ context = XCreateGC ( display, window,
+ GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
+ &values );
+
+ if ( context == NULL )
+ {
+ vpERROR_TRACE ( "Can't create graphics context." );
+ throw ( vpDisplayException ( vpDisplayException::XWindowsError,
+ "Can't create graphics context" ) ) ;
+ }
+
+ // Pixmap creation.
+ pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
+
+ // Hangs when forward X11 is used to send the display to an other computer
+// do
+// XNextEvent ( display, &event );
+// while ( event.xany.type != Expose );
+
+ {
+ Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
+ screen_depth, ZPixmap, 0, NULL,
+ width, height, XBitmapPad ( display ), 0 );
+
+ Ximage->data = ( char * ) malloc ( height * (unsigned int)Ximage->bytes_per_line );
+ ximage_data_init = true;
+ }
+ displayHasBeenInitialized = true ;
+
+ XSync ( display, true );
+
+ XStoreName ( display, window, title_.c_str() );
+}
+
+/*!
+
+ Set the font used to display a text in overlay. The display is
+ performed using displayCharString().
+
+ \param font : The expected font name. The available fonts are given by
+ the "xlsfonts" binary. To choose a font you can also use the
+ "xfontsel" binary.
+
+ \note Under UNIX, to know all the available fonts, use the
+ "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
+
+ \sa displayCharString()
+*/
+void vpDisplayX::setFont( const char* font )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if (font!=NULL)
+ {
+ try
+ {
+ Font stringfont;
+ stringfont = XLoadFont (display, font) ; //"-adobe-times-bold-r-normal--18*");
+ XSetFont (display, context, stringfont);
+ }
+ catch(...)
+ {
+ vpERROR_TRACE ( "Bad font " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,"Bad font" ) ) ;
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Set the window title.
+ \param title : Window title.
+*/
+void
+vpDisplayX::setTitle ( const char *title )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if(title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+ XStoreName ( display, window, title_.c_str() );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Set the window position in the screen.
+
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+ \exception vpDisplayException::notInitializedError : If the video
+ device is not initialized.
+*/
+void vpDisplayX::setWindowPosition(int winx, int winy)
+{
+ if ( displayHasBeenInitialized ) {
+ XMoveWindow(display, window, winx, winy);
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayX::displayImage ( const vpImage<unsigned char> &I )
+{
+
+ if ( displayHasBeenInitialized )
+ {
+ switch ( screen_depth )
+ {
+ case 8:
+ {
+ unsigned char *src_8 = NULL;
+ unsigned char *dst_8 = NULL;
+ src_8 = ( unsigned char * ) I.bitmap;
+ dst_8 = ( unsigned char * ) Ximage->data;
+ // Correction de l'image de facon a liberer les niveaux de gris
+ // ROUGE, VERT, BLEU, JAUNE
+ {
+ unsigned int i = 0;
+ unsigned int size_ = width * height;
+ unsigned char nivGris;
+ unsigned char nivGrisMax = 255 - vpColor::id_unknown;
+
+ while ( i < size_ )
+ {
+ nivGris = src_8[i] ;
+ if ( nivGris > nivGrisMax )
+ dst_8[i] = 255;
+ else
+ dst_8[i] = nivGris;
+ i++ ;
+ }
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+// XClearWindow ( display, window );
+// XSync ( display,1 );
+ break;
+ }
+ case 16:
+ {
+ unsigned short *dst_16 = ( unsigned short* ) Ximage->data;
+ unsigned char *dst_8 = NULL;
+ unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
+ for ( unsigned int i = 0; i < height ; i++ ) {
+ dst_8 = (unsigned char*) Ximage->data + i * bytes_per_line;
+ dst_16 = (unsigned short *) dst_8;
+ for ( unsigned int j=0 ; j < width; j++ )
+ {
+ * ( dst_16 + j ) = ( unsigned short ) colortable[I[i][j]] ;
+ }
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+// XClearWindow ( display, window );
+// XSync ( display,1 );
+ break;
+ }
+
+ case 24:
+ default:
+ {
+ unsigned char *dst_32 = NULL;
+ unsigned int size_ = width * height ;
+ dst_32 = ( unsigned char* ) Ximage->data;
+ unsigned char *bitmap = I.bitmap ;
+ unsigned char *n = I.bitmap + size_;
+ //for (unsigned int i = 0; i < size; i++) // suppression de l'iterateur i
+ while ( bitmap < n )
+ {
+ unsigned char val = * ( bitmap++ );
+ * ( dst_32 ++ ) = val; // Composante Rouge.
+ * ( dst_32 ++ ) = val; // Composante Verte.
+ * ( dst_32 ++ ) = val; // Composante Bleue.
+ * ( dst_32 ++ ) = val;
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+// XClearWindow ( display, window );
+// XSync ( display,1 );
+ break;
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+/*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
+{
+ if ( displayHasBeenInitialized )
+ {
+ switch ( screen_depth )
+ {
+ case 16: {
+ unsigned short *dst_16 = NULL;
+ unsigned char *dst_8 = NULL;
+ vpRGBa* bitmap = I.bitmap;
+ unsigned int r, g, b;
+ unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
+
+ for ( unsigned int i = 0; i < height ; i++ ) {
+ dst_8 = (unsigned char*) Ximage->data + i * bytes_per_line;
+ dst_16 = (unsigned short *) dst_8;
+ for ( unsigned int j=0 ; j < width; j++ )
+ {
+ r = bitmap->R;
+ g = bitmap->G;
+ b = bitmap->B;
+ * ( dst_16 + j ) = (((r << 8) >> RShift) & RMask) |
+ (((g << 8) >> GShift) & GMask) |
+ (((b << 8) >> BShift) & BMask);
+ bitmap++;
+ }
+ }
+
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+
+ break;
+ }
+ case 24:
+ case 32:
+ {
+ /*
+ * 32-bit source, 24/32-bit destination
+ */
+ unsigned char *dst_32 = NULL;
+ dst_32 = ( unsigned char* ) Ximage->data;
+ vpRGBa* bitmap = I.bitmap;
+ unsigned int sizeI = I.getWidth() * I.getHeight();
+ if (XImageByteOrder(display) == 1) {
+ // big endian
+ for ( unsigned int i = 0; i < sizeI ; i++ ) {
+ *(dst_32++) = bitmap->A;
+ *(dst_32++) = bitmap->R;
+ *(dst_32++) = bitmap->G;
+ *(dst_32++) = bitmap->B;
+ bitmap++;
+ }
+ }
+ else {
+ // little endian
+ for ( unsigned int i = 0; i < sizeI; i++ ) {
+ *(dst_32++) = bitmap->B;
+ *(dst_32++) = bitmap->G;
+ *(dst_32++) = bitmap->R;
+ *(dst_32++) = bitmap->A;
+ bitmap++;
+ }
+ }
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+ // XClearWindow ( display, window );
+ // XSync ( display,1 );
+ break;
+
+ }
+ default:
+ vpERROR_TRACE ( "Unsupported depth (%d bpp) for color display",
+ screen_depth ) ;
+ throw ( vpDisplayException ( vpDisplayException::depthNotSupportedError,
+ "Unsupported depth for color display" ) ) ;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display an image with a reference to the bitmap.
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing.
+
+ \param I : Pointer to the image bitmap.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayX::displayImage ( const unsigned char *I )
+{
+ unsigned char *dst_32 = NULL;
+
+ if ( displayHasBeenInitialized )
+ {
+
+ dst_32 = ( unsigned char* ) Ximage->data;
+
+ for ( unsigned int i = 0; i < width * height; i++ )
+ {
+ * ( dst_32 ++ ) = *I; // red component.
+ * ( dst_32 ++ ) = *I; // green component.
+ * ( dst_32 ++ ) = *I; // blue component.
+ * ( dst_32 ++ ) = *I; // luminance component.
+ I++;
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+// XClearWindow ( display, window );
+// XSync ( display,1 );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+
+/*!
+ Display a selection of the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w, h : Width and height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP,
+ const unsigned int w, const unsigned int h )
+{
+ if ( displayHasBeenInitialized )
+ {
+ switch ( screen_depth )
+ {
+ case 8:
+ {
+ unsigned char *src_8 = NULL;
+ unsigned char *dst_8 = NULL;
+ src_8 = ( unsigned char * ) I.bitmap;
+ dst_8 = ( unsigned char * ) Ximage->data;
+ // Correction de l'image de facon a liberer les niveaux de gris
+ // ROUGE, VERT, BLEU, JAUNE
+ {
+ //int size = width * height;
+ unsigned char nivGris;
+ unsigned char nivGrisMax = 255 - vpColor::id_unknown;
+
+ //unsigned int iwidth = I.getWidth();
+ unsigned int iwidth = I.getWidth();
+
+ src_8 = src_8 + (int)(iP.get_i()*iwidth+ iP.get_j());
+ dst_8 = dst_8 + (int)(iP.get_i()*this->width+ iP.get_j());
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ nivGris = *(src_8+j);
+ if ( nivGris > nivGrisMax )
+ *(dst_8+j) = 255;
+ else
+ *(dst_8+j) = nivGris;
+ j++;
+ }
+ src_8 = src_8 + iwidth;
+ dst_8 = dst_8 + this->width;
+ i++;
+ }
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+ // XClearWindow ( display, window );
+ // XSync ( display,1 );
+ break;
+ }
+ case 16:
+ {
+ unsigned short *dst_16 = NULL;
+ unsigned char *dst_8 = NULL;
+ unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
+ for ( unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i()+h) ; i++ ) {
+ dst_8 = (unsigned char *) Ximage->data + i * bytes_per_line;
+ dst_16 = (unsigned short *) dst_8;
+ for ( unsigned int j=(unsigned int)iP.get_j() ; j < (unsigned int)(iP.get_j()+w); j++ )
+ {
+ * ( dst_16 + j ) = ( unsigned short ) colortable[I[i][j]] ;
+ }
+ }
+
+// unsigned char *src_8 = (unsigned char *) I.bitmap;
+// unsigned char *dst_8 = (unsigned char *) Ximage->data;
+// unsigned short *dst_16 = NULL;
+
+// unsigned int iwidth = I.getWidth();
+
+// src_8 += (int)(iP.get_i()*iwidth + iP.get_j());
+// dst_8 += (int)(iP.get_i()*Ximage->bytes_per_line + iP.get_j()*Ximage->bits_per_pixel/8);
+// dst_16 = (unsigned short *) dst_8;
+
+// unsigned int i = 0;
+// while (i < h) {
+// unsigned int j = 0;
+
+// while (j < w) {
+// *(dst_16 + j) = ( unsigned short ) colortable[*(src_8+j)];
+// j++;
+// }
+// src_8 = src_8 + iwidth;
+// dst_16 = dst_16 + Ximage->bytes_per_line;
+// i++;
+// }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+ // XClearWindow ( display, window );
+ // XSync ( display,1 );
+ break;
+ }
+
+ case 24:
+ default:
+ {
+ unsigned char *dst_32 = NULL;
+ //unsigned int size = width * height ;
+ dst_32 = ( unsigned char* ) Ximage->data;
+ unsigned char *src_8 = I.bitmap ;
+ //unsigned char *n = I.bitmap + size;
+
+ unsigned int iwidth = I.getWidth();
+
+ src_8 = src_8 + (int)(iP.get_i()*iwidth+ iP.get_j());
+ dst_32 = dst_32 + (int)(iP.get_i()*4*this->width+ iP.get_j()*4);
+
+ unsigned int i = 0;
+ while (i < h)
+ {
+ unsigned int j = 0;
+ while (j < w)
+ {
+ unsigned char val = *(src_8+j);
+ *(dst_32+4*j) = val;
+ *(dst_32+4*j+1) = val;
+ *(dst_32+4*j+2) = val;
+ *(dst_32+4*j+3) = val;
+ j++;
+ }
+ src_8 = src_8 + iwidth;
+ dst_32 = dst_32 + 4*this->width;
+ i++;
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+ // XClearWindow ( display, window );
+ // XSync ( display,1 );
+ break;
+ }
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+
+/*!
+ Display a selection of the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param w, h : Width and height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
+ const unsigned int w, const unsigned int h )
+{
+ if ( displayHasBeenInitialized )
+ {
+ switch ( screen_depth )
+ {
+ case 16: {
+ unsigned short *dst_16 = NULL;
+ unsigned char *dst_8 = NULL;
+ unsigned int r, g, b;
+ unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
+ for ( unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i()+h) ; i++ ) {
+ dst_8 = (unsigned char *) Ximage->data + i * bytes_per_line;
+ dst_16 = (unsigned short *) dst_8;
+ for ( unsigned int j=(unsigned int)iP.get_j() ; j < (unsigned int)(iP.get_j()+w); j++ )
+ {
+ r = I[i][j].R;
+ g = I[i][j].G;
+ b = I[i][j].B;
+ * ( dst_16 + j ) = (((r << 8) >> RShift) & RMask) |
+ (((g << 8) >> GShift) & GMask) |
+ (((b << 8) >> BShift) & BMask);
+ }
+ }
+
+ XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+
+ break;
+ }
+ case 24:
+ case 32:
+ {
+ /*
+ * 32-bit source, 24/32-bit destination
+ */
+
+ unsigned char *dst_32 = NULL;
+ dst_32 = ( unsigned char* ) Ximage->data;
+ vpRGBa* src_32 = I.bitmap;
+ //unsigned int sizeI = I.getWidth() * I.getHeight();
+
+ unsigned int iwidth = I.getWidth();
+
+ src_32 = src_32 + (int)(iP.get_i()*iwidth+ iP.get_j());
+ dst_32 = dst_32 + (int)(iP.get_i()*4*this->width+ iP.get_j()*4);
+
+ unsigned int i = 0;
+
+ if (XImageByteOrder(display) == 1) {
+ // big endian
+ while (i < h) {
+ unsigned int j = 0;
+ while (j < w) {
+ *(dst_32+4*j) = (src_32+j)->A;
+ *(dst_32+4*j+1) = (src_32+j)->R;
+ *(dst_32+4*j+2) = (src_32+j)->G;
+ *(dst_32+4*j+3) = (src_32+j)->B;
+
+ j++;
+ }
+ src_32 = src_32 + iwidth;
+ dst_32 = dst_32 + 4*this->width;
+ i++;
+ }
+
+ }
+ else {
+ // little endian
+ while (i < h) {
+ unsigned int j = 0;
+ while (j < w) {
+ *(dst_32+4*j) = (src_32+j)->B;
+ *(dst_32+4*j+1) = (src_32+j)->G;
+ *(dst_32+4*j+2) = (src_32+j)->R;
+ *(dst_32+4*j+3) = (src_32+j)->A;
+
+ j++;
+ }
+ src_32 = src_32 + iwidth;
+ dst_32 = dst_32 + 4*this->width;
+ i++;
+ }
+ }
+
+ // Affichage de l'image dans la Pixmap.
+ XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
+ XSetWindowBackgroundPixmap ( display, window, pixmap );
+ // XClearWindow ( display, window );
+ // XSync ( display,1 );
+ break;
+
+ }
+ default:
+ vpERROR_TRACE ( "Unsupported depth (%d bpp) for color display",
+ screen_depth ) ;
+ throw ( vpDisplayException ( vpDisplayException::depthNotSupportedError,
+ "Unsupported depth for color display" ) ) ;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+
+ Close the window.
+
+ \sa init()
+
+*/
+void vpDisplayX::closeDisplay()
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( ximage_data_init == true )
+ free ( Ximage->data );
+
+ Ximage->data = NULL;
+ XDestroyImage ( Ximage );
+
+ XFreePixmap ( display, pixmap );
+
+ XFreeGC ( display, context );
+ XDestroyWindow ( display, window );
+ XCloseDisplay ( display );
+
+ displayHasBeenInitialized = false;
+
+ if (x_color != NULL) {
+ delete [] x_color;
+ x_color = NULL;
+ }
+ }
+}
+
+
+/*!
+ Flushes the X buffer.
+ It's necessary to use this function to see the results of any drawing.
+
+*/
+void vpDisplayX::flushDisplay()
+{
+ if ( displayHasBeenInitialized )
+ {
+ XClearWindow ( display, window );
+ //XClearArea ( display, window,0,0,100,100,0 );
+ XFlush ( display );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Flushes a part of the X buffer.
+ It's necessary to use this function to see the results of any drawing.
+
+ \param iP : Top left corner of the region of interest
+ \param w,h : Width and height of the region of interest
+*/
+void vpDisplayX::flushDisplayROI(const vpImagePoint &iP, const unsigned int w, const unsigned int h)
+{
+ if ( displayHasBeenInitialized )
+ {
+ //XClearWindow ( display, window );
+ XClearArea ( display, window,(int)iP.get_u(),(int)iP.get_v(),w,h,0 );
+ XFlush ( display );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+
+/*!
+ Set the window backgroud to \e color.
+ \param color : Background color.
+*/
+void vpDisplayX::clearDisplay ( const vpColor &color )
+{
+ if ( displayHasBeenInitialized )
+ {
+
+ if (color.id < vpColor::id_unknown)
+ XSetWindowBackground ( display, window, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XClearWindow ( display, window );
+
+ XFreePixmap ( display, pixmap );
+ // Pixmap creation.
+ pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpDisplayX::displayArrow ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w, unsigned int h,
+ unsigned int thickness)
+{
+ if ( displayHasBeenInitialized )
+ {
+ try
+ {
+ double a = ip2.get_i() - ip1.get_i() ;
+ double b = ip2.get_j() - ip1.get_j() ;
+ double lg = sqrt ( vpMath::sqr ( a ) + vpMath::sqr ( b ) ) ;
+
+ //if ( ( a==0 ) && ( b==0 ) )
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
+ {
+ // DisplayCrossLarge(i1,j1,3,col) ;
+ }
+ else
+ {
+ a /= lg ;
+ b /= lg ;
+
+ vpImagePoint ip3;
+ ip3.set_i(ip2.get_i() - w*a);
+ ip3.set_j(ip2.get_j() - w*b);
+
+ vpImagePoint ip4;
+ ip4.set_i( ip3.get_i() - b*h );
+ ip4.set_j( ip3.get_j() + a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ ip4.set_i( ip3.get_i() + b*h );
+ ip4.set_j( ip3.get_j() - a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) )
+ displayLine ( ip2, ip4, color, thickness ) ;
+
+ displayLine ( ip1, ip2, color, thickness ) ;
+ }
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a string at the image point \e ip location.
+
+ To select the font used to display the string, use setFont().
+
+ \param ip : Upper left image point location of the string in the display.
+ \param text : String to display in overlay.
+ \param color : String color.
+
+ \sa setFont()
+*/
+void vpDisplayX::displayCharString ( const vpImagePoint &ip,
+ const char *text,
+ const vpColor &color )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+ XDrawString ( display, pixmap, context,
+ (int)ip.get_u(), (int)ip.get_v(),
+ text, (int)strlen ( text ) );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a circle.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the circle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void vpDisplayX::displayCircle ( const vpImagePoint ¢er,
+ unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XSetLineAttributes ( display, context, thickness,
+ LineSolid, CapButt, JoinBevel );
+
+ if ( fill == false )
+ {
+ XDrawArc ( display, pixmap, context,
+ vpMath::round( center.get_u()-radius ),
+ vpMath::round( center.get_v()-radius ),
+ radius*2, radius*2, 0, 23040 ); /* 23040 = 360*64 */
+ }
+ else
+ {
+ XFillArc ( display, pixmap, context,
+ vpMath::round( center.get_u()-radius ),
+ vpMath::round( center.get_v()-radius ),
+ radius*2, radius*2, 0, 23040 ); /* 23040 = 360*64 */
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a cross at the image point \e ip location.
+ \param ip : Cross location.
+ \param cross_size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplayX::displayCross ( const vpImagePoint &ip,
+ unsigned int cross_size,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ if ( displayHasBeenInitialized )
+ {
+ try
+ {
+ double i = ip.get_i();
+ double j = ip.get_j();
+ vpImagePoint ip1, ip2;
+
+ ip1.set_i( i-cross_size/2 );
+ ip1.set_j( j );
+ ip2.set_i( i+cross_size/2 );
+ ip2.set_j( j );
+ displayLine ( ip1, ip2, color, thickness ) ;
+
+ ip1.set_i( i );
+ ip1.set_j( j-cross_size/2 );
+ ip2.set_i( i );
+ ip2.set_j( j+cross_size/2 );
+
+ displayLine ( ip1, ip2, color, thickness ) ;
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+ }
+
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+
+}
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayX::displayDotLine ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XSetLineAttributes ( display, context, thickness,
+ LineOnOffDash, CapButt, JoinBevel );
+
+ XDrawLine ( display, pixmap, context,
+ vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ),
+ vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayX::displayLine ( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XSetLineAttributes ( display, context, thickness,
+ LineSolid, CapButt, JoinBevel );
+
+ XDrawLine ( display, pixmap, context,
+ vpMath::round( ip1.get_u() ),
+ vpMath::round( ip1.get_v() ),
+ vpMath::round( ip2.get_u() ),
+ vpMath::round( ip2.get_v() ) );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a point at the image point \e ip location.
+ \param ip : Point location.
+ \param color : Point color.
+*/
+void vpDisplayX::displayPoint ( const vpImagePoint &ip,
+ const vpColor &color )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XDrawPoint ( display, pixmap, context,
+ vpMath::round( ip.get_u() ),
+ vpMath::round( ip.get_v() ) );
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param w,h : Rectangle size in terms of width and height.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
+ unsigned int w, unsigned int h,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+ XSetLineAttributes ( display, context, thickness,
+ LineSolid, CapButt, JoinBevel );
+ if ( fill == false )
+ {
+ XDrawRectangle ( display, pixmap, context,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ w-1, h-1 );
+ }
+ else
+ {
+ XFillRectangle ( display, pixmap, context,
+ vpMath::round( topLeft.get_u() ),
+ vpMath::round( topLeft.get_v() ),
+ w, h );
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a rectangle.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+*/
+void
+vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XSetLineAttributes ( display, context, thickness,
+ LineSolid, CapButt, JoinBevel );
+
+ unsigned int w = (unsigned int)vpMath::round( std::fabs(bottomRight.get_u() - topLeft.get_u()) );
+ unsigned int h = (unsigned int)vpMath::round( std::fabs(bottomRight.get_v() - topLeft.get_v()) );
+ if ( fill == false )
+ {
+
+ XDrawRectangle ( display, pixmap, context,
+ vpMath::round( topLeft.get_u() < bottomRight.get_u() ? topLeft.get_u() : bottomRight.get_u() ),
+ vpMath::round( topLeft.get_v() < bottomRight.get_v() ? topLeft.get_v() : bottomRight.get_v() ),
+ w > 0 ? w-1 : 1, h > 0 ? h : 1 );
+ }
+ else
+ {
+ XFillRectangle ( display, pixmap, context,
+ vpMath::round( topLeft.get_u() < bottomRight.get_u() ? topLeft.get_u() : bottomRight.get_u() ),
+ vpMath::round( topLeft.get_v() < bottomRight.get_v() ? topLeft.get_v() : bottomRight.get_v() ),
+ w, h );
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Display a rectangle.
+
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+
+ \param thickness : Thickness of the four lines used to display the
+ rectangle. This parameter is only useful when \e fill is set to
+ false.
+
+*/
+void
+vpDisplayX::displayRectangle ( const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ if ( displayHasBeenInitialized )
+ {
+ if ( thickness == 1 ) thickness = 0;
+ if (color.id < vpColor::id_unknown)
+ XSetForeground ( display, context, x_color[color.id] );
+ else {
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
+ xcolor.green = 256 * color.G;
+ xcolor.blue = 256 * color.B;
+ XAllocColor ( display, lut, &xcolor );
+ XSetForeground ( display, context, xcolor.pixel );
+ }
+
+ XSetLineAttributes ( display, context, thickness,
+ LineSolid, CapButt, JoinBevel );
+
+ if ( fill == false )
+ {
+ XDrawRectangle ( display, pixmap, context,
+ vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getTop() ),
+ (unsigned int)vpMath::round( rectangle.getWidth()-1 ),
+ (unsigned int)vpMath::round( rectangle.getHeight()-1 ) );
+ }
+ else
+ {
+ XFillRectangle ( display, pixmap, context,
+ vpMath::round( rectangle.getLeft() ),
+ vpMath::round( rectangle.getTop() ),
+ (unsigned int)vpMath::round( rectangle.getWidth() ),
+ (unsigned int)vpMath::round( rectangle.getHeight() ) );
+ }
+
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+
+ Wait for a click from one of the mouse button.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayX::getClick(bool blocking)
+{
+
+ bool ret = false;
+
+ if ( displayHasBeenInitialized ) {
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+
+ // Event testing
+ if(blocking){
+ XCheckMaskEvent(display , ButtonPressMask, &event);
+ XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ XMaskEvent ( display, ButtonPressMask ,&event );
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ }
+
+ if(ret){
+ /* Recuperation de la coordonnee du pixel clique. */
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {}
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret;
+}
+
+/*!
+
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool
+vpDisplayX::getClick ( vpImagePoint &ip, bool blocking )
+{
+
+ bool ret = false;
+ if ( displayHasBeenInitialized ) {
+
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+ // Event testing
+ if(blocking){
+ XCheckMaskEvent(display , ButtonPressMask, &event);
+ XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ XMaskEvent ( display, ButtonPressMask ,&event );
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ }
+
+ if(ret){
+ // Get mouse position
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {
+ ip.set_u( (double)event.xbutton.x );
+ ip.set_v( (double)event.xbutton.y );
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret ;
+}
+
+/*!
+
+ Wait for a mouse button click and get the position of the clicked
+ pixel. The button used to click is also set.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+*/
+bool
+vpDisplayX::getClick ( vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking )
+{
+
+ bool ret = false;
+ if ( displayHasBeenInitialized ) {
+
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+
+ // Event testing
+ if(blocking){
+ XCheckMaskEvent(display , ButtonPressMask, &event);
+ XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ XMaskEvent ( display, ButtonPressMask ,&event );
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ }
+
+ if(ret){
+ // Get mouse position
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {
+ ip.set_u( (double)event.xbutton.x );
+ ip.set_v( (double)event.xbutton.y );
+ switch ( event.xbutton.button ) {
+ case Button1: button = vpMouseButton::button1; break;
+ case Button2: button = vpMouseButton::button2; break;
+ case Button3: button = vpMouseButton::button3; break;
+ }
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret ;
+}
+
+/*!
+
+ Wait for a mouse button click release and get the position of the
+ image point were the click release occurs. The button used to click is
+ also set. Same method as getClick(unsigned int&, unsigned int&,
+ vpMouseButton::vpMouseButtonType &, bool).
+
+ \param ip [out] : Position of the clicked image point.
+
+ \param button [in] : Button used to click.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+ \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
+
+*/
+bool
+vpDisplayX::getClickUp ( vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType &button,
+ bool blocking )
+{
+
+ bool ret = false;
+ if ( displayHasBeenInitialized ) {
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+
+ // Event testing
+ if(blocking){
+ XCheckMaskEvent(display , ButtonPressMask, &event);
+ XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ XMaskEvent ( display, ButtonReleaseMask ,&event );
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ }
+
+ if(ret){
+ /* Recuperation de la coordonnee du pixel clique. */
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {
+ ip.set_u( (double)event.xbutton.x );
+ ip.set_v( (double)event.xbutton.y );
+ switch ( event.xbutton.button ) {
+ case Button1: button = vpMouseButton::button1; break;
+ case Button2: button = vpMouseButton::button2; break;
+ case Button3: button = vpMouseButton::button3; break;
+ }
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret ;
+}
+
+/*
+ Gets the displayed image (including the overlay plane)
+ and returns an RGBa image.
+
+ \param I : Image to get.
+*/
+void vpDisplayX::getImage ( vpImage<vpRGBa> &I )
+{
+ if ( displayHasBeenInitialized )
+ {
+ XImage *xi ;
+
+ XCopyArea (display,window, pixmap, context,
+ 0,0, getWidth(), getHeight(), 0, 0);
+
+ xi= XGetImage ( display,pixmap, 0,0, getWidth(), getHeight(),
+ AllPlanes, ZPixmap ) ;
+
+ try
+ {
+ I.resize ( getHeight(), getWidth() ) ;
+ }
+ catch ( ... )
+ {
+ vpERROR_TRACE ( "Error caught" ) ;
+ throw ;
+ }
+
+ unsigned char *src_32 = NULL;
+ src_32 = ( unsigned char* ) xi->data;
+
+ if (screen_depth == 16) {
+ for ( unsigned int i = 0; i < I.getHeight() ; i++ ) {
+ size_t i_ = i*I.getWidth();
+ for ( unsigned int j = 0; j < I.getWidth() ; j++ ) {
+ size_t ij_ = i_+j;
+ unsigned long pixel = XGetPixel(xi, (int)j, (int)i);
+ I.bitmap[ij_].R = (((pixel & RMask) << RShift) >> 8);
+ I.bitmap[ij_].G = (((pixel & GMask) << GShift) >> 8);
+ I.bitmap[ij_].B = (((pixel & BMask) << BShift) >> 8);
+ I.bitmap[ij_].A = 0;
+ }
+ }
+
+ }
+ else {
+ if (XImageByteOrder(display) == 1) {
+ // big endian
+ for ( unsigned int i = 0; i < I.getWidth() * I.getHeight() ; i++ ) {
+ I.bitmap[i].A = src_32[i*4] ;
+ I.bitmap[i].R = src_32[i*4 + 1] ;
+ I.bitmap[i].G = src_32[i*4 + 2] ;
+ I.bitmap[i].B = src_32[i*4 + 3] ;
+ }
+ }
+ else {
+ // little endian
+ for ( unsigned int i = 0; i < I.getWidth() * I.getHeight() ; i++ ) {
+ I.bitmap[i].B = src_32[i*4] ;
+ I.bitmap[i].G = src_32[i*4 + 1] ;
+ I.bitmap[i].R = src_32[i*4 + 2] ;
+ I.bitmap[i].A = src_32[i*4 + 3] ;
+ }
+ }
+ }
+ XDestroyImage ( xi ) ;
+ }
+ else
+ {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+}
+
+/*!
+ Gets the window depth (8, 16, 24, 32).
+*/
+unsigned int vpDisplayX::getScreenDepth()
+{
+ Display *display_;
+ int screen_;
+ unsigned int depth;
+
+ if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
+ {
+ vpERROR_TRACE ( "Can't connect display on server %s.",
+ XDisplayName ( NULL ) );
+ throw ( vpDisplayException ( vpDisplayException::connexionError,
+ "Can't connect display on server." ) ) ;
+ }
+ screen_ = DefaultScreen ( display_ );
+ depth = (unsigned int)DefaultDepth ( display_, screen_ );
+
+ XCloseDisplay ( display_ );
+
+ return ( depth );
+}
+
+/*!
+ Gets the window size.
+ \param w, h : Size of the display in therms of width and height.
+ */
+void vpDisplayX::getScreenSize ( unsigned int &w, unsigned int &h )
+{
+ Display *display_;
+ int screen_;
+
+ if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
+ {
+ vpERROR_TRACE ( "Can't connect display on server %s.",
+ XDisplayName ( NULL ) );
+ throw ( vpDisplayException ( vpDisplayException::connexionError,
+ "Can't connect display on server." ) ) ;
+ }
+ screen_ = DefaultScreen ( display_ );
+ w = (unsigned int)DisplayWidth ( display_, screen_ );
+ h = (unsigned int)DisplayHeight ( display_, screen_ );
+
+ XCloseDisplay ( display_ );
+}
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+
+*/
+bool
+vpDisplayX::getKeyboardEvent(bool blocking)
+{
+
+ bool ret = false;
+
+ if ( displayHasBeenInitialized ) {
+ // Event testing
+ if(blocking){
+ XMaskEvent ( display, KeyPressMask ,&event );
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , KeyPressMask, &event);
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret;
+}
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+
+*/
+bool
+vpDisplayX::getKeyboardEvent(char *string, bool blocking)
+{
+
+ bool ret = false;
+ KeySym keysym;
+// int count;
+ XComposeStatus compose_status;
+ char buffer;
+
+ if ( displayHasBeenInitialized ) {
+ // Event testing
+ if(blocking){
+ XMaskEvent ( display, KeyPressMask ,&event );
+ /* count = */ XLookupString ((XKeyEvent *)&event, &buffer, 1,
+ &keysym, &compose_status);
+ //std::cout <<"count: " << count << " get \"" << buffer << "\"" << std::endl;
+ sprintf(string, "%c", buffer);
+ ret = true;
+ }
+ else{
+ ret = XCheckMaskEvent(display , KeyPressMask, &event);
+ if (ret) {
+ /* count = */ XLookupString ((XKeyEvent *)&event, &buffer, 1,
+ &keysym, &compose_status);
+ sprintf(string, "%c", buffer);
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret;
+}
+/*!
+
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+
+*/
+bool
+vpDisplayX::getPointerMotionEvent ( vpImagePoint &ip)
+{
+
+ bool ret = false;
+ if ( displayHasBeenInitialized ) {
+
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+ // Event testing
+ ret = XCheckMaskEvent(display , PointerMotionMask, &event);
+
+ if(ret){
+ // Get mouse position
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {
+ ip.set_u( (double)event.xbutton.x );
+ ip.set_v( (double)event.xbutton.y );
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret ;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayX::getPointerPosition ( vpImagePoint &ip)
+{
+
+ bool ret = false;
+ if ( displayHasBeenInitialized ) {
+
+ Window rootwin, childwin ;
+ int root_x, root_y, win_x, win_y ;
+ unsigned int modifier ;
+ // Event testing
+ ret = true;
+
+ if(ret){
+ // Get mouse position
+ if ( XQueryPointer ( display,
+ window,
+ &rootwin, &childwin,
+ &root_x, &root_y,
+ &win_x, &win_y,
+ &modifier ) ) {
+ ip.set_u( (double)win_x );
+ ip.set_v( (double)win_y );
+ }
+ }
+ }
+ else {
+ vpERROR_TRACE ( "X not initialized " ) ;
+ throw ( vpDisplayException ( vpDisplayException::notInitializedError,
+ "X not initialized" ) ) ;
+ }
+ return ret ;
+}
+
+/*!
+ Get the position of the most significant bit.
+*/
+int vpDisplayX::getMsb(unsigned int u32val)
+{
+ int i;
+
+ for (i = 31; i >= 0; --i) {
+ if (u32val & 0x80000000L)
+ break;
+ u32val <<= 1;
+ }
+ return i;
+}
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayX.cpp.o) has no symbols
+void dummy_vpDisplayX() {};
+#endif
+
diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp
new file mode 100644
index 0000000..43481ed
--- /dev/null
+++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp
@@ -0,0 +1,1216 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * D3D renderer for windows 32 display
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
+
+#include <visp3/gui/vpD3DRenderer.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpMath.h>
+
+/*
+ Be careful, when using :
+
+ pd3dText->LockRect(0, &d3dLRect, &r, 0)
+ ...
+ pd3dText->UnlockRect(0, &d3dLRect, &r, 0)
+
+ to write directly to the texture's surface,
+ the pointer returned in d3dLRect points to
+ the beginning of the locked suface and not
+ to the beginning of the texture's surface.
+ That's why setBufferPixel and other accesses
+ to this buffer are done in the locked surface
+ coordinates system.
+
+ Moreover, when directly writing to a texture's surface,
+ you musn't forget to take the pitch of this texture
+ into account (see Direct3D documentation).
+
+*/
+
+/*!
+ Constructor.
+ Initializes the color palettes and the font.
+*/
+vpD3DRenderer::vpD3DRenderer()
+{
+ pD3D=NULL;
+ pd3dDevice=NULL;
+ pd3dText=NULL;
+ pd3dVideoText=NULL;
+
+ //D3D palette
+ vpColor pcolor; // Predefined colors
+ pcolor = vpColor::black;
+ colors[vpColor::id_black] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightBlue;
+ colors[vpColor::id_lightBlue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::blue;
+ colors[vpColor::id_blue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkBlue;
+ colors[vpColor::id_darkBlue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::cyan;
+ colors[vpColor::id_cyan] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGreen;
+ colors[vpColor::id_lightGreen] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::green;
+ colors[vpColor::id_green] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGreen;
+ colors[vpColor::id_darkGreen] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightRed;
+ colors[vpColor::id_lightRed] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::red;
+ colors[vpColor::id_red] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkRed;
+ colors[vpColor::id_darkRed] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::white;
+ colors[vpColor::id_white] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGray;
+ colors[vpColor::id_lightGray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::gray;
+ colors[vpColor::id_gray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGray;
+ colors[vpColor::id_darkGray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::yellow;
+ colors[vpColor::id_yellow]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::orange;
+ colors[vpColor::id_orange]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::purple;
+ colors[vpColor::id_purple]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
+
+ //initialize the GDI palette
+ pcolor = vpColor::black;
+ colorsGDI[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightBlue;
+ colorsGDI[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::blue;
+ colorsGDI[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkBlue;
+ colorsGDI[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::cyan;
+ colorsGDI[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGreen;
+ colorsGDI[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::green;
+ colorsGDI[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGreen;
+ colorsGDI[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightRed;
+ colorsGDI[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::red;
+ colorsGDI[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkRed;
+ colorsGDI[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::white;
+ colorsGDI[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGray;
+ colorsGDI[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::gray;
+ colorsGDI[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGray;
+ colorsGDI[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::yellow;
+ colorsGDI[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::orange;
+ colorsGDI[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::purple;
+ colorsGDI[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
+
+ //Creates a logical font
+ hFont = CreateFont(18, 0, 0, 0, FW_NORMAL, false, false, false,
+ DEFAULT_CHARSET, OUT_DEFAULT_PRECIS,
+ CLIP_DEFAULT_PRECIS, DEFAULT_QUALITY,
+ DEFAULT_PITCH | FF_DONTCARE, NULL);
+}
+
+/*!
+ Destructor.
+ Releases all the remaining interfaces.
+*/
+vpD3DRenderer::~vpD3DRenderer()
+{
+ DeleteObject(hFont);
+
+ if(pd3dDevice != NULL)
+ pd3dDevice->Release();
+ if(pD3D != NULL)
+ pD3D->Release();
+ if(pd3dText != NULL)
+ pd3dText->Release();
+ if(pd3dVideoText != NULL)
+ pd3dVideoText->Release();
+}
+
+/*!
+ Computes the nearest power of 2 superior to n.
+ \param n Number whose nearest superior power of 2 we want.
+ \return Nearest power of 2 superior to n.
+*/
+unsigned int vpD3DRenderer::supPowerOf2(unsigned int n)
+{
+ unsigned int i=0;
+ while(n>1)
+ {
+ n>>=1;
+ i++;
+ }
+ return static_cast<unsigned int>(1<<(i+1));
+}
+
+/*!
+ Initialize the Direct3D renderer.
+ \param hwnd The window's handle.
+ \param width The window's width.
+ \param height The window's height.
+
+*/
+bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height)
+{
+ //simple stuff
+ nbCols = width;
+ nbRows = height;
+ hWnd = hwnd;
+
+ //D3D initialize
+ if(NULL == (pD3D = Direct3DCreate9(D3D_SDK_VERSION)))
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't initialize D3D!");
+
+ D3DDISPLAYMODE d3ddm;
+ if(FAILED(pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm)))
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't get the adapter's display mode!");
+
+
+ D3DPRESENT_PARAMETERS d3dpp;
+ ZeroMemory(&d3dpp, sizeof(d3dpp));
+ d3dpp.BackBufferCount=1;
+ d3dpp.Windowed = TRUE;
+ d3dpp.SwapEffect = D3DSWAPEFFECT_DISCARD;
+ d3dpp.BackBufferFormat = d3ddm.Format;
+
+ //creates a d3d device
+ if( FAILED(pD3D->CreateDevice(D3DADAPTER_DEFAULT , D3DDEVTYPE_HAL, hWnd,
+ D3DCREATE_SOFTWARE_VERTEXPROCESSING |
+ D3DCREATE_MULTITHREADED,
+ &d3dpp, &pd3dDevice )))
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't create the Direct3D device!");
+
+
+ //disables scene lightning
+ pd3dDevice->SetRenderState(D3DRS_LIGHTING, FALSE);
+
+
+ //inits the direct3D view (for 2D rendering)
+ initView((float)nbCols,(float)nbRows);
+
+
+ //computes texture size (needs to be a power-of-2 large square)
+ textWidth = supPowerOf2( (nbCols>nbRows) ? nbCols : nbRows );
+
+ //creates the system memory texture (the one we will directly modify)
+ //unfortunately, needs to be X8R8G8B8 in order to be able to use GDI drawing
+ //functions
+ if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, 0,
+ D3DFMT_X8R8G8B8, D3DPOOL_SYSTEMMEM , &pd3dText)
+ != D3D_OK)
+ {
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't create memory texture!");
+ }
+
+ //creates the video memory texture (the one we will display) -
+ if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT,
+ D3DUSAGE_DYNAMIC ,
+ D3DFMT_X8R8G8B8, D3DPOOL_DEFAULT, &pd3dVideoText)
+ != D3D_OK)
+ {
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't create video texture!");
+ }
+
+ //creates the sprite used to render the texture
+ if(D3DXCreateSprite(pd3dDevice, &pSprite) != S_OK)
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't create the texture's sprite!");
+
+
+
+ return true;
+}
+
+
+
+
+/*!
+ Initialize the view (orthogonal ...).
+ \param WindowWidth The Width of the window.
+ \param WindowHeight The height of the window.
+*/
+void vpD3DRenderer::initView(float WindowWidth, float WindowHeight)
+{
+ D3DXMATRIX Ortho2D;
+ D3DXMATRIX Identity;
+
+ D3DXMatrixOrthoLH(&Ortho2D, WindowWidth, WindowHeight, 0.0f, 1.0f);
+ D3DXMatrixIdentity(&Identity);
+
+ if( pd3dDevice->SetTransform(D3DTS_PROJECTION, &Ortho2D) != D3D_OK
+ || pd3dDevice->SetTransform(D3DTS_WORLD, &Identity) != D3D_OK
+ || pd3dDevice->SetTransform(D3DTS_VIEW, &Identity) != D3D_OK)
+ throw vpDisplayException(vpDisplayException::notInitializedError,
+ "Can't set the view!");
+}
+
+
+/*!
+ Converts a ViSP RGBA image to the Direct3D texture format (BGRA).
+ \param I Image to convert.
+ \param imBuffer Destination buffer.
+ \param pitch Pitch of the destination texture.
+*/
+void vpRGBaToTexture(const vpImage<vpRGBa>& I, unsigned char * imBuffer,
+ unsigned int pitch)
+{
+ unsigned int j = I.getWidth();
+
+ unsigned int k=0;
+ for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
+ {
+ if(j==0){
+ k += pitch - (I.getWidth() * 4);
+ j = I.getWidth();
+ }
+
+ imBuffer[k+0] = I.bitmap[i].B;
+ imBuffer[k+1] = I.bitmap[i].G;
+ imBuffer[k+2] = I.bitmap[i].R;
+ imBuffer[k+3] = I.bitmap[i].A;// pb in vpimconvert? , 0xFF?
+ k+=4;
+ j--;
+ }
+}
+
+/*!
+ Converts a ViSP gray image to the Direct3D texture format (BGRA).
+ \param I Image to convert.
+ \param imBuffer Destination buffer.
+ \param pitch Pitch of the destination texture.
+*/
+void vpGreyToTexture(const vpImage<unsigned char>& I,
+ unsigned char * imBuffer,
+ unsigned int pitch)
+{
+ unsigned int j = I.getWidth();
+
+ unsigned int k=0;
+ for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
+ {
+ if(j==0){
+ k += pitch - (I.getWidth() * 4);
+ j = I.getWidth();
+ }
+
+ imBuffer[k+0] = imBuffer[k+1] = imBuffer[k+2] = I.bitmap[i];
+ imBuffer[k+3] = 0xFF; //full opacity
+
+ k+=4;
+ j--;
+ }
+}
+
+/*!
+ Sets the image to display.
+ \param im The image to display.
+*/
+void vpD3DRenderer::setImg(const vpImage<vpRGBa>& im)
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top=0; r.left=0;
+ r.bottom=static_cast<signed long>(nbRows);
+ r.right=static_cast<signed long>(nbCols);
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //fills this texture with the image data (converted to bgra)
+ vpRGBaToTexture(im, buf, pitch);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+
+ }
+}
+
+/*!
+ Sets the image to display.
+ \param im The image to display.
+*/
+void vpD3DRenderer::setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top=(LONG)iP.get_v(); r.left=(LONG)iP.get_u();
+ r.bottom=(LONG)(iP.get_v()+height-1); r.right=(LONG)(iP.get_u()+width);
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //fills this texture with the image data (converted to bgra)
+ vpRGBaToTexture(im, buf, pitch);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+
+ }
+}
+
+
+/*!
+ Sets the image to display.
+ \param im The image to display.
+*/
+void vpD3DRenderer::setImg(const vpImage<unsigned char>& im)
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>(nbRows);
+ r.right = static_cast<LONG>(nbCols);
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //fills this texture with the image data (converted to bgra)
+ vpGreyToTexture(im, buf, pitch);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+
+}
+
+
+/*!
+ Sets the image to display.
+ \param im The image to display.
+*/
+void vpD3DRenderer::setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top=(LONG)iP.get_v(); r.left=(LONG)iP.get_u();
+ r.bottom=(LONG)(iP.get_v()+height-1); r.right=(LONG)(iP.get_u()+width);
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //fills this texture with the image data (converted to bgra)
+ vpGreyToTexture(im, buf, pitch);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+
+}
+
+/*!
+ Renders the memory texture to the screen.
+ \return True.
+
+*/
+bool vpD3DRenderer::render()
+{
+ // Clears the back buffer to a blue color
+ //pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET, D3DCOLOR_XRGB(0,0,255), 1.0f, 0 );
+
+ //Begins the scene.
+ pd3dDevice->BeginScene();
+
+ //Texture rectangle to display
+ RECT r;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>(nbRows);
+ r.right = static_cast<LONG>(nbCols);
+
+ //Updates the video memory texture with the content of the system
+ //memory texture
+ pd3dDevice->UpdateTexture(pd3dText,pd3dVideoText);
+
+ //Displays this texture as a sprite
+
+#if (D3DX_SDK_VERSION <= 9)
+ pSprite->Begin(); //
+ pSprite->Draw(pd3dVideoText, &r, NULL, NULL, NULL, NULL, 0xFFFFFFFF );
+#else
+ pSprite->Begin(0);
+ pSprite->Draw(pd3dVideoText, &r, NULL, NULL, 0xFFFFFFFF );
+#endif
+ pSprite->End();
+
+ //Ends the scene.
+ pd3dDevice->EndScene();
+ //Presents the backbuffer
+ pd3dDevice->Present( NULL, NULL, NULL, NULL );
+
+ return true;
+}
+
+
+/*!
+ Sets a pixel to color at position (j,i).
+
+ \param ip : The pixel coordinates.
+ \param color : the color of the point.
+*/
+void vpD3DRenderer::setPixel(const vpImagePoint &iP,
+ const vpColor &color)
+{
+ if(iP.get_i()<0 || iP.get_j()<0 || iP.get_i()>=(int)nbRows || iP.get_j()>=(int)nbCols)
+ {
+ return;
+ }
+
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+
+ r.top=(LONG)iP.get_i();
+ r.left=(LONG)iP.get_j();
+ r.bottom=(LONG)iP.get_i()+1;
+ r.right=(LONG)iP.get_j()+1;
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //the coordinates are in the locked area base
+ setBufferPixel(buf, pitch, 0, 0,color);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+
+}
+
+/*!
+ Draws a line.
+ \param ip1,ip2 : Initial and final image point.
+ \param color the line's color
+ \param thickness : Thickness of the line.
+ \param style style of the line
+*/
+void vpD3DRenderer::drawLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness, int style)
+{
+// if(i1<0 || j1<0 || i2<0 || j2<0 || e<0)
+// {
+// vpCERROR<<"Invalid parameters!"<<std::endl;
+// return;
+// }
+
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+
+ //Will contain the texture's surface drawing context
+ HDC hDCMem;
+
+ //The texture's surface
+ IDirect3DSurface9 * pd3dSurf;
+ pd3dText->GetSurfaceLevel(0, &pd3dSurf);
+
+ //We get its DC
+ pd3dSurf->GetDC(&hDCMem);
+
+ //create the pen
+ HPEN hPen;
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(style, static_cast<int>(thickness), colorsGDI[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
+ }
+
+ //we don't use the bkColor
+ SetBkMode(hDCMem, TRANSPARENT);
+
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+ //move to the starting point
+ MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
+ //Draw the line
+ LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
+
+
+ //Releases the DC
+ pd3dSurf->ReleaseDC(hDCMem);
+ //Releases the surface's interface
+ pd3dSurf->Release();
+ //Deletes additional objects
+ DeleteObject(hPen);
+ }
+}
+
+
+/*!
+ Draws a rectangle.
+ \param topLeft its top left point's coordinates
+ \param width width of the rectangle
+ \param height height of the rectangle
+ \param color The rectangle's color
+ \param fill When set to true fill the rectangle.
+ \param thickness : Line thickness
+*/
+void vpD3DRenderer::drawRect(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill ,
+ unsigned int /*thickness*/)
+{
+ if(topLeft.get_i()>(int)nbRows-1 || topLeft.get_j()>(int)nbCols-1|| topLeft.get_i()+height<0 ||topLeft.get_j()+width<0)
+ {
+ // vpCERROR<<"Invalid parameters!"<<std::endl;
+ return;
+ }
+
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top= (LONG)((topLeft.get_i()>0)? topLeft.get_i() : 0 );
+ r.left=(LONG)((topLeft.get_j()>0)? topLeft.get_j() : 0 );
+ r.bottom=(LONG)((topLeft.get_i()+height < (int)nbRows) ? topLeft.get_i()+height : nbRows-1);
+ r.right=(LONG)((topLeft.get_j()+width < (int)nbCols) ? topLeft.get_j()+width : nbCols-1);
+
+ /* unsigned */ int rectW = r.right - r.left;
+ /* unsigned */ int rectH = r.bottom - r.top;
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ /* unsigned */ int x= 0;
+ /* unsigned */ int y= 0;
+
+ if(fill == false)
+ {
+ //draws the top horizontal line
+ if(topLeft.get_i()>=0)
+ for(; x<rectW ; x++)
+ setBufferPixel(buf, pitch, x, y, color);
+
+ //draws the right vertical line
+ if(topLeft.get_j()+width < nbCols)
+ for(; y<rectH ; y++)
+ setBufferPixel(buf, pitch, x, y, color);
+
+ //draws the bottom horizontal line
+ if(topLeft.get_i()+height < nbRows)
+ for(; x>0 ; x--)
+ setBufferPixel(buf, pitch, x, y, color);
+
+ //draws the left vertical line
+ if(topLeft.get_j()>=0)
+ for(; y>0 ; y--)
+ setBufferPixel(buf, pitch, x, y, color);
+ }
+
+ else
+ {
+ if(topLeft.get_i()>=0 && topLeft.get_j()+width < nbCols && topLeft.get_i()+height < nbRows && topLeft.get_j()>=0)
+ {
+ for (x = 0; x<rectW; x++)
+ {
+ for (y = 0; y<rectH; y++)
+ setBufferPixel(buf, pitch, x, y, color);
+ }
+ }
+ }
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+}
+
+/*!
+ Clears the image to a specific color.
+ \param color The color used to fill the image.
+*/
+void vpD3DRenderer::clear(const vpColor &color)
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>( nbRows );
+ r.right = static_cast<LONG>( nbCols );
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ long * buf = (long *) ( d3dLRect.pBits );
+
+ unsigned long c;
+ if (color.id < vpColor::id_unknown)
+ c = colors[color.id];
+ else {
+ c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
+ }
+ long * end = (long*)((long)buf + (pitch * nbRows));
+
+ //fills the whole image
+ while (buf < end)
+ *buf++ = static_cast<long>( c );
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+}
+
+
+
+//writes current circle pixels using symetry to reduce the algorithm's complexity
+void vpD3DRenderer::subDrawCircle(int i, int j,
+ int x, int y,
+ vpColor col,
+ unsigned char* buf, unsigned int pitch,
+ unsigned int maxX, unsigned int maxY)
+{
+ if (x == 0) {
+ setBufferPixel(buf, pitch, i , j+y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i , j-y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i+y, j, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-y, j, col, maxX, maxY);
+ } else
+ if (x == y) {
+ setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
+ } else
+ if (x < y) {
+ setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i+y,j+x,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-y,j+x,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i+y,j-x,col, maxX, maxY);
+ setBufferPixel(buf, pitch, i-y,j-x,col, maxX, maxY);
+ }
+
+}
+
+/*!
+ Draws a circle.
+ \param center its center point's coordinates
+ \param radius The circle's radius
+ \param color The circle's color
+*/
+void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool /*fill*/, unsigned int /*thickness*/)
+{
+ if(radius<1 || vpMath::round(center.get_i()+radius)<0 || vpMath::round(center.get_i()-radius) > (int)nbRows || vpMath::round(center.get_j()+radius)<0 || vpMath::round(center.get_j()-radius) > (int)nbCols)
+ return;
+
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT rec;
+ int radius_ = static_cast<int>( radius );
+ int rleft = (vpMath::round(center.get_j()-radius_) > 0) ? vpMath::round(center.get_j())-radius_ : 0;
+ int rtop = (vpMath::round(center.get_i()-radius_) > 0) ? vpMath::round(center.get_i())-radius_ : 0;
+
+ rec.top= rtop;
+ rec.left= rleft;
+ rec.bottom=(LONG)((vpMath::round(center.get_i()+radius_) < (int)nbRows) ? center.get_i()+radius_ : nbRows-1);
+ rec.right=(LONG)((vpMath::round(center.get_j()+radius_) < (int)nbCols) ? center.get_j()+radius_ : nbCols-1);
+
+ //used as maxX and maxY for setBufferPixel
+ unsigned int rectW = static_cast<unsigned int> ( rec.right - rleft );
+ unsigned int rectH = static_cast<unsigned int> ( rec.bottom - rtop );
+
+ //locks the texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &rec, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ // Bresenham 's circle algorithm
+
+ int x = 0;
+ int y = static_cast<int>( radius );
+ int p = (3 - (y<<1));
+
+ vpImagePoint ip;
+ ip.set_i(center.get_i()-rtop);
+ ip.set_j(center.get_j()-rleft);
+
+ subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH);
+ while(x < y){
+ x++;
+ if (p < 0)
+ {
+ p += ((x<<1)+1)<<1;
+ }
+ else
+ {
+ y--;
+ p += (((x-y)<<1)+1)<<1;
+ }
+ subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH);
+ }
+
+
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+}
+
+
+
+/*!
+ Draws some text.
+ \param ip its top left point's coordinates
+ \param text The string to display
+ \param color The text's color
+*/
+void vpD3DRenderer::drawText(const vpImagePoint &ip, const char * text,
+ const vpColor &color)
+{
+ //Will contain the texture's surface drawing context
+ HDC hDCMem;
+
+ //The texture's surface
+ IDirect3DSurface9 * pd3dSurf;
+ pd3dText->GetSurfaceLevel(0, &pd3dSurf);
+
+ //We get its DC
+ pd3dSurf->GetDC(&hDCMem);
+
+ //Select the font
+ SelectObject(hDCMem, hFont);
+
+ //set the text color
+ if (color.id < vpColor::id_unknown)
+ SetTextColor(hDCMem, colorsGDI[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ SetTextColor(hDCMem, gdicolor);
+ }
+
+ //we don't use the bkColor
+ SetBkMode(hDCMem, TRANSPARENT);
+
+ SIZE size;
+ int length = (int) strlen(text);
+
+ //get the displayed string dimensions
+ GetTextExtentPoint32(hDCMem, text, length, &size);
+
+ //displays the string
+ TextOut(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v()), text, length);
+
+ //Releases the DC
+ pd3dSurf->ReleaseDC(hDCMem);
+ //Releases the surface's interface
+ pd3dSurf->Release();
+ //Deletes additional objects
+ DeleteObject(hFont);
+}
+
+
+/*!
+ Draws a cross.
+ \param ip its center point's coordinates
+ \param size Size of the cross
+ \param color The cross' color
+ \param thickness width of the cross
+*/
+void vpD3DRenderer::drawCross(const vpImagePoint &ip,
+ unsigned int size,
+ const vpColor &color, unsigned int thickness)
+{
+ if(ip.get_i()<0 || ip.get_j()<0 || ip.get_i()>(int)nbRows || ip.get_j()>(int)nbCols || thickness<=0)
+ return;
+
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT rec;
+ thickness = (thickness<size)? thickness : size;
+ int half_size_ = static_cast<int>( size/2 );
+ //if j-size/2 is inferior to 0, use 0
+ int rleft = ( (vpMath::round(ip.get_j()) - half_size_) < 0 ) ? 0 : vpMath::round(ip.get_j()) - half_size_;
+ //if j-size/2 is inferior to 0, use 0
+ int rtop = ( (vpMath::round(ip.get_i()) - half_size_) < 0 ) ? 0 : vpMath::round(ip.get_i()) - half_size_;
+
+ rec.top = rtop;
+ rec.left = rleft;
+ rec.bottom= (LONG)(ip.get_i() + (size/2));
+ rec.right = (LONG)(ip.get_j() + (size/2));
+
+ //locks the texture to directly access it
+ if( pd3dText->LockRect(0, &d3dLRect, &rec, 0) != D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ /* unsigned */ int x; //xpos
+
+ //y-coordinate of the line in the locked rectangle base
+
+ /* unsigned */ int y =( vpMath::round(ip.get_i()) < half_size_ ) ? vpMath::round(ip.get_i()) : half_size_;
+
+ /* unsigned */ int cpt = 0; //number of lines
+ unsigned int re = thickness; //remaining "width"
+
+ //horizontal lines
+ //stops when there is enough line for e
+ while(re!=0)
+ {
+ //draws a line
+ for(x=0; x<(rec.right - rec.left); x++)
+ setBufferPixel(buf, pitch, x, y, color);
+
+ re--;
+ cpt++;
+
+ //write alternatively a line at the top and a line at the bottom
+ //eg : y=4 -> y=5 -> y=3 -> y=6
+ y += ( (re&1) != 0u) ? cpt : -cpt;
+ }
+
+ cpt = 0;
+ re = thickness;
+
+ //x-coordinate of the line in the locked rectangle base
+ x = ( vpMath::round(ip.get_j()) < half_size_ ) ? vpMath::round(ip.get_j()) : half_size_;
+
+ //vertical lines
+ while(re!=0)
+ {
+ //draws a vertical line
+ for(y=0; y<rec.bottom - rec.top; y++)
+ setBufferPixel(buf, pitch, x, y, color);
+
+ re--;
+ cpt++;
+
+ //write alternatively a line on the left and a line on the right
+ x += ( (re&1) != 0) ? cpt : -cpt;
+ }
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+}
+
+
+/*!
+ Draws an arrow.
+ \param ip1,ip2 : Initial and final image point.
+ \param color The arrow's color
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpD3DRenderer::drawArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w,unsigned int h, unsigned int thickness)
+{
+ double a = ip2.get_i() - ip1.get_i();
+ double b = ip2.get_j() - ip1.get_j();
+ double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+ int _h = static_cast<int>( h );
+
+ //Will contain the texture's surface drawing context
+ HDC hDCMem;
+
+ //The texture's surface
+ IDirect3DSurface9 * pd3dSurf;
+ pd3dText->GetSurfaceLevel(0, &pd3dSurf);
+
+ //We get its DC
+ pd3dSurf->GetDC(&hDCMem);
+
+ //create the pen
+ HPEN hPen;
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colorsGDI[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
+ }
+
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+ //based on code from other displays
+ if ((a==0)&&(b==0))
+ {
+ // DisplayCrossLarge(i1,j1,3,col) ;
+ }
+ else
+ {
+ a /= lg ;
+ b /= lg ;
+
+ vpImagePoint ip3;
+ ip3.set_i( ip2.get_i() - w*a );
+ ip3.set_j( ip2.get_j() - w*b );
+
+
+ vpImagePoint ip4 ;
+
+ ip4.set_i( ip3.get_i() + b*_h );
+ ip4.set_j( ip3.get_j() - a*_h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
+ MoveToEx(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()), NULL);
+ LineTo(hDCMem, vpMath::round(ip4.get_j()), vpMath::round(ip4.get_i()));
+ }
+
+ ip4.set_i( ip3.get_i() - b*h );
+ ip4.set_j( ip3.get_j() + a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
+ MoveToEx(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()), NULL);
+ LineTo(hDCMem, vpMath::round(ip4.get_j()), vpMath::round(ip4.get_i()));
+ }
+
+ MoveToEx(hDCMem, vpMath::round(ip1.get_j()), vpMath::round(ip1.get_i()), NULL);
+ LineTo(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()));
+
+ }
+ //Deletes the pen
+ DeleteObject(hPen);
+ //Releases the DC
+ pd3dSurf->ReleaseDC(hDCMem);
+ //Releases the surface's interface
+ pd3dSurf->Release();
+
+
+}
+
+/*!
+ Converts the D3D textures to vpImage<vpRGBa>.
+ \param I The destination image.
+ \param imBuffer The texture's data.
+ \param pitch The texture's pitch.
+*/
+void TextureToRGBa(vpImage<vpRGBa>& I, unsigned char * imBuffer,
+ unsigned int pitch)
+{
+ unsigned int j = I.getWidth();
+
+ unsigned int k=0;
+ for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
+ {
+ //go to the next line
+ if(j==0){
+ k += pitch - (I.getWidth() * 4);
+ j = I.getWidth();
+ }
+
+ //simple conversion from bgra to rgba
+ I.bitmap[i].B = imBuffer[k+0];
+ I.bitmap[i].G = imBuffer[k+1];
+ I.bitmap[i].R = imBuffer[k+2];
+ I.bitmap[i].A = imBuffer[k+3];
+
+ k+=4;
+ j--;
+ }
+}
+
+/*!
+ Gets the currently displayed image with its overlay.
+ \param I The image to fill.
+*/
+void vpD3DRenderer::getImage(vpImage<vpRGBa> &I)
+{
+ //if the device has been initialized
+ if(pd3dDevice != NULL)
+ {
+
+ //resize the destination image as needed
+ I.resize(nbRows, nbCols);
+
+ D3DLOCKED_RECT d3dLRect;
+
+ RECT r;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>( nbRows );
+ r.right = static_cast<LONG>( nbCols );
+
+ //locks the whole texture to directly access it
+ if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
+ {
+ vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ return;
+ }
+
+ //gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+
+ //fills this image with the texture's data
+ TextureToRGBa(I,buf, pitch);
+
+ //unlocks the texture
+ if( pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpD3DRenderer.cpp.o) has no symbols
+void dummy_vpD3DRenderer() {};
+#endif
+#endif
diff --git a/modules/gui/src/display/windows/vpDisplayD3D.cpp b/modules/gui/src/display/windows/vpDisplayD3D.cpp
new file mode 100644
index 0000000..4f0f314
--- /dev/null
+++ b/modules/gui/src/display/windows/vpDisplayD3D.cpp
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * windows 32 display using D3D
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+/*!
+\file vpDisplayD3D.cpp
+\brief windows 32 display using D3D
+*/
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
+
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpD3DRenderer.h>
+
+/*!
+ \brief Basic constructor.
+*/
+vpDisplayD3D::vpDisplayD3D(): vpDisplayWin32(new vpD3DRenderer()){}
+
+/*!
+
+ \brief Constructor : Initialize a display.
+
+ \param winx, winy The window is set at position x,y (column index, row index).
+ \param title Window's title.
+
+*/
+vpDisplayD3D::vpDisplayD3D(int winx, int winy, const char *title)
+ : vpDisplayWin32(new vpD3DRenderer())
+{
+ windowXPosition = winx;
+ windowYPosition = winy;
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+}
+
+/*!
+
+\brief Constructor : initialize a display to visualize a RGBa image
+(32 bits).
+
+\param I : Image to be displayed (note that image has to be initialized).
+\param winx, winy : The window is set at position x,y (column index, row index).
+\param title : Window's title.
+
+*/
+vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I,
+ int winx, int winy,
+ const char *title)
+ : vpDisplayWin32(new vpD3DRenderer())
+{
+ init(I,winx,winy,title);
+}
+
+/*!
+
+\brief Constructor : initialize a display to visualize a grayscale image
+(8 bits).
+
+\param I Image to be displayed (note that image has to be initialized).
+\param winx, winy The window is set at position x,y (column index, row index).
+\param title Window's title.
+
+*/
+vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I,
+ int winx, int winy,
+ const char *title)
+ : vpDisplayWin32(new vpD3DRenderer())
+{
+ init(I,winx,winy,title);
+}
+
+/*!
+ \brief Basic destructor.
+*/
+vpDisplayD3D::~vpDisplayD3D(){}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayD3D.cpp.o) has no symbols
+void dummy_vpDisplayD3D() {};
+#endif
+
diff --git a/modules/gui/src/display/windows/vpDisplayGDI.cpp b/modules/gui/src/display/windows/vpDisplayGDI.cpp
new file mode 100644
index 0000000..af5b938
--- /dev/null
+++ b/modules/gui/src/display/windows/vpDisplayGDI.cpp
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * GDI based Display for windows 32.
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+/*!
+\file vpDisplayGDI.cpp
+\brief GDI based Display for windows 32.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_GDI) )
+
+#include <visp3/gui/vpDisplayGDI.h>
+
+//A vpDisplayGDI is just a vpDisplayWin32 which uses a vpGDIRenderer to do the drawing.
+
+/*!
+ \brief Basic constructor.
+*/
+vpDisplayGDI::vpDisplayGDI(): vpDisplayWin32(new vpGDIRenderer()){}
+
+/*!
+
+ \brief Constructor : Initialize a display.
+
+ \param winx, winy The window is set at position x,y (column index, row index).
+ \param title Window's title.
+
+*/
+vpDisplayGDI::vpDisplayGDI(int winx, int winy, const char *title)
+ : vpDisplayWin32(new vpGDIRenderer())
+{
+ windowXPosition = winx;
+ windowYPosition = winy;
+
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+}
+
+/*!
+
+ \brief Constructor : Initialize a display to visualize a RGBa image
+ (32 bits).
+
+ \param I : image to be displayed (note that image has to be initialized).
+ \param winx, winy The window is set at position x,y (column index, row index).
+ \param title Window's title.
+
+*/
+vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I,
+ int winx, int winy,
+ const char *title)
+ : vpDisplayWin32(new vpGDIRenderer())
+{
+ init(I,winx,winy,title);
+}
+
+/*!
+
+ \brief Constructor : Initialize a display to visualize a grayscale image
+ (8 bits).
+
+ \param I Image to be displayed (note that image has to be initialized).
+ \param winx, winy The window is set at position x,y (column index, row index).
+ \param title Window's title.
+
+*/
+vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I,
+ int winx, int winy,
+ const char *title)
+ : vpDisplayWin32(new vpGDIRenderer())
+{
+ init(I,winx,winy,title);
+}
+
+/*!
+ \brief Basic destructor.
+*/
+vpDisplayGDI::~vpDisplayGDI(){}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayGDI.cpp.o) has no symbols
+void dummy_vpDisplayGDI() {};
+#endif
diff --git a/modules/gui/src/display/windows/vpDisplayWin32.cpp b/modules/gui/src/display/windows/vpDisplayWin32.cpp
new file mode 100644
index 0000000..72106eb
--- /dev/null
+++ b/modules/gui/src/display/windows/vpDisplayWin32.cpp
@@ -0,0 +1,943 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display base class
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+
+#include <visp3/gui/vpDisplayWin32.h>
+#include <visp3/core/vpDisplayException.h>
+#include <string>
+
+const int vpDisplayWin32::MAX_INIT_DELAY = 5000;
+
+/*!
+ Thread entry point.
+ Used as a detour to initWindow.
+*/
+void vpCreateWindow(threadParam * param)
+{
+ //char* title = param->title;
+ (param->vpDisp)->window.initWindow(param->title.c_str(), param->x, param->y,
+ param->w, param->h);
+ delete param;
+}
+
+/*!
+ Constructor.
+*/
+vpDisplayWin32::vpDisplayWin32(vpWin32Renderer * rend) :
+ iStatus(false), window(rend)
+{
+}
+
+
+/*!
+ Destructor.
+*/
+vpDisplayWin32::~vpDisplayWin32()
+{
+ closeDisplay();
+}
+
+
+/*!
+
+ Constructor. Initialize a display to visualize a gray level image
+ (8 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void vpDisplayWin32::init(vpImage<unsigned char> &I,
+ int x,
+ int y,
+ const char *title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+
+ window.renderer->setImg(I);
+
+ init (I.getWidth(), I.getHeight(), x, y, title) ;
+ I.display = this ;
+}
+
+
+/*!
+ Constructor. Initialize a display to visualize a RGBa level image
+ (32 bits).
+
+ \param I : Image to be displayed (not that image has to be initialized)
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+void vpDisplayWin32::init(vpImage<vpRGBa> &I,
+ int x,
+ int y,
+ const char *title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth()==0))
+ {
+ vpERROR_TRACE("Image not initialized " ) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Image not initialized")) ;
+ }
+
+ window.renderer->setImg(I);
+
+ init (I.getWidth(), I.getHeight(), x, y, title) ;
+ I.display = this ;
+}
+
+
+/*!
+ Initialize the display size, position and title.
+
+ \param width, height : Width and height of the window.
+ \param x, y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+
+*/
+void vpDisplayWin32::init(unsigned int width, unsigned int height,
+ int x, int y,
+ const char *title)
+{
+ if (title != NULL)
+ title_ = std::string(title);
+ else
+ title_ = std::string(" ");
+
+ if (x != -1)
+ windowXPosition = x;
+ if (y != -1)
+ windowYPosition = y;
+
+ //we prepare the window's thread creation
+ threadParam * param = new threadParam;
+ param->x = windowXPosition;
+ param->y = windowYPosition;
+ param->w = width;
+ param->h = height;
+ param->vpDisp = this;
+ param->title = this->title_;
+
+ //creates the window in a separate thread
+ hThread = CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)vpCreateWindow,
+ param,0,&threadId);
+
+ //the initialization worked
+ iStatus = (hThread != (HANDLE)NULL);
+
+ displayHasBeenInitialized = true;
+}
+
+
+/*!
+ If the window is not initialized yet, wait a little (MAX_INIT_DELAY).
+ \exception notInitializedError : the window isn't initialized
+*/
+void vpDisplayWin32::waitForInit()
+{
+ //if the window is not initialized yet
+ if(!window.isInitialized())
+ {
+ //wait
+ if( WAIT_OBJECT_0 != WaitForSingleObject(window.semaInit,MAX_INIT_DELAY))
+ throw(vpDisplayException(vpDisplayException::notInitializedError,
+ "Window not initialized")) ;
+ //problem : the window is not initialized
+ }
+}
+
+
+/*!
+ Display the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning suppres the overlay drawing
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayWin32::displayImage(const vpImage<vpRGBa> &I)
+{
+ //waits if the window is not initialized
+ waitForInit();
+
+ //sets the image to render
+ window.renderer->setImg(I);
+ //sends a message to the window
+ //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+}
+
+
+/*!
+ Display a selection of the color image \e I in RGBa format (32bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param width : Width of the region of interest
+
+ \param height : Height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayWin32::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //waits if the window is not initialized
+ waitForInit();
+
+ //sets the image to render
+ window.renderer->setImgROI(I,iP,width,height);
+ //sends a message to the window
+ //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+}
+
+
+/*!
+ Display the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning suppres the overlay drawing
+
+ \param I : Image to display.
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayWin32::displayImage(const vpImage<unsigned char> &I)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ //sets the image to render
+ window.renderer->setImg(I);
+ //sends a message to the window
+ //PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
+}
+
+/*!
+ Display a selection of the gray level image \e I (8bits).
+
+ \warning Display has to be initialized.
+
+ \warning Suppress the overlay drawing in the region of interest.
+
+ \param I : Image to display.
+
+ \param iP : Top left corner of the region of interest
+
+ \param width : Width of the region of interest
+
+ \param height : Height of the region of interest
+
+ \sa init(), closeDisplay()
+*/
+void vpDisplayWin32::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //waits if the window is not initialized
+ waitForInit();
+
+ //sets the image to render
+ window.renderer->setImgROI(I,iP,width,height);
+ //sends a message to the window
+ //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+}
+
+
+/*!
+ Wait for a click from one of the mouse button.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+*/
+bool vpDisplayWin32::getClick( bool blocking)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ bool ret = false;
+ //sends a message to the window
+// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
+
+ //waits for a button to be pressed
+ if(blocking){
+ WaitForSingleObject(window.semaClick, 0);
+ WaitForSingleObject(window.semaClickUp, 0); //to erase previous events
+ WaitForSingleObject(window.semaClick, INFINITE);
+ ret = true;
+ }
+ else {
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
+ }
+
+ return ret;
+}
+
+
+/*!
+ Wait for a click from one of the mouse button and get the position
+ of the clicked image point.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+*/
+bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ bool ret = false ;
+ double u, v;
+ //tells the window there has been a getclick demand
+// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
+ //waits for a click
+ if(blocking){
+ WaitForSingleObject(window.semaClick, 0);
+ WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
+ WaitForSingleObject(window.semaClick, INFINITE);
+ ret = true;
+ }
+ else {
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
+ }
+
+ u = window.clickX;
+ v = window.clickY;
+ ip.set_u( u );
+ ip.set_v( v );
+
+ return ret;
+}
+
+
+/*!
+ Wait for a mouse button click and get the position of the clicked
+ pixel. The button used to click is also set.
+
+ \param ip [out] : The coordinates of the clicked image point.
+
+ \param button [out] : The button used to click.
+
+ \param blocking [in] :
+ - When set to true, this method waits until a mouse button is
+ pressed and then returns always true.
+ - When set to false, returns true only if a mouse button is
+ pressed, otherwise returns false.
+
+ \return true if a mouse button is pressed, false otherwise. If a
+ button is pressed, the location of the mouse pointer is updated in
+ \e ip.
+*/
+bool vpDisplayWin32::getClick(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ bool ret = false;
+ double u, v;
+ //tells the window there has been a getclickup demand
+// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
+ //waits for a click
+ if(blocking){
+ WaitForSingleObject(window.semaClick, 0);
+ WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
+ WaitForSingleObject(window.semaClick, INFINITE);
+ ret = true;
+ }
+ else
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
+
+ u = window.clickX;
+ v = window.clickY;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = window.clickButton;
+
+ return ret;
+}
+
+
+/*!
+ Wait for a mouse button click release and get the position of the
+ image point were the click release occurs. The button used to click is
+ also set. Same method as getClick(unsigned int&, unsigned int&,
+ vpMouseButton::vpMouseButtonType &, bool).
+
+ \param ip [out] : Position of the clicked image point.
+
+ \param button [in] : Button used to click.
+
+ \param blocking [in] : true for a blocking behaviour waiting a mouse
+ button click, false for a non blocking behaviour.
+
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
+ to \e true.
+ - false if no button was clicked. This can occur if blocking is set
+ to \e false.
+
+ \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
+
+*/
+bool vpDisplayWin32::getClickUp(vpImagePoint &ip,
+ vpMouseButton::vpMouseButtonType& button,
+ bool blocking)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ bool ret = false;
+ double u, v;
+ //tells the window there has been a getclickup demand
+// PostMessage(window.getHWnd(), vpWM_GETCLICKUP, 0,0);
+
+ //waits for a click release
+ if(blocking){
+ WaitForSingleObject(window.semaClickUp, 0);
+ WaitForSingleObject(window.semaClick, 0);//to erase previous events
+ WaitForSingleObject(window.semaClickUp, INFINITE);
+ ret = true;
+ }
+ else
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClickUp, 0));
+
+ u = window.clickXUp;
+ v = window.clickYUp;
+ ip.set_u( u );
+ ip.set_v( v );
+ button = window.clickButtonUp;
+
+ return ret;
+}
+
+/*!
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool vpDisplayWin32::getKeyboardEvent( bool blocking )
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ bool ret = false ;
+ //waits for a keyboard event
+ if(blocking){
+ WaitForSingleObject(window.semaKey, 0); // key down
+ WaitForSingleObject(window.semaKey, 0); // key up
+ WaitForSingleObject(window.semaKey, INFINITE);
+ ret = true;
+ }
+ else
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
+
+ return ret;
+}
+/*!
+
+ Get a keyboard event.
+
+ \param blocking [in] : Blocking behavior.
+ - When set to true, this method waits until a key is
+ pressed and then returns always true.
+ - When set to false, returns true only if a key is
+ pressed, otherwise returns false.
+
+ \param string [out]: If possible, an ISO Latin-1 character
+ corresponding to the keyboard key.
+
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
+ to \e true.
+ - false if no key was pressed. This can occur if blocking is set
+ to \e false.
+*/
+bool vpDisplayWin32::getKeyboardEvent(char *string, bool blocking)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ bool ret = false ;
+ //waits for a keyboard event
+ if(blocking){
+ WaitForSingleObject(window.semaKey, 0); // key down
+ WaitForSingleObject(window.semaKey, 0); // key up
+ WaitForSingleObject(window.semaKey, INFINITE);
+ ret = true;
+ }
+ else {
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
+ }
+ // printf("key: %ud\n", window.key);
+ sprintf(string, "%s", window.lpString);
+
+ return ret;
+}
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true if a pointer motion event was received, false otherwise.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayWin32::getPointerMotionEvent (vpImagePoint &ip)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ bool ret = false;
+
+ ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaMove, 0));
+ if (ret)
+ {
+ double u, v;
+ std::cout << "toto";
+ //tells the window there has been a getclick demand
+ //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
+
+ u = window.coordX;
+ v = window.coordY;
+ ip.set_u( u );
+ ip.set_v( v );
+ }
+
+ return ret;
+}
+
+/*!
+ Get the coordinates of the mouse pointer.
+
+ \param ip [out] : The coordinates of the mouse pointer.
+
+ \return true.
+
+ \exception vpDisplayException::notInitializedError : If the display
+ was not initialized.
+*/
+bool
+vpDisplayWin32::getPointerPosition (vpImagePoint &ip)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ bool ret = true ;
+ double u, v;
+ //tells the window there has been a getclick demand
+ //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
+
+ u = window.coordX;
+ v = window.coordY;
+ ip.set_u( u );
+ ip.set_v( v );
+
+ return ret;
+}
+
+/*!
+ Changes the window's position.
+
+ \param winx, winy : Position of the upper-left window's border in the screen.
+
+*/
+void vpDisplayWin32::setWindowPosition(int winx, int winy)
+{
+ //wait if the window is not initialized
+ waitForInit();
+
+ //cahange the window position only
+ SetWindowPos(window.hWnd,HWND_TOP, winx, winy, 0, 0,
+ SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER |SWP_NOSIZE);
+
+}
+
+
+/*!
+ Changes the window's titlebar text
+
+ \param windowtitle : Window title.
+*/
+void vpDisplayWin32::setTitle(const char *windowtitle)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ SetWindowText(window.hWnd, windowtitle);
+}
+
+
+/*!
+ \brief Set the font used to display text.
+ \param fontname : Name of the font.
+ */
+
+void vpDisplayWin32::setFont(const char * /* fontname */)
+{
+ vpERROR_TRACE("Not yet implemented" ) ;
+}
+
+
+/*!
+ \brief flush the Win32 buffer
+ It's necessary to use this function to see the results of any drawing
+
+*/
+void vpDisplayWin32::flushDisplay()
+{
+ //waits if the window is not initialized
+ waitForInit();
+
+ //sends a message to the window
+ PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
+}
+
+/*!
+ \brief flush the Win32 buffer
+ It's necessary to use this function to see the results of any drawing
+
+*/
+void vpDisplayWin32::flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height)
+{
+ //waits if the window is not initialized
+ waitForInit();
+ /*
+ Under windows, flushing an ROI takes more time than
+ flushing the whole image.
+ Therefore, we update the maximum area even when asked to update a region.
+ */
+ WORD left = (WORD)iP.get_u();
+ WORD right = (WORD)(iP.get_u()+width-1);
+
+ WORD top = (WORD)iP.get_v();
+ WORD bottom = (WORD)(iP.get_v()+height-1);
+
+ //sends a message to the window
+ WPARAM wp = MAKEWPARAM(left, right);
+ LPARAM lp = MAKELPARAM(top, bottom);
+
+ PostMessage(window.getHWnd(), vpWM_DISPLAY_ROI, wp,lp);
+}
+
+
+/*!
+ Display a point at the image point \e ip location.
+ \param ip : Point location.
+ \param color : Point color.
+*/
+void vpDisplayWin32::displayPoint(const vpImagePoint &ip,
+ const vpColor &color )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->setPixel(ip, color);
+}
+
+/*!
+ Display a line from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayWin32::displayLine( const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawLine(ip1, ip2, color, thickness);
+}
+
+
+/*!
+ Display a dashed line from image point \e ip1 to image point \e ip2.
+
+ \warning This line is a dashed line only if the thickness is equal to 1.
+
+ \param ip1,ip2 : Initial and final image points.
+ \param color : Line color.
+ \param thickness : Line thickness.
+*/
+void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawLine(ip1,ip2,color,thickness,PS_DASHDOT);
+}
+
+/*!
+ Display a rectangle with \e topLeft as the top-left corner and \e
+ width and \e height the rectangle size.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param width,height : Rectangle size.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+ \warning The thickness can not be set if the display uses the d3d library.
+*/
+void vpDisplayWin32::displayRectangle( const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
+}
+
+
+/*!
+ Display a rectangle.
+
+ \param topLeft : Top-left corner of the rectangle.
+ \param bottomRight : Bottom-right corner of the rectangle.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+ \warning The thickness can not be set if the display uses the d3d library.
+*/
+void vpDisplayWin32::displayRectangle( const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ unsigned int width = static_cast<unsigned int>( bottomRight.get_j() - topLeft.get_j() );
+ unsigned int height = static_cast<unsigned int>(bottomRight.get_i() - topLeft.get_i() );
+ window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
+}
+
+/*!
+ Display a rectangle.
+
+ \param rectangle : Rectangle characteristics.
+ \param color : Rectangle color.
+ \param fill : When set to true fill the rectangle.
+ \param thickness : Thickness of the four lines used to display the
+ rectangle.
+
+ \warning The thickness can not be set if the display uses the d3d library.
+*/
+void vpDisplayWin32::displayRectangle( const vpRect &rectangle,
+ const vpColor &color, bool fill,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ vpImagePoint topLeft;
+ topLeft.set_i(rectangle.getTop());
+ topLeft.set_j(rectangle.getLeft());
+ window.renderer->drawRect(topLeft,
+ static_cast<unsigned int>( rectangle.getWidth() ),
+ static_cast<unsigned int>( rectangle.getHeight() ),
+ color, fill, thickness);
+}
+
+
+/*!
+ Display a circle.
+ \param center : Circle center position.
+ \param radius : Circle radius.
+ \param color : Circle color.
+ \param fill : When set to true fill the circle.
+ \param thickness : Thickness of the circle. This parameter is only useful
+ when \e fill is set to false.
+*/
+void vpDisplayWin32::displayCircle(const vpImagePoint ¢er,
+ unsigned int radius,
+ const vpColor &color,
+ bool fill,
+ unsigned int thickness )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawCircle(center,radius,color,fill,thickness);
+}
+
+/*!
+ Displays a string.
+ \param ip : its top left point's coordinates
+ \param text : The string to display
+ \param color : The text's color
+*/
+void vpDisplayWin32::displayCharString(const vpImagePoint &ip,
+ const char *text,
+ const vpColor &color )
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawText(ip,text,color);
+}
+
+/*!
+ Display a cross at the image point \e ip location.
+ \param ip : Cross location.
+ \param size : Size (width and height) of the cross.
+ \param color : Cross color.
+ \param thickness : Thickness of the lines used to display the cross.
+*/
+void vpDisplayWin32::displayCross( const vpImagePoint &ip,
+ unsigned int size,
+ const vpColor &color,
+ unsigned int thickness)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawCross(ip, size, color, thickness);
+}
+
+
+/*!
+ Display an arrow from image point \e ip1 to image point \e ip2.
+ \param ip1,ip2 : Initial and final image point.
+ \param color : Arrow color.
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpDisplayWin32::displayArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w,unsigned int h,
+ unsigned int thickness)
+
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->drawArrow(ip1, ip2, color, w, h, thickness);
+}
+
+
+/*!
+ Clears the display.
+ \param color : the color to fill the display with
+*/
+void vpDisplayWin32::clearDisplay(const vpColor &color){
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->clear(color);
+}
+
+
+/*!
+ Closes the display.
+ Destroys the window.
+*/
+void vpDisplayWin32::closeDisplay()
+{
+ if (displayHasBeenInitialized) {
+ waitForInit();
+ PostMessage(window.getHWnd(), vpWM_CLOSEDISPLAY, 0,0);
+ //if the destructor is called for a reason different than a
+ //problem in the thread creation
+ if (iStatus) {
+ //waits for the thread to end
+ WaitForSingleObject(hThread, INFINITE);
+ CloseHandle(hThread);
+ }
+ displayHasBeenInitialized = false ;
+ window.initialized = false ;
+ }
+}
+
+/*!
+ Gets the displayed image (if overlay, if any).
+ \param I : Image to fill.
+*/
+void vpDisplayWin32::getImage(vpImage<vpRGBa> &I)
+{
+ //wait if the window is not initialized
+ waitForInit();
+ window.renderer->getImage(I);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpDisplayWin32.cpp.o) has no symbols
+void dummy_vpDisplayWin32() {};
+#endif
diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp
new file mode 100644
index 0000000..9a1ad12
--- /dev/null
+++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp
@@ -0,0 +1,1163 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * GDI renderer for windows 32 display
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#define GDI_ROBUST
+#if ( defined(VISP_HAVE_GDI) )
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpGDIRenderer.h>
+
+/*!
+ Constructor.
+*/
+vpGDIRenderer::vpGDIRenderer()
+{
+ //if the screen depth is not 32bpp, throw an exception
+ int bpp = GetDeviceCaps(GetDC(NULL), BITSPIXEL);
+ if( bpp != 32 )
+ throw vpDisplayException(vpDisplayException::depthNotSupportedError,
+ "vpGDIRenderer supports only 32bits depth: screen is %dbits depth!", bpp);
+
+ InitializeCriticalSection(&CriticalSection);
+
+ //initialize GDI the palette
+ vpColor pcolor; // Predefined colors
+
+ pcolor = vpColor::black;
+ colors[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightBlue;
+ colors[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::blue;
+ colors[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkBlue;
+ colors[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::cyan;
+ colors[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGreen;
+ colors[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::green;
+ colors[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGreen;
+ colors[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightRed;
+ colors[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::red;
+ colors[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkRed;
+ colors[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::white;
+ colors[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::lightGray;
+ colors[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::gray;
+ colors[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::darkGray;
+ colors[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::yellow;
+ colors[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::orange;
+ colors[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ pcolor = vpColor::purple;
+ colors[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
+
+ nbCols = 0;
+ nbRows = 0;
+ bmp = NULL;
+}
+
+/*!
+ Destructor.
+*/
+vpGDIRenderer::~vpGDIRenderer()
+{
+ //Deletes the critical section object
+ DeleteCriticalSection(&CriticalSection);
+ //Deletes the bitmap
+ DeleteObject(bmp);
+ //Deletes the font object
+ DeleteObject(hFont);
+}
+
+/*!
+ Initialiaze the renderer
+ \param hWindow Handle of the window we are working with
+ \param width The window's width.
+ \param height The window's height.
+*/
+bool vpGDIRenderer::init(HWND hWindow, unsigned int width, unsigned int height)
+{
+ timelost = 0.;
+ hWnd = hWindow;
+ nbCols = width;
+ nbRows = height;
+
+ //creates the font
+ hFont = CreateFont(18, 0, 0, 0, FW_NORMAL, false, false, false,
+ DEFAULT_CHARSET, OUT_DEFAULT_PRECIS,
+ CLIP_DEFAULT_PRECIS, DEFAULT_QUALITY,
+ DEFAULT_PITCH | FF_DONTCARE, NULL);
+ return true;
+}
+
+/*!
+ Sets the image to display.
+ \param I : The rgba image to display.
+*/
+void vpGDIRenderer::setImg(const vpImage<vpRGBa>& I)
+{
+ //converts the image into a HBITMAP
+ convert(I, bmp);
+ //updates the size of the image
+ nbCols=I.getWidth();
+ nbRows=I.getHeight();
+}
+
+/*!
+ Sets the image to display.
+ \param I : The rgba image to display.
+ \param iP : Top left coordinates of the ROI.
+ \param width, height : ROI width and height.
+*/
+void vpGDIRenderer::setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //converts the image into a HBITMAP
+ convertROI(I, iP, width, height);
+ //updates the size of the image
+ nbCols=I.getWidth();
+ nbRows=I.getHeight();
+}
+
+/*!
+ Sets the image to display.
+ \param I : The grayscale image to display.
+*/
+void vpGDIRenderer::setImg(const vpImage<unsigned char>& I)
+{
+ //converts the image into a HBITMAP
+ convert(I, bmp);
+ //updates the size of the image
+ nbCols=I.getWidth();
+ nbRows=I.getHeight();
+}
+
+/*!
+ Sets the image to display.
+ \param I : The rgba image to display.
+ \param iP : Top left coordinates of the ROI.
+ \param width, height : ROI width and height.
+*/
+void vpGDIRenderer::setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+{
+ //converts the image into a HBITMAP
+ convertROI(I, iP, width, height);
+ //updates the size of the image
+ nbCols=I.getWidth();
+ nbRows=I.getHeight();
+}
+
+/*!
+ Render the current image buffer.
+*/
+bool vpGDIRenderer::render()
+{
+ //gets the window's DC
+ PAINTSTRUCT ps;
+ HDC hDCScreen = BeginPaint(hWnd, &ps);
+
+ //create a memory DC
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //selects this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+
+ //blits it on the window's DC
+ BitBlt(hDCScreen, 0, 0,
+ static_cast<int>( nbCols ),
+ static_cast<int>( nbRows ),
+ hDCMem, 0, 0, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+ //DeleteDC(hDCMem);
+ DeleteObject(hDCMem);
+
+ EndPaint(hWnd, &ps);
+
+ return true;
+}
+
+/*!
+ Converts the image form ViSP in GDI's image format (bgra with padding).
+ \param I The image to convert.
+ \param hBmp The destination image.
+*/
+void vpGDIRenderer::convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp)
+{
+ //get the image's width and height
+ unsigned int w = I.getWidth();
+ unsigned int h = I.getHeight();
+
+ //each line of a HBITMAP needs to be word aligned
+ //we need padding if the width is an odd number
+ bool needPad = ((w%2) == 0) ? false : true;
+ unsigned int newW = w;
+
+ //in case of padding, the new width is width+1
+ newW = (needPad) ? (w+1) : w;
+
+ //allocate the buffer
+ unsigned char * imBuffer = new unsigned char[newW * h * 4];
+
+ //if we need padding (width needs to be a multiple of 2)
+ if(needPad)
+ {
+ unsigned int j = 0;
+ for(unsigned int i=0, k=0 ; i<newW * h * 4; i+=4, k++)
+ {
+ //end of a line = padding = inserts 0s
+ if(j==w && needPad)
+ {
+ imBuffer[i+0] = 0;
+ imBuffer[i+1] = 0;
+ imBuffer[i+2] = 0;
+ imBuffer[i+3] = 0;
+ j = 0;
+ k --;
+ }
+ else
+ {
+ //RGBA -> BGRA
+ imBuffer[i+0] = I.bitmap[k].B;
+ imBuffer[i+1] = I.bitmap[k].G;
+ imBuffer[i+2] = I.bitmap[k].R;
+ imBuffer[i+3] = I.bitmap[k].A;
+ j++;
+ }
+ }
+ }
+ else
+ //Simple conversion (no padding)
+ {
+ for(unsigned int i=0, k=0 ; i<w * h * 4 ; i+=4, k++)
+ {
+ imBuffer[i+0] = I.bitmap[k].B;
+ imBuffer[i+1] = I.bitmap[k].G;
+ imBuffer[i+2] = I.bitmap[k].R;
+ imBuffer[i+3] = I.bitmap[k].A;
+ }
+ }
+
+ //updates the bitmap's pixel data
+ updateBitmap(hBmp,imBuffer, newW, h);
+
+ //we don't need this buffer anymore
+ delete [] imBuffer;
+}
+
+/*!
+ Converts the image form ViSP in GDI's image format (bgra with padding).
+ \param I : The image to convert.
+ \param iP : Top left coordinates of the ROI.
+ \param width, height : ROI width and height.
+*/
+void vpGDIRenderer::convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
+{
+ //get the image's width and height
+ unsigned int w = width;
+ unsigned int h = height;
+
+ //each line of a HBITMAP needs to be word aligned
+ //we need padding if the width is an odd number
+ bool needPad = ((w%2) == 0) ? false : true;
+ unsigned int newW = w;
+
+ //in case of padding, the new width is width+1
+ newW = (needPad) ? (w+1) : w;
+
+ //allocate the buffer
+ unsigned char * imBuffer = new unsigned char[newW * h * 4];
+
+ vpRGBa* bitmap = I.bitmap;
+ unsigned int iwidth = I.getWidth();
+ bitmap = bitmap + (int)(iP.get_i()*iwidth+ iP.get_j());
+
+ //if we need padding (width needs to be a multiple of 2)
+ if(needPad)
+ {
+ unsigned int j = 0;
+ unsigned int k = 0;
+ for(unsigned int i=0 ; i<newW * h * 4; i+=4)
+ {
+ //end of a line = padding = inserts 0s
+ if(j==w && needPad)
+ {
+ imBuffer[i+0] = 0;
+ imBuffer[i+1] = 0;
+ imBuffer[i+2] = 0;
+ imBuffer[i+3] = 0;
+ j = 0;
+ }
+ else
+ {
+ //RGBA -> BGRA
+ imBuffer[i+0] = (bitmap+k)->B;
+ imBuffer[i+1] = (bitmap+k)->G;
+ imBuffer[i+2] = (bitmap+k)->R;
+ imBuffer[i+3] = (bitmap+k)->A;
+ //bitmap++;
+ j++;
+ k++;
+ }
+ if (k == newW)
+ {
+ bitmap = bitmap+iwidth;
+ k = 0;
+ }
+ }
+ }
+ else
+ //Simple conversion (no padding)
+ {
+ unsigned int k = 0;
+ for (unsigned int i=0 ; i < w * h * 4 ; i+=4)
+ {
+ imBuffer[i+0] = (bitmap+k)->B;
+ imBuffer[i+1] = (bitmap+k)->G;
+ imBuffer[i+2] = (bitmap+k)->R;
+ imBuffer[i+3] = (bitmap+k)->A;
+ //bitmap++;
+ k++;
+ if (k == newW)
+ {
+ bitmap = bitmap+iwidth;
+ k = 0;
+ }
+ }
+ }
+
+ //updates the bitmap's pixel data
+ updateBitmapROI(imBuffer,iP, newW, h);
+
+ //we don't need this buffer anymore
+ delete [] imBuffer;
+}
+
+
+/*!
+ Converts the image form ViSP in GDI's image format (bgra with padding).
+ \param I The image to convert.
+ \param hBmp The destination image.
+*/
+void vpGDIRenderer::convert(const vpImage<unsigned char> &I, HBITMAP& hBmp)
+{
+ //get the image's width and height
+ unsigned int w = I.getWidth();
+ unsigned int h = I.getHeight();
+
+ //each line of a HBITMAP needs to be word aligned
+ //we need padding if the width is an odd number
+ bool needPad = ((w%2) == 0) ? false : true;
+ unsigned int newW = w;
+
+ //in case of padding, the new width is width+1
+ newW = (needPad) ? (w+1) : w;
+
+ //allocate the buffer
+ unsigned char * imBuffer = new unsigned char[newW * h * 4];
+
+ //if we need padding
+ if(needPad)
+ {
+ unsigned int j = 0;
+ for(unsigned int i=0, k=0 ; i<newW * h * 4; i+=4, k++)
+ {
+ //end of a line = padding = inserts 0s
+ if(j==w && needPad)
+ {
+ imBuffer[i+0] = 0;
+ imBuffer[i+1] = 0;
+ imBuffer[i+2] = 0;
+ imBuffer[i+3] = 0;
+ j = 0;
+ k --;
+ }
+ else
+ {
+ imBuffer[i+0] = I.bitmap[k];
+ imBuffer[i+1] = I.bitmap[k];
+ imBuffer[i+2] = I.bitmap[k];
+ imBuffer[i+3] = I.bitmap[k];
+ j++;
+ }
+ }
+ }
+ else
+ //Simple conversion
+ {
+ for(unsigned int i=0, k=0 ; i<w * h * 4 ; i+=4, k++)
+ {
+ imBuffer[i+0] = I.bitmap[k];
+ imBuffer[i+1] = I.bitmap[k];
+ imBuffer[i+2] = I.bitmap[k];
+ imBuffer[i+3] = I.bitmap[k];
+ }
+ }
+
+ //updates the bitmap's pixel data
+ updateBitmap(hBmp,imBuffer, newW, h);
+
+ //we don't need this buffer anymore
+ delete [] imBuffer;
+}
+
+
+/*!
+ Converts the image form ViSP in GDI's image format (bgra with padding).
+ \param I The image to convert.
+ \param iP : Top left coordinates of the ROI.
+ \param width, height : ROI width and height.
+*/
+void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
+{
+ //get the image's width and height
+ unsigned int w = width;
+ unsigned int h = height;
+
+ //each line of a HBITMAP needs to be word aligned
+ //we need padding if the width is an odd number
+ bool needPad = ((w%2) == 0) ? false : true;
+ unsigned int newW = w;
+
+ //in case of padding, the new width is width+1
+ newW = (needPad) ? (w+1) : w;
+
+ //allocate the buffer
+ unsigned char * imBuffer = new unsigned char[newW * h * 4];
+
+ unsigned char* bitmap = I.bitmap;
+ unsigned int iwidth = I.getWidth();
+ bitmap = bitmap + (int)(iP.get_i()*iwidth+ iP.get_j());
+
+ //if we need padding (width needs to be a multiple of 2)
+ if(needPad)
+ {
+ unsigned int j = 0;
+ unsigned int k = 0;
+ for(unsigned int i=0 ; i<newW * h * 4; i+=4)
+ {
+ //end of a line = padding = inserts 0s
+ if(j==w && needPad)
+ {
+ imBuffer[i+0] = 0;
+ imBuffer[i+1] = 0;
+ imBuffer[i+2] = 0;
+ imBuffer[i+3] = 0;
+ j = 0;
+ }
+ else
+ {
+ //RGBA -> BGRA
+ imBuffer[i+0] = *(bitmap+k);
+ imBuffer[i+1] = *(bitmap+k);
+ imBuffer[i+2] = *(bitmap+k);
+ imBuffer[i+3] = *(bitmap+k);
+ //bitmap++;
+ j++;
+ k++;
+ }
+ if (k == newW)
+ {
+ bitmap = bitmap+iwidth;
+ k = 0;
+ }
+ }
+ }
+ else
+ //Simple conversion (no padding)
+ {
+ unsigned int k = 0;
+ for (unsigned int i=0 ; i < w * h * 4 ; i+=4)
+ {
+ imBuffer[i+0] = *(bitmap+k);
+ imBuffer[i+1] = *(bitmap+k);
+ imBuffer[i+2] = *(bitmap+k);
+ imBuffer[i+3] = *(bitmap+k);
+ //bitmap++;
+ k++;
+ if (k == newW)
+ {
+ bitmap = bitmap+iwidth;
+ k = 0;
+ }
+ }
+ }
+
+ //updates the bitmap's pixel data
+ updateBitmapROI(imBuffer,iP, newW, h);
+
+ //we don't need this buffer anymore
+ delete [] imBuffer;
+}
+
+/*!
+ Updates the bitmap to display.
+ Contains a critical section.
+ \param hBmp The bitmap to update
+ \param imBuffer The new pixel data
+ \param w The image's width
+ \param h The image's height
+
+ \return the operation succefulness
+*/
+bool vpGDIRenderer::updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
+ unsigned int w, unsigned int h)
+{
+ //the bitmap may only be accessed by one thread at the same time
+ //that's why we enter critical section
+ EnterCriticalSection(&CriticalSection);
+
+ //if the existing bitmap object is of the right size
+ if( (nbCols==w) && (nbRows==h) )
+ {
+ //just replace the content
+ SetBitmapBits(hBmp, w * h * 4, imBuffer);
+ }
+ else
+ {
+ if(hBmp != NULL)
+ {
+ //delete the old BITMAP
+ DeleteObject(hBmp);
+ }
+ //create a new BITMAP from this buffer
+ if( (hBmp = CreateBitmap(static_cast<int>(w),static_cast<int>(h),
+ 1,32,(void*)imBuffer)) == NULL)
+ return false;
+ }
+
+ LeaveCriticalSection(&CriticalSection);
+ return true;
+}
+
+
+/*!
+ Updates the bitmap to display.
+ Contains a critical section.
+ \param imBuffer The new pixel data
+ \param iP The topleft corner of the roi
+ \param w The roi's width
+ \param h The roi's height
+
+ \return the operation succefulness
+*/
+bool vpGDIRenderer::updateBitmapROI(unsigned char * imBuffer, const vpImagePoint &iP,
+ unsigned int w, unsigned int h)
+{
+ int w_ = static_cast<int>(w);
+ int h_ = static_cast<int>(h);
+ HBITMAP htmp = CreateBitmap(w_,h_,1,32,(void*)imBuffer);
+
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+ HDC hDCMem2 = CreateCompatibleDC(hDCScreen);
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+ SelectObject(hDCMem2, htmp);
+
+ BitBlt(hDCMem,(int)iP.get_u(),(int)iP.get_v(),w_,h_, hDCMem2, 0, 0,SRCCOPY);
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ DeleteObject(htmp);
+
+ return true;
+}
+
+/*!
+ Sets a pixel to color at position (j,i).
+
+ \param ip : The pixel coordinates.
+ \param color : the color of the point.
+*/
+void vpGDIRenderer::setPixel(const vpImagePoint &iP,
+ const vpColor &color)
+{
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ if (color.id < vpColor::id_unknown)
+ SetPixel(hDCMem, vpMath::round(iP.get_u()), vpMath::round(iP.get_v()),
+ colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ SetPixel(hDCMem, vpMath::round(iP.get_u()), vpMath::round(iP.get_v()),
+ gdicolor);
+ }
+ //display the result (flush)
+ // BitBlt(hDCScreen, x, y, 1, 1, hDCMem, x, y, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+}
+
+/*!
+ Draws a line.
+ \param ip1,ip2 : Initial and final image point.
+ \param color the line's color
+ \param thickness : Thickness of the line.
+ \param style style of the line
+*/
+void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int thickness, int style)
+{
+ HDC hDCScreen= NULL, hDCMem = NULL;
+ HPEN hPen = NULL;
+#ifdef GDI_ROBUST
+ double start = vpTime::measureTimeMs();
+ while(vpTime::measureTimeMs()-start<1000){
+ hDCScreen = GetDC(hWnd);
+ if(!hDCScreen) continue;
+ hDCMem = CreateCompatibleDC(hDCScreen);
+ if(!hDCMem){
+ ReleaseDC(hWnd, hDCScreen);
+ continue;
+ }
+
+ //create the pen
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(style, static_cast<int>(thickness), colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
+ }
+ if(!hPen){
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ continue;
+ }
+ if(!SetBkMode(hDCMem, TRANSPARENT)){
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ continue;
+ }
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+
+ if(!SelectObject(hDCMem, bmp)){
+ LeaveCriticalSection(&CriticalSection);
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ continue;
+ }
+
+ //select the pen
+ if(!SelectObject(hDCMem, hPen)){
+ LeaveCriticalSection(&CriticalSection);
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ continue;
+ }
+ break;
+ }
+ timelost+=(vpTime::measureTimeMs()-start);
+#else
+ //get the window's DC
+ hDCScreen = GetDC(hWnd);
+ hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //create the pen
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(style, static_cast<int>(thickness), colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
+ }
+ SetBkMode(hDCMem, TRANSPARENT);
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //select the pen
+ SelectObject(hDCMem, hPen);
+#endif
+ //move to the starting point
+ MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
+ //Draw the line
+ LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
+
+ //computes the coordinates of the rectangle to blit
+// int x = (j2 >= j1) ? j1 : j2;
+// int y = (i2 >= i1) ? i1 : i2;
+// int w = (j2 >= j1) ? j2-j1 : j1-j2;
+// int h = (i2 >= i1) ? i2-i1 : i1-i2;
+
+ //display the result (flush)
+ // BitBlt(hDCScreen, x, y, w, h, hDCMem, x, y, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+
+}
+
+/*!
+ Draws a rectangle.
+ \param topLeft its top left point's coordinates
+ \param width width of the rectangle
+ \param height height of the rectangle
+ \param color The rectangle's color
+ \param fill When set to true fill the rectangle.
+ \param thickness : Line thickness
+*/
+void vpGDIRenderer::drawRect(const vpImagePoint &topLeft,
+ unsigned int width, unsigned int height,
+ const vpColor &color, bool fill,
+ unsigned int thickness)
+{
+ if (thickness == 0) thickness = 1;
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //create the pen
+ HPEN hPen;
+ COLORREF gdicolor = RGB(0,0,0);
+
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
+ else {
+ gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
+ }
+
+ //create an hollow or solid brush (depends on boolean fill)
+ LOGBRUSH lBrush;
+ if(fill) {
+ lBrush.lbStyle = BS_SOLID;
+ if (color.id < vpColor::id_unknown)
+ lBrush.lbColor = colors[color.id];
+ else {
+ lBrush.lbColor = gdicolor;
+ }
+ }
+ else lBrush.lbStyle = BS_HOLLOW;
+ HBRUSH hbrush = CreateBrushIndirect(&lBrush);
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //select the brush
+ SelectObject(hDCMem, hbrush);
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+ //draw the rectangle
+ Rectangle(hDCMem, vpMath::round(topLeft.get_u()), vpMath::round(topLeft.get_v()),
+ vpMath::round(topLeft.get_u())+static_cast<int>(width),
+ vpMath::round(topLeft.get_v())+static_cast<int>(height));
+
+ //display the result (flush)
+// BitBlt(hDCScreen, j, i, width, height, hDCMem, j, i, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteObject(hbrush);
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+
+}
+
+/*!
+ Clears the image to a specific color.
+ \param color The color used to fill the image.
+*/
+void vpGDIRenderer::clear(const vpColor &color)
+{
+ vpImagePoint ip;
+ ip.set_i(0);
+ ip.set_j(0);
+ drawRect(ip, nbCols, nbRows, color, true, 0);
+}
+
+
+/*!
+ Draws a circle.
+ \param center its center point's coordinates
+ \param radius The circle's radius
+ \param color The circle's color
+ \param fill When set to true fill the circle.
+ \param thickness : Line thickness
+*/
+void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill, unsigned int thickness)
+{
+
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //create the pen
+ HPEN hPen;
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
+ }
+
+ //create an hollow brush
+ LOGBRUSH lBrush;
+ lBrush.lbStyle = BS_HOLLOW;
+ HBRUSH hbrush = CreateBrushIndirect(&lBrush);
+
+ //computes bounding rectangle
+ int radius_ = static_cast<int>(radius);
+ int x1 = vpMath::round(center.get_u())-radius_;
+ int y1 = vpMath::round(center.get_v())-radius_;
+ int x2 = vpMath::round(center.get_u())+radius_;
+ int y2 = vpMath::round(center.get_v())+radius_;
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //select the brush
+ SelectObject(hDCMem, hbrush);
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+ //draw the circle
+ if (fill==false)
+ Ellipse(hDCMem, x1, y1, x2, y2);
+
+ else
+ {
+ while (x2-x1 > 0)
+ {
+ x1++;
+ x2--;
+ y1++;
+ y2--;
+ Ellipse(hDCMem, x1, y1, x2, y2);
+ }
+ }
+
+ //display the result (flush)
+ // BitBlt(hDCScreen, x1, y1, x2-x1, y2-y1, hDCMem, x1, y1, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteObject(hbrush);
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+}
+
+
+/*!
+ Draws some text.
+ \param ip its top left point's coordinates
+ \param text The string to display
+ \param color The text's color
+*/
+void vpGDIRenderer::drawText(const vpImagePoint &ip, const char * text,
+ const vpColor &color)
+{
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //Select the font
+ SelectObject(hDCMem, hFont);
+
+ //set the text color
+ if (color.id < vpColor::id_unknown)
+ SetTextColor(hDCMem, colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ SetTextColor(hDCMem, gdicolor);
+ }
+
+ //we don't use the bkColor
+ SetBkMode(hDCMem, TRANSPARENT);
+
+ SIZE size;
+ int length = (int) strlen(text);
+
+ //get the displayed string dimensions
+ GetTextExtentPoint32(hDCMem, text, length, &size);
+
+ //displays the string
+ TextOut(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v()), text, length);
+
+ //display the result (flush)
+ // BitBlt(hDCScreen, j, i, size.cx, size.cy, hDCMem, j, i, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+}
+
+
+
+/*!
+ Draws a cross.
+ \param ip its center point's coordinates
+ \param size Size of the cross
+ \param color The cross' color
+ \param thickness width of the cross
+*/
+void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness)
+{
+ /* unsigned */ int half_size = static_cast<int>( size/2 );
+
+ // if half_size is equal to zero, nothing is displayed with the code
+ // just below. So, if half_size is equal to zero we just draw the
+ // pixel.
+ if (half_size) {
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //create the pen
+ HPEN hPen;
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
+ }
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+ //move to the starting point
+ MoveToEx(hDCMem, vpMath::round(ip.get_u())-half_size, vpMath::round(ip.get_v()), NULL);
+ //Draw the first line (horizontal)
+ LineTo(hDCMem, vpMath::round(ip.get_u())+half_size, vpMath::round(ip.get_v()));
+
+ //move to the starting point
+ MoveToEx(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v())-half_size, NULL);
+ //Draw the second line (vertical)
+ LineTo(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v())+half_size);
+
+ //display the result (flush)
+ // BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size,
+// hDCMem, j-(size/2), i-(size/2), SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+ }
+ else {
+ setPixel(ip, color);
+ }
+
+
+}
+
+/*!
+ Draws an arrow.
+ \param ip1,ip2 : Initial and final image point.
+ \param color The arrow's color
+ \param w,h : Width and height of the arrow.
+ \param thickness : Thickness of the lines used to display the arrow.
+*/
+void vpGDIRenderer::drawArrow(const vpImagePoint &ip1,
+ const vpImagePoint &ip2,
+ const vpColor &color,
+ unsigned int w,unsigned int h, unsigned int thickness)
+{
+ double a = ip2.get_i() - ip1.get_i() ;
+ double b = ip2.get_j() - ip1.get_j() ;
+ double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+
+ //computes the coordinates of the rectangle to blit later
+// unsigned int x = (j2 >= j1) ? j1 : j2;
+// unsigned int y = (i2 >= i1) ? i1 : i2;
+// unsigned int w = (j2 >= j1) ? j2-j1 : j1-j2;
+// unsigned int h = (i2 >= i1) ? i2-i1 : i1-i2;
+
+ //get the window's DC
+ HDC hDCScreen = GetDC(hWnd);
+ HDC hDCMem = CreateCompatibleDC(hDCScreen);
+
+ //create the pen
+ HPEN hPen;
+ if (color.id < vpColor::id_unknown)
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
+ else {
+ COLORREF gdicolor = RGB(color.R, color.G, color.B);
+ hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
+ }
+
+ //select this bmp in memory
+ EnterCriticalSection(&CriticalSection);
+ SelectObject(hDCMem, bmp);
+
+ //select the pen
+ SelectObject(hDCMem, hPen);
+
+
+ if ((a==0)&&(b==0))
+ {
+ // DisplayCrossLarge(i1,j1,3,col) ;
+ }
+ else
+ {
+ a /= lg ;
+ b /= lg ;
+
+ vpImagePoint ip3;
+ ip3.set_i( ip2.get_i() - w*a );
+ ip3.set_j( ip2.get_j() - w*b );
+
+
+ vpImagePoint ip4;
+
+ //double t = 0 ;
+ //while (t<=_l)
+ {
+ ip4.set_i( ip3.get_i() - b*h );
+ ip4.set_j( ip3.get_j() + a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
+ MoveToEx(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()), NULL);
+ LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v()));
+ }
+ // t+=0.1 ;
+ }
+
+ //t = 0 ;
+ //while (t>= -_l)
+ {
+ ip4.set_i( ip3.get_i() + b*h );
+ ip4.set_j( ip3.get_j() - a*h );
+
+ if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
+ MoveToEx(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()), NULL);
+ LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v()));
+ }
+
+ // t-=0.1 ;
+ }
+ MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
+ LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
+
+ }
+
+
+ //display the result (flush)
+// BitBlt(hDCScreen, x, y, w, h, hDCMem, x, y, SRCCOPY);
+
+ LeaveCriticalSection(&CriticalSection);
+
+ DeleteObject(hPen);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDCScreen);
+}
+
+/*!
+ Gets the currently displayed image.
+ \param I Image returned.
+*/
+void vpGDIRenderer::getImage(vpImage<vpRGBa> &I)
+{
+ //size of image buffer : nbCols*nbRows*4
+ unsigned int size = nbCols*nbRows*4;
+ unsigned char * imBuffer = new unsigned char[size];
+
+ //gets the hbitmap's bitmap
+ GetBitmapBits(bmp, static_cast<LONG>(size), (void *)imBuffer);
+
+ //resize the destination image as needed
+ I.resize(nbRows, nbCols);
+
+ //copy the content
+ for(unsigned int i=0 ; i<size ; i+=4)
+ {
+ I.bitmap[i>>2].R = imBuffer[i+2];
+ I.bitmap[i>>2].G = imBuffer[i+1];
+ I.bitmap[i>>2].B = imBuffer[i+0];
+ I.bitmap[i>>2].A = 0xFF; //255 = maximum opacity
+ }
+
+ delete [] imBuffer;
+}
+#endif
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpGDIRenderer.cpp.o) has no symbols
+void dummy_vpGDIRenderer() {};
+#endif
diff --git a/modules/gui/src/display/windows/vpWin32API.cpp b/modules/gui/src/display/windows/vpWin32API.cpp
new file mode 100644
index 0000000..517cb6c
--- /dev/null
+++ b/modules/gui/src/display/windows/vpWin32API.cpp
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * GDI renderer for windows 32 display
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <iostream>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#include <visp3/gui/vpWin32API.h>
+#include <visp3/core/vpTime.h>
+DWORD vpProcessErrors(const std::string &api_name){
+ LPVOID lpMsgBuf;
+ DWORD err = GetLastError();
+
+ FormatMessage(
+ FORMAT_MESSAGE_ALLOCATE_BUFFER |
+ FORMAT_MESSAGE_FROM_SYSTEM |
+ FORMAT_MESSAGE_IGNORE_INSERTS,
+ NULL,
+ err,
+ MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+ (LPTSTR) &lpMsgBuf,
+ 0, NULL );
+ std::cout << "call to " << api_name << " failed with the following error code: " << err << "(" << (LPTSTR)lpMsgBuf << ")" << std::endl;
+ return err;
+}
+
+BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd){
+ BOOL ret = LineTo(hdc, nXEnd, nYEnd);
+ if(ret == 0)
+ vpProcessErrors("LineTo");
+ return ret;
+}
+
+BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint){
+ BOOL ret = MoveToEx(hdc, X, Y, lpPoint);
+ if(ret == 0)
+ vpProcessErrors("MoveToEx");
+ return ret;
+}
+
+BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop){
+ BOOL ret = BitBlt(hdcDest,nXDest,nYDest, nWidth, nHeight, hdcSrc, nXSrc, nYSrc, dwRop);
+ if(ret == 0)
+ vpProcessErrors("BitBlt");
+ return ret;
+}
+
+BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase){
+ BOOL ret = InvalidateRect(hWnd,lpRect,bErase);
+ if(ret == 0)
+ vpProcessErrors("InvalidateRect");
+ return ret;
+}
+
+void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h){
+
+ HGDIOBJ ret = SelectObject(hDCMem,h);
+ if(ret==NULL){
+ vpProcessErrors("SelectObject");
+
+ double ms = vpTime::measureTimeMs();
+
+ while(ret==NULL && vpTime::measureTimeMs()-ms<5000){
+ DeleteObject(h);
+ DeleteDC(hDCMem);
+ ReleaseDC(hWnd, hDC);
+ }
+ }
+}
+
+BOOL vpReleaseSemaphore(HANDLE hSemaphore,LONG IReleaseCount,LPLONG lpPreviousCount){
+ BOOL ret = ReleaseSemaphore(hSemaphore,IReleaseCount,lpPreviousCount);
+#ifndef __MINGW32__
+ if(ret==0){
+ vpProcessErrors("ReleaseSemaphore");
+ }
+#endif
+ return ret;
+}
+
+void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection){
+ EnterCriticalSection(lpCriticalSection);
+}
+void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection){
+ LeaveCriticalSection(lpCriticalSection);
+}
+
+COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor){
+ COLORREF ret = SetPixel(hdc, X, Y, crColor);
+ if(ret == 0)
+ vpProcessErrors("SetPixel");
+ return ret;
+}
+
+HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, UINT cBitsPerPel, const VOID *lpvBits){
+ HBITMAP ret = CreateBitmap(nWidth, nHeight, cPlanes, cBitsPerPel, lpvBits);
+ if (ret==NULL)
+ vpProcessErrors("CreateBitmap");
+
+ return ret;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpWin32API.cpp.o) has no symbols
+void dummy_vpWin32API() {};
+#endif
diff --git a/modules/gui/src/display/windows/vpWin32Window.cpp b/modules/gui/src/display/windows/vpWin32Window.cpp
new file mode 100644
index 0000000..4533215
--- /dev/null
+++ b/modules/gui/src/display/windows/vpWin32Window.cpp
@@ -0,0 +1,336 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Windows 32 display's window class
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+#include <iostream>
+#include <visp3/gui/vpWin32API.h>
+
+#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+
+#define MAX_SEM_COUNT 2147483647
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/gui/vpWin32Window.h>
+
+//Should be already defined ...
+#ifndef GET_X_LPARAM
+# define GET_X_LPARAM(lp) ((int)(short)LOWORD(lp))
+#endif
+
+#ifndef GET_Y_LPARAM
+# define GET_Y_LPARAM(lp) ((int)(short)HIWORD(lp))
+#endif
+
+//declares the window as thread local
+//allows multiple displays
+#ifdef __MINGW32__
+vpWin32Window * window;
+#else
+_declspec(thread) vpWin32Window * window;
+#endif
+
+bool vpWin32Window::registered = false;
+/*!
+ The message callback
+*/
+LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
+{
+ //the first time this callback is executed, put the window in initialized state
+ if (window != NULL)
+ {
+ if (!window->isInitialized())
+ {
+ window->initialized = true;
+ vpReleaseSemaphore(window->semaInit,1,NULL);
+ }
+ }
+ switch (message)
+ {
+ case vpWM_DISPLAY:
+ //redraw the whole window
+ InvalidateRect(window->getHWnd(), NULL, TRUE);
+ UpdateWindow(window->getHWnd());
+ break;
+
+ case vpWM_DISPLAY_ROI:
+ {
+ RECT rect;
+
+ rect.left = LOWORD(wParam);
+ rect.right = HIWORD(wParam);
+
+ rect.top = LOWORD(lParam);
+ rect.bottom = HIWORD(lParam);
+
+ InvalidateRect(window->getHWnd(), &rect, TRUE);
+ UpdateWindow(window->getHWnd());
+ }
+ break;
+
+ case WM_LBUTTONDOWN:
+ {
+ window->clickX = GET_X_LPARAM(lParam);
+ window->clickY = GET_Y_LPARAM(lParam);
+
+ window->clickButton = vpMouseButton::button1;
+ vpReleaseSemaphore(window->semaClick,1,NULL);
+ }
+ break;
+
+ case WM_MBUTTONDOWN:
+ {
+ window->clickX = GET_X_LPARAM(lParam);
+ window->clickY = GET_Y_LPARAM(lParam);
+
+ window->clickButton = vpMouseButton::button2;
+ vpReleaseSemaphore(window->semaClick,1,NULL);
+ }
+ break;
+
+ case WM_RBUTTONDOWN:
+ {
+ window->clickX = GET_X_LPARAM(lParam);
+ window->clickY = GET_Y_LPARAM(lParam);
+
+ window->clickButton = vpMouseButton::button3;
+ vpReleaseSemaphore(window->semaClick,1,NULL);
+ }
+ break;
+
+ case WM_LBUTTONUP:
+ {
+ window->clickXUp = GET_X_LPARAM(lParam);
+ window->clickYUp = GET_Y_LPARAM(lParam);
+
+ window->clickButtonUp = vpMouseButton::button1;
+ vpReleaseSemaphore(window->semaClickUp,1,NULL);
+ }
+ break;
+
+ case WM_MBUTTONUP:
+ {
+ window->clickXUp = GET_X_LPARAM(lParam);
+ window->clickYUp = GET_Y_LPARAM(lParam);
+
+ window->clickButtonUp = vpMouseButton::button2;
+ vpReleaseSemaphore(window->semaClickUp,1,NULL);
+ }
+ break;
+
+ case WM_RBUTTONUP:
+ {
+ window->clickXUp = GET_X_LPARAM(lParam);
+ window->clickYUp = GET_Y_LPARAM(lParam);
+
+ window->clickButtonUp = vpMouseButton::button3;
+ vpReleaseSemaphore(window->semaClickUp,1,NULL);
+ }
+ break;
+ case WM_MOUSEMOVE:
+ {
+ window->coordX = GET_X_LPARAM(lParam);
+ window->coordY = GET_Y_LPARAM(lParam);
+ vpReleaseSemaphore(window->semaMove,1,NULL);
+ }
+ break;
+
+ case WM_SYSKEYDOWN:
+ // case WM_SYSKEYUP:
+ case WM_KEYDOWN:
+ //case WM_KEYUP:
+ {
+ GetKeyNameText((LONG)lParam, window->lpString, 10); // 10 is the size of lpString
+ //window->key = MapVirtualKey(wParam, MAPVK_VK_TO_CHAR);
+ vpReleaseSemaphore(window->semaKey,1,NULL);
+ break;
+ }
+
+ case WM_COMMAND:
+
+ break;
+
+ //we must prevent the window from erasing the background each time a
+ //repaint is needed
+ case WM_ERASEBKGND:
+ return (LRESULT)1;
+
+ case WM_PAINT:
+ //render the display
+ window->renderer->render();
+ break;
+
+ case vpWM_CLOSEDISPLAY:
+ //cleanup code here, if needed
+ //destroys the window
+ DestroyWindow(hWnd);
+ break;
+
+ case WM_DESTROY:
+ PostQuitMessage(0);
+ break;
+ default:
+ return DefWindowProc(hWnd, message, wParam, lParam);
+ }
+ return 0;
+}
+
+/*!
+ Constructor.
+*/
+vpWin32Window::vpWin32Window(vpWin32Renderer * rend): initialized(false)
+{
+ renderer = rend;
+
+ //registered is static member class and is initialized at the beginning of this file (registered = false)
+
+ //creates the semaphores
+ semaInit = CreateSemaphore(NULL,0,1,NULL);
+ semaClick = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
+ semaClickUp = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
+ semaKey = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
+ semaMove = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
+
+}
+
+/*!
+ Destructor.
+*/
+vpWin32Window::~vpWin32Window()
+{
+ delete renderer;
+ CloseHandle(semaInit);
+ CloseHandle(semaClick);
+ CloseHandle(semaClickUp);
+ CloseHandle(semaKey);
+ CloseHandle(semaMove);
+}
+
+
+/*!
+ A standard window creation procedure.
+ Initialize the renderer and associate it with this thread
+ \param title String to display in the window's titlebar
+ \param posx Initial window X position
+ \param posy Initial window Y position
+ \param w Initial window's width
+ \param h Initial window's height
+
+*/
+void vpWin32Window::initWindow(const char* title, int posx, int posy, unsigned int w, unsigned int h)
+{
+ //the window's style
+ DWORD style = WS_OVERLAPPEDWINDOW | WS_VISIBLE;
+ const char g_szClassName[] = "ViSPWindowClass";
+
+ RECT rect;
+ rect.left=0;
+ rect.right=static_cast<int>(w);
+ rect.top=0;
+ rect.bottom=static_cast<int>(h);
+
+ //now we register the window's class
+ WNDCLASSEX wcex;
+
+ wcex.cbSize = sizeof(WNDCLASSEX);
+
+ wcex.style = CS_HREDRAW | CS_VREDRAW | CS_NOCLOSE ;
+ wcex.lpfnWndProc = (WNDPROC)WndProc;
+ wcex.cbClsExtra = 0;
+ wcex.cbWndExtra = 0;
+ wcex.hInstance = hInst;
+ wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
+ wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
+ wcex.hbrBackground =(HBRUSH)(COLOR_WINDOW+1);
+ wcex.lpszMenuName = NULL;
+ wcex.lpszClassName = g_szClassName;
+ wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
+
+ RegisterClassEx(&wcex);
+
+ AdjustWindowRectEx(&rect, style, false, style);
+ // std::cout << "win client size (orig)(w,h): " << rect.left << " " << rect.top << " " << rect.right << " " << rect.bottom << std::endl;
+
+ //creates the window
+ hWnd = CreateWindowEx(WS_EX_APPWINDOW, g_szClassName, title, style,
+ posx, posy, rect.right-rect.left, rect.bottom-rect.top, NULL, NULL, hInst, NULL);
+ if (hWnd == NULL)
+ {
+ DWORD err= GetLastError();
+ std::cout << "err CreateWindowEx=" << err << std::endl;
+ throw vpDisplayException(vpDisplayException::cannotOpenWindowError,
+ "Can't create the window!");
+ }
+ SetWindowPos(hWnd, NULL, 0, 0, rect.right - rect.left, rect.bottom - rect.top, SWP_NOZORDER | SWP_NOMOVE);
+
+ //needed if we want to access it from the callback method (message handler)
+ window = this;
+
+ //initialize the renderer
+ renderer->init(hWnd, w, h);
+
+ //displays the window
+ ShowWindow(hWnd, SW_SHOWDEFAULT);
+ //ShowWindow(hWnd, SW_SHOW);
+ UpdateWindow(hWnd);
+
+ MSG msg;
+
+ //starts the message loop
+ while (true)
+ {
+ BOOL val = GetMessage(&msg, NULL, 0, 0);
+ if(val==-1){
+ std::cout << "GetMessage error:" << GetLastError() << std::endl;
+ break;
+ }else if(val==0){
+ break;
+ }else{
+ if (!TranslateAccelerator(msg.hwnd, NULL, &msg))
+ {
+ TranslateMessage(&msg);
+ DispatchMessage(&msg);
+ }
+ }
+ }
+}
+
+#endif
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpWin32Window.cpp.o) has no symbols
+void dummy_vpWin32Window() {};
+#endif
diff --git a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp
new file mode 100644
index 0000000..4153e12
--- /dev/null
+++ b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpProjectionDisplay.cpp
+ \brief interface with the image for feature display
+*/
+
+#include <visp3/core/vpConfig.h>
+#if defined (VISP_HAVE_DISPLAY)
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+
+//Color / image / display
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+#include <visp3/core/vpDisplay.h>
+
+#include <visp3/gui/vpProjectionDisplay.h>
+
+//#include <visp3/visual_features/vpBasicFeature.h>
+
+
+void
+vpProjectionDisplay::insert( vpForwardProjection &fp)
+{
+ // vpForwardProjection *f ;
+ // f = fp.duplicate() ;
+ // f->setDeallocate(vpForwardProjection::vpDisplayForwardProjection) ;
+
+ listFp.push_back(&fp);
+}
+
+void
+vpProjectionDisplay::init()
+{
+ o.setWorldCoordinates(0,0,0) ;
+ x.setWorldCoordinates(0.1,0,0) ;
+ y.setWorldCoordinates(0,0.1,0) ;
+ z.setWorldCoordinates(0,0,0.1) ;
+ traj.resize(0,2);
+}
+void
+vpProjectionDisplay::init(const int select)
+{
+ if (select & vpProjectionDisplay::internalView())
+ {
+ Icam.resize(256,256) ;
+ dIcam.init(Icam,100,100) ;
+ }
+ if (select & vpProjectionDisplay::externalView())
+ {
+ Iext.resize(256,256) ;
+ dIext.init(Iext,400,100) ;
+ }
+
+ init() ;
+}
+
+
+void
+vpProjectionDisplay::close()
+{
+
+}
+
+void
+vpProjectionDisplay::display(vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const vpColor &color,
+ const bool &displayTraj,
+ const unsigned int thickness)
+{
+
+ for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin() ; it != listFp.end(); ++it )
+ {
+ vpForwardProjection *fp = *it ;
+ fp->display(I,cextMo, cam, color, thickness) ;
+ }
+
+ if(displayTraj) // display past camera positions
+ for(unsigned int i=0;i<traj.getRows();++i)
+ vpDisplay::displayCircle(I,(int)traj[i][0],(int)traj[i][1],2,vpColor::green,true);
+
+ displayCamera(I, cextMo, cMo, cam, thickness);
+
+ if(displayTraj) // store current camera position
+ {
+ const unsigned int n = traj.getRows();
+ traj.resize(n+1, 2, false);
+ vpMeterPixelConversion::convertPoint(cam,o.p[0],o.p[1],traj[n][1],traj[n][0]);
+ }
+}
+
+
+void
+vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam,
+ const unsigned int thickness)
+{
+ vpHomogeneousMatrix c1Mc ;
+ c1Mc = cextMo*cMo.inverse() ;
+
+ o.track(c1Mc) ;
+
+ if(o.get_Z() < 0) // do not print camera if behind the external camera
+ return;
+
+ x.track(c1Mc) ;
+ y.track(c1Mc) ;
+ z.track(c1Mc) ;
+
+ vpImagePoint ipo;
+ vpImagePoint ipx;
+
+ vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
+
+ vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;
+
+ vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpProjectionDisplay.cpp.o) has no symbols
+void dummy_vpProjectionDisplay() {};
+#endif
diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp
new file mode 100644
index 0000000..592c9fe
--- /dev/null
+++ b/modules/gui/src/plot/vpPlot.cpp
@@ -0,0 +1,709 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Plot curves.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_DISPLAY)
+#include <visp3/gui/vpPlot.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <fstream>
+#include <list>
+#include <vector>
+
+/*!
+ Default constructor.
+
+ Needs then a call to init().
+
+*/
+vpPlot::vpPlot() : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40),
+ factori(1.f), factorj(1.)
+{
+}
+/*!
+ This constructor creates a new window where the curves
+ will be drawn. The number of graphics in the window must be set.
+
+ \warning You can modify the default window size, but this is not advised.
+
+ \warning Under Unix, when X11 is used to display the curves, the
+ default font is set to "-adobe-times-medium-i-normal--10-100-75-75-p-52-iso8859-*".
+ Note that you can chose an other one using "xfontsel".
+
+ \param graph_nbr : The number of graph in the window.
+ \param height : Height of the window.
+ \param width : Width of the window.
+ \param x,y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+vpPlot::vpPlot(const unsigned int graph_nbr,
+ const unsigned int height, const unsigned int width,
+ const int x, const int y, const char *title)
+ : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40),
+ factori(1.f), factorj(1.)
+{
+ init(graph_nbr, height, width, x, y, title);
+}
+
+/*!
+ Creates a new window where the curves
+ will be drawn. The number of graphics in the window must be set.
+
+ \warning You can modify the default window size, but this is not advised.
+
+ \param graph_nbr : The number of graph in the window.
+ \param height : Height of the window.
+ \param width : Width of the window.
+ \param x,y : The window is set at position x,y (column index, row index).
+ \param title : Window title.
+*/
+void vpPlot::init(const unsigned int graph_nbr,
+ const unsigned int height, const unsigned int width,
+ const int x, const int y, const char *title)
+{
+ I.init(height,width,255);
+
+#if defined VISP_HAVE_X11
+ display = new vpDisplayX;
+#elif defined VISP_HAVE_GDI
+ display = new vpDisplayGDI;
+#elif defined VISP_HAVE_OPENCV
+ display = new vpDisplayOpenCV;
+#elif defined VISP_HAVE_GTK
+ display = new vpDisplayGTK;
+#elif defined VISP_HAVE_D3D9
+ display = new vpDisplayD3D;
+#endif
+
+ display->init(I, x, y, title);
+
+ vpDisplay::display(I);
+
+ factori = height/700.0f;
+ factorj = width/700.0f;
+
+ initNbGraph(graph_nbr);
+}
+
+/*!
+ Basic destructor
+*/
+vpPlot::~vpPlot()
+{
+ if (graphList != NULL)
+ {
+ delete[] graphList;
+ graphList = NULL;
+ }
+ if (display != NULL)
+ {
+ delete display;
+ display = NULL;
+ }
+}
+
+/*!
+ Function called by the constructor to initialize the window and the basic parameters.
+
+ \exception vpException::dimensionError if the parameter exceed the possible
+ number of graph (4).
+
+ \param graphNbr : The number of graph in the window.
+*/
+void
+vpPlot::initNbGraph (unsigned int nbGraph)
+{
+ if(nbGraph > 4){
+ throw vpException(vpException::dimensionError, "Cannot create more than 4 graphs");
+ }
+ graphList = new vpPlotGraph[nbGraph];
+ graphNbr = nbGraph;
+
+ switch (nbGraph)
+ {
+ case 1 :
+ graphList[0].initSize(vpImagePoint(0,0), (unsigned int)(700*factorj),(unsigned int)(700*factori),margei,margej);
+ break;
+ case 2 :
+ graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[1].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
+ break;
+ case 3 :
+ graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
+ break;
+ case 4 :
+ graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ graphList[3].initSize(vpImagePoint((unsigned int)(350*factori),(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
+ break;
+ }
+
+ for (unsigned int i = 0; i < graphNbr; i++)
+ {
+ strcpy(graphList[i].title, "");
+ strcpy(graphList[i].unitx, "");
+ strcpy(graphList[i].unity, "");
+ strcpy(graphList[i].unitz, "");
+ }
+}
+
+/*!
+ Function which enables to initialize the number of curves which belongs to a 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 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*/,
+// const bool gx, const 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.
+*/
+void
+vpPlot::initRange (const unsigned int graphNum,
+ double xmin, double xmax,
+ double ymin, double ymax)
+{
+ (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,true,true);
+}
+
+/*!
+ 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.
+*/
+void
+vpPlot::initRange (const unsigned int graphNum,
+ double xmin, double xmax, double ymin,
+ double ymax, double zmin, double zmax)
+{
+ (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,zmin,zmax,10,true,true);
+}
+
+/*!
+ 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
+*/
+void
+vpPlot::setColor (const unsigned int graphNum, const unsigned int curveNum, vpColor color)
+{
+ (graphList+graphNum)->setCurveColor(curveNum, color);
+}
+
+/*!
+ display the grid for all graphics.
+*/
+void
+vpPlot::displayGrid()
+{
+ for (unsigned int i = 0; i < graphNbr; i++)
+ graphList[i].displayGrid(I);
+}
+
+/*!
+ This function enables you to add a new point in the curve. This point is 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.
+*/
+void
+vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
+{
+ (graphList+graphNum)->plot(I,curveNum,x,y);
+}
+
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+*/
+void vpPlot::plot(const unsigned int graphNum,
+ const double x, const vpColVector &v_y)
+{
+ if((graphList+graphNum)->curveNbr == v_y.getRows())
+ {
+ for(unsigned int i = 0;i < v_y.getRows();++i)
+ this->plot(graphNum, i, x, v_y[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+}
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+*/
+void vpPlot::plot(const unsigned int graphNum,
+ const double x, const vpRowVector &v_y)
+{
+ if((graphList+graphNum)->curveNbr == v_y.getRows())
+ {
+ for(unsigned int i = 0;i < v_y.getRows();++i)
+ this->plot(graphNum, i, x, v_y[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+}
+
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+*/
+void vpPlot::plot(const unsigned int graphNum,
+ const double x, const vpPoseVector &v_y)
+{
+ if((graphList+graphNum)->curveNbr == v_y.getRows())
+ {
+ for(unsigned int i = 0;i < v_y.getRows();++i)
+ this->plot(graphNum, i, x, v_y[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+}
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+*/
+void vpPlot::plot(const unsigned int graphNum,
+ const double x, const vpTranslationVector &v_y)
+{
+ if((graphList+graphNum)->curveNbr == v_y.getRows())
+ {
+ for(unsigned int i = 0;i < v_y.getRows();++i)
+ this->plot(graphNum, i, x, v_y[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+}
+
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+*/
+void vpPlot::plot(const unsigned int graphNum,
+ const double x, const vpRotationVector &v_y)
+{
+ if((graphList+graphNum)->curveNbr == v_y.size())
+ {
+ for(unsigned int i = 0;i < v_y.size();++i)
+ this->plot(graphNum, i, x, v_y[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+}
+
+
+/*!
+ This function enables you to add a new point in the curve. This point is 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.
+ \param z : The coordinate of the new point along the z axis and given in the user unit system.
+*/
+vpMouseButton::vpMouseButtonType
+vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z)
+{
+ return (graphList+graphNum)->plot(I,curveNum,x,y,z);
+}
+
+/*!
+ This function enables you to add new points in all curves of a plot. These 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.
+ \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(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z)
+{
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
+ if((graphList+graphNum)->curveNbr == v_y.getRows() && (graphList+graphNum)->curveNbr == v_z.getRows())
+ {
+ for(unsigned int i = 0;i < v_y.getRows();++i)
+ button = this->plot(graphNum, i, x, v_y[i], v_z[i]);
+ }
+ else
+ vpTRACE("error in plot vector : not the right dimension");
+ return button;
+}
+
+/*!
+ This method allows to change the point of view with the mouse if you have a 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.
+*/
+void
+vpPlot::navigate()
+{
+ vpMouseButton::vpMouseButtonType b = vpMouseButton::none;
+
+ bool blocked = false;
+ unsigned int iblocked = 0;
+ vpImagePoint iP;
+
+ while (b != vpMouseButton::button3)
+ {
+ if (!blocked)
+ {
+ vpDisplay::getPointerPosition(I,iP);
+ for (unsigned int i = 0; i < graphNbr ; i++)
+ {
+ if (iP.inRectangle((graphList+i)->graphZone))
+ {
+ iblocked = i;
+ break;
+ }
+ }
+ if ((graphList+iblocked)->move(I, b))
+ {
+ (graphList+iblocked)->replot3D(I);
+ }
+ blocked = (graphList+iblocked)->blocked;
+ }
+ else
+ {
+ if ((graphList+iblocked)->move(I, b))
+ {
+ (graphList+iblocked)->replot3D(I);
+ }
+ blocked = (graphList+iblocked)->blocked;
+ }
+ vpTime::sleepMs(20);
+ }
+}
+
+/*!
+ This method displays the coordinates of the point pointed by the mouse pointer.
+
+ \param block : If true, this method waits a click.
+*/
+void
+vpPlot::getPixelValue(const bool block)
+{
+ vpImagePoint iP;
+
+ if (block)
+ vpDisplay::getClick(I,iP);
+ else
+ vpDisplay::getPointerPosition(I,iP);
+
+ for (unsigned int i = 0; i < graphNbr; i++)
+ {
+ if ((graphList+i)->getPixelValue(I,iP)) break;
+ }
+}
+
+/*!
+ Sets the title of a 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 title : The graphic title.
+*/
+void
+vpPlot::setTitle (const unsigned int graphNum, const char *title)
+{
+ (graphList+graphNum)->setTitle(title);
+}
+
+/*!
+ Sets the unit system of the x axis.
+
+ \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.
+*/
+void
+vpPlot::setUnitX (const unsigned int graphNum, const char *unitx)
+{
+ (graphList+graphNum)->setUnitX(unitx);
+}
+
+/*!
+ Sets the unit system of the y axis.
+
+ \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.
+*/
+void
+vpPlot::setUnitY (const unsigned int graphNum, const char *unity)
+{
+ (graphList+graphNum)->setUnitY(unity);
+}
+
+/*!
+ Sets the unit system of the z axis.
+
+ \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.
+*/
+void
+vpPlot::setUnitZ (const unsigned int graphNum, const char *unitz)
+{
+ (graphList+graphNum)->setUnitZ(unitz);
+}
+
+/*!
+ Sets the legend of a 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 legend : The legend of the curve.
+*/
+void
+vpPlot::setLegend (const unsigned int graphNum, const unsigned int curveNum, const char *legend)
+{
+ (graphList+graphNum)->setLegend(curveNum, legend);
+}
+
+/*!
+ This method enables to erase the list of points stored for the graphic number \f$ graphNum \f$.
+
+ \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.
+*/
+void
+vpPlot::resetPointList (const unsigned int graphNum)
+{
+ for (unsigned int i = 0; i < (graphList+graphNum)->curveNbr; i++)
+ (graphList+graphNum)->resetPointList(i);
+}
+
+/*!
+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.
+ \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
+*/
+void
+vpPlot::setThickness (const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness)
+{
+ (graphList+graphNum)->setCurveThickness(curveNum, thickness);
+}
+
+/*!
+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.
+ \param thickness : The thickness you want to use
+*/
+void
+vpPlot::setGraphThickness (const unsigned int graphNum, const unsigned int thickness)
+{
+ for (unsigned int curveNum=0; curveNum < (graphList+graphNum)->curveNbr; curveNum++)
+ (graphList+graphNum)->setCurveThickness(curveNum, thickness);
+}
+
+/*!
+ This function enables you to choose the thickness used to draw the grid and the axis of 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.
+ \param thickness : The thickness you want to use
+*/
+void
+vpPlot::setGridThickness (const unsigned int graphNum, const unsigned int thickness)
+{
+ (graphList+graphNum)->setGridThickness(thickness);
+}
+
+/*!
+ This method enables to erase the list of points stored for the curve number \f$ curveNum \f$ contained in the graphic number \f$ graphNum \f$.
+
+ \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.
+*/
+void
+vpPlot::resetPointList (const unsigned int graphNum, const unsigned int curveNum)
+{
+ (graphList+graphNum)->resetPointList(curveNum);
+}
+
+/*!
+ This function enables to save in a text file all the plotted points of a graphic.
+
+ The first line of the text file is the graphic title. Then the points coordinates are given. If the graphic has to curves:
+ - the first column corresponds to the x axis of the first curve
+ - the second column corresponds to the y axis of the first curve
+ - the third column corresponds to the z axis of the first curve
+ - the fourth column corresponds to the x axis of the second curve
+ - the fifth column corresponds to the y axis of the second curve
+ - the sixth column corresponds to the z axis of the second curve
+
+ The column are delimited thanks to tabultaions.
+
+ \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.
+*/
+void vpPlot::saveData(const unsigned int graphNum, const char* dataFile)
+{
+ std::ofstream fichier;
+ fichier.open(dataFile);
+
+ unsigned int ind;
+ double *p = new double[3];
+ bool end=false;
+
+ std::vector< std::list<double>::const_iterator > vec_iter_pointListx((graphList+graphNum)->curveNbr);
+ std::vector< std::list<double>::const_iterator > vec_iter_pointListy((graphList+graphNum)->curveNbr);
+ std::vector< std::list<double>::const_iterator > vec_iter_pointListz((graphList+graphNum)->curveNbr);
+
+ fichier << (graphList+graphNum)->title << std::endl;
+
+ for(ind=0;ind<(graphList+graphNum)->curveNbr;ind++)
+ {
+ vec_iter_pointListx[ind] = (graphList+graphNum)->curveList[ind].pointListx.begin();
+ vec_iter_pointListy[ind] = (graphList+graphNum)->curveList[ind].pointListy.begin();
+ vec_iter_pointListz[ind] = (graphList+graphNum)->curveList[ind].pointListz.begin();
+// (graphList+graphNum)->curveList[ind].pointListx.front();
+// (graphList+graphNum)->curveList[ind].pointListy.front();
+// (graphList+graphNum)->curveList[ind].pointListz.front();
+ }
+
+ while (end == false)
+ {
+ end = true;
+ for(ind=0;ind<(graphList+graphNum)->curveNbr;ind++)
+ {
+// if (!(graphList+graphNum)->curveList[ind].pointListx.outside()
+// && !(graphList+graphNum)->curveList[ind].pointListy.outside()
+// && !(graphList+graphNum)->curveList[ind].pointListz.outside())
+ if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
+ && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
+ && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
+ {
+ p[0] = *vec_iter_pointListx[ind];
+ p[1] = *vec_iter_pointListy[ind];
+ p[2] = *vec_iter_pointListz[ind];
+// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
+// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
+// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
+
+ fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
+ ++vec_iter_pointListx[ind];
+ ++vec_iter_pointListy[ind];
+ ++vec_iter_pointListz[ind];
+// (graphList+graphNum)->curveList[ind].pointListx.next();
+// (graphList+graphNum)->curveList[ind].pointListy.next();
+// (graphList+graphNum)->curveList[ind].pointListz.next();
+// if(!(graphList+graphNum)->curveList[ind].pointListx.nextOutside()
+// && !(graphList+graphNum)->curveList[ind].pointListy.nextOutside()
+// && !(graphList+graphNum)->curveList[ind].pointListz.nextOutside())
+ if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
+ && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
+ && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
+ end = false;
+ }
+ else
+ {
+// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
+// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
+// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
+ p[0] = (graphList+graphNum)->curveList[ind].pointListx.back();
+ p[1] = (graphList+graphNum)->curveList[ind].pointListy.back();
+ p[2] = (graphList+graphNum)->curveList[ind].pointListz.back();
+ fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
+ }
+ }
+ fichier << std::endl;
+ }
+
+ delete[] p;
+ fichier.close();
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpPlot.cpp.o) has no symbols
+void dummy_vpPlot() {};
+#endif
diff --git a/modules/gui/src/plot/vpPlotCurve.cpp b/modules/gui/src/plot/vpPlotCurve.cpp
new file mode 100644
index 0000000..88a8419
--- /dev/null
+++ b/modules/gui/src/plot/vpPlotCurve.cpp
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Define a curve for the vpPlot class.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/gui/vpPlotCurve.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+#if defined(VISP_HAVE_DISPLAY)
+vpPlotCurve::vpPlotCurve() :
+ color(vpColor::red), curveStyle(point), thickness(1), nbPoint(0), lastPoint(),
+ pointListx(), pointListy(), pointListz(), xmin(0), xmax(0), ymin(0), ymax(0)
+{
+}
+
+vpPlotCurve::~vpPlotCurve()
+{
+ pointListx.clear();
+ pointListy.clear();
+ pointListz.clear();
+}
+
+void
+vpPlotCurve::plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y)
+{
+ nbPoint++;
+
+ if (nbPoint > 1)
+ {
+ vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
+ }
+#if defined (VISP_HAVE_DISPLAY)
+ double top;
+ double left;
+ double width;
+ double height;
+
+ if (iP.get_i() <= lastPoint.get_i()) {top = iP.get_i()-5; height = lastPoint.get_i() - top+10;}
+ else {top = lastPoint.get_i()-5; height = iP.get_i() - top+10;}
+ if (iP.get_j() <= lastPoint.get_j()) {left = iP.get_j()-5; width = lastPoint.get_j() - left+10;}
+ else {left = lastPoint.get_j()-5; width = iP.get_j() - left+10;}
+ vpDisplay::flushROI(I,vpRect(left,top,width,height));
+#endif
+ lastPoint = iP;
+ pointListx.push_back(x);
+ pointListy.push_back(y);
+ pointListz.push_back(0.0);
+}
+
+void
+vpPlotCurve::plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy)
+{
+ std::list<double>::const_iterator it_ptListx = pointListx.begin();
+ std::list<double>::const_iterator it_ptListy = pointListy.begin();
+
+ unsigned int k = 0;
+ vpImagePoint iP;
+ while (k < nbPoint)
+ {
+ iP.set_ij(yorg-(zoomy*(*it_ptListy)),xorg+(zoomx*(*it_ptListx)));
+
+ if (k > 0)
+ vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
+
+ lastPoint = iP;
+
+ ++it_ptListx;
+ ++it_ptListy;
+ k++;
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpPlotCurve.cpp.o) has no symbols
+void dummy_vpPlotCurve() {};
+#endif
+#endif
diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp
new file mode 100644
index 0000000..b5c9c6b
--- /dev/null
+++ b/modules/gui/src/plot/vpPlotGraph.cpp
@@ -0,0 +1,1424 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Define a graph for the vpPlot class.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+#define FLUSH_ON_PLOT
+
+#include <visp3/core/vpConfig.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/gui/vpPlotGraph.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+//#include <visp3/vision/vpPose.h>
+
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+#include <cmath> // std::fabs
+#include <visp3/core/vpMath.h>
+#include <limits> // numeric_limits
+
+#if defined(VISP_HAVE_DISPLAY)
+
+int laFonctionSansNom (const double delta);
+void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3);
+
+
+vpPlotGraph::vpPlotGraph()
+ : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10),
+ xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(), curveNbr(1), curveList(NULL),
+ scaleInitialized(false), firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
+ width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
+ dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
+ w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.),
+ nbDivisionz(10), zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(),
+ blockedr(false), blockedz(false), blocked(false), epsi(5), epsj(6),
+ dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
+{
+ gridColor.setColor(200,200,200);
+
+ old_iPr = vpImagePoint(-1,-1);
+ old_iPz = vpImagePoint(-1,-1);
+
+ gridThickness = 1;
+}
+
+vpPlotGraph::~vpPlotGraph()
+{
+ if (curveList != NULL)
+ {
+ delete[] curveList;
+ curveList = NULL;
+ }
+}
+
+void
+vpPlotGraph::initGraph (unsigned int nbCurve)
+{
+ curveList = new vpPlotCurve[nbCurve];
+ curveNbr = nbCurve;
+
+ vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpColor::orange,vpColor::purple};
+
+ for (unsigned int i = 0; i < curveNbr; i++)
+ {
+ (curveList+i)->color = colors[i%6];
+ (curveList+i)->curveStyle = line;
+ (curveList+i)->pointListx.clear();
+ (curveList+i)->pointListy.clear();
+ strcpy((curveList+i)->legend,"");
+ }
+}
+
+void
+vpPlotGraph::initSize (vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei, unsigned int margej)
+{
+ this->topLeft = top_left;
+ this->width = w;
+ this->height = h;
+ graphZone.setTopLeft(topLeft);
+ graphZone.setWidth(width);
+ graphZone.setHeight(height);
+
+ this->dTopLeft = vpImagePoint(topLeft.get_i()+margei,topLeft.get_j()+margej);
+ this->dWidth = width-margej-10;
+ this->dHeight = height-2*margei;
+ dGraphZone.setTopLeft(dTopLeft);
+ dGraphZone.setWidth(dWidth+1);
+ dGraphZone.setHeight(dHeight+1);
+
+ this->dTopLeft3D = vpImagePoint(topLeft.get_i()+margei,topLeft.get_j()+10);
+ dGraphZone3D.setTopLeft(dTopLeft3D);
+ dGraphZone3D.setWidth(dWidth+1);
+ dGraphZone3D.setHeight(dHeight+1);
+
+ if (this->dWidth > this->dHeight)
+ {
+ w_ysize = 1.0;
+ w_xsize = this->dWidth/this->dHeight;
+ w_zsize = w_xsize;
+
+ w_yval = w_ysize/2.0;
+ w_xval = w_xsize/2.0;
+ w_zval = w_zsize/2.0;
+ }
+ else if (this->dWidth == this->dHeight)
+ {
+ w_ysize = 1.0;
+ w_xsize = 1.0;
+ w_zsize = 1.0;
+
+ w_yval = 0.5;
+ w_xval = 0.5;
+ w_zval = 0.5;
+ }
+ else if (this->dWidth < this->dHeight)
+ {
+ w_xsize = 1.0;
+ w_ysize = this->dHeight/this->dWidth;
+ w_zsize = w_ysize;
+
+ w_yval = w_ysize/2.0;
+ w_xval = w_xsize/2.0;
+ w_zval = w_zsize/2.0;
+ }
+
+ cam.initPersProjWithoutDistortion(1000,1000,this->dWidth/2.0,this->dHeight/2.0);
+
+ findPose();
+
+ cMf.buildFrom(0,0,cMo[2][3],0,0,0);
+}
+
+
+void
+vpPlotGraph::findPose()
+{
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-w_xval,-w_yval,-w_zval);
+ point[1].setWorldCoordinates(w_xval,-w_yval,-w_zval);
+ point[2].setWorldCoordinates(w_xval,w_yval,-w_zval);
+ point[3].setWorldCoordinates(-w_xval,w_yval,-w_zval);
+
+ vpImagePoint iP[4];
+ iP[0].set_ij(0,0);
+ iP[1].set_ij(0,dWidth-1);
+ iP[2].set_ij(dHeight-1,dWidth-1);
+ iP[3].set_ij(dHeight-1,0);
+
+ double x=0, y=0;
+#if 0
+ // Modified by FS to remove dependency with visp_vision (pose) module
+ vpPose pose;
+ pose.clearPoint();
+
+ for (unsigned int i=0 ; i < 4 ; i++)
+ {
+ vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
+ point[i].set_x(x);
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
+ }
+
+ pose.computePose(vpPose::LAGRANGE, cMo) ;
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+#else
+ // Instead of pose computation we use an approximation
+ double Z = 0;
+ for(unsigned int i=0; i< 4; i++) {
+ vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
+ Z = vpMath::maximum(Z, point[i].get_oX() / x);
+ Z = vpMath::maximum(Z, point[i].get_oY() / y);
+ }
+ cMo[2][3] = Z;
+#endif
+}
+
+void
+vpPlotGraph::computeGraphParameters()
+{
+ zoomx = dWidth/(xmax-xmin);
+ zoomy = dHeight/(ymax-ymin);
+ xorg = dTopLeft.get_j() - (xmin*zoomx);
+ yorg = dTopLeft.get_i() + (ymax*zoomy);
+}
+
+void
+vpPlotGraph::setCurveColor(const unsigned int curveNum, const vpColor color)
+{
+ (curveList+curveNum)->color = color;
+}
+
+void
+vpPlotGraph::setTitle (const char *title_)
+{
+ if (strlen(title_) >= 256) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the title"));
+ }
+
+ strcpy(this->title, title_);
+ dispTitle = true;
+}
+
+void
+vpPlotGraph::setUnitX (const char *unit_x)
+{
+ if (strlen(unit_x) >= 256) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the unit along x axis"));
+ }
+
+ strcpy(this->unitx, unit_x);
+ dispUnit = true;
+}
+
+void
+vpPlotGraph::setUnitY (const char *unit_y)
+{
+ if (strlen(unit_y) >= 256) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the unit along y axis"));
+ }
+
+ strcpy(this->unity, unit_y);
+ dispUnit = true;
+}
+
+void
+vpPlotGraph::setUnitZ (const char *unit_z)
+{
+ if (strlen(unit_z) >= 256) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the unit along z axis"));
+ }
+
+ strcpy(this->unitz, unit_z);
+ dispUnit = true;
+}
+
+void
+vpPlotGraph::setLegend (const unsigned int curveNum, const char *legend)
+{
+ if (strlen(legend) >= 256) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the legend"));
+ }
+
+ strcpy((curveList+curveNum)->legend,legend);
+ dispLegend = true;
+}
+
+void
+vpPlotGraph::setCurveThickness(const unsigned int curveNum, const unsigned int thickness)
+{
+ (curveList+curveNum)->thickness = thickness;
+}
+
+int
+laFonctionSansNom (const double delta)
+{
+ double d = delta;
+ int power = 0;
+ if (d < 1)
+ {
+ while (d < 1)
+ {
+ d = d * 10;
+ power++;
+ }
+ power--;
+ return power;
+ }
+
+ if (d >= 10)
+ {
+ while (d > 10)
+ {
+ d = d / 10;
+ power--;
+ }
+ power--;
+ return power;
+ }
+
+ return 0;
+}
+
+void
+vpPlotGraph::displayGrid (vpImage<unsigned char> &I)
+{
+ computeGraphParameters();
+
+ xdelt = (xmax-xmin)/nbDivisionx;
+ ydelt = (ymax-ymin)/nbDivisiony;
+
+ double t;
+ char valeur[20];
+ int power;
+
+ power = laFonctionSansNom(xdelt);
+ for(t=xmin;t<=xmax;t=t+xdelt)
+ {
+ double x = xorg+(zoomx*t);
+ if(gridy)
+ vpDisplay::displayDotLine(I,vpImagePoint(dTopLeft.get_i(),x), vpImagePoint(dTopLeft.get_i()+dHeight,x), gridColor, gridThickness);
+ else
+ vpDisplay::displayDotLine(I,vpImagePoint(yorg,x), vpImagePoint(yorg-3,x), vpColor::black, gridThickness);
+
+ if (t+xdelt <= xmax+1e-10)
+ {
+ double ttemp;
+ if (power!=0)
+ ttemp = t*pow(10.0,power);
+ else ttemp = t;
+ sprintf(valeur, "%.2f", ttemp);
+#if defined VISP_HAVE_X11
+ vpDisplay::displayCharString(I,vpImagePoint(yorg + 3*epsi,x),valeur, vpColor::black);
+#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ vpDisplay::displayCharString(I,vpImagePoint(yorg + epsi,x),valeur, vpColor::black);
+#endif
+ }
+ }
+ if (power != 0)
+ {
+ sprintf(valeur, "x10e%d", -power);
+#if defined VISP_HAVE_X11
+ vpDisplay::displayCharString(I,vpImagePoint(yorg+4*epsi,dTopLeft.get_j()+dWidth-6*epsj),valeur, vpColor::black);
+#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ vpDisplay::displayCharString(I,vpImagePoint(yorg+4*epsi,dTopLeft.get_j()+dWidth-10*epsj),valeur, vpColor::black);
+#endif
+ }
+
+ power = laFonctionSansNom(ydelt);
+ for(t=ymin;t<=ymax;t=t+ydelt)
+ {
+ double y = yorg-(zoomy*t);
+ if(gridx)
+ vpDisplay::displayDotLine(I,vpImagePoint(y,dTopLeft.get_j()), vpImagePoint(y,dTopLeft.get_j()+dWidth), gridColor, gridThickness);
+ else
+ vpDisplay::displayDotLine(I,vpImagePoint(y,xorg), vpImagePoint(y,xorg+3), vpColor::black, gridThickness);
+
+ double ttemp;
+ if (power!=0)
+ ttemp = t*pow(10.0,power);
+ else ttemp = t;
+
+ sprintf(valeur, "%.2f", ttemp);
+#if defined VISP_HAVE_X11
+ vpDisplay::displayCharString(I,vpImagePoint(y+epsi,topLeft.get_j()+epsj),valeur, vpColor::black);
+#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ vpDisplay::displayCharString(I,vpImagePoint(y-epsi,topLeft.get_j()+epsj),valeur, vpColor::black);
+#endif
+ }
+ if (power != 0)
+ {
+ sprintf(valeur, "x10e%d", -power);
+#if defined VISP_HAVE_X11
+ vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i()-3*epsi,dTopLeft.get_j()-6*epsj),valeur, vpColor::black);
+#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i()-3*epsi,dTopLeft.get_j()-6*epsj),valeur, vpColor::black);
+#endif
+ }
+
+ //Ligne horizontal
+ vpDisplay::displayArrow(I,vpImagePoint(yorg,dTopLeft.get_j()), vpImagePoint(yorg,dTopLeft.get_j()+dWidth), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
+ //Ligne verticale
+ vpDisplay::displayArrow(I, vpImagePoint(dTopLeft.get_i()+dHeight,xorg), vpImagePoint(dTopLeft.get_i(),xorg), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
+
+ if (dispUnit)
+ displayUnit(I);
+ if (dispTitle)
+ displayTitle(I);
+ if (dispLegend)
+ displayLegend(I);
+
+ //vpDisplay::flushROI(I,graphZone);
+}
+
+void
+vpPlotGraph::displayUnit (vpImage<unsigned char> &
+#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ I
+#endif
+ )
+{
+ unsigned int offsetx = vpMath::minimum<unsigned int>((unsigned int)strlen(unitx), dWidth);
+
+#if defined VISP_HAVE_X11
+ vpDisplay::displayCharString(I,vpImagePoint(yorg-2*epsi,dTopLeft.get_j()+dWidth-offsetx*epsj),unitx, vpColor::black);
+ vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i(),dTopLeft.get_j()+epsj),unity, vpColor::black);
+#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
+ vpDisplay::displayCharString(I,vpImagePoint(yorg-5*epsi,dTopLeft.get_j()+dWidth-offsetx*epsj),unitx, vpColor::black);
+ vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i(),dTopLeft.get_j()+epsj),unity, vpColor::black);
+#endif
+}
+
+void
+vpPlotGraph::displayTitle (vpImage<unsigned char> &I)
+{
+ double size = (double)strlen(title);
+ size = size/2.0;
+ vpDisplay::displayCharString(I,
+ vpImagePoint(dTopLeft.get_i()-3*epsi,
+ dTopLeft.get_j()+dWidth/2.0-4*size),
+ title,
+ vpColor::black);
+}
+
+void
+vpPlotGraph::displayLegend (vpImage<unsigned char> &I)
+{
+ size_t offsetj = 0;
+ for (unsigned int i = 0; i < curveNbr; i++) {
+ size_t offset = epsj * strlen((curveList+i)->legend);
+ offsetj = vpMath::maximum(offset, offsetj);
+ }
+ if (offsetj > dWidth) offsetj = dWidth;
+
+ for (unsigned int i = 0; i < curveNbr; i++) {
+ vpDisplay::displayCharString(I,
+ vpImagePoint(dTopLeft.get_i()+i*5*epsi,
+ dTopLeft.get_j()+dWidth-offsetj),
+ (curveList+i)->legend,
+ (curveList+i)->color);
+ }
+}
+
+void
+vpPlotGraph::rescalex(unsigned int side, double extremity)
+{
+ switch (side)
+ {
+ case 0:
+ xmin = (3*extremity-xmax)/2;
+ break;
+ case 1:
+ xmax = (3*extremity-xmin)/2;
+ break;
+ }
+
+ xdelt = (xmax-xmin)/(double)nbDivisionx;
+}
+
+void
+vpPlotGraph::rescaley(unsigned int side, double extremity)
+{
+ switch (side)
+ {
+ case 0:
+ ymin = (3*extremity-ymax)/2;
+ break;
+ case 1:
+ ymax = (3*extremity-ymin)/2;
+ break;
+ }
+
+ ydelt = (ymax-ymin)/(double)nbDivisiony;
+}
+
+void
+vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
+ const double y_min, const double y_max, const int nbDivy, const bool gx, const bool gy)
+{
+ this->xmin = x_min;
+ this->xmax = x_max;
+ this->ymin = y_min;
+ this->ymax = y_max;
+ this->gridx = gx;
+ this->gridy = gy;
+ this->nbDivisionx = nbDivx;
+ this->nbDivisiony = nbDivy;
+ computeGraphParameters();
+ clearGraphZone(I);
+ displayGrid(I);
+ vpDisplay::flushROI(I,graphZone);
+ scaleInitialized = true;
+}
+
+
+void
+vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
+ const double y_min, const double y_max, const int nbDivy,
+ const double z_min, const double z_max, const int nbDivz, const bool gx, const bool gy)
+{
+ this->xmin = x_min;
+ this->xmax = x_max;
+ this->ymin = y_min;
+ this->ymax = y_max;
+ this->zmin = z_min;
+ this->zmax = z_max;
+ this->gridx = gx;
+ this->gridy = gy;
+ this->nbDivisionx = nbDivx;
+ this->nbDivisiony = nbDivy;
+ this->nbDivisionz = nbDivz;
+ computeGraphParameters();
+ clearGraphZone(I);
+ displayGrid(I);
+ vpDisplay::flushROI(I,graphZone);
+ scaleInitialized = true;
+}
+
+void
+vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y)
+{
+ if (!scaleInitialized)
+ {
+ if (x < 0)
+ {
+ xmax = 0;
+ rescalex(0,x);
+ }
+ if (x > 0)
+ {
+ xmin = 0;
+ rescalex(1,x);
+ }
+ if (y < 0)
+ {
+ ymax = 0;
+ rescaley(0,y);
+ }
+ if (y > 0)
+ {
+ ymin = 0;
+ rescaley(1,y);
+ }
+ scaleInitialized = true;
+ computeGraphParameters();
+ clearGraphZone(I);
+ displayGrid(I);
+ //if (y == 0)
+ if (std::fabs(y) <= std::numeric_limits<double>::epsilon())
+ scaleInitialized = false;
+ }
+
+ if (firstPoint)
+ {
+// clearGraphZone(I);
+// displayGrid(I);
+// vpDisplay::flushROI(I,graphZone);
+ replot(I);
+ firstPoint = false;
+ }
+
+ double i = yorg-(zoomy*y);
+ double j = xorg+(zoomx*x);
+
+ vpImagePoint iP(i,j);
+
+ if (!iP.inRectangle(dGraphZone))
+ {
+ if (x > xmax) rescalex(1,x);
+ else if(x < xmin) rescalex(0,x);
+
+ if (y > ymax) rescaley(1,y);
+ else if(y < ymin) rescaley(0,y);
+
+ computeGraphParameters();
+
+ replot(I);
+ i = yorg-(zoomy*y);
+ j = xorg+(zoomx*x);
+
+ iP.set_ij(i,j);
+ }
+
+ (curveList+curveNb)->plotPoint(I, iP, x, y);
+#if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
+ vpDisplay::flushROI(I,graphZone);
+ //vpDisplay::flush(I);
+#endif
+}
+
+void
+vpPlotGraph::replot (vpImage<unsigned char> &I)
+{
+ clearGraphZone(I);
+ displayGrid(I);
+ for (unsigned int i = 0; i < curveNbr; i++)
+ (curveList+i)->plotList(I,xorg,yorg,zoomx,zoomy);
+ vpDisplay::flushROI(I,graphZone);
+}
+
+void
+vpPlotGraph::clearGraphZone(vpImage<unsigned char> &I)
+{
+ vpDisplay::displayROI(I,graphZone);
+}
+
+bool
+vpPlotGraph::getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP)
+{
+ if (iP.inRectangle(dGraphZone))
+ {
+ double x = (iP.get_j()-xorg)/zoomx;
+ double y = (yorg-iP.get_i())/zoomy;
+
+ vpDisplay::displayROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
+ char valeur[200];
+ sprintf(valeur, " x: %f", x);
+ vpDisplay::displayCharString(I,vpImagePoint(topLeft.get_i()+height-2,topLeft.get_j()+5*epsj),valeur, vpColor::black);
+ sprintf(valeur, " y: %f", y);
+ vpDisplay::displayCharString(I,vpImagePoint(topLeft.get_i()+height-2,topLeft.get_j()+width/2.0),valeur, vpColor::black);
+// vpDisplay::flush(I);
+ vpDisplay::flushROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
+ return true;
+ }
+ return false;
+}
+
+void
+vpPlotGraph::resetPointList(const unsigned int curveNum)
+{
+ (curveList+curveNum)->pointListx.clear();
+ (curveList+curveNum)->pointListy.clear();
+ (curveList+curveNum)->pointListz.clear();
+ (curveList+curveNum)->nbPoint = 0;
+ firstPoint = true;
+}
+
+
+/************************************************************************************************/
+
+bool
+vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
+{
+ bool iP1In = iP1.inRectangle(dGraphZone3D);
+ bool iP2In = iP2.inRectangle(dGraphZone3D);
+
+ if (!iP1In || !iP2In)
+ {
+ double dTopLeft_i = dTopLeft3D.get_i();
+ double dTopLeft_j = dTopLeft3D.get_j();
+ double dBottomRight_i = dTopLeft_i+dHeight;
+ double dBottomRight_j = dTopLeft_j+dWidth;
+
+ //Cas vertical
+ if (vpImagePoint::distance(iP1,iP2) < 9)
+ return false;
+ if (fabs(iP2.get_j()-iP1.get_j()) <=2)
+ {
+ if (!iP1In && !iP2In)
+ {
+ if (iP1.get_i() < dTopLeft_i && iP2.get_i() < dTopLeft_i)
+ return false;
+ if (iP1.get_i() > dBottomRight_i && iP2.get_i() > dBottomRight_i)
+ return false;
+ if (iP1.get_j() < dTopLeft_j || iP1.get_j() > dBottomRight_j)
+ return false;
+ if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
+ else iP1.set_i(dBottomRight_i);
+ if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
+ else iP2.set_i(dBottomRight_i);
+ }
+ else if (!iP1In)
+ {
+ if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
+ if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
+ if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
+ if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
+ return true;
+ }
+ else if (!iP2In)
+ {
+ if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
+ if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
+ if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
+ if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
+ return true;
+ }
+ }
+ //cas horizontal
+ else if (fabs(iP2.get_i()-iP1.get_i()) <= 2)
+ {
+ if (!iP1In && !iP2In)
+ {
+ if (iP1.get_j() < dTopLeft_j && iP2.get_j() < dTopLeft_j)
+ return false;
+ if (iP1.get_j() > dBottomRight_j && iP2.get_j() > dBottomRight_j)
+ return false;
+ if (iP1.get_i() < dTopLeft_i || iP1.get_i() > dBottomRight_i)
+ return false;
+ if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
+ else iP1.set_j(dBottomRight_j);
+ if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
+ else iP2.set_j(dBottomRight_j);
+ }
+ else if (!iP1In)
+ {
+ if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
+ if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
+ if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
+ if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
+ return true;
+ }
+ else if (!iP2In)
+ {
+ if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
+ if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
+ if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
+ if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
+ return true;
+ }
+ }
+
+ double a = (iP2.get_i()-iP1.get_i())/(iP2.get_j()-iP1.get_j());
+ double b = iP1.get_i()-a*iP1.get_j();
+
+ //test horizontal
+ double jtop = (dTopLeft_i-b)/a;
+ double jlow = (dBottomRight_i-b)/a;
+ //test vertical
+ double ileft = dTopLeft_j*a+b;
+ double iright = (dBottomRight_j)*a+b;
+
+ vpImagePoint iP[2];
+ int n = 0;
+
+ if(jtop >= dTopLeft_j && jtop <= dBottomRight_j)
+ {
+ iP[n].set_ij(dTopLeft_i,jtop);
+ n++;
+ }
+ if(jlow >= dTopLeft_j && jlow <= dBottomRight_j)
+ {
+ iP[n].set_ij(dBottomRight_i,jlow);
+ n++;
+ }
+ if(ileft >= dTopLeft_i && ileft <= dBottomRight_i && n <2)
+ {
+ iP[n].set_ij(ileft,dTopLeft_j);
+ n++;
+ }
+ if(iright >= dTopLeft_i && iright <= dBottomRight_i && n <2)
+ {
+ iP[n].set_ij(iright,dBottomRight_j);
+ n++;
+ }
+
+ if (n < 2)
+ return false;
+
+ if (!iP1In && !iP2In)
+ {
+ if (fabs(a) < 1)
+ {
+ if (vpMath::sign(iP1.get_j()-iP[0].get_j()) == vpMath::sign(iP2.get_j()-iP[0].get_j()))
+ return false;
+ int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
+ if (sign == vpMath::sign(iP[0].get_j()-iP[1].get_j()))
+ {
+ iP1 = iP[0]; iP2 = iP[1];
+ }
+ else
+ {
+ iP1 = iP[1]; iP2 = iP[0];
+ }
+ }
+ else
+ {
+ if (vpMath::sign(iP1.get_i()-iP[0].get_i()) == vpMath::sign(iP2.get_i()-iP[0].get_i()))
+ return false;
+ int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
+ if (sign == vpMath::sign(iP[0].get_i()-iP[1].get_i()))
+ {
+ iP1 = iP[0];iP2 = iP[1];
+ }
+ else
+ {
+ iP1 = iP[1];iP2 = iP[0];
+ }
+ }
+ }
+ else if (!iP1In)
+ {
+ vpImagePoint iPtemp = iP1;
+ if (fabs(a) < 1)
+ {
+ int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
+ if (fabs(iP[0].get_j()-iP2.get_j()) > 5)
+ {
+ if (sign == vpMath::sign(iP[0].get_j()-iP2.get_j())) iP1 = iP[0];
+ else iP1 = iP[1];
+ }
+ else
+ {
+ if (sign == vpMath::sign(iP[1].get_j()-iP2.get_j())) iP1 = iP[1];
+ else iP1 = iP[0];
+ }
+ }
+ else
+ {
+ int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
+ if (fabs(iP[0].get_i()-iP2.get_i()) > 5)
+ {
+ if (sign == vpMath::sign(iP[0].get_i()-iP2.get_i())) iP1 = iP[0];
+ else iP1 = iP[1];
+ }
+ else
+ {
+ if (sign == vpMath::sign(iP[1].get_i()-iP2.get_i())) iP1 = iP[1];
+ else iP1 = iP[0];
+ }
+ }
+ if (vpImagePoint::distance(iP1,iP2) < 9)
+ {
+ iP1 = iPtemp;
+ return false;
+ }
+ }
+ else if (!iP2In)
+ {
+ vpImagePoint iPtemp = iP2;
+ if (fabs(a) < 1)
+ {
+ int sign = vpMath::sign(iP2.get_j() - iP1.get_j());
+ if (fabs(iP[0].get_j()-iP1.get_j()) > 5)
+ {
+ if (sign == vpMath::sign(iP[0].get_j()-iP1.get_j())) iP2 = iP[0];
+ else iP2 = iP[1];
+ }
+ else
+ {
+ if (sign == vpMath::sign(iP[1].get_j()-iP1.get_j())) iP2 = iP[1];
+ else iP2 = iP[0];
+ }
+ }
+ else
+ {
+ int sign = vpMath::sign(iP2.get_i() - iP1.get_i());
+ if (fabs(iP[0].get_i()-iP1.get_i()) > 5)
+ {
+ if (sign == vpMath::sign(iP[0].get_i()-iP1.get_i())) iP2 = iP[0];
+ else iP2 = iP[1];
+ }
+ else
+ {
+ if (sign == vpMath::sign(iP[1].get_i()-iP1.get_i())) iP2 = iP[1];
+ else iP2 = iP[0];
+ }
+ }
+ if (vpImagePoint::distance(iP1,iP2) < 9)
+ {
+ iP2 = iPtemp;
+ return false;
+ }
+ }
+ }
+ return true;
+}
+
+bool
+vpPlotGraph::check3Dpoint(vpImagePoint &iP)
+{
+ if (!iP.inRectangle(dGraphZone3D))
+ {
+ if (iP.get_i() < dTopLeft3D.get_i())
+ iP.set_i(dTopLeft3D.get_i());
+ else if (iP.get_i() > dTopLeft3D.get_i()+dHeight)
+ iP.set_i(dTopLeft3D.get_i()+dHeight-1);
+ if (iP.get_j() <dTopLeft3D.get_j())
+ iP.set_j(dTopLeft3D.get_j());
+ else if (iP.get_j() > dTopLeft3D.get_j()+dWidth)
+ iP.set_j(dTopLeft3D.get_j()+dWidth-1);
+ return false;
+ }
+ return true;
+}
+
+void
+vpPlotGraph::computeGraphParameters3D()
+{
+ zoomx_3D = w_xsize/(xmax-xmin);
+ zoomy_3D = w_ysize/(ymax-ymin);
+ zoomz_3D = w_zsize/(zmax-zmin);
+ ptXorg = w_xval - zoomx_3D*xmax;
+ ptYorg = w_yval + zoomy_3D*ymin;
+ ptZorg = w_zval - zoomz_3D*zmax;
+}
+
+void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3)
+{
+ if (pente <= 1)
+ {
+ ip1 = iPunit-vpImagePoint(3,0);
+ ip2 = iPunit+vpImagePoint(3,0);
+ ip3 = iPunit-vpImagePoint(6,6);
+ }
+ else
+ {
+ ip1 = iPunit-vpImagePoint(0,3);
+ ip2 = iPunit+vpImagePoint(0,3);
+ ip3 = iPunit+vpImagePoint(6,6);
+ }
+}
+
+void
+vpPlotGraph::displayGrid3D (vpImage<unsigned char> &I)
+{
+ computeGraphParameters3D();
+
+ xdelt = (xmax-xmin)/nbDivisionx;
+ ydelt = (ymax-ymin)/nbDivisiony;
+ zdelt = (zmax-zmin)/nbDivisionz;
+
+ vpPoint pt[6];
+ pt[0].setWorldCoordinates(-w_xval,ptYorg,ptZorg);
+ pt[1].setWorldCoordinates(w_xval,ptYorg,ptZorg);
+ pt[2].setWorldCoordinates(ptXorg,-w_yval,ptZorg);
+ pt[3].setWorldCoordinates(ptXorg,w_yval,ptZorg);
+ pt[4].setWorldCoordinates(ptXorg,ptYorg,-w_zval);
+ pt[5].setWorldCoordinates(ptXorg,ptYorg,w_zval);
+
+ vpImagePoint iP[6];
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ pt[i].track(cMo);
+ double u=0.0,v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,pt[i].get_x(),pt[i].get_y(),u,v);
+ iP[i].set_uv(u,v);
+ iP[i] = iP[i] + dTopLeft3D;
+ }
+
+ int power;
+ double t;
+ char valeur[20];
+ vpPoint ptunit;
+ vpImagePoint iPunit;
+ double pente;
+ vpImagePoint ip1;
+ vpImagePoint ip2;
+ vpImagePoint ip3;
+ vpImagePoint ip4;
+
+ power = laFonctionSansNom(xdelt);
+ ptunit.setWorldCoordinates(-w_xval,ptYorg,ptZorg);
+ //if (iP[0].get_j()-iP[1].get_j() != 0)
+ if (std::fabs(iP[0].get_j()-iP[1].get_j()) >
+ vpMath::maximum(std::fabs(iP[0].get_j()), std::fabs(iP[1].get_j()))* std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[0].get_i()-iP[1].get_i())/(iP[0].get_j()-iP[1].get_j()));
+ else pente = 2;
+
+ unsigned int count = 1;
+ for(t=xmin;t<=xmax;t=t+xdelt)
+ {
+ double x = ptXorg+(zoomx_3D*t);
+ ptunit.set_oX(x);
+ ptunit.track(cMo);
+ double u=0.0, v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
+ iPunit.set_uv(u,v);
+ iPunit = iPunit + dTopLeft3D;
+
+ getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
+
+ if(check3Dline(ip1,ip2))
+ {
+ vpDisplay::displayLine(I,ip2, ip1, vpColor::black);
+ if (count%2 == 1)
+ {
+ double ttemp;
+ if (power!=0)
+ ttemp = t*pow(10.0,power);
+ else ttemp = t;
+ sprintf(valeur, "%.1f", ttemp);
+ vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
+ }
+ }
+ count++;
+ }
+ if (power != 0)
+ {
+ ip4 = iP[1] -vpImagePoint(-15,10);
+ sprintf(valeur, "x10e%d", -power);
+ if(check3Dpoint(ip4))
+ vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
+ }
+
+ power = laFonctionSansNom(ydelt);
+ ptunit.setWorldCoordinates(ptXorg,-w_yval,ptZorg);
+ //if (iP[2].get_j()-iP[3].get_j() != 0)
+ if (std::fabs(iP[2].get_j()-iP[3].get_j()) >
+ vpMath::maximum(std::fabs(iP[2].get_j()), std::fabs(iP[3].get_j()))* std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[2].get_i()-iP[3].get_i())/(iP[2].get_j()-iP[3].get_j()));
+ else pente = 2;
+ count = 0;
+ for(t=ymin;t<=ymax;t=t+ydelt)
+ {
+ double y = ptYorg-(zoomy_3D*t);
+ ptunit.set_oY(y);
+ ptunit.track(cMo);
+ double u=0.0, v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
+ iPunit.set_uv(u,v);
+ iPunit = iPunit + dTopLeft3D;
+
+ getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
+
+ if(check3Dline(ip1,ip2))
+ {
+ vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
+ if (count%2 == 1)
+ {
+ double ttemp;
+ if (power!=0)
+ ttemp = t*pow(10.0,power);
+ else ttemp = t;
+ sprintf(valeur, "%.1f", ttemp);
+ vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
+ }
+ }
+ count++;
+ }
+ if (power != 0)
+ {
+ ip4 = iP[2] -vpImagePoint(-15,10);
+ sprintf(valeur, "x10e%d", -power);
+ if(check3Dpoint(ip4))
+ vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
+ }
+
+ power = laFonctionSansNom(zdelt);
+ ptunit.setWorldCoordinates(ptXorg,ptYorg,-w_zval);
+ //if (iP[4].get_j()-iP[5].get_j() != 0)
+ if (std::fabs(iP[4].get_j()-iP[5].get_j()) >
+ vpMath::maximum(std::fabs(iP[4].get_j()), std::fabs(iP[5].get_j()))* std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[4].get_i()-iP[5].get_i())/(iP[4].get_j()-iP[5].get_j()));
+ else pente = 2;
+ count = 0;
+ for(t=zmin;t<=zmax;t=t+zdelt)
+ {
+ double z = ptZorg+(zoomz_3D*t);
+ ptunit.set_oZ(z);
+ ptunit.track(cMo);
+ double u=0.0, v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
+ iPunit.set_uv(u,v);
+ iPunit = iPunit + dTopLeft3D;
+
+ getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
+
+ if(check3Dline(ip1,ip2))
+ {
+ vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
+ if (count%2 == 1)
+ {
+ double ttemp;
+ if (power!=0)
+ ttemp = t*pow(10.0,power);
+ else ttemp = t;
+ sprintf(valeur, "%.1f", ttemp);
+ vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
+ }
+ }
+ count++;
+ }
+ if (power != 0)
+ {
+ ip4 = iP[5] -vpImagePoint(-15,10);
+ sprintf(valeur, "x10e%d", -power);
+ if(check3Dpoint(ip4))
+ vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
+ }
+
+
+//Ligne horizontal
+ if (check3Dline(iP[0],iP[1]))
+ {
+ vpDisplay::displayArrow(I,iP[0],iP[1], vpColor::black, gridThickness);
+ if (dispUnit)
+ {
+ iPunit.set_ij(iP[1].get_i(),iP[1].get_j()-10*epsj);
+ check3Dpoint (iPunit);
+ vpDisplay::displayCharString(I,iPunit,unitx, vpColor::black);
+ }
+ }
+ if (check3Dline(iP[3],iP[2]))
+ {
+ vpDisplay::displayArrow(I,iP[3],iP[2], vpColor::black, gridThickness);
+ if (dispUnit)
+ {
+ iPunit.set_ij(iP[2].get_i(),iP[2].get_j()-10*epsj);
+ check3Dpoint (iPunit);
+ vpDisplay::displayCharString(I,iPunit,unity, vpColor::black);
+ }
+ }
+ if (check3Dline(iP[4],iP[5]))
+ {
+ vpDisplay::displayArrow(I,iP[4],iP[5], vpColor::black, gridThickness);
+ if (dispUnit)
+ {
+ iPunit.set_ij(iP[5].get_i(),iP[5].get_j()-10*epsj);
+ check3Dpoint (iPunit);
+ vpDisplay::displayCharString(I,iPunit,unitz, vpColor::black);
+ }
+ }
+
+ if (dispTitle)
+ displayTitle(I);
+ if (dispLegend)
+ displayLegend(I);
+}
+
+vpMouseButton::vpMouseButtonType
+vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z)
+{
+ if (!scaleInitialized)
+ {
+ if (x < 0)
+ {
+ xmax = 0;
+ rescalex(0,x);
+ }
+ if (x > 0)
+ {
+ xmin = 0;
+ rescalex(1,x);
+ }
+ if (y < 0)
+ {
+ ymax = 0;
+ rescaley(0,y);
+ }
+ if (y > 0)
+ {
+ ymin = 0;
+ rescaley(1,y);
+ }
+ if (z < 0)
+ {
+ zmax = 0;
+ rescalez(0,z);
+ }
+ if (z > 0)
+ {
+ zmin = 0;
+ rescalez(1,z);
+ }
+ scaleInitialized = true;
+ computeGraphParameters3D();
+ clearGraphZone(I);
+ displayGrid3D(I);
+ //if (std::fabs(y) == 0 || z == 0)
+ if (std::fabs(y) <= std::numeric_limits<double>::epsilon() || std::fabs(z) <= std::numeric_limits<double>::epsilon())
+ scaleInitialized = false;
+ }
+
+ if (firstPoint)
+ {
+ clearGraphZone(I);
+ displayGrid3D(I);
+ vpDisplay::flushROI(I,graphZone);
+ firstPoint = false;
+ }
+
+ bool changed = false;
+ if (x > xmax) {rescalex(1,x); changed = true;}
+ else if(x < xmin) {rescalex(0,x);changed = true;}
+
+ if (y > ymax) {rescaley(1,y);changed = true;}
+ else if(y < ymin) {rescaley(0,y);changed = true;}
+
+ if (z > zmax) {rescalez(1,z);changed = true;}
+ else if(z < zmin) {rescalez(0,z);changed = true;}
+
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
+
+ if (changed || move(I, button))
+ {
+ computeGraphParameters3D();
+ replot3D(I);
+ }
+
+ vpPoint pointPlot;
+ pointPlot.setWorldCoordinates(ptXorg+(zoomx_3D*x),ptYorg-(zoomy_3D*y),ptZorg+(zoomz_3D*z));
+ pointPlot.track(cMo);
+ double u=0.0, v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,pointPlot.get_x(),pointPlot.get_y(),u,v);
+ vpImagePoint iP;
+ iP.set_uv(u,v);
+ iP = iP + dTopLeft3D;
+
+ (curveList+curveNb)->pointListx.end();
+ (curveList+curveNb)->pointListy.end();
+ (curveList+curveNb)->pointListz.end();
+ if((curveList+curveNb)->nbPoint)
+ {
+ if (check3Dline((curveList+curveNb)->lastPoint,iP))
+ vpDisplay::displayLine(I,(curveList+curveNb)->lastPoint, iP, (curveList+curveNb)->color, (curveList+curveNb)->thickness);
+ }
+#if( defined VISP_HAVE_X11 || defined VISP_HAVE_GDI )
+ double top;
+ double left;
+ double width_;
+ double height_;
+
+ if (iP.get_i() <= (curveList+curveNb)->lastPoint.get_i()) {top = iP.get_i()-5; height_ = (curveList+curveNb)->lastPoint.get_i() - top+10;}
+ else {top = (curveList+curveNb)->lastPoint.get_i()-5; height_ = iP.get_i() - top+10;}
+ if (iP.get_j() <= (curveList+curveNb)->lastPoint.get_j()) {left = iP.get_j()-5; width_ = (curveList+curveNb)->lastPoint.get_j() - left+10;}
+ else {left = (curveList+curveNb)->lastPoint.get_j()-5; width_ = iP.get_j() - left+10;}
+ vpDisplay::flushROI(I,vpRect(left,top,width_,height_));
+#endif
+
+ (curveList+curveNb)->lastPoint = iP;
+ (curveList+curveNb)->pointListx.push_back(x);
+ (curveList+curveNb)->pointListy.push_back(y);
+ (curveList+curveNb)->pointListz.push_back(z);
+ (curveList+curveNb)->nbPoint++;
+
+#if( !defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
+ vpDisplay::flushROI(I,graphZone);
+#endif
+ return button;
+}
+
+void
+vpPlotGraph::replot3D (vpImage<unsigned char> &I)
+{
+ clearGraphZone(I);
+ displayGrid3D(I);
+
+ for (unsigned int i = 0; i < curveNbr; i++)
+ {
+ std::list<double>::const_iterator it_ptListx = (curveList+i)->pointListx.begin();
+ std::list<double>::const_iterator it_ptListy = (curveList+i)->pointListy.begin();
+ std::list<double>::const_iterator it_ptListz = (curveList+i)->pointListz.begin();
+
+ unsigned int k = 0;
+ vpImagePoint iP;
+ vpPoint pointPlot;
+ double x,y,z;
+ while (k < (curveList+i)->nbPoint)
+ {
+ x = *it_ptListx;
+ y = *it_ptListy;
+ z = *it_ptListz;
+ pointPlot.setWorldCoordinates(ptXorg+(zoomx_3D*x),ptYorg-(zoomy_3D*y),ptZorg+(zoomz_3D*z));
+ pointPlot.track(cMo);
+ double u=0.0, v=0.0;
+ vpMeterPixelConversion::convertPoint(cam,pointPlot.get_x(),pointPlot.get_y(),u,v);
+ iP.set_uv(u,v);
+ iP = iP + dTopLeft3D;
+
+ //vpDisplay::displayCross(I,iP,3,vpColor::cyan);
+ if (k > 0)
+ {
+ if (check3Dline((curveList+i)->lastPoint,iP))
+ vpDisplay::displayLine(I,(curveList+i)->lastPoint, iP, (curveList+i)->color);
+ //vpDisplay::displayCross(I,iP,3,vpColor::orange);
+ }
+
+ (curveList+i)->lastPoint = iP;
+
+ ++it_ptListx;
+ ++it_ptListy;
+ ++it_ptListz;
+ k++;
+ }
+ }
+ vpDisplay::flushROI(I,graphZone);
+}
+
+
+
+void
+vpPlotGraph::rescalez(unsigned int side, double extremity)
+{
+ switch (side)
+ {
+ case 0:
+ zmin = (3*extremity-zmax)/2;
+ break;
+ case 1:
+ zmax = (3*extremity-zmin)/2;
+ break;
+ }
+
+ zdelt = (zmax-zmin)/(double)nbDivisionz;
+}
+
+/*!
+ \param I : Image used to display the graph.
+ \param button : Mouse button used during navigation.
+ */
+bool
+vpPlotGraph::move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button)
+{
+ bool changed = false;
+ vpHomogeneousMatrix displacement = navigation(I, changed, button);
+
+ //if (displacement[2][3] != 0)
+ if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
+ cMf = cMf*displacement;
+ vpHomogeneousMatrix fMo = cMf.inverse()*cMo;
+
+ cMo = cMf* displacement * fMo;
+ return changed;
+}
+
+vpHomogeneousMatrix
+vpPlotGraph::navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b)
+{
+ vpImagePoint iP;
+ vpImagePoint trash;
+ bool clicked = false;
+ bool clickedUp = false;
+
+ vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ changed = false;
+
+ //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
+
+ if(!blocked) clicked = vpDisplay::getClick(I,iP,b,false);
+
+ if(blocked) clickedUp = vpDisplay::getClickUp(I,trash, b,false);
+
+ if (clicked)
+ {
+ if (!iP.inRectangle(graphZone))
+ return mov;
+ }
+
+ if(clicked)
+ {
+ if (b == vpMouseButton::button1) blockedr = true;
+ if (b == vpMouseButton::button2) blockedz = true;
+ blocked = true;
+ }
+
+ else if(clickedUp)
+ {
+ if (b == vpMouseButton::button1)
+ {
+ old_iPr = vpImagePoint(-1,-1);
+ blockedr = false;
+ }
+ if (b == vpMouseButton::button2)
+ {
+ old_iPz = vpImagePoint(-1,-1);
+ blockedz = false;
+ }
+ if (!(blockedr || blockedz))
+ {
+ blocked = false;
+ //while (vpDisplay::getClick(I,trash,b,false)) {};
+ }
+ }
+
+ vpDisplay::getPointerPosition(I,iP);
+
+ double anglei = 0;
+ double anglej = 0;
+
+ if (old_iPr != vpImagePoint(-1,-1) && blockedr)
+ {
+ double width_ = vpMath::minimum(I.getWidth(),I.getHeight());
+
+ double diffi = iP.get_i() - old_iPr.get_i();
+ double diffj = iP.get_j() - old_iPr.get_j();
+
+ anglei = diffi*360/width_;
+ anglej = diffj*360/width_;
+ mov.buildFrom(0,0,0,vpMath::rad(anglei),vpMath::rad(-anglej),0);
+ changed = true;
+ }
+
+ if (blockedr) old_iPr = iP;
+
+ if (old_iPz != vpImagePoint(-1,-1) && blockedz)
+ {
+ double diffi = iP.get_i() - old_iPz.get_i();
+ mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ changed = true;
+ }
+
+ if (blockedz) old_iPz = iP;
+
+ return mov;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpPlotGraph.cpp.o) has no symbols
+void dummy_vpPlotGraph() {};
+#endif
+#endif
diff --git a/modules/gui/test/display/testClick.cpp b/modules/gui/test/display/testClick.cpp
new file mode 100644
index 0000000..4fb1df9
--- /dev/null
+++ b/modules/gui/test/display/testClick.cpp
@@ -0,0 +1,439 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for mouse click manipulations.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <iostream>
+#include <string>
+
+#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+/*!
+ \example testClick.cpp
+
+ \brief Mouse click manipulations.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "i:hlt:dc"
+
+typedef enum {
+ vpX11,
+ vpGTK,
+ vpGDI,
+ vpD3D,
+ vpCV
+} vpDisplayType;
+
+void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
+bool getOptions(int argc, const char **argv,
+ std::string &ipath, vpDisplayType &dtype, bool &list,
+ bool &click_allowed, bool &display );
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param dtype : Type of video device.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype)
+{
+ fprintf(stdout, "\n\
+Test click functionnalities in video devices or display.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] \n\
+ [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
+", name);
+
+ std::string display;
+ switch(dtype) {
+ case vpX11: display = "X11"; break;
+ case vpGTK: display = "GTK"; break;
+ case vpGDI: display = "GDI"; break;
+ case vpD3D: display = "D3D"; break;
+ case vpCV: display = "CV"; break;
+ }
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -t <type of video device> \"%s\"\n\
+ String specifying the video device to use.\n\
+ Possible values:\n\
+ \"X11\": only on UNIX platforms,\n\
+ \"GTK\": on all plaforms,\n\
+ \"GDI\": only on Windows platform (Graphics Device Interface),\n\
+ \"D3D\": only on Windows platform (Direct3D).\n\
+ \"CV\" : (OpenCV).\n\
+\n\
+ -l\n\
+ Print the list of video-devices available and exit.\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), display.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param dtype : Type of video device.
+ \param list : Set as true,list the available video-devices.
+ \param click_allowed : Enable/disable mouse click.
+ \param display : Set as true, activates the image display. This is
+ the default configuration. When set to false, the display is
+ disabled. This can be useful for automatic tests using crontab
+ under Unix or using the task manager under Windows.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv,
+ std::string &ipath, vpDisplayType &dtype, bool &list,
+ bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ std::string sDisplayType;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'l': list = true; break;
+ case 't': sDisplayType = optarg_;
+ // Parse the display type option
+ if (sDisplayType.compare("X11") == 0) {
+ dtype = vpX11;
+ }
+ else if (sDisplayType.compare("GTK") == 0) {
+ dtype = vpGTK;
+ }
+ else if (sDisplayType.compare("GDI") == 0) {
+ dtype = vpGDI;
+ }
+ else if (sDisplayType.compare("D3D") == 0) {
+ dtype = vpD3D;
+ }
+ else if (sDisplayType.compare("CV") == 0) {
+ dtype = vpCV;
+ }
+
+ break;
+ case 'h': usage(argv[0], NULL, ipath, dtype); return false; break;
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+
+ default:
+ usage(argv[0], optarg_, ipath, dtype); return false; break;
+ }
+ }
+
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, dtype);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ bool opt_list = false; // To print the list of video devices
+ vpDisplayType opt_dtype; // Type of display to use
+ std::string ipath;
+ std::string filename;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Default display is one available
+#if defined VISP_HAVE_GTK
+ opt_dtype = vpGTK;
+#elif defined VISP_HAVE_X11
+ opt_dtype = vpX11;
+#elif defined VISP_HAVE_GDI
+ opt_dtype = vpGDI;
+#elif defined VISP_HAVE_D3D9
+ opt_dtype = vpD3D;
+#elif defined VISP_HAVE_OPENCV
+ opt_dtype = vpCV;
+#endif
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Print the list of video-devices available
+ if (opt_list) {
+ unsigned nbDevices = 0;
+ std::cout << "List of video-devices available: \n";
+#if defined VISP_HAVE_GTK
+ std::cout << " GTK (use \"-t GTK\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_X11
+ std::cout << " X11 (use \"-t X11\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_GDI
+ std::cout << " GDI (use \"-t GDI\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_D3D9
+ std::cout << " D3D (use \"-t D3D\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_OPENCV
+ std::cout << " CV (use \"-t CV\" option to use it)\n";
+ nbDevices ++;
+#endif
+ if (!nbDevices) {
+ std::cout << " No display is available\n";
+ }
+ return (0);
+ }
+
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_dtype);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ // Create a grey level image
+ vpImage<unsigned char> I ;
+
+ // Load a grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+
+ // Create a display for the image
+ vpDisplay *display = NULL;
+
+ switch(opt_dtype) {
+ case vpX11:
+ std::cout << "Requested X11 display functionnalities..." << std::endl;
+#if defined VISP_HAVE_X11
+ display = new vpDisplayX;
+#else
+ std::cout << " Sorry, X11 video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGTK:
+ std::cout << "Requested GTK display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GTK
+ display = new vpDisplayGTK;
+#else
+ std::cout << " Sorry, GTK video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGDI:
+ std::cout << "Requested GDI display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GDI
+ display = new vpDisplayGDI;
+#else
+ std::cout << " Sorry, GDI video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpD3D:
+ std::cout << "Requested D3D display functionnalities..." << std::endl;
+#if defined VISP_HAVE_D3D9
+ display = new vpDisplayD3D;
+#else
+ std::cout << " Sorry, D3D video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpCV:
+ std::cout << "Requested OpenCV display functionnalities..." << std::endl;
+#if defined(VISP_HAVE_OPENCV)
+ display = new vpDisplayOpenCV;
+#else
+ std::cout << " Sorry, OpenCV video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ }
+
+ if (opt_display){
+
+ // We open a window using either X11 or GTK or GDI.
+ // Its size is automatically defined by the image (I) size
+ display->init(I, 100, 100,"Display...") ;
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(I) ;
+ //Flush the display
+ vpDisplay::flush(I) ;
+ if ( opt_click_allowed ){
+ std::cout << "Click on a pixel to get his coordinates...\n";
+ vpImagePoint ip;
+ vpMouseButton::vpMouseButtonType button;
+ vpDisplay::getClick(I, ip, button);
+ std::cout << " You click down on pixel (" << ip <<") ";
+ switch(button) {
+ case vpMouseButton::button1: std::cout << "with left button.\n"; break;
+ case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
+ case vpMouseButton::button3: std::cout << "with right button.\n"; break;
+ case vpMouseButton::none: break;
+ }
+ vpDisplay::getClickUp(I, ip, button);
+ std::cout << " You click up on pixel (" << ip <<") ";
+ switch(button) {
+ case vpMouseButton::button1: std::cout << "with left button.\n"; break;
+ case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
+ case vpMouseButton::button3: std::cout << "with right button.\n"; break;
+ case vpMouseButton::none: break;
+ }
+ vpDisplay::getPointerPosition(I,ip);
+ std::cout << " Pointer poisition : " << ip << std::endl;
+ std::cout << "A click to exit...\n";
+ vpDisplay::getClick(I) ;
+ }
+ }
+ delete display;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error while displaying the image") ;
+ exit(-1);
+ }
+}
+
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have display functionalities...");
+}
+
+#endif
diff --git a/modules/gui/test/display/testDisplayRoi.cpp b/modules/gui/test/display/testDisplayRoi.cpp
new file mode 100644
index 0000000..1ed0e0c
--- /dev/null
+++ b/modules/gui/test/display/testDisplayRoi.cpp
@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image roi display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testDisplayRoi.cpp
+
+ Test display of an image roi.
+
+*/
+
+#include <stdlib.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpRect.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Read an image on the disk, display it using X11, display some\n\
+features (line, circle, caracters) in overlay and finaly write \n\
+the image and the overlayed features in an image on the disk.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Disable the image display. This can be useful \n\
+ for automatic tests using crontab under Unix or \n\
+ using the task manager under Windows.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Enable/disable mouse click.
+ \param display : Set as true, activates the image display. This is
+ the default configuration. When set to false, the display is
+ disabled. This can be useful for automatic tests using crontab
+ under Unix or using the task manager under Windows.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char ** argv)
+{
+#ifdef VISP_HAVE_DISPLAY
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ if (opt_display) {
+
+ vpImage<unsigned char> I(480,640,255);
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX d;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK d;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d;
+#elif defined(VISP_HAVE_D3D9)
+ vpDisplayD3D d;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d;
+#endif
+ d.init(I);
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ I = 0;
+
+ vpRect roi(I.getWidth()/4, I.getHeight()/4, I.getWidth()/2, I.getHeight()/2);
+ vpDisplay::displayROI(I, roi);
+ vpDisplay::flush(I);
+ if (opt_click_allowed) {
+ std::cout << "A click in the image to continue..." << std::endl;
+ vpDisplay::getClick(I);
+ }
+ vpDisplay::close(I);
+
+ vpImage<vpRGBa> C(480,640,vpRGBa(255,0,0,0));
+
+ //vpDisplayX d;
+ d.init(C);
+ vpDisplay::display(C);
+ vpDisplay::flush(C);
+
+ C = vpRGBa(0,255,0,0);
+
+ vpDisplay::displayROI(C, roi);
+ vpDisplay::flushROI(C, roi);
+ if (opt_click_allowed) {
+ std::cout << "A click in the image to exit..." << std::endl;
+ vpDisplay::getClick(C);
+ }
+ }
+#else
+ (void)argc;
+ (void)argv;
+#endif
+ return 0;
+}
diff --git a/modules/gui/test/display/testDisplays.cpp b/modules/gui/test/display/testDisplays.cpp
new file mode 100644
index 0000000..4e53afd
--- /dev/null
+++ b/modules/gui/test/display/testDisplays.cpp
@@ -0,0 +1,404 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image display.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <iostream>
+#include <string>
+
+#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpRect.h>
+
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+/*!
+ \example testDisplays.cpp
+
+ \brief Test all the displays. Draws several shapes.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "hldc"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display);
+void draw(vpImage<vpRGBa> &I);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test video devices or display.\n\
+\n\
+SYNOPSIS\n\
+ %s [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
+", name);
+
+ std::string display;
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -l\n\
+ Print the list of video-devices available and exit.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param list : To get the list of available display.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ std::string sDisplayType;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'l': list = true; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+
+ default:
+ usage(argv[0], optarg_); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+void draw(vpImage<vpRGBa> &I)
+{
+ vpImagePoint iP1, iP2;
+ unsigned int w,h;
+
+ iP1.set_i(20);
+ iP1.set_j(10);
+ iP2.set_i(20);
+ iP2.set_j(30);
+ vpDisplay::displayArrow (I, iP1, iP2, vpColor::green, 4, 2, 3);
+
+ iP1.set_i(20);
+ iP1.set_j(60);
+ vpDisplay::displayText (I, iP1, "Test...", vpColor::black);
+
+ iP1.set_i(80);
+ iP1.set_j(220);
+ iP2.set_i(80);
+ iP2.set_j(480);
+ vpDisplay::displayCircle (I, iP1, 30, vpColor::red, false, 3);
+ vpDisplay::displayCircle (I, iP2, 30, vpColor::red, true, 3);
+
+ iP1.set_i(20);
+ iP1.set_j(220);
+ vpDisplay::displayCross (I, iP1, 5, vpColor::blue, 1);
+
+ iP1.set_i(140);
+ iP1.set_j(10);
+ iP2.set_i(140);
+ iP2.set_j(50);
+ vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+
+ iP1.set_i(120);
+ iP1.set_j(180);
+ iP2.set_i(160);
+ iP2.set_j(250);
+ vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+
+ iP1.set_i(160);
+ iP1.set_j(280);
+ iP2.set_i(120);
+ iP2.set_j(340);
+ vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+
+ //static void displayFrame (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, vpColor color)
+
+ iP1.set_i(140);
+ iP1.set_j(80);
+ iP2.set_i(140);
+ iP2.set_j(150);
+ vpDisplay::displayLine (I, iP1, iP2, vpColor::orange, 3);
+
+ iP1.set_i(140);
+ iP1.set_j(400);
+ vpDisplay::displayPoint (I, iP1, vpColor::red);
+
+ iP1.set_i(350);
+ iP1.set_j(20);
+ w = 60;
+ h = 50;
+ vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, false, 3);
+
+ iP1.set_i(350);
+ iP1.set_j(110);
+ vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, true, 3);
+
+ iP1.set_i(350);
+ iP1.set_j(200);
+ iP2.set_i(400);
+ iP2.set_j(260);
+ vpDisplay::displayRectangle (I, iP1, iP2, vpColor::orange, false, 3);
+
+ iP1.set_i(350);
+ iP1.set_j(290);
+ iP2.set_i(400);
+ iP2.set_j(350);
+ vpRect rectangle(iP1, iP2);
+ vpDisplay::displayRectangle (I, rectangle, vpColor::yellow, false, 3);
+
+ iP1.set_i(380);
+ iP1.set_j(400);
+ vpDisplay::displayRectangle (I, iP1, 45, w, h, vpColor::green, 3);
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try{
+ bool opt_list = false; // To print the list of video devices
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_list,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Print the list of video-devices available
+ if (opt_list) {
+ unsigned nbDevices = 0;
+ std::cout << "List of video-devices available: \n";
+#if defined VISP_HAVE_GTK
+ std::cout << " GTK\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_X11
+ std::cout << " X11\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_GDI
+ std::cout << " GDI\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_D3D9
+ std::cout << " D3D\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_OPENCV
+ std::cout << " OpenCV\n";
+ nbDevices ++;
+#endif
+ if (!nbDevices) {
+ std::cout << " No display is available\n";
+ }
+ return (0);
+ }
+
+ // Create a grey level image
+ vpImage<unsigned char> I;
+ // Create a color image
+ vpImage<vpRGBa> Irgba;
+
+ // Create a color image for each display.
+ vpImage<vpRGBa> Ix;
+ vpImage<vpRGBa> Igtk;
+ vpImage<vpRGBa> Icv;
+ vpImage<vpRGBa> Igdi;
+ vpImage<vpRGBa> Id3d;
+
+#if defined VISP_HAVE_X11
+ vpDisplayX *displayX = NULL;
+ displayX = new vpDisplayX;
+ Ix.init(480, 640, 255);
+ if (opt_display)
+ {
+ displayX->init(Ix, 100, 100,"Display X11") ;
+ vpDisplay::display(Ix) ;
+ draw(Ix);
+ vpDisplay::flush(Ix);
+ if (opt_click_allowed)
+ vpDisplay::getClick(Ix);
+ }
+#endif
+
+#if defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV *displayCv = NULL;
+ displayCv = new vpDisplayOpenCV;
+ Icv.init(480, 640, 255);
+ if (opt_display)
+ {
+ displayCv->init(Icv, 100, 100,"Display OpenCV") ;
+ vpDisplay::display(Icv) ;
+ draw(Icv);
+ vpDisplay::flush(Icv);
+ if (opt_click_allowed)
+ vpDisplay::getClick(Icv);
+ }
+#endif
+
+#if defined VISP_HAVE_GTK
+ vpDisplayGTK *displayGtk = NULL;
+ displayGtk = new vpDisplayGTK;
+ Igtk.init(480, 640, 255);
+ if (opt_display)
+ {
+ displayGtk->init(Igtk, 100, 100,"Display GTK") ;
+ vpDisplay::display(Igtk) ;
+ draw(Igtk);
+ vpDisplay::flush(Igtk);
+ if (opt_click_allowed)
+ vpDisplay::getClick(Igtk);
+ }
+#endif
+
+#if defined VISP_HAVE_GDI
+ vpDisplayGDI *displayGdi = NULL;
+ displayGdi = new vpDisplayGDI;
+ Igdi.init(480, 640, 255);
+ if (opt_display)
+ {
+ displayGdi->init(Igdi, 100, 100,"Display GDI") ;
+ vpDisplay::display(Igdi) ;
+ draw(Igdi);
+ vpDisplay::flush(Igdi);
+ if (opt_click_allowed)
+ vpDisplay::getClick(Igdi);
+ }
+#endif
+
+#if defined VISP_HAVE_D3D9
+ vpDisplayD3D *displayD3d = NULL;
+ displayD3d = new vpDisplayD3D;
+ Id3d.init(480, 640, 255);
+ if (opt_display)
+ {
+ displayD3d->init(Id3d, 100, 100,"Display Direct 3D") ;
+ vpDisplay::display(Id3d) ;
+ draw(Id3d);
+ vpDisplay::flush(Id3d);
+ if (opt_click_allowed)
+ vpDisplay::getClick(Id3d);
+ }
+#endif
+
+
+
+#if defined VISP_HAVE_X11
+ delete displayX;
+#endif
+
+#if defined VISP_HAVE_GTK
+ delete displayGtk;
+#endif
+
+#if defined(VISP_HAVE_OPENCV)
+ delete displayCv;
+#endif
+
+#if defined VISP_HAVE_GDI
+ delete displayGdi;
+#endif
+
+#if defined VISP_HAVE_D3D9
+ delete displayD3d;
+#endif
+ }
+ catch(...) {
+ vpERROR_TRACE("Error while displaying the image") ;
+ exit(-1);
+ }
+}
+
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have display functionalities...");
+}
+
+#endif
diff --git a/modules/gui/test/display/testMouseEvent.cpp b/modules/gui/test/display/testMouseEvent.cpp
new file mode 100644
index 0000000..9dda70c
--- /dev/null
+++ b/modules/gui/test/display/testMouseEvent.cpp
@@ -0,0 +1,589 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read an image sequence from the disk and display it.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+/*!
+ \file testMouseEvent.cpp
+
+ \brief Read an image sequence from the disk and display it.
+
+ The sequence is made of separate images. Each image corresponds to a
+ PGM file.
+
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sstream>
+#include <iomanip>
+
+#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/core/vpMouseButton.h>
+
+#include <visp3/core/vpTime.h>
+
+/*!
+ \example testMouseEvent.cpp
+
+ Read an image sequence from the disk and display it.
+
+ The sequence is made of separate images. Each image corresponds to a
+ PGM file.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:lp:ht:f:n:s:w"
+typedef enum {
+ vpX11,
+ vpGTK,
+ vpGDI,
+ vpD3D,
+} vpDisplayType;
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string ppath,
+ unsigned first, unsigned nimages, unsigned step, vpDisplayType &dtype);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath,
+ unsigned &first, unsigned &nimages, unsigned &step,
+ vpDisplayType &dtype, bool &list, bool &display, bool &click, bool &wait);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath : Input image path.
+ \param ppath : Personal image path.
+ \param first : First image.
+ \param nimages : Number of images to manipulate.
+ \param step : Step between two images.
+ \param dtype : Type of video device.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string ppath,
+ unsigned first, unsigned nimages, unsigned step, vpDisplayType &dtype)
+{
+ fprintf(stdout, "\n\
+Read an image sequence from the disk and display it.\n\
+The sequence is made of separate images. Each image corresponds\n\
+to a PGM file.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <test image path>] [-p <personal image path>]\n\
+ [-f <first image>] [-n <number of images>] [-s <step>] \n\
+ [-t <type of video device>] [-l] [-w] [-c] [-d] [-h]\n \
+ ", name);
+
+ std::string display;
+ switch(dtype) {
+ case vpX11: display = "X11"; break;
+ case vpGTK: display = "GTK"; break;
+ case vpGDI: display = "GDI"; break;
+ case vpD3D: display = "D3D"; break;
+ }
+
+ fprintf(stdout, "\n\
+ OPTIONS: Default\n\
+ -i <test image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ images. These images come from ViSP-images-x.y.z.tar.gz\n\
+ available on the ViSP website.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+ \n\
+ -p <personal image path> %s\n\
+ Specify a personal sequence containing images \n\
+ to process.\n\
+ By image sequence, we mean one file per image.\n\
+ The following image file formats PNM (PGM P5, PPM P6)\n\
+ are supported. The format is selected by analysing \n\
+ the filename extension.\n\
+ Example : \"/Temp/ViSP-images/cube/image.%%04d.pgm\"\n\
+ %%04d is for the image numbering.\n\
+ \n\
+ -f <first image> %u\n\
+ First image number of the sequence.\n\
+ \n\
+ -n <number of images> %u\n\
+ Number of images to load from the sequence.\n\
+ \n\
+ -s <step> %u\n\
+ Step between two images.\n\
+\n\
+ -t <type of video device> \"%s\"\n\
+ String specifying the video device to use.\n\
+ Possible values:\n\
+ \"X11\": only on UNIX platforms,\n\
+ \"GTK\": on all plaforms,\n\
+ \"GDI\": only on Windows platform (Graphics Device Interface),\n\
+ \"D3D\": only on Windows platform (Direct3D).\n\
+\n\
+ -l\n\
+ Print the list of video-devices available and exit.\n\
+\n\
+ -c\n\
+ Disable mouse click.\n\
+\n\
+ -d\n\
+ Disable the image display. This can be useful \n\
+ for automatic tests using crontab under Unix or \n\
+ using the task manager under Windows.\n\
+\n\
+ -w\n\
+ Wait for a mouse click between two images.\n\
+ If the image display is disabled (using -d)\n\
+ this option is without effect.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(),ppath.c_str(), first, nimages, step, display.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath : Input image path.
+ \param ppath : Personal image path.
+ \param first : First image.
+ \param nimages : Number of images to display.
+ \param step : Step between two images.
+ \param dtype : Type of video device.
+ \param list : Set as true,list the available video-devices.
+ \param click : Set as true, activates the mouse click.
+ \param display : Set as true, activates the image display. This is
+ the default configuration. When set to false, the display is
+ disabled. This can be useful for automatic tests using crontab
+ under Unix or using the task manager under Windows.
+
+ \param wait : Boolean for waiting a mouse click between two images.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath,
+ unsigned &first, unsigned &nimages, unsigned &step,
+ vpDisplayType &dtype, bool &list, bool &display, bool &click, bool &wait)
+{
+ const char *optarg_;
+ int c;
+ std::string sDisplayType;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click = false; break;
+ case 'd': display = false; break;
+ case 't': sDisplayType = optarg_;
+ // Parse the display type option
+ if (sDisplayType.compare("X11") == 0) {
+ dtype = vpX11;
+ }
+ else if (sDisplayType.compare("GTK") == 0) {
+ dtype = vpGTK;
+ }
+ else if (sDisplayType.compare("GDI") == 0) {
+ dtype = vpGDI;
+ }
+ else if (sDisplayType.compare("D3D") == 0) {
+ dtype = vpD3D;
+ }
+
+ break;
+ case 'i': ipath = optarg_; break;
+ case 'l': list = true; break;
+ case 'p': ppath = optarg_; break;
+ case 'f': first = (unsigned) atoi(optarg_); break;
+ case 'n': nimages = (unsigned) atoi(optarg_); break;
+ case 's': step = (unsigned) atoi(optarg_); break;
+ case 'w': wait = true; break;
+ case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
+ return false; break;
+
+ default:
+ usage(argv[0], optarg_, ipath, ppath, first, nimages, step, dtype);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string opt_ppath;
+ std::string dirname;
+ std::string filename;
+ unsigned opt_first = 30;
+ unsigned opt_nimages = 10;
+ unsigned opt_step = 1;
+ vpDisplayType opt_dtype; // Type of display to use
+ bool opt_list = false; // To print the list of video devices
+ bool opt_display = true;
+ bool opt_click = true;
+ bool opt_click_blocking = false;
+
+ // Default display is one available
+#if defined VISP_HAVE_GTK
+ opt_dtype = vpGTK;
+#elif defined VISP_HAVE_X11
+ opt_dtype = vpX11;
+#elif defined VISP_HAVE_GDI
+ opt_dtype = vpGDI;
+#elif defined VISP_HAVE_D3D9
+ opt_dtype = vpD3D;
+#endif
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
+ opt_step, opt_dtype, opt_list, opt_display, opt_click,
+ opt_click_blocking) == false) {
+ exit (-1);
+ }
+ // Print the list of video-devices available
+ if (opt_list) {
+ unsigned nbDevices = 0;
+ std::cout << "List of video-devices available: \n";
+#if defined VISP_HAVE_GTK
+ std::cout << " GTK (use \"-t GTK\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_X11
+ std::cout << " X11 (use \"-t X11\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_GDI
+ std::cout << " GDI (use \"-t GDI\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_D3D9
+ std::cout << " D3D (use \"-t D3D\" option to use it)\n";
+ nbDevices ++;
+#endif
+ if (!nbDevices) {
+ std::cout << " No display is available\n";
+ }
+ return (0);
+ }
+
+ if ( ! opt_display )
+ opt_click_blocking = false; // turn off the waiting
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step,opt_dtype);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << " Use -p <personal image path> option if you want to "<<std::endl
+ << " use personal images." << std::endl
+ << std::endl;
+
+ exit(-1);
+ }
+
+ // 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<unsigned char> I ;
+
+ unsigned iter = opt_first;
+ std::ostringstream s;
+ char cfilename[FILENAME_MAX];
+
+ if (opt_ppath.empty()){
+
+
+ // Warning :
+ // the image sequence is not provided with the ViSP package
+ // therefore the program will return you an error :
+ // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
+ // ViSP-images/cube/image.0001.pgm
+ // !! vpDotExample.cpp: main(#95) :Error while reading the image
+ // terminate called after throwing an instance of 'vpImageException'
+ //
+ // The sequence is available on the visp www site
+ // http://www.irisa.fr/lagadic/visp/visp.html
+ // in the download section. It is named "ViSP-images.tar.gz"
+
+ // Set the path location of the image sequence
+ dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+
+ // Build the name of the image file
+
+ s.setf(std::ios::right, std::ios::adjustfield);
+ s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
+ filename = vpIoTools::createFilePath(dirname, s.str());
+ }
+ else {
+
+ sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ filename = cfilename;
+ }
+ // Read the PGM image named "filename" on the disk, and put the
+ // bitmap into the image structure I. I is initialized to the
+ // correct size
+ //
+ // exception readPGM may throw various exception if, for example,
+ // the file does not exist, or if the memory cannot be allocated
+ try{
+ vpImageIo::read(I, filename) ;
+ }
+ catch(...)
+ {
+ // an exception is throwned if an exception from readPGM has been catched
+ // here this will result in the end of the program
+ // Note that another error message has been printed from readPGM
+ // to give more information about the error
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot read " << filename << std::endl;
+ std::cerr << " Check your -i " << ipath << " option, " << std::endl
+ << " or your -p " << opt_ppath << " option " <<std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable"
+ << std::endl;
+ exit(-1);
+ }
+ // Create a display for the image
+ vpDisplay *display = NULL;
+
+ switch(opt_dtype) {
+ case vpX11:
+ std::cout << "Requested X11 display functionnalities..." << std::endl;
+#if defined VISP_HAVE_X11
+ display = new vpDisplayX;
+#else
+ std::cout << " Sorry, X11 video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGTK:
+ std::cout << "Requested GTK display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GTK
+ display = new vpDisplayGTK;
+#else
+ std::cout << " Sorry, GTK video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGDI:
+ std::cout << "Requested GDI display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GDI
+ display = new vpDisplayGDI;
+#else
+ std::cout << " Sorry, GDI video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpD3D:
+ std::cout << "Requested D3D display functionnalities..." << std::endl;
+#if defined VISP_HAVE_D3D9
+ display = new vpDisplayD3D;
+#else
+ std::cout << " Sorry, D3D video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ }
+
+ if (opt_display) {
+ try {
+ // We open a window using either X11 or GTK or GDI.
+ // Its size is automatically defined by the image (I) size
+ display->init(I, 100, 100,"Display...") ;
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error while displaying the image") ;
+ delete display;
+ exit(-1);
+ }
+ }
+
+ // double tms_1 = vpTime::measureTimeMs() ;
+ unsigned niter=0 ;
+ // this is the loop over the image sequence
+ while (iter < opt_first + opt_nimages*opt_step) {
+ try {
+ double tms = vpTime::measureTimeMs() ;
+
+ // set the new image name
+
+ if (opt_ppath.empty()){
+ s.str("");
+ s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
+ filename = vpIoTools::createFilePath(dirname, s.str());
+ }
+ else {
+ sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ filename = cfilename;
+ }
+
+ std::cout << "read : " << filename << std::endl;
+ // read the image
+ vpImageIo::read(I, filename);
+ if (opt_display) {
+ // Display the image
+ vpDisplay::display(I) ;
+ //Flush the display
+ vpDisplay::flush(I) ;
+
+ if (opt_click_blocking) {
+ std::cout << "A click in the image to continue..." << std::endl;
+ }
+ vpImagePoint ip;
+ vpMouseButton::vpMouseButtonType button;
+
+ if (opt_click) {
+ bool pressed = vpDisplay::getClick(I, ip, button, opt_click_blocking);
+ if (pressed) {
+ switch (button) {
+ case vpMouseButton::button1:
+ std::cout << "Left button was pressed." << std::endl;
+ break;
+ case vpMouseButton::button2:
+ std::cout << "Middle button was pressed." << std::endl;
+ break;
+ case vpMouseButton::button3:
+ std::cout << "Right button was pressed. Bye. " << std::endl;
+ delete display;
+ return 0; break;
+ case vpMouseButton::none: break;
+ }
+ }
+ }
+
+ vpTime::wait(tms, 1000);
+ }
+
+ else {
+ // Synchronise the loop to 40 ms
+ vpTime::wait(tms, 40) ;
+ }
+ niter++ ;
+ }
+ catch(...) {
+ delete display;
+ exit(-1) ;
+ }
+ iter += opt_step ;
+ }
+ delete display;
+ // double tms_2 = vpTime::measureTimeMs() ;
+ // double tms_total = tms_2 - tms_1 ;
+ // std::cout << "Total Time : "<< tms_total<<std::endl;
+
+}
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have X11 or GTK display functionalities...");
+}
+
+#endif
diff --git a/modules/gui/test/display/testVideoDevice.cpp b/modules/gui/test/display/testVideoDevice.cpp
new file mode 100644
index 0000000..4a2a63c
--- /dev/null
+++ b/modules/gui/test/display/testVideoDevice.cpp
@@ -0,0 +1,481 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image display.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <iostream>
+#include <string>
+#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+/*!
+ \example testVideoDevice.cpp
+
+ \brief Image display manipulations.
+
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "i:hlt:dc"
+
+typedef enum {
+ vpX11,
+ vpGTK,
+ vpGDI,
+ vpD3D,
+ vpCV
+} vpDisplayType;
+
+void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
+bool getOptions(int argc, const char **argv,
+ std::string &ipath, vpDisplayType &dtype, bool &list,
+ bool &click_allowed, bool &display );
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param dtype : Type of video device.
+
+ */
+void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype)
+{
+ fprintf(stdout, "\n\
+Test video devices or display.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] \n\
+ [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
+", name);
+
+ std::string display;
+ switch(dtype) {
+ case vpX11: display = "X11"; break;
+ case vpGTK: display = "GTK"; break;
+ case vpGDI: display = "GDI"; break;
+ case vpD3D: display = "D3D"; break;
+ case vpCV: display = "CV"; break;
+ }
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -t <type of video device> \"%s\"\n\
+ String specifying the video device to use.\n\
+ Possible values:\n\
+ \"X11\": only on UNIX platforms,\n\
+ \"GTK\": on all plaforms,\n\
+ \"GDI\": only on Windows platform (Graphics Device Interface),\n\
+ \"D3D\": only on Windows platform (Direct3D).\n\
+ \"CV\" : (OpenCV).\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -l\n\
+ Print the list of video-devices available and exit.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ ipath.c_str(), display.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param dtype : Type of display.
+ \param list : To get the list of available display.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv,
+ std::string &ipath, vpDisplayType &dtype, bool &list,
+ bool &click_allowed, bool &display )
+{
+ const char *optarg_;
+ int c;
+ std::string sDisplayType;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i': ipath = optarg_; break;
+ case 'l': list = true; break;
+ case 't': sDisplayType = optarg_;
+ // Parse the display type option
+ if (sDisplayType.compare("X11") == 0) {
+ dtype = vpX11;
+ }
+ else if (sDisplayType.compare("GTK") == 0) {
+ dtype = vpGTK;
+ }
+ else if (sDisplayType.compare("GDI") == 0) {
+ dtype = vpGDI;
+ }
+ else if (sDisplayType.compare("D3D") == 0) {
+ dtype = vpD3D;
+ }
+ else if (sDisplayType.compare("CV") == 0) {
+ dtype = vpCV;
+ }
+
+ break;
+ case 'h': usage(argv[0], NULL, ipath,dtype); return false; break;
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+
+ default:
+ usage(argv[0], optarg_, ipath,dtype); return false; break;
+ }
+ }
+
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, dtype);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int
+main(int argc, const char ** argv)
+{
+ try{
+ std::string env_ipath;
+ std::string opt_ipath;
+ bool opt_list = false; // To print the list of video devices
+ vpDisplayType opt_dtype; // Type of display to use
+ std::string ipath;
+ std::string filename;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Default display is one available
+#if defined VISP_HAVE_GTK
+ opt_dtype = vpGTK;
+#elif defined VISP_HAVE_X11
+ opt_dtype = vpX11;
+#elif defined VISP_HAVE_GDI
+ opt_dtype = vpGDI;
+#elif defined VISP_HAVE_D3D9
+ opt_dtype = vpD3D;
+#elif defined VISP_HAVE_OPENCV
+ opt_dtype = vpCV;
+#endif
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Print the list of video-devices available
+ if (opt_list) {
+ unsigned nbDevices = 0;
+ std::cout << "List of video-devices available: \n";
+#if defined VISP_HAVE_GTK
+ std::cout << " GTK (use \"-t GTK\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_X11
+ std::cout << " X11 (use \"-t X11\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_GDI
+ std::cout << " GDI (use \"-t GDI\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_D3D9
+ std::cout << " D3D (use \"-t D3D\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_OPENCV
+ std::cout << " CV (use \"-t CV\" option to use it)\n";
+ nbDevices ++;
+#endif
+ if (!nbDevices) {
+ std::cout << " No display is available\n";
+ }
+ return (0);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath, opt_dtype);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+ // Create a grey level image
+ vpImage<unsigned char> I ;
+ // Create a color image
+ vpImage<vpRGBa> Irgba ;
+
+ // Load a grey image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(I, filename) ;
+
+ // Load a color image from the disk
+ filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Irgba, filename) ;
+
+
+ // Create a display for the image
+ vpDisplay *display = NULL;
+
+ switch(opt_dtype) {
+ case vpX11:
+ std::cout << "Requested X11 display functionnalities..." << std::endl;
+#if defined VISP_HAVE_X11
+ display = new vpDisplayX;
+#else
+ std::cout << " Sorry, X11 video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGTK:
+ std::cout << "Requested GTK display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GTK
+ display = new vpDisplayGTK;
+#else
+ std::cout << " Sorry, GTK video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGDI:
+ std::cout << "Requested GDI display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GDI
+ display = new vpDisplayGDI;
+#else
+ std::cout << " Sorry, GDI video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpD3D:
+ std::cout << "Requested D3D display functionnalities..." << std::endl;
+#if defined VISP_HAVE_D3D9
+ display = new vpDisplayD3D;
+#else
+ std::cout << " Sorry, D3D video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpCV:
+ std::cout << "Requested OpenCV display functionnalities..." << std::endl;
+#if defined(VISP_HAVE_OPENCV)
+ display = new vpDisplayOpenCV;
+#else
+ std::cout << " Sorry, OpenCV video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ }
+ if (opt_display){
+
+ // We open a window using either X11 or GTK or GDI or D3D.
+ // Its size is automatically defined by the image (I) size
+ display->init(I, 100, 100,"Display...") ;
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(I) ;
+ //Flush the display
+ vpDisplay::flush(I) ;
+ std::cout << "A click to continue...\n";
+ if ( opt_click_allowed )
+ vpDisplay::getClick(I) ;
+
+ display->close(I);
+
+ // We open a window using either X11 or GTK or GDI or D3D
+ // but using anothe function who doesn't take title.
+ // Its size is automatically defined by the image (I) size
+
+ display->init(I, 100, 100);
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(I) ;
+ //Flush the display
+ vpDisplay::flush(I) ;
+ std::cout << "A click to continue...\n";
+ if ( opt_click_allowed )
+ vpDisplay::getClick(I) ;
+
+ display->close(I);
+
+ // We open a window using either X11 or GTK or GDI or D3D.
+ // Its size is automatically defined by the image (I) size
+
+ display->init(Irgba, 100, 100,"Color display...");
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(Irgba) ;
+ //Flush the display
+ vpDisplay::flush(Irgba) ;
+
+ std::cout << "A click to continue...\n";
+ if ( opt_click_allowed )
+ vpDisplay::getClick(Irgba) ;
+
+ display->close(Irgba);
+
+ // We open a window using either X11 or GTK or GDI or D3D
+ // but using anothe function who doesn't take title.
+ // Its size is automatically defined by the image (I) size
+
+ display->init(Irgba, 100, 100);
+
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(Irgba) ;
+ //Flush the display
+ vpDisplay::flush(Irgba) ;
+
+ std::cout << "A click to exit...\n";
+ if ( opt_click_allowed )
+ vpDisplay::getClick(Irgba) ;
+ }
+ delete display;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error while displaying the image") ;
+ exit(-1);
+ }
+}
+
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have display functionalities...");
+}
+
+#endif
+
diff --git a/modules/gui/test/display/testVideoDeviceDual.cpp b/modules/gui/test/display/testVideoDeviceDual.cpp
new file mode 100644
index 0000000..24cab4e
--- /dev/null
+++ b/modules/gui/test/display/testVideoDeviceDual.cpp
@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for image display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/io/vpParseArgv.h>
+#include <stdlib.h>
+#include <iostream>
+#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+
+/*!
+ \example testVideoDeviceDual.cpp
+
+ \brief Example showing how to open two video devices.
+
+*/
+
+
+// List of allowed command line options
+#define GETOPTARGS "hlt:dc"
+
+typedef enum {
+ vpX11,
+ vpGTK,
+ vpGDI,
+ vpD3D,
+ vpCV
+} vpDisplayType;
+
+void usage(const char *name, const char *badparam, vpDisplayType &dtype);
+bool getOptions(int argc, const char **argv, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param dtype : Type of video device.
+
+ */
+void usage(const char *name, const char *badparam, vpDisplayType &dtype)
+{
+ fprintf(stdout, "\n\
+Test to open video devices or display.\n\
+\n\
+SYNOPSIS\n\
+ %s [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
+", name);
+
+ std::string display;
+ switch(dtype) {
+ case vpX11: display = "X11"; break;
+ case vpGTK: display = "GTK"; break;
+ case vpGDI: display = "GDI"; break;
+ case vpD3D: display = "D3D"; break;
+ case vpCV: display = "CV"; break;
+ }
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -t <type of video device> \"%s\"\n\
+ String specifying the video device to use.\n\
+ Possible values:\n\
+ \"X11\": only on UNIX platforms,\n\
+ \"GTK\": on all plaforms,\n\
+ \"GDI\": only on Windows platform (Graphics Device Interface),\n\
+ \"D3D\": only on Windows platform (Direct3D).\n\
+ \"CV\" : (OpenCV).\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -l\n\
+ Print the list of video-devices available and exit.\n\
+\n\
+ -h\n\
+ Print the help.\n\n",
+ display.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param dtype : Type of display.
+ \param list : To get the list of available display.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv,
+ vpDisplayType &dtype, bool &list,
+ bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ std::string sDisplayType;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'l': list = true; break;
+ case 't': sDisplayType = optarg_;
+ // Parse the display type option
+ if (sDisplayType.compare("X11") == 0) {
+ dtype = vpX11;
+ }
+ else if (sDisplayType.compare("GTK") == 0) {
+ dtype = vpGTK;
+ }
+ else if (sDisplayType.compare("GDI") == 0) {
+ dtype = vpGDI;
+ }
+ else if (sDisplayType.compare("D3D") == 0) {
+ dtype = vpD3D;
+ }
+ else if (sDisplayType.compare("CV") == 0) {
+ dtype = vpCV;
+ }
+
+ break;
+ case 'h': usage(argv[0], NULL, dtype); return false; break;
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+
+ default:
+ usage(argv[0], optarg_, dtype); return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, dtype);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char ** argv)
+{
+ try {
+ bool opt_list = false; // To print the list of video devices
+ vpDisplayType opt_dtype; // Type of display to use
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Default display is one available
+#if defined VISP_HAVE_GTK
+ opt_dtype = vpGTK;
+#elif defined VISP_HAVE_X11
+ opt_dtype = vpX11;
+#elif defined VISP_HAVE_GDI
+ opt_dtype = vpGDI;
+#elif defined VISP_HAVE_D3D9
+ opt_dtype = vpD3D;
+#elif defined VISP_HAVE_OPENCV
+ opt_dtype = vpCV;
+#endif
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_dtype, opt_list,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Print the list of video-devices available
+ if (opt_list) {
+ unsigned nbDevices = 0;
+ std::cout << "List of video-devices available: \n";
+#if defined VISP_HAVE_GTK
+ std::cout << " GTK (use \"-t GTK\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_X11
+ std::cout << " X11 (use \"-t X11\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_GDI
+ std::cout << " GDI (use \"-t GDI\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_D3D9
+ std::cout << " D3D (use \"-t D3D\" option to use it)\n";
+ nbDevices ++;
+#endif
+#if defined VISP_HAVE_OPENCV
+ std::cout << " CV (use \"-t CV\" option to use it)\n";
+ nbDevices ++;
+#endif
+ if (!nbDevices) {
+ std::cout << " No display is available\n";
+ }
+ return (0);
+ }
+
+ // Create 2 images
+ vpImage<unsigned char> I1(240, 320), I2(240, 320);
+ I1 = 128;
+ I2 = 255;
+
+ // Create 2 display
+ vpDisplay *d1 = NULL, *d2 = NULL;
+
+ // Initialize the displays
+ switch(opt_dtype) {
+ case vpX11:
+ std::cout << "Requested X11 display functionnalities..." << std::endl;
+#if defined VISP_HAVE_X11
+ d1 = new vpDisplayX;
+ d2 = new vpDisplayX;
+#else
+ std::cout << " Sorry, X11 video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGTK:
+ std::cout << "Requested GTK display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GTK
+ d1 = new vpDisplayGTK;
+ d2 = new vpDisplayGTK;
+#else
+ std::cout << " Sorry, GTK video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpGDI:
+ std::cout << "Requested GDI display functionnalities..." << std::endl;
+#if defined VISP_HAVE_GDI
+ d1 = new vpDisplayGDI;
+ d2 = new vpDisplayGDI;
+#else
+ std::cout << " Sorry, GDI video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpD3D:
+ std::cout << "Requested D3D display functionnalities..." << std::endl;
+#if defined VISP_HAVE_D3D9
+ d1 = new vpDisplayD3D;
+ d2 = new vpDisplayD3D;
+#else
+ std::cout << " Sorry, D3D video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ case vpCV:
+ std::cout << "Requested OpenCV display functionnalities..." << std::endl;
+#if defined(VISP_HAVE_OPENCV)
+ d1 = new vpDisplayOpenCV;
+ d2 = new vpDisplayOpenCV;
+#else
+ std::cout << " Sorry, OpenCV video device is not available.\n";
+ std::cout << "Use \"" << argv[0]
+ << " -l\" to print the list of available devices.\n";
+ return 0;
+#endif
+ break;
+ }
+
+ if (opt_display){
+ int winx1 = 100, winy1 = 200;
+ d1->init(I1, winx1, winy1, "Display 1");
+
+ int winx2 = winx1+10+(int)I1.getWidth(), winy2 = winy1;
+ d2->init(I2, winx2, winy2, "Display 2");
+
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ }
+
+ std::cout << "A click in display 1 to exit..." << std::endl;
+ if ( opt_click_allowed )
+ vpDisplay::getClick(I1);
+
+ delete d1;
+ delete d2;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have display functionalities...");
+}
+
+#endif
diff --git a/modules/io/CMakeLists.txt b/modules/io/CMakeLists.txt
new file mode 100644
index 0000000..3b47ac6
--- /dev/null
+++ b/modules/io/CMakeLists.txt
@@ -0,0 +1,68 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+# Image i/o: jpeg, png, OpenCV (already included by core)
+if(USE_JPEG)
+ list(APPEND opt_incs ${JPEG_INCLUDE_DIR})
+ list(APPEND opt_libs ${JPEG_LIBRARIES})
+ # If Qt3 and libjpeg are used, we add here QT_CLEAN_NAMESPACE define to
+ # handle a compilation error "conflicting declaration 'typedef long int INT32'
+ if(VISP_HAVE_QT3_FOUND)
+ add_definitions("-DQT_CLEAN_NAMESPACE")
+ endif()
+endif()
+if(USE_PNG)
+ list(APPEND opt_incs ${PNG_INCLUDE_DIR})
+ list(APPEND opt_libs ${PNG_LIBRARIES})
+ add_definitions(${PNG_DEFINITIONS})
+endif()
+
+# videos: ffmpeg
+if(USE_FFMPEG)
+ list(APPEND opt_incs ${FFMPEG_INCLUDE_DIRS})
+ list(APPEND opt_libs ${FFMPEG_LIBRARIES})
+endif()
+
+vp_add_module(io visp_core)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+vp_add_tests(DEPENDS_ON visp_features)
diff --git a/modules/io/include/visp3/io/vpDiskGrabber.h b/modules/io/include/visp3/io/vpDiskGrabber.h
new file mode 100644
index 0000000..62faa10
--- /dev/null
+++ b/modules/io/include/visp3/io/vpDiskGrabber.h
@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Disk framegrabber.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpDiskGrabber.h
+ \brief Class to load image sequence from the disk.
+*/
+#ifndef vpDiskGrabber_hh
+#define vpDiskGrabber_hh
+
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpDebug.h>
+
+/*!
+ \class vpDiskGrabber
+
+ \ingroup group_io_video
+
+ \brief Class to grab (ie. read) images from the disk.
+
+ Defined a virtual video device. "Grab" the images from the disk.
+ Derived from the vpFrameGrabber class.
+
+ \sa vpFrameGrabber
+
+ Here an example of capture from the directory
+ "/local/soft/ViSP/ViSP-images/cube". We want to acquire 10 images
+ from the first named "image.0001.pgm" by steps of 2.
+
+\code
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpDiskGrabber.h>
+
+int main(){
+ vpImage<unsigned char> I; // Grey level image
+
+ // Declare a framegrabber able to read a sequence of successive
+ // images from the disk
+ vpDiskGrabber g;
+
+ // Set the path to the directory containing the sequence
+ g.setDirectory("/local/soft/ViSP/ViSP-images/cube");
+ // Set the image base name. The directory and the base name constitute
+ // the constant part of the full filename
+ g.setBaseName("image.");
+ // Set the step between two images of the sequence
+ g.setStep(2);
+ // Set the number of digits to build the image number
+ g.setNumberOfZero(4);
+ // Set the first frame number of the sequence
+ g.setImageNumber(1);
+ // Set the image file extension
+ g.setExtension("pgm");
+
+ // Open the framegrabber by loading the first image of the sequence
+ g.open(I) ;
+
+ unsigned int cpt = 1;
+ // this is the loop over the image sequence
+ while(cpt ++ < 10)
+ {
+ // read the image and then increment the image counter so that the next
+ // call to acquire(I) will get the next image
+ g.acquire(I) ;
+ }
+}
+\endcode
+*/
+class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber
+{
+private:
+ long image_number ; //!< id of the next image to be read
+ int image_step ; //!< increment between two image id
+ unsigned int number_of_zero ; //!< number of zero in the image name (image.00000.pgm)
+
+ char directory[FILENAME_MAX] ; //!< image location
+ char base_name[FILENAME_MAX] ; //!< image base name
+ char extension[FILENAME_MAX] ; //!< image extension
+
+ bool useGenericName;
+ char genericName[FILENAME_MAX];
+
+public:
+ vpDiskGrabber();
+ vpDiskGrabber(const char *genericName);
+ vpDiskGrabber(const char *dir, const char *basename,
+ long number, int step, unsigned int noz,
+ const char *ext) ;
+ virtual ~vpDiskGrabber() ;
+
+ void open(vpImage<unsigned char> &I) ;
+ void open(vpImage<vpRGBa> &I) ;
+ void open(vpImage<float> &I) ;
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+ void acquire(vpImage<float> &I) ;
+ void acquire(vpImage<unsigned char> &I, long image_number);
+ void acquire(vpImage<vpRGBa> &I, long image_number);
+ void acquire(vpImage<float> &I, long image_number) ;
+
+ void close();
+
+ void setDirectory(const char *dir);
+ void setBaseName(const char *name);
+ void setImageNumber(long number) ;
+ void setStep(int a);
+ void setNumberOfZero(unsigned int noz);
+ void setExtension(const char *ext);
+ void setGenericName(const char *genericName);
+
+ /*!
+ Return the current image number.
+ */
+ long getImageNumber() { return image_number; };
+} ;
+
+#endif
+
diff --git a/modules/io/include/visp3/io/vpFFMPEG.h b/modules/io/include/visp3/io/vpFFMPEG.h
new file mode 100644
index 0000000..f3a4aa1
--- /dev/null
+++ b/modules/io/include/visp3/io/vpFFMPEG.h
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class that manages the FFMPEG library.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpFFMPEG.h
+ \brief Class that manages the FFMPEG library
+*/
+
+#ifndef vpFFMPEG_H
+#define vpFFMPEG_H
+
+#include <visp3/io/vpImageIo.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+
+#ifdef VISP_HAVE_FFMPEG
+
+// Fix for the following compilation error:
+// libavutil/common.h:154: error: UINT64_C was not declared in this scope
+// libavutil/common.h is no more autosufficient for C++ program because
+// stdint.h defines UINT64_C only for C program and not C++ program
+#ifdef __cplusplus
+# ifndef __STDC_CONSTANT_MACROS
+# define __STDC_CONSTANT_MACROS
+# endif
+# ifdef _STDINT_H
+# undef _STDINT_H
+# endif
+// On OS X
+# ifdef _STDINT_H_
+# undef _STDINT_H_
+# endif
+# include <stdint.h>
+
+# ifndef INT64_C
+# define INT64_C(c) (c ## LL)
+# endif
+# ifndef UINT64_C
+# define UINT64_C(c) (c ## ULL)
+# endif
+#endif
+// end fix
+
+
+extern "C"
+{
+#include <libavcodec/avcodec.h> // requested for CodecID enum
+//#include <avformat.h>
+//#include <swscale.h>
+}
+
+struct AVFormatContext;
+struct AVCodecContext;
+struct AVCodec;
+struct AVFrame;
+struct AVFrame;
+struct AVFrame;
+struct AVPacket;
+struct SwsContext;
+
+/*!
+ \class vpFFMPEG
+ \ingroup group_io_video
+
+ \brief This class interfaces the FFmpeg library to enable video stream reading or writing.
+
+ Here an example which explains how to use the class to read a video stream.
+ \code
+#include <visp3/io/vpFFMPEG.h>
+
+int main ()
+{
+#ifdef VISP_HAVE_FFMPEG
+ vpImage<vpRGBa> I; // The image to stores the frames
+ vpFFMPEG ffmpeg;
+ // Initialization
+ ffmpeg.openStream("video.mpeg", vpFFMPEG::COLORED);
+ ffmpeg.initStream();
+ std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
+ std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
+ // Video reading
+ int frameIndex = 0;
+ ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
+#endif
+}
+ \endcode
+
+ If you want to open the video as a gray scaled video, you can use the following example.
+ \code
+#include <visp3/io/vpFFMPEG.h>
+
+int main ()
+{
+#ifdef VISP_HAVE_FFMPEG
+ vpImage<unsigned char> I; // The image to stores the frames
+ vpFFMPEG ffmpeg;
+ // Initialization
+ ffmpeg.openStream("video.mpeg", vpFFMPEG::GRAY_SCALED);
+ ffmpeg.initStream();
+ std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
+ std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
+ // Video reading
+ int frameIndex = 0;
+ ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
+#endif
+}
+ \endcode
+*/
+class VISP_EXPORT vpFFMPEG
+{
+ public:
+ typedef enum
+ {
+ COLORED,
+ GRAY_SCALED,
+ }vpFFMPEGColorType;
+
+ private:
+ //! Video's height and width
+ int width, height;
+ //! Number of frame in the video.
+ unsigned long frameNumber;
+ //! FFMPEG variables
+ AVFormatContext *pFormatCtx;
+ AVCodecContext *pCodecCtx;
+ AVCodec *pCodec;
+ AVFrame *pFrame;
+ AVFrame *pFrameRGB;
+ AVFrame *pFrameGRAY;
+ AVPacket *packet;
+ SwsContext *img_convert_ctx ;
+ unsigned int videoStream;
+ int numBytes ;
+ uint8_t * buffer ;
+ std::vector<int64_t> index;
+ //! Indicates if the openStream method was executed
+ bool streamWasOpen;
+ //! Indicates if the initStream method was executed
+ bool streamWasInitialized;
+ //! Indicates the video's color output.
+ vpFFMPEGColorType color_type;
+
+ //!The file which is the video file
+ FILE *f;
+ //!Buffers
+ uint8_t *outbuf, *picture_buf;
+ //!Buffer size
+ int outbuf_size;
+ //!Size of the data to write in the file
+ int out_size;
+ //!Bit rate of the video to write
+ unsigned int bit_rate;
+ //!Indicates if the openEncoder method was executed
+ bool encoderWasOpened;
+ double framerate_stream; // input stream
+ int framerate_encoder; // output stream
+
+ public:
+ vpFFMPEG();
+ ~vpFFMPEG();
+
+ bool acquire(vpImage<vpRGBa> &I);
+ bool acquire(vpImage<unsigned char> &I);
+
+ void closeStream();
+
+ bool endWrite();
+
+ bool getFrame(vpImage<vpRGBa> &I, unsigned int frameNumber);
+ bool getFrame(vpImage<unsigned char> &I, unsigned int frameNumber);
+ /*!
+ Gets the video's frame number.
+
+ \return The value of the video's frame number.
+ */
+ inline unsigned long getFrameNumber() const {return frameNumber;}
+ /*!
+ Return the framerate used to encode the video. The video stream need
+ to be opened before calling this function.
+
+ \exception vpException::ioError : If the video stream was not opened priviously.
+ */
+ inline double getFramerate() const {
+ if (streamWasOpen)
+ return framerate_stream;
+ throw vpException(vpException::ioError, "Video stream not opened.");
+ }
+ /*!
+ Gets the video's height.
+
+ \return The value of the video's height.
+ */
+ inline int getHeight() const {return height;}
+ /*!
+ Gets the video's width.
+
+ \return The value of the video's width.
+ */
+ inline int getWidth() const {return width;}
+
+ bool initStream();
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ bool openEncoder(const char *filename, unsigned int width, unsigned int height, CodecID codec = CODEC_ID_MPEG1VIDEO);
+#else
+ bool openEncoder(const char *filename, unsigned int width, unsigned int height, AVCodecID codec = AV_CODEC_ID_MPEG1VIDEO);
+#endif
+ bool openStream(const char *filename,vpFFMPEGColorType color_type);
+
+ bool saveFrame(vpImage<vpRGBa> &I);
+ bool saveFrame(vpImage<unsigned char> &I);
+ /*!
+ Sets the bit rate of the video when encoding.
+
+ \param rate : the expected bit rate.
+ */
+ inline void setBitRate(const unsigned int rate) {this->bit_rate = rate;}
+ /*!
+ Sets the framerate of the video when encoding.
+
+ \param framerate : the expected framerate.
+ */
+ inline void setFramerate(const int framerate) {framerate_encoder = framerate;}
+
+ private:
+ void copyBitmap(vpImage<vpRGBa> &I);
+ void copyBitmap(vpImage<unsigned char> &I);
+ void writeBitmap(vpImage<vpRGBa> &I);
+ void writeBitmap(vpImage<unsigned char> &I);
+};
+#endif
+#endif
diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h
new file mode 100644
index 0000000..77e7437
--- /dev/null
+++ b/modules/io/include/visp3/io/vpImageIo.h
@@ -0,0 +1,252 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read/write images.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpImageIo.h
+ \brief Read/write images
+*/
+
+#ifndef vpIMAGEIO_H
+#define vpIMAGEIO_H
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpImageConvert.h>
+
+#include <stdio.h>
+#include <iostream>
+
+#if defined(_WIN32)
+# include <windows.h>
+#endif
+
+#if defined(VISP_HAVE_JPEG)
+#include <jpeglib.h>
+#include <jerror.h>
+#endif
+
+#if defined(VISP_HAVE_PNG)
+#include <png.h>
+#endif
+
+
+/*!
+ \class vpImageIo
+
+ \ingroup group_io_image
+
+ \brief Read/write images with various image format.
+
+ The code below shows how to convert an PPM P6 image file format into
+ a PGM P5 image file format. The extension of the filename is here
+ used in read() and write() functions to set the image file format
+ (".pgm" for PGM P5 and ".ppm" for PPM P6).
+
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+ vpImage<unsigned char> I;
+#if defined(_WIN32)
+ std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
+#else // UNIX
+ std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
+#endif
+
+ vpImageIo::read(I, filename); // Convert the color image in a gray level image
+ vpImageIo::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format
+}
+ \endcode
+
+ This other example available in tutorial-image-reader.cpp shows how to read/write
+ jpeg images. It supposes that \c libjpeg is installed.
+ \include tutorial-image-reader.cpp
+*/
+
+class VISP_EXPORT vpImageIo
+{
+
+private:
+
+ typedef enum
+ {
+ FORMAT_PGM,
+ FORMAT_PPM,
+ FORMAT_JPEG,
+ FORMAT_PNG,
+ // Formats supported by opencv
+ FORMAT_TIFF,
+ FORMAT_BMP,
+ FORMAT_DIB,
+ FORMAT_PBM,
+ FORMAT_RASTER,
+ FORMAT_JPEG2000,
+ FORMAT_UNKNOWN
+ } vpImageFormatType;
+
+ static const int vpMAX_LEN;
+
+ static FILE * openFileRead(const char *filename) ;
+ static FILE * openFileWrite(const char *filename, const char *mode="w") ;
+
+ static FILE * openFileRead(const std::string filename) ;
+ static FILE * openFileWrite(const std::string filename,
+ const std::string mode="w") ;
+
+ static vpImageFormatType getFormat(const char *filename) ;
+ static std::string getExtension(const std::string &filename);
+
+public:
+
+ static
+ void read(vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void read(vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void read(vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void read(vpImage<vpRGBa> &I, const std::string filename) ;
+
+ static
+ void write(const vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void write(const vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void write(const vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void write(const vpImage<vpRGBa> &I, const std::string filename) ;
+
+ static
+ void readPFM(vpImage<float> &I, const char *filename) ;
+
+
+ static
+ void readPGM(vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void readPGM(vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void readPGM(vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void readPGM(vpImage<vpRGBa> &I, const std::string filename) ;
+
+ static
+ void readPPM(vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void readPPM(vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void readPPM(vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void readPPM(vpImage<vpRGBa> &I, const std::string filename) ;
+
+#if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV))
+ static
+ void readJPEG(vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void readJPEG(vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void readJPEG(vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void readJPEG(vpImage<vpRGBa> &I, const std::string filename) ;
+#endif
+
+#if (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV))
+ static
+ void readPNG(vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void readPNG(vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void readPNG(vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void readPNG(vpImage<vpRGBa> &I, const std::string filename) ;
+#endif
+
+ static
+ void writePFM(const vpImage<float> &I, const char *filename) ;
+
+
+
+ static
+ void writePGM(const vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void writePGM(const vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void writePGM(const vpImage<short> &I, const char *filename) ;
+ static
+ void writePGM(const vpImage<short> &I, const std::string filename) ;
+ static
+ void writePGM(const vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void writePGM(const vpImage<vpRGBa> &I, const std::string filename) ;
+
+ static
+ void writePPM(const vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void writePPM(const vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void writePPM(const vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void writePPM(const vpImage<vpRGBa> &I, const std::string filename) ;
+
+#if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV))
+ static
+ void writeJPEG(const vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void writeJPEG(const vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void writeJPEG(const vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void writeJPEG(const vpImage<vpRGBa> &I, const std::string filename) ;
+#endif
+
+#if (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV))
+ static
+ void writePNG(const vpImage<unsigned char> &I, const char *filename) ;
+ static
+ void writePNG(const vpImage<unsigned char> &I, const std::string filename) ;
+ static
+ void writePNG(const vpImage<vpRGBa> &I, const char *filename) ;
+ static
+ void writePNG(const vpImage<vpRGBa> &I, const std::string filename) ;
+#endif
+
+ } ;
+#endif
+
diff --git a/modules/io/include/visp3/io/vpKeyboard.h b/modules/io/include/visp3/io/vpKeyboard.h
new file mode 100644
index 0000000..dcbb798
--- /dev/null
+++ b/modules/io/include/visp3/io/vpKeyboard.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Keybord management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpKeyboard_h
+#define vpKeyboard_h
+
+/*!
+ \file vpKeyboard.h
+ \brief Keybord management under unix.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+
+# include <iostream>
+# include <termios.h>
+# include <unistd.h>
+# include <stdlib.h>
+
+
+/*!
+
+ \class vpKeyboard
+ \ingroup group_io_keyboard
+ \brief Keybord management under unix (Linux or OSX). This class is not available under windows.
+
+ Gets a key from the keyboard without waiting for the "Enter" key.
+ \warning The key that was pressed is only detected if the terminal where the binary was launched is active.
+
+ \code
+ vpKeyboard keyboard; // Turn on keyboard raw mode
+ int c;
+ while (...) {
+ ...
+ if (keyboard.kbhit()) { // Detect if a key was pressed
+ c = keyboard.getchar (void); // Get the pressed key
+ if (c == 'q' || c == 'Q') {
+ break; // Quit the while()
+ }
+ ...
+ }
+ }
+
+ // Keyboard raw mode is turned off by the vpKeyboard destructor
+ \endcode
+
+*/
+class VISP_EXPORT vpKeyboard
+{
+
+public:
+ vpKeyboard();
+ ~vpKeyboard();
+ int kbhit();
+ int getchar();
+
+private:
+ void init();
+ void end();
+
+ void setRawMode(bool active);
+
+ struct termios initial_settings, new_settings;
+} ;
+
+#endif // defined UNIX
+
+#endif
diff --git a/modules/io/include/visp3/io/vpParallelPort.h b/modules/io/include/visp3/io/vpParallelPort.h
new file mode 100644
index 0000000..aee9838
--- /dev/null
+++ b/modules/io/include/visp3/io/vpParallelPort.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Parallel port management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpParallelPort_h
+#define vpParallelPort_h
+
+/*!
+ \file vpParallelPort.h
+ \brief Parallel port management under unix.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_PARPORT
+
+#include <iostream>
+# include <stdio.h>
+# include <errno.h>
+# include <linux/parport.h>
+# include <linux/ppdev.h>
+
+# include <visp3/io/vpParallelPortException.h>
+
+/*!
+
+ \class vpParallelPort
+ \ingroup group_io_parallel_port
+ \brief Parallel port management under unix.
+
+ The code below shows how to send a data over the parallel port.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/io/vpParallelPort.h>
+
+int main()
+{
+#ifdef VISP_HAVE_PARPORT
+ vpParallelPort parport;
+
+ unsigned char data = 5; // 0x00000101 = 5 in decimal
+ parport.sendData(data); // D0 and D2 are set to logical level 1
+#endif
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpParallelPort
+{
+
+public:
+ vpParallelPort();
+ ~vpParallelPort();
+
+ void sendData(unsigned char &data);
+ unsigned char getData();
+
+private:
+ void open();
+ void close();
+
+private:
+ int fd; // parallel port descriptor
+ char device[FILENAME_MAX];
+} ;
+
+#endif
+
+#endif
diff --git a/modules/io/include/visp3/io/vpParallelPortException.h b/modules/io/include/visp3/io/vpParallelPortException.h
new file mode 100644
index 0000000..3570060
--- /dev/null
+++ b/modules/io/include/visp3/io/vpParallelPortException.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpParallelPort class and its derivates.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpParallelPortException_H
+#define __vpParallelPortException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/*!
+
+ \file vpParallelPortException.h
+
+ \brief Error that can be emited by the vpParallelPort class and its
+ derivates.
+
+*/
+
+/* Classes standards. */
+
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpParallelPortException
+
+ \brief Error that can be emited by the vpParallelPort class and its
+ derivates.
+ */
+class VISP_EXPORT vpParallelPortException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible errors than can be emmited while calling
+ vpParallelPort member
+ */
+ enum error
+ {
+ opening, /*!< Cannot access to the parallel port device. */
+ closing /*!< Cannot close the parallel port device. */
+ } ;
+
+ public:
+ vpParallelPortException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpParallelPortException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpParallelPortException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif
diff --git a/modules/io/include/visp3/io/vpParseArgv.h b/modules/io/include/visp3/io/vpParseArgv.h
new file mode 100644
index 0000000..ac1c348
--- /dev/null
+++ b/modules/io/include/visp3/io/vpParseArgv.h
@@ -0,0 +1,194 @@
+/****************************************************************************
+ *
+ * Declarations for Tk-related things that are visible
+ * outside of the Tk module itself.
+ *
+ * Copyright 1989-1992 Regents of the University of California.
+ * Permission to use, copy, modify, and distribute this
+ * software and its documentation for any purpose and without
+ * fee is hereby granted, provided that the above copyright
+ * notice appear in all copies. The University of California
+ * makes no representations about the suitability of this
+ * software for any purpose. It is provided "as is" without
+ * express or implied warranty.
+ *
+ * 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)
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpParseArgv.h
+ \brief Command line argument parsing.
+*/
+
+#ifndef vpParseArgv_h
+#define vpParseArgv_h
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+/*!
+ \class vpParseArgv
+ \ingroup module_io_cmd_parser
+ \brief Command line argument parsing.
+
+ The code below shows a first way to parse command line arguments
+ using vpParseArgv class. It allows to specify an option
+ name with more than one character.
+
+ \code
+#include <stdio.h>
+#include <visp3/io/vpParseArgv.h>
+
+// Usage : [-int <integer value>] [-float <float value>] [-double <double value>] [-h]
+int main(int argc, const char ** argv)
+{
+ // Variables to set by command line parsing
+ int i_val = 0;
+ float f_val = 0;
+ double d_val = 0;
+
+ // Parse the command line to set the variables
+ vpParseArgv::vpArgvInfo argTable[] =
+ {
+ {"-int", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &i_val,
+ "An integer value."},
+ {"-float", vpParseArgv::ARGV_FLOAT, (char*) NULL, (char *) &f_val,
+ "A float value."},
+ {"-double", vpParseArgv::ARGV_DOUBLE, (char*) NULL, (char *) &d_val,
+ "A double value."},
+ {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
+ "Print the help."},
+ {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
+ } ;
+
+ // Read the command line options
+ if(vpParseArgv::parse(&argc, argv, argTable,
+ vpParseArgv::ARGV_NO_LEFTOVERS |
+ vpParseArgv::ARGV_NO_ABBREV |
+ vpParseArgv::ARGV_NO_DEFAULTS)) {
+ return (false);
+ }
+
+ // i_val, f_val, d_val may have new values
+}
+ \endcode
+
+ The code below shows an other way to parse command line arguments using
+ vpParseArgv class. Here command line options are only one character long.
+ \code
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "d:f:i:h" // double point mean here that the preceding option request an argument
+
+// Usage : [-i <integer value>] [-f <float value>] [-d <double value>] [-h]
+int main(int argc, const char ** argv)
+{
+ // Variables to set by command line parsing
+ int i_val = 0;
+ float f_val = 0;
+ double d_val = 0;
+
+ // Parse the command line to set the variables
+ const char *optarg;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
+
+ switch (c) {
+ case 'd': d_val = atof(optarg); break;
+ case 'f': f_val = (float) atof(optarg); break;
+ case 'i': i_val = atoi(optarg); break;
+ case 'h': printf("Usage: ...\n"); return true; break;
+
+ default:
+ printf("Usage: ...\n"); return true; break;
+ }
+ }
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ printf("Usage: ...\n");
+ return false;
+ }
+
+ // i_val, f_val, d_val may have new values
+}
+ \endcode
+
+
+*/
+
+
+class VISP_EXPORT vpParseArgv
+{
+ public:
+ /*!
+ Legal values for the type field of a vpArgvInfo.
+ */
+ typedef enum {
+ ARGV_CONSTANT,/*!< Stand alone argument. */
+ ARGV_INT, /*!< Argument is associated to an int. */
+ ARGV_LONG, /*!< Argument is associated to a long. */
+ ARGV_STRING, /*!< Argument is associated to a char * string. */
+ ARGV_REST,
+ ARGV_FLOAT, /*!< Argument is associated to a float. */
+ ARGV_DOUBLE, /*!< Argument is associated to a double. */
+ ARGV_FUNC,
+ ARGV_GENFUNC,
+ ARGV_HELP, /*!< Argument is for help displaying. */
+ ARGV_END /*!< End of the argument list. */
+ } vpArgvType;
+
+ /*!
+ Flag bits.
+ */
+ typedef enum {
+ ARGV_NO_DEFAULTS = 0x1, /*!< No default options like -help. */
+ ARGV_NO_LEFTOVERS = 0x2, /*!< Print an error message if an option is not in the argument list. */
+ ARGV_NO_ABBREV = 0x4, /*!< No abrevation. Print an error message if an option is abrevated (ie "-i" in place of "-int" which is requested). */
+ ARGV_DONT_SKIP_FIRST_ARG = 0x8, /*!< Don't skip first argument. */
+ ARGV_NO_PRINT = 0x10 /*!< No printings. */
+ } vpArgvFlags;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ /*!
+
+ Structure used to specify how to handle argv options.
+ */
+ typedef struct {
+ const char *key; /*!< The key string that flags the option in the
+ * argv array. */
+ vpArgvType type; /*!< Indicates option type; see below. */
+ const char *src; /*!< Value to be used in setting dst; usage
+ * depends on type. */
+ const char *dst; /*!< Address of value to be modified; usage
+ * depends on type. */
+ const char *help; /*!< Documentation message describing this option. */
+ } vpArgvInfo;
+#endif /* DOXYGEN_SHOULD_SKIP_THIS */
+
+public:
+ static vpArgvInfo defaultTable[2];
+ static bool parse(int *argcPtr, const char **argv,
+ vpArgvInfo *argTable, int flags);
+ static int parse(int argc, const char** argv, const char* validOpts, const char** param);
+
+ private:
+ static void printUsage (vpArgvInfo *argTable, int flags);
+
+
+
+} ;
+
+
+#endif
diff --git a/modules/io/include/visp3/io/vpVideoReader.h b/modules/io/include/visp3/io/vpVideoReader.h
new file mode 100644
index 0000000..6e6b549
--- /dev/null
+++ b/modules/io/include/visp3/io/vpVideoReader.h
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read videos and sequences of images .
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpVideoReader.h
+ \brief Read videos and image sequences
+*/
+
+#ifndef vpVideoReader_H
+#define vpVideoReader_H
+
+#include <string>
+
+#include <visp3/io/vpDiskGrabber.h>
+#include <visp3/io/vpFFMPEG.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020200
+#include "opencv2/highgui/highgui.hpp"
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020000
+#include "opencv/highgui.h"
+#endif
+
+/*!
+ \class vpVideoReader
+
+ \ingroup group_io_video
+
+ \brief Class that enables to manipulate easily a video file or a sequence of
+ images. As it inherits from the vpFrameGrabber Class, it can be used like an
+ other frame grabber class.
+
+ The following example available in tutorial-video-reader.cpp shows how this
+ class is really easy to use. It enables to read a video file named video.mpeg.
+ \include tutorial-video-reader.cpp
+
+ As shown in the next example, this class allows also to access to a specific
+ frame. But be careful, for video files, the getFrame() method is not precise
+ and returns the nearest intra key frame from the expected frame. You can use
+ the getFrame() method to position the reader in the video and then use the
+ acquire() method to get the following frames one by one.
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpVideoReader.h>
+
+int main()
+{
+#ifdef VISP_HAVE_FFMPEG
+ vpImage<vpRGBa> I;
+
+ vpVideoReader reader;
+
+ // Initialize the reader.
+ reader.setFileName("video.mpeg");
+ reader.open(I);
+
+ // Read the nearest key frame from the 3th frame
+ reader.getFrame(I,2);
+
+ // After positionning the video reader use acquire to read the video frame by frame
+ reader.acquire(I);
+
+ return 0;
+#endif
+}
+ \endcode
+
+ The other following example explains how to use the class to read a
+ sequence of images. The images are stored in the folder "./image" and are
+ named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ... As explained
+ in setFirstFrameIndex() and setLastFrameIndex() it is also possible to set the
+ first and last image numbers to read a portion of the sequence. If these two
+ functions are not used, first and last image numbers are set automatically to
+ match the first and image images of the sequence.
+
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpVideoReader.h>
+
+int main()
+{
+ vpImage<vpRGBa> I;
+
+ vpVideoReader reader;
+
+ // Initialize the reader.
+ reader.setFileName("./image/image%04d.jpeg");
+ reader.setFirstFrameIndex(10);
+ reader.setLastFrameIndex(20);
+ reader.open(I);
+
+ while (! reader.end() )
+ reader.acquire(I);
+
+ return 0;
+}
+ \endcode
+
+ Note that it is also possible to access to a specific frame using getFrame().
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpVideoReader.h>
+
+int main()
+{
+ vpImage<vpRGBa> I;
+
+ vpVideoReader reader;
+
+ // Initialize the reader.
+ reader.setFileName("./image/image%04d.jpeg");
+ reader.open(I);
+
+ // Read the 3th frame
+ reader.getFrame(I,2);
+
+ return 0;
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpVideoReader : public vpFrameGrabber
+{
+ private:
+ //!To read sequences of images
+ vpDiskGrabber *imSequence;
+#ifdef VISP_HAVE_FFMPEG
+ //!To read video files
+ vpFFMPEG *ffmpeg;
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ //!To read video files with OpenCV
+ cv::VideoCapture capture;
+ cv::Mat frame;
+#endif
+ //!Types of available formats
+ typedef enum
+ {
+ FORMAT_PGM,
+ FORMAT_PPM,
+ FORMAT_JPEG,
+ FORMAT_PNG,
+ // Formats supported by opencv
+ FORMAT_TIFF,
+ FORMAT_BMP,
+ FORMAT_DIB,
+ FORMAT_PBM,
+ FORMAT_RASTER,
+ FORMAT_JPEG2000,
+ // Video format
+ FORMAT_AVI,
+ FORMAT_MPEG,
+ FORMAT_MPEG4,
+ FORMAT_MOV,
+ FORMAT_OGV,
+ FORMAT_WMV,
+ FORMAT_FLV,
+ FORMAT_MKV,
+ FORMAT_UNKNOWN
+ } vpVideoFormatType;
+
+ //!Video's format which has to be read
+ vpVideoFormatType formatType;
+
+ //!Path to the video
+ char fileName[FILENAME_MAX];
+ //!Indicates if the path to the video is set.
+ bool initFileName;
+ //!Indicates if the video is "open".
+ bool isOpen;
+ //!Count the frame number when the class is used as a grabber.
+ long frameCount; // Index of the next image
+ //!The first frame index
+ long firstFrame;
+ //!The last frame index
+ long lastFrame;
+ bool firstFrameIndexIsSet;
+ bool lastFrameIndexIsSet;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpVideoReader(const vpVideoReader &)
+// : vpFrameGrabber(), imSequence(NULL),
+// #ifdef VISP_HAVE_FFMPEG
+// ffmpeg(NULL),
+// #elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+// capture(), frame(),
+// #endif
+// formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
+// firstFrame(0), lastFrame(0), firstFrameIndexIsSet(false), lastFrameIndexIsSet(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpVideoReader &operator=(const vpVideoReader &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpVideoReader();
+ ~vpVideoReader();
+
+ void acquire(vpImage< vpRGBa > &I);
+ void acquire(vpImage< unsigned char > &I);
+ void close(){;}
+
+ /*!
+ \return true if the end of the sequence is reached.
+ */
+ inline bool end() {
+ if (frameCount > lastFrame )
+ return true;
+ return false;
+ }
+ bool getFrame(vpImage<vpRGBa> &I, long frame);
+ bool getFrame(vpImage<unsigned char> &I, long frame);
+ double getFramerate();
+
+ /*!
+ Get the frame index of the next image. This index is updated at each call of the
+ acquire method. It can be used to detect the end of a file (comparison
+ with getLastFrameIndex()).
+
+ \return Returns the current frame index.
+ */
+ inline long getFrameIndex() const { return frameCount;}
+
+ /*!
+ Gets the first frame index.
+
+ \return Returns the first frame index.
+ */
+ inline long getFirstFrameIndex() const {return firstFrame;}
+ /*!
+ Gets the last frame index.
+
+ \return Returns the last frame index.
+ */
+ inline long getLastFrameIndex() const {return lastFrame;}
+ void open (vpImage< vpRGBa > &I);
+ void open (vpImage< unsigned char > &I);
+
+ /*!
+ Reset the frame counter and sets it to the first image index.
+
+ By default the first frame index is set to 0.
+
+ This method is useful if you use the class like a frame grabber (ie with theacquire method).
+ */
+ inline void resetFrameCounter() {frameCount = firstFrame;}
+ void setFileName(const char *filename);
+ void setFileName(const std::string &filename);
+ /*!
+ Enables to set the first frame index if you want to use the class like a grabber (ie with the
+ acquire method).
+
+ \param first_frame : The first frame index.
+
+ \sa setLastFrameIndex()
+ */
+ inline void setFirstFrameIndex(const long first_frame) {
+ this->firstFrameIndexIsSet = true;
+ this->firstFrame = first_frame;
+ }
+ /*!
+ Enables to set the last frame index.
+
+ \param last_frame : The last frame index.
+
+ \sa setFirstFrameIndex()
+ */
+ inline void setLastFrameIndex(const long last_frame) {
+ this->lastFrameIndexIsSet = true;
+ this->lastFrame = last_frame;
+ }
+
+ private:
+ vpVideoFormatType getFormat(const char *filename);
+ static std::string getExtension(const std::string &filename);
+ void findFirstFrameIndex();
+ void findLastFrameIndex();
+ bool isImageExtensionSupported();
+ bool isVideoExtensionSupported();
+};
+
+#endif
diff --git a/modules/io/include/visp3/io/vpVideoWriter.h b/modules/io/include/visp3/io/vpVideoWriter.h
new file mode 100644
index 0000000..b48e0ec
--- /dev/null
+++ b/modules/io/include/visp3/io/vpVideoWriter.h
@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Write videos and sequences of images.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpVideoWriter.h
+ \brief Write videos and sequences of images.
+*/
+
+#ifndef vpVideoWriter_H
+#define vpVideoWriter_H
+
+#include <string>
+
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpFFMPEG.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020200
+# include <opencv2/highgui/highgui.hpp>
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020000
+# include <opencv/highgui.h>
+#endif
+
+
+/*!
+ \class vpVideoWriter
+
+ \ingroup group_io_video
+
+ \brief Class that enables to write easily a video file or a sequence of images.
+
+ The following example available in tutorial-video-recorder.cpp shows how this
+ class can be used to record a video from a camera by default in an mpeg file.
+ \include tutorial-video-recorder.cpp
+
+ The following example shows also how this class can be used to write an image sequence.
+ The images are stored in the folder "./image" and are named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ...
+
+ \code
+ #include <visp3/core/vpConfig.h>
+ #include <visp3/io/vpVideoWriter.h>
+
+ int main()
+ {
+ vpImage<vpRGBa> I;
+
+ vpVideoWriter writer;
+
+ //Initialize the writer.
+ writer.setFileName("./image/image%04d.jpeg");
+
+ writer.open(I);
+
+ for ( ; ; )
+ {
+ //Here the code to capture or create an image and stores it in I.
+
+ //Save the image
+ writer.saveFrame(I);
+ }
+
+ writer.close();
+
+ return 0;
+ }
+ \endcode
+
+ The other following example explains how to use the class to write directly an mpeg file.
+
+ \code
+#include <visp3/io/vpVideoWriter.h>
+
+int main()
+{
+ vpImage<vpRGBa> I;
+
+ vpVideoWriter writer;
+
+ // Set up the framerate to 30Hz. Default is 25Hz.
+ writer.setFramerate(30);
+
+#ifdef VISP_HAVE_FFMPEG
+ // Set up the bit rate
+ writer.setBitRate(1000000);
+ // Set up the codec to use
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ writer.setCodec(CODEC_ID_MPEG2VIDEO);
+#else
+ writer.setCodec(AV_CODEC_ID_MPEG2VIDEO);
+#endif
+#elif defined VISP_HAVE_OPENCV
+ writer.setCodec( CV_FOURCC('P','I','M','1') );
+#endif
+ writer.setFileName("./test.mpeg");
+
+ writer.open(I);
+
+ for ( ; ; )
+ {
+ // Here the code to capture or create an image and store it in I.
+
+ // Save the image
+ writer.saveFrame(I);
+ }
+
+ writer.close();
+
+ return 0;
+}
+ \endcode
+*/
+
+class VISP_EXPORT vpVideoWriter
+{
+ private:
+#ifdef VISP_HAVE_FFMPEG
+ //!To read video files
+ vpFFMPEG *ffmpeg;
+ //!The codec to use
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ CodecID codec;
+#else
+ AVCodecID codec;
+#endif
+ //!The bite rate
+ unsigned int bit_rate;
+ int framerate;
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ cv::VideoWriter writer;
+ int fourcc;
+ double framerate;
+#endif
+ //!Types of available formats
+ typedef enum
+ {
+ FORMAT_PGM,
+ FORMAT_PPM,
+ FORMAT_JPEG,
+ FORMAT_PNG,
+ FORMAT_AVI,
+ FORMAT_MPEG,
+ FORMAT_MPEG4,
+ FORMAT_MOV,
+ FORMAT_UNKNOWN
+ } vpVideoFormatType;
+
+ //!Video's format which has to be writen
+ vpVideoFormatType formatType;
+
+ //!Path to the image sequence
+ char fileName[FILENAME_MAX];
+
+ //!Indicates if the path to the image sequence is set.
+ bool initFileName;
+
+ //!Indicates if the video is "open".
+ bool isOpen;
+
+ //!Count the frame number.
+ unsigned int frameCount;
+
+ //!The first frame index.
+ unsigned int firstFrame;
+
+ //!Size of the frame
+ unsigned int width;
+ unsigned int height;
+
+ public:
+ vpVideoWriter();
+ ~vpVideoWriter();
+
+ void close();
+
+ /*!
+ Gets the current frame index.
+
+ \return Returns the current frame index.
+ */
+ inline unsigned int getCurrentFrameIndex() const {return frameCount;}
+
+ void open (vpImage< vpRGBa > &I);
+ void open (vpImage< unsigned char > &I);
+ /*!
+ Reset the frame counter and sets it to the first image index.
+
+ By default the first frame index is set to 0.
+ */
+ inline void resetFrameCounter() {frameCount = firstFrame;}
+
+ void saveFrame (vpImage< vpRGBa > &I);
+ void saveFrame (vpImage< unsigned char > &I);
+
+#ifdef VISP_HAVE_FFMPEG
+ /*!
+ Sets the bit rate of the video when encoding.
+
+ \param bitrate : the expected bit rate.
+
+ By default the bit rate is set to 500 000.
+ */
+ inline void setBitRate(const unsigned int bitrate) {this->bit_rate = bitrate;}
+
+ /*!
+ Sets the codec used to encode the video.
+
+ \param codec_id : the expected codec.
+
+ By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
+ AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
+ AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
+
+ Of course to use the codec it must be installed on your computer.
+ */
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ inline void setCodec(const CodecID codec_id) {this->codec = codec_id;}
+#else
+ inline void setCodec(const AVCodecID codec_id) {this->codec = codec_id;}
+#endif
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ inline void setCodec(const int fourcc_codec) {this->fourcc = fourcc_codec;}
+#endif
+
+ void setFileName(const char *filename);
+ void setFileName(const std::string &filename);
+ /*!
+ Enables to set the first frame index.
+
+ \param first_frame : The first frame index.
+ */
+ inline void setFirstFrameIndex(const unsigned int first_frame) {this->firstFrame = first_frame;}
+#ifdef VISP_HAVE_FFMPEG
+ /*!
+ Sets the framerate in Hz of the video when encoding.
+
+ \param frame_rate : the expected framerate.
+
+ By default the framerate is set to 25Hz.
+ */
+ inline void setFramerate(const int frame_rate) {
+ this->framerate = frame_rate;
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ /*!
+ Sets the framerate in Hz of the video when encoding.
+
+ \param frame_rate : the expected framerate.
+
+ By default the framerate is set to 25Hz.
+ */
+ inline void setFramerate(const double frame_rate) {
+ this->framerate = frame_rate;
+ }
+#endif
+
+ private:
+ vpVideoFormatType getFormat(const char *filename);
+ static std::string getExtension(const std::string &filename);
+};
+
+#endif
diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp
new file mode 100644
index 0000000..7ee24a9
--- /dev/null
+++ b/modules/io/src/image/vpImageIo.cpp
@@ -0,0 +1,3273 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read/write images.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpImageIo.cpp
+ \brief Read/write images
+*/
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h> //image conversion
+#include <visp3/core/vpIoTools.h>
+
+const int vpImageIo::vpMAX_LEN = 100;
+
+/*!
+
+ Open a file with read access.
+
+ \param filename : Name of the file to open.
+
+ \return File descriptor.
+*/
+FILE *
+vpImageIo::openFileRead(const char *filename)
+{
+
+ FILE *fd ;
+
+ // Lecture du nom du fichier image.
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,
+ "filename empty ")) ;
+ }
+
+ // Ouverture de l'image.
+ if ((fd = fopen(filename, "r")) == NULL)
+ {
+ vpERROR_TRACE("cannot open file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot open file")) ;
+ }
+ return fd ;
+}
+
+/*!
+
+ Open a file with write access.
+
+ \param filename : Name of the file to open.
+
+ \param mode : Access mode. By default set to "w" for write
+ access. Could be changed to set for example the access mode to "wa"
+ to append data in the file.
+
+ \return File descriptor.
+*/
+FILE *
+vpImageIo::openFileWrite(const char *filename, const char *mode)
+{
+ FILE *fd ;
+
+ // Lecture du nom du fichier image.
+ if (!filename || *filename == '\0')
+ {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,
+ "filename empty ")) ;
+ }
+
+ // Ouverture de l'image.
+ if ((fd = fopen(filename, mode)) == NULL)
+ {
+ vpERROR_TRACE("cannot open file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot open file")) ;
+ }
+ return fd ;
+}
+
+/*!
+
+ Open a file with read access.
+
+ \param filename : Name of the file to open.
+
+ \return File descriptor.
+*/
+FILE *
+vpImageIo::openFileRead(const std::string filename)
+{
+
+ FILE *fd ;
+
+ // Lecture du nom du fichier image.
+ if (filename.empty()) {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,
+ "filename empty ")) ;
+ }
+
+ // Ouverture de l'image.
+ if ((fd = fopen(filename.c_str(), "r")) == NULL)
+ {
+ vpERROR_TRACE("cannot open file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot open file")) ;
+ }
+ return fd ;
+}
+
+/*!
+
+ Open a file with write access.
+
+ \param filename : Name of the file to open.
+
+ \param mode : Access mode. By default set to "w" for write
+ access. Could be changed to set for example the access mode to "wa"
+ to append data in the file.
+
+ \return File descriptor.
+*/
+FILE *
+vpImageIo::openFileWrite(const std::string filename,
+ const std::string mode)
+{
+ FILE *fd ;
+
+ // Lecture du nom du fichier image.
+ if (filename.empty())
+ {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,
+ "filename empty ")) ;
+ }
+
+ // Ouverture de l'image.
+ if ((fd = fopen(filename.c_str(), mode.c_str())) == NULL)
+ {
+ vpERROR_TRACE("cannot open file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot open file")) ;
+ }
+ return fd ;
+}
+
+vpImageIo::vpImageFormatType
+vpImageIo::getFormat(const char *filename)
+{
+ std::string sfilename(filename);
+
+ std::string ext = vpImageIo::getExtension(sfilename);
+
+ if (ext.compare(".PGM") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".pgm") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".PPM") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".ppm") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".JPG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".JPEG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpeg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".PNG") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".png") == 0)
+ return FORMAT_PNG;
+ // Formats supported by opencv
+ else if (ext.compare(".TIFF") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".tiff") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".TIF") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".tif") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".BMP") == 0)
+ return FORMAT_BMP;
+ else if (ext.compare(".bmp") == 0)
+ return FORMAT_BMP;
+ else if (ext.compare(".DIB") == 0)
+ return FORMAT_DIB;
+ else if (ext.compare(".dib") == 0)
+ return FORMAT_DIB;
+ else if (ext.compare(".PBM") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".pbm") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".SR") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".sr") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".RAS") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".ras") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".JP2") == 0)
+ return FORMAT_JPEG2000;
+ else if (ext.compare(".jp2") == 0)
+ return FORMAT_JPEG2000;
+ else
+ return FORMAT_UNKNOWN;
+}
+
+// return the extension of the file including the dot
+std::string vpImageIo::getExtension(const std::string &filename)
+{
+ // extract the extension
+ size_t dot = filename.find_last_of(".");
+ std::string ext = filename.substr(dot, filename.size()-1);
+ return ext;
+}
+
+
+/*!
+ Read the contents of the image filename, allocate memory for the corresponding
+ greyscale image, update its content, and return a reference to the image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::read(vpImage<unsigned char> &I, const char *filename)
+{
+ bool exist = vpIoTools::checkFilename(filename);
+ if (!exist) {
+ std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
+ throw (vpImageException(vpImageException::ioError, message));
+ }
+ bool try_opencv_reader = false;
+
+ switch(getFormat(filename)){
+ case FORMAT_PGM :
+ readPGM(I,filename); break;
+ case FORMAT_PPM :
+ readPPM(I,filename); break;
+ case FORMAT_JPEG :
+#ifdef VISP_HAVE_JPEG
+ readJPEG(I,filename);
+#else
+ try_opencv_reader = true;
+#endif
+ break;
+ case FORMAT_PNG :
+#if defined(VISP_HAVE_PNG)
+ readPNG(I,filename);
+#else
+ try_opencv_reader = true;
+#endif
+ break;
+ case FORMAT_TIFF :
+ case FORMAT_BMP :
+ case FORMAT_DIB :
+ case FORMAT_PBM :
+ case FORMAT_RASTER :
+ case FORMAT_JPEG2000 :
+ case FORMAT_UNKNOWN :
+ try_opencv_reader = true;
+ break;
+ }
+
+ if (try_opencv_reader) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ //std::cout << "Use opencv to read the image" << std::endl;
+ cv::Mat cvI = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+ if (cvI.cols == 0 && cvI.rows == 0) {
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+ }
+ vpImageConvert::convert(cvI, I);
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ //std::cout << "Use opencv to read the image" << std::endl;
+ cv::Mat cvI = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if (cvI.cols == 0 && cvI.rows == 0) {
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+ }
+ vpImageConvert::convert(cvI, I);
+#else
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+#endif
+ }
+}
+/*!
+ Read the contents of the image filename, allocate memory for the corresponding
+ greyscale image, update its content, and return a reference to the image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::read(vpImage<unsigned char> &I, const std::string filename)
+{
+ read(I,filename.c_str());
+}
+/*!
+ Read the contents of the image filename, allocate memory for the corresponding
+ color image, update its content, and return a reference to the image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::read(vpImage<vpRGBa> &I, const char *filename)
+{
+ bool exist = vpIoTools::checkFilename(filename);
+ if (!exist) {
+ std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
+ throw (vpImageException(vpImageException::ioError, message));
+ }
+
+ bool try_opencv_reader = false;
+
+ switch(getFormat(filename)){
+ case FORMAT_PGM :
+ readPGM(I,filename); break;
+ case FORMAT_PPM :
+ readPPM(I,filename); break;
+ case FORMAT_JPEG :
+#ifdef VISP_HAVE_JPEG
+ readJPEG(I,filename);
+#else
+ try_opencv_reader = true;
+#endif
+ break;
+ case FORMAT_PNG :
+#if defined(VISP_HAVE_PNG)
+ readPNG(I,filename);
+#else
+ try_opencv_reader = true;
+#endif
+ break;
+ case FORMAT_TIFF :
+ case FORMAT_BMP :
+ case FORMAT_DIB :
+ case FORMAT_PBM :
+ case FORMAT_RASTER :
+ case FORMAT_JPEG2000 :
+ case FORMAT_UNKNOWN :
+ try_opencv_reader = true;
+ break;
+ }
+
+ if (try_opencv_reader) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ // std::cout << "Use opencv to read the image" << std::endl;
+ cv::Mat cvI = cv::imread(filename, cv::IMREAD_COLOR);
+ if (cvI.cols == 0 && cvI.rows == 0) {
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+ }
+ vpImageConvert::convert(cvI, I);
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ // std::cout << "Use opencv to read the image" << std::endl;
+ cv::Mat cvI = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
+ if (cvI.cols == 0 && cvI.rows == 0) {
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+ }
+ vpImageConvert::convert(cvI, I);
+#else
+ std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
+ throw (vpImageException(vpImageException::ioError, message)) ;
+#endif
+ }
+}
+/*!
+ Read the contents of the image filename, allocate memory for the corresponding
+ color image, update its content, and return a reference to the image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::read(vpImage<vpRGBa> &I, const std::string filename)
+{
+ read(I,filename.c_str());
+}
+
+/*!
+ Write the content of the image in the file which name is given by \e
+ filename.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to write.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::write(const vpImage<unsigned char> &I, const char *filename)
+{
+ bool try_opencv_writer = false;
+
+ switch(getFormat(filename)){
+ case FORMAT_PGM :
+ writePGM(I,filename); break;
+ case FORMAT_PPM :
+ writePPM(I,filename); break;
+ case FORMAT_JPEG :
+#ifdef VISP_HAVE_JPEG
+ writeJPEG(I,filename);
+#else
+ try_opencv_writer = true;
+#endif
+ break;
+ case FORMAT_PNG :
+#ifdef VISP_HAVE_PNG
+ writePNG(I,filename);
+#else
+ try_opencv_writer = true;
+#endif
+ break;
+ case FORMAT_TIFF :
+ case FORMAT_BMP :
+ case FORMAT_DIB :
+ case FORMAT_PBM :
+ case FORMAT_RASTER :
+ case FORMAT_JPEG2000 :
+ case FORMAT_UNKNOWN :
+ try_opencv_writer = true;
+ break;
+ }
+
+ if (try_opencv_writer) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ // std::cout << "Use opencv to write the image" << std::endl;
+ cv::Mat cvI;
+ vpImageConvert::convert(I, cvI);
+ cv::imwrite(filename, cvI);
+#else
+ vpCERROR << "Cannot write file: Image format not supported..." << std::endl;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot write file: Image format not supported")) ;
+#endif
+ }
+}
+/*!
+ Write the content of the image in the file which name is given by \e
+ filename.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to write.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::write(const vpImage<unsigned char> &I, const std::string filename)
+{
+ write(I,filename.c_str());
+}
+/*!
+ Write the content of the image in the file which name is given by \e
+ filename.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to write.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::write(const vpImage<vpRGBa> &I, const char *filename)
+{
+ bool try_opencv_writer = false;
+
+ switch(getFormat(filename)){
+ case FORMAT_PGM :
+ writePGM(I,filename); break;
+ case FORMAT_PPM :
+ writePPM(I,filename); break;
+ case FORMAT_JPEG :
+#ifdef VISP_HAVE_JPEG
+ writeJPEG(I,filename);
+#else
+ try_opencv_writer = true;
+#endif
+ break;
+ case FORMAT_PNG :
+#ifdef VISP_HAVE_PNG
+ writePNG(I,filename);
+#else
+ try_opencv_writer = true;
+#endif
+ break;
+ case FORMAT_TIFF :
+ case FORMAT_BMP :
+ case FORMAT_DIB :
+ case FORMAT_PBM :
+ case FORMAT_RASTER :
+ case FORMAT_JPEG2000 :
+ case FORMAT_UNKNOWN :
+ try_opencv_writer = true;
+ break;
+ }
+
+ if (try_opencv_writer) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ // std::cout << "Use opencv to write the image" << std::endl;
+ cv::Mat cvI;
+ vpImageConvert::convert(I, cvI);
+ cv::imwrite(filename, cvI);
+#else
+ vpCERROR << "Cannot write file: Image format not supported..." << std::endl;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot write file: Image format not supported")) ;
+#endif
+ }
+}
+/*!
+ Write the content of the image in the file which name is given by \e
+ filename.
+
+ Always supported formats are *.pgm and *.ppm.
+ If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
+ If \c libpng 3rd party is used, we support also *.png files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+
+ \param I : Image to write.
+ \param filename : Name of the file containing the image.
+ */
+void
+vpImageIo::write(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ write(I,filename.c_str());
+}
+//--------------------------------------------------------------------------
+// PFM
+//--------------------------------------------------------------------------
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function is built like portable gray pixmap (eg PGM P5) file.
+ but considers float image data.
+
+ \param I : Image to save as a (PFM P8) file.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::writePFM(const vpImage<float> &I,
+ const char *filename)
+{
+
+ FILE* fd;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ fd = fopen(filename, "wb");
+
+ if (fd == NULL) {
+ vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ // Write the head
+ fprintf(fd, "P8\n"); // Magic number
+ fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
+
+ // Write the bitmap
+ size_t ierr;
+ size_t nbyte = I.getWidth()*I.getHeight();
+
+ ierr = fwrite(I.bitmap, sizeof(float), nbyte, fd) ;
+ if (ierr != nbyte) {
+ fclose(fd);
+ vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
+ nbyte, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ fflush(fd);
+ fclose(fd);
+
+}
+//--------------------------------------------------------------------------
+// PGM
+//--------------------------------------------------------------------------
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::writePGM(const vpImage<unsigned char> &I,
+ const char *filename)
+{
+
+ FILE* fd;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ fd = fopen(filename, "wb");
+
+ if (fd == NULL) {
+ vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ // Write the head
+ fprintf(fd, "P5\n"); // Magic number
+ fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
+
+ // Write the bitmap
+ size_t ierr;
+ size_t nbyte = I.getWidth()*I.getHeight();
+
+ ierr = fwrite(I.bitmap, sizeof(unsigned char), nbyte, fd) ;
+ if (ierr != nbyte) {
+ fclose(fd);
+ vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
+ nbyte, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ fflush(fd);
+ fclose(fd);
+
+}
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePGM(const vpImage<short> &I, const char *filename)
+{
+ vpImage<unsigned char> Iuc ;
+ unsigned int nrows = I.getHeight();
+ unsigned int ncols = I.getWidth();
+
+ Iuc.resize(nrows, ncols);
+
+ for (unsigned int i=0 ; i < nrows * ncols ; i++)
+ Iuc.bitmap[i] = (unsigned char)I.bitmap[i] ;
+
+ vpImageIo::writePGM(Iuc, filename) ;
+
+
+}
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+ Color image is converted into a grayscale image.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::writePGM(const vpImage<vpRGBa> &I, const char *filename)
+{
+
+ FILE* fd;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ fd = fopen(filename, "wb");
+
+ if (fd == NULL) {
+ vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ // Write the head
+ fprintf(fd, "P5\n"); // Magic number
+ fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
+
+ // Write the bitmap
+ size_t ierr;
+ size_t nbyte = I.getWidth()*I.getHeight();
+
+
+ vpImage<unsigned char> Itmp ;
+ vpImageConvert::convert(I,Itmp) ;
+
+ ierr = fwrite(Itmp.bitmap, sizeof(unsigned char), nbyte, fd) ;
+ if (ierr != nbyte) {
+ fclose(fd);
+ vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
+ nbyte, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ fflush(fd);
+ fclose(fd);
+
+}
+
+/*!
+ Read a PFM P8 file and initialize a float image.
+
+ Read the contents of the portable gray pixmap (PFM P8) filename, allocate
+ memory for the corresponding image, and set the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::readPFM(vpImage<float> &I, const char *filename)
+{
+ FILE* fd = NULL; // File descriptor
+ int ierr;
+ int line;
+ int is255;
+ char* err ;
+ char str[vpMAX_LEN];
+ unsigned int w, h;
+
+ // Test the filename
+ if (!filename || *filename == '\0')
+ {
+ vpERROR_TRACE("no filename") ;
+ throw (vpImageException(vpImageException::ioError,
+ " no filename")) ;
+
+ }
+
+ // Open the filename
+ fd = fopen(filename, "rb");
+ if (fd == NULL)
+ {
+ vpERROR_TRACE("couldn't read file \"%s\"", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "couldn't read file")) ;
+ }
+
+ // Read the first line with magic number P8
+ line = 0;
+
+ err = fgets(str, vpMAX_LEN - 1, fd);
+ line++;
+ if (err == NULL)
+ {
+ fclose (fd);
+ vpERROR_TRACE("couldn't read line %d of file \"%s\"\n", line, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "couldn't read file")) ;
+ }
+
+ if (strlen(str) < 3)
+ {
+ fclose (fd);
+ vpERROR_TRACE("\"%s\" is not a PFM file\n", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "this is not a PFM file")) ;
+ }
+
+ str[2] = '\0';
+ if (strcmp(str, "P8") != 0)
+ {
+ fclose (fd);
+ vpERROR_TRACE("\"%s\" is not a PFM file\n", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "this is not a PFM file")) ;
+ }
+
+ // Jump the possible comment, or empty line and read the following line
+ do {
+ err = fgets(str, vpMAX_LEN - 1, fd);
+ line++;
+ if (err == NULL) {
+ fprintf(stderr, "couldn't read line %d of file \"%s\"\n", line, filename);
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+ } while ((str[0] == '#') || (str[0] == '\n'));
+
+ // Extract image size
+ ierr = sscanf(str, "%d %d", &w, &h);
+ if (w > 100000 || h>100000) {
+ fclose (fd);
+ throw(vpException(vpException::badValue, "Bad image size"));
+ }
+
+ if(ierr == 1){// the norm allows to have the two values on two separated lines.
+ do {
+ err = fgets(str, vpMAX_LEN - 1, fd);
+ line++;
+ if (err == NULL) {
+ fprintf(stderr, "couldn't read line %d of file \"%s\"\n", line, filename);
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+ } while ((str[0] == '#') || (str[0] == '\n'));
+ ierr = sscanf(str, "%d", &h);
+ }
+ if (ierr == EOF)
+ {
+ fclose (fd);
+ vpERROR_TRACE("couldn't read line %d of file \"%s\"\n",line, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+
+ if ((h != I.getHeight())||( w != I.getWidth()))
+ {
+ try
+ {
+ I.resize(h,w) ;
+ }
+ catch(...)
+ {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+ }
+
+ // Read 255
+ err = fgets(str, vpMAX_LEN - 1, fd);
+ line++;
+ if (err == NULL) {
+ fclose (fd);
+ vpERROR_TRACE("couldn't read line %d of file \"%s\"\n",line, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+
+ ierr = sscanf(str, "%d", &is255);
+ if (ierr == EOF) {
+ fclose (fd);
+ vpERROR_TRACE("couldn't read line %d of file \"%s\"\n", line, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+
+ if (is255 != 255)
+ {
+ fclose (fd);
+ vpERROR_TRACE("MAX_VAL is not 255 in file \"%s\"\n", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+
+ unsigned int nbyte = I.getHeight()*I.getWidth();
+ if (fread (I.bitmap, sizeof(float), nbyte, fd ) != nbyte)
+ {
+ fclose (fd);
+ vpERROR_TRACE("couldn't read %d bytes in file \"%s\"\n", nbyte, filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read content of PFM file")) ;
+ }
+
+ fclose (fd);
+}
+
+
+
+/*!
+ Read a PGM P5 file and initialize a scalar image.
+
+ Read the contents of the portable gray pixmap (PGM P5) filename, allocate
+ memory for the corresponding image, and set the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::readPGM(vpImage<unsigned char> &I, const char *filename)
+{
+ FILE* fd = NULL; // File descriptor
+ int ierr;
+ char* err ;
+ char str[vpMAX_LEN];
+ unsigned int magic=5, w=0, h=0, maxval=255;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ throw (vpImageException(vpImageException::ioError,
+ "No filename")) ;
+ }
+
+ // Open the filename
+ if ((fd = fopen(filename, "rb")) == NULL) {
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read file \"%s\"", filename)) ;
+ }
+
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "P%u %u %u %u", &magic, &w, &h, &maxval)) == 0) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+
+ if (magic != 5) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "\"%s\" is not a PGM P5 file", filename));
+ }
+
+ // Depending on ierr the line may contain:
+ // 1 : P5
+ // 2 : P5 w
+ // 3 : P5 w h
+ // 4 : P5 w h maxval
+
+ if (ierr == 1) {
+// std::cout << "magic: " << magic << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u %u", &w, &h, &maxval)) == 0) || (ierr != 1 && ierr != 2 && ierr != 3)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ // Depending on ierr the line may contain:
+ // 1 : w
+ // 2 : w h
+ // 3 : w h maxval
+ if (ierr == 1) {
+// std::cout << "w: " << w << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (ierr == 1) {
+// std::cout << "h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ }
+// else {
+// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 2) {
+// std::cout << "w: " << w << " h: " << h << std::endl;
+
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else {
+// std::cout << "w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 2) {
+// std::cout << "magic: " << magic << " w: " << w << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (ierr == 1) {
+// std::cout << "h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else {
+// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 3) {
+// std::cout << "magic: " << magic << " w: " << w << " h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else if (ierr == 4) {
+// std::cout << "magic: " << magic << " w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
+// }
+
+ if (w > 100000 || h>100000) {
+ fclose (fd);
+ throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename));
+ }
+ if (maxval != 255)
+ {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Bad maxval in \"%s\"", filename));
+ }
+
+ if ((h != I.getHeight())||( w != I.getWidth())) {
+ I.resize(h,w) ;
+ }
+
+ unsigned int nbyte = I.getHeight()*I.getWidth();
+ size_t n;
+ if ((n = fread (I.bitmap, sizeof(unsigned char), nbyte, fd)) != nbyte) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Read only %d of %d bytes in file \"%s\"", n, nbyte, filename));
+ }
+
+ fclose (fd);
+}
+
+
+/*!
+ Read a PGM P5 file and initialize a scalar image.
+
+ Read the contents of the portable gray pixmap (PGM P5) filename, allocate
+ memory for the corresponding image, and set the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ The gray level image contained in the \e filename is converted in a
+ color image in \e I.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::readPGM(vpImage<vpRGBa> &I, const char *filename)
+{
+ try
+ {
+ vpImage<unsigned char> Itmp ;
+
+ vpImageIo::readPGM(Itmp, filename) ;
+
+ vpImageConvert::convert(Itmp, I) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+
+//--------------------------------------------------------------------------
+// PPM
+//--------------------------------------------------------------------------
+
+/*!
+ Read the contents of the portable pixmap (PPM P6) filename, allocate memory
+ for the corresponding gray level image, convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. The quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPPM(vpImage<unsigned char> &I, const char *filename)
+{
+
+ try
+ {
+ vpImage<vpRGBa> Itmp ;
+
+ vpImageIo::readPPM(Itmp, filename) ;
+
+ vpImageConvert::convert(Itmp, I) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Read the contents of the portable pixmap (PPM P6) filename,
+ allocate memory for the corresponding vpRGBa image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPPM(vpImage<vpRGBa> &I, const char *filename)
+{
+ FILE* fd = NULL; // File descriptor
+ int ierr;
+ char* err ;
+ char str[vpMAX_LEN];
+ unsigned int magic=5, w=0, h=0, maxval=255;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ throw (vpImageException(vpImageException::ioError,
+ "No filename")) ;
+ }
+
+ // Open the filename
+ if ((fd = fopen(filename, "rb")) == NULL) {
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read file \"%s\"", filename)) ;
+ }
+
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "P%u %u %u %u", &magic, &w, &h, &maxval)) == 0) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+
+ if (magic != 6) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "\"%s\" is not a PGM P6 file", filename));
+ }
+
+ // Depending on ierr the line may contain:
+ // 1 : P6
+ // 2 : P6 w
+ // 3 : P6 w h
+ // 4 : P6 w h maxval
+
+ if (ierr == 1) {
+// std::cout << "magic: " << magic << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u %u", &w, &h, &maxval)) == 0) || (ierr != 1 && ierr != 2 && ierr != 3)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ // Depending on ierr the line may contain:
+ // 1 : w
+ // 2 : w h
+ // 3 : w h maxval
+ if (ierr == 1) {
+// std::cout << "w: " << w << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (ierr == 1) {
+// std::cout << "h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ }
+// else {
+// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 2) {
+// std::cout << "w: " << w << " h: " << h << std::endl;
+
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else {
+// std::cout << "w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 2) {
+// std::cout << "magic: " << magic << " w: " << w << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if (ierr == 1) {
+// std::cout << "h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else {
+// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
+// }
+ }
+ else if (ierr == 3) {
+// std::cout << "magic: " << magic << " w: " << w << " h: " << h << std::endl;
+ while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
+ if (err == NULL) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+ if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read header of file \"%s\"", filename));
+ }
+// std::cout << "maxval: " << maxval << std::endl;
+ }
+// else if (ierr == 4) {
+// std::cout << "magic: " << magic << " w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
+// }
+
+ if (w > 100000 || h>100000) {
+ fclose (fd);
+ throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename));
+ }
+ if (maxval != 255)
+ {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Bad maxval in \"%s\"", filename));
+ }
+
+ if ((h != I.getHeight())||( w != I.getWidth())) {
+ I.resize(h,w) ;
+ }
+
+ for(unsigned int i=0;i<I.getHeight();i++)
+ {
+ for(unsigned int j=0;j<I.getWidth();j++)
+ {
+ vpRGBa v ;
+ size_t res = fread(&v.R,sizeof(v.R),1,fd) ;
+ res |= fread(&v.G,sizeof(v.G),1,fd) ;
+ res |= fread(&v.B,sizeof(v.B),1,fd) ;
+ if (res==0)
+ {
+ fclose (fd);
+ throw (vpImageException(vpImageException::ioError,
+ "Cannot read bytes in file \"%s\"\n", filename));
+ }
+ I[i][j] = v ;
+ }
+ }
+
+ fclose (fd);
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PPM P6) file.
+ grayscale image is converted into a color image vpRGBa.
+
+ \param I : Image to save as a (PPM P6) file.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::writePPM(const vpImage<unsigned char> &I, const char *filename)
+{
+
+ try
+ {
+ vpImage<vpRGBa> Itmp ;
+
+ vpImageConvert::convert(I, Itmp) ;
+
+ vpImageIo::writePPM(Itmp, filename) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PPM P6) file.
+
+ \param I : Image to save as a (PPM P6) file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePPM(const vpImage<vpRGBa> &I, const char *filename)
+{
+
+ FILE* f;
+
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ f = fopen(filename, "wb");
+
+ if (f == NULL) {
+ vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+
+
+ fprintf(f,"P6\n"); // Magic number
+ fprintf(f,"%d %d\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(f,"%d\n",255); // Max level
+
+ for(unsigned int i=0;i<I.getHeight();i++)
+ {
+ for(unsigned int j=0;j<I.getWidth();j++)
+ {
+ vpRGBa P ;
+ size_t res ;
+ P = I[i][j] ;
+ unsigned char tmp ;
+ tmp = P.R ;
+ res = fwrite(&tmp,sizeof(tmp),1,f) ;
+ if (res==0)
+ {
+ fclose(f);
+ vpERROR_TRACE("couldn't write file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+ tmp = P.G;
+ res = fwrite(&tmp,sizeof(tmp),1,f) ;
+ if (res==0)
+ {
+ fclose(f);
+ vpERROR_TRACE("couldn't write file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+ tmp = P.B ;
+ res = fwrite(&tmp,sizeof(tmp),1,f) ;
+ if (res==0)
+ {
+ fclose(f);
+ vpERROR_TRACE("couldn't write file") ;
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+ }
+ }
+
+ fflush(f);
+ fclose(f);
+}
+
+
+/*!
+ Read a PGM P5 file and initialize a scalar image.
+
+ Read the contents of the portable gray pixmap (PGM P5) filename, allocate
+ memory for the corresponding image, and set the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::readPGM(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readPGM(I, filename.c_str());
+}
+
+/*!
+ Read a PGM P5 file and initialize a scalar image.
+
+ Read the contents of the portable gray pixmap (PGM P5) filename, allocate
+ memory for the corresponding image, and set the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readPGM(I, filename.c_str());
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::writePGM(const vpImage<unsigned char> &I,
+ const std::string filename)
+{
+ vpImageIo::writePGM(I, filename.c_str());
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+*/
+
+void
+vpImageIo::writePGM(const vpImage<short> &I, const std::string filename)
+{
+
+ vpImageIo::writePGM(I, filename.c_str());
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PGM P5) file.
+ Color image is converted into a grayscale image.
+
+ \param I : Image to save as a (PGM P5) file.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::writePGM(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writePGM(I, filename.c_str());
+}
+
+//--------------------------------------------------------------------------
+// PPM
+//--------------------------------------------------------------------------
+
+/*!
+ Read the contents of the portable pixmap (PPM P6) filename, allocate memory
+ for the corresponding gray level image, convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. The quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPPM(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readPPM(I, filename.c_str());
+}
+
+/*!
+ Read the contents of the portable pixmap (PPM P6) filename,
+ allocate memory for the corresponding vpRGBa image.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPPM(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readPPM(I, filename.c_str());
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PPM P6) file.
+ grayscale image is converted into a color image vpRGBa.
+
+ \param I : Image to save as a (PPM P6) file.
+ \param filename : Name of the file containing the image.
+
+*/
+
+void
+vpImageIo::writePPM(const vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::writePPM(I, filename.c_str());
+}
+
+/*!
+ Write the content of the bitmap in the file which name is given by \e
+ filename. This function writes a portable gray pixmap (PPM P6) file.
+
+ \param I : Image to save as a (PPM P6) file.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::writePPM(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writePPM(I, filename.c_str());
+}
+
+
+//--------------------------------------------------------------------------
+// JPEG
+//--------------------------------------------------------------------------
+
+#if defined(VISP_HAVE_JPEG)
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const char *filename)
+{
+ struct jpeg_compress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+ FILE *file;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_compress(&cinfo);
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "wb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ jpeg_stdio_dest(&cinfo, file);
+
+ cinfo.image_width = width;
+ cinfo.image_height = height;
+ cinfo.input_components = 1;
+ cinfo.in_color_space = JCS_GRAYSCALE;
+ jpeg_set_defaults(&cinfo);
+
+ jpeg_start_compress(&cinfo,TRUE);
+
+ unsigned char *line;
+ line = new unsigned char[width];
+ unsigned char* input = (unsigned char*)I.bitmap;
+ while (cinfo.next_scanline < cinfo.image_height)
+ {
+ for (unsigned int i = 0; i < width; i++)
+ {
+ line[i] = *(input);
+ input++;
+ }
+ jpeg_write_scanlines(&cinfo, &line, 1);
+ }
+
+ jpeg_finish_compress(&cinfo);
+ jpeg_destroy_compress(&cinfo);
+ delete [] line;
+ fclose(file);
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::writeJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const char *filename)
+{
+ struct jpeg_compress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+ FILE *file;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_compress(&cinfo);
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "wb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ jpeg_stdio_dest(&cinfo, file);
+
+ cinfo.image_width = width;
+ cinfo.image_height = height;
+ cinfo.input_components = 3;
+ cinfo.in_color_space = JCS_RGB;
+ jpeg_set_defaults(&cinfo);
+
+ jpeg_start_compress(&cinfo,TRUE);
+
+ unsigned char *line;
+ line = new unsigned char[3*width];
+ unsigned char* input = (unsigned char*)I.bitmap;
+ while (cinfo.next_scanline < cinfo.image_height)
+ {
+ for (unsigned int i = 0; i < width; i++)
+ {
+ line[i*3] = *(input); input++;
+ line[i*3+1] = *(input); input++;
+ line[i*3+2] = *(input); input++;
+ input++;
+ }
+ jpeg_write_scanlines(&cinfo, &line, 1);
+ }
+
+ jpeg_finish_compress(&cinfo);
+ jpeg_destroy_compress(&cinfo);
+ delete [] line;
+ fclose(file);
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writeJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Read the contents of the JPEG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readJPEG(vpImage<unsigned char> &I, const char *filename)
+{
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+ FILE *file;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_decompress(&cinfo);
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "rb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot read file")) ;
+ }
+
+ jpeg_stdio_src(&cinfo, file);
+ jpeg_read_header(&cinfo, TRUE);
+
+ unsigned int width = cinfo.image_width;
+ unsigned int height = cinfo.image_height;
+
+ if ( (width != I.getWidth()) || (height != I.getHeight()) )
+ I.resize(height,width);
+
+ jpeg_start_decompress(&cinfo);
+
+ unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
+ JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
+ ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
+
+ if (cinfo.out_color_space == JCS_RGB) {
+ vpImage<vpRGBa> Ic(height,width);
+ unsigned char* output = (unsigned char*)Ic.bitmap;
+ while (cinfo.output_scanline<cinfo.output_height) {
+ jpeg_read_scanlines(&cinfo,buffer,1);
+ for (unsigned int i = 0; i < width; i++) {
+ *(output++) = buffer[0][i*3];
+ *(output++) = buffer[0][i*3+1];
+ *(output++) = buffer[0][i*3+2];
+ *(output++) = 0;
+ }
+ }
+ vpImageConvert::convert(Ic,I) ;
+ }
+
+ else if (cinfo.out_color_space == JCS_GRAYSCALE)
+ {
+ unsigned int row;
+ while (cinfo.output_scanline<cinfo.output_height)
+ {
+ row = cinfo.output_scanline;
+ jpeg_read_scanlines(&cinfo,buffer,1);
+ memcpy(I[row], buffer[0], rowbytes);
+ }
+ }
+
+ jpeg_finish_decompress(&cinfo);
+ jpeg_destroy_decompress(&cinfo);
+ fclose(file);
+}
+
+
+/*!
+ Read the contents of the JPEG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Read a JPEG file and initialize a scalar image.
+
+ Read the contents of the JPEG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readJPEG(vpImage<vpRGBa> &I, const char *filename)
+{
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+ FILE *file;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_decompress(&cinfo);
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "rb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot read file")) ;
+ }
+
+ jpeg_stdio_src(&cinfo, file);
+
+ jpeg_read_header(&cinfo, TRUE);
+
+ unsigned int width = cinfo.image_width;
+ unsigned int height = cinfo.image_height;
+
+ if ( (width != I.getWidth()) || (height != I.getHeight()) )
+ I.resize(height,width);
+
+ jpeg_start_decompress(&cinfo);
+
+ unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
+ JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
+ ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
+
+ if (cinfo.out_color_space == JCS_RGB)
+ {
+ unsigned char* output = (unsigned char*)I.bitmap;
+ while (cinfo.output_scanline<cinfo.output_height)
+ {
+ jpeg_read_scanlines(&cinfo,buffer,1);
+ for (unsigned int i = 0; i < width; i++) {
+ *(output++) = buffer[0][i*3];
+ *(output++) = buffer[0][i*3+1];
+ *(output++) = buffer[0][i*3+2];
+ *(output++) = 0;
+ }
+ }
+ }
+
+ else if (cinfo.out_color_space == JCS_GRAYSCALE)
+ {
+ vpImage<unsigned char> Ig(height,width);
+
+ unsigned int row;
+ while (cinfo.output_scanline<cinfo.output_height)
+ {
+ row = cinfo.output_scanline;
+ jpeg_read_scanlines(&cinfo,buffer,1);
+ memcpy(Ig[row], buffer[0], rowbytes);
+ }
+
+ vpImageConvert::convert(Ig,I) ;
+ }
+
+ jpeg_finish_decompress(&cinfo);
+ jpeg_destroy_decompress(&cinfo);
+ fclose(file);
+}
+
+
+/*!
+ Read a JPEG file and initialize a scalar image.
+
+ Read the contents of the JPEG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readJPEG(I, filename.c_str());
+}
+
+#elif defined(VISP_HAVE_OPENCV)
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip;
+ vpImageConvert::convert(I, Ip);
+ cv::imwrite(filename, Ip);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(I, Ip);
+
+ cvSaveImage(filename, Ip);
+
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::writeJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip;
+ vpImageConvert::convert(I, Ip);
+ cv::imwrite(filename, Ip);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(I, Ip);
+
+ cvSaveImage(filename, Ip);
+
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a JPEG file.
+
+ \param I : Image to save as a JPEG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writeJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Read the contents of the JPEG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readJPEG(vpImage<unsigned char> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#else
+ IplImage* Ip = NULL;
+ Ip = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if (Ip != NULL)
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError,
+ "Can't read the image")) ;
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Read the contents of the JPEG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readJPEG(I, filename.c_str());
+}
+
+
+/*!
+ Read a JPEG file and initialize a scalar image.
+
+ Read the contents of the JPEG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readJPEG(vpImage<vpRGBa> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#else
+ IplImage* Ip = NULL;
+ Ip = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
+ if (Ip != NULL)
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Read a JPEG file and initialize a scalar image.
+
+ Read the contents of the JPEG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readJPEG(I, filename.c_str());
+}
+
+#endif
+
+
+
+
+
+
+//--------------------------------------------------------------------------
+// PNG
+//--------------------------------------------------------------------------
+
+#if defined(VISP_HAVE_PNG)
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<unsigned char> &I, const char *filename)
+{
+ FILE *file;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "wb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ /* create a png info struct */
+ png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING,NULL, NULL, NULL);
+ if (!png_ptr)
+ {
+ fclose (file);
+ vpERROR_TRACE("Error during png_create_write_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ png_infop info_ptr = png_create_info_struct(png_ptr);
+ if (!info_ptr)
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, NULL);
+ vpERROR_TRACE("Error during png_create_info_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ /* initialize the setjmp for returning properly after a libpng error occured */
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+ vpERROR_TRACE("Error during init_io\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ /* setup libpng for using standard C fwrite() function with our FILE pointer */
+ png_init_io (png_ptr, file);
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+ int bit_depth = 8;
+ int color_type = PNG_COLOR_TYPE_GRAY;
+ /* set some useful information from header */
+
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+ vpERROR_TRACE("Error during write header\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ png_set_IHDR(png_ptr, info_ptr, width, height,
+ bit_depth, color_type, PNG_INTERLACE_NONE,
+ PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
+
+ png_write_info(png_ptr, info_ptr);
+
+ png_bytep* row_ptrs = new png_bytep[height];
+ for (unsigned int i = 0; i < height; i++)
+ row_ptrs[i] = new png_byte[width];
+
+ unsigned char* input = (unsigned char*)I.bitmap;
+
+ for (unsigned int i = 0; i < height; i++)
+ {
+ png_byte* row = row_ptrs[i];
+ for(unsigned int j = 0; j < width; j++)
+ {
+ row[j] = *(input);
+ input++;
+ }
+ }
+
+ png_write_image(png_ptr, row_ptrs);
+
+ png_write_end(png_ptr, NULL);
+
+ for(unsigned int j = 0; j < height; j++)
+ delete[] row_ptrs[j];
+
+ delete[] row_ptrs;
+
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+
+ fclose(file);
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::writePNG(I, filename.c_str());
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<vpRGBa> &I, const char *filename)
+{
+ FILE *file;
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "wb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot write file")) ;
+ }
+
+ /* create a png info struct */
+ png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING,NULL, NULL, NULL);
+ if (!png_ptr)
+ {
+ fclose (file);
+ vpERROR_TRACE("Error during png_create_write_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ png_infop info_ptr = png_create_info_struct(png_ptr);
+ if (!info_ptr)
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, NULL);
+ vpERROR_TRACE("Error during png_create_info_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ /* initialize the setjmp for returning properly after a libpng error occured */
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+ vpERROR_TRACE("Error during init_io\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ /* setup libpng for using standard C fwrite() function with our FILE pointer */
+ png_init_io (png_ptr, file);
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+ int bit_depth = 8;
+ int color_type = PNG_COLOR_TYPE_RGB;
+ /* set some useful information from header */
+
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+ vpERROR_TRACE("Error during write header\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG write error")) ;
+ }
+
+ png_set_IHDR(png_ptr, info_ptr, width, height,
+ bit_depth, color_type, PNG_INTERLACE_NONE,
+ PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
+
+ png_write_info(png_ptr, info_ptr);
+
+ png_bytep* row_ptrs = new png_bytep[height];
+ for (unsigned int i = 0; i < height; i++)
+ row_ptrs[i] = new png_byte[3*width];
+
+ unsigned char* input = (unsigned char*)I.bitmap;;
+
+ for (unsigned int i = 0; i < height; i++)
+ {
+ png_byte* row = row_ptrs[i];
+ for(unsigned int j = 0; j < width; j++)
+ {
+ row[3*j] = *(input);input++;
+ row[3*j+1] = *(input);input++;
+ row[3*j+2] = *(input);input++;
+ input++;
+ }
+ }
+
+ png_write_image(png_ptr, row_ptrs);
+
+ png_write_end(png_ptr, NULL);
+
+ for(unsigned int j = 0; j < height; j++)
+ delete[] row_ptrs[j];
+
+ delete[] row_ptrs;
+
+ png_destroy_write_struct (&png_ptr, &info_ptr);
+
+ fclose(file);
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writePNG(I, filename.c_str());
+}
+
+/*!
+ Read the contents of the PNG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPNG(vpImage<unsigned char> &I, const char *filename)
+{
+ FILE *file;
+ png_byte magic[8];
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "rb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot read file")) ;
+ }
+
+ /* read magic number */
+ if (fread (magic, 1, sizeof (magic), file) != sizeof (magic))
+ {
+ fclose (file);
+ vpERROR_TRACE("couldn't read magic number in file \"%s\"\n", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "error reading png file")) ;
+ }
+
+ /* check for valid magic number */
+ if (png_sig_cmp (magic,0, sizeof (magic)))
+ {
+ fprintf (stderr, "error: \"%s\" is not a valid PNG image!\n",filename);
+ fclose (file);
+ throw (vpImageException(vpImageException::ioError,
+ "error reading png file")) ;
+ }
+
+ /* create a png read struct */
+ //printf("version %s\n", PNG_LIBPNG_VER_STRING);
+ png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ if (png_ptr == NULL)
+ {
+ fprintf (stderr, "error: can't create a png read structure!\n");
+ fclose (file);
+ throw (vpImageException(vpImageException::ioError,
+ "error reading png file")) ;
+ }
+
+ /* create a png info struct */
+ png_infop info_ptr = png_create_info_struct (png_ptr);
+ if (info_ptr == NULL)
+ {
+ fprintf (stderr, "error: can't create a png info structure!\n");
+ fclose (file);
+ png_destroy_read_struct (&png_ptr, NULL, NULL);
+ throw (vpImageException(vpImageException::ioError,
+ "error reading png file")) ;
+ }
+
+ /* initialize the setjmp for returning properly after a libpng error occured */
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ vpERROR_TRACE("Error during init io\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG read error")) ;
+ }
+
+ /* setup libpng for using standard C fread() function with our FILE pointer */
+ png_init_io (png_ptr, file);
+
+ /* tell libpng that we have already read the magic number */
+ png_set_sig_bytes (png_ptr, sizeof (magic));
+
+ /* read png info */
+ png_read_info (png_ptr, info_ptr);
+
+ unsigned int width = png_get_image_width(png_ptr, info_ptr);
+ unsigned int height = png_get_image_height(png_ptr, info_ptr);
+
+ unsigned int bit_depth, channels, color_type;
+ /* get some useful information from header */
+ bit_depth = png_get_bit_depth (png_ptr, info_ptr);
+ channels = png_get_channels(png_ptr, info_ptr);
+ color_type = png_get_color_type (png_ptr, info_ptr);
+
+ /* convert index color images to RGB images */
+ if (color_type == PNG_COLOR_TYPE_PALETTE)
+ png_set_palette_to_rgb (png_ptr);
+
+ /* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
+ if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
+ png_set_expand (png_ptr);
+
+// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
+// png_set_tRNS_to_alpha (png_ptr);
+
+ if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
+ png_set_strip_alpha(png_ptr);
+
+ if (bit_depth == 16)
+ png_set_strip_16 (png_ptr);
+ else if (bit_depth < 8)
+ png_set_packing (png_ptr);
+
+ /* update info structure to apply transformations */
+ png_read_update_info (png_ptr, info_ptr);
+
+ channels = png_get_channels(png_ptr, info_ptr);
+
+ if ( (width != I.getWidth()) || (height != I.getHeight()) )
+ I.resize(height,width);
+
+ png_bytep* rowPtrs = new png_bytep[height];
+
+ unsigned int stride = width * bit_depth * channels / 8;
+ unsigned char* data = new unsigned char[stride * height];
+
+ for (unsigned int i =0; i < height; i++)
+ rowPtrs[i] = (png_bytep)data + (i * stride);
+
+ png_read_image(png_ptr, rowPtrs);
+
+ vpImage<vpRGBa> Ic(height,width);
+ unsigned char* output;
+
+ switch (channels)
+ {
+ case 1:
+ output = (unsigned char*)I.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i];
+ }
+ break;
+ case 2:
+ output = (unsigned char*)I.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*2];
+ }
+ break;
+ case 3:
+
+ output = (unsigned char*)Ic.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*3];
+ *(output++) = data[i*3+1];
+ *(output++) = data[i*3+2];
+ *(output++) = 0;
+ }
+ vpImageConvert::convert(Ic,I) ;
+ break;
+ case 4:
+ output = (unsigned char*)Ic.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*4];
+ *(output++) = data[i*4+1];
+ *(output++) = data[i*4+2];
+ *(output++) = data[i*4+3];
+ }
+ vpImageConvert::convert(Ic,I) ;
+ break;
+ }
+
+ delete [] (png_bytep)rowPtrs;
+ delete [] data;
+ png_read_end (png_ptr, NULL);
+ png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ fclose(file);
+}
+
+
+/*!
+ Read the contents of the PNG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readPNG(I, filename.c_str());
+}
+
+
+/*!
+ Read a PNG file and initialize a scalar image.
+
+ Read the contents of the PNG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPNG(vpImage<vpRGBa> &I, const char *filename)
+{
+ FILE *file;
+ png_byte magic[8];
+
+ // Test the filename
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("no filename\n");
+ throw (vpImageException(vpImageException::ioError,
+ "no filename")) ;
+ }
+
+ file = fopen(filename, "rb");
+
+ if (file == NULL) {
+ vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
+ throw (vpImageException(vpImageException::ioError,
+ "cannot read file")) ;
+ }
+
+ /* read magic number */
+ if (fread (magic, 1, sizeof (magic), file) != sizeof (magic))
+ {
+ fclose (file);
+ vpERROR_TRACE("couldn't read magic number in file \"%s\"\n", filename) ;
+ throw (vpImageException(vpImageException::ioError,
+ "error reading pgm file")) ;
+ }
+
+ /* check for valid magic number */
+ if (png_sig_cmp (magic,0, sizeof (magic)))
+ {
+ fclose (file);
+ vpERROR_TRACE("error: \"%s\" is not a valid PNG image!\n",filename);
+ throw (vpImageException(vpImageException::ioError,
+ "PNG read error")) ;
+ }
+
+ /* create a png read struct */
+ png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ if (!png_ptr)
+ {
+ fclose (file);
+ vpERROR_TRACE("Error during png_create_read_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG read error")) ;
+ }
+
+ /* create a png info struct */
+ png_infop info_ptr = png_create_info_struct (png_ptr);
+ if (!info_ptr)
+ {
+ fclose (file);
+ png_destroy_read_struct (&png_ptr, NULL, NULL);
+ vpERROR_TRACE("Error during png_create_info_struct()\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG read error")) ;
+ }
+
+ /* initialize the setjmp for returning properly after a libpng error occured */
+ if (setjmp (png_jmpbuf (png_ptr)))
+ {
+ fclose (file);
+ png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ vpERROR_TRACE("Error during init io\n");
+ throw (vpImageException(vpImageException::ioError,
+ "PNG read error")) ;
+ }
+
+ /* setup libpng for using standard C fread() function with our FILE pointer */
+ png_init_io (png_ptr, file);
+
+ /* tell libpng that we have already read the magic number */
+ png_set_sig_bytes (png_ptr, sizeof (magic));
+
+ /* read png info */
+ png_read_info (png_ptr, info_ptr);
+
+ unsigned int width = png_get_image_width(png_ptr, info_ptr);
+ unsigned int height = png_get_image_height(png_ptr, info_ptr);
+
+ unsigned int bit_depth, channels, color_type;
+ /* get some useful information from header */
+ bit_depth = png_get_bit_depth (png_ptr, info_ptr);
+ channels = png_get_channels(png_ptr, info_ptr);
+ color_type = png_get_color_type (png_ptr, info_ptr);
+
+ /* convert index color images to RGB images */
+ if (color_type == PNG_COLOR_TYPE_PALETTE)
+ png_set_palette_to_rgb (png_ptr);
+
+ /* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
+ if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
+ png_set_expand (png_ptr);
+
+// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
+// png_set_tRNS_to_alpha (png_ptr);
+
+ if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
+ png_set_strip_alpha(png_ptr);
+
+ if (bit_depth == 16)
+ png_set_strip_16 (png_ptr);
+ else if (bit_depth < 8)
+ png_set_packing (png_ptr);
+
+ /* update info structure to apply transformations */
+ png_read_update_info (png_ptr, info_ptr);
+
+ channels = png_get_channels(png_ptr, info_ptr);
+
+ if ( (width != I.getWidth()) || (height != I.getHeight()) )
+ I.resize(height,width);
+
+ png_bytep* rowPtrs = new png_bytep[height];
+
+ unsigned int stride = width * bit_depth * channels / 8;
+ unsigned char* data = new unsigned char[stride * height];
+
+
+ for (unsigned int i =0; i < height; i++)
+ rowPtrs[i] = (png_bytep)data + (i * stride);
+
+ png_read_image(png_ptr, rowPtrs);
+
+ vpImage<unsigned char> Ig(height,width);
+ unsigned char* output;
+
+ switch (channels)
+ {
+ case 1:
+ output = (unsigned char*)Ig.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i];
+ }
+ vpImageConvert::convert(Ig,I) ;
+ break;
+ case 2:
+ output = (unsigned char*)Ig.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*2];
+ }
+ vpImageConvert::convert(Ig,I) ;
+ break;
+ case 3:
+
+ output = (unsigned char*)I.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*3];
+ *(output++) = data[i*3+1];
+ *(output++) = data[i*3+2];
+ *(output++) = 0;
+ }
+ break;
+ case 4:
+ output = (unsigned char*)I.bitmap;
+ for (unsigned int i = 0; i < width*height; i++)
+ {
+ *(output++) = data[i*4];
+ *(output++) = data[i*4+1];
+ *(output++) = data[i*4+2];
+ *(output++) = data[i*4+3];
+ }
+ break;
+ }
+
+ delete [] (png_bytep)rowPtrs;
+ delete [] data;
+ png_read_end (png_ptr, NULL);
+ png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ fclose(file);
+}
+
+
+/*!
+ Read a PNG file and initialize a scalar image.
+
+ Read the contents of the PNG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readPNG(I, filename.c_str());
+}
+
+#elif defined(VISP_HAVE_OPENCV)
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<unsigned char> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip;
+ vpImageConvert::convert(I, Ip);
+ cv::imwrite(filename, Ip);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(I, Ip);
+
+ cvSaveImage(filename, Ip);
+
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::writePNG(I, filename.c_str());
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<vpRGBa> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip;
+ vpImageConvert::convert(I, Ip);
+ cv::imwrite(filename, Ip);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(I, Ip);
+
+ cvSaveImage(filename, Ip);
+
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Write the content of the image bitmap in the file which name is given by \e
+ filename. This function writes a PNG file.
+
+ \param I : Image to save as a PNG file.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::writePNG(I, filename.c_str());
+}
+
+
+/*!
+ Read the contents of the PNG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPNG(vpImage<unsigned char> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#else
+ IplImage* Ip = NULL;
+ Ip = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if (Ip != NULL)
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError,
+ "Can't read the image")) ;
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Read the contents of the PNG file, allocate memory
+ for the corresponding gray level image, if necessary convert the data in gray level, and
+ set the bitmap whith the gray level data. That means that the image \e I is a
+ "black and white" rendering of the original image in \e filename, as in a
+ black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
+ 0,587 g + 0,114 b\f$.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ \param I : Image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+
+*/
+void
+vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string filename)
+{
+ vpImageIo::readPNG(I, filename.c_str());
+}
+
+
+/*!
+ Read a PNG file and initialize a scalar image.
+
+ Read the contents of the PNG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPNG(vpImage<vpRGBa> &I, const char *filename)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if ( ! Ip.empty())
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
+#else
+ IplImage* Ip = NULL;
+ Ip = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
+ if (Ip != NULL)
+ vpImageConvert::convert(Ip, I);
+ else
+ throw (vpImageException(vpImageException::ioError,
+ "Can't read the image")) ;
+ cvReleaseImage(&Ip);
+#endif
+}
+
+
+/*!
+ Read a PNG file and initialize a scalar image.
+
+ Read the contents of the PNG file, allocate
+ memory for the corresponding image, and set
+ the bitmap whith the content of
+ the file.
+
+ If the image has been already initialized, memory allocation is done
+ only if the new image size is different, else we re-use the same
+ memory space.
+
+ If the file corresponds to a grayscaled image, a conversion is done to deal
+ with \e I which is a color image.
+
+ \param I : Color image to set with the \e filename content.
+ \param filename : Name of the file containing the image.
+*/
+void
+vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string filename)
+{
+ vpImageIo::readPNG(I, filename.c_str());
+}
+
+#endif
diff --git a/modules/io/src/parallel-port/vpParallelPort.cpp b/modules/io/src/parallel-port/vpParallelPort.cpp
new file mode 100644
index 0000000..ddbf4ec
--- /dev/null
+++ b/modules/io/src/parallel-port/vpParallelPort.cpp
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Parallel port management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_PARPORT
+
+# include <sys/types.h>
+# include <sys/stat.h>
+# include <fcntl.h>
+# include <sys/ioctl.h>
+# include <unistd.h>
+
+# include <visp3/io/vpParallelPort.h>
+
+/*!
+ \file vpParallelPort.cpp
+ \brief Parallel port management under unix.
+*/
+
+
+
+static unsigned char vpParallelPortData;
+
+/*!
+
+ Constructor to acces to a parallel port.
+
+ Open and initialise the parallel port by sending 0 to the data bus.
+
+ Set the default device to "/dev/parport0".
+
+ \exception vpParallelPortException::opening If the device used to access to
+ the parallel port can't be opened. A possible reason is that you don't have
+ write access.
+
+*/
+vpParallelPort::vpParallelPort() : fd(0), device()
+{
+ sprintf(device, "/dev/parport0");
+
+ this->open();
+
+ unsigned char data = 0;
+ this->sendData(data);
+}
+
+/*!
+ Destructor to close the parallel port.
+
+ \exception vpParallelPortException::closing If the device used to access to
+ the parallel port can't be closed.
+*/
+vpParallelPort::~vpParallelPort()
+{
+ this->close();
+}
+
+/*!
+ Open the parallel port with write access and initialise it.
+
+ \exception vpParallelPortException::opening If the device used to access to
+ the parallel port can't be opened. A possible reason is that you don't have
+ write access.
+
+ \sa close()
+*/
+void vpParallelPort::open()
+{
+ fd = ::open("/dev/parport0", O_WRONLY);
+ if (fd < 0) {
+ printf("Can't open /dev/parport0\n");
+ printf("Check if you have write access to /dev/parport0\n");
+ perror("Open parallel port");
+ throw(vpParallelPortException(vpParallelPortException::opening,
+ "Can't open /dev/parport0")) ;
+ }
+
+ int i;
+
+ ioctl(fd, PPCLAIM);
+ i = PARPORT_MODE_COMPAT;
+ ioctl(fd, PPSETMODE, & i);
+ i = IEEE1284_MODE_COMPAT;
+ ioctl(fd, PPNEGOT, & i);
+}
+
+/*!
+ Send a data to the parallel port.
+
+ \param data : Value [D7, D6, ... D0] to send to the data bus.
+
+ The code bellow shows how to set D0 and D2 to logical level 1:
+
+ \code
+ vpParallelPort parport;
+
+ unsigned char data = 5; // 0x00000101 = 5 in decimal
+ parport.sendData(data); // D0 and D2 are set to logical level 1
+
+ \endcode
+
+ \return true if the device was close, false if an error occurs.
+*/
+void vpParallelPort::sendData(unsigned char &data)
+{
+ ioctl(fd, PPWDATA, & data);
+
+ // Memorise the last sended data to the static variable
+ vpParallelPortData = data;
+}
+
+/*!
+
+ Get the last data sent to the parallel port.
+
+*/
+unsigned char vpParallelPort::getData()
+{
+ return vpParallelPortData;
+}
+
+/*!
+ Close the parallel port.
+
+ \exception vpParallelPortException::closing If the device used to access to
+ the parallel port can't be closed.
+
+*/
+void vpParallelPort::close()
+{
+ ioctl(fd, PPRELEASE);
+
+ int err;
+ err = ::close(fd);
+
+ if (err !=0) {
+ printf("Can't close the parallel port\n");
+ throw(vpParallelPortException(vpParallelPortException::closing,
+ "Can't close the parallel port")) ;
+ }
+
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpParallelPort.cpp.o) has no symbols
+void dummy_vpParallelPort() {};
+#endif
+
diff --git a/modules/io/src/tools/vpKeyboard.cpp b/modules/io/src/tools/vpKeyboard.cpp
new file mode 100644
index 0000000..8d42f8c
--- /dev/null
+++ b/modules/io/src/tools/vpKeyboard.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Keybord management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+# include <stdio.h>
+# include <visp3/io/vpKeyboard.h>
+
+/*!
+ \file vpKeyboard.cpp
+ \brief Keybord management under unix.
+*/
+
+
+/*!
+ Activates the raw mode to read keys in an non blocking way.
+*/
+vpKeyboard::vpKeyboard() : initial_settings(), new_settings()
+{
+ init();
+}
+
+/*!
+ Stops the raw mode.
+*/
+vpKeyboard::~vpKeyboard()
+{
+ end();
+}
+
+/*!
+
+ Get the hit key. kbhit() indicates if a key was hitten.
+*/
+int
+vpKeyboard::getchar()
+{
+ int c;
+ c = ::getchar();
+ return c;
+}
+
+/*!
+
+
+ \return 1 : if a key was hit.
+*/
+int
+vpKeyboard::kbhit()
+{
+ struct timeval tv = { 0, 0 };
+ fd_set readfds;
+
+ FD_ZERO(&readfds);
+ FD_SET(STDIN_FILENO, &readfds);
+
+ return select(STDIN_FILENO + 1, &readfds, NULL, NULL, &tv) == 1;
+}
+
+/*!
+
+ Activates the raw mode to read keys in an non blocking way.
+*/
+void
+vpKeyboard::init()
+{
+ setRawMode(true);
+}
+
+/*!
+
+ Stops the raw mode.
+*/
+void
+vpKeyboard::end()
+{
+ setRawMode(false);
+}
+
+/*!
+ Turns on/off the 'raw' mode.
+
+ \param active : true to activate the raw mode, false to turn it off.
+
+ If raw mode is active, there is no need to press return to get a key.
+*/
+void
+vpKeyboard::setRawMode(bool active)
+{
+ if (active) {
+
+ tcgetattr(STDIN_FILENO, &initial_settings);
+
+ new_settings = initial_settings;
+ // cfmakeraw(&new_settings);
+ new_settings = initial_settings;
+ new_settings.c_lflag &= (unsigned int)~ICANON;
+ new_settings.c_lflag &= (unsigned int)~ECHO;
+ new_settings.c_lflag &= (unsigned int)~ISIG;
+ //new_settings.c_oflag &= (unsigned int)~NL0;
+ //new_settings.c_oflag &= (unsigned int)~CR0;
+ new_settings.c_oflag &= (unsigned int)~TAB0;
+ //new_settings.c_oflag &= (unsigned int)~BS0;
+ new_settings.c_cc[VMIN] = 1;
+ new_settings.c_cc[VTIME] = 0;
+ tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
+
+ }
+ else {
+ tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings);
+ }
+}
+
+#endif // defined UNIX
diff --git a/modules/io/src/tools/vpParseArgv.cpp b/modules/io/src/tools/vpParseArgv.cpp
new file mode 100644
index 0000000..0cca560
--- /dev/null
+++ b/modules/io/src/tools/vpParseArgv.cpp
@@ -0,0 +1,576 @@
+/****************************************************************************
+ *
+ * This file contains a procedure that handles table-based
+ * argv-argc parsing.
+ *
+ * Copyright 1990 Regents of the University of California
+ * Permission to use, copy, modify, and distribute this
+ * software and its documentation for any purpose and without
+ * fee is hereby granted, provided that the above copyright
+ * notice appear in all copies. The University of California
+ * makes no representations about the suitability of this
+ * software for any purpose. It is provided "as is" without
+ * express or implied warranty.
+ *
+ * This file has been modified to not rely on tcl, tk or X11.
+ * Based on tkArgv.c from tk2.3 :
+ *
+ * Modifications by Peter Neelin (November 27, 1992)
+ * Modifications by Fabien Spindler (June 20, 2006)
+ */
+
+/*!
+ \file vpParseArgv.cpp
+ \brief Command line argument parsing.
+*/
+
+
+
+#include <visp3/io/vpParseArgv.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include <ctype.h>
+
+/*
+ * Default table of argument descriptors. These are normally available
+ * in every application.
+ */
+
+vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = {
+ {"-help", ARGV_HELP, (char *) NULL, (char *) NULL,
+ "Print summary of command-line options and abort.\n"},
+ {NULL, ARGV_END, (char *) NULL, (char *) NULL,
+ (char *) NULL}
+};
+
+int (*handlerProc1)(const char *dst, const char *key, const char *argument);
+int (*handlerProc2)(const char *dst, const char *key, int valargc, const char **argument);
+
+
+/*!
+ Process an argv array according to a table of expectedvcommand-line options.
+
+ The return value is a boolean value with true indicating an
+ error. If an error occurs then an error message is printed on
+ stderr. Under normal conditions, both *argcPtr and *argv are
+ modified to return the arguments that couldn't be processed here
+ (they didn't match the option table, or followed an
+ vpParseArgv::ARGV_REST argument).
+
+ \param argcPtr: Pointer to the count of command line arguments.
+
+ \param argv: Array of command line argument strings.
+
+ \param argTable: Array of command-specific argument descriptions.
+
+ \param flags: This parameter is to set with vpParseArgv::vpArgvFlags
+ values or combinations of these values using the OR operator
+ (vpParseArgv::ARGV_NO_LEFTOVERS | vpParseArgv::ARGV_NO_DEFAULTS). If
+ the vpParseArgv::ARGV_NO_DEFAULTS bit is set, then
+ don't generate information for default options.
+*/
+bool
+vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable,
+ int flags)
+
+{
+ register vpArgvInfo *infoPtr; /* Pointer to the current entry in the
+ * table of argument descriptions. */
+ vpArgvInfo *matchPtr; /* Descriptor that matches current argument. */
+ const char *curArg; /* Current argument */
+ register char c; /* Second character of current arg (used for
+ * quick check for matching; use 2nd char.
+ * because first char. will almost always
+ * be '-'). */
+ int srcIndex; /* Location from which to read next argument
+ * from argv. */
+ int dstIndex; /* Index into argv to which next unused
+ * argument should be copied (never greater
+ * than srcIndex). */
+ int argc; /* # arguments in argv still to process. */
+ size_t length; /* Number of characters in current argument. */
+ unsigned long long nargs; /* Number of following arguments to get. */
+
+/* Macro to optionally print errors */
+#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
+
+ if (flags & ARGV_DONT_SKIP_FIRST_ARG) {
+ srcIndex = dstIndex = 0;
+ argc = *argcPtr;
+ } else {
+ srcIndex = dstIndex = 1;
+ argc = *argcPtr-1;
+ }
+
+ while (argc > 0) {
+ curArg = argv[srcIndex];
+ srcIndex++;
+ argc--;
+ c = curArg[1];
+ length = strlen(curArg);
+
+ /*
+ * Loop throught the argument descriptors searching for one with
+ * the matching key string. If found, leave a pointer to it in
+ * matchPtr.
+ */
+
+ matchPtr = NULL;
+ for (unsigned int i = 0; i < 2; i++) {
+ if (i == 0) {
+ infoPtr = argTable;
+ } else {
+ infoPtr = defaultTable;
+ }
+ for (; infoPtr->type != ARGV_END; infoPtr++) {
+ if (infoPtr->key == NULL) {
+ continue;
+ }
+ if ((infoPtr->key[1] != c)
+ || (strncmp(infoPtr->key, curArg, length) != 0)) {
+ continue;
+ }
+ if (infoPtr->key[length] == 0) {
+ matchPtr = infoPtr;
+ goto gotMatch;
+ }
+ if (flags & ARGV_NO_ABBREV) {
+ continue;
+ }
+ if (matchPtr != NULL) {
+ FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg);
+ return true;
+ }
+ matchPtr = infoPtr;
+ }
+ }
+ if (matchPtr == NULL) {
+
+ /*
+ * Unrecognized argument. Just copy it down, unless the caller
+ * prefers an error to be registered.
+ */
+
+ if (flags & ARGV_NO_LEFTOVERS) {
+ FPRINTF(stderr, "unrecognized argument \"%s\"\n", curArg);
+ }
+ argv[dstIndex] = curArg;
+ dstIndex++;
+ continue;
+ }
+
+ /*
+ * Take the appropriate action based on the option type
+ */
+ gotMatch:
+ infoPtr = matchPtr;
+ switch (infoPtr->type) {
+ case ARGV_CONSTANT:
+ *((int *) infoPtr->dst) = 1;
+ break;
+ case ARGV_INT:
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long i=0; i<nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr=NULL;
+
+ *(((int *) infoPtr->dst)+i) =
+ strtol(argv[srcIndex], &endPtr, 0);
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr,
+ "expected integer argument for \"%s\" but got \"%s\"\n",
+ infoPtr->key, argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_LONG:
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long i=0; i<nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr=NULL;
+
+ *(((long *) infoPtr->dst)+i) =
+ strtol(argv[srcIndex], &endPtr, 0);
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr,
+ "expected long argument for \"%s\" but got \"%s\"\n",
+ infoPtr->key, argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_STRING:
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long i=0; i<nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ *(((const char **)infoPtr->dst)+i) = argv[srcIndex];
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_REST:
+ *((int *) infoPtr->dst) = dstIndex;
+ goto argsDone;
+ case ARGV_FLOAT:
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long i=0; i<nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr;
+
+ *(((float *) infoPtr->dst)+i) =
+ (float)strtod(argv[srcIndex], &endPtr); // Here we use strtod
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr,
+ "expected floating-point argument for \"%s\" but got\"%s\"\n",
+ infoPtr->key, argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_DOUBLE:
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long i=0; i<nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr;
+
+ *(((double *) infoPtr->dst)+i) =
+ strtod(argv[srcIndex], &endPtr);
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr,
+ "expected double-point argument for \"%s\" but got\"%s\"\n",
+ infoPtr->key, argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+
+ case ARGV_FUNC: {
+ handlerProc1 = (int (*)(const char *dst, const char *key, const char *argument))infoPtr->src;
+
+ if ((*handlerProc1)(infoPtr->dst, infoPtr->key, argv[srcIndex]))
+ {
+ srcIndex += 1;
+ argc -= 1;
+ }
+ break;
+ }
+ case ARGV_GENFUNC: {
+ handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, const char **argument))infoPtr->src;
+
+ argc = (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv+srcIndex);
+ if (argc < 0) {
+ return true;
+ }
+ break;
+ }
+
+ case ARGV_HELP:
+ printUsage (argTable, flags);
+ return true;
+ case ARGV_END:
+ default:
+ FPRINTF(stderr, "bad argument type %d in vpArgvInfo",
+ infoPtr->type);
+ return true;
+ }
+ }
+
+ /*
+ * If we broke out of the loop because of an OPT_REST argument,
+ * copy the remaining arguments down.
+ */
+
+ argsDone:
+ while (argc) {
+ argv[dstIndex] = argv[srcIndex];
+ srcIndex++;
+ dstIndex++;
+ argc--;
+ }
+ argv[dstIndex] = (char *) NULL;
+ *argcPtr = dstIndex;
+ return false;
+
+ missingArg:
+ FPRINTF(stderr, "\"%s\" option requires an additional argument\n", curArg);
+ return true;
+
+#undef FPRINTF
+}
+
+/*!
+ Generate a help string describing command-line options.
+
+ Prints on stderr (unless vpParseArgv::ARGV_NO_PRINT is specified in
+ flags) a help string describing all the options in argTable, plus
+ all those in the default table unless vpParseArgv::ARGV_NO_DEFAULTS
+ is specified in flags.
+
+ \param argTable: Array of command-specific argument.descriptions.
+
+ \param flags: If the vpParseArgv::ARGV_NO_DEFAULTS bit is set in
+ this word, then don't generate information for default options.
+
+*/
+void
+vpParseArgv::printUsage(vpArgvInfo * argTable, int flags)
+{
+ register vpArgvInfo *infoPtr;
+ int width;
+ int numSpaces;
+#define NUM_SPACES 20
+ static char spaces[] = " ";
+/* char tmp[30]; */
+ unsigned long long nargs;
+
+/* Macro to optionally print errors */
+#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
+
+ /*
+ * First, compute the width of the widest option key, so that we
+ * can make everything line up.
+ */
+
+ width = 4;
+ for (unsigned int i = 0; i < 2; i++) {
+ for (infoPtr = i ? defaultTable : argTable;
+ infoPtr->type != ARGV_END; infoPtr++) {
+ int length;
+ if (infoPtr->key == NULL) {
+ continue;
+ }
+ length = (int)strlen(infoPtr->key);
+ if (length > width) {
+ width = length;
+ }
+ }
+ }
+
+ FPRINTF(stderr, "Command-specific options:");
+ for (unsigned int i = 0; ; i++) {
+ for (infoPtr = i ? defaultTable : argTable;
+ infoPtr->type != ARGV_END; infoPtr++) {
+ if ((infoPtr->type == ARGV_HELP) && (infoPtr->key == NULL)) {
+ FPRINTF(stderr, "\n%s", infoPtr->help);
+ continue;
+ }
+ FPRINTF(stderr, "\n %s:", infoPtr->key);
+ numSpaces = width + 1 - (int)strlen(infoPtr->key);
+ while (numSpaces > 0) {
+ if (numSpaces >= NUM_SPACES) {
+ FPRINTF(stderr, "%s",spaces);
+ } else {
+ FPRINTF(stderr, "%s",spaces+NUM_SPACES-numSpaces);
+ }
+ numSpaces -= NUM_SPACES;
+ }
+ FPRINTF(stderr, "%s",infoPtr->help);
+ switch (infoPtr->type) {
+ case ARGV_INT: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long j=0; j<nargs; j++) {
+ FPRINTF(stderr, " %d", *(((int *) infoPtr->dst)+j));
+ }
+ break;
+ }
+ case ARGV_LONG: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long j=0; j<nargs; j++) {
+ FPRINTF(stderr, " %ld", *(((long *) infoPtr->dst)+j));
+ }
+ break;
+ }
+ case ARGV_FLOAT: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long j=0; j<nargs; j++) {
+ FPRINTF(stderr, " %f", *(((float *) infoPtr->dst)+j));
+ }
+ break;
+ }
+ case ARGV_DOUBLE: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ for (unsigned long j=0; j<nargs; j++) {
+ FPRINTF(stderr, " %g", *(((double *) infoPtr->dst)+j));
+ }
+ break;
+ }
+ case ARGV_STRING: {
+ const char *string;
+
+ nargs = (uintptr_t) infoPtr->src;
+ if (nargs<1) nargs=1;
+ string = *((const char **) infoPtr->dst);
+ if ((nargs==1) && (string == NULL)) break;
+ for (unsigned long j=0; j<nargs; j++) {
+ string = *(((const char **) infoPtr->dst)+j);
+ if (string != NULL) {
+ FPRINTF(stderr, " \"%s\"", string);
+ }
+ }
+
+ break;
+ }
+ case ARGV_END:
+ case ARGV_HELP:
+ case ARGV_GENFUNC:
+ case ARGV_FUNC:
+ case ARGV_REST:
+ case ARGV_CONSTANT:
+ default: {
+ break;
+ }
+ }
+ }
+
+ if ((flags & ARGV_NO_DEFAULTS) || (i > 0)) {
+ break;
+ }
+ FPRINTF(stderr, "\nGeneric options for all commands:");
+ }
+
+ FPRINTF(stderr, "\n");
+#undef FPRINTF
+}
+
+/*!
+
+ Get next command line option and parameter.
+
+ \param argc: Count of command line arguments.
+
+ \param argv: Array of command line argument strings.
+
+ \param validOpts: String of valid case-sensitive option characters, a
+ ':' following a given character means that option can take a parameter.
+
+ \param param: Pointer to a pointer to a string for output.
+
+ \return If valid option is found, the character value of that option is
+ returned, and *param points to the parameter if given, or is NULL if no
+ param.
+
+ \return If standalone parameter (with no option) is found, 1 is returned, and
+ *param points to the standalone parameter
+
+ \return If option is found, but it is not in the list of valid options, -1 is
+ returned, and *param points to the invalid argument.
+
+ \return When end of argument list is reached, 0 is returned, and *param
+ is NULL.
+
+*/
+int
+vpParseArgv::parse(int argc, const char** argv, const char* validOpts,
+ const char** param)
+{
+ static int iArg = 1;
+ int chOpt;
+ const char* psz = NULL;
+ const char* pszParam = NULL;
+
+ if (iArg < argc) {
+ psz = &(argv[iArg][0]);
+ if (*psz == '-') { // || *psz == '/') {
+ // we have an option specifier
+ chOpt = argv[iArg][1];
+ if (isalnum(chOpt) || ispunct(chOpt)) {
+ // we have an option character
+ psz = strchr(validOpts, chOpt);
+ if (psz != NULL) {
+ // option is valid, we want to return chOpt
+ if (psz[1] == ':') {
+ // option can have a parameter
+ psz = &(argv[iArg][2]);
+ if (*psz == '\0') {
+ // must look at next argv for param
+ if (iArg+1 < argc) {
+ psz = &(argv[iArg+1][0]);
+ // next argv is the param
+ iArg++;
+ pszParam = psz;
+ }
+ else {
+ // reached end of args looking for param
+ // option specified without parameter
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
+ }
+
+ }
+ else {
+ // param is attached to option
+ pszParam = psz;
+ }
+ }
+ else {
+ // option is alone, has no parameter
+ }
+ }
+ else {
+ // option specified is not in list of valid options
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
+ }
+ }
+ else {
+ // though option specifier was given, option character
+ // is not alpha or was was not specified
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
+ }
+ }
+ else {
+ // standalone arg given with no option specifier
+ chOpt = 1;
+ pszParam = &(argv[iArg][0]);
+ }
+ }
+ else {
+ // end of argument list
+ chOpt = 0;
+ }
+
+ iArg++;
+ *param = pszParam;
+ return (chOpt);
+}
diff --git a/modules/io/src/video/vpDiskGrabber.cpp b/modules/io/src/video/vpDiskGrabber.cpp
new file mode 100644
index 0000000..16c0c3b
--- /dev/null
+++ b/modules/io/src/video/vpDiskGrabber.cpp
@@ -0,0 +1,412 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Disk framegrabber.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/io/vpDiskGrabber.h>
+
+
+/*!
+ Elementary constructor.
+*/
+vpDiskGrabber::vpDiskGrabber()
+ : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
+{
+ setDirectory("/tmp");
+ setBaseName("I");
+ setExtension("pgm");
+
+ init = false;
+}
+
+
+vpDiskGrabber::vpDiskGrabber(const char *generic_name)
+ : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
+{
+ setDirectory("/tmp");
+ setBaseName("I");
+ setExtension("pgm");
+
+ init = false;
+ if (strlen( generic_name ) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the generic name"));
+ }
+
+ strcpy(this->genericName, generic_name);
+ useGenericName = true;
+}
+
+
+/*!
+ 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 char *dir, const char *basename,
+ long number,
+ int step, unsigned int noz,
+ const char *ext)
+ : image_number(number), image_step(step), number_of_zero(noz), useGenericName(false)
+{
+ setDirectory(dir);
+ setBaseName(basename);
+ setExtension(ext);
+
+ init = false;
+}
+
+void
+vpDiskGrabber::open(vpImage<unsigned char> &I)
+{
+ long first_number = getImageNumber();
+
+ vpDEBUG_TRACE(2, "first %ld", first_number);
+
+ acquire(I);
+
+ setImageNumber(first_number);
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+ init = true;
+}
+
+/*!
+ Read the fist image of the sequence.
+ The image number is not incremented.
+
+*/
+void
+vpDiskGrabber::open(vpImage<vpRGBa> &I)
+{
+ // First we save the image number, so that it can be reaffected after the
+ // acquisition. That means that the first image is readed twice
+ long first_number = getImageNumber();
+ vpDEBUG_TRACE(2, "first %ld", first_number);
+
+ acquire(I);
+
+ setImageNumber(first_number);
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+ init = true;
+}
+
+/*!
+ Read the fist image of the sequence.
+ The image number is not incremented.
+
+*/
+void
+vpDiskGrabber::open(vpImage<float> &I)
+{
+ // First we save the image number, so that it can be reaffected after the
+ // acquisition. That means that the first image is readed twice
+ long first_number = getImageNumber();
+ vpDEBUG_TRACE(2, "first %ld", first_number);
+
+ acquire(I);
+
+ setImageNumber(first_number);
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+ init = true;
+}
+
+/*!
+ Acquire an image: read a pgm image from the disk.
+ After this call, the image number is incremented considering the step.
+
+ \param I the read image
+ */
+void
+vpDiskGrabber::acquire(vpImage<unsigned char> &I)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,image_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
+
+ image_number += image_step ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::read(I, name) ;
+
+ width = I.getWidth();
+ height = I.getHeight();
+}
+
+/*!
+ Acquire an image: read a ppm image from the disk.
+ After this call, the image number is incremented considering the step.
+
+ \param I the read image
+ */
+void
+vpDiskGrabber::acquire(vpImage<vpRGBa> &I)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,image_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
+
+ image_number += image_step ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::read(I, name) ;
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+}
+
+/*!
+ Acquire an image: read a pfm image from the disk.
+ After this call, the image number is incremented considering the step.
+
+ \param I the read image
+ */
+void
+vpDiskGrabber::acquire(vpImage<float> &I)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,image_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
+
+ image_number += image_step ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::readPFM(I, name) ;
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+}
+
+/*!
+ Acquire an image: read a pgm image 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<unsigned char> &I, long img_number)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,img_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::read(I, name) ;
+
+ width = I.getWidth();
+ height = I.getHeight();
+}
+
+/*!
+ Acquire an image: read a ppm image 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<vpRGBa> &I, long img_number)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,img_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::read(I, name) ;
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+}
+
+
+/*!
+ Acquire an image: read a pfm image 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<float> &I, long img_number)
+{
+
+ char name[FILENAME_MAX] ;
+
+ if(useGenericName)
+ sprintf(name,genericName,img_number) ;
+ else
+ sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
+
+ vpDEBUG_TRACE(2, "load: %s\n", name);
+
+ vpImageIo::readPFM(I, name) ;
+
+ width = I.getWidth();
+ 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 char *dir)
+{
+ sprintf(directory, "%s", dir) ;
+}
+
+/*!
+ Set the image base name.
+*/
+void
+vpDiskGrabber::setBaseName(const char *name)
+{
+ sprintf(base_name, "%s", name) ;
+}
+
+/*!
+ Set the image extension.
+ */
+void
+vpDiskGrabber::setExtension(const char *ext)
+{
+ sprintf(extension, "%s", ext) ;
+}
+
+/*!
+ Set the number of the image to be read.
+*/
+void
+vpDiskGrabber::setImageNumber(long number)
+{
+ image_number = number ;
+ vpDEBUG_TRACE(2, "image number %ld", image_number);
+
+}
+
+/*!
+ Set the step between two images.
+*/
+void
+vpDiskGrabber::setStep(int step)
+{
+ image_step = step;
+}
+/*!
+ Set the step between two images.
+*/
+void
+vpDiskGrabber::setNumberOfZero(unsigned int noz)
+{
+ number_of_zero = noz ;
+}
+
+void
+vpDiskGrabber::setGenericName(const char *generic_name)
+{
+ if (strlen( generic_name ) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the generic name"));
+ }
+
+ strcpy(this->genericName, generic_name) ;
+ useGenericName = true;
+}
diff --git a/modules/io/src/video/vpFFMPEG.cpp b/modules/io/src/video/vpFFMPEG.cpp
new file mode 100644
index 0000000..2d0ad4d
--- /dev/null
+++ b/modules/io/src/video/vpFFMPEG.cpp
@@ -0,0 +1,934 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class that manages the FFMPEG library.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpFFMPEG.cpp
+ \brief Class that manages the FFMPEG library
+*/
+
+#include <stdio.h>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpFFMPEG.h>
+#include <visp3/core/vpImageConvert.h>
+
+#ifdef VISP_HAVE_FFMPEG
+
+extern "C"
+{
+//#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libswscale/swscale.h>
+}
+
+/*!
+ Basic constructor.
+*/
+vpFFMPEG::vpFFMPEG()
+ : width(-1), height(-1), frameNumber(0), pFormatCtx(NULL), pCodecCtx(NULL),
+ pCodec(NULL), pFrame(NULL), pFrameRGB(NULL), pFrameGRAY(NULL), packet(NULL),
+ img_convert_ctx(NULL), videoStream(0), numBytes(0), buffer(NULL), index(),
+ streamWasOpen(false), streamWasInitialized(false), color_type(COLORED),
+ f(NULL), outbuf(NULL), picture_buf(NULL), outbuf_size(0), out_size(0),
+ bit_rate(500000), encoderWasOpened(false),
+ framerate_stream(-1), framerate_encoder(25)
+{
+ packet = new AVPacket;
+}
+
+/*!
+ Basic destructor.
+*/
+vpFFMPEG::~vpFFMPEG()
+{
+ closeStream();
+ delete packet;
+}
+
+/*!
+ Allocates and initializes the parameters depending on the video and the desired color type.
+ One the stream is opened, it is possible to get the video encoding framerate getFramerate(),
+ and the dimension of the images using getWidth() and getHeight().
+
+ \param filename : Path to the video which has to be read.
+ \param colortype : Desired color map used to open the video.
+ The parameter can take two values : COLORED and GRAY_SCALED.
+
+ \return It returns true if the paramters could be initialized. Else it returns false.
+*/
+bool vpFFMPEG::openStream(const char *filename, vpFFMPEGColorType colortype)
+{
+ this->color_type = colortype;
+
+ av_register_all();
+#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,0,0) // libavformat 52.84.0
+ if (av_open_input_file (&pFormatCtx, filename, NULL, 0, NULL) != 0)
+#else
+ if (avformat_open_input (&pFormatCtx, filename, NULL, NULL) != 0) // libavformat 53.4.0
+#endif
+ {
+ vpTRACE("Couldn't open file ");
+ return false;
+ }
+
+#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,21,0) // libavformat 53.21.0
+ if (av_find_stream_info (pFormatCtx) < 0)
+#else
+ if (avformat_find_stream_info (pFormatCtx, NULL) < 0)
+#endif
+ return false;
+
+ videoStream = 0;
+ bool found_codec = false;
+
+ /*
+ * Detect streams types
+ */
+ for (unsigned int i = 0; i < pFormatCtx->nb_streams; i++)
+ {
+#if LIBAVUTIL_VERSION_INT < AV_VERSION_INT(51,0,0)
+ if(pFormatCtx->streams[i]->codec->codec_type==CODEC_TYPE_VIDEO) // avutil 50.33.0
+#else
+ if(pFormatCtx->streams[i]->codec->codec_type==AVMEDIA_TYPE_VIDEO) // avutil 51.9.1
+#endif
+ {
+ videoStream = i;
+ //std::cout << "rate: " << pFormatCtx->streams[i]->r_frame_rate.num << " " << pFormatCtx->streams[i]->r_frame_rate.den << std::endl;
+#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(55,12,0)
+ framerate_stream = pFormatCtx->streams[i]->r_frame_rate.num;
+ framerate_stream /= pFormatCtx->streams[i]->r_frame_rate.den;
+#else
+ framerate_stream = pFormatCtx->streams[i]->avg_frame_rate.num;
+ framerate_stream /= pFormatCtx->streams[i]->avg_frame_rate.den;
+#endif
+ found_codec= true;
+ break;
+ }
+ }
+
+ if (found_codec)
+ {
+ pCodecCtx = pFormatCtx->streams[videoStream]->codec;
+ pCodec = avcodec_find_decoder(pCodecCtx->codec_id);
+
+ if (pCodec == NULL)
+ {
+ vpTRACE("unsuported codec");
+ return false; // Codec not found
+ }
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
+ if (avcodec_open (pCodecCtx, pCodec) < 0)
+#else
+ if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0)
+#endif
+ {
+ vpTRACE("Could not open codec");
+ return false; // Could not open codec
+ }
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
+ pFrame = avcodec_alloc_frame();
+#else
+ pFrame = av_frame_alloc(); // libavcodec 55.34.1
+#endif
+
+ if (color_type == vpFFMPEG::COLORED)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
+ pFrameRGB=avcodec_alloc_frame();
+#else
+ pFrameRGB=av_frame_alloc(); // libavcodec 55.34.1
+#endif
+
+ if (pFrameRGB == NULL)
+ return false;
+
+ numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
+ }
+
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
+ pFrameGRAY=avcodec_alloc_frame();
+#else
+ pFrameGRAY=av_frame_alloc(); // libavcodec 55.34.1
+#endif
+
+ if (pFrameGRAY == NULL)
+ return false;
+
+ numBytes = avpicture_get_size (PIX_FMT_GRAY8,pCodecCtx->width,pCodecCtx->height);
+ }
+
+ /*
+ * Determine required buffer size and allocate buffer
+ */
+ width = pCodecCtx->width ;
+ height = pCodecCtx->height ;
+ buffer = (uint8_t *) malloc ((unsigned int)(sizeof (uint8_t)) * (unsigned int)numBytes);
+ }
+ else
+ {
+ vpTRACE("Didn't find a video stream");
+ return false;
+ }
+
+ if (color_type == vpFFMPEG::COLORED)
+ avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
+
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ avpicture_fill((AVPicture *)pFrameGRAY, buffer, PIX_FMT_GRAY8, pCodecCtx->width, pCodecCtx->height);
+
+ streamWasOpen = true;
+
+ return true;
+}
+
+/*!
+ This method initializes the conversion parameters.
+
+ It browses the video and lists all the frame. It sets the number of frame in the video.
+
+ \returns It returns true if the method was executed without any problem. Else it returns false.
+*/
+bool vpFFMPEG::initStream()
+{
+ if (color_type == vpFFMPEG::COLORED)
+ img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_RGB24, SWS_BICUBIC, NULL, NULL, NULL);
+
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_GRAY8, SWS_BICUBIC, NULL, NULL, NULL);
+
+ int ret = av_seek_frame(pFormatCtx, (int)videoStream, 0, AVSEEK_FLAG_ANY) ;
+ if (ret < 0 )
+ {
+ vpTRACE("Error rewinding stream for full indexing") ;
+ return false ;
+ }
+ avcodec_flush_buffers(pCodecCtx) ;
+
+ int frame_no = 0 ;
+ int frameFinished ;
+
+ av_init_packet(packet);
+ while (av_read_frame (pFormatCtx, packet) >= 0)
+ {
+ if (packet->stream_index == (int)videoStream)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
+ ret = avcodec_decode_video(pCodecCtx, pFrame,
+ &frameFinished, packet->data, packet->size);
+#else
+ ret = avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
+#endif
+ if (frameFinished)
+ {
+ if (ret < 0 )
+ {
+ vpTRACE("Unable to decode video picture");
+ }
+ index.push_back(packet->dts);
+ frame_no++ ;
+ }
+ }
+ }
+
+ frameNumber = index.size();
+ av_free_packet(packet);
+
+ streamWasInitialized = true;
+
+ return true;
+}
+
+
+/*!
+ Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
+
+ \param I : The vpImage used to stored the video's frame.
+ \param frame : The index of the frame which has to be read.
+
+ \return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpFFMPEG::getFrame(vpImage<vpRGBa> &I, unsigned int frame)
+{
+
+ if (frame < frameNumber && streamWasInitialized== true)
+ {
+ int64_t targetPts = index[frame];
+ av_seek_frame(pFormatCtx, (int)videoStream,targetPts, AVSEEK_FLAG_ANY);
+ }
+ else
+ {
+ vpTRACE("Couldn't get a frame");
+ return false;
+ }
+
+ avcodec_flush_buffers(pCodecCtx) ;
+
+ int frameFinished ;
+
+ av_init_packet(packet);
+ while (av_read_frame (pFormatCtx, packet) >= 0)
+ {
+ if (packet->stream_index == (int)videoStream)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
+ avcodec_decode_video(pCodecCtx, pFrame,
+ &frameFinished, packet->data, packet->size);
+#else
+ avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
+#endif
+ if (frameFinished)
+ {
+ if (color_type == vpFFMPEG::COLORED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
+
+ copyBitmap(I);
+ break;
+ }
+ }
+ }
+
+ av_free_packet(packet);
+ return true;
+}
+
+
+/*!
+ Gets the Next frame in the video.
+
+ \param I : The vpImage used to stored the video's frame.
+
+ \return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpFFMPEG::acquire(vpImage<vpRGBa> &I)
+{
+ int frameFinished ;
+
+ if (streamWasInitialized == false)
+ {
+ vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
+ return false;
+ }
+
+ av_init_packet(packet);
+ while (av_read_frame (pFormatCtx, packet) >= 0)
+ {
+ if (packet->stream_index == (int)videoStream)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
+ avcodec_decode_video(pCodecCtx, pFrame,
+ &frameFinished, packet->data, packet->size);
+#else
+ avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
+#endif
+ if (frameFinished)
+ {
+ if (color_type == vpFFMPEG::COLORED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
+
+ copyBitmap(I);
+ break;
+ }
+ }
+ }
+ av_free_packet(packet);
+ return true;
+}
+
+/*!
+ Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
+
+ \param I : The vpImage used to stored the video's frame.
+ \param frame : The index of the frame which has to be read.
+
+ \return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpFFMPEG::getFrame(vpImage<unsigned char> &I, unsigned int frame)
+{
+
+ if (frame < frameNumber && streamWasInitialized== true)
+ {
+ int64_t targetPts = index[frame];
+ av_seek_frame(pFormatCtx,(int)videoStream,targetPts, AVSEEK_FLAG_ANY);
+ }
+ else
+ {
+ vpTRACE("Couldn't get a frame");
+ return false;
+ }
+
+ avcodec_flush_buffers(pCodecCtx) ;
+
+ int frameFinished ;
+
+ av_init_packet(packet);
+ while (av_read_frame (pFormatCtx, packet) >= 0)
+ {
+ if (packet->stream_index == (int)videoStream)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
+ avcodec_decode_video(pCodecCtx, pFrame,
+ &frameFinished, packet->data, packet->size);
+#else
+ avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
+#endif
+ if (frameFinished)
+ {
+ if (color_type == vpFFMPEG::COLORED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
+
+ copyBitmap(I);
+ break;
+ }
+ }
+ }
+
+ av_free_packet(packet);
+ return true;
+}
+
+
+/*!
+ Gets the Next frame in the video.
+
+ \param I : The vpImage used to stored the video's frame.
+
+ \return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpFFMPEG::acquire(vpImage<unsigned char> &I)
+{
+ int frameFinished ;
+
+ if (streamWasInitialized == false)
+ {
+ vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
+ return false;
+ }
+
+ av_init_packet(packet);
+ while (av_read_frame (pFormatCtx, packet) >= 0)
+ {
+ if (packet->stream_index == (int)videoStream)
+ {
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
+ avcodec_decode_video(pCodecCtx, pFrame,
+ &frameFinished, packet->data, packet->size);
+#else
+ avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
+#endif
+ if (frameFinished)
+ {
+ if (color_type == vpFFMPEG::COLORED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
+
+ copyBitmap(I);
+ break;
+ }
+ }
+ }
+ av_free_packet(packet);
+ return true;
+}
+
+
+/*!
+ This method enable to fill the vpImage bitmap thanks to the selected frame.
+
+ \throw vpException::dimensionError if either the height or the width
+ associated to the class is negative.
+
+ \param I : the image to fill.
+*/
+void vpFFMPEG::copyBitmap(vpImage<vpRGBa> &I)
+{
+ if(height < 0 || width < 0){
+ throw vpException(vpException::dimensionError, "width or height negative.");
+ }
+ I.resize((unsigned int)height, (unsigned int)width);
+
+ unsigned char* line;
+ unsigned char* beginOutput = (unsigned char*)I.bitmap;
+ unsigned char* output = NULL;
+
+ if (color_type == COLORED)
+ {
+ unsigned char* input = (unsigned char*)pFrameRGB->data[0];
+ int widthStep = pFrameRGB->linesize[0];
+ for(int i=0 ; i < height ; i++)
+ {
+ line = input;
+ output = beginOutput + 4 * width * i;
+ for(int j=0 ; j < width ; j++)
+ {
+ *(output++) = *(line);
+ *(output++) = *(line+1);
+ *(output++) = *(line+2);
+ *(output++) = 0;
+
+ line+=3;
+ }
+ //go to the next line
+ input+=widthStep;
+ }
+ }
+
+ else if (color_type == GRAY_SCALED)
+ {
+ unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
+ int widthStep = pFrameGRAY->linesize[0];
+ for(int i=0 ; i < height ; i++)
+ {
+ line = input;
+ output = beginOutput + 4 * width * i;
+ for(int j=0 ; j < width ; j++)
+ {
+ *output++ = *(line);
+ *output++ = *(line);
+ *output++ = *(line);
+ *output++ = *(line);;
+
+ line++;
+ }
+ //go to the next line
+ input+=widthStep;
+ }
+ }
+}
+
+/*!
+ This method enable to fill the vpImage bitmap thanks to the selected frame.
+
+ \throw vpException::dimensionError if either the height or the width
+ associated to the class is negative.
+
+ \param I : the image to fill.
+*/
+void vpFFMPEG::copyBitmap(vpImage<unsigned char> &I)
+{
+ if(height < 0 || width < 0){
+ throw vpException(vpException::dimensionError, "width or height negative.");
+ }
+ I.resize((unsigned int)height, (unsigned int)width);
+
+ unsigned char* line;
+ unsigned char* beginOutput = (unsigned char*)I.bitmap;
+ unsigned char* output = NULL;
+
+ if (color_type == GRAY_SCALED)
+ {
+ unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
+ int widthStep = pFrameGRAY->linesize[0];
+ for(int i=0 ; i < height ; i++)
+ {
+ line = input;
+ output = beginOutput + width * i;
+ for(int j=0 ; j < width ; j++)
+ {
+ *(output++) = *(line);
+
+ line++;
+ }
+ //go to the next line
+ input+=widthStep;
+ }
+ }
+
+ if (color_type == COLORED)
+ {
+ unsigned char* input = (unsigned char*)pFrameRGB->data[0];
+ int widthStep = pFrameRGB->linesize[0];
+ for (int i = 0 ; i < height ; i++)
+ {
+ vpImageConvert::RGBToGrey(input + i*widthStep, beginOutput + i*width, (unsigned int)width, 1, false);
+ }
+ }
+}
+
+/*!
+ Deallocates all the FFMPEG parameters.
+*/
+void vpFFMPEG::closeStream()
+{
+ if (streamWasOpen)
+ {
+ if (buffer != NULL) {
+ free(buffer);
+ buffer = NULL;
+ }
+
+ if (color_type == vpFFMPEG::COLORED)
+ av_free(pFrameRGB);
+
+ else if (color_type == vpFFMPEG::GRAY_SCALED)
+ av_free(pFrameGRAY);
+
+ // Free the YUV frame
+ av_free(pFrame);
+
+ // Close the codec
+ if (pCodecCtx) avcodec_close(pCodecCtx);
+
+ // Close the video file
+#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,17,0) // libavformat 53.17.0
+ av_close_input_file(pFormatCtx);
+#else
+ avformat_close_input(&pFormatCtx);
+#endif
+ }
+ streamWasOpen = false;
+
+ if (encoderWasOpened)
+ {
+ if(f!=NULL) endWrite();
+
+ if(buffer!=NULL) delete[] buffer;
+
+ if(outbuf != NULL) delete[] outbuf;
+
+ if(picture_buf != NULL) delete[] picture_buf;
+
+ av_free(pFrameRGB);
+ av_free(pFrame);
+ if (pCodecCtx) avcodec_close(pCodecCtx);
+ }
+
+ encoderWasOpened = false;
+
+ if(streamWasInitialized || encoderWasOpened){
+ sws_freeContext (img_convert_ctx);
+ }
+ streamWasInitialized = false;
+}
+
+/*!
+ Allocates and initializes the parameters depending on the video to write.
+
+ \param filename : Path to the video which has to be writen.
+ \param w,h : Width and height of the image which will be saved.
+ \param codec : Type of codec used to encode the video.
+
+ By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
+ AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
+ AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
+
+ Of course to use the codec it must be installed on your computer.
+
+ \return It returns true if the paramters could be initialized. Else it returns false.
+*/
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, CodecID codec)
+#else
+bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, AVCodecID codec)
+#endif
+{
+ av_register_all();
+
+ /* find the mpeg1 video encoder */
+ pCodec = avcodec_find_encoder(codec);
+ if (pCodec == NULL) {
+ fprintf(stderr, "codec not found\n");
+ return false;
+ }
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,5,0) // libavcodec 53.5.0
+ pCodecCtx = avcodec_alloc_context();
+#else
+ pCodecCtx = avcodec_alloc_context3(NULL);
+#endif
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
+ pFrame = avcodec_alloc_frame();
+ pFrameRGB = avcodec_alloc_frame();
+#else
+ pFrame = av_frame_alloc(); // libavcodec 55.34.1
+ pFrameRGB = av_frame_alloc(); // libavcodec 55.34.1
+#endif
+
+ /* put sample parameters */
+ pCodecCtx->bit_rate = (int)bit_rate;
+ /* resolution must be a multiple of two */
+ pCodecCtx->width = (int)w;
+ pCodecCtx->height = (int)h;
+ this->width = (int)w;
+ this->height = (int)h;
+ /* frames per second */
+ pCodecCtx->time_base.num = 1;
+ pCodecCtx->time_base.den = framerate_encoder;
+ pCodecCtx->gop_size = 10; /* emit one intra frame every ten frames */
+ pCodecCtx->max_b_frames=1;
+ pCodecCtx->pix_fmt = PIX_FMT_YUV420P;
+
+ /* open it */
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
+ if (avcodec_open (pCodecCtx, pCodec) < 0) {
+#else
+ if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0) {
+#endif
+ fprintf(stderr, "could not open codec\n");
+ return false;
+ }
+
+ /* the codec gives us the frame size, in samples */
+
+ f = fopen(filename, "wb");
+ if (!f) {
+ fprintf(stderr, "could not open %s\n", filename);
+ return false;
+ }
+
+ outbuf_size = 100000;
+ outbuf = new uint8_t[outbuf_size];
+
+ numBytes = avpicture_get_size (PIX_FMT_YUV420P,pCodecCtx->width,pCodecCtx->height);
+ picture_buf = new uint8_t[numBytes];
+ avpicture_fill((AVPicture *)pFrame, picture_buf, PIX_FMT_YUV420P, pCodecCtx->width, pCodecCtx->height);
+
+ numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
+ buffer = new uint8_t[numBytes];
+ avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
+
+ img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, PIX_FMT_RGB24, pCodecCtx->width,pCodecCtx->height,PIX_FMT_YUV420P, SWS_BICUBIC, NULL, NULL, NULL);
+
+ encoderWasOpened = true;
+
+ return true;
+}
+
+
+/*!
+ Saves the image I as frame of the video.
+
+ \param I : the image to save.
+
+ \return It returns true if the image could be saved.
+*/
+bool vpFFMPEG::saveFrame(vpImage<vpRGBa> &I)
+{
+ if (encoderWasOpened == false)
+ {
+ vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
+ return false;
+ }
+
+ writeBitmap(I);
+ sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
+ out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
+ fwrite(outbuf, 1, (size_t)out_size, f);
+#else
+ AVPacket pkt;
+ av_init_packet(&pkt);
+ pkt.data = NULL; // packet data will be allocated by the encoder
+ pkt.size = 0;
+
+ int got_output;
+ int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
+ if (ret < 0) {
+ std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
+ return false;
+ }
+ if (got_output) {
+ fwrite(pkt.data, 1, pkt.size, f);
+ av_free_packet(&pkt);
+ }
+#endif
+ fflush(stdout);
+ return true;
+}
+
+
+/*!
+ Saves the image I as frame of the video.
+
+ \param I : the image to save.
+
+ \return It returns true if the image could be saved.
+*/
+bool vpFFMPEG::saveFrame(vpImage<unsigned char> &I)
+{
+ if (encoderWasOpened == false)
+ {
+ vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
+ return false;
+ }
+
+ writeBitmap(I);
+ sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
+ out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
+ fwrite(outbuf, 1, (size_t)out_size, f);
+#else
+ AVPacket pkt;
+ av_init_packet(&pkt);
+ pkt.data = NULL; // packet data will be allocated by the encoder
+ pkt.size = 0;
+
+ int got_output;
+ int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
+ if (ret < 0) {
+ std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
+ return false;
+ }
+ if (got_output) {
+ fwrite(pkt.data, 1, pkt.size, f);
+ av_free_packet(&pkt);
+ }
+#endif
+
+ fflush(stdout);
+ return true;
+}
+
+/*!
+ Ends the writing of the video and close the file.
+
+ \return It returns true if the file was closed without problem
+*/
+bool vpFFMPEG::endWrite()
+{
+ if (encoderWasOpened == false)
+ {
+ vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
+ return false;
+ }
+
+ int ret = 1;
+ while (ret != 0)
+ {
+
+#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
+ ret = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, NULL);
+ fwrite(outbuf, 1, (size_t)out_size, f);
+#else
+ AVPacket pkt;
+ av_init_packet(&pkt);
+ pkt.data = NULL; // packet data will be allocated by the encoder
+ pkt.size = 0;
+ int got_output;
+ ret = avcodec_encode_video2(pCodecCtx, &pkt, NULL, &got_output);
+ if (ret < 0) {
+ std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
+ return false;
+ }
+ if (got_output) {
+ fwrite(pkt.data, 1, pkt.size, f);
+ av_free_packet(&pkt);
+ }
+#endif
+ }
+
+ /*The end of a mpeg file*/
+ outbuf[0] = 0x00;
+ outbuf[1] = 0x00;
+ outbuf[2] = 0x01;
+ outbuf[3] = 0xb7;
+ fwrite(outbuf, 1, 4, f);
+ fclose(f);
+ f = NULL;
+ return true;
+}
+
+/*!
+ This method enables to fill the frame bitmap thanks to the vpImage bitmap.
+*/
+void vpFFMPEG::writeBitmap(vpImage<vpRGBa> &I)
+{
+ unsigned char* beginInput = (unsigned char*)I.bitmap;
+ unsigned char* input = NULL;
+ unsigned char* output = NULL;
+ unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
+ int widthStep = pFrameRGB->linesize[0];
+
+ for(int i=0 ; i < height ; i++)
+ {
+ input = beginInput + 4 * i * width;
+ output = beginOutput + i * widthStep;
+ for(int j=0 ; j < width ; j++)
+ {
+ *(output++) = *(input);
+ *(output++) = *(input+1);
+ *(output++) = *(input+2);
+
+ input+=4;
+ }
+ }
+}
+
+
+/*!
+ This method enables to fill the frame bitmap thanks to the vpImage bitmap.
+*/
+void vpFFMPEG::writeBitmap(vpImage<unsigned char> &I)
+{
+ unsigned char* beginInput = (unsigned char*)I.bitmap;
+ unsigned char* input = NULL;
+ unsigned char* output = NULL;
+ unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
+ int widthStep = pFrameRGB->linesize[0];
+
+ for(int i=0 ; i < height ; i++)
+ {
+ input = beginInput + i * width;
+ output = beginOutput + i * widthStep;
+ for(int j=0 ; j < width ; j++)
+ {
+ *(output++) = *(input);
+ *(output++) = *(input);
+ *(output++) = *(input);
+
+ input++;
+ }
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_core.a(vpFFMPEG.cpp.o) has no symbols
+void dummy_vpFFMPEG() {};
+#endif
diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp
new file mode 100644
index 0000000..47631e5
--- /dev/null
+++ b/modules/io/src/video/vpVideoReader.cpp
@@ -0,0 +1,777 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Read videos and image sequences.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+\file vpVideoReader.cpp
+\brief Read videos and image sequences
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpVideoReader.h>
+
+#include <iostream>
+#include <fstream>
+#include <limits> // numeric_limits
+
+/*!
+Basic constructor.
+*/
+vpVideoReader::vpVideoReader()
+ : vpFrameGrabber(), imSequence(NULL),
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg(NULL),
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ capture(), frame(),
+#endif
+ formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
+ firstFrame(0), lastFrame(0), firstFrameIndexIsSet(false), lastFrameIndexIsSet(false)
+{
+}
+
+/*!
+Basic destructor.
+*/
+vpVideoReader::~vpVideoReader()
+{
+ if (imSequence != NULL)
+ {
+ delete imSequence;
+ }
+#ifdef VISP_HAVE_FFMPEG
+ if (ffmpeg != NULL)
+ {
+ delete ffmpeg;
+ }
+#endif
+}
+
+
+/*!
+It enables to set the path and the name of the file(s) which as/have to be read.
+
+If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
+
+If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+
+\param filename : Path to a video file or file name template of a image sequence.
+*/
+void vpVideoReader::setFileName(const char *filename)
+{
+ if (!filename || *filename == '\0')
+ {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
+ }
+
+ if (strlen( filename ) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the file name"));
+ }
+
+ strcpy(this->fileName,filename);
+
+ formatType = getFormat(fileName);
+
+ if (formatType == FORMAT_UNKNOWN) {
+ throw(vpException(vpException::badValue, "Filename extension not supported"));
+ }
+
+ initFileName = true;
+}
+
+/*!
+It enables to set the path and the name of the file(s) which as/have to be read.
+
+If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
+
+If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+
+\param filename : Path to a video file or file name template of a image sequence.
+*/
+void vpVideoReader::setFileName(const std::string &filename)
+{
+ setFileName(filename.c_str());
+}
+
+/*!
+Sets all the parameters needed to read the video or the image sequence.
+
+Grab the first frame and stores it in the image \f$ I \f$.
+
+\param I : The image where the frame is stored.
+*/
+void vpVideoReader::open(vpImage< vpRGBa > &I)
+{
+ if (!initFileName)
+ {
+ vpERROR_TRACE("The generic filename has to be set");
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
+ }
+
+ if (isImageExtensionSupported())
+ {
+ imSequence = new vpDiskGrabber;
+ imSequence->setGenericName(fileName);
+ if (firstFrameIndexIsSet)
+ imSequence->setImageNumber(firstFrame);
+ }
+ else if (isVideoExtensionSupported())
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg = new vpFFMPEG;
+ if(!ffmpeg->openStream(fileName, vpFFMPEG::COLORED))
+ throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
+ ffmpeg->initStream();
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ capture.open(fileName);
+
+ if(!capture.isOpened())
+ {
+ throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
+ }
+#else
+ //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
+ throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+#endif
+ }
+ else if (formatType == FORMAT_UNKNOWN)
+ {
+ //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
+ throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
+ }
+
+ findFirstFrameIndex();
+ frameCount = firstFrame;
+ if(!getFrame(I, firstFrame))
+ {
+ //vpERROR_TRACE("Could not read the video first frame");
+ throw (vpException(vpException::ioError ,"Could not read the video first frame"));
+ }
+
+ height = I.getHeight();
+ width = I.getWidth();
+
+ isOpen = true;
+ findLastFrameIndex();
+ frameCount = firstFrame; // open() should not increase the frame counter
+}
+
+
+/*!
+Sets all the parameters needed to read the video or the image sequence.
+
+Grab the first frame and stores it in the image \f$ I \f$.
+
+\param I : The image where the frame is stored.
+*/
+void vpVideoReader::open(vpImage<unsigned char> &I)
+{
+ if (!initFileName)
+ {
+ vpERROR_TRACE("The generic filename has to be set");
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
+ }
+
+ if (isImageExtensionSupported())
+ {
+ imSequence = new vpDiskGrabber;
+ imSequence->setGenericName(fileName);
+ if (firstFrameIndexIsSet)
+ imSequence->setImageNumber(firstFrame);
+ }
+ else if (isVideoExtensionSupported())
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg = new vpFFMPEG;
+ if (!ffmpeg->openStream(fileName, vpFFMPEG::GRAY_SCALED))
+ throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
+ ffmpeg->initStream();
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ capture.open(fileName);
+
+ if(!capture.isOpened())
+ {
+ throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
+ }
+#else
+ //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
+ throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv >= 2.1.0 3rd party libraries."));
+#endif
+ }
+ else if (formatType == FORMAT_UNKNOWN)
+ {
+ //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
+ throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
+ }
+
+ findFirstFrameIndex();
+ frameCount = firstFrame;
+ if(!getFrame(I,firstFrame))
+ {
+ //vpERROR_TRACE("Could not read the video first frame");
+ throw (vpException(vpException::ioError ,"Could not read the video first frame"));
+ }
+
+ height = I.getHeight();
+ width = I.getWidth();
+
+ isOpen = true;
+ findLastFrameIndex();
+ frameCount = firstFrame; // open() should not increase the frame counter
+}
+
+
+/*!
+Grabs the current (k) image in the stack of frames and increments the frame counter
+in order to grab the next image (k+1) during the next use of the method. If open()
+was not called previously, this method opens the video reader.
+
+This method enables to use the class as frame grabber.
+
+\param I : The image where the frame is stored.
+*/
+void vpVideoReader::acquire(vpImage< vpRGBa > &I)
+{
+ if (!isOpen) {
+ open(I);
+ }
+
+ //getFrame(I,frameCount);
+ if (imSequence != NULL)
+ {
+ imSequence->acquire(I);
+ frameCount++; // next index
+ }
+#ifdef VISP_HAVE_FFMPEG
+ else if (ffmpeg !=NULL)
+ {
+ ffmpeg->acquire(I);
+ frameCount++; // next index
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else
+ {
+ capture >> frame;
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+#else
+ frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
+#endif
+
+ if(frame.empty())
+ setLastFrameIndex(frameCount-1);
+ else
+ vpImageConvert::convert(frame, I);
+ }
+#endif
+}
+
+
+/*!
+Grabs the kth image in the stack of frames and increments the frame counter in order to grab the next image (k+1) during the next use of the method.
+
+This method enables to use the class as frame grabber.
+
+\param I : The image where the frame is stored.
+*/
+void vpVideoReader::acquire(vpImage< unsigned char > &I)
+{
+ if (!isOpen) {
+ open(I);
+ }
+
+ if (imSequence != NULL)
+ {
+ imSequence->acquire(I);
+ frameCount++; // next index
+ }
+#ifdef VISP_HAVE_FFMPEG
+ else if (ffmpeg != NULL)
+ {
+ ffmpeg->acquire(I);
+ frameCount++; // next index
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else
+ {
+ capture >> frame;
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+#else
+ frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
+#endif
+
+ if(frame.empty())
+ setLastFrameIndex(frameCount-1);
+ else
+ vpImageConvert::convert(frame, I);
+ }
+#endif
+}
+
+
+/*!
+Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
+
+\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
+But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
+one after one.
+
+\param I : The vpImage used to stored the frame.
+\param frame_index : The index of the frame which has to be read.
+
+\return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpVideoReader::getFrame(vpImage<vpRGBa> &I, long frame_index)
+{
+ if (imSequence != NULL)
+ {
+ try
+ {
+ imSequence->acquire(I, frame_index);
+ frameCount = frame_index + 1; // next index
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
+ return false;
+ }
+ }
+ else
+ {
+#ifdef VISP_HAVE_FFMPEG
+ if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ return false;
+ }
+ frameCount = frame_index + 1; // next index
+#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ return false;
+ }
+
+ capture >> frame;
+ frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+ if(frame.empty()) {
+ // New trial that makes things working with opencv 3.0.0
+ capture >> frame;
+ if(frame.empty()) {
+ setLastFrameIndex(frameCount-1);
+ return false;
+ }
+ else {
+ vpImageConvert::convert(frame, I);
+ }
+ }
+ else
+ vpImageConvert::convert(frame, I);
+#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+ if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ return false;
+ }
+
+ capture >> frame;
+ frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
+ if(frame.empty())
+ setLastFrameIndex(frameCount-1);
+ else
+ vpImageConvert::convert(frame, I);
+#endif
+ }
+
+ return true;
+}
+
+
+/*!
+Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
+
+\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
+But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
+one after one.
+
+\param I : The vpImage used to stored the frame.
+\param frame_index : The index of the frame which has to be read.
+
+\return It returns true if the frame could be read. Else it returns false.
+*/
+bool vpVideoReader::getFrame(vpImage<unsigned char> &I, long frame_index)
+{
+ if (imSequence != NULL)
+ {
+ try
+ {
+ imSequence->acquire(I, frame_index);
+ frameCount = frame_index + 1;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
+ return false;
+ }
+ }
+ else
+ {
+#ifdef VISP_HAVE_FFMPEG
+ if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ return false;
+ }
+ frameCount = frame_index + 1; // next index
+#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ return false;
+ }
+ capture >> frame;
+ frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+ if(frame.empty()) {
+ // New trial that makes things working with opencv 3.0.0
+ capture >> frame;
+ if(frame.empty()) {
+ setLastFrameIndex(frameCount-1);
+ return false;
+ }
+ else {
+ vpImageConvert::convert(frame, I);
+ }
+ }
+ else
+ vpImageConvert::convert(frame, I);
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
+ {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); // next index
+ return false;
+ }
+ capture >> frame;
+ frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES);
+ if(frame.empty())
+ setLastFrameIndex(frameCount-1);
+ else
+ vpImageConvert::convert(frame, I);
+#endif
+ }
+
+ return true;
+}
+
+
+/*!
+Gets the format of the file(s) which has/have to be read.
+
+\return Returns the format.
+*/
+vpVideoReader::vpVideoFormatType
+ vpVideoReader::getFormat(const char *filename)
+{
+ std::string sfilename(filename);
+
+ std::string ext = vpVideoReader::getExtension(sfilename);
+
+ if (ext.compare(".PGM") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".pgm") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".PPM") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".ppm") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".JPG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".JPEG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpeg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".PNG") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".png") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".TIFF") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".tiff") == 0)
+ return FORMAT_TIFF;
+ else if (ext.compare(".BMP") == 0)
+ return FORMAT_BMP;
+ else if (ext.compare(".bmp") == 0)
+ return FORMAT_BMP;
+ else if (ext.compare(".DIB") == 0)
+ return FORMAT_DIB;
+ else if (ext.compare(".dib") == 0)
+ return FORMAT_DIB;
+ else if (ext.compare(".PBM") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".PBM") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".SR") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".sr") == 0)
+ return FORMAT_PBM;
+ else if (ext.compare(".RAS") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".ras") == 0)
+ return FORMAT_RASTER;
+ else if (ext.compare(".JP2") == 0)
+ return FORMAT_JPEG2000;
+ else if (ext.compare(".jp2") == 0)
+ return FORMAT_JPEG2000;
+ else if (ext.compare(".AVI") == 0)
+ return FORMAT_AVI;
+ else if (ext.compare(".avi") == 0)
+ return FORMAT_AVI;
+ else if (ext.compare(".MPEG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpeg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPEG4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mpeg4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MP4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mp4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MOV") == 0)
+ return FORMAT_MOV;
+ else if (ext.compare(".mov") == 0)
+ return FORMAT_MOV;
+ else if (ext.compare(".OGV") == 0)
+ return FORMAT_OGV;
+ else if (ext.compare(".ogv") == 0)
+ return FORMAT_OGV;
+ else if (ext.compare(".WMV") == 0)
+ return FORMAT_WMV;
+ else if (ext.compare(".wmv") == 0)
+ return FORMAT_WMV;
+ else if (ext.compare(".FLV") == 0)
+ return FORMAT_FLV;
+ else if (ext.compare(".flv") == 0)
+ return FORMAT_FLV;
+ else if (ext.compare(".MKV") == 0)
+ return FORMAT_MKV;
+ else if (ext.compare(".mkv") == 0)
+ return FORMAT_MKV;
+ else
+ return FORMAT_UNKNOWN;
+}
+
+// return the extension of the file including the dot
+std::string vpVideoReader::getExtension(const std::string &filename)
+{
+ // extract the extension
+ size_t dot = filename.find_last_of(".");
+ std::string ext = filename.substr(dot, filename.size()-1);
+ return ext;
+}
+
+
+/*!
+Get the last frame index (update the lastFrame attribute).
+*/
+void vpVideoReader::findLastFrameIndex()
+{
+ if (!isOpen) {
+ vpERROR_TRACE("Use the open method before");
+ throw (vpException(vpException::notInitialized,"file not yet opened"));
+ }
+
+ if (imSequence != NULL) {
+ if (! lastFrameIndexIsSet) {
+ char name[FILENAME_MAX];
+ int image_number = firstFrame;
+ bool failed;
+ do {
+ std::fstream file;
+ sprintf(name,fileName,image_number) ;
+ file.open(name, std::ios::in);
+ failed = file.fail();
+ if (!failed) {
+ file.close();
+ image_number++;
+ }
+ } while(!failed);
+
+ lastFrame = image_number -1;
+ }
+ }
+
+#ifdef VISP_HAVE_FFMPEG
+ else if (ffmpeg != NULL) {
+ if (! lastFrameIndexIsSet) {
+ lastFrame = (long)(ffmpeg->getFrameNumber());
+ }
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ else if (! lastFrameIndexIsSet)
+ {
+ lastFrame = (long) capture.get(cv::CAP_PROP_FRAME_COUNT);
+ if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 3.0.0
+ {
+ //std::cout << "Warning: Problem with cv::CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
+ lastFrame = 100000; // Set lastFrame to an arbitrary value
+ }
+ lastFrame--; //Last frame index = total frame count - 1
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else if (! lastFrameIndexIsSet)
+ {
+ lastFrame = (long) capture.get(CV_CAP_PROP_FRAME_COUNT);
+ if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 2.4.10
+ {
+ //std::cout << "Warning: Problem with CV_CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
+ lastFrame = 100000; // Set lastFrame to an arbitrary value
+ }
+ lastFrame--; //Last frame index = total frame count - 1
+ }
+#endif
+}
+/*!
+Get the first frame index (update the firstFrame attribute).
+*/
+void
+ vpVideoReader::findFirstFrameIndex()
+{
+ if (imSequence != NULL)
+ {
+ if (! firstFrameIndexIsSet) {
+ char name[FILENAME_MAX];
+ int image_number = 0;
+ bool failed;
+ do {
+ std::fstream file;
+ sprintf(name, fileName, image_number) ;
+ file.open(name, std::ios::in);
+ failed = file.fail();
+ if (!failed) file.close();
+ image_number++;
+ } while(failed);
+
+ firstFrame = image_number - 1;
+ imSequence->setImageNumber(firstFrame);
+ }
+ }
+#ifdef VISP_HAVE_FFMPEG
+ else if (ffmpeg != NULL) {
+ if (! firstFrameIndexIsSet) {
+ firstFrame = (long)(0);
+ }
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else if (! firstFrameIndexIsSet)
+ {
+ firstFrame = (long) (0);
+ }
+#endif
+}
+
+/*!
+Return the framerate in Hz used to encode the video stream.
+
+If the video is a sequence of images, return -1.
+*/
+double vpVideoReader::getFramerate()
+{
+ double framerate = -1.;
+
+#ifdef VISP_HAVE_FFMPEG
+ if (ffmpeg != NULL)
+ {
+ framerate = ffmpeg->getFramerate();
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ framerate = capture.get(cv::CAP_PROP_FPS);
+ // if(framerate == 0)
+ if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
+ {
+ vpERROR_TRACE("Problem with cv::CAP_PROP_FPS") ;
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ framerate = capture.get(CV_CAP_PROP_FPS);
+ // if(framerate == 0)
+ if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
+ {
+ vpERROR_TRACE("Problem with CV_CAP_PROP_FPS") ;
+ }
+#endif
+ return framerate;
+}
+
+/*!
+Return true if the image file extension is supported, false otherwise.
+*/
+bool vpVideoReader::isImageExtensionSupported()
+{
+ return (formatType == FORMAT_PGM ||
+ formatType == FORMAT_PPM ||
+ formatType == FORMAT_JPEG ||
+ formatType == FORMAT_PNG ||
+ formatType == FORMAT_TIFF ||
+ formatType == FORMAT_BMP ||
+ formatType == FORMAT_DIB ||
+ formatType == FORMAT_PBM ||
+ formatType == FORMAT_RASTER ||
+ formatType == FORMAT_JPEG2000);
+}
+
+/*!
+Return true if the video file extension is supported, false otherwise.
+*/
+bool vpVideoReader::isVideoExtensionSupported()
+{
+ return (formatType == FORMAT_AVI ||
+ formatType == FORMAT_MPEG ||
+ formatType == FORMAT_MPEG4 ||
+ formatType == FORMAT_MOV ||
+ formatType == FORMAT_OGV ||
+ formatType == FORMAT_WMV ||
+ formatType == FORMAT_FLV ||
+ formatType == FORMAT_MKV);
+}
diff --git a/modules/io/src/video/vpVideoWriter.cpp b/modules/io/src/video/vpVideoWriter.cpp
new file mode 100644
index 0000000..95aba72
--- /dev/null
+++ b/modules/io/src/video/vpVideoWriter.cpp
@@ -0,0 +1,431 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Write image sequences.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpVideoWriter.cpp
+ \brief Write image sequences.
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpVideoWriter.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020200
+# include <opencv2/imgproc/imgproc.hpp>
+#endif
+
+
+/*!
+ Basic constructor.
+*/
+vpVideoWriter::vpVideoWriter() :
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg(NULL),
+# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ codec(CODEC_ID_MPEG1VIDEO),
+# else
+ codec(AV_CODEC_ID_MPEG1VIDEO),
+# endif
+ bit_rate(500000),
+ framerate(25),
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ writer(), fourcc(0), framerate(0.),
+#endif
+ formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
+ firstFrame(0), width(0), height(0)
+{
+ initFileName = false;
+ firstFrame = 0;
+ frameCount = 0;
+ isOpen = false;
+ width = height = 0;
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg = NULL;
+# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
+ codec = CODEC_ID_MPEG1VIDEO;
+# else
+ codec = AV_CODEC_ID_MPEG1VIDEO;
+# endif
+ bit_rate = 500000;
+ framerate = 25;
+#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ framerate = 25.0;
+ fourcc = cv::VideoWriter::fourcc('P','I','M','1');
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ framerate = 25.0;
+ fourcc = CV_FOURCC('P','I','M','1'); // default is a MPEG-1 codec
+#endif
+}
+
+
+/*!
+ Basic destructor.
+*/
+vpVideoWriter::~vpVideoWriter()
+{
+ #ifdef VISP_HAVE_FFMPEG
+ if (ffmpeg != NULL)
+ delete ffmpeg;
+ #endif
+}
+
+
+/*!
+ It enables to set the path and the name of the files which will be saved.
+
+ If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+
+ \param filename : filename template of an image sequence.
+*/
+void vpVideoWriter::setFileName(const char *filename)
+{
+ if (!filename || *filename == '\0')
+ {
+ vpERROR_TRACE("filename empty ") ;
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
+ }
+
+ if (strlen( filename ) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the file name"));
+ }
+
+ strcpy(this->fileName,filename);
+
+ formatType = getFormat(fileName);
+
+ if (formatType == FORMAT_UNKNOWN) {
+ throw(vpException(vpException::badValue, "Filename extension not supported"));
+ }
+
+ initFileName = true;
+}
+
+/*!
+ It enables to set the path and the name of the files which will be saved.
+
+ If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+
+ \param filename : filename template of an image sequence.
+*/
+void vpVideoWriter::setFileName(const std::string &filename)
+{
+ setFileName(filename.c_str());
+}
+
+/*!
+ Sets all the parameters needed to write the video or the image sequence.
+
+ \param I : One image with the right dimensions.
+*/
+void vpVideoWriter::open(vpImage< vpRGBa > &I)
+{
+ if (!initFileName)
+ {
+ vpERROR_TRACE("The generic filename has to be set");
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
+ }
+
+ if (formatType == FORMAT_PGM ||
+ formatType == FORMAT_PPM ||
+ formatType == FORMAT_JPEG ||
+ formatType == FORMAT_PNG)
+ {
+ width = I.getWidth();
+ height = I.getHeight();
+ }
+ else if (formatType == FORMAT_AVI ||
+ formatType == FORMAT_MPEG ||
+ formatType == FORMAT_MPEG4 ||
+ formatType == FORMAT_MOV)
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg = new vpFFMPEG;
+ ffmpeg->setFramerate(framerate);
+ ffmpeg->setBitRate(bit_rate);
+ if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
+ throw (vpException(vpException::ioError ,"Could not open encode the video with ffmpeg"));
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ writer = cv::VideoWriter(fileName, fourcc, framerate, cv::Size((int)I.getWidth(), (int)I.getHeight()));
+
+ if(!writer.isOpened())
+ {
+ //vpERROR_TRACE("Could not open encode the video with opencv");
+ throw (vpException(vpException::fatalError , "Could not open encode the video with opencv"));
+ }
+#else
+ //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
+ throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+#endif
+ }
+
+ frameCount = firstFrame;
+
+ isOpen = true;
+}
+
+
+/*!
+ Sets all the parameters needed to write the video or the image sequence.
+
+ \param I : One image with the right dimensions.
+*/
+void vpVideoWriter::open(vpImage< unsigned char > &I)
+{
+ if (!initFileName)
+ {
+ vpERROR_TRACE("The generic filename has to be set");
+ throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
+ }
+
+ if (formatType == FORMAT_PGM ||
+ formatType == FORMAT_PPM ||
+ formatType == FORMAT_JPEG ||
+ formatType == FORMAT_PNG)
+ {
+ width = I.getWidth();
+ height = I.getHeight();
+ }
+ else if (formatType == FORMAT_AVI ||
+ formatType == FORMAT_MPEG ||
+ formatType == FORMAT_MPEG4 ||
+ formatType == FORMAT_MOV)
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg = new vpFFMPEG;
+ ffmpeg->setFramerate(framerate);
+ ffmpeg->setBitRate(bit_rate);
+ if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
+ throw (vpException(vpException::ioError ,"Could not encode the video with ffmpeg"));
+ }
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ writer = cv::VideoWriter(fileName, fourcc, framerate, cv::Size((int)I.getWidth(), (int)I.getHeight()));
+
+ if(!writer.isOpened())
+ {
+ vpERROR_TRACE("Could not encode the video with opencv");
+ throw (vpException(vpException::ioError ,"Could not encode the video with opencv"));
+ }
+#else
+ //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
+ throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+#endif
+ }
+
+ frameCount = firstFrame;
+
+ isOpen = true;
+}
+
+
+/*!
+ Saves the image as a frame of the video or as an image belonging to the image sequence.
+
+ Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
+
+ \param I : The image which has to be saved
+*/
+void vpVideoWriter::saveFrame (vpImage< vpRGBa > &I)
+{
+ if (!isOpen)
+ {
+ vpERROR_TRACE("The video has to be open first with the open method");
+ throw (vpException(vpException::notInitialized,"file not yet opened"));
+ }
+
+
+ if (formatType == FORMAT_PGM ||
+ formatType == FORMAT_PPM ||
+ formatType == FORMAT_JPEG ||
+ formatType == FORMAT_PNG)
+ {
+ char name[FILENAME_MAX];
+
+ sprintf(name,fileName,frameCount);
+
+ vpImageIo::write(I, name);
+ }
+ else
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg->saveFrame(I);
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ cv::Mat matFrame;
+ vpImageConvert::convert(I, matFrame);
+ writer << matFrame;
+#endif
+ }
+
+ frameCount++;
+}
+
+
+/*!
+ Saves the image as a frame of the video or as an image belonging to the image sequence.
+
+ Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
+
+ \param I : The image which has to be saved
+*/
+void vpVideoWriter::saveFrame (vpImage< unsigned char > &I)
+{
+ if (!isOpen)
+ {
+ vpERROR_TRACE("The video has to be open first with the open method");
+ throw (vpException(vpException::notInitialized,"file not yet opened"));
+ }
+
+ if (formatType == FORMAT_PGM ||
+ formatType == FORMAT_PPM ||
+ formatType == FORMAT_JPEG ||
+ formatType == FORMAT_PNG)
+ {
+ char name[FILENAME_MAX];
+
+ sprintf(name,fileName,frameCount);
+
+ vpImageIo::write(I, name);
+ }
+ else
+ {
+#ifdef VISP_HAVE_FFMPEG
+ ffmpeg->saveFrame(I);
+#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ cv::Mat matFrame, rgbMatFrame;
+ vpImageConvert::convert(I, matFrame);
+ cv::cvtColor(matFrame, rgbMatFrame, cv::COLOR_GRAY2BGR);
+ writer << rgbMatFrame;
+#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ cv::Mat matFrame, rgbMatFrame;
+ vpImageConvert::convert(I, matFrame);
+ cv::cvtColor(matFrame, rgbMatFrame, CV_GRAY2BGR);
+ writer << rgbMatFrame;
+#endif
+ }
+
+ frameCount++;
+}
+
+
+/*!
+ Deallocates parameters use to write the video or the image sequence.
+*/
+void vpVideoWriter::close()
+{
+ if (!isOpen)
+ {
+ vpERROR_TRACE("The video has to be open first with the open method");
+ throw (vpException(vpException::notInitialized,"file not yet opened"));
+ }
+ #ifdef VISP_HAVE_FFMPEG
+ if (ffmpeg != NULL)
+ {
+ ffmpeg->endWrite();
+ }
+ #endif
+}
+
+
+/*!
+ Gets the format of the file(s) which has/have to be written.
+
+ \return Returns the format.
+*/
+vpVideoWriter::vpVideoFormatType
+vpVideoWriter::getFormat(const char *filename)
+{
+ std::string sfilename(filename);
+
+ std::string ext = vpVideoWriter::getExtension(sfilename);
+
+ if (ext.compare(".PGM") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".pgm") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".PPM") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".ppm") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".JPG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".JPEG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpeg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".PNG") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".png") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".AVI") == 0)
+ return FORMAT_AVI;
+ else if (ext.compare(".avi") == 0)
+ return FORMAT_AVI;
+ else if (ext.compare(".MPEG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpeg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPEG4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mpeg4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MP4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mp4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MOV") == 0)
+ return FORMAT_MOV;
+ else if (ext.compare(".mov") == 0)
+ return FORMAT_MOV;
+ else
+ return FORMAT_UNKNOWN;
+}
+
+// return the extension of the file including the dot
+std::string vpVideoWriter::getExtension(const std::string &filename)
+{
+ // extract the extension
+ size_t dot = filename.find_last_of(".");
+ std::string ext = filename.substr(dot, filename.size()-1);
+ return ext;
+}
diff --git a/modules/robot/CMakeLists.txt b/modules/robot/CMakeLists.txt
new file mode 100644
index 0000000..92a4158
--- /dev/null
+++ b/modules/robot/CMakeLists.txt
@@ -0,0 +1,102 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+if(USE_AFMA4 OR USE_AFMA6 OR USE_VIPER650 OR USE_VIPER850)
+ list(APPEND opt_incs ${RAW1394_INCLUDE_DIRS})
+ list(APPEND opt_incs ${CALINUX_INCLUDE_DIRS})
+ list(APPEND opt_incs ${IRISA_INCLUDE_DIRS})
+ list(APPEND opt_libs ${IRISA_LIBRARIES})
+ list(APPEND opt_libs ${CALINUX_LIBRARIES})
+ list(APPEND opt_libs ${RAW1394_LIBRARIES})
+ list(APPEND opt_libs ${RT_LIBRARIES})
+endif()
+
+if(USE_BICLOPS)
+ list(APPEND opt_incs ${BICLOPS_INCLUDE_DIRS})
+ list(APPEND opt_libs ${BICLOPS_LIBRARIES})
+endif()
+
+if(USE_PTU46)
+ list(APPEND opt_incs ${PTU46_INCLUDE_DIRS})
+ list(APPEND opt_libs ${PTU46_LIBRARIES})
+endif()
+
+if(USE_ARIA AND UNIX AND USE_PTHREAD AND RT_FOUND AND DL_FOUND)
+ # Under Unix we need Aria, pthread, dl and rt libraries
+ list(APPEND opt_incs ${ARIA_INCLUDE_DIRS})
+ list(APPEND opt_libs ${ARIA_LIBRARIES})
+ list(APPEND opt_incs ${PTHREAD_INCLUDE_DIRS})
+ list(APPEND opt_libs ${PTHREAD_LIBRARIES})
+ list(APPEND opt_libs ${RT_LIBRARIES})
+ list(APPEND opt_libs ${DL_LIBRARIES})
+elseif(USE_ARIA AND NOT UNIX)
+ list(APPEND opt_incs ${ARIA_INCLUDE_DIRS})
+ list(APPEND opt_libs ${ARIA_LIBRARIES})
+endif()
+
+# for the wireframe simulator we can use optional opengl and coin
+if(USE_COIN3D)
+ if(WIN32)
+ add_definitions("-DCOIN_DLL")
+ endif()
+ list(APPEND opt_incs ${COIN3D_INCLUDE_DIRS})
+ # On OSX cmake 2.8 found OpenGL but OPENGL_INCLUDE_DIR was set to NOT_FOUND
+ # We add a test to be sure that the OPENGL vars exist.
+ if(OPENGL_INCLUDE_DIR)
+ list(APPEND opt_incs ${OPENGL_INCLUDE_DIR})
+ endif()
+ if(OPENGL_LIBRARIES)
+ list(APPEND opt_libs ${OPENGL_LIBRARIES})
+ endif()
+ list(APPEND opt_libs ${COIN3D_LIBRARIES})
+endif()
+
+vp_add_module(robot visp_core OPTIONAL visp_io visp_gui)
+vp_glob_module_sources()
+
+# copy robot and wireframe simulator data
+vp_glob_module_copy_data("src/robot-simulator/arms/*.bnd" data/robot-simulator)
+vp_glob_module_copy_data("src/wireframe-simulator/scene/*.bnd" data/wireframe-simulator)
+vp_glob_module_copy_data("src/wireframe-simulator/scene/*.sld" data/wireframe-simulator)
+
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+vp_add_tests(DEPENDS_ON visp_sensor visp_vision visp_blob visp_gui)
diff --git a/modules/robot/include/visp3/robot/vpAfma4.h b/modules/robot/include/visp3/robot/vpAfma4.h
new file mode 100644
index 0000000..08012db
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpAfma4.h
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpAfma4_h
+#define __vpAfma4_h
+
+/*!
+
+ \file vpAfma4.h
+
+ Modelisation of Irisa's cylindrical robot named Afma4.
+
+*/
+
+/*!
+
+ \class vpAfma4
+
+ \ingroup group_robot_real_cylindrical
+
+ \brief Modelisation of Irisa's cylindrical robot named Afma4.
+
+ This robot has five degrees of freedom, but only four motorized
+ joints (joint 3 is not motorized). Joint 2 and 3 are prismatic. The
+ other ones are revolute joints.
+
+ The non modified Denavit-Hartenberg representation of the robot is
+ given in the table below, where \f$q_1^*, q_2^*,q_4^*, q_5^*\f$
+ are the variable joint positions.
+
+ \f[
+ \begin{tabular}{|c|c|c|c|c|}
+ \hline
+ Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
+ \hline
+ 1 & 0 & 0 & 0 & $q_1^*$ \\
+ 2 & $a_1$ & $q_2^*$ & $-\pi/2$ & 0 \\
+ 3 & 0 & $d_3$ & $\pi/2$ & 0 \\
+ 4 & 0 & $d_4$ & $-\pi/2$ & $q_4^*-\pi/2$ \\
+ 5 & 0 & 0 & 0 & $q_5^*$ \\
+ \hline
+ \end{tabular}
+ \f]
+
+ The forward kinematics of the robot is given by the homogeneous
+ matrix \f${^f}M_e\f$ which is implemented in get_fMe().
+
+ \f[
+ {^f}M_e = \left[\begin{array}{cccc}
+ c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
+ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
+ -s_5 & -c_5 & d_4+q_2 \\
+ 0 & 0 & 0 & 1 \\
+ \end{array}
+ \right]
+ \f]
+
+ The robot forward jacobian used to compute the cartesian velocities
+ from joint ones is given and implemented in get_fJe() and
+ get_eJe().
+
+ The robot inverse jacobian used to compute the joint velocities from
+ cartesian ones are given and implemented in get_fJe_inverse().
+
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+
+class VISP_EXPORT vpAfma4
+{
+ public:
+ vpAfma4();
+ /*! Destructor that does nothing. */
+ virtual ~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;
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const;
+ void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
+ void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
+ void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const;
+
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpAfma4 & afma4);
+
+ vpColVector getJointMin() const;
+ vpColVector getJointMax() const;
+
+ public:
+
+ static const unsigned int njoint; ///< Number of joint.
+
+ protected:
+ // Denavit Hartenberg parameters
+ double _a1; // distance along x2
+ double _d3; // distance along z2
+ double _d4; // distance along z3
+ double _joint_max[4]; // Maximal value of the joints
+ double _joint_min[4]; // Minimal value of the joints
+
+ // Minimal representation of _eMc
+ vpTranslationVector _etc; // meters
+ vpRxyzVector _erc; // radian
+
+ vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
+};
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpAfma6.h b/modules/robot/include/visp3/robot/vpAfma6.h
new file mode 100644
index 0000000..1519f43
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpAfma6.h
@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma6 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpAfma6_h
+#define __vpAfma6_h
+
+/*!
+
+ \file vpAfma6.h
+
+ Modelisation of Irisa's gantry robot named Afma6.
+
+*/
+
+/*!
+
+ \class vpAfma6
+
+ \ingroup group_robot_real_gantry group_robot_simu_gantry
+
+ \brief Modelisation of Irisa's gantry robot named Afma6.
+
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+class VISP_EXPORT vpAfma6
+{
+ public:
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ //! File where constant parameters in relation with the robot are stored:
+ //! joint max, min, coupling factor between 4 ant 5 joint, distance between 5
+ //! and 6 joint, tranformation eMc between end-effector and camera frame.
+ static const char * const CONST_AFMA6_FILENAME;
+ static const char * const CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_CAMERA_AFMA6_FILENAME;
+#endif
+ /*!
+ Name of the camera attached to the CCMOP tool (vpAfma6ToolType::TOOL_CCMOP).
+ */
+ static const char * const CONST_CCMOP_CAMERA_NAME;
+ /*!
+ Name of the camera attached to the 2 fingers gripper tool
+ (vpAfma6ToolType::TOOL_GRIPPER).
+ */
+ static const char * const CONST_GRIPPER_CAMERA_NAME;
+ /*!
+ Name of the camera attached to the vacuum gripper tool
+ (vpAfma6ToolType::TOOL_VACUUM).
+ */
+ static const char * const CONST_VACUUM_CAMERA_NAME;
+ /*!
+ Name of the generic camera attached to the robot hand
+ (vpAfma6ToolType::TOOL_GENERIC_CAMERA).
+ */
+ static const char * const CONST_GENERIC_CAMERA_NAME;
+
+ //! List of possible tools that can be attached to the robot end-effector.
+ typedef enum
+ {
+ TOOL_CCMOP, /*!< Pneumatic CCMOP gripper. */
+ TOOL_GRIPPER, /*!< Pneumatic gripper with 2 fingers. */
+ TOOL_VACUUM, /*!< Pneumatic vaccum gripper. */
+ TOOL_GENERIC_CAMERA /*!< A generic camera. */
+ } vpAfma6ToolType;
+
+ //! Default tool attached to the robot end effector
+ static const vpAfma6ToolType defaultTool;
+
+ public:
+ vpAfma6();
+ /*! Destructor that does nothing. */
+ virtual ~vpAfma6() {};
+
+ void init (void);
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ void init (const char * paramAfma6, const char * paramCamera);
+#endif
+ void init (vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel =
+ vpCameraParameters::perspectiveProjWithoutDistortion);
+
+ vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
+ int getInverseKinematics(const vpHomogeneousMatrix & fMc,
+ vpColVector & q, const bool &nearest=true,
+ const bool &verbose=false) const;
+ 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;
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
+ void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ void parseConfigFile (const char * filename);
+#endif
+
+ //! Get the current tool type
+ vpAfma6ToolType getToolType() const {
+ return tool_current;
+ };
+ //! Get the current camera model projection type
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const{
+ return projModel;
+ };
+
+ void getCameraParameters(vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const;
+ void getCameraParameters(vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
+
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpAfma6 & afma6);
+
+ vpColVector getJointMin() const;
+ vpColVector getJointMax() const;
+ double getCoupl56() const;
+ double getLong56() const;
+
+ protected:
+ //! Set the current tool type
+ void setToolType(vpAfma6::vpAfma6ToolType tool){
+ tool_current = tool;
+ };
+
+ public:
+
+ static const unsigned int njoint; ///< Number of joint.
+
+
+ protected:
+ double _coupl_56; // coupling between join 5 and 6
+ double _long_56; // distance between join 5 and 6
+ double _joint_max[6]; // Maximal value of the joints
+ double _joint_min[6]; // Minimal value of the joints
+ // Minimal representation of _eMc
+ vpTranslationVector _etc; // meters
+ vpRxyzVector _erc; // radian
+
+ vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
+
+ protected:
+ //! Current tool in use
+ vpAfma6ToolType tool_current;
+ // Used projection model
+ vpCameraParameters::vpCameraParametersProjType projModel;
+
+};
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpArit.h b/modules/robot/include/visp3/robot/vpArit.h
new file mode 100644
index 0000000..0279f86
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpArit.h
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module contient les procedures arithmetiques.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+#ifndef vpArit_h
+#define vpArit_h
+
+#include <visp3/core/vpConfig.h>
+#include <stdio.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#define ADD_COORD2(r,a,b) { (r).x = (a).x + (b).x;\
+ (r).y = (a).y + (b).y; }
+
+#define ADD_COORD3(r,a,b) { (r).x = (a).x + (b).x;\
+ (r).y = (a).y + (b).y;\
+ (r).z = (a).z + (b).z; }
+
+#define INC_COORD2(r,a) { (r).x += (a).x; (r).y += (a).y; }
+
+#define INC_COORD3(r,a) { (r).x += (a).x; (r).y += (a).y;\
+ (r).z += (a).z; }
+
+#define CROSS_PRODUCT(r,a,b) { (r).x = (a).y * (b).z - (a).z * (b).y;\
+ (r).y = (a).z * (b).x - (a).x * (b).z;\
+ (r).z = (a).x * (b).y - (a).y * (b).x; }
+
+#define DIF_COORD2(r,a,b) { (r).x = (a).x - (b).x;\
+ (r).y = (a).y - (b).y; }
+
+#define DIF_COORD3(r,a,b) { (r).x = (a).x - (b).x;\
+ (r).y = (a).y - (b).y;\
+ (r).z = (a).z - (b).z; }
+
+#define DOT_PRODUCT(a,b) ( ((a).x * (b).x) +\
+ ((a).y * (b).y) +\
+ ((a).z * (b).z) )
+
+#define LENGTH3(a) (sqrt((double) DOT_PRODUCT((a),(a))))
+
+#define MID_COORD3(r,a,b) { (r).x = ((a).x + (b).x) / 2.0;\
+ (r).y = ((a).y + (b).y) / 2.0;\
+ (r).z = ((a).z + (b).z) / 2.0; }
+
+#define MUL_COORD3(r,a,b,c) { (r).x *= (a); (r).y *= (b); (r).z *= (c); }
+
+#define PAR_COORD3(r,t,a,b) { (r).x = ((b).x - (a).x) * (t) + (a).x;\
+ (r).y = ((b).y - (a).y) * (t) + (a).y;\
+ (r).z = ((b).z - (a).z) * (t) + (a).z; }
+
+#define SET_COORD2(r,a,b) { (r).x = (a); (r).y = (b); }
+
+#define SET_COORD3(r,a,b,c) { (r).x = (a); (r).y = (b); (r).z = (c); }
+
+#define SUB_COORD2(r,a) { (r).x -= (a).x; (r).y -= (a).y; }
+
+#define SUB_COORD3(r,a) { (r).x -= (a).x; (r).y -= (a).y;\
+ (r).z -= (a).z; }
+
+#define COORD3_COL(x,y,z,m,i) ( ((x) * (m)[0][i]) +\
+ ((y) * (m)[1][i]) +\
+ ((z) * (m)[2][i]) +\
+ (m)[3][i] )
+
+#define COORD4_COL(x,y,z,w,m,i) ( ((x) * (m)[0][i]) +\
+ ((y) * (m)[1][i]) +\
+ ((z) * (m)[2][i]) +\
+ ((w) * (m)[3][i]) )
+
+#define M_POLY1(x,a,b) ((a) * (x) + (b))
+#define M_POLY2(x,a,b,c) (M_POLY1((x),(a),(b)) * (x) + (c))
+#define M_POLY3(x,a,b,c,d) (M_POLY2((x),(a),(b),(c)) * (x) + (d))
+
+
+typedef struct {
+ int x, y;
+} Point2i;
+
+typedef struct {
+ short x, y;
+} Point2s;
+
+typedef struct {
+ int x, y, z;
+} Point3i;
+
+typedef struct {
+ float x, y, z;
+} Point3f;
+
+typedef struct {
+ float x,y,z,w;
+} Point4f;
+
+typedef struct {
+ float x,y,z;
+} Vector;
+
+/*
+ * MATRIX
+ * ______
+ *
+ * Matrice homogene ou non.
+ * | Rotation | 0 |
+ * Matrice non homogene = | 3x3 | 0 |
+ * |-------------| 0 |
+ * | Translation | 1 |
+ */
+typedef float Matrix[4][4];
+
+#define IDENTITY_MATRIX { {1.0, 0.0, 0.0, 0.0},\
+ {0.0, 1.0, 0.0, 0.0},\
+ {0.0, 0.0, 1.0, 0.0},\
+ {0.0, 0.0, 0.0, 1.0} }
+
+
+/*
+ * 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.
+ */
+typedef struct {
+ Vector rotate; /* vecteur rotation */
+ Vector scale; /* vecteur homothetie */
+ Vector translate; /* vecteur translation */
+} AritPosition;
+
+#define IDENTITY_ROTATE { 0.0, 0.0, 0.0 }
+#define IDENTITY_SCALE { 1.0, 1.0, 1.0 }
+#define IDENTITY_TRANSLATE { 0.0, 0.0, 0.0 }
+
+#define IDENTITY_POSITION { IDENTITY_ROTATE,\
+ IDENTITY_SCALE,\
+ IDENTITY_TRANSLATE }
+
+void fprintf_matrix (FILE *fp, Matrix m);
+void ident_matrix (Matrix m);
+void premult_matrix (Matrix a, Matrix b);
+void premult3_matrix (Matrix a, Matrix b);
+void prescale_matrix (Matrix m, Vector *vp);
+void pretrans_matrix (Matrix m, Vector *vp);
+void postleft_matrix (Matrix m, char axis);
+void postmult_matrix (Matrix a, Matrix b);
+void postmult3_matrix (Matrix a, Matrix b);
+void postscale_matrix (Matrix m, Vector *vp);
+void posttrans_matrix (Matrix m, Vector *vp);
+void transpose_matrix (Matrix m);
+
+float cosin_to_angle (float ca, float sa);
+float norm_vector (Vector *vp);
+void point_matrix (Point4f *p4, Point3f *p3, Matrix m);
+void plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
+void point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op);
+void point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4);
+void rotate_vector (Vector *vp, float a, Vector *axis);
+void upright_vector (Vector *vp, Vector *up);
+
+void Matrix_to_Position (Matrix m, AritPosition *pp);
+void Matrix_to_Rotate (Matrix m, Vector *vp);
+void Position_to_Matrix (AritPosition *pp, Matrix m);
+void Rotate_to_Matrix (Vector *vp, Matrix m);
+void Rotaxis_to_Matrix (float a, Vector *axis, Matrix m);
+void Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m);
+void Scale_to_Matrix (Vector *vp, Matrix m);
+void Translate_to_Matrix (Vector *vp, Matrix m);
+
+void fscanf_Point3f (Point3f *pp);
+void fscanf_Vector (Vector *vp);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpBiclops.h b/modules/robot/include/visp3/robot/vpBiclops.h
new file mode 100644
index 0000000..a1ec61b
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpBiclops.h
@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpBiclops_H
+#define __vpBiclops_H
+
+/* ----------------------------------------------------------------------- */
+/* --- INCLUDES -------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* --- ViSP --- */
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+/* --- GENERAL --- */
+#include <iostream>
+
+/*!
+
+ \class vpBiclops
+
+ \ingroup group_robot_real_ptu
+
+ \brief Jacobian, geometric model functionnalities... for biclops, pan, tilt
+ head.
+
+ Two different Denavit Hartenberg representations of the robot are implemented.
+ As mentionned in vpBiclops::DenavitHartenbergModel they differ in the orientation
+ of the tilt axis. Use setDenavitHartenbergModel() to select the representation.
+
+ See http://www.traclabs.com/tracbiclops.htm for more details concerning the hardware.
+
+*/
+
+class VISP_EXPORT vpBiclops
+{
+public:
+ /*!
+ Two different Denavit Hartenberg representations of the robot are implemented.
+ They differ in the orientation of the tilt axis.
+
+ The first representation, vpBiclops::DH1 is given by:
+
+ \f[
+ \begin{tabular}{|c|c|c|c|c|}
+ \hline
+ Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
+ \hline
+ 1 & 0 & 0 & $-\pi/2$ & $q_1^*$ \\
+ 2 & 0 & 0 & $ \pi/2$ & $q_2^* + \pi/2$ \\
+ \hline
+ \end{tabular}
+ \f]
+
+ The second one, vpBiclops::DH2 is given by:
+
+ \f[
+ \begin{tabular}{|c|c|c|c|c|}
+ \hline
+ Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
+ \hline
+ 1 & 0 & 0 & $ \pi/2$ & $q_1^*$ \\
+ 2 & 0 & 0 & $ -\pi/2$ & $q_2^* - \pi/2$ \\
+ \hline
+ \end{tabular}
+ \f]
+
+ where \f$q_1^*, q_2^*\f$ are respectively the pan and tilt joint positions.
+
+ In those representations, the pan is oriented from left to right, while the tilt is oriented
+ - in vpBiclops::DH1 from down to top,
+ - in vpBiclops::DH2 from top to down.
+
+
+ */
+ typedef enum
+ {
+ DH1, /*!< First Denavit Hartenberg representation. */
+ DH2 /*!< Second Denavit Hartenberg representation. */
+ } DenavitHartenbergModel;
+
+public: /* Constants */
+
+ static const unsigned int ndof; /*!< Number of dof */
+
+ /* Geometric model */
+ static const float h;
+
+ static const float panJointLimit;
+ static const float tiltJointLimit;
+ static const float speedLimit;
+
+
+protected:
+ DenavitHartenbergModel dh_model_;
+ vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
+
+public:
+ vpBiclops (void);
+ /*! Destructor that does nothing. */
+ virtual ~vpBiclops() {};
+
+ void init (void);
+
+ void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
+ vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
+ void computeMGD (const vpColVector &q, vpPoseVector &fvc) const;
+
+ /*!
+ Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame and the end
+ effector frame.
+ */
+ vpHomogeneousMatrix get_cMe() const
+ {
+ return cMe_;
+ }
+
+ void get_cVe(vpVelocityTwistMatrix &_cVe) const;
+ void get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const;
+ void get_fMc (const vpColVector &q, vpPoseVector &fvc) const;
+ vpHomogeneousMatrix get_fMc (const vpColVector &q) const;
+ vpHomogeneousMatrix get_fMe (const vpColVector &q) const;
+
+ void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
+ void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
+
+ /*!
+ Return the Denavit Hartenberg representation used to model the head.
+ \sa vpBiclops::DenavitHartenbergModel
+ */
+ inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const
+ {
+ return dh_model_;
+ }
+
+ void set_cMe() ;
+ /*!
+ Set the transformation between the camera frame and the end effector frame.
+ */
+ void set_cMe(const vpHomogeneousMatrix &cMe)
+ {
+ cMe_ = cMe;
+ }
+ /*!
+ Set the Denavit Hartenberg representation used to model the head.
+
+ \sa vpBiclops::DenavitHartenbergModel
+ */
+ inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1)
+ {
+ dh_model_ = m;
+ }
+
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & constant);
+};
+
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpBound.h b/modules/robot/include/visp3/robot/vpBound.h
new file mode 100644
index 0000000..db6baa5
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpBound.h
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le fichier "bound.h" contient les macros et le types
+ * utilises par le modele geometrique surfacique polygonale 3D.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpBound_H
+#define vpBound_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpMy.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+#define START_ARG 0
+#define NEXT_ARG 1
+
+#define BND_NULL (-1)
+
+#define BND_BLOCK 0
+#define BND_CARDIOIDE 1
+#define BND_CONE 2
+#define BND_CYLINDER 3
+#define BND_SPHERE 4
+#define BND_TORUS 5
+#define BND_WEDGE 6
+
+#define BND_F3 7
+#define BND_F4 8
+
+#define BND_GRID 9
+#define BND_PIPE 10
+#define BND_SECTION 11
+
+#define BND_NBR 12
+
+
+#define BOUND_NBR 1024
+#define FACE_NBR 6144 /* Tailles de tableaux */
+#define VERTEX_NBR 16
+#define POINT_NBR 6144
+#ifdef face_normal
+#define VECTOR_NBR 6144
+#endif //face_normal
+
+#ifdef face_edge
+typedef struct {
+ 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 */
+} Edge_list;
+#endif //face_edge
+
+#define DEFAULT_VSIZE 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;
+ */
+typedef struct {
+ Index nbr; /* nombre de sommets */
+ Index *ptr; /* liste dynamique */
+ Index tbl[DEFAULT_VSIZE];
+} Vertex_list;
+
+typedef struct {
+ Index nbr; /* nombre de points */
+ Point3f *ptr; /* liste dynamique */
+} Point3f_list;
+
+#ifdef face_normal
+typedef struct {
+ Index nbr; /* nombre de vecteurs */
+ Vector *ptr; /* liste dynamique */
+} Vector_list;
+#endif //face_normal
+
+typedef struct {
+ unsigned is_polygonal:1; /* face polygonale */
+ unsigned is_visible :1; /* face affichable */
+#ifdef face_edge
+ Edge_list edge; /* liste d'aretes */
+#endif //face_edge
+ Vertex_list vertex; /* liste de sommets */
+#ifdef face_normal
+ Vector normal; /* vecteur normal */
+#endif //face_normal
+} Face;
+
+typedef struct {
+ Index nbr; /* nombre de faces */
+ Face *ptr; /* liste dynamique */
+} Face_list;
+
+typedef struct {
+ 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 */
+#endif //face_edge
+ Face_list face; /* liste de faces */
+ Point3f_list point; /* points aux sommets */
+#ifdef face_normal
+ Vector_list normal; /* normales aux sommets */
+#endif //face_normal
+} Bound;
+
+typedef struct {
+ Index nbr; /* nombre de surfaces */
+ Bound *ptr; /* liste dynamique */
+} Bound_list;
+
+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 */
+} Bounding_box;
+
+typedef struct {
+ char *name; /* nom de la scene */
+ Bound_list bound; /* liste de surfaces */
+} Bound_scene;
+
+typedef struct {
+ Index nbr; /* nombre de scenes */
+ Bound_scene *ptr; /* liste dynamique */
+} Bound_scene_list;
+
+extern void malloc_huge_Bound (Bound *bp);
+extern void free_huge_Bound (Bound *bp);
+extern void fscanf_Bound (Bound *bp);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpCgiconstants.h b/modules/robot/include/visp3/robot/vpCgiconstants.h
new file mode 100644
index 0000000..e0ce04f
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpCgiconstants.h
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpCgiconstants_H
+#define vpCgiconstants_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/* errors */
+#define NO_ERROR 0
+#define ENOTCGCL 1
+#define ENOTCGOP 2
+#define ENOTVSOP 3
+#define ENOTVSAC 4
+#define ENOTOPOP 5
+#define EVSIDINV 10
+#define ENOWSTYP 11
+#define EMAXVSOP 12
+#define EVSNOTOP 13
+#define EVSISACT 14
+#define EVSNTACT 15
+#define EINQALTL 16
+#define EBADRCTD 20
+#define EBDVIEWP 21
+#define ECLIPTOL 22
+#define ECLIPTOS 23
+#define EVDCSDIL 24
+#define EBTBUNDL 30
+#define EBBDTBDI 31
+#define EBTUNDEF 32
+#define EBADLINX 33
+#define EBDWIDTH 34
+#define ECINDXLZ 35
+#define EBADCOLX 36
+#define EBADMRKX 37
+#define EBADSIZE 38
+#define EBADFABX 39
+#define EPATARTL 40
+#define EPATSZTS 41
+#define ESTYLLEZ 42
+#define ENOPATNX 43
+#define EPATITOL 44
+#define EBADTXTX 45
+#define EBDCHRIX 46
+#define ETXTFLIN 47
+#define ECEXFOOR 48
+#define ECHHTLEZ 49
+#define ECHRUPVZ 50
+#define ECOLRNGE 51
+#define ENMPTSTL 60
+#define EPLMTWPT 61
+#define EPLMTHPT 62
+#define EGPLISFL 63
+#define EARCPNCI 64
+#define EARCPNEL 65
+#define ECELLATS 66
+#define ECELLPOS 67
+#define ECELLTLS 68
+#define EVALOVWS 69
+#define EPXNOTCR 70
+#define EINDNOEX 80
+#define EINDINIT 81
+#define EINDALIN 82
+#define EINASAEX 83
+#define EINAIIMP 84
+#define EINNTASD 85
+#define EINTRNEX 86
+#define EINNECHO 87
+#define EINECHON 88
+#define EINEINCP 89
+#define EINERVWS 90
+#define EINETNSU 91
+#define EINENOTO 92
+#define EIAEVNEN 93
+#define EINEVNEN 94
+#define EBADDATA 95
+#define ESTRSIZE 96
+#define EINQOVFL 97
+#define EINNTRQE 98
+#define EINNTRSE 99
+#define EINNTQUE 100
+#define EMEMSPAC 110
+#define ENOTCSTD 111
+#define ENOTCCPW 112
+#define EFILACC 113
+#define ECGIWIN 114
+
+/* devices */
+#define BW1DD 1
+#define BW2DD 2
+#define CG1DD 3
+#define BWPIXWINDD 4
+#define CGPIXWINDD 5
+#define GP1DD 6
+#define CG2DD 7
+#define CG4DD 8
+#define PIXWINDD 9
+
+#define VWSURF_NEWFLG 1
+#define _CGI_KEYBORDS 1
+#define _CGI_LOCATORS 4
+#define _CGI_STROKES 3
+#define _CGI_VALUATRS 3
+#define _CGI_CHOICES 3
+#define _CGI_PICKS 3
+#define MAXVWS 5
+#define MAXTRIG 6
+#define MAXASSOC 5
+#define MAXEVENTS 1024
+
+/* limits */
+#define MAXAESSIZE 10 /* maximum number of AES table entries */
+#define MAXNUMPATS 50 /* maximum number of pattern table entries */
+#define MAXPATSIZE 256 /* maximum pattern size */
+
+#define MAXPTS 1024 /* maximum number of pts per polygon */
+#define MAXCHAR 256 /* maximum number of chars in a string */
+
+#define OUTFUNS 67 /* number of output functions */
+#define INFUNS 22 /* number of input functions */
+
+/* attributes */
+
+/* fonts */
+#define ROMAN 0
+#define GREEK 1
+#define SCRIPT 2
+#define OLDENGLISH 3
+#define STICK 4
+#define SYMBOLS 5
+
+#define DEVNAMESIZE 20
+/* Warning: Because of the ; separators,
+ * do not use the following macro unless it is surrounded with { }s.
+ * Beware interactions with if-else and ?: constructs.
+ */
+#define NORMAL_VWSURF(dev,surf) \
+ dev.screenname[0] = '\0'; \
+ dev.windowname[0] = '\0'; \
+ dev.windowfd = 0; \
+ dev.retained = 0; \
+ dev.dd = surf; \
+ dev.cmapsize = 0; \
+ dev.cmapname[0] ='\0'; \
+ dev.flags = 0; \
+ dev.ptr= '\0' ;
+
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpImageSimulator.h b/modules/robot/include/visp3/robot/vpImageSimulator.h
new file mode 100644
index 0000000..f1d6b4c
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpImageSimulator.h
@@ -0,0 +1,313 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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: Class which enables to project an image in the 3D space
+ * and get the view of a virtual camera.
+ *
+ *
+ * Authors:
+ * Amaury Dame
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpImageSimulator_h
+#define vpImageSimulator_h
+
+/*!
+ \class vpImageSimulator
+
+ \ingroup group_robot_image_simu
+
+ \brief Class which enables to project an image in the 3D space and get the view of a virtual camera.
+
+ The image is represented by a rectangular image whose corners coordinates are known in the 3D frame linked to the 3D rectangle.
+
+ The 3D rectangle is positionned relative to a virtual camera (represented by its intrinsic parameters). Indeed, the pose \f$ cMt \f$ has to be given by the user of the class.
+
+ And finally, the view of the virtual camera is given by the geImage() method.
+
+ You can use a colored or a gray scaled image.
+
+ To avoid the aliasing especially when the camera is very near from the image plane, a bilinear interpolation can be done for every pixels which have to be filled in. By default this functionality is not used because it consumes lot of time.
+
+ The following example explain how to use the class.
+
+ \code
+ #include <visp3/core/vpImage.h>
+ #include <visp3/robot/vpImageSimulator.h>
+
+ int main()
+ {
+ vpImage<vpRGBa> Icamera(480,640,0);
+ vpImage<vpRGBa> Iimage(60,60);
+
+ // Initialise the image which will be projected into the image Icamera
+ vpRGBa colorb(0,0,255);
+ vpRGBa colorw(255,255,255);
+ vpRGBa colorr(255,0,0);
+ for(int i = 0; i < 60; i++)
+ {
+ for(int j = 0; j < 20; j++)
+ Iimage[i][j] = colorb;
+ for(int j = 20; j < 40; j++)
+ Iimage[i][j] = colorw;
+ for(int j = 40; j < 60; j++)
+ Iimage[i][j] = colorr;
+ }
+
+ // Initialise the 3D coordinates of the Iimage corners
+ vpColVector X[4];
+ for (int i = 0; i < 4; i++) X[i].resize(3);
+ // Top left corner
+ X[0][0] = -1;
+ X[0][1] = -1;
+ X[0][2] = 0;
+
+ // Top right corner
+ X[1][0] = 1;
+ X[1][1] = -1;
+ X[1][2] = 0;
+
+ // Bottom right corner
+ X[2][0] = 1;
+ X[2][1] = 1;
+ X[2][2] = 0;
+
+ //Bottom left corner
+ X[3][0] = -1;
+ X[3][1] = 1;
+ X[3][2] = 0;
+
+ vpImageSimulator sim;
+ sim.init(Iimage, X);
+
+ sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
+
+ vpCameraParameters cam(868.0, 869.0, 320, 240);
+
+ sim.getImage(Icamera,cam);
+
+ return 0;
+ }
+ \endcode
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTriangle.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPoint.h>
+
+#include <vector>
+#include <list>
+
+class VISP_EXPORT vpImageSimulator
+{
+ public:
+ typedef enum {
+ COLORED,
+ GRAY_SCALED
+ } vpColorPlan;
+
+ typedef enum {
+ SIMPLE,
+ BILINEAR_INTERPOLATION
+ } vpInterpolationType;
+
+ private:
+ vpColVector X[4];
+ vpHomogeneousMatrix cMt;
+ vpColVector X2[4];
+ std::vector<vpPoint> pt;
+ std::vector<vpPoint> ptClipped;
+
+ vpInterpolationType interp;
+
+ //normal repere objet
+ vpColVector normal_obj;
+ //normal repere cam
+ vpColVector normal_Cam;
+ //normal repere cam sous forme de pointeur sinon vpColVector prend trop de temps
+ double *normal_Cam_optim;
+
+ //distance du plan au centre de camera
+ double distance;
+ double visible_result;
+ bool visible;
+
+
+ //point 3D du plan definissant ou sera le pixel (0,0) de l'image
+ double *X0_2_optim;
+
+ //variable pour intersection algebre : vecteurs de la base 2D et leur norme
+ double euclideanNorm_u,euclideanNorm_v;
+
+ //2 vecteur 3D (typiquement deux cotes du plan) qui definissent le repere 2D u,v de l'image
+ vpColVector vbase_u,vbase_v;
+ //version pointeur des vecteurs de base
+ double *vbase_u_optim;
+ double *vbase_v_optim;
+
+ //allocation memoire du point 3D d'intersection entre le vecteur (centre_cam - point_plan_image) et le plan
+ double *Xinter_optim;
+
+ //triangles de projection du plan
+ std::vector<vpTriangle> listTriangle;
+
+ //image de texture
+ vpColorPlan colorI;
+ vpImage<unsigned char> Ig;
+ vpImage<vpRGBa> Ic;
+
+ vpRect rect;
+ bool cleanPrevImage;
+ bool setBackgroundTexture; // flag set when the background is to a texture using setBackGroundTexture()
+ vpColor bgColor;
+
+ vpColVector focal;
+
+ //boolean to tell if the points in the camera frame have to be clipped
+ bool needClipping;
+
+ public:
+ vpImageSimulator(const vpColorPlan &col = COLORED);
+ vpImageSimulator(const vpImageSimulator &text);
+ virtual ~vpImageSimulator();
+
+ vpImageSimulator& operator=(const vpImageSimulator& sim);
+
+
+ //creation du plan a partir de ses coordonnees 3D ds repere objet et de son image texture
+ void init(const vpImage<unsigned char> &I,vpColVector* X);
+ void init(const vpImage<vpRGBa> &I,vpColVector* X);
+#ifdef VISP_HAVE_MODULE_IO
+ void init(const char* file_image,vpColVector* X);
+#endif
+ void init(const vpImage<unsigned char> &I, const std::vector<vpPoint>& X);
+ void init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X);
+#ifdef VISP_HAVE_MODULE_IO
+ void init(const char* file_image, const std::vector<vpPoint>& X);
+#endif
+
+ //projection du plan par cMo => creation des deux triangles definissant projection du plan sur plan image (coord en metre)
+ void setCameraPosition(const vpHomogeneousMatrix &cMt);
+
+ void setInterpolationType (const vpInterpolationType interplt) {this->interp = interplt;}
+
+ void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam);
+ void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam);
+
+ void getImage(vpImage<unsigned char> &I, vpImage<unsigned char> &Isrc,
+ const vpCameraParameters &cam);
+ void getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
+ const vpCameraParameters &cam);
+
+ void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ vpMatrix &zBuffer);
+ void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
+ vpMatrix &zBuffer);
+
+ static void getImage(vpImage<unsigned char> &I,
+ std::list<vpImageSimulator> &list,
+ const vpCameraParameters &cam);
+ static void getImage(vpImage<vpRGBa> &I,
+ std::list <vpImageSimulator> &list,
+ const vpCameraParameters &cam);
+
+ std::vector<vpColVector> get3DcornersTextureRectangle();
+
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/);
+
+ /*!
+ As it can be time consuming to reset all the image to a default baground value, this function enable to reset only the pixel which changed the previous time.
+
+ By default this functionality is disabled. and the background color is white.
+
+ \param clean : Enable the reset method.
+ \param color : Color of the back ground.
+ */
+ void setCleanPreviousImage(const bool &clean, const vpColor &color = vpColor::white) {
+ cleanPrevImage = clean;
+ bgColor = color;
+ }
+
+ /*!
+ This function is to set the background to a texture instead of default black
+ The corresponding flag is set
+ \param Iback: Image/Texture for the background
+ */
+ inline void
+ setBackGroundTexture(const vpImage<unsigned char>& Iback) {
+ setBackgroundTexture = true;
+ Ig = Iback;
+ }
+
+ private:
+ void initPlan(vpColVector* X);
+
+ //result = plan est visible.
+ //ie: un plan est oriente dans si normal_plan.focal < 0 => plan est visible sinon invisible.
+ bool isVisible() {return visible;}
+
+ //function that project a point x,y on the plane, return true if the projection is on the limited plane
+ // and in this case return the corresponding image pixel Ipixelplan
+ bool getPixel(const vpImagePoint &iP,unsigned char &Ipixelplan);
+ bool getPixel(const vpImagePoint &iP,vpRGBa &Ipixelplan);
+ bool getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP,
+ unsigned char &Ipixelplan);
+ bool getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
+ vpRGBa &Ipixelplan);
+ bool getPixelDepth(const vpImagePoint &iP, double &Zpixelplan);
+ bool getPixelVisibility(const vpImagePoint &iP, double &Zpixelplan);
+
+ //operation 3D de base :
+ void project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt,
+ vpColVector &_vout);
+ //donne coordonnes homogenes de _v;
+ void getHomogCoord(const vpColVector &_v, vpColVector &_vH);
+ //donne coordonnes _v en fction coord homogenes _vH;
+ void getCoordFromHomog(const vpColVector &_vH, vpColVector &_v);
+
+ void getRoi(const unsigned int &Iwidth, const unsigned int &Iheight,
+ const vpCameraParameters &cam, const std::vector<vpPoint> &point, vpRect &rect);
+};
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+
diff --git a/modules/robot/include/visp3/robot/vpImstack.h b/modules/robot/include/visp3/robot/vpImstack.h
new file mode 100644
index 0000000..96de22c
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpImstack.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "imstack.h" contient les macros, les types et
+ * les specifications des procedures de gestion de la pile
+ * des images (IMage STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+#ifndef vpImstack_H
+#define vpImstack_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void fprintf_imstack (void);
+void load_imstack (void);
+void pop_imstack (void);
+void push_imstack (void);
+void swap_imstack (void);
+void add_imstack (void);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpKeyword.h b/modules/robot/include/visp3/robot/vpKeyword.h
new file mode 100644
index 0000000..c64f1a1
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpKeyword.h
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "keyword.c" contient les procedures de gestion
+ * des mots cles retournes par l'analyseur lexical "lex".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpKeyword_H
+#define vpKeyword_H
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpMy.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+extern Index get_symbol (char *ident, int length);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpLex.h b/modules/robot/include/visp3/robot/vpLex.h
new file mode 100644
index 0000000..5982501
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpLex.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "lex.c" contient les procedures de gestion
+ * de l'analyse lexicale de l'analyseur lexicale "lex"
+ * d'un fichier source dont la grammaire possede
+ * les symboles terminaux suivants (ecrit en "LEX", UNIX) :
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpLex_h
+#define vpLex_h
+
+#include <visp3/core/vpConfig.h>
+#include <stdio.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+int lex(void);
+void unlex (void);
+void lexerr (const char* path, ...);
+void pusherr (const char *str);
+void popuperr (const char *str);
+void poperr (void);
+int lexecho (FILE *f, int token);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpMy.h b/modules/robot/include/visp3/robot/vpMy.h
new file mode 100644
index 0000000..65de18b
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpMy.h
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Module de Macros et de Types de bases en langage "C".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+#ifndef vpMy_H
+#define vpMy_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#ifndef NULL
+#define NULL 0
+#endif
+#ifndef FALSE
+#define FALSE 0
+#endif
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef STDIN
+#define STDIN 0
+#endif
+#ifndef STDOUT
+#define STDOUT 1
+#endif
+#ifndef STDERR
+#define STDERR 2
+#endif
+
+#define NAMESIZE 80
+#define LINESIZE 256
+
+
+#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)) (MIN) = (M);\
+ else if ((M) > (MAX)) (MAX) = (M)
+
+#define TWO_POWER(P) (((P) > 0) ? 1 << (P) : 1)
+#define SWAP(A,B,T) { (T) = (A); (A) = (B); (B) = (T); }
+
+typedef unsigned char Byte;
+typedef unsigned short Index;
+typedef char Type;
+
+void fscanf_float (float *fp);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpPioneer.h b/modules/robot/include/visp3/robot/vpPioneer.h
new file mode 100644
index 0000000..cdae6cc
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpPioneer.h
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Common features for Pioneer unicycle mobile robots.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef VPPIONEER_H
+#define VPPIONEER_H
+
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpUnicycle.h>
+
+
+/*!
+
+ \class vpPioneer
+
+ \ingroup group_robot_real_unicycle group_robot_simu_unicycle
+
+ \brief Generic functions for Pioneer mobile robots.
+
+ This class provides common features for Pioneer mobile robots.
+ This robot has two control velocities \f$(v_x, w_z)\f$, the translational and
+ rotational velocities of the mobile platform respectively.
+
+ The figure below shows the position of the frames that are used to model the robot.
+ The end effector frame is here located at the middle point between the two wheels.
+
+ \image html pioneer.png
+
+ The robot jacobian at the end effector frame, the point located at the
+ middle between the two wheels is given by:
+
+ \f[
+ {^e}{\bf J}_e = \left(\begin{array}{cc}
+ 1 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 1 \\
+ \end{array}
+ \right)
+ \f]
+
+ Considering \f$(v_x, w_z)\f$, it is possible to compute \f$\bf v\f$ the six
+ dimention velocity skew expressed at the end effector frame by:
+
+ \f[
+ {\bf v} = {^e}{\bf J}_e \;
+ \left(\begin{array}{c}
+ v_x \\
+ w_z \\
+ \end{array}
+ \right)
+ \f].
+
+*/
+class VISP_EXPORT vpPioneer: public vpUnicycle
+{
+public:
+ /*!
+ Create a default Pioneer robot.
+ */
+ vpPioneer() : vpUnicycle()
+ {
+ set_cMe();
+ set_eJe();
+ }
+
+ /*!
+ Destructor that does nothing.
+ */
+ virtual ~vpPioneer() {};
+
+private:
+ /*!
+ Set the transformation between the camera frame and the mobile platform
+ end effector frame.
+ */
+ void set_cMe()
+ {
+ // Position of the camera in the mobile platform frame
+ double l = 0.13; // distance between the camera frame and the mobile robot frame
+ vpTranslationVector cte; // meters
+ vpRxyzVector cre; // radian
+ cte.set(0, 0, -l);
+ cre.buildFrom(vpMath::rad(90.), 0, vpMath::rad(90.));
+ cMe_.buildFrom(cte, vpRotationMatrix(cre));
+ }
+
+ /*!
+ Set the robot jacobian at the end effector frame, the point located at the
+ middle between the two wheels.
+
+ Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z]\f$ with
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
+ of the mobile robot and \f$\bf v\f$ the six dimention velocity skew expressed at the
+ end effector frame, the robot jacobian is given by:
+
+ \f[
+ {^e}{\bf J}_e = \left(\begin{array}{cc}
+ 1 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 0 \\
+ 0 & 1 \\
+ \end{array}
+ \right)
+ \f]
+
+ */
+ void set_eJe()
+ {
+ eJe_.resize(6, 2); // pioneer jacobian expressed at point M
+ eJe_ = 0;
+ eJe_[0][0] = 1; // vx
+ eJe_[5][1] = 1; // wz
+ }
+};
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpPioneerPan.h b/modules/robot/include/visp3/robot/vpPioneerPan.h
new file mode 100644
index 0000000..2265985
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpPioneerPan.h
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Common features for Pioneer unicycle mobile robots.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef VPPIONEERPAN_H
+#define VPPIONEERPAN_H
+
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpUnicycle.h>
+
+
+/*!
+
+ \class vpPioneerPan
+
+ \ingroup group_robot_real_unicycle group_robot_simu_unicycle
+
+ \brief Generic functions for Pioneer mobile robots equiped with a pan head.
+
+ This class provides common features for Pioneer mobile robots equiped with a pan head.
+
+ This robot has three control velocities \f$(v_x, w_z, \dot{q_1})\f$, the translational and
+ rotational velocities of the mobile platform, the pan head velocity respectively.
+
+ The figure below shows the position of the frames that are used to model the robot.
+ The end effector frame is here located at the pan axis.
+
+ \image html pioneer-pan.png
+
+ Considering
+ \f[{\bf v} = {^e}{\bf J}_e \;
+ \left(\begin{array}{c}
+ v_x \\
+ w_z \\
+ \dot{q_1} \\
+ \end{array}
+ \right)
+ \f]
+ with
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
+ of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
+ the six dimention velocity skew expressed at point E in frame E,
+ the robot jacobian is given by:
+
+ \f[
+ {^e}{\bf J}_e = \left(\begin{array}{ccc}
+ c_1 & -c_1*p_y - s_1*p_x & 0 \\
+ 0 & 0 & 0 \\
+ s_1 & -s_1*p_y + c_1*p_x & 0 \\
+ 0 & 0 & 0 \\
+ 0 & -1 & 1 \\
+ 0 & 0 & 0 \\
+ \end{array}
+ \right)
+ \f]
+
+ with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
+ located at the middle point between the two weels.
+
+*/
+class VISP_EXPORT vpPioneerPan: public vpUnicycle
+{
+public:
+ /*!
+ Create a pioneer mobile robot equiped with a pan head.
+ */
+ vpPioneerPan() : mMp_(), pMe_()
+ {
+ double q = 0; // Initial position of the pan axis
+ set_mMp();
+ set_pMe( q );
+ set_cMe();
+ set_eJe( q );
+ }
+
+ /*!
+ Destructor that does nothing.
+ */
+ virtual ~vpPioneerPan() {};
+
+ /*!
+ Set the robot jacobian expressed at point E the end effector frame located on the pan head.
+
+ Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z, \dot{q_1}]\f$ with
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
+ of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
+ the six dimention velocity skew expressed at point E in frame E,
+ the robot jacobian is given by:
+
+ \f[
+ {^e}{\bf J}_e = \left(\begin{array}{ccc}
+ c_1 & -c_1*p_y - s_1*p_x & 0 \\
+ 0 & 0 & 0 \\
+ s_1 & -s_1*p_y + c_1*p_x & 0 \\
+ 0 & 0 & 0 \\
+ 0 & -1 & 1 \\
+ 0 & 0 & 0 \\
+ \end{array}
+ \right)
+ \f]
+
+ with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
+ located at the middle point between the two weels.
+
+ */
+ void set_eJe(double q_pan)
+ {
+ double px = mMp_[0][3];
+ double py = mMp_[1][3];
+ double c1 = cos(q_pan);
+ double s1 = sin(q_pan);
+
+ eJe_.resize(6,3); // robot jacobian expressed at the pan head end effector
+
+ eJe_ = 0;
+ eJe_[0][0] = c1;
+ eJe_[0][1] = -c1*py - s1*px;
+
+ eJe_[2][0] = s1;
+ eJe_[2][1] = -s1*py + c1*px;
+
+ eJe_[4][1] = -1;
+ eJe_[4][2] = 1;
+ }
+
+protected:
+ /*!
+ Set the transformation between the camera frame and the pan head end effector frame.
+ */
+ void set_cMe()
+ {
+ // Position of the camera in the pan frame
+ double cx = 0;
+ double cy = -0.065; // distance between camera and tilt axis
+ double cz = 0;
+ vpTranslationVector etc(cx, cy, cz);
+ vpRotationMatrix eRc;
+ eRc[0][0] = eRc[1][1] = eRc[2][2] = 0;
+ eRc[0][2] = 1;
+ eRc[1][1] = 1;
+ eRc[2][0] = -1;
+
+ vpHomogeneousMatrix eMc;
+ eMc.buildFrom(etc, eRc);
+
+ cMe_ = eMc.inverse();
+ }
+
+ /*!
+ Set the transformation between the mobile platform frame
+ located at the middle point between the two weels and the base frame of the pan head.
+ */
+ void set_mMp()
+ {
+ // Position of the pan head in the mobile platform frame
+ double px = 0.103; // distance between the pan frame and the robot frame
+ double py = 0;
+ double pz = 0.27;
+ vpTranslationVector mtp;
+ mtp.set(px, py, pz);
+
+ vpRotationMatrix mRp; // set to Identity
+ mRp[1][1] = mRp[2][2] = -1.;
+
+ mMp_.insert(mtp);
+ mMp_.insert(mRp);
+ }
+
+ /*!
+ Set the transformation between the pan head reference frame and the
+ end-effector frame.
+
+ \param q : Position in rad of the pan axis.
+
+ */
+ void set_pMe(const double q)
+ {
+ vpRotationMatrix pRe;
+ pRe[0][0] = cos(q);
+ pRe[0][2] = pRe[1][0] = sin(q);
+ pRe[1][1] = pRe[2][2] = 0.;
+ pRe[2][1] = 1.;
+ pRe[1][2] = - pRe[0][0];
+
+ pMe_.insert(pRe);
+ }
+
+protected:
+ vpHomogeneousMatrix mMp_; // constant
+ vpHomogeneousMatrix pMe_; // depends on q pan
+
+};
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpPtu46.h b/modules/robot/include/visp3/robot/vpPtu46.h
new file mode 100644
index 0000000..4d4ee3c
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpPtu46.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ptu-46 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef __vpPtu46_H
+#define __vpPtu46_H
+
+
+/* ----------------------------------------------------------------------- */
+/* --- INCLUDES -------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* --- GENERAL --- */
+#include <iostream>
+
+/* --- ViSP --- */
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoseVector.h>
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+/*!
+
+ \class vpPtu46
+
+ \ingroup group_robot_real_ptu
+
+ \brief Jacobian, geometric model functionnalities... for ptu46, pan, tilt
+ head from Directed Perception.
+
+ See http://www.DPerception.com for more details.
+
+*/
+
+
+class VISP_EXPORT vpPtu46
+{
+
+public: /* Constants */
+
+ /** Nombre d'articulations du robot. */
+ static const unsigned int ndof; /*!< Number of dof */
+
+ /** Geometric model */
+ static const float L;
+ static const float h;
+
+
+public: /* Methodes publiques */
+
+ vpPtu46 (void);
+ /*! Destructor that does nothing. */
+ virtual ~vpPtu46() {};
+
+ void init (void);
+
+ void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
+ vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
+ void computeMGD (const vpColVector & q, vpPoseVector & r) const;
+
+ void get_cMe(vpHomogeneousMatrix &_cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &_cVe) const;
+ void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
+ void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
+
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & constant);
+};
+
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRfstack.h b/modules/robot/include/visp3/robot/vpRfstack.h
new file mode 100644
index 0000000..388fcb5
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRfstack.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "rfstack.h" contient les macros, les types et
+ * les specifications des procedures de gestion de la pile
+ * des drapeaux d'elimination de faces (Remove Faces STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+#ifndef vpRfstack_h
+#define vpRfstack_h
+
+#include <visp3/core/vpConfig.h>
+#include <stdio.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void fprintf_rfstack (FILE *fp);
+int *get_rfstack (void);
+void load_rfstack (int i);
+void pop_rfstack (void);
+void push_rfstack (void);
+void swap_rfstack (void);
+void add_rfstack (int i);
+void sub_rfstack (int i);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRingLight.h b/modules/robot/include/visp3/robot/vpRingLight.h
new file mode 100644
index 0000000..a20e667
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRingLight.h
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Ring light management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRingLight_h
+#define vpRingLight_h
+
+/*!
+ \file vpRingLight.h
+ \brief Ring light management under unix.
+*/
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_IO) && defined(VISP_HAVE_PARPORT)
+
+#include <iostream>
+
+#include <visp3/robot/vpRingLight.h>
+#include <visp3/io/vpParallelPort.h>
+/*!
+
+ \class vpRingLight
+ \ingroup group_robot_real_gantry
+ \brief Ring light management under unix.
+
+ \warning This class works only at Irisa with the Edixia's ring light system.
+
+ Here is an example showing how to synchronise the framegrabbing with
+ the lighting system.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpRingLight.h>
+
+int main()
+{
+#if defined(VISP_HAVE_PARPORT) && defined(VISP_HAVE_DC1394)
+ vp1394TwoGrabber g; // Firewire framegrabber based on libdc1394-2.x third party lib
+ vpImage<unsigned char> I;
+
+ vpRingLight light; // Open the device to access to the ring light.
+
+ for (int i=0; i < 10; i++) {
+ light.pulse(); // Send a pulse to the lighting system
+ g.acquire(I); // Acquire an image
+ }
+#endif
+}
+ \endcode
+
+ Here is an example showing how to turn on the lighting during 10 seconds.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRingLight.h>
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+#ifdef VISP_HAVE_PARPORT
+ vpRingLight light; // Open the device to access to the ring light.
+
+ int nsec = 10; // Time to wait in seconds
+ light.on(); // Turn the ring light on
+ vpTime::wait(nsec * 1000); // Wait 10 s
+ light.off(); // and then turn the ring light off
+#endif
+}
+ \endcode
+
+
+
+*/
+class VISP_EXPORT vpRingLight
+{
+
+public:
+ vpRingLight();
+ ~vpRingLight();
+
+ void pulse();
+ void pulse(double time);
+ void on();
+ void off();
+
+private:
+ vpParallelPort parport;
+
+} ;
+
+#endif
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobot.h b/modules/robot/include/visp3/robot/vpRobot.h
new file mode 100644
index 0000000..b3eb07a
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobot.h
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic virtual robot.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpRobot_H
+#define vpRobot_H
+
+/*!
+ \file vpRobot.h
+ \brief class that defines a generic virtual robot
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpPoseVector.h>
+
+
+/*!
+ \class vpRobot
+ \ingroup group_robot_real_gantry group_robot_real_cylindrical group_robot_real_arm
+ \ingroup group_robot_real_ptu group_robot_real_unicycle group_robot_real_template
+ \brief class that defines a generic virtual robot
+*/
+class VISP_EXPORT vpRobot
+{
+public:
+ /*!
+ Robot control states.
+ */
+ typedef enum
+ {
+ STATE_STOP, /*!< Stops robot motion especially in velocity and
+ acceleration control. */
+ STATE_VELOCITY_CONTROL, //!< Initialize the velocity controller.
+ STATE_POSITION_CONTROL, //!< Initialize the position controller.
+ STATE_ACCELERATION_CONTROL //!< Initialize the acceleration controller.
+ } vpRobotStateType ;
+
+ /*!
+ Robot control frames.
+ */
+ typedef enum
+ {
+ REFERENCE_FRAME, /*!< Corresponds to a fixed reference frame
+ attached to the robot structure. */
+ ARTICULAR_FRAME, /*!< Corresponds to the joint space. */
+ CAMERA_FRAME, /*!< Corresponds to a frame attached to the
+ camera mounted on the robot end-effector. */
+ MIXT_FRAME /*!< Corresponds to a "virtual" frame where
+ translations are expressed in the reference frame, and
+ rotations in the camera frame.*/
+ } vpControlFrameType ;
+
+private: /* Membres privees */
+ vpRobot::vpRobotStateType stateRobot;
+ vpRobot::vpControlFrameType frameRobot;
+
+protected:
+ double maxTranslationVelocity;
+ static const double maxTranslationVelocityDefault;// = 0.2;
+ double maxRotationVelocity;
+ static const double maxRotationVelocityDefault;// = 0.7;
+
+ //! number of degrees of freedom
+ int nDof ;
+ //! robot Jacobian expressed in the end-effector frame
+ vpMatrix eJe ;
+ //! is the robot Jacobian expressed in the end-effector frame available
+ int eJeAvailable ;
+ //! robot Jacobian expressed in the robot reference frame available
+ vpMatrix fJe ;
+ //! is the robot Jacobian expressed in the robot reference frame available
+ int fJeAvailable ;
+
+ int areJointLimitsAvailable ;
+ double *qmin;
+ double *qmax ;
+
+ bool verbose_;
+
+public:
+ vpRobot (void);
+ vpRobot (const vpRobot &robot);
+ virtual ~vpRobot();
+
+ //---------- Jacobian -----------------------------
+ //! Get the robot Jacobian expressed in the end-effector frame
+ virtual void get_eJe(vpMatrix &_eJe) = 0 ;
+ //! Get the robot Jacobian expressed in the robot reference (or world) frame.
+ virtual void get_fJe(vpMatrix &_fJe) = 0 ;
+
+ //! Get a displacement (frame as to ve specified) between two successive position control.
+ virtual void getDisplacement(const vpRobot::vpControlFrameType frame,
+ vpColVector &q) = 0 ;
+
+ double getMaxTranslationVelocity (void) const ;
+ double getMaxRotationVelocity (void) const;
+ //! Get the robot position (frame has to be specified).
+ virtual void getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &q) = 0 ;
+
+ // Return the robot position (frame has to be specified).
+ vpColVector getPosition (const vpRobot::vpControlFrameType frame);
+ virtual vpRobotStateType getRobotState (void) const { return stateRobot ; }
+
+ virtual void init() = 0 ;
+
+ vpRobot & operator=(const vpRobot &robot);
+
+ static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false);
+
+ void setMaxRotationVelocity (const double maxVr);
+ void setMaxTranslationVelocity (const double maxVt);
+ //! Set a displacement (frame has to be specified) in position control.
+ virtual void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &q) = 0 ;
+ virtual vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
+
+ //! Set the velocity (frame has to be specified) that will be applied to the velocity controller.
+ virtual void setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &vel) = 0 ;
+ inline void setVerbose(bool verbose) { verbose_ = verbose; };
+
+protected:
+ vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame);
+ vpControlFrameType getRobotFrame (void) const { return frameRobot ; }
+} ;
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotAfma4.h b/modules/robot/include/visp3/robot/vpRobotAfma4.h
new file mode 100644
index 0000000..80c028a
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotAfma4.h
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotAfma4_h
+#define vpRobotAfma4_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_AFMA4
+
+#include <iostream>
+#include <stdio.h>
+
+#include <visp3/robot/vpRobot.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpAfma4.h>
+
+// low level controller api
+extern "C" {
+# include "irisa_Afma4.h"
+# include "trycatch.h"
+}
+
+
+/*!
+ \class vpRobotAfma4
+
+ \ingroup group_robot_real_cylindrical
+
+ \brief Control of Irisa's cylindrical robot named Afma4.
+
+ Implementation of the vpRobot class in order to control Irisa's
+ Afma4 robot. This robot is a cylindrical robot with five degrees of
+ freedom but only four are actuated (see vpAfma4 documentation). It
+ was manufactured in 1995 by the french Afma-Robots company. In 2008,
+ the low level controller change for a more recent Adept technology
+ based on the MotionBlox controller. A firewire camera is mounted on
+ the end-effector to allow eye-in-hand visual servoing. The control
+ of this camera is achieved by the vp1394TwoGrabber class. A
+ Servolens lens is attached to the camera. It allows to control the
+ focal lens, the iris and the focus throw a serial link. The control
+ of the lens is possible using the vpServolens class.
+
+ This class allows to control the Afma4 cylindrical robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+
+ Mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame is not implemented.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The Denavit-Hartenberg representation as well as the direct and
+ inverse kinematics models are given and implemented in the vpAfma4
+ class.
+
+ \warning A Ctrl-C, a segmentation fault or other system errors are
+ catched by this class to stop the robot.
+
+ To communicate with the robot, you may first create an instance of this
+ class by calling the default constructor:
+
+ \code
+ vpRobotAfma4 robot;
+ \endcode
+
+ This initialize the robot kinematics with the eMc extrinsic camera
+ parameters.
+
+ To control the robot in position, you may set the controller
+ to position control and than send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+ vpColVector q(4);
+ // Set a joint position
+ q[0] = M_PI/2; // X axis, in radian
+ q[1] = 0.2; // Y axis, in meter
+ q[2] = -M_PI/2; // A axis, in radian
+ q[3] = M_PI/8; // B axis, in radian
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ The robot moves to the specified position with the default
+ positioning velocity vpRobotAfma4::defaultPositioningVelocity. The
+ setPositioningVelocity() method allows to change the maximal
+ velocity used to reach the desired position.
+
+ \code
+ // Set the max velocity to 40%
+ robot.setPositioningVelocity(40);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and than send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = M_PI/8; // X axis, in rad/s
+ qvel[1] = 0.2; // Y axis, in m/s
+ qvel[2] = 0; // A axis, in rad/s
+ qvel[3] = M_PI/8; // B axis, in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ while (...) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP)
+ \endcode
+
+ There is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and writePosFile()
+ methods.
+*/
+class VISP_EXPORT vpRobotAfma4
+ :
+ public vpAfma4,
+ public vpRobot
+{
+
+private: /* Not allowed functions. */
+
+ /*!
+ Copy constructor not allowed.
+ */
+ vpRobotAfma4 (const vpRobotAfma4 & robot);
+
+private: /* Attributs prives. */
+
+ /** \brief Vrai ssi aucun objet de la classe vpRobotAfma4 n'existe.
+ *
+ * Il ne peut exister simultanement qu'un seul objet de la classe
+ * vpRobotAfma4, car il correspond a un seul robot AFMA4. Creer
+ * simultanement deux objets peut engendrer des conflits. Le constructeur
+ * lance une erreur si le champ n'est pas FAUX puis positionne le champ
+ * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
+ * alors la creation d'un nouvel objet.
+ */
+ static bool robotAlreadyCreated;
+
+ double positioningVelocity;
+
+ // Variables used to compute the measured velocities (see getVelocity() )
+ vpColVector q_prev_getvel;
+ vpHomogeneousMatrix fMc_prev_getvel;
+ double time_prev_getvel;
+ bool first_time_getvel;
+
+ // Variables used to compute the measured displacement (see
+ // getDisplacement() )
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+
+public: /* Constantes */
+
+ /* Vitesse maximale par default lors du positionnement du robot.
+ * C'est la valeur a la construction de l'attribut prive \a
+ * positioningVelocity. Cette valeur peut etre changee par la fonction
+ * #setPositioningVelocity.
+ */
+ static const double defaultPositioningVelocity; // = 20.0;
+
+public: /* Methode publiques */
+
+ vpRobotAfma4 (bool verbose=true);
+ virtual ~vpRobotAfma4 (void);
+
+ void getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position, double ×tamp);
+
+ double getPositioningVelocity (void);
+ bool getPowerState();
+
+ double getTime() const;
+
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity);
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp);
+
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_cVf(vpVelocityTwistMatrix &cVf) const;
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void init (void);
+
+ void move(const char *filename) ;
+
+ void powerOn() ;
+ void powerOff() ;
+
+ static bool readPosFile(const char *filename, vpColVector &q) ;
+ static bool savePosFile(const char *filename, const vpColVector &q) ;
+
+ /* --- POSITIONNEMENT --------------------------------------------------- */
+ void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &position) ;
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double q1, const double q2,
+ const double q4, const double q5) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+
+ /* --- ETAT ------------------------------------------------------------- */
+
+ vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+
+ /* --- VITESSE ---------------------------------------------------------- */
+
+ void setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & velocity);
+
+ void stopMotion() ;
+
+private:
+ void getArticularDisplacement(vpColVector &displacement);
+ void getCameraDisplacement(vpColVector &displacement);
+};
+
+#endif
+#endif /* #ifndef vpRobotAfma4_h */
diff --git a/modules/robot/include/visp3/robot/vpRobotAfma6.h b/modules/robot/include/visp3/robot/vpRobotAfma6.h
new file mode 100644
index 0000000..0a35c83
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotAfma6.h
@@ -0,0 +1,353 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma6 robot controlled by an Adept MotionBlox.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotAfma6_h
+#define vpRobotAfma6_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_AFMA6
+
+#include <iostream>
+#include <stdio.h>
+
+#include <visp3/robot/vpRobot.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpAfma6.h>
+
+// low level controller api
+extern "C" {
+# include "irisa_Afma6.h"
+# include "trycatch.h"
+}
+
+
+/*!
+ \class vpRobotAfma6
+
+ \ingroup group_robot_real_gantry
+
+ \brief Control of Irisa's gantry robot named Afma6.
+
+ Implementation of the vpRobot class in order to control Irisa's
+ Afma6 robot. This robot is a gantry robot with six degrees of
+ freedom manufactured in 1992 by the french Afma-Robots company. In
+ 2008, the low level controller change for a more recent Adept
+ technology based on the MotionBlox controller. A firewire camera is
+ mounted on the end-effector to allow eye-in-hand visual
+ servoing. The control of this camera is achieved by the
+ vp1394TwoGrabber class. A ring light is attached around the
+ camera. The control of this ring light is possible throw the
+ vpRingLight class. A CCMOP gripper is also mounted on the
+ end-effector. The pneumatic control of this gripper is possible
+ throw the openGripper() or closeGripper() member functions.
+
+ This class allows to control the Afma6 gantry robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The direct and inverse kinematics models are implemented in the
+ vpAfma6 class.
+
+ \warning A Ctrl-C, a segmentation fault or other system errors are
+ catched by this class to stop the robot.
+
+ To communicate with the robot, you may first create an instance of this
+ class by calling the default constructor:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotAfma6.h>
+
+#ifdef VISP_HAVE_AFMA6
+int main()
+{
+ vpRobotAfma6 robot;
+}
+#else
+int main() {}
+#endif
+ \endcode
+
+ This initialize the robot kinematics with the eMc extrinsic camera
+ parameters obtained with a projection model without distortion. To
+ set the robot kinematics with the eMc matrix obtained with a camera
+ perspective model including distortion you need to initialize the
+ robot with:
+
+ \code
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distortion parameter
+ robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ You can get the intrinsic camera parameters of the image I
+ acquired with the camera, with:
+
+ \code
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distortion.
+ \endcode
+
+ To control the robot in position, you may set the controller
+ to position control and than send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+ vpColVector q(6);
+ // Set a joint position
+ q[0] = 0.1; // x axis, in meter
+ q[1] = 0.2; // y axis, in meter
+ q[2] = 0.3; // z axis, in meter
+ q[3] = M_PI/8; // rotation around A axis, in rad
+ q[4] = M_PI/4; // rotation around B axis, in rad
+ q[5] = M_PI; // rotation around C axis, in rad
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ The robot moves to the specified position with the default
+ positioning velocity vpRobotAfma6::defaultPositioningVelocity. The
+ setPositioningVelocity() method allows to change the maximal
+ velocity used to reach the desired position.
+
+ \code
+ // Set the max velocity to 40%
+ robot.setPositioningVelocity(40);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and than send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // x axis, in m/s
+ qvel[1] = 0.2; // y axis, in m/s
+ qvel[2] = 0; // z axis, in m/s
+ qvel[3] = M_PI/8; // rotation around A axis, in rad/s
+ qvel[4] = 0; // rotation around B axis, in rad/s
+ qvel[5] = 0; // rotation around C axis, in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ while (...) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP)
+ \endcode
+
+ There is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and writePosFile()
+ methods.
+*/
+class VISP_EXPORT vpRobotAfma6
+ :
+ public vpAfma6,
+ public vpRobot
+{
+
+private: /* Not allowed functions. */
+
+ /*!
+ Copy constructor not allowed.
+ */
+ vpRobotAfma6 (const vpRobotAfma6 & robot);
+
+private: /* Attributs prives. */
+
+ /** \brief Vrai ssi aucun objet de la classe vpRobotAfma6 n'existe.
+ *
+ * Il ne peut exister simultanement qu'un seul objet de la classe
+ * vpRobotAfma6, car il correspond a un seul robot AFMA6. Creer
+ * simultanement deux objets peut engendrer des conflits. Le constructeur
+ * lance une erreur si le champ n'est pas FAUX puis positionne le champ
+ * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
+ * alors la creation d'un nouvel objet.
+ */
+ static bool robotAlreadyCreated;
+
+ double positioningVelocity;
+
+ // Variables used to compute the measured velocities (see getVelocity() )
+ vpColVector q_prev_getvel;
+ vpHomogeneousMatrix fMc_prev_getvel;
+ double time_prev_getvel;
+ bool first_time_getvel;
+
+ // Variables used to compute the measured displacement (see
+ // getDisplacement() )
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+ vpHomogeneousMatrix fMc_prev_getdis;
+
+
+public: /* Constantes */
+
+ /* Vitesse maximale par default lors du positionnement du robot.
+ * C'est la valeur a la construction de l'attribut prive \a
+ * positioningVelocity. Cette valeur peut etre changee par la fonction
+ * #setPositioningVelocity.
+ */
+ static const double defaultPositioningVelocity; // = 20.0;
+
+public: /* Methode publiques */
+
+ vpRobotAfma6 (bool verbose=true);
+ virtual ~vpRobotAfma6 (void);
+
+ bool checkJointLimits(vpColVector& jointsStatus);
+
+ void closeGripper() ;
+
+ void getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position, double ×tamp);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position, double ×tamp);
+
+ double getPositioningVelocity (void);
+ bool getPowerState();
+ double getTime() const;
+
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity);
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp);
+
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &_cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &_cVe) const;
+ void get_eJe(vpMatrix &_eJe);
+ void get_fJe(vpMatrix &_fJe);
+
+ void init (void);
+ void init (vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType
+ projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
+
+ void move(const char *filename) ;
+ void move(const char *filename, const double velocity) ;
+
+ void openGripper() ;
+
+ void powerOn() ;
+ void powerOff() ;
+
+ static bool readPosFile(const char *filename, vpColVector &q) ;
+ static bool savePosFile(const char *filename, const vpColVector &q) ;
+
+ /* --- POSITIONNEMENT --------------------------------------------------- */
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const vpPoseVector & pose );
+ void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &position) ;
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1, const double pos2, const double pos3,
+ const double pos4, const double pos5, const double pos6) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+
+ /* --- ETAT ------------------------------------------------------------- */
+
+ vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+
+
+ /* --- VITESSE ---------------------------------------------------------- */
+
+ void setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & velocity);
+
+ void stopMotion() ;
+
+private:
+ void getArticularDisplacement(vpColVector &displacement);
+ void getCameraDisplacement(vpColVector &displacement);
+};
+
+
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
+#endif /* #ifndef vpRobotAfma6_h */
diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h
new file mode 100644
index 0000000..abca970
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_BICLOPS
+
+#ifndef __vpROBOT_BICLOPS_H
+#define __vpROBOT_BICLOPS_H
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INCLUDES ----------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/* --- GENERAL --- */
+#include <iostream>
+#include <stdio.h>
+#include <pthread.h>
+
+
+/* --- ViSP --- */
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpBiclops.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotBiclopsController.h>
+
+
+/* ------------------------------------------------------------------------ */
+/* --- CLASS -------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+
+/*!
+ \class vpRobotBiclops
+
+ \ingroup group_robot_real_ptu
+
+ \brief Interface for the biclops, pan, tilt head control.
+
+ See http://www.traclabs.com/biclopspt.html for more details.
+
+ This class provide a position and a speed control interface for the biclops
+ head. To manage the biclops joint limits in speed control, a control loop is
+ running in a seperate thread (see vpRobotBiclopsSpeedControlLoop()).
+
+ The control of the head is done by vpRobotBiclopsController class.
+
+ \warning Velocity control mode is not exported from the top-level Biclops API
+ class provided by Traclabs. That means that there is no protection in this
+ mode to prevent an axis from striking its hard limit. In position mode,
+ Traclabs put soft limits in that keep any command from driving to a position
+ too close to the hard limits. In velocity mode this protection does not exist
+ in the current API.
+
+ \warning With the understanding that hitting the hard limits at full
+ speed/power can damage the unit, damage due to velocity mode commanding is
+ under user responsibility.
+
+
+*/
+class VISP_EXPORT vpRobotBiclops
+ :
+ public vpBiclops,
+ public vpRobot
+{
+
+private:
+
+ void getCameraDisplacement(vpColVector &d);
+ void getArticularDisplacement(vpColVector &d);
+
+private:
+ static bool robotAlreadyCreated;
+ pthread_t control_thread;
+
+ char configfile[FILENAME_MAX]; // Biclops config file
+
+ vpRobotBiclopsController controller;
+
+ double positioningVelocity;
+ vpColVector q_previous;
+ bool controlThreadCreated;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// /*! \brief No copy constructor allowed. */
+// vpRobotBiclops(const vpRobotBiclops &)
+// : vpBiclops(), vpRobot(), control_thread(), controller(),
+// positioningVelocity(0), q_previous(), controlThreadCreated(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpRobotBiclops &operator=(const vpRobotBiclops &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+
+ static const double defaultPositioningVelocity;
+
+ vpRobotBiclops (void);
+ vpRobotBiclops (const char * filename);
+ virtual ~vpRobotBiclops (void);
+
+ void init (void);
+
+ void get_cMe(vpHomogeneousMatrix &_cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &_cVe) const;
+ void get_eJe(vpMatrix &_eJe) ;
+ void get_fJe(vpMatrix &_fJe) ;
+
+ void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d);
+ void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
+ double getPositioningVelocity (void);
+ void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+
+ bool readPositionFile(const char *filename, vpColVector &q) ;
+
+ void setConfigFile (const char * filename="/usr/share/BiclopsDefault.cfg");
+ void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) ;
+ void setPosition (const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+ vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
+ void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & q_dot);
+
+ void stopMotion() ;
+
+ static void * vpRobotBiclopsSpeedControlLoop (void * arg);
+};
+
+
+
+#endif /* #ifndef __vpROBOT_BICLOPS_H */
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h
new file mode 100644
index 0000000..52fb141
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h
@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_BICLOPS
+
+#ifndef __vpROBOT_BICLOPS_CONTROLLER_H
+#define __vpROBOT_BICLOPS_CONTROLLER_H
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INCLUDES ----------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/* --- GENERAL --- */
+#include <iostream> /* Classe std::ostream. */
+#include <stdio.h> /* Classe std::ostream. */
+#include <pthread.h> /* Classe std::ostream. */
+
+#include <visp3/core/vpConfig.h>
+
+#include "Biclops.h" // Contrib for Biclops robot
+#include "PMDUtils.h" // Contrib for Biclops robot
+
+#if defined(_WIN32)
+class VISP_EXPORT Biclops; //needed for dll creation
+#endif
+
+/* ------------------------------------------------------------------------ */
+/* --- CLASS ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ \class vpRobotBiclopsController
+
+ \ingroup group_robot_real_ptu
+
+ \brief Interface to Biclops, pan, tilt, verge head for computer vision
+ applications.
+
+ See http://www.traclabs.com/tracbiclops.htm for more details.
+
+ This class uses libraries libBiclops.so, libUtils.so and libPMD.so and
+ includes Biclops.h and PMDUtils.h provided by Traclabs.
+
+*/
+class VISP_EXPORT vpRobotBiclopsController
+{
+public:
+ typedef enum {
+ STOP, /*!< Have to stop the robot. */
+ SPEED /*!< Can send the desired speed. */
+ } vpControllerStatusType;
+
+public:
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // SHM
+ typedef struct /* ControllerShm_struct */ {
+ vpControllerStatusType status[2];
+ double q_dot[2]; /*!< Desired speed. */
+ double actual_q[2]; /*!< Current measured position of each axes. */
+ double actual_q_dot[2]; /*!< Current measured velocity of each axes. */
+ bool jointLimit[2]; /*!< Indicates if an axe is in joint limit. */
+ } shmType;
+#endif /* DOXYGEN_SHOULD_SKIP_THIS */
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpRobotBiclopsController(const vpRobotBiclopsController &)
+// : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL),
+// panProfile(), tiltProfile(), vergeProfile(), shm(), stopControllerThread_(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpRobotBiclopsController &operator=(const vpRobotBiclopsController &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpRobotBiclopsController();
+ virtual ~vpRobotBiclopsController();
+ void init(const char *configfile);
+ void setPosition(const vpColVector & q, const double percentVelocity);
+ void setVelocity(const vpColVector & q_dot);
+ vpColVector getPosition();
+ vpColVector getActualPosition();
+ vpColVector getVelocity();
+ vpColVector getActualVelocity();
+ PMDAxisControl * getPanAxis() { return panAxis; };
+ PMDAxisControl * getTiltAxis() { return tiltAxis; };
+ PMDAxisControl * getVergeAxis() { return vergeAxis; };
+ void writeShm(shmType &shm);
+ shmType readShm();
+ bool isStopRequested()
+ {
+ return stopControllerThread_;
+ }
+
+ void stopRequest(bool stop)
+ {
+ stopControllerThread_ = stop;
+ }
+
+private:
+ Biclops biclops; // THE interface to Biclops.
+ int axisMask;
+
+ // Pointers to each axis (populated once controller is initialized).
+ PMDAxisControl *panAxis;
+ PMDAxisControl *tiltAxis;
+ PMDAxisControl *vergeAxis;
+
+ PMDAxisControl::Profile panProfile;
+ PMDAxisControl::Profile tiltProfile;
+ PMDAxisControl::Profile vergeProfile;
+
+ shmType shm;
+ bool stopControllerThread_;
+
+};
+
+#endif /* #ifndef __vpROBOT_BICLOPS_CONTROLLER_H */
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotCamera.h b/modules/robot/include/visp3/robot/vpRobotCamera.h
new file mode 100644
index 0000000..abea253
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotCamera.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines the simplest robot : a free flying camera.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRobotCamera_H
+#define vpRobotCamera_H
+
+/*!
+ \file vpRobotCamera.h
+ \brief class that defines the simplest robot : a free flying camera
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/robot/vpRobotSimulator.h>
+
+/*!
+ \class vpRobotCamera
+ \ingroup group_robot_simu_camera
+
+ \deprecated This class is deprecated since ViSP 3.0.0.
+
+ \brief Class that defines the simplest robot: a free flying camera. We recommend to use
+ vpSimulatorCamera instead.
+
+ This free flying camera has 6 dof; 3 in translation and 3 in rotation.
+ It evolves as a gentry robot with respect to a world frame. This class
+ is similar to vpSimulatorCamera class except that here the position of the robot
+ is provided as the transformation from camera frame to world frame; cMw. Since
+ the position of the camera frame evolves, this representation is less intuitive
+ than the one implemented in vpSimulatorCamera where the transformation from world
+ to camera frame is considered; wMc.
+
+ For this particular simulated robot, the end-effector and camera frame are confused.
+ That means that the cMe transformation is equal to identity.
+
+ The robot jacobian expressed in the end-effector frame
+ \f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
+
+ The following code shows how to control this robot in position and velocity.
+ \code
+#include <visp3/robot/vpRobotCamera.h>
+
+int main()
+{
+ vpHomogeneousMatrix cMw;
+ vpRobotCamera robot;
+
+ robot.getPosition(cMw); // Position of the camera in the world frame
+ std::cout << "Default position of the camera in the world frame cMw:\n" << cMw << std::endl;
+
+ cMw[2][3] = 1.; // World frame is 1 meter along z axis in front of the camera frame
+ robot.setPosition(cMw); // Set the new position of the camera wrt the world frame
+ std::cout << "New position of the camera wrt the world frame cMw:\n" << cMw << std::endl;
+
+ robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
+ robot.setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ robot.setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
+
+ vpColVector v(6);
+ v = 0;
+ v[2] = 1.; // set v_z to 1 m/s
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ // The robot has moved from 0.1 meters along the z axis
+ robot.getPosition(cMw); // Position of the camera wrt the world frame
+ std::cout << "New position of the camera cMw:\n" << cMw << std::endl;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpRobotCamera : public vpRobotSimulator
+{
+protected:
+ vpHomogeneousMatrix cMw_; // camera to world
+
+public:
+ vpRobotCamera() ;
+ virtual ~vpRobotCamera() ;
+
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_eJe(vpMatrix &eJe);
+
+ void getPosition(vpHomogeneousMatrix &cMw) const ;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+
+ void setPosition(const vpHomogeneousMatrix &cMw) ;
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) ;
+
+private:
+ void init() ;
+
+ // Non implemented virtual pure functions
+ void get_fJe(vpMatrix & /*_fJe */) {};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
+} ;
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h
new file mode 100644
index 0000000..7c9751f
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotException.h
@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception that can be emited by the vpRobot class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpRobotException_H
+#define __vpRobotException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpRobotException.h
+ \brief error that can be emited by the vpRobot class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpRobotException
+ \brief Error that can be emited by the vpRobot class and its derivates.
+ */
+class VISP_EXPORT vpRobotException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpRobot member
+ */
+ enum errorRobotCodeEnum
+ {
+
+ /** Erreur lancee par le 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. */
+ uniqueRobotError,
+
+ /** Erreur lancee par les fonctions de commande si le
+ * robot n'est pas dans le bon etat au moment du passage
+ * d'ordre. */
+ wrongStateError,
+
+ /** Erreur lancee par les fonctions de changement d'etat
+ * si le changement demandee n'est pas possible. */
+ stateModificationError,
+
+ /** Erreur generee lors d'un retour non nulle d'une fonction
+ * de communication de la lib Afma6. */
+ communicationError,
+
+ /** Erreur lancee apres un appel a une fonction de la lib
+ * bas-niveau de control de l'afma6 ayant renvoye une erreur. */
+ lowLevelError,
+
+ /** Erreur lancee par la fonction de parsing des parametres du
+ * robot, si le fichier donne en entree n'est pas valide.
+ */
+ 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. */
+ 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'.
+ */
+ notImplementedError,
+ /** Position is out of range.
+ */
+ positionOutOfRangeError
+ } ;
+
+ public:
+ vpRobotException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpRobotException (const int id, const std::string & msg)
+ : vpException(id, msg)
+ {
+ }
+ vpRobotException (const int id)
+ : vpException(id)
+ {
+ }
+
+};
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotPioneer.h b/modules/robot/include/visp3/robot/vpRobotPioneer.h
new file mode 100644
index 0000000..9155334
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotPioneer.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for Pioneer mobile robots based on Aria 3rd party library.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef VPROBOTPIONEER_H
+#define VPROBOTPIONEER_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_PIONEER
+
+#include <Aria.h>
+
+// Warning: vpMath.h included from vpRobot.h or vpPioneer.h should be
+// included after Aria.h to avoid the build issue:
+// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
+// in this scope"
+// This error is due to cmath header included from vpMath.h that makes
+// isfinite() ambiguous between ::isfinite() and std::isfinite()
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpPioneer.h>
+
+/*!
+
+ \class vpRobotPioneer
+
+ \ingroup group_robot_real_unicycle
+
+ \brief Interface for Pioneer mobile robots based on Aria 3rd party library.
+
+ This class provides a position and speed control interface for Pioneer mobile robots.
+ It inherits from the Aria ArRobot class. For more information about the model of the robot,
+ see vpPioneer documentation.
+
+*/
+class VISP_EXPORT vpRobotPioneer: public vpRobot, public vpPioneer, public ArRobot
+{
+private: /* Not allowed functions. */
+
+ /*!
+ Copy constructor not allowed.
+ */
+ vpRobotPioneer(const vpRobotPioneer &robot);
+
+public:
+ vpRobotPioneer();
+ virtual ~vpRobotPioneer();
+
+ /*!
+ Get the robot Jacobian expressed at point E, the point located at the
+ middle between the two wheels.
+
+ \param eJe : Robot jacobian such as \f$(v_x, w_z) = {^e}{\bf J}e \; {\bf v}\f$ with
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
+ of the mobile robot, \f$\bf v\f$ the six dimention velocity skew, and where
+
+ \sa get_eJe()
+
+ */
+ void get_eJe(vpMatrix & eJe)
+ {
+ eJe = vpUnicycle::get_eJe();
+ }
+
+private: // Set as private since not implemented
+ /*!
+ Get the robot Jacobian expressed in the robot reference (or world) frame.
+ \warning Not implemented.
+ */
+ void get_fJe(vpMatrix & /*fJe*/) {} ;
+
+ /*!
+ Get a displacement (frame as to ve specified) between two successive position control.
+ \warning Not implemented.
+ */
+ void getDisplacement(const vpRobot::vpControlFrameType /*frame*/, vpColVector &/*q*/) {};
+
+public:
+ void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+
+private: // Set as private since not implemented
+ /*!
+ Get the robot position (frame has to be specified).
+ \warning Not implemented.
+ */
+ void getPosition(const vpRobot::vpControlFrameType /*frame*/, vpColVector &/*q*/) {};
+
+public:
+ void init();
+
+private: // Set as private since not implemented
+ /*!
+ Set a displacement (frame has to be specified) in position control.
+ \warning Not implemented.
+ */
+ void setPosition(const vpRobot::vpControlFrameType /*frame*/, const vpColVector &/*q*/) {};
+
+public:
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel);
+
+ /*!
+ Enable or disable sonar device usage.
+ */
+ void useSonar(bool usage)
+ {
+ this->comInt(ArCommands::SONAR, usage);
+ }
+
+protected:
+ bool isInitialized;
+};
+
+#endif
+
+#endif // VPROBOTPIONEER_H
+
diff --git a/modules/robot/include/visp3/robot/vpRobotPtu46.h b/modules/robot/include/visp3/robot/vpRobotPtu46.h
new file mode 100644
index 0000000..4a55b46
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotPtu46.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ptu-46 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_PTU46
+
+
+#ifndef __vpROBOT_PTU46_H
+#define __vpROBOT_PTU46_H
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INCLUDES ----------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/* --- GENERAL --- */
+#include <iostream>
+#include <stdio.h>
+
+
+/* --- ViSP --- */
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpPtu46.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+#include <ptu.h> // Contrib for Ptu-46 robot
+
+/* ------------------------------------------------------------------------ */
+/* --- CLASS ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+/*!
+
+ \class vpRobotPtu46
+
+ \ingroup group_robot_real_ptu
+
+ \brief Interface for the Directed Perception ptu-46 pan, tilt head .
+
+ See http://www.DPerception.com for more details.
+
+ This class provide a position and a speed control interface for the ptu-46
+ head.
+
+*/
+class VISP_EXPORT vpRobotPtu46
+ :
+ public vpPtu46,
+ public vpRobot
+{
+
+private:
+
+ /*! \brief No copy constructor allowed. */
+ vpRobotPtu46 (const vpRobotPtu46 & ass);
+
+ /*! Object to control. This is a contribution. */
+ Ptu ptu;
+
+private:
+ static bool robotAlreadyCreated;
+ double positioningVelocity;
+ int velocityMesureTempo;
+ char *device;
+
+public:
+ static const double defaultPositioningVelocity;
+
+ vpRobotPtu46 (const char *device="/dev/ttyS0");
+ vpRobotPtu46 (vpRobotPtu46 * pub);
+ virtual ~vpRobotPtu46 (void);
+
+ void get_cMe(vpHomogeneousMatrix &_cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &_cVe) const;
+ void get_eJe(vpMatrix &_eJe);
+ void get_fJe(vpMatrix &_fJe);
+
+ void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
+ double getPositioningVelocity (void);
+ void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+
+ void init (void);
+
+ bool readPositionFile(const char *filename, vpColVector &q);
+
+ void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) ;
+ void setPosition (const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+ vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+
+ void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & q_dot);
+
+ void stopMotion() ;
+
+private:
+ void getCameraDisplacement(vpColVector &d);
+ void getArticularDisplacement(vpColVector &d);
+};
+
+
+
+#endif /* #ifndef __vpROBOT_PTU46_H */
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotSimulator.h b/modules/robot/include/visp3/robot/vpRobotSimulator.h
new file mode 100644
index 0000000..099b075
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotSimulator.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic class used to make robot simulators.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotSimulator_HH
+#define vpRobotSimulator_HH
+
+/*!
+ \file vpRobotSimulator.h
+ \brief Basic class used to make robot simulators.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+
+/*!
+ \class vpRobotSimulator
+
+ \ingroup group_robot_simu_gantry group_robot_simu_arm group_robot_simu_unicycle
+ \ingroup group_robot_simu_camera
+
+ \brief This class aims to be a basis used to create all the
+ robot simulators.
+
+*/
+class VISP_EXPORT vpRobotSimulator : public vpRobot
+{
+protected:
+ double delta_t_; // sampling time in second
+
+public:
+ vpRobotSimulator();
+ /*!
+ Basic destructor
+ */
+ virtual ~vpRobotSimulator() {};
+
+ /*!
+ Return the sampling time.
+
+ \return Sampling time in second used to compute the robot displacement from
+ the velocity applied to the robot during this time.
+ */
+ inline double getSamplingTime() const
+ {
+ return(this->delta_t_);
+ }
+
+ /*!
+ Set the sampling time.
+
+ \param delta_t : Sampling time in second used to compute the robot displacement from
+ the velocity applied to the robot during this time.
+
+ */
+ virtual inline void setSamplingTime(const double &delta_t)
+ {
+ this->delta_t_ = delta_t;
+ }
+};
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotTemplate.h b/modules/robot/include/visp3/robot/vpRobotTemplate.h
new file mode 100644
index 0000000..372bbf9
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotTemplate.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a robot just to show which function you must implement.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpRobotTemplate_H
+#define vpRobotTemplate_H
+
+/*!
+ \file vpRobotTemplate.h
+ \brief class that defines a robot just to show which function you must implement
+*/
+
+#include <visp3/robot/vpRobot.h>
+
+/*!
+ \class vpRobotTemplate
+ \ingroup group_robot_real_template
+ \brief class that defines a robot just to show which function you must implement
+*/
+
+class VISP_EXPORT vpRobotTemplate : public vpRobot
+{
+
+public:
+
+ //! basic initialization
+ void init() ;
+
+ //! constructor
+ vpRobotTemplate() ;
+ //! destructor
+ virtual ~vpRobotTemplate() ;
+
+
+ //! get the robot Jacobian expressed in the end-effector frame
+ void get_eJe(vpMatrix &_eJe) ;
+ //! get the robot Jacobian expressed in the robot reference frame
+ void get_fJe(vpMatrix &_fJe) ;
+
+ //! send to the controller a velocity expressed in the camera frame
+ void sendCameraVelocity(const vpColVector &v) ;
+ //! send to the controller a velocity expressed in the articular frame
+ void sendArticularVelocity(const vpColVector &qdot) ;
+ //! send to the controller a velocity (frame as to be specified)
+ void setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &vel) ;
+
+ //! get a position expressed in the robot reference frame
+ void getPosition(vpPoseVector &q) ;
+ //! get a position expressed in the articular frame
+ void getArticularPosition(vpColVector &q) ;
+ //! get a displacement (frame as to be specified)
+ void getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &q) ;
+ //! set a displacement (frame as to be specified)
+ void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &q) ;
+
+ //! get a displacement (frame as to be specified)
+ void getDisplacement(const vpRobot::vpControlFrameType frame,
+ vpColVector &q) ;
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/robot/include/visp3/robot/vpRobotViper650.h b/modules/robot/include/visp3/robot/vpRobotViper650.h
new file mode 100644
index 0000000..379f6f9
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotViper650.h
@@ -0,0 +1,429 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Viper S650 robot controlled by an Adept MotionBlox.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotViper650_h
+#define vpRobotViper650_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_VIPER650
+
+#include <iostream>
+#include <stdio.h>
+
+#include <visp3/robot/vpRobot.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper650.h>
+
+// low level controller api
+extern "C" {
+# include "irisa_Viper650.h"
+# include "trycatch.h"
+}
+
+
+/*!
+ \class vpRobotViper650
+
+ \ingroup group_robot_real_arm
+
+ \brief Control of Irisa's Viper S650 robot named Viper650.
+
+ Implementation of the vpRobot class in order to control Irisa's
+ Viper650 robot. This robot is an ADEPT six degrees of freedom arm.
+ A firewire camera is mounted on the end-effector to allow
+ eye-in-hand visual servoing. The control of this camera is achieved
+ by the vp1394TwoGrabber class.
+
+ This class allows to control the Viper650 arm robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The direct and inverse kinematics models are implemented in the
+ vpViper650 class.
+
+ \warning A Ctrl-C, a segmentation fault or other system errors are
+ catched by this class to stop the robot.
+
+ To communicate with the robot, you may first create an instance of this
+ class by calling the default constructor:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+#endif
+}
+ \endcode
+
+ This initialize the robot kinematics with the \f$^e{\bf M}_c\f$
+ extrinsic camera parameters obtained with a projection model without
+ distortion. To set the robot kinematics with the \f$^e{\bf M}_c\f$
+ transformation obtained with a camera perspective model including
+ distortion you need to initialize the robot with:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distortion parameter
+ robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+#endif
+}
+ \endcode
+
+ You can get the intrinsic camera parameters of an image
+ acquired by the camera attached to the robot, with:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+ g.acquire(I);
+
+ vpRobotViper650 robot;
+
+ // ...
+
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distortion.
+#endif
+}
+ \endcode
+
+ To control the robot in position, you may set the controller
+ to position control and than send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ vpColVector q(6);
+ // Set a joint position
+ q[0] = vpMath::rad(10); // Joint 1 position, in rad
+ q[1] = 0.2; // Joint 2 position, in rad
+ q[2] = 0.3; // Joint 3 position, in rad
+ q[3] = M_PI/8; // Joint 4 position, in rad
+ q[4] = M_PI/4; // Joint 5 position, in rad
+ q[5] = M_PI; // Joint 6 position, in rad
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ The robot moves to the specified position with the default
+ positioning velocity vpRobotViper650::defaultPositioningVelocity. The
+ setPositioningVelocity() method allows to change the maximal
+ velocity used to reach the desired position.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ vpColVector q(6);
+ // Set q[i] with i in [0:5]
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 40%
+ robot.setPositioningVelocity(40);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and than send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+#endif
+}
+ \endcode
+
+ It is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and savePosFile()
+ methods.
+*/
+class VISP_EXPORT vpRobotViper650
+ :
+ public vpViper650,
+ public vpRobot
+{
+
+public: /* Constantes */
+
+ /*! \enum vpControlModeType Control mode. */
+ typedef enum {
+ AUTO, //!< Automatic control mode (default).
+ MANUAL, //!< Manual control mode activated when the dead man switch is in use.
+ ESTOP //!< Emergency stop activated.
+ } vpControlModeType;
+
+ /* Vitesse maximale par default lors du positionnement du robot.
+ * C'est la valeur a la construction de l'attribut prive \a
+ * positioningVelocity. Cette valeur peut etre changee par la fonction
+ * #setPositioningVelocity.
+ */
+ static const double defaultPositioningVelocity; // = 20.0;
+
+private: /* Not allowed functions. */
+
+ /*!
+ Copy constructor not allowed.
+ */
+ vpRobotViper650 (const vpRobotViper650 & robot);
+
+private: /* Attributs prives. */
+
+ /** \brief Vrai ssi aucun objet de la classe vpRobotViper650 n'existe.
+ *
+ * Il ne peut exister simultanement qu'un seul objet de la classe
+ * vpRobotViper650, car il correspond a un seul robot AFMA6. Creer
+ * simultanement deux objets peut engendrer des conflits. Le constructeur
+ * lance une erreur si le champ n'est pas FAUX puis positionne le champ
+ * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
+ * alors la creation d'un nouvel objet.
+ */
+ static bool robotAlreadyCreated;
+
+ double positioningVelocity;
+
+ // Variables used to compute the measured velocities (see getVelocity() )
+ vpColVector q_prev_getvel;
+ vpHomogeneousMatrix fMc_prev_getvel;
+ double time_prev_getvel;
+ bool first_time_getvel;
+
+ // Variables used to compute the measured displacement (see
+ // getDisplacement() )
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+ vpControlModeType controlMode;
+
+
+public: /* Methode publiques */
+
+ vpRobotViper650 (bool verbose=true);
+ virtual ~vpRobotViper650 (void);
+
+ // Force/Torque control
+ void biasForceTorqueSensor() const;
+
+ void disableJoint6Limits() const;
+ void enableJoint6Limits() const;
+
+ /*!
+ \return The control mode indicating if the robot is in automatic,
+ manual (usage of the dead man switch) or emergnecy stop mode.
+ */
+ vpControlModeType getControlMode() const {
+ return controlMode;
+ }
+
+ void getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+ void getForceTorque(vpColVector &H) const;
+
+ double getMaxRotationVelocityJoint6() const;
+
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position,
+ double ×tamp);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position,
+ double ×tamp);
+
+ double getPositioningVelocity (void) const;
+ bool getPowerState() const;
+
+ double getTime () const;
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity);
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp);
+
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void init (void);
+ void init (vpViper650::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType
+ projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
+ void init (vpViper650::vpToolType tool,
+ const std::string &filename);
+ void init (vpViper650::vpToolType tool,
+ const vpHomogeneousMatrix &eMc_);
+
+ void set_eMc(const vpHomogeneousMatrix &eMc_);
+ void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_);
+
+ void move(const char *filename);
+
+ void powerOn();
+ void powerOff();
+
+ static bool readPosFile(const char *filename, vpColVector &q);
+ static bool savePosFile(const char *filename, const vpColVector &q);
+
+ void setMaxRotationVelocity(double w_max);
+ void setMaxRotationVelocityJoint6(double w6_max);
+
+ // Position control
+ void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &position) ;
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1, const double pos2, const double pos3,
+ const double pos4, const double pos5, const double pos6) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+
+ // State
+ vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+ // Velocity control
+ void setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & velocity);
+
+ void stopMotion();
+
+private:
+ void getArticularDisplacement(vpColVector &displacement);
+ void getCameraDisplacement(vpColVector &displacement);
+ double maxRotationVelocity_joint6;
+};
+
+#endif
+#endif /* #ifndef vpRobotViper650_h */
diff --git a/modules/robot/include/visp3/robot/vpRobotViper850.h b/modules/robot/include/visp3/robot/vpRobotViper850.h
new file mode 100644
index 0000000..5345587
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotViper850.h
@@ -0,0 +1,436 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Viper S850 robot controlled by an Adept MotionBlox.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotViper850_h
+#define vpRobotViper850_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_VIPER850
+
+#include <iostream>
+#include <stdio.h>
+
+#include <visp3/robot/vpRobot.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper850.h>
+
+// low level controller api
+extern "C" {
+# include "irisa_Viper850.h"
+# include "trycatch.h"
+}
+
+
+/*!
+ \class vpRobotViper850
+
+ \ingroup group_robot_real_arm
+
+ \brief Control of Irisa's Viper S850 robot named Viper850.
+
+ Implementation of the vpRobot class in order to control Irisa's
+ Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
+ A firewire camera is mounted on the end-effector to allow
+ eye-in-hand visual servoing. The control of this camera is achieved
+ by the vp1394TwoGrabber class.
+
+ This class allows to control the Viper850 arm robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The direct and inverse kinematics models are implemented in the
+ vpViper850 class.
+
+ \warning A Ctrl-C, a segmentation fault or other system errors are
+ catched by this class to stop the robot.
+
+ To communicate with the robot, you may first create an instance of this
+ class by calling the default constructor:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+#endif
+}
+ \endcode
+
+ This initialize the robot kinematics with the \f$^e{\bf M}_c\f$
+ extrinsic camera parameters obtained with a projection model without
+ distortion. To set the robot kinematics with the \f$^e{\bf M}_c\f$
+ transformation obtained with a camera perspective model including
+ distortion you need to initialize the robot with:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distortion parameter
+ robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+#endif
+}
+ \endcode
+
+ You can get the intrinsic camera parameters of an image
+ acquired by the camera attached to the robot, with:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+ g.acquire(I);
+
+ vpRobotViper850 robot;
+
+ // ...
+
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distortion.
+#endif
+}
+ \endcode
+
+ To control the robot in position, you may set the controller
+ to position control and than send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ vpColVector q(6);
+ // Set a joint position
+ q[0] = vpMath::rad(10); // Joint 1 position, in rad
+ q[1] = 0.2; // Joint 2 position, in rad
+ q[2] = 0.3; // Joint 3 position, in rad
+ q[3] = M_PI/8; // Joint 4 position, in rad
+ q[4] = M_PI/4; // Joint 5 position, in rad
+ q[5] = M_PI; // Joint 6 position, in rad
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ The robot moves to the specified position with the default
+ positioning velocity vpRobotViper850::defaultPositioningVelocity. The
+ setPositioningVelocity() method allows to change the maximal
+ velocity used to reach the desired position.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ vpColVector q(6);
+ // Set q[i] with i in [0:5]
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 40%
+ robot.setPositioningVelocity(40);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and than send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+#endif
+}
+ \endcode
+
+ It is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and savePosFile()
+ methods.
+*/
+class VISP_EXPORT vpRobotViper850
+ :
+ public vpViper850,
+ public vpRobot
+{
+
+public: /* Constantes */
+
+ /*! \enum vpControlModeType Control mode. */
+ typedef enum {
+ AUTO, //!< Automatic control mode (default).
+ MANUAL, //!< Manual control mode activated when the dead man switch is in use.
+ ESTOP //!< Emergency stop activated.
+ } vpControlModeType;
+
+ /* Vitesse maximale par default lors du positionnement du robot.
+ * C'est la valeur a la construction de l'attribut prive \a
+ * positioningVelocity. Cette valeur peut etre changee par la fonction
+ * #setPositioningVelocity.
+ */
+ static const double defaultPositioningVelocity; // = 20.0;
+
+private: /* Not allowed functions. */
+
+ /*!
+ Copy constructor not allowed.
+ */
+ vpRobotViper850 (const vpRobotViper850 & robot);
+
+private: /* Attributs prives. */
+
+ /** \brief Vrai ssi aucun objet de la classe vpRobotViper850 n'existe.
+ *
+ * Il ne peut exister simultanement qu'un seul objet de la classe
+ * vpRobotViper850, car il correspond a un seul robot AFMA6. Creer
+ * simultanement deux objets peut engendrer des conflits. Le constructeur
+ * lance une erreur si le champ n'est pas FAUX puis positionne le champ
+ * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
+ * alors la creation d'un nouvel objet.
+ */
+ static bool robotAlreadyCreated;
+
+ double positioningVelocity;
+
+ // Variables used to compute the measured velocities (see getVelocity() )
+ vpColVector q_prev_getvel;
+ vpHomogeneousMatrix fMc_prev_getvel;
+ double time_prev_getvel;
+ bool first_time_getvel;
+
+ // Variables used to compute the measured displacement (see
+ // getDisplacement() )
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+ vpControlModeType controlMode;
+
+
+public: /* Methode publiques */
+
+ vpRobotViper850 (bool verbose=true);
+ virtual ~vpRobotViper850 (void);
+
+ // Force/Torque control
+ void biasForceTorqueSensor() const;
+
+ void closeGripper() const;
+
+ void disableJoint6Limits() const;
+ void enableJoint6Limits() const;
+
+ void getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+ /*!
+ \return The control mode indicating if the robot is in automatic,
+ manual (usage of the dead man switch) or emergnecy stop mode.
+ */
+ vpControlModeType getControlMode() const {
+ return controlMode;
+ }
+
+ void getForceTorque(vpColVector &H) const;
+
+ double getMaxRotationVelocityJoint6() const;
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector &position, double ×tamp);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position);
+ void getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position, double ×tamp);
+
+ double getPositioningVelocity (void) const;
+ bool getPowerState() const;
+
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity);
+ void getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp);
+
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ double getTime() const;
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void init (void);
+ void init (vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType
+ projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
+
+ void move(const char *filename) ;
+
+ void openGripper();
+
+ void powerOn() ;
+ void powerOff() ;
+
+ static bool readPosFile(const char *filename, vpColVector &q) ;
+ static bool savePosFile(const char *filename, const vpColVector &q) ;
+
+ void setMaxRotationVelocity(double w_max);
+ void setMaxRotationVelocityJoint6(double w6_max);
+
+ // Position control
+ void setPosition(const vpRobot::vpControlFrameType frame,
+ const vpColVector &position) ;
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1, const double pos2, const double pos3,
+ const double pos4, const double pos5, const double pos6) ;
+ void setPosition(const char *filename) ;
+ void setPositioningVelocity (const double velocity);
+
+ // State
+ vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+
+ // Velocity control
+ void setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & velocity);
+
+ void stopMotion() ;
+
+private:
+ void getArticularDisplacement(vpColVector &displacement);
+ void getCameraDisplacement(vpColVector &displacement);
+
+ double maxRotationVelocity_joint6;
+};
+
+
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
+#endif /* #ifndef vpRobotViper850_h */
diff --git a/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h
new file mode 100644
index 0000000..2b62934
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h
@@ -0,0 +1,473 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic class used to make robot simulators.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpRobotWireFrameSimulator_HH
+#define vpRobotWireFrameSimulator_HH
+
+/*!
+ \file vpRobotWireFrameSimulator.h
+ \brief Basic class used to make robot simulators.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#if defined(_WIN32)
+# include <windows.h>
+#elif defined(VISP_HAVE_PTHREAD)
+# include <pthread.h>
+#endif
+
+#include <visp3/robot/vpWireFrameSimulator.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotSimulator.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayD3D.h>
+
+/*!
+ \class vpRobotWireFrameSimulator
+
+ \ingroup group_robot_simu_gantry group_robot_simu_arm
+
+ \brief This class aims to be a basis used to create all the
+ simulators of robots.
+
+ Thus in this class you will find all the parameters and methods
+ which are necessary to create a simulator. Several methods are pure
+ virtual. In this case it means that they are specific to the each
+ robot, for example the computation of the geometrical model.
+
+ \warning This class uses threading capabilities. Thus on Unix-like
+ platforms, the libpthread third-party library need to be
+ installed. On Windows, we use the native threading capabilities.
+*/
+class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
+{
+ public:
+ vpImage<vpRGBa> I;
+
+ typedef enum
+ {
+ MODEL_3D,
+ MODEL_DH
+ } vpDisplayRobotType;
+
+
+ protected:
+ /*! cpu time at the begining of the robot's movement*/
+ double tcur;
+ /*! cpu time at the end of the last robot's movement*/
+ double tprev;
+
+ /*! Contains the 3D model of the robot's arms*/
+ Bound_scene* robotArms;
+
+ /*! Size of the fMi table*/
+ unsigned int size_fMi;
+ /*! Table containing all the homogeneous matrices between the reference frame of the robot and the frames you used to compute the Denavit-Hartenberg representation
+
+ If you use a camera at the end of the effector, the last homogeneous matrix has to be the one between the reference frame and the camera frame (fMc)*/
+ vpHomogeneousMatrix* fMi;
+
+ /*! The articular coordinates*/
+ vpColVector artCoord;
+ /*! The articular velocity*/
+ vpColVector artVel;
+ /*! The velocity in the current frame (articular, camera or reference)*/
+ vpColVector velocity;
+
+#if defined(_WIN32)
+ HANDLE hThread;
+ HANDLE mutex_fMi;
+ HANDLE mutex_artVel;
+ HANDLE mutex_artCoord;
+ HANDLE mutex_velocity;
+ HANDLE mutex_display;
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_t thread;
+ pthread_attr_t attr;
+ pthread_mutex_t mutex_fMi;
+ pthread_mutex_t mutex_artVel;
+ pthread_mutex_t mutex_artCoord;
+ pthread_mutex_t mutex_velocity;
+ pthread_mutex_t mutex_display;
+#endif
+
+ bool displayBusy;
+
+ /*! True if the robot has to be stopped*/
+ bool robotStop;
+ /*! True if one of the joint reach the limit*/
+ bool jointLimit;
+ /*! Index of the joint which is in limit*/
+ unsigned int jointLimitArt;
+ /*! True if the singularity are automatically managed */
+ bool singularityManagement;
+
+ /*! External camera parameters*/
+ vpCameraParameters cameraParam;
+
+ #if defined VISP_HAVE_X11
+ vpDisplayX display;
+ #elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+ #elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display;
+ #elif defined VISP_HAVE_D3D9
+ vpDisplayD3D display;
+ #elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+ #endif
+
+ vpDisplayRobotType displayType;
+
+ bool displayAllowed;
+ //! Flag used to force the sampling time in the thread computing the robot's displacement to a constant value (\e samplingTime). It may be useful if the main thread (computing the features) is very time consumming. False by default.
+ bool constantSamplingTimeMode;
+
+ //! Flag used to specify to the thread managing the robot displacements that the setVelocity() method has been called.
+ bool setVelocityCalled;
+
+ bool verbose_;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpRobotWireFrameSimulator(const vpRobotWireFrameSimulator &)
+// : vpWireFrameSimulator(), vpRobotSimulator(),
+// I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
+// #if defined(_WIN32)
+// #elif defined(VISP_HAVE_PTHREAD)
+// thread(), attr(),
+// #endif
+// mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
+// displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
+// cameraParam(),
+// #if defined(VISP_HAVE_DISPLAY)
+// display(),
+// #endif
+// displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
+// setVelocityCalled(false), verbose_(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpRobotWireFrameSimulator &operator=(const vpRobotWireFrameSimulator &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpRobotWireFrameSimulator();
+ vpRobotWireFrameSimulator(bool display);
+ virtual ~vpRobotWireFrameSimulator();
+
+ /*!
+ Get the parameters of the virtual external camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getExternalCameraParameters() const {
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
+ else
+ {
+ unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
+ return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ }
+ }
+ /*!
+ Get the external camera's position relative to the the world reference frame.
+
+ \return the main external camera position relative to the the world reference frame.
+ */
+ vpHomogeneousMatrix getExternalCameraPosition() const
+ {
+ return this->vpWireFrameSimulator::getExternalCameraPosition();
+ }
+
+ void getInternalView(vpImage<vpRGBa> &I);
+ void getInternalView(vpImage<unsigned char> &I);
+
+ vpHomogeneousMatrix get_cMo();
+ /*!
+ Get the pose between the object and the fixed world frame.
+
+ \return The pose between the object and the fixed world frame.
+ */
+ vpHomogeneousMatrix get_fMo() const {return fMo;}
+
+ /* Display functions */
+ void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
+ void initScene(const char* obj, const char* desiredObject);
+ void initScene (const vpSceneObject &obj);
+ void initScene(const char* obj);
+
+ /*!
+ Set the color used to display the camera in the external view.
+
+ \param col : The desired color.
+ */
+ void setCameraColor(const vpColor col) {camColor = col;}
+
+ /*!
+ Set the flag used to force the sampling time in the thread computing the
+ robot's displacement to a constant value; see setSamplingTime(). It may be
+ useful if the main thread (computing the features) is very time consuming.
+ False by default.
+
+ \param _constantSamplingTimeMode : The new value of the constantSamplingTimeMode flag.
+ */
+ inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
+ constantSamplingTimeMode = _constantSamplingTimeMode;
+ }
+
+ /*!
+ Set the color used to display the object at the current position in the robot's camera view.
+
+ \param col : The desired color.
+ */
+ void setCurrentViewColor(const vpColor col) {curColor = col;}
+
+ /*!
+ Set the color used to display the object at the desired position in the robot's camera view.
+
+ \param col : The desired color.
+ */
+ void setDesiredViewColor(const vpColor col) {desColor = col;}
+
+ /*!
+ Set the desired position of the robot's camera relative to the object.
+
+ \param cdMo_ : The desired pose of the camera.
+ */
+ void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo_)
+ {
+ this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
+ }
+
+ /*!
+ Set the way to draw the robot. Depending on what you choose you can display a 3D wireframe model or a set of lines linking the frames used to compute the geometrical model.
+
+ \param dispType : Type of display. Can be MODEL_3D or MODEL_DH.
+ */
+ inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
+ /*!
+ Set the external camera point of view.
+
+ \param camMf_ : The pose of the external camera relative to the world reference frame.
+ */
+ void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
+ {
+ this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
+ }
+ /*!
+ Specify the thickness of the graphics drawings.
+ */
+ void setGraphicsThickness(unsigned int thickness)
+ {
+ this->thickness_ = thickness;
+ }
+
+ /*!
+ Set the sampling time.
+
+ \param delta_t : Sampling time in second used to compute the robot displacement from
+ the velocity applied to the robot during this time.
+
+ Since the wireframe simulator is threaded, the sampling time is set to vpTime::getMinTimeForUsleepCall() / 1000 seconds.
+
+ */
+ inline void setSamplingTime(const double &delta_t)
+ {
+ if(delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)){
+ this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
+ } else {
+ this->delta_t_ = delta_t;
+ }
+ }
+ /*! Set the parameter which enable or disable the singularity mangement */
+ void setSingularityManagement (const bool sm) {singularityManagement = sm;}
+
+ /*!
+ Activates extra printings when the robot reaches joint limits...
+ */
+ void setVerbose(bool verbose)
+ {
+ this->verbose_ = verbose;
+ }
+
+ /*!
+ Set the pose between the object and the fixed world frame.
+
+ \param fMo_ : The pose between the object and the fixed world frame.
+ */
+ void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;}
+
+ protected:
+ /*!
+ Function used to launch the thread which moves the robot.
+ */
+ #if defined(_WIN32)
+ static DWORD WINAPI launcher( LPVOID lpParam )
+ {
+ ((vpRobotWireFrameSimulator *)lpParam)->updateArticularPosition();
+ return 0;
+ }
+ #elif defined(VISP_HAVE_PTHREAD)
+ static void* launcher(void *arg)
+ {
+ (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
+ // pthread_exit((void*) 0);
+ return NULL;
+ }
+ #endif
+
+ /* Robot functions */
+ void init() {;}
+ /*! Method lauched by the thread to compute the position of the robot in the articular frame. */
+ virtual void updateArticularPosition() = 0;
+ /*! Method used to check if the robot reached a joint limit. */
+ virtual int isInJointLimit () = 0;
+ /*! Compute the articular velocity relative to the velocity in another frame. */
+ virtual void computeArticularVelocity() = 0;
+
+ /* Display functions */
+ void initDisplay() {;}
+ virtual void initArms() = 0;
+
+ #if defined(_WIN32)
+ vpColVector get_artCoord() const {
+ WaitForSingleObject(mutex_artCoord,INFINITE);
+ vpColVector artCoordTmp (6);
+ artCoordTmp = artCoord;
+ ReleaseMutex(mutex_artCoord);
+ return artCoordTmp;}
+ void set_artCoord(const vpColVector &coord) {
+ WaitForSingleObject(mutex_artCoord,INFINITE);
+ artCoord = coord;
+ ReleaseMutex(mutex_artCoord);}
+
+ vpColVector get_artVel() const {
+ WaitForSingleObject(mutex_artVel,INFINITE);
+ vpColVector artVelTmp (artVel);
+ ReleaseMutex(mutex_artVel);
+ return artVelTmp;}
+ void set_artVel(const vpColVector &vel) {
+ WaitForSingleObject(mutex_artVel,INFINITE);
+ artVel = vel;
+ ReleaseMutex(mutex_artVel);}
+
+ vpColVector get_velocity() {
+ WaitForSingleObject(mutex_velocity,INFINITE);
+ vpColVector velocityTmp = velocity;
+ ReleaseMutex(mutex_velocity);
+ return velocityTmp;}
+ void set_velocity(const vpColVector &vel) {
+ WaitForSingleObject(mutex_velocity,INFINITE);
+ velocity = vel;
+ ReleaseMutex(mutex_velocity);}
+
+ void set_displayBusy (const bool &status) {
+ WaitForSingleObject(mutex_display,INFINITE);
+ displayBusy = status;
+ ReleaseMutex(mutex_display);}
+ bool get_displayBusy () {
+ WaitForSingleObject(mutex_display,INFINITE);
+ bool status = displayBusy;
+ if (!displayBusy) displayBusy = true;
+ ReleaseMutex(mutex_display);
+ return status;}
+
+ #elif defined(VISP_HAVE_PTHREAD)
+ vpColVector get_artCoord() {
+ pthread_mutex_lock (&mutex_artCoord);
+ vpColVector artCoordTmp (6);
+ artCoordTmp = artCoord;
+ pthread_mutex_unlock (&mutex_artCoord);
+ return artCoordTmp;}
+ void set_artCoord(const vpColVector &coord) {
+ pthread_mutex_lock (&mutex_artCoord);
+ artCoord = coord;
+ pthread_mutex_unlock (&mutex_artCoord);}
+
+ vpColVector get_artVel() {
+ pthread_mutex_lock (&mutex_artVel);
+ vpColVector artVelTmp (artVel);
+ pthread_mutex_unlock (&mutex_artVel);
+ return artVelTmp;}
+ void set_artVel(const vpColVector &vel) {
+ pthread_mutex_lock (&mutex_artVel);
+ artVel = vel;
+ pthread_mutex_unlock (&mutex_artVel);}
+
+ vpColVector get_velocity() {
+ pthread_mutex_lock (&mutex_velocity);
+ vpColVector velocityTmp = velocity;
+ pthread_mutex_unlock (&mutex_velocity);
+ return velocityTmp;}
+ void set_velocity(const vpColVector &vel) {
+ pthread_mutex_lock (&mutex_velocity);
+ velocity = vel;
+ pthread_mutex_unlock (&mutex_velocity);}
+
+ void set_displayBusy (const bool &status) {
+ pthread_mutex_lock (&mutex_display);
+ displayBusy = status;
+ pthread_mutex_unlock (&mutex_display);}
+ bool get_displayBusy () {
+ pthread_mutex_lock (&mutex_display);
+ bool status = displayBusy;
+ if (!displayBusy) displayBusy = true;
+ pthread_mutex_unlock (&mutex_display);
+ return status;}
+ #endif
+
+ /*! Get a table of poses between the reference frame and the frames you used to compute the Denavit-Hartenberg representation */
+ virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
+};
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpServolens.h b/modules/robot/include/visp3/robot/vpServolens.h
new file mode 100644
index 0000000..66aa1d5
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpServolens.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Servolens lens attached to the camera fixed on the
+ * Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef __vpServolens_h
+#define __vpServolens_h
+
+#include <visp3/core/vpConfig.h>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+
+/*!
+
+ \file vpServolens.h
+
+ Interface for the Servolens lens attached to the camera fixed on the
+ Afma4 robot.
+
+*/
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+/*!
+
+ \class vpServolens
+
+ \ingroup group_robot_real_cylindrical
+
+ \brief Interface for the Servolens lens attached to the camera fixed on the
+ Afma4 robot.
+
+ The code below shows how to manipulate this class to get and modify
+ the position of the focal lens.
+
+ \code
+#include <iostream>
+#include <visp3/vs/vpServolens.h>
+
+int main()
+{
+ // Open the serial device to communicate with the Servolens lens
+ vpServolens servolens("/dev/ttyS0");
+
+ // Get the current zoom position
+ unsigned zoom;
+ servolens.getPosition(vpServolens::ZOOM, zoom);
+ std::cout << "Actual zoom value: " << zoom << std::endl;
+
+ // Set a new zoom value
+ servolens.setPosition(vpServolens::ZOOM, zoom+1000);
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpServolens
+{
+ public:
+ typedef enum {
+ ZOOM = 1,
+ FOCUS= 2,
+ IRIS = 3
+ } vpServoType;
+ typedef enum {
+ ZOOM_MAX = 10000, // Valeur maxi zoom (mm/100)
+ ZOOM_MIN = 1000, // Valeur mini zoom (mm/100)
+ FOCUS_MAX = 1500, // Valeur maxi focus (metres/100)
+ FOCUS_MIN = 100, // Valeur mini focus (metres/100)
+ IRIS_MAX = 1000, // Valeur maxi diaph (ouverture/100)
+ IRIS_MIN = 160 // Valeur mini disph (ouverture/100)
+ } vpLimitsType;
+ typedef enum {
+ AUTO = 1,
+ CONTROLLED = 2,
+ RELEASED = 3
+ } vpControllerType;
+
+ vpServolens();
+ vpServolens(const char *port);
+ ~vpServolens();
+
+ void open(const char *port="/dev/ttyS0");
+ void close();
+ void reset() const;
+
+ void setController(vpControllerType controller) const;
+ void setAutoIris(bool enable) const;
+ void setPosition(vpServoType servo, unsigned int position) const;
+ bool getPosition(vpServoType servo, unsigned int &position) const;
+ vpCameraParameters getCameraParameters(vpImage<unsigned char> &I) const;
+
+ void enablePrompt(bool active) const;
+
+ private:
+
+ void init() const;
+
+ void enableCmdComplete(vpServoType servo, bool active) const;
+
+ char wait() const;
+ void wait(vpServoType servo) const;
+
+ bool read(char *c, long timeout_s) const;
+ void write(const char *s) const;
+
+ bool clean(const char *in, char *out) const;
+
+ int remfd; // file pointer of the host's tty
+ bool isinit;
+};
+
+#endif
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpSimulatorAfma6.h b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h
new file mode 100644
index 0000000..8aaba87
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class which provides a simulator for the robot Afma6.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpSimulatorAfma6_HH
+#define vpSimulatorAfma6_HH
+
+/*!
+ \file vpSimulatorAfma6.h
+ \brief Class which provides a simulator for the robot Afma6.
+*/
+
+#include <visp3/robot/vpRobotWireFrameSimulator.h>
+#include <visp3/robot/vpAfma6.h>
+
+#include <string>
+
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+
+/*!
+ \class vpSimulatorAfma6
+
+ \ingroup group_robot_simu_gantry
+
+ \brief Simulator of Irisa's gantry robot named Afma6.
+
+ Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
+ Afma6 robot. This robot is a gantry robot with six degrees of
+ freedom manufactured in 1992 by the french Afma-Robots company.
+
+ \warning This class uses threading capabilities. Thus on Unix-like
+ platforms, the libpthread third-party library need to be
+ installed. On Windows, we use the native threading capabilities.
+
+ This class allows to control the Afma6 gantry robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The direct and inverse kinematics models are implemented in the
+ vpAfma6 class.
+
+ To control the robot in position, you may set the controller
+ to position control and then send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorAfma6 robot;
+
+ robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
+
+ vpColVector q(6);
+ // Set a joint position
+ q[0] = 0.1; // Joint 1 position, in meter
+ q[1] = 0.2; // Joint 2 position, in meter
+ q[2] = 0.3; // Joint 3 position, in meter
+ q[3] = M_PI/8; // Joint 4 position, in rad
+ q[4] = M_PI/4; // Joint 5 position, in rad
+ q[5] = M_PI; // Joint 6 position, in rad
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ return 0;
+}
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and then send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorAfma6 robot;
+
+ robot.init(vpAfma6::TOOL_GRIPPER, vpCameraParameters::perspectiveProjWithoutDistortion);
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in m/s
+ qvel[1] = 0.1; // Joint 2 velocity in m/s
+ qvel[2] = 0.1; // Joint 3 velocity in m/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+
+ return 0;
+}
+ \endcode
+
+ It is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and savePosFile()
+ methods.
+
+ To know how this class can be used to achieve a visual servoing simulation,
+ you can follow the \ref tutorial-ibvs.
+
+*/
+
+
+class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
+{
+ public:
+ static const double defaultPositioningVelocity;
+
+ private:
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+
+ double positioningVelocity;
+
+ vpColVector zeroPos;
+ vpColVector reposPos;
+
+ bool toolCustom;
+ std::string arm_dir;
+
+public:
+ vpSimulatorAfma6();
+ vpSimulatorAfma6(bool display);
+ virtual ~vpSimulatorAfma6();
+
+ void getCameraParameters(vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height);
+ void getCameraParameters(vpCameraParameters &cam,
+ const vpImage<unsigned char> &I);
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
+ void getDisplacement(const vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+ double getPositioningVelocity (void){return positioningVelocity;}
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe);
+ void get_cVe(vpVelocityTwistMatrix &cVe);
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
+ bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
+ void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
+
+ void move(const char *filename) ;
+
+ static bool readPosFile(const char *filename, vpColVector &q);
+ static bool savePosFile(const char *filename, const vpColVector &q);
+ void setCameraParameters(const vpCameraParameters &cam) ;
+ void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
+
+ void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6);
+ void setPosition(const char *filename);
+ void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
+ bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
+ vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
+
+ void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
+
+ void stopMotion();
+
+
+protected:
+ void computeArticularVelocity();
+ void compute_fMi();
+ void findHighestPositioningSpeed(vpColVector &q);
+ void getExternalImage(vpImage<vpRGBa> &I);
+ inline void get_fMi(vpHomogeneousMatrix *fMit) {
+#if defined(_WIN32)
+ WaitForSingleObject(mutex_fMi,INFINITE);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ ReleaseMutex(mutex_fMi);
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock (&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ pthread_mutex_unlock (&mutex_fMi);
+#endif
+ }
+ void init();
+ void initArms();
+ void initDisplay();
+ int isInJointLimit (void);
+ bool singularityTest(const vpColVector q, vpMatrix &J);
+ void updateArticularPosition();
+
+private:
+ void getCameraDisplacement(vpColVector &displacement);
+ void getArticularDisplacement(vpColVector &displacement);
+};
+
+#endif
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpSimulatorCamera.h b/modules/robot/include/visp3/robot/vpSimulatorCamera.h
new file mode 100644
index 0000000..df55824
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSimulatorCamera.h
@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines the simplest robot : a free flying camera.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpSimulatorCamera_H
+#define vpSimulatorCamera_H
+
+/*!
+ \file vpSimulatorCamera.h
+ \brief class that defines the simplest robot : a free flying camera
+*/
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotSimulator.h>
+
+/*!
+ \class vpSimulatorCamera
+
+ \ingroup group_robot_simu_Camera
+
+ \brief Class that defines the simplest robot: a free flying camera.
+
+ This free flying camera has 6 dof; 3 in translation and 3 in rotation.
+ It evolves as a gentry robot with respect to a world frame. This class
+ is similar to vpRobotCamera class except that here the position of the robot
+ is provided as the transformation from world frame to camera frame; wMc. This
+ representation is more intuitive than the one implemented in vpRobotCamera
+ where the transformation from camera to world frame is considered; cMw.
+
+ For this particular simulated robot, the end-effector and camera frame are confused.
+ That means that the cMe transformation is equal to identity.
+
+ The robot jacobian expressed in the end-effector frame
+ \f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
+
+ The following code shows how to control this robot in position and velocity.
+ \code
+#include <visp3/robot/vpSimulatorCamera.h>
+
+int main()
+{
+ vpHomogeneousMatrix wMc;
+ vpSimulatorCamera robot;
+
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
+
+ wMc[2][3] = 1.; // Camera frame is 1 meter along z axis in front of the world frame
+ robot.setPosition(wMc); // Set the new position of the camera in the world frame
+ std::cout << "New position of the camera in the world frame wMc:\n" << wMc << std::endl;
+
+ robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
+ robot.setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ robot.setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
+
+ vpColVector v(6);
+ v = 0;
+ v[2] = 1.; // set v_z to 1 m/s
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ // The robot has moved from 0.1 meters along the z axis
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
+}
+ \endcode
+
+ To know how this class can be used to achieve a visual servoing simulation,
+ you can follow the \ref tutorial-ibvs.
+*/
+class VISP_EXPORT vpSimulatorCamera : public vpRobotSimulator
+{
+protected:
+ vpHomogeneousMatrix wMc_; // world to camera
+
+public:
+ vpSimulatorCamera() ;
+ virtual ~vpSimulatorCamera() ;
+
+public:
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_eJe(vpMatrix &eJe);
+
+ vpHomogeneousMatrix getPosition() const;
+ void getPosition(vpHomogeneousMatrix &wMc) const;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void setPosition(const vpHomogeneousMatrix &wMc);
+ void setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &vel) ;
+
+private:
+ void init() ;
+
+ // Non implemented virtual pure functions
+ void get_fJe(vpMatrix & /*_fJe */) {};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
+} ;
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpSimulatorPioneer.h b/modules/robot/include/visp3/robot/vpSimulatorPioneer.h
new file mode 100644
index 0000000..ca23174
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSimulatorPioneer.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pioneer mobile robot simulator without display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpSimulatorPioneer_H
+#define vpSimulatorPioneer_H
+
+/*!
+ \file vpSimulatorPioneer.h
+ \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
+*/
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/robot/vpPioneer.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotSimulator.h>
+
+/*!
+ \class vpSimulatorPioneer
+
+ \ingroup group_robot_simu_unicycle
+
+ \brief Class that defines the Pioneer mobile robot simulator equipped with a static camera.
+
+ It intends to simulate the mobile robot described in vpPioneer class.
+ This robot has 2 dof: \f$(v_x, w_z)\f$, the translational and
+ rotational velocities that are applied at point E.
+
+ The robot position evolves with respect to a world frame; wMc. When a new joint velocity
+ is applied to the robot using setVelocity(), the position of the camera wrt the world frame
+ is updated.
+
+ \image html pioneer.png
+
+ The following code shows how to control this robot in position and velocity.
+ \code
+#include <visp3/robot/vpSimulatorPioneer.h>
+
+int main()
+{
+ vpHomogeneousMatrix wMc;
+ vpSimulatorPioneer robot;
+
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
+
+ robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
+ robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s
+ robot.setMaxRotationVelocity(vpMath::rad(90)); // wz max set to 90 deg/s
+
+ vpColVector v(2); // we control vx and wz dof
+ v = 0;
+ v[0] = 1.; // set vx to 1 m/s
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
+ // The robot has moved from 0.1 meters along the z axis
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
+}
+ \endcode
+
+ The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
+
+*/
+class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, public vpRobotSimulator
+{
+
+protected:
+ // world to camera
+ vpHomogeneousMatrix wMc_ ;
+ // world to end effector frame which is also the mobile
+ // robot frame located between the two wheels
+ vpHomogeneousMatrix wMe_ ;
+ // cMe_ is a protected member of vpUnicycle
+
+ double xm_;
+ double ym_;
+ double theta_;
+
+public:
+ vpSimulatorPioneer() ;
+ virtual ~vpSimulatorPioneer();
+
+public:
+ void get_eJe(vpMatrix &eJe);
+
+ void getPosition(vpHomogeneousMatrix &wMc) const;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &vel) ;
+
+private:
+ void init() ;
+
+ // Non implemented virtual pure functions
+ void get_fJe(vpMatrix & /*_fJe */) {};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
+
+} ;
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h b/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h
new file mode 100644
index 0000000..d0f290f
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pioneer mobile robot equipped with a pan head simulator without display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpSimulatorPioneerPan_H
+#define vpSimulatorPioneerPan_H
+
+/*!
+ \file vpSimulatorPioneerPan.h
+ \brief class that defines the Pioneer mobile robot simulator equipped
+ with a camera able to move in pan.
+*/
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/robot/vpPioneerPan.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotSimulator.h>
+
+/*!
+ \class vpSimulatorPioneerPan
+
+ \ingroup group_robot_simu_unicycle
+
+ \brief Class that defines the Pioneer mobile robot simulator equipped
+ with a camera able to move in pan.
+
+ It intends to simulate the mobile robot described in vpPioneerPan class.
+ This robot has 3 dof: \f$(v_x, w_z, \dot{q_1})\f$, the translational and
+ rotational velocities of the mobile platform, the pan head velocity respectively.
+
+ The robot position evolves with respect to a world frame; wMc. When a new joint velocity
+ is applied to the robot using setVelocity(), the position of the camera wrt the world frame
+ is updated.
+
+ \image html pioneer-pan.png
+
+ The following code shows how to control this robot in position and velocity.
+ \code
+#include <visp3/robot/vpSimulatorPioneerPan.h>
+
+int main()
+{
+ vpHomogeneousMatrix wMc;
+ vpSimulatorPioneerPan robot;
+
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
+
+ robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
+ robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s
+ robot.setMaxRotationVelocity(vpMath::rad(90)); // wz max set to 90 deg/s
+
+ vpColVector v(3); // we control vx, wz and q_pan
+ v = 0;
+ v[0] = 1.; // set vx to 1 m/s
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
+ // The robot has moved from 0.1 meters along the z axis
+ robot.getPosition(wMc); // Position of the camera in the world frame
+ std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
+}
+ \endcode
+
+ The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
+
+*/
+class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, public vpRobotSimulator
+{
+
+protected:
+ //! robot / camera location in the world frame
+ vpHomogeneousMatrix wMc_ ; // world to camera
+ vpHomogeneousMatrix wMm_ ; // world to mobile robot frame located between the two weels
+ // mMp_ mobile robot to pan frame is a protected member of vpPioneerPan
+ // pMe_ pan head to end effector frame is a protected member of vpPioneerPan
+ // cMe_ is a protected member of vpUnicycle
+
+ double xm_;
+ double ym_;
+ double theta_;
+ double q_pan_;
+
+public:
+ vpSimulatorPioneerPan() ;
+ virtual ~vpSimulatorPioneerPan();
+
+public:
+ void get_eJe(vpMatrix &eJe);
+
+ void getPosition(vpHomogeneousMatrix &wMc) const;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &vel) ;
+
+private:
+ void init() ;
+
+ // Non implemented virtual pure functions
+ void get_fJe(vpMatrix & /*_fJe */) {};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
+} ;
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpSimulatorViper850.h b/modules/robot/include/visp3/robot/vpSimulatorViper850.h
new file mode 100644
index 0000000..cda80e1
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSimulatorViper850.h
@@ -0,0 +1,310 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class which provides a simulator for the robot Viper850.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpSimulatorViper850_HH
+#define vpSimulatorViper850_HH
+
+/*!
+ \file vpSimulatorViper850.h
+ \brief Class which provides a simulator for the robot Viper850..
+*/
+
+#include <visp3/robot/vpRobotWireFrameSimulator.h>
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+
+#include <string>
+
+#include <visp3/robot/vpViper850.h>
+
+/*!
+ \class vpSimulatorViper850
+
+ \ingroup group_robot_simu_arm
+
+ \brief Simulator of Irisa's Viper S850 robot named Viper850.
+
+ Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
+ Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
+
+ \warning This class uses threading capabilities. Thus on Unix-like
+ platforms, the libpthread third-party library need to be
+ installed. On Windows, we use the native threading capabilities.
+
+ This class allows to control the Viper850 arm robot in position
+ and velocity:
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the camera frame (vpRobot::CAMERA_FRAME),
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame and rotations in the camera frame.
+
+ All the translations are expressed in meters for positions and m/s
+ for the velocities. Rotations are expressed in radians for the
+ positions, and rad/s for the rotation velocities.
+
+ The direct and inverse kinematics models are implemented in the
+ vpViper850 class.
+
+ To control the robot in position, you may set the controller
+ to position control and then send the position to reach in a specific
+ frame like here in the joint space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ vpColVector q(6);
+ // Set a joint position
+ q[0] = vpMath::rad(10); // Joint 1 position, in rad
+ q[1] = 0.2; // Joint 2 position, in rad
+ q[2] = 0.3; // Joint 3 position, in rad
+ q[3] = M_PI/8; // Joint 4 position, in rad
+ q[4] = M_PI/4; // Joint 5 position, in rad
+ q[5] = M_PI; // Joint 6 position, in rad
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ return 0;
+}
+ \endcode
+
+ The robot moves to the specified position with the default
+ positioning velocity vpRobotViper850::defaultPositioningVelocity. The
+ setPositioningVelocity() method allows to change the maximal
+ velocity used to reach the desired position.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ vpColVector q(6);
+ // Set q[i] with i in [0:5]
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 40%
+ robot.setPositioningVelocity(40);
+
+ // Moves the robot in the joint space
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ return 0;
+}
+ \endcode
+
+ To control the robot in velocity, you may set the controller to
+ velocity control and then send the velocities. To end the velocity
+ control and stop the robot you have to set the controller to the
+ stop state. Here is an example of a velocity control in the joint
+ space:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+
+ return 0;
+}
+ \endcode
+
+ It is also possible to measure the robot current position with
+ getPosition() method and the robot current velocities with the getVelocity()
+ method.
+
+ For convenience, there is also the ability to read/write joint
+ positions from a position file with readPosFile() and savePosFile()
+ methods.
+
+ To know how this class can be used to achieve a visual servoing simulation,
+ you can follow the \ref tutorial-ibvs.
+
+*/
+
+
+class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, public vpViper850
+{
+ public:
+ static const double defaultPositioningVelocity;
+
+ private:
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+
+ double positioningVelocity;
+
+ vpColVector zeroPos;
+ vpColVector reposPos;
+
+ bool toolCustom;
+ std::string arm_dir;
+
+ public:
+ vpSimulatorViper850();
+ vpSimulatorViper850(bool display);
+ virtual ~vpSimulatorViper850();
+
+ void getCameraParameters(vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height);
+ void getCameraParameters(vpCameraParameters &cam,
+ const vpImage<unsigned char> &I);
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
+
+ void getDisplacement(const vpRobot::vpControlFrameType frame,
+ vpColVector &displacement);
+
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+ double getPositioningVelocity (void){return positioningVelocity;}
+
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe);
+ void get_cVe(vpVelocityTwistMatrix &cVe);
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void init (vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
+ bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
+ void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
+
+ void move(const char *filename) ;
+
+ static bool readPosFile(const char *filename, vpColVector &q);
+ static bool savePosFile(const char *filename, const vpColVector &q);
+
+ void setCameraParameters(const vpCameraParameters &cam) ;
+ void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
+ void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
+ void setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6);
+ void setPosition(const char *filename);
+ void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
+ vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
+
+ void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
+
+ void stopMotion();
+
+protected:
+ void computeArticularVelocity();
+ void compute_fMi();
+ void findHighestPositioningSpeed(vpColVector &q);
+ void getExternalImage(vpImage<vpRGBa> &I);
+
+ inline void get_fMi(vpHomogeneousMatrix *fMit) {
+#if defined(_WIN32)
+ WaitForSingleObject(mutex_fMi,INFINITE);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ ReleaseMutex(mutex_fMi);
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock (&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ pthread_mutex_unlock (&mutex_fMi);
+#endif
+ }
+ void init();
+ void initArms();
+ void initDisplay();
+ int isInJointLimit (void);
+ bool singularityTest(const vpColVector q, vpMatrix &J);
+ void updateArticularPosition();
+
+private:
+ void getArticularDisplacement(vpColVector &displacement);
+ void getCameraDisplacement(vpColVector &displacement);
+};
+
+#endif
+
+#endif
diff --git a/modules/robot/include/visp3/robot/vpSkipio.h b/modules/robot/include/visp3/robot/vpSkipio.h
new file mode 100644
index 0000000..c505b13
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpSkipio.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "skipio.c" contient les procedures d'analyse
+ * syntaxique du fichier "source" qui permettent de traiter
+ * les commandes inconnues.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpSkipio_h
+#define vpSkipio_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void skip_cmd (void);
+void skip_keyword (int token, const char *err);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpTmstack.h b/modules/robot/include/visp3/robot/vpTmstack.h
new file mode 100644
index 0000000..251abf5
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpTmstack.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "tmstack.h" contient les macros, les types et
+ * les specifications des procedures de gestion de la pile
+ * de matrices de transformation (Transformation Matrix STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpTmstack_h
+#define vpTmstack_h
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+Matrix *get_tmstack (void);
+void load_tmstack (Matrix m);
+void pop_tmstack (void);
+void push_tmstack (void);
+void swap_tmstack (void);
+
+void postmult_tmstack (Matrix m);
+void postrotate_tmstack (Vector *vp);
+void postscale_tmstack (Vector *vp);
+void posttranslate_tmstack (Vector *vp);
+void premult_tmstack (Matrix m);
+void prerotate_tmstack (Vector *vp);
+void prescale_tmstack (Vector *vp);
+void pretranslate_tmstack (Vector *vp);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpToken.h b/modules/robot/include/visp3/robot/vpToken.h
new file mode 100644
index 0000000..1f8bf6d
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpToken.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "token.h" contient les Macros et les Types
+ * des jetons de l'analyseur lexicale .
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+#ifndef vpToken_H
+#define vpToken_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+typedef struct {
+ const char *ident; /* identifateur */
+ Index token; /* code du jeton */
+} Keyword;
+
+#define T_EOF 256
+#define T_FLOAT 257
+#define T_IDENT 258
+#define T_INT 259
+#define T_STRING 260
+
+extern float myfloat;
+extern int myint;
+extern int mylength;
+extern int mylineno;
+extern char *mytext;
+extern Keyword keyword_tbl[];
+
+
+/*
+ * Jetons superieurs a 270 (voir "../mylex/token.h").
+ */
+#define T_ABOVE 270
+#define T_BACK 271
+#define T_BELOW 272
+#define T_BOUND 273
+#define T_COP 274
+#define T_DEPTH 275
+#define T_EXIT 276
+#define T_FACE_LIST 277
+#define T_FILE 278
+#define T_FRONT 279
+#define T_IMAGE 280
+#define T_LEFT 281
+#define T_NONE 282
+#define T_ORIGIN 283
+#define T_PARALLEL 284
+#define T_PERSPECTIVE 285
+#define T_POINT_LIST 286
+#define T_REMOVE 287
+#define T_RIGHT 288
+#define T_SIZE 289
+#define T_TYPE 290
+#define T_VIEW 291
+#define T_VPN 292
+#define T_VRP 293
+#define T_VUP 294
+#define T_WINDOW 295
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpUnicycle.h b/modules/robot/include/visp3/robot/vpUnicycle.h
new file mode 100644
index 0000000..821dd5a
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpUnicycle.h
@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Common features for unicycle mobile robots.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef VPUNICYCLE_H
+#define VPUNICYCLE_H
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+
+/*!
+
+ \class vpUnicycle
+
+ \ingroup group_robot_simu_unicycle
+
+ \brief Generic functions for unicycle mobile robots.
+
+ This class provides common features for unicycle mobile robots.
+
+*/
+class VISP_EXPORT vpUnicycle
+{
+public:
+ /*!
+ Default constructor that does nothing.
+ */
+ vpUnicycle() : cMe_(), eJe_()
+ {
+ };
+ /*!
+ Destructor that does nothing.
+ */
+ virtual ~vpUnicycle() {};
+
+ /*!
+ Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame
+ and the mobile robot end effector frame.
+ */
+ vpHomogeneousMatrix get_cMe() const
+ {
+ return cMe_;
+ }
+
+ /*!
+
+ Return the twist transformation from camera frame to the mobile robot
+ end effector frame. This transformation allows to compute a velocity expressed
+ in the end effector frame into the camera frame.
+ */
+ vpVelocityTwistMatrix get_cVe() const
+ {
+ vpVelocityTwistMatrix cVe;
+ cVe.buildFrom(cMe_) ;
+ return cVe;
+ }
+
+ /*!
+
+ Return the twist transformation from camera frame to the mobile robot
+ end effector frame. This transformation allows to compute a velocity expressed
+ in the end effector frame into the camera frame.
+
+ \sa get_cVe()
+ */
+ void get_cVe(vpVelocityTwistMatrix &cVe) const
+ {
+ cVe = vpUnicycle::get_cVe();
+ }
+
+ /*!
+ Return the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
+
+ \return The robot jacobian such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
+ \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
+ */
+ vpMatrix get_eJe() const
+ {
+ return eJe_;
+ }
+
+ /*!
+ Set the transformation between the camera frame and the end effector frame.
+ */
+ void set_cMe(const vpHomogeneousMatrix &cMe)
+ {
+ cMe_ = cMe;
+ }
+
+ /*!
+ Set the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
+
+ \param eJe : The robot jacobian to set such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
+ \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
+ */
+ void set_eJe(const vpMatrix &eJe)
+ {
+ eJe_ = eJe;
+ }
+
+protected:
+ vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
+ vpMatrix eJe_; // Robot jacobian
+};
+
+#endif
+
+
diff --git a/modules/robot/include/visp3/robot/vpView.h b/modules/robot/include/visp3/robot/vpView.h
new file mode 100644
index 0000000..7ef9298
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpView.h
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "view.h" contient les Macros et les types
+ * des parametres de visualisation et de transformation 3D.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpView_H
+#define vpView_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*
+ * Macros de numerotation des 6 plans de decoupage :
+ * - 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
+ */
+#define PLANE_ABOVE 0
+#define PLANE_BELOW 1
+#define PLANE_RIGHT 2
+#define PLANE_LEFT 3
+#define PLANE_BACK 4
+#define PLANE_FRONT 5
+#define PLANE_NBR 6
+
+/*
+ * Macros de positionnement des points 4D :
+ * Le positionnement d'un point 4D dans l'espace de l'observateur virtuel
+ * se fait par rapport aux 6 plans de decoupage.
+ * A chaque point 4D on associe 6 bits, un par plan de decoupage.
+ */
+#define IS_INSIDE 0x00
+#define IS_ABOVE 0x01
+#define IS_BELOW 0x02
+#define IS_RIGHT 0x04
+#define IS_LEFT 0x08
+#define IS_BACK 0x10
+#define IS_FRONT 0x20
+
+#define vpDEFAULT_REMOVE IS_INSIDE
+
+#define PARALLEL 0
+#define PERSPECTIVE 1
+
+#define vpDEFAULT_EYE { 0.0, 0.0, 1.0 }
+#define vpDEFAULT_TARGET { 0.0, 0.0, 0.0 }
+#define vpDEFAULT_FOCAL 1.0
+#define vpDEFAULT_ANGLE 45.0
+#define vpDEFAULT_TWIST 0.0
+#define vpDEFAULT_SPEED 0.0
+#define vpDEFAULT_CAMERA { vpDEFAULT_EYE, vpDEFAULT_TARGET,\
+ vpDEFAULT_FOCAL,vpDEFAULT_ANGLE, vpDEFAULT_TWIST,\
+ vpDEFAULT_SPEED }
+
+#define vpDEFAULT_COP { 0.0, 0.0, 1.0 }
+#define vpDEFAULT_VRP { 0.0, 0.0, 0.0 }
+#define vpDEFAULT_VPN { 0.0, 0.0,-1.0 }
+#define vpDEFAULT_VUP { 0.0, 1.0, 0.0 }
+#define vpDEFAULT_VWD {-1.0, 1.0,-1.0, 1.0 }
+#define vpDEFAULT_DEPTH { 0.0, 1.0 }
+#define vpDEFAULT_TYPE PERSPECTIVE
+
+#define vpDEFAULT_VIEW { vpDEFAULT_TYPE,\
+ vpDEFAULT_COP, vpDEFAULT_VRP,\
+ vpDEFAULT_VPN, vpDEFAULT_VUP,\
+ vpDEFAULT_VWD, vpDEFAULT_DEPTH }
+
+#define vpDEFAULT_WC { 1.0, 0.0, 0.0, 0.0,\
+ 0.0, 1.0, 0.0, 0.0,\
+ 0.0, 0.0, 1.0, 0.0,\
+ 0.0, 0.0, 0.0, 1.0 }
+
+
+/*
+ * 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).
+ */
+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 */
+} Camera_parameters;
+
+typedef struct {
+ 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") */
+} 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 */
+} View_parameters;
+
+#endif
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpViper.h b/modules/robot/include/visp3/robot/vpViper.h
new file mode 100644
index 0000000..4658783
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpViper.h
@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for a generic ADEPT Viper (either 650 or 850) robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpViper_h
+#define vpViper_h
+
+/*!
+
+ \file vpViper.h
+
+ Modelisation of the ADEPT Viper 650 or 850 robot.
+
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+
+/*!
+
+ \class vpViper
+
+ \ingroup group_robot_real_arm group_robot_simu_arm
+
+ \brief Modelisation of the ADEPT Viper robot
+
+ This robot has six degrees of freedom.
+
+ The non modified Denavit-Hartenberg representation of the robot is
+ given in the table below, where \f$q_1^*, \ldots, q_6^*\f$
+ are the variable joint positions.
+
+ \f[
+ \begin{tabular}{|c|c|c|c|c|}
+ \hline
+ Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
+ \hline
+ 1 & $a_1$ & $d_1$ & $-\pi/2$ & $q_1^*$ \\
+ 2 & $a_2$ & 0 & 0 & $q_2^*$ \\
+ 3 & $a_3$ & 0 & $-\pi/2$ & $q_3^* - \pi$ \\
+ 4 & 0 & $d_4$ & $\pi/2$ & $q_4^*$ \\
+ 5 & 0 & 0 & $-\pi/2$ & $q_5^*$ \\
+ 6 & 0 & 0 & 0 & $q_6^*-\pi$ \\
+ 7 & 0 & $d_6$ & 0 & 0 \\
+ \hline
+ \end{tabular}
+ \f]
+
+ In this modelisation, different frames have to be considered.
+ - \f$ {\cal F}_f \f$: the reference frame, also called world frame,
+
+ - \f$ {\cal F}_w \f$: the wrist frame located at the intersection of
+ the last three rotations, with \f$ ^f{\bf M}_w = ^0{\bf M}_6 \f$,
+
+ - \f$ {\cal F}_e \f$: the end-effector frame, with \f$^f{\bf M}_e =
+ 0{\bf M}_7 \f$,
+
+ - \f$ {\cal F}_c \f$: the camera frame, with \f$^f{\bf M}_c = ^f{\bf
+ M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
+ a calibration stage.
+
+ The forward kinematics of the robot is implemented in get_fMw(),
+ get_fMe() and get_fMc().
+
+ The robot forward jacobian used to compute the cartesian velocities
+ from joint ones is given and implemented in get_fJw(), get_fJe() and
+ get_eJe().
+
+*/
+class VISP_EXPORT vpViper
+{
+ public:
+ vpViper();
+ virtual ~vpViper() {};
+
+ vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
+ unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose=false) const;
+ unsigned int getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose=false) const;
+ vpHomogeneousMatrix get_fMc (const vpColVector & q) const;
+ void get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const;
+ void get_wMe(vpHomogeneousMatrix & wMe) const;
+ void get_eMc(vpHomogeneousMatrix & eMc) const;
+ void get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const;
+ void get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const;
+
+ void get_cMe(vpHomogeneousMatrix &cMe) const;
+ void get_cVe(vpVelocityTwistMatrix &cVe) const;
+ void get_fJw(const vpColVector &q, vpMatrix &fJw) const;
+ void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
+ void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
+
+ virtual void set_eMc(const vpHomogeneousMatrix &eMc_);
+ virtual void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_);
+
+ friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper);
+
+ vpColVector getJointMin() const;
+ vpColVector getJointMax() const;
+ double getCoupl56() const;
+
+ private:
+ bool convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose=false) const;
+
+ public:
+ static const unsigned int njoint; ///< Number of joint.
+
+ protected:
+ vpHomogeneousMatrix eMc; //!< End effector to camera transformation
+ // Minimal representation of eMc
+ vpTranslationVector etc; // meters
+ vpRxyzVector erc; // radian
+
+ // Denavit-Hartenberg parameters
+ double a1, d1; //!< for joint 1
+ double a2; //!< for joint 2
+ double a3; //!< for joint 3
+ double d4; //!< for joint 4
+ double d6; //!< for joint 6
+ double c56; //!< Mechanical coupling between joint 5 and joint 6
+
+ // Software joint limits in radians
+ vpColVector joint_max; // Maximal value of the joints
+ vpColVector joint_min; // Minimal value of the joints
+};
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpViper650.h b/modules/robot/include/visp3/robot/vpViper650.h
new file mode 100644
index 0000000..0d38a5a
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpViper650.h
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ADEPT Viper 650 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpViper650_h
+#define vpViper650_h
+
+/*!
+
+ \file vpViper650.h
+
+ Modelisation of the ADEPT Viper 650 robot.
+
+*/
+
+/*!
+
+ \class vpViper650
+
+ \ingroup group_robot_real_arm
+
+ \brief Modelisation of the ADEPT Viper 650 robot.
+
+*/
+
+#include <visp3/robot/vpViper.h>
+
+
+class VISP_EXPORT vpViper650: public vpViper
+{
+ public:
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ //! Files where constant tranformation between end-effector and camera frame
+ //! are stored.
+ static const char * const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_CAMERA_FILENAME;
+#endif
+ /*!
+ Name of the camera attached to the end-effector.
+ */
+ static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
+ static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
+ static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
+ static const char * const CONST_GENERIC_CAMERA_NAME;
+
+ //! List of possible tools that can be attached to the robot end-effector.
+ typedef enum {
+ TOOL_MARLIN_F033C_CAMERA, /*!< Marlin F033C camera. */
+ TOOL_PTGREY_FLEA2_CAMERA, /*!< Point Grey Flea 2 camera. */
+ TOOL_SCHUNK_GRIPPER_CAMERA, /*!< Camera attached to the Schunk gripper. */
+ TOOL_GENERIC_CAMERA, /*!< A generic camera. */
+ TOOL_CUSTOM /*!< A user defined tool. */
+ } vpToolType;
+
+ //! Default tool attached to the robot end effector
+ static const vpToolType defaultTool;
+
+ vpViper650();
+ virtual ~vpViper650() {};
+
+ void init (void);
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ void init(const char *camera_extrinsic_parameters);
+#endif
+ void init (vpViper650::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel =
+ vpCameraParameters::perspectiveProjWithoutDistortion);
+ void init (vpViper650::vpToolType tool,
+ const std::string &filename);
+ void init (vpViper650::vpToolType tool,
+ const vpHomogeneousMatrix &eMc_);
+
+
+ //! Get the current camera model projection type
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const {
+ return projModel;
+ };
+
+ void getCameraParameters(vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const;
+ void getCameraParameters(vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
+
+ //! Get the current tool type
+ vpToolType getToolType() const{
+ return tool_current;
+ };
+
+ void parseConfigFile (const char * filename);
+
+ protected:
+ //! Set the current tool type
+ void setToolType(vpViper650::vpToolType tool){
+ tool_current = tool;
+ };
+
+ protected:
+ //! Current tool in use
+ vpToolType tool_current;
+ // Used projection model
+ vpCameraParameters::vpCameraParametersProjType projModel;
+
+};
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpViper850.h b/modules/robot/include/visp3/robot/vpViper850.h
new file mode 100644
index 0000000..cd266c6
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpViper850.h
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ADEPT Viper 850 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpViper850_h
+#define vpViper850_h
+
+/*!
+
+ \file vpViper850.h
+
+ Modelisation of the ADEPT Viper 850 robot.
+
+*/
+
+/*!
+
+ \class vpViper850
+
+ \ingroup group_robot_real_arm group_robot_simu_arm
+
+ \brief Modelisation of the ADEPT Viper 850 robot.
+
+*/
+
+#include <visp3/robot/vpViper.h>
+
+
+class VISP_EXPORT vpViper850: public vpViper
+{
+ public:
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ //! Files where constant tranformation between end-effector and camera frame
+ //! are stored.
+ static const char * const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ static const char * const CONST_CAMERA_FILENAME;
+#endif
+ /*!
+ Name of the camera attached to the end-effector.
+ */
+ static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
+ static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
+ static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
+ static const char * const CONST_GENERIC_CAMERA_NAME;
+
+ //! List of possible tools that can be attached to the robot end-effector.
+ typedef enum {
+ TOOL_MARLIN_F033C_CAMERA, /*!< Marlin F033C camera. */
+ TOOL_PTGREY_FLEA2_CAMERA, /*!< Point Grey Flea 2 camera. */
+ TOOL_SCHUNK_GRIPPER_CAMERA, /*!< Camera attached to the Schunk gripper. */
+ TOOL_GENERIC_CAMERA /*!< A generic camera. */
+ } vpToolType;
+
+ //! Default tool attached to the robot end effector
+ static const vpToolType defaultTool;
+
+ vpViper850();
+ virtual ~vpViper850() {};
+
+ void init (void);
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ void init(const char *camera_extrinsic_parameters);
+#endif
+ void init (vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel =
+ vpCameraParameters::perspectiveProjWithoutDistortion);
+
+
+ //! Get the current camera model projection type
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const{
+ return projModel;
+ };
+
+ void getCameraParameters(vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const;
+ void getCameraParameters(vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
+
+ //! Get the current tool type
+ vpToolType getToolType() const {
+ return tool_current;
+ };
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ void parseConfigFile (const char * filename);
+#endif
+
+ protected:
+ //! Set the current tool type
+ void setToolType(vpViper850::vpToolType tool){
+ tool_current = tool;
+ };
+
+ protected:
+ //! Current tool in use
+ vpToolType tool_current;
+ // Used projection model
+ vpCameraParameters::vpCameraParametersProjType projModel;
+
+};
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
+#endif
+
diff --git a/modules/robot/include/visp3/robot/vpVwstack.h b/modules/robot/include/visp3/robot/vpVwstack.h
new file mode 100644
index 0000000..f08385b
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpVwstack.h
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "vwstack.h" contient les macros, les types et
+ * les specifications des procedures de gestion de la pile
+ * des points de vue (VieW STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#ifndef vpVwstack_H
+#define vpVwstack_H
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void fprintf_vwstack (FILE *fp, char *argv);
+extern View_parameters *get_vwstack (void);
+void load_vwstack (View_parameters *vp);
+void pop_vwstack (void);
+void push_vwstack (void);
+void swap_vwstack (void);
+void add_vwstack (const char* path, ...);
+
+#endif
+#endif
diff --git a/modules/robot/include/visp3/robot/vpWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h
new file mode 100644
index 0000000..234430a
--- /dev/null
+++ b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h
@@ -0,0 +1,570 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Wire frame simulator
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#ifndef vpWireFrameSimulator_HH
+#define vpWireFrameSimulator_HH
+
+/*!
+ \file vpWireFrameSimulator.h
+ \brief Implementation of a wire frame simulator.
+*/
+#include <visp3/core/vpConfig.h>
+#include <stdio.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <list>
+#include <string>
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpBound.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpTmstack.h>
+#include <visp3/robot/vpVwstack.h>
+#include <visp3/robot/vpRfstack.h>
+#include <visp3/robot/vpArit.h>
+
+void open_display();
+void close_display();
+void open_clipping();
+void close_clipping();
+void open_keyword (Keyword *kwp);
+void open_lex (void);
+void open_source (FILE *fd, const char *str);
+void malloc_Bound_scene (Bound_scene *bsp, const char *name,Index bn);
+void free_Bound_scene (Bound_scene *bsp);
+void parser (Bound_scene *bsp);
+void close_source (void);
+void close_lex (void);
+void close_keyword (void);
+void display_scene(Matrix mat, Bound_scene sc);
+void View_to_Matrix (View_parameters *vp, Matrix m);
+Bound *clipping_Bound (Bound *bp, Matrix m);
+void set_Bound_face_display (Bound *bp, Byte b);
+void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2);
+void wireframe_Face (Face *fp, Point2i *pp);
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDisplay.h>
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+# include <visp3/core/vpList.h>
+#endif
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/robot/vpImageSimulator.h>
+
+void set_scene (const char*, Bound_scene *, float);
+void vp2jlc_matrix (const vpHomogeneousMatrix, Matrix&);
+
+
+/*!
+ \class vpWireFrameSimulator
+
+ \ingroup group_robot_simu_gantry group_robot_simu_arm
+
+ \brief Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not
+ require third party libraries to be used.
+
+ The simulator uses several frames to display the scene in the image. There are:
+
+ - The world frame : This is a fixed frame used to compute the position of the other frames against each other. By default this frame is positionned at the same location as the initial object frame position.
+
+ - The object frame : It is the frame linked to the object.
+
+ - The camera frame : It is the frame linked to the main camera.
+
+ - The external camera frame : It is the frame which corresponds to one external point of view.
+
+ The most used homogeneous matrices which describes the displacement between two frames are :
+
+ - fMo which is the displacement between the world frame and the object frame.
+
+ - cMo which is the displacement between the main camera frame and the object frame.
+
+ - cextMf which is the displacement between one external camera frame and the world frame.
+
+ The following picture summarize all the previous information.
+
+ \image html vpWireFrameSimulator.jpeg
+ \image latex vpWireFrameSimulator.ps
+
+ The simulator uses .bnd or .wrl files as 3D scene descriptors. Several scenes can be found in the data folder which is in the directory where you build ViSP.
+
+ You can move the main external view while clicking in the image. The left click enables to turn, the middle button enables to zoom and the left to translate along x and y.
+
+ The simulator is able to take into account to camera parameters. You can set the internal and external cameras parameters thanks to a vpCameraParameters.
+
+ The following example shows how it is easy to use.
+
+ \code
+
+ #include <visp3/core/vpImage.h>
+ #include <visp3/robot/vpWireFrameSimulator.h>
+
+ int main()
+ {
+ vpWireFrameSimulator sim;
+
+ vpImage<vpRGBa> Iint(480,640,255);
+ vpImage<vpRGBa> Iext(480,640,255);
+
+ //Set the type of scene to use
+ sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
+
+ //Set the initial pose of the camera
+ sim.setCameraPositionRelObj(vpHomogeneousMatrix(0,0,0.5,vpMath::rad(0),vpMath::rad(10),0));
+ //Set the desired pose of the camera (for the internal view)
+ sim.setDesiredCameraPosition(vpHomogeneousMatrix(0.0,0,0.5,0,0,0));
+ //Set the pose of the reference frame (for the external view)
+ sim.setExternalCameraPosition(vpHomogeneousMatrix(0.1,0,0.2,0,0,0));
+
+ //Set the camera parameters
+ vpCameraParameters camera(1000,1000,320,240);
+ sim.setInternalCameraParameters(camera);
+ sim.setExternalCameraParameters(camera);
+
+ //Get the internal view
+ sim.getInternalImage(Iint);
+
+ //Get the external view
+ sim.getExternalImage(Iext);
+
+ return 0;
+ }
+ \endcode
+*/
+
+class VISP_EXPORT vpWireFrameSimulator
+{
+public:
+
+ /*!
+ Type of scene used to display the object at the current position.
+ */
+ typedef enum
+ {
+ THREE_PTS, //!< A 40cm by 40cm plate with 3 points at coordinates (0,0,0), (0.1,0,0), (0,0.1,0). Each point is represented by a circle with 2cm radius.
+ CUBE, //!< A 12.5cm size cube.
+ PLATE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
+ SMALL_PLATE, //!< 4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0), (0.03,0.03,0), (0.03,0.03,0). Each point is represented by a circle with 1cm radius.
+ RECTANGLE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0.07,-0.05,0), (-0.07,-0.05,0). Each point is represented by a circle with 2cm radius.
+ SQUARE_10CM, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0), (-0.05,-0.05,0). Each point is represented by a circle with 2cm radius.
+ DIAMOND, //!< A 40cm by 40cm plate with 4 points at coordinates (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is represented by a circle with 2cm radius.
+ TRAPEZOID, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0), (0.025,-0.05,0). Each point is represented by a circle with 2cm radius.
+ THREE_LINES, //!< Three parallel lines with equation y=-5, y=0, y=5.
+ ROAD, //!< Three parallel lines representing a road.
+ TIRE, //!< A tire represented by 2 circles with radius 10cm and 15cm.
+ PIPE, //!< A pipe represented by a cylinder of 25 cm length and 15cm radius.
+ CIRCLE, //!< A 10cm radius circle.
+ SPHERE, //!< A 15cm radius sphere.
+ CYLINDER, //!< A cylinder of 80cm length and 10cm radius.
+ PLAN, //!< A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.
+ POINT_CLOUD, //!< A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
+ } vpSceneObject;
+
+ /*!
+ Type of scene used to display the object at the desired pose (in the internal view).
+
+ - D_STANDARD will use the vpSceneObject used to be the object at the current position.
+ - D_OUTIL will display a tool which is attached to the camera.
+ */
+ typedef enum
+ {
+ D_STANDARD, //!< The object displayed at the desired position is the same than the scene object defined in vpSceneObject.
+ D_CIRCLE, //!< The object displayed at the desired position is a circle.
+ D_TOOL //!< A cylindrical tool is attached to the camera.
+ } vpSceneDesiredObject;
+
+ typedef enum
+ {
+ CT_LINE,
+ CT_POINT
+ } vpCameraTrajectoryDisplayType;
+
+protected:
+ Bound_scene scene;
+ Bound_scene desiredScene;
+ Bound_scene camera;
+ std::list<vpImageSimulator> objectImage;
+
+ vpHomogeneousMatrix fMo;
+ vpHomogeneousMatrix fMc;
+ vpHomogeneousMatrix camMf;
+ vpHomogeneousMatrix refMo;
+ vpHomogeneousMatrix cMo;
+ vpHomogeneousMatrix cdMo;
+
+ vpSceneObject object;
+ vpSceneDesiredObject desiredObject;
+
+ vpColor camColor;
+ vpColor camTrajColor;
+ vpColor curColor;
+ vpColor desColor;
+
+ bool sceneInitialized;
+
+ bool displayCameraTrajectory;
+ std::list<vpImagePoint> cameraTrajectory;
+ std::list<vpHomogeneousMatrix> poseList;
+ std::list<vpHomogeneousMatrix> fMoList;
+ unsigned int nbrPtLimit;
+
+ vpImagePoint old_iPr;
+ vpImagePoint old_iPz;
+ vpImagePoint old_iPt;
+ bool blockedr;
+ bool blockedz;
+ bool blockedt;
+ bool blocked;
+
+ vpHomogeneousMatrix camMf2;
+ vpHomogeneousMatrix f2Mf;
+
+ double px_int;
+ double py_int;
+ double px_ext;
+ double py_ext;
+
+ bool displayObject;
+ bool displayDesiredObject;
+ bool displayCamera;
+ bool displayImageSimulator;
+
+ float cameraFactor;
+
+ vpCameraTrajectoryDisplayType camTrajType;
+
+ bool extCamChanged;
+
+ vpHomogeneousMatrix rotz;
+
+ unsigned int thickness_;
+
+private:
+ std::string scene_dir;
+
+public:
+ vpWireFrameSimulator();
+ virtual ~vpWireFrameSimulator();
+
+ /*!
+ Delete the history of the main camera position which are displayed in the external views.
+ */
+ inline void deleteCameraPositionHistory() {
+ cameraTrajectory.clear();
+ poseList.clear();
+ fMoList.clear();}
+
+ void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
+ void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
+
+ /*!
+ Get the parameters of the virtual external camera.
+
+ \param I : The image used to display the view of the camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const {
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
+ else
+ {
+ unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
+ return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ }
+ }
+ /*!
+ Get the parameters of the virtual external camera.
+
+ \param I : The image used to display the view of the camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const {
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
+ else
+ {
+ unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
+ return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ }
+ }
+ /*!
+ Get the main external camera's position relative to the the world reference frame.
+
+ \return the main external camera position relative to the the world reference frame.
+ */
+ inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf;}
+
+ void getExternalImage(vpImage<unsigned char> &I);
+ void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
+ void getExternalImage(vpImage<vpRGBa> &I);
+ void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
+
+ /*!
+ Get the parameters of the virtual internal camera.
+
+ \param I : The image used to display the view of the camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const {
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
+ else
+ {
+ unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
+ return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ }
+ }
+ /*!
+ Get the parameters of the virtual internal camera.
+
+ \param I : The image used to display the view of the camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const {
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
+ else
+ {
+ unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
+ return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ }
+ }
+
+ void getInternalImage(vpImage<unsigned char> &I);
+ void getInternalImage(vpImage<vpRGBa> &I);
+
+ /*!
+ Get the pose between the object and the camera.
+
+ \return The pose between between the object and the camera.
+ */
+ vpHomogeneousMatrix get_cMo() const {return rotz*cMo;}
+
+ /*!
+ Get the homogeneous matrices cMo stored to display the camera trajectory.
+
+ \param cMo_history : The list of the homogeneous matrices cMo.
+ */
+ void get_cMo_History(std::list<vpHomogeneousMatrix>& cMo_history) {
+ cMo_history.clear();
+ for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
+ cMo_history.push_back(rotz*(*it));
+ }
+ }
+
+ /*!
+ Get the pose between the object and the fixed world frame.
+
+ \return The pose between the object and the fixed world frame.
+ */
+ vpHomogeneousMatrix get_fMo() const {return fMo;}
+
+ /*!
+ Get the homogeneous matrices fMo stored to display the camera trajectory.
+
+ \param fMo_history : The list of the homogeneous matrices fMo.
+ */
+ void get_fMo_History(std::list<vpHomogeneousMatrix>& fMo_history) {fMo_history = fMoList;}
+
+ void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
+ void initScene(const char* obj, const char* desiredObject);
+ void initScene(const vpSceneObject &obj);
+ void initScene(const char* obj);
+
+ void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list<vpImageSimulator> &imObj);
+ void initScene(const char* obj, const char* desiredObject, const std::list<vpImageSimulator> &imObj);
+ void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
+ void initScene(const char* obj, const std::list<vpImageSimulator> &imObj);
+
+ /*!
+ Set the color used to display the camera in the external view.
+
+ \param col : The desired color.
+ */
+ void setCameraColor(const vpColor &col) {camColor = col;}
+ /*!
+ Set the position of the camera relative to the object.
+
+ \param cMo_ : The pose of the camera.
+ */
+ void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_) {this->cMo = rotz * cMo_; fMc = fMo*this->cMo.inverse();}
+
+ /*!
+ Set the position of the the world reference frame relative to the camera.
+
+ \param fMc_ : The pose of the camera.
+ */
+ void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_) {this->fMc = fMc_*rotz; cMo = this->fMc.inverse()*fMo;}
+
+ /*!
+ Set the parameter which enables to choose the size of the main camera in the external camera views. By default this parameter is set to 1.
+
+ \param factor : The ration for the camera size.
+ */
+ inline void setCameraSizeFactor (const float factor) {cameraFactor = factor;}
+
+ /*!
+ Set the color used to display the camera trajectory in the external view.
+
+ \param col : The desired color.
+ */
+ void setCameraTrajectoryColor(const vpColor &col) {camTrajColor = col;}
+
+ /*!
+ Set the way to display the history of the main camera trajectory in the main external view. The choice is given between displaying lines and points.
+
+ \param camTraj_type : The chosen way to display the camera trajectory.
+ */
+ inline void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType &camTraj_type) {this->camTrajType = camTraj_type;}
+
+ /*!
+ Set the color used to display the object at the current position.
+
+ \param col : The desired color.
+ */
+ void setCurrentViewColor(const vpColor &col) {curColor = col;}
+ /*!
+ Set the desired position of the camera relative to the object.
+
+ \param cdMo_ : The desired pose of the camera.
+ */
+ void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) {this->cdMo = rotz * cdMo_;}
+ /*!
+ Set the color used to display the object at the desired position.
+
+ \param col : The desired color.
+ */
+ void setDesiredViewColor(const vpColor &col) {desColor = col;}
+ /*!
+ Enable or disable the displaying of the camera trajectory in the main external camera view.
+
+ By default the trajectory is displayed.
+
+ \param do_display : Set to true to display the camera trajectory.
+ */
+ void setDisplayCameraTrajectory (const bool &do_display) {this->displayCameraTrajectory = do_display;}
+
+ /*!
+ Set the internal camera parameters.
+
+ \param cam : The desired camera parameters.
+ */
+ inline void setExternalCameraParameters(const vpCameraParameters &cam) {
+ px_ext = cam.get_px();
+ py_ext = cam.get_py();
+ }
+ /*!
+ Set the external camera point of view.
+
+ \param cam_Mf : The pose of the external camera relative to the world reference frame.
+ */
+ void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
+ {
+ this->camMf = rotz * cam_Mf;
+ vpTranslationVector T;
+ this->camMf.extract (T);
+ this->camMf2.buildFrom(0,0,T[2],0,0,0);
+ f2Mf = camMf2.inverse()*this->camMf;
+ extCamChanged = true;
+ }
+
+ /*!
+ Specify the thickness of the graphics drawings.
+ */
+ void setGraphicsThickness(unsigned int thickness)
+ {
+ this->thickness_ = thickness;
+ }
+
+ /*!
+ Set the internal camera parameters.
+
+ \param cam : The desired camera parameters.
+ */
+ inline void setInternalCameraParameters(const vpCameraParameters &cam) {
+ px_int = cam.get_px();
+ py_int = cam.get_py();
+ }
+
+ /*!
+ Set the maximum number of main camera's positions which are stored. Those position can be displayed in the external camera field of view. By default this parameter is set to 1000.
+
+ \param nbPt : The desired number of position which are saved.
+ */
+ inline void setNbPtTrajectory(const unsigned int nbPt) {nbrPtLimit = nbPt;}
+
+ /*!
+ Set the pose between the object and the fixed world frame.
+
+ \param fMo_ : The pose between the object and the fixed world frame.
+ */
+ void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;/*this->cMo = fMc.inverse()*fMo;*/}
+
+protected:
+ void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
+ void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
+ vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
+ vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
+ vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
+ vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
+ vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
+ vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
+};
+
+#endif
diff --git a/modules/robot/src/image-simulator/vpImageSimulator.cpp b/modules/robot/src/image-simulator/vpImageSimulator.cpp
new file mode 100644
index 0000000..56594cc
--- /dev/null
+++ b/modules/robot/src/image-simulator/vpImageSimulator.cpp
@@ -0,0 +1,1651 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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: Class which enables to project an image in the 3D space
+ * and get the view of a virtual camera.
+ *
+ * Authors:
+ * Amaury Dame
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpPolygon3D.h>
+
+#ifdef VISP_HAVE_MODULE_IO
+# include <visp3/io/vpImageIo.h>
+#endif
+
+/*!
+ Basic constructor.
+
+ You can choose if you want to use a colored or gray scaled image.
+
+ \param col : Enable to choose the color space to use for the image which is projected.
+
+ By default the class uses colored images.
+*/
+vpImageSimulator::vpImageSimulator(const vpColorPlan &col)
+ : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
+ distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
+ euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
+ vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
+ colorI(col), Ig(), Ic(), rect(), cleanPrevImage(false),
+ setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
+{
+ for(int i=0;i<4;i++)
+ X[i].resize(3);
+
+ for(int i=0;i<4;i++)
+ X2[i].resize(3);
+
+ normal_obj.resize(3);
+ visible=false;
+ normal_Cam.resize(3);
+
+ //Xinter.resize(3);
+
+ vbase_u.resize(3);
+ vbase_v.resize(3);
+
+ focal.resize(3);
+ focal=0;
+ focal[2]=1;
+
+ normal_Cam_optim = new double[3];
+ X0_2_optim = new double[3];
+ vbase_u_optim = new double[3];
+ vbase_v_optim = new double[3];
+ Xinter_optim = new double[3];
+
+ pt.resize(4);
+}
+
+
+/*!
+ Copy constructor
+*/
+vpImageSimulator::vpImageSimulator(const vpImageSimulator &text)
+ : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
+ distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
+ euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
+ vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
+ colorI(GRAY_SCALED), Ig(), Ic(), rect(), cleanPrevImage(false),
+ setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
+{
+ pt.resize(4);
+ for(unsigned int i=0;i<4;i++)
+ {
+ X[i] = text.X[i];
+ pt[i] = text.pt[i];
+ }
+
+ for(int i=0;i<4;i++)
+ X2[i].resize(3);
+
+ Ic = text.Ic;
+ Ig = text.Ig;
+
+ focal.resize(3);
+ focal=0;
+ focal[2]=1;
+
+ normal_obj = text.normal_obj;
+ euclideanNorm_u = text.euclideanNorm_u;
+ euclideanNorm_v = text.euclideanNorm_v;
+
+ normal_Cam.resize(3);
+ vbase_u.resize(3);
+ vbase_v.resize(3);
+
+
+ normal_Cam_optim = new double[3];
+ X0_2_optim = new double[3];
+ vbase_u_optim = new double[3];
+ vbase_v_optim = new double[3];
+ Xinter_optim = new double[3];
+
+ colorI = text.colorI;
+ interp = text.interp;
+ bgColor = text.bgColor;
+ cleanPrevImage = text.cleanPrevImage;
+ setBackgroundTexture = false;
+
+ setCameraPosition(text.cMt);
+}
+
+/*!
+ Basic destructor.
+*/
+vpImageSimulator::~vpImageSimulator()
+{
+ delete[] normal_Cam_optim;
+ delete[] X0_2_optim;
+ delete[] vbase_u_optim;
+ delete[] vbase_v_optim;
+ delete[] Xinter_optim;
+}
+
+
+vpImageSimulator&
+vpImageSimulator::operator=(const vpImageSimulator& sim)
+{
+ for(unsigned int i=0;i<4;i++)
+ {
+ X[i] = sim.X[i];
+ pt[i] = sim.pt[i];
+ }
+
+ Ic = sim.Ic;
+ Ig = sim.Ig;
+
+ bgColor = sim.bgColor;
+ cleanPrevImage = sim.cleanPrevImage;
+
+ focal = sim.focal;
+
+ normal_obj = sim.normal_obj;
+ euclideanNorm_u = sim.euclideanNorm_u;
+ euclideanNorm_v = sim.euclideanNorm_v;
+
+ colorI = sim.colorI;
+ interp = sim.interp;
+
+ setCameraPosition(sim.cMt);
+
+ return *this;
+}
+
+/*!
+ Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
+ \param I : The image used to store the result.
+ \param cam : The parameters of the virtual camera.
+*/
+void
+vpImageSimulator::getImage(vpImage<unsigned char> &I,
+ const vpCameraParameters &cam)
+{
+ int nb_point_dessine = 0;
+ if (setBackgroundTexture)
+ // The Ig has been set to a previously defined background texture
+ I = Ig;
+ else
+ {
+ if (cleanPrevImage)
+ {
+ unsigned char col = (unsigned char) (0.2126 * bgColor.R
+ + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = col;
+ }
+ }
+ }
+ }
+
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ unsigned char *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ if (colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan = 0;
+ if(getPixel(ip,Ipixelplan))
+ {
+ *(bitmap+i*width+j)=Ipixelplan;
+ nb_point_dessine++;
+ }
+ }
+ else if (colorI == COLORED)
+ {
+ vpRGBa Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap+i*width+j)=pixelgrey;
+ nb_point_dessine++;
+ }
+ }
+ }
+ }
+ }
+}
+
+
+/*!
+ Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
+
+ \param I : The image used to store the result.
+ \param Isrc : The image which is projected into \f$ I \f$.
+ \param cam : The parameters of the virtual camera.
+*/
+void
+vpImageSimulator::getImage(vpImage<unsigned char> &I,
+ vpImage<unsigned char> &Isrc,
+ const vpCameraParameters &cam)
+{
+ int nb_point_dessine = 0;
+ if (cleanPrevImage)
+ {
+ unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = col;
+ }
+ }
+ }
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ unsigned char *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ unsigned char Ipixelplan = 0;
+ if(getPixel(Isrc,ip,Ipixelplan))
+ {
+ *(bitmap+i*width+j)=Ipixelplan;
+ nb_point_dessine++;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
+
+ To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
+
+ \param I : The image used to store the result.
+ \param cam : The parameters of the virtual camera.
+ \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
+*/
+void
+vpImageSimulator::getImage(vpImage<unsigned char> &I,
+ const vpCameraParameters &cam, vpMatrix &zBuffer)
+{
+ if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
+ throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
+
+ int nb_point_dessine = 0;
+ if (cleanPrevImage)
+ {
+ unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = col;
+ }
+ }
+ }
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ unsigned char *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ if (colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
+ {
+ *(bitmap+i*width+j)=Ipixelplan;
+ nb_point_dessine++;
+ zBuffer[i][j] = Xinter_optim[2];
+ }
+ }
+ }
+ else if (colorI == COLORED)
+ {
+ vpRGBa Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
+ {
+ unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap+i*width+j)=pixelgrey;
+ nb_point_dessine++;
+ zBuffer[i][j] = Xinter_optim[2];
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
+
+ \param I : The image used to store the result.
+ \param cam : The parameters of the virtual camera.
+*/
+void
+vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
+{
+ int nb_point_dessine = 0;
+ if (cleanPrevImage)
+ {
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = bgColor;
+ }
+ }
+ }
+
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ vpRGBa *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ if (colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ vpRGBa pixelcolor;
+ pixelcolor.R = Ipixelplan;
+ pixelcolor.G = Ipixelplan;
+ pixelcolor.B = Ipixelplan;
+ *(bitmap+i*width+j) = pixelcolor;
+ nb_point_dessine++;
+ }
+ }
+ else if (colorI == COLORED)
+ {
+ vpRGBa Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ *(bitmap+i*width+j) = Ipixelplan;
+ nb_point_dessine++;
+ }
+ }
+ }
+ }
+ }
+}
+
+
+/*!
+ Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
+
+ \param I : The image used to store the result.
+ \param Isrc : The image which is projected into \f$ I \f$.
+ \param cam : The parameters of the virtual camera.
+*/
+void
+vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
+ const vpCameraParameters &cam)
+{
+ int nb_point_dessine = 0;
+ if (cleanPrevImage)
+ {
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = bgColor;
+ }
+ }
+ }
+
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ vpRGBa *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ vpRGBa Ipixelplan;
+ if(getPixel(Isrc,ip,Ipixelplan))
+ {
+ *(bitmap+i*width+j) = Ipixelplan;
+ nb_point_dessine++;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
+
+ To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
+
+ \param I : The image used to store the result.
+ \param cam : The parameters of the virtual camera.
+ \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
+*/
+void
+vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
+ vpMatrix &zBuffer)
+{
+ if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
+ throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
+
+ int nb_point_dessine = 0;
+ if (cleanPrevImage)
+ {
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ {
+ I[i][j] = bgColor;
+ }
+ }
+ }
+ if(visible)
+ {
+ if(!needClipping)
+ getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ else
+ getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+
+ double top = rect.getTop();
+ double bottom = rect.getBottom();
+ double left = rect.getLeft();
+ double right= rect.getRight();
+
+ vpRGBa *bitmap = I.bitmap;
+ unsigned int width = I.getWidth();
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
+ {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
+ {
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ if (colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
+ {
+ vpRGBa pixelcolor;
+ pixelcolor.R = Ipixelplan;
+ pixelcolor.G = Ipixelplan;
+ pixelcolor.B = Ipixelplan;
+ *(bitmap+i*width+j) = pixelcolor;
+ nb_point_dessine++;
+ zBuffer[i][j] = Xinter_optim[2];
+ }
+ }
+ }
+ else if (colorI == COLORED)
+ {
+ vpRGBa Ipixelplan;
+ if(getPixel(ip,Ipixelplan))
+ {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
+ {
+ *(bitmap+i*width+j) = Ipixelplan;
+ nb_point_dessine++;
+ zBuffer[i][j] = Xinter_optim[2];
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
+ With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
+
+ The following example shows how to use the method:
+
+ \code
+ #include <list>
+ #include <visp3/core/vpImage.h>
+ #include <visp3/robot/vpImageSimulator.h>
+
+ int main()
+ {
+ vpImage<vpRGBa> Icamera(480,640,0);
+ vpImage<vpRGBa> Iimage(60,60);
+
+ // Initialise the image which will be projected into the image Icamera
+ vpRGBa colorb(0,0,255);
+ vpRGBa colorw(255,255,255);
+ vpRGBa colorr(255,0,0);
+ for(int i = 0; i < 60; i++)
+ {
+ for(int j = 0; j < 20; j++)
+ Iimage[i][j] = colorb;
+ for(int j = 20; j < 40; j++)
+ Iimage[i][j] = colorw;
+ for(int j = 40; j < 60; j++)
+ Iimage[i][j] = colorr;
+ }
+
+ // Initialise the 3D coordinates of the Iimage corners
+ vpColVector X[4];
+ for (int i = 0; i < 4; i++) X[i].resize(3);
+ // Top left corner
+ X[0][0] = -1;
+ X[0][1] = -1;
+ X[0][2] = 1;
+
+ // Top right corner
+ X[1][0] = 1;
+ X[1][1] = -1;
+ X[1][2] = 1;
+
+ // Bottom right corner
+ X[2][0] = 1;
+ X[2][1] = 1;
+ X[2][2] = 1;
+
+ //Bottom left corner
+ X[3][0] = -1;
+ X[3][1] = 1;
+ X[3][2] = 1;
+
+ vpImageSimulator sim;
+ sim.init(Iimage, X);
+
+ // Top left corner
+ X[0][0] = -1;
+ X[0][1] = -1;
+ X[0][2] = 1;
+
+ // Top right corner
+ X[1][0] = 1;
+ X[1][1] = -1;
+ X[1][2] = 1;
+
+ // Bottom right corner
+ X[2][0] = 1;
+ X[2][1] = 1;
+ X[2][2] = 1;
+
+ //Bottom left corner
+ X[3][0] = -1;
+ X[3][1] = 1;
+ X[3][2] = 1;
+
+ vpImageSimulator sim2;
+ sim2.init(Iimage, X);
+
+ sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
+ sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
+
+ std::list<vpImageSimulator> listSim;
+ listSim.addRight(sim);
+ listSim.addRight(sim2);
+
+ sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
+
+ vpCameraParameters cam(868.0, 869.0, 320, 240);
+
+ vpImageSimulator::getImage(Icamera,listSim,cam);
+
+ return 0;
+ }
+ \endcode
+
+ \param I : The image used to store the result
+ \param list : List of vpImageSimulator to project
+ \param cam : The parameters of the virtual camera
+*/
+void
+vpImageSimulator::getImage(vpImage<unsigned char> &I,
+ std::list<vpImageSimulator> &list,
+ const vpCameraParameters &cam)
+{
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ unsigned int nbsimList = (unsigned int)list.size();
+
+ if (nbsimList < 1)
+ return;
+
+ vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
+
+ double topFinal = height+1;;
+ double bottomFinal = -1;
+ double leftFinal = width+1;
+ double rightFinal = -1;
+
+ unsigned int unvisible = 0;
+ unsigned int indexSimu=0;
+ for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
+ vpImageSimulator* sim = &(*it);
+ if (sim->visible)
+ simList[indexSimu] = sim;
+ else
+ unvisible++;
+ }
+ nbsimList = nbsimList - unvisible;
+
+ if (nbsimList < 1)
+ {
+ delete[] simList;
+ return;
+ }
+
+
+ for (unsigned int i = 0; i < nbsimList; i++)
+ {
+ if(!simList[i]->needClipping)
+ simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
+ else
+ simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
+
+ if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
+ if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
+ if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
+ if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
+ }
+
+ double zmin = -1;
+ int indice = -1;
+ unsigned char *bitmap = I.bitmap;
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
+ {
+ for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
+ {
+ zmin = -1;
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ for (int k = 0; k < (int)nbsimList; k++)
+ {
+ double z = 0;
+ if(simList[k]->getPixelDepth(ip,z))
+ {
+ if (z < zmin || zmin < 0)
+ {
+ zmin = z;
+ indice = k;
+ }
+ }
+ }
+ if (indice >= 0)
+ {
+ if (simList[indice]->colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan = 255;
+ simList[indice]->getPixel(ip,Ipixelplan);
+ *(bitmap+i*width+j)=Ipixelplan;
+ }
+ else if (simList[indice]->colorI == COLORED)
+ {
+ vpRGBa Ipixelplan(255,255,255);
+ simList[indice]->getPixel(ip,Ipixelplan);
+ unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap+i*width+j)=pixelgrey;
+ }
+ }
+ }
+ }
+
+ delete[] simList;
+}
+
+
+/*!
+ Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
+
+ With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
+
+ The following example shows how to use the method:
+
+ \code
+ #include <list>
+ #include <visp3/core/vpImage.h>
+ #include <visp3/robot/vpImageSimulator.h>
+
+ int main()
+ {
+ vpImage<vpRGBa> Icamera(480,640,0);
+ vpImage<vpRGBa> Iimage(60,60);
+
+ // Initialise the image which will be projected into the image Icamera
+ vpRGBa colorb(0,0,255);
+ vpRGBa colorw(255,255,255);
+ vpRGBa colorr(255,0,0);
+ for(int i = 0; i < 60; i++)
+ {
+ for(int j = 0; j < 20; j++)
+ Iimage[i][j] = colorb;
+ for(int j = 20; j < 40; j++)
+ Iimage[i][j] = colorw;
+ for(int j = 40; j < 60; j++)
+ Iimage[i][j] = colorr;
+ }
+
+ // Initialise the 3D coordinates of the Iimage corners
+ vpColVector X[4];
+ for (int i = 0; i < 4; i++) X[i].resize(3);
+ // Top left corner
+ X[0][0] = -1;
+ X[0][1] = -1;
+ X[0][2] = 1;
+
+ // Top right corner
+ X[1][0] = 1;
+ X[1][1] = -1;
+ X[1][2] = 1;
+
+ // Bottom right corner
+ X[2][0] = 1;
+ X[2][1] = 1;
+ X[2][2] = 1;
+
+ //Bottom left corner
+ X[3][0] = -1;
+ X[3][1] = 1;
+ X[3][2] = 1;
+
+ vpImageSimulator sim;
+ sim.init(Iimage, X);
+
+ // Top left corner
+ X[0][0] = -1;
+ X[0][1] = -1;
+ X[0][2] = 1;
+
+ // Top right corner
+ X[1][0] = 1;
+ X[1][1] = -1;
+ X[1][2] = 1;
+
+ // Bottom right corner
+ X[2][0] = 1;
+ X[2][1] = 1;
+ X[2][2] = 1;
+
+ //Bottom left corner
+ X[3][0] = -1;
+ X[3][1] = 1;
+ X[3][2] = 1;
+
+ vpImageSimulator sim2;
+ sim2.init(Iimage, X);
+
+ sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
+ sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
+
+ std::list<vpImageSimulator> listSim;
+ listSim.addRight(sim);
+ listSim.addRight(sim2);
+
+ sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
+
+ vpCameraParameters cam(868.0, 869.0, 320, 240);
+
+ vpImageSimulator::getImage(Icamera,listSim,cam);
+
+ return 0;
+ }
+ \endcode
+
+ \param I : The image used to store the result
+ \param list : List of vpImageSimulator to project
+ \param cam : The parameters of the virtual camera
+*/
+void
+vpImageSimulator::getImage(vpImage<vpRGBa> &I,
+ std::list<vpImageSimulator> &list,
+ const vpCameraParameters &cam)
+{
+
+ unsigned int width = I.getWidth();
+ unsigned int height = I.getHeight();
+
+ unsigned int nbsimList = (unsigned int)list.size();
+
+ if (nbsimList < 1)
+ return;
+
+ vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
+
+ double topFinal = height+1;;
+ double bottomFinal = -1;
+ double leftFinal = width+1;
+ double rightFinal = -1;
+
+ unsigned int unvisible = 0;
+ unsigned int indexSimu = 0;
+ for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
+ vpImageSimulator* sim = &(*it);
+ if (sim->visible)
+ simList[indexSimu] = sim;
+ else
+ unvisible++;
+ }
+
+ nbsimList = nbsimList - unvisible;
+
+ if (nbsimList < 1)
+ {
+ delete[] simList;
+ return;
+ }
+
+ for (unsigned int i = 0; i < nbsimList; i++)
+ {
+ if(!simList[i]->needClipping)
+ simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
+ else
+ simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
+
+ if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
+ if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
+ if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
+ if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
+ }
+
+ double zmin = -1;
+ int indice = -1;
+ vpRGBa *bitmap = I.bitmap;
+ vpImagePoint ip;
+
+ for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
+ {
+ for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
+ {
+ zmin = -1;
+ double x=0,y=0;
+ ip.set_ij(i,j);
+ vpPixelMeterConversion::convertPoint(cam,ip, x,y);
+ ip.set_ij(y,x);
+ for (int k = 0; k < (int)nbsimList; k++)
+ {
+ double z = 0;
+ if(simList[k]->getPixelDepth(ip,z))
+ {
+ if (z < zmin || zmin < 0)
+ {
+ zmin = z;
+ indice = k;
+ }
+ }
+ }
+ if (indice >= 0)
+ {
+ if (simList[indice]->colorI == GRAY_SCALED)
+ {
+ unsigned char Ipixelplan = 255;
+ simList[indice]->getPixel(ip,Ipixelplan);
+ vpRGBa pixelcolor;
+ pixelcolor.R = Ipixelplan;
+ pixelcolor.G = Ipixelplan;
+ pixelcolor.B = Ipixelplan;
+ *(bitmap+i*width+j) = pixelcolor;
+ }
+ else if (simList[indice]->colorI == COLORED)
+ {
+ vpRGBa Ipixelplan(255,255,255);
+ simList[indice]->getPixel(ip,Ipixelplan);
+ //unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B;
+ *(bitmap+i*width+j)=Ipixelplan;
+ }
+ }
+ }
+ }
+
+ delete[] simList;
+}
+
+/*!
+ Enable to set the position of the 3D plane relative to the virtual camera.
+
+ \param cMt_ : The pose of the plane relative to the virtual camera.
+*/
+void
+vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
+{
+ cMt = cMt_;
+ vpRotationMatrix R;
+ cMt.extract(R);
+ needClipping = false;
+
+ normal_Cam = R * normal_obj;
+
+ visible_result = vpColVector::dotProd(normal_Cam,focal);
+
+ for(unsigned int i = 0; i < 4; i++)
+ pt[i].track(cMt);
+
+ vpColVector e1(3) ;
+ vpColVector e2(3) ;
+ vpColVector facenormal(3) ;
+
+ e1[0] = pt[1].get_X() - pt[0].get_X() ;
+ e1[1] = pt[1].get_Y() - pt[0].get_Y() ;
+ e1[2] = pt[1].get_Z() - pt[0].get_Z() ;
+
+ e2[0] = pt[2].get_X() - pt[1].get_X() ;
+ e2[1] = pt[2].get_Y() - pt[1].get_Y() ;
+ e2[2] = pt[2].get_Z() - pt[1].get_Z() ;
+
+ facenormal = vpColVector::crossProd(e1,e2) ;
+
+ double angle = pt[0].get_X()*facenormal[0] + pt[0].get_Y()*facenormal[1] + pt[0].get_Z()*facenormal[2] ;
+
+ if (angle > 0){
+ visible=true;
+ }
+ else {
+ visible=false;
+ }
+
+ if(visible)
+ {
+ for(unsigned int i = 0; i < 4; i++)
+ {
+ project(X[i],cMt,X2[i]);
+ pt[i].track(cMt);
+ if(pt[i].get_Z() < 0)
+ needClipping = true;
+ }
+
+ vbase_u = X2[1]-X2[0];
+ vbase_v = X2[3]-X2[0];
+
+ distance = vpColVector::dotProd(normal_Cam,X2[1]);
+
+
+ if(distance < 0)
+ {
+ visible = false;
+ return;
+ }
+
+ for(unsigned int i = 0; i < 3; i++)
+ {
+ normal_Cam_optim[i] = normal_Cam[i];
+ X0_2_optim[i] = X2[0][i];
+ vbase_u_optim[i] = vbase_u[i];
+ vbase_v_optim[i] = vbase_v[i];
+ }
+
+ std::vector<vpPoint> *ptPtr = &pt;
+ if(needClipping){
+ vpPolygon3D::getClippedPolygon(pt,ptClipped,cMt,vpPolygon3D::NEAR_CLIPPING);
+ ptPtr = &ptClipped;
+ }
+
+ listTriangle.clear();
+ for(unsigned int i = 1 ; i < (*ptPtr).size()-1 ; i++){
+ vpImagePoint ip1, ip2, ip3;
+ ip1.set_j((*ptPtr)[0].get_x());
+ ip1.set_i((*ptPtr)[0].get_y());
+
+ ip2.set_j((*ptPtr)[i].get_x());
+ ip2.set_i((*ptPtr)[i].get_y());
+
+ ip3.set_j((*ptPtr)[i+1].get_x());
+ ip3.set_i((*ptPtr)[i+1].get_y());
+
+ vpTriangle tri(ip1,ip2,ip3);
+ listTriangle.push_back(tri);
+ }
+ }
+}
+
+void
+vpImageSimulator::initPlan(vpColVector* X_)
+{
+ for (unsigned int i = 0; i < 4; i++)
+ {
+ X[i]=X_[i];
+ pt[i].setWorldCoordinates(X_[i][0],X_[i][1],X_[i][2]);
+ }
+
+ normal_obj=vpColVector::crossProd(X[1]-X[0],X[3]-X[0]);
+ normal_obj=normal_obj/normal_obj.euclideanNorm();
+
+ euclideanNorm_u=(X[1]-X[0]).euclideanNorm();
+ euclideanNorm_v=(X[3]-X[0]).euclideanNorm();
+}
+
+/*!
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ The table must have a size of 4!
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param I : The image which is projected.
+ \param X_ : table of the 3D coordinates corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const vpImage<unsigned char> &I,vpColVector* X_)
+{
+ Ig = I;
+ vpImageConvert::convert(I,Ic);
+ initPlan(X_);
+}
+
+/*!
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ The table must have a size of 4!
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param I : The image which is projected.
+ \param X_ : table of the 3D coordinates corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const vpImage<vpRGBa> &I,vpColVector* X_)
+{
+ Ic = I;
+ vpImageConvert::convert(I,Ig);
+ initPlan(X_);
+}
+
+#ifdef VISP_HAVE_MODULE_IO
+/*!
+ Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ The table must have a size of 4!
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param file_image : The adress of an image file.
+ \param X_ : table of the 3D coordinates corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const char* file_image,vpColVector* X_)
+{
+ vpImageIo::read(Ig,file_image);
+ vpImageIo::read(Ic,file_image);
+ initPlan(X_);
+}
+#endif
+
+/*!
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ \throw vpException::dimensionError if the _X vector is not of size 4.
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param I : The image which is projected.
+ \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
+ corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint>& X_)
+{
+ if(X_.size() != 4){
+ throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
+ }
+ vpColVector Xvec[4];
+ for(unsigned int i=0; i<4; ++i){
+ Xvec[i].resize(3);
+ Xvec[i][0] = X_[i].get_oX();
+ Xvec[i][1] = X_[i].get_oY();
+ Xvec[i][2] = X_[i].get_oZ();
+ }
+
+ Ig = I;
+ vpImageConvert::convert(I,Ic);
+ initPlan(Xvec);
+}
+/*!
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ \throw vpException::dimensionError if the X_ vector is not of size 4.
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param I : The image which is projected.
+ \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
+ corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X_)
+{
+ if(X_.size() != 4){
+ throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
+ }
+ vpColVector Xvec[4];
+ for(unsigned int i=0; i<4; ++i){
+ Xvec[i].resize(3);
+ Xvec[i][0] = X_[i].get_oX();
+ Xvec[i][1] = X_[i].get_oY();
+ Xvec[i][2] = X_[i].get_oZ();
+ }
+
+ Ic = I;
+ vpImageConvert::convert(I,Ig);
+ initPlan(Xvec);
+}
+
+#ifdef VISP_HAVE_MODULE_IO
+/*!
+ Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
+
+ \throw vpException::dimensionError if the X_ vector is not of size 4.
+
+ - \f$ X[0] \f$ :Top left corner.
+ - \f$ X[1] \f$ :Top right corner.
+ - \f$ X[2] \f$ :Bottom right corner.
+ - \f$ X[3] \f$ :Bottom left corner.
+
+ \param file_image : The adress of an image file.
+ \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
+ corresponding to the image corners.
+*/
+void
+vpImageSimulator::init(const char* file_image, const std::vector<vpPoint>& X_)
+{
+ if(X_.size() != 4){
+ throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
+ }
+ vpColVector Xvec[4];
+ for(unsigned int i=0; i<4; ++i){
+ Xvec[i].resize(3);
+ Xvec[i][0] = X_[i].get_oX();
+ Xvec[i][1] = X_[i].get_oY();
+ Xvec[i][2] = X_[i].get_oZ();
+ }
+
+ vpImageIo::read(Ig,file_image);
+ vpImageIo::read(Ic,file_image);
+ initPlan(Xvec);
+}
+#endif
+
+bool
+vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
+{
+// std::cout << "In get Pixel" << std::endl;
+ //test si pixel dans zone projetee
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
+//// std::cout << "The pixel is inside the projected area" << std::endl;
+// return false;}
+
+ //methoed algebrique
+ double z;
+
+ //calcul de la profondeur de l'intersection
+ z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ //calcul coordonnees 3D intersection
+ Xinter_optim[0]=iP.get_u()*z;
+ Xinter_optim[1]=iP.get_v()*z;
+ Xinter_optim[2]=z;
+
+ //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])
+ //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ double u = 0, v = 0;
+ double diff = 0;
+ for(unsigned int i = 0; i < 3; i++)
+ {
+ diff = (Xinter_optim[i]-X0_2_optim[i]);
+ u += diff*vbase_u_optim[i];
+ v += diff*vbase_v_optim[i];
+ }
+ u = u/(euclideanNorm_u*euclideanNorm_u);
+ v = v/(euclideanNorm_v*euclideanNorm_v);
+
+ if( u > 0 && v > 0 && u < 1. && v < 1.)
+ {
+ double i2,j2;
+ i2=v*(Ig.getHeight()-1);
+ j2=u*(Ig.getWidth()-1);
+ if (interp == BILINEAR_INTERPOLATION)
+ Ipixelplan = Ig.getValue(i2,j2);
+ else if (interp == SIMPLE)
+ Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
+ return true;
+ }
+ else
+ return false;
+}
+
+bool
+vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc,
+ const vpImagePoint &iP, unsigned char &Ipixelplan)
+{
+ //test si pixel dans zone projetee
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+// return false;
+
+ //methoed algebrique
+ double z;
+
+ //calcul de la profondeur de l'intersection
+ z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ //calcul coordonnees 3D intersection
+ Xinter_optim[0]=iP.get_u()*z;
+ Xinter_optim[1]=iP.get_v()*z;
+ Xinter_optim[2]=z;
+
+ //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])
+ //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ double u = 0, v = 0;
+ double diff = 0;
+ for(unsigned int i = 0; i < 3; i++)
+ {
+ diff = (Xinter_optim[i]-X0_2_optim[i]);
+ u += diff*vbase_u_optim[i];
+ v += diff*vbase_v_optim[i];
+ }
+ u = u/(euclideanNorm_u*euclideanNorm_u);
+ v = v/(euclideanNorm_v*euclideanNorm_v);
+
+ if( u > 0 && v > 0 && u < 1. && v < 1.)
+ {
+ double i2,j2;
+ i2=v*(Isrc.getHeight()-1);
+ j2=u*(Isrc.getWidth()-1);
+ if (interp == BILINEAR_INTERPOLATION)
+ Ipixelplan = Isrc.getValue(i2,j2);
+ else if (interp == SIMPLE)
+ Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
+ return true;
+ }
+ else
+ return false;
+}
+
+
+bool
+vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
+{
+ //test si pixel dans zone projetee
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+// return false;
+
+ //methoed algebrique
+ double z;
+
+ //calcul de la profondeur de l'intersection
+ z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ //calcul coordonnees 3D intersection
+ Xinter_optim[0]=iP.get_u()*z;
+ Xinter_optim[1]=iP.get_v()*z;
+ Xinter_optim[2]=z;
+
+ //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])
+ //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ double u = 0, v = 0;
+ double diff = 0;
+ for(unsigned int i = 0; i < 3; i++)
+ {
+ diff = (Xinter_optim[i]-X0_2_optim[i]);
+ u += diff*vbase_u_optim[i];
+ v += diff*vbase_v_optim[i];
+ }
+ u = u/(euclideanNorm_u*euclideanNorm_u);
+ v = v/(euclideanNorm_v*euclideanNorm_v);
+
+ if( u > 0 && v > 0 && u < 1. && v < 1.)
+ {
+ double i2,j2;
+ i2=v*(Ic.getHeight()-1);
+ j2=u*(Ic.getWidth()-1);
+ if (interp == BILINEAR_INTERPOLATION)
+ Ipixelplan = Ic.getValue(i2,j2);
+ else if (interp == SIMPLE)
+ Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
+ return true;
+ }
+ else
+ return false;
+}
+
+bool
+vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
+ vpRGBa &Ipixelplan)
+{
+ //test si pixel dans zone projetee
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+// return false;
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+
+ //methoed algebrique
+ double z;
+
+ //calcul de la profondeur de l'intersection
+ z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ //calcul coordonnees 3D intersection
+ Xinter_optim[0]=iP.get_u()*z;
+ Xinter_optim[1]=iP.get_v()*z;
+ Xinter_optim[2]=z;
+
+ //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])
+ //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ double u = 0, v = 0;
+ double diff = 0;
+ for(unsigned int i = 0; i < 3; i++)
+ {
+ diff = (Xinter_optim[i]-X0_2_optim[i]);
+ u += diff*vbase_u_optim[i];
+ v += diff*vbase_v_optim[i];
+ }
+ u = u/(euclideanNorm_u*euclideanNorm_u);
+ v = v/(euclideanNorm_v*euclideanNorm_v);
+
+ if( u > 0 && v > 0 && u < 1. && v < 1.)
+ {
+ double i2,j2;
+ i2=v*(Isrc.getHeight()-1);
+ j2=u*(Isrc.getWidth()-1);
+ if (interp == BILINEAR_INTERPOLATION)
+ Ipixelplan = Isrc.getValue(i2,j2);
+ else if (interp == SIMPLE)
+ Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
+ return true;
+ }
+ else
+ return false;
+}
+
+bool
+vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan)
+{
+ //test si pixel dans zone projetee
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+// return false;
+
+ Zpixelplan = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ return true;
+}
+
+bool
+vpImageSimulator::getPixelVisibility(const vpImagePoint &iP,
+ double &Visipixelplan)
+{
+ //test si pixel dans zone projetee
+ bool inside = false;
+ for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
+ if(listTriangle[i].inTriangle(iP)){
+ inside = true;
+ break;
+ }
+ if(!inside) return false;
+// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+// return false;
+
+ Visipixelplan = visible_result;
+ return true;
+}
+
+void
+vpImageSimulator::project(const vpColVector &_vin,
+ const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
+{
+ vpColVector XH(4);
+ getHomogCoord(_vin,XH);
+ getCoordFromHomog(_cMt*XH,_vout);
+}
+
+void
+vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
+{
+ for(unsigned int i=0;i<3;i++)
+ _vH[i]=_v[i];
+ _vH[3]=1.;
+}
+
+void
+vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, vpColVector &_v)
+{
+ for(unsigned int i=0;i<3;i++)
+ _v[i]=_vH[i]/_vH[3];
+}
+
+
+void
+vpImageSimulator::getRoi(const unsigned int &Iwidth,
+ const unsigned int &Iheight,
+ const vpCameraParameters &cam,
+ const std::vector<vpPoint> &point,
+ vpRect &rectangle)
+{
+ double top = Iheight+1;
+ double bottom = -1;
+ double right = -1;
+ double left= Iwidth+1;
+
+ for( unsigned int i = 0; i < point.size(); i++)
+ {
+ double u=0,v=0;
+ vpMeterPixelConversion::convertPoint(cam,point[i].get_x(),point[i].get_y(),u,v);
+ if (v < top) top = v;
+ if (v > bottom) bottom = v;
+ if (u < left) left = u;
+ if (u > right) right = u;
+ }
+ if (top < 0) top = 0;
+ if(top >= Iheight) top = Iheight-1;
+ if (bottom < 0) bottom = 0;
+ if(bottom >= Iheight) bottom = Iheight-1;
+ if(left < 0) left = 0;
+ if(left >= Iwidth) left = Iwidth-1;
+ if(right < 0) right = 0;
+ if(right >= Iwidth) right = Iwidth-1;
+
+ rectangle.setTop(top);
+ rectangle.setBottom(bottom);
+ rectangle.setLeft(left);
+ rectangle.setRight(right);
+}
+
+std::vector<vpColVector>
+vpImageSimulator::get3DcornersTextureRectangle()
+{
+ std::vector<vpColVector> X_;
+ for (int i=0; i<4; i++)
+ X_.push_back(X[i]);
+ return X_;
+}
+
+VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/)
+{
+ os << "";
+ return os;
+}
diff --git a/modules/robot/src/light/vpRingLight.cpp b/modules/robot/src/light/vpRingLight.cpp
new file mode 100644
index 0000000..da99b92
--- /dev/null
+++ b/modules/robot/src/light/vpRingLight.cpp
@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Ring light management.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_IO) && defined(VISP_HAVE_PARPORT)
+
+# include <sys/types.h>
+# include <sys/stat.h>
+# include <fcntl.h>
+# include <sys/ioctl.h>
+# include <sys/time.h>
+# include <unistd.h>
+
+# include <visp3/robot/vpRingLight.h>
+# include <visp3/core/vpDebug.h>
+# include <visp3/core/vpTime.h>
+
+/*!
+ \file vpRingLight.cpp
+ \brief Ring light management under unix.
+*/
+
+
+/*!
+
+ Constructor to acces to the ring light device connected to the parallel port.
+
+ Open and initialise the default parallel port device "/dev/parport0" to
+ communicate with the ring light.
+
+ \exception vpParallelPortException::opening If the device used to access to
+ the parallel port can't be opened. A possible reason is that you don't have
+ write access.
+
+ Turn the ring light off.
+*/
+vpRingLight::vpRingLight() : parport()
+{
+ off();
+}
+
+/*!
+ Destructor to close the device.
+
+ Turn the ring light off.
+
+ \exception vpParallelPortException::closing If the device used to access to
+ the parallel port can't be closed.
+*/
+vpRingLight::~vpRingLight()
+{
+ off();
+}
+
+/*!
+ Activates the ring light by sending a pulse throw the parallel port.
+
+ The pulse width is 500 us. This pulse activates a NE555 which turns on on the
+ light during 10ms.
+
+*/
+void vpRingLight::pulse()
+{
+ // Data set by the parallel port:
+ // - D1: need to send a 500us pulse width
+ // - D2: 0 }
+ // - D3: 0 } To control the light throw the NE555
+ // D2 and D3 are used to select the multiplexer output.
+ // Light must be connected to output 1+,1-
+
+ // To activates the light we send a pulse
+ int mask_mode_pulse_d2 = 0x00; // D2 is low
+ int mask_pulse_d1 = 0x02; // we send a pulse on D1 : L, H, L
+ unsigned char data = 0x00;
+ // data = parport.getData(); // actual value of the data bus
+ // vpTRACE("Actual data 0x%x = %d\n", data, data);
+
+ data = data | mask_pulse_d1 | mask_mode_pulse_d2;
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data); // send a 0-1 pulse
+
+ // Wait 500 micro seconds
+ struct timeval ti, tc; // Initial and current time
+ struct timeval tempo;
+ tempo.tv_usec = 500;
+ gettimeofday(&ti,0L);
+ do {
+ gettimeofday(&tc,0L);
+ } while (tc.tv_usec < ti.tv_usec + tempo.tv_usec);
+
+ data = data & (~mask_pulse_d1);
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data); // send a 1-0 pulse
+}
+
+/*!
+
+ Activates the ring light by sending a pulse throw the parallel port during a
+ specified duration.
+
+ \param time : Duration in milli-second (ms) during while the light is turned
+ on.
+
+*/
+void vpRingLight::pulse(double time)
+{
+ // Data set by the parallel port:
+ // - D1: a pulse with duration fixed by time
+ // - D2: 0 }
+ // - D3: 1 } To control the light directly throw the pulse comming from D1
+ // D2 and D3 are used to select the multiplexer output.
+ // Light must be connected to output 1+,1-
+
+ // To activates the light we send a pulse
+ int mask_mode_pulse_d3 = 0x08; // D3 is hight, D2 is low
+ int mask_pulse_d1 = 0x02; // we send a pulse on D1 : L, H, L
+ unsigned char data = 0x00;
+ // data = parport.getData(); // actual value of the data bus
+ // vpTRACE("Actual data 0x%x = %d\n", data, data);
+
+ data = data | mask_pulse_d1 | mask_mode_pulse_d3;
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data); // send a 0-1 pulse
+
+ // Wait 500 micro seconds
+ struct timeval ti, tc; // Initial and current time
+ gettimeofday(&ti,0);
+ do {
+ gettimeofday(&tc,0);
+ } while (tc.tv_usec < ti.tv_usec + time*1000);
+
+ data = data & (~mask_pulse_d1);
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data); // send a 1-0 pulse
+}
+
+/*!
+ Turn the ring light on.
+
+ To turn the ring light off, see off().
+
+*/
+void vpRingLight::on()
+{
+ // Data set by the parallel port:
+ // - D1: 0 to turn OFF, 1 to turn ON
+ // - D2: 1 }
+ // - D3: 0 } To control the light throw D1
+ // D2 and D3 are used to select the multiplexer output.
+ // Light must be connected to output 1+,1-
+
+ // To activates the light we send a pulse
+ int mask_mode_onoff_d2 = 0x04; // D2 is Hight
+ int mask_on_d1 = 0x02; // D1 is Hight to turn the light on
+ unsigned char data = 0x00;
+ //data = parport.getData(); // actual value of the data bus
+
+ data = data | mask_on_d1 | mask_mode_onoff_d2;
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data);
+}
+
+/*!
+ Turn the ring light off.
+
+ To turn the ring light on, see on().
+
+*/
+void vpRingLight::off()
+{
+ // Data set by the parallel port:
+ // - D1: 0 to turn OFF, 1 to turn ON
+ // - D2: 1 }
+ // - D3: 0 } To control the light throw D1
+ // D2 and D3 are used to select the multiplexer output.
+ // Light must be connected to output 1+,1-
+
+ // To activates the light we send a pulse
+ int mask_mode_onoff_d2 = 0x04; // D2 is Hight
+ int mask_off_d1 = 0x00; // D1 is Low to turn the light off
+ unsigned char data = 0x00;
+ //data = parport.getData(); // actual value of the data bus
+
+ data = data | mask_off_d1 | mask_mode_onoff_d2;
+ //vpTRACE("Send 0x%x = %d\n", data, data);
+ parport.sendData(data);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRingLight.cpp.o) has no symbols
+void dummy_vpRingLight() {};
+#endif
diff --git a/modules/robot/src/real-robot/afma4/vpAfma4.cpp b/modules/robot/src/real-robot/afma4/vpAfma4.cpp
new file mode 100644
index 0000000..8555751
--- /dev/null
+++ b/modules/robot/src/real-robot/afma4/vpAfma4.cpp
@@ -0,0 +1,646 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpAfma4.cpp
+
+ Control of Irisa's cylindrical robot named Afma4.
+
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/robot/vpAfma4.h>
+
+
+/* ----------------------------------------------------------------------- */
+/* --- STATIC ------------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+const unsigned int vpAfma4::njoint = 4;
+
+
+/*!
+
+ Default constructor.
+
+*/
+vpAfma4::vpAfma4()
+ : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
+{
+ // Set the default parameters in case of the config files on the NAS
+ // at Inria are not available.
+
+ //
+ // Geometric model constant parameters
+ //
+ this->_a1 = 0.205; // distance along x2
+ this->_d3 = 0.403; // distance along z2
+ this->_d4 = 0.14; // distance along z3
+
+ // Maximal value of the joints
+ this->_joint_max[0] = 1.8; // rad
+ this->_joint_max[1] = 0.9; // meter
+ this->_joint_max[2] = 0.9; // rad
+ this->_joint_max[3] = 0.76; // rad
+ // Minimal value of the joints
+ this->_joint_min[0] = -1.5; // rad
+ this->_joint_min[1] = -0.9; // meter
+ this->_joint_min[2] = -3.5; // rad
+ this->_joint_min[3] = -0.76;// rad
+
+ // Camera extrinsic parameters: effector to camera frame
+ this->_etc[0] = 0.; // Translation
+ this->_etc[1] = 0.;
+ this->_etc[2] = 0.;
+ this->_erc[0] = 0.; // Rotation
+ this->_erc[1] = -M_PI/2.;
+ this->_erc[2] = 0;
+
+ vpRotationMatrix eRc(_erc);
+ this->_eMc.buildFrom(_etc, eRc);
+
+ init();
+}
+
+
+/*!
+
+ Does nothing for the moment.
+ */
+void
+vpAfma4::init (void)
+{
+ return;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the four joints.
+
+ This method is the same than get_fMc(const vpColVector & q).
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \return The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ camera frame (\f${^f}M_c\f$) with:
+ \f[
+ {^f}M_c = {^f}M_e * {^e}M_c
+ \f]
+
+ \sa get_fMc(const vpColVector & q)
+ \sa getInverseKinematics()
+
+*/
+vpHomogeneousMatrix
+vpAfma4::getForwardKinematics(const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ fMc = get_fMc(q);
+
+ return fMc;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the four joints.
+
+ This method is the same than getForwardKinematics(const vpColVector & q).
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \return The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ camera frame (\f${^f}M_c\f$) with:
+ \f[
+ {^f}M_c = {^f}M_e * {^e}M_c
+ \f]
+
+ \sa getForwardKinematics(const vpColVector & q)
+*/
+vpHomogeneousMatrix
+vpAfma4::get_fMc (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ get_fMc(q, fMc);
+
+ return fMc;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the four joints.
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param fMc : The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ camera frame (\f${^f}M_c\f$) with:
+ \f[
+ {^f}M_c = {^f}M_e * {^e}M_c
+ \f]
+
+*/
+void
+vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+{
+
+ // Compute the direct geometric model: fMe = transformation between
+ // fix and end effector frame.
+ vpHomogeneousMatrix fMe;
+
+ get_fMe(q, fMe);
+
+ fMc = fMe * this->_eMc;
+
+ return;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the end effector with respect to the base frame given the
+ articular positions of all the four variable joints.
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param fMe The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ end effector frame (\f${^f}M_e\f$) with
+
+ \f[
+ {^f}M_e = \left[\begin{array}{cccc}
+ c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
+ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
+ -s_5 & -c_5 & d_4+q_2 \\
+ 0 & 0 & 0 & 1 \\
+ \end{array}
+ \right]
+ \f]
+
+*/
+void
+vpAfma4::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+{
+ double q1 = q[0]; // rot touret
+ double q2 = q[1]; // vertical translation
+ double q4 = q[2]; // pan
+ double q5 = q[3]; // tilt
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+
+ /* Calcul du modele d'apres les angles. */
+ fMe[0][0] = c1*s4*c5 + s1*c4*c5;
+ fMe[0][1] = -c1*s4*s5 - s1*c4*s5;
+ fMe[0][2] = c1*c4 - s1*s4;
+ fMe[0][3] = c1*this->_a1 - s1*(this->_d3);
+
+ fMe[1][0] = s1*s4*c5 - c1*c4*c5;
+ fMe[1][1] = -s1*s4*s5 + c1*c4*s5;
+ fMe[1][2] = s1*c4+c1*s4;
+ fMe[1][3] = s1*this->_a1 + c1*(this->_d3);
+
+ fMe[2][0] = -s5;
+ fMe[2][1] = -c5;
+ fMe[2][2] = 0.f;
+ fMe[2][3] = this->_d4 + q2;
+
+ fMe[3][0] = 0.f;
+ fMe[3][1] = 0.f;
+ fMe[3][2] = 0.f;
+ fMe[3][3] = 1;
+
+ // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
+
+ return;
+}
+
+/*!
+
+ Get the geometric transformation between the camera frame and the
+ end-effector frame. This transformation is constant and correspond
+ to the extrinsic camera parameters estimated by hand or by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ cMe = this->_eMc.inverse();
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+vpAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+
+ return;
+}
+
+/*!
+
+ Get the twist transformation from camera frame to the reference
+ frame. This transformation allows to compute a velocity expressed
+ in the reference frame into the camera frame.
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param cVf : Twist transformation.
+
+*/
+void
+vpAfma4::get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const
+{
+ vpHomogeneousMatrix fMc, cMf ;
+ get_fMc(q, fMc) ;
+ cMf = fMc.inverse();
+
+ cVf.buildFrom(cMf) ;
+
+ return;
+}
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame:
+
+ \f[
+ {^e}J_e = \left[\begin{array}{cccc}
+ -c_5(a_1c_4+d_3s_4) & -s_5 & 0 & 0 \\
+ s_5(a_1c_4+d_3s_4) & -c_5 & 0 & 0 \\
+ a_1s_4-d_3c_4 & 0 & 0 & 0 \\
+ -s_5 & 0 & -s_5 & 0 \\
+ -c_5 & 0 & -c_5 & 0 \\
+ 0 & 0 & 0 & 1 \\
+ \end{array}
+ \right]
+ \f]
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param eJe : Robot jacobian expressed in the end-effector frame, with:
+ \f[
+ {^e}J_e = \left[\begin{array}{cc}
+ {^f}R_e^T & 0_{3 \times 3} \\
+ 0_{3 \times 3} & {^f}R_e^T \\
+ \end{array}
+ \right] {^f}J_e
+ \f]
+
+ \sa get_fJe()
+*/
+void
+vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+{
+ double q4 = q[2]; // pan
+ double q5 = q[3]; // tilt
+
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+
+ eJe.resize(6, 4);
+
+ eJe = 0;
+
+ eJe[0][0] = -(this->_a1*c4 + this->_d3*s4)*c5; eJe[0][1] = -s5;
+ eJe[1][0] = (this->_a1*c4 + this->_d3*s4)*s5; eJe[1][1] = -c5;
+ eJe[2][0] = (this->_a1*s4 - this->_d3*c4);
+ eJe[3][0] = eJe[3][2] = -s5;
+ eJe[4][0] = eJe[4][2] = -c5;
+ eJe[5][3] = 1.;
+}
+
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame:
+
+ \f[
+ {^f}J_e = \left[\begin{array}{cccc}
+ -a_1s_1-d_3c_1 & 0 & 0 & 0 \\
+ a_1c_1-d_3s_1 & 0 & 0 & 0 \\
+ 0 & 1 & 0 & 0 \\
+ 0 & 0 & 0 & c_{14} \\
+ 0 & 0 & 0 & s_{14} \\
+ 1 & 0 & 1 & 0 \\
+ \end{array}
+ \right]
+ \f]
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param fJe : Robot jacobian expressed in the robot reference frame.
+
+ \sa get_eJe() and get_fJe_inverse()
+*/
+
+void
+vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+{
+ fJe.resize(6,4) ;
+
+ double q1 = q[0]; // rot touret
+ double q4 = q[2]; // pan
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c14 = cos(q1 + q4);
+ double s14 = sin(q1 + q4);
+
+ fJe = 0;
+
+ fJe[0][0] = -s1*this->_a1 - c1*this->_d3;
+
+ fJe[1][0] = c1*this->_a1 - s1*this->_d3;
+
+ fJe[2][1] = 1.0;
+
+ fJe[3][3] = c14;
+
+ fJe[4][3] = s14;
+
+ fJe[5][0] = fJe[5][2] = 1.0;
+}
+
+/*!
+
+ Get the inverse jacobian.
+
+ \f[
+ {^f}J_e^+ = \left[\begin{array}{cccccc}
+ -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\
+ 0 & 0 & 1 & 0 & 0 & 0 \\
+ (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\
+ 0 & 0 & 0 & c_{14} & s_{14} & 0 \\
+ \end{array}
+ \right]
+ \f]
+
+ \param q : Articular position of the four joints: q[0] corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q[1] corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
+ q[3] are expressed in radians. The translation q[1] is expressed in
+ meters.
+
+ \param fJe_inverse : Inverse robot jacobian expressed in the robot
+ reference frame.
+
+ \sa get_eJe() and get_fJe()
+
+*/
+void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
+{
+ fJe_inverse.resize(4, 6) ;
+ fJe_inverse = 0;
+
+ double q1 = q[0]; // rot touret
+ double q4 = q[2]; // pan
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c14 = cos(q1 + q4);
+ double s14 = sin(q1 + q4);
+
+ double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
+
+ fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
+ fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;
+
+ fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
+
+ fJe_inverse[2][0] = - fJe_inverse[0][0];
+ fJe_inverse[2][1] = - fJe_inverse[0][1];
+
+ fJe_inverse[3][3] = c14;
+ fJe_inverse[3][4] = s14;
+}
+
+/*!
+ Get min joint values.
+
+ \return Minimal joint values for the 4 dof
+ X, Y, A, B. Translation Y is expressed in meters. Rotations
+ X,A and B in radians.
+
+*/
+vpColVector
+vpAfma4::getJointMin() const
+{
+ vpColVector qmin(4);
+ for (unsigned int i=0; i < 4; i ++)
+ qmin[i] = this->_joint_min[i];
+ return qmin;
+}
+
+/*!
+ Get max joint values.
+
+ \return Maximal joint values for the 4 dof
+ X, Y, A, B. Translation Y is expressed in meters. Rotations
+ X, A and B in radians.
+
+*/
+vpColVector
+vpAfma4::getJointMax() const
+{
+ vpColVector qmax(4);
+ for (unsigned int i=0; i < 4; i ++)
+ qmax[i] = this->_joint_max[i];
+ return qmax;
+}
+
+
+
+/*!
+
+ Print on the output stream \e os the robot parameters (joint
+ min/max, distance between axis 5 and 6, coupling factor between axis
+ 5 and 6, hand-to-eye homogeneous matrix.
+
+ \param os : Output stream.
+ \param afma4 : Robot parameters.
+*/
+VISP_EXPORT std::ostream & operator << (std::ostream & os,
+ const vpAfma4 & afma4)
+{
+ vpRotationMatrix eRc;
+ afma4._eMc.extract(eRc);
+ vpRxyzVector rxyz(eRc);
+
+ os
+ << "Joint Max:" << std::endl
+ << "\t" << afma4._joint_max[0]
+ << "\t" << afma4._joint_max[1]
+ << "\t" << afma4._joint_max[2]
+ << "\t" << afma4._joint_max[3]
+ << "\t" << std::endl
+
+ << "Joint Min: " << std::endl
+ << "\t" << afma4._joint_min[0]
+ << "\t" << afma4._joint_min[1]
+ << "\t" << afma4._joint_min[2]
+ << "\t" << afma4._joint_min[3]
+ << "\t" << std::endl
+
+ << "a1: " << std::endl
+ << "\t" << afma4._a1
+ << "\t" << std::endl
+
+ << "d3: " << std::endl
+ << "\t" << afma4._d3
+ << "\t" << std::endl
+
+ << "d4: " << std::endl
+ << "\t" << afma4._d4
+ << "\t" << std::endl
+
+ << "eMc: "<< std::endl
+ << "\tTranslation (m): "
+ << afma4._eMc[0][3] << " "
+ << afma4._eMc[1][3] << " "
+ << afma4._eMc[2][3]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (rad) : "
+ << rxyz[0] << " "
+ << rxyz[1] << " "
+ << rxyz[2]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : "
+ << vpMath::deg(rxyz[0]) << " "
+ << vpMath::deg(rxyz[1]) << " "
+ << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
+
+ return os;
+}
diff --git a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp
new file mode 100644
index 0000000..35c7ac9
--- /dev/null
+++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp
@@ -0,0 +1,1860 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_AFMA4
+
+#include <signal.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/robot/vpRobotAfma4.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ----------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+bool vpRobotAfma4::robotAlreadyCreated = false;
+
+/*!
+
+ Default positioning velocity in percentage of the maximum
+ velocity. This value is set to 15. The member function
+ setPositioningVelocity() allows to change this value.
+
+*/
+const double vpRobotAfma4::defaultPositioningVelocity = 15.0;
+
+
+/* ---------------------------------------------------------------------- */
+/* --- EMERGENCY STOP --------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Emergency stops the robot if the program is interrupted by a SIGINT
+ (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
+ or SIGQUIT signal.
+
+*/
+void emergencyStopAfma4(int signo)
+{
+ std::cout << "Stop the Afma4 application by signal ("
+ << signo << "): " << (char)7 ;
+ switch(signo)
+ {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ case SIGBUS:
+ std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ case SIGSEGV:
+ std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ case SIGKILL:
+ std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ case SIGQUIT:
+ std::cout <<"SIGQUIT " << std::endl ; break ;
+ default :
+ std::cout << signo << std::endl ;
+ }
+ // std::cout << "Emergency stop called\n";
+ // PrimitiveESTOP_Afma4();
+ PrimitiveSTOP_Afma4();
+ std::cout << "Robot was stopped\n";
+
+ // Free allocated resources
+ // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
+
+ fprintf(stdout, "Application ");
+ fflush(stdout);
+ kill(getpid(), SIGKILL);
+ exit(1) ;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ The only available constructor.
+
+ This contructor calls init() to initialise the connection with the MotionBox
+ or low level controller, power on the robot and wait 1 sec before returning
+ to be sure the initialisation is done.
+
+ It also set the robot state to vpRobot::STATE_STOP.
+
+*/
+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
+ */
+
+ signal(SIGINT, emergencyStopAfma4);
+ signal(SIGBUS, emergencyStopAfma4) ;
+ signal(SIGSEGV, emergencyStopAfma4) ;
+ signal(SIGKILL, emergencyStopAfma4);
+ signal(SIGQUIT, emergencyStopAfma4);
+
+ setVerbose(verbose);
+ if (verbose_)
+ std::cout << "Open communication with MotionBlox.\n";
+ try {
+ this->init();
+ this->setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...) {
+ // vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ positioningVelocity = defaultPositioningVelocity ;
+
+ vpRobotAfma4::robotAlreadyCreated = true;
+
+ return ;
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INITIALISATION ----------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Initialise the connection with the MotionBox or low level
+ controller, power on the
+ robot and wait 1 sec before returning to be sure the initialisation
+ is done.
+
+*/
+void
+vpRobotAfma4::init (void)
+{
+ int stt;
+ InitTry;
+
+ // Initialise private variables used to compute the measured velocities
+ q_prev_getvel.resize(4);
+ q_prev_getvel = 0;
+ time_prev_getvel = 0;
+ first_time_getvel = true;
+
+ // Initialise private variables used to compute the measured displacement
+ q_prev_getdis.resize(4);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+ // Initialize the firewire connection
+ Try( stt = InitializeConnection(verbose_) );
+
+ if (stt != SUCCESS) {
+ vpERROR_TRACE ("Cannot open connexion with the motionblox.");
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+
+ // Connect to the servoboard using the servo board GUID
+ Try( stt = InitializeNode_Afma4() );
+
+ if (stt != SUCCESS) {
+ vpERROR_TRACE ("Cannot open connexion with the motionblox.");
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+ Try( PrimitiveRESET_Afma4() );
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // Print the robot status
+ if (verbose_) {
+ std::cout << "Robot status: ";
+ switch(EStopStatus) {
+ case ESTOP_AUTO:
+ case ESTOP_MANUAL:
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+ case ESTOP_ACTIVATED:
+ std::cout << "Emergency stop is activated" << std::endl;
+ break;
+ default:
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ }
+ std::cout << std::endl;
+ }
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max) );
+// for (unsigned int i=0; i < njoint; i++) {
+// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
+// }
+
+ // If an error occur in the low level controller, goto here
+ // CatchPrint();
+ Catch();
+
+ // Test if an error occurs
+ if (TryStt == -20001)
+ printf("No connection detected. Check if the robot is powered on \n"
+ "and if the firewire link exist between the MotionBlox and this computer.\n");
+ else if (TryStt == -675)
+ printf(" Timeout enabling power...\n");
+
+ if (TryStt < 0) {
+ // Power off the robot
+ PrimitivePOWEROFF_Afma4();
+ // Free allocated resources
+ ShutDownConnection();
+
+ std::cout << "Cannot open connexion with the motionblox..." << std::endl;
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+ return ;
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- DESTRUCTOR --------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Destructor.
+
+ Free allocated resources.
+*/
+vpRobotAfma4::~vpRobotAfma4 (void)
+{
+ InitTry;
+
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+// if (HIPowerStatus == 1) {
+// fprintf(stdout, "Power OFF the robot\n");
+// fflush(stdout);
+
+// Try( PrimitivePOWEROFF_Afma4() );
+// }
+
+ // Free allocated resources
+ ShutDownConnection();
+
+ vpRobotAfma4::robotAlreadyCreated = false;
+
+ CatchPrint();
+ return;
+}
+
+
+
+
+
+/*!
+
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ InitTry;
+
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ if (vpRobot::STATE_STOP != getRobotState ()) {
+ Try( PrimitiveSTOP_Afma4() );
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try( PrimitiveSTOP_Afma4() );
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ default:
+ break ;
+ }
+
+ CatchPrint();
+
+ return vpRobot::setRobotState (newState);
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- STOP --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Stop the robot and set the robot state to vpRobot::STATE_STOP.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+*/
+void
+vpRobotAfma4::stopMotion(void)
+{
+ InitTry;
+ Try( PrimitiveSTOP_Afma4() );
+ setRobotState (vpRobot::STATE_STOP);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot stop robot motion");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot stop robot motion.");
+ }
+}
+
+
+/*!
+
+ Power on the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot power on.
+
+ \sa powerOff(), getPowerState()
+*/
+void
+vpRobotAfma4::powerOn(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ bool firsttime = true;
+ unsigned int nitermax = 10;
+
+ for (unsigned int i=0; i<nitermax; i++) {
+ Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ if (EStopStatus == ESTOP_AUTO) {
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_MANUAL) {
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_ACTIVATED) {
+ if (firsttime) {
+ std::cout << "Emergency stop is activated! \n"
+ << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ firsttime = false;
+ }
+ fprintf(stdout, "Remaining time %ds \r", nitermax-i);
+ fflush(stdout);
+ CAL_Wait(1);
+ }
+ else {
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ ShutDownConnection();
+ exit(0);
+ }
+ }
+
+ if (EStopStatus == ESTOP_ACTIVATED)
+ std::cout << std::endl;
+
+ if (EStopStatus == ESTOP_ACTIVATED) {
+ std::cout << "Sorry, cannot power on the robot." << std::endl;
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power on the robot.");
+ }
+
+ if (HIPowerStatus == 0) {
+ fprintf(stdout, "Power ON the Afma4 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWERON_Afma4() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power on the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Power off the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+
+ \sa powerOn(), getPowerState()
+*/
+void
+vpRobotAfma4::powerOff(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ fprintf(stdout, "Power OFF the Afma4 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWEROFF_Afma4() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power off the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Get the robot power state indication if power is on or off.
+
+ \return true if power is on. false if power is off
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error.
+
+ \sa powerOn(), powerOff()
+*/
+bool
+vpRobotAfma4::getPowerState(void)
+{
+ InitTry;
+ bool status = false;
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ status = true;
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the power status");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get the power status.");
+ }
+ return status;
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ vpAfma4::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the twist transformation from camera frame to the reference
+ frame. This transformation allows to compute a velocity expressed
+ in the reference frame into the camera frame.
+
+ \param cVf : Twist transformation.
+
+*/
+void
+vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const
+{
+ double position[this->njoint];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(this->njoint);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try {
+ vpAfma4::get_cVf(q, cVf) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }}
+
+/*!
+
+ Get the geometric transformation between the camera frame and the
+ end-effector frame. This transformation is constant and correspond
+ to the extrinsic camera parameters estimated by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpAfma4::get_cMe(cMe) ;
+}
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame. To have
+ acces to the analytic form of this jacobian see vpAfma4::get_eJe().
+
+ To compute eJe, we communicate with the low level controller to get
+ the articular joint position of the robot.
+
+ \param eJe : Robot jacobian expressed in the end-effector frame.
+
+ \sa vpAfma4::get_eJe()
+*/
+void
+vpRobotAfma4::get_eJe(vpMatrix &eJe)
+{
+
+ double position[this->njoint];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(this->njoint);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try {
+ vpAfma4::get_eJe(q, eJe) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame. To have acces to the analytic form of this
+ jacobian see vpAfma4::get_fJe().
+
+ To compute fJe, we communicate with the low level controller to get
+ the articular joint position of the robot.
+
+ \param fJe : Robot jacobian expressed in the reference frame.
+
+ \sa vpAfma4::get_fJe()
+*/
+
+void vpRobotAfma4::get_fJe(vpMatrix &fJe)
+{
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try {
+ vpAfma4::get_fJe(q, fJe) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+/*!
+
+ Set the maximal velocity percentage to use for a position control.
+
+ The default positioning velocity is defined by
+ vpRobotAfma4::defaultPositioningVelocity. This method allows to
+ change this default positioning velocity
+
+ \param velocity : Percentage of the maximal velocity. Values should
+ be in ]0:100].
+
+ \code
+ vpColVector q[4]);
+ q = 0; // position in meter and rad
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot to the joint position [0,0,0,0]
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ \sa getPositioningVelocity()
+*/
+void
+vpRobotAfma4::setPositioningVelocity (const double velocity)
+{
+ positioningVelocity = velocity;
+}
+
+/*!
+ Get the maximal velocity percentage used for a position control.
+
+ \sa setPositioningVelocity()
+*/
+double
+vpRobotAfma4::getPositioningVelocity (void)
+{
+ return positioningVelocity;
+}
+
+
+/*!
+
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+
+ \warning The position to reach can only be specified in joint space
+ coordinates.
+
+ This method owerloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param position : The joint positions to reach. position[0]
+ corresponds to the first rotation of the turret around the vertical
+ axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
+ to the vertical translation (joint 2 with value \f$q_2\f$), while
+ position[2] and position[3] correspond to the pan and tilt of the
+ camera (respectively joint 4 and 5 with values \f$q_4\f$ and
+ \f$q_5\f$). Rotations position[0], position[2] and position[3] are
+ expressed in radians. The translation q[1] is expressed in meters.
+
+ \param frame : Frame in which the position is expressed.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+ // Set positions in the joint space
+ vpColVector q[4];
+ double q[0] = M_PI/8; // Joint 1, in radian
+ double q[1] = 0.2; // Joint 2, in meter
+ double q[2] = M_PI/4; // Joint 4, in radian
+ double q[3] = M_PI/8; // Joint 5, in radian
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the requested frame
+ (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, or vpRobot::MIXT_FRAME) are
+ requested since they are not implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, q);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ \endcode
+
+*/
+
+void
+vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & position )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ int error = 0;
+
+ switch(frame) {
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Positionning error. Reference frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Reference frame not implemented.");
+ break ;
+ case vpRobot::CAMERA_FRAME :
+ vpERROR_TRACE ("Positionning error. Camera frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Camera frame not implemented.");
+ break ;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+
+ case vpRobot::ARTICULAR_FRAME: {
+ break ;
+
+ }
+ }
+ if (position.getRows() != this->njoint) {
+ vpERROR_TRACE ("Positionning error: bad vector dimension.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Positionning error: bad vector dimension.");
+ }
+
+ InitTry;
+
+ Try( PrimitiveMOVE_Afma4(position.data, positioningVelocity) );
+ Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
+
+ CatchPrint();
+ if (TryStt == InvalidPosition || TryStt == -1023)
+ std::cout << " : Position out of range.\n";
+ else if (TryStt < 0)
+ std::cout << " : Unknown error (see Fabien).\n";
+ else if (error == -1)
+ std::cout << "Position out of range.\n";
+
+ if (TryStt < 0 || error < 0) {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+
+ return ;
+}
+
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+
+ \warning The position to reach can only be specified in joint space
+ coordinates.
+
+ This method owerloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param q1, q2, q4, q5 : The four joint positions to reach. q1 corresponds to
+ the first rotation (joint 1 with value \f$q_1\f$) of the turret
+ around the vertical axis, while q2 corresponds to the vertical
+ translation (joint 2 with value \f$q_2\f$), while q4 and q5
+ correspond to the pan and tilt of the camera (respectively joint 4
+ and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q1, q4 and
+ q5 are expressed in radians. The translation q2 is expressed in
+ meters.
+
+ \param frame : Frame in which the position is expressed.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+ // Set positions in the camera frame
+ double q1 = M_PI/8; // Joint 1, in radian
+ double q2 = 0.2; // Joint 2, in meter
+ double q4 = M_PI/4; // Joint 4, in radian
+ double q5 = M_PI/8; // Joint 5, in radian
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q1, q2, q4, q5);
+ \endcode
+
+ \sa setPosition()
+*/
+void vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
+ const double q1,
+ const double q2,
+ const double q4,
+ const double q5)
+{
+ try {
+ vpColVector position(this->njoint) ;
+ position[0] = q1 ;
+ position[1] = q2 ;
+ position[2] = q4 ;
+ position[3] = q5 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+
+
+
+/*!
+
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+ vpColVector q;
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL)
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+
+*/
+void vpRobotAfma4::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+ Returns the robot controller current time (in second) since last robot power on.
+*/
+double vpRobotAfma4::getTime() const
+{
+ double timestamp;
+ PrimitiveACQ_TIME_Afma4(×tamp);
+ return timestamp;
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 4 dimension vector corresponding to the joint position of
+ each dof. position[0]
+ corresponds to the first rotation of the turret around the vertical
+ axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
+ to the vertical translation (joint 2 with value \f$q_2\f$), while
+ position[2] and position[3] correspond to the pan and tilt of the
+ camera (respectively joint 4 and 5 with values \f$q_4\f$ and
+ \f$q_5\f$). Rotations position[0], position[2] and position[3] are
+ expressed in radians. The translation q[1] is expressed in meters.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Time in second since last robot power on.
+
+ \code
+ vpRobotAfma4 robot;
+ vpColVector r;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, r);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame pose in terms of a homogeneous matrix
+ vpHomogeneousMatrix fMc(fRc, ftc);
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the position cannot
+ be get from the low level controller.
+
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const
+ vpColVector & r)
+
+*/
+void
+vpRobotAfma4::getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector & position, double ×tamp)
+{
+
+ InitTry;
+
+ position.resize (this->njoint);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME : {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ double _q[njoint];
+ Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
+ for (unsigned int i=0; i < this->njoint; i ++) {
+ position[i] = _q[i];
+ }
+
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ double _q[njoint];
+ Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
+
+ vpColVector q(this->njoint);
+ for (unsigned int i=0; i < this->njoint; i++)
+ q[i] = _q[i];
+
+ // Compute fMc
+ vpHomogeneousMatrix fMc;
+ vpAfma4::get_fMc(q, fMc);
+
+ // From fMc extract the pose
+ vpRotationMatrix fRc;
+ fMc.extract(fRc);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(fRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ position[i] = fMc[i][3]; // translation x,y,z
+ position[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+ break ;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get position.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position.");
+ }
+
+ return;
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in the joint space or in the camera frame. The reference
+ frame and mixt frame are not implemented.
+
+ \param vel : Velocity vector. Translation velocities are expressed in m/s
+ while rotation velocities in rad/s. The size of this vector may be 4 (the
+ number of dof) if frame is set to vpRobot::ARTICULAR_FRAME. The size of this
+ vector is 2 when the control is in the camera frame (frame is than set to
+ vpRobot::CAMERA_FRAME).
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} t_x, ^{c} t_y, ^{c} t_z,^{c}
+ \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t\f$ is expressed in the
+ camera frame. Since only four dof are available, in camera frame we
+ control control only the camera pan and tilt.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(4);
+ q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
+ q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
+ q_dot[3] = M_PI/8; // Joint 5 velocity, in rad/s
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+ \endcode
+*/
+void
+vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & vel)
+{
+
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ // Check the dimension of the velocity vector to see if it is
+ // compatible with the requested frame
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ //if (vel.getRows() != 2) {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE ("Bad dimension of the velocity vector in camera frame");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Bad dimension of the velocity vector "
+ "in camera frame");
+ }
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ if (vel.getRows() != this->njoint) {
+ vpERROR_TRACE ("Bad dimension of the articular velocity vector");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Bad dimension of the articular "
+ "velocity vector ");
+ }
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::MIXT_FRAME : {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the mixt frame:"
+ "functionality not implemented");
+ break ;
+ }
+ default: {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot ");
+ }
+ }
+
+
+ //
+ // Velocities saturation with normalization
+ //
+ vpColVector joint_vel(this->njoint);
+
+ // Case of the camera frame where we control only 2 dof
+ if (frame == vpRobot::CAMERA_FRAME) {
+ vpColVector vel_max(6);
+
+ for (unsigned int i=0; i<3; i++) {
+ vel_max[i] = getMaxTranslationVelocity();
+ vel_max[i+3] = getMaxRotationVelocity();
+ }
+
+ vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
+
+#if 0 // ok
+ vpMatrix eJe(4,6);
+ eJe = 0;
+ eJe[2][4] = -1;
+ eJe[3][3] = 1;
+
+ joint_vel = eJe * velocity; // Compute the articular velocity
+#endif
+ vpColVector q;
+ getPosition(vpRobot::ARTICULAR_FRAME, q);
+ vpMatrix fJe_inverse;
+ get_fJe_inverse(q, fJe_inverse);
+ vpHomogeneousMatrix fMe;
+ get_fMe(q, fMe);
+ vpTranslationVector t;
+ t=0;
+ vpRotationMatrix fRe;
+ fMe.extract(fRe);
+ vpVelocityTwistMatrix fVe(t, fRe);
+ // compute the inverse jacobian in the end-effector frame
+ vpMatrix eJe_inverse = fJe_inverse * fVe;
+
+ // Transform the velocities from camera to end-effector frame
+ vpVelocityTwistMatrix eVc;
+ eVc.buildFrom(this->_eMc);
+ 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]);
+ }
+
+ // Case of the joint control where we control all the joints
+ else if (frame == vpRobot::ARTICULAR_FRAME) {
+
+ vpColVector vel_max(4);
+
+ vel_max[0] = getMaxRotationVelocity();
+ vel_max[1] = getMaxTranslationVelocity();
+ vel_max[2] = getMaxRotationVelocity();
+ vel_max[3] = getMaxRotationVelocity();
+
+ joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
+ }
+
+ InitTry;
+
+ // Send a joint velocity to the low level controller
+ Try( PrimitiveMOVESPEED_Afma4(joint_vel.data) );
+
+ Catch();
+ if (TryStt < 0) {
+ if (TryStt == VelStopOnJoint) {
+ UInt32 axisInJoint[njoint];
+ PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
+ for (unsigned int i=0; i < njoint; i ++) {
+ if (axisInJoint[i])
+ std::cout << "\nWarning: Velocity control stopped: axis "
+ << i+1 << " on joint limit!" <<std::endl;
+ }
+ }
+ else {
+ printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
+ if (TryString != NULL) {
+ // The statement is in TryString, but we need to check the validity
+ printf(" Error sentence %s\n", TryString); // Print the TryString
+ }
+ else {
+ printf("\n");
+ }
+ }
+ }
+
+ return;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- GET ---------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param velocity : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \warning The first time this method is called, \e velocity is set to 0. The
+ first call is used to intialise the velocity computation for the next call.
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(4);
+ q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
+ q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
+ q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ vpColVector q_dot_mes; // Measured velocities
+
+ // Initialisation of the velocity measurement
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 4, the number of joint
+
+ double timestamp;
+ while (1) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
+ }
+ \endcode
+*/
+void
+vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp)
+{
+
+ switch (frame) {
+ case vpRobot::ARTICULAR_FRAME:
+ velocity.resize (this->njoint);
+ default:
+ velocity.resize (6);
+ }
+
+ velocity = 0;
+
+ double q[4];
+ vpColVector q_cur(4);
+ vpHomogeneousMatrix fMc_cur;
+ vpHomogeneousMatrix cMc; // camera displacement
+ double time_cur;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
+ time_cur = timestamp;
+
+ for (unsigned int i=0; i < this->njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ // Get the camera pose from the direct kinematics
+ vpAfma4::get_fMc(q_cur, fMc_cur);
+
+ if ( ! first_time_getvel ) {
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
+
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a displacement in the mixt frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getvel = false;
+ }
+
+ // Memorize the camera pose for the next call
+ fMc_prev_getvel = fMc_cur;
+
+ // Memorize the joint position for the next call
+ q_prev_getvel = q_cur;
+
+ // Memorize the time associated to the joint position for the next call
+ time_prev_getvel = time_cur;
+
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
+ except that the timestamp is not returned.
+
+ */
+void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity)
+{
+ double timestamp;
+ getVelocity(frame, velocity, timestamp);
+}
+
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(4);
+ q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
+ q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
+ q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
+
+ vpRobotAfma4 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 4, the number of joint
+
+ vpColVector q_dot_mes; // Measured velocities
+ double timestamp;
+ while (1) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
+ }
+ \endcode
+*/
+vpColVector
+vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ vpColVector velocity;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
+ except that the timestamp is not returned.
+
+ */
+vpColVector vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector velocity;
+ double timestamp;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+Read joint positions in a specific Afma4 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#AFMA4 - Position - Version 2.01
+# file: "myposition.pos "
+#
+# R: X Y A B
+# Joint position: X : rotation of the turret in degrees (joint 1)
+# Y : vertical translation in meters (joint 2)
+# A : pan rotation of the camera in degrees (joint 4)
+# B : tilt rotation of the camera in degrees (joint 5)
+#
+
+R: 45 0.3 -20 30
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : Joint positions: X,Y,A,B. Translations Y is
+expressed in meters, while joint rotations X,A,B in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+vpRobotAfma4 robot;
+vpColVector q; // Joint position
+robot.readPosFile("myposition.pos", q); // Set the joint position from the file
+robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+\endcode
+
+\sa savePosFile()
+*/
+
+bool
+vpRobotAfma4::readPosFile(const char *filename, vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+// else
+// return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ return (false); /* fin fichier */
+ }
+
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(4);
+ sscanf(line, "%s %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2], &q[3]);
+
+ // converts rotations from degrees into radians
+ q[0] = vpMath::rad(q[0]);
+ q[2] = vpMath::rad(q[2]);
+ q[3] = vpMath::rad(q[3]);
+
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Save joint (articular) positions in a specific Afma4 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : Joint positions: X,Y,A,B. Translations Y is
+ expressed in meters, while joint rotations X,A,B in radians.
+
+ \warning The joint rotations X,A,B written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+
+bool
+vpRobotAfma4::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#AFMA4 - Position - Version 2.01\n\
+#\n\
+# R: X Y A B\n\
+# Joint position: X : rotation of the turret in degrees (joint 1)\n\
+# Y : vertical translation in meters (joint 2)\n\
+# A : pan rotation of the camera in degrees (joint 4)\n\
+# B : tilt rotation of the camera in degrees (joint 5)\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf\n",
+ vpMath::deg(q[0]),
+ q[1],
+ vpMath::deg(q[2]),
+ vpMath::deg(q[3]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Moves the robot to the joint position specified in the filename. The
+ positioning velocity is set to 10% of the robot maximal velocity.
+
+ \param filename: File containing a joint position.
+
+ \sa readPosFile
+
+*/
+void
+vpRobotAfma4::move(const char *filename)
+{
+ vpColVector q;
+
+ this->readPosFile(filename, q) ;
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->setPositioningVelocity(10);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+vpRobotAfma4::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+/*!
+
+ Get the robot articular displacement since the last call of this method.
+
+ \param displacement : The measured articular displacement. The
+ dimension of \e displacement is 6 (the number of axis of the
+ robot). Translations are expressed in meters, rotations in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotAfma4::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ cartesian space. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call of this
+ method. The dimension of \e displacement is always 4, the number of
+ joints. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+
+ double q[6];
+ vpColVector q_cur(6);
+ double timestamp;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
+ for (unsigned int i=0; i < njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ if ( ! first_time_getdis ) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no symbols
+void dummy_vpRobotAfma4() {};
+#endif
+
diff --git a/modules/robot/src/real-robot/afma4/vpServolens.cpp b/modules/robot/src/real-robot/afma4/vpServolens.cpp
new file mode 100644
index 0000000..60ba537
--- /dev/null
+++ b/modules/robot/src/real-robot/afma4/vpServolens.cpp
@@ -0,0 +1,895 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Servolens lens attached to the camera fixed on the
+ * Afma4 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpServolens.cpp
+
+ Interface for the Servolens lens attached to the camera fixed on the
+ Afma4 robot.
+
+*/
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+
+#include <unistd.h>
+#include <termios.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <visp3/robot/vpServolens.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTime.h>
+
+/*!
+ Default constructor. Does nothing.
+
+ \sa open()
+
+*/
+vpServolens::vpServolens() : remfd(0), isinit(false)
+{
+}
+
+/*!
+ Open and initialize the Servolens serial link at 9600 bauds, 7
+ data bits, even parity, 1 stop bit. The cariage return mode is not
+ active, that means that each character is directly read without
+ waitong for a cariage return.
+
+ \sa open()
+*/
+vpServolens::vpServolens(const char *port) : remfd(0), isinit(false)
+{
+ this->open(port);
+}
+
+/*!
+ Destructor.
+
+ Close the Servolens serial link.
+
+ \sa close()
+*/
+vpServolens::~vpServolens()
+{
+ this->close();
+}
+
+/*!
+
+ Open and initialize the Servolens serial link at 9600 bauds, 7
+ data bits, even parity, 1 stop bit. The cariage return mode is not
+ active, that means that each character is directly read without
+ waitong for a cariage return.
+
+ \param port : Serial device like /dev/ttyS0 or /dev/ttya.
+
+ \exception vpRobotException::communicationError : If cannot open
+ Servolens serial port or intialize the serial link.
+
+ \sa close()
+*/
+void
+vpServolens::open(const char *port)
+{
+ if (! isinit) {
+ struct termios info;
+
+ printf("\nOpen the Servolens serial port \"%s\"\n", port);
+
+ if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
+ vpERROR_TRACE ("Cannot open Servolens serial port.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot open Servolens serial port.");
+ }
+
+ // Lecture des parametres courants de la liaison serie.
+ if (tcgetattr(this->remfd, &info) < 0) {
+ ::close(this->remfd);
+ vpERROR_TRACE ("Error using TCGETS in ioctl.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Error using TCGETS in ioctl");
+ }
+
+ //
+ // Configuration de la liaison serie:
+ // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
+ //
+
+ // Traitement sur les caracteres recus
+ info.c_iflag = 0;
+ info.c_iflag |= INLCR;
+
+ // Traitement sur les caracteres envoyes sur la RS232.
+ info.c_oflag = 0; // idem
+
+ // Traitement des lignes
+ info.c_lflag = 0;
+
+ // Controle materiel de la liaison
+ info.c_cflag = 0;
+ info.c_cflag |= CREAD; // Validation reception
+ info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
+
+ // Caracteres immediatement disponibles.
+ // info.c_cc[VMIN] = 1;
+ // info.c_cc[VTIME] = 0;
+
+ if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
+ ::close(this->remfd);
+ vpERROR_TRACE ("Error using TCGETS in ioctl.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Error using TCGETS in ioctl");
+ }
+
+ // Supprime tous les caracteres recus mais non encore lus par read()
+ tcflush(this->remfd, TCIFLUSH);
+
+ isinit = true;
+
+ this->init();
+
+ // Try to get the position of the zoom to check if the lens is really connected
+ unsigned int izoom;
+ if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
+ vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with the servolens. Check if the serial link is connected.");
+
+ }
+
+ }
+}
+
+/*!
+ Close the Servolens serial link.
+ \sa open()
+*/
+void
+vpServolens::close()
+{
+ if (isinit) {
+ printf("\nClose the serial connexion with Servolens\n");
+ ::close(this->remfd);
+ isinit = false;
+ }
+}
+
+/*!
+ Reset the Servolens.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+*/
+void
+vpServolens::reset() const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ /* suppression de l'echo */
+ sprintf(commande, "SE1");
+ this->write(commande);
+
+ /* initialisation de l'objectif, idem qu'a la mise sous tension */
+ sprintf(commande, "SR0");
+ this->write(commande);
+
+ vpTime::wait(25000);
+
+ this->wait();
+
+ /* suppression de l'echo */
+ sprintf(commande, "SE0");
+ this->write(commande);
+
+ /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
+ sprintf(commande, "VW0");
+ this->write(commande);
+}
+/*!
+ Initialize the Servolens lens.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+ \sa open()
+*/
+void
+vpServolens::init() const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+
+ char commande[10];
+
+ /* suppression de l'echo */
+ sprintf(commande, "SE0");
+ this->write(commande);
+
+ /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
+ sprintf(commande, "VW0");
+ this->write(commande);
+
+ /* L'experience montre qu'une petite tempo est utile. */
+ vpTime::wait(500);
+}
+
+/*!
+ Set or remove the Servolens command complete status at the end of servoing.
+
+ \param servo : Servolens servo motor.
+
+ \param active : true to activate the emission of a command complete
+ flag at the end of motion. false to disable this functionality.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+*/
+void
+vpServolens::enableCmdComplete(vpServoType servo, bool active) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
+ * une information de fin de mouvement (ex: ZF, FF, DF).
+ */
+ switch(servo) {
+ case ZOOM:
+ if (active)
+ sprintf(commande, "ZF1");
+ else
+ sprintf(commande, "ZF0");
+ break;
+ case FOCUS:
+ if (active)
+ sprintf(commande, "FF1");
+ else
+ sprintf(commande, "FF0");
+ break;
+ case IRIS:
+ if (active)
+ sprintf(commande, "DF1");
+ else
+ sprintf(commande, "DF0");
+ break;
+ }
+
+ /* envoie de la commande */
+ this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
+}
+
+/*!
+ Enable or disable the emission of the Servolens prompt "SERVOLENS>".
+
+ \param active : true to activate the emission of the prompy. false
+ to disable this functionality.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+*/
+void
+vpServolens::enablePrompt(bool active) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ /* suppression de l'echo */
+ if (active == true)
+ sprintf(commande, "SE1");
+ else
+ sprintf(commande, "SE0");
+
+ this->write(commande);
+}
+
+/*!
+ Set the controller type.
+
+ \param controller : Controller type.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+*/
+void
+vpServolens::setController(vpControllerType controller) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ switch(controller) {
+ case AUTO:
+ /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
+ sprintf(commande, "VW1");
+ this->write(commande);
+ break;
+ case CONTROLLED:
+ /* nettoyage : mot d'etat vide 0000 */
+ sprintf(commande,"SX0842");
+ this->write(commande);
+ /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
+ sprintf(commande, "VW0");
+ this->write(commande);
+ break;
+ case RELEASED:
+ sprintf(commande,"SX1084");
+ this->write(commande);
+ /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
+ sprintf(commande, "VW0");
+ this->write(commande);
+ break;
+ }
+
+}
+
+/*!
+ Activates the auto iris mode of the Servolens.
+
+ \param enable : true to activate the auto iris.
+
+*/
+void
+vpServolens::setAutoIris(bool enable) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ if (enable)
+ sprintf(commande, "DA1");
+ else
+ sprintf(commande, "DA0");
+
+ this->write(commande);
+}
+
+/*!
+ Set the Servolens servo to the desired position.
+
+ \param servo : Servolens servo motor to actuate.
+ \param position : Desired position of the servo.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+*/
+void
+vpServolens::setPosition(vpServoType servo, unsigned int position) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+
+ /* attente du prompt pour envoyer une commande */
+ /*
+ printf("attente prompt\n");
+ this->wait();
+ */
+
+#ifdef FINSERVO
+ /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
+ /* une commande de fin de mouvement (ex: ZF, FF, DF). */
+ this->enableCommandComplete();
+#endif /* FINSERVO */
+
+ // 08/08/00 Fabien S. - Correction de la consigne demandee
+ // pour prendre en compte l'erreur entre la consigne demandee
+ // et la consigne mesuree.
+ // A la consigne du zoom on retranche 1.
+ // A la consigne du focus on ajoute 1.
+ // A la consigne du iris on ajoute 1.
+ switch (servo) {
+ case ZOOM:
+ //printf("zoom demande: %d ", position);
+ position --;
+ if (position < ZOOM_MIN) position = ZOOM_MIN;
+ //printf("zoom corrige: %d \n", position);
+ break;
+ case FOCUS:
+ //printf("focus demande: %d ", position);
+ position ++;
+ if (position > FOCUS_MAX) position = FOCUS_MAX;
+ //printf("focus corrige: %d \n", position);
+ break;
+ case IRIS:
+ // printf("iris demande: %d ", position);
+ position ++;
+ if (position > IRIS_MAX) position = IRIS_MAX;
+ //printf("iris corrige: %s \n", position);
+ break;
+ }
+
+ /* commande a envoyer aux servomoteurs */
+ switch(servo) {
+ case ZOOM:
+ sprintf(commande, "ZD%d", position);
+ break;
+ case FOCUS:
+ sprintf(commande, "FD%d", position);
+ break;
+ case IRIS:
+ sprintf(commande, "DD%d", position);
+ break;
+ }
+ /* envoie de la commande */
+#ifdef PRINT
+ printf("\ncommande: %s", commande);
+#endif
+
+ this->write(commande);
+
+#ifdef FINSERVO
+ /* on attend la fin du mouvement des objectifs */
+ this->wait(servo); /* on attend les codes ZF, FF, DF */
+#endif
+}
+
+/*!
+ Get the Servolens current servo position.
+
+ \param servo : Servolens servo motor to actuate.
+ \param position : Measured position of the servo.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+*/
+bool
+vpServolens::getPosition(vpServoType servo, unsigned int &position) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ char commande[10];
+ char posit[10], *pt_posit;
+ char c;
+ short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
+ short posit_car; /* donne la position du caractere lu */
+ short lecture_posit_en_cours; /* indique si on lit la position courante */
+
+ /* attente du prompt pour envoyer une commande */
+ /*
+ this->wait();
+ */
+ pt_posit = posit;
+
+ /* envoie des commandes pour obtenir la position des servo-moteurs. */
+ switch (servo) {
+ case ZOOM:
+ sprintf(commande, "ZD?");
+ break;
+ case FOCUS:
+ sprintf(commande, "FD?");
+ break;
+ case IRIS:
+ sprintf(commande, "DD?");
+ break;
+ default:
+ break;
+ }
+ /* envoie de la commande */
+ // printf("\ncommande: %s", commande);
+
+ this->write(commande);
+
+ /* on cherche a lire la position du servo-moteur */
+ /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
+ fin_lect_posit = 0;
+ posit_car = 0;
+ lecture_posit_en_cours = 0;
+ do {
+ if (this->read(&c, 1) == true) {
+
+ // printf("caractere lu: %c\n", c);
+ switch (posit_car){
+ /* on lit le 1er caractere; (soit Z, soit F, soit D) */
+ case 0:
+ /* sauvegarde du pointeur */
+ pt_posit = posit;
+
+ switch (servo) {
+ case ZOOM:
+ if( c == 'Z') posit_car = 1;
+ break;
+ case FOCUS:
+ if( c == 'F') posit_car = 1;
+ break;
+ case IRIS:
+ if( c == 'D') posit_car = 1;
+ break;
+ }
+ break;
+
+ /* si le 1er caractere est correct, on lit le 2eme caractere */
+ /* (toujours D) */
+ case 1:
+ if( c == 'D') posit_car = 2;
+ else posit_car = 0; /* le 2eme caractere n'est pas correct */
+ break;
+
+ /* si on a lu les 2 premiers caracteres, on peut lire la */
+ /* position du servo-moteur */
+ case 2:
+ if (c >= '0' && c <= '9')
+ {
+ *pt_posit++ = c; /* sauvegarde de la position */
+ lecture_posit_en_cours = 1;
+ }
+ else if (lecture_posit_en_cours)
+ {
+ fin_lect_posit = 1;
+ *pt_posit = '\0';
+ }
+ else posit_car = 0;
+ break;
+ }
+
+ }
+ else {
+ // Timout sur la lecture, on retoure FALSE
+ return false;
+ }
+ }
+ while ( !fin_lect_posit );
+
+ // printf("\nChaine lue: posit: %s", posit);
+
+ /* toilettage de la position courantes lue */
+ this->clean(posit, posit);
+
+ // printf("\nChaine toilettee: posit: %s", posit);
+ position = (unsigned int)atoi(posit);
+
+ return(true);
+}
+
+/*!
+
+ These parameters are computed from the Dragonfly2 DR2-COL camera sensor pixel
+ size (7.4 um) and from the servolens zoom position.
+
+ \param I : An image coming from the Dragonfly2 camera attached to the
+ servolens.
+
+\code
+ #include <visp3/vs/vpServolens.h>
+
+ int main()
+ {
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ vpServolens servolens("/dev/ttyS0");
+
+ vpImage<unsigned char> I(240, 320);
+ vpCameraParameters cam = servolens.getCameraParameters(I);
+ std::cout << "Camera parameters: " << cam << std::endl;
+#endif
+ }
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+\endcode
+
+ */
+vpCameraParameters
+vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ vpCameraParameters cam;
+ double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
+ double px=1000, py=1000, u0=320, v0=240;
+ // Determine if the image is subsampled.
+ // Dragonfly2 native images are 640 by 480
+ double subsample_factor = 1.;
+ double width = I.getWidth();
+ double height= I.getHeight();
+
+
+ if (width > 300 && width < 340 && height > 220 && height < 260)
+ subsample_factor = 2;
+ else if (width > 140 && width < 1800 && height > 100 && height < 140)
+ subsample_factor = 4;
+
+ unsigned zoom;
+ getPosition(vpServolens::ZOOM, zoom);
+ //std::cout << "Actual zoom value: " << zoom << std::endl;
+
+ // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
+ // aux parametres de calibration de la camera (donnees constructeur) pour des
+ // tailles d'images CCIR (768x576), donc avec scale = 1.
+ double focale = zoom * 1.0e-5; // Transformation en metres
+ px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
+ py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
+ u0 = I.getWidth() / 2.;
+ v0 = I.getHeight() / 2.;
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+
+ return cam;
+}
+
+/*!
+ Waits for the Servolens promt '>'.
+
+ \return The prompt character.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+*/
+char
+vpServolens::wait() const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+
+ ssize_t r;
+ r = ::write(this->remfd, "\r\n", strlen("\r\n"));
+ if (r != (ssize_t)(strlen("\r\n"))) {
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot write on Servolens.");
+ }
+ char c;
+ do {
+ r = ::read(this->remfd, &c, 1);
+ c &= 0x7f;
+ }
+ while (c != '>');
+ return c;
+
+}
+
+/*!
+
+ Waits the end of motion of the corresponding servo motor.
+
+ \param servo : Servolens servo motor.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+
+*/
+void
+vpServolens::wait(vpServoType servo) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+
+ char c;
+ char fin_mvt[3];
+ bool sortie = false;
+
+ switch (servo) {
+ case ZOOM:
+ sprintf(fin_mvt, "ZF");
+ break;
+ case FOCUS:
+ sprintf(fin_mvt, "FF");
+ break;
+ case IRIS:
+ default:
+ sprintf(fin_mvt, "DF");
+ break;
+
+ }
+
+ /* lecture des caracteres recus */
+ do {
+ /* lecture des caracteres */
+ if (::read(this->remfd,&c,1) != 1) {
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot read on Servolens.");
+ }
+ c &= 0x7f;
+
+ /* tests si fin de mouvement */
+ if (c == fin_mvt[0]) {
+ /* lecture du caractere suivant */
+ if (::read(this->remfd,&c,1) != 1) {
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot read on Servolens.");
+ }
+
+ c &= 0x7f;
+ if (c == fin_mvt[1]) {
+ sortie = true;
+ }
+ }
+ }
+ while ( !sortie);
+
+ /* printf("\nmouvement fini: chaine lue = %s", chaine); */
+}
+
+/*!
+ Read one character form the serial link.
+
+ \param c : The character that was read.
+ \param timeout_s : Timeout in seconds.
+
+ \return true if the reading was successfully achieved, false otherwise.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+*/
+bool
+vpServolens::read(char *c, long timeout_s) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+
+ int n;
+ fd_set readfds; /* list of fds for select to listen to */
+ struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
+
+ FD_ZERO(&readfds);
+ FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
+
+ if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
+ (fd_set *)NULL, &timeout) > 0) {
+ n = ::read(this->remfd, c, 1); /* read one character at a time */
+ if (n != 1)
+ return false;
+ *c &= 0x7f;
+ //printf("lecture 1 car: %c\n", *c);
+ return(true);
+ }
+
+ return (false);
+}
+
+/*!
+ Write a string to the serial link.
+
+ \param s : String to send to the Servolens.
+
+ \exception vpRobotException::communicationError : If cannot dial
+ with Servolens.
+*/
+void
+vpServolens::write(const char *s) const
+{
+ if (!isinit) {
+ vpERROR_TRACE ("Cannot dial with Servolens.");
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot dial with Servolens.");
+ }
+ ssize_t r = 0;
+ r = ::write(this->remfd,"\r", strlen("\r"));
+ r += ::write(this->remfd, s, strlen(s));
+ r += ::write(this->remfd,"\r", strlen("\r"));
+ if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
+ throw vpRobotException (vpRobotException::communicationError,
+ "Cannot write on Servolens.");
+ }
+
+
+ /*
+ * Une petite tempo pour laisser le temps a la liaison serie de
+ * digerer la commande envoyee. En fait, la liaison serie fonctionne
+ * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
+ * Les plus longues commandes envoyees sur la liaison serie sont du type:
+ * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
+ * envoyer la commande SX0842.
+ * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
+ * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
+ * Ici on rajoute une marge pour amener la tempo a 20 ms.
+ */
+ vpTime::wait(20);
+}
+
+/*!
+ Suppress all the zero characters on the left hand of \e in string.
+
+ \param in : Input string.
+ \param out : Output string without zero characters on the left.
+
+*/
+bool
+vpServolens::clean(const char *in, char *out) const
+{
+ short nb_car, i=0;
+ bool error = false;
+
+ nb_car = strlen(in);
+
+ /* on se positionne sur le 1er caractere different de zero */
+ while( *(in) == '0' && i++ < nb_car ) {
+ in++;
+ if (i == nb_car)
+ {
+ error = true; /* la chaine ne contient pas une position */
+ *(out++) = '0'; /* mise a zero de la position */
+ }
+ }
+
+ /* copie de la position epuree des zeros de gauche */
+ while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
+ /* de chaine \0 */
+ *(out++) = *(in++);
+ }
+ return (error);
+}
+
+#endif
diff --git a/modules/robot/src/real-robot/afma6/vpAfma6.cpp b/modules/robot/src/real-robot/afma6/vpAfma6.cpp
new file mode 100644
index 0000000..02a1429
--- /dev/null
+++ b/modules/robot/src/real-robot/afma6/vpAfma6.cpp
@@ -0,0 +1,1581 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma6 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpAfma6.cpp
+
+ Control of Irisa's gantry robot named Afma6.
+
+*/
+
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/robot/vpAfma6.h>
+
+/* ----------------------------------------------------------------------- */
+/* --- STATIC ------------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+static const char *opt_Afma6[] = {"JOINT_MAX","JOINT_MIN","LONG_56","COUPL_56",
+ "CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
+ NULL};
+
+const char * const vpAfma6::CONST_AFMA6_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
+#endif
+
+const char * const vpAfma6::CONST_CAMERA_AFMA6_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Afma6/current/include/const_camera_Afma6.xml";
+#else
+= "/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml";
+#endif
+
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+const char * const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
+const char * const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
+const char * const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
+const char * const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
+
+const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP;
+
+const unsigned int vpAfma6::njoint = 6;
+
+/*!
+
+ Default constructor.
+
+*/
+vpAfma6::vpAfma6()
+ : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(),
+ tool_current(vpAfma6::defaultTool),
+ projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
+{
+ // Set the default parameters in case of the config files on the NAS
+ // at Inria are not available.
+
+ //
+ // Geometric model constant parameters
+ //
+ // coupling between join 5 and 6
+ this->_coupl_56 = 0.009091;
+ // distance between join 5 and 6
+ this->_long_56 = -0.06924;
+ // Camera extrinsic parameters: effector to camera frame
+ this->_eMc.eye(); // Default values are initialized ...
+ // ... in init (vpAfma6::vpAfma6ToolType tool,
+ // vpCameraParameters::vpCameraParametersProjType projModel)
+ // Maximal value of the joints
+ this->_joint_max[0] = 0.7001;
+ this->_joint_max[1] = 0.5201;
+ this->_joint_max[2] = 0.4601;
+ this->_joint_max[3] = 2.7301;
+ this->_joint_max[4] = 2.4801;
+ this->_joint_max[5] = 1.5901;
+ // Minimal value of the joints
+ this->_joint_min[0] = -0.6501;
+ this->_joint_min[1] = -0.6001;
+ this->_joint_min[2] = -0.5001;
+ this->_joint_min[3] = -2.7301;
+ this->_joint_min[4] = -0.1001;
+ this->_joint_min[5] = -1.5901;
+
+ init();
+
+}
+
+/*!
+
+ Initialize the robot with the default tool vpAfma6::defaultTool.
+ */
+void
+vpAfma6::init (void)
+{
+ this->init ( vpAfma6::defaultTool);
+ return;
+}
+
+/*!
+
+ Read files containing the constant parameters related to the robot
+ kinematics and to the end-effector to camera transformation.
+
+ \warning This function is only available if the macro
+ VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
+
+ \param paramAfma6 : Filename containing the constant parameters of
+ the robot kinematics.
+
+ \param paramCamera : Filename containing the camera extrinsic parameters.
+
+*/
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+void
+vpAfma6::init (const char * paramAfma6,
+ const char * paramCamera)
+{
+ // vpTRACE ("Parsage fichier robot.");
+ this->parseConfigFile (paramAfma6);
+
+ //vpTRACE ("Parsage fichier camera.");
+ this->parseConfigFile (paramCamera);
+
+ return ;
+}
+#endif
+
+/*!
+
+ Get the constant parameters related to the robot kinematics and to
+ the end-effector to camera transformation (eMc) corresponding to the
+ camera extrinsic parameters. These last parameters depend on the
+ camera and projection model in use.
+
+ \param tool : Camera in use.
+
+ \param proj_model : Projection model of the camera.
+
+*/
+void
+vpAfma6::init (vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType proj_model)
+{
+
+ this->projModel = proj_model;
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ // Read the robot parameters from files
+ char filename_eMc [FILENAME_MAX];
+ switch (tool) {
+ case vpAfma6::TOOL_CCMOP: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ 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;
+ }
+ }
+
+ this->init (vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
+
+#else // VISP_HAVE_ACCESS_TO_NAS
+
+ // Use here default values of the robot constant parameters.
+ switch (tool) {
+ case vpAfma6::TOOL_CCMOP: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ _erc[0] = vpMath::rad(164.35); // rx
+ _erc[1] = vpMath::rad( 89.64); // ry
+ _erc[2] = vpMath::rad(-73.05); // rz
+ _etc[0] = 0.0117; // tx
+ _etc[1] = 0.0033; // ty
+ _etc[2] = 0.2272; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ _erc[0] = vpMath::rad(33.54); // rx
+ _erc[1] = vpMath::rad(89.34); // ry
+ _erc[2] = vpMath::rad(57.83); // rz
+ _etc[0] = 0.0373; // tx
+ _etc[1] = 0.0024; // ty
+ _etc[2] = 0.2286; // tz
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ _erc[0] = vpMath::rad( 88.33); // rx
+ _erc[1] = vpMath::rad( 72.07); // ry
+ _erc[2] = vpMath::rad( 2.53); // rz
+ _etc[0] = 0.0783; // tx
+ _etc[1] = 0.1234; // ty
+ _etc[2] = 0.1638; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ _erc[0] = vpMath::rad(86.69); // rx
+ _erc[1] = vpMath::rad(71.93); // ry
+ _erc[2] = vpMath::rad( 4.17); // rz
+ _etc[0] = 0.1034; // tx
+ _etc[1] = 0.1142; // ty
+ _etc[2] = 0.1642; // tz
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ _erc[0] = vpMath::rad( 90.40); // rx
+ _erc[1] = vpMath::rad( 75.11); // ry
+ _erc[2] = vpMath::rad( 0.18); // rz
+ _etc[0] = 0.0038; // tx
+ _etc[1] = 0.1281; // ty
+ _etc[2] = 0.1658; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ _erc[0] = vpMath::rad(91.61); // rx
+ _erc[1] = vpMath::rad(76.17); // ry
+ _erc[2] = vpMath::rad(-0.98); // rz
+ _etc[0] = 0.0815; // tx
+ _etc[1] = 0.1162; // ty
+ _etc[2] = 0.1658; // tz
+ break;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ // set eMc to identity
+ _erc[0] = 0; // rx
+ _erc[1] = 0; // ry
+ _erc[2] = 0; // rz
+ _etc[0] = 0; // tx
+ _etc[1] = 0; // ty
+ _etc[2] = 0; // tz
+ break;
+ }
+ break;
+ }
+ }
+ vpRotationMatrix eRc(_erc);
+ this->_eMc.buildFrom(_etc, eRc);
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+ setToolType(tool);
+ return ;
+}
+
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the six joints.
+
+ This method is the same than get_fMc(const vpColVector & q).
+
+ \param q : Articular position of the six joints: q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \return The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the base frame and the
+ camera frame (fMc).
+
+ \sa get_fMc(const vpColVector & q)
+ \sa getInverseKinematics()
+
+*/
+vpHomogeneousMatrix
+vpAfma6::getForwardKinematics(const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ fMc = get_fMc(q);
+
+ return fMc;
+}
+
+/*!
+
+ Compute the inverse kinematics (inverse geometric model).
+
+ By inverse kinematics we mean here the six articular values of the joint
+ positions given the position and the orientation of the camera frame
+ relative to the base frame.
+
+ \param fMc : Homogeneous matrix describing the transformation from
+ base frame to the camera frame.
+
+ \param q : In input, the current articular joint position of the
+ robot. In output, the solution of the inverse kinematics. Articular
+ position of the six joints: q[0], q[1], q[2] correspond to the first
+ 3 translations expressed in meter, while q[3], q[4] and q[5]
+ correspond to the 3 succesives rotations expressed in radians.
+
+ \param nearest : true to return the nearest solution to q. false to
+ return the farest.
+
+ \param verbose : Activates printings when no solution is found.
+
+ \return The number of solutions (1 or 2) of the inverse geometric
+ model. O, if no solution can be found.
+
+ The code below shows how to compute the inverse geometric model:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotAfma6.h>
+
+int main()
+{
+#ifdef VISP_HAVE_AFMA6
+ vpColVector q1(6), q2(6);
+ vpHomogeneousMatrix fMc;
+
+ vpRobotAfma6 robot;
+
+ // Get the current articular position of the robot
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
+
+ // Compute the pose of the camera in the reference frame using the
+ // direct geometric model
+ fMc = robot.getForwardKinematics(q1);
+ // this is similar to fMc = robot.get_fMc(q1);
+ // or robot.get_fMc(q1, fMc);
+
+ // Compute the inverse geometric model
+ int nbsol; // number of solutions (0, 1 or 2) of the inverse geometric model
+ // get the nearest solution to the current articular position
+ nbsol = robot.getInverseKinematics(fMc, q1, true);
+
+ if (nbsol == 0)
+ std::cout << "No solution of the inverse geometric model " << std::endl;
+ else if (nbsol >= 1)
+ std::cout << "First solution: " << q1 << std::endl;
+
+ if (nbsol == 2) {
+ // Compute the other solution of the inverse geometric model
+ q2 = q1;
+ robot.getInverseKinematics(fMc, q2, false);
+ std::cout << "Second solution: " << q2 << std::endl;
+ }
+#endif
+}
+ \endcode
+
+ \sa getForwardKinematics()
+
+*/
+int
+vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
+ vpColVector & q, const bool &nearest, const bool &verbose) const
+{
+ vpHomogeneousMatrix fMe;
+ double q_[2][6],d[2],t;
+ int ok[2];
+ double cord[6];
+
+ int nbsol = 0;
+
+ if (q.getRows() != njoint)
+ q.resize(6);
+
+
+ // for(unsigned int i=0;i<3;i++) {
+ // fMe[i][3] = fMc[i][3];
+ // for(int j=0;j<3;j++) {
+ // fMe[i][j] = 0.0;
+ // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
+ // fMe[i][3] -= fMe[i][j]*rpi[j][3];
+ // }
+ // }
+
+ // std::cout << "\n\nfMc: " << fMc;
+ // std::cout << "\n\neMc: " << _eMc;
+
+ fMe = fMc * this->_eMc.inverse();
+ // std::cout << "\n\nfMe: " << fMe;
+
+ if (fMe[2][2] >= .99999f)
+ {
+ vpTRACE("singularity\n");
+ q_[0][4] = q_[1][4] = M_PI/2.f;
+ t = atan2(fMe[0][0],fMe[0][1]);
+ q_[1][3] = q_[0][3] = q[3];
+ 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 */
+ {
+ q_[1][5] -= vpMath::rad(10);
+ q_[1][3] += vpMath::rad(10);
+ }
+ while (q_[1][5] <= this->_joint_min[5])
+ {
+ q_[1][5] += vpMath::rad(10);
+ q_[1][3] -= vpMath::rad(10);
+ }
+ }
+ else if (fMe[2][2] <= -.99999)
+ {
+ vpTRACE("singularity\n");
+ q_[0][4] = q_[1][4] = -M_PI/2;
+ t = atan2(fMe[1][1],fMe[1][0]);
+ 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 */
+ {
+ q_[1][5] -= vpMath::rad(10);
+ q_[1][3] -= vpMath::rad(10);
+ }
+ while (q_[1][5] <= this->_joint_min[5])
+ {
+ q_[1][5] += vpMath::rad(10);
+ q_[1][3] += vpMath::rad(10);
+ }
+ }
+ else
+ {
+ q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
+ if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
+ else q_[1][3] = q_[0][3] + M_PI;
+
+ q_[0][4] = asin(fMe[2][2]);
+ if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
+ else q_[1][4] = -M_PI - q_[0][4];
+
+ q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
+ if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
+ else q_[1][5] = q_[0][5] + M_PI;
+ }
+ q_[0][0] = fMe[0][3] - this->_long_56*cos(q_[0][3]);
+ q_[1][0] = fMe[0][3] - this->_long_56*cos(q_[1][3]);
+ q_[0][1] = fMe[1][3] - this->_long_56*sin(q_[0][3]);
+ 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 */
+ q_[0][5] += this->_coupl_56*q_[0][4];
+ q_[1][5] += this->_coupl_56*q_[1][4];
+
+ for (int j=0;j<2;j++)
+ {
+ ok[j] = 1;
+ // test is position is reachable
+ for (unsigned int i=0;i<6;i++) {
+ if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
+ if (verbose) {
+ if (i < 3)
+ std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < " << this->_joint_max[i] << std::endl;
+ else
+ std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < " << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
+ }
+ ok[j] = 0;
+ }
+ }
+ }
+ if (ok[0] == 0)
+ {
+ if (ok[1] == 0) {
+ std::cout << "No solution..." << std::endl;
+ nbsol = 0;
+ return nbsol;
+ }
+ else if (ok[1] == 1) {
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
+ nbsol = 1;
+ }
+ }
+ else
+ {
+ if (ok[1] == 0) {
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ nbsol = 1;
+ }
+ else
+ {
+ nbsol = 2;
+ //vpTRACE("2 solutions\n");
+ for (int j=0;j<2;j++)
+ {
+ d[j] = 0.0;
+ for (unsigned int i=3;i<6;i++)
+ d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
+ }
+ if (nearest == true)
+ {
+ if (d[0] <= d[1])
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ else
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
+ }
+ else
+ {
+ if (d[0] <= d[1])
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
+ else
+ for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ }
+ }
+ }
+ for(unsigned int i=0; i<6; i++)
+ q[i] = cord[i] ;
+
+ return nbsol;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the six joints.
+
+ This method is the same than getForwardKinematics(const vpColVector & q).
+
+ \param q : Articular position of the six joints: q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \return The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the base frame and the
+ camera frame (fMc).
+
+ \sa getForwardKinematics(const vpColVector & q)
+*/
+vpHomogeneousMatrix
+vpAfma6::get_fMc (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ get_fMc(q, fMc);
+
+ return fMc;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the articular positions of all
+ the six joints.
+
+ \param q : Articular joint position of the robot. q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \param fMc The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ camera frame (fMc).
+
+*/
+void
+vpAfma6::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+{
+
+ // Compute the direct geometric model: fMe = transformation between
+ // fix and end effector frame.
+ vpHomogeneousMatrix fMe;
+
+ get_fMe(q, fMe);
+
+ fMc = fMe * this->_eMc;
+
+ return;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the end effector with respect to the base frame given the
+ articular positions of all the six joints.
+
+ \param q : Articular joint position of the robot. q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \param fMe The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ end effector frame (fMe).
+
+*/
+void
+vpAfma6::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+{
+ double q0 = q[0]; // meter
+ double q1 = q[1]; // meter
+ double q2 = q[2]; // meter
+
+ /* Decouplage liaisons 2 et 3. */
+ double q5 = q[5] - this->_coupl_56 * q[4];
+
+ double c1 = cos(q[3]);
+ double s1 = sin(q[3]);
+ double c2 = cos(q[4]);
+ double s2 = sin(q[4]);
+ double c3 = cos(q5);
+ double s3 = sin(q5);
+
+ // Compute the direct geometric model: fMe = transformation betwee
+ // fix and end effector frame.
+ fMe[0][0] = s1*s2*c3 + c1*s3;
+ fMe[0][1] = -s1*s2*s3 + c1*c3;
+ fMe[0][2] = -s1*c2;
+ fMe[0][3] = q0 + this->_long_56*c1;
+
+ fMe[1][0] = -c1*s2*c3 + s1*s3;
+ fMe[1][1] = c1*s2*s3 + s1*c3;
+ fMe[1][2] = c1*c2;
+ fMe[1][3] = q1 + this->_long_56*s1;
+
+ fMe[2][0] = c2*c3;
+ fMe[2][1] = -c2*s3;
+ fMe[2][2] = s2;
+ fMe[2][3] = q2;
+
+ fMe[3][0] = 0;
+ fMe[3][1] = 0;
+ fMe[3][2] = 0;
+ fMe[3][3] = 1;
+
+ // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
+
+ return;
+}
+
+/*!
+
+ Get the geometric transformation between the camera frame and the
+ end-effector frame. This transformation is constant and correspond
+ to the extrinsic camera parameters estimated by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ cMe = this->_eMc.inverse();
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+
+ return;
+}
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame.
+
+ \param q : Articular joint position of the robot. q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \param eJe : Robot jacobian expressed in the end-effector frame.
+
+*/
+void
+vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+{
+
+ eJe.resize(6,6) ;
+
+ double s4,c4,s5,c5,s6,c6 ;
+
+ s4=sin(q[3]); c4=cos(q[3]);
+ s5=sin(q[4]); c5=cos(q[4]);
+ s6=sin(q[5]-this->_coupl_56*q[4]); c6=cos(q[5]-this->_coupl_56*q[4]);
+
+ eJe = 0;
+ eJe[0][0] = s4*s5*c6+c4*s6;
+ eJe[0][1] = -c4*s5*c6+s4*s6;
+ eJe[0][2] = c5*c6;
+ eJe[0][3] = - this->_long_56*s5*c6;
+
+ eJe[1][0] = -s4*s5*s6+c4*c6;
+ eJe[1][1] = c4*s5*s6+s4*c6;
+ eJe[1][2] = -c5*s6;
+ eJe[1][3] = this->_long_56*s5*s6;
+
+ eJe[2][0] = -s4*c5;
+ eJe[2][1] = c4*c5;
+ eJe[2][2] = s5;
+ eJe[2][3] = this->_long_56*c5;
+
+ eJe[3][3] = c5*c6;
+ eJe[3][4] = s6;
+
+ eJe[4][3] = -c5*s6;
+ eJe[4][4] = c6;
+
+ eJe[5][3] = s5;
+ eJe[5][4] = -this->_coupl_56;
+ eJe[5][5] = 1;
+
+ return;
+}
+
+
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ \f[
+ {^f}J_e = \left(\begin{array}{cccccc}
+ 1 & 0 & 0 & -Ls4 & 0 & 0 \\
+ 0 & 1 & 0 & Lc4 & 0 & 0 \\
+ 0 & 0 & 1 & 0 & 0 & 0 \\
+ 0 & 0 & 0 & 0 & c4+\gamma s4c5 & -s4c5 \\
+ 0 & 0 & 0 & 0 & s4-\gamma c4c5 & c4c5 \\
+ 0 & 0 & 0 & 1 & -gamma s5 & s5 \\
+ \end{array}
+ \right)
+ \f]
+ where \f$\gamma\f$ is the coupling factor between join 5 and 6.
+
+ \param q : Articular joint position of the robot. q[0], q[1], q[2]
+ correspond to the first 3 translations expressed in meter, while
+ q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
+ radians.
+
+ \param fJe : Robot jacobian expressed in the robot reference frame.
+
+*/
+
+void
+vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+{
+
+ fJe.resize(6,6) ;
+
+ // block superieur gauche
+ fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
+
+ double s4 = sin(q[3]) ;
+ double c4 = cos(q[3]) ;
+
+
+ // block superieur droit
+ fJe[0][3] = - this->_long_56*s4 ;
+ fJe[1][3] = this->_long_56*c4 ;
+
+
+ double s5 = sin(q[4]) ;
+ double c5 = cos(q[4]) ;
+ // block inferieur droit
+ fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
+ fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
+ fJe[5][3] = 1 ; fJe[5][5] = s5 ;
+
+ // coupling between joint 5 and 6
+ fJe[3][4] += this->_coupl_56*s4*c5;
+ fJe[4][4] += -this->_coupl_56*c4*c5;
+ fJe[5][4] += -this->_coupl_56*s5;
+
+ return;
+}
+
+
+/*!
+ Get min joint values.
+
+ \return Minimal joint values for the 6 dof
+ X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
+ A,B,C in radians.
+
+*/
+vpColVector
+vpAfma6::getJointMin() const
+{
+ vpColVector qmin(6);
+ for (unsigned int i=0; i < 6; i ++)
+ qmin[i] = this->_joint_min[i];
+ return qmin;
+}
+
+/*!
+ Get max joint values.
+
+ \return Maximal joint values for the 6 dof
+ X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
+ A,B,C in radians.
+
+*/
+vpColVector
+vpAfma6::getJointMax() const
+{
+ vpColVector qmax(6);
+ for (unsigned int i=0; i < 6; i ++)
+ qmax[i] = this->_joint_max[i];
+ return qmax;
+}
+
+/*!
+
+ Return the coupling factor between join 5 and 6.
+
+ \return Coupling factor between join 5 and 6.
+*/
+double
+vpAfma6::getCoupl56() const
+{
+ return _coupl_56;
+}
+
+/*!
+
+ Return the distance between join 5 and 6.
+
+ \return Distance between join 5 and 6.
+*/
+double
+vpAfma6::getLong56() const
+{
+ return _long_56;
+}
+
+
+/*!
+
+ This function gets the robot constant parameters from a file.
+
+ \warning This function is only available if the macro
+ VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
+
+ \param filename : File name containing the robot constant
+ parameters, like max/min joint values, distance between 5 and 6 axis,
+ coupling factor between axis 5 and 6, and the hand-to-eye homogeneous matrix.
+
+*/
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+void
+vpAfma6::parseConfigFile (const char * filename)
+{
+ size_t dim;
+ int code;
+ char Ligne[FILENAME_MAX];
+ char namoption[100];
+ FILE * fdtask;
+ int numLn = 0;
+ double rot_eMc[3]; // rotation
+ double trans_eMc[3]; // translation
+ bool get_rot_eMc = false;
+ bool get_trans_eMc = false;
+
+ //vpTRACE("Read the config file for constant parameters %s.", filename);
+ if ((fdtask = fopen(filename, "r" )) == NULL)
+ {
+ vpERROR_TRACE ("Impossible to read the config file %s.",
+ filename);
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the config file.");
+ }
+
+ while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
+ numLn ++;
+ if ('#' == Ligne[0]) { continue; }
+ sscanf(Ligne, "%s", namoption);
+ dim = strlen(namoption);
+
+ for (code = 0;
+ NULL != opt_Afma6[code];
+ ++ code)
+ {
+ if (strncmp(opt_Afma6[code], namoption, dim) == 0)
+ {
+ break;
+ }
+ }
+
+ switch(code)
+ {
+ case 0:
+ sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf",
+ namoption,
+ &this->_joint_max[0], &this->_joint_max[1],
+ &this->_joint_max[2], &this->_joint_max[3],
+ &this->_joint_max[4], &this->_joint_max[5]);
+ break;
+
+ case 1:
+ sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf", namoption,
+ &this->_joint_min[0], &this->_joint_min[1],
+ &this->_joint_min[2], &this->_joint_min[3],
+ &this->_joint_min[4], &this->_joint_min[5]);
+ break;
+
+ case 2:
+ sscanf(Ligne, "%s %lf", namoption, &this->_long_56);
+ break;
+
+ case 3:
+ sscanf(Ligne, "%s %lf", namoption, &this->_coupl_56);
+ break;
+
+ case 4:
+ break; // Nothing to do: camera name
+
+ case 5:
+ sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &rot_eMc[0],
+ &rot_eMc[1],
+ &rot_eMc[2]);
+
+ // Convert rotation from degrees to radians
+ rot_eMc[0] *= M_PI / 180.0;
+ rot_eMc[1] *= M_PI / 180.0;
+ rot_eMc[2] *= M_PI / 180.0;
+ get_rot_eMc = true;
+ break;
+
+ case 6:
+ sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &trans_eMc[0],
+ &trans_eMc[1],
+ &trans_eMc[2]);
+ get_trans_eMc = true;
+ break;
+
+ default:
+ vpERROR_TRACE ("Bad configuration file %s "
+ "ligne #%d.", filename, numLn);
+ } /* SWITCH */
+ } /* WHILE */
+
+ fclose (fdtask);
+
+ // Compute the eMc matrix from the translations and rotations
+ if (get_rot_eMc && get_trans_eMc) {
+ for (unsigned int i=0; i < 3; i ++) {
+ _erc[i] = rot_eMc[i];
+ _etc[i] = trans_eMc[i];
+ }
+
+ vpRotationMatrix eRc(_erc);
+ this->_eMc.buildFrom(_etc, eRc);
+ }
+
+ return;
+}
+#endif
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpAfma6::CONST_CAMERA_AFMA6_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param image_width : Image width used to compute camera calibration.
+ \param image_height : Image height used to compute camera calibration.
+
+ The code below shows how to get the camera parameters of the camera
+ attached to the robot.
+
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+
+ vpRobotAfma6 robot;
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ // Camera parameters are read from
+ // /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
+ // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined in vpConfig.h file
+ try {
+ robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+*/
+
+void
+vpAfma6::getCameraParameters (vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const
+{
+#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
+ vpXmlParserCamera parser;
+ switch (getToolType()) {
+ case vpAfma6::TOOL_CCMOP: {
+ std::cout << "Get camera parameters for camera \""
+ << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
+ vpAfma6::CONST_CCMOP_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ std::cout << "Get camera parameters for camera \""
+ << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
+ vpAfma6::CONST_GRIPPER_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ std::cout << "Get camera parameters for camera \""
+ << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
+ vpAfma6::CONST_VACUUM_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
+ vpAfma6::CONST_GENERIC_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ 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).");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ break;
+ }
+ }
+#else
+ // Set default parameters
+ switch (getToolType()) {
+ case vpAfma6::TOOL_CCMOP: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ default:
+ vpERROR_TRACE ("This error should not occur!");
+ break;
+ }
+#endif
+ return;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ Camera parameters are read from
+ /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A B&W image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+
+ vpRobotAfma6 robot;
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+void
+vpAfma6::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
+/*!
+ \brief Get the current intrinsic camera parameters obtained by calibration.
+
+ Camera parameters are read from
+ /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A color image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
+ vpImage<vpRGBa> I;
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+
+ vpRobotAfma6 robot;
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+
+void
+vpAfma6::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
+
+
+/*!
+
+ Print on the output stream \e os the robot parameters (joint
+ min/max, distance between axis 5 and 6, coupling factor between axis
+ 5 and 6, hand-to-eye homogeneous matrix.
+
+ \param os : Output stream.
+ \param afma6 : Robot parameters.
+*/
+VISP_EXPORT std::ostream & operator << (std::ostream & os,
+ const vpAfma6 & afma6)
+{
+ vpRotationMatrix eRc;
+ afma6._eMc.extract(eRc);
+ vpRxyzVector rxyz(eRc);
+
+ os
+ << "Joint Max:" << std::endl
+ << "\t" << afma6._joint_max[0]
+ << "\t" << afma6._joint_max[1]
+ << "\t" << afma6._joint_max[2]
+ << "\t" << afma6._joint_max[3]
+ << "\t" << afma6._joint_max[4]
+ << "\t" << afma6._joint_max[5]
+ << "\t" << std::endl
+
+ << "Joint Min: " << std::endl
+ << "\t" << afma6._joint_min[0]
+ << "\t" << afma6._joint_min[1]
+ << "\t" << afma6._joint_min[2]
+ << "\t" << afma6._joint_min[3]
+ << "\t" << afma6._joint_min[4]
+ << "\t" << afma6._joint_min[5]
+ << "\t" << std::endl
+
+ << "Long 5-6: " << std::endl
+ << "\t" << afma6._long_56
+ << "\t" << std::endl
+
+ << "Coupling 5-6:" << std::endl
+ << "\t" << afma6._coupl_56
+ << "\t" << std::endl
+
+ << "eMc: "<< std::endl
+ << "\tTranslation (m): "
+ << afma6._eMc[0][3] << " "
+ << afma6._eMc[1][3] << " "
+ << afma6._eMc[2][3]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (rad) : "
+ << rxyz[0] << " "
+ << rxyz[1] << " "
+ << rxyz[2]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : "
+ << vpMath::deg(rxyz[0]) << " "
+ << vpMath::deg(rxyz[1]) << " "
+ << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
+
+ return os;
+}
+
diff --git a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp
new file mode 100644
index 0000000..f47d8bc
--- /dev/null
+++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp
@@ -0,0 +1,2240 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Afma6 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_AFMA6
+
+#include <signal.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpRotationMatrix.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ----------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+bool vpRobotAfma6::robotAlreadyCreated = false;
+
+/*!
+
+ Default positioning velocity in percentage of the maximum
+ velocity. This value is set to 15. The member function
+ setPositioningVelocity() allows to change this value.
+
+*/
+const double vpRobotAfma6::defaultPositioningVelocity = 15.0;
+
+/* ---------------------------------------------------------------------- */
+/* --- EMERGENCY STOP --------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Emergency stops the robot if the program is interrupted by a SIGINT
+ (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
+ or SIGQUIT signal.
+
+*/
+void emergencyStopAfma6(int signo)
+{
+ std::cout << "Stop the Afma6 application by signal ("
+ << signo << "): " << (char)7 ;
+ switch(signo)
+ {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ case SIGBUS:
+ std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ case SIGSEGV:
+ std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ case SIGKILL:
+ std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ case SIGQUIT:
+ std::cout <<"SIGQUIT " << std::endl ; break ;
+ default :
+ std::cout << signo << std::endl ;
+ }
+ //std::cout << "Emergency stop called\n";
+ // PrimitiveESTOP_Afma6();
+ PrimitiveSTOP_Afma6();
+ std::cout << "Robot was stopped\n";
+
+ // Free allocated resources
+ // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
+
+ fprintf(stdout, "Application ");
+ fflush(stdout);
+ kill(getpid(), SIGKILL);
+ exit(1) ;
+}
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ The only available constructor.
+
+ This contructor calls init() to initialise
+ the connection with the MotionBox or low level controller, send the
+ default eMc homogeneous matrix, power on the robot and wait 1 sec
+ before returning to be sure the initialisation is done.
+
+ It also set the robot state to vpRobot::STATE_STOP.
+
+ To set the extrinsic camera parameters related to the eMc matrix
+ obtained with a camera perspective projection model including the
+ distorsion, use the code below:
+
+ \code
+ vpRobotAfma6 robot;
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters of the image I
+ acquired with the camera, with:
+
+ \code
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+ \endcode
+
+ \sa vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType)
+
+*/
+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
+ */
+
+ signal(SIGINT, emergencyStopAfma6);
+ signal(SIGBUS, emergencyStopAfma6) ;
+ signal(SIGSEGV, emergencyStopAfma6) ;
+ signal(SIGKILL, emergencyStopAfma6);
+ signal(SIGQUIT, emergencyStopAfma6);
+
+ setVerbose(verbose);
+ if (verbose_)
+ std::cout << "Open communication with MotionBlox.\n";
+ try {
+ this->init();
+ this->setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...) {
+ // vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ positioningVelocity = defaultPositioningVelocity ;
+
+ vpRobotAfma6::robotAlreadyCreated = true;
+
+ return ;
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INITIALISATION ----------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Initialise the connection with the MotionBox or low level
+ controller, send the default eMc homogeneous matrix, power on the
+ robot and wait 1 sec before returning to be sure the initialisation
+ is done.
+
+ \warning This method sets the camera extrinsic parameters (matrix
+ eMc) to the one obtained by calibration with a camera projection
+ model without distorsion by calling
+ init(vpAfma6::defaultCameraRobot). If you want to set the extrinsic
+ camera parameters to those obtained with a camera perspective model
+ including the distorsion you have to call the
+ init(vpAfma6::vpAfma6CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType) method.
+
+ \sa vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType)
+*/
+void
+vpRobotAfma6::init (void)
+{
+ InitTry;
+
+ // Initialise private variables used to compute the measured velocities
+ q_prev_getvel.resize(6);
+ q_prev_getvel = 0;
+ time_prev_getvel = 0;
+ first_time_getvel = true;
+
+ // Initialise private variables used to compute the measured displacement
+ q_prev_getdis.resize(6);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+ // Initialize the firewire connection
+ Try( InitializeConnection(verbose_) );
+
+ // Connect to the servoboard using the servo board GUID
+ Try( InitializeNode_Afma6() );
+
+ Try( PrimitiveRESET_Afma6() );
+
+ // Update the eMc matrix in the low level controller
+ init(vpAfma6::defaultTool);
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // Print the robot status
+ if (verbose_) {
+ std::cout << "Robot status: ";
+ switch(EStopStatus) {
+ case ESTOP_AUTO:
+ case ESTOP_MANUAL:
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+ case ESTOP_ACTIVATED:
+ std::cout << "Emergency stop is activated" << std::endl;
+ break;
+ default:
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ }
+ std::cout << std::endl;
+ }
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
+// for (unsigned int i=0; i < njoint; i++) {
+// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
+// }
+
+ // If an error occur in the low level controller, goto here
+ //CatchPrint();
+ Catch();
+
+ // Test if an error occurs
+ if (TryStt == -20001)
+ printf("No connection detected. Check if the robot is powered on \n"
+ "and if the firewire link exist between the MotionBlox and this computer.\n");
+ else if (TryStt == -675)
+ printf(" Timeout enabling power...\n");
+
+ if (TryStt < 0) {
+ // Power off the robot
+ PrimitivePOWEROFF_Afma6();
+ // Free allocated resources
+ ShutDownConnection();
+
+ std::cout << "Cannot open connexion with the motionblox..." << std::endl;
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+ return ;
+}
+
+/*!
+
+ Initialize the robot kinematics with the extrinsic calibration
+ parameters associated to a specific camera (set the eMc homogeneous
+ parameters in the low level controller) and also get the joint
+ limits from the low-level controller.
+
+ The eMc parameters depend on the camera and the projection model in use.
+
+ \param tool : Tool to use.
+
+ \param projModel : Projection model associated to the camera.
+
+ To set the extrinsic camera parameters related to the eMc matrix
+ obtained with a camera perspective projection model including the
+ distorsion, use the code below:
+
+ \code
+ vpRobotAfma6 robot;
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters of the image \e I
+ acquired with the camera, with:
+
+ \code
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+ \endcode
+
+ \sa vpCameraParameters, init()
+*/
+void
+vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel)
+{
+
+ InitTry;
+ // Read the robot constants from files
+ // - joint [min,max], coupl_56, long_56
+ // - camera extrinsic parameters relative to eMc
+ vpAfma6::init(tool, projModel);
+
+ // Set the robot constant (coupl_56, long_56) in the MotionBlox
+ Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
+
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i=0; i < 3; i ++) {
+ eMc_pose[i] = _etc[i]; // translation in meters
+ eMc_pose[i+3] = _erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
+
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
+// for (unsigned int i=0; i < njoint; i++) {
+// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
+// }
+
+ setToolType(tool);
+
+ CatchPrint();
+ return ;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- DESTRUCTOR --------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Destructor.
+
+ Free allocated resources.
+*/
+vpRobotAfma6::~vpRobotAfma6 (void)
+{
+ InitTry;
+
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+// if (HIPowerStatus == 1) {
+// fprintf(stdout, "Power OFF the robot\n");
+// fflush(stdout);
+
+// Try( PrimitivePOWEROFF_Afma6() );
+// }
+
+ // Free allocated resources
+ ShutDownConnection();
+
+ vpRobotAfma6::robotAlreadyCreated = false;
+
+ CatchPrint();
+ return;
+}
+
+
+
+
+/*!
+
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ InitTry;
+
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ if (vpRobot::STATE_STOP != getRobotState ()) {
+ Try( PrimitiveSTOP_Afma6() );
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try( PrimitiveSTOP_Afma6() );
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ default:
+ break ;
+ }
+
+ CatchPrint();
+
+ return vpRobot::setRobotState (newState);
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- STOP --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Stop the robot and set the robot state to vpRobot::STATE_STOP.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+*/
+void
+vpRobotAfma6::stopMotion(void)
+{
+ InitTry;
+ Try( PrimitiveSTOP_Afma6() );
+ setRobotState (vpRobot::STATE_STOP);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot stop robot motion");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot stop robot motion.");
+ }
+}
+
+/*!
+
+ Power on the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot power on.
+
+ \sa powerOff(), getPowerState()
+*/
+void
+vpRobotAfma6::powerOn(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ bool firsttime = true;
+ unsigned int nitermax = 10;
+
+ for (unsigned int i=0; i<nitermax; i++) {
+ Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ if (EStopStatus == ESTOP_AUTO) {
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_MANUAL) {
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_ACTIVATED) {
+ if (firsttime) {
+ std::cout << "Emergency stop is activated! \n"
+ << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ firsttime = false;
+ }
+ fprintf(stdout, "Remaining time %ds \r", nitermax-i);
+ fflush(stdout);
+ CAL_Wait(1);
+ }
+ else {
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ ShutDownConnection();
+ exit(0);
+ }
+ }
+
+ if (EStopStatus == ESTOP_ACTIVATED)
+ std::cout << std::endl;
+
+ if (EStopStatus == ESTOP_ACTIVATED) {
+ std::cout << "Sorry, cannot power on the robot." << std::endl;
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power on the robot.");
+ }
+
+ if (HIPowerStatus == 0) {
+ fprintf(stdout, "Power ON the Afma6 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWERON_Afma6() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power on the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Power off the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+
+ \sa powerOn(), getPowerState()
+*/
+void
+vpRobotAfma6::powerOff(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ fprintf(stdout, "Power OFF the Afma6 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWEROFF_Afma6() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power off the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Get the robot power state indication if power is on or off.
+
+ \return true if power is on. false if power is off
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error.
+
+ \sa powerOn(), powerOff()
+*/
+bool
+vpRobotAfma6::getPowerState(void)
+{
+ InitTry;
+ bool status = false;
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ status = true;
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the power status");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get the power status.");
+ }
+ return status;
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+vpRobotAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ vpAfma6::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the geometric transformation between the camera frame and the
+ end-effector frame. This transformation is constant and correspond
+ to the extrinsic camera parameters estimated by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+vpRobotAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpAfma6::get_cMe(cMe) ;
+}
+
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame.
+
+ To compute eJe, we communicate with the low level controller to get
+ the articular joint position of the robot.
+
+ \param eJe : Robot jacobian expressed in the end-effector frame.
+
+*/
+void
+vpRobotAfma6::get_eJe(vpMatrix &eJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try
+ {
+ vpAfma6::get_eJe(q, eJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ To compute fJe, we communicate with the low level controller to get
+ the articular joint position of the robot.
+
+ \f[
+ {^f}J_e = \left(\begin{array}{cccccc}
+ 1 & 0 & 0 & -Ls4 & 0 & 0 \\
+ 0 & 1 & 0 & Lc4 & 0 & 0 \\
+ 0 & 0 & 1 & 0 & 0 & 0 \\
+ 0 & 0 & 0 & 0 & c4 & -s4c5 \\
+ 0 & 0 & 0 & 0 & s4 & c4c5 \\
+ 0 & 0 & 0 & 1 & 0 & s5 \\
+ \end{array}
+ \right)
+ \f]
+
+ \param fJe : Robot jacobian expressed in the reference frame.
+*/
+
+ void
+ vpRobotAfma6::get_fJe(vpMatrix &fJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try
+ {
+ vpAfma6::get_fJe(q, fJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Set the maximal velocity percentage to use for a position control.
+
+ The default positioning velocity is defined by
+ vpRobotAfma6::defaultPositioningVelocity. This method allows to
+ change this default positioning velocity
+
+ \param velocity : Percentage of the maximal velocity. Values should
+ be in ]0:100].
+
+ \code
+ vpColVector position(6);
+ position = 0; // position in meter and rad
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot to the joint position [0,0,0,0,0,0]
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
+ \endcode
+
+ \sa getPositioningVelocity()
+*/
+void
+vpRobotAfma6::setPositioningVelocity (const double velocity)
+{
+ positioningVelocity = velocity;
+}
+
+/*!
+ Get the maximal velocity percentage used for a position control.
+
+ \sa setPositioningVelocity()
+*/
+double
+vpRobotAfma6::getPositioningVelocity (void)
+{
+ return positioningVelocity;
+}
+
+/*!
+
+ Move the robot to an absolute cartesian position with a given percent of max
+ velocity. The percent of max velocity is to set with
+ setPositioningVelocity(). The position to reach can only be specified in
+ camera frame or in the reference frame. In joint, an exception is thrown.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pose : A six dimension pose vector corresponding to the position
+ to reach. The three first parameters are the translations in meter, the three
+ last parameters are the rotations expressed as a theta u vector in
+ radians. If the position is out of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the camera and the reference frame, rotations are
+ represented by a vpThetaUVector.
+
+ - Mixt frame of joint frame is not implemented.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
+ and vpRobot::ARTICULAR_FRAME not implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpPoseVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_AFMA6
+ vpPoseVector pose;
+ // Set positions in the reference frame
+ pose[0] = 0.1; // x axis, in meter
+ pose[1] = 0.; // y axis, in meter
+ pose[2] = 0.3; // z axis, in meter
+ pose[3] = M_PI/8; // ThetaU rotation around x axis, in rad
+ pose[4] = M_PI/4; // ThetaU rotation around y axis, in rad
+ pose[5] = 0.; // ThetaU rotation around z axis, in rad
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::REFERENCE_FRAME, pose);
+
+ return 0;
+#endif
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::REFERENCE_FRAME, pose);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ }
+ \endcode
+
+*/
+void
+vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpPoseVector & pose )
+
+{
+ vpColVector position(6);
+ vpRxyzVector rxyz;
+ vpRotationMatrix R;
+
+ R.buildFrom(pose[3], pose[4], pose[5]); //thetau
+ rxyz.buildFrom(R);
+
+ for (unsigned int i=0; i < 3; i++) {
+ position[i] = pose[i];
+ position[i+3] = rxyz[i];
+ }
+ if (frame == vpRobot::ARTICULAR_FRAME) {
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Joint frame not implemented for pose positionning.");
+ }
+ setPosition (frame, position);
+}
+/*!
+
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param position : A six dimension vector corresponding to the
+ position to reach. All the positions are expressed in meters for the
+ translations and radians for the rotations. If the position is out
+ of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X, Y, Z, A, B, C,
+ with X,Y,Z the translations, and A,B,C the rotations of the
+ end-effector.
+
+ - In the camera and the reference frame, rotations are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/robot/vpRobotException.h>
+
+int main()
+{
+#ifdef VISP_HAVE_AFMA6
+ vpColVector position(6);
+ // Set positions in the camera frame
+ position[0] = 0.1; // x axis, in meter
+ position[1] = 0.2; // y axis, in meter
+ position[2] = 0.3; // z axis, in meter
+ position[3] = M_PI/8; // rotation around x axis, in rad
+ position[4] = M_PI/4; // rotation around y axis, in rad
+ position[5] = M_PI/10;// rotation around z axis, in rad
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+
+ return 0;
+#endif
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ }
+ \endcode
+
+*/
+
+void
+vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & position )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ double _destination[6];
+ int error = 0;
+ double timestamp;
+
+ InitTry;
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ double _q[njoint];
+ Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
+
+ vpColVector q(njoint);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = _q[i];
+
+ // Get fMc from the inverse kinematics
+ vpHomogeneousMatrix fMc;
+ vpAfma6::get_fMc(q, fMc);
+
+ // Set cMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i+3];
+ }
+
+ // Compute cMc2
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
+
+ // Compute the new position to reach: fMc*cMc2
+ vpHomogeneousMatrix fMc2 = fMc * cMc2;
+
+ // Compute the corresponding joint position from the inverse kinematics
+ bool nearest = true;
+ int solution = this->getInverseKinematics(fMc2, q, nearest);
+ if (solution) { // Position is reachable
+ for (unsigned int i=0; i < njoint; i ++) {
+ _destination[i] = q[i];
+ }
+ Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
+ Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
+ }
+ else {
+ // Cartesian position is out of range
+ error = -1;
+ }
+
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ for (unsigned int i=0; i < njoint; i ++) {
+ _destination[i] = position[i];
+ }
+ Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
+ Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
+ break ;
+
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ // Set fMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i+3];
+ }
+ // Compute fMc from the input position
+ vpRotationMatrix fRc(rxyz);
+ vpHomogeneousMatrix fMc(txyz, fRc);
+
+ // Compute the corresponding joint position from the inverse kinematics
+ vpColVector q(6);
+ bool nearest = true;
+ int solution = this->getInverseKinematics(fMc, q, nearest);
+ if (solution) { // Position is reachable
+ for (unsigned int i=0; i < njoint; i ++) {
+ _destination[i] = q[i];
+ }
+ Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
+ Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
+ }
+ else {
+ // Cartesian position is out of range
+ error = -1;
+ }
+
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt == InvalidPosition || TryStt == -1023)
+ std::cout << " : Position out of range.\n";
+ else if (TryStt < 0)
+ std::cout << " : Unknown error (see Fabien).\n";
+ else if (error == -1)
+ std::cout << "Position out of range.\n";
+
+ if (TryStt < 0 || error < 0) {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+
+ return ;
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ This method owerloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
+ the position to reach. All the positions are expressed in meters for
+ the translations and radians for the rotations.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X (pos1), Y (pos2),
+ Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
+ translations, and A,B,C the rotations of the end-effector.
+
+ - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotAfma6.h>
+
+int main()
+{
+#ifdef VISP_HAVE_AFMA6
+ // Set positions in the camera frame
+ double pos1 = 0.1; // x axis, in meter
+ double pos2 = 0.2; // y axis, in meter
+ double pos3 = 0.3; // z axis, in meter
+ double pos4 = M_PI/8; // rotation around x axis, in rad
+ double pos5 = M_PI/4; // rotation around y axis, in rad
+ double pos6 = M_PI; // rotation around z axis, in rad
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
+
+ return 0;
+#endif
+}
+ \endcode
+
+ \sa setPosition()
+*/
+void vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6)
+{
+ try{
+ vpColVector position(6) ;
+ position[0] = pos1 ;
+ position[1] = pos2 ;
+ position[2] = pos3 ;
+ position[3] = pos4 ;
+ position[4] = pos5 ;
+ position[5] = pos6 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_AFMA6
+ vpRobotAfma6 robot;
+ vpColVector q; // joint position
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ return 0;
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+
+*/
+void vpRobotAfma6::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the articular
+ position of each dof, first the 3 translations, then the 3
+ articular rotation positions represented by a vpRxyzVector.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Time in second since last robot power on.
+
+ \code
+ vpRobotAfma6 robot;
+ vpColVector position;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, position);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame pose in terms of a homogeneous matrix
+ vpHomogeneousMatrix fMc(ftc, fRc);
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the position cannot
+ be get from the low level controller.
+
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const
+ vpColVector & r)
+
+*/
+void
+vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector & position, double ×tamp)
+{
+
+ InitTry;
+
+ position.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME : {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ double _q[njoint];
+ Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
+ for (unsigned int i=0; i < njoint; i ++) {
+ position[i] = _q[i];
+ }
+
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ double _q[njoint];
+ Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
+
+ vpColVector q(njoint);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = _q[i];
+
+ // Compute fMc
+ vpHomogeneousMatrix fMc;
+ vpAfma6::get_fMc(q, fMc);
+
+ // From fMc extract the pose
+ vpRotationMatrix fRc;
+ fMc.extract(fRc);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(fRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ position[i] = fMc[i][3]; // translation x,y,z
+ position[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+ break ;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get position.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position.");
+ }
+
+ return;
+}
+/*!
+ Returns the robot controller current time (in second) since last robot power on.
+*/
+double vpRobotAfma6::getTime() const
+{
+ double timestamp;
+ PrimitiveACQ_TIME_Afma6(×tamp);
+ return timestamp;
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+
+ Get the current position of the robot.
+ Similar as :
+ void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position)
+ as the difference the position is returned using a ThetaU representation.
+
+*/
+void
+vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position, double ×tamp)
+{
+ vpColVector posRxyz;
+ //recupere position en Rxyz
+ this->getPosition(frame, posRxyz, timestamp);
+ vpRxyzVector RxyzVect;
+ for(unsigned int j=0;j<3;j++)
+ RxyzVect[j]=posRxyz[j+3];
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(RxyzVect);
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for(unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in articular, camera frame, reference frame or mixt frame.
+
+ \param vel : Velocity vector. Translation velocities are expressed
+ in m/s while rotation velocities in rad/s. The size of this vector
+ is always 6.
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
+ camera frame.
+
+ - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
+ reference frame.
+
+ - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
+ v_y, v_z \f$ are expressed in the reference frame and rotations \f$
+ \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // X axis, in meter/s
+ q_dot[1] = 0.2; // Y axis, in meter/s
+ q_dot[2] = 0.3; // Z axis, in meter/s
+ q_dot[3] = M_PI/8; // A axis, in rad/s
+ q_dot[4] = M_PI/4; // B axis, in rad/s
+ q_dot[5] = M_PI/16;// C axis, in rad/s
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+ \endcode
+*/
+void
+vpRobotAfma6::setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ vpColVector vel_max(6);
+
+ for (unsigned int i=0; i<3; i++)
+ vel_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i=3; i<6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+
+ vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+
+ InitTry;
+
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6) );
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
+ Try( PrimitiveMOVESPEED_Afma6(vel_sat.data) );
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6) );
+ break ;
+ }
+ case vpRobot::MIXT_FRAME : {
+ Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6) );
+ break ;
+ }
+ default: {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
+ }
+
+ Catch();
+ if (TryStt < 0) {
+ if (TryStt == VelStopOnJoint) {
+ Int32 axisInJoint[njoint];
+ PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
+ for (unsigned int i=0; i < njoint; i ++) {
+ if (axisInJoint[i])
+ std::cout << "\nWarning: Velocity control stopped: axis "
+ << i+1 << " on joint limit!" <<std::endl;
+ }
+ }
+ else {
+ printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
+ if (TryString != NULL) {
+ // The statement is in TryString, but we need to check the validity
+ printf(" Error sentence %s\n", TryString); // Print the TryString
+ }
+ else {
+ printf("\n");
+ }
+ }
+ }
+
+ return;
+}
+
+
+
+
+
+
+/* ------------------------------------------------------------------------ */
+/* --- GET ---------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param velocity : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \warning The first time this method is called, \e velocity is set to 0. The
+ first call is used to intialise the velocity computation for the next call.
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // X axis, in meter/s
+ q_dot[1] = 0.2; // Y axis, in meter/s
+ q_dot[2] = 0.3; // Z axis, in meter/s
+ q_dot[3] = M_PI/8; // A axis, in rad/s
+ q_dot[4] = M_PI/4; // B axis, in rad/s
+ q_dot[5] = M_PI/16;// C axis, in rad/s
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ vpColVector q_dot_mes; // Measured velocities
+
+ // Initialisation of the velocity measurement
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ double timestamp;
+ while (1) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+ \endcode
+*/
+void
+vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp)
+{
+ velocity.resize (6);
+ velocity = 0;
+
+ double q[6];
+ vpColVector q_cur(6);
+ vpHomogeneousMatrix fMc_cur;
+ vpHomogeneousMatrix cMc; // camera displacement
+ double time_cur;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
+ time_cur = timestamp;
+ for (unsigned int i=0; i < njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ // Get the camera pose from the direct kinematics
+ vpAfma6::get_fMc(q_cur, fMc_cur);
+
+ if ( ! first_time_getvel ) {
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
+
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the ThetaU representation for the rotation
+ vpRotationMatrix cRc;
+ cMc.extract(cRc);
+ vpThetaUVector thetaU;
+ thetaU.buildFrom(cRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ // Compute the translation displacement in the reference frame
+ velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
+ // Update the rotation displacement in the camera frame
+ velocity[i+3] = thetaU[i];
+ }
+
+ // Compute the velocity
+ velocity /= (time_cur - time_prev_getvel);
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getvel = false;
+ }
+
+ // Memorize the camera pose for the next call
+ fMc_prev_getvel = fMc_cur;
+
+ // Memorize the joint position for the next call
+ q_prev_getvel = q_cur;
+
+ // Memorize the time associated to the joint position for the next call
+ time_prev_getvel = time_cur;
+
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
+ except that the timestamp is not returned.
+
+ */
+void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity)
+{
+ double timestamp;
+ getVelocity(frame, velocity, timestamp);
+}
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+ // Set joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // X axis, in meter/s
+ q_dot[1] = 0.2; // Y axis, in meter/s
+ q_dot[2] = 0.3; // Z axis, in meter/s
+ q_dot[3] = M_PI/8; // A axis, in rad/s
+ q_dot[4] = M_PI/4; // B axis, in rad/s
+ q_dot[5] = M_PI/16;// C axis, in rad/s
+
+ vpRobotAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ vpColVector q_dot_mes; // Measured velocities
+ double timestamp;
+ while (1) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+ \endcode
+*/
+vpColVector
+vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ vpColVector velocity;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
+ except that the timestamp is not returned.
+
+ */
+vpColVector vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector velocity;
+ double timestamp;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+Read joint positions in a specific Afma6 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#AFMA6 - Position - Version 2.01
+# file: "myposition.pos "
+#
+# R: X Y Z A B C
+# Joint position: X, Y, Z: translations in meters
+# A, B, C: rotations in degrees
+#
+
+R: 0.1 0.3 -0.25 -80.5 80 0
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are
+expressed in meters, while joint rotations A,B,C in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+vpRobotAfma6 robot;
+vpColVector q; // Joint position
+robot.readPosFile("myposition.pos", q); // Set the joint position from the file
+robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+\endcode
+
+\sa savePosFile()
+*/
+
+bool
+vpRobotAfma6::readPosFile(const char *filename, vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+// else
+// return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ return (false); /* fin fichier */
+ }
+
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(njoint);
+ sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2],
+ &q[3], &q[4], &q[5]);
+
+ // converts rotations from degrees into radians
+ for (unsigned int i=3; i < njoint; i ++)
+ q[i] = vpMath::rad(q[i]) ;
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Save joint (articular) positions in a specific Afma6 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : Joint positions [X,Y,Z,A,B,C] to save in the
+ filename. Translations X,Y,Z are expressed in meters, while
+ rotations A,B,C in radians.
+
+ \warning The joint rotations A,B,C written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+
+bool
+vpRobotAfma6::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#AFMA6 - Position - Version 2.01\n\
+#\n\
+# R: X Y Z A B C\n\
+# Joint position: X, Y, Z: translations in meters\n\
+# A, B, C: rotations in degrees\n\
+#\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
+ q[0],
+ q[1],
+ q[2],
+ vpMath::deg(q[3]),
+ vpMath::deg(q[4]),
+ vpMath::deg(q[5]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Moves the robot to the joint position specified in the filename. The
+ positioning velocity is set to 10% of the robot maximal velocity.
+
+ \param filename : File containing a joint position to reach.
+
+ \sa readPosFile(), move(const char *, const double)
+
+*/
+void
+vpRobotAfma6::move(const char *filename)
+{
+ vpColVector q;
+
+ this->readPosFile(filename, q) ;
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->setPositioningVelocity(10);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+}
+
+/*!
+
+ Moves the robot to the joint position specified in the \e filename with a
+ specified positioning \e velocity.
+
+ \param filename : File containing a joint position to reach.
+ \param velocity : Percentage of the maximal velocity. Values should
+ be in ]0:100].
+
+ \sa readPosFile(), move(const char *)
+
+*/
+void
+vpRobotAfma6::move(const char *filename, const double velocity)
+{
+ vpColVector q;
+
+ this->readPosFile(filename, q) ;
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->setPositioningVelocity(velocity);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+}
+
+/*!
+
+ Open the pneumatic CCMOP gripper.
+
+ \sa closeGripper()
+*/
+void
+vpRobotAfma6::openGripper()
+{
+ InitTry;
+ Try( PrimitiveGripper_Afma6(1) );
+ std::cout << "Open the gripper..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot open the gripper");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot open the gripper.");
+ }
+}
+
+/*!
+
+ Close the pneumatic CCMOP gripper.
+
+ \sa openGripper()
+
+*/
+void
+vpRobotAfma6::closeGripper()
+{
+ InitTry;
+ Try( PrimitiveGripper_Afma6(0) );
+ std::cout << "Close the gripper..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot close the gripper");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot close the gripper.");
+ }
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+vpRobotAfma6::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+/*!
+
+ Get the robot articular displacement since the last call of this method.
+
+ \param displacement : The measured articular displacement. The
+ dimension of \e displacement is 6 (the number of axis of the
+ robot). Translations are expressed in meters, rotations in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotAfma6::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ reference and mixt frames. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME) and in the camera frame (vpRobot::CAMERA_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call
+ of this method. The dimension of \e displacement is always
+ 6. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+
+ double q[6];
+ vpColVector q_cur(6);
+ vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
+ double timestamp;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
+ for (unsigned int i=0; i < njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ // Compute the camera pose in the reference frame
+ fMc_cur = get_fMc(q_cur);
+
+ if ( ! first_time_getdis ) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Compute the camera dispacement from the previous pose
+ c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
+
+ vpTranslationVector t;
+ vpRotationMatrix R;
+ c_prevMc_cur.extract(t);
+ c_prevMc_cur.extract(R);
+
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(R);
+
+ for (unsigned int i=0; i<3; i++) {
+ displacement[i] = t[i];
+ displacement[i+3] = rxyz[i];
+ }
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+
+ // Memorize the pose for the next call
+ fMc_prev_getdis = fMc_cur;
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Test the joints of the robot to detect if one or more is at its limit.
+
+ \param jointsStatus : A vector (size 6) of the status of the joints. For each
+ joint, the value is equal to 1 if the joint is at its maximal limit, -1 if the
+ joint is at its minimal value and 0 otherwise.
+
+ \return false if at least one joint is at one of its limit.
+*/
+bool
+vpRobotAfma6::checkJointLimits(vpColVector& jointsStatus)
+{
+ Int32 axisInJoint[njoint];
+ bool status = true;
+ jointsStatus.resize(6);
+ InitTry;
+
+ Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
+NULL));
+ for (unsigned int i=0; i < njoint; i ++) {
+ if (axisInJoint[i]){
+ std::cout << "\nWarning: Velocity control stopped: axis "
+ << i+1 << " on joint limit!" <<std::endl;
+ jointsStatus[i] = axisInJoint[i];
+ status = false;
+ }
+ else{
+ jointsStatus[i] = 0;
+ }
+ }
+
+ Catch();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot check joint limits.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot check joint limits.");
+ }
+
+ return status;
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no symbols
+void dummy_vpRobotAfma6() {};
+#endif
+
diff --git a/modules/robot/src/real-robot/biclops/vpBiclops.cpp b/modules/robot/src/real-robot/biclops/vpBiclops.cpp
new file mode 100644
index 0000000..d8f3271
--- /dev/null
+++ b/modules/robot/src/real-robot/biclops/vpBiclops.cpp
@@ -0,0 +1,451 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/* ----------------------------------------------------------------------- */
+/* --- INCLUDE ----------------------------------------------------------- */
+/* ----------------------------------------------------------------------- */
+
+
+
+#include <visp3/robot/vpBiclops.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpMath.h>
+#include <math.h>
+
+
+/* ------------------------------------------------------------------------ */
+/* --- COMPUTE ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
+const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
+const float vpBiclops::panJointLimit = (float)(M_PI); /*!< Pan range (in rad): from -panJointLimit to + panJointLimit */
+const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5); /*!< Tilt range (in rad): from -tiltJointLimit to + tiltJointLimit */
+
+const float vpBiclops::speedLimit = (float)(M_PI/3.0); /*!< Maximum speed (in rad/s) to perform a displacement */
+
+/*!
+ Compute the direct geometric model of the camera: fMc
+
+ \warning Provided for compatibilty with previous versions. Use rather
+ get_fMc(const vpColVector &, vpHomogeneousMatrix &).
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fMc : Homogeneous matrix corresponding to the direct geometric model
+ of the camera. Describes the transformation between the robot reference frame
+ (called fixed) and the camera frame.
+
+ \sa get_fMc(const vpColVector &, vpHomogeneousMatrix &)
+*/
+void
+vpBiclops::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
+{
+ vpHomogeneousMatrix fMe = get_fMe(q);
+ fMc = fMe * cMe_.inverse();
+
+ vpCDEBUG (6) << "camera position: " << std::endl << fMc;
+
+ return ;
+}
+
+/*!
+ Compute the direct geometric model of the camera: fMc
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fMc : Homogeneous matrix corresponding to the direct geometric model
+ of the camera. Describes the transformation between the robot reference frame
+ (called fixed) and the camera frame.
+
+*/
+void
+vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
+{
+ vpHomogeneousMatrix fMe = get_fMe(q);
+ fMc = fMe * cMe_.inverse();
+
+ vpCDEBUG (6) << "camera position: " << std::endl << fMc;
+
+ return ;
+}
+
+/*!
+ Return the direct geometric model of the camera: fMc
+
+ \warning Provided for compatibilty with previous versions. Use rather
+ get_fMc(const vpColVector &).
+
+ \param q : Articular position for pan and tilt axis.
+
+ \return fMc, the homogeneous matrix corresponding to the direct geometric
+ model of the camera. Describes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
+
+ \sa get_fMc(const vpColVector &)
+*/
+vpHomogeneousMatrix
+vpBiclops::computeMGD (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+
+ computeMGD (q, fMc);
+
+ return fMc;
+}
+
+/*!
+ Return the direct geometric model of the camera: fMc
+
+ \param q : Articular position for pan and tilt axis.
+
+ \return fMc, the homogeneous matrix corresponding to the direct geometric
+ model of the camera. Discribes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
+
+*/
+vpHomogeneousMatrix
+vpBiclops::get_fMc (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+
+ get_fMc (q, fMc);
+
+ return fMc;
+}
+
+/*!
+ Return the direct geometric model of the end effector: fMe
+
+ \param q : Articular position for pan and tilt axis.
+
+ \return fMe, the homogeneous matrix corresponding to the direct geometric
+ model of the end effector. Describes the transformation between the robot reference
+ frame (called fixed) and the end effector frame.
+
+*/
+vpHomogeneousMatrix
+vpBiclops::get_fMe (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMe;
+
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for biclops articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
+ }
+
+ double q1 = q[0]; // pan
+ double q2 = q[1]; // tilt
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
+
+ if (dh_model_ == DH1)
+ {
+ fMe[0][0] = -c1*s2;
+ fMe[0][1] = -s1;
+ fMe[0][2] = c1*c2;
+ fMe[0][3] = 0;
+
+ fMe[1][0] = -s1*s2;
+ fMe[1][1] = c1;
+ fMe[1][2] = s1*c2;
+ fMe[1][3] = 0;
+
+ fMe[2][0] = -c2;
+ fMe[2][1] = 0;
+ fMe[2][2] = -s2;
+ fMe[2][3] = 0;
+
+ fMe[3][0] = 0;
+ fMe[3][1] = 0;
+ fMe[3][2] = 0;
+ fMe[3][3] = 1;
+ }
+ else
+ {
+ fMe[0][0] = c1*s2;
+ fMe[0][1] = -s1;
+ fMe[0][2] = c1*c2;
+ fMe[0][3] = 0;
+
+ fMe[1][0] = s1*s2;
+ fMe[1][1] = c1;
+ fMe[1][2] = s1*c2;
+ fMe[1][3] = 0;
+
+ fMe[2][0] = -c2;
+ fMe[2][1] = 0;
+ fMe[2][2] = s2;
+ fMe[2][3] = 0;
+
+ fMe[3][0] = 0;
+ fMe[3][1] = 0;
+ fMe[3][2] = 0;
+ fMe[3][3] = 1;
+ }
+
+ return fMe;
+}
+
+/*!
+ Compute the direct geometric model of the camera in terms of pose vector.
+
+ \warning Provided for compatibilty with previous versions. Use rather
+ get_fMc(const vpColVector &, vpPoseVector &).
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fvc : Pose vector corresponding to the transformation between the robot
+ reference frame (called fixed) and the camera frame.
+
+ \sa get_fMc(const vpColVector &, vpPoseVector &)
+*/
+
+void
+vpBiclops::computeMGD (const vpColVector &q, vpPoseVector &fvc) const
+{
+ vpHomogeneousMatrix fMc;
+
+ get_fMc (q, fMc);
+ fvc.buildFrom(fMc.inverse());
+
+ return ;
+}
+
+/*!
+ Compute the direct geometric model of the camera in terms of pose vector.
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fvc : Pose vector corresponding to the transformation between the robot
+ reference frame (called fixed) and the camera frame.
+
+*/
+
+void
+vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc) const
+{
+ vpHomogeneousMatrix fMc;
+
+ get_fMc (q, fMc);
+ fvc.buildFrom(fMc.inverse());
+
+ return ;
+}
+
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Default construtor. Call init().
+
+*/
+vpBiclops::vpBiclops (void)
+ : dh_model_(DH1), cMe_()
+{
+ init();
+}
+/* ---------------------------------------------------------------------- */
+/* --- PRIVATE ---------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+/*!
+ Initialization.
+ Set the default \f${^c}{\bf M}_e\f$ transformation.
+
+*/
+void
+vpBiclops::init ()
+{
+ dh_model_ = DH1;
+ set_cMe();
+ return ;
+}
+
+
+/* ----------------------------------------------------------------------- */
+/* --- DISPLAY ----------------------------------------------------------- */
+/* ----------------------------------------------------------------------- */
+
+VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & /*constant*/)
+{
+ os
+ << "Geometric parameters: " << std::endl
+ << "h: "
+ << "\t" << vpBiclops::h << std::endl;
+
+ return os;
+}
+
+
+/*!
+
+ Get the twist matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cVe : Twist transformation between camera and end effector frame to
+ expess a velocity skew from end effector frame in camera frame.
+
+*/
+void
+vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ cVe.buildFrom(cMe_) ;
+}
+
+/*!
+
+ Set the default homogeneous matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+*/
+void
+vpBiclops::set_cMe()
+{
+ vpHomogeneousMatrix eMc ;
+
+ eMc[0][0] = 0;
+ eMc[0][1] = -1;
+ eMc[0][2] = 0;
+ eMc[0][3] = h;
+
+ eMc[1][0] = 1;
+ eMc[1][1] = 0;
+ eMc[1][2] = 0;
+ eMc[1][3] = 0;
+
+ eMc[2][0] = 0;
+ eMc[2][1] = 0;
+ eMc[2][2] = 1;
+ eMc[2][3] = 0;
+
+ eMc[3][0] = 0;
+ eMc[3][1] = 0;
+ eMc[3][2] = 0;
+ eMc[3][3] = 1;
+
+ cMe_ = eMc.inverse() ;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ \warning Re is not the embedded camera frame. It corresponds to the frame
+ associated to the tilt axis (see also get_cMe).
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param eJe : Jacobian between end effector frame and end effector frame (on
+ tilt axis).
+
+*/
+void
+vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+{
+ eJe.resize(6,2) ;
+
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for biclops articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
+ }
+
+ double s2 = sin(q[1]) ;
+ double c2 = cos(q[1]) ;
+
+ eJe = 0;
+
+ if (dh_model_ == DH1)
+ {
+ eJe[3][0] = -c2;
+ eJe[4][1] = 1;
+ eJe[5][0] = -s2;
+ }
+ else
+ {
+ eJe[3][0] = -c2;
+ eJe[4][1] = -1;
+ eJe[5][0] = s2;
+
+ }
+
+}
+/*!
+ Get the robot jacobian expressed in the robot reference frame
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fJe : Jacobian between reference frame (or fix frame) and end effector
+ frame (on tilt axis).
+
+*/
+
+void
+vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+{
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for biclops articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
+ }
+
+ fJe.resize(6,2) ;
+
+ double s1 = sin(q[0]) ;
+ double c1 = cos(q[0]) ;
+
+ fJe = 0;
+
+ if (dh_model_ == DH1)
+ {
+ fJe[3][1] = -s1;
+ fJe[4][1] = c1;
+ fJe[5][0] = 1;
+ }
+ else
+ {
+ fJe[3][1] = s1;
+ fJe[4][1] = -c1;
+ fJe[5][0] = 1;
+ }
+}
diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp
new file mode 100644
index 0000000..48f4d3c
--- /dev/null
+++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp
@@ -0,0 +1,1423 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <signal.h>
+#include <errno.h>
+#include <string.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#include <visp3/core/vpTime.h>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_BICLOPS
+
+#include <visp3/robot/vpBiclops.h>
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpExponentialMap.h>
+
+//#define VP_DEBUG // Activate the debug mode
+//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
+#include <visp3/core/vpDebug.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+
+bool vpRobotBiclops::robotAlreadyCreated = false;
+const double vpRobotBiclops::defaultPositioningVelocity = 10.0;
+
+static pthread_mutex_t vpEndThread_mutex;
+static pthread_mutex_t vpShm_mutex;
+static pthread_mutex_t vpMeasure_mutex;
+
+/* ----------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+
+/*!
+
+ Default constructor.
+
+ Does nothing more than setting the default configuration file
+ to /usr/share/BiclopsDefault.cfg.
+
+ As shown in the following example,the turret need to be initialized
+ using init() function.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotBiclops.h>
+
+int main()
+{
+#ifdef VISP_HAVE_BICLOPS
+ vpRobotBiclops robot; // Use the default config file in /usr/share/BiclopsDefault.cfg"
+
+ // Specify the config file location
+ robot.setConfigFile("/usr/share/BiclopsDefault.cfg"); // Not mandatory since the file is the default one
+
+ // Initialize the head
+ robot.init();
+
+ // Move the robot to a specified pan and tilt
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ vpColVector q(2);
+ q[0] = vpMath::rad(20); // pan
+ q[1] = vpMath::rad(40); // tilt
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+ return 0;
+}
+ \endcode
+
+*/
+vpRobotBiclops::vpRobotBiclops ()
+ : vpBiclops(), vpRobot(), control_thread(), controller(),
+ positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
+{
+ vpDEBUG_TRACE (12, "Begin default constructor.");
+
+ vpRobotBiclops::robotAlreadyCreated = false;
+ setConfigFile("/usr/share/BiclopsDefault.cfg");
+
+ // Initialize the mutex dedicated to she shm protection
+ pthread_mutex_init (&vpShm_mutex, NULL);
+ pthread_mutex_init (&vpEndThread_mutex, NULL);
+ pthread_mutex_init (&vpMeasure_mutex, NULL);
+
+ control_thread = 0;
+}
+
+/*!
+
+ Default constructor.
+
+ Initialize the biclops pan, tilt head by reading the
+ configuration file provided by Traclabs
+ and do the homing sequence.
+
+ The following example shows how to use the constructor.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotBiclops.h>
+
+int main()
+{
+#ifdef VISP_HAVE_BICLOPS
+ // Specify the config file location and initialize the turret
+ vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg");
+
+ // Move the robot to a specified pan and tilt
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+
+ vpColVector q(2);
+ q[0] = vpMath::rad(-20); // pan
+ q[1] = vpMath::rad(10); // tilt
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+ return 0;
+}
+ \endcode
+
+*/
+vpRobotBiclops::vpRobotBiclops (const char * filename)
+ : vpBiclops(), vpRobot(), control_thread(), controller(),
+ positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
+{
+ vpDEBUG_TRACE (12, "Begin default constructor.");
+
+ vpRobotBiclops::robotAlreadyCreated = false;
+ setConfigFile(filename);
+
+ // Initialize the mutex dedicated to she shm protection
+ pthread_mutex_init (&vpShm_mutex, NULL);
+ pthread_mutex_init (&vpEndThread_mutex, NULL);
+ pthread_mutex_init (&vpMeasure_mutex, NULL);
+
+ init();
+
+ return ;
+}
+
+/*!
+
+ Destructor.
+ Wait the end of the control thread.
+
+*/
+
+vpRobotBiclops::~vpRobotBiclops (void)
+{
+
+ vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+ pthread_mutex_unlock(&vpEndThread_mutex);
+
+ /* wait the end of the control thread */
+ int code;
+ vpDEBUG_TRACE (12, "Wait end of control thread");
+
+ if (controlThreadCreated == true) {
+ code = pthread_join(control_thread, NULL);
+ if (code != 0) {
+ vpCERROR << "Cannot terminate the control thread: " << code
+ << " strErr=" << strerror(errno)
+ << " strCode=" << strerror(code)
+ << std::endl;
+ }
+ }
+
+ pthread_mutex_destroy (&vpShm_mutex);
+ pthread_mutex_destroy (&vpEndThread_mutex);
+ pthread_mutex_destroy (&vpMeasure_mutex);
+
+ vpRobotBiclops::robotAlreadyCreated = false;
+
+ vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
+ return;
+}
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INITIALISATION ------------------------------------------------------ */
+/* ------------------------------------------------------------------------- */
+
+/*!
+
+ Set the Biclops config filename.
+
+*/
+void
+vpRobotBiclops::setConfigFile(const char * filename)
+{
+ sprintf(configfile, "%s", filename);
+}
+
+/*!
+
+ Set the Biclops config filename.
+ Check if the config file exists and initialize the head.
+
+ \exception vpRobotException::constructionError If the config file cannot be
+ oppened.
+
+*/
+void
+vpRobotBiclops::init ()
+{
+ // test if the config file exists
+ FILE *fd = fopen(configfile, "r");
+ if (fd == NULL) {
+ vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with biclops");
+ }
+ fclose(fd);
+
+ // Initialize the controller
+ controller.init(configfile);
+
+ try
+ {
+ setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+ vpRobotBiclops::robotAlreadyCreated = true;
+
+ // Initialize previous articular position to manage getDisplacement()
+ q_previous.resize(vpBiclops::ndof);
+ q_previous = 0;
+
+ controlThreadCreated = false;
+
+ return ;
+}
+
+/*
+ Control loop to manage the biclops joint limits in speed control.
+
+ This control loop is running in a seperate thread in order to detect each 5
+ ms joint limits during the speed control. If a joint limit is detected the
+ axis should be halted.
+
+ \warning Velocity control mode is not exported from the top-level Biclops API
+ class provided by Traclabs. That means that there is no protection in this
+ mode to prevent an axis from striking its hard limit. In position mode,
+ Traclabs put soft limits in that keep any command from driving to a position
+ too close to the hard limits. In velocity mode this protection does not exist
+ in the current API.
+
+ \warning With the understanding that hitting the hard limits at full
+ speed/power can damage the unit, damage due to velocity mode commanding is
+ under user responsibility.
+*/
+void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
+{
+ vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
+
+ int iter = 0;
+// PMDAxisControl *panAxis = controller->getPanAxis();
+// PMDAxisControl *tiltAxis = controller->getTiltAxis();
+ vpRobotBiclopsController::shmType shm;
+
+ vpDEBUG_TRACE(10, "Start control loop");
+ vpColVector mes_q;
+ vpColVector mes_q_dot;
+ vpColVector softLimit(vpBiclops::ndof);
+ vpColVector q_dot(vpBiclops::ndof);
+ bool *new_q_dot = new bool [ vpBiclops::ndof ];
+ bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
+ bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
+ bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
+ vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
+ double secure = vpMath::rad(2); // add a security angle before joint limit
+
+
+ // Set the soft limits
+ softLimit[0] = vpBiclops::panJointLimit - secure;
+ softLimit[1] = vpBiclops::tiltJointLimit - secure;
+ vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
+ vpMath::deg(softLimit[0]),
+ vpMath::deg(softLimit[1]));
+
+ // Initilisation
+ vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+ shm = controller->readShm();
+
+ vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ prev_q_dot [i] = shm.q_dot[i];
+ new_q_dot [i] = false;
+ change_dir [i] = false;
+ force_halt [i] = false;
+ enable_limit[i] = true;
+ }
+
+ // Initialize actual position and velocity
+ mes_q = controller->getActualPosition();
+ mes_q_dot = controller->getActualVelocity();
+
+ vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+ shm = controller->readShm();
+ // Updates the shm
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ shm.actual_q[i] = mes_q[i];
+ shm.actual_q_dot[i] = mes_q_dot[i];
+ }
+ // Update the actuals positions
+ controller->writeShm(shm);
+
+ vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
+ pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
+
+ while (! controller->isStopRequested()) {
+
+ // Get actual position and velocity
+ mes_q = controller->getActualPosition();
+ mes_q_dot = controller->getActualVelocity();
+
+ vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+
+ shm = controller->readShm();
+
+ // Updates the shm
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ shm.actual_q[i] = mes_q[i];
+ shm.actual_q_dot[i] = mes_q_dot[i];
+ }
+
+ vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
+ vpMath::deg(mes_q[0]),
+ vpMath::deg(mes_q[1]));
+ vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
+ vpMath::deg(mes_q_dot[0]),
+ vpMath::deg(mes_q_dot[1]));
+ vpDEBUG_TRACE(12, "desired q_dot : %f %f",
+ vpMath::deg(shm.q_dot[0]),
+ vpMath::deg(shm.q_dot[1]));
+ vpDEBUG_TRACE(13, "previous q_dot : %f %f",
+ vpMath::deg(prev_q_dot[0]),
+ vpMath::deg(prev_q_dot[1]));
+
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ // test if joint limits are reached
+ if (mes_q[i] < -softLimit[i]) {
+ vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
+ shm.status[i] = vpRobotBiclopsController::STOP;
+ shm.jointLimit[i] = true;
+ }
+ else if (mes_q[i] > softLimit[i]) {
+ vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
+ shm.status[i] = vpRobotBiclopsController::STOP;
+ shm.jointLimit[i] = true;
+ }
+ else {
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ shm.jointLimit[i] = false;
+ }
+
+ // Test if new a speed is demanded
+ //if (shm.q_dot[i] != prev_q_dot[i])
+ if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
+ new_q_dot[i] = true;
+ else
+ new_q_dot[i] = false;
+
+ // Test if desired speed change of sign
+ if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
+ change_dir[i] = true;
+ else
+ change_dir[i] = false;
+
+ }
+ vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
+ vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
+ vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
+ vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
+ vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
+ vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
+
+
+ bool updateVelocity = false;
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ // Test if a new desired speed is to apply
+ if (new_q_dot[i]) {
+ // A new desired speed is to apply
+ if (shm.status[i] == vpRobotBiclopsController::STOP) {
+ // Axis in joint limit
+ if (change_dir[i] == false) {
+ // New desired speed without change of direction
+ // We go in the joint limit
+ if (enable_limit[i] == true) { // limit detection active
+ // We have to stop this axis
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ else {
+ // We have to apply the desired speed to go away the joint
+ // Update the desired speed
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ force_halt[i] = false;
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ else {
+ // New desired speed and change of direction.
+ if (enable_limit[i] == true) { // limit detection active
+ // Update the desired speed to go away the joint limit
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ force_halt[i] = false;
+ enable_limit[i] = false; // Disable joint limit detection
+ updateVelocity = true; // We have to send this new speed
+ }
+ else {
+ // We have to stop this axis
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ enable_limit[i] = true; // Joint limit detection must be active
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ }
+ }
+ else {
+ // Axis not in joint limit
+
+ // Update the desired speed
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ enable_limit[i] = true; // Joint limit detection must be active
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ else {
+ // No change of the desired speed. We have to stop the robot in case of
+ // joint limit
+ if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
+ if (enable_limit[i] == true) { // limit detection active
+
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ // We have to stop this axis
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ }
+ else {
+ // No need to stop the robot
+ enable_limit[i] = true; // Normal situation, activate limit detection
+ }
+ }
+ }
+ // Update the actuals positions
+ controller->writeShm(shm);
+
+ vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ if (updateVelocity) {
+ vpDEBUG_TRACE(12, "apply q_dot : %f %f",
+ vpMath::deg(q_dot[0]),
+ vpMath::deg(q_dot[1]));
+
+ // Apply the velocity
+ controller -> setVelocity( q_dot );
+ }
+
+
+ // Update the previous speed for next iteration
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++)
+ prev_q_dot[i] = shm.q_dot[i];
+
+ vpDEBUG_TRACE(12, "iter: %d", iter);
+
+ //wait 5 ms
+ vpTime::wait(5.0);
+
+// if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
+// vpDEBUG_TRACE (12, "Calling thread will end");
+// vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+// std::cout << "Calling thread will end" << std::endl;
+// std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
+//
+// pthread_mutex_unlock(&vpEndThread_mutex);
+// break;
+// }
+
+ iter ++;
+ }
+ controller->stopRequest(false);
+ // Stop the robot
+ vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
+ q_dot = 0;
+ controller -> setVelocity( q_dot );
+
+ delete [] new_q_dot;
+ delete [] change_dir;
+ delete [] force_halt;
+ delete [] enable_limit;
+ vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
+ pthread_mutex_unlock(&vpEndThread_mutex);
+
+ vpDEBUG_TRACE (10, "Exit control thread ");
+ // pthread_exit(0);
+
+ return NULL;
+}
+
+
+/*!
+
+ Change the state of the robot either to stop them, or to set position or
+ speed control.
+
+*/
+vpRobot::vpRobotStateType
+vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ switch (newState)
+ {
+ case vpRobot::STATE_STOP:
+ {
+ if (vpRobot::STATE_STOP != getRobotState ())
+ {
+ stopMotion();
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL:
+ {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
+ {
+ vpDEBUG_TRACE (12, "Speed to position control.");
+ stopMotion();
+ }
+
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL:
+ {
+
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
+ {
+ vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
+ pthread_mutex_lock(&vpEndThread_mutex);
+
+ vpDEBUG_TRACE (12, "Create speed control thread");
+ int code;
+ code = pthread_create(&control_thread, NULL,
+ &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop,
+ &controller);
+ if (code != 0) {
+ vpCERROR << "Cannot create speed biclops control thread: " << code
+ << " strErr=" << strerror(errno)
+ << " strCode=" << strerror(code)
+ << std::endl;
+ }
+
+ controlThreadCreated = true;
+
+ vpDEBUG_TRACE (12, "Speed control thread created");
+ }
+ break;
+ }
+ default:
+ break ;
+ }
+
+ return vpRobot::setRobotState (newState);
+}
+
+
+
+/*!
+
+ Halt all the axis.
+
+*/
+void
+vpRobotBiclops::stopMotion(void)
+{
+ vpColVector q_dot(vpBiclops::ndof);
+ q_dot = 0;
+ controller.setVelocity(q_dot);
+ //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
+ controller.stopRequest(true);
+}
+
+/*!
+
+ Get the twist matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cVe : Twist transformation between camera and end effector frame to
+ expess a velocity skew from end effector frame in camera frame.
+
+*/
+void
+vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ cMe = vpBiclops::get_cMe() ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the homogeneous matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cMe : Homogeneous matrix between camera and end effector frame.
+
+*/
+void
+vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ cMe = vpBiclops::get_cMe() ;
+}
+
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ \warning Re is not the embedded camera frame. It corresponds to the frame
+ associated to the tilt axis (see also get_cMe).
+
+ \param _eJe : Jacobian between end effector frame and end effector frame (on
+ tilt axis).
+
+*/
+void
+vpRobotBiclops::get_eJe(vpMatrix &_eJe)
+{
+ vpColVector q(2) ;
+ getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+
+ try
+ {
+ vpBiclops::get_eJe(q, _eJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+
+/*!
+ Get the robot jacobian expressed in the robot reference frame
+
+ \param _fJe : Jacobian between reference frame (or fix frame) and end effector
+ frame (on tilt axis).
+
+*/
+void
+vpRobotBiclops::get_fJe(vpMatrix &_fJe)
+{
+ vpColVector q(2) ;
+ getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+
+ try
+ {
+ vpBiclops::get_fJe(q,_fJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+
+}
+
+/*!
+
+ Set the velocity used for a position control.
+
+ \param velocity : Velocity in % of the maximum velocity between [0,100]. The
+ maximum velocity is given vpBiclops::speedLimit.
+*/
+void
+vpRobotBiclops::setPositioningVelocity (const double velocity)
+{
+ if (velocity < 0 || velocity > 100) {
+ vpERROR_TRACE("Bad positionning velocity");
+ throw vpRobotException (vpRobotException::constructionError,
+ "Bad positionning velocity");
+ }
+
+ positioningVelocity = velocity;
+}
+/*!
+ Get the velocity in % used for a position control.
+
+ \return Positionning velocity in [0, 100.0]. The
+ maximum positionning velocity is given vpBiclops::speedLimit.
+
+*/
+double
+vpRobotBiclops::getPositioningVelocity (void)
+{
+ return positioningVelocity;
+}
+
+
+/*!
+ Move the robot in position control.
+
+ \warning This method is blocking. That mean that it waits the end of the
+ positionning.
+
+ \param frame : Control frame. This biclops head can only be controlled in
+ articular.
+
+ \param q : The position to set for each axis in radians.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+void
+vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & q )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in camera frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in camera frame: "
+ "not implemented");
+ break;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in reference frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in reference frame: "
+ "not implemented");
+ break;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in mixt frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in mixt frame: "
+ "not implemented");
+ break;
+ case vpRobot::ARTICULAR_FRAME:
+ break ;
+ }
+
+ // test if position reachable
+// if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
+// (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
+// vpERROR_TRACE ("Positionning error.");
+// throw vpRobotException (vpRobotException::wrongStateError,
+// "Positionning error.");
+// }
+
+ vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
+ pthread_mutex_lock(&vpEndThread_mutex);
+ controller.setPosition( q, positioningVelocity );
+ vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+ pthread_mutex_unlock(&vpEndThread_mutex);
+ return ;
+}
+
+/*!
+ Move the robot in position control.
+
+ \warning This method is blocking. That mean that it wait the end of the
+ positionning.
+
+ \param frame : Control frame. This biclops head can only be controlled in
+ articular.
+
+ \param q1 : The pan position to set in radians.
+ \param q2 : The tilt position to set in radians.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+void vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
+ const double &q1, const double &q2)
+{
+ try{
+ vpColVector q(2) ;
+ q[0] = q1 ;
+ q[1] = q2 ;
+
+ setPosition(frame,q) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Read the content of the position file and moves to head to articular
+ position.
+
+ \param filename : Position filename
+
+ \exception vpRobotException::readingParametersError : If the articular
+ position cannot be read from file.
+
+ \sa readPositionFile()
+
+*/
+void
+vpRobotBiclops::setPosition(const char *filename)
+{
+ vpColVector q ;
+ if (readPositionFile(filename, q) == false) {
+ vpERROR_TRACE ("Cannot get biclops position from file");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Cannot get biclops position from file");
+ }
+ setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+}
+
+/*!
+
+ Return the position of each axis.
+ - In positionning control mode, call vpRobotBiclopsController::getPosition()
+ - In speed control mode, call vpRobotBiclopsController::getActualPosition()
+
+ \param frame : Control frame. This biclops head can only be controlled in
+ articular.
+
+ \param q : The position of the axis in radians.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+void
+vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector & q)
+{
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in camera frame: "
+ "not implemented");
+ break;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in reference frame: "
+ "not implemented");
+ break;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break;
+ case vpRobot::ARTICULAR_FRAME:
+ break ;
+ }
+
+ vpRobot::vpRobotStateType state;
+ state = vpRobot::getRobotState();
+
+ switch (state) {
+ case STATE_STOP:
+ case STATE_POSITION_CONTROL:
+ q = controller.getPosition();
+
+ break;
+ case STATE_VELOCITY_CONTROL:
+ case STATE_ACCELERATION_CONTROL:
+ default:
+ q.resize(vpBiclops::ndof);
+
+ vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
+ pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
+
+ vpRobotBiclopsController::shmType shm;
+
+ vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+ shm = controller.readShm();
+
+ vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ q[i] = shm.actual_q[i];
+ }
+
+ vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
+
+ vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
+ pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
+
+ break;
+
+ }
+}
+
+
+/*!
+
+ Send a velocity on each axis.
+
+ \param frame : Control frame. This biclops head can only be controlled in
+ articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference
+ frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are
+ not implemented.
+
+ \param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot
+ {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the
+ camera and \f$ \dot{q}_2\f$ the tilt of the camera.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME or vpRobot::MIXT_FRAME) is
+ given.
+
+ \warning Velocities could be saturated if one of them exceed the maximal
+ autorized speed (see vpRobot::maxRotationVelocity).
+
+*/
+void
+vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & q_dot)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the camera frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ if ( q_dot.getRows() != 2) {
+ vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Bad dimension for speed vector "
+ "in articular frame");
+ }
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the mixt frame:"
+ "functionality not implemented");
+ break ;
+ }
+ default:
+ {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot ");
+ }
+ }
+
+ vpDEBUG_TRACE (12, "Velocity limitation.");
+ bool norm = false; // Flag to indicate when velocities need to be nomalized
+
+ // Saturate articular speed
+ double max = vpBiclops::speedLimit;
+ vpColVector q_dot_sat(vpBiclops::ndof);
+
+ // init q_dot_saturated
+ q_dot_sat = q_dot;
+
+ for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
+ {
+ if (fabs (q_dot[i]) > max)
+ {
+ norm = true;
+ max = fabs (q_dot[i]);
+ vpERROR_TRACE ("Excess velocity: ROTATION "
+ "(axe nr.%d).", i);
+ }
+ }
+ // Rotations velocities normalisation
+ if (norm == true) {
+ max = vpBiclops::speedLimit / max;
+ q_dot_sat = q_dot * max;
+ }
+
+ vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
+
+ vpRobotBiclopsController::shmType shm;
+
+ vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+ shm = controller.readShm();
+
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++)
+ shm.q_dot[i] = q_dot[i];
+
+ controller.writeShm(shm);
+
+ vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ return;
+}
+
+
+/* ------------------------------------------------------------------------- */
+/* --- GET ----------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/*!
+
+ Get the articular velocity.
+
+ \param frame : Control frame. This head can only be controlled in articular.
+
+ \param q_dot : The measured articular velocity in rad/s.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+*/
+void
+vpRobotBiclops::getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & q_dot)
+{
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in camera frame: "
+ "not implemented");
+ break;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in reference frame: "
+ "not implemented");
+ break;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break;
+ case vpRobot::ARTICULAR_FRAME:
+ break ;
+ }
+
+ vpRobot::vpRobotStateType state;
+ state = vpRobot::getRobotState();
+
+ switch (state) {
+ case STATE_STOP:
+ case STATE_POSITION_CONTROL:
+ q_dot = controller.getVelocity();
+
+ break;
+ case STATE_VELOCITY_CONTROL:
+ case STATE_ACCELERATION_CONTROL:
+ default:
+ q_dot.resize(vpBiclops::ndof);
+
+ vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
+ pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
+
+ vpRobotBiclopsController::shmType shm;
+
+ vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ pthread_mutex_lock(&vpShm_mutex);
+
+ shm = controller.readShm();
+
+ vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ pthread_mutex_unlock(&vpShm_mutex);
+
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ q_dot[i] = shm.actual_q_dot[i];
+ }
+
+ vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
+
+ vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
+ pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
+
+ break;
+
+ }
+}
+
+
+/*!
+
+ Return the articular velocity.
+
+ \param frame : Control frame. This head can only be controlled in articular.
+
+ \return The measured articular velocity in rad/s.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+*/
+vpColVector
+vpRobotBiclops::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector q_dot;
+ getVelocity (frame, q_dot);
+
+ return q_dot;
+}
+
+/*!
+
+ Get an articular position from the position file.
+
+ \param filename : Position file.
+
+ \param q : The articular position read in the file.
+
+ \code
+ # Example of biclops position file
+ # The axis positions must be preceed by R:
+ # First value : pan articular position in degrees
+ # Second value: tilt articular position in degrees
+ R: 15.0 5.0
+ \endcode
+
+ \return true if a position was found, false otherwise.
+
+*/
+bool
+vpRobotBiclops::readPositionFile(const char *filename, vpColVector &q)
+{
+ FILE * pt_f ;
+ pt_f = fopen(filename,"r") ;
+
+ if (pt_f == NULL) {
+ vpERROR_TRACE ("Can not open biclops position file %s", filename);
+ return false;
+ }
+
+ char line[FILENAME_MAX];
+ char head[] = "R:";
+ bool end = false;
+
+ do {
+ // skip lines begining with # for comments
+ if (fgets (line, 100, pt_f) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // this line is not a comment
+ if ( fscanf (pt_f, "%s", line) != EOF) {
+ if ( strcmp (line, head) == 0)
+ end = true; // robot position was found
+ }
+ else {
+ fclose(pt_f);
+ return (false); // end of file without position
+ }
+ }
+ }
+ else {
+ fclose(pt_f);
+ return (false);// end of file
+ }
+
+ }
+ while ( end != true );
+
+ double q1,q2;
+ // Read positions
+ if (fscanf(pt_f, "%lf %lf", &q1, &q2) == EOF) {
+ fclose(pt_f);
+ std::cout << "Cannot read joint positions." << std::endl;
+ return false;
+ }
+ q.resize(vpBiclops::ndof) ;
+
+ q[0] = vpMath::rad(q1) ; // Rot tourelle
+ q[1] = vpMath::rad(q2) ;
+
+ fclose(pt_f) ;
+ return (true);
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param d The measured displacement in camera frame. The dimension of d is 6
+ (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
+ radians.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+vpRobotBiclops::getCameraDisplacement(vpColVector &d)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, d);
+
+}
+/*!
+
+ Get the robot articular displacement since the last call of this method.
+
+ \param d The measured articular displacement. The dimension of d is 2 (the
+ number of axis of the robot) with respectively d[0] (pan displacement),
+ d[1] (tilt displacement)
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, d);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning The first call of this method gives not a good value for the
+ displacement.
+
+ \param frame The frame in which the measured displacement is expressed.
+
+ \param d The displacement:
+
+ - In articular, the dimension of q is 2 (the number of axis of the robot)
+ with respectively d[0] (pan displacement), d[1] (tilt displacement).
+
+ - In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz).
+ Translations are expressed in meters, rotations in radians with the theta U
+ representation.
+
+ \exception vpRobotException::wrongStateError If a not supported frame type is
+ given.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &d)
+{
+ vpColVector q_current; // current position
+
+ getPosition(vpRobot::ARTICULAR_FRAME, q_current);
+
+ switch(frame) {
+ case vpRobot::ARTICULAR_FRAME:
+ d.resize(vpBiclops::ndof);
+ d = q_current - q_previous;
+ break ;
+
+ case vpRobot::CAMERA_FRAME: {
+ d.resize(6);
+ vpHomogeneousMatrix fMc_current;
+ vpHomogeneousMatrix fMc_previous;
+ fMc_current = vpBiclops::get_fMc(q_current);
+ fMc_previous = vpBiclops::get_fMc(q_previous);
+ vpHomogeneousMatrix c_previousMc_current;
+ // fMc_c = fMc_p * c_pMc_c
+ // => c_pMc_c = (fMc_p)^-1 * fMc_c
+ c_previousMc_current = fMc_previous.inverse() * fMc_current;
+
+ // Compute the instantaneous velocity from this homogeneous matrix.
+ d = vpExponentialMap::inverse( c_previousMc_current );
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a velocity in the reference frame:"
+ "functionality not implemented");
+ break ;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a velocity in the mixt frame:"
+ "functionality not implemented");
+ break ;
+ }
+
+
+ q_previous = q_current; // Update for next call of this method
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no symbols
+void dummy_vpRobotBiclops() {};
+#endif
+
diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp
new file mode 100644
index 0000000..70e86fa
--- /dev/null
+++ b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp
@@ -0,0 +1,487 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Biclops robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <signal.h>
+#include <string.h>
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+# include <unistd.h>
+#endif
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_BICLOPS
+
+/* Headers */
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/robot/vpRobotBiclopsController.h>
+#include <visp3/robot/vpRobotException.h>
+
+//#define VP_DEBUG // Activate the debug mode
+//#define VP_DEBUG_MODE 20 // Activate debug level 1 and 2
+
+#include <visp3/core/vpDebug.h>
+
+
+
+/* ----------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+ Default constructor.
+*/
+vpRobotBiclopsController::vpRobotBiclopsController()
+ : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL),
+ panProfile(), tiltProfile(), vergeProfile(), shm(), stopControllerThread_(false)
+{
+ axisMask = Biclops::PanMask
+ + Biclops::TiltMask
+ /*+ Biclops::VergeMask*/; // add this if you want verge.
+
+
+ // Set Debug level depending on how much info you want to see about
+ // the inner workings of the API. Level 2 is highest with 0 being
+ // the default (i.e., no messages).
+ biclops.SetDebugLevel(0);
+
+ // initialize the shared data structure
+ for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ shm.status[i] = STOP;
+ shm.q_dot[i] = 0.;
+ shm.actual_q[i] = 0.;
+ shm.jointLimit[i] = false;
+ shm.status[i] = STOP;
+ }
+}
+
+/*!
+
+ Destructor.
+
+*/
+vpRobotBiclopsController::~vpRobotBiclopsController()
+{
+}
+
+/*!
+
+ Initialize the biclops by homing all axis.
+
+ \param configfile : Biclops configuration file.
+
+ \exception vpRobotException::notInitializedError If the biclops head connot
+ be initialized. The initialization can failed,
+ - if the head is not powered on,
+ - if the head is not connected to your computer throw a serial cable,
+ - if you try to open a bad serial port. Check you config file to verify
+ which is the used serial port.
+*/
+void
+vpRobotBiclopsController::init(const char *configfile)
+{
+ vpDEBUG_TRACE (12, "Initialize biclops.");
+ bool binit = false;
+ for (int i=0; i<1; i++) {
+ try {
+ std::cout << "Try to initialize biclops head " << std::endl;
+ binit = biclops.Initialize(configfile);
+ usleep(100000);
+ if (binit) {
+ // Initialization completed successfully. Close the config file.
+ std::cout << "Initialization succeed...\n";
+ break;
+ }
+ else {
+ std::cout << "Initialization failed...\n";
+ }
+ }
+ catch(...)
+ {
+ std::cout << "Initialization failed..."<< std::endl;
+ }
+ }
+
+ if (! binit) {
+ std::cout << "Cannot initialize biclops head. " << std::endl;
+ std::cout << "Check if the serial cable is connected." << std::endl;
+ std::cout << "Check if the robot is powered on." << std::endl;
+ std::cout << "Check if you try to open the good serial port." << std::endl;
+ std::cout << "Try to power off/on and restart..." << std::endl;
+
+ throw vpRobotException (vpRobotException::notInitializedError,
+ "Cannot initialize biclops head.");
+
+ }
+
+
+ vpDEBUG_TRACE(12, "Biclops initialized");
+
+ // Get shortcut references to each axis.
+ panAxis = biclops.GetAxis(Biclops::Pan);
+ tiltAxis = biclops.GetAxis(Biclops::Tilt);
+ if ((axisMask & Biclops::VergeMask) != 0)
+ vergeAxis = biclops.GetAxis(Biclops::Verge);
+
+#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
+ if (!panAxis -> GetHomedState() || !tiltAxis -> GetHomedState()) {
+ vpDEBUG_TRACE(12, "Biclops is not homed");
+ }
+#else // old API
+ if (!panAxis -> IsAlreadyHomed() || !tiltAxis -> IsAlreadyHomed()) {
+ vpDEBUG_TRACE(12, "Biclops is not homed");
+ }
+#endif
+
+ //Execute the homing sequence for all axes.
+ vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
+ vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
+ if ( biclops.HomeAxes(axisMask))
+ vpDEBUG_TRACE(12, "Homing sequence succeed.");
+ else {
+ vpERROR_TRACE("Homing sequence failed. Program is stopped");
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with biclops");
+ }
+
+ // Get the currently defined (default) motion profiles.
+ // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
+ panAxis->GetProfile(panProfile);
+ tiltAxis->GetProfile(tiltProfile);
+ if ((axisMask & Biclops::VergeMask) != 0)
+ vergeAxis->GetProfile(vergeProfile);
+}
+
+/*!
+
+ Set the biclops axis position. The motion of the axis is synchronized to end
+ on the same time.
+
+ \warning Wait the end of the positionning.
+
+ \param q : The position to set for each axis.
+
+ \param percentVelocity : The velocity displacement to reach the new position
+ in the range [0: 100.0]. 100 % corresponds to the maximal admissible
+ speed. The maximal admissible speed is given by vpBiclops::speedLimit.
+
+*/
+
+void
+vpRobotBiclopsController::setPosition(const vpColVector & q,
+ const double percentVelocity )
+{
+ if (q.getRows() != vpBiclops::ndof )
+ {
+ vpERROR_TRACE ("Bad dimension for positioning vector.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad dimension for positioning vector.");
+ }
+
+ panAxis -> SetProfileMode(PMDTrapezoidalProfile);
+ tiltAxis -> SetProfileMode(PMDTrapezoidalProfile);
+
+ // Create the list of axes we want to coordinate
+ PMDUtils::AxisList axisList;
+ axisList.push_back(panAxis);
+ axisList.push_back(tiltAxis);
+
+#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
+ // Get the currently defined (default) motion profiles.
+ //PMDAxisControl::Profile panProfile,tiltProfile;
+ panAxis->GetProfile(panProfile);
+ tiltAxis->GetProfile(tiltProfile);
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ panProfile.pos = PMDUtils::RadsToRevs(q[0]);
+ panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
+ * percentVelocity / 100.);
+
+ tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
+ tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
+ * percentVelocity / 100.);
+
+ // Inform the controller of the new desired position.
+ panAxis->SetProfile(panProfile);
+ tiltAxis->SetProfile(tiltProfile);
+
+#else // old API
+
+ PMDAxisControl::CountsProfile desired_profile;
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ panProfile.pos = PMDUtils::RadsToRevs(q[0]);
+ panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
+ * percentVelocity / 100.);
+
+ vpDEBUG_TRACE(12, "Speed percent: %lf",
+ vpBiclops::speedLimit * percentVelocity / 100.);
+
+ panAxis -> ProfileToCounts(panProfile, desired_profile);
+ vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
+ vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
+
+ panAxis -> SetProfile(desired_profile);
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
+ tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
+ * percentVelocity / 100.);
+
+ tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
+ vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
+ vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
+
+ tiltAxis -> SetProfile(desired_profile);
+#endif
+
+ // Coordinate motion
+ PMDUtils::Coordinate(axisList);
+ biclops.Move(Biclops::PanMask + Biclops::TiltMask/*, 0*/); //
+
+}
+
+/*!
+
+ Apply a velocity to each axis of the biclops robot.
+
+ \warning This method is non blocking.
+
+ \param q_dot : Velocity to apply.
+
+*/
+void
+vpRobotBiclopsController::setVelocity(const vpColVector & q_dot)
+{
+ if (q_dot.getRows() != vpBiclops::ndof )
+ {
+ vpERROR_TRACE ("Bad dimension for velocity vector.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad dimension for velocity vector.");
+ }
+
+
+#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
+ // Get the currently defined (default) motion profiles.
+ //PMDAxisControl::Profile panProfile, tiltProfile;
+ panAxis->GetProfile(panProfile);
+ tiltAxis->GetProfile(tiltProfile);
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
+ tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
+
+ // Inform the controller of the new desired position.
+ panAxis->SetProfile(panProfile);
+ tiltAxis->SetProfile(tiltProfile);
+
+ panAxis -> SetProfileMode(PMDVelocityContouringProfile);
+ tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
+#else // old API
+ panAxis -> SetProfileMode(PMDVelocityContouringProfile);
+ tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
+\
+ PMDAxisControl::CountsProfile desired_profile;
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
+
+ panAxis -> ProfileToCounts(panProfile, desired_profile);
+ panAxis -> SetProfile(desired_profile);
+
+ // Set a position to move to by modifying the respective profiles.
+ // NOTE: profile values are in revolutions, so here we convert
+ // from degrees (divide by 360) for readability.
+ tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
+
+ tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
+ tiltAxis -> SetProfile(desired_profile);
+#endif
+ // Coordinate motion
+ biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
+}
+
+/*!
+
+ Get the biclops articular position.
+
+ \return The axis articular position in radians.
+
+*/
+vpColVector
+vpRobotBiclopsController::getPosition()
+{
+ vpDEBUG_TRACE (12, "Start vpRobotBiclopsController::getPosition() ");
+ vpColVector q(vpBiclops::ndof);
+ PMDint32 panpos, tiltpos;
+
+ panAxis -> GetPosition(panpos);
+ tiltAxis -> GetPosition(tiltpos);
+
+ q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
+ q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
+
+ vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
+ vpDEBUG_TRACE (12, "End vpRobotBiclopsController::getPosition()");
+
+ return q;
+}
+
+/*!
+
+ Get the biclops actual articular position.
+
+ \return The axis actual articular position in radians.
+
+*/
+vpColVector
+vpRobotBiclopsController::getActualPosition()
+{
+ vpColVector q(vpBiclops::ndof);
+ PMDint32 panpos, tiltpos;
+
+ panAxis -> GetActualPosition(panpos);
+ tiltAxis -> GetActualPosition(tiltpos);
+
+ q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
+ q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
+
+ return q;
+}
+
+/*!
+
+ Get the biclops articular velocity.
+
+ \return The axis articular velocity in rad/s.
+
+*/
+vpColVector
+vpRobotBiclopsController::getVelocity()
+{
+ vpColVector q_dot(vpBiclops::ndof);
+ PMDint32 pan_vel, tilt_vel;
+
+ panAxis -> GetVelocity(pan_vel);
+ tiltAxis -> GetVelocity(tilt_vel);
+
+ q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
+ q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
+
+ return q_dot;
+}
+
+/*!
+
+ Get the biclops actual articular velocity.
+
+ \return The axis actual articular velocity in rad/s.
+
+*/
+vpColVector
+vpRobotBiclopsController::getActualVelocity()
+{
+ vpColVector q_dot(vpBiclops::ndof);
+ PMDint32 pan_vel, tilt_vel;
+
+ panAxis -> GetActualVelocity(pan_vel);
+ tiltAxis -> GetActualVelocity(tilt_vel);
+
+ q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
+ q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
+
+ return q_dot;
+}
+
+/*!
+
+ Update the shared memory.
+
+ \param shm_ : Content to write in the shared memory.
+*/
+void
+vpRobotBiclopsController::writeShm(shmType &shm_)
+{
+ for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
+ }
+ memcpy(&this->shm, &shm_, sizeof(shmType));
+ //this->shm = shm_;
+ for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
+ }
+}
+
+/*!
+
+ Get a copy of the shared memory.
+
+ \return A copy of the shared memory.
+*/
+vpRobotBiclopsController::shmType
+vpRobotBiclopsController::readShm()
+{
+ shmType tmp_shm;
+
+ for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
+ }
+ memcpy(&tmp_shm, &this->shm, sizeof(shmType));
+ //tmp_shm = shm;
+ for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
+ }
+
+ return tmp_shm;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclopsController.cpp.o) has no symbols
+void dummy_vpRobotBiclopsController() {};
+#endif
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
diff --git a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp
new file mode 100644
index 0000000..4ecc949
--- /dev/null
+++ b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp
@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for Pioneer robots based on Aria 3rd party library.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpRobotPioneer.h>
+// Warning: vpMath.h should be included after Aria.h to avoid the build issue:
+// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
+// in this scope"
+// This error is due to cmath header included from vpMath.h that makes
+// isfinite() ambiguous between ::isfinite() and std::isfinite()
+#include <visp3/core/vpMath.h>
+
+#ifdef VISP_HAVE_PIONEER
+
+/*!
+ Default constructor that initializes Aria.
+ */
+vpRobotPioneer::vpRobotPioneer() : vpPioneer(), ArRobot()
+{
+ isInitialized = false;
+
+ Aria::init();
+}
+
+/*!
+ Destructor.
+ */
+vpRobotPioneer::~vpRobotPioneer()
+{
+#if 0
+ std::cout << "Ending robot thread..." << std::endl;
+ stopRunning();
+
+ // wait for the thread to stop
+ waitForRunExit();
+#endif
+}
+
+/*!
+ Set the velocity (frame has to be specified) that will be applied to the robot.
+
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
+ and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
+ rotational velocities are implemented.
+
+ \param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
+ second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
+ independently.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
+ Second value is the rotational velocity in rad/s.
+
+ Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
+ which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
+ the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
+ \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+ */
+void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
+{
+ init();
+
+ /*
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ } */
+
+ if (vel.size() != 2)
+ {
+ throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
+ }
+
+ vpColVector vel_max(2);
+ vpColVector vel_sat;
+
+ if (frame == vpRobot::REFERENCE_FRAME)
+ {
+ vel_max[0] = getMaxTranslationVelocity();
+ vel_max[1] = getMaxRotationVelocity();
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ this->lock();
+ this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
+ this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
+ this->unlock();
+ }
+ else if (frame == vpRobot::ARTICULAR_FRAME)
+ {
+ vel_max[0] = getMaxTranslationVelocity();
+ vel_max[1] = getMaxTranslationVelocity();
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ this->lock();
+ //std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
+ this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
+ this->unlock();
+ }
+ else
+ {
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send the robot velocity in the specified control frame");
+ }
+}
+
+/*!
+ Initialize the robot.
+ - Sets the robot in asynchronous mode by starting a low level thread. The robot will be stopped
+ if there is no connection to the robot at any given point.
+ - Enables the motors on the robot, if it is connected.
+
+ */
+void vpRobotPioneer::init()
+{
+ if ( ! isInitialized )
+ {
+ // Start the robot processing cycle running in the background.
+ // True parameter means that if the connection is lost, then the
+ // run loop ends.
+ this->runAsync(true);
+ this->lock();
+ this->enableMotors();
+ this->unlock();
+
+ isInitialized = true;
+ }
+}
+
+/*!
+ Gets the current translational velocity of the robot.
+
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
+ and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
+ rotational velocities are implemented.
+
+ \param velocity : A two dimension vector that corresponds to the current velocities applied to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
+ second value is the velocity of the right wheel in m/s.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
+ Second value is the rotational velocity in rad/s.
+
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
+ \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+
+ \sa getVelocity(const vpRobot::vpControlFrameType)
+ */
+void vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity)
+{
+ init();
+ velocity.resize(2);
+
+ if (frame == vpRobot::ARTICULAR_FRAME)
+ {
+ this->lock();
+ velocity[0] = this->getLeftVel() / 1000.;
+ velocity[1] = this->getRightVel() / 1000;
+ this->unlock();
+ }
+ else if (frame == vpRobot::REFERENCE_FRAME)
+ {
+ this->lock();
+ velocity[0] = this->getVel() / 1000.;
+ velocity[1] = vpMath::rad( this->getRotVel() );
+ this->unlock();
+ }
+ else {
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get the robot volocity in the specified control frame");
+ }
+}
+
+/*!
+ Gets the current translational velocity of the robot.
+
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
+ and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
+ rotational velocities are implemented.
+
+ \return A two dimension vector that corresponds to the current velocities applied to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
+ second value is the velocity of the right wheel in m/s.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
+ Second value is the rotational velocity in rad/s.
+
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
+ \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+
+ \sa getVelocity(const vpRobot::vpControlFrameType, vpColVector &)
+ */
+vpColVector vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame)
+{
+ vpColVector velocity;
+ getVelocity(frame, velocity);
+ return velocity;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no symbols
+void dummy_vpRobotPioneer() {};
+#endif
diff --git a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp
new file mode 100644
index 0000000..76c4eb5
--- /dev/null
+++ b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp
@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ptu-46 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+/* ----------------------------------------------------------------------- */
+/* --- INCLUDE ----------------------------------------------------------- */
+/* ----------------------------------------------------------------------- */
+
+
+#include <visp3/robot/vpPtu46.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobotException.h>
+
+
+/* Inclusion des fichiers standards. */
+#include <math.h>
+#include <visp3/core/vpMath.h>
+
+/* ------------------------------------------------------------------------ */
+/* --- COMPUTE ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
+const float vpPtu46::L = 0.0765f; /*! Horizontal offset along the last joint,
+ from last joint to camera frame. */
+const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
+ camera frame. */
+
+
+/*!
+ Compute the direct geometric model of the camera: fMc
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fMc : Homogeneous matrix corresponding to the direct geometric model
+ of the camera. Describes the transformation between the robot reference frame
+ (called fixed) and the camera frame.
+
+*/
+void
+vpPtu46::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
+{
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
+ }
+
+ double q1 = q[0]; // pan
+ double q2 = q[1]; // tilt
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
+
+ fMc[0][0] = s1;
+ fMc[0][1] = c1*s2;
+ fMc[0][2] = c1*c2;
+ fMc[0][3] = -h*c1*s2 - L*s1;
+
+ fMc[1][0] = -c1;
+ fMc[1][1] = s1*s2;
+ fMc[1][2] = s1*c2;
+ fMc[1][3] = -h*s1*s2 + L*c1;
+
+ fMc[2][0] = 0;
+ fMc[2][1] = -c2;
+ fMc[2][2] = s2;
+ fMc[2][3] = h*c2;
+
+ fMc[3][0] = 0;
+ fMc[3][1] = 0;
+ fMc[3][2] = 0;
+ fMc[3][3] = 1;
+
+ vpCDEBUG (6) << "Position de la camera: " << std::endl << fMc;
+
+ return ;
+}
+
+/*!
+ Return the direct geometric model of the camera: fMc
+
+ \param q : Articular position for pan and tilt axis.
+
+ \return fMc, the homogeneous matrix corresponding to the direct geometric
+ model of the camera. Describes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
+
+*/
+vpHomogeneousMatrix
+vpPtu46::computeMGD (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+
+ computeMGD (q, fMc);
+
+ return fMc;
+}
+
+/*!
+ Compute the direct geometric model of the camera in terms of pose vector.
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param r : Pose vector corresponding to the transformation between the robot
+ reference frame (called fixed) and the camera frame.
+
+*/
+void
+vpPtu46::computeMGD (const vpColVector & q, vpPoseVector & r) const
+{
+ vpHomogeneousMatrix fMc;
+
+ computeMGD (q, fMc);
+ r.buildFrom(fMc.inverse());
+
+ return ;
+}
+
+
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Default construtor. Call init().
+
+*/
+vpPtu46::vpPtu46 (void)
+{
+ init();
+}
+/* ---------------------------------------------------------------------- */
+/* --- PRIVATE ---------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+
+/*!
+ Initialization. Here nothing to do.
+
+*/
+void
+vpPtu46::init ()
+{
+ return ;
+}
+
+
+/* ----------------------------------------------------------------------- */
+/* --- DISPLAY ----------------------------------------------------------- */
+/* ----------------------------------------------------------------------- */
+
+VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & /* constant */)
+{
+ os
+ << "Geometric parameters: " << std::endl
+ << "L: "
+ << "\t" << vpPtu46::L << std::endl
+ << "h: "
+ << "\t" << vpPtu46::h << std::endl;
+
+ return os;
+}
+
+
+/*!
+
+ Get the twist matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cVe : Twist transformation between camera and end effector frame to
+ expess a velocity skew from end effector frame in camera frame.
+
+*/
+void
+vpPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the homogeneous matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cMe : Homogeneous matrix between camera and end effector frame.
+
+*/
+void
+vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpHomogeneousMatrix eMc ;
+
+ eMc[0][0] = 0;
+ eMc[0][1] = -1;
+ eMc[0][2] = 0;
+ eMc[0][3] = h;
+
+ eMc[1][0] = 1;
+ eMc[1][1] = 0;
+ eMc[1][2] = 0;
+ eMc[1][3] = -L;
+
+ eMc[2][0] = 0;
+ eMc[2][1] = 0;
+ eMc[2][2] = 1;
+ eMc[2][3] = 0;
+
+ eMc[3][0] = 0;
+ eMc[3][1] = 0;
+ eMc[3][2] = 0;
+ eMc[3][3] = 1;
+
+ cMe = eMc.inverse() ;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ \warning Re is not the embedded camera frame. It corresponds to the frame
+ associated to the tilt axis (see also get_cMe).
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param eJe : Jacobian between end effector frame and end effector frame (on
+ tilt axis).
+
+*/
+void
+vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+{
+
+
+ eJe.resize(6,2) ;
+
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
+ }
+
+ double s2 = sin(q[1]) ;
+ double c2 = cos(q[1]) ;
+
+ eJe = 0;
+
+ eJe[3][0] = c2;
+ eJe[4][1] = 1;
+ eJe[5][0] = s2;
+}
+
+/*!
+ Get the robot jacobian expressed in the robot reference frame
+
+ \param q : Articular position for pan and tilt axis.
+
+ \param fJe : Jacobian between reference frame (or fix frame) and end effector
+ frame (on tilt axis).
+
+*/
+void
+vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+{
+
+ if (q.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
+ throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
+ }
+
+ fJe.resize(6,2) ;
+
+ double s1 = sin(q[0]) ;
+ double c1 = cos(q[0]) ;
+
+ fJe = 0;
+
+ fJe[3][1] = s1;
+ fJe[4][1] = -c1;
+ fJe[5][0] = 1;
+}
diff --git a/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp
new file mode 100644
index 0000000..dc4afbe
--- /dev/null
+++ b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp
@@ -0,0 +1,930 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ptu-46 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <signal.h>
+#include <string.h>
+
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_PTU46
+
+/* Headers des fonctions implementees. */
+#include <visp3/robot/vpPtu46.h>
+#include <visp3/robot/vpRobotPtu46.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+
+bool vpRobotPtu46::robotAlreadyCreated = false;
+const double vpRobotPtu46::defaultPositioningVelocity = 10.0;
+
+/* ----------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+
+/*!
+
+ Default constructor.
+
+ Initialize the ptu-46 pan, tilt head by opening the serial port.
+
+ \sa init()
+
+*/
+vpRobotPtu46::vpRobotPtu46 (const char *device)
+ :
+ vpRobot ()
+{
+ this->device = new char [FILENAME_MAX];
+
+ sprintf(this->device, "%s", device);
+
+ vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
+ try
+ {
+ init();
+ }
+ catch(...)
+ {
+ delete [] this->device;
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+ try
+ {
+ setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...)
+ {
+ delete [] this->device;
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ positioningVelocity = defaultPositioningVelocity ;
+ return ;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- DESTRUCTOR -------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Destructor.
+ Close the serial connection with the head.
+
+*/
+vpRobotPtu46::~vpRobotPtu46 (void)
+{
+
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ if (0 != ptu.close())
+ {
+ vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
+ }
+
+ vpRobotPtu46::robotAlreadyCreated = false;
+
+ delete [] device;
+
+ return;
+}
+
+
+/* -------------------------------------------------------------------------- */
+/* --- INITIALISATION ------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+
+ Open the serial port.
+
+
+ \exception vpRobotException::constructionError : If the device cannot be
+ oppened.
+
+*/
+void
+vpRobotPtu46::init ()
+{
+
+ vpDEBUG_TRACE (12, "Open connection Ptu-46.");
+ if (0 != ptu.init(device) )
+ {
+ vpERROR_TRACE ("Cannot open connexion with ptu-46.");
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with ptu-46");
+ }
+
+ return ;
+}
+
+
+/*!
+
+ Change the state of the robot either to stop them, or to set position or
+ speed control.
+
+*/
+vpRobot::vpRobotStateType
+vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ switch (newState)
+ {
+ case vpRobot::STATE_STOP:
+ {
+ if (vpRobot::STATE_STOP != getRobotState ())
+ {
+ ptu.stop();
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL:
+ {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
+ {
+ vpDEBUG_TRACE (12, "Passage vitesse -> position.");
+ ptu.stop();
+ }
+ else
+ {
+ vpDEBUG_TRACE (1, "Passage arret -> position.");
+ }
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL:
+ {
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpDEBUG_TRACE (10, "Arret du robot...");
+ ptu.stop();
+ }
+ break;
+ }
+ default:
+ break ;
+ }
+
+ return vpRobot::setRobotState (newState);
+}
+
+/*!
+
+ Halt all the axis.
+
+*/
+void
+vpRobotPtu46::stopMotion(void)
+{
+ ptu.stop();
+ setRobotState (vpRobot::STATE_STOP);
+}
+
+
+/*!
+
+ Get the twist matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cVe : Twist transformation between camera and end effector frame to
+ expess a velocity skew from end effector frame in camera frame.
+
+*/
+void
+vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ vpPtu46::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the homogeneous matrix corresponding to the transformation between the
+ camera frame and the end effector frame. The end effector frame is located on
+ the tilt axis.
+
+ \param cMe : Homogeneous matrix between camera and end effector frame.
+
+*/
+void
+vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpPtu46::get_cMe(cMe) ;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ \warning Re is not the embedded camera frame. It corresponds to the frame
+ associated to the tilt axis (see also get_cMe).
+
+ \param eJe : Jacobian between end effector frame and end effector frame (on
+ tilt axis).
+
+*/
+void
+vpRobotPtu46::get_eJe(vpMatrix &eJe)
+{
+ vpColVector q(2) ;
+ getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+
+ try
+ {
+ vpPtu46::get_eJe(q,eJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+
+/*!
+ Get the robot jacobian expressed in the robot reference frame
+
+ \param fJe : Jacobian between reference frame (or fix frame) and end effector
+ frame (on tilt axis).
+
+*/
+void
+vpRobotPtu46::get_fJe(vpMatrix &fJe)
+{
+ vpColVector q(2) ;
+ getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+
+ try
+ {
+ vpPtu46::get_fJe(q,fJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+
+}
+
+
+/*!
+
+ Set the velocity used for a position control.
+
+ \param velocity : Velocity in % of the maximum velocity between [0,100].
+*/
+void
+vpRobotPtu46::setPositioningVelocity (const double velocity)
+{
+ positioningVelocity = velocity;
+}
+/*!
+ Get the velocity in % used for a position control.
+
+ \return Positionning velocity in [0, 100]
+
+*/
+double
+vpRobotPtu46::getPositioningVelocity (void)
+{
+ return positioningVelocity;
+}
+
+
+/*!
+ Move the robot in position control.
+
+ \warning This method is blocking. That mean that it waits the end of the
+ positionning.
+
+ \param frame : Control frame. This head can only be controlled in
+ articular.
+
+ \param q : The position to set for each axis.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+
+void
+vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & q )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in camera frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in camera frame: "
+ "not implemented");
+ break;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in reference frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in reference frame: "
+ "not implemented");
+ break;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot move the robot in mixt frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot move the robot in mixt frame: "
+ "not implemented");
+ break;
+ case vpRobot::ARTICULAR_FRAME:
+ break ;
+ }
+
+ // Interface for the controller
+ double artpos[2];
+
+ artpos[0] = q[0];
+ artpos[1] = q[1];
+
+ if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
+ {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error.");
+ }
+
+ return ;
+}
+
+
+/*!
+ Move the robot in position control.
+
+ \warning This method is blocking. That mean that it wait the end of the
+ positionning.
+
+ \param frame : Control frame. This head can only be controlled in
+ articular.
+
+ \param q1 : The pan position to set.
+ \param q2 : The tilt position to set.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+void
+vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
+ const double &q1, const double &q2)
+{
+ try{
+ vpColVector q(2) ;
+ q[0] = q1 ;
+ q[1] = q2 ;
+
+ setPosition(frame,q) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Read the content of the position file and moves to head to articular
+ position.
+
+ \param filename : Position filename
+
+ \exception vpRobotException::readingParametersError : If the articular
+ position cannot be read from file.
+
+ \sa readPositionFile()
+
+*/
+void
+vpRobotPtu46::setPosition(const char *filename)
+{
+ vpColVector q ;
+ if (readPositionFile(filename, q) == false) {
+ vpERROR_TRACE ("Cannot get ptu-46 position from file");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Cannot get ptu-46 position from file");
+ }
+ setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+}
+
+/*!
+
+ Return the position of each axis.
+
+ \param frame : Control frame. This head can only be controlled in
+ articular.
+
+ \param q : The position of the axis.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+
+*/
+void
+vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
+ vpColVector & q)
+{
+ vpDEBUG_TRACE (9, "# Entree.");
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in camera frame: "
+ "not implemented");
+ break;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in reference frame: "
+ "not implemented");
+ break;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break;
+ case vpRobot::ARTICULAR_FRAME:
+ break ;
+ }
+
+ double artpos[2];
+
+ if (0 != ptu.getCurrentPosition( artpos ) )
+ {
+ vpERROR_TRACE ("Error when calling recup_posit_Afma4.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Error when calling recup_posit_Afma4.");
+ }
+
+ q.resize (vpPtu46::ndof);
+
+ q[0] = artpos[0];
+ q[1] = artpos[1];
+}
+
+
+/*!
+
+ Send a velocity on each axis.
+
+ \param frame : Control frame. This head can only be controlled in articular
+ and camera frame. Be aware, the reference frame (vpRobot::REFERENCE_FRAME)
+ and the mixt frame (vpRobot::MIXT_FRAME) are not implemented.
+
+ \param v : The desired velocity of the axis. The size of this vector is
+ always 2. Velocitoes are expressed in rad/s.
+
+ - In camera frame, \f$ v = [\omega_x, \omega_y]^t \f$.
+
+ - In articular, we control the 2 dof, \f$ v = [\dot{q}_1, \dot{q}_2]^t \f$
+ with \f$ \dot{q}_1 \f$ the pan of the camera and \f$ \dot{q}_2\f$ the tilt of
+ the camera.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ (vpRobot::REFERENCE_FRAME, vpRobot::MIXT_FRAME) is given.
+
+ \warning Velocities could be saturated if one of them exceed the maximal
+ autorized speed (see vpRobot::maxRotationVelocity).
+*/
+
+void
+vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & v)
+{
+ TPtuFrame ptuFrameInterface;
+
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ ptuFrameInterface = PTU_CAMERA_FRAME;
+ if ( v.getRows() != 2) {
+ vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Bad dimension for speed vector "
+ "in camera frame");
+ }
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ ptuFrameInterface = PTU_ARTICULAR_FRAME;
+ if ( v.getRows() != 2) {
+ vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Bad dimension for speed vector "
+ "in articular frame");
+ }
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "in the mixt frame:"
+ "functionality not implemented");
+ break ;
+ }
+ default:
+ {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot ");
+ }
+ }
+
+ vpDEBUG_TRACE (12, "Velocity limitation.");
+ bool norm = false; // Flag to indicate when velocities need to be nomalized
+ double ptuSpeedInterface[2];
+
+ switch(frame) {
+ case vpRobot::ARTICULAR_FRAME :
+ case vpRobot::CAMERA_FRAME : {
+ double max = this ->maxRotationVelocity;
+ for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
+ {
+ if (fabs (v[i]) > max)
+ {
+ norm = true;
+ max = fabs (v[i]);
+ vpERROR_TRACE ("Excess velocity: ROTATION "
+ "(axe nr.%d).", i);
+ }
+ }
+ // Rotations velocities normalisation
+ if (norm == true) {
+ max = this ->maxRotationVelocity / max;
+ for (unsigned int i = 0 ; i < 2; ++ i)
+ ptuSpeedInterface [i] = v[i]*max;
+ }
+ break;
+ }
+ default:
+ // Should never occur
+ break;
+
+ }
+
+ vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
+ << " " << ptuSpeedInterface[1] << std::endl;
+ ptu.move(ptuSpeedInterface, ptuFrameInterface);
+ return;
+}
+
+
+/* ------------------------------------------------------------------------- */
+/* --- GET ----------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/*!
+
+ Get the articular velocity.
+
+ \param frame : Control frame. This head can only be controlled in articular.
+
+ \param q_dot : The measured articular velocity in rad/s.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+*/
+void
+vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
+ vpColVector & q_dot)
+{
+
+ TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
+
+ switch (frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ {
+ vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a velocity in the camera frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ ptuFrameInterface = PTU_ARTICULAR_FRAME;
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME:
+ {
+ vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a velocity in the reference frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+
+ vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a velocity in the mixt frame:"
+ "functionality not implemented");
+ break ;
+ }
+ }
+
+ q_dot.resize(vpPtu46::ndof);
+ double ptuSpeedInterface[2];
+
+ ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
+
+ q_dot[0] = ptuSpeedInterface[0];
+ q_dot[1] = ptuSpeedInterface[1];
+
+}
+
+
+/*!
+
+ Return the articular velocity.
+
+ \param frame : Control frame. This head can only be controlled in articular.
+
+ \return The measured articular velocity in rad/s.
+
+ \exception vpRobotException::wrongStateError : If a not supported frame type
+ is given.
+*/
+vpColVector
+vpRobotPtu46::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector q_dot;
+ getVelocity (frame, q_dot);
+
+ return q_dot;
+}
+
+/*!
+
+ Get an articular position from the position file.
+
+ \param filename : Position file.
+
+ \param q : The articular position read in the file.
+
+ \code
+ # Example of ptu-46 position file
+ # The axis positions must be preceed by R:
+ # First value : pan articular position in degrees
+ # Second value: tilt articular position in degrees
+ R: 15.0 5.0
+ \endcode
+
+ \return true if a position was found, false otherwise.
+
+*/
+bool
+vpRobotPtu46::readPositionFile(const char *filename, vpColVector &q)
+{
+ FILE * pt_f ;
+ pt_f = fopen(filename,"r") ;
+
+ if (pt_f == NULL) {
+ vpERROR_TRACE ("Can not open ptu-46 position file %s", filename);
+ return false;
+ }
+
+ char line[FILENAME_MAX];
+ char head[] = "R:";
+ bool end = false;
+
+ do {
+ // skip lines begining with # for comments
+ if (fgets (line, 100, pt_f) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // this line is not a comment
+ if ( fscanf (pt_f, "%s", line) != EOF) {
+ if ( strcmp (line, head) == 0)
+ end = true; // robot position was found
+ }
+ else
+ return (false); // end of file without position
+ }
+ }
+ else {
+ return (false);// end of file
+ }
+
+ }
+ while ( end != true );
+
+ double q1,q2;
+ // Read positions
+ fscanf(pt_f, "%lf %lf", &q1, &q2);
+ q.resize(vpPtu46::ndof) ;
+
+ q[0] = vpMath::rad(q1) ; // Rot tourelle
+ q[1] = vpMath::rad(q2) ;
+
+ fclose(pt_f) ;
+ return (true);
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param v The measured displacement in camera frame. The dimension of v is 6
+ (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
+ radians.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+vpRobotPtu46::getCameraDisplacement(vpColVector &v)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, v);
+
+}
+/*!
+
+ Get the robot articular displacement since the last call of this method.
+
+ \param d The measured articular displacement. The dimension of d is 2 (the
+ number of axis of the robot) with respectively d[0] (pan displacement),
+ d[1] (tilt displacement)
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void vpRobotPtu46::getArticularDisplacement(vpColVector &d)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, d);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning The first call of this method gives not a good value for the
+ displacement.
+
+ \param frame The frame in which the measured displacement is expressed.
+
+ \param d The displacement:
+ - In articular, the dimension of q is 2 (the number of axis of the robot)
+ with respectively d[0] (pan displacement), d[1] (tilt displacement).
+ - In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians.
+
+ \exception vpRobotException::wrongStateError If a not supported frame type is
+ given.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &d)
+{
+ double d_[6];
+
+ switch (frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ {
+ d.resize (6);
+ ptu.measureDpl(d_, PTU_CAMERA_FRAME);
+ d[0]=d_[0];
+ d[1]=d_[1];
+ d[2]=d_[2];
+ d[3]=d_[3];
+ d[4]=d_[4];
+ d[5]=d_[5];
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
+ d.resize (vpPtu46::ndof);
+ d[0]=d_[0]; // pan
+ d[1]=d_[1]; // tilt
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME:
+ {
+ vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a displacement in the reference frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot get a displacement in the reference frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no symbols
+void dummy_vpRobotPtu46() {};
+#endif
+
diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp
new file mode 100644
index 0000000..c91d467
--- /dev/null
+++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp
@@ -0,0 +1,2535 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Viper S650 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_VIPER650
+
+#include <signal.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ----------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+bool vpRobotViper650::robotAlreadyCreated = false;
+
+/*!
+
+ Default positioning velocity in percentage of the maximum
+ velocity. This value is set to 15. The member function
+ setPositioningVelocity() allows to change this value.
+
+*/
+const double vpRobotViper650::defaultPositioningVelocity = 15.0;
+
+/* ---------------------------------------------------------------------- */
+/* --- EMERGENCY STOP --------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Emergency stops the robot if the program is interrupted by a SIGINT
+ (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
+ or SIGQUIT signal.
+
+*/
+void emergencyStopViper650(int signo)
+{
+ std::cout << "Stop the Viper650 application by signal ("
+ << signo << "): " << (char)7 ;
+ switch(signo)
+ {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ case SIGBUS:
+ std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ case SIGSEGV:
+ std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ case SIGKILL:
+ std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ case SIGQUIT:
+ std::cout <<"SIGQUIT " << std::endl ; break ;
+ default :
+ std::cout << signo << std::endl ;
+}
+ //std::cout << "Emergency stop called\n";
+ // PrimitiveESTOP_Viper650();
+ PrimitiveSTOP_Viper650();
+ std::cout << "Robot was stopped\n";
+
+ // Free allocated resources
+ // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
+
+ fprintf(stdout, "Application ");
+ fflush(stdout);
+ kill(getpid(), SIGKILL);
+ exit(1) ;
+}
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ The only available constructor.
+
+ This contructor calls init() to initialise the connection with the
+ MotionBox or low level controller, send the default \f$^e{\bf
+ M}_c\f$ homogeneous matrix and power on the robot.
+
+ It also set the robot state to vpRobot::STATE_STOP.
+
+ To set the extrinsic camera parameters related to the \f$^e{\bf
+ M}_c\f$ matrix obtained with a camera perspective projection model
+ including the distorsion, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters associated to an
+ image acquired by the camera attached to the robot, with:
+
+ \code
+ vpImage<unsigned char> I(480, 640);
+
+ // Get an image from the camera attached to the robot
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+ g.acquire(I);
+#endif
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+#endif
+}
+ \endcode
+
+ \sa vpCameraParameters, init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType)
+
+*/
+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
+ */
+
+ signal(SIGINT, emergencyStopViper650);
+ signal(SIGBUS, emergencyStopViper650) ;
+ signal(SIGSEGV, emergencyStopViper650) ;
+ signal(SIGKILL, emergencyStopViper650);
+ signal(SIGQUIT, emergencyStopViper650);
+
+ setVerbose(verbose);
+ if (verbose_)
+ std::cout << "Open communication with MotionBlox.\n";
+ try {
+ this->init();
+ this->setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...) {
+ // vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ positioningVelocity = defaultPositioningVelocity ;
+
+ maxRotationVelocity_joint6 = maxRotationVelocity;
+
+ vpRobotViper650::robotAlreadyCreated = true;
+
+ return ;
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INITIALISATION ----------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Initialise the connection with the MotionBox or low level
+ controller, send the default eMc homogeneous matrix, power on the
+ robot and wait 1 sec before returning to be sure the initialisation
+ is done.
+
+ \warning This method sets the camera extrinsic parameters (matrix
+ eMc) to the one obtained by calibration with a camera projection
+ model without distorsion by calling
+ init(vpViper650::defaultCameraRobot). If you want to set the extrinsic
+ camera parameters to those obtained with a camera perspective model
+ including the distorsion you have to call the
+ init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType) method.
+ If you want to set custom extrinsic camera parameters you have to call
+ the init(vpViper650::vpToolType, const vpHomogeneousMatrix&) method.
+
+ \sa vpCameraParameters, init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
+ init(vpViper650::vpToolType, const vpHomogeneousMatrix&),
+ init(vpViper650::vpToolType, const std::string&)
+*/
+void
+ vpRobotViper650::init (void)
+{
+ InitTry;
+
+ // Initialise private variables used to compute the measured velocities
+ q_prev_getvel.resize(6);
+ q_prev_getvel = 0;
+ time_prev_getvel = 0;
+ first_time_getvel = true;
+
+ // Initialise private variables used to compute the measured displacement
+ q_prev_getdis.resize(6);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+ // Initialize the firewire connection
+ Try( InitializeConnection(verbose_) );
+
+ // Connect to the servoboard using the servo board GUID
+ Try( InitializeNode_Viper650() );
+
+ Try( PrimitiveRESET_Viper650() );
+
+ // Enable the joint limits on axis 6
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
+
+ // Update the eMc matrix in the low level controller
+ init(vpViper650::defaultTool);
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // Print the robot status
+ if (verbose_) {
+ std::cout << "Robot status: ";
+ switch(EStopStatus) {
+ case ESTOP_AUTO:
+ controlMode = AUTO;
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+
+ case ESTOP_MANUAL:
+ controlMode = MANUAL;
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+ case ESTOP_ACTIVATED:
+ controlMode = ESTOP;
+ std::cout << "Emergency stop is activated" << std::endl;
+ break;
+ default:
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ }
+ std::cout << std::endl;
+ }
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ // If an error occur in the low level controller, goto here
+ //CatchPrint();
+ Catch();
+
+ // Test if an error occurs
+ if (TryStt == -20001)
+ printf("No connection detected. Check if the robot is powered on \n"
+ "and if the firewire link exist between the MotionBlox and this computer.\n");
+ else if (TryStt == -675)
+ printf(" Timeout enabling power...\n");
+
+ if (TryStt < 0) {
+ // Power off the robot
+ PrimitivePOWEROFF_Viper650();
+ // Free allocated resources
+ ShutDownConnection();
+
+ std::cout << "Cannot open connexion with the motionblox..." << std::endl;
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+ return ;
+}
+
+/*!
+
+ Initialize the robot kinematics with the extrinsic calibration
+ parameters associated to a specific camera (set the eMc homogeneous
+ parameters in the low level controller) and also get the joint
+ limits from the low-level controller.
+
+ The eMc parameters depend on the camera and the projection model in use.
+
+ \param tool : Tool to use.
+
+ \param projModel : Projection model associated to the camera.
+
+ To set the extrinsic camera parameters related to the \f$^e{\bf
+ M}_c\f$ matrix obtained with a camera perspective projection model
+ including the distorsion, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters associated to an
+ image acquired by the camera attached to the robot, with:
+
+ \code
+ vpImage<unsigned char> I(480, 640);
+
+ // Get an image from the camera attached to the robot
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+ g.acquire(I);
+#endif
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+#endif
+}
+ \endcode
+
+ \sa vpCameraParameters,
+ init(vpViper650::vpToolType, const vpHomogeneousMatrix&),
+ init(vpViper650::vpToolType, const std::string&)
+*/
+void
+ vpRobotViper650::init (vpViper650::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel)
+{
+ vpViper650::init(tool, projModel);
+
+ InitTry;
+
+ // Get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ CatchPrint();
+ return ;
+}
+
+/*!
+
+ Initialize the robot kinematics (set the eMc homogeneous
+ parameters in the low level controller) from a file and
+ also get the joint limits from the low-level controller.
+
+ \param tool : Tool to use.
+
+ \param filename : Path of the configuration file containing the
+ transformation between the end-effector frame and the tool frame.
+
+ To set the transformation parameters related to the \f$^e{\bf
+ M}_c\f$ matrix, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Set the transformation between the end-effector frame
+ // and the tool frame from a file
+ std::string filename("./EffectorToolTransformation.cnf");
+
+ robot.init(vpViper650::TOOL_CUSTOM, filename);
+#endif
+}
+ \endcode
+
+ The configuration file should have the form below:
+
+ \code
+# Start with any number of consecutive lines
+# beginning with the symbol '#'
+#
+# The 3 following lines contain the name of the camera,
+# the rotation parameters of the geometric transformation
+# using the Euler angles in degrees with convention XYZ and
+# the translation parameters expressed in meters
+CAMERA CameraName
+eMc_ROT_XYZ 10.0 -90.0 20.0
+eMc_TRANS_XYZ 0.05 0.01 0.06
+ \endcode
+
+ \sa vpCameraParameters, init(), init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
+ init(vpViper650::vpToolType, const vpHomogeneousMatrix&)
+*/
+void
+vpRobotViper650::init (vpViper650::vpToolType tool,
+ const std::string &filename)
+{
+ vpViper650::init(tool, filename);
+
+ InitTry;
+
+ // Get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ CatchPrint();
+ return ;
+}
+
+/*!
+
+ Initialize the robot kinematics with user defined parameters
+ (set the eMc homogeneous parameters in the low level controller)
+ and also get the joint limits from the low-level controller.
+
+ \param tool : Tool to use.
+
+ \param eMc_ : Transformation between the end-effector frame
+ and the tool frame.
+
+ To set the transformation parameters related to the \f$^e{\bf
+ M}_c\f$ matrix, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Set the transformation between the end-effector frame
+ // and the tool frame.
+ vpHomogeneousMatrix eMc(0.001, 0.0, 0.1, 0.0, 0.0, M_PI/2);
+
+ robot.init(vpViper650::TOOL_CUSTOM, eMc);
+#endif
+}
+ \endcode
+
+ \sa vpCameraParameters, init(), init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
+ init(vpViper650::vpToolType, const std::string&)
+*/
+void
+vpRobotViper650::init (vpViper650::vpToolType tool,
+ const vpHomogeneousMatrix &eMc_)
+{
+ vpViper650::init(tool, eMc_);
+
+ InitTry;
+
+ // Get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ CatchPrint();
+ return ;
+}
+
+/*!
+
+ Set the geometric transformation between the end-effector frame and
+ the tool frame in the low level controller.
+
+ \warning This function overwrite the transformation parameters that were
+ potentially set using one of the init functions
+
+ \param eMc_ : Transformation between the end-effector frame
+ and the tool frame.
+*/
+void
+vpRobotViper650::set_eMc(const vpHomogeneousMatrix &eMc_)
+{
+ this->vpViper650::set_eMc(eMc_);
+
+ InitTry;
+
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i=0; i < 3; i ++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i+3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try( PrimitiveCONST_Viper650(eMc_pose) );
+
+ CatchPrint();
+
+ return ;
+}
+
+/*!
+
+ Set the geometric transformation between the end-effector frame and
+ the tool frame in the low level controller.
+
+ \warning This function overwrite the transformation parameters that were
+ potentially set using one of the init functions.
+
+ \param etc_ : Translation between the end-effector frame and the tool frame.
+ \param erc_ : Rotation between the end-effector frame and the tool frame
+ using the Euler angles in radians with the XYZ convention.
+*/
+void
+vpRobotViper650::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
+{
+ this->vpViper650::set_eMc(etc_,erc_);
+
+ InitTry;
+
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i=0; i < 3; i ++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i+3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try( PrimitiveCONST_Viper650(eMc_pose) );
+
+ CatchPrint();
+
+ return ;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- DESTRUCTOR --------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Destructor.
+
+ Free allocated resources.
+*/
+vpRobotViper650::~vpRobotViper650 (void)
+{
+ InitTry;
+
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // if (HIPowerStatus == 1) {
+ // fprintf(stdout, "Power OFF the robot\n");
+ // fflush(stdout);
+
+ // Try( PrimitivePOWEROFF_Viper650() );
+ // }
+
+ // Free allocated resources
+ ShutDownConnection();
+
+ vpRobotViper650::robotAlreadyCreated = false;
+
+ CatchPrint();
+ return;
+}
+
+
+
+
+/*!
+
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+ vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ InitTry;
+
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ // Start primitive STOP only if the current state is Velocity
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ Try( PrimitiveSTOP_Viper650() );
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try( PrimitiveSTOP_Viper650() );
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ default:
+ break ;
+ }
+
+ CatchPrint();
+
+ return vpRobot::setRobotState (newState);
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- STOP --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Stop the robot and set the robot state to vpRobot::STATE_STOP.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+*/
+void
+ vpRobotViper650::stopMotion(void)
+{
+ if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
+ return;
+
+ InitTry;
+ Try( PrimitiveSTOP_Viper650() );
+ setRobotState (vpRobot::STATE_STOP);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot stop robot motion");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot stop robot motion.");
+ }
+}
+
+/*!
+
+ Power on the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot power on.
+
+ \sa powerOff(), getPowerState()
+*/
+void
+ vpRobotViper650::powerOn(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ bool firsttime = true;
+ unsigned int nitermax = 10;
+
+ for (unsigned int i=0; i<nitermax; i++) {
+ Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ if (EStopStatus == ESTOP_AUTO) {
+ controlMode = AUTO;
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_MANUAL) {
+ controlMode = MANUAL;
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_ACTIVATED) {
+ controlMode = ESTOP;
+ if (firsttime) {
+ std::cout << "Emergency stop is activated! \n"
+ << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ firsttime = false;
+ }
+ fprintf(stdout, "Remaining time %ds \r", nitermax-i);
+ fflush(stdout);
+ CAL_Wait(1);
+ }
+ else {
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ ShutDownConnection();
+ exit(0);
+ }
+ }
+
+ if (EStopStatus == ESTOP_ACTIVATED)
+ std::cout << std::endl;
+
+ if (EStopStatus == ESTOP_ACTIVATED) {
+ std::cout << "Sorry, cannot power on the robot." << std::endl;
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power on the robot.");
+ }
+
+ if (HIPowerStatus == 0) {
+ fprintf(stdout, "Power ON the Viper650 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWERON_Viper650() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power on the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Power off the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+
+ \sa powerOn(), getPowerState()
+*/
+void
+ vpRobotViper650::powerOff(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ fprintf(stdout, "Power OFF the Viper650 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWEROFF_Viper650() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power off the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Get the robot power state indication if power is on or off.
+
+ \return true if power is on. false if power is off.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error.
+
+ \sa powerOn(), powerOff()
+*/
+bool
+ vpRobotViper650::getPowerState(void) const
+{
+ InitTry;
+ bool status = false;
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ status = true;
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the power status");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get the power status.");
+ }
+ return status;
+}
+
+/*!
+
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
+ end-effector frame. This transformation allows to compute a
+ velocity expressed in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+ vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ vpViper650::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the geometric transformation \f$^c{\bf M}_e\f$ between the
+ camera frame and the end-effector frame. This transformation is
+ constant and correspond to the extrinsic camera parameters estimated
+ by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+ vpRobotViper650::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpViper650::get_cMe(cMe) ;
+}
+
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame.
+
+ To compute \f$^e{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param eJe : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
+ end-effector frame.
+
+*/
+void
+ vpRobotViper650::get_eJe(vpMatrix &eJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_J_Viper650(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = vpMath::rad(position[i]);
+
+ try
+ {
+ vpViper650::get_eJe(q, eJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ To compute \f$^f{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param fJe : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
+ reference frame.
+*/
+
+void
+ vpRobotViper650::get_fJe(vpMatrix &fJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Viper650(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try
+ {
+ vpViper650::get_fJe(q, fJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Set the maximal velocity percentage to use for a position control.
+
+ The default positioning velocity is defined by
+ vpRobotViper650::defaultPositioningVelocity. This method allows to
+ change this default positioning velocity
+
+ \param velocity : Percentage of the maximal velocity. Values should
+ be in ]0:100].
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpColVector position(6);
+ position = 0; // position in rad
+
+ vpRobotViper650 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot to the joint position [0,0,0,0,0,0]
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
+#endif
+}
+ \endcode
+
+ \sa getPositioningVelocity()
+*/
+void
+ vpRobotViper650::setPositioningVelocity (const double velocity)
+{
+ positioningVelocity = velocity;
+}
+
+/*!
+ Get the maximal velocity percentage used for a position control.
+
+ \sa setPositioningVelocity()
+*/
+double
+ vpRobotViper650::getPositioningVelocity (void) const
+{
+ return positioningVelocity;
+}
+
+
+/*!
+
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param position : A six dimension vector corresponding to the
+ position to reach. All the positions are expressed in meters for the
+ translations and radians for the rotations. If the position is out
+ of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are the six joint rotations starting
+ from the base to the end-effector.
+
+ - In the camera and the reference frame, positions are respectively
+ X,Y,Z translations and 3 rotations arround the X, Y and Z
+ axis. Rotations are represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpColVector position(6);
+ // Set positions in the camera frame
+ position[0] = 0.1; // x axis, in meter
+ position[1] = 0.2; // y axis, in meter
+ position[2] = 0.3; // z axis, in meter
+ position[3] = M_PI/8; // rotation around x axis, in rad
+ position[4] = M_PI/4; // rotation around y axis, in rad
+ position[5] = M_PI; // rotation around z axis, in rad
+
+ vpRobotViper650 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+#endif
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ \endcode
+
+*/
+void
+ vpRobotViper650::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & position )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ vpColVector destination(njoint);
+ int error = 0;
+ double timestamp;
+
+ InitTry;
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ vpColVector q(njoint);
+ Try( PrimitiveACQ_POS_Viper650(q.data, ×tamp) );
+
+ // Convert degrees into rad
+ q.deg2rad();
+
+ // Get fMc from the inverse kinematics
+ vpHomogeneousMatrix fMc;
+ vpViper650::get_fMc(q, fMc);
+
+ // Set cMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i+3];
+ }
+
+ // Compute cMc2
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
+
+ // Compute the new position to reach: fMc*cMc2
+ vpHomogeneousMatrix fMc2 = fMc * cMc2;
+
+ // Compute the corresponding joint position from the inverse kinematics
+ unsigned int solution = this->getInverseKinematics(fMc2, q);
+ if (solution) { // Position is reachable
+ destination = q;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
+ Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
+ Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
+ }
+ else {
+ // Cartesian position is out of range
+ error = -1;
+ }
+
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ destination = position;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
+
+ //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
+ Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
+ Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
+ break ;
+
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ // Convert angles from Rxyz representation to Rzyz representation
+ vpRxyzVector rxyz(position[3],position[4],position[5]);
+ vpRotationMatrix R(rxyz);
+ vpRzyzVector rzyz(R);
+
+ for (unsigned int i=0; i <3; i++) {
+ destination[i] = position[i];
+ destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
+ }
+ int configuration = 0; // keep the actual configuration
+
+ //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
+ Try( PrimitiveMOVE_C_Viper650(destination.data, configuration,
+ positioningVelocity) );
+ Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
+
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt == InvalidPosition || TryStt == -1023)
+ std::cout << " : Position out of range.\n";
+
+ else if (TryStt == -3019) {
+ if (frame == vpRobot::ARTICULAR_FRAME)
+ std::cout << " : Joint position out of range.\n";
+ else
+ std::cout << " : Cartesian position leads to a joint position out of range.\n";
+ }
+ else if (TryStt < 0)
+ std::cout << " : Unknown error (see Fabien).\n";
+ else if (error == -1)
+ std::cout << "Position out of range.\n";
+
+ if (TryStt < 0 || error < 0) {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+
+ return ;
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ This method owerloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
+ the position to reach. All the positions are expressed in meters for
+ the translations and radians for the rotations.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X (pos1), Y (pos2),
+ Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
+ translations, and A,B,C the rotations of the end-effector.
+
+ - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ // Set positions in the camera frame
+ double pos1 = 0.1; // x axis, in meter
+ double pos2 = 0.2; // y axis, in meter
+ double pos3 = 0.3; // z axis, in meter
+ double pos4 = M_PI/8; // rotation around x axis, in rad
+ double pos5 = M_PI/4; // rotation around y axis, in rad
+ double pos6 = M_PI; // rotation around z axis, in rad
+
+ vpRobotViper650 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
+#endif
+}
+ \endcode
+
+ \sa setPosition()
+*/
+void vpRobotViper650::setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6)
+{
+ try{
+ vpColVector position(6) ;
+ position[0] = pos1 ;
+ position[1] = pos2 ;
+ position[2] = pos3 ;
+ position[3] = pos4 ;
+ position[4] = pos5 ;
+ position[5] = pos6 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpColVector q;
+ vpRobotViper650 robot;
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+
+*/
+void vpRobotViper650::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Time in second since last robot power on.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+ double timestamp;
+
+ vpColVector position;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, position, timestamp);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (unsigned int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame transformation in terms of a
+ // homogeneous matrix
+ vpHomogeneousMatrix fMc(ftc, fRc);
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the position cannot
+ be get from the low level controller.
+
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const
+ vpColVector & r)
+
+*/
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &position,
+ double ×tamp)
+{
+
+ InitTry;
+
+ position.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME : {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ Try( PrimitiveACQ_POS_J_Viper650(position.data, ×tamp) );
+ //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
+ position.deg2rad();
+
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ Try( PrimitiveACQ_POS_C_Viper650(position.data, ×tamp) );
+ // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
+ // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
+ // Convert Euler Rzyz angles from deg to rad
+ for (unsigned int i=3; i <6; i++)
+ position[i] = vpMath::rad(position[i]);
+ // Convert Rzyz angles into Rxyz representation
+ vpRzyzVector rzyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rzyz);
+ vpRxyzVector rxyz(R);
+
+ // Update the position using Rxyz representation
+ for (unsigned int i=0; i <3; i++)
+ position[i+3] = rxyz[i];
+ // vpCTRACE << "Cartesian position Rxyz (deg)"
+ // << position[0] << " " << position[1] << " " << position[2] << " "
+ // << vpMath::deg(position[3]) << " "
+ // << vpMath::deg(position[4]) << " "
+ // << vpMath::deg(position[5]) << std::endl;
+
+ break ;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get position.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position.");
+ }
+
+ return;
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector &position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+
+ The difference is here that the position is returned using a ThetaU
+ representation for the rotation.
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+*/
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position,
+ double ×tamp)
+{
+ vpColVector posRxyz;
+ //recupere position en Rxyz
+ this->getPosition(frame, posRxyz, timestamp);
+ vpRxyzVector RxyzVect;
+ for (unsigned int j=0;j<3;j++)
+ RxyzVect[j]=posRxyz[j+3];
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(RxyzVect);
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+
+ The difference is here that the timestamp is not returned.
+
+*/
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+ Returns the robot controller current time (in second) since last robot power on.
+*/
+double vpRobotViper650::getTime() const
+{
+ double timestamp;
+ PrimitiveACQ_TIME_Viper650(×tamp);
+ return timestamp;
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in articular, camera frame, reference frame or mixt frame.
+
+ \param vel : Velocity vector. Translation velocities are expressed
+ in m/s while rotation velocities in rad/s. The size of this vector
+ is always 6.
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
+ camera frame.
+
+ - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
+ reference frame.
+
+ - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
+ v_y, v_z \f$ are expressed in the reference frame and rotations \f$
+ \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ while (1) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+#endif
+}
+ \endcode
+*/
+void
+ vpRobotViper650::setVelocity (const vpRobot::vpControlFrameType frame,
+ const vpColVector & vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ vpColVector vel_sat(6);
+
+ // Velocity saturation
+ switch(frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ case vpRobot::MIXT_FRAME : {
+ vpColVector vel_max(6);
+
+ for (unsigned int i=0; i<3; i++)
+ vel_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i=3; i<6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+
+ break;
+ }
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME : {
+ vpColVector vel_max(6);
+
+ if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
+ for (unsigned int i=0; i<6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ }
+ else {
+ for (unsigned int i=0; i<5; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ vel_max[5] = getMaxRotationVelocityJoint6();
+ }
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+
+ }
+ }
+
+ InitTry;
+
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ // Send velocities in m/s and rad/s
+ // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
+ Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650) );
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ // Convert all the velocities from rad/s into deg/s
+ vel_sat.rad2deg();
+ //std::cout << "Vitesse appliquee: " << vel_sat.t();
+ //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
+ Try( PrimitiveMOVESPEED_Viper650(vel_sat.data) );
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ // Send velocities in m/s and rad/s
+ std::cout << "Vitesse ref appliquee: " << vel_sat.t();
+ Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650) );
+ break ;
+ }
+ case vpRobot::MIXT_FRAME : {
+ //Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
+ break ;
+ }
+ default: {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
+ }
+
+ Catch();
+ if (TryStt < 0) {
+ if (TryStt == VelStopOnJoint) {
+ UInt32 axisInJoint[njoint];
+ PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
+ for (unsigned int i=0; i < njoint; i ++) {
+ if (axisInJoint[i])
+ std::cout << "\nWarning: Velocity control stopped: axis "
+ << i+1 << " on joint limit!" <<std::endl;
+ }
+ }
+ else {
+ printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
+ if (TryString != NULL) {
+ // The statement is in TryString, but we need to check the validity
+ printf(" Error sentence %s\n", TryString); // Print the TryString
+ }
+ else {
+ printf("\n");
+ }
+ }
+ }
+
+ return;
+}
+
+
+
+
+
+
+/* ------------------------------------------------------------------------ */
+/* --- GET ---------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param velocity : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \warning The first time this method is called, \e velocity is set to 0. The
+ first call is used to intialise the velocity computation for the next call.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpRobotViper650 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ while (1) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+#endif
+}
+ \endcode
+*/
+void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp)
+{
+ velocity.resize (6);
+ velocity = 0;
+
+ vpColVector q_cur(6);
+ vpHomogeneousMatrix fMc_cur;
+ vpHomogeneousMatrix cMc; // camera displacement
+ double time_cur;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_J_Viper650(q_cur.data, ×tamp) );
+ time_cur = timestamp;
+ q_cur.deg2rad();
+
+ // Get the camera pose from the direct kinematics
+ vpViper650::get_fMc(q_cur, fMc_cur);
+
+ if ( ! first_time_getvel ) {
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
+
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the ThetaU representation for the rotation
+ vpRotationMatrix cRc;
+ cMc.extract(cRc);
+ vpThetaUVector thetaU;
+ thetaU.buildFrom(cRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ // Compute the translation displacement in the reference frame
+ velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
+ // Update the rotation displacement in the camera frame
+ velocity[i+3] = thetaU[i];
+ }
+
+ // Compute the velocity
+ velocity /= (time_cur - time_prev_getvel);
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getvel = false;
+ }
+
+ // Memorize the camera pose for the next call
+ fMc_prev_getvel = fMc_cur;
+
+ // Memorize the joint position for the next call
+ q_prev_getvel = q_cur;
+
+ // Memorize the time associated to the joint position for the next call
+ time_prev_getvel = time_cur;
+
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
+ except that the timestamp is not returned.
+
+ */
+void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity)
+{
+ double timestamp;
+ getVelocity(frame, velocity, timestamp);
+}
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpRobotViper650 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ double timestamp;
+ while (1) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+#endif
+}
+ \endcode
+*/
+vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ vpColVector velocity;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
+ except that the timestamp is not returned.
+
+ */
+vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector velocity;
+ double timestamp;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+Read joint positions in a specific Viper650 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#Viper - Position - Version 1.0
+# file: "myposition.pos "
+#
+# R: A B C D E F
+# Joint position in degrees
+#
+
+R: 0.1 0.3 -0.25 -80.5 80 0
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : The six joint positions. Values are expressed in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+
+ // Enable the position control of the robot
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Get the current robot joint positions
+ vpColVector q; // Current joint position
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ // Save this position in a file named "current.pos"
+ robot.savePosFile("current.pos", q);
+
+ // Get the position from a file and move to the registered position
+ robot.readPosFile("current.pos", q); // Set the joint position from the file
+
+ robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+#endif
+}
+\endcode
+
+\sa savePosFile()
+*/
+
+bool vpRobotViper650::readPosFile(const char *filename, vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+ // else
+ // return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ return (false); /* fin fichier */
+ }
+
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(njoint);
+ sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2],
+ &q[3], &q[4], &q[5]);
+
+ // converts rotations from degrees into radians
+ q.deg2rad();
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Save joint (articular) positions in a specific Viper650 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : The six joint positions to save in the
+ filename. Values are expressed in radians.
+
+ \warning All the six joint rotations written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+
+bool
+ vpRobotViper650::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#Viper650 - Position - Version 1.00\n\
+#\n\
+# R: A B C D E F\n\
+# Joint position in degrees\n\
+#\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
+ vpMath::deg(q[0]),
+ vpMath::deg(q[1]),
+ vpMath::deg(q[2]),
+ vpMath::deg(q[3]),
+ vpMath::deg(q[4]),
+ vpMath::deg(q[5]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Moves the robot to the joint position specified in the filename. The
+ positioning velocity is set to 10% of the robot maximal velocity.
+
+ \param filename: File containing a joint position.
+
+ \sa readPosFile
+
+*/
+void
+ vpRobotViper650::move(const char *filename)
+{
+ vpColVector q;
+
+ try {
+ this->readPosFile(filename, q) ;
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->setPositioningVelocity(10);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ }
+ catch(...) {
+ throw;
+ }
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in the camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+ vpRobotViper650::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+/*!
+
+ Get the robot joint displacement since the last call of this method.
+
+ \param displacement : The measured joint displacement. The dimension
+ of \e displacement is 6 (the robot joint number). All the values are
+ expressed in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void
+ vpRobotViper650::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ cartesian space. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call
+ of this method. The dimension of \e displacement is always
+ 6. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+ vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+
+ double q[6];
+ vpColVector q_cur(6);
+ double timestamp;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Viper650(q, ×tamp) );
+ for (unsigned int i=0; i < njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ if ( ! first_time_getdis ) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Bias the force/torque sensor.
+
+ \warning This function waits 500 ms after the bias request to be sure the
+ next measures take into account the bias.
+
+ \exception vpRobotException::lowLevelError : If the force/torque sensor bias
+ cannot be done on the low level controller.
+
+ \sa getForceTorque()
+
+*/
+void
+ vpRobotViper650::biasForceTorqueSensor() const
+{
+ InitTry;
+
+ Try( PrimitiveTFS_BIAS_Viper650() );
+
+ // Wait 500 ms to be sure the next measures take into account the bias
+ vpTime::wait(500);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot bias the force/torque sensor.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot bias the force/torque sensor.");
+ }
+}
+
+/*!
+
+ Get the rough force/torque sensor measures.
+
+ \param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
+
+ The code below shows how to get the force/torque measures after a sensor bias.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER650
+ vpColVector H; // force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz]
+
+ vpRobotViper650 robot;
+
+ // Bias the force/torque sensor
+ robot.biasForceTorqueSensor();
+
+ for (unsigned int i=0; i< 10; i++) {
+ robot.getForceTorque(H) ;
+ std::cout << "Measured force/torque: " << H.t() << std::endl;
+ vpTime::wait(5);
+ }
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the force/torque measures
+ cannot be get from the low level controller.
+
+ \sa biasForceTorqueSensor()
+
+*/
+void
+ vpRobotViper650::getForceTorque(vpColVector &H) const
+{
+ InitTry;
+
+ H.resize (6);
+
+ Try( PrimitiveTFS_ACQ_Viper650(H.data) );
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the force/torque measures.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get force/torque measures.");
+ }
+}
+
+/*!
+ Enable the joint limits on axis number 6. This is the default.
+
+ \sa disbleJoint6Limits()
+*/
+void vpRobotViper650::enableJoint6Limits() const
+{
+ InitTry;
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
+ std::cout << "Enable joint limits on axis 6..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot enable joint limits on axis 6");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot enable joint limits on axis 6.");
+ }
+}
+
+/*!
+ \warning Each call to this function should be done carefully.
+
+ Disable the joint limits on axis number 6. When joint 6 is outside the limits,
+ a call to this function allows to bring the robot to a position inside the limits.
+ Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
+
+ \sa enableJoint6Limits()
+*/
+void vpRobotViper650::disableJoint6Limits() const
+{
+ InitTry;
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1) );
+ std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot disable joint limits on axis 6");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot disable joint limits on axis 6.");
+ }
+}
+
+/*!
+
+ Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+
+ \param w_max : Maximum rotation velocity expressed in rad/s.
+*/
+
+void
+vpRobotViper650::setMaxRotationVelocity (double w_max)
+{
+ vpRobot::setMaxRotationVelocity(w_max);
+ setMaxRotationVelocityJoint6(w_max);
+
+ return;
+}
+
+/*!
+
+ Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+
+ This function affects only the velocities that are sent as joint velocities.
+
+ \code
+ vpRobotViper650 robot;
+ robot.setMaxRotationVelocity( vpMath::rad(20) );
+ robot.setMaxRotationVelocityJoint6( vpMath::rad(50) );
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ robot.setVelocity(ARTICULAR_FRAME, v);
+ \endcode
+
+
+ \param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
+*/
+
+void
+vpRobotViper650::setMaxRotationVelocityJoint6 (const double w6_max)
+{
+ maxRotationVelocity_joint6 = w6_max;
+ return;
+}
+
+/*!
+
+ Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+
+ \return Maximum rotation velocity on joint 6 expressed in rad/s.
+*/
+double
+vpRobotViper650::getMaxRotationVelocityJoint6() const
+{
+ return maxRotationVelocity_joint6;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has no symbols
+void dummy_vpRobotViper650() {};
+#endif
diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp
new file mode 100644
index 0000000..71b433c
--- /dev/null
+++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp
@@ -0,0 +1,2399 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the Irisa's Viper S850 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_VIPER850
+
+#include <signal.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ----------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+bool vpRobotViper850::robotAlreadyCreated = false;
+
+/*!
+
+ Default positioning velocity in percentage of the maximum
+ velocity. This value is set to 15. The member function
+ setPositioningVelocity() allows to change this value.
+
+*/
+const double vpRobotViper850::defaultPositioningVelocity = 15.0;
+
+/* ---------------------------------------------------------------------- */
+/* --- EMERGENCY STOP --------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ Emergency stops the robot if the program is interrupted by a SIGINT
+ (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
+ or SIGQUIT signal.
+
+*/
+void emergencyStopViper850(int signo)
+{
+ std::cout << "Stop the Viper850 application by signal ("
+ << signo << "): " << (char)7 ;
+ switch(signo)
+ {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ case SIGBUS:
+ std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ case SIGSEGV:
+ std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ case SIGKILL:
+ std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ case SIGQUIT:
+ std::cout <<"SIGQUIT " << std::endl ; break ;
+ default :
+ std::cout << signo << std::endl ;
+}
+ //std::cout << "Emergency stop called\n";
+ // PrimitiveESTOP_Viper850();
+ PrimitiveSTOP_Viper850();
+ std::cout << "Robot was stopped\n";
+
+ // Free allocated resources
+ // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
+
+ fprintf(stdout, "Application ");
+ fflush(stdout);
+ kill(getpid(), SIGKILL);
+ exit(1) ;
+}
+
+/* ---------------------------------------------------------------------- */
+/* --- CONSTRUCTOR ------------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+
+/*!
+
+ The only available constructor.
+
+ This contructor calls init() to initialise the connection with the
+ MotionBox or low level controller, send the default \f$^e{\bf
+ M}_c\f$ homogeneous matrix and power on the robot.
+
+ It also set the robot state to vpRobot::STATE_STOP.
+
+ To set the extrinsic camera parameters related to the \f$^e{\bf
+ M}_c\f$ matrix obtained with a camera perspective projection model
+ including the distorsion, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters associated to an
+ image acquired by the camera attached to the robot, with:
+
+ \code
+ vpImage<unsigned char> I(480, 640);
+
+ // Get an image from the camera attached to the robot
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+ g.acquire(I);
+#endif
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+#endif
+}
+ \endcode
+
+ \sa vpCameraParameters, init(vpViper850::vpViper850CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType)
+
+*/
+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
+ */
+
+ signal(SIGINT, emergencyStopViper850);
+ signal(SIGBUS, emergencyStopViper850) ;
+ signal(SIGSEGV, emergencyStopViper850) ;
+ signal(SIGKILL, emergencyStopViper850);
+ signal(SIGQUIT, emergencyStopViper850);
+
+ setVerbose(verbose);
+ if (verbose_)
+ std::cout << "Open communication with MotionBlox.\n";
+ try {
+ this->init();
+ this->setRobotState(vpRobot::STATE_STOP) ;
+ }
+ catch(...) {
+ // vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ positioningVelocity = defaultPositioningVelocity ;
+
+ maxRotationVelocity_joint6 = maxRotationVelocity;
+
+ vpRobotViper850::robotAlreadyCreated = true;
+
+ return ;
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- INITIALISATION ----------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Initialise the connection with the MotionBox or low level
+ controller, send the default eMc homogeneous matrix, power on the
+ robot and wait 1 sec before returning to be sure the initialisation
+ is done.
+
+ \warning This method sets the camera extrinsic parameters (matrix
+ eMc) to the one obtained by calibration with a camera projection
+ model without distorsion by calling
+ init(vpViper850::defaultCameraRobot). If you want to set the extrinsic
+ camera parameters to those obtained with a camera perspective model
+ including the distorsion you have to call the
+ init(vpViper850::vpViper850CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType) method.
+
+ \sa vpCameraParameters, init(vpViper850::vpViper850CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType)
+*/
+void
+ vpRobotViper850::init (void)
+{
+ InitTry;
+
+ // Initialise private variables used to compute the measured velocities
+ q_prev_getvel.resize(6);
+ q_prev_getvel = 0;
+ time_prev_getvel = 0;
+ first_time_getvel = true;
+
+ // Initialise private variables used to compute the measured displacement
+ q_prev_getdis.resize(6);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+
+ // Initialize the firewire connection
+ Try( InitializeConnection(verbose_) );
+
+ // Connect to the servoboard using the servo board GUID
+ Try( InitializeNode_Viper850() );
+
+ Try( PrimitiveRESET_Viper850() );
+
+ // Enable the joint limits on axis 6
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
+
+ // Update the eMc matrix in the low level controller
+ init(vpViper850::defaultTool);
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // Print the robot status
+ if (verbose_) {
+ std::cout << "Robot status: ";
+ switch(EStopStatus) {
+ case ESTOP_AUTO:
+ controlMode = AUTO;
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+
+ case ESTOP_MANUAL:
+ controlMode = MANUAL;
+ if (HIPowerStatus == 0)
+ std::cout << "Power is OFF" << std::endl;
+ else
+ std::cout << "Power is ON" << std::endl;
+ break;
+ case ESTOP_ACTIVATED:
+ controlMode = ESTOP;
+ std::cout << "Emergency stop is activated" << std::endl;
+ break;
+ default:
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ }
+ std::cout << std::endl;
+ }
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ // If an error occur in the low level controller, goto here
+ //CatchPrint();
+ Catch();
+
+ // Test if an error occurs
+ if (TryStt == -20001)
+ printf("No connection detected. Check if the robot is powered on \n"
+ "and if the firewire link exist between the MotionBlox and this computer.\n");
+ else if (TryStt == -675)
+ printf(" Timeout enabling power...\n");
+
+ if (TryStt < 0) {
+ // Power off the robot
+ PrimitivePOWEROFF_Viper850();
+ // Free allocated resources
+ ShutDownConnection();
+
+ std::cout << "Cannot open connexion with the motionblox..." << std::endl;
+ throw vpRobotException (vpRobotException::constructionError,
+ "Cannot open connexion with the motionblox");
+ }
+ return ;
+}
+
+/*!
+
+ Initialize the robot kinematics with the extrinsic calibration
+ parameters associated to a specific camera (set the eMc homogeneous
+ parameters in the low level controller) and also get the joint
+ limits from the low-level controller.
+
+ The eMc parameters depend on the camera and the projection model in use.
+
+ \param tool : Tool to use.
+
+ \param projModel : Projection model associated to the camera.
+
+ To set the extrinsic camera parameters related to the \f$^e{\bf
+ M}_c\f$ matrix obtained with a camera perspective projection model
+ including the distorsion, use the code below:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpCameraParameters.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ // Set the extrinsic camera parameters obtained with a perpective
+ // projection model including a distorsion parameter
+ robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ \endcode
+
+ Now, you can get the intrinsic camera parameters associated to an
+ image acquired by the camera attached to the robot, with:
+
+ \code
+ vpImage<unsigned char> I(480, 640);
+
+ // Get an image from the camera attached to the robot
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+ g.acquire(I);
+#endif
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, I);
+ // In cam, you get the intrinsic parameters of the projection model
+ // with distorsion.
+#endif
+}
+ \endcode
+
+ \sa vpCameraParameters, init()
+*/
+void
+ vpRobotViper850::init (vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel)
+{
+
+ InitTry;
+ // Read the robot constants from files
+ // - joint [min,max], coupl_56, long_56
+ // - camera extrinsic parameters relative to eMc
+ vpViper850::init(tool, projModel);
+
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i=0; i < 3; i ++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i+3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try( PrimitiveCONST_Viper850(eMc_pose) );
+
+ // get real joint min/max from the MotionBlox
+ Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
+
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // }
+
+ setToolType(tool);
+
+ CatchPrint();
+ return ;
+}
+
+/* ------------------------------------------------------------------------ */
+/* --- DESTRUCTOR --------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Destructor.
+
+ Free allocated resources.
+*/
+vpRobotViper850::~vpRobotViper850 (void)
+{
+ InitTry;
+
+ setRobotState(vpRobot::STATE_STOP) ;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ // if (HIPowerStatus == 1) {
+ // fprintf(stdout, "Power OFF the robot\n");
+ // fflush(stdout);
+
+ // Try( PrimitivePOWEROFF_Viper850() );
+ // }
+
+ // Free allocated resources
+ ShutDownConnection();
+
+ vpRobotViper850::robotAlreadyCreated = false;
+
+ CatchPrint();
+ return;
+}
+
+
+
+
+/*!
+
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+ vpRobotViper850::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ InitTry;
+
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ // Start primitive STOP only if the current state is Velocity
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ Try( PrimitiveSTOP_Viper850() );
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try( PrimitiveSTOP_Viper850() );
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ this->powerOn();
+ break;
+ }
+ default:
+ break ;
+ }
+
+ CatchPrint();
+
+ return vpRobot::setRobotState (newState);
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* --- STOP --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+/*!
+
+ Stop the robot and set the robot state to vpRobot::STATE_STOP.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+*/
+void
+ vpRobotViper850::stopMotion(void)
+{
+ if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
+ return;
+
+ InitTry;
+ Try( PrimitiveSTOP_Viper850() );
+ setRobotState (vpRobot::STATE_STOP);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot stop robot motion");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot stop robot motion.");
+ }
+}
+
+/*!
+
+ Power on the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot power on.
+
+ \sa powerOff(), getPowerState()
+*/
+void
+ vpRobotViper850::powerOn(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ UInt32 EStopStatus;
+ bool firsttime = true;
+ unsigned int nitermax = 10;
+
+ for (unsigned int i=0; i<nitermax; i++) {
+ Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
+ &HIPowerStatus));
+ if (EStopStatus == ESTOP_AUTO) {
+ controlMode = AUTO;
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_MANUAL) {
+ controlMode = MANUAL;
+ break; // exit for loop
+ }
+ else if (EStopStatus == ESTOP_ACTIVATED) {
+ controlMode = ESTOP;
+ if (firsttime) {
+ std::cout << "Emergency stop is activated! \n"
+ << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ firsttime = false;
+ }
+ fprintf(stdout, "Remaining time %ds \r", nitermax-i);
+ fflush(stdout);
+ CAL_Wait(1);
+ }
+ else {
+ std::cout << "Sorry there is an error on the emergency chain." << std::endl;
+ std::cout << "You have to call Adept for maintenance..." << std::endl;
+ // Free allocated resources
+ ShutDownConnection();
+ exit(0);
+ }
+ }
+
+ if (EStopStatus == ESTOP_ACTIVATED)
+ std::cout << std::endl;
+
+ if (EStopStatus == ESTOP_ACTIVATED) {
+ std::cout << "Sorry, cannot power on the robot." << std::endl;
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power on the robot.");
+ }
+
+ if (HIPowerStatus == 0) {
+ fprintf(stdout, "Power ON the Viper850 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWERON_Viper850() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power on the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Power off the robot.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error during robot stopping.
+
+ \sa powerOn(), getPowerState()
+*/
+void
+ vpRobotViper850::powerOff(void)
+{
+ InitTry;
+
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ fprintf(stdout, "Power OFF the Viper850 robot\n");
+ fflush(stdout);
+
+ Try( PrimitivePOWEROFF_Viper850() );
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot power off the robot");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot power off the robot.");
+ }
+}
+
+/*!
+
+ Get the robot power state indication if power is on or off.
+
+ \return true if power is on. false if power is off.
+
+ \exception vpRobotException::lowLevelError : If the low level
+ controller returns an error.
+
+ \sa powerOn(), powerOff()
+*/
+bool
+ vpRobotViper850::getPowerState(void) const
+{
+ InitTry;
+ bool status = false;
+ // Look if the power is on or off
+ UInt32 HIPowerStatus;
+ Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
+ &HIPowerStatus));
+ CAL_Wait(0.1);
+
+ if (HIPowerStatus == 1) {
+ status = true;
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the power status");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get the power status.");
+ }
+ return status;
+}
+
+/*!
+
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
+ end-effector frame. This transformation allows to compute a
+ velocity expressed in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+
+*/
+void
+ vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ vpViper850::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+
+ Get the geometric transformation \f$^c{\bf M}_e\f$ between the
+ camera frame and the end-effector frame. This transformation is
+ constant and correspond to the extrinsic camera parameters estimated
+ by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+*/
+void
+ vpRobotViper850::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ vpViper850::get_cMe(cMe) ;
+}
+
+
+/*!
+
+ Get the robot jacobian expressed in the end-effector frame.
+
+ To compute \f$^e{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param eJe : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
+ end-effector frame.
+
+*/
+void
+ vpRobotViper850::get_eJe(vpMatrix &eJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = vpMath::rad(position[i]);
+
+ try
+ {
+ vpViper850::get_eJe(q, eJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+/*!
+
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ To compute \f$^f{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param fJe : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
+ reference frame.
+*/
+
+void
+ vpRobotViper850::get_fJe(vpMatrix &fJe)
+{
+
+ double position[6];
+ double timestamp;
+
+ InitTry;
+ Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
+ CatchPrint();
+
+ vpColVector q(6);
+ for (unsigned int i=0; i < njoint; i++)
+ q[i] = position[i];
+
+ try
+ {
+ vpViper850::get_fJe(q, fJe) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Set the maximal velocity percentage to use for a position control.
+
+ The default positioning velocity is defined by
+ vpRobotViper850::defaultPositioningVelocity. This method allows to
+ change this default positioning velocity
+
+ \param velocity : Percentage of the maximal velocity. Values should
+ be in ]0:100].
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpColVector position(6);
+ position = 0; // position in rad
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot to the joint position [0,0,0,0,0,0]
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
+#endif
+}
+ \endcode
+
+ \sa getPositioningVelocity()
+*/
+void
+ vpRobotViper850::setPositioningVelocity (const double velocity)
+{
+ positioningVelocity = velocity;
+}
+
+/*!
+ Get the maximal velocity percentage used for a position control.
+
+ \sa setPositioningVelocity()
+*/
+double
+ vpRobotViper850::getPositioningVelocity (void) const
+{
+ return positioningVelocity;
+}
+
+
+/*!
+
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param position : A six dimension vector corresponding to the
+ position to reach. All the positions are expressed in meters for the
+ translations and radians for the rotations. If the position is out
+ of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are the six joint rotations starting
+ from the base to the end-effector.
+
+ - In the camera and the reference frame, positions are respectively
+ X,Y,Z translations and 3 rotations arround the X, Y and Z
+ axis. Rotations are represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpColVector position(6);
+ // Set positions in the camera frame
+ position[0] = 0.1; // x axis, in meter
+ position[1] = 0.2; // y axis, in meter
+ position[2] = 0.3; // z axis, in meter
+ position[3] = M_PI/8; // rotation around x axis, in rad
+ position[4] = M_PI/4; // rotation around y axis, in rad
+ position[5] = M_PI; // rotation around z axis, in rad
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+#endif
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ \endcode
+
+*/
+void
+ vpRobotViper850::setPosition (const vpRobot::vpControlFrameType frame,
+ const vpColVector & position )
+{
+
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ vpColVector destination(njoint);
+ int error = 0;
+ double timestamp;
+
+ InitTry;
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ vpColVector q(njoint);
+ Try( PrimitiveACQ_POS_Viper850(q.data, ×tamp) );
+
+ // Convert degrees into rad
+ q.deg2rad();
+
+ // Get fMc from the inverse kinematics
+ vpHomogeneousMatrix fMc;
+ vpViper850::get_fMc(q, fMc);
+
+ // Set cMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i+3];
+ }
+
+ // Compute cMc2
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
+
+ // Compute the new position to reach: fMc*cMc2
+ vpHomogeneousMatrix fMc2 = fMc * cMc2;
+
+ // Compute the corresponding joint position from the inverse kinematics
+ unsigned int solution = this->getInverseKinematics(fMc2, q);
+ if (solution) { // Position is reachable
+ destination = q;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
+ Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
+ Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
+ }
+ else {
+ // Cartesian position is out of range
+ error = -1;
+ }
+
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ destination = position;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
+
+ //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
+ Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
+ Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
+ break ;
+
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ // Convert angles from Rxyz representation to Rzyz representation
+ vpRxyzVector rxyz(position[3],position[4],position[5]);
+ vpRotationMatrix R(rxyz);
+ vpRzyzVector rzyz(R);
+
+ for (unsigned int i=0; i <3; i++) {
+ destination[i] = position[i];
+ destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
+ }
+ int configuration = 0; // keep the actual configuration
+
+ //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
+ Try( PrimitiveMOVE_C_Viper850(destination.data, configuration,
+ positioningVelocity) );
+ Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
+
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt == InvalidPosition || TryStt == -1023)
+ std::cout << " : Position out of range.\n";
+ else if (TryStt == -3019) {
+ if (frame == vpRobot::ARTICULAR_FRAME)
+ std::cout << " : Joint position out of range.\n";
+ else
+ std::cout << " : Cartesian position leads to a joint position out of range.\n";
+ }
+ else if (TryStt < 0)
+ std::cout << " : Unknown error (see Fabien).\n";
+ else if (error == -1)
+ std::cout << "Position out of range.\n";
+
+ if (TryStt < 0 || error < 0) {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+
+ return ;
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ This method owerloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
+ the position to reach. All the positions are expressed in meters for
+ the translations and radians for the rotations.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X (pos1), Y (pos2),
+ Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
+ translations, and A,B,C the rotations of the end-effector.
+
+ - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ // Set positions in the camera frame
+ double pos1 = 0.1; // x axis, in meter
+ double pos2 = 0.2; // y axis, in meter
+ double pos3 = 0.3; // z axis, in meter
+ double pos4 = M_PI/8; // rotation around x axis, in rad
+ double pos5 = M_PI/4; // rotation around y axis, in rad
+ double pos6 = M_PI; // rotation around z axis, in rad
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
+#endif
+}
+ \endcode
+
+ \sa setPosition()
+*/
+void vpRobotViper850::setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6)
+{
+ try{
+ vpColVector position(6) ;
+ position[0] = pos1 ;
+ position[1] = pos2 ;
+ position[2] = pos3 ;
+ position[3] = pos4 ;
+ position[4] = pos5 ;
+ position[5] = pos6 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpColVector q;
+ vpRobotViper850 robot;
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+
+*/
+void vpRobotViper850::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Time in second since last robot power on.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ vpColVector position;
+ double timestamp;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, position, timestamp);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (unsigned int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame transformation in terms of a
+ // homogeneous matrix
+ vpHomogeneousMatrix fMc(ftc, fRc);
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the position cannot
+ be get from the low level controller.
+
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const
+ vpColVector & r)
+
+*/
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector & position, double ×tamp)
+{
+
+ InitTry;
+
+ position.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME : {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ Try( PrimitiveACQ_POS_J_Viper850(position.data, ×tamp) );
+ //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
+ position.deg2rad();
+
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ Try( PrimitiveACQ_POS_C_Viper850(position.data, ×tamp) );
+ // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
+ // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
+ // Convert Euler Rzyz angles from deg to rad
+ for (unsigned int i=3; i <6; i++)
+ position[i] = vpMath::rad(position[i]);
+ // Convert Rzyz angles into Rxyz representation
+ vpRzyzVector rzyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rzyz);
+ vpRxyzVector rxyz(R);
+
+ // Update the position using Rxyz representation
+ for (unsigned int i=0; i <3; i++)
+ position[i+3] = rxyz[i];
+ // vpCTRACE << "Cartesian position Rxyz (deg)"
+ // << position[0] << " " << position[1] << " " << position[2] << " "
+ // << vpMath::deg(position[3]) << " "
+ // << vpMath::deg(position[4]) << " "
+ // << vpMath::deg(position[5]) << std::endl;
+
+ break ;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position in mixt frame: "
+ "not implemented");
+ break ;
+ }
+ }
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get position.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get position.");
+ }
+
+ return;
+}
+
+/*!
+ Returns the robot controller current time (in second) since last robot power on.
+*/
+double vpRobotViper850::getTime() const
+{
+ double timestamp;
+ PrimitiveACQ_TIME_Viper850(×tamp);
+ return timestamp;
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
+ vpColVector & position)
+{
+ double timestamp;
+ getPosition(frame, position, timestamp);
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+
+ The difference is here that the position is returned using a ThetaU
+ representation.
+
+*/
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position,
+ double ×tamp)
+{
+ vpColVector posRxyz;
+ //recupere position en Rxyz
+ this->getPosition(frame, posRxyz, timestamp);
+ vpRxyzVector RxyzVect;
+ for (unsigned int j=0;j<3;j++)
+ RxyzVect[j]=posRxyz[j+3];
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(RxyzVect);
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+
+ The difference is here that the timestamp is not used.
+
+*/
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position)
+{
+ vpColVector posRxyz;
+ double timestamp;
+ //recupere position en Rxyz
+ this->getPosition(frame, posRxyz, timestamp);
+ vpRxyzVector RxyzVect;
+ for (unsigned int j=0;j<3;j++)
+ RxyzVect[j]=posRxyz[j+3];
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(RxyzVect);
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in articular, camera frame, reference frame or mixt frame.
+
+ \param vel : Velocity vector. Translation velocities are expressed
+ in m/s while rotation velocities in rad/s. The size of this vector
+ is always 6.
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
+ camera frame.
+
+ - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
+ reference frame.
+
+ - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
+ v_y, v_z \f$ are expressed in the reference frame and rotations \f$
+ \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ while (1) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+#endif
+}
+ \endcode
+*/
+void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector & vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ vpColVector vel_sat(6);
+
+ // Velocity saturation
+ switch(frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ case vpRobot::MIXT_FRAME : {
+ vpColVector vel_max(6);
+
+ for (unsigned int i=0; i<3; i++)
+ vel_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i=3; i<6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+
+ break;
+ }
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME : {
+ vpColVector vel_max(6);
+
+ if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
+ for (unsigned int i=0; i<6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ }
+ else {
+ for (unsigned int i=0; i<5; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ vel_max[5] = getMaxRotationVelocityJoint6();
+ }
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ }
+ }
+
+ InitTry;
+
+ switch(frame) {
+ case vpRobot::CAMERA_FRAME : {
+ // Send velocities in m/s and rad/s
+ // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
+ Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850) );
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME : {
+ // Convert all the velocities from rad/s into deg/s
+ vel_sat.rad2deg();
+ //std::cout << "Vitesse appliquee: " << vel_sat.t();
+ //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
+ Try( PrimitiveMOVESPEED_Viper850(vel_sat.data) );
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME : {
+ // Send velocities in m/s and rad/s
+ std::cout << "Vitesse ref appliquee: " << vel_sat.t();
+ Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850) );
+ break ;
+ }
+ case vpRobot::MIXT_FRAME : {
+ //Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
+ break ;
+ }
+ default: {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
+ }
+
+ Catch();
+ if (TryStt < 0) {
+ if (TryStt == VelStopOnJoint) {
+ UInt32 axisInJoint[njoint];
+ PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
+ for (unsigned int i=0; i < njoint; i ++) {
+ if (axisInJoint[i])
+ std::cout << "\nWarning: Velocity control stopped: axis "
+ << i+1 << " on joint limit!" <<std::endl;
+ }
+ }
+ else {
+ printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
+ if (TryString != NULL) {
+ // The statement is in TryString, but we need to check the validity
+ printf(" Error sentence %s\n", TryString); // Print the TryString
+ }
+ else {
+ printf("\n");
+ }
+ }
+ }
+
+ return;
+}
+
+
+
+
+
+
+/* ------------------------------------------------------------------------ */
+/* --- GET ---------------------------------------------------------------- */
+/* ------------------------------------------------------------------------ */
+
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param velocity : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \warning The first time this method is called, \e velocity is set to 0. The
+ first call is used to intialise the velocity computation for the next call.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ while (1) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+#endif
+}
+ \endcode
+*/
+void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity, double ×tamp)
+{
+
+ velocity.resize (6);
+ velocity = 0;
+
+ vpColVector q_cur(6);
+ vpHomogeneousMatrix fMc_cur;
+ vpHomogeneousMatrix cMc; // camera displacement
+ double time_cur;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp) );
+ time_cur = timestamp;
+ q_cur.deg2rad();
+
+ // Get the camera pose from the direct kinematics
+ vpViper850::get_fMc(q_cur, fMc_cur);
+
+ if ( ! first_time_getvel ) {
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ velocity = (q_cur - q_prev_getvel)
+ / (time_cur - time_prev_getvel);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
+
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the ThetaU representation for the rotation
+ vpRotationMatrix cRc;
+ cMc.extract(cRc);
+ vpThetaUVector thetaU;
+ thetaU.buildFrom(cRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ // Compute the translation displacement in the reference frame
+ velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
+ // Update the rotation displacement in the camera frame
+ velocity[i+3] = thetaU[i];
+ }
+
+ // Compute the velocity
+ velocity /= (time_cur - time_prev_getvel);
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getvel = false;
+ }
+
+ // Memorize the camera pose for the next call
+ fMc_prev_getvel = fMc_cur;
+
+ // Memorize the joint position for the next call
+ q_prev_getvel = q_cur;
+
+ // Memorize the time associated to the joint position for the next call
+ time_prev_getvel = time_cur;
+
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
+ except that the timestamp is not returned.
+
+ */
+void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
+ vpColVector & velocity)
+{
+ double timestamp;
+ getVelocity(frame, velocity, timestamp);
+}
+
+/*!
+
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Time in second since last robot power on.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
+ // q_dot_mes is resized to 6, the number of joint
+
+ double timestamp;
+ while (1) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+#endif
+}
+ \endcode
+*/
+vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ vpColVector velocity;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+ Get robot velocities.
+
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
+ except that the timestamp is not returned.
+
+ */
+vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector velocity;
+ double timestamp;
+ getVelocity (frame, velocity, timestamp);
+
+ return velocity;
+}
+
+/*!
+
+Read joint positions in a specific Viper850 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#Viper - Position - Version 1.0
+# file: "myposition.pos "
+#
+# R: A B C D E F
+# Joint position in degrees
+#
+
+R: 0.1 0.3 -0.25 -80.5 80 0
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : The six joint positions. Values are expressed in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+
+ // Enable the position control of the robot
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Get the current robot joint positions
+ vpColVector q; // Current joint position
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ // Save this position in a file named "current.pos"
+ robot.savePosFile("current.pos", q);
+
+ // Get the position from a file and move to the registered position
+ robot.readPosFile("current.pos", q); // Set the joint position from the file
+
+ robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+#endif
+}
+\endcode
+
+\sa savePosFile()
+*/
+
+bool
+ vpRobotViper850::readPosFile(const char *filename, vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+ // else
+ // return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ return (false); /* fin fichier */
+ }
+
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(njoint);
+ sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2],
+ &q[3], &q[4], &q[5]);
+
+ // converts rotations from degrees into radians
+ q.deg2rad();
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Save joint (articular) positions in a specific Viper850 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : The six joint positions to save in the
+ filename. Values are expressed in radians.
+
+ \warning All the six joint rotations written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+
+bool
+ vpRobotViper850::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#Viper850 - Position - Version 1.00\n\
+#\n\
+# R: A B C D E F\n\
+# Joint position in degrees\n\
+#\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
+ vpMath::deg(q[0]),
+ vpMath::deg(q[1]),
+ vpMath::deg(q[2]),
+ vpMath::deg(q[3]),
+ vpMath::deg(q[4]),
+ vpMath::deg(q[5]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+
+ Moves the robot to the joint position specified in the filename. The
+ positioning velocity is set to 10% of the robot maximal velocity.
+
+ \param filename: File containing a joint position.
+
+ \sa readPosFile
+
+*/
+void
+ vpRobotViper850::move(const char *filename)
+{
+ vpColVector q;
+
+ try {
+ this->readPosFile(filename, q) ;
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->setPositioningVelocity(10);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ }
+ catch(...) {
+ throw;
+ }
+}
+
+/*!
+
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in the camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+
+*/
+void
+ vpRobotViper850::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+/*!
+
+ Get the robot joint displacement since the last call of this method.
+
+ \param displacement : The measured joint displacement. The dimension
+ of \e displacement is 6 (the robot joint number). All the values are
+ expressed in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+
+*/
+void
+ vpRobotViper850::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ cartesian space. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call
+ of this method. The dimension of \e displacement is always
+ 6. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+
+*/
+void
+ vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+
+ double q[6];
+ vpColVector q_cur(6);
+ double timestamp;
+
+ InitTry;
+
+ // Get the current joint position
+ Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
+ for (unsigned int i=0; i < njoint; i ++) {
+ q_cur[i] = q[i];
+ }
+
+ if ( ! first_time_getdis ) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get velocity.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get velocity.");
+ }
+}
+
+/*!
+
+ Bias the force/torque sensor.
+
+ \warning This function waits 500 ms after the bias request to be sure the
+ next measures take into account the bias.
+
+ \exception vpRobotException::lowLevelError : If the force/torque sensor bias
+ cannot be done on the low level controller.
+
+ \sa getForceTorque()
+
+*/
+void
+ vpRobotViper850::biasForceTorqueSensor() const
+{
+ InitTry;
+
+ Try( PrimitiveTFS_BIAS_Viper850() );
+
+ // Wait 500 ms to be sure the next measures take into account the bias
+ vpTime::wait(500);
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot bias the force/torque sensor.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot bias the force/torque sensor.");
+ }
+}
+
+/*!
+
+ Get the rough force/torque sensor measures.
+
+ \param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
+
+ The code below shows how to get the force/torque measures after a sensor bias.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTime.h>
+
+int main()
+{
+#ifdef VISP_HAVE_VIPER850
+ vpColVector H; // force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz]
+
+ vpRobotViper850 robot;
+
+ // Bias the force/torque sensor
+ robot.biasForceTorqueSensor();
+
+ for (unsigned int i=0; i< 10; i++) {
+ robot.getForceTorque(H) ;
+ std::cout << "Measured force/torque: " << H.t() << std::endl;
+ vpTime::wait(5);
+ }
+#endif
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : If the force/torque measures
+ cannot be get from the low level controller.
+
+ \sa biasForceTorqueSensor()
+
+*/
+void
+ vpRobotViper850::getForceTorque(vpColVector &H) const
+{
+ InitTry;
+
+ H.resize (6);
+
+ Try( PrimitiveTFS_ACQ_Viper850(H.data) );
+
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot get the force/torque measures.");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot get force/torque measures.");
+ }
+}
+/*!
+
+ Open the electric two fingers Schunk gripper.
+
+ \sa closeGripper()
+*/
+void
+ vpRobotViper850::openGripper()
+{
+ InitTry;
+ Try( PrimitiveGripper_Viper850(1) );
+ std::cout << "Open the gripper..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot open the gripper");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot open the gripper.");
+ }
+}
+
+/*!
+
+ Close the electric two fingers Schunk gripper.
+
+ \sa openGripper()
+
+*/
+void vpRobotViper850::closeGripper() const
+{
+ InitTry;
+ Try( PrimitiveGripper_Viper850(0) );
+ std::cout << "Close the gripper..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot close the gripper");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot close the gripper.");
+ }
+}
+
+/*!
+ Enable the joint limits on axis number 6. This is the default.
+
+ \sa disbleJoint6Limits()
+*/
+void vpRobotViper850::enableJoint6Limits() const
+{
+ InitTry;
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
+ std::cout << "Enable joint limits on axis 6..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot enable joint limits on axis 6");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot enable joint limits on axis 6.");
+ }
+}
+
+/*!
+ \warning Each call to this function should be done carefully.
+
+ Disable the joint limits on axis number 6. When joint 6 is outside the limits,
+ a call to this function allows to bring the robot to a position inside the limits.
+ Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
+
+ \sa enableJoint6Limits()
+*/
+void vpRobotViper850::disableJoint6Limits() const
+{
+ InitTry;
+ Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1) );
+ std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
+ CatchPrint();
+ if (TryStt < 0) {
+ vpERROR_TRACE ("Cannot disable joint limits on axis 6");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Cannot disable joint limits on axis 6.");
+ }
+}
+
+/*!
+
+ Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+
+ \param w_max : Maximum rotation velocity expressed in rad/s.
+*/
+
+void
+vpRobotViper850::setMaxRotationVelocity (double w_max)
+{
+ vpRobot::setMaxRotationVelocity(w_max);
+ setMaxRotationVelocityJoint6(w_max);
+
+ return;
+}
+
+/*!
+
+ Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+
+ This function affects only the velocities that are sent as joint velocities.
+
+ \code
+ vpRobotViper850 robot;
+ robot.setMaxRotationVelocity( vpMath::rad(20) );
+ robot.setMaxRotationVelocityJoint6( vpMath::rad(50) );
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ robot.setVelocity(ARTICULAR_FRAME, v);
+ \endcode
+
+
+ \param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
+*/
+
+void
+vpRobotViper850::setMaxRotationVelocityJoint6 (const double w6_max)
+{
+ maxRotationVelocity_joint6 = w6_max;
+ return;
+}
+
+/*!
+
+ Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+
+ \return Maximum rotation velocity on joint 6 expressed in rad/s.
+*/
+double
+vpRobotViper850::getMaxRotationVelocityJoint6() const
+{
+ return maxRotationVelocity_joint6;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has no symbols
+void dummy_vpRobotViper850() {};
+#endif
+
diff --git a/modules/robot/src/real-robot/viper/vpViper.cpp b/modules/robot/src/real-robot/viper/vpViper.cpp
new file mode 100644
index 0000000..76e212d
--- /dev/null
+++ b/modules/robot/src/real-robot/viper/vpViper.cpp
@@ -0,0 +1,1334 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for a generic ADEPT Viper (either 650 or 850) robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpViper.cpp
+
+ Modelisation of the ADEPT Viper 650 or 850 robot.
+
+*/
+
+
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/robot/vpViper.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+const unsigned int vpViper::njoint = 6;
+
+/*!
+
+ Default constructor.
+
+*/
+vpViper::vpViper()
+ : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), c56(0), joint_max(), joint_min()
+{
+ // Default values are initialized
+
+ // Denavit Hartenberg parameters
+ a1 = 0.075;
+ a2 = 0.365;
+ a3 = 0.090;
+ d1 = 0.335;
+ d4 = 0.405;
+ d6 = 0.080;
+ c56 = -341.33 / 9102.22;
+
+ // Software joint limits in radians
+ joint_min.resize(njoint);
+ joint_min[0] = vpMath::rad(-170);
+ joint_min[1] = vpMath::rad(-190);
+ joint_min[2] = vpMath::rad(-29);
+ joint_min[3] = vpMath::rad(-190);
+ joint_min[4] = vpMath::rad(-120);
+ joint_min[5] = vpMath::rad(-360);
+ joint_max.resize(njoint);
+ joint_max[0] = vpMath::rad(170);
+ joint_max[1] = vpMath::rad(45);
+ joint_max[2] = vpMath::rad(256);
+ joint_max[3] = vpMath::rad(190);
+ joint_max[4] = vpMath::rad(120);
+ joint_max[5] = vpMath::rad(360);
+
+ // End effector to camera transformation
+ eMc.eye();
+}
+
+
+
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the six joint positions.
+
+ This method is the same than get_fMc(const vpColVector & q).
+
+ \param q : A six dimension vector corresponding to the robot joint
+ positions expressed in radians.
+
+ \return The homogeneous matrix \f$^f{\bf M}_c \f$ corresponding to
+ the direct geometric model which expresses the transformation
+ between the base frame and the camera frame.
+
+ \sa get_fMc(const vpColVector & q)
+ \sa getInverseKinematics()
+
+*/
+vpHomogeneousMatrix
+vpViper::getForwardKinematics(const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ fMc = get_fMc(q);
+
+ return fMc;
+}
+
+/*!
+
+ Convert a joint position by applying modulo \f$2 \pi\f$ to ensure
+ that the position is in the joint limits.
+
+ \param joint : Joint to consider.
+
+ \param q : A joint position.
+
+ \param q_mod : The joint position modified by considering modulo
+ \f$2 \pi\f$ to be in the joint limits.
+
+ \return true if the joint position is in the joint limits. false otherwise.
+ */
+bool
+vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose) const
+{
+ double eps = 0.01;
+ if (q >= joint_min[joint]-eps && q <= joint_max[joint]+eps ) {
+ q_mod = q;
+ return true;
+ }
+
+ q_mod = q + 2*M_PI;
+ if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
+ return true;
+ }
+
+ q_mod = q - 2*M_PI;
+ if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
+ return true;
+ }
+
+ if (verbose) {
+ std::cout << "Joint " << joint << " not in limits: "
+ << this->joint_min[joint] << " < " << q << " < " << this->joint_max[joint] << std::endl;
+
+ }
+
+ return false;
+}
+
+/*!
+
+ Compute the inverse kinematics (inverse geometric model).
+
+ By inverse kinematics we mean here the six joint values given the
+ position and the orientation of the camera frame relative to the
+ base frame.
+
+ \param fMw : Homogeneous matrix \f$^f{\bf M}_w \f$ describing the
+ transformation from base frame to the wrist frame.
+
+ \param q : In input, a six dimension vector corresponding to the
+ current joint positions expressed in radians. In output, the
+ solution of the inverse kinematics, ie. the joint positions
+ corresponding to \f$^f{\bf M}_w \f$.
+
+ \param verbose : Add extra printings.
+
+ \return Add printings if no solution was found.
+
+ \return The number of solutions (1 to 8) of the inverse geometric
+ model. O, if no solution can be found.
+
+ The code below shows how to compute the inverse geometric model:
+
+ \code
+ vpColVector q1(6), q2(6);
+ vpHomogeneousMatrix fMw;
+
+ vpViper robot;
+
+ // Get the current joint position of the robot
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
+
+ // Compute the pose of the wrist in the reference frame using the
+ // direct geometric model
+ robot.get_fMw(q1, fMw);
+
+ // Compute the inverse geometric model
+ int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
+ // get the nearest solution to the current joint position
+ nbsol = robot.getInverseKinematicsWrist(fMw, q1);
+
+ if (nbsol == 0)
+ std::cout << "No solution of the inverse geometric model " << std::endl;
+ else if (nbsol >= 1)
+ std::cout << "Nearest solution: " << q1 << std::endl;
+ \endcode
+
+ \sa getForwardKinematics(), getInverseKinematics()
+
+*/
+unsigned int
+vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose) const
+{
+ vpColVector q_sol[8];
+
+ for (unsigned int i=0; i<8; i++)
+ q_sol[i].resize(6);
+
+ double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+
+ bool ok[8];
+
+ if (q.getRows() != njoint)
+ q.resize(6);
+
+ for (unsigned int i=0; i< 8; i++)
+ ok[i] = true;
+
+ double px = fMw[0][3]; // a*c1
+ double py = fMw[1][3]; // a*s1
+ double pz = fMw[2][3];
+
+ // Compute q1
+ double a_2 = px*px+py*py;
+ //if (a_2 == 0) {// singularity
+ if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {// singularity
+ c1[0] = cos(q[0]);
+ s1[0] = sin(q[0]);
+ c1[4] = cos(q[0]+M_PI);
+ s1[4] = sin(q[0]+M_PI);
+ }
+ else {
+ double a = sqrt(a_2);
+ c1[0] = px/a;
+ s1[0] = py/a;
+ c1[4] = -px/a;
+ s1[4] = -py/a;
+ }
+
+ double q1_mod;
+ for(unsigned int i=0;i<8;i+=4) {
+ q_sol[i][0] = atan2(s1[i],c1[i]);
+ if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
+ q_sol[i][0] = q1_mod;
+ for(unsigned int j=1;j<4;j++) {
+ c1[i+j] = c1[i];
+ s1[i+j] = s1[i];
+ q_sol[i+j][0] = q_sol[i][0];
+ }
+ }
+ else {
+ for(unsigned int j=1;j<4;j++)
+ ok[i+j] = false;
+ }
+ }
+
+ // Compute q3
+ double K, q3_mod;
+ for(unsigned int i=0; i<8; i+=4) {
+ if(ok[i] == true) {
+ K = (px*px+py*py+pz*pz+a1*a1-a2*a2-a3*a3+d1*d1-d4*d4
+ - 2*(a1*c1[i]*px + a1*s1[i]*py + d1*pz)) / (2*a2);
+ double d4_a3_K = d4*d4+a3*a3-K*K;
+
+ q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
+ q_sol[i+2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
+
+ for (unsigned int j=0; j<4; j+=2) {
+ if (d4_a3_K < 0) {
+ for(unsigned int k=0; k<2; k++)
+ ok[i+j+k] = false;
+ }
+ else {
+ if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod, verbose) == true) {
+ for(unsigned int k=0; k<2; k++) {
+ q_sol[i+j+k][2] = q3_mod;
+ c3[i+j+k] = cos(q3_mod);
+ s3[i+j+k] = sin(q3_mod);
+ }
+ }
+ else {
+ for(unsigned int k=0; k<2; k++)
+ ok[i+j+k] = false;
+ }
+ }
+ }
+ }
+ }
+ // std::cout << "ok apres q3: ";
+ // for (unsigned int i=0; i< 8; i++)
+ // std::cout << ok[i] << " ";
+ // std::cout << std::endl;
+
+ // Compute q2
+ double q23[8], q2_mod;
+ for (unsigned int i=0; i<8; i+=2) {
+ if (ok[i] == true) {
+ // Compute q23 = q2+q3
+ c23[i] = (-(a3-a2*c3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(d4+a2*s3[i]))
+ / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
+ s23[i] = ((d4+a2*s3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(a3-a2*c3[i]))
+ / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
+ q23[i] = atan2(s23[i],c23[i]);
+ //std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] << std::endl;
+ // q2 = q23 - q3
+ q_sol[i][1] = q23[i] - q_sol[i][2];
+
+ if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
+ for(unsigned int j=0; j<2; j++) {
+ q_sol[i+j][1] = q2_mod;
+ c23[i+j] = c23[i];
+ s23[i+j] = s23[i];
+ }
+ }
+ else {
+ for(unsigned int j=0; j<2; j++)
+ ok[i+j] = false;
+ }
+ }
+ }
+ // std::cout << "ok apres q2: ";
+ // for (unsigned int i=0; i< 8; i++)
+ // std::cout << ok[i] << " ";
+ // std::cout << std::endl;
+
+ // Compute q4 as long as s5 != 0
+ double r13 = fMw[0][2];
+ double r23 = fMw[1][2];
+ double r33 = fMw[2][2];
+ double s4s5, c4s5, q4_mod, q5_mod;
+ for (unsigned int i=0; i<8; i+=2) {
+ if (ok[i] == true) {
+ s4s5 = -s1[i]*r13+c1[i]*r23;
+ c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
+ if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
+ // s5 = 0
+ c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
+ //std::cout << "Singularity: s5 near 0: ";
+ if (c5[i] > 0.)
+ q_sol[i][4] = 0.0;
+ else
+ q_sol[i][4] = M_PI;
+
+ if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
+ for(unsigned int j=0; j<2; j++) {
+ q_sol[i+j][3] = q[3]; // keep current q4
+ q_sol[i+j][4] = q5_mod;
+ c4[i] = cos(q_sol[i+j][3]);
+ s4[i] = sin(q_sol[i+j][3]);
+ }
+ }
+ else {
+ for(unsigned int j=0; j<2; j++)
+ ok[i+j] = false;
+ }
+ }
+ else {
+ // s5 != 0
+ //if (c4s5 == 0) {
+ if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
+ // c4 = 0
+ // vpTRACE("c4 = 0");
+ // q_sol[i][3] = q[3]; // keep current position
+ q_sol[i][3] = atan2(s4s5, c4s5);
+ }
+ else {
+ q_sol[i][3] = atan2(s4s5, c4s5);
+ }
+ if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
+ q_sol[i][3] = q4_mod;
+ c4[i] = cos(q4_mod);
+ s4[i] = sin(q4_mod);
+ }
+ else {
+ ok[i] = false;
+ }
+ if (q_sol[i][3] > 0.)
+ q_sol[i+1][3] = q_sol[i][3] + M_PI;
+ else
+ q_sol[i+1][3] = q_sol[i][3] - M_PI;
+ if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod, verbose) == true) {
+ q_sol[i+1][3] = q4_mod;
+ c4[i+1] = cos(q4_mod);
+ s4[i+1] = sin(q4_mod);
+ }
+ else {
+ ok[i+1] = false;
+ }
+
+ // Compute q5
+ for (unsigned int j=0; j<2; j++) {
+ if (ok[i+j] == true) {
+ c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
+ s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
+ +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
+
+ q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
+ if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod, verbose) == true) {
+ q_sol[i+j][4] = q5_mod;
+ }
+ else {
+
+ ok[i+j] = false;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ // Compute q6
+ // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
+ double r12 = fMw[0][1];
+ double r22 = fMw[1][1];
+ double r32 = fMw[2][1];
+ double q6_mod;
+ for (unsigned int i=0; i<8; i++) {
+ c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
+ +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
+ s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
+ -s1[i]*s4[i]*c5[i])*r12
+ -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
+ +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
+
+ q_sol[i][5] = atan2(s6[i], c6[i]);
+ if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
+ q_sol[i][5] = q6_mod;
+ }
+ else {
+ ok[i] = false;
+ }
+ }
+
+ // Select the best config in terms of distance from the current position
+ unsigned int nbsol = 0;
+ unsigned int sol = 0;
+ vpColVector dist(8);
+ for (unsigned int i=0; i<8; i++) {
+ if (ok[i] == true) {
+ nbsol ++;
+ sol = i;
+ // dist[i] = vpColVector::distance(q, q_sol[i]);
+ vpColVector weight(6);
+ weight = 1;
+ weight[0] = 8;
+ weight[1] = weight[2] = 4;
+ dist[i] = 0;
+ for (unsigned int j=0; j< 6; j++) {
+ double rought_dist = q[j]- q_sol[i][j];
+ double modulo_dist = rought_dist;
+ if (rought_dist > 0) {
+ if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
+ modulo_dist = rought_dist - 2*M_PI;
+ }
+ else {
+ 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) {
+ for (unsigned int i=0; i<8; i++) {
+ if (ok[i] == true)
+ if (dist[i] < dist[sol]) sol = i;
+ }
+ // 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;
+
+}
+
+/*!
+
+ Compute the inverse kinematics (inverse geometric model).
+
+ By inverse kinematics we mean here the six joint values given the
+ position and the orientation of the camera frame relative to the
+ base frame.
+
+ \param fMc : Homogeneous matrix \f$^f{\bf M}_c \f$ describing the
+ transformation from base frame to the camera frame.
+
+ \param q : In input, a six dimension vector corresponding to the
+ current joint positions expressed in radians. In output, the
+ solution of the inverse kinematics, ie. the joint positions
+ corresponding to \f$^f{\bf M}_c \f$.
+
+ \param verbose : Add extra printings.
+
+ \return Add printings if no solution was found.
+
+ \return The number of solutions (1 to 8) of the inverse geometric
+ model. O, if no solution can be found.
+
+ The code below shows how to compute the inverse geometric model:
+
+ \code
+ vpColVector q1(6), q2(6);
+ vpHomogeneousMatrix fMc;
+
+ vpViper robot;
+
+ // Get the current joint position of the robot
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
+
+ // Compute the pose of the camera in the reference frame using the
+ // direct geometric model
+ fMc = robot.getForwardKinematics(q1);
+ // this is similar to fMc = robot.get_fMc(q1);
+ // or robot.get_fMc(q1, fMc);
+
+ // Compute the inverse geometric model
+ int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
+ // get the nearest solution to the current joint position
+ nbsol = robot.getInverseKinematics(fMc, q1);
+
+ if (nbsol == 0)
+ std::cout << "No solution of the inverse geometric model " << std::endl;
+ else if (nbsol >= 1)
+ std::cout << "Nearest solution: " << q1 << std::endl;
+ \endcode
+
+ \sa getForwardKinematics(), getInverseKinematicsWrist
+
+*/
+unsigned int
+vpViper::getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose) const
+{
+ vpHomogeneousMatrix fMw;
+ vpHomogeneousMatrix wMe;
+ vpHomogeneousMatrix eMc_;
+ this->get_wMe(wMe);
+ this->get_eMc(eMc_);
+ fMw = fMc * eMc_.inverse() * wMe.inverse();
+
+ return (getInverseKinematicsWrist(fMw, q, verbose));
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the joint positions of all
+ the six joints.
+
+ \f[
+ ^f{\bf M}_c = ^f{\bf M}_e \; ^e{\bf M}_c
+ \f]
+
+ This method is the same than getForwardKinematics(const vpColVector & q).
+
+ \param q : Vector of six joint positions expressed in
+ radians.
+
+ \return The homogeneous matrix corresponding to the direct geometric
+ model which expresses the transformation between the base frame and the
+ camera frame (fMc).
+
+ \sa getForwardKinematics(const vpColVector & q), get_fMe(), get_eMc()
+
+*/
+vpHomogeneousMatrix
+vpViper::get_fMc (const vpColVector & q) const
+{
+ vpHomogeneousMatrix fMc;
+ get_fMc(q, fMc);
+
+ return fMc;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix.
+
+ By forward kinematics we mean here the position and the orientation
+ of the camera relative to the base frame given the six joint positions.
+
+ \f[
+ ^f{\bf M}_c = ^f{\bf M}_e \; {^e}{\bf M}_c
+ \f]
+
+ \param q : Vector of six joint positions expressed in
+ radians.
+
+ \param fMc The homogeneous matrix \f$^f{\bf M}_c\f$corresponding to
+ the direct geometric model which expresses the transformation
+ between the fix frame and the camera frame.
+
+ \sa get_fMe(), get_eMc()
+*/
+void
+vpViper::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+{
+
+ // Compute the direct geometric model: fMe = transformation between
+ // fix and end effector frame.
+ vpHomogeneousMatrix fMe;
+
+ get_fMe(q, fMe);
+
+ fMc = fMe * this->eMc;
+
+ return;
+}
+
+/*!
+
+ Compute the forward kinematics (direct geometric model) as an
+ homogeneous matrix \f${^f}{\bf M}_e\f$.
+
+ By forward kinematics we mean here the position and the orientation
+ of the end effector with respect to the base frame given the
+ motor positions of all the six joints.
+
+ \f[
+ {^f}M_e = \left(\begin{array}{cccc}
+ r_{11} & r_{12} & r_{13} & t_x \\
+ r_{21} & r_{22} & r_{23} & t_y \\
+ r_{31} & r_{32} & r_{33} & t_z \\
+ \end{array}
+ \right)
+ \f]
+
+ with
+ \f[
+ \begin{array}{l}
+ r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\
+ r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\
+ r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\
+ \\
+ r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\
+ r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\
+ r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\
+ \\
+ r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\
+ r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\
+ r_{33} = -s23c4s5+c23c5\\
+ \\
+ t_x = c1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)-s1s4s5d6\\
+ t_y = s1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)+c1s4s5d6\\
+ t_z = s23(a3-c4s5d6)+c23(c5d6+d4)-a2s2+d1\\
+ \end{array}
+ \f]
+
+ \param q : A 6-dimension vector that contains the 6 joint positions
+ expressed in radians.
+
+ \param fMe The homogeneous matrix \f${^f}{\bf M}_e\f$ corresponding to the direct geometric
+ model which expresses the transformation between the fix frame and the
+ end effector frame.
+
+ Note that this transformation can also be computed by considering the wrist
+ frame \f${^f}{\bf M}_e = {^f}{\bf M}_w *{^w}{\bf M}_e\f$.
+
+ \code
+#include <visp3/robot/vpViper.h>
+
+int main()
+{
+ vpViper robot;
+ vpColVector q(6); // The measured six joint positions
+
+ vpHomogeneousMatrix fMe; // Transformation from fix frame to end-effector
+ robot.get_fMe(q, fMe); // Get the forward kinematics
+
+ // The forward kinematics can also be computed by considering the wrist frame
+ vpHomogeneousMatrix fMw; // Transformation from fix frame to wrist frame
+ robot.get_fMw(q, fMw);
+ vpHomogeneousMatrix wMe; // Transformation from wrist frame to end-effector
+ robot.get_wMe(wMe); // Constant transformation
+
+ // Compute the forward kinematics
+ fMe = fMw * wMe;
+}
+ \endcode
+
+*/
+void
+vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+{
+ double q1 = q[0];
+ double q2 = q[1];
+ double q3 = q[2];
+ double q4 = q[3];
+ double q5 = q[4];
+ double q6 = q[5];
+ // We turn off the coupling since the measured positions are joint position
+ // taking into account the coupling factor. The coupling factor is relevant
+ // 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);
+ double s2 = sin(q2);
+ //double c3 = cos(q3);
+ //double s3 = sin(q3);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+ double c6 = cos(q6);
+ double s6 = sin(q6);
+ double c23 = cos(q2+q3);
+ double s23 = sin(q2+q3);
+
+ fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
+ fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
+ fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+
+ fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
+ fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
+ fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+
+ fMe[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
+ fMe[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
+ fMe[2][2] = -s23*c4*s5+c23*c5;
+
+ fMe[0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
+ fMe[1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
+ fMe[2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
+
+ // std::cout << "Effector position fMe: " << std::endl << fMe;
+
+ return;
+}
+/*!
+
+ Compute the transformation between the fix frame and the wrist frame. The
+ wrist frame is located on the intersection of the 3 last rotations.
+
+ \param q : A 6-dimension vector that contains the 6 joint positions
+ expressed in radians.
+
+ \param fMw The homogeneous matrix corresponding to the transformation between
+ the fix frame and the wrist frame (fMw).
+
+ \f[
+ {^f}M_w = \left(\begin{array}{cccc}
+ r_{11} & r_{12} & r_{13} & t_x \\
+ r_{21} & r_{22} & r_{23} & t_y \\
+ r_{31} & r_{32} & r_{33} & t_z \\
+ \end{array}
+ \right)
+ \f]
+
+ with
+ \f[
+ \begin{array}{l}
+ r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\
+ r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\
+ r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\
+ \\
+ r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\
+ r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\
+ r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\
+ \\
+ r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\
+ r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\
+ r_{33} = -s23c4s5+c23c5\\
+ \\
+ t_x = c1(-c23a3+s23d4+a1+a2c2)\\
+ t_y = s1(-c23a3+s23d4+a1+a2c2)\\
+ t_z = s23a3+c23d4-a2s2+d1\\
+ \end{array}
+ \f]
+
+*/
+void
+vpViper::get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const
+{
+ double q1 = q[0];
+ double q2 = q[1];
+ double q3 = q[2];
+ double q4 = q[3];
+ double q5 = q[4];
+ double q6 = q[5];
+ // We turn off the coupling since the measured positions are joint position
+ // taking into account the coupling factor. The coupling factor is relevant
+ // 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);
+ double s2 = sin(q2);
+ // double c3 = cos(q3);
+ //double s3 = sin(q3);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+ double c6 = cos(q6);
+ double s6 = sin(q6);
+ double c23 = cos(q2+q3);
+ double s23 = sin(q2+q3);
+
+ fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
+ fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
+ fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+
+ fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
+ fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
+ fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+
+ fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
+ fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
+ fMw[2][2] = -s23*c4*s5+c23*c5;
+
+ fMw[0][3] = c1*(-c23*a3+s23*d4+a1+a2*c2);
+ fMw[1][3] = s1*(-c23*a3+s23*d4+a1+a2*c2);
+ fMw[2][3] = s23*a3+c23*d4-a2*s2+d1;
+
+ //std::cout << "Wrist position fMw: " << std::endl << fMw;
+
+ return;
+}
+
+/*!
+
+ Return the transformation between the wrist frame and the end-effector. The
+ wrist frame is located on the intersection of the 3 last rotations.
+
+
+ \param wMe The homogeneous matrix corresponding to the transformation between
+ the wrist frame and the end-effector frame (wMe).
+
+*/
+void
+vpViper::get_wMe(vpHomogeneousMatrix & wMe) const
+{
+ // Set the rotation as identity
+ wMe.eye();
+
+ // Set the translation
+ wMe[2][3] = d6;
+}
+
+/*!
+
+ Get the geometric transformation between the end-effector frame and
+ the camera frame. This transformation is constant and correspond to
+ the extrinsic camera parameters estimated by calibration.
+
+ \param eMc_ : Transformation between the the
+ end-effector frame and the camera frame.
+
+ \sa get_cMe()
+*/
+void
+vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const
+{
+ eMc_ = this->eMc;
+}
+
+/*!
+
+ Get the geometric transformation between the camera frame and the
+ end-effector frame. This transformation is constant and correspond
+ to the extrinsic camera parameters estimated by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+
+ \sa get_eMc()
+*/
+void
+vpViper::get_cMe(vpHomogeneousMatrix &cMe) const
+{
+ cMe = this->eMc.inverse();
+}
+
+/*!
+
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+ \f[
+ ^c{\bf V}_e = \left(\begin{array}{cc}
+ ^c{\bf R}_e & [^c{\bf t}_e]_\times ^c{\bf R}_e\\
+ {\bf 0}_{3\times 3} & ^c{\bf R}_e
+ \end{array}
+ \right)
+ \f]
+ \param cVe : Twist transformation \f$^c{\bf V}_e\f$.
+
+*/
+void
+vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpHomogeneousMatrix cMe ;
+ get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+
+ return;
+}
+
+/*!
+
+ Get the robot jacobian \f${^e}{\bf J}_e\f$ which gives the velocity
+ of the origin of the end-effector frame expressed in end-effector frame.
+
+ \f[
+ {^e}{\bf J}_e = \left[\begin{array}{cc}
+ {^w}{\bf R}_f & {[{^e}{\bf t}_w}]_\times \; {^w}{\bf R}_f \\
+ 0_{3\times3} & {^w}{\bf R}_f
+ \end{array}
+ \right] \;
+ {^f}{\bf J}_w
+ \f]
+
+ \param q : A six-dimension vector that contains the joint positions
+ of the robot expressed in radians.
+
+ \param eJe : Robot jacobian \f${^e}{\bf J}_e\f$ that express the
+ velocity of the end-effector in the robot end-effector frame.
+
+ \sa get_fJw()
+*/
+void
+vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+{
+ vpMatrix V(6,6);
+ V = 0;
+ // Compute the first and last block of V
+ vpHomogeneousMatrix fMw;
+ get_fMw(q, fMw);
+ vpRotationMatrix fRw;
+ fMw.extract(fRw);
+ vpRotationMatrix wRf;
+ wRf = fRw.inverse();
+ for (unsigned int i=0; i<3; i++ ) {
+ for (unsigned int j=0; j<3; j++ ) {
+ V[i][j] = V[i+3][j+3] = wRf[i][j];
+ }
+ }
+ // Compute the second block of V
+ vpHomogeneousMatrix wMe;
+ get_wMe(wMe);
+ vpHomogeneousMatrix eMw;
+ eMw = wMe.inverse();
+ vpTranslationVector etw;
+ eMw.extract(etw);
+ vpMatrix block2 = etw.skew()*wRf;
+ for (unsigned int i=0; i<3; i++ ) {
+ for (unsigned int j=0; j<3; j++ ) {
+ V[i][j+3] = block2[i][j];
+ }
+ }
+ // Compute eJe
+ vpMatrix fJw;
+ get_fJw(q, fJw);
+ eJe = V * fJw;
+
+ return;
+}
+
+
+/*!
+
+ Get the robot jacobian \f${^f}{\bf J}_w\f$ which express the
+ velocity of the origin of the wrist frame in the robot reference
+ frame also called fix frame.
+
+ \f[
+ {^f}J_w = \left(\begin{array}{cccccc}
+ J_{11} & J_{12} & J_{13} & 0 & 0 & 0 \\
+ J_{21} & J_{22} & J_{23} & 0 & 0 & 0 \\
+ 0 & J_{32} & J_{33} & 0 & 0 & 0 \\
+ 0 & -s1 & -s1 & c1s23 & J_{45} & J_{46} \\
+ 0 & c1 & c1 & s1s23 & J_{55} & J_{56} \\
+ 1 & 0 & 0 & c23 & s23s4 & J_{56} \\
+ \end{array}
+ \right)
+ \f]
+
+ with
+ \f[
+ \begin{array}{l}
+ J_{11} = -s1(-c23a3+s23d4+a1+a2c2) \\
+ J_{21} = c1(-c23a3+s23d4+a1+a2c2) \\
+ J_{12} = c1(s23a3+c23d4-a2s2) \\
+ J_{22} = s1(s23a3+c23d4-a2s2) \\
+ J_{32} = c23a3-s23d4-a2c2 \\
+ J_{13} = c1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\
+ J_{23} = s1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\
+ J_{33} = -a3(s2s3-c2c3)-d4(s2c3+c2s3)\\
+ J_{45} = -c23c1s4-s1c4\\
+ J_{55} = c1c4-c23s1s4\\
+ J_{46} = (c1c23c4-s1s4)s5+c1s23c5\\
+ J_{56} = (s1c23c4+c1s4)s5+s1s23c5\\
+ J_{66} = -s23c4s5+c23c5\\
+ \end{array}
+ \f]
+
+ \param q : A six-dimension vector that contains the joint positions
+ of the robot expressed in radians.
+
+ \param fJw : Robot jacobian \f${^f}{\bf J}_w\f$ that express the
+ velocity of the point \e w (origin of the wrist frame) in the robot
+ reference frame.
+
+ \sa get_fJe(), get_eJe()
+*/
+
+void
+vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
+{
+ double q1 = q[0];
+ double q2 = q[1];
+ double q3 = q[2];
+ double q4 = q[3];
+ double q5 = q[4];
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
+ double c3 = cos(q3);
+ double s3 = sin(q3);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+ double c23 = cos(q2+q3);
+ double s23 = sin(q2+q3);
+
+ vpColVector J1(6);
+ vpColVector J2(6);
+ vpColVector J3(6);
+ vpColVector J4(6);
+ vpColVector J5(6);
+ vpColVector J6(6);
+
+ // Jacobian when d6 is set to zero
+ J1[0] = -s1*(-c23*a3+s23*d4+a1+a2*c2);
+ J1[1] = c1*(-c23*a3+s23*d4+a1+a2*c2);
+ J1[2] = 0;
+ J1[3] = 0;
+ J1[4] = 0;
+ J1[5] = 1;
+
+ J2[0] = c1*(s23*a3+c23*d4-a2*s2);
+ J2[1] = s1*(s23*a3+c23*d4-a2*s2);
+ J2[2] = c23*a3-s23*d4-a2*c2;
+ J2[3] = -s1;
+ J2[4] = c1;
+ J2[5] = 0;
+
+ J3[0] = c1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
+ J3[1] = s1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
+ J3[2] = -a3*(s2*s3-c2*c3)-d4*(s2*c3+c2*s3);
+ J3[3] = -s1;
+ J3[4] = c1;
+ J3[5] = 0;
+
+ J4[0] = 0;
+ J4[1] = 0;
+ J4[2] = 0;
+ J4[3] = c1*s23;
+ J4[4] = s1*s23;
+ J4[5] = c23;
+
+ J5[0] = 0;
+ J5[1] = 0;
+ J5[2] = 0;
+ J5[3] = -c23*c1*s4-s1*c4;
+ J5[4] = c1*c4-c23*s1*s4;
+ J5[5] = s23*s4;
+
+ J6[0] = 0;
+ J6[1] = 0;
+ J6[2] = 0;
+ J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
+ J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
+ J6[5] = -s23*c4*s5+c23*c5;
+
+ fJw.resize(6,6) ;
+ for (unsigned int i=0;i<6;i++) {
+ fJw[i][0] = J1[i];
+ fJw[i][1] = J2[i];
+ fJw[i][2] = J3[i];
+ fJw[i][3] = J4[i];
+ fJw[i][4] = J5[i];
+ fJw[i][5] = J6[i];
+ }
+ return;
+}
+/*!
+
+ Get the robot jacobian \f${^f}{\bf J}_e\f$ which gives the velocity
+ of the origin of the end-effector frame expressed in the robot
+ reference frame also called fix frame.
+
+ \f[
+ {^f}{\bf J}_e = \left[\begin{array}{cc}
+ I_{3\times3} & [{^f}{\bf R}_w \; {^e}{\bf t}_w]_\times \\
+ 0_{3\times3} & I_{3\times3}
+ \end{array}
+ \right]
+ {^f}{\bf J}_w
+ \f]
+
+ \param q : A six-dimension vector that contains the joint positions
+ of the robot expressed in radians.
+
+ \param fJe : Robot jacobian \f${^f}{\bf J}_e\f$ that express the
+ velocity of the end-effector in the robot reference frame.
+
+ \sa get_fJw
+*/
+void
+vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+{
+ vpMatrix V(6,6);
+ V = 0;
+ // Set the first and last block to identity
+ for (unsigned int i=0; i<6; i++ )
+ V[i][i] = 1;
+
+ // Compute the second block of V
+ vpHomogeneousMatrix fMw;
+ get_fMw(q, fMw);
+ vpRotationMatrix fRw;
+ fMw.extract(fRw);
+ vpHomogeneousMatrix wMe;
+ get_wMe(wMe);
+ vpHomogeneousMatrix eMw;
+ eMw = wMe.inverse();
+ vpTranslationVector etw;
+ eMw.extract(etw);
+ vpMatrix block2 = (fRw*etw).skew();
+ // Set the second block
+ for (unsigned int i=0; i<3; i++ )
+ for (unsigned int j=0; j<3; j++ )
+ V[i][j+3] = block2[i][j];
+
+ // Compute fJe
+ vpMatrix fJw;
+ get_fJw(q, fJw);
+ fJe = V * fJw;
+
+ return;
+}
+
+
+/*!
+ Get minimal joint values.
+
+ \return A 6-dimension vector that contains the minimal joint values
+ for the 6 dof. All the values are expressed in radians.
+
+*/
+vpColVector
+vpViper::getJointMin() const
+{
+ return joint_min;
+}
+
+/*!
+ Get maximal joint values.
+
+ \return A 6-dimension vector that contains the maximal joint values
+ for the 6 dof. All the values are expressed in radians.
+
+*/
+vpColVector
+vpViper::getJointMax() const
+{
+ return joint_max;
+}
+
+/*!
+
+ Return the coupling factor between join 5 and joint 6.
+
+ This factor should be only useful when motor positions are
+ considered. Since the positions returned by the robot are joint
+ positions which takes into account the coupling factor, it has not to
+ be considered in the modelization of the robot.
+
+*/
+double
+vpViper::getCoupl56() const
+{
+ return c56;
+}
+
+/*!
+
+ Set the geometric transformation between the end-effector frame and
+ the tool frame (commonly a camera).
+
+ \param eMc_ : Transformation between the end-effector frame
+ and the tool frame.
+*/
+void
+vpViper::set_eMc(const vpHomogeneousMatrix &eMc_)
+{
+ this->eMc = eMc_;
+ this->eMc.extract(etc);
+ vpRotationMatrix R(this->eMc);
+ this->erc.buildFrom(R);
+}
+
+/*!
+
+ Set the geometric transformation between the end-effector frame and
+ the tool frame (commonly a camera frame).
+
+ \param etc_ : Translation between the end-effector frame
+ and the tool frame.
+ \param erc_ : Rotation between the end-effector frame and the tool
+ frame using the Euler angles in radians with the XYZ convention.
+*/
+void
+vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
+{
+ this->etc = etc_;
+ this->erc = erc_;
+ vpRotationMatrix eRc(erc);
+ this->eMc.buildFrom(etc,eRc);
+}
+
+/*!
+
+ Print on the output stream \e os the robot parameters (joint
+ min/max, coupling factor between axis 5 and 6, hand-to-eye constant
+ homogeneous matrix \f$^e{\bf M}_c \f$.
+
+ \param os : Output stream.
+ \param viper : Robot parameters.
+*/
+VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper)
+{
+ vpRotationMatrix eRc;
+ viper.eMc.extract(eRc);
+ vpRxyzVector rxyz(eRc);
+
+ // Convert joint limits in degrees
+ vpColVector jmax = viper.joint_max;
+ vpColVector jmin = viper.joint_min;
+ jmax.rad2deg();
+ jmin.rad2deg();
+
+ os
+ << "Joint Max (deg):" << std::endl
+ << "\t" << jmax.t() << std::endl
+
+ << "Joint Min (deg): " << std::endl
+ << "\t" << jmin.t() << std::endl
+
+ << "Coupling 5-6:" << std::endl
+ << "\t" << viper.c56 << std::endl
+
+ << "eMc: "<< std::endl
+ << "\tTranslation (m): "
+ << viper.eMc[0][3] << " "
+ << viper.eMc[1][3] << " "
+ << viper.eMc[2][3]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (rad) : "
+ << rxyz[0] << " "
+ << rxyz[1] << " "
+ << rxyz[2]
+ << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : "
+ << vpMath::deg(rxyz[0]) << " "
+ << vpMath::deg(rxyz[1]) << " "
+ << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
+
+ return os;
+}
diff --git a/modules/robot/src/real-robot/viper/vpViper650.cpp b/modules/robot/src/real-robot/viper/vpViper650.cpp
new file mode 100644
index 0000000..a10d82f
--- /dev/null
+++ b/modules/robot/src/real-robot/viper/vpViper650.cpp
@@ -0,0 +1,967 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ADEPT Viper 650 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpViper650.cpp
+
+ Modelisation of the ADEPT Viper 650 robot.
+
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpXmlParserCamera.h>
+
+static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
+ NULL};
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+const char * const vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_schunk_gripper_without_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_schunk_gripper_without_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_generic_without_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_generic_without_distortion_Viper650.cnf";
+#endif
+
+const char * const vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_eMc_generic_with_distortion_Viper650.cnf";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_eMc_generic_with_distortion_Viper650.cnf";
+#endif
+
+
+const char * const vpViper650::CONST_CAMERA_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper650/current/include/const_camera_Viper650.xml";
+#else
+= "/udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml";
+#endif
+
+
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+const char * const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
+const char * const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
+const char * const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
+const char * const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
+
+const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA;
+
+
+
+/*!
+
+ Default constructor.
+ Sets the specific parameters like the Denavit Hartenberg parameters.
+
+*/
+vpViper650::vpViper650()
+ : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
+{
+ // Denavit Hartenberg parameters
+ a1 = 0.075;
+ a2 = 0.270;
+ a3 = 0.090;
+ d1 = 0.335;
+ d4 = 0.295;
+ d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
+ c56 = -341.33 / 9102.22;
+
+ // Software joint limits in radians
+ joint_min[0] = vpMath::rad(-170);
+ joint_min[1] = vpMath::rad(-190);
+ joint_min[2] = vpMath::rad(-29);
+ joint_min[3] = vpMath::rad(-190);
+ joint_min[4] = vpMath::rad(-120);
+ joint_min[5] = vpMath::rad(-360);
+ joint_max[0] = vpMath::rad(170);
+ joint_max[1] = vpMath::rad(45);
+ joint_max[2] = vpMath::rad(256);
+ joint_max[3] = vpMath::rad(190);
+ joint_max[4] = vpMath::rad(120);
+ joint_max[5] = vpMath::rad(360);
+
+ init(); // Set the default tool
+}
+
+/*!
+
+ Initialize the robot with the default tool vpViper650::defaultTool.
+ */
+void
+vpViper650::init (void)
+{
+ this->init(vpViper650::defaultTool);
+ return;
+}
+
+/*!
+
+ Read files containing the constant parameters related to the robot
+ tools in order to set the end-effector to tool transformation.
+
+ \param camera_extrinsic_parameters : Filename containing the camera
+ extrinsic parameters.
+
+*/
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+void
+vpViper650::init (const char *camera_extrinsic_parameters)
+{
+ //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
+ this->parseConfigFile (camera_extrinsic_parameters);
+
+ return ;
+}
+#endif
+
+/*!
+
+ Set the constant parameters related to the robot kinematics and to
+ the end-effector to camera transformation (\f$^e{\bf M}c\f$)
+ corresponding to the camera extrinsic parameters. These last
+ parameters depend on the camera and projection model in use and are
+ loaded from predefined files or parameters.
+
+ \warning If the macro VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h
+ this function reads the camera extrinsic parameters from the file
+ corresponding to the specified camera type and projection type.
+ Otherwise corresponding default parameters are loaded.
+
+ \param tool : Camera in use.
+
+ \param proj_model : Projection model of the camera.
+
+ \sa init(vpViper650::vpToolType, const std::string&),
+ init(vpViper650::vpToolType, const vpHomogeneousMatrix&)
+
+*/
+void
+vpViper650::init (vpViper650::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType proj_model)
+{
+
+ this->projModel = proj_model;
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ // Read the robot parameters from files
+ char filename_eMc [FILENAME_MAX];
+ switch (tool) {
+ case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_GENERIC_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_CUSTOM: {
+ vpERROR_TRACE ("No predefined file available for a custom tool"
+ "You should use init(vpViper650::vpToolType, const std::string&) or"
+ "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
+ throw vpRobotException (vpRobotException::badValue, "No predefined file available for a custom tool");
+ break;
+ }
+ 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;
+ }
+ }
+
+ this->init (filename_eMc);
+
+#else // VISP_HAVE_ACCESS_TO_NAS
+
+ // Use here default values of the robot constant parameters.
+ switch (tool) {
+ case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
+ erc[2] = vpMath::rad(-91.50); // rz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = vpMath::rad(0.26); // rx
+ erc[1] = vpMath::rad(2.12); // ry
+ erc[2] = vpMath::rad(-91.31); // rz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0005; // ty
+ etc[2] = 0.1022; // tz
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
+ case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ erc[0] = vpMath::rad(0.15); // rx
+ erc[1] = vpMath::rad(1.28); // ry
+ erc[2] = vpMath::rad(-90.8); // rz
+ etc[0] = -0.0456; // tx
+ etc[1] = -0.0013; // ty
+ etc[2] = 0.001; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = vpMath::rad(0.72); // rx
+ erc[1] = vpMath::rad(2.10); // ry
+ erc[2] = vpMath::rad(-90.5); // rz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0012; // ty
+ etc[2] = 0.078; // tz
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_GENERIC_CAMERA: {
+ // Set eMc to identity
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = 0; // rx
+ erc[1] = 0; // ry
+ erc[2] = 0; // rz
+ etc[0] = 0; // tx
+ etc[1] = 0; // ty
+ etc[2] = 0; // tz
+ break;
+ }
+ break;
+ }
+ case vpViper650::TOOL_CUSTOM: {
+ vpERROR_TRACE ("No predefined parameters available for a custom tool"
+ "You should use init(vpViper650::vpToolType, const std::string&) or"
+ "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
+ throw vpRobotException (vpRobotException::badValue, "No predefined parameters available for a custom tool");
+ break;
+ }
+ }
+ vpRotationMatrix eRc(erc);
+ this->eMc.buildFrom(etc, eRc);
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+ setToolType(tool);
+ return ;
+}
+
+/*!
+
+ Set the type of tool attached to the robot and transformation
+ between the end-effector and the tool (\f$^e{\bf M}c\f$).
+ This last parameter is loaded from a file.
+
+ \param tool : Type of tool in use.
+
+ \param filename : Path of the configuration file containing the
+ transformation between the end-effector frame and the tool frame.
+
+ The configuration file should have the form below:
+
+ \code
+# Start with any number of consecutive lines
+# beginning with the symbol '#'
+#
+# The 3 following lines contain the name of the camera,
+# the rotation parameters of the geometric transformation
+# using the Euler angles in degrees with convention XYZ and
+# the translation parameters expressed in meters
+CAMERA CameraName
+eMc_ROT_XYZ 10.0 -90.0 20.0
+eMc_TRANS_XYZ 0.05 0.01 0.06
+ \endcode
+
+ \sa init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
+ init(vpViper650::vpToolType, const vpHomogeneousMatrix&)
+*/
+void
+vpViper650::init (vpViper650::vpToolType tool,
+ const std::string &filename)
+{
+ this->setToolType(tool);
+ this->parseConfigFile(filename.c_str());
+}
+
+/*!
+
+ Set the type of tool attached to the robot and the transformation
+ between the end-effector and the tool (\f$^e{\bf M}c\f$).
+
+ \param tool : Type of tool in use.
+
+ \param eMc_ : Homogeneous matrix representation of the transformation
+ between the end-effector frame and the tool frame.
+
+ \sa init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
+ init(vpViper650::vpToolType, const std::string&)
+
+*/
+void
+vpViper650::init (vpViper650::vpToolType tool,
+ const vpHomogeneousMatrix &eMc_)
+{
+ this->setToolType(tool);
+ this->set_eMc(eMc_);
+}
+
+/*!
+
+ This function gets the robot constant parameters from a file.
+
+ \param filename : File name containing the robot constant
+ parameters, like the hand-to-eye transformation.
+
+*/
+void
+vpViper650::parseConfigFile (const char * filename)
+{
+ size_t dim;
+ int code;
+ char Ligne[FILENAME_MAX];
+ char namoption[100];
+ FILE * fdtask;
+ int numLn = 0;
+ vpRxyzVector rot_eMc; // rotation
+ vpTranslationVector trans_eMc; // translation
+ bool get_rot_eMc = false;
+ bool get_trans_eMc = false;
+
+ //vpTRACE("Read the config file for constant parameters %s.", filename);
+ if ((fdtask = fopen(filename, "r" )) == NULL)
+ {
+ vpERROR_TRACE ("Impossible to read the config file %s.",
+ filename);
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the config file.");
+ }
+
+ while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
+ numLn ++;
+ if ('#' == Ligne[0]) { continue; }
+ if (sscanf(Ligne, "%s", namoption) != 1) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve option name"));
+ }
+
+ dim = strlen(namoption);
+
+ for (code = 0;
+ NULL != opt_viper650[code];
+ ++ code)
+ {
+ if (strncmp(opt_viper650[code], namoption, dim) == 0)
+ {
+ break;
+ }
+ }
+
+ switch(code) {
+ case 0:
+ break; // Nothing to do: camera name
+
+ case 1:
+ if (sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &rot_eMc[0], &rot_eMc[1], &rot_eMc[2]) != 4) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve translation"));
+ }
+
+ // Convert rotation from degrees to radians
+ rot_eMc[0] *= M_PI / 180.0;
+ rot_eMc[1] *= M_PI / 180.0;
+ rot_eMc[2] *= M_PI / 180.0;
+ get_rot_eMc = true;
+ break;
+
+ case 2:
+ if (sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &trans_eMc[0], &trans_eMc[1], &trans_eMc[2]) != 4) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve rotation"));
+ }
+ get_trans_eMc = true;
+ break;
+
+ default:
+ vpERROR_TRACE ("Bad configuration file %s "
+ "ligne #%d.", filename, numLn);
+ } /* SWITCH */
+ } /* WHILE */
+
+ fclose (fdtask);
+
+ // Compute the eMc matrix from the translations and rotations
+ if (get_rot_eMc && get_trans_eMc) {
+ this->set_eMc(trans_eMc,rot_eMc);
+ }
+ else {
+ vpERROR_TRACE ("Could not read translation and rotation parameters from config file %s.",
+ filename);
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Could not read translation and rotation parameters from config file ");
+ }
+
+ return;
+}
+
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper650::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param image_width : Image width used to compute camera calibration.
+ \param image_height : Image height used to compute camera calibration.
+
+ The code below shows how to get the camera parameters of the camera
+ attached to the robot.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+#else
+ vpViper650 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ // Camera parameters are read from
+ // /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
+ // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined
+ // in vpConfig.h file
+ try {
+ robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+
+void
+vpViper650::getCameraParameters (vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const
+{
+#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
+ vpXmlParserCamera parser;
+ switch (getToolType()) {
+ case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper650::CONST_CAMERA_FILENAME,
+ vpViper650::CONST_MARLIN_F033C_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper650::CONST_CAMERA_FILENAME,
+ vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper650::CONST_CAMERA_FILENAME,
+ vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_GENERIC_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper650::CONST_CAMERA_FILENAME,
+ vpViper650::CONST_GENERIC_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_CUSTOM: {
+ vpERROR_TRACE ("No intrinsic parameters available for a custom tool");
+ throw vpRobotException (vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
+ break;
+ }
+ 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).");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ break;
+ }
+ }
+#else
+ // Set default parameters
+ switch (getToolType()) {
+ case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
+ case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_GENERIC_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper650::TOOL_CUSTOM: {
+ vpERROR_TRACE ("No intrinsic parameters available for a custom tool");
+ throw vpRobotException (vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
+ break;
+ }
+ default:
+ vpERROR_TRACE ("This error should not occur!");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ break;
+ }
+#endif
+ return;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper650::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A B&W image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+#else
+ vpViper650 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+void
+vpViper650::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
+/*!
+ \brief Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper650::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A color image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/robot/vpRobotViper650.h>
+
+int main()
+{
+ vpImage<vpRGBa> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER650
+ vpRobotViper650 robot;
+#else
+ vpViper650 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+
+void
+vpViper650::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
diff --git a/modules/robot/src/real-robot/viper/vpViper850.cpp b/modules/robot/src/real-robot/viper/vpViper850.cpp
new file mode 100644
index 0000000..d62315d
--- /dev/null
+++ b/modules/robot/src/real-robot/viper/vpViper850.cpp
@@ -0,0 +1,881 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface for the ADEPT Viper 850 robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+
+ \file vpViper850.cpp
+
+ Modelisation of the ADEPT Viper 850 robot.
+
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpXmlParserCamera.h>
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
+ NULL};
+
+const char * const vpViper850::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
+#endif
+
+const char * const vpViper850::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
+#endif
+
+
+const char * const vpViper850::CONST_CAMERA_FILENAME
+#if defined(_WIN32)
+= "Z:/robot/Viper850/current/include/const_camera_Viper850.xml";
+#else
+= "/udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml";
+#endif
+
+
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+const char * const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
+const char * const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
+const char * const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
+const char * const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
+
+const vpViper850::vpToolType vpViper850::defaultTool = vpViper850::TOOL_PTGREY_FLEA2_CAMERA;
+
+
+
+/*!
+
+ Default constructor.
+ Sets the specific parameters like the Denavit Hartenberg parameters.
+
+*/
+vpViper850::vpViper850()
+ : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
+
+{
+ // Denavit Hartenberg parameters
+ a1 = 0.075;
+ a2 = 0.365;
+ a3 = 0.090;
+ d1 = 0.335;
+ d4 = 0.405;
+ d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
+ c56 = -341.33 / 9102.22;
+
+ // Software joint limits in radians
+ joint_min[0] = vpMath::rad(-170);
+ joint_min[1] = vpMath::rad(-190);
+ joint_min[2] = vpMath::rad(-29);
+ joint_min[3] = vpMath::rad(-190);
+ joint_min[4] = vpMath::rad(-120);
+ joint_min[5] = vpMath::rad(-360);
+ joint_max[0] = vpMath::rad(170);
+ joint_max[1] = vpMath::rad(45);
+ joint_max[2] = vpMath::rad(256);
+ joint_max[3] = vpMath::rad(190);
+ joint_max[4] = vpMath::rad(120);
+ joint_max[5] = vpMath::rad(360);
+
+ init(); // Set the default tool
+}
+
+/*!
+
+ Initialize the robot with the default tool vpViper850::defaultTool.
+ */
+void
+vpViper850::init (void)
+{
+ this->init(vpViper850::defaultTool);
+ return;
+}
+
+/*!
+
+ Read files containing the constant parameters related to the robot
+ tools in order to set the end-effector to camera transformation.
+
+ \warning This function is only available if the macro
+ VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
+
+ \param camera_extrinsic_parameters : Filename containing the camera
+ extrinsic parameters.
+
+*/
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+void
+vpViper850::init (const char *camera_extrinsic_parameters)
+{
+ //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
+ this->parseConfigFile (camera_extrinsic_parameters);
+
+ return ;
+}
+#endif
+
+/*!
+
+ Set the constant parameters related to the robot kinematics and to
+ the end-effector to camera transformation (\f$^e{\bf M}c\f$)
+ corresponding to the camera extrinsic parameters. These last
+ parameters depend on the camera and projection model in use.
+
+ \param tool : Camera in use.
+
+ \param proj_model : Projection model of the camera.
+
+*/
+void
+vpViper850::init (vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType proj_model)
+{
+
+ this->projModel = proj_model;
+
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+ // Read the robot parameters from files
+ char filename_eMc [FILENAME_MAX];
+ switch (tool) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
+#endif
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#else // _WIN32
+ _snprintf(filename_eMc, FILENAME_MAX, "%s",
+ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
+#endif
+ break;
+ }
+ break;
+ }
+ 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;
+ }
+ }
+
+ this->init (filename_eMc);
+
+#else // VISP_HAVE_ACCESS_TO_NAS
+
+ // Use here default values of the robot constant parameters.
+ switch (tool) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
+ erc[2] = vpMath::rad(-91.50); // rz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = vpMath::rad(0.26); // rx
+ erc[1] = vpMath::rad(2.12); // ry
+ erc[2] = vpMath::rad(-91.31); // rz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0005; // ty
+ etc[2] = 0.1022; // tz
+ break;
+ }
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA:
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ erc[0] = vpMath::rad(0.15); // rx
+ erc[1] = vpMath::rad(1.28); // ry
+ erc[2] = vpMath::rad(-90.8); // rz
+ etc[0] = -0.0456; // tx
+ etc[1] = -0.0013; // ty
+ etc[2] = 0.001; // tz
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = vpMath::rad(0.72); // rx
+ erc[1] = vpMath::rad(2.10); // ry
+ erc[2] = vpMath::rad(-90.5); // rz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0012; // ty
+ etc[2] = 0.078; // tz
+ break;
+ }
+ break;
+ }
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ // Set eMc to identity
+ switch(projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ erc[0] = 0; // rx
+ erc[1] = 0; // ry
+ erc[2] = 0; // rz
+ etc[0] = 0; // tx
+ etc[1] = 0; // ty
+ etc[2] = 0; // tz
+ break;
+ }
+ break;
+ }
+ }
+ vpRotationMatrix eRc(erc);
+ this->eMc.buildFrom(etc, eRc);
+#endif // VISP_HAVE_ACCESS_TO_NAS
+
+ setToolType(tool);
+ return ;
+}
+
+/*!
+
+ This function gets the robot constant parameters from a file.
+
+ \warning This function is only available if the macro
+ VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
+
+ \param filename : File name containing the robot constant
+ parameters, like the hand-to-eye transformation.
+
+*/
+#ifdef VISP_HAVE_ACCESS_TO_NAS
+void
+vpViper850::parseConfigFile (const char * filename)
+{
+ size_t dim;
+ int code;
+ char Ligne[FILENAME_MAX];
+ char namoption[100];
+ FILE * fdtask;
+ int numLn = 0;
+ double rot_eMc[3]; // rotation
+ double trans_eMc[3]; // translation
+ bool get_rot_eMc = false;
+ bool get_trans_eMc = false;
+
+ //vpTRACE("Read the config file for constant parameters %s.", filename);
+ if ((fdtask = fopen(filename, "r" )) == NULL)
+ {
+ vpERROR_TRACE ("Impossible to read the config file %s.",
+ filename);
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the config file.");
+ }
+
+ while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
+ numLn ++;
+ if ('#' == Ligne[0]) { continue; }
+ if (sscanf(Ligne, "%s", namoption) != 1) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve option name"));
+ }
+ dim = strlen(namoption);
+
+ for (code = 0;
+ NULL != opt_viper850[code];
+ ++ code)
+ {
+ if (strncmp(opt_viper850[code], namoption, dim) == 0)
+ {
+ break;
+ }
+ }
+
+ switch(code) {
+ case 0:
+ break; // Nothing to do: camera name
+
+ case 1:
+ if (sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &rot_eMc[0], &rot_eMc[1], &rot_eMc[2]) != 4) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve translation"));
+ }
+
+ // Convert rotation from degrees to radians
+ rot_eMc[0] *= M_PI / 180.0;
+ rot_eMc[1] *= M_PI / 180.0;
+ rot_eMc[2] *= M_PI / 180.0;
+ get_rot_eMc = true;
+ break;
+
+ case 2:
+ if (sscanf(Ligne, "%s %lf %lf %lf", namoption,
+ &trans_eMc[0], &trans_eMc[1], &trans_eMc[2]) != 4) {
+ fclose (fdtask);
+ throw(vpException(vpException::badValue,
+ "Cannot parse configuration file %s to retrieve rotation"));
+ }
+ get_trans_eMc = true;
+ break;
+
+ default:
+ vpERROR_TRACE ("Bad configuration file %s "
+ "ligne #%d.", filename, numLn);
+ } /* SWITCH */
+ } /* WHILE */
+
+ fclose (fdtask);
+
+ // Compute the eMc matrix from the translations and rotations
+ if (get_rot_eMc && get_trans_eMc) {
+ for (unsigned int i=0; i < 3; i ++) {
+ erc[i] = rot_eMc[i];
+ etc[i] = trans_eMc[i];
+ }
+
+ vpRotationMatrix eRc(erc);
+ this->eMc.buildFrom(etc, eRc);
+ }
+
+ return;
+}
+#endif
+
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper850::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param image_width : Image width used to compute camera calibration.
+ \param image_height : Image height used to compute camera calibration.
+
+ The code below shows how to get the camera parameters of the camera
+ attached to the robot.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+#else
+ vpViper850 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ // Camera parameters are read from
+ // /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
+ // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined
+ // in vpConfig.h file
+ try {
+ robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+
+void
+vpViper850::getCameraParameters (vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height) const
+{
+#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
+ vpXmlParserCamera parser;
+ switch (getToolType()) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper850::CONST_CAMERA_FILENAME,
+ vpViper850::CONST_MARLIN_F033C_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper850::CONST_CAMERA_FILENAME,
+ vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper850::CONST_CAMERA_FILENAME,
+ vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ std::cout << "Get camera parameters for camera \""
+ << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
+ << "from the XML file: \""
+ << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
+ if (parser.parse(cam,
+ vpViper850::CONST_CAMERA_FILENAME,
+ vpViper850::CONST_GENERIC_CAMERA_NAME,
+ projModel,
+ image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ 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).");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ break;
+ }
+ }
+#else
+ // Set default parameters
+ switch (getToolType()) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA:
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
+ switch(this->projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion :
+ cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion :
+ cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
+ break;
+ }
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ }
+ break;
+ }
+ default:
+ vpERROR_TRACE ("This error should not occur!");
+ throw vpRobotException (vpRobotException::readingParametersError,
+ "Impossible to read the camera parameters.");
+ break;
+ }
+#endif
+ return;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper850::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A B&W image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+ vpImage<unsigned char> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+#else
+ vpViper850 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+void
+vpViper850::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<unsigned char> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
+/*!
+ \brief Get the current intrinsic camera parameters obtained by calibration.
+
+ \warning This method needs XML library to parse the file defined in
+ vpViper850::CONST_CAMERA_FILENAME and containing the camera
+ parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ \warning Thid method needs also an access to the file located on
+ Inria's NAS server and containing the camera parameters in XML
+ format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
+ defined in include/visp3/core/vpConfig.h file.
+
+ - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
+ this method gets the camera parameters from
+ /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
+ config file.
+
+ - If these two macros are not defined, this method set the camera parameters
+ to default one.
+
+ \param cam : In output, camera parameters to fill.
+ \param I : A color image send by the current camera in use.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/robot/vpRobotViper850.h>
+
+int main()
+{
+ vpImage<vpRGBa> I(480, 640);
+
+#ifdef VISP_HAVE_DC1394
+ vp1394TwoGrabber g;
+
+ // Acquire an image to update image structure
+ g.acquire(I) ;
+#endif
+
+#ifdef VISP_HAVE_VIPER850
+ vpRobotViper850 robot;
+#else
+ vpViper850 robot;
+#endif
+
+ vpCameraParameters cam ;
+ // Get the intrinsic camera parameters depending on the image size
+ try {
+ robot.getCameraParameters (cam, I);
+ }
+ catch(...) {
+ std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
+ }
+ std::cout << "Camera parameters: " << cam << std::endl;
+}
+ \endcode
+
+ \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+
+*/
+
+void
+vpViper850::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I) const
+{
+ getCameraParameters(cam,I.getWidth(),I.getHeight());
+}
diff --git a/src/robot/simulator-robot/arms/afma6_arm1.bnd b/modules/robot/src/robot-simulator/arms/afma6_arm1.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_arm1.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_arm1.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_arm2.bnd b/modules/robot/src/robot-simulator/arms/afma6_arm2.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_arm2.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_arm2.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_arm3.bnd b/modules/robot/src/robot-simulator/arms/afma6_arm3.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_arm3.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_arm3.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_arm4.bnd b/modules/robot/src/robot-simulator/arms/afma6_arm4.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_arm4.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_arm4.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_gate.bnd b/modules/robot/src/robot-simulator/arms/afma6_gate.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_gate.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_gate.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_tool_ccmop.bnd b/modules/robot/src/robot-simulator/arms/afma6_tool_ccmop.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_tool_ccmop.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_tool_ccmop.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_tool_gripper.bnd b/modules/robot/src/robot-simulator/arms/afma6_tool_gripper.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_tool_gripper.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_tool_gripper.bnd
diff --git a/src/robot/simulator-robot/arms/afma6_tool_vacuum.bnd b/modules/robot/src/robot-simulator/arms/afma6_tool_vacuum.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/afma6_tool_vacuum.bnd
rename to modules/robot/src/robot-simulator/arms/afma6_tool_vacuum.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm1.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm1.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm1.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm1.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm2.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm2.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm2.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm2.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm3.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm3.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm3.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm3.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm4.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm4.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm4.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm4.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm5.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm5.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm5.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm5.bnd
diff --git a/src/robot/simulator-robot/arms/viper850_arm6.bnd b/modules/robot/src/robot-simulator/arms/viper850_arm6.bnd
similarity index 100%
rename from src/robot/simulator-robot/arms/viper850_arm6.bnd
rename to modules/robot/src/robot-simulator/arms/viper850_arm6.bnd
diff --git a/modules/robot/src/robot-simulator/vpRobotCamera.cpp b/modules/robot/src/robot-simulator/vpRobotCamera.cpp
new file mode 100644
index 0000000..a5fa825
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpRobotCamera.cpp
@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines the simplest robot : a free flying camera.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpRobotCamera.cpp
+ \brief class that defines the simplest robot : a free flying camera
+*/
+
+#include <visp3/robot/vpRobotCamera.h>
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpExponentialMap.h>
+
+/*!
+ Constructor.
+
+ Initialise the robot by a call to init().
+
+ Sampling time is set to 40 ms. To change it you should call
+ setSamplingTime().
+
+ Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
+ is set to identity (see get_eJe()).
+
+ \code
+ vpRobotCamera robot;
+
+ robot.setSamplingTime(0.020); // Set the sampling time to 20 ms.
+
+ \endcode
+
+*/
+vpRobotCamera::vpRobotCamera()
+ : cMw_()
+{
+ init() ;
+}
+
+/*!
+ Robot initialisation.
+
+ Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
+ is set to identity (see get_eJe()).
+
+*/
+void vpRobotCamera::init()
+{
+ nDof = 6;
+ eJe.eye(6,6) ;
+ eJeAvailable = true;
+ fJeAvailable = false;
+ areJointLimitsAvailable = false;
+ qmin = NULL;
+ qmax = NULL;
+
+ setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
+}
+
+
+/*!
+ Destructor.
+
+*/
+vpRobotCamera::~vpRobotCamera()
+{
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation. Here this transformation is equal to identity
+ since camera frame and end-effector frame are at the same location.
+
+*/
+void
+vpRobotCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpVelocityTwistMatrix cVe_;
+ cVe = cVe_;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+ For that simple robot the Jacobian is the identity.
+
+ \param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
+ J}_e\f$ expressed in the end-effector frame.
+*/
+void
+vpRobotCamera::get_eJe(vpMatrix &eJe_)
+{
+ eJe_ = this->eJe ;
+}
+
+/*!
+ Send to the controller a velocity.
+
+ \param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
+ and camera frame (vpRobot::CAMERA_FRAME) are implemented.
+
+ \param v : Velocity to apply to the robot.
+
+ - In the camera frame, this velocity is represented by a vector of dimension 6
+ \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
+ {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ - In articular, this velocity is represented by a 6 dimension vector \f$
+ \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
+ u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
+ robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
+ here set to identity.
+
+ We use the exponential map (vpExponentialMap) to update the camera location.
+ Sampling time can be set using setSamplingTime().
+
+ \sa setSamplingTime()
+
+*/
+void
+vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &v)
+{
+ switch (frame)
+ {
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::CAMERA_FRAME: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
+
+ vpColVector v_max(6);
+
+ for (unsigned int i=0; i<3; i++)
+ v_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i=3; i<6; i++)
+ v_max[i] = getMaxRotationVelocity();
+
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+
+ this->cMw_ = vpExponentialMap::direct(v_sat, delta_t_).inverse()*this->cMw_ ;
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the reference frame:"
+ "functionality not implemented");
+ break ;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+}
+
+
+/*!
+ Get the robot position as the transformation from camera frame to world frame.
+*/
+void
+vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const
+{
+ cMw = this->cMw_ ;
+}
+
+/*
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the articular
+ position of each dof, first the 3 translations, then the 3
+ articular rotation positions represented by a vpRxyzVector.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
+*/
+void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME :
+ q = 0;
+ break;
+
+ case vpRobot::ARTICULAR_FRAME :
+ case vpRobot::REFERENCE_FRAME : {
+ // Convert wMc_ to a position
+ // From fMc extract the pose
+ vpRotationMatrix cRw;
+ this->cMw_.extract(cRw);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(cRw);
+
+ for (unsigned int i=0; i < 3; i++) {
+ q[i] = this->cMw_[i][3]; // translation x,y,z
+ q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
+ }
+}
+
+/*!
+ Set the robot position as the transformation from camera frame to world frame.
+*/
+void
+vpRobotCamera::setPosition(const vpHomogeneousMatrix &cMw)
+{
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ }
+
+ this->cMw_ = cMw ;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotCamera.cpp.o) has no symbols
+void dummy_vpRobotCamera() {};
+#endif
diff --git a/modules/robot/src/robot-simulator/vpRobotSimulator.cpp b/modules/robot/src/robot-simulator/vpRobotSimulator.cpp
new file mode 100644
index 0000000..e80da79
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpRobotSimulator.cpp
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic class used to make robot simulators.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/robot/vpRobotSimulator.h>
+
+/*!
+ Basic constructor that sets the sampling time by default to 40ms.
+*/
+vpRobotSimulator::vpRobotSimulator() : vpRobot(), delta_t_(0.040f)
+{
+}
diff --git a/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp
new file mode 100644
index 0000000..91df555
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp
@@ -0,0 +1,372 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Basic class used to make robot simulators.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+#include <visp3/robot/vpRobotWireFrameSimulator.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+
+/*!
+ Basic constructor
+*/
+vpRobotWireFrameSimulator::vpRobotWireFrameSimulator()
+ : vpWireFrameSimulator(), vpRobotSimulator(),
+ I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
+#if defined(_WIN32)
+#elif defined(VISP_HAVE_PTHREAD)
+ thread(), attr(),
+#endif
+ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
+ displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
+ cameraParam(),
+#if defined(VISP_HAVE_DISPLAY)
+ display(),
+#endif
+ displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
+ setVelocityCalled(false), verbose_(false)
+{
+ setSamplingTime(0.010);
+ velocity.resize(6);
+ I.resize(480,640);
+ I = 255;
+#if defined(VISP_HAVE_DISPLAY)
+ display.init(I, 0, 0,"The External view");
+#endif
+
+ //pid_t pid = getpid();
+ // setpriority (PRIO_PROCESS, pid, 19);
+}
+
+/*!
+ Default constructor.
+ \param do_display : When true, enables the display of the external view.
+ */
+vpRobotWireFrameSimulator::vpRobotWireFrameSimulator(bool do_display)
+ : vpWireFrameSimulator(), vpRobotSimulator(),
+ I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
+#if defined(_WIN32)
+#elif defined(VISP_HAVE_PTHREAD)
+ thread(), attr(),
+#endif
+ /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
+ displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
+ cameraParam(),
+#if defined(VISP_HAVE_DISPLAY)
+ display(),
+#endif
+ displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false),
+ setVelocityCalled(false), verbose_(false)
+{
+ setSamplingTime(0.010);
+ velocity.resize(6);
+ I.resize(480,640);
+ I = 255;
+
+#if defined(VISP_HAVE_DISPLAY)
+ if (do_display)
+ this->display.init(I, 0, 0,"The External view");
+#endif
+
+ //pid_t pid = getpid();
+ // setpriority (PRIO_PROCESS, pid, 19);
+}
+
+
+
+/*!
+ Basic destructor
+*/
+vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator()
+{
+}
+
+/*!
+ Initialize the display. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current position.
+ \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
+*/
+void
+vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
+{
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ vpWireFrameSimulator::initScene(obj, desired_object);
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ displayCamera = false;
+}
+
+/*!
+ Initialize the display. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file.
+ It is also possible to use a vrml (.wrl) file.
+
+ \param obj : Path to the scene file you want to use.
+ \param desired_object : Path to the scene file you want to use.
+*/
+void
+vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
+{
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ vpWireFrameSimulator::initScene(obj, desired_object);
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ displayCamera = false;
+}
+
+/*!
+ Initialize the display. It enables to choose the type of object which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current position.
+*/
+void
+vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
+{
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ vpWireFrameSimulator::initScene(obj);
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ displayCamera = false;
+}
+
+/*!
+ Initialize the display. It enables to choose the type of scene which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file, or the vrml file.
+
+ \param obj : Path to the scene file you want to use.
+*/
+void
+vpRobotWireFrameSimulator::initScene(const char* obj)
+{
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ vpWireFrameSimulator::initScene(obj);
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ displayCamera = false;
+}
+
+/*!
+ Get the view of the camera's robot.
+
+ According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
+
+ \param I_ : The image where the internal view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
+{
+
+ if (!sceneInitialized)
+ throw;
+
+ double u;
+ double v;
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I_.getWidth()/(2*px_int);
+ v = (double)I_.getHeight()/(2*py_int);
+ }
+ else
+ {
+ u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ }
+
+ float o44c[4][4],o44cd[4][4],x,y,z;
+ Matrix id = IDENTITY_MATRIX;
+
+ vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ get_fMi(fMit);
+ this->cMo = fMit[size_fMi-1].inverse()*fMo;
+ this->cMo = rotz*cMo;
+
+ vp2jlc_matrix(cMo.inverse(),o44c);
+ vp2jlc_matrix(cdMo.inverse(),o44cd);
+
+ while (get_displayBusy()) vpTime::wait(2);
+
+ add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ x = o44c[2][0] + o44c[3][0];
+ y = o44c[2][1] + o44c[3][1];
+ z = o44c[2][2] + o44c[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
+ add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayObject)
+ display_scene(id,this->scene,I_, curColor);
+
+ add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ x = o44cd[2][0] + o44cd[3][0];
+ y = o44cd[2][1] + o44cd[3][1];
+ z = o44cd[2][2] + o44cd[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
+ add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayDesiredObject)
+ {
+ if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
+ else display_scene(id,desiredScene,I_, desColor);
+ }
+ delete[] fMit;
+ set_displayBusy(false);
+}
+
+/*!
+ Get the view of the camera's robot.
+
+ According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
+
+ \param I_ : The image where the internal view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
+{
+
+ if (!sceneInitialized)
+ throw;
+
+ double u;
+ double v;
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_int);
+ v = (double)I.getHeight()/(2*py_int);
+ }
+ else
+ {
+ u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ }
+
+ float o44c[4][4],o44cd[4][4],x,y,z;
+ Matrix id = IDENTITY_MATRIX;
+
+ vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ get_fMi(fMit);
+ this->cMo = fMit[size_fMi-1].inverse()*fMo;
+ this->cMo = rotz*cMo;
+
+ vp2jlc_matrix(cMo.inverse(),o44c);
+ vp2jlc_matrix(cdMo.inverse(),o44cd);
+
+ while (get_displayBusy()) vpTime::wait(2);
+
+ add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ x = o44c[2][0] + o44c[3][0];
+ y = o44c[2][1] + o44c[3][1];
+ z = o44c[2][2] + o44c[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
+ add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayObject)
+ {
+ display_scene(id,this->scene,I_, curColor);
+ }
+
+ add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ x = o44cd[2][0] + o44cd[3][0];
+ y = o44cd[2][1] + o44cd[3][1];
+ z = o44cd[2][2] + o44cd[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
+ add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayDesiredObject)
+ {
+ if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
+ else display_scene(id,desiredScene,I_, desColor);
+ }
+ delete[] fMit;
+ set_displayBusy(false);
+}
+
+/*!
+ Get the pose between the object and the robot's camera.
+
+ \return The pose between the object and the fixed world frame.
+*/
+vpHomogeneousMatrix
+vpRobotWireFrameSimulator::get_cMo()
+{
+ vpHomogeneousMatrix cMoTemp;
+ vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ get_fMi(fMit);
+ cMoTemp = fMit[size_fMi-1].inverse()*fMo;
+ delete[] fMit;
+ return cMoTemp;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
+void dummy_vpRobotWireFrameSimulator() {};
+#endif
diff --git a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp
new file mode 100644
index 0000000..a75ec23
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp
@@ -0,0 +1,2626 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class which provides a simulator for the robot Afma6.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpConfig.h>
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpIoTools.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <string>
+const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0;
+
+
+/*!
+ Basic constructor
+*/
+vpSimulatorAfma6::vpSimulatorAfma6()
+ : vpRobotWireFrameSimulator(), vpAfma6(),
+ q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
+ zeroPos(), reposPos(), toolCustom(false), arm_dir()
+{
+ init();
+ initDisplay();
+
+ tcur = vpTime::measureTimeMs();
+
+ #if defined(_WIN32)
+ mutex_fMi = CreateMutex(NULL,FALSE,NULL);
+ mutex_artVel = CreateMutex(NULL,FALSE,NULL);
+ mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
+ mutex_velocity = CreateMutex(NULL,FALSE,NULL);
+ mutex_display = CreateMutex(NULL,FALSE,NULL);
+
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(
+ NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+ #elif defined (VISP_HAVE_PTHREAD)
+ pthread_mutex_init(&mutex_fMi, NULL);
+ pthread_mutex_init(&mutex_artVel, NULL);
+ pthread_mutex_init(&mutex_artCoord, NULL);
+ pthread_mutex_init(&mutex_velocity, NULL);
+ pthread_mutex_init(&mutex_display, NULL);
+
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+ pthread_create(&thread, NULL, launcher, (void *)this);
+ #endif
+
+ compute_fMi();
+}
+
+/*!
+ Constructor used to enable or disable the external view of the robot.
+
+ \param do_display : When true, enables the display of the external view.
+
+*/
+vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
+ : vpRobotWireFrameSimulator(do_display),
+ q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
+ zeroPos(), reposPos(), toolCustom(false), arm_dir()
+{
+ init();
+ initDisplay();
+
+ tcur = vpTime::measureTimeMs();
+
+ #if defined(_WIN32)
+ mutex_fMi = CreateMutex(NULL,FALSE,NULL);
+ mutex_artVel = CreateMutex(NULL,FALSE,NULL);
+ mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
+ mutex_velocity = CreateMutex(NULL,FALSE,NULL);
+ mutex_display = CreateMutex(NULL,FALSE,NULL);
+
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(
+ NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_init(&mutex_fMi, NULL);
+ pthread_mutex_init(&mutex_artVel, NULL);
+ pthread_mutex_init(&mutex_artCoord, NULL);
+ pthread_mutex_init(&mutex_velocity, NULL);
+ pthread_mutex_init(&mutex_display, NULL);
+
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+ pthread_create(&thread, NULL, launcher, (void *)this);
+ #endif
+
+ compute_fMi();
+}
+
+/*!
+ Basic destructor
+*/
+vpSimulatorAfma6::~vpSimulatorAfma6()
+{
+ robotStop = true;
+
+ #if defined(_WIN32)
+ WaitForSingleObject(hThread,INFINITE);
+ CloseHandle(hThread);
+ CloseHandle(mutex_fMi);
+ CloseHandle(mutex_artVel);
+ CloseHandle(mutex_artCoord);
+ CloseHandle(mutex_velocity);
+ CloseHandle(mutex_display);
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_attr_destroy(&attr);
+ pthread_join(thread, NULL);
+ pthread_mutex_destroy(&mutex_fMi);
+ pthread_mutex_destroy(&mutex_artVel);
+ pthread_mutex_destroy(&mutex_artCoord);
+ pthread_mutex_destroy(&mutex_velocity);
+ pthread_mutex_destroy(&mutex_display);
+ #endif
+
+ if (robotArms != NULL)
+ {
+ for(int i = 0; i < 6; i++)
+ free_Bound_scene (&(robotArms[i]));
+ }
+
+ delete[] robotArms;
+ delete[] fMi;
+}
+
+/*!
+ Method which initialises the parameters linked to the robot caracteristics.
+
+ Set the path to the arm files (*.bnd and *.sln) used by the
+ simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is
+ not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
+ variable that the user has to set.
+*/
+void
+vpSimulatorAfma6::init()
+{
+ // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
+ // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
+ std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
+ bool armDirExists = false;
+ for(size_t i=0; i < arm_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
+ arm_dir = arm_dirs[i];
+ armDirExists = true;
+ break;
+ }
+ if (! armDirExists) {
+ try {
+ arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
+ std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
+ }
+ catch (...) {
+ std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
+ }
+ }
+
+ this->init(vpAfma6::TOOL_CCMOP);
+ toolCustom = false;
+
+ size_fMi = 8;
+ fMi = new vpHomogeneousMatrix[8];
+ artCoord.resize(njoint);
+ artVel.resize(njoint);
+
+ zeroPos.resize(njoint);
+ zeroPos = 0;
+ reposPos.resize(njoint);
+ reposPos = 0;
+ reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
+
+ artCoord = zeroPos;
+ artVel = 0;
+
+ q_prev_getdis.resize(njoint);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+ positioningVelocity = defaultPositioningVelocity ;
+
+ setRobotFrame (vpRobot::ARTICULAR_FRAME);
+ this->setRobotState(vpRobot::STATE_STOP);
+
+ // Software joint limits in radians
+ //_joint_min.resize(njoint);
+ _joint_min[0] = -0.6501;
+ _joint_min[1] = -0.6001;
+ _joint_min[2] = -0.5001;
+ _joint_min[3] = -2.7301;
+ _joint_min[4] = -0.1001;
+ _joint_min[5] = -1.5901;
+ //_joint_max.resize(njoint);
+ _joint_max[0] = 0.7001;
+ _joint_max[1] = 0.5201;
+ _joint_max[2] = 0.4601;
+ _joint_max[3] = 2.7301;
+ _joint_max[4] = 2.4801;
+ _joint_max[5] = 1.5901;
+}
+
+/*!
+ Method which initialises the parameters linked to the display part.
+*/
+void
+vpSimulatorAfma6::initDisplay()
+{
+ robotArms = NULL;
+ robotArms = new Bound_scene[6];
+ initArms();
+ setExternalCameraPosition(vpHomogeneousMatrix(0,0,0,0,0,vpMath::rad(180))*vpHomogeneousMatrix(-0.1,0,4,vpMath::rad(90),0,0));
+ cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
+ setExternalCameraParameters(cameraParam);
+ vpCameraParameters tmp;
+ getCameraParameters(tmp,640,480);
+ px_int = tmp.get_px();
+ py_int = tmp.get_py();
+ sceneInitialized = true;
+}
+
+
+/*!
+
+ Initialize the robot kinematics with the extrinsic calibration
+ parameters associated to a specific camera.
+
+ The eMc parameters depend on the camera.
+
+ \warning Only perspective projection without distortion is available!
+
+ \param tool : Tool to use. Note that the generic camera is not handled.
+
+ \param proj_model : Projection model associated to the camera.
+
+ \sa vpCameraParameters, init()
+*/
+void
+vpSimulatorAfma6::init (vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType proj_model)
+{
+ this->projModel = proj_model;
+ unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
+ if (arm_dir.size() > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+ unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
+ if (full_length > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+
+ // Use here default values of the robot constant parameters.
+ switch (tool) {
+ case vpAfma6::TOOL_CCMOP: {
+ _erc[0] = vpMath::rad(164.35); // rx
+ _erc[1] = vpMath::rad( 89.64); // ry
+ _erc[2] = vpMath::rad(-73.05); // rz
+ _etc[0] = 0.0117; // tx
+ _etc[1] = 0.0033; // ty
+ _etc[2] = 0.2272; // tz
+
+ setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
+
+ if (robotArms != NULL)
+ {
+ while (get_displayBusy()) vpTime::wait(2);
+ free_Bound_scene (&(robotArms[5]));
+ char *name_arm = new char [full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_tool_ccmop.bnd");
+ set_scene(name_arm, robotArms+5, 1.0);
+ set_displayBusy(false);
+ delete [] name_arm;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ _erc[0] = vpMath::rad( 88.33); // rx
+ _erc[1] = vpMath::rad( 72.07); // ry
+ _erc[2] = vpMath::rad( 2.53); // rz
+ _etc[0] = 0.0783; // tx
+ _etc[1] = 0.1234; // ty
+ _etc[2] = 0.1638; // tz
+
+ setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
+
+ if (robotArms != NULL)
+ {
+ while (get_displayBusy()) vpTime::wait(2);
+ free_Bound_scene (&(robotArms[5]));
+ char *name_arm = new char [full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_tool_gripper.bnd");
+ set_scene(name_arm, robotArms+5, 1.0);
+ set_displayBusy(false);
+ delete [] name_arm;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ _erc[0] = vpMath::rad( 90.40); // rx
+ _erc[1] = vpMath::rad( 75.11); // ry
+ _erc[2] = vpMath::rad( 0.18); // rz
+ _etc[0] = 0.0038; // tx
+ _etc[1] = 0.1281; // ty
+ _etc[2] = 0.1658; // tz
+
+ setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
+
+ if (robotArms != NULL)
+ {
+ while (get_displayBusy()) vpTime::wait(2);
+ free_Bound_scene (&(robotArms[5]));
+
+ char *name_arm = new char [full_length];
+
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_tool_vacuum.bnd");
+ set_scene(name_arm, robotArms+5, 1.0);
+ set_displayBusy(false);
+ delete [] name_arm;
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
+ }
+ }
+
+ vpRotationMatrix eRc(_erc);
+ this->_eMc.buildFrom(_etc, eRc);
+
+ setToolType(tool);
+ return ;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param image_width : Image width used to compute camera calibration.
+ \param image_height : Image height used to compute camera calibration.
+
+ \warning The image size must be : 640x480 !
+*/
+
+void
+vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height)
+{
+ if (toolCustom)
+ {
+ vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
+ }
+ // Set default parameters
+ switch (getToolType()) {
+ case vpAfma6::TOOL_CCMOP: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480)
+ {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
+ cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
+ cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ }
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA:
+ case vpAfma6::TOOL_VACUUM: {
+ std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
+ break;
+ }
+ default:
+ vpERROR_TRACE ("This error should not occur!");
+ break;
+ }
+ return;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param I_ : A B&W image send by the current camera in use.
+
+ \warning The image size must be : 640x480 !
+*/
+void
+vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<unsigned char> &I_)
+{
+ getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param I_ : A B&W image send by the current camera in use.
+
+ \warning The image size must be : 640x480 !
+*/
+void
+vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I_)
+{
+ getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+}
+
+
+/*!
+ Set the intrinsic camera parameters.
+
+ \param cam : The desired camera parameters.
+*/
+void
+vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
+{
+ px_int = cam.get_px();
+ py_int = cam.get_py();
+ toolCustom = true;
+}
+
+
+/*!
+ Method lauched by the thread to compute the position of the robot in the articular frame.
+*/
+void
+vpSimulatorAfma6::updateArticularPosition()
+{
+ double tcur_1 = tcur;// temporary variable used to store the last time since the last command
+
+ while (!robotStop)
+ {
+ //Get current time
+ tprev = tcur_1;
+ tcur = vpTime::measureTimeMs();
+
+ if(setVelocityCalled || !constantSamplingTimeMode){
+ setVelocityCalled = false;
+
+ computeArticularVelocity();
+
+ double ellapsedTime = (tcur - tprev) * 1e-3;
+ if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
+ ellapsedTime = getSamplingTime(); // in second
+ }
+
+ vpColVector articularCoordinates = get_artCoord();
+ articularCoordinates = get_artCoord();
+ vpColVector articularVelocities = get_artVel();
+
+ if (jointLimit)
+ {
+ double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
+ if (art <= _joint_min[jointLimitArt-1] || art >= _joint_max[jointLimitArt-1]) {
+ if (verbose_) {
+ std::cout << "Joint " << jointLimitArt-1
+ << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt-1]) << " < "
+ << vpMath::deg(art) << " < " << vpMath::deg(_joint_max[jointLimitArt-1]) << std::endl;
+ }
+
+ articularVelocities = 0.0;
+ }
+ else
+ jointLimit = false;
+ }
+
+ articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
+ articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
+ articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
+ articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
+ articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
+ articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
+
+ int jl = isInJointLimit();
+
+ if (jl != 0 && jointLimit == false)
+ {
+ if (jl < 0)
+ ellapsedTime = (_joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
+ else
+ ellapsedTime = (_joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
+
+ for (unsigned int i = 0; i < 6; i++)
+ articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
+
+ jointLimit = true;
+ jointLimitArt = (unsigned int)fabs((double)jl);
+ }
+
+ set_artCoord(articularCoordinates);
+ set_artVel(articularVelocities);
+
+ compute_fMi();
+
+ if (displayAllowed)
+ {
+ vpDisplay::display(I);
+ vpDisplay::displayFrame(I,getExternalCameraPosition (),cameraParam,0.2,vpColor::none, thickness_);
+ vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[7],cameraParam,0.1,vpColor::none, thickness_);
+ }
+
+ if (displayType == MODEL_3D && displayAllowed)
+ {
+ while (get_displayBusy()) vpTime::wait(2);
+ vpSimulatorAfma6::getExternalImage(I);
+ set_displayBusy(false);
+ }
+
+
+ if (0/*displayType == MODEL_DH && displayAllowed*/)
+ {
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+
+ //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
+
+ vpImagePoint iP, iP_1;
+ vpPoint pt(0,0,0);
+
+ pt.track(getExternalCameraPosition ());
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
+ pt.track(getExternalCameraPosition ()*fMit[0]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
+ vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
+ for (unsigned int k = 1; k < 7; k++)
+ {
+ pt.track(getExternalCameraPosition ()*fMit[k-1]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
+
+ pt.track(getExternalCameraPosition ()*fMit[k]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
+
+ vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
+ }
+ vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
+ }
+
+ vpDisplay::flush(I);
+
+
+ vpTime::wait( tcur, 1000*getSamplingTime() );
+ tcur_1 = tcur;
+ }else{
+ vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
+ }
+ }
+}
+
+/*!
+ Compute the pose between the robot reference frame and the frames used used to compute the Denavit-Hartenberg representation. The last element of the table corresponds to the pose between the reference frame and the camera frame.
+
+ To compute the diferent homogeneous matrices, this function needs the articular coordinates as input.
+
+ Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,\f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
+*/
+void
+vpSimulatorAfma6::compute_fMi()
+{
+ //vpColVector q = get_artCoord();
+ vpColVector q(6);//; = get_artCoord();
+ q = get_artCoord();
+
+ vpHomogeneousMatrix fMit[8];
+
+ double q1 = q[0];
+ double q2 = q[1];
+ double q3 = q[2];
+ double q4 = q[3];
+ double q5 = q[4];
+ double q6 = q[5];
+
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+ double c6 = cos(q6);
+ double s6 = sin(q6);
+
+ fMit[0][0][0] = 1;
+ fMit[0][1][0] = 0;
+ fMit[0][2][0] = 0;
+ fMit[0][0][1] = 0;
+ fMit[0][1][1] = 1;
+ fMit[0][2][1] = 0;
+ fMit[0][0][2] = 0;
+ fMit[0][1][2] = 0;
+ fMit[0][2][2] = 1;
+ fMit[0][0][3] = q1;
+ fMit[0][1][3] = 0;
+ fMit[0][2][3] = 0;
+
+ fMit[1][0][0] = 1;
+ fMit[1][1][0] = 0;
+ fMit[1][2][0] = 0;
+ fMit[1][0][1] = 0;
+ fMit[1][1][1] = 1;
+ fMit[1][2][1] = 0;
+ fMit[1][0][2] = 0;
+ fMit[1][1][2] = 0;
+ fMit[1][2][2] = 1;
+ fMit[1][0][3] = q1;
+ fMit[1][1][3] = q2;
+ fMit[1][2][3] = 0;
+
+ fMit[2][0][0] = 1;
+ fMit[2][1][0] = 0;
+ fMit[2][2][0] = 0;
+ fMit[2][0][1] = 0;
+ fMit[2][1][1] = 1;
+ fMit[2][2][1] = 0;
+ fMit[2][0][2] = 0;
+ fMit[2][1][2] = 0;
+ fMit[2][2][2] = 1;
+ fMit[2][0][3] = q1;
+ fMit[2][1][3] = q2;
+ fMit[2][2][3] = q3;
+
+ fMit[3][0][0] = s4;
+ fMit[3][1][0] = -c4;
+ fMit[3][2][0] = 0;
+ fMit[3][0][1] = c4;
+ fMit[3][1][1] = s4;
+ fMit[3][2][1] = 0;
+ fMit[3][0][2] = 0;
+ fMit[3][1][2] = 0;
+ fMit[3][2][2] = 1;
+ fMit[3][0][3] = q1;
+ fMit[3][1][3] = q2;
+ fMit[3][2][3] = q3;
+
+ fMit[4][0][0] = s4*s5;
+ fMit[4][1][0] = -c4*s5;
+ fMit[4][2][0] = c5;
+ fMit[4][0][1] = s4*c5;
+ fMit[4][1][1] = -c4*c5;
+ fMit[4][2][1] = -s5;
+ fMit[4][0][2] = c4;
+ fMit[4][1][2] = s4;
+ fMit[4][2][2] = 0;
+ fMit[4][0][3] = c4*this->_long_56+q1;
+ fMit[4][1][3] = s4*this->_long_56+q2;
+ fMit[4][2][3] = q3;
+
+ fMit[5][0][0] = s4*s5*c6+c4*s6;
+ fMit[5][1][0] = -c4*s5*c6+s4*s6;
+ fMit[5][2][0] = c5*c6;
+ fMit[5][0][1] = -s4*s5*s6+c4*c6;
+ fMit[5][1][1] = c4*s5*s6+s4*c6;
+ fMit[5][2][1] = -c5*s6;
+ fMit[5][0][2] = -s4*c5;
+ fMit[5][1][2] = c4*c5;
+ fMit[5][2][2] = s5;
+ fMit[5][0][3] = c4*this->_long_56+q1;
+ fMit[5][1][3] = s4*this->_long_56+q2;
+ fMit[5][2][3] = q3;
+
+ fMit[6][0][0] = fMit[5][0][0];
+ fMit[6][1][0] = fMit[5][1][0];
+ fMit[6][2][0] = fMit[5][2][0];
+ fMit[6][0][1] = fMit[5][0][1];
+ fMit[6][1][1] = fMit[5][1][1];
+ fMit[6][2][1] = fMit[5][2][1];
+ fMit[6][0][2] = fMit[5][0][2];
+ fMit[6][1][2] = fMit[5][1][2];
+ fMit[6][2][2] = fMit[5][2][2];
+ fMit[6][0][3] = fMit[5][0][3];
+ fMit[6][1][3] = fMit[5][1][3];
+ fMit[6][2][3] = fMit[5][2][3];
+
+// vpHomogeneousMatrix cMe;
+// get_cMe(cMe);
+// cMe = cMe.inverse();
+// fMit[7] = fMit[6] * cMe;
+ vpAfma6::get_fMc(q,fMit[7]);
+
+ #if defined(_WIN32)
+ WaitForSingleObject(mutex_fMi,INFINITE);
+ for (int i = 0; i < 8; i++)
+ fMi[i] = fMit[i];
+ ReleaseMutex(mutex_fMi);
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock (&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMi[i] = fMit[i];
+ pthread_mutex_unlock (&mutex_fMi);
+ #endif
+}
+
+
+/*!
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ // Start primitive STOP only if the current state is Velocity
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ stopMotion();
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ stopMotion();
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ break;
+ }
+ case vpRobot::STATE_ACCELERATION_CONTROL:
+ default:
+ break ;
+ }
+
+ return vpRobot::setRobotState (newState);
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in articular, camera frame, reference frame or mixt frame.
+
+ \param vel : Velocity vector. Translation velocities are expressed
+ in m/s while rotation velocities in rad/s. The size of this vector
+ is always 6.
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
+ camera frame.
+
+ - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
+ reference frame.
+
+ - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
+ v_y, v_z \f$ are expressed in the reference frame and rotations \f$
+ \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorAfma6 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in m/s
+ qvel[1] = 0.1; // Joint 2 velocity in m/s
+ qvel[2] = 0.1; // Joint 3 velocity in m/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+}
+ \endcode
+*/
+void
+vpSimulatorAfma6::setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ vpColVector vel_sat(6);
+
+ double scale_trans_sat = 1;
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+ double vel_trans_max = getMaxTranslationVelocity();
+ double vel_rot_max = getMaxRotationVelocity();
+
+ double vel_abs; // Absolute value
+
+ // Velocity saturation
+ switch(frame)
+ {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ {
+ if (vel.getRows() != 6)
+ {
+ vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
+
+ for (unsigned int i = 0 ; i < 3; ++ i)
+ {
+ vel_abs = fabs (vel[i]);
+ if (vel_abs > vel_trans_max && !jointLimit)
+ {
+ vel_trans_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
+ "(axis nr. %d).", vel[i], i+1);
+ }
+
+ vel_abs = fabs (vel[i+3]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", vel[i+3], i+4);
+ }
+ }
+
+ if (vel_trans_max > getMaxTranslationVelocity())
+ scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
+
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+
+ if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
+ {
+ if (scale_trans_sat < scale_rot_sat)
+ scale_sat = scale_trans_sat;
+ else
+ scale_sat = scale_rot_sat;
+ }
+ break;
+ }
+
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ if (vel.getRows() != 6)
+ {
+ vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
+ for (unsigned int i = 0 ; i < 6; ++ i)
+ {
+ vel_abs = fabs (vel[i]);
+ if (vel_abs > vel_rot_max && !jointLimit)
+ {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", vel[i], i+1);
+ }
+ }
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if ( scale_rot_sat < 1 )
+ scale_sat = scale_rot_sat;
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+
+ set_velocity (vel * scale_sat);
+ setRobotFrame (frame);
+ setVelocityCalled = true;
+}
+
+
+/*!
+ Compute the articular velocity relative to the velocity in another frame.
+*/
+void
+vpSimulatorAfma6::computeArticularVelocity()
+{
+ vpRobot::vpControlFrameType frame = getRobotFrame();
+
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+ double vel_rot_max = getMaxRotationVelocity();
+ double vel_abs;
+
+ vpColVector articularCoordinates = get_artCoord();
+ vpColVector velocityframe = get_velocity();
+ vpColVector articularVelocity;
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix eVc(_eMc);
+ vpAfma6::get_eJe(articularCoordinates,eJe_);
+ eJe_ = eJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates,eJe_);
+ articularVelocity = eJe_*eVc*velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpMatrix fJe_;
+ vpAfma6::get_fJe(articularCoordinates,fJe_);
+ fJe_ = fJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates,fJe_);
+ articularVelocity = fJe_*velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ articularVelocity = velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+
+
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ {
+ for (unsigned int i = 0 ; i < 6; ++ i)
+ {
+ vel_abs = fabs (articularVelocity[i]);
+ if (vel_abs > vel_rot_max && !jointLimit)
+ {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", articularVelocity[i], i+1);
+ }
+ }
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if ( scale_rot_sat < 1 )
+ scale_sat = scale_rot_sat;
+
+ set_artVel(articularVelocity * scale_sat);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+}
+
+
+/*!
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param vel : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in m/s
+ q_dot[1] = 0.2; // Joint 2 velocity in m/s
+ q_dot[2] = 0.3; // Joint 3 velocity in m/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpSimulatorAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+
+ for ( ; ; ) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+}
+ \endcode
+*/
+void
+vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
+{
+ vel.resize(6);
+
+ vpColVector articularCoordinates = get_artCoord();
+ vpColVector articularVelocity = get_artVel();
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix cVe(_eMc);
+ vpAfma6::get_eJe(articularCoordinates,eJe_);
+ vel = cVe*eJe_*articularVelocity;
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ vel = articularVelocity;
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpMatrix fJe_;
+ vpAfma6::get_fJe(articularCoordinates,fJe_);
+ vel = fJe_*articularVelocity;
+ break ;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break ;
+ }
+ default:
+ {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
+ }
+}
+
+/*!
+ Get the robot time stamped velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param vel : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
+*/
+void
+vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getVelocity(frame, vel);
+}
+
+/*!
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpSimulatorAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+
+ for ( ; ; ) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+}
+ \endcode
+*/
+vpColVector
+vpSimulatorAfma6::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector vel(6);
+ getVelocity (frame, vel);
+
+ return vel;
+}
+
+/*!
+ Get the time stamped robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \sa getVelocity(vpRobot::vpControlFrameType frame)
+*/
+vpColVector
+vpSimulatorAfma6::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ vpColVector vel(6);
+ getVelocity (frame, vel);
+
+ return vel;
+}
+
+void
+vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
+{
+ double vel_rot_max = getMaxRotationVelocity();
+ double velmax = fabs(q[0]);
+ for (unsigned int i = 1; i < 6; i++)
+ {
+ if (velmax < fabs(q[i]))
+ velmax = fabs(q[i]);
+ }
+
+ double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
+ q = q * alpha;
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param q : A six dimension vector corresponding to the
+ position to reach. All the positions are expressed in meters for the
+ translations and radians for the rotations. If the position is out
+ of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X, Y, Z, A, B, C,
+ with X,Y,Z the translations, and A,B,C the rotations of the
+ end-effector.
+
+ - In the camera and the reference frame, rotations are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector position(6);
+ // Set positions in the camera frame
+ position[0] = 0.1; // x axis, in meter
+ position[1] = 0.2; // y axis, in meter
+ position[2] = 0.3; // z axis, in meter
+ position[3] = M_PI/8; // rotation around x axis, in rad
+ position[4] = M_PI/4; // rotation around y axis, in rad
+ position[5] = M_PI; // rotation around z axis, in rad
+
+ vpSimulatorAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ \endcode
+
+*/
+void
+vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q)
+{
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ vpColVector articularCoordinates = get_artCoord();
+
+ vpColVector error(6);
+ double errsqr = 0;
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++)
+ {
+ txyz[i] = q[i];
+ rxyz[i] = q[i+3];
+ }
+
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
+
+ vpHomogeneousMatrix fMc_;
+ vpAfma6::get_fMc(articularCoordinates, fMc_);
+
+ vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
+
+ do
+ {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0)
+ {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (qdes);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+ }while (errsqr > 1e-8 && nbSol > 0);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ do
+ {
+ articularCoordinates = get_artCoord();
+ error = q - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (q);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }while (errsqr > 1e-8);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++)
+ {
+ txyz[i] = q[i];
+ rxyz[i] = q[i+3];
+ }
+
+ vpRotationMatrix fRc(rxyz);
+ vpHomogeneousMatrix fMc_(txyz, fRc);
+
+ do
+ {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0)
+ {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (qdes);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }
+ else
+ vpERROR_TRACE ("Positionning error. Position unreachable");
+ }while (errsqr > 1e-8 && nbSol > 0);
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ This method overloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
+ the position to reach. All the positions are expressed in meters for
+ the translations and radians for the rotations.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X (pos1), Y (pos2),
+ Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
+ translations, and A,B,C the rotations of the end-effector.
+
+ - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+
+int main()
+{
+ // Set positions in the camera frame
+ double pos1 = 0.1; // x axis, in meter
+ double pos2 = 0.2; // y axis, in meter
+ double pos3 = 0.3; // z axis, in meter
+ double pos4 = M_PI/8; // rotation around x axis, in rad
+ double pos5 = M_PI/4; // rotation around y axis, in rad
+ double pos6 = M_PI; // rotation around z axis, in rad
+
+ vpSimulatorAfma6 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
+}
+ \endcode
+
+ \sa setPosition()
+*/
+void vpSimulatorAfma6::setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6)
+{
+ try{
+ vpColVector position(6) ;
+ position[0] = pos1 ;
+ position[1] = pos2 ;
+ position[2] = pos3 ;
+ position[3] = pos4 ;
+ position[4] = pos5 ;
+ position[5] = pos6 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector q;
+ vpSimulatorAfma6 robot;
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+*/
+void vpSimulatorAfma6::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param q : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ vpSimulatorAfma6 robot;
+
+ vpColVector position;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, position);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame transformation in terms of a
+ // homogeneous matrix
+ vpHomogeneousMatrix fMc(fRc, ftc);
+}
+ \endcode
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
+
+*/
+void
+vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize(6);
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ q = 0;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ q = get_artCoord();
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ vpHomogeneousMatrix fMc_;
+ vpAfma6::get_fMc (get_artCoord(), fMc_);
+
+ vpRotationMatrix fRc;
+ fMc_.extract(fRc);
+ vpRxyzVector rxyz(fRc);
+
+ vpTranslationVector txyz;
+ fMc_.extract(txyz);
+
+ for (unsigned int i=0; i <3; i++)
+ {
+ q[i] = txyz[i];
+ q[i+3] = rxyz[i];
+ }
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+}
+
+/*!
+
+ Get the current time stamped position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param q : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+ */
+void
+vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getPosition(frame, q);
+}
+
+
+/*!
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+
+ The difference is here that the position is returned using a ThetaU
+ representation.
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+*/
+void
+vpSimulatorAfma6::getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position)
+{
+ vpColVector posRxyz;
+ //recupere position en Rxyz
+ this->getPosition(frame,posRxyz);
+
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for(unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+
+ Get the current time stamped position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+
+ The difference is here that the position is returned using a ThetaU
+ representation.
+
+ */
+void
+vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getPosition(frame, position);
+}
+
+/*!
+ This method enables to set the minimum and maximum joint limits for all the six axis of the robot. The three first values have to be given in meter and the others in radian.
+
+ \param limitMin : The minimum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
+ \param limitMax : The maximum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
+*/
+void
+vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
+{
+ if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
+ {
+ vpTRACE("Joint limit vector has not a size of 6 !");
+ return;
+ }
+
+ _joint_min[0] = limitMin[0];
+ _joint_min[1] = limitMin[1];
+ _joint_min[2] = limitMin[2];
+ _joint_min[3] = limitMin[3];
+ _joint_min[4] = limitMin[4];
+ _joint_min[5] = limitMin[5];
+
+ _joint_max[0] = limitMax[0];
+ _joint_max[1] = limitMax[1];
+ _joint_max[2] = limitMax[2];
+ _joint_max[3] = limitMax[3];
+ _joint_max[4] = limitMax[4];
+ _joint_max[5] = limitMax[5];
+
+}
+
+/*!
+ Test to detect if the robot is near one of its singularities.
+
+ The goal is to avoid the problems du to such configurations.
+*/
+bool
+vpSimulatorAfma6::singularityTest(const vpColVector q, vpMatrix &J)
+{
+ double q5 = q[4];
+
+ bool cond = fabs(q5-M_PI/2) < 1e-1;
+
+ if(cond)
+ {
+ J[0][3] = 0;
+ J[0][4] = 0;
+ J[1][3] = 0;
+ J[1][4] = 0;
+ J[3][3] = 0;
+ J[3][4] = 0;
+ J[5][3] = 0;
+ J[5][4] = 0;
+ return true;
+ }
+
+ return false;
+}
+
+/*!
+ Method used to check if the robot reached a joint limit.
+*/
+int
+vpSimulatorAfma6::isInJointLimit ()
+{
+ int artNumb = 0;
+ double diff = 0;
+ double difft = 0;
+
+ vpColVector articularCoordinates = get_artCoord();
+
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ if (articularCoordinates[i] <= _joint_min[i])
+ {
+ difft = _joint_min[i] - articularCoordinates[i];
+ if (difft > diff)
+ {
+ diff = difft;
+ artNumb = -(int)i-1;
+ }
+ }
+ }
+
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ if (articularCoordinates[i] >= _joint_max[i])
+ {
+ difft = articularCoordinates[i] - _joint_max[i];
+ if (difft > diff)
+ {
+ diff = difft;
+ artNumb = (int)(i+1);
+ }
+ }
+ }
+
+ if (artNumb != 0)
+ std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
+
+ return artNumb;
+}
+
+/*!
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in the camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+*/
+void
+vpSimulatorAfma6::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+
+/*!
+ Get the robot joint displacement since the last call of this method.
+
+ \param displacement : The measured joint displacement. The dimension
+ of \e displacement is 6 (the robot joint number). All the values are
+ expressed in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+*/
+void
+vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ cartesian space. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call
+ of this method. The dimension of \e displacement is always
+ 6. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+*/
+void
+vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+ vpColVector q_cur(6);
+
+ q_cur = get_artCoord();
+
+ if ( ! first_time_getdis )
+ {
+ switch (frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME:
+ {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else
+ {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+}
+
+/*!
+Read joint positions in a specific Afma6 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#AFMA6 - Position - Version 2.01
+# file: "myposition.pos "
+#
+# R: X Y Z A B C
+# Joint position: X, Y, Z: translations in meters
+# A, B, C: rotations in degrees
+#
+
+R: 0.1 0.3 -0.25 -80.5 80 0
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are
+expressed in meters, while joint rotations A,B,C in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+vpSimulatorAfma6 robot;
+vpColVector q; // Joint position
+robot.readPosFile("myposition.pos", q); // Set the joint position from the file
+robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+\endcode
+
+\sa savePosFile()
+*/
+bool
+vpSimulatorAfma6::readPosFile(const char *filename, vpColVector &q)
+{
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+ // else
+ // return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ fclose(fd) ;
+ return (false); /* fin fichier */
+ }
+
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(njoint);
+ int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
+
+ if (ret != 7) {
+ fclose(fd) ;
+ return false;
+ }
+
+ // converts rotations from degrees into radians
+ //q.deg2rad();
+
+ q[3] = vpMath::rad(q[3]);
+ q[4] = vpMath::rad(q[4]);
+ q[5] = vpMath::rad(q[5]);
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+ Save joint (articular) positions in a specific Afma6 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : Joint positions [X,Y,Z,A,B,C] to save in the
+ filename. Translations X,Y,Z are expressed in meters, while
+ rotations A,B,C in radians.
+
+ \warning The joint rotations A,B,C written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+bool
+vpSimulatorAfma6::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#AFMA6 - Position - Version 2.01\n\
+#\n\
+# R: X Y Z A B C\n\
+# Joint position: X, Y, Z: translations in meters\n\
+# A, B, C: rotations in degrees\n\
+#\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
+ q[0],
+ q[1],
+ q[2],
+ vpMath::deg(q[3]),
+ vpMath::deg(q[4]),
+ vpMath::deg(q[5]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+ Moves the robot to the joint position specified in the filename.
+
+ \param filename: File containing a joint position.
+
+ \sa readPosFile
+*/
+void
+vpSimulatorAfma6::move(const char *filename)
+{
+ vpColVector q;
+
+ try
+ {
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
+ }
+ catch(...)
+ {
+ throw;
+ }
+}
+
+/*!
+ Get the geometric transformation \f$^c{\bf M}_e\f$ between the
+ camera frame and the end-effector frame. This transformation is
+ constant and correspond to the extrinsic camera parameters estimated
+ by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+*/
+void
+vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe)
+{
+ vpAfma6::get_cMe(cMe) ;
+}
+
+/*!
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
+ end-effector frame. This transformation allows to compute a
+ velocity expressed in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+*/
+void
+vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
+{
+ vpHomogeneousMatrix cMe ;
+ vpAfma6::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ To compute \f$^e{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
+ end-effector frame.
+*/
+void
+vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
+{
+ try
+ {
+ vpAfma6::get_eJe(get_artCoord(), eJe_) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+
+/*!
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ To compute \f$^f{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
+ reference frame.
+*/
+void
+vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
+{
+ try
+ {
+ vpColVector articularCoordinates = get_artCoord();
+ vpAfma6::get_fJe(articularCoordinates, fJe_) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+ Stop the robot.
+*/
+void
+vpSimulatorAfma6::stopMotion()
+{
+ if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
+ return;
+
+ vpColVector stop(6);
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpRobot::setRobotState (vpRobot::STATE_STOP);
+}
+
+
+
+/**********************************************************************************/
+/**********************************************************************************/
+/**********************************************************************************/
+/**********************************************************************************/
+
+/*!
+ Initialise the display of the robot's arms.
+
+ Set the path to the scene files (*.bnd and *.sln) used by the
+ simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
+ not valid, the path is set from the VISP_SCENES_DIR environment
+ variable that the user has to set.
+*/
+void
+vpSimulatorAfma6::initArms()
+{
+ // set scene_dir from #define VISP_SCENE_DIR if it exists
+ // VISP_SCENES_DIR may contain multiple locations separated by ";"
+ std::string scene_dir_;
+ std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
+ bool sceneDirExists = false;
+ for(size_t i=0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir_ = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (! sceneDirExists) {
+ try {
+ scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
+ std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
+ }
+ catch (...) {
+ std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
+ }
+ }
+
+
+ unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
+ if (scene_dir_.size() > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+ unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
+ if (full_length > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+
+ char *name_cam = new char [full_length];
+
+ strcpy(name_cam, scene_dir_.c_str());
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+
+ if (arm_dir.size() > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+ full_length = (unsigned int)arm_dir.size() + name_length;
+ if (full_length > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+
+ char *name_arm = new char [full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_gate.bnd");
+ std::cout <<"name arm: " << name_arm << std::endl;
+ set_scene(name_arm, robotArms, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_arm1.bnd");
+ set_scene(name_arm, robotArms+1, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_arm2.bnd");
+ set_scene(name_arm, robotArms+2, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_arm3.bnd");
+ set_scene(name_arm, robotArms+3, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/afma6_arm4.bnd");
+ set_scene(name_arm, robotArms+4, 1.0);
+
+ vpAfma6::vpAfma6ToolType tool;
+ tool = getToolType();
+ strcpy(name_arm, arm_dir.c_str());
+ switch (tool) {
+ case vpAfma6::TOOL_CCMOP: {
+ strcat(name_arm,"/afma6_tool_ccmop.bnd");
+ break;
+ }
+ case vpAfma6::TOOL_GRIPPER: {
+ strcat(name_arm,"/afma6_tool_gripper.bnd");
+ break;
+ }
+ case vpAfma6::TOOL_VACUUM: {
+ strcat(name_arm,"/afma6_tool_vacuum.bnd");
+ break;
+ }
+ case vpAfma6::TOOL_GENERIC_CAMERA: {
+ std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
+ break;
+ }
+ }
+ set_scene(name_arm, robotArms+5, 1.0);
+
+ add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+//
+// sceneInitialized = true;
+// displayObject = true;
+ displayCamera = true;
+
+ delete [] name_cam;
+ delete [] name_arm;
+}
+
+
+void
+vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
+{
+ bool changed = false;
+ vpHomogeneousMatrix displacement = navigation(I_,changed);
+
+ //if (displacement[2][3] != 0)
+ if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
+ camMf2 = camMf2*displacement;
+
+ f2Mf = camMf2.inverse()*camMf;
+
+ camMf = camMf2* displacement * f2Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I_.getWidth()/(2*px_ext);
+ v = (double)I_.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ }
+
+ float w44o[4][4],w44cext[4][4],x,y,z;
+
+ vp2jlc_matrix(camMf.inverse(),w44cext);
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+
+ vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
+ display_scene(w44o,robotArms[0],I_, curColor);
+
+ vp2jlc_matrix(fMit[0],w44o);
+ display_scene(w44o,robotArms[1],I_, curColor);
+
+ vp2jlc_matrix(fMit[2],w44o);
+ display_scene(w44o,robotArms[2],I_, curColor);
+
+ vp2jlc_matrix(fMit[3],w44o);
+ display_scene(w44o,robotArms[3],I_, curColor);
+
+ vp2jlc_matrix(fMit[4],w44o);
+ display_scene(w44o,robotArms[4],I_, curColor);
+
+ vp2jlc_matrix(fMit[5],w44o);
+ display_scene(w44o,robotArms[5],I_, curColor);
+
+ if (displayCamera)
+ {
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
+ cMe = cMe.inverse();
+ cMe = fMit[6] * cMe;
+ vp2jlc_matrix(cMe,w44o);
+ display_scene(w44o,camera, I_, camColor);
+ }
+
+ if (displayObject)
+ {
+ vp2jlc_matrix(fMo,w44o);
+ display_scene(w44o,scene,I_, curColor);
+ }
+}
+
+/*!
+ This method enables to initialise the joint coordinates of the robot in order
+ to position the camera relative to the object.
+
+ Before using this method it is advised to set the position of the object thanks
+ to the set_fMo() method.
+
+ In other terms, set the world to camera transformation
+ \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}\f$, and from the inverse kinematics
+ set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
+
+ \param cMo_ : the desired pose of the camera.
+
+ \return false if the robot kinematics is not able to reach the cMo position.
+
+*/
+bool
+vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
+{
+ vpColVector stop(6);
+ bool status = true;
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpHomogeneousMatrix fMc_;
+ fMc_ = fMo * cMo_.inverse();
+
+ vpColVector articularCoordinates = get_artCoord();
+ int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
+
+ if (nbSol == 0) {
+ status = false;
+ vpERROR_TRACE ("Positionning error. Position unreachable");
+ }
+
+ if (verbose_)
+ std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
+
+ set_artCoord(articularCoordinates);
+
+ compute_fMi();
+
+ return status;
+}
+
+/*!
+ This method enables to initialise the pose between the object and the reference frame,
+ in order to position the object relative to the camera.
+
+ Before using this method it is advised to set the articular coordinates of the robot.
+
+ In other terms, set the world to object transformation
+ \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
+ with \f${\bf q}\f$ the robot joint position
+
+ \param cMo_ : the desired pose of the camera.
+*/
+void
+vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
+{
+ vpColVector stop(6);
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+ fMo = fMit[7] * cMo_;
+}
+
+/*!
+ This method enable to move the robot with respect to the initialized object.
+ The robot trajectory is a straight line from the current position to the one corresponding to the desired pose (3D visual servoing).
+
+ \param cdMo_ : the desired pose of the camera wrt. the object
+ \param Iint : pointer to the image where the internal view is displayed
+ \param errMax : maximum error to consider the pose is reached
+
+ \return True is the pose is reached, False else
+*/
+bool
+vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
+{
+ // get rid of max velocity
+ double vMax = getMaxTranslationVelocity();
+ double wMax = getMaxRotationVelocity();
+ setMaxTranslationVelocity(10.*vMax);
+ setMaxRotationVelocity(10.*wMax);
+
+ vpColVector v(3),w(3),vel(6);
+ vpHomogeneousMatrix cdMc;
+ vpTranslationVector cdTc;vpRotationMatrix cdRc;vpThetaUVector cdTUc;
+ vpColVector err(6);err=1.;
+ const double lambda = 5.;
+ double t;
+
+ vpVelocityTwistMatrix cVe;
+
+ unsigned int i,iter=0;
+ while((iter++<300) & (err.euclideanNorm()>errMax))
+ {
+ t = vpTime::measureTimeMs();
+
+ // update image
+ if(Iint != NULL)
+ {
+ vpDisplay::display(*Iint);
+ getInternalView(*Iint);
+ vpDisplay::flush(*Iint);
+ }
+
+ // update pose error
+ cdMc = cdMo_*get_cMo().inverse();
+ cdMc.extract(cdRc);
+ cdMc.extract(cdTc);
+ cdTUc.buildFrom(cdRc);
+
+ // compute v,w and velocity
+ v = -lambda*cdRc.t()*cdTc;
+ w = -lambda*cdTUc;
+ for(i=0;i<3;++i)
+ {
+ vel[i] = v[i];
+ vel[i+3] = w[i];
+ err[i] = cdTc[i];
+ err[i+3] = cdTUc[i];
+ }
+
+ // update feat
+ setVelocity(vpRobot::CAMERA_FRAME,vel);
+
+ // wait for it
+ vpTime::wait(t,10);
+ }
+ vel=0.;
+ set_velocity(vel);
+ set_artVel(vel);
+ setMaxTranslationVelocity(vMax);
+ setMaxRotationVelocity(wMax);
+
+ //std::cout << "setPosition: final error " << err.t() << std::endl;
+ return(err.euclideanNorm()<= errMax);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has no symbols
+void dummy_vpSimulatorAfma6() {};
+#endif
diff --git a/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp
new file mode 100644
index 0000000..572139c
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp
@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines the simplest robot : a free flying camera.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpSimulatorCamera.cpp
+ \brief class that defines the simplest robot : a free flying camera
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+
+
+/*!
+ Default constructor that sets the transformation between
+ world frame and camera frame to identity.
+
+*/
+vpSimulatorCamera::vpSimulatorCamera() : wMc_()
+{
+ init() ;
+}
+
+/*!
+ Robot initialisation.
+
+ Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
+ is set to identity (see get_eJe()).
+
+*/
+void vpSimulatorCamera::init()
+{
+ nDof = 6;
+ eJe.eye(6,6) ;
+ eJeAvailable = true;
+ fJeAvailable = false;
+ areJointLimitsAvailable = false;
+ qmin = NULL;
+ qmax = NULL;
+
+ setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
+}
+
+
+/*!
+ Destructor.
+
+*/
+vpSimulatorCamera::~vpSimulatorCamera()
+{
+}
+
+/*!
+
+ Get the twist transformation from camera frame to end-effector
+ frame. This transformation allows to compute a velocity expressed
+ in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation. Here this transformation is equal to identity
+ since camera frame and end-effector frame are at the same location.
+
+*/
+void
+vpSimulatorCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
+{
+ vpVelocityTwistMatrix cVe_;
+ cVe = cVe_;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+ For that simple robot the Jacobian is the identity.
+
+ \param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
+ J}_e\f$ expressed in the end-effector frame. Yhis matrix is equal to identity.
+*/
+void
+vpSimulatorCamera::get_eJe(vpMatrix &eJe_)
+{
+ eJe_ = this->eJe ;
+}
+
+/*!
+ Get the camera position in the world frame.
+
+*/
+void
+vpSimulatorCamera::getPosition(vpHomogeneousMatrix &wMc) const
+{
+ wMc = this->wMc_ ;
+}
+/*!
+ Return the camera position in the world frame.
+
+*/
+vpHomogeneousMatrix
+vpSimulatorCamera::getPosition() const
+{
+ return(this->wMc_);
+}
+
+/*
+ Get the current position of the camera.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the articular
+ position of each dof, first the 3 translations, then the 3
+ articular rotation positions represented by a vpRxyzVector.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
+*/
+void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME :
+ q = 0;
+ break;
+
+ case vpRobot::ARTICULAR_FRAME :
+ case vpRobot::REFERENCE_FRAME : {
+ // Convert wMc_ to a position
+ // From fMc extract the pose
+ vpRotationMatrix wRc;
+ this->wMc_.extract(wRc);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(wRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ q[i] = this->wMc_[i][3]; // translation x,y,z
+ q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
+ }
+}
+
+/*!
+ Send to the controller a velocity.
+
+ \param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
+ and camera frame (vpRobot::CAMERA_FRAME) are implemented.
+
+ \param v : Velocity to apply to the robot.
+
+ - In the camera frame, this velocity is represented by a vector of dimension 6
+ \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
+ {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+
+ - In articular, this velocity is represented by a 6 dimension vector \f$
+ \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
+ translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
+ vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
+ u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
+ robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
+ here set to identity.
+
+ We use the exponential map (vpExponentialMap) to update the camera location.
+ Sampling time can be set using setSamplingTime().
+
+ \sa setSamplingTime()
+
+*/
+void
+vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &v)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
+
+ switch (frame)
+ {
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::CAMERA_FRAME: {
+ vpColVector v_max(6);
+
+ for (unsigned int i=0; i<3; i++)
+ v_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i=3; i<6; i++)
+ v_max[i] = getMaxRotationVelocity();
+
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+
+ wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
+ setRobotFrame(frame);
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the reference frame:"
+ "functionality not implemented");
+ break ;
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+}
+
+/*!
+ Set the robot position in the world frame.
+
+ \param wMc : Transformation from world frame to camera frame.
+*/
+void vpSimulatorCamera::setPosition(const vpHomogeneousMatrix &wMc)
+{
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ }
+
+ this->wMc_ = wMc;
+}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp
new file mode 100644
index 0000000..43c46e0
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp
@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pioneer mobile robot simulator without display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpSimulatorPioneer.cpp
+ \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
+
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpExponentialMap.h>
+
+
+/*!
+ Constructor.
+
+*/
+vpSimulatorPioneer::vpSimulatorPioneer()
+ : wMc_(), wMe_(), xm_(0), ym_(0), theta_(0)
+{
+ init() ;
+}
+
+/*!
+ Robot initialisation.
+
+ Sampling time is set to 40 ms. To change it you should call
+ setSamplingTime().
+
+*/
+void vpSimulatorPioneer::init()
+{
+ xm_ = 0;
+ ym_ = 0;
+ theta_ = 0;
+
+ nDof = 2;
+ eJeAvailable = true;
+ fJeAvailable = false;
+ areJointLimitsAvailable = false;
+ qmin = NULL;
+ qmax = NULL;
+
+ wMc_ = wMe_ * cMe_.inverse();
+}
+
+
+/*!
+ Destructor.
+
+*/
+vpSimulatorPioneer::~vpSimulatorPioneer()
+{
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+ The jacobian expression is given in vpPioneer class.
+
+ \param _eJe : A 6 by 2 matrix representing the robot jacobian \f$ {^e}{\bf
+ J}_e\f$ expressed in the end-effector frame.
+*/
+void
+vpSimulatorPioneer::get_eJe(vpMatrix &_eJe)
+{
+ _eJe = vpUnicycle::get_eJe();
+}
+
+
+/*!
+ Send to the controller a velocity.
+
+ \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
+
+ \param v : Velocity vector \f$(v_x, w_z)\f$ to apply to the robot.
+
+ Depending on the velocity specified as input, the robot position is updated
+ using the sampling time that can be modified using setSamplingTime().
+
+ \sa setSamplingTime()
+
+*/
+void
+vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &v)
+{
+ switch (frame)
+ {
+ case vpRobot::ARTICULAR_FRAME: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
+ setRobotFrame(frame);
+
+ // v is a 2 dimension vector that contains v,w
+ if (v.size() != 2) {
+ vpERROR_TRACE ("Bad dimension of the control vector");
+ throw vpRobotException (vpRobotException::dimensionError,
+ "Bad dimension of the control vector");
+ }
+
+ vpColVector v_max(2);
+
+ v_max[0] = getMaxTranslationVelocity();
+ v_max[1] = getMaxRotationVelocity();
+
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+
+ xm_ += delta_t_ * v_sat[0] * cos(theta_);
+ ym_ += delta_t_ * v_sat[0] * sin(theta_);
+ theta_ += delta_t_ * v_sat[1];
+
+ vpRotationMatrix wRe(0, 0, theta_);
+ vpTranslationVector wte(xm_, ym_, 0);
+ wMe_.buildFrom(wte, wRe);
+ wMc_ = wMe_ * cMe_.inverse();
+
+ break ;
+ }
+ break ;
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the camera frame:"
+ "functionality not implemented");
+ break ;
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the articular frame:"
+ "functionality not implemented");
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+}
+
+/*!
+ Get the robot position in the world frame.
+
+*/
+void
+vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const
+{
+ wMc = this->wMc_ ;
+}
+
+/*
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes (not implemented)
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, this functionality is not implemented.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
+*/
+void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME :
+ q = 0;
+ break;
+
+ case vpRobot::ARTICULAR_FRAME :
+ std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
+ break;
+ case vpRobot::REFERENCE_FRAME : {
+ // Convert wMc_ to a position
+ // From fMc extract the pose
+ vpRotationMatrix wRc;
+ this->wMc_.extract(wRc);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(wRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ q[i] = this->wMc_[i][3]; // translation x,y,z
+ q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
+ }
+}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp b/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp
new file mode 100644
index 0000000..480e0a0
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp
@@ -0,0 +1,261 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pioneer mobile robot equipped with a pan head simulator without display.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpSimulatorPioneerPan.cpp
+ \brief class that defines the Pioneer mobile robot simulator equipped
+ with a camera able to move in pan.
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpSimulatorPioneerPan.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpExponentialMap.h>
+
+
+/*!
+ Constructor.
+
+ Initialise the robot by a call to init().
+
+ Sampling time is set to 40 ms. To change it you should call
+ setSamplingTime().
+
+*/
+vpSimulatorPioneerPan::vpSimulatorPioneerPan() : wMc_(), wMm_(), xm_(0), ym_(0), theta_(0), q_pan_()
+{
+ init() ;
+}
+
+/*!
+ Robot initialisation.
+
+ Sampling time is set to 40 ms. To change it you should call
+ setSamplingTime().
+
+ Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
+ is set to identity (see get_eJe()).
+
+*/
+void vpSimulatorPioneerPan::init()
+{
+ xm_ = 0;
+ ym_ = 0;
+ theta_ = 0;
+ q_pan_ = 0;
+
+ nDof = 3;
+ eJeAvailable = true;
+ fJeAvailable = false;
+ areJointLimitsAvailable = false;
+ qmin = NULL;
+ qmax = NULL;
+
+ wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
+}
+
+
+/*!
+ Destructor.
+
+*/
+vpSimulatorPioneerPan::~vpSimulatorPioneerPan()
+{
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+ The jacobian expression is given in vpPioneerPan class.
+
+ \param _eJe : A 6 by 3 matrix representing the robot jacobian \f$ {^e}{\bf
+ J}_e\f$ expressed in the end-effector frame.
+*/
+void
+vpSimulatorPioneerPan::get_eJe(vpMatrix &_eJe)
+{
+ _eJe = vpUnicycle::get_eJe();
+}
+
+/*!
+ Send to the controller a velocity.
+
+ \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
+
+ \param v : Velocity vector \f$(v_x, w_z, \dot q)\f$ to apply to the robot.
+
+ Depending on the velocity specified as input, the robot position is updated
+ using the sampling time that can be modified using setSamplingTime().
+
+ \sa setSamplingTime()
+
+*/
+void
+vpSimulatorPioneerPan::setVelocity(const vpRobot::vpControlFrameType frame,
+ const vpColVector &v)
+{
+ switch (frame)
+ {
+ case vpRobot::ARTICULAR_FRAME:{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
+
+ setRobotFrame(frame);
+
+ // v is a 3 dimension vector that contains vx, wz, qpan
+ if (v.size() != 3) {
+ vpERROR_TRACE ("Bad dimension of the control vector");
+ throw vpRobotException (vpRobotException::dimensionError,
+ "Bad dimension of the control vector");
+ }
+
+ vpColVector v_max(3);
+
+ v_max[0] = getMaxTranslationVelocity();
+ v_max[1] = getMaxRotationVelocity();
+ v_max[2] = getMaxRotationVelocity();
+
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+
+ xm_ += delta_t_ * v_sat[0] * cos(theta_);
+ ym_ += delta_t_ * v_sat[0] * sin(theta_);
+ theta_ += delta_t_ * v_sat[1];
+ q_pan_ += delta_t_ * v_sat[2];
+
+ vpRotationMatrix wRm(0, 0, theta_);
+ vpTranslationVector wtm(xm_, ym_, 0);
+ wMm_.buildFrom(wtm, wRm);
+
+ // Update the end effector pose
+ set_pMe(q_pan_);
+
+ // Update the camera pose
+ wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
+
+ // Update the jacobian
+ set_eJe(q_pan_);
+
+ break ;
+ }
+ case vpRobot::CAMERA_FRAME: {
+ vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the camera frame:"
+ "functionality not implemented");
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the reference frame:"
+ "functionality not implemented");
+ break ;
+
+ case vpRobot::MIXT_FRAME:
+ vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
+
+ break ;
+ }
+}
+
+/*!
+ Get the robot position in the world frame.
+
+*/
+void
+vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const
+{
+ wMc = this->wMc_ ;
+}
+
+/*
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes (not implemented)
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+
+ \param position : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, this functionality is not implemented.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
+*/
+void vpSimulatorPioneerPan::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize (6);
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME :
+ q = 0;
+ break;
+
+ case vpRobot::ARTICULAR_FRAME :
+ std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
+ break;
+ case vpRobot::REFERENCE_FRAME : {
+ // Convert wMc_ to a position
+ // From fMc extract the pose
+ vpRotationMatrix wRc;
+ this->wMc_.extract(wRc);
+ vpRxyzVector rxyz;
+ rxyz.buildFrom(wRc);
+
+ for (unsigned int i=0; i < 3; i++) {
+ q[i] = this->wMc_[i][3]; // translation x,y,z
+ q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ }
+
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
+ }
+}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp
new file mode 100644
index 0000000..28bf915
--- /dev/null
+++ b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp
@@ -0,0 +1,2530 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class which provides a simulator for the robot Viper850.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/robot/vpSimulatorViper850.h>
+
+#if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
+
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpIoTools.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <string>
+
+const double vpSimulatorViper850::defaultPositioningVelocity = 25.0;
+
+/*!
+ Basic constructor
+*/
+vpSimulatorViper850::vpSimulatorViper850()
+ : vpRobotWireFrameSimulator(), vpViper850(),
+ q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
+ zeroPos(), reposPos(), toolCustom(false), arm_dir()
+{
+ init();
+ initDisplay();
+
+ tcur = vpTime::measureTimeMs();
+
+ #if defined(_WIN32)
+ mutex_fMi = CreateMutex(NULL,FALSE,NULL);
+ mutex_artVel = CreateMutex(NULL,FALSE,NULL);
+ mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
+ mutex_velocity = CreateMutex(NULL,FALSE,NULL);
+ mutex_display = CreateMutex(NULL,FALSE,NULL);
+
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(
+ NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+ #elif defined (VISP_HAVE_PTHREAD)
+ pthread_mutex_init(&mutex_fMi, NULL);
+ pthread_mutex_init(&mutex_artVel, NULL);
+ pthread_mutex_init(&mutex_artCoord, NULL);
+ pthread_mutex_init(&mutex_velocity, NULL);
+ pthread_mutex_init(&mutex_display, NULL);
+
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+ pthread_create(&thread, NULL, launcher, (void *)this);
+ #endif
+
+ compute_fMi();
+}
+
+/*!
+ Constructor used to enable or disable the external view of the robot.
+
+ \param do_display : When true, enables the display of the external view.
+
+*/
+vpSimulatorViper850::vpSimulatorViper850(bool do_display)
+ : vpRobotWireFrameSimulator(do_display),
+ q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
+ zeroPos(), reposPos(), toolCustom(false), arm_dir()
+{
+ init();
+ initDisplay();
+
+ tcur = vpTime::measureTimeMs();
+
+ #if defined(_WIN32)
+ mutex_fMi = CreateMutex(NULL,FALSE,NULL);
+ mutex_artVel = CreateMutex(NULL,FALSE,NULL);
+ mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
+ mutex_velocity = CreateMutex(NULL,FALSE,NULL);
+ mutex_display = CreateMutex(NULL,FALSE,NULL);
+
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(
+ NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_init(&mutex_fMi, NULL);
+ pthread_mutex_init(&mutex_artVel, NULL);
+ pthread_mutex_init(&mutex_artCoord, NULL);
+ pthread_mutex_init(&mutex_velocity, NULL);
+ pthread_mutex_init(&mutex_display, NULL);
+
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+ pthread_create(&thread, NULL, launcher, (void *)this);
+ #endif
+
+ compute_fMi();
+}
+
+/*!
+ Basic destructor
+*/
+vpSimulatorViper850::~vpSimulatorViper850()
+{
+ robotStop = true;
+
+ #if defined(_WIN32)
+ WaitForSingleObject(hThread,INFINITE);
+ CloseHandle(hThread);
+ CloseHandle(mutex_fMi);
+ CloseHandle(mutex_artVel);
+ CloseHandle(mutex_artCoord);
+ CloseHandle(mutex_velocity);
+ CloseHandle(mutex_display);
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_attr_destroy(&attr);
+ pthread_join(thread, NULL);
+ pthread_mutex_destroy(&mutex_fMi);
+ pthread_mutex_destroy(&mutex_artVel);
+ pthread_mutex_destroy(&mutex_artCoord);
+ pthread_mutex_destroy(&mutex_velocity);
+ pthread_mutex_destroy(&mutex_display);
+ #endif
+
+ if (robotArms != NULL)
+ {
+ // free_Bound_scene (&(camera));
+ for(int i = 0; i < 6; i++)
+ free_Bound_scene (&(robotArms[i]));
+ }
+
+ delete[] robotArms;
+ delete[] fMi;
+}
+
+/*!
+ Method which initialises the parameters linked to the robot caracteristics.
+
+ Set the path to the arm files (*.bnd and *.sln) used by the
+ simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is
+ not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
+ variable that the user has to set.
+*/
+void
+vpSimulatorViper850::init()
+{
+ // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
+ // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
+ std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
+ bool armDirExists = false;
+ for(size_t i=0; i < arm_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
+ arm_dir = arm_dirs[i];
+ armDirExists = true;
+ break;
+ }
+ if (! armDirExists) {
+ try {
+ arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
+ std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
+ }
+ catch (...) {
+ std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
+ }
+ }
+
+ this->init(vpViper850::defaultTool);
+ toolCustom = false;
+
+ size_fMi = 8;
+ fMi = new vpHomogeneousMatrix[8];
+ artCoord.resize(njoint);
+ artVel.resize(njoint);
+
+ zeroPos.resize(njoint);
+ zeroPos = 0;
+ zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
+ reposPos.resize(njoint);
+ reposPos = 0;
+ reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
+
+ artCoord = reposPos;
+ artVel = 0;
+
+ q_prev_getdis.resize(njoint);
+ q_prev_getdis = 0;
+ first_time_getdis = true;
+
+ positioningVelocity = defaultPositioningVelocity ;
+
+ setRobotFrame (vpRobot::ARTICULAR_FRAME);
+ this->setRobotState(vpRobot::STATE_STOP);
+
+ // Software joint limits in radians
+ //joint_min.resize(njoint);
+ joint_min[0] = vpMath::rad(-50);
+ joint_min[1] = vpMath::rad(-135);
+ joint_min[2] = vpMath::rad(-40);
+ joint_min[3] = vpMath::rad(-190);
+ joint_min[4] = vpMath::rad(-110);
+ joint_min[5] = vpMath::rad(-184);
+ //joint_max.resize(njoint);
+ joint_max[0] = vpMath::rad(50);
+ joint_max[1] = vpMath::rad(-40);
+ joint_max[2] = vpMath::rad(215);
+ joint_max[3] = vpMath::rad(190);
+ joint_max[4] = vpMath::rad(110);
+ joint_max[5] = vpMath::rad(184);
+}
+
+/*!
+ Method which initialises the parameters linked to the display part.
+*/
+void
+vpSimulatorViper850::initDisplay()
+{
+ robotArms = NULL;
+ robotArms = new Bound_scene[6];
+ initArms();
+ setExternalCameraPosition(vpHomogeneousMatrix(0.0,0.5,1.5,vpMath::rad(90),0,0));
+ cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
+ setExternalCameraParameters(cameraParam);
+ vpCameraParameters tmp;
+ getCameraParameters(tmp,640,480);
+ px_int = tmp.get_px();
+ py_int = tmp.get_py();
+ sceneInitialized = true;
+}
+
+
+/*!
+
+ Initialize the robot kinematics with the extrinsic calibration
+ parameters associated to a specific camera.
+
+ The eMc parameters depend on the camera.
+
+ \warning Only perspective projection without distortion is available!
+
+ \param tool : Tool to use.
+
+ \param proj_model : Projection model associated to the camera.
+
+ \sa vpCameraParameters, init()
+*/
+void
+vpSimulatorViper850::init (vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType proj_model)
+{
+ this->projModel = proj_model;
+
+ // Use here default values of the robot constant parameters.
+ switch (tool) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
+ erc[2] = vpMath::rad(-91.50); // rz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
+
+ setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
+ erc[0] = vpMath::rad(0.1527764261); // rx
+ erc[1] = vpMath::rad(1.285092455); // ry
+ erc[2] = vpMath::rad(-90.75777848); // rz
+ etc[0] = -0.04558630174; // tx
+ etc[1] = -0.00134326752; // ty
+ etc[2] = 0.001000828017; // tz
+
+ setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
+ break;
+ }
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
+ break;
+ }
+ }
+
+ vpRotationMatrix eRc(erc);
+ this->eMc.buildFrom(etc, eRc);
+
+ setToolType(tool);
+ return ;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param image_width : Image width used to compute camera calibration.
+ \param image_height : Image height used to compute camera calibration.
+
+ \warning The image size must be : 640x480 !
+*/
+
+void
+vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
+ const unsigned int &image_width,
+ const unsigned int &image_height)
+{
+ if (toolCustom)
+ {
+ vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
+ }
+ // Set default parameters
+ switch (getToolType()) {
+ case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480)
+ {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
+ cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ }
+ break;
+ }
+ case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
+ // Set default intrinsic camera parameters for 640x480 images
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \""
+ << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
+ cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
+ }
+ else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ }
+ break;
+ }
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
+ case vpViper850::TOOL_GENERIC_CAMERA: {
+ std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
+ break;
+ }
+ }
+ return;
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param I_ : A B&W image send by the current camera in use.
+
+ \warning The image size must be : 640x480 !
+*/
+void
+vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<unsigned char> &I_)
+{
+ getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+}
+
+/*!
+ Get the current intrinsic camera parameters obtained by calibration.
+
+ \param cam : In output, camera parameters to fill.
+ \param I_ : A B&W image send by the current camera in use.
+
+ \warning The image size must be : 640x480 !
+*/
+void
+vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I_)
+{
+ getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+}
+
+
+/*!
+ Set the intrinsic camera parameters.
+
+ \param cam : The desired camera parameters.
+*/
+void
+vpSimulatorViper850::setCameraParameters(const vpCameraParameters &cam)
+{
+ px_int = cam.get_px();
+ py_int = cam.get_py();
+ toolCustom = true;
+}
+
+
+/*!
+ Method lauched by the thread to compute the position of the robot in the articular frame.
+*/
+void
+vpSimulatorViper850::updateArticularPosition()
+{
+ double tcur_1 = tcur;// temporary variable used to store the last time since the last command
+
+ while (!robotStop)
+ {
+ //Get current time
+ tprev = tcur_1;
+ tcur = vpTime::measureTimeMs();
+
+
+ if(setVelocityCalled || !constantSamplingTimeMode){
+ setVelocityCalled = false;
+ computeArticularVelocity();
+
+ double ellapsedTime = (tcur - tprev) * 1e-3;
+ if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
+ ellapsedTime = getSamplingTime(); // in second
+ }
+
+ vpColVector articularCoordinates = get_artCoord();
+ articularCoordinates = get_artCoord();
+ vpColVector articularVelocities = get_artVel();
+
+ if (jointLimit)
+ {
+ double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
+ if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
+ if (verbose_) {
+ std::cout << "Joint " << jointLimitArt-1
+ << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
+ }
+ articularVelocities = 0.0;
+ }
+ else
+ jointLimit = false;
+ }
+
+ articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
+ articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
+ articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
+ articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
+ articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
+ articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
+
+ int jl = isInJointLimit();
+
+ if (jl != 0 && jointLimit == false)
+ {
+ if (jl < 0)
+ ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
+ else
+ ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
+
+ for (unsigned int i = 0; i < 6; i++)
+ articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
+
+ jointLimit = true;
+ jointLimitArt = (unsigned int)fabs((double)jl);
+ }
+
+ set_artCoord(articularCoordinates);
+ set_artVel(articularVelocities);
+
+ compute_fMi();
+
+ if (displayAllowed)
+ {
+ vpDisplay::display(I);
+ vpDisplay::displayFrame(I,getExternalCameraPosition (),cameraParam,0.2,vpColor::none, thickness_);
+ vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[7],cameraParam,0.1,vpColor::none, thickness_);
+ }
+
+ if (displayType == MODEL_3D && displayAllowed)
+ {
+ while (get_displayBusy()) vpTime::wait(2);
+ vpSimulatorViper850::getExternalImage(I);
+ set_displayBusy(false);
+ }
+
+
+ if (displayType == MODEL_DH && displayAllowed)
+ {
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+
+ //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
+
+ vpImagePoint iP, iP_1;
+ vpPoint pt(0,0,0);
+
+ pt.track(getExternalCameraPosition ());
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
+ pt.track(getExternalCameraPosition ()*fMit[0]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
+ vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
+ for (int k = 1; k < 7; k++)
+ {
+ pt.track(getExternalCameraPosition ()*fMit[k-1]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
+
+ pt.track(getExternalCameraPosition ()*fMit[k]);
+ vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
+
+ vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
+ }
+ vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
+ }
+
+ vpDisplay::flush(I);
+
+ vpTime::wait( tcur, 1000 * getSamplingTime() );
+ tcur_1 = tcur;
+ }else{
+ vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
+ }
+ }
+}
+
+/*!
+ Compute the pose between the robot reference frame and the frames used to compute the Denavit-Hartenberg
+ representation. The last element of the table corresponds to the pose between the reference frame and
+ the camera frame.
+
+ To compute the different homogeneous matrices, this function needs the articular coordinates as input.
+
+ Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,
+ \f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
+*/
+void
+vpSimulatorViper850::compute_fMi()
+{
+ //vpColVector q = get_artCoord();
+ vpColVector q(6);//; = get_artCoord();
+ q = get_artCoord();
+
+ vpHomogeneousMatrix fMit[8];
+
+ double q1 = q[0];
+ double q2 = q[1];
+ double q3 = q[2];
+ double q4 = q[3];
+ double q5 = q[4];
+ double q6 = q[5];
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
+ double c23 = cos(q2+q3);
+ double s23 = sin(q2+q3);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
+ double c6 = cos(q6);
+ double s6 = sin(q6);
+
+ fMit[0][0][0] = c1;
+ fMit[0][1][0] = s1;
+ fMit[0][2][0] = 0;
+ fMit[0][0][1] = 0;
+ fMit[0][1][1] = 0;
+ fMit[0][2][1] = -1;
+ fMit[0][0][2] = -s1;
+ fMit[0][1][2] = c1;
+ fMit[0][2][2] = 0;
+ fMit[0][0][3] = a1*c1;
+ fMit[0][1][3] = a1*s1;
+ fMit[0][2][3] = d1;
+
+ fMit[1][0][0] = c1*c2;
+ fMit[1][1][0] = s1*c2;
+ fMit[1][2][0] = -s2;
+ fMit[1][0][1] = -c1*s2;
+ fMit[1][1][1] = -s1*s2;
+ fMit[1][2][1] = -c2;
+ fMit[1][0][2] = -s1;
+ fMit[1][1][2] = c1;
+ fMit[1][2][2] = 0;
+ fMit[1][0][3] = c1*(a2*c2+a1);
+ fMit[1][1][3] = s1*(a2*c2+a1);
+ fMit[1][2][3] = d1-a2*s2;
+
+ double quickcomp1 = a3*c23-a2*c2-a1;
+
+ fMit[2][0][0] = -c1*c23;
+ fMit[2][1][0] = -s1*c23;
+ fMit[2][2][0] = s23;
+ fMit[2][0][1] = s1;
+ fMit[2][1][1] = -c1;
+ fMit[2][2][1] = 0;
+ fMit[2][0][2] = c1*s23;
+ fMit[2][1][2] = s1*s23;
+ fMit[2][2][2] = c23;
+ fMit[2][0][3] = -c1*quickcomp1;
+ fMit[2][1][3] = -s1*quickcomp1;
+ fMit[2][2][3] = a3*s23-a2*s2+d1;
+
+ double quickcomp2 = c1*(s23*d4 - quickcomp1);
+ double quickcomp3 = s1*(s23*d4 - quickcomp1);
+
+ fMit[3][0][0] = -c1*c23*c4+s1*s4;
+ fMit[3][1][0] = -s1*c23*c4-c1*s4;
+ fMit[3][2][0] = s23*c4;
+ fMit[3][0][1] = c1*s23;
+ fMit[3][1][1] = s1*s23;
+ fMit[3][2][1] = c23;
+ fMit[3][0][2] = -c1*c23*s4-s1*c4;
+ fMit[3][1][2] = -s1*c23*s4+c1*c4;
+ fMit[3][2][2] = s23*s4;
+ fMit[3][0][3] = quickcomp2;
+ fMit[3][1][3] = quickcomp3;
+ fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
+
+ fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
+ fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
+ fMit[4][2][0] = s23*c4*c5+c23*s5;
+ fMit[4][0][1] = c1*c23*s4+s1*c4;
+ fMit[4][1][1] = s1*c23*s4-c1*c4;
+ fMit[4][2][1] = -s23*s4;
+ fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
+ fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
+ fMit[4][2][2] = -s23*c4*s5+c23*c5;
+ fMit[4][0][3] = quickcomp2;
+ fMit[4][1][3] = quickcomp3;
+ fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
+
+ fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
+ fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
+ fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+ fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
+ fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
+ fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+ fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
+ fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
+ fMit[5][2][2] = -s23*c4*s5+c23*c5;
+ fMit[5][0][3] = quickcomp2;
+ fMit[5][1][3] = quickcomp3;
+ fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
+
+ fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
+ fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
+ fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+ fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
+ fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
+ fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+ fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
+ fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
+ fMit[6][2][2] = -s23*c4*s5+c23*c5;
+ fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
+ fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
+ fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
+
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
+ cMe = cMe.inverse();
+// fMit[7] = fMit[6] * cMe;
+ vpViper::get_fMc(q,fMit[7]);
+
+ #if defined(_WIN32)
+ WaitForSingleObject(mutex_fMi,INFINITE);
+ for (int i = 0; i < 8; i++)
+ fMi[i] = fMit[i];
+ ReleaseMutex(mutex_fMi);
+ #elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock (&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMi[i] = fMit[i];
+ pthread_mutex_unlock (&mutex_fMi);
+ #endif
+}
+
+
+/*!
+Change the robot state.
+
+\param newState : New requested robot state.
+*/
+vpRobot::vpRobotStateType
+vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState)
+{
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ // Start primitive STOP only if the current state is Velocity
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ stopMotion();
+ }
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ stopMotion();
+ }
+ else {
+ //std::cout << "Change the control mode from stop to position control.\n";
+ }
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
+ }
+ break;
+ }
+ case vpRobot::STATE_ACCELERATION_CONTROL:
+ default:
+ break ;
+ }
+
+ return vpRobot::setRobotState (newState);
+}
+
+/*!
+ Apply a velocity to the robot.
+
+ \param frame : Control frame in which the velocity is expressed. Velocities
+ could be expressed in articular, camera frame, reference frame or mixt frame.
+
+ \param vel : Velocity vector. Translation velocities are expressed
+ in m/s while rotation velocities in rad/s. The size of this vector
+ is always 6.
+
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+
+ - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
+ camera frame.
+
+ - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
+ reference frame.
+
+ - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
+ v_y, v_z \f$ are expressed in the reference frame and rotations \f$
+ \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+
+ \exception vpRobotException::wrongStateError : If a the robot is not
+ configured to handle a velocity. The robot can handle a velocity only if the
+ velocity control mode is set. For that, call setRobotState(
+ vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
+
+ \warning Velocities could be saturated if one of them exceed the
+ maximal autorized speed (see vpRobot::maxTranslationVelocity and
+ vpRobot::maxRotationVelocity). To change these values use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ vpColVector qvel(6);
+ // Set a joint velocity
+ qvel[0] = 0.1; // Joint 1 velocity in rad/s
+ qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
+ qvel[2] = 0; // Joint 3 velocity in rad/s
+ qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
+ qvel[4] = 0; // Joint 5 velocity in rad/s
+ qvel[5] = 0; // Joint 6 velocity in rad/s
+
+ // Initialize the controller to position control
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ for ( ; ; ) {
+ // Apply a velocity in the joint space
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+
+ // Compute new velocities qvel...
+ }
+ // Stop the robot
+ robot.setRobotState(vpRobot::STATE_STOP);
+}
+ \endcode
+*/
+void
+vpSimulatorViper850::setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ vpERROR_TRACE ("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException (vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
+ vpColVector vel_sat(6);
+
+ double scale_trans_sat = 1;
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+ double vel_trans_max = getMaxTranslationVelocity();
+ double vel_rot_max = getMaxRotationVelocity();
+
+ double vel_abs; // Absolute value
+
+ // Velocity saturation
+ switch(frame)
+ {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ {
+ if (vel.getRows() != 6)
+ {
+ vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
+
+ for (unsigned int i = 0 ; i < 3; ++ i)
+ {
+ vel_abs = fabs (vel[i]);
+ if (vel_abs > vel_trans_max && !jointLimit)
+ {
+ vel_trans_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
+ "(axis nr. %d).", vel[i], i+1);
+ }
+
+ vel_abs = fabs (vel[i+3]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", vel[i+3], i+4);
+ }
+ }
+
+ if (vel_trans_max > getMaxTranslationVelocity())
+ scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
+
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+
+ if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
+ {
+ if (scale_trans_sat < scale_rot_sat)
+ scale_sat = scale_trans_sat;
+ else
+ scale_sat = scale_rot_sat;
+ }
+ break;
+ }
+
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ if (vel.getRows() != 6)
+ {
+ vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
+ for (unsigned int i = 0 ; i < 6; ++ i)
+ {
+ vel_abs = fabs (vel[i]);
+ if (vel_abs > vel_rot_max && !jointLimit)
+ {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", vel[i], i+1);
+ }
+ }
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if ( scale_rot_sat < 1 )
+ scale_sat = scale_rot_sat;
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+
+ set_velocity (vel * scale_sat);
+ setRobotFrame (frame);
+ setVelocityCalled = true;
+}
+
+
+/*!
+ Compute the articular velocity relative to the velocity in another frame.
+*/
+void
+vpSimulatorViper850::computeArticularVelocity()
+{
+ vpRobot::vpControlFrameType frame = getRobotFrame();
+
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+ double vel_rot_max = getMaxRotationVelocity();
+ double vel_abs;
+
+ vpColVector articularCoordinates = get_artCoord();
+ vpColVector velocityframe = get_velocity();
+ vpColVector articularVelocity;
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix eVc(eMc);
+ vpViper850::get_eJe(articularCoordinates,eJe_);
+ eJe_ = eJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates,eJe_);
+ articularVelocity = eJe_*eVc*velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpMatrix fJe_;
+ vpViper850::get_fJe(articularCoordinates,fJe_);
+ fJe_ = fJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates,fJe_);
+ articularVelocity = fJe_*velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ articularVelocity = velocityframe;
+ set_artVel (articularVelocity);
+ break;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+
+
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ case vpRobot::REFERENCE_FRAME :
+ {
+ for (unsigned int i = 0 ; i < 6; ++ i)
+ {
+ vel_abs = fabs (articularVelocity[i]);
+ if (vel_abs > vel_rot_max && !jointLimit)
+ {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).", articularVelocity[i], i+1);
+ }
+ }
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if ( scale_rot_sat < 1 )
+ scale_sat = scale_rot_sat;
+
+ set_artVel(articularVelocity * scale_sat);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ case vpRobot::MIXT_FRAME :
+ {
+ break;
+ }
+ }
+}
+
+
+/*!
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param vel : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpSimulatorViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+
+ for ( ; ; ) {
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+}
+ \endcode
+*/
+void
+vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
+{
+ vel.resize(6);
+
+ vpColVector articularCoordinates = get_artCoord();
+ vpColVector articularVelocity = get_artVel();
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix cVe(eMc);
+ vpViper850::get_eJe(articularCoordinates,eJe_);
+ vel = cVe*eJe_*articularVelocity;
+ break ;
+ }
+ case vpRobot::ARTICULAR_FRAME :
+ {
+ vel = articularVelocity;
+ break ;
+ }
+ case vpRobot::REFERENCE_FRAME :
+ {
+ vpMatrix fJe_;
+ vpViper850::get_fJe(articularCoordinates,fJe_);
+ vel = fJe_*articularVelocity;
+ break ;
+ }
+ case vpRobot::MIXT_FRAME :
+ {
+ break ;
+ }
+ default:
+ {
+ vpERROR_TRACE ("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
+ }
+}
+
+/*!
+ Get the robot time stamped velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param vel : Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \warning In camera frame, reference frame and mixt frame, the representation
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
+ z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+
+ \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
+*/
+void
+vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getVelocity(frame, vel);
+}
+
+/*!
+ Get the robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ // Set requested joint velocities
+ vpColVector q_dot(6);
+ q_dot[0] = 0.1; // Joint 1 velocity in rad/s
+ q_dot[1] = 0.2; // Joint 2 velocity in rad/s
+ q_dot[2] = 0.3; // Joint 3 velocity in rad/s
+ q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
+ q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
+ q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
+
+ vpSimulatorViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+
+ // Moves the joint in velocity
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
+
+ // Initialisation of the velocity measurement
+ vpColVector q_dot_mes; // Measured velocities
+
+ for ( ; ; ) {
+ q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
+ vpTime::wait(40); // wait 40 ms
+ // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
+ }
+}
+ \endcode
+*/
+vpColVector
+vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame)
+{
+ vpColVector vel(6);
+ getVelocity (frame, vel);
+
+ return vel;
+}
+
+/*!
+ Get the time stamped robot velocities.
+
+ \param frame : Frame in wich velocities are mesured.
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \return Measured velocities. Translations are expressed in m/s
+ and rotations in rad/s.
+
+ \sa getVelocity(vpRobot::vpControlFrameType frame)
+*/
+vpColVector
+vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ vpColVector vel(6);
+ getVelocity (frame, vel);
+
+ return vel;
+}
+
+void
+vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
+{
+ double vel_rot_max = getMaxRotationVelocity();
+ double velmax = fabs(q[0]);
+ for (unsigned int i = 1; i < 6; i++)
+ {
+ if (velmax < fabs(q[i]))
+ velmax = fabs(q[i]);
+ }
+
+ double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
+ q = q * alpha;
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param q : A six dimension vector corresponding to the
+ position to reach. All the positions are expressed in meters for the
+ translations and radians for the rotations. If the position is out
+ of range, an exception is provided.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X, Y, Z, A, B, C,
+ with X,Y,Z the translations, and A,B,C the rotations of the
+ end-effector.
+
+ - In the camera and the reference frame, rotations are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector position(6);
+ // Set positions in the camera frame
+ position[0] = 0.1; // x axis, in meter
+ position[1] = 0.2; // y axis, in meter
+ position[2] = 0.3; // z axis, in meter
+ position[3] = M_PI/8; // rotation around x axis, in rad
+ position[4] = M_PI/4; // rotation around y axis, in rad
+ position[5] = M_PI; // rotation around z axis, in rad
+
+ vpSimulatorViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+}
+ \endcode
+
+ To catch the exception if the position is out of range, modify the code like:
+
+ \code
+ try {
+ robot.setPosition(vpRobot::CAMERA_FRAME, position);
+ }
+ catch (vpRobotException e) {
+ if (e.getCode() == vpRobotException::positionOutOfRangeError) {
+ std::cout << "The position is out of range" << std::endl;
+ }
+ \endcode
+
+*/
+void
+vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q)
+{
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
+ {
+ vpERROR_TRACE ("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ }
+
+ vpColVector articularCoordinates = get_artCoord();
+
+ vpColVector error(6);
+ double errsqr = 0;
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ unsigned int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++)
+ {
+ txyz[i] = q[i];
+ rxyz[i] = q[i+3];
+ }
+
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
+
+ vpHomogeneousMatrix fMc_;
+ vpViper850::get_fMc(articularCoordinates, fMc_);
+
+ vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
+
+ do
+ {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc2, qdes, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0)
+ {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (qdes);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }
+ else
+ {
+ vpERROR_TRACE ("Positionning error.");
+ throw vpRobotException (vpRobotException::positionOutOfRangeError,
+ "Position out of range.");
+ }
+ }while (errsqr > 1e-8 && nbSol > 0);
+
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ do
+ {
+ articularCoordinates = get_artCoord();
+ error = q - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (q);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }while (errsqr > 1e-8);
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ unsigned int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i=0; i < 3; i++)
+ {
+ txyz[i] = q[i];
+ rxyz[i] = q[i+3];
+ }
+
+ vpRotationMatrix fRc(rxyz);
+ vpHomogeneousMatrix fMc_(txyz, fRc);
+
+ do
+ {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc_, qdes, verbose_);
+ if (nbSol > 0)
+ {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ //findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4)
+ {
+ set_artCoord (qdes);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
+ }
+ }
+ else
+ vpERROR_TRACE ("Positionning error. Position unreachable");
+ }while (errsqr > 1e-8 && nbSol > 0);
+ break ;
+ }
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+}
+
+/*!
+ Move to an absolute position with a given percent of max velocity.
+ The percent of max velocity is to set with setPositioningVelocity().
+ The position to reach can be specified in joint coordinates, in the
+ camera frame or in the reference frame.
+
+ This method overloads setPosition(const
+ vpRobot::vpControlFrameType, const vpColVector &).
+
+ \warning This method is blocking. It returns only when the position
+ is reached by the robot.
+
+ \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
+ the position to reach. All the positions are expressed in meters for
+ the translations and radians for the rotations.
+
+ \param frame : Frame in which the position is expressed.
+
+ - In the joint space, positions are respectively X (pos1), Y (pos2),
+ Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
+ translations, and A,B,C the rotations of the end-effector.
+
+ - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
+ represented by a vpRxyzVector.
+
+ - Mixt frame is not implemented. By mixt frame we mean, translations
+ expressed in the reference frame, and rotations in the camera
+ frame.
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+
+int main()
+{
+ // Set positions in the camera frame
+ double pos1 = 0.1; // x axis, in meter
+ double pos2 = 0.2; // y axis, in meter
+ double pos3 = 0.3; // z axis, in meter
+ double pos4 = M_PI/8; // rotation around x axis, in rad
+ double pos5 = M_PI/4; // rotation around y axis, in rad
+ double pos6 = M_PI; // rotation around z axis, in rad
+
+ vpRobotViper850 robot;
+
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Set the max velocity to 20%
+ robot.setPositioningVelocity(20);
+
+ // Moves the robot in the camera frame
+ robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
+}
+ \endcode
+
+ \sa setPosition()
+*/
+void vpSimulatorViper850::setPosition (const vpRobot::vpControlFrameType frame,
+ const double pos1,
+ const double pos2,
+ const double pos3,
+ const double pos4,
+ const double pos5,
+ const double pos6)
+{
+ try{
+ vpColVector position(6) ;
+ position[0] = pos1 ;
+ position[1] = pos2 ;
+ position[2] = pos3 ;
+ position[3] = pos4 ;
+ position[4] = pos5 ;
+ position[5] = pos6 ;
+
+ setPosition(frame, position) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+ Move to an absolute joint position with a given percent of max
+ velocity. The robot state is set to position control. The percent
+ of max velocity is to set with setPositioningVelocity(). The
+ position to reach is defined in the position file.
+
+ \param filename : Name of the position file to read. The
+ readPosFile() documentation shows a typical content of such a
+ position file.
+
+ This method has the same behavior than the sample code given below;
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpColVector q;
+ vpSimulatorViper850 robot;
+
+ robot.readPosFile("MyPositionFilename.pos", q);
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+ \endcode
+
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
+ implemented.
+
+ \exception vpRobotException::positionOutOfRangeError : The requested
+ position is out of range.
+
+ \sa setPositioningVelocity()
+*/
+void vpSimulatorViper850::setPosition(const char *filename)
+{
+ vpColVector q;
+ bool ret;
+
+ ret = this->readPosFile(filename, q);
+
+ if (ret == false) {
+ vpERROR_TRACE ("Bad position in \"%s\"", filename);
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Bad position in filename.");
+ }
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+}
+
+/*!
+ Get the current position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param q : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ vpColVector position;
+ robot.getPosition(vpRobot::REFERENCE_FRAME, position);
+
+ vpTranslationVector ftc; // reference frame to camera frame translations
+ vpRxyzVector frc; // reference frame to camera frame rotations
+
+ // Update the transformation between reference frame and camera frame
+ for (int i=0; i < 3; i++) {
+ ftc[i] = position[i]; // tx, ty, tz
+ frc[i] = position[i+3]; // ry, ry, rz
+ }
+
+ // Create a rotation matrix from the Rxyz rotation angles
+ vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
+
+ // Create the camera to fix frame transformation in terms of a
+ // homogeneous matrix
+ vpHomogeneousMatrix fMc(fRc, ftc);
+}
+ \endcode
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
+ \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
+
+*/
+void
+vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+{
+ q.resize(6);
+
+ switch(frame)
+ {
+ case vpRobot::CAMERA_FRAME :
+ {
+ q = 0;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ q = get_artCoord();
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ vpHomogeneousMatrix fMc_;
+ vpViper::get_fMc (get_artCoord(), fMc_);
+
+ vpRotationMatrix fRc;
+ fMc_.extract(fRc);
+ vpRxyzVector rxyz(fRc);
+
+ vpTranslationVector txyz;
+ fMc_.extract(txyz);
+
+ for (unsigned int i=0; i <3; i++)
+ {
+ q[i] = txyz[i];
+ q[i+3] = rxyz[i];
+ }
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME:
+ {
+ vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
+ throw vpRobotException (vpRobotException::lowLevelError,
+ "Positionning error: "
+ "Mixt frame not implemented.");
+ break ;
+ }
+ }
+}
+
+/*!
+
+ Get the current time stamped position of the robot.
+
+ \param frame : Control frame type in which to get the position, either :
+ - in the camera cartesien frame,
+ - joint (articular) coordinates of each axes
+ - in a reference or fixed cartesien frame attached to the robot base
+ - in a mixt cartesien frame (translation in reference
+ frame, and rotation in camera frame)
+
+ \param q : Measured position of the robot:
+ - in camera cartesien frame, a 6 dimension vector, set to 0.
+
+ - in articular, a 6 dimension vector corresponding to the joint
+ position of each dof in radians.
+
+ - in reference frame, a 6 dimension vector, the first 3 values correspond to
+ the translation tx, ty, tz in meters (like a vpTranslationVector), and the
+ last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
+ below show how to convert this position into a vpHomogeneousMatrix:
+
+ \param timestamp : Unix time in second since January 1st 1970.
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+ */
+void
+vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getPosition(frame, q);
+}
+
+
+/*!
+ Get the current position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+
+ The difference is here that the position is returned using a ThetaU
+ representation.
+
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+*/
+void
+vpSimulatorViper850::getPosition (const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position)
+{
+ vpColVector posRxyz;
+ //recupere position en Rxyz
+ this->getPosition(frame,posRxyz);
+
+ //recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));
+
+ //remplit le vpPoseVector avec translation et rotation ThetaU
+ for(unsigned int j=0;j<3;j++)
+ {
+ position[j]=posRxyz[j];
+ position[j+3]=RtuVect[j];
+ }
+}
+
+/*!
+
+ Get the current time stamped position of the robot.
+
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+
+ The difference is here that the position is returned using a ThetaU
+ representation.
+
+ */
+void
+vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame,
+ vpPoseVector &position, double ×tamp)
+{
+ timestamp = vpTime::measureTimeSecond();
+ getPosition(frame, position);
+}
+
+/*!
+ This method enables to set the minimum and maximum joint limits for all the six axis of the robot. All the values have to be given in radian.
+
+ \param limitMin : The minimum joint limits are given in a vector of size 6. All the value must be in radian.
+ \param limitMax : The maximum joint limits are given in a vector of size 6. All the value must be in radian.
+*/
+void
+vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
+{
+ if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
+ {
+ vpTRACE("Joint limit vector has not a size of 6 !");
+ return;
+ }
+
+ joint_min[0] = limitMin[0];
+ joint_min[1] = limitMin[1];
+ joint_min[2] = limitMin[2];
+ joint_min[3] = limitMin[3];
+ joint_min[4] = limitMin[4];
+ joint_min[5] = limitMin[5];
+
+ joint_max[0] = limitMax[0];
+ joint_max[1] = limitMax[1];
+ joint_max[2] = limitMax[2];
+ joint_max[3] = limitMax[3];
+ joint_max[4] = limitMax[4];
+ joint_max[5] = limitMax[5];
+
+}
+
+/*!
+ Test to detect if the robot is near one of its singularities.
+
+ The goal is to avoid the problems du to such configurations.
+*/
+bool
+vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
+{
+ double q2 = q[1];
+ double q3 = q[2];
+ double q5 = q[4];
+
+ double c2 = cos(q2);
+ double c3 = cos(q3);
+ double s3 = sin(q3);
+ double c23 = cos(q2+q3);
+ double s23 = sin(q2+q3);
+ double s5 = sin(q5);
+
+ bool cond1 = fabs(s5) < 1e-1;
+ bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
+ bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
+
+ if(cond1)
+ {
+ J[3][0] = 0;
+ J[5][0] = 0;
+ J[3][1] = 0;
+ J[5][1] = 0;
+ J[3][2] = 0;
+ J[5][2] = 0;
+ J[3][3] = 0;
+ J[5][3] = 0;
+ J[3][4] = 0;
+ J[5][4] = 0;
+ J[3][5] = 0;
+ J[5][5] = 0;
+ return true;
+ }
+
+ if(cond2)
+ {
+ J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
+ J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
+ J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
+ return true;
+ }
+
+ if(cond3)
+ {
+ J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
+ J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
+ }
+
+ return false;
+}
+
+/*!
+ Method used to check if the robot reached a joint limit.
+*/
+int
+vpSimulatorViper850::isInJointLimit ()
+{
+ int artNumb = 0;
+ double diff = 0;
+ double difft = 0;
+
+ vpColVector articularCoordinates = get_artCoord();
+
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ if (articularCoordinates[i] <= joint_min[i])
+ {
+ difft = joint_min[i] - articularCoordinates[i];
+ if (difft > diff)
+ {
+ diff = difft;
+ artNumb = -(int)i-1;
+ }
+ }
+ }
+
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ if (articularCoordinates[i] >= joint_max[i])
+ {
+ difft = articularCoordinates[i] - joint_max[i];
+ if (difft > diff)
+ {
+ diff = difft;
+ artNumb = (int)(i+1);
+ }
+ }
+ }
+
+ if (artNumb != 0)
+ std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
+
+ return artNumb;
+}
+
+/*!
+ Get the robot displacement expressed in the camera frame since the last call
+ of this method.
+
+ \param displacement : The measured displacement in the camera frame. The
+ dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
+ rz). Translations are expressed in meters, rotations in radians with
+ the Euler Rxyz representation.
+
+ \sa getDisplacement(), getArticularDisplacement()
+*/
+void
+vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::CAMERA_FRAME, displacement);
+}
+
+/*!
+ Get the robot joint displacement since the last call of this method.
+
+ \param displacement : The measured joint displacement. The dimension
+ of \e displacement is 6 (the robot joint number). All the values are
+ expressed in radians.
+
+ \sa getDisplacement(), getCameraDisplacement()
+*/
+void
+vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
+{
+ getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+}
+
+/*!
+ Get the robot displacement since the last call of this method.
+
+ \warning This functionnality is not implemented for the moment in the
+ cartesian space. It is only available in the joint space
+ (vpRobot::ARTICULAR_FRAME).
+
+ \param frame : The frame in which the measured displacement is expressed.
+
+ \param displacement : The measured displacement since the last call
+ of this method. The dimension of \e displacement is always
+ 6. Translations are expressed in meters, rotations in radians.
+
+ In camera or reference frame, rotations are expressed with the
+ Euler Rxyz representation.
+
+ \sa getArticularDisplacement(), getCameraDisplacement()
+*/
+void
+vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame,
+ vpColVector &displacement)
+{
+ displacement.resize (6);
+ displacement = 0;
+ vpColVector q_cur(6);
+
+ q_cur = get_artCoord();
+
+ if ( ! first_time_getdis )
+ {
+ switch (frame)
+ {
+ case vpRobot::CAMERA_FRAME:
+ {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::ARTICULAR_FRAME:
+ {
+ displacement = q_cur - q_prev_getdis;
+ break ;
+ }
+
+ case vpRobot::REFERENCE_FRAME:
+ {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ break ;
+ }
+
+ case vpRobot::MIXT_FRAME:
+ {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ break ;
+ }
+ }
+ }
+ else
+ {
+ first_time_getdis = false;
+ }
+
+ // Memorize the joint position for the next call
+ q_prev_getdis = q_cur;
+}
+
+/*!
+Read joint positions in a specific Viper850 position file.
+
+This position file has to start with a header. The six joint positions
+are given after the "R:" keyword. The first 3 values correspond to the
+joint translations X,Y,Z expressed in meters. The 3 last values
+correspond to the joint rotations A,B,C expressed in degres to be more
+representative for the user. Theses values are then converted in
+radians in \e q. The character "#" starting a line indicates a
+comment.
+
+A typical content of such a file is given below:
+
+\code
+#Viper - Position - Version 1.0
+# file: "myposition.pos "
+#
+# R: A B C D E F
+# Joint position in degrees
+#
+
+R: 0.1 0.3 -0.25 -80.5 80 0
+\endcode
+
+\param filename : Name of the position file to read.
+
+\param q : The six joint positions. Values are expressed in radians.
+
+\return true if the positions were successfully readen in the file. false, if
+an error occurs.
+
+The code below shows how to read a position from a file and move the robot to this position.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpSimulatorViper850 robot;
+
+ // Enable the position control of the robot
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+
+ // Get the current robot joint positions
+ vpColVector q; // Current joint position
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ // Save this position in a file named "current.pos"
+ robot.savePosFile("current.pos", q);
+
+ // Get the position from a file and move to the registered position
+ robot.readPosFile("current.pos", q); // Set the joint position from the file
+
+ robot.setPositioningVelocity(5); // Positioning velocity set to 5%
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
+}
+\endcode
+
+\sa savePosFile()
+*/
+bool
+vpSimulatorViper850::readPosFile(const char *filename, vpColVector &q)
+{
+ FILE * fd ;
+ fd = fopen(filename, "r") ;
+ if (fd == NULL)
+ return false;
+
+ char line[FILENAME_MAX];
+ char dummy[FILENAME_MAX];
+ char head[] = "R:";
+ bool sortie = false;
+
+ do {
+ // Saut des lignes commencant par #
+ if (fgets (line, FILENAME_MAX, fd) != NULL) {
+ if ( strncmp (line, "#", 1) != 0) {
+ // La ligne n'est pas un commentaire
+ if ( strncmp (line, head, sizeof(head)-1) == 0) {
+ sortie = true; // Position robot trouvee.
+ }
+ // else
+ // return (false); // fin fichier sans position robot.
+ }
+ }
+ else {
+ fclose(fd) ;
+ return (false); /* fin fichier */
+ }
+ }
+ while ( sortie != true );
+
+ // Lecture des positions
+ q.resize(njoint);
+ int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
+ dummy,
+ &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
+ if (ret != 7) {
+ fclose(fd) ;
+ return false;
+ }
+
+ // converts rotations from degrees into radians
+ q.deg2rad();
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+ Save joint (articular) positions in a specific Viper850 position file.
+
+ This position file starts with a header on the first line. After
+ convertion of the rotations in degrees, the joint position \e q is
+ written on a line starting with the keyword "R: ". See readPosFile()
+ documentation for an example of such a file.
+
+ \param filename : Name of the position file to create.
+
+ \param q : The six joint positions to save in the
+ filename. Values are expressed in radians.
+
+ \warning All the six joint rotations written in the file are converted
+ in degrees to be more representative for the user.
+
+ \return true if the positions were successfully saved in the file. false, if
+ an error occurs.
+
+ \sa readPosFile()
+*/
+bool
+vpSimulatorViper850::savePosFile(const char *filename, const vpColVector &q)
+{
+
+ FILE * fd ;
+ fd = fopen(filename, "w") ;
+ if (fd == NULL)
+ return false;
+
+ fprintf(fd, "\
+#Viper - Position - Version 1.0\n\
+#\n\
+# R: A B C D E F\n\
+# Joint position in degrees\n\
+#\n\
+#\n\n");
+
+ // Save positions in mm and deg
+ fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
+ vpMath::deg(q[0]),
+ vpMath::deg(q[1]),
+ vpMath::deg(q[2]),
+ vpMath::deg(q[3]),
+ vpMath::deg(q[4]),
+ vpMath::deg(q[5]));
+
+ fclose(fd) ;
+ return (true);
+}
+
+/*!
+ Moves the robot to the joint position specified in the filename.
+
+ \param filename: File containing a joint position.
+
+ \sa readPosFile
+*/
+void
+vpSimulatorViper850::move(const char *filename)
+{
+ vpColVector q;
+
+ try
+ {
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
+ }
+ catch(...)
+ {
+ throw;
+ }
+}
+
+/*!
+ Get the geometric transformation \f$^c{\bf M}_e\f$ between the
+ camera frame and the end-effector frame. This transformation is
+ constant and correspond to the extrinsic camera parameters estimated
+ by calibration.
+
+ \param cMe : Transformation between the camera frame and the
+ end-effector frame.
+*/
+void
+vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe)
+{
+ vpViper850::get_cMe(cMe) ;
+}
+
+/*!
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
+ end-effector frame. This transformation allows to compute a
+ velocity expressed in the end-effector frame into the camera frame.
+
+ \param cVe : Twist transformation.
+*/
+void
+vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
+{
+ vpHomogeneousMatrix cMe ;
+ vpViper850::get_cMe(cMe) ;
+
+ cVe.buildFrom(cMe) ;
+}
+
+/*!
+ Get the robot jacobian expressed in the end-effector frame.
+
+ To compute \f$^e{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
+ end-effector frame.
+*/
+void
+vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
+{
+ try
+ {
+ vpViper850::get_eJe(get_artCoord(), eJe_) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ throw ;
+ }
+}
+
+/*!
+ Get the robot jacobian expressed in the robot reference frame also
+ called fix frame.
+
+ To compute \f$^f{\bf J}_e\f$, we communicate with the low level
+ controller to get the joint position of the robot.
+
+ \param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
+ reference frame.
+*/
+void
+vpSimulatorViper850::get_fJe(vpMatrix &fJe_)
+{
+ try
+ {
+ vpColVector articularCoordinates = get_artCoord();
+ vpViper850::get_fJe(articularCoordinates, fJe_) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught");
+ throw ;
+ }
+}
+
+/*!
+ Stop the robot.
+*/
+void
+vpSimulatorViper850::stopMotion()
+{
+ if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
+ return;
+
+ vpColVector stop(6);
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpRobot::setRobotState (vpRobot::STATE_STOP);
+}
+
+
+
+/**********************************************************************************/
+/**********************************************************************************/
+/**********************************************************************************/
+/**********************************************************************************/
+
+/*!
+ Initialise the display of the robot's arms.
+
+ Set the path to the scene files (*.bnd and *.sln) used by the
+ simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
+ not valid, the path is set from the VISP_SCENES_DIR environment
+ variable that the user has to set.
+*/
+void
+vpSimulatorViper850::initArms()
+{
+ // set scene_dir from #define VISP_SCENE_DIR if it exists
+ // VISP_SCENES_DIR may contain multiple locations separated by ";"
+ std::string scene_dir_;
+ std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
+ bool sceneDirExists = false;
+ for(size_t i=0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir_ = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (! sceneDirExists) {
+ try {
+ scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
+ std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
+ }
+ catch (...) {
+ std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
+ }
+ }
+
+ unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
+ if (scene_dir_.size() > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+
+ unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
+ if (full_length > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+
+ char *name_cam = new char [full_length];
+
+ strcpy(name_cam, scene_dir_.c_str());
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+
+ if (arm_dir.size() > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+ full_length = (unsigned int)arm_dir.size() + name_length;
+ if (full_length > FILENAME_MAX)
+ throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+
+ char *name_arm = new char [full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm1.bnd");
+ set_scene(name_arm, robotArms, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm2.bnd");
+ set_scene(name_arm, robotArms+1, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm3.bnd");
+ set_scene(name_arm, robotArms+2, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm4.bnd");
+ set_scene(name_arm, robotArms+3, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm5.bnd");
+ set_scene(name_arm, robotArms+4, 1.0);
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm,"/viper850_arm6.bnd");
+ set_scene(name_arm, robotArms+5, 1.0);
+
+// set_scene("./arm2.bnd", robotArms+1, 1.0);
+// set_scene("./arm3.bnd", robotArms+2, 1.0);
+// set_scene("./arm4.bnd", robotArms+3, 1.0);
+// set_scene("./arm5.bnd", robotArms+4, 1.0);
+// set_scene("./arm6.bnd", robotArms+5, 1.0);
+
+ add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+//
+// sceneInitialized = true;
+// displayObject = true;
+ displayCamera = true;
+
+ delete [] name_cam;
+ delete [] name_arm;
+}
+
+
+void
+vpSimulatorViper850::getExternalImage(vpImage<vpRGBa> &I_)
+{
+ bool changed = false;
+ vpHomogeneousMatrix displacement = navigation(I_,changed);
+
+ //if (displacement[2][3] != 0)
+ if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
+ camMf2 = camMf2*displacement;
+
+ f2Mf = camMf2.inverse()*camMf;
+
+ camMf = camMf2* displacement * f2Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I_.getWidth()/(2*px_ext);
+ v = (double)I_.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ }
+
+ float w44o[4][4],w44cext[4][4],x,y,z;
+
+ vp2jlc_matrix(camMf.inverse(),w44cext);
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+
+ vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
+ display_scene(w44o,robotArms[0],I_, curColor);
+
+ vp2jlc_matrix(fMit[0],w44o);
+ display_scene(w44o,robotArms[1],I_, curColor);
+
+ vp2jlc_matrix(fMit[1],w44o);
+ display_scene(w44o,robotArms[2],I_, curColor);
+
+ vp2jlc_matrix(fMit[2],w44o);
+ display_scene(w44o,robotArms[3],I_, curColor);
+
+ vp2jlc_matrix(fMit[3],w44o);
+ display_scene(w44o,robotArms[4],I_, curColor);
+
+ vp2jlc_matrix(fMit[6],w44o);
+ display_scene(w44o,robotArms[5],I_, curColor);
+
+ if (displayCamera)
+ {
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
+ cMe = cMe.inverse();
+ cMe = fMit[6] * cMe;
+ vp2jlc_matrix(cMe,w44o);
+ display_scene(w44o,camera, I_, camColor);
+ }
+
+ if (displayObject)
+ {
+ vp2jlc_matrix(fMo,w44o);
+ display_scene(w44o,scene,I_, curColor);
+ }
+}
+
+/*!
+ This method enables to initialise the joint coordinates of the robot in order to
+ position the camera relative to the object.
+
+ Before using this method it is advised to set the position of the object thanks
+ to the set_fMo() method.
+
+ In other terms, set the world to camera transformation
+ \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}{_o})^{-1}\f$, and from the inverse kinematics
+ set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
+
+ \param cMo_ : the desired pose of the camera.
+
+ \return false if the robot kinematics is not able to reach the cMo position.
+*/
+bool
+vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
+{
+ vpColVector stop(6);
+ bool status = true;
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpHomogeneousMatrix fMc_;
+ fMc_ = fMo * cMo_.inverse();
+
+ vpColVector articularCoordinates = get_artCoord();
+ unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
+
+ if (nbSol == 0) {
+ status = false;
+ vpERROR_TRACE ("Positionning error. Position unreachable");
+ }
+
+ if (verbose_)
+ std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
+
+ set_artCoord(articularCoordinates);
+
+ compute_fMi();
+
+ return status;
+}
+
+/*!
+ This method enables to initialise the pose between the object and the reference frame,
+ in order to position the object relative to the camera.
+
+ Before using this method it is advised to set the articular coordinates of the robot.
+
+ In other terms, set the world to object transformation
+ \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
+ with \f${\bf q}\f$ the robot joint position
+
+ \param cMo_ : the desired pose of the camera.
+*/
+void
+vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
+{
+ vpColVector stop(6);
+ stop = 0;
+ set_artVel(stop);
+ set_velocity(stop);
+ vpHomogeneousMatrix fMit[8];
+ get_fMi(fMit);
+ fMo = fMit[7] * cMo_;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o) has no symbols
+void dummy_vpSimulatorViper850() {};
+#endif
diff --git a/modules/robot/src/vpRobot.cpp b/modules/robot/src/vpRobot.cpp
new file mode 100644
index 0000000..ab60f95
--- /dev/null
+++ b/modules/robot/src/vpRobot.cpp
@@ -0,0 +1,279 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic virtual robot.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/core/vpDebug.h>
+
+
+const double vpRobot::maxTranslationVelocityDefault = 0.2;
+const double vpRobot::maxRotationVelocityDefault = 0.7;
+
+/* ------------------------------------------------------------------------- */
+/* --- CONSTRUCTEUR -------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+vpRobot::vpRobot (void)
+ :
+ stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
+ maxTranslationVelocity (maxTranslationVelocityDefault),
+ maxRotationVelocity (maxRotationVelocityDefault),
+ nDof(0),
+ eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
+ qmin(NULL), qmax(NULL), verbose_(true)
+{
+}
+
+vpRobot::vpRobot (const vpRobot &robot)
+ :
+ stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
+ maxTranslationVelocity (maxTranslationVelocityDefault),
+ maxRotationVelocity (maxRotationVelocityDefault),
+ nDof(0),
+ eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
+ qmin(NULL), qmax(NULL), verbose_(true)
+{
+ *this = robot;
+}
+
+/*!
+ Destructor that free allocated memory.
+ */
+vpRobot::~vpRobot()
+{
+ if (qmin != NULL) {
+ delete [] qmin;
+ qmin = NULL;
+ }
+ if (qmax != NULL) {
+ delete [] qmax;
+ qmax = NULL;
+ }
+}
+
+/*! Copy operator. */
+vpRobot & vpRobot::operator=(const vpRobot &robot)
+{
+ stateRobot = robot.stateRobot;
+ frameRobot = robot.frameRobot;
+ maxTranslationVelocity = robot.maxTranslationVelocity;
+ maxRotationVelocity = robot.maxRotationVelocity;
+ nDof = robot.nDof;
+ eJe = robot.eJe;
+ eJeAvailable = robot.eJeAvailable;
+ fJe= robot.fJe;
+ fJeAvailable = robot.fJeAvailable;
+ areJointLimitsAvailable = robot.areJointLimitsAvailable;
+ qmin = new double [nDof];
+ qmax = new double [nDof];
+ for (int i = 0; i< nDof; i++) {
+ qmin[i] = robot.qmin[i];
+ qmax[i] = robot.qmax[i];
+ }
+ verbose_ = robot.verbose_;
+
+ return (*this);
+}
+/*!
+ Saturate velocities.
+
+ \param v_in : Vector of input velocities to saturate. Translation velocities should
+ be expressed in m/s while rotation velocities in rad/s.
+
+ \param v_max : Vector of maximal allowed velocities. Maximal translation velocities
+ should be expressed in m/s while maximal rotation velocities in rad/s.
+
+ \param verbose : Print a message indicating which axis causes the saturation.
+
+ \return Saturated velocities.
+
+ \exception vpRobotException::dimensionError : If the input vectors have different dimensions.
+
+ The code below shows how to use this static method in order to saturate a velocity skew vector.
+
+ \code
+#include <iostream>
+
+#include <visp3/robot/vpRobot.h>
+
+int main()
+{
+ // Set a velocity skew vector
+ vpColVector v(6);
+ v[0] = 0.1; // vx in m/s
+ v[1] = 0.2; // vy
+ v[2] = 0.3; // vz
+ v[3] = vpMath::rad(10); // wx in rad/s
+ v[4] = vpMath::rad(-10); // wy
+ v[5] = vpMath::rad(20); // wz
+
+ // Set the maximal allowed velocities
+ vpColVector v_max(6);
+ for (int i=0; i<3; i++)
+ v_max[i] = 0.3; // in translation (m/s)
+ for (int i=3; i<6; i++)
+ v_max[i] = vpMath::rad(10); // in rotation (rad/s)
+
+ // Compute the saturated velocity skew vector
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+
+ std::cout << "v : " << v.t() << std::endl;
+ std::cout << "v max: " << v_max.t() << std::endl;
+ std::cout << "v sat: " << v_sat.t() << std::endl;
+
+ return 0;
+}
+ \endcode
+ */
+vpColVector
+vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose)
+{
+ unsigned int size = v_in.size();
+ if (size != v_max.size())
+ throw vpRobotException (vpRobotException::dimensionError, "Velocity vectors should have the same dimension");
+
+ double scale = 1; // global scale factor to saturate all the axis
+ for (unsigned int i = 0; i < size; i++)
+ {
+ double v_i = fabs(v_in[i]);
+ double v_max_i = fabs(v_max[i]);
+ if ( v_i > v_max_i ) // Test if we should saturate the axis
+ {
+ double scale_i = v_max_i/v_i;
+ if (scale_i < scale)
+ scale = scale_i;
+
+ if (verbose)
+ std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl;
+ }
+ }
+
+ vpColVector v_sat(size);
+ v_sat = v_in * scale;
+
+ return v_sat;
+}
+
+
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ \file vpRobot.cpp
+ \brief class that defines a generic virtual robot
+*/
+vpRobot::vpRobotStateType
+vpRobot::setRobotState (const vpRobot::vpRobotStateType newState)
+{
+ stateRobot = newState ;
+ return newState ;
+}
+
+vpRobot::vpControlFrameType
+vpRobot::setRobotFrame (vpRobot::vpControlFrameType newFrame)
+{
+ frameRobot = newFrame ;
+ return newFrame ;
+}
+
+/*!
+ Return the current robot position in the specified frame.
+*/
+vpColVector
+vpRobot::getPosition (vpRobot::vpControlFrameType frame)
+{
+ vpColVector r;
+ this ->getPosition (frame, r);
+
+ return r;
+}
+
+/* ------------------------------------------------------------------------- */
+/* --- VELOCITY CONTROL ---------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+
+ Set the maximal translation velocity that can be sent to the robot during a velocity control.
+
+ \param v_max : Maximum translation velocity expressed in m/s.
+
+*/
+void
+vpRobot::setMaxTranslationVelocity (const double v_max)
+{
+ this ->maxTranslationVelocity = v_max;
+ return;
+}
+
+/*!
+ Get the maximal translation velocity that can be sent to the robot during a velocity control.
+
+ \return Maximum translation velocity expressed in m/s.
+*/
+double
+vpRobot::getMaxTranslationVelocity (void) const
+{
+ return this ->maxTranslationVelocity;
+}
+/*!
+
+ Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+
+ \param w_max : Maximum rotation velocity expressed in rad/s.
+*/
+
+void
+vpRobot::setMaxRotationVelocity (const double w_max)
+{
+ this ->maxRotationVelocity = w_max;
+ return;
+}
+
+/*!
+
+ Get the maximal rotation velocity that can be sent to the robot during a velocity control.
+
+ \return Maximum rotation velocity expressed in rad/s.
+*/
+double
+vpRobot::getMaxRotationVelocity (void) const
+{
+ return this ->maxRotationVelocity;
+}
+
diff --git a/modules/robot/src/vpRobotTemplate.cpp b/modules/robot/src/vpRobotTemplate.cpp
new file mode 100644
index 0000000..26dd645
--- /dev/null
+++ b/modules/robot/src/vpRobotTemplate.cpp
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Defines a robot just to show which function you must implement.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpRobotTemplate.cpp
+ \brief class that defines a robot just to show which function you must implement
+*/
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotTemplate.h>
+#include <visp3/core/vpDebug.h>
+
+//! basic initialization
+void vpRobotTemplate::init()
+{
+ vpTRACE(" Get the joint limits " ) ;
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+//! constructor
+vpRobotTemplate::vpRobotTemplate()
+{
+ init() ;
+}
+
+
+//! constructor
+vpRobotTemplate::~vpRobotTemplate()
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+/*
+
+AT LEAST ONE OF THESE TWO FUNCTIONS HAS TO BE IMPLEMENTED
+
+get_eJe
+get_fJe
+
+*/
+
+//! get the robot Jacobian expressed in the end-effector frame
+void
+vpRobotTemplate::get_eJe(vpMatrix &/* _eJe */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+//! get the robot Jacobian expressed in the robot reference frame
+void
+vpRobotTemplate::get_fJe(vpMatrix &/* _fJe */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+
+/*
+
+AT LEAST ONE OF THESE TWO FUNCTIONS HAS TO BE IMPLEMENTED
+
+sendCameraVelocity
+sendArticularVelocity
+
+
+*/
+//! send to the controller a velocity expressed in the camera frame
+void vpRobotTemplate::sendCameraVelocity(const vpColVector &/* v */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+ std::cout << "To implement me you need : " << std::endl ;
+ std::cout << "\t to known the robot jacobian expressed in " ;
+ std::cout << "the end-effector frame (eJe) " <<std::endl ;
+ std::cout << "\t the frame transformation between camera frame " ;
+ std::cout << "and end-effector frame (cMe)" << std::endl ;
+}
+
+//! send to the controller a velocity expressed in the articular frame
+void
+vpRobotTemplate::sendArticularVelocity(const vpColVector &/* qdot */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+//! send to the controller a velocity (frame as to ve specified)
+void
+vpRobotTemplate::setVelocity(const vpRobot::vpControlFrameType /* frame */,
+ const vpColVector &/* vel */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+
+/*
+
+THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
+
+*/
+
+//! get a position expressed in the robot reference frame
+void
+vpRobotTemplate::getPosition(vpPoseVector &/* q */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+//! get a position expressed in the articular frame
+void
+vpRobotTemplate::getArticularPosition(vpColVector &/* q */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+//! get a displacement (frame as to ve specified)
+void
+vpRobotTemplate::getPosition( const vpRobot::vpControlFrameType/*frame*/,
+ vpColVector &/* q */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+//! set a displacement (frame as to ve specified)
+void
+vpRobotTemplate::setPosition( const vpRobot::vpControlFrameType/*frame*/,
+ const vpColVector &/* q */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+//! get a displacement (frame as to ve specified)
+void
+vpRobotTemplate::getDisplacement(const vpRobot::vpControlFrameType /* frame */,
+ vpColVector &/* q */)
+{
+ std::cout << "Not implemented ! " << std::endl;
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/robot/src/wireframe-simulator/core/vpArit.cpp b/modules/robot/src/wireframe-simulator/core/vpArit.cpp
new file mode 100644
index 0000000..8c2a004
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpArit.cpp
@@ -0,0 +1,810 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "arit.c" contient les procedures arithmetiques.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/core/vpMath.h>
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+/*
+ * La procedure "fprintf_matrix" affiche une matrice sur un fichier.
+ * Entree :
+ * fp Fichier en sortie.
+ * m Matrice a ecrire.
+ */
+void
+fprintf_matrix (FILE *fp, Matrix m)
+{
+ int i;
+
+ fprintf (fp, "(matrix\n");
+ for (i = 0; i < 4; i++)
+ fprintf (fp, "\t%.4f\t%.4f\t%.4f\t%.4f\n",
+ m[i][0], m[i][1], m[i][2], m[i][3]);
+ fprintf (fp, ")\n");
+}
+
+/*
+ * La procedure "ident_matrix" initialise la matrice par la matrice identite.
+ * Entree :
+ * m Matrice a initialiser.
+ */
+void
+ident_matrix (Matrix m)
+{
+ static Matrix identity = IDENTITY_MATRIX;
+
+ //bcopy ((char *) identity, (char *) m, sizeof (Matrix));
+ memmove ((char *) m, (char *) identity, sizeof (Matrix));
+/*
+ * Version moins rapide.
+ *
+ * int i, j;
+ *
+ * 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.
+ */
+void
+premult_matrix (Matrix a, Matrix b)
+{
+ Matrix m;
+ int i, j;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ m[i][j] = b[i][0] * a[0][j] +
+ b[i][1] * a[1][j] +
+ b[i][2] * a[2][j] +
+ b[i][3] * a[3][j];
+ //bcopy ((char *) m, (char *) a, sizeof (Matrix));
+ memmove ((char *) a, (char *) m, sizeof (Matrix));
+}
+
+/*
+ * 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.
+ */
+void
+premult3_matrix (Matrix a, Matrix b)
+{
+ Matrix m;
+ int i, j;
+
+ //bcopy ((char *) a, (char *) m, sizeof (Matrix));
+ memmove ((char *) m, (char *) a, sizeof (Matrix));
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 4; j++)
+ a[i][j] = b[i][0] * m[0][j] +
+ b[i][1] * m[1][j] +
+ b[i][2] * m[2][j];
+}
+
+/*
+ * La procedure "prescale_matrix" premultiplie la matrice par l'homothetie.
+ * Entree :
+ * m Matrice a multiplier m = vp * m.
+ * vp Vecteur d'homothetie.
+ */
+void
+prescale_matrix (Matrix m, Vector *vp)
+{
+ int i;
+
+ for (i = 0; i < 4; i++) {
+ m[0][i] *= vp->x;
+ m[1][i] *= vp->y;
+ m[2][i] *= vp->z;
+ }
+}
+
+/*
+ * La procedure "pretrans_matrix" premultiplie la matrice par la translation.
+ * Entree :
+ * m Matrice a multiplier m = vp * m.
+ * vp Vecteur de translation.
+ */
+void
+pretrans_matrix (Matrix m, Vector *vp)
+{
+ int i;
+
+ for (i = 0; i < 4; i++)
+ m[3][i] += vp->x * m[0][i] + vp->y * m[1][i] + vp->z * m[2][i];
+}
+
+/*
+ * 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'.
+ */
+void
+postleft_matrix (Matrix m, char axis)
+{
+ static char proc_name[] = "postleft_matrix";
+
+ int i;
+
+ switch (axis) {
+ case 'x' :
+ for (i = 0; i < 4; i++) m[i][0] = - m[i][0];
+ break;
+ case 'y' :
+ for (i = 0; i < 4; i++) m[i][1] = - m[i][1];
+ break;
+ case 'z' :
+ for (i = 0; i < 4; i++) m[i][2] = - m[i][2];
+ break;
+ default :
+ fprintf (stderr, "%s: axis unknown\n", proc_name);
+ break;
+ }
+}
+
+/*
+ * 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.
+ */
+void
+postmult_matrix (Matrix a, Matrix b)
+{
+ Matrix m;
+ int i, j;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ m[i][j] = a[i][0] * b[0][j] +
+ a[i][1] * b[1][j] +
+ a[i][2] * b[2][j] +
+ a[i][3] * b[3][j];
+ //bcopy ((char *) m, (char *) a, sizeof (Matrix));
+ memmove ((char *) a, (char *) m, sizeof (Matrix));
+}
+
+/*
+ * 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.
+ */
+void
+postmult3_matrix (Matrix a, Matrix b)
+{
+ Matrix m;
+ int i, j;
+
+ //bcopy ((char *) a, (char *) m, sizeof (Matrix));
+ memmove ((char *) m, (char *) a, sizeof (Matrix));
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 3; j++)
+ a[i][j] = m[i][0] * b[0][j] +
+ m[i][1] * b[1][j] +
+ m[i][2] * b[2][j];
+}
+
+/*
+ * La procedure "postscale_matrix" post multiplie la matrice par l'homothetie.
+ * Entree :
+ * m Matrice a multiplier m = m * vp.
+ * vp Vecteur d'homothetie.
+ */
+void
+postscale_matrix (Matrix m, Vector *vp)
+{
+ int i;
+
+ for (i = 0; i < 4; i++) {
+ m[i][0] *= vp->x;
+ m[i][1] *= vp->y;
+ m[i][2] *= vp->z;
+ }
+}
+
+/*
+ * La procedure "posttrans_matrix" post mutiplie la matrice par la translation.
+ * Entree :
+ * m Matrice a multiplier m = m * vp.
+ * vp Vecteur de translation.
+ */
+void
+posttrans_matrix (Matrix m, Vector *vp)
+{
+ int i;
+
+ for (i = 0; i < 4; i++) {
+ m[i][0] += m[i][3] * vp->x;
+ m[i][1] += m[i][3] * vp->y;
+ m[i][2] += m[i][3] * vp->z;
+ }
+}
+
+/*
+ * La procedure "transpose_matrix" transpose la matrice.
+ * Entree :
+ * m Matrice a transposer.
+ */
+void
+transpose_matrix (Matrix m)
+{
+ unsigned int i, j;
+ float t;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < i; j++)
+ SWAP(m[i][j],m[j][i],t);
+}
+
+/*
+ * 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.
+ * Sortie :
+ * Angle en radians.
+ */
+float
+cosin_to_angle (float ca, float sa)
+{
+ float a; /* angle a calculer */
+
+ if (FABS(ca) < M_EPSILON) {
+ a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2);
+ }
+ else {
+ a = (float) atan((double) (sa / ca));
+ if (ca < (float)0.0) a += (sa > (float)0.0) ? (float)M_PI : (float)(- M_PI);
+ }
+ return (a);
+}
+
+/*
+ * 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".
+ */
+void
+cosin_to_lut (Index level, float *coslut, float *sinlut)
+{
+ int i;
+ int i_pi_2 = TWO_POWER(level);
+ int quad; /* quadrant courant */
+ float ca; /* cosinus 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 */
+ quad += i_pi_2;
+ coslut[quad] = 0.0; sinlut[quad] = 1.0; /* PI/2 */
+ quad += i_pi_2;
+ coslut[quad] = -1.0; sinlut[quad] = 0.0; /* PI */
+ quad += i_pi_2;
+ coslut[quad] = 0.0; sinlut[quad] = -1.0; /* 3PI/2*/
+
+ for (i = 1, a = step; i < i_pi_2; i++, a += step) {
+ ca = (float) cos (a);
+ quad = 0;
+ 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) */
+ quad += i_pi_2;
+ 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) */
+ quad += i_pi_2;
+ coslut[quad - i] = ca; /* cos(2PI-a) */
+ }
+}
+
+/*
+ * La procedure "norm_vector" normalise le vecteur.
+ * Si la norme est nulle la normalisation n'est pas effectuee.
+ * Entree :
+ * vp Le vecteur a norme.
+ * Sortie :
+ * La norme du vecteur.
+ */
+float
+norm_vector (Vector *vp)
+{
+ static char proc_name[] = "norm_vector";
+
+ float norm; /* norme du vecteur */
+
+ if ((norm = (float) sqrt ((double) DOT_PRODUCT(*vp,*vp))) > M_EPSILON) {
+ vp->x /= norm;
+ vp->y /= norm;
+ vp->z /= norm;
+ }
+ else fprintf (stderr, "%s: nul vector\n", proc_name);
+ return (norm);
+}
+
+/*
+ * 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.
+ */
+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(v,*cp,*ap);
+ norm_vector (&u);
+ norm_vector (&v);
+ CROSS_PRODUCT (*np,u,v);
+}
+
+/*
+ * 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.
+ */
+void
+point_matrix (Point4f *p4, Point3f *p3, Matrix m)
+{
+ float x = p3->x, y = p3->y, z = p3->z;
+
+ p4->x = COORD3_COL(x,y,z,m,0);
+ p4->y = COORD3_COL(x,y,z,m,1);
+ p4->z = COORD3_COL(x,y,z,m,2);
+ p4->w = COORD3_COL(x,y,z,m,3);
+}
+
+/*
+ * 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 resultat.
+ */
+void
+point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op)
+{
+ Point3f *pend = ip + size; /* borne de ip */
+ float x, y, z; /* [xyz] = *ip */
+
+ for (; ip < pend; ip++, op++) {
+ x = ip->x;
+ y = ip->y;
+ z = ip->z;
+
+ op->x = COORD3_COL(x,y,z,m,0);
+ op->y = COORD3_COL(x,y,z,m,1);
+ op->z = COORD3_COL(x,y,z,m,2);
+ }
+}
+
+/*
+ * 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 4D resultat.
+ */
+void
+point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4)
+{
+ Point3f *pend = p3 + size; /* borne de p3 */
+ float x, y, z; /* [xyz] = *p3 */
+
+ for (; p3 < pend; p3++, p4++) {
+ x = p3->x;
+ y = p3->y;
+ z = p3->z;
+
+ p4->x = COORD3_COL(x,y,z,m,0);
+ p4->y = COORD3_COL(x,y,z,m,1);
+ p4->z = COORD3_COL(x,y,z,m,2);
+ p4->w = COORD3_COL(x,y,z,m,3);
+ }
+}
+
+/*
+ * 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.
+ */
+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 */
+
+ n = *axis; /* norme le vecteur directeur */
+ norm_vector (&n);
+
+ /*
+ * Avant rotation, vp vaut :
+ * u + v
+ * Apres rotation, vp vaut :
+ * u + cos(a) * v + sin(a) * (n^vp)
+ * = u + cos(a) * v + sin(a) * (n^v)
+ * avec u = (vp.n) * n, v = vp-u;
+ * ou "u" est la projection de "vp" sur l'axe "axis",
+ * et "v" est la composante de "vp" perpendiculaire a "axis".
+ */
+ f = DOT_PRODUCT(*vp, n);
+ u = n;
+ MUL_COORD3(u,f,f,f); /* (vp.n) * n */
+
+ DIF_COORD3(v,*vp,u); /* calcule "v" */
+
+ f = (float) cos ((double) 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) */
+
+ SET_COORD3(*vp,
+ u.x + v.x + cross.x,
+ u.y + v.y + cross.y,
+ u.z + v.z + cross.z);
+}
+
+/*
+ * La procedure "upright_vector" calcule un vecteur perpendiculaire.
+ * Les vecteurs ont un produit scalaire nul.
+ * Entree :
+ * vp Vecteur origine.
+ * Entree/Sortie :
+ * up Vecteur perpendiculaire a vp.
+ */
+void
+upright_vector (Vector *vp, Vector *up)
+{
+ static char proc_name[] = "upright_vector";
+
+ 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 */
+ 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 */
+ up->x = - (vp->y + vp->z) / vp->x;
+ up->y = up->z = 1.0;
+ }
+ else {
+ up->x = up->y = up->z = 0.0;
+ fprintf (stderr, "%s: nul vector\n", proc_name);
+ return;
+ }
+}
+
+/*
+ * La procedure "Matrix_to_Position" initialise la position par la matrice.
+ * 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.
+ */
+void
+Matrix_to_Position (Matrix m, AritPosition *pp)
+{
+ Matrix_to_Rotate (m, &pp->rotate);
+ SET_COORD3(pp->scale,1.0,1.0,1.0);
+ SET_COORD3(pp->translate,m[3][0],m[3][1],m[3][2]);
+}
+
+/*
+ * 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 |
+ *
+ * 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
+ * Ry rotation autour de Oy d'angle O2
+ * 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".
+ *
+ * Entree :
+ * 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)
+{
+ float cx, sx; /* cosinus et sinus de la rotation sur l'axe X */
+ float cy, sy; /* cosinus et sinus de la rotation sur l'axe Y */
+ float cz, sz; /* cosinus et sinus de la rotation sur l'axe Z */
+
+ sy = - m[0][2];
+ cy = (float) sqrt (1.0 - (double) (sy * sy));
+
+ if (FABS(cy) > M_EPSILON) {
+ sz = m[0][1] / cy;
+ cz = m[0][0] / cy;
+
+ sx = m[1][2] / cy;
+ 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 */
+ 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->y *= (float)180.0 / (float)M_PI;
+ vp->z *= (float)180.0 / (float)M_PI;
+}
+
+/*
+ * 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.
+ */
+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 */
+ m[3][1] = pp->translate.y;
+ m[3][2] = pp->translate.z;
+}
+
+/*
+ * 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 |
+ *
+ * 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.
+ *
+ * Entree :
+ * 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 */
+ 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);
+
+ m[0][0] = cy * cz;
+ m[1][0] = (sx * sy * cz) - (cx * sz);
+ m[2][0] = (cx * sy * cz) + (sx * sz);
+
+ m[0][1] = cy * sz;
+ m[1][1] = (sx * sy * sz) + (cx * cz);
+ m[2][1] = (cx * sy * sz) - (sx * cz);
+
+ m[0][2] = - sy;
+ m[1][2] = sx * cy;
+ m[2][2] = cx * cy;
+
+ m[0][3] = m[1][3] = m[2][3] = 0.0;
+ m[3][0] = m[3][1] = m[3][2] = 0.0;
+ m[3][3] = 1.0;
+}
+
+/*
+ * La procedure "Rotaxis_to_Matrix" initialise la matrice par la rotation
+ * 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~
+ *
+ * | 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 |
+ * Entree :
+ * 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 */
+ Vector n; /* vecteur norme*/
+ Vector conv; /* verO n */
+ Vector tilde; /* sinO n */
+
+ 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 */
+ norm_vector (&n);
+ tilde = conv = n;
+ MUL_COORD3(conv,vera,vera,vera);
+ MUL_COORD3(tilde,sina,sina,sina);
+
+ m[0][0] = conv.x * n.x + cosa;
+ m[0][1] = conv.x * n.y + tilde.z;
+ m[0][2] = conv.x * n.z - tilde.y;
+
+ m[1][0] = conv.y * n.x - tilde.z;
+ m[1][1] = conv.y * n.y + cosa;
+ m[1][2] = conv.y * n.z + tilde.x;
+
+ m[2][0] = conv.z * n.x + tilde.y;
+ m[2][1] = conv.z * n.y - tilde.x;
+ m[2][2] = conv.z * n.z + cosa;
+
+ m[0][3] = m[2][3] = m[1][3] = 0.0;
+ m[3][0] = m[3][1] = m[3][2] = 0.0;
+ m[3][3] = 1.0;
+}
+
+/*
+ * 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.
+ */
+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 */
+ m[3][1] = tp->y;
+ m[3][2] = tp->z;
+}
+
+/*
+ * La procedure "Scale_to_Matrix" initialise la matrice par l'homothetie.
+ * Entree :
+ * vp Vecteur des coordonnees d'homothetie.
+ * m Matrice a initialiser.
+ */
+void
+Scale_to_Matrix (Vector *vp, Matrix m)
+{
+ ident_matrix (m);
+ m[0][0] = vp->x;
+ m[1][1] = vp->y;
+ m[2][2] = vp->z;
+}
+
+/*
+ * La procedure "Translate_to_Matrix" initialise la matrice par la translation.
+ * Entree :
+ * vp Vecteur des coordonnees de translation.
+ * m Matrice a initialiser.
+ */
+void
+Translate_to_Matrix (Vector *vp, Matrix m)
+{
+ ident_matrix (m);
+ m[3][0] = vp->x;
+ m[3][1] = vp->y;
+ m[3][2] = vp->z;
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpAritio.cpp b/modules/robot/src/wireframe-simulator/core/vpAritio.cpp
new file mode 100644
index 0000000..5ecbb39
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpAritio.cpp
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+#include "visp3/core/vpConfig.h"
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpLex.h>
+
+#include <stdio.h>
+/*
+ * La procedure "fprintf_Position" ecrit en ascii un positionnement.
+ * Entree :
+ * f Fichier en sortie.
+ * pp Positionnement a ecrite.
+ */
+void fprintf_Position (FILE *f, AritPosition *pp)
+{
+ fprintf (f, "%.3f\t%.3f\t%.3f\n%.3f\t%.3f\t%.3f\n%.3f\t%.3f\t%.3f\n",
+ pp->rotate.x, pp->rotate.y, pp->rotate.z,
+ pp->scale.x, pp->scale.y, pp->scale.z,
+ pp->translate.x, pp->translate.y, pp->translate.z);
+}
+
+/*
+ * La procedure "fscanf_Point3f" lit en ascii un point flottant 3D.
+ * Entree :
+ * 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. */
+
+ 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. */
+
+ 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. */
+
+ if ((t= lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
+ pp->z = (t == T_INT) ? (float) myint : myfloat;
+}
+
+/*
+ * La procedure "fscanf_Vector" lit en ascii un vecteur.
+ * Entree :
+ * vp Vecteur a lire.
+ */
+void fscanf_Vector (Vector *vp)
+{
+static const char *err_tbl[] = {
+"float expected (coordinate ",
+" of vector)"
+};
+
+ int t;
+
+ /* 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. */
+
+ 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. */
+
+ if ((t= lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
+ vp->z = (t == T_INT) ? (float) myint : myfloat;
+}
+
+/*
+ * La procedure "fscanf_Position" lit en ascii un positionnement.
+ * Entree :
+ * pp Positionnement a lire.
+ */
+void fscanf_Position (AritPosition *pp)
+{
+ pusherr ("rotate: ");
+ fscanf_Vector (&pp->rotate);
+ popuperr ("scale: ");
+ fscanf_Vector (&pp->scale);
+ popuperr ("translate: ");
+ fscanf_Vector (&pp->translate);
+ poperr ();
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpBound.cpp b/modules/robot/src/wireframe-simulator/core/vpBound.cpp
new file mode 100644
index 0000000..39fb314
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpBound.cpp
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le fichier "bound.c" contient les procedures de gestion des scenes de modele geometrique surfacique.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include "visp3/core/vpConfig.h"
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpBound.h>
+#include <errno.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+/*
+ * La procedure "free_Bound" libere la memoire d'une surface.
+ * Les champs "bound.face.edge" ne sont pas utilises.
+ * Entree :
+ * 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 */
+ if (fp->vertex.ptr != fp->vertex.tbl)
+ free ((char *) fp->vertex.ptr);
+ }
+ 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 */
+ free ((char *) bp->point.ptr);
+ bp->point.ptr = NULL;
+ }
+#ifdef face_normal
+ if (bp->normal.ptr != NULL) { /* libere les vecteurs */
+ free ((char *) bp->normal.ptr);
+ bp->normal.ptr = NULL;
+ }
+#endif /* face_normal */
+ bp->is_display = FALSE;
+}
+
+/*
+ * 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.
+ */
+void free_huge_Bound (Bound *bp)
+{
+ 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.
+ */
+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 */
+ free_Bound (bp);
+ }
+ if (bsp->name != NULL) { /* libere le nom */
+ free ((char *) bsp->name);
+ bsp->name = NULL;
+ }
+ if (bsp->bound.ptr != NULL) { /* libere le tableau */
+ free ((char *) bsp->bound.ptr);
+ bsp->bound.ptr = NULL;
+ }
+}
+
+/*
+ * 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.
+ */
+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 */
+ bp->face.ptr = NULL;
+ else if ((bp->face.ptr = (Face *) malloc (fn * sizeof (Face)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+
+ 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);
+ exit (1);
+ }
+
+#ifdef face_normal
+ /* 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) {
+ perror (proc_name);
+ exit (1);
+ }
+#endif /* face_normal */
+
+ bp->type = type;
+ bp->is_display = TRUE;
+ bp->is_polygonal = (unsigned)polygonal;
+}
+
+/*
+ * La procedure "malloc_huge_Bound" alloue une surface de taille maximale.
+ * 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.
+ * 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.
+ */
+void malloc_huge_Bound (Bound *bp)
+{
+ static char proc_name[] = "malloc_Huge_Bound";
+
+#ifdef face_normal
+ malloc_Bound (bp, (Type) BND_NULL, FALSE, FACE_NBR, POINT_NBR);
+#else
+ malloc_Bound (bp, (Type) BND_NULL, TRUE, FACE_NBR, POINT_NBR);
+#endif /* face_normal */
+ if ((bp->face.ptr->vertex.ptr =
+ (Index *) malloc (FACE_NBR * VERTEX_NBR * sizeof (Index))) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+}
+
+/*
+ * La procedure "malloc_Bound_scene" alloue une scene de surfaces.
+ * 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.
+ */
+void malloc_Bound_scene (Bound_scene *bsp, const char *name, Index bn)
+{
+ static char proc_name[] = "malloc_Bound_scene";
+
+ if ((bsp->name = (char *) malloc ((strlen (name) + 1) * sizeof (char)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ if ((bsp->bound.nbr = bn) == 0)
+ bsp->bound.ptr = NULL;
+ else if ((bsp->bound.ptr = (Bound *) malloc (bn * sizeof (Bound)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ strcpy (bsp->name, name);
+ bsp->bound.nbr = 0;
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpBoundio.cpp b/modules/robot/src/wireframe-simulator/core/vpBoundio.cpp
new file mode 100644
index 0000000..f90495e
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpBoundio.cpp
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "boundio.c" contient les procedures d'entree/sortie
+ * des types definis dans le module "bound.h".
+ * Les entrees non specifiees sont effectuees
+ * sur le fichier "source" de "lex.c".
+ * Pour les mots cles des "fprintf_..." voir "token.c".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+#include "visp3/core/vpConfig.h"
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpBound.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpSkipio.h>
+#include <visp3/robot/vpLex.h>
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+void fscanf_Point3f_list (Point3f_list *);
+void fscanf_Face_list (Face_list *);
+
+/*
+ * La procedure "fscanf_Bound" lit en ascii une surface.
+ * Entree :
+ * bp Surface a lire.
+ */
+void fscanf_Bound (Bound *bp)
+{
+ /* 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. */
+
+ 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. */
+
+ skip_keyword (T_FACE_LIST, "bound: keyword \"face_list\" expected");
+ pusherr ("bound_face_list: ");
+ fscanf_Face_list (&bp->face);
+ poperr ();
+}
+
+/*
+ * La procedure "fscanf_Face_list" lit en ascii une liste de faces.
+ * Entree :
+ * lp Liste de faces a lire.
+ */
+void fscanf_Face_list (Face_list *lp)
+{
+ static char proc_name[] = "fscanf_Face_list";
+
+ Face *fp; /* face courante */
+ Face *fend; /* borne de "fp" */
+
+
+ /* 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. */
+
+ if (lp->nbr == 0)
+ lp->ptr = NULL;
+ else if ((lp->ptr = (Face *) malloc (lp->nbr * sizeof (Face)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+
+ /* 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" */
+
+ /* 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. */
+
+ if (lex () != T_INT)
+ lexerr ("start", "integer expected (number of vertices)", NULL);
+ vlp->nbr = (Index) myint;
+
+ /* Allocation dynamique du polygone de la face. */
+
+ if (vlp->nbr <= DEFAULT_VSIZE)
+ vlp->ptr = vlp->tbl;
+ else if ((vlp->ptr = (Index *) malloc (vlp->nbr * sizeof (Index)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+
+ /* Lecture des sommets de la face un a un. */
+
+ vp = vlp->ptr;
+ vend = vp + vlp->nbr;
+ for (; vp < vend; *vp++ = (Index) myint)
+ if (lex () != T_INT)
+ lexerr ("start", "integer expected (index of points 3D)", NULL);
+ }
+}
+
+/*
+ * La procedure "fscanf_Point_list" lit en ascii une liste de points 3D.
+ * Entree :
+ * lp Liste de points a lire.
+ */
+void fscanf_Point3f_list (Point3f_list *lp)
+{
+ static const char proc_name[] = "fscanf_Point3f_list";
+
+static const char *err_tbl[] = {
+"float expected (coordinate ",
+" of point)"
+};
+ Point3f *pp; /* point courant */
+ Point3f *pend; /* borne de "pp" */
+
+ /* 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. */
+
+ if (lp->nbr == 0)
+ lp->ptr = NULL;
+ else if ((lp->ptr = (Point3f *) malloc (lp->nbr * sizeof (Point3f)))
+ == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+
+ /* Lecture des points de la liste un a un. */
+
+ pp = lp->ptr;
+ pend = pp + lp->nbr;
+ for (; pp < pend; pp++) {
+ int t;
+
+ if ((t = lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start", err_tbl[0], "X", err_tbl[1], NULL);
+/* FC printf("X %d %f\n",myint, myfloat); */
+ pp->x = (t == T_INT) ? (float) myint : myfloat;
+
+ if ((t = lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start", err_tbl[0], "Y", err_tbl[1], NULL);
+/* FC printf("Y %d %f\n",myint, myfloat); */
+ pp->y = (t == T_INT) ? (float) myint : myfloat;
+
+ if ((t = lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start", err_tbl[0], "Z", err_tbl[1], NULL);
+/* FC printf("Z %d %f\n",myint, myfloat); */
+ pp->z = (t == T_INT) ? (float) myint : myfloat;
+ }
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpClipping.cpp b/modules/robot/src/wireframe-simulator/core/vpClipping.cpp
new file mode 100644
index 0000000..61234b9
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpClipping.cpp
@@ -0,0 +1,486 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "clipping.c" contient les procedures de decoupage
+ * d'une scene 3D par l'algorithme de Sutherland et Hodgman.
+ * Pour plus de reseignements, voir :
+ * I. Sutherland, E. Hodgman, W. Gary.
+ * "Reentrant Polygon Clipping".
+ * Communications of the ACM,
+ * Junary 1974, Volume 17, Number 1, pp 32-44.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpBound.h>
+#include <visp3/robot/vpView.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits>
+#include <cmath>
+
+void open_clipping (void);
+void close_clipping (void);
+static Index clipping (Byte mask, Index vni, Index *pi, Index *po);
+static Index clipping_Face (Face *fi, Face *fo);
+static Index clipping_Face (Face *fi, Face *fo);
+static void inter (Byte mask, Index v0, Index v1);
+static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
+void set_Point4f_code (Point4f *p4, int size, Byte *cp);
+Byte where_is_Point4f (Point4f *p4);
+
+/*
+ * Variables utilisees par le decoupage :
+ *
+ * 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.
+ *
+ * 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 :
+ * 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.
+ * Le tableau recoit ensuite un a un les booleans des points 4D d'intersection
+ * de la surface avec les 6 plans de decoupage.
+ *
+ * POINT4F :
+ * Tableau de points durant le decoupage.
+ * Le tableau est initialise par les points de la surface en entree apres
+ * transforation en coordonnees homogenes 4D.
+ * 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 */
+
+#if clip_opt
+static Index *poly0, *poly1; /* polygones temporaires*/
+#else
+static Index *poly_tmp; /* polygone temporaire */
+#endif /* clip_opt */
+
+
+/*
+ * La procedure "open_clipping" alloue et initialise les variables utilisees
+ * par le mode "clipping".
+ */
+void open_clipping (void)
+{
+ static char proc_name[] = "open_clipping";
+
+ /* alloue la surface de travail */
+ malloc_huge_Bound (&clip);
+
+ /* alloue les tableaux */
+ if ((code = (Byte *) malloc (POINT_NBR * sizeof (Byte))) == NULL
+ || (point4f = (Point4f *) malloc (POINT_NBR * sizeof (Point4f))) == NULL
+#ifdef clip_opt
+ || (poly0 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL
+ || (poly1 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+#else
+ || (poly_tmp = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL){
+ perror (proc_name);
+ exit (1);
+ }
+#endif /* clip_opt */
+}
+
+/*
+ * La procedure "close_clipping" libere les variables utilisees par
+ * le mode "clipping".
+ */
+void close_clipping (void)
+{
+ free_huge_Bound (&clip);
+ free ((char *) code);
+ free ((char *) point4f);
+#ifdef clip_opt
+ free ((char *) poly0);
+ free ((char *) poly1);
+#else
+ free ((char *) poly_tmp);
+#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.
+ * Sortie :
+ * 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.
+ */
+ Index vno = vni; /* nombre de sommets */
+ Index vs = pi[vni-1]; /* premier sommet */
+ Index vp; /* second sommet */
+ Byte ins = code[vs] & mask; /* code de "vs" */
+ Byte inp; /* code de "vp" */
+
+ while (vni--) { /* pour tous les sommets */
+ vp = *pi++; /* second sommet */
+ inp = code[vp] & mask; /* code du plan courant */
+
+ if (ins == IS_INSIDE) {
+ if (inp == IS_INSIDE) { /* arete interieure */
+ *po++ = vp;
+ }
+ else { /* intersection */
+ inter (mask, vs, vp);
+ *po++ = point4f_nbr++;
+ }
+ }
+ else {
+ if (inp == IS_INSIDE) { /* intersection */
+ inter (mask, vs, vp);
+ *po++ = point4f_nbr++;
+ *po++ = vp;
+ vno++;
+ }
+ else { /* arete exterieure */
+ vno--;
+ }
+ }
+ vs = vp;
+ ins = inp;
+ }
+ return (vno);
+}
+
+
+/*
+ * 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.
+ * Sortie :
+ * 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 */
+
+ if ((vn = clipping (IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0)
+ if ((vn = clipping (IS_BELOW, vn, flip, flop)) != 0)
+ if ((vn = clipping (IS_RIGHT, vn, flop, flip)) != 0)
+ 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 */
+ 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 */
+ return (vn);
+ }
+ return (0);
+}
+
+/*
+ * La procedure "clipping_Bound" decoupe une surface par rapport aux 6 plans
+ * de decoupage de la pyramide tronquee de vision.
+ * 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.
+ * Sortie :
+ * 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 *fend = fi + bp->face.nbr; /* borne de "fi"*/
+ Face *fo = clip.face.ptr; /* face clippee */
+
+ /* recopie de "bp" dans les tableaux intermediaires */
+
+ point4f_nbr = bp->point.nbr;
+ point_3D_4D (bp->point.ptr, (int) point4f_nbr, m, point4f);
+ set_Point4f_code (point4f, (int) point4f_nbr, code);
+#ifdef face_normal
+ if (! (clip.is_polygonal = bp->is_polygonal))
+ //bcopy (bp->normal.ptr, clip.normal.ptr,
+ // bp->normal.nbr * sizeof (Vector));
+ memmove (clip.normal.ptr, bp->normal.ptr,
+ bp->normal.nbr * sizeof (Vector));
+#endif /* face_normal */
+ for (; fi < fend; fi++) { /* pour toutes les faces*/
+ if (clipping_Face (fi, fo) != 0) {
+ fo++; /* ajoute la face a "clip" */
+ /*
+ * Construction a la volee du future polygone.
+ * dont l'espace memoire est deja alloue (voir
+ * la procedure "malloc_huge_Bound").
+ */
+ fo->vertex.ptr = (fo-1)->vertex.ptr+(fo-1)->vertex.nbr;
+ }
+ }
+
+ if (fo == clip.face.ptr)
+ return (NULL); /* Rien a voir, circulez... */
+
+ /* recopie des tableaux intermediaires dans "clip" */
+
+ point_4D_3D (point4f, (int) point4f_nbr, code, clip.point.ptr);
+ clip.type = bp->type;
+ clip.face.nbr = (Index)( fo - clip.face.ptr );
+ clip.point.nbr = point4f_nbr;
+#ifdef face_normal
+ if (! bp->is_polygonal)
+ clip.normal.nbr = point4f_nbr;
+#endif /* face_normal */
+ return (&clip);
+}
+
+/*
+ * 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.
+ */
+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 */
+
+ /* 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) - (p1->w - p1->y);
+ //t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t;
+ t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t;
+ PAR_COORD3(*p,t,*p0,*p1);
+ 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) - (p1->w + p1->y);
+ //t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t;
+ t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t;
+ PAR_COORD3(*p,t,*p0,*p1);
+ 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) - (p1->w - p1->x);
+ //t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t;
+ t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t;
+ PAR_COORD3(*p,t,*p0,*p1);
+ 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) - (p1->w + p1->x);
+ //t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t;
+ t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t;
+ PAR_COORD3(*p,t,*p0,*p1);
+ 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) - (p1->w - p1->z);
+ //t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t;
+ t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t;
+ PAR_COORD3(*p,t,*p0,*p1);
+ p->w = p->z; /* propriete du point d'intersection */
+ break;
+
+ case IS_FRONT :
+ /* 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<double>::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 */
+ break;
+ }
+ /* 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" */
+#ifdef face_normal
+ if (! clip.is_polygonal) {
+ Vector *n0 = clip.normal.ptr + v0;
+ Vector *n1 = clip.normal.ptr + v1;
+ Vector *n = clip.normal.ptr + point4f_nbr;
+
+ 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 */
+}
+
+/*
+ * 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.
+ * 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.
+ */
+static void
+point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3)
+{
+ Point4f *pend = p4 + size; /* borne de p4 */
+ float w;
+
+ for (; p4 < pend; p4++, p3++) {
+ if (*cp++ == IS_INSIDE) { /* point visible */
+ w = p4->w;
+
+ p3->x = p4->x / w; /* projection 4D en 3D */
+ p3->y = p4->y / w;
+ p3->z = p4->z / w;
+ }
+ else { /* marque invisible */
+ p3->z = -1.0;
+ }
+ }
+}
+
+/*
+ * 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 resultat.
+ */
+void
+set_Point4f_code (Point4f *p4, int size, Byte *cp)
+{
+ Point4f *pend = p4 + size; /* borne de p4 */
+ Byte b; /* code de p4 */
+
+ for (; p4 < pend; p4++, *cp++ = b) {
+ b = IS_INSIDE;
+ if ( p4->w < p4->y) b |= IS_ABOVE;
+ else if (- p4->w > p4->y) b |= IS_BELOW;
+ if ( p4->w < p4->x) b |= IS_RIGHT;
+ else if (- p4->w > p4->x) b |= IS_LEFT;
+ if ( p4->w < p4->z) b |= IS_BACK;
+ else if ( -0.9 > p4->z) b |= IS_FRONT;
+ }
+}
+
+
+/*
+ * La procedure "where_is_Point4f" localise un point 4D par rapport aux 6 plans
+ * de decoupage de la pyramide tronquee de vision.
+ * Entree :
+ * p4 Point homogene 4D a localiser.
+ * Sortie :
+ * Code indiquant le position de "p4" par rapport aux 6 plans.
+ */
+Byte
+where_is_Point4f (Point4f *p4)
+{
+ Byte b = IS_INSIDE; /* code de "p4" */
+
+ if ( p4->w < p4->y) b |= IS_ABOVE;
+ else if (- p4->w > p4->y) b |= IS_BELOW;
+ if ( p4->w < p4->x) b |= IS_RIGHT;
+ else if (- p4->w > p4->x) b |= IS_LEFT;
+ if ( p4->w < p4->z) b |= IS_BACK;
+ else if ( -0.9 > p4->z) b |= IS_FRONT;
+ return (b);
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpCoreDisplay.cpp b/modules/robot/src/wireframe-simulator/core/vpCoreDisplay.cpp
new file mode 100644
index 0000000..ae00a70
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpCoreDisplay.cpp
@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "display.c" contient les procedures de d'affichage
+ * des scenes de modele geometrique surfacique.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <stdio.h>
+#include <stdlib.h>
+/*
+#include <suntool/sunview.h>
+*/
+#ifdef suncgi
+#include "cgidefs.h"
+#endif /* suncgi */
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+//#include "graph.h"
+
+#include <visp3/robot/vpBound.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpImstack.h>
+#include <visp3/robot/vpRfstack.h>
+#include <visp3/robot/vpVwstack.h>
+
+
+/*
+ * POINT2I :
+ * Tableau de points 2D dans l'espace ecran servant a l'affichage fil-de-fer.
+ *
+ * RENAME :
+ * Tableau de renommage des sommets ou tableau de compteurs associes aux points.
+ */
+Point2i *point2i = (Point2i *) NULL;
+Point2i *listpoint2i = (Point2i *) NULL;
+static int *rename_jlc = (int *) NULL;
+
+
+/*
+ * La procedure "open_display" alloue et initialise les variables utilisees
+ * par le mode "display".
+ */
+void open_display (void)
+{
+ static char proc_name[] = "open_display";
+
+ if ((point2i = (Point2i *) malloc (POINT_NBR*sizeof (Point2i))) == NULL
+ || (listpoint2i = (Point2i *) malloc (50*sizeof (Point2i))) == NULL
+ || (rename_jlc = (int *) malloc (POINT_NBR * sizeof (int))) == NULL)
+ {
+ perror (proc_name);
+ exit (1);
+ }
+}
+
+/*
+ * La procedure "close_display" libere les variables utilisees par le mode
+ * "display".
+ */
+void close_display (void)
+{
+ free ((char *) point2i);
+ free ((char *) listpoint2i);
+ free ((char *) rename_jlc);
+ point2i = (Point2i *) NULL;
+ listpoint2i = (Point2i *) NULL;
+ rename_jlc = (int *) NULL;
+}
+
+/*
+ * La procedure "point_3D_2D" projette les points 3D du volume canonique
+ * dans l'espace image 2D.
+ *
+ * Volume canonique Espace image
+ * ________________ ____________
+ *
+ * - 1 < X < 1 0 < X < xsize
+ * - 1 < Y < 1 0 < Y < ysize
+ * 0 < Z < 1
+ *
+ * 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.
+ */
+// static
+void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
+{
+ Point3f *pend = p3 + size; /* borne de p3 */
+ float xdiv2 = ((float) xsize) / (float)2.0;
+ float ydiv2 = ((float) ysize) / (float)2.0;
+
+ for (; p3 < pend; p3++, p2++) {
+ p2->x = (int) ((1.0 + p3->x) * xdiv2);
+ p2->y = (int) ((1.0 - p3->y) * ydiv2);
+ }
+}
+
+/*
+ * La procedure "set_Bound_face_display" marque les faces affichables
+ * de la surface "bp".
+ * 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 |
+ *
+ * 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.
+ * Entree :
+ * bp Surface a initialiser.
+ * b Drapeaux indiquant les faces non affichables.
+ */
+void set_Bound_face_display (Bound *bp, Byte b)
+{
+ Face *fp = bp->face.ptr;
+ Face *fend = fp + bp->face.nbr;
+ Point3f *pp = bp->point.ptr;
+
+ for (; fp < fend; fp++) {
+ Index *vp;
+ Point3f *p0; /* premier sommet */
+ Point3f *p1; /* second sommet */
+ Point3f *p2; /* dernier sommet */
+
+ fp->is_visible = TRUE;
+ if (b == IS_INSIDE) continue;
+ vp = fp->vertex.ptr;
+ p0 = pp + *vp;
+ p1 = pp + *(vp + 1);
+ p2 = pp + *(vp + fp->vertex.nbr - 1);
+ if (b & IS_ABOVE) {
+ fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
+ >= (p1->x - p0->x) * (p2->z - p0->z));
+ }
+ if (! fp->is_visible) continue;
+ if (b & IS_BELOW) {
+ fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
+ <= (p1->x - p0->x) * (p2->z - p0->z));
+ }
+ if (! fp->is_visible) continue;
+ if (b & IS_RIGHT) {
+ fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
+ >= (p1->z - p0->z) * (p2->y - p0->y));
+ }
+ if (! fp->is_visible) continue;
+ if (b & IS_LEFT) {
+ fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
+ <= (p1->z - p0->z) * (p2->y - p0->y));
+ }
+ if (! fp->is_visible) continue;
+ if (b & IS_BACK) {
+ fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
+ >= (p1->y - p0->y) * (p2->x - p0->x));
+ }
+ if (! fp->is_visible) continue;
+ if (b & IS_FRONT) {
+ fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
+ <= (p1->y - p0->y) * (p2->x - p0->x));
+ }
+ }
+}
+
+
+/*
+ * La procedure "wireframe_Face" affiche une face "fp" en "fil de fer".
+ * sur la fenetre graphique de "suncgi" sur "SUN".
+ * 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.
+ */
+void wireframe_Face (Face *fp, Point2i *pp)
+{
+// extern Window id_window;
+
+ Index *vp = fp->vertex.ptr;
+ Index *vend = vp + fp->vertex.nbr;
+ Point2i *cp = listpoint2i;
+
+ if (fp->vertex.nbr < 2) return;
+ if (fp->vertex.nbr > 50)
+ {
+ printf("pb malloc listpoint2i (display.c)\n"); return;
+ }
+ for (; vp < vend; vp++, cp++) {
+ SET_COORD2(*cp, pp[*vp].x, pp[*vp].y);
+ }
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpKeyword.cpp b/modules/robot/src/wireframe-simulator/core/vpKeyword.cpp
new file mode 100644
index 0000000..0c5a327
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpKeyword.cpp
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "keyword.c" contient les procedures de gestion
+ * des mots cles retournes par l'analyseur lexical "lex".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpToken.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void open_keyword (Keyword *kwp);
+static void open_hash (void);
+static void close_hash (void);
+static int hashpjw (const char *str);
+static void insert_keyword (const char *str, Index token);
+Index get_symbol (char *ident, int length);
+
+#ifdef debug
+static void delete_keyword (void);
+static char *get_keyword (void);
+#endif /* debug */
+
+
+#define PRIME 211
+#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 */
+} Bucket;
+
+
+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.
+ */
+void open_keyword (Keyword *kwp)
+{
+ open_hash ();
+ for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */
+ insert_keyword (kwp->ident, kwp->token);
+}
+
+/*
+ * La procedure "close_keyword" libere les variables utilisees
+ * par les procedures de gestion des mots cles.
+ */
+void close_keyword (void)
+{
+ close_hash ();
+}
+
+/*
+ * La procedure "open_hash" alloue et initialise la table de codage.
+ */
+static void
+open_hash (void)
+{
+ static char proc_name[] = "open_hash";
+
+ Bucket **head, **bend;
+
+ if ((hash_tbl = (Bucket **) malloc (sizeof (Bucket *) * PRIME))==NULL){
+ perror (proc_name);
+ exit (1);
+ }
+ head = hash_tbl;
+ bend = head + PRIME;
+ for (; head < bend; *head++ = NULL) {};
+}
+
+/*
+ * La procedure "close_hash" libere la table de codage et ses elements.
+ */
+static void
+close_hash (void)
+{
+ Bucket **head = hash_tbl;
+ Bucket **bend = head + PRIME;
+ Bucket *bp; /* element courant */
+ Bucket *next; /* element suivant */
+
+ 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 */
+}
+
+/*
+ * 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.
+ * Entree :
+ * str Chaine de caracteres a coder.
+ * Sortie :
+ * Le code de la chaine.
+ */
+static int
+hashpjw (const char *str)
+{
+ unsigned h = 0; /* "hash value" */
+ unsigned g;
+
+ for (; *str != '\0'; str++) {
+ h = (h << 4) + (unsigned)(*str);
+ if ((g = h & ~0xfffffff) != 0) {
+ h ^= g >> 24;
+ h ^= g;
+ }
+ }
+ return ((int)(h % PRIME));
+}
+
+
+/*
+ * La procedure "insert_keyword" insere en tete d'un point d'entree
+ * 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.
+ */
+static void
+insert_keyword (const char *str, Index token)
+{
+ static const char proc_name[] = "insert_keyword";
+
+ Bucket **head = hash_tbl + hashpjw (str);
+ Bucket *bp;
+ Byte length;
+
+ length = (Byte)( strlen(str) ); // Warning! Overflow possible!
+ if ((bp = (Bucket *) malloc (sizeof (Bucket) + length + 1)) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ bp->length = length;
+ bp->token = token;
+ bp->ident = (char *) (bp + 1);
+ strcpy (bp->ident, str);
+
+ bp->next = *head; /* insere "b" en tete de "head" */
+ *head = bp;
+}
+
+/*
+ * 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.
+ * Note :
+ * La chaine "ident" n'est pas terminee par '\0'.
+ * Sortie :
+ * Valeur du jeton associe si c'est un mot cle, 0 sinon.
+ */
+Index get_symbol (char *ident, int length)
+{
+ Bucket *bp;
+ const char *kwd;
+ char *idn = ident;
+ int len = length;
+
+ { /* calcule le code de hachage (voir "hashpjw") */
+ unsigned h = 0; /* "hash value" */
+ unsigned g;
+
+ for (; len != 0; idn++, len--) {
+ h = (h << 4) + (unsigned)(*idn);
+ if ((g = h & ~0xfffffff) != 0) {
+ h ^= g >> 24;
+ h ^= g;
+ }
+ }
+ bp = hash_tbl[h % PRIME];
+ }
+
+ /* recherche le mot cle */
+
+ for (; bp != NULL; NEXT(bp)) {
+ if (length == bp->length) {
+ idn = ident;
+ len = length;
+ kwd = bp->ident;
+ for (; *idn == *kwd; idn++, kwd++)
+ if (--len == 0) return (bp->token);
+ }
+ }
+ return (0); /* identificateur */
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpLex.cpp b/modules/robot/src/wireframe-simulator/core/vpLex.cpp
new file mode 100644
index 0000000..a7ca1db
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpLex.cpp
@@ -0,0 +1,819 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "lex.c" contient les procedures de gestion
+ * de l'analyse lexicale de l'analyseur lexicale "lex"
+ * d'un fichier source dont la grammaire possede
+ * les symboles terminaux suivants (ecrit en "LEX", UNIX) :
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpKeyword.h>
+
+#include <ctype.h>
+#include <math.h>
+#include <stdio.h>
+//#include <unistd.h>
+#include <fcntl.h>
+
+
+// #include <varargs.h> /* modif pour Fedora */
+#include <stdarg.h>
+
+#include <stdlib.h>
+#include <string.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+static void count (void);
+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 */
+
+#define ASCII_NBR 128 /* nombre de codes ASCII*/
+
+#ifndef EOB
+#define EOB (-2) /* fin de buffer */
+#endif
+#ifndef EOF
+#define EOF (-1) /* fin de fichier */
+#endif
+#ifndef EOL
+#define EOL 10 /* fin de ligne */
+#endif
+
+#define CHAR_NBR 130 /* nombre de caracteres */
+
+/* Tests des drapeaux */
+
+#define isnult(c) (scantbl[c] == _NULT)
+#define iscmtt(c) (scantbl[c] & _CMTT)
+#define isfptt(c) (scantbl[c] & _FPTT)
+#define isidnt(c) (scantbl[c] & _IDNT)
+#define isintt(c) (scantbl[c] & _INTT)
+#define issgnt(c) (scantbl[c] & _SGNT)
+#define isstgt(c) (scantbl[c] & _STGT)
+
+/*
+ * Codes des messages d'erreur de l'analyseur lexicale.
+ */
+#define E_UNKNOWN 0
+#define E_SYMBOL 1
+#define E_CMT_EOF 2
+#define E_FLOAT 3
+#define E_INT 4
+#define E_KEYWORD 5
+#define E_STG_EOF 6
+#define E_STG_EOL 7
+#define E_STRING 8
+#define E_9 9
+
+
+const char *lex_errtbl[] = { /* table des messages d'erreur */
+ "error unknown",
+ "symbol undefined",
+ "unexpected EOF in comment",
+ "float expected",
+ "int expected",
+ "keyword expected",
+ "unexpected EOF in string or char constant",
+ "newline in string or char constant",
+ "string expected",
+ ""
+};
+
+char *mytext = NULL;
+int mylength = 0;
+int mylineno = 1;
+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 Byte *chtbl; /* premiers caracteres des terminaux */
+static Byte *scantbl; /* caracteres suivants des terminaux */
+
+
+/*
+ * La procedure "open_lex" alloue et initialise les variables utilisees
+ * par l'analyseur lexical "lex".
+ */
+void open_lex (void)
+{
+ static char proc_name[] = "open_lex";
+
+ int i;
+
+ if ((chtbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL
+ || (scantbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ chtbl += 2; /* 2 sentinelles non affichables */
+ scantbl += 2;
+
+ /* initialise les premiers caracteres des symboles terminaux */
+
+ for (i = 0; i < ASCII_NBR; i++) {
+ if (isalpha(i)) chtbl[i] = IDNT;
+ else if (isdigit(i)) chtbl[i] = INTT;
+ else if (isspace(i)) chtbl[i] = SPCT;
+ else switch (i) {
+ case '"' : chtbl[i] = STGT; break;
+ case '+' :
+ case '-' : chtbl[i] = SGNT; break;
+ case '.' : chtbl[i] = FPTT; break;
+ case '/' : chtbl[i] = CMTT; break;
+ case '_' : chtbl[i] = IDNT; break;
+ default : chtbl[i] = NULT; break;
+ }
+ }
+
+ /* Initialise les sentinelles comme des terminaux. */
+
+ chtbl[EOB] = EOBT;
+ chtbl[EOF] = EOFT;
+ chtbl[EOL] = EOLT;
+
+ /* Initialise les caracteres suivants des symboles terminaux. */
+
+ for (i = 0; i < ASCII_NBR; i++) {
+ if (isalpha(i)) scantbl[i] = _CMTT|_IDNT|_STGT;
+ else if (isdigit(i)) scantbl[i] = _CMTT|_IDNT|_INTT|_STGT;
+ else switch (i) {
+ case '"' : scantbl[i] = _CMTT; break;
+ case '+' :
+ case '-' : scantbl[i] = _CMTT|_SGNT|_STGT; break;
+ case '.' : scantbl[i] = _CMTT|_FPTT|_STGT; break;
+ case '/' : scantbl[i] = _STGT; break;
+ case '_' : scantbl[i] = _CMTT|_IDNT|_STGT; break;
+ default : scantbl[i] = _CMTT|_STGT; break;
+ }
+ }
+
+ /* Initialise les sentinelles comme des terminaux. */
+
+ scantbl[EOB] = _NULT;
+ scantbl[EOF] = _NULT;
+ scantbl[EOL] = _NULT;
+}
+
+/*
+ * La procedure "close_lex" libere les variables utilisees
+ * par l'analyseur lexical "lex".
+ */
+void close_lex (void)
+{
+ 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 */
+
+
+/*
+ * La procedure "lex" contient l'analyseur lexical.
+ * Note :
+ * 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.
+ */
+int lex (void)
+{
+lex_loop :
+
+ for (; chtbl[(int)CURC] == SPCT; mysptr++) {}; /* saute les espaces */
+
+ switch (chtbl[(int)CURC]) {
+
+ case NULT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ return (*mytext);
+ break;
+ case EOBT :
+ next_source ();
+ goto lex_loop;
+ break;
+ case EOFT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ return (T_EOF);
+ break;
+ case EOLT :
+ if (mysptr == lastline) next_source ();
+ else mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto lex_loop;
+ break;
+ case CMTT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (CURC != '*')
+ return (*mytext);
+ mysptr++;
+comment :
+ for (; iscmtt((int)CURC); mysptr++) {};
+ switch (chtbl[(int)CURC]) {
+ case EOBT :
+ next_source ();
+ goto comment;
+ break;
+ case EOFT :
+ lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
+ return (T_EOF);
+ break;
+ case EOLT :
+ if (mysptr == lastline) next_source ();
+ else mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto comment;
+ break;
+ case CMTT :
+ if (PREVC == '*') { /* veritable fin */
+ mysptr++;
+ goto lex_loop;
+ }
+ mysptr++; /* pseudo fin */
+ goto comment;
+ break;
+ }
+ break;
+ case IDNT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ for (; isidnt((int)CURC); mysptr++) {};
+ mylength = (int)(mysptr - mytext);
+ return (get_symbol (mytext, mylength));
+ break;
+ case INTT :
+ 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 */
+float_part :
+ mysptr++;
+ for (; isintt((int)CURC); mysptr++) {};
+ if (CURC != 'E' && CURC != 'e') {
+ myfloat = (float) atof (mytext);
+/* FC
+printf("mytext %s, myfloat %f\n",mytext,myfloat);
+*/
+ return (T_FLOAT);
+ }
+ break;
+ case 'E' : /* lecture exposant */
+ case 'e' :
+ mysptr++;
+ if (isintt((int)CURC))
+ mysptr++;
+ else if (issgnt((int)CURC) && isintt((int)NEXTC))
+ mysptr +=2;
+ else {
+ mysptr--;
+ myfloat = (float) atof (mytext);
+ return (T_FLOAT);
+ }
+ for (; isintt((int)CURC); mysptr++) {};
+ myfloat = (float) atof (mytext);
+ return (T_FLOAT);
+ break;
+ default :
+ if (*mytext == '-')
+ myint = - myint;
+ return (T_INT);
+ break;
+ }
+ break;
+ case FPTT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (! isintt((int)CURC)) /* pas de fraction */
+ return (*mytext);
+ goto float_part;
+ break;
+ case SGNT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (isintt((int)CURC))
+ goto int_part;
+ if (isfptt((int)CURC) && isintt((int)NEXTC))
+ goto float_part;
+ return (*mytext);
+ break;
+ case STGT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+string :
+ for (; isstgt((int)CURC); mysptr++) {};
+ switch (chtbl[(int)CURC]) {
+ case EOBT :
+ next_source ();
+ goto string;
+ break;
+ case EOFT :
+ lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
+ return ('\n');
+ break;
+ case EOLT :
+ lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
+ return ('\n');
+ break;
+ case STGT :
+ if (PREVC != '\\') { /* veritable fin */
+ mytext++;
+ mylength = (int)(mysptr - mytext);
+ mysptr++;
+ return (T_STRING);
+ }
+ mysptr++; /* pseudo fin */
+ goto string;
+ break;
+ }
+ break;
+ default :
+ ECHO;
+ mysptr++;
+ goto lex_loop;
+ break;
+ }
+ return (T_EOF);
+}
+
+/*
+ * La procedure "lexecho" contient l'analyseur lexical "lex" :
+ * 1 Analyse le fichier source,
+ * 2 Affiche le fichier source sur le fichier "f",
+ * 3 Stoppe devant le jeton "token".
+ * Note :
+ * 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.
+ * Sortie :
+ * Code du symbole terminale analyse.
+ */
+int lexecho (FILE *f, int token)
+{
+lex_loop :
+ 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 */
+ mysptr++;
+ if (token != *mytext)
+ fwrite (mytext, 1, 1, f);
+ return (*mytext);
+ break;
+ case EOBT :
+ next_source ();
+ goto lex_loop;
+ break;
+ case EOFT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ return (T_EOF);
+ break;
+ case EOLT :
+ fwrite (mysptr, 1, 1, f);
+ if (mysptr == lastline) next_source ();
+ else mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto lex_loop;
+ break;
+ case CMTT :
+ fwrite (mysptr, 1, 1, f);
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (CURC != '*')
+ return (*mytext);
+ fwrite (mysptr, 1, 1, f);
+ mysptr++;
+comment :
+ for (; iscmtt((int)CURC); mysptr++)
+ fwrite (mysptr, 1, 1, f);
+ switch (chtbl[(int)CURC]) {
+ case EOBT :
+ next_source ();
+ goto comment;
+ break;
+ case EOFT :
+ lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
+ return (T_EOF);
+ break;
+ case EOLT :
+ fwrite (mysptr, 1, 1, f);
+ if (mysptr == lastline) next_source ();
+ else mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto comment;
+ break;
+ case CMTT :
+ fwrite (mysptr, 1, 1, f);
+ if (PREVC == '*') { /* veritable fin */
+ mysptr++;
+ goto lex_loop;
+ }
+ mysptr++; /* pseudo fin */
+ goto comment;
+ break;
+ }
+ break;
+ case IDNT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ for (; isidnt((int)CURC); mysptr++) {};
+ mylength = (int)(mysptr - mytext);
+ if (token != get_symbol (mytext, mylength))
+ fwrite (mytext, (size_t)mylength, 1, f);
+ return (get_symbol (mytext, mylength));
+ break;
+ case INTT :
+ mytext = mysptr; /* sauvegarde le jeton */
+int_part :
+ mysptr++;
+ for (; isintt((int)CURC); mysptr++) {};
+ switch (CURC) {
+ case '.' : /* lecture fraction */
+float_part :
+ mysptr++;
+ for (; isintt((int)CURC); mysptr++) {};
+ if (CURC != 'E' && CURC != 'e') {
+ if (token != T_FLOAT)
+ fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ return (T_FLOAT);
+ }
+ break;
+ case 'E' : /* lecture exposant */
+ case 'e' :
+ mysptr++;
+ if (isintt((int)CURC)) mysptr++;
+ else if (issgnt((int)CURC) && isintt((int)NEXTC)) mysptr +=2;
+ else {
+ mysptr--;
+ if (token != T_FLOAT)
+ fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ return (T_FLOAT);
+ }
+ for (; isintt((int)CURC); mysptr++) {};
+ if (token != T_FLOAT)
+ fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ return (T_FLOAT);
+ break;
+ default :
+ if (token != T_INT)
+ fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ return (T_INT);
+ break;
+ }
+ break;
+ case FPTT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (! isintt((int)CURC)) { /* pas de fraction */
+ if (token != *mytext)
+ fwrite (mytext, 1, 1, f);
+ return (*mytext);
+ }
+ goto float_part;
+ break;
+ case SGNT :
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (isintt((int)CURC)) goto int_part;
+ if (isfptt((int)CURC) && isintt((int)NEXTC)) goto float_part;
+ if (token != *mytext)
+ fwrite (mytext, 1, 1, f);
+ return (*mytext);
+ break;
+ case STGT :
+ fwrite (mysptr, 1, 1, f);
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+string :
+ for (; isstgt((int)CURC); mysptr++)
+ fwrite (mysptr, 1, 1, f);
+ switch (chtbl[(int)CURC]) {
+ case EOBT :
+ next_source ();
+ goto comment;
+ break;
+ case EOFT :
+ lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
+ return (T_EOF);
+ break;
+ case EOLT :
+ lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
+ return ('\n');
+ break;
+ case STGT :
+ fwrite (mysptr, 1, 1, f);
+ if (PREVC != '\\') { /* veritable fin */
+ mytext++;
+ mylength = (int)(mysptr - mytext);
+ mysptr++;
+ return (T_STRING);
+ }
+ mysptr++; /* pseudo fin */
+ goto string;
+ break;
+ }
+ break;
+ default :
+ fwrite (mysptr, 1, 1, f);
+ mysptr++;
+ goto lex_loop;
+ break;
+ }
+ return (T_EOF);
+}
+
+
+#undef BUFSIZE
+#undef LINESIZE
+#undef TEXTSIZE
+
+#define BUFSIZE (BUFSIZ << 5)
+#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 */
+
+
+/*
+ * La procedure "unlex" recule la tete de lecture devant le dernier jeton.
+ */
+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.
+ */
+void open_source (FILE *fd, const char *str)
+{
+ static char proc_name[] = "open_source";
+
+ if ((source = (char *) malloc((strlen(str)+1)*sizeof(char))) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ strcpy (source, str);
+ if ((botbuf = (char *) malloc (TEXTSIZE * sizeof (char))) == NULL) {
+ perror (proc_name);
+ exit (1);
+ }
+ fds = fd;
+ buf = botbuf + 1 + LINESIZE;
+ topbuf = buf + 1;
+ mylineno = 1;
+ next_source ();
+}
+
+/*
+ * La procedure "close_source" libere les variables utilisees pour la gestion
+ * des entrees du programme source.
+ */
+void close_source (void)
+{
+ free ((char *) source);
+ free ((char *) botbuf);
+}
+
+/*
+ * La procedure "next_source" remplit le buffer courant.
+ */
+static void next_source (void)
+{
+ size_t size;
+ char *bot = buf;
+ char *top = topbuf;
+
+ /* recopie la derniere ligne devant "buf" */
+
+ *bot = EOL; /* evite le debordement de "buf" */
+ while ((*--bot = *--top) != EOL) {};
+ myline = mysptr = bot + 1;
+
+ size = fread (buf,sizeof (char), BUFSIZE,fds);
+ if (size == 0) {
+ topbuf = buf + 1;
+ *buf = EOF;
+ *topbuf = EOB; /* sentinelle de fin de fichier */
+ mysptr = buf;
+ }
+ else {
+ topbuf = buf + size;
+ *topbuf = EOB; /* sentinelle de fin de buffer */
+
+ /* recherche de la derniere ligne */
+ top = topbuf;
+ while (*--top != EOL) {};
+ lastline = top;
+ }
+}
+
+
+/*
+ * 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".
+ */
+#define ERR_STACK_MAX 32
+
+
+static const char *err_stack[ERR_STACK_MAX];
+static int size_stack = 0;
+
+
+/*
+ * La procedure "count" calcule la distance en espaces entre
+ * le premier caractere "*mytext" et le caractere de debut de ligne "*myline".
+ */
+static void count (void)
+{
+ char *str;
+
+ mycolumno = 0;
+ for (str = myline; str <= mytext; str++) {
+ (*str == '\t') ? mycolumno += 8 - (mycolumno % 8) : mycolumno++;
+ }
+}
+
+/*
+ * La procedure "lexerr" affiche les messages d'erreur.
+ * 1 elle affiche la ligne d'erreur du fichier source.
+ * 2 elle indique la position de l'erreur dans la ligne.
+ * 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.
+ */
+
+//lexerr (va_alist)
+//va_dcl
+
+void lexerr (const char* path, ...)
+{
+ va_list ap;
+ char *cp;
+ int i;
+
+ /* 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. */
+
+ for (i = 0; i < size_stack; i++)
+ fprintf (stderr, "%s", err_stack[i]);
+
+ /* Affiche les messages d'erreur en parametres. */
+
+ va_start(ap,path);
+ while ((cp = (char *) va_arg(ap, char *)) != NULL)
+ fprintf (stderr, "%s", cp);
+ fprintf (stderr,"\n");
+ va_end(ap);
+
+ exit (1);
+}
+
+/*
+ * La procedure "poperr" depile le message d'erreur du sommet de pile.
+ */
+void poperr (void)
+{
+ static char proc_name[] = "poperr";
+
+ if (--size_stack < 0) {
+ fprintf (stderr, "%s: error stack underflow\n", proc_name);
+ exit (1);
+ }
+}
+
+/*
+ * La procedure "popup_error" remplace le message d'erreur du sommet de pile.
+ */
+void popuperr (const char *str)
+{
+ static const char proc_name[] = "popuerr";
+
+ if (size_stack <= 0) {
+ fprintf (stderr, "%s: error stack underflow\n", proc_name);
+ exit (1);
+ }
+ err_stack[size_stack-1] = str;
+}
+
+/*
+ * La procedure "pusherr" empile le message d'erreur.
+ */
+void pusherr (const char *str)
+{
+ static const char proc_name[] = "pusherr";
+
+ if (size_stack >= ERR_STACK_MAX) {
+ fprintf (stderr, "%s: error stack overflow\n", proc_name);
+ exit (1);
+ }
+ err_stack[size_stack++] = str;
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpMyio.cpp b/modules/robot/src/wireframe-simulator/core/vpMyio.cpp
new file mode 100644
index 0000000..0bfc687
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpMyio.cpp
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "myio.c" contient les procedures d'entree/sortie
+ * des types definis dans le module "my.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
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpLex.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+extern char *mytext; /* chaine du symbole courant */
+
+
+/*
+ * La procedure "fscanf_float" lit en ascii un nombre flottant.
+ * Entree :
+ * fp Nombre flottant a lire.
+ */
+void fscanf_float (float *fp)
+{
+ int t;
+
+ if ((t = lex ()) != T_FLOAT && t != T_INT)
+ lexerr ("start", "float expected", NULL);
+ *fp = (t == T_INT) ? (float) myint : myfloat;
+}
+
+/*
+ * La procedure "fscanf_Index" lit en ascii un indice.
+ * Entree :
+ * ip Indice a lire.
+ */
+void fscanf_Index (Index *ip)
+{
+ if (lex () != T_INT)
+ lexerr ("start", "integer expected", NULL);
+ *ip = (Index) myint;
+}
+
+/*
+ * La procedure "fscanf_int" lit en ascii un nombre entier.
+ * Entree :
+ * ip Nombre entier a lire.
+ */
+void fscanf_int (int *ip)
+{
+ if (lex () != T_INT)
+ lexerr ("start", "integer expected", NULL);
+ *ip = myint;
+}
+
+/*
+ * La procedure "fscanf_string" lit en ascii une chaine de caracteres.
+ * Entree :
+ * str Chaine a lire.
+ */
+void fscanf_string (char **str)
+{
+ if (lex () != T_STRING)
+ lexerr ("start", "string expected", NULL);
+ if (*str == NULL)
+ *str = (char *) malloc ((size_t)(mylength + 1) * sizeof (char));
+ else
+ *str = (char *) realloc (*str, (size_t)(mylength + 1) * sizeof (char));
+
+ if (*str == NULL) {
+ printf("Unable to read the string: bad memory allocation");
+ return;
+ }
+
+ strncpy (*str, mytext, (size_t)mylength);
+}
+
+/*
+ * La procedure "fscanf_Type" lit en ascii un octet.
+ * Entree :
+ * ip Type a lire.
+ */
+void fscanf_Type (Type *ip)
+{
+ if (lex () != T_INT)
+ lexerr ("start", "integer expected", NULL);
+ *ip = (Type ) myint;
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpParser.cpp b/modules/robot/src/wireframe-simulator/core/vpParser.cpp
new file mode 100644
index 0000000..8f43d30
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpParser.cpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "parser.c" contient les procedures de gestion
+ * de l'analyse syntaxique d'un fichier source dont la grammaire
+ * possede les symboles terminaux de "lex.c".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpBound.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpLex.h>
+#include <visp3/robot/vpSkipio.h>
+
+#include <stdio.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#ifdef used
+extern Byte *get_remove (void);
+extern View_parameters *get_view_parameters (void);
+#endif /* used */
+
+/*
+ * La procedure "parser" fait l'analyse syntaxique du fichier source.
+ * Entree/Sortie :
+ * bsp Scene surfacique polygonale a lire.
+ */
+void parser (Bound_scene *bsp)
+{
+ int token;
+
+ while ((token = lex ()) != T_EOF)
+ switch (token) {
+ case '$' :
+ switch (lex ()) {
+ case T_IDENT : /* saute la commande inconnue */
+ skip_cmd (/* stderr */);
+ unlex ();
+ break;
+ case T_EXIT :
+ return;
+ break;
+ case T_BOUND :
+ if (bsp->bound.nbr == BOUND_NBR) {
+ fprintf (stderr, "mire: too much bound\n");
+ return;
+ }
+ fscanf_Bound (
+ &(bsp->bound.ptr[bsp->bound.nbr++]));
+ break;
+#ifdef used
+ case T_REMOVE :
+ fscanf_Remove (get_remove ());
+ break;
+ case T_VIEW :
+ fscanf_View_parameters (get_view_parameters ());
+ set_projection (void);
+ break;
+#endif /* used */
+ default :
+ lexerr ("start", "keyword expected", NULL);
+ break;
+ }
+ break;
+ default :
+ lexerr ("start", "symbol '$' expected", NULL);
+ break;
+ }
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpProjection.cpp b/modules/robot/src/wireframe-simulator/core/vpProjection.cpp
new file mode 100644
index 0000000..287ccc6
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpProjection.cpp
@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "projection.c" contient les procedures de calcul
+ * des matrices de projection perspective et parallele.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpView.h>
+#include <stdio.h>
+#include <math.h>
+void set_parallel (View_parameters*, Matrix);
+void set_perspective (View_parameters*, Matrix);
+
+
+
+/*
+ * 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.
+ */
+void View_to_Matrix (View_parameters *vp, Matrix m)
+{
+ static char proc_name[] = "View_to_Matrix";
+
+ switch (vp->type) {
+ case PARALLEL :
+ set_parallel (vp, m);
+ break;
+ case PERSPECTIVE :
+ set_perspective (vp, m);
+ break;
+ default :
+ fprintf (stderr, "%s: bad view type\n", proc_name);
+ set_perspective (vp, m);
+ break;
+ }
+}
+
+
+/*
+ * 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.
+ * Entree :
+ * m Matrice a initialiser.
+ * v0 Premier vecteur.
+ * v1 Second vecteur.
+ */
+static void set_zy (Matrix m, Vector *v0, Vector *v1)
+{
+ Vector rx, ry, rz;
+
+ SET_COORD3(rz, - v0->x,- v0->y, - v0->z);
+ CROSS_PRODUCT(rx, *v0, *v1);
+ norm_vector (&rx);
+ norm_vector (&rz);
+ CROSS_PRODUCT (ry,rz,rx); /* ry est norme */
+
+ m[0][0] = rx.x; m[0][1] = ry.x; m[0][2] = rz.x; m[0][3] = 0.0;
+ m[1][0] = rx.y; m[1][1] = ry.y; m[1][2] = rz.y; m[1][3] = 0.0;
+ m[2][0] = rx.z; m[2][1] = ry.z; m[2][2] = rz.z; m[2][3] = 0.0;
+ m[3][0] = 0.0 ; m[3][1] = 0.0 ; m[3][2] = 0.0 ; m[3][3] = 1.0;
+}
+
+/*
+ * 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.
+ * Entree :
+ * vp Parametres de visualisation.
+ * wc Matrice a initialiser.
+ */
+void set_parallel (View_parameters *vp, Matrix wc)
+{
+ Matrix m = IDENTITY_MATRIX;
+ Point3f cop;
+ Point4f doprim;
+ Vector dop, v;
+
+ /*
+ * 1 : Translation du point de reference VRP a l'origine.
+ */
+ SET_COORD3(v,- vp->vrp.x, - vp->vrp.y, - vp->vrp.z);
+ Translate_to_Matrix (&v, wc);
+ /*
+ * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
+ * 3 : Rotation pour rendre la projection de VUP sur le plan de
+ * projection parallele a l'axe Y.
+ */
+ set_zy (m, &vp->vpn, &vp->vup);
+ /*
+ * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
+ */
+ postleft_matrix (m, 'z');
+ postmult_matrix (wc, m);
+ /*
+ * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
+ * COP = DOP = Direction of Projection.
+ * DOPRIM = DOP * R_TRL
+ * Pas de translation dans la matrice R_TRL pour la transformation
+ * du vecteur DOP.
+ */
+ SET_COORD3(dop,
+ vp->vrp.x - vp->cop.x,
+ vp->vrp.y - vp->cop.y,
+ vp->vrp.z - vp->cop.z);
+ norm_vector (&dop);
+ SET_COORD3(cop,dop.x,dop.y,dop.z);
+ point_matrix (&doprim, &cop, m);
+ ident_matrix (m);
+ m[2][0] = - doprim.x / doprim.z;
+ m[2][1] = - doprim.y / doprim.z;
+ postmult_matrix (wc, m);
+ /*
+ * 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
+ * je prefere, afin de rester coherent avec la projection perspective,
+ * -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));
+ posttrans_matrix (wc, &v);
+ SET_COORD3(v,
+ (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)),
+ (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
+ (float)(1.0 / (vp->depth.back - vp->depth.front)));
+ postscale_matrix (wc, &v);
+}
+
+/*
+ * 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.
+ * Entree :
+ * vp Parametres de visualisation.
+ * wc Matrice a initialiser.
+ */
+void set_perspective (View_parameters *vp, Matrix wc)
+{
+ Matrix m = IDENTITY_MATRIX;
+ Point4f vrprim, cw;
+ float zmin;
+ Vector v;
+
+ /*
+ * 1 : Translation du centre de projection COP a l'origine.
+ */
+ SET_COORD3(v,- vp->cop.x, - vp->cop.y, - vp->cop.z);
+ Translate_to_Matrix (&v, wc);
+ /*
+ * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
+ * 3 : Rotation pour rendre la projection de VUP sur le plan de
+ * projection parallele a l'axe Y.
+ */
+ set_zy (m, &vp->vpn, &vp->vup);
+ postmult_matrix (wc, m);
+ /*
+ * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
+ */
+ postleft_matrix (wc, 'z');
+ /*
+ * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
+ */
+ point_matrix (&vrprim, &vp->vrp, wc);
+ cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
+ cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
+ cw.z = (float)(vrprim.z);
+ ident_matrix (m);
+ m[2][0] = - cw.x / cw.z;
+ m[2][1] = - cw.y / cw.z;
+ postmult_matrix (wc, m);
+ /*
+ * 6 : Mise a l'echelle de la pyramide.
+ */
+ SET_COORD3(v,
+ (float)((2.0*vrprim.z)/((vp->vwd.umax-vp->vwd.umin)*(vrprim.z+vp->depth.back))),
+ (float)((2.0*vrprim.z)/((vp->vwd.vmax-vp->vwd.vmin)*(vrprim.z+vp->depth.back))),
+ (float)( 1.0/(vrprim.z+vp->depth.back)));
+ postscale_matrix (wc, &v);
+ /*
+ * 7 : Transformation perspective.
+ */
+ zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
+ ident_matrix (m);
+ m[2][2] = (float)(1.0 / (1.0 - zmin)); m[2][3] = 1.0;
+ m[3][2] = (float)(- zmin / (1.0 - zmin)); m[3][3] = 0.0;
+ postmult_matrix (wc, m);
+}
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpRfstack.cpp b/modules/robot/src/wireframe-simulator/core/vpRfstack.cpp
new file mode 100644
index 0000000..63bc09c
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpRfstack.cpp
@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "rfstack.c" contient les procedures de gestion
+ * de la pile d'elimination de faces (Remove Faces STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpRfstack.h>
+#include <stdio.h>
+#include <string.h>
+#define STACKSIZE 32
+
+
+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.
+ */
+void
+fprintf_rfstack (FILE *fp)
+{
+ int flg;
+ flg = 0; /* nul si element unique */
+
+ if (*sp == IS_INSIDE) {
+ fprintf (fp, "(null)\n");
+ return;
+ }
+ fprintf (fp, "(");
+ if (*sp & IS_ABOVE) {
+ //if (flg) fprintf (fp, " "); Removed since if (flg) cannot be true
+ flg ++;
+ fprintf (fp, "above");
+ }
+ if (*sp & IS_BELOW) {
+ if (flg) fprintf (fp, " ");
+ flg ++;
+ fprintf (fp, "below");
+ }
+ if (*sp & IS_RIGHT) {
+ if (flg) fprintf (fp, " ");
+ flg ++;
+ fprintf (fp, "right");
+ }
+ if (*sp & IS_LEFT) {
+ if (flg) fprintf (fp, " ");
+ flg ++;
+ fprintf (fp, "left");
+ }
+ if (*sp & IS_BACK) {
+ if (flg) fprintf (fp, " ");
+ flg ++;
+ fprintf (fp, "back");
+ }
+ if (*sp & IS_FRONT) {
+ /*if (flg)*/ fprintf (fp, " ");
+ flg ++;
+ fprintf (fp, "front");
+ }
+ fprintf (fp, ")\n");
+}
+
+/*
+ * 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.
+ */
+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.
+ */
+void
+load_rfstack (int i)
+{
+ *sp = i;
+}
+
+/*
+ * La procedure "pop_rfstack" depile les drapeaux au sommet
+ * de la pile des drapeaux d'elimination de faces.
+ */
+void
+pop_rfstack (void)
+{
+ static char proc_name[] = "pop_rfstack";
+
+ if (sp == stack) {
+ fprintf (stderr, "%s: stack underflow\n", proc_name);
+ return;
+ }
+ else sp--;
+}
+
+/*
+ * La procedure "push_rfstack" empile et duplique les drapeaux du sommet
+ * de la pile des drapeaux d'elimination de faces.
+ */
+void
+push_rfstack (void)
+{
+ static char proc_name[] = "push_rfstack";
+
+ if (sp == stack + STACKSIZE - 1) {
+ fprintf (stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ *sp = *(sp - 1);
+}
+
+/*
+ * La procedure "swap_rfstack" echange les deux premiers elements
+ * de la pile des drapeaux d'elimination de faces.
+ */
+void
+swap_rfstack (void)
+{
+ int *ip, tmp;
+
+ ip = (sp == stack) ? sp + 1 : sp - 1;
+ SWAP(*sp, *ip, tmp);
+}
+
+/*
+ * La procedure "add_rfstack" ajoute des drapeaux au sommet
+ * de la pile des drapeaux d'elimination de faces.
+ */
+void
+add_rfstack (int i)
+{
+ *sp |= i;
+}
+
+/*
+ * La procedure "sub_rfstack" soustrait des drapeaux au sommet
+ * de la pile des drapeaux d'elimination de faces.
+ */
+void
+sub_rfstack (int i)
+{
+ *sp &= ~i;
+}
+
+#endif
+
diff --git a/modules/robot/src/wireframe-simulator/core/vpSkipio.cpp b/modules/robot/src/wireframe-simulator/core/vpSkipio.cpp
new file mode 100644
index 0000000..9a7c978
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpSkipio.cpp
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "skipio.c" contient les procedures d'analyse
+ * syntaxique du fichier "source" qui permettent de traiter
+ * les commandes inconnues.
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpLex.h>
+#include <visp3/robot/vpSkipio.h>
+#include <stdio.h>
+/*
+ * La procedure "skip_cmd" saute les structures d'une commande
+ * jusqu'a reconnaitre le debut d'une nouvelle commande.
+ * Entree :
+ * f Fichier en sortie.
+ */
+void skip_cmd (void)
+{
+ int token;
+
+ fprintf (stderr, "\n$ ");
+ fwrite (mytext, (size_t)mylength, 1, stderr);
+ while ((token = lexecho (stderr, '$')) != T_EOF && token != '$') {};
+ unlex ();
+}
+
+/*
+ * 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.
+ */
+void skip_keyword (int token, const char *err)
+{
+ int t;
+
+ switch (t = lex ()) {
+ case T_IDENT : /* saute le mot cle inconnu */
+ while ((t = lex ()) != 0){
+ switch (t) {
+ case '$' : /* nouvelle commande */
+ case T_EOF : /* fin de fichier */
+ lexerr ("start", err, NULL);
+ break;
+ default :
+ if (t == token) return;
+ break;
+ }
+ }
+ break;
+ default :
+ if (t == token) return;
+ break;
+ }
+ lexerr ("start", err, NULL);
+}
+
+#endif
+
diff --git a/modules/robot/src/wireframe-simulator/core/vpTmstack.cpp b/modules/robot/src/wireframe-simulator/core/vpTmstack.cpp
new file mode 100644
index 0000000..a522582
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpTmstack.cpp
@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "tmstack.c" contient les procedures de gestion
+ * de la pile de matrices de transformation (Transformation
+ * Matrix STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpTmstack.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#define STACKSIZE 32
+
+
+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.
+ */
+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.
+ */
+void
+load_tmstack (Matrix m)
+{
+ //bcopy ((char *) m, (char *) *sp, sizeof (Matrix));
+ memmove ((char *) *sp, (char *) m, sizeof (Matrix));
+}
+
+/*
+ * La procedure "pop_tmstack" depile la matrice au sommet
+ * de la pile des matrices de transformation.
+ */
+void
+pop_tmstack (void)
+{
+ static char proc_name[] = "pop_tmstack";
+
+ if (sp == stack) {
+ fprintf (stderr, "%s: stack underflow\n", proc_name);
+ return;
+ }
+ else sp--;
+}
+
+/*
+ * La procedure "push_tmstack" empile et duplique le sommet
+ * de la pile des matrices de transformation.
+ */
+void
+push_tmstack (void)
+{
+ static char proc_name[] = "push_tmstack";
+
+ if (sp == stack + STACKSIZE - 1) {
+ fprintf (stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ //bcopy ((char *) (sp - 1), (char *) sp, sizeof (Matrix));
+ memmove ((char *) sp, (char *) (sp - 1), sizeof (Matrix));
+}
+
+/*
+ * La procedure "swap_tmstack" echange les deux premieres matrices
+ * de la pile des matrices de transformation.
+ */
+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));
+ memmove ((char *) tmp, (char *) *sp, sizeof (Matrix));
+ memmove ((char *) *sp, (char *) *mp, sizeof (Matrix));
+ memmove ((char *) *mp, (char *) tmp, sizeof (Matrix));
+}
+
+/*
+ * La procedure "postmult_tmstack" postmultiplie la matrice au sommet
+ * de la pile des matrices de transformation.
+ * Entree :
+ * m Matrice multiplicative.
+ */
+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.
+ */
+void
+postrotate_tmstack (Vector *vp)
+{
+ Matrix m;
+
+ Rotate_to_Matrix (vp, m);
+ postmult3_matrix (*sp, m);
+}
+
+/*
+ * La procedure "postscale_tmstack" postmultiplie la matrice au sommet
+ * de la pile des matrices de transformation par une homothetie.
+ * Entree :
+ * vp Vecteur d'homothetie.
+ */
+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.
+ */
+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.
+ */
+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.
+ */
+void
+prerotate_tmstack (Vector *vp)
+{
+ Matrix m;
+
+ Rotate_to_Matrix (vp, m);
+ premult3_matrix (*sp, m);
+}
+
+/*
+ * La procedure "prescale_tmstack" premultiplie la matrice au sommet
+ * de la pile des matrices de transformation par une homothetie.
+ * Entree :
+ * vp Vecteur d'homothetie.
+ */
+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.
+ */
+void
+pretranslate_tmstack (Vector *vp)
+{
+ pretrans_matrix (*sp, vp);
+}
+
+#endif
+
diff --git a/modules/robot/src/wireframe-simulator/core/vpToken.cpp b/modules/robot/src/wireframe-simulator/core/vpToken.cpp
new file mode 100644
index 0000000..cf7bd9b
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpToken.cpp
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "token.c" contient la declaration des mots cles.
+ * de l'analyseur lexical "lex".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpToken.h>
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+Keyword keyword_tbl[] = { /* tableau des mots cles termine par NULL*/
+ { "above", T_ABOVE },
+ { "back", T_BACK },
+ { "below", T_BELOW },
+ { "bound", T_BOUND },
+ { "cop", T_COP },
+ { "depth", T_DEPTH },
+ { "exit", T_EXIT },
+ { "face_list", T_FACE_LIST },
+ { "front", T_FRONT },
+ { "left", T_LEFT },
+ { "none", T_NONE },
+ { "parallel", T_PARALLEL },
+ { "perspective",T_PERSPECTIVE },
+ { "point_list", T_POINT_LIST },
+ { "remove", T_REMOVE },
+ { "right", T_RIGHT },
+ { "type", T_TYPE },
+ { "view", T_VIEW },
+ { "vpn", T_VPN },
+ { "vrp", T_VRP },
+ { "vup", T_VUP },
+ { "window", T_WINDOW },
+ { NULL, NULL }
+};
+
+#endif
diff --git a/modules/robot/src/wireframe-simulator/core/vpViewio.cpp b/modules/robot/src/wireframe-simulator/core/vpViewio.cpp
new file mode 100644
index 0000000..ca4dd67
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpViewio.cpp
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "viewio.c" contient les procedures d'entree/sortie
+ * des types definis dans le module "view.h".
+ * Les entrees non specifiees sont effectuees
+ * sur le fichier source de "lex.c".
+ * Pour les mots cles des "fprintf_..." voir "token.c".
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <stdio.h>
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpToken.h>
+#include <visp3/robot/vpLex.h>
+#include <visp3/robot/vpSkipio.h>
+
+
+/*
+ * La procedure "fscanf_Remove" lit en ascii les parametres d'elimination
+ * des faces.
+ * Entree :
+ * bp Parametres a lire.
+ */
+void fscanf_Remove (Byte *bp)
+{
+ switch (lex ()) {
+ case T_NONE : *bp = IS_INSIDE; break;
+ case T_ABOVE : *bp |= IS_ABOVE; break;
+ case T_BACK : *bp |= IS_BACK; break;
+ case T_BELOW : *bp |= IS_BELOW; break;
+ case T_FRONT : *bp |= IS_FRONT; break;
+ case T_LEFT : *bp |= IS_LEFT; break;
+ case T_RIGHT : *bp |= IS_RIGHT; break;
+ default :
+ lexerr ("start", "remove: keyword \"none|above|back|below|front|left|right\" expected");
+ break;
+ }
+}
+
+/*
+ * La procedure "fscanf_View_parameters" lit en ascii les parametres
+ * de visualisation.
+ * Entree :
+ * vp Parametres de visualisation a lire.
+ */
+void fscanf_View_parameters (View_parameters *vp)
+{
+ /* Lecture du type de projection lors de la prise de vue. */
+
+ skip_keyword (T_TYPE, "view: keyword \"type\" expected");
+ switch (lex ()) {
+ case T_PARALLEL :
+ vp->type = PARALLEL;
+ break;
+ case T_PERSPECTIVE :
+ vp->type = PERSPECTIVE;
+ break;
+ default :
+ lexerr ("start", "view_type: keyword \"parallel|perspective\" expected");
+ break;
+ }
+
+ /* 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. */
+
+ 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. */
+
+ 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. */
+
+ 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. */
+
+ skip_keyword (T_WINDOW, "view: keyword \"window\" expected");
+ pusherr ("view_window_umin: ");
+ fscanf_float (&vp->vwd.umin);
+ popuperr ("view_window_umax: ");
+ fscanf_float (&vp->vwd.umax);
+ popuperr ("view_window_vmin: ");
+ fscanf_float (&vp->vwd.vmin);
+ popuperr ("view_window_vmax: ");
+ fscanf_float (&vp->vwd.vmax);
+ poperr ();
+
+ /* Lecture des profondeurs de decoupage avant et arriere. */
+
+ skip_keyword (T_DEPTH, "view: keyword \"depth\" expected");
+ pusherr ("view_depth_front: ");
+ fscanf_float (&vp->depth.front);
+ popuperr ("view_depth_back: ");
+ fscanf_float (&vp->depth.back);
+ poperr ();
+}
+
+#endif
+
+
diff --git a/modules/robot/src/wireframe-simulator/core/vpVwstack.cpp b/modules/robot/src/wireframe-simulator/core/vpVwstack.cpp
new file mode 100644
index 0000000..c2f6768
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/core/vpVwstack.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Le module "vwstack.c" contient les procedures de gestion
+ * de la pile des points de vue (VieW STACK).
+ *
+ * Authors:
+ * Jean-Luc CORRE
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <stdio.h>
+#include <string.h>
+#include <limits>
+#include <cmath> // std::fabs()
+
+// #include <varargs.h> /* modif Fedora
+#include <stdarg.h>
+
+#include <visp3/robot/vpMy.h>
+#include <visp3/robot/vpArit.h>
+#include <visp3/robot/vpView.h>
+#include <visp3/robot/vpVwstack.h>
+
+
+#define STACKSIZE 4
+
+
+static View_parameters stack[STACKSIZE]= { vpDEFAULT_VIEW };
+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.
+ */
+void
+fprintf_vwstack (FILE *fp, char *argv)
+{
+ static char proc_name[] = "fprintf_vwstack";
+
+ if (argv == NULL || strcmp (argv, "type") == 0) {
+ const char *typetoa;
+
+ switch (sp->type) {
+ case PARALLEL :
+ typetoa = "parallel";
+ break;
+ case PERSPECTIVE :
+ typetoa = "perspective";
+ break;
+ default :
+ typetoa = "unknown";
+ break;
+ }
+ fprintf (fp, "(type\t%s)\n", typetoa);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "cop") == 0) {
+ fprintf (fp, "(cop\t%.3f\t%.3f\t%.3f)\n",
+ sp->cop.x, sp->cop.y, sp->cop.z);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "vrp") == 0) {
+ fprintf (fp, "(vrp\t%.3f\t%.3f\t%.3f)\n",
+ sp->vrp.x, sp->vrp.y, sp->vrp.z);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "vpn") == 0) {
+ fprintf (fp, "(vpn\t%.3f\t%.3f\t%.3f)\n",
+ sp->vpn.x, sp->vpn.y, sp->vpn.z);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "vup") == 0) {
+ fprintf (fp, "(vup\t%.3f\t%.3f\t%.3f)\n",
+ sp->vup.x, sp->vup.y, sp->vup.z);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "window") == 0) {
+ fprintf (fp, "(window\t%.3f\t%.3f\t%.3f\t%.3f)\n",
+ sp->vwd.umin,sp->vwd.umax,sp->vwd.vmin,sp->vwd.vmax);
+ if (argv != NULL) return;
+ }
+ if (argv == NULL || strcmp (argv, "depth") == 0) {
+ fprintf (fp, "(depth\t%.3f\t%.3f)\n",
+ sp->depth.front, sp->depth.back);
+ if (argv != NULL) return;
+ }
+ if (argv != NULL)
+ fprintf (stderr, "%s: argument unknown\n", proc_name);
+}
+
+/*
+ * 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.
+ */
+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.
+ */
+void
+load_vwstack (View_parameters *vp)
+{
+ *sp = *vp;
+}
+
+/*
+ * La procedure "pop_vwstack" depile le point de vue au sommet
+ * de la pile des points de vue.
+ */
+void
+pop_vwstack (void)
+{
+ static char proc_name[] = "pop_vwstack";
+
+ if (sp == stack) {
+ fprintf (stderr, "%s: stack underflow\n", proc_name);
+ return;
+ }
+ else sp--;
+}
+
+/*
+ * La procedure "push_vwstack" empile et duplique le point de vue au sommet
+ * de la pile des points de vue.
+ */
+void
+push_vwstack (void)
+{
+ static char proc_name[] = "push_vwstack";
+
+ if (sp == stack + STACKSIZE - 1) {
+ fprintf (stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ *sp = *(sp - 1);
+}
+
+/*
+ * La procedure "swap_vwstack" echange les deux premiers elements
+ * de la pile des points de vue.
+ */
+void
+swap_vwstack (void)
+{
+ View_parameters *vp, tmp;
+
+ vp = (sp == stack) ? sp + 1 : sp - 1;
+ SWAP(*sp, *vp, tmp);
+}
+
+/*
+ * 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.
+ */
+
+void
+add_vwstack (const char* path, ... )
+//add_vwstack (va_alist)
+// va_dcl
+{
+ static char proc_name[] = "add_vwstack";
+
+ va_list ap;
+ char *argv;
+
+ va_start (ap,path);
+ argv = va_arg (ap, char *);
+ if (strcmp (argv, "cop") == 0) {
+ /* 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 */
+ 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 */
+ sp->type = (Type) va_arg (ap, int);
+ }
+ else if (strcmp (argv, "vpn") == 0) {
+ /* 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);
+
+ //if (x == 0 && y == 0 && z == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon())
+ fprintf (stderr, "%s: bad vpn\n", proc_name);
+ else {
+ SET_COORD3(sp->vpn,x,y,z);
+ }
+ }
+ else if (strcmp (argv, "vrp") == 0) {
+ /* 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 */
+ float x = (float) va_arg (ap, double);
+ float y = (float) va_arg (ap, double);
+ float z = (float) va_arg (ap, double);
+
+ //if (x == 0 && y == 0 && z == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon())
+ fprintf (stderr, "%s: bad vup\n", proc_name);
+ else {
+ SET_COORD3(sp->vup,x,y,z);
+ }
+ }
+ else if (strcmp (argv, "window") == 0) {
+ /* 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);
+ sp->vwd.vmax = (float) va_arg (ap, double);
+ }
+ else
+ fprintf (stderr, "%s: bad argument\n", proc_name);
+ va_end (ap);
+}
+
+#endif
+
diff --git a/src/simulator/wireframe-simulator/scene/3pts.bnd b/modules/robot/src/wireframe-simulator/scene/3pts.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/3pts.bnd
rename to modules/robot/src/wireframe-simulator/scene/3pts.bnd
diff --git a/src/simulator/wireframe-simulator/scene/3pts.sld b/modules/robot/src/wireframe-simulator/scene/3pts.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/3pts.sld
rename to modules/robot/src/wireframe-simulator/scene/3pts.sld
diff --git a/src/simulator/wireframe-simulator/scene/camera.bnd b/modules/robot/src/wireframe-simulator/scene/camera.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/camera.bnd
rename to modules/robot/src/wireframe-simulator/scene/camera.bnd
diff --git a/src/simulator/wireframe-simulator/scene/camera.sld b/modules/robot/src/wireframe-simulator/scene/camera.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/camera.sld
rename to modules/robot/src/wireframe-simulator/scene/camera.sld
diff --git a/src/simulator/wireframe-simulator/scene/circle.bnd b/modules/robot/src/wireframe-simulator/scene/circle.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circle.bnd
rename to modules/robot/src/wireframe-simulator/scene/circle.bnd
diff --git a/src/simulator/wireframe-simulator/scene/circle.sld b/modules/robot/src/wireframe-simulator/scene/circle.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circle.sld
rename to modules/robot/src/wireframe-simulator/scene/circle.sld
diff --git a/src/simulator/wireframe-simulator/scene/circle_sq2.bnd b/modules/robot/src/wireframe-simulator/scene/circle_sq2.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circle_sq2.bnd
rename to modules/robot/src/wireframe-simulator/scene/circle_sq2.bnd
diff --git a/src/simulator/wireframe-simulator/scene/circle_sq2.sld b/modules/robot/src/wireframe-simulator/scene/circle_sq2.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circle_sq2.sld
rename to modules/robot/src/wireframe-simulator/scene/circle_sq2.sld
diff --git a/src/simulator/wireframe-simulator/scene/circles2.bnd b/modules/robot/src/wireframe-simulator/scene/circles2.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circles2.bnd
rename to modules/robot/src/wireframe-simulator/scene/circles2.bnd
diff --git a/src/simulator/wireframe-simulator/scene/circles2.sld b/modules/robot/src/wireframe-simulator/scene/circles2.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/circles2.sld
rename to modules/robot/src/wireframe-simulator/scene/circles2.sld
diff --git a/src/simulator/wireframe-simulator/scene/cube.bnd b/modules/robot/src/wireframe-simulator/scene/cube.bnd
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/cube.bnd
rename to modules/robot/src/wireframe-simulator/scene/cube.bnd
diff --git a/src/simulator/wireframe-simulator/scene/cube.sld b/modules/robot/src/wireframe-simulator/scene/cube.sld
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/cube.sld
rename to modules/robot/src/wireframe-simulator/scene/cube.sld
diff --git a/src/simulator/wireframe-simulator/scene/cylinder.bnd b/modules/robot/src/wireframe-simulator/scene/cylinder.bnd
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/cylinder.bnd
rename to modules/robot/src/wireframe-simulator/scene/cylinder.bnd
diff --git a/src/simulator/wireframe-simulator/scene/cylinder.sld b/modules/robot/src/wireframe-simulator/scene/cylinder.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/cylinder.sld
rename to modules/robot/src/wireframe-simulator/scene/cylinder.sld
diff --git a/src/simulator/wireframe-simulator/scene/diamond.bnd b/modules/robot/src/wireframe-simulator/scene/diamond.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/diamond.bnd
rename to modules/robot/src/wireframe-simulator/scene/diamond.bnd
diff --git a/src/simulator/wireframe-simulator/scene/diamond.sld b/modules/robot/src/wireframe-simulator/scene/diamond.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/diamond.sld
rename to modules/robot/src/wireframe-simulator/scene/diamond.sld
diff --git a/src/simulator/wireframe-simulator/scene/line.bnd b/modules/robot/src/wireframe-simulator/scene/line.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/line.bnd
rename to modules/robot/src/wireframe-simulator/scene/line.bnd
diff --git a/src/simulator/wireframe-simulator/scene/line.sld b/modules/robot/src/wireframe-simulator/scene/line.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/line.sld
rename to modules/robot/src/wireframe-simulator/scene/line.sld
diff --git a/src/simulator/wireframe-simulator/scene/pipe.bnd b/modules/robot/src/wireframe-simulator/scene/pipe.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/pipe.bnd
rename to modules/robot/src/wireframe-simulator/scene/pipe.bnd
diff --git a/src/simulator/wireframe-simulator/scene/pipe.sld b/modules/robot/src/wireframe-simulator/scene/pipe.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/pipe.sld
rename to modules/robot/src/wireframe-simulator/scene/pipe.sld
diff --git a/src/simulator/wireframe-simulator/scene/plan.bnd b/modules/robot/src/wireframe-simulator/scene/plan.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plan.bnd
rename to modules/robot/src/wireframe-simulator/scene/plan.bnd
diff --git a/src/simulator/wireframe-simulator/scene/plan.sld b/modules/robot/src/wireframe-simulator/scene/plan.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plan.sld
rename to modules/robot/src/wireframe-simulator/scene/plan.sld
diff --git a/src/simulator/wireframe-simulator/scene/plate.bnd b/modules/robot/src/wireframe-simulator/scene/plate.bnd
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate.bnd
rename to modules/robot/src/wireframe-simulator/scene/plate.bnd
diff --git a/src/simulator/wireframe-simulator/scene/plate.sld b/modules/robot/src/wireframe-simulator/scene/plate.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate.sld
rename to modules/robot/src/wireframe-simulator/scene/plate.sld
diff --git a/src/simulator/wireframe-simulator/scene/plate_6cm.bnd b/modules/robot/src/wireframe-simulator/scene/plate_6cm.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate_6cm.bnd
rename to modules/robot/src/wireframe-simulator/scene/plate_6cm.bnd
diff --git a/src/simulator/wireframe-simulator/scene/plate_6cm.sld b/modules/robot/src/wireframe-simulator/scene/plate_6cm.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate_6cm.sld
rename to modules/robot/src/wireframe-simulator/scene/plate_6cm.sld
diff --git a/src/simulator/wireframe-simulator/scene/plate_8cm.bnd b/modules/robot/src/wireframe-simulator/scene/plate_8cm.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate_8cm.bnd
rename to modules/robot/src/wireframe-simulator/scene/plate_8cm.bnd
diff --git a/src/simulator/wireframe-simulator/scene/plate_8cm.sld b/modules/robot/src/wireframe-simulator/scene/plate_8cm.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/plate_8cm.sld
rename to modules/robot/src/wireframe-simulator/scene/plate_8cm.sld
diff --git a/src/simulator/wireframe-simulator/scene/point.bnd b/modules/robot/src/wireframe-simulator/scene/point.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/point.bnd
rename to modules/robot/src/wireframe-simulator/scene/point.bnd
diff --git a/src/simulator/wireframe-simulator/scene/point.sld b/modules/robot/src/wireframe-simulator/scene/point.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/point.sld
rename to modules/robot/src/wireframe-simulator/scene/point.sld
diff --git a/src/simulator/wireframe-simulator/scene/point_cloud.bnd b/modules/robot/src/wireframe-simulator/scene/point_cloud.bnd
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/point_cloud.bnd
rename to modules/robot/src/wireframe-simulator/scene/point_cloud.bnd
diff --git a/src/simulator/wireframe-simulator/scene/point_cloud.sld b/modules/robot/src/wireframe-simulator/scene/point_cloud.sld
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/point_cloud.sld
rename to modules/robot/src/wireframe-simulator/scene/point_cloud.sld
diff --git a/src/simulator/wireframe-simulator/scene/rectangle.bnd b/modules/robot/src/wireframe-simulator/scene/rectangle.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/rectangle.bnd
rename to modules/robot/src/wireframe-simulator/scene/rectangle.bnd
diff --git a/src/simulator/wireframe-simulator/scene/rectangle.sld b/modules/robot/src/wireframe-simulator/scene/rectangle.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/rectangle.sld
rename to modules/robot/src/wireframe-simulator/scene/rectangle.sld
diff --git a/src/simulator/wireframe-simulator/scene/road.bnd b/modules/robot/src/wireframe-simulator/scene/road.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/road.bnd
rename to modules/robot/src/wireframe-simulator/scene/road.bnd
diff --git a/src/simulator/wireframe-simulator/scene/road.sld b/modules/robot/src/wireframe-simulator/scene/road.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/road.sld
rename to modules/robot/src/wireframe-simulator/scene/road.sld
diff --git a/src/simulator/wireframe-simulator/scene/sphere.bnd b/modules/robot/src/wireframe-simulator/scene/sphere.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/sphere.bnd
rename to modules/robot/src/wireframe-simulator/scene/sphere.bnd
diff --git a/src/simulator/wireframe-simulator/scene/sphere.sld b/modules/robot/src/wireframe-simulator/scene/sphere.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/sphere.sld
rename to modules/robot/src/wireframe-simulator/scene/sphere.sld
diff --git a/src/simulator/wireframe-simulator/scene/square10cm.bnd b/modules/robot/src/wireframe-simulator/scene/square10cm.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/square10cm.bnd
rename to modules/robot/src/wireframe-simulator/scene/square10cm.bnd
diff --git a/src/simulator/wireframe-simulator/scene/square10cm.sld b/modules/robot/src/wireframe-simulator/scene/square10cm.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/square10cm.sld
rename to modules/robot/src/wireframe-simulator/scene/square10cm.sld
diff --git a/src/simulator/wireframe-simulator/scene/tool.bnd b/modules/robot/src/wireframe-simulator/scene/tool.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/tool.bnd
rename to modules/robot/src/wireframe-simulator/scene/tool.bnd
diff --git a/src/simulator/wireframe-simulator/scene/tool.sld b/modules/robot/src/wireframe-simulator/scene/tool.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/tool.sld
rename to modules/robot/src/wireframe-simulator/scene/tool.sld
diff --git a/src/simulator/wireframe-simulator/scene/trapezoid.bnd b/modules/robot/src/wireframe-simulator/scene/trapezoid.bnd
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/trapezoid.bnd
rename to modules/robot/src/wireframe-simulator/scene/trapezoid.bnd
diff --git a/src/simulator/wireframe-simulator/scene/trapezoid.sld b/modules/robot/src/wireframe-simulator/scene/trapezoid.sld
old mode 100755
new mode 100644
similarity index 100%
rename from src/simulator/wireframe-simulator/scene/trapezoid.sld
rename to modules/robot/src/wireframe-simulator/scene/trapezoid.sld
diff --git a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp
new file mode 100644
index 0000000..4481f7a
--- /dev/null
+++ b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp
@@ -0,0 +1,1860 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Wire frame simulator
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpWireFrameSimulator.cpp
+ \brief Implementation of a wire frame simulator.
+*/
+
+#include <visp3/robot/vpWireFrameSimulator.h>
+#include <fcntl.h>
+#include <string.h>
+#include <stdio.h>
+#include <vector>
+
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpIoTools.h>
+
+//Inventor includes
+#if defined(VISP_HAVE_COIN3D)
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/actions/SoSearchAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/actions/SoGetMatrixAction.h>
+#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#endif
+
+extern Point2i *point2i;
+extern Point2i *listpoint2i;
+
+typedef enum
+{
+ BND_MODEL,
+ WRL_MODEL,
+ UNKNOWN_MODEL
+} Model_3D;
+
+Model_3D getExtension(const char* file);
+void set_scene_wrl (const char* str, Bound_scene *sc, float factor);
+
+/*
+ Get the extension of the file and return it
+*/
+Model_3D
+getExtension(const char* file)
+{
+ std::string sfilename(file);
+
+ size_t bnd = sfilename.find("bnd");
+ size_t BND = sfilename.find("BND");
+ size_t wrl = sfilename.find("wrl");
+ size_t WRL = sfilename.find("WRL");
+
+ size_t size = sfilename.size();
+
+ if ((bnd>0 && bnd<size ) || (BND>0 && BND<size))
+ return BND_MODEL;
+ else if ((wrl>0 && wrl<size) || ( WRL>0 && WRL<size))
+ {
+#if defined(VISP_HAVE_COIN3D)
+ return WRL_MODEL;
+#else
+ std::cout << "Coin not installed, cannot read VRML files" << std::endl;
+ throw std::string("Coin not installed, cannot read VRML files");
+#endif
+ }
+ return UNKNOWN_MODEL;
+}
+
+/*
+ Enable to initialize the scene
+*/
+void set_scene (const char* str, Bound_scene *sc, float factor)
+{
+ FILE *fd;
+
+ //if ((fd = fopen (str, 0)) == -1)
+ if ((fd = fopen (str, "r")) == NULL)
+ {
+ std::string error = "The file " + std::string(str) + " can not be opened";
+
+ throw(vpException(vpException::ioError, error.c_str())) ;
+ }
+ open_keyword (keyword_tbl);
+ open_lex ();
+ open_source (fd, str);
+ malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
+ parser (sc);
+
+ //if (factor != 1)
+ if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
+ {
+ for (int i = 0; i < sc->bound.nbr; i++)
+ {
+ for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
+ {
+ sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
+ sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
+ sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
+ }
+ }
+ }
+
+ close_source ();
+ close_lex ();
+ close_keyword ();
+ fclose(fd);
+}
+
+#if defined(VISP_HAVE_COIN3D)
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+typedef struct indexFaceSet
+{
+ indexFaceSet() : nbPt(0), pt(), nbIndex(0), index() {};
+ int nbPt;
+ std::vector<vpPoint> pt;
+ int nbIndex;
+ std::vector<int> index;
+} indexFaceSet;
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+
+void extractFaces(SoVRMLIndexedFaceSet*, indexFaceSet *ifs);
+void ifsToBound (Bound*, std::list<indexFaceSet*> &);
+void destroyIfs(std::list<indexFaceSet*> &);
+
+
+void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
+{
+ //Load the sceneGraph
+ SoDB::init();
+ SoInput in;
+ SbBool ok = in.openFile(str);
+ SoSeparator *sceneGraph;
+ SoVRMLGroup *sceneGraphVRML2;
+
+ if (!ok) {
+ vpERROR_TRACE("can't open file \"%s\" \n Please check the Marker_Less.ini file", str);
+ exit(1);
+ }
+
+ if(!in.isFileVRML2())
+ {
+ sceneGraph = SoDB::readAll(&in);
+ if (sceneGraph == NULL) { /*return -1;*/ }
+ sceneGraph->ref();
+
+ SoToVRML2Action tovrml2;
+ tovrml2.apply(sceneGraph);
+ sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
+ sceneGraphVRML2->ref();
+ sceneGraph->unref();
+ }
+ else
+ {
+ sceneGraphVRML2 = SoDB::readAllVRML(&in);
+ if (sceneGraphVRML2 == NULL) {
+ /*return -1;*/
+ throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
+ }
+ sceneGraphVRML2->ref();
+ }
+
+ in.closeFile();
+
+ int nbShapes = sceneGraphVRML2->getNumChildren();
+
+ SoNode * child;
+
+ malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
+
+ int iterShapes = 0;
+ for (int i = 0; i < nbShapes; i++)
+ {
+ int nbFaces = 0;
+ child = sceneGraphVRML2->getChild(i);
+ if (child->getTypeId() == SoVRMLShape::getClassTypeId())
+ {
+ std::list<indexFaceSet*> ifs_list;
+ SoChildList * child2list = child->getChildren();
+ for (int j = 0; j < child2list->getLength(); j++)
+ {
+ if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
+ {
+ indexFaceSet *ifs = new indexFaceSet;
+ SoVRMLIndexedFaceSet * face_set;
+ face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
+ extractFaces(face_set,ifs);
+ ifs_list.push_back(ifs);
+ nbFaces++;
+ }
+// if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
+// {
+// std::cout << "> We found a line" << std::endl;
+// SoVRMLIndexedLineSet * line_set;
+// line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
+// extractLines(line_set);
+// }
+ }
+ sc->bound.nbr++;
+ ifsToBound (&(sc->bound.ptr[iterShapes]), ifs_list);
+ destroyIfs(ifs_list);
+ iterShapes++;
+ }
+ }
+
+ //if (factor != 1)
+ if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
+ {
+ for (int i = 0; i < sc->bound.nbr; i++)
+ {
+ for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
+ {
+ sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
+ sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
+ sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
+ }
+ }
+ }
+}
+
+
+void
+extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
+{
+// vpList<vpPoint> pointList;
+// pointList.kill();
+ SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
+ int coordSize = coord->point.getNum();
+
+ ifs->nbPt = coordSize;
+ for (int i = 0; i < coordSize; i++)
+ {
+ SbVec3f point(0,0,0);
+ point[0]=coord->point[i].getValue()[0];
+ point[1]=coord->point[i].getValue()[1];
+ point[2]=coord->point[i].getValue()[2];
+ vpPoint pt(point[0],point[1],point[2]);
+ ifs->pt.push_back(pt);
+ }
+
+ SoMFInt32 indexList = face_set->coordIndex;
+ int indexListSize = indexList.getNum();
+
+ ifs->nbIndex = indexListSize;
+ for (int i = 0; i < indexListSize; i++)
+ {
+ int index = face_set->coordIndex[i];
+ ifs->index.push_back(index);
+ }
+}
+
+void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
+{
+ int nbPt = 0;
+ for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
+ nbPt += (*it)->nbPt;
+ }
+ bptr->point.nbr = (Index)nbPt;
+ bptr->point.ptr = (Point3f *) malloc ((unsigned int)nbPt * sizeof (Point3f));
+
+ ifs_list.front();
+ unsigned int iter = 0;
+ for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
+ {
+ indexFaceSet* ifs = *it;
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++)
+ {
+ bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
+ bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
+ bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
+ iter++;
+ }
+ }
+
+ unsigned int nbFace = 0;
+ ifs_list.front();
+ std::list<int> indSize;
+ int indice = 0;
+ for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
+ {
+ indexFaceSet* ifs = *it;
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
+ {
+ if(ifs->index[j] == -1)
+ {
+ nbFace++;
+ indSize.push_back(indice);
+ indice = 0;
+ }
+ else indice++;
+ }
+ }
+
+ bptr->face.nbr = (Index)nbFace;
+ bptr->face.ptr = (Face *) malloc (nbFace * sizeof (Face));
+
+
+ std::list<int>::const_iterator iter_indSize = indSize.begin();
+ for (unsigned int i = 0; i < indSize.size(); i++)
+ {
+ bptr->face.ptr[i].vertex.nbr = (Index)*iter_indSize;
+ bptr->face.ptr[i].vertex.ptr = (Index *) malloc ((unsigned int)*iter_indSize * sizeof (Index));
+ ++iter_indSize;
+ }
+
+ int offset = 0;
+ indice = 0;
+ for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
+ {
+ indexFaceSet* ifs = *it;
+ iter = 0;
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
+ {
+ if(ifs->index[j] != -1)
+ {
+ bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
+ iter++;
+ }
+ else
+ {
+ iter = 0;
+ indice++;
+ }
+ }
+ offset += ifs->nbPt;
+ }
+}
+
+void destroyIfs(std::list<indexFaceSet*> &ifs_list)
+{
+ for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
+ delete *it;
+ }
+ ifs_list.clear();
+}
+#else
+void set_scene_wrl (const char* /*str*/, Bound_scene* /*sc*/, float /*factor*/)
+{
+}
+#endif
+
+
+/*
+ Convert the matrix format to deal with the one in the simulator
+*/
+void vp2jlc_matrix (const vpHomogeneousMatrix vpM, Matrix &jlcM)
+{
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 0; j < 4; j++)
+ jlcM[j][i] = (float)vpM[i][j];
+ }
+}
+
+/*
+ Copy the scene corresponding to the registeresd parameters in the image.
+*/
+void
+vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
+{
+ // extern Bound *clipping_Bound ();
+ Bound *bp, *bend;
+ Bound *clip; /* surface apres clipping */
+ Byte b = (Byte) *get_rfstack ();
+ Matrix m;
+
+ //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
+ memmove((char *) m, (char *) mat, sizeof (Matrix));
+ View_to_Matrix (get_vwstack (), *(get_tmstack ()));
+ postmult_matrix (m, *(get_tmstack ()));
+ bp = sc.bound.ptr;
+ bend = bp + sc.bound.nbr;
+ for (; bp < bend; bp++)
+ {
+ if ((clip = clipping_Bound (bp, m)) != NULL)
+ {
+ Face *fp = clip->face.ptr;
+ Face *fend = fp + clip->face.nbr;
+
+ set_Bound_face_display (clip, b); //regarde si is_visible
+
+ point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
+ for (; fp < fend; fp++)
+ {
+ if (fp->is_visible)
+ {
+ wireframe_Face (fp, point2i);
+ Point2i *pt = listpoint2i;
+ for (int i = 1; i < fp->vertex.nbr; i++)
+ {
+ vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
+ pt++;
+ }
+ if (fp->vertex.nbr > 2)
+ {
+ vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
+ }
+ }
+ }
+ }
+ }
+}
+
+/*
+ Copy the scene corresponding to the registeresd parameters in the image.
+*/
+void
+vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color)
+{
+ //extern Bound *clipping_Bound ();
+
+ Bound *bp, *bend;
+ Bound *clip; /* surface apres clipping */
+ Byte b = (Byte) *get_rfstack ();
+ Matrix m;
+
+ //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
+ memmove((char *) m, (char *) mat, sizeof (Matrix));
+ View_to_Matrix (get_vwstack (), *(get_tmstack ()));
+ postmult_matrix (m, *(get_tmstack ()));
+ bp = sc.bound.ptr;
+ bend = bp + sc.bound.nbr;
+ for (; bp < bend; bp++)
+ {
+ if ((clip = clipping_Bound (bp, m)) != NULL)
+ {
+ Face *fp = clip->face.ptr;
+ Face *fend = fp + clip->face.nbr;
+
+ set_Bound_face_display (clip, b); //regarde si is_visible
+
+ point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
+ for (; fp < fend; fp++)
+ {
+ if (fp->is_visible)
+ {
+ wireframe_Face (fp, point2i);
+ Point2i *pt = listpoint2i;
+ for (int i = 1; i < fp->vertex.nbr; i++)
+ {
+ vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
+ pt++;
+ }
+ if (fp->vertex.nbr > 2)
+ {
+ vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
+ }
+ }
+ }
+ }
+ }
+}
+
+/*************************************************************************************************************/
+
+/*!
+ Basic constructor.
+
+ Set the path to the scene files (*.bnd and *.sln) used by the
+ simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
+ not valid, the path is set from the VISP_SCENES_DIR environment
+ variable that the user has to set.
+*/
+vpWireFrameSimulator::vpWireFrameSimulator()
+ : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(),
+ refMo(), cMo(), cdMo(), object(PLATE), desiredObject(D_STANDARD),
+ camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
+ desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true),
+ cameraTrajectory(), poseList(), fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(),
+ old_iPt(), blockedr(false), blockedz(false), blockedt(false), blocked(false),
+ camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
+ displayDesiredObject(false), displayCamera(false), displayImageSimulator(false),
+ cameraFactor(1.), camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
+{
+ // set scene_dir from #define VISP_SCENE_DIR if it exists
+ // VISP_SCENES_DIR may contain multiple locations separated by ";"
+ std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
+ bool sceneDirExists = false;
+ for(size_t i=0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (! sceneDirExists) {
+ try {
+ scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
+ std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
+ }
+ catch (...) {
+ std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
+ }
+ }
+
+ open_display();
+ open_clipping();
+
+ old_iPr = vpImagePoint(-1,-1);
+ old_iPz = vpImagePoint(-1,-1);
+ old_iPt = vpImagePoint(-1,-1);
+
+ rotz.buildFrom(0,0,0,0,0,vpMath::rad(180));
+
+ scene.name = NULL;
+ scene.bound.ptr = NULL;
+ scene.bound.nbr = 0;
+
+ desiredScene.name = NULL;
+ desiredScene.bound.ptr = NULL;
+ desiredScene.bound.nbr = 0;
+
+ camera.name = NULL;
+ camera.bound.ptr = NULL;
+ camera.bound.nbr = 0;
+}
+
+
+/*!
+ Basic destructor
+*/
+vpWireFrameSimulator::~vpWireFrameSimulator()
+{
+ if(sceneInitialized)
+ {
+ if(displayObject)
+ free_Bound_scene (&(this->scene));
+ if(displayCamera){
+ free_Bound_scene (&(this->camera));
+ }
+ if(displayDesiredObject)
+ free_Bound_scene (&(this->desiredScene));
+ }
+ close_clipping();
+ close_display ();
+
+ cameraTrajectory.clear();
+ poseList.clear();
+ fMoList.clear();
+}
+
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current position.
+ \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
+*/
+void
+vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
+{
+ char name_cam[FILENAME_MAX];
+ char name[FILENAME_MAX];
+
+ object = obj;
+ this->desiredObject = desired_object;
+
+ const char *scene_dir_ = scene_dir.c_str();
+ if (strlen(scene_dir_) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the camera name"));
+ }
+
+ strcpy(name_cam, scene_dir_);
+ if (desiredObject != D_TOOL)
+ {
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+ }
+ else
+ {
+ strcat(name_cam,"/tool.bnd");
+ set_scene(name_cam,&(this->camera),1.0);
+ }
+
+ strcpy(name, scene_dir_);
+ switch (obj)
+ {
+ case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
+ case CUBE : { strcat(name, "/cube.bnd"); break; }
+ case PLATE : { strcat(name, "/plate.bnd"); break; }
+ case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
+ case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
+ case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
+ case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
+ case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
+ case THREE_LINES : { strcat(name, "/line.bnd"); break; }
+ case ROAD : { strcat(name, "/road.bnd"); break; }
+ case TIRE : { strcat(name, "/circles2.bnd"); break; }
+ case PIPE : { strcat(name, "/pipe.bnd"); break; }
+ case CIRCLE : { strcat(name, "/circle.bnd"); break; }
+ case SPHERE : { strcat(name, "/sphere.bnd"); break; }
+ case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
+ case PLAN: { strcat(name, "/plan.bnd"); break; }
+ case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
+ }
+ set_scene(name,&(this->scene),1.0);
+
+ scene_dir_ = scene_dir.c_str();
+ if (strlen(scene_dir_) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the desired object name"));
+ }
+
+ switch (desiredObject)
+ {
+ case D_STANDARD : { break; }
+ case D_CIRCLE : {
+ strcpy(name, scene_dir_);
+ strcat(name, "/circle_sq2.bnd");
+ break; }
+ case D_TOOL : {
+ strcpy(name, scene_dir_);
+ strcat(name, "/tool.bnd");
+ break; }
+ }
+ set_scene(name, &(this->desiredScene), 1.0);
+
+ if (obj == PIPE) load_rfstack(IS_INSIDE);
+ else add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+
+ sceneInitialized = true;
+ displayObject = true;
+ displayDesiredObject = true;
+ displayCamera = true;
+ displayImageSimulator = true;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+
+ \param obj : Type of scene used to display the object at the current position.
+ \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
+ \param imObj : A list of vpImageSimulator instances.
+*/
+void
+vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, const std::list<vpImageSimulator> &imObj)
+{
+ initScene(obj, desired_object);
+ objectImage = imObj;
+ displayImageSimulator = true;
+}
+
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+
+ \param obj : Path to the scene file you want to use.
+ \param desired_object : Path to the scene file you want to use.
+*/
+void
+vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
+{
+ char name_cam[FILENAME_MAX];
+ char name[FILENAME_MAX];
+
+ object = THREE_PTS;
+ this->desiredObject = D_STANDARD;
+
+ const char *scene_dir_ = scene_dir.c_str();
+ if (strlen(scene_dir_) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the camera name"));
+ }
+
+ strcpy(name_cam, scene_dir_);
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+
+ if (strlen(obj) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the name"));
+ }
+
+ strcpy(name,obj);
+ Model_3D model;
+ model = getExtension(obj);
+ if (model == BND_MODEL)
+ set_scene(name,&(this->scene),1.0);
+ else if (model == WRL_MODEL)
+ set_scene_wrl(name,&(this->scene),1.0);
+ else if (model == UNKNOWN_MODEL)
+ {
+ vpERROR_TRACE("Unknown file extension for the 3D model");
+ }
+
+ if (strlen(desired_object) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the camera name"));
+ }
+
+ strcpy(name,desired_object);
+ model = getExtension(desired_object);
+ if (model == BND_MODEL)
+ set_scene(name,&(this->desiredScene),1.0);
+ else if (model == WRL_MODEL)
+ set_scene_wrl(name,&(this->desiredScene),1.0);
+ else if (model == UNKNOWN_MODEL)
+ {
+ vpERROR_TRACE("Unknown file extension for the 3D model");
+ }
+
+ add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+
+ sceneInitialized = true;
+ displayObject = true;
+ displayDesiredObject = true;
+ displayCamera = true;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position and at the desired position.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+
+ It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+
+ \param obj : Path to the scene file you want to use.
+ \param desired_object : Path to the scene file you want to use.
+ \param imObj : A list of vpImageSimulator instances.
+*/
+void
+vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, const std::list<vpImageSimulator> &imObj)
+{
+ initScene(obj, desired_object);
+ objectImage = imObj;
+ displayImageSimulator = true;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of object which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current position.
+*/
+void
+vpWireFrameSimulator::initScene(const vpSceneObject &obj)
+{
+ char name_cam[FILENAME_MAX];
+ char name[FILENAME_MAX];
+
+ object = obj;
+
+ const char *scene_dir_ = scene_dir.c_str();
+ if (strlen(scene_dir_) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the camera name"));
+ }
+
+ strcpy(name_cam, scene_dir_);
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+
+ strcpy(name, scene_dir_);
+ switch (obj)
+ {
+ case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
+ case CUBE : { strcat(name, "/cube.bnd"); break; }
+ case PLATE : { strcat(name, "/plate.bnd"); break; }
+ case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
+ case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
+ case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
+ case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
+ case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
+ case THREE_LINES : { strcat(name, "/line.bnd"); break; }
+ case ROAD : { strcat(name, "/road.bnd"); break; }
+ case TIRE : { strcat(name, "/circles2.bnd"); break; }
+ case PIPE : { strcat(name, "/pipe.bnd"); break; }
+ case CIRCLE : { strcat(name, "/circle.bnd"); break; }
+ case SPHERE : { strcat(name, "/sphere.bnd"); break; }
+ case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
+ case PLAN: { strcat(name, "/plan.bnd"); break; }
+ case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
+ }
+ set_scene(name,&(this->scene),1.0);
+
+ if (obj == PIPE) load_rfstack(IS_INSIDE);
+ else add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+
+ sceneInitialized = true;
+ displayObject = true;
+ displayCamera = true;
+
+ displayDesiredObject = false;
+ displayImageSimulator = false;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of object which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+
+ It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+
+ \param obj : Type of scene used to display the object at the current position.
+ \param imObj : A list of vpImageSimulator instances.
+*/
+void
+vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
+{
+ initScene(obj);
+ objectImage = imObj;
+ displayImageSimulator = true;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+
+ \param obj : Path to the scene file you want to use.
+*/
+void
+vpWireFrameSimulator::initScene(const char* obj)
+{
+ char name_cam[FILENAME_MAX];
+ char name[FILENAME_MAX];
+
+ object = THREE_PTS;
+
+ const char *scene_dir_ = scene_dir.c_str();
+ if (strlen(scene_dir_) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the camera name"));
+ }
+
+ strcpy(name_cam, scene_dir_);
+ strcat(name_cam,"/camera.bnd");
+ set_scene(name_cam,&camera,cameraFactor);
+
+ if (strlen(obj) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the name"));
+ }
+
+ strcpy(name,obj);
+ Model_3D model;
+ model = getExtension(obj);
+ if (model == BND_MODEL)
+ set_scene(name,&(this->scene),1.0);
+ else if (model == WRL_MODEL)
+ set_scene_wrl(name,&(this->scene),1.0);
+ else if (model == UNKNOWN_MODEL)
+ {
+ vpERROR_TRACE("Unknown file extension for the 3D model");
+ }
+
+ add_rfstack(IS_BACK);
+
+ add_vwstack ("start","depth", 0.0, 100.0);
+ add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
+ add_vwstack ("start","type", PERSPECTIVE);
+
+ sceneInitialized = true;
+ displayObject = true;
+ displayCamera = true;
+}
+
+/*!
+ Initialize the simulator. It enables to choose the type of scene which will be used to display the object
+ at the current position. The object at the desired position is not displayed.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+
+ It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+
+ \param obj : Path to the scene file you want to use.
+ \param imObj : A list of vpImageSimulator instances.
+*/
+void
+vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulator> &imObj)
+{
+ initScene(obj);
+ objectImage = imObj;
+ displayImageSimulator = true;
+}
+
+/*!
+ Get the internal view ie the view of the camera.
+
+ \param I : The image where the internal view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &I)
+{
+ if (!sceneInitialized)
+ throw(vpException(vpException::notInitialized,"The scene has to be initialized")) ;
+
+ double u;
+ double v;
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_int);
+ v = (double)I.getHeight()/(2*py_int);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ float o44c[4][4],o44cd[4][4],x,y,z;
+ Matrix id = IDENTITY_MATRIX;
+
+ vp2jlc_matrix(cMo.inverse(),o44c);
+ vp2jlc_matrix(cdMo.inverse(),o44cd);
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*cMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ x = o44c[2][0] + o44c[3][0];
+ y = o44c[2][1] + o44c[3][1];
+ z = o44c[2][2] + o44c[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
+ add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayObject)
+ display_scene(id,this->scene,I, curColor);
+
+
+ add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ x = o44cd[2][0] + o44cd[3][0];
+ y = o44cd[2][1] + o44cd[3][1];
+ z = o44cd[2][2] + o44cd[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
+ add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayDesiredObject)
+ {
+ if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
+ else display_scene(id,desiredScene,I, desColor);
+ }
+}
+
+
+/*!
+ Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
+
+ \param I : The image where the external view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+
+void
+vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
+{
+ bool changed = false;
+ vpHomogeneousMatrix displacement = navigation(I,changed);
+
+ //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ camMf2 = camMf2*displacement;
+
+ f2Mf = camMf2.inverse()*camMf;
+
+ camMf = camMf2* displacement * f2Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_ext);
+ v = (double)I.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+
+ vp2jlc_matrix(camMf.inverse(),w44cext);
+ vp2jlc_matrix(fMc,w44c);
+ vp2jlc_matrix(fMo,w44o);
+
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if ((object == CUBE) || (object == SPHERE))
+ {
+ add_vwstack ("start","type", PERSPECTIVE);
+ }
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*camMf*fMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ if (displayObject)
+ display_scene(w44o,this->scene,I, curColor);
+
+ if (displayCamera)
+ display_scene(w44c,camera, I, camColor);
+
+ if (displayCameraTrajectory)
+ {
+ vpImagePoint iP;
+ vpImagePoint iP_1;
+ poseList.push_back(cMo);
+ fMoList.push_back(fMo);
+
+ int iter = 0;
+
+ if (changed || extCamChanged)
+ {
+ cameraTrajectory.clear();
+ std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
+ std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
+
+ while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end()))
+ {
+ iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
+ cameraTrajectory.push_back(iP);
+ if (camTrajType == CT_LINE)
+ {
+ if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
+ }
+ else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I,iP,camTrajColor);
+ ++iter_poseList;
+ ++iter_fMoList;
+ iter++;
+ iP_1 = iP;
+ }
+ extCamChanged = false;
+ }
+ else
+ {
+ iP = projectCameraTrajectory(I, cMo, fMo);
+ cameraTrajectory.push_back(iP);
+
+ for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
+ if (camTrajType == CT_LINE)
+ {
+ if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
+ }
+ else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, *it, camTrajColor);
+ iter++;
+ iP_1 = *it;
+ }
+ }
+
+ if (poseList.size() > nbrPtLimit)
+ {
+ poseList.pop_front();
+ }
+ if (fMoList.size() > nbrPtLimit)
+ {
+ fMoList.pop_front();
+ }
+ if (cameraTrajectory.size() > nbrPtLimit)
+ {
+ cameraTrajectory.pop_front();
+ }
+ }
+}
+
+
+/*!
+ Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
+
+ \param I : The image where the external view is displayed.
+ \param cam_Mf : The pose between the point of view and the fixed world frame.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
+{
+ float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+
+ vpHomogeneousMatrix camMft = rotz * cam_Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_ext);
+ v = (double)I.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ vp2jlc_matrix(camMft.inverse(),w44cext);
+ vp2jlc_matrix(fMo*cMo.inverse(),w44c);
+ vp2jlc_matrix(fMo,w44o);
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*cam_Mf*fMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ if (displayObject)
+ display_scene(w44o,this->scene,I, curColor);
+ if (displayCamera)
+ display_scene(w44c,camera, I, camColor);
+}
+
+
+/*!
+ Get the internal view ie the view of the camera.
+
+ \param I : The image where the internal view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &I)
+{
+ if (!sceneInitialized)
+ throw(vpException(vpException::notInitialized,"The scene has to be initialized")) ;
+
+ double u;
+ double v;
+ //if(px_int != 1 && py_int != 1)
+ // we assume px_int and py_int > 0
+ if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_int);
+ v = (double)I.getHeight()/(2*py_int);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ float o44c[4][4],o44cd[4][4],x,y,z;
+ Matrix id = IDENTITY_MATRIX;
+
+ vp2jlc_matrix(cMo.inverse(),o44c);
+ vp2jlc_matrix(cdMo.inverse(),o44cd);
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*camMf*fMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ x = o44c[2][0] + o44c[3][0];
+ y = o44c[2][1] + o44c[3][1];
+ z = o44c[2][2] + o44c[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
+ add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayObject)
+ display_scene(id,this->scene,I, curColor);
+
+
+ add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ x = o44cd[2][0] + o44cd[3][0];
+ y = o44cd[2][1] + o44cd[3][1];
+ z = o44cd[2][2] + o44cd[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
+ add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if (displayDesiredObject)
+ {
+ if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
+ else display_scene(id,desiredScene,I, desColor);
+ }
+}
+
+
+/*!
+ Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
+
+ \param I : The image where the external view is displayed.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+
+void
+vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
+{
+ bool changed = false;
+ vpHomogeneousMatrix displacement = navigation(I,changed);
+
+ //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ camMf2 = camMf2*displacement;
+
+ f2Mf = camMf2.inverse()*camMf;
+
+ camMf = camMf2* displacement * f2Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_ext);
+ v = (double)I.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+
+ vp2jlc_matrix(camMf.inverse(),w44cext);
+ vp2jlc_matrix(fMc,w44c);
+ vp2jlc_matrix(fMo,w44o);
+
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+ if ((object == CUBE) || (object == SPHERE))
+ {
+ add_vwstack ("start","type", PERSPECTIVE);
+ }
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*camMf*fMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ if (displayObject)
+ display_scene(w44o,this->scene,I, curColor);
+
+ if (displayCamera)
+ display_scene(w44c,camera, I, camColor);
+
+ if (displayCameraTrajectory)
+ {
+ vpImagePoint iP;
+ vpImagePoint iP_1;
+ poseList.push_back(cMo);
+ fMoList.push_back(fMo);
+
+ int iter = 0;
+
+ if (changed || extCamChanged)
+ {
+ cameraTrajectory.clear();
+ std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
+ std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
+
+ while ((iter_poseList!=poseList.end()) && (iter_fMoList!=fMoList.end()) )
+ {
+ iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
+ cameraTrajectory.push_back(iP);
+ //vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
+ if (camTrajType == CT_LINE)
+ {
+ if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
+ }
+ else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I,iP,camTrajColor);
+ ++iter_poseList;
+ ++iter_fMoList;
+ iter++;
+ iP_1 = iP;
+ }
+ extCamChanged = false;
+ }
+ else
+ {
+ iP = projectCameraTrajectory(I, cMo,fMo);
+ cameraTrajectory.push_back(iP);
+
+ for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
+ if (camTrajType == CT_LINE){
+ if (iter != 0) vpDisplay::displayLine(I,iP_1,*it,camTrajColor, thickness_);
+ }
+ else if(camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, *it, camTrajColor);
+ iter++;
+ iP_1 = *it;
+ }
+ }
+
+ if (poseList.size() > nbrPtLimit)
+ {
+ poseList.pop_front();
+ }
+ if (fMoList.size() > nbrPtLimit)
+ {
+ fMoList.pop_front();
+ }
+ if (cameraTrajectory.size() > nbrPtLimit)
+ {
+ cameraTrajectory.pop_front();
+ }
+ }
+}
+
+
+/*!
+ Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
+
+ \param I : The image where the external view is displayed.
+ \param cam_Mf : The pose between the point of view and the fixed world frame.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not modified.
+*/
+void
+vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
+{
+ float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+
+ vpHomogeneousMatrix camMft = rotz * cam_Mf;
+
+ double u;
+ double v;
+ //if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
+ && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
+ {
+ u = (double)I.getWidth()/(2*px_ext);
+ v = (double)I.getHeight()/(2*py_ext);
+ }
+ else
+ {
+ u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ }
+
+ vp2jlc_matrix(camMft.inverse(),w44cext);
+ vp2jlc_matrix(fMo*cMo.inverse(),w44c);
+ vp2jlc_matrix(fMo,w44o);
+
+ add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ x = w44cext[2][0] + w44cext[3][0];
+ y = w44cext[2][1] + w44cext[3][1];
+ z = w44cext[2][2] + w44cext[3][2];
+ add_vwstack ("start","vrp", x,y,z);
+ add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
+ add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
+ add_vwstack ("start","window", -u, u, -v, v);
+
+ if (displayImageSimulator)
+ {
+ I = 255;
+ for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
+ vpImageSimulator* imSim = &(*it);
+ imSim->setCameraPosition(rotz*cam_Mf*fMo);
+ imSim->getImage(I,getInternalCameraParameters(I));
+ }
+ if (I.display != NULL)
+ vpDisplay::display(I);
+ }
+
+ if (displayObject)
+ display_scene(w44o,this->scene,I, curColor);
+ if (displayCamera)
+ display_scene(w44c,camera, I, camColor);
+}
+
+/*!
+ Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
+
+ The two lists must have the same size of homogeneous matrices must have the same size.
+
+ \param I : The image where the trajectory is displayed.
+ \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
+ \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
+ \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
+*/
+void
+vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
+{
+ if (list_cMo.size() != list_fMo.size())
+ throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
+
+ vpImagePoint iP;
+ vpImagePoint iP_1;
+ int iter = 0;
+
+ std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
+ std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
+
+ while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
+ {
+ iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
+ if (camTrajType == CT_LINE)
+ {
+ if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
+ }
+ else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I,iP,camTrajColor);
+ ++it_cMo;
+ ++it_fMo;
+ iter++;
+ iP_1 = iP;
+ }
+}
+
+/*!
+ Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
+
+ The two lists must have the same size of homogeneous matrices must have the same size.
+
+ \param I : The image where the trajectory is displayed.
+ \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
+ \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
+ \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
+*/
+void
+vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
+{
+ if (list_cMo.size() != list_fMo.size())
+ throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
+
+ vpImagePoint iP;
+ vpImagePoint iP_1;
+ int iter = 0;
+
+ std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
+ std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
+
+ while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
+ {
+ iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
+ if (camTrajType == CT_LINE)
+ {
+ if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
+ }
+ else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I,iP,camTrajColor);
+ ++it_cMo;
+ ++it_fMo;
+ iter++;
+ iP_1 = iP;
+ }
+}
+
+
+/*!
+ Enables to change the external camera position.
+*/
+vpHomogeneousMatrix
+vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
+{
+ double width = vpMath::minimum(I.getWidth(),I.getHeight());
+ vpImagePoint iP;
+ vpImagePoint trash;
+ bool clicked = false;
+ bool clickedUp = false;
+ vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
+
+ vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ changed = false;
+
+ //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
+
+ if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
+
+ if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
+
+ if(clicked)
+ {
+ if (b == vpMouseButton::button1) blockedr = true;
+ if (b == vpMouseButton::button2) blockedz = true;
+ if (b == vpMouseButton::button3) blockedt = true;
+ blocked = true;
+ }
+ if(clickedUp)
+ {
+ if (b == vpMouseButton::button1)
+ {
+ old_iPr = vpImagePoint(-1,-1);
+ blockedr = false;
+ }
+ if (b == vpMouseButton::button2)
+ {
+ old_iPz = vpImagePoint(-1,-1);
+ blockedz = false;
+ }
+ if (b == vpMouseButton::button3)
+ {
+ old_iPt = vpImagePoint(-1,-1);
+ blockedt = false;
+ }
+ if (!(blockedr || blockedz || blockedt))
+ {
+ blocked = false;
+ while (vpDisplay::getClick(I,trash,b,false)) {};
+ }
+ }
+
+ vpDisplay::getPointerPosition(I,iP);
+
+ double anglei = 0;
+ double anglej = 0;
+
+ if (old_iPr != vpImagePoint(-1,-1) && blockedr)
+ {
+ double diffi = iP.get_i() - old_iPr.get_i();
+ double diffj = iP.get_j() - old_iPr.get_j();
+ //cout << "delta :" << diffj << endl;;
+ anglei = diffi*360/width;
+ anglej = diffj*360/width;
+ mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
+ changed = true;
+ }
+
+ if (blockedr) old_iPr = iP;
+
+ if (old_iPz != vpImagePoint(-1,-1) && blockedz)
+ {
+ double diffi = iP.get_i() - old_iPz.get_i();
+ mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ changed = true;
+ }
+
+ if (blockedz) old_iPz = iP;
+
+ if (old_iPt != vpImagePoint(-1,-1) && blockedt)
+ {
+ double diffi = iP.get_i() - old_iPt.get_i();
+ double diffj = iP.get_j() - old_iPt.get_j();
+ mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
+ changed = true;
+ }
+
+ if (blockedt) old_iPt = iP;
+
+ return mov;
+}
+
+
+/*!
+ Enables to change the external camera position.
+*/
+vpHomogeneousMatrix
+vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
+{
+ double width = vpMath::minimum(I.getWidth(),I.getHeight());
+ vpImagePoint iP;
+ vpImagePoint trash;
+ bool clicked = false;
+ bool clickedUp = false;
+ vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
+
+ vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ changed = false;
+
+ //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
+
+ if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
+
+ if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
+
+ if(clicked)
+ {
+ if (b == vpMouseButton::button1) blockedr = true;
+ if (b == vpMouseButton::button2) blockedz = true;
+ if (b == vpMouseButton::button3) blockedt = true;
+ blocked = true;
+ }
+ if(clickedUp)
+ {
+ if (b == vpMouseButton::button1)
+ {
+ old_iPr = vpImagePoint(-1,-1);
+ blockedr = false;
+ }
+ if (b == vpMouseButton::button2)
+ {
+ old_iPz = vpImagePoint(-1,-1);
+ blockedz = false;
+ }
+ if (b == vpMouseButton::button3)
+ {
+ old_iPt = vpImagePoint(-1,-1);
+ blockedt = false;
+ }
+ if (!(blockedr || blockedz || blockedt))
+ {
+ blocked = false;
+ while (vpDisplay::getClick(I,trash,b,false)) {};
+ }
+ }
+
+ vpDisplay::getPointerPosition(I,iP);
+
+ //std::cout << "point : " << iP << std::endl;
+
+ double anglei = 0;
+ double anglej = 0;
+
+ if (old_iPr != vpImagePoint(-1,-1) && blockedr)
+ {
+ double diffi = iP.get_i() - old_iPr.get_i();
+ double diffj = iP.get_j() - old_iPr.get_j();
+ //cout << "delta :" << diffj << endl;;
+ anglei = diffi*360/width;
+ anglej = diffj*360/width;
+ mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
+ changed = true;
+ }
+
+ if (blockedr) old_iPr = iP;
+
+ if (old_iPz != vpImagePoint(-1,-1) && blockedz)
+ {
+ double diffi = iP.get_i() - old_iPz.get_i();
+ mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ changed = true;
+ }
+
+ if (blockedz) old_iPz = iP;
+
+ if (old_iPt != vpImagePoint(-1,-1) && blockedt)
+ {
+ double diffi = iP.get_i() - old_iPt.get_i();
+ double diffj = iP.get_j() - old_iPt.get_j();
+ mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
+ changed = true;
+ }
+
+ if (blockedt) old_iPt = iP;
+
+ return mov;
+}
+
+/*!
+ Project the center of the internal camera into the external camera view.
+*/
+vpImagePoint
+vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_)
+{
+ vpPoint point;
+ point.setWorldCoordinates(0,0,0);
+
+ point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
+
+ vpImagePoint iP;
+
+ vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
+
+ return iP;
+}
+
+/*!
+ Project the center of the internal camera into the external camera view.
+*/
+vpImagePoint
+vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_)
+{
+ vpPoint point;
+ point.setWorldCoordinates(0,0,0);
+
+ point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
+
+ vpImagePoint iP;
+
+ vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
+
+ return iP;
+}
+
+/*!
+ Project the center of the internal camera into the external camera view.
+*/
+vpImagePoint
+vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
+{
+ vpPoint point;
+ point.setWorldCoordinates(0,0,0);
+
+ point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
+
+ vpImagePoint iP;
+
+ vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
+
+ return iP;
+}
+
+/*!
+ Project the center of the internal camera into the external camera view.
+*/
+vpImagePoint
+vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
+{
+ vpPoint point;
+ point.setWorldCoordinates(0,0,0);
+
+ point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
+
+ vpImagePoint iP;
+
+ vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
+
+ return iP;
+}
+
diff --git a/modules/robot/test/servo-afma4/testAfma4.cpp b/modules/robot/test/servo-afma4/testAfma4.cpp
new file mode 100644
index 0000000..f59b45f
--- /dev/null
+++ b/modules/robot/test/servo-afma4/testAfma4.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testAfma4.cpp
+
+ Example of a real robot control, the Afma4 robot (cartesian robot, with 6
+ degrees of freedom).
+*/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpAfma4.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <iostream>
+
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpAfma4 class..." << std::endl;
+
+ vpAfma4 afma4;
+
+ std::cout << afma4 << std::endl;
+
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/robot/test/servo-afma4/testRobotAfma4.cpp b/modules/robot/test/servo-afma4/testRobotAfma4.cpp
new file mode 100644
index 0000000..b081fed
--- /dev/null
+++ b/modules/robot/test/servo-afma4/testRobotAfma4.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 4 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobotAfma4.cpp
+
+ Example of a real robot control, the Afma4 robot (cylindrical robot, with 4
+ degrees of freedom).
+*/
+
+#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <iostream>
+
+
+#ifdef VISP_HAVE_AFMA4
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpRobotAfma4 class..." << std::endl;
+
+ vpRobotAfma4 afma4;
+
+ std::cout << afma4 << std::endl;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
+#else
+int main()
+{
+ std::cout << "The real Afma4 robot controller is not available." << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/modules/robot/test/servo-afma6/testAfma6.cpp b/modules/robot/test/servo-afma6/testAfma6.cpp
new file mode 100644
index 0000000..ab17da5
--- /dev/null
+++ b/modules/robot/test/servo-afma6/testAfma6.cpp
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testAfma6.cpp
+
+ Example of a real robot control, the Afma6 robot (cartesian robot, with 6
+ degrees of freedom).
+*/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpAfma6.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <iostream>
+
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpAfma6 class..." << std::endl;
+
+ vpAfma6 afma6;
+ vpCameraParameters cam;
+
+ std::cout << "-- Default settings for Afma6 ---"
+ << std::endl;
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_CCMOP);
+
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the CCMOP tool with distortion ------"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_CCMOP,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the gripper tool without distortion ---"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_GRIPPER);
+
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the gripper tool with distortion ------"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_GRIPPER,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/robot/test/servo-afma6/testRobotAfma6.cpp b/modules/robot/test/servo-afma6/testRobotAfma6.cpp
new file mode 100644
index 0000000..964be6d
--- /dev/null
+++ b/modules/robot/test/servo-afma6/testRobotAfma6.cpp
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobotAfma6.cpp
+
+ Example of a real robot control, the Afma6 robot (cartesian robot, with 6
+ degrees of freedom).
+*/
+
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <iostream>
+
+
+#ifdef VISP_HAVE_AFMA6
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpRobotAfma6 class..." << std::endl;
+
+ vpRobotAfma6 afma6;
+ vpCameraParameters cam;
+
+ std::cout << "-- Default settings for Afma6 ---"
+ << std::endl;
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_CCMOP);
+
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to CCMOP tool with distortion ------"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_CCMOP,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the gripper tool without distortion ---"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_GRIPPER);
+
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to gripper tool with distortion ------"
+ << std::endl;
+ afma6.init( vpAfma6::TOOL_GRIPPER,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << afma6 << std::endl;
+ afma6.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
+#else
+int main()
+{
+ std::cout << "The real Afma6 robot controller is not available." << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp
new file mode 100644
index 0000000..986e26b
--- /dev/null
+++ b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobotAfma6Pose.cpp
+
+ Example of robot pose usage.
+
+ Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
+ rMc from the robot position.
+
+*/
+
+
+
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpDebug.h>
+#include <iostream>
+#if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394)
+
+int main()
+{
+ try {
+ // Create an image B&W container
+ vpImage<unsigned char> I;
+
+ // Create a firewire grabber based on libdc1394-2.x
+ vp1394TwoGrabber g;
+
+ // Grab an image from the firewire camera
+ g.acquire(I);
+
+ // Create an image viewer for the image
+#ifdef VISP_HAVE_X11
+ vpDisplayX display(I,100,100,"Current image") ;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV display(I,100,100,"Current image") ;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK display(I,100,100,"Current image") ;
+#endif
+
+ // Display the image
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+
+ // Define a squared target
+ // The target is made of 4 planar points (square dim = 0.077m)
+ double sdim = 0.077; // square width and height
+ vpPoint target[4] ;
+ // Set the point world coordinates (x,y,z) in the object frame
+ // o ----> x
+ // |
+ // |
+ // \/
+ // y
+ target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
+ target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
+ target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
+ target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
+
+ // Image processing to extract the 2D coordinates in sub-pixels of the 4
+ // points from the image acquired by the camera
+ // Creation of 4 trackers
+ vpDot dot[4];
+ vpImagePoint cog;
+ for (int i=0; i < 4; i ++) {
+ dot[i].setGraphics(true); // to display the tracking results
+ std::cout << "Click on dot " << i << std::endl;
+ dot[i].initTracking( I );
+ // The tracker computes the sub-pixels coordinates in the image
+ // i ----> u
+ // |
+ // |
+ // \/
+ // v
+ std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
+ // Flush the tracking results in the viewer
+ vpDisplay::flush(I) ;
+ }
+
+ // Create an intrinsic camera parameters structure
+ vpCameraParameters cam;
+
+ // Create a robot access
+ vpRobotAfma6 robot;
+
+ // Load the end-effector to camera frame transformation obtained
+ // using a camera intrinsic model with distortion
+ robot.init(vpAfma6::TOOL_CCMOP,
+ vpCameraParameters::perspectiveProjWithDistortion);
+
+ // Get the intrinsic camera parameters associated to the image
+ robot.getCameraParameters(cam, I);
+
+ // Using the camera parameters, compute the perspective projection (transform
+ // the dot sub-pixel coordinates into coordinates in the camera frame in
+ // meter)
+ for (int i=0; i < 4; i ++) {
+ double x=0, y=0 ; // coordinates of the dots in the camera frame
+ // c ----> x
+ // |
+ // |
+ // \/
+ // y
+ //pixel to meter conversion
+ cog = dot[i].getCog();
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
+ target[i].set_x(x) ;
+ target[i].set_y(y) ;
+ }
+
+ // From now, in target[i], we have the 3D coordinates of a point in the
+ // object frame, and their correspondances in the camera frame. We can now
+ // compute the pose cMo between the camera and the object.
+ vpPose pose;
+ // Add the 4 points to compute the pose
+ for (int i=0; i < 4; i ++) {
+ pose.addPoint(target[i]) ;
+ }
+ // Create an homogeneous matrix for the camera to object transformation
+ // computed just bellow
+ vpHomogeneousMatrix cMo;
+ vpRotationMatrix R;
+ vpRxyzVector r;
+ // Compute the pose: initialisation is done by Lagrange method, and the final
+ // pose is computed by the more accurate Virtual Visual Servoing method.
+ pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
+
+
+ std::cout << "Pose cMo: " << std::endl << cMo;
+ cMo.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ // Get the robot position in the reference frame
+ vpHomogeneousMatrix rMc;
+ vpColVector p; // position x,y,z,rx,ry,rz
+ robot.getPosition(vpRobotAfma6::REFERENCE_FRAME, p);
+ std::cout << "Robot pose in reference frame: " << p << std::endl;
+ vpTranslationVector t;
+ t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
+ r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
+ R.buildFrom(r);
+ rMc.buildFrom(t, R);
+ std::cout << "Pose rMc: " << std::endl << rMc;
+ rMc.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ robot.getPosition(vpRobotAfma6::ARTICULAR_FRAME, p);
+ std::cout << "Robot pose in articular: " << p << std::endl;
+
+ robot.get_fMc(p, rMc);
+ std::cout << "Pose rMc from MGD: " << std::endl << rMc;
+ rMc.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ vpHomogeneousMatrix rMo;
+ rMo = rMc * cMo;
+ std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
+ rMo.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
+#else
+int main()
+{
+ std::cout << "Sorry, test not valid. You should have an Afma6 robot..."
+ << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/modules/robot/test/servo-viper/testRobotViper850.cpp b/modules/robot/test/servo-viper/testRobotViper850.cpp
new file mode 100644
index 0000000..ca55c0a
--- /dev/null
+++ b/modules/robot/test/servo-viper/testRobotViper850.cpp
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobotViper850.cpp
+
+ Example of a real robot control, the Viper850 robot (arm, with 6
+ degrees of freedom).
+*/
+
+
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <iostream>
+#ifdef VISP_HAVE_VIPER850
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpRobotViper850 class..." << std::endl;
+
+ vpRobotViper850 viper850;
+ vpCameraParameters cam;
+
+ std::cout << "-- Default settings for Viper850 ---"
+ << std::endl;
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
+ << std::endl;
+ viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
+
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
+ << std::endl;
+ viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+
+ std::cout << "-- Current joint position:" << std::endl;
+ vpColVector q;
+ viper850.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ std::cout << " " << q.t() << std::endl;
+
+ std::cout << "-- Current fMe:" << std::endl;
+
+ vpHomogeneousMatrix fMe;
+ viper850.get_fMe(q, fMe);
+ vpTranslationVector t;
+ fMe.extract(t);
+ vpRotationMatrix R;
+ fMe.extract(R);
+ vpRzyzVector rzyz;
+ rzyz.buildFrom(R);
+
+ std::cout << "fMe:" << std::endl
+ << "\tt: " << t.t() << std::endl
+ << "\trzyz (deg): " << vpMath::deg(rzyz[0])
+ << " " << vpMath::deg(rzyz[1])
+ << " " << vpMath::deg(rzyz[2]) << std::endl;
+
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
+#else
+int main()
+{
+ std::cout << "The real Viper850 robot controller is not available." << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp
new file mode 100644
index 0000000..8eed9c6
--- /dev/null
+++ b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp
@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Afma 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRobotViper850Pose.cpp
+
+ Example of robot pose usage.
+
+ Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
+ rMc from the robot position.
+
+*/
+
+
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpDebug.h>
+#include <iostream>
+#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394)
+
+int main()
+{
+ try {
+ // Create an image B&W container
+ vpImage<unsigned char> I;
+
+ // Create a firewire grabber based on libdc1394-2.x
+ bool reset = false;
+ vp1394TwoGrabber g(reset);
+
+ // Grab an image from the firewire camera
+ g.acquire(I);
+
+ // Create an image viewer for the image
+#ifdef VISP_HAVE_X11
+ vpDisplayX display(I,100,100,"Current image") ;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV display(I,100,100,"Current image") ;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK display(I,100,100,"Current image") ;
+#endif
+
+ // Display the image
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+
+ // Define a squared target
+ // The target is made of 4 planar points (square dim = 0.077m)
+ double sdim = 0.077; // square width and height
+ vpPoint target[4] ;
+ // Set the point world coordinates (x,y,z) in the object frame
+ // o ----> x
+ // |
+ // |
+ // \/
+ // y
+ target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
+ target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
+ target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
+ target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
+
+ // Image processing to extract the 2D coordinates in sub-pixels of the 4
+ // points from the image acquired by the camera
+ // Creation of 4 trackers
+ vpDot dot[4];
+ vpImagePoint cog;
+ for (int i=0; i < 4; i ++) {
+ dot[i].setGraphics(true); // to display the tracking results
+ std::cout << "Click on dot " << i << std::endl;
+ dot[i].initTracking( I );
+ // The tracker computes the sub-pixels coordinates in the image
+ // i ----> u
+ // |
+ // |
+ // \/
+ // v
+ std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
+ // Flush the tracking results in the viewer
+ vpDisplay::flush(I) ;
+ }
+
+ // Create an intrinsic camera parameters structure
+ vpCameraParameters cam;
+
+ // Create a robot access
+ vpRobotViper850 robot;
+
+ // Load the end-effector to camera frame transformation obtained
+ // using a camera intrinsic model with distortion
+ robot.init(vpViper850::defaultTool,
+ vpCameraParameters::perspectiveProjWithDistortion);
+
+ // Get the intrinsic camera parameters associated to the image
+ robot.getCameraParameters(cam, I);
+
+ // Using the camera parameters, compute the perspective projection (transform
+ // the dot sub-pixel coordinates into coordinates in the camera frame in
+ // meter)
+ for (int i=0; i < 4; i ++) {
+ double x=0, y=0 ; // coordinates of the dots in the camera frame
+ // c ----> x
+ // |
+ // |
+ // \/
+ // y
+ //pixel to meter conversion
+ cog = dot[i].getCog();
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
+ target[i].set_x(x) ;
+ target[i].set_y(y) ;
+ }
+
+ // From now, in target[i], we have the 3D coordinates of a point in the
+ // object frame, and their correspondances in the camera frame. We can now
+ // compute the pose cMo between the camera and the object.
+ vpPose pose;
+ // Add the 4 points to compute the pose
+ for (int i=0; i < 4; i ++) {
+ pose.addPoint(target[i]) ;
+ }
+ // Create an homogeneous matrix for the camera to object transformation
+ // computed just bellow
+ vpHomogeneousMatrix cMo;
+ vpRotationMatrix R;
+ vpRxyzVector r;
+ // Compute the pose: initialisation is done by Lagrange method, and the final
+ // pose is computed by the more accurate Virtual Visual Servoing method.
+ pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
+
+
+ std::cout << "Pose cMo: " << std::endl << cMo;
+ cMo.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ // Get the robot position in the reference frame
+ vpHomogeneousMatrix rMc;
+ vpColVector p; // position x,y,z,rx,ry,rz
+ robot.getPosition(vpRobotViper850::REFERENCE_FRAME, p);
+ std::cout << "Robot pose in reference frame: " << p << std::endl;
+ vpTranslationVector t;
+ t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
+ r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
+ R.buildFrom(r);
+ rMc.buildFrom(t, R);
+ std::cout << "Pose rMc: " << std::endl << rMc;
+ rMc.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, p);
+ std::cout << "Robot pose in articular: " << p << std::endl;
+
+ robot.get_fMc(p, rMc);
+ std::cout << "Pose rMc from MGD: " << std::endl << rMc;
+ rMc.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ vpHomogeneousMatrix rMo;
+ rMo = rMc * cMo;
+ std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
+ rMo.extract(R);
+ r.buildFrom(R);
+ std::cout << " rotation: "
+ << vpMath::deg(r[0]) << " "
+ << vpMath::deg(r[1]) << " "
+ << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+ return 0;
+}
+#else
+int main()
+{
+ std::cout << "Sorry, test not valid. You should have an Viper850 robot..."
+ << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/modules/robot/test/servo-viper/testViper850.cpp b/modules/robot/test/servo-viper/testViper850.cpp
new file mode 100644
index 0000000..4bd69f9
--- /dev/null
+++ b/modules/robot/test/servo-viper/testViper850.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test for Viper850 6 dof robot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testViper850.cpp
+
+ Example of a real robot control, the Viper robot (arm robot, with 6
+ degrees of freedom).
+*/
+
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/core/vpDebug.h>
+
+#include <iostream>
+
+int main()
+{
+ try {
+
+ std::cout << "a test for vpViper850 class..." << std::endl;
+
+ vpViper850 viper850;
+ vpCameraParameters cam;
+
+ std::cout << "-- Default settings for Viper 850 ---"
+ << std::endl;
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
+ << std::endl;
+ viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
+
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
+ << std::endl;
+ viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA,
+ vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << viper850 << std::endl;
+ viper850.getCameraParameters(cam, 640, 480);
+ std::cout << cam << std::endl;
+
+ vpColVector qmotor(6);
+#if 1
+ qmotor[0] = vpMath::rad(30);
+ qmotor[1] = vpMath::rad(-100);
+ qmotor[2] = vpMath::rad(180);
+ qmotor[3] = vpMath::rad(20);
+ qmotor[4] = vpMath::rad(90);
+ qmotor[5] = vpMath::rad(13.37);
+#else
+ qmotor[0] = vpMath::rad(0);
+ qmotor[1] = vpMath::rad(0-90);
+ qmotor[2] = vpMath::rad(0);
+ qmotor[3] = vpMath::rad(0);
+ qmotor[4] = vpMath::rad(0);
+ qmotor[5] = vpMath::rad(0);
+#endif
+ vpHomogeneousMatrix fMe;
+ viper850.get_fMe(qmotor, fMe);
+
+ vpTranslationVector t;
+ fMe.extract(t);
+ vpRotationMatrix R;
+ fMe.extract(R);
+ vpRzyzVector r;
+ r.buildFrom(R);
+
+ std::cout << "fMe:" << std::endl
+ << "\tt: " << t.t() << std::endl
+ << "\trzyz (rad): " << r.t() << std::endl
+ << "\trzyz (deg): " << vpMath::deg(r[0])
+ << " " << vpMath::deg(r[1])
+ << " " << vpMath::deg(r[2]) << std::endl;
+
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
diff --git a/modules/sensor/CMakeLists.txt b/modules/sensor/CMakeLists.txt
new file mode 100644
index 0000000..9a3ca10
--- /dev/null
+++ b/modules/sensor/CMakeLists.txt
@@ -0,0 +1,74 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+#vp_define_module(sensor)
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+# camera devices: v4l, dc1394, cmu1394, directshow ?, freenect
+if(USE_V4L2)
+ list(APPEND opt_incs ${V4L2_INCLUDE_DIRS})
+ list(APPEND opt_libs ${V4L2_LIBRARIES})
+endif()
+if(USE_DC1394)
+ list(APPEND opt_incs ${DC1394_INCLUDE_DIRS})
+ list(APPEND opt_libs ${DC1394_LIBRARY})
+endif()
+if(USE_CMU1394)
+ list(APPEND opt_incs ${CMU1394_INCLUDE_DIRS})
+ list(APPEND opt_libs ${CMU1394_LIBRARIES})
+endif()
+if(USE_DIRECTSHOW)
+ list(APPEND opt_incs ${DIRECTSHOW_INCLUDE_DIRS})
+ list(APPEND opt_libs ${DIRECTSHOW_LIBRARIES})
+endif()
+if(USE_LIBFREENECT)
+ list(APPEND opt_incs ${LIBFREENECT_INCLUDE_DIRS})
+ list(APPEND opt_libs ${LIBFREENECT_LIBRARIES})
+endif()
+if(USE_LIBUSB_1)
+ list(APPEND opt_incs ${LIBUSB_1_INCLUDE_DIRS})
+ list(APPEND opt_libs ${LIBUSB_1_LIBRARIES})
+endif()
+
+vp_add_module(sensor visp_core)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
+vp_add_tests(CTEST_EXCLUDE_PATH framegrabber DEPENDS_ON visp_io)
diff --git a/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h
new file mode 100644
index 0000000..e5009eb
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h
@@ -0,0 +1,299 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
+ *
+ * Authors:
+ * Lucas Lopes Lemos FEMTO-ST, AS2M departement, Besancon
+ * Guillaume Laurent FEMTO-ST, AS2M departement, Besancon
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vp1394CMUGrabber_h
+#define vp1394CMUGrabber_h
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_CMU1394
+
+#include <windows.h>
+#include <1394Camera.h> // CMU library
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpFrameGrabberException.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \file vp1394CMUGrabber.h
+ \brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
+*/
+
+/*!
+ \class vp1394CMUGrabber
+
+ \ingroup group_sensor_camera
+
+ \brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
+
+ This block is based on the CMU 1394 Digital Camera SDK. The CMU 1394 Digital
+ Camera Driver must be installed (go to http://www.cs.cmu.edu/~iwan/1394/index.html
+ to download it).
+ - Parameters:
+ - Camera index (0, 1, 2, ... or 10). First camera has index 0.
+ - Image format
+ - Frame rate. Real frame rate depends on device capacities.
+ - Control : shutter speed and gain can be internally set, but it is possible to set manually them.
+ * Exposure time register value. Real exposure time depends on device capacities.
+ * Gain register value. Real gain depends on device capacities.
+
+ This first example available in tutorial-grabber-CMU1394.cpp shows how to grab
+ and display images from a firewire camera under Windows.
+
+ \include tutorial-grabber-CMU1394.cpp
+
+ This other example shows how to consider more than one firewire camera, and how
+ to grab and display images from the first camera found on the bus.
+
+ \code
+#include <iostream>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_CMU1394)
+ std::cout << "ViSP Image acquisition example" << std::endl;
+
+ vpImage<unsigned char> I;
+ vp1394CMUGrabber g;
+
+ if( g.getNumberOfConnectedCameras() > 1 )
+ std::cout << "There are " << g.getNumberOfConnectedCameras() << " connected cameras." << std::endl;
+ if( g.getNumberOfConnectedCameras() == 1 )
+ std::cout << "There is " << g.getNumberOfConnectedCameras() << " connected camera." << std::endl;
+ else
+ std::cout << "There is no connected camera." << std::endl;
+
+ // Setting camera parameters manually
+ g.selectCamera(0);
+ g.setGain(0);
+ g.setShutter(2000);
+ g.setFramerate(3); // 15 FPS
+ g.setVideoMode(0, 5); // 640x480 - MONO
+
+ g.acquire(I);
+
+ // Display camera description
+ g.displayCameraDescription(0);
+ g.displayCameraModel();
+ std::cout << "Height: " << g.getHeight() << " Width: " << g.getWidth() << std::endl;
+
+ vpDisplayOpenCV d(I);
+ vpDisplay::display(I);
+
+ for(;;)
+ {
+ g.acquire(I);
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ if (vpDisplay::getClick(I, false)) // a click to exit
+ break;
+ }
+
+ g.close();
+#endif
+ std::cout << "ViSP exiting..." <<std::endl;
+ return 0;
+}
+ \endcode
+*/
+
+class VISP_EXPORT vp1394CMUGrabber : public vpFrameGrabber
+{
+public:
+ /*!
+ Enumeration of color codings.
+ */
+ typedef enum {
+ YUV444,
+ YUV422,
+ YUV411,
+ RGB8,
+ MONO8,
+ MONO16,
+ UNKNOWN
+ } vpColorCodingType;
+
+private :
+ //! Current camera handle
+ C1394Camera *camera;
+ //! Camera index on the bus
+ int index;
+ //! Current video format
+ unsigned long _format;
+ //! Current video mode
+ unsigned long _mode ;
+ //! Current video frame rate
+ unsigned long _fps ;
+ //! Current auto mode
+ bool _modeauto;
+ //! Current gain
+ unsigned short _gain;
+ //! Current shutter
+ unsigned short _shutter;
+ //! Color coding
+ vpColorCodingType _color;
+
+public:
+
+ // Constructor.
+ vp1394CMUGrabber();
+ // Destructor.
+ virtual ~vp1394CMUGrabber();
+
+ // Acquire one frame in a greyscale image.
+ void acquire(vpImage<unsigned char> &I);
+
+ // Acquire one frame in a color image.
+ void acquire(vpImage<vpRGBa> &I);
+
+ // Stop the acquisition.
+ void close();
+
+ // Display information about the camera on the standard output.
+ void displayCameraDescription(int cam_id);
+
+ // Display camera model on the standard output. Call it after open the grabber.
+ void displayCameraModel();
+
+ // Get the video framerate
+ int getFramerate();
+
+ // Get the gain min and max values.
+ void getGainMinMax(unsigned short &min, unsigned short &max);
+
+ // Get the number of connected cameras.
+ int getNumberOfConnectedCameras() const ;
+
+ // Get the shutter min and max values.
+ void getShutterMinMax(unsigned short &min, unsigned short &max);
+
+ //! Get the video color coding format.
+ vpColorCodingType getVideoColorCoding() const
+ {
+ vpColorCodingType color = vp1394CMUGrabber::UNKNOWN;
+ if (_format == 0)
+ {
+ switch(_mode)
+ {
+ case 0: color = vp1394CMUGrabber::YUV444; break;
+ case 1: color = vp1394CMUGrabber::YUV422; break;
+ case 2: color = vp1394CMUGrabber::YUV411; break;
+ case 3: color = vp1394CMUGrabber::YUV422; break;
+ case 4: color = vp1394CMUGrabber::RGB8; break;
+ case 5: color = vp1394CMUGrabber::MONO8; break;
+ case 6: color = vp1394CMUGrabber::MONO16; break;
+ }
+ }
+ else if (_format == 1)
+ {
+ switch(_mode)
+ {
+ case 0: color = vp1394CMUGrabber::YUV422; break;
+ case 1: color = vp1394CMUGrabber::RGB8; break;
+ case 2: color = vp1394CMUGrabber::MONO8; break;
+ case 3: color = vp1394CMUGrabber::YUV422; break;
+ case 4: color = vp1394CMUGrabber::RGB8; break;
+ case 5: color = vp1394CMUGrabber::MONO8; break;
+ case 6: color = vp1394CMUGrabber::MONO16; break;
+ case 7: color = vp1394CMUGrabber::MONO16; break;
+ }
+ }
+ else if (_format == 2)
+ {
+ switch(_mode)
+ {
+ case 0: color = vp1394CMUGrabber::YUV422; break;
+ case 1: color = vp1394CMUGrabber::RGB8; break;
+ case 2: color = vp1394CMUGrabber::MONO8; break;
+ case 3: color = vp1394CMUGrabber::YUV422; break;
+ case 4: color = vp1394CMUGrabber::RGB8; break;
+ case 5: color = vp1394CMUGrabber::MONO8; break;
+ case 6: color = vp1394CMUGrabber::MONO16; break;
+ case 7: color = vp1394CMUGrabber::MONO16; break;
+ }
+ }
+
+ return color;
+ }
+
+ // Initialization of the grabber using a greyscale image.
+ void open(vpImage<unsigned char> &I);
+
+ // Initialization of the grabber using a color image.
+ void open(vpImage<vpRGBa> &I);
+
+ // Select the camera on the bus. Call it before open the grabber.
+ void selectCamera(int cam_id);
+
+ // Enable auto gain
+ void setAutoGain();
+
+ // Enable auto shutter
+ void setAutoShutter();
+
+ // Set the gain and the shutter values. Call it before open the grabber
+ void setControl(unsigned short gain, unsigned short shutter);
+
+ // Set the frame rate. Call it before open the grabber.
+ void setFramerate(unsigned long fps);
+
+ // Set the shutter value. Call it before open the grabber
+ void setShutter(unsigned short shutter);
+
+ // Set the gain value. Call it before open the grabber
+ void setGain(unsigned short gain);
+
+ // Set the video format and mode. Call it before open the grabber.
+ void setVideoMode(unsigned long format, unsigned long mode );
+
+private :
+
+ void initCamera();
+
+};
+
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h
new file mode 100644
index 0000000..007e709
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h
@@ -0,0 +1,472 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vp1394TwoGrabber.h
+ \brief class for firewire cameras video capture using libdc1394-2.x api
+
+ \warning This class needs libdc1394-2 and
+ libraw1394 (version greater than 1.1.0). These libraries are available from
+ http://sourceforge.net/projects/libdc1394 and
+ http://sourceforge.net/projects/libraw1394 .
+
+ vp1394TwoGrabber was tested with libdc1394-2.0.0 (svn version) and
+ with the following cameras:
+ - Marlin F033C
+ - Marlin F131B
+ - PointGrey Dragonfly 2
+ - APPLE iSight extern webcam
+
+*/
+
+#ifndef vp1394TwoGrabber_h
+#define vp1394TwoGrabber_h
+
+#include <visp3/core/vpConfig.h>
+
+#include <list>
+
+#if defined(VISP_HAVE_DC1394)
+
+
+/*!
+ \class vp1394TwoGrabber
+
+ \ingroup group_sensor_camera
+
+ \brief Class for firewire ieee1394 video devices using libdc1394-2.x api.
+
+
+ Needs libraw1394-1.2.0 and libdc1394-2.0.0 or more recent versions
+ available on http://sourceforge.net.
+
+ This class was tested with Marlin F033C and F131B cameras and with
+ Point Grey Dragonfly 2, Flea 2 and Flea 3 cameras.
+
+ \ingroup libdevice
+
+ This grabber allows single or multi camera acquisition.
+
+ - Here you will find an example of single capture from the first camera found
+ on the bus. This example is available in tutorial-grabber-1394.cpp:
+ \include tutorial-grabber-1394.cpp
+ A line by line explanation of this example is provided in \ref tutorial-grabber.
+ An other example that shows how to use format 7
+ and the auto-shutter is provided in vp1394TwoGrabber() constructor:
+
+ - If more than one camera is connected, it is also possible to select a specific camera by its GUID:
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I; // Create a gray level image container
+ bool reset = false; // Disable bus reset during construction
+ vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
+
+ unsigned int ncameras; // Number of cameras on the bus
+ ncameras = g.getNumCameras();
+ std::cout << ncameras << " cameras found:" << std::endl;
+
+ for(unsigned int i=0; i< ncameras; i++)
+ {
+ g.setCamera(i);
+ uint64_t guid = g.getGuid();
+ printf("camera %d with guid 0x%lx\n", i, (long unsigned int)guid);
+ }
+
+ // produce:
+ // 2 cameras found:
+ // camera 0 with guid 0xb09d01009b329c
+ // camera 1 with guid 0xb09d01007e0ee7
+ g.setCamera( (uint64_t)0xb09d01009b329cULL );
+
+ printf("Use camera with GUID: 0x%lx\n", (long unsigned int)g.getGuid());
+ g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
+
+ vpImageIo::write(I, "image.pgm"); // Write image on the disk
+#endif
+}
+ \endcode
+
+ - Here an example of multi camera capture. An other example is available in setCamera():
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ bool reset = false; // Disable bus reset during construction
+ vp1394TwoGrabber g(reset); // Creation of a grabber instance based on libdc1394-2.x third party lib.
+ unsigned int ncameras; // Number of cameras on the bus
+ ncameras = g.getNumCameras();
+
+ // Create an image container for each camera
+ vpImage<unsigned char> *I = new vpImage<unsigned char> [ncameras];
+ char filename[FILENAME_MAX];
+
+ // If the first camera supports vpVIDEO_MODE_640x480_YUV422 video mode
+ g.setCamera(0);
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_YUV422);
+
+ // If the second camera support 30 fps acquisition
+ g.setCamera(1);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
+
+ // Acquire an image from each camera
+ for (unsigned int camera=0; camera < ncameras; camera ++) {
+ g.setCamera(camera);
+ g.acquire(I[camera]);
+ sprintf(filename, "image-cam%d.pgm", camera);
+ vpImageIo::write(I[camera], filename);
+ }
+ delete [] I;
+#endif
+}
+ \endcode
+
+ \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
+
+*/
+
+/*
+ * Interface with libdc1394 2.x
+ */
+#include <string.h>
+
+#include <dc1394/control.h>
+#include <dc1394/utils.h>
+#include <dc1394/vendor/avt.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpRGBa.h>
+
+
+class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
+{
+
+ public:
+ static const char * strVideoMode[DC1394_VIDEO_MODE_NUM];
+ static const char * strFramerate[DC1394_FRAMERATE_NUM];
+ static const char * strColorCoding[DC1394_COLOR_CODING_NUM];
+
+ /*!
+ Enumeration of iso speed. See libdc1394 2.x header file
+ dc1394/control.h
+ */
+ typedef enum {
+ vpISO_SPEED_100 = DC1394_ISO_SPEED_100,
+ vpISO_SPEED_200 = DC1394_ISO_SPEED_200,
+ vpISO_SPEED_400 = DC1394_ISO_SPEED_400,
+ vpISO_SPEED_800 = DC1394_ISO_SPEED_800,
+ vpISO_SPEED_1600 = DC1394_ISO_SPEED_1600,
+ vpISO_SPEED_3200 = DC1394_ISO_SPEED_3200
+ } vp1394TwoIsoSpeedType;
+
+ /*!
+ Enumeration of video modes. See libdc1394 2.x header file
+ dc1394/control.h
+ */
+ typedef enum {
+ vpVIDEO_MODE_160x120_YUV444 = DC1394_VIDEO_MODE_160x120_YUV444,
+ vpVIDEO_MODE_320x240_YUV422 = DC1394_VIDEO_MODE_320x240_YUV422,
+ vpVIDEO_MODE_640x480_YUV411 = DC1394_VIDEO_MODE_640x480_YUV411,
+ vpVIDEO_MODE_640x480_YUV422 = DC1394_VIDEO_MODE_640x480_YUV422,
+ vpVIDEO_MODE_640x480_RGB8 = DC1394_VIDEO_MODE_640x480_RGB8,
+ vpVIDEO_MODE_640x480_MONO8 = DC1394_VIDEO_MODE_640x480_MONO8,
+ vpVIDEO_MODE_640x480_MONO16 = DC1394_VIDEO_MODE_640x480_MONO16,
+ vpVIDEO_MODE_800x600_YUV422 = DC1394_VIDEO_MODE_800x600_YUV422,
+ vpVIDEO_MODE_800x600_RGB8 = DC1394_VIDEO_MODE_800x600_RGB8,
+ vpVIDEO_MODE_800x600_MONO8 = DC1394_VIDEO_MODE_800x600_MONO8,
+ vpVIDEO_MODE_1024x768_YUV422 = DC1394_VIDEO_MODE_1024x768_YUV422,
+ vpVIDEO_MODE_1024x768_RGB8 = DC1394_VIDEO_MODE_1024x768_RGB8,
+ vpVIDEO_MODE_1024x768_MONO8 = DC1394_VIDEO_MODE_1024x768_MONO8,
+ vpVIDEO_MODE_800x600_MONO16 = DC1394_VIDEO_MODE_800x600_MONO16,
+ vpVIDEO_MODE_1024x768_MONO16 = DC1394_VIDEO_MODE_1024x768_MONO16,
+ vpVIDEO_MODE_1280x960_YUV422 = DC1394_VIDEO_MODE_1280x960_YUV422,
+ vpVIDEO_MODE_1280x960_RGB8 = DC1394_VIDEO_MODE_1280x960_RGB8,
+ vpVIDEO_MODE_1280x960_MONO8 = DC1394_VIDEO_MODE_1280x960_MONO8,
+ vpVIDEO_MODE_1600x1200_YUV422 = DC1394_VIDEO_MODE_1600x1200_YUV422,
+ vpVIDEO_MODE_1600x1200_RGB8 = DC1394_VIDEO_MODE_1600x1200_RGB8,
+ vpVIDEO_MODE_1600x1200_MONO8 = DC1394_VIDEO_MODE_1600x1200_MONO8,
+ vpVIDEO_MODE_1280x960_MONO16 = DC1394_VIDEO_MODE_1280x960_MONO16,
+ vpVIDEO_MODE_1600x1200_MONO16 = DC1394_VIDEO_MODE_1600x1200_MONO16,
+ vpVIDEO_MODE_EXIF = DC1394_VIDEO_MODE_EXIF,
+ vpVIDEO_MODE_FORMAT7_0 = DC1394_VIDEO_MODE_FORMAT7_0,
+ vpVIDEO_MODE_FORMAT7_1 = DC1394_VIDEO_MODE_FORMAT7_1,
+ vpVIDEO_MODE_FORMAT7_2 = DC1394_VIDEO_MODE_FORMAT7_2,
+ vpVIDEO_MODE_FORMAT7_3 = DC1394_VIDEO_MODE_FORMAT7_3,
+ vpVIDEO_MODE_FORMAT7_4 = DC1394_VIDEO_MODE_FORMAT7_4,
+ vpVIDEO_MODE_FORMAT7_5 = DC1394_VIDEO_MODE_FORMAT7_5,
+ vpVIDEO_MODE_FORMAT7_6 = DC1394_VIDEO_MODE_FORMAT7_6,
+ vpVIDEO_MODE_FORMAT7_7 = DC1394_VIDEO_MODE_FORMAT7_7
+ } vp1394TwoVideoModeType;
+
+ /*!
+ Enumeration of framerates. See libdc1394 2.x header file
+ dc1394/control.h
+ */
+ typedef enum {
+ vpFRAMERATE_1_875 = DC1394_FRAMERATE_1_875,
+ vpFRAMERATE_3_75 = DC1394_FRAMERATE_3_75,
+ vpFRAMERATE_7_5 = DC1394_FRAMERATE_7_5,
+ vpFRAMERATE_15 = DC1394_FRAMERATE_15,
+ vpFRAMERATE_30 = DC1394_FRAMERATE_30,
+ vpFRAMERATE_60 = DC1394_FRAMERATE_60,
+ vpFRAMERATE_120 = DC1394_FRAMERATE_120,
+ vpFRAMERATE_240 = DC1394_FRAMERATE_240
+ } vp1394TwoFramerateType;
+
+ /*!
+ Enumeration of color codings. See libdc1394 2.x header file
+ dc1394/control.h
+ */
+ typedef enum {
+ vpCOLOR_CODING_MONO8 = DC1394_COLOR_CODING_MONO8,
+ vpCOLOR_CODING_YUV411 = DC1394_COLOR_CODING_YUV411,
+ vpCOLOR_CODING_YUV422 = DC1394_COLOR_CODING_YUV422,
+ vpCOLOR_CODING_YUV444 = DC1394_COLOR_CODING_YUV444,
+ vpCOLOR_CODING_RGB8 = DC1394_COLOR_CODING_RGB8,
+ vpCOLOR_CODING_MONO16 = DC1394_COLOR_CODING_MONO16,
+ vpCOLOR_CODING_RGB16 = DC1394_COLOR_CODING_RGB16,
+ vpCOLOR_CODING_MONO16S = DC1394_COLOR_CODING_MONO16S,
+ vpCOLOR_CODING_RGB16S = DC1394_COLOR_CODING_RGB16S,
+ vpCOLOR_CODING_RAW8 = DC1394_COLOR_CODING_RAW8,
+ vpCOLOR_CODING_RAW16 = DC1394_COLOR_CODING_RAW16
+ } vp1394TwoColorCodingType;
+
+ /*!
+ Enumeration of the parameters that can be modified. See libdc1394 2.x header
+ file dc1394/control.h
+ */
+ typedef enum {
+ vpFEATURE_BRIGHTNESS = DC1394_FEATURE_BRIGHTNESS,
+ vpFEATURE_EXPOSURE = DC1394_FEATURE_EXPOSURE,
+ vpFEATURE_SHARPNESS = DC1394_FEATURE_SHARPNESS,
+// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ vpFEATURE_HUE = DC1394_FEATURE_HUE,
+ vpFEATURE_SATURATION = DC1394_FEATURE_SATURATION,
+ vpFEATURE_GAMMA = DC1394_FEATURE_GAMMA,
+ vpFEATURE_SHUTTER = DC1394_FEATURE_SHUTTER,
+ vpFEATURE_GAIN = DC1394_FEATURE_GAIN,
+ vpFEATURE_IRIS = DC1394_FEATURE_IRIS,
+// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+// vpFEATURE_PAN = DC1394_FEATURE_PAN,
+// vpFEATURE_TILT = DC1394_FEATURE_TILT,
+// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ } vp1394TwoParametersType;
+
+
+ private:
+
+ /*!
+ Control structure of the values that can be modified during the execution.
+ */
+ typedef struct{
+ uint32_t brightness ;
+ uint32_t exposure;
+ uint32_t sharpness;
+// uint32_t whiteBalance;
+ uint32_t hue;
+ uint32_t saturation;
+ uint32_t gamma;
+ uint32_t shutter ;
+ uint32_t gain ;
+ uint32_t iris;
+// uint32_t focus;
+// uint32_t temperature ;
+// uint32_t trigger ;
+// uint32_t triggerDelay ;
+// uint32_t whiteShadding ;
+// uint32_t frameRate ;
+// uint32_t zoom;
+// uint32_t pan;
+// uint32_t tilt ;
+// uint32_t opticalFilter ;
+// uint32_t captureSize;
+// uint32_t captureQuality ;
+ } vpDc1394TwoCameraParametersData;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vp1394TwoGrabber(const vp1394TwoGrabber &)
+// : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), camIsOpen(NULL),
+// num_buffers(4), // ring buffer size
+// isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
+// #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+// , d(NULL),
+// list(NULL)
+// #endif
+// {
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// }
+// vp1394TwoGrabber &operator=(const vp1394TwoGrabber &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vp1394TwoGrabber(bool reset=true);
+ virtual ~vp1394TwoGrabber();
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id);
+ void acquire(vpImage<vpRGBa> &I);
+ void acquire(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id);
+
+ void close();
+
+ static std::string colorCoding2string(vp1394TwoColorCodingType colorcoding);
+
+ dc1394video_frame_t *dequeue();
+ dc1394video_frame_t *dequeue(vpImage<unsigned char> &I);
+ dc1394video_frame_t *dequeue(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id);
+ dc1394video_frame_t *dequeue(vpImage<vpRGBa> &I);
+ dc1394video_frame_t *dequeue(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id);
+ void enqueue(dc1394video_frame_t *frame);
+
+ static std::string framerate2string(vp1394TwoFramerateType fps);
+
+ void getAutoGain(unsigned int &minvalue, unsigned int &maxvalue);
+ void getAutoShutter(unsigned int &minvalue, unsigned int &maxvalue);
+
+ uint64_t getCamera();
+ void getCamera(uint64_t &camera);
+ void getColorCoding(vp1394TwoColorCodingType & coding);
+ uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode,
+ std::list<vp1394TwoColorCodingType> & codings);
+ void getFramerate(vp1394TwoFramerateType & fps);
+ uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode,
+ std::list<vp1394TwoFramerateType> & fps);
+ uint64_t getGuid();
+ void getGuid(uint64_t &guid);
+ void getHeight(unsigned int &height);
+ unsigned int getHeight();
+ void getNumCameras(unsigned int &ncameras) const;
+ unsigned int getNumCameras() const;
+ unsigned int getParameterValue(vp1394TwoParametersType param);
+ unsigned int getRingBufferSize() const;
+ void getVideoMode(vp1394TwoVideoModeType & videomode);
+ uint32_t getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes);
+ void getWidth(unsigned int &width);
+ unsigned int getWidth();
+
+ bool isColor();
+ bool isColorCodingSupported(vp1394TwoVideoModeType videomode,
+ vp1394TwoColorCodingType coding);
+ bool isFramerateSupported(vp1394TwoVideoModeType videomode,
+ vp1394TwoFramerateType fps);
+ bool isVideoModeSupported(vp1394TwoVideoModeType videomode) ;
+ bool isVideoModeFormat7(vp1394TwoVideoModeType videomode);
+
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ void printCameraInfo();
+
+ void resetBus();
+
+ void setAutoGain(bool enable = true);
+ void setAutoGain(unsigned int minvalue, unsigned int maxvalue);
+ void setAutoShutter(bool enable = true);
+ void setAutoShutter(unsigned int minvalue, unsigned int maxvalue);
+ void setCamera(uint64_t camera);
+ void setColorCoding(vp1394TwoColorCodingType coding);
+ void setFormat7ROI(unsigned int left=0, unsigned int top=0,
+ unsigned int width=0, unsigned int height=0);
+ void setFramerate(vp1394TwoFramerateType fps);
+ void setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed);
+ void setPanControl(unsigned int panControlValue);
+ void setParameterValue(vp1394TwoParametersType param, unsigned int val);
+ void setRingBufferSize(unsigned int size);
+ void setVideoMode(vp1394TwoVideoModeType videomode);
+
+ static vp1394TwoVideoModeType string2videoMode(std::string videomode);
+ static vp1394TwoFramerateType string2framerate(std::string fps);
+ static vp1394TwoColorCodingType string2colorCoding(std::string colorcoding);
+
+ static std::string videoMode2string(vp1394TwoVideoModeType videomode);
+
+ private:
+ void open();
+ void initialize(bool reset);
+ void setCapture(dc1394switch_t _switch);
+ void setTransmission(dc1394switch_t _switch);
+ inline void updateDataCamToStruct();
+ inline void updateDataStructToCam();
+
+ private:
+ dc1394camera_t *camera, **cameras;
+ unsigned int num_cameras;
+ unsigned int camera_id;
+
+ bool verbose;
+ bool *camIsOpen;
+ unsigned int num_buffers;
+
+ /* parameters for the cameras */
+ bool* isDataModified;
+ dc1394feature_mode_t* initialShutterMode;
+ vpDc1394TwoCameraParametersData* dataCam;
+
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE
+ dc1394_t * d;
+ dc1394camera_list_t * list;
+#endif
+};
+
+
+#endif
+#endif
+
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h
new file mode 100644
index 0000000..96576b3
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow device description.
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#ifndef vpDirectShowDevice_hh
+#define vpDirectShowDevice_hh
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <atlbase.h>
+#include <iostream>
+#include <string>
+#include <dshow.h>
+
+class VISP_EXPORT vpDirectShowDevice
+{
+
+ std::string name; //the device's name
+ std::string desc; //the device's description
+ std::string devPath; //the device's device path (unique)
+
+ bool inUse; //true if the device is already used by a grabber
+
+public:
+ vpDirectShowDevice() : inUse(false){}
+ vpDirectShowDevice(const CComPtr<IMoniker>& moniker) : inUse(false){ init(moniker); }
+
+ bool init(const CComPtr<IMoniker>& moniker);
+
+ bool getState(){ return inUse; }
+ void setInUse(){ inUse=true; }
+ void resetInUse() { inUse=false; }
+
+ std::string& getName(){ return name; }
+ std::string& getDesc(){ return desc; }
+ std::string& getDevPath(){ return devPath; }
+
+ bool operator==(vpDirectShowDevice& dev);
+
+ friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpDirectShowDevice& dev);
+};
+#endif
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h b/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h
new file mode 100644
index 0000000..6893e26
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber.
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+#ifndef vpDirectShowGrabber_hh
+#define vpDirectShowGrabber_hh
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpFrameGrabber.h>
+
+class vpDirectShowGrabberImpl;
+
+/*!
+ \class vpDirectShowGrabber
+ \brief class for windows direct show devices
+
+ This class' goal is to totally hide the implementation of the framegrabber
+ from the rest of the application. This is necessary to avoid conflicts
+ between dshow's Qedit.h and Direct3D's includes.
+
+ \ingroup group_sensor_camera
+*/
+
+class VISP_EXPORT vpDirectShowGrabber : public vpFrameGrabber
+{
+ vpDirectShowGrabberImpl * grabber;
+
+
+ public:
+
+ unsigned int getHeight();
+ unsigned int getWidth();
+
+ vpDirectShowGrabber();
+ virtual ~vpDirectShowGrabber();
+
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+
+ void close();
+
+ //get the number of capture devices
+ unsigned int getDeviceNumber();
+
+ //change the capture device
+ bool setDevice(unsigned int id);
+
+ //displays a list of available devices
+ void displayDevices();
+
+ //set image size
+ bool setImageSize(unsigned int width,unsigned int height);
+
+ //set source framerate
+ bool setFramerate(double framerate);
+
+
+ //set capture format
+ bool setFormat(unsigned int width,unsigned int height, double framerate);
+
+ //get capture format
+ void getFormat(unsigned int &width,unsigned int &height, double &framerate);
+
+ //Get the available capture formats
+ bool getStreamCapabilities();
+
+ //Set capture MediaType
+ bool setMediaType(int mediaTypeID);
+
+ //Get current capture MediaType
+ int getMediaType();
+
+};
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h
new file mode 100644
index 0000000..60979d5
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h
@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber Implementation.
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#ifndef vpDirectShowGrabberImpl_hh
+#define vpDirectShowGrabberImpl_hh
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <atlbase.h>
+#include <qedit.h>
+#include <dshow.h>
+
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpFrameGrabberException.h>
+
+#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
+#include <visp3/sensor/vpDirectShowDevice.h>
+#include <visp3/core/vpDebug.h>
+/*!
+ \class vpDirectShowGrabberImpl
+ \brief class for windows direct show devices - implementation
+
+ This class uses CComPtr which is the best way to be sure that all the interfaces
+ are released sooner or later.
+
+*/
+class VISP_EXPORT vpDirectShowGrabberImpl : public vpFrameGrabber
+{
+ static const int MAX_DELAY = 10000;
+ static const int MAX_DEVICES = 10;
+
+
+ public:
+
+ /*!
+ Enumeration of video subtypes.
+*/
+/*
+ typedef enum {
+ //Known RGB formats
+ vpMEDIASUBTYPE_ARGB32 = MEDIASUBTYPE_ARGB32,
+ vpMEDIASUBTYPE_RGB32 = MEDIASUBTYPE_RGB32,
+ vpMEDIASUBTYPE_RGB24 = MEDIASUBTYPE_RGB24,
+ vpMEDIASUBTYPE_RGB555 = MEDIASUBTYPE_RGB555,
+ vpMEDIASUBTYPE_RGB565 = MEDIASUBTYPE_RGB565,
+ vpMEDIASUBTYPE_RGB8 = MEDIASUBTYPE_RGB8,
+ vpMEDIASUBTYPE_RGB4 = MEDIASUBTYPE_RGB4,
+ vpMEDIASUBTYPE_RGB1 = MEDIASUBTYPE_RGB1,
+ //Known YUV formats
+ vpMEDIASUBTYPE_AYUV = MEDIASUBTYPE_AYUV,
+ vpMEDIASUBTYPE_UYVY = MEDIASUBTYPE_UYVY,
+ vpMEDIASUBTYPE_Y411 = MEDIASUBTYPE_Y411,
+ vpMEDIASUBTYPE_Y41P = MEDIASUBTYPE_Y41P,
+ vpMEDIASUBTYPE_Y211 = MEDIASUBTYPE_Y211,
+ vpMEDIASUBTYPE_YUY2 = MEDIASUBTYPE_YUY2,
+ vpMEDIASUBTYPE_YVYU = MEDIASUBTYPE_YVYU,
+ vpMEDIASUBTYPE_YUYV = MEDIASUBTYPE_YUYV,
+ vpMEDIASUBTYPE_IF09 = MEDIASUBTYPE_IF09,
+ vpMEDIASUBTYPE_IYUV = MEDIASUBTYPE_IYUV,
+ vpMEDIASUBTYPE_YV12 = MEDIASUBTYPE_YV12,
+ vpMEDIASUBTYPE_YVU9 = MEDIASUBTYPE_YVU9
+ } vpDirectShowMediaSubtype;
+*/
+
+ vpDirectShowGrabberImpl();
+ virtual ~vpDirectShowGrabberImpl();
+
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+
+ void close();
+
+ /*!
+ Gets the number of capture devices
+ */
+ unsigned int getDeviceNumber() {return nbDevices;}
+
+ //change the capture device
+ bool setDevice(unsigned int id);
+
+ //displays a list of available devices
+ void displayDevices();
+
+ //set image size
+ bool setImageSize(unsigned int width,unsigned int height);
+
+ //set capture framerate
+ bool setFramerate(double framerate);
+
+ //set capture format
+ bool setFormat(unsigned int width,unsigned int height, double framerate);
+
+ //get capture format
+ void getFormat(unsigned int &width,unsigned int &height, double &framerate);
+
+ //set capture MediaType
+ bool setMediaType(int mediaTypeID);
+
+ //get current capture MediaType
+ int getMediaType();
+
+ //Get the available capture formats
+ bool getStreamCapabilities();
+
+
+ private:
+
+ CComPtr<IGraphBuilder> pGraph; //our DS filter graph
+
+ CComPtr<ICaptureGraphBuilder2> pBuild; //the interface to the capture graph builder
+ //used to build the filter graph
+
+ CComPtr<IBaseFilter> pCapSource; //the capture source filter
+
+ CComPtr<ISampleGrabber> pGrabberI; //the sample grabber's interface and filter
+ CComPtr<IBaseFilter> pGrabberFilter;
+
+ CComPtr<IMediaControl> pControl; //The DS filter graph control interface
+ CComPtr<IMediaEvent> pEvent; //The DS filter graph event interface
+
+ vpDirectShowSampleGrabberI sgCB; //Interface used to implement the frame grabber callback
+
+ HRESULT hr; //contains the result of the last operation
+
+ static vpDirectShowDevice * deviceList; //This contains the list of the available capture devices
+ //it is shared by all the DirectShow Grabbers
+
+ static unsigned int nbDevices; //the number of available devices
+ int currentDevice; //the number of the current device
+
+ // flag to manage CoInitialize() and CoUnInitialze()
+ bool initCo ;
+ //setup the directshow filtergraph with the first available device
+ bool initDirectShow();
+
+ //enumerates the different video inputs
+ bool enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum);
+
+ //selects a random video input from the enumeration and returns the associated filter
+ bool selectRandomSource(CComPtr<IEnumMoniker>& ppVideoInputEnum, CComPtr<IBaseFilter>& pCapSource);
+
+ //creates the filter graph
+ bool createGraph();
+
+ //creates the sample grabber
+ bool createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter);
+
+ //checks the capture filter's media type and sets flags as needed
+ bool checkSourceType(CComPtr<IPin>& pCapSourcePin);
+
+ //connects the filters as needed
+ bool connectSourceToGrabber(CComPtr<IBaseFilter>& pCapSource, CComPtr<IBaseFilter>& pGrabberFilter);
+
+ //used to convert HRESULT-associated error message to a string
+ void HRtoStr(std::string str);
+
+ //create the list of the available devices
+ bool createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum);
+
+ //get the n-th device if it is available
+ bool getDevice(unsigned int n, CComPtr<IBaseFilter>& ppCapSource);
+
+ //get the first available device if any
+ unsigned int getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice);
+
+ //removes all the filters in the graph
+ bool removeAll();
+
+ //Deletes an allocated AM_MEDIA_TYPE structure, including the format block
+ void MyDeleteMediaType(AM_MEDIA_TYPE *pmt);
+
+ //Frees the format block in an AM_MEDIA_TYPE structure
+ void MyFreeMediaType(AM_MEDIA_TYPE& mt);
+
+};
+
+#endif
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h
new file mode 100644
index 0000000..b29579f
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber callback.
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#ifndef vpDirectShowSampleGrabberI_hh
+#define vpDirectShowSampleGrabberI_hh
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <qedit.h>
+#include <stdio.h>
+#include <dshow.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+ This class is needed in order to implement a callback function
+ associated with the grabber
+*/
+class VISP_EXPORT vpDirectShowSampleGrabberI : public ISampleGrabberCB
+{
+ vpDirectShowSampleGrabberI();
+ virtual ~vpDirectShowSampleGrabberI();
+
+ //needed by the interface
+ STDMETHODIMP_(ULONG) AddRef() { return 1; }
+ STDMETHODIMP_(ULONG) Release() { return 2; }
+
+ STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject);
+
+ //not implemented
+ STDMETHODIMP SampleCB(double Time, IMediaSample *pSample)
+ {
+ return E_NOTIMPL;
+ }
+
+ //our callback function
+ STDMETHODIMP BufferCB(double Time, BYTE *pBuffer, long BufferLen);
+
+private:
+ //the currently connected media type
+ AM_MEDIA_TYPE connectedMediaType;
+
+ //true if the source media type is not a standard one
+ bool specialMediaType;
+ //true if the image needs to be flipped (only for special media types)
+ bool invertedSource;
+
+ //booleans used to signal a demand from acquire
+ bool acqGrayDemand;
+ bool acqRGBaDemand;
+
+ //pointer on the image to fill during the next callback if there has been a demand
+ vpImage<vpRGBa> * rgbaIm;
+ vpImage<unsigned char> * grayIm;
+
+ //semaphore used to synchronize the productor (callback) and the consumer (acquire)
+ HANDLE copySem;
+
+ friend class vpDirectShowGrabberImpl;
+};
+
+#endif
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpKinect.h b/modules/sensor/include/visp3/sensor/vpKinect.h
new file mode 100644
index 0000000..cd822dc
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpKinect.h
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * API for using a Microsoft Kinect device
+ * Requires libfreenect as a third party library
+ *
+ * Authors:
+ * Celine Teuliere
+ *
+ *****************************************************************************/
+
+
+#ifndef __VP_KINECT__
+#define __VP_KINECT__
+
+#include <visp3/core/vpConfig.h>
+// Note that libfreenect needs libusb-1.0 and libpthread
+#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
+
+#include <iostream>
+#include <libfreenect.hpp>
+
+#include <visp3/core/vpMutex.h> // need pthread
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+/*!
+
+ \class vpKinect
+
+ \ingroup group_sensor_rgbd
+
+ \brief Driver for the Kinect device.
+
+ The following example shows how to use this class to acquire data
+ (depth map and color image) from a Kinect.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vpKinect.h>
+
+int main() {
+#ifdef VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES
+ // Init Kinect device
+#ifdef VISP_HAVE_LIBFREENECT_OLD
+ // This is the way to initialize Freenect with an old version of libfreenect
+ // package under ubuntu lucid 10.04
+ Freenect::Freenect<vpKinect> freenect;
+ vpKinect * kinect = &freenect.createDevice(0);
+#else
+ Freenect::Freenect freenect;
+ vpKinect * kinect = &freenect.createDevice<vpKinect>(0);
+#endif
+ kinect->start(); // Start acquisition thread
+
+ // Set tilt angle
+ float angle = -5;
+ kinect->setTiltDegrees(angle);
+
+ vpImage<unsigned char> I(480,640);
+ vpImage<vpRGBa> Irgb(480,640);
+ vpImage<float> dmap(480,640);
+
+ // Acquisition loop
+ for (int i=0; i<100; i++)
+ {
+ kinect->getDepthMap(dmap,I);
+ kinect->getRGB(Irgb);
+ }
+ kinect->stop(); // Stop acquisition thread
+#endif
+ return 0;
+}
+ \endcode
+*/
+class VISP_EXPORT vpKinect : public Freenect::FreenectDevice
+{
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpKinect(const vpKinect &); // Not implemented!
+// vpKinect &operator=(const vpKinect &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ /*!
+ Depth map resolution.
+ */
+ typedef enum {
+ DMAP_LOW_RES, /*!< Depth map has a resolution of 320 by 240. */
+ DMAP_MEDIUM_RES /*!< Depth map has a resolution of 640 by 480. */
+ } vpDMResolution;
+
+ vpKinect(freenect_context *ctx, int index);
+ virtual ~vpKinect();
+
+ void start(vpKinect::vpDMResolution res = DMAP_LOW_RES);
+ void stop();
+
+ bool getDepthMap(vpImage<float>& map);
+ bool getDepthMap(vpImage<float>& map, vpImage<unsigned char>& Imap);
+ bool getRGB(vpImage<vpRGBa>& IRGB);
+
+
+ inline void getIRCamParameters(vpCameraParameters &cam) const {
+ cam = IRcam;
+ }
+ inline void getRGBCamParameters(vpCameraParameters &cam) const {
+ cam = RGBcam;
+ }
+ inline void setIRCamParameters(const vpCameraParameters &cam) {
+ IRcam = cam;
+ }
+ inline void setRGBCamParameters(const vpCameraParameters &cam) {
+ RGBcam = cam;
+ }
+
+ void warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped);//warp the RGB image into the Depth camera frame
+
+ private:
+ //!Instantiation of Freenect virtual functions
+ // Do not call directly even in child
+ void VideoCallback(void* rgb, uint32_t timestamp);
+
+ // Do not call directly even in child
+ void DepthCallback(void* depth, uint32_t timestamp);
+
+ private:
+ vpMutex m_rgb_mutex;
+ vpMutex m_depth_mutex;
+
+ vpCameraParameters RGBcam, IRcam;//intrinsic parameters of the two cameras
+ vpHomogeneousMatrix rgbMir;//Transformation from IRcam coordinate frame to RGBcam coordinate frame.
+ vpHomogeneousMatrix irMrgb;//Transformation from RGBcam coordinate frame to IRcam coordinate frame .
+ vpDMResolution DMres;
+ unsigned int hd;//height of the depth map
+ unsigned int wd;//width of the depth map
+
+ //Access protected by a mutex:
+ vpImage<float> dmap;
+ vpImage<vpRGBa> IRGB;
+ bool m_new_rgb_frame;
+ bool m_new_depth_map;
+ bool m_new_depth_image;
+ unsigned int height;//height of the rgb image
+ unsigned int width;//width of the rgb image
+
+};
+
+#endif
+
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpLaserScan.h b/modules/sensor/include/visp3/sensor/vpLaserScan.h
new file mode 100644
index 0000000..877e263
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpLaserScan.h
@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Laser scan data structure.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpLaserScan_h
+#define vpLaserScan_h
+
+/*!
+ \file vpLaserScan.h
+
+ \brief Implements a laser scan data structure.
+
+*/
+
+#include "visp3/sensor/vpScanPoint.h"
+
+#include <vector>
+
+/*!
+
+ \class vpLaserScan
+ \ingroup group_sensor_laserscanner
+
+ \brief Implements a laser scan data structure that contains
+ especially the list of scanned points that have been recorded for
+ this laser scan.
+
+ Other data as the start/stop angle, the start/end timestamp are
+ also considered.
+ */
+class VISP_EXPORT vpLaserScan
+{
+ public:
+ /*! Default constructor that initialize all the internal variable to zero. */
+ vpLaserScan()
+ : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
+ numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
+ {
+ }
+ /*! Copy constructor. */
+ vpLaserScan(const vpLaserScan &scan)
+ : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
+ numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
+ {
+ startTimestamp = scan.startTimestamp;
+ endTimestamp = scan.endTimestamp;
+ measurementId = scan.measurementId;
+ numSteps = scan.numSteps;
+ startAngle = scan.startAngle;
+ stopAngle = scan.stopAngle;
+ numPoints = scan.numPoints;
+ listScanPoints = scan.listScanPoints;
+ }
+ /*! Default destructor that does nothing. */
+ virtual ~vpLaserScan() {};
+ /*! Add the scan point at the end of the list. */
+ inline void addPoint(const vpScanPoint &p) {
+ listScanPoints.push_back( p );
+ }
+ /*! Drop the list of points. */
+ inline void clear() {
+ listScanPoints.clear( );
+ }
+ /*! Get the list of points. */
+ inline std::vector<vpScanPoint> getScanPoints() {
+ return listScanPoints;
+ }
+ /*! Specifies the id of former measurements and increases with
+ every measurement. */
+ inline void setMeasurementId(const unsigned short &id) {
+ this->measurementId = id;
+ }
+ /*! Start time of measurement. */
+ inline void setStartTimestamp(const double &start_timestamp) {
+ this->startTimestamp = start_timestamp;
+ }
+ /*! End time of measurement. */
+ inline void setEndTimestamp(const double &end_timestamp) {
+ this->endTimestamp = end_timestamp;
+ }
+ /*! Angular steps per scanner rotation. */
+ inline void setNumSteps(const unsigned short &num_steps) {
+ this->numSteps = num_steps;
+ }
+ /*! Start angle of the measurement in angular steps. */
+ inline void setStartAngle(const short &start_angle) {
+ this->startAngle = start_angle;
+ }
+ /*! Stop angle of the measurement in angular steps. */
+ inline void setStopAngle(const short &stop_angle) {
+ this->stopAngle = stop_angle;
+ }
+ /*! Number of measured points of the measurement. */
+ inline void setNumPoints(const unsigned short &num_points) {
+ this->numPoints = num_points;
+ }
+ /*! Return the measurement start time. */
+ inline double getStartTimestamp() {
+ return startTimestamp;
+ }
+ /*! Return the measurement end time. */
+ inline double getEndTimestamp() {
+ return endTimestamp;
+ }
+
+ private:
+ std::vector<vpScanPoint> listScanPoints;
+ double startTimestamp;
+ double endTimestamp;
+ unsigned short measurementId;
+ unsigned short numSteps;
+ short startAngle;
+ short stopAngle;
+ unsigned short numPoints;
+
+};
+
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpLaserScanner.h b/modules/sensor/include/visp3/sensor/vpLaserScanner.h
new file mode 100644
index 0000000..7c9cb72
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpLaserScanner.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic laser scanner.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpLaserScanner_h
+#define vpLaserScanner_h
+
+
+#include "visp3/core/vpConfig.h"
+
+/*!
+
+ \file vpLaserScanner.h
+
+ \brief Implements a generic laser scanner.
+ */
+
+/*!
+
+ \class vpLaserScanner
+ \ingroup group_sensor_laserscanner
+
+ \brief Class that defines a generic laser scanner.
+ */
+class VISP_EXPORT vpLaserScanner
+{
+ public:
+ /*! Default constructor that initialize all the internal variable to zero. */
+ vpLaserScanner() : ip("null"), port(0) {};
+ /*! Copy constructor. */
+ vpLaserScanner(const vpLaserScanner &scanner)
+ : ip("null"), port(0)
+ {
+ ip = scanner.ip;
+ port = scanner.port;
+ };
+ /*! Default destructor that does nothing. */
+ virtual ~vpLaserScanner() {};
+
+ /*! Set the Ethernet address of the laser. */
+ void setIpAddress(std::string ip_address) {
+ this->ip = ip_address;
+ };
+
+ /*! Set the communication port. */
+ void setPort(int com_port) {
+ this->port = com_port;
+ };
+
+ protected:
+ std::string ip;
+ int port;
+};
+
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h
new file mode 100644
index 0000000..397b0ae
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Cameras video capture using OpenCV library.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpOpenCVGrabber.h
+ \brief class for cameras video capture using OpenCV library.
+*/
+
+#ifndef vpOpenCVGrabber_h
+#define vpOpenCVGrabber_h
+
+#include <visp3/core/vpConfig.h>
+
+#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020101
+# include <opencv2/highgui/highgui.hpp>
+#else
+# include <highgui.h>
+#endif
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+ \class vpOpenCVGrabber
+
+ \ingroup Framegrabber CameraDriver
+
+ \brief Class for cameras video capture using OpenCV library.
+
+ Needs OpenCV available on http://opencv.willowgarage.com/wiki/.
+
+ The code below available in tutorial-grabber-opencv.cpp shows how to grab and
+ display images using OpenCV wrappers impremented in ViSP.
+ \include tutorial-grabber-opencv.cpp
+
+ Note that it is also possible to grab images using OpenCV library by using
+ directly OpenCV cv::VideoCapture class. The following code corresponding to
+ tutorial-grabber-opencv-bis.cpp shows how to grab images in a cv::Mat structure
+ and then convert OpenCV images in ViSP images.
+ \include tutorial-grabber-opencv-bis.cpp
+
+ An other example very close to the previous one available in grabOpenCV-2.cpp
+ shows how to grab images in OpenCV IplImage structure and then how to convert
+ OpenCV images in ViSP images.
+
+ */
+class VISP_EXPORT vpOpenCVGrabber : public vpFrameGrabber
+{
+ private:
+
+ CvCapture *capture;
+ int DeviceType;
+ bool flip;
+
+ public:
+
+ vpOpenCVGrabber();
+ ~vpOpenCVGrabber();
+
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+ IplImage* acquire();
+
+ void close();
+
+ void getFramerate(double & framerate);
+ void setFramerate(const double framerate);
+
+ void setWidth(const unsigned int width);
+ void setHeight(const unsigned int height);
+
+ void setDeviceType(int type);
+
+ void setFlip(bool flipType);
+};
+
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpScanPoint.h b/modules/sensor/include/visp3/sensor/vpScanPoint.h
new file mode 100644
index 0000000..52357c0
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpScanPoint.h
@@ -0,0 +1,263 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Single laser scanner point.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpScanPoint_h
+#define vpScanPoint_h
+
+#include <visp3/core/vpMath.h>
+
+#include <ostream>
+#include <sstream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <math.h>
+
+/*!
+ \file vpScanPoint.h
+
+ \brief Implements a single laser scanner point.
+*/
+
+/*!
+
+ \class vpScanPoint
+ \ingroup group_sensor_laserscanner
+
+ \brief Class that defines a single laser scanner point.
+
+ This class stores data of a single scan point as:
+
+ - cartesian coordinates in the 3D space that are available throw
+ getX(), getY() and getZ() methods.
+
+ - polar coordinates that are the native data provided by a laser
+ scanner. By polar coordinates we mean here the radial distance and the
+ horizontal angle of a point in the scanner layer and an additional
+ vertical angle that gives the orientation of the layer.
+
+*/
+class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not be added since this class is complete inline
+{
+ public:
+ /*! Default constructor. */
+ inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {}
+ /*! Copy constructor. */
+ inline vpScanPoint(const vpScanPoint &scanpoint) : rDist(0), hAngle(0), vAngle(0) {
+ this->rDist = scanpoint.rDist;
+ this->hAngle = scanpoint.hAngle;
+ this->vAngle = scanpoint.vAngle;
+ }
+ /*!
+ Set the polar point coordinates.
+ \param r_dist : Radial distance in meter.
+ \param h_angle : Horizontal angle in radian.
+ \param v_angle : Vertical angle in radian.
+ */
+ inline vpScanPoint(double r_dist, double h_angle, double v_angle)
+ : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
+ {
+ this->rDist = r_dist;
+ this->hAngle = h_angle;
+ this->vAngle = v_angle;
+ }
+ /*! Destructor that does nothing. */
+ inline virtual ~vpScanPoint() {};
+ /*!
+ Set the polar point coordinates.
+ \param r_dist : Radial distance in meter.
+ \param h_angle : Horizontal angle in radian.
+ \param v_angle : Vertical angle in radian.
+ */
+ inline void setPolar(double r_dist, double h_angle, double v_angle) {
+ this->rDist = r_dist;
+ this->hAngle = h_angle;
+ this->vAngle = v_angle;
+ }
+ /*!
+ Return the radial distance in meter.
+ */
+ inline double getRadialDist() const {
+ return ( this->rDist );
+ }
+ /*!
+ Returns the polar elevation (vertical) angle in radian.
+ */
+ inline double getVAngle() const {
+ return ( this->vAngle );
+ }
+ /*!
+ Returns the polar elevation (vertical) angle in radian.
+ */
+ inline double getHAngle() const {
+ return ( this->hAngle );
+ }
+ /*!
+ Returns the cartesian x coordinate.
+
+ The x and y axis define an horizontal plane, where x is oriented
+ positive in front of the laser while y on the left side.
+
+ */
+ inline double getX() const {
+ return ( rDist * cos(this->hAngle) * cos(this->vAngle) );
+ }
+ /*!
+ Returns the cartesian y coordinate.
+
+ The x and y axis define an horizontal plane, where x is oriented
+ positive in front of the laser while y on the left side.
+
+ */
+ inline double getY() const {
+ return ( rDist * sin(this->hAngle) );
+ }
+ /*!
+ Returns the cartesian z coordinate.
+
+ The z axis is vertical and oriented in direction of the sky.
+
+ */
+ inline double getZ() const {
+ return ( rDist * cos(this->hAngle) * sin(this->vAngle) );
+ }
+
+ friend inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p);
+
+ /*!
+
+ Returns true if sp1 and sp2 are equal; otherwire returns false.
+
+ */
+ 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();
+ double rd2 = sp2.getRadialDist();
+ double ha2 = sp2.getHAngle();
+ double va2 = sp2.getVAngle();
+
+ return ( ( std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
+ &&
+ ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
+ &&
+ ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
+ }
+
+ /*!
+
+ Returns true if sp1 and sp2 are different; otherwire returns false.
+
+ */
+ 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();
+ double rd2 = sp2.getRadialDist();
+ double ha2 = sp2.getHAngle();
+ double va2 = sp2.getVAngle();
+ return ( ( std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
+ ||
+ ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
+ ||
+ ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
+ }
+
+ private:
+ double rDist;
+ double hAngle;
+ double vAngle;
+};
+
+/*!
+
+ Print the values of the scan point on the output stream. Data are
+ separated by a white space. Data that are print are first the
+ polar coordinates, than the cartesian coordinates:
+ - the radial distance in meter
+ - the horizontal angle in radian
+ - the vertical angle in radian
+ - the cartesian X coordinate
+ - the cartesian Y coordinate
+ - the cartesian Z coordinate
+
+ The following code
+
+ \code
+#include <iostream>
+#include <visp3/sensor/vpScanPoint.h>
+
+int main()
+{
+vpScanPoint p;
+double radialDistance = 3; // 3 meters
+double horizontalAngle = 1.12; // 1.12 radian
+double verticalAngle = 0; // 0 radian for a horizontal layer
+
+p.setPolar(radialDistance, horizontalAngle, verticalAngle);
+
+std::cout << p << std::endl;
+}
+ \endcode
+ will produce the prints
+ \code
+"3 1.12 0 1.307047339 2.700301327 0"
+ \endcode
+
+ */
+inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p) {
+ std::ios_base::fmtflags original_flags = s.flags();
+
+ s.precision(10);
+ s << p.getRadialDist() << " "
+ << p.getHAngle() << " "
+ << p.getVAngle() << " "
+ << p.getX() << " "
+ << p.getY() << " " << p.getZ();
+
+ s.setf(original_flags); // restore s to standard state
+
+ return s;
+}
+
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpSickLDMRS.h b/modules/sensor/include/visp3/sensor/vpSickLDMRS.h
new file mode 100644
index 0000000..0f6f1b8
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpSickLDMRS.h
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Sick LD-MRS laser driver.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpSickLDMRS_h
+#define vpSickLDMRS_h
+
+#include <visp3/core/vpConfig.h>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+
+#include <arpa/inet.h>
+#include <iostream>
+#include <vector>
+#include <string.h>
+
+#include <visp3/sensor/vpScanPoint.h>
+#include <visp3/sensor/vpLaserScan.h>
+#include <visp3/sensor/vpLaserScanner.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpException.h>
+
+/*!
+
+ \file vpSickLDMRS.h
+
+ \brief Driver for the Sick LD-MRS laser scanner.
+*/
+
+/*!
+
+ \class vpSickLDMRS
+
+ \ingroup group_sensor_laserscanner
+
+ \brief Driver for the Sick LD-MRS laser scanner.
+
+ \warning For the moment, this driver works only on UNIX platform.
+
+ The code below shows how the four laser scan provided by the Sick
+ LD-MRS could be acquired.
+
+ \code
+#include "visp3/sensor/vpSickLDMRS.h"
+
+int main()
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ std::string ip = "131.254.12.119";
+
+ vpSickLDMRS laser;
+ laser.setIpAddress(ip);
+ laser.setup();
+
+ vpLaserScan laserscan[4];
+ for ( ; ; ) {
+ // Get the measured points in the four layers
+ laser.measure(laserscan);
+
+ // Prints all the measured points
+ for (int layer=0; layer<4; layer++) {
+ std::vector<vpScanPoint> pointsInLayer = laserscan[layer].getScanPoints();
+ vpScanPoint p;
+
+ for (unsigned int i=0; i < pointsInLayer.size(); i++) {
+ std::cout << pointsInLayer[i] << std::endl;
+ }
+ }
+ }
+#endif
+}
+ \endcode
+*/
+class VISP_EXPORT vpSickLDMRS : public vpLaserScanner
+{
+ public:
+ enum MagicWord {
+ MagicWordC2 = 0xAFFEC0C2 ///< The magic word that allows to identify the messages that are sent by the Sick LD-MRS.
+ };
+ enum DataType {
+ MeasuredData = 0x2202 ///< Flag to indicate that the body of a message contains measured data.
+ };
+ vpSickLDMRS();
+ /*! Copy constructor. */
+ vpSickLDMRS(const vpSickLDMRS &sick)
+ : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), time_offset(0),
+ isFirstMeasure(true), maxlen_body(104000)
+ {
+ *this = sick;
+ };
+ virtual ~vpSickLDMRS();
+ /*! Copy constructor. */
+ vpSickLDMRS &operator=(const vpSickLDMRS &sick)
+ {
+ socket_fd = sick.socket_fd;
+ vAngle = sick.vAngle;
+ time_offset = sick.time_offset;
+ isFirstMeasure = sick.isFirstMeasure;
+ maxlen_body = sick.maxlen_body;
+ if (body) delete [] body;
+ body = new unsigned char [104000];
+ memcpy(body, sick.body, maxlen_body);
+ return (*this);
+ };
+
+ bool setup(std::string ip, int port);
+ bool setup();
+ bool measure(vpLaserScan laserscan[4]);
+
+ protected:
+#if defined(_WIN32)
+ SOCKET socket_fd;
+#else
+ int socket_fd;
+#endif
+ unsigned char *body;
+ vpColVector vAngle; // constant vertical angle for each layer
+ double time_offset;
+ bool isFirstMeasure;
+ size_t maxlen_body;
+ };
+
+#endif
+
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h
new file mode 100644
index 0000000..88f0638
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h
@@ -0,0 +1,372 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Video for linux two framegrabber.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpV4l2Grabber.h
+ \brief class for the Video For Linux 2 video device framegrabbing.
+ \ingroup libdevice
+*/
+
+#ifndef vpV4l2Grabber_hh
+#define vpV4l2Grabber_hh
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_V4L2
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h> // Video For Linux Two interface
+#include <libv4l2.h> // Video For Linux Two interface
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+ \class vpV4l2Grabber
+
+ \ingroup group_sensor_camera
+
+ \brief Class for the Video4Linux2 video device.
+
+ Information about Video4Linux can be found on
+ http://linuxtv.org/v4lwiki/index.php/Main_Page
+
+ This class was tested with a Pinnacle PCTV Studio/Rave board but
+ also with the following webcams (Logitech QuickCam Vision Pro 9000,
+ Logitech QuickCam Orbit AF, Dell latitude E6400 internal webcam).
+
+ If the grabbing fail with a webcam, it means probably that you don't
+ have the read/write permission on the /dev/video%%d device. You can
+ set the right permissions by "sudo chmod a+rw /dev/video*".
+
+ If the grabbing fails when the camera is attached to a bttv PCI
+ card, it means potentially that you have not configured the linux
+ bttv kernel module according to your board.
+
+ For that, depending on your linux distribution check the card id in
+ - /usr/share/doc/kernel-doc-2.4.20/video4linux/bttv/CARDLIST
+ - or /usr/share/doc/kernel-doc-2.6.20/Documentation/video4linux/CARDLIST.bttv
+
+ For example, the card id of a Pinnacle PCTV Studio/Rave board is 39.
+ Once this id is determined, you have to set the bttv driver with, by adding
+ \verbatim
+ options bttv card=39
+ \endverbatim
+ in one of theses files :
+ - /etc/modules.conf
+ - or /etc/modprobe.conf
+
+ This first example available in tutorial-grabber-v4l2.cpp shows how to grab
+ and display images from an usb camera.
+ \include tutorial-grabber-v4l2.cpp
+
+ This other example shows how to use this grabber with an analogic camera
+ attached to a bttv PCI card.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#if defined(VISP_HAVE_V4L2)
+ vpImage<unsigned char> I;
+ vpV4l2Grabber g;
+ g.setInput(2); // Input 2 on the board
+ g.setFramerate(vpV4l2Grabber::framerate_25fps); // 25 fps
+ g.setWidth(768); // Acquired images are 768 width
+ g.setHeight(576); // Acquired images are 576 height
+ g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
+ g.open(I); // Open the grabber
+
+ g.acquire(I); // Acquire a 768x576 grey image
+ vpImageIo::write(I, "image.pgm"); // Save the image on the disk
+#endif
+}
+ \endcode
+
+
+ \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
+
+
+ \sa vpFrameGrabber
+*/
+class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber
+{
+public:
+ static const unsigned int DEFAULT_INPUT;
+ static const unsigned int DEFAULT_SCALE;
+ static const __u32 MAX_INPUTS;
+ static const __u32 MAX_NORM;
+ static const __u32 MAX_FORMAT;
+ static const unsigned int MAX_CTRL;
+ static const unsigned int MAX_BUFFERS;
+ static const unsigned int FRAME_SIZE;
+
+ /*! \enum vpV4l2FramerateType
+ Frame rate type for capture.
+ */
+ typedef enum
+ {
+ framerate_50fps, //!< 50 frames per second
+ framerate_25fps //!< 25 frames per second
+ } vpV4l2FramerateType;
+
+ /*! \enum vpV4l2FrameFormatType
+ Frame format type for capture.
+ */
+ typedef enum
+ {
+ V4L2_FRAME_FORMAT, /*!< a field only */
+ V4L2_IMAGE_FORMAT /*!< an interlaced image */
+ } vpV4l2FrameFormatType;
+
+ /*! \enum vpV4l2PixelFormatType
+ Pixel format type for capture.
+ */
+ typedef enum {
+ V4L2_GREY_FORMAT, /*!< 8 Greyscale */
+ V4L2_RGB24_FORMAT, /*!< 24 RGB-8-8-8 */
+ V4L2_RGB32_FORMAT, /*!< 32 RGB-8-8-8-8 */
+ V4L2_BGR24_FORMAT, /*!< 24 BGR-8-8-8 */
+ V4L2_YUYV_FORMAT, /*!< 16 YUYV 4:2:2 */
+ V4L2_MAX_FORMAT
+ } vpV4l2PixelFormatType;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ struct ng_video_fmt {
+ unsigned int pixelformat; /* VIDEO_* */
+ unsigned int width;
+ unsigned int height;
+ unsigned int bytesperline; /* zero for compressed formats */
+ };
+
+
+ struct ng_video_buf {
+ struct ng_video_fmt fmt;
+ size_t size;
+ unsigned char *data;
+ int refcount;
+ };
+#endif
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpV4l2Grabber(const vpV4l2Grabber &)
+// : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+// fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+// waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
+// m_input(vpV4l2Grabber::DEFAULT_INPUT),
+// m_framerate(vpV4l2Grabber::framerate_25fps),
+// m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+// m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+// {
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// }
+// vpV4l2Grabber &operator=(const vpV4l2Grabber &){
+// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpV4l2Grabber();
+ vpV4l2Grabber(bool verbose);
+ vpV4l2Grabber(unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
+ vpV4l2Grabber(vpImage<unsigned char> &I,
+ unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
+ vpV4l2Grabber(vpImage<vpRGBa> &I,
+ unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
+ virtual ~vpV4l2Grabber() ;
+
+public:
+ void open(vpImage<unsigned char> &I) ;
+ void open(vpImage<vpRGBa> &I) ;
+
+ void acquire(vpImage<unsigned char> &I) ;
+ void acquire(vpImage<unsigned char> &I, struct timeval ×tamp) ;
+ void acquire(vpImage<vpRGBa> &I) ;
+ void acquire(vpImage<vpRGBa> &I, struct timeval ×tamp) ;
+ bool getField();
+ vpV4l2FramerateType getFramerate();
+ /*!
+
+ Get the pixel format used for capture.
+
+ \return Camera pixel format coding.
+
+ */
+ inline vpV4l2PixelFormatType getPixelFormat()
+ {
+ return (this->m_pixelformat);
+ }
+ /*!
+ Activates the verbose mode to print additional information on stdout.
+ \param verbose : If true activates the verbose mode.
+ */
+ void setVerboseMode(bool verbose) {
+ this->m_verbose = verbose;
+ };
+ void setFramerate(vpV4l2FramerateType framerate);
+
+ void setInput(unsigned input = vpV4l2Grabber::DEFAULT_INPUT) ;
+
+ /*!
+ Set image width to acquire.
+
+ */
+ inline void setWidth(unsigned w)
+ {
+ this->width = w;
+ }
+ /*!
+ Set image height to acquire.
+
+ */
+ inline void setHeight(unsigned h)
+ {
+ this->height = h;
+ }
+
+ void setScale(unsigned scale = vpV4l2Grabber::DEFAULT_SCALE) ;
+
+ /*!
+
+ Set the number of buffers required for streaming data.
+
+ For non real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to set
+ the number of buffers to 3.
+
+ \param nbuffers : Number of ring buffers.
+
+ */
+ inline void setNBuffers(unsigned nbuffers)
+ {
+ this->m_nbuffers = nbuffers;
+ }
+
+ /*!
+ Set the device name.
+
+ \param devname : Device name (like /dev/video0).
+
+ */
+ inline void setDevice(const std::string &devname)
+ {
+ sprintf(device, "%s", devname.c_str());
+ }
+ /*!
+
+ Set the pixel format for capture.`If the specified pixel format is
+ out of range, we set the V4L2_RGB24_FORMAT.
+
+ \param pixelformat : Camera pixel format coding.
+
+ */
+ inline void setPixelFormat(vpV4l2PixelFormatType pixelformat)
+ {
+ this->m_pixelformat = pixelformat;
+ if (this->m_pixelformat >= V4L2_MAX_FORMAT)
+ this->m_pixelformat = V4L2_RGB24_FORMAT;
+ }
+
+ void close();
+
+private:
+
+ void setFormat();
+ /*!
+ Set the frame format.
+
+ \param frameformat :
+ - vpV4l2Grabber::V4L2_FRAME_FORMAT: capture alternate fields (or frames),
+ - vpV4l2Grabber::V4L2_IMAGE_FORMAT: capture interlaced images.
+ */
+ inline void setFrameFormat(vpV4l2FrameFormatType frameformat)
+ {
+ this->m_frameformat = frameformat;
+ }
+ void open();
+ void getCapabilities();
+ void startStreaming();
+ void stopStreaming();
+ unsigned char * waiton(__u32 &index, struct timeval ×tamp);
+ int queueBuffer();
+ void queueAll();
+ void printBufInfo(struct v4l2_buffer buf);
+
+
+ int fd;
+ char device[FILENAME_MAX];
+ /* device descriptions */
+ struct v4l2_capability cap;
+ struct v4l2_streamparm streamparm;
+ struct v4l2_input *inp; //[vpV4l2Grabber::MAX_INPUTS];
+ struct v4l2_standard *std; //[vpV4l2Grabber::MAX_NORM];
+ struct v4l2_fmtdesc *fmt; //[vpV4l2Grabber::MAX_FORMAT];
+ struct v4l2_queryctrl *ctl; //[vpV4l2Grabber::MAX_CTRL*2];
+
+ /* capture */
+ int fps;
+ struct v4l2_format fmt_v4l2;
+ struct ng_video_fmt fmt_me;
+ struct v4l2_requestbuffers reqbufs;
+ struct v4l2_buffer *buf_v4l2; //[vpV4l2Grabber::MAX_BUFFERS];
+ struct ng_video_buf *buf_me; //[vpV4l2Grabber::MAX_BUFFERS];
+ unsigned int queue;
+ unsigned int waiton_cpt;
+ __u32 index_buffer; //!< index of the buffer in use
+
+ bool m_verbose;
+ unsigned m_nbuffers;
+ unsigned int field;
+ bool streaming;
+
+ unsigned m_input;
+ vpV4l2FramerateType m_framerate;
+ vpV4l2FrameFormatType m_frameformat;
+ vpV4l2PixelFormatType m_pixelformat;
+} ;
+
+#endif
+#endif
+
diff --git a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp
new file mode 100644
index 0000000..786026d
--- /dev/null
+++ b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp
@@ -0,0 +1,732 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
+ *
+ * Authors:
+ * Lucas Lopes Lemos FEMTO-ST, AS2M departement, Besancon
+ * Guillaume Laurent FEMTO-ST, AS2M departement, Besancon
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_CMU1394
+
+#include <iostream>
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+
+/*!
+ Basic constructor.
+ */
+vp1394CMUGrabber::vp1394CMUGrabber()
+{
+ // public members
+ init = false;
+
+ // protected members
+ width = height = -1;
+
+ // private members
+ camera = new C1394Camera;
+ index = 0; // If a camera was not selected the first one (index = 0) will be used
+ _format = _mode = _fps = -1;
+ _modeauto=true;
+}
+
+/*!
+ Destructor.
+ */
+vp1394CMUGrabber::~vp1394CMUGrabber( )
+{
+ close();
+ // delete camera instance
+ if (camera) {
+ delete camera;
+ camera = NULL;
+ }
+}
+
+/*!
+ Select the camera on the bus from its index. The first camera found on the bus has index 0.
+ \param cam_id : Camera index.
+*/
+void
+vp1394CMUGrabber::selectCamera(int cam_id)
+{
+ int camerror;
+
+ index = cam_id ;
+
+ camerror = camera->SelectCamera(index);
+ if ( camerror!= CAM_SUCCESS)
+ {
+ switch (camerror)
+ {
+ case CAM_ERROR_PARAM_OUT_OF_RANGE:
+ vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is not present") );
+ break;
+ case CAM_ERROR_BUSY:
+ vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
+ break;
+ case CAM_ERROR:
+ vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
+ break;
+ }
+ close();
+ }
+} // end camera select
+
+/*!
+ Init the selected camera.
+ */
+void
+vp1394CMUGrabber::initCamera()
+{
+ if (init == false)
+ {
+ int camerror;
+
+ if (camera->CheckLink() != CAM_SUCCESS)
+ {
+ vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no detected camera") );
+ }
+
+ camerror = camera->InitCamera();
+ if ( camerror != CAM_SUCCESS )
+ {
+ switch (camerror)
+ {
+ case CAM_ERROR_NOT_INITIALIZED:
+ vpERROR_TRACE("vp1394CMUGrabber error: No camera selected",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no selected camera") );
+ break;
+ case CAM_ERROR_BUSY:
+ vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
+ break;
+ case CAM_ERROR:
+ vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
+ break;
+ }
+ close();
+ }
+
+ if (camera->Has1394b())
+ camera->Set1394b(TRUE);
+
+ // Get the current settings
+ _format = camera->GetVideoFormat();
+ _mode = camera->GetVideoMode();
+ _color = getVideoColorCoding();
+ //std::cout << "format: " << _format << std::endl;
+ //std::cout << "mode: " << _mode << std::endl;
+ //std::cout << "color coding: " << _color << std::endl;
+
+ // Set trigger off
+ camera->GetCameraControlTrigger()->SetOnOff(false);
+
+ unsigned long w, h;
+ camera->GetVideoFrameDimensions(&w, &h);
+ this->width = w;
+ this->height = h;
+
+ // start acquisition
+ if (camera->StartImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while starting image acquisition") );
+ }
+
+ init = true;
+ }
+
+} // end camera init
+
+
+/*!
+ Initialization of the grabber using a greyscale image.
+ \param I : gray level image.
+ */
+void
+vp1394CMUGrabber::open(vpImage<unsigned char> &I)
+{
+ initCamera();
+ I.resize(this->height, this->width);
+}
+
+/*!
+ Initialization of the grabber using a color image.
+ \param I : color image.
+ */
+void
+vp1394CMUGrabber::open(vpImage<vpRGBa> &I)
+{
+ initCamera();
+ I.resize(this->height, this->width);
+}
+
+/*!
+ Grabs a grayscale image from the selected camera. If the camera color
+ coding differs from vp1394CMUGrabber::MONO8, the acquired image is
+ converted in a gray level image to match the requested format.
+
+ \param I : Acquired gray level image.
+ */
+void
+vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
+{
+ // get image data
+ unsigned long length;
+ unsigned char *rawdata = NULL ;
+ int dropped;
+ unsigned int size;
+
+ open(I);
+
+ camera->AcquireImageEx(TRUE,&dropped);
+ rawdata = camera->GetRawData(&length);
+
+ size = I.getSize();
+ switch(_color) {
+ case vp1394CMUGrabber::MONO8:
+ memcpy(I.bitmap, (unsigned char *) rawdata, size);
+ break;
+ case vp1394CMUGrabber::MONO16:
+ vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV411:
+ vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV422:
+ vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV444:
+ vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::RGB8:
+ vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Format conversion not implemented. "
+ "Acquisition failed.") );
+ break;
+ };
+
+ //unsigned short depth = 0;
+ //camera->GetVideoDataDepth(&depth);
+ //std::cout << "depth: " << depth << " computed: " << (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
+
+
+ //memcpy(I.bitmap,rawdata,length);
+
+}
+
+/*!
+ Grabs a color image from the selected camera. Since the cameras
+ are not able to grab RGBa color coding format, the acquired image is
+ converted in a RGBa to match the requested format. This transformation
+ could be time consuming.
+
+ \param I : Acquired color image in RGBa format.
+ */
+void
+vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I)
+{
+ // get image data
+ unsigned long length;
+ unsigned char *rawdata = NULL;
+ int dropped;
+ unsigned int size;
+
+ open(I);
+
+ camera->AcquireImageEx(TRUE,&dropped);
+ rawdata = camera->GetRawData(&length);
+ size = I.getWidth() * I.getHeight();
+
+ switch (_color) {
+ case vp1394CMUGrabber::MONO8:
+ vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::MONO16:
+ vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV411:
+ vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV422:
+ vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV444:
+ vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::RGB8:
+ size = length / 3;
+ vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Format conversion not implemented. "
+ "Acquisition failed.") );
+ break;
+ };
+}
+
+/*!
+ Stop the acquisition of images and free the camera.
+ */
+void
+vp1394CMUGrabber::close()
+{
+ // stop acquisition
+ if (camera->IsAcquiring()) {
+ // stop acquisition
+ if (camera->StopImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while stopping image acquisition") );
+ }
+ }
+
+ init = false;
+}
+
+/*!
+ Set the gain and the shutter values.
+ \sa setGain(), setShutter()
+ */
+void
+vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
+{
+ setShutter(shutter);
+ setGain(gain);
+}
+
+/*!
+ Get the number of connected cameras.
+ */
+int
+vp1394CMUGrabber::getNumberOfConnectedCameras() const
+{
+ int n_cam = camera->RefreshCameraList();
+
+ return n_cam;
+}
+
+/*!
+ Get the gain min and max values.
+
+ \sa setAutoGain(), setGain()
+ */
+void vp1394CMUGrabber::getGainMinMax(unsigned short &min, unsigned short &max)
+{
+ initCamera();
+
+ C1394CameraControl *Control;
+ Control = camera->GetCameraControl(FEATURE_GAIN);
+ Control->Inquire();
+ Control->GetRange(&min, &max);
+}
+/*!
+ Enable auto gain.
+
+ \sa setGain()
+ */
+void vp1394CMUGrabber::setAutoGain()
+{
+ initCamera();
+ camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true);
+}
+/*!
+ Disable auto gain and set the gain to the requested value.
+
+ \sa setAutoGain()
+ */
+void vp1394CMUGrabber::setGain(unsigned short gain)
+{
+ initCamera();
+ _gain = gain;
+
+ unsigned short min,max;
+ C1394CameraControl *Control;
+
+ Control = camera->GetCameraControl(FEATURE_GAIN);
+ Control->Inquire();
+ Control->GetRange(&min,&max);
+
+ if (_gain < min)
+ {
+ _gain = min;
+ std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be less than " << _gain << std::endl;
+ }
+ else if (_gain > max)
+ {
+ _gain = max;
+ std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be greater than " << _gain << std::endl;
+ }
+
+ Control->SetAutoMode(false);
+ if(Control->SetValue(_gain) != CAM_SUCCESS)
+ {
+ std::cout << "vp1394CMUGrabber warning: Can't set gain register value of IEEE 1394 camera number " << index << std::endl;
+ }
+}
+
+/*!
+ Get the shutter min and max values.
+
+ \sa setAutoShutter(), setShutter()
+ */
+void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max)
+{
+ initCamera();
+
+ C1394CameraControl *Control;
+ Control = camera->GetCameraControl(FEATURE_SHUTTER);
+ Control->Inquire();
+ Control->GetRange(&min, &max);
+}
+
+/*!
+ Enable auto shutter.
+
+ \sa setShutter()
+ */
+void vp1394CMUGrabber::setAutoShutter()
+{
+ initCamera();
+ camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true);
+}
+/*!
+ Disable auto shutter and set the shutter to the requested value.
+
+ \sa setAutoShutter()
+ */
+void vp1394CMUGrabber::setShutter(unsigned short shutter)
+{
+ initCamera();
+
+ _shutter = shutter;
+
+ unsigned short min,max;
+ C1394CameraControl *Control;
+
+ Control = camera->GetCameraControl(FEATURE_SHUTTER);
+ Control->Inquire();
+ Control->GetRange(&min,&max);
+
+ if (_shutter < min)
+ {
+ _shutter = min;
+ std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be less than " << _shutter << std::endl;
+ }
+ else if (_shutter > max)
+ {
+ _shutter = max;
+ std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be greater than " << _shutter << std::endl;
+ }
+ Control->SetAutoMode(false);
+ if(Control->SetValue(_shutter) != CAM_SUCCESS)
+ {
+ std::cout << "vp1394CMUGrabber warning: Can't set exposure time register value of IEEE 1394 camera number " << index << std::endl;
+ }
+}
+
+/*!
+ Display information about the camera on the standard output.
+ */
+void
+vp1394CMUGrabber::displayCameraDescription(int cam_id)
+{
+ char buf[512];
+
+ if( camera->GetNumberCameras() > cam_id )
+ {
+ camera->GetNodeDescription(cam_id,buf,512);
+ std::cout << "Camera " << cam_id << ": " << buf << std::endl ;
+
+ }
+ else {
+ std::cout << "Camera " << cam_id << ": camera not found" << std::endl ;
+ }
+}
+
+
+/*!
+ Display camera model on the standard output. Call it after open the grabber.
+ */
+void
+vp1394CMUGrabber::displayCameraModel()
+{
+ char vendor[256] , model[256] , buf[256];
+ LARGE_INTEGER ID;
+
+ camera->GetCameraName(model,sizeof(model));
+ camera->GetCameraVendor(vendor,sizeof(vendor));
+ camera->GetCameraUniqueID(&ID);
+
+ std::cout << "Vendor: " << vendor << std::endl;
+ std::cout << "Model: " << model << std::endl;
+
+ sprintf(buf,"%08X%08X",ID.HighPart,ID.LowPart);
+ std::cout << "UniqueID: " << buf << std::endl;
+
+}
+
+
+/*!
+ Set the camera format and video mode.
+ This method has to be called before open().
+
+ \param format : Camera video format.
+ \param mode : Camera video mode.
+
+ See the following table for the correspondances between the input
+ format and mode and the resulting video color coding.
+
+ <TABLE BORDER="1">
+ <TR><TH> Format </TH><TH> Mode </TH><TH> (H) x (W) </TH><TH> Color </TH></TR>
+ <TR><TD> 0 </TD><TD> 0 </TD><TD> 160 x 120 </TD><TD> YUV444 </TD></TR>
+ <TR><TD> 0 </TD><TD> 1 </TD><TD> 320 x 240 </TD><TD> YUV422 </TD></TR>
+ <TR><TD> 0 </TD><TD> 2 </TD><TD> 640 x 480 </TD><TD> YUV411 </TD></TR>
+ <TR><TD> 0 </TD><TD> 3 </TD><TD> 640 x 480 </TD><TD> YUV422 </TD></TR>
+ <TR><TD> 0 </TD><TD> 4 </TD><TD> 640 x 480 </TD><TD> RGB8 </TD></TR>
+ <TR><TD> 0 </TD><TD> 5 </TD><TD> 640 x 480 </TD><TD> MONO8 </TD></TR>
+ <TR><TD> 0 </TD><TD> 6 </TD><TD> 640 x 480 </TD><TD> MONO16 </TD></TR>
+ <TR><TD> 1 </TD><TD> 0 </TD><TD> 800 x 600 </TD><TD> YUV422 </TD></TR>
+ <TR><TD> 1 </TD><TD> 1 </TD><TD> 800 x 600 </TD><TD> RGB8 </TD></TR>
+ <TR><TD> 1 </TD><TD> 2 </TD><TD> 800 x 600 </TD><TD> MONO8 </TD></TR>
+ <TR><TD> 1 </TD><TD> 3 </TD><TD>1024 x 768 </TD><TD> YUV422 </TD></TR>
+ <TR><TD> 1 </TD><TD> 4 </TD><TD>1024 x 768 </TD><TD> RGB8 </TD></TR>
+ <TR><TD> 1 </TD><TD> 5 </TD><TD>1024 x 768 </TD><TD> MONO8 </TD></TR>
+ <TR><TD> 1 </TD><TD> 6 </TD><TD> 800 x 600 </TD><TD> MONO16 </TD></TR>
+ <TR><TD> 1 </TD><TD> 7 </TD><TD>1024 x 768 </TD><TD> MONO16 </TD></TR>
+ <TR><TD> 2 </TD><TD> 0 </TD><TD>1280 x 960 </TD><TD> YUV422 </TD></TR>
+ <TR><TD> 2 </TD><TD> 1 </TD><TD>1280 x 960 </TD><TD> RGB8 </TD></TR>
+ <TR><TD> 2 </TD><TD> 2 </TD><TD>1280 x 960 </TD><TD> MONO8 </TD></TR>
+ <TR><TD> 2 </TD><TD> 3 </TD><TD>1600 x 1200</TD><TD> YUV422 </TD></TR>
+ <TR><TD> 2 </TD><TD> 4 </TD><TD>1600 x 1200</TD><TD> RGB8 </TD></TR>
+ <TR><TD> 2 </TD><TD> 5 </TD><TD>1600 x 1200</TD><TD> MONO8 </TD></TR>
+ <TR><TD> 2 </TD><TD> 6 </TD><TD>1280 x 960 </TD><TD> MONO16 </TD></TR>
+ <TR><TD> 2 </TD><TD> 7 </TD><TD>1600 x 1200</TD><TD> MONO16 </TD></TR>
+ </TABLE>
+
+ */
+void
+vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode )
+{
+ initCamera();
+
+ _format = format ;
+ _mode = mode ;
+
+ // Set format and mode
+ if ((_format != -1) && (_mode != -1))
+ {
+ if (!camera->HasVideoMode(_format, _mode))
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: The image format is not supported by the IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Video mode not supported") );
+ }
+
+ if (camera->IsAcquiring()) {
+ // stop acquisition
+ if (camera->StopImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while stopping image acquisition") );
+ }
+ }
+
+ if (camera->SetVideoFormat(_format) != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video format") );
+ }
+
+ if (camera->SetVideoMode(_mode) != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video mode") );
+ }
+
+ // start acquisition
+ if (camera->StartImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while starting image acquisition") );
+ }
+
+ // Update Image dimension
+ unsigned long w, h;
+ camera->GetVideoFrameDimensions(&w, &h);
+ this->width = w;
+ this->height = h;
+
+ // Update the color coding
+ _color = getVideoColorCoding();
+ }
+}
+
+/*!
+ Set camera framerate rate. This method has to be called before open().
+
+ \param fps : Value between 0 to 7 used to select a specific camera framerate.
+ See the following table for the correspondances between the input
+ value and the framerate.
+
+ <TABLE BORDER="1">
+ <TR><TH> Value </TH> <TH> Frame rate </TH></TR>
+ <TR><TD> 0 </TD> <TD> 1.875 fps </TD></TR>
+ <TR><TD> 1 </TD> <TD> 3.75 fps </TD></TR>
+ <TR><TD> 2 </TD> <TD> 7.5 fps </TD></TR>
+ <TR><TD> 3 </TD> <TD> 15 fps </TD></TR>
+ <TR><TD> 4 </TD> <TD> 30 fps </TD></TR>
+ <TR><TD> 5 </TD> <TD> 60 fps </TD></TR>
+ <TR><TD> 6 </TD> <TD> 120 fps </TD></TR>
+ <TR><TD> 7 </TD> <TD> 240 fps </TD></TR>
+ </TABLE>
+
+ \sa getFramerate()
+ */
+void
+vp1394CMUGrabber::setFramerate(unsigned long fps)
+{
+ initCamera();
+
+ _fps = fps;
+
+ // Set fps
+ if (_fps!=-1)
+ {
+ if (!camera->HasVideoFrameRate(_format,_mode,_fps))
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported by the IEEE 1394 camera number %i for the selected image format",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"The frame rate is not supported") );
+ }
+
+ if (camera->IsAcquiring()) {
+ // stop acquisition
+ if (camera->StopImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while stopping image acquisition") );
+ }
+ }
+ if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video frame rate") );
+ }
+ // start acquisition
+ if (camera->StartImageAcquisition() != CAM_SUCCESS)
+ {
+ close();
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Error while starting image acquisition") );
+ }
+
+ }
+}
+/*!
+ Get the video framerate.
+
+ \return Value between 0 to 7 corresponding to a specific camera framerate.
+ See the following table for the correspondances between the returned
+ value and the framerate.
+
+ <TABLE BORDER="1">
+ <TR><TH> Value </TH> <TH> Frame rate </TH></TR>
+ <TR><TD> 0 </TD> <TD> 1.875 fps </TD></TR>
+ <TR><TD> 1 </TD> <TD> 3.75 fps </TD></TR>
+ <TR><TD> 2 </TD> <TD> 7.5 fps </TD></TR>
+ <TR><TD> 3 </TD> <TD> 15 fps </TD></TR>
+ <TR><TD> 4 </TD> <TD> 30 fps </TD></TR>
+ <TR><TD> 5 </TD> <TD> 60 fps </TD></TR>
+ <TR><TD> 6 </TD> <TD> 120 fps </TD></TR>
+ <TR><TD> 7 </TD> <TD> 240 fps </TD></TR>
+ </TABLE>
+
+ \sa setFramerate()
+*/
+int
+vp1394CMUGrabber::getFramerate()
+{
+ initCamera();
+ int fps = camera->GetVideoFrameRate();
+ return fps;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vp1394CMUGrabber.cpp.o) has no symbols
+void dummy_vp1394CMUGrabber() {};
+#endif
+
diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp
new file mode 100644
index 0000000..6c63364
--- /dev/null
+++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp
@@ -0,0 +1,3667 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vp1394TwoGrabber.cpp
+ \brief member functions for firewire cameras
+ \ingroup libdevice
+*/
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+/*
+ * Interface with libdc1394 2.x
+ */
+#if defined(VISP_HAVE_DC1394)
+#include <unistd.h>
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpFrameGrabberException.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpTime.h>
+
+const char * vp1394TwoGrabber::strVideoMode[DC1394_VIDEO_MODE_NUM]= {
+ "MODE_160x120_YUV444",
+ "MODE_320x240_YUV422",
+ "MODE_640x480_YUV411",
+ "MODE_640x480_YUV422",
+ "MODE_640x480_RGB8",
+ "MODE_640x480_MONO8",
+ "MODE_640x480_MONO16",
+ "MODE_800x600_YUV422",
+ "MODE_800x600_RGB8",
+ "MODE_800x600_MONO8",
+ "MODE_1024x768_YUV422",
+ "MODE_1024x768_RGB8",
+ "MODE_1024x768_MONO8",
+ "MODE_800x600_MONO16",
+ "MODE_1024x768_MONO16",
+ "MODE_1280x960_YUV422",
+ "MODE_1280x960_RGB8",
+ "MODE_1280x960_MONO8",
+ "MODE_1600x1200_YUV422",
+ "MODE_1600x1200_RGB8",
+ "MODE_1600x1200_MONO8",
+ "MODE_1280x960_MONO16",
+ "MODE_1600x1200_MONO16",
+ "MODE_EXIF",
+ "MODE_FORMAT7_0",
+ "MODE_FORMAT7_1",
+ "MODE_FORMAT7_2",
+ "MODE_FORMAT7_3",
+ "MODE_FORMAT7_4",
+ "MODE_FORMAT7_5",
+ "MODE_FORMAT7_6",
+ "MODE_FORMAT7_7"
+};
+
+const char * vp1394TwoGrabber::strFramerate[DC1394_FRAMERATE_NUM]= {
+ "FRAMERATE_1_875",
+ "FRAMERATE_3_75",
+ "FRAMERATE_7_5",
+ "FRAMERATE_15",
+ "FRAMERATE_30",
+ "FRAMERATE_60",
+ "FRAMERATE_120",
+ "FRAMERATE_240"
+};
+
+const char * vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM]= {
+ "COLOR_CODING_MONO8",
+ "COLOR_CODING_YUV411",
+ "COLOR_CODING_YUV422",
+ "COLOR_CODING_YUV444",
+ "COLOR_CODING_RGB8",
+ "COLOR_CODING_MONO16",
+ "COLOR_CODING_RGB16",
+ "COLOR_CODING_MONO16S",
+ "COLOR_CODING_RGB16S",
+ "COLOR_CODING_RAW8",
+ "COLOR_CODING_RAW16",
+};
+
+
+
+/*!
+ Default constructor.
+
+ By default:
+ - the camera is the first found on the bus,
+ - the ring buffer size is set to 4,
+ - reset the bus attached to the first camera found on the bus.
+
+ Current camera settings can be changed using setCamera() to select the active
+ camera on the bus and than setVideoMode() or setFramerate() to fix the active
+ camera settings. The list of supported video modes and framerates is
+ available using respectively getVideoModeSupported() and
+ getFramerateSupported(). To change the ring buffer size use setRingBufferSize().
+
+ \param reset : If "true", reset the bus attached to the first
+ camera found. Bus reset may help to make firewire working if the
+ program was not properly stopped by a CTRL-C.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \sa setCamera(), setVideoMode(), setFramerate()
+
+*/
+vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
+ : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), camIsOpen(NULL),
+ num_buffers(4), // ring buffer size
+ isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
+ #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ , d(NULL),
+ list(NULL)
+ #endif
+{
+ // protected members
+ width = height = 0;
+
+ // private members
+ init = false;
+
+ reset = false;
+ initialize(reset);
+
+ // open();
+}
+
+/*!
+
+ Destructor.
+
+ Close the firewire grabber.
+
+ \sa close()
+
+*/
+vp1394TwoGrabber::~vp1394TwoGrabber()
+{
+/* if(num_cameras >= 1){
+ delete[] isDataModified;
+ delete[] initialShutterMode;
+ delete[] dataCam;
+ }*/
+ close();
+}
+
+/*!
+
+ If multiples cameras are connected on the bus, select the camero to dial
+ with.
+
+ \param cam_id : A camera identifier or GUID. By identifier, we
+ mean a value comprised between 0 (the first camera found on the bus)
+ and the number of cameras found on the bus and returned by
+ getNumCameras() minus 1. If two cameras are connected on the bus,
+ setting \e camera_id to one allows to communicate with the second
+ one. This identifier is not unique. That is why, it is also possible
+ to select a camera by its GUID, which is unique.
+
+ \exception vpFrameGrabberException::settingError : If the required camera is
+ not reachable.
+
+ Here an example of single capture from the last camera found on the bus:
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ unsigned int ncameras; // Number of cameras on the bus
+ vpImage<unsigned char> I;
+
+ vp1394TwoGrabber g;
+ ncameras = g.getNumCameras();
+
+ int last = 0;
+ if (ncameras > 1)
+ last = ncameras-1;
+
+ g.setCamera(last); // To dial with the last camera on the bus
+ while(1)
+ g.acquire(I);// I contains the frame captured by the last camera on the bus
+#endif
+}
+ \endcode
+
+ If more than one camera is connected, it is also possible to select a specific camera by its GUID:
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I; // Create a gray level image container
+ bool reset = false; // Disable bus reset during construction
+ vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
+
+ unsigned int ncameras; // Number of cameras on the bus
+ ncameras = g.getNumCameras();
+ std::cout << ncameras << " cameras found:" << std::endl;
+
+ for(int i=0; i< ncameras; i++)
+ {
+ g.setCamera(i);
+ uint64_t guid = g.getGuid();
+ printf("camera %d with guid 0x%lx\n", i, guid);
+ }
+
+ // produce:
+ // 2 cameras found:
+ // camera 0 with guid 0xb09d01009b329c
+ // camera 1 with guid 0xb09d01007e0ee7
+
+ g.setCamera( 0xb09d01009b329c );
+
+ printf("Use camera with GUID: 0x%lx\n", g.getGuid());
+ g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
+
+ vpImageIo::write(I, "image.pgm"); // Write image on the disk
+#endif
+}
+ \endcode
+
+ Here an example of multi camera capture:
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ unsigned int ncameras; // Number of cameras on the bus
+ vp1394TwoGrabber g;
+ ncameras = g.getNumCameras();
+ vpImage<unsigned char> *I = new vpImage<unsigned char> [ncameras];
+
+ // If the first camera supports vpVIDEO_MODE_640x480_YUV422 video mode
+ g.setCamera(0);
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_YUV422);
+
+ if (ncameras >= 2) {
+ // If the second camera support 30 fps acquisition
+ g.setCamera(1);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
+ }
+
+ while(1) {
+ for (unsigned int camera=0; camera < ncameras; camera ++) {
+ g.setCamera(camera);
+ g.acquire(I[camera]);
+ }
+ }
+ delete [] I;
+#endif
+}
+ \endcode
+
+ \sa setFormat(), setVideoMode(), setFramerate(), getNumCameras()
+
+*/
+void
+vp1394TwoGrabber::setCamera(uint64_t cam_id)
+{
+ // Suppose that if camera_id is a camera GUID, this value is greater
+ // than the number of cameras connected to the bus
+ if (cam_id >= num_cameras) {
+ // Check if camera_id is a camera guid
+ bool is_guid = false;
+ // check if the camera_id is a guid
+ for (unsigned int i=0; i< num_cameras; i++) {
+ if (cameras[i]->guid == cam_id) {
+ this->camera_id = i;
+ is_guid = true;
+ break;
+ }
+ }
+ if (is_guid == false) {
+ std::cout << "Error: The camera with guid 0x" << std::hex << cam_id << " is not present" << std::endl;
+ std::cout << num_cameras << " camera(s) connected" << std::endl;
+ for (unsigned int i=0; i< num_cameras; i++) {
+ std::cout << " - camera " << i << " with guid 0x" << std::hex << cameras[i]->guid << std::endl;
+ }
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "The required camera is not present") );
+ }
+ }
+ else {
+ this->camera_id = (unsigned int) cam_id; // The input cam_id is not a uint64_t guid, but the index of the camera
+ }
+
+ // create a pointer to the working camera
+ camera = cameras[this->camera_id];
+}
+
+
+/*!
+
+ Get the active camera identifier on the bus.
+
+ \param cam_id : The active camera identifier. The value is
+ comprised between 0 (the first camera) and the number of cameras
+ found on the bus returned by getNumCameras() minus 1.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera is found.
+
+ \sa setCamera(), getNumCameras()
+
+*/
+void
+vp1394TwoGrabber::getCamera(uint64_t &cam_id)
+{
+ if (num_cameras) {
+ cam_id = this->camera_id;
+ }
+ else {
+ close();
+ vpERROR_TRACE("No cameras found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No cameras found") );
+ }
+}
+
+/*!
+
+ Get the active camera identifier on the bus.
+
+ \return The active camera identifier. The value is
+ comprised between 0 (the first camera) and the number of cameras
+ found on the bus returned by getNumCameras() minus 1.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera is found.
+
+ \sa setCamera(), getNumCameras()
+
+*/
+uint64_t
+vp1394TwoGrabber::getCamera()
+{
+ if (num_cameras) {
+ return this->camera_id;
+ }
+ else {
+ close();
+ vpERROR_TRACE("No cameras found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No cameras found") );
+ }
+}
+
+/*!
+
+ Return the number of cameras connected on the bus.
+
+ \param ncameras : The number of cameras found on the bus.
+
+*/
+void
+vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
+{
+ if (! num_cameras) {
+ vpCTRACE << "No camera found..."<< std::endl;
+ ncameras = 0;
+ }
+
+ ncameras = num_cameras;
+}
+
+/*!
+
+ Return the number of cameras connected on the bus.
+
+ \return The number of cameras found on the bus.
+
+*/
+unsigned int
+vp1394TwoGrabber::getNumCameras() const
+{
+ unsigned int ncameras = 0;
+ if (! num_cameras) {
+ vpCTRACE << "No camera found..."<< std::endl;
+ ncameras = 0;
+ }
+
+ ncameras = num_cameras;
+ return ncameras;
+}
+
+/*!
+
+ Set the camera video capture mode. Image size is than updated with respect to
+ the new video capture mode.
+
+ The iso transmission (setTransmission()) and the dma capture (see
+ setCapture()) are first stopped. Then, the camera video capture mode is
+ set. Finaly, the dma capture and the iso transmission are re-started.
+
+ \param videomode : The camera video capture mode. The current camera mode is
+ given by getVideoMode(). The camera supported modes are given by
+ getVideoModeSupported().
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the video mode.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \sa getVideoMode(), getVideoModeSupported(), setCamera()
+
+*/
+void
+vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
+{
+ open();
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ if (!isVideoModeSupported(videomode)){
+ vpERROR_TRACE("Video mode not supported by camera %d",camera_id);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Video mode not supported") );
+ return ;
+ }
+ // Stop dma capture if started
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+
+ if (dc1394_video_set_mode(camera, (dc1394video_mode_t) videomode) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't set video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't set video mode") );
+ }
+
+ setCapture(DC1394_ON);
+ setTransmission(DC1394_ON);
+
+ // Updates image size from new video mode
+ if (dc1394_get_image_size_from_video_mode(camera,
+ (dc1394video_mode_t) videomode,
+ &this->width, &this->height)
+ != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't set video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get image size") );
+ }
+
+}
+
+/*!
+
+ Query the actual capture video mode of the active camera. All
+ the active camera supported modes are given by getVideoModeSupported().
+
+ \param videomode : The camera capture video mode.
+
+ \exception vpFrameGrabberException::initializationError : If the
+ required camera is not present.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the camera actual video mode.
+
+ \sa setVideoMode(), getVideoModeSupported(), setCamera()
+
+*/
+void
+vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType & videomode)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394video_mode_t _videomode;
+ if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current video mode") );
+ }
+ videomode = (vp1394TwoVideoModeType) _videomode;
+
+}
+
+
+
+/*!
+
+ Query the available active camera video modes.
+
+
+ \param videomodes : The list of supported camera video modes.
+
+ \return The number of supported camera modes, 0 if an error occurs.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ video modes.
+
+ \sa setVideoMode(), getVideoMode(), getCamera()
+*/
+uint32_t
+vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes)
+{
+ // Refresh the list of supported modes
+ videomodes.clear();
+
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ dc1394video_modes_t _videomodes;
+
+ // get video modes:
+ if (dc1394_video_get_supported_modes(camera, &_videomodes)!=DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get video modes");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get video modes") );
+ }
+
+ // parse the video modes to add in the list
+ for (unsigned i=0; i < _videomodes.num; i++) {
+ vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType) _videomodes.modes[i];
+ videomodes.push_back( _mode );
+ }
+
+ // return the number of available video modes
+ return _videomodes.num;
+}
+/*!
+ Check for the active camera video mode.
+
+ \param videomode : The video mode to check for.
+
+ \return true if the active camera supports the desired video mode.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ video modes.
+
+ \sa setVideoMode(), getVideoMode(), getCamera()
+ */
+bool
+vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ dc1394video_modes_t _videomodes;
+
+ // get video modes:
+ if (dc1394_video_get_supported_modes(camera, &_videomodes)!=DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get video modes");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get video modes") );
+ }
+
+ // parse the video modes to check with the desired
+ for (unsigned i=0; i < _videomodes.num; i++) {
+ if ((vp1394TwoVideoModeType) _videomodes.modes[i] == videomode)
+ return true;
+ }
+ return false;
+}
+
+/*!
+
+ Indicates if the video mode is format 7.
+
+ \param videomode : The video mode to check for.
+
+ \return true : If the video mode is scalable (Format 7).
+ \return false : If the video mode is not Format 7 like.
+
+ \sa setVideoMode(), getVideoModeSupported(), setCamera()
+
+*/
+bool
+vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
+{
+
+ if (dc1394_is_video_mode_scalable((dc1394video_mode_t) videomode))
+ return true;
+
+ return false;
+}
+
+/*!
+
+ Indicates if the active camera is grabbing color or grey images.
+
+ We consider color images if the color coding is either YUV (411, 422, 444) or
+ RGB (8, 16, 16S). We consider grey images if the color coding is MONO (8,
+ 16, 16S) or RAW (8, 16). vp1394TwoColorCodingType gives the supported color
+ codings.
+
+ \return true : If color images are acquired.
+ \return false : If grey images are acquired.
+
+ \sa getColorCoding(), setCamera()
+
+*/
+bool
+vp1394TwoGrabber::isColor()
+{
+ vp1394TwoColorCodingType coding;
+ getColorCoding(coding);
+
+ switch (coding) {
+ case vpCOLOR_CODING_MONO8:
+ case vpCOLOR_CODING_MONO16:
+ case vpCOLOR_CODING_MONO16S:
+ case vpCOLOR_CODING_RAW8:
+ case vpCOLOR_CODING_RAW16:
+ return false;
+ case vpCOLOR_CODING_YUV411:
+ case vpCOLOR_CODING_YUV422:
+ case vpCOLOR_CODING_YUV444:
+ case vpCOLOR_CODING_RGB8:
+ case vpCOLOR_CODING_RGB16:
+ case vpCOLOR_CODING_RGB16S:
+ return true;
+ }
+ return false;
+}
+
+/*!
+
+ Set the active camera framerate for non scalable video modes.
+
+ The iso transmission (setTransmission()) and the dma capture (see
+ setCapture()) are first stopped. Then, the camera framerate capture mode is
+ set. Finaly, the dma capture and the iso transmission are re-started.
+
+ If the current video mode is scalable (Format 7), this function is without
+ effect.
+
+ \param fps : The camera framerate. The current framerate of the camera is
+ given by getFramerate(). The camera supported framerates are given by
+ getFramerateSupported().
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the framerate.
+
+ \sa getFramerate(), getFramerateSupported() , setCamera()
+
+*/
+void
+vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
+{
+ open();
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ vp1394TwoVideoModeType cur_videomode;
+ getVideoMode(cur_videomode);
+ if (isVideoModeFormat7(cur_videomode))
+ return;
+
+ if (!isFramerateSupported(cur_videomode,fps)){
+ vpERROR_TRACE("Framerate not supported by camera %d",camera_id);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Framerate not supported") );
+ return ;
+ }
+
+ // Stop dma capture if started
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+
+ if (dc1394_video_set_framerate(camera, (dc1394framerate_t) fps) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't set framerate");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't set framerate") );
+ }
+
+ setCapture(DC1394_ON);
+ setTransmission(DC1394_ON);
+}
+
+/*!
+
+ Query the actual camera framerate of the active camera. The camera supported
+ framerates are given by getFramerateSupported().
+
+ \param fps : The camera capture framerate.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the framerate.
+
+ \sa setFramerate(), getFramerateSupported(), setCamera()
+
+*/
+void
+vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType & fps)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ dc1394framerate_t _fps;
+ if (dc1394_video_get_framerate(camera, &_fps) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current framerate");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current framerate") );
+ }
+ fps = (vp1394TwoFramerateType) _fps;
+
+}
+
+/*!
+
+ Query the available framerates for the given camera video mode (see
+ file dc1394/control.h). No framerate is associated to the following
+ camera modes :
+
+ - vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6),
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 (format 7):
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_1 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_2 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_3 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_4 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_5 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_6 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_7 (format 7)
+
+ \param mode : Camera video mode.
+
+ \param fps : The list of supported camera framerates for the given camera
+ video mode.
+
+ \return The number of supported framerates, 0 if no framerate is available.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the supported framerates.
+
+ \sa setFramerate(), getFramerate(), setCamera()
+*/
+uint32_t
+vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
+ std::list<vp1394TwoFramerateType> & fps)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ // Refresh the list of supported framerates
+ fps.clear();
+
+ switch (mode) {
+ // Framerate not available for:
+ // - vpVIDEO_MODE_EXIF ie Format_6
+ // - vpVIDEO_MODE_FORMAT7... ie the Format_7
+ case vpVIDEO_MODE_EXIF:
+ case vpVIDEO_MODE_FORMAT7_0:
+ case vpVIDEO_MODE_FORMAT7_1:
+ case vpVIDEO_MODE_FORMAT7_2:
+ case vpVIDEO_MODE_FORMAT7_3:
+ case vpVIDEO_MODE_FORMAT7_4:
+ case vpVIDEO_MODE_FORMAT7_5:
+ case vpVIDEO_MODE_FORMAT7_6:
+ case vpVIDEO_MODE_FORMAT7_7:
+ return 0;
+ break;
+ default:
+ {
+ dc1394framerates_t _fps;
+ if (dc1394_video_get_supported_framerates(camera,
+ (dc1394video_mode_t)mode,
+ &_fps) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported frametates for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported framerates") );
+ }
+ if (_fps.num == 0)
+ return 0;
+
+ for (unsigned int i = 0; i < _fps.num; i ++)
+ fps.push_back((vp1394TwoFramerateType)_fps.framerates[i]);
+
+ return _fps.num;
+ }
+ break;
+ }
+}
+/*!
+
+ Check if the desired framerate is supported for the given camera video mode
+ (see file dc1394/control.h). No framerate is associated to the following
+ camera modes :
+
+ - vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6),
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 (format 7):
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_1 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_2 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_3 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_4 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_5 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_6 (format 7)
+ - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_7 (format 7)
+
+ \param mode : Camera video mode.
+
+ \param fps : The desired camera framerates for the given camera
+ video mode.
+
+ \return true if the desired framerate is supported by the active camera.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the supported framerates.
+
+\exception vpFrameGrabberException::settingError : If the framerate is not
+ supported.
+ \sa setFramerate(), getFramerate(), setCamera()
+ */
+bool
+vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode,
+ vp1394TwoFramerateType fps)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ switch (mode) {
+ // Framerate not available for:
+ // - vpVIDEO_MODE_EXIF ie Format_6
+ // - vpVIDEO_MODE_FORMAT7... ie the Format_7
+ case vpVIDEO_MODE_EXIF:
+ case vpVIDEO_MODE_FORMAT7_0:
+ case vpVIDEO_MODE_FORMAT7_1:
+ case vpVIDEO_MODE_FORMAT7_2:
+ case vpVIDEO_MODE_FORMAT7_3:
+ case vpVIDEO_MODE_FORMAT7_4:
+ case vpVIDEO_MODE_FORMAT7_5:
+ case vpVIDEO_MODE_FORMAT7_6:
+ case vpVIDEO_MODE_FORMAT7_7:
+ return 0;
+ break;
+ default:
+ {
+ dc1394framerates_t _fps;
+ if (dc1394_video_get_supported_framerates(camera,
+ (dc1394video_mode_t)mode,
+ &_fps) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported frametates for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported framerates") );
+ }
+ if (_fps.num == 0)
+ return 0;
+
+ for (unsigned int i = 0; i < _fps.num; i ++){
+ if (fps==(vp1394TwoFramerateType)_fps.framerates[i]){
+ return true;
+ }
+ }
+ return false;
+ }
+ break;
+ }
+}
+
+/*!
+
+ Set the active camera Format 7 color coding.
+
+ The iso transmission (setTransmission()) and the dma capture (see
+ setCapture()) are first stopped. Then, the active camera Format 7 is
+ set. Finaly, the dma capture and the iso transmission are re-started.
+
+ \warning Setting color coding for non format 7 video mode will be
+ without effect.
+
+ \param coding : The camera color coding for Format 7 video mode. The
+ current color coding of the camera is given by getColorCoding(). The
+ camera supported color codings are given by
+ getColorCodingSupported().
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the color coding for Format 7 video mode.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \sa getColorCoding(), getColorCodingSupported() , setCamera()
+
+*/
+void
+vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394video_mode_t _videomode;
+ if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current video mode") );
+ }
+
+ if (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode,coding)){
+ vpERROR_TRACE("Color coding not supported by camera %d",camera_id);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Color coding not supported") );
+ return ;
+ }
+
+ // Format 7 video mode
+ if (dc1394_is_video_mode_scalable(_videomode)) {
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+
+ if (dc1394_format7_set_color_coding(camera, _videomode,
+ (dc1394color_coding_t) coding)
+ != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't set color coding");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't set color coding") );
+ }
+
+ setCapture(DC1394_ON);
+ setTransmission(DC1394_ON);
+ }
+}
+
+/*!
+
+ Query the actual color coding of the active camera. The camera supported
+ color codings are given by getColorCodingSupported().
+
+ \param coding : The camera capture color coding.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the actual color coding. Occurs if current video mode is
+ vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6).
+
+ \sa setColorCoding(), getColorCodingSupported(), setCamera()
+
+*/
+void
+vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType & coding)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ dc1394video_mode_t _videomode;
+ if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current video mode") );
+ }
+
+ dc1394color_coding_t _coding;
+ if (dc1394_is_video_mode_scalable(_videomode)) {
+ // Format 7 video mode
+ if (dc1394_format7_get_color_coding(camera, _videomode, &_coding)
+ != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current color coding");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't query current color coding") );
+ }
+ }
+ else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)_videomode)) {
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "No color coding for format 6 video mode"));
+ }
+ else {
+ // Not Format 7 and not Format 6 video modes
+ if (dc1394_get_color_coding_from_video_mode(camera,
+ (dc1394video_mode_t)_videomode,
+ &_coding) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported color coding for mode %d\n",
+ _videomode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't query current color coding"));
+ }
+ }
+ coding = (vp1394TwoColorCodingType) _coding;
+}
+
+/*!
+
+ Query the available color codings for the given camera video mode (see
+ file dc1394/control.h).
+
+ \param mode : Camera video mode.
+
+ \param codings : The list of supported color codings for the given camera
+ video mode.
+
+ \return The number of supported color codings, 0 if no color codings
+ is available.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the color codingss.
+
+ \sa setColorCoding(), getColorCoding(), setCamera()
+*/
+uint32_t
+vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
+ std::list<vp1394TwoColorCodingType> & codings)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ // Refresh the list of supported framerates
+ codings.clear();
+
+ if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
+ // Format 7 video mode
+ dc1394color_codings_t _codings;
+ if (dc1394_format7_get_color_codings(camera,
+ (dc1394video_mode_t)mode,
+ &_codings) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported color codings for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported color codings") );
+ }
+ if (_codings.num == 0)
+ return 0;
+
+ for (unsigned int i = 0; i < _codings.num; i ++)
+ codings.push_back((vp1394TwoColorCodingType)_codings.codings[i]);
+
+ return _codings.num;
+ }
+ else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
+ // Format 6 video mode
+ return 0;
+ }
+ else {
+ // Not Format 7 and not Format 6 video modes
+ dc1394color_coding_t _coding;
+ if (dc1394_get_color_coding_from_video_mode(camera,
+ (dc1394video_mode_t)mode,
+ &_coding) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported color coding for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported color coding") );
+ }
+ codings.push_back((vp1394TwoColorCodingType)_coding);
+ return 1;
+ }
+}
+/*!
+
+ Check if the color coding is supported for the given camera video mode (see
+ file dc1394/control.h).
+
+ \param mode : Camera video mode.
+
+ \param coding : Desired color coding for the given camera
+ video mode.
+
+ \return true if the color coding is supported.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't get
+ the color codingss.
+ \exception vpFrameGrabberException::settingError : If the color coding is
+ not supported.
+ \sa setColorCoding(), getColorCoding(), setCamera()
+ */
+bool
+vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode,
+ vp1394TwoColorCodingType coding)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+
+ if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
+ // Format 7 video mode
+ dc1394color_codings_t _codings;
+ if (dc1394_format7_get_color_codings(camera,
+ (dc1394video_mode_t)mode,
+ &_codings) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported color codings for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported color codings") );
+ }
+ if (_codings.num == 0)
+ return 0;
+
+ for (unsigned int i = 0; i < _codings.num; i ++){
+ if (coding==(vp1394TwoColorCodingType)_codings.codings[i])
+ return true;
+ }
+ return false;
+ }
+ else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
+ // Format 6 video mode
+ return false;
+ }
+ else {
+ // Not Format 7 and not Format 6 video modes
+ dc1394color_coding_t _coding;
+ if (dc1394_get_color_coding_from_video_mode(camera,
+ (dc1394video_mode_t)mode,
+ &_coding) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported color coding for mode %d\n",
+ mode);
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not query supported color coding") );
+ return false;
+ }
+ if (coding==(vp1394TwoColorCodingType)_coding)
+ return true;
+
+ return false;
+ }
+}
+
+
+/*!
+
+ Set the grabbed region of interest ie roi position and size for format 7
+ video mode.
+
+ The iso transmission (setTransmission()) and the dma capture (see
+ setCapture()) are first stopped. Then, the format 7 roi is
+ set. Finaly, the dma capture and the iso transmission are re-started.
+
+ \warning Setting format 7 roi takes only effect if video mode is
+ format 7 like.
+
+ \param left : Position of the upper left roi corner.
+
+ \param top : Position of the upper left roi corner.
+
+ \param w : Roi width. If width is set to 0, uses the maximum
+ allowed image width.
+
+ \param h : Roi height. If width is set to 0, uses the maximum
+ allowed image height.
+
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ roi.
+
+ \sa isVideoModeFormat7()
+*/
+void
+vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top,
+ unsigned int w, unsigned int h)
+{
+ open();
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394video_mode_t _videomode;
+ if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get current video mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current video mode") );
+ }
+ if (dc1394_is_video_mode_scalable(_videomode)) {
+ // Stop dma capture if started
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+ // Format 7 video mode
+ unsigned int max_width, max_height;
+ if (dc1394_format7_get_max_image_size(camera, _videomode,
+ &max_width, &max_height)
+ != DC1394_SUCCESS) {
+
+ close();
+ vpERROR_TRACE("Can't get format7 max image size");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get format7 max image size") );
+ }
+#if 0
+ vpTRACE("left: %d top: %d width: %d height: %d", left, top,
+ width == 0 ? DC1394_USE_MAX_AVAIL: w,
+ height == 0 ? DC1394_USE_MAX_AVAIL : h);
+ vpTRACE("max_width: %d max_height: %d", max_width, max_height);
+#endif
+
+ if (left > max_width) {
+ vpERROR_TRACE("Can't set format7 ROI");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't set format7 ROI") );
+ }
+ if (top > max_height) {
+ vpERROR_TRACE("Can't set format7 ROI");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't set format7 ROI") );
+ }
+
+ int32_t roi_width;
+ int32_t roi_height;
+
+ if (w != 0) {
+ // Check if roi width is acceptable (ie roi is contained in the image)
+ if (w > (max_width - left))
+ w = (max_width - left);
+ roi_width = (int32_t)w;
+ }
+ else {
+ roi_width = DC1394_USE_MAX_AVAIL;
+ }
+
+ if (h != 0) {
+ // Check if roi height is acceptable (ie roi is contained in the image)
+ if (h > (max_height - top))
+ h = (max_height - top);
+ roi_height = (int32_t)h;
+ }
+ else {
+ roi_height = DC1394_USE_MAX_AVAIL;
+ }
+
+ if (dc1394_format7_set_roi(camera, _videomode,
+ (dc1394color_coding_t) DC1394_QUERY_FROM_CAMERA, // color_coding
+ DC1394_USE_MAX_AVAIL/*DC1394_QUERY_FROM_CAMERA*/, // bytes_per_packet
+ (int32_t)left, // left
+ (int32_t)top, // top
+ roi_width,
+ roi_height)
+ != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Can't set format7 roi");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get current video mode") );
+ }
+ // Update the image size
+ if (dc1394_format7_get_image_size(camera, _videomode,
+ &this->width,
+ &this->height)
+ != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Can't get format7 image size");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Can't get format7 image size") );
+ }
+
+ setCapture(DC1394_ON);
+ setTransmission(DC1394_ON);
+ }
+}
+/*!
+
+ Open ohci and asign handle to it and get the camera nodes and
+ describe them as we find them.
+
+ \param reset : If "true", reset the bus attached to the first
+ camera found. Bus reset may help to make firewire working if the
+ program was not properly stopped by a CTRL-C.
+
+ \exception initializationError : If a raw1394 handle can't be aquired,
+ or if no camera is found.
+
+ \sa close()
+ */
+void
+vp1394TwoGrabber::initialize(bool reset)
+{
+ if (init == false){
+ // Find cameras
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ if (d != NULL)
+ dc1394_free (d);
+
+ d = dc1394_new ();
+ if (dc1394_camera_enumerate (d, &list) != DC1394_SUCCESS) {
+ dc1394_camera_free_list (list);
+ close();
+ vpERROR_TRACE("Failed to enumerate cameras\n");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Failed to enumerate cameras") );
+ }
+
+ if (list->num == 0) {
+ dc1394_camera_free_list (list);
+ close();
+ vpERROR_TRACE("No cameras found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No cameras found") );
+ }
+
+ if (cameras != NULL)
+ delete [] cameras;
+
+ cameras = new dc1394camera_t * [list->num];
+
+ num_cameras = 0;
+
+ for (unsigned int i=0; i < list->num; i ++) {
+ cameras[i] = dc1394_camera_new (d, list->ids[i].guid);
+ if (!cameras[i]) {
+ vpTRACE ("Failed to initialize camera with guid \"%ld\"\n",
+ list->ids[i].guid);
+ continue;
+ }
+ // Update the number of working cameras
+ num_cameras ++;
+ }
+
+ if (reset) {
+ // Reset the bus to make firewire working if the program was not properly
+ // stopped by a CTRL-C. We reset here only the bus attached to the first
+ // camera
+ dc1394_reset_bus(cameras[0]);
+ }
+
+ // if (list != NULL)
+ dc1394_camera_free_list (list);
+ list = NULL;
+
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ if (cameras != NULL)
+ free(cameras);
+ cameras = NULL;
+ int err = dc1394_find_cameras(&cameras, &num_cameras);
+
+ if (err!=DC1394_SUCCESS && err != DC1394_NO_CAMERA) {
+ close();
+ vpERROR_TRACE("Unable to look for cameras\n\n"
+ "Please check \n"
+ " - if the kernel modules `ieee1394',`raw1394' and `ohci1394' are loaded \n"
+ " - if you have read/write access to /dev/raw1394\n\n");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Unable to look for cameras") );
+
+ }
+#endif
+
+ if (num_cameras == 0) {
+ close();
+ vpERROR_TRACE("No cameras found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No cameras found") );
+ }
+
+ // allocation for the parameters
+ isDataModified = new bool[num_cameras];
+ for(unsigned int i=0; i<num_cameras; i++)
+ isDataModified[i] = false;
+ initialShutterMode = new dc1394feature_mode_t[num_cameras];
+ dataCam = new vpDc1394TwoCameraParametersData[num_cameras];
+
+ if (camera_id >= num_cameras) {
+ // Bad camera id
+ close();
+ vpERROR_TRACE("Bad camera id: %u", camera_id);
+ vpERROR_TRACE("Only %u camera on the bus.", num_cameras);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Bad camera id") );
+ }
+
+ if (verbose) {
+ std::cout << "------ Bus information ------" << std::endl;
+ std::cout << "Number of camera(s) on the bus : " << num_cameras <<std::endl;
+ std::cout << "-----------------------------" << std::endl;
+ }
+
+ if (camIsOpen != NULL) delete [] camIsOpen;
+ camIsOpen = new bool [num_cameras];
+ for (unsigned int i = 0;i<num_cameras;i++){
+ camIsOpen[i]=false;
+ }
+
+ init = true;
+ }
+}
+/*!
+
+ Start the iso transmission and the dma capture of the current camera.
+
+ \exception initializationError : If a raw1394 handle can't be aquired,
+ or if no camera is found.
+
+ \sa close()
+*/
+void
+vp1394TwoGrabber::open()
+{
+ if (init == false) initialize(false);
+ if (camIsOpen[camera_id] == false){
+ dc1394switch_t status = DC1394_OFF;
+
+ //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ dc1394_video_get_transmission(cameras[camera_id], &status);
+ if (status != DC1394_OFF){
+ //#endif
+ if (dc1394_video_set_transmission(cameras[camera_id],DC1394_OFF)!=DC1394_SUCCESS)
+ vpTRACE("Could not stop ISO transmission");
+ else {
+ vpTime::wait(500);
+ if (dc1394_video_get_transmission(cameras[camera_id], &status)!=DC1394_SUCCESS)
+ vpTRACE("Could get ISO status");
+ else {
+ if (status==DC1394_ON) {
+ vpTRACE("ISO transmission refuses to stop");
+ }
+#ifdef VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ // No yet in the new API
+ cameras[camera_id]->is_iso_on=status;
+#endif
+ }
+ //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ }
+ //#endif
+ }
+ setCamera(camera_id);
+ //setIsoSpeed(DC1394_ISO_SPEED_400);
+ setCapture(DC1394_ON);
+ setTransmission(DC1394_ON);
+ camIsOpen[camera_id] = true;
+ }
+}
+/*!
+
+ Close the firewire grabber.
+
+ Stops the capture and the iso transmission of the active cameras and than
+ releases all the cameras.
+
+*/
+void
+vp1394TwoGrabber::close()
+{
+ if (init){
+ if (num_cameras) {
+ for (unsigned int i = 0; i < num_cameras;i++) {
+ if (camIsOpen[i]) {
+ camera = cameras[i];
+ this->camera_id = i;// set camera id for the function updateDataStructToCam
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+ if(isDataModified[i]){
+ // reset values
+ try{
+ updateDataStructToCam();
+ }
+ catch(...){
+ }
+ // reset mode (manual, auto, ...)
+ if (dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_EXPOSURE, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_SHARPNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_HUE, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_SATURATION, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_GAMMA, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, initialShutterMode[i]) != DC1394_SUCCESS ||
+ dc1394_feature_set_mode(camera, DC1394_FEATURE_IRIS, initialShutterMode[i])){
+
+ vpERROR_TRACE("Unable to reset the initial mode");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to reset the initial mode"));
+ }
+ }
+ if (dc1394_camera_set_power(camera, DC1394_OFF) != DC1394_SUCCESS)
+ std::cout << "Unable to turn camera off" << std::endl;
+ }
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ dc1394_camera_free(cameras[i]);
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ dc1394_free_camera(cameras[i]);
+#endif
+ }
+ }
+ if (camIsOpen != NULL) {
+ delete [] camIsOpen;
+ camIsOpen = NULL;
+ }
+
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ if (cameras != NULL) {
+ delete [] cameras;
+ cameras = NULL;
+ }
+ if (d != NULL) {
+ dc1394_free (d);
+ d = NULL;
+ }
+
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ if (cameras != NULL) {
+ free(cameras);
+ cameras = NULL;
+ }
+#endif
+
+ camIsOpen = NULL;
+ num_cameras = 0;
+
+ // remove data for the parameters
+ if(isDataModified != NULL){
+ delete[] isDataModified;
+ isDataModified = NULL;
+ }
+ if(initialShutterMode != NULL){
+ delete[] initialShutterMode;
+ initialShutterMode = NULL;
+ }
+ if(dataCam != NULL){
+ delete[] dataCam;
+ dataCam = NULL;
+ }
+
+ init = false;
+ }
+}
+
+/*!
+
+ Set the ring buffer size used for the capture.
+ To know the current ring buffer size see getRingBufferSize().
+
+ \param size : Ring buffer size.
+
+ \exception vpFrameGrabberException::settingError : If ring buffer size is not valid.
+
+ \sa getRingBufferSize()
+*/
+void
+vp1394TwoGrabber::setRingBufferSize(unsigned int size)
+{
+ if (size < 1) {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not set ring buffer size") );
+ }
+
+ if (size != num_buffers) {
+ // We need to change the ring buffer size
+ num_buffers = size;
+ if(camIsOpen[camera_id]){
+ setCapture(DC1394_OFF);
+ setCapture(DC1394_ON);
+ }
+ }
+}
+
+/*!
+
+ Get the current ring buffer size used for the capture. To change the
+ ring buffer size see setRingBufferSize().
+
+ \return Current ring buffer size.
+
+ \sa setRingBufferSize()
+*/
+unsigned int
+vp1394TwoGrabber::getRingBufferSize() const
+{
+ return num_buffers;
+}
+
+/*!
+ Enable auto shutter. It is also possible to set auto shutter min
+ and max exposure time, but only for AVT cameras. In that case
+ use setAutoShutter(unsigned int, unsigned int).
+
+ \param enable : Flag to enable or disable the auto shutter. If false, set the
+ shutter as manual.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(); // Enable auto shutter
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the auto shutter.
+
+ \sa setAutoShutter(unsigned int, unsigned int), getAutoShutter()
+*/
+void
+vp1394TwoGrabber::setAutoShutter(bool enable)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394feature_mode_t mode;
+ if (enable) {
+ mode = DC1394_FEATURE_MODE_AUTO;
+ }
+ else {
+ mode = DC1394_FEATURE_MODE_MANUAL;
+ }
+
+ if (dc1394_feature_set_power(camera, DC1394_FEATURE_SHUTTER, DC1394_ON)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set shutter on. \n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set shutter on") );
+ }
+
+ if (dc1394_feature_set_mode(camera,
+ DC1394_FEATURE_SHUTTER,
+ mode)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set auto shutter. \n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set auto shutter") );
+ }
+}
+/*!
+ Set auto shutter. If available set min and max exposure time.
+
+ \warning Setting min and max exposure time feature is only available
+ for AVT cameras.
+
+ \param minvalue : Min shutter exposure time.
+ \param maxvalue : Max shutter exposure time.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the auto shutter.
+
+ \sa setAutoShutter(), getAutoShutter()
+*/
+void
+vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue)
+{
+ setAutoShutter();
+
+ if (dc1394_avt_set_auto_shutter(camera, minvalue, maxvalue)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set auto shutter min and max values. Is the camera an AVT one?\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set auto shutter min and max values") );
+ }
+}
+
+/*!
+ Get auto shutter min and max values.
+
+ \warning Getting min and max auto shutter values is only available
+ for AVT cameras.
+
+ \param minvalue : Min shutter exposure time.
+ \param maxvalue : Max shutter exposure time.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+*/
+void
+vp1394TwoGrabber::getAutoShutter(unsigned int &minvalue, unsigned int &maxvalue)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ if (dc1394_avt_get_auto_shutter(camera, &minvalue, &maxvalue)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot get auto shutter min and max values. Is the camera an AVT one?\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot get auto shutter min and max values") );
+ }
+}
+
+/*!
+ Enable auto gain. It is also possible to set the min
+ and max gain, but only for AVT cameras. In that case
+ use setAutoGain(unsigned int, unsigned int).
+
+ \param enable : Flag to enable or disable the auto gain. If false, set the
+ gain as manual.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoGain(true); // Enable auto gain
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the auto shutter.
+
+ \sa setAutoGain(unsigned int, unsigned int), getAutoGain()
+*/
+void
+vp1394TwoGrabber::setAutoGain(bool enable)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394feature_mode_t mode;
+ if (enable) {
+ mode = DC1394_FEATURE_MODE_AUTO;
+ }
+ else {
+ mode = DC1394_FEATURE_MODE_MANUAL;
+ }
+
+ if (dc1394_feature_set_power(camera, DC1394_FEATURE_GAIN, DC1394_ON)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set shutter on. \n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set shutter on") );
+ }
+
+ if (dc1394_feature_set_mode(camera,
+ DC1394_FEATURE_GAIN,
+ mode)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set auto gain. \n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set auto gain") );
+ }
+}
+/*!
+ Enable auto gain. If available set min and max gain values.
+
+ \warning Setting min and max gain feature is only available
+ for AVT cameras.
+
+ \param minvalue : Min gain.
+ \param maxvalue : Max gain.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoGain(1600*20-1, 1600*20); // Set gain min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the auto shutter.
+
+ \sa setAutoGain(), getAutoGain()
+*/
+void
+vp1394TwoGrabber::setAutoGain(unsigned int minvalue, unsigned int maxvalue)
+{
+ setAutoGain();
+
+ if (dc1394_avt_set_auto_gain(camera, minvalue, maxvalue)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set auto gain min and max values. Is the camera an AVT one?\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot set auto gain min and max values") );
+ }
+}
+
+/*!
+ Get auto gain min and max values.
+
+ \warning Getting min and max auto gain values is only available
+ for AVT cameras.
+
+ \param minvalue : Min gain.
+ \param maxvalue : Max gain.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+*/
+void
+vp1394TwoGrabber::getAutoGain(unsigned int &minvalue, unsigned int &maxvalue)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ if (dc1394_avt_get_auto_gain(camera, &minvalue, &maxvalue)
+ != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot get auto gain min and max values. Is the camera an AVT one?\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Cannot get auto gain min and max values") );
+ }
+}
+
+
+/*!
+
+ Setup camera capture using dma. A ring buffer is used for the
+ capture. It's size can be set using setRingBufferSize().
+
+ \param _switch : Camera capture switch:
+ - DC1394_ON to start dma capture,
+ - DC1394_OFF to stop camera capture.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ dma capture.
+
+ \sa setRingBufferSize(), setVideoMode(), setFramerate()
+*/
+void
+vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ if (_switch == DC1394_ON) {
+ //if (dc1394_capture_setup(camera, num_buffers) != DC1394_SUCCESS) {
+ // To be compatible with libdc1394 svn 382 version
+ if (dc1394_capture_setup(camera, num_buffers,
+ DC1394_CAPTURE_FLAGS_DEFAULT) != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to setup camera capture-\n"
+ "make sure that the video mode and framerate are "
+ "supported by your camera.\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not setup dma capture") );
+ }
+ }
+ else { // _switch == DC1394_OFF
+ dc1394error_t code = dc1394_capture_stop(camera);
+
+ if (code != DC1394_SUCCESS && code != DC1394_CAPTURE_IS_NOT_SET) {
+ vpERROR_TRACE("Unable to stop camera capture\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not setup dma capture") );
+ }
+ }
+}
+
+
+/*!
+
+ Setup camera transmission.
+
+ \param _switch : Transmission switch:
+ - DC1394_ON to start iso transmission,
+ - DC1394_OFF to stop iso transmission.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the video mode.
+*/
+void
+vp1394TwoGrabber::setTransmission(dc1394switch_t _switch)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394switch_t status = DC1394_OFF;
+
+ if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to get transmision status");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not setup dma capture") );
+ }
+
+ // if (status!=_switch){
+ // Start dma capture if halted
+ if (dc1394_video_set_transmission(camera, _switch) != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to setup camera capture-\n"
+ "make sure that the video mode and framerate are "
+ "supported by your camera.\n");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not setup dma capture") );
+ }
+
+ if (_switch == DC1394_ON) {
+ status = DC1394_OFF;
+
+ int i = 0;
+ while ( status == DC1394_OFF && i++ < 5 ) {
+ usleep(50000);
+ if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to get transmision status");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Could not setup dma capture") );
+ }
+ }
+ }
+ // }
+}
+
+/*!
+ Set the camera iso data transmission speed.
+ Speeds over 400Mbps are only available in "B" mode.
+
+ \param isospeed : Iso data transmission speed.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If we can't set
+ the iso speed transmission.
+
+ Below you will find an example that shows how to grab images in
+ Format7 with a transmission speed set to 800Mbps in 1394b mode.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g(false); // Don't reset the bus
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
+ g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
+ g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
+ while(1)
+ g.acquire(I);
+#endif
+}
+ \endcode
+*/
+void
+vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394operation_mode_t op_mode;
+ dc1394speed_t speed;
+
+ // 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") );
+ }
+}
+
+/*!
+ Exist only for compatibility with other grabbing devices.
+
+ Call acquire(vpImage<unsigned char> &I)
+
+ \param I : Image data structure (8 bits image)
+
+ \sa acquire(vpImage<unsigned char> &I)
+
+*/
+void
+vp1394TwoGrabber::open(vpImage<unsigned char> &I)
+{
+ open();
+ acquire(I);
+}
+
+/*!
+ Exist only for compatibility with other grabbing devices.
+
+ Call acquire(vpImage<vpRGBa> &I)
+
+ \param I : Image data structure (RGBa format)
+
+ \sa acquire(vpImage<vpRGBa> &I)
+
+*/
+void
+vp1394TwoGrabber::open(vpImage<vpRGBa> &I)
+{
+ open();
+ acquire(I);
+}
+
+
+/*!
+
+ Get an image from the active camera frame buffer. This buffer neads to be
+ released by enqueue().
+
+ \return Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images. The
+ dequeue() ensure to get the last image, while the enqueue() frees
+ the ring buffer to be sure that the next image is the last one.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vp1394TwoGrabber g;
+ dc1394video_frame_t *frame;
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ while(1) {
+ frame = g.dequeue();
+ // Current image is now in frame structure
+ // Do your stuff
+ g.enqueue(frame);
+ }
+#endif
+}
+ \endcode
+
+ \sa enqueue()
+*/
+dc1394video_frame_t *
+vp1394TwoGrabber::dequeue()
+{
+
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ dc1394video_frame_t *frame = NULL;
+
+ if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame)
+ !=DC1394_SUCCESS) {
+ vpERROR_TRACE ("Error: Failed to capture from camera %d\n", camera_id);
+ }
+
+ return frame;
+}
+
+/*!
+
+ Get an image from the active camera frame buffer. This buffer neads to be
+ released by enqueue().
+
+ \param I : Image data structure (8 bits image).
+
+ \return Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images. The
+ dequeue() ensure to get the last image, while the enqueue() frees
+ the ring buffer to be sure that the next image is the last one.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+ dc1394video_frame_t *frame;
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ while(1) {
+ frame = g.dequeue(I);
+ // Current image is now in frame structure and in I
+ // Do your stuff
+ g.enqueue(frame);
+ }
+#endif
+}
+ \endcode
+
+ \sa enqueue()
+*/
+dc1394video_frame_t *
+vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I)
+{
+ uint64_t timestamp;
+ uint32_t id;
+
+ dc1394video_frame_t *frame;
+
+ frame = dequeue(I, timestamp, id);
+
+ return frame;
+}
+
+/*!
+
+ Get an image from the active camera frame buffer. This buffer neads to be
+ released by enqueue().
+
+ \param I : Image data structure (8 bits image).
+
+ \param timestamp : The unix time in microseconds
+ at which the frame was captured in the ring buffer.
+
+ \param id : The frame position in the ring buffer.
+
+ \return Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab images. The
+ dequeue() ensure to get the last image, while the enqueue() frees
+ the ring buffer to be sure that the next image is the last one.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I;
+ vp1394TwoGrabber g;
+ dc1394video_frame_t *frame;
+ uint64_t timestamp_us; // timestamp in us
+ uint32_t id;
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ while(1) {
+ frame = g.dequeue(I, timestamp_us, id); // get the last image
+ // Current image is now in frame structure and in I
+ // Do your stuff
+ g.enqueue(frame);
+ }
+#endif
+}
+ \endcode
+
+ \sa enqueue()
+*/
+dc1394video_frame_t *
+vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I,
+ uint64_t ×tamp,
+ uint32_t &id)
+{
+
+ open();
+
+ dc1394video_frame_t *frame;
+
+ frame = dequeue();
+
+ // Timeval data structure providing the unix time
+ // [microseconds] at which the frame was captured in the ring buffer.
+ timestamp = frame->timestamp;
+ id = frame->id;
+
+ this->width = frame->size[0];
+ this->height = frame->size[1];
+ unsigned int size = this->width * this->height;
+
+ if ((I.getWidth() != this->width)||(I.getHeight() != this->height))
+ I.resize(this->height, this->width);
+
+ switch(frame->color_coding) {
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ memcpy(I.bitmap, (unsigned char *) frame->image,
+ size*sizeof(unsigned char));
+ break;
+ case DC1394_COLOR_CODING_MONO16:
+ case DC1394_COLOR_CODING_RAW16:
+ vpImageConvert::MONO16ToGrey( (unsigned char *) frame->image,
+ I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToGrey( (unsigned char *) frame->image,
+ I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToGrey( (unsigned char *) frame->image,
+ I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToGrey( (unsigned char *) frame->image,
+ I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToGrey((unsigned char *) frame->image, I.bitmap, size);
+ break;
+
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Format conversion not implemented. "
+ "Acquisition failed.") );
+ break;
+ };
+
+ return frame;
+}
+
+/*!
+
+ Get an image from the active camera frame buffer. This buffer neads to be
+ released by enqueue().
+
+ \param I : Image data structure (32 bits RGBa image).
+
+ \return Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab color images. The
+ dequeue() ensure to get the last image, while the enqueue() frees
+ the ring buffer to be sure that the next image is the last one.
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<vpRGBa> I;
+ vp1394TwoGrabber g;
+ dc1394video_frame_t *frame;
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ while(1) {
+ frame = g.dequeue(I);
+ // Current image is now in frame structure and in I
+ // Do your stuff
+ g.enqueue(frame);
+ }
+#endif
+}
+ \endcode
+
+ \sa enqueue()
+*/
+dc1394video_frame_t *
+vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I)
+{
+ uint64_t timestamp;
+ uint32_t id;
+
+ dc1394video_frame_t *frame;
+
+ frame = dequeue(I, timestamp, id);
+
+ return frame;
+}
+
+/*!
+
+ Get an image from the active camera frame buffer. This buffer neads to be
+ released by enqueue().
+
+ \param I : Image data structure (32 bits RGBa image).
+
+ \param timestamp : The unix time in microseconds
+ at which the frame was captured in the ring buffer.
+
+ \param id : The frame position in the ring buffer.
+
+ \return Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ Below you will find an example that shows how to grab color images. The
+ dequeue() ensure to get the last image, while the enqueue() frees
+ the ring buffer to be sure that the next image is the last one.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<vpRGBa> I;
+ vp1394TwoGrabber g;
+ dc1394video_frame_t *frame;
+ uint64_t timestamp_us; // timestamp in us
+ uint32_t id;
+ g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ while(1) {
+ frame = g.dequeue(I, timestamp_us, id); // get the last image
+ // Current image is now in frame structure and in I
+ // Do your stuff
+ g.enqueue(frame);
+ }
+#endif
+}
+ \endcode
+
+ \sa enqueue()
+*/
+dc1394video_frame_t *
+vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I,
+ uint64_t ×tamp,
+ uint32_t &id)
+{
+
+ open();
+
+ dc1394video_frame_t *frame;
+
+ frame = dequeue();
+
+ // Timeval data structure providing the unix time
+ // [microseconds] at which the frame was captured in the ring buffer.
+ timestamp = frame->timestamp;
+ id = frame->id;
+
+ this->width = frame->size[0];
+ this->height = frame->size[1];
+ unsigned int size = this->width * this->height;
+
+ if ((I.getWidth() != width)||(I.getHeight() != height))
+ I.resize(height, width);
+
+ switch (frame->color_coding) {
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_MONO16:
+ case DC1394_COLOR_CODING_RAW16:
+ vpImageConvert::MONO16ToRGBa((unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Format conversion not implemented. "
+ "Acquisition failed.") );
+ break;
+ };
+
+ return frame;
+}
+
+/*!
+ Release the frame buffer used by the active camera.
+
+ \param frame : Pointer to the libdc1394-2.x image data structure.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \sa dequeue()
+*/
+void
+vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
+{
+
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ if (frame)
+ dc1394_capture_enqueue(camera, frame);
+}
+
+
+/*!
+ Acquire a grey level image from the active camera.
+
+ \param I : Image data structure (8 bits image).
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus or if can't get camera settings.
+
+ \exception vpFrameGrabberException::otherError : If format
+ conversion to return a 8 bits image is not implemented.
+
+ \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
+*/
+void
+vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
+{
+ uint64_t timestamp;
+ uint32_t id;
+
+ dc1394video_frame_t *frame;
+
+ frame = dequeue(I, timestamp, id);
+ enqueue(frame);
+}
+
+/*!
+ Acquire a grey level image from the active camera.
+
+ \param I : Image data structure (8 bits image).
+
+ \param timestamp : The unix time in microseconds
+ at which the frame was captured in the ring buffer.
+
+ \param id : The frame position in the ring buffer.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus or if can't get camera settings.
+
+ \exception vpFrameGrabberException::otherError : If format
+ conversion to return a 8 bits image is not implemented.
+
+ \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
+*/
+void
+vp1394TwoGrabber::acquire(vpImage<unsigned char> &I,
+ uint64_t ×tamp,
+ uint32_t &id)
+{
+ dc1394video_frame_t *frame;
+
+ open();
+ frame = dequeue(I, timestamp, id);
+ enqueue(frame);
+}
+
+
+
+/*!
+ Acquire a color image from the active camera.
+
+ \param I : Image data structure (32 bits RGBa image).
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::otherError : If format
+ conversion to return a RGBa bits image is not implemented.
+
+ \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
+*/
+void
+vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
+{
+ uint64_t timestamp;
+ uint32_t id;
+ dc1394video_frame_t *frame;
+
+ open();
+ frame = dequeue(I, timestamp, id);
+ enqueue(frame);
+}
+
+/*!
+ Acquire a color image from the active camera.
+
+ \param I : Image data structure (32 bits RGBa image).
+
+ \param timestamp : The unix time in microseconds
+ at which the frame was captured in the ring buffer.
+
+ \param id : The frame position in the ring buffer.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::otherError : If format
+ conversion to return a RGBa bits image is not implemented.
+
+ \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
+*/
+void
+vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I,
+ uint64_t ×tamp,
+ uint32_t &id)
+{
+ dc1394video_frame_t *frame;
+
+ open();
+ frame = dequeue();
+ // Timeval data structure providing the unix time
+ // [microseconds] at which the frame was captured in the ring buffer.
+ timestamp = frame->timestamp;
+ id = frame->id;
+
+ this->width = frame->size[0];
+ this->height = frame->size[1];
+ unsigned int size = this->width * this->height;
+
+ if ((I.getWidth() != width)||(I.getHeight() != height))
+ I.resize(height, width);
+
+ switch (frame->color_coding) {
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
+ (unsigned char *) I.bitmap, size);
+ break;
+
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Format conversion not implemented. "
+ "Acquisition failed.") );
+ break;
+ };
+
+ enqueue(frame);
+
+}
+
+/*!
+
+ Get the image width. It depends on the camera video mode setVideoMode(). The
+ image size is only available after a call to open() or acquire().
+
+ \param w : The image width, zero if the required camera is not available.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \warning Has to be called after open() or acquire() to be sure that camera
+ settings are send to the camera.
+
+ \sa getHeight(), open(), acquire()
+
+*/
+void vp1394TwoGrabber::getWidth(unsigned int &w)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ w = this->width;
+}
+
+/*!
+
+ Get the image width. It depends on the camera video mode setVideoMode(). The
+ image size is only available after a call to open() or acquire().
+
+ \return width : The image width, zero if the required camera is not available.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \warning Has to be called after open() or acquire() to be sure that camera
+ settings are send to the camera.
+
+ \sa getHeight(), open(), acquire()
+
+*/
+unsigned int vp1394TwoGrabber::getWidth()
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ return this->width;
+}
+
+/*!
+
+ Get the image height. It depends on the camera vide mode
+ setVideoMode(). The image size is only available after a call to
+ open() or acquire().
+
+ \param h : The image height.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \warning Has to be called after open() or acquire() to be sure that camera
+ settings are send to the camera.
+
+ \sa getWidth()
+
+*/
+void vp1394TwoGrabber::getHeight(unsigned int &h)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ h = this->height;
+}
+/*!
+
+ Get the image height. It depends on the camera vide mode
+ setVideoMode(). The image size is only available after a call to
+ open() or acquire().
+
+ \return The image height.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \warning Has to be called after open() or acquire() to be sure that camera
+ settings are send to the camera.
+
+ \sa getWidth()
+
+*/
+unsigned int vp1394TwoGrabber::getHeight()
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ return this->height;
+}
+
+/*!
+ Display camera information for the active camera.
+
+ \sa setCamera()
+*/
+void
+vp1394TwoGrabber::printCameraInfo()
+{
+ std::cout << "----------------------------------------------------------"
+ << std::endl
+ << "----- Information for camera " << camera_id
+ << " -----" << std::endl
+ << "----------------------------------------------------------" << std::endl;
+
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ dc1394_camera_print_info( camera, stdout);
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ dc1394_print_camera_info( camera);
+#endif
+
+ dc1394featureset_t features;
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ if (dc1394_feature_get_all(camera, &features) != DC1394_SUCCESS)
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ if (dc1394_get_camera_feature_set(camera, &features) != DC1394_SUCCESS)
+#endif
+ {
+ close();
+ vpERROR_TRACE("unable to get feature set for camera %d\n", camera_id);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Cannot get camera features") );
+
+ } else {
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ dc1394_feature_print_all(&features, stdout);
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+ dc1394_print_feature_set(&features);
+#endif
+ }
+ std::cout << "----------------------------------------------------------" << std::endl;
+}
+
+/*!
+
+ Converts the video mode identifier into a string containing the description
+ of the mode.
+
+ \param videomode : The camera capture video mode.
+
+ \return A string describing the mode, an empty string if the mode is not
+ supported.
+
+ \sa string2videoMode()
+*/
+std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode)
+{
+ std::string _str = "";
+ dc1394video_mode_t _videomode = (dc1394video_mode_t) videomode;
+
+ if ((_videomode >= DC1394_VIDEO_MODE_MIN)
+ && (_videomode <= DC1394_VIDEO_MODE_MAX)) {
+ _str = strVideoMode[_videomode - DC1394_VIDEO_MODE_MIN];
+ }
+ else {
+ vpCERROR << "The video mode " << (int)videomode
+ << " is not supported by the camera" << std::endl;
+ }
+
+ return _str;
+}
+
+/*!
+
+ Converts the framerate identifier into a string containing the description
+ of the framerate.
+
+ \param fps : The camera capture framerate.
+
+ \return A string describing the framerate, an empty string if the framerate
+ is not supported.
+
+ \sa string2framerate()
+*/
+std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
+{
+ std::string _str = "";
+ dc1394framerate_t _fps = (dc1394framerate_t) fps;
+
+ if ((_fps >= DC1394_FRAMERATE_MIN)
+ && (_fps <= DC1394_FRAMERATE_MAX)) {
+ _str = strFramerate[_fps - DC1394_FRAMERATE_MIN];
+ }
+ else {
+ vpCERROR << "The framerate " << (int)fps
+ << " is not supported by the camera" << std::endl;
+ }
+
+ return _str;
+}
+
+/*!
+
+ Converts the color coding identifier into a string containing the description
+ of the color coding.
+
+ \param colorcoding : The color coding format.
+
+ \return A string describing the color coding, an empty string if the
+ color coding is not supported.
+
+ \sa string2colorCoding()
+*/
+std::string vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding)
+{
+ std::string _str = "";
+ dc1394color_coding_t _coding = (dc1394color_coding_t) colorcoding;
+
+ if ((_coding >= DC1394_COLOR_CODING_MIN)
+ && (_coding <= DC1394_COLOR_CODING_MAX)) {
+ _str = strColorCoding[_coding - DC1394_COLOR_CODING_MIN];
+
+ }
+ else {
+ vpCERROR << "The color coding " << (int)colorcoding
+ << " is not supported by the camera" << std::endl;
+ }
+
+ return _str;
+}
+
+/*!
+
+ Converts the string containing the description of the vide mode into
+ the video mode identifier.
+
+ \param videomode : The string describing the video mode.
+
+ \return The camera capture video mode identifier.
+
+ \exception vpFrameGrabberException::settingError : If the required videomode
+ is not valid.
+
+ This method returns 0 if the string does not match to a video mode string.
+
+ \sa videoMode2string()
+
+*/
+vp1394TwoGrabber::vp1394TwoVideoModeType
+vp1394TwoGrabber::string2videoMode(std::string videomode)
+{
+ vp1394TwoVideoModeType _id;
+
+ for (int i = DC1394_VIDEO_MODE_MIN; i <= DC1394_VIDEO_MODE_MAX; i ++) {
+ _id = (vp1394TwoVideoModeType) i;
+ if (videomode.compare(videoMode2string(_id)) == 0)
+ return _id;
+ };
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "The required videomode is not valid") );
+
+ return (vp1394TwoVideoModeType) 0;
+}
+
+
+/*!
+
+ Converts the string containing the description of the framerate into the
+ framerate identifier.
+
+ \param framerate : The string describing the framerate.
+
+ \return The camera capture framerate identifier.
+
+ \exception vpFrameGrabberException::settingError : If the required framerate
+ is not valid.
+
+ This method returns 0 if the string does not match to a framerate string.
+
+ \sa framerate2string()
+
+*/
+vp1394TwoGrabber::vp1394TwoFramerateType
+vp1394TwoGrabber::string2framerate(std::string framerate)
+{
+ vp1394TwoFramerateType _id;
+
+ for (int i = DC1394_FRAMERATE_MIN; i <= DC1394_FRAMERATE_MAX; i ++) {
+ _id = (vp1394TwoFramerateType) i;
+ if (framerate.compare(framerate2string(_id)) == 0)
+ return _id;
+ };
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "The required framerate is not valid") );
+
+ return (vp1394TwoFramerateType) 0;
+}
+
+/*!
+
+ Converts the string containing the description of the color coding into the
+ color coding identifier.
+
+ \param colorcoding : The string describing the color coding format.
+
+ \return The camera capture color coding identifier.
+
+ \exception vpFrameGrabberException::settingError : If the required
+ color coding is not valid.
+
+ This method returns 0 if the string does not match to a color coding string.
+
+ \sa colorCoding2string()
+
+*/
+vp1394TwoGrabber::vp1394TwoColorCodingType
+vp1394TwoGrabber::string2colorCoding(std::string colorcoding)
+{
+ vp1394TwoColorCodingType _id;
+
+ for (int i = DC1394_COLOR_CODING_MIN; i <= DC1394_COLOR_CODING_MAX; i ++) {
+ _id = (vp1394TwoColorCodingType) i;
+ if (colorcoding.compare(colorCoding2string(_id)) == 0)
+ return _id;
+ };
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "The required color coding is not valid") );
+
+ return (vp1394TwoColorCodingType) 0;
+}
+
+/*!
+ Resets the IEEE1394 bus which camera is attached to. Calling this function is
+ "rude" to other devices because it causes them to re-enumerate on the bus and
+ may cause a temporary disruption in their current activities. Thus, use it
+ sparingly. Its primary use is if a program shuts down uncleanly and needs to
+ free leftover ISO channels or bandwidth. A bus reset will free those things
+ as a side effect.
+
+ The example below shows how to reset the bus attached to the last
+ camera found.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ unsigned int ncameras; // Number of cameras on the bus
+ vp1394TwoGrabber g;
+ ncameras = g.getNumCameras();
+ g.setCamera(ncameras-1); // To dial with the last camera on the bus
+ g.resetBus(); // Reset the bus attached to "ncameras-1"
+#endif
+}
+ \endcode
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera is found.
+
+*/
+void vp1394TwoGrabber::resetBus()
+{
+ for (unsigned int i = 0; i < num_cameras;i++) {
+ if (camIsOpen[i]) {
+ camera = cameras[i];
+ setTransmission(DC1394_OFF);
+ setCapture(DC1394_OFF);
+ }
+ }
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ setCamera(camera_id);
+ // free the other cameras
+ for (unsigned int i=0;i<num_cameras;i++){
+ if (i!=camera_id) dc1394_camera_free(cameras[i]);
+ }
+
+ printf ("Reseting bus...\n");
+ dc1394_reset_bus (camera);
+
+ dc1394_camera_free (camera);
+ dc1394_free (d);
+ d = NULL;
+ //if (cameras != NULL)
+ delete [] cameras;
+ cameras = NULL ;
+#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
+
+ setCamera(camera_id);
+ // free the other cameras
+ for (unsigned int i=0;i<num_cameras;i++){
+ if (i!=camera_id) dc1394_free_camera(cameras[i]);
+ }
+ free(cameras);
+ cameras = NULL;
+
+ dc1394_reset_bus(camera);
+ dc1394_free_camera(camera);
+
+#endif
+ if (camIsOpen != NULL)
+ delete [] camIsOpen;
+ camIsOpen = NULL ;
+
+ num_cameras = 0;
+
+ init = false;
+ vpTime::wait(1000);
+ initialize(false);
+}
+
+/*!
+
+ This method is useful for controlling single or multiple image
+ transmission from stereo vision cameras.
+
+ The PAN register 0x884 used to control which images are transmitted
+ is set to the corresponding \e panControlValue value.
+
+ \param panControlValue : Value used to set the PAN register 0x884.
+ The Format_7 Mode 0 transmits images from one camera. When set to 0,
+ transmit right image. When set to 1, transmit left (Bumblebee2) or
+ center (Bumblebee XB3) image and when set to 2 or higher transmit
+ left image. The Format_7 Mode 3 allows to transmit two images
+ simultaneously. If your camera has two sensors, you can use the PAN
+ register to control the order of the images when
+ transmitting. Writing a value of 0 to this register transmits images
+ in right-left format. A value of 1 transmits images in left-right
+ format. If your camera has three sensors, such as the Bumblebee XB3,
+ you can adjust the PAN register as follows to specify which two
+ sensors are capturing images. When \e panControlValue is set to 0,
+ transmit right-left images, when set to 1, transmit right-center
+ images.
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : If the register was not set.
+
+ */
+void vp1394TwoGrabber::setPanControl(unsigned int panControlValue)
+{
+ open();
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ uint64_t offset = 0x884;
+ uint32_t value = 0x82000000 + (uint32_t)panControlValue;
+ dc1394error_t err;
+ err = dc1394_set_control_register(camera, offset, value);
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to set PAN register");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to set PAN register") );
+ }
+}
+
+
+/*!
+
+ This method get the value of one of the parameters of the camera.
+
+ \param param : The parameter expressing the value to return
+
+ \return the parameter's value
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError : if the parameter cannot be
+ retrieved.
+
+ \sa setParameterValue()
+*/
+unsigned int vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ uint32_t value;
+ dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS;// = (dc1394feature_t)param;
+ switch(param) {
+ case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; break;
+ case vpFEATURE_EXPOSURE: feature = DC1394_FEATURE_EXPOSURE; break;
+ case vpFEATURE_SHARPNESS: feature = DC1394_FEATURE_SHARPNESS; break;
+// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ case vpFEATURE_HUE: feature = DC1394_FEATURE_HUE; break;
+ case vpFEATURE_SATURATION: feature = DC1394_FEATURE_SATURATION; break;
+ case vpFEATURE_GAMMA: feature = DC1394_FEATURE_GAMMA; break;
+ case vpFEATURE_SHUTTER: feature = DC1394_FEATURE_SHUTTER; break;
+ case vpFEATURE_GAIN: feature = DC1394_FEATURE_GAIN; break;
+ case vpFEATURE_IRIS: feature = DC1394_FEATURE_IRIS; break;
+// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+// vpFEATURE_PAN = DC1394_FEATURE_PAN,
+// vpFEATURE_TILT = DC1394_FEATURE_TILT,
+// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ }
+
+ dc1394error_t err;
+ err = dc1394_feature_get_value(camera, feature, &value);
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to get the information");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to get the information") );
+ }
+ return (unsigned int)value;
+}
+
+
+/*!
+
+ This method set the value of one of the parameters of the camera. The initial
+ value of the parameter is recorded and reset when the destructor is called.
+
+ \warning If the program crashes and the destructor is not called, then the
+ modified parameters will remain in the camera.
+
+ \param param : The parameters to modify
+ \param val : the new value of this parameter
+
+ \exception vpFrameGrabberException::initializationError : If no
+ camera found on the bus.
+
+ \exception vpFrameGrabberException::settingError if a manual mode is not
+ available for the parameter, an exception is thrown.
+
+ \sa getParameterValue()
+
+*/
+void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param,
+ unsigned int val)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+ uint32_t value = (uint32_t)val;
+ dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS;// = (dc1394feature_t)param;
+ switch(param) {
+ case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; break;
+ case vpFEATURE_EXPOSURE: feature = DC1394_FEATURE_EXPOSURE; break;
+ case vpFEATURE_SHARPNESS: feature = DC1394_FEATURE_SHARPNESS; break;
+// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ case vpFEATURE_HUE: feature = DC1394_FEATURE_HUE; break;
+ case vpFEATURE_SATURATION: feature = DC1394_FEATURE_SATURATION; break;
+ case vpFEATURE_GAMMA: feature = DC1394_FEATURE_GAMMA; break;
+ case vpFEATURE_SHUTTER: feature = DC1394_FEATURE_SHUTTER; break;
+ case vpFEATURE_GAIN: feature = DC1394_FEATURE_GAIN; break;
+ case vpFEATURE_IRIS: feature = DC1394_FEATURE_IRIS; break;
+// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+// vpFEATURE_PAN = DC1394_FEATURE_PAN,
+// vpFEATURE_TILT = DC1394_FEATURE_TILT,
+// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ }
+
+ dc1394error_t err;
+ dc1394bool_t hasManualMode = DC1394_FALSE;
+ dc1394feature_modes_t modesAvailable;
+
+ // test wether we can set the shutter value (manual mode available or not)
+ err = dc1394_feature_get_modes(camera, feature, &modesAvailable);
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to detect the manual mode information");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to detect the manual mode information"));
+ }
+
+ for(unsigned int i=0; i<modesAvailable.num; i++){
+ if(modesAvailable.modes[i] == DC1394_FEATURE_MODE_MANUAL){
+ hasManualMode = DC1394_TRUE;
+ }
+ }
+
+ if(hasManualMode == DC1394_TRUE){
+
+ if(!isDataModified[camera_id]){// to ensure we save the first mode even after several set
+ /* we update the structure */
+ updateDataCamToStruct();
+ err = dc1394_feature_get_mode(camera, feature, &(initialShutterMode[camera_id]));
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to get the initial mode");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to get the initial mode"));
+ }
+ isDataModified[camera_id] = true;
+ }
+
+ dc1394feature_mode_t manualMode = DC1394_FEATURE_MODE_MANUAL;
+ err = dc1394_feature_set_mode(camera, feature, manualMode);
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to set the muanual mode");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to set the manual mode") );
+ }
+ err = dc1394_feature_set_value(camera, feature, value);
+ if (err != DC1394_SUCCESS) {
+ vpERROR_TRACE("Unable to set the shutter information");
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Unable to set the shutter information") );
+ }
+ }
+ else{
+ vpERROR_TRACE("The camera does not have a manual mode.\nCannot change the value");
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "The camera does not have a manual mode"));
+ }
+}
+/*!
+
+ Query the actual camera GUID.
+
+ \sa setCamera(), getCamera()
+
+*/
+void
+vp1394TwoGrabber::getGuid(uint64_t & guid)
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ guid = camera->guid;
+}
+
+/*!
+
+ Return the actual camera GUID.
+
+ \sa setCamera(), getCamera()
+
+*/
+uint64_t
+vp1394TwoGrabber::getGuid()
+{
+ if (! num_cameras) {
+ close();
+ vpERROR_TRACE("No camera found");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No camera found") );
+ }
+
+ return camera->guid;
+}
+
+
+/*!
+ update the data structure used to record the value of the current camera.
+
+*/
+inline void
+vp1394TwoGrabber::updateDataCamToStruct()
+{
+ dataCam[camera_id].brightness = getParameterValue(vpFEATURE_BRIGHTNESS);
+ dataCam[camera_id].exposure = getParameterValue(vpFEATURE_EXPOSURE);
+ dataCam[camera_id].sharpness = getParameterValue(vpFEATURE_SHARPNESS);
+ dataCam[camera_id].hue = getParameterValue(vpFEATURE_HUE);
+ dataCam[camera_id].saturation = getParameterValue(vpFEATURE_SATURATION);
+ dataCam[camera_id].gamma = getParameterValue(vpFEATURE_GAMMA);
+ dataCam[camera_id].shutter = getParameterValue(vpFEATURE_SHUTTER);
+ dataCam[camera_id].gain = getParameterValue(vpFEATURE_GAIN);
+ dataCam[camera_id].iris = getParameterValue(vpFEATURE_IRIS);
+}
+
+/*!
+ set the values of several parameters of the current camera with the value
+ previously recorded.
+*/
+inline void
+vp1394TwoGrabber::updateDataStructToCam()
+{
+ setParameterValue(vpFEATURE_BRIGHTNESS, dataCam[camera_id].brightness);
+ setParameterValue(vpFEATURE_EXPOSURE, dataCam[camera_id].exposure);
+ setParameterValue(vpFEATURE_SHARPNESS, dataCam[camera_id].sharpness);
+ setParameterValue(vpFEATURE_HUE, dataCam[camera_id].hue);
+ setParameterValue(vpFEATURE_SATURATION, dataCam[camera_id].saturation);
+ setParameterValue(vpFEATURE_GAMMA, dataCam[camera_id].gamma);
+ setParameterValue(vpFEATURE_SHUTTER, dataCam[camera_id].shutter);
+ setParameterValue(vpFEATURE_GAIN, dataCam[camera_id].gain);
+ setParameterValue(vpFEATURE_IRIS, dataCam[camera_id].iris);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vp1394TwoGrabber.cpp.o) has no symbols
+void dummy_vp1394TwoGrabber() {};
+#endif
+
diff --git a/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp
new file mode 100644
index 0000000..3051298
--- /dev/null
+++ b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Cameras video capture using OpenCV library.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpOpenCVGrabber.cpp
+ \brief class for cameras video capture using OpenCV library.
+*/
+
+#include <visp3/sensor/vpOpenCVGrabber.h>
+
+#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpFrameGrabberException.h>
+
+#include <iostream>
+#include <math.h>
+
+/*!
+ Basic Constructor.
+*/
+vpOpenCVGrabber::vpOpenCVGrabber()
+ : capture(NULL), DeviceType(0), flip(false)
+{
+ // public memebers
+ init = false;
+
+ // protected members
+ width = height = 0;
+}
+
+
+/*!
+ Basic destructor that calls the close() method.
+
+ \sa close()
+*/
+vpOpenCVGrabber::~vpOpenCVGrabber( )
+{
+ close();
+}
+
+
+/*!
+ Generic initialization of the grabber.
+*/
+void vpOpenCVGrabber::open()
+{
+
+ capture = cvCreateCameraCapture(DeviceType);
+
+ if (capture != NULL)
+ {
+ init = true;
+ }
+
+ else
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done : camera already used or no camera found") );
+ }
+}
+
+
+/*!
+ Initialization of the grabber.
+ Call the generic initialization method.
+
+ \param I : Gray level image. This parameter is not used.
+
+ \sa open()
+*/
+void vpOpenCVGrabber::open(vpImage<unsigned char> &/*I*/)
+{
+ open();
+}
+
+
+/*!
+ Initialization of the grabber.
+ Call the generic initialization method.
+
+ \param I : Color image. This parameter is not used.
+
+ \sa open()
+*/
+void vpOpenCVGrabber::open(vpImage<vpRGBa> &/*I*/)
+{
+ open();
+}
+
+
+/*!
+ Grab a gray level image.
+
+ \param I : Acquired gray level image.
+
+ \exception vpFrameGrabberException::initializationError If the
+ initialization of the grabber was not done previously.
+*/
+void vpOpenCVGrabber::acquire(vpImage<unsigned char> &I)
+{
+ IplImage *im;
+
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ cvGrabFrame(capture);
+ im = cvRetrieveFrame(capture);
+ vpImageConvert::convert(im, I, flip);
+}
+
+/*!
+ Grab a color image.
+
+ \param I : Acquired color image.
+
+ \exception vpFrameGrabberException::initializationError If the
+ initialization of the grabber was not done previously.
+*/
+void vpOpenCVGrabber::acquire(vpImage<vpRGBa> &I)
+{
+ IplImage *im;
+
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ cvGrabFrame(capture);
+ im = cvRetrieveFrame(capture);
+ vpImageConvert::convert(im, I, flip);
+}
+
+/*!
+ Grab an image direclty in the OpenCV format.
+
+ \return Pointer to the image (must not be freed).
+
+ \exception vpFrameGrabberException::initializationError If the
+ initialization of the grabber was not done previously.
+*/
+IplImage* vpOpenCVGrabber::acquire()
+{
+ IplImage *im;
+
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ cvGrabFrame(capture);
+ im = cvRetrieveFrame(capture);
+ return im;
+}
+
+/*!
+ Stop the acquisition of images and free the camera.
+*/
+void vpOpenCVGrabber::close()
+{
+ init = false;
+ cvReleaseCapture( &capture );
+ capture = NULL;
+}
+
+
+/*!
+ Gets the capture frame rate.
+
+ \param framerate : The value of the framerate is returned here.
+
+*/
+void vpOpenCVGrabber::getFramerate(double & framerate)
+{
+ framerate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
+}
+
+
+/*!
+ Sets the capture frame rate
+
+ \param framerate : The requested value of the capture framerate.
+
+*/
+void vpOpenCVGrabber::setFramerate(const double framerate)
+{
+ cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, framerate);
+}
+
+
+/*!
+ Sets the captured image width.
+
+ \warning This function must be called after open() method.
+
+ \param w : The requested value of the captured image width.
+
+ \exception vpFrameGrabberException::initializationError If no
+ camera was found.
+*/
+void vpOpenCVGrabber::setWidth(const unsigned int w)
+{
+ if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w))
+ {
+ close();
+ vpERROR_TRACE("Impossible to set the size of the grabber");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Impossible to set the size of the grabber") );
+ }
+
+ this->width = w;
+}
+
+/*!
+ Sets the captured image height.
+
+ \warning This function must be called after open() method.
+
+ \param h : The requested value of the captured image height.
+
+ \exception vpFrameGrabberException::initializationError If no
+ camera was found.
+*/
+void vpOpenCVGrabber::setHeight(const unsigned int h)
+{
+ if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h))
+ {
+ close();
+ vpERROR_TRACE("Impossible to set the size of the grabber");
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Impossible to set the size of the grabber") );
+ }
+
+ this->height = h;
+}
+
+/*!
+ Set the expected type of device.
+
+ \param type : expected type of device
+ - CV_CAP_ANY
+ - CV_CAP_MIL
+ - CV_CAP_VFW
+ - CV_CAP_V4L
+ - CV_CAP_V4L2
+ - CV_CAP_FIREWIRE
+ - CV_CAP_IEEE1394
+ - CV_CAP_DC1394
+ - CV_CAP_CMU_1394
+*/
+void vpOpenCVGrabber::setDeviceType(int type)
+{
+ DeviceType = type;
+
+ if ( DeviceType != 0 && DeviceType != 100 &&DeviceType != 200 && DeviceType != 300)
+ {
+ vpTRACE("The expected type of device may be unknown.");
+ }
+}
+
+
+/*!
+ Set the boolean variable flip to the expected value.
+
+ \param flipType : Expected value of the variable flip. True means that the image is flipped during each image acquisition.
+
+ \warning This function is only useful under Windows.
+
+ \note The aim of this function is to fix a problem which appears under Windows. Indeed with several cameras the aquired images are flipped.
+*/
+void vpOpenCVGrabber::setFlip(bool flipType)
+{
+ flip = flipType;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpOpenCVGrabber.cpp.o) has no symbols
+void dummy_vpOpenCVGrabber() {};
+#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp
new file mode 100644
index 0000000..d1f3ee7
--- /dev/null
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow device description.
+ *
+ * Authors:
+ * Bruno Renier
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <stdio.h>
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <visp3/sensor/vpDirectShowDevice.h>
+
+
+/*!
+ Initialize the vpDirectShowDevice with the moniker's information
+ \param pMoniker The moniker that contains the device's information
+ \return Was the operation successfull
+*/
+bool vpDirectShowDevice::init(const CComPtr<IMoniker>& pMoniker)
+{
+ HRESULT hr;
+
+ //Get the properties
+ CComPtr<IPropertyBag> pPropBag;
+ hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag,
+ (void**)(&pPropBag));
+
+ //get the name of the input
+ VARIANT varName;
+ VARIANT varDesc;
+ VARIANT varDevPath;
+ VariantInit(&varName);
+ VariantInit(&varDesc);
+ VariantInit(&varDevPath);
+ char tmp[FILENAME_MAX];
+
+ hr = pPropBag->Read(L"FriendlyName", &varName, 0);
+
+ //successfully got the name
+ if (SUCCEEDED(hr))
+ {
+ sprintf(tmp, "%S", varName.bstrVal);
+ name = tmp;
+ }
+
+ VariantClear(&varName);
+
+ hr = pPropBag->Read(L"Description", &varDesc, 0);
+
+ //successfully got the description
+ if (SUCCEEDED(hr))
+ {
+ sprintf(tmp, "%S", varDesc.bstrVal);
+ desc = tmp;
+ }
+
+ VariantClear(&varDesc);
+
+ hr = pPropBag->Read(L"DevicePath", &varDevPath, 0);
+
+ //successfully got the device path
+ if (SUCCEEDED(hr))
+ {
+ sprintf(tmp, "%S",varDevPath.bstrVal);
+ devPath = tmp;
+ }
+
+ VariantClear(&varDevPath);
+
+ inUse=false;
+
+ return true;
+}
+
+/*!
+ Compares the two vpDirectShowDevice.
+ \return true if they are equal
+*/
+bool vpDirectShowDevice::operator==(vpDirectShowDevice& dev)
+{
+ return name==dev.name
+ && desc==dev.desc
+ && devPath==dev.devPath;
+}
+
+VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpDirectShowDevice& dev)
+{
+ return os<<dev.name<<std::endl<<dev.desc<<std::endl<<dev.devPath;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowDevice.cpp.o) has no symbols
+void dummy_vpDirectShowDevice() {};
+#endif
+#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp
new file mode 100644
index 0000000..bd194e2
--- /dev/null
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber.
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <visp3/sensor/vpDirectShowGrabber.h>
+#include <visp3/sensor/vpDirectShowGrabberImpl.h>
+
+/*!
+ Returns the number of rows in the grabbed image
+*/
+unsigned int vpDirectShowGrabber::getHeight(){ return grabber->getHeight(); }
+
+/*!
+ Returns the number of colunms in the grabbed image
+*/
+unsigned int vpDirectShowGrabber::getWidth(){ return grabber->getWidth(); }
+
+
+/*!
+ Constructor.
+ Initializes COM.
+*/
+vpDirectShowGrabber::vpDirectShowGrabber()
+{
+ grabber = new vpDirectShowGrabberImpl();
+}
+
+/*!
+ Destructor
+*/
+vpDirectShowGrabber::~vpDirectShowGrabber()
+{
+ delete grabber;
+}
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabber::open()
+{
+ grabber->open();
+}
+
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabber::open(vpImage<unsigned char> &I)
+{
+ grabber->open();
+}
+
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabber::open(vpImage<vpRGBa> &I)
+{
+ grabber->open();
+}
+
+
+/*!
+ Grabs a grayscale image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
+
+ \param I The image to fill with the grabbed frame
+
+ \exception otherError Can't grab the frame
+*/
+void vpDirectShowGrabber::acquire(vpImage<unsigned char> &I)
+{
+ grabber->acquire(I);
+}
+
+/*!
+ Grabs a rgba image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
+
+ \param I The image to fill with the grabbed frame
+
+ \exception otherError Can't grab the frame
+*/
+void vpDirectShowGrabber::acquire(vpImage<vpRGBa> &I)
+{
+ grabber->acquire(I);
+}
+
+
+/*!
+ Stops the framegrabber
+*/
+void vpDirectShowGrabber::close() { grabber->close(); }
+
+/*!
+ Gets the number of capture devices
+ \return the number of capture devices
+*/
+unsigned int vpDirectShowGrabber::getDeviceNumber()
+{
+ return grabber->getDeviceNumber();
+}
+
+/*!
+ Set the device (or camera) from which images will be grabbed.
+ \param id : Identifier of the device to use.
+
+ \return true if the device is set successfully, false otherwise.
+*/
+bool vpDirectShowGrabber::setDevice(unsigned int id)
+{
+ return grabber->setDevice(id);
+}
+
+/*!
+ Displays the list of devices on the standard output
+*/
+void vpDirectShowGrabber::displayDevices()
+{
+ grabber->displayDevices();
+}
+/*!
+ Set image Size
+ \param width : Image width
+ \param height : Image height
+
+ \return true if successful
+*/
+bool vpDirectShowGrabber::setImageSize(unsigned int width,unsigned int height)
+{
+ return grabber->setImageSize(width, height);
+}
+/*!
+ Set capture framerate
+ \param framerate : Framerate in fps
+
+ \return true if successful
+*/
+bool vpDirectShowGrabber::setFramerate(double framerate)
+{
+ return grabber->setFramerate(framerate);
+}
+/*!
+ Set capture format
+ \param width : Image width in pixel
+ \param height : Image height in pixel
+ \param framerate : Framerate in fps
+
+ \return true if successful
+*/
+bool vpDirectShowGrabber::setFormat(unsigned int width,unsigned int height,double framerate)
+{
+ return grabber->setFormat(width, height, framerate);
+}
+/*
+ Get capture format
+ \param width : Pointer to a variable that receives the width in pixel
+ \param height : Pointer to a variable that receives the height in pixel
+ \param framerate : Pointer to a variable that receives the framerate in fps
+*/
+void vpDirectShowGrabber::getFormat(unsigned int &width,unsigned int &height, double &framerate)
+{
+ grabber->getFormat(width, height, framerate);
+}
+
+/*!
+ Get the available capture formats
+
+ \return true if successful
+*/
+bool vpDirectShowGrabber::getStreamCapabilities()
+{
+ return grabber->getStreamCapabilities();
+}
+
+/*!
+ Set capture MediaType
+ \param mediaTypeID : mediaTypeID (available in calling getStreamCapabilities)
+
+ \return true if successful
+*/
+bool vpDirectShowGrabber::setMediaType(int mediaTypeID)
+{
+ return grabber->setMediaType(mediaTypeID);
+}
+
+/*
+ Get current capture MediaType
+
+ \return the current mediaTypeID
+*/
+int vpDirectShowGrabber::getMediaType()
+{
+ return grabber->getMediaType();
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowGrabber.cpp.o) has no symbols
+void dummy_vpDirectShowGrabber() {};
+#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
new file mode 100644
index 0000000..bd8b08c
--- /dev/null
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
@@ -0,0 +1,1238 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber implementation.
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+#include <visp3/sensor/vpDirectShowGrabberImpl.h>
+
+vpDirectShowDevice * vpDirectShowGrabberImpl::deviceList = NULL;
+unsigned int vpDirectShowGrabberImpl::nbDevices ;
+
+/*!
+ Converts a HRESULT into the corresponding error message
+*/
+void vpDirectShowGrabberImpl::HRtoStr(std::string str)
+{
+ TCHAR szErr[MAX_ERROR_TEXT_LEN];
+ DWORD res = AMGetErrorText(hr, szErr, MAX_ERROR_TEXT_LEN);
+
+ if (res == 0) str = "Unknown Error: 0x%2x";
+
+ char msg[MAX_ERROR_TEXT_LEN];
+ sprintf(msg,"%s",szErr);
+ str = msg;
+}
+
+
+/*!
+ Constructor.
+ Initializes COM.
+*/
+vpDirectShowGrabberImpl::vpDirectShowGrabberImpl()
+{
+ init = false ;
+ initCo = false ;
+ //COM initialization
+ if (FAILED(hr = CoInitializeEx(NULL,COINIT_MULTITHREADED)))
+ {
+ std::string err;
+ HRtoStr(err);
+ throw(vpFrameGrabberException(
+ vpFrameGrabberException::initializationError,
+ "Can't initialize COM\n"+ err));
+ }
+ initCo = true ;
+
+ //create the device list
+ if(deviceList == NULL) {
+ CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
+
+ if(enumerate(pVideoInputEnum)) {
+ createDeviceList(pVideoInputEnum);
+ }
+ //not used anymore, so we release it
+ pVideoInputEnum.Release();
+ }
+}
+
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabberImpl::open()
+{
+ //create the device list
+ if(deviceList == NULL) {
+ CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
+
+ if(enumerate(pVideoInputEnum)) {
+ createDeviceList(pVideoInputEnum);
+ }
+ //not used anymore, so we release it
+ pVideoInputEnum.Release();
+ }
+
+ init = initDirectShow();
+ if(! init )
+ {
+ std::string err;
+ HRtoStr(err);
+ throw (vpFrameGrabberException(
+ vpFrameGrabberException::initializationError, err));
+ }
+}
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabberImpl::open(vpImage<unsigned char> &I)
+{
+ open();
+}
+
+/*!
+ Creates the filter graph and select the first available device.
+ \exception initializationError
+*/
+void vpDirectShowGrabberImpl::open(vpImage<vpRGBa> &I)
+{
+ open();
+}
+
+/*!
+ Initialization method
+ Creates the capture filter graph
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::initDirectShow()
+{
+
+
+ //get the first working device's filter (unused and getdevice works on it)
+ currentDevice = getFirstUnusedDevice(pCapSource);
+
+ if(currentDevice == nbDevices)
+ return false;
+
+ //create the filter graph
+ if(!createGraph())
+ return false;
+
+ //we add the capture source to the filter graph
+ if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
+ return false;
+
+ //we create a sample grabber
+ if(!createSampleGrabber(pGrabberFilter))
+ return false;
+
+ //we add the grabber to the filter graph
+ if(FAILED(hr = pGraph->AddFilter(pGrabberFilter, L"SampleGrabber")))
+ return false;
+
+ //we connect the pins
+ if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
+ return false;
+
+
+ //get the current connected media type (needed by the callback)
+ if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+
+ //Gets the various graph's interfaces
+ CComPtr<IMediaFilter> pMediaFilter;
+
+ pGraph->QueryInterface(IID_IMediaFilter, (void **)&pMediaFilter);
+ pGraph->QueryInterface(IID_IMediaControl, reinterpret_cast<void**>(&pControl));
+ pGraph->QueryInterface(IID_IMediaEvent, (void **)&pEvent);
+
+ pMediaFilter->SetSyncSource(NULL);
+ pMediaFilter.Release();
+
+ return true;
+}
+
+/*!
+ Destructor
+*/
+vpDirectShowGrabberImpl::~vpDirectShowGrabberImpl()
+{
+ close();
+}
+
+/*!
+ Create a video device enumerator.
+ \param ppVideoInputEnum The video device enumerator
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum)
+{
+ CComPtr<ICreateDevEnum> pDevEnum = NULL;
+ bool res = false;
+
+ //Enumerate system devices
+ hr = pDevEnum.CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER);
+
+ //if it is a success
+ if (SUCCEEDED(hr))
+ {
+ //Create a video input device enumerator
+ hr = pDevEnum->CreateClassEnumerator(
+ CLSID_VideoInputDeviceCategory,
+ &ppVideoInputEnum, 0);
+
+ if(hr == S_OK) res = true;
+ }
+
+
+ pDevEnum.Release();
+ return res;
+}
+
+
+/*!
+ Create the device list by enumerating the video input devices
+ \param ppVideoInputEnum A video device enumerator
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum)
+{
+ CComPtr<IMoniker> pMoniker[10]; //const max devices
+ unsigned long nbMoniker;
+
+ ppVideoInputEnum->Reset();
+
+ //Enumerates the different inputs
+ ppVideoInputEnum->Next(10, reinterpret_cast<IMoniker **>(&pMoniker), &nbMoniker);
+
+ //if no input device
+ if(nbMoniker == 0) return false;
+
+ deviceList = new vpDirectShowDevice[nbMoniker];
+
+ nbDevices = (unsigned int)nbMoniker;
+
+ //we try to get the properties of each moniker, if it fails, we skip to the next one and
+ //decrement the number of valid devices
+ unsigned int i=0;
+ unsigned int j=0;
+ while(i<nbDevices)
+ {
+ if(!deviceList[i].init(pMoniker[j]))
+ {
+ //if we can't get the device properties, skip to the next device
+ j++;
+ nbDevices--;
+ }
+ else
+ {
+ i++;
+ j++;
+ }
+ }
+
+ //if no working input device
+ if(nbDevices == 0) return false;
+
+ //we release the monikers
+ for(unsigned int i=0 ; i<nbMoniker ;i++)
+ {
+ pMoniker[i].Release();
+ }
+
+ return true;
+}
+
+
+/*!
+ Gets the filter associated with device n if it exists.
+ \param n Number of the device
+ \param ppDevice The n-th device
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::getDevice(unsigned int n, CComPtr<IBaseFilter>& ppDevice)
+{
+ //if n is invalid, quit
+ if(n>=nbDevices)
+ return false;
+
+ //if the device is already in use, quit
+ if(deviceList[n].getState() == true)
+ return false;
+
+ //if we can't enumerate the devices, quit
+ CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
+ if(!enumerate(pVideoInputEnum))
+ return false;
+
+ CComPtr<IMoniker> pMoniker = NULL;
+ bool deviceFound = false;
+
+ //Enumerates the different inputs
+ while (pVideoInputEnum->Next(1, &pMoniker, NULL) == S_OK
+ && !deviceFound)
+ {
+ //implicit conversion should work ...
+ if(deviceList[n] == vpDirectShowDevice(pMoniker))
+ {
+ //we get the filter
+ if(SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, (void**)&ppDevice)))
+ {
+ //now the device is in use
+ deviceList[n].setInUse();
+ deviceFound = true;
+ }
+ else{ break; } //we can't get the device's filter, quit
+ }
+ pMoniker.Release();
+ }
+
+ pVideoInputEnum.Release();
+
+ return deviceFound;
+}
+
+/*!
+ Searches for the first unused device.
+ \param ppDevice The first device filter's interface
+ \return number of the found device. If the operation wasn't successfull, n=nbDevices.
+*/
+unsigned int vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice)
+{
+ unsigned int n=0;
+ bool found=false;
+
+ for(n=0;n<nbDevices && !found; n++)
+ {
+ //if the device is not being used
+ if(!deviceList[n].getState())
+ {
+ if(getDevice(n,ppDevice))
+ {
+ found = true;
+ deviceList[n].setInUse();
+ return n;
+ }
+ }
+ }
+
+ return n;
+}
+
+/*!
+ Create the capture graph
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::createGraph()
+{
+
+ // Create the Capture Graph Builder.
+ hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, CLSCTX_INPROC_SERVER);
+
+ if (SUCCEEDED(hr))
+ {
+ // Create the Filter Graph Manager.
+ hr = pGraph.CoCreateInstance(CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER);
+
+ if (SUCCEEDED(hr))
+ {
+ // Initialize the Capture Graph Builder.
+ pBuild->SetFiltergraph(pGraph);
+
+ return true;
+ }
+ }
+
+ return false;
+}
+
+/*!
+ Creates the grabber.
+ \param ppGrabberFilter The created grabber filter's interface
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter)
+{
+ //Creates the sample grabber
+ hr = ppGrabberFilter.CoCreateInstance(CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER);
+
+ if (FAILED(hr))
+ return false;
+
+ //gets the SampleGrabber interface in order to configure it later
+ hr = ppGrabberFilter->QueryInterface(IID_ISampleGrabber,
+ reinterpret_cast<void**>(&pGrabberI));
+
+ if (FAILED(hr))
+ return false;
+
+ //configure the grabber
+ AM_MEDIA_TYPE mt;
+ ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
+
+ mt.majortype = MEDIATYPE_Video;
+
+ //ask for a connection
+ mt.subtype = MEDIATYPE_NULL;
+
+ if(FAILED(hr = pGrabberI->SetMediaType(&mt)))
+ return false;
+
+ //configure the callback of the grabber
+ pGrabberI->SetCallback(&sgCB,1);
+
+ //grab only one frame at a time
+ pGrabberI->SetOneShot(TRUE);
+
+ //no need to bufferize the sample in the grabber
+ pGrabberI->SetBufferSamples(false);
+
+ return true;
+}
+
+/*!
+ Checks the capture filter's ouput pin media type.
+ Some formats' orientation is automatically handled by directshow but this isn't the
+ case for all the existing formats.
+ If the grabbed image is inverted, the source format's orientation may not be
+ known by directshow so consider adding an additional handler for this FourCC format.
+
+ \return True if the connection media type was a video media type.
+*/
+bool vpDirectShowGrabberImpl::checkSourceType(CComPtr<IPin>& pCapSourcePin)
+{
+ //retrieves the connected media type
+ AM_MEDIA_TYPE mt;
+ if(FAILED (pCapSourcePin->ConnectionMediaType(&mt)))
+ return false;
+
+ if(mt.majortype != MEDIATYPE_Video)
+ return false;
+
+ //Known RGB formats
+ if(mt.subtype == MEDIASUBTYPE_ARGB32 ||
+ mt.subtype == MEDIASUBTYPE_RGB32 ||
+ mt.subtype == MEDIASUBTYPE_RGB24 ||
+ mt.subtype == MEDIASUBTYPE_RGB555 ||
+ mt.subtype == MEDIASUBTYPE_RGB565 ||
+ mt.subtype == MEDIASUBTYPE_RGB8 ||
+ mt.subtype == MEDIASUBTYPE_RGB4 ||
+ mt.subtype == MEDIASUBTYPE_RGB1 )
+ {
+ //image orientation will be handled "automatically"
+ sgCB.specialMediaType = false;
+ }
+ //Known YUV formats
+ else if(mt.subtype == MEDIASUBTYPE_AYUV ||
+ mt.subtype == MEDIASUBTYPE_UYVY ||
+ mt.subtype == MEDIASUBTYPE_Y411 ||
+ mt.subtype == MEDIASUBTYPE_Y41P ||
+ mt.subtype == MEDIASUBTYPE_Y211 ||
+ mt.subtype == MEDIASUBTYPE_YUY2 ||
+ mt.subtype == MEDIASUBTYPE_YVYU ||
+ mt.subtype == MEDIASUBTYPE_YUYV ||
+ mt.subtype == MEDIASUBTYPE_IF09 ||
+ mt.subtype == MEDIASUBTYPE_IYUV ||
+ mt.subtype == MEDIASUBTYPE_YV12 ||
+ mt.subtype == MEDIASUBTYPE_YVU9 )
+ {
+ //image orientation will be handled "automatically"
+ sgCB.specialMediaType = false;
+ }
+ //FOURCC formats
+ else
+ {
+ //invertedSource boolean will decide the bitmap orientation
+ sgCB.specialMediaType = true;
+
+ DWORD format;
+ VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(mt.pbFormat);
+ BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
+
+ //get the fourcc code
+ format = ((bmpInfo.biCompression&0xFF000000)>>24) |
+ ((bmpInfo.biCompression&0x00FF0000)>>8) |
+ ((bmpInfo.biCompression&0x0000FF00)<<8) |
+ (bmpInfo.biCompression&0x000000FF)<<24;
+
+ std::cout<<"This format is not one of the standard YUV or RGB format supported by DirectShow.\n"
+ <<"FourCC : "
+ <<(char)(bmpInfo.biCompression&0x000000FF)
+ <<(char)((bmpInfo.biCompression&0x0000FF00)>>8)
+ <<(char)((bmpInfo.biCompression&0x00FF0000)>>16)
+ <<(char)((bmpInfo.biCompression&0xFF000000)>>24)<<std::endl;
+
+ //Y800 is top-down oriented so the image doesn't have to be flipped vertically
+ if(format == 'Y800')
+ {
+ sgCB.invertedSource = false;
+ }
+ //cyuv seems to be the only yuv bottom-up oriented format (image has to be flipped)
+ else if(format == 'cyuv')
+ {
+ sgCB.invertedSource = true;
+ }
+ //insert code for other fourcc formats here
+ //see fourcc.org to know which format is bottom-up oriented and thus needs invertedSource sets to true
+ else
+ {
+ std::cout<<"Unknown FourCC compression type, assuming top-down orientation. Image may be inverted."<<std::endl;
+ sgCB.invertedSource = false; //consider that the image is topdown oriented by default
+ }
+ }
+
+ return true;
+}
+
+/*!
+ Connects the capture device's output pin to the grabber's input pin
+ \param pCapSource The capture device
+ \param pGrabber The grabber
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr<IBaseFilter>& _pCapSource, CComPtr<IBaseFilter>& _pGrabberFilter)
+{
+ /*
+ //get the capture source's output pin
+ CComPtr<IPin> pCapSourcePin;
+ if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
+ return false;
+
+ //get the grabber's input pin
+ CComPtr<IPin> pGrabberInputPin;
+ if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_INPUT, NULL, NULL, false, 0, &pGrabberInputPin)))
+ return false;
+
+ //connect the two of them
+ if(FAILED(pGraph->Connect(pCapSourcePin, pGrabberInputPin)))
+ return false;
+
+ //not used anymore, we can release it
+ pGrabberInputPin.Release();
+ */
+ if(FAILED(hr = pBuild->RenderStream(NULL,NULL, _pCapSource, NULL, _pGrabberFilter)))
+ return false;
+
+ /*
+ //get the grabber's output pin
+ CComPtr<IPin> pGrabberOutputPin;
+ if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_OUTPUT, NULL, NULL, false, 0, &pGrabberOutputPin)))
+ return false;
+ */
+ //get the Null renderer
+ CComPtr<IBaseFilter> pNull = NULL;
+ if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER)))
+ return false;
+/*
+ //get the null renderer's input pin
+ CComPtr<IPin> 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)))
+ return false;
+
+ //get the capture source's output pin
+ CComPtr<IPin> pCapSourcePin;
+ if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
+ return false;
+ //checks the media type of the capture filter
+ //and if the image needs to be inverted
+ if(!checkSourceType(pCapSourcePin))
+ return false;
+
+ //release the remaining interfaces
+ pCapSourcePin.Release();
+ pNull.Release();
+// pGrabberOutputPin.Release();
+// pNullInputPin.Release();
+
+ return true;
+}
+
+
+/*!
+ Removes all the filters from the filter graph
+
+ \return Was the operation successful
+*/
+bool vpDirectShowGrabberImpl::removeAll()
+{
+ CComPtr<IEnumFilters> pEnum = NULL;
+ CComPtr<IBaseFilter> pFilter;
+ ULONG cFetched;
+
+ if (FAILED(hr = pGraph->EnumFilters(&pEnum)))
+ return false;
+
+ while(pEnum->Next(1, &pFilter, &cFetched) == S_OK)
+ {
+ if(FAILED(hr = pGraph->RemoveFilter(pFilter))) return false;
+ pFilter.Release();
+ pEnum->Reset();
+ }
+
+ pEnum.Release();
+ return true;
+}
+
+
+/*!
+ Grabs a rgba image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
+
+ \param I The image to fill with the grabbed frame
+
+ \exception otherError Can't grab the frame
+*/
+void vpDirectShowGrabberImpl::acquire(vpImage<vpRGBa> &I)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ //set the rgbaIm pointer on I (will be filled on the next framegrabber callback)
+ sgCB.rgbaIm = &I;
+ //there is an acquire demand (execute copy during callback)
+ sgCB.acqRGBaDemand = true;
+
+ //Run the graph to grab a frame
+ pControl->Run();
+
+ // Wait untill it's done
+ long ev;
+ hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+ //wait for the end of the next callback (copy)
+ if( WaitForSingleObject(sgCB.copySem,MAX_DELAY) != WAIT_OBJECT_0)
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't grab the frame, callback timeout") );
+
+
+}
+
+/*!
+ Grabs a grayscale image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
+
+ \param I The image to fill with the grabbed frame
+
+ \exception otherError Can't grab the frame
+*/
+void vpDirectShowGrabberImpl::acquire(vpImage<unsigned char> &I)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ //set the grayIm pointer on I (will be filled on the next framegrabber callback)
+ sgCB.grayIm = &I;
+ //there is an acquire demand (execute copy during callback)
+ sgCB.acqGrayDemand = true;
+
+ //Run the graph to grab a frame
+ pControl->Run();
+
+ // Wait untill it's done
+ long ev;
+ hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
+
+ width = I.getWidth();
+ height = I.getHeight();
+
+ //wait for the end of the next callback (copy)
+ if( WaitForSingleObject(sgCB.copySem,MAX_DELAY) != WAIT_OBJECT_0)
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't grab the frame, callback timeout") );
+}
+
+/*!
+ Set the device (or camera) used by the grabber
+ \param id : Identifier of the device to use.
+
+ \return true id the device is set successfully, false otherwise.
+*/
+bool vpDirectShowGrabberImpl::setDevice(unsigned int id)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+
+ //if n is invalid, or the device is already in use, quit
+ if(id>=nbDevices || deviceList[id].getState()==true)
+ return false;
+
+ //we stop the graph
+ pControl->Stop();
+
+ //then we can safely remove all the filters
+ if(!removeAll()) return false;
+
+
+ //we release the previous source's interface
+ pCapSource.Release();
+
+ //here reset inUse in the old DSDevice
+ deviceList[currentDevice].resetInUse();
+
+ //we add the grabber back in the graph
+ pGraph->AddFilter(pGrabberFilter,L"SampleGrabber");
+
+ //get the n-th device's filter
+ if(!getDevice(id, pCapSource))
+ return false;
+
+ //we add the capture source to the filter graph
+ if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
+ return false;
+
+ //we connect the pins
+ if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
+ return false;
+
+
+ //get the current connected media type (needed by the callback)
+ if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ {
+ return false;
+ }
+
+ //the device is now in use
+ deviceList[id].setInUse();
+ currentDevice=id;
+
+ return true;
+}
+
+/*!
+ Displays the list of devices on the standard output
+*/
+void vpDirectShowGrabberImpl::displayDevices()
+{
+ if(deviceList == NULL)
+ {
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ for(unsigned int i=0 ; i<nbDevices ; i++)
+ std::cout<<i<<" : "<< deviceList[i].getName() <<std::endl;
+
+ std::cout<<"Current device : "<<currentDevice<<std::endl<<std::endl;
+}
+
+
+/*!
+ Stops the framegrabber
+*/
+
+void vpDirectShowGrabberImpl::close()
+{
+ //the current device isn't being used anymore
+ if (init) {
+ deviceList[currentDevice].resetInUse();
+ init = false ;
+ }
+ if (initCo) {
+ //uninstalls COM
+ CoUninitialize();
+ initCo = false;
+ }
+}
+/*!
+ Set image size
+*/
+bool vpDirectShowGrabberImpl::setImageSize(unsigned int width,unsigned int height)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ return setFormat(width, height,NULL);
+
+}
+
+/*!
+ Set capture framerate
+*/
+bool vpDirectShowGrabberImpl::setFramerate(double framerate)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
+ return setFormat(pVih->bmiHeader.biWidth,pVih->bmiHeader.biHeight,framerate);
+
+}
+/*!
+ Set the capture format
+*/
+bool vpDirectShowGrabberImpl::setFormat(unsigned int width,unsigned int height, double framerate)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ bool found=false;
+
+ //gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if(FAILED(hr = pBuild->FindInterface(
+ &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void**)&pConfig)))
+ return false;
+
+ //gets the video control interface
+ IAMVideoControl *pVideoControl = NULL;
+
+ if(FAILED(hr = pBuild->FindInterface(
+ &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMVideoControl, (void**)&pVideoControl)))
+ return false;
+
+ //get the grabber's input pin
+ CComPtr<IPin> pCapSourcePin;
+ if(FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
+ return false;
+
+ int iCount = 0, iSize = 0;
+ if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return false;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
+ {
+ // Use the video capabilities structure.
+
+ for (int iFormat = 0; iFormat < iCount; iFormat++)
+ {
+ 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 : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<" at "<<10000000 /pVih->AvgTimePerFrame<<std::endl;
+// std::cout<<"compression : "<<pVih->bmiHeader.biCompression<<std::endl;
+ if (SUCCEEDED(hr)&& found==false)
+ {
+ /* Examine the format, and possibly use it. */
+ if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
+ (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
+ (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
+ (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
+ (pmtConfig->pbFormat != NULL))
+ {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
+
+ LONG lWidth = pVih->bmiHeader.biWidth;
+ LONG lHeight = pVih->bmiHeader.biHeight;
+ if(framerate != NULL)
+ {
+ if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
+ {
+
+ pVih->AvgTimePerFrame = (LONGLONG)(10000000 / framerate);
+ //set the capture media type and the grabber media type
+ if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
+ FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
+ return false;
+ //Run the graph to grab a frame
+ pControl->Run();
+
+ //get the current connected media type (needed by the callback)
+ if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
+ LONGLONG ActualFrameDuration;
+ if(FAILED(hr = pVideoControl->GetCurrentActualFrameRate(pCapSourcePin,&ActualFrameDuration)))
+ std::cout<<"Current format (not sure): "<<width <<" x "<< height <<" at "<< 10000000/pVih->AvgTimePerFrame <<" fps"<<std::endl<<std::endl;
+ else {
+ std::cout<<"Current format : "<<width <<" x "<< height <<" at "<< 10000000/ActualFrameDuration <<" fps"<<std::endl<<std::endl;
+ pVih->AvgTimePerFrame = ActualFrameDuration;
+ }
+ found=true;
+ }
+ }
+ else
+ {
+ if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
+ {
+ pVih->AvgTimePerFrame = scc.MinFrameInterval;
+ //set the capture media type and the grabber media type
+ if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
+ FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
+ return false;
+ //get the current connected media type (needed by the callback)
+ if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
+ found=true;
+ std::cout<<"Current format : "<<width <<" x "<< height <<" at "<<(10000000 /pVih->AvgTimePerFrame) <<" fps"<<std::endl<<std::endl;
+ }
+
+ }
+ }
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ if(!found)
+ if(framerate != NULL)
+ std::cout << "The "<<width <<" x "<< height <<" at " <<framerate<<" fps source image format is not available. "<<std::endl<<std::endl;
+ else
+ std::cout << "The "<<width <<" x "<< height <<"source image size is not available. "<<std::endl<<std::endl;
+
+ return found;
+}
+/*!
+ Get the current capture format and framerate.
+ \param width : Image width.
+ \param height : Image height.
+ \param framerate : Framerate acquisition.
+*/
+void vpDirectShowGrabberImpl::getFormat(unsigned int &width,unsigned int &height, double &framerate)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
+ width = (unsigned int)pVih->bmiHeader.biWidth;
+ height = (unsigned int)pVih->bmiHeader.biHeight;
+ framerate = (double)(10000000/pVih->AvgTimePerFrame);
+
+}
+/*!
+ Get the available capture formats
+*/
+bool vpDirectShowGrabberImpl::getStreamCapabilities()
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ //gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if(FAILED(hr = pBuild->FindInterface(
+ &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void**)&pConfig)))
+ return false;
+
+ int iCount = 0, iSize = 0;
+ if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return false;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
+ {
+ std::cout<<"Available MediaTypes : "<<std::endl<<std::endl;
+ // Use the video capabilities structure.
+ for (int iFormat = 0; iFormat < iCount; iFormat++)
+ {
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
+
+ if (SUCCEEDED(hr))
+ {
+ /* 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)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_ARGB32"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB32)
+ std::cout<<"subtype : MEDIASUBTYPE_RGB32"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB24)
+ std::cout<<"subtype : MEDIASUBTYPE_RGB24"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB555)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB555"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB565)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB565"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB8)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB8"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB4)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB4"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_RGB1)
+ std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB1"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_YV12)
+ std::cout<<"subtype : MEDIASUBTYPE_YV12"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_YVU9)
+ std::cout<<"subtype : MEDIASUBTYPE_YVU9"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_YUY2)
+ std::cout<<"subtype : MEDIASUBTYPE_YUY2"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_YUYV)
+ std::cout<<"subtype : MEDIASUBTYPE_YUYV"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_YVYU)
+ std::cout<<"subtype : MEDIASUBTYPE_YVYU"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_IYUV)
+ std::cout<<"subtype : MEDIASUBTYPE_IYUV"<<std::endl;
+ else if(pmtConfig->subtype==MEDIASUBTYPE_UYVY)
+ std::cout<<"subtype : MEDIASUBTYPE_UYVY"<<std::endl;
+ else if((((pVih->bmiHeader.biCompression&0xFF000000)>>24) |
+ ((pVih->bmiHeader.biCompression&0x00FF0000)>>8) |
+ ((pVih->bmiHeader.biCompression&0x0000FF00)<<8) |
+ ((pVih->bmiHeader.biCompression&0x000000FF)<<24)) == 'I420')
+ std::cout<<"subtype : I420"<<std::endl;
+ else std::cout<<"subtype (not supported) :"
+ <<(char)(pVih->bmiHeader.biCompression&0x000000FF)
+ <<(char)((pVih->bmiHeader.biCompression&0x0000FF00)>>8)
+ <<(char)((pVih->bmiHeader.biCompression&0x00FF0000)>>16)
+ <<(char)((pVih->bmiHeader.biCompression&0xFF000000)>>24)<<std::endl;
+
+ std::cout<<"image size : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<std::endl;
+ std::cout<<"framerate range: ["<< 10000000/scc.MaxFrameInterval<<","<<10000000/scc.MinFrameInterval<<"]"<<std::endl<<std::endl;
+
+/*
+ long frameRateNum;
+ LONGLONG *frameRateList;
+ if(FAILED(hr = pVideoControl->GetFrameRateList(pCapSourcePin,iFormat,dimensions, //inputs
+ &frameRateNum, &frameRateList))) //outputs
+ return false;
+ for(int i=0; i<(int)frameRateNum ; i++)
+ {
+ std::cout<<(float)(10000000/frameRateList[i])<<" fps"<<std::endl;
+ }
+ std::cout<<std::endl;
+*/
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ return true;
+}
+/*!
+ Set capture Mediatype
+*/
+bool vpDirectShowGrabberImpl::setMediaType(int mediaTypeID)
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ //gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if(FAILED(hr = pBuild->FindInterface(
+ &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void**)&pConfig)))
+ return false;
+
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(mediaTypeID, &pmtConfig, (BYTE*)&scc);
+
+
+ if (SUCCEEDED(hr))
+ {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
+ pVih->AvgTimePerFrame = scc.MinFrameInterval ;
+ //set the capture media type and the grabber media type
+ if(FAILED(hr = pGrabberI->SetMediaType(pmtConfig))||
+ FAILED(hr = pConfig->SetFormat(pmtConfig)))
+ return false;
+ //get the current connected media type (needed by the callback)
+ if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ return true;
+}
+
+/*
+ Get current capture MediaType
+ \return mediaTypeID (-1 if failed)
+*/
+int vpDirectShowGrabberImpl::getMediaType()
+{
+ if (init==false)
+ {
+ close();
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done") );
+ }
+
+ int mediaTypeID = -1;
+ VIDEOINFOHEADER *pVihConnected = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
+
+ //gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if(FAILED(hr = pBuild->FindInterface(
+ &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void**)&pConfig)))
+ return -1;
+
+ int iCount = 0, iSize = 0;
+ if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return -1;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
+ {
+ // Use the video capabilities structure.
+ for (int iFormat = 0; iFormat < iCount; iFormat++)
+ {
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
+
+ if (SUCCEEDED(hr))
+ {
+ /* Examine the format, and possibly use it. */
+ if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
+ (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
+ (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
+ (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
+ (pmtConfig->pbFormat != NULL))
+ {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
+ if(pVih->bmiHeader.biWidth == pVihConnected->bmiHeader.biWidth &&
+ pVih->bmiHeader.biHeight == pVihConnected->bmiHeader.biHeight)
+ mediaTypeID = iFormat ;
+ }
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ return mediaTypeID;
+}
+
+
+/*!
+ Deletes an allocated AM_MEDIA_TYPE structure, including the format block
+*/
+void vpDirectShowGrabberImpl::MyDeleteMediaType(AM_MEDIA_TYPE *pmt)
+{
+ if (pmt != NULL)
+ {
+ MyFreeMediaType(*pmt); // See FreeMediaType for the implementation.
+ CoTaskMemFree(pmt);
+ }
+}
+
+
+/*!
+ Frees the format block in an AM_MEDIA_TYPE structure.
+*/
+void vpDirectShowGrabberImpl::MyFreeMediaType(AM_MEDIA_TYPE& mt)
+{
+ if (mt.cbFormat != 0)
+ {
+ CoTaskMemFree((PVOID)mt.pbFormat);
+ mt.cbFormat = 0;
+ mt.pbFormat = NULL;
+ }
+ if (mt.pUnk != NULL)
+ {
+ // Unecessary because pUnk should not be used, but safest.
+ mt.pUnk->Release();
+ mt.pUnk = NULL;
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowGrabberImpl.cpp.o) has no symbols
+void dummy_vpDirectShowGrabberImpl() {};
+#endif
+#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
new file mode 100644
index 0000000..56df83a
--- /dev/null
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
@@ -0,0 +1,330 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * DirectShow framegrabber.
+ *
+ * Authors:
+ * Bruno Renier
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
+#include <visp3/core/vpImageConvert.h>
+
+#include <visp3/core/vpConfig.h>
+#if ( defined(VISP_HAVE_DIRECTSHOW) )
+
+
+
+/*!
+ Constructor - creates the semaphore
+*/
+vpDirectShowSampleGrabberI::vpDirectShowSampleGrabberI()
+ : acqGrayDemand(false), acqRGBaDemand(false), specialMediaType(false), invertedSource(false)
+{
+ //semaphore(0), max value = 1
+ copySem = CreateSemaphore (NULL,0,1,NULL);
+}
+
+/*!
+ Destructor - destroys the semaphore
+*/
+vpDirectShowSampleGrabberI::~vpDirectShowSampleGrabberI()
+{
+ //destroys the semaphore
+ CloseHandle(copySem);
+}
+
+
+STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, void **ppvObject)
+{
+ if (NULL == ppvObject) return E_POINTER;
+ if (riid == __uuidof(IUnknown))
+ {
+ *ppvObject = static_cast<IUnknown*>(this);
+ return S_OK;
+ }
+ if (riid == __uuidof(ISampleGrabberCB))
+ {
+ *ppvObject = static_cast<ISampleGrabberCB*>(this);
+ return S_OK;
+ }
+ return E_NOTIMPL;
+}
+
+/*!
+ The frame grabber callback -
+ Called when the input buffer is full.
+ Rq : BufferLen == bmpInfo.biWidth*bmpInfo.biHeight*sizeof(vpRGBa)
+*/
+STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, long BufferLen)
+{
+ //if there has been a frame demand
+ if(acqGrayDemand || acqRGBaDemand)
+ {
+ //check if the connected media is compatible
+ if(connectedMediaType.formattype==FORMAT_VideoInfo)
+ {
+ //retrieve the image information
+ VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(connectedMediaType.pbFormat);
+ BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
+
+ //if biHeight > 0 and the source is not special
+ //then the image needs to be verticaly flipped
+ bool flip;
+ if(!specialMediaType)
+ flip = bmpInfo.biHeight>=0;
+ //the source is fourcc and the image is inverted with this compression
+ else if(invertedSource)
+ flip = true;
+ //fourcc and the image doesn't need to be flipped
+ else
+ flip = false;
+
+ //if the buffer contains a RGB24 image (DS RGB24 <=> BGR)
+ if(connectedMediaType.subtype==MEDIASUBTYPE_RGB24)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::BGRToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth() , rgbaIm->getHeight(), flip);
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::BGRToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth(), grayIm->getHeight(), flip);
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else
+ {
+ unsigned long FourCC;
+ FourCC = ((bmpInfo.biCompression&0xFF000000)>>24) |
+ ((bmpInfo.biCompression&0x00FF0000)>>8) |
+ ((bmpInfo.biCompression&0x0000FF00)<<8) |
+ (bmpInfo.biCompression&0x000000FF)<<24;
+ //if the buffer contains a like YUV420 image
+ if(connectedMediaType.subtype==MEDIASUBTYPE_IYUV || FourCC == 'I420')
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV420ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth() , rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_YV12)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YV12ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth() , rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_YVU9)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YVU9ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth() , rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_YUY2 ||
+ connectedMediaType.subtype==MEDIASUBTYPE_YUYV)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YCbCrToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth()*rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_YVYU)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YCrCbToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth()*rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_UYVY)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV422ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
+ rgbaIm->getWidth()*rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::YUV422ToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ else if(connectedMediaType.subtype==MEDIASUBTYPE_RGB32)
+ {
+ //if it was an RGBa image demand
+ if(acqRGBaDemand)
+ {
+ //first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ //copy(pBuffer ,pBuffer + 4*rgbaIm->getWidth()*rgbaIm->getHeight(),rgbaIm->bitmap);
+ memcpy(rgbaIm->bitmap,pBuffer ,4*rgbaIm->getWidth()*rgbaIm->getHeight());
+ //reset the demand boolean
+ acqRGBaDemand = false;
+ }
+ else//if it was a grayscale image demand
+ {
+ //first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ //copy and convert the image
+ vpImageConvert::RGBaToGrey(pBuffer, grayIm->bitmap,
+ grayIm->getWidth() * grayIm->getHeight());
+ //reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ }
+ }
+
+ //increment the semaphore - allows acquire to continue execution
+ ReleaseSemaphore(copySem, 1, NULL);
+ }
+ return S_OK;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowSampleGrabberI.cpp.o) has no symbols
+void dummy_vpDirectShowSampleGrabberI() {};
+#endif
+
+#endif
diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp
new file mode 100644
index 0000000..1b511ab
--- /dev/null
+++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp
@@ -0,0 +1,1367 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Framegrabber based on Video4Linux2 driver.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpV4l2Grabber.cpp
+ \brief class for the Video For Linux 2 video device framegrabbing.
+ \ingroup libdevice
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_V4L2
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/mman.h>
+#include <errno.h>
+#include <iostream>
+
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/core/vpFrameGrabberException.h>
+//#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
+const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
+const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
+const __u32 vpV4l2Grabber::MAX_NORM = 16;
+const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
+const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
+const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
+const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
+#define vpCLEAR(x) memset (&(x), 0, sizeof (x))
+
+/*!
+ Default constructor.
+
+ Setup the Video For Linux Two (V4L2) driver in streaming mode.
+
+ Default settings are:
+
+ - Device name: /dev/video0: To change it use setDevice()
+
+ - Number of ring buffers: 3. To change this value use setNBuffers(). For non
+ real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to
+ set the number of buffers to 3.
+
+ - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
+ fps. These framerates are reachable only if enought buffers are set.
+
+ - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
+ to change it.
+
+ - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
+ or setWidth() and setHeight to change it.
+
+ \code
+ vpImage<unsigned char> I; // Grey level image
+
+ vpV4l2Grabber g;
+ g.setInput(2); // Input 2 on the board
+ g.setWidth(768); // Acquired images are 768 width
+ g.setHeight(576); // Acquired images are 576 height
+ g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
+ g.open(I); // Open the grabber
+
+ g.acquire(I); // Acquire a 768x576 grey image
+
+ \endcode
+
+*/
+vpV4l2Grabber::vpV4l2Grabber()
+ : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+ fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+ waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
+ m_input(vpV4l2Grabber::DEFAULT_INPUT),
+ m_framerate(vpV4l2Grabber::framerate_25fps),
+ m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+ m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+{
+ setDevice("/dev/video0");
+ setNBuffers(3);
+ setFramerate(vpV4l2Grabber::framerate_25fps);
+ setInput(vpV4l2Grabber::DEFAULT_INPUT);
+ setScale(vpV4l2Grabber::DEFAULT_SCALE);
+ setPixelFormat(V4L2_YUYV_FORMAT);
+
+ init = false;
+}
+
+/*!
+ Default constructor.
+
+ Setup the Video For Linux Two (V4L2) driver in streaming mode.
+
+ \param verbose : If true activates the verbose mode.
+
+ Default settings are:
+
+ - Device name: /dev/video0: To change it use setDevice()
+
+ - Number of ring buffers: 3. To change this value use setNBuffers(). For non
+ real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to
+ set the number of buffers to 3.
+
+ - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
+ fps. These framerates are reachable only if enought buffers are set.
+
+ - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
+ to change it.
+
+ - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
+ or setWidth() and setHeight to change it.
+
+ \code
+ vpImage<unsigned char> I; // Grey level image
+
+ vpV4l2Grabber g(true); // Activates the verbose mode
+ g.setInput(2); // Input 2 on the board
+ g.setWidth(768); // Acquired images are 768 width
+ g.setHeight(576); // Acquired images are 576 height
+ g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
+ g.open(I); // Open the grabber
+
+ g.acquire(I); // Acquire a 768x576 grey image
+
+ \endcode
+
+*/
+vpV4l2Grabber::vpV4l2Grabber(bool verbose)
+ : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+ fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+ waiton_cpt(0), index_buffer(0), m_verbose(verbose), m_nbuffers(3), field(0), streaming(false),
+ m_input(vpV4l2Grabber::DEFAULT_INPUT),
+ m_framerate(vpV4l2Grabber::framerate_25fps),
+ m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+ m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+{
+ setDevice("/dev/video0");
+ setNBuffers(3);
+ setFramerate(vpV4l2Grabber::framerate_25fps);
+ setInput(vpV4l2Grabber::DEFAULT_INPUT);
+ setScale(vpV4l2Grabber::DEFAULT_SCALE);
+ setPixelFormat(V4L2_YUYV_FORMAT);
+
+ init = false;
+}
+
+
+/*!
+ Constructor.
+
+ Setup the Video For Linux Two (V4L2) driver in streaming mode.
+
+ \param input : Video input port.
+ \param scale : Decimation factor.
+
+ Default settings are:
+
+ - Device name: /dev/video0: To change it use setDevice()
+
+ - Number of ring buffers: 3. To change this value use setNBuffers(). For non
+ real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to
+ set the number of buffers to 3.
+
+ - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
+ fps. These framerates are reachable only if enought buffers are set.
+
+ \code
+ vpImage<unsigned char> I; // Grey level image
+
+ vpV4l2Grabber g(1, 2); // Select input 1, and half full size resolution images.
+ g.open(I); // Open the grabber
+
+ g.acquire(I); // Acquire a 384x288 grey image
+
+ \endcode
+*/
+vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
+ : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+ fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+ waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
+ m_input(vpV4l2Grabber::DEFAULT_INPUT),
+ m_framerate(vpV4l2Grabber::framerate_25fps),
+ m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+ m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+{
+ setDevice("/dev/video0");
+ setNBuffers(3);
+ setFramerate(vpV4l2Grabber::framerate_25fps);
+ setInput(input);
+ setScale(scale);
+
+ init = false;
+}
+
+/*!
+ Constructor.
+
+ Setup the Video For Linux Two (V4L2) driver in streaming mode.
+
+ \param I : Image data structure (grey 8 bits image)
+ \param input : Video input port.
+ \param scale : Decimation factor.
+
+ Default settings are:
+
+ - Device name: /dev/video0: To change it use setDevice()
+
+ - Number of ring buffers: 3. To change this value use setNBuffers(). For non
+ real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to
+ set the number of buffers to 3.
+
+ - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
+ fps. These framerates are reachable only if enought buffers are set.
+
+ \code
+ vpImage<unsigned char> I; // Grey level image
+
+ vpV4l2Grabber g(I, 1, 2); // Select input 1, and half full size resolution
+ // images and open the grabber
+
+ g.acquire(I); // Acquire a 384x288 grey image
+
+ \endcode
+*/
+vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale )
+ : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+ fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+ waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
+ m_input(vpV4l2Grabber::DEFAULT_INPUT),
+ m_framerate(vpV4l2Grabber::framerate_25fps),
+ m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+ m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+{
+ setDevice("/dev/video0");
+ setNBuffers(3);
+ setFramerate(vpV4l2Grabber::framerate_25fps);
+ setInput(input);
+ setScale(scale);
+
+ init = false;
+
+ open(I);
+}
+
+/*!
+ Constructor.
+
+ Setup the Video For Linux Two (V4L2) driver in streaming mode.
+
+ \param I : Image data structure (Color RGB32 bits image)
+ \param input : Video input port.
+ \param scale : Decimation factor.
+
+ Default settings are:
+
+ - Device name: /dev/video0: To change it use setDevice()
+
+ - Number of ring buffers: 3. To change this value use setNBuffers(). For non
+ real-time applications the number of buffers should be set to 1. For
+ real-time applications to reach 25 fps or 50 fps a good compromise is to
+ set the number of buffers to 3.
+
+ - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
+ fps. These framerates are reachable only if enought buffers are set.
+
+ \code
+ vpImage<vpRGBa> I; // Color image
+
+ vpV4l2Grabber g(I, 1, 2); // Select input 1, and half full size resolution
+ // images and open the grabber
+
+ g.acquire(I); // Acquire a 384x288 color image
+
+ \endcode
+
+*/
+vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale )
+ : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
+ fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
+ waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
+ m_input(vpV4l2Grabber::DEFAULT_INPUT),
+ m_framerate(vpV4l2Grabber::framerate_25fps),
+ m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
+ m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
+{
+ setDevice("/dev/video0");
+ setNBuffers(3);
+ setFramerate(vpV4l2Grabber::framerate_25fps);
+ setInput(input);
+ setScale(scale);
+
+ init = false;
+
+ open(I);
+}
+
+/*!
+ Destructor.
+
+ \sa close()
+*/
+vpV4l2Grabber::~vpV4l2Grabber()
+{
+ close() ;
+}
+
+/*!
+ Set the video input port on the board.
+*/
+void
+vpV4l2Grabber::setInput(unsigned input)
+{
+ this->m_input = input;
+}
+
+/*!
+ Set the decimation factor applied to full resolution images (768x576).
+
+ \exception vpFrameGrabberException::settingError : Wrong scale (should be
+ between 1 and 16).
+
+ \param scale : Decimation factor. If scale is set to 2, 384x288 images will
+ be acquired.
+
+ An other way to specify the image size is to use setWidth() and setHeight().
+
+*/
+void
+vpV4l2Grabber::setScale(unsigned scale)
+{
+ if ((scale <1) || (scale >16))
+ {
+ close();
+
+ vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16",scale) ;
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Wrong scale") );
+ }
+
+ setWidth(640/scale);
+ setHeight(480/scale);
+}
+
+/*!
+ Initialize image acquisition in grey format.
+ Set the pixel format acquisition to vpV4l2Grabber::V4L2_GREY_FORMAT.
+
+ \param I : Image data structure (8 bits image). Once the device is open,
+ the image is resized to the current acquisition size.
+
+ \exception vpFrameGrabberException::settingError : Wrong input channel.
+
+*/
+void
+vpV4l2Grabber::open(vpImage<unsigned char> &I)
+{
+ open();
+
+ if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
+ {
+ std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
+ }
+
+ vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
+
+ try {
+ setFormat();
+
+ startStreaming();
+ }
+ catch(...) {
+ if (m_verbose) {
+ std::cout << "Requested pixel format [" << req_pixelformat
+ << "] not compatible with camera" << std::endl;
+ std::cout << "Try to found a compatible pixel format..." << std::endl;
+ }
+
+ // try to fing a compatible format
+ for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
+ if (format == req_pixelformat) {
+ continue;
+ }
+ try {
+ setPixelFormat((vpV4l2PixelFormatType)format);
+ setFormat();
+ startStreaming();
+ if (m_verbose)
+ std::cout << "This format [" << m_pixelformat
+ << "] is compatible with camera" << std::endl;
+
+ break;
+ }
+ catch (...) {
+ if (m_verbose)
+ std::cout << "This format [" << m_pixelformat
+ << "] is not compatible with camera" << std::endl;
+ if (format == (int)V4L2_MAX_FORMAT) {
+ std::cout << "No pixel format compatible with the camera was found" << std::endl;
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "No pixel format compatible with the camera was found"));
+ }
+ }
+ }
+ }
+
+ I.resize(height, width) ;
+
+ init = true;
+}
+
+/*!
+ Initialize image acquisition in color RGB32 format.
+ Set the pixel format acquisition to vpV4l2Grabber::V4L2_RGB32_FORMAT.
+
+
+ \param I : Image data structure (RGB32 bits image). Once the device is open,
+ the image is resized to the current acquisition size.
+
+*/
+void
+vpV4l2Grabber::open(vpImage<vpRGBa> &I)
+{
+ open();
+
+ if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
+ {
+ std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
+ }
+
+ vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
+
+ try {
+ setFormat();
+
+ startStreaming();
+ }
+ catch(...) {
+ if (m_verbose) {
+ std::cout << "Requested pixel format [" << m_pixelformat
+ << "] not compatible with camera" << std::endl;
+ std::cout << "Try to found a compatible pixel format..." << std::endl;
+ }
+
+ // try to fing a compatible format
+ for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
+ if (format == req_pixelformat) {
+ continue;
+ }
+ try {
+ setPixelFormat((vpV4l2PixelFormatType)format);
+ setFormat();
+ startStreaming();
+ if (m_verbose)
+ std::cout << "This format [" << m_pixelformat
+ << "] is compatible with camera" << std::endl;
+
+ break;
+ }
+ catch (...) {
+ if (m_verbose)
+ std::cout << "This format [" << m_pixelformat
+ << "] is not compatible with camera" << std::endl;
+
+ }
+ }
+ }
+
+ I.resize(height, width) ;
+
+ init = true;
+}
+
+
+
+/*!
+ Acquire a grey level image.
+
+ \param I : Image data structure (8 bits image)
+
+ \exception vpFrameGrabberException::initializationError : Frame grabber not
+ initialized.
+
+ \sa getField()
+*/
+void
+vpV4l2Grabber::acquire(vpImage<unsigned char> &I)
+{
+ struct timeval timestamp;
+
+ acquire(I, timestamp);
+}
+
+/*!
+ Acquire a grey level image.
+
+ \param I : Image data structure (8 bits image).
+
+ \param timestamp : Timeval data structure providing the unix time
+ at which the frame was captured in the ringbuffer.
+
+ \exception vpFrameGrabberException::initializationError : Frame grabber not
+ initialized.
+
+ \sa getField()
+*/
+void
+vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval ×tamp)
+{
+ if (init==false)
+ {
+ open(I);
+ }
+
+ if (init==false)
+ {
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "V4l2 frame grabber not initialized") );
+ }
+
+ unsigned char *bitmap ;
+ bitmap = waiton(index_buffer, timestamp);
+
+ if ((I.getWidth() != width)||(I.getHeight() != height))
+ I.resize(height, width) ;
+
+ switch(m_pixelformat) {
+ case V4L2_GREY_FORMAT:
+ memcpy(I.bitmap, bitmap, height * width*sizeof(unsigned char));
+ break;
+ case V4L2_RGB24_FORMAT:
+ vpImageConvert::RGBToGrey((unsigned char *) bitmap, I.bitmap, width*height);
+
+ break;
+ case V4L2_RGB32_FORMAT:
+ vpImageConvert::RGBaToGrey((unsigned char *) bitmap, I.bitmap, width*height);
+
+ break;
+ case V4L2_BGR24_FORMAT:
+ vpImageConvert::BGRToGrey( (unsigned char *) bitmap, I.bitmap, width, height, false);
+
+ break;
+ case V4L2_YUYV_FORMAT:
+ vpImageConvert::YUYVToGrey( (unsigned char *) bitmap, I.bitmap, width*height);
+
+ break;
+ default:
+ std::cout << "V4L2 conversion not handled" << std::endl;
+ break;
+ }
+
+ queueAll();
+}
+
+/*!
+ Acquire a color image.
+
+ \param I : Image data structure (32 bits image)
+
+ \exception vpFrameGrabberException::initializationError : Frame grabber not
+ initialized.
+
+ \sa getField()
+*/
+void
+vpV4l2Grabber::acquire(vpImage<vpRGBa> &I)
+{
+ struct timeval timestamp;
+
+ acquire(I, timestamp);
+}
+
+/*!
+ Acquire a color image.
+
+ \param I : Image data structure (32 bits image).
+
+ \param timestamp : Timeval data structure providing the unix time
+ at which the frame was captured in the ringbuffer.
+
+ \exception vpFrameGrabberException::initializationError : Frame grabber not
+ initialized.
+
+ \sa getField()
+*/
+void
+vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp)
+{
+ if (init==false)
+ {
+ open(I);
+ }
+
+ if (init==false)
+ {
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "V4l2 frame grabber not initialized") );
+ }
+
+ unsigned char *bitmap ;
+ bitmap = waiton(index_buffer, timestamp);
+
+ if ((I.getWidth() != width)||(I.getHeight() != height))
+ I.resize(height, width) ;
+
+ // The framegrabber acquire aRGB format. We just shift the data from 1 byte all the data and initialize the last byte
+
+ switch(m_pixelformat) {
+ case V4L2_GREY_FORMAT:
+ vpImageConvert::GreyToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width*height);
+ break;
+ case V4L2_RGB24_FORMAT:
+ vpImageConvert::RGBToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width*height);
+
+ break;
+ case V4L2_RGB32_FORMAT:
+ // The framegrabber acquire aRGB format. We just shift the data
+ // from 1 byte all the data and initialize the last byte
+ memcpy(I.bitmap, bitmap + 1, height * width * sizeof(vpRGBa) - 1);
+ I[height-1][width-1].A = 0;
+ break;
+ case V4L2_BGR24_FORMAT:
+ vpImageConvert::BGRToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width, height, false);
+ break;
+ case V4L2_YUYV_FORMAT:
+ vpImageConvert::YUYVToRGBa( (unsigned char *) bitmap, (unsigned char *) I.bitmap, width, height);
+ break;
+ default:
+ std::cout << "V4l2 conversion not handled" << std::endl;
+ break;
+ }
+
+ queueAll();
+}
+/*!
+
+ Return the field (odd or even) corresponding to the last acquired
+ frame.
+
+ This method is to call after acquire() and has only a mean if the acquisition
+ framerate is set to 50 fps.
+
+ \return Field of the acquired frame (0 if odd field, 1 if even field).
+
+ \exception vpFrameGrabberException::otherError : Video device returns a bad
+ frame field.
+
+ \sa acquire(), setFramerate()
+
+*/
+bool
+vpV4l2Grabber::getField()
+{
+ if(field == 2) return 0; //top field
+ else if (field == 3) return 1; //bottom field;
+ else {
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "V4l2 returns a bad frame field") );
+ return false;
+ }
+}
+/*!
+
+ Set the frame format depending on the framerate acquisition.
+
+ \param framerate : The framerate for the acquisition.
+ - If vpV4l2Grabber::framerate_25fps use vpV4l2Grabber::V4L2_IMAGE_FORMAT,
+ - else if vpV4l2Grabber::framerate_50fps use vpV4l2Grabber::V4L2_FRAME_FORMAT.
+ \warning If you want to acquire frames at 25 fps or 50 fps, you have to be
+ aware of the number of buffers required for the streaming. A typical value
+ could be 3 (see setNBuffers()).
+
+ \sa getFramerate(), setNBuffers()
+
+*/
+void
+vpV4l2Grabber::setFramerate(vpV4l2Grabber::vpV4l2FramerateType framerate)
+{
+ this->m_framerate = framerate;
+
+ if (framerate == vpV4l2Grabber::framerate_25fps)
+ setFrameFormat(V4L2_IMAGE_FORMAT);
+ else
+ setFrameFormat(V4L2_FRAME_FORMAT);
+}
+
+/*!
+
+ Return the framerate of the acquisition.
+
+ \return The actual framerate of the framegrabber.
+
+ \sa setFramerate()
+*/
+
+
+vpV4l2Grabber::vpV4l2FramerateType
+vpV4l2Grabber::getFramerate()
+{
+ return m_framerate;
+}
+
+
+/*!
+ Close the video device.
+*/
+void
+vpV4l2Grabber::close()
+{
+ stopStreaming();
+ streaming = false;
+
+ if (fd >= 0){
+ //vpTRACE("v4l2_close()");
+ v4l2_close (fd);
+ fd = -1;
+ }
+
+ if (inp != NULL) { delete [] inp; inp = NULL; }
+ if (std != NULL) { delete [] std; std = NULL; }
+ if (fmt != NULL) { delete [] fmt; fmt = NULL; }
+ if (ctl != NULL) { delete [] ctl; ctl = NULL; }
+ if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
+ if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
+}
+
+/*!
+
+ Open the Video For Linux Two device.
+
+ \exception vpFrameGrabberException::initializationError : Can't access to
+ video device.
+
+ \exception vpFrameGrabberException::otherError : Can't query video
+ capabilities.
+
+*/
+void
+vpV4l2Grabber::open()
+{
+ /* Open Video Device */
+ struct stat st;
+
+ if (-1 == stat (device, &st)) {
+ fprintf (stderr, "Cannot identify '%s': %d, %s\n",
+ device, errno, strerror (errno));
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Cannot identify video device") );
+
+ }
+
+ if (!S_ISCHR (st.st_mode)) {
+ fprintf (stderr, "%s is no device\n", device);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "No device") );
+
+ }
+ fd = v4l2_open (device, O_RDWR | O_NONBLOCK, 0);
+ if (fd < 0) {
+ close();
+
+ vpERROR_TRACE ("No video device \"%s\"\n", device);
+ throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Can't access to video device") );
+
+ }
+
+ if (inp != NULL) { delete [] inp; inp = NULL; }
+ if (std != NULL) { delete [] std; std = NULL; }
+ if (fmt != NULL) { delete [] fmt; fmt = NULL; }
+ if (ctl != NULL) { delete [] ctl; ctl = NULL; }
+ if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
+ if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
+
+ inp = new struct v4l2_input [vpV4l2Grabber::MAX_INPUTS];
+ std = new struct v4l2_standard [vpV4l2Grabber::MAX_NORM];
+ fmt = new struct v4l2_fmtdesc [vpV4l2Grabber::MAX_FORMAT];
+ ctl = new struct v4l2_queryctrl [vpV4l2Grabber::MAX_CTRL*2];
+ buf_v4l2 = new struct v4l2_buffer [vpV4l2Grabber::MAX_BUFFERS];
+ buf_me = new struct ng_video_buf [vpV4l2Grabber::MAX_BUFFERS];
+
+ /* Querry Video Device Capabilities */
+ if ( v4l2_ioctl (fd, VIDIOC_QUERYCAP, &cap) == -1 ) {
+ close();
+ fprintf (stderr, "%s is no V4L2 device\n", device);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Is not a V4L2 device") );
+ }
+ if (m_verbose) {
+ fprintf(stdout, "v4l2 info:\n"
+ " device: %s\n"
+ " %s %d.%d.%d / %s @ %s\n",
+ device,
+ cap.driver,
+ (cap.version >> 16) & 0xff,
+ (cap.version >> 8) & 0xff,
+ cap.version & 0xff,
+ cap.card, cap.bus_info);
+ if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
+ fprintf(stdout, " Support overlay\n");
+ else
+ fprintf(stdout, " Does not support overlay\n");
+ if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
+ fprintf(stdout, " Support capture\n");
+ else
+ fprintf(stdout, " Does not support capture\n");
+ if (cap.capabilities & V4L2_CAP_TUNER)
+ fprintf(stdout, " Support tuning\n");
+ else
+ fprintf(stdout, " Does not support tuning\n");
+ if (cap.capabilities & V4L2_CAP_STREAMING)
+ fprintf(stdout, " Support streaming capture.\n");
+ else
+ fprintf(stdout, " Does not support streaming capture\n");
+ if(cap.capabilities & V4L2_CAP_ASYNCIO)
+ fprintf(stdout, " Support asynchronous I/O methods\n");
+ else
+ fprintf(stdout, " Does not support asynchronous I/O methods\n");
+ if(cap.capabilities & V4L2_CAP_TIMEPERFRAME)
+ fprintf(stdout, " Support time per frame field\n");
+ else
+ fprintf(stdout, " Does not support time per frame field\n");
+ // Get framerate
+ streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
+ fprintf(stdout, " Current acquisition framerate: %d fps\n",
+ streamparm.parm.output.timeperframe.denominator);
+ }
+ }
+
+ getCapabilities();
+}
+
+/*!
+
+ Get device capabilities.
+
+ \exception vpFrameGrabberException::otherError : Can't get video parameters.
+*/
+void
+vpV4l2Grabber::getCapabilities()
+{
+ for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
+ inp[ninputs].index = ninputs;
+ if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
+ break;
+ }
+ for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
+ std[nstds].index = nstds;
+ if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
+ break;
+
+ }
+ for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
+ fmt[nfmts].index = nfmts;
+ fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
+ break;
+ }
+
+ streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1)
+ {
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't get video parameters") );
+ }
+}
+
+/*!
+
+ Set the capture format using the settings:
+ - image size : specified by setWidth(), setHeight() or setScale()
+ - frame format : specified by setFramerate()
+ - pixel format : specified by setPixelFormat().
+
+
+ \exception vpFrameGrabberException::settingError : Bad format, probably do to
+ a wrong scale.
+
+ \exception vpFrameGrabberException::otherError : Can't get video format.
+*/
+void
+vpV4l2Grabber::setFormat()
+{
+ fmt_me.width = width;
+ fmt_me.height = height;
+ //fmt_me.bytesperline = width; // bad (normally width * depth / 8), but works
+ // because initialized later by an ioctl call to VIDIOC_S_FMT
+
+ switch(m_pixelformat) {
+ case V4L2_GREY_FORMAT : fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
+ if (m_verbose)
+ fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
+ break;
+ case V4L2_RGB24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
+ if (m_verbose)
+ fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
+ break;
+ case V4L2_RGB32_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
+ if (m_verbose)
+ fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
+ break;
+ case V4L2_BGR24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
+ if (m_verbose)
+ fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
+ break;
+ case V4L2_YUYV_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
+ if (m_verbose)
+ fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
+ break;
+
+ default:
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
+ "Bad format, probably do to a wrong scale"));
+ }
+
+ /* Get Video Format */
+ vpCLEAR (fmt_v4l2);
+
+ fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (v4l2_ioctl (fd, VIDIOC_G_FMT, &fmt_v4l2) == -1 ) {
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't get video format") );
+ }
+ fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
+ fmt_v4l2.fmt.pix.width = fmt_me.width;
+ fmt_v4l2.fmt.pix.height = fmt_me.height;
+ //printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
+
+
+ switch (m_frameformat) {
+ case V4L2_FRAME_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
+ if (m_verbose) {
+ fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
+ }
+ break;
+ case V4L2_IMAGE_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ if (m_verbose) {
+ fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
+ }
+ break;
+ default:
+ close();
+
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Unrecognized frame format") );
+ }
+
+ //height and width of the captured image or frame
+ if( m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE )
+ {
+ height = FRAME_SIZE;
+ }
+ //printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
+
+ if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't set video format") );
+ }
+
+ if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Bad pixel format") );
+ }
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt_v4l2.fmt.pix.width * 2;
+ if (fmt_v4l2.fmt.pix.bytesperline < min)
+ fmt_v4l2.fmt.pix.bytesperline = min;
+ min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
+ if (fmt_v4l2.fmt.pix.sizeimage < min)
+ fmt_v4l2.fmt.pix.sizeimage = min;
+
+ fmt_me.width = fmt_v4l2.fmt.pix.width;
+ fmt_me.height = fmt_v4l2.fmt.pix.height;
+ fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
+
+ if (m_verbose) {
+ fprintf(stdout,"v4l2: new capture params (%dx%d, %c%c%c%c, %d byte, %d bytes per line)\n",
+ fmt_me.width, fmt_me.height,
+ fmt_v4l2.fmt.pix.pixelformat & 0xff,
+ (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff,
+ (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
+ (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff,
+ fmt_v4l2.fmt.pix.sizeimage,
+ fmt_v4l2.fmt.pix.bytesperline);
+ }
+
+}
+/*!
+
+ Launch the streaming capture mode and map device memory into application
+ address space.
+
+ \exception vpFrameGrabberException::otherError : If a problem occurs.
+
+*/
+void
+vpV4l2Grabber::startStreaming()
+{
+ if (streaming == true) { // Acquisition in process.
+ stopStreaming();
+ streaming = false;
+ }
+
+ /* setup buffers */
+ memset (&(reqbufs), 0, sizeof (reqbufs));
+ reqbufs.count = m_nbuffers;
+ reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ reqbufs.memory = V4L2_MEMORY_MMAP;
+
+
+ if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1)
+ {
+ if (EINVAL == errno) {
+ fprintf (stderr, "%s does not support "
+ "memory mapping\n", device);
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Does not support memory mapping") );
+ }
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't require video buffers") );
+ }
+
+ for (unsigned i = 0; i < reqbufs.count; i++) {
+ // Clear the buffer
+ memset (&(buf_v4l2[i]), 0, sizeof (buf_v4l2[i]));
+ buf_v4l2[i].index = i;
+ buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
+ buf_v4l2[i].length = 0;
+ if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1)
+ {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't query video buffers") );
+ }
+ 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);
+
+ if(buf_me[i].data == MAP_FAILED)
+ {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't map memory") );
+ }
+
+ 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]);
+ }
+
+ /* queue up all buffers */
+ queueAll();
+
+ /* Set video stream capture on */
+ if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type)<0)
+ {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't start streaming") );
+ }
+
+ streaming = true;
+}
+
+/*!
+
+ Stops the streaming capture mode and unmap the device memory.
+
+ \exception vpFrameGrabberException::otherError : if can't stop streaming.
+*/
+void
+vpV4l2Grabber::stopStreaming()
+{
+ unsigned int i;
+
+ //nothing to do if (fd < 0) or if (streaming == false)
+ if ((fd >= 0) && (streaming == true)) {
+
+ //vpTRACE(" Stop the streaming...");
+ /* stop capture */
+ fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (v4l2_ioctl(fd, VIDIOC_STREAMOFF,&fmt_v4l2.type)) {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't stop streaming") );
+ }
+ /* free buffers */
+ for (i = 0; i < reqbufs.count; i++) {
+ if (m_verbose)
+ printBufInfo(buf_v4l2[i]);
+ //vpTRACE("v4l2_munmap()");
+
+ if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't unmap memory") );
+ }
+ }
+ queue = 0;
+ waiton_cpt = 0;
+ streaming = false;
+ }
+}
+
+/*!
+ Fill the next buffer. If all the buffers are filled return NULL.
+
+ Update the buffer index. If all the buffers are filled index is set to -1.
+
+ \param index : Index in the buffer where image is available.
+
+ \param timestamp : Timeval data structure providing the unix time
+ [microseconds] at which the frame was captured in the ringbuffer.
+
+ \exception vpFrameGrabberException::otherError : If can't access to the
+ frame.
+*/
+unsigned char *
+vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp)
+{
+ struct v4l2_buffer buf;
+ struct timeval tv;
+ fd_set rdset;
+
+ /* wait for the next frame */
+ again:
+
+ tv.tv_sec = 30;
+ tv.tv_usec = 0;
+ FD_ZERO(&rdset);
+ FD_SET(static_cast<unsigned int>(fd), &rdset);
+ switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
+ case -1:
+ if (EINTR == errno)
+ goto again;
+ index = 0;
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't access to the frame") );
+ return NULL;
+ case 0:
+ index = 0;
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "Can't access to the frame: timeout") );
+ return NULL;
+ }
+
+
+ /* get it */
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
+ if (-1 == v4l2_ioctl(fd,VIDIOC_DQBUF, &buf)) {
+ index = 0;
+ switch(errno)
+ {
+ case EAGAIN:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_DQBUF: EAGAIN") );
+ break;
+ case EINVAL:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_DQBUF: EINVAL") );
+ break;
+ case ENOMEM:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_DQBUF: ENOMEM") );
+ break;
+ default:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_DQBUF") );
+ break;
+ }
+ return NULL;
+ }
+
+ waiton_cpt++;
+ buf_v4l2[buf.index] = buf;
+
+ index = buf.index;
+
+ field = buf_v4l2[index].field;
+
+ timestamp = buf_v4l2[index].timestamp;
+
+ // if(m_verbose)
+ // {
+ // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
+
+ // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
+ // }
+ return buf_me[buf.index].data;
+}
+
+/*!
+
+ Capture helpers.
+
+*/
+int
+vpV4l2Grabber::queueBuffer()
+{
+ unsigned int frame = queue % reqbufs.count;
+ int rc;
+
+
+ if (0 != buf_me[frame].refcount) {
+ if (0 != queue - waiton_cpt)
+ return -1;
+ fprintf(stderr,"v4l2: waiting for a free buffer..............\n");
+ //ng_waiton_video_buf(h->buf_me+frame);
+ std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
+ }
+
+ // std::cout << "frame: " << frame << std::endl;
+ rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
+ if (0 == rc)
+ queue++;
+ else
+ {
+ switch(errno)
+ {
+ case EAGAIN:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_QBUF: EAGAIN") );
+ break;
+ case EINVAL:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_QBUF: EINVAL") );
+ break;
+ case ENOMEM:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_QBUF: ENOMEM") );
+ break;
+ default:
+ throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
+ "VIDIOC_QBUF") );
+ break;
+ }
+ }
+ return rc;
+}
+
+/*!
+
+ Call the queue buffer private method if needed
+
+*/
+void
+vpV4l2Grabber::queueAll()
+{
+ for (;;) {
+ if (queue - waiton_cpt >= reqbufs.count) {
+ return;
+ }
+ if (0 != queueBuffer()) {
+ return;
+ }
+ }
+}
+
+/*!
+
+ Get device capabilities.
+
+*/
+void
+vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
+{
+ char type[40];
+
+ switch(buf.type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE: sprintf(type, "video-cap"); break;
+ case V4L2_BUF_TYPE_VIDEO_OVERLAY: sprintf(type, "video-over"); break;
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT: sprintf(type, "video-out"); break;
+ case V4L2_BUF_TYPE_VBI_CAPTURE: sprintf(type, "vbi-cap"); break;
+ case V4L2_BUF_TYPE_VBI_OUTPUT: sprintf(type, "vbi-out"); break;
+ default: sprintf(type, "unknown"); break;
+ }
+
+ fprintf(stdout,"v4l2: buf %d: %d ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n",
+ buf.index, buf.type, buf.m.userptr, buf.m.offset,
+ buf.length, buf.length, buf.bytesused);
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no symbols
+void dummy_vpV4l2Grabber() {};
+#endif
diff --git a/modules/sensor/src/kinect/vpKinect.cpp b/modules/sensor/src/kinect/vpKinect.cpp
new file mode 100644
index 0000000..e902523
--- /dev/null
+++ b/modules/sensor/src/kinect/vpKinect.cpp
@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * API for using a Microsoft Kinect device
+ * Requires libfreenect as a third party library
+ *
+ * Authors:
+ * Celine Teuliere
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+// Note that libfreenect needs libusb-1.0 and libpthread
+#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
+
+#include <limits> // numeric_limits
+
+#include <visp3/sensor/vpKinect.h>
+#include <visp3/core/vpXmlParserCamera.h>
+
+/*!
+ Default constructor.
+*/
+vpKinect::vpKinect(freenect_context *ctx, int index)
+ : Freenect::FreenectDevice(ctx, index),
+ m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(),
+ rgbMir(), irMrgb(), DMres(DMAP_LOW_RES),
+ hd(240), wd(320),
+ dmap(), IRGB(),
+ m_new_rgb_frame(false),
+ m_new_depth_map(false),
+ m_new_depth_image(false),
+ height(480), width(640)
+{
+ dmap.resize(height, width);
+ IRGB.resize(height, width);
+ vpPoseVector r(-0.0266,-0.0047,-0.0055,0.0320578,0.0169041,-0.0076519 );//!Those are the parameters found for our Kinect device. Note that they can differ from one device to another.
+ rgbMir.buildFrom(r);
+ irMrgb = rgbMir.inverse();
+}
+
+/*!
+ Destructor.
+*/
+vpKinect::~vpKinect()
+{
+
+}
+
+void vpKinect::start(vpKinect::vpDMResolution res)
+{
+ DMres = res;
+ height = 480;
+ width = 640;
+ //!Calibration parameters are the parameters found for our Kinect device. 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;
+ }
+
+#if defined(VISP_HAVE_ACCESS_TO_NAS) && defined(VISP_HAVE_XML2)
+ vpXmlParserCamera cameraParser;
+ char cameraXmlFile[FILENAME_MAX];
+ sprintf(cameraXmlFile, "/udd/fspindle/robot/Viper850/Viper850-code/include/const_camera_Viper850.xml");
+ cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width, height);
+#else
+// RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
+// RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45, 273.27,0.04,-0.04);//old
+// RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
+ RGBcam.initPersProjWithDistortion(522.5431816996,522.7191431808,311.4001982614,267.4283562142,0.0477365207,-0.0462326418);//new
+#endif
+
+ this->startVideo();
+ this->startDepth();
+}
+
+void vpKinect::stop()
+{
+ this->stopVideo();
+ this->stopDepth();
+}
+
+
+/*!
+ Acquire a new RGB image.
+*/
+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<uint8_t*>(rgb);
+ for (unsigned i = 0; i< height;i++){
+ for (unsigned j = 0 ; j < width ; j++)
+ {
+ IRGB[i][j].R = rgb_[3*(width*i +j)+0];
+ IRGB[i][j].G = rgb_[3*(width*i +j)+1];
+ IRGB[i][j].B = rgb_[3*(width*i +j)+2];
+ }
+ }
+
+ m_new_rgb_frame = true;
+}
+
+/*!
+ Acquire a new depth image.
+
+ Depth value send by the kinect is coded on 11 bits : 10 for the
+ value itself (between 0 and 1023) and one for overflow.
+
+ In this function this value is converted into a metric depth map and
+ stored in dmap. (range : 0.3 - 5m).
+
+*/
+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<uint16_t*>(depth);
+ for (unsigned i = 0; i< height;i++){
+ for (unsigned j = 0 ; j < width ; j++)
+ {
+ dmap[i][j] = 0.1236f * tan(depth_[width*i +j] / 2842.5f + 1.1863f);//formula from http://openkinect.org/wiki/Imaging_Information
+ if(depth_[width*i +j]>1023){//Depth cannot be computed
+ dmap[i][j] = -1;
+ }
+ }
+ }
+ m_new_depth_map = true;
+ m_new_depth_image = true;
+}
+
+
+/*!
+ Get metric depth map (float).
+*/
+bool vpKinect::getDepthMap(vpImage<float>& map)
+{
+ vpMutex::vpScopedLock lock(m_depth_mutex);
+ if (!m_new_depth_map)
+ return false;
+ map = this->dmap;
+ m_new_depth_map = false;
+ return true;
+}
+
+
+/*!
+ * Get metric depth map (float) and corresponding image.
+ */
+bool vpKinect::getDepthMap(vpImage<float>& map,vpImage<unsigned char>& Imap)
+{
+ // vpMutex::vpScopedLock lock(m_depth_mutex);
+ vpImage<float> tempMap;
+ m_depth_mutex.lock();
+ if (!m_new_depth_map && !m_new_depth_image)
+ {
+ m_depth_mutex.unlock();
+ return false;
+ }
+ tempMap = dmap;
+
+ m_new_depth_map = false;
+ m_new_depth_image = false;
+ m_depth_mutex.unlock();
+
+ if ((Imap.getHeight()!=hd )||(map.getHeight()!=hd))
+ vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
+ if (DMres == DMAP_LOW_RES){
+ for(unsigned int i = 0; i < hd; i++)
+ for(unsigned int j = 0; j < wd; j++){
+ map[i][j] = tempMap[i<<1][j<<1];
+ //if (map[i][j] != -1)
+ if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
+ Imap[i][j] = (unsigned char)(255*map[i][j]/5);
+ else
+ Imap[i][j] = 255;
+ }
+ }
+ else
+ {
+ for (unsigned i = 0; i< height;i++)
+ for (unsigned j = 0 ; j < width ; j++){
+ map[i][j] = tempMap[i][j];
+ //if (map[i][j] != -1)
+ if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
+ Imap[i][j] = (unsigned char)(255*map[i][j]/5);
+ else
+ Imap[i][j] = 255;
+ }
+ }
+
+ return true;
+}
+
+
+/*!
+ Get RGB image
+*/
+bool vpKinect::getRGB(vpImage<vpRGBa>& I_RGB)
+{
+ vpMutex::vpScopedLock lock(m_rgb_mutex);
+ if (!m_new_rgb_frame)
+ return false;
+ I_RGB = this->IRGB;
+ m_new_rgb_frame = false;
+ return true;
+}
+
+/*!
+ Warp the RGB frame to the depth camera frame. The size of the resulting IrgbWarped frame is the same as the size of the depth map Idepth
+*/
+void vpKinect::warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped)
+{
+ if ((Idepth.getHeight()!=hd )||(Idepth.getWidth()!=wd)){
+ vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
+ }
+ else{
+ if((IrgbWarped.getHeight()!=hd )||(IrgbWarped.getWidth()!=wd))
+ IrgbWarped.resize(hd, wd);
+ IrgbWarped=0;
+ double x1=0., y1=0., x2=0., y2=0., Z1, Z2;
+ vpImagePoint imgPoint(0,0);
+ double u=0., v=0.;
+ vpColVector P1(4),P2(4);
+
+// std::cout <<"rgbMir : "<<rgbMir<<std::endl;
+
+ for (unsigned int i = 0; i< hd;i++)
+ for (unsigned int j = 0 ; j < wd ; j++){
+ //! Compute metric coordinates in the ir camera Frame :
+ vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
+ Z1 = Idepth[i][j];
+ //if (Z1!=-1){
+ if (std::fabs(Z1+1) <= std::numeric_limits<double>::epsilon()){
+ P1[0]=x1*Z1;
+ P1[1]=y1*Z1;
+ P1[2]=Z1;
+ P1[3]=1;
+
+ //! Change frame :
+ P2 = rgbMir*P1;
+ Z2 = P2[2];
+ //if (Z2!= 0){
+ if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()){
+ x2 = P2[0]/P2[2];
+ y2 = P2[1]/P2[2];
+ }
+ else
+ std::cout<<"Z2 = 0 !!"<<std::endl;
+
+ //! compute pixel coordinates of the corresponding point in the depth image
+ vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
+
+ unsigned int u_ = (unsigned int)u;
+ unsigned int v_ = (unsigned int)v;
+ //!Fill warped image value
+ if ((u_<width)&&(v_<height)){
+ IrgbWarped[i][j] = Irgb[v_][u_];
+ }
+ else
+ IrgbWarped[i][j] = 0;
+ }
+ }
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no symbols
+void dummy_vpKinect() {};
+#endif // VISP_HAVE_LIBFREENECT
diff --git a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp
new file mode 100644
index 0000000..aca8eec
--- /dev/null
+++ b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp
@@ -0,0 +1,288 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Sick LD-MRS laser driver on UNIX platform.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+
+#include <visp3/sensor/vpSickLDMRS.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTime.h>
+#include <sys/socket.h>
+#include <sys/select.h>
+#include <netinet/in.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <netdb.h>
+#include <string.h>
+//#include <strings.h>
+#include <math.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <limits.h>
+
+
+
+/*!
+
+ \file vpSickLDMRS.cpp
+
+ \brief Driver for the Sick LD-MRS laser scanner.
+*/
+
+/*!
+
+ Default constructor that initialize the Ethernet address to
+ "131.254.12.119", set the port to 12002 and allocates memory for the
+ body messages.
+*/
+vpSickLDMRS::vpSickLDMRS()
+ : socket_fd(-1), body(NULL), vAngle(), time_offset(0),
+ isFirstMeasure(true), maxlen_body(104000)
+{
+ ip = "131.254.12.119";
+ port = 12002;
+ body = new unsigned char [maxlen_body];
+
+ vAngle.resize(4); // Vertical angle of the 4 layers
+ vAngle[0] = vpMath::rad(-1.2);
+ vAngle[1] = vpMath::rad(-0.4);
+ vAngle[2] = vpMath::rad( 0.4);
+ vAngle[3] = vpMath::rad( 1.2);
+}
+
+/*!
+ Destructor that deallocate the memory for the body messages.
+*/
+vpSickLDMRS::~vpSickLDMRS()
+{
+ if (body)
+ delete [] body;
+}
+
+/*!
+ Initialize the connexion with the Sick LD-MRS laser scanner.
+
+ \param ip_address : Ethernet address of the laser.
+ \param com_port : Ethernet port of the laser.
+
+ \return true if the device was initialized, false otherwise.
+
+*/
+bool vpSickLDMRS::setup(std::string ip_address, int com_port)
+{
+ setIpAddress( ip_address );
+ setPort( com_port );
+ return ( this->setup() );
+}
+
+/*!
+ Initialize the connexion with the Sick LD-MRS laser scanner.
+
+ \return true if the device was initialized, false otherwise.
+*/
+bool vpSickLDMRS::setup()
+{
+ struct sockaddr_in serv_addr;
+ int res;
+ struct timeval tv;
+ fd_set myset;
+
+ // Create the TCP socket
+ socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+ if (socket_fd < 0) {
+ fprintf(stderr, "Failed to create socket\n");
+ return false;
+ }
+ //bzero(&serv_addr, sizeof(serv_addr));
+ memset(&serv_addr, 0, sizeof(serv_addr));
+ serv_addr.sin_family = AF_INET; // Internet/IP
+ serv_addr.sin_addr.s_addr = inet_addr(ip.c_str()); // IP address
+ serv_addr.sin_port = htons(port); // server port
+
+ // Establish connection
+ res = connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) ;
+ if (errno == EINPROGRESS) {
+ tv.tv_sec = 3;
+ tv.tv_usec = 0;
+ FD_ZERO(&myset);
+ FD_SET(static_cast<unsigned int>(socket_fd), &myset);
+ res = select(socket_fd+1, NULL, &myset, NULL, &tv);
+ if (res < 0 && errno != EINTR) {
+ fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno));
+ return false;
+ }
+ else if (res > 0) {
+ fprintf(stderr,"ok");
+ }
+ else {
+ fprintf(stderr, "Timeout in select() - Cancelling!\n");
+ return false;
+ }
+ }
+
+ return true;
+}
+
+/*!
+ Get the measures of the four scan layers.
+
+ \return true if the measures are retrieven, false otherwise.
+
+*/
+bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
+{
+ unsigned int *uintptr;
+ unsigned short *ushortptr;
+ static unsigned char header[24];
+ ushortptr=(unsigned short *)header;
+ uintptr=(unsigned int *)header;
+
+ assert (sizeof(header) == 24);
+ //std::cout << "size " << sizeof(header) << std::endl;
+
+ double time_second = 0;
+
+ if (isFirstMeasure) {
+ time_second = vpTime::measureTimeSecond();
+ }
+
+ // read the 24 bytes header
+ if (recv(socket_fd, header, sizeof(header), MSG_WAITALL) == -1) {
+ printf("recv\n");
+ perror("recv");
+ return false;
+ }
+
+ if (ntohl(uintptr[0]) != vpSickLDMRS::MagicWordC2) {
+ printf("Error, wrong magic number !!!\n");
+ return false;
+ }
+
+ // get the message body
+ uint16_t msgtype = ntohs(ushortptr[7]);
+ uint32_t msgLength = ntohl(uintptr[2]);
+
+ ssize_t len = recv(socket_fd, body, msgLength, MSG_WAITALL);
+ if (len != (ssize_t)msgLength){
+ printf("Error, wrong msg length: %d of %d bytes.\n", (int)len, msgLength);
+ return false;
+ }
+
+ if (msgtype!=vpSickLDMRS::MeasuredData){
+ //printf("The message in not relative to measured data !!!\n");
+ return true;
+ }
+
+ // decode measured data
+
+ // get the measurement number
+ unsigned short measurementId;
+ ushortptr = (unsigned short *) body;
+ measurementId = ushortptr[0];
+
+ // get the start timestamp
+ uintptr=(unsigned int *) (body+6);
+ unsigned int seconds = uintptr[1];
+ unsigned int fractional=uintptr[0];
+ double startTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
+
+ // get the end timestamp
+ uintptr=(unsigned int *) (body+14);
+ seconds = uintptr[1];
+ fractional=uintptr[0];
+ double endTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
+
+ // compute the time offset to bring the measures in the Unix time reference
+ if (isFirstMeasure) {
+ time_offset = time_second - startTimestamp;
+ isFirstMeasure = false;
+ }
+
+ startTimestamp += time_offset;
+ endTimestamp += time_offset;
+
+ // get the number of steps per scanner rotation
+ unsigned short numSteps = ushortptr[11];
+
+ // 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];
+
+ int nlayers = 4;
+ for (int i=0; i < nlayers; i++) {
+ laserscan[i].clear();
+ laserscan[i].setMeasurementId(measurementId);
+ laserscan[i].setStartTimestamp(startTimestamp);
+ laserscan[i].setEndTimestamp(endTimestamp);
+ laserscan[i].setNumSteps(numSteps);
+ laserscan[i].setStartAngle(startAngle);
+ laserscan[i].setStopAngle(stopAngle);
+ laserscan[i].setNumPoints(numPoints);
+ }
+
+ // decode the measured points
+ double hAngle; // horizontal angle in rad
+ double rDist; // radial distance in meters
+ vpScanPoint scanPoint;
+
+ if (numPoints > USHRT_MAX-2)
+ throw(vpException (vpException::ioError, "Out of range number of point"));
+
+ for (int i=0; i < numPoints; i++) {
+ ushortptr = (unsigned short *) (body+44+i*10);
+ unsigned char layer = ((unsigned char) body[44+i*10])&0x0F;
+ unsigned char echo = ((unsigned char) body[44+i*10])>>4;
+ //unsigned char flags = (unsigned char) body[44+i*10+1];
+
+ if (echo==0) {
+ hAngle = (2.f * M_PI / numSteps)*(short) ushortptr[1];
+ rDist = 0.01 * ushortptr[2]; // cm to meters conversion
+
+ //vpTRACE("layer: %d d: %f hangle: %f", layer, rDist, hAngle);
+ scanPoint.setPolar(rDist, hAngle, vAngle[layer]);
+ laserscan[layer].addPoint(scanPoint);
+ }
+ }
+ return true;
+}
+
+#endif
diff --git a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp
new file mode 100644
index 0000000..60a2fa8
--- /dev/null
+++ b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file test1394TwoGrabber.cpp
+
+ \brief Aquire images using libdc1394-2.x library.
+
+*/
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <iostream>
+#include <string>
+
+#if defined(VISP_HAVE_DC1394)
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+/*!
+ \example test1394TwoGrabber.cpp
+
+
+
+*/
+int
+main()
+{
+ try {
+ std::cout << "IEEE1394 test in progress..." << std::endl;
+
+ // Get the user name
+ std::string username;
+ vpIoTools::getUserName(username);
+ std::string outputpath = "/tmp/" + username;
+ vpIoTools::makeDirectory(outputpath);
+
+ // Creation of an empty image container
+ vpImage<unsigned char> I;
+
+ // Creation of a framegrabber
+ vp1394TwoGrabber g;
+ uint64_t guid;
+
+ // Get the number of cameras connected on the bus
+ unsigned int ncameras; // Number of cameras on the bus
+ ncameras = g.getNumCameras();
+ for (unsigned int i=0; i < ncameras; i++) {
+ g.setCamera(i);
+ g.getGuid(guid);
+ std::cout << "Detected camera with GUID 0x"
+ << std::hex << guid << std::endl;
+ }
+ // If more than one camera connected, use the last one
+ if (ncameras > 1) {
+ g.setCamera(ncameras -1);
+ g.getGuid(guid);
+ std::cout << "Use camera with GUID 0x" << std::hex << guid << std::endl;
+ g.setCamera(0); // to be sure that the setCamera() in the next line with guid as parameter works
+ g.setCamera(guid);
+ }
+
+ for (int i=0;i<10;i++) g.acquire(I);
+ g.close();
+ std::string filename = outputpath + "/imagetest1.pgm";
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename);
+
+ std::cout << "New connection..."<< std::endl;
+ g.open(I);
+ g.close();
+
+ std::cout << "New connection..."<< std::endl;
+ g.open(I);
+ //g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_800x600_YUV422);
+ g.acquire(I);
+ g.close();
+ filename = outputpath + "/imagetest2.pgm";
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename);
+ }
+ catch (...) {
+ vpCERROR << "Failure: exit" << std::endl;
+ }
+}
+#else
+int
+main()
+{
+ vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
+ "You should install libdc1394-2 to use this binary.") ;
+}
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp
new file mode 100644
index 0000000..2c062d0
--- /dev/null
+++ b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Firewire cameras video capture.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file test1394TwoResetBus.cpp
+
+ \brief Resets the IEEE1394 bus using libdc1394-2.x library.
+
+*/
+
+
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <iostream>
+
+#if defined(VISP_HAVE_DC1394)
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+
+
+/*!
+ \example test1394TwoResetBus.cpp
+
+ Resets the IEEE1394 bus which first camera is attached to. Resetting
+ the bus is "rude" to other devices because it causes them to
+ re-enumerate on the bus and may cause a temporary disruption in
+ their current activities. Thus, use it sparingly. Its primary use
+ is if a program shuts down uncleanly and needs to free leftover ISO
+ channels or bandwidth. A bus reset will free those things as a side
+ effect.
+
+*/
+int
+main()
+{
+ try {
+ std::cout << "IEEE1394 bus reset in progress..." << std::endl;
+ vp1394TwoGrabber g;
+ g.resetBus(); // Reset the bus attached to the first camera found
+ std::cout << "IEEE1394 bus was reset." << std::endl;
+
+ vpImage<unsigned char> I;
+ g.acquire(I);
+ // std::cout << "write /tmp/test.pgm" << std::endl;
+ // vpImageIo::write(I, "/tmp/test.pgm");
+ }
+ catch (...) {
+ vpCERROR << "Failure: exit" << std::endl;
+ }
+}
+#else
+int
+main()
+{
+ vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
+ "You should install libdc1394-2 to use this binary.") ;
+}
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/tracker/blob/CMakeLists.txt b/modules/tracker/blob/CMakeLists.txt
new file mode 100644
index 0000000..941dec0
--- /dev/null
+++ b/modules/tracker/blob/CMakeLists.txt
@@ -0,0 +1,42 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(blob visp_core)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+vp_add_tests(DEPENDS_ON visp_features)
diff --git a/modules/tracker/blob/include/visp3/blob/vpDot.h b/modules/tracker/blob/include/visp3/blob/vpDot.h
new file mode 100644
index 0000000..4ae57e6
--- /dev/null
+++ b/modules/tracker/blob/include/visp3/blob/vpDot.h
@@ -0,0 +1,421 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Track a white dot.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpDot.h
+ \brief Track a white dot
+*/
+
+#ifndef vpDot_hh
+#define vpDot_hh
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpTracker.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <math.h>
+#include <fstream>
+#include <list>
+#include <vector>
+
+#ifdef VISP_USE_MSVC
+# pragma comment(linker, "/STACK:256000000") // Increase max recursion depth
+#endif
+
+/*!
+ \class vpDot
+
+ \ingroup module_blob
+
+ \brief This tracker is meant to track a dot (connected pixels with same
+ gray level) on a vpImage.
+
+ The underground algorithm is based on a binarization of the image
+ and a connex component segmentation to determine the dot
+ characteristics (location, moments, size...).
+
+ The following sample code shows how to grab images from a firewire camera,
+ track a blob and display the tracking results.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpImage.h>
+
+int main()
+{
+#if defined(VISP_HAVE_DC1394)
+ vpImage<unsigned char> I; // Create a gray level image container
+ vp1394TwoGrabber g(false); // Create a grabber based on libdc1394-2.x third party lib
+ g.acquire(I); // Acquire an image
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX d(I, 0, 0, "Camera view");
+#endif
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ vpDot blob;
+ blob.initTracking(I);
+ blob.setGraphics(true);
+
+ while(1) {
+ g.acquire(I); // Acquire an image
+ vpDisplay::display(I);
+ blob.track(I);
+
+ vpDisplay::flush(I);
+ }
+#endif
+}
+ \endcode
+
+ \sa vpDot2
+*/
+class VISP_EXPORT vpDot : public vpTracker
+{
+public :
+ /*! \enum vpConnexityType
+ Type of connexity 4, or 8.
+ */
+ typedef enum {
+ CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
+ right, up, down) */
+ CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
+ right, up, down, and the 4 pixels located on the diagonal) */
+ } vpConnexityType;
+
+ static const unsigned int SPIRAL_SEARCH_SIZE; /*!< Spiral size for the dot
+ search. */
+ double m00; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
+ with \f$i = j = 0 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m01; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 1 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m10; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 0 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m11; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double m20; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
+ obtained with \f$i = 2 \f$ and \f$ j = 0 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double m02; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 2 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
+ \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
+ \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
+ \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+
+ vpDot() ;
+ vpDot(const vpImagePoint &ip) ;
+ vpDot(const vpDot& d) ;
+ virtual ~vpDot() ;
+
+ void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
+ unsigned int thickness=1) const;
+
+ /*!
+
+ Return the dot bounding box.
+
+ \sa getWidth(), getHeight()
+
+ */
+ inline vpRect getBBox() const {
+ vpRect bbox;
+
+ bbox.setRect(this->u_min,
+ this->v_min,
+ this->u_max - this->u_min + 1,
+ this->v_max - this->v_min + 1);
+
+ return (bbox);
+ };
+ /*!
+ Return the location of the dot center of gravity.
+
+ \return The coordinates of the center of gravity.
+ */
+ inline vpImagePoint getCog() const {
+ return cog;
+ }
+
+ /*!
+ Return the list of all the image points on the border of the dot.
+
+ \warning Doesn't return the image points inside the dot anymore. To get those points see getConnexities().
+ */
+ inline std::list<vpImagePoint> getEdges() const {
+ return this->ip_edges_list;
+ };
+
+ /*!
+
+ Return the list of all the image points inside the dot.
+
+ \return The list of all the images points in the dot.
+ This list is updated after a call to track().
+
+ */
+ inline std::list<vpImagePoint> getConnexities() const {
+ return this->ip_connexities_list;
+ };
+
+ inline double getGamma() const {return this->gamma;};
+ /*!
+
+ Return the precision of the gray level of the dot. It is a double
+ precision float witch value is in ]0,1]. 1 means full precision, whereas
+ values close to 0 show a very bad precision.
+
+ */
+ double getGrayLevelPrecision() const {return grayLevelPrecision;}
+ double getMaxDotSize() const {
+ return this->maxDotSizePercentage;
+ }
+ /*!
+ Return the mean gray level value of the dot.
+ */
+ double getMeanGrayLevel() const {
+ return (this->mean_gray_level);
+ };
+
+ /*!
+
+ Return the width of the dot.
+
+ \sa getHeight()
+
+ */
+ inline unsigned int getWidth() const {
+ return (this->u_max - this->u_min + 1);
+ };
+
+ /*!
+
+ Return the width of the dot.
+
+ \sa getHeight()
+
+ */
+ inline unsigned int getHeight() const {
+ return (this->v_max - this->v_min + 1);
+ };
+
+ void initTracking(const vpImage<unsigned char> &I) ;
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip,
+ unsigned int gray_level_min, unsigned int gray_level_max);
+
+ vpDot& operator =(const vpDot& d) ;
+ bool operator ==(const vpDot& d);
+ bool operator !=(const vpDot& d);
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d);
+
+ void print(std::ostream& os) { os << *this << std::endl ; }
+
+ /*!
+ Initialize the dot coordinates with \e ip.
+ */
+ inline void setCog(const vpImagePoint &ip) {
+ this->cog = ip;
+ }
+
+ /*!
+
+ Activates the dot's moments computation.
+
+ \param activate true, if you want to compute the moments. If false, moments
+ are not computed.
+
+ Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
+ vpDot::m20, vpDot::m02.
+
+ The coordinates of the region's centroid (u, v) can be computed from the
+ moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$.
+
+ */
+ void setComputeMoments(const bool activate) { compute_moment = activate; }
+
+ /*!
+ Set the type of connexity: 4 or 8.
+ */
+ void setConnexity(vpConnexityType type) {this->connexityType = type; };
+ void setMaxDotSize(double percentage) ;
+ void setGrayLevelMin( const unsigned int &level_min ) {
+ this->gray_level_min = level_min;
+ };
+ void setGrayLevelMax( const unsigned int &level_max ) {
+ this->gray_level_max = level_max;
+ };
+ void setGrayLevelPrecision( const double & grayLevelPrecision );
+
+ /*!
+ Activates the display of all the pixels of the dot during the tracking.
+ The default thickness of the overlayed drawings can be modified using
+ setGraphicsThickness().
+
+ \warning To effectively display the dot graphics a call to
+ vpDisplay::flush() is needed.
+
+ \param activate true to activate the display of dot pixels, false to turn
+ off the display.
+
+ \sa setGraphicsThickness()
+ */
+ void setGraphics(const bool activate) { graphics = activate ; }
+ /*!
+ Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
+
+ \sa setGraphics()
+ */
+ void setGraphicsThickness(unsigned int t) {this->thickness = t;};
+
+ void track(const vpImage<unsigned char> & I) ;
+ void track(const vpImage<unsigned char> & I, vpImagePoint &ip) ;
+
+private:
+ //! internal use only
+ std::list<vpImagePoint> ip_connexities_list;
+
+ //! List of border points
+ std::list<vpImagePoint> ip_edges_list;
+
+ /*! Type of connexity
+
+ \warning In previous version this variable was called connexity
+ */
+ vpConnexityType connexityType;
+
+ //! coordinates of the point center of gravity
+ vpImagePoint cog;
+
+ // Bounding box
+ unsigned int u_min, u_max, v_min, v_max;
+
+ // Flag used to allow display
+ bool graphics ;
+
+ unsigned int thickness; // Graphics thickness
+
+ double maxDotSizePercentage;
+ unsigned char gray_level_out;
+
+ double mean_gray_level; // Mean gray level of the dot
+ unsigned int gray_level_min; // left threshold for binarisation
+ unsigned int gray_level_max; // right threshold for binarisation
+ double grayLevelPrecision; //precision of the gray level of the dot.
+ //It is a double precision float witch value is in ]0,1].
+ //1 means full precision, whereas values close to 0 show a very bad precision
+ double gamma ;
+ //! flag : true moment are computed
+ bool compute_moment ;
+ double nbMaxPoint;
+
+ void init() ;
+ void setGrayLevelOut();
+ bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
+ double &mean_value, double &u_cog, double &v_cog, double &n);
+ bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
+ double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab);
+ void COG(const vpImage<unsigned char> &I,double& u, double& v) ;
+
+//Static Functions
+public:
+ static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
+ unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
+ unsigned int thickness=1);
+} ;
+
+#endif
+
+
+
diff --git a/modules/tracker/blob/include/visp3/blob/vpDot2.h b/modules/tracker/blob/include/visp3/blob/vpDot2.h
new file mode 100644
index 0000000..537d734
--- /dev/null
+++ b/modules/tracker/blob/include/visp3/blob/vpDot2.h
@@ -0,0 +1,555 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Track a white dot.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*
+ \file vpDot2.h
+ \brief This tracker is meant to track some zones on a vpImage.
+*/
+
+#ifndef vpDot2_hh
+#define vpDot2_hh
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpTracker.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <vector>
+#include <list>
+
+/*!
+ \class vpDot2
+
+ \ingroup module_blob
+
+ \brief This tracker is meant to track a blob (connex pixels with same
+ gray level) on a vpImage.
+
+ The underground algorithm is based on a binarisation of the image
+ and then on a contour detection using the Freeman chain coding to
+ determine the blob characteristics (location, moments, size...).
+
+ The binarisation is done using gray level minimum and maximum values
+ that define the admissible gray levels of the blob. You can specify these
+ levels by setGrayLevelMin() and setGrayLevelMax(). These levels are also
+ set automatically by setGrayLevelPrecision(). The algorithm allows
+ to track white objects on a black background and vice versa.
+
+ When a blob is found, some tests are done to see if it is valid:
+ - A blob is considered by default as ellipsoid. The found blob could
+ be rejected if the shape is not ellipsoid. To determine if the shape
+ is ellipsoid the algorithm consider an inner and outside ellipse.
+ Sampled points on these two ellipses should have the right gray levels.
+ Along the inner ellipse the sampled points should have gray levels
+ that are in the gray level minimum and maximum bounds, while
+ on the outside ellipse, the gray levels should be out of the gray level
+ bounds. To set the percentage of the sample points which should have the right
+ levels use setEllipsoidBadPointsPercentage(). The distance between the
+ inner ellpsoid and the blob contour, as well the distance between the
+ blob contour and the outside ellipse is fixed by setEllipsoidShapePrecision().
+ If you want to track a non ellipsoid shape, and turn off this validation test,
+ you have to call setEllipsoidShapePrecision(0).
+ - The width, height and surface of the blob are compared to the
+ corresponding values of the previous blob. If they differ to much
+ the blob could be rejected. To set the admissible distance you can
+ use setSizePrecision().
+
+ Note that track() and searchDotsInArea() are the most important features
+ of this class.
+
+ - track() estimate the current position of the dot using its previous
+ position, then try to compute the new parameters of the dot. If everything
+ went ok, tracking succeeds, otherwise we search this dot in a window
+ around the last position of the dot.
+
+ - searchDotsInArea() enable to find dots similar to this dot in a window. It
+ is used when there was a problem performing basic tracking of the dot, but
+ can also be used to find a certain type of dots in the full image.
+
+ The following sample code available in tutorial-blob-tracker-live-firewire.cpp shows how to
+ grab images from a firewire camera, track a blob and display the tracking
+ results.
+
+ \include tutorial-blob-tracker-live-firewire.cpp
+ A line by line explanation of the previous example is provided in
+ \ref tutorial-tracking-blob.
+
+ This other example available in tutorial-blob-auto-tracker.cpp shows firstly
+ how to detect in the first image all the blobs that match some characteristics
+ in terms of size, area, gray level. Secondly, it shows how to track all the
+ dots that are detected.
+
+ \include tutorial-blob-auto-tracker.cpp
+ A line by line explanation of this last example is also provided in
+ \ref tutorial-tracking-blob, section \ref tracking_blob_tracking.
+
+ \sa vpDot
+*/
+class VISP_EXPORT vpDot2 : public vpTracker
+{
+public:
+ vpDot2();
+ vpDot2(const vpImagePoint &ip) ;
+ vpDot2(const vpDot2& twinDot );
+ virtual ~vpDot2();
+
+ static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col = vpColor::blue, bool trackDot = true);
+
+ void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
+ unsigned int thickness=1) const;
+
+ double getArea() const;
+ /*!
+
+ Return the dot bounding box.
+
+ \sa getWidth(), getHeight()
+
+ */
+ inline vpRect getBBox() const {
+ vpRect bbox;
+
+ bbox.setRect(this->bbox_u_min,
+ this->bbox_v_min,
+ this->bbox_u_max - this->bbox_u_min + 1,
+ this->bbox_v_max - this->bbox_v_min + 1);
+
+ return (bbox);
+ };
+ /*!
+ Return the location of the dot center of gravity.
+
+ \return The coordinates of the center of gravity.
+ */
+ inline vpImagePoint getCog() const {
+ return cog;
+ }
+
+ double getDistance( const vpDot2& distantDot ) const;
+ /*!
+
+ Return the list of all the image points on the dot
+ border.
+
+ \param edges_list : The list of all the images points on the dot
+ border. This list is update after a call to track().
+
+ */
+ void getEdges(std::list<vpImagePoint> &edges_list) const {
+ edges_list = this->ip_edges_list;
+ };
+ /*!
+
+ Return the list of all the image points on the dot
+ border.
+
+ \return The list of all the images points on the dot
+ border. This list is update after a call to track().
+
+ */
+ std::list<vpImagePoint> getEdges() const {
+ return(this->ip_edges_list);
+ };
+ /*!
+ Get the percentage of sampled points that are considered non conform
+ in terms of the gray level on the inner and the ouside ellipses.
+
+ \sa setEllipsoidBadPointsPercentage()
+ */
+ double getEllipsoidBadPointsPercentage() const
+ {
+ return allowedBadPointsPercentage_;
+ }
+
+ double getEllipsoidShapePrecision() const;
+ void getFreemanChain(std::list<unsigned int> &freeman_chain) const;
+
+ inline double getGamma() const {return this->gamma;};
+ /*!
+ Return the color level of pixels inside the dot.
+
+ \sa getGrayLevelMax()
+ */
+ inline unsigned int getGrayLevelMin() const {
+ return gray_level_min;
+ };
+ /*!
+ Return the color level of pixels inside the dot.
+
+ \sa getGrayLevelMin()
+ */
+ inline unsigned int getGrayLevelMax() const {
+ return gray_level_max;
+ };
+ double getGrayLevelPrecision() const;
+
+ double getHeight() const;
+ double getMaxSizeSearchDistancePrecision() const;
+ /*!
+ \return The mean gray level value of the dot.
+ */
+ double getMeanGrayLevel() const {
+ return (this->mean_gray_level);
+ };
+ double getSizePrecision() const;
+ double getWidth() const;
+
+ void initTracking(const vpImage<unsigned char>& I, unsigned int size = 0);
+ void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
+ unsigned int size = 0);
+ void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
+ unsigned int gray_lvl_min, unsigned int gray_lvl_max,
+ unsigned int size = 0 );
+
+ vpDot2& operator=(const vpDot2& twinDot );
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d);
+
+ void print(std::ostream& os) { os << *this << std::endl ; }
+ void searchDotsInArea(const vpImage<unsigned char>& I,
+ int area_u, int area_v,
+ unsigned int area_w, unsigned int area_h, std::list<vpDot2> &niceDots );
+
+ void searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots );
+
+ void setArea( const double & area );
+ /*!
+ Initialize the dot coordinates with \e ip.
+ */
+ inline void setCog(const vpImagePoint &ip) {
+ this->cog = ip;
+ }
+ /*!
+
+ Activates the dot's moments computation.
+
+ \param activate true, if you want to compute the moments. If false, moments
+ are not computed.
+
+ Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
+ vpDot::m20, vpDot::m02.
+
+ The coordinates of the region's centroid (u, v) can be computed from the
+ moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$.
+
+ */
+ void setComputeMoments(const bool activate) { compute_moment = activate; }
+
+ /*!
+ Set the percentage of sampled points that are considered non conform
+ in terms of the gray level on the inner and the ouside ellipses.
+ Points located on the inner ellipse should have the same gray level
+ than the blob, while points located on the outside ellipse should
+ have a different gray level.
+
+ \param percentage : Percentage of points sampled with bad gray level
+ on the inner and outside ellipses that are admissible. 0 means
+ that all the points should have a right level, while a value of 1
+ means that all the points can have a bad gray level.
+ */
+ void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
+ {
+ if (percentage < 0.)
+ allowedBadPointsPercentage_ = 0.;
+ else if (percentage > 1.)
+ allowedBadPointsPercentage_ = 1.;
+ else
+ allowedBadPointsPercentage_ = percentage;
+ }
+
+ void setEllipsoidShapePrecision(const double & ellipsoidShapePrecision);
+ /*!
+ Activates the display of the border of the dot during the tracking.
+ The default thickness of the overlayed drawings can be modified using
+ setGraphicsThickness().
+
+ \warning To effectively display the dot graphics a call to
+ vpDisplay::flush() is needed.
+
+ \param activate If true, the border of the dot will be painted. false to
+ turn off border painting.
+
+ \sa setGraphicsThickness()
+ */
+ void setGraphics(const bool activate) { graphics = activate ; }
+ /*!
+ Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
+
+ \sa setGraphics()
+ */
+ void setGraphicsThickness(unsigned int t) {this->thickness = t;};
+ /*!
+
+ Set the color level of the dot to search a dot in a region of interest. This level will be
+ used to know if a pixel in the image belongs to the dot or not. Only pixels
+ with higher level can belong to the dot. If the level is lower than the
+ minimum level for a dot, set the level to MIN_IN_LEVEL.
+
+ \param min : Color level of a dot to search in a region of interest.
+
+ \sa setGrayLevelMax(), setGrayLevelPrecision()
+
+ */
+ inline void setGrayLevelMin( const unsigned int & min ) {
+ if (min > 255)
+ this->gray_level_min = 255;
+ else
+ this->gray_level_min = min;
+ };
+
+ /*!
+
+ Set the color level of pixels surrounding the dot. This is meant to be used
+ to search a dot in a region of interest.
+
+ \param max : Intensity level of a dot to search in a region of interest.
+
+ \sa setGrayLevelMin(), setGrayLevelPrecision()
+ */
+ inline void setGrayLevelMax( const unsigned int & max ) {
+ if (max > 255)
+ this->gray_level_max = 255;
+ else
+ this->gray_level_max = max;
+ };
+ void setGrayLevelPrecision( const double & grayLevelPrecision );
+ void setHeight( const double & height );
+ void setMaxSizeSearchDistancePrecision(const double & maxSizeSearchDistancePrecision);
+ void setSizePrecision( const double & sizePrecision );
+ void setWidth( const double & width );
+
+ void track(const vpImage<unsigned char> &I);
+ void track(const vpImage<unsigned char> &I, vpImagePoint &cog);
+
+ static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
+ std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar = NULL);
+
+public:
+ double m00; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
+ with \f$i = j = 0 \f$. This moment corresponds to the dot
+ surface.
+
+ \sa setComputeMoments()
+ */
+ double m10; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 0 \f$. \f$ m_{10} \f$
+ corresponds to the inertia first order moment along the v axis.
+
+ \sa setComputeMoments()
+ */
+ double m01; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 1 \f$. \f$ m_{01} \f$
+ corresponds to the inertia first order moment along the u axis.
+
+ \sa setComputeMoments()
+ */
+ double m11; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m20; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
+ obtained with \f$i = 2 \f$ and \f$ j = 0 \f$. \f$ m_{20} \f$
+ corresponds to the inertia second order moment along the v
+ axis.
+
+ \sa setComputeMoments()
+ */
+ double m02; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 2 \f$. \f$ m_{02} \f$
+ corresponds to the inertia second order moment along the u
+ axis.
+
+ \sa setComputeMoments()
+ */
+ double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
+ \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
+ \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
+ \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+
+private:
+ virtual bool isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot);
+
+ virtual bool hasGoodLevel(const vpImage<unsigned char>& I,
+ const unsigned int &u,
+ const unsigned int &v) const;
+ virtual bool hasReverseLevel(const vpImage<unsigned char>& I,
+ const unsigned int &u,
+ const unsigned int &v) const;
+
+ virtual vpDot2* getInstance();
+
+ void init();
+
+ bool computeParameters(const vpImage<unsigned char> &I,
+ const double &u = -1.0,
+ const double &v = -1.0);
+
+
+
+ bool findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u,
+ const unsigned int &v, unsigned int &border_u,
+ unsigned int &border_v);
+ void computeMeanGrayLevel(const vpImage<unsigned char>& I);
+
+ /*!
+
+ Get the starting point on a dot border. The dot border is
+ computed from this point.
+
+ \sa getFirstBorder_v()
+
+ */
+ unsigned int getFirstBorder_u() const {
+ return this->firstBorder_u;
+ }
+ /*!
+
+ Get the starting point on a dot border. The dot border is
+ computed from this point.
+
+ \sa getFirstBorder_u()
+
+ */
+ unsigned int getFirstBorder_v() const {
+ return this->firstBorder_v;
+ }
+
+ bool computeFreemanChainElement(const vpImage<unsigned char> &I,
+ const unsigned int &u,
+ const unsigned int &v,
+ unsigned int &element);
+ void computeFreemanParameters(const int &u_p,
+ const int &v_p, unsigned int &element,
+ int &du, int &dv, float &dS,
+ float &dMu, float &dMv,
+ float &dMuv,
+ float &dMu2, float &dMv2);
+ void updateFreemanPosition( unsigned int& u, unsigned int& v,
+ const unsigned int &dir );
+
+
+ bool isInImage(const vpImage<unsigned char> &I ) const;
+ bool isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const;
+
+ bool isInArea(const unsigned int &u, const unsigned int &v) const;
+
+ void getGridSize( unsigned int &gridWidth, unsigned int &gridHeight );
+ void setArea(const vpImage<unsigned char> &I,
+ int u, int v, unsigned int w, unsigned int h);
+ void setArea(const vpImage<unsigned char> &I);
+ void setArea(const vpRect & a);
+
+ unsigned char getMeanGrayLevel(vpImage<unsigned char>& I) const;
+ //! coordinates (float) of the point center of gravity
+ vpImagePoint cog;
+
+ double width;
+ double height;
+ double surface;
+ unsigned int gray_level_min; // minumum gray level for the dot.
+ // pixel with lower level don't belong
+ // to this dot.
+
+ unsigned int gray_level_max; // maximum gray level for the dot.
+ // pixel with higher level don't belong
+ // to this dot.
+ double mean_gray_level; // Mean gray level of the dot
+ double grayLevelPrecision ;
+ double gamma ;
+ double sizePrecision ;
+ double ellipsoidShapePrecision;
+ double maxSizeSearchDistancePrecision;
+ double allowedBadPointsPercentage_;
+ // Area where the dot is to search
+ vpRect area;
+
+ // other
+ std::list<unsigned int> direction_list;
+ std::list<vpImagePoint> ip_edges_list;
+
+ // flag
+ bool compute_moment ; // true moment are computed
+ bool graphics ; // true for graphic overlay display
+
+ unsigned int thickness; // Graphics thickness
+
+ // Bounding box
+ int bbox_u_min, bbox_u_max, bbox_v_min, bbox_v_max;
+
+ // The first point coodinate on the dot border
+ unsigned int firstBorder_u;
+ unsigned int firstBorder_v;
+
+//Static funtions
+public:
+ static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
+ unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
+ unsigned int thickness=1);
+
+};
+
+#endif
+
+
diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp
new file mode 100644
index 0000000..f974e3d
--- /dev/null
+++ b/modules/tracker/blob/src/dots/vpDot.cpp
@@ -0,0 +1,968 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Track a white dot.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*
+ \file vpDot.cpp
+ \brief Track a white dot
+*/
+
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpTrackingException.h>
+
+#include <vector>
+
+/*
+ \class vpDot
+ \brief Track a white dot
+*/
+
+/* spiral size for the dot search */
+const unsigned int vpDot::SPIRAL_SEARCH_SIZE = 350;
+
+/*!
+
+ Initialize the tracker with default parameters.
+ - connexityType is set to 4 (see setConnexityType())
+ - dot maximal size is set to 25% of the image size (see setMaxDotSize())
+*/
+void vpDot::init()
+{
+ cog.set_u(0);
+ cog.set_v(0);
+
+ compute_moment = false ;
+ graphics = false ;
+ thickness = 1;
+ maxDotSizePercentage = 0.25 ; // 25 % of the image size
+
+ mean_gray_level = 0;
+ gray_level_min = 128;
+ gray_level_max = 255;
+ grayLevelPrecision = 0.85;
+ gamma = 1.5 ;
+
+ m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu02 = mu20 = 0 ;
+
+ connexityType = CONNEXITY_4;
+
+ u_min = u_max = v_min = v_max = 0;
+
+ gray_level_out = 0;
+ nbMaxPoint = 0;
+}
+
+vpDot::vpDot()
+ : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
+ cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
+ gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
+ gamma(1.5), compute_moment(false), nbMaxPoint(0)
+{
+}
+
+/*!
+ \brief Constructor with initialization of the dot location.
+
+ \param ip : An image point with sub-pixel coordinates.
+ */
+vpDot::vpDot(const vpImagePoint &ip)
+ : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
+ cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
+ gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
+ gamma(1.5), compute_moment(false), nbMaxPoint(0)
+{
+ cog = ip;
+}
+
+/*!
+ \brief Copy constructor.
+ */
+vpDot::vpDot(const vpDot& d)
+ : vpTracker(d),
+ m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
+ cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
+ gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
+ gamma(1.5), compute_moment(false), nbMaxPoint(0)
+{
+ *this = d ;
+}
+
+/*!
+ \brief Destructor.
+ */
+vpDot::~vpDot()
+{
+ ip_connexities_list.clear() ;
+}
+
+/*!
+ \brief Copy operator.
+ */
+vpDot&
+vpDot::operator=(const vpDot& d)
+{
+ ip_edges_list = d.ip_edges_list;
+ ip_connexities_list = d.ip_connexities_list;
+ connexityType = d.connexityType;
+ cog = d.getCog();
+
+ u_min = d.u_min;
+ v_min = d.v_min;
+ u_max = d.u_max;
+ v_max = d.v_max;
+
+ graphics = d.graphics ;
+ thickness = d.thickness;
+ maxDotSizePercentage = d.maxDotSizePercentage;
+ gray_level_out = d.gray_level_out;
+ mean_gray_level = d.mean_gray_level ;
+ gray_level_min = d.gray_level_min ;
+ gray_level_max = d.gray_level_max ;
+ grayLevelPrecision = d.grayLevelPrecision;
+ gamma = d.gamma;
+ compute_moment = d.compute_moment ;
+ nbMaxPoint = d.nbMaxPoint;
+
+ m00 = d.m00;
+ m01 = d.m01;
+ m10 = d.m10;
+ m11 = d.m11;
+ m02 = d.m02;
+ m20 = d.m20;
+
+ mu11 = d.mu11;
+ mu20 = d.mu20;
+ mu02 = d.mu02;
+
+ return *this ;
+}
+
+bool
+vpDot::operator!=(const vpDot& d)
+{
+ return ( cog != d.getCog() );
+}
+
+bool
+vpDot::operator==(const vpDot& d)
+{
+ return ( cog == d.getCog() );
+}
+
+/*!
+
+ This method choose a gray level (default is 0) used to modify the
+ "in" dot level in "out" dot level. This gray level is here needed to
+ stop the recursivity . The pixels of the dot are set to this new
+ gray level "\out\" when connexe() is called.
+
+ \exception vpTrackingException::initializationError : No valid gray
+ level "out" can be found. This means that the min gray level is set to 0
+ and the max gray level to 255. This should not occur
+*/
+void
+vpDot::setGrayLevelOut()
+{
+ if (gray_level_min == 0) {
+ if (gray_level_max == 255) {
+ // gray_level_min = 0 and gray_level_max = 255: this should not occur
+ //vpERROR_TRACE("Unable to choose a good \"out\" level") ;
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Unable to choose a good \"out\" level")) ;
+ }
+ gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
+ }
+}
+
+/*!
+ Perform the tracking of a dot by connex components.
+
+ \param mean_value : Threshold to use for the next call to track()
+ and corresponding to the mean value of the dot intensity.
+
+ \warning The content of the image is modified thanks to
+ setGrayLevelOut() called before. This method choose a gray level
+ (default is 0) used to modify the "in" dot level in "out" dot
+ level. This gray level is here needed to stop the recursivity . The
+ pixels of the dot are set to this new gray level "\out\".
+
+ \return vpDot::out if an error occurs, vpDot::in otherwise.
+
+ \sa setGrayLevelOut()
+*/
+
+bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
+ double &mean_value, double &u_cog, double &v_cog, double &n)
+{
+ std::vector<bool> checkTab(I.getWidth()*I.getHeight(),false);
+ return connexe(I,u,v,mean_value,u_cog,v_cog,n,checkTab);
+}
+/*!
+ Perform the tracking of a dot by connex components.
+
+ \param mean_value : Threshold to use for the next call to track()
+ and corresponding to the mean value of the dot intensity.
+
+ \warning The content of the image is modified thanks to
+ setGrayLevelOut() called before. This method choose a gray level
+ (default is 0) used to modify the "in" dot level in "out" dot
+ level. This gray level is here needed to stop the recursivity . The
+ pixels of the dot are set to this new gray level "\out\".
+
+ \return vpDot::out if an error occurs, vpDot::in otherwise.
+
+ \sa setGrayLevelOut()
+*/
+
+bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
+ double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab)
+{
+
+ unsigned int width = I.getWidth();
+ unsigned int height= I.getHeight();
+
+ // Test if we are in the image
+ if ( (u >= width) || (v >= height) )
+ {
+ //std::cout << "out of bound" << std::endl;
+ return false;
+ }
+
+ if(checkTab[u + v*I.getWidth()])
+ return true;
+
+ vpImagePoint ip;
+ ip.set_u(u);
+ ip.set_v(v);
+
+ if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
+ {
+ checkTab[v*I.getWidth() + u] = true;
+
+ ip_connexities_list.push_back(ip);
+
+ u_cog += u ;
+ v_cog += v ;
+ 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() "
+ "method.",
+ n, n / (I.getWidth() * I.getHeight()),
+ nbMaxPoint, maxDotSizePercentage)) ;
+ }
+
+ // Bounding box update
+ if (u < this->u_min) this->u_min = u;
+ if (u > this->u_max) this->u_max = u;
+ if (v < this->v_min) this->v_min = v;
+ if (v > this->v_max) this->v_max = v;
+
+ // Mean value of the dot intensities
+ mean_value = (mean_value *(n-1) + I[v][u]) / n;
+ if (compute_moment==true)
+ {
+ m00++ ;
+ m10 += u ;
+ m01 += v ;
+ m11 += (u*v) ;
+ m20 += u*u ;
+ m02 += v*v ;
+ }
+ }
+ else
+ {
+ //std::cout << "not in" << std::endl;
+ return false;
+ }
+
+ bool edge = false;
+
+ //if((int)u-1 >= 0)
+ if(u >= 1)
+ if(!checkTab[u-1 + v*I.getWidth()])
+ if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
+ edge = true;
+
+ if(u+1 < I.getWidth())
+ if(!checkTab[u+1+v*I.getWidth()])
+ if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if(v >= 1)
+ if(!checkTab[u+(v-1)*I.getWidth()])
+ if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if(v+1 < I.getHeight())
+ if(!checkTab[u+(v+1)*I.getWidth()])
+ if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if (connexityType == CONNEXITY_8) {
+ if(v >= 1 && u >= 1)
+ if(!checkTab[u-1+(v-1)*I.getWidth()])
+ if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if(v >= 1 && u+1 < I.getWidth())
+ if(!checkTab[u+1+(v-1)*I.getWidth()])
+ if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if(v+1 < I.getHeight() && u >= 1)
+ if(!checkTab[u-1+(v+1)*I.getWidth()])
+ if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if(v+1 < I.getHeight() && u+1 < I.getWidth())
+ if(!checkTab[u+1+(v+1)*I.getWidth()])
+ if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
+ edge = true;
+ }
+
+ if(edge){
+ ip_edges_list.push_back(ip);
+ if (graphics==true)
+ {
+ vpImagePoint ip_(ip);
+ for(unsigned int t=0; t<thickness; t++) {
+ ip_.set_u(ip.get_u() + t);
+ vpDisplay::displayPoint(I, ip_, vpColor::red) ;
+ }
+ //vpDisplay::flush(I);
+ }
+ }
+
+ return true;
+}
+
+/*!
+
+ Compute the center of gravity (COG) of the dot using connex
+ components. We assume the origin pixel (u, v) is in the dot. If
+ not, the dot is seach around this origin using a spiral search.
+
+ \param I : Image to process.
+ \param u : Starting pixel coordinate along the columns from where the
+ dot is searched .
+
+ \param v : Starting pixel coordinate along the rows from where the
+ dot is searched .
+
+ \warning The content of the image is modified.
+
+ \exception vpTrackingException::featureLostError : If the tracking fails.
+
+ \sa connexe()
+*/
+void
+vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
+{
+ // Set the maximal number of points considering the maximal dot size
+ // image percentage
+ nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
+
+ // segmentation de l'image apres seuillage
+ // (etiquetage des composante connexe)
+ if (compute_moment)
+ m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;
+
+ double u_cog = 0 ;
+ double v_cog = 0 ;
+ double npoint = 0 ;
+ this->mean_gray_level = 0 ;
+
+ ip_connexities_list.clear() ;
+ ip_edges_list.clear();
+
+ // Initialise the boundig box
+ this->u_min = I.getWidth();
+ this->u_max = 0;
+ this->v_min = I.getHeight();
+ this->v_max = 0;
+
+#if 0
+ // Original version
+ if ( connexe(I, (unsigned int)u, (unsigned int)v,
+ gray_level_min, gray_level_max,
+ mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
+ {
+ bool sol = false ;
+ unsigned int pas ;
+ for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
+ {
+ for (int k=-1 ; k <=1 ; k++) if (sol==false)
+ for (int l=-1 ; l <=1 ; l++) if (sol==false)
+ {
+ u_cog = 0 ;
+ v_cog = 0 ;
+ ip_connexities_list.clear() ;
+
+ this->mean_gray_level = 0 ;
+ if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
+ gray_level_min, gray_level_max,
+ mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
+ {
+ sol = true ; u += k*pas ; v += l*pas ;
+ }
+ }
+ }
+ if (sol == false)
+ {
+ //vpERROR_TRACE("Dot has been lost") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError,
+ "Dot has been lost")) ;
+ }
+ }
+#else
+ // If the dot is not found, search around using a spiral
+ if ( !connexe(I,(unsigned int)u,(unsigned int)v, mean_gray_level, u_cog, v_cog, npoint) )
+ {
+ bool sol = false ;
+
+ unsigned int right = 1;
+ unsigned int botom = 1;
+ unsigned int left = 2;
+ unsigned int up = 2;
+ double u_ = u, v_ = v;
+ unsigned int k;
+
+ // Spiral search from the center to find the nearest dot
+ while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
+ for (k=1; k <= right; k++) if(sol==false) {
+ u_cog = 0 ;
+ v_cog = 0 ;
+ ip_connexities_list.clear() ;
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0 ;
+ if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
+ sol = true; u = u_+k; v = v_;
+ }
+ }
+ u_ += k;
+ right += 2;
+
+ for (k=1; k <= botom; k++) if (sol==false) {
+ u_cog = 0 ;
+ v_cog = 0 ;
+ ip_connexities_list.clear() ;
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0 ;
+
+ if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
+ sol = true; u = u_; v = v_+k;
+ }
+ }
+ v_ += k;
+ botom += 2;
+
+ for (k=1; k <= left; k++) if (sol==false) {
+ u_cog = 0 ;
+ v_cog = 0 ;
+ ip_connexities_list.clear() ;
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0 ;
+
+ if ( connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),mean_gray_level,u_cog, v_cog, npoint) ) {
+ sol = true ; u = u_-k; v = v_;
+ }
+ }
+ u_ -= k;
+ left += 2;
+
+ for (k=1; k <= up; k++) if(sol==false) {
+ u_cog = 0 ;
+ v_cog = 0 ;
+ ip_connexities_list.clear() ;
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0 ;
+
+ if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),mean_gray_level,u_cog, v_cog, npoint) ) {
+ sol = true ; u = u_; v = v_-k;
+ }
+ }
+ v_ -= k;
+ up += 2;
+ }
+
+ if (sol == false) {
+ //vpERROR_TRACE("Dot has been lost") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError,
+ "Dot has been lost")) ;
+ }
+ }
+
+#endif
+/*
+ vpImagePoint ip;
+ unsigned int i, j;
+ std::list<vpImagePoint>::iterator it;
+ for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it ++) {
+ ip = *it;
+ i = (unsigned int) ip.get_i();
+ j = (unsigned int) ip.get_j();
+ I[i][j] = 255 ;
+ }*/
+
+ u_cog = u_cog/npoint ;
+ v_cog = v_cog/npoint ;
+
+ u = u_cog ;
+ v = v_cog ;
+
+ // Initialize the threshold for the next call to track()
+ double Ip = pow((double)this->mean_gray_level/255,1/gamma);
+
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+
+ //vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
+ //vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
+
+ if (npoint < 5)
+ {
+ //vpERROR_TRACE("Dot to small") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError,
+ "Dot to small")) ;
+ }
+
+ 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 using the setMaxDotSize() method.",
+ npoint, npoint / (I.getWidth() * I.getHeight()),
+ nbMaxPoint, maxDotSizePercentage)) ;
+ }
+}
+
+/*!
+ Maximal size of the region to track in terms of image size percentage.
+
+ \param percentage : Image size percentage corresponding to the dot
+ maximal size. Values should be in ]0 : 1]. If 1, that means that the
+ dot to track could take up the whole image.
+
+ During the tracking, if the dot size if bigger than the maximal size
+ allowed an exception is throwed :
+ vpTrackingException::featureLostError.
+
+*/
+void
+vpDot::setMaxDotSize(double percentage)
+{
+ if (percentage <= 0.0 || percentage > 1.0) {
+ // print a warning. We keep the default percentage
+ vpTRACE("Max dot size percentage is requested to be set to %lf.",
+ "Value should be in ]0:1]. Value will be set to %lf.",
+ percentage, maxDotSizePercentage);
+ }
+ else {
+ maxDotSizePercentage = percentage;
+ }
+}
+
+/*!
+
+ Initialize the tracking with a mouse click and update the dot
+ characteristics (center of gravity, moments).
+
+ Wait a user click in a gray area in the image I. The clicked pixel
+ will be the starting point from which the dot will be tracked.
+
+ The threshold used to segment the dot is set with the grayLevelPrecision
+ parameter. See the formula in setGrayLevelPrecision() function.
+
+ The sub pixel coordinates of the dot are updated. To get the center
+ of gravity coordinates of the dot, use getCog(). To
+ compute the moments use setComputeMoments(true) before a call to
+ initTracking().
+
+ \warning The content of the image modified since we call track() to
+ compute the dot characteristics.
+
+ \param I : Image to process.
+
+ \sa track(), getCog()
+*/
+void
+vpDot::initTracking(const vpImage<unsigned char>& I)
+{
+ while (vpDisplay::getClick(I, cog) != true) ;
+
+ unsigned int i = (unsigned int)cog.get_i();
+ unsigned int j = (unsigned int)cog.get_j();
+
+ double Ip = pow((double)I[i][j]/255, 1/gamma);
+
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+
+ try {
+ track( I );
+ }
+ catch(vpException &e)
+ {
+ throw(e) ;
+ }
+}
+
+/*!
+
+ Initialize the tracking for a dot supposed to be located at (u,v) and
+ updates the dot characteristics (center of gravity, moments).
+
+ The threshold used to segment the dot is set to 80 percent of the
+ gray level of the pixel (u,v).
+
+ The sub pixel coordinates of the dot are updated. To get the center
+ of gravity coordinates of the dot, use getCog(). To
+ compute the moments use setComputeMoments(true) before a call to
+ initTracking().
+
+ \warning The content of the image modified since we call track() to
+ compute the dot characteristics.
+
+ \param I : Image to process.
+
+ \param ip : Location of the starting point from which the dot will be
+ tracked in the image.
+
+ \sa track()
+*/
+void
+vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip)
+{
+
+ cog = ip ;
+
+ unsigned int i = (unsigned int)cog.get_i();
+ unsigned int j = (unsigned int)cog.get_j();
+
+ double Ip = pow((double)I[i][j]/255, 1/gamma);
+
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+ try {
+ track( I );
+ }
+ catch(vpException &e)
+ {
+ throw(e) ;
+ }
+}
+
+/*!
+
+ Initialize the tracking for a dot supposed to be located at (u,v) and
+ updates the dot characteristics (center of gravity, moments).
+
+ The sub pixel coordinates of the dot are updated. To get the center
+ of gravity coordinates of the dot, use getCog(). To
+ compute the moments use setComputeMoments(true) before a call to
+ initTracking().
+
+ \warning The content of the image modified since we call track() to
+ compute the dot characteristics.
+
+ \param I : Image to process.
+
+ \param ip : Location of the starting point from which the dot will
+ be tracked in the image.
+
+ \param level_min : Minimum gray level threshold used to segment the dot;
+ value comprised between 0 and 255.
+
+ \param level_max : Maximum gray level threshold used to segment the
+ dot; value comprised between 0 and 255. \e gray_level_max should be
+ greater than \e gray_level_min.
+
+ \sa track(), getCog()
+*/
+void
+vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
+ unsigned int level_min, unsigned int level_max)
+{
+
+ cog = ip ;
+
+ this->gray_level_min = level_min;
+ this->gray_level_max = level_max;
+
+ try {
+ track( I );
+ }
+ catch(vpException &e)
+ {
+ throw(e) ;
+ }
+}
+
+
+/*!
+ Track and compute the dot characteristics.
+
+ To get the center of gravity coordinates of the dot, use
+ getCog(). To compute the moments use setComputeMoments(true) before
+ a call to initTracking().
+
+ \warning The image is modified (all the pixels that belong to the point
+ are set to white (ie to 255).
+
+ \param I : Image to process.
+
+ \sa getCog()
+*/
+void
+vpDot::track(const vpImage<unsigned char> &I)
+{
+ try{
+ setGrayLevelOut();
+ double u = this->cog.get_u();
+ double v = this->cog.get_v();
+
+ COG( I, u, v ) ;
+
+ this->cog.set_u( u );
+ this->cog.set_v( v );
+
+ if (compute_moment==true)
+ {
+ mu11 = m11 - u*m01;
+ mu02 = m02 - v*m01;
+ mu20 = m20 - u*m10;
+ }
+
+ if (graphics) {
+ // display a red cross at the center of gravity's location in the image.
+ vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
+ }
+
+ }
+ catch(vpException &e)
+ {
+ throw(e) ;
+ }
+}
+
+/*!
+
+ Track and updates the new dot coordinates
+
+ To compute the moments use setComputeMoments(true) before a call to
+ initTracking() or track().
+
+ \warning The image is modified (all the pixels that belong to the point
+ are set to white (ie to 255).
+
+ \param I : Image to process.
+
+ \param ip [out] : Sub pixel coordinate of the tracked dot center of gravity.
+*/
+void
+vpDot::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
+{
+ track( I ) ;
+
+ ip = this->cog;
+}
+
+/*!
+ Display in overlay the dot edges and center of gravity.
+
+ \param I : Image.
+ \param color : The color used for the display.
+ \param thick : Thickness of the displayed cross located at the dot cog.
+*/
+void vpDot::display(const vpImage<unsigned char>& I, vpColor color, unsigned int thick) const
+{
+ vpDisplay::displayCross(I, cog, 3*thickness+8, color, thick);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+/*!
+
+ Set the precision of the gray level of the dot.
+
+ \param precision : It is a double precision float which value is
+ in ]0,1]:
+ - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - Values lower or equal to 0 are brought back to an epsion>0
+ - Values higher than 1 are brought back to 1
+ If the initial gray level is I, the gray levels of the dot will be between :
+ \f$Imin=255*\big((\frac{I}{255})^{{\gamma}^{-1}}-(1-grayLevelPrecision)\big)^{\gamma}\f$
+ and
+ \f$Imax=255*\big((\frac{I}{255})^{{\gamma}^{-1}}+(1-grayLevelPrecision)\big)^{\gamma}\f$
+ with \f$\gamma=1.5\f$ .
+
+ \sa setWidth(), setHeight(), setGrayLevelMin(), setGrayLevelMax()
+*/
+void vpDot::setGrayLevelPrecision( const double & precision )
+{
+ double epsilon = 0.05;
+ if( grayLevelPrecision<epsilon )
+ {
+ this->grayLevelPrecision = epsilon;
+ }
+ else if( grayLevelPrecision>1 )
+ {
+ this->grayLevelPrecision = 1.0;
+ }
+ else
+ {
+ this->grayLevelPrecision = precision;
+ }
+}
+
+/*!
+
+ Display the dot center of gravity and its list of edges.
+
+ \param I : The image used as background.
+
+ \param cog : The center of gravity.
+
+ \param edges_list : The list of edges;
+
+ \param color : Color used to display the dot.
+
+ \param thickness : Thickness of the dot.
+*/
+void vpDot::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color,
+ unsigned int thickness)
+{
+ vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = edges_list.begin(); it != edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+/*!
+
+ Display the dot center of gravity and its list of edges.
+
+ \param I : The image used as background.
+
+ \param cog : The center of gravity.
+
+ \param edges_list : The list of edges;
+
+ \param color : Color used to display the dot.
+
+ \param thickness : Thickness of the dot.
+*/
+void vpDot::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color,
+ unsigned int thickness)
+{
+ vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = edges_list.begin(); it != edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+/*!
+ Writes the dot center of gravity coordinates in the frame (i,j) (For more details
+ about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
+ and returns a reference to the stream.
+*/
+VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d) {
+ return (os << "(" << d.getCog() << ")" ) ;
+} ;
+
+
diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp
new file mode 100644
index 0000000..012e2b3
--- /dev/null
+++ b/modules/tracker/blob/src/dots/vpDot2.cpp
@@ -0,0 +1,2626 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Track a white dot.
+ *
+ * Authors:
+ * Fabien Spindler
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpDot2.cpp
+ \brief Track a dot.
+*/
+
+//#define DEBUG
+
+
+#include <visp3/core/vpDisplay.h>
+
+// exception handling
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpIoTools.h>
+
+#include <visp3/blob/vpDot2.h>
+#include <math.h>
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+/******************************************************************************
+ *
+ * CONSTRUCTORS AND DESTRUCTORS
+ *
+ *****************************************************************************/
+/*!
+
+ Initialize the tracker with default parameters.
+
+*/
+void vpDot2::init()
+{
+ cog.set_u(0);
+ cog.set_v(0);
+
+ width = 0;
+ height = 0;
+ surface = 0;
+ mean_gray_level = 0;
+ gray_level_min = 128;
+ gray_level_max = 255;
+ grayLevelPrecision = 0.80;
+ gamma = 1.5 ;
+
+ sizePrecision = 0.65;
+ ellipsoidShapePrecision = 0.65;
+ maxSizeSearchDistancePrecision = 0.65;
+ setEllipsoidBadPointsPercentage();
+ m00 = m11 = m02 = m20 = m10 = m01 = 0.;
+ mu11 = mu02 = mu20 = 0.;
+
+ bbox_u_min = bbox_u_max = bbox_v_min = bbox_v_max = 0;
+
+ firstBorder_u = 0;
+ firstBorder_v = 0;
+
+ compute_moment = false ;
+ graphics = false;
+ thickness = 1;
+}
+
+/*!
+ Default constructor. Just do basic default initialization.
+*/
+vpDot2::vpDot2()
+ : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
+ gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
+ sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
+ allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
+ graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
+ firstBorder_u(0), firstBorder_v()
+{
+}
+
+/*!
+
+ Constructor initialize the coordinates of the gravity center of the dot to
+ the image point \e ip. Rest is the same as the default constructor.
+
+ \param ip : An image point with sub-pixel coordinates.
+
+*/
+vpDot2::vpDot2(const vpImagePoint &ip)
+ : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
+ gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
+ sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
+ allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
+ graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
+ firstBorder_u(0), firstBorder_v()
+{
+ cog = ip;
+}
+
+/*!
+ Copy constructor.
+*/
+vpDot2::vpDot2(const vpDot2& twinDot )
+ : vpTracker(twinDot),
+ m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
+ mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
+ gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
+ sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
+ allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
+ graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
+ firstBorder_u(0), firstBorder_v()
+{
+ *this = twinDot;
+}
+
+/*!
+ = operator.
+*/
+vpDot2& vpDot2::operator=(const vpDot2& twinDot )
+{
+ cog = twinDot.cog;
+
+ width = twinDot.width;
+ height = twinDot.height;
+ surface = twinDot.surface;
+ gray_level_min = twinDot.gray_level_min;
+ gray_level_max = twinDot.gray_level_max;
+ mean_gray_level = twinDot.mean_gray_level;
+ grayLevelPrecision = twinDot.grayLevelPrecision;
+ gamma = twinDot.gamma; ;
+ sizePrecision = twinDot.sizePrecision;
+ ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision ;
+ maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
+ allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
+ area = twinDot.area;
+
+ direction_list = twinDot.direction_list;
+ ip_edges_list = twinDot.ip_edges_list;
+
+ compute_moment = twinDot.compute_moment;
+ graphics = twinDot.graphics;
+ thickness = twinDot.thickness;
+
+ bbox_u_min = twinDot.bbox_u_min;
+ bbox_u_max = twinDot.bbox_u_max;
+ bbox_v_min = twinDot.bbox_v_min;
+ bbox_v_max = twinDot.bbox_v_max;
+
+ firstBorder_u = twinDot.firstBorder_u;
+ firstBorder_v = twinDot.firstBorder_v;
+
+ m00 = twinDot.m00;
+ m01 = twinDot.m01;
+ m11 = twinDot.m11;
+ m10 = twinDot.m10;
+ m02 = twinDot.m02;
+ m20 = twinDot.m20;
+
+ mu11 = twinDot.mu11;
+ mu20 = twinDot.mu20;
+ mu02 = twinDot.mu02;
+
+ return (*this);
+}
+
+/*!
+ Destructor... do nothing for the moment.
+*/
+vpDot2::~vpDot2(){}
+
+
+/******************************************************************************
+ *
+ * PUBLIC METHODS
+ *****************************************************************************/
+
+/*!
+ Display in overlay the dot edges and center of gravity.
+
+ \param I : Image.
+ \param color : The color used for the display.
+ \param t : Thickness of the displayed cross located at the dot cog.
+*/
+void vpDot2::display(const vpImage<unsigned char>& I, vpColor color, unsigned int t) const
+{
+ vpDisplay::displayCross(I, cog, 3*t+8, color, t);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+
+/*!
+
+ Initialize the tracking with a mouse click on the image and update the dot
+ characteristics (center of gravity, moments) by a call to track().
+
+ Wait a user click in a white area in the image. The clicked pixel
+ will be the starting point from which the dot will be tracked.
+
+ To get center of gravity of the dot, see getCog(). To compute the
+ moments see setComputeMoments(). To get the width or height of the
+ dot, call getWidth() and getHeight(). The area of the dot is
+ given by getArea().
+
+ \param I : Image.
+ \param size : Size of the dot to track.
+
+ If no valid dot was found in the window, return an exception.
+
+ \exception vpTrackingException::featureLostError : If the dot initialization
+ failed. The initialization can fail if the following characteristics are
+ not valid;
+ - The gray level is between gray level min and gray level max.
+
+ - The shape should be ellipsoid if
+ setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
+ This is the default case. To track a non ellipsoid shape use
+ setEllipsoidShapePrecision(0).
+
+ \sa track()
+
+*/
+void vpDot2::initTracking(const vpImage<unsigned char>& I, unsigned int size)
+{
+ while ( vpDisplay::getClick(I, cog) != true) ;
+
+ unsigned int i = (unsigned int)cog.get_i();
+ unsigned int j = (unsigned int)cog.get_j();
+
+ double Ip = pow((double)I[i][j]/255,1/gamma);
+
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+
+ setWidth(size);
+ setHeight(size);
+
+ try {
+ track( I );
+ }
+ catch(vpException e)
+ {
+ //vpERROR_TRACE("Error caught") ;
+ throw(e) ;
+ }
+}
+
+/*!
+
+ Initialize the tracking for a dot supposed to be located at (u,v) and update
+ the dot characteristics (center of gravity, moments) by a call to track().
+
+ \param I : Image to process.
+
+ \param ip : Location of the starting point from which the dot will be
+ tracked in the image.
+
+ \param size : Size of the dot to track.
+
+ To get center of gravity of the dot, see getCog(). To compute the
+ moments see setComputeMoments().
+
+ If no valid dot was found in the window, return an exception.
+
+ \exception vpTrackingException::featureLostError : If the dot initialization
+ failed. The initialization can fail if the following characteristics are
+ not valid;
+ - The gray level is between gray level min and gray level max.
+
+ - The shape should be ellipsoid if
+ setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
+ This is the default case. To track a non ellipsoid shape use
+ setEllipsoidShapePrecision(0).
+*/
+void vpDot2::initTracking(const vpImage<unsigned char>& I,
+ const vpImagePoint &ip, unsigned int size)
+{
+ cog = ip ;
+
+ unsigned int i = (unsigned int)cog.get_i();
+ unsigned int j = (unsigned int)cog.get_j();
+
+ double Ip = pow((double)I[i][j]/255,1/gamma);
+
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+
+ setWidth(size);
+ setHeight(size);
+
+ try {
+ track( I );
+ }
+ catch(vpException &e)
+ {
+ //vpERROR_TRACE("Error caught") ;
+ throw(e) ;
+ }
+}
+
+/*!
+
+ Initialize the tracking for a dot supposed to be located at (u,v) and update
+ the dot characteristics (center of gravity, moments) by a call to track().
+
+ The sub pixel coordinates of the dot are updated. To get the center
+ of gravity coordinates of the dot, use getCog(). To
+ compute the moments use setComputeMoments(true) before a call to
+ initTracking().
+
+ \param I : Image to process.
+
+ \param ip : Location of the starting point from which the dot will
+ be tracked in the image.
+
+ \param gray_lvl_min : Minimum gray level threshold used to segment the dot;
+ value comprised between 0 and 255.
+
+ \param gray_lvl_max : Maximum gray level threshold used to segment the
+ dot; value comprised between 0 and 255. \e gray_level_max should be
+ greater than \e gray_level_min.
+
+ \param size : Size of the dot to track.
+
+ If no valid dot was found in the window, return an exception.
+
+ \exception vpTrackingException::featureLostError : If the dot initialization
+ failed. The initialization can fail if the following characteristics are
+ not valid;
+ - The gray level is between gray level min and gray level max.
+
+ - The shape should be ellipsoid if
+ setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
+ This is the default case. To track a non ellipsoid shape use
+ setEllipsoidShapePrecision(0).
+
+ \sa track(), getCog()
+
+*/
+void vpDot2::initTracking(const vpImage<unsigned char>& I,
+ const vpImagePoint &ip,
+ unsigned int gray_lvl_min,
+ unsigned int gray_lvl_max,
+ unsigned int size)
+{
+ cog = ip ;
+
+ this->gray_level_min = gray_lvl_min;
+ this->gray_level_max = gray_lvl_max;
+
+ setWidth(size);
+ setHeight(size);
+
+ try {
+ track( I );
+ }
+ catch(vpException &e)
+ {
+ //vpERROR_TRACE("Error caught") ;
+ throw(e) ;
+ }
+}
+
+
+
+/*!
+
+ Try to locate the dot in the image:
+
+ - First, estimate the new position of the dot, using its previous position.
+ - Then compute the center of gravity (surface, width height) of the
+ tracked entity from the Freeman chain elements.
+ - If the dot is lost (estimated point too dark, too much surface change,...),
+ search the dot in a window around the previous position.
+ - If no valid dot was found in the window, return an exception.
+
+ \param I : Image.
+
+ \exception vpTrackingException::featureLostError : If the dot tracking
+ failed. The tracking can fail if the following characteristics are not
+ valid;
+ - The gray level is between gray level min and gray level max.
+
+ - The size (width or height) and the surface (in terms of number of
+ pixels) should not differ to much with the previous dot.
+
+ - The shape should be ellipsoid if
+ setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
+ This is the default case. To track a non ellipsoid shape use
+ setEllipsoidShapePrecision(0).
+
+ To get the center of gravity of the dot, call getCog(). To get the
+ width or height of the dot, call getWidth() and getHeight(). The area of the
+ dot is given by getArea().
+
+ To compute all the inertia moments associated to the dot see
+ setComputeMoments().
+
+ To get the pixels coordinates on the dot boundary, see getList_u() and
+ getList_v().
+
+
+*/
+void vpDot2::track(const vpImage<unsigned char> &I)
+{
+ m00 = m11 = m02 = m20 = m10 = m01 = 0 ;
+
+ // First, we will estimate the position of the tracked point
+
+ // Set the search area to the entire image
+ setArea(I);
+
+ // create a copy of the dot to search
+ // This copy can be saw as the previous dot used to check if the current one
+ // found with computeParameters() is similar to the previous one (see isValid()
+ // function).
+ // If the found dot is not similar (or valid), we use this copy to set the current
+ // found dot to the previous one (see below).
+ vpDot2 wantedDot(*this);
+
+ // vpDEBUG_TRACE(0, "Previous dot: ");
+ // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
+ // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
+ bool found = false;
+ found = computeParameters(I, cog.get_u(), cog.get_v());
+
+ if (found) {
+ // test if the found dot is valid (ie similar to the previous one)
+ found = isValid( I, wantedDot);
+ if (! found) {
+ *this = wantedDot;
+ //std::cout << "The found dot is not valid" << std::endl;
+ }
+ }
+
+ if (! found) {
+ // vpDEBUG_TRACE(0, "Search the dot in a biggest window around the last position");
+ // vpDEBUG_TRACE(0, "Bad computed dot: ");
+ // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
+ // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
+
+ // if estimation was wrong (get an error tracking), look for the dot
+ // closest from the estimation,
+ // i.e. search for dots in an a region of interest around the this dot and get the first
+ // element in the area.
+
+ // first get the size of the search window from the dot size
+ double searchWindowWidth, searchWindowHeight;
+ //if( getWidth() == 0 || getHeight() == 0 )
+ if( std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon() || std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon() )
+ {
+ searchWindowWidth = 80.;
+ searchWindowHeight = 80.;
+ }
+ else
+ {
+ searchWindowWidth = getWidth() * 5;
+ searchWindowHeight = getHeight() * 5;
+ }
+ std::list<vpDot2> candidates;
+ searchDotsInArea( I,
+ (int)(this->cog.get_u()-searchWindowWidth /2.0),
+ (int)(this->cog.get_v()-searchWindowHeight/2.0),
+ (unsigned int)searchWindowWidth,
+ (unsigned int)searchWindowHeight,
+ candidates);
+
+ // if the vector is empty, that mean we didn't find any candidate
+ // in the area, return an error tracking.
+ if( candidates.empty() )
+ {
+ //vpERROR_TRACE("No dot was found") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError,
+ "No dot was found")) ;
+ }
+
+ // otherwise we've got our dot, update this dot's parameters
+ vpDot2 movingDot = candidates.front();
+
+ setCog( movingDot.getCog() );
+ setArea( movingDot.getArea() );
+ setWidth( movingDot.getWidth() );
+ setHeight( movingDot.getHeight() );
+
+ // Update the moments
+ m00 = movingDot.m00;
+ m01 = movingDot.m01;
+ m10 = movingDot.m10;
+ m11 = movingDot.m11;
+ m20 = movingDot.m20;
+ m02 = movingDot.m02;
+
+ // Update the bounding box
+ bbox_u_min = movingDot.bbox_u_min;
+ bbox_u_max = movingDot.bbox_u_max;
+ bbox_v_min = movingDot.bbox_v_min;
+ bbox_v_max = movingDot.bbox_v_max;
+ }
+ // 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 ) )
+ {
+ //vpERROR_TRACE("The center of gravity of the dot is not in the image") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError,
+ "The center of gravity of the dot is not in the image")) ;
+ }
+
+ // Get dots center of gravity
+ // unsigned int u = (unsigned int) this->cog.get_u();
+ // unsigned int v = (unsigned int) this->cog.get_v();
+ // Updates the min and max gray levels for the next iteration
+ // double Ip = pow((double)I[v][u]/255,1/gamma);
+ double Ip = pow(getMeanGrayLevel()/255,1/gamma);
+ //printf("current value of gray level center : %i\n", I[v][u]);
+
+ //getMeanGrayLevel(I);
+ if(Ip - (1 - grayLevelPrecision)<0){
+ gray_level_min = 0 ;
+ }
+ else{
+ gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (gray_level_min > 255)
+ gray_level_min = 255;
+ }
+ gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ if (gray_level_max > 255)
+ gray_level_max = 255;
+
+ //printf("%i %i \n",gray_level_max,gray_level_min);
+ if (graphics) {
+ // display a red cross at the center of gravity's location in the image.
+
+ vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
+ //vpDisplay::flush(I);
+ }
+}
+
+/*!
+
+ Track and get the new dot coordinates. See track() for a more complete
+ description
+
+ \param I : Image to process.
+
+ \param ip [out] : Sub pixel coordinate of the tracked dot center of gravity.
+
+ The behavior of this method is similar to the following code:
+ \code
+ vpDot2 d;
+ d.track(I);
+ vpImagePoint cog = d.getCog();
+ \endcode
+
+ \sa track()
+*/
+void
+vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
+{
+ track(I);
+
+ ip = this->cog;
+}
+
+///// GET METHODS /////////////////////////////////////////////////////////////
+
+/*!
+ Return the width of the dot.
+
+ \sa getHeight()
+*/
+double vpDot2::getWidth() const
+{
+ return width;
+}
+
+/*!
+ Return the height of the dot.
+
+ \sa getWidth()
+*/
+double vpDot2::getHeight() const
+{
+ return height;
+}
+
+/*!
+ Return the area of the dot.
+
+ The area of the dot is also given by \f$|m00|\f$.
+*/
+double vpDot2::getArea() const
+{
+ return fabs(surface);
+}
+
+/*!
+ Return the precision of the gray level of the dot. It is a double
+ precision float witch value is in ]0,1]. 1 means full precision, whereas
+ values close to 0 show a very bad precision.
+*/
+double vpDot2::getGrayLevelPrecision() const
+{
+ return grayLevelPrecision;
+}
+
+/*!
+ Return the precision of the size of the dot. It is a double
+ precision float witch value is in ]0,1]. 1 means full precision, whereas
+ values close to 0 show a very bad precision.
+*/
+double vpDot2::getSizePrecision() const
+{
+ return sizePrecision;
+}
+
+/*!
+ Return the precision of the ellipsoid shape of the dot. It is a double
+ precision float witch value is in [0,1]. 1 means full precision, whereas
+ values close to 0 show a very bad precision.
+
+ \sa setEllipsoidShapePrecision()
+*/
+double vpDot2::getEllipsoidShapePrecision() const
+{
+ return ellipsoidShapePrecision;
+}
+
+/*!
+ Return the precision of the search maximum distance to get the starting point on a dot border. It is a double
+ precision float witch value is in [0.05,1]. 1 means full precision, whereas
+ values close to 0 show a very bad precision.
+*/
+double vpDot2::getMaxSizeSearchDistancePrecision() const{
+ return maxSizeSearchDistancePrecision;
+}
+
+/*!
+ Return the distance between the two center of dots.
+*/
+double vpDot2::getDistance( const vpDot2& distantDot ) const
+{
+ vpImagePoint cogDistantDot = distantDot.getCog();
+ double diff_u = this->cog.get_u() - cogDistantDot.get_u();
+ double diff_v = this->cog.get_v() - cogDistantDot.get_v();
+ return sqrt( diff_u*diff_u + diff_v*diff_v );
+}
+
+
+///// SET METHODS ////////////////////////////////////////////////////////////
+
+
+/*!
+
+ Set the width of the dot. This is meant to be used to search a dot in an
+ area.
+
+ \param w : Width of a dot to search in a region of interest.
+
+ \sa setHeight(), setArea(), setSizePrecision()
+*/
+void vpDot2::setWidth( const double & w )
+{
+ this->width = w;
+}
+
+/*!
+
+ Set the height of the dot. This is meant to be used to search a dot in an
+ area.
+
+ \param h : Height of a dot to search in a region of interest.
+
+ \sa setWidth(), setArea(), setSizePrecision()
+
+*/
+void vpDot2::setHeight( const double & h )
+{
+ this->height = h;
+}
+
+/*!
+
+ Set the area of the dot. This is meant to be used to search a dot in a region of interest.
+
+ \param a : Area of a dot to search in a region of interest.
+
+ \sa setWidth(), setHeight(), setSizePrecision()
+
+*/
+void vpDot2::setArea( const double & a )
+{
+ this->surface = a;
+}
+
+/*!
+
+ Set the precision of the gray level of the dot.
+
+ \param precision : It is a double precision float which value is in ]0,1]:
+ - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - Values lower or equal to 0 are brought back to an epsilon>0
+ - Values higher than 1 are brought back to 1
+ If the initial gray level is I, the gray levels of the dot will be between :
+ \f$Imin=255*\big((\frac{I}{255})^{{\gamma}^{-1}}-(1-grayLevelPrecision)\big)^{\gamma}\f$
+ and
+ \f$Imax=255*\big((\frac{I}{255})^{{\gamma}^{-1}}+(1-grayLevelPrecision)\big)^{\gamma}\f$
+ with \f$\gamma=1.5\f$ .
+
+ \sa setGrayLevelMin(), setGrayLevelMax()
+*/
+void vpDot2::setGrayLevelPrecision( const double & precision )
+{
+ double epsilon = 0.05;
+ if( grayLevelPrecision<epsilon )
+ {
+ this->grayLevelPrecision = epsilon;
+ }
+ else if( grayLevelPrecision>1 )
+ {
+ this->grayLevelPrecision = 1.0;
+ }
+ else
+ {
+ this->grayLevelPrecision = precision;
+ }
+}
+/*!
+
+ Set the precision of the size of the dot. Used to test the validity of the dot
+
+ \param precision : It is a double precision float which value is in [0,1]:
+ - this is the limit ratio between the tested parameter and the measured one.
+ minSize = sizePrecision*originalSize ; maxSize = originalSize/sizePrecision ;
+ - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - Values lower or equal to 0 are brought back to 0.
+ - Values higher than 1 are brought back to 1.
+ - To desactivate validity test set sizePrecision to 0
+
+ \sa setWidth(), setHeight(), setArea()
+*/
+void vpDot2::setSizePrecision( const double & precision )
+{
+ if( sizePrecision<0 )
+ {
+ this->sizePrecision = 0;
+ }
+ else if( sizePrecision>1 )
+ {
+ this->sizePrecision = 1.0;
+ }
+ else
+ {
+ this->sizePrecision = precision;
+ }
+}
+
+/*!
+ Indicates if the dot should have an ellipsoid shape to be valid.
+ \param precision : It is a double precision float which value is in [0,1]:
+
+ - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - Values lower or equal to 0 are brought back to 0.
+ - Values higher than 1 are brought back to 1.
+ To track a non ellipsoid shape use setEllipsoidShapePrecision(0).
+
+ The following example show how to track a blob with a height constraint on an ellipsoid shape.
+ The tracking will fail if the shape is not ellipsoid.
+ \code
+ vpDot2 dot;
+ dot.setEllipsoidShapePrecision(0.9); // to track a blob with a height constraint attendee on a circle shape
+ ...
+ dot.track();
+ \endcode
+
+ This other example shows how to remove any constraint on the shape. Here the tracker will be able to track
+ any shape, including square or rectangular shapes.
+ \code
+ vpDot2 dot;
+ dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
+ ...
+ dot.track();
+ \endcode
+
+ \sa getEllipsoidShapePrecision()
+*/
+void vpDot2::setEllipsoidShapePrecision(const double & precision) {
+
+ if( ellipsoidShapePrecision<0 )
+ {
+ this->ellipsoidShapePrecision = 0;
+ }
+ else if( ellipsoidShapePrecision>1 )
+ {
+ this->ellipsoidShapePrecision = 1.0;
+ }
+ else
+ {
+ this->ellipsoidShapePrecision = precision;
+ }
+}
+
+/*!
+
+ Set the precision of the search maximum distance to get the starting point on a dot border. A too low value
+ mean a large search area.
+
+ \param precision : It is a double precision float which value is in [0.05,1]:
+ - this is the limit ratio between the tested parameter and the measured one.
+ distance < getWidth()/(getSizePrecision()+epsilon);
+ - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - Values lower or equal to 0.05 are brought back to 0.05
+ - Values higher than 1 are brought back to 1.
+
+*/
+void vpDot2::setMaxSizeSearchDistancePrecision( const double & precision )
+{
+ double epsilon = 0.05;
+ if( maxSizeSearchDistancePrecision<epsilon )
+ {
+ this-> maxSizeSearchDistancePrecision = epsilon;
+ }
+ else if( maxSizeSearchDistancePrecision >1 )
+ {
+ this->maxSizeSearchDistancePrecision = 1.0;
+ }
+ else
+ {
+ this->maxSizeSearchDistancePrecision = precision;
+ }
+}
+
+/*!
+
+ Set the parameters of the area in which a dot is search to the image
+ dimension.
+
+ \param I : Image.
+
+*/
+void
+vpDot2::setArea(const vpImage<unsigned char> &I)
+{
+ setArea(I, 0, 0, I.getWidth(), I.getHeight());
+}
+
+/*!
+
+ Set the parameters of an area by setting the upper-left corner coordinates
+ (u, v), width and height.
+
+ \param I : The image we are working with.
+ \param u : Area horizontal left coordinate.
+ \param v : Area vertical top coordinate.
+ \param w : Area width.
+ \param h : Area height.
+
+*/
+void
+vpDot2::setArea(const vpImage<unsigned char> &I,
+ int u, int v,
+ unsigned int w, unsigned int h)
+{
+ unsigned int image_w = I.getWidth();
+ unsigned int image_h = I.getHeight();
+
+ // Bounds the area to the image
+ if (u < 0) u = 0;
+ else if (u >= (int)image_w) u = (int)image_w - 1;
+ if (v < 0) v = 0;
+ else if (v >= (int)image_h) v = (int)image_h - 1;
+
+ if (((unsigned int)u + w) > image_w) w = image_w - (unsigned int)u - 1;
+ if (((unsigned int)v + h) > image_h) h = image_h - (unsigned int)v - 1;
+
+ area.setRect(u, v, w, h);
+}
+
+/*!
+
+ Set the parameters of the area.
+
+ \param a : Area.
+
+*/
+void
+ vpDot2::setArea(const vpRect & a)
+{
+ area = a;
+}
+
+///// CLASS FUNCTIONALITY ////////////////////////////////////////////////////
+
+/*!
+
+ Look for a list of dot matching this dot parameters within the entire
+ image.
+
+ \warning Allocates memory for the list of dots returned by this method.
+ Desallocation has to be done by yourself.
+
+ \param I : Image.
+
+ \param niceDots: List of the dots that are found.
+
+ Before calling this method, dot characteristics to found have to be set like:
+
+ \code
+ vpDot2 d;
+
+ // Set dot characteristics for the auto detection
+ d.setWidth(24);
+ d.setHeight(23);
+ d.setArea(412);
+ d.setGrayLevelMin(160);
+ d.setGrayLevelMax(255);
+ d.setGrayLevelPrecision(0.8);
+ d.setSizePrecision(0.65);
+ d.setEllipsoidShapePrecision(0.65);
+ \endcode
+
+ To search dots in the whole image:
+ \code
+ std::list<vpDot2> list_d;
+ d.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), list_d) ;
+ \endcode
+
+ The number of dots found in the area is given by:
+ \code
+ std::cout << list_d.size();
+ \endcode
+
+ To parse all the dots:
+ \code
+ std::list<vpDot2>::iterator it;
+ for (it = list_d.begin(); it != list_d.end(); ++it) {
+ vpDot2 tmp_d = *it;
+ }
+ \endcode
+
+ \sa searchDotsInArea(vpImage<unsigned char>&, int, int, unsigned int, unsigned int, std::list<vpDot2> &)
+*/
+void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots)
+{
+ searchDotsInArea( I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
+}
+
+/*!
+
+ Look for a list of dot matching this dot parameters within a region of interest
+ defined by a rectangle in the image. The rectangle upper-left coordinates are given by
+ (\e area_u, \e area_v). The size of the rectangle is given by \e area_w and
+ \e area_h.
+
+ \param I : Image to process.
+ \param area_u : Coordinate (column) of the upper-left area corner.
+ \param area_v : Coordinate (row) of the upper-left area corner.
+
+ \param area_w : Width or the area in which a dot is searched.
+ \param area_h : Height or the area in which a dot is searched.
+
+ \param niceDots: List of the dots that are found.
+
+ \warning Allocates memory for the list of vpDot2 returned by this method.
+ Desallocation has to be done by yourself, see searchDotsInArea()
+
+ \sa searchDotsInArea(vpImage<unsigned char>& I, std::list<vpDot2> &)
+*/
+void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
+ int area_u,
+ int area_v,
+ unsigned int area_w,
+ unsigned int area_h,
+ std::list<vpDot2> &niceDots)
+
+{
+ // clear the list of nice dots
+ niceDots.clear();
+
+ // Fit the input area in the image; we keep only the common part between this
+ // area and the image.
+ setArea(I, area_u, area_v, area_w, area_h);
+
+ // compute the size of the search grid
+ unsigned int gridWidth;
+ unsigned int gridHeight;
+ getGridSize( gridWidth, gridHeight );
+
+ if (graphics) {
+ // Display the area were the dot is search
+ vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness);
+ //vpDisplay::flush(I);
+ }
+
+#ifdef DEBUG
+ vpDisplay::displayRectangle(I, area, vpColor::blue);
+ vpDisplay::flush(I);
+#endif
+ // start the search loop; for all points of the search grid,
+ // test if the pixel belongs to a valid dot.
+ // if it is so eventually add it to the vector of valid dots.
+ std::list<vpDot2> badDotsVector;
+ std::list<vpDot2>::iterator itnice;
+ std::list<vpDot2>::iterator itbad;
+
+ vpDot2* dotToTest = NULL;
+ vpDot2 tmpDot;
+
+ unsigned int area_u_min = (unsigned int) area.getLeft();
+ unsigned int area_u_max = (unsigned int) area.getRight();
+ unsigned int area_v_min = (unsigned int) area.getTop();
+ unsigned int area_v_max = (unsigned int) area.getBottom();
+
+ unsigned int u, v;
+ vpImagePoint cogTmpDot;
+
+ for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
+ {
+ for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
+ {
+ // if the pixel we're in doesn't have the right color (outside the
+ // graylevel interval), no need to check further, just get to the
+ // next grid intersection.
+ if( !hasGoodLevel(I, u, v) ) continue;
+
+ // Test if an other germ is inside the bounding box of a dot previously
+ // detected
+ bool good_germ = true;
+
+ itnice = niceDots.begin();
+ while( itnice != niceDots.end() && good_germ == true) {
+ tmpDot = *itnice;
+
+ cogTmpDot = tmpDot.getCog();
+ double u0 = cogTmpDot.get_u();
+ double v0 = cogTmpDot.get_v();
+ double half_w = tmpDot.getWidth() / 2.;
+ double half_h = tmpDot.getHeight() / 2.;
+
+ if ( u >= (u0-half_w) && u <= (u0+half_w) &&
+ v >= (v0-half_h) && v <= (v0+half_h) ) {
+ // Germ is in a previously detected dot
+ good_germ = false;
+ }
+ ++ itnice;
+ }
+
+ if (! good_germ)
+ continue;
+
+ // Compute the right border position for this possible germ
+ unsigned int border_u;
+ unsigned int border_v;
+ if(findFirstBorder(I, u, v, border_u, border_v) == false){
+ // germ is not good.
+ // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ u = border_u;
+ v = border_v;
+ continue;
+ }
+
+ itbad = badDotsVector.begin();
+#define vpBAD_DOT_VALUE (*itbad)
+ vpImagePoint cogBadDot;
+
+ while( itbad != badDotsVector.end() && good_germ == true) {
+ if( (double)u >= vpBAD_DOT_VALUE.bbox_u_min
+ && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
+ (double)v >= vpBAD_DOT_VALUE.bbox_v_min
+ && (double)v <= vpBAD_DOT_VALUE.bbox_v_max){
+ std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
+ while (it_edges != ip_edges_list.end() && good_germ == true){
+ // Test if the germ belong to a previously detected dot:
+ // - from the germ go right to the border and compare this
+ // position to the list of pixels of previously detected dots
+ cogBadDot = *it_edges;
+ //if( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) {
+ if( (std::fabs(border_u - cogBadDot.get_u()) <= vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u()))*std::numeric_limits<double>::epsilon() )
+ &&
+ (std::fabs(v - cogBadDot.get_v()) <= vpMath::maximum(std::fabs((double)v), std::fabs(cogBadDot.get_v()))*std::numeric_limits<double>::epsilon() )) {
+ good_germ = false;
+ }
+ ++ it_edges;
+ }
+ }
+ ++itbad;
+ }
+#undef vpBAD_DOT_VALUE
+
+ if (! good_germ) {
+ // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ u = border_u;
+ v = border_v;
+ continue;
+ }
+
+ vpTRACE(4, "Try germ (%d, %d)", u, v);
+
+ vpImagePoint germ;
+ germ.set_u( u );
+ germ.set_v( v );
+
+ // otherwise estimate the width, height and surface of the dot we
+ // created, and test it.
+ if( dotToTest != NULL ) delete dotToTest;
+ dotToTest = getInstance();
+ dotToTest->setCog( germ );
+ dotToTest->setGrayLevelMin ( getGrayLevelMin() );
+ dotToTest->setGrayLevelMax ( getGrayLevelMax() );
+ dotToTest->setGrayLevelPrecision( getGrayLevelPrecision() );
+ dotToTest->setSizePrecision( getSizePrecision() );
+ dotToTest->setGraphics( graphics );
+ dotToTest->setGraphicsThickness( thickness );
+ dotToTest->setComputeMoments( true );
+ dotToTest->setArea( area );
+ dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
+
+ // first compute the parameters of the dot.
+ // if for some reasons this caused an error tracking
+ // (dot partially out of the image...), check the next intersection
+ if( dotToTest->computeParameters( I ) == false ) {
+ // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ u = border_u;
+ v = border_v;
+ continue;
+ }
+ // if the dot to test is valid,
+ if( dotToTest->isValid( I, *this ) )
+ {
+ vpImagePoint cogDotToTest = dotToTest->getCog();
+ // Compute the distance to the center. The center used here is not the
+ // area center available by area.getCenter(area_center_u,
+ // area_center_v) but the center of the input area which may be
+ // partially outside the image.
+
+ double area_center_u = area_u + area_w/2.0 - 0.5;
+ double area_center_v = area_v + area_h/2.0 - 0.5;
+
+ double thisDiff_u = cogDotToTest.get_u() - area_center_u;
+ double thisDiff_v = cogDotToTest.get_v() - area_center_v;
+ double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
+
+ bool stopLoop = false;
+ itnice = niceDots.begin();
+
+ while( itnice != niceDots.end() && stopLoop == false )
+ {
+ tmpDot = *itnice;
+
+ //double epsilon = 0.001; // detecte +sieurs points
+ double epsilon = 3.0;
+ // if the center of the dot is the same than the current
+ // don't add it, test the next point of the grid
+ cogTmpDot = tmpDot.getCog();
+
+ if( fabs( cogTmpDot.get_u() - cogDotToTest.get_u() ) < epsilon &&
+ fabs( cogTmpDot.get_v() - cogDotToTest.get_v() ) < epsilon )
+ {
+ stopLoop = true;
+ // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
+ u = border_u;
+ v = border_v;
+ continue;
+ }
+
+ double otherDiff_u = cogTmpDot.get_u() - area_center_u;
+ double otherDiff_v = cogTmpDot.get_v() - area_center_v;
+ double otherDist = sqrt( otherDiff_u*otherDiff_u +
+ otherDiff_v*otherDiff_v );
+
+
+ // if the distance of the curent vector element to the center
+ // is greater than the distance of this dot to the center,
+ // then add this dot before the current vector element.
+ if( otherDist > thisDist )
+ {
+ niceDots.insert(itnice, *dotToTest );
+ ++ itnice;
+ stopLoop = true;
+ // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
+ u = border_u;
+ v = border_v;
+ continue;
+ }
+ ++itnice;
+ }
+ vpTRACE(4, "End while (%d, %d)", u, v);
+
+ // if we reached the end of the vector without finding the dot
+ // or inserting it, insert it now.
+ if( itnice == niceDots.end() && stopLoop == false )
+ {
+ niceDots.push_back( *dotToTest );
+ }
+ }
+ else {
+ // Store bad dots
+ badDotsVector.push_front( *dotToTest );
+ }
+ }
+ }
+ if( dotToTest != NULL ) delete dotToTest;
+}
+
+/*!
+
+ Check if the dot is "like" the wanted dot passed in.
+
+ Compare the following characteristics of the dot to the wanted dot;
+ - the size (width or height)
+ - the surface (number of pixels)
+ - the geometry; the shape should be ellipsoid if
+ setEllipsoidShapePrecision(double ellispoidShapePrecision) is used.
+
+ \return If it is so, return true, otherwise return false.
+
+ \warning Parameters of the wanted dot (width, height, surface, in level, out
+ level, accuracy) must already be set before; see
+ searchDotsInArea(vpImage<unsigned char>& I)
+
+ \param I : Image.
+ \param wantedDot : Wanted dot passed in.
+
+*/
+bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
+{
+ double size_precision = wantedDot.getSizePrecision();
+ double ellipsoidShape_precision = wantedDot.getEllipsoidShapePrecision();
+ double epsilon = 0.001;
+
+ //
+ // First, check the width, height and surface of the dot. Those parameters
+ // must be the same.
+ //
+ //if ( (wantedDot.getWidth() != 0)
+ // && (wantedDot.getHeight() != 0)
+ // && (wantedDot.getArea() != 0) )
+ if ( (std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon())
+ &&
+ (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon())
+ &&
+ (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon()) )
+ // if (size_precision!=0){
+ if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()){
+#ifdef DEBUG
+ std::cout << "test size precision......................\n";
+ std::cout << "wanted dot: " << "w=" << wantedDot.getWidth()
+ << " h=" << wantedDot.getHeight()
+ << " s=" << wantedDot.getArea()
+ << " precision=" << size_precision
+ << " epsilon=" << epsilon << std::endl;
+ std::cout << "dot found: " << "w=" << getWidth()
+ << " h=" << getHeight()
+ << " s=" << getArea() << std::endl;
+#endif
+ if( ( wantedDot.getWidth()*size_precision-epsilon < getWidth() ) == false )
+ {
+ vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad width > for dot (%g, %g)\n", cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+
+ if( ( getWidth() < wantedDot.getWidth()/(size_precision+epsilon ) )== false )
+ {
+ vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad width %g > %g for dot (%g, %g)\n",
+ getWidth(), wantedDot.getWidth()/(size_precision+epsilon),
+ cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+
+ if( ( wantedDot.getHeight()*size_precision-epsilon < getHeight() ) == false )
+ {
+ vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad height %g > %g for dot (%g, %g)\n",
+ wantedDot.getHeight()*size_precision-epsilon, getHeight(),
+ cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+
+ if( ( getHeight() < wantedDot.getHeight()/(size_precision+epsilon )) == false )
+ {
+ vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad height %g > %g for dot (%g, %g)\n",
+ getHeight(), wantedDot.getHeight()/(size_precision+epsilon),
+ cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+
+ if( ( wantedDot.getArea()*(size_precision*size_precision)-epsilon < getArea() ) == false )
+ {
+ vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad surface %g > %g for dot (%g, %g)\n",
+ wantedDot.getArea()*(size_precision*size_precision)-epsilon,
+ getArea(),
+ cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+
+ if( ( getArea() < wantedDot.getArea()/(size_precision*size_precision+epsilon )) == false )
+ {
+ vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)",
+ cog.get_u(), cog.get_v());
+#ifdef DEBUG
+ printf("Bad surface %g < %g for dot (%g, %g)\n",
+ getArea(), wantedDot.getArea()/(size_precision*size_precision+epsilon),
+ cog.get_u(), cog.get_v());
+#endif
+ return false;
+ }
+ }
+ //
+ // Now we can proceed to more advanced (and costy) checks.
+ // First check there is a white (>level) elipse within dot
+ // Then check the dot is surrounded by a black ellipse.
+ //
+ int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid
+ int nb_bad_points = 0;
+ int nb_max_bad_points = (int)(nb_point_to_test*allowedBadPointsPercentage_);
+ double step_angle = 2*M_PI / nb_point_to_test;
+
+ // if (ellipsoidShape_precision != 0 && compute_moment) {
+ if (std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon() && compute_moment) {
+ // std::cout << "test shape precision......................\n";
+ // See F. Chaumette. Image moments: a general and useful set of features
+ // for visual servoing. IEEE Trans. on Robotics, 20(4):713-723, August 2004.
+
+ // mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
+ // = m11 - m10 * m01 / m00
+ // mu20 = m20 - m00 * xg^2 = m20 - m00 * m10/m00 * m10/m00
+ // = m20 - m10^2 / m00
+ // mu02 = m02 - m01^2 / m00
+ // alpha = 1/2 arctan( 2 * mu11 / (mu20 - mu02) )
+ //
+ // a1^2 = 2 / m00 * (mu02 + mu20 + sqrt( (mu20 - mu02)^2 + 4mu11^2) )
+ //
+ // a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) )
+
+ //we compute parameters of the estimated ellipse
+ double tmp1 = (m01*m01 -m10*m10)/m00+(m20-m02);
+ double tmp2 = m11 -m10*m01/m00 ;
+ double Sqrt = sqrt(tmp1*tmp1 + 4*tmp2*tmp2);
+ double a1 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 + Sqrt));
+ double a2 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 - Sqrt));
+ double alpha = 0.5*atan2(2*(m11*m00-m10*m01),
+ ((m20-m02)*m00-m10*m10+m01*m01));
+
+ // to be able to track small dots, minorize the ellipsoid radius for the
+ // inner test
+ a1 -= 1.0;
+ a2 -= 1.0;
+
+ double innerCoef = ellipsoidShape_precision ;
+ unsigned int u, v;
+ double cog_u = this->cog.get_u();
+ double cog_v = this->cog.get_v();
+
+ vpImagePoint ip;
+ nb_bad_points = 0;
+ for( double theta = 0. ; theta<2*M_PI ; theta+= step_angle ) {
+ 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 cercle pixel (%d, %d) has bad level for dot (%g, %g)",
+ // u, v, cog_u, cog_v);
+#ifdef DEBUG
+ printf("Inner cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
+ u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
+#endif
+ //return false;
+ nb_bad_points ++;
+ }
+ if (graphics) {
+ for (unsigned int t=0; t< thickness; t++) {
+ ip.set_u( u + t );
+ ip.set_v( v );
+ vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ }
+ }
+#ifdef DEBUG
+ vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ vpDisplay::flush(I);
+#endif
+ }
+ if (nb_bad_points > nb_max_bad_points)
+ {
+#ifdef DEBUG
+ printf("Inner ellipse has %d bad points. Max allowed is %d\n",
+ nb_bad_points, nb_max_bad_points);
+#endif
+ return false;
+ }
+ // to be able to track small dots, maximize the ellipsoid radius for the
+ // inner test
+ a1 += 2.0;
+ a2 += 2.0;
+
+ double outCoef = 2-ellipsoidShape_precision; //1.6;
+ nb_bad_points = 0;
+ for( double theta=0. ; theta<2*M_PI ; theta+= step_angle ) {
+ u = (unsigned int) (cog_u + outCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
+ v = (unsigned int) (cog_v + outCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
+#ifdef DEBUG
+ //vpDisplay::displayRectangle(I, area, vpColor::yellow);
+ vpDisplay::displayCross( I, v, u, 7, vpColor::purple ) ;
+ vpDisplay::flush(I);
+#endif
+ // If outside the area, continue
+ if ((double)u < area.getLeft() || (double)u > area.getRight()
+ || (double)v < area.getTop() || (double)v > area.getBottom()) {
+ continue;
+ }
+ if( ! this->hasReverseLevel( I, u, v ) ) {
+ // vpTRACE("Outside cercle pixel (%d, %d) has bad level for dot (%g, %g)",
+ // u, v, cog_u, cog_v);
+#ifdef DEBUG
+ printf("Outside cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
+ u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
+#endif
+ nb_bad_points ++;
+ //return false;
+ }
+ if (graphics) {
+ for(unsigned int t=0; t<thickness; t++) {
+ ip.set_u( u + t);
+ ip.set_v( v );
+
+ vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ }
+ }
+ }
+ }
+ if (nb_bad_points > nb_max_bad_points)
+ {
+#ifdef DEBUG
+ printf("Outside ellipse has %d bad points. Max allowed is %d\n",
+ nb_bad_points, nb_max_bad_points);
+#endif
+ return false;
+ }
+
+ return true;
+}
+
+
+
+/*!
+
+ Check if a the pixel of coordinates (u, v) is in the image and has
+ a good level to belong to this dot.
+
+ \param I : Image.
+ \param u : Pixel to test.
+ \param v : Pixel to test.
+
+ \return true : If the pixel of coordinates (u, v) is in the area and
+ has a value between the min and max levels fixed by setGrayLevelMin() and setGrayLevelMax().
+
+ \return false : Otherwise
+
+ \sa setGrayLevelMin(), setGrayLevelMax()
+
+*/
+bool vpDot2::hasGoodLevel(const vpImage<unsigned char>& I,
+ const unsigned int &u,
+ const unsigned int &v) const
+{
+ if( !isInArea( u, v ) )
+ return false;
+
+ if( I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+/*!
+
+ Check if a the pixel of coordinates (u, v) in the image has a good level to
+ be a dark zone around the dot.
+
+ \param I : Image.
+ \param u : Pixel to test.
+ \param v : Pixel to test.
+
+ \return true if it is so, and false otherwise.
+
+*/
+bool vpDot2::hasReverseLevel(const vpImage<unsigned char>& I,
+ const unsigned int &u,
+ const unsigned int &v) const
+{
+
+ if( !isInArea( u, v ) )
+ return false;
+
+ if( I[v][u] < gray_level_min || I[v][u] > gray_level_max)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+/*!
+ Return a new instance of vpDot2.
+
+ Should be used by child classed to return their own instance of vpDot2.
+
+ \return An instance of vpDot2.
+
+*/
+vpDot2* vpDot2::getInstance()
+{
+ return new vpDot2();
+}
+
+/*!
+
+ Returns the list of Freeman chain code used to turn around the dot
+ counterclockwise.
+
+ \return List of Freeman chain list [0, ..., 7]
+ - 0 : right
+ - 1 : top right
+ - 2 : top
+ - 3 : top left
+ - 4 : left
+ - 5 : down left
+ - 6 : down
+ - 7 : down right
+*/
+void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const
+{
+ freeman_chain = direction_list;
+}
+
+
+
+/******************************************************************************
+ *
+ * PRIVATE METHODS
+ *
+ ******************************************************************************/
+
+
+
+/*!
+
+ Compute all the parameters of the dot (center, width, height, surface,
+ inertia moments...).
+
+ This is done the following way:
+
+ - First, we check the point (_u, _v) passed in has the right level in the
+ image
+
+ - Then we cross the tracked entity from left to right until we reach it's
+ border.
+
+ - We follow this border until we come back to the first point or we get to
+ border of the image. Each time we update variables used to compute the
+ dot parameters
+
+ \param I : The image we are working with.
+
+ \param _u : A pixel coordinate inside the dot.
+
+ \param _v : A pixel coordinate inside the dot.
+
+ \return false : If a dot can't be found around pixel coordinates given as
+ parameter
+
+ \return true : If a dot was found.
+
+ \sa getFirstBorder_u(), getFirstBorder_v()
+
+*/
+bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
+ const double &_u,
+ const double &_v)
+{
+ direction_list.clear();
+ ip_edges_list.clear();
+
+ double est_u = _u; // estimated
+ double est_v = _v;
+
+ // if u has default value, set it to the actual center value
+ //if( est_u == -1.0 )
+ if( std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u),1.)*std::numeric_limits<double>::epsilon() )
+ {
+ est_u = this->cog.get_u();
+ }
+
+ // if v has default value, set it to the actual center value
+ //if( est_v == -1.0 )
+ if( std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v),1.)*std::numeric_limits<double>::epsilon() )
+ {
+ est_v = this->cog.get_v();
+ }
+
+ // if the estimated position of the dot is out of the image, not need to continue,
+ // return an error tracking
+ if( !isInArea( (unsigned int) est_u, (unsigned int) est_v ) )
+ {
+ vpDEBUG_TRACE(3, "Initial pixel coordinates (%d, %d) for dot tracking are not in the area",
+ (int) est_u, (int) est_v) ;
+ return false;
+ }
+
+ bbox_u_min = (int)I.getWidth();
+ bbox_u_max = 0;
+ bbox_v_min = (int)I.getHeight();
+ bbox_v_max = 0;
+
+ // if the first point doesn't have the right level then there's no point to
+ // continue.
+ if( !hasGoodLevel( I, (unsigned int) est_u, (unsigned int) est_v ) )
+ {
+ vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates",
+ (int) est_u, (int) est_v) ;
+ return false;
+ }
+
+ // find the border
+
+ if(!findFirstBorder(I, (unsigned int) est_u, (unsigned int) est_v,
+ this->firstBorder_u, this->firstBorder_v)) {
+
+ vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates",
+ (int) est_u, (int) est_v) ;
+ return false;
+ }
+
+ unsigned int dir = 6;
+
+ // Determine the first element of the Freeman chain
+ computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir);
+ unsigned int firstDir = dir;
+
+ // if we are now out of the image, return an error tracking
+ if( !isInArea( this->firstBorder_u, this->firstBorder_v ) )
+ {
+ vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area",
+ this->firstBorder_u, this->firstBorder_v);
+ return false;
+ }
+
+ // store the new direction and dot border coordinates.
+ direction_list.push_back( dir );
+ vpImagePoint ip;
+ ip.set_u( this->firstBorder_u );
+ ip.set_v( this->firstBorder_v );
+
+ ip_edges_list.push_back( ip );
+
+ 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;
+ m10 = 0.0;
+ m01 = 0.0;
+ m11 = 0.0;
+ m20 = 0.0;
+ m02 = 0.0;
+ // while we didn't come back to the first point, follow the border
+ do {
+ // if it was asked, show the border
+ if (graphics) {
+ for(int t=0; t< (int)thickness; t++) {
+ ip.set_u ( border_u + t);
+ ip.set_v ( border_v );
+
+ vpDisplay::displayPoint(I, ip, vpColor::red) ;
+ }
+ //vpDisplay::flush(I);
+ }
+#ifdef DEBUG
+ vpDisplay::displayPoint(I, border_v, border_u, vpColor::red);
+ vpDisplay::flush(I);
+#endif
+ // Determine the increments for the parameters
+ computeFreemanParameters(border_u, border_v, dir, du, dv,
+ dS, // surface
+ dMu, dMv, // first order moments
+ dMuv, dMu2, dMv2); // second order moment
+
+ // Update the parameters
+ border_u += du; // Next position on the border
+ border_v += dv;
+ m00 += dS; // enclosed area
+ m10 += dMu; // First order moment along v axis
+ m01 += dMv; // First order moment along u axis
+ if (compute_moment) {
+ m11 += dMuv; // Second order moment
+ m20 += dMu2; // Second order moment along v axis
+ m02 += dMv2; // Second order moment along u axis
+ }
+ // if we are now out of the image, return an error tracking
+ if( !isInArea( (unsigned int)border_u, (unsigned int)border_v ) ) {
+
+ vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v);
+ // Can Occur on a single pixel dot located on the top border
+ return false;
+ }
+
+ // store the new direction and dot border coordinates.
+
+ direction_list.push_back( dir );
+
+ ip.set_u( border_u );
+ ip.set_v( border_v );
+ ip_edges_list.push_back( ip );
+
+ // vpDisplay::getClick(I);
+
+ // update the extreme point of the dot.
+ if( border_v < bbox_v_min ) bbox_v_min = border_v;
+ if( border_v > bbox_v_max ) bbox_v_max = border_v;
+ if( border_u < bbox_u_min ) bbox_u_min = border_u;
+ if( border_u > bbox_u_max ) bbox_u_max = border_u;
+
+ // move around the tracked entity by following the border.
+ if (computeFreemanChainElement(I, (unsigned int)border_u, (unsigned int)border_v, dir) == false) {
+ vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)",
+ border_u, border_v);
+ return false;
+ }
+
+ // vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v, dir);
+
+ }
+ while( (getFirstBorder_u() != (unsigned int)border_u
+ || getFirstBorder_v() != (unsigned int)border_v
+ || firstDir != dir) &&
+ isInArea( (unsigned int)border_u, (unsigned int)border_v ) );
+
+#ifdef VP_DEBUG
+#if VP_DEBUG_MODE == 3
+ vpDisplay::flush(I);
+#endif
+#endif
+
+ // if the surface is one or zero , the center of gravity wasn't properly
+ // detected. Return an error tracking.
+ //if( m00 == 0 || m00 == 1 )
+ if( std::fabs(m00) <= std::numeric_limits<double>::epsilon()
+ || std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.)*std::numeric_limits<double>::epsilon() )
+ {
+ vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected");
+ return false;
+ }
+ else // compute the center
+ {
+ // this magic formula gives the coordinates of the center of gravity
+ double tmpCenter_u = m10 / m00;
+ double tmpCenter_v = m01 / m00;
+
+ //Updates the central moments
+ if (compute_moment)
+ {
+ mu11 = m11 - tmpCenter_u*m01;
+ mu02 = m02 - tmpCenter_v*m01;
+ 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 );
+ }
+
+ width = bbox_u_max - bbox_u_min + 1;
+ height = bbox_v_max - bbox_v_min + 1;
+ surface = m00;
+
+ computeMeanGrayLevel(I);
+ return true;
+}
+
+
+/*!
+ Find the starting point on a dot border from an other point in the dot.
+ the dot border is computed from this point.
+
+ \param I : Image.
+ \param u : The row coordinate of a pixel in the dot.
+ \param v : The column coordinate of a pixel in the dot.
+ \param border_u : The row coordinate of the found starting point.
+ \param border_v : The column coordinate of the found starting point.
+
+ \return false if the width of this dot was initialised and we already crossed
+the dot on more than the max possible width. Return true if success.
+
+ \sa computeParameters()
+*/
+bool
+ vpDot2::findFirstBorder(const vpImage<unsigned char> &I,
+ const unsigned int &u,
+ const unsigned int &v,
+ unsigned int &border_u,
+ unsigned int &border_v)
+{
+ // find the border
+
+ // NOTE:
+ // from here we use int and not double. This is because we don't have
+ // rounding problems and it's actually more a trouble than smth else to
+ // work with double when navigating around the dot.
+ border_u = u;
+ border_v = v;
+ double epsilon =0.001;
+
+#ifdef DEBUG
+ std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl;
+#endif
+ while( hasGoodLevel( I, border_u+1, border_v ) &&
+ border_u < area.getRight()/*I.getWidth()*/ ) {
+ // if the width of this dot was initialised and we already crossed the dot
+ // on more than the max possible width, no need to continue, return an
+ // error tracking
+ if( getWidth() > 0 && ( border_u - u ) > getWidth()/(getMaxSizeSearchDistancePrecision()+epsilon) ) {
+ vpDEBUG_TRACE(3, "The found dot (%d, %d, %d) has a greater width than the required one", u, v, border_u);
+ return false;
+ }
+#ifdef DEBUG
+ vpDisplay::displayPoint(I, border_v, border_u+1, vpColor::green);
+ vpDisplay::flush(I);
+#endif
+
+ border_u++;
+ }
+ return true;
+}
+
+
+/*!
+
+ Considering a pixel (u, v) compute the next element of the Freeman chain
+ code.
+
+ According to the gray level of pixel (u, v) and his eight neighbors determine
+ the next element of the chain in order to turn around the dot
+ counterclockwise.
+
+ \param I : The image we are working with.
+ \param v : The row coordinate of a pixel on a border.
+ \param u : The column coordinate of the pixel on a border.
+ \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
+ with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
+
+ \return false if an element cannot be found. Occurs for example with an area
+ constituted by a single pixel. Return true if success.
+*/
+bool
+ vpDot2::computeFreemanChainElement(const vpImage<unsigned char> &I,
+ const unsigned int &u,
+ const unsigned int &v,
+ unsigned int &element)
+{
+
+ if (hasGoodLevel( I, u, v )) {
+ unsigned int _u = u;
+ unsigned int _v = v;
+ // get the point on the right of the point passed in
+ updateFreemanPosition( _u, _v, (element + 2) %8 );
+ if (hasGoodLevel( I, _u, _v )) {
+ element = (element + 2) % 8; // turn right
+ }
+ else {
+ unsigned int _u1 = u;
+ unsigned int _v1 = v;
+ updateFreemanPosition( _u1, _v1, (element + 1) %8 );
+
+ if ( hasGoodLevel( I, _u1, _v1 )) {
+ element = (element + 1) % 8; // turn diag right
+ }
+ else {
+ unsigned int _u2 = u;
+ unsigned int _v2 = v;
+ updateFreemanPosition( _u2, _v2, element ); // same direction
+
+ if ( hasGoodLevel( I, _u2, _v2 )) {
+ //element = element; // keep same dir
+ }
+ else {
+ unsigned int _u3 = u;
+ unsigned int _v3 = v;
+ updateFreemanPosition( _u3, _v3, (element + 7) %8 ); // diag left
+
+ if ( hasGoodLevel( I, _u3, _v3 )) {
+ element = (element + 7) %8; // turn diag left
+ }
+ else {
+ unsigned int _u4 = u;
+ unsigned int _v4 = v;
+ updateFreemanPosition( _u4, _v4, (element + 6) %8 ); // left
+
+ if ( hasGoodLevel( I, _u4, _v4 )) {
+ element = (element + 6) %8 ; // turn left
+ }
+ else {
+ unsigned int _u5 = u;
+ unsigned int _v5 = v;
+ updateFreemanPosition( _u5, _v5, (element + 5) %8 ); // left
+
+ if ( hasGoodLevel( I, _u5, _v5 )) {
+ element = (element + 5) %8 ; // turn diag down
+ }
+ else {
+ unsigned int _u6 = u;
+ unsigned int _v6 = v;
+ updateFreemanPosition( _u6, _v6, (element + 4) %8 ); // left
+
+ if ( hasGoodLevel( I, _u6, _v6 )) {
+ element = (element + 4) %8 ; // turn down
+ }
+ else {
+ unsigned int _u7 = u;
+ unsigned int _v7 = v;
+ updateFreemanPosition( _u7, _v7, (element + 3) %8 ); // diag
+
+ if ( hasGoodLevel( I, _u7, _v7 )) {
+ element = (element + 3) %8 ; // turn diag right down
+ }
+ else {
+ // No neighbor with a good level
+ //
+ return false;
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ else {
+ return false;
+ }
+
+ return true;
+
+}
+
+/*!
+
+ Given the previous position of a pixel (u_p, v_p) on the dot border and the
+ direction to reach the next pixel on the border, compute Freeman parameters.
+
+ \param u_p : Previous value of the row coordinate of a pixel on a border.
+ \param v_p : Previous value of the column coordinate of a pixel on a border.
+ \param du : Increment to go from previous to next pixel on the dot border.
+ \param dv : Increment to go from previous to next pixel on the dot border.
+
+ \param dS : Enclosed area increases. Cumulated values of dS gives m00.
+
+ \param dMu : First order moment along v axis increases. Cumulated values of
+ dMu gives m10.
+
+ \param dMv : First order moment along u axis increases. Cumulated values of
+ dMv gives m01.
+
+ \param dMuv : Moment increases. Cumulated values of dMuv gives m11.
+
+ \param dMu2 : Second order moment along v axis increases. Cumulated values of
+ dMu2 gives m20.
+
+ \param dMv2 : Second order moment along u axis increases. Cumulated values of
+ dMv2 gives m02.
+
+ Considering the previous coordinates (u_p, v_p) of a pixel on a border, the
+ next coordinates (u, v) are given by: u = u_p + du and v = v_p + dv
+
+
+*/
+void
+ vpDot2::computeFreemanParameters(const int &u_p,
+ const int &v_p,
+ unsigned int &element,
+ int &du, int &dv,
+ float &dS,
+ float &dMu, float &dMv,
+ float &dMuv,
+ float &dMu2, float &dMv2)
+{
+ du = 0;
+ dv = 0;
+ dMuv = 0;
+ dMu2 = 0;
+ dMv2 = 0;
+
+ /*
+ 3 2 1
+ \ | /
+ \|/
+ 4 ------- 0
+ /|\
+ / | \
+ 5 6 7
+ */
+ switch(element) {
+ case 0: // go right
+ du = 1;
+ dS = (float) v_p;
+ dMu = 0.0;
+ dMv = (float)(0.5 * v_p * v_p);
+ if (compute_moment) {
+ dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
+ dMu2 = 0;
+ dMv2 = (float)(1.0/ 3. * v_p * v_p * v_p);
+ }
+ break;
+
+ case 1: // go right top
+ du = 1;
+ dv = 1;
+ dS = (float)(v_p + 0.5);
+ dMu = - (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
+ dMv = (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
+ if (compute_moment) {
+ float half_u_p = (float)(0.5*u_p);
+ dMuv = (float)(v_p*v_p*(0.25+half_u_p) + v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
+ dMu2 = (float)(-1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1./12.0);
+ dMv2 = (float)( 1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1./12.0);
+ }
+ break;
+
+ case 2: // go top
+ dv = 1;
+ dS = 0.0;
+ dMu = (float)(- 0.5 * u_p * u_p);
+ dMv = 0.0;
+ if (compute_moment) {
+ dMuv = 0;
+ dMu2 = (float)(-1.0/ 3. * u_p * u_p * u_p);
+ dMv2 = 0;
+ }
+ break;
+
+ case 3:
+ du = -1;
+ dv = 1;
+ dS = (float)(- v_p - 0.5);
+ dMu = - (float)(0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
+ dMv = - (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
+ if (compute_moment) {
+ float half_u_p = (float)(0.5*u_p);
+ dMuv = (float)(v_p*v_p*(0.25-half_u_p) + v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
+ dMu2 = (float)(-1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
+ dMv2 = (float)(-1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1./12.0);
+ }
+ break;
+
+ case 4:
+ du = -1;
+ dS = (float)(- v_p);
+ dMv = (float)(- 0.5 * v_p * v_p);
+ dMu = 0.0;
+ if (compute_moment) {
+ dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
+ dMu2 = 0;
+ dMv2 = (float)(-1.0/ 3. * v_p * v_p * v_p);
+ }
+ break;
+
+ case 5:
+ du = -1;
+ dv = -1;
+ dS = (float)(- v_p + 0.5);
+ dMu = (float)( 0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
+ dMv = (float)(- (0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0));
+ if (compute_moment) {
+ float half_u_p = (float)(0.5*u_p);
+ dMuv = (float)(v_p*v_p*(0.25-half_u_p) - v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
+ dMu2 = (float)( 1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
+ dMv2 = (float)(-1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
+ }
+ break;
+
+ case 6:
+ dv = -1;
+ dS = 0.0;
+ dMu = (float)(0.5 * u_p * u_p);
+ dMv = 0.0;
+ if (compute_moment) {
+ dMuv = 0;
+ dMu2 = (float)(1.0/ 3. * u_p * u_p * u_p);
+ dMv2 = 0;
+ }
+ break;
+
+ case 7:
+ du = 1;
+ dv = -1;
+ dS = (float)(v_p - 0.5);
+ dMu = (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
+ dMv = (float)(0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0);
+ if (compute_moment) {
+ float half_u_p = (float)(0.5*u_p);
+ dMuv = (float)(v_p*v_p*(0.25+half_u_p) - v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
+ dMu2 = (float)(1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1./12.0);
+ dMv2 = (float)(1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
+ }
+ break;
+ }
+}
+
+
+/*!
+
+ From a pixel coordinate and a direction, update the pixel coordinates after
+ moving forward.
+
+ \param v : The row coordinate of the pixel, updated by this method.
+
+ \param u : The column coordinate of the pixel, updated by this method.
+
+ \param dir : The direction in the image, 0=right, 1, 2=down, 3, 4=left, 5,
+ 6=up and 7.
+
+*/
+void vpDot2::updateFreemanPosition( unsigned int& u, unsigned int& v,
+ const unsigned int &dir )
+{
+ switch(dir) {
+ case 0: u += 1; break;
+ case 1: u += 1; v += 1; break;
+ case 2: v += 1; break;
+ case 3: u -= 1; v += 1; break;
+ case 4: u -= 1; break;
+ case 5: u -= 1; v -= 1; break;
+ case 6: v -= 1; break;
+ case 7: u += 1; v -= 1; break;
+ }
+}
+
+/*!
+
+ Test if a pixel is in the image. Points of the border are not
+ considered to be in the image. Call the isInImage( vpImage<unsigned
+ char> &I, const vpImagePoint &) method.
+
+ \param I : The image.
+
+ \return true if the pixel of coordinates (posI, posJ) is in the image and
+ false otherwise.
+*/
+bool vpDot2::isInImage(const vpImage<unsigned char> &I) const
+{
+ return isInImage( I, cog);
+}
+
+/*!
+
+ Test if a pixel is in the image. Points of the border are not considered to
+ be in the image.
+
+ \param I : The image.
+ \param ip : An image point.
+
+ \return true if the image point \e ip is in the image and false
+ otherwise.
+*/
+bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const
+{
+ unsigned int h = I.getHeight();
+ unsigned int w = I.getWidth();
+ double u = ip.get_u();
+ double v = ip.get_v();
+
+ if( u < 0 || u >= w ) return false;
+ if( v < 0 || v >= h ) return false;
+ return true;
+}
+
+/*!
+
+ Test if a pixel is in a region of interest. Points of the border are not considered to
+ be in the area.
+
+ \param u : The column coordinate of the pixel.
+ \param v : The row coordinate of the pixel .
+
+ \return true if the pixel of coordinates (u, v) is in the image and false
+ otherwise.
+*/
+bool vpDot2::isInArea( const unsigned int &u, const unsigned int &v) const
+{
+ unsigned int area_u_min = (unsigned int) area.getLeft();
+ unsigned int area_u_max = (unsigned int) area.getRight();
+ unsigned int area_v_min = (unsigned int) area.getTop();
+ unsigned int area_v_max = (unsigned int) area.getBottom();
+
+ if( u < area_u_min || u > area_u_max ) return false;
+ if( v < area_v_min || v > area_v_max ) return false;
+ return true;
+}
+
+
+/*!
+
+ Get the search grid size used to found a dot in a region of interest. This grid is used to
+ parse only some pixels of the search area.
+
+ \param gridWidth : Number of pixels between to vertical lines of the grid
+
+ \param gridHeight : Number of pixels between to horizontal lines of the grid
+
+
+*/
+void vpDot2::getGridSize( unsigned int &gridWidth, unsigned int &gridHeight )
+{
+ // first get the research grid width and height Note that
+ // 1/sqrt(2)=cos(pi/4). The grid squares should be small enough to be
+ // contained in the dot. We gent this here if the dot is a perfect disc.
+ // More accurate criterium to define the grid should be implemented if
+ // necessary
+ gridWidth = (unsigned int) (getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.));
+ gridHeight = (unsigned int) (getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0));
+
+ if( gridWidth == 0 ) gridWidth = 1;
+ if( gridHeight == 0 ) gridHeight = 1;
+}
+
+
+
+/*!
+
+ Compute an approximation of mean gray level of the dot.
+ We compute it by searching the mean of vertical and diagonal points
+ which gray is between min and max gray level.
+
+ \param I: The image.
+
+ \return the mean gray level
+
+
+*/
+void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char>& I)
+{
+ int cog_u = (int)cog.get_u();
+ int cog_v = (int)cog.get_v();
+
+ unsigned int sum_value =0;
+ unsigned int nb_pixels =0;
+
+ for(unsigned int i=(unsigned int)this->bbox_u_min; i <=(unsigned int)this->bbox_u_max ; i++){
+ unsigned int pixel_gray =(unsigned int) I[(unsigned int)cog_v][i];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ sum_value += pixel_gray;
+ nb_pixels ++;
+ }
+ }
+ for(unsigned int i=(unsigned int)this->bbox_v_min; i <=(unsigned int)this->bbox_v_max ; i++){
+ unsigned char pixel_gray =I[i][(unsigned int)cog_u];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ sum_value += pixel_gray;
+ nb_pixels ++;
+ }
+ }
+ if(nb_pixels < 10){ //could be good to choose the min nb points from area of dot
+ //add diagonals points to have enough point
+ int imin,imax;
+ if( (cog_u - bbox_u_min) > (cog_v - bbox_v_min)){
+ imin=cog_v - bbox_v_min;
+ }
+ else{ imin = cog_u - bbox_u_min;}
+ if( (bbox_u_max - cog_u) > (bbox_v_max - cog_v)){
+ imax=bbox_v_max - cog_v;
+ }
+ else{ imax = bbox_u_max - cog_u;}
+ for(int i=-imin; i <=imax ; i++){
+ unsigned int pixel_gray =(unsigned int) I[(unsigned int)(cog_v + i)][(unsigned int)(cog_u + i)];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ sum_value += pixel_gray;
+ nb_pixels ++;
+ }
+ }
+
+ if( (cog_u - bbox_u_min) > (bbox_v_max - cog_v)){
+ imin = bbox_v_max - cog_v;
+ }
+ else{ imin = cog_u - bbox_u_min;}
+ if( (bbox_u_max - cog_u) > (cog_v - bbox_v_min)){
+ imax = cog_v - bbox_v_min;
+ }
+ else{ imax = bbox_u_max - cog_u;}
+
+ for(int i=-imin; i <=imax ; i++){
+ unsigned char pixel_gray =I[(unsigned int)(cog_v - i)][(unsigned int)(cog_u + i)];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ sum_value += pixel_gray;
+ nb_pixels ++;
+ }
+ }
+ }
+
+ if(nb_pixels== 0){
+ //should never happen
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError,"No point was found"));
+ }
+ else{
+ mean_gray_level = sum_value/nb_pixels;
+ }
+}
+
+/*!
+ Define a number of dots from a file.
+ If the file does not exist, define it by clicking an image, the dots are then saved into the file.
+
+ If the dots from the file cannot be tracked in the image, will ask to click them.
+
+ \param dot : dot2 array
+ \param n : number of dots, array dimension
+ \param dotFile : path for the file
+ \param I : image
+ \param col : color to print the dots (default Blue)
+ \param trackDot : if true, tracks the dots in the image, if false simply loads the coordinates (default true)
+
+ \return an nx2 matrix with the coordinates of the dots
+*/
+vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col, bool trackDot)
+{
+ vpMatrix Cogs(n, 2);
+ vpImagePoint cog;
+ unsigned int i;
+ bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
+ if(fromFile)
+ {
+ vpMatrix::loadMatrix(dotFile, Cogs);
+ std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
+ }
+
+ // test number of cogs in file
+ if(Cogs.getRows() < n)
+ {
+ std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
+ fromFile = false;
+ }
+
+ // read from file and tracks the dots
+ if(fromFile)
+ {
+ try
+ {
+ for(i=0;i<n;++i)
+ {
+ cog.set_uv(Cogs[i][0], Cogs[i][1]);
+ dot[i].setGraphics(true);
+ dot[i].setCog(cog);
+ if(trackDot)
+ {
+ dot[i].initTracking(I,cog);
+ dot[i].track(I);
+ vpDisplay::displayCross(I, cog, 10, col);
+ }
+ }
+ }
+ catch(...)
+ {
+ std::cout << "Cannot track dots from file" << std::endl;
+ fromFile = false;
+ }
+ vpDisplay::flush(I);
+
+ // check that dots are far away ones from the other
+ double d;
+ for(i=0;i<n && fromFile;++i)
+ {
+ d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth()));
+ for(unsigned int j=0;j<n && fromFile;++j)
+ if(j!=i)
+ if(dot[i].getDistance(dot[j]) < d)
+ {
+ fromFile = false;
+ std::cout << "Dots from file seem incoherent" << std::endl;
+ }
+ }
+ }
+
+ if(!fromFile)
+ {
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
+ for (i = 0; i < n; i++)
+ {
+ if(trackDot)
+ {
+ dot[i].setGraphics(true);
+ dot[i].initTracking(I);
+ cog = dot[i].getCog();
+ }
+ else
+ {
+ vpDisplay::getClick(I, cog);
+ dot[i].setCog(cog);
+ }
+ Cogs[i][0] = cog.get_u();
+ Cogs[i][1] = cog.get_v();
+ vpDisplay::displayCross(I, cog, 10, col);
+ vpDisplay::flush(I);
+ }
+ }
+
+ if (!fromFile & (dotFile != ""))
+ {
+ vpMatrix::saveMatrix(dotFile, Cogs);
+ std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
+ }
+
+ // back to non graphic mode
+ for(i=0;i<n;++i)
+ dot[i].setGraphics(false);
+
+ return Cogs;
+}
+
+/*!
+ Tracks a number of dots in an image and displays their trajectories
+
+ \param dot : dot2 array
+ \param n : number of dots, array dimension
+ \param I : image
+ \param cogs : vector of vpImagePoint that will be updated with the new dots, will be displayed in green
+ \param cogStar (optional) : array of vpImagePoint indicating the desired position (default NULL), will be displayed in red
+*/
+void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I, std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar)
+{
+ unsigned int i;
+ // tracking
+ for(i=0;i<n;++i)
+ {
+ dot[i].track(I);
+ cogs.push_back(dot[i].getCog());
+ }
+ // trajectories
+ for(i=n;i<cogs.size();++i)
+ vpDisplay::displayCircle(I,cogs[i],4,vpColor::green,true);
+ // initial position
+ for(i=0;i<n;++i)
+ vpDisplay::displayCircle(I,cogs[i],4,vpColor::blue,true);
+ // if exists, desired position
+ if(cogStar != NULL)
+ for(i=0;i<n;++i)
+ {
+ vpDisplay::displayDotLine(I,cogStar[i],dot[i].getCog(),vpColor::red);
+ vpDisplay::displayCircle(I,cogStar[i],4,vpColor::red,true);
+ }
+ vpDisplay::flush(I);
+}
+
+/*!
+
+ Display the dot center of gravity and its list of edges.
+
+ \param I : The image used as background.
+
+ \param cog : The center of gravity.
+
+ \param edges_list : The list of edges;
+
+ \param color : Color used to display the dot.
+
+ \param thickness : Thickness of the dot.
+*/
+void vpDot2::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color,
+ unsigned int thickness)
+{
+ vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = edges_list.begin(); it != edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+/*!
+
+ Display the dot center of gravity and its list of edges.
+
+ \param I : The image used as background.
+
+ \param cog : The center of gravity.
+
+ \param edges_list : The list of edges;
+
+ \param color : Color used to display the dot.
+
+ \param thickness : Thickness of the dot.
+*/
+void vpDot2::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color,
+ unsigned int thickness)
+{
+ vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ std::list<vpImagePoint>::const_iterator it;
+
+ for (it = edges_list.begin(); it != edges_list.end(); ++it)
+ {
+ vpDisplay::displayPoint(I, *it, color);
+ }
+}
+
+/*!
+ Writes the dot center of gravity coordinates in the frame (i,j) (For more details
+ about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
+ and returns a reference to the stream.
+*/
+VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d) {
+ return (os << "(" << d.getCog() << ")" ) ;
+}
diff --git a/modules/tracker/blob/test/tracking/testTrackDot.cpp b/modules/tracker/blob/test/tracking/testTrackDot.cpp
new file mode 100644
index 0000000..fd52bf1
--- /dev/null
+++ b/modules/tracker/blob/test/tracking/testTrackDot.cpp
@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test auto detection of dots.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpConfig.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sstream>
+#include <iomanip>
+#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/blob/vpDot2.h>
+#ifdef VISP_HAVE_MODULE_FEATURES
+# include <visp3/visual_features/vpFeatureEllipse.h>
+# include <visp3/visual_features/vpFeatureBuilder.h>
+#endif
+
+/*!
+ \example testTrackDot.cpp
+
+ \brief Test dot tracking on an image sequence by using vpDot.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:h"
+
+bool getOptions(int argc, const char **argv, std::string &ipath,
+ bool &click_allowed, bool &display);
+
+void usage(const char *name, const char *badparam, std::string ipath);
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+
+*/
+void usage(const char *name, const char *badparam, std::string ipath)
+{
+ fprintf(stdout, "\n\
+Test dot tracking.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read image \n\
+ \"ViSP-images/ellipse/ellipse.pgm\"\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n",
+ ipath.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+
+}
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath : Input image path.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath,
+ bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'i': ipath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath); return false; break;
+
+ default:
+ usage(argv[0], optarg_, ipath);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string dirname;
+ std::string filename;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+
+ // 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<unsigned char> I ;
+
+ // Set the path location of the image sequence
+ dirname = vpIoTools::createFilePath(ipath, "ViSP-images/ellipse");
+
+ // Build the name of the image file
+ filename = vpIoTools::createFilePath(dirname, "ellipse.pgm");
+
+ // Read the PGM image named "filename" on the disk, and put the
+ // bitmap into the image structure I. I is initialized to the
+ // correct size
+ //
+ // exception readPGM may throw various exception if, for example,
+ // the file does not exist, or if the memory cannot be allocated
+ try{
+ vpCTRACE << "Load: " << filename << std::endl;
+
+ vpImageIo::read(I, filename) ;
+ }
+ catch(...)
+ {
+ // an exception is throwned if an exception from readPGM has been catched
+ // here this will result in the end of the program
+ // Note that another error message has been printed from readPGM
+ // to give more information about the error
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot read " << filename << std::endl;
+ std::cerr << " Check your -i " << ipath << " option " << std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable."
+ << std::endl;
+ exit(-1);
+ }
+
+ // We open a window using either X11, GTK or GDI.
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#endif
+
+ if (opt_display) {
+ // Display size is automatically defined by the image (I) size
+ display.init(I, 100, 100,"Display...") ;
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(I) ;
+ //Flush the display
+ vpDisplay::flush(I) ;
+ }
+
+ vpDot2 dot ;
+ std::cout << "debut 1\n";
+ //dot.setMaxDotSize(0.50); // dot max size = 50% of the image size
+ vpImagePoint ip;
+ ip.set_i( 140 );
+ ip.set_j( 140 );
+ dot.initTracking(I, ip);
+ if (opt_display) {
+ dot.setGraphics(true) ;
+ }
+ else {
+ dot.setGraphics(false) ;
+ }
+ dot.setComputeMoments(true);
+ dot.track(I) ;
+
+ vpCameraParameters cam;
+
+#ifdef VISP_HAVE_MODULE_FEATURES
+ vpFeatureEllipse e;
+ vpFeatureBuilder::create(e,cam,dot);
+#endif
+ if (opt_display) {
+#ifdef VISP_HAVE_MODULE_FEATURES
+ e.display(cam, I, vpColor::red) ;
+#endif
+ vpDisplay::flush(I);
+ if (opt_click_allowed) {
+ std::cout << "A click to exit..." << std::endl;
+ vpDisplay::getClick(I) ;
+ }
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+#else
+int
+main()
+{
+ vpERROR_TRACE("You do not have X11, GTK or GDI display functionalities...");
+}
+
+#endif
diff --git a/modules/tracker/klt/CMakeLists.txt b/modules/tracker/klt/CMakeLists.txt
new file mode 100644
index 0000000..23e2acc
--- /dev/null
+++ b/modules/tracker/klt/CMakeLists.txt
@@ -0,0 +1,42 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(klt visp_core)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+vp_add_tests()
diff --git a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h
new file mode 100644
index 0000000..1e03b39
--- /dev/null
+++ b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h
@@ -0,0 +1,412 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
+ * with opencv.
+ *
+ * Authors:
+ * Fabien Servant
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpKltOpencv.h
+
+ \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
+ implemented with opencv.
+*/
+
+#ifndef vpKltOpencv_h
+#define vpKltOpencv_h
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408))
+
+#include <opencv2/video/tracking.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+/*!
+ \class vpKltOpencv
+
+ \ingroup module_klt
+
+ \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
+ implemented in 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:
+ vpKltOpencv();
+ vpKltOpencv(const vpKltOpencv& copy);
+ virtual ~vpKltOpencv();
+
+ void addFeature(const float &x, const float &y);
+ void addFeature(const long &id, const float &x, const float &y);
+ void addFeature(const cv::Point2f &f);
+
+ void display(const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::red, unsigned int thickness=1);
+ static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
+ const vpColor &color = vpColor::green, unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
+ const vpColor &color = vpColor::green, unsigned int thickness=1);
+ static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
+ const std::vector<long> &featuresid,
+ const vpColor &color = vpColor::green, unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
+ const std::vector<long> &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;}
+ void getFeature(const int &index, int &id, float &x, float &y) const;
+ //! Get the list of current features.
+ std::vector<cv::Point2f> getFeatures() const {return m_points[1];}
+ //CvPoint2D32f* getFeatures() const {return features;}
+ //! Get the unique id of each feature.
+ std::vector<long> getFeaturesId() const {return m_points_id;}
+ //long* getFeaturesId() const {return featuresid;}
+ //! Get the free parameter of the Harris detector.
+ double getHarrisFreeParameter() const {return m_harris_k;}
+ //! Get the list of lost feature
+ //bool *getListOfLostFeature() const { return lostDuringTrack; }
+ //! Get the maximum number of features to track in the image.
+ int getMaxFeatures() const {return m_maxCount;}
+ //! Get the minimal Euclidean distance between detected corners during initialization.
+ double getMinDistance() const {return m_minDistance;}
+ //! Get the number of current features
+ int getNbFeatures() const { return (int)m_points[1].size(); }
+ //! Get the number of previous features.
+ int getNbPrevFeatures() const { return (int)m_points[0].size(); }
+ //void getPrevFeature(int index, int &id, float &x, float &y) const;
+ //! Get the list of previous features
+ std::vector<cv::Point2f> getPrevFeatures() const {return m_points[0];}
+ //CvPoint2D32f* getPrevFeatures() const {return prev_features;}
+ //! Get the list of features id
+ //long* getPrevFeaturesId() const {return prev_featuresid;}
+ //! Get the maximal pyramid level.
+ int getPyramidLevels() const {return m_pyrMaxLevel;}
+ //! Get the parameter characterizing the minimal accepted quality of image corners.
+ double getQuality() const {return m_qualityLevel;}
+ //! Get the window size used to refine the corner locations.
+ int getWindowSize() const {return m_winSize;}
+
+ void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat());
+ void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts);
+ void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids);
+
+ vpKltOpencv & operator=(const vpKltOpencv& copy);
+ void track(const cv::Mat &I);
+ void setBlockSize(const int blockSize);
+ void setHarrisFreeParameter(double harris_k);
+ void setInitialGuess(const std::vector<cv::Point2f> &guess_pts);
+ void setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid);
+ void setMaxFeatures(const int maxCount);
+ void setMinDistance(double minDistance);
+ void setMinEigThreshold(double minEigThreshold);
+ void setPyramidLevels(const int pyrMaxLevel);
+ void setQuality(double 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(const int useHarrisDetector);
+ void setWindowSize(const int winSize);
+ void suppressFeature(const int &index);
+
+protected:
+ cv::Mat m_gray, m_prevGray;
+ std::vector<cv::Point2f> m_points[2]; //!< Previous [0] and current [1] keypoint location
+ std::vector<long> 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;
+
+};
+
+#elif defined(VISP_HAVE_OPENCV)
+#ifdef _CH_
+#pragma package <opencv>
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/video/tracking.hpp>
+# include <opencv2/legacy/legacy.hpp>
+# include <opencv2/highgui/highgui.hpp>
+#else
+# ifndef _EiC
+# include <cv.h>
+# include <highgui.h>
+# include <stdio.h>
+# include <ctype.h>
+# endif
+#endif
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpTrackingException.h>
+
+typedef int (*funccheck)(int,double,double);
+typedef void (*funcinfo)(int,int,int,double,double);
+typedef void (*funcevent)(int);
+
+/*!
+ \class vpKltOpencv
+
+ \ingroup module_klt
+
+ \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
+ implemented in 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
+{
+private:
+ int initialized; //Is the tracker ready ?
+
+ int maxFeatures; //Maximum number of features to track (Default 50)
+ int globalcountFeatures; //Keep over time for ID
+
+ int win_size; //Size of search window for tracker (default 10)
+ double quality; //Multiplier for the maxmin eigenvalue; specifies minimal accepted quality of image corners (default 0.01)
+ double min_distance; //Limit, specifying minimum possible distance between returned corners; Euclidian distance is used. (default 10)
+ double harris_free_parameter; //Harris detector free parameter. (default 0.04)
+ int block_size; //Size of the averaging block used by the corner detector (default 3)
+ int use_harris; //0 use a simple Minimum EigenValue Detector, != 0 use Harris (default 1)
+ int pyramid_level; //Number of level for the tracker's gaussian pyramid data (default 3)
+ int _tid; //tracker id for multiple trackers
+
+ IplImage *image; //Image buffer
+ IplImage *prev_image; //Image buffer for the previous iteration
+ IplImage *pyramid; //Gaussian pyramid data
+ IplImage *prev_pyramid; //Gaussian pyramid data for the previous iteration
+ IplImage *swap_temp; //Internal
+
+ int countFeatures; //Currently tracked features
+ int countPrevFeatures; //Previously tracked features
+
+ CvPoint2D32f *features; //List of features
+ CvPoint2D32f *prev_features; //List of features for previous iteration
+
+ long *featuresid; //Array of ids for features
+ long *prev_featuresid; //Array of ids for previous features
+
+ int flags; //Flags for tracking (internal)
+
+ bool initial_guess; //Bool to precise if the next call to track() uses an initial guess
+
+ bool *lostDuringTrack; // Result of the tracker for every feature : 1 = lost, 0 = still present
+ char *status; //Result of the tracker for every features : 0 = lost, 1 = found
+
+
+
+ //EVENT FUNCTION POINTERS
+ funcevent OnInitialize;
+ funcinfo OnFeatureLost;
+ funcinfo OnNewFeature;
+ funcinfo OnMeasureFeature;
+ funccheck IsFeatureValid;
+
+private:
+
+ //Internal
+ void clean();
+ void cleanAll();
+ void reset();
+
+public:
+ vpKltOpencv();
+ vpKltOpencv(const vpKltOpencv& copy);
+ virtual ~vpKltOpencv();
+
+ void addFeature(const int &id, const float &x, const float &y);
+
+ //Draw the tracked features on the given image
+ void display(const vpImage<unsigned char> &I,
+ vpColor color = vpColor::red, unsigned int thickness=1);
+
+ //! Get the block size
+ int getBlockSize() const {return block_size;}
+ void getFeature(int index, int &id, float &x, float &y) const;
+ //! Get the list of features
+ CvPoint2D32f* getFeatures() const {return features;}
+ //! Get the list of features id
+ long* getFeaturesId() const {return featuresid;}
+ //! Get Harris free parameter
+ double getHarrisFreeParameter() const {return harris_free_parameter;}
+ //! Get the list of lost feature
+ bool *getListOfLostFeature() const { return lostDuringTrack; }
+ //! Get Max number of features
+ int getMaxFeatures() const {return maxFeatures;}
+ //! Get Min Distance
+ double getMinDistance() const {return min_distance;}
+ //! Get the current number of features
+ int getNbFeatures() const { return countFeatures; }
+ //! Get the previous number of features
+ int getNbPrevFeatures() const { return countPrevFeatures; }
+ void getPrevFeature(int index, int &id, float &x, float &y) const;
+ //! Get the list of features
+ CvPoint2D32f* getPrevFeatures() const {return prev_features;}
+ //! Get the list of features id
+ long* getPrevFeaturesId() const {return prev_featuresid;}
+ //! Get the number of pyramid levels
+ int getPyramidLevels() const {return pyramid_level;}
+ //! Get the quality of the tracker
+ double getQuality() const {return quality;}
+ //! Get Max number of features
+ int getWindowSize() const {return win_size;}
+
+ //Detect corners in the image. Initialize the tracker
+ void initTracking(const IplImage *I, const IplImage *mask = NULL);
+ void initTracking(const IplImage *I, CvPoint2D32f *pts, int size);
+ void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size);
+ vpKltOpencv & operator=(const vpKltOpencv& copy);
+ //Track !
+ void track(const IplImage *I);
+
+
+ //Seters
+ /*!
+ 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 input : The new size of the block.
+ */
+ void setBlockSize(const int input) {initialized = 0; block_size=input;}
+ /*!
+ Set the Harris parameter (The \e k value).
+
+ \warning The tracker must be re-initialised using the method initTracking().
+
+ \param input : The new Harris parameter.
+ */
+ void setHarrisFreeParameter(double input) {initialized = 0; harris_free_parameter=input;}
+ void setInitialGuess(CvPoint2D32f **guess_pts);
+ void setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size);
+ /*!
+ Is a feature valid (e.g. : test if not too close to borders) -> event(id_tracker, x, y)
+ */
+ void setIsFeatureValid(funccheck input) {IsFeatureValid = input;}
+
+ /* Should be used only before initTracking */
+ void setMaxFeatures(const int input);
+ /*!
+ Set the minimal distance between two points during the initialisation.
+
+ \warning The tracker must be re-initialised using the method initTracking().
+
+ \param input : The new minimal distance between two points.
+ */
+ void setMinDistance(double input) {initialized = 0; min_distance=input;}
+
+ //Functors
+
+ //Event when tracker is initialized -> event(id_tracker)
+ void setOnInitialize(funcevent input) {OnInitialize = input;}
+ //Event when a feature is lost -> event(id_tracker, index, uid, x, y)
+ void setOnFeatureLost(funcinfo input) {OnFeatureLost = input;}
+ //Event when a new feature is found -> event(id_tracker, index, uid, x, y)
+ void setOnNewFeature(funcinfo input) {OnNewFeature = input;}
+ //Event when a feature is found while tracking -> event(id_tracker, index, uid, x, y)
+ void setOnMeasureFeature(funcinfo input) {OnMeasureFeature = input;}
+ /*!
+ Set the maximal pyramid level. If the level is zero, then no pyramid is
+ computed for the optical flow.
+
+ \warning The tracker must be re-initialised using the method initTracking().
+
+ \param input : The new maximal pyramid level.
+ */
+ void setPyramidLevels(const int input) {initialized = 0; pyramid_level=input;}
+ void setQuality(double input) {initialized = 0; quality=input;}
+ void setTrackerId(int tid) {_tid = tid;}
+ /*!
+ Set the window size for the sub-pixel computation.
+
+ \warning The tracker must be re-initialised using the method initTracking().
+
+ \param input : The new number of maximum features.
+ */
+ void setUseHarris(const int input) {initialized = 0; use_harris=input;}
+ void setWindowSize(const int input) {initialized = 0; win_size=input;}
+
+ void suppressFeature(int index);
+
+ //Static Functions
+public:
+ static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
+ const int &nbFeatures, vpColor color = vpColor::green,
+ unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
+ const int &nbFeatures, vpColor color = vpColor::green,
+ unsigned int thickness=1);
+
+ static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
+ const long *featuresid_list, const int &nbFeatures,
+ vpColor color = vpColor::green, unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
+ const long *featuresid_list, const int &nbFeatures,
+ vpColor color = vpColor::green, unsigned int thickness=1);
+
+};
+
+#endif
+#endif
diff --git a/modules/tracker/klt/src/vpKltOpencv.cpp b/modules/tracker/klt/src/vpKltOpencv.cpp
new file mode 100644
index 0000000..bb5aa64
--- /dev/null
+++ b/modules/tracker/klt/src/vpKltOpencv.cpp
@@ -0,0 +1,1355 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
+ * with opencv.
+ *
+ * Authors:
+ * Fabien Servant
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpKltOpencv.cpp
+
+ \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
+ implemented with opencv.
+*/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+
+#include <string>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpTrackingException.h>
+
+/*!
+ 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), m_pyrMaxLevel(3),
+ m_next_points_id(0), m_initial_guess(false)
+{
+ m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS, 20, 0.03);
+
+}
+
+/*!
+ Copy constructor.
+ */
+vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
+ : 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), m_pyrMaxLevel(3),
+ m_next_points_id(0), m_initial_guess(false)
+{
+ *this = copy;
+}
+
+/*!
+ Copy operator.
+ */
+vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
+{
+ m_gray = copy.m_gray;
+ m_prevGray = copy.m_prevGray;
+ m_points[0] = copy.m_points[0];
+ m_points[1] = copy.m_points[1];
+ m_points_id = copy.m_points_id;
+ m_maxCount = copy.m_maxCount;
+ m_termcrit = copy.m_termcrit;
+ m_winSize = copy.m_winSize;
+ m_qualityLevel = copy.m_qualityLevel;
+ m_minDistance = copy.m_minDistance;
+ m_minEigThreshold = copy.m_minEigThreshold;
+ m_harris_k = copy.m_harris_k;
+ m_blockSize = copy.m_blockSize;
+ m_useHarrisDetector = copy.m_useHarrisDetector;
+ m_pyrMaxLevel = copy.m_pyrMaxLevel;
+ m_next_points_id = copy.m_next_points_id;
+ m_initial_guess = copy.m_initial_guess;
+
+ return *this;
+}
+
+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;
+
+ //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
+ I.copyTo(m_gray);
+
+ for (size_t i=0; i<2; i++) {
+ m_points[i].clear();
+ }
+
+ m_points_id.clear();
+
+ cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, 0, m_harris_k);
+
+ if(m_points[1].size() > 0){
+ cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1,-1), m_termcrit);
+
+ for (size_t i=0; i < m_points[1].size(); i++)
+ m_points_id.push_back(m_next_points_id++);
+ }
+}
+
+/*!
+ 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)
+ throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
+
+ std::vector<float> err;
+ int flags = 0;
+
+ cv::swap(m_prevGray, m_gray);
+
+ if (m_initial_guess) {
+ flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
+ m_initial_guess = false;
+ }
+ else {
+ std::swap(m_points[1], m_points[0]);
+ }
+
+ //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
+ I.copyTo(m_gray);
+
+ if(m_prevGray.empty()){
+ m_gray.copyTo(m_prevGray);
+ }
+
+ std::vector<uchar> status;
+
+ cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
+ m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);
+
+ // Remove points that are lost
+ for (int i=(int)status.size()-1; i>=0; i--) {
+ if (status[(size_t)i] == 0) { // point is lost
+ m_points[0].erase(m_points[0].begin()+i);
+ m_points[1].erase(m_points[1].begin()+i);
+ m_points_id.erase(m_points_id.begin()+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, int &id, float &x, float &y) const
+{
+ if ((size_t)index >= m_points[1].size()){
+ throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
+ }
+
+ x = m_points[1][(size_t)index].x;
+ y = m_points[1][(size_t)index].y;
+ 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<unsigned char> &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<unsigned char> &I, const std::vector<cv::Point2f> &features,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (size_t i = 0 ; i < features.size() ; i++) {
+ ip.set_u( vpMath::round(features[i].x ) );
+ ip.set_v( vpMath::round(features[i].y ) );
+ vpDisplay::displayCross(I, ip, 10+thickness, 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<vpRGBa> &I, const std::vector<cv::Point2f> &features,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (size_t i = 0 ; i < features.size() ; i++) {
+ ip.set_u( vpMath::round(features[i].x ) );
+ ip.set_v( vpMath::round(features[i].y ) );
+ vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
+ }
+}
+
+/*!
+
+ 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
+*/
+void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
+ const std::vector<long> &featuresid,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (size_t i = 0; i < features.size(); i++) {
+ ip.set_u( vpMath::round(features[i].x ) );
+ ip.set_v( vpMath::round(features[i].y ) );
+ vpDisplay::displayCross(I, ip, 10, color, thickness);
+
+ std::ostringstream id;
+ id << featuresid[i];
+ ip.set_u( vpMath::round( features[i].x + 5 ) );
+ vpDisplay::displayText(I, ip, id.str(), color);
+ }
+}
+
+/*!
+
+ 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
+*/
+void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
+ const std::vector<long> &featuresid,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (size_t i = 0 ; i < features.size() ; i++) {
+ ip.set_u( vpMath::round(features[i].x ) );
+ ip.set_v( vpMath::round(features[i].y ) );
+ vpDisplay::displayCross(I, ip, 10, color, thickness);
+
+ std::ostringstream id;
+ id << featuresid[i];
+ ip.set_u( vpMath::round( features[i].x + 5 ) );
+ vpDisplay::displayText(I, ip, id.str(), color);
+ }
+}
+
+/*!
+ 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 vpKltOpencv::setMaxFeatures(const int maxCount)
+{
+ m_maxCount = maxCount;
+}
+
+/*!
+ 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 vpKltOpencv::setWindowSize(const int winSize)
+{
+ m_winSize = winSize;
+}
+
+/*!
+ 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 vpKltOpencv::setQuality(double qualityLevel)
+{
+ m_qualityLevel = qualityLevel;
+}
+
+/*!
+ 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 vpKltOpencv::setHarrisFreeParameter(double harris_k)
+{
+ m_harris_k = harris_k;
+}
+
+/*!
+ 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 vpKltOpencv::setUseHarris(const int useHarrisDetector)
+{
+ m_useHarrisDetector = useHarrisDetector;
+}
+
+/*!
+ 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 vpKltOpencv::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 vpKltOpencv::setMinEigThreshold(double minEigThreshold)
+{
+ m_minEigThreshold = minEigThreshold;
+}
+
+/*!
+ 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 vpKltOpencv::setBlockSize(const int blockSize)
+{
+ m_blockSize = blockSize;
+}
+
+/*!
+ 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 vpKltOpencv::setPyramidLevels(const int pyrMaxLevel)
+{
+ m_pyrMaxLevel = pyrMaxLevel;
+}
+
+/*!
+ 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
+vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
+{
+ if(guess_pts.size() != m_points[1].size()){
+ throw(vpException(vpException::badValue,
+ "Cannot set initial guess: size feature vector [%d] and guess vector [%d] doesn't match",
+ m_points[1].size(), guess_pts.size()));
+ }
+
+ m_points[1] = 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<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid)
+{
+ if(guess_pts.size() != init_pts.size()){
+ throw(vpException(vpException::badValue,
+ "Cannot set initial guess: size init vector [%d] and guess vector [%d] doesn't match",
+ init_pts.size(), guess_pts.size()));
+ }
+
+ m_points[0] = init_pts;
+ m_points[1] = guess_pts;
+ m_points_id = fid;
+ 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<cv::Point2f> &pts)
+{
+ m_initial_guess = false;
+ m_points[1] = pts;
+ m_next_points_id = 0;
+ m_points_id.clear();
+ for(size_t i=0; i < m_points[1].size(); i++) {
+ m_points_id.push_back(m_next_points_id ++);
+ }
+
+ I.copyTo(m_gray);
+}
+
+void
+vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
+{
+ m_initial_guess = false;
+ m_points[1] = pts;
+ m_points_id.clear();
+
+ if(ids.size() != pts.size()){
+ m_next_points_id = 0;
+ for(size_t i=0; i < m_points[1].size(); i++)
+ m_points_id.push_back(m_next_points_id ++);
+ }
+ else{
+ long max = 0;
+ for(size_t i=0; i < m_points[1].size(); i++){
+ m_points_id.push_back(ids[i]);
+ if(ids[i] > max) max = ids[i];
+ }
+ m_next_points_id = max + 1;
+ }
+
+ 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);
+ m_points[1].push_back(f);
+ 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);
+ m_points[1].push_back(f);
+ m_points_id.push_back(id);
+ if (id >= m_next_points_id)
+ 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()){
+ throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
+ }
+
+ m_points[1].erase(m_points[1].begin()+index);
+ m_points_id.erase(m_points_id.begin()+index);
+}
+
+
+#elif defined(VISP_HAVE_OPENCV)
+
+#include <string>
+
+#include <visp3/klt/vpKltOpencv.h>
+
+void vpKltOpencv::clean()
+{
+ if (image) cvReleaseImage(&image);
+ if (prev_image) cvReleaseImage(&prev_image);
+ if (pyramid) cvReleaseImage(&pyramid);
+ if (prev_pyramid) cvReleaseImage(&prev_pyramid);
+
+ image = NULL;
+ prev_image = NULL;
+ pyramid = NULL;
+ prev_pyramid = NULL;
+
+ swap_temp = NULL;
+ countFeatures = 0;
+ countPrevFeatures = 0;
+ flags = 0;
+ initialized = 0;
+ globalcountFeatures = 0;
+}
+
+void vpKltOpencv::cleanAll()
+{
+ clean();
+ if (features) cvFree(&features);
+ if (prev_features) cvFree(&prev_features);
+ if (status) cvFree(&status);
+ if (lostDuringTrack) cvFree(&lostDuringTrack);
+ if (featuresid) cvFree(&featuresid);
+ if (prev_featuresid) cvFree(&prev_featuresid);
+ features = NULL;
+ prev_features = NULL;
+ status = NULL;
+ lostDuringTrack = 0;
+ featuresid = NULL;
+ prev_featuresid = NULL;
+}
+
+void vpKltOpencv::reset()
+{
+ clean();
+
+}
+
+/*!
+ Default constructor.
+ */
+vpKltOpencv::vpKltOpencv()
+ : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
+ min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
+ pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
+ prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
+ features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
+ flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
+ OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
+{
+ features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(features[0]));
+ prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(prev_features[0]));
+ status = (char*)cvAlloc((size_t)maxFeatures);
+ lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
+ featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+ prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+}
+
+/*!
+ Copy constructor.
+ */
+vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
+ : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
+ min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
+ pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
+ prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
+ features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
+ flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
+ OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
+{
+ *this = copy;
+}
+
+/*!
+ Copy operator.
+ */
+vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
+{
+ //Shallow copy of primitives
+ initialized = copy.initialized;
+ maxFeatures = copy.maxFeatures;
+ countFeatures = copy.countFeatures;
+ countPrevFeatures = copy.countPrevFeatures;
+ globalcountFeatures = copy.globalcountFeatures;
+ flags = copy.flags;
+ win_size = copy.win_size;
+ quality = copy.quality;
+ min_distance = copy.min_distance;
+ harris_free_parameter = copy.harris_free_parameter;
+ block_size = copy.block_size;
+ use_harris = copy.use_harris;
+ pyramid_level = copy.pyramid_level;
+ _tid = copy._tid;
+
+ OnInitialize = copy.OnInitialize;
+ OnFeatureLost = copy.OnFeatureLost;
+ OnNewFeature = copy.OnNewFeature;
+ OnMeasureFeature = copy.OnMeasureFeature;
+ IsFeatureValid = copy.IsFeatureValid;
+
+ initial_guess = copy.initial_guess;
+ lostDuringTrack = copy.lostDuringTrack;
+
+ if (!initialized) {
+ status = 0;
+ lostDuringTrack = 0;
+ countFeatures = 0;
+ countPrevFeatures = 0;
+ flags = 0;
+ initialized = 0;
+ globalcountFeatures = 0;
+ }
+
+ if (copy.image)
+ {
+ image = cvCreateImage(cvGetSize(copy.image), 8, 1);
+ // /*IplImage **/cvCopyImage(copy.image,image);
+ cvCopy(copy.image, image, 0);
+ }
+
+ if (copy.prev_image)
+ {
+ prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
+ // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
+ cvCopy(copy.prev_image, prev_image, 0);
+ }
+
+ if (copy.pyramid)
+ {
+ pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
+ // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
+ cvCopy(copy.pyramid, pyramid, 0);
+ }
+
+ if (copy.prev_pyramid)
+ {
+ prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
+ // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
+ cvCopy(copy.prev_pyramid, prev_pyramid, 0);
+ }
+
+ //Deep copy of arrays
+ if (copy.features) {
+ /*CvPoint2D32f **/features =
+ (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ features[i] = copy.features[i];
+ }
+
+ if (copy.prev_features) {
+ /*CvPoint2D32f **/prev_features =
+ (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ prev_features[i] = copy.prev_features[i];
+ }
+
+ if (copy.featuresid) {
+ /*long **/featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ featuresid[i] = copy.featuresid[i];
+ }
+
+ if (copy.prev_featuresid) {
+ /*long **/prev_featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ prev_featuresid[i] = copy.prev_featuresid[i];
+ }
+
+ if (copy.status) {
+ /*char **/status = (char*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(char));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ status[i] = copy.status[i];
+ }
+
+ if (copy.lostDuringTrack) {
+ /*bool **/lostDuringTrack = (bool*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(bool));
+ for (int i = 0; i < copy.maxFeatures; i++)
+ lostDuringTrack[i] = copy.lostDuringTrack[i];
+ }
+
+ return *this;
+}
+
+vpKltOpencv::~vpKltOpencv()
+{
+ cleanAll();
+}
+
+/*!
+ Set the maximum number of features to track in the image.
+
+ \warning The tracker must be re-initialised using the method initTracking().
+
+ \param input : The new number of maximum features.
+*/
+void vpKltOpencv::setMaxFeatures(const int input) {
+ initialized = 0; maxFeatures=input;
+
+ if (features) cvFree(&features);
+ if (prev_features) cvFree(&prev_features);
+ if (status) cvFree(&status);
+ if (lostDuringTrack) cvFree(&lostDuringTrack);
+ if (featuresid) cvFree(&featuresid);
+ if (prev_featuresid) cvFree(&prev_featuresid);
+
+
+ features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
+ prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
+ status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
+ lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
+ featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+ prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+}
+
+/*!
+ 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 IplImage *I, const IplImage *mask)
+{
+ if (!I) {
+ throw(vpException(vpTrackingException::initializationError, "Image Not initialized")) ;
+ }
+
+ if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
+ }
+
+ if (mask) {
+ if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
+ }
+ }
+
+ //Creation des buffers
+ CvSize Sizeim, SizeI;
+ SizeI = cvGetSize(I);
+ bool b_imOK = true;
+ if(image != NULL){
+ Sizeim = cvGetSize(image);
+ if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ }
+ if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
+ reset();
+ image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
+ prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ }else{
+ swap_temp = 0;
+ countFeatures = 0;
+ countPrevFeatures = 0;
+ flags = 0;
+ initialized = 0;
+ globalcountFeatures = 0;
+ }
+
+ initialized = 1;
+
+ //Import
+ cvCopy(I, image, 0);
+
+ //Recherche de points d'interets
+ countFeatures = maxFeatures;
+ countPrevFeatures = 0;
+ IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
+ IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
+ cvGoodFeaturesToTrack(image, eig, temp, features,
+ &countFeatures, quality, min_distance,
+ mask, block_size, use_harris, harris_free_parameter);
+ cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
+ cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
+ CV_TERMCRIT_EPS,20,0.03));
+ cvReleaseImage(&eig);
+ cvReleaseImage(&temp);
+
+ if (OnInitialize)
+ OnInitialize(_tid);
+
+ //printf("Number of features at init: %d\n", countFeatures);
+ for (int boucle=0; boucle<countFeatures;boucle++) {
+ featuresid[boucle] = globalcountFeatures;
+ globalcountFeatures++;
+
+ if (OnNewFeature){
+ OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
+ features[boucle].y);
+ }
+ }
+}
+
+/*!
+ 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 IplImage *I, CvPoint2D32f *pts, int size)
+{
+ if (size > maxFeatures)
+ throw(vpException(vpTrackingException::initializationError,
+ "Cannot initialize tracker from points"));
+
+ //Creation des buffers
+ CvSize Sizeim, SizeI;
+ SizeI = cvGetSize(I);
+ bool b_imOK = true;
+ if(image != NULL){
+ Sizeim = cvGetSize(image);
+ if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ }
+ if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
+ reset();
+ image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
+ prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ } else {
+ flags = 0;
+ }
+ // Save current features as previous features
+ countFeatures = size;
+ for (int i=0; i<countFeatures;i++) {
+ features[i] = pts[i];
+ featuresid[i] = i;
+ }
+
+ globalcountFeatures = size;
+ initialized = 1;
+
+ cvCopy(I, image, 0);
+}
+
+void
+vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
+{
+ if (size > maxFeatures)
+ throw(vpException(vpTrackingException::initializationError,
+ "Cannot initialize tracker from points"));
+
+ //Creation des buffers
+ CvSize Sizeim, SizeI;
+ SizeI = cvGetSize(I);
+ bool b_imOK = true;
+ if(image != NULL){
+ Sizeim = cvGetSize(image);
+ if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ }
+ if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
+ reset();
+ image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
+ prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
+ } else {
+ flags = 0;
+ }
+ // Save current features as previous features
+ countFeatures = size;
+ long max = 0;
+ for (int i=0; i<countFeatures;i++) {
+ features[i] = pts[i];
+ featuresid[i] = fid[i];
+ if (fid[i] > max)
+ max = fid[i];
+ }
+
+ globalcountFeatures = max + 1;
+ initialized = 1;
+
+ cvCopy(I, image, 0);
+}
+
+void vpKltOpencv::track(const IplImage *I)
+{
+ if (!initialized) {
+ vpERROR_TRACE("KLT Not initialized") ;
+ throw(vpException(vpTrackingException::initializationError,
+ "KLT Not initialized")) ;
+ }
+
+ if (!I) {
+ throw(vpException(vpTrackingException::initializationError,
+ "Image Not initialized")) ;
+ }
+
+ if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError,
+ "Bad Image format")) ;
+ }
+
+
+
+ CV_SWAP(prev_image, image, swap_temp);
+ CV_SWAP(prev_pyramid, pyramid, swap_temp);
+
+ cvCopy(I, image, 0);
+
+ if(!initial_guess){
+ // Save current features as previous features
+ countPrevFeatures = countFeatures;
+ for (int boucle=0; boucle<countFeatures;boucle++) {
+ prev_featuresid[boucle] = featuresid[boucle];
+ }
+
+ CvPoint2D32f *swap_features = 0;
+ CV_SWAP(prev_features, features, swap_features);
+ }
+
+ if (countFeatures <= 0) return;
+
+ cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
+ prev_features, features, countFeatures,
+ cvSize(win_size, win_size), pyramid_level,
+ status, 0, cvTermCriteria(CV_TERMCRIT_ITER
+ |CV_TERMCRIT_EPS,20,0.03),
+ flags );
+
+ if(!initial_guess)
+ flags |= CV_LKFLOW_PYR_A_READY;
+ else{
+ flags = CV_LKFLOW_PYR_A_READY;
+ initial_guess = false;
+ }
+
+ int i,k;
+ for (i = k = 0; i < countFeatures ; i++) {
+ if (!status[i]) {
+ lostDuringTrack[i] = 1;
+ if (OnFeatureLost)
+ OnFeatureLost(_tid, i, featuresid[i], features[i].x,
+ features[i].y);
+ continue;
+ }
+
+ if (IsFeatureValid) {
+ if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
+ lostDuringTrack[i] = 1;
+ if (OnFeatureLost)
+ OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
+ continue;
+ }
+ }
+ features[k] = features[i];
+ featuresid[k] = featuresid[i];
+
+ if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
+
+ lostDuringTrack[i] = 0;
+ k++;
+ }
+ countFeatures = k;
+}
+
+/*!
+ 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<unsigned char> &I,
+ vpColor color, unsigned int thickness)
+{
+ if ((features == 0) || (I.bitmap==0) || (!initialized))
+ {
+ vpERROR_TRACE(" Memory problem ");
+ throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ }
+
+ vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
+}
+
+/*!
+
+ 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(int index, int &id, float &x, float &y) const
+{
+ if (index >= countFeatures)
+ {
+ vpERROR_TRACE(" Memory problem ");
+ throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ }
+
+ x = features[index].x;
+ y = features[index].y;
+ id = featuresid[index];
+}
+
+
+/*!
+ Set the points that will be used as initial guess during the next call to track().
+
+ \warning Those points will be used just one time (next track()).
+
+ \param guess_pts : Reference on an array of CvPoint2D32f allocated with cvAlloc().
+*/
+void
+vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
+{
+ // Save current features as previous features
+ countPrevFeatures = countFeatures;
+ for (int boucle=0; boucle<countFeatures;boucle++) {
+ prev_featuresid[boucle] = featuresid[boucle];
+ }
+
+ CvPoint2D32f *swap_features = NULL;
+ CV_SWAP(prev_features, *guess_pts, swap_features);
+
+ CV_SWAP(features, prev_features, swap_features);
+
+ flags |= CV_LKFLOW_INITIAL_GUESSES;
+
+ 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.
+ \param size : size of the vectors.
+
+ \sa getPrevFeatures(),getPrevFeaturesId
+ \sa getFeatures(), getFeaturesId
+ \sa initTracking()
+*/
+void
+vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
+{
+ countPrevFeatures = size;
+ countFeatures = size;
+ for (int boucle=0; boucle<size;boucle++) {
+ prev_featuresid[boucle] = fid[boucle];
+ featuresid[boucle] = fid[boucle];
+ }
+
+ CvPoint2D32f *swap_features = NULL;
+ CvPoint2D32f *swap_features2 = NULL;
+ CV_SWAP(prev_features, *init_pts, swap_features);
+
+// if(swap_features) cvFree(&swap_features);
+// swap_features = NULL;
+
+ CV_SWAP(features, *guess_pts, swap_features2);
+
+ flags |= CV_LKFLOW_INITIAL_GUESSES;
+
+ initial_guess = true;
+}
+
+/*!
+
+ Get the 'index'th previous feature image coordinates. Beware that
+ getPrevFeature(i,...) may not represent the same feature before and
+ after a tracking iteration (if a feature is lost, features are
+ shifted in the array).
+
+*/
+void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
+{
+ if (index >= countPrevFeatures)
+ {
+ vpERROR_TRACE(" Memory problem ");
+ throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ }
+
+ x = prev_features[index].x;
+ y = prev_features[index].y;
+ id = prev_featuresid[index];
+}
+
+/*!
+
+Add at the end of the feauture list.
+
+If there is no space left, the feature is not added (just return)
+*/
+void vpKltOpencv::addFeature(const int &id,
+ const float &x, const float &y)
+{
+ if (maxFeatures == countFeatures)
+ {
+ vpERROR_TRACE(" Cannot add the feature ");
+ return;
+ }
+
+ CvPoint2D32f f;
+ f.x = x;
+ f.y = y;
+ features[countFeatures] = f;
+ featuresid[countFeatures] = id;
+ countFeatures ++;
+}
+
+void vpKltOpencv::suppressFeature(int index)
+{
+ if (index >= countFeatures)
+ {
+ vpERROR_TRACE(" Memory problem ");
+ throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ }
+
+ countFeatures --;
+
+ for (int i=index ; i < countFeatures; i ++) {
+ features[i] = features[i+1];
+ featuresid[i] = featuresid[i+1];
+ }
+}
+
+/*!
+
+ Display features list.
+
+ \param I : The image used as background.
+
+ \param features_list : List of features
+
+ \param nbFeatures : Number of features
+
+ \param color : Color used to display the points.
+
+ \param thickness : Thickness of the points.
+*/
+void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
+ const int &nbFeatures, vpColor color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (int i = 0 ; i < nbFeatures ; i++)
+ {
+ ip.set_u( vpMath::round(features_list[i].x ) );
+ ip.set_v( vpMath::round(features_list[i].y ) );
+ vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+ }
+}
+/*!
+
+ Display features list.
+
+ \param I : The image used as background.
+
+ \param features_list : List of features
+
+ \param nbFeatures : Number of features
+
+ \param color : Color used to display the points.
+
+ \param thickness : Thickness of the points.
+*/
+void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
+ const int &nbFeatures, vpColor color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (int i = 0 ; i < nbFeatures ; i++)
+ {
+ ip.set_u( vpMath::round(features_list[i].x ) );
+ ip.set_v( vpMath::round(features_list[i].y ) );
+ vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+ }
+}
+
+/*!
+
+ Display features list with ids.
+
+ \param I : The image used as background.
+
+ \param features_list : List of features
+
+ \param featuresid_list : List of ids corresponding to the features list
+
+ \param nbFeatures : Number of features
+
+ \param color : Color used to display the points.
+
+ \param thickness : Thickness of the points
+*/
+void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
+ const long *featuresid_list, const int &nbFeatures,
+ vpColor color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (int i = 0 ; i < nbFeatures ; i++)
+ {
+ ip.set_u( vpMath::round(features_list[i].x ) );
+ ip.set_v( vpMath::round(features_list[i].y ) );
+ vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+
+ char id[10];
+ sprintf(id, "%ld", featuresid_list[i]);
+ ip.set_u( vpMath::round( features_list[i].x + 5 ) );
+ vpDisplay::displayText(I, ip, id, color);
+ }
+}
+
+/*!
+
+ Display features list with ids.
+
+ \param I : The image used as background.
+
+ \param features_list : List of features
+
+ \param featuresid_list : List of ids corresponding to the features list
+
+ \param nbFeatures : Number of features
+
+ \param color : Color used to display the points.
+
+ \param thickness : Thickness of the points
+*/
+void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
+ const long *featuresid_list, const int &nbFeatures,
+ vpColor color, unsigned int thickness)
+{
+ vpImagePoint ip;
+ for (int i = 0 ; i < nbFeatures ; i++)
+ {
+ ip.set_u( vpMath::round(features_list[i].x ) );
+ ip.set_v( vpMath::round(features_list[i].y ) );
+ vpDisplay::displayCross(I, ip, 10, color, thickness) ;
+
+ char id[10];
+ sprintf(id, "%ld", featuresid_list[i]);
+ ip.set_u( vpMath::round( features_list[i].x + 5 ) );
+ vpDisplay::displayText(I, ip, id, color);
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_vision.a(vpKltOpencv.cpp.o) has no symbols
+void dummy_vpKltOpencv() {};
+#endif
diff --git a/modules/tracker/mbt/CMakeLists.txt b/modules/tracker/mbt/CMakeLists.txt
new file mode 100644
index 0000000..13cb8da
--- /dev/null
+++ b/modules/tracker/mbt/CMakeLists.txt
@@ -0,0 +1,115 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# Add optional 3rd parties
+set(opt_incs "")
+set(opt_libs "")
+
+# If
+if(USE_COIN3D AND NOT HAVE_visp_ar)
+ if(WIN32)
+ add_definitions("-DCOIN_DLL")
+ endif()
+ list(APPEND opt_incs ${COIN3D_INCLUDE_DIRS})
+ # On OSX cmake 2.8 found OpenGL but OPENGL_INCLUDE_DIR was set to NOT_FOUND
+ # We add a test to be sure that the OPENGL vars exist.
+ if(OPENGL_INCLUDE_DIR)
+ list(APPEND opt_incs ${OPENGL_INCLUDE_DIR})
+ endif()
+ if(OPENGL_LIBRARIES)
+ list(APPEND opt_libs ${OPENGL_LIBRARIES})
+ endif()
+
+ list(APPEND opt_libs ${COIN3D_LIBRARIES})
+ if(USE_SOWIN)
+ add_definitions("-DSOWIN_DLL")
+ list(APPEND opt_incs ${SOWIN_INCLUDE_DIRS})
+ list(APPEND opt_libs ${SOWIN_LIBRARIES})
+ endif()
+
+ if(USE_SOQT AND USE_QT)
+ list(APPEND opt_incs ${SOQT_INCLUDE_DIRS})
+ list(APPEND opt_incs ${QT_INCLUDE_DIR})
+ list(APPEND opt_incs ${QT_INCLUDES})
+ list(APPEND opt_libs ${SOQT_LIBRARIES})
+ if(WIN32)
+ add_definitions("-DSOQT_DLL")
+ endif()
+
+ # We manage QT libraries
+ if(DESIRED_QT_VERSION MATCHES 3)
+ #Add Qt3 libraries
+ set(VISP_HAVE_QT3_FOUND "yes") # for ViSP-third-party.txt
+ #message("QT_QT_LIBRARY ${QT_QT_LIBRARY}")
+ list(APPEND opt_libs ${QT_QT_LIBRARY})
+ elseif(DESIRED_QT_VERSION MATCHES 4)
+ #Add Qt4 libraries
+ set(VISP_HAVE_QT4_FOUND "yes") # for ViSP-third-party.txt
+ #message("QT_QTGUI_LIBRARY ${QT_QTGUI_LIBRARY}")
+ #message("QT_QTGUI_LIBRARY_RELEASE ${QT_QTGUI_LIBRARY_RELEASE}")
+ #message("QT_QTGUI_LIBRARY_DEBUG ${QT_QTGUI_LIBRARY_DEBUG}")
+ if(QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE AND QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
+ list(APPEND opt_libs optimized ${QT_QTGUI_LIBRARY_RELEASE})
+ list(APPEND opt_libs optimized ${QT_QTCORE_LIBRARY_RELEASE})
+ list(APPEND opt_libs debug ${QT_QTGUI_LIBRARY_DEBUG})
+ list(APPEND opt_libs debug ${QT_QTCORE_LIBRARY_DEBUG})
+ elseif(QT_QTGUI_LIBRARY_RELEASE AND QT_QTCORE_LIBRARY_RELEASE)
+ list(APPEND opt_libs ${QT_QTGUI_LIBRARY_RELEASE})
+ list(APPEND opt_libs ${QT_QTCORE_LIBRARY_RELEASE})
+ elseif(QT_QTGUI_LIBRARY_DEBUG AND QT_QTCORE_LIBRARY_DEBUG)
+ list(APPEND opt_libs ${QT_QTGUI_LIBRARY_DEBUG})
+ list(APPEND opt_libs ${QT_QTCORE_LIBRARY_DEBUG})
+ endif()
+ endif()
+
+ # Because in QT_DEFINITIONS defs are separated by ";", parse the
+ # QT_DEFINITIONS in order to build a space separated string
+ vp_list_remove_separator(QT_DEFINITIONS)
+ add_definitions(${QT_DEFINITIONS})
+ add_definitions("-DQT_DLL")
+ endif(USE_SOQT AND USE_QT)
+
+ if(USE_SOXT)
+ # OpenGL and SoXt are found
+ list(APPEND opt_libs ${SOXT_LIBRARIES})
+ endif()
+endif()
+
+vp_add_module(mbt visp_vision visp_core visp_me visp_visual_features OPTIONAL visp_ar visp_klt visp_gui)
+vp_glob_module_sources()
+vp_module_include_directories(${opt_incs})
+vp_create_module(${opt_libs})
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h
new file mode 100644
index 0000000..6181ed7
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h
@@ -0,0 +1,378 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbEdgeKltTracker.h
+ \brief Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
+*/
+
+#ifndef vpMbEdgeKltTracker_HH
+#define vpMbEdgeKltTracker_HH
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpSubMatrix.h>
+#include <visp3/core/vpSubColVector.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/mbt/vpMbtEdgeKltXmlParser.h>
+#include <visp3/mbt/vpMbKltTracker.h>
+
+/*!
+ \class vpMbEdgeKltTracker
+ \ingroup group_mbt_trackers
+ \warning This class is only available if OpenCV is installed, and used.
+
+ \brief Hybrid tracker based on moving-edges and keypoints tracked using KLT
+ tracker.
+
+ The tracker requires the knowledge of the 3D model that could be provided in a vrml
+ or in a cao file. The cao format is described in loadCAOModel().
+ It may also use an xml file used to tune the behavior of the tracker and an
+ init file used to compute the pose at the very first image.
+
+ The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
+
+\code
+#include <visp/vpMbEdgeKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpException.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Hybrid Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
+ tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
+
+ while(true){
+ // Acquire a new image
+ vpDisplay::display(I);
+ tracker.track(I); // Track the object on this image
+ tracker.getPose(cMo); // Get the pose
+
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+#if defined VISP_HAVE_XML2
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+
+ The tracker can also be used without display, in that case the initial pose
+ must be known (object always at the same initial pose for example) or computed
+ using another method:
+
+\code
+#include <visp/vpMbEdgeKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpImageIo.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
+ //acquire an image
+ vpImageIo::read(I, "cube.pgm"); // Example of acquisition
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ tracker.initFromPose(I, cMo); // initialise the tracker with the given pose.
+
+ while(true){
+ // acquire a new image
+ tracker.track(I); // track the object on this image
+ tracker.getPose(cMo); // get the pose
+ }
+
+#if defined VISP_HAVE_XML2
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+
+ Finally it can be used not to track an object but just to display a model at a
+ given pose:
+
+\code
+#include <visp/vpMbEdgeKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Hybrid Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+
+ while(true){
+ // acquire a new image
+ // Get the pose using any method
+ vpDisplay::display(I);
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+#endif
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+*/
+class VISP_EXPORT vpMbEdgeKltTracker: public vpMbKltTracker, public vpMbEdgeTracker
+{
+protected:
+ //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
+ bool compute_interaction;
+ //! The gain of the virtual visual servoing stage.
+ double lambda;
+ //! The threshold used in the robust estimation of KLT.
+ double thresholdKLT;
+ //! The threshold used in the robust estimation of MBT.
+ double thresholdMBT;
+ //! The maximum iteration of the virtual visual servoing stage.
+ unsigned int maxIter;
+
+public:
+
+ vpMbEdgeKltTracker();
+ virtual ~vpMbEdgeKltTracker();
+
+ virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
+
+ /*!
+ Get the value of the gain used to compute the control law.
+
+ \return the value for the gain.
+ */
+ virtual inline double getLambda() const {return lambda;}
+
+ /*!
+ Get the maximum iteration of the virtual visual servoing stage.
+
+ \return the number of iteration
+ */
+ virtual inline unsigned int getMaxIter() const {return maxIter;}
+
+ /*!
+ Get the near distance for clipping.
+
+ \return Near clipping value.
+ */
+ virtual inline double getNearClippingDistance() const { return vpMbKltTracker::getNearClippingDistance(); }
+
+ void loadConfigFile(const char* configFile);
+ virtual void loadConfigFile(const std::string& configFile);
+
+ void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
+ const bool verbose=false);
+ void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
+ const bool verbose=false);
+ void resetTracker();
+
+ virtual void setCameraParameters(const vpCameraParameters& cam);
+
+ /*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param flags : New clipping flags.
+ */
+ virtual void setClipping(const unsigned int &flags) {vpMbEdgeTracker::setClipping(flags); }
+
+ /*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+ */
+ virtual void setFarClippingDistance(const double &dist) { vpMbEdgeTracker::setFarClippingDistance(dist); }
+
+ /*!
+ Set the value of the gain used to compute the control law.
+
+ \param gain : the desired value for the gain.
+ */
+ virtual inline void setLambda(const double gain) {this->lambda = gain; vpMbEdgeTracker::setLambda(lambda); vpMbKltTracker::setLambda(lambda);}
+
+ /*!
+ Set the maximum iteration of the virtual visual servoing stage.
+
+ \param max : the desired number of iteration
+ */
+ virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
+
+ /*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+ */
+ virtual void setNearClippingDistance(const double &dist) { vpMbEdgeTracker::setNearClippingDistance(dist); }
+
+ /*!
+ Use Ogre3D for visibility tests
+
+ \warning This function has to be called before the initialization of the tracker.
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setOgreVisibilityTest(const bool &v){
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Hybrid");
+#endif
+ }
+
+ /*!
+ Use Scanline algorithm for visibility tests
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setScanLineVisibilityTest(const bool &v){
+ vpMbEdgeTracker::setScanLineVisibilityTest(v);
+ vpMbKltTracker::setScanLineVisibilityTest(v);
+ }
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
+ /*!
+ Set if the projection error criteria has to be computed.
+
+ \param flag : True if the projection error criteria has to be computed, false otherwise
+ */
+ virtual void setProjectionErrorComputation(const bool &flag) {
+ vpMbEdgeTracker::setProjectionErrorComputation(flag);
+ }
+
+
+ virtual void testTracking(){};
+ virtual void track(const vpImage<unsigned char>& I);
+
+protected:
+ void computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt,
+ vpColVector &w_klt, const unsigned int lvl=0);
+
+ virtual void init(const vpImage<unsigned char>& I);
+ virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double r, const int idFace=0,
+ const std::string &name="");
+ virtual void initCylinder(const vpPoint&, const vpPoint &, const double r, const int idFace,
+ const std::string &name="");
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon);
+ virtual void initFaceFromLines(vpMbtPolygon &polygon);
+ unsigned int initMbtTracking(const unsigned int level=0);
+
+ bool postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
+ const unsigned int lvl=0);
+ void postTrackingMbt(vpColVector &w, const unsigned int level=0);
+
+ unsigned int trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl = 0);
+ void trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &_error,
+ vpHomogeneousMatrix& cMo, const unsigned int lvl=0);
+};
+
+#endif
+
+#endif //VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h
new file mode 100644
index 0000000..0f96a86
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h
@@ -0,0 +1,493 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbEdgeTracker.h
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+
+#ifndef vpMbEdgeTracker_HH
+#define vpMbEdgeTracker_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/mbt/vpMbtMeLine.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
+#include <visp3/mbt/vpMbtDistanceCircle.h>
+#include <visp3/mbt/vpMbtDistanceCylinder.h>
+#include <visp3/core/vpXmlParser.h>
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <list>
+
+#if defined(VISP_HAVE_COIN3D)
+//Inventor includes
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/actions/SoSearchAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/actions/SoGetMatrixAction.h>
+#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#endif
+
+#ifdef VISP_HAVE_OPENCV
+# if VISP_HAVE_OPENCV_VERSION >= 0x020101
+# include <opencv2/core/core.hpp>
+# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/imgproc/imgproc_c.h>
+# else
+# include <cv.h>
+# endif
+#endif
+
+/*!
+ \class vpMbEdgeTracker
+ \ingroup group_mbt_trackers
+ \brief Make the complete tracking of an object by using its CAD model.
+
+ This class allows to track an object or a scene given its 3D model. A
+ video can be found in the \e http://www.irisa.fr/lagadic/visp/computer-vision.html web page. The \ref tutorial-tracking-mb is also a good starting point to use this class.
+
+ The tracker requires the knowledge of the 3D model that could be provided in a vrml
+ or in a cao file. The cao format is described in loadCAOModel().
+ It may also use an xml file used to tune the behavior of the tracker and an
+ init file used to compute the pose at the very first image.
+
+ The following code shows the simplest way to use the tracker.
+
+\code
+#include <visp/vpMbEdgeTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpException.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+ vpMbEdgeTracker tracker; // Create a model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Edge Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
+ tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
+
+ while(true){
+ // Acquire a new image
+ vpDisplay::display(I);
+ tracker.track(I); // Track the object on this image
+ tracker.getPose(cMo); // Get the pose
+
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+
+ return 0;
+}
+\endcode
+
+ For application with large inter-images displacement, multi-scale tracking is also possible, by setting the number of scales used and by activating (or not) them
+ using a vector of booleans, as presented in the following code:
+
+\code
+ ...
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpCameraParameters cam;
+
+ std::vector< bool > scales(3); //Three scales used
+ scales.push_back(true); //First scale : active
+ scales.push_back(false); //Second scale (/2) : not active
+ scales.push_back(true); //Third scale (/4) : active
+ tracker.setScales(scales); // Set active scales for multi-scale tracking
+
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ ...
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+\endcode
+
+ The tracker can also be used without display, in that case the initial pose
+ must be known (object always at the same initial pose for example) or computed
+ using another method:
+
+\code
+#include <visp/vpMbEdgeTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpImageIo.h>
+
+int main()
+{
+ vpMbEdgeTracker tracker; // Create a model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
+ //acquire an image
+ vpImageIo::read(I, "cube.pgm"); // Example of acquisition
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ tracker.initFromPose(I, cMo); // initialize the tracker with the given pose.
+
+ while(true){
+ // acquire a new image
+ tracker.track(I); // track the object on this image
+ tracker.getPose(cMo); // get the pose
+ }
+
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+
+ return 0;
+}
+\endcode
+
+ Finally it can be used not to track an object but just to display a model at a
+ given pose:
+
+\code
+#include <visp/vpMbEdgeTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+ vpMbEdgeTracker tracker; // Create a model based tracker.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Edge Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+
+ while(true){
+ // acquire a new image
+ // Get the pose using any method
+ vpDisplay::display(I);
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+
+ return 0;
+}
+\endcode
+
+*/
+
+class VISP_EXPORT vpMbEdgeTracker: virtual public vpMbTracker
+{
+ protected :
+
+ /*! If this flag is true, the interaction matrix
+ extracted from the feature set is computed at each
+ iteration in the visual servoing loop.
+ */
+ int compute_interaction;
+ //! The gain of the virtual visual servoing stage.
+ double lambda;
+
+ //! The moving edges parameters.
+ vpMe me;
+ //! Vector of list of all the lines tracked (each line is linked to a list of moving edges). Each element of the vector is for a scale (element 0 = level 0 = no subsampling).
+ std::vector< std::list< vpMbtDistanceLine*> > lines;
+
+ //! Vector of the tracked circles.
+ std::vector< std::list< vpMbtDistanceCircle*> > circles;
+
+ //! Vector of the tracked cylinders.
+ std::vector< std::list< vpMbtDistanceCylinder*> > cylinders;
+
+ //! Index of the polygon to add, and total number of polygon extracted so far.
+ unsigned int nline;
+
+ //! Index of the circle to add, and total number of circles extracted so far.
+ unsigned int ncircle;
+
+ //! Index of the cylinder to add, and total number of cylinders extracted so far.
+ unsigned int ncylinder;
+
+ //! Number of polygon (face) currently visible.
+ unsigned int nbvisiblepolygone;
+
+ //! Percentage of good points over total number of points below which tracking is supposed to have failed.
+ double percentageGdPt;
+
+ //! Vector of scale level to use for the multi-scale tracking.
+ std::vector<bool> scales;
+
+ //! Pyramid of image associated to the current image. This pyramid is computed in the init() and in the track() methods.
+ std::vector< const vpImage<unsigned char>* > Ipyramid;
+
+ //! Current scale level used. This attribute must not be modified outside of the downScale() and upScale() methods, as it used to specify to some methods which set of distanceLine use.
+ unsigned int scaleLevel;
+
+public:
+
+ vpMbEdgeTracker();
+ virtual ~vpMbEdgeTracker();
+
+ void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
+
+ /*!
+ Get the value of the gain used to compute the control law.
+
+ \return the value for the gain.
+ */
+ virtual inline double getLambda() const {return lambda;}
+
+ void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level = 0);
+ void getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level = 0);
+ void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level = 0);
+
+ /*!
+ Get the moving edge parameters.
+
+ \return an instance of the moving edge parameters used by the tracker.
+ */
+ inline void getMovingEdge(vpMe &p_me ) const { p_me = this->me;}
+ /*!
+ Get the moving edge parameters.
+
+ \return an instance of the moving edge parameters used by the tracker.
+ */
+ inline vpMe getMovingEdge() const { return this->me;}
+
+ unsigned int getNbPoints(const unsigned int level=0) const;
+
+ /*!
+ Return the scales levels used for the tracking.
+
+ \return The scales levels used for the tracking.
+ */
+ std::vector<bool> getScales() const {return scales;}
+ /*!
+ \return The threshold value between 0 and 1 over good moving edges ratio. It allows to
+ decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
+ moving edges should be considered as good to have a valid pose, while 0.1 means that
+ 10% of the moving edge are enough to declare a pose valid.
+
+ \sa setGoodMovingEdgesRatioThreshold()
+ */
+ inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt;}
+
+ void loadConfigFile(const std::string &configFile);
+ void loadConfigFile(const char* configFile);
+ void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
+ const bool verbose=false);
+ void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
+ const bool verbose=false);
+ void resetTracker();
+
+ /*!
+ Set the camera parameters.
+
+ \param camera : the new camera parameters
+ */
+ virtual void setCameraParameters(const vpCameraParameters& camera) {
+ this->cam = camera;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ (*it)->setCameraParameters(cam);
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ (*it)->setCameraParameters(cam);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ (*it)->setCameraParameters(cam);
+ }
+ }
+ }
+ }
+
+ virtual void setClipping(const unsigned int &flags);
+
+ virtual void setFarClippingDistance(const double &dist);
+
+ virtual void setNearClippingDistance(const double &dist);
+
+ /*!
+ Use Ogre3D for visibility tests
+
+ \warning This function has to be called before the initialization of the tracker.
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setOgreVisibilityTest(const bool &v){
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Edge");
+#endif
+ }
+
+ /*!
+ Use Scanline algorithm for visibility tests
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setScanLineVisibilityTest(const bool &v){
+ vpMbTracker::setScanLineVisibilityTest(v);
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ (*it)->useScanLine = v;
+ }
+ }
+ }
+ }
+
+ /*!
+ Set the threshold value between 0 and 1 over good moving edges ratio. It allows to
+ decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
+ moving edges should be considered as good to have a valid pose, while 0.1 means that
+ 10% of the moving edge are enough to declare a pose valid.
+
+ \param threshold : Value between 0 and 1 that corresponds to the ratio of good
+ moving edges that is necessary to consider that the estimated pose is valid.
+ Default value is 0.4.
+
+ \sa getGoodMovingEdgesRatioThreshold()
+ */
+ void setGoodMovingEdgesRatioThreshold(const double threshold) {percentageGdPt = threshold;}
+
+ /*!
+ Set the value of the gain used to compute the control law.
+
+ \param gain : the desired value for the gain.
+ */
+ virtual inline void setLambda(const double gain) {this->lambda = gain;}
+
+ void setMovingEdge(const vpMe &me);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
+
+ void setScales(const std::vector<bool>& _scales);
+
+ void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);
+
+ void track(const vpImage<unsigned char> &I);
+
+protected:
+ bool samePoint(const vpPoint &P1, const vpPoint &P2);
+ void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace = -1, const std::string& name = "");
+ void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string& name = "");
+ void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
+ void addPolygon(vpMbtPolygon &p) ;
+ void cleanPyramid(std::vector<const vpImage<unsigned char>* >& _pyramid);
+ void computeProjectionError(const vpImage<unsigned char>& _I);
+ void computeVVS(const vpImage<unsigned char>& _I);
+ void downScale(const unsigned int _scale);
+ void init(const vpImage<unsigned char>& I);
+ virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace=0, const std::string &name="");
+ virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
+ const std::string &name="");
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon);
+ virtual void initFaceFromLines(vpMbtPolygon &polygon);
+ void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo) ;
+ void initPyramid(const vpImage<unsigned char>& _I, std::vector<const vpImage<unsigned char>* >& _pyramid);
+ void reInitLevel(const unsigned int _lvl);
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
+ void removeCircle(const std::string& name);
+ void removeCylinder(const std::string& name);
+ void removeLine(const std::string& name);
+ void resetMovingEdge();
+ void testTracking();
+ void trackMovingEdge(const vpImage<unsigned char> &I) ;
+ void updateMovingEdge(const vpImage<unsigned char> &I) ;
+ void upScale(const unsigned int _scale);
+ void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ;
+};
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h
new file mode 100644
index 0000000..653c5fb
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h
@@ -0,0 +1,892 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic model based tracker. This class declares the methods to implement in
+ * order to have a model based tracker.
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#pragma once
+
+#ifndef vpMbHiddenFaces_HH
+#define vpMbHiddenFaces_HH
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/mbt/vpMbtPolygon.h>
+#include <visp3/mbt/vpMbScanLine.h>
+
+#ifdef VISP_HAVE_OGRE
+ #include <visp3/ar/vpAROgre.h>
+#endif
+
+#include <vector>
+#include <limits>
+
+/*!
+ \class vpMbHiddenFaces
+
+ \brief Implementation of the polygons management for the model-based trackers.
+
+ \ingroup group_mbt_faces
+
+ */
+template<class PolygonType = vpMbtPolygon>
+class vpMbHiddenFaces
+{
+ private:
+ //! List of polygons
+ std::vector<PolygonType *> Lpol ;
+ //! Number of visible polygon
+ unsigned int nbVisiblePolygon;
+ vpMbScanLine scanlineRender;
+
+#ifdef VISP_HAVE_OGRE
+ vpImage<unsigned char> ogreBackground;
+ bool ogreInitialised;
+ unsigned int nbRayAttempts;
+ double ratioVisibleRay;
+ vpAROgre *ogre;
+ std::vector< Ogre::ManualObject* > lOgrePolygons;
+ bool ogreShowConfigDialog;
+#endif
+
+ unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed,
+ bool useOgre = false, bool testRoi = false,
+ const vpImage<unsigned char> &I = vpImage<unsigned char>(),
+ const vpCameraParameters &cam = vpCameraParameters()) ;
+
+ public :
+ vpMbHiddenFaces() ;
+ ~vpMbHiddenFaces() ;
+
+ void addPolygon(PolygonType *p) ;
+
+ bool computeVisibility(const vpHomogeneousMatrix &cMo,
+ const double &angleAppears, const double &angleDisappears,
+ bool &changed, bool useOgre, bool testRoi,
+ const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpTranslationVector &cameraPos,
+ unsigned int index);
+
+ void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+
+ void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);
+
+ void computeScanLineQuery(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults = false);
+
+ vpMbScanLine& getMbScanLineRenderer() { return scanlineRender; }
+
+#ifdef VISP_HAVE_OGRE
+ void displayOgre(const vpHomogeneousMatrix &cMo);
+#endif
+
+ /*!
+ Get the list of polygons.
+
+ \return Mbt Klt polygons list.
+ */
+ std::vector<PolygonType*>& getPolygon() {return Lpol;}
+
+#ifdef VISP_HAVE_OGRE
+ void initOgre(const vpCameraParameters &cam = vpCameraParameters());
+#endif
+
+ /*!
+ get the number of visible polygons.
+
+ \return number of visible polygons.
+ */
+ unsigned int getNbVisiblePolygon() const {return nbVisiblePolygon;}
+
+#ifdef VISP_HAVE_OGRE
+ /*!
+ Get the number of rays that will be sent toward each polygon for visibility test.
+ Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+
+ \sa getGoodNbRayCastingAttemptsRatio()
+
+ \return Number of rays sent.
+ */
+ unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }
+
+ /*!
+ Get the Ogre3D Context.
+
+ \return A pointer on a vpAROgre instance.
+ */
+ vpAROgre* getOgreContext(){return ogre;}
+
+ /*!
+ Get the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+
+ \sa getNbRayCastingAttemptsForVisibility()
+
+ \return Ratio of succesful attempts that has to be considered. Value will be between 0.0 (0%) and 1.0 (100%).
+ */
+ double getGoodNbRayCastingAttemptsRatio(){ return ratioVisibleRay; }
+#endif
+
+ bool isAppearing(const unsigned int i){ return Lpol[i]->isAppearing(); }
+
+
+#ifdef VISP_HAVE_OGRE
+ /*!
+ Tell whether if Ogre Context is initialised or not.
+
+ \return True if it does, false otherwise.
+ */
+ bool isOgreInitialised() { return ogreInitialised; }
+#endif
+
+ /*!
+ Check if the polygon at position i in the list is visible.
+
+ \param i : TPosition in the list.
+
+ \return Return true if the polygon is visible.
+ */
+ bool isVisible(const unsigned int i){ return Lpol[i]->isVisible(); }
+
+#ifdef VISP_HAVE_OGRE
+ bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
+#endif
+
+ //! operator[] as modifier.
+ inline PolygonType* operator[](const unsigned int i) { return Lpol[i];}
+ //! operator[] as reader.
+ inline const PolygonType* operator[](const unsigned int i) const { return Lpol[i];}
+
+ void reset();
+
+#ifdef VISP_HAVE_OGRE
+ /*!
+ Set the background size (by default it is 640x480).
+ The background size has to match with the size of the image that you are using for the traking.
+
+ \warning This function has to be called before initOgre().
+
+ \param h : Height of the background
+ \param w : Width of the background
+ */
+ void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w) { ogreBackground = vpImage<unsigned char>(h, w, 0); }
+
+ /*!
+ Set the number of rays that will be sent toward each polygon for visibility test.
+ Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+
+ \sa setGoodNbRayCastingAttemptsRatio(const double &)
+
+ \param attempts Number of rays to be sent.
+ */
+ void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }
+
+ /*!
+ Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+
+ \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
+
+ \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ */
+ void setGoodNbRayCastingAttemptsRatio(const double &ratio) {ratioVisibleRay = ratio; if(ratioVisibleRay > 1.0) ratioVisibleRay = 1.0; if(ratioVisibleRay < 0.0) ratioVisibleRay = 0.0;}
+ /*!
+ Enable/Disable the appearance of Ogre config dialog on startup.
+
+ \warning This method has only effect when Ogre is used and Ogre visibility test is
+ enabled using setOgreVisibilityTest() with true parameter.
+
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
+ rendering options) when Ogre visibility is enabled. By default, this functionality
+ is turned off.
+ */
+ inline void setOgreShowConfigDialog(const bool showConfigDialog){
+ ogreShowConfigDialog = showConfigDialog;
+ }
+#endif
+
+ unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed) ;
+ unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
+ unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
+
+#ifdef VISP_HAVE_OGRE
+ unsigned int setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
+ unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
+#endif
+ /*!
+ Get the number of polygons.
+
+ \return Size of the list.
+ */
+ inline unsigned int size() const { return (unsigned int)Lpol.size(); }
+} ;
+
+/*!
+ Basic constructor.
+*/
+template<class PolygonType>
+vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces()
+ : Lpol(), nbVisiblePolygon(0), scanlineRender()
+{
+#ifdef VISP_HAVE_OGRE
+ ogreInitialised = false;
+ nbRayAttempts = 1;
+ ratioVisibleRay = 1.0;
+ ogreShowConfigDialog = false;
+ ogre = new vpAROgre();
+ ogreBackground = vpImage<unsigned char>(480, 640, 0);
+#endif
+}
+
+/*!
+ Basic destructor.
+*/
+template<class PolygonType>
+vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
+{
+ for(unsigned int i = 0 ; i < Lpol.size() ; i++){
+ if (Lpol[i]!=NULL){
+ delete Lpol[i] ;
+ }
+ Lpol[i] = NULL ;
+ }
+ Lpol.resize(0);
+
+#ifdef VISP_HAVE_OGRE
+ if(ogre != NULL){
+ delete ogre;
+ ogre = NULL;
+ }
+
+ // This is already done by calling "delete ogre"
+// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
+// if (lOgrePolygons[i]!=NULL){
+// delete lOgrePolygons[i] ;
+// }
+// lOgrePolygons[i] = NULL ;
+// }
+
+ lOgrePolygons.resize(0);
+#endif
+}
+
+/*!
+ Add a polygon to the list of polygons.
+
+ \param p : The polygon to add.
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
+{
+ PolygonType *p_new = new PolygonType;
+ p_new->index = p->index;
+ p_new->setNbPoint(p->nbpt);
+ p_new->isvisible = p->isvisible;
+ p_new->useLod = p->useLod;
+ p_new->minLineLengthThresh = p->minLineLengthThresh;
+ p_new->minPolygonAreaThresh = p->minPolygonAreaThresh;
+ p_new->setName(p->name);
+ p_new->hasOrientation = p->hasOrientation;
+
+ for(unsigned int i = 0; i < p->nbpt; i++)
+ p_new->p[i]= p->p[i];
+ Lpol.push_back(p_new);
+}
+
+/*!
+ Reset the Hidden faces (remove the list of PolygonType)
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::reset()
+{
+ nbVisiblePolygon = 0;
+ for(unsigned int i = 0 ; i < Lpol.size() ; i++){
+ if (Lpol[i]!=NULL){
+ delete Lpol[i] ;
+ }
+ Lpol[i] = NULL ;
+ }
+ Lpol.resize(0);
+
+#ifdef VISP_HAVE_OGRE
+ if(ogre != NULL){
+ delete ogre;
+ ogre = NULL;
+ }
+
+ // This is already done by calling "delete ogre"
+// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
+// if (lOgrePolygons[i]!=NULL){
+// delete lOgrePolygons[i] ;
+// }
+// lOgrePolygons[i] = NULL ;
+// }
+
+ lOgrePolygons.resize(0);
+
+ ogreInitialised = false;
+ nbRayAttempts = 1;
+ ratioVisibleRay = 1.0;
+ ogre = new vpAROgre();
+ ogreBackground = vpImage<unsigned char>(480, 640);
+#endif
+}
+
+/*!
+ Compute the clipped points of the polygons that have been added via addPolygon().
+
+ \param cMo : Pose that will be used to clip the polygons.
+ \param cam : Camera parameters that will be used to clip the polygons.
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+{
+ for (unsigned int i = 0; i < Lpol.size(); i++){
+ // For fast result we could just clip visible polygons.
+ // However clipping all of them gives us the possibility to return more information in the scanline visibility results
+// if(Lpol[i]->isVisible())
+ {
+ Lpol[i]->changeFrame(cMo);
+ Lpol[i]->computePolygonClipped(cam);
+ }
+ }
+}
+
+/*!
+ Render the scene in order to perform, later via computeScanLineQuery(), visibility tests.
+
+ \param cam : Camera parameters that will be used to render the scene.
+ \param w : Width of the render window.
+ \param h : Height of the render window.
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h){
+ std::vector<std::vector<std::pair<vpPoint, unsigned int> > > polyClipped(Lpol.size());
+ std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > listPolyClipped;
+ std::vector<int> listPolyIndices;
+
+ for (unsigned int i = 0; i < Lpol.size(); i++){
+ // For fast result we could just use visible polygons.
+ // However using all of them gives us the possibility to return more information in the scanline visibility results
+// if(Lpol[i]->isVisible())
+ {
+ polyClipped[i].clear();
+ Lpol[i]->getPolygonClipped(polyClipped[i]);
+ if(polyClipped[i].size() != 0)
+ {
+ listPolyClipped.push_back(&polyClipped[i]);
+ listPolyIndices.push_back(Lpol[i]->getIndex());
+ }
+ }
+ }
+
+ scanlineRender.drawScene(listPolyClipped, listPolyIndices, cam, w, h);
+}
+
+/*!
+ Compute Scanline visibility results for a line.
+
+ \warning computeScanLineRender() function has to be called before
+
+ \param a : First point of the line.
+ \param b : Second point of the line.
+ \param lines : Result of the scanline visibility. List of the visible parts of the line.
+ \param displayResults : True if the results have to be displayed. False otherwise
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::computeScanLineQuery(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults)
+{
+ scanlineRender.queryLineVisibility(a,b,lines,displayResults);
+}
+
+/*!
+ Compute the number of visible polygons.
+
+ \param cMo : The pose of the camera
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+ \param useOgre : True if a Ogre is used to test the visibility, False otherwise
+ \param testRoi : True if a face have to be entirely in the image False otherwise
+ \param I : Image used to test if a face is entirely projected in the image.
+ \param cam : Camera parameters.
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
+ const double &angleAppears, const double &angleDisappears,
+ bool &changed, bool useOgre, bool testRoi,
+ const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam)
+{
+ nbVisiblePolygon = 0;
+ changed = false;
+
+ vpTranslationVector cameraPos;
+
+ if(useOgre){
+#ifdef VISP_HAVE_OGRE
+ cMo.inverse().extract(cameraPos);
+ ogre->renderOneFrame(ogreBackground, cMo);
+#else
+ vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
+#endif
+ }
+
+ for (unsigned int i = 0; i < Lpol.size(); i++){
+ //std::cout << "Calling poly: " << i << std::endl;
+ if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, testRoi, I, cam, cameraPos, i))
+ nbVisiblePolygon ++;
+ }
+ return nbVisiblePolygon;
+}
+
+/*!
+ Compute the visibility of a given face index.
+
+ \param cMo : The pose of the camera
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+ \param useOgre : True if a Ogre is used to test the visibility, False otherwise
+ \param testRoi : True if a face have to be entirely in the image False otherwise
+ \param I : Image used to test if a face is entirely projected in the image.
+ \param cam : Camera parameters.
+ \param cameraPos : Position of the camera. Used only when Ogre is used as 3rd party.
+ \param index : Index of the face to consider.
+
+ \return Return true if the face is visible.
+*/
+template<class PolygonType>
+bool
+vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo,
+ const double &angleAppears, const double &angleDisappears,
+ bool &changed, bool useOgre, bool /* testRoi */,
+ const vpImage<unsigned char> & I,
+ const vpCameraParameters & cam,
+ const vpTranslationVector &
+ #ifdef VISP_HAVE_OGRE
+ cameraPos
+ #endif
+ ,
+ unsigned int index)
+{
+ unsigned int i = index;
+ Lpol[i]->changeFrame(cMo);
+ Lpol[i]->isappearing = false;
+
+ //Commented because we need to compute visibility
+ // even when dealing with line in level of detail case
+ /*if(Lpol[i]->getNbPoint() <= 2)
+ {
+ Lpol[i]->isvisible = true;
+ }
+ else*/{
+ if(Lpol[i]->isVisible())
+ {
+ bool testDisappear = false;
+ unsigned int nbCornerInsidePrev = 0;
+
+// if(testRoi){
+// nbCornerInsidePrev = Lpol[i]->getNbCornerInsidePrevImage();
+// if(Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
+// testDisappear = true;
+// }
+
+ if(!testDisappear){
+ if(useOgre)
+#ifdef VISP_HAVE_OGRE
+ testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, I)) || !isVisibleOgre(cameraPos,i));
+#else
+ testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
+#endif
+ else
+ testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
+ }
+
+ // test if the face is still visible
+ if(testDisappear){
+// std::cout << "Face " << i << " disappears" << std::endl;
+ changed = true;
+ Lpol[i]->isvisible = false;
+ }
+ else {
+ //nbVisiblePolygon++;
+ Lpol[i]->isvisible = true;
+
+ if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
+ changed = true;
+ }
+ }
+ else
+ {
+ bool testAppear = true;
+
+// if(testRoi && Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
+// testAppear = false;
+
+ if(testAppear){
+ if(useOgre)
+#ifdef VISP_HAVE_OGRE
+ testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, I)) && isVisibleOgre(cameraPos,i));
+#else
+ testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
+#endif
+ else
+ testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
+ }
+
+ if(testAppear){
+// std::cout << "Face " << i << " appears" << std::endl;
+ Lpol[i]->isvisible = true;
+ changed = true;
+ //nbVisiblePolygon++;
+ }
+ else{
+// std::cout << "Problem" << std::endl;
+ Lpol[i]->isvisible = false;
+ }
+ }
+ }
+ // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon << std::endl;
+ return Lpol[i]->isvisible;
+}
+
+/*!
+ Compute the number of visible polygons.
+
+ \param I : Image used to check if the region of interest is inside the image.
+ \param cam : Camera parameters.
+ \param cMo : The pose of the camera.
+ \param angle : Angle used to test the appearance and disappearance of a face.
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
+{
+ return setVisible(I, cam, cMo, angle, angle, changed);
+}
+
+/*!
+ Compute the number of visible polygons.
+
+ \param I : Image used to check if the region of interest is inside the image.
+ \param cam : Camera parameters.
+ \param cMo : The pose of the camera
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+{
+ return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false,true,I,cam);
+}
+
+/*!
+ Compute the number of visible polygons.
+
+ \param cMo : The pose of the camera
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+{
+ return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false);
+}
+
+#ifdef VISP_HAVE_OGRE
+/*!
+ Initialise the ogre context for face visibility tests.
+
+ \param cam : Camera parameters.
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
+{
+ ogreInitialised = true;
+ ogre->setCameraParameters(cam);
+ ogre->setShowConfigDialog(ogreShowConfigDialog);
+ ogre->init(ogreBackground, false, true);
+
+ for(unsigned int n = 0 ; n < Lpol.size(); n++){
+ Ogre::ManualObject* manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
+
+ manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
+ for(unsigned int i = 0; i < Lpol[n]->nbpt; i++){
+ manual->position( (Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(), (Ogre::Real)Lpol[n]->p[i].get_oZ());
+ manual->colour(1.0, 1.0, 1.0);
+ manual->index(i);
+ }
+
+ manual->index(0);
+ manual->end();
+
+ ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
+
+ lOgrePolygons.push_back(manual);
+ }
+}
+
+/*!
+ Update the display in Ogre Window.
+
+ \param cMo : Pose used to display.
+*/
+template<class PolygonType>
+void
+vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
+{
+ if(ogreInitialised && !ogre->isWindowHidden()){
+ for(unsigned int i = 0 ; i < Lpol.size() ; i++){
+ if(Lpol[i]->isVisible()){
+ lOgrePolygons[i]->setVisible(true);
+ }
+ else
+ lOgrePolygons[i]->setVisible(false);
+ }
+ ogre->display(ogreBackground, cMo);
+ }
+}
+
+/*!
+ Compute the number of visible polygons through Ogre3D.
+
+ \param I : Image used to check if the region of interest is inside the image.
+ \param cam : Camera parameters.
+ \param cMo : The pose of the camera
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+{
+ return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true,true,I,cam);
+}
+
+/*!
+ Compute the number of visible polygons through Ogre3D.
+
+ \param cMo : The pose of the camera
+ \param angleAppears : Angle used to test the appearance of a face
+ \param angleDisappears : Angle used to test the disappearance of a face
+ \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
+
+ \return Return the number of visible polygons
+*/
+template<class PolygonType>
+unsigned int
+vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+{
+ return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true);
+}
+
+/*!
+ Test the visibility of a polygon through Ogre3D via RayCasting.
+
+ \param cameraPos : Position of the camera in the 3D world.
+ \param index : Index of the polygon.
+
+ \return Return true if the polygon is visible, False otherwise.
+*/
+template<class PolygonType>
+bool
+vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
+{
+ Ogre::Vector3 camera((Ogre::Real)cameraPos[0],(Ogre::Real)cameraPos[1],(Ogre::Real)cameraPos[2]);
+ if(!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())){
+ lOgrePolygons[index]->setVisible(false);
+ Lpol[index]->isvisible = false;
+ return false;
+ }
+
+ //Get the center of gravity
+ bool visible = false;
+ unsigned int nbVisible = 0;
+
+ for(unsigned int i = 0; i < nbRayAttempts; i++)
+ {
+ Ogre::Vector3 origin(0,0,0);
+ Ogre::Real totalFactor = 0.0f;
+
+ for(unsigned int j = 0 ; j < Lpol[index]->getNbPoint() ; j++)
+ {
+ Ogre::Real factor = 1.0f;
+
+ if(nbRayAttempts > 1){
+ int r = rand() % 101;
+
+ if(r != 0)
+ factor = ((Ogre::Real)r)/100.0f;
+ }
+
+ Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(), (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
+ tmp *= factor;
+ origin += tmp;
+ totalFactor += factor;
+ }
+
+ origin /= totalFactor;
+
+ Ogre::Vector3 direction = origin - camera;
+ Ogre::Real distanceCollision = direction.length();
+
+ direction.normalise();
+ Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction));
+ mRaySceneQuery->setSortByDistance(true);
+
+ Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
+ Ogre::RaySceneQueryResult::iterator it = result.begin();
+
+// while(it != result.end()){
+// std::cout << it->movable->getName() << "(" << it->distance<< ") : " << std::flush;
+// it++;
+// }
+// std::cout << std::endl;
+// it = result.begin();
+
+ if(it != result.end())
+ if(it->movable->getName().find("SimpleRenderable") != Ogre::String::npos) //Test if the ogreBackground is intersect in first
+ it++;
+
+ double distance, distancePrev;
+ // In a case of a two-axis aligned segment, ray collision is not always working.
+ if(Lpol[index]->getNbPoint() == 2 &&
+ (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) < std::numeric_limits<double>::epsilon()) +
+ (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) < std::numeric_limits<double>::epsilon()) +
+ (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) < std::numeric_limits<double>::epsilon())) >= 2 ))
+ {
+ if(it != result.end())
+ {
+ if(it->movable->getName() == Ogre::StringConverter::toString(index)){
+ nbVisible++;
+ }
+ else
+ {
+ distance = it->distance;
+ // Cannot use epsilon for comparison as ray lenght is slightly different from the collision distance returned by Ogre::RaySceneQueryResult.
+ if(distance > distanceCollision || std::fabs(distance - distanceCollision) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
+ nbVisible++;
+ }
+ }
+ else
+ nbVisible++; // Collision not detected but present.
+ }
+ else
+ {
+ if(it != result.end())
+ {
+ distance = it->distance;
+ distancePrev = distance;
+
+ //std::cout << "For " << Ogre::StringConverter::toString(index) << ": " << it->movable->getName() << " / " << std::flush;
+
+ if(it->movable->getName() == Ogre::StringConverter::toString(index)){
+ nbVisible++;
+ }
+ else
+ {
+ it++;
+ while(it != result.end())
+ {
+ distance = it->distance;
+
+ if(std::fabs(distance - distancePrev) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/){
+ //std::cout << it->movable->getName() << " / " << std::flush;
+ if(it->movable->getName() == Ogre::StringConverter::toString(index)){
+ nbVisible++;
+ break;
+ }
+ it++;
+ distancePrev = distance;
+ }
+ else
+ break;
+ }
+ }
+ }
+ }
+
+ ogre->getSceneManager()->destroyQuery(mRaySceneQuery);
+
+ }
+
+ if(((double)nbVisible)/((double)nbRayAttempts) > ratioVisibleRay ||
+ std::fabs(((double)nbVisible)/((double)nbRayAttempts) - ratioVisibleRay) < ratioVisibleRay * std::numeric_limits<double>::epsilon())
+ visible = true;
+ else
+ visible = false;
+
+ if(visible){
+ lOgrePolygons[index]->setVisible(true);
+ Lpol[index]->isvisible = true;
+ }
+ else{
+ lOgrePolygons[index]->setVisible(false);
+ Lpol[index]->isvisible = false;
+ }
+
+ return Lpol[index]->isvisible;
+}
+#endif //VISP_HAVE_OGRE
+
+#endif // vpMbHiddenFaces
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h
new file mode 100644
index 0000000..fbaf680
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h
@@ -0,0 +1,442 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Model based tracker using only KLT
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+/*!
+ \file vpMbKltTracker.h
+ \brief Model based tracker using only KLT
+*/
+
+#ifndef vpMbKltTracker_h
+#define vpMbKltTracker_h
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/mbt/vpMbtKltXmlParser.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpSubColVector.h>
+#include <visp3/core/vpSubMatrix.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/mbt/vpMbtDistanceKltPoints.h>
+#include <visp3/mbt/vpMbtDistanceCircle.h>
+#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
+
+/*!
+ \class vpMbKltTracker
+ \ingroup group_mbt_trackers
+ \warning This class is only available if OpenCV is installed, and used.
+
+ \brief Model based tracker using only KLT
+
+ The tracker requires the knowledge of the 3D model that could be provided in a vrml
+ or in a cao file. The cao format is described in loadCAOModel().
+ It may also use an xml file used to tune the behavior of the tracker and an
+ init file used to compute the pose at the very first image.
+
+ The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
+
+\code
+#include <visp/vpMbKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpException.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbKltTracker tracker; // Create a model based tracker via KLT points.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Klt Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
+ tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
+
+ while(true){
+ // Acquire a new image
+ vpDisplay::display(I);
+ tracker.track(I); // Track the object on this image
+ tracker.getPose(cMo); // Get the pose
+
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+#if defined VISP_HAVE_XML2
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+
+ The tracker can also be used without display, in that case the initial pose
+ must be known (object always at the same initial pose for example) or computed
+ using another method:
+
+\code
+#include <visp/vpMbKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpImageIo.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
+ //acquire an image
+ vpImageIo::read(I, "cube.pgm"); // Example of acquisition
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ tracker.initFromPose(I, cMo); // initialize the tracker with the given pose.
+
+ while(true){
+ // acquire a new image
+ tracker.track(I); // track the object on this image
+ tracker.getPose(cMo); // get the pose
+ }
+
+#if defined VISP_HAVE_XML2
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+
+ Finally it can be used not to track an object but just to display a model at a
+ given pose:
+
+\code
+#include <visp/vpMbKltTracker.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpCameraParameters.h>
+#include <visp/vpDisplayX.h>
+
+int main()
+{
+#if defined VISP_HAVE_OPENCV
+ vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpCameraParameters cam;
+
+ // Acquire an image
+ vpImageIo::read(I, "cube.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+ display.init(I,100,100,"Mb Klt Tracker");
+#endif
+
+#if defined VISP_HAVE_XML2
+ tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
+#endif
+ tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
+ tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+
+ while(true){
+ // acquire a new image
+ // Get the pose using any method
+ vpDisplay::display(I);
+ tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
+ vpDisplay::flush(I);
+ }
+
+#if defined VISP_HAVE_XML2
+ // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+ return 0;
+#endif
+}
+\endcode
+*/
+class VISP_EXPORT vpMbKltTracker: virtual public vpMbTracker
+{
+protected:
+ //! Temporary OpenCV image for fast conversion.
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat cur;
+#else
+ IplImage *cur;
+#endif
+ //! Initial pose.
+ vpHomogeneousMatrix c0Mo;
+ //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
+ bool compute_interaction;
+ //! Flag to specify whether the init method is called the first or not (specific calls to realize in this case).
+ bool firstInitialisation;
+ //! Erosion of the mask
+ unsigned int maskBorder;
+ //! The gain of the virtual visual servoing stage.
+ double lambda;
+ //! The maximum iteration of the virtual visual servoing stage.
+ unsigned int maxIter;
+ //! Threshold below which the weight associated to a point to consider this one as an outlier.
+ double threshold_outlier;
+ //! Percentage of good points, according to the initial number, that must have the tracker.
+ double percentGood;
+ //! The estimated displacement of the pose between the current instant and the initial position.
+ vpHomogeneousMatrix ctTc0;
+ //! Points tracker.
+ vpKltOpencv tracker;
+ //!
+ std::list<vpMbtDistanceKltPoints*> kltPolygons;
+ //!
+ std::list<vpMbtDistanceKltCylinder*> kltCylinders;
+ //! Vector of the circles used here only to display the full model.
+ std::list<vpMbtDistanceCircle*> circles_disp;
+
+public:
+ vpMbKltTracker();
+ virtual ~vpMbKltTracker();
+
+ void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="");
+ virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, const vpColor& col, const unsigned int thickness=1,
+ const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col, const unsigned int thickness=1, const bool displayFullModel = false);
+
+protected:
+ virtual void init(const vpImage<unsigned char>& I);
+ virtual void reinit(const vpImage<unsigned char>& I);
+
+public:
+ /*! Return the address of the circle feature list. */
+ std::list<vpMbtDistanceCircle*> &getFeaturesCircle() { return circles_disp; }
+ /*! Return the address of the cylinder feature list. */
+ std::list<vpMbtDistanceKltCylinder*> &getFeaturesKltCylinder() { return kltCylinders; }
+ /*! Return the address of the Klt feature list. */
+ std::list<vpMbtDistanceKltPoints*> &getFeaturesKlt() { return kltPolygons; }
+ virtual void loadConfigFile(const std::string& configFile);
+ void loadConfigFile(const char* configFile);
+
+ /*!
+ Get the current list of KLT points.
+
+ \return the list of KLT points through vpKltOpencv.
+ */
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ inline std::vector<cv::Point2f> getKltPoints() {return tracker.getFeatures();}
+#else
+ inline CvPoint2D32f* getKltPoints() {return tracker.getFeatures();}
+#endif
+
+ std::vector<vpImagePoint> getKltImagePoints() const;
+
+ std::map<int, vpImagePoint> getKltImagePointsWithId() const;
+
+ /*!
+ Get the klt tracker at the current state.
+
+ \return klt tracker.
+ */
+ inline vpKltOpencv getKltOpencv() const { return tracker; }
+
+ /*!
+ Get the value of the gain used to compute the control law.
+
+ \return the value for the gain.
+ */
+ virtual inline double getLambda() const {return lambda;}
+
+ /*!
+ Get the erosion of the mask used on the Model faces.
+
+ \return The erosion.
+ */
+ inline unsigned int getMaskBorder() const { return maskBorder; }
+
+ /*!
+ Get the maximum iteration of the virtual visual servoing stage.
+
+ \return the number of iteration
+ */
+ virtual inline unsigned int getMaxIter() const {return maxIter;}
+
+ /*!
+ Get the current number of klt points.
+
+ \return the number of features
+ */
+ inline int getNbKltPoints() const {return tracker.getNbFeatures();}
+
+ /*!
+ Get the threshold for the acceptation of a point.
+
+ \return threshold_outlier : Threshold for the weight below which a point is rejected.
+ */
+ inline double getThresholdAcceptation() const { return threshold_outlier;}
+
+ void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
+ const bool verbose=false);
+ void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
+ const bool verbose=false);
+ void resetTracker();
+
+ void setCameraParameters(const vpCameraParameters& cam);
+
+ void setKltOpencv(const vpKltOpencv& t);
+
+ /*!
+ Set the value of the gain used to compute the control law.
+
+ \param gain : the desired value for the gain.
+ */
+ virtual inline void setLambda(const double gain) {this->lambda = gain;}
+
+ /*!
+ Set the erosion of the mask used on the Model faces.
+
+ \param e : The desired erosion.
+ */
+ inline void setMaskBorder(const unsigned int &e)
+ {
+ maskBorder = e;
+ //if(useScanLine)
+ faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
+ }
+
+ /*!
+ Set the maximum iteration of the virtual visual servoing stage.
+
+ \param max : the desired number of iteration
+ */
+ virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
+
+ /*!
+ Use Ogre3D for visibility tests
+
+ \warning This function has to be called before the initialization of the tracker.
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setOgreVisibilityTest(const bool &v){
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Klt");
+#endif
+ }
+
+ /*!
+ Use Scanline algorithm for visibility tests
+
+ \param v : True to use it, False otherwise
+ */
+ virtual void setScanLineVisibilityTest(const bool &v){
+ vpMbTracker::setScanLineVisibilityTest(v);
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it)
+ (*it)->useScanLine = v;
+ }
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
+
+ /*!
+ Set if the projection error criteria has to be computed.
+
+ \param flag : True if the projection error criteria has to be computed, false otherwise
+ */
+ virtual void setProjectionErrorComputation(const bool &flag) {
+ if(flag)
+ vpCTRACE << "This option is not yet implemented in vpMbKltTracker, projection error computation set to false." << std::endl ; }
+
+ /*!
+ Set the threshold for the acceptation of a point.
+
+ \param th : Threshold for the weight below which a point is rejected.
+ */
+ inline void setThresholdAcceptation(const double th) {threshold_outlier = th;}
+
+ void setUseKltTracking(const std::string &name, const bool &useKltTracking);
+
+ virtual void testTracking();
+ virtual void track(const vpImage<unsigned char>& I);
+
+protected:
+ void computeVVS(const unsigned int &nbInfos, vpColVector &w);
+
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon);
+ virtual void initFaceFromLines(vpMbtPolygon &polygon);
+ virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double, const int,
+ const std::string &name="");
+ virtual void initCylinder(const vpPoint&, const vpPoint &, const double, const int,
+ const std::string &name="");
+
+ void preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed);
+ bool postTracking(const vpImage<unsigned char>& I, vpColVector &w);
+};
+
+#endif
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h
new file mode 100644
index 0000000..3551e3a
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Compute the visibility of 3D polygons already transformed in the camera frame
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpMbScanLine_HH
+#define vpMbScanLine_HH
+
+#include <vector>
+#include <list>
+#include <deque>
+#include <set>
+#include <map>
+#include <limits> // numeric_limits
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+
+//#define DEBUG_DISP // Uncomment to get visibility debug display
+
+#if defined(DEBUG_DISP)
+#include <visp3/core/vpDisplay.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ \class vpMbScanLine
+
+ \ingroup group_mbt_faces
+
+ */
+class vpMbScanLine
+{
+public:
+ //! Structure to define if a scanline intersection is a starting or an ending point of a polygon, or just a single line intersection.
+ typedef enum
+ {
+ START = 1,
+ END = 0,
+ POINT = 2
+ } vpMbScanLineType ;
+
+ //! Structure to define a scanline edge (basically a pair of (X,Y,Z) vectors).
+ typedef std::pair<vpColVector, vpColVector> vpMbScanLineEdge;
+
+ //! Structure to define a scanline intersection.
+ struct vpMbScanLineSegment
+ {
+ vpMbScanLineSegment() : type(START), edge(), p(0), P1(0), P2(0), Z1(0), Z2(0), ID(0), b_sample_Y(false) {};
+ vpMbScanLineType type;
+ vpMbScanLineEdge edge;
+ double p; // This value can be either x or y-coordinate value depending if the structure is used in X or Y-axis scanlines computation.
+ double P1, P2; // Same comment as previous value.
+ double Z1, Z2;
+ int ID;
+ bool b_sample_Y;
+ };
+
+ //! vpMbScanLineEdge Comparator.
+ struct vpMbScanLineEdgeComparator
+ {
+ inline bool operator()(const vpMbScanLineEdge &l0, const vpMbScanLineEdge &l1) const
+ {
+ for(unsigned int i = 0 ; i < 3 ; ++i)
+ if (l0.first[i] < l1.first[i])
+ return true;
+ else if(l0.first[i] > l1.first[i])
+ return false;
+ for(unsigned int i = 0 ; i < 3 ; ++i)
+ if (l0.second[i] < l1.second[i])
+ return true;
+ else if(l0.second[i] > l1.second[i])
+ return false;
+ return false;
+ }
+ };
+
+ //! vpMbScanLineSegment Comparators.
+ struct vpMbScanLineSegmentComparator
+ {
+ inline bool operator()(const vpMbScanLineSegment &a, const vpMbScanLineSegment &b) const
+ {
+ //return a.p == b.p ? a.type < b.type : a.p < b.p;
+ return (std::fabs(a.p - b.p) <= std::numeric_limits<double>::epsilon()) ? a.type < b.type : a.p < b.p;
+ }
+
+ inline bool operator()(const std::pair<double,vpMbScanLineSegment> &a, const std::pair<double, vpMbScanLineSegment> &b) const
+ {
+ return a.first < b.first;
+ }
+ };
+
+private:
+ unsigned int w, h;
+ vpCameraParameters K;
+ unsigned int maskBorder;
+ vpImage<unsigned char> mask;
+ vpImage<int> primitive_ids;
+ std::map<vpMbScanLineEdge, std::set<int>, vpMbScanLineEdgeComparator> visibility_samples;
+ double depthTreshold;
+
+public:
+#if defined(DEBUG_DISP)
+ vpDisplay *dispMaskDebug;
+ vpDisplay *dispLineDebug;
+ vpImage<unsigned char> linedebugImg;
+#endif
+
+ vpMbScanLine();
+ ~vpMbScanLine();
+
+ void drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
+ std::vector<int> listPolyIndices,
+ const vpCameraParameters &K, unsigned int w, unsigned int h);
+
+ /*!
+ If there is one polygon behind another,
+ this threshold defines the minimum distance between both polygons to still consider the one behind as visible.
+
+ \return Current Threshold.
+ */
+ double getDepthTreshold() { return depthTreshold; }
+ unsigned int getMaskBorder() { return maskBorder; }
+ const vpImage<unsigned char>& getMask() const { return mask; }
+ const vpImage<int>& getPrimitiveIDs() const { return primitive_ids; }
+
+ void queryLineVisibility(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults = false);
+
+ /*!
+ If there is one polygon behind another,
+ this threshold defines the minimum distance between both polygons to still consider the one behind as visible.
+
+ \param treshold : New Threshold.
+ */
+ void setDepthTreshold(const double &treshold) { depthTreshold = treshold; }
+ void setMaskBorder(const unsigned int &mb){ maskBorder = mb; }
+
+
+private:
+ void createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
+ std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
+ const unsigned int &size);
+
+ void drawLineY(const vpColVector &a,
+ const vpColVector &b,
+ const vpMbScanLineEdge &line_ID,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
+
+ void drawLineX(const vpColVector &a,
+ const vpColVector &b,
+ const vpMbScanLineEdge &line_ID,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
+
+ void drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
+
+ void drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
+
+ // Static functions
+ static vpMbScanLineEdge makeMbScanLineEdge(const vpPoint &a, const vpPoint &b);
+ static void createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K);
+ static double getAlpha(double x, double X0, double Z0, double X1, double Z1);
+ static double mix(double a, double b, double alpha);
+ static vpPoint mix(const vpPoint &a, const vpPoint &b, double alpha);
+ static double norm(const vpPoint &a, const vpPoint &b);
+};
+
+#endif // doxygen should skip this
+
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h
new file mode 100644
index 0000000..e09eae0
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h
@@ -0,0 +1,661 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic model based tracker. This class declares the methods to implement in
+ * order to have a model based tracker.
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbTracker.h
+ \brief Generic model based tracker.
+*/
+#ifndef vpMbTracker_hh
+#define vpMbTracker_hh
+
+#include <vector>
+#include <string>
+#include <sstream>
+#include <fstream>
+#include <map>
+#include <algorithm>
+#include <cctype>
+#include <locale>
+#include <functional> // std::not1
+
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbtPolygon.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+#include <visp3/core/vpPolygon.h>
+
+#ifdef VISP_HAVE_COIN3D
+//Work around to avoid type redefinition int8_t with Coin
+// #if defined(_WIN32) && defined(VISP_HAVE_OGRE) && (_MSC_VER >= 1600) // Visual Studio 2010
+// #define HAVE_INT8_T 1
+// #endif
+
+//Inventor includes
+# include <Inventor/VRMLnodes/SoVRMLGroup.h>
+# include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
+# include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
+#endif
+
+/*!
+ \class vpMbTracker
+ \ingroup group_mbt_trackers
+ \brief Main methods for a model-based tracker.
+
+ This class provides the main methods for a model based tracker. This pure
+ virtual class must be used in inheritance for a tracker that compute the
+ interaction matrix and the residu vector using a defined information (edge,
+ points of interest, patch, ...)
+
+ This class intends to define a common basis for object tracking. This is
+ realised by implementing the main functions:
+ - init() : Initialisation of the tracker (it includes re-initialisation). This
+ method is called at the end of the initClick() method.
+ - initFaceFromCorners() : Initialisation of the lines that has to be tracked.
+ - track() : Tracking on the current image
+ - testTracking() : Test the tracking. This method throws exception if the
+ tracking failed.
+ - display() : Display the model and eventually other information.
+
+*/
+class VISP_EXPORT vpMbTracker
+{
+public:
+ typedef enum {
+ GAUSS_NEWTON_OPT = 0,
+ LEVENBERG_MARQUARDT_OPT = 1
+ } vpMbtOptimizationMethod;
+
+protected:
+ //! The camera parameters.
+ vpCameraParameters cam;
+ //! The current pose.
+ vpHomogeneousMatrix cMo;
+ //! The Degrees of Freedom to estimate
+ vpMatrix oJo;
+ //! Boolean to know if oJo is identity (for fast computation)
+ bool isoJoIdentity;
+ //! The name of the file containing the model (it is used to create a file name.0.pos used to store the compute pose in the initClick method).
+ std::string modelFileName;
+ //! Flag used to ensure that the CAD model is loaded before the initialisation.
+ bool modelInitialised;
+ //! Filename used to save the initial pose computed using the initClick() method. It is also used to read a previous pose in the same method.
+ std::string poseSavingFilename;
+ //! Flag used to specify if the covariance matrix has to be computed or not.
+ bool computeCovariance;
+ //! Covariance matrix
+ vpMatrix covarianceMatrix;
+ //! Flag used to specify if the gradient error criteria has to be computed or not.
+ bool computeProjError;
+ //! Error angle between the gradient direction of the model features projected at the resulting pose and their normal.
+ double projectionError;
+ //! If true, the features are displayed.
+ bool displayFeatures;
+ //! Weights used in the robust scheme
+ vpColVector m_w;
+ //! Error s-s*
+ vpColVector m_error;
+ //! Optimization method used
+ vpMbtOptimizationMethod m_optimizationMethod;
+
+ //! Set of faces describing the object.
+ vpMbHiddenFaces<vpMbtPolygon> faces;
+ //! Angle used to detect a face appearance
+ double angleAppears;
+ //! Angle used to detect a face disappearance
+ double angleDisappears;
+ //! Distance for near clipping
+ double distNearClip;
+ //! Distance for near clipping
+ double distFarClip;
+ //! Flags specifying which clipping to used
+ unsigned int clippingFlag;
+ //! Use Ogre3d for visibility tests
+ bool useOgre;
+ bool ogreShowConfigDialog;
+ //! Use Scanline for visibility tests
+ bool useScanLine;
+ //! Number of points in CAO model
+ unsigned int nbPoints;
+ //! Number of lines in CAO model
+ unsigned int nbLines;
+ //! Number of polygon lines in CAO model
+ unsigned int nbPolygonLines;
+ //! Number of polygon points in CAO model
+ unsigned int nbPolygonPoints;
+ //! Number of cylinders in CAO model
+ unsigned int nbCylinders;
+ //! Number of circles in CAO model
+ unsigned int nbCircles;
+ //! True if LOD mode is enabled
+ bool useLodGeneral;
+ //! True if the CAO model is loaded before the call to loadConfigFile, (deduced by the number of polygons)
+ bool applyLodSettingInConfig;
+ //! Minimum line length threshold for LOD mode (general setting)
+ double minLineLengthThresholdGeneral;
+ //! Minimum polygon area threshold for LOD mode (general setting)
+ double minPolygonAreaThresholdGeneral;
+ //! Map with [map.first]=parameter_names and [map.second]=type (string, number or boolean)
+ std::map<std::string, std::string> mapOfParameterNames;
+
+public:
+ vpMbTracker();
+ virtual ~vpMbTracker();
+
+ /*!
+ Display the 3D model at a given position using the given camera parameters
+ on a grey level image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param cam : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+ */
+ virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
+ /*!
+ Display the 3D model at a given position using the given camera parameters
+ on a color (RGBa) image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param cam : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+ */
+ virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
+
+ /*! Return the angle used to test polygons appearance. */
+ virtual inline double getAngleAppear() const { return angleAppears; }
+
+ /*! Return the angle used to test polygons disappearance. */
+ virtual inline double getAngleDisappear() const { return angleDisappears; }
+
+ /*!
+ Get the camera parameters.
+
+ \param camera : copy of the camera parameters used by the tracker.
+ */
+ virtual void getCameraParameters(vpCameraParameters& camera) const { camera = this->cam;}
+
+ /*!
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType.
+
+ \return Clipping flags.
+ */
+ virtual inline unsigned int getClipping() const { return clippingFlag; }
+
+ /*!
+ Get the covariance matrix.
+ */
+ virtual vpMatrix getCovarianceMatrix() const {
+ if(!computeCovariance)
+ vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
+
+ return covarianceMatrix;
+ }
+
+ /*!
+ Get the error angle between the gradient direction of the model features projected at the resulting pose and their normal.
+
+ \return the value for the error.
+ */
+ virtual double getProjectionError() const { return projectionError; }
+
+ virtual vpColVector getEstimatedDoF();
+
+ /*!
+ Return the error vector \f$(s-s^*)\f$ reached after the virtual visual servoing process used to estimate the pose.
+
+ The following example shows how to use this function to compute the norm of the residual and the norm of the residual
+ normalized by the number of features that are tracked:
+ \code
+ tracker.track(I);
+ std::cout << "Residual: " << sqrt( (tracker.getError()).sumSquare()) << std::endl;
+ std::cout << "Residual normalized: " << sqrt( (tracker.getError()).sumSquare())/tracker.getError().size() << std::endl;
+ \endcode
+
+ \sa getRobustWeights()
+ */
+ virtual vpColVector getError() {
+ return m_error;
+ }
+
+ /*! Return a reference to the faces structure. */
+ inline vpMbHiddenFaces<vpMbtPolygon>& getFaces() { return faces;}
+
+ /*!
+ Get the far distance for clipping.
+
+ \return Far clipping value.
+ */
+ virtual inline double getFarClippingDistance() const { return distFarClip; }
+
+ /*!
+ Return the weights vector \f$w_i\f$ computed by the robust scheme.
+
+ The following example shows how to use this function to compute the norm of the weighted residual
+ and the norm of the weighted residual normalized by the sum of the weights associated to the
+ features that are tracked:
+ \code
+ tracker.track(I);
+ vpColVector w = tracker.getRobustWeights();
+ vpColVector e = tracker.getError();
+ vpColVector we(w.size());
+ for(unsigned int i=0; i<w.size(); i++)
+ we[i] = w[i]*e[i];
+
+ std::cout << "Weighted residual: " << sqrt( (we).sumSquare() ) << std::endl;
+ std::cout << "Weighted residual normalized: " << sqrt( (we).sumSquare() ) / w.sum() << std::endl;
+ \endcode
+
+ \sa getError()
+ */
+ virtual vpColVector getRobustWeights() {
+ return m_w;
+ }
+
+ /*!
+ Get the number of polygons (faces) representing the object to track.
+
+ \return Number of polygons.
+ */
+ virtual inline unsigned int getNbPolygon() const {
+ return static_cast<unsigned int>(faces.size());
+ }
+
+ /*!
+ Get the near distance for clipping.
+
+ \return Near clipping value.
+ */
+ virtual inline double getNearClippingDistance() const { return distNearClip; }
+
+ /*!
+ Get the optimization method used during the tracking.
+ 0 = Gauss-Newton approach.
+ 1 = Levenberg-Marquardt approach.
+
+ \return Optimization method.
+ */
+ virtual inline vpMbtOptimizationMethod getOptimizationMethod() const { return m_optimizationMethod; }
+
+ /*!
+ Return the polygon (face) "index".
+
+ \exception vpException::dimensionError if index does not represent a good
+ polygon.
+
+ \param index : Index of the polygon to return.
+ \return Pointer to the polygon index.
+ */
+ virtual inline vpMbtPolygon* getPolygon(const unsigned int index) {
+ if(index >= static_cast<unsigned int>(faces.size()) ){
+ throw vpException(vpException::dimensionError, "index out of range");
+ }
+
+ return faces[index];
+ }
+
+ virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > getPolygonFaces(const bool orderPolygons=true,
+ const bool useVisibility=true);
+
+ /*!
+ Get the current pose between the object and the camera.
+ cMo is the matrix which can be used to express
+ coordinates from the object frame to camera frame.
+
+ \param cMo_ : the pose
+ */
+ inline void getPose(vpHomogeneousMatrix& cMo_) const {cMo_ = this->cMo;}
+
+ /*!
+ Get the current pose between the object and the camera.
+ cMo is the matrix which can be used to express
+ coordinates from the object frame to camera frame.
+
+ \return the current pose
+ */
+ inline vpHomogeneousMatrix getPose() const {return this->cMo;}
+
+ /* PURE VIRTUAL METHODS */
+
+ /*!
+ Initialise the tracking.
+
+ \param I : Input image.
+ */
+ virtual void init(const vpImage<unsigned char>& I)=0;
+
+ // Intializer
+
+#ifdef VISP_HAVE_MODULE_GUI
+ virtual void initClick( const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp = false );
+ virtual void initClick( const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile = "" );
+#endif
+
+ virtual void initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile );
+ virtual void initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list );
+
+ virtual void initFromPose(const vpImage<unsigned char>& I, const std::string &initFile);
+ virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
+ virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
+
+ /*!
+ Load a config file to parameterise the behavior of the tracker.
+
+ Pure virtual method to adapt to each tracker.
+
+ \param configFile : An xml config file to parse.
+ */
+ virtual void loadConfigFile(const std::string& configFile)=0;
+
+ virtual void loadModel(const char *modelFile, const bool verbose=false);
+ virtual void loadModel(const std::string &modelFile, const bool verbose=false);
+
+ /*!
+ Reset the tracker.
+ */
+ virtual void resetTracker() = 0;
+
+ void savePose(const std::string &filename);
+
+ /*!
+ Set the angle used to test polygons appearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value lower than
+ this parameter, the polygon is considered as appearing.
+ The polygon will then be tracked.
+
+ \param a : new angle in radian.
+ */
+ virtual inline void setAngleAppear(const double &a) { angleAppears = a; }
+
+ /*!
+ Set the angle used to test polygons disappearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value greater than
+ this parameter, the polygon is considered as disappearing.
+ The tracking of the polygon will then be stopped.
+
+ \param a : new angle in radian.
+ */
+ virtual inline void setAngleDisappear(const double &a) { angleDisappears = a; }
+
+ /*!
+ Set the camera parameters.
+
+ \param camera : the new camera parameters
+ */
+ virtual void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
+
+ virtual void setClipping(const unsigned int &flags);
+
+ /*!
+ Set if the covaraince matrix has to be computed.
+
+ \param flag : True if the covariance has to be computed, false otherwise
+ */
+ virtual void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
+
+ /*!
+ Enable to display the features. By features, we meant the moving edges (ME) and the klt points if used.
+
+ Note that if present, the moving edges can be displayed with different colors:
+ - If green : The ME is a good point.
+ - If blue : The ME is removed because of a contrast problem during the tracking phase.
+ - If purple : The ME is removed because of a threshold problem during the tracking phase.
+ - If red : The ME is removed because it is rejected by the robust approach in the virtual visual servoing scheme.
+
+ \param displayF : set it to true to display the features.
+ */
+ void setDisplayFeatures(const bool displayF) {displayFeatures = displayF;}
+
+ virtual void setEstimatedDoF(const vpColVector& v);
+
+ virtual void setFarClippingDistance(const double &dist);
+
+ virtual void setLod(const bool useLod, const std::string &name="");
+
+ virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="");
+
+ virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="");
+
+ virtual void setNearClippingDistance(const double &dist);
+
+ /*!
+ Set the optimization method used during the tracking.
+
+ \param opt : Optimization method to use.
+ */
+ virtual inline void setOptimizationMethod(const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; }
+
+ /*!
+ Set the pose to be used in entry of the next call to the track() function.
+ This pose will be just used once.
+
+ \warning This function has to be called after the initialisation of the tracker.
+
+ \param I : image corresponding to the desired pose.
+ \param cdMo : Pose to affect.
+ */
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo) = 0;
+
+ /*!
+ Set the filename used to save the initial pose computed using the
+ initClick() method. It is also used to read a previous pose in the same method.
+ If the file is not set then, the initClick() method will create a .0.pos
+ file in the root directory. This directory is the path to the file given to
+ the method initClick() used to know the coordinates in the object frame.
+
+ \param filename : The new filename.
+ */
+ inline void setPoseSavingFilename(const std::string& filename){
+ poseSavingFilename = filename;
+ }
+
+ /*!
+ Set if the projection error criteria has to be computed.
+
+ \param flag : True if the projection error criteria has to be computed, false otherwise
+ */
+ virtual void setProjectionErrorComputation(const bool &flag) { computeProjError = flag; }
+
+ virtual void setScanLineVisibilityTest(const bool &v){ useScanLine = v; }
+
+ /*!
+ Enable/Disable the appearance of Ogre config dialog on startup.
+
+ \warning This method has only effect when Ogre is used and Ogre visibility test is
+ enabled using setOgreVisibilityTest() with true parameter.
+
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
+ rendering options) when Ogre visibility is enabled. By default, this functionality
+ is turned off.
+ */
+ inline void setOgreShowConfigDialog(const bool showConfigDialog){
+ ogreShowConfigDialog = showConfigDialog;
+ }
+
+ virtual void setOgreVisibilityTest(const bool &v);
+
+#ifdef VISP_HAVE_OGRE
+ /*!
+ Set the number of rays that will be sent toward each polygon for visibility test.
+ Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+
+ \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
+
+ \param attempts Number of rays to be sent.
+ */
+ void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { faces.setNbRayCastingAttemptsForVisibility(attempts); }
+
+ /*!
+ Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+
+ \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
+
+ \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ */
+ void setGoodNbRayCastingAttemptsRatio(const double &ratio){ faces.setGoodNbRayCastingAttemptsRatio(ratio); }
+#endif
+
+ /*!
+ Test the quality of the tracking.
+
+ \throw vpException if the test fail.
+ */
+ virtual void testTracking() = 0;
+
+ /*!
+ Track the object in the given image
+
+ \param I : The current image.
+ */
+ virtual void track(const vpImage<unsigned char>& I)=0;
+
+protected:
+ void addPolygon(const std::vector<vpPoint>& corners, const int idFace=-1, const std::string &polygonName="",
+ const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0);
+ void addPolygon(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=-1,
+ const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0);
+ void addPolygon(const vpPoint& p1, const vpPoint &p2, const int idFace=-1, const std::string &polygonName="",
+ const bool useLod=false, const double minLineLengthThreshold=50);
+ void addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace=-1, const std::string &polygonName="",
+ const bool useLod=false, const double minLineLengthThreshold=50);
+
+ void createCylinderBBox(const vpPoint& p1, const vpPoint &p2, const double &radius, std::vector<std::vector<vpPoint> > &listFaces);
+
+ void computeJTR(const vpMatrix& J, const vpColVector& R, vpColVector& JTR);
+
+#ifdef VISP_HAVE_COIN3D
+ virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace);
+ virtual void extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="");
+ virtual void extractLines(SoVRMLIndexedLineSet* line_set, int &idFace, const std::string &polygonName="");
+ virtual void extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="");
+#endif
+
+ vpPoint getGravityCenter(const std::vector<vpPoint>& _pts);
+
+ /*!
+ Add a circle to track from its center, 3 points (including the center) defining the plane that contain
+ the circle and its radius.
+
+ \param p1 : Center of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param radius : Radius of the circle.
+ \param idFace : Id of the face associated to the circle.
+ \param name : Name of the circle.
+ */
+ virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace=0, const std::string &name="")=0;
+ /*!
+ Add a cylinder to track from two points on the axis (defining the length of
+ the cylinder) and its radius.
+
+ \param p1 : First point on the axis.
+ \param p2 : Second point on the axis.
+ \param radius : Radius of the cylinder.
+ \param idFace : Id of the face associated to the cylinder.
+ \param name : Name of the cylinder.
+ */
+ virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
+ const std::string &name="")=0;
+
+ /*!
+ Add the lines to track from the polygon description. If the polygon has only
+ two points, it defines a single line that is always visible. If it has three or
+ more corners, it defines a face. In that case the visibility of the face is computed
+ in order to track the corresponding lines only if the face is visible.
+
+ The id of the polygon is supposed to be set prior calling this function.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+ */
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0;
+ virtual void initFaceFromLines(vpMbtPolygon &polygon)=0;
+
+ virtual void loadVRMLModel(const std::string& modelFile);
+ virtual void loadCAOModel(const std::string& modelFile, std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
+ const bool verbose=false, const bool parent=true);
+
+ void removeComment(std::ifstream& fileId);
+
+ inline bool parseBoolean(std::string &input) {
+ std::transform(input.begin(), input.end(), input.begin(), ::tolower);
+ std::istringstream is(input);
+ bool b;
+ //Parse string to boolean either in the textual representation (True/False)
+ //or in numeric representation (1/0)
+ is >> (input.size() > 1 ? std::boolalpha : std::noboolalpha) >> b;
+ return b;
+ }
+
+ std::map<std::string, std::string> parseParameters(std::string& endLine);
+
+ inline std::string <rim(std::string &s) {
+ s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
+ return s;
+ }
+
+ inline std::string &rtrim(std::string &s) {
+ s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
+ return s;
+ }
+
+ inline std::string &trim(std::string &s) {
+ return ltrim(rtrim(s));
+ }
+};
+
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h
new file mode 100644
index 0000000..5e9bd29
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML Parameter for Model Based Tracker.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ * \file vpMbXmlParser.h
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+*/
+
+#ifndef vpMbXmlParser_HH
+#define vpMbXmlParser_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/core/vpXmlParser.h>
+#include <visp3/core/vpCameraParameters.h>
+
+/*!
+ \class vpMbXmlParser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+ \ingroup group_mbt_xml_parser
+
+ Data parser for the model based tracker.
+
+ */
+class VISP_EXPORT vpMbXmlParser: public vpXmlParser
+{
+protected:
+ //! Camera parameters.
+ vpCameraParameters cam;
+ //! Angle to determine if a face appeared
+ double angleAppear;
+ //! Angle to determine if a face disappeared
+ double angleDisappear;
+ //! Is near clipping distance specified?
+ bool hasNearClipping;
+ //! Near clipping distance
+ double nearClipping;
+ //! Is far clipping distance specified?
+ bool hasFarClipping;
+ //! Near clipping distance
+ double farClipping;
+ //! Fov Clipping
+ bool fovClipping;
+ //! If true, the LOD is enabled, otherwise it is not
+ bool useLod;
+ //! Minimum line length to track a segment when LOD is enabled
+ double minLineLengthThreshold;
+ //! Minimum polygon area to track a face when LOD is enabled
+ double minPolygonAreaThreshold;
+
+ typedef enum{
+ conf,
+ face,
+ angle_appear,
+ angle_disappear,
+ near_clipping,
+ far_clipping,
+ fov_clipping,
+ camera,
+ height,
+ width,
+ u0,
+ v0,
+ px,
+ py,
+ lod,
+ use_lod,
+ min_line_length_threshold,
+ min_polygon_area_threshold,
+ last
+ } dataToParseMb;
+
+
+public:
+
+ vpMbXmlParser();
+ virtual ~vpMbXmlParser();
+
+ /*!
+ Get the angle to determine if a face appeared.
+
+ \return angleAppear
+ */
+ inline double getAngleAppear() const {return angleAppear;}
+
+ /*!
+ Get the angle to determine if a face disappeared.
+
+ \return angleDisappear
+ */
+ inline double getAngleDisappear() const {return angleDisappear;}
+
+ void getCameraParameters(vpCameraParameters& _cam) const { _cam = this->cam;}
+
+ /*!
+ Get the far clipping distance.
+
+ \return farClipping
+ */
+ inline double getFarClippingDistance() const {return farClipping;}
+
+ /*!
+ Use FOV clipping
+
+ \return True if yes, False otherwise.
+ */
+ inline bool getFovClipping() const {return fovClipping;}
+
+ /*!
+ Get the state of LOD setting.
+
+ \return True if LOD is enabled, false otherwise.
+ */
+ inline bool getLodState() const {
+ return useLod;
+ }
+
+ /*!
+ Get the minimum line length to track a segment when LOD is enabled.
+
+ \return The minimum line length.
+ */
+ inline double getMinLineLengthThreshold() const {
+ return minLineLengthThreshold;
+ }
+
+ /*!
+ Get the minimum polygon area to track a face when LOD is enabled.
+
+ \return The minimum polygon area.
+ */
+ inline double getMinPolygonAreaThreshold() const {
+ return minPolygonAreaThreshold;
+ }
+
+ /*!
+ Get the near clipping distance.
+
+ \return nearClipping
+ */
+ inline double getNearClippingDistance() const {return nearClipping;}
+
+ /*!
+ Has Far clipping been specified?
+
+ \return True if yes, False otherwise.
+ */
+ inline bool hasFarClippingDistance() const {return hasFarClipping;}
+
+ /*!
+ Has Near clipping been specified?
+
+ \return True if yes, False otherwise.
+ */
+ inline bool hasNearClippingDistance() const {return hasNearClipping;}
+
+ void parse(const char * filename);
+
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+ void read_camera (xmlDocPtr doc, xmlNodePtr node);
+ void read_face(xmlDocPtr doc, xmlNodePtr node);
+ void read_lod (xmlDocPtr doc, xmlNodePtr node);
+
+ /*!
+ Set the angle to determine if a face appeared.
+
+ \param aappear : New angleAppear
+ */
+ inline void setAngleAppear(const double &aappear) {angleAppear = aappear;}
+
+ /*!
+ Set the angle to determine if a face disappeared.
+
+ \param adisappear : New angleDisappear
+ */
+ inline void setAngleDisappear(const double &adisappear) {angleDisappear = adisappear;}
+
+ void setCameraParameters(const vpCameraParameters &_cam){ cam = _cam; }
+
+ /*!
+ Set the far clipping distance.
+
+ \param fclip : New farClipping
+ */
+ inline void setFarClippingDistance(const double &fclip) {farClipping = fclip;}
+
+ /*!
+ Set the near clipping distance.
+
+ \param nclip : New nearClipping
+ */
+ inline void setNearClippingDistance(const double &nclip) {nearClipping = nclip;}
+
+ void writeMainClass(xmlNodePtr node);
+
+protected:
+ void init();
+
+};
+
+#endif
+
+#endif /* NMBXMLPARSER_H_ */
+
+
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h
new file mode 100644
index 0000000..a6b760a
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Manage a circle used in the model-based tracker.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtDistanceCircle.h
+ \brief Manage a circle used in the model-based tracker.
+*/
+
+#ifndef vpMbtDistanceCircle_HH
+#define vpMbtDistanceCircle_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbtMeEllipse.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+
+/*!
+ \class vpMbtDistanceCircle
+
+ \brief Manage a circle used in the model-based tracker.
+
+ \ingroup group_mbt_features
+*/
+class VISP_EXPORT vpMbtDistanceCircle
+{
+ private :
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ //double alpha;
+ double wmean;
+ vpFeatureEllipse featureEllipse ;
+ //! Polygon describing the circle bbox
+// vpMbtPolygon poly;
+ bool isTrackedCircle;
+
+ public:
+ //! The moving edge containers
+ vpMbtMeEllipse *meEllipse;
+
+ //! The circle to track
+ vpCircle *circle;
+
+ //! The radius of the circle
+ double radius;
+
+ //! The center of the circle
+ vpPoint *p1;
+ //! A point on the plane containing the circle
+ vpPoint *p2;
+ //! An other point on the plane containing the circle
+ vpPoint *p3;
+
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ unsigned int nbFeature;
+ //! Indicates if the circle has to be reinitialized
+ bool Reinit;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the faces which contain the line
+ int index_polygon;
+ //! Indicates if the circle is visible or not
+ bool isvisible;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMbtDistanceCircle(const vpMbtDistanceCircle &)
+// : name(), index(0), cam(), me(NULL), wmean(1),
+// featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
+// circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
+// L(), error(), nbFeature(0), Reinit(false),
+// hiddenface(NULL), index_polygon(-1), isvisible(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMbtDistanceCircle &operator=(const vpMbtDistanceCircle &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpMbtDistanceCircle() ;
+ ~vpMbtDistanceCircle() ;
+
+ void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r);
+
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
+
+ /*!
+ Get the index of the circle.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() {return index ;}
+
+ /*!
+ Get the mean weight of the circle. The mean weight is computed thanks to the weight of each moving edge.
+ Those weights are computed by the robust estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the circle.
+ */
+ inline double getMeanWeight() const {return wmean;}
+
+ /*!
+ Get the name of the circle.
+
+ \return Return the name of the circle.
+ */
+ inline std::string getName() const {return name;}
+
+ /*!
+ Get the polygon associated to the circle.
+
+ \return poly.
+ */
+// inline vpMbtPolygon& getPolygon() {return poly;}
+
+ void initInteractionMatrixError();
+
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ /*!
+ Return if the circle is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const {return isTrackedCircle;}
+
+ /*!
+ Check if the circle is visible in the image or not.
+
+ \return Return true if the circle is visible
+ */
+ inline bool isVisible() const {return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
+
+ /*!
+ Set if the circle has to considered during tracking phase.
+
+ \param track : True if the circle has to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool& track) {this->isTrackedCircle = track;}
+
+ /*!
+ Set the index of the circle.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) {index = i;}
+
+ /*!
+ Set the mean weight of the circle.
+
+ \param _wmean : The mean weight of the circle.
+ */
+ inline void setMeanWeight(const double _wmean) {this->wmean = _wmean;}
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the circle.
+
+ \param circle_name : The name of the circle.
+ */
+ inline void setName(const std::string& circle_name) {this->name = circle_name;}
+
+ /*!
+ Set the name of the circle.
+
+ \param circle_name : The name of the circle.
+ */
+ inline void setName(const char* circle_name) {this->name = std::string(circle_name);}
+
+ /*!
+ Set a boolean parameter to indicates if the circle is visible in the image or not.
+
+ \param _isvisible : Set to true if the circle is visible
+ */
+ inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
+
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ private:
+ void project(const vpHomogeneousMatrix &cMo);
+} ;
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h
new file mode 100644
index 0000000..90ab15b
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Manage a cylinder used in the model-based tracker.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Bertrand Delabarre
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtDistanceCylinder.h
+ \brief Manage a cylinder used in the model-based tracker.
+*/
+
+#ifndef vpMbtDistanceCylinder_HH
+#define vpMbtDistanceCylinder_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbtMeLine.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+
+/*!
+ \class vpMbtDistanceCylinder
+
+ \brief Manage a cylinder used in the model-based tracker.
+
+ \ingroup group_mbt_features
+*/
+class VISP_EXPORT vpMbtDistanceCylinder
+{
+ private :
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ double wmean1;
+ double wmean2;
+ vpFeatureLine featureline1 ;
+ vpFeatureLine featureline2 ;
+ bool isTrackedCylinder;
+
+ public:
+ //! The moving edge containers (first line of the cylinder)
+ vpMbtMeLine *meline1;
+ //! The moving edge containers (second line of the cylinder)
+ vpMbtMeLine *meline2;
+
+ //! The upper circle limiting the cylinder
+ vpCircle *cercle1;
+ //! The lower circle limiting the cylinder
+ vpCircle *cercle2;
+
+ //! The radius of the cylinder
+ double radius;
+
+ //! The first extremity on the axe
+ vpPoint *p1;
+ //! The second extremity on the axe
+ vpPoint *p2;
+
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ unsigned int nbFeature;
+ //! The number of moving edges on line 1
+ unsigned int nbFeaturel1;
+ //! The number of moving edges on line 2
+ unsigned int nbFeaturel2;
+ //! Indicates if the line has to be reinitialized
+ bool Reinit;
+ //! The cylinder
+ vpCylinder *c;
+
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the face which contains the cylinder
+ int index_polygon;
+ //! Indicates if the cylinder is visible or not
+ bool isvisible;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMbtDistanceCylinder(const vpMbtDistanceCylinder &)
+// : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1),
+// featureline1(), featureline2(), isTrackedCylinder(true), meline1(NULL), meline2(NULL),
+// cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
+// error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
+// c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMbtDistanceCylinder &operator=(const vpMbtDistanceCylinder &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpMbtDistanceCylinder() ;
+ ~vpMbtDistanceCylinder() ;
+
+ void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r);
+
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
+
+ /*!
+ Get the index of the cylinder.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() {return index ;}
+
+ /*!
+ Get the mean weight of the first line. The mean weight is computed thanks to the weight of each moving edge.
+ Those weights are computed by the robust estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the first line.
+ */
+ inline double getMeanWeight1() const {return wmean1;}
+
+ /*!
+ Get the mean weight of the second line. The mean weight is computed thanks to the weight of each moving edge.
+ Those weights are computed by the robust estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the second line.
+ */
+ inline double getMeanWeight2() const {return wmean2;}
+
+ /*!
+ Get the name of the cylinder.
+
+ \return Return the name of the cylinder
+ */
+ inline std::string getName() const {return name;}
+
+ void initInteractionMatrixError();
+
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Return if the cylinder is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const {return isTrackedCylinder;}
+
+ /*!
+ Check if the cylinder is visible in the image or not.
+
+ \return Return true if the cylinder is visible
+ */
+ inline bool isVisible() const {return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
+
+ /*!
+ Set if the cylinder has to considered during tracking phase.
+
+ \param track : True if the cylinder has to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool& track) {this->isTrackedCylinder = track;}
+
+ /*!
+ Set the index of the cylinder.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) {index = i;}
+
+ /*!
+ Set the mean weight of the first line.
+
+ \param wmean : The mean weight of the first line.
+ */
+ inline void setMeanWeight1(const double wmean) {this->wmean1 = wmean;}
+
+ /*!
+ Set the mean weight of the second line.
+
+ \param wmean : The mean weight of the second line.
+ */
+ inline void setMeanWeight2(const double wmean) {this->wmean2 = wmean;}
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the cylinder.
+
+ \param cyl_name : The name of the cylinder.
+ */
+ inline void setName(const std::string& cyl_name) {this->name = cyl_name;}
+
+ /*!
+ Set the name of the cylinder.
+
+ \param cyl_name : The name of the cylinder
+ */
+ inline void setName(const char* cyl_name) {this->name = std::string(cyl_name);}
+
+ /*!
+ Set a boolean parameter to indicates if the cylinder is visible in the image or not.
+
+ \param _isvisible : Set to true if the cylinder is visible
+ */
+ inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ private:
+ void project(const vpHomogeneousMatrix &cMo);
+} ;
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h
new file mode 100644
index 0000000..b30e3ec
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h
@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Klt cylinder, containing points of interest.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpMbtDistanceKltCylinder_h
+#define vpMbtDistanceKltCylinder_h
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+#include <map>
+
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpGEMM.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpCircle.h>
+
+/*!
+ \class vpMbtDistanceKltCylinder
+
+ \brief Implementation of a polygon of the model containing points of interest. It is used by the model-based tracker KLT, and hybrid.
+
+ \warning This class is only available if OpenCV is installed, and used.
+
+ \ingroup group_mbt_features
+*/
+class VISP_EXPORT vpMbtDistanceKltCylinder
+{
+private:
+ //! Pose at initialisation
+ vpHomogeneousMatrix c0Mo;
+ //! First extremity of the cylinder (used for display)
+ vpPoint p1Ext;
+ //! Second extremity of the cylinder (used for display)
+ vpPoint p2Ext;
+ //! Cylinder
+ vpCylinder cylinder;
+ //! The upper circle limiting the cylinder (used for display)
+ vpCircle circle1;
+ //! The lower circle limiting the cylinder (used for display)
+ vpCircle circle2;
+ //! Initial points and their ID
+ std::map<int, vpImagePoint> initPoints;
+ //! Initial points and their ID
+ std::map<int, vpPoint> initPoints3D;
+ //! Current points and their ID
+ std::map<int, vpImagePoint> curPoints;
+ //! Current points ID and their indexes
+ std::map<int, int> curPointsInd;
+ //! number of points detected
+ unsigned int nbPointsCur;
+ //! initial number of points
+ unsigned int nbPointsInit;
+ //! Minimal number of points to be tracked
+ unsigned int minNbPoint;
+ //! Boolean to know if there is enough point to be tracked
+ bool enoughPoints;
+ //! Camera parameters
+ vpCameraParameters cam;
+ //! Boolean to specify if the klt points have to be tracked or not
+ bool isTrackedKltCylinder;
+
+public:
+ //! Pointer to the polygon that define a face
+ std::vector<int> listIndicesCylinderBBox;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Use scanline rendering
+ bool useScanLine;
+
+private:
+ double computeZ(const double &x, const double &y);
+ bool isTrackedFeature(const int id);
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMbtDistanceKltCylinder(const vpMbtDistanceKltCylinder &)
+// : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(),
+// initPoints(), initPoints3D(), curPoints(), curPointsInd(),
+// nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false),
+// cam(), isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMbtDistanceKltCylinder &operator=(const vpMbtDistanceKltCylinder &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpMbtDistanceKltCylinder();
+ virtual ~vpMbtDistanceKltCylinder();
+
+ void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r);
+
+ unsigned int computeNbDetectedCurrent(const vpKltOpencv& _tracker);
+ void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector& _R, vpMatrix& _J);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ void displayPrimitive(const vpImage<unsigned char>& _I);
+ void displayPrimitive(const vpImage<vpRGBa>& _I);
+
+ /*!
+ Get the camera parameters of the face.
+
+ \return cam : the camera parameters of the face.
+ */
+ inline vpCameraParameters& getCameraParameters(){ return cam; }
+
+ inline std::map<int, vpImagePoint>& getCurrentPoints() {return curPoints; }
+
+ inline std::map<int, int>& getCurrentPointsInd() {return curPointsInd; }
+
+ /*!
+ Get the number of point that was belonging to the face at the initialisation
+
+ \return the number of initial point.
+
+ \sa getCurrentNumberPoints()
+ */
+ inline unsigned int getInitialNumberPoint() const { return nbPointsInit;}
+ /*!
+ Get the number of points detected in the last image.
+
+ \warning To have the real number of points, the function computeNbDetectedCurrent()
+ must be called first.
+
+ \return the number of points detected in the current image.
+
+ \sa getInitialNumberPoint()
+ */
+ inline unsigned int getCurrentNumberPoints() const {return nbPointsCur;}
+
+ inline bool hasEnoughPoints() const {return enoughPoints;}
+
+ /*!
+ Return if the klt cylinder is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const {return isTrackedKltCylinder;}
+
+ void init(const vpKltOpencv& _tracker, const vpHomogeneousMatrix &cMo);
+
+ void removeOutliers(const vpColVector& weight, const double &threshold_outlier);
+
+ /*!
+ Set the camera parameters
+
+ \param _cam : the new camera parameters
+ */
+ virtual inline void setCameraParameters(const vpCameraParameters& _cam){ cam = _cam; }
+
+ /*!
+ Set if the klt cylinder has to be considered during tracking phase.
+
+ \param track : True if is has to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool& track) {this->isTrackedKltCylinder = track;}
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
+#else
+ void updateMask(IplImage* mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
+#endif
+};
+
+#endif
+
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h
new file mode 100644
index 0000000..4bb687a
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h
@@ -0,0 +1,213 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Klt polygon, containing points of interest.
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#ifndef vpMbtDistanceKltPoints_h
+#define vpMbtDistanceKltPoints_h
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+#include <map>
+
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpGEMM.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+
+/*!
+ \class vpMbtDistanceKltPoints
+
+ \brief Implementation of a polygon of the model containing points of interest. It is used by the model-based tracker KLT, and hybrid.
+
+ \warning This class is only available if OpenCV is installed, and used.
+
+ \ingroup group_mbt_features
+*/
+class VISP_EXPORT vpMbtDistanceKltPoints
+{
+private:
+ //! the homography in meter
+ vpMatrix H;
+ //! normal to the initial plane
+ vpColVector N;
+ //! current normal
+ vpColVector N_cur;
+ //! inverse of the distance between the plane and the camera at the initial position (speed up computation)
+ double invd0;
+ //! cRc0_0n (temporary variable to speed up the computation)
+ vpColVector cRc0_0n;
+ //! Initial points and their ID
+ std::map<int, vpImagePoint> initPoints;
+ //! Current points and their ID
+ std::map<int, vpImagePoint> curPoints;
+ //! Current points ID and their indexes
+ std::map<int, int> curPointsInd;
+ //! number of points detected
+ unsigned int nbPointsCur;
+ //! initial number of points
+ unsigned int nbPointsInit;
+ //! Minimal number of points to be tracked
+ unsigned int minNbPoint;
+ //! Boolean to know if there is enough point to be tracked
+ bool enoughPoints;
+ //! current camera to plane distance to speed up the computation
+ double dt;
+ //! distance between the plane and the camera at the initial position
+ double d0;
+ //! Camera parameters
+ vpCameraParameters cam;
+ //! Boolean to specify if the klt points have to be tracked or not
+ bool isTrackedKltPoints;
+
+public:
+ //! Pointer to the polygon that define a face
+ vpMbtPolygon *polygon;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Use scanline rendering
+ bool useScanLine;
+
+private:
+
+ double compute_1_over_Z(const double x, const double y);
+ void computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& cHc0);
+ bool isTrackedFeature(const int id);
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
+// : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
+// nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
+// cam(), isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL), useScanLine(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ vpMbtDistanceKltPoints();
+ virtual ~vpMbtDistanceKltPoints();
+
+ unsigned int computeNbDetectedCurrent(const vpKltOpencv& _tracker);
+ void computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& cHc0);
+ void computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ void displayPrimitive(const vpImage<unsigned char>& _I);
+ void displayPrimitive(const vpImage<vpRGBa>& _I);
+
+ /*!
+ Get the camera parameters of the face.
+
+ \return cam : the camera parameters of the face.
+ */
+ inline vpCameraParameters& getCameraParameters(){ return cam; }
+
+ inline vpColVector getCurrentNormal() const {return N_cur; }
+
+ inline std::map<int, vpImagePoint>& getCurrentPoints() {return curPoints; }
+
+ inline std::map<int, int>& getCurrentPointsInd() {return curPointsInd; }
+
+ /*!
+ Get the number of point that was belonging to the face at the initialisation
+
+ \return the number of initial point.
+
+ \sa getCurrentNumberPoints()
+ */
+ inline unsigned int getInitialNumberPoint() const { return nbPointsInit;}
+ /*!
+ Get the number of points detected in the last image.
+
+ \warning To have the real number of points, the function computeNbDetectedCurrent()
+ must be called first.
+
+ \return the number of points detected in the current image.
+
+ \sa getInitialNumberPoint()
+ */
+ inline unsigned int getCurrentNumberPoints() const {return nbPointsCur;}
+
+ inline bool hasEnoughPoints() const {return enoughPoints;}
+
+ void init(const vpKltOpencv& _tracker);
+
+ /*!
+ Return if the klt points are used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const {return isTrackedKltPoints;}
+
+ void removeOutliers(const vpColVector& weight, const double &threshold_outlier);
+
+ /*!
+ Set the camera parameters
+
+ \param _cam : the new camera parameters
+ */
+ virtual inline void setCameraParameters(const vpCameraParameters& _cam){ cam = _cam; }
+
+ /*!
+ Set if the klt points have to considered during tracking phase.
+
+ \param track : True if they have to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool& track) {this->isTrackedKltPoints = track;}
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
+#else
+ void updateMask(IplImage* mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
+#endif
+};
+
+#endif
+
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h
new file mode 100644
index 0000000..9324ed8
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h
@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Manage the line of a polygon used in the model-based tracker.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtDistanceLine.h
+ \brief Manage the line of a polygon used in the model-based tracker.
+*/
+
+#ifndef vpMbtDistanceLine_HH
+#define vpMbtDistanceLine_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbtMeLine.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+
+#include <list>
+
+/*!
+ \class vpMbtDistanceLine
+
+ \brief Manage the line of a polygon used in the model-based tracker.
+
+ \ingroup group_mbt_features
+
+ */
+class VISP_EXPORT vpMbtDistanceLine
+{
+ private :
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ bool isTrackedLine;
+ bool isTrackedLineWithVisibility;
+ double wmean;
+ vpFeatureLine featureline ;
+ //! Polygon describing the line
+ vpMbtPolygon poly;
+
+ public:
+ //! Use scanline rendering
+ bool useScanLine;
+ //! The moving edge container
+ //vpMbtMeLine *meline;
+ std::vector<vpMbtMeLine*> meline;
+ //! The 3D line
+ vpLine *line;
+ //! The first extremity
+ vpPoint *p1;
+ //! The second extremity
+ vpPoint *p2;
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ //unsigned int nbFeature;
+ std::vector<unsigned int> nbFeature;
+ //! The number of moving edges
+ unsigned int nbFeatureTotal;
+ //! Indicates if the line has to be reinitialized
+ bool Reinit;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the faces which contain the line
+ std::list<int> Lindex_polygon;
+ //! Vector of bool associated with Lindex_polygon to know if Lindex_polygon[i] is tracked
+ std::vector<bool> Lindex_polygon_tracked;
+ //! Indicates if the line is visible or not
+ bool isvisible;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpMbtDistanceLine(const vpMbtDistanceLine &)
+// : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true),
+// wmean(1), featureline(), poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(),
+// error(), nbFeature(), nbFeatureTotal(0), Reinit(false), hiddenface(NULL), Lindex_polygon(),
+// Lindex_polygon_tracked(), isvisible(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpMbtDistanceLine &operator=(const vpMbtDistanceLine &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ vpMbtDistanceLine() ;
+ ~vpMbtDistanceLine() ;
+
+ void addPolygon(const int &index);
+
+ void buildFrom(vpPoint &_p1, vpPoint &_p2);
+
+ bool closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold);
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters& camera) const {camera = this->cam;}
+
+ /*!
+ Get the index of the line.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() const {return index ;}
+
+ /*!
+ Get the mean weight of the line. The mean weight is computed thanks to the weight of each moving edge.
+ Those weights are computed by the robust estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the line.
+ */
+ inline double getMeanWeight() const {return wmean;}
+
+ /*!
+ Get the name of the line.
+
+ \return Return the name of the line
+ */
+ inline std::string getName() const {return name;}
+
+ /*!
+ Get the polygon associated to the line.
+
+ \return poly.
+ */
+ inline vpMbtPolygon& getPolygon() {return poly;}
+
+ void initInteractionMatrixError();
+
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Return if the line is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const {return isTrackedLineWithVisibility;}
+
+ /*!
+ Check if the line is visible in the image or not.
+
+ \return Return true if the line is visible
+ */
+ inline bool isVisible() const {return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
+
+ /*!
+ Set the index of the line.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) {index = i;}
+
+ /*!
+ Set the mean weight of the line.
+
+ \param w_mean : The mean weight of the line.
+ */
+ inline void setMeanWeight(const double w_mean) {this->wmean = w_mean;}
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the line.
+
+ \param line_name : The name of the line.
+ */
+ inline void setName(const std::string line_name) {this->name = line_name;}
+
+ /*!
+ Set the name of the line.
+
+ \param line_name : The name of the line.
+ */
+ inline void setName(const char* line_name) {this->name = std::string(line_name);}
+
+ void setTracked(const std::string &name, const bool &track);
+
+ /*!
+ Set a boolean parameter to indicates if the line is visible in the image or not.
+
+ \param _isvisible : Set to true if the line is visible
+ */
+ void setVisible(bool _isvisible) {isvisible = _isvisible ; }
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateTracked();
+
+ private:
+ void project(const vpHomogeneousMatrix &cMo);
+} ;
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h
new file mode 100644
index 0000000..ef1760f
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using edges and point features).
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ * \file vpMbtEdgeKltXmlParser.h
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+*/
+
+#ifndef vpMbtEdgeKltXmlParser_HH
+#define vpMbtEdgeKltXmlParser_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbtXmlParser.h>
+#include <visp3/mbt/vpMbtKltXmlParser.h>
+
+/*!
+ \class vpMbtEdgeKltXmlParser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+ \ingroup group_mbt_xml_parser
+
+ Data parser for the model based tracker.
+
+ */
+class VISP_EXPORT vpMbtEdgeKltXmlParser: public vpMbtXmlParser, public vpMbtKltXmlParser
+{
+protected:
+ typedef enum{
+ camera,
+ face,
+ klt,
+ ecm,
+ lod
+ } dataToParseMbtEdgeKlt;
+public:
+
+ vpMbtEdgeKltXmlParser();
+ virtual ~vpMbtEdgeKltXmlParser();
+
+ void parse(const char * filename);
+
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+
+ void writeMainClass(xmlNodePtr node);
+
+protected:
+ void init();
+
+};
+
+#endif
+
+#endif /* NMBTEDGEKLTXMLPARSER_H_ */
+
+
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h
new file mode 100644
index 0000000..fcbe335
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using point features).
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ * \file vpMbtKltXmlParser.h
+ * \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
+*/
+#ifndef vpMbtKltXmlParser_HH
+#define vpMbtKltXmlParser_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbXmlParser.h>
+
+/*!
+ \class vpMbtKltXmlParser
+ \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
+ \ingroup group_mbt_xml_parser
+
+ Data parser for the KLT model based tracker.
+
+*/
+class VISP_EXPORT vpMbtKltXmlParser: virtual public vpMbXmlParser
+{
+protected:
+ //! Border of the mask used on Klt points
+ unsigned int maskBorder;
+ //! Maximum of Klt features
+ unsigned int maxFeatures;
+ //! Windows size
+ unsigned int winSize;
+ //! Quality of the Klt points
+ double qualityValue;
+ //! Minimum distance between klt points
+ double minDist;
+ //! Harris free parameters
+ double harrisParam;
+ //! Block size
+ unsigned int blockSize;
+ //! Number of pyramid levels
+ unsigned int pyramidLevels;
+
+ typedef enum{
+ klt = vpMbXmlParser::last,
+ mask_border,
+ max_features,
+ window_size,
+ quality,
+ min_distance,
+ harris,
+ size_block,
+ pyramid_lvl,
+ last
+ } dataToParseMbKlt;
+
+
+public:
+
+ vpMbtKltXmlParser();
+ virtual ~vpMbtKltXmlParser();
+
+ /*!
+ Get the size of a block.
+
+ \return blockSize
+ */
+ inline unsigned int getBlockSize() const {return blockSize;}
+
+ /*!
+ Get the Harris free parameter.
+
+ \return harrisParam
+ */
+ inline double getHarrisParam() const {return harrisParam;}
+
+ /*!
+ Get the Border of the mask.
+
+ \return faceBorder
+ */
+ inline unsigned int getMaskBorder() const {return maskBorder;}
+
+ /*!
+ Get the maximum number of features for the KLT.
+
+ \return maxFeatures
+ */
+ inline unsigned int getMaxFeatures() const {return maxFeatures;}
+
+ /*!
+ Get the minimum distance between KLT points.
+
+ \return minDist
+ */
+ inline double getMinDistance() const {return minDist;}
+
+ /*!
+ Get the number of pyramid levels
+
+ \return pyramidLevels
+ */
+ inline unsigned int getPyramidLevels() const {return pyramidLevels;}
+
+ /*!
+ Get the quality of the KLT.
+
+ \return quality
+ */
+ inline double getQuality() const {return qualityValue;}
+
+ /*!
+ Get the size of the window used in the KLT tracker.
+
+ \return winSize
+ */
+ inline unsigned int getWindowSize() const {return winSize;}
+
+ void parse(const char * filename);
+
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+ void read_klt(xmlDocPtr doc, xmlNodePtr node);
+
+ /*!
+ Set the size of a block.
+
+ \param bs : New blockSize
+ */
+ inline void setBlockSize(const unsigned int &bs) {blockSize = bs;}
+
+ /*!
+ Set the Harris free parameter.
+
+ \param hp : New harrisParam
+ */
+ inline void setHarrisParam(const double &hp) {harrisParam = hp;}
+
+ /*!
+ Set the Border of the mask.
+
+ \param mb = new maskBorder
+ */
+ inline void setMaskBorder(const unsigned int &mb) {maskBorder = mb;}
+
+ /*!
+ Set the maximum number of features for the KLT.
+
+ \param mF : New maxFeatures
+ */
+ inline void setMaxFeatures(const unsigned int &mF) {maxFeatures = mF;}
+
+ /*!
+ Set the minimum distance between KLT points.
+
+ \param mD : New minDist
+ */
+ inline void setMinDistance(const double &mD) {minDist = mD;}
+
+ /*!
+ Set the number of pyramid levels
+
+ \param pL : New pyramidLevels
+ */
+ inline void setPyramidLevels(const unsigned int &pL) {pyramidLevels = pL;}
+
+ /*!
+ Set the quality of the KLT.
+
+ \param q : New quality
+ */
+ inline void setQuality(const double &q) {qualityValue = q;}
+
+ /*!
+ Set the size of the window used in the KLT tracker.
+
+ \param w : New winSize
+ */
+ inline void setWindowSize(const unsigned int &w) {winSize = w;}
+
+ void writeMainClass(xmlNodePtr node);
+
+protected:
+ void init();
+
+};
+
+#endif
+
+#endif
+
+
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h
new file mode 100644
index 0000000..f2ccd25
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h
@@ -0,0 +1,242 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtMeEllipse.h
+ \brief Moving edges on an ellipse
+*/
+
+#ifndef vpMbtMeEllipse_HH
+#define vpMbtMeEllipse_HH
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
+
+#include <math.h>
+#include <list>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ \class vpMbtMeEllipse
+ \ingroup group_mbt_features
+
+ \brief Class that tracks an ellipse moving edges.
+
+ In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
+
+ \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
+
+ If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
+
+ The five parameters are stored in the public attribute K.
+
+ An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
+ major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
+
+ \image html vpMbtMeEllipse.gif
+ \image latex vpMbtMeEllipse.ps width=10cm
+
+ It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
+
+ \f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
+ \f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
+
+ Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
+
+ \image html vpMbtMeEllipse2.gif
+ \image latex vpMbtMeEllipse2.ps width=10cm
+
+ The example below available in tutorial-me-ellipse-tracker.cpp and described
+ in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
+
+ \include tutorial-me-ellipse-tracker.cpp
+
+ */
+/*
+ The code below shows how to use this class.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/mbt/vpMbtMeEllipse.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImage<unsigned char> I;
+
+ // I is the image containing the ellipse to track
+
+ // Set the moving-edges tracker parameters
+ vpMe me;
+ me.setRange(25);
+ me.setPointsToTrack(20);
+ me.setThreshold(15000);
+ me.setSampleStep(10);
+
+ // Initialize the moving-edges ellipse tracker parameters
+ vpMbtMeEllipse ellipse;
+ ellipse.setMe(&me);
+
+ // Initialize the tracking. You have to click on five different points belonging to the ellipse.
+ ellipse.initTracking(I);
+
+ while ( 1 )
+ {
+ // ... Here the code to read or grab the next image.
+
+ // Track the ellipse.
+ ellipse.track(I);
+ }
+ return 0;
+}
+\endcode
+
+ \note It is possible to display the ellipse as an overlay. For that you
+ must use the display function of the class vpMbtMeEllipse.
+*/
+
+class VISP_EXPORT vpMbtMeEllipse : public vpMeTracker
+{
+public:
+ vpMbtMeEllipse() ;
+ vpMbtMeEllipse(const vpMbtMeEllipse &meellipse) ;
+ virtual ~vpMbtMeEllipse() ;
+
+ void computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures);
+
+ void display(const vpImage<unsigned char>&I, vpColor col) ;
+ void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
+ /*!
+ \return Expected number of moving edges to track along the ellipse.
+ */
+ int getExpectedDensity() {return (int)expecteddensity;};
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{11} \f$.
+
+ \return the value of \f$ \mu_{11} \f$.
+ */
+ inline double get_mu11() const {return mu11;}
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{02} \f$.
+
+ \return the value of \f$ \mu_{02} \f$.
+ */
+ inline double get_mu02() const {return mu02;}
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{20} \f$.
+
+ \return the value of \f$ \mu_{20} \f$.
+ */
+ inline double get_mu20() const {return mu20;}
+
+ /*!
+ Gets the center of the ellipse.
+ */
+ inline vpImagePoint getCenter() const {return iPc; }
+
+ /*!
+ Gets the semiminor axis of the ellipse.
+ */
+ inline double getA() const {return a; }
+
+ /*!
+ Gets the semimajor axis of the ellipse.
+ */
+ inline double getB() const {return b; }
+
+ /*!
+ Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+ */
+ inline double getE() const {return e; }
+
+ /*!
+ Gets the parameters a, b, e of the ellipse.
+ */
+ void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
+
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
+ double mu20_p, double mu11_p, double mu02_p) ;
+
+ void track(const vpImage<unsigned char>& Im);
+
+ void updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic,
+ double mu20_p, double mu11_p, double mu02_p);
+
+
+protected:
+ //! The coordinates of the ellipse center.
+ vpImagePoint iPc;
+ //! \f$ a \f$ is the semiminor axis of the ellipse.
+ double a;
+ //! \f$ b \f$ is the semimajor axis of the ellipse.
+ double b;
+ //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
+ double e;
+
+protected:
+ //! Value of cos(e).
+ double ce;
+ //! Value of sin(e).
+ double se;
+
+ //! Second order central moments
+ double mu11,mu20, mu02;
+ //! Threshold for the robust least square.
+ double thresholdWeight;
+ //! Expected number of me to track along the ellipse.
+ double expecteddensity;
+
+private:
+ void sample(const vpImage<unsigned char>&image);
+ void reSample(const vpImage<unsigned char> &I) ;
+ void updateTheta();
+ void suppressPoints() ;
+};
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h
new file mode 100644
index 0000000..d19c0f1
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation of a line used by the model-based tracker.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtMeLine.h
+ \brief Implementation of a line used by the model-based tracker.
+*/
+
+#ifndef vpMbtMeLine_HH
+#define vpMbtMeLine_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeTracker.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ \class vpMbtMeLine
+ \brief Implementation of a line used by the model-based tracker.
+ \ingroup group_mbt_features
+
+ */
+class VISP_EXPORT vpMbtMeLine : public vpMeTracker
+{
+ private:
+ vpMeSite PExt[2] ;
+ double rho, theta, theta_1;
+ double delta ,delta_1;
+ int sign;
+ double a,b,c;
+
+ public:
+ int imin, imax;
+ int jmin, jmax;
+ double expecteddensity;
+
+ public:
+ vpMbtMeLine();
+ ~vpMbtMeLine();
+
+ void computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures);
+
+ void display(const vpImage<unsigned char>& /*I*/, vpColor /*col*/) {;}
+ void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
+
+ \return : The a coefficient of the moving edge
+ */
+ inline double get_a() const { return this->a;}
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
+
+ \return : The b coefficient of the moving edge
+ */
+ inline double get_b() const { return this->b;}
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
+
+ \return : The c coefficient of the moving edge
+ */
+ inline double get_c() const { return this->c;}
+
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, double theta);
+
+ void track(const vpImage<unsigned char> &I);
+
+ void updateParameters(const vpImage<unsigned char> &I, double rho, double theta);
+ void updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2, double rho, double theta);
+
+ private:
+ void bubbleSortI();
+ void bubbleSortJ();
+ void findSignal(const vpImage<unsigned char>& I, const vpMe *me, double *conv);
+ void sample(const vpImage<unsigned char>&image);
+ void seekExtremities(const vpImage<unsigned char> &I);
+ void setExtremities();
+ void suppressPoints(const vpImage<unsigned char> &I);
+ void reSample(const vpImage<unsigned char>&image);
+ void reSample(const vpImage<unsigned char>&image, vpImagePoint ip1, vpImagePoint ip2);
+ void updateDelta();
+} ;
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h
new file mode 100644
index 0000000..a68ea6f
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h
@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implements a polygon of the model used by the model-based tracker.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbtPolygon.h
+ \brief Implements a polygon of the model used by the model-based tracker.
+*/
+
+#ifndef vpMbtPolygon_HH
+#define vpMbtPolygon_HH
+
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPolygon3D.h>
+
+#include <vector>
+
+/*!
+ \class vpMbtPolygon
+
+ \brief Implementation of a polygon of the model used by the model-based tracker.
+
+ \ingroup group_mbt_faces
+
+ */
+class VISP_EXPORT vpMbtPolygon : public vpPolygon3D
+{
+public:
+ //! Index of the polygon. Cannot be unsigned int because default value is -1.
+ int index;
+ //! flag to specify whether the face is visible or not
+ bool isvisible;
+ //! flag to specify whether the face is appearing or not
+ bool isappearing;
+ //! Flag to specify if the visibility of the polygon depends also of the current level of detail (LOD)
+ bool useLod;
+ //! Threshold for minimum line length in pixel to consider if the line is visible or not in LOD case
+ double minLineLengthThresh;
+ //! Threshold for minimum polygon area in pixel to consider if the polygon is visible or not in LOD case
+ double minPolygonAreaThresh;
+ //! Name of the polygon
+ std::string name;
+ //! Boolean that specify if the polygon has an orientation or not (mainly used for cylinders)
+ bool hasOrientation;
+
+public:
+ vpMbtPolygon() ;
+ vpMbtPolygon(const vpMbtPolygon& mbtp) ;
+ virtual ~vpMbtPolygon() ;
+
+ /*!
+ Get the index of the face.
+
+ \return index : the index of the face.
+ */
+ inline int getIndex() const {return index ;}
+
+ /*!
+ Get the name of the face.
+
+ \return Name of the face.
+ */
+ inline std::string getName() const {return name;}
+
+ inline bool isAppearing() const {return isappearing;}
+ inline bool isPolygonOriented() { return hasOrientation; }
+ virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo = false,
+ const vpCameraParameters &cam = vpCameraParameters(), const vpImage<unsigned char> &I = vpImage<unsigned char>());
+ bool isVisible() const {return isvisible;}
+
+ vpMbtPolygon& operator=(const vpMbtPolygon& mbtp) ;
+
+ /*!
+ Set the index of the face.
+
+ \param i : the new index of the face.
+ */
+ virtual inline void setIndex(const int i ) { index = i ; }
+
+ // Due to a doxygen warning include the sample code in the doc, we remove the inline and put the doc in the *.cpp file
+ void setLod(const bool use_lod);
+ /*!
+ Set the threshold for the minimum line length to be considered as visible in the LOD
+ (level of detail) case. This threshold is only used when setLoD() is turned on.
+
+ \param min_line_length : threshold for the minimum line length in pixel. When a single line that doesn't
+ belong to a face is considered by the tracker, this line is tracked only if its lenght in pixel is
+ greater than \e min_line_length.
+
+ \sa setLoD()
+ */
+ inline void setMinLineLengthThresh(const double min_line_length) {
+ this->minLineLengthThresh = min_line_length;
+ }
+ /*!
+ Set the minimum polygon area to be considered as visible in the LOD (level of detail)
+ case. This threshold is only used when setLoD() is turned on.
+
+ \param min_polygon_area : threshold for the minimum polygon area in pixel. When a face
+ is considered by the tracker, this face is tracked only if its area in pixel is
+ greater than \e min_polygon_area.
+
+ \sa setLoD()
+ */
+ inline void setMinPolygonAreaThresh(const double min_polygon_area) {
+ this->minPolygonAreaThresh = min_polygon_area;
+ }
+
+ /*!
+ Set the name of the face.
+
+ \param face_name : name of the face.
+ */
+ inline void setName(const std::string &face_name) {
+ this->name = face_name;
+ }
+
+ /*!
+ Set if the polygon is oriented or not.
+
+ \param oriented : True if the polygon is oriented, false otherwise.
+ */
+ inline void setIsPolygonOriented(const bool &oriented) {
+ this->hasOrientation = oriented;
+ }
+};
+
+#endif
+
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h
new file mode 100644
index 0000000..7d2810d
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h
@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using edges).
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ * \file vpMbtXmlParser.h
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+*/
+
+#ifndef vpMbtXmlParser_HH
+#define vpMbtXmlParser_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbXmlParser.h>
+#include <visp3/me/vpMe.h>
+
+/*!
+ \class vpMbtXmlParser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
+ \ingroup group_mbt_xml_parser
+
+ Data parser for the model based tracker.
+
+ */
+class VISP_EXPORT vpMbtXmlParser: virtual public vpMbXmlParser
+{
+protected:
+ typedef enum{
+ ecm = vpMbXmlParser::last,
+ mask,
+ size,
+ nb_mask,
+ range,
+ tracking,
+ contrast,
+ edge_threshold,
+ mu1,
+ mu2,
+ sample,
+ step,
+ last
+ } dataToParseMb;
+
+ //! Moving edges parameters.
+ vpMe m_ecm;
+
+
+public:
+
+ vpMbtXmlParser();
+ virtual ~vpMbtXmlParser();
+
+ void getMe(vpMe& _ecm) const { _ecm = this->m_ecm;}
+
+ void parse(const char * filename);
+
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+ void read_ecm (xmlDocPtr doc, xmlNodePtr node);
+ void read_sample (xmlDocPtr doc, xmlNodePtr node);
+ void read_sample_deprecated (xmlDocPtr doc, xmlNodePtr node);
+ void read_mask (xmlDocPtr doc, xmlNodePtr node);
+ void read_range (xmlDocPtr doc, xmlNodePtr node);
+ void read_contrast (xmlDocPtr doc, xmlNodePtr node);
+
+ void setMovingEdge(const vpMe &_ecm){ m_ecm = _ecm; }
+
+ void writeMainClass(xmlNodePtr node);
+
+protected:
+ void init();
+
+};
+
+#endif
+
+#endif /* NMBTXMLPARSER_H_ */
+
+
+
diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp
new file mode 100644
index 0000000..0696a03
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp
@@ -0,0 +1,2862 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbEdgeTracker.cpp
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
+#include <visp3/mbt/vpMbtXmlParser.h>
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+#include <limits>
+#include <string>
+#include <sstream>
+#include <float.h>
+#include <map>
+
+bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold);
+
+/*!
+ Basic constructor
+*/
+vpMbEdgeTracker::vpMbEdgeTracker()
+ : compute_interaction(1), lambda(1), me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0),
+ nbvisiblepolygone(0), percentageGdPt(0.4), scales(1),
+ Ipyramid(0), scaleLevel(0)
+{
+ angleAppears = vpMath::rad(89);
+ angleDisappears = vpMath::rad(89);
+
+ scales[0] = true;
+
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Edge");
+#endif
+}
+
+/*!
+ Basic destructor useful to deallocate the memory.
+*/
+vpMbEdgeTracker::~vpMbEdgeTracker()
+{
+ vpMbtDistanceLine *l ;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if (l!=NULL){
+ delete l ;
+ }
+ l = NULL ;
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if (cy!=NULL){
+ delete cy ;
+ }
+ cy = NULL ;
+ }
+
+ lines[i].clear();
+ cylinders[i].clear();
+ }
+ }
+ for (unsigned int i = 0; i < circles.size(); i += 1){
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if (ci!=NULL){
+ delete ci ;
+ }
+ ci = NULL ;
+ }
+ circles[i].clear();
+ }
+ cleanPyramid(Ipyramid);
+}
+
+/*!
+ Set the moving edge parameters.
+
+ \param p_me : an instance of vpMe containing all the desired parameters
+*/
+void
+vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
+{
+ this->me = p_me;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ (*it)->setMovingEdge(&(this->me)) ;
+ }
+
+ vpMbtDistanceCylinder *cy;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ cy->setMovingEdge(&(this->me)) ;
+ }
+
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ ci->setMovingEdge(&(this->me)) ;
+ }
+ }
+ }
+}
+
+/*!
+ Compute the visual servoing loop to get the pose of the feature set.
+
+ \exception vpTrackingException::notEnoughPointError if the number of detected
+ feature is equal to zero.
+
+ \param _I : The current image.
+ */
+void
+vpMbEdgeTracker::computeVVS(const vpImage<unsigned char>& _I)
+{
+ double residu_1 =1e3;
+ double r =1e3-1;
+ vpMatrix LTL;
+ vpColVector LTR;
+
+ // compute the interaction matrix and its pseudo inverse
+ vpMbtDistanceLine *l ;
+ vpMbtDistanceCylinder *cy ;
+ vpMbtDistanceCircle *ci ;
+
+ //vpColVector w;
+ vpColVector factor;
+ //vpColVector error; // s-s*
+ vpColVector weighted_error; // Weighted error vector wi(s-s)*
+ vpVelocityTwistMatrix cVo;
+
+ unsigned int iter = 0;
+
+ //Nombre de moving edges
+ unsigned int nbrow = 0;
+ unsigned int nberrors_lines = 0;
+ unsigned int nberrors_cylinders = 0;
+ unsigned int nberrors_circles = 0;
+
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+
+ l = *it;
+
+ if(l->isVisible() && l->isTracked())
+ {
+ nbrow += l->nbFeatureTotal;
+ nberrors_lines+=l->nbFeatureTotal;
+ l->initInteractionMatrixError();
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ cy = *it;
+
+ if(cy->isVisible() && cy->isTracked())
+ {
+ nbrow += cy->nbFeature ;
+ nberrors_cylinders += cy->nbFeature ;
+ cy->initInteractionMatrixError() ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ ci = *it;
+
+ if(ci->isVisible() && ci->isTracked())
+ {
+ nbrow += ci->nbFeature ;
+ nberrors_circles += ci->nbFeature ;
+ ci->initInteractionMatrixError() ;
+ }
+ }
+
+ if (nbrow==0){
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
+ }
+
+ vpMatrix L(nbrow,6), Lp;
+
+ // compute the error vector
+ m_error.resize(nbrow);
+ unsigned int nerror = m_error.getRows();
+ vpColVector v ;
+
+ double limite = 3; //Une limite de 3 pixels
+ limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
+
+ //Parametre pour la premiere phase d'asservissement
+ double e_prev = 0, e_cur, e_next;
+ bool reloop = true;
+ double count = 0;
+
+ bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
+ if (isoJoIdentity_)
+ oJo.eye();
+
+ /*** First phase ***/
+
+ while ( reloop == true && iter<10)
+ {
+ if(iter==0)
+ {
+ weighted_error.resize(nerror) ;
+ m_w.resize(nerror);
+ m_w = 0;
+ factor.resize(nerror);
+ factor = 1;
+ }
+
+ count = 0;
+
+ unsigned int n = 0;
+ reloop = false;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked())
+ {
+ l = *it;
+ l->computeInteractionMatrixError(cMo);
+
+ double fac = 1;
+ if (iter == 0)
+ {
+ for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ int index = *itindex;
+ if (l->hiddenface->isAppearing((unsigned int)index))
+ {
+ fac = 0.2;
+ break;
+ }
+ if(l->closeToImageBorder(_I, 10))
+ {
+ fac = 0.1;
+ break;
+ }
+ }
+ }
+
+ std::list<vpMeSite>::const_iterator itListLine;
+
+ unsigned int indexFeature = 0;
+
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++)
+ {
+ if (iter == 0 && l->meline[a] != NULL)
+ itListLine = l->meline[a]->getMeList().begin();
+
+ for (unsigned int i=0 ; i < l->nbFeature[a] ; i++)
+ {
+ for (unsigned int j=0; j < 6 ; j++)
+ {
+ L[n+i][j] = l->L[indexFeature][j]; //On remplit la matrice d'interaction globale
+ }
+ m_error[n+i] = l->error[indexFeature]; //On remplit la matrice d'erreur
+
+ if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+
+ m_w[n+i] = 0;
+
+ if (iter == 0)
+ {
+ factor[n+i] = fac;
+ vpMeSite site = *itListLine;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ ++itListLine;
+ }
+
+ //If pour la premiere extremite des moving edges
+ if (indexFeature == 0)
+ {
+ e_cur = l->error[0];
+ if (l->nbFeature[a] > 1)
+ {
+ e_next = l->error[1];
+ if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
+ {
+ m_w[n+i] = 1/*0.5*/;
+ }
+ e_prev = e_cur;
+ }
+ else m_w[n+i] = 1;
+ }
+
+ //If pour la derniere extremite des moving edges
+ else if(indexFeature == l->nbFeatureTotal-1)
+ {
+ e_cur = l->error[indexFeature];
+ if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
+ {
+ m_w[n+i] += 1/*0.5*/;
+ }
+ }
+
+ else
+ {
+ e_cur = l->error[indexFeature];
+ e_next = l->error[indexFeature+1];
+ if ( fabs(e_cur - e_prev) < limite )
+ {
+ m_w[n+i] += 0.5;
+ }
+ if ( fabs(e_cur - e_next) < limite )
+ {
+ m_w[n+i] += 0.5;
+ }
+ e_prev = e_cur;
+ }
+ indexFeature++;
+ }
+ n += l->nbFeature[a] ;
+ }
+ }
+ }
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked())
+ {
+ cy = *it;
+ cy->computeInteractionMatrixError(cMo, _I);
+ double fac = 1.0;
+
+ std::list<vpMeSite>::const_iterator itCyl1;
+ std::list<vpMeSite>::const_iterator itCyl2;
+ if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
+ itCyl1 = cy->meline1->getMeList().begin();
+ itCyl2 = cy->meline2->getMeList().begin();
+ }
+
+ for(unsigned int i=0 ; i < cy->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
+ }
+ m_error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
+
+ if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+
+ m_w[n+i] = 0;
+
+ if (iter == 0)
+ {
+ factor[n+i] = fac;
+ vpMeSite site;
+ if(i<cy->nbFeaturel1) {
+ site= *itCyl1;
+ ++itCyl1;
+ }
+ else{
+ site= *itCyl2;
+ ++itCyl2;
+ }
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ }
+
+ //If pour la premiere extremite des moving edges
+ if (i == 0)
+ {
+ e_cur = cy->error[0];
+ if (cy->nbFeature > 1)
+ {
+ e_next = cy->error[1];
+ if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
+ {
+ m_w[n+i] = 1/*0.5*/;
+ }
+ e_prev = e_cur;
+ }
+ else m_w[n+i] = 1;
+ }
+ if (i == cy->nbFeaturel1)
+ {
+ e_cur = cy->error[i];
+ if (cy->nbFeaturel2 > 1)
+ {
+ e_next = cy->error[i+1];
+ if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
+ {
+ m_w[n+i] = 1/*0.5*/;
+ }
+ e_prev = e_cur;
+ }
+ else m_w[n+i] = 1;
+ }
+
+ //If pour la derniere extremite des moving edges
+ else if(i == cy->nbFeaturel1-1)
+ {
+ e_cur = cy->error[i];
+ if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
+ {
+ m_w[n+i] += 1/*0.5*/;
+ }
+ }
+ //If pour la derniere extremite des moving edges
+ else if(i == cy->nbFeature-1)
+ {
+ e_cur = cy->error[i];
+ if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
+ {
+ m_w[n+i] += 1/*0.5*/;
+ }
+ }
+
+ else
+ {
+ e_cur = cy->error[i];
+ e_next = cy->error[i+1];
+ if ( fabs(e_cur - e_prev) < limite ){
+ m_w[n+i] += 0.5;
+ }
+ if ( fabs(e_cur - e_next) < limite ){
+ m_w[n+i] += 0.5;
+ }
+ e_prev = e_cur;
+ }
+ }
+
+ n+= cy->nbFeature ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked())
+ {
+ ci = *it;
+ ci->computeInteractionMatrixError(cMo);
+ double fac = 1.0;
+
+ std::list<vpMeSite>::const_iterator itCir;
+ if (iter == 0 && (ci->meEllipse != NULL)) {
+ itCir = ci->meEllipse->getMeList().begin();
+ }
+
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = ci->L[i][j]; //On remplit la matrice d'interaction globale
+ }
+ m_error[n+i] = ci->error[i]; //On remplit la matrice d'erreur
+
+ if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+
+ m_w[n+i] = 0;
+
+ if (iter == 0)
+ {
+ factor[n+i] = fac;
+ vpMeSite site = *itCir;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ ++itCir;
+ }
+
+ //If pour la premiere extremite des moving edges
+ if (i == 0)
+ {
+ e_cur = ci->error[0];
+ if (ci->nbFeature > 1)
+ {
+ e_next = ci->error[1];
+ if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
+ {
+ m_w[n+i] = 1/*0.5*/;
+ }
+ e_prev = e_cur;
+ }
+ else m_w[n+i] = 1;
+ }
+
+ //If pour la derniere extremite des moving edges
+ else if(i == ci->nbFeature-1)
+ {
+ e_cur = ci->error[i];
+ if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
+ {
+ m_w[n+i] += 1/*0.5*/;
+ }
+ }
+
+ else
+ {
+ e_cur = ci->error[i];
+ e_next = ci->error[i+1];
+ if ( fabs(e_cur - e_prev) < limite ){
+ m_w[n+i] += 0.5;
+ }
+ if ( fabs(e_cur - e_next) < limite ){
+ m_w[n+i] += 0.5;
+ }
+ e_prev = e_cur;
+ }
+ }
+
+ n+= ci->nbFeature ;
+ }
+ }
+
+ count = count / (double)nbrow;
+ if (count < 0.85){
+ reloop = true;
+ }
+
+ double num=0;
+ double den=0;
+
+ double wi ; double eri ;
+ for(unsigned int i = 0; i < nerror; i++){
+ wi = m_w[i]*factor[i];
+ eri = m_error[i];
+ num += wi*vpMath::sqr(eri);
+ den += wi ;
+
+ weighted_error[i] = wi*eri ;
+ }
+
+ if((iter==0) || compute_interaction){
+ for (unsigned int i=0 ; i < nerror ; i++){
+ for (unsigned int j=0 ; j < 6 ; j++){
+ L[i][j] = m_w[i]*factor[i]*L[i][j] ;
+ }
+ }
+ }
+
+ // If all the 6 dof should be estimated, we check if the interaction matrix is full rank.
+ // If not we remove automatically the dof that cannot be estimated
+ // This is particularly useful when consering circles (rank 5) and cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+
+ vpMatrix K; // kernel
+ unsigned int rank = (L*cVo).kernel(K);
+ if(rank == 0) {
+ throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
+ }
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I-K.AtA();
+
+ isoJoIdentity_ = false;
+ }
+ }
+
+ if(isoJoIdentity_){
+ LTL = L.AtA();
+ computeJTR(L, weighted_error, LTR);
+ v = -0.7*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+ }
+ else{
+ cVo.buildFrom(cMo);
+ vpMatrix LVJ = (L*cVo*oJo);
+ vpMatrix LVJTLVJ = (LVJ).AtA();
+ vpColVector LVJTR;
+ computeJTR(LVJ, weighted_error, LVJTR);
+ v = -0.7*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+ }
+
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+
+ iter++;
+ }
+
+// std::cout << "\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl ;
+
+
+/*** Second phase ***/
+
+ vpRobust robust_lines(nberrors_lines);
+ vpRobust robust_cylinders(nberrors_cylinders);
+ vpRobust robust_circles(nberrors_circles);
+ robust_lines.setIteration(0) ;
+ robust_cylinders.setIteration(0) ;
+ robust_circles.setIteration(0) ;
+ iter = 0;
+ vpColVector w_lines(nberrors_lines);
+ vpColVector w_cylinders(nberrors_cylinders);
+ vpColVector w_circles(nberrors_circles);
+ vpColVector error_lines(nberrors_lines);
+ vpColVector error_cylinders(nberrors_cylinders);
+ vpColVector error_circles(nberrors_circles);
+
+ vpHomogeneousMatrix cMoPrev;
+ vpColVector W_true;
+ vpMatrix L_true;
+ vpMatrix LVJ_true;
+
+ double mu = 0.01;
+ vpColVector m_error_prev(nbrow);
+ vpColVector m_w_prev(nbrow);
+
+ while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
+ {
+ unsigned int n = 0 ;
+ unsigned int nlines = 0;
+ unsigned int ncylinders = 0;
+ unsigned int ncircles = 0;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ l->computeInteractionMatrixError(cMo) ;
+ for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
+ for (unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = l->L[i][j];
+ m_error[n+i] = l->error[i];
+ error_lines[nlines+i] = m_error[n+i];
+ }
+ }
+ n+= l->nbFeatureTotal;
+ nlines+= l->nbFeatureTotal;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ cy->computeInteractionMatrixError(cMo, _I) ;
+ for(unsigned int i=0 ; i < cy->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = cy->L[i][j];
+ m_error[n+i] = cy->error[i];
+ error_cylinders[ncylinders+i] = m_error[n+i];
+ }
+ }
+
+ n+= cy->nbFeature ;
+ ncylinders+= cy->nbFeature ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ ci->computeInteractionMatrixError(cMo) ;
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = ci->L[i][j];
+ m_error[n+i] = ci->error[i];
+ error_circles[ncircles+i] = m_error[n+i];
+ }
+ }
+
+ n+= ci->nbFeature ;
+ ncircles+= ci->nbFeature ;
+ }
+ }
+
+ bool reStartFromLastIncrement = false;
+ if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
+ if(m_error.sumSquare()/(double)nbrow > m_error_prev.sumSquare()/(double)nbrow){
+ mu *= 10.0;
+
+ if(mu > 1.0)
+ throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+
+ cMo = cMoPrev;
+ m_error = m_error_prev;
+ m_w = m_w_prev;
+ reStartFromLastIncrement = true;
+ }
+ }
+
+ if(!reStartFromLastIncrement){
+ if(iter==0)
+ {
+ weighted_error.resize(nerror);
+ m_w.resize(nerror);
+ m_w = 1;
+ w_lines.resize(nberrors_lines);
+ w_lines = 1;
+ w_cylinders.resize(nberrors_cylinders);
+ w_cylinders = 1;
+ w_circles.resize(nberrors_circles);
+ w_circles = 1;
+
+ robust_lines.setThreshold(2/cam.get_px());
+ robust_cylinders.setThreshold(2/cam.get_px());
+ robust_circles.setThreshold(vpMath::sqr(2/cam.get_px()));
+ if(nberrors_lines > 0)
+ robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
+ if(nberrors_cylinders > 0)
+ robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
+ if(nberrors_circles > 0)
+ robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
+ }
+ else
+ {
+ robust_lines.setIteration(iter);
+ robust_cylinders.setIteration(iter);
+ robust_circles.setIteration(iter);
+ if(nberrors_lines > 0)
+ robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
+ if(nberrors_cylinders > 0)
+ robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
+ if(nberrors_circles > 0)
+ robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
+ }
+
+ unsigned int cpt = 0;
+ while(cpt<nbrow){
+ if(cpt<nberrors_lines){
+ m_w[cpt] = w_lines[cpt];
+ }
+ else if (cpt<nberrors_lines+nberrors_cylinders){
+ m_w[cpt] = w_cylinders[cpt-nberrors_lines];
+ }
+ else {
+ m_w[cpt] = w_circles[cpt-nberrors_lines-nberrors_cylinders];
+ }
+ cpt++;
+ }
+
+ double num=0;
+ double den=0;
+ double wi;
+ double eri;
+
+ L_true = L;
+ W_true = vpColVector(nerror);
+
+ if(computeCovariance){
+ L_true = L;
+ if(!isoJoIdentity_){
+ cVo.buildFrom(cMo);
+ LVJ_true = (L*cVo*oJo);
+ }
+ }
+
+ for(unsigned int i=0; i<nerror; i++){
+ wi = m_w[i]*factor[i];
+ W_true[i] = wi;
+ eri = m_error[i];
+ num += wi*vpMath::sqr(eri);
+ den += wi;
+
+ weighted_error[i] = wi*eri ;
+ }
+
+ if((iter==0)|| compute_interaction){
+ for (unsigned int i=0 ; i < nerror ; i++){
+ for (unsigned int j=0 ; j < 6 ; j++){
+ L[i][j] = m_w[i]*factor[i]*L[i][j];
+ }
+ }
+ }
+
+ if(isoJoIdentity_){
+ LTL = L.AtA();
+ computeJTR(L, weighted_error, LTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LTL.getRows(), LTL.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LTL + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ m_w_prev = m_w;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ v = -lambda*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+ }
+ }
+ else{
+ cVo.buildFrom(cMo);
+ vpMatrix LVJ = (L*cVo*oJo);
+ vpMatrix LVJTLVJ = (LVJ).AtA();
+ vpColVector LVJTR;
+ computeJTR(LVJ, weighted_error, LVJTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ m_w_prev = m_w;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ {
+ v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+ break;
+ }
+ }
+ }
+
+ residu_1 = r;
+ r = sqrt(num/den); //Le critere d'arret prend en compte le poids
+
+ cMoPrev = cMo;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+
+ } // endif(!restartFromLast)
+
+ iter++;
+ }
+
+ // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
+ if(computeCovariance){
+ vpMatrix D;
+ D.diag(W_true);
+
+ // Note that here the covariance is computed on cMoPrev for time computation efficiency
+ if(isoJoIdentity_){
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
+ }
+ else{
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
+ }
+ }
+
+ unsigned int n =0 ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ unsigned int indexLine = 0;
+ double wmean = 0 ;
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++)
+ {
+ std::list<vpMeSite>::iterator itListLine;
+ if (l->nbFeature[a] > 0) itListLine = l->meline[a]->getMeList().begin();
+
+ for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
+ wmean += m_w[n+indexLine] ;
+ vpMeSite p = *itListLine;
+ if (m_w[n+indexLine] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListLine = p;
+ }
+
+ ++itListLine;
+ indexLine++;
+ }
+ }
+ n+= l->nbFeatureTotal ;
+
+ if (l->nbFeatureTotal!=0)
+ wmean /= l->nbFeatureTotal ;
+ else
+ wmean = 1;
+
+ l->setMeanWeight(wmean);
+
+ if (wmean < 0.8)
+ l->Reinit = true;
+ }
+ }
+
+ // Same thing with cylinders as with lines
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ double wmean = 0 ;
+ std::list<vpMeSite>::iterator itListCyl1;
+ std::list<vpMeSite>::iterator itListCyl2;
+
+ if (cy->nbFeature > 0){
+ itListCyl1 = cy->meline1->getMeList().begin();
+ itListCyl2 = cy->meline2->getMeList().begin();
+ }
+
+ wmean = 0;
+ for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
+ wmean += m_w[n+i] ;
+ vpMeSite p = *itListCyl1;
+ if (m_w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCyl1 = p;
+ }
+
+ ++itListCyl1;
+ }
+
+ if (cy->nbFeaturel1!=0)
+ wmean /= cy->nbFeaturel1 ;
+ else
+ wmean = 1;
+
+ cy->setMeanWeight1(wmean);
+
+ if (wmean < 0.8){
+ cy->Reinit = true;
+ }
+
+ wmean = 0;
+ for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
+ wmean += m_w[n+i] ;
+ vpMeSite p = *itListCyl2;
+ if (m_w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCyl2 = p;
+ }
+
+ ++itListCyl2;
+ }
+
+ if (cy->nbFeaturel2!=0)
+ wmean /= cy->nbFeaturel2 ;
+ else
+ wmean = 1;
+
+ cy->setMeanWeight2(wmean);
+
+ if (wmean < 0.8){
+ cy->Reinit = true;
+ }
+
+ n+= cy->nbFeature ;
+ }
+ }
+
+ // Same thing with circles as with lines
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ double wmean = 0 ;
+ std::list<vpMeSite>::iterator itListCir;
+
+ if (ci->nbFeature > 0){
+ itListCir = ci->meEllipse->getMeList().begin();
+ }
+
+ wmean = 0;
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ wmean += m_w[n+i] ;
+ vpMeSite p = *itListCir;
+ if (m_w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCir = p;
+ }
+
+ ++itListCir;
+ }
+
+ if (ci->nbFeature!=0)
+ wmean /= ci->nbFeature ;
+ else
+ wmean = 1;
+
+ ci->setMeanWeight(wmean);
+
+ if (wmean < 0.8){
+ ci->Reinit = true;
+ }
+
+ n+= ci->nbFeature ;
+ }
+ }
+}
+
+/*!
+ Compute the projection error of the model.
+ This approach compares the gradient direction around samples of each lines of the model with their direction.
+ Error is expressed in degrees between 0 and 90;
+
+ \param _I : Image in which the model appears.
+*/
+void
+vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
+{
+ vpMbtDistanceLine *l ;
+ projectionError = 0.0;
+ unsigned int nbFeatures = 0;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ l = *it;
+ if (l->isVisible() && l->isTracked())
+ {
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++){
+ if(l->meline[a] != NULL){
+ double lineNormGradient;
+ unsigned int lineNbFeatures;
+ l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures);
+ projectionError += lineNormGradient;
+ nbFeatures += lineNbFeatures;
+ }
+ }
+ }
+ }
+
+ vpMbtDistanceCylinder *cy;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ cy = *it;
+ if (cy->isVisible() && cy->isTracked())
+ {
+ if(cy->meline1 != NULL)
+ {
+ double cylinderNormGradient = 0;
+ unsigned int cylinderNbFeatures = 0;
+ cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
+ projectionError += cylinderNormGradient;
+ nbFeatures += cylinderNbFeatures;
+ }
+
+ if(cy->meline2 != NULL)
+ {
+ double cylinderNormGradient = 0;
+ unsigned int cylinderNbFeatures = 0;
+ cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
+ projectionError += cylinderNormGradient;
+ nbFeatures += cylinderNbFeatures;
+ }
+ }
+ }
+
+ vpMbtDistanceCircle *c;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ c = *it;
+ if (c->isVisible() && c->isTracked() && c->meEllipse != NULL)
+ {
+ double circleNormGradient = 0;
+ unsigned int circleNbFeatures = 0;
+ c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures);
+ projectionError += circleNormGradient;
+ nbFeatures += circleNbFeatures;
+ }
+ }
+
+ if(nbFeatures > 0)
+ projectionError = vpMath::deg(projectionError/(double)nbFeatures);
+ else
+ projectionError = 90.0;
+// std::cout << "Norm Gradient = " << errorGradient << std::endl;
+}
+
+/*!
+ Check if the tracking failed.
+
+ \throw vpTrackingException::fatalError if the test fails.
+*/
+void
+vpMbEdgeTracker::testTracking()
+{
+ int nbExpectedPoint = 0;
+ int nbGoodPoint = 0;
+ int nbBadPoint = 0;
+
+ vpMbtDistanceLine *l ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ l = *it;
+ if (l->isVisible() && l->isTracked())
+ {
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++){
+ if(l->meline[a] != NULL){
+ nbExpectedPoint += (int)l->meline[a]->expecteddensity;
+ for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
+ vpMeSite pix = *itme;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
+ else nbBadPoint++;
+ }
+ }
+ }
+ }
+ }
+
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ cy = *it;
+ if ((cy->meline1 !=NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked())
+ {
+ nbExpectedPoint += (int)cy->meline1->expecteddensity;
+ for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
+ vpMeSite pix = *itme1;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
+ else nbBadPoint++;
+ }
+ nbExpectedPoint += (int)cy->meline2->expecteddensity;
+ for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
+ vpMeSite pix = *itme2;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
+ else nbBadPoint++;
+ }
+ }
+ }
+
+ vpMbtDistanceCircle *ci ;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ ci = *it;
+ if (ci->isVisible() && ci->isTracked() && ci->meEllipse !=NULL)
+ {
+ nbExpectedPoint += ci->meEllipse->getExpectedDensity();
+ for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
+ vpMeSite pix = *itme;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
+ else nbBadPoint++;
+ }
+ }
+ }
+
+ // Compare the number of good points with the min between the number of expected points and
+ // number of points that are tracked
+ int nb_min = (int)vpMath::minimum(percentageGdPt *nbExpectedPoint, percentageGdPt *(nbGoodPoint + nbBadPoint) );
+ //int nb_min = std::min(val1, val2);
+ if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
+ std::ostringstream oss;
+ oss << "Not enough moving edges ("
+ << nbGoodPoint
+ << ") to track the object: expected "
+ << nb_min
+ << ". Try to reduce the threshold="
+ << percentageGdPt
+ << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
+ throw vpTrackingException(vpTrackingException::fatalError, oss.str());
+ }
+}
+
+/*!
+ Compute each state of the tracking procedure for all the feature sets.
+
+ If the tracking is considered as failed an exception is thrown.
+
+ \param I : The image.
+ */
+void
+vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
+{
+ initPyramid(I, Ipyramid);
+
+// for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
+ unsigned int lvl = (unsigned int)scales.size();
+ do{
+ lvl--;
+
+ projectionError = 90.0;
+
+ if(scales[lvl]){
+ vpHomogeneousMatrix cMo_1 = cMo;
+ try
+ {
+ downScale(lvl);
+
+ try
+ {
+ trackMovingEdge(*Ipyramid[lvl]);
+ }
+ catch(...)
+ {
+ vpTRACE("Error in moving edge tracking") ;
+ throw ;
+ }
+
+ vpMbtDistanceLine *l ;
+ vpMbtDistanceCylinder *cy ;
+ vpMbtDistanceCircle *ci ;
+ // initialize the vector that contains the error and the matrix that contains
+ // the interaction matrix
+ // AY: Useless as it is done in coputeVVS()
+ /*
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ l = *it;
+ if (l->isVisible()){
+ l->initInteractionMatrixError() ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ cy = *it;
+ if(cy->isVisible()) {
+ cy->initInteractionMatrixError();
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
+ ci = *it;
+ if (ci->isVisible()){
+ ci->initInteractionMatrixError();
+ }
+ }
+ */
+
+ try
+ {
+ computeVVS(*Ipyramid[lvl]);
+ }
+ catch(vpException &e)
+ {
+ covarianceMatrix = -1;
+ throw e;
+ }
+
+ try
+ {
+ testTracking();
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+
+ if (displayFeatures)
+ {
+ if(lvl == 0){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ l = *it;
+ if (l->isVisible() && l->isTracked()){
+ l->displayMovingEdges(I);
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ cy = *it;
+ if(cy->isVisible() && cy->isTracked()) {
+ cy->displayMovingEdges(I);
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
+ ci = *it;
+ if (ci->isVisible() && ci->isTracked()){
+ ci->displayMovingEdges(I);
+ }
+ }
+ }
+ }
+
+ // Looking for new visible face
+ bool newvisibleface = false ;
+ visibleFace(I, cMo, newvisibleface) ;
+
+ //cam.computeFov(I.getWidth(), I.getHeight());
+ if(useScanLine){
+ faces.computeClippedPolygons(cMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ try
+ {
+ updateMovingEdge(I);
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+
+ initMovingEdge(I,cMo) ;
+ // Reinit the moving edge for the lines which need it.
+ reinitMovingEdge(I,cMo);
+
+ if(computeProjError)
+ computeProjectionError(I);
+
+ upScale(lvl);
+ }
+ catch(vpException &e)
+ {
+ if(lvl != 0){
+ cMo = cMo_1;
+ reInitLevel(lvl);
+ upScale(lvl);
+ }
+ else{
+ upScale(lvl);
+ throw(e) ;
+ }
+ }
+ }
+ } while(lvl != 0);
+
+ cleanPyramid(Ipyramid);
+}
+
+/*!
+ Initialize the tracking.
+
+ \param I : The image.
+*/
+void vpMbEdgeTracker::init(const vpImage<unsigned char>& I)
+{
+ if(!modelInitialised){
+ throw vpException(vpException::fatalError, "model not initialized");
+ }
+
+ bool a = false;
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre){
+ if(!faces.isOgreInitialised()){
+ faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
+ faces.setOgreShowConfigDialog(ogreShowConfigDialog);
+ faces.initOgre(cam);
+ // Turn off Ogre config dialog display for the next call to this function
+ // since settings are saved in the ogre.cfg file and used during the next
+ // call
+ ogreShowConfigDialog = false;
+ }
+ }
+#endif
+
+
+ initPyramid(I, Ipyramid);
+ visibleFace(I, cMo, a);
+ unsigned int i = (unsigned int)scales.size();
+
+ resetMovingEdge();
+
+ if(useScanLine){
+ cam.computeFov(I.getWidth(), I.getHeight());
+ faces.computeClippedPolygons(cMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ do {
+ i--;
+ if(scales[i]){
+ downScale(i);
+ initMovingEdge(*Ipyramid[i], cMo);
+ upScale(i);
+ }
+ } while(i != 0);
+
+ cleanPyramid(Ipyramid);
+}
+
+/*!
+ Set the pose to be used in entry of the next call to the track() function.
+ This pose will be just used once.
+
+ \param I : image corresponding to the desired pose.
+ \param cdMo : Pose to affect.
+*/
+void
+vpMbEdgeTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+{
+ cMo = cdMo;
+
+ init(I);
+}
+
+/*!
+ Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
+ From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \param configFile : full name of the xml file.
+
+ \sa loadConfigFile(const char*), vpXmlParser::cleanup()
+*/
+void
+vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
+{
+ vpMbEdgeTracker::loadConfigFile(configFile.c_str());
+}
+
+/*!
+ Load the xml configuration file.
+ From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \throw vpException::ioError if the file has not been properly parsed (file not
+ found or wrong format for the data).
+
+ \param configFile : full name of the xml file.
+
+ The XML configuration file has the following form:
+ \code
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>7</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>5000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <face>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>0.90</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+ <camera>
+ <u0>320</u0>
+ <v0>240</v0>
+ <px>686.24</px>
+ <py>686.24</py>
+ </camera>
+</conf>
+ \endcode
+
+ \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
+*/
+void
+vpMbEdgeTracker::loadConfigFile(const char* configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtXmlParser xmlp;
+
+ xmlp.setCameraParameters(cam);
+ xmlp.setAngleAppear(vpMath::deg(angleAppears));
+ xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
+ xmlp.setMovingEdge(me);
+
+ try{
+ std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
+ xmlp.parse(configFile);
+ }
+ catch(...){
+ throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile);
+ }
+
+ vpCameraParameters camera;
+ vpMe meParser;
+ xmlp.getCameraParameters(camera);
+ xmlp.getMe(meParser);
+
+ setCameraParameters(camera);
+ setMovingEdge(meParser);
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ if(xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if(xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if(xmlp.getFovClipping())
+ setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);
+
+ useLodGeneral = xmlp.getLodState();
+ minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold();
+ minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
+
+ applyLodSettingInConfig = false;
+ if(this->getNbPolygon() > 0) {
+ applyLodSettingInConfig = true;
+ setLod(useLodGeneral);
+ setMinLineLengthThresh(minLineLengthThresholdGeneral);
+ setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+ }
+
+#else
+ vpTRACE("You need the libXML2 to read the config file %s", configFile);
+#endif
+}
+
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I : The image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+*/
+void
+vpMbEdgeTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor& col,
+ const unsigned int thickness, const bool displayFullModel)
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ break ; //displaying model on one scale only
+ }
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I : The image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+*/
+void
+vpMbEdgeTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor& col,
+ const unsigned int thickness, const bool displayFullModel)
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+ break ; //displaying model on one scale only
+ }
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+
+/*!
+ Initialize the moving edge thanks to a given pose of the camera.
+ The 3D model is projected into the image to create moving edges along the lines.
+
+ \param I : The image.
+ \param _cMo : The pose of the camera used to initialize the moving edges.
+*/
+void
+vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
+{
+ vpMbtDistanceLine *l ;
+
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ l = *it;
+ bool isvisible = false ;
+
+ for(std::list<int>::const_iterator itindex=l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ int index = *itindex;
+ if (index ==-1) isvisible =true ;
+ else
+ {
+ if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
+ }
+ }
+
+ //Si la ligne n'appartient a aucune face elle est tout le temps visible
+ if (l->Lindex_polygon.empty()) isvisible = true; // Not sure that this can occur
+
+ if (isvisible)
+ {
+ l->setVisible(true) ;
+ l->updateTracked();
+ if (l->meline.size() == 0 && l->isTracked())
+ l->initMovingEdge(I, _cMo) ;
+ }
+ else
+ {
+ l->setVisible(false) ;
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++){
+ if (l->meline[a] != NULL) delete l->meline[a] ;
+ l->nbFeature[a] = 0;
+ }
+ l->nbFeatureTotal = 0;
+ l->meline.clear();
+ }
+ }
+
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ cy = *it;
+
+ bool isvisible = false ;
+
+ int index = cy->index_polygon;
+ if (index ==-1) isvisible =true ;
+ else
+ {
+ if (cy->hiddenface->isVisible((unsigned int)index +1 ) ||
+ cy->hiddenface->isVisible((unsigned int)index +2 ) ||
+ cy->hiddenface->isVisible((unsigned int)index +3 ) ||
+ cy->hiddenface->isVisible((unsigned int)index +4 ))
+ isvisible = true ;
+ }
+// vpTRACE("cyl with index %d is visible: %d", index, isvisible);
+
+ if (isvisible)
+ {
+ cy->setVisible(true) ;
+ if (cy->meline1==NULL || cy->meline2==NULL){
+ if(cy->isTracked())
+ cy->initMovingEdge(I, _cMo) ;
+ }
+ }
+ else
+ {
+ cy->setVisible(false) ;
+ if (cy->meline1!=NULL) delete cy->meline1;
+ if (cy->meline2!=NULL) delete cy->meline2;
+ cy->meline1=NULL;
+ cy->meline2=NULL;
+ cy->nbFeature = 0;
+ cy->nbFeaturel1 = 0;
+ cy->nbFeaturel2= 0;
+ }
+ }
+
+ vpMbtDistanceCircle *ci ;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ ci = *it;
+ bool isvisible = false ;
+
+ int index = ci->index_polygon;
+ if (index ==-1) isvisible =true ;
+ else
+ {
+ if (ci->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
+ }
+
+ if (isvisible)
+ {
+ ci->setVisible(true) ;
+ if (ci->meEllipse==NULL)
+ {
+ if(ci->isTracked())
+ ci->initMovingEdge(I, _cMo) ;
+ }
+ }
+ else
+ {
+ ci->setVisible(false) ;
+ if (ci->meEllipse!=NULL) delete ci->meEllipse;
+ ci->meEllipse=NULL;
+ ci->nbFeature = 0;
+ }
+ }
+}
+
+
+/*!
+ Track the moving edges in the image.
+
+ \param I : the image.
+*/
+void
+vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
+{
+ vpMbtDistanceLine *l ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ l = *it;
+ if(l->isVisible() && l->isTracked()){
+ if(l->meline.size() == 0){
+ l->initMovingEdge(I, cMo);
+ }
+ l->trackMovingEdge(I, cMo) ;
+ }
+ }
+
+ vpMbtDistanceCylinder *cy;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ cy = *it;
+ if(cy->isVisible() && cy->isTracked()) {
+ if(cy->meline1 == NULL || cy->meline2 == NULL){
+ cy->initMovingEdge(I, cMo);
+ }
+ cy->trackMovingEdge(I, cMo) ;
+ }
+ }
+
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ ci = *it;
+ if(ci->isVisible() && ci->isTracked()){
+ if(ci->meEllipse == NULL){
+ ci->initMovingEdge(I, cMo);
+ }
+ ci->trackMovingEdge(I, cMo) ;
+ }
+ }
+}
+
+
+/*!
+ Update the moving edges at the end of the virtual visual servoing.
+
+ \param I : the image.
+*/
+void
+vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
+{
+ vpMbtDistanceLine *l ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ l->updateMovingEdge(I, cMo) ;
+ if (l->nbFeatureTotal == 0 && l->isVisible()){
+ l->Reinit = true;
+ }
+ }
+ }
+
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ cy->updateMovingEdge(I, cMo) ;
+ if((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()){
+ cy->Reinit = true;
+ }
+ }
+ }
+
+ vpMbtDistanceCircle *ci ;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ ci->updateMovingEdge(I, cMo) ;
+ if(ci->nbFeature == 0 && ci->isVisible()){
+ ci->Reinit = true;
+ }
+ }
+ }
+}
+
+
+/*!
+ Reinitialize the lines if it is required.
+
+ A line is reinitialized if the 2D line do not match enough with the projected 3D line.
+
+ \param I : the image.
+ \param _cMo : the pose of the used to re-initialize the moving edges
+*/
+void
+vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
+{
+ vpMbtDistanceLine *l ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ if (l->Reinit && l->isVisible())
+ l->reinitMovingEdge(I, _cMo);
+ }
+ }
+
+ vpMbtDistanceCylinder*cy;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ if (cy->Reinit && cy->isVisible())
+ cy->reinitMovingEdge(I, _cMo);
+ }
+ }
+
+ vpMbtDistanceCircle*ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ if (ci->Reinit && ci->isVisible())
+ ci->reinitMovingEdge(I, _cMo);
+ }
+ }
+}
+
+void
+vpMbEdgeTracker::resetMovingEdge(){
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if (scales[i]) {
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for(unsigned int a = 0 ; a < (*it)->meline.size() ; a++){
+ if((*it)->meline[a] != NULL){
+ delete (*it)->meline[a];
+ (*it)->meline[a] = NULL;
+ }
+ (*it)->meline.clear();
+ (*it)->nbFeature.clear();
+ (*it)->nbFeatureTotal = 0;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ if((*it)->meline1 != NULL){
+ delete (*it)->meline1;
+ (*it)->meline1 = NULL;
+ }
+ if((*it)->meline2 != NULL){
+ delete (*it)->meline2;
+ (*it)->meline2 = NULL;
+ }
+
+ (*it)->nbFeature = 0;
+ (*it)->nbFeaturel1 = 0;
+ (*it)->nbFeaturel2 = 0;
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ if((*it)->meEllipse != NULL){
+ delete (*it)->meEllipse;
+ (*it)->meEllipse = NULL;
+ }
+ (*it)->nbFeature = 0;
+ }
+ }
+ }
+}
+
+/*!
+ Check if two vpPoints are similar.
+
+ To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon \f$.
+
+ \param P1 : The first point to compare
+ \param P2 : The second point to compare
+*/
+bool
+vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2)
+{
+ double dx = fabs(P1.get_oX() - P2.get_oX());
+ double dy = fabs(P1.get_oY() - P2.get_oY());
+ double dz = fabs(P1.get_oZ() - P2.get_oZ());
+
+ if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() && dz <= std::numeric_limits<double>::epsilon())
+ return true ;
+ else
+ return false ;
+}
+
+
+/*!
+ Add a line belonging to the \f$ index \f$ the polygon to the list of lines. It is defined by its two extremities.
+
+ If the line already exists, the ploygone's index is added to the list of polygon to which it belongs.
+
+ \param P1 : The first extremity of the line.
+ \param P2 : The second extremity of the line.
+ \param polygon : The index of the polygon to which the line belongs.
+ \param name : the optional name of the line
+*/
+void
+vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
+{
+ //suppress line already in the model
+ bool already_here = false ;
+ vpMbtDistanceLine *l ;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ downScale(i);
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
+ (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
+ already_here = true ;
+ l->addPolygon(polygon);
+ l->hiddenface = &faces ;
+ }
+ }
+
+ if (!already_here){
+ l = new vpMbtDistanceLine ;
+
+ l->setCameraParameters(cam) ;
+ l->buildFrom(P1,P2) ;
+ l->addPolygon(polygon);
+ l->setMovingEdge(&me) ;
+ l->hiddenface = &faces ;
+ l->useScanLine = useScanLine;
+
+ l->setIndex(nline) ;
+ l->setName(name);
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ l->getPolygon().setClipping(clippingFlag);
+
+ if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ l->getPolygon().setNearClippingDistance(distNearClip);
+
+ if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ l->getPolygon().setFarClippingDistance(distFarClip);
+
+ nline +=1 ;
+ lines[i].push_back(l);
+ }
+ upScale(i);
+ }
+ }
+}
+
+/*!
+ Remove a line using its name.
+
+ \param name : The name of the line to remove.
+*/
+void
+vpMbEdgeTracker::removeLine(const std::string& name)
+{
+ vpMbtDistanceLine *l;
+
+ for(unsigned int i=0; i<scales.size(); i++){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if (name.compare(l->getName()) == 0){
+ lines[i].erase(it);
+ break;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Add a circle to the list of circles.
+
+ \param P1 : Center of the circle.
+ \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param r : Radius of the circle.
+ \param idFace : Id of the face that is associated to the circle to handle visibility test.
+ \param name : the optional name of the circle.
+*/
+void
+vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace, const std::string& name)
+{
+ bool already_here = false ;
+ vpMbtDistanceCircle *ci ;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ downScale(i);
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
+ (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
+ already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
+ }
+ }
+
+ if (!already_here){
+ ci = new vpMbtDistanceCircle ;
+
+ ci->setCameraParameters(cam);
+ ci->buildFrom(P1, P2, P3, r);
+ ci->setMovingEdge(&me);
+ ci->setIndex(ncircle);
+ ci->setName(name);
+ ci->index_polygon = idFace;
+ ci->hiddenface = &faces ;
+
+// if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+// ci->getPolygon().setClipping(clippingFlag);
+
+// if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+// ci->getPolygon().setNearClippingDistance(distNearClip);
+
+// if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+// ci->getPolygon().setFarClippingDistance(distFarClip);
+
+ ncircle +=1;
+ circles[i].push_back(ci);
+ }
+ upScale(i);
+ }
+ }
+}
+
+/*!
+ Add a cylinder to the list of cylinders.
+
+ \param P1 : The first extremity of the axis.
+ \param P2 : The second extremity of the axis.
+ \param r : The radius of the cylinder.
+ \param idFace : The index of the face.
+ \param name : the optional name of the cylinder
+*/
+void
+vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string& name)
+{
+ bool already_here = false ;
+ vpMbtDistanceCylinder *cy ;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ downScale(i);
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
+ (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
+ already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
+ }
+ }
+
+ if (!already_here){
+ cy = new vpMbtDistanceCylinder ;
+
+ cy->setCameraParameters(cam);
+ cy->buildFrom(P1, P2, r);
+ cy->setMovingEdge(&me);
+ cy->setIndex(ncylinder);
+ cy->setName(name);
+ cy->index_polygon = idFace;
+ cy->hiddenface = &faces ;
+ ncylinder +=1;
+ cylinders[i].push_back(cy);
+ }
+ upScale(i);
+ }
+ }
+}
+
+/*!
+ Remove a cylinder by its name.
+
+ \param name : The name of the cylinder to remove.
+*/
+void
+vpMbEdgeTracker::removeCylinder(const std::string& name)
+{
+ vpMbtDistanceCylinder *cy;
+
+ for(unsigned int i=0; i<scales.size(); i++){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if (name.compare(cy->getName()) == 0){
+ cylinders[i].erase(it);
+ break;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Remove a circle by its name.
+
+ \param name : The name of the circle to remove.
+*/
+void
+vpMbEdgeTracker::removeCircle(const std::string& name)
+{
+ vpMbtDistanceCircle *ci;
+
+ for(unsigned int i=0; i<scales.size(); i++){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceCircle*>::iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if (name.compare(ci->getName()) == 0){
+ circles[i].erase(it);
+ break;
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Add a polygon to the list of polygons.
+
+ \param p : The polygon to add.
+*/
+void
+vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
+{
+ unsigned int nbpt = p.getNbPoint() ;
+ if(nbpt > 0){
+ for (unsigned int i=0 ; i < nbpt-1 ; i++)
+ addLine(p.p[i], p.p[i+1], p.getIndex()) ;
+ addLine(p.p[nbpt-1], p.p[0], p.getIndex()) ;
+ }
+}
+
+/*!
+ Detect the visible faces in the image and says if a new one appeared.
+
+ \warning If in one iteration one face appears and one disappears, then the
+ function will not detect the new face.
+
+ \param _I : Image to test if a face is entirely in the image.
+ \param _cMo : The pose of the camera used to project the 3D model into the image.
+ \param newvisibleline : This parameter is set to true if a new face appeared.
+*/
+void
+vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> & _I,
+ const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
+{
+ unsigned int n ;
+ bool changed = false;
+
+ if(!useOgre) {
+ //n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
+ n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
+ }
+ else{
+#ifdef VISP_HAVE_OGRE
+ n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
+#else
+ n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
+#endif
+ }
+
+ if (n > nbvisiblepolygone)
+ {
+ //cout << "une nouvelle face est visible " << endl ;
+ newvisibleline = true ;
+ }
+ else
+ newvisibleline = false ;
+
+ nbvisiblepolygone= n ;
+}
+
+/*!
+ Add the lines to track from the polygon description. If the polygon has only
+ two points, it defines a single line that is always visible. If it has three or
+ more corners, it defines a face. In that case the visibility of the face is computed
+ in order to track the corresponding lines only if the face is visible.
+
+ The id of the polygon is supposed to be set prior calling this function.
+
+ This method is called from the loadModel() one to add a face of the object to track.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+{
+ unsigned int nbpt = polygon.getNbPoint() ;
+ if(nbpt > 0){
+ for (unsigned int i=0 ; i < nbpt-1 ; i++)
+ addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
+ addLine(polygon.p[nbpt-1], polygon.p[0], polygon.getIndex(), polygon.getName());
+ }
+}
+/*!
+ Add the lines to track from the polygon description. If the polygon has only
+ two points, it defines a single line that is always visible. If it has three or
+ more corners, it defines a face. In that case the visibility of the face is computed
+ in order to track the corresponding lines only if the face is visible.
+
+ The id of the polygon is supposed to be set prior calling this function.
+
+ This method is called from the loadModel() one to add a face of the object to track.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
+{
+ unsigned int nbpt = polygon.getNbPoint() ;
+ if(nbpt > 0){
+ for (unsigned int i=0 ; i < nbpt-1 ; i++)
+ addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
+ }
+}
+
+/*!
+ Add a circle to track from its center, 3 points (including the center) defining the plane that contain
+ the circle and its radius.
+
+ \param p1 : Center of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param radius : Radius of the circle.
+ \param idFace : Index of the face associated to the circle to handle visibility test.
+ \param name : The optional name of the circle.
+*/
+void
+vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace, const std::string &name)
+{
+ addCircle(p1, p2, p3, radius, (int)idFace, name);
+}
+
+/*!
+ Add a cylinder to track from two points on the axis (defining the length of
+ the cylinder) and its radius.
+
+ \param p1 : First point on the axis.
+ \param p2 : Second point on the axis.
+ \param radius : Radius of the cylinder.
+ \param idFace : Id of the face that is associated to the cylinder to handle visibility test.
+ \param name : The optional name of the cylinder.
+*/
+void
+vpMbEdgeTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string &name)
+{
+ addCylinder(p1, p2, radius, (int)idFace, name);
+}
+
+/*!
+ Reset the tracker. The model is removed and the pose is set to identity.
+ The tracker needs to be initialized with a new model and a new pose.
+
+*/
+void
+vpMbEdgeTracker::resetTracker()
+{
+ this->cMo.eye();
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if (l!=NULL) delete l ;
+ l = NULL ;
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if (cy!=NULL) delete cy;
+ cy = NULL;
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if (ci!=NULL) delete ci;
+ ci = NULL;
+ }
+ lines[i].clear();
+ cylinders[i].clear();
+ circles[i].clear();
+ }
+ }
+
+ faces.reset();
+
+ useScanLine = false;
+
+#ifdef VISP_HAVE_OGRE
+ useOgre = false;
+#endif
+
+ compute_interaction=1;
+ nline = 0;
+ ncylinder = 0;
+ lambda = 1;
+ nbvisiblepolygone = 0;
+ percentageGdPt = 0.4;
+
+ angleAppears = vpMath::rad(89);
+ angleDisappears = vpMath::rad(89);
+ clippingFlag = vpPolygon3D::NO_CLIPPING;
+
+ m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
+
+ // reinitialization of the scales.
+ this->setScales(scales);
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ reInitModel(I, cad_name.c_str(), cMo_, verbose);
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ this->cMo.eye();
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if (l!=NULL) delete l ;
+ l = NULL ;
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if (cy!=NULL) delete cy;
+ cy = NULL;
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if (ci!=NULL) delete ci;
+ ci = NULL;
+ }
+
+ lines[i].clear();
+ cylinders[i].clear();
+ circles[i].clear();
+ }
+ }
+
+ faces.reset();
+
+ //compute_interaction=1;
+ nline = 0;
+ ncylinder = 0;
+ ncircle = 0;
+ //lambda = 1;
+ nbvisiblepolygone = 0;
+
+ loadModel(cad_name, verbose);
+ initFromPose(I, cMo_);
+}
+
+/*!
+ Return the number of good points (vpMeSite) tracked. A good point is a
+ vpMeSite with its flag "state" equal to 0. Only these points are used
+ during the virtual visual servoing stage.
+
+ \exception vpException::dimensionError if level does not represent a used
+ level.
+
+ \return the number of good points.
+*/
+unsigned int
+vpMbEdgeTracker::getNbPoints(const unsigned int level) const
+{
+ if((level > scales.size()) || !scales[level]){
+ throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used", level);
+ }
+
+ unsigned int nbGoodPoints = 0;
+ vpMbtDistanceLine *l ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
+ l = *it;
+ if (l->isVisible() && l->isTracked())
+ {
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++){
+ if(l->nbFeature[a] != 0)
+ for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
+ if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ }
+ }
+ }
+ }
+
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
+ cy = *it;
+ if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL))
+ {
+ for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
+ if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ }
+ for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
+ if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ }
+ }
+ }
+
+ vpMbtDistanceCircle *ci ;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[level].begin(); it!=circles[level].end(); ++it){
+ ci = *it;
+ if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL)
+ {
+ for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
+ if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ }
+ }
+ }
+
+ return nbGoodPoints;
+}
+
+/*!
+ Set the scales to use to realize the tracking. The vector of boolean activates
+ or not the scales to set for the object tracking. The first element of the list
+ correspond to the tracking on the full image, the second element corresponds
+ to the tracking on an image subsampled by two.
+
+ Using multi scale tracking allows to track the object with greater moves. It
+ requires the computation of a pyramid of images, but the total tracking can be
+ faster than a tracking based only on the full scale. The pose is computed from
+ the smallest image to the biggest. This may be dangerous if the object to
+ track is small in the image, because the subsampled scale(s) will have only
+ few points to compute the pose (it could result in a loss of precision).
+
+ \warning This method must be used before the tracker has been initialized (
+ before the call of the loadConfigFile() or loadModel() methods).
+
+ \warning At least one level must be activated.
+
+ \param scale : The vector describing the levels to use.
+*/
+void
+vpMbEdgeTracker::setScales(const std::vector<bool>& scale)
+{
+ unsigned int nbActivatedLevels = 0;
+ for (unsigned int i = 0; i < scale.size(); i += 1){
+ if(scale[i]){
+ nbActivatedLevels++;
+ }
+ }
+ if((scale.size() < 1) || (nbActivatedLevels == 0)){
+ vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
+ this->scales.resize(0);
+ this->scales.push_back(true);
+ lines.resize(1);
+ lines[0].clear();
+ cylinders.resize(1);
+ cylinders[0].clear();
+ }
+ else{
+ this->scales = scale;
+ lines.resize(scale.size());
+ cylinders.resize(scale.size());
+ for (unsigned int i = 0; i < lines.size(); i += 1){
+ lines[i].clear();
+ cylinders[i].clear();
+ }
+ }
+}
+
+/*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+*/
+void
+vpMbEdgeTracker::setFarClippingDistance(const double &dist)
+{
+ if( (clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
+ vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
+ else if ( dist < 0 )
+ vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
+ else{
+ vpMbTracker::setFarClippingDistance(dist);
+ vpMbtDistanceLine *l;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ l->getPolygon().setFarClippingDistance(distFarClip);
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+*/
+void
+vpMbEdgeTracker::setNearClippingDistance(const double &dist)
+{
+ if( (clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
+ vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
+ else if ( dist < 0 )
+ vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
+ else{
+ vpMbTracker::setNearClippingDistance(dist);
+ vpMbtDistanceLine *l;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ l->getPolygon().setNearClippingDistance(distNearClip);
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param flags : New clipping flags.
+*/
+void
+vpMbEdgeTracker::setClipping(const unsigned int &flags)
+{
+ vpMbTracker::setClipping(flags);
+
+ vpMbtDistanceLine *l;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ l->getPolygon().setClipping(clippingFlag);
+ }
+ }
+ }
+}
+
+/*!
+ Compute the pyramid of image associated to the image in parameter. The scales
+ computed are the ones corresponding to the scales attribute of the class. If
+ OpenCV is detected, the functions used to computed a smoothed pyramid come
+ from OpenCV, otherwise a simple subsampling (no smoothing, no interpolation)
+ is realized.
+
+ \warning The pyramid contains pointers to vpImage. To properly deallocate the
+ pyramid. All the element but the first (which is a pointer to the input image)
+ must be freed. A proper cleaning is implemented in the cleanPyramid() method.
+
+ \param _I : The input image.
+ \param _pyramid : The pyramid of image to build from the input image.
+*/
+void
+vpMbEdgeTracker::initPyramid(const vpImage<unsigned char>& _I, std::vector< const vpImage<unsigned char>* >& _pyramid)
+{
+ _pyramid.resize(scales.size());
+
+ if(scales[0]){
+ _pyramid[0] = &_I;
+ }
+ else{
+ _pyramid[0] = NULL;
+ }
+
+ for(unsigned int i=1; i<_pyramid.size(); i += 1){
+ if(scales[i]){
+ unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
+ vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
+ IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
+ vpI0->imageData = (char*)(_I.bitmap);
+ IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
+ cvResize(vpI0, vpI, CV_INTER_NN);
+ vpImageConvert::convert(vpI, *I);
+ cvReleaseImage(&vpI);
+ vpI0->imageData = NULL;
+ cvReleaseImageHeader(&vpI0);
+#else
+ for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
+ for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
+ (*I)[k][l] = _I[ii][jj];
+ }
+ }
+#endif
+ _pyramid[i] = I;
+ }
+ else{
+ _pyramid[i] = NULL;
+ }
+ }
+}
+
+/*!
+ Clean the pyramid of image allocated with the initPyramid() method. The vector
+ has a size equal to zero at the end of the method.
+
+ \param _pyramid : The pyramid of image to clean.
+*/
+void
+vpMbEdgeTracker::cleanPyramid(std::vector< const vpImage<unsigned char>* >& _pyramid)
+{
+ if(_pyramid.size() > 0){
+ _pyramid[0] = NULL;
+ for (unsigned int i = 1; i < _pyramid.size(); i += 1){
+ if(_pyramid[i] != NULL){
+ delete _pyramid[i];
+ _pyramid[i] = NULL;
+ }
+ }
+ _pyramid.resize(0);
+ }
+}
+
+/*!
+ Get the list of the lines tracked for the specified level. Each line contains
+ the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not correspond
+ to an used level.
+
+ \param level : Level corresponding to the list to return.
+ \param linesList : The list of the lines of the model.
+*/
+void
+vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
+{
+ if(level > scales.size() || !scales[level]){
+ std::ostringstream oss;
+ oss << level;
+ std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
+ throw vpException(vpException::dimensionError, errorMsg);
+ }
+
+ linesList = lines[level];
+}
+
+
+/*!
+ Get the list of the cylinders tracked for the specified level. Each cylinder
+ contains the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not correspond
+ to an used level.
+
+ \param level : Level corresponding to the list to return.
+ \param cylindersList : The list of the cylinders of the model.
+*/
+void
+vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
+{
+ if(level > scales.size() || !scales[level]){
+ std::ostringstream oss;
+ oss << level;
+ std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
+ throw vpException(vpException::dimensionError, errorMsg);
+ }
+
+ cylindersList = cylinders[level];
+}
+
+
+/*!
+ Get the list of the circles tracked for the specified level. Each circle
+ contains the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not correspond
+ to an used level.
+
+ \param level : Level corresponding to the list to return.
+ \param circlesList : The list of the circles of the model.
+*/
+void
+vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level)
+{
+ if(level > scales.size() || !scales[level]){
+ std::ostringstream oss;
+ oss << level;
+ std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
+ throw vpException(vpException::dimensionError, errorMsg);
+ }
+
+ circlesList = circles[level];
+}
+
+/*!
+ Modify the camera parameters to have them corresponding to the current scale.
+ The new parameters are divided by \f$ 2^{\_scale} \f$.
+
+ \param _scale : Scale to use.
+*/
+void
+vpMbEdgeTracker::downScale(const unsigned int _scale)
+{
+ const double ratio = pow(2., (int)_scale);
+ scaleLevel = _scale;
+
+ vpMatrix K = cam.get_K();
+
+ K[0][0] /= ratio;
+ K[1][1] /= ratio;
+ K[0][2] /= ratio;
+ K[1][2] /= ratio;
+
+ cam.initFromCalibrationMatrix(K);
+}
+
+/*!
+ Modify the camera parameters to have them corresponding to the current scale.
+ The new parameters are multiplied by \f$ 2^{\_scale} \f$.
+
+ \param _scale : Scale to use.
+*/
+void
+vpMbEdgeTracker::upScale(const unsigned int _scale)
+{
+ const double ratio = pow(2., (int)_scale);
+ scaleLevel = 0;
+
+ vpMatrix K = cam.get_K();
+
+ K[0][0] *= ratio;
+ K[1][1] *= ratio;
+ K[0][2] *= ratio;
+ K[1][2] *= ratio;
+
+
+ cam.initFromCalibrationMatrix(K);
+}
+
+/*!
+ Re initialize the moving edges associated to a given level. This method is
+ used to re-initialize the level if the tracking failed on this level but
+ succeeded on the other one.
+
+ \param _lvl : The level to re-initialize.
+*/
+void
+vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
+{
+ unsigned int scaleLevel_1 = scaleLevel;
+ scaleLevel = _lvl;
+
+ vpMbtDistanceLine *l;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
+ }
+ }
+
+ vpMbtDistanceCylinder *cy;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
+ }
+ }
+
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ ci->reinitMovingEdge(*Ipyramid[_lvl], cMo);
+ }
+ }
+
+ trackMovingEdge(*Ipyramid[_lvl]);
+ updateMovingEdge(*Ipyramid[_lvl]);
+ scaleLevel = scaleLevel_1;
+}
+
+/*!
+ Set if the polygons that have the given name have to be considered during the tracking phase.
+
+ \param name : name of the polygon(s).
+ \param useEdgeTracking : True if it has to be considered, False otherwise.
+*/
+void
+vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ /*(*it)->setTracked(useEdgeTracking);
+ for(std::list<int>::const_iterator itpoly=(*it)->Lindex_polygon.begin(); itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
+ if(faces[(*itpoly)]->getName() != name){
+ (*it)->setTracked(true);
+ break;
+ }
+ }*/
+
+ (*it)->setTracked(name,useEdgeTracking);
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
+ (*it)->setTracked(useEdgeTracking);
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
+ (*it)->setTracked(useEdgeTracking);
+ }
+ }
+ }
+ }
+}
diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp
new file mode 100644
index 0000000..2fa8564
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp
@@ -0,0 +1,427 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model. Circle
+ * tracking.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \file vpMbtDistanceCircle.cpp
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+
+#include <stdlib.h>
+#include <algorithm>
+
+#include <visp3/mbt/vpMbtDistanceCircle.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vision/vpPose.h>
+
+/*!
+ Basic constructor
+*/
+vpMbtDistanceCircle::vpMbtDistanceCircle()
+ : name(), index(0), cam(), me(NULL), wmean(1),
+ featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
+ circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
+ L(), error(), nbFeature(0), Reinit(false),
+ hiddenface(NULL), index_polygon(-1), isvisible(false)
+{
+}
+
+/*!
+ Basic destructor useful to deallocate the memory.
+*/
+vpMbtDistanceCircle::~vpMbtDistanceCircle()
+{
+ if (meEllipse != NULL) delete meEllipse;
+ if (circle != NULL) delete circle;
+ if (p1 != NULL) delete p1;
+ if (p2 != NULL) delete p2;
+ if (p3 != NULL) delete p3;
+}
+
+/*!
+ Project the circle into the image.
+
+ \param cMo : The pose of the camera used to project the circle into the image.
+*/
+void
+vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo)
+{
+ circle->project(cMo) ;
+}
+
+
+/*!
+ Build a vpMbtDistanceCircle thanks to its center, 3 points (including the center) with
+ coordinates expressed in the object frame and defining the plane that contain
+ the circle and its radius.
+
+ \param _p1 : Center of the circle.
+ \param _p2,_p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param r : Radius of the circle.
+*/
+void
+vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
+{
+ circle = new vpCircle ;
+ p1 = new vpPoint ;
+ p2 = new vpPoint ;
+ p3 = new vpPoint ;
+
+ // Get the points
+ *p1 = _p1;
+ *p2 = _p2;
+ *p3 = _p3;
+
+ // Get the radius
+ radius = r;
+
+ vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
+
+ // Build our circle
+ circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
+}
+
+
+/*!
+ Set the moving edge parameters.
+
+ \param _me : an instance of vpMe containing all the desired parameters
+*/
+void
+vpMbtDistanceCircle::setMovingEdge(vpMe *_me)
+{
+ me = _me ;
+ if (meEllipse != NULL)
+ {
+ meEllipse->setMe(me) ;
+ }
+}
+
+/*!
+ Initialize the moving edge thanks to a given pose of the camera.
+ The 3D model is projected into the image to create moving edges along the circle.
+
+ \param I : The image.
+ \param cMo : The pose of the camera used to initialize the moving edges.
+ \return false if an error occur, true otherwise.
+*/
+bool
+vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(isvisible){
+ // Perspective projection
+ circle->changeFrame(cMo);
+
+ try{
+ circle->projection();
+ }
+ catch(...){
+ std::cout<<"Problem when projecting circle\n";
+ return false;
+ }
+
+ // Create the moving edges containers
+ meEllipse = new vpMbtMeEllipse;
+ meEllipse->setMe(me) ;
+
+ //meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
+ meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
+
+ try
+ {
+ vpImagePoint ic;
+ double mu20_p, mu11_p, mu02_p;
+ vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
+ meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p);
+ }
+ catch(...)
+ {
+ //vpTRACE("the circle can't be initialized");
+ return false;
+ }
+ }
+ return true;
+}
+
+/*!
+ Track the moving edges in the image.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+{
+ if(isvisible){
+ try
+ {
+ meEllipse->track(I) ;
+ }
+ catch(...)
+ {
+ //std::cout << "Track meEllipse failed" << std::endl;
+ meEllipse->reset();
+ Reinit = true;
+ }
+
+ // Update the number of features
+ nbFeature = (unsigned int)meEllipse->getMeList().size();
+ }
+}
+
+
+/*!
+ Update the moving edges internal parameters.
+
+ \warning : Not implemented.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(isvisible){
+ // Perspective projection
+ circle->changeFrame(cMo);
+
+ try{
+ circle->projection();
+ }
+ catch(...){std::cout<<"Problem when projecting circle\n";}
+
+ try
+ {
+
+ vpImagePoint ic;
+ double mu20_p, mu11_p, mu02_p;
+ vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
+ meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
+ }
+ catch(...)
+ {
+ Reinit = true;
+ }
+ nbFeature = (unsigned int)meEllipse->getMeList().size();
+ }
+}
+
+
+/*!
+ Reinitialize the circle if it is required.
+
+ A circle is reinitialized if the ellipse do not match enough with the projected 3D circle.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(meEllipse!= NULL)
+ delete meEllipse;
+
+ meEllipse = NULL;
+
+ if(initMovingEdge(I,cMo) == false)
+ Reinit = true;
+
+ Reinit = false;
+}
+
+
+/*!
+ Display the circle. The 3D circle is projected into the image as an ellipse.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : When true, display the circle even if non visible.
+ If false, display the circle only if visible.
+*/
+void
+vpMbtDistanceCircle::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
+ const bool displayFullModel )
+{
+ if(isvisible || displayFullModel){
+ // Perspective projection
+ circle->changeFrame(cMo);
+
+ try{
+ circle->projection();
+ }
+ catch(...){std::cout<<"Cannot project the circle";}
+
+ vpImagePoint center;
+ double mu20_p, mu11_p, mu02_p;
+ vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
+ vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
+ }
+}
+
+/*!
+ Display the cylinder. The 3D cylinder is projected into the image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : When true, display the circle even if non visible.
+ If false, display the circle only if visible.
+*/
+void
+vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor col,
+ const unsigned int thickness, const bool displayFullModel)
+{
+ if(isvisible || displayFullModel){
+ // Perspective projection
+ circle->changeFrame(cMo);
+
+ try{
+ circle->projection();
+ }
+ catch(...){std::cout<<"Cannot project the circle";}
+
+ vpImagePoint center;
+ double mu20_p, mu11_p, mu02_p;
+ vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
+ vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
+ }
+}
+
+
+/*!
+ Enable to display the points along the ellipse 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 (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
+ - If blue : The point is removed because of the robust method in the virtual visual servoing.
+
+ \param I : The image.
+*/
+void
+vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
+{
+ if (meEllipse != NULL)
+ {
+ meEllipse->display(I); // display the me
+ if (vpDEBUG_ENABLE(3))
+ vpDisplay::flush(I);
+ }
+}
+
+/*!
+ Initialize the size of the interaction matrix and the error vector.
+*/
+void
+vpMbtDistanceCircle::initInteractionMatrixError()
+{
+ if (isvisible == true)
+ {
+ nbFeature = (unsigned int)meEllipse->getMeList().size();
+ L.resize(nbFeature, 6);
+ error.resize(nbFeature);
+ }
+ else
+ nbFeature = 0 ;
+}
+
+/*!
+ Compute the interaction matrix and the error vector corresponding to the point to ellipse algebraic distance.
+*/
+void
+vpMbtDistanceCircle::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
+{
+ if (isvisible)
+ {
+ // Perspective projection
+ circle->changeFrame(cMo) ;
+ try{
+ circle->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle\n";}
+
+ vpFeatureBuilder::create(featureEllipse, *circle);
+
+ vpMatrix H1 = featureEllipse.interaction();
+
+ vpRowVector H(5);
+ double x=0, y=0;
+
+ // Get the parameters of the ellipse in the image plane
+ double xg = circle->p[0];
+ double yg = circle->p[1];
+ double mu20 = circle->p[2];
+ double mu11 = circle->p[3];
+ double mu02 = circle->p[4];
+
+ unsigned int j = 0;
+
+ for(std::list<vpMeSite>::const_iterator it=meEllipse->getMeList().begin(); it!=meEllipse->getMeList().end(); ++it){
+ vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
+ H[0] = 2*(mu11*(y-yg)+mu02*(xg-x));
+ H[1] = 2*(mu20*(yg-y)+mu11*(x-xg));
+ H[2] = vpMath::sqr(y-yg)-mu02;
+ H[3] = 2*(yg*(x-xg)+y*xg+mu11-x*y);
+ H[4] = vpMath::sqr(x-xg)-mu20;
+
+ for (unsigned int k=0; k<6; k++)
+ L[j][k] = H[0]*H1[0][k] + H[1]*H1[1][k] + H[2]*H1[2][k] + H[3]*H1[3][k] + H[4]*H1[4][k];
+
+ error[j] = mu02*vpMath::sqr(x) + mu20*vpMath::sqr(y) - 2*mu11*x*y
+ + 2*(mu11*yg-mu02*xg)*x + 2*(mu11*xg-mu20*yg)*y
+ + mu02*vpMath::sqr(xg) + mu20*vpMath::sqr(yg) - 2*mu11*xg*yg
+ + vpMath::sqr(mu11) - mu20*mu02;
+
+ j++;
+ }
+ }
+}
+
diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp
new file mode 100644
index 0000000..477aa49
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp
@@ -0,0 +1,719 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model. Cylinder
+ * tracking.
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Bertrand Delabarre
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \file vpMbtDistanceCylinder.cpp
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+
+#include <visp3/mbt/vpMbtDistanceCylinder.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <stdlib.h>
+#include <algorithm>
+
+#include <visp3/vision/vpPose.h>
+
+/*!
+ Basic constructor
+*/
+vpMbtDistanceCylinder::vpMbtDistanceCylinder()
+ : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1),
+ featureline1(), featureline2(), isTrackedCylinder(true), meline1(NULL), meline2(NULL),
+ cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
+ error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
+ c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
+{
+}
+
+/*!
+ Basic destructor useful to deallocate the memory.
+*/
+vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
+{
+// cout << "Deleting cylinder " << index << endl ;
+
+ if (p1 != NULL) delete p1 ;
+ if (p2 != NULL) delete p2 ;
+ if (c != NULL) delete c ;
+ if (meline1 != NULL) delete meline1 ;
+ if (meline2 != NULL) delete meline2 ;
+ if (cercle1 != NULL) delete cercle1 ;
+ if (cercle2 != NULL) delete cercle2 ;
+}
+
+/*!
+ Project the cylinder and the two points corresponding to its extremities into the image.
+
+ \param cMo : The pose of the camera used to project the cylinder into the image.
+*/
+void
+vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
+{
+ c->project(cMo) ;
+ p1->project(cMo) ;
+ p2->project(cMo) ;
+ cercle1->project(cMo) ;
+ cercle2->project(cMo) ;
+}
+
+
+/*!
+ Build a vpMbtDistanceCylinder thanks to two points corresponding to the extremities of its axis and its radius.
+
+ \param _p1 : The first extremity on the axis.
+ \param _p2 : The second extremity on the axis.
+ \param r : Radius of the cylinder.
+*/
+void
+vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
+{
+ c = new vpCylinder ;
+ p1 = new vpPoint ;
+ p2 = new vpPoint ;
+ cercle1 = new vpCircle;
+ cercle2 = new vpCircle;
+
+ // Get the points
+ *p1 = _p1;
+ *p2 = _p2;
+
+ // Get the radius
+ radius = r;
+
+ vpColVector ABC(3);
+ vpColVector V1(3);
+ vpColVector V2(3);
+
+ V1[0] = _p1.get_oX();
+ V1[1] = _p1.get_oY();
+ V1[2] = _p1.get_oZ();
+ V2[0] = _p2.get_oX();
+ V2[1] = _p2.get_oY();
+ V2[2] = _p2.get_oZ();
+
+ // Get the axis of the cylinder
+ ABC = V1-V2;
+
+ // Build our extremity circles
+ cercle1->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p1.get_oX(),_p1.get_oY(),_p1.get_oZ(),r);
+ cercle2->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p2.get_oX(),_p2.get_oY(),_p2.get_oZ(),r);
+
+ // Build our cylinder
+ c->setWorldCoordinates(ABC[0],ABC[1],ABC[2],(_p1.get_oX()+_p2.get_oX())/2.0,(_p1.get_oY()+_p2.get_oY())/2.0,(_p1.get_oZ()+_p2.get_oZ())/2.0,r);
+}
+
+
+/*!
+ Set the moving edge parameters.
+
+ \param _me : an instance of vpMe containing all the desired parameters
+*/
+void
+vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
+{
+ me = _me ;
+ if (meline1 != NULL)
+ {
+ meline1->setMe(me) ;
+ }
+ if (meline2 != NULL)
+ {
+ meline2->setMe(me) ;
+ }
+}
+
+/*!
+ Initialize the moving edge thanks to a given pose of the camera.
+ The 3D model is projected into the image to create moving edges along the lines.
+
+ \param I : The image.
+ \param cMo : The pose of the camera used to initialize the moving edges.
+ \return false if an error occur, true otherwise.
+*/
+bool
+vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(isvisible){
+ // Perspective projection
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+ cercle1->changeFrame(cMo);
+ cercle2->changeFrame(cMo);
+ c->changeFrame(cMo);
+
+ p1->projection();
+ p2->projection();
+ try{
+ cercle1->projection();
+ }
+ catch(...){
+ //std::cout<<"Problem when projecting circle 1\n";
+ return false;
+ }
+ try{
+ cercle2->projection();
+ }
+ catch(...){
+ //std::cout<<"Problem when projecting circle 2\n";
+ return false;
+ }
+ c->projection();
+
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Create the moving edges containers
+ meline1 = new vpMbtMeLine ;
+ meline1->setMe(me) ;
+ meline2 = new vpMbtMeLine ;
+ meline2->setMe(me) ;
+
+ // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
+ meline1->setInitRange(0);
+ meline2->setInitRange(0);
+
+ // Conversion meter to pixels
+ vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // update limits of the melines.
+ int marge = /*10*/5; //ou 5 normalement
+ if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
+ if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
+
+ if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
+ if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
+
+ // Initialize the tracking
+ while (theta1 > M_PI) { theta1 -= M_PI ; }
+ while (theta1 < -M_PI) { theta1 += M_PI ; }
+
+ if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
+ else theta1 = M_PI/2.0 - theta1;
+
+ while (theta2 > M_PI) { theta2 -= M_PI ; }
+ while (theta2 < -M_PI) { theta2 += M_PI ; }
+
+ if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
+ else theta2 = M_PI/2.0 - theta2;
+
+ try
+ {
+ meline1->initTracking(I,ip11,ip12,rho1,theta1);
+ }
+ catch(...)
+ {
+ //vpTRACE("the line can't be initialized");
+ return false;
+ }
+ try
+ {
+ meline2->initTracking(I,ip21,ip22,rho2,theta2);
+ }
+ catch(...)
+ {
+ //vpTRACE("the line can't be initialized");
+ return false;
+ }
+ }
+ return true;
+}
+
+
+
+/*!
+ Track the moving edges in the image.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+{
+ if(isvisible){
+ try
+ {
+ meline1->track(I) ;
+ }
+ catch(...)
+ {
+ //std::cout << "Track meline1 failed" << std::endl;
+ meline1->reset();
+ Reinit = true;
+ }
+ try
+ {
+ meline2->track(I) ;
+ }
+ catch(...)
+ {
+ //std::cout << "Track meline2 failed" << std::endl;
+ meline2->reset();
+ Reinit = true;
+ }
+
+ // Update the number of features
+ nbFeaturel1 = (unsigned int)meline1->getMeList().size();
+ nbFeaturel2 = (unsigned int)meline2->getMeList().size();
+ nbFeature = nbFeaturel1 + nbFeaturel2;
+ }
+}
+
+
+/*!
+ Update the moving edges internal parameters.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(isvisible){
+ // Perspective projection
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+ cercle1->changeFrame(cMo);
+ cercle2->changeFrame(cMo);
+ c->changeFrame(cMo);
+
+ p1->projection();
+ p2->projection();
+ try{
+ cercle1->projection();
+ }
+ catch(...){std::cout<<"Probleme projection cercle 1\n";}
+ try{
+ cercle2->projection();
+ }
+ catch(...){std::cout<<"Probleme projection cercle 2\n";}
+ c->projection();
+
+ // Get the limbos
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Conversion meter to pixels
+ vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // update limits of the meline.
+ int marge = /*10*/5; //ou 5 normalement
+ if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
+ if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
+
+ if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
+ if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
+
+ // Initialize the tracking
+ while (theta1 > M_PI) { theta1 -= M_PI ; }
+ while (theta1 < -M_PI) { theta1 += M_PI ; }
+
+ if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
+ else theta1 = M_PI/2.0 - theta1;
+
+ while (theta2 > M_PI) { theta2 -= M_PI ; }
+ while (theta2 < -M_PI) { theta2 += M_PI ; }
+
+ if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
+ else theta2 = M_PI/2.0 - theta2;
+
+ try
+ {
+ //meline1->updateParameters(I,rho1,theta1) ;
+ meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
+ }
+ catch(...)
+ {
+ Reinit = true;
+ }
+ try
+ {
+ //meline2->updateParameters(I,rho2,theta2) ;
+ meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
+ }
+ catch(...)
+ {
+ Reinit = true;
+ }
+
+ // Update the numbers of features
+ nbFeaturel1 = (unsigned int)meline1->getMeList().size();
+ nbFeaturel2 = (unsigned int)meline2->getMeList().size();
+ nbFeature = nbFeaturel1 + nbFeaturel2;
+ }
+}
+
+
+/*!
+ Reinitialize the cylinder if it is required.
+
+ A line is reinitialized if the 2D lines do not match enough with the projected 3D lines.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(meline1!= NULL)
+ delete meline1;
+ if(meline2!= NULL)
+ delete meline2;
+
+ meline1 = NULL;
+ meline2 = NULL;
+
+ if (initMovingEdge(I,cMo) == false)
+ Reinit = true;
+
+ Reinit = false;
+}
+
+
+/*!
+ Display the cylinder. The 3D cylinder is projected into the image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : When true, display the circle even if non visible.
+*/
+void
+vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters&camera, const vpColor col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ if(isvisible || displayFullModel){
+ // Perspective projection
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+ cercle1->changeFrame(cMo);
+ cercle2->changeFrame(cMo);
+ c->changeFrame(cMo);
+
+ p1->projection();
+ p2->projection();
+ try{
+ cercle1->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 1";}
+ try{
+ cercle2->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 2";}
+ c->projection();
+
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Meters to pixels conversion
+ vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // Display
+ vpDisplay::displayLine(I,ip11,ip12,col, thickness);
+ vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ }
+}
+
+/*!
+ Display the cylinder. The 3D cylinder is projected into the image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : When true, display the circle even if non visible.
+*/
+void
+vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ if(isvisible || displayFullModel){
+ // Perspective projection
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+ cercle1->changeFrame(cMo);
+ cercle2->changeFrame(cMo);
+ c->changeFrame(cMo);
+
+ p1->projection();
+ p2->projection();
+ try{
+ cercle1->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 1";}
+ try{
+ cercle2->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 2";}
+ c->projection();
+
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Meters to pixels conversion
+ vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
+
+ vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // Display
+ vpDisplay::displayLine(I,ip11,ip12,col, thickness);
+ vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ }
+}
+
+
+/*!
+ Enable to display the points along the lines 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 (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
+ - If blue : The point is removed because of the robust method in the virtual visual servoing.
+
+ \param I : The image.
+*/
+void
+vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
+{
+ if (meline1 != NULL)
+ {
+ meline1->display(I);
+ }
+ if (meline2 != NULL)
+ {
+ meline2->display(I);
+ }
+}
+
+/*!
+ Initialize the size of the interaction matrix and the error vector.
+*/
+void
+vpMbtDistanceCylinder::initInteractionMatrixError()
+{
+ if (isvisible == true) {
+ nbFeaturel1 = (unsigned int)meline1->getMeList().size();
+ nbFeaturel2 = (unsigned int)meline2->getMeList().size();
+ nbFeature = nbFeaturel1 + nbFeaturel2;
+ L.resize(nbFeature, 6);
+ error.resize(nbFeature);
+ }
+ else {
+ nbFeature = 0 ;
+ nbFeaturel1 = 0;
+ nbFeaturel2 = 0;
+ }
+}
+
+/*!
+ Compute the interaction matrix and the error vector corresponding to the cylinder.
+*/
+void
+vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
+{
+ if (isvisible) {
+ // Perspective projection
+ c->changeFrame(cMo) ;
+ c->projection() ;
+ cercle1->changeFrame(cMo) ;
+ cercle1->changeFrame(cMo) ;
+ try{
+ cercle1->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 1\n";}
+ try{
+ cercle2->projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 2\n";}
+
+ bool disp = false;
+ bool disp2 = false;
+ if (disp || disp2) vpDisplay::flush(I);
+
+ // Build the lines
+ vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
+ vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
+
+ double rho1 = featureline1.getRho() ;
+ double theta1 = featureline1.getTheta() ;
+ double rho2 = featureline2.getRho() ;
+ double theta2 = featureline2.getTheta() ;
+
+ double co1 = cos(theta1);
+ double si1 = sin(theta1);
+ double co2 = cos(theta2);
+ double si2 = sin(theta2);
+
+ double mx = 1.0/cam.get_px() ;
+ double my = 1.0/cam.get_py() ;
+ double xc = cam.get_u0() ;
+ double yc = cam.get_v0() ;
+
+ double alpha1 ;
+ vpMatrix H1 ;
+ H1 = featureline1.interaction() ;
+ double alpha2 ;
+ vpMatrix H2 ;
+ H2 = featureline2.interaction() ;
+
+ double x,y ;
+ vpMeSite p ;
+ unsigned int j =0 ;
+ for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
+ x = (double)it->j;
+ y = (double)it->i;
+
+ x = (x-xc)*mx ;
+ y = (y-yc)*my ;
+
+ alpha1 = x*si1 - y*co1;
+
+ double *Lrho = H1[0] ;
+ double *Ltheta = H1[1] ;
+ // Calculate interaction matrix for a distance
+ for (unsigned int k=0 ; k < 6 ; k++){
+ L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
+ }
+ error[j] = rho1 - ( x*co1 + y*si1) ;
+
+ if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
+
+ j++;
+ }
+
+ for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
+ x = (double)it->j;
+ y = (double)it->i;
+
+ x = (x-xc)*mx ;
+ y = (y-yc)*my ;
+
+ alpha2 = x*si2 - y*co2;
+
+ double *Lrho = H2[0] ;
+ double *Ltheta = H2[1] ;
+ // Calculate interaction matrix for a distance
+ for (unsigned int k=0 ; k < 6 ; k++){
+ L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
+ }
+ error[j] = rho2 - ( x*co2 + y*si2) ;
+
+ if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
+
+ j++;
+ }
+ }
+}
+
diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp
new file mode 100644
index 0000000..c2f5c7c
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp
@@ -0,0 +1,849 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \file vpMbtDistanceLine.cpp
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+
+#include <visp3/mbt/vpMbtDistanceLine.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <stdlib.h>
+
+void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
+void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
+
+/*!
+ Basic constructor
+*/
+vpMbtDistanceLine::vpMbtDistanceLine()
+ : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true),
+ wmean(1), featureline(), poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(),
+ error(), nbFeature(), nbFeatureTotal(0), Reinit(false), hiddenface(NULL), Lindex_polygon(),
+ Lindex_polygon_tracked(), isvisible(false)
+{
+}
+
+/*!
+ Basic destructor useful to deallocate the memory.
+*/
+vpMbtDistanceLine::~vpMbtDistanceLine()
+{
+// cout << "Deleting line " << index << endl ;
+ if (line != NULL) delete line ;
+
+ for(unsigned int i = 0 ; i < meline.size() ; i++)
+ if (meline[i] != NULL) delete meline[i] ;
+
+ meline.clear();
+}
+
+/*!
+ Project the line and the two points corresponding to its extremities into the image.
+
+ \param cMo : The pose of the camera used to project the line into the image.
+*/
+void
+vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
+{
+ line->project(cMo) ;
+ p1->project(cMo) ;
+ p2->project(cMo) ;
+}
+
+/*!
+ Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
+
+ \param P : The first point to define the plane
+ \param Q : The second point to define the plane
+ \param R : The third point to define the plane
+ \param plane : The vpPlane instance used to store the computed plane equation.
+*/
+void
+buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
+{
+ vpColVector a(3);
+ vpColVector b(3);
+ vpColVector n(3);
+ //Calculate vector corresponding to PQ
+ a[0]=P.get_oX()-Q.get_oX();
+ a[1]=P.get_oY()-Q.get_oY();
+ a[2]=P.get_oZ()-Q.get_oZ();
+
+ //Calculate vector corresponding to PR
+ b[0]=P.get_oX()-R.get_oX();
+ b[1]=P.get_oY()-R.get_oY();
+ b[2]=P.get_oZ()-R.get_oZ();
+
+ //Calculate normal vector to plane PQ x PR
+ n=vpColVector::cross(a,b);
+
+ //Equation of the plane is given by:
+ double A = n[0];
+ double B = n[1];
+ double C = n[2];
+ double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
+
+ double norm = sqrt(A*A+B*B+C*C) ;
+ plane.setA(A/norm) ;
+ plane.setB(B/norm) ;
+ plane.setC(C/norm) ;
+ plane.setD(D/norm) ;
+}
+
+
+/*!
+ Build a line thanks to 4 points.
+
+ The method is the following : Two plane are computed thanks to (P1,P2,P3) and (P1,P2,P4) (see the buildPlane method). Then the line equation is computed thanks to the intersection between the two planes.
+
+ \param P1 : The first point to compute the line.
+ \param P2 : The second point to compute the line.
+ \param P3 : The third point to compute the line.
+ \param P4 : The fourth point to compute the line.
+ \param L : The instance of vpLine to store the computed line equation.
+*/
+void
+buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
+{
+ vpPlane plane1;
+ vpPlane plane2 ;
+ buildPlane(P1,P2,P3,plane1) ;
+ buildPlane(P1,P2,P4,plane2) ;
+
+ L.setWorldCoordinates(plane1.getA(),plane1.getB(), plane1.getC(),plane1.getD(),
+ plane2.getA(),plane2.getB(), plane2.getC(),plane2.getD()) ;
+}
+
+
+/*!
+ Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
+
+ \param _p1 : The first extremity.
+ \param _p2 : The second extremity.
+*/
+void
+vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
+{
+ line = new vpLine ;
+ poly.setNbPoint(2);
+ poly.addPoint(0, _p1);
+ poly.addPoint(1, _p2);
+
+ p1 = &poly.p[0];
+ p2 = &poly.p[1];
+
+ vpColVector V1(3);
+ vpColVector V2(3);
+ vpColVector V3(3);
+ vpColVector V4(3);
+
+ V1[0] = p1->get_oX();
+ V1[1] = p1->get_oY();
+ V1[2] = p1->get_oZ();
+ V2[0] = p2->get_oX();
+ V2[1] = p2->get_oY();
+ V2[2] = p2->get_oZ();
+
+ //if((V1-V2).sumSquare()!=0)
+ if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
+ {
+ {
+ V3[0]=double(rand()%1000)/100;
+ V3[1]=double(rand()%1000)/100;
+ V3[2]=double(rand()%1000)/100;
+
+
+ vpColVector v_tmp1,v_tmp2;
+ v_tmp1 = V2-V1;
+ v_tmp2 = V3-V1;
+ V4=vpColVector::cross(v_tmp1,v_tmp2);
+ }
+
+ vpPoint P3(V3[0],V3[1],V3[2]);
+ vpPoint P4(V4[0],V4[1],V4[2]);
+ buildLine(*p1,*p2, P3,P4, *line) ;
+ }
+ else
+ {
+ vpPoint P3(V1[0],V1[1],V1[2]);
+ vpPoint P4(V2[0],V2[1],V2[2]);
+ buildLine(*p1,*p2,P3,P4,*line) ;
+ }
+}
+
+/*!
+ Add a polygon to the list of polygons the line belongs to.
+
+ \param idx : Index of the polygon
+*/
+void
+vpMbtDistanceLine::addPolygon(const int &idx)
+{
+ Lindex_polygon.push_back(idx);
+ Lindex_polygon_tracked.push_back(true);
+}
+
+/*!
+ Set if the line has to considered during tracking phase.
+ Line won't be considered if all of its polygons are desactivated.
+
+ \param polyname : name of the polygons that have to be modified.
+ \param track : True if the polygon has to be tracked, False otherwise.
+*/
+void
+vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
+{
+ unsigned int ind = 0;
+ for(std::list<int>::const_iterator itpoly=Lindex_polygon.begin(); itpoly!=Lindex_polygon.end(); ++itpoly){
+ if((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname){
+ Lindex_polygon_tracked[ind] = track;
+ }
+ ind++;
+ }
+
+ isTrackedLine = false;
+ for(unsigned int i = 0 ; i < Lindex_polygon_tracked.size() ; i++)
+ if(Lindex_polygon_tracked[i])
+ {
+ isTrackedLine = true;
+ break;
+ }
+
+ if(!isTrackedLine){
+ isTrackedLineWithVisibility = false;
+ return;
+ }
+
+ updateTracked();
+}
+
+/*!
+ Update the boolean specifying if the line has to be tracked.
+ It takes into account the desactivated polygons and the visibility of the others.
+*/
+void
+vpMbtDistanceLine::updateTracked()
+{
+ if(!isTrackedLine){
+ isTrackedLineWithVisibility = false;
+ return;
+ }
+
+ unsigned int ind = 0;
+ isTrackedLineWithVisibility = false;
+ for(std::list<int>::const_iterator itpoly=Lindex_polygon.begin(); itpoly!=Lindex_polygon.end(); ++itpoly){
+ if((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]){
+ isTrackedLineWithVisibility = true;
+ break;
+ }
+ ind++;
+ }
+}
+
+/*!
+ Set the moving edge parameters.
+
+ \param _me : an instance of vpMe containing all the desired parameters
+*/
+void
+vpMbtDistanceLine::setMovingEdge(vpMe *_me)
+{
+ me = _me ;
+
+ for(unsigned int i = 0 ; i < meline.size() ; i++)
+ if (meline[i] != NULL)
+ {
+// nbFeature[i] = 0;
+ meline[i]->reset();
+ meline[i]->setMe(me) ;
+ }
+
+// nbFeatureTotal = 0;
+}
+
+
+/*!
+ Initialize the moving edge thanks to a given pose of the camera.
+ The 3D model is projected into the image to create moving edges along the line.
+
+ \param I : The image.
+ \param cMo : The pose of the camera used to initialize the moving edges.
+ \return false if an error occur, true otherwise.
+*/
+bool
+vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ if (meline[i] != NULL) delete meline[i] ;
+ }
+
+ meline.clear();
+ nbFeature.clear();
+ nbFeatureTotal = 0;
+
+ if(isvisible)
+ {
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+
+ if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ poly.computePolygonClipped(cam);
+
+ if(poly.polyClipped.size() == 2){ //Les points sont visibles.
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+
+ if(useScanLine){
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst);
+ }
+ else{
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ }
+
+ if(linesLst.size() == 0){
+// isvisible = false;
+ return false;
+ }
+
+ // To have the exact same pose values as the old version (angle or ogre visibility test only), points should be reorganised when using scanline algorithm.
+// if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].first.get_X())) > sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].second.get_X())))
+// {
+// std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
+// for(int i = linesLst.size()-1 ; i >= 0 ; i--)
+// linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
+// linesLst = linesLstTmp;
+// }
+
+ line->changeFrame(cMo);
+ line->projection();
+ double rho,theta;
+ //rho theta uv
+ vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
+
+ while (theta > M_PI) { theta -= M_PI ; }
+ while (theta < -M_PI) { theta += M_PI ; }
+
+ if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
+ else theta = M_PI/2.0 - theta;
+
+
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ vpImagePoint ip1, ip2;
+
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+
+ vpMeterPixelConversion::convertPoint(cam,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(cam,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ vpMbtMeLine *melinePt = new vpMbtMeLine ;
+ melinePt->setMe(me) ;
+
+ // meline[i]->setDisplay(vpMeSite::RANGE_RESULT) ;
+ melinePt->setInitRange(0);
+
+ int marge = /*10*/5; //ou 5 normalement
+ if (ip1.get_j()<ip2.get_j()) { melinePt->jmin = (int)ip1.get_j()-marge ; melinePt->jmax = (int)ip2.get_j()+marge ; } else{ melinePt->jmin = (int)ip2.get_j()-marge ; melinePt->jmax = (int)ip1.get_j()+marge ; }
+ if (ip1.get_i()<ip2.get_i()) { melinePt->imin = (int)ip1.get_i()-marge ; melinePt->imax = (int)ip2.get_i()+marge ; } else{ melinePt->imin = (int)ip2.get_i()-marge ; melinePt->imax = (int)ip1.get_i()+marge ; }
+
+ try
+ {
+ melinePt->initTracking(I,ip1,ip2,rho,theta);
+ meline.push_back(melinePt);
+ // nbFeature.push_back((unsigned int) melinePt->getMeList().size());
+ // nbFeatureTotal += nbFeature.back();
+ }
+ catch(...)
+ {
+ //vpTRACE("the line can't be initialized");
+ // if (melinePt!=NULL) delete melinePt;
+ // melinePt=NULL;
+ // isvisible = false;
+ return false;
+ }
+ }
+ }
+ else{
+ isvisible = false;
+// return false;
+ }
+ }
+
+// trackMovingEdge(I,cMo) ;
+ return true;
+}
+
+
+
+/*!
+ Track the moving edges in the image.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+{
+
+ if (isvisible)
+ {
+// p1->changeFrame(cMo) ;
+// p2->changeFrame(cMo) ;
+//
+// p1->projection() ;
+// p2->projection() ;
+//
+// vpImagePoint ip1, ip2;
+//
+// vpMeterPixelConversion::convertPoint(*cam,p1->get_x(),p1->get_y(),ip1) ;
+// vpMeterPixelConversion::convertPoint(*cam,p2->get_x(),p2->get_y(),ip2) ;
+//
+// int marge = /*10*/5; //ou 5 normalement
+// if (ip1.get_j()<ip2.get_j()) { meline->jmin = ip1.get_j()-marge ; meline->jmax = ip2.get_j()+marge ; }
+// else{ meline->jmin = ip2.get_j()-marge ; meline->jmax = ip1.get_j()+marge ; }
+// if (ip1.get_i()<ip2.get_i()) { meline->imin = ip1.get_i()-marge ; meline->imax = ip2.get_i()+marge ; }
+// else{ meline->imin = ip2.get_i()-marge ; meline->imax = ip1.get_i()+marge ; }
+
+ try
+ {
+ nbFeatureTotal = 0;
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ meline[i]->track(I) ;
+ nbFeature.push_back((unsigned int) meline[i]->getMeList().size());
+ nbFeatureTotal += (unsigned int) meline[i]->getMeList().size();
+ }
+ }
+ catch(...)
+ {
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ if (meline[i] != NULL) delete meline[i] ;
+ }
+
+ nbFeature.clear();
+ meline.clear();
+ nbFeatureTotal = 0;
+ Reinit = true;
+ isvisible = false;
+ }
+ }
+}
+
+
+/*!
+ Update the moving edges internal parameters.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ if(isvisible){
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+
+ if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ poly.computePolygonClipped(cam);
+
+ if(poly.polyClipped.size() == 2){ //Les points sont visibles.
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+
+ if(useScanLine){
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst);
+ }
+ else{
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ }
+
+ if(linesLst.size() != meline.size() || linesLst.size() == 0){
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ if (meline[i] != NULL) delete meline[i] ;
+ }
+
+ meline.clear();
+ nbFeature.clear();
+ nbFeatureTotal = 0;
+ isvisible = false;
+ Reinit = true;
+ }
+ else{
+
+ // To have the exact same pose values as the old version (angle or ogre visibility test only), points should be reorganised when using scanline algorithm.
+// if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].first.get_X())) > sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].second.get_X())))
+// {
+// std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
+// for(int i = linesLst.size()-1 ; i >= 0 ; i--)
+// linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
+// linesLst = linesLstTmp;
+// }
+
+ line->changeFrame(cMo);
+ line->projection();
+ double rho,theta;
+ //rho theta uv
+ vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
+
+ while (theta > M_PI) { theta -= M_PI ; }
+ while (theta < -M_PI) { theta += M_PI ; }
+
+ if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
+ else theta = M_PI/2.0 - theta;
+
+
+ try
+ {
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ vpImagePoint ip1, ip2;
+
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+
+ vpMeterPixelConversion::convertPoint(cam,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(cam,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ int marge = /*10*/5; //ou 5 normalement
+ if (ip1.get_j()<ip2.get_j()) { meline[i]->jmin = (int)ip1.get_j()-marge ; meline[i]->jmax = (int)ip2.get_j()+marge ; } else{ meline[i]->jmin = (int)ip2.get_j()-marge ; meline[i]->jmax = (int)ip1.get_j()+marge ; }
+ if (ip1.get_i()<ip2.get_i()) { meline[i]->imin = (int)ip1.get_i()-marge ; meline[i]->imax = (int)ip2.get_i()+marge ; } else{ meline[i]->imin = (int)ip2.get_i()-marge ; meline[i]->imax = (int)ip1.get_i()+marge ; }
+
+ meline[i]->updateParameters(I,ip1,ip2,rho,theta) ;
+ nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
+ nbFeatureTotal += nbFeature[i];
+ }
+ }
+ catch(...)
+ {
+ for(unsigned int j = 0 ; j < meline.size() ; j++){
+ if (meline[j] != NULL) delete meline[j] ;
+ }
+
+ meline.clear();
+ nbFeature.clear();
+ nbFeatureTotal = 0;
+ isvisible = false;
+ Reinit = true;
+ }
+ }
+ }
+ else{
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ if (meline[i] != NULL) delete meline[i] ;
+ }
+ nbFeature.clear();
+ meline.clear();
+ nbFeatureTotal = 0;
+ isvisible = false;
+ }
+ }
+}
+
+
+/*!
+ Reinitialize the line if it is required.
+
+ A line is reinitialized if the 2D line do not match enough with the projected 3D line.
+
+ \param I : the image.
+ \param cMo : The pose of the camera.
+*/
+void
+vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+{
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ if (meline[i] != NULL) delete meline[i] ;
+ }
+
+ nbFeature.clear();
+ meline.clear();
+ nbFeatureTotal = 0;
+
+ if (initMovingEdge(I,cMo) == false)
+ Reinit = true;
+
+ Reinit = false;
+}
+
+
+/*!
+ Display the line. The 3D line is projected into the image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the line.
+ \param displayFullModel : If true, the line is displayed even if it is not visible.
+*/
+void
+vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor col, const unsigned int thickness, const bool displayFullModel)
+{
+ if(isvisible || displayFullModel){
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+
+ vpImagePoint ip1, ip2;
+ vpCameraParameters c = camera;
+ if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ poly.computePolygonClipped(c);
+
+ if( poly.polyClipped.size() == 2 &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+ if(useScanLine && !displayFullModel){
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst,true);
+ }
+ else{
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ }
+
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ vpDisplay::displayLine(I,ip1,ip2,col, thickness);
+ }
+ }
+ }
+}
+
+
+/*!
+ Display the line. The 3D line is projected into the image.
+
+ \param I : The image.
+ \param cMo : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the line.
+ \param displayFullModel : If true, the line is displayed even if it is not visible.
+*/
+void
+vpMbtDistanceLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor col,
+ const unsigned int thickness, const bool displayFullModel)
+{
+ if(isvisible || displayFullModel){
+ p1->changeFrame(cMo);
+ p2->changeFrame(cMo);
+
+ vpImagePoint ip1, ip2;
+ vpCameraParameters c = camera;
+ if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ poly.computePolygonClipped(c);
+
+ if( poly.polyClipped.size() == 2 &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+ if(useScanLine && !displayFullModel){
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst,true);
+ }
+ else{
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ }
+
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ vpDisplay::displayLine(I,ip1,ip2,col, thickness);
+ }
+ }
+ }
+}
+
+
+/*!
+ Enable to display the points along the line 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 (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
+ - If blue : The point is removed because of the robust method in the virtual visual servoing.
+
+ \param I : The image.
+*/
+void
+vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
+{
+ for(unsigned int i = 0 ; i < meline.size() ; i++)
+ if (meline[i] != NULL)
+ {
+ meline[i]->display(I);
+ }
+}
+
+/*!
+ Initialize the size of the interaction matrix and the error vector.
+*/
+void
+vpMbtDistanceLine::initInteractionMatrixError()
+{
+ if (isvisible == true)
+ {
+ L.resize(nbFeatureTotal,6) ;
+ error.resize(nbFeatureTotal) ;
+ }
+ else{
+ for(unsigned int i = 0 ; i < meline.size() ; i++)
+ nbFeature[i] = 0;
+ nbFeatureTotal = 0 ;
+ }
+}
+
+/*!
+ Compute the interaction matrix and the error vector corresponding to the line.
+*/
+void
+vpMbtDistanceLine::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
+{
+ if (isvisible)
+ {
+ // feature projection
+ line->changeFrame(cMo) ;
+ line->projection() ;
+
+ vpFeatureBuilder::create(featureline,*line) ;
+
+ double rho = featureline.getRho() ;
+ double theta = featureline.getTheta() ;
+
+ double co = cos(theta);
+ double si = sin(theta);
+
+ double mx = 1.0/cam.get_px() ;
+ double my = 1.0/cam.get_py() ;
+ double xc = cam.get_u0() ;
+ double yc = cam.get_v0() ;
+
+ double alpha_ ;
+ vpMatrix H ;
+ H = featureline.interaction() ;
+
+ double x,y ;
+ vpMeSite p ;
+ unsigned int j =0 ;
+
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ for(std::list<vpMeSite>::const_iterator it=meline[i]->getMeList().begin(); it!=meline[i]->getMeList().end(); ++it){
+ x = (double)it->j ;
+ y = (double)it->i ;
+
+ x = (x-xc)*mx ;
+ y = (y-yc)*my ;
+
+ alpha_ = x*si - y*co;
+
+ double *Lrho = H[0] ;
+ double *Ltheta = H[1] ;
+ // Calculate interaction matrix for a distance
+ for (unsigned int k=0 ; k < 6 ; k++)
+ {
+ L[j][k] = (Lrho[k] + alpha_*Ltheta[k]);
+ }
+ error[j] = rho - ( x*co + y*si) ;
+ j++;
+ }
+ }
+ }
+}
+
+/*!
+ Test wether the line is close to the border of the image (at a given threshold)
+
+ \param I : the input image (to know its size)
+ \param threshold : the threshold in pixel
+ \return true if the line is near the border of the image
+*/
+bool
+vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold)
+{
+ if(threshold > I.getWidth() || threshold > I.getHeight()){
+ return true;
+ }
+ if (isvisible){
+
+ for(unsigned int i = 0 ; i < meline.size() ; i++){
+ for(std::list<vpMeSite>::const_iterator it=meline[i]->getMeList().begin(); it!=meline[i]->getMeList().end(); ++it){
+ int i_ = it->i ;
+ int j_ = it->j ;
+
+ if(i_ < 0 || j_ < 0){ //out of image.
+ return true;
+ }
+
+ if( ((unsigned int)i_ > (I.getHeight()- threshold) ) || (unsigned int)i_ < threshold ||
+ ((unsigned int)j_ > (I.getWidth ()- threshold) ) || (unsigned int)j_ < threshold ) {
+ return true;
+ }
+ }
+ }
+ }
+ return false;
+}
diff --git a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp
new file mode 100644
index 0000000..9bd8041
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp
@@ -0,0 +1,545 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#include <visp3/mbt/vpMbtMeEllipse.h>
+
+#include <visp3/me/vpMe.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <algorithm> // std::min
+
+
+/*!
+ Basic constructor that calls the constructor of the class vpMeTracker.
+*/
+vpMbtMeEllipse::vpMbtMeEllipse()
+ : iPc(), a(0.), b(0.), e(0.),
+ ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
+{
+}
+
+/*!
+ Copy constructor.
+*/
+vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse)
+ : vpMeTracker(meellipse), iPc(), a(0.), b(0.), e(0.),
+ ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
+{
+ iPc = meellipse.iPc;
+ a = meellipse.a;
+ b = meellipse.b;
+ e = meellipse.e;
+
+ ce = meellipse.ce;
+ se = meellipse.se;
+
+ mu11 = meellipse.mu11;
+ mu20 = meellipse.mu20;
+ mu02 = meellipse.mu02;
+
+ expecteddensity = meellipse.expecteddensity;
+}
+
+/*!
+ Basic destructor.
+*/
+vpMbtMeEllipse::~vpMbtMeEllipse()
+{
+ list.clear();
+}
+
+/*!
+ Compute the projection error of the ellipse.
+ Compare the gradient direction around samples of the ellipse to the normal of the tangent of the considered sample.
+ Error is expressed in radians between 0 and M_PI/2.0;
+
+ \param _I : Image in which the line appears.
+ \param _sumErrorRad : sum of the error per feature.
+ \param _nbFeatures : Number of features used to compute _sumErrorRad.
+*/
+void
+vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures)
+{
+ _sumErrorRad = 0;
+ _nbFeatures = 0;
+
+// vpMatrix filterX(3,3);
+// filterX[0][0] = -1;
+// filterX[1][0] = -2;
+// filterX[2][0] = -1;
+
+// filterX[0][1] = 0;
+// filterX[1][1] = 0;
+// filterX[2][1] = 0;
+
+// filterX[0][2] = 1;
+// filterX[1][2] = 2;
+// filterX[2][2] = 1;
+
+// vpMatrix filterY(3,3);
+// filterY[0][0] = -1;
+// filterY[0][1] = -2;
+// filterY[0][2] = -1;
+
+// filterY[1][0] = 0;
+// filterY[1][1] = 0;
+// filterY[1][2] = 0;
+
+// filterY[2][0] = 1;
+// filterY[2][1] = 2;
+// filterY[2][2] = 1;
+
+ vpMatrix filterX(5,5);
+ filterX[0][0] = -1;
+ filterX[1][0] = -4;
+ filterX[2][0] = -6;
+ filterX[3][0] = -4;
+ filterX[4][0] = -1;
+
+ filterX[0][1] = -2;
+ filterX[1][1] = -8;
+ filterX[2][1] = -12;
+ filterX[3][1] = -8;
+ filterX[4][1] = -2;
+
+ filterX[0][2] = 0;
+ filterX[1][2] = 0;
+ filterX[2][2] = 0;
+ filterX[3][2] = 0;
+ filterX[4][2] = 0;
+
+ filterX[0][3] = 2;
+ filterX[1][3] = 8;
+ filterX[2][3] = 12;
+ filterX[3][3] = 8;
+ filterX[4][3] = 2;
+
+ filterX[0][4] = 1;
+ filterX[1][4] = 4;
+ filterX[2][4] = 6;
+ filterX[3][4] = 4;
+ filterX[4][4] = 1;
+
+ vpMatrix filterY(5,5);
+ filterY[0][0] = -1;
+ filterY[0][1] = -4;
+ filterY[0][2] = -6;
+ filterY[0][3] = -4;
+ filterY[0][4] = -1;
+
+ filterY[1][0] = -2;
+ filterY[1][1] = -8;
+ filterY[1][2] = -12;
+ filterY[1][3] = -8;
+ filterY[1][4] = -2;
+
+ filterY[2][0] = 0;
+ filterY[2][1] = 0;
+ filterY[2][2] = 0;
+ filterY[2][3] = 0;
+ filterY[2][4] = 0;
+
+ filterY[3][0] = 2;
+ filterY[3][1] = 8;
+ filterY[3][2] = 12;
+ filterY[3][3] = 8;
+ filterY[3][4] = 2;
+
+ filterY[4][0] = 1;
+ filterY[4][1] = 4;
+ filterY[4][2] = 6;
+ filterY[4][3] = 4;
+ filterY[4][4] = 1;
+
+ double offset = std::floor(filterX.getRows() / 2.0f);
+// std::cout << "offset=" << offset << std::endl;
+ int height = (int) _I.getHeight() ;
+ int width = (int) _I.getWidth() ;
+
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ double iSite = it->ifloat;
+ double jSite = it->jfloat;
+
+ if(!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height, width)) { // Check if necessary
+ // The tangent angle to the ellipse at a site
+ double theta = atan( (-mu02*jSite + mu02*iPc.get_j() + mu11*iSite - mu11*iPc.get_i())
+ / (mu20*iSite - mu11*jSite + mu11*iPc.get_j() - mu20*iPc.get_i()))
+ - M_PI/2;
+
+ double deltaNormalized = theta;
+ while (deltaNormalized<0) deltaNormalized += M_PI;
+ while (deltaNormalized>M_PI) deltaNormalized -= M_PI;
+
+ vpColVector vecSite(2);
+ vecSite[0] = cos(deltaNormalized);
+ vecSite[1] = sin(deltaNormalized);
+ vecSite.normalize();
+
+ double gradientX = 0;
+ double gradientY = 0;
+
+ for(unsigned int i = 0; i<filterX.getRows() ; i++){
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j< filterX.getCols(); j++){
+ double jImg = jSite + (j-offset);
+
+ if(iImg < 0) iImg = 0.0;
+ if(jImg < 0) jImg = 0.0;
+
+ if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
+ if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+
+ gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ }
+ }
+
+ for(unsigned int i = 0; i<filterY.getRows() ; i++){
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j< filterY.getCols(); j++){
+ double jImg = jSite + (j-offset);
+
+ if(iImg < 0) iImg = 0.0;
+ if(jImg < 0) jImg = 0.0;
+
+ if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
+ if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+
+ gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ }
+ }
+
+ double angle = atan2(gradientY,gradientX);
+ while (angle<0) angle += M_PI;
+ while (angle>M_PI) angle -= M_PI;
+
+// if(std::fabs(deltaNormalized-angle) > M_PI / 2)
+// {
+// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI / 2;
+// } else {
+// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
+// }
+
+// double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
+// double angle2 = sqrt(vpMath::sqr(deltaNormalized- (angle-M_PI)));
+
+ vpColVector vecGrad(2);
+ vecGrad[0] = cos(angle);
+ vecGrad[1] = sin(angle);
+ vecGrad.normalize();
+
+ double angle1 = acos(vecSite * vecGrad);
+ double angle2 = acos(vecSite * (-vecGrad));
+
+ _sumErrorRad += std::min(angle1,angle2);
+
+ _nbFeatures++;
+ }
+ }
+}
+
+/*!
+ 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.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMbtMeEllipse::sample(const vpImage<unsigned char> & I)
+{
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ int height = (int)I.getHeight() ;
+ int width = (int)I.getWidth() ;
+
+ //if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+ std::cout << "In vpMbtMeEllipse::sample: " ;
+ std::cout << "function called with sample step = 0" ;
+ //return fatalError ;
+ }
+
+ double j, i;//, j11, i11;
+ vpImagePoint iP11;
+ j = i = 0.0 ;
+
+ // Approximation of the circumference of an ellipse:
+ // [Ramanujan, S., "Modular Equations and Approximations to ,"
+ // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
+ double t = (a-b)/(a+b);
+ double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
+ int nb_points_to_track = (int)(circumference / me->getSampleStep());
+ double incr = 2*M_PI/nb_points_to_track;
+
+ expecteddensity = 0;//nb_points_to_track;
+
+ // Delete old list
+ list.clear();
+
+ // sample positions
+ double k = 0 ;
+ double iP_i, iP_j;
+ for (int pt=0; pt < nb_points_to_track; pt++)
+ {
+ j = a *cos(k) ; // equation of an ellipse
+ i = b *sin(k) ; // equation of an ellipse
+
+ iP_j = iPc.get_j() + ce *j - se *i;
+ iP_i = iPc.get_i() + se *j + ce *i;
+
+ //vpColor col = vpColor::red ;
+ //vpDisplay::displayCross(I, vpImagePoint(iP_i, iP_j), 5, col) ; //debug only
+
+ // If point is in the image, add to the sample list
+ if(!outOfImage(vpMath::round(iP_i), vpMath::round(iP_j), 0, height, width))
+ {
+ // The tangent angle to the ellipse at a site
+ double theta = atan( (-mu02*iP_j + mu02*iPc.get_j() + mu11*iP_i - mu11*iPc.get_i())
+ / (mu20*iP_i - mu11*iP_j + mu11*iPc.get_j() - mu20*iPc.get_i()))
+ - M_PI/2;
+
+ vpMeSite pix ;
+ pix.init((int)iP_i, (int)iP_j, theta) ;
+ pix.setDisplay(selectDisplay) ;
+ pix.setState(vpMeSite::NO_SUPPRESSION);
+
+ list.push_back(pix);
+ expecteddensity ++;
+ }
+ k += incr ;
+
+ }
+
+ vpMeTracker::initTracking(I) ;
+}
+
+
+/*!
+
+ Resample the ellipse if the number of sample is less than 90% of the
+ expected value.
+
+ \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
+ and the parameter which indicates the number of degrees between
+ two points (vpMe::sample_step).
+
+ \param I : Image in which the ellipse appears.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
+{
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ unsigned int n = numberOfSignal() ;
+ if ((double)n<0.9*expecteddensity){
+ sample(I) ;
+ vpMeTracker::track(I) ;
+ }
+}
+
+/*!
+ Compute the oriantation angle for each vpMeSite.
+
+ \note The \f$ \theta \f$ angle is useful during the tracking part.
+*/
+void
+vpMbtMeEllipse::updateTheta()
+{
+ vpMeSite p_me;
+ double theta;
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ vpImagePoint iP;
+ iP.set_i(p_me.ifloat);
+ iP.set_j(p_me.jfloat);
+
+ // The tangent angle to the ellipse at a site
+ theta = atan( (-mu02*p_me.jfloat + mu02*iPc.get_j() + mu11*p_me.ifloat - mu11*iPc.get_i())
+ / (mu20*p_me.ifloat - mu11*p_me.jfloat + mu11*iPc.get_j() - mu20*iPc.get_i()))
+ - M_PI/2;
+
+ p_me.alpha = theta ;
+ *it = p_me;
+ }
+}
+
+/*!
+ Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
+*/
+void
+vpMbtMeEllipse::suppressPoints()
+{
+ // Loop through list of sites to track
+ for(std::list<vpMeSite>::iterator itList=list.begin(); itList!=list.end();){
+ vpMeSite s = *itList;//current reference pixel
+ if (s.getState() != vpMeSite::NO_SUPPRESSION)
+ itList = list.erase(itList);
+ else
+ ++itList;
+ }
+}
+
+/*!
+ Display the 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.
+ */
+void
+vpMbtMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
+{
+ vpDisplay::displayEllipse(I, iPc, mu20, mu11, mu02, true, col);
+}
+
+void
+vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
+ double mu20_p, double mu11_p, double mu02_p)
+{
+ iPc = ic;
+ mu20 = mu20_p;
+ mu11 = mu11_p;
+ mu02 = mu02_p;
+
+ if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
+
+ double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p)/2);
+ b = sqrt((mu20_p + mu02_p - val_p)/2);
+
+ e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
+ }
+ else {
+ a = sqrt(mu20_p);
+ b = sqrt(mu02_p);
+ e = 0.;
+ }
+
+ e = atan(e);
+
+ ce = cos(e);
+ se = sin(e);
+
+ sample(I) ;
+
+ vpMeTracker::initTracking(I) ;
+
+ try{
+ track(I) ;
+ }
+ catch(vpException &exception)
+ {
+ throw(exception) ;
+ }
+}
+
+/*!
+ Track the ellipse in the image I.
+
+ \param I : Image in which the ellipse appears.
+*/
+void
+vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
+{
+ try{
+ vpMeTracker::track(I) ;
+ }
+ catch(vpException &exception)
+ {
+ throw(exception) ;
+ }
+}
+
+void
+vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p, double mu02_p)
+{
+ iPc = ic;
+ mu20 = mu20_p;
+ mu11 = mu11_p;
+ mu02 = mu02_p;
+
+ if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
+
+ double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p)/2);
+ b = sqrt((mu20_p + mu02_p - val_p)/2);
+
+ e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
+ }
+ else {
+ a = sqrt(mu20_p);
+ b = sqrt(mu02_p);
+ e = 0.;
+ }
+
+ e = atan(e);
+
+ ce = cos(e);
+ se = sin(e);
+
+ suppressPoints();
+ reSample(I);
+
+ // remet a jour l'angle delta pour chaque point de la liste
+ updateTheta();
+}
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp
new file mode 100644
index 0000000..ec5fd1e
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp
@@ -0,0 +1,867 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ \file vpMbtMeLine.cpp
+ \brief Make the complete tracking of an object by using its CAD model.
+*/
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <algorithm> // std::min
+
+#include <visp3/mbt/vpMbtMeLine.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpRobust.h>
+
+//! Normalize an angle between -Pi and Pi
+static void
+normalizeAngle(double &delta)
+{
+ while (delta > M_PI) { delta -= M_PI ; }
+ while (delta < -M_PI) { delta += M_PI ; }
+}
+
+
+/*!
+ Basic constructor that calls the constructor of the class vpMeTracker.
+*/
+vpMbtMeLine::vpMbtMeLine()
+ : rho(0.), theta(0.), theta_1(M_PI/2), delta(0.), delta_1(0), sign(1),
+ a(0.), b(0.), c(0.), imin(0), imax(0), jmin(0), jmax(0),
+ expecteddensity(0.)
+{
+}
+
+/*!
+ Basic destructor.
+*/
+vpMbtMeLine::~vpMbtMeLine()
+{
+ list.clear();
+}
+
+/*!
+ Initialization of the tracking. The line is defined thanks to the
+ coordinates of two points corresponding to the extremities and its (\f$\rho \: \theta\f$) parameters.
+
+ Remember the equation of a line : \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
+
+ \param I : Image in which the line appears.
+ \param ip1 : Coordinates of the first point.
+ \param ip2 : Coordinates of the second point.
+ \param rho_ : The \f$\rho\f$ parameter
+ \param theta_ : The \f$\theta\f$ parameter
+*/
+void
+vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ double rho_, double theta_)
+{
+ vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
+
+ try
+ {
+ // 1. On fait ce qui concerne les droites (peut etre vide)
+ // Points extremites
+ PExt[0].ifloat = (float)ip1.get_i() ;
+ PExt[0].jfloat = (float)ip1.get_j() ;
+ PExt[1].ifloat = (float)ip2.get_i() ;
+ PExt[1].jfloat = (float)ip2.get_j() ;
+
+ this->rho = rho_;
+ this->theta = theta_;
+ theta_1 = theta_;
+
+ a = cos(theta);
+ b = sin(theta);
+ c = -rho;
+
+ delta = - theta + M_PI/2.0;
+ normalizeAngle(delta);
+ delta_1 = delta;
+
+ sample(I) ;
+ expecteddensity = (double)list.size();
+
+ vpMeTracker::track(I);
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+ vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
+}
+
+
+/*!
+ 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.
+*/
+void
+vpMbtMeLine::sample(const vpImage<unsigned char>& I)
+{
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ double n_sample;
+
+ //if (me->getSampleStep==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "Function vpMbtMeLine::sample() called with moving-edges sample step = 0")) ;
+ }
+
+ // i, j portions of the line_p
+ double diffsi = PExt[0].ifloat-PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+
+ double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
+
+ // number of samples along line_p
+ n_sample = length_p/(double)me->getSampleStep();
+
+ double stepi = diffsi/(double)n_sample;
+ double stepj = diffsj/(double)n_sample;
+
+ // Choose starting point
+ double is = PExt[1].ifloat;
+ double js = PExt[1].jfloat;
+
+ // Delete old list
+ list.clear();
+
+ // sample positions at i*me->getSampleStep() interval along the
+ // line_p, starting at PSiteExt[0]
+
+ vpImagePoint ip;
+ for(int i=0; i<=vpMath::round(n_sample); i++)
+ {
+ // If point is in the image, add to the sample list
+ if(!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
+ {
+ vpMeSite pix ; //= list.value();
+ pix.init((int)is, (int)js, delta, 0, sign) ;
+
+ pix.track(I, me, false);
+
+ pix.setDisplay(selectDisplay) ;
+
+ if(vpDEBUG_ENABLE(3))
+ {
+ ip.set_i( is );
+ ip.set_j( js );
+ vpDisplay::displayCross(I, ip, 2, vpColor::blue);
+ }
+
+ list.push_back(pix);
+ }
+ is += stepi;
+ js += stepj;
+
+ }
+
+ vpCDEBUG(1) << "end vpMeLine::sample() : ";
+ vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl ;
+}
+
+
+/*!
+ Suppress the moving which belong no more to the line.
+
+ \param I : The image.
+*/
+void
+vpMbtMeLine::suppressPoints(const vpImage<unsigned char> & I)
+{
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
+ vpMeSite s = *it;//current reference pixel
+
+ if (fabs(sin(theta)) > 0.9) // Vertical line management
+ {
+ if ((s.i < imin) ||(s.i > imax))
+ {
+ s.setState(vpMeSite::CONSTRAST);
+ }
+ }
+
+ else if (fabs(cos(theta)) > 0.9) // Horizontal line management
+ {
+ if ((s.j < jmin) || (s.j > jmax))
+ {
+ s.setState(vpMeSite::CONSTRAST);
+ }
+ }
+
+ else
+ {
+ if ((s.i < imin) ||(s.i > imax) || (s.j < jmin) || (s.j > jmax) )
+ {
+ s.setState(vpMeSite::CONSTRAST);
+ }
+
+ }
+
+ if (outOfImage(s.i, s.j, (int)(me->getRange()+me->getMaskSize()+1), (int)I.getHeight(), (int)I.getWidth()))
+ {
+ s.setState(vpMeSite::TOO_NEAR);
+ }
+
+ if (s.getState() != vpMeSite::NO_SUPPRESSION)
+ it = list.erase(it);
+ else
+ ++it;
+ }
+}
+
+
+/*!
+ 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.
+*/
+void
+vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
+{
+ vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
+
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ double n_sample;
+
+ //if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "Function called with sample step = 0")) ;
+ }
+
+ // i, j portions of the line_p
+ double diffsi = PExt[0].ifloat-PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+
+ double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
+
+ double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
+ double dj = diffsj/sqrt(s) ;
+
+ double length_p = sqrt(s); /*(vpMath::sqr(diffsi)+vpMath::sqr(diffsj))*/
+
+ // number of samples along line_p
+ n_sample = length_p/(double)me->getSampleStep();
+ double sample_step = (double)me->getSampleStep();
+
+ vpMeSite P ;
+ P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
+ P.setDisplay(selectDisplay) ;
+
+ unsigned int memory_range = me->getRange() ;
+ me->setRange(1);
+
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
+
+ if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
+ {
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
+ }
+ else
+ if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
+ }
+ else
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
+ }
+ }
+
+ P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
+ P.setDisplay(selectDisplay) ;
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
+
+ if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
+ {
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
+ }
+
+ else
+ if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
+ }
+ else
+ if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
+ }
+ }
+
+ me->setRange(memory_range);
+
+ vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
+ vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
+}
+
+
+/*!
+ Compute the projection error of the line. Compare the gradient direction around samples of the line to its direction.
+ Error is expressed in radians between 0 and M_PI/2.0;
+
+ \param _I : Image in which the line appears.
+ \param _sumErrorRad : sum of the error per feature.
+ \param _nbFeatures : Number of features used to compute _sumErrorRad.
+*/
+void
+vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures)
+{
+ _sumErrorRad = 0;
+ _nbFeatures = 0;
+ double deltaNormalized = theta;
+ unsigned int iter = 0;
+
+ while (deltaNormalized<0) deltaNormalized += M_PI;
+ while (deltaNormalized>M_PI) deltaNormalized -= M_PI;
+
+ vpColVector vecLine(2);
+ vecLine[0] = cos(deltaNormalized);
+ vecLine[1] = sin(deltaNormalized);
+ vecLine.normalize();
+
+// vpMatrix filterX(3,3);
+// filterX[0][0] = -1;
+// filterX[1][0] = -2;
+// filterX[2][0] = -1;
+
+// filterX[0][1] = 0;
+// filterX[1][1] = 0;
+// filterX[2][1] = 0;
+
+// filterX[0][2] = 1;
+// filterX[1][2] = 2;
+// filterX[2][2] = 1;
+
+// vpMatrix filterY(3,3);
+// filterY[0][0] = -1;
+// filterY[0][1] = -2;
+// filterY[0][2] = -1;
+
+// filterY[1][0] = 0;
+// filterY[1][1] = 0;
+// filterY[1][2] = 0;
+
+// filterY[2][0] = 1;
+// filterY[2][1] = 2;
+// filterY[2][2] = 1;
+
+ vpMatrix filterX(5,5);
+ filterX[0][0] = -1;
+ filterX[1][0] = -4;
+ filterX[2][0] = -6;
+ filterX[3][0] = -4;
+ filterX[4][0] = -1;
+
+ filterX[0][1] = -2;
+ filterX[1][1] = -8;
+ filterX[2][1] = -12;
+ filterX[3][1] = -8;
+ filterX[4][1] = -2;
+
+ filterX[0][2] = 0;
+ filterX[1][2] = 0;
+ filterX[2][2] = 0;
+ filterX[3][2] = 0;
+ filterX[4][2] = 0;
+
+ filterX[0][3] = 2;
+ filterX[1][3] = 8;
+ filterX[2][3] = 12;
+ filterX[3][3] = 8;
+ filterX[4][3] = 2;
+
+ filterX[0][4] = 1;
+ filterX[1][4] = 4;
+ filterX[2][4] = 6;
+ filterX[3][4] = 4;
+ filterX[4][4] = 1;
+
+ vpMatrix filterY(5,5);
+ filterY[0][0] = -1;
+ filterY[0][1] = -4;
+ filterY[0][2] = -6;
+ filterY[0][3] = -4;
+ filterY[0][4] = -1;
+
+ filterY[1][0] = -2;
+ filterY[1][1] = -8;
+ filterY[1][2] = -12;
+ filterY[1][3] = -8;
+ filterY[1][4] = -2;
+
+ filterY[2][0] = 0;
+ filterY[2][1] = 0;
+ filterY[2][2] = 0;
+ filterY[2][3] = 0;
+ filterY[2][4] = 0;
+
+ filterY[3][0] = 2;
+ filterY[3][1] = 8;
+ filterY[3][2] = 12;
+ filterY[3][3] = 8;
+ filterY[3][4] = 2;
+
+ filterY[4][0] = 1;
+ filterY[4][1] = 4;
+ filterY[4][2] = 6;
+ filterY[4][3] = 4;
+ filterY[4][4] = 1;
+
+ double offset = std::floor(filterX.getRows() / 2.0f);
+
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ if(iter != 0 && iter+1 != list.size()){
+ double gradientX = 0;
+ double gradientY = 0;
+
+ double iSite = it->ifloat;
+ double jSite = it->jfloat;
+
+ for(unsigned int i = 0; i<filterX.getRows() ; i++){
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j< filterX.getCols(); j++){
+ double jImg = jSite + (j-offset);
+
+ if(iImg < 0) iImg = 0.0;
+ if(jImg < 0) jImg = 0.0;
+
+ if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
+ if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+
+ gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ }
+ }
+
+ for(unsigned int i = 0; i<filterY.getRows() ; i++){
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j< filterY.getCols(); j++){
+ double jImg = jSite + (j-offset);
+
+ if(iImg < 0) iImg = 0.0;
+ if(jImg < 0) jImg = 0.0;
+
+ if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
+ if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+
+ gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ }
+ }
+
+ double angle = atan2(gradientX,gradientY);
+ while (angle<0) angle += M_PI;
+ while (angle>M_PI) angle -= M_PI;
+
+ vpColVector vecGrad(2);
+ vecGrad[0] = cos(angle);
+ vecGrad[1] = sin(angle);
+ vecGrad.normalize();
+
+ double angle1 = acos(vecLine * vecGrad);
+ double angle2 = acos(vecLine * (-vecGrad));
+
+// double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
+// double angle2 = sqrt(vpMath::sqr(deltaNormalized- (angle-M_PI)));
+
+ _sumErrorRad += std::min(angle1,angle2);
+
+// if(std::fabs(deltaNormalized-angle) > M_PI / 2)
+// {
+// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI / 2;
+// } else {
+// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
+// }
+
+ _nbFeatures++;
+ }
+ iter++;
+ }
+}
+
+/*!
+ Resample the line if the number of sample is less than 50% 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
+vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
+{
+ unsigned int n = numberOfSignal() ;
+
+ if ((double)n<0.5*expecteddensity && n > 0)
+ {
+ double delta_new = delta;
+ delta = delta_1;
+ sample(I) ;
+ expecteddensity = (double)list.size();
+ delta = delta_new;
+ // 2. On appelle ce qui n'est pas specifique
+ {
+ vpMeTracker::initTracking(I) ;
+ }
+ }
+}
+
+
+/*!
+ Resample the line if the number of sample is less than 50% 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.
+ \param ip1 : The first extremity of the line.
+ \param ip2 : The second extremity of the line.
+*/
+void
+vpMbtMeLine::reSample(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2)
+{
+ size_t n = list.size();
+
+ if ((double)n<0.5*expecteddensity /*&& n > 0*/) // n is always > 0
+ {
+ double delta_new = delta;
+ delta = delta_1;
+ PExt[0].ifloat = (float)ip1.get_i() ;
+ PExt[0].jfloat = (float)ip1.get_j() ;
+ PExt[1].ifloat = (float)ip2.get_i() ;
+ PExt[1].jfloat = (float)ip2.get_j() ;
+ sample(I) ;
+ expecteddensity = (double)list.size();
+ delta = delta_new;
+ vpMeTracker::track(I) ;
+ }
+}
+
+/*!
+ Set the alpha value of the different vpMeSite to the value of delta.
+*/
+void
+vpMbtMeLine::updateDelta()
+{
+ vpMeSite p_me ;
+
+ double diff = 0;
+
+ //if(fabs(theta) == M_PI )
+ if(std::fabs(std::fabs(theta) - M_PI) <= vpMath::maximum(std::fabs(theta), (double)M_PI)*std::numeric_limits<double>::epsilon() )
+ {
+ theta = 0 ;
+ }
+
+ diff = fabs(theta - theta_1);
+ if (diff > M_PI/2.0)
+ sign *= -1;
+
+ theta_1 = theta;
+
+ delta = - theta + M_PI/2.0;
+ normalizeAngle(delta);
+
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ p_me.alpha = delta ;
+ p_me.mask_sign = sign;
+ *it = p_me;
+ }
+ delta_1 = delta;
+}
+
+/*!
+ Track the line in the image I.
+
+ \param I : Image in which the line appears.
+ */
+void
+vpMbtMeLine::track(const vpImage<unsigned char> &I)
+{
+ // 2. On appelle ce qui n'est pas specifique
+ try
+ {
+ vpMeTracker::track(I);
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+
+ // supression des points rejetes par les ME
+ // suppressPoints(I);
+ // setExtremities();
+}
+
+
+/*!
+ Update the moving edges parameters after the virtual visual servoing.
+
+ \param I : The image.
+ \param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
+ \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
+*/
+void
+vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
+{
+ this->rho = rho_;
+ this->theta = theta_;
+ a = cos(theta);
+ b = sin(theta);
+ c = -rho;
+ // recherche de point aux extremite de la droites
+ // dans le cas d'un glissement
+ suppressPoints(I);
+ seekExtremities(I);
+ suppressPoints(I);
+ setExtremities();
+ //reechantillonage si necessaire
+ reSample(I);
+
+ // remet a jour l'angle delta pour chaque point de la liste
+ updateDelta();
+}
+
+
+/*!
+ Update the moving edges parameters after the virtual visual servoing.
+
+ \param I : The image.
+ \param ip1 : The first extremity of the line.
+ \param ip2 : The second extremity of the line.
+ \param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
+ \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
+*/
+void
+vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2,
+ double rho_, double theta_)
+{
+ this->rho = rho_;
+ this->theta = theta_;
+ a = cos(theta);
+ b = sin(theta);
+ c = -rho;
+ // recherche de point aux extremite de la droites
+ // dans le cas d'un glissement
+ suppressPoints(I);
+ seekExtremities(I);
+ suppressPoints(I);
+ setExtremities();
+ //reechantillonage si necessaire
+ reSample(I,ip1,ip2);
+
+ // remet a jour l'angle delta pour chaque point de la liste
+ updateDelta();
+}
+
+
+/*!
+ Seek in the list of available points the two extremities of the line.
+*/
+void
+vpMbtMeLine::setExtremities()
+{
+ double i_min = +1e6 ;
+ double j_min = +1e6;
+ double i_max = -1 ;
+ double j_max = -1 ;
+
+ // Loop through list of sites to track
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite s = *it;//current reference pixel
+ if (s.ifloat < i_min)
+ {
+ i_min = s.ifloat ;
+ j_min = s.jfloat ;
+ }
+
+ if (s.ifloat > i_max)
+ {
+ i_max = s.ifloat ;
+ j_max = s.jfloat ;
+ }
+ }
+
+ if ( ! list.empty() )
+ {
+ PExt[0].ifloat = i_min ;
+ PExt[0].jfloat = j_min ;
+ PExt[1].ifloat = i_max ;
+ PExt[1].jfloat = j_max ;
+ }
+
+ if (fabs(i_min-i_max) < 25)
+ {
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite s = *it;//current reference pixel
+ if (s.jfloat < j_min)
+ {
+ i_min = s.ifloat ;
+ j_min = s.jfloat ;
+ }
+
+ if (s.jfloat > j_max)
+ {
+ i_max = s.ifloat ;
+ j_max = s.jfloat ;
+ }
+ }
+
+ if (! list.empty())
+ {
+ PExt[0].ifloat = i_min ;
+ PExt[0].jfloat = j_min ;
+ PExt[1].ifloat = i_max ;
+ PExt[1].jfloat = j_max ;
+ }
+ bubbleSortJ();
+ }
+
+ else
+ bubbleSortI();
+}
+
+
+static bool sortByI(const vpMeSite& s1, const vpMeSite& s2){
+ return (s1.ifloat > s2.ifloat);
+}
+
+void
+vpMbtMeLine::bubbleSortI()
+{
+#if 0
+ unsigned int nbElmt = list.size();
+ for (unsigned int pass = 1; pass < nbElmt; pass++)
+ {
+ list.front();
+ for (unsigned int i=0; i < nbElmt-pass; i++)
+ {
+ vpMeSite s1 = list.value() ;
+ vpMeSite s2 = list.nextValue() ;
+ if (s1.ifloat > s2.ifloat)
+ list.swapRight();
+ else
+ list.next();
+ }
+ }
+#endif
+ list.sort(sortByI);
+}
+
+
+static bool sortByJ(const vpMeSite& s1, const vpMeSite& s2){
+ return (s1.jfloat > s2.jfloat);
+}
+
+void
+vpMbtMeLine::bubbleSortJ()
+{
+#if 0
+ unsigned int nbElmt = list.size();
+ for(unsigned int pass=1; pass < nbElmt; pass++)
+ {
+ list.front();
+ for (unsigned int i=0; i < nbElmt-pass; i++)
+ {
+ vpMeSite s1 = list.value() ;
+ vpMeSite s2 = list.nextValue() ;
+ if (s1.jfloat > s2.jfloat)
+ list.swapRight();
+ else
+ list.next();
+ }
+ }
+#endif
+ list.sort(sortByJ);
+}
+
+
+void
+vpMbtMeLine::findSignal(const vpImage<unsigned char>& I, const vpMe *p_me, double *conv)
+{
+ vpImagePoint itest(PExt[0].ifloat+(PExt[1].ifloat-PExt[0].ifloat)/2, PExt[0].jfloat+(PExt[1].jfloat-PExt[0].jfloat)/2);
+
+ vpMeSite pix ; //= list.value();
+ pix.init(itest.get_i(), itest.get_j(), delta, 0, sign);
+
+ vpMeSite *list_query_pixels;
+// double convolution = 0;
+ unsigned int range = p_me->getRange();
+
+ list_query_pixels = pix.getQueryList(I, (int)range);
+
+ vpDisplay::displayCross(I,itest,5,vpColor::cyan,3);
+ vpDisplay::displayLine(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),vpImagePoint(list_query_pixels[2*range].ifloat,list_query_pixels[2*range].jfloat),vpColor::cyan);
+ vpDisplay::displayCross(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),5,vpColor::orange,3);
+
+ for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
+ {
+ conv[n] = list_query_pixels[n].convolution(I, p_me);
+ }
+ delete [] list_query_pixels;
+}
+
+#endif
+
diff --git a/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp
new file mode 100644
index 0000000..8af522d
--- /dev/null
+++ b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp
@@ -0,0 +1,522 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using edges).
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <map>
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbtXmlParser.h>
+
+
+/*!
+ Default constructor.
+
+*/
+vpMbtXmlParser::vpMbtXmlParser() : m_ecm()
+{
+ init();
+}
+
+/*!
+ Default destructor.
+*/
+vpMbtXmlParser::~vpMbtXmlParser()
+{
+}
+
+/*!
+ Initialise internal variables (including the map).
+*/
+void
+vpMbtXmlParser::init()
+{
+ vpMbXmlParser::init();
+
+ nodeMap["ecm"] = ecm;
+ nodeMap["mask"] = mask;
+ nodeMap["size"] = size;
+ nodeMap["nb_mask"] = nb_mask;
+ nodeMap["range"] = range;
+ nodeMap["tracking"] = tracking;
+ nodeMap["contrast"] = contrast;
+ nodeMap["edge_threshold"] = edge_threshold;
+ nodeMap["mu1"] = mu1;
+ nodeMap["mu2"] = mu2;
+ nodeMap["sample"] = sample;
+ nodeMap["step"] = step;
+}
+
+/*!
+ Parse the file in parameters.
+ This method is deprecated, use parse() instead.
+
+ \param filename : File to parse.
+*/
+void
+vpMbtXmlParser::parse(const char * filename)
+{
+ std::string file = filename;
+ vpXmlParser::parse(file);
+}
+
+/*!
+ Write info to file.
+
+ \warning Useless, so not yet implemented => Throw exception.
+*/
+void
+vpMbtXmlParser::writeMainClass(xmlNodePtr /*node*/)
+{
+ throw vpException(vpException::notImplementedError, "Not yet implemented." );
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void
+vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool camera_node = false;
+ bool face_node = false;
+ bool ecm_node = false;
+ bool lod_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case camera:{
+ this->read_camera (doc, dataNode);
+ camera_node = true;
+ }break;
+ case face:{
+ this->read_face(doc, dataNode);
+ face_node = true;
+ }break;
+ case ecm:{
+ this->read_ecm (doc, dataNode);
+ ecm_node = true;
+ }break;
+ case sample:{
+ this->read_sample_deprecated (doc, dataNode);
+ }break;
+ case lod:{
+ this->read_lod(doc, dataNode);
+ lod_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!camera_node) {
+ std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+ std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ }
+
+ if(!face_node) {
+ std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
+ std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ }
+
+ if(!ecm_node) {
+ std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
+ std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+ std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+ std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+ std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ }
+
+ if(!lod_node) {
+ std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
+ std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
+ }
+}
+
+
+/*!
+ Read ecm information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the ecm information.
+*/
+void
+vpMbtXmlParser::read_ecm (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool mask_node = false;
+ bool range_node = false;
+ bool contrast_node = false;
+ bool sample_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case mask:{
+ this->read_mask (doc, dataNode);
+ mask_node = true;
+ }break;
+ case range:{
+ this->read_range (doc, dataNode);
+ range_node = true;
+ }break;
+ case contrast:{
+ this->read_contrast (doc, dataNode);
+ contrast_node = true;
+ }break;
+ case sample:{
+ this->read_sample (doc, dataNode);
+ sample_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_ecm : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!mask_node) {
+ std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
+ std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+ }
+
+ if(!range_node) {
+ std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+ }
+
+ if(!contrast_node) {
+ std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+ }
+
+ if(!sample_node) {
+ std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ }
+}
+
+/*!
+ Read sample information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the sample information.
+*/
+void
+vpMbtXmlParser::read_sample (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool step_node = false;
+
+ // current data values.
+ double d_stp = this->m_ecm.getSampleStep();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case step:{
+ d_stp = xmlReadIntChild(doc, dataNode);
+ step_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->m_ecm.setSampleStep(d_stp);
+// this->m_ecm.setNbTotalSample(d_nb_sample);
+
+ if(!step_node)
+ std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ else
+ std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<<std::endl;
+}
+
+/*!
+ Read sample information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the sample information.
+*/
+void
+vpMbtXmlParser::read_sample_deprecated (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool step_node = false;
+ //bool nb_sample_node = false;
+
+ // current data values.
+ double d_stp = this->m_ecm.getSampleStep();
+// int d_nb_sample = this->m_ecm.getNbTotalSample();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case step:{
+ d_stp = xmlReadIntChild(doc, dataNode);
+ step_node = true;
+ }break;
+// case nb_sample:{
+// d_nb_sample = xmlReadIntChild(doc, dataNode);
+// nb_sample_node = true;
+// }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->m_ecm.setSampleStep(d_stp);
+// this->m_ecm.setNbTotalSample(d_nb_sample);
+
+ if(!step_node)
+ std::cout <<"[DEPRECATED] sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ else
+ std::cout <<"[DEPRECATED] sample : sample_step : "<< this->m_ecm.getSampleStep()<<std::endl;
+
+// if(!nb_sample_node)
+// std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
+// else
+// std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<<std::endl;
+
+ std::cout <<" WARNING : This node (sample) is deprecated." << std::endl;
+ std::cout << " It should be moved in the ecm node (ecm : sample)." << std::endl;
+}
+
+/*!
+ Read mask information for the vpMeSite.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the mask information.
+*/
+void
+vpMbtXmlParser::read_mask (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool size_node = false;
+ bool nb_mask_node = false;
+
+ // current data values.
+ unsigned int d_size = this->m_ecm.getMaskSize();
+ unsigned int d_nb_mask = this->m_ecm.getMaskNumber();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case size:{
+ d_size = xmlReadUnsignedIntChild(doc, dataNode);
+ size_node = true;
+ }break;
+ case nb_mask:{
+ d_nb_mask = xmlReadUnsignedIntChild(doc, dataNode);
+ nb_mask_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_mask : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->m_ecm.setMaskSize(d_size) ;
+ // Check to ensure that d_nb_mask > 0
+ if (! d_nb_mask)
+ throw(vpException(vpException::badValue, "Model-based tracker mask size parameter should be different from zero in xml file"));
+ this->m_ecm.setMaskNumber(d_nb_mask);
+
+ if(!size_node)
+ std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
+ else
+ std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() <<std::endl;
+
+ if(!nb_mask_node)
+ std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+ else
+ std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() <<std::endl;
+}
+
+/*!
+ Read range information for the vpMeSite.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the range information.
+*/
+void
+vpMbtXmlParser::read_range (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool tracking_node = false;
+
+ // current data values.
+ unsigned int m_range_tracking = this->m_ecm.getRange();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case tracking:{
+ m_range_tracking = xmlReadUnsignedIntChild(doc, dataNode);
+ tracking_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_range : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->m_ecm.setRange(m_range_tracking);
+
+ if(!tracking_node)
+ std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+ else
+ std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<<std::endl;
+}
+
+
+/*!
+ Read the contrast information from the xml file.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the contrast information.
+*/
+void
+vpMbtXmlParser::read_contrast (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool edge_threshold_node = false;
+ bool mu1_node = false;
+ bool mu2_node = false;
+
+ // current data values.
+ double d_edge_threshold = this->m_ecm.getThreshold();
+ double d_mu1 = this->m_ecm.getMu1();
+ double d_mu2 = this->m_ecm.getMu2();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case edge_threshold:{
+ d_edge_threshold = xmlReadDoubleChild(doc, dataNode);
+ edge_threshold_node = true;
+ }break;
+ case mu1:{
+ d_mu1 = xmlReadDoubleChild(doc, dataNode);
+ mu1_node = true;
+ }break;
+ case mu2:{
+ d_mu2 = xmlReadDoubleChild(doc, dataNode);
+ mu2_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->m_ecm.setMu1(d_mu1);
+ this->m_ecm.setMu2(d_mu2);
+ this->m_ecm.setThreshold(d_edge_threshold);
+
+ if(!edge_threshold_node)
+ std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
+ else
+ std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<std::endl;
+
+ if(!mu1_node)
+ std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
+ else
+ std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<std::endl;
+
+ if(!mu2_node)
+ std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+ else
+ std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<std::endl;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtXmlParser.cpp.o) has no symbols
+void dummy_vpMbtXmlParser() {};
+#endif
+
diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp
new file mode 100644
index 0000000..b3c03ed
--- /dev/null
+++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp
@@ -0,0 +1,1398 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+//#define VP_DEBUG_MODE 1 // Activate debug level 1
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+vpMbEdgeKltTracker::vpMbEdgeKltTracker()
+ : compute_interaction(true), lambda(0.8), thresholdKLT(2.), thresholdMBT(2.), maxIter(200)
+{
+ computeCovariance = false;
+
+ vpMbKltTracker::setMaxIter(30);
+
+ angleAppears = vpMath::rad(65);
+ angleDisappears = vpMath::rad(75);
+
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Hybrid");
+#endif
+}
+
+/*!
+ Basic constructor
+
+*/
+vpMbEdgeKltTracker::~vpMbEdgeKltTracker()
+{
+}
+
+/*!
+ Initialization of the tracker using a known initial pose.
+ The 3D model must first have been loaded.
+
+ \param I : Input image.
+*/
+void
+vpMbEdgeKltTracker::init(const vpImage<unsigned char>& I)
+{
+ vpMbKltTracker::init(I);
+
+ initPyramid(I, Ipyramid);
+
+ vpMbEdgeTracker::resetMovingEdge();
+
+ unsigned int i = (unsigned int)scales.size();
+ do {
+ i--;
+ if(scales[i]){
+ downScale(i);
+ initMovingEdge(*Ipyramid[i], cMo);
+ upScale(i);
+ }
+ } while(i != 0);
+
+ cleanPyramid(Ipyramid);
+}
+
+/*!
+ Set the pose to be used in entry (as guess) of the next call to the track() function.
+ This pose will be just used once.
+
+ \warning This functionnality is not available when tracking cylinders.
+
+ \param I : image corresponding to the desired pose.
+ \param cdMo : Pose to affect.
+*/
+void
+vpMbEdgeKltTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+{
+ vpMbKltTracker::setPose(I, cdMo);
+
+ resetMovingEdge();
+
+ if(useScanLine){
+ cam.computeFov(I.getWidth(), I.getHeight());
+ faces.computeClippedPolygons(cMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ initPyramid(I, Ipyramid);
+
+ unsigned int i = (unsigned int)scales.size();
+ do {
+ i--;
+ if(scales[i]){
+ downScale(i);
+ initMovingEdge(*Ipyramid[i], cMo);
+ upScale(i);
+ }
+ } while(i != 0);
+
+ cleanPyramid(Ipyramid);
+}
+
+/*!
+ Reset the tracker. The model is removed and the pose is set to identity.
+ The tracker needs to be initialized with a new model and a new pose.
+*/
+void
+vpMbEdgeKltTracker::resetTracker()
+{
+ vpMbEdgeTracker::resetTracker();
+ vpMbKltTracker::resetTracker();
+}
+
+unsigned int
+vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
+{
+ vpMbtDistanceLine *l ;
+ vpMbtDistanceCylinder *cy ;
+ vpMbtDistanceCircle *ci ;
+
+ if(lvl >= scales.size() || !scales[lvl]){
+ throw vpException(vpException::dimensionError, "lvl not used.");
+ }
+
+ unsigned int nbrow = 0;
+ for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ l = *it;
+
+ if(l->isVisible() && l->isTracked()){
+ nbrow += l->nbFeatureTotal ;
+ l->initInteractionMatrixError() ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ cy = *it;
+
+ if(cy->isVisible() && cy->isTracked()){
+ nbrow += cy->nbFeature ;
+ cy->initInteractionMatrixError() ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
+ ci = *it;
+
+ if(ci->isVisible() && ci ->isTracked()){
+ nbrow += ci->nbFeature ;
+ ci->initInteractionMatrixError() ;
+ }
+ }
+
+ return nbrow;
+}
+
+/*!
+ Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
+ From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \param configFile : full name of the xml file.
+
+ \sa loadConfigFile(const char*), vpXmlParser::cleanup()
+*/
+void
+vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
+{
+ vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
+}
+
+/*!
+ Load the xml configuration file.
+ From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \throw vpException::ioError if the file has not been properly parsed (file not
+ found or wrong format for the data).
+
+ \param configFile : full name of the xml file.
+
+ The XML configuration file has the following form:
+ \code
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>10</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>7000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <camera>
+ <width>640</width>
+ <height>480</height>
+ <u0>320</u0>
+ <v0>240</v0>
+ <px>686.24</px>
+ <py>686.24</py>
+ </camera>
+ <face>
+ <angle_appear>65</angle_appear>
+ <angle_disappear>85</angle_disappear>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>0.90</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+ <klt>
+ <mask_border>10</mask_border>
+ <max_features>10000</max_features>
+ <window_size>5</window_size>
+ <quality>0.02</quality>
+ <min_distance>10</min_distance>
+ <harris>0.02</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+</conf>
+ \endcode
+
+ \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
+*/
+void
+vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtEdgeKltXmlParser xmlp;
+
+ xmlp.setCameraParameters(cam);
+ xmlp.setAngleAppear(vpMath::deg(angleAppears));
+ xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
+
+ xmlp.setMovingEdge(me);
+
+ xmlp.setMaxFeatures(10000);
+ xmlp.setWindowSize(5);
+ xmlp.setQuality(0.01);
+ xmlp.setMinDistance(5);
+ xmlp.setHarrisParam(0.01);
+ xmlp.setBlockSize(3);
+ xmlp.setPyramidLevels(3);
+ xmlp.setMaskBorder(maskBorder);
+
+ try{
+ std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
+ xmlp.parse(configFile);
+ }
+ catch(...){
+ vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
+ throw vpException(vpException::ioError, "problem to parse configuration file.");
+ }
+
+ vpCameraParameters camera;
+ xmlp.getCameraParameters(camera);
+ setCameraParameters(camera);
+
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ if(xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if(xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if(xmlp.getFovClipping()){
+ setClipping(vpMbEdgeTracker::clippingFlag | vpPolygon3D::FOV_CLIPPING);
+ }
+
+ useLodGeneral = xmlp.getLodState();
+ minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold();
+ minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
+
+ applyLodSettingInConfig = false;
+ if(this->getNbPolygon() > 0) {
+ applyLodSettingInConfig = true;
+ setLod(useLodGeneral);
+ setMinLineLengthThresh(minLineLengthThresholdGeneral);
+ setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+ }
+
+ vpMe meParser;
+ xmlp.getMe(meParser);
+ vpMbEdgeTracker::setMovingEdge(meParser);
+
+ tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
+ tracker.setWindowSize((int)xmlp.getWindowSize());
+ tracker.setQuality(xmlp.getQuality());
+ tracker.setMinDistance(xmlp.getMinDistance());
+ tracker.setHarrisFreeParameter(xmlp.getHarrisParam());
+ tracker.setBlockSize((int)xmlp.getBlockSize());
+ tracker.setPyramidLevels((int)xmlp.getPyramidLevels());
+ maskBorder = xmlp.getMaskBorder();
+
+ //if(useScanLine)
+ faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
+
+#else
+ vpTRACE("You need the libXML2 to read the config file %s", configFile);
+#endif
+}
+
+/*!
+ Realize the post tracking operations. Mostly visibility tests
+*/
+bool
+vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
+ const unsigned int lvl)
+{
+
+ postTrackingMbt(w_mbt,lvl);
+
+ if (displayFeatures)
+ {
+ if(lvl == 0){
+ vpMbtDistanceLine* l;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ l = *it;
+ if (l->isVisible() && l->isTracked()){
+ l->displayMovingEdges(I);
+ }
+ }
+
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ cy = *it;
+ // A cylinder is always visible: #FIXME AY: Still valid?
+ if(cy->isTracked())
+ cy->displayMovingEdges(I);
+ }
+
+ vpMbtDistanceCircle *ci ;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
+ ci = *it;
+ if (ci->isVisible() && ci->isTracked()){
+ ci->displayMovingEdges(I);
+ }
+ }
+ }
+ }
+
+ bool reInit = vpMbKltTracker::postTracking(I, w_klt);
+
+ if(useScanLine){
+ cam.computeFov(I.getWidth(), I.getHeight());
+ faces.computeClippedPolygons(cMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ vpMbEdgeTracker::updateMovingEdge(I);
+
+ vpMbEdgeTracker::initMovingEdge(I, cMo) ;
+ vpMbEdgeTracker::reinitMovingEdge(I, cMo);
+
+ if(computeProjError)
+ vpMbEdgeTracker::computeProjectionError(I);
+
+ if(reInit)
+ return true;
+
+ return false;
+}
+
+/*!
+ Post tracking computation. Compute the mean weight of a line and, check the
+ weight associated to a site (to eventually remove an outlier) and eventually
+ set a flag to re-initialize the line.
+
+ \warning level parameter not yet implemented.
+
+ \param w : Vector of weight associated to the residual.
+ \param lvl : Optional parameter to specify the level to track.
+*/
+void
+vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
+{
+ if(lvl >= scales.size() || !scales[lvl]){
+ throw vpException(vpException::dimensionError, "_lvl not used.");
+ }
+
+ unsigned int n =0 ;
+ vpMbtDistanceLine* l;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ unsigned int indexLine = 0;
+ double wmean = 0 ;
+
+ for(unsigned int a = 0 ; a < l->meline.size() ; a++)
+ {
+ std::list<vpMeSite>::iterator itListLine;
+ if (l->nbFeature[a] > 0) itListLine = l->meline[a]->getMeList().begin();
+
+ for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
+ wmean += w[n+indexLine] ;
+ vpMeSite p = *itListLine;
+ if (w[n+indexLine] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+ *itListLine = p;
+ }
+
+ ++itListLine;
+ indexLine++;
+ }
+ }
+
+ n+= l->nbFeatureTotal ;
+
+ if (l->nbFeatureTotal!=0)
+ wmean /= l->nbFeatureTotal ;
+ else
+ wmean = 1;
+
+ l->setMeanWeight(wmean);
+
+ if (wmean < 0.8)
+ l->Reinit = true;
+ }
+ }
+
+ // Same thing with cylinders as with lines
+ vpMbtDistanceCylinder *cy ;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ double wmean = 0 ;
+ std::list<vpMeSite>::iterator itListCyl1;
+ std::list<vpMeSite>::iterator itListCyl2;
+ if (cy->nbFeature > 0){
+ itListCyl1 = cy->meline1->getMeList().begin();
+ itListCyl2 = cy->meline2->getMeList().begin();
+ }
+
+ wmean = 0;
+ for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
+ wmean += w[n+i] ;
+ vpMeSite p = *itListCyl1;
+ if (w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCyl1 = p;
+ }
+
+ ++itListCyl1;
+ }
+
+ if (cy->nbFeaturel1!=0)
+ wmean /= cy->nbFeaturel1 ;
+ else
+ wmean = 1;
+
+ cy->setMeanWeight1(wmean);
+
+ if (wmean < 0.8){
+ cy->Reinit = true;
+ }
+
+ wmean = 0;
+ for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
+ wmean += w[n+i] ;
+ vpMeSite p = *itListCyl2;
+ if (w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCyl2 = p;
+ }
+
+ ++itListCyl2;
+ }
+
+ if (cy->nbFeaturel2!=0)
+ wmean /= cy->nbFeaturel2 ;
+ else
+ wmean = 1;
+
+ cy->setMeanWeight2(wmean);
+
+ if (wmean < 0.8){
+ cy->Reinit = true;
+ }
+
+ n+= cy->nbFeature ;
+ }
+ }
+
+ // Same thing with circles as with lines
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ double wmean = 0 ;
+ std::list<vpMeSite>::iterator itListCir;
+
+ if (ci->nbFeature > 0){
+ itListCir = ci->meEllipse->getMeList().begin();
+ }
+
+ wmean = 0;
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ wmean += w[n+i] ;
+ vpMeSite p = *itListCir;
+ if (w[n+i] < 0.5){
+ p.setState(vpMeSite::M_ESTIMATOR);
+
+ *itListCir = p;
+ }
+
+ ++itListCir;
+ }
+
+ if (ci->nbFeature!=0)
+ wmean /= ci->nbFeature ;
+ else
+ wmean = 1;
+
+ ci->setMeanWeight(wmean);
+
+ if (wmean < 0.8){
+ ci->Reinit = true;
+ }
+
+ n+= ci->nbFeature ;
+ }
+ }
+}
+
+/*!
+ Realize the VVS loop for the tracking
+
+ \param I : current image.
+ \param nbInfos : Size of the features.
+ \param w_mbt : weight vector for MBT.
+ \param w_klt : weight vector for KLT.
+ \param lvl : level of the pyramid.
+*/
+void
+vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
+{
+ vpColVector factor;
+ unsigned int nbrow = trackFirstLoop(I, factor, lvl);
+
+ if(nbrow < 4 && nbInfos < 4){
+ vpERROR_TRACE("\n\t\t Error-> not enough data") ;
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
+ }
+ else if(nbrow < 4)
+ nbrow = 0;
+
+ double residu = 0;
+ double residu_1 = -1;
+ unsigned int iter = 0;
+
+ vpMatrix *L;
+ vpMatrix L_mbt, L_klt; // interaction matrix
+ vpColVector *R;
+ vpColVector R_mbt, R_klt; // residu
+ vpMatrix L_true;
+ vpMatrix LVJ_true;
+ //vpColVector R_true;
+ vpColVector w_true;
+
+ if(nbrow != 0){
+ L_mbt.resize(nbrow,6);
+ R_mbt.resize(nbrow);
+ }
+
+ if(nbInfos != 0){
+ L_klt.resize(2*nbInfos,6);
+ R_klt.resize(2*nbInfos);
+ }
+
+ //vpColVector w; // weight from MEstimator
+ vpColVector v; // "speed" for VVS
+ vpRobust robust_mbt(0), robust_klt(0);
+ vpHomography H;
+
+ vpMatrix LTL;
+ vpColVector LTR;
+
+ double factorMBT = 1.0;
+ double factorKLT = 1.0;
+
+ //More efficient weight repartition for hybrid tracker should come soon...
+// factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
+// factorKLT = 1.0 - factorMBT;
+ factorMBT = 0.35;
+ factorKLT = 0.65;
+
+ if (nbrow < 4)
+ factorKLT = 1.;
+ if (nbInfos < 4)
+ factorMBT = 1.;
+
+ double residuMBT = 0;
+ double residuKLT = 0;
+
+ vpHomogeneousMatrix cMoPrev;
+ vpHomogeneousMatrix ctTc0_Prev;
+ vpColVector m_error_prev(2*nbInfos + nbrow);
+ vpColVector m_w_prev(2*nbInfos + nbrow);
+ double mu = 0.01;
+
+ while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
+ L = new vpMatrix();
+ R = new vpColVector();
+
+ if(nbrow >= 4)
+ trackSecondLoop(I,L_mbt,R_mbt,cMo,lvl);
+
+ if(nbInfos >= 4){
+ unsigned int shift = 0;
+ vpMbtDistanceKltPoints *kltpoly;
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=vpMbKltTracker::kltPolygons.begin(); it!=vpMbKltTracker::kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()){
+ vpSubColVector subR(R_klt, shift, 2*kltpoly->getCurrentNumberPoints());
+ vpSubMatrix subL(L_klt, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
+ kltpoly->computeHomography(ctTc0, H);
+ kltpoly->computeInteractionMatrixAndResidu(subR, subL);
+ shift += 2*kltpoly->getCurrentNumberPoints();
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ {
+ vpSubColVector subR(R_klt, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
+ vpSubMatrix subL(L_klt, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
+ try{
+ kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0,subR, subL);
+ }catch(...){
+ throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
+ }
+
+ shift += 2*kltPolyCylinder->getCurrentNumberPoints();
+ }
+ }
+ }
+
+ bool reStartFromLastIncrement = false;
+ if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
+ if(m_error.sumSquare()/(double)(2*nbInfos + nbrow) > m_error_prev.sumSquare()/(double)(2*nbInfos + nbrow)){
+ mu *= 10.0;
+
+ if(mu > 1.0)
+ throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+
+ cMo = cMoPrev;
+ m_error = m_error_prev;
+ m_w = m_w_prev;
+ ctTc0 = ctTc0_Prev;
+ reStartFromLastIncrement = true;
+ }
+ }
+
+ if(!reStartFromLastIncrement){
+ if(iter == 0){
+ m_w.resize(nbrow + 2*nbInfos);
+ m_w=1;
+
+ if(nbrow != 0){
+ w_mbt.resize(nbrow);
+ w_mbt = 1;
+ robust_mbt.resize(nbrow);
+ }
+
+ if(nbInfos != 0){
+ w_klt.resize(2*nbInfos);
+ w_klt = 1;
+ robust_klt.resize(2*nbInfos);
+ }
+
+ w_true.resize(nbrow + 2*nbInfos);
+ }
+
+ /* robust */
+ if(nbrow > 3){
+ residuMBT = 0;
+ for(unsigned int i = 0; i < R_mbt.getRows(); i++)
+ residuMBT += fabs(R_mbt[i]);
+ residuMBT /= R_mbt.getRows();
+
+ robust_mbt.setIteration(iter);
+ robust_mbt.setThreshold(thresholdMBT/cam.get_px());
+ robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
+ L->stack(L_mbt);
+ R->stack(R_mbt);
+ }
+
+ if(nbInfos > 3){
+ residuKLT = 0;
+ for(unsigned int i = 0; i < R_klt.getRows(); i++)
+ residuKLT += fabs(R_klt[i]);
+ residuKLT /= R_klt.getRows();
+
+ robust_klt.setIteration(iter);
+ robust_klt.setThreshold(thresholdKLT/cam.get_px());
+ robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
+
+ L->stack(L_klt);
+ R->stack(R_klt);
+ }
+
+ unsigned int cpt = 0;
+ while(cpt< (nbrow+2*nbInfos)){
+ if(cpt<(unsigned)nbrow){
+ m_w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
+ }
+ else
+ m_w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
+ cpt++;
+ }
+
+ m_error = (*R);
+ if(computeCovariance){
+ L_true = (*L);
+ if(!isoJoIdentity){
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = ((*L)*cVo*oJo);
+ }
+ }
+
+ residu_1 = residu;
+ residu = 0;
+ double num = 0;
+ double den = 0;
+ for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
+ num += m_w[i]*vpMath::sqr((*R)[i]);
+ den += m_w[i];
+
+ w_true[i] = m_w[i];
+ (*R)[i] *= m_w[i];
+ if(compute_interaction){
+ for (unsigned int j = 0; j < 6; j += 1){
+ (*L)[i][j] *= m_w[i];
+ }
+ }
+ }
+
+ residu = sqrt(num/den);
+
+ if(isoJoIdentity){
+ LTL = L->AtA();
+ computeJTR(*L, *R, LTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LTL.getRows(), LTL.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LTL + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ m_w_prev = m_w;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
+ }
+ }
+ else{
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ vpMatrix LVJ = ((*L)*cVo*oJo);
+ vpMatrix LVJTLVJ = (LVJ).AtA();
+ vpColVector LVJTR;
+ computeJTR(LVJ, *R, LVJTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ m_w_prev = m_w;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ {
+ v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+ break;
+ }
+ }
+ }
+
+ cMoPrev = cMo;
+ ctTc0_Prev = ctTc0;
+ ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
+ cMo = ctTc0 * c0Mo;
+ }
+
+ iter++;
+
+ delete L;
+ delete R;
+ }
+
+ if(computeCovariance){
+ vpMatrix D;
+ D.diag(w_true);
+
+ // Note that here the covariance is computed on cMoPrev for time computation efficiency
+ if(isoJoIdentity){
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
+ }
+ else{
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
+ }
+ }
+}
+
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed.
+
+ \param I : the input image.
+*/
+void
+vpMbEdgeKltTracker::track(const vpImage<unsigned char>& I)
+{
+ unsigned int nbInfos = 0;
+ unsigned int nbFaceUsed = 0;
+ vpColVector w_klt;
+
+ try{
+ vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
+ }
+ catch(...){}
+
+ if(nbInfos >= 4)
+ vpMbKltTracker::computeVVS(nbInfos, w_klt);
+ else{
+ nbInfos = 0;
+ // std::cout << "[Warning] Unable to init with KLT" << std::endl;
+ }
+
+ vpMbEdgeTracker::trackMovingEdge(I);
+
+ vpColVector w_mbt;
+ computeVVS(I, nbInfos, w_mbt, w_klt);
+
+ if(postTracking(I, w_mbt, w_klt)){
+ vpMbKltTracker::reinit(I);
+
+ // AY : Removed as edge tracked, if necessary, is reinitialized in postTracking()
+
+// initPyramid(I, Ipyramid);
+
+// unsigned int i = (unsigned int)scales.size();
+// do {
+// i--;
+// if(scales[i]){
+// downScale(i);
+// initMovingEdge(*Ipyramid[i], cMo);
+// upScale(i);
+// }
+// } while(i != 0);
+
+// cleanPyramid(Ipyramid);
+ }
+}
+
+unsigned int
+vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl)
+{
+ vpMbtDistanceLine *l ;
+ vpMbtDistanceCylinder *cy ;
+ vpMbtDistanceCircle *ci ;
+
+ if(lvl >= scales.size() || !scales[lvl]){
+ throw vpException(vpException::dimensionError, "_lvl not used.");
+ }
+
+ unsigned int nbrow = initMbtTracking(lvl);
+
+ if (nbrow==0){
+// vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
+// throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
+ return nbrow;
+ }
+
+ factor.resize(nbrow);
+ factor = 1;
+
+ unsigned int n = 0;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ l->computeInteractionMatrixError(cMo);
+
+ double fac = 1;
+ for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ int index = *itindex;
+ if (l->hiddenface->isAppearing((unsigned int)index)) {
+ fac = 0.2;
+ break;
+ }
+ if(l->closeToImageBorder(I, 10)){
+ fac = 0.1;
+ break;
+ }
+ }
+
+ unsigned int indexFeature = 0;
+ for(unsigned int a = 0 ; a < l->meline.size(); a++){
+ std::list<vpMeSite>::const_iterator itListLine;
+ if (l->meline[a] != NULL)
+ {
+ itListLine = l->meline[a]->getMeList().begin();
+
+ for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
+ factor[n+i] = fac;
+ vpMeSite site = *itListLine;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ ++itListLine;
+ indexFeature++;
+ }
+ n+= l->nbFeature[a] ;
+ }
+ }
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ cy->computeInteractionMatrixError(cMo, I);
+ double fac = 1.0;
+
+ std::list<vpMeSite>::const_iterator itCyl1;
+ std::list<vpMeSite>::const_iterator itCyl2;
+ if ((cy->meline1 != NULL || cy->meline2 != NULL)){
+ itCyl1 = cy->meline1->getMeList().begin();
+ itCyl2 = cy->meline2->getMeList().begin();
+ }
+
+ for(unsigned int i=0 ; i < cy->nbFeature ; i++){
+ factor[n+i] = fac;
+ vpMeSite site;
+ if(i<cy->nbFeaturel1) {
+ site= *itCyl1;
+ ++itCyl1;
+ }
+ else{
+ site= *itCyl2;
+ ++itCyl2;
+ }
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ }
+
+ n+= cy->nbFeature ;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ ci->computeInteractionMatrixError(cMo);
+ double fac = 1.0;
+
+ std::list<vpMeSite>::const_iterator itCir;
+ if (ci->meEllipse != NULL) {
+ itCir = ci->meEllipse->getMeList().begin();
+ }
+
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ factor[n+i] = fac;
+ vpMeSite site = *itCir;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ ++itCir;
+ }
+
+ n+= ci->nbFeature ;
+ }
+ }
+
+ return nbrow;
+}
+
+void
+vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &error,
+ vpHomogeneousMatrix& cMo_, const unsigned int lvl)
+{
+ vpMbtDistanceLine* l;
+ vpMbtDistanceCylinder *cy ;
+ vpMbtDistanceCircle *ci ;
+
+ unsigned int n = 0 ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ if((*it)->isTracked()){
+ l = *it;
+ l->computeInteractionMatrixError(cMo_) ;
+ for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
+ for (unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = l->L[i][j];
+ error[n+i] = l->error[i];
+ }
+ }
+ n+= l->nbFeatureTotal;
+ }
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ if((*it)->isTracked()){
+ cy = *it;
+ cy->computeInteractionMatrixError(cMo_, I) ;
+ for(unsigned int i=0 ; i < cy->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = cy->L[i][j];
+ error[n+i] = cy->error[i];
+ }
+ }
+ n+= cy->nbFeature ;
+ }
+ }
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ if((*it)->isTracked()){
+ ci = *it;
+ ci->computeInteractionMatrixError(cMo) ;
+ for(unsigned int i=0 ; i < ci->nbFeature ; i++){
+ for(unsigned int j=0; j < 6 ; j++){
+ L[n+i][j] = ci->L[i][j];
+ error[n+i] = ci->error[i];
+ }
+ }
+
+ n+= ci->nbFeature ;
+ }
+ }
+}
+
+/*!
+ Set the camera parameters
+
+ \param camera : the new camera parameters
+*/
+void
+vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters& camera)
+{
+ this->cam = camera;
+
+ vpMbEdgeTracker::setCameraParameters(cam);
+ vpMbKltTracker::setCameraParameters(cam);
+}
+
+/*!
+ Initialise a new face from the coordinates given in parameter.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+{
+ vpMbEdgeTracker::initFaceFromCorners(polygon);
+ vpMbKltTracker::initFaceFromCorners(polygon);
+}
+/*!
+ Initialise a new face from the coordinates given in parameter.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
+{
+ vpMbEdgeTracker::initFaceFromLines(polygon);
+ vpMbKltTracker::initFaceFromLines(polygon);
+}
+
+/*!
+ Add a circle to track from its center, 3 points (including the center) defining the plane that contain
+ the circle and its radius.
+
+ \param p1 : Center of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param radius : Radius of the circle.
+ \param idFace : Id of the face associated to the circle.
+ \param name : The optional name of the circle.
+*/
+void
+vpMbEdgeKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace, const std::string &name)
+{
+ vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
+}
+
+/*!
+ Add a cylinder to track from tow points on the axis (defining the length of
+ the cylinder) and its radius.
+
+ \param p1 : First point on the axis.
+ \param p2 : Second point on the axis.
+ \param radius : Radius of the cylinder.
+ \param idFace : Id of the face associated to the cylinder.
+ \param name : The optional name of the cylinder.
+*/
+void
+vpMbEdgeKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string &name)
+{
+ vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
+ vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
+}
+
+/*!
+ Display the 3D model at a given position using the given camera parameters
+
+ \param I : The image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
+*/
+void
+vpMbEdgeKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
+ const vpColor& col, const unsigned int thickness, const bool displayFullModel)
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ break ; //displaying model on one scale only
+ }
+ }
+
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+/*!
+ Display the 3D model at a given position using the given camera parameters
+
+ \param I : The color image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
+*/
+void
+vpMbEdgeKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
+ const vpColor& col , const unsigned int thickness, const bool displayFullModel)
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ break ; //displaying model on one scale only
+ }
+ }
+
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ reInitModel(I, cad_name.c_str(), cMo_, verbose);
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ // Reinit klt
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if(cur != NULL){
+ cvReleaseImage(&cur);
+ cur = NULL;
+ }
+#endif
+
+ // delete the Klt Polygon features
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if (kltpoly!=NULL){
+ delete kltpoly ;
+ }
+ kltpoly = NULL ;
+ }
+ kltPolygons.clear();
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if (kltPolyCylinder!=NULL){
+ delete kltPolyCylinder ;
+ }
+ kltPolyCylinder = NULL ;
+ }
+ kltCylinders.clear();
+
+ // delete the structures used to display circles
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ ci = *it;
+ if (ci!=NULL){
+ delete ci ;
+ }
+ ci = NULL ;
+ }
+
+ circles_disp.clear();
+
+ firstInitialisation = true;
+
+ // Reinit edge
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1){
+ if(scales[i]){
+ for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ l = *it;
+ if (l!=NULL) delete l ;
+ l = NULL ;
+ }
+
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ cy = *it;
+ if (cy!=NULL) delete cy;
+ cy = NULL;
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ ci = *it;
+ if (ci!=NULL) delete ci;
+ ci = NULL;
+ }
+
+ lines[i].clear();
+ cylinders[i].clear();
+ circles[i].clear();
+ }
+ }
+
+ //compute_interaction=1;
+ nline = 0;
+ ncylinder = 0;
+ ncircle = 0;
+ //lambda = 1;
+ nbvisiblepolygone = 0;
+
+ // Reinit common parts
+ faces.reset();
+
+ loadModel(cad_name, verbose);
+
+ this->cMo = cMo_;
+ init(I);
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has no symbols
+void dummy_vpMbEdgeKltTracker() {};
+#endif //VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp
new file mode 100644
index 0000000..9d2ce7d
--- /dev/null
+++ b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp
@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using edges and point features).
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <map>
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbtEdgeKltXmlParser.h>
+
+
+/*!
+ Default constructor.
+
+*/
+vpMbtEdgeKltXmlParser::vpMbtEdgeKltXmlParser()
+{
+ init();
+}
+
+/*!
+ Default destructor.
+*/
+vpMbtEdgeKltXmlParser::~vpMbtEdgeKltXmlParser()
+{
+}
+
+/*!
+ Initialise internal variables (including the map).
+*/
+void
+vpMbtEdgeKltXmlParser::init()
+{
+ vpMbtXmlParser::init();
+ vpMbtKltXmlParser::init();
+
+ nodeMap["camera"] = vpMbtEdgeKltXmlParser::camera;
+ nodeMap["face"] = vpMbtEdgeKltXmlParser::face;
+ nodeMap["klt"] = vpMbtEdgeKltXmlParser::klt;
+ nodeMap["ecm"] = vpMbtEdgeKltXmlParser::ecm;
+ nodeMap["lod"] = vpMbtEdgeKltXmlParser::lod;
+}
+
+/*!
+ Parse the file in parameters.
+ This method is deprecated, use parse() instead.
+
+ \param filename : File to parse.
+*/
+void
+vpMbtEdgeKltXmlParser::parse(const char * filename)
+{
+ std::string file = filename;
+ vpXmlParser::parse(file);
+}
+
+/*!
+ Write info to file.
+
+ \warning Useless, so not yet implemented => Throw exception.
+*/
+void
+vpMbtEdgeKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
+{
+ throw vpException(vpException::notImplementedError, "Not yet implemented." );
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void
+vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool camera_node = false;
+ bool face_node = false;
+ bool ecm_node = false;
+ bool klt_node = false;
+ bool lod_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case vpMbtEdgeKltXmlParser::camera:{
+ this->read_camera (doc, dataNode);
+ camera_node = true;
+ }break;
+ case vpMbtEdgeKltXmlParser::face:{
+ this->read_face(doc, dataNode);
+ face_node = true;
+ }break;
+ case vpMbtEdgeKltXmlParser::klt:{
+ this->read_klt(doc, dataNode);
+ klt_node = true;
+ }break;
+ case vpMbtEdgeKltXmlParser::ecm:{
+ this->read_ecm (doc, dataNode);
+ ecm_node = true;
+ }break;
+ case sample:{
+ this->read_sample_deprecated (doc, dataNode);
+ }break;
+ case vpMbtEdgeKltXmlParser::lod:{
+ this->read_lod(doc, dataNode);
+ lod_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!camera_node) {
+ std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+ std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ }
+
+ if(!face_node) {
+ std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
+ std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ }
+
+ if(!klt_node) {
+ std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
+ std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
+ std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
+ std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
+ std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
+ std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
+ std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
+ std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ }
+
+ if(!ecm_node) {
+ std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
+ std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+ std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+ std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
+ std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+ std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ }
+
+ if(!lod_node) {
+ std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
+ std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtEdgeKltXmlParser.cpp.o) has no symbols
+void dummy_vpMbtEdgeKltXmlParser() {};
+#endif
+
diff --git a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp
new file mode 100644
index 0000000..ac83e75
--- /dev/null
+++ b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp
@@ -0,0 +1,1398 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Model based tracker using only KLT
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/mbt/vpMbKltTracker.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpTrackingException.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+vpMbKltTracker::vpMbKltTracker()
+ :
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cur(),
+#else
+ cur(NULL),
+#endif
+ c0Mo(), compute_interaction(true),
+ firstInitialisation(true), maskBorder(5), lambda(0.8), maxIter(200), threshold_outlier(0.5),
+ percentGood(0.6), ctTc0(), tracker(), kltPolygons(), kltCylinders(), circles_disp()
+{
+ tracker.setTrackerId(1);
+ tracker.setUseHarris(1);
+ tracker.setMaxFeatures(10000);
+ tracker.setWindowSize(5);
+ tracker.setQuality(0.01);
+ tracker.setMinDistance(5);
+ tracker.setHarrisFreeParameter(0.01);
+ tracker.setBlockSize(3);
+ tracker.setPyramidLevels(3);
+
+ angleAppears = vpMath::rad(65);
+ angleDisappears = vpMath::rad(75);
+
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Klt");
+#endif
+}
+
+/*!
+ Basic destructor.
+
+*/
+vpMbKltTracker::~vpMbKltTracker()
+{
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if(cur != NULL){
+ cvReleaseImage(&cur);
+ cur = NULL;
+ }
+#endif
+
+ // delete the Klt Polygon features
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if (kltpoly!=NULL){
+ delete kltpoly ;
+ }
+ kltpoly = NULL ;
+ }
+ kltPolygons.clear();
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if (kltPolyCylinder!=NULL){
+ delete kltPolyCylinder ;
+ }
+ kltPolyCylinder = NULL ;
+ }
+ kltCylinders.clear();
+
+ // delete the structures used to display circles
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ ci = *it;
+ if (ci!=NULL){
+ delete ci ;
+ }
+ ci = NULL ;
+ }
+
+ circles_disp.clear();
+}
+
+void
+vpMbKltTracker::init(const vpImage<unsigned char>& I)
+{
+ if(!modelInitialised){
+ throw vpException(vpException::fatalError, "model not initialized");
+ }
+
+ bool reInitialisation = false;
+ if(!useOgre)
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ else{
+#ifdef VISP_HAVE_OGRE
+ if(!faces.isOgreInitialised()){
+ faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
+ faces.setOgreShowConfigDialog(ogreShowConfigDialog);
+ faces.initOgre(cam);
+ // Turn off Ogre config dialog display for the next call to this function
+ // since settings are saved in the ogre.cfg file and used during the next
+ // call
+ ogreShowConfigDialog = false;
+ }
+
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+
+#else
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+ reinit(I);
+}
+
+void
+vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
+{
+ c0Mo = cMo;
+ ctTc0.eye();
+
+ vpImageConvert::convert(I, cur);
+
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ if(useScanLine){
+ faces.computeClippedPolygons(cMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ // mask
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
+#else
+ IplImage* mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
+ cvZero(mask);
+#endif
+
+ vpMbtDistanceKltPoints *kltpoly;
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ if(useScanLine){
+ vpImageConvert::convert(faces.getMbScanLineRenderer().getMask(), mask);
+ }
+ else{
+ unsigned char val = 255/* - i*15*/;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ kltpoly->polygon->computePolygonClipped(cam); // Might not be necessary when scanline is activated
+ kltpoly->updateMask(mask, val, maskBorder);
+ }
+ }
+
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked())
+ {
+ for(unsigned int k = 0 ; k < kltPolyCylinder->listIndicesCylinderBBox.size() ; k++)
+ {
+ int indCylBBox = kltPolyCylinder->listIndicesCylinderBBox[k];
+ if(faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2){
+ faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated
+ }
+ }
+
+ kltPolyCylinder->updateMask(mask, val, maskBorder);
+ }
+ }
+ }
+
+ tracker.initTracking(cur, mask);
+// tracker.track(cur); // AY: Not sure to be usefull but makes sure that the points are valid for tracking and avoid too fast reinitialisations.
+// vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << " points" << std::endl;
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ kltpoly->init(tracker);
+ }
+ }
+
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked())
+ kltPolyCylinder->init(tracker, cMo);
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ cvReleaseImage(&mask);
+#endif
+}
+
+/*!
+ Reset the tracker. The model is removed and the pose is set to identity.
+ The tracker needs to be initialized with a new model and a new pose.
+*/
+void
+vpMbKltTracker::resetTracker()
+{
+ cMo.eye();
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if(cur != NULL){
+ cvReleaseImage(&cur);
+ cur = NULL;
+ }
+#endif
+
+ // delete the Klt Polygon features
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if (kltpoly!=NULL){
+ delete kltpoly ;
+ }
+ kltpoly = NULL ;
+ }
+ kltPolygons.clear();
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if (kltPolyCylinder!=NULL){
+ delete kltPolyCylinder ;
+ }
+ kltPolyCylinder = NULL ;
+ }
+ kltCylinders.clear();
+
+ // delete the structures used to display circles
+ vpMbtDistanceCircle *ci;
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ ci = *it;
+ if (ci!=NULL){
+ delete ci ;
+ }
+ ci = NULL ;
+ }
+
+ circles_disp.clear();
+
+ compute_interaction = true;
+ firstInitialisation = true;
+ computeCovariance = false;
+
+ tracker.setTrackerId(1);
+ tracker.setUseHarris(1);
+
+ tracker.setMaxFeatures(10000);
+ tracker.setWindowSize(5);
+ tracker.setQuality(0.01);
+ tracker.setMinDistance(5);
+ tracker.setHarrisFreeParameter(0.01);
+ tracker.setBlockSize(3);
+ tracker.setPyramidLevels(3);
+
+ angleAppears = vpMath::rad(65);
+ angleDisappears = vpMath::rad(75);
+
+ clippingFlag = vpPolygon3D::NO_CLIPPING;
+
+ maskBorder = 5;
+ threshold_outlier = 0.5;
+ percentGood = 0.7;
+
+ lambda = 0.8;
+ maxIter = 200;
+
+ faces.reset();
+
+ m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
+
+ useScanLine = false;
+
+#ifdef VISP_HAVE_OGRE
+ useOgre = false;
+#endif
+}
+
+/*!
+ Get the current list of KLT points.
+
+ \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
+
+ \return the list of KLT points through vpKltOpencv.
+*/
+std::vector<vpImagePoint>
+vpMbKltTracker::getKltImagePoints() const
+{
+ std::vector<vpImagePoint> kltPoints;
+ for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
+ int id;
+ float x_tmp, y_tmp;
+ tracker.getFeature((int)i, id, x_tmp, y_tmp);
+ kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
+ }
+
+ return kltPoints;
+}
+
+/*!
+ Get the current list of KLT points and their id.
+
+ \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
+
+ \return the list of KLT points and their id through vpKltOpencv.
+*/
+std::map<int, vpImagePoint>
+vpMbKltTracker::getKltImagePointsWithId() const
+{
+ std::map<int, vpImagePoint> kltPoints;
+ for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
+ int id;
+ float x_tmp, y_tmp;
+ tracker.getFeature((int)i, id, x_tmp, y_tmp);
+ kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
+ }
+
+ return kltPoints;
+}
+
+/*!
+ Set the new value of the klt tracker.
+
+ \param t : Klt tracker containing the new values.
+*/
+void
+vpMbKltTracker::setKltOpencv(const vpKltOpencv& t){
+ tracker.setMaxFeatures(t.getMaxFeatures());
+ tracker.setWindowSize(t.getWindowSize());
+ tracker.setQuality(t.getQuality());
+ tracker.setMinDistance(t.getMinDistance());
+ tracker.setHarrisFreeParameter(t.getHarrisFreeParameter());
+ tracker.setBlockSize(t.getBlockSize());
+ tracker.setPyramidLevels(t.getPyramidLevels());
+}
+
+/*!
+ Set the camera parameters.
+
+ \param camera : the new camera parameters.
+*/
+void
+vpMbKltTracker::setCameraParameters(const vpCameraParameters& camera)
+{
+// for (unsigned int i = 0; i < faces.size(); i += 1){
+// faces[i]->setCameraParameters(camera);
+// }
+
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ kltpoly->setCameraParameters(camera);
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ kltPolyCylinder->setCameraParameters(camera);
+ }
+
+ this->cam = camera;
+}
+
+/*!
+ Set the pose to be used in entry (as guess) of the next call to the track() function.
+ This pose will be just used once.
+
+ \warning This functionnality is not available when tracking cylinders.
+
+ \param I : image corresponding to the desired pose.
+ \param cdMo : Pose to affect.
+*/
+void
+vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+{
+ if((int)(kltCylinders.size()) != 0)
+ {
+ std::cout << "WARNING: Cannot set pose when model contains cylinder(s). This feature is not implemented yet." << std::endl;
+ std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
+ cMo = cdMo;
+ init(I);
+ }
+ else
+ {
+ vpMbtDistanceKltPoints *kltpoly;
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ std::vector<cv::Point2f> init_pts;
+ std::vector<long> init_ids;
+ std::vector<cv::Point2f> guess_pts;
+#else
+ unsigned int nbp = 0;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
+ nbp += (*it)->getCurrentNumberPoints();
+ }
+
+ CvPoint2D32f* init_pts = NULL;
+ init_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(init_pts[0]));
+ long *init_ids = (long*)cvAlloc((unsigned int)tracker.getMaxFeatures()*sizeof(long));
+ unsigned int iter_pts = 0;
+
+ CvPoint2D32f* guess_pts = NULL;
+ guess_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(guess_pts[0]));
+#endif
+
+ vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
+ vpHomogeneousMatrix cMcd = cdMc.inverse();
+
+ vpRotationMatrix cdRc;
+ vpTranslationVector cdtc;
+
+ cdMc.extract(cdRc);
+ cdMc.extract(cdtc);
+
+ unsigned int nbCur = 0;
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
+ kltpoly = *it;
+
+ if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
+ kltpoly->polygon->changeFrame(cdMo);
+
+ //Get the normal to the face at the current state cMo
+ vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
+ plan.changeFrame(cMcd);
+
+ vpColVector Nc = plan.getNormal();
+ Nc.normalize();
+
+ double invDc = 1.0 / plan.getD();
+
+ //Create the homography
+ vpMatrix cdHc;
+ vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
+ cdHc /= cdHc[2][2];
+
+ //Create the 2D homography
+ vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
+
+ //Points displacement
+ std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
+ nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
+ for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
+ vpColVector cdp(3);
+ cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Point2f p((float)cdp[0], (float)cdp[1]);
+ init_pts.push_back(p);
+ init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
+#else
+ init_pts[iter_pts].x = (float)cdp[0];
+ init_pts[iter_pts].y = (float)cdp[1];
+ init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
+#endif
+
+ double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
+
+ if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
+ cdp[0] = 0.0;
+ cdp[1] = 0.0;
+ throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
+ }
+
+ cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
+ cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
+
+ //Set value to the KLT tracker
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
+ guess_pts.push_back(p_guess);
+#else
+ guess_pts[iter_pts].x = (float)cdp[0];
+ guess_pts[iter_pts++].y = (float)cdp[1];
+#endif
+ }
+ }
+ }
+
+ vpImageConvert::convert(I, cur);
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ tracker.setInitialGuess(init_pts, guess_pts, init_ids);
+#else
+ tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
+
+ if(init_pts) cvFree(&init_pts);
+ init_pts = NULL;
+
+ if(guess_pts) cvFree(&guess_pts);
+ guess_pts = NULL;
+
+ if(init_ids)cvFree(&init_ids);
+ init_ids = NULL;
+#endif
+
+ bool reInitialisation = false;
+ if(!useOgre)
+ faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
+ else{
+#ifdef VISP_HAVE_OGRE
+ faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
+#else
+ faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ if(useScanLine){
+ faces.computeClippedPolygons(cdMo,cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
+ kltpoly->polygon->computePolygonClipped(cam);
+ kltpoly->init(tracker);
+ }
+ }
+
+ cMo = cdMo;
+ c0Mo = cMo;
+ ctTc0.eye();
+ }
+}
+
+/*!
+ Initialise a new face from the coordinates given in parameter.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+{
+ vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
+ kltPoly->setCameraParameters(cam) ;
+ kltPoly->polygon = &polygon;
+ kltPoly->hiddenface = &faces ;
+ kltPoly->useScanLine = useScanLine;
+ kltPolygons.push_back(kltPoly);
+}
+/*!
+ Initialise a new face from the coordinates given in parameter.
+
+ \param polygon : The polygon describing the set of lines that has to be tracked.
+*/
+void
+vpMbKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
+{
+ vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
+ kltPoly->setCameraParameters(cam) ;
+ kltPoly->polygon = &polygon;
+ kltPoly->hiddenface = &faces ;
+ kltPoly->useScanLine = useScanLine;
+ kltPolygons.push_back(kltPoly);
+}
+
+/*!
+ Achieve the tracking of the KLT features and associate the features to the faces.
+
+ \param I : The input image.
+ \param nbInfos : Size of the features.
+ \param nbFaceUsed : Number of face used for the tracking.
+*/
+void
+vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
+{
+ vpImageConvert::convert(I, cur);
+ tracker.track(cur);
+
+ nbInfos = 0;
+ nbFaceUsed = 0;
+ vpMbtDistanceKltPoints *kltpoly;
+// for (unsigned int i = 0; i < faces.size(); i += 1){
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ kltpoly->computeNbDetectedCurrent(tracker);
+// faces[i]->ransac();
+ if(kltpoly->hasEnoughPoints()){
+ nbInfos += kltpoly->getCurrentNumberPoints();
+ nbFaceUsed++;
+ }
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked())
+ {
+ kltPolyCylinder->computeNbDetectedCurrent(tracker);
+ if(kltPolyCylinder->hasEnoughPoints()){
+ nbInfos += kltPolyCylinder->getCurrentNumberPoints();
+ nbFaceUsed++;
+ }
+ }
+ }
+}
+
+/*!
+ Realize the post tracking operations. Mostly visibility tests
+*/
+bool
+vpMbKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w)
+{
+ // # For a better Post Tracking, tracker should reinitialize if so faces don't have enough points but are visible.
+ // # Here we are not doing it for more speed performance.
+ bool reInitialisation = false;
+
+ unsigned int initialNumber = 0;
+ unsigned int currentNumber = 0;
+ unsigned int shift = 0;
+ vpMbtDistanceKltPoints *kltpoly;
+// for (unsigned int i = 0; i < faces.size(); i += 1){
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ initialNumber += kltpoly->getInitialNumberPoint();
+ if(kltpoly->hasEnoughPoints()){
+ vpSubColVector sub_w(w, shift, 2*kltpoly->getCurrentNumberPoints());
+ shift += 2*kltpoly->getCurrentNumberPoints();
+ kltpoly->removeOutliers(sub_w, threshold_outlier);
+
+ currentNumber += kltpoly->getCurrentNumberPoints();
+ }
+// else{
+// reInitialisation = true;
+// break;
+// }
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked())
+ {
+ initialNumber += kltPolyCylinder->getInitialNumberPoint();
+ if(kltPolyCylinder->hasEnoughPoints()){
+ vpSubColVector sub_w(w, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
+ shift += 2*kltPolyCylinder->getCurrentNumberPoints();
+ kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
+
+ currentNumber += kltPolyCylinder->getCurrentNumberPoints();
+ }
+ }
+ }
+
+// if(!reInitialisation){
+ double value = percentGood * (double)initialNumber;
+ if((double)currentNumber < value){
+// std::cout << "Too many point disappear : " << initialNumber << "/" << currentNumber << std::endl;
+ reInitialisation = true;
+ }
+ else{
+ if(!useOgre)
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ else{
+#ifdef VISP_HAVE_OGRE
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#else
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+ }
+// }
+
+ if(reInitialisation)
+ return true;
+
+ return false;
+}
+
+/*!
+ Realize the VVS loop for the tracking
+
+ \param nbInfos : Size of the features
+ \param w : weight of the features after M-Estimation.
+*/
+void
+vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
+{
+ vpMatrix L; // interaction matrix
+ vpColVector R; // residu
+ vpMatrix L_true; // interaction matrix
+ vpMatrix LVJ_true;
+ //vpColVector R_true; // residu
+ vpColVector v; // "speed" for VVS
+ vpHomography H;
+ vpColVector w_true;
+ vpRobust robust(2*nbInfos);
+
+ vpMatrix LTL;
+ vpColVector LTR;
+ vpHomogeneousMatrix cMoPrev;
+ vpHomogeneousMatrix ctTc0_Prev;
+ vpColVector m_error_prev(2*nbInfos);
+ double mu = 0.01;
+
+ double normRes = 0;
+ double normRes_1 = -1;
+ unsigned int iter = 0;
+
+ R.resize(2*nbInfos);
+ L.resize(2*nbInfos, 6, 0);
+
+ while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
+
+ unsigned int shift = 0;
+ vpMbtDistanceKltPoints *kltpoly;
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
+ kltpoly->hasEnoughPoints()){
+ vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
+ vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
+ try{
+ kltpoly->computeHomography(ctTc0, H);
+ kltpoly->computeInteractionMatrixAndResidu(subR, subL);
+ }catch(...){
+ throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
+ }
+
+ shift += 2*kltpoly->getCurrentNumberPoints();
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ {
+ vpSubColVector subR(R, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
+ vpSubMatrix subL(L, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
+ try{
+ kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0,subR, subL);
+ }catch(...){
+ throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
+ }
+
+ shift += 2*kltPolyCylinder->getCurrentNumberPoints();
+ }
+ }
+
+ bool reStartFromLastIncrement = false;
+ if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
+ if(m_error.sumSquare()/(double)(2*nbInfos) > m_error_prev.sumSquare()/(double)(2*nbInfos)){
+ mu *= 10.0;
+
+ if(mu > 1.0)
+ throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+
+ cMo = cMoPrev;
+ m_error = m_error_prev;
+ ctTc0 = ctTc0_Prev;
+ reStartFromLastIncrement = true;
+ }
+ }
+
+ if(!reStartFromLastIncrement){
+ if(iter == 0){
+ w_true.resize(2*nbInfos);
+ w.resize(2*nbInfos);
+ w = 1;
+ w_true = 1;
+ }
+ robust.setIteration(iter);
+ robust.setThreshold(2/cam.get_px());
+ robust.MEstimator( vpRobust::TUKEY, R, w);
+
+ m_error = R;
+ if(computeCovariance){
+ L_true = L;
+ if(!isoJoIdentity){
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (L*cVo*oJo);
+ }
+ }
+
+ normRes_1 = normRes;
+ normRes = 0;
+ for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
+ w_true[i] = w[i];
+ R[i] = R[i] * w[i];
+ normRes += R[i];
+ }
+
+ if((iter == 0) || compute_interaction){
+ for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
+ for(unsigned int j=0; j<6; j++){
+ L[i][j] *= w[i];
+ }
+ }
+ }
+
+ if(isoJoIdentity){
+ LTL = L.AtA();
+ computeJTR(L, R, LTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LTL.getRows(), LTL.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LTL + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
+ }
+ }
+ else{
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ vpMatrix LVJ = (L*cVo*oJo);
+ vpMatrix LVJTLVJ = (LVJ).AtA();
+ vpColVector LVJTR;
+ computeJTR(LVJ, R, LVJTR);
+
+ switch(m_optimizationMethod){
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
+ {
+ vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
+ v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+
+ if(iter != 0)
+ mu /= 10.0;
+
+ m_error_prev = m_error;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ {
+ v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
+ v = cVo * v;
+ break;
+ }
+ }
+ }
+
+ cMoPrev = cMo;
+ ctTc0_Prev = ctTc0;
+ ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
+ cMo = ctTc0 * c0Mo;
+ } // endif(!restartFromLast)
+
+ iter++;
+ }
+
+ if(computeCovariance){
+ vpMatrix D;
+ D.diag(w_true);
+
+ // Note that here the covariance is computed on cMoPrev for time computation efficiency
+ if(isoJoIdentity){
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
+ }
+ else{
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
+ }
+ }
+}
+
+/*!
+ Realize the tracking of the object in the image
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param I : the input image
+*/
+void
+vpMbKltTracker::track(const vpImage<unsigned char>& I)
+{
+ unsigned int nbInfos = 0;
+ unsigned int nbFaceUsed = 0;
+
+ try{
+ preTracking(I, nbInfos, nbFaceUsed);
+ }
+ catch(vpException &e){
+ throw e;
+ }
+
+ if(nbInfos < 4 || nbFaceUsed == 0){
+ vpERROR_TRACE("\n\t\t Error-> not enough data") ;
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
+ }
+
+ //vpColVector w;
+ computeVVS(nbInfos, m_w);
+
+ if(postTracking(I, m_w))
+ reinit(I);
+}
+
+/*!
+ Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
+ From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \param configFile : full name of the xml file.
+
+ \sa loadConfigFile(const char*), vpXmlParser::cleanup()
+*/
+void
+vpMbKltTracker::loadConfigFile(const std::string& configFile)
+{
+ vpMbKltTracker::loadConfigFile(configFile.c_str());
+}
+
+/*!
+ Load the xml configuration file.
+ From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+
+ \warning To clean up memory allocated by the xml library, the user has to call
+ vpXmlParser::cleanup() before the exit().
+
+ \throw vpException::ioError if the file has not been properly parsed (file not
+ found or wrong format for the data).
+
+ \param configFile : full name of the xml file.
+
+ The XML configuration file has the following form:
+ \code
+<?xml version="1.0"?>
+<conf>
+ <camera>
+ <width>640</width>
+ <height>480</height>
+ <u0>320</u0>
+ <v0>240</v0>
+ <px>686.24</px>
+ <py>686.24</py>
+ </camera>
+ <face>
+ <angle_appear>65</angle_appear>
+ <angle_disappear>85</angle_disappear>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>0.90</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+ <klt>
+ <mask_border>10</mask_border>
+ <max_features>10000</max_features>
+ <window_size>5</window_size>
+ <quality>0.02</quality>
+ <min_distance>10</min_distance>
+ <harris>0.02</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+</conf>
+ \endcode
+
+ \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
+*/
+void
+vpMbKltTracker::loadConfigFile(const char* configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtKltXmlParser xmlp;
+
+ xmlp.setMaxFeatures(10000);
+ xmlp.setWindowSize(5);
+ xmlp.setQuality(0.01);
+ xmlp.setMinDistance(5);
+ xmlp.setHarrisParam(0.01);
+ xmlp.setBlockSize(3);
+ xmlp.setPyramidLevels(3);
+ xmlp.setMaskBorder(maskBorder);
+ xmlp.setAngleAppear(vpMath::deg(angleAppears));
+ xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
+
+ try{
+ std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
+ xmlp.parse(configFile);
+ }
+ catch(...){
+ vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
+ throw vpException(vpException::ioError, "problem to parse configuration file.");
+ }
+
+ vpCameraParameters camera;
+ xmlp.getCameraParameters(camera);
+ setCameraParameters(camera);
+
+ tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
+ tracker.setWindowSize((int)xmlp.getWindowSize());
+ tracker.setQuality(xmlp.getQuality());
+ tracker.setMinDistance(xmlp.getMinDistance());
+ tracker.setHarrisFreeParameter(xmlp.getHarrisParam());
+ tracker.setBlockSize((int)xmlp.getBlockSize());
+ tracker.setPyramidLevels((int)xmlp.getPyramidLevels());
+ maskBorder = xmlp.getMaskBorder();
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ //if(useScanLine)
+ faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
+
+ if(xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if(xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if(xmlp.getFovClipping())
+ setClipping(clippingFlag = clippingFlag | vpPolygon3D::FOV_CLIPPING);
+
+ useLodGeneral = xmlp.getLodState();
+ minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold();
+ minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
+
+ applyLodSettingInConfig = false;
+ if(this->getNbPolygon() > 0) {
+ applyLodSettingInConfig = true;
+ setLod(useLodGeneral);
+ setMinLineLengthThresh(minLineLengthThresholdGeneral);
+ setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+ }
+
+#else
+ vpTRACE("You need the libXML2 to read the config file %s", configFile);
+#endif
+}
+
+/*!
+ Display the 3D model at a given position using the given camera parameters
+
+ \param I : The image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are visible.
+*/
+void
+vpMbKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
+ const vpColor& col, const unsigned int thickness, const bool displayFullModel)
+{
+ vpCameraParameters c = camera;
+
+ if(clippingFlag > 3) // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ vpMbtDistanceKltPoints *kltpoly;
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+
+ // Previous version 12/08/2015
+// for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+// kltpoly = *it;
+// kltpoly->polygon->changeFrame(cMo_);
+// kltpoly->polygon->computePolygonClipped(c);
+// }
+ faces.computeClippedPolygons(cMo_,c);
+
+ if(useScanLine && !displayFullModel)
+ faces.computeScanLineRender(cam,I.getWidth(), I.getHeight());
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+
+ kltpoly->display(I,cMo_,camera,col,thickness,displayFullModel);
+
+ if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
+ kltpoly->displayPrimitive(I);
+// faces[i]->displayNormal(I);
+ }
+ }
+
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ kltPolyCylinder->display(I,cMo_,camera,col,thickness,displayFullModel);
+
+ if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+/*!
+ Display the 3D model at a given position using the given camera parameters
+
+ \param I : The color image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param camera : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are not visible.
+*/
+void
+vpMbKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
+ const vpColor& col , const unsigned int thickness, const bool displayFullModel)
+{
+ vpCameraParameters c = camera;
+
+ if(clippingFlag > 3) // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ vpMbtDistanceKltPoints *kltpoly;
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+
+ // Previous version 12/08/2015
+// for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+// kltpoly = *it;
+// kltpoly->polygon->changeFrame(cMo_);
+// kltpoly->polygon->computePolygonClipped(c);
+// }
+ faces.computeClippedPolygons(cMo_,c);
+
+ if(useScanLine && !displayFullModel)
+ faces.computeScanLineRender(cam,I.getWidth(), I.getHeight());
+
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+
+ kltpoly->display(I,cMo_,camera,col,thickness,displayFullModel);
+
+ if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
+ kltpoly->displayPrimitive(I);
+// faces[i]->displayNormal(I);
+ }
+ }
+
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+
+ kltPolyCylinder->display(I,cMo_,camera,col,thickness,displayFullModel);
+
+ if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+
+ for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ (*it)->display(I, cMo_, camera, col, thickness);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if(useOgre)
+ faces.displayOgre(cMo_);
+#endif
+}
+
+/*!
+ Test the quality of the tracking.
+ The tracking is supposed to fail if less than 10 points are tracked.
+
+ \todo Find a efficient way to test the quality.
+
+ \throw vpTrackingException::fatalError if the test fails.
+*/
+void
+vpMbKltTracker::testTracking()
+{
+ unsigned int nbTotalPoints = 0;
+ vpMbtDistanceKltPoints *kltpoly;
+// for (unsigned int i = 0; i < faces.size(); i += 1){
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()){
+ nbTotalPoints += kltpoly->getCurrentNumberPoints();
+ }
+ }
+
+ vpMbtDistanceKltCylinder *kltPolyCylinder;
+ for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ kltPolyCylinder = *it;
+ if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
+ }
+
+ if(nbTotalPoints < 10){
+ std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
+ throw vpTrackingException(vpTrackingException::fatalError,
+ "test tracking failed (too few points to realize a good tracking).");
+ }
+}
+
+/*!
+ Add a cylinder to display (not for tracking) from two points on the axis (defining the length of
+ the cylinder) and its radius.
+
+ \param p1 : First point on the axis.
+ \param p2 : Second point on the axis.
+ \param radius : Radius of the cylinder.
+ \param idFace : Identifier of the polygon representing the revolution axis of the cylinder.
+ \param name : The optional name of the cylinder.
+*/
+void
+vpMbKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string &/*name*/)
+{
+ vpMbtDistanceKltCylinder *kltPoly = new vpMbtDistanceKltCylinder();
+ kltPoly->setCameraParameters(cam) ;
+
+ kltPoly->buildFrom(p1,p2,radius);
+
+ // Add the Cylinder BBox to the list of polygons
+ kltPoly->listIndicesCylinderBBox.push_back(idFace+1);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace+2);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace+3);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace+4);
+
+ kltPoly->hiddenface = &faces ;
+ kltPoly->useScanLine = useScanLine;
+ kltCylinders.push_back(kltPoly);
+}
+
+/*!
+ Add a circle to display (not for tracking) from its center, 3 points (including the center) defining the plane that contain
+ the circle and its radius.
+
+ \param p1 : Center of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param radius : Radius of the circle.
+ \param name : The optional name of the circle.
+*/
+void
+vpMbKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int /*idFace*/, const std::string &name)
+{
+ addCircle(p1, p2, p3, radius, name);
+}
+
+/*!
+ Add a circle to the list of circles.
+
+ \param P1 : Center of the circle.
+ \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
+ defining the plane that contains the circle.
+ \param r : Radius of the circle.
+ \param name : Name of the circle.
+*/
+void
+vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name)
+{
+ bool already_here = false ;
+ vpMbtDistanceCircle *ci ;
+
+// for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
+// ci = *it;
+// if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
+// (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
+// already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
+// }
+// }
+
+ if (!already_here){
+ ci = new vpMbtDistanceCircle ;
+
+ ci->setCameraParameters(cam);
+ ci->setName(name);
+ ci->buildFrom(P1, P2, P3, r);
+ circles_disp.push_back(ci);
+ }
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ reInitModel(I, cad_name.c_str(), cMo_, verbose);
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
+ const vpHomogeneousMatrix& cMo_, const bool verbose)
+{
+ this->cMo.eye();
+
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if(cur != NULL){
+ cvReleaseImage(&cur);
+ cur = NULL;
+ }
+#endif
+
+ firstInitialisation = true;
+
+ faces.reset();
+
+ loadModel(cad_name, verbose);
+ initFromPose(I, cMo_);
+}
+
+/*!
+ Set if the polygons that have the given name have to be considered during the tracking phase.
+
+ \param name : name of the polygon(s).
+ \param useKltTracking : True if it has to be considered, False otherwise.
+*/
+void
+vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
+{
+ vpMbtDistanceKltPoints *kltpoly;
+ for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ kltpoly = *it;
+ if(kltpoly->polygon->getName() == name){
+ kltpoly->setTracked(useKltTracking);
+ }
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no symbols
+void dummy_vpMbKltTracker() {};
+#endif //VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp
new file mode 100644
index 0000000..048f253
--- /dev/null
+++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp
@@ -0,0 +1,623 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Klt cylinder, containing points of interest.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
+#include <visp3/mbt/vpMbtDistanceKltPoints.h>
+#include <visp3/core/vpPolygon.h>
+
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+/*!
+ Basic constructor.
+
+*/
+vpMbtDistanceKltCylinder::vpMbtDistanceKltCylinder()
+ : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(),
+ initPoints(), initPoints3D(), curPoints(), curPointsInd(),
+ nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false),
+ cam(), isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
+{
+}
+
+/*!
+ Basic destructor.
+
+*/
+vpMbtDistanceKltCylinder::~vpMbtDistanceKltCylinder()
+{}
+
+
+
+void
+vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
+{
+ p1Ext = p1;
+ p2Ext = p2;
+
+ vpColVector ABC(3);
+ vpColVector V1(3);
+ vpColVector V2(3);
+
+ V1[0] = p1.get_oX();
+ V1[1] = p1.get_oY();
+ V1[2] = p1.get_oZ();
+ V2[0] = p2.get_oX();
+ V2[1] = p2.get_oY();
+ V2[2] = p2.get_oZ();
+
+ // Get the axis of the cylinder
+ ABC = V1-V2;
+
+ // Build our extremity circles
+ circle1.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p1.get_oX(),p1.get_oY(),p1.get_oZ(),r);
+ circle2.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p2.get_oX(),p2.get_oY(),p2.get_oZ(),r);
+
+ // Build our cylinder
+ cylinder.setWorldCoordinates(ABC[0],ABC[1],ABC[2],(p1.get_oX()+p2.get_oX())/2.0,(p1.get_oY()+p2.get_oY())/2.0,(p1.get_oZ()+p2.get_oZ())/2.0,r);
+}
+
+/*!
+ Initialise the cylinder to track. All the points in the map, representing all the
+ map detected in the image, are parsed in order to extract the id of the points
+ that are indeed in the face.
+
+ \param _tracker : ViSP OpenCV KLT Tracker.
+ \param cMo : Pose of the object in the camera frame at initialization.
+*/
+void
+vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousMatrix &cMo)
+{
+ c0Mo = cMo;
+ cylinder.changeFrame(cMo);
+
+ // extract ids of the points in the face
+ nbPointsInit = 0;
+ nbPointsCur = 0;
+ initPoints = std::map<int, vpImagePoint>();
+ initPoints3D = std::map<int, vpPoint>();
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
+ int id;
+ float x_tmp, y_tmp;
+ _tracker.getFeature((int)i, id, x_tmp, y_tmp);
+
+ bool add = false;
+
+ if(useScanLine)
+ {
+ if((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
+ (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth())
+ {
+ for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
+ if(hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == listIndicesCylinderBBox[kc])
+ {
+ add = true;
+ break;
+ }
+ }
+ }
+ else
+ {
+ std::vector<vpImagePoint> roi;
+ for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
+ {
+ hiddenface->getPolygon()[listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
+ if(vpPolygon::isInside(roi, y_tmp, x_tmp))
+ {
+ add = true;
+ break;
+ }
+ roi.clear();
+ }
+ }
+
+ if(add){
+
+ double xm=0, ym=0;
+ vpPixelMeterConversion::convertPoint(cam, x_tmp, y_tmp, xm, ym);
+ double Z = computeZ(xm,ym);
+ if(!vpMath::isNaN(Z)){
+ initPoints[id] = vpImagePoint(y_tmp, x_tmp);
+ curPoints[id] = vpImagePoint(y_tmp, x_tmp);
+ curPointsInd[id] = (int)i;
+ nbPointsInit++;
+ nbPointsCur++;
+
+
+ vpPoint p;
+ p.setWorldCoordinates(xm * Z, ym * Z, Z);
+ initPoints3D[id] = p;
+ //std::cout << "Computed Z for : " << xm << "," << ym << " : " << computeZ(xm,ym) << std::endl;
+ }
+ }
+ }
+
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+
+ //std::cout << "Nb detected points in cylinder : " << nbPointsCur << std::endl;
+}
+
+/*!
+ compute the number of point in this instanciation of the tracker that corresponds
+ to the points of the cylinder
+
+ \param _tracker : the KLT tracker
+ \return the number of points that are tracked in this face and in this instanciation of the tracker
+*/
+unsigned int
+vpMbtDistanceKltCylinder::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
+{
+ int id;
+ float x, y;
+ nbPointsCur = 0;
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
+ _tracker.getFeature((int)i, id, x, y);
+ if(isTrackedFeature(id)){
+ curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPointsInd[id] = (int)i;
+ nbPointsCur++;
+ }
+ }
+
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+
+ return nbPointsCur;
+}
+
+/*!
+ This method removes the outliers. A point is considered as outlier when its
+ associated weight is below a given threshold (threshold_outlier).
+
+ \param _w : Vector containing the weight of all the tracked points.
+ \param threshold_outlier : Threshold to specify wether or not a point has to be deleted.
+*/
+void
+vpMbtDistanceKltCylinder::removeOutliers(const vpColVector& _w, const double &threshold_outlier)
+{
+ std::map<int, vpImagePoint> tmp;
+ std::map<int, int> tmp2;
+ unsigned int nbSupp = 0;
+ unsigned int k = 0;
+
+ nbPointsCur = 0;
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
+// if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
+ tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
+ tmp2[iter->first] = curPointsInd[iter->first];
+ nbPointsCur++;
+ }
+ else{
+ nbSupp++;
+ initPoints.erase(iter->first);
+ }
+
+ k+=2;
+ }
+
+ if(nbSupp != 0){
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+
+ curPoints = tmp;
+ curPointsInd = tmp2;
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+ }
+}
+
+/*!
+ Compute the interaction matrix and the residu vector for the face.
+ The method assumes that these two objects are properly sized in order to be
+ able to improve the speed with the use of SubCoVector and subMatrix.
+
+ \warning The function preCompute must be called before the this method.
+
+ \param _cMc0 : camera displacement since initialisation
+ \param _R : the residu vector
+ \param _J : the interaction matrix
+*/
+void
+vpMbtDistanceKltCylinder::computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &_cMc0, vpColVector& _R, vpMatrix& _J)
+{
+ unsigned int index_ = 0;
+
+ cylinder.changeFrame(_cMc0 * c0Mo);
+
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
+
+ double x_cur(0), y_cur(0);
+ vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
+
+ vpPoint p0 = initPoints3D[id];
+ p0.changeFrame(_cMc0);
+ p0.project();
+
+ double x0_transform(p0.get_x()), y0_transform(p0.get_y()) ;
+
+ double Z = computeZ(x_cur, y_cur);
+
+ if(vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()){
+// std::cout << "Z is Nan : " << A << " , " << B << " , " << C << " | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
+// std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C << std::endl;
+
+ _J[2*index_][0] = 0;
+ _J[2*index_][1] = 0;
+ _J[2*index_][2] = 0;
+ _J[2*index_][3] = 0;
+ _J[2*index_][4] = 0;
+ _J[2*index_][5] = 0;
+
+ _J[2*index_+1][0] = 0;
+ _J[2*index_+1][1] = 0;
+ _J[2*index_+1][2] = 0;
+ _J[2*index_+1][3] = 0;
+ _J[2*index_+1][4] = 0;
+ _J[2*index_+1][5] = 0;
+
+ _R[2*index_] = (x0_transform - x_cur);
+ _R[2*index_+1] = (y0_transform - y_cur);
+ index_++;
+ }
+ else
+ {
+ double invZ = 1.0/Z;
+
+ _J[2*index_][0] = - invZ;
+ _J[2*index_][1] = 0;
+ _J[2*index_][2] = x_cur * invZ;
+ _J[2*index_][3] = x_cur * y_cur;
+ _J[2*index_][4] = -(1+x_cur*x_cur);
+ _J[2*index_][5] = y_cur;
+
+ _J[2*index_+1][0] = 0;
+ _J[2*index_+1][1] = - invZ;
+ _J[2*index_+1][2] = y_cur * invZ;
+ _J[2*index_+1][3] = (1+y_cur*y_cur);
+ _J[2*index_+1][4] = - y_cur * x_cur;
+ _J[2*index_+1][5] = - x_cur;
+
+ _R[2*index_] = (x0_transform - x_cur);
+ _R[2*index_+1] = (y0_transform - y_cur);
+ index_++;
+ }
+ }
+}
+
+/*!
+ Test whether the feature with identifier id in paramters is in the list of tracked
+ features.
+
+ \param _id : the id of the current feature to test
+ \return true if the id is in the list of tracked feature
+*/
+bool
+vpMbtDistanceKltCylinder::isTrackedFeature(const int _id)
+{
+ std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
+ if(iter != initPoints.end())
+ return true;
+
+ return false;
+}
+
+/*!
+ Modification of all the pixels that are in the roi to the value of _nb (
+ default is 255).
+
+ \param mask : the mask to update (0, not in the object, _nb otherwise).
+ \param nb : Optionnal value to set to the pixels included in the face.
+ \param shiftBorder : Optionnal shift for the border in pixel (sort of built-in erosion) to avoid to consider pixels near the limits of the face.
+*/
+void
+vpMbtDistanceKltCylinder::updateMask(
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat &mask,
+#else
+ IplImage* mask,
+#endif
+ unsigned char nb, unsigned int shiftBorder)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ int width = mask.cols;
+ int height = mask.rows;
+#else
+ int width = mask->width;
+ int height = mask->height;
+#endif
+
+ for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
+ {
+ if((*hiddenface)[listIndicesCylinderBBox[kc]]->isVisible() && (*hiddenface)[listIndicesCylinderBBox[kc]]->getNbPoint() > 2)
+ {
+ int i_min, i_max, j_min, j_max;
+ std::vector<vpImagePoint> roi;
+ (*hiddenface)[listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
+ vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);
+
+ /* check image boundaries */
+ if(i_min > height){ //underflow
+ i_min = 0;
+ }
+ if(i_max > height){
+ i_max = height;
+ }
+ if(j_min > width){ //underflow
+ j_min = 0;
+ }
+ if(j_max > width){
+ j_max = width;
+ }
+
+ double shiftBorder_d = (double) shiftBorder;
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ for(int i=i_min; i< i_max; i++){
+ double i_d = (double) i;
+ for(int j=j_min; j< j_max; j++){
+ double j_d = (double) j;
+ if(shiftBorder != 0){
+ if( vpPolygon::isInside(roi, i_d, j_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
+ mask.at<unsigned char>(i,j) = nb;
+ }
+ }
+ else{
+ if(vpPolygon::isInside(roi, i, j)){
+ mask.at<unsigned char>(i,j) = nb;
+ }
+ }
+ }
+ }
+ #else
+ unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
+ for(int i=i_min; i< i_max; i++){
+ double i_d = (double) i;
+ for(int j=j_min; j< j_max; j++){
+ double j_d = (double) j;
+ if(shiftBorder != 0){
+ if( vpPolygon::isInside(roi, i_d, j_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
+ *(ptrData++) = nb;
+ }
+ else{
+ ptrData++;
+ }
+ }
+ else{
+ if(vpPolygon::isInside(roi, i, j)){
+ *(ptrData++) = nb;
+ }
+ else{
+ ptrData++;
+ }
+ }
+ }
+ ptrData += mask->widthStep - j_max + j_min;
+ }
+ #endif
+ }
+ }
+}
+
+/*!
+ Display the primitives tracked for the cylinder.
+
+ \param _I : The image where to display.
+*/
+void
+vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<unsigned char>& _I)
+{
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ vpImagePoint iP;
+ iP.set_i(static_cast<double>(iter->second.get_i()));
+ iP.set_j(static_cast<double>(iter->second.get_j()));
+
+ vpDisplay::displayCross(_I, iP, 10, vpColor::red);
+
+ iP.set_i( vpMath::round( iP.get_i() + 7 ) );
+ iP.set_j( vpMath::round( iP.get_j() + 7 ) );
+ char ide[10];
+ sprintf(ide, "%ld", static_cast<long int>(id));
+ vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ }
+}
+
+/*!
+ Display the primitives tracked for the cylinder.
+
+ \param _I : The image where to display.
+*/
+void
+vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<vpRGBa>& _I)
+{
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ vpImagePoint iP;
+ iP.set_i(static_cast<double>(iter->second.get_i()));
+ iP.set_j(static_cast<double>(iter->second.get_j()));
+
+ vpDisplay::displayCross(_I, iP, 10, vpColor::red);
+
+ iP.set_i( vpMath::round( iP.get_i() + 7 ) );
+ iP.set_j( vpMath::round( iP.get_j() + 7 ) );
+ char ide[10];
+ sprintf(ide, "%ld", static_cast<long int>(id));
+ vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ }
+}
+
+void
+vpMbtDistanceKltCylinder::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera,
+ const vpColor col, const unsigned int thickness, const bool /*displayFullModel*/)
+{
+ //if(isvisible || displayFullModel)
+ {
+ // Perspective projection
+ circle1.changeFrame(cMo);
+ circle2.changeFrame(cMo);
+ cylinder.changeFrame(cMo);
+
+ try{
+ circle1.projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 1";}
+ try{
+ circle2.projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 2";}
+
+ cylinder.projection();
+
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Meters to pixels conversion
+ vpMeterPixelConversion::convertLine(camera,cylinder.getRho1(),cylinder.getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(camera,cylinder.getRho2(),cylinder.getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+
+ vpCircle::computeIntersectionPoint(circle1, cam, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(circle2, cam, rho1, theta1, i12, j12);
+
+ vpCircle::computeIntersectionPoint(circle1, cam, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(circle2, cam, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // Display
+ vpDisplay::displayLine(I,ip11,ip12,col, thickness);
+ vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ }
+}
+
+void
+vpMbtDistanceKltCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera,
+ const vpColor col, const unsigned int thickness, const bool /*displayFullModel*/)
+{
+ //if(isvisible || displayFullModel)
+ {
+ // Perspective projection
+ circle1.changeFrame(cMo);
+ circle2.changeFrame(cMo);
+ cylinder.changeFrame(cMo);
+
+ try{
+ circle1.projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 1";}
+ try{
+ circle2.projection();
+ }
+ catch(...){std::cout<<"Problem projection circle 2";}
+
+ cylinder.projection();
+
+ double rho1,theta1;
+ double rho2,theta2;
+
+ // Meters to pixels conversion
+ vpMeterPixelConversion::convertLine(camera,cylinder.getRho1(),cylinder.getTheta1(),rho1,theta1);
+ vpMeterPixelConversion::convertLine(camera,cylinder.getRho2(),cylinder.getTheta2(),rho2,theta2);
+
+ // Determine intersections between circles and limbos
+ double i11,i12,i21,i22,j11,j12,j21,j22;
+
+ vpCircle::computeIntersectionPoint(circle1, camera, rho1, theta1, i11, j11);
+ vpCircle::computeIntersectionPoint(circle2, camera, rho1, theta1, i12, j12);
+
+ vpCircle::computeIntersectionPoint(circle1, camera, rho2, theta2, i21, j21);
+ vpCircle::computeIntersectionPoint(circle2, camera, rho2, theta2, i22, j22);
+
+ // Create the image points
+ vpImagePoint ip11,ip12,ip21,ip22;
+ ip11.set_ij(i11,j11);
+ ip12.set_ij(i12,j12);
+ ip21.set_ij(i21,j21);
+ ip22.set_ij(i22,j22);
+
+ // Display
+ vpDisplay::displayLine(I,ip11,ip12,col, thickness);
+ vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ }
+}
+
+
+// ######################
+// Private Functions
+// ######################
+
+double
+vpMbtDistanceKltCylinder::computeZ(const double &x, const double &y)
+{
+ double A = x*x + y*y + 1 - ((cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()) * (cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()));
+ double B = (x * cylinder.getX() + y * cylinder.getY() + cylinder.getZ());
+ double C = cylinder.getX() * cylinder.getX() + cylinder.getY() * cylinder.getY() + cylinder.getZ() * cylinder.getZ() - cylinder.getR() * cylinder.getR();
+
+ return (B - std::sqrt(B*B - A*C))/A;
+}
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtDistanceKltCylinder.cpp.o) has no symbols
+void dummy_vpMbtDistanceKltCylinder() {};
+#endif
diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp
new file mode 100644
index 0000000..52f3965
--- /dev/null
+++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp
@@ -0,0 +1,591 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Klt polygon, containing points of interest.
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/mbt/vpMbtDistanceKltPoints.h>
+#include <visp3/core/vpPolygon.h>
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+/*!
+ Basic constructor.
+
+*/
+vpMbtDistanceKltPoints::vpMbtDistanceKltPoints()
+ : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
+ nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
+ cam(), isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL), useScanLine(false)
+{
+ initPoints = std::map<int, vpImagePoint>();
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+}
+
+/*!
+ Basic destructor.
+
+*/
+vpMbtDistanceKltPoints::~vpMbtDistanceKltPoints()
+{}
+
+/*!
+ Initialise the face to track. All the points in the map, representing all the
+ map detected in the image, are parsed in order to extract the id of the points
+ that are indeed in the face.
+
+ \param _tracker : ViSP OpenCV KLT Tracker.
+*/
+void
+vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
+{
+ // extract ids of the points in the face
+ nbPointsInit = 0;
+ nbPointsCur = 0;
+ initPoints = std::map<int, vpImagePoint>();
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+ std::vector<vpImagePoint> roi;
+ polygon->getRoiClipped(cam, roi);
+
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
+ int id;
+ float x_tmp, y_tmp;
+ _tracker.getFeature((int)i, id, x_tmp, y_tmp);
+
+ bool add = false;
+
+ if(useScanLine)
+ {
+ if((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
+ (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
+ hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == polygon->getIndex())
+ add = true;
+ }
+ else if(vpPolygon::isInside(roi, y_tmp, x_tmp))
+ {
+ add = true;
+ }
+
+ if(add){
+ initPoints[id] = vpImagePoint(y_tmp, x_tmp);
+ curPoints[id] = vpImagePoint(y_tmp, x_tmp);
+ curPointsInd[id] = (int)i;
+ nbPointsInit++;
+ nbPointsCur++;
+ }
+ }
+
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+
+ // initialisation of the value for the computation in SE3
+ vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), polygon->getPoint(2));
+
+ d0 = plan.getD();
+ N = plan.getNormal();
+
+ N.normalize();
+ N_cur = N;
+ invd0 = 1.0 / d0;
+}
+
+/*!
+ compute the number of point in this instanciation of the tracker that corresponds
+ to the points of the face
+
+ \param _tracker : the KLT tracker
+ \return the number of points that are tracked in this face and in this instanciation of the tracker
+*/
+unsigned int
+vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
+{
+ int id;
+ float x, y;
+ nbPointsCur = 0;
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
+ _tracker.getFeature((int)i, id, x, y);
+ if(isTrackedFeature(id)){
+ curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPointsInd[id] = (int)i;
+ nbPointsCur++;
+ }
+ }
+
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+
+ return nbPointsCur;
+}
+
+/*!
+ Compute the interaction matrix and the residu vector for the face.
+ The method assumes that these two objects are properly sized in order to be
+ able to improve the speed with the use of SubCoVector and subMatrix.
+
+ \warning The function preCompute must be called before the this method.
+
+ \param _R : the residu vector
+ \param _J : the interaction matrix
+*/
+void
+vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J)
+{
+ unsigned int index_ = 0;
+
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
+
+ double x_cur(0), y_cur(0);
+ vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
+
+ vpImagePoint iP0 = initPoints[id];
+ double x0(0), y0(0);
+ vpPixelMeterConversion::convertPoint(cam, iP0, x0, y0);
+
+ double x0_transform, y0_transform ;// equivalent x and y in the first image (reference)
+ computeP_mu_t(x0, y0, x0_transform, y0_transform, H );
+
+ double invZ = compute_1_over_Z(x_cur, y_cur);
+
+ _J[2*index_][0] = - invZ;
+ _J[2*index_][1] = 0;
+ _J[2*index_][2] = x_cur * invZ;
+ _J[2*index_][3] = x_cur * y_cur;
+ _J[2*index_][4] = -(1+x_cur*x_cur);
+ _J[2*index_][5] = y_cur;
+
+ _J[2*index_+1][0] = 0;
+ _J[2*index_+1][1] = - invZ;
+ _J[2*index_+1][2] = y_cur * invZ;
+ _J[2*index_+1][3] = (1+y_cur*y_cur);
+ _J[2*index_+1][4] = - y_cur * x_cur;
+ _J[2*index_+1][5] = - x_cur;
+
+ _R[2*index_] = (x0_transform - x_cur);
+ _R[2*index_+1] = (y0_transform - y_cur);
+ index_++;
+ }
+}
+
+double
+vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
+{
+ double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
+ double den = -(d0 - dt);
+ return num/den;
+}
+
+/*!
+ Compute the new coordinates of a point given by its \f$(x,y)\f$ coordinates
+ after the homography.
+
+ \f$ P_t = {}^{c_t}H_{c_0} . P_0 \f$
+
+ \param x_in : the x coordinates of the input point
+ \param y_in : the y coordinates of the input point
+ \param x_out : the x coordinates of the output point
+ \param y_out : the y coordinates of the output point
+ \param _cHc0 : the homography used to transfer the point
+*/
+inline void
+vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& _cHc0)
+{
+ double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];
+
+ if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
+ x_out = 0.0;
+ y_out = 0.0;
+ throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
+ }
+
+ x_out = (x_in * _cHc0[0][0] + y_in * _cHc0[0][1] + _cHc0[0][2]) / p_mu_t_2;
+ y_out = (x_in * _cHc0[1][0] + y_in * _cHc0[1][1] + _cHc0[1][2]) / p_mu_t_2;
+}
+
+/*!
+ compute the homography using a displacement matrix.
+
+ the homography is given by:
+
+ \f$ {}^cH_{c_0} = {}^cR_{c_0} + \frac{{}^cT_{c_0} . {}^tN}{d_0} \f$
+
+ Several internal variables are computed (dt, cRc0_0n)
+
+ \param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
+ \param _cHc0 : the homography of the plane
+*/
+void
+vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
+{
+ vpRotationMatrix cRc0;
+ vpTranslationVector ctransc0;
+
+ _cTc0.extract(cRc0);
+ _cTc0.extract(ctransc0);
+ vpMatrix cHc0 = _cHc0.convert();
+
+// vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
+ vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
+ cHc0 /= cHc0[2][2];
+
+ H = cHc0;
+
+// vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
+// vpQuaternionVector RotQuat(cRc0);
+// vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
+// vpQuaternionVector partial = RotQuat * NQuat;
+// vpQuaternionVector resQuat = (partial * RotQuatConj);
+//
+// cRc0_0n = vpColVector(3);
+// cRc0_0n[0] = resQuat.x();
+// cRc0_0n[1] = resQuat.y();
+// cRc0_0n[2] = resQuat.z();
+
+ cRc0_0n = cRc0*N;
+
+// vpPlane p(corners[0], corners[1], corners[2]);
+// vpColVector Ncur = p.getNormal();
+// Ncur.normalize();
+ N_cur = cRc0_0n;
+ dt = 0.0;
+ for (unsigned int i = 0; i < 3; i += 1){
+ dt += ctransc0[i] * (N_cur[i]);
+ }
+}
+
+/*!
+ Test whether the feature with identifier id in paramters is in the list of tracked
+ features.
+
+ \param _id : the id of the current feature to test
+ \return true if the id is in the list of tracked feature
+*/
+bool
+vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
+{
+// std::map<int, vpImagePoint>::const_iterator iter = initPoints.begin();
+// while(iter != initPoints.end()){
+// if(iter->first == _id){
+// return true;
+// }
+// iter++;
+// }
+
+ std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
+ if(iter != initPoints.end())
+ return true;
+
+ return false;
+}
+
+/*!
+ Modification of all the pixels that are in the roi to the value of _nb (
+ default is 255).
+
+ \param mask : the mask to update (0, not in the object, _nb otherwise).
+ \param nb : Optionnal value to set to the pixels included in the face.
+ \param shiftBorder : Optionnal shift for the border in pixel (sort of built-in erosion) to avoid to consider pixels near the limits of the face.
+*/
+void
+vpMbtDistanceKltPoints::updateMask(
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat &mask,
+#else
+ IplImage* mask,
+#endif
+ unsigned char nb, unsigned int shiftBorder)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ int width = mask.cols;
+ int height = mask.rows;
+#else
+ int width = mask->width;
+ int height = mask->height;
+#endif
+
+ int i_min, i_max, j_min, j_max;
+ std::vector<vpImagePoint> roi;
+ polygon->getRoiClipped(cam, roi);
+ vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);
+
+ /* check image boundaries */
+ if(i_min > height){ //underflow
+ i_min = 0;
+ }
+ if(i_max > height){
+ i_max = height;
+ }
+ if(j_min > width){ //underflow
+ j_min = 0;
+ }
+ if(j_max > width){
+ j_max = width;
+ }
+
+ double shiftBorder_d = (double) shiftBorder;
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ for(int i=i_min; i< i_max; i++){
+ double i_d = (double) i;
+ for(int j=j_min; j< j_max; j++){
+ double j_d = (double) j;
+ if(shiftBorder != 0){
+ if( vpPolygon::isInside(roi, i_d, j_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
+ mask.at<unsigned char>(i,j) = nb;
+ }
+ }
+ else{
+ if(vpPolygon::isInside(roi, i, j)){
+ mask.at<unsigned char>(i,j) = nb;
+ }
+ }
+ }
+ }
+#else
+ unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
+ for(int i=i_min; i< i_max; i++){
+ double i_d = (double) i;
+ for(int j=j_min; j< j_max; j++){
+ double j_d = (double) j;
+ if(shiftBorder != 0){
+ if( vpPolygon::isInside(roi, i_d, j_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
+ && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
+ *(ptrData++) = nb;
+ }
+ else{
+ ptrData++;
+ }
+ }
+ else{
+ if(vpPolygon::isInside(roi, i, j)){
+ *(ptrData++) = nb;
+ }
+ else{
+ ptrData++;
+ }
+ }
+ }
+ ptrData += mask->widthStep - j_max + j_min;
+ }
+#endif
+}
+
+/*!
+ This method removes the outliers. A point is considered as outlier when its
+ associated weight is below a given threshold (threshold_outlier).
+
+ \param _w : Vector containing the weight of all the tracked points.
+ \param threshold_outlier : Threshold to specify wether or not a point has to be deleted.
+*/
+void
+vpMbtDistanceKltPoints::removeOutliers(const vpColVector& _w, const double &threshold_outlier)
+{
+ std::map<int, vpImagePoint> tmp;
+ std::map<int, int> tmp2;
+ unsigned int nbSupp = 0;
+ unsigned int k = 0;
+
+ nbPointsCur = 0;
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
+// if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
+ tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
+ tmp2[iter->first] = curPointsInd[iter->first];
+ nbPointsCur++;
+ }
+ else{
+ nbSupp++;
+ initPoints.erase(iter->first);
+ }
+
+ k+=2;
+ }
+
+ if(nbSupp != 0){
+ curPoints = std::map<int, vpImagePoint>();
+ curPointsInd = std::map<int, int>();
+
+ curPoints = tmp;
+ curPointsInd = tmp2;
+ if(nbPointsCur >= minNbPoint) enoughPoints = true;
+ else enoughPoints = false;
+ }
+}
+
+/*!
+ Display the primitives tracked for the face.
+
+ \param _I : The image where to display.
+*/
+void
+vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
+{
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ vpImagePoint iP;
+ iP.set_i(static_cast<double>(iter->second.get_i()));
+ iP.set_j(static_cast<double>(iter->second.get_j()));
+
+ vpDisplay::displayCross(_I, iP, 10, vpColor::red);
+
+ iP.set_i( vpMath::round( iP.get_i() + 7 ) );
+ iP.set_j( vpMath::round( iP.get_j() + 7 ) );
+ char ide[10];
+ sprintf(ide, "%ld", static_cast<long int>(id));
+ vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ }
+}
+
+/*!
+ Display the primitives tracked for the face.
+
+ \param _I : The image where to display.
+*/
+void
+vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa>& _I)
+{
+ std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
+ for( ; iter != curPoints.end(); iter++){
+ int id(iter->first);
+ vpImagePoint iP;
+ iP.set_i(static_cast<double>(iter->second.get_i()));
+ iP.set_j(static_cast<double>(iter->second.get_j()));
+
+ vpDisplay::displayCross(_I, iP, 10, vpColor::red);
+
+ iP.set_i( vpMath::round( iP.get_i() + 7 ) );
+ iP.set_j( vpMath::round( iP.get_j() + 7 ) );
+ char ide[10];
+ sprintf(ide, "%ld", static_cast<long int>(id));
+ vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ }
+}
+
+void
+vpMbtDistanceKltPoints::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &/*cMo*/, const vpCameraParameters &camera,
+ const vpColor col, const unsigned int thickness, const bool displayFullModel)
+{
+ if(polygon->isVisible() || displayFullModel)
+ {
+ std::vector<std::pair<vpPoint,unsigned int> > roi;
+ polygon->getPolygonClipped(roi);
+
+ for (unsigned int j = 0; j < roi.size(); j += 1){
+ if(((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+
+ vpImagePoint ip1, ip2;
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+
+ if(useScanLine && !displayFullModel)
+ hiddenface->computeScanLineQuery(roi[j].first,roi[(j+1)%roi.size()].first,linesLst, true);
+ else
+ linesLst.push_back(std::make_pair(roi[j].first,roi[(j+1)%roi.size()].first));
+
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ vpDisplay::displayLine(I,ip1,ip2,col, thickness);
+ }
+ }
+ }
+ }
+}
+
+
+void
+vpMbtDistanceKltPoints::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &/*cMo*/, const vpCameraParameters &camera,
+ const vpColor col, const unsigned int thickness, const bool displayFullModel)
+{
+ if(polygon->isVisible() || displayFullModel)
+ {
+ std::vector<std::pair<vpPoint,unsigned int> > roi;
+ polygon->getPolygonClipped(roi);
+
+ for (unsigned int j = 0; j < roi.size(); j += 1){
+ if(((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+
+ vpImagePoint ip1, ip2;
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+
+ if(useScanLine && !displayFullModel)
+ hiddenface->computeScanLineQuery(roi[j].first,roi[(j+1)%roi.size()].first,linesLst, true);
+ else
+ linesLst.push_back(std::make_pair(roi[j].first,roi[(j+1)%roi.size()].first));
+
+ for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
+ vpMeterPixelConversion::convertPoint(camera,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+
+ vpDisplay::displayLine(I,ip1,ip2,col, thickness);
+ }
+ }
+ }
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtDistanceKltPoints.cpp.o) has no symbols
+void dummy_vpMbKltTracker() {};
+#endif
diff --git a/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp
new file mode 100644
index 0000000..2d31f09
--- /dev/null
+++ b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp
@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML parameters of the Model based tracker (using point features).
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <map>
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbtKltXmlParser.h>
+
+
+/*!
+ Default constructor.
+
+*/
+vpMbtKltXmlParser::vpMbtKltXmlParser()
+ : maskBorder(0), maxFeatures(0), winSize(0), qualityValue(0.), minDist(0.),
+ harrisParam(0.), blockSize(0), pyramidLevels(0)
+{
+ init();
+}
+
+/*!
+ Default destructor.
+*/
+vpMbtKltXmlParser::~vpMbtKltXmlParser()
+{
+}
+
+/*!
+ Initialise internal variables (including the map).
+*/
+void
+vpMbtKltXmlParser::init()
+{
+ vpMbXmlParser::init();
+
+ nodeMap["klt"] = klt;
+ nodeMap["mask_border"] = mask_border;
+ nodeMap["max_features"] = max_features;
+ nodeMap["window_size"] = window_size;
+ nodeMap["quality"] = quality;
+ nodeMap["min_distance"] = min_distance;
+ nodeMap["harris"] = harris;
+ nodeMap["size_block"] = size_block;
+ nodeMap["pyramid_lvl"] = pyramid_lvl;
+}
+
+/*!
+ Parse the file in parameters.
+ This method is deprecated, use parse() instead.
+
+ \param filename : File to parse.
+*/
+void
+vpMbtKltXmlParser::parse(const char * filename)
+{
+ std::string file = filename;
+ vpXmlParser::parse(file);
+}
+
+/*!
+ Write info to file.
+
+ \warning Useless, so not yet implemented => Throw exception.
+*/
+void
+vpMbtKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
+{
+ throw vpException(vpException::notImplementedError, "Not yet implemented." );
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void
+vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool camera_node = false;
+ bool face_node = false;
+ bool klt_node = false;
+ bool lod_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case camera:{
+ this->read_camera (doc, dataNode);
+ camera_node = true;
+ }break;
+ case face:{
+ this->read_face(doc, dataNode);
+ face_node = true;
+ }break;
+ case klt:{
+ this->read_klt(doc, dataNode);
+ klt_node = true;
+ }break;
+ case lod:{
+ this->read_lod(doc, dataNode);
+ lod_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!camera_node) {
+ std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+ std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ }
+
+ if(!face_node) {
+ std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
+ std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ }
+
+ if(!klt_node) {
+ std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
+ std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
+ std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
+ std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
+ std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
+ std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
+ std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
+ std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ }
+
+ if(!lod_node) {
+ std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
+ std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
+ }
+}
+
+/*!
+ Read klt information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void
+vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool mask_border_node = false;
+ bool max_features_node = false;
+ bool window_size_node = false;
+ bool quality_node = false;
+ bool min_distance_node = false;
+ bool harris_node = false;
+ bool size_block_node = false;
+ bool pyramid_lvl_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case mask_border:{
+ maskBorder = xmlReadUnsignedIntChild(doc, dataNode);
+ mask_border_node = true;
+ }break;
+ case max_features:{
+ maxFeatures = xmlReadUnsignedIntChild(doc, dataNode);
+ max_features_node = true;
+ }break;
+ case window_size:{
+ winSize = xmlReadUnsignedIntChild(doc, dataNode);
+ window_size_node = true;
+ }break;
+ case quality:{
+ qualityValue = xmlReadDoubleChild(doc, dataNode);
+ quality_node = true;
+ }break;
+ case min_distance:{
+ minDist = xmlReadDoubleChild(doc, dataNode);
+ min_distance_node = true;
+ }break;
+ case harris:{
+ harrisParam = xmlReadDoubleChild(doc, dataNode);
+ harris_node = true;
+ }break;
+ case size_block:{
+ blockSize = xmlReadUnsignedIntChild(doc, dataNode);
+ size_block_node = true;
+ }break;
+ case pyramid_lvl:{
+ pyramidLevels = xmlReadUnsignedIntChild(doc, dataNode);
+ pyramid_lvl_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!mask_border_node)
+ std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Mask Border : "<< maskBorder <<std::endl;
+
+ if(!max_features_node)
+ std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Max Features : "<< maxFeatures <<std::endl;
+
+ if(!window_size_node)
+ std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Windows Size : "<< winSize <<std::endl;
+
+ if(!quality_node)
+ std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Quality : "<< qualityValue <<std::endl;
+
+ if(!min_distance_node)
+ std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Min Distance : "<< minDist <<std::endl;
+
+ if(!harris_node)
+ std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Harris Parameter : "<< harrisParam <<std::endl;
+
+ if(!size_block_node)
+ std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Block Size : "<< blockSize <<std::endl;
+
+ if(!pyramid_lvl_node)
+ std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ else
+ std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<std::endl;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtKltXmlParser.cpp.o) has no symbols
+void dummy_vpMbtKltXmlParser() {};
+#endif
+
diff --git a/modules/tracker/mbt/src/vpMbScanLine.cpp b/modules/tracker/mbt/src/vpMbScanLine.cpp
new file mode 100644
index 0000000..bcf7715
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbScanLine.cpp
@@ -0,0 +1,836 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined _MSC_VER && _MSC_VER >= 1200
+# define NOMINMAX
+#endif
+
+#include <cmath>
+#include <algorithm>
+#include <cstddef>
+#include <iostream>
+#include <utility>
+
+#include <visp3/mbt/vpMbScanLine.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+#if defined(DEBUG_DISP)
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#endif
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+vpMbScanLine::vpMbScanLine()
+ : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(),
+ visibility_samples(), depthTreshold(1e-06)
+#if defined(DEBUG_DISP)
+ ,dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
+#endif
+{
+#if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
+ dispLineDebug = new vpDisplayX();
+ dispMaskDebug = new vpDisplayX();
+#elif defined(VISP_HAVE_GDI) && defined(DEBUG_DISP)
+ dispLineDebug = new vpDisplayGDI();
+ dispMaskDebug = new vpDisplayGDI();
+#endif
+}
+
+vpMbScanLine::~vpMbScanLine()
+{
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
+ if (dispLineDebug != NULL) delete dispLineDebug;
+ if (dispMaskDebug != NULL) delete dispMaskDebug;
+#endif
+}
+/*!
+ Compute the intersections between Y-axis scanlines and a given line (two points polygon).
+
+ \param a : First point of the line.
+ \param b : Second point of the line.
+ \param edge : Pair with the two points of the line.
+ \param ID : Id of the given line (has to be know when using queries).
+ \param scanlines : Resulting intersections.
+*/
+void vpMbScanLine::drawLineY(const vpColVector &a,
+ const vpColVector &b,
+ const vpMbScanLineEdge &edge,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+{
+ double x0 = a[0] / a[2];
+ double y0 = a[1] / a[2];
+ double z0 = a[2];
+ double x1 = b[0] / b[2];
+ double y1 = b[1] / b[2];
+ double z1 = b[2];
+ if (y0 > y1)
+ {
+ std::swap(x0, x1);
+ std::swap(y0, y1);
+ std::swap(z0, z1);
+ }
+
+ //if (y0 >= h - 1 || y1 < 0 || y1 == y0)
+ if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
+ return;
+
+ const unsigned int _y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(y0)));
+ const double _y1 = std::min<double>(h, y1);
+
+ const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
+
+ for(unsigned int y = _y0 ; y < _y1 ; ++y)
+ {
+ const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
+ const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
+ vpMbScanLineSegment s;
+ s.p = x;
+ s.type = POINT;
+ s.Z2 = s.Z1 = mix(z0, z1, alpha);
+ s.P2 = s.P1 = s.p * s.Z1;
+ s.ID = ID;
+ s.edge = edge;
+ s.b_sample_Y = b_sample_Y;
+ scanlines[y].push_back(s);
+ }
+}
+
+/*!
+ Compute the intersections between X-axis scanlines and a given line (two points polygon).
+
+ \param a : First point of the line.
+ \param b : Second point of the line.
+ \param edge : Pair with the two points of the line.
+ \param ID : Id of the given line (has to be know when using queries).
+ \param scanlines : Resulting intersections.
+*/
+void vpMbScanLine::drawLineX(const vpColVector &a,
+ const vpColVector &b,
+ const vpMbScanLineEdge &edge,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+{
+ double x0 = a[0] / a[2];
+ double y0 = a[1] / a[2];
+ double z0 = a[2];
+ double x1 = b[0] / b[2];
+ double y1 = b[1] / b[2];
+ double z1 = b[2];
+ if (x0 > x1)
+ {
+ std::swap(x0, x1);
+ std::swap(y0, y1);
+ std::swap(z0, z1);
+ }
+
+ //if (x0 >= w - 1 || x1 < 0 || x1 == x0)
+ if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
+ return;
+
+ const unsigned int _x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(x0)));
+ const double _x1 = std::min<double>(w, x1);
+
+ const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
+
+ for(unsigned int x = _x0 ; x < _x1 ; ++x)
+ {
+ const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
+ const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
+ vpMbScanLineSegment s;
+ s.p = y;
+ s.type = POINT;
+ s.Z2 = s.Z1 = mix(z0, z1, alpha);
+ s.P2 = s.P1 = s.p * s.Z1;
+ s.ID = ID;
+ s.edge = edge;
+ s.b_sample_Y = b_sample_Y;
+ scanlines[x].push_back(s);
+ }
+}
+
+
+/*!
+ Compute the Y-axis scanlines intersections of a polygon.
+
+ \param polygon : Polygon composed by an array of lines.
+ \param ID : ID of the polygon (has to be know when using queries).
+ \param scanlines : Resulting intersections.
+*/
+void
+vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+{
+ if (polygon.size() < 2)
+ return;
+
+ if (polygon.size() == 2)
+ {
+ vpColVector p1, p2;
+ createVectorFromPoint(polygon.front().first, p1, K);
+ createVectorFromPoint(polygon.back().first, p2, K);
+
+ drawLineY(p1,
+ p2,
+ makeMbScanLineEdge(polygon.front().first, polygon.back().first),
+ ID,
+ scanlines);
+ return;
+ }
+
+ std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
+ local_scanlines.resize(h);
+
+ for(size_t i = 0 ; i < polygon.size() ; ++i)
+ {
+ vpColVector p1, p2;
+ createVectorFromPoint(polygon[i].first, p1, K);
+ createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
+
+ drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
+ }
+
+ createScanLinesFromLocals(scanlines,local_scanlines,h);
+}
+
+/*!
+ Compute the X-axis scanlines intersections of a polygon.
+
+ \param polygon : Polygon composed by an array of lines.
+ \param ID : ID of the polygon (has to be know when using queries).
+ \param scanlines : Resulting intersections.
+*/
+void
+vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
+ const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+{
+ if (polygon.size() < 2)
+ return;
+
+ if (polygon.size() == 2)
+ {
+ vpColVector p1, p2;
+ createVectorFromPoint(polygon.front().first, p1, K);
+ createVectorFromPoint(polygon.back().first, p2, K);
+
+ drawLineX(p1,
+ p2,
+ makeMbScanLineEdge(polygon.front().first, polygon.back().first),
+ ID,
+ scanlines);
+ return;
+ }
+
+ std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
+ local_scanlines.resize(w);
+
+ for(size_t i = 0 ; i < polygon.size() ; ++i)
+ {
+ vpColVector p1, p2;
+ createVectorFromPoint(polygon[i].first, p1, K);
+ createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
+
+ drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
+ }
+
+ createScanLinesFromLocals(scanlines,local_scanlines,w);
+}
+
+/*!
+ Organise local scanlines in a global scanline vector.
+ It also marks the computed intersections as starting or ending points.
+ This function will only be called by the drawPolygons functions.
+
+ \param scanlines : Global scanline vector.
+ \param localScanlines : Local scanline vector (X or Y-axis).
+ \param size : Corresponding size (typically the width or the height).
+*/
+void
+vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
+ std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
+ const unsigned int &size)
+{
+ for(unsigned int j = 0 ; j < size ; ++j)
+ {
+ std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
+ sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator()); // Not sure its necessary
+
+ bool b_start = true;
+ for(size_t i = 0 ; i < scanline.size() ; ++i)
+ {
+ vpMbScanLineSegment s = scanline[i];
+ if (b_start)
+ {
+ s.type = START;
+ s.P1 = s.p * s.Z1;
+ b_start = false;
+ }
+ else
+ {
+ vpMbScanLineSegment &prev = scanlines[j].back();
+ s.type = END;
+ s.P1 = prev.P1;
+ s.Z1 = prev.Z1;
+ s.P2 = s.p * s.Z2;
+ prev.P2 = s.P2;
+ prev.Z2 = s.Z2;
+ b_start = true;
+ }
+ scanlines[j].push_back(s);
+ }
+ }
+}
+
+/*!
+ Render a scene of polygons and compute scanlines intersections in order to use queries.
+
+ \param polygons : List of polygons composed by arrays of lines.
+ \param listPolyIndices : List of polygons IDs (has to be know when using queries).
+ \param cam : Camera parameters.
+ \param width : Width of the image (render window).
+ \param height : Height of the image (render window).
+*/
+void
+vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
+ std::vector<int> listPolyIndices,
+ const vpCameraParameters &cam, unsigned int width, unsigned int height)
+{
+ this->w = width;
+ this->h = height;
+ this->K = cam;
+
+ visibility_samples.clear();
+
+ std::vector<std::vector<vpMbScanLineSegment> > scanlinesY;
+ scanlinesY.resize(h);
+ std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
+ scanlinesX.resize(w);
+
+ mask.resize(h,w,0);
+
+ vpImage<unsigned char> maskY(h,w,0);
+ vpImage<unsigned char> maskX(h,w,0);
+
+ primitive_ids.resize(h, w, -1);
+
+ for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
+ {
+ drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
+ drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
+ }
+
+ // Y
+ int last_ID = -1;
+ vpMbScanLineSegment last_visible;
+ for(unsigned int y = 0 ; y < scanlinesY.size() ; ++y)
+ {
+ std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
+ sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+
+ std::vector<std::pair<double, vpMbScanLineSegment> > stack;
+ for(size_t i = 0 ; i < scanline.size() ; ++i)
+ {
+ const vpMbScanLineSegment &s = scanline[i];
+
+ switch(s.type)
+ {
+ case START:
+ stack.push_back(std::make_pair(s.Z1, s));
+ break;
+ case END:
+ for(size_t j = 0 ; j < stack.size() ; ++j)
+ if (stack[j].second.ID == s.ID)
+ {
+ stack[j] = stack.back();
+ stack.pop_back();
+ break;
+ }
+ break;
+ case POINT:
+ break;
+ }
+
+ for(size_t j = 0 ; j < stack.size() ; ++j)
+ {
+ const vpMbScanLineSegment &s0 = stack[j].second;
+ stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
+ }
+ sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
+
+ int new_ID = stack.empty() ? -1 : stack.front().second.ID;
+
+ if (new_ID != last_ID || s.type == POINT)
+ {
+ if (s.b_sample_Y)
+ switch(s.type)
+ {
+ case POINT:
+ if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
+ visibility_samples[s.edge].insert((int)y);
+ break;
+ case START:
+ if (new_ID == s.ID)
+ visibility_samples[s.edge].insert((int)y);
+ break;
+ case END:
+ if (last_ID == s.ID)
+ visibility_samples[s.edge].insert((int)y);
+ break;
+ }
+
+ // This part will only be used for MbKltTracking
+ if (last_ID != -1)
+ {
+ const unsigned int x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
+ const double x1 = std::min<double>(w, s.p);
+ for(unsigned int x = x0 + maskBorder ; x < x1 - maskBorder; ++x)
+ {
+ primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
+
+ if(maskBorder != 0)
+ maskY[(unsigned int)y][(unsigned int)x] = 255;
+ else
+ mask[(unsigned int)y][(unsigned int)x] = 255;
+ }
+ }
+
+ last_ID = new_ID;
+ if (!stack.empty())
+ {
+ last_visible = stack.front().second;
+ last_visible.p = s.p;
+ }
+ }
+ }
+ }
+
+ // X
+ last_ID = -1;
+ for(unsigned int x = 0 ; x < scanlinesX.size() ; ++x)
+ {
+ std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
+ sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+
+ std::vector<std::pair<double, vpMbScanLineSegment> > stack;
+ for(size_t i = 0 ; i < scanline.size() ; ++i)
+ {
+ const vpMbScanLineSegment &s = scanline[i];
+
+ switch(s.type)
+ {
+ case START:
+ stack.push_back(std::make_pair(s.Z1, s));
+ break;
+ case END:
+ for(size_t j = 0 ; j < stack.size() ; ++j)
+ if (stack[j].second.ID == s.ID)
+ {
+ stack[j] = stack.back();
+ stack.pop_back();
+ break;
+ }
+ break;
+ case POINT:
+ break;
+ }
+
+ for(size_t j = 0 ; j < stack.size() ; ++j)
+ {
+ const vpMbScanLineSegment &s0 = stack[j].second;
+ stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
+ }
+ sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
+
+ int new_ID = stack.empty() ? -1 : stack.front().second.ID;
+
+ if (new_ID != last_ID || s.type == POINT)
+ {
+ if (!s.b_sample_Y)
+ switch(s.type)
+ {
+ case POINT:
+ if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
+ visibility_samples[s.edge].insert((int)x);
+ break;
+ case START:
+ if (new_ID == s.ID)
+ visibility_samples[s.edge].insert((int)x);
+ break;
+ case END:
+ if (last_ID == s.ID)
+ visibility_samples[s.edge].insert((int)x);
+ break;
+ }
+
+ // This part will only be used for MbKltTracking
+ if (maskBorder != 0 && last_ID != -1)
+ {
+ const unsigned int y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
+ const double y1 = std::min<double>(h, s.p);
+ for(unsigned int y = y0 + maskBorder ; y < y1 - maskBorder; ++y)
+ {
+ //primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
+ maskX[(unsigned int)y][(unsigned int)x] = 255;
+ }
+ }
+
+ last_ID = new_ID;
+ if (!stack.empty())
+ {
+ last_visible = stack.front().second;
+ last_visible.p = s.p;
+ }
+ }
+ }
+ }
+
+ if(maskBorder != 0)
+ for(unsigned int i = 0 ; i < h ; i++)
+ for(unsigned int j = 0 ; j < w ; j++)
+ if(maskX[i][j] == 255 && maskY[i][j] == 255)
+ mask[i][j] = 255;
+
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
+ if(!dispMaskDebug->isInitialised()){
+ dispMaskDebug->init(mask, 800, 600);
+ }
+
+ vpDisplay::display(mask);
+
+ for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
+ {
+ for(unsigned int i = 0 ; i < polygons[ID]->size() ; i++){
+ vpPoint p1 = (*(polygons[ID]))[i].first;
+ vpPoint p2 = (*(polygons[ID]))[(i+1)%polygons[ID]->size()].first;
+ double i1=0,j1=0,i2=0,j2=0;
+ p1.project();
+ p2.project();
+ vpMeterPixelConversion::convertPoint(K,p1.get_x(), p1.get_y(),j1,i1);
+ vpMeterPixelConversion::convertPoint(K,p2.get_x(), p2.get_y(),j2,i2);
+
+ vpDisplay::displayLine(mask,i1,j1,i2,j2,vpColor::red,3);
+ }
+ }
+
+ vpDisplay::flush(mask);
+
+
+ if(!dispLineDebug->isInitialised()){
+ linedebugImg.resize(h,w,0);
+ dispLineDebug->init(linedebugImg, 800, 100);
+ }
+ vpDisplay::display(linedebugImg);
+#endif
+
+}
+
+/*!
+ Test the visibility of a line. As a result, a subsampled line of the given one with all its visible parts.
+
+ \param a : First point of the line.
+ \param b : Second point of the line.
+ \param lines : List of lines corresponding of the visible parts of the given line.
+ \param displayResults : True if the results have to be displayed. False otherwise.
+*/
+void
+vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults)
+{
+ vpColVector _a, _b;
+ createVectorFromPoint(a, _a, K);
+ createVectorFromPoint(b, _b, K);
+
+ double x0 = _a[0] / _a[2];
+ double y0 = _a[1] / _a[2];
+ double z0 = _a[2];
+ double x1 = _b[0] / _b[2];
+ double y1 = _b[1] / _b[2];
+ double z1 = _b[2];
+
+ vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
+ lines.clear();
+
+ if(displayResults){
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
+ double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
+ vpPoint a_(a), b_(b);
+ a_.project();
+ b_.project();
+ vpMeterPixelConversion::convertPoint(K,a_.get_x(), a_.get_y(),j1,i1);
+ vpMeterPixelConversion::convertPoint(K,b_.get_x(), b_.get_y(),j2,i2);
+
+ vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::yellow,3);
+#endif
+ }
+
+ if (!visibility_samples.count(edge))
+ return;
+
+ // Initialized as the biggest difference between the two points is on the X-axis
+ double *v0(&x0), *w0(&z0);
+ double *v1(&x1), *w1(&z1);
+ unsigned int size(w);
+
+ if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis
+ {
+ v0 = &y0;
+ v1 = &y1;
+ size = h;
+ }
+
+ // Cannot call swap(a,b) since a and b are const
+ // The fix consists in 2 new points that contain the right points
+ vpPoint a_;
+ vpPoint b_;
+
+ if (*v0 > *v1)
+ {
+ std::swap(v0, v1);
+ std::swap(w0, w1);
+ //std::swap(a, b);
+ // Cannot call swap(a,b) since a and b are const
+ // Instead of swap we set the right address of the corresponding pointers
+ a_ = b;
+ b_ = a;
+ }
+ else {
+ a_ = a;
+ b_ = b;
+ }
+
+ //if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
+ if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
+ return;
+
+ const int _v0 = std::max(0, int(std::ceil(*v0)));
+ const int _v1 = std::min<int>((int)(size - 1), (int)(std::ceil(*v1) - 1));
+
+ const std::set<int> &visible_samples = visibility_samples[edge];
+ int last = _v0;
+ vpPoint line_start;
+ vpPoint line_end;
+ bool b_line_started = false;
+ for(std::set<int>::const_iterator it = visible_samples.begin() ; it != visible_samples.end() ; ++it)
+ {
+ const int v = *it;
+ const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
+ //const vpPoint p = mix(a, b, alpha);
+ const vpPoint p = mix(a_, b_, alpha);
+ if (last + 1 != v)
+ {
+ if(b_line_started)
+ lines.push_back(std::make_pair(line_start, line_end));
+ b_line_started = false;
+ }
+ if (v == _v0)
+ {
+ //line_start = a;
+ line_start = a_;
+ line_end = p;
+ b_line_started = true;
+ }
+ else if (v == _v1)
+ {
+ //line_end = b;
+ line_end = b_;
+ if (!b_line_started)
+ line_start = p;
+ b_line_started = true;
+ }
+ else
+ {
+ line_end = p;
+ if (!b_line_started)
+ line_start = p;
+ b_line_started = true;
+ }
+ last = v;
+ }
+ if (b_line_started)
+ lines.push_back(std::make_pair(line_start, line_end));
+
+ if(displayResults){
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
+ double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
+ for(unsigned int i = 0 ; i < lines.size() ; i++){
+ lines[i].first.project();
+ lines[i].second.project();
+ vpMeterPixelConversion::convertPoint(K,lines[i].first.get_x(), lines[i].first.get_y(),j1,i1);
+ vpMeterPixelConversion::convertPoint(K,lines[i].second.get_x(), lines[i].second.get_y(),j2,i2);
+
+ vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::red,3);
+ }
+ vpDisplay::flush(linedebugImg);
+#endif
+ }
+}
+
+/*!
+ Create a vpMbScanLineEdge from two points while ordering them.
+
+ \param a : First point of the line.
+ \param b : Second point of the line.
+
+ \return Resulting vpMbScanLineEdge.
+*/
+vpMbScanLine::vpMbScanLineEdge
+vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
+{
+ vpColVector _a(3);
+ vpColVector _b(3);
+
+ _a[0] = std::ceil((a.get_X() * 1e8) * 1e-6);
+ _a[1] = std::ceil((a.get_Y() * 1e8) * 1e-6);
+ _a[2] = std::ceil((a.get_Z() * 1e8) * 1e-6);
+
+ _b[0] = std::ceil((b.get_X() * 1e8) * 1e-6);
+ _b[1] = std::ceil((b.get_Y() * 1e8) * 1e-6);
+ _b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);
+
+ bool b_comp = false;
+ for(unsigned int i = 0 ; i < 3 ; ++i)
+ if (_a[i] < _b[i])
+ {
+ b_comp = true;
+ break;
+ }
+ else if(_a[i] > _b[i])
+ break;
+
+ if (b_comp)
+ return std::make_pair(_a, _b);
+
+ return std::make_pair(_b, _a);
+}
+
+/*!
+ Create a vpColVector of a projected point.
+
+ \param p : Point to project.
+ \param v : Resulting vector.
+ \param K : Camera parameters.
+*/
+void
+vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K)
+{
+ v = vpColVector(3);
+
+ v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
+ v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
+ v[2] = p.get_Z();
+}
+
+/*!
+ Compute the interpolation factor.
+
+ \param x : Value used as basis to compute the interpolation factor
+ \param X0 : First extremity.
+ \param Z0 : First extremity.
+ \param X1 : Second extremity.
+ \param Z1 : Second extremity.
+*/
+double
+vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
+{
+ const double N = X0 - x * Z0;
+ const double D = x * (Z1 - Z0) - (X1 - X0);
+ double alpha = N / D;
+ if (vpMath::isNaN(alpha) || vpMath::isInf(alpha))
+ return 0.0;
+
+ alpha = std::min(1.0,alpha);
+ alpha = std::max(0.0,alpha);
+ return alpha;
+}
+
+/*!
+ Interpolate two values.
+
+ \param a : first value.
+ \param b : second value.
+ \param alpha : interpolation factor.
+
+ \return Interpolated value.
+*/
+double
+vpMbScanLine::mix(double a, double b, double alpha)
+{
+ return a + (b - a) * alpha;
+}
+
+/*!
+ Interpolate two vpPoints.
+
+ \param a : first point.
+ \param b : second point.
+ \param alpha : interpolation factor.
+
+ \return Interpolated vpPoint.
+*/
+vpPoint
+vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
+{
+ vpPoint res;
+ res.set_X(a.get_X() + ( b.get_X() - a.get_X() ) * alpha);
+ res.set_Y(a.get_Y() + ( b.get_Y() - a.get_Y() ) * alpha);
+ res.set_Z(a.get_Z() + ( b.get_Z() - a.get_Z() ) * alpha);
+
+ return res;
+}
+
+/*!
+ Compute the norm of two vpPoints.
+
+ \param a : first point.
+ \param b : second point.
+
+ \return Resulting norm.
+*/
+double
+vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
+{
+ return sqrt(vpMath::sqr(a.get_X()-b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) + vpMath::sqr(a.get_Z() - b.get_Z()));
+}
+
+#endif
diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp
new file mode 100644
index 0000000..e70e3a9
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbTracker.cpp
@@ -0,0 +1,2526 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic model based tracker
+ *
+ * Authors:
+ * Romain Tallonneau
+ * Aurelien Yol
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMbTracker.cpp
+ \brief Generic model based tracker
+*/
+
+#include <iostream>
+#include <limits>
+#include <algorithm>
+#include <map>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpDisplay.h>
+#ifdef VISP_HAVE_MODULE_GUI
+# include <visp3/gui/vpDisplayOpenCV.h>
+# include <visp3/gui/vpDisplayX.h>
+# include <visp3/gui/vpDisplayGDI.h>
+#endif
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpException.h>
+#ifdef VISP_HAVE_MODULE_IO
+# include <visp3/io/vpImageIo.h>
+#endif
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpIoTools.h>
+
+#ifdef VISP_HAVE_COIN3D
+//Inventor includes
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/actions/SoSearchAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/actions/SoGetMatrixAction.h>
+#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+#include <Inventor/VRMLnodes/SoVRMLTransform.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#endif
+
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ Structure to store info about segment in CAO model files.
+ */
+struct SegmentInfo {
+ SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
+
+ std::vector<vpPoint> extremities;
+ std::string name;
+ bool useLod;
+ double minLineLengthThresh;
+};
+
+/*!
+ Structure to store info about a polygon face represented by a vpPolygon and by a list of vpPoint
+ representing the corners of the polygon face in 3D.
+ */
+struct PolygonFaceInfo {
+ PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
+: distanceToCamera(dist), polygon(poly), faceCorners(corners) {}
+
+ bool operator<(const PolygonFaceInfo &pfi) const {
+ return distanceToCamera < pfi.distanceToCamera;
+ }
+
+ double distanceToCamera;
+ vpPolygon polygon;
+ std::vector<vpPoint> faceCorners;
+};
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ Basic constructor.
+ Set default values.
+
+*/
+vpMbTracker::vpMbTracker()
+: cam(), cMo(), oJo(6,6), isoJoIdentity(true), modelFileName(), modelInitialised(false),
+ poseSavingFilename(), computeCovariance(false), covarianceMatrix(), computeProjError(false),
+ projectionError(90.0), displayFeatures(false), m_w(), m_error(), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT),
+ faces(), angleAppears( vpMath::rad(89) ), angleDisappears( vpMath::rad(89) ), distNearClip(0.001),
+ distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING), useOgre(false), ogreShowConfigDialog(false), useScanLine(false),
+ nbPoints(0), nbLines(0), nbPolygonLines(0), nbPolygonPoints(0), nbCylinders(0), nbCircles(0),
+ useLodGeneral(false), applyLodSettingInConfig(false), minLineLengthThresholdGeneral(50.0),
+ minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames()
+{
+ oJo.eye();
+ //Map used to parse additional information in CAO model files,
+ //like name of faces or LOD setting
+ mapOfParameterNames["name"] = "string";
+ mapOfParameterNames["minPolygonAreaThreshold"] = "number";
+ mapOfParameterNames["minLineLengthThreshold"] = "number";
+ mapOfParameterNames["useLod"] = "boolean";
+}
+
+/*!
+ Basic destructor that doest nothing.
+*/
+vpMbTracker::~vpMbTracker()
+{
+}
+
+#ifdef VISP_HAVE_MODULE_GUI
+/*!
+ Initialise the tracking by clicking on the image points corresponding to the
+ 3D points (object frame) in the file initFile. The structure of this file
+ is (without the comments):
+ \code
+ 4 // Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 // \
+ ... // | 3D coordinates in the object basis
+ 0.01 -0.01 -0.01 // /
+ \endcode
+
+ \param I : Input image
+ \param initFile : File containing the points where to click
+ \param displayHelp : Optionnal display of an image ( 'initFile.ppm' ). This
+ image may be used to show where to click. This functionality is only available
+ if visp_io module is used.
+
+ \exception vpException::ioError : The file specified in initFile doesn't exist.
+
+ \sa setPathNamePoseSaving()
+
+*/
+void
+vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp)
+{
+ vpHomogeneousMatrix last_cMo;
+ vpPoseVector init_pos;
+ vpImagePoint ip;
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
+
+ std::string ext = ".init";
+ std::string str_pose = "";
+ size_t pos = (unsigned int)initFile.rfind(ext);
+
+ // Load the last poses from files
+ std::fstream finitpos ;
+ std::fstream finit ;
+ char s[FILENAME_MAX];
+ if(poseSavingFilename.empty()){
+ if( pos == initFile.size()-ext.size() && pos != 0)
+ str_pose = initFile.substr(0,pos) + ".0.pos";
+ else
+ str_pose = initFile + ".0.pos";
+
+ finitpos.open(str_pose.c_str() ,std::ios::in) ;
+ sprintf(s, "%s", str_pose.c_str());
+ }else{
+ finitpos.open(poseSavingFilename.c_str() ,std::ios::in) ;
+ sprintf(s, "%s", poseSavingFilename.c_str());
+ }
+ if(finitpos.fail() ){
+ std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
+ last_cMo.eye();
+ }
+ else{
+ for (unsigned int i = 0; i < 6; i += 1){
+ finitpos >> init_pos[i];
+ }
+
+ finitpos.close();
+ last_cMo.buildFrom(init_pos) ;
+
+ std::cout <<"last_cMo : "<<std::endl << last_cMo <<std::endl;
+
+ vpDisplay::display(I);
+ display(I, last_cMo, cam, vpColor::green, 1, true);
+ vpDisplay::displayFrame(I, last_cMo, cam, 0.05, vpColor::green);
+ vpDisplay::flush(I);
+
+ std::cout << "No modification : left click " << std::endl;
+ std::cout << "Modify initial pose : right click " << std::endl ;
+
+ vpDisplay::displayText(I, 15, 10,
+ "left click to validate, right click to modify initial pose",
+ vpColor::red);
+
+ vpDisplay::flush(I) ;
+
+ while (!vpDisplay::getClick(I, ip, button)) ;
+ }
+
+
+ if (!finitpos.fail() && button == vpMouseButton::button1){
+ cMo = last_cMo ;
+ }
+ else
+ {
+ vpDisplay *d_help = NULL;
+
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+
+ vpPose pose ;
+
+ pose.clearPoint() ;
+
+ // file parser
+ // number of points
+ // X Y Z
+ // X Y Z
+
+ double X,Y,Z ;
+
+ if( pos == initFile.size()-ext.size() && pos != 0)
+ sprintf(s,"%s", initFile.c_str());
+ else
+ sprintf(s,"%s.init", initFile.c_str());
+
+ std::cout << "Load 3D points from: " << s << std::endl ;
+ finit.open(s,std::ios::in) ;
+ if (finit.fail()){
+ std::cout << "cannot read " << s << std::endl;
+ throw vpException(vpException::ioError, "cannot read init file");
+ }
+
+#ifdef VISP_HAVE_MODULE_IO
+ //Display window creation and initialisation
+ try{
+ if(displayHelp){
+ std::string dispF;
+ if( pos == initFile.size()-ext.size() && pos != 0)
+ dispF = initFile.substr(0,pos) + ".ppm";
+ else
+ dispF = initFile + ".ppm";
+
+ if (vpIoTools::checkFilename(dispF)) {
+ std::cout << "Load image to help initialization: " << dispF << std::endl;
+#if defined VISP_HAVE_X11
+ d_help = new vpDisplayX ;
+#elif defined VISP_HAVE_GDI
+ d_help = new vpDisplayGDI;
+#elif defined VISP_HAVE_OPENCV
+ d_help = new vpDisplayOpenCV;
+#endif
+
+ vpImage<vpRGBa> Iref ;
+ vpImageIo::read(Iref, dispF) ;
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
+ d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
+ "Where to initialize...") ;
+ vpDisplay::display(Iref) ;
+ vpDisplay::flush(Iref);
+#endif
+ }
+ }
+ }
+ catch(...) {
+ if(d_help != NULL) {
+ delete d_help;
+ d_help = NULL;
+ }
+ }
+#else //#ifdef VISP_HAVE_MODULE_IO
+ (void)(displayHelp);
+#endif //#ifdef VISP_HAVE_MODULE_IO
+ char c;
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+
+ unsigned int n ;
+ finit >> n ;
+ finit.ignore(256, '\n'); // skip the rest of the line
+ std::cout << "Number of 3D points " << n << std::endl ;
+ if (n > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of points.");
+ }
+
+ vpPoint *P = new vpPoint [n] ;
+ for (unsigned int i=0 ; i < n ; i++){
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+
+ finit >> X ;
+ finit >> Y ;
+ finit >> Z ;
+ finit.ignore(256, '\n'); // skip the rest of the line
+
+ std::cout << "Point " << i+1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
+ P[i].setWorldCoordinates(X,Y,Z) ; // (X,Y,Z)
+ }
+
+ finit.close();
+
+////////////////////////////////
+ bool isWellInit = false;
+ while(!isWellInit)
+ {
+////////////////////////////////
+ std::vector<vpImagePoint> mem_ip;
+ for(unsigned int i=0 ; i< n ; i++)
+ {
+ std::ostringstream text;
+ text << "Click on point " << i+1;
+ vpDisplay::display(I);
+ vpDisplay::displayText(I, 15, 10, text.str(), vpColor::red);
+ for (unsigned int k=0; k<mem_ip.size(); k++) {
+ vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2) ;
+ }
+ vpDisplay::flush(I) ;
+
+ std::cout << "Click on point " << i+1 << " ";
+ double x=0,y=0;
+ vpDisplay::getClick(I, ip) ;
+ mem_ip.push_back(ip);
+ vpDisplay::flush(I) ;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+
+ std::cout << "with 2D coordinates: " << ip << std::endl;
+
+ pose.addPoint(P[i]) ; // and added to the pose computation point list
+ }
+ vpDisplay::flush(I) ;
+ vpDisplay::display(I) ;
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1) ;
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2) ;
+ double d2 = pose.computeResidual(cMo2);
+
+ if(d1 < d2){
+ cMo = cMo1;
+ }
+ else{
+ cMo = cMo2;
+ }
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ display(I, cMo, cam, vpColor::green, 1, true);
+ vpDisplay::displayText(I, 15, 10,
+ "left click to validate, right click to re initialize object",
+ vpColor::red);
+
+ vpDisplay::flush(I) ;
+
+ button = vpMouseButton::button1;
+ while (!vpDisplay::getClick(I, ip, button)) ;
+
+
+ if (button == vpMouseButton::button1)
+ {
+ isWellInit = true;
+ }
+ else
+ {
+ pose.clearPoint() ;
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+ }
+////////////////////////////////////
+ vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
+
+ delete [] P;
+
+ //save the pose into file
+ if(poseSavingFilename.empty())
+ savePose(str_pose);
+ else
+ savePose(poseSavingFilename);
+
+ if(d_help != NULL) {
+ delete d_help;
+ d_help = NULL;
+ }
+ }
+
+ std::cout <<"cMo : "<<std::endl << cMo <<std::endl;
+
+ init(I);
+}
+
+/*!
+ Initialise the tracking by clicking on the image points corresponding to the
+ 3D points (object frame) in the list points3D_list.
+
+ \param I : Input image
+ \param points3D_list : List of the 3D points (object frame).
+ \param displayFile : Path to the image used to display the help. This functionality
+ is only available if visp_io module is used.
+*/
+void vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile)
+{
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+ vpDisplay *d_help = NULL;
+
+ vpPose pose ;
+ std::vector<vpPoint> P;
+ for (unsigned int i=0 ; i < points3D_list.size() ; i++)
+ P.push_back( vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()) );
+
+#ifdef VISP_HAVE_MODULE_IO
+ vpImage<vpRGBa> Iref ;
+ //Display window creation and initialisation
+ if(vpIoTools::checkFilename(displayFile)){
+ try{
+ std::cout << "Load image to help initialization: " << displayFile << std::endl;
+#if defined VISP_HAVE_X11
+ d_help = new vpDisplayX ;
+#elif defined VISP_HAVE_GDI
+ d_help = new vpDisplayGDI;
+#elif defined VISP_HAVE_OPENCV
+ d_help = new vpDisplayOpenCV;
+#endif
+
+ vpImageIo::read(Iref, displayFile) ;
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
+ d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
+ "Where to initialize...") ;
+ vpDisplay::display(Iref) ;
+ vpDisplay::flush(Iref);
+#endif
+ }
+ catch(...) {
+ if(d_help != NULL) {
+ delete d_help;
+ d_help = NULL;
+ }
+ }
+ }
+#else //#ifdef VISP_HAVE_MODULE_IO
+ (void)(displayFile);
+#endif //#ifdef VISP_HAVE_MODULE_IO
+
+ vpImagePoint ip;
+ bool isWellInit = false;
+ while(!isWellInit)
+ {
+ for(unsigned int i=0 ; i< points3D_list.size() ; i++)
+ {
+ std::cout << "Click on point " << i+1 << std::endl ;
+ double x=0,y=0;
+ vpDisplay::getClick(I, ip) ;
+ vpDisplay::displayCross(I, ip, 5,vpColor::green) ;
+ vpDisplay::flush(I) ;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+
+ std::cout << "Click on point " << ip << std::endl;
+
+ vpDisplay::displayPoint (I, ip, vpColor::green); //display target point
+ pose.addPoint(P[i]) ; // and added to the pose computation point list
+ }
+ vpDisplay::flush(I) ;
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1) ;
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2) ;
+ double d2 = pose.computeResidual(cMo2);
+
+ if(d1 < d2){
+ cMo = cMo1;
+ }
+ else{
+ cMo = cMo2;
+ }
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ display(I, cMo, cam, vpColor::green, 1, true);
+ vpDisplay::displayText(I, 15, 10,
+ "left click to validate, right click to re initialize object",
+ vpColor::red);
+
+ vpDisplay::flush(I) ;
+
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
+ while (!vpDisplay::getClick(I, ip, button)) ;
+
+ if (button == vpMouseButton::button1)
+ {
+ isWellInit = true;
+ }
+ else
+ {
+ pose.clearPoint() ;
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+ }
+
+ vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
+
+ if(d_help != NULL) {
+ delete d_help;
+ d_help = NULL;
+ }
+
+ init(I);
+}
+#endif //#ifdef VISP_HAVE_MODULE_GUI
+
+/*!
+ Initialise the tracking by reading the 3D points (object frame) and the image points
+ in initFile. The structure of this file is (without the comments):
+ \code
+ 4 // Number of 3D points in the file (minimum is four)
+ 0.01 0.01 0.01 // \
+ ... // | 3D coordinates in meters in the object frame
+ 0.01 -0.01 -0.01 // /
+ 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 // /
+ \endcode
+
+ \param I : Input image
+ \param initFile : Path to the file containing all the points.
+*/
+void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile )
+{
+ char s[FILENAME_MAX];
+ std::fstream finit ;
+
+ std::string ext = ".init";
+ size_t pos = initFile.rfind(ext);
+
+ if( pos == initFile.size()-ext.size() && pos != 0)
+ sprintf(s,"%s", initFile.c_str());
+ else
+ sprintf(s,"%s.init", initFile.c_str());
+
+ std::cout << "filename " << s << std::endl ;
+ finit.open(s,std::ios::in) ;
+ if (finit.fail()){
+ std::cout << "cannot read " << s << std::endl;
+ throw vpException(vpException::ioError, "cannot read init file");
+ }
+
+ unsigned int size;
+ double X, Y, Z;
+ finit >> size ;
+ std::cout << "number of points " << size << std::endl ;
+
+ if (size > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of points.");
+ }
+
+ vpPoint *P = new vpPoint [size];
+ vpPose pose ;
+
+ for(unsigned int i=0 ; i< size ; i++)
+ {
+ finit >> X ;
+ finit >> Y ;
+ finit >> Z ;
+ P[i].setWorldCoordinates(X,Y,Z) ;
+ }
+
+ unsigned int size2;
+ double x, y;
+ vpImagePoint ip;
+ finit >> size2 ;
+ if(size != size2)
+ vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
+
+ for(unsigned int i=0 ; i< size ; i++)
+ {
+ finit >> x;
+ finit >> y;
+ ip = vpImagePoint(x,y);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+ pose.addPoint(P[i]);
+ }
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1) ;
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2) ;
+ double d2 = pose.computeResidual(cMo2);
+
+ if(d1 < d2)
+ cMo = cMo1;
+ else
+ cMo = cMo2;
+
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ delete [] P;
+
+ init(I);
+}
+
+/*!
+ Initialise the tracking with the list of image points (points2D_list) and
+ the list of corresponding 3D points (object frame) (points3D_list).
+
+ \param I : Input image
+ \param points2D_list : List of image points.
+ \param points3D_list : List of 3D points (object frame).
+*/
+void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list,
+ const std::vector<vpPoint> &points3D_list )
+{
+ if(points2D_list.size() != points3D_list.size())
+ vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
+
+ size_t size = points3D_list.size();
+ std::vector<vpPoint> P;
+ vpPose pose ;
+
+ for(size_t i=0 ; i< size ; i++)
+ {
+ P.push_back( vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()) );
+ double x=0,y=0;
+ vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+ pose.addPoint(P[i]);
+ }
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1) ;
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2) ;
+ double d2 = pose.computeResidual(cMo2);
+
+ if(d1 < d2)
+ cMo = cMo1;
+ else
+ cMo = cMo2;
+
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ init(I);
+}
+
+/*!
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
+ The structure of this file is (without the comments):
+ \code
+ // The six value of the pose vector
+ 0.0000 // \
+ 0.0000 // |
+ 1.0000 // | Exemple of value for the pose vector where Z = 1 meter
+ 0.0000 // |
+ 0.0000 // |
+ 0.0000 // /
+ \endcode
+
+ Where the three firsts lines refer to the translation and the three last to the rotation in thetaU parametrisation (see vpThetaUVector).
+ \param I : Input image
+ \param initFile : Path to the file containing the pose.
+*/
+void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile)
+{
+ char s[FILENAME_MAX];
+ std::fstream finit ;
+ vpPoseVector init_pos;
+
+ std::string ext = ".pos";
+ size_t pos = initFile.rfind(ext);
+
+ if( pos == initFile.size()-ext.size() && pos != 0)
+ sprintf(s,"%s", initFile.c_str());
+ else
+ sprintf(s,"%s.pos", initFile.c_str());
+
+ finit.open(s,std::ios::in) ;
+ if (finit.fail()){
+ std::cout << "cannot read " << s << std::endl;
+ throw vpException(vpException::ioError, "cannot read init file");
+ }
+
+ for (unsigned int i = 0; i < 6; i += 1){
+ finit >> init_pos[i];
+ }
+
+ cMo.buildFrom(init_pos);
+ init(I);
+}
+
+/*!
+ Initialise the tracking thanks to the pose.
+
+ \param I : Input image
+ \param cMo_ : Pose matrix.
+*/
+void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_)
+{
+ this->cMo = cMo_;
+ init(I);
+}
+
+/*!
+ Initialise the tracking thanks to the pose vector.
+
+ \param I : Input image
+ \param cPo : Pose vector.
+*/
+void vpMbTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo)
+{
+ vpHomogeneousMatrix _cMo(cPo);
+ initFromPose(I, _cMo);
+}
+
+/*!
+ Save the pose in the given filename
+
+ \param filename : Path to the file used to save the pose.
+*/
+void vpMbTracker::savePose(const std::string &filename)
+{
+ vpPoseVector init_pos;
+ std::fstream finitpos ;
+ char s[FILENAME_MAX];
+
+ sprintf(s,"%s", filename.c_str());
+ finitpos.open(s, std::ios::out) ;
+
+ init_pos.buildFrom(cMo);
+ finitpos << init_pos;
+ finitpos.close();
+}
+
+
+void vpMbTracker::addPolygon(const std::vector<vpPoint>& corners, const int idFace, const std::string &polygonName,
+ const bool useLod, const double minPolygonAreaThreshold, const double minLineLengthThreshold)
+{
+ std::vector<vpPoint> corners_without_duplicates;
+ corners_without_duplicates.push_back(corners[0]);
+ for (unsigned int i=0; i < corners.size()-1; i++) {
+ if (std::fabs(corners[i].get_oX() - corners[i+1].get_oX()) > std::fabs(corners[i].get_oX())*std::numeric_limits<double>::epsilon()
+ || std::fabs(corners[i].get_oY() - corners[i+1].get_oY()) > std::fabs(corners[i].get_oY())*std::numeric_limits<double>::epsilon()
+ || std::fabs(corners[i].get_oZ() - corners[i+1].get_oZ()) > std::fabs(corners[i].get_oZ())*std::numeric_limits<double>::epsilon()) {
+ corners_without_duplicates.push_back(corners[i+1]);
+ }
+ }
+
+ vpMbtPolygon polygon;
+ polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
+ polygon.setIndex((int)idFace);
+ polygon.setName(polygonName);
+ polygon.setLod(useLod);
+
+// //if(minPolygonAreaThreshold != -1.0) {
+// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
+// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+// }
+//
+// //if(minLineLengthThreshold != -1.0) {
+// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
+// polygon.setMinLineLengthThresh(minLineLengthThreshold);
+// }
+
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+ polygon.setMinLineLengthThresh(minLineLengthThreshold);
+
+ for(unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
+ polygon.addPoint(j, corners_without_duplicates[j]);
+ }
+
+ faces.addPolygon(&polygon);
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
+
+ if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+
+ if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+}
+
+void vpMbTracker::addPolygon(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace, const std::string &polygonName, const bool useLod, const double minPolygonAreaThreshold)
+{
+ vpMbtPolygon polygon;
+ polygon.setNbPoint(4);
+ polygon.setName(polygonName);
+ polygon.setLod(useLod);
+
+// //if(minPolygonAreaThreshold != -1.0) {
+// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
+// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+// }
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+ //Non sense to set minLineLengthThreshold for circle
+ //but used to be coherent when applying LOD settings for all polygons
+ polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);
+
+ {
+ // Create the 4 points of the circle bounding box
+ vpPlane plane(p1, p2, p3, vpPlane::object_frame);
+
+ // Matrice de passage entre world et circle frame
+ double norm_X = sqrt(vpMath::sqr(p2.get_oX()-p1.get_oX())
+ + vpMath::sqr(p2.get_oY()-p1.get_oY())
+ + vpMath::sqr(p2.get_oZ()-p1.get_oZ()));
+ double norm_Y = sqrt(vpMath::sqr(plane.getA())
+ + vpMath::sqr(plane.getB())
+ + vpMath::sqr(plane.getC()));
+ vpRotationMatrix wRc;
+ vpColVector x(3),y(3),z(3);
+ // X axis is P2-P1
+ x[0] = (p2.get_oX()-p1.get_oX()) / norm_X;
+ x[1] = (p2.get_oY()-p1.get_oY()) / norm_X;
+ x[2] = (p2.get_oZ()-p1.get_oZ()) / norm_X;
+ // Y axis is the normal of the plane
+ y[0] = plane.getA() / norm_Y;
+ y[1] = plane.getB() / norm_Y;
+ y[2] = plane.getC() / norm_Y;
+ // Z axis = X ^ Y
+ z = vpColVector::crossProd(x, y);
+ for (unsigned int i=0; i< 3; i++) {
+ wRc[i][0] = x[i];
+ wRc[i][1] = y[i];
+ wRc[i][2] = z[i];
+ }
+
+ vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
+ vpHomogeneousMatrix wMc(wtc, wRc);
+
+ vpColVector c_p(4); // A point in the circle frame that is on the bbox
+ c_p[0] = radius;
+ c_p[1] = 0;
+ c_p[2] = radius;
+ c_p[3] = 1;
+
+ // Matrix to rotate a point by 90 deg around Y in the circle frame
+ for(unsigned int i=0; i<4; i++) {
+ vpColVector w_p(4); // A point in the word frame
+ vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0,vpMath::rad(90*i), 0));
+ w_p = wMc * cMc_90 * c_p;
+
+ vpPoint w_P;
+ w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
+
+ polygon.addPoint(i,w_P);
+ }
+ }
+
+ polygon.setIndex(idFace);
+ faces.addPolygon(&polygon);
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
+
+ if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+
+ if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+}
+
+void vpMbTracker::addPolygon(const vpPoint& p1, const vpPoint &p2, const int idFace, const std::string &polygonName,
+ const bool useLod, const double minLineLengthThreshold)
+{
+ // A polygon as a single line that corresponds to the revolution axis of the cylinder
+ vpMbtPolygon polygon;
+ polygon.setNbPoint(2);
+
+ polygon.addPoint(0, p1);
+ polygon.addPoint(1, p2);
+
+ polygon.setIndex(idFace) ;
+ polygon.setName(polygonName);
+ polygon.setLod(useLod);
+
+// //if(minLineLengthThreshold != -1.0) {
+// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
+// polygon.setMinLineLengthThresh(minLineLengthThreshold);
+// }
+ polygon.setMinLineLengthThresh(minLineLengthThreshold);
+ //Non sense to set minPolygonAreaThreshold for cylinder
+ //but used to be coherent when applying LOD settings for all polygons
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+
+ faces.addPolygon(&polygon) ;
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
+
+ if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+
+ if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+}
+
+void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace, const std::string &polygonName,
+ const bool useLod, const double minLineLengthThreshold)
+{
+ int id = idFace;
+ for(unsigned int i = 0 ; i < listFaces.size() ; i++)
+ {
+ vpMbtPolygon polygon;
+ polygon.setNbPoint((unsigned int)listFaces[i].size());
+ for(unsigned int j = 0 ; j < listFaces[i].size() ; j++)
+ polygon.addPoint(j, listFaces[i][j]);
+
+ polygon.setIndex(id) ;
+ polygon.setName(polygonName);
+ polygon.setIsPolygonOriented(false);
+ polygon.setLod(useLod);
+ polygon.setMinLineLengthThresh(minLineLengthThreshold);
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+
+ faces.addPolygon(&polygon) ;
+
+ if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
+
+ if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+
+ if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+
+ id++;
+ }
+}
+
+/*!
+ Load a 3D model from the file in parameter. This file must either be a vrml
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
+
+ \warning When this class is called to load a vrml model, remember that you
+ have to call Call SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \throw vpException::ioError if the file cannot be open, or if its extension is
+ not wrl or cao.
+
+ \param modelFile : the file containing the the 3D model description.
+ The extension of this file is either .wrl or .cao.
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbTracker::loadModel(const char *modelFile, const bool verbose)
+{
+ loadModel( std::string(modelFile), verbose );
+}
+
+/*!
+ Load a 3D model from the file in parameter. This file must either be a vrml
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
+
+ \warning When this class is called to load a vrml model, remember that you
+ have to call Call SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \throw vpException::ioError if the file cannot be open, or if its extension is
+ not wrl or cao.
+
+ \param modelFile : the file containing the the 3D model description.
+ The extension of this file is either .wrl or .cao.
+ \param verbose : verbose option to print additional information when loading CAO model files which include other
+ CAO model files.
+*/
+void
+vpMbTracker::loadModel(const std::string& modelFile, const bool verbose)
+{
+ std::string::const_iterator it;
+
+ if(vpIoTools::checkFilename(modelFile)) {
+ it = modelFile.end();
+ if((*(it-1) == 'o' && *(it-2) == 'a' && *(it-3) == 'c' && *(it-4) == '.') ||
+ (*(it-1) == 'O' && *(it-2) == 'A' && *(it-3) == 'C' && *(it-4) == '.') ){
+ std::vector<std::string> vectorOfModelFilename;
+ int startIdFace = faces.size();
+ nbPoints = 0;
+ nbLines = 0;
+ nbPolygonLines = 0;
+ nbPolygonPoints = 0;
+ nbCylinders = 0;
+ nbCircles = 0;
+ loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true);
+ }
+ else if((*(it-1) == 'l' && *(it-2) == 'r' && *(it-3) == 'w' && *(it-4) == '.') ||
+ (*(it-1) == 'L' && *(it-2) == 'R' && *(it-3) == 'W' && *(it-4) == '.') ){
+ loadVRMLModel(modelFile);
+ }
+ else{
+ throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
+ }
+ }
+ else{
+ throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
+ }
+
+ this->modelInitialised = true;
+ this->modelFileName = modelFile;
+}
+
+
+/*!
+ Load the 3D model of the object from a vrml file. Only LineSet and FaceSet are
+ extracted from the vrml file.
+
+ \warning When this class is called, remember that you have to call Call
+ SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \warning The cylinders extracted using this method do not use the Cylinder
+ keyword of vrml since vrml exporter such as Blender or AC3D consider a
+ cylinder as an IndexedFaceSet. To test whether an indexedFaceSet is a cylinder
+ or not, the name of the geometry is read. If the name begins with "cyl" then
+ the faceset is supposed to be a cylinder. For example, the line
+ \code
+geometry DEF cyl_cylinder1 IndexedFaceSet
+ \endcode
+ defines a cylinder named cyl_cylinder1.
+
+ \throw vpException::fatalError if the file cannot be open.
+
+ \param modelFile : The full name of the file containing the 3D model.
+*/
+void
+vpMbTracker::loadVRMLModel(const std::string& modelFile)
+{
+#ifdef VISP_HAVE_COIN3D
+ SoDB::init(); // Call SoDB::finish() before ending the program.
+
+ SoInput in;
+ SbBool ok = in.openFile(modelFile.c_str());
+ SoSeparator *sceneGraph;
+ SoVRMLGroup *sceneGraphVRML2;
+
+ if (!ok) {
+ vpERROR_TRACE("can't open file to load model");
+ throw vpException(vpException::fatalError, "can't open file to load model");
+ }
+
+ if(!in.isFileVRML2())
+ {
+ sceneGraph = SoDB::readAll(&in);
+ if (sceneGraph == NULL) { /*return -1;*/ }
+ sceneGraph->ref();
+
+ SoToVRML2Action tovrml2;
+ tovrml2.apply(sceneGraph);
+
+ sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
+ sceneGraphVRML2->ref();
+ sceneGraph->unref();
+ }
+ else
+ {
+ sceneGraphVRML2 = SoDB::readAllVRML(&in);
+ if (sceneGraphVRML2 == NULL) { /*return -1;*/ }
+ sceneGraphVRML2->ref();
+ }
+
+ in.closeFile();
+
+ vpHomogeneousMatrix transform;
+ int indexFace = faces.size();
+ extractGroup(sceneGraphVRML2, transform, indexFace);
+
+ sceneGraphVRML2->unref();
+#else
+ vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
+ throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
+#endif
+}
+
+void vpMbTracker::removeComment(std::ifstream& fileId) {
+ char c;
+
+ fileId.get(c);
+ while (!fileId.fail() && (c == '#')) {
+ fileId.ignore(256, '\n');
+ fileId.get(c);
+ }
+ if (fileId.fail())
+ throw(vpException(vpException::ioError, "Reached end of file"));
+ fileId.unget();
+}
+
+std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& endLine) {
+ std::map<std::string, std::string> mapOfParams;
+
+ bool exit = false;
+ while (!endLine.empty() && !exit) {
+ exit = true;
+
+ for (std::map<std::string, std::string>::const_iterator it =
+ mapOfParameterNames.begin(); it != mapOfParameterNames.end();
+ ++it) {
+ endLine = trim(endLine);
+ // std::cout << "endLine=" << endLine << std::endl;
+ std::string param(it->first + "=");
+
+ //Compare with a potential parameter
+ if (endLine.compare(0, param.size(), param) == 0) {
+ exit = false;
+ endLine = endLine.substr(param.size());
+
+ bool parseQuote = false;
+ if (it->second == "string") {
+ //Check if the string is between quotes
+ if (endLine.size() > 2 && endLine[0] == '"') {
+ parseQuote = true;
+ endLine = endLine.substr(1);
+ size_t pos = endLine.find_first_of('"');
+
+ if (pos != std::string::npos) {
+ mapOfParams[it->first] = endLine.substr(0, pos);
+ endLine = endLine.substr(pos + 1);
+ } else {
+ parseQuote = false;
+ }
+ }
+ }
+
+ if (!parseQuote) {
+ //Deal with space or tabulation after parameter value to substring
+ // to the next sequence
+ size_t pos1 = endLine.find_first_of(' ');
+ size_t pos2 = endLine.find_first_of('\t');
+ size_t pos = pos1 < pos2 ? pos1 : pos2;
+
+ mapOfParams[it->first] = endLine.substr(0, pos);
+ endLine = endLine.substr(pos + 1);
+ }
+ }
+ }
+ }
+
+// for(std::map<std::string, std::string>::const_iterator it = mapOfParams.begin(); it != mapOfParams.end(); ++it) {
+// std::cout << it->first << "=" << it->second << std::endl;
+// }
+
+ return mapOfParams;
+}
+
+/*!
+ Load a 3D model contained in a *.cao file.
+
+ Since ViSP 2.9.1, lines starting with # character are considered as comments.
+ It is also possible to add comment at the end of the lines. No specific character is requested before the comment.
+ In the following example we use "//" but it could be an other character.
+
+ Since ViSP 2.9.1, circles are supported.
+
+ The structure of the file is :
+ \code
+ V1
+ # Number of points describing the object
+ 8
+ 0.01 0.01 0.01 // point with index 0 \
+ ... // ... | coordinates of the points in the object frame (in m.)
+ 0.01 0.01 0.01 // point with index 7 /
+ # Number of lines to track.
+ 3
+ 0 2 // line with index 0 \
+ 1 4 // | Index of the points representing the extremities of the lines
+ 1 5 // line with index 2 /
+ # Number of polygon (face) to track using the line previously described
+ 1
+ 3 0 1 2 // Face described as follow : nbLine indexLine1 indexLine2 ... indexLineN
+ # Number of polygon (face) to track using the points previously described
+ 3
+ 4 0 2 3 4 // Face described as follow : nbPoint IndexPoint1 IndexPoint2 ... IndexPointN
+ 4 1 3 5 7
+ 3 1 5 6
+ # Number of cylinder
+ 1
+ 6 7 0.05 // Index of the limits points on the axis (used to know the 'height' of the cylinder) and radius of the cyclinder (in m.)
+ # Number of circle
+ 1
+ 0.5 0 1 2 // radius, index center point, index 2 other points on the plane containing the circle
+ \endcode
+
+ \param modelFile : Full name of the main *.cao file containing the model.
+ \param vectorOfModelFilename : A vector of *.cao files.
+ \param startIdFace : Current Id of the face.
+ \param verbose : If true, will print additional information with CAO model files which include other CAO model files.
+ \param parent : This parameter is set to true when parsing a parent CAO model file, and false when parsing an included
+ CAO model file.
+*/
+void
+vpMbTracker::loadCAOModel(const std::string& modelFile,
+ std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
+ const bool verbose, const bool parent) {
+ std::ifstream fileId;
+ fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
+ fileId.open(modelFile.c_str(), std::ifstream::in);
+ if (fileId.fail()) {
+ std::cout << "cannot read CAO model file: " << modelFile << std::endl;
+ throw vpException(vpException::ioError, "cannot read CAO model file");
+ }
+
+ if(verbose) {
+ std::cout << "Model file : " << modelFile << std::endl;
+ }
+ vectorOfModelFilename.push_back(modelFile);
+
+ try {
+ char c;
+ // Extraction of the version (remove empty line and commented ones (comment
+ // line begin with the #)).
+ //while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
+ removeComment(fileId);
+
+ //////////////////////////Read CAO Version (V1, V2, ...)//////////////////////////
+ int caoVersion;
+ fileId.get(c);
+ if (c == 'V') {
+ fileId >> caoVersion;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+ } else {
+ std::cout
+ << "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...";
+ throw vpException(vpException::badValue,
+ "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...");
+ }
+
+ removeComment(fileId);
+
+
+ //////////////////////////Read the header part if present//////////////////////////
+ std::string line;
+ std::string prefix = "load";
+
+ fileId.get(c);
+ fileId.unget();
+ bool header = false;
+ while(c == 'l' || c == 'L') {
+ header = true;
+
+ getline(fileId, line);
+ if(!line.compare(0, prefix.size(), prefix)) {
+
+ //Get the loaded model pathname
+ std::string headerPathname = line.substr(6);
+ size_t firstIndex = headerPathname.find_first_of("\")");
+ headerPathname = headerPathname.substr(0, firstIndex);
+
+ std::string headerPath = headerPathname;
+ if(!vpIoTools::isAbsolutePathname(headerPathname)) {
+ std::string parentDirectory = vpIoTools::getParent(modelFile);
+ headerPath = vpIoTools::createFilePath(parentDirectory, headerPathname);
+ }
+
+ bool cyclic = false;
+ std::string headerModelFilename = vpIoTools::getName(headerPath);
+ if (!headerModelFilename.compare(vpIoTools::getName(modelFile))) {
+ cyclic = true;
+ }
+
+ for (std::vector<std::string>::const_iterator it =
+ vectorOfModelFilename.begin();
+ it != vectorOfModelFilename.end() - 1 && !cyclic;
+ ++it) {
+ std::string loadedModelFilename = vpIoTools::getName(*it);
+ if (!headerModelFilename.compare(loadedModelFilename)) {
+ cyclic = true;
+ }
+ }
+
+ if (!cyclic) {
+ if (vpIoTools::checkFilename(modelFile)) {
+ loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false);
+ } else {
+ throw vpException(vpException::ioError,
+ "file cannot be open");
+ }
+ } else {
+ std::cout << "WARNING Cyclic dependency detected with file "
+ << headerPath << " declared in " << modelFile << std::endl;
+ }
+ } else {
+ header = false;
+ }
+
+ removeComment(fileId);
+ fileId.get(c);
+ fileId.unget();
+ }
+
+
+ //////////////////////////Read the point declaration part//////////////////////////
+ unsigned int caoNbrPoint;
+ fileId >> caoNbrPoint;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbPoints += caoNbrPoint;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbrPoint << " points" << std::endl;
+ }
+
+ if (caoNbrPoint > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of points in the CAO model.");
+ }
+
+ if (caoNbrPoint == 0 && !header) {
+ throw vpException(vpException::badValue,
+ "in vpMbTracker::loadCAOModel() -> no points are defined");
+ }
+ vpPoint *caoPoints = new vpPoint[caoNbrPoint];
+
+ double x; // 3D coordinates
+ double y;
+ double z;
+
+ int i; // image coordinate (used for matching)
+ int j;
+
+ for (unsigned int k = 0; k < caoNbrPoint; k++) {
+ removeComment(fileId);
+
+ fileId >> x;
+ fileId >> y;
+ fileId >> z;
+
+ if (caoVersion == 2) {
+ fileId >> i;
+ fileId >> j;
+ }
+
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ caoPoints[k].setWorldCoordinates(x, y, z);
+ }
+
+
+ removeComment(fileId);
+
+
+ //////////////////////////Read the segment declaration part//////////////////////////
+ //Store in a map the potential segments to add
+ std::map<std::pair<unsigned int, unsigned int>, SegmentInfo > segmentTemporaryMap;
+ unsigned int caoNbrLine;
+ fileId >> caoNbrLine;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbLines += caoNbrLine;
+ unsigned int *caoLinePoints = NULL;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbrLine << " lines" << std::endl;
+ }
+
+ if (caoNbrLine > 100000) {
+ delete[] caoPoints;
+ throw vpException(vpException::badValue,
+ "Exceed the max number of lines in the CAO model.");
+ }
+
+ if (caoNbrLine > 0)
+ caoLinePoints = new unsigned int[2 * caoNbrLine];
+
+ unsigned int index1, index2;
+ //Initialization of idFace with startIdFace for dealing with recursive load in header
+ int idFace = startIdFace;
+
+ for (unsigned int k = 0; k < caoNbrLine; k++) {
+ removeComment(fileId);
+
+ fileId >> index1;
+ fileId >> index2;
+
+ //////////////////////////Read the parameter value if present//////////////////////////
+ //Get the end of the line
+ char buffer[256];
+ fileId.getline(buffer, 256);
+ std::string endLine(buffer);
+ std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
+
+// fileId.ignore(256, '\n'); // skip the rest of the line
+
+ std::string segmentName = "";
+ double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
+ bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
+ if(mapOfParams.find("name") != mapOfParams.end()) {
+ segmentName = mapOfParams["name"];
+ }
+ if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
+ minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
+ }
+ if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ useLod = parseBoolean(mapOfParams["useLod"]);
+ }
+
+ SegmentInfo segmentInfo;
+ segmentInfo.name = segmentName;
+ segmentInfo.useLod = useLod;
+ segmentInfo.minLineLengthThresh = minLineLengthThresh;
+
+ caoLinePoints[2 * k] = index1;
+ caoLinePoints[2 * k + 1] = index2;
+
+ if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
+ std::vector<vpPoint> extremities;
+ extremities.push_back(caoPoints[index1]);
+ extremities.push_back(caoPoints[index2]);
+ segmentInfo.extremities = extremities;
+
+ std::pair<unsigned int, unsigned int> key(index1, index2);
+
+ segmentTemporaryMap[key] = segmentInfo;
+// addPolygon(extremities, idFace++);
+// initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ } else {
+ vpTRACE(" line %d has wrong coordinates.", k);
+ }
+ }
+
+ removeComment(fileId);
+
+
+ //////////////////////////Read the face segment declaration part//////////////////////////
+ /* Load polygon from the lines extracted earlier (the first point of the line is used)*/
+ //Store in a vector the indexes of the segments added in the face segment case
+ std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
+ unsigned int caoNbrPolygonLine;
+ fileId >> caoNbrPolygonLine;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbPolygonLines += caoNbrPolygonLine;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
+ }
+
+ if (caoNbrPolygonLine > 100000) {
+ delete[] caoPoints;
+ delete[] caoLinePoints;
+ throw vpException(vpException::badValue,
+ "Exceed the max number of polygon lines.");
+ }
+
+ unsigned int index;
+ for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
+ removeComment(fileId);
+
+ unsigned int nbLinePol;
+ fileId >> nbLinePol;
+ std::vector<vpPoint> corners;
+ if (nbLinePol > 100000) {
+ throw vpException(vpException::badValue, "Exceed the max number of lines.");
+ }
+
+ for (unsigned int n = 0; n < nbLinePol; n++) {
+ fileId >> index;
+// if (2 * index > 2 * caoNbrLine - 1) {
+ if(index >= caoNbrLine) {
+ throw vpException(vpException::badValue, "Exceed the max number of lines.");
+ }
+ corners.push_back(caoPoints[caoLinePoints[2 * index]]);
+ corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
+
+ std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
+ faceSegmentKeyVector.push_back(key);
+ }
+
+
+ //////////////////////////Read the parameter value if present//////////////////////////
+ //Get the end of the line
+ char buffer[256];
+ fileId.getline(buffer, 256);
+ std::string endLine(buffer);
+ std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
+
+// fileId.ignore(256, '\n'); // skip the rest of the line
+
+ std::string polygonName = "";
+ bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
+ double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
+ if(mapOfParams.find("name") != mapOfParams.end()) {
+ polygonName = mapOfParams["name"];
+ }
+ if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
+ }
+ if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ useLod = parseBoolean(mapOfParams["useLod"]);
+ }
+
+ addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
+ // initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ }
+
+ //Add the segments which were not already added in the face segment case
+ for(std::map<std::pair<unsigned int, unsigned int>, SegmentInfo >::const_iterator it =
+ segmentTemporaryMap.begin(); it != segmentTemporaryMap.end(); ++it) {
+ if(std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) == faceSegmentKeyVector.end()) {
+ addPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
+ it->second.minLineLengthThresh);
+ initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ }
+ }
+
+ removeComment(fileId);
+
+
+ //////////////////////////Read the face point declaration part//////////////////////////
+ /* Extract the polygon using the point coordinates (top of the file) */
+ unsigned int caoNbrPolygonPoint;
+ fileId >> caoNbrPolygonPoint;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbPolygonPoints += caoNbrPolygonPoint;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbrPolygonPoint << " polygon points"
+ << std::endl;
+ }
+
+ if (caoNbrPolygonPoint > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of polygon point.");
+ }
+
+ for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
+ removeComment(fileId);
+
+ unsigned int nbPointPol;
+ fileId >> nbPointPol;
+ if (nbPointPol > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of points.");
+ }
+ std::vector<vpPoint> corners;
+ for (unsigned int n = 0; n < nbPointPol; n++) {
+ fileId >> index;
+ if (index > caoNbrPoint - 1) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of points.");
+ }
+ corners.push_back(caoPoints[index]);
+ }
+
+
+ //////////////////////////Read the parameter value if present//////////////////////////
+ //Get the end of the line
+ char buffer[256];
+ fileId.getline(buffer, 256);
+ std::string endLine(buffer);
+ std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
+
+// fileId.ignore(256, '\n'); // skip the rest of the line
+
+ std::string polygonName = "";
+ bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
+ double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
+ if(mapOfParams.find("name") != mapOfParams.end()) {
+ polygonName = mapOfParams["name"];
+ }
+ if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
+ }
+ if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ useLod = parseBoolean(mapOfParams["useLod"]);
+ }
+
+
+ addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
+ initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ }
+
+ //////////////////////////Read the cylinder declaration part//////////////////////////
+ unsigned int caoNbCylinder;
+ try {
+ removeComment(fileId);
+
+ if (fileId.eof()) { // check if not at the end of the file (for old style files)
+ delete[] caoPoints;
+ delete[] caoLinePoints;
+ return;
+ }
+
+ /* Extract the cylinders */
+ fileId >> caoNbCylinder;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbCylinders += caoNbCylinder;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
+ }
+
+ if (caoNbCylinder > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of cylinders.");
+ }
+
+ for (unsigned int k = 0; k < caoNbCylinder; ++k) {
+ removeComment(fileId);
+
+ double radius;
+ unsigned int indexP1, indexP2;
+ fileId >> indexP1;
+ fileId >> indexP2;
+ fileId >> radius;
+
+ //////////////////////////Read the parameter value if present//////////////////////////
+ //Get the end of the line
+ char buffer[256];
+ fileId.getline(buffer, 256);
+ std::string endLine(buffer);
+ std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
+
+// fileId.ignore(256, '\n'); // skip the rest of the line
+
+ std::string polygonName = "";
+ bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
+ double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
+ if(mapOfParams.find("name") != mapOfParams.end()) {
+ polygonName = mapOfParams["name"];
+ }
+ if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
+ minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
+ }
+ if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ useLod = parseBoolean(mapOfParams["useLod"]);
+ }
+
+ int idRevolutionAxis = idFace;
+ addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
+
+ std::vector<std::vector<vpPoint> > listFaces;
+ createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius,listFaces);
+ addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
+ idFace+=4;
+
+ initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);
+ }
+
+ } catch (...) {
+ std::cerr
+ << "Cannot get the number of cylinders. Defaulting to zero."
+ << std::endl;
+ caoNbCylinder = 0;
+ }
+
+
+ //////////////////////////Read the circle declaration part//////////////////////////
+ unsigned int caoNbCircle;
+ try {
+ removeComment(fileId);
+
+ if (fileId.eof()) { // check if not at the end of the file (for old style files)
+ delete[] caoPoints;
+ delete[] caoLinePoints;
+ return;
+ }
+
+ /* Extract the circles */
+ fileId >> caoNbCircle;
+ fileId.ignore(256, '\n'); // skip the rest of the line
+
+ nbCircles += caoNbCircle;
+ if(verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbCircle << " circles" << std::endl;
+ }
+
+ if (caoNbCircle > 100000) {
+ throw vpException(vpException::badValue,
+ "Exceed the max number of cicles.");
+ }
+
+ for (unsigned int k = 0; k < caoNbCircle; ++k) {
+ removeComment(fileId);
+
+ double radius;
+ unsigned int indexP1, indexP2, indexP3;
+ fileId >> radius;
+ fileId >> indexP1;
+ fileId >> indexP2;
+ fileId >> indexP3;
+
+ //////////////////////////Read the parameter value if present//////////////////////////
+ //Get the end of the line
+ char buffer[256];
+ fileId.getline(buffer, 256);
+ std::string endLine(buffer);
+ std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
+
+// fileId.ignore(256, '\n'); // skip the rest of the line
+
+ std::string polygonName = "";
+ bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
+ double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
+ if(mapOfParams.find("name") != mapOfParams.end()) {
+ polygonName = mapOfParams["name"];
+ }
+ if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
+ }
+ if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ useLod = parseBoolean(mapOfParams["useLod"]);
+ }
+
+ addPolygon(caoPoints[indexP1], caoPoints[indexP2],
+ caoPoints[indexP3], radius, idFace, polygonName, useLod, minPolygonAreaThreshold);
+
+ initCircle(caoPoints[indexP1], caoPoints[indexP2],
+ caoPoints[indexP3], radius, idFace++, polygonName);
+ }
+
+ } catch (...) {
+ std::cerr << "Cannot get the number of circles. Defaulting to zero."
+ << std::endl;
+ caoNbCircle = 0;
+ }
+
+ startIdFace = idFace;
+
+ delete[] caoPoints;
+ delete[] caoLinePoints;
+
+ if(vectorOfModelFilename.size() > 1 && parent) {
+ if(verbose) {
+ std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
+ std::cout << "Total nb of points : " << nbPoints << std::endl;
+ std::cout << "Total nb of lines : " << nbLines << std::endl;
+ std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
+ std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
+ std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
+ std::cout << "Total nb of circles : " << nbCircles << std::endl;
+ } else {
+ std::cout << "> " << nbPoints << " points" << std::endl;
+ std::cout << "> " << nbLines << " lines" << std::endl;
+ std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
+ std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
+ std::cout << "> " << nbCylinders << " cylinders" << std::endl;
+ std::cout << "> " << nbCircles << " circles" << std::endl;
+ }
+ }
+ } catch (...) {
+ std::cerr << "Cannot read line!" << std::endl;
+ throw vpException(vpException::ioError, "cannot read line");
+ }
+}
+
+#ifdef VISP_HAVE_COIN3D
+/*!
+ Extract a VRML object Group.
+
+ \param sceneGraphVRML2 : Current node (either Transform, or Group node).
+ \param transform : Transformation matrix for this group.
+ \param idFace : Index of the face.
+*/
+void
+vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
+{
+ vpHomogeneousMatrix transformCur;
+ SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
+ if(sceneGraphVRML2Trasnform){
+ float rx, ry, rz, rw;
+ sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
+ vpRotationMatrix rotMat(vpQuaternionVector(rx,ry,rz,rw));
+// std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " << rw << std::endl;
+
+ float tx, ty, tz;
+ tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
+ ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
+ tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
+ vpTranslationVector transVec(tx,ty,tz);
+// std::cout << "Translation: " << tx << " " << ty << " " << tz << std::endl;
+
+ float sx, sy, sz;
+ sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
+ sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
+ sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
+// std::cout << "Scale: " << sx << " " << sy << " " << sz << std::endl;
+
+ for(unsigned int i = 0 ; i < 3 ; i++)
+ rotMat[0][i] *= sx;
+ for(unsigned int i = 0 ; i < 3 ; i++)
+ rotMat[1][i] *= sy;
+ for(unsigned int i = 0 ; i < 3 ; i++)
+ rotMat[2][i] *= sz;
+
+ transformCur = vpHomogeneousMatrix(transVec,rotMat);
+ transform = transform * transformCur;
+ }
+
+ int nbShapes = sceneGraphVRML2->getNumChildren();
+// std::cout << sceneGraphVRML2->getTypeId().getName().getString() << std::endl;
+// std::cout << "Nb object in VRML : " << nbShapes << std::endl;
+
+ SoNode * child;
+
+ for (int i = 0; i < nbShapes; i++)
+ {
+ vpHomogeneousMatrix transform_recursive(transform);
+ child = sceneGraphVRML2->getChild(i);
+
+ if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
+ extractGroup((SoVRMLGroup*)child, transform_recursive, idFace);
+ }
+
+ if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
+ extractGroup((SoVRMLTransform*)child, transform_recursive, idFace);
+ }
+
+ if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
+ SoChildList * child2list = child->getChildren();
+ std::string name = child->getName().getString();
+
+ for (int j = 0; j < child2list->getLength(); j++)
+ {
+ if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
+ {
+ SoVRMLIndexedFaceSet * face_set;
+ face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
+ if(!strncmp(face_set->getName().getString(),"cyl",3)){
+ extractCylinders(face_set, transform, idFace, name);
+ }else{
+ extractFaces(face_set, transform, idFace, name);
+ }
+ }
+ if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
+ {
+ SoVRMLIndexedLineSet * line_set;
+ line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
+ extractLines(line_set, idFace, name);
+ }
+ }
+ }
+ }
+}
+
+/*!
+ Extract a face of the object to track from the VMRL model. This method calls
+ the initFaceFromCorners() method implemented in the child class.
+
+ \param face_set : Pointer to the face in the vrml format.
+ \param transform : Transformation matrix applied to the face.
+ \param idFace : Face id.
+ \param polygonName: Name of the polygon.
+*/
+void
+vpMbTracker::extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName)
+{
+ std::vector<vpPoint> corners;
+ corners.resize(0);
+
+// SoMFInt32 indexList = _face_set->coordIndex;
+// int indexListSize = indexList.getNum();
+ int indexListSize = face_set->coordIndex.getNum();
+
+ vpColVector pointTransformed(4);
+ vpPoint pt;
+ SoVRMLCoordinate *coord;
+
+ for (int i = 0; i < indexListSize; i++)
+ {
+ if (face_set->coordIndex[i] == -1)
+ {
+ if(corners.size() > 1)
+ {
+ addPolygon(corners, idFace++, polygonName);
+ initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ corners.resize(0);
+ }
+ }
+ else
+ {
+ coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
+ int index = face_set->coordIndex[i];
+ pointTransformed[0]=coord->point[index].getValue()[0];
+ pointTransformed[1]=coord->point[index].getValue()[1];
+ pointTransformed[2]=coord->point[index].getValue()[2];
+ pointTransformed[3] = 1.0;
+
+ pointTransformed = transform * pointTransformed;
+
+ pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
+ corners.push_back(pt);
+ }
+ }
+}
+
+/*!
+ Extract a cylinder to track from the VMRL model. This method calls
+ the initCylinder() method implemented in the child class.
+
+ \warning This method extract cylinder described using an indexed face set not
+ a cylinder set since software such as AC3D or blender export a cylinder using
+ this data type. the object name is used, if it begins with "cyl" then this
+ method is called otherwise the extractFaces() is used.
+
+ \param face_set : Pointer to the cylinder in the vrml format.
+ \param transform : Transformation matrix applied to the cylinder.
+ \param idFace : Id of the face.
+ \param polygonName: Name of the polygon.
+*/
+void
+vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName)
+{
+ std::vector<vpPoint> corners_c1, corners_c2;//points belonging to the first circle and to the second one.
+ SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
+
+ unsigned int indexListSize = (unsigned int)coords->point.getNum();
+
+ if(indexListSize % 2 == 1){
+ std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
+ throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
+ }
+ corners_c1.resize(indexListSize / 2);
+ corners_c2.resize(indexListSize / 2);
+ vpColVector pointTransformed(4);
+ vpPoint pt;
+
+
+ // extract all points and fill the two sets.
+
+ for(int i=0; i<coords->point.getNum(); ++i){
+ pointTransformed[0]=coords->point[i].getValue()[0];
+ pointTransformed[1]=coords->point[i].getValue()[1];
+ pointTransformed[2]=coords->point[i].getValue()[2];
+ pointTransformed[3] = 1.0;
+
+ pointTransformed = transform * pointTransformed;
+
+ pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
+
+ if(i < (int)corners_c1.size()){
+ corners_c1[(unsigned int)i] = pt;
+ }else{
+ corners_c2[(unsigned int)i-corners_c1.size()] = pt;
+ }
+ }
+
+ vpPoint p1 = getGravityCenter(corners_c1);
+ vpPoint p2 = getGravityCenter(corners_c2);
+
+ vpColVector dist(3);
+ dist[0] = p1.get_oX() - corners_c1[0].get_oX();
+ dist[1] = p1.get_oY() - corners_c1[0].get_oY();
+ dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
+ double radius_c1 = sqrt(dist.sumSquare());
+ dist[0] = p2.get_oX() - corners_c2[0].get_oX();
+ dist[1] = p2.get_oY() - corners_c2[0].get_oY();
+ dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
+ double radius_c2 = sqrt(dist.sumSquare());
+
+ if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))){
+ std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
+ throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
+ }
+
+ //addPolygon(p1, p2, idFace, polygonName);
+ //initCylinder(p1, p2, radius_c1, idFace++);
+
+ int idRevolutionAxis = idFace;
+ addPolygon(p1, p2, idFace++, polygonName);
+
+ std::vector<std::vector<vpPoint> > listFaces;
+ createCylinderBBox(p1, p2, radius_c1, listFaces);
+ addPolygon(listFaces, idFace, polygonName);
+ idFace+=4;
+
+ initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
+}
+
+/*!
+ Extract a line of the object to track from the VMRL model. This method calls
+ the initFaceFromCorners() method implemented in the child class.
+
+ \param line_set : Pointer to the line in the vrml format.
+ \param idFace : Id of the face.
+ \param polygonName: Name of the polygon.
+*/
+void
+vpMbTracker::extractLines(SoVRMLIndexedLineSet* line_set, int &idFace, const std::string &polygonName)
+{
+ std::vector<vpPoint> corners;
+ corners.resize(0);
+
+ int indexListSize = line_set->coordIndex.getNum();
+
+ SbVec3f point(0,0,0);
+ vpPoint pt;
+ SoVRMLCoordinate *coord;
+
+ for (int i = 0; i < indexListSize; i++)
+ {
+ if (line_set->coordIndex[i] == -1)
+ {
+ if(corners.size() > 1)
+ {
+ addPolygon(corners, idFace++, polygonName);
+ initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
+ corners.resize(0);
+ }
+ }
+ else
+ {
+ coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
+ int index = line_set->coordIndex[i];
+ point[0]=coord->point[index].getValue()[0];
+ point[1]=coord->point[index].getValue()[1];
+ point[2]=coord->point[index].getValue()[2];
+
+ pt.setWorldCoordinates(point[0],point[1],point[2]);
+ corners.push_back(pt);
+ }
+ }
+}
+
+#endif // VISP_HAVE_COIN3D
+
+/*!
+ Compute the center of gravity of a set of point. This is used in the cylinder
+ extraction to find the center of the circles.
+
+ \throw vpException::dimensionError if the set is empty.
+
+ \param pts : Set of point to extract the center of gravity.
+ \return Center of gravity of the set.
+*/
+vpPoint
+vpMbTracker::getGravityCenter(const std::vector<vpPoint>& pts)
+{
+ if(pts.empty()){
+ std::cout << "Cannot extract center of gravity of empty set." << std::endl;
+ throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
+ }
+ double oX = 0;
+ double oY = 0;
+ double oZ = 0;
+ vpPoint G;
+
+ for(unsigned int i=0; i<pts.size(); ++i){
+ oX += pts[i].get_oX();
+ oY += pts[i].get_oY();
+ oZ += pts[i].get_oZ();
+ }
+
+ G.setWorldCoordinates(oX/pts.size(), oY/pts.size(), oZ/pts.size());
+ return G;
+}
+
+/*!
+ Get the list of polygons faces (a vpPolygon representing the projection of the face in the image and a list of face corners
+ in 3D), with the possibility to order by distance to the camera or to use the visibility check to consider if the polygon
+ face must be retrieved or not.
+
+ \param orderPolygons : If true, the resulting list is ordered from the nearest polygon faces to the farther.
+ \param useVisibility : If true, only visible faces will be retrieved.
+ \return A pair object containing the list of vpPolygon and the list of face corners.
+ */
+std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
+vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility)
+{
+ //Temporary variable to permit to order polygons by distance
+ std::vector<vpPolygon> polygonsTmp;
+ std::vector<std::vector<vpPoint> > roisPtTmp;
+
+ //Pair containing the list of vpPolygon and the list of face corners
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
+
+ for (unsigned int i = 0; i < getNbPolygon(); i++) {
+ std::vector<vpImagePoint> roi;
+ std::vector<vpPoint> roiPt;
+ //A face has at least three points
+ if (getPolygon(i)->nbpt >= 3) {
+ if((useVisibility && getPolygon(i)->isvisible) || !useVisibility) {
+ for (unsigned int j = 0; j < getPolygon(i)->nbpt; j++) {
+ vpPoint pt(getPolygon(i)->p[j]);
+ pt.project(cMo);
+ double u = 0, v = 0;
+ vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
+ roi.push_back(vpImagePoint(v, u));
+ roiPt.push_back(pt);
+ }
+
+ polygonsTmp.push_back(vpPolygon(roi));
+ roisPtTmp.push_back(roiPt);
+ }
+ }
+ }
+
+ if(orderPolygons) {
+ //Order polygons by distance (near to far)
+ std::vector<PolygonFaceInfo> listOfPolygonFaces;
+ for(unsigned int i = 0; i < polygonsTmp.size(); i++) {
+ double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
+ for(unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
+ x_centroid += roisPtTmp[i][j].get_X();
+ y_centroid += roisPtTmp[i][j].get_Y();
+ z_centroid += roisPtTmp[i][j].get_Z();
+ }
+
+ x_centroid /= roisPtTmp[i].size();
+ y_centroid /= roisPtTmp[i].size();
+ z_centroid /= roisPtTmp[i].size();
+
+ double squared_dist = x_centroid*x_centroid + y_centroid*y_centroid + z_centroid*z_centroid;
+ listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
+ }
+
+ //Sort the list of polygon faces
+ std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
+
+ polygonsTmp.resize(listOfPolygonFaces.size());
+ roisPtTmp.resize(listOfPolygonFaces.size());
+
+ size_t cpt = 0;
+ for(std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
+ ++it, cpt++) {
+ polygonsTmp[cpt] = it->polygon;
+ roisPtTmp[cpt] = it->faceCorners;
+ }
+
+ pairOfPolygonFaces.first = polygonsTmp;
+ pairOfPolygonFaces.second = roisPtTmp;
+ } else {
+ pairOfPolygonFaces.first = polygonsTmp;
+ pairOfPolygonFaces.second = roisPtTmp;
+ }
+
+ return pairOfPolygonFaces;
+}
+
+/*!
+ Use Ogre3D for visibility tests
+
+ \warning This function has to be called before the initialization of the tracker.
+
+ \param v : True to use it, False otherwise
+*/
+void
+vpMbTracker::setOgreVisibilityTest(const bool &v)
+{
+ useOgre = v;
+ if(useOgre){
+#ifndef VISP_HAVE_OGRE
+ useOgre = false;
+ std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
+#endif
+ }
+}
+
+/*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+*/
+void
+vpMbTracker::setFarClippingDistance(const double &dist)
+{
+ if( (clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
+ vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
+ else if ( dist < 0 )
+ vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
+ else{
+ clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);
+ distFarClip = dist;
+ for (unsigned int i = 0; i < faces.size(); i ++){
+ faces[i]->setFarClippingDistance(distFarClip);
+ }
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setFarClippingDistance(distFarClip);
+#endif
+ }
+}
+
+/*!
+ Set the flag to consider if the level of detail (LOD) is used.
+
+ \param useLod : true if the level of detail must be used, false otherwise. When true,
+ two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
+ \param name : name of the face we want to modify the LOD parameter.
+
+ \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
+ */
+void
+vpMbTracker::setLod(const bool useLod, const std::string &name)
+{
+ for (unsigned int i = 0; i < faces.size(); i++)
+ {
+ if(name.empty() || faces[i]->name == name) {
+ faces[i]->setLod(useLod);
+ }
+ }
+}
+
+/*!
+ Set the threshold for the minimum line length to be considered as visible in the LOD case.
+
+ \param minLineLengthThresh : threshold for the minimum line length in pixel.
+ \param name : name of the face we want to modify the LOD threshold.
+
+ \sa setLod(), setMinPolygonAreaThresh()
+ */
+void
+vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
+{
+ for (unsigned int i = 0; i < faces.size(); i++)
+ {
+ if(name.empty() || faces[i]->name == name) {
+ faces[i]->setMinLineLengthThresh(minLineLengthThresh);
+ }
+ }
+}
+
+/*!
+ Set the minimum polygon area to be considered as visible in the LOD case.
+
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
+ \param name : name of the face we want to modify the LOD threshold.
+
+ \sa setLod(), setMinLineLengthThresh()
+ */
+void
+vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
+{
+ for (unsigned int i = 0; i < faces.size(); i++)
+ {
+ if(name.empty() || faces[i]->name == name) {
+ faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
+ }
+ }
+}
+
+/*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+*/
+void
+vpMbTracker::setNearClippingDistance(const double &dist)
+{
+ if( (clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
+ vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
+ else if ( dist < 0 )
+ vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
+ else{
+ clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);
+ distNearClip = dist;
+ for (unsigned int i = 0; i < faces.size(); i ++){
+ faces[i]->setNearClippingDistance(distNearClip);
+ }
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setNearClippingDistance(distNearClip);
+#endif
+ }
+}
+
+/*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param flags : New clipping flags.
+*/
+void
+vpMbTracker::setClipping(const unsigned int &flags)
+{
+ clippingFlag = flags;
+ for (unsigned int i = 0; i < faces.size(); i ++)
+ faces[i]->setClipping(clippingFlag);
+}
+
+/*!
+ Compute \f$ J^T R \f$, with J the interaction matrix and R the vector of
+ residu.
+
+ \throw vpMatrixException::incorrectMatrixSizeError if the sizes of the
+ matrices do not allow the computation.
+
+ \warning The JTR vector is resized.
+
+ \param interaction : The interaction matrix (size Nx6).
+ \param error : The residu vector (size Nx1).
+ \param JTR : The resulting JTR column vector (size 6x1).
+
+*/
+void
+vpMbTracker::computeJTR(const vpMatrix& interaction, const vpColVector& error, vpColVector& JTR)
+{
+ if(interaction.getRows() != error.getRows() || interaction.getCols() != 6 ){
+ throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
+ "Incorrect matrices size in computeJTR.");
+ }
+
+ JTR.resize(6);
+ const unsigned int N = interaction.getRows();
+
+ for (unsigned int i = 0; i < 6; i += 1){
+ double ssum = 0;
+ for (unsigned int j = 0; j < N; j += 1){
+ ssum += interaction[j][i] * error[j];
+ }
+ JTR[i] = ssum;
+ }
+}
+
+/*!
+ Get a 1x6 vpColVector representing the estimated degrees of freedom.
+ vpColVector[0] = 1 if translation on X is estimated, 0 otherwise;
+ vpColVector[1] = 1 if translation on Y is estimated, 0 otherwise;
+ vpColVector[2] = 1 if translation on Z is estimated, 0 otherwise;
+ vpColVector[3] = 1 if rotation on X is estimated, 0 otherwise;
+ vpColVector[4] = 1 if rotation on Y is estimated, 0 otherwise;
+ vpColVector[5] = 1 if rotation on Z is estimated, 0 otherwise;
+
+ \return 1x6 vpColVector representing the estimated degrees of freedom.
+*/
+vpColVector
+vpMbTracker::getEstimatedDoF()
+{
+ vpColVector v(6);
+ for(unsigned int i = 0 ; i < 6 ; i++)
+ v[i] = oJo[i][i];
+ return v;
+}
+
+/*!
+ Set a 1x6 vpColVector representing the estimated degrees of freedom. The vector has to be this form:
+ vpColVector[0] = 1 if translation on X is estimated, 0 otherwise;
+ vpColVector[1] = 1 if translation on Y is estimated, 0 otherwise;
+ vpColVector[2] = 1 if translation on Z is estimated, 0 otherwise;
+ vpColVector[3] = 1 if rotation on X is estimated, 0 otherwise;
+ vpColVector[4] = 1 if rotation on Y is estimated, 0 otherwise;
+ vpColVector[5] = 1 if rotation on Z is estimated, 0 otherwise;
+
+*/
+void
+vpMbTracker::setEstimatedDoF(const vpColVector& v)
+{
+ if(v.getRows() == 6)
+ {
+ isoJoIdentity = true;
+ for(unsigned int i = 0 ; i < 6 ; i++){
+ // if(v[i] != 0){
+ if(std::fabs(v[i]) > std::numeric_limits<double>::epsilon()){
+ oJo[i][i] = 1.0;
+ }
+ else{
+ oJo[i][i] = 0.0;
+ isoJoIdentity = false;
+ }
+ }
+ }
+}
+
+
+void
+vpMbTracker::createCylinderBBox(const vpPoint& p1, const vpPoint &p2, const double &radius, std::vector<std::vector<vpPoint> > &listFaces)
+{
+ listFaces.clear();
+
+// std::vector<vpPoint> revolutionAxis;
+// revolutionAxis.push_back(p1);
+// revolutionAxis.push_back(p2);
+// listFaces.push_back(revolutionAxis);
+
+ vpColVector axis(3);
+ axis[0] = p1.get_oX() - p2.get_oX();
+ axis[1] = p1.get_oY() - p2.get_oY();
+ axis[2] = p1.get_oZ() - p2.get_oZ();
+
+ vpColVector randomVec(3);
+ randomVec = 0;
+
+ vpColVector axisOrtho(3);
+
+ randomVec[0] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
+
+ if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
+ {
+ randomVec = 0;
+ randomVec[1] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
+ if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
+ {
+ randomVec = 0;
+ randomVec[2] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
+ if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
+ }
+ }
+
+ axisOrtho.normalize();
+
+ vpColVector axisOrthoBis(3);
+ axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
+ axisOrthoBis.normalize();
+
+ //First circle
+ vpColVector p1Vec(3);
+ p1Vec[0] = p1.get_oX();
+ p1Vec[1] = p1.get_oY();
+ p1Vec[2] = p1.get_oZ();
+ vpColVector fc1 = p1Vec + axisOrtho*radius;
+ vpColVector fc2 = p1Vec + axisOrthoBis*radius;
+ vpColVector fc3 = p1Vec - axisOrtho*radius;
+ vpColVector fc4 = p1Vec - axisOrthoBis*radius;
+
+ vpColVector p2Vec(3);
+ p2Vec[0] = p2.get_oX();
+ p2Vec[1] = p2.get_oY();
+ p2Vec[2] = p2.get_oZ();
+ vpColVector sc1 = p2Vec + axisOrtho*radius;
+ vpColVector sc2 = p2Vec + axisOrthoBis*radius;
+ vpColVector sc3 = p2Vec - axisOrtho*radius;
+ vpColVector sc4 = p2Vec - axisOrthoBis*radius;
+
+ std::vector<vpPoint> pointsFace;
+ pointsFace.push_back( vpPoint(fc1[0], fc1[1], fc1[2]) );
+ pointsFace.push_back( vpPoint(sc1[0], sc1[1], sc1[2]) );
+ pointsFace.push_back( vpPoint(sc2[0], sc2[1], sc2[2]) );
+ pointsFace.push_back( vpPoint(fc2[0], fc2[1], fc2[2]) );
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back( vpPoint(fc2[0], fc2[1], fc2[2]) );
+ pointsFace.push_back( vpPoint(sc2[0], sc2[1], sc2[2]) );
+ pointsFace.push_back( vpPoint(sc3[0], sc3[1], sc3[2]) );
+ pointsFace.push_back( vpPoint(fc3[0], fc3[1], fc3[2]) );
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back( vpPoint(fc3[0], fc3[1], fc3[2]) );
+ pointsFace.push_back( vpPoint(sc3[0], sc3[1], sc3[2]) );
+ pointsFace.push_back( vpPoint(sc4[0], sc4[1], sc4[2]) );
+ pointsFace.push_back( vpPoint(fc4[0], fc4[1], fc4[2]) );
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back( vpPoint(fc4[0], fc4[1], fc4[2]) );
+ pointsFace.push_back( vpPoint(sc4[0], sc4[1], sc4[2]) );
+ pointsFace.push_back( vpPoint(sc1[0], sc1[1], sc1[2]) );
+ pointsFace.push_back( vpPoint(fc1[0], fc1[1], fc1[2]) );
+ listFaces.push_back(pointsFace);
+}
+
diff --git a/modules/tracker/mbt/src/vpMbXmlParser.cpp b/modules/tracker/mbt/src/vpMbXmlParser.cpp
new file mode 100644
index 0000000..bec3d8a
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbXmlParser.cpp
@@ -0,0 +1,386 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Load XML Parameter for Model Based Tracker.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <map>
+
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+
+#include <visp3/mbt/vpMbXmlParser.h>
+
+
+/*!
+ Default constructor.
+
+*/
+vpMbXmlParser::vpMbXmlParser()
+ : cam(), angleAppear(70), angleDisappear(80),
+ hasNearClipping(false), nearClipping(false),
+ hasFarClipping(false), farClipping(false), fovClipping(false),
+ useLod(false), minLineLengthThreshold(50.0), minPolygonAreaThreshold(2500.0)
+
+{
+ init();
+}
+
+/*!
+ Default destructor.
+*/
+vpMbXmlParser::~vpMbXmlParser()
+{
+}
+
+/*!
+ Initialise internal variables (including the map).
+*/
+void
+vpMbXmlParser::init()
+{
+ setMainTag("conf");
+
+ nodeMap["conf"] = conf;
+ nodeMap["face"] = face;
+ nodeMap["angle_appear"] = angle_appear;
+ nodeMap["angle_disappear"] = angle_disappear;
+ nodeMap["near_clipping"] = near_clipping;
+ nodeMap["far_clipping"] = far_clipping;
+ nodeMap["fov_clipping"] = fov_clipping;
+ nodeMap["camera"] = camera;
+ nodeMap["height"] = height;
+ nodeMap["width"] = width;
+ nodeMap["u0"] = u0;
+ nodeMap["v0"] = v0;
+ nodeMap["px"] = px;
+ nodeMap["py"] = py;
+ nodeMap["lod"] = lod;
+ nodeMap["use_lod"] = use_lod;
+ nodeMap["min_line_length_threshold"] = min_line_length_threshold;
+ nodeMap["min_polygon_area_threshold"] = min_polygon_area_threshold;
+}
+
+/*!
+ Parse the file in parameters.
+ This method is deprecated, use parse() instead.
+
+ \param filename : File to parse.
+*/
+void
+vpMbXmlParser::parse(const char * filename)
+{
+ std::string file = filename;
+ vpXmlParser::parse(file);
+}
+
+/*!
+ Write info to file.
+
+ \warning Useless, so not yet implemented => Throw exception.
+*/
+void
+vpMbXmlParser::writeMainClass(xmlNodePtr /*node*/)
+{
+ throw vpException(vpException::notImplementedError, "Not yet implemented." );
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void
+vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool camera_node = false;
+ bool face_node = false;
+ bool lod_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case camera:{
+ this->read_camera (doc, dataNode);
+ camera_node = true;
+ }break;
+ case face:{
+ this->read_face(doc, dataNode);
+ face_node = true;
+ }break;
+ case lod:{
+ this->read_lod(doc, dataNode);
+ lod_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!camera_node) {
+ std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+ std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ }
+
+ if(!face_node) {
+ std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
+ std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ }
+
+ if(!lod_node) {
+ std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
+ std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
+ }
+}
+
+/*!
+ Read camera information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void
+vpMbXmlParser::read_camera (xmlDocPtr doc, xmlNodePtr node)
+{
+ bool u0_node = false;
+ bool v0_node = false;
+ bool px_node = false;
+ bool py_node = false;
+
+ // current data values.
+ double d_u0 = this->cam.get_u0();
+ double d_v0 = this->cam.get_v0();
+ double d_px = this->cam.get_px();
+ double d_py = this->cam.get_py();
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case u0:{
+ d_u0 = xmlReadDoubleChild(doc, dataNode);
+ u0_node = true;
+ }break;
+ case v0:{
+ d_v0 = xmlReadDoubleChild(doc, dataNode);
+ v0_node = true;
+ }break;
+ case px:{
+ d_px = xmlReadDoubleChild(doc, dataNode);
+ px_node = true;
+ }break;
+ case py:{
+ d_py = xmlReadDoubleChild(doc, dataNode);
+ py_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ this->cam.initPersProjWithoutDistortion(d_px, d_py, d_u0, d_v0) ;
+
+ if(!u0_node)
+ std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+ else
+ std::cout << "camera : u0 : "<< this->cam.get_u0() <<std::endl;
+
+ if(!v0_node)
+ std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ else
+ std::cout << "camera : v0 : "<< this->cam.get_v0() <<std::endl;
+
+ if(!px_node)
+ std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ else
+ std::cout << "camera : px : "<< this->cam.get_px() <<std::endl;
+
+ if(!py_node)
+ std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ else
+ std::cout << "camera : py : "<< this->cam.get_py() <<std::endl;
+}
+
+/*!
+ Read face information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void
+vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool angle_appear_node = false;
+ bool angle_disappear_node = false;
+ bool near_clipping_node = false;
+ bool far_clipping_node = false;
+ bool fov_clipping_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case angle_appear:{
+ angleAppear = xmlReadDoubleChild(doc, dataNode);
+ angle_appear_node = true;
+ }break;
+ case angle_disappear:{
+ angleDisappear = xmlReadDoubleChild(doc, dataNode);
+ angle_disappear_node = true;
+ }break;
+ case near_clipping:{
+ nearClipping = xmlReadDoubleChild(doc, dataNode);
+ near_clipping_node = true;
+ hasNearClipping = true;
+ }break;
+ case far_clipping:{
+ farClipping = xmlReadDoubleChild(doc, dataNode);
+ far_clipping_node = true;
+ hasFarClipping = true;
+ }break;
+ case fov_clipping:{
+ if (xmlReadIntChild(doc, dataNode))
+ fovClipping = true;
+ else
+ fovClipping = false;
+ fov_clipping_node = true;
+ }break;
+ default:{
+// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!angle_appear_node)
+ std::cout << "face : Angle Appear : "<< angleAppear << " (default)" <<std::endl;
+ else
+ std::cout << "face : Angle Appear : "<< angleAppear <<std::endl;
+
+ if(!angle_disappear_node)
+ std::cout << "face : Angle Disappear : "<< angleDisappear << " (default)" <<std::endl;
+ else
+ std::cout << "face : Angle Disappear : "<< angleDisappear <<std::endl;
+
+ if(near_clipping_node)
+ std::cout << "face : Near Clipping : "<< nearClipping <<std::endl;
+
+ if(far_clipping_node)
+ std::cout << "face : Far Clipping : "<< farClipping <<std::endl;
+
+ if(fov_clipping_node) {
+ if(fovClipping)
+ std::cout << "face : Fov Clipping : True" <<std::endl;
+ else
+ std::cout << "face : Fov Clipping : False" <<std::endl;
+ }
+}
+
+void
+vpMbXmlParser::read_lod (xmlDocPtr doc, xmlNodePtr node) {
+ bool use_lod_node = false;
+ bool min_line_length_threshold_node = false;
+ bool min_polygon_area_threshold_node = false;
+
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case use_lod:
+ useLod = (xmlReadIntChild(doc, dataNode) != 0);
+ use_lod_node = true;
+ break;
+ case min_line_length_threshold:
+ minLineLengthThreshold = xmlReadDoubleChild(doc, dataNode);
+ min_line_length_threshold_node = true;
+ break;
+ case min_polygon_area_threshold:
+ minPolygonAreaThreshold = xmlReadDoubleChild(doc, dataNode);
+ min_polygon_area_threshold_node = true;
+ break;
+ default:{
+// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
+ }break;
+ }
+ }
+ }
+ }
+
+ if(!use_lod_node)
+ std::cout << "lod : use lod : " << useLod << " (default)" <<std::endl;
+ else
+ std::cout << "lod : use lod : " << useLod << std::endl;
+
+ if(!min_line_length_threshold_node)
+ std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<" (default)" <<std::endl;
+ else
+ std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<std::endl;
+
+ if(!min_polygon_area_threshold_node)
+ std::cout <<"lod : min polygon area threshold : " << minPolygonAreaThreshold <<" (default)" <<std::endl;
+ else
+ std::cout <<"lod : min polygon area threshold : " << minPolygonAreaThreshold <<std::endl;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbXmlParser.cpp.o) has no symbols
+void dummy_vpMbXmlParser() {};
+#endif
+
diff --git a/modules/tracker/mbt/src/vpMbtPolygon.cpp b/modules/tracker/mbt/src/vpMbtPolygon.cpp
new file mode 100644
index 0000000..6c75bcd
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbtPolygon.cpp
@@ -0,0 +1,285 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Make the complete tracking of an object by using its CAD model
+ *
+ * Authors:
+ * Nicolas Melchior
+ * Romain Tallonneau
+ * Eric Marchand
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <limits.h>
+
+#include <visp3/core/vpConfig.h>
+/*!
+ \file vpMbtPolygon.cpp
+ \brief Implements a polygon of the model used by the model-based tracker.
+*/
+
+#include <visp3/mbt/vpMbtPolygon.h>
+#include <visp3/core/vpPolygon.h>
+
+/*!
+ Basic constructor.
+*/
+vpMbtPolygon::vpMbtPolygon()
+ : index(-1), isvisible(false), isappearing(false),
+ useLod(false), minLineLengthThresh(50.0), minPolygonAreaThresh(2500.0), name(""),
+ hasOrientation(true)
+{
+}
+
+vpMbtPolygon::vpMbtPolygon(const vpMbtPolygon& mbtp)
+ : vpPolygon3D(mbtp), index(mbtp.index), isvisible(mbtp.isvisible), isappearing(mbtp.isappearing),
+ useLod(mbtp.useLod), minLineLengthThresh(mbtp.minLineLengthThresh), minPolygonAreaThresh(mbtp.minPolygonAreaThresh), name(mbtp.name),
+ hasOrientation(mbtp.hasOrientation)
+{
+ //*this = mbtp; // Should not be called by copy contructor to avoid multiple assignements.
+}
+
+vpMbtPolygon& vpMbtPolygon::operator=(const vpMbtPolygon& mbtp)
+{
+ vpPolygon3D::operator=(mbtp);
+ index = mbtp.index;
+ isvisible = mbtp.isvisible;
+ isappearing = mbtp.isappearing;
+ useLod = mbtp.useLod;
+ minLineLengthThresh = mbtp.minLineLengthThresh;
+ minPolygonAreaThresh = mbtp.minPolygonAreaThresh;
+ name = mbtp.name;
+ hasOrientation = mbtp.hasOrientation;
+
+ return (*this);
+}
+
+/*!
+ Basic destructor.
+*/
+vpMbtPolygon::~vpMbtPolygon()
+{
+}
+
+/*!
+ Check if the polygon is visible in the image and if the angle between the normal
+ to the face and the line vector going from the optical center to the cog of the face is below
+ the given threshold.
+ To do that, the polygon is projected into the image thanks to the camera pose.
+
+ \param cMo : The pose of the camera.
+ \param alpha : Maximum angle to detect if the face is visible (in rad).
+ \param modulo : Indicates if the test should also consider faces that are not oriented
+ counter clockwise. If true, the orientation of the face is without importance.
+ \param cam : Camera parameters (intrinsics parameters)
+ \param I : Image used to consider level of detail.
+
+ \return Return true if the polygon is visible.
+*/
+bool
+vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
+ const vpCameraParameters &cam, const vpImage<unsigned char> &I)
+{
+ // std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
+
+ changeFrame(cMo);
+
+ if(nbpt <= 2) {
+ //Level of detail (LOD)
+ if(useLod) {
+ vpCameraParameters c = cam;
+ if(clippingFlag > 3) { // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+ }
+ computePolygonClipped(c);
+ std::vector<vpImagePoint> roiImagePoints;
+ getRoiClipped(c, roiImagePoints);
+
+ if (roiImagePoints.size() == 2) {
+ double x1 = roiImagePoints[0].get_u();
+ double y1 = roiImagePoints[0].get_v();
+ double x2 = roiImagePoints[1].get_u();
+ double y2 = roiImagePoints[1].get_v();
+ double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+// std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
+// vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);
+
+ if (length < minLineLengthThresh) {
+ isvisible = false;
+ isappearing = false;
+ return false;
+ }
+ }
+ }
+
+ /* a line is always visible when LOD is not used */
+ isvisible = true;
+ isappearing = false;
+ return true ;
+ }
+
+ // If the polygon has no orientation, the angle check visibility is always valid.
+ // Feature mainly used for cylinders.
+ if(!hasOrientation)
+ {
+ isvisible = true;
+ isappearing = false;
+ return true;
+ }
+
+ //Check visibility from normal
+ //Newell's Method for calculating the normal of an arbitrary 3D polygon
+ //https://www.opengl.org/wiki/Calculating_a_Surface_Normal
+ vpColVector faceNormal(3);
+ vpColVector currentVertex, nextVertex;
+ for(unsigned int i = 0; i<nbpt; i++) {
+ currentVertex = p[i].cP;
+ nextVertex = p[(i+1) % nbpt].cP;
+
+ faceNormal[0] += (currentVertex[1] - nextVertex[1]) * (currentVertex[2] + nextVertex[2]);
+ faceNormal[1] += (currentVertex[2] - nextVertex[2]) * (currentVertex[0] + nextVertex[0]);
+ faceNormal[2] += (currentVertex[0] - nextVertex[0]) * (currentVertex[1] + nextVertex[1]);
+ }
+ faceNormal.normalize();
+
+ vpColVector e4(3) ;
+ vpPoint pt;
+ for (unsigned int i = 0; i < nbpt; i += 1){
+ pt.set_X(pt.get_X() + p[i].get_X());
+ pt.set_Y(pt.get_Y() + p[i].get_Y());
+ pt.set_Z(pt.get_Z() + p[i].get_Z());
+ }
+ e4[0] = -pt.get_X() / (double)nbpt;
+ e4[1] = -pt.get_Y() / (double)nbpt;
+ e4[2] = -pt.get_Z() / (double)nbpt;
+ e4.normalize();
+
+ double angle = acos(vpColVector::dotProd(e4, faceNormal));
+
+// vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;
+
+ if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
+ isvisible = true;
+ isappearing = false;
+
+ if (useLod) {
+ vpCameraParameters c = cam;
+ if(clippingFlag > 3) { // Contains at least one FOV constraint
+ c.computeFov(I.getWidth(), I.getHeight());
+ }
+ computePolygonClipped(c);
+ std::vector<vpImagePoint> roiImagePoints;
+ getRoiClipped(c, roiImagePoints);
+
+ vpPolygon roiPolygon(roiImagePoints);
+ double area = roiPolygon.getArea();
+// std::cout << "After normal test ; Index=" << index << " ; area=" << area << " ; clippingFlag="
+// << clippingFlag << std::endl;
+ if (area < minPolygonAreaThresh) {
+ isappearing = false;
+ isvisible = false;
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ if (angle < alpha+vpMath::rad(1) ){
+ isappearing = true;
+ }
+ else if (modulo && (M_PI - angle) < alpha+vpMath::rad(1) ){
+ isappearing = true;
+ }
+ else {
+ isappearing = false;
+ }
+
+ isvisible = false ;
+ return false ;
+}
+
+//###################################
+// Static functions
+//###################################
+
+/*!
+ Set the flag to consider if the level of detail (LOD) is used or not.
+ When activated, lines and faces of the 3D model are tracked if respectively their
+ projected lenght and area in the image are significative enough. By significative, we mean:
+ - if the lenght of the projected line in the image is greater that a threshold set by
+ setMinLineLengthThresh()
+ - if the area of the projected face in the image is greater that a threshold set by
+ setMinPolygonAreaThresh().
+
+ \param use_lod : true if level of detail must be used, false otherwise.
+
+ The sample code below shows how to introduce this feature:
+ \code
+#include <visp/vpMbEdgeTracker.h>
+#include <visp/vpImageIo.h>
+
+int main()
+{
+vpImage<unsigned char> I;
+
+// Acquire an image
+vpImageIo::read(I, "my-image.pgm");
+
+std::string object = "my-object";
+vpMbEdgeTracker tracker;
+tracker.loadConfigFile( object+".xml" );
+tracker.loadModel( object+".cao" );
+
+tracker.setLod(true);
+tracker.setMinLineLengthThresh(20.);
+tracker.setMinPolygonAreaThresh(20.*20.);
+
+tracker.initClick(I, object+".init" );
+
+while (true) {
+ // tracking loop
+}
+vpXmlParser::cleanup();
+
+return 0;
+}
+ \endcode
+
+ \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
+ */
+void
+vpMbtPolygon::setLod(const bool use_lod)
+{
+ this->useLod = use_lod;
+}
+
+
diff --git a/modules/tracker/me/CMakeLists.txt b/modules/tracker/me/CMakeLists.txt
new file mode 100644
index 0000000..726c0a1
--- /dev/null
+++ b/modules/tracker/me/CMakeLists.txt
@@ -0,0 +1,42 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(me visp_core)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+vp_add_tests(DEPENDS_ON visp_io visp_gui)
diff --git a/modules/tracker/me/include/visp3/me/vpMe.h b/modules/tracker/me/include/visp3/me/vpMe.h
new file mode 100644
index 0000000..22f3518
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMe.h
@@ -0,0 +1,301 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ * Andrew Comport
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMe.h
+ \brief Moving edges
+*/
+
+#ifndef vpMe_H
+#define vpMe_H
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpImage.h>
+
+/*!
+ \class vpMe
+ \ingroup module_me
+
+ This class defines predetermined masks for sites and holds moving edges
+ tracking parameters.
+ */
+class VISP_EXPORT vpMe
+{
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+#else
+private:
+#endif
+ double threshold ;//! Likelihood ratio threshold
+ double mu1; //! Contrast continuity parameter (left boundary)
+ double mu2; //! Contrast continuity parameter (right boundary)
+ double min_samplestep;
+ unsigned int anglestep;
+ int mask_sign;
+ unsigned int range; //! Seek range - on both sides of the reference pixel
+ double sample_step; //! Distance between sampled points (in pixels)
+ int ntotal_sample;
+ int points_to_track;
+ //! convolution masks' size in pixels (masks are square), \warning should not be public, use setMaskSize() and getMaskSize() instead (kept public for compatibility reasons).
+ unsigned int mask_size;
+ //! the number of convolution masks available for tracking ; defines resolution. \warning Should not be public, use setMaskNumber() and getMaskNumber() instead (kept public for compatibility reasons).
+ unsigned int n_mask;
+ //strip: defines a "security strip" such that Seek_Extremities()
+ //cannot return a new extremity which is too close to the
+ //frame borders which may cause Get_Sampling_Grid to refuse
+ //the that extremity
+ int strip;
+ //int graph ;
+ vpMatrix *mask ; //! Array of matrices defining the different masks (one for every angle step).
+
+public:
+ vpMe() ;
+ vpMe(const vpMe &me) ;
+ virtual ~vpMe() ;
+
+ const vpMe& operator=(const vpMe &me);
+
+
+ void initMask() ;// convolution masks - offset computation
+ void checkSamplestep(double &a) { if(a < min_samplestep) a = min_samplestep ; }
+ void print( ) ;
+
+ /*!
+ Get the matrix of the mask.
+
+ \return the value of mask.
+ */
+ inline vpMatrix* getMask() const { return mask; }
+
+ /*!
+ Set the number of mask applied to determine the object contour. The number of mask determines the precision of
+ the normal of the edge for every sample. If precision is 2deg, then there
+ are 360/2 = 180 masks.
+
+ \param a : the number of mask.
+ */
+ void setMaskNumber(const unsigned int &a) ;
+
+ /*!
+ Return the number of mask applied to determine the object contour. The number of mask determines the precision of
+ the normal of the edge for every sample. If precision is 2deg, then there
+ are 360/2 = 180 masks.
+
+ \return the current number of mask.
+ */
+ inline unsigned int getMaskNumber() const { return n_mask; }
+
+ /*!
+ Set the mask sign.
+
+ \param a : new mask sign.
+ */
+ void setMaskSign(const int &a){ mask_sign = a ; }
+
+ /*!
+ Return the mask sign.
+
+ \return Value of mask_sign.
+ */
+ inline int getMaskSign() const { return mask_sign; }
+
+ /*!
+ Set the mask size (in pixel) used to compute the image gradient and determine the object contour.
+ The mask size defines the size of the convolution mask used to detect an edge.
+
+ \param a : new mask size.
+ */
+ void setMaskSize(const unsigned int &a);
+
+ /*!
+ Return the actual mask size (in pixel) used to compute the image gradient and determine the object contour.
+ The mask size defines the size of the convolution mask used to detect an edge.
+
+ \return the current mask size.
+ */
+ inline unsigned int getMaskSize() const { return mask_size; }
+
+ /*!
+ Set the minimum image contrast allowed to detect a contour.
+
+ \param mu_1 : new mu1.
+ */
+ void setMu1(const double &mu_1) { this->mu1 = mu_1 ; }
+
+ /*!
+ Get the minimum image contrast allowed to detect a contour.
+
+ \return Value of mu1.
+ */
+ inline double getMu1() const { return mu1; }
+
+ /*!
+ Set the maximum image contrast allowed to detect a contour.
+
+ \param mu_2 : new mu2.
+ */
+ void setMu2(const double &mu_2) { this->mu2 = mu_2 ; }
+
+ /*!
+ Get the maximum image contrast allowed to detect a contour.
+
+ \return Value of mu2.
+ */
+ inline double getMu2() const { return mu2; }
+
+ /*!
+ Set how many discretizied points are used to track the feature.
+
+ \param nb : new total number of sample.
+ */
+ void setNbTotalSample(const int &nb) { ntotal_sample = nb; }
+
+ /*!
+ Get how many discretizied points are used to track the feature.
+
+ \return Value of ntotal_sample.
+ */
+ inline int getNbTotalSample() const { return ntotal_sample; }
+
+ /*!
+ Set the number of points to track.
+
+ \param n : new number of points to track.
+
+ \warning This method is useful only for the vpMeNurbsTracker.
+ */
+ void setPointsToTrack(const int &n) { points_to_track = n; }
+
+ /*!
+ Return the number of points to track.
+
+ \return Value of points_to_track.
+ */
+ inline int getPointsToTrack() const { return points_to_track ; }
+
+ /*!
+ Set the seek range on both sides of the reference pixel.
+
+ \param r : new range.
+ */
+ void setRange(const unsigned int &r) { range = r ; }
+
+ /*!
+ Return the seek range on both sides of the reference pixel.
+
+ \return Value of range.
+ */
+ inline unsigned int getRange() const { return range; }
+
+ /*!
+ Set the angle step.
+
+ \param a : new angle step.
+ */
+ void setAngleStep(const unsigned int &a) { anglestep = a ; }
+
+ /*!
+ Return the angle step.
+
+ \return Value of anglestep.
+ */
+ inline unsigned int getAngleStep() const { return anglestep; }
+
+ /*!
+ Set the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
+
+ \param min : new minimum sample step.
+ */
+ void setMinSampleStep(const double &min) { min_samplestep = min ; }
+
+ /*!
+ Get the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
+
+ \return Value of min_samplestep.
+ */
+ inline double getMinSampleStep() const { return min_samplestep; }
+
+ /*!
+ Set the minimum distance in pixel between two discretized points.
+
+ \param s : new sample_step.
+ */
+ void setSampleStep(const double &s) { sample_step = s ; }
+
+ /*!
+ Get the minimum distance in pixel between two discretized points.
+
+ \return Value of sample_step.
+ */
+ inline double getSampleStep() const { return sample_step ; }
+
+ /*!
+ Set the number of pixels that are ignored around the image borders.
+
+ \param a : new strip.
+ */
+ void setStrip(const int &a) { strip = a ; }
+
+ /*!
+ Get the number of pixels that are ignored around the image borders.
+
+ \return the value of strip.
+ */
+ inline int getStrip() const { return strip; }
+
+ /*!
+ Set the likelihood threshold used to determined if the moving edge is valid or not.
+
+ \param t : new threshold.
+ */
+ void setThreshold(const double &t) { threshold = t ; }
+
+ /*!
+ Return the likelihood threshold used to determined if the moving edge is valid or not.
+
+ \return Value of threshold.
+ */
+ inline double getThreshold() const { return threshold; }
+};
+
+
+#endif
+
+
diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h
new file mode 100644
index 0000000..e6290df
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h
@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeEllipse.h
+ \brief Moving edges on an ellipse
+*/
+
+#ifndef vpMeEllipse_HH
+#define vpMeEllipse_HH
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/core/vpImagePoint.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
+
+#include <math.h>
+#include <list>
+
+/*!
+ \class vpMeEllipse
+ \ingroup module_me
+
+ \brief Class that tracks an ellipse moving edges.
+
+ In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
+
+ \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
+
+ If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
+
+ The five parameters are stored in the public attribute K.
+
+ An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
+ major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
+
+ \image html vpMeEllipse.gif
+ \image latex vpMeEllipse.ps width=10cm
+
+ It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
+
+ \f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
+ \f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
+
+ Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
+
+ \image html vpMeEllipse2.gif
+ \image latex vpMeEllipse2.ps width=10cm
+
+ The example below available in tutorial-me-ellipse-tracker.cpp and described
+ in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
+
+ \include tutorial-me-ellipse-tracker.cpp
+
+*/
+class VISP_EXPORT vpMeEllipse : public vpMeTracker
+{
+public:
+ vpMeEllipse() ;
+ vpMeEllipse(const vpMeEllipse &meellipse) ;
+ virtual ~vpMeEllipse() ;
+
+ /*!
+ \return Expected number of moving edges to track along the ellipse.
+ */
+ int getExpectedDensity() {return (int)expecteddensity;};
+ void track(const vpImage<unsigned char>& Im);
+
+ void initTracking(const vpImage<unsigned char> &I) ;
+ void initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint* iP);
+ void initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p, double low_alpha, double high_alpha) ;
+ void display(const vpImage<unsigned char>&I, vpColor col) ;
+ void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
+ void printParameters() ;
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ void initTracking(const vpImage<unsigned char> &I, const unsigned int n,
+ unsigned *i, unsigned *j) ;
+ //@}
+#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
+
+ /*!
+ Gets the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
+
+ \return the value of \f$ m_{00} \f$.
+ */
+ inline double get_m00() const {return m00;}
+
+ /*!
+ Gets the 1 order raw moment \f$ m_{10} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
+
+ \return the value of \f$ m_{10} \f$.
+ */
+ inline double get_m10() const {return m10;}
+
+ /*!
+ Gets the 1 order raw moment \f$ m_{01} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
+
+ \return the value of \f$ m_{01} \f$.
+ */
+ inline double get_m01() const {return m01;}
+
+ /*!
+ Gets the 2 order raw moment \f$ m_{11} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
+
+ \return the value of \f$ m_{11} \f$.
+ */
+ inline double get_m11() const {return m11;}
+
+ /*!
+ Gets the 2 order raw moment \f$ m_{20} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
+
+ \return the value of \f$ m_{11} \f$.
+ */
+ inline double get_m20() const {return m20;}
+
+ /*!
+ Gets the 2 order raw moment \f$ m_{02} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
+
+ \return the value of \f$ m_{11} \f$.
+ */
+ inline double get_m02() const {return m02;}
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{11} \f$.
+
+ \return the value of \f$ \mu_{11} \f$.
+ */
+ inline double get_mu11() const {return mu11;}
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{02} \f$.
+
+ \return the value of \f$ \mu_{02} \f$.
+ */
+ inline double get_mu02() const {return mu02;}
+
+ /*!
+ Gets the 2 order central moment \f$ \mu_{20} \f$.
+
+ \return the value of \f$ \mu_{20} \f$.
+ */
+ inline double get_mu20() const {return mu20;}
+
+ /*!
+ Gets the center of the ellipse.
+ */
+ inline vpImagePoint getCenter() const {return iPc; }
+
+ /*!
+ Gets the semiminor axis of the ellipse.
+ */
+ inline double getA() const {return a; }
+
+ /*!
+ Gets the semimajor axis of the ellipse.
+ */
+ inline double getB() const {return b; }
+
+ /*!
+ Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+ */
+ inline double getE() const {return e; }
+
+ /*!
+ Gets the equation parameters of the ellipse
+ */
+ void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
+
+ /*!
+ Gets the smallest \f$ alpha \f$ angle
+ */
+ inline double getSmallestAngle() { return alpha1; }
+
+ /*!
+ Gets the highest \f$ alpha \f$ angle
+ */
+ inline double getHighestAngle() { return alpha2; }
+
+ /*!
+ Set the new threshold for the robust estimation of the parameters of the
+ ellipse equation.
+ 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(const double threshold){
+ if(threshold<0){
+ thresholdWeight = 0;
+ }else if(threshold>1){
+ thresholdWeight = 1;
+ }else{
+ thresholdWeight = threshold;
+ }
+ }
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+#else
+protected:
+#endif
+ /*! Parameters of the ellipse to define the set of points that satisfy the implicit equation :
+ \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
+ */
+ vpColVector K ;
+ //! The coordinates of the ellipse center.
+ vpImagePoint iPc;
+ //! \f$ a \f$ is the semiminor axis of the ellipse.
+ double a;
+ //! \f$ b \f$ is the semimajor axis of the ellipse.
+ double b;
+ //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
+ double e;
+
+protected:
+ //! The coordinates of the point corresponding to the smallest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
+ vpImagePoint iP1;
+ //! The coordinates of the point corresponding to the highest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
+ vpImagePoint iP2;
+ //! The smallest \f$ alpha \f$ angle.
+ double alpha1 ;
+ //! The highest \f$ alpha \f$ angle.
+ double alpha2 ;
+ //! Value of cos(e).
+ double ce;
+ //! Value of sin(e).
+ double se;
+ //! Stores the value of the \f$ alpha \f$ angle for each vpMeSite.
+ std::list<double> angle;
+ //! Surface
+ double m00;
+ //! Second order central moments
+ double mu11,mu20, mu02;
+ //! First order raw moments
+ double m10,m01;
+ //! Second order raw moments
+ double m11,m02,m20;
+ //! Threshold for the robust least square.
+ double thresholdWeight;
+ //! Expected number of me to track along the ellipse.
+ double expecteddensity;
+
+private:
+ void computeAngle(vpImagePoint pt1, vpImagePoint pt2);
+ void sample(const vpImage<unsigned char>&image);
+ void reSample(const vpImage<unsigned char> &I) ;
+ void leastSquare() ;
+ void updateTheta();
+ void suppressPoints() ;
+ void seekExtremities(const vpImage<unsigned char> &I) ;
+ void setExtremities();
+ void getParameters() ;
+ void computeMoments();
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ void computeAngle(int ip1, int jp1,int ip2, int jp2) ;
+ void computeAngle(int ip1, int jp1, double &alpha1,
+ int ip2, int jp2, double &alpha2) ;
+ /*!
+ \deprecated This function does nothing and shouldn't be used since we modify
+ the code to detect automatically if the object to track becomes a circle
+ rather an ellipse.
+ */
+ vp_deprecated void setCircle(bool ) { }
+
+ //@}
+#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
+
+//Static Function
+public:
+ static void display(const vpImage<unsigned char>& 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);
+ static void display(const vpImage<vpRGBa>& 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);
+
+};
+
+#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeLine.h b/modules/tracker/me/include/visp3/me/vpMeLine.h
new file mode 100644
index 0000000..99309ba
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMeLine.h
@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeLine.h
+ \brief Moving edges on a line
+*/
+
+#ifndef vpMeLine_HH
+#define vpMeLine_HH
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/me/vpMeTracker.h>
+
+#include <math.h>
+#include <iostream>
+
+/*!
+ \class vpMeLine
+
+ \ingroup module_me
+
+ \brief Class that tracks in an image a line moving edges.
+
+ In this class the line is defined by its equation in the \f$ (i,j) =
+ (line,column) \f$ image plane. Two kinds of parametrization are available to
+ describe a 2D line. The first one corresponds to the following
+ equation
+
+ \f[ ai + bj + c = 0 \f]
+
+ where \f$ i \f$ and \f$ j \f$ are the coordinates of the points
+ belonging to the line. The line features are \f$ (a, b, c) \f$.
+
+ The second way to write the line equation is to consider polar coordinates
+ \f[ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f]
+
+ where \f$ i \f$ and \f$ j \f$ are still the coordinates of the
+ points belonging to the line. But now the line features are \f$
+ (\rho, \theta) \f$. The computation of \f$ \rho \f$ and \f$ \theta
+ \f$ is easy thanks to \f$ (a, b, c) \f$.
+
+ \f[ \theta = arctan(b/a) \f]
+ \f[ \rho = -c/\sqrt{a^2+b^2} \f]
+
+ The value of \f$ \theta \f$ is between \f$ 0 \f$ and \f$ 2\pi
+ \f$. And the value of \f$ \rho \f$ can be positive or negative. The
+ conventions to find the right values of the two features are
+ illustrated in the following pictures.
+
+ \image html vpMeLine.gif
+ \image latex vpMeLine.ps width=10cm
+
+ The angle \f$\theta\f$ is computed thanks to the direction of the
+ arrow. The arrow points to the side of the line which is darker.
+
+ The example below available in tutorial-me-line-tracker.cpp and described
+ in \ref tutorial-tracking-me shows how to use this class.
+
+ \include tutorial-me-line-tracker.cpp
+*/
+
+/*
+ The code below shows how to use this class.
+\code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320);
+
+ // Fill the image with a black rectangle
+ I = 0;
+ for (int i = 100; i < 180; i ++) {
+ for (int j = 120; j < 250; j ++) {
+ I[i][j] = 255;
+ }
+ }
+
+ // Set the moving-edges tracker parameters
+ vpMe me;
+ me.setRange(25);
+ me.setThreshold(15000);
+ me.setSampleStep(10);
+
+ // Initialize the moving-edges line tracker parameters
+ vpMeLine line;
+ line.setMe(&me);
+
+ // Initialize the location of the vertical line to track
+ vpImagePoint ip1, ip2; // Two points belonging to the line to track
+ ip1.set_i( 120 );
+ ip1.set_j( 119 );
+ ip2.set_i( 170 );
+ ip2.set_j( 122 );
+
+ line.initTracking(I, ip1, ip2);
+
+ while ( 1 )
+ {
+ // ... Here the code to read or grab the next image.
+
+ // Track the line.
+ line.track(I);
+ }
+ return 0;
+}
+\endcode
+
+ \note It is possible to display the line as an overlay. For that you
+ must use the display function of the class vpMeLine.
+*/
+
+class VISP_EXPORT vpMeLine : public vpMeTracker
+{
+private:
+ static void update_indices(double theta,int incr,int i,int j,int& i1,int& i2,int& j1,int& j2);
+
+protected:
+ vpMeSite PExt[2] ;
+
+ double rho, theta ;
+ double delta ,delta_1;
+ double angle, angle_1;
+ int sign;
+
+ //! Flag to specify wether the intensity of the image at the middle point is used to compute the sign of rho or not.
+ bool _useIntensityForRho;
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+#else
+protected:
+#endif
+
+ double a; //!< Parameter a of the line equation a*i + b*j + c = 0
+ double b; //!< Parameter b of the line equation a*i + b*j + c = 0
+ double c; //!< Parameter c of the line equation a*i + b*j + c = 0
+
+public:
+ vpMeLine() ;
+ vpMeLine(const vpMeLine &meline);
+ virtual ~vpMeLine() ;
+
+ void display(const vpImage<unsigned char>& I, vpColor col) ;
+
+ void track(const vpImage<unsigned char>& Im);
+
+ void sample(const vpImage<unsigned char>&image);
+ void reSample(const vpImage<unsigned char> &I) ;
+ void leastSquare() ;
+ void updateDelta();
+ void setExtremities() ;
+ void seekExtremities(const vpImage<unsigned char> &I) ;
+ void suppressPoints() ;
+
+ void initTracking(const vpImage<unsigned char> &I) ;
+ void initTracking(const vpImage<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2) ;
+
+ void computeRhoTheta(const vpImage<unsigned char> &I) ;
+ double getRho() const ;
+ double getTheta() const ;
+ void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) ;
+
+ /*!
+ Gets the equation parameters of the line
+ */
+ void getEquationParam(double &A, double &B, double &C) { A = a; B = b; C = c; }
+
+ /*!
+ Gets parameter a of the line equation a*i + b*j + c = 0
+ */
+ inline double getA() const {return a; }
+
+ /*!
+ Gets parameter b of the line equation a*i + b*j + c = 0
+ */
+ inline double getB() const {return b; }
+
+ /*!
+ Gets parameter c of the line equation a*i + b*j + c = 0
+ */
+ inline double getC() const {return c; }
+
+ static bool intersection(const vpMeLine &line1, const vpMeLine &line2,
+ vpImagePoint &ip);
+
+ /*!
+ This method allows to turn off the computation of the sign of the rho
+ attribute based on the intensity near the middle point of the line. This is
+ usually done to distinguish between a black/white and a white/black edge but
+ it may be source of problem (ex. for a servoing example) when this point can
+ be occluded.
+
+ \param useIntensityForRho : new value of the flag.
+ */
+ inline void computeRhoSignFromIntensity(const bool useIntensityForRho){
+ _useIntensityForRho = useIntensityForRho;
+ }
+
+//Static Functions
+public:
+ static void display(const vpImage<unsigned char>& 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);
+ static void display(const vpImage<vpRGBa>& 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);
+
+ static void display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list,
+ const double &A, const double &B, const double &C,
+ const vpColor &color = vpColor::green, unsigned int thickness=1);
+ static void display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &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
new file mode 100644
index 0000000..7ca6344
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMeNurbs.h
@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeNurbs.h
+ \brief Moving edges on a form reprsented by a NURBS (Non Uniform Rational B-Spline)
+*/
+
+#ifndef vpMeNurbs_HH
+#define vpMeNurbs_HH
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/me/vpNurbs.h>
+
+#include <math.h>
+#include <iostream>
+#include <list>
+
+/*!
+ \class vpMeNurbs
+
+ \ingroup module_me
+
+ \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.
+
+ It is possible to have a direct access to the nurbs. It is indeed a public parameter.
+
+ The code below shows how to use this class.
+\code
+#include <visp3/core/vpImage.h>
+#include <visp3/me/vpMeNurbs.h>
+#include <visp3/core/vpImagePoint.h>
+
+int main()
+{
+ vpImage<unsigned char> I(240, 320);
+
+ // Fill the image with a black rectangle
+ I = 0;
+ for (int i = 100; i < 180; i ++) {
+ for (int j = 0; j < 320; j ++) {
+ I[i][j] = 255;
+ }
+ }
+
+ // Set the moving-edges tracker parameters
+ vpMe me;
+ me.setRange(25);
+ me.setPointsToTrack(20);
+ me.setThreshold(15000);
+ me.setSampleStep(10);
+
+ // Initialize the moving-edges tracker parameters
+ vpMeNurbs meNurbs;
+ meNurbs.setNbControlPoints(4);
+ meNurbs.setMe(&me);
+
+ // Initialize the location of the edge to track (here a horizontal line
+ std::list<vpImagePoint> ipList; //List of points belonginig to the edge
+ ipList.push_back(vpImagePoint(110,119));
+ ipList.push_back(vpImagePoint(140,119));
+ ipList.push_back(vpImagePoint(160,119));
+ ipList.push_back(vpImagePoint(170,119));
+
+ meNurbs.initTracking(I, ipList);
+
+ while ( 1 )
+ {
+ // ... Here the code to read or grab the next image.
+
+ // Track the line.
+ meNurbs.track(I);
+ }
+ return 0;
+}
+\endcode
+
+ \note It is possible to display the nurbs as an overlay. For that you
+ must use the display function of the class vpMeNurbs.
+
+ \note In case of an edge which is not smooth, it can be interesting to use the
+ canny detection to find the extremities. In this case, use the method
+ setEnableCannyDetection to enable it. Warning : This function requires OpenCV.
+*/
+
+class VISP_EXPORT vpMeNurbs : public vpMeTracker
+{
+ #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ public:
+ #else
+ private:
+ #endif
+ //! The Nurbs which represents the tracked edge.
+ vpNurbs nurbs;
+
+ private:
+ //! Size of the nurbs
+ double dist;
+ //! Number of control points used to compute the Nurbs.
+ unsigned int nbControlPoints;
+ //! counter used to trigger the canny edge detection at the beginning of the Nurbs.
+ int beginPtFound;
+ //! counter used to trigger the canny edge detection at the end of the Nurbs.
+ int endPtFound;
+ //! True if the canny detection has to be used during the extremities search.
+ bool enableCannyDetection;
+ //! First canny threshold
+ double cannyTh1;
+ //! Second canny threshold
+ double cannyTh2;
+
+ public:
+ vpMeNurbs();
+ vpMeNurbs(const vpMeNurbs &menurbs);
+ virtual ~vpMeNurbs();
+
+ /*!
+ 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(const 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.
+ */
+ 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;
+ */
+ void setCannyThreshold(const double th1, const double th2)
+ {
+ this->cannyTh1 = th1;
+ this->cannyTh2 = th2;
+ }
+
+ void initTracking(const vpImage<unsigned char> &I) ;
+ void initTracking(const vpImage<unsigned char> &I,
+ const std::list<vpImagePoint> &ptList) ;
+
+ void track(const vpImage<unsigned char>& Im);
+
+ void sample(const vpImage<unsigned char>&image);
+ void reSample(const vpImage<unsigned char> &I) ;
+ void updateDelta();
+ void setExtremities() ;
+ void seekExtremities(const vpImage<unsigned char> &I) ;
+ void seekExtremitiesCanny(const vpImage<unsigned char> &I) ;
+ void suppressPoints() ;
+
+ void supressNearPoints();
+ void localReSample(const vpImage<unsigned char> &I);
+
+ /*!
+ Gets the nurbs;
+ */
+ inline vpNurbs getNurbs() const {return nurbs; }
+
+ void display(const vpImage<unsigned char>& I, vpColor col) ;
+
+ private:
+ bool computeFreemanChainElement(const vpImage<unsigned char> &I,
+ vpImagePoint &iP,
+ unsigned int &element);
+
+ bool hasGoodLevel(const vpImage<unsigned char>& I,
+ const vpImagePoint& iP) const;
+
+ bool isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const;
+
+ void computeFreemanParameters( unsigned int element, vpImagePoint &diP);
+
+ bool farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP);
+
+public:
+ static void display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color = vpColor::green);
+ static void display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color = vpColor::green);
+};
+
+#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h
new file mode 100644
index 0000000..63d2396
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMeSite.h
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ * Andrew Comport
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeSite.h
+ \brief Moving edges
+*/
+
+
+
+#ifndef vpMeSite_H
+#define vpMeSite_H
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/me/vpMe.h>
+
+/*!
+ \class vpMeSite
+ \ingroup module_me
+
+ \brief Performs search in a given direction(normal) for a given
+ distance(pixels) for a given 'site'. Gives the most likely site
+ given the probablility from an ME mask
+
+ - Bug fix: rewrote application of masks to use the temporal
+ information instead of applying both temporal masks to the same
+ image. ie: spatial -> spatio/temporal
+
+ - Added new tracking function to choose the most similar edge
+ amongst all edges found.
+
+ - sample step.
+ */
+class VISP_EXPORT vpMeSite
+{
+public:
+ typedef enum
+ {
+ NONE,
+ RANGE,
+ RESULT,
+ RANGE_RESULT
+ } vpMeSiteDisplayType;
+
+ typedef enum
+ {
+ NO_SUPPRESSION = 0,
+ CONSTRAST = 1,
+ THRESHOLD = 2,
+ M_ESTIMATOR = 3,
+ TOO_NEAR = 4,
+ UNKNOW = 5
+ } vpMeSiteState;
+
+public:
+ int i,j ;
+ int i_1, j_1 ;
+ double ifloat, jfloat ;
+ unsigned char v ;
+ int mask_sign ;
+ // Angle of tangent at site
+ double alpha;
+ // Convolution of Site in previous image
+ double convlt ;
+ // Convolution of Site in previous image
+ double normGradient ;
+ // Uncertainty of point given as a probability between 0 and 1
+ double weight;
+
+private:
+ vpMeSiteDisplayType selectDisplay ;
+ vpMeSiteState state;
+
+public:
+ void init() ;
+ void init(double ip, double jp, double alphap) ;
+ void init(double ip, double jp, double alphap, double convltp) ;
+ void init(double ip, double jp, double alphap, double convltp, int sign) ;
+
+ vpMeSite () ;
+ vpMeSite(double ip, double jp) ;
+ vpMeSite (const vpMeSite &mesite) ;
+ virtual ~vpMeSite() {} ;
+
+ void display(const vpImage<unsigned char>& I);
+
+ double convolution(const vpImage<unsigned char>& ima, const vpMe *me) ;
+
+ vpMeSite *getQueryList(const vpImage<unsigned char> &I, const int range) ;
+
+ void track(const vpImage<unsigned char>& im,
+ const vpMe *me,
+ const bool test_contraste=true);
+
+ /*!
+ 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
+ */
+ inline double getAlpha() const { return alpha; }
+
+
+ void setDisplay(vpMeSiteDisplayType select) { selectDisplay = select ; }
+
+ /*!
+ 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
+ */
+ inline int get_j() const { return j; }
+
+ /*!
+ 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
+ */
+ inline double get_jfloat() const { return jfloat; }
+
+ /*!
+ Set the state of the site
+
+ \param flag : flag corresponding to vpMeSiteState
+
+ \sa vpMeSiteState
+ */
+ void setState(const vpMeSiteState &flag){
+ state = flag;
+
+ #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ suppress = (int)flag;
+ #endif
+ }
+
+ /*!
+ 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
+ */
+ void setWeight(const double &w) { weight = w; }
+
+ /*!
+ Get the weight of the site
+
+ \return value of weight
+ */
+ inline double getWeight() const { return weight; }
+
+//Operators
+ vpMeSite &operator=(const vpMeSite &m) ;
+ int operator!=(const vpMeSite &m) ;
+
+ friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS);
+
+//Static functions
+ /*!
+ Compute the distance \f$ |S1 - S2| = \sqrt{(i_1-i_2)^2+(j_1-j_2)^2} \f$
+
+ \param S1 : First site
+ \param S2 : Second site
+
+ \return the distance between the two sites.
+ */
+ static double distance (const vpMeSite S1, const vpMeSite S2) {
+ return(sqrt(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat)));}
+
+ /*!
+ Compute the distance \f$ |S1 - S2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
+
+ \param S1 : First site
+ \param S2 : Second site
+
+ \return the distance between the two sites.
+ */
+ static double sqrDistance (const vpMeSite S1, const vpMeSite S2) {
+ return(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat));}
+
+ static void display(const vpImage<unsigned char>& I, const double &i, const double &j,
+ const vpMeSiteState &state = NO_SUPPRESSION);
+ static void display(const vpImage<vpRGBa>& I, const double &i, const double &j,
+ const vpMeSiteState &state = NO_SUPPRESSION);
+
+//Deprecated
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+ //! Flag to indicate whether point is rejected or not
+ //! 1 = contrast, 2 = threshold, 3 = M-estimator, 0 = nosupp
+ int suppress;
+
+ vp_deprecated void getSign(const vpImage<unsigned char> &I, const int range) ;
+#endif
+} ;
+
+#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeTracker.h b/modules/tracker/me/include/visp3/me/vpMeTracker.h
new file mode 100644
index 0000000..289fb84
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpMeTracker.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Andrew Comport
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeTracker.h
+ \brief Contains abstract elements for a Distance to Feature type feature.
+*/
+
+#ifndef vpMeTracker_HH
+#define vpMeTracker_HH
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/core/vpTracker.h>
+
+#include <math.h>
+#include <iostream>
+#include <list>
+
+/*!
+ \class vpMeTracker
+
+ \ingroup module_me
+ \brief Contains abstract elements for a Distance to Feature type feature.
+
+ 2D state = list of points, 3D state = feature
+*/
+class VISP_EXPORT vpMeTracker : public vpTracker
+{
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+#else
+protected:
+#endif
+ //! Tracking dependent variables/functions
+ //! List of tracked moving edges points.
+ std::list<vpMeSite> list ;
+ //! Moving edges initialisation parameters
+ vpMe *me ;
+ unsigned int init_range;
+ int nGoodElement;
+
+protected:
+ vpMeSite::vpMeSiteDisplayType selectDisplay ;
+
+public:
+ // Constructor/Destructor
+ vpMeTracker() ;
+ vpMeTracker(const vpMeTracker& meTracker) ;
+ virtual ~vpMeTracker() ;
+
+ void init() ;
+ void initTracking(const vpImage<unsigned char>& I);
+
+ //! Track sampled pixels.
+ void track(const vpImage<unsigned char>& I);
+
+ unsigned int numberOfSignal() ;
+ unsigned int totalNumberOfSignal() ;
+
+ virtual void display(const vpImage<unsigned char> &I, vpColor col)=0;
+ virtual void display(const vpImage<unsigned char>& I);
+ void display(const vpImage<unsigned char>& I, vpColVector &w, unsigned int &index_w);
+
+ void setDisplay(vpMeSite::vpMeSiteDisplayType select) {
+ selectDisplay = select ;
+ }
+
+ vpMeTracker& operator =(vpMeTracker& f);
+
+ int outOfImage( int i , int j , int half , int rows , int cols) ;
+ int outOfImage( vpImagePoint iP , int half , int rows , int cols) ;
+
+ void reset();
+
+ //!Sample pixels at a given interval
+ virtual void sample(const vpImage<unsigned char> &image)=0;
+
+
+ /*!
+ Set the initial range.
+
+ \param r : initial range.
+ */
+ void setInitRange(const unsigned int &r) { init_range = r; }
+
+ /*!
+ Return the initial range.
+
+ \return Value of init_range.
+ */
+ inline unsigned int getInitRange() { return init_range; }
+
+ /*!
+ Set the moving edges initialisation parameters
+
+ \param p_me : Moving Edges.
+ */
+ void setMe(vpMe *p_me) { this->me = p_me ; }
+
+ /*!
+ Return the moving edges initialisation parameters
+
+ \return Moving Edges.
+ */
+ inline vpMe* getMe(){ return me; }
+
+ /*!
+ Set the list of moving edges
+
+ \param l : list of Moving Edges.
+ */
+ void setMeList(const std::list<vpMeSite> &l) { list = l; }
+
+ /*!
+ Return the list of moving edges
+
+ \return List of Moving Edges.
+ */
+ inline std::list<vpMeSite>& getMeList() { return list; }
+ inline std::list<vpMeSite> getMeList() const { return list; }
+
+ /*!
+ Return the number of points that has not been suppressed.
+
+ \return Number of good points.
+ */
+ inline int getNbPoints() const { return nGoodElement; }
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+ int query_range;
+ bool display_point;// if 1 (TRUE) displays the line that is being tracked
+#endif
+};
+
+
+#endif
+
+
diff --git a/modules/tracker/me/include/visp3/me/vpNurbs.h b/modules/tracker/me/include/visp3/me/vpNurbs.h
new file mode 100644
index 0000000..7d95e17
--- /dev/null
+++ b/modules/tracker/me/include/visp3/me/vpNurbs.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * This class implements the Non Uniform Rational B-Spline (NURBS)
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#ifndef vpNurbs_H
+#define vpNurbs_H
+
+/*!
+ \file vpNurbs.h
+ \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
+*/
+
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/core/vpBSpline.h>
+#include <visp3/core/vpList.h>
+
+#include <list>
+
+/*!
+ \class vpNurbs
+ \ingroup module_me
+
+ \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
+
+ The different parameters are :
+
+ - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1} i = 0, ...,m \f$.
+ To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
+
+ - The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
+ \f[ N_{i,0}(u) = \left\{\begin{array}{cc}
+ 1 & \mbox{if } u_i \leq u_{i+1} \\ 0 & else
+ \end{array}\right.\f]
+
+ \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
+
+ where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
+
+ - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
+
+ - The weight \f$ {w_i} \f$ associated to each control points.The wheights value is upper than 0.
+
+ It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
+ \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]
+
+ You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
+*/
+
+class VISP_EXPORT vpNurbs : public vpBSpline
+{
+protected:
+ std::vector<double> weights; //Vector which contains the weights associated to each control Points
+
+
+protected:
+ static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ vpMatrix computeCurveDers(double u, unsigned int der);
+
+public:
+
+ vpNurbs();
+ vpNurbs(const vpNurbs &nurbs);
+ virtual ~vpNurbs();
+
+ /*!
+ Gets all the weights relative to the control points.
+
+ \return list : A std::list containing weights relative to the control points.
+ */
+ inline void get_weights(std::list<double>& list) const {
+ list.clear();
+ for (unsigned int i = 0; i < weights.size(); i++)
+ list.push_back(*(&(weights[0])+i));
+ }
+
+ /*!
+ Sets all the knots.
+
+ \param list : A std::list containing the value of the knots.
+ */
+ inline void set_weights(const std::list<double> &list) {
+ weights.clear();
+ for(std::list<double>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ weights.push_back(*it);
+ }
+ }
+
+ static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ vpImagePoint computeCurvePoint(double u);
+
+ static vpImagePoint* computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ vpImagePoint* computeCurveDersPoint(double u, unsigned int der);
+
+ static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ void curveKnotIns(double u, unsigned int s = 0, unsigned int r = 1);
+
+ static void refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ void refineKnotVectCurve(double* x, unsigned int r);
+
+ 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL);
+
+ static void globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ void globalCurveInterp(vpList<vpMeSite>& l_crossingPoints);
+ void globalCurveInterp(const std::list<vpImagePoint>& l_crossingPoints);
+ void globalCurveInterp(const std::list<vpMeSite>& l_crossingPoints);
+ void globalCurveInterp();
+
+ static void globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ void globalCurveApprox(vpList<vpMeSite>& l_crossingPoints, unsigned int n);
+ void globalCurveApprox(const std::list<vpImagePoint>& l_crossingPoints, unsigned int n);
+ void globalCurveApprox(const std::list<vpMeSite>& l_crossingPoints, unsigned int n);
+ void globalCurveApprox(unsigned int n);
+};
+
+#endif
diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp
new file mode 100644
index 0000000..c75e0cb
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMe.cpp
@@ -0,0 +1,476 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ * Andrew Comport
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMe.cpp
+ \brief Moving edges
+*/
+
+
+
+#include <visp3/me/vpMe.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <stdlib.h>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+struct point
+{
+ double x ;
+ double y ;
+} ;
+
+struct droite
+{
+ double a ;
+ double b ;
+ double c ;
+} ;
+
+
+
+
+
+
+
+template <class Type>
+inline void
+permute(Type & a, Type & b)
+{
+ Type t = a;
+ a = b;
+ b = t;
+}
+
+static droite
+droite_cartesienne(point P, point Q)
+{
+ droite PQ;
+
+ PQ.a = P.y-Q.y;
+ PQ.b = Q.x-P.x;
+ PQ.c = Q.y*P.x - Q.x*P.y;
+
+ return(PQ);
+}
+
+
+
+static point
+point_intersection(droite D1, droite D2)
+{
+ point I;
+ double det; // determinant des 2 vect.normaux
+
+ det = (D1.a*D2.b - D2.a*D1.b); // interdit D1,D2 paralleles
+ I.x = (D2.c*D1.b - D1.c*D2.b)/det;
+ I.y = (D1.c*D2.a - D2.c*D1.a)/det;
+
+ return(I);
+}
+
+static void
+recale(point & P,
+ double Xmin, double Ymin, double Xmax, double Ymax)
+{
+ if(vpMath::equal(P.x,Xmin))
+ P.x=Xmin; // a peu pres => exactement !
+ if(vpMath::equal(P.x,Xmax))
+ P.x=Xmax;
+
+ if(vpMath::equal(P.y,Ymin))
+ P.y=Ymin;
+ if(vpMath::equal(P.y,Ymax))
+ P.y=Ymax;
+}
+
+
+static void
+permute(point &A, point &B)
+{
+ point C ;
+
+ if (A.x>B.x) // fonction sans doute a tester...
+ {
+ C = A ;
+ A = B ;
+ B = C ;
+ }
+}
+
+
+// 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
+{
+ droite AB, D[4];
+ D[0].a = 1; D[0].b = 0; D[0].c = -Xmin;
+ D[1].a = 1; D[1].b = 0; D[1].c = -Xmax;
+ D[2].a = 0; D[2].b = 1; D[2].c = -Ymin;
+ D[3].a = 0; D[3].b = 1; D[3].c = -Ymax;
+
+ point P[2];
+ P[0]=A; P[1]=B;
+ int code_P[2], // codes de P[n]
+ i, bit_i, // i -> (0000100...)
+ n;
+
+ AB = droite_cartesienne(A,B);
+
+ for ( ; ; ) // 2 sorties directes internes
+ {
+ // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
+ // ================================================
+ for(n=0; n<2; n++)
+ {
+ code_P[n] = 0000;
+
+ if( P[n].x < Xmin )
+ code_P[n] |= 1; // positionne bit0
+ if( P[n].x > Xmax )
+ code_P[n] |= 2; // .. bit1
+ if( P[n].y < Ymin )
+ code_P[n] |= 4; // .. bit2
+ if( P[n].y > Ymax )
+ code_P[n] |= 8; // .. bit3
+ }
+
+
+ // 2 CAS OU L'ON PEUT CONCLURE => sortie
+ // =====================================
+ if((code_P[0] | code_P[1])==0000) // Aucun bit a 1
+ /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
+ { if(P[0].x < P[1].x) // Rend le couple de points
+ { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
+ else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
+ */
+ {
+ Ac=P[0]; Bc=P[1];
+ if(vpMath::equal(Ac.x,Bc.x) && vpMath::equal(Ac.y,Bc.y))
+ return(false); // AB = 1 point = invisible
+ else
+ return(true); // Partie de AB clippee visible!
+ }
+
+ if((code_P[0] & code_P[1])!=0000) // au moins 1 bit commun
+ {
+ return(false); // AB completement invisible!
+ }
+
+
+ // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
+ // - clippe le point P[n] qui sort de la fenetre (coupe Droite i)
+ // - reboucle avec le nouveau couple de points
+ // ================================================================
+ if(code_P[0] != 0000)
+ {
+ n=0; // c'est P[0] qu'on clippera
+ for(i=0,bit_i=1; !(code_P[0] & bit_i); i++,bit_i<<=1){;}
+ }
+ else
+ {
+ n=1; // c'est P[1] qu'on clippera
+ for(i=0,bit_i=1; !(code_P[1] & bit_i); i++,bit_i<<=1){;}
+ }
+
+ P[n] = point_intersection(AB,D[i]); // clippe le point concerne
+
+
+ // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
+ // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
+ // =======================================================
+ recale(P[n], Xmin,Ymin,Xmax,Ymax);
+
+ }
+
+}
+
+
+// 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)
+{
+
+ if(Q.x < P.x) // tri le couple de points
+ permute(P,Q); // selon leur abscisse x
+
+
+
+ recale(P, Xmin,Ymin,Xmax,Ymax); // permet des calculs de S_relative
+ recale(Q, Xmin,Ymin,Xmax,Ymax); // moins approximatifs.
+
+ //if(P.x==Xmin && Q.x==Xmax)
+ if((std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon())
+ &&
+ (std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon()))
+ return( fabs(Ymax+Ymin-P.y-Q.y) );
+
+ //if( (P.y==Ymin && Q.y==Ymax) ||
+ // (Q.y==Ymin && P.y==Ymax))
+ if( ( (std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
+ &&
+ (std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) )
+ ||
+ ( (std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
+ &&
+ (std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) )
+ return( fabs(Xmax+Xmin-P.x-Q.x) );
+
+ //if( P.x==Xmin && Q.y==Ymax )
+ if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
+ &&
+ std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon() )
+ return( 1-(Ymax-P.y)*(Q.x-Xmin) );
+ //if( P.x==Xmin && Q.y==Ymin )
+ if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
+ &&
+ std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon() )
+ return( 1-(P.y-Ymin)*(Q.x-Xmin) );
+ //if( P.y==Ymin && Q.x==Xmax )
+ if( std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()
+ && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon() )
+ return( 1-(Xmax-P.x)*(Q.y-Ymin) );
+ //if( P.y==Ymax && Q.x==Xmax )
+ if( std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()
+ && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon())
+ return( 1-(Xmax-P.x)*(Ymax-Q.y) );
+
+
+ printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n",
+ P.x,P.y,Q.x,Q.y,Xmin,Ymin,Xmax,Ymax);
+ exit(-1); // DEBUG Stoppe net l'execution
+}
+
+
+static void
+calcul_masques(vpColVector &angle, // definitions des angles theta
+ unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
+ vpMatrix *M) // resultat M[theta](n,n)
+{
+ // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)
+
+ unsigned int i_theta, // indice (boucle sur les masques)
+ i,j; // indices de boucle sur M(i,j)
+ double X,Y, // point correspondant/centre du masque
+ theta, cos_theta, sin_theta, tan_theta,
+ moitie = ((double)n)/2.0; // moitie REELLE du masque
+ point P1,Q1,P,Q; // clippe Droite(theta) P1,Q1 -> P,Q
+ int sgn; // signe de M(i,j)
+ double v; // ponderation de M(i,j)
+
+ unsigned int nb_theta = angle.getRows() ;
+
+ for(i_theta=0; i_theta<nb_theta; i_theta++)
+ {
+ theta = M_PI/180*angle[i_theta]; // indice i -> theta(i) en radians
+ // angle[] dans [0,180[
+ cos_theta = cos(theta); // vecteur directeur de l'ECM
+ sin_theta = sin(theta); // associe au masque
+
+ // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
+ // =========================================================
+ //if( angle[i_theta]==90 ) // => tan(theta) infinie !
+ if( std::fabs(angle[i_theta]-90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.)*std::numeric_limits<double>::epsilon() ) // => tan(theta) infinie !
+ {
+ P1.x=0; P1.y=-(int)n;
+ Q1.x=0; Q1.y= n;
+ }
+ else
+ {
+ tan_theta = sin_theta/cos_theta; // pente de la droite D(theta)
+ P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
+ Q1.x=n; Q1.y=tan_theta*n;
+ }
+
+ // CALCULE MASQUE M(theta)
+ // ======================
+ M[i_theta].resize(n,n); // allocation (si necessaire)
+
+ for(i=0,Y=-moitie+0.5 ; i<n ; i++,Y++)
+ {
+ for(j=0,X=-moitie+0.5 ; j<n ; j++,X++)
+ {
+ // produit vectoriel dir_droite*(X,Y)
+ sgn = vpMath::sign(cos_theta*Y - sin_theta*X);
+
+ // Resultat = P,Q
+ if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
+ {
+ // v dans [0,1]
+ v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
+ }
+ else
+ v=1; // PQ ne coupe pas le pixel(i,j)
+
+ M[i_theta][i][j] = vpMath::round(100*sgn*v);
+
+ // 2 chiffres significatifs
+ // M(i,j) sans incorporer le coef a
+ }
+ }
+ }
+
+}
+
+#endif
+
+/*!
+ Initialise the array of matrices with the defined size and the number of
+ matrices to create.
+
+*/
+void
+vpMe::initMask()
+{
+
+ if (mask != NULL)
+ delete [] mask;
+
+ mask = new vpMatrix[n_mask] ;
+
+ vpColVector angle(n_mask) ;
+
+ unsigned int angle_pas ;
+ angle_pas = 180 / n_mask ;
+
+ unsigned int k =0 ;
+ for (unsigned int i = 0 ; /* i < 180, */ k < n_mask ; i += angle_pas)
+ angle[k++] = i ;
+
+ calcul_masques(angle, mask_size, mask ) ;
+
+}
+
+
+
+void
+vpMe::print( )
+{
+
+ std::cout<< std::endl ;
+ std::cout<<"Moving edges settings " <<std::endl ;
+ std::cout<< std::endl ;
+ std::cout<<" Size of the convolution masks...."<<mask_size<<"x"<<mask_size<<" pixels"<<std::endl ;
+ std::cout<<" Number of masks.................."<<n_mask<<" "<<std::endl ;
+ std::cout<<" Query range +/- J................"<<range<<" pixels "<<std::endl ;
+ std::cout<<" Likelihood test ratio............"<<threshold<<std::endl ;
+ std::cout<<" Contrast tolerance +/-..........."<< mu1 * 100<<"% and "<<mu2 * 100<<"% "<<std::endl ;
+ std::cout<<" Sample step......................"<<sample_step<<" pixels"<<std::endl ;
+ std::cout<<" Strip............................"<<strip<<" pixels "<<std::endl ;
+ std::cout<<" Min_Samplestep..................."<<min_samplestep<<" pixels "<<std::endl ;
+}
+
+vpMe::vpMe()
+ : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
+ range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
+ n_mask(180), strip(2), mask(NULL)
+{
+ //ntotal_sample = 0; // not sure that it is used
+ //points_to_track = 500; // not sure that it is used
+ anglestep = (180 / n_mask) ;
+
+ initMask() ;
+}
+
+vpMe::vpMe(const vpMe &me)
+ : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
+ range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
+ n_mask(180), strip(2), mask(NULL)
+{
+ *this = me;
+}
+
+const
+vpMe& vpMe::operator=(const vpMe &me)
+{
+ if (mask != NULL) {
+ delete [] mask;
+ mask = NULL ;
+ }
+ threshold = me.threshold ;
+ mu1 = me.mu1 ;
+ mu2 = me.mu2 ;
+ min_samplestep = me.min_samplestep ;
+ anglestep = me.anglestep ;
+ mask_size = me.mask_size ;
+ n_mask = me.n_mask ;
+ mask_sign = me.mask_sign ;
+ range = me.range ;
+ sample_step = me.sample_step ;
+ ntotal_sample = me.ntotal_sample;
+ points_to_track = me.points_to_track;
+ strip = me.strip ;
+
+ initMask() ;
+ return *this;
+}
+
+vpMe::~vpMe()
+{
+ if (mask != NULL)
+ {
+ delete [] mask ;
+ mask = NULL;
+ }
+}
+
+
+void
+vpMe::setMaskNumber(const unsigned int &n)
+{
+ n_mask = n ;
+ anglestep = 180 / n_mask ;
+ initMask() ;
+}
+
+void
+vpMe::setMaskSize(const unsigned int &s)
+{
+ mask_size = s ;
+ initMask() ;
+}
+
+
+
+
diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp
new file mode 100644
index 0000000..2f82d2f
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp
@@ -0,0 +1,1240 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/me/vpMeEllipse.h>
+
+#include <visp3/me/vpMe.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDebug.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <vector>
+
+void computeTheta(double &theta, vpColVector &K, vpImagePoint iP);
+
+/*!
+ Computes the \f$ \theta \f$ angle which represents the angle between the tangente to the curve and the i axis.
+
+ \param theta : The computed value.
+ \param K : The parameters of the ellipse.
+ \param iP : the point belonging th the ellipse where the angle is computed.
+*/
+void
+computeTheta(double &theta, vpColVector &K, vpImagePoint iP)
+{
+ double i = iP.get_i();
+ double j = iP.get_j();
+
+ double A = 2*i+2*K[1]*j + 2*K[2] ;
+ double B = 2*K[0]*j + 2*K[1]*i + 2*K[3];
+
+ theta = atan2(A,B) ; //Angle between the tangente and the i axis.
+
+ while (theta > M_PI) { theta -= M_PI ; }
+ while (theta < 0) { theta += M_PI ; }
+}
+
+
+/*!
+ 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.), mu11(0.), mu20(0.), mu02(0.),
+ m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
+ thresholdWeight(0.2), expecteddensity(0.)
+{
+ // redimensionnement du vecteur de parametre
+ // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+
+ K.resize(5) ;
+
+ //j1 = j2 = i1 = i2 = 0 ;
+ iP1.set_i(0);
+ iP1.set_j(0);
+ iP2.set_i(0);
+ iP2.set_j(0);
+}
+
+/*!
+ Copy constructor.
+*/
+vpMeEllipse::vpMeEllipse(const vpMeEllipse &meellipse)
+ : vpMeTracker(meellipse), K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2*M_PI),
+ ce(0.), se(0.), angle(), m00(0.), mu11(0.), mu20(0.), mu02(0.),
+ m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
+ thresholdWeight(0.2), expecteddensity(0.)
+{
+ K = meellipse.K;
+ iPc = meellipse.iPc;
+ a = meellipse.a;
+ b = meellipse.b;
+ e = meellipse.e;
+
+ iP1 = meellipse.iP1;
+ iP2 = meellipse.iP2;
+ alpha1 = meellipse.alpha1;
+ alpha2 = meellipse.alpha2;
+ ce = meellipse.ce;
+ se = meellipse.se;
+
+ angle = meellipse.angle;
+
+ m00 = meellipse.m00;
+ mu11 = meellipse.mu11;
+ mu20 = meellipse.mu20;
+ mu02 = meellipse.mu02;
+ m10 = meellipse.m10;
+ m01 = meellipse.m01;
+ m11 = meellipse.m11;
+ m02 = meellipse.m02;
+ m20 = meellipse.m20;
+ thresholdWeight = meellipse.thresholdWeight;
+
+ expecteddensity = meellipse.expecteddensity;
+}
+
+/*!
+ Basic destructor.
+*/
+vpMeEllipse::~vpMeEllipse()
+{
+ list.clear();
+ angle.clear();
+}
+
+
+/*!
+ 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.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMeEllipse::sample(const vpImage<unsigned char> & I)
+{
+ if (!me) {
+ throw(vpException(vpException::fatalError,
+ "Moving edges on ellipse tracking not initialized")) ;
+ }
+
+ int height = (int)I.getHeight() ;
+ int width = (int)I.getWidth() ;
+
+ //if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+ std::cout << "In vpMeEllipse::sample: " ;
+ std::cout << "function called with sample step = 0" ;
+ //return fatalError ;
+ }
+
+ double j, i;//, j11, i11;
+ vpImagePoint iP11;
+ j = i = 0.0 ;
+
+ double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
+ vpColor col = vpColor::red ;
+ getParameters() ;
+
+ // Delete old list
+ list.clear();
+
+ angle.clear();
+
+ // sample positions
+ double k = alpha1 ;
+ while (k<alpha2)
+ {
+// j = a *cos(k) ; // equation of an ellipse
+// i = b *sin(k) ; // equation of an ellipse
+
+ j = a *sin(k) ; // equation of an ellipse
+ i = b *cos(k) ; // equation of an ellipse
+
+ // (i,j) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+// iP11.set_j( iPc.get_j() + ce *j - se *i );
+// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
+
+ iP11.set_j( iPc.get_j() + ce *j + se *i );
+ iP11.set_i( iPc.get_i() - se *j + ce *i );
+
+ vpDisplay::displayCross(I, iP11, 5, col) ;
+
+ double theta ;
+ computeTheta(theta, K, iP11) ;
+
+ // If point is in the image, add to the sample list
+ if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
+ {
+ vpMeSite pix ;
+ pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
+ pix.setDisplay(selectDisplay) ;
+ pix.setState(vpMeSite::NO_SUPPRESSION);
+
+ if(vpDEBUG_ENABLE(3))
+ {
+ vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
+ }
+ list.push_back(pix);
+ angle.push_back(k);
+ }
+ k += incr ;
+
+ }
+ vpMeTracker::initTracking(I) ;
+}
+
+
+/*!
+
+ Resample the ellipse if the number of sample is less than 90% of the
+ expected value.
+
+ \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
+ and the parameter which indicates the number of degrees between
+ two points (vpMe::sample_step).
+
+ \param I : Image in which the ellipse appears.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMeEllipse::reSample(const vpImage<unsigned char> &I)
+{
+ if (!me) {
+ throw(vpException(vpException::fatalError,
+ "Moving edges on ellipse tracking not initialized")) ;
+ }
+
+ unsigned int n = numberOfSignal() ;
+ expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
+ if ((double)n<0.9*expecteddensity){
+ sample(I) ;
+ }
+}
+
+
+/*!
+ Computes the coordinates of the ellipse center, length of the semiminor axis \f$ a \f$, the length of the semimajor axis \f$ b \f$ and,
+ \f$ e \f$ which is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
+
+ All those computations are made thanks to the parameters \f$ K = {K_0, ..., K_4} \f$.
+*/
+void
+vpMeEllipse::getParameters()
+{
+ double k[6] ;
+ for (unsigned int i=0 ; i < 5 ; i++)
+ k[i+1] = K[i] ;
+ k[0] = 1 ;
+
+ double d = k[2]*k[2] - k[0]*k[1];
+
+ iPc.set_i( (k[1] * k[3] - k[2] * k[4]) / d );
+ iPc.set_j( (k[0] * k[4] - k[2] * k[3]) / d );
+
+ double sq = sqrt(vpMath::sqr(k[1]-k[0]) + 4.0*vpMath::sqr(k[2])) ;
+
+ if (std::fabs(k[2]) <= std::numeric_limits<double>::epsilon()) {
+ e = 0;
+ }
+ else {
+ e = (k[1] - k[0] + sq) / (2.0*k[2]);
+ e = (-1/e) ;
+ }
+
+ e = atan(e) ;
+
+ if(e < 0.0) e += M_PI ;
+
+ ce = cos(e) ;
+ se = sin(e) ;
+
+ double num = 2.0*(k[0]*iPc.get_i()*iPc.get_i() + 2.0*k[2]*iPc.get_j()*iPc.get_i() + k[1]*iPc.get_j()*iPc.get_j() - k[5]) ;
+ double a2 = num / (k[0] + k[1] + sq ) ;
+ double b2 = num / (k[0] + k[1] - sq ) ;
+
+ a = sqrt( a2 ) ;
+ b = sqrt( b2 ) ;
+}
+
+/*!
+ Print the parameters \f$ K = {K_0, ..., K_4} \f$ and the coordinates of the ellipse center.
+*/
+void
+vpMeEllipse::printParameters()
+{
+ std::cout << "K" << std::endl ;
+ std::cout << K.t() ;
+ std::cout << iPc << std::endl ;
+}
+
+/*!
+ Computes the \f$ alpha \f$ counter clockwise angle of the two points and store them into alpha1 for the
+ smallest and alpha2 for the biggest.
+
+ \note This function is useful only during the initialization.
+
+ \param pt1 : First point whose \f$ alpha \f$ angle is computed.
+ \param pt2 : Second point whose \f$ alpha \f$ angle is computed.
+*/
+void
+vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
+{
+ getParameters() ;
+ double j1, i1, j11, i11;
+ j1 = i1 = 0.0 ;
+
+ int number_of_points = 2000 ;
+ double incr = 2 * M_PI / number_of_points ; // angle increment
+
+ double dmin1 = 1e6 ;
+ double dmin2 = 1e6 ;
+
+ double k = 0 ;
+ while(k < 2*M_PI) {
+
+// j1 = a *cos(k) ; // equation of an ellipse
+// i1 = b *sin(k) ; // equation of an ellipse
+
+ j1 = a *sin(k) ; // equation of an ellipse
+ i1 = b *cos(k) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+// j11 = iPc.get_j() + ce *j1 - se *i1 ;
+// i11 = iPc.get_i() -( se *j1 + ce *i1) ;
+
+ j11 = iPc.get_j() + ce *j1 + se *i1 ;
+ i11 = iPc.get_i() - se *j1 + ce *i1 ;
+
+ double d = vpMath::sqr(pt1.get_i()-i11) + vpMath::sqr(pt1.get_j()-j11) ;
+ if (d < dmin1)
+ {
+ dmin1 = d ;
+ alpha1 = k ;
+ }
+ d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
+ if (d < dmin2)
+ {
+ dmin2 = d ;
+ alpha2 = k ;
+ }
+ k += incr ;
+ }
+
+ if (alpha2 < alpha1)
+ alpha2 += 2 * M_PI;
+ //else if (alpha2 == alpha1)
+ else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits<double>::epsilon())
+ alpha2 += 2 * M_PI;
+}
+
+
+/*!
+ 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;
+ double theta;
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ vpImagePoint iP;
+ iP.set_i(p_me.ifloat);
+ iP.set_j(p_me.jfloat);
+ computeTheta(theta, K, iP) ;
+ p_me.alpha = theta ;
+ *it = p_me;
+ }
+}
+
+/*!
+ Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
+*/
+void
+vpMeEllipse::suppressPoints()
+{
+ // Loop through list of sites to track
+ std::list<vpMeSite>::iterator itList = list.begin();
+ for(std::list<double>::iterator it=angle.begin(); it!=angle.end(); ){
+ vpMeSite s = *itList;//current reference pixel
+ if (s.getState() != vpMeSite::NO_SUPPRESSION)
+ {
+ itList = list.erase(itList) ;
+ it = angle.erase(it);
+ }
+ else
+ {
+ ++itList;
+ ++it;
+ }
+ }
+}
+
+
+/*!
+ Seek along the ellipse edge defined by its equation, the two extremities of
+ the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.
+
+ \param I : Image in which the ellipse appears.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
+{
+ if (!me) {
+ throw(vpException(vpException::fatalError,
+ "Moving edges on ellipse tracking not initialized")) ;
+ }
+
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+
+ vpImagePoint ip;
+
+ unsigned int memory_range = me->getRange() ;
+ me->setRange(2);
+
+ double memory_mu1 = me->getMu1();
+ me->setMu1(0.5);
+
+ double memory_mu2 = me->getMu2();
+ me->setMu2(0.5);
+
+ double incr = vpMath::rad(2.0) ;
+
+ if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
+ {
+ vpMeSite P;
+ double k = alpha1;
+ double i1,j1;
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ k -= incr;
+ //while ( k < -M_PI ) { k+=2*M_PI; }
+
+ i1 = b *cos(k) ; // equation of an ellipse
+ j1 = a *sin(k) ; // equation of an ellipse
+ P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
+ P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
+
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ angle.push_back(k);
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+
+ vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+
+ k = alpha2;
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ k += incr;
+ //while ( k > M_PI ) { k-=2*M_PI; }
+
+ i1 = b *cos(k) ; // equation of an ellipse
+ j1 = a *sin(k) ; // equation of an ellipse
+ P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
+ P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
+
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ angle.push_back(k);
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+
+ vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+ }
+
+ suppressPoints() ;
+
+ me->setRange(memory_range);
+ me->setMu1(memory_mu1);
+ me->setMu2(memory_mu2);
+}
+
+
+/*!
+ Finds in the list of vpMeSite the two points with the smallest and the biggest \f$ \alpha \f$ angle value, and stores them.
+*/
+void
+vpMeEllipse::setExtremities()
+{
+ double alphamin = +1e6;
+ double alphamax = -1e6;
+ double imin = 0;
+ double jmin = 0;
+ double imax = 0;
+ double jmax = 0;
+
+ // Loop through list of sites to track
+ std::list<double>::const_iterator itAngle = angle.begin();
+
+ for(std::list<vpMeSite>::const_iterator itList=list.begin(); itList!=list.end(); ++itList){
+ vpMeSite s = *itList;//current reference pixel
+ double alpha = *itAngle;
+ if (alpha < alphamin)
+ {
+ alphamin = alpha;
+ imin = s.ifloat ;
+ jmin = s.jfloat ;
+ }
+
+ if (alpha > alphamax)
+ {
+ alphamax = alpha;
+ imax = s.ifloat ;
+ jmax = s.jfloat ;
+ }
+ ++itAngle;
+ }
+
+ alpha1 = alphamin;
+ alpha2 = alphamax;
+ iP1.set_ij(imin,jmin);
+ iP2.set_ij(imax,jmax);
+}
+
+
+/*!
+ 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 ellipse.
+*/
+void
+vpMeEllipse::leastSquare()
+{
+ // Construction du systeme Ax=b
+ // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+ // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
+ unsigned int i ;
+
+ vpMeSite p_me ;
+
+ unsigned int iter =0 ;
+ vpColVector b_(numberOfSignal()) ;
+ vpRobust r(numberOfSignal()) ;
+ r.setThreshold(2);
+ r.setIteration(0) ;
+ vpMatrix D(numberOfSignal(),numberOfSignal()) ;
+ D.eye() ;
+ vpMatrix DA, DAmemory ;
+ vpColVector DAx ;
+ vpColVector w(numberOfSignal()) ;
+ w =1 ;
+ unsigned int nos_1 = numberOfSignal() ;
+
+ if (list.size() < 3)
+ {
+ throw(vpException(vpException::dimensionError,
+ "Not enought moving edges to track the ellipse")) ;
+ }
+
+ vpMatrix A(numberOfSignal(),5) ;
+ vpColVector x(5);
+
+ unsigned int k =0 ;
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ A[k][0] = vpMath::sqr(p_me.jfloat) ;
+ A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
+ A[k][2] = 2 * p_me.ifloat ;
+ A[k][3] = 2 * p_me.jfloat ;
+ A[k][4] = 1 ;
+
+ b_[k] = - vpMath::sqr(p_me.ifloat) ;
+ k++ ;
+ }
+ }
+
+ while (iter < 4 )
+ {
+ DA = D*A ;
+ vpMatrix DAp ;
+
+ x = DA.pseudoInverse(1e-26) *D*b_ ;
+
+ vpColVector residu(nos_1);
+ residu = b_ - A*x;
+ r.setIteration(iter) ;
+ r.MEstimator(vpRobust::TUKEY,residu,w) ;
+
+ k = 0;
+ for (i=0 ; i < nos_1 ; i++)
+ {
+ D[k][k] =w[k] ;
+ k++;
+ }
+ iter++;
+ }
+
+ k =0 ;
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ if (w[k] < thresholdWeight)
+ {
+ p_me.setState(vpMeSite::M_ESTIMATOR);
+
+ *it = p_me;
+ }
+ k++ ;
+ }
+ }
+ for(i = 0; i < 5; i ++)
+ K[i] = x[i];
+
+ getParameters() ;
+}
+
+
+/*!
+ Display the 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.
+ */
+void
+vpMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
+{
+ vpMeEllipse::display(I,iPc,a,b,e,alpha1,alpha2,col);
+}
+
+
+/*!
+ Initilization of the tracking. Ask the user to click counter clockwise on five points
+ located on the ellipse edge to track.
+
+ \warning The points should be selected counter clockwise.
+
+ \param I : Image in which the ellipse appears.
+*/
+void
+vpMeEllipse::initTracking(const vpImage<unsigned char> &I)
+{
+ const unsigned int n=5 ;
+ vpImagePoint iP[n];
+
+ for (unsigned int k =0 ; k < n ; k++)
+ {
+ std::cout << "Click points "<< k+1 <<"/" << n ;
+ std::cout << " on the ellipse in the trigonometric order" <<std::endl ;
+ vpDisplay::getClick(I, iP[k], true);
+ vpDisplay::displayCross(I, iP[k], 7, vpColor::red);
+ vpDisplay::flush(I);
+ std::cout << iP[k] << std::endl;
+ }
+
+ iP1 = iP[0];
+ iP2 = iP[n-1];
+
+ initTracking(I, n, iP) ;
+}
+
+
+/*!
+ Initialization of the tracking. The ellipse is defined thanks to the
+ coordinates of n points.
+
+ \warning It is better to use at least five points to well estimate the ellipse parameters.
+ \warning The n points should be selected counter clockwise.
+
+ \param I : Image in which the ellipse appears.
+ \param n : The number of points in the list.
+ \param iP : A pointer to a list of points belonging to the ellipse edge.
+*/
+void
+vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
+ vpImagePoint *iP)
+{
+ vpMatrix A(n,5) ;
+ vpColVector b_(n) ;
+ vpColVector x(5) ;
+
+ // Construction du systeme Ax=b
+ // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+ // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
+
+ for (unsigned int k =0 ; k < n ; k++)
+ {
+ A[k][0] = vpMath::sqr(iP[k].get_j()) ;
+ A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
+ A[k][2] = 2* iP[k].get_i() ;
+ A[k][3] = 2* iP[k].get_j() ;
+ A[k][4] = 1 ;
+
+ b_[k] = - vpMath::sqr(iP[k].get_i()) ;
+ }
+
+ K = A.pseudoInverse(1e-26)*b_ ;
+
+ iP1 = iP[0];
+ iP2 = iP[n-1];
+
+ getParameters() ;
+
+ computeAngle(iP1, iP2) ;
+
+ expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
+
+ display(I, vpColor::green) ;
+ sample(I) ;
+
+ vpMeTracker::initTracking(I) ;
+
+ track(I) ;
+
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+}
+
+/*!
+ Initialization of the tracking. The ellipse is defined thanks to a vector of
+ n points.
+
+ \warning It is better to use at least five points to well estimate the ellipse parameters.
+ \warning The n points should be selected counter clockwise.
+
+ \param I : Image in which the ellipse appears.
+ \param iP : A vector of points belonging to the ellipse edge used to initialize the tracking.
+*/
+void
+vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
+{
+ unsigned int n = (unsigned int)(iP.size());
+ vpMatrix A(n,5) ;
+ vpColVector b_(n) ;
+ vpColVector x(5) ;
+
+ // Construction du systeme Ax=b
+ // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+ // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
+
+ for (unsigned int k =0 ; k < n ; k++)
+ {
+ A[k][0] = vpMath::sqr(iP[k].get_j()) ;
+ A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
+ A[k][2] = 2* iP[k].get_i() ;
+ A[k][3] = 2* iP[k].get_j() ;
+ A[k][4] = 1 ;
+
+ b_[k] = - vpMath::sqr(iP[k].get_i()) ;
+ }
+
+ K = A.pseudoInverse(1e-26)*b_ ;
+
+ iP1 = iP[0];
+ iP2 = iP[n-1];
+
+ getParameters() ;
+
+ computeAngle(iP1, iP2) ;
+
+ expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
+
+ display(I, vpColor::green) ;
+ sample(I) ;
+
+ vpMeTracker::initTracking(I) ;
+
+ track(I) ;
+
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+}
+
+void
+vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p,
+ double low_alpha, double high_alpha)
+{
+ iPc = ic;
+ a = a_p;
+ b = b_p;
+ e = e_p;
+ alpha1 = low_alpha;
+ alpha2 = high_alpha;
+
+ if (alpha2 <alpha1)
+ alpha2 += 2 * M_PI;
+
+ ce = cos(e);
+ se = sin(e);
+
+ display(I, vpColor::green) ;
+ sample(I) ;
+
+ vpMeTracker::initTracking(I) ;
+
+ track(I) ;
+
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+}
+
+/*!
+ Track the ellipse in the image I.
+
+ \param I : Image in which the ellipse appears.
+*/
+void
+vpMeEllipse::track(const vpImage<unsigned char> &I)
+{
+ vpMeTracker::track(I) ;
+
+ // Estimation des parametres de la droite aux moindres carre
+ suppressPoints() ;
+ setExtremities() ;
+
+ leastSquare() ;
+ seekExtremities(I) ;
+ setExtremities() ;
+ leastSquare() ;
+
+ // suppression des points rejetes par la regression robuste
+ suppressPoints() ;
+ setExtremities() ;
+
+ //reechantillonage si necessaire
+ reSample(I) ;
+
+ // remet a jour l'angle delta pour chaque point de la liste
+
+ updateTheta() ;
+
+ computeMoments();
+
+ // Remise a jour de delta dans la liste de site me
+ if (vpDEBUG_ENABLE(2))
+ {
+ display(I,vpColor::red) ;
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+}
+
+/*!
+ Computes the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
+
+ Computes the second central moments \f$ \mu_{20} \f$, \f$ \mu_{02} \f$ and \f$ \mu_{11} \f$
+*/
+void
+vpMeEllipse::computeMoments()
+{
+ double tane = tan(-1/e);
+ m00 = M_PI*a*b;
+ m10 = m00*iPc.get_i();
+ m01 = m00*iPc.get_j();
+ m20 = m00*(a*a+b*b*tane*tane)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_i();
+ m02 = m00*(a*a*tane*tane+b*b)/(4*(1+tane*tane))+m00*iPc.get_j()*iPc.get_j();
+ m11 = m00*tane*(a*a-b*b)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_j();
+ mu11 = m11 - iPc.get_j()*m10;
+ mu02 = m02 - iPc.get_j()*m01;
+ mu20 = m20 - iPc.get_i()*m10;
+}
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+
+/*!
+ * \brief computeAngle
+ */
+void
+vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1,
+ int ip2, int jp2, double &_alpha2)
+{
+ getParameters() ;
+ double j1, i1, j11, i11;
+ j1 = i1 = 0.0 ;
+
+ int number_of_points = 2000 ;
+ double incr = 2 * M_PI / number_of_points ; // angle increment
+
+ double dmin1 = 1e6 ;
+ double dmin2 = 1e6 ;
+
+ double k = -M_PI ;
+ while(k < M_PI) {
+
+ j1 = a *cos(k) ; // equation of an ellipse
+ i1 = b *sin(k) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ j11 = iPc.get_j() + ce *j1 - se *i1 ;
+ i11 = iPc.get_i() -( se *j1 + ce *i1) ;
+
+ double d = vpMath::sqr(ip1-i11) + vpMath::sqr(jp1-j11) ;
+ if (d < dmin1)
+ {
+ dmin1 = d ;
+ alpha1 = k ;
+ _alpha1 = k ;
+ }
+ d = vpMath::sqr(ip2-i11) + vpMath::sqr(jp2-j11) ;
+ if (d < dmin2)
+ {
+ dmin2 = d ;
+ alpha2 = k ;
+ _alpha2 = k ;
+ }
+ k += incr ;
+ }
+
+ if (alpha2 <alpha1) alpha2 += 2*M_PI ;
+
+ vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
+
+}
+
+
+/*!
+ * \brief computeAngle
+ */
+void
+vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
+{
+
+ double a1, a2 ;
+ computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
+}
+
+
+void
+vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
+ unsigned *i, unsigned *j)
+{
+ vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
+
+ //if (circle==false)
+ if (1)
+ {
+ vpMatrix A(n,5) ;
+ vpColVector b_(n) ;
+ vpColVector x(5) ;
+
+ // Construction du systeme Ax=b
+ //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+ // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
+
+ for (unsigned int k =0 ; k < n ; k++)
+ {
+ A[k][0] = vpMath::sqr(j[k]) ;
+ A[k][1] = 2* i[k] * j[k] ;
+ A[k][2] = 2* i[k] ;
+ A[k][3] = 2* j[k] ;
+ A[k][4] = 1 ;
+
+ b_[k] = - vpMath::sqr(i[k]) ;
+ }
+
+ K = A.pseudoInverse(1e-26)*b_ ;
+ std::cout << K << std::endl;
+ }
+ else
+ {
+ vpMatrix A(n,3) ;
+ vpColVector b_(n) ;
+ vpColVector x(3) ;
+
+ vpColVector Kc(3) ;
+ for (unsigned int k =0 ; k < n ; k++)
+ {
+ A[k][0] = 2* i[k] ;
+ A[k][1] = 2* j[k] ;
+
+ A[k][2] = 1 ;
+ b_[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
+ }
+
+ Kc = A.pseudoInverse(1e-26)*b_ ;
+ K[0] = 1 ;
+ K[1] = 0 ;
+ K[2] = Kc[0] ;
+ K[3] = Kc[1] ;
+ K[4] = Kc[2] ;
+
+ std::cout << K << std::endl;
+ }
+ iP1.set_i( i[0] );
+ iP1.set_j( j[0] );
+ iP2.set_i( i[n-1] );
+ iP2.set_j( j[n-1] );
+
+ getParameters() ;
+ computeAngle(iP1, iP2) ;
+ display(I, vpColor::green) ;
+
+ sample(I) ;
+
+ // 2. On appelle ce qui n'est pas specifique
+ {
+ vpMeTracker::initTracking(I) ;
+ }
+
+ try{
+ track(I) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+
+}
+#endif // Deprecated
+
+/*!
+
+ Display of the ellipse thanks to the equation parameters.
+
+ \param I : The image used as background.
+
+ \param center : Center of the ellipse
+
+ \param A : Semiminor axis of the ellipse.
+
+ \param B : Semimajor axis of the ellipse.
+
+ \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+
+ \param smallalpha : Smallest \f$ alpha \f$ angle in rad.
+
+ \param highalpha : Highest \f$ alpha \f$ angle in rad.
+
+ \param color : Color used to display th lines.
+
+ \param thickness : Thickness of the drawings.
+*/
+void vpMeEllipse::display(const vpImage<unsigned char>& 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)
+{
+ double j1, i1;
+ vpImagePoint iP11;
+ double j2, i2;
+ vpImagePoint iP22;
+ j1 = j2 = i1 = i2 = 0 ;
+
+ double incr = vpMath::rad(2) ; // angle increment
+
+ vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
+
+ double k = smallalpha ;
+ while (k+incr<highalpha)
+ {
+ j1 = A *cos(k) ; // equation of an ellipse
+ i1 = B *sin(k) ; // equation of an ellipse
+
+ j2 = A *cos(k+incr) ; // equation of an ellipse
+ i2 = B *sin(k+incr) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
+ iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
+ iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+
+ vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
+
+ k += incr ;
+ }
+
+ j1 = A *cos(smallalpha) ; // equation of an ellipse
+ i1 = B *sin(smallalpha) ; // equation of an ellipse
+
+ j2 = A *cos(highalpha) ; // equation of an ellipse
+ i2 = B *sin(highalpha) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
+ iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
+ iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+
+ vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
+ vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
+}
+
+/*!
+
+ Display of the ellipse thanks to the equation parameters.
+
+ \param I : The image used as background.
+
+ \param center : Center of the ellipse
+
+ \param A : Semiminor axis of the ellipse.
+
+ \param B : Semimajor axis of the ellipse.
+
+ \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+
+ \param smallalpha : Smallest \f$ alpha \f$ angle in rad.
+
+ \param highalpha : Highest \f$ alpha \f$ angle in rad.
+
+ \param color : Color used to display th lines.
+
+ \param thickness : Thickness of the drawings.
+*/
+void vpMeEllipse::display(const vpImage<vpRGBa>& 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)
+{
+ double j1, i1;
+ vpImagePoint iP11;
+ double j2, i2;
+ vpImagePoint iP22;
+ j1 = j2 = i1 = i2 = 0 ;
+
+ double incr = vpMath::rad(2) ; // angle increment
+
+ vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
+
+ double k = smallalpha ;
+ while (k+incr<highalpha)
+ {
+ j1 = A *cos(k) ; // equation of an ellipse
+ i1 = B *sin(k) ; // equation of an ellipse
+
+ j2 = A *cos(k+incr) ; // equation of an ellipse
+ i2 = B *sin(k+incr) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
+ iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
+ iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+
+ vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
+
+ k += incr ;
+ }
+
+ j1 = A *cos(smallalpha) ; // equation of an ellipse
+ i1 = B *sin(smallalpha) ; // equation of an ellipse
+
+ j2 = A *cos(highalpha) ; // equation of an ellipse
+ i2 = B *sin(highalpha) ; // equation of an ellipse
+
+ // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // a rotation by "e" and a translation by (xci,jc) are done
+ // to get the coordinates of the point on the shifted ellipse
+ iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
+ iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ // to get the coordinates of the point on the shifted ellipse
+ iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
+ iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+
+ vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
+ vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
+}
diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp
new file mode 100644
index 0000000..1c5bf30
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMeLine.cpp
@@ -0,0 +1,1446 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpMeLine.cpp
+ \brief Moving edges
+*/
+
+
+
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <algorithm> // std::min
+
+#define INCR_MIN 1
+
+void computeDelta(double &delta, int i1, int j1, int i2, int j2);
+
+static void
+normalizeAngle(double &delta)
+{
+ while (delta > M_PI) { delta -= M_PI ; }
+ while (delta < -M_PI) { delta += M_PI ; }
+}
+
+void
+computeDelta(double &delta, int i1, int j1, int i2, int j2)
+{
+
+ double B = double(i1-i2) ;
+ double A = double(j1-j2) ;
+
+ delta = atan2(B,A) ;
+ delta -= M_PI/2.0 ;
+ normalizeAngle(delta) ;
+
+}
+
+static void
+project(double a, double b, double c,
+ double i, double j, double &ip,double &jp)
+{
+ if (fabs(a)>fabs(b))
+ {
+ jp = (vpMath::sqr(a)*j - a*b*i - c*b)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
+ ip = (-c-b*jp)/a;
+ }
+ else
+ {
+ ip = (vpMath::sqr(b)*i-a*b*j-c*a)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
+ jp = (-c-a*ip)/b;
+ }
+}
+
+/*!
+
+ 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.)
+
+{
+ rho = meline.rho;
+ theta = meline.theta;
+ delta = meline.delta;
+ delta_1 = meline.delta_1;
+ angle = meline.angle;
+ angle_1 = meline.angle_1;
+ sign = meline.sign;
+
+ a = meline.a;
+ b = meline.b;
+ c = meline.c;
+ _useIntensityForRho = meline._useIntensityForRho;
+ PExt[0] = meline.PExt[0];
+ 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.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMeLine::sample(const vpImage<unsigned char>& I)
+{
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ double n_sample;
+
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+ vpERROR_TRACE("function called with sample step = 0") ;
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "sample step = 0")) ;
+ }
+
+ // i, j portions of the line_p
+ double diffsi = PExt[0].ifloat-PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+
+ double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
+ if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
+ throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
+ // number of samples along line_p
+ n_sample = length_p/(double)me->getSampleStep();
+
+ double stepi = diffsi/(double)n_sample;
+ double stepj = diffsj/(double)n_sample;
+
+ // Choose starting point
+ double is = PExt[1].ifloat;
+ double js = PExt[1].jfloat;
+
+ // Delete old list
+ list.clear();
+
+ // sample positions at i*me->getSampleStep() interval along the
+ // line_p, starting at PSiteExt[0]
+
+ vpImagePoint ip;
+ for(int i=0; i<=vpMath::round(n_sample); i++)
+ {
+ // If point is in the image, add to the sample list
+ if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
+ {
+ vpMeSite pix ; //= list.value();
+ pix.init((int)is, (int)js, delta, 0, sign) ;
+ pix.setDisplay(selectDisplay) ;
+
+ if(vpDEBUG_ENABLE(3))
+ {
+ ip.set_i( is );
+ ip.set_j( js );
+ vpDisplay::displayCross(I, ip, 2, vpColor::blue);
+ }
+
+ list.push_back(pix);
+ }
+ is += stepi;
+ js += stepj;
+
+ }
+
+ vpCDEBUG(1) << "end vpMeLine::sample() : ";
+ 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 col : Color of the displayed line. Note that a moving edge
+ that is considered as an outlier is displayed in green.
+
+ */
+void
+vpMeLine::display(const vpImage<unsigned char>&I, vpColor col)
+{
+ vpMeLine::display(I,PExt[0],PExt[1],list,a,b,c,col);
+}
+
+
+/*!
+
+ Initilization 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<unsigned char> &I)
+{
+ vpImagePoint ip1, ip2;
+
+ std::cout << "Click on the line first point..." <<std::endl ;
+ while (vpDisplay::getClick(I, ip1)!=true) ;
+ vpDisplay::displayCross(I, ip1, 7, vpColor::red);
+ vpDisplay::flush(I);
+ std::cout << "Click on the line second point..." <<std::endl ;
+ while (vpDisplay::getClick(I, ip2)!=true) ;
+ vpDisplay::displayCross(I, ip2, 7, vpColor::red);
+ vpDisplay::flush(I);
+
+ try
+ {
+ initTracking(I, ip1, ip2) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+}
+
+
+/*!
+
+ 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) ;
+ vpColVector x(2), x_1(2) ;
+ x_1 = 0;
+
+ unsigned int i ;
+
+ vpRobust r(numberOfSignal()) ;
+ r.setThreshold(2);
+ r.setIteration(0) ;
+ vpMatrix D(numberOfSignal(),numberOfSignal()) ;
+ D.eye() ;
+ vpMatrix DA, DAmemory ;
+ vpColVector DAx ;
+ vpColVector w(numberOfSignal()) ;
+ vpColVector B(numberOfSignal()) ;
+ w =1 ;
+ vpMeSite p_me ;
+ unsigned int iter =0 ;
+ unsigned int nos_1 = 0 ;
+ double distance = 100;
+
+ if (list.size() <= 2 || numberOfSignal() <= 2)
+ {
+ //vpERROR_TRACE("Not enough point") ;
+ vpCDEBUG(1) << "Not enough point";
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError,
+ "not enough point")) ;
+ }
+
+ if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
+ // a i + j + c = 0
+ // A = (i 1) B = (-j)
+ {
+ nos_1 = numberOfSignal() ;
+ unsigned int k =0 ;
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ A[k][0] = p_me.ifloat ;
+ A[k][1] = 1 ;
+ B[k] = -p_me.jfloat ;
+ k++ ;
+ }
+ }
+
+ while (iter < 4 && distance > 0.05)
+ {
+ DA = D*A ;
+ x = DA.pseudoInverse(1e-26) *D*B ;
+
+ vpColVector residu(nos_1);
+ residu = B - A*x;
+ r.setIteration(iter) ;
+ r.MEstimator(vpRobust::TUKEY,residu,w) ;
+
+ k = 0;
+ for (i=0 ; i < nos_1 ; i++)
+ {
+ D[k][k] =w[k] ;
+ k++;
+ }
+ iter++ ;
+ distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
+ x_1 = x;
+ }
+
+ k =0 ;
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ if (w[k] < 0.2)
+ {
+ p_me.setState(vpMeSite::M_ESTIMATOR);
+
+ *it = p_me;
+ }
+ k++ ;
+ }
+ }
+
+ // mise a jour de l'equation de la droite
+ a = x[0] ;
+ b = 1 ;
+ c = x[1] ;
+
+ double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
+ a /= s ;
+ b /= s ;
+ c /= s ;
+ }
+
+
+ else // Construction du systeme Ax=B
+ // i + bj + c = 0
+ // A = (j 1) B = (-i)
+ {
+ nos_1 = numberOfSignal() ;
+ unsigned int k =0 ;
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ A[k][0] = p_me.jfloat ;
+ A[k][1] = 1 ;
+ B[k] = -p_me.ifloat ;
+ k++ ;
+ }
+ }
+
+ while (iter < 4 && distance > 0.05)
+ {
+ DA = D*A ;
+ x = DA.pseudoInverse(1e-26) *D*B ;
+
+ vpColVector residu(nos_1);
+ residu = B - A*x;
+ r.setIteration(iter) ;
+ r.MEstimator(vpRobust::TUKEY,residu,w) ;
+
+ k = 0;
+ for (i=0 ; i < nos_1 ; i++)
+ {
+ D[k][k] =w[k] ;
+ k++;
+ }
+ iter++ ;
+ distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
+ x_1 = x;
+ }
+
+ k =0 ;
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ if (w[k] < 0.2)
+ {
+ p_me.setState(vpMeSite::M_ESTIMATOR);
+
+ *it = p_me;
+ }
+ k++ ;
+ }
+ }
+ a = 1 ;
+ b = x[0] ;
+ c = x[1] ;
+
+ double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+ a /= s ;
+ b /= s ;
+ c /= s ;
+ }
+
+ // mise a jour du delta
+ delta = atan2(a,b) ;
+
+ 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<unsigned char> &I,
+ const vpImagePoint &ip1,
+ const vpImagePoint &ip2)
+{
+ vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
+
+ int i1s, j1s, i2s, j2s;
+
+ i1s = vpMath::round( ip1.get_i() );
+ i2s = vpMath::round( ip2.get_i() );
+ j1s = vpMath::round( ip1.get_j() );
+ j2s = vpMath::round( ip2.get_j() );
+
+ try{
+
+ // 1. On fait ce qui concerne les droites (peut etre vide)
+ {
+ // Points extremites
+ PExt[0].ifloat = (float)ip1.get_i() ;
+ PExt[0].jfloat = (float)ip1.get_j() ;
+ PExt[1].ifloat = (float)ip2.get_i() ;
+ PExt[1].jfloat = (float)ip2.get_j() ;
+
+ double angle_ = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;
+ a = cos(angle_) ;
+ b = sin(angle_) ;
+
+ // Real values of a, b can have an other sign. So to get the good values
+ // of a and b in order to initialise then c, we call track(I) just below
+
+ computeDelta(delta,i1s,j1s,i2s,j2s) ;
+ delta_1 = delta;
+
+ // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);
+
+ sample(I) ;
+
+ }
+ // 2. On appelle ce qui n'est pas specifique
+ {
+ vpMeTracker::initTracking(I) ;
+ }
+ // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display
+ track(I);
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ 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
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
+ vpMeSite s = *it;//current reference pixel
+
+ if (s.getState() != vpMeSite::NO_SUPPRESSION)
+ it = list.erase(it);
+ else
+ ++it;
+ }
+}
+
+
+/*!
+ Seek in the list of available points the two extremities of the line.
+*/
+void
+vpMeLine::setExtremities()
+{
+ double imin = +1e6 ;
+ double jmin = +1e6;
+ double imax = -1 ;
+ double jmax = -1 ;
+
+
+ // Loop through list of sites to track
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite s = *it;//current reference pixel
+ if (s.ifloat < imin)
+ {
+ imin = s.ifloat ;
+ jmin = s.jfloat ;
+ }
+
+ if (s.ifloat > imax)
+ {
+ imax = s.ifloat ;
+ jmax = s.jfloat ;
+ }
+ }
+
+ PExt[0].ifloat = imin ;
+ PExt[0].jfloat = jmin ;
+ PExt[1].ifloat = imax ;
+ PExt[1].jfloat = jmax ;
+
+ if (fabs(imin-imax) < 25)
+ {
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite s = *it;//current reference pixel
+ if (s.jfloat < jmin)
+ {
+ imin = s.ifloat ;
+ jmin = s.jfloat ;
+ }
+
+ if (s.jfloat > jmax)
+ {
+ imax = s.ifloat ;
+ jmax = s.jfloat ;
+ }
+ }
+ PExt[0].ifloat = imin ;
+ PExt[0].jfloat = jmin ;
+ PExt[1].ifloat = imax ;
+ PExt[1].jfloat = jmax ;
+ }
+}
+
+/*!
+
+ 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<unsigned char> &I)
+{
+ vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
+
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ double n_sample;
+
+ //if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
+ {
+
+ vpERROR_TRACE("function called with sample step = 0") ;
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "sample step = 0")) ;
+ }
+
+ // i, j portions of the line_p
+ double diffsi = PExt[0].ifloat-PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+
+ double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
+
+ double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
+ double dj = diffsj/sqrt(s) ;
+
+ double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
+
+ // number of samples along line_p
+ n_sample = length_p/(double)me->getSampleStep();
+ double sample_step = (double)me->getSampleStep();
+
+ vpMeSite P ;
+ P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
+ P.setDisplay(selectDisplay) ;
+
+ unsigned int memory_range = me->getRange() ;
+ me->setRange(1);
+
+ vpImagePoint ip;
+
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
+
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+
+ vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+
+ P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
+ P.setDisplay(selectDisplay) ;
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
+
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false) ;
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P);
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+ vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i( P.i );
+ ip.set_j( P.j );
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+
+ me->setRange(memory_range);
+
+ vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
+ 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<unsigned char> &I)
+{
+ double i1,j1,i2,j2 ;
+
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
+ project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
+
+ // Points extremites
+ PExt[0].ifloat = i1 ;
+ PExt[0].jfloat = j1 ;
+ PExt[1].ifloat = i2 ;
+ PExt[1].jfloat = j2 ;
+
+ double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
+
+ unsigned int n = numberOfSignal() ;
+ double expecteddensity = d / (double)me->getSampleStep();
+
+ if ((double)n<0.9*expecteddensity)
+ {
+ double delta_new = delta;
+ delta = delta_1;
+ sample(I) ;
+ delta = delta_new;
+ // 2. On appelle ce qui n'est pas specifique
+ {
+ vpMeTracker::initTracking(I) ;
+ }
+ }
+}
+
+/*!
+
+ Set the alpha value of the different vpMeSite to the value of delta.
+*/
+void
+vpMeLine::updateDelta()
+{
+ vpMeSite p_me ;
+
+ double angle_ = delta + M_PI/2;
+ double diff = 0;
+
+ while (angle_<0) angle_ += M_PI;
+ while (angle_>M_PI) angle_ -= M_PI;
+
+ angle_ = vpMath::round(angle_ * 180 / M_PI) ;
+
+ //if(fabs(angle_) == 180 )
+ if(std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon())
+ {
+ angle_= 0 ;
+ }
+
+ //std::cout << "angle theta : " << theta << std::endl ;
+ diff = fabs(angle_ - angle_1);
+ if (diff > 90)
+ sign *= -1;
+
+ angle_1 = angle_;
+
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ p_me = *it;
+ p_me.alpha = delta ;
+ p_me.mask_sign = sign;
+ *it = p_me;
+ }
+ delta_1 = delta;
+}
+
+
+/*!
+
+ Track the line in the image I.
+
+ \param I : Image in which the line appears.
+*/
+void
+vpMeLine::track(const vpImage<unsigned char> &I)
+{
+ vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;
+
+ // 1. On fait ce qui concerne les droites (peut etre vide)
+ {
+ }
+ // 2. On appelle ce qui n'est pas specifique
+ {
+ vpMeTracker::track(I) ;
+ }
+
+ // 3. On revient aux droites
+ {
+ // supression des points rejetes par les ME
+ suppressPoints() ;
+ setExtremities() ;
+
+
+ // Estimation des parametres de la droite aux moindres carre
+ try
+ {
+ leastSquare() ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+
+ // recherche de point aux extremite de la droites
+ // dans le cas d'un glissement
+ seekExtremities(I) ;
+
+ setExtremities() ;
+ try
+ {
+ leastSquare() ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+ // suppression des points rejetes par la regression robuste
+ suppressPoints() ;
+ setExtremities() ;
+
+ //reechantillonage si necessaire
+ reSample(I) ;
+
+ // remet a jour l'angle delta pour chaque point de la liste
+
+ updateDelta() ;
+
+ // Remise a jour de delta dans la liste de site me
+ if (vpDEBUG_ENABLE(2))
+ {
+ display(I,vpColor::red) ;
+ vpMeTracker::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+
+
+ }
+
+ computeRhoTheta(I) ;
+
+ vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
+}
+
+void vpMeLine::update_indices(double theta,int i,int j,int incr,int& i1,int& i2,int& j1,int& j2){
+ i1 = (int)(i + cos(theta) *incr) ;
+ j1 = (int)(j + sin(theta) *incr) ;
+
+ i2 = (int)(i - cos(theta) *incr) ;
+ 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<unsigned char>& I)
+{
+ //rho = -c ;
+ //theta = atan2(a,b) ;
+ rho = fabs(c);
+ theta = atan2(b,a) ;
+
+ while (theta >= M_PI) theta -=M_PI ;
+ while (theta < 0) 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) ;
+ j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
+
+ int end = false ;
+ int incr = 10 ;
+
+
+ int i1=0,i2=0,j1=0,j2=0 ;
+ unsigned char v1=0,v2=0 ;
+
+ int width_ = (int)I.getWidth();
+ int height_ = (int)I.getHeight();
+ update_indices(theta,i,j,incr,i1,i2,j1,j2);
+
+ if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
+ j1<0 || j1>=width_ || j2<0 || j2>=width_){
+ double rho_lim1 = fabs((double)i/cos(theta));
+ double rho_lim2 = fabs((double)j/sin(theta));
+
+ double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
+ double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
+
+ double rho_lim = std::min(rho_lim1,rho_lim2);
+ double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
+ incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
+ if(incr<INCR_MIN){
+ vpERROR_TRACE("increment is too small") ;
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "increment is too small")) ;
+ }
+ update_indices(theta,i,j,incr,i1,i2,j1,j2);
+ }
+
+ while (!end)
+ {
+ end = true;
+ unsigned int i1_ = static_cast<unsigned int>(i1);
+ unsigned int j1_ = static_cast<unsigned int>(j1);
+ unsigned int i2_ = static_cast<unsigned int>(i2);
+ unsigned int j2_ = static_cast<unsigned int>(j2);
+ v1=I[i1_][j1_];
+ v2=I[i2_][j2_];
+ if (abs(v1-v2) < 1)
+ {
+
+ incr-- ;
+ end = false ;
+ if (incr==1)
+ {
+ std::cout << "In CStraightLine::GetParameters() " ;
+ std::cout << " Error Tracking " << abs(v1-v2) << std::endl ;
+ }
+ }
+ update_indices(theta,i,j,incr,i1,i2,j1,j2);
+ }
+
+ if (theta >=0 && theta <= M_PI/2)
+ {
+ if (v2 < v1)
+ {
+ theta += M_PI;
+ rho *= -1;
+ }
+ }
+
+ else
+ {
+ double jinter;
+ jinter = -c/b;
+ if (v2 < v1)
+ {
+ theta += M_PI;
+ if (jinter > 0)
+ {
+ rho *= -1;
+ }
+ }
+
+ else
+ {
+ if (jinter < 0)
+ {
+ rho *= -1;
+ }
+ }
+ }
+
+ if (vpDEBUG_ENABLE(2))
+ {
+ vpImagePoint ip, ip1, ip2;
+
+ ip.set_i( i );
+ ip.set_j( j );
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+
+ vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
+ vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
+ }
+ }
+
+}
+
+/*!
+
+ 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*/
+ ip1.set_i( PExt[0].ifloat );
+ ip1.set_j( PExt[0].jfloat );
+ ip2.set_i( PExt[1].ifloat );
+ 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 denom = 0;
+ double a1 = line1.a;
+ double b1 = line1.b;
+ double c1 = line1.c;
+ double a2 = line2.a;
+ double b2 = line2.b;
+ double c2 = line2.c;
+ double i=0, j=0;
+
+ try{
+
+ if (a1 > 0.1)
+ {
+ denom = (-(a2/a1) * b1 + b2);
+
+ //if (denom == 0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
+ {
+ std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
+ return (false);
+ }
+
+ //if (denom != 0 )
+ if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
+ {
+ j = ( (a2/a1)*c1 - c2 ) / denom;
+ i = (-b1*j - c1) / a1;
+ }
+ }
+
+ else
+ {
+ denom = (-(b2/b1) * a1 + a2);
+
+ //if (denom == 0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
+ {
+ std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
+ return (false);
+ }
+
+ //if (denom != 0 )
+ if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
+ {
+ i = ( (b2/b1)*c1 - c2 ) / denom;
+ j = (-a1*i - c1) / b1;
+ }
+ }
+ ip.set_i( i );
+ ip.set_j( j );
+
+ return (true);
+ }
+ catch(...)
+ {
+ return (false);
+ }
+}
+
+/*!
+ 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::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip1, ip2;
+
+ if (fabs(A) < fabs(B)) {
+ double i1, j1, i2, j2;
+ i1 = 0;
+ j1 = (-A*i1 -C) / B;
+ i2 = I.getHeight() - 1.0;
+ j2 = (-A*i2 -C) / B;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+
+ }
+ else {
+ double i1, j1, i2, j2;
+ j1 = 0;
+ i1 = -(B * j1 + C) / A;
+ j2 = I.getWidth() - 1.0;
+ i2 = -(B * j2 + C) / A;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+ }
+
+ ip1.set_i( PExt1.ifloat );
+ ip1.set_j( PExt1.jfloat );
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+
+ ip1.set_i( PExt2.ifloat );
+ ip1.set_j( PExt2.jfloat );
+ 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::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip1, ip2;
+
+ if (fabs(A) < fabs(B)) {
+ double i1, j1, i2, j2;
+ i1 = 0;
+ j1 = (-A*i1 -C) / B;
+ i2 = I.getHeight() - 1.0;
+ j2 = (-A*i2 -C) / B;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+
+ }
+ else {
+ double i1, j1, i2, j2;
+ j1 = 0;
+ i1 = -(B * j1 + C) / A;
+ j2 = I.getWidth() - 1.0;
+ i2 = -(B * j2 + C) / A;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+ }
+
+ ip1.set_i( PExt1.ifloat );
+ ip1.set_j( PExt1.jfloat );
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+
+ ip1.set_i( PExt2.ifloat );
+ ip1.set_j( PExt2.jfloat );
+ 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::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list,
+ const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+
+ for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
+ vpMeSite pix = *it;
+ ip.set_i( pix.ifloat );
+ ip.set_j( pix.jfloat );
+
+ if (pix.getState() == vpMeSite::M_ESTIMATOR)
+ vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
+ else
+ vpDisplay::displayCross(I, ip, 5, color,thickness);
+
+ //vpDisplay::flush(I);
+ }
+
+ vpImagePoint ip1, ip2;
+
+ if (fabs(A) < fabs(B)) {
+ double i1, j1, i2, j2;
+ i1 = 0;
+ j1 = (-A*i1 -C) / B;
+ i2 = I.getHeight() - 1.0;
+ j2 = (-A*i2 -C) / B;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+
+ }
+ else {
+ double i1, j1, i2, j2;
+ j1 = 0;
+ i1 = -(B * j1 + C) / A;
+ j2 = I.getWidth() - 1.0;
+ i2 = -(B * j2 + C) / A;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+ }
+
+ ip1.set_i( PExt1.ifloat );
+ ip1.set_j( PExt1.jfloat );
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+
+ ip1.set_i( PExt2.ifloat );
+ ip1.set_j( PExt2.jfloat );
+ 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::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list,
+ const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
+{
+ vpImagePoint ip;
+
+ for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
+ vpMeSite pix = *it;
+ ip.set_i( pix.ifloat );
+ ip.set_j( pix.jfloat );
+
+ if (pix.getState() == vpMeSite::M_ESTIMATOR)
+ vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
+ else
+ vpDisplay::displayCross(I, ip, 5, color,thickness);
+
+ //vpDisplay::flush(I);
+ }
+
+ vpImagePoint ip1, ip2;
+
+ if (fabs(A) < fabs(B)) {
+ double i1, j1, i2, j2;
+ i1 = 0;
+ j1 = (-A*i1 -C) / B;
+ i2 = I.getHeight() - 1.0;
+ j2 = (-A*i2 -C) / B;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+
+ }
+ else {
+ double i1, j1, i2, j2;
+ j1 = 0;
+ i1 = -(B * j1 + C) / A;
+ j2 = I.getWidth() - 1.0;
+ i2 = -(B * j2 + C) / A;
+
+ ip1.set_i( i1 );
+ ip1.set_j( j1 );
+ ip2.set_i( i2 );
+ ip2.set_j( j2 );
+ vpDisplay::displayLine(I, ip1, ip2, color);
+ //vpDisplay::flush(I);
+ }
+
+ ip1.set_i( PExt1.ifloat );
+ ip1.set_j( PExt1.jfloat );
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+
+ ip1.set_i( PExt2.ifloat );
+ ip1.set_j( PExt2.jfloat );
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+}
+
diff --git a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp
new file mode 100644
index 0000000..8825013
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp
@@ -0,0 +1,1368 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpMeNurbs.cpp
+ \brief Moving edges
+*/
+
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeNurbs.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#ifdef VISP_HAVE_OPENCV
+# if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+//# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/imgproc/imgproc_c.h>
+# else
+# include <cv.h>
+# endif
+#endif
+
+double computeDelta(double deltai, double deltaj);
+void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ vpMe* me, double &angle, double &convlt);
+vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP);
+bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
+#endif
+
+//Compute the angle delta = arctan(deltai/deltaj)
+//and normalize it between 0 and pi
+double
+computeDelta(double deltai, double deltaj)
+{
+ double delta;
+ delta = atan2(deltai,deltaj) ;
+ delta -= M_PI/2.0 ;
+ while (delta > M_PI) { delta -= M_PI ; }
+ while (delta < 0) { delta += M_PI ; }
+ return(delta);
+}
+
+//Check if the image point is in the image and not to close to
+//its edge to enable the computation of a convolution whith a mask.
+static
+bool outOfImage( vpImagePoint iP , int half , int rows , int cols)
+{
+ return((iP.get_i() < half + 1) || ( iP.get_i() > (rows - half - 3) )
+ ||(iP.get_j() < half + 1) || (iP.get_j() > (cols - half - 3) )) ;
+}
+
+//if iP is a edge point, it computes the angle corresponding to the
+//highest convolution result. the angle is between 0 an 179.
+//The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg alpha angle)
+//and the corresponding convolution result.
+void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ vpMe* me, double &angle, double &convlt)
+{
+ int Iheight = (int)I.getHeight();
+ int Iwidth = (int)I.getWidth();
+ angle = 0.0;
+ convlt = 0.0;
+ for (int i = 0; i < 180; i++)
+ {
+ double conv = 0.0;
+ unsigned int half;
+ int index_mask;
+ half = (me->getMaskSize() - 1) >> 1 ;
+
+ if(outOfImage( iP , (int)half + me->getStrip() , Iheight, Iwidth))
+ {
+ conv = 0.0 ;
+ }
+ else
+ {
+ if (me->getAngleStep() !=0)
+ index_mask = (int)(i/(double)me->getAngleStep());
+ else
+ throw (vpException(vpException::divideByZeroError,"angle step = 0"));
+
+ unsigned int ihalf = (unsigned int)(iP.get_i()-half) ;
+ unsigned int jhalf = (unsigned int)(iP.get_j()-half) ;
+ unsigned int ihalfa ;
+ unsigned int a ;
+ unsigned int b ;
+ for( a = 0 ; a < me->getMaskSize() ; a++ )
+ {
+ ihalfa = ihalf+a ;
+ for( b = 0 ; b < me->getMaskSize() ; b++ )
+ {
+ conv += me->getMask()[index_mask][a][b] *
+ I(ihalfa,jhalf+b) ;
+ }
+ }
+ }
+ conv = fabs(conv);
+ if (conv > convlt)
+ {
+ convlt = conv;
+ angle = vpMath::rad(i);
+ angle += M_PI/2;
+ while (angle > M_PI) { angle -= M_PI ; }
+ while (angle < 0) { angle += M_PI ; }
+ }
+ }
+}
+
+
+//Find the point belonging to the edge of the sub image which respects the following hypotheses:
+//- the value of the pixel is upper than zero.
+//- the distantce between the point and iP is less than 4 pixels.
+//The function returns the nearest point of iP which respect the hypotheses
+//If no point is found the returned point is (-1,-1)
+vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP)
+{
+ double dist = 1e6;
+ double dist_1 = 1e6;
+ vpImagePoint index(-1,-1);
+ for (unsigned int i = 0; i <= Isub.getHeight(); i++)
+ {
+ for (unsigned int j = 0; j <= Isub.getWidth(); j++)
+ {
+ if(i == 0 || i == Isub.getHeight()-1 || j == 0 || j == Isub.getWidth()-1)
+ {
+ if (Isub(i,j) > 0)
+ {
+ dist = vpImagePoint::sqrDistance(vpImagePoint(iP),vpImagePoint(i,j));
+ if (dist <= 16 && dist < dist_1)
+ {
+ dist_1 = dist;
+ index.set_ij(i,j);
+ }
+ }
+ }
+ }
+ }
+ return index;
+}
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
+//from the center of the sub image (ie the point (15,15).
+vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
+{
+ ip_edges_list->front();
+ double dist;
+ while (!ip_edges_list->outside())
+ {
+ vpImagePoint iP = ip_edges_list->value();
+ dist = vpImagePoint::sqrDistance(iP,vpImagePoint(15,15));
+ if (dist <= 16)
+ {
+ return true;
+ }
+ ip_edges_list->next();
+ }
+ return false;
+}
+#endif
+
+//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
+//from the center of the sub image (ie the point (15,15).
+bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
+{
+ double dist;
+ for(std::list<vpImagePoint>::const_iterator it=ip_edges_list->begin(); it!=ip_edges_list->end(); ++it){
+ vpImagePoint iP = *it;
+ dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15,15));
+ if (dist <= 16)
+ {
+ return true;
+ }
+ }
+ return false;
+}
+
+/***************************************/
+
+/*!
+ 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(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false),
+ cannyTh1(100.), cannyTh2(200.)
+{
+ nurbs = menurbs.nurbs;
+ dist = menurbs.dist;
+ nbControlPoints = menurbs.nbControlPoints;
+ beginPtFound = menurbs.beginPtFound;
+ endPtFound = menurbs.endPtFound;
+ enableCannyDetection = menurbs.enableCannyDetection;
+ cannyTh1 = menurbs.cannyTh1;
+ 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<unsigned char> &I)
+{
+ std::list<vpImagePoint> ptList;
+ vpImagePoint pt;
+ vpMouseButton::vpMouseButtonType b;
+
+ while (vpDisplay::getClick(I, pt, b))
+ {
+ if (b == vpMouseButton::button1)
+ {
+ //std::cout<<pt<<std::endl;
+ ptList.push_back(pt);
+ vpDisplay::displayCross(I,pt,10,vpColor::green);
+ vpDisplay::flush(I);
+ }
+ if (b == vpMouseButton::button3) break;
+ }
+ if (ptList.size() > 3)
+ initTracking(I, ptList);
+ else
+ 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<unsigned char> &I,
+ const std::list<vpImagePoint> &ptList)
+{
+ nurbs.globalCurveInterp(ptList);
+
+ sample(I);
+
+ vpMeTracker::initTracking(I) ;
+ track(I);
+}
+
+/*!
+ 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.
+*/
+void
+vpMeNurbs::sample(const vpImage<unsigned char>& I)
+{
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ double step = 1.0 / (double)me->getPointsToTrack();
+
+ // Delete old list
+ list.clear();
+
+ vpImagePoint ip;
+ double u = 0.0;
+ vpImagePoint *pt = NULL;
+ vpImagePoint pt_1(-rows,-cols);
+ while (u <= 1.0)
+ {
+ if (pt!=NULL) delete[] pt;
+ pt = nurbs.computeCurveDersPoint(u, 1);
+ double delta = computeDelta(pt[1].get_i(),pt[1].get_j());
+
+ // If point is in the image, add to the sample list
+ if(!outOfImage(pt[0], 0, rows, cols) && vpImagePoint::sqrDistance(pt[0],pt_1) >= vpMath::sqr(me->getSampleStep()))
+ {
+ vpMeSite pix ; //= list.value();
+ pix.init(pt[0].get_i(), pt[0].get_j(), delta) ;
+ pix.setDisplay(selectDisplay) ;
+
+ list.push_back(pix);
+ pt_1 = pt[0];
+ }
+ u = u+step;
+ }
+ if (pt!=NULL) 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<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
+ vpMeSite s = *it;//current reference pixel
+
+ if (s.getState() != vpMeSite::NO_SUPPRESSION)
+ {
+ it = list.erase(it);
+ }
+ else
+ ++it;
+ }
+}
+
+
+/*!
+ 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;
+ double d = 1e6;
+ double d_1 = 1e6;
+ std::list<vpMeSite>::iterator it=list.begin();
+
+ vpImagePoint Cu;
+ vpImagePoint* der = NULL;
+ double step = 0.01;
+ while (u < 1 && it!=list.end())
+ {
+ vpMeSite s = *it;
+ vpImagePoint pt(s.i,s.j);
+ while (d <= d_1 && u<1)
+ {
+ Cu = nurbs.computeCurvePoint(u);
+ d_1=d;
+ d = vpImagePoint::distance(pt,Cu);
+ u +=step;
+ }
+
+ u-=step;
+ if (der != NULL) delete[] der;
+ der = nurbs.computeCurveDersPoint(u, 1);
+ //vpImagePoint toto(der[0].get_i(),der[0].get_j());
+ //vpDisplay::displayCross(I,toto,4,vpColor::red);
+
+ s.alpha = computeDelta(der[1].get_i(),der[1].get_j());
+ *it = s;
+ ++it;
+ d = 1e6;
+ d_1 = 1.5e6;
+ }
+ if (der != NULL) 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<unsigned char> &I)
+{
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+
+ vpImagePoint* begin = NULL;
+ vpImagePoint* end = NULL;
+
+ begin = nurbs.computeCurveDersPoint(0.0,1);
+ end = nurbs.computeCurveDersPoint(1.0,1);
+
+ //Check if the two extremities are not to close to eachother.
+ double d = vpImagePoint::distance(begin[0],end[0]);
+ double threshold = 3*me->getSampleStep();
+ double sample_step = me->getSampleStep();
+ vpImagePoint pt;
+ if ( d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/)
+ {
+ vpMeSite P ;
+
+ //Init vpMeSite
+ P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign) ;
+ P.setDisplay(selectDisplay) ;
+
+ //Set the range
+ unsigned int memory_range = me->getRange() ;
+ me->setRange(2);
+
+ //Point at the beginning of the list
+ bool beginPtAdded = false;
+ vpImagePoint pt_max = begin[0];
+ double angle = atan2(begin[1].get_i(),begin[1].get_j());
+ double co = vpMath::abs(cos(angle));
+ co = co * vpMath::sign(begin[1].get_j());
+ double si = vpMath::abs(sin(angle));
+ si = si * vpMath::sign(begin[1].get_i());
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat - si*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat - co*sample_step ; P.j = (int)P.jfloat ;
+ pt.set_ij(P.ifloat,P.jfloat);
+ if (vpImagePoint::distance(end[0],pt) < threshold) break;
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false);
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_front(P) ;
+ beginPtAdded = true;
+ pt_max = pt;
+ if (vpDEBUG_ENABLE(3)) {
+ vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+
+ if (!beginPtAdded) beginPtFound++;
+
+ P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
+ P.setDisplay(selectDisplay);
+
+ bool endPtAdded = false;
+ angle = atan2(end[1].get_i(),end[1].get_j());
+ co = vpMath::abs(cos(angle));
+ co = co * vpMath::sign(end[1].get_j());
+ si = vpMath::abs(sin(angle));
+ si = si * vpMath::sign(end[1].get_i());
+ for (int i=0 ; i < 3 ; i++)
+ {
+ P.ifloat = P.ifloat + si*sample_step ; P.i = (int)P.ifloat ;
+ P.jfloat = P.jfloat + co*sample_step ; P.j = (int)P.jfloat ;
+ pt.set_ij(P.ifloat,P.jfloat);
+ if (vpImagePoint::distance(begin[0],pt) < threshold) break;
+ if(!outOfImage(P.i, P.j, 5, rows, cols))
+ {
+ P.track(I,me,false);
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.push_back(P) ;
+ endPtAdded = true;
+ if (vpDEBUG_ENABLE(3)) {
+ vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
+ }
+ }
+ else {
+ if (vpDEBUG_ENABLE(3)) {
+ vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
+ }
+ }
+ }
+ }
+ if (!endPtAdded) endPtFound++;
+ me->setRange(memory_range);
+ }
+ else
+ {
+ list.pop_front();
+ }
+ /*if(begin != NULL)*/ delete[] begin;
+ /*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 initialize at this points.
+
+ This method is practicle 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.
+*/
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+void
+vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> &I)
+#else
+void
+vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
+#endif
+{
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+ vpMeSite pt = list.front();
+ vpImagePoint firstPoint(pt.ifloat,pt.jfloat);
+ pt = list.back();
+ vpImagePoint lastPoint(pt.ifloat,pt.jfloat);
+ if (beginPtFound >=3 && farFromImageEdge(I, firstPoint))
+ {
+ vpImagePoint *begin = NULL;
+ begin = nurbs.computeCurveDersPoint(0.0,1);
+ vpImage<unsigned char> Isub(32,32); //Sub image.
+ vpImagePoint topLeft(begin[0].get_i()-15,begin[0].get_j()-15);
+ vpRect rect(topLeft,32,32);
+
+ vpDisplay::displayRectangle(I,rect,vpColor::green);
+
+ vpImageTools::createSubImage(I,rect,Isub);
+
+ vpImagePoint lastPtInSubIm(begin[0]);
+ double u = 0.0;
+ double step =0.0001;
+ //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
+ while (inRectangle(lastPtInSubIm,rect) && u < 1)
+ {
+ u += step;
+ lastPtInSubIm = nurbs.computeCurvePoint(u);
+ }
+
+ u -= step;
+ if( u > 0)
+ lastPtInSubIm = nurbs.computeCurvePoint(u);
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(Isub, Ip);
+
+ IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
+ cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
+
+ vpImageConvert::convert(dst, Isub);
+#endif
+
+ vpImagePoint firstBorder(-1,-1);
+
+ firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
+
+ std::list<vpImagePoint> ip_edges_list;
+ if (firstBorder != vpImagePoint(-1, -1))
+ {
+ unsigned int dir;
+ double fi = firstBorder.get_i();
+ double fj = firstBorder.get_j();
+ double w = Isub.getWidth()-1;
+ double h = Isub.getHeight()-1;
+ //if (firstBorder.get_i() == 0) dir = 4;
+ if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
+ //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
+ else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
+ //else if (firstBorder.get_j() == 0) dir = 2;
+ else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
+ //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
+ else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
+ computeFreemanChainElement(Isub, firstBorder , dir);
+ unsigned int firstDir = dir;
+ ip_edges_list.push_back( firstBorder );
+ vpImagePoint border(firstBorder);
+ vpImagePoint dBorder;
+ do
+ {
+ computeFreemanParameters(dir, dBorder);
+ border = border + dBorder;
+ vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
+
+ ip_edges_list.push_back( border );
+
+ computeFreemanChainElement(Isub, border , dir);
+ } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
+ }
+
+ if (findCenterPoint(&ip_edges_list))
+ {
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); /*++it*/){
+ vpMeSite s = *it;
+ vpImagePoint iP(s.ifloat,s.jfloat);
+ if (inRectangle(iP,rect))
+ it = list.erase(it) ;
+ else
+ break;
+ }
+
+ std::list<vpMeSite>::iterator itList=list.begin();
+ double convlt;
+ double delta = 0;
+ int nbr = 0;
+ std::list<vpMeSite> addedPt;
+ for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
+ vpMeSite s = *itList;
+ vpImagePoint iPtemp = *itEdges + topLeft;
+ vpMeSite pix;
+ pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
+ dist = vpMeSite::sqrDistance(s,pix);
+ if (dist >= vpMath::sqr(me->getSampleStep())/*25*/)
+ {
+ bool exist = false;
+ for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
+ dist = vpMeSite::sqrDistance(pix, *itAdd);
+ if (dist < vpMath::sqr(me->getSampleStep())/*25*/)
+ exist = true;
+ }
+ if (!exist)
+ {
+ findAngle(I, iPtemp, me, delta, convlt);
+ pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
+ pix.setDisplay(selectDisplay);
+ --itList;
+ list.insert(itList, pix);
+ ++itList;
+ addedPt.push_front(pix);
+ nbr++;
+ }
+ }
+ }
+
+ unsigned int memory_range = me->getRange();
+ me->setRange(3);
+ std::list<vpMeSite>::iterator itList2=list.begin();
+ for (int j = 0; j < nbr; j++)
+ {
+ vpMeSite s = *itList2;
+ s.track(I,me,false);
+ *itList2 = s;
+ ++itList2;
+ }
+ me->setRange(memory_range);
+ }
+
+ /* if (begin != NULL) */ delete[] begin;
+ beginPtFound = 0;
+ }
+
+ if(endPtFound >= 3 && farFromImageEdge(I, lastPoint))
+ {
+ vpImagePoint *end = NULL;
+ end = nurbs.computeCurveDersPoint(1.0,1);
+
+ vpImage<unsigned char> Isub(32,32); //Sub image.
+ vpImagePoint topLeft(end[0].get_i()-15,end[0].get_j()-15);
+ vpRect rect(topLeft,32,32);
+
+ vpDisplay::displayRectangle(I,rect,vpColor::green);
+
+ vpImageTools::createSubImage(I,rect,Isub);
+
+ vpImagePoint lastPtInSubIm(end[0]);
+ double u = 1.0;
+ double step =0.0001;
+ //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
+ while (inRectangle(lastPtInSubIm,rect) && u > 0)
+ {
+ u -= step;
+ lastPtInSubIm = nurbs.computeCurvePoint(u);
+ }
+
+ u += step;
+ if( u < 1.0)
+ lastPtInSubIm = nurbs.computeCurvePoint(u);
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
+#else
+ IplImage* Ip = NULL;
+ vpImageConvert::convert(Isub, Ip);
+
+ IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
+ cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
+
+ vpImageConvert::convert(dst, Isub);
+#endif
+
+ vpImagePoint firstBorder(-1,-1);
+
+ firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
+
+ std::list<vpImagePoint> ip_edges_list;
+ if (firstBorder != vpImagePoint(-1, -1))
+ {
+ unsigned int dir;
+ double fi = firstBorder.get_i();
+ double fj = firstBorder.get_j();
+ double w = Isub.getWidth()-1;
+ double h = Isub.getHeight()-1;
+ //if (firstBorder.get_i() == 0) dir = 4;
+ if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
+ //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
+ else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
+ //else if (firstBorder.get_j() == 0) dir = 2;
+ else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
+ //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
+ else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
+
+
+ computeFreemanChainElement(Isub, firstBorder , dir);
+ unsigned int firstDir = dir;
+ ip_edges_list.push_back( firstBorder );
+ vpImagePoint border(firstBorder);
+ vpImagePoint dBorder;
+ do
+ {
+ computeFreemanParameters(dir, dBorder);
+ border = border + dBorder;
+ vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
+
+ ip_edges_list.push_back( border );
+
+ computeFreemanChainElement(Isub, border , dir);
+ } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
+ }
+
+ if (findCenterPoint(&ip_edges_list))
+ {
+// list.end();
+ vpMeSite s;
+ while(true)//{//!list.outside())
+ {
+ s = list.back();//list.value() ;
+ vpImagePoint iP(s.ifloat,s.jfloat);
+ if (inRectangle(iP,rect))
+ {
+ list.erase(list.end()) ;
+// list.end();
+ }
+ else
+ break;
+ }
+
+ std::list<vpMeSite>::iterator itList = list.end();
+ --itList; // Move on the last element
+ double convlt;
+ double delta;
+ int nbr = 0;
+ std::list<vpMeSite> addedPt;
+ for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
+ s = *itList;
+ vpImagePoint iPtemp = *itEdges + topLeft;
+ vpMeSite pix;
+ pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
+ dist = vpMeSite::sqrDistance(s,pix);
+ if (dist >= vpMath::sqr(me->getSampleStep()))
+ {
+ bool exist = false;
+ for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
+ dist = vpMeSite::sqrDistance(pix, *itAdd);
+ if (dist < vpMath::sqr(me->getSampleStep()))
+ exist = true;
+ }
+ if (!exist)
+ {
+ findAngle(I, iPtemp, me, delta, convlt);
+ pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
+ pix.setDisplay(selectDisplay);
+ list.push_back(pix);
+ addedPt.push_back(pix);
+ nbr++;
+ }
+ }
+ }
+
+ unsigned int memory_range = me->getRange();
+ me->setRange(3);
+ std::list<vpMeSite>::iterator itList2 = list.end();
+ --itList2; // Move to the last element
+ for (int j = 0; j < nbr; j++)
+ {
+ vpMeSite me_s = *itList2;
+ me_s.track(I,me,false);
+ *itList2 = me_s;
+ --itList2;
+ }
+ me->setRange(memory_range);
+ }
+
+ /* if (end != NULL) */ delete[] end;
+ endPtFound = 0;
+ }
+#else
+ vpTRACE("To use the canny detection, OpenCV has to be installed.");
+#endif
+}
+
+
+/*!
+ 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<unsigned char> &I)
+{
+ unsigned int n = numberOfSignal();
+ double nbPt = floor(dist / me->getSampleStep());
+
+ if ((double)n < 0.7*nbPt)
+ {
+ sample(I);
+ vpMeTracker::initTracking(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<unsigned char> &I)
+{
+ int rows = (int)I.getHeight() ;
+ int cols = (int)I.getWidth() ;
+ vpImagePoint* iP = NULL;
+
+ int n = (int)numberOfSignal();
+
+// list.front();
+ std::list<vpMeSite>::iterator it=list.begin();
+ std::list<vpMeSite>::iterator itNext=list.begin();
+ ++itNext;
+
+ unsigned int range_tmp = me->getRange();
+ me->setRange(2);
+
+ while(itNext!=list.end() && n <= me->getPointsToTrack())
+ {
+ vpMeSite s = *it;//current reference pixel
+ vpMeSite s_next = *itNext;//current reference pixel
+
+ double d = vpMeSite::sqrDistance(s,s_next);
+ if(d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600)
+ {
+ vpImagePoint iP0(s.ifloat,s.jfloat);
+ vpImagePoint iPend(s_next.ifloat,s_next.jfloat);
+ vpImagePoint iP_1(s.ifloat,s.jfloat);
+
+ double u = 0.0;
+ double ubegin = 0.0;
+ double uend = 0.0;
+ double dmin1_1 = 1e6;
+ double dmin2_1 = 1e6;
+ while(u < 1)
+ {
+ u+=0.01;
+ double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iP0);
+ double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iPend);
+
+ if (dmin1 < dmin1_1)
+ {
+ dmin1_1 = dmin1;
+ ubegin = u;
+ }
+
+ if (dmin2 < dmin2_1)
+ {
+ dmin2_1 = dmin2;
+ uend = u;
+ }
+ }
+ u = ubegin;
+
+ //if(( u != 1.0 || uend != 1.0)
+ if( (std::fabs(u-1.0) > std::fabs(vpMath::maximum(u, 1.0))*std::numeric_limits<double>::epsilon())
+ || (std::fabs(uend-1.0) > std::fabs(vpMath::maximum(uend, 1.0))*std::numeric_limits<double>::epsilon()))
+ {
+ iP = nurbs.computeCurveDersPoint(u, 1);
+
+ while (vpImagePoint::sqrDistance(iP[0],iPend) > vpMath::sqr(me->getSampleStep()) && u < uend)
+ {
+ u+=0.01;
+ /*if (iP!=NULL)*/ {
+ delete[] iP;
+ iP = NULL;
+ }
+ iP = nurbs.computeCurveDersPoint(u, 1);
+ if ( vpImagePoint::sqrDistance(iP[0],iP_1) > vpMath::sqr(me->getSampleStep()) && !outOfImage(iP[0], 0, rows, cols))
+ {
+ double delta = computeDelta(iP[1].get_i(),iP[1].get_j());
+ vpMeSite pix ; //= list.value();
+ pix.init(iP[0].get_i(), iP[0].get_j(), delta) ;
+ pix.setDisplay(selectDisplay) ;
+ pix.track(I,me,false);
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ list.insert(it, pix);
+ iP_1 = iP[0];
+ }
+ }
+ }
+ /*if (iP!=NULL)*/ {
+ delete[] iP;
+ iP = NULL;
+ }
+ }
+ }
+ ++it;
+ ++itNext;
+ }
+ 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
+ // Loop through list of sites to track
+ list.front();
+ while(!list.nextOutside())
+ {
+ vpMeSite s = list.value() ;//current reference pixel
+ vpMeSite s_next = list.nextValue() ;//current reference pixel
+
+ if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
+ {
+ s_next.setState(vpMeSite::TOO_NEAR);
+
+ list.next();
+ list.modify(s_next);
+ if (!list.nextOutside()) list.next();
+ }
+ else
+ list.next() ;
+ }
+#endif
+ std::list<vpMeSite>::const_iterator it=list.begin();
+ std::list<vpMeSite>::iterator itNext=list.begin();
+ ++itNext;
+ for(;itNext!=list.end();){
+ vpMeSite s = *it;//current reference pixel
+ vpMeSite s_next = *itNext;//current reference pixel
+
+ if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep())){
+ s_next.setState(vpMeSite::TOO_NEAR);
+
+ *itNext = s_next;
+ ++it;
+ ++itNext;
+ if(itNext!=list.end()){
+ ++it;
+ ++itNext;
+ }
+ }
+ else{
+ ++it;
+ ++itNext;
+ }
+ }
+}
+
+
+/*!
+ Track the edge in the image I.
+
+ \param I : Image in which the edge appears.
+*/
+void
+vpMeNurbs::track(const vpImage<unsigned char> &I)
+{
+ //Tracking des vpMeSites
+ vpMeTracker::track(I);
+
+ //Suppress points which are too close to each other
+ supressNearPoints();
+
+ //Suppressions des points ejectes par le tracking
+ suppressPoints();
+
+ if (list.size() == 1)
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
+
+ //Recalcule les parametres
+// nurbs.globalCurveInterp(list);
+ nurbs.globalCurveApprox(list,nbControlPoints);
+
+ //On resample localement
+ localReSample(I);
+
+ seekExtremities(I);
+ if(enableCannyDetection)
+ seekExtremitiesCanny(I);
+
+// nurbs.globalCurveInterp(list);
+ nurbs.globalCurveApprox(list,nbControlPoints);
+
+ double u = 0.0;
+ vpImagePoint pt;
+ vpImagePoint pt_1;
+ dist = 0;
+ while (u<=1.0)
+ {
+ pt = nurbs.computeCurvePoint(u);
+ //if(u!=0)
+ if(std::fabs(u) > std::numeric_limits<double>::epsilon())
+ dist = dist + vpImagePoint::distance(pt,pt_1);
+ pt_1 = pt;
+ u=u+0.01;
+ }
+
+ updateDelta();
+
+ 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 col : Color of the displayed line.
+
+ */
+void
+vpMeNurbs::display(const vpImage<unsigned char>&I, vpColor col)
+{
+ vpMeNurbs::display(I,nurbs,col);
+}
+
+
+/*!
+ Considering a pixel iP compute the next element of the Freeman chain
+ code.
+
+ According to the gray level of pixel iP and his eight neighbors determine
+ the next element of the chain in order to turn around the dot
+ counterclockwise.
+
+ \param I : The image we are working with.
+ \param iP : The current pixel.
+ \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
+ with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
+
+ \return false if an element cannot be found. Occurs for example with an area
+ constituted by a single pixel. Return true if success.
+*/
+bool
+vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I,
+ vpImagePoint &iP,
+ unsigned int &element)
+{
+ vpImagePoint diP;
+ vpImagePoint iPtemp;
+ if (hasGoodLevel( I, iP )) {
+ // get the point on the right of the point passed in
+ computeFreemanParameters((element + 2) %8, diP);
+ iPtemp = iP + diP;
+ if (hasGoodLevel( I, iPtemp )) {
+ element = (element + 2) % 8; // turn right
+ }
+ else {
+ computeFreemanParameters((element + 1) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 1) % 8; // turn diag right
+ }
+ else {
+ computeFreemanParameters(element, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ //element = element; // keep same dir
+ }
+ else {
+ computeFreemanParameters((element + 7) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 7) %8; // turn diag left
+ }
+ else {
+ computeFreemanParameters((element + 6) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 6) %8 ; // turn left
+ }
+ else {
+ computeFreemanParameters((element + 5) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 5) %8 ; // turn diag down
+ }
+ else {
+ computeFreemanParameters((element + 4) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 4) %8 ; // turn down
+ }
+ else {
+ computeFreemanParameters((element + 3) %8, diP);
+ iPtemp = iP + diP;
+
+ if ( hasGoodLevel( I, iPtemp )) {
+ element = (element + 3) %8 ; // turn diag right down
+ }
+ else {
+ // No neighbor with a good level
+ //
+ return false;
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ else {
+ return false;
+ }
+ return true;
+}
+
+
+/*!
+ Check if the pixel iP is in the image and has
+ a good level to belong to the edge.
+
+ \param I : Image.
+ \param iP : Pixel to test
+
+ \return true : If the pixel iP is in the area and
+ has a value greater than 0.
+
+ \return false : Otherwise
+*/
+bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char>& I,
+ const vpImagePoint& iP) const
+{
+ if( !isInImage( I, iP ) )
+ return false;
+
+ if( I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+/*!
+ Test if a pixel is in the image. Points of the border are not considered to
+ be in the image.
+
+ \param I : The image.
+ \param iP : An image point.
+
+ \return true if the image point \e iP is in the image and false
+ otherwise.
+*/
+bool vpMeNurbs::isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const
+{
+ return (iP.get_i() >= 0
+ && iP.get_j() >= 0
+ && iP.get_i() < I.getHeight()
+ && iP.get_j() < I.getWidth());
+}
+
+
+/*!
+ Gives the displacement corresponding to the value of the parameter \e element.
+
+ - If element = 0 diP = (0,1).
+ - If element = 1 diP = (1,1).
+ - If element = 2 diP = (1,0).
+ - If element = 3 diP = (1,-1).
+ - If element = 4 diP = (0,-1).
+ - If element = 5 diP = (-1,-1).
+ - If element = 6 diP = (-1,0).
+ - If element = 7 diP = (-1,1).
+
+ \param element : the element value(typically given by the method computeFreemanChainElement).
+ \param diP : the output parameter which contains the displacement cooresponding to the value of \e element.
+*/
+void
+vpMeNurbs::computeFreemanParameters( unsigned int element, vpImagePoint &diP)
+{
+ /*
+ 5 6 7
+ \ | /
+ \|/
+ 4 ------- 0
+ /|\
+ / | \
+ 3 2 1
+ */
+ switch(element) {
+ case 0: // go right
+ diP.set_ij(0,1);
+ break;
+
+ case 1: // go right top
+ diP.set_ij(1,1);
+ break;
+
+ case 2: // go top
+ diP.set_ij(1,0);
+ break;
+
+ case 3:
+ diP.set_ij(1,-1);
+ break;
+
+ case 4:
+ diP.set_ij(0,-1);
+ break;
+
+ case 5:
+ diP.set_ij(-1,-1);
+ break;
+
+ case 6:
+ diP.set_ij(-1,0);
+ break;
+
+ case 7:
+ diP.set_ij(-1,1);
+ break;
+ }
+}
+
+
+/*!
+ Check if the point is far enough from the image edges
+
+ \param I : The image.
+ \param iP : An image point.
+
+ \return true if the point iP is at least 20 pixels far from the image edeges.
+*/
+bool
+vpMeNurbs::farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP)
+{
+ unsigned int height = I.getHeight();
+ unsigned int width = I.getWidth();
+ 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.
+*/
+void vpMeNurbs::display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color)
+{
+ double u = 0.0;
+ vpImagePoint pt;
+ while (u <= 1)
+ {
+ pt = n.computeCurvePoint(u);
+ vpDisplay::displayCross(I,pt,4,color);
+ u+=0.01;
+ }
+}
+
+/*!
+
+ 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.
+*/
+void vpMeNurbs::display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color)
+{
+ double u = 0.0;
+ vpImagePoint pt;
+ while (u <= 1)
+ {
+ pt = n.computeCurvePoint(u);
+ vpDisplay::displayCross(I,pt,4,color);
+ u+=0.01;
+ }
+}
+
diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp
new file mode 100644
index 0000000..2f1cbd2
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp
@@ -0,0 +1,734 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Eric Marchand
+ * Andrew Comport
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMeSite.cpp
+ \brief Moving edges
+*/
+
+
+
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/core/vpTrackingException.h>
+#include <stdlib.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <visp3/me/vpMeSite.h>
+
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+static
+bool horsImage(int i , int j, int half, int rows, int cols)
+{
+ //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
+ int half_1 = half + 1;
+ int half_3 = half + 3;
+ //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
+ return( (0 < (half_1 - i) ) || ( (i - rows + half_3) > 0 ) || ( 0 < (half_1 -j) ) || ( (j - cols + half_3) > 0 ) ) ;
+}
+#endif
+
+void
+vpMeSite::init()
+{
+ // Site components
+ alpha = 0.0 ;
+ convlt = 0.0 ;
+ weight=-1;
+
+ selectDisplay = NONE ;
+
+ // Pixel components
+ i = 0 ;
+ j = 0 ;
+ v = 0 ;
+ ifloat =i ;
+ jfloat = j ;
+ i_1 = 0 ;
+ j_1 = 0 ;
+
+ mask_sign = 1 ;
+
+ normGradient = 0;
+
+ state = NO_SUPPRESSION;
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ suppress = 0;
+#endif
+}
+
+vpMeSite::vpMeSite()
+ : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
+ convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ , suppress(0)
+#endif
+{
+}
+
+vpMeSite::vpMeSite(double ip, double jp)
+ : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
+ convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ , suppress(0)
+#endif
+{
+ i = vpMath::round(ip) ;
+ j = vpMath::round(jp) ;
+ ifloat = ip ;
+ jfloat = jp ;
+}
+
+/*!
+ Copy constructor.
+*/
+vpMeSite::vpMeSite (const vpMeSite &mesite)
+ : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
+ convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ , suppress(0)
+#endif
+{
+ *this = mesite;
+}
+
+// More an Update than init
+// For points in meter form (to avoid approximations)
+void
+vpMeSite::init(double ip, double jp, double alphap)
+{
+ // Note: keep old value of convlt, suppress and contrast
+ selectDisplay = NONE ;
+
+ ifloat = ip;
+ i= vpMath::round(ip);
+ jfloat = jp ;
+ j = vpMath::round(jp);
+ alpha = alphap ;
+ mask_sign =1 ;
+
+ v = 0 ;
+ i_1 = 0 ;
+ j_1 = 0 ;
+
+}
+
+// initialise with convolution
+void
+vpMeSite::init(double ip, double jp, double alphap, double convltp)
+{
+ selectDisplay = NONE ;
+ ifloat = ip ;
+ i= (int)ip ;
+ jfloat = jp ;
+ j =(int)jp ;
+ alpha = alphap ;
+ convlt = convltp;
+ mask_sign =1 ;
+
+ v = 0 ;
+ i_1 = 0 ;
+ j_1 = 0 ;
+}
+// initialise with convolution and sign
+void
+vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
+{
+ selectDisplay = NONE ;
+ ifloat = ip ;
+ i= (int)ip ;
+ jfloat = jp ;
+ j =(int)jp ;
+ alpha = alphap ;
+ convlt = convltp;
+ mask_sign = sign ;
+
+ v = 0 ;
+ i_1 = 0 ;
+ j_1 = 0 ;
+}
+
+vpMeSite &vpMeSite::operator=(const vpMeSite &m)
+{
+ i = m.i;
+ j = m.j;
+ i_1 = m.i_1;
+ j_1 = m.j_1;
+ ifloat = m.ifloat;
+ jfloat = m.jfloat;
+ v = m.v;
+ mask_sign = m.mask_sign;
+ alpha = m.alpha;
+ convlt = m.convlt;
+ normGradient = m.normGradient;
+ weight = m.weight;
+ selectDisplay = m.selectDisplay;
+ state = m.state;
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ suppress = m.suppress;
+#endif
+
+ 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<unsigned char> &I, const int range)
+{
+
+ int k ;
+
+ int n;
+ double ii , jj ;
+ vpMeSite *list_query_pixels ;
+ list_query_pixels = NULL ;
+
+ unsigned int range_ = static_cast<unsigned int>(range);
+ // Size of query list includes the point on the line
+ list_query_pixels = new vpMeSite[2 * range_ + 1] ;
+
+ // range : +/- the range within which the pixel's
+ //correspondent will be sought
+
+ double salpha = sin(alpha);
+ double calpha = cos(alpha);
+ n = 0 ;
+ vpImagePoint ip;
+
+ for(k = -range ; k <= range ; k++)
+ {
+ ii = (ifloat+k*salpha);
+ jj = (jfloat+k*calpha);
+
+ // Display
+ if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RANGE)) {
+ ip.set_i( ii );
+ ip.set_j( jj );
+ vpDisplay::displayCross(I, ip, 1, vpColor::yellow) ;
+ }
+
+ // Copy parent's convolution
+ vpMeSite pel ;
+ pel.init(ii, jj, alpha, convlt,mask_sign) ;
+ pel.setDisplay(selectDisplay) ;// Display
+
+ // Add site to the query list
+ list_query_pixels[n] = pel ;
+ n++ ;
+ }
+
+ return(list_query_pixels) ;
+}
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+// ===================================================================
+/*!
+ * get the sign (according to the difference of values of the intensities of the extremities).
+ * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set.
+ * \param I : Image in which the sign is computed.
+ * \param range : +/- the range within which the pixel's correspondent is sought
+ * \post : mask_sign is computed
+ */
+// ===================================================================
+void
+vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
+{
+
+ int k ;
+
+ double salpha = sin(alpha);
+ double calpha = cos(alpha);
+
+ //First extremity
+ k = -range ;
+ unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
+ unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
+
+ //Second extremity
+ k = range ;
+ unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
+ unsigned int j2 = static_cast<unsigned int>(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 mask_sign = -1 ;
+}
+#endif
+
+// Specific function for ME
+double
+vpMeSite::convolution(const vpImage<unsigned char>&I, const vpMe *me)
+{
+ int half;
+ unsigned int index_mask ;
+ int height_ = static_cast<int>(I.getHeight());
+ int width_ = static_cast<int>(I.getWidth());
+
+ double conv = 0.0 ;
+ unsigned int msize = me->getMaskSize();
+ half = (static_cast<int>(msize) - 1) >> 1 ;
+
+ if(horsImage( i , j , half + me->getStrip() , height_, width_))
+ {
+ conv = 0.0 ;
+ i = 0 ; j = 0 ;
+ }
+ else
+ {
+ // Calculate tangent angle from normal
+ double theta = alpha+M_PI/2;
+ // Move tangent angle to within 0->M_PI for a positive
+ // mask index
+ while (theta<0) theta += M_PI;
+ while (theta>M_PI) theta -= M_PI;
+
+ // Convert radians to degrees
+ int thetadeg = vpMath::round(theta * 180 / M_PI) ;
+
+ if(abs(thetadeg) == 180 )
+ {
+ thetadeg= 0 ;
+ }
+
+ index_mask = (unsigned int)(thetadeg/(double)me->getAngleStep());
+
+ unsigned int i_ = static_cast<unsigned int>(i);
+ unsigned int j_ = static_cast<unsigned int>(j);
+ unsigned int half_ = static_cast<unsigned int>(half);
+
+ unsigned int ihalf = i_-half_ ;
+ unsigned int jhalf = j_-half_ ;
+ unsigned int ihalfa ;
+
+ for(unsigned int a = 0 ; a < msize ; a++ )
+ {
+ ihalfa = ihalf+a ;
+ for(unsigned int b = 0 ; b < msize ; b++ )
+ {
+ conv += mask_sign* me->getMask()[index_mask][a][b] *
+ // I(i-half+a,j-half+b) ;
+ I(ihalfa,jhalf+b) ;
+ }
+ }
+
+ }
+
+ return(conv) ;
+}
+
+
+/*!
+
+ Specific function for ME.
+
+ \warning To display the moving edges graphics a call to vpDisplay::flush()
+ is needed.
+
+*/
+void
+vpMeSite::track(const vpImage<unsigned char>& I,
+ const vpMe *me,
+ const bool test_contraste)
+{
+ // vpMeSite *list_query_pixels ;
+ // int max_rank =0 ;
+ // int max_rank1=0 ;
+ // int max_rank2 = 0;
+ // double convolution = 0 ;
+ // double max_convolution = 0 ;
+ // double max = 0 ;
+ // double contraste = 0;
+ // // vpDisplay::display(I) ;
+ // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
+ // // vpDisplay::getClick(I) ;
+ //
+ // // range = +/- range of pixels within which the correspondent
+ // // of the current pixel will be sought
+ // int range = me->getRange() ;
+ //
+ // // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
+ // list_query_pixels = getQueryList(I, range) ;
+ //
+ // double contraste_max = 1 + me->getMu2();
+ // double contraste_min = 1 - me->getMu1();
+ //
+ // // array in which likelihood ratios will be stored
+ // double *likelihood= new double[ 2 * range + 1 ] ;
+ //
+ // int ii_1 = i ;
+ // int jj_1 = j ;
+ // i_1 = i ;
+ // j_1 = j ;
+ // double threshold;
+ // threshold = me->getThreshold();
+ //
+ // // std::cout <<"---------------------"<<std::endl ;
+ // for(int n = 0 ; n < 2 * range + 1 ; n++)
+ // {
+ //
+ // // convolution results
+ // convolution = list_query_pixels[n].convolution(I, me) ;
+ //
+ // // luminance ratio of reference pixel to potential correspondent pixel
+ // // the luminance must be similar, hence the ratio value should
+ // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
+ // if( test_contraste )
+ // {
+ // // Include this to eliminate temporal calculation
+ // if (convlt==0)
+ // {
+ // std::cout << "vpMeSite::track : Division by zero convlt = 0" << std::endl ;
+ // delete []list_query_pixels ;
+ // delete []likelihood;
+ // throw(vpTrackingException(vpTrackingException::initializationError,
+ // "Division by zero")) ;
+ // }
+ //
+ // // contraste = fabs(convolution / convlt) ;
+ // contraste = convolution / convlt ;
+ // // likelihood ratios
+ // if((contraste > contraste_min) && (contraste < contraste_max))
+ // likelihood[n] = fabs(convolution + convlt ) ;
+ // else
+ // likelihood[n] = 0 ;
+ // }
+ // else
+ // likelihood[n] = fabs(2*convolution) ;
+ //
+ //
+ // // establishment of the maximal likelihood ratios's rank
+ // // in the array, the value of the likelihood ratio can now be
+ // // referenced by its rank in the array
+ // if (likelihood[n] > max)
+ // {
+ // max_convolution= convolution;
+ // max = likelihood[n] ;
+ // max_rank = n ;
+ // max_rank2 = max_rank1;
+ // max_rank1 = max_rank;
+ // }
+ //
+ // }
+ //
+ // // test on the likelihood threshold if threshold==-1 then
+ // // the me->threshold is selected
+ //
+ // vpImagePoint ip;
+ //
+ // // if (test_contrast)
+ // if(max > threshold)
+ // {
+ // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
+ // {
+ // ip.set_i( list_query_pixels[max_rank].i );
+ // ip.set_j( list_query_pixels[max_rank].j );
+ // vpDisplay::displayPoint(I, ip, vpColor::red);
+ // }
+ //
+ // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by the vpMeSite of max likelihood
+ // normGradient = vpMath::sqr(max_convolution);
+ //
+ // convlt = max_convolution;
+ // i_1 = ii_1; //list_query_pixels[max_rank].i ;
+ // j_1 = jj_1; //list_query_pixels[max_rank].j ;
+ // delete []list_query_pixels ;
+ // delete []likelihood;
+ // }
+ // else //none of the query sites is better than the threshold
+ // {
+ // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
+ // {
+ // ip.set_i( list_query_pixels[max_rank].i );
+ // ip.set_j( list_query_pixels[max_rank].j );
+ // vpDisplay::displayPoint(I, ip, vpColor::green);
+ // }
+ // normGradient = 0 ;
+ // if(max == 0)
+ // suppress = 1; // contrast suppression
+ // else
+ // suppress = 2; // threshold suppression
+ //
+ // delete []list_query_pixels ;
+ // delete []likelihood; // modif portage
+ // }
+
+ vpMeSite *list_query_pixels ;
+ int max_rank =-1 ;
+ // int max_rank1=-1 ;
+ // int max_rank2 = -1;
+ double convolution_ = 0 ;
+ double max_convolution = 0 ;
+ double max = 0 ;
+ double contraste = 0;
+ // vpDisplay::display(I) ;
+ // vpERROR_TRACE("getclcik %d",me->range) ;
+ // vpDisplay::getClick(I) ;
+
+ // range = +/- range of pixels within which the correspondent
+ // of the current pixel will be sought
+ unsigned int range = me->getRange() ;
+
+ // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
+ list_query_pixels = getQueryList(I, (int)range) ;
+
+ double contraste_max = 1 + me->getMu2();
+ double contraste_min = 1 - me->getMu1();
+
+ // array in which likelihood ratios will be stored
+ double *likelihood= new double[ 2 * range + 1 ] ;
+
+ int ii_1 = i ;
+ int jj_1 = j ;
+ i_1 = i ;
+ j_1 = j ;
+ double threshold;
+ threshold = me->getThreshold() ;
+ double diff = 1e6;
+
+ // std::cout <<"---------------------"<<std::endl ;
+ for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
+ {
+ // convolution results
+ convolution_ = list_query_pixels[n].convolution(I, me) ;
+
+ // luminance ratio of reference pixel to potential correspondent pixel
+ // the luminance must be similar, hence the ratio value should
+ // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
+ if( test_contraste )
+ {
+ likelihood[n] = fabs(convolution_ + convlt );
+ if (likelihood[n]> threshold)
+ {
+ contraste = convolution_ / convlt;
+ if((contraste > contraste_min) && (contraste < contraste_max) && fabs(1-contraste) < diff)
+ {
+ diff = fabs(1-contraste);
+ max_convolution= convolution_;
+ max = likelihood[n] ;
+ max_rank = (int)n ;
+ // max_rank2 = max_rank1;
+ // max_rank1 = max_rank;
+ }
+ }
+ }
+
+ else
+ {
+ likelihood[n] = fabs(2*convolution_) ;
+ if (likelihood[n] > max && likelihood[n] > threshold)
+ {
+ max_convolution= convolution_;
+ max = likelihood[n] ;
+ max_rank = (int)n ;
+ // max_rank2 = max_rank1;
+ // max_rank1 = max_rank;
+ }
+ }
+ }
+
+ // test on the likelihood threshold if threshold==-1 then
+ // the me->threshold is selected
+
+ vpImagePoint ip;
+
+ // if (test_contrast)
+ if(max_rank >= 0)
+ {
+ if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
+ {
+ ip.set_i( list_query_pixels[max_rank].i );
+ ip.set_j( list_query_pixels[max_rank].j );
+ vpDisplay::displayPoint(I, ip, vpColor::red);
+ }
+
+ *this = list_query_pixels[max_rank] ;//The vpMeSite2 is replaced by the vpMeSite2 of max likelihood
+ normGradient = vpMath::sqr(max_convolution);
+
+ convlt = max_convolution;
+ i_1 = ii_1; //list_query_pixels[max_rank].i ;
+ j_1 = jj_1; //list_query_pixels[max_rank].j ;
+ delete []list_query_pixels ;
+ delete []likelihood;
+ }
+ else //none of the query sites is better than the threshold
+ {
+ if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
+ {
+ ip.set_i( list_query_pixels[0].i );
+ ip.set_j( list_query_pixels[0].j );
+ vpDisplay::displayPoint(I, ip, vpColor::green);
+ }
+ normGradient = 0 ;
+ //if(contraste != 0)
+ if(std::fabs(contraste) > std::numeric_limits<double>::epsilon())
+ state = CONSTRAST; // contrast suppression
+ else
+ state = THRESHOLD; // threshold suppression
+
+ delete []list_query_pixels ;
+ delete []likelihood; // modif portage
+ }
+}
+
+int vpMeSite::operator!=(const vpMeSite &m)
+{
+ return((m.i != i) || (m.j != j)) ;
+
+}
+
+VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS)
+{
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ return (os << "Alpha: " << vpMeS.alpha
+ << " Convolution: " << vpMeS.convlt
+ << " Flag: " << vpMeS.suppress
+ << " Weight: " << vpMeS.weight );
+#else
+ return (os << "Alpha: " << vpMeS.alpha
+ << " Convolution: " << vpMeS.convlt
+ << " Weight: " << vpMeS.weight );
+#endif
+}
+
+void vpMeSite::display(const vpImage<unsigned char>& I)
+{
+ vpMeSite::display(I,ifloat,jfloat,state);
+}
+
+//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 (constrast 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<unsigned char>& I, const double &i, const double &j, const vpMeSiteState &state)
+{
+ switch(state)
+ {
+ case NO_SUPPRESSION:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
+ break;
+
+ case CONSTRAST:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
+ break;
+
+ case THRESHOLD:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
+ break;
+
+ case M_ESTIMATOR:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
+ break;
+
+ case TOO_NEAR:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
+ break;
+
+ default:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
+ }
+}
+
+/*!
+ 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 (constrast 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<vpRGBa>& I, const double &i, const double &j, const vpMeSiteState &state)
+{
+ switch(state)
+ {
+ case NO_SUPPRESSION:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
+ break;
+
+ case CONSTRAST:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
+ break;
+
+ case THRESHOLD:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
+ break;
+
+ case M_ESTIMATOR:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
+ break;
+
+ case TOO_NEAR:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
+ break;
+
+ default:
+ vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
+ }
+}
diff --git a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp
new file mode 100644
index 0000000..433ca5b
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp
@@ -0,0 +1,371 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Moving edges.
+ *
+ * Authors:
+ * Andrew Comport
+ *
+ *****************************************************************************/
+
+/*!
+\file vpMeTracker.cpp
+\brief Contains abstract elements for a Distance to Feature type feature.
+*/
+
+
+
+#include <visp3/me/vpMeTracker.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpColor.h>
+
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpDebug.h>
+#include <algorithm>
+
+#define DEBUG_LEVEL1 0
+#define DEBUG_LEVEL2 0
+
+void
+vpMeTracker::init()
+{
+ vpTracker::init() ;
+ p.resize(2) ;
+ selectDisplay = vpMeSite::NONE ;
+}
+
+vpMeTracker::vpMeTracker()
+ : list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ , query_range (0), display_point(false)
+#endif
+{
+ init();
+}
+
+vpMeTracker::vpMeTracker(const vpMeTracker& meTracker)
+ : vpTracker(meTracker),
+ list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ , query_range (0), display_point(false)
+#endif
+{
+ init();
+
+ me = meTracker.me;
+ list = meTracker.list;
+ nGoodElement = meTracker.nGoodElement;
+ init_range = meTracker.init_range;
+ selectDisplay = meTracker.selectDisplay;
+
+ #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ display_point = meTracker.display_point;
+ query_range = meTracker.query_range;
+ #endif
+}
+
+/*!
+ Reset the tracker by removing all the moving edges.
+ */
+void vpMeTracker::reset()
+{
+ nGoodElement = 0;
+ list.clear();
+}
+
+vpMeTracker::~vpMeTracker()
+{
+ reset();
+}
+
+vpMeTracker&
+vpMeTracker::operator = (vpMeTracker& p_me)
+{
+ list = p_me.list;
+ me = p_me.me;
+ selectDisplay = p_me.selectDisplay ;
+
+ return *this;
+}
+
+static bool isSuppressZero(const vpMeSite& P){
+ return (P.getState() == vpMeSite::NO_SUPPRESSION);
+}
+
+unsigned int
+vpMeTracker::numberOfSignal()
+{
+ unsigned int number_signal=0;
+
+ // Loop through all the points tracked from the contour
+ number_signal = static_cast<unsigned int>(std::count_if(list.begin(), list.end(), isSuppressZero));
+ return number_signal;
+}
+
+unsigned int
+vpMeTracker::totalNumberOfSignal()
+{
+ return (unsigned int)list.size();
+}
+
+int
+vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
+{
+ return (! ((i> half+2) &&
+ (i< rows -(half+2)) &&
+ (j>half+2) &&
+ (j<cols-(half+2)))
+ ) ;
+}
+
+int
+vpMeTracker::outOfImage(vpImagePoint iP, int half, int rows, int cols)
+{
+ int i = vpMath::round(iP.get_i());
+ int j = vpMath::round(iP.get_j());
+ 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<unsigned char>& I)
+{
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ // Must set range to 0
+ unsigned int range_tmp = me->getRange();
+ me->setRange(init_range);
+
+ nGoodElement=0;
+
+ int d = 0;
+ vpImagePoint ip1, ip2;
+
+ // Loop through list of sites to track
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite refp = *it;//current reference pixel
+
+ d++ ;
+ // If element hasn't been suppressed
+ if(refp.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ try {
+ refp.track(I,me,false);
+ }
+ catch(...)
+ {
+ // EM verifier quel signal est de sortie !!!
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ if(refp.getState() == vpMeSite::NO_SUPPRESSION) nGoodElement++;
+ }
+
+
+#if (DEBUG_LEVEL2)
+ {
+ double a,b ;
+ a = refp.i_1 - refp.i ;
+ b = refp.j_1 - refp.j ;
+ if(refp.getState() == vpMeSite::NO_SUPPRESSION) {
+ ip1.set_i( refp.i );
+ ip1.set_j( refp.j );
+ ip2.set_i( refp.i+a );
+ ip2.set_j( refp.j+b );
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
+ }
+ }
+#endif
+ *it = refp;
+ }
+
+ /*
+ if (res != OK)
+ {
+ std::cout<< "In vpMeTracker::initTracking(): " ;
+ switch (res)
+ {
+ case ERR_TRACKING:
+ std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " << std::endl ;
+ break ;
+ case fatalError:
+ std::cout << "vpMeTracker::initTracking:Track return fatalError" << std::endl ;
+ break ;
+ default:
+ std::cout << "vpMeTracker::initTracking:Track return error " << res << std::endl ;
+ }
+ return res ;
+ }
+ */
+
+ me->setRange(range_tmp);
+}
+
+/*!
+ Track moving-edges.
+
+ \param I : Image.
+
+ \exception vpTrackingException::initializationError : Moving edges not initialized.
+
+*/
+void
+vpMeTracker::track(const vpImage<unsigned char>& I)
+{
+ if (!me) {
+ vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
+ throw(vpTrackingException(vpTrackingException::initializationError,
+ "Moving edges not initialized")) ;
+ }
+
+ if (list.empty())
+ {
+ vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError,
+ "too few pixel to track")) ;
+
+ }
+
+ vpImagePoint ip1, ip2;
+ nGoodElement=0;
+ // int d =0;
+ // Loop through list of sites to track
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite s = *it;//current reference pixel
+
+ // d++ ;
+ // If element hasn't been suppressed
+ if(s.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+
+ try{
+ // vpERROR_TRACE("%d",d ) ;
+ // vpERROR_TRACE("range %d",me->range) ;
+ s.track(I,me,true);
+ }
+ catch(vpTrackingException)
+ {
+ vpERROR_TRACE("catch exception ") ;
+ s.setState(vpMeSite::THRESHOLD);
+ }
+
+ if(s.getState() != vpMeSite::THRESHOLD)
+ {
+ nGoodElement++;
+
+#if (DEBUG_LEVEL2)
+ {
+ double a,b ;
+ a = s.i_1 - s.i ;
+ b = s.j_1 - s.j ;
+ if(s.getState() == vpMeSite::NO_SUPPRESSION) {
+ ip1.set_i( s.i );
+ ip1.set_j( s.j );
+ ip2.set_i( s.i+a*5 );
+ ip2.set_j( s.j+b*5 );
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
+ }
+ }
+#endif
+
+ }
+ *it = s;
+ }
+ }
+}
+
+/*!
+ 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 (constrast 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<unsigned char>& I)
+{
+#if (DEBUG_LEVEL1)
+ {
+ std::cout <<"begin vpMeTracker::displayList() " << std::endl ;
+ std::cout<<" There are "<<list.size()<< " sites in the list " << std::endl ;
+ }
+#endif
+ for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite p_me = *it;
+ p_me.display(I);
+ }
+}
+
+/*! Displays the status of moving edge sites
+
+ \param I : The image.
+ \param w : vector
+ \param index_w : index
+*/
+void
+vpMeTracker::display(const vpImage<unsigned char>& I,vpColVector &w, unsigned int &index_w)
+{
+ for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ vpMeSite P = *it;
+
+ if(P.getState() == vpMeSite::NO_SUPPRESSION)
+ {
+ P.weight = w[index_w];
+ index_w++;
+ }
+
+ *it = P;
+ }
+ display(I);
+}
+
+#undef DEBUG_LEVEL1
+#undef DEBUG_LEVEL2
+
+
diff --git a/modules/tracker/me/src/moving-edges/vpNurbs.cpp b/modules/tracker/me/src/moving-edges/vpNurbs.cpp
new file mode 100644
index 0000000..a9a4a48
--- /dev/null
+++ b/modules/tracker/me/src/moving-edges/vpNurbs.cpp
@@ -0,0 +1,1082 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * This class implements the Non Uniform Rational B-Spline (NURBS)
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/me/vpNurbs.h>
+#include <visp3/core/vpColVector.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+/*
+ Compute the distance d = |Pw1-Pw2|
+*/
+inline double distance(const vpImagePoint &iP1, const double w1, const vpImagePoint &iP2, const double w2)
+{
+ double distancei = iP1.get_i() - iP2.get_i();
+ double distancej = iP1.get_j() - iP2.get_j();
+ double distancew = w1 -w2;
+ 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()
+{
+ weights = nurbs.weights;
+}
+
+/*!
+ Basic destructor
+*/
+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<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
+{
+ vpBasisFunction* N = NULL;
+ N = computeBasisFuns(l_u, l_i, l_p, l_knots);
+ vpImagePoint pt;
+
+ double ic = 0;
+ double jc = 0;
+ double wc = 0;
+ for(unsigned int j = 0; j <= l_p; j++)
+ {
+ ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i() * l_weights[l_i-l_p+j];
+ jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j() * l_weights[l_i-l_p+j];
+ wc = wc + N[j].value * l_weights[l_i-l_p+j];
+ }
+
+ pt.set_i(ic/wc);
+ pt.set_j(jc/wc);
+
+ if (N != NULL) delete[] N;
+
+ 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;
+ N = computeBasisFuns(u);
+ vpImagePoint pt;
+
+ double ic = 0;
+ double jc = 0;
+ double wc = 0;
+ for(unsigned int j = 0; j <= p; j++)
+ {
+ ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i() * weights[N[0].i+j]; //N[0].i = findSpan(u)-p
+ jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j() * weights[N[0].i+j];
+ wc = wc + N[j].value * weights[N[0].i+j];
+ }
+
+ pt.set_i(ic/wc);
+ pt.set_j(jc/wc);
+
+ if (N != NULL) delete[] N;
+
+ 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<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
+{
+ vpMatrix derivate(l_der+1,3);
+ vpBasisFunction** N = NULL;
+ N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
+
+ for(unsigned int k = 0; k <= l_der; k++)
+ {
+ derivate[k][0] = 0.0;
+ derivate[k][1] = 0.0;
+ derivate[k][2] = 0.0;
+
+ for(unsigned int j = 0; j<= l_p; j++)
+ {
+ derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
+ derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
+ derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
+ }
+ }
+
+ if (N!=NULL) {
+ for(unsigned int i = 0; i <= l_der; i++)
+ delete [] N[i];
+ delete[] N;
+ }
+ 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);
+ vpBasisFunction** N = NULL;
+ N = computeDersBasisFuns(u, der);
+
+ for(unsigned int k = 0; k <= der; k++)
+ {
+ derivate[k][0] = 0.0;
+ derivate[k][1] = 0.0;
+ derivate[k][2] = 0.0;
+ for(unsigned int j = 0; j<= p; j++)
+ {
+ derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
+ derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
+ derivate[k][2] = derivate[k][2] + N[k][j].value*(weights[N[0][0].i-p+j]);
+ }
+ }
+
+ if (N!=NULL) delete[] N;
+
+ 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<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
+{
+ std::vector<vpImagePoint> A;
+ vpImagePoint pt;
+ for(unsigned int j = 0; j < l_controlPoints.size(); j++)
+ {
+ pt = l_controlPoints[j];
+ pt.set_i(pt.get_i()*l_weights[j]);
+ pt.set_j(pt.get_j()*l_weights[j]);
+ A.push_back(pt);
+ }
+
+ vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights);
+
+ vpImagePoint* CK = new vpImagePoint[l_der+1];
+ double ic,jc;
+
+ for(unsigned int k = 0; k <= l_der; k++)
+ {
+ ic = Awders[k][0];
+ jc = Awders[k][1];
+ for(unsigned int j = 1; j <= k; j++)
+ {
+ double tmpComb = static_cast<double>( vpMath::comb(k,j) );
+ ic = ic - tmpComb*Awders[k][2]*(CK[k-j].get_i());
+ jc = jc - tmpComb*Awders[j][2]*(CK[k-j].get_j());
+ }
+ CK[k].set_ij(ic/Awders[0][2],jc/Awders[0][2]);
+ }
+ 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+{
+ vpMatrix Rw(l_p+1,3);
+ std::vector<vpImagePoint>::iterator it1;
+ std::vector<double>::iterator it2;
+ vpImagePoint pt;
+ double w = 0;
+
+ for (unsigned int j = 0; j <= l_p-l_s; j++)
+ {
+ Rw[j][0] = (l_controlPoints[l_k-l_p+j]).get_i() * l_weights[l_k-l_p+j];
+ Rw[j][1] = (l_controlPoints[l_k-l_p+j]).get_j() * l_weights[l_k-l_p+j];
+ Rw[j][2] = l_weights[l_k-l_p+j];
+ }
+
+ it1 = l_controlPoints.begin();
+ l_controlPoints.insert(it1+(int)l_k-(int)l_s, l_r, pt);
+ it2 = l_weights.begin();
+ l_weights.insert(it2+(int)l_k-(int)l_s, l_r, w);
+
+ unsigned int L=0;
+ double alpha;
+ for (unsigned int j = 1; j <= l_r; j++)
+ {
+ L = l_k - l_p +j;
+
+ for (unsigned int i = 0; i <=l_p-j-l_s; i++)
+ {
+ alpha = (l_u - l_knots[L+i])/(l_knots[i+l_k+1] - l_knots[L+i]);
+ Rw[i][0]= alpha*Rw[i+1][0]+(1.0-alpha)*Rw[i][0];
+ Rw[i][1]= alpha*Rw[i+1][1]+(1.0-alpha)*Rw[i][1];
+ Rw[i][2]= alpha*Rw[i+1][2]+(1.0-alpha)*Rw[i][2];
+ }
+
+ pt.set_ij(Rw[0][0]/Rw[0][2],Rw[0][1]/Rw[0][2]);
+ l_controlPoints[L] = pt;
+ l_weights[L] = Rw[0][2];
+
+ pt.set_ij(Rw[l_p-j-l_s][0]/Rw[l_p-j-l_s][2],Rw[l_p-j-l_s][1]/Rw[l_p-j-l_s][2]);
+ l_controlPoints[l_k+l_r-j-l_s] = pt;
+ l_weights[l_k+l_r-j-l_s] = Rw[l_p-j-l_s][2];
+ }
+
+ for(unsigned int j = L+1; j < l_k-l_s; j++)
+ {
+ pt.set_ij(Rw[j-L][0]/Rw[j-L][2],Rw[j-L][1]/Rw[j-L][2]);
+ l_controlPoints[j] = pt;
+ l_weights[j] = Rw[j-L][2];
+ }
+
+ it2 = l_knots.begin();
+ 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+{
+ unsigned int a = findSpan(l_x[0], l_p, l_knots);
+ unsigned int b = findSpan(l_x[l_r], l_p, l_knots);
+ b++;
+
+ unsigned int n = (unsigned int)l_controlPoints.size();
+ unsigned int m = (unsigned int)l_knots.size();
+
+ for (unsigned int j = 0; j < n; j++)
+ {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
+ }
+
+ std::vector<double> l_knots_tmp(l_knots);
+ std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
+ std::vector<double> l_weights_tmp(l_weights);
+
+ vpImagePoint pt;
+ double w = 0;
+
+ for (unsigned int j = 0; j <= l_r; j++)
+ {
+ l_controlPoints.push_back(pt);
+ l_weights.push_back(w);
+ l_knots.push_back(w);
+ }
+
+ for(unsigned int j = b+l_p; j <= m-1; j++) l_knots[j+l_r+1] = l_knots_tmp[j];
+
+ for (unsigned int j = b-1; j <= n-1; j++)
+ {
+ l_controlPoints[j+l_r+1] = l_controlPoints_tmp[j];
+ l_weights[j+l_r+1] = l_weights_tmp[j];
+ }
+
+ unsigned int i = b+l_p-1;
+ unsigned int k = b+l_p+l_r;
+
+ {
+ unsigned int j = l_r + 1;
+ do{
+ j--;
+ while(l_x[j] <= l_knots[i] && i > a)
+ {
+ l_controlPoints[k-l_p-1] = l_controlPoints_tmp[i-l_p-1];
+ l_weights[k-l_p-1] = l_weights_tmp[i-l_p-1];
+ l_knots[k] = l_knots_tmp[i];
+ k--;
+ i--;
+ }
+
+ l_controlPoints[k-l_p-1] = l_controlPoints[k-l_p];
+ l_weights[k-l_p-1] = l_weights[k-l_p];
+
+ for (unsigned int l = 1; l <= l_p; l++)
+ {
+ unsigned int ind = k-l_p+l;
+ double alpha = l_knots[k+l] - l_x[j];
+ //if (vpMath::abs(alpha) == 0.0)
+ if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
+ {
+ l_controlPoints[ind-1] = l_controlPoints[ind];
+ l_weights[ind-1] = l_weights[ind];
+ }
+ 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());
+ l_controlPoints[ind-1].set_j( alpha * l_controlPoints[ind-1].get_j() + (1.0-alpha) * l_controlPoints[ind].get_j());
+ l_weights[ind-1] = alpha * l_weights[ind-1] + (1.0-alpha) * l_weights[ind];
+ }
+ }
+ l_knots[k] = l_x[j];
+ k--;
+ }while(j != 0);
+ }
+
+ for (unsigned int j = 0; j < n; j++)
+ {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i()/l_weights[j],l_controlPoints[j].get_j()/l_weights[j]);
+ }
+}
+
+
+/*!
+ 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+{
+ unsigned int n = (unsigned int)l_controlPoints.size();
+ unsigned int m = n + l_p + 1;
+
+ for (unsigned int j = 0; j < n; j++)
+ {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
+ }
+
+ unsigned int ord = l_p + 1;
+ double fout = (2*l_r-l_s-l_p)/2.;
+ unsigned int last = l_r - l_s;
+ unsigned int first = l_r - l_p;
+ unsigned int tblSize = 2*l_p+1;
+ vpImagePoint *tempP = new vpImagePoint[tblSize];
+ double *tempW = new double[tblSize];
+ vpImagePoint pt;
+ unsigned int t = 0;
+ double alfi = 0;
+ double alfj = 0;
+ unsigned int i, j, ii, jj;
+
+ for(t = 0; t < l_num; t++)
+ {
+ unsigned int off = first - 1;
+ tempP[0] = l_controlPoints[off];
+ tempW[0] = l_weights[off];
+ tempP[last+1-off] = l_controlPoints[last+1];
+ tempW[last+1-off] = l_weights[last+1];
+ i = first;
+ j = last;
+ ii = 1;
+ jj = last -off;
+ int remflag = 0;
+ while (j-i > t)
+ {
+ alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
+ alfj = (l_u - l_knots[j-t])/(l_knots[j+ord]-l_knots[j-t]);
+ pt.set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
+ tempP[ii].set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
+ tempP[ii].set_j((l_controlPoints[i].get_j()-(1.0-alfi)*tempP[ii-1].get_j())/alfi);
+ tempW[ii] = ((l_weights[i]-(1.0-alfi)*tempW[ii-1])/alfi);
+ tempP[jj].set_i((l_controlPoints[j].get_i()-alfj*tempP[jj+1].get_i())/(1.0-alfj));
+ tempP[jj].set_j((l_controlPoints[j].get_j()-alfj*tempP[jj+1].get_j())/(1.0-alfj));
+ tempW[jj] = ((l_weights[j]-alfj*tempW[jj+1])/(1.0-alfj));
+ i++;
+ j--;
+ ii++;
+ jj--;
+ }
+
+ if(j-i < t)
+ {
+ double distancei = tempP[ii-1].get_i() - tempP[jj+1].get_i();
+ double distancej = tempP[ii-1].get_j() - tempP[jj+1].get_j();
+ double distancew = tempW[ii-1] - tempW[jj+1];
+ double distance = sqrt(vpMath::sqr(distancei)+vpMath::sqr(distancej)+vpMath::sqr(distancew));
+ if(distance <= l_TOL) remflag = 1;
+ }
+ 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());
+ double distancej = 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) remflag = 1;
+ }
+ if (remflag == 0) break;
+ else
+ {
+ i = first;
+ j = last;
+ while (j-i>t)
+ {
+ l_controlPoints[i].set_i(tempP[i-off].get_i());
+ l_controlPoints[i].set_j(tempP[i-off].get_j());
+ l_weights[i] = tempW[i-off];
+ l_controlPoints[j].set_i(tempP[j-off].get_i());
+ l_controlPoints[j].set_j(tempP[j-off].get_j());
+ l_weights[j] = tempW[j-off];
+ i++;
+ j--;
+ }
+ }
+ first--;
+ last++;
+ }
+ if (t == 0) {
+ delete[] tempP;
+ delete[] tempW;
+ return t;
+ }
+ for(unsigned int k = l_r+1; k <= m; k++) l_knots[k-t] = l_knots[k];
+ j = (unsigned int)fout;
+ i = j;
+ for(unsigned int k = 1; k< t; k++)
+ {
+ if (k%2) i++;
+ else j--;
+ }
+ for(unsigned int k = i+1; k <= n; k++)
+ {
+ l_controlPoints[j].set_i(l_controlPoints[k].get_i());
+ l_controlPoints[j].set_j(l_controlPoints[k].get_j());
+ l_weights[j] = l_weights[k];
+ j++;
+ }
+ for(unsigned int k = 0; k < t; k++)
+ {
+ l_knots.erase(l_knots.end()-1);
+ l_controlPoints.erase(l_controlPoints.end()-1);
+ }
+
+ for(unsigned int k = 0; k < l_controlPoints.size(); k++)
+ l_controlPoints[k].set_ij(l_controlPoints[k].get_i()/l_weights[k],l_controlPoints[k].get_j()/l_weights[k]);
+
+ delete[] tempP;
+ delete[] tempW;
+ 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)
+{
+ return removeCurveKnot(u, r, num, 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<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+{
+ if (l_p == 0) {
+ //vpERROR_TRACE("Bad degree of the NURBS basis functions");
+ throw(vpException(vpException::badValue,
+ "Bad degree of the NURBS basis functions")) ;
+ }
+
+ l_knots.clear();
+ l_controlPoints.clear();
+ l_weights.clear();
+ unsigned int n = (unsigned int)l_crossingPoints.size()-1;
+ unsigned int m = n+l_p+1;
+
+ double d = 0;
+ for(unsigned int k=1; k<=n; k++)
+ d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
+
+ //Compute ubar
+ std::vector<double> ubar;
+ ubar.push_back(0.0);
+ for(unsigned int k=1; k<n; k++)
+ ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
+ ubar.push_back(1.0);
+
+ //Compute the knot vector
+ for(unsigned int k = 0; k <= l_p; k++)
+ l_knots.push_back(0.0);
+
+ double sum = 0;
+ for(unsigned int k = 1; k <= l_p; k++)
+ sum = sum + ubar[k];
+
+ //Centripetal method
+ for(unsigned int k = 1; k <= n-l_p; k++)
+ {
+ l_knots.push_back(sum/l_p);
+ sum = sum - ubar[k-1] + ubar[l_p+k-1];
+ }
+
+ for(unsigned int k = m-l_p; k <= m; k++)
+ l_knots.push_back(1.0);
+
+ vpMatrix A(n+1,n+1);
+ vpBasisFunction* N;
+
+ for(unsigned int i = 0; i <= n; i++)
+ {
+ unsigned int span = findSpan(ubar[i], l_p, l_knots);
+ N = computeBasisFuns(ubar[i], span, l_p, l_knots);
+ for (unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
+ delete[] N;
+ }
+ //vpMatrix Ainv = A.inverseByLU();
+ vpMatrix Ainv;
+ A.pseudoInverse(Ainv);
+ vpColVector Qi(n+1);
+ vpColVector Qj(n+1);
+ vpColVector Qw(n+1);
+ for (unsigned int k = 0; k <= n; k++)
+ {
+ Qi[k] = l_crossingPoints[k].get_i();
+ Qj[k] = l_crossingPoints[k].get_j();
+ }
+ Qw = 1;
+ vpColVector Pi = Ainv*Qi;
+ vpColVector Pj = Ainv*Qj;
+ vpColVector Pw = Ainv*Qw;
+
+ vpImagePoint pt;
+ for (unsigned int k = 0; k <= n; k++)
+ {
+ pt.set_ij(Pi[k],Pj[k]);
+ l_controlPoints.push_back(pt);
+ l_weights.push_back(Pw[k]);
+ }
+}
+
+/*!
+ 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<vpMeSite> &l_crossingPoints)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ l_crossingPoints.front();
+ vpMeSite s = l_crossingPoints.value();
+ vpImagePoint pt(s.ifloat,s.jfloat);
+ vpImagePoint pt_1 = pt;
+ v_crossingPoints.push_back(pt);
+ l_crossingPoints.next();
+ while(!l_crossingPoints.outside())
+ {
+ s = l_crossingPoints.value();
+ pt.set_ij(s.ifloat,s.jfloat);
+ if (vpImagePoint::distance(pt_1,pt) >= 10)
+ {
+ v_crossingPoints.push_back(pt);
+ pt_1 = pt;
+ }
+ l_crossingPoints.next();
+ }
+ 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<vpImagePoint> &l_crossingPoints)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ v_crossingPoints.push_back(*it);
+ }
+ 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<vpMeSite> &l_crossingPoints)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ vpMeSite s = l_crossingPoints.front();
+ vpImagePoint pt(s.ifloat,s.jfloat);
+ vpImagePoint pt_1 = pt;
+ v_crossingPoints.push_back(pt);
+ std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
+ ++it;
+ for(; it!=l_crossingPoints.end(); ++it){
+ vpImagePoint pt_tmp(it->ifloat, it->jfloat);
+ if (vpImagePoint::distance(pt_1, pt_tmp) >= 10){
+ v_crossingPoints.push_back(pt_tmp);
+ pt_1 = pt_tmp;
+ }
+ }
+ 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<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+{
+ l_knots.clear();
+ l_controlPoints.clear();
+ l_weights.clear();
+ unsigned int m = (unsigned int)l_crossingPoints.size()-1;
+
+ double d = 0;
+ for(unsigned int k=1; k<=m; k++)
+ d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
+
+ //Compute ubar
+ std::vector<double> ubar;
+ ubar.push_back(0.0);
+ for(unsigned int k=1; k<m; k++)
+ ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
+ ubar.push_back(1.0);
+
+ //Compute the knot vector
+ for(unsigned int k = 0; k <= l_p; k++)
+ l_knots.push_back(0.0);
+
+ d = (double)(m+1)/(double)(l_n-l_p+1);
+
+ double alpha;
+ for(unsigned int j = 1; j <= l_n-l_p; j++)
+ {
+ double i = floor(j*d);
+ alpha = j*d-i;
+ l_knots.push_back((1.0-alpha)*ubar[(unsigned int)i-1]+alpha*ubar[(unsigned int)i]);
+ }
+
+ for(unsigned int k = 0; k <= l_p ; k++)
+ l_knots.push_back(1.0);
+
+ //Compute Rk
+ std::vector<vpImagePoint> Rk;
+ vpBasisFunction* N;
+ for(unsigned int k = 1; k <= m-1; k++)
+ {
+ unsigned int span = findSpan(ubar[k], l_p, l_knots);
+ if (span == l_p && span == l_n)
+ {
+ N = computeBasisFuns(ubar[k], span, l_p, l_knots);
+ vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
+ l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
+ Rk.push_back(pt);
+ delete[] N;
+ }
+ 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)
+ {
+ 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
+ {
+ Rk.push_back(l_crossingPoints[k]);
+ }
+ }
+
+ vpMatrix A(m-1,l_n-1);
+ //Compute A
+ for(unsigned int i = 1; i <= m-1; i++)
+ {
+ unsigned int span = findSpan(ubar[i], l_p, l_knots);
+ N = computeBasisFuns(ubar[i], span, l_p, l_knots);
+ for (unsigned int k = 0; k <= l_p; k++)
+ {
+ if (N[k].i > 0 && N[k].i < l_n)
+ A[i-1][N[k].i-1] = N[k].value;
+ }
+ delete[] N;
+ }
+
+ vpColVector Ri(l_n-1);
+ vpColVector Rj(l_n-1);
+ vpColVector Rw(l_n-1);
+ double sum;
+ for (unsigned int i = 0; i < l_n-1; i++)
+ {
+ sum =0;
+ for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_i();
+ Ri[i] = sum;
+ sum = 0;
+ for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_j();
+ Rj[i] = sum;
+ sum = 0;
+ for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]; //The crossing points weigths are equal to 1.
+ Rw[i] = sum;
+ }
+
+ vpMatrix AtA = A.AtA();
+ vpMatrix AtAinv;
+ AtA.pseudoInverse(AtAinv);
+
+ vpColVector Pi = AtAinv*Ri;
+ vpColVector Pj = AtAinv*Rj;
+ vpColVector Pw = AtAinv*Rw;
+
+ vpImagePoint pt;
+ l_controlPoints.push_back(l_crossingPoints[0]);
+ l_weights.push_back(1.0);
+ for (unsigned int k = 0; k < l_n-1; k++)
+ {
+ pt.set_ij(Pi[k],Pj[k]);
+ l_controlPoints.push_back(pt);
+ l_weights.push_back(Pw[k]);
+ }
+ l_controlPoints.push_back(l_crossingPoints[m]);
+ 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<vpMeSite> &l_crossingPoints, unsigned int n)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ l_crossingPoints.front();
+ while(!l_crossingPoints.outside())
+ {
+ vpMeSite s = l_crossingPoints.value();
+ vpImagePoint pt(s.ifloat,s.jfloat);
+ v_crossingPoints.push_back(pt);
+ l_crossingPoints.next();
+ }
+ 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<vpImagePoint> &l_crossingPoints, unsigned int n)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ v_crossingPoints.push_back(*it);
+ }
+ 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<vpMeSite> &l_crossingPoints, unsigned int n)
+{
+ std::vector<vpImagePoint> v_crossingPoints;
+ for(std::list<vpMeSite>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ vpImagePoint pt(it->ifloat, it->jfloat);
+ v_crossingPoints.push_back(pt);
+ }
+ 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/testNurbs.cpp b/modules/tracker/me/test/testNurbs.cpp
new file mode 100644
index 0000000..5204657
--- /dev/null
+++ b/modules/tracker/me/test/testNurbs.cpp
@@ -0,0 +1,385 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exemple of a Nurbs curve.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+/*!
+ \example testNurbs.cpp
+
+ \brief Describe a curve thanks to a Nurbs.
+*/
+
+#include <visp3/core/vpDebug.h>
+
+#include <visp3/me/vpNurbs.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
+#ifdef VISP_HAVE_MODULE_GUI
+# include <visp3/gui/vpDisplayGTK.h>
+# include <visp3/gui/vpDisplayGDI.h>
+# include <visp3/gui/vpDisplayOpenCV.h>
+# include <visp3/gui/vpDisplayD3D.h>
+# include <visp3/gui/vpDisplayX.h>
+#endif
+
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpIoTools.h>
+#include <cstdlib>
+#if defined(VISP_HAVE_DISPLAY)
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Describe a curve thanks to a Nurbs.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed,
+ opt_display) == false) {
+ exit (-1);
+ }
+
+ // 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<unsigned char> I(540,480);
+ vpImage<unsigned char> I2(540,480);
+ vpImage<unsigned char> I3(540,480);
+
+ // We open a window using either X11, GTK or GDI.
+#if defined VISP_HAVE_X11
+ vpDisplayX display[3];
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display[3];
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display[3];
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display[3];
+#endif
+
+ if (opt_display) {
+ // Display size is automatically defined by the image (I) size
+ display[0].init(I, 100, 100,"Points as control points") ;
+ vpDisplay::display(I) ;
+ vpDisplay::flush(I) ;
+ }
+
+ vpNurbs Nurbs;
+ std::list<double> knots;
+ knots.push_back(0);
+ knots.push_back(0);
+ knots.push_back(0);
+ knots.push_back(1);
+ knots.push_back(2);
+ knots.push_back(3);
+ knots.push_back(4);
+ knots.push_back(4);
+ knots.push_back(5);
+ knots.push_back(5);
+ knots.push_back(5);
+
+ std::list<vpImagePoint> controlPoints;
+ std::list<double> weights;
+ vpImagePoint pt;
+ pt.set_ij(50,300);
+ controlPoints.push_back(pt);
+ weights.push_back(1);
+ pt.set_ij(100,130);
+ controlPoints.push_back(pt);
+ weights.push_back(5);
+ pt.set_ij(150,400);
+ controlPoints.push_back(pt);
+ weights.push_back(0.2);
+ pt.set_ij(200,370);
+ controlPoints.push_back(pt);
+ weights.push_back(10);
+ pt.set_ij(250,120);
+ controlPoints.push_back(pt);
+ weights.push_back(1);
+ pt.set_ij(300,250);
+ controlPoints.push_back(pt);
+ weights.push_back(2);
+ pt.set_ij(350,200);
+ controlPoints.push_back(pt);
+ weights.push_back(3);
+ pt.set_ij(400,300);
+ controlPoints.push_back(pt);
+ weights.push_back(1);
+
+ Nurbs.set_p(2);
+ Nurbs.set_knots(knots);
+ Nurbs.set_controlPoints(controlPoints);
+ Nurbs.set_weights(weights);
+
+ std::cout << "The parameters are :" <<std::endl;
+ std::cout << "p : " << Nurbs.get_p() <<std::endl;
+ std::cout << "" <<std::endl;
+ std::cout << "The knot vector :" <<std::endl;
+ std::list<double> knots_cur;
+ Nurbs.get_knots(knots_cur);
+ unsigned int i_display=0;
+ for(std::list<double>::const_iterator it=knots_cur.begin(); it!=knots_cur.end(); ++it, ++i_display){
+ std::cout << i_display << " ---> " << *it << std::endl;
+ }
+ std::cout << "The control points are :" <<std::endl;
+ std::list<vpImagePoint> controlPoints_cur;
+ Nurbs.get_controlPoints(controlPoints_cur);
+ i_display=0;
+ for(std::list<vpImagePoint>::const_iterator it=controlPoints_cur.begin(); it!=controlPoints_cur.end(); ++it, ++i_display){
+ std::cout << i_display << " ---> " << *it << std::endl;
+ }
+ std::cout << "The associated weights are :" <<std::endl;
+ std::list<double> weights_cur;
+ Nurbs.get_weights(weights_cur);
+ i_display=0;
+ for(std::list<double>::const_iterator it=weights_cur.begin(); it!=weights_cur.end(); ++it, ++i_display){
+ std::cout << i_display << " ---> " << *it << std::endl;
+ }
+
+ unsigned int i = Nurbs.findSpan(5/2.0);
+ std::cout << "The knot interval number for the value u = 5/2 is : " << i <<std::endl;
+
+ vpBasisFunction *N = NULL;
+ N = Nurbs.computeBasisFuns(5/2.0);
+ std::cout << "The nonvanishing basis functions N(u=5/2) are :" << std::endl;
+ for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ std::cout << N[j].value << std::endl;
+
+ vpBasisFunction **N2 = NULL;
+ N2 = Nurbs.computeDersBasisFuns(5/2.0, 2);
+ std::cout << "The first derivatives of the basis functions N'(u=5/2) are :" << std::endl;
+ for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ std::cout << N2[1][j].value << std::endl;
+
+ std::cout << "The second derivatives of the basis functions N''(u=5/2) are :" << std::endl;
+ for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ std::cout << N2[2][j].value << std::endl;
+
+ if (opt_display && opt_click_allowed)
+ {
+ double u = 0.0;
+ while (u <= 5)
+ {
+ pt = Nurbs.computeCurvePoint(u);
+ vpDisplay::displayCross(I,pt,4,vpColor::red);
+ u+=0.01;
+ }
+ for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ vpDisplay::displayCross(I, *it, 4, vpColor::green);
+ }
+
+ vpDisplay::flush(I) ;
+ vpDisplay::getClick(I);
+ }
+
+ if (opt_display) {
+ try{
+ // Display size is automatically defined by the image (I) size
+ display[1].init(I2, 100, 100,"Points interpolation") ;
+ vpDisplay::display(I2) ;
+ vpDisplay::flush(I2) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error while displaying the image") ;
+ exit(-1);
+ }
+ }
+
+ Nurbs.globalCurveInterp(controlPoints);
+
+ if (opt_display && opt_click_allowed)
+ {
+ double u = 0.0;
+ while (u <= 1)
+ {
+ pt = Nurbs.computeCurvePoint(u);
+ vpDisplay::displayCross(I2,pt,4,vpColor::red);
+ u+=0.01;
+ }
+
+ for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ vpDisplay::displayCross(I2, *it, 4, vpColor::green);
+ }
+ vpDisplay::flush(I2) ;
+ vpDisplay::getClick(I2);
+ }
+
+
+ if (opt_display) {
+ try{
+ // Display size is automatically defined by the image (I) size
+ display[2].init(I3, 100, 100,"Points approximation") ;
+ vpDisplay::display(I3) ;
+ vpDisplay::flush(I3) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error while displaying the image") ;
+ exit(-1);
+ }
+ }
+
+ Nurbs.globalCurveApprox(controlPoints,5);
+
+ if (opt_display && opt_click_allowed)
+ {
+ double u = 0.0;
+ while (u <= 1)
+ {
+ pt = Nurbs.computeCurvePoint(u);
+ vpDisplay::displayCross(I3,pt,4,vpColor::red);
+ u+=0.01;
+ }
+
+ for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ vpDisplay::displayCross(I3, *it, 4, vpColor::green);
+ }
+
+ vpDisplay::flush(I3) ;
+ vpDisplay::getClick(I3);
+ }
+
+ if (N != NULL) delete[] N;
+ if (N2 != NULL)
+ {
+ for (int j = 0; j <= 2; j++)
+ delete[] N2[j];
+ delete[] N2;
+ }
+
+ return 0;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int main()
+{
+ std::cout << "This example requires a video device. "
+ << std::endl
+ << "You should install X11, GTK, OpenCV, GDI or Direct3D"
+ << std::endl
+ << "to be able to execute this example."
+ << std::endl;
+ return 0;
+}
+#endif
diff --git a/modules/tracker/tt/CMakeLists.txt b/modules/tracker/tt/CMakeLists.txt
new file mode 100644
index 0000000..1bb195f
--- /dev/null
+++ b/modules/tracker/tt/CMakeLists.txt
@@ -0,0 +1,41 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(tt visp_vision visp_core)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h
new file mode 100644
index 0000000..370fdca
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h
@@ -0,0 +1,272 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTracker.h
+ \brief
+*/
+
+#ifndef vpTemplateTracker_hh
+#define vpTemplateTracker_hh
+
+#include <math.h>
+
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/tt/vpTemplateTrackerZone.h>
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+#include <visp3/core/vpImageFilter.h>
+
+/*!
+ \class vpTemplateTracker
+ \ingroup group_tt_tracker
+*/
+class VISP_EXPORT vpTemplateTracker
+{
+ protected:
+ //traitement pyramidal
+ unsigned int nbLvlPyr; // If = 1, disable pyramidal usage
+ unsigned int l0Pyr;
+ bool pyrInitialised;
+
+ vpTemplateTrackerPoint *ptTemplate;
+ vpTemplateTrackerPoint **ptTemplatePyr;
+ bool ptTemplateInit;
+ unsigned int templateSize;
+ unsigned int *templateSizePyr;
+ bool *ptTemplateSelect;
+ bool **ptTemplateSelectPyr;
+ bool ptTemplateSelectInit;
+ unsigned int templateSelectSize;
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; //pour inverse et compo
+ vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; //pour inverse et compo
+ #endif
+
+ vpTemplateTrackerPointCompo *ptTemplateCompo; //pour ESM
+ vpTemplateTrackerPointCompo **ptTemplateCompoPyr; //pour ESM
+ vpTemplateTrackerZone *zoneTracked;
+ vpTemplateTrackerZone *zoneTrackedPyr;
+
+ vpImage<unsigned char> *pyr_IDes;
+
+ vpMatrix H;
+ vpMatrix Hdesire;
+ vpMatrix *HdesirePyr;
+ vpMatrix HLM;
+ vpMatrix HLMdesire;
+ vpMatrix *HLMdesirePyr;
+ vpMatrix HLMdesireInverse;
+ vpMatrix *HLMdesireInversePyr;
+ vpColVector G;
+
+ double gain;
+ double thresholdGradient;
+ bool costFunctionVerification;
+ bool blur;
+ bool useBrent;
+ unsigned int nbIterBrent;
+ unsigned int taillef;
+ double *fgG;
+ double *fgdG;
+ double ratioPixelIn;
+ int mod_i;
+ int mod_j;//variable de sampling de zone de reference
+ unsigned int nbParam ;
+ double lambdaDep ;
+ unsigned int iterationMax ;
+ //pour BFGS
+ unsigned int iterationGlobale;
+ //diverge is set to true if there is no more point in the tracked area
+ bool diverge;
+ unsigned int nbIteration;
+ bool useCompositionnal;
+ bool useInverse;
+
+ vpTemplateTrackerWarp *Warp;
+ //Parametre de deplacement
+ vpColVector p;
+ vpColVector dp;
+
+ //temporary values for warping
+ vpColVector X1;
+ vpColVector X2;
+ //temporary derivative matrix
+ vpMatrix dW;
+
+ vpImage<double> BI;
+ vpImage<double> dIx ;
+ vpImage<double> dIy ;
+ vpTemplateTrackerZone zoneRef_; // Reference zone
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpTemplateTracker(const vpTemplateTracker &)
+// : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
+// ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
+// ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0),
+// ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
+// zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(NULL),
+// HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
+// G(), gain(0), thresholdGradient(0), costFunctionVerification(false),
+// blur(false), useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
+// ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0), iterationMax(0),
+// iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(false),
+// useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpTemplateTracker &operator=(const vpTemplateTracker &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+ public:
+ //! Default constructor.
+ vpTemplateTracker()
+ : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
+ ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
+ ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0),
+ ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
+ zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(NULL),
+ HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
+ G(), gain(0), thresholdGradient(0), costFunctionVerification(false),
+ blur(false), useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
+ ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0), iterationMax(0),
+ iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(false),
+ useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
+ {}
+ vpTemplateTracker(vpTemplateTrackerWarp *_warp);
+ virtual ~vpTemplateTracker();
+
+ void display(const vpImage<unsigned char> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
+ void display(const vpImage<vpRGBa> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
+
+ bool getDiverge() const {return diverge;}
+ vpColVector getdp(){ return dp; }
+ vpColVector getG() const { return G; }
+ vpMatrix getH() const { return H; }
+ unsigned int getNbParam() const { return nbParam ; }
+ unsigned int getNbIteration() const { return nbIteration; }
+ vpColVector getp() const { return p;}
+ double getRatioPixelIn() const {return ratioPixelIn;}
+
+ /*!
+
+ \return The pointer to the warper.
+ */
+ vpTemplateTrackerWarp *getWarp() const {return Warp;}
+
+ /*!
+ Return the reference template zone.
+ */
+ vpTemplateTrackerZone getZoneRef() const { return zoneRef_; }
+
+ void initClick(const vpImage<unsigned char> &I, bool delaunay=false);
+ void initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false);
+ void initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone);
+
+ void resetTracker();
+
+ void setBlur(bool b){blur = b;}
+ void setCostFunctionVerification(bool b){costFunctionVerification = b;}
+ void setGain(double g){gain=g;}
+ void setGaussianFilterSize(unsigned int new_taill);
+ void setHDes(vpMatrix &tH){ Hdesire=tH; vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire); HLMdesireInverse = HLMdesire.inverseByLU();}
+ /*!
+ Set the maximum number of iteration of the estimation scheme.
+ \param n : Maximum number of iterations to stop the estimation scheme. A typical value is arround 100.
+ */
+ void setIterationMax(const unsigned int &n) { iterationMax = n ; }
+ /*!
+ Set the convergence gain used in the estimation scheme.
+ \param l : Gain. A typical value is 0.001.
+ */
+ void setLambda(double l) { lambdaDep = l ; }
+ void setNbIterBrent(const unsigned int &b){nbIterBrent=b;}
+ void setp(const vpColVector &tp){ p=tp; diverge=false; iterationGlobale=0; }
+ /*!
+ Set the number of pyramid levels used in the multi-resolution scheme.
+ If \e nlevels > 1, the tracker uses a pyramidal approach.
+
+ \param nlevels : Number of pyramid levels. Algorithm starts at level nlevels-1.
+ \param level_to_stop : Last level of the pyramid that will be considered. Lowest level is zero.
+ */
+ void setPyramidal(unsigned int nlevels=2, unsigned int level_to_stop=1) {
+ nbLvlPyr = nlevels;
+ l0Pyr = level_to_stop;
+ if(l0Pyr >= nlevels){
+ std::cout << "Warning: level_to_stop: " << level_to_stop << " higher than level_to_start: " << nlevels-1 << " (nlevels-1)" <<std::endl;
+ std::cout << "Level to stop put to: " << nlevels-1 << std::endl;
+ l0Pyr = nlevels-1;
+ }
+ }
+ /*!
+ Set the pixel sampling parameters along the rows and the columns.
+ \param sample_i : Sampling factor along the rows.
+ If 1 all the lines are considered. If 2, consider one line over two.
+
+ \param sample_j : Sampling factor along the columns.
+ If 1 all the columns are considered. If 2, consider one column over two.
+ */
+ void setSampling(int sample_i,int sample_j){mod_i=sample_i; mod_j=sample_j;}
+ void setThresholdGradient(double threshold){thresholdGradient=threshold;}
+ /*! By default Brent usage is disabled. */
+ void setUseBrent(bool b){useBrent = b;}
+
+ void track(const vpImage<unsigned char> &I);
+ void trackRobust(const vpImage<unsigned char> &I);
+
+ protected:
+
+ void computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha);
+ virtual double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) = 0;
+ void getGaussianBluredImage(const vpImage<unsigned char> &I){ vpImageFilter::filter(I, BI,fgG,taillef); }
+ void initCompInverse(const vpImage<unsigned char> &I);
+ virtual void initCompInversePyr(const vpImage<unsigned char> &I);
+ virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
+ virtual void initHessienDesiredPyr(const vpImage<unsigned char> &I);
+ virtual void initPyramidal(unsigned int nbLvl,unsigned int l0);
+ void initTracking(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
+ virtual void initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
+ virtual void trackPyr(const vpImage<unsigned char> &I);
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h
new file mode 100644
index 0000000..6603dc8
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerBSpline.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerBSpline_hh
+#define vpTemplateTrackerBSpline_hh
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+class VISP_EXPORT vpTemplateTrackerBSpline
+{
+public:
+ static double Bspline4(double diff);
+
+ static double getSubPixBspline4(const vpImage<double> &I, double r, double t);
+};
+#endif
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h
new file mode 100644
index 0000000..3db4832
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerHeader.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerHeader_hh
+#define vpTemplateTrackerHeader_hh
+
+#include <stdio.h>
+
+/*!
+ \struct vpTemplateTrackerZPoint
+ \ingroup group_tt_tools
+*/
+struct vpTemplateTrackerZPoint {
+ int x,y;
+
+ vpTemplateTrackerZPoint() : x(0), y(0) {}
+};
+/*!
+ \struct vpTemplateTrackerDPoint
+ \ingroup group_tt_tools
+*/
+struct vpTemplateTrackerDPoint {
+ double x,y;
+
+ vpTemplateTrackerDPoint() : x(0), y(0) {}
+};
+/*!
+ \struct vpTemplateTrackerPoint
+ \ingroup group_tt_tools
+*/
+struct vpTemplateTrackerPoint {
+ int x,y;
+ double dx,dy;
+ double val;
+ double *dW;
+ double *HiG;
+
+ vpTemplateTrackerPoint() : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) {}
+};
+/*!
+ \struct vpTemplateTrackerPointCompo
+ \ingroup group_tt_tools
+*/
+struct vpTemplateTrackerPointCompo {
+ double *dW;
+ vpTemplateTrackerPointCompo() : dW(NULL) {}
+};
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+struct vpTemplateTrackerPointSuppMIInv {
+ double et;
+ int ct;
+ double *BtInit;
+ double *Bt;
+ double *dBt;
+ double *d2W;
+ double *d2Wx;
+ double *d2Wy;
+ vpTemplateTrackerPointSuppMIInv() : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL),
+ d2W(NULL), d2Wx(NULL), d2Wy(NULL) {}
+};
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h
new file mode 100644
index 0000000..38c5dc7
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerSSD.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerSSD_hh
+#define vpTemplateTrackerSSD_hh
+
+#include <math.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpHomography.h>
+
+/*!
+ \class vpTemplateTrackerSSD
+ \ingroup group_tt_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerSSD: public vpTemplateTracker
+{
+ protected:
+ vpRowVector DI;
+ vpRowVector temp;
+
+ protected:
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getCost(const vpImage<unsigned char> &I){ return getCost(I,p); }
+ virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
+
+ public:
+ vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp);
+
+ double getSSD(const vpImage<unsigned char> &I, const vpColVector &tp);
+ void setGain(double g){ gain=g; }
+};
+
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h
new file mode 100644
index 0000000..fc05712
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerSSDESM.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerSSDESM_hh
+#define vpTemplateTrackerSSDESM_hh
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+
+/*!
+ \class vpTemplateTrackerSSDESM
+ \ingroup group_tt_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerSSDESM: public vpTemplateTrackerSSD
+{
+ protected:
+ bool compoInitialised;
+ vpMatrix HDir;
+ vpMatrix HInv;
+ vpMatrix HLMDir;
+ vpMatrix HLMInv;
+ vpColVector GDir;
+ vpColVector GInv;
+
+ protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+ public:
+ vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h
new file mode 100644
index 0000000..bb05685
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerSSDForwardAdditional.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerSSDForwardAdditional_hh
+#define vpTemplateTrackerSSDForwardAdditional_hh
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+
+/*!
+ \ingroup group_tt_tracker
+ The algorithm implemented in this class is described in \cite Baker04a.
+ */
+class VISP_EXPORT vpTemplateTrackerSSDForwardAdditional: public vpTemplateTrackerSSD
+{
+ public:
+ /*! Minimization method. */
+ typedef enum {
+ USE_NEWTON,
+ USE_LMA,
+ USE_GRADIENT,
+ USE_QUASINEWTON
+ } vpMinimizationTypeSSDForwardAdditional;
+
+ private:
+ vpMinimizationTypeSSDForwardAdditional minimizationMethod;
+ //valeur pour calculer Quasi_Newton
+ vpColVector p_prec;
+ vpColVector G_prec;
+ vpMatrix KQuasiNewton;
+
+ protected:
+ void initHessienDesired(const vpImage<unsigned char> &/*I*/){}
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+ public:
+ vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp);
+
+ void setMinimizationMethod(vpMinimizationTypeSSDForwardAdditional method){minimizationMethod=method;}
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h
new file mode 100644
index 0000000..0e04cb4
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerSSDForwardCompositional.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerSSDForwardCompositional_hh
+#define vpTemplateTrackerSSDForwardCompositional_hh
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+
+/*!
+ \ingroup group_tt_tracker
+ The algorithm implemented in this class is described in \cite Baker04a.
+ */
+class VISP_EXPORT vpTemplateTrackerSSDForwardCompositional: public vpTemplateTrackerSSD
+{
+ protected:
+ bool compoInitialised;
+
+ protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompo(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+ public:
+ vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h
new file mode 100644
index 0000000..d86ff97
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerSSDInverseCompositional.h
+ \brief
+*/
+#ifndef vpTemplateTrackerSSDInverseCompositional_hh
+#define vpTemplateTrackerSSDInverseCompositional_hh
+
+#include <vector>
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+
+/*!
+ \ingroup group_tt_tracker
+ The algorithm implemented in this class is described in \cite Baker04a.
+ */
+class VISP_EXPORT vpTemplateTrackerSSDInverseCompositional: public vpTemplateTrackerSSD
+{
+ protected:
+ bool compoInitialised;
+ vpMatrix HInv;
+ vpMatrix HCompInverse;
+ bool useTemplateSelect;//use only the strong gradient pixels to compute the Jabocian
+ //pour eval evolRMS
+ double evolRMS;
+ std::vector<double> x_pos;
+ std::vector<double> y_pos;
+ double threshold_RMS;
+
+ protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
+
+ public:
+ vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp);
+
+ /*! Use only the strong gradient pixels to compute the Jabobian. By default this feature is disabled. */
+ void setUseTemplateSelect(bool b) {useTemplateSelect = b;}
+ void setThresholdRMS(double threshold){threshold_RMS=threshold;}
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h
new file mode 100644
index 0000000..5a55289
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerTriangle.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerTriangle_hh
+#define vpTemplateTrackerTriangle_hh
+
+#include <assert.h>
+#include <vector>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+
+/*!
+ \class vpTemplateTrackerTriangle
+ \ingroup group_tt_tools
+*/
+class VISP_EXPORT vpTemplateTrackerTriangle
+{
+ protected:
+ double minx_temp;
+ double miny_temp;
+ vpTemplateTrackerDPoint C1; //! Corner 1
+ vpTemplateTrackerDPoint C2; //! Corner 2
+ vpTemplateTrackerDPoint C3; //! Corner 3
+
+ double l_t;
+ double h_t;
+
+ bool not_good;
+ double uvinv00;
+ double uvinv01;
+ double uvinv10;
+ double uvinv11;
+ double marge_triangle;
+ double area;
+
+ private:
+ vpColVector getCorner1() const;
+ vpColVector getCorner2() const;
+ vpColVector getCorner3() const;
+
+ public:
+ vpTemplateTrackerTriangle();
+ vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T);
+ vpTemplateTrackerTriangle(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3);
+ vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
+ vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
+ vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3);
+
+ /*!
+ Return the area of the triangle.
+
+ \return The area of the triangle.
+ */
+ inline double getArea() const{
+ return this->area;
+ }
+
+ vpTemplateTrackerTriangle getPyramidDown() const;
+ void getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const;
+ void getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const;
+ void getCorners(std::vector<vpImagePoint> &c) const;
+
+ /*!
+ Returns the coordinates of a triangle corner.
+ \param i : Allowed values are 0, 1 or 2.
+ \return
+ - if i = 0, return corner 1 coordinates,
+ - if i = 1, return corner 2 coordinates,
+ - if i = 2, return corner 3 coordinates.
+
+ The coordinates are returned as a 2 dimension vector (x,y).
+ */
+ vpColVector getCorner(unsigned int i) const {
+ assert(i<3);
+ if(i==0) return getCorner1();
+ else if(i==1) return getCorner2();
+ else /*if(i==2)*/ return getCorner3();
+ };
+ /*!
+ Returns the coordinates of a triangle corner.
+ \param i : Allowed values are 0, 1 or 2.
+ - if i = 0, return corner 1 coordinates,
+ - if i = 1, return corner 2 coordinates,
+ - if i = 2, return corner 3 coordinates.
+ \param x,y : The coordinates of the corner.
+ */
+ void getCorner(unsigned int i, double &x, double &y) const {
+ assert(i<3);
+ if(i==0) {
+ x = C1.x;
+ y = C1.y;
+ }
+ else if(i==1) {
+ x = C2.x;
+ y = C2.y;
+ }
+ else /*if(i==2)*/ {
+ x = C3.x;
+ y = C3.y;
+ }
+ };
+
+ double getMaxx() const;
+ double getMaxy() const;
+ double getMinx() const;
+ double getMiny() const;
+
+ void getSize(double &w, double &h) const;
+ void getSize(int &w, int &h) const;
+
+ void init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3);
+ void init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
+ void init(int x1,int y1, int x2,int y2, int x3,int y3);
+ void init(double x1,double y1, double x2,double y2, double x3,double y3);
+ bool inTriangle(const vpImagePoint &ip) const;
+ bool inTriangle(const int &i, const int &j) const;
+ bool inTriangle(const double &i,const double &j) const;
+
+ vpTemplateTrackerTriangle & operator=(const vpTemplateTrackerTriangle& T);
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h
new file mode 100644
index 0000000..9063c5c
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h
@@ -0,0 +1,246 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarp.h
+ \brief
+*/
+
+
+#ifndef vpTemplateTrackerWarp_hh
+#define vpTemplateTrackerWarp_hh
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/tt/vpTemplateTrackerTriangle.h>
+#include <visp3/tt/vpTemplateTrackerZone.h>
+#include <visp3/core/vpTrackingException.h>
+
+/*!
+ \class vpTemplateTrackerWarp
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarp
+{
+ protected:
+ double denom;
+ vpMatrix dW;
+ unsigned int nbParam ;
+
+ public:
+ //constructor;
+ vpTemplateTrackerWarp() : denom(1.), dW(), nbParam(0) {}
+ virtual ~vpTemplateTrackerWarp(){}
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ virtual void computeCoeff(const vpColVector &p)=0;
+ virtual void computeDenom(vpColVector &vX, const vpColVector &ParamM)=0;
+ #endif
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ virtual void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW) = 0;
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ virtual void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW) = 0;
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
+ #endif
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ virtual void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW) = 0;
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ virtual void getdWdp0(const int &i,const int &j,double *dIdW) = 0;
+
+ /*!
+ Compute the distance between a zone and its associated warped zone.
+
+ \param Z : Zone to consider.
+ \param p : Parameters of the warping function.
+ */
+ double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z,const vpColVector &p);
+
+ /*!
+ Get the number of parameters of the warping function.
+
+ \return Number of parameters.
+ */
+ unsigned int getNbParam() const {return nbParam;}
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ virtual void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const = 0;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ virtual void getParamPyramidDown(const vpColVector &p,vpColVector &pdown) =0;
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ virtual void getParamPyramidUp(const vpColVector &p,vpColVector &pup) =0;
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ virtual bool isESMcompatible() const =0;
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ virtual void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const = 0;
+
+ /*!
+ Set the number of parameters of the warping function.
+
+ \param nb : New number of parameters.
+ */
+ void setNbParam(unsigned int nb){nbParam=nb;dW.resize(2,nbParam);}
+
+ /*!
+ Warp a list of points.
+
+ \param ut0 : List of u coordinates of the points.
+ \param vt0 : List of v coordinates of the points.
+ \param nb_pt : Number of points to consider.
+ \param p : Parameters of the warp.
+ \param u : Resulting u coordinates.
+ \param v : resulting v coordinates.
+ */
+ void warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warp.
+ */
+ virtual void warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM) = 0;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ virtual void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ virtual void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
+
+ /*!
+ Warp a triangle and store the result in a new zone.
+
+ \param in : Triangle to warp.
+ \param p : Parameters of the warping function. These parameters are estimated by the template
+ tracker and returned using vpTemplateTracker::getp().
+ \param out : Resulting triangle.
+ */
+ void warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out);
+
+ /*!
+ Warp a zone and store the result in a new zone.
+
+ \param in : Zone to warp.
+ \param p : Parameters of the warping function. These parameters are estimated by the template
+ tracker and returned using vpTemplateTracker::getp().
+ \param out : Resulting zone.
+ */
+ void warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out);
+
+};
+
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h
new file mode 100644
index 0000000..98b18d8
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpAffine.h
+ \brief Affine warping function: w(X)=AX+b with: A=[[1+p0, p2], [p1, 1+p3]] and b= [p4, p5]]
+*/
+
+
+#ifndef vpTemplateTrackerWarpAffine_hh
+#define vpTemplateTrackerWarpAffine_hh
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+
+/*!
+ \class vpTemplateTrackerWarpAffine
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpAffine: public vpTemplateTrackerWarp
+{
+ public:
+ //constructor;
+ vpTemplateTrackerWarpAffine();
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector &/*p*/){}
+ void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
+ #endif
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return false;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the columns) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h
new file mode 100644
index 0000000..e16ae17
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h
@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpHomography.h
+ \brief warping function of an homography: the homography is directly defined by the diplacement parameter: H=[[1+p0, p3, p6], [p1, 1+p4, p7], [p2, p5, 1]]
+*/
+
+
+#ifndef vpTemplateTrackerWarpHomography_hh
+#define vpTemplateTrackerWarpHomography_hh
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+#include <visp3/vision/vpHomography.h>
+
+/*!
+ \class vpTemplateTrackerWarpHomography
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpHomography: public vpTemplateTrackerWarp
+{
+ public:
+ //constructor;
+ vpTemplateTrackerWarpHomography();
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector &/*p*/){}
+ #endif
+
+ /*!
+ Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
+
+ \param vX : Point to consider
+ \param ParamM : parameters of the warping function.
+ */
+ void computeDenom(vpColVector &vX, const vpColVector &ParamM);
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Return the homography defined by the given parameters
+
+ \param ParamM : Parameters of the homography.
+
+ \return An Homography via vpHomography.
+ */
+ vpHomography getHomography(const vpColVector &ParamM) const;
+
+ /*!
+ Compute the parameters of the homography warping function according to the given vpHomography
+
+ \param H : Homography used to compute the parameters.
+ \param par : Resulting warping function parameters.
+ */
+ void getParam(const vpHomography &H,vpColVector &par) const;
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return false;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h
new file mode 100644
index 0000000..4d52d89
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h
@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpHomographySL3.h
+ \brief warping function of an homography: the homography is defined on the sl3 lie algebra H=exp(Sum(p[i]* A_i)) A_i is the basis of the SL3 Algebra
+*/
+
+
+#ifndef vpTemplateTrackerWarpHomographySL3_hh
+#define vpTemplateTrackerWarpHomographySL3_hh
+
+#include <vector>
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+#include <visp3/vision/vpHomography.h>
+
+/*!
+ \class vpTemplateTrackerWarpHomographySL3
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpHomographySL3: public vpTemplateTrackerWarp
+{
+ protected:
+ vpMatrix G;
+ vpMatrix dGx;
+ std::vector<vpMatrix> A;
+
+ public:
+ //constructor;
+ vpTemplateTrackerWarpHomographySL3();
+ ~vpTemplateTrackerWarpHomographySL3();
+
+ /*!
+ Compute the exponential of the homography matrix defined by the given parameters
+
+ \param p : Parameters of the SL3 homography warping function.
+ */
+ void computeCoeff(const vpColVector &p);
+
+ /*!
+ Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
+
+ \param vX : Point to consider
+ \param ParamM : parameters of the warping function.
+ */
+ void computeDenom(vpColVector &vX, const vpColVector &ParamM);
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Find the displacement/warping function parameters from a list of points.
+
+ \param ut0 : Original u coordinates.
+ \param vt0 : Original v coordinates.
+ \param u : Warped u coordinates.
+ \param v : Warped v coordinates.
+ \param nb_pt : Number of points.
+ \param p : Resulting warping function parameters.
+ */
+ void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const double &i, const double &j,double *dIdW);
+
+ /*!
+ Return the homography defined by the warping function
+
+ \return An Homography via vpHomography.
+ */
+ vpHomography getHomography() const;
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return true;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void warpXInv(const vpColVector &/*vX*/,vpColVector &/*vXres*/,const vpColVector &/*ParamM*/) {}
+ #endif
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h
new file mode 100644
index 0000000..5db4f1c
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpRT.h
+ \brief sRt warping function
+*/
+
+
+#ifndef vpTemplateTrackerWarpRT_hh
+#define vpTemplateTrackerWarpRT_hh
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+
+/*!
+ \class vpTemplateTrackerWarpRT
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpRT: public vpTemplateTrackerWarp
+{
+public:
+ //constructor;
+ vpTemplateTrackerWarpRT();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector &/*p*/){}
+ void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
+#endif
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return false;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h
new file mode 100644
index 0000000..8930ce2
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpSRT.h
+ \brief sRt warping function
+*/
+
+
+#ifndef vpTemplateTrackerWarpSRT_hh
+#define vpTemplateTrackerWarpSRT_hh
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+
+/*!
+ \class vpTemplateTrackerWarpSRT
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpSRT: public vpTemplateTrackerWarp
+{
+ public:
+ //constructor;
+ vpTemplateTrackerWarpSRT();
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector &/*p*/){}
+ void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
+ #endif
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return false;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h
new file mode 100644
index 0000000..63551a6
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerWarpTranslation.h
+ \brief Translation warping function: w(X)=X+b with: b= [p0, p1]]
+*/
+
+
+#ifndef vpTemplateTrackerWarpTranslation_hh
+#define vpTemplateTrackerWarpTranslation_hh
+
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+
+/*!
+ \class vpTemplateTrackerWarpTranslation
+ \ingroup group_tt_warp
+*/
+class VISP_EXPORT vpTemplateTrackerWarpTranslation: public vpTemplateTrackerWarp
+{
+ public:
+ //constructor;
+ vpTemplateTrackerWarpTranslation();
+
+ #ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector &/*p*/){}
+ void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
+ #endif
+
+ /*!
+ Compute the derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warp.
+ \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
+ \param dW : Resulting compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
+ \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the image.
+ \param j : j coordinate (along the columns) of the point to consider in the image.
+ \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ */
+ void getdWdp0(const int &i,const int &j,double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const ;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const {return true;}
+
+ /*!
+ Get the displacement resulting from the composition of two other displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+};
+#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h
new file mode 100644
index 0000000..31df8fd
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerZNCC.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerZNCC_hh
+#define vpTemplateTrackerZNCC_hh
+
+#include <math.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpHomography.h>
+
+#define APPROX_NCC
+
+/*!
+ \class vpTemplateTrackerZNCC
+ \ingroup group_tt_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerZNCC: public vpTemplateTracker
+{
+ protected:
+ vpRowVector DI;
+ vpRowVector temp;
+
+ protected:
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) ;
+ double getCost(const vpImage<unsigned char> &I) {vpColVector tp; return getCost(I,tp);}
+ virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I)=0;
+
+ public:
+ vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp);
+
+ void setGain(double _gain){gain=_gain;}
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h
new file mode 100644
index 0000000..847a8fe
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerZNCCForwardAdditional.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerZNCCForwardAdditional_hh
+#define vpTemplateTrackerZNCCForwardAdditional_hh
+
+
+#include <visp3/tt/vpTemplateTrackerZNCC.h>
+
+/*!
+ \class vpTemplateTrackerZNCCForwardAdditional
+ \ingroup group_tt_tracker
+ The algorithm implemented in this class is described in \cite Irani98a.
+ */
+class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional: public vpTemplateTrackerZNCC
+{
+ protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+ public:
+ vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp);
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h
new file mode 100644
index 0000000..f8d037b
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerZNCCInverseCompositional.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerZNCCInverseCompositional_hh
+#define vpTemplateTrackerZNCCInverseCompositional_hh
+
+#include <vector>
+
+#include <visp3/tt/vpTemplateTrackerZNCC.h>
+
+/*!
+ \class vpTemplateTrackerZNCCInverseCompositional
+ \ingroup group_tt_tracker
+ The algorithm implemented in this class is described in \cite Irani98a.
+ */
+class VISP_EXPORT vpTemplateTrackerZNCCInverseCompositional: public vpTemplateTrackerZNCC
+{
+ protected:
+ bool compoInitialised;
+ //pour eval evolRMS
+ double evolRMS;
+ std::vector<double> x_pos;
+ std::vector<double> y_pos;
+ double threshold_RMS;
+ vpColVector moydIrefdp;
+
+ protected:
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
+
+ public:
+ vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp);
+
+ void setThresholdRMS(double threshold){threshold_RMS=threshold;}
+};
+#endif
+
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h
new file mode 100644
index 0000000..e1c0fe0
--- /dev/null
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerZone_hh
+#define vpTemplateTrackerZone_hh
+
+#include <vector>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/tt/vpTemplateTrackerTriangle.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpRect.h>
+
+/*!
+ \class vpTemplateTrackerZone
+ \ingroup group_tt_tools
+ A zone is defined by a set of triangles defined as vpTemplateTrackerTriangle.
+
+ A zone can be initialized either by user interaction using mouse click in a display device
+ throw initClick(), or by a list of points throw initFromPoints().
+ */
+class VISP_EXPORT vpTemplateTrackerZone
+{
+ protected:
+ std::vector<vpTemplateTrackerTriangle> Zone; //!< Vector of triangles that defines the zone.
+ int min_x; //!< Bounding box parameter
+ int min_y; //!< Bounding box parameter
+ int max_x; //!< Bounding box parameter
+ int max_y; //!< Bounding box parameter
+
+ public:
+ vpTemplateTrackerZone();
+ vpTemplateTrackerZone(const vpTemplateTrackerZone &z);
+ ~vpTemplateTrackerZone();
+
+ //add a triangle to the zone
+ void add(const vpTemplateTrackerTriangle &t);
+ void clear();
+ void copy(const vpTemplateTrackerZone& z);
+
+ //display the area on an image
+ void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
+ void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
+
+ //colorie le tieme triangle
+ void fillTriangle(vpImage<unsigned char>& I, unsigned int id, unsigned char gray_level);
+
+ double getArea() const;
+ vpImagePoint getCenter() const;
+ vpImagePoint getCenter(int borne_x, int borne_y) const;
+ //get bounds of the area
+ int getMaxx() const;
+ int getMaxy() const;
+ int getMinx() const;
+ int getMiny() const;
+ vpRect getBoundingBox() const;
+
+ /*! Return the number of triangles that define the zone. \sa getTriangle() */
+ unsigned int getNbTriangle() const { return (unsigned int)Zone.size(); }
+ vpTemplateTrackerZone getPyramidDown() const;
+ //renvoie le ieme triangle de la zone
+ void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const;
+ vpTemplateTrackerTriangle getTriangle(unsigned int i) const;
+
+ //create an area by clicking on an image
+ void initClick(const vpImage<unsigned char>& I, bool delaunay = false);
+ //create an area with a pointer of integer that describes a series of triangles:
+ // *pt= t0.S1.x,t0.S1.y,t0.S2.x,t0.S2.y,t0.S3.x,t0.S3.y, t1.S1.x ...
+ void initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &ip, bool delaunay = false);
+
+ //check if a point is in the area
+ bool inZone(const int &i,const int &j) const;
+ bool inZone(const double &i,const double &j) const;
+ //check if a point is in the area and return the corresponding triangle id_triangle where the point is.
+ bool inZone(const int &i,const int &j, unsigned int &id_triangle) const;
+ bool inZone(const double &i,const double &j, unsigned int &id_triangle) const;
+
+ vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z);
+};
+#endif
+
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp
new file mode 100644
index 0000000..bb18a35
--- /dev/null
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/tt/vpTemplateTrackerSSD.h>
+
+vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
+ : vpTemplateTracker(warp), DI(), temp()
+{
+ dW.resize(2,nbParam);
+ G.resize(nbParam);
+ H.resize(nbParam,nbParam);
+ HLM.resize(nbParam,nbParam);
+
+ temp.resize(nbParam);
+
+ X1.resize(2);
+ X2.resize(2);
+ DI.resize(2);
+}
+
+double vpTemplateTrackerSSD::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ double erreur=0;
+ double IW,Tij;
+ int i,j;
+ double i2,j2;
+ int Nbpoint=0;
+
+ Warp->computeCoeff(tp);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ //IW=getSubPixBspline4(I,i2,j2);
+ erreur+=((double)Tij-IW)*((double)Tij-IW);
+ Nbpoint++;
+ }
+ }
+ ratioPixelIn=(double)Nbpoint/(double)templateSize;
+
+ if(Nbpoint==0)return 10e10;
+ return erreur/Nbpoint;
+}
+
+
+double vpTemplateTrackerSSD::getSSD(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ double erreur=0;
+ double IW,Tij;
+ int i,j;
+ double i2,j2;
+ unsigned int Nbpoint=0;
+
+ if(pyrInitialised)
+ {
+ templateSize=templateSizePyr[0];
+ ptTemplate=ptTemplatePyr[0];
+ }
+
+ Warp->computeCoeff(tp);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+
+ j2=X2[0];i2=X2[1];
+ if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
+ {
+ Tij=ptTemplate[point].val;
+ IW=I.getValue(i2,j2);
+ //IW=getSubPixBspline4(I,i2,j2);
+ erreur+=((double)Tij-IW)*((double)Tij-IW);
+ Nbpoint++;
+ }
+ }
+ if(Nbpoint==0)return 10e10;
+ return erreur/Nbpoint;
+}
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp
new file mode 100644
index 0000000..1254961
--- /dev/null
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp
@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerSSDESM.h>
+#include <visp3/core/vpImageFilter.h>
+
+vpTemplateTrackerSSDESM::vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerSSD(warp), compoInitialised(false), HDir(), HInv(),
+ HLMDir(), HLMInv(), GDir(), GInv()
+{
+ useCompositionnal=false;
+ useInverse=false;
+
+ if(!Warp->isESMcompatible())
+ std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
+
+ HInv.resize(nbParam,nbParam);
+ HDir.resize(nbParam,nbParam);
+ HLMInv.resize(nbParam,nbParam);
+ HLMDir.resize(nbParam,nbParam);
+ GInv.resize(nbParam);
+ GDir.resize(nbParam);
+}
+
+void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompInverse(I);
+}
+
+void vpTemplateTrackerSSDESM::initCompInverse(const vpImage<unsigned char> &/*I*/)
+{
+ //std::cout<<"Initialise precomputed value of ESM with templateSize: "<< templateSize<<std::endl;
+ ptTemplateCompo=new vpTemplateTrackerPointCompo[templateSize];
+ int i,j;
+ //direct
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ ptTemplateCompo[point].dW=new double[2*nbParam];
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
+
+ }
+
+ //inverse
+ HInv=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ ptTemplate[point].dW=new double[nbParam];
+ Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ HInv[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
+ }
+ vpMatrix::computeHLM(HInv,lambdaDep,HLMInv);
+
+compoInitialised=true;
+}
+
+void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ double erreur=0;
+ unsigned int Nbpoint=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ double IW,dIWx,dIWy;
+ double Tij;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ double alpha=2.;
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ dp=0;
+ HDir=0;
+ GDir=0;
+ GInv=0;
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ //INVERSE
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ Nbpoint++;
+ double er=(Tij-IW);
+ for(unsigned int it=0;it<nbParam;it++)
+ GInv[it]+=er*ptTemplate[point].dW[it];
+
+ erreur+=er*er;
+
+ //DIRECT
+ //dIWx=dIx.getValue(i2,j2);
+ //dIWy=dIy.getValue(i2,j2);
+
+ dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
+ dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;
+
+ //Calcul du Hessien
+ //Warp->dWarp(X1,X2,p,dW);
+ Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ HDir[it][jt]+=tempt[it]*tempt[jt];
+
+ for(unsigned int it=0;it<nbParam;it++)
+ GDir[it]+=er*tempt[it];
+ delete[] tempt;
+ }
+
+
+ }
+ if(Nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+
+ vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);
+
+ try
+ {
+ //dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
+ //dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
+ //dp=HLMInv.inverseByLU()*GInv;
+ dp=(HLMDir).inverseByLU()*(GDir);
+ }
+ catch(vpException &e)
+ {
+ //std::cout<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+
+ dp=gain*dp;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
+ dp=alpha*dp;
+ }
+
+ //Warp->pRondp(p,dp,p);
+ p+=dp;
+ iteration++;
+
+ }
+ while( (iteration < iterationMax));
+
+ nbIteration=iteration;
+}
+
+
+/*void vpTemplateTrackerSSDESM::InitCompInverse(vpImage<unsigned char> &I)
+{
+ ptTempateCompo=new vpTemplateTrackerPointCompo[taille_template];
+ int i,j;
+ for(int point=0;point<taille_template;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ ptTempateCompo[point].dW=new double[2*nbParam];
+ Warp->getdWdp0(i,j,ptTempateCompo[point].dW);
+ }
+
+}
+
+void vpTemplateTrackerSSDESM::track(vpImage<unsigned char> &I)
+{
+ double erreur=0,erreur_prec=1e38;
+ int Nbpoint=0;
+
+ int taillefiltre=taille_filtre_dgaussien;
+ double *fg=new double[taillefiltre+1] ;
+ getGaussianDerivativeKernel(fg, taillefiltre) ;
+ getGradX(I, dIx,fg,taillefiltre);
+ getGradY(I, dIy,fg,taillefiltre);
+ delete[] fg;
+
+ vpColVector dpinv(nbParam);
+ double lambda=lambdaDep;
+ double IW,dIWx,dIWy;
+ double Tij;
+ int iteration=0;
+ int i,j;
+ double i2,j2;
+ vpTemplateTrackerPoint *pt;
+ do
+ {
+ Nbpoint=0;
+ erreur_prec=erreur;
+ erreur=0;
+ dp=0;
+ HDir=0;
+ GDir=0;
+ GInv=0;
+ Warp->ComputeCoeff(p);
+ for(int point=0;point<taille_template;point++)
+ {
+ pt=&ptTemplate[point];
+ i=pt->y;
+ j=pt->x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->ComputeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((j2<I.getWidth())&&(i2<I.getHeight())&&(i2>0)&&(j2>0))
+ {
+ //INVERSE
+ Tij=pt->val;
+ IW=I.getPixelBI(j2,i2);
+ Nbpoint++;
+ double er=(Tij-IW);
+ for(int it=0;it<nbParam;it++)
+ GInv[it]+=er*ptTemplate[point].dW[it];
+
+ erreur+=er*er;
+
+ //DIRECT COMPO
+ Tij=ptTemplate[point].val;
+ IW=I.getPixelBI(j2,i2);
+ dIWx=dIx.getPixelBI(j2,i2);
+ dIWy=dIy.getPixelBI(j2,i2);
+ Nbpoint++;
+ Warp->dWarpCompo(X1,X2,p,ptTempateCompo[point].dW,dW);
+ double *tempt=new double[nbParam];
+ for(int it=0;it<nbParam;it++)
+ tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+ for(int it=0;it<nbParam;it++)
+ for(int jt=0;jt<nbParam;jt++)
+ HDir[it][jt]+=tempt[it]*tempt[jt];
+
+ for(int it=0;it<nbParam;it++)
+ GDir[it]+=er*tempt[it];
+
+ delete[] tempt;
+ }
+
+
+ }
+ if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl;
+ try
+ {
+ dp=(HInv+HDir).inverseByLU()*(GInv+GDir);
+ }
+ catch(...)
+ {
+ std::cout<<"probleme inversion"<<std::endl;
+ break;
+ }
+
+ if(Compare)write_infos(p,erreur/Nbpoint);
+
+ p+=Gain*dp;
+ iteration++;
+
+ }
+ while( (iteration < IterationMax));
+
+ NbIteration=iteration;
+}*/
+
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
new file mode 100644
index 0000000..2722aa3
--- /dev/null
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <limits> // numeric_limits
+
+#include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
+#include <visp3/core/vpImageTools.h>
+
+vpTemplateTrackerSSDForwardAdditional::vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerSSD(warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
+{
+ useCompositionnal=false;
+}
+
+void vpTemplateTrackerSSDForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ double erreur=0;
+ unsigned int Nbpoint=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ dW=0;
+
+ double lambda=lambdaDep;
+ double IW,dIWx,dIWy;
+ double Tij;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ double alpha=2.;
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ G=0;
+ H=0 ;
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dIWx=dIx.getValue(i2,j2);
+ dIWy=dIy.getValue(i2,j2);
+ Nbpoint++;
+ //Calcul du Hessien
+ Warp->dWarp(X1,X2,p,dW);
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ H[it][jt]+=tempt[it]*tempt[jt];
+
+ double er=(Tij-IW);
+ for(unsigned int it=0;it<nbParam;it++)
+ G[it]+=er*tempt[it];
+
+ erreur+=(er*er);
+ delete[] tempt;
+ }
+
+
+ }
+ if(Nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+
+ vpMatrix::computeHLM(H,lambda,HLM);
+ try
+ {
+ dp=1.*HLM.inverseByLU()*G;
+ }
+ catch(vpException &e)
+ {
+ throw(e);
+ }
+
+ switch(minimizationMethod)
+ {
+ case vpTemplateTrackerSSDForwardAdditional::USE_LMA:
+ {
+ vpColVector p_test_LMA(nbParam);
+ p_test_LMA=p+1.*dp;
+ erreur=-getCost(I,p);
+ double erreur_LMA=-getCost(I,p_test_LMA);
+ if(erreur_LMA<erreur)
+ {
+ p=p_test_LMA;
+ lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
+ }
+ else
+ {
+ lambda=(lambda*10.<1e6)?1e6:lambda*10.;
+ }
+ }
+ break;
+ case vpTemplateTrackerSSDForwardAdditional::USE_GRADIENT:
+ {
+ dp=gain*0.000001*G;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur,dp,alpha);
+ dp=alpha*dp;
+ }
+ p += 1. * dp;
+ break;
+ }
+
+ case vpTemplateTrackerSSDForwardAdditional::USE_QUASINEWTON:
+ {
+ double s_scal_y;
+ if(iterationGlobale!=0)
+ {
+ vpColVector s_quasi=p-p_prec;
+ vpColVector y_quasi=G-G_prec;
+ s_scal_y=s_quasi.t()*y_quasi;
+ //if(s_scal_y!=0)//BFGS
+ // KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
+ //if(s_scal_y!=0.0)//DFP
+ if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) //DFP
+ KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
+ }
+ dp=-KQuasiNewton*G;
+ p_prec=p;
+ G_prec=G;
+ p-=1.01*dp;
+ }
+ break;
+
+ case vpTemplateTrackerSSDForwardAdditional::USE_NEWTON:
+ default:
+ {
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur,dp,alpha);
+ dp=alpha*dp;
+ }
+
+ p+=1.*dp;
+ break;
+ }
+ }
+
+ iteration++;
+ iterationGlobale++;
+ }
+ while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
+
+ //std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration=iteration;
+}
+
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
new file mode 100644
index 0000000..e8c186d
--- /dev/null
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
+#include <visp3/core/vpImageFilter.h>
+
+vpTemplateTrackerSSDForwardCompositional::vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerSSD(warp), compoInitialised(false)
+{
+}
+
+void vpTemplateTrackerSSDForwardCompositional::initCompo(const vpImage<unsigned char> &/*I*/)
+{
+ // std::cout<<"Initialise precomputed value of Compositionnal Direct"<<std::endl;
+ int i,j;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ ptTemplate[point].dW=new double[2*nbParam];
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ Warp->getdWdp0(i,j,ptTemplate[point].dW);
+
+ }
+ compoInitialised=true;
+}
+
+void vpTemplateTrackerSSDForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompo(I);
+}
+
+void vpTemplateTrackerSSDForwardCompositional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ if(!compoInitialised)
+ std::cout<<"Compositionnal tracking no initialised\nUse InitCompo(vpImage<unsigned char> &I) function"<<std::endl;
+ double erreur=0;
+ unsigned int Nbpoint=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ dW=0;
+
+ double lambda=lambdaDep;
+ double IW,dIWx,dIWy;
+ double Tij;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ double alpha=2.;
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ G=0;
+ H=0 ;
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ dIWx=dIx.getValue(i2,j2);
+ dIWy=dIy.getValue(i2,j2);
+ Nbpoint++;
+ //Calcul du Hessien
+ /*Warp->dWarp(X1,X2,p,dW);
+ double *tempt=new double[nbParam];
+ for(int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;*/
+
+ Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ H[it][jt]+=tempt[it]*tempt[jt];
+
+ double er=(Tij-IW);
+ for(unsigned int it=0;it<nbParam;it++)
+ G[it]+=er*tempt[it];
+
+ erreur+=(er*er);
+ delete[] tempt;
+ }
+
+
+ }
+ if(Nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+
+ vpMatrix::computeHLM(H,lambda,HLM);
+
+ try
+ {
+ dp=1.*HLM.inverseByLU()*G;
+ }
+ catch(vpException &e)
+ {
+ //std::cout<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+
+ dp=gain*dp;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
+ dp=alpha*dp;
+ }
+ Warp->pRondp(p,dp,p);
+ //p+=Gain*dp;
+ iteration++;
+ }
+ while( /*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax));
+
+ nbIteration=iteration;
+}
+
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
new file mode 100644
index 0000000..07e282f
--- /dev/null
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
+#include <visp3/core/vpImageTools.h>
+
+vpTemplateTrackerSSDInverseCompositional::vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerSSD(warp), compoInitialised(false), HInv(), HCompInverse(), useTemplateSelect(false),
+ evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8)
+{
+ useInverse=true;
+ HInv.resize(nbParam,nbParam);
+ HCompInverse.resize(nbParam,nbParam);
+}
+
+void vpTemplateTrackerSSDInverseCompositional::initCompInverse(const vpImage<unsigned char> &/*I*/)
+{
+
+ H=0;
+ int i,j;
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ if((!useTemplateSelect)||(ptTemplateSelect[point]))
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ ptTemplate[point].dW=new double[nbParam];
+
+ Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ H[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
+ }
+
+ }
+ HInv=H;
+ vpMatrix HLMtemp(nbParam,nbParam);
+ vpMatrix::computeHLM(H,lambdaDep,HLMtemp);
+
+ HCompInverse.resize(nbParam,nbParam);
+ HCompInverse=HLMtemp.inverseByLU();
+ //std::cout<<Hinverse<<std::endl;
+ vpColVector dWtemp(nbParam);
+ vpColVector HiGtemp(nbParam);
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ if((!useTemplateSelect)||(ptTemplateSelect[point]))
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ for(unsigned int it=0;it<nbParam;it++)
+ dWtemp[it]=ptTemplate[point].dW[it];
+
+ HiGtemp = -1.*HCompInverse*dWtemp;
+ ptTemplate[point].HiG=new double[nbParam];
+
+ for(unsigned int it=0;it<nbParam;it++)
+ ptTemplate[point].HiG[it]=HiGtemp[it];
+ }
+ }
+ compoInitialised=true;
+}
+
+void vpTemplateTrackerSSDInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompInverse(I);
+}
+
+void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ double erreur=0;
+ unsigned int Nbpoint=0;
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+
+ vpColVector dpinv(nbParam);
+ double IW;
+ double Tij;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ double alpha=2.;
+ //vpTemplateTrackerPointtest *pt;
+ initPosEvalRMS(p);
+
+ vpTemplateTrackerPoint *pt;
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ dp=0;
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ if((!useTemplateSelect)||(ptTemplateSelect[point]))
+ {
+ //pt=&ptTemplatetest[point];
+ pt=&ptTemplate[point];
+ i=pt->y;
+ j=pt->x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=pt->val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ Nbpoint++;
+ double er=(Tij-IW);
+ for(unsigned int it=0;it<nbParam;it++)
+ dp[it]+=er*pt->HiG[it];
+
+ erreur+=er*er;
+ }
+ }
+ }
+ //std::cout << "npoint: " << Nbpoint << std::endl;
+ if(Nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ deletePosEvalRMS();
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+ dp=gain*dp;
+ //std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
+ dp=alpha*dp;
+ }
+ Warp->getParamInverse(dp,dpinv);
+ Warp->pRondp(p,dpinv,p);
+ iteration++;
+
+ computeEvalRMS(p);
+ //std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
+ //std::cout << "evolRMS: " << evolRMS << " threshold: " << threshold_RMS << std::endl;
+ }
+ while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));
+
+ nbIteration=iteration;
+ deletePosEvalRMS();
+}
+
+void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS(const vpColVector &p_)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+ x_pos.resize(nb_corners);
+ y_pos.resize(nb_corners);
+
+ Warp->computeCoeff(p_);
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,p_);
+ Warp->warpX(X1,X2,p_);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+}
+
+void vpTemplateTrackerSSDInverseCompositional::computeEvalRMS(const vpColVector &p_)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+
+ Warp->computeCoeff(p_);
+ evolRMS=0;
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,p_);
+ Warp->warpX(X1,X2,p_);
+ evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+ evolRMS=evolRMS/nb_corners;
+}
+
+void vpTemplateTrackerSSDInverseCompositional::deletePosEvalRMS()
+{
+}
diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp
new file mode 100644
index 0000000..eb47127
--- /dev/null
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerBSpline.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage<double> &I, double r, double t)
+{
+ double res=0;
+ int cr=(int)(r);
+ int ct=(int)(t);
+ double er=(double)r-cr;
+ double et=(double)t-ct;
+ int height=(int)I.getHeight();//r
+ int width=(int)I.getWidth();//t
+ volatile int tr,tt;
+ for(int ir=-1;ir<=2;ir++)
+ {
+ tr=ir+cr;
+ for(int it=-1;it<=2;it++)
+ {
+ tt=it+ct;
+ if(tr>=0 && tr <height && tt>=0 && tt <width)
+ res+=Bspline4((double)ir-er)*Bspline4((double)it-et)*I[tr][tt];
+ }
+ }
+ return res;
+}
+
+double vpTemplateTrackerBSpline::Bspline4(double diff)
+{
+ //double result;
+ double aDiff=vpMath::abs(diff);
+ if(aDiff<1.)
+ return (aDiff*aDiff*aDiff/2.-aDiff*aDiff+4./6.);
+ //return (0.5*(1.-aDiff)*(1.-aDiff)*(1.-aDiff)+0.5*(1.-aDiff)*(1.-aDiff)-0.5*(1.-aDiff)+1./6.);
+ else if(aDiff<2.)
+ return ((2.-aDiff)*(2.-aDiff)*(2.-aDiff)/6.);
+ else
+ return 0;
+}
+
+#endif
diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp
new file mode 100644
index 0000000..ec59ef0
--- /dev/null
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp
@@ -0,0 +1,435 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerTriangle.h>
+
+/*!
+ Default constructor.
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle()
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+}
+
+/*!
+ Copy constructor.
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+ *this = T;
+}
+
+/*!
+ Copy operator.
+ */
+vpTemplateTrackerTriangle & vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle& T)
+{
+ minx_temp=T.minx_temp;
+ miny_temp=T.miny_temp;
+
+ l_t=T.l_t;
+ h_t=T.h_t;
+ C1.x=T.C1.x;
+ C1.y=T.C1.y;
+ C2.x=T.C2.x;
+ C2.y=T.C2.y;
+ C3.x=T.C3.x;
+ C3.y=T.C3.y;
+ //uvinv.resize(2,2);
+ //uvinv=T.uvinv;
+ //p_ds_uv.resize(2);
+ //p_ds_uv=T.p_ds_uv;
+ //ptempo.resize(2);
+ //ptempo=T.ptempo;
+ not_good=T.not_good;
+
+ uvinv00=T.uvinv00;
+ uvinv01=T.uvinv01;
+ uvinv10=T.uvinv10;
+ uvinv11=T.uvinv11;
+
+ marge_triangle = T.marge_triangle;
+ area = T.area;
+
+ return (*this);
+}
+
+/*!
+ Create a triangle from 3 corners.
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+
+ The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpColVector &c1,
+ const vpColVector &c2,
+ const vpColVector &c3)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+ init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
+}
+/*!
+ Return a triangle with coordinates that are down scaled by a factor 2.
+ */
+vpTemplateTrackerTriangle vpTemplateTrackerTriangle::getPyramidDown() const
+{
+ vpTemplateTrackerTriangle Ttemp;
+ Ttemp.init(C1.x/2.,C1.y/2.,C2.x/2.,C2.y/2.,C3.x/2.,C3.y/2.);
+ return Ttemp;
+}
+
+/*!
+ Create a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ - x coordinate is along the columns
+ - y coordinate is along the rows.
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1,int y1, int x2,int y2, int x3,int y3)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+ init(x1,y1,x2,y2,x3,y3);
+}
+
+/*!
+ Create a triangle from 3 corners defined as image points.
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+ init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
+}
+
+/*!
+ Create a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ - x coordinate is along the columns
+ - y coordinate is along the rows.
+ */
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1,double y1, double x2,double y2, double x3,double y3)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
+ uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+{
+ init(x1,y1,x2,y2,x3,y3);
+}
+/*!
+ Initializes a triangle from 3 corners.
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+
+ The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
+ */
+void vpTemplateTrackerTriangle::init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3)
+{
+ init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
+}
+/*!
+ Initializes a triangle from 3 corners defined as image points.
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+ */
+void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
+{
+ init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
+}
+
+/*!
+ Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ - x coordinate is along the columns
+ - y coordinate is along the rows.
+ */
+void vpTemplateTrackerTriangle::init(int x1, int y1, int x2,int y2, int x3, int y3)
+{
+ init((double)x1,(double)y1,(double)x2,(double)y2,(double)x3,(double)y3);
+}
+
+/*!
+ Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ - x coordinate is along the columns
+ - y coordinate is along the rows.
+ */
+void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2, double x3, double y3)
+{
+ C1.x=x1;C1.y=y1;
+ C2.x=x2;C2.y=y2;
+ C3.x=x3;C3.y=y3;
+
+ double minx,miny,maxx,maxy;
+ //calcul du rectangle minimal contenant le triangle seletionne
+ minx=(x1<x2)?x1:x2;
+ miny=(y1<y2)?y1:y2;
+ minx=(minx<x3)?minx:x3;
+ miny=(miny<y3)?miny:y3;
+ maxx=(x1>x2)?x1:x2;
+ maxy=(y1>y2)?y1:y2;
+ maxx=(maxx>x3)?maxx:x3;
+ maxy=(maxy>y3)?maxy:y3;
+
+ vpColVector u;
+ vpColVector v;
+ u.resize(2);
+ v.resize(2);
+ vpMatrix uv(2,2);
+ vpMatrix uvinv(2,2);
+
+ u[0]=C2.x-C1.x;
+ u[1]=C2.y-C1.y;
+
+ v[0]=C3.x-C1.x;
+ v[1]=C3.y-C1.y;
+
+ uv[0][0]=u[0];uv[1][0]=v[0];
+ uv[0][1]=u[1];uv[1][1]=v[1];
+ try
+ {
+ uvinv=uv.inverseByLU();
+ not_good=false;
+ }
+ catch(...)
+ {
+ not_good=true;
+ std::cout<<"Triangle vide"<<std::endl;
+
+ }
+ uvinv00=uvinv[0][0];
+ uvinv01=uvinv[0][1];
+ uvinv10=uvinv[1][0];
+ uvinv11=uvinv[1][1];
+
+ l_t=maxx-minx;
+ h_t=maxy-miny;
+ minx_temp=minx;
+ miny_temp=miny;
+
+ marge_triangle = 0.00001;
+ area = 0.5 * fabs(uv.det());
+}
+
+//marge ajoutee a zone pour que sommet soit pris en compte
+
+/*!
+ Indicates if a point with coordinates (i,j) is in the triangle.
+ \param i : Coordinate along the rows.
+ \param j : Coordinate along the columns.
+ */
+bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
+{
+ if(not_good)
+ return false;
+
+ /*ptempo[0]=j-C1.x;
+ ptempo[1]=i-C1.y;
+
+ p_ds_uv=ptempo*uvinv;
+ return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
+
+ double ptempo0=j-C1.x;
+ double ptempo1=i-C1.y;
+ double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
+ double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
+ return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
+}
+
+/*!
+ Indicates if a point with coordinates (i,j) is in the triangle.
+ \param i : Coordinate along the rows.
+ \param j : Coordinate along the columns.
+ */
+bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
+{
+ if(not_good)
+ return false;
+ /*ptempo[0]=j-C1.x;
+ ptempo[1]=i-C1.y;
+
+ p_ds_uv=ptempo*uvinv;
+ return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
+ double ptempo0=j-C1.x;
+ double ptempo1=i-C1.y;
+ double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
+ double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
+ return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
+}
+
+/*!
+ Indicates if an image point is in the triangle.
+ \param ip : Image point to consider.
+ */
+bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const
+{
+ return inTriangle(ip.get_i(), ip.get_j());
+}
+/*!
+ Returns the coordinates of the triangle corners as an image point.
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+ */
+void vpTemplateTrackerTriangle::getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const
+{
+ c1.set_uv(C1.x, C1.y);
+ c2.set_uv(C2.x, C2.y);
+ c3.set_uv(C3.x, C3.y);
+}
+
+/*!
+ Returns the coordinates of the triangle corners as a 3 dimension vector of image points.
+ \param c : 3 dimension vector of image points that correspond to the triangle corners.
+ */
+void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
+{
+ c.resize(3);
+ c[0].set_uv(C1.x, C1.y);
+ c[1].set_uv(C2.x, C2.y);
+ c[2].set_uv(C3.x, C3.y);
+}
+
+/*!
+ Returns the coordinates of the triangle corners as a 2 dimension vector (x,y).
+ \param c1 : First corner.
+ \param c2 : Second corner.
+ \param c3 : Third corner.
+ */
+void vpTemplateTrackerTriangle::getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const
+{
+ c1=getCorner1();
+ c2=getCorner2();
+ c3=getCorner3();
+}
+
+/*!
+ Returns the coordinates of the triangle first corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ */
+vpColVector vpTemplateTrackerTriangle::getCorner1() const
+{
+ vpColVector c(2);
+ c[0]=C1.x;
+ c[1]=C1.y;
+
+ return c;
+}
+/*!
+ Returns the coordinates of the triangle second corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ */
+vpColVector vpTemplateTrackerTriangle::getCorner2() const
+{
+ vpColVector c(2);
+ c[0]=C2.x;
+ c[1]=C2.y;
+ return c;
+}
+
+/*!
+ Returns the coordinates of the triangle third corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ */
+vpColVector vpTemplateTrackerTriangle::getCorner3() const
+{
+ vpColVector c(2);
+ c[0]=C3.x;
+ c[1]=C3.y;
+ return c;
+}
+
+/*!
+ Get the size of the triangle bounding box.
+ \param w : Bounding box width.
+ \param h : Bounding box height.
+ */
+void vpTemplateTrackerTriangle::getSize(double &w,double &h) const
+{
+ w=l_t;
+ h=h_t;
+}
+/*!
+ Get the size of the triangle bounding box.
+ \param w : Bounding box width.
+ \param h : Bounding box height.
+ */
+void vpTemplateTrackerTriangle::getSize(int &w,int &h) const
+{
+ w=(int)l_t+1;
+ h=(int)h_t+1;
+}
+
+/*!
+ \return The minimal x coordinate (along the columns of the image) of the points that are in the triangle.
+ \sa getMaxx()
+ */
+double vpTemplateTrackerTriangle::getMinx() const
+{
+ return minx_temp-1;
+}
+/*!
+ \return The minimal y coordinate (along the rows of the image) of the points that are in the triangle.
+ \sa getMaxy()
+ */
+double vpTemplateTrackerTriangle::getMiny() const
+{
+ return miny_temp-1;
+}
+/*!
+ \return The maximal x coordinate (along the columns of the image) of the points that are in the triangle.
+ \sa getMinx()
+ */
+double vpTemplateTrackerTriangle::getMaxx() const
+{
+ return minx_temp+l_t+1;
+}
+/*!
+ \return The maximal y coordinate (along the rows of the image) of the points that are in the triangle.
+ \sa getMaxx()
+ */
+double vpTemplateTrackerTriangle::getMaxy() const
+{
+ return miny_temp+h_t+1;
+}
+
+
+
diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp
new file mode 100644
index 0000000..6104472
--- /dev/null
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp
@@ -0,0 +1,616 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <limits> // numeric_limits
+
+#include <visp3/core/vpConfig.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020300
+#include <opencv2/imgproc/imgproc.hpp>
+#endif
+
+#include <visp3/tt/vpTemplateTrackerZone.h>
+
+
+/*!
+ Default constructor.
+ */
+vpTemplateTrackerZone::vpTemplateTrackerZone()
+ : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
+{
+}
+
+/*!
+ Copy constructor.
+ */
+vpTemplateTrackerZone::vpTemplateTrackerZone(const vpTemplateTrackerZone &z)
+ : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
+{
+ *this = z;
+}
+
+/*!
+ Remove all the triangles that define the zone.
+ */
+void vpTemplateTrackerZone::clear()
+{
+ min_x=-1;
+ min_y=-1;
+ max_x=-1;
+ max_y=-1;
+
+ Zone.clear();
+}
+
+/*!
+ Copy operator.
+ */
+vpTemplateTrackerZone & vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z)
+{
+ clear();
+
+ this->copy(z);
+ return (*this);
+}
+
+/*!
+ Initialize a zone in image \e I using mouse click.
+
+ \param I : Image used to select the zone.
+ \param delaunay : Flag used to enable Delaunay triangulation.
+ - If true, from the image points selected by the user, a Delaunay triangulation is performed
+ to initialize the zone.
+ - A left click select a image point;
+ - A right click select the last image point and ends the initialisation stage.
+ In that case at least 3 points need to be selected by the user.
+ - If false, the user select directly points as successive triangle corners.
+ Three successive points define a triangle. It is not mandatory
+ that triangles have one edge in common; they can define a discontinued area.
+ - A left click select a triangle corner;
+ - A right click select the last triangle corner and ends the initialisation stage.
+ The number of points that are selected by the user should be a multiple of 3.
+ For example, to select a zone as two triangles, the user has to left click
+ five times and finish the selection on the sixth corner with a right click.
+
+ */
+void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool delaunay)
+{
+ Zone.clear();
+
+ std::vector<vpImagePoint> vip;
+
+ bool end = false;
+
+ do {
+ vpImagePoint p;
+ vpMouseButton::vpMouseButtonType button;
+ if (vpDisplay::getClick(I, p, button, false) ) {
+ vip.push_back(p);
+
+ vpDisplay::displayCross(I, p, 7, vpColor::red);
+
+ if (vip.size() > 1) {
+ if (delaunay) {
+ // Draw a line between the 2 last points
+ vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
+ }
+ else {
+ if(vip.size() % 3 ==2)
+ // draw line between point 2-1
+ vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
+ else if(vip.size() % 3 ==0) {
+ // draw line between point 3-2
+ vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
+ // draw line between point 3-1
+ vpDisplay::displayLine(I, p, vip[vip.size()-3], vpColor::blue, 3);
+ }
+
+ }
+ }
+
+ if (button == vpMouseButton::button3)
+ end = true;
+ }
+
+ vpTime::wait(20);
+ vpDisplay::flush(I);
+ } while(!end);
+
+ initFromPoints(I, vip, delaunay);
+}
+
+/*!
+
+ Initialize the zone using a vector of image points.
+
+ \param I : Image to process.
+ \param vip : Vector of image points used as initialization.
+ \param delaunay :
+ - If true, a Delaunay triangulation is perfomed on the vector of image points. This functionality
+ is only available if ViSP is build with OpenCV >2.3 third-party.
+ - If false, the vector of image points describe triangles. Its size is then a multiple of 3.
+ */
+void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &vip, bool delaunay)
+{
+ if (delaunay) {
+ if(vip.size() == 3) {
+ initFromPoints(I, vip, false);
+ }
+ else if(vip.size() == 4) {
+ std::vector<vpImagePoint> vip_delaunay;
+ vip_delaunay.push_back(vip[0]);
+ vip_delaunay.push_back(vip[1]);
+ vip_delaunay.push_back(vip[2]);
+ vip_delaunay.push_back(vip[2]);
+ vip_delaunay.push_back(vip[3]);
+ vip_delaunay.push_back(vip[0]);
+ initFromPoints(I, vip_delaunay, false);
+ }
+ else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020300
+ // Init Delaunay
+ cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
+ for(size_t i=0; i< vip.size(); i++) {
+ cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
+ //std::cout << "Click point: " << vip[i] << std::endl;
+ subdiv.insert(fp);
+ }
+
+ // Compute Delaunay triangulation
+ std::vector<cv::Vec6f> triangleList;
+ subdiv.getTriangleList(triangleList);
+
+ // Keep only the Delaunay points that are inside the area
+ vpRect rect(0, 0, I.getWidth(), I.getHeight());
+
+ std::vector<vpImagePoint> vip_delaunay;
+ for( size_t i = 0; i < triangleList.size(); i++ ) {
+ cv::Vec6f t = triangleList[i];
+ std::vector<vpImagePoint> p(3);
+
+ p[0].set_uv(t[0], t[1]);
+ p[1].set_uv(t[2], t[3]);
+ p[2].set_uv(t[4], t[5]);
+
+ if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
+ vip_delaunay.push_back(p[0]);
+ vip_delaunay.push_back(p[1]);
+ vip_delaunay.push_back(p[2]);
+ }
+ }
+
+ initFromPoints(I, vip_delaunay, false);
+#else
+ throw vpException(vpException::functionNotImplementedError,"Delaunay triangulation is not available!");
+#endif
+ }
+ }
+ else {
+ Zone.clear();
+ for(unsigned int i=0; i<vip.size(); i+=3) {
+ vpTemplateTrackerTriangle triangle(vip[i], vip[i+1], vip[i+2]);
+ add(triangle);
+
+// vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green, 1);
+// vpDisplay::displayLine(I, vip[i+1], vip[i+2], vpColor::green, 1);
+// vpDisplay::displayLine(I, vip[i+2], vip[i], vpColor::green,1);
+// vpDisplay::flush(I) ;
+
+ // Update the bounding box
+ if((triangle.getMinx()<min_x)||(min_x==-1))
+ min_x=(int)triangle.getMinx();
+ if((triangle.getMaxx()>max_x)||(max_x==-1))
+ max_x=(int)triangle.getMaxx();
+ if((triangle.getMiny()<min_y)||(min_y==-1))
+ min_y=(int)triangle.getMiny();
+ if((triangle.getMaxy()>max_y)||(max_y==-1))
+ max_y=(int)triangle.getMaxy();
+ }
+ }
+}
+
+/*!
+ Add a triangle to the zone and update the bounding box.
+ \param t : Triangle to introduce in the zone.
+ */
+void vpTemplateTrackerZone::add(const vpTemplateTrackerTriangle &t)
+{
+ Zone.push_back(t);
+
+ // Update the bounding box
+ if((t.getMinx()<min_x)||(min_x==-1))
+ min_x=(int)t.getMinx();
+ if((t.getMaxx()>max_x)||(max_x==-1))
+ max_x=(int)t.getMaxx();
+ if((t.getMiny()<min_y)||(min_y==-1))
+ min_y=(int)t.getMiny();
+ if((t.getMaxy()>max_y)||(max_y==-1))
+ max_y=(int)t.getMaxy();
+}
+
+/*!
+ Test if a pixel with coordinates (i,j) is in the zone..
+ \param i, j : Coordinates of the pixel to test.
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ */
+bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
+{
+ std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
+ for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
+ {
+ if(Iterateurvecteur->inTriangle(i,j))
+ return true;
+ }
+ return false;
+}
+
+/*!
+ Test if a pixel with coordinates (i,j) is in the zone..
+ \param i, j : Coordinates of the pixel to test.
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ */
+bool vpTemplateTrackerZone::inZone(const double &i,const double &j) const
+{
+ std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
+ for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
+ {
+ if(Iterateurvecteur->inTriangle(i,j))
+ return true;
+ }
+ return false;
+}
+
+/*!
+ Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
+ the triangle that contains the pixel.
+ \param i, j : Coordinates of the pixel to test.
+ \param id_triangle : Index of the triangle that contains the pixel (i,j).
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ */
+bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_triangle) const
+{
+ unsigned int id=0;
+ std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
+ for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
+ {
+ if(Iterateurvecteur->inTriangle(i,j))
+ {
+ id_triangle=id;
+ return true;
+ }
+ id++;
+ }
+ return false;
+}
+
+/*!
+ Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
+ the triangle that contains the pixel.
+ \param i, j : Coordinates of the pixel to test.
+ \param id_triangle : Index of the triangle that contains the pixel (i,j).
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ */
+bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int &id_triangle) const
+{
+ unsigned int id=0;
+ std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
+ for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
+ {
+ if(Iterateurvecteur->inTriangle(i,j))
+ {
+ id_triangle=id;
+ return true;
+ }
+ id++;
+ }
+ return false;
+}
+
+/*!
+ A zone is defined by a set of triangles. This function returns the ith triangle.
+ \param i : Index of the triangle to return.
+ \param T : The triangle corresponding to index i.
+ \return true if the triangle with index i was found, false otherwise.
+
+ The following sample code shows how to use this function:
+ \code
+ vpTemplateTrackerZone zone;
+ ...
+ for (unsigned int i=0; i < zone.getNbTriangle(); i++) {
+ vpTemplateTrackerTriangle triangle;
+ zone.getTriangle(i, triangle);
+ }
+ \endcode
+ */
+void vpTemplateTrackerZone::getTriangle(unsigned int i,vpTemplateTrackerTriangle &T) const
+{
+ if (i > getNbTriangle()-1)
+ throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
+
+ T = Zone[i];
+}
+/*!
+ A zone is defined by a set of triangles. This function returns the ith triangle.
+ \param i : Index of the triangle to return.
+ \return The triangle corresponding to index i.
+
+ The following sample code shows how to use this function:
+ \code
+ vpTemplateTrackerZone zone;
+ ...
+ for (unsigned int i=0; i < zone.getNbTriangle(); i++) {
+ vpTemplateTrackerTriangle triangle = zone.getTriangle(i);
+ }
+ \endcode
+ */
+vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) const
+{
+ if (i > getNbTriangle()-1)
+ throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
+
+ return Zone[i];
+}
+/*!
+ Return the position of the center of gravity of the zone.
+ \exception vpException::divideByZeroError: The size of the zone is null.
+ */
+vpImagePoint vpTemplateTrackerZone::getCenter() const
+{
+ double xc=0;
+ double yc=0;
+ int cpt=0;
+ for(int i=min_y;i<max_y;i++)
+ for(int j=min_x;j<max_x;j++)
+ if(inZone(i,j))
+ {
+ xc+=j;
+ yc+=i;
+ cpt ++;
+ }
+ if(! cpt) {
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot compute the zone center: size = 0")) ;
+ }
+ xc=xc/cpt;
+ yc=yc/cpt;
+ vpImagePoint ip;
+ ip.set_uv(xc, yc);
+ return ip;
+}
+
+/*!
+ \return The maximal x coordinate (along the columns of the image) of the points that are in the zone.
+ \sa getMinx(), getBoundingBox()
+ */
+int vpTemplateTrackerZone::getMaxx() const
+{
+ return max_x;
+}
+/*!
+ \return The maximal y coordinate (along the rows of the image) of the points that are in the zone.
+ \sa getMiny(), getBoundingBox()
+ */
+int vpTemplateTrackerZone::getMaxy() const
+{
+ return max_y;
+}
+/*!
+ \return The minimal x coordinate (along the columns of the image) of the points that are in the zone.
+ \sa getMaxx(), getBoundingBox()
+ */
+int vpTemplateTrackerZone::getMinx() const
+{
+ return min_x;
+}
+/*!
+ \return The minimal y coordinate (along the rows of the image) of the points that are in the zone.
+ \sa getMaxy(), getBoundingBox()
+ */
+int vpTemplateTrackerZone::getMiny() const
+{
+ return min_y;
+}
+
+/*!
+ Return a rectangle that defines the bounding box of the zone.
+ \sa getMinx(), getMiny(), getMaxx(), getMaxy()
+ */
+vpRect vpTemplateTrackerZone::getBoundingBox() const
+{
+ vpRect bbox;
+ bbox.setTopLeft(vpImagePoint(min_y, min_x));
+ bbox.setBottomRight(vpImagePoint(max_y, max_x));
+ return bbox;
+}
+
+/*!
+ If a display device is associated to image \c I, display in overlay the triangles that define the zone.
+ \param I : Image.
+ \param col : Color used to display the triangles.
+ \param thickness : Thickness of the triangle lines.
+ */
+void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
+{
+ std::vector<vpImagePoint> ip;
+ for (unsigned int i=0; i < Zone.size(); i++) {
+ vpTemplateTrackerTriangle triangle;
+ Zone[i].getCorners(ip);
+ vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
+ vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
+ vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
+ }
+}
+
+/*!
+ If a display device is associated to image \c I, display in overlay the triangles that define the zone.
+ \param I : Image.
+ \param col : Color used to display the triangles.
+ \param thickness : Thickness of the triangle lines.
+ */
+void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
+{
+ std::vector<vpImagePoint> ip;
+ for (unsigned int i=0; i < Zone.size(); i++) {
+ vpTemplateTrackerTriangle triangle;
+ Zone[i].getCorners(ip);
+ vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
+ vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
+ vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
+ }
+}
+
+/*!
+ Destructor.
+ */
+vpTemplateTrackerZone::~vpTemplateTrackerZone()
+{
+ clear();
+}
+
+/*!
+ Modify all the pixels inside a triangle with a given gray level.
+ \param I: Output image.
+ \param id: Triangle id. This value should be less than the number
+ of triangles used to define the zone and available using getNbTriangle().
+ \param gray_level: Color used to fill the triangle with.
+ */
+void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char>& I, unsigned int id,unsigned char gray_level)
+{
+ assert(id < getNbTriangle());
+ vpTemplateTrackerTriangle triangle;
+ getTriangle(id, triangle);
+ for (int i=0 ; i < (int) I.getHeight() ; i++)
+ {
+ for (int j=0 ; j < (int) I.getWidth() ; j++)
+ {
+ if(triangle.inTriangle(i,j))
+ {
+ I[i][j]=gray_level;
+ }
+ }
+ }
+}
+
+/*!
+ Return a zone with triangles that are down scaled by a factor 2.
+ */
+vpTemplateTrackerZone vpTemplateTrackerZone::getPyramidDown() const
+{
+ vpTemplateTrackerZone tempZone;
+ vpTemplateTrackerTriangle Ttemp;
+ vpTemplateTrackerTriangle TtempDown;
+ for(unsigned int i=0;i<getNbTriangle();i++)
+ {
+ getTriangle(i,Ttemp);
+ TtempDown=Ttemp.getPyramidDown();
+ tempZone.add(TtempDown);
+ }
+ return tempZone;
+}
+
+/*!
+ Copy all the triangles that define zone \c Z in the current zone (*this) and
+ update the zone bounding box.
+ \param z : Zone with a set of triangles provided as input.
+ */
+void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone& z)
+{
+ vpTemplateTrackerTriangle triangle;
+ for(unsigned int i=0;i<z.getNbTriangle();i++)
+ {
+ z.getTriangle(i, triangle);
+ add(triangle);
+ // Update the bounding box
+ if((triangle.getMinx()<min_x)||(min_x==-1))
+ min_x=(int)triangle.getMinx();
+ if((triangle.getMaxx()>max_x)||(max_x==-1))
+ max_x=(int)triangle.getMaxx();
+ if((triangle.getMiny()<min_y)||(min_y==-1))
+ min_y=(int)triangle.getMiny();
+ if((triangle.getMaxy()>max_y)||(max_y==-1))
+ max_y=(int)triangle.getMaxy();
+ }
+}
+
+/*!
+ Return the position of the center of gravity in a given area.
+ \param borne_x : Right coordinate of the area to consider.
+ \param borne_y : Bottom coordinate of the area to consider.
+ \exception vpException::divideByZeroError: The size of the zone is null.
+ */
+
+vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
+{
+ int cpt_pt=0;
+ double x_center=0,y_center=0;
+ for(int j=0;j<borne_x;j++)
+ for(int i=0;i<borne_y;i++)
+ if(inZone(i,j))
+ {
+ x_center+=j;
+ y_center+=i;
+ cpt_pt++;
+ }
+
+ if(! cpt_pt) {
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot compute the zone center: size = 0")) ;
+ }
+
+ x_center=x_center/cpt_pt;
+ y_center=y_center/cpt_pt;
+ vpImagePoint center;
+ center.set_uv(x_center, y_center);
+ return center;
+}
+
+/*!
+ Return the area of the template zone.
+ */
+double vpTemplateTrackerZone::getArea() const
+{
+ double area = 0;
+ vpTemplateTrackerTriangle triangle;
+ for (unsigned int i=0; i < getNbTriangle(); i++) {
+ getTriangle(i, triangle);
+ area += triangle.getArea();
+ }
+ return area;
+}
+
diff --git a/modules/tracker/tt/src/vpTemplateTracker.cpp b/modules/tracker/tt/src/vpTemplateTracker.cpp
new file mode 100644
index 0000000..ebe2c3f
--- /dev/null
+++ b/modules/tracker/tt/src/vpTemplateTracker.cpp
@@ -0,0 +1,978 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerBSpline.h>
+
+vpTemplateTracker::vpTemplateTracker(vpTemplateTrackerWarp *_warp)
+ : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
+ ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
+ ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false),
+ templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
+ ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
+ pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(),
+ HLMdesireInverse(), HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40),
+ costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
+ taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0),
+ lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0),
+ useCompositionnal(true), useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(),
+ dW(), BI(), dIx(), dIy(), zoneRef_()
+{
+ nbParam = Warp->getNbParam() ;
+ p.resize(nbParam);
+ dp.resize(nbParam);
+
+ fgG=new double[(taillef+1)/2] ;
+ vpImageFilter::getGaussianKernel(fgG, taillef) ;
+
+ fgdG=new double[(taillef+1)/2] ;
+ vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
+}
+
+void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill)
+{
+ taillef=new_taill;
+ if (fgG) delete[] fgG;
+ fgG=new double[taillef] ;
+ vpImageFilter::getGaussianKernel(fgG, taillef) ;
+
+ if (fgdG) delete[] fgdG;
+ fgdG=new double[taillef] ;
+ vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
+}
+
+void vpTemplateTracker::initTracking(const vpImage<unsigned char>& I, vpTemplateTrackerZone &zone)
+{
+ // std::cout<<"\tInitialise reference..."<<std::endl;
+ zoneTracked=&zone;
+
+ int largeur_im=(int)I.getWidth();
+ int hauteur_im=(int)I.getHeight();
+
+ unsigned int NbPointDsZone=0;
+ //double xtotal=0,ytotal=0;
+ int mod_fi,mod_fj;
+ mod_fi=mod_i;mod_fj=mod_i;
+
+ for(int i=0;i<hauteur_im;i+=mod_fi) {
+ for(int j=0;j<largeur_im;j+=mod_fj) {
+ if(zone.inZone(i,j)) {
+ NbPointDsZone++;
+ //xtotal+=j;
+ //ytotal+=i;
+ }
+ }
+ }
+
+ //Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
+
+ templateSize=NbPointDsZone;
+ ptTemplate = new vpTemplateTrackerPoint[templateSize];ptTemplateInit=true;
+ ptTemplateSelect = new bool[templateSize];ptTemplateSelectInit=true;
+
+ Hdesire.resize(nbParam,nbParam);
+ HLMdesire.resize(nbParam,nbParam);
+
+ vpTemplateTrackerPoint pt;
+ //vpTemplateTrackerZPoint ptZ;
+ vpImage<double> GaussI ;
+ vpImageFilter::filter(I, GaussI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ unsigned int cpt_point=0;
+ templateSelectSize=0;
+ for(int i=0;i<hauteur_im;i+=mod_i)
+ {
+ //for(int j=minx_t;j<maxx_t;j++)
+ for(int j=0;j<largeur_im;j+=mod_j)
+ {
+// if(i%mod_i ==0 && j%mod_j ==0)
+ if(zone.inZone(i,j))
+ {
+ pt.x=j;
+ pt.y=i;
+
+ pt.dx=dIx[i][j];
+ pt.dy=dIy[i][j];
+
+ if(pt.dx*pt.dx+pt.dy*pt.dy>thresholdGradient)
+ {
+ ptTemplateSelect[cpt_point]=true;
+ templateSelectSize++;
+ }
+ else
+ ptTemplateSelect[cpt_point]=false;
+ //ptTemplate_select[cpt_point]=true;
+
+ /*if(blur)
+ pt.val=GaussI[i][j];
+ else
+ pt.val=I[i][j];*/
+ pt.val=vpTemplateTrackerBSpline::getSubPixBspline4(GaussI,i,j);
+ //ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
+
+ ptTemplate[cpt_point]=pt;
+ cpt_point++;
+ }
+ }
+ }
+
+ // std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
+ // std::cout<<"utilisation de "<<taille_template_select<<"/"<<cpt_point<<" = "<<100.*taille_template_select/cpt_point<<"% pour calcul derivees"<<std::endl;
+
+ templateSize=cpt_point;
+ GaussI.destroy();
+ // std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
+}
+
+vpTemplateTracker::~vpTemplateTracker()
+{
+ // vpTRACE("destruction tracker");
+ delete[] fgG;
+ delete[] fgdG;
+
+ resetTracker();
+}
+
+/*!
+ Reset the tracker by freeing the memory allocated by the template tracker during the initialization.
+ */
+void vpTemplateTracker::resetTracker()
+{
+ // reset the tracker parameters
+ p = 0;
+
+ // vpTRACE("resetTracking");
+ if(pyrInitialised)
+ {
+ if(ptTemplatePyr){
+ for(unsigned int i=0;i<nbLvlPyr;i++)
+ {
+ if(ptTemplatePyr[i]){
+ for(unsigned int point=0;point<templateSizePyr[i];point++)
+ {
+ delete[] ptTemplatePyr[i][point].dW;
+ delete[] ptTemplatePyr[i][point].HiG;
+ }
+ delete[] ptTemplatePyr[i];
+ }
+ }
+ delete[] ptTemplatePyr;
+ ptTemplatePyr = NULL;
+ }
+
+ if (ptTemplateCompoPyr) {
+ for(unsigned int i=0;i<nbLvlPyr;i++)
+ {
+ if (ptTemplateCompoPyr[i]) {
+ for(unsigned int point=0;point<templateSizePyr[i];point++) {
+ delete[] ptTemplateCompoPyr[i][point].dW;
+ }
+ delete[] ptTemplateCompoPyr[i];
+ }
+ }
+ delete[] ptTemplateCompoPyr;
+ ptTemplateCompoPyr = NULL;
+ }
+
+ if (ptTemplateSuppPyr) {
+ for(unsigned int i=0;i<nbLvlPyr;i++)
+ {
+ if (ptTemplateSuppPyr[i]) {
+ for(unsigned int point=0;point<templateSizePyr[i];point++) {
+ delete[] ptTemplateSuppPyr[i][point].Bt;
+ delete[] ptTemplateSuppPyr[i][point].BtInit;
+ delete[] ptTemplateSuppPyr[i][point].dBt;
+ delete[] ptTemplateSuppPyr[i][point].d2W;
+ delete[] ptTemplateSuppPyr[i][point].d2Wx;
+ delete[] ptTemplateSuppPyr[i][point].d2Wy;
+ }
+ delete[] ptTemplateSuppPyr[i];
+ }
+ }
+ delete[] ptTemplateSuppPyr;
+ ptTemplateSuppPyr = NULL;
+ }
+
+ if(ptTemplateSelectPyr){
+ for(unsigned int i=0;i<nbLvlPyr;i++){
+ if(ptTemplateSelectPyr[i])
+ delete[] ptTemplateSelectPyr[i];
+ }
+ delete[] ptTemplateSelectPyr;
+ ptTemplateSelectPyr = NULL;
+ }
+
+ if(templateSizePyr){
+ delete[] templateSizePyr;
+ templateSizePyr = NULL;
+ }
+
+ if(HdesirePyr) {
+ delete[] HdesirePyr;
+ HdesirePyr = NULL;
+ }
+
+ if(HLMdesirePyr){
+ delete[] HLMdesirePyr;
+ HLMdesirePyr = NULL;
+ }
+
+ if(HLMdesireInversePyr){
+ delete[] HLMdesireInversePyr;
+ HLMdesireInversePyr = NULL;
+ }
+
+ if(zoneTrackedPyr){
+ delete[] zoneTrackedPyr;
+ zoneTrackedPyr = NULL;
+ }
+
+ if(pyr_IDes){
+ delete[] pyr_IDes;
+ pyr_IDes = NULL;
+ }
+ }
+ else
+ {
+ if(ptTemplateInit)
+ {
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ delete[] ptTemplate[point].dW;
+ delete[] ptTemplate[point].HiG;
+ }
+ delete[] ptTemplate;
+ ptTemplate = NULL;
+ ptTemplateInit = false;
+ }
+ if (ptTemplateCompo) {
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ delete[] ptTemplateCompo[point].dW;
+ }
+ delete[] ptTemplateCompo;
+ ptTemplateCompo = NULL;
+ }
+ if (ptTemplateSupp) {
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ delete[] ptTemplateSupp[point].Bt;
+ delete[] ptTemplateSupp[point].BtInit;
+ delete[] ptTemplateSupp[point].dBt;
+ delete[] ptTemplateSupp[point].d2W;
+ delete[] ptTemplateSupp[point].d2Wx;
+ delete[] ptTemplateSupp[point].d2Wy;
+ }
+ delete[] ptTemplateSupp;
+ ptTemplateSupp = NULL;
+ }
+ if(ptTemplateSelectInit) {
+ if (ptTemplateSelect) {
+ delete[] ptTemplateSelect; ptTemplateSelect = NULL;
+ }
+ }
+ }
+}
+
+/*!
+ Display the warped reference template in an image.
+
+ \param I: Image in which the warped zone has to be displayed.
+ \param col: Color used to draw the triangle edges.
+ \param thickness: Thickness of the lines.
+
+ The following code shows how to use display capabilities:
+ \code
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
+
+int main()
+{
+ vpImage<unsigned char> I;
+ vpTemplateTrackerWarpHomography warp;
+ vpTemplateTrackerSSDInverseCompositional tracker(&warp);
+ vpTemplateTrackerZone zoneRef, zoneWarped;
+
+ // Display the warped zone
+ tracker.display(I, vpColor::red);
+
+ // Display the reference zone
+ zoneRef = tracker.getZoneRef();
+ zoneRef.display(I, vpColor::green);
+
+ // Display the warped zone
+ vpColVector p = tracker.getp();
+ warp.warpZone(zoneRef, p, zoneWarped);
+ zoneWarped.display(I, vpColor::blue);
+}
+ \endcode
+ */
+void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor& col, const unsigned int thickness)
+{
+ if (I.display) { // Only if a display is associated to the image
+ vpTemplateTrackerZone zoneWarped;
+ Warp->warpZone(*zoneTracked, p, zoneWarped);
+ zoneWarped.display(I, col, thickness);
+ }
+}
+
+/*!
+ Display the warped reference template in an image.
+
+ \param I: Image in which the warped zone has to be displayed.
+ \param col: Color used to draw the triangle edges.
+ \param thickness: Thickness of the lines.
+
+ The following code shows how to use display capabilities:
+ \code
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
+
+int main()
+{
+ vpImage<vpRGBa> I;
+ vpTemplateTrackerWarpHomography warp;
+ vpTemplateTrackerSSDInverseCompositional tracker(&warp);
+ vpTemplateTrackerZone zoneRef, zoneWarped;
+
+ // Display the warped zone
+ tracker.display(I, vpColor::red);
+
+ // Display the reference zone
+ zoneRef = tracker.getZoneRef();
+ zoneRef.display(I, vpColor::green);
+
+ // Display the warped zone
+ vpColVector p = tracker.getp();
+ warp.warpZone(zoneRef, p, zoneWarped);
+ zoneWarped.display(I, vpColor::blue);
+}
+ \endcode
+ */
+void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor& col, const unsigned int thickness)
+{
+ if (I.display) { // Only if a display is associated to the image
+ vpTemplateTrackerZone zoneWarped;
+ Warp->warpZone(*zoneTracked, p, zoneWarped);
+ zoneWarped.display(I, col, thickness);
+ }
+}
+
+void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha)
+{
+ vpColVector **ptp;
+ ptp=new vpColVector*[4];
+ vpColVector p0(nbParam);
+ p0=tp;
+
+ //valeur necessaire si conditionnel
+ vpColVector dpt(Warp->getNbParam());
+ vpColVector adpt(Warp->getNbParam());
+
+ vpColVector p1(nbParam);
+ if(useCompositionnal)
+ {
+ if(useInverse)
+ Warp->getParamInverse(direction,dpt);
+ else
+ dpt=direction;
+ Warp->pRondp(tp,dpt,p1);
+ }
+ else
+ {
+ p1=tp+direction;
+ }
+
+ vpColVector p2(nbParam);
+ if(useCompositionnal)
+ {
+ adpt=alpha*direction;
+ if(useInverse)
+ Warp->getParamInverse(adpt,dpt);
+ else
+ dpt=adpt;
+ Warp->pRondp(tp,dpt,p2);
+ }
+ else
+ {
+ p2=tp+alpha*direction;
+ }
+ vpColVector p3(nbParam);
+ ptp[0]=&p0;
+ ptp[1]=&p1;
+ ptp[2]=&p2;
+ ptp[3]=&p3;
+
+
+ double *Cost=new double[4];
+ //Cost[0]=getCost(I,p0);
+ Cost[0]=tMI;
+ Cost[1]=getCost(I,p1);
+ Cost[2]=getCost(I,p2);
+
+
+ double *talpha=new double[4];
+ talpha[0]=0;
+ talpha[1]=1.;
+ talpha[2]=alpha;
+
+ //for(int i=0;i<3;i++)
+ // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] = "<<Cost[i]<<std::endl;
+
+ //Utilise trois estimï¿œes de paraboles succesive ...
+ //A changer pour rendre adaptable
+ for(unsigned int opt=0;opt<nbIterBrent;opt++)
+ {
+ //double a=talpha[0];
+ //double b=talpha[1];
+ //double c=talpha[2];
+ //double Cost0=Cost[0];
+ //double Cost1=Cost[1];
+ //double Cost2=Cost[2];
+
+ vpMatrix A(3,3);
+ for(unsigned int i=0;i<3;i++){A[i][0]=talpha[i]*talpha[i];A[i][1]=talpha[i];A[i][2]=1.;}
+ //std::cout<<"A="<<A<<std::endl;
+ vpColVector B(3);for(unsigned int i=0;i<3;i++)B[i]=Cost[i];
+ vpColVector parabol(3);parabol=(A.t()*A).inverseByLU()*A.t()*B;
+ //vpColVector parabol(3);parabol=A.pseudoInverse()*B;
+
+ //si convexe
+ if(parabol[0]>0)
+ {
+ talpha[3]=-0.5*parabol[1]/parabol[0];
+ //std::cout<<"parabol = "<<parabol<<std::endl;
+ //std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
+ }
+ else//si concave
+ {
+ int tindic_x_min=0;
+ int tindic_x_max=0;
+ for(int i=1;i<3;i++)
+ {
+ if(talpha[i]<talpha[tindic_x_min])
+ tindic_x_min=i;
+ if(talpha[i]>talpha[tindic_x_max])
+ tindic_x_max=i;
+ }
+
+ if(Cost[tindic_x_max]<Cost[tindic_x_min])
+ {
+ //talpha[3]=talpha[tindic_x_max]+1.;
+ talpha[3]=talpha[tindic_x_max]+1.;
+ /*if(talpha[tindic_x_min]>talpha[tindic_x_max])
+ talpha[3]=talpha[tindic_x_min]+1.;
+ else
+ talpha[3]=talpha[tindic_x_min]-1.;*/
+ }
+ else
+ {
+ //talpha[3]=talpha[tindic_x_min]-1.;
+ talpha[3]=talpha[tindic_x_min]-1.;
+ /*if(talpha[tindic_x_min]<talpha[tindic_x_max])
+ talpha[3]=talpha[tindic_x_max]+1.;
+ else
+ talpha[3]=talpha[tindic_x_max]-1.;*/
+ }
+ //std::cout<<"concave talpha="<<talpha[3]<<std::endl;
+ }
+ //std::cout<<"talpha="<<talpha[3]<<std::endl;
+ int indic_x_min=0;
+ int indic_x_max=0;
+ for(int i=1;i<3;i++)
+ {
+ if(talpha[i]<talpha[indic_x_min])
+ indic_x_min=i;
+ if(talpha[i]>talpha[indic_x_max])
+ indic_x_max=i;
+ }
+ //std::cout<<"talpha = "<<talpha[3]<<std::endl;
+ if(talpha[3]>talpha[indic_x_max])
+ if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+4.;
+ if(talpha[3]<talpha[indic_x_min])
+ if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-4.;
+
+ /*if(((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0))==0)
+ {Cost[3]=1000;break;}
+
+ //calcul du gain correspondant au minimum de la parabole estimï¿œe
+ talpha[3]=b-0.5*((b-a)*(b-a)*(Cost1-Cost2)-(b-c)*(b-c)*(Cost1-Cost0))/((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0));
+ int indic_x_min=0;
+ int indic_x_max=0;
+ for(int i=1;i<3;i++)
+ {
+ if(talpha[i]<talpha[indic_x_min])
+ indic_x_min=i;
+ if(talpha[i]>talpha[indic_x_max])
+ indic_x_max=i;
+ }
+ std::cout<<"talpha = "<<talpha[3]<<std::endl;
+ if(talpha[3]>talpha[indic_x_max])
+ if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+alpha;
+ if(talpha[3]<talpha[indic_x_min])
+ if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-alpha;*/
+
+ //p3=tp-talpha[3]*direction;
+ if(useCompositionnal)
+ {
+ adpt=talpha[3]*direction;
+ if(useInverse)
+ Warp->getParamInverse(adpt,dpt);
+ else
+ dpt=adpt;
+ Warp->pRondp(tp,dpt,p3);
+ }
+ else
+ {
+ p3=tp+talpha[3]*direction;
+ }
+
+ Cost[3]=getCost(I,p3);
+ //std::cout<<"new cost="<<Cost[3]<<std::endl;
+
+ int indice_f_max=0;
+ for(int i=1;i<4;i++)
+ if(Cost[i]>Cost[indice_f_max])
+ indice_f_max=i;
+ if(indice_f_max!=3)
+ {
+ *ptp[indice_f_max]=*ptp[3];
+ Cost[indice_f_max]=Cost[3];
+ talpha[indice_f_max]=talpha[3];
+ }
+ else
+ break;
+ }
+
+ int indice_f_min=0;
+ for(int i=0;i<4;i++)
+ if(Cost[i]<Cost[indice_f_min])
+ indice_f_min=i;
+
+ alpha=talpha[indice_f_min];
+
+ if(alpha<1)alpha=1.;
+
+ delete[] ptp;
+ delete[] Cost;
+ delete[] talpha;
+}
+
+
+/*!
+ \param nbLvl : Number of levels in the pyramid.
+ \param l0 : Pyramid level where the tracking is stopped. The level with the highest resolution is 0.
+ */
+void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0)
+{
+ // vpTRACE("init_pyramidal");
+ nbLvlPyr=nbLvl;
+ l0Pyr=l0;
+
+ zoneTrackedPyr=new vpTemplateTrackerZone[nbLvlPyr];
+ pyr_IDes=new vpImage<unsigned char>[nbLvlPyr];
+ ptTemplatePyr=new vpTemplateTrackerPoint*[nbLvlPyr];
+ ptTemplateSelectPyr=new bool*[nbLvlPyr];
+ ptTemplateSuppPyr=new vpTemplateTrackerPointSuppMIInv*[nbLvlPyr];
+ ptTemplateCompoPyr=new vpTemplateTrackerPointCompo*[nbLvlPyr];
+ for(unsigned int i=0; i< nbLvlPyr; i++) {
+ ptTemplatePyr[i] = NULL;
+ ptTemplateSuppPyr[i] = NULL;
+ ptTemplateSelectPyr[i] = NULL;
+ ptTemplateCompoPyr[i] = NULL;
+ }
+ templateSizePyr=new unsigned int[nbLvlPyr];
+ HdesirePyr=new vpMatrix[nbLvlPyr];
+ HLMdesirePyr=new vpMatrix[nbLvlPyr];
+ HLMdesireInversePyr=new vpMatrix[nbLvlPyr];
+
+ pyrInitialised=true;
+ // vpTRACE("fin init_pyramidal");
+
+}
+void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone)
+{
+ // vpTRACE("initTrackingPyr");
+ zoneTrackedPyr[0].copy(zone);
+ //vpTRACE("fin copy zone");
+
+ pyr_IDes[0]=I;
+ initTracking(pyr_IDes[0],zoneTrackedPyr[0]);
+ ptTemplatePyr[0]=ptTemplate;
+ ptTemplateSelectPyr[0]=ptTemplateSelect;
+ templateSizePyr[0]=templateSize;
+
+ //creationpyramide de zones et images desiree
+ if(nbLvlPyr>1)
+ {
+ for(unsigned int i=1;i<nbLvlPyr;i++)
+ {
+ zoneTrackedPyr[i]=zoneTrackedPyr[i-1].getPyramidDown();
+ vpImageFilter::getGaussPyramidal(pyr_IDes[i-1],pyr_IDes[i]);
+
+ initTracking(pyr_IDes[i],zoneTrackedPyr[i]);
+ ptTemplatePyr[i]=ptTemplate;
+ ptTemplateSelectPyr[i]=ptTemplateSelect;
+ templateSizePyr[i]=templateSize;
+ //reste probleme avec le Hessien
+ }
+ }
+ /*for(int i=0;i<nbLvlPyr;i++)
+ {
+ vpColVector ptemp(8);ptemp=0;
+ zoneTracked=&zoneTrackedPyr[i];
+ init_pos_evalRMS(ptemp);
+ }*/
+ zoneTracked=&zoneTrackedPyr[0];
+
+ // vpTRACE("fin initTrackingPyr");
+
+}
+
+/*!
+ Select the reference template in image \e I using mouse click.
+
+ \param I: Image containing the reference template.
+ \param delaunay: Flag used to enable Delaunay triangulation.
+ - If true, from the image points selected by the user, a Delaunay triangulation is performed
+ to initialize the reference template.
+ - A left click select a image point;
+ - A right click select the last image point and ends the initialisation stage.
+ - If false, the user select directly points as successive triangle corners.
+ The size of \e v_ip vector should be a multiple of 3. It is not mandatory
+ that triangles have one edge in common; they can define a discontinued area.
+ - A left click select a triangle corner;
+ - A right click select the last triangle corner and ends the initialisation stage.
+ For example, to select the reference template as two triangles, the user has to left click
+ five times and finish the selection on the sixth corner with a right click.
+
+ */
+void vpTemplateTracker::initClick(const vpImage<unsigned char> &I, bool delaunay)
+{
+ zoneRef_.initClick(I, delaunay);
+
+ if (nbLvlPyr > 1) {
+ initPyramidal(nbLvlPyr, l0Pyr);
+ initTrackingPyr(I, zoneRef_);
+ initHessienDesiredPyr(I);
+ }
+ else {
+ initTracking(I, zoneRef_);
+ initHessienDesired(I);
+// trackNoPyr(I);
+ }
+}
+
+/*!
+ Initialize the reference template from a vector of points.
+
+ \param I: Image containing the reference template.
+ \param v_ip: Vector of image points defining the reference template.
+ \param delaunay:
+ - If true, from the image points defining the reference template enable Delaunay triangulation.
+ - If false, the vector of image points define the reference template as a list of triangles.
+ The size of \e v_ip vector should be a multiple of 3.
+ */
+void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay)
+{
+ zoneRef_.initFromPoints(I, v_ip, delaunay);
+
+ if (nbLvlPyr > 1) {
+ initPyramidal(nbLvlPyr, l0Pyr);
+ initTrackingPyr(I, zoneRef_);
+ initHessienDesiredPyr(I);
+ }
+ else {
+ initTracking(I, zoneRef_);
+ initHessienDesired(I);
+ //trackNoPyr(I);
+ }
+}
+
+/*!
+ Initialize the reference template from a vector of points.
+
+ \param I: Image containing the reference template.
+ \param zone: The zone that describes the reference template.
+ */
+void vpTemplateTracker::initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone)
+{
+ zoneRef_ = zone;
+
+ if (nbLvlPyr > 1) {
+ initPyramidal(nbLvlPyr, l0Pyr);
+ initTrackingPyr(I, zoneRef_);
+ initHessienDesiredPyr(I);
+ }
+ else {
+ initTracking(I, zoneRef_);
+ initHessienDesired(I);
+// trackNoPyr(I);
+ }
+}
+
+void vpTemplateTracker::initCompInversePyr(const vpImage<unsigned char> &I)
+{
+ vpTRACE("initCompInversePyr");
+ templateSize=templateSizePyr[0];
+ //ptTemplateSupp=ptTemplateSuppPyr[0];
+ //ptTemplateCompo=ptTemplateCompoPyr[0];
+ ptTemplate=ptTemplatePyr[0];
+ ptTemplateSelect=ptTemplateSelectPyr[0];
+ initCompInversePyr(I);
+ ptTemplateSuppPyr[0]=ptTemplateSupp;
+ ptTemplateCompoPyr[0]=ptTemplateCompo;
+
+
+ if(nbLvlPyr>1)
+ {
+ vpImage<unsigned char> Itemp=I;
+ for(unsigned int i=1;i<nbLvlPyr;i++)
+ {
+ vpImageFilter::getGaussPyramidal(Itemp,Itemp);
+
+ templateSize=templateSizePyr[i];
+ ptTemplate=ptTemplatePyr[i];
+ ptTemplateSelect=ptTemplateSelectPyr[i];
+ initCompInversePyr(Itemp);
+ ptTemplateSuppPyr[i]=ptTemplateSupp;
+ ptTemplateCompoPyr[i]=ptTemplateCompo;
+ }
+ }
+ // vpTRACE("fin initCompInversePyr");
+
+}
+void vpTemplateTracker::initHessienDesiredPyr(const vpImage<unsigned char> &I)
+{
+ // vpTRACE("initHessienDesiredPyr");
+
+ templateSize=templateSizePyr[0];
+ //ptTemplateSupp=ptTemplateSuppPyr[0];
+ //ptTemplateCompo=ptTemplateCompoPyr[0];
+ ptTemplate=ptTemplatePyr[0];
+ ptTemplateSelect=ptTemplateSelectPyr[0];
+// ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ try{
+ initHessienDesired(I);
+ ptTemplateSuppPyr[0]=ptTemplateSupp;
+ ptTemplateCompoPyr[0]=ptTemplateCompo;
+ HdesirePyr[0]=Hdesire;
+ HLMdesirePyr[0]=HLMdesire;
+ HLMdesireInversePyr[0]=HLMdesireInverse;
+ }
+ catch(vpException &e){
+ ptTemplateSuppPyr[0]=ptTemplateSupp;
+ ptTemplateCompoPyr[0]=ptTemplateCompo;
+ HdesirePyr[0]=Hdesire;
+ HLMdesirePyr[0]=HLMdesire;
+ HLMdesireInversePyr[0]=HLMdesireInverse;
+ throw(e);
+ }
+
+ if(nbLvlPyr>1)
+ {
+ vpImage<unsigned char> Itemp;Itemp=I;
+ for(unsigned int i=1;i<nbLvlPyr;i++)
+ {
+ vpImageFilter::getGaussPyramidal(Itemp,Itemp);
+
+ templateSize=templateSizePyr[i];
+ ptTemplate=ptTemplatePyr[i];
+ ptTemplateSelect=ptTemplateSelectPyr[i];
+ //ptTemplateSupp=ptTemplateSuppPyr[i];
+ //ptTemplateCompo=ptTemplateCompoPyr[i];
+ try{
+ initHessienDesired(Itemp);
+ ptTemplateSuppPyr[i]=ptTemplateSupp;
+ ptTemplateCompoPyr[i]=ptTemplateCompo;
+ HdesirePyr[i]=Hdesire;
+ HLMdesirePyr[i]=HLMdesire;
+ HLMdesireInversePyr[i]=HLMdesireInverse;
+ }
+ catch(vpException &e){
+ ptTemplateSuppPyr[i]=ptTemplateSupp;
+ ptTemplateCompoPyr[i]=ptTemplateCompo;
+ HdesirePyr[i]=Hdesire;
+ HLMdesirePyr[i]=HLMdesire;
+ HLMdesireInversePyr[i]=HLMdesireInverse;
+ throw(e);
+ }
+ }
+ }
+ // vpTRACE("fin initHessienDesiredPyr");
+}
+
+/*!
+ Track the template on image \e I.
+ \param I: Image to process.
+ */
+void vpTemplateTracker::track(const vpImage<unsigned char> &I)
+{
+ if (nbLvlPyr > 1)
+ trackPyr(I);
+ else
+ trackNoPyr(I);
+}
+
+void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
+{
+ //vpTRACE("trackPyr");
+ vpImage<unsigned char> *pyr_I;
+// pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
+ pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
+ pyr_I[0]=I;
+
+ try
+ {
+ vpColVector ptemp(nbParam);
+ if(nbLvlPyr>1)
+ {
+ // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
+ // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
+
+ // p_sauv[0]=p;
+ for(unsigned int i=1;i<nbLvlPyr;i++)
+ {
+ vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
+ //test getParamPyramidDown
+ /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
+ vpColVector vX_test2(2);
+ Warp->computeCoeff(p);
+ Warp->computeDenom(vX_test,p);
+ Warp->warpX(vX_test,vX_test2,p);
+ std::cout<<"p = "<<p.t()<<std::endl;*/
+ //std::cout<<"get p down"<<std::endl;
+ Warp->getParamPyramidDown(p,ptemp);
+ p=ptemp;
+ zoneTracked=&zoneTrackedPyr[i];
+
+ // p_sauv[i]=p;
+ /*std::cout<<"p_down = "<<p.t()<<std::endl;
+
+ vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
+ vpColVector vX_testd2(2);
+ Warp->computeCoeff(p);
+ Warp->computeDenom(vX_testd,p);
+ Warp->warpX(vX_testd,vX_testd2,p);
+ std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
+
+ }
+
+ for(int i=(int)nbLvlPyr-1;i>=0;i--)
+ {
+ if(i>=(int)l0Pyr)
+ {
+ templateSize=templateSizePyr[i];
+ ptTemplate=ptTemplatePyr[i];
+ ptTemplateSelect=ptTemplateSelectPyr[i];
+ ptTemplateSupp=ptTemplateSuppPyr[i];
+ ptTemplateCompo=ptTemplateCompoPyr[i];
+ H=HdesirePyr[i];
+ HLM=HLMdesirePyr[i];
+ HLMdesireInverse=HLMdesireInversePyr[i];
+ // zoneTracked=&zoneTrackedPyr[i];
+ trackRobust(pyr_I[i]);
+ }
+ //std::cout<<"get p up"<<std::endl;
+ // ptemp=p_sauv[i-1];
+ if (i > 0) {
+ Warp->getParamPyramidUp(p,ptemp);
+ p=ptemp;
+ zoneTracked=&zoneTrackedPyr[i-1];
+ }
+ }
+ #if 0
+ if(l0Pyr==0)
+ {
+ templateSize=templateSizePyr[0];
+ ptTemplate=ptTemplatePyr[0];
+ ptTemplateSelect=ptTemplateSelectPyr[0];
+ ptTemplateSupp=ptTemplateSuppPyr[0];
+ ptTemplateCompo=ptTemplateCompoPyr[0];
+ H=HdesirePyr[0];
+ HLM=HLMdesirePyr[0];
+ HLMdesireInverse=HLMdesireInversePyr[0];
+ zoneTracked=&zoneTrackedPyr[0];
+ trackRobust(pyr_I[0]);
+ }
+
+ if (l0Pyr > 0) {
+ // for (int l=(int)l0Pyr; l >=0; l--) {
+ // Warp->getParamPyramidUp(p,ptemp);
+ // p=ptemp;
+ // }
+ zoneTracked=&zoneTrackedPyr[0];
+ }
+ #endif
+ // delete [] p_sauv;
+ }
+ else
+ {
+ //std::cout<<"reviens a tracker de base"<<std::endl;
+ trackRobust(I);
+ }
+ delete[] pyr_I;
+ }
+ catch(vpException &e){
+ delete[] pyr_I;
+ throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
+ }
+}
+
+void vpTemplateTracker::trackRobust(const vpImage<unsigned char> &I)
+{
+ if(costFunctionVerification)
+ {
+ vpColVector p_pre_estimation;p_pre_estimation=p;
+ getGaussianBluredImage(I);
+ double pre_fcost=getCost(I,p);
+
+ trackNoPyr(I);
+
+ //std::cout<<"fct avant : "<<pre_fcost<<std::endl;
+ double post_fcost=getCost(I,p);
+ //std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
+ if(pre_fcost<post_fcost)
+ p=p_pre_estimation;
+ }
+ else
+ trackNoPyr(I);
+}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp
new file mode 100644
index 0000000..fcd2051
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp
@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarp.h>
+
+void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out)
+{
+ if (p.size() < 2) {
+ vpCTRACE << "Bad template tracker warp parameters dimension. Should never occur. " << std::endl;
+ throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
+ }
+ vpColVector S1(2),S2(2),S3(2);
+ vpColVector rS1(2),rS2(2),rS3(2);
+ in.getCorners(S1,S2,S3);
+ computeDenom(S1,p);
+ warpX(S1,rS1,p);
+ computeDenom(S2,p);
+ warpX(S2,rS2,p);
+ computeDenom(S3,p);
+ warpX(S3,rS3,p);
+ out.init(rS1,rS2,rS3);
+}
+void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out)
+{
+ vpTemplateTrackerTriangle TR,TT;
+ out.clear();
+ for(unsigned int i=0;i<in.getNbTriangle();i++)
+ {
+ in.getTriangle(i,TR);
+ warpTriangle(TR,p,TT);
+ out.add(TT);
+ }
+}
+
+double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
+{
+ unsigned int nb_corners = Z.getNbTriangle() * 3;
+ computeCoeff(p);
+ vpColVector X1(2),X2(2);
+
+ double res=0;
+ vpTemplateTrackerTriangle triangle;
+ for(unsigned int i=0;i<Z.getNbTriangle();i++)
+ {
+ Z.getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ computeDenom(X1,p);
+ warpX(X1,X2,p);
+ res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
+ }
+ }
+
+ return res/nb_corners;
+}
+
+void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
+{
+ computeCoeff(p);
+ vpColVector X1(2),X2(2);
+ for(int i=0;i<nb_pt;i++)
+ {
+ X1[0]=ut0[i];
+ X1[1]=vt0[i];
+ computeDenom(X1,p);
+ warpX(X1,X2,p);
+ u[i]=X2[0];
+ v[i]=X2[1];
+ //std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
+ }
+}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
+{
+ vpMatrix dW_(2,nbParam);
+ vpMatrix dX(2,1);
+ vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
+ vpMatrix G(nbParam,1);
+
+ int cpt=0;
+ vpColVector X1(2);
+ vpColVector fX1(2);
+ vpColVector X2(2);
+ double erreur=0;
+ double erreur_prec;
+ double lambda=0.01;
+ do
+ {
+ erreur_prec=erreur;
+ H=0;
+ G=0;
+ erreur=0;
+ computeCoeff(p);
+ for(int i=0;i<nb_pt;i++)
+ {
+ X1[0]=ut0[i];
+ X1[1]=vt0[i];
+ computeDenom(X1,p);
+ warpX(X1,fX1,p);
+ dWarp(X1,fX1,p,dW_);
+ H+=dW_.AtA();
+
+ X2[0]=u[i];
+ X2[1]=v[i];
+
+ dX=X2-fX1;
+ G+=dW_.t()*dX;
+
+ erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
+
+ }
+
+ vpMatrix::computeHLM(H, lambda, HLM);
+ try{
+ p+=(vpColVector)(HLM.inverseByLU()*G, 0);
+ }
+ catch(vpException &e) {
+ //std::cout<<"Cannot inverse the matrix by LU " << std::endl;
+ throw(e);
+ }
+ cpt++;
+ }
+ while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
+ //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
+}
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp
new file mode 100644
index 0000000..6fa68ea
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpAffine.h>
+
+
+vpTemplateTrackerWarpAffine::vpTemplateTrackerWarpAffine()
+{
+ nbParam = 6 ;
+ dW.resize(2,nbParam);
+}
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpAffine::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ pdown=p;
+ pdown[4]=p[4]/2.;
+ pdown[5]=p[5]/2.;
+}
+
+void vpTemplateTrackerWarpAffine::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ pup=p;
+ pup[4]=p[4]*2.;
+ pup[5]=p[5]*2.;
+}
+/*calcul de di*dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpAffine::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+{
+ dIdW[0]=j*dx;
+ dIdW[1]=j*dy;
+ dIdW[2]=i*dx;
+ dIdW[3]=i*dy;
+ dIdW[4]=dx;
+ dIdW[5]=dy;
+}
+/*calcul de dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpAffine::getdWdp0(const int &i,const int &j,double *dIdW)
+{
+ dIdW[0]=j;
+ dIdW[1]=0;
+ dIdW[2]=i;
+ dIdW[3]=0;
+ dIdW[4]=1.;
+ dIdW[5]=0;
+
+ dIdW[6]=0;
+ dIdW[7]=j;
+ dIdW[8]=0;
+ dIdW[9]=i;
+ dIdW[10]=0;
+ dIdW[11]=1.;
+}
+
+void vpTemplateTrackerWarpAffine::warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM)
+{
+ j2=(1+ParamM[0])*j+ParamM[2]*i+ParamM[4];
+ i2=ParamM[1]*j+(1+ParamM[3])*i+ParamM[5];
+}
+
+
+void vpTemplateTrackerWarpAffine::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=(1.0+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
+ vXres[1]=ParamM[1]*vX[0]+(1.0+ParamM[3])*vX[1]+ParamM[5];
+}
+
+void vpTemplateTrackerWarpAffine::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+{
+ double j=X1[0];
+ double i=X1[1];
+ dW_=0;
+ dW_[0][0]=j;dW_[0][2]=i;dW_[0][4]=1;
+ dW_[1][1]=j;dW_[1][3]=i;dW_[1][5]=1;
+}
+
+/*compute dw=dw/dx*dw/dp
+*/
+void vpTemplateTrackerWarpAffine::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
+ const double *dwdp0, vpMatrix &dW_)
+{
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=(1.+ParamM[0])*dwdp0[i]+ParamM[2]*dwdp0[i+nbParam];
+ dW_[1][i]=ParamM[1]*dwdp0[i]+(1.+ParamM[3])*dwdp0[i+nbParam];
+ }
+}
+
+void vpTemplateTrackerWarpAffine::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=(1+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
+ vXres[1]=ParamM[1]*vX[0]+(1+ParamM[3])*vX[1]+ParamM[5];
+}
+void vpTemplateTrackerWarpAffine::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ vpColVector Trans(2);
+ vpMatrix MWrap(2,2);
+ Trans[0]=ParamM[4];Trans[1]=ParamM[5];
+ MWrap[0][0]=1+ParamM[0];MWrap[0][1]=ParamM[2];
+ MWrap[1][0]=ParamM[1];MWrap[1][1]=1+ParamM[3];
+
+ vpMatrix MWrapInv(2,2);MWrapInv=MWrap.inverseByLU();
+ vpColVector TransInv(2);TransInv=-1*MWrapInv*Trans;
+
+ ParamMinv[0]=MWrapInv[0][0]-1;ParamMinv[2]=MWrapInv[0][1];
+ ParamMinv[1]=MWrapInv[1][0];ParamMinv[3]=MWrapInv[1][1]-1;
+ ParamMinv[4]=TransInv[0];ParamMinv[5]=TransInv[1];
+}
+
+void vpTemplateTrackerWarpAffine::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ vpColVector Trans1(2);
+ vpMatrix MWrap1(2,2);
+ Trans1[0]=p1[4];Trans1[1]=p1[5];
+ MWrap1[0][0]=1+p1[0];MWrap1[0][1]=p1[2];
+ MWrap1[1][0]=p1[1];MWrap1[1][1]=1+p1[3];
+
+ vpColVector Trans2(2);
+ vpMatrix MWrap2(2,2);
+ Trans2[0]=p2[4];Trans2[1]=p2[5];
+ MWrap2[0][0]=1+p2[0];MWrap2[0][1]=p2[2];
+ MWrap2[1][0]=p2[1];MWrap2[1][1]=1+p2[3];
+
+ vpColVector TransRes(2);
+ vpMatrix MWrapRes(2,2);
+ TransRes=MWrap1*Trans2+Trans1;
+ MWrapRes=MWrap1*MWrap2;
+
+ pres[0]=MWrapRes[0][0]-1;pres[2]=MWrapRes[0][1];
+ pres[1]=MWrapRes[1][0];pres[3]=MWrapRes[1][1]-1;
+ pres[4]=TransRes[0];pres[5]=TransRes[1];
+}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp
new file mode 100644
index 0000000..bac2f85
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp
@@ -0,0 +1,218 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
+#include <visp3/core/vpTrackingException.h>
+
+vpTemplateTrackerWarpHomography::vpTemplateTrackerWarpHomography()
+{
+ nbParam = 8 ;
+ dW.resize(2,nbParam);
+}
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpHomography::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ pdown=p;
+ pdown[2]=p[2]*2.;
+ pdown[5]=p[5]*2.;
+ pdown[6]=p[6]/2.;
+ pdown[7]=p[7]/2.;
+}
+
+void vpTemplateTrackerWarpHomography::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ pup=p;
+ pup[2]=p[2]/2.;
+ pup[5]=p[5]/2.;
+ pup[6]=p[6]*2.;
+ pup[7]=p[7]*2.;
+}
+
+/*calcul de di*dw(x,p0)/dp */
+void vpTemplateTrackerWarpHomography::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+{
+ dIdW[0]=j*dx;
+ dIdW[1]=j*dy;
+ dIdW[2]=-j*j*dx-i*j*dy;
+ dIdW[3]=i*dx;
+ dIdW[4]=i*dy;
+ dIdW[5]=-i*j*dx-i*i*dy;
+ dIdW[6]=dx;
+ dIdW[7]=dy;
+}
+/*calcul de dw(x,p0)/dp */
+void vpTemplateTrackerWarpHomography::getdWdp0(const int &i,const int &j,double *dIdW)
+{
+ dIdW[0]=j;
+ dIdW[1]=0;
+ dIdW[2]=-j*j;
+ dIdW[3]=i;
+ dIdW[4]=0;
+ dIdW[5]=-i*j;
+ dIdW[6]=1.;
+ dIdW[7]=0;
+
+ dIdW[8]=0;
+ dIdW[9]=j;
+ dIdW[10]=-i*j;
+ dIdW[11]=0;
+ dIdW[12]=i;
+ dIdW[13]=-i*i;
+ dIdW[14]=0;
+ dIdW[15]=1.;
+}
+void vpTemplateTrackerWarpHomography::computeDenom(vpColVector &vX, const vpColVector &ParamM)
+{
+ denom=(1./(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1.));
+}
+
+void vpTemplateTrackerWarpHomography::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+{
+ j2=((1.+ParamM[0])*j+ParamM[3]*i+ParamM[6])*denom;
+ i2=(ParamM[1]*j+(1.+ParamM[4])*i+ParamM[7])*denom;
+}
+
+
+void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ //if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
+ if((denom)>0)// FS optimisation
+ {
+ vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
+ vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
+ }
+ else
+ throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
+}
+
+void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+{
+ double j=X1[0];
+ double i=X1[1];
+ dW_=0;
+ dW_[0][0]=j*denom;
+ dW_[0][2]=-j*X2[0]*denom;
+ dW_[0][3]=i*denom;
+ dW_[0][5]=-i*X2[0]*denom;
+ dW_[0][6]=denom;
+
+ dW_[1][1]=j*denom;
+ dW_[1][2]=-j*X2[1]*denom;
+ dW_[1][4]=i*denom;
+ dW_[1][5]=-i*X2[1]*denom;
+ dW_[1][7]=denom;
+}
+
+/*compute dw=dw/dx*dw/dp */
+void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &ParamM,
+ const double *dwdp0,vpMatrix &dW_)
+{
+ double dwdx0,dwdx1;
+ double dwdy0,dwdy1;
+
+ dwdx0=((1.+ParamM[0])-X2[0]*ParamM[2])*denom;
+ dwdx1=(ParamM[1]-X2[1]*ParamM[2])*denom;
+ dwdy0=(ParamM[3]-X2[0]*ParamM[5])*denom;
+ dwdy1=((1.+ParamM[4])-X2[1]*ParamM[5])*denom;
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=dwdx0*dwdp0[i]+dwdy0*dwdp0[i+nbParam];
+ dW_[1][i]=dwdx1*dwdp0[i]+dwdy1*dwdp0[i+nbParam];
+ }
+}
+
+void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+
+ if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)<0)//si dans le plan image reel
+ {
+ vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
+ vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
+ }
+ else
+ throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
+}
+void vpTemplateTrackerWarpHomography::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ vpHomography H = getHomography(ParamM);
+ vpHomography Hinv = H.inverse();
+ getParam(Hinv, ParamMinv);
+}
+
+
+vpHomography vpTemplateTrackerWarpHomography::getHomography(const vpColVector &ParamM) const
+{
+ vpHomography H;
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ {
+ if(i+3*j!=8)
+ {
+ H[i][j]=ParamM[i+3*j];
+ if(i==j)H[i][j]++;
+ }
+ else
+ H[i][j]=1.;
+ }
+
+ return H;
+}
+void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H,vpColVector &par) const
+{
+ par=0;
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ {
+ if(i+3*j!=8)
+ {
+ par[i+3*j]=H[i][j]/H[2][2];
+ if(i==j)par[i+3*j]--;
+ }
+ }
+}
+
+
+
+void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ vpHomography H1 = getHomography(p1);
+ vpHomography H2 = getHomography(p2);
+ vpHomography H = H1*H2;
+ getParam(H,pres);
+}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp
new file mode 100644
index 0000000..cafde7e
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp
@@ -0,0 +1,399 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
+
+//findWarp special a SL3 car methode additionnelle ne marche pas (la derivee n est calculable qu en p=0)
+// => resout le probleme de maniere compositionnelle
+void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0,const double *vt0,
+ const double *u, const double *v,int nb_pt,vpColVector& p)
+{
+ //std::cout<<"findWarp OVERLOADE"<<std::endl;
+ vpColVector dp(nbParam);
+ vpMatrix dW_(2,nbParam);
+ vpMatrix dX(2,1);
+ vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
+ vpMatrix G_(nbParam,1);
+
+ //vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
+ double **dW_ddp0=new double*[(unsigned int)nb_pt];
+ for(int i=0;i<nb_pt;i++)
+ {
+ //dW_ddp0[i].resize(2,nbParam);
+ dW_ddp0[i]=new double[2*nbParam];
+ //getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
+ //std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
+ getdWdp0(v[i],u[i],dW_ddp0[i]);
+ }
+
+ int cpt=0;
+ vpColVector X1(2);
+ vpColVector fX1(2);
+ vpColVector X2(2);
+ double erreur=0;
+ double erreur_prec;
+ double lambda=0.00001;
+ do
+ {
+ erreur_prec=erreur;
+ H=0;
+ G_=0;
+ erreur=0;
+ computeCoeff(p);
+ for(int i=0;i<nb_pt;i++)
+ {
+ X1[0]=ut0[i];
+ X1[1]=vt0[i];
+ computeDenom(X1,p);
+ warpX(X1,fX1,p);
+ //dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
+ //dWarp(X1,fX1,p,dW);
+ for(unsigned int ip=0;ip<nbParam;ip++)
+ {
+ dW_[0][ip]=dW_ddp0[i][ip];
+ dW_[1][ip]=dW_ddp0[i][ip+nbParam];
+ }
+
+ H+=dW_.AtA();
+
+ X2[0]=u[i];
+ X2[1]=v[i];
+
+ dX=X2-fX1;
+ G_+=dW_.t()*dX;
+
+ erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
+ }
+
+ vpMatrix::computeHLM(H, lambda, HLM);
+ try{
+ dp=HLM.inverseByLU()*G_;
+ }
+ catch(vpException &e){
+ //std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
+ throw(e);
+ }
+ pRondp(p,dp,p);
+
+ cpt++;
+ // std::cout<<"erreur ="<<erreur<<std::endl;
+ }
+ //while((cpt<1500));
+ while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
+
+ //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
+ for(int i=0;i<nb_pt;i++)
+ delete[] dW_ddp0[i];
+ delete[] dW_ddp0;
+
+}
+
+vpTemplateTrackerWarpHomographySL3::vpTemplateTrackerWarpHomographySL3()
+ : G(), dGx(), A()
+{
+ nbParam = 8 ;
+ G.resize(3,3);
+ dGx.resize(3,nbParam);
+
+ A.resize(8);
+ for(unsigned int i=0;i<8;i++)
+ {
+ A[i].resize(3,3);
+ A[i]=0;
+ }
+ A[0][0][2]=1;
+ A[1][1][2]=1;
+ A[2][0][1]=1;
+ A[3][1][0]=1;
+ A[4][0][0]=1;
+ A[4][1][1]=-1;
+ A[5][1][1]=-1;
+ A[5][2][2]=1;
+ A[6][2][0]=1;
+ A[7][2][1]=1;
+}
+
+vpTemplateTrackerWarpHomographySL3::~vpTemplateTrackerWarpHomographySL3()
+{
+}
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+//a refaire de facon analytique
+void vpTemplateTrackerWarpHomographySL3::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ double *u,*v;u=new double[4];v=new double[4];
+ //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
+ u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
+ double *u2,*v2;u2=new double[4];v2=new double[4];
+ warp(u,v,4,p,u2,v2);
+ //p=0;findWarp(u,v,u2,v2,4,p);
+ for(int i=0;i<4;i++)
+ {
+ u[i]=u[i]/2.;
+ v[i]=v[i]/2.;
+ u2[i]=u2[i]/2.;
+ v2[i]=v2[i]/2.;
+ //std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
+ }
+ pdown=p;
+ findWarp(u,v,u2,v2,4,pdown);
+ delete[] u;
+ delete[] v;
+ delete[] u2;
+ delete[] v2;
+}
+
+void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ double *u,*v;u=new double[4];v=new double[4];
+ //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
+ u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
+ //u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
+ double *u2,*v2;u2=new double[4];v2=new double[4];
+
+ //pup=p;
+
+ /*vpColVector ptest=pup;
+ warp(u,v,4,ptest,u2,v2);
+ for(int i=0;i<4;i++)
+ std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
+
+ warp(u,v,4,p,u2,v2);
+ //p=0;findWarp(u,v,u2,v2,4,p);
+
+
+ for(int i=0;i<4;i++)
+ {
+ u[i]=u[i]*2.;
+ v[i]=v[i]*2.;
+ u2[i]=u2[i]*2.;
+ v2[i]=v2[i]*2.;
+ /*std::cout<<"#########################################################################################"<<std::endl;
+ std::cout<<"#########################################################################################"<<std::endl;
+ std::cout<<"#########################################################################################"<<std::endl;
+ std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
+ }
+ findWarp(u,v,u2,v2,4,pup);
+
+ delete[] u;
+ delete[] v;
+ delete[] u2;
+ delete[] v2;
+}
+
+void vpTemplateTrackerWarpHomographySL3::computeDenom(vpColVector &vX, const vpColVector &/*ParamM*/)
+{
+ denom=vX[0]*G[2][0]+vX[1]*G[2][1]+G[2][2];
+}
+
+void vpTemplateTrackerWarpHomographySL3::computeCoeff(const vpColVector &p)
+{
+ vpMatrix pA(3,3);
+ pA[0][0]=p[4];
+ pA[0][1]=p[2];
+ pA[0][2]=p[0];
+
+ pA[1][0]=p[3];
+ pA[1][1]=-p[4]-p[5];
+ pA[1][2]=p[1];
+
+ pA[2][0]=p[6];
+ pA[2][1]=p[7];
+ pA[2][2]=p[5];
+
+ G=pA.expm();
+}
+
+
+void vpTemplateTrackerWarpHomographySL3::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &/*ParamM*/)
+{
+ double i=vX[1],j=vX[0];
+ vXres[0]=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
+ vXres[1]=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
+}
+void vpTemplateTrackerWarpHomographySL3::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &/*ParamM*/)
+{
+ j2=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
+ i2=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
+}
+
+vpHomography vpTemplateTrackerWarpHomographySL3::getHomography() const
+{
+ vpHomography H;
+ for (unsigned int i=0; i<3; i++)
+ for (unsigned int j=0; j<3; j++)
+ H[i][j] = G[i][j];
+ return H;
+}
+
+void vpTemplateTrackerWarpHomographySL3::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+{
+ vpMatrix dhdx(2,3);
+ dhdx=0;
+ dhdx[0][0]=1./denom;dhdx[1][1]=1./denom;dhdx[0][2]=-X2[0]/(denom);dhdx[1][2]=-X2[1]/(denom);
+ dGx=0;
+ for(unsigned int i=0;i<3;i++)
+ {
+ dGx[i][0]=G[i][0];
+ dGx[i][1]=G[i][1];
+ dGx[i][2]=G[i][0]*X1[1];
+ dGx[i][3]=G[i][1]*X1[0];
+ dGx[i][4]=G[i][0]*X1[0]-G[i][1]*X1[1];
+ dGx[i][5]=G[i][2]-G[i][1]*X1[1];
+ dGx[i][6]=G[i][2]*X1[0];
+ dGx[i][7]=G[i][2]*X1[1];
+ }
+ dW_=dhdx*dGx;
+
+}
+
+/*calcul de di*dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+{
+ vpMatrix dhdx(1,3);
+ dhdx=0;
+ dhdx[0][0]=dx;dhdx[0][1]=dy;dhdx[0][2]=-j*dx-i*dy;
+ G.eye();
+
+ dGx=0;
+ for(unsigned int par=0;par<3;par++)
+ {
+ dGx[par][0]=G[par][0];
+ dGx[par][1]=G[par][1];
+ dGx[par][2]=G[par][0]*i;
+ dGx[par][3]=G[par][1]*j;
+ dGx[par][4]=G[par][0]*j-G[par][1]*i;
+ dGx[par][5]=G[par][2]-G[par][1]*i;
+ dGx[par][6]=G[par][2]*j;
+ dGx[par][7]=G[par][2]*i;
+ }
+
+ for(unsigned int par=0;par<nbParam;par++)
+ {
+ double res=0;
+ for(unsigned int par2=0;par2<3;par2++)
+ res+=dhdx[0][par2]*dGx[par2][par];
+ dIdW[par]=res;
+ }
+
+}
+/*calcul de dw(x,p0)/dp
+*/
+
+void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i,const int &j,double *dIdW)
+{
+ vpMatrix dhdx(2,3);
+ dhdx=0;
+ dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
+ G.eye();
+
+ dGx=0;
+ for(unsigned int par=0;par<3;par++)
+ {
+ dGx[par][0]=G[par][0];
+ dGx[par][1]=G[par][1];
+ dGx[par][2]=G[par][0]*i;
+ dGx[par][3]=G[par][1]*j;
+ dGx[par][4]=G[par][0]*j-G[par][1]*i;
+ dGx[par][5]=G[par][2]-G[par][1]*i;
+ dGx[par][6]=G[par][2]*j;
+ dGx[par][7]=G[par][2]*i;
+ }
+ vpMatrix dIdW_temp(2,nbParam);
+ dIdW_temp=dhdx*dGx;
+
+ for(unsigned int par=0;par<nbParam;par++)
+ {
+ dIdW[par]=dIdW_temp[0][par];
+ dIdW[par+nbParam]=dIdW_temp[1][par];
+ }
+
+}
+void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i,const double &j,double *dIdW)
+{
+ vpMatrix dhdx(2,3);
+ dhdx=0;
+ dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
+ G.eye();
+
+ dGx=0;
+ for(unsigned int par=0;par<3;par++)
+ {
+ dGx[par][0]=G[par][0];
+ dGx[par][1]=G[par][1];
+ dGx[par][2]=G[par][0]*i;
+ dGx[par][3]=G[par][1]*j;
+ dGx[par][4]=G[par][0]*j-G[par][1]*i;
+ dGx[par][5]=G[par][2]-G[par][1]*i;
+ dGx[par][6]=G[par][2]*j;
+ dGx[par][7]=G[par][2]*i;
+ }
+ vpMatrix dIdW_temp(2,nbParam);
+ dIdW_temp=dhdx*dGx;
+
+ for(unsigned int par=0;par<nbParam;par++)
+ {
+ dIdW[par]=dIdW_temp[0][par];
+ dIdW[par+nbParam]=dIdW_temp[1][par];
+ }
+
+}
+/*compute dw=dw/dx*dw/dp
+*/
+
+void vpTemplateTrackerWarpHomographySL3::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &/*ParamM*/,
+ const double *dwdp0,vpMatrix &dW_)
+{
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=denom*((G[0][0]-X2[0]*G[2][0])*dwdp0[i]+(G[0][1]-X2[0]*G[2][1])*dwdp0[i+nbParam]);
+ dW_[1][i]=denom*((G[1][0]-X2[1]*G[2][0])*dwdp0[i]+(G[1][1]-X2[1]*G[2][1])*dwdp0[i+nbParam]);
+ }
+}
+
+void vpTemplateTrackerWarpHomographySL3::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ ParamMinv=-ParamM;
+}
+void vpTemplateTrackerWarpHomographySL3::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ //vrai que si commutatif ...
+ pres=p1+p2;
+}
+
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp
new file mode 100644
index 0000000..52c2180
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpRT.h>
+
+
+vpTemplateTrackerWarpRT::vpTemplateTrackerWarpRT()
+{
+ nbParam = 3;
+ dW.resize(2,nbParam);
+}
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpRT::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ pdown=p;
+ pdown[1]=p[1]/2.;
+ pdown[2]=p[2]/2.;
+}
+
+void vpTemplateTrackerWarpRT::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ pup=p;
+ pup[1]=p[1]*2.;
+ pup[2]=p[2]*2.;
+}
+/*calcul de di*dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpRT::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+{
+ // std::cout << "getdW0" << std::endl;
+ dIdW[0]=-i*dx + j*dy;
+ dIdW[1]=dx;
+ dIdW[2]=dy;
+}
+/*calcul de dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpRT::getdWdp0(const int &i,const int &j,double *dIdW)
+{
+ dIdW[0]=-i;
+ dIdW[1]=1.;
+ dIdW[2]=0;
+
+ dIdW[3]=j;
+ dIdW[4]=0;
+ dIdW[5]=1.;
+}
+
+void vpTemplateTrackerWarpRT::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+{
+ j2=(cos(ParamM[0])*j) - (sin(ParamM[0])*i) + ParamM[1];
+ i2=(sin(ParamM[0])*j) + (cos(ParamM[0])*i) + ParamM[2];
+}
+
+
+void vpTemplateTrackerWarpRT::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=(cos(ParamM[0])*vX[0]) - (sin(ParamM[0])*vX[1]) + ParamM[1];
+ vXres[1]=(sin(ParamM[0])*vX[0]) + (cos(ParamM[0])*vX[1]) + ParamM[2];
+}
+
+void vpTemplateTrackerWarpRT::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &ParamM,vpMatrix &dW_)
+{
+ double j=X1[0];
+ double i=X1[1];
+ dW_=0;
+ dW_[0][0]=(-sin(ParamM[0])*j) - (cos(ParamM[0])*i);
+ dW_[0][1]=1;
+
+ dW_[1][0]=cos(ParamM[0])*j - sin(ParamM[0])*i;
+ dW_[1][2]=1;
+}
+
+/*compute dw=dw/dx*dw/dp
+*/
+void vpTemplateTrackerWarpRT::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
+ const double *dwdp0,vpMatrix &dW_)
+{
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=(cos(ParamM[0])*dwdp0[i]) - (sin(ParamM[0])*dwdp0[i+nbParam]);
+ dW_[1][i]=(sin(ParamM[0])*dwdp0[i]) + (cos(ParamM[0])*dwdp0[i+nbParam]);
+ }
+}
+
+void vpTemplateTrackerWarpRT::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ // std::cout << "warpXspe" << std::endl;
+ vXres[0]=(cos(ParamM[0])*vX[0]) - (sin(ParamM[0])*vX[1]) + ParamM[1];
+ vXres[1]=(sin(ParamM[0])*vX[0]) + (cos(ParamM[0])*vX[1]) + ParamM[2];
+}
+
+void vpTemplateTrackerWarpRT::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ vpColVector Trans(2);
+ vpMatrix MWrap(2,2);
+ Trans[0]=ParamM[1];
+ Trans[1]=ParamM[2];
+ MWrap[0][0]=cos(ParamM[0]);
+ MWrap[0][1]=-sin(ParamM[0]);
+ MWrap[1][0]=sin(ParamM[0]);
+ MWrap[1][1]=cos(ParamM[0]);
+
+ vpMatrix MWrapInv(2,2);
+ MWrapInv=MWrap.transpose();
+ vpColVector TransInv(2);
+ TransInv=(-1.0)*MWrapInv*Trans;
+
+ ParamMinv[0]= atan2(MWrapInv[1][0],MWrapInv[1][1]);
+ ParamMinv[1]=TransInv[0];
+ ParamMinv[2]=TransInv[1];
+}
+
+void vpTemplateTrackerWarpRT::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ vpColVector Trans1(2);
+ vpMatrix MWrap1(2,2);
+ Trans1[0]=p1[1];Trans1[1]=p1[2];
+
+ MWrap1[0][0]=cos(p1[0]);
+ MWrap1[0][1]=-sin(p1[0]);
+ MWrap1[1][0]=sin(p1[0]);
+ MWrap1[1][1]=cos(p1[0]);
+
+ vpColVector Trans2(2);
+ vpMatrix MWrap2(2,2);
+ Trans2[0]=p2[1];Trans2[1]=p2[1];
+
+ MWrap2[0][0]=cos(p2[0]);
+ MWrap2[0][1]=-sin(p2[0]);
+ MWrap2[1][0]=sin(p2[0]);
+ MWrap2[1][1]=cos(p2[0]);
+
+ vpColVector TransRes(2);
+ vpMatrix MWrapRes(2,2);
+ TransRes=MWrap1*Trans2+Trans1;
+ MWrapRes=MWrap1*MWrap2;
+
+ pres[0]=atan2(MWrapRes[1][0],MWrapRes[1][1]);
+ pres[1]=TransRes[0];
+ pres[2]=TransRes[1];
+}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp
new file mode 100644
index 0000000..b289f2e
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpSRT.h>
+
+
+vpTemplateTrackerWarpSRT::vpTemplateTrackerWarpSRT()
+{
+ nbParam = 4 ;
+ dW.resize(2,nbParam);
+}
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpSRT::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ pdown=p;
+ pdown[2]=p[2]/2.;
+ pdown[3]=p[3]/2.;
+}
+
+void vpTemplateTrackerWarpSRT::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ pup=p;
+ pup[2]=p[2]*2.;
+ pup[3]=p[3]*2.;
+}
+/*calcul de di*dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpSRT::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+{
+ // std::cout << "getdW0" << std::endl;
+ dIdW[0]=j*dx + i*dy;
+ dIdW[1]=-i*dx + j*dy;
+ dIdW[2]=dx;
+ dIdW[3]=dy;
+}
+/*calcul de dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpSRT::getdWdp0(const int &i,const int &j,double *dIdW)
+{
+ dIdW[0]=j;
+ dIdW[1]=-i;
+ dIdW[2]=1.;
+ dIdW[3]=0;
+
+ dIdW[4]=i;
+ dIdW[5]=j;
+ dIdW[6]=0;
+ dIdW[7]=1.;
+}
+
+void vpTemplateTrackerWarpSRT::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+{
+ j2=((1.0+ParamM[0])*cos(ParamM[1])*j) - ((1.0+ParamM[0])*sin(ParamM[1])*i) + ParamM[2];
+ i2=((1.0+ParamM[0])*sin(ParamM[1])*j) + ((1.0+ParamM[0])*cos(ParamM[1])*i) + ParamM[3];
+}
+
+
+void vpTemplateTrackerWarpSRT::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
+ vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
+}
+
+void vpTemplateTrackerWarpSRT::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &ParamM,vpMatrix &dW_)
+{
+ double j=X1[0];
+ double i=X1[1];
+ dW_=0;
+ dW_[0][0]=cos(ParamM[1])*j - sin(ParamM[1])*i;
+ dW_[0][1]=(-(1.0+ParamM[0])*sin(ParamM[1])*j) - ((1.0+ParamM[0])*cos(ParamM[1])*i);
+ dW_[0][2]=1;
+
+ dW_[1][0]=sin(ParamM[1])*j + cos(ParamM[1])*i;
+ dW_[1][1]=(1.0+ParamM[0])*cos(ParamM[1])*j - (1.0+ParamM[0])*sin(ParamM[1])*i;
+ dW_[1][3]=1;
+}
+
+/*compute dw=dw/dx*dw/dp
+*/
+void vpTemplateTrackerWarpSRT::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
+ const double *dwdp0,vpMatrix &dW_)
+{
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=((1.+ParamM[0])*cos(ParamM[1])*dwdp0[i]) - ((1.0+ParamM[0])*sin(ParamM[1])*dwdp0[i+nbParam]);
+ dW_[1][i]=((1.+ParamM[0])*sin(ParamM[1])*dwdp0[i]) + ((1.0+ParamM[0])*cos(ParamM[1])*dwdp0[i+nbParam]);
+ }
+}
+
+void vpTemplateTrackerWarpSRT::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ // std::cout << "warpXspe" << std::endl;
+ vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
+ vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
+}
+
+void vpTemplateTrackerWarpSRT::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ vpColVector Trans(2);
+ vpMatrix MWrap(2,2);
+ Trans[0]=ParamM[2];
+ Trans[1]=ParamM[3];
+ MWrap[0][0]=cos(ParamM[1]);
+ MWrap[0][1]=-sin(ParamM[1]);
+ MWrap[1][0]=sin(ParamM[1]);
+ MWrap[1][1]=cos(ParamM[1]);
+
+ vpMatrix MWrapInv(2,2);
+ MWrapInv=MWrap.transpose();
+ vpColVector TransInv(2);
+ TransInv=(-1.0/(1.0+ParamM[0]))*MWrapInv*Trans;
+
+ ParamMinv[0]=1.0/(1.0+ParamM[0]) - 1.0;
+ ParamMinv[1]= atan2(MWrapInv[1][0],MWrapInv[1][1]);
+ ParamMinv[2]=TransInv[0];
+ ParamMinv[3]=TransInv[1];
+}
+
+void vpTemplateTrackerWarpSRT::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ vpColVector Trans1(2);
+ vpMatrix MWrap1(2,2);
+ Trans1[0]=p1[2];Trans1[1]=p1[3];
+
+ MWrap1[0][0]=cos(p1[1]);
+ MWrap1[0][1]=-sin(p1[1]);
+ MWrap1[1][0]=sin(p1[1]);
+ MWrap1[1][1]=cos(p1[1]);
+
+ vpColVector Trans2(2);
+ vpMatrix MWrap2(2,2);
+ Trans2[0]=p2[2];Trans2[1]=p2[3];
+
+ MWrap2[0][0]=cos(p2[1]);
+ MWrap2[0][1]=-sin(p2[1]);
+ MWrap2[1][0]=sin(p2[1]);
+ MWrap2[1][1]=cos(p2[1]);
+
+ vpColVector TransRes(2);
+ vpMatrix MWrapRes(2,2);
+ TransRes=(1.0+p1[0])*MWrap1*Trans2+Trans1;
+ MWrapRes=MWrap1*MWrap2;
+
+ pres[0]=(1.0+p1[0])*(1.0+p2[0]) - 1.0;
+ pres[1]=atan2(MWrapRes[1][0],MWrapRes[1][1]);
+
+ pres[2]=TransRes[0];
+ pres[3]=TransRes[1];
+}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp
new file mode 100644
index 0000000..c0ad366
--- /dev/null
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerWarpTranslation.h>
+
+vpTemplateTrackerWarpTranslation::vpTemplateTrackerWarpTranslation()
+{
+ nbParam = 2 ;
+ dW.resize(2,nbParam);
+}
+
+
+//get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpTranslation::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+{
+ pdown[0]=p[0]/2.;
+ pdown[1]=p[1]/2.;
+}
+
+void vpTemplateTrackerWarpTranslation::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+{
+ pup[0]=p[0]*2.;
+ pup[1]=p[1]*2.;
+}
+
+/*calcul de di*dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpTranslation::getdW0(const int &/*i*/,const int &/*j*/,const double &dy,const double &dx,double *dIdW)
+{
+ dIdW[0]=dx;
+ dIdW[1]=dy;
+}
+/*calcul de dw(x,p0)/dp
+*/
+void vpTemplateTrackerWarpTranslation::getdWdp0(const int &/*i*/,const int &/*j*/,double *dIdW)
+{
+ dIdW[0]=1.;
+ dIdW[1]=0;
+
+ dIdW[2]=0;
+ dIdW[3]=1.;
+}
+
+void vpTemplateTrackerWarpTranslation::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+{
+ j2=j+ParamM[0];
+ i2=i+ParamM[1];
+}
+
+
+void vpTemplateTrackerWarpTranslation::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=vX[0]+ParamM[0];
+ vXres[1]=vX[1]+ParamM[1];
+}
+
+void vpTemplateTrackerWarpTranslation::dWarp(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
+ vpMatrix &dW_)
+{
+ dW_[0][0]=1;dW_[0][1]=0;
+ dW_[1][0]=0;dW_[1][1]=1;
+}
+
+/*compute dw=dw/dx*dw/dp
+*/
+void vpTemplateTrackerWarpTranslation::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
+ const double *dwdp0,vpMatrix &dW_)
+{
+ for(unsigned int i=0;i<nbParam;i++)
+ {
+ dW_[0][i]=dwdp0[i];
+ dW_[1][i]=dwdp0[i+nbParam];
+ }
+}
+
+void vpTemplateTrackerWarpTranslation::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+{
+ vXres[0]=vX[0]+ParamM[0];
+ vXres[1]=vX[1]+ParamM[1];
+}
+void vpTemplateTrackerWarpTranslation::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+{
+ ParamMinv[0]=-ParamM[0];
+ ParamMinv[1]=-ParamM[1];
+}
+
+void vpTemplateTrackerWarpTranslation::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+{
+ pres[0]=p1[0]+p2[0];
+ pres[1]=p1[1]+p2[1];
+}
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp
new file mode 100644
index 0000000..1c68377
--- /dev/null
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerZNCC.h>
+
+vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
+ : vpTemplateTracker(warp), DI(), temp()
+{
+ dW.resize(2,nbParam);
+ G.resize(nbParam);
+ H.resize(nbParam,nbParam);
+ HLM.resize(nbParam,nbParam);
+
+ temp.resize(nbParam);
+
+ X1.resize(2);
+ X2.resize(2);
+ DI.resize(2);
+}
+
+
+double vpTemplateTrackerZNCC::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ double IW,Tij;
+ int i,j;
+ double i2,j2;
+ int Nbpoint=0;
+
+ Warp->computeCoeff(tp);
+
+ double moyTij=0;
+ double moyIW=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+
+ j2=X2[0];i2=X2[1];
+ if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
+ {
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ //IW=getSubPixBspline4(I,i2,j2);
+ moyTij+=Tij;
+ moyIW+=IW;
+ Nbpoint++;
+ }
+ }
+ ratioPixelIn=(double)Nbpoint/(double)templateSize;
+ if(! Nbpoint) {
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot get cost: size = 0")) ;
+ }
+
+ moyTij=moyTij/Nbpoint;
+ moyIW=moyIW/Nbpoint;
+
+ double nom=0,denom=0;
+ double var1=0,var2=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+
+ j2=X2[0];i2=X2[1];
+ if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
+ {
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+ //IW=getSubPixBspline4(I,i2,j2);
+ nom+=(Tij-moyTij)*(IW-moyIW);
+ denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ var1+=(IW-moyIW)*(IW-moyIW);
+ var2+=(Tij-moyTij)*(Tij-moyTij);
+
+ Nbpoint++;
+ }
+ }
+ // if(Nbpoint==0)return 10e10; // cannot occur
+ //return -nom/sqrt(denom);
+ return -nom/sqrt(var1*var2);
+}
+
+
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
new file mode 100644
index 0000000..1e17f81
--- /dev/null
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
+#include <visp3/core/vpImageFilter.h>
+
+vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp):vpTemplateTrackerZNCC(warp)
+{
+ useCompositionnal=false;
+}
+
+void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ vpImage<double> dIxx,dIxy,dIyx,dIyy;
+ vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
+ vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
+
+ vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
+ vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
+
+ Warp->computeCoeff(p);
+ double IW,dIWx,dIWy;
+ double Tij;
+ int i,j;
+ double i2,j2;
+ int Nbpoint=0;
+
+ double moyTij=0;
+ double moyIW=0;
+ double denom=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ X2[0]=j;X2[1]=i;
+
+ Warp->computeDenom(X1,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ Nbpoint++;
+ moyTij+=Tij;
+ moyIW+=IW;
+ }
+ }
+ moyTij=moyTij/Nbpoint;
+ moyIW=moyIW/Nbpoint;
+ Hdesire=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ X2[0]=j;X2[1]=i;
+
+ Warp->computeDenom(X1,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dIWx=dIx.getValue(i2,j2);
+ dIWy=dIy.getValue(i2,j2);
+ //Calcul du Hessien
+ Warp->dWarp(X1,X2,p,dW);
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+
+ double prod=(Tij-moyTij);
+
+ double d_Ixx=dIxx.getValue(i2,j2);
+ double d_Iyy=dIyy.getValue(i2,j2);
+ double d_Ixy=dIxy.getValue(i2,j2);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ Hdesire[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
+ +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
+ /*Hdesire[0][0]+=prod*d_Ixx;
+ Hdesire[1][0]+=prod*d_Ixy;
+ Hdesire[0][1]+=prod*d_Ixy;
+ Hdesire[1][1]+=prod*d_Iyy;*/
+
+ denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ delete[] tempt;
+ }
+
+
+ }
+
+ Hdesire=Hdesire/sqrt(denom);
+ vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
+ HLMdesireInverse=HLMdesire.inverseByLU();
+ //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+
+}
+
+void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ double erreur=0;
+ int Nbpoint=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ /*vpImage<double> dIxx,dIxy,dIyx,dIyy;
+ getGradX(dIx, dIxx, fgdG,taillef);
+ getGradY(dIx, dIxy, fgdG,taillef);
+
+ getGradX(dIy, dIyx, fgdG,taillef);
+ getGradY(dIy, dIyy, fgdG,taillef);*/
+
+ dW=0;
+
+ //double lambda=lambdaDep;
+ double IW,dIWx,dIWy;
+ double Tij;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ double alpha=2.;
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ G=0;
+ H=0 ;
+ Warp->computeCoeff(p);
+ double moyTij=0;
+ double moyIW=0;
+ double denom=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ Nbpoint++;
+ moyTij+=Tij;
+ moyIW+=IW;
+ }
+ }
+
+ if(! Nbpoint) {
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot track the template: no point")) ;
+ }
+
+ moyTij=moyTij/Nbpoint;
+ moyIW=moyIW/Nbpoint;
+ //vpMatrix d2Wx(nbParam,nbParam);
+ //vpMatrix d2Wy(nbParam,nbParam);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Tij=ptTemplate[point].val;
+
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dIWx=dIx.getValue(i2,j2);
+ dIWy=dIy.getValue(i2,j2);
+ //Calcul du Hessien
+ Warp->dWarp(X1,X2,p,dW);
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+
+
+ double prod=(Tij-moyTij);
+ for(unsigned int it=0;it<nbParam;it++)
+ G[it]+=prod*tempt[it];
+
+ /* Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
+ for(int it=0;it<nbParam;it++)
+ for(int jt=0;jt<nbParam;jt++)
+ H[it][jt]+=prod*(d2Wx[it][jt]*dIWx+d2Wx[it][jt]*dIWy);*/
+ /*double d_Ixx=dIxx.getValue(i2,j2);
+ double d_Iyy=dIyy.getValue(i2,j2);
+ double d_Ixy=dIxy.getValue(i2,j2);
+
+ for(int it=0;it<nbParam;it++)
+ for(int jt=0;jt<nbParam;jt++)
+ H[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
+ +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));*/
+ /*H[0][0]+=prod*d_Ixx;
+ H[1][0]+=prod*d_Ixy;
+ H[0][1]+=prod*d_Ixy;
+ H[1][1]+=prod*d_Iyy;*/
+
+ double er=(Tij-IW);
+ erreur+=(er*er);
+ denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ delete[] tempt;
+ }
+
+
+ }
+ /*std::cout<<"G="<<G<<std::endl;
+ std::cout<<"H="<<H<<std::endl;
+ std::cout<<" denom="<<denom<<std::endl;*/
+ G=G/sqrt(denom);
+ //std::cout<<G<<std::endl;
+ H=H/sqrt(denom);
+
+ //if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl; // cannot occur
+
+ try
+ {
+ //vpMatrix::computeHLM(H,lambda,HLM);
+ //dp=1.*HLM.inverseByLU()*G;
+ dp=1.*HLMdesireInverse*G;
+ }
+ catch(vpException &e)
+ {
+ //std::cout<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+
+ dp=gain*dp;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
+ dp=alpha*dp;
+ }
+ p-=dp;
+ iteration++;
+ }
+ while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
+
+ //std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration=iteration;
+}
+
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
new file mode 100644
index 0000000..2ba051c
--- /dev/null
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
@@ -0,0 +1,436 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <limits> // numeric_limits
+
+#include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
+#include <visp3/core/vpImageFilter.h>
+
+vpTemplateTrackerZNCCInverseCompositional::vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerZNCC(warp), compoInitialised(false),
+ evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8), moydIrefdp()
+{
+ useInverse=true;
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
+{
+ //std::cout<<"Initialise precomputed value of Compositionnal Inverse"<<std::endl;
+ int i,j;
+
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ ptTemplate[point].dW=new double[nbParam];
+
+ double dx=ptTemplate[point].dx;
+ double dy=ptTemplate[point].dy;
+ //std::cout<<ptTemplate[point].dx<<","<<ptTemplate[point].dy<<std::endl;
+
+ Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
+
+ }
+ //vpTRACE("fin Comp Inverse");
+ compoInitialised=true;
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompInverse(I);
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ vpImage<double> dIxx,dIxy,dIyx,dIyy;
+ vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
+ vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
+
+ vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
+ vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
+
+ Warp->computeCoeff(p);
+ double Ic,dIcx=0.,dIcy=0.;
+ double Iref;
+ int i,j;
+ double i2,j2;
+ int Nbpoint=0;
+
+ double moyIref=0;
+ double moyIc=0;
+ double denom=0;
+ moydIrefdp.resize(nbParam); moydIrefdp=0;
+ vpMatrix moyd2Iref(nbParam,nbParam);moyd2Iref=0;
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ X2[0]=j;X2[1]=i;
+
+ Warp->computeDenom(X1,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Iref=ptTemplate[point].val;
+
+ if(!blur)
+ Ic=I.getValue(i2,j2);
+ else
+ Ic=BI.getValue(i2,j2);
+
+ Nbpoint++;
+ moyIref+=Iref;
+ moyIc+=Ic;
+
+ for(unsigned int it=0;it<nbParam;it++)
+ moydIrefdp[it]+=ptTemplate[point].dW[it];
+
+
+ Warp->dWarp(X1,X2,p,dW);
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
+ double d_Ixx=dIxx.getValue(i2,j2);
+ double d_Iyy=dIyy.getValue(i2,j2);
+ double d_Ixy=dIxy.getValue(i2,j2);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ {
+ moyd2Iref[it][jt] +=(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
+ +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
+ }
+
+ delete[] tempt;
+
+
+ }
+ }
+
+ moyIref=moyIref/Nbpoint;
+ moydIrefdp=moydIrefdp/Nbpoint;
+ moyd2Iref=moyd2Iref/Nbpoint;
+ moyIc=moyIc/Nbpoint;
+ Hdesire=0;
+ double covarIref=0,covarIc=0;
+ double sIcIref=0;
+ vpColVector sIcdIref(nbParam);sIcdIref=0;
+ vpMatrix sIcd2Iref(nbParam,nbParam);sIcd2Iref=0;
+ vpMatrix sdIrefdIref(nbParam,nbParam);sdIrefdIref=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ X2[0]=j;X2[1]=i;
+
+ Warp->computeDenom(X1,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Iref=ptTemplate[point].val;
+
+ if(!blur)
+ Ic=I.getValue(i2,j2);
+ else
+ Ic=BI.getValue(i2,j2);
+
+ dIcx=dIx.getValue(i2,j2);
+ dIcy=dIy.getValue(i2,j2);
+
+ Warp->dWarp(X1,X2,p,dW);
+
+ double *tempt=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
+
+ double prodIc=(Ic-moyIc);
+
+ double d_Ixx=dIxx.getValue(i2,j2);
+ double d_Iyy=dIyy.getValue(i2,j2);
+ double d_Ixy=dIxy.getValue(i2,j2);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ {
+ sIcd2Iref[it][jt] +=prodIc*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
+ +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy)-moyd2Iref[it][jt]);
+ sdIrefdIref[it][jt] +=(ptTemplate[point].dW[it]-moydIrefdp[it])*(ptTemplate[point].dW[jt]-moydIrefdp[jt]);
+ }
+
+
+ delete[] tempt;
+
+ for(unsigned int it=0;it<nbParam;it++)
+ sIcdIref[it]+=prodIc*(ptTemplate[point].dW[it]-moydIrefdp[it]);
+
+ covarIref+=(Iref-moyIref)*(Iref-moyIref);
+ covarIc+=(Ic-moyIc)*(Ic-moyIc);
+ sIcIref+=(Iref-moyIref)*(Ic-moyIc);
+ }
+
+
+ }
+ covarIref=sqrt(covarIref);
+ covarIc=sqrt(covarIc);
+
+ denom=covarIref*covarIc;
+
+ double NCC=sIcIref/denom;
+ //std::cout<<"NCC = "<<NCC<<std::endl;
+ vpColVector dcovarIref(nbParam);dcovarIref=-sIcdIref/covarIref;
+
+ vpColVector dNCC(nbParam);dNCC=(sIcdIref/denom-NCC*dcovarIref/covarIref);
+ vpMatrix d2covarIref(nbParam,nbParam);
+ d2covarIref=-(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/covarIref;
+#ifdef APPROX_NCC
+ Hdesire=sIcd2Iref/denom;
+#else
+ Hdesire=(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/denom;
+#endif
+ vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
+ HLMdesireInverse=HLMdesire.inverseByLU();
+ //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+
+ double erreur=0;
+ unsigned int Nbpoint=0;
+ vpColVector dpinv(nbParam);
+ double Ic;
+ double Iref;
+ unsigned int iteration=0;
+ int i,j;
+ double i2,j2;
+ initPosEvalRMS(p);
+ do
+ {
+ Nbpoint=0;
+ erreur=0;
+ G=0;
+ Warp->computeCoeff(p);
+ double moyIref=0;
+ double moyIc=0;
+ double denom=0;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Iref=ptTemplate[point].val;
+
+ if(!blur)
+ Ic=I.getValue(i2,j2);
+ else
+ Ic=BI.getValue(i2,j2);
+
+ Nbpoint++;
+ moyIref+=Iref;
+ moyIc+=Ic;
+ }
+
+
+ }
+ if(Nbpoint > 0)
+ {
+ moyIref=moyIref/Nbpoint;
+ moyIc=moyIc/Nbpoint;
+ double sIcIref=0;
+ double covarIref=0,covarIc=0;
+ vpColVector sIcdIref(nbParam);sIcdIref=0;
+ vpColVector sIrefdIref(nbParam);sIrefdIref=0;
+
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Iref=ptTemplate[point].val;
+
+ if(!blur)
+ Ic=I.getValue(i2,j2);
+ else
+ Ic=BI.getValue(i2,j2);
+
+
+ double prod=(Ic-moyIc);
+ for(unsigned int it=0;it<nbParam;it++)
+ sIcdIref[it]+=prod*(ptTemplate[point].dW[it]-moydIrefdp[it]);
+ for(unsigned int it=0;it<nbParam;it++)
+ sIrefdIref[it]+=(Iref-moyIref)*(ptTemplate[point].dW[it]-moydIrefdp[it]);
+
+ double er=(Iref-Ic);
+ erreur+=(er*er);
+ //denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
+ covarIref+=(Iref-moyIref)*(Iref-moyIref);
+ covarIc+=(Ic-moyIc)*(Ic-moyIc);
+ sIcIref+=(Iref-moyIref)*(Ic-moyIc);
+ }
+
+
+ }
+ covarIref=sqrt(covarIref);
+ covarIc=sqrt(covarIc);
+ denom=covarIref*covarIc;
+
+ //if(denom==0.0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
+ {
+ diverge=true;
+ }
+ else
+ {
+ double NCC=sIcIref/denom;
+ vpColVector dcovarIref(nbParam);dcovarIref=sIrefdIref/covarIref;
+ G=1.*(sIcdIref/denom-NCC*dcovarIref/covarIref);
+
+
+ try
+ {
+ dp=-1.*HLMdesireInverse*G;
+ }
+ catch(...)
+ {
+ std::cout<<"probleme inversion"<<std::endl;
+ break;
+ }
+
+ Warp->getParamInverse(dp,dpinv);
+ Warp->pRondp(p,dpinv,p);
+
+ computeEvalRMS(p);
+ }
+ }
+ else
+ diverge=true;
+
+ iteration++;
+ }
+ while( (!diverge &&(evolRMS>threshold_RMS) && (iteration < iterationMax)));
+
+ //std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration=iteration;
+
+ deletePosEvalRMS();
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::initPosEvalRMS(const vpColVector &p_)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+ x_pos.resize(nb_corners);
+ y_pos.resize(nb_corners);
+
+ Warp->computeCoeff(p);
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,p_);
+ Warp->warpX(X1,X2,p_);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::computeEvalRMS(const vpColVector &p_)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+
+ Warp->computeCoeff(p_);
+ evolRMS=0;
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,p_);
+ Warp->warpX(X1,X2,p_);
+ evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+ evolRMS=evolRMS/nb_corners;
+
+}
+
+void vpTemplateTrackerZNCCInverseCompositional::deletePosEvalRMS()
+{
+}
diff --git a/modules/tracker/tt_mi/CMakeLists.txt b/modules/tracker/tt_mi/CMakeLists.txt
new file mode 100644
index 0000000..4231776
--- /dev/null
+++ b/modules/tracker/tt_mi/CMakeLists.txt
@@ -0,0 +1,45 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_define_module(tt_mi visp_tt)
+
+# The previous line is similar to the following:
+#vp_add_module(tt_mi visp_tt)
+#vp_glob_module_sources()
+#vp_module_include_directories()
+#vp_create_module()
+#vp_add_tests()
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h
new file mode 100644
index 0000000..82b1dba
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerMI_hh
+#define vpTemplateTrackerMI_hh
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpImageFilter.h>
+
+/*!
+ \class vpTemplateTrackerMI
+ \ingroup group_tt_mi_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerMI: public vpTemplateTracker
+{
+public:
+ /*! Hessian approximation. */
+ typedef enum {
+ HESSIAN_NONSECOND = -1,
+ HESSIAN_0,
+ HESSIAN_d2I,
+ HESSIAN_YOUCEF,
+ HESSIAN_EXACT,
+ HESSIAN_NEW
+ } vpHessienApproximationType;
+
+ /*! Hessian computation. */
+ typedef enum {
+ USE_HESSIEN_NORMAL,
+ USE_HESSIEN_DESIRE,
+ USE_HESSIEN_BEST_COND
+ } vpHessienType;
+
+ /*! Hessian computation. */
+ typedef enum {
+ BSPLINE_THIRD_ORDER = 3,
+ BSPLINE_FOURTH_ORDER = 4
+ } vpBsplineType;
+
+protected:
+ vpHessienType hessianComputation;
+ vpHessienApproximationType ApproxHessian;
+ double lambda;
+
+ double *temp;
+ double *Prt;
+ double *dPrt;
+ double *Pt;
+ double *Pr;
+ double *d2Prt;
+ double *PrtTout;
+ double *dprtemp;
+
+ double *PrtD;
+ double *dPrtD;
+ int influBspline;
+
+ int bspline;
+ //Nombre de couleur concidere dans l'histogramme
+ int Nc;
+ int Ncb;
+
+ vpImage<double> d2Ix ;
+ vpImage<double> d2Iy ;
+ vpImage<double> d2Ixy ;
+
+ double MI_preEstimation;
+ double MI_postEstimation;
+
+ double NMI_preEstimation;
+ double NMI_postEstimation;
+
+ vpMatrix covarianceMatrix;
+ bool computeCovariance;
+
+protected:
+ void computeGradient();
+ void computeHessien(vpMatrix &H);
+ void computeHessienNormalized(vpMatrix &H);
+ void computeMI(double &MI);
+ void computeProba(int &nbpoint);
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getCost(const vpImage<unsigned char> &I){return getCost(I,p);}
+ double getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getNormalizedCost(const vpImage<unsigned char> &I){return getNormalizedCost(I,p);}
+ virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I)=0;
+ void zeroProbabilities();
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpTemplateTrackerMI(const vpTemplateTrackerMI &)
+// : vpTemplateTracker(), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_0), lambda(0),
+// temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
+// dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(0), Nc(0), Ncb(0),
+// d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
+// NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpTemplateTrackerMI &operator=(const vpTemplateTrackerMI &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ //constructeur
+ //! Default constructor.
+ vpTemplateTrackerMI()
+ : vpTemplateTracker(), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_0), lambda(0),
+ temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
+ dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(0), Nc(0), Ncb(0),
+ d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
+ NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
+ {}
+ vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp);
+ ~vpTemplateTrackerMI();
+ vpMatrix getCovarianceMatrix() const { return covarianceMatrix; }
+ double getMI() const {return MI_postEstimation;}
+ double getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline,vpColVector &tp);
+ double getMI256(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getNMI() const {return NMI_postEstimation;}
+ //initialisation du Hessien en position desiree
+ void setApprocHessian(vpHessienApproximationType approx){ApproxHessian=approx;}
+ void setCovarianceComputation(const bool & flag){ computeCovariance = flag; }
+ void setHessianComputation(vpHessienType type){hessianComputation=type;}
+ void setBspline(const vpBsplineType &newbs);
+ void setLambda(double _l) {lambda = _l ; }
+ void setNc(int newNc);
+};
+
+#endif
+
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h
new file mode 100644
index 0000000..73b7f2d
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \file vpTemplateTrackerMIBSpline.h
+ \brief
+*/
+
+#ifndef vpTemplateTrackerMIBSpline_hh
+#define vpTemplateTrackerMIBSpline_hh
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/tt/vpTemplateTrackerBSpline.h>
+
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+class VISP_EXPORT vpTemplateTrackerMIBSpline
+{
+public:
+ static void PutPVBsplineD(double *Prt, int cr, double er, int ct, double et,int Nc, double val, const int °re);
+ static void PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et,int Nc, double val);
+ static void PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et,int Nc, double val);
+
+ static void PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et,int Nc, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, double et,int Ncb, double *val, unsigned int &NbParam);
+
+ // AY Optimisation
+ static void PutTotPVBspline3(double *Prt, double &er, double *et, unsigned int NbParam);
+ static void PutTotPVBspline4(double *Prt, double &er, double *et, unsigned int NbParam);
+ //
+
+ static void PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam);
+
+ static void PutTotPVBsplinePrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3PrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam);
+ static void PutTotPVBspline4PrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam);
+
+ static void PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb);
+ static void PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb);
+
+ static double Bspline3(double diff);
+ static double Bspline4i(double diff,int &interv);
+
+ static double dBspline3(double diff);
+ static double dBspline4(double diff);
+
+ static double d2Bspline3(double diff);
+ static double d2Bspline4(double diff);
+};
+
+#endif
+#endif
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h
new file mode 100644
index 0000000..e31e2c8
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerMIESM_hh
+#define vpTemplateTrackerMIESM_hh
+
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpImageFilter.h>
+
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+
+/*!
+ \class vpTemplateTrackerMIESM
+ \ingroup group_tt_mi_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerMIESM: public vpTemplateTrackerMI
+{
+ /*! Minimization method. */
+ typedef enum {
+ USE_NEWTON, // not used
+ USE_LMA, // not used
+ USE_GRADIENT,
+ USE_QUASINEWTON //not used => see default equivalence
+ } vpMinimizationTypeMIESM;
+
+protected:
+ vpMinimizationTypeMIESM minimizationMethod;
+ bool CompoInitialised;
+ vpMatrix HDirect;
+ vpMatrix HInverse;
+ vpMatrix HdesireDirect;
+ vpMatrix HdesireInverse;
+ vpColVector GDirect;
+ vpColVector GInverse;
+
+protected:
+ void initCompInverse();
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpTemplateTrackerMIESM(const vpTemplateTrackerMIESM &)
+// : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), CompoInitialised(false),
+// HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpTemplateTrackerMIESM &operator=(const vpTemplateTrackerMIESM &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ //! Default constructor.
+ vpTemplateTrackerMIESM()
+ : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), CompoInitialised(false),
+ HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
+ {}
+ vpTemplateTrackerMIESM(vpTemplateTrackerWarp *_warp);
+
+ void setMinimizationMethod(vpMinimizationTypeMIESM method){minimizationMethod=method;}
+};
+
+#endif
+
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h
new file mode 100644
index 0000000..968aec3
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerMIForwardAdditional_hh
+#define vpTemplateTrackerMIForwardAdditional_hh
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpImageFilter.h>
+
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+
+/*!
+ \class vpTemplateTrackerMIForwardAdditional
+ \ingroup group_tt_mi_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerMIForwardAdditional: public vpTemplateTrackerMI
+{
+public:
+ /*! Minimization method. */
+ typedef enum {
+ USE_NEWTON,
+ USE_LMA,
+ USE_GRADIENT,
+ USE_QUASINEWTON
+ } vpMinimizationTypeMIForwardAdditional;
+
+private:
+ vpMinimizationTypeMIForwardAdditional minimizationMethod;
+ //pour eval evolRMS
+ double evolRMS;
+ double *x_pos;
+ double *y_pos;
+ double threshold_RMS;
+ //valeur pour calculer Quasi_Newton
+ vpColVector p_prec;
+ vpColVector G_prec;
+ vpMatrix KQuasiNewton;
+
+protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpTemplateTrackerMIForwardAdditional(const vpTemplateTrackerMIForwardAdditional &)
+// : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
+// threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpTemplateTrackerMIForwardAdditional &operator=(const vpTemplateTrackerMIForwardAdditional &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ //! Default constructor.
+ vpTemplateTrackerMIForwardAdditional()
+ : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
+ threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
+ {}
+ vpTemplateTrackerMIForwardAdditional(vpTemplateTrackerWarp *_warp);
+ void setThresholdRMS(double threshold){threshold_RMS=threshold;}
+ void setMinimizationMethod(vpMinimizationTypeMIForwardAdditional method){minimizationMethod=method;}
+};
+
+#endif
+
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h
new file mode 100644
index 0000000..62ad416
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerMIForwardCompositional_hh
+#define vpTemplateTrackerMIForwardCompositional_hh
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpImageFilter.h>
+
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+
+/*!
+ \class vpTemplateTrackerMIForwardCompositional
+ \ingroup group_tt_mi_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerMIForwardCompositional: public vpTemplateTrackerMI
+{
+protected:
+ bool CompoInitialised;
+
+protected:
+ void initCompo();
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+public:
+ vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp);
+};
+
+#endif
+
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h
new file mode 100644
index 0000000..b6cc39a
--- /dev/null
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#ifndef vpTemplateTrackerMIInverseCompositional_hh
+#define vpTemplateTrackerMIInverseCompositional_hh
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/tt/vpTemplateTracker.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/core/vpImageFilter.h>
+
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+
+/*!
+ \class vpTemplateTrackerMIInverseCompositional
+ \ingroup group_tt_mi_tracker
+*/
+class VISP_EXPORT vpTemplateTrackerMIInverseCompositional: public vpTemplateTrackerMI
+{
+public:
+ /*! Minimization method. */
+ typedef enum {
+ USE_NEWTON,
+ USE_LMA,
+ USE_GRADIENT,
+ USE_QUASINEWTON
+ } vpMinimizationTypeMIInverseCompositional;
+
+private:
+ vpMinimizationTypeMIInverseCompositional minimizationMethod;
+ bool CompoInitialised;
+ bool useTemplateSelect;//use only the strong gradient pixels to compute the Jabocian
+ //pour eval evolRMS
+ double evolRMS;
+ double *x_pos;
+ double *y_pos;
+ double threshold_RMS;
+ //valeur pour calculer Quasi_Newton
+ vpColVector p_prec;
+ vpColVector G_prec;
+ vpMatrix KQuasiNewton;
+
+ //bool useAYOptim;
+
+public: // AY Optimisation
+ void initTemplateRefBspline(unsigned int ptIndex, double &et);
+
+protected:
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpTemplateTrackerMIInverseCompositional(const vpTemplateTrackerMIInverseCompositional &)
+// : vpTemplateTrackerMI(), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
+// evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()//, useAYOptim(false)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpTemplateTrackerMIInverseCompositional &operator=(const vpTemplateTrackerMIInverseCompositional &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ //! Default constructor.
+ vpTemplateTrackerMIInverseCompositional()
+ : vpTemplateTrackerMI(), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
+ evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()//, useAYOptim(false)
+ {}
+ vpTemplateTrackerMIInverseCompositional(vpTemplateTrackerWarp *_warp);
+
+ /*! Use only the strong gradient pixels to compute the Jabobian. By default this feature is disabled. */
+ void setUseTemplateSelect(bool b) {useTemplateSelect = b;}
+ void setThresholdRMS(double threshold){threshold_RMS=threshold;}
+ void setMinimizationMethod(vpMinimizationTypeMIInverseCompositional method){minimizationMethod=method;}
+};
+#endif
+
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp
new file mode 100644
index 0000000..526e5c7
--- /dev/null
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp
@@ -0,0 +1,859 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/core/vpException.h>
+#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+
+void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs)
+{
+ bspline=(int)newbs;
+ influBspline=bspline*bspline;
+ Ncb=Nc+bspline;
+ if (Pt) delete[] Pt;
+ if (Pr) delete[] Pr;
+ if (Prt) delete[] Prt;
+ if (dPrt) delete[] dPrt;
+ if (d2Prt) delete[] d2Prt;
+ if (PrtD) delete[] PrtD;
+ if (dPrtD) delete[] dPrtD;
+ if (PrtTout) delete[] PrtTout;
+
+ Pt= new double[Ncb];
+ Pr= new double[Ncb];
+
+ Prt= new double[Ncb*Ncb];
+ dPrt= new double[Ncb*Ncb*(int)(nbParam)];
+ d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
+
+ /*std::cout<<Nc*Nc*influBspline<<std::endl;std::cout<<Nc*Nc*nbParam*influBspline<<std::endl;*/
+ PrtD= new double[Nc*Nc*influBspline];
+ dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
+ PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+
+ hessianComputation=USE_HESSIEN_DESIRE;
+}
+
+
+vpTemplateTrackerMI::vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp)
+ : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0),
+ temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
+ dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(3), Nc(8), Ncb(0),
+ d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
+ NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
+{
+ Ncb=Nc+bspline;
+ influBspline=bspline*bspline;
+
+ dW.resize(2,nbParam);
+ H.resize(nbParam,nbParam);
+ G.resize(nbParam);
+ Hdesire.resize(nbParam,nbParam);
+ HLM.resize(nbParam,nbParam);
+ HLMdesire.resize(nbParam,nbParam);
+ dprtemp= new double[nbParam];
+ temp= new double[nbParam];
+
+ X1.resize(2);
+ X2.resize(2);
+
+ PrtD= new double[Nc*Nc*influBspline];//(r,t)
+ dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
+
+ Prt= new double[Ncb*Ncb];//(r,t)
+ Pt= new double[Ncb];
+ Pr= new double[Ncb];
+ dPrt= new double[Ncb*Ncb*(int)(nbParam)];
+ d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
+
+ PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+
+ lambda=lambdaDep;
+}
+
+void vpTemplateTrackerMI::setNc(int nc)
+{
+ Nc=nc;
+ Ncb=Nc+bspline;
+
+ if (Pt) delete[] Pt;
+ if (Pr) delete[] Pr;
+ if (Prt) delete[] Prt;
+ if (dPrt) delete[] dPrt;
+ if (d2Prt) delete[] d2Prt;
+ if (PrtD) delete[] PrtD;
+ if (dPrtD) delete[] dPrtD;
+ if (PrtTout) delete[] PrtTout;
+
+ PrtD= new double[Nc*Nc*influBspline];//(r,t)
+ dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
+ Prt= new double[Ncb*Ncb];//(r,t)
+ dPrt= new double[Ncb*Ncb*(int)(nbParam)];
+ Pt= new double[Ncb];
+ Pr= new double[Ncb];
+ d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];//(r,t)
+ PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+}
+
+
+double vpTemplateTrackerMI::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ double MI=0;
+ int Nbpoint=0;
+ double i2,j2;
+ double Tij;
+ double IW;
+
+ int i,j;
+ int cr,ct;
+ double er,et;
+ unsigned int Ncb_ = (unsigned int) Ncb;
+
+ memset(Prt, 0, Ncb*Ncb*sizeof(double));
+ memset(PrtD, 0, Nc*Nc*influBspline*sizeof(double));
+
+ //Warp->ComputeMAtWarp(tp);
+ Warp->computeCoeff(tp);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+ j2=X2[0];i2=X2[1];
+
+ //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ cr=(int)((IW*(Nc-1))/255.);
+ ct=(int)((Tij*(Nc-1))/255.);
+ er=(IW*(Nc-1))/255.-cr;
+ et=((double)Tij*(Nc-1))/255.-ct;
+
+ //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
+ vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1.,bspline);
+ }
+ }
+
+ ratioPixelIn=(double)Nbpoint/(double)templateSize;
+
+ double *pt=PrtD;
+ for(int r=0;r<Nc;r++)
+ for(int t=0;t<Nc;t++)
+ {
+ for(i=0;i<influBspline;i++)
+ {
+ int r2,t2;
+ r2=r+i/bspline;
+ t2=t+i%bspline;
+ Prt[r2*Ncb+t2]+=*pt;
+
+ pt++;
+ }
+ }
+
+ if(Nbpoint==0)
+ return 0;
+ for(unsigned int r=0;r<Ncb_;r++)
+ for(unsigned int t=0;t<Ncb_;t++)
+ //printf("%f ",Prt[r*Ncb+t]);
+ Prt[r*Ncb_+t]=Prt[r*Ncb_+t]/Nbpoint;
+ //calcul Pr;
+ memset(Pr, 0, Ncb_*sizeof(double));
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ for(unsigned int t=0;t<Ncb_;t++)
+ Pr[r]+=Prt[r*Ncb_+t];
+ }
+
+ //calcul Pt;
+ memset(Pt, 0, Ncb_*sizeof(double));
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ for(unsigned int r=0;r<Ncb_;r++)
+ Pt[t]+=Prt[r*Ncb_+t];
+ }
+ for(unsigned int r=0;r<Ncb_;r++)
+ //if(Pr[r]!=0)
+ if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
+ MI-=Pr[r]*log(Pr[r]);
+
+ for(unsigned int t=0;t<Ncb_;t++)
+ //if(Pt[t]!=0)
+ if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
+ MI-=Pt[t]*log(Pt[t]);
+
+ for(unsigned int r=0;r<Ncb_;r++)
+ for(unsigned int t=0;t<Ncb_;t++)
+ //if(Prt[r*Ncb+t]!=0)
+ if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
+ MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+
+ return -MI;
+}
+
+double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ // Attention, cette version calculée de la NMI ne pourra pas atteindre le maximum de 2
+ // Ceci est du au fait que par defaut, l'image est floutée dans vpTemplateTracker::initTracking()
+
+ double MI=0;
+ double Nbpoint=0;
+ double i2,j2;
+ double Tij;
+ double IW;
+
+ double Pr_[256];
+ double Pt_[256];
+ double Prt_[256][256];
+
+ memset(Pr_, 0, 256*sizeof(double));
+ memset(Pt_, 0, 256*sizeof(double));
+ memset(Prt_, 0, 256*256*sizeof(double));
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ int i=ptTemplate[point].y;
+ int j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I[(int)i2][(int)j2];
+ else
+ IW=BI.getValue(i2,j2);
+
+ Pr_[(int)Tij]++;
+ Pt_[(int)IW]++;
+ Prt_[(int)Tij][(int)IW]++;
+ }
+ }
+
+ for(int i = 0 ; i < 256 ; i++)
+ {
+ Pr_[i] /= Nbpoint;
+ Pt_[i] /= Nbpoint;
+ for(int j = 0 ; j < 256 ; j++)
+ Prt_[i][j] /= Nbpoint;
+ }
+
+ for(int r=0;r<256;r++)
+ //if(Pr_[r]!=0)
+ if(std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon())
+ MI-=Pr_[r]*log(Pr_[r]);
+
+ for(int t=0;t<256;t++)
+ //if(Pt_[t]!=0)
+ if(std::fabs(Pt_[t]) > std::numeric_limits<double>::epsilon())
+ MI-=Pt_[t]*log(Pt_[t]);
+
+ double denom = 0;
+ for(int r=0;r<256;r++)
+ for(int t=0;t<256;t++)
+ //if(Prt_[r][t]!=0)
+ if(std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon())
+ denom-=(Prt_[r][t]*log(Prt_[r][t]));
+
+ //if(denom != 0)
+ if(std::fabs(denom) > std::numeric_limits<double>::epsilon())
+ MI = MI/denom;
+ else MI = 0;
+
+ return -MI;
+}
+
+vpTemplateTrackerMI::~vpTemplateTrackerMI()
+{
+ if (Pt) delete[] Pt;
+ if (Pr) delete[] Pr;
+ if (Prt) delete[] Prt;
+ if (dPrt) delete[] dPrt;
+ if (d2Prt) delete[] d2Prt;
+ if (PrtD) delete[] PrtD;
+ if (dPrtD) delete[] dPrtD;
+ if (PrtTout) delete[] PrtTout;
+ if (temp) delete[] temp;
+ if (dprtemp) delete[] dprtemp;
+}
+
+void vpTemplateTrackerMI::computeProba(int &nbpoint)
+{
+ double *pt=PrtTout;
+ unsigned int Nc_ = (unsigned int)Nc;
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ unsigned int bspline_ = (unsigned int)bspline;
+
+ for(unsigned int r=0;r<Nc_;r++) {
+ for(unsigned int t=0;t<Nc_;t++)
+ {
+ for(unsigned int r2=0;r2<bspline_;r2++) {
+ for(unsigned int t2=0;t2<bspline_;t2++)
+ {
+ Prt[((r2+r)*Ncb_+(t2+t))]+=*pt++;
+ for(unsigned int ip=0;ip<nbParam;ip++)
+ {
+ dPrt[((r2+r)*Ncb_+(t2+t))*nbParam+ip]+=*pt++;
+ for(unsigned int it=0;it<nbParam;it++){
+ d2Prt[((r2+r)*Ncb_+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ // for(unsigned int r=0;r<Nc;r++)
+ // for(unsigned int t=0;t<Nc;t++)
+ // {
+ // for(int r2=0;r2<bspline;r2++)
+ // for(int t2=0;t2<bspline;t2++)
+ // {
+ // Prt[((r2+r)*Ncb+(t2+t))]+=*pt++;
+ // for(int ip=0;ip<nbParam;ip++)
+ // {
+ // dPrt[((r2+r)*Ncb+(t2+t))*nbParam+ip]+=*pt++;
+ // }
+ // for(int ip=0;ip<nbParam;ip++)
+ // {
+ // for(int it=0;it<nbParam;it++)
+ // d2Prt[((r2+r)*Ncb+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
+ // }
+ // }
+ // }
+
+ // int vr2, vt2, vt2ip;
+ // for(unsigned int r=0;r<Nc;r++)
+ // for(unsigned int t=0;t<Nc;t++)
+ // {
+ // for(int r2=0;r2<bspline;r2++)
+ // {
+ // vr2 = (r2+r)*Ncb;
+ // for(int t2=0;t2<bspline;t2++)
+ // {
+ // vt2 = vr2+(t2+t);
+ // Prt[vt2]+=*pt++;
+ // vt2 *= nbParam;
+
+ // for(int ip=0;ip<nbParam;ip++)
+ // dPrt[vt2+ip]+=*pt++;
+
+ // for(int ip=0;ip<nbParam;ip++)
+ // {
+ // vt2ip = vt2*nbParam+ip*nbParam;
+ // for(int it=0;it<nbParam;it++)
+ // d2Prt[vt2ip+it]+=*pt++;
+ // }
+ // }
+ // }
+ // }
+
+ if(nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+ unsigned int indd, indd2;
+ indd = indd2 = 0;
+ for(int i=0;i<Ncb*Ncb;i++){
+ Prt[i]=Prt[i]/nbpoint;
+ for(unsigned int j=0;j<nbParam;j++){
+ dPrt[indd]=dPrt[indd]/nbpoint;
+ indd++;
+ for(unsigned int k=0;k<nbParam;k++){
+ d2Prt[indd2]=d2Prt[indd2]/nbpoint;
+ indd2++;
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMI::computeMI(double &MI)
+{
+ unsigned int Ncb_ = (unsigned int) Ncb;
+
+ //calcul Pr;
+ memset(Pr, 0, Ncb_*sizeof(double));
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ for(unsigned int t=0;t<Ncb_;t++)
+ Pr[r]+=Prt[r*Ncb_+t];
+ }
+
+ //calcul Pt;
+ memset(Pt, 0, Ncb_*sizeof(double));
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ for(unsigned int r=0;r<Ncb_;r++)
+ Pt[t]+=Prt[r*Ncb_+t];
+ }
+
+ //calcul Entropies;
+ double entropieI=0;
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ //if(Pr[r]!=0)
+ if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
+ {
+ entropieI-=Pr[r]*log(Pr[r]);
+ MI-=Pr[r]*log(Pr[r]);
+ }
+ }
+
+ double entropieT=0;
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ //if(Pt[t]!=0)
+ if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
+ {
+ entropieT-=Pt[t]*log(Pt[t]);
+ MI-=Pt[t]*log(Pt[t]);
+ }
+ }
+
+ for(unsigned int r=0;r<Ncb_;r++)
+ for(unsigned int t=0;t<Ncb_;t++)
+ //if(Prt[r*Ncb+t]!=0)
+ if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
+ MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+}
+
+void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
+{
+ double seuilevitinf=1e-200;
+ Hessian=0;
+ double dtemp;
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ //if(Pt[t]!=0)
+ if(Pt[t]>seuilevitinf)
+ {
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ //if(Prt[r*Ncb+t]!=0)
+ if(Prt[r*Ncb_+t]>seuilevitinf)
+ {
+ for(unsigned int it=0;it<nbParam;it++)
+ dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
+
+ dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
+ for(unsigned int it=0;it<nbParam;it++)
+ for(unsigned int jt=0;jt<nbParam;jt++)
+ if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_NEW)
+ Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t])+d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
+ else if(ApproxHessian==HESSIAN_NEW)
+ Hessian[it][jt]+=d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
+ else
+ Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t]);
+ /*std::cout<<"Prt[r*Ncb+t]="<<Prt[r*Ncb+t]<<std::endl;
+ std::cout<<"Pt[t]="<<Pt[t]<<std::endl;
+
+ std::cout<<"Hessian="<<Hessian<<std::endl;*/
+ }
+ }
+ }
+ }
+ // std::cout<<"Hessian="<<Hessian<<std::endl;
+}
+
+void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
+{
+ double seuilevitinf=1e-200;
+ //double dtemp;
+ double u=0,v=0,B=0;
+ double *du = new double[nbParam];
+ double *dv = new double[nbParam];
+ double *A = new double[nbParam];
+ double *dB = new double[nbParam];
+ double **d2u = new double *[nbParam];
+ double **d2v = new double *[nbParam];
+ double **dA = new double *[nbParam];
+ for (unsigned int i = 0; i < nbParam; i++) {
+ d2u[i] = new double[nbParam];
+ d2v[i] = new double[nbParam];
+ dA[i] = new double[nbParam];
+ }
+
+ memset(du, 0, nbParam*sizeof(double));
+ memset(dv, 0, nbParam*sizeof(double));
+ memset(A, 0, nbParam*sizeof(double));
+ memset(dB, 0, nbParam*sizeof(double));
+ memset(dprtemp, 0, nbParam*sizeof(double));
+
+ for(unsigned int it=0;it<nbParam;it++){
+ for(unsigned int jt=0;jt<nbParam;jt++){
+ memset(d2u[it], 0, nbParam*sizeof(double));
+ memset(d2v[it], 0, nbParam*sizeof(double));
+ memset(dA[it], 0, nbParam*sizeof(double));
+ }
+ }
+
+ unsigned int Ncb_ = (unsigned int)Ncb;
+
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ //if(Pt[t]!=0)
+ if(Pt[t]>seuilevitinf)
+ {
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ //if(Prt[r*Ncb+t]!=0)
+ if(Prt[r*Ncb_+t]>seuilevitinf)
+ {
+ for(unsigned int it=0;it<nbParam;it++){
+ // dPxy/dt
+ dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
+ }
+ //dtemp=1.+log(Prt[r*Ncb+t]/Pt[t]);
+ // u = som(Pxy.logPxPy)
+ u += Prt[r*Ncb_+t]*log(Pt[t]*Pr[r]);
+ // v = som(Pxy.logPxy)
+ v += Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+
+ for(unsigned int it=0;it<nbParam;it++){
+ // u' = som dPxylog(PxPy)
+ du[it] += dprtemp[it]*log(Pt[t]*Pr[r]);
+ // v' = som dPxy(1+log(Pxy))
+ dv[it] += dprtemp[it]*(1+log(Prt[r*Ncb_+t]));
+
+ }
+ for(unsigned int it=0;it<nbParam;it++){
+ for(unsigned int jt=0;jt<nbParam;jt++){
+ d2u[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*log(Pt[t]*Pr[r])
+ + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
+ d2v[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*(1+log(Prt[r*Ncb_+t]))
+ + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
+ }
+ }
+ }
+ }
+ }
+ }
+ // B = v2
+ B = (v*v);
+ for(unsigned int it=0;it<nbParam;it++){
+ // A = u'v-uv'
+ A[it] = du[it] * v - u * dv[it];
+ // B' = 2vv'
+ dB[it] = 2 * v * dv[it];
+ // G = (u'v-v'u)/v2
+ // G[it] = A[it]/B;
+ for(unsigned int jt=0;jt<nbParam;jt++){
+ // A' = u''v-v''u
+ dA[it][jt] = d2u[it][jt]*v-d2v[it][jt]*u;
+ // Hessian = (A'B-AB')/B2
+ Hessian[it][jt] = (dA[it][jt] * B -A[it] * dB[it])/(B*B);
+ }
+ }
+
+ delete[] du;
+ delete[] dv;
+ delete[] A;
+ delete[] dB;
+ for (unsigned int i = 0; i < nbParam; i++) {
+ delete[] d2u[i];
+ delete[] d2v[i];
+ delete[] dA[i];
+ }
+ delete[] d2u;
+ delete[] d2v;
+ delete[] dA;
+
+ // std::cout<<"Hdes - compute Hessien\n"<<u<<"\n"<<v<<"\n"<<du[0]<<" "<<du[1]<<" "<<du[2]<<"\n"<<dv[2]<<"\n"<<d2u[2][2]<<"\n"<<d2v[2][2]<<"\n"<<H<<std::endl;
+}
+
+
+void vpTemplateTrackerMI::computeGradient()
+{
+
+ double seuilevitinf=1e-200;
+ G=0;
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ double dtemp;
+ for(unsigned int t=0;t<Ncb_;t++)
+ {
+ //if(Pt[t]!=0)
+ if(Pt[t]>seuilevitinf)
+ {
+ for(unsigned int r=0;r<Ncb_;r++)
+ {
+ if(Prt[r*Ncb_+t]>seuilevitinf)
+ //if(Prt[r*Ncb+t]!=0)
+ {
+ for(unsigned int it=0;it<nbParam;it++)
+ dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
+
+ dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
+
+ for(unsigned int it=0;it<nbParam;it++)
+ G[it]+=dtemp*dprtemp[it];
+ }
+ }
+ }
+ }
+
+}
+void vpTemplateTrackerMI::zeroProbabilities()
+{
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ unsigned int Nc_ = (unsigned int)Nc;
+ unsigned int influBspline_ = (unsigned int)influBspline;
+
+ memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
+ memset(dPrt, 0, Ncb_*Ncb_*nbParam*sizeof(double));
+ memset(d2Prt, 0, Ncb_*Ncb_*nbParam*nbParam*sizeof(double));
+ memset(PrtTout, 0, Nc_*Nc_*influBspline_*(1+nbParam+nbParam*nbParam)*sizeof(double));
+
+ // std::cout << Ncb*Ncb << std::endl;
+ // std::cout << Ncb*Ncb*nbParam << std::endl;
+ // std::cout << Ncb*Ncb*nbParam*nbParam << std::endl;
+ // std::cout << Ncb*Ncb*influBspline*(1+nbParam+nbParam*nbParam) << std::endl;
+}
+
+double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline_,vpColVector &tp)
+{
+ int tNcb=nc+bspline_;
+ int tinfluBspline=bspline_*bspline_;
+ double *tPrtD = new double[nc*nc*tinfluBspline];
+ double *tPrt = new double[tNcb*tNcb];
+ double *tPr = new double[tNcb];
+ double *tPt = new double[tNcb];
+
+ double MI=0;
+ int Nbpoint=0;
+ double i2,j2;
+ double Tij;
+ double IW;
+
+ vpImage<double> GaussI ;
+ vpImageFilter::filter(I, GaussI,fgG,taillef);
+
+ int i,j;
+ int cr,ct;
+ double er,et;
+ memset(tPrt, 0, tNcb*tNcb*sizeof(double));
+ memset(tPrtD, 0, nc*nc*tinfluBspline*sizeof(double));
+
+ //Warp->ComputeMAtWarp(tp);
+ Warp->computeCoeff(tp);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+ j2=X2[0];i2=X2[1];
+
+ //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
+ {
+ Nbpoint++;
+
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=GaussI.getValue(i2,j2);
+
+ cr=(int)((IW*(nc-1))/255.);
+ ct=(int)((Tij*(nc-1))/255.);
+ er=(IW*(nc-1))/255.-cr;
+ et=((double)Tij*(nc-1))/255.-ct;
+
+ //Calcul de l'histogramme joint par interpolation bilineaire (Bspline_ ordre 1)
+ vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1.,bspline_);
+ }
+ }
+ double *pt=tPrtD;
+ for(int r=0;r<nc;r++)
+ for(int t=0;t<nc;t++)
+ {
+ for(i=0;i<tinfluBspline;i++)
+ {
+ int r2,t2;
+ r2=r+i/bspline_;
+ t2=t+i%bspline_;
+ tPrt[r2*tNcb+t2]+=*pt;
+
+ pt++;
+ }
+ }
+
+ if (Nbpoint == 0) {
+ delete[] tPrtD;
+ delete[] tPrt;
+ delete[] tPr;
+ delete[] tPt;
+
+ return 0;
+ }
+ else
+ {
+ for(int r=0;r<tNcb;r++)
+ for(int t=0;t<tNcb;t++)
+ //printf("%f ",tPrt[r*tNcb+t]);
+ tPrt[r*tNcb+t]=tPrt[r*tNcb+t]/Nbpoint;
+ //calcul Pr;
+ memset(tPr, 0, tNcb*sizeof(double));
+ for(int r=0;r<tNcb;r++)
+ {
+ for(int t=0;t<tNcb;t++)
+ tPr[r]+=tPrt[r*tNcb+t];
+ }
+
+ //calcul Pt;
+ memset(tPt, 0, tNcb*sizeof(double));
+ for(int t=0;t<tNcb;t++)
+ {
+ for(int r=0;r<tNcb;r++)
+ tPt[t]+=tPrt[r*tNcb+t];
+ }
+ for(int r=0;r<tNcb;r++)
+ //if(tPr[r]!=0)
+ if(std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
+ MI-=tPr[r]*log(tPr[r]);
+
+ for(int t=0;t<tNcb;t++)
+ //if(tPt[t]!=0)
+ if(std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
+ MI-=tPt[t]*log(tPt[t]);
+
+ for(int r=0;r<tNcb;r++)
+ for(int t=0;t<tNcb;t++)
+ //if(tPrt[r*tNcb+t]!=0)
+ if(std::fabs(tPrt[r*tNcb+t]) > std::numeric_limits<double>::epsilon())
+ MI+=tPrt[r*tNcb+t]*log(tPrt[r*tNcb+t]);
+ }
+ delete[] tPrtD;
+ delete[] tPrt;
+ delete[] tPr;
+ delete[] tPt;
+
+ return MI;
+}
+
+double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
+{
+ vpMatrix Prt256(256,256);Prt256=0;
+ vpColVector Pr256(256);Pr256=0;
+ vpColVector Pt256(256);Pt256=0;
+
+ int Nbpoint=0;
+ double i2,j2;
+ unsigned int Tij,IW;
+
+ int i,j;
+ vpImage<double> GaussI ;
+ if(blur)
+ vpImageFilter::filter(I, GaussI,fgG,taillef);
+
+ //Warp->ComputeMAtWarp(tp);
+ Warp->computeCoeff(tp);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,tp);
+ Warp->warpX(X1,X2,tp);
+ j2=X2[0];i2=X2[1];
+
+ //Tij=Templ[i-(int)Triangle->GetMiny()][j-(int)Triangle->GetMinx()];
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth())-1)
+ {
+ Nbpoint++;
+
+ Tij=(unsigned int)ptTemplate[point].val;
+ if(!blur)
+ IW=(unsigned int)I.getValue(i2,j2);
+ else
+ IW=(unsigned int)GaussI.getValue(i2,j2);
+
+ Prt256[Tij][IW]++;
+ Pr256[Tij]++;
+ Pt256[IW]++;
+ }
+ }
+
+ if (Nbpoint == 0){
+ throw(vpException(vpException::divideByZeroError,
+ "Cannot get MI; number of points = 0"));
+ }
+ Prt256=Prt256/Nbpoint;
+ Pr256=Pr256/Nbpoint;
+ Pt256=Pt256/Nbpoint;
+
+ double MI=0;
+
+ for(unsigned int t=0;t<256;t++)
+ {
+ for(unsigned int r=0;r<256;r++)
+ {
+ //if(Prt256[r][t]!=0)
+ if(std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
+ MI+=Prt256[r][t]*log(Prt256[r][t]);
+ }
+ //if(Pt256[t]!=0)
+ if(std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
+ MI+=-Pt256[t]*log(Pt256[t]);
+ //if(Pr256[t]!=0)
+ if(std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
+ MI+=-Pr256[t]*log(Pr256[t]);
+
+ }
+ return MI;
+}
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp
new file mode 100644
index 0000000..74b879e
--- /dev/null
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp
@@ -0,0 +1,492 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
+
+#ifdef VISP_HAVE_OPENMP
+#include <omp.h>
+#endif
+
+vpTemplateTrackerMIESM::vpTemplateTrackerMIESM(vpTemplateTrackerWarp *_warp)
+ : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false),
+ HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
+{
+ useCompositionnal=false;
+ useInverse=false;
+ if(!Warp->isESMcompatible())
+ std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
+}
+
+void vpTemplateTrackerMIESM::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompInverse();
+ std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+
+ dW=0;
+
+ double erreur=0;
+ int Nbpoint=0;
+
+ double i2,j2;
+ double Tij;
+ double IW,dx,dy;
+ //int cr,ct;
+ //double er,et;
+ int i,j;
+
+ Nbpoint=0;
+ erreur=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+// if(blur)
+// IW=BI.getValue(i2,j2);
+// else
+// IW=I.getValue(i2,j2);
+
+ //ct=ptTemplateSupp[point].ct;
+ //et=ptTemplateSupp[point].et;
+ //cr=(int)((IW*(Nc-1))/255.);
+ //er=((double)IW*(Nc-1))/255.-cr;
+
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // else
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,false);
+ }
+ }
+
+
+ double MI;
+ computeProba(Nbpoint);
+ computeMI(MI);
+ computeHessien(HdesireInverse);
+ //std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;
+
+ /////////////////////////////////////////////////////////////////////////
+ // DIRECT COMPO
+
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
+ {
+ vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
+ vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
+ vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
+ }
+
+ Nbpoint=0;
+ erreur=0;
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight())&&(j2<I.getWidth()))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ //cr=ptTemplateSupp[point].ct;
+ //er=ptTemplateSupp[point].et;
+ //ct=(int)((IW*(Nc-1))/255.);
+ //et=((double)IW*(Nc-1))/255.-ct;
+
+ Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+
+ double *tptemp=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+
+
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,
+ // hessianComputation== vpTemplateTrackerMI::USE_HESSIEN_DESIRE);
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ // else
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,false);
+
+ delete[] tptemp;
+ }
+ }
+
+ computeProba(Nbpoint);
+ computeMI(MI);
+ computeHessien(HdesireDirect);
+ //std::cout<<"HdesireDirect"<<std::endl<<HdesireDirect<<std::endl;
+
+ lambda=lambdaDep;
+
+ Hdesire=HdesireDirect+HdesireInverse;
+ //Hdesire=HdesireDirect;
+ //Hdesire=HdesireInverse;
+
+ //std::cout<<"HdesireDirect "<<HdesireDirect<<std::endl;
+ //std::cout<<"HdesireInverse "<<HdesireInverse<<std::endl;
+ vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
+ HLMdesireInverse=HLMdesire.inverseByLU();
+
+ //std::cout<<"\tEnd initialisation..."<<std::endl;
+
+}
+
+void vpTemplateTrackerMIESM::initCompInverse()
+{
+ //std::cout<<"Initialise precomputed value of ESM Tracker"<<std::endl;
+
+ HDirect.resize(nbParam,nbParam);
+ HInverse.resize(nbParam,nbParam);
+ HdesireDirect.resize(nbParam,nbParam);
+ HdesireInverse.resize(nbParam,nbParam);
+ GDirect.resize(nbParam);
+ GInverse.resize(nbParam);
+
+ ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ ptTemplateCompo=new vpTemplateTrackerPointCompo[templateSize];
+ int i,j;
+ double et;
+ int ct;
+ double Tij;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+
+ ptTemplateCompo[point].dW=new double[2*nbParam];
+ Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
+
+ ptTemplate[point].dW=new double[nbParam];
+ double dx=ptTemplate[point].dx*(Nc-1)/255.;
+ double dy=ptTemplate[point].dy*(Nc-1)/255.;
+ Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
+
+ Tij=ptTemplate[point].val;
+ ct=(int)((Tij*(Nc-1))/255.);
+ et=(Tij*(Nc-1))/255.-ct;
+ ptTemplateSupp[point].et=et;
+ ptTemplateSupp[point].ct=ct;
+ ptTemplateSupp[point].Bt=new double[4];
+ ptTemplateSupp[point].dBt=new double[4];
+ for(char it=-1;it<=2;it++)
+ {
+ ptTemplateSupp[point].Bt[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ ptTemplateSupp[point].dBt[it+1] =vpTemplateTrackerMIBSpline::dBspline4(-it+et);
+ }
+ }
+ CompoInitialised=true;
+}
+
+void vpTemplateTrackerMIESM::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ if(!CompoInitialised)
+ std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
+ dW=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ /* if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
+ {
+ getGradX(dIx, d2Ix,fgdG,taillef);
+ getGradY(dIx, d2Ixy,fgdG,taillef);
+ getGradY(dIy, d2Iy,fgdG,taillef);
+ }*/
+
+ double erreur=0;
+ int Nbpoint=0;
+ int point;
+
+ MI_preEstimation=-getCost(I,p);
+
+ lambda=lambdaDep;
+ double MI=0;
+ //double MIprec=-1000;
+
+ double i2,j2;
+ double Tij;
+ double IW;
+ //int cr,ct;
+ //double er,et;
+
+ vpColVector dpinv(nbParam);
+
+ double dx,dy;
+ double alpha=2.;
+
+ int i,j;
+ unsigned int iteration=0;
+ do
+ {
+ Nbpoint=0;
+ //MIprec=MI;
+ MI=0;
+ erreur=0;
+
+ zeroProbabilities();
+
+ /////////////////////////////////////////////////////////////////////////
+ // Inverse
+ Warp->computeCoeff(p);
+ for(point=0;point<(int)templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ //Tij=ptTemplate[point].val;
+ //if(!blur)
+ // IW=I.getValue(i2,j2);
+ //else
+ // IW=BI.getValue(i2,j2);
+
+ //ct=ptTemplateSupp[point].ct;
+ //et=ptTemplateSupp[point].et;
+ //cr=(int)((IW*(Nc-1))/255.);
+ //er=((double)IW*(Nc-1))/255.-cr;
+
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // else
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ }
+ }
+
+ if(Nbpoint==0)
+ {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge=true;
+ MI=0;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+ else
+ {
+ computeProba(Nbpoint);
+ computeMI(MI);
+ if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ computeHessien(HInverse);
+ computeGradient();
+ GInverse=G;
+
+ /////////////////////////////////////////////////////////////////////////
+ // DIRECT
+
+ Nbpoint=0;
+ //MIprec=MI;
+ MI=0;
+ erreur=0;
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+#ifdef VISP_HAVE_OPENMP
+ int nthreads = omp_get_num_procs() ;
+ //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
+ omp_set_num_threads(nthreads);
+#pragma omp parallel for private(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
+#endif
+ for(point=0;point<(int)templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ Warp->warpX(i,j,i2,j2,p);
+ X2[0]=j2;X2[1]=i2;
+
+ //Warp->computeDenom(X1,p);
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ //Tij=Iterateurvecteur->val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ //ct=(int)((IW*(Nc-1))/255.);
+ //et=((double)IW*(Nc-1))/255.-ct;
+ //cr=ptTemplateSupp[point].ct;
+ //er=ptTemplateSupp[point].et;
+
+ Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+
+ double *tptemp=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+
+
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+ // if(bspline==3)
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
+ // else
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
+ // hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);
+
+ delete[] tptemp;
+
+ }
+ }
+
+ computeProba(Nbpoint);
+ computeMI(MI);
+ if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ computeHessien(HDirect);
+ computeGradient();
+ GDirect=G;
+
+ if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ {
+ H=HDirect+HInverse;
+ vpMatrix::computeHLM(H,lambda,HLM);
+ }
+ G=GDirect-GInverse;
+ //G=GDirect;
+ //G=-GInverse;
+
+ try
+ {
+ if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
+ dp=-gain*0.3*G;
+ else
+ {
+ switch(hessianComputation)
+ {
+ case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
+ dp=gain*HLMdesireInverse*G;
+ break;
+ case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
+ if(HLM.cond()>HLMdesire.cond())
+ dp=gain*HLMdesireInverse*G;
+ else
+ dp=gain*0.3*HLM.inverseByLU()*G;
+ break;
+ default:
+ dp=gain*0.3*HLM.inverseByLU()*G;
+ break;
+ }
+ }
+ }
+ catch(vpException &e)
+ {
+ //std::cerr<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ dp=-1.*dp;
+
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,-MI,dp,alpha);
+ dp=alpha*dp;
+ }
+ p+=dp;
+
+ iteration++;
+ }
+ }
+ while( /*(MI!=MIprec) &&*/(iteration< iterationMax));
+
+ MI_postEstimation=-getCost(I,p);
+ if(MI_preEstimation>MI_postEstimation)
+ {
+ MI_postEstimation = -1;
+ }
+
+ nbIteration=iteration;
+}
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp
new file mode 100644
index 0000000..3d7f16f
--- /dev/null
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp
@@ -0,0 +1,436 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
+
+#ifdef VISP_HAVE_OPENMP
+#include <omp.h>
+#endif
+
+vpTemplateTrackerMIForwardAdditional::vpTemplateTrackerMIForwardAdditional(vpTemplateTrackerWarp *_warp)
+ : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
+ threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
+{
+ evolRMS = 0;
+ x_pos = y_pos = NULL;
+ useCompositionnal=false;
+ threshold_RMS=1e-20;
+ minimizationMethod=USE_NEWTON;
+}
+
+void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+
+ dW=0;
+
+ int Nbpoint=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ double i2,j2;
+ double Tij;
+ double IW,dx,dy;
+ int cr,ct;
+ double er,et;
+
+ int i,j;
+
+ Nbpoint=0;
+
+ zeroProbabilities();
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ X2[0]=j;X2[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ ct=(int)((IW*(Nc-1))/255.);
+ cr=(int)((Tij*(Nc-1))/255.);
+ et=(IW*(Nc-1))/255.-ct;
+ er=((double)Tij*(Nc-1))/255.-cr;
+ //std::cout<<"test"<<std::endl;
+ Warp->dWarp(X1,X2,p,dW);
+
+ double *tptemp=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
+ vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+
+ delete[] tptemp;
+ }
+ }
+
+ if(Nbpoint>0)
+ {
+ double MI;
+ computeProba(Nbpoint);
+ computeMI(MI);
+ computeHessien(Hdesire);
+
+
+ // double conditionnement=GetConditionnement(Hdesire);
+ // std::cout<<"conditionnement : "<<conditionnement<<std::endl;
+ vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
+ try
+ {
+ HLMdesireInverse=HLMdesire.inverseByLU();
+ }
+ catch(vpException &e)
+ {
+ //std::cerr<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+ //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+ //std::cout<<"\tEnd initialisation..."<<std::endl;
+ }
+
+}
+
+void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ dW=0;
+
+ double erreur=0;
+ int Nbpoint=0;
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ double MI=0,MIprec=-1000;
+
+ MI_preEstimation=-getCost(I,p);
+
+ double i2,j2;
+ double Tij;
+ double IW,dx,dy;
+ //unsigned
+ int cr,ct;
+ double er,et;
+ double alpha=2.;
+
+ int i,j;
+ unsigned int iteration=0;
+
+ initPosEvalRMS(p);
+ do
+ {
+ if(iteration%5==0)
+ initHessienDesired(I);
+ Nbpoint=0;
+ MIprec=MI;
+ MI=0;
+ erreur=0;
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+#ifdef VISP_HAVE_OPENMP
+ int nthreads = omp_get_num_procs() ;
+ //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
+ omp_set_num_threads(nthreads);
+#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
+#endif
+ for(int point=0;point<(int)templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ //Tij=Iterateurvecteur->val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ ct=(int)((IW*(Nc-1))/255.);
+ cr=(int)((Tij*(Nc-1))/255.);
+ et=(IW*(Nc-1))/255.-ct;
+ er=((double)Tij*(Nc-1))/255.-cr;
+
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+
+ //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
+ Warp->dWarp(X1,X2,p,dW);
+
+ //double *tptemp=temp;
+ double *tptemp=new double[nbParam];;
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
+ //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
+ //std::cout<<cr<<" "<<ct<<" ; ";
+ if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
+ vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+
+ delete[] tptemp;
+ }
+ }
+
+ if(Nbpoint==0)
+ {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge=true;
+ MI=0;
+ deletePosEvalRMS();
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+ else
+ {
+ computeProba(Nbpoint);
+ computeMI(MI);
+ //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
+ computeHessien(H);
+ computeGradient();
+
+ vpMatrix::computeHLM(H,lambda,HLM);
+ try
+ {
+ switch(hessianComputation)
+ {
+ case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
+ dp=gain*HLMdesireInverse*G;
+ break;
+ case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
+ if(HLM.cond()>HLMdesire.cond())
+ dp=gain*HLMdesireInverse*G;
+ else
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ default:
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ }
+ }
+ catch(vpException &e)
+ {
+ //std::cerr<<"probleme inversion"<<std::endl;
+ deletePosEvalRMS();
+ throw(e);
+ }
+ }
+
+ switch(minimizationMethod)
+ {
+ case vpTemplateTrackerMIForwardAdditional::USE_LMA:
+ {
+ vpColVector p_test_LMA(nbParam);
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ p_test_LMA=p-100000.1*dp;
+ else
+ p_test_LMA=p+1.*dp;
+ MI=-getCost(I,p);
+ double MI_LMA=-getCost(I,p_test_LMA);
+ if(MI_LMA>MI)
+ {
+ p=p_test_LMA;
+ lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
+ }
+ else
+ {
+ lambda=(lambda*10.<1e6)?1e6:lambda*10.;
+ }
+ }
+ break;
+ case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
+ {
+ dp=-gain*6.0*G;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,-MI,dp,alpha);
+ dp=alpha*dp;
+ }
+ p+=1.*dp;
+ break;
+ }
+
+ case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
+ {
+ double s_scal_y;
+ if(iterationGlobale!=0)
+ {
+ vpColVector s_quasi=p-p_prec;
+ vpColVector y_quasi=G-G_prec;
+ s_scal_y=s_quasi.t()*y_quasi;
+ //if(s_scal_y!=0)//BFGS
+ // KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
+ //if(s_scal_y!=0)//DFP
+ if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
+ KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
+ }
+ dp=-KQuasiNewton*G;
+ p_prec=p;
+ G_prec=G;
+ p-=1.01*dp;
+ }
+ break;
+
+ default:
+ {
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ dp=-0.1*dp;
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,-MI,dp,alpha);
+ //std::cout<<alpha<<std::endl;
+ dp=alpha*dp;
+ }
+
+ p+=1.*dp;
+ break;
+ }
+ }
+
+ computeEvalRMS(p);
+ iteration++;
+ iterationGlobale++;
+
+ }
+ while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
+ //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
+ if(Nbpoint==0) {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ deletePosEvalRMS();
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+
+ nbIteration=iteration;
+ MI_postEstimation=-getCost(I,p);
+ if(MI_preEstimation>MI_postEstimation)
+ {
+ MI_postEstimation = -1;
+ }
+ deletePosEvalRMS();
+}
+
+void vpTemplateTrackerMIForwardAdditional::initPosEvalRMS(const vpColVector &pw)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+ x_pos=new double[nb_corners];
+ y_pos=new double[nb_corners];
+
+ Warp->computeCoeff(pw);
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,pw);
+ Warp->warpX(X1,X2,pw);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+}
+
+void vpTemplateTrackerMIForwardAdditional::computeEvalRMS(const vpColVector &pw)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+
+ Warp->computeCoeff(pw);
+ evolRMS=0;
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,pw);
+ Warp->warpX(X1,X2,pw);
+ evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+ evolRMS=evolRMS/nb_corners;
+}
+
+void vpTemplateTrackerMIForwardAdditional::deletePosEvalRMS()
+{
+ delete[] x_pos;
+ delete[] y_pos;
+}
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp
new file mode 100644
index 0000000..6d37c0d
--- /dev/null
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
+
+vpTemplateTrackerMIForwardCompositional::vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp)
+ : vpTemplateTrackerMI(_warp), CompoInitialised(false)
+{
+}
+
+void vpTemplateTrackerMIForwardCompositional::initCompo()
+{
+ std::cout<<"Initialise precomputed value of Compositionnal Direct"<<std::endl;
+ ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ int i,j;
+ double et;
+ int ct;
+ double Tij;
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->computeDenom(X1,p);
+ ptTemplate[point].dW=new double[2*nbParam];
+ Warp->getdWdp0(i,j,ptTemplate[point].dW);
+
+ Tij=ptTemplate[point].val;
+ ct=(int)((Tij*(Nc-1))/255.);
+ et=(Tij*(Nc-1))/255.-ct;
+ ptTemplateSupp[point].et=et;
+ ptTemplateSupp[point].ct=ct;
+ ptTemplateSupp[point].Bt=new double[4];
+ ptTemplateSupp[point].dBt=new double[4];
+ for(char it=-1;it<=2;it++)
+ {
+ ptTemplateSupp[point].Bt[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ ptTemplateSupp[point].dBt[it+1] =vpTemplateTrackerMIBSpline::dBspline4(-it+et);
+ }
+
+ }
+ CompoInitialised=true;
+}
+void vpTemplateTrackerMIForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompo();
+
+ //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+
+ dW=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ double erreur=0;
+ int Nbpoint=0;
+
+ double i2,j2;
+ double Tij;
+ double IW,dx,dy;
+ int cr,ct;
+ double er,et;
+
+ int i,j;
+
+ Nbpoint=0;
+ erreur=0;
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ cr=ptTemplateSupp[point].ct;
+ er=ptTemplateSupp[point].et;
+ ct=(int)((IW*(Nc-1))/255.);
+ et=((double)IW*(Nc-1))/255.-ct;
+
+ Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+
+ double *tptemp=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+
+ vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc,tptemp, nbParam, bspline);
+
+ delete[] tptemp;
+ }
+ }
+ double MI;
+ computeProba(Nbpoint);
+ computeMI(MI);
+ computeHessien(Hdesire);
+
+ lambda=lambdaDep;
+
+ vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
+ HLMdesireInverse=HLMdesire.inverseByLU();
+ //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+ //std::cout<<"\tEnd initialisation..."<<std::endl;
+}
+
+
+void vpTemplateTrackerMIForwardCompositional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ if(!CompoInitialised)
+ std::cout<<"Compositionnal tracking no initialised\nUse initCompo(vpImage<unsigned char> &I) function"<<std::endl;
+ dW=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ double erreur=0;
+ int Nbpoint=0;
+
+
+ lambda=lambdaDep;
+ double MI=0,MIprec=-1000;
+
+ MI_preEstimation=-getCost(I,p);
+
+ double i2,j2;
+ double Tij;
+ double IW;
+ //unsigned
+ int cr,ct;
+ double er,et;
+ double dx,dy;
+
+ vpColVector dpinv(nbParam);
+ double alpha=2.;
+
+ int i,j;
+ unsigned int iteration=0;
+ do
+ {
+ Nbpoint=0;
+ MIprec=MI;
+ MI=0;
+ erreur=0;
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+ Warp->warpX(i,j,i2,j2,p);
+ X2[0]=j2;X2[1]=i2;
+
+ Warp->computeDenom(X1,p);
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+ if(!blur)
+ IW=I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
+ dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+
+ ct=(int)((IW*(Nc-1))/255.);
+ et=((double)IW*(Nc-1))/255.-ct;
+ cr=ptTemplateSupp[point].ct;
+ er=ptTemplateSupp[point].et;
+
+ Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+
+ double *tptemp=new double[nbParam];
+ for(unsigned int it=0;it<nbParam;it++)
+ tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+
+
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+
+ if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ else if(ApproxHessian==HESSIAN_0|| ApproxHessian==HESSIAN_NEW)
+ vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+
+ delete[] tptemp;
+
+ }
+ }
+ if(Nbpoint==0)
+ {
+ //std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge=true;
+ MI=0;
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+ }
+ else
+ {
+ computeProba(Nbpoint);
+ computeMI(MI);
+ if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ computeHessien(H);
+ computeGradient();
+
+ vpMatrix::computeHLM(H,lambda,HLM);
+
+ try
+ {
+ switch(hessianComputation)
+ {
+ case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
+ dp=gain*HLMdesireInverse*G;
+ break;
+ case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
+ if(HLM.cond()>HLMdesire.cond())
+ dp=gain*HLMdesireInverse*G;
+ else
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ default:
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ }
+ }
+ catch(vpException &e)
+ {
+ //std::cerr<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+ }
+
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ dp=-0.04*dp;
+ else
+ dp=1.*dp;
+
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,-MI,dp,alpha);
+ dp=alpha*dp;
+ }
+ Warp->pRondp(p,dp,p);
+
+ iteration++;
+
+
+ }
+ while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) && (iteration< iterationMax) );
+ //while( (MI!=MIprec) && (iteration< iterationMax) );
+ nbIteration=iteration;
+
+ MI_postEstimation=-getCost(I,p);
+ if(MI_preEstimation>MI_postEstimation)
+ {
+ MI_postEstimation = -1;
+ }
+}
+
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp
new file mode 100644
index 0000000..36c5b87
--- /dev/null
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp
@@ -0,0 +1,588 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of template tracking.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
+#include <visp3/core/vpTrackingException.h>
+
+#include <memory>
+
+vpTemplateTrackerMIInverseCompositional::vpTemplateTrackerMIInverseCompositional(vpTemplateTrackerWarp *_warp)
+ : vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
+ evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(1e-20), p_prec(), G_prec(), KQuasiNewton() //, useAYOptim(false)
+{
+ useInverse=true;
+}
+
+void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned int ptIndex, double &et) //AY : Optim
+{
+ ptTemplateSupp[ptIndex].BtInit=new double[(1 + nbParam + nbParam*nbParam)*(unsigned int)bspline];
+
+ unsigned int index = 0;
+ int endIndex = 1;
+
+ double (*ptBspFct)(double);
+ double (*ptdBspFct)(double);
+ double (*ptd2BspFct)(double);
+ if(bspline == 3){
+ if(et>0.5){et=et-1;}
+ ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
+ ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
+ ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
+ }
+ else{
+ ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
+ ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
+ ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
+ endIndex = 2;
+ }
+
+ for(int it=-1; it<=endIndex; it++)
+ {
+ ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it)+et);
+
+ for(unsigned int ip=0;ip<nbParam;++ip)
+ {
+ ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
+ for(unsigned int ip2=0;ip2<nbParam;++ip2)
+ {
+ ptTemplateSupp[ptIndex].BtInit[index++] = (*ptd2BspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMIInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
+{
+ ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ int i,j;
+ double et;
+ int ct;
+ double Tij;
+
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+
+ if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
+ {
+ vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
+ vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
+ vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
+ }
+
+ Warp->computeCoeff(p);
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ ptTemplate[point].dW=new double[nbParam];
+
+ double dx=ptTemplate[point].dx*(Nc-1)/255.;
+ double dy=ptTemplate[point].dy*(Nc-1)/255.;
+
+ Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
+ Tij=ptTemplate[point].val;
+ ct=(int)((Tij*(Nc-1))/255.);
+ et=(Tij*(Nc-1))/255.-ct;
+
+ ptTemplateSupp[point].et=et;
+ ptTemplateSupp[point].ct=ct;
+
+ // ###### AY Optim
+ // if(useAYOptim)
+ // if(ApproxHessian != HESSIAN_NONSECOND /*&& hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE*/)
+ // initTemplateRefBspline(point, et);
+ // ###################
+ }
+ CompoInitialised=true;
+
+}
+void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
+{
+ initCompInverse(I);
+
+ double erreur=0;
+ int Nbpoint=0;
+
+ double i2,j2;
+ double Tij;
+ double IW;
+ int cr,ct;
+ double er,et;
+
+ int i,j;
+
+ Nbpoint=0;
+ erreur=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+
+ zeroProbabilities();
+ Warp->computeCoeff(p);
+
+ // AY : Optim
+ // unsigned int totParam = (bspline * bspline)*(1+nbParam+nbParam*nbParam);
+ // unsigned int size = (1 + nbParam + nbParam*nbParam)*bspline;
+ // double *ptb;
+
+ for(unsigned int point=0;point<templateSize;point++)
+ {
+ i=ptTemplate[point].y;
+ j=ptTemplate[point].x;
+ X1[0]=j;X1[1]=i;
+
+ Warp->computeDenom(X1,p);
+ Warp->warpX(X1,X2,p);
+
+ j2=X2[0];i2=X2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+ Nbpoint++;
+ Tij=ptTemplate[point].val;
+
+ if(blur)
+ IW=BI.getValue(i2,j2);
+ else
+ IW=I.getValue(i2,j2);
+
+ ct=ptTemplateSupp[point].ct;
+ et=ptTemplateSupp[point].et;
+ cr=(int)((IW*(Nc-1))/255.);
+ er=((double)IW*(Nc-1))/255.-cr;
+
+ //calcul de l'erreur
+ erreur+=(Tij-IW)*(Tij-IW);
+
+ if( ApproxHessian==HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect) )
+ {
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ }
+ else if ((ApproxHessian==HESSIAN_0||ApproxHessian==HESSIAN_NEW) && (ptTemplateSelect[point] || !useTemplateSelect))
+ {
+ if(bspline==3){
+ vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
+ // {
+ // if(et>0.5){ct++;}
+ // if(er>0.5){cr++;}
+ // int index = (cr*Nc+ct)*totParam;
+ // double *ptb = &PrtTout[index];
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
+ // }
+ }
+ else{
+ vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
+
+ // {
+ // // ################### AY : Optim
+ // unsigned int index = (cr*Nc+ct)*totParam;
+ // ptb = &PrtTout[index];
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb, er, ptTemplateSupp[point].BtInit, size);
+ // // ###################
+ // }
+ }
+ }
+ else if (ptTemplateSelect[point] || !useTemplateSelect)
+ vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc,nbParam, bspline);
+ }
+ }
+
+ double MI;
+ computeProba(Nbpoint);
+ computeMI(MI);
+ computeHessien(Hdesire);
+
+ lambda=lambdaDep;
+
+ vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
+
+ HLMdesireInverse=HLMdesire.inverseByLU();
+ KQuasiNewton=HLMdesireInverse;
+}
+
+void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
+{
+ if(!CompoInitialised)
+ std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
+ dW=0;
+
+ if(blur)
+ vpImageFilter::filter(I, BI,fgG,taillef);
+
+ int Nbpoint=0;
+
+ lambda=lambdaDep;
+ double MI=0,MIprec=-1000;
+
+ vpColVector p_avant_estimation;p_avant_estimation=p;
+ MI_preEstimation=-getCost(I,p);
+ NMI_preEstimation=-getNormalizedCost(I,p);
+
+ // std::cout << "MI avant: " << MI_preEstimation << std::endl;
+ // std::cout << "NMI avant: " << NMI_preEstimation << std::endl;
+
+ initPosEvalRMS(p);
+
+ vpColVector dpinv(nbParam);
+ double alpha=2.;
+
+ unsigned int iteration=0;
+
+ //unsigned int bspline_ = (unsigned int) bspline;
+ //unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);
+
+ vpMatrix Hnorm(nbParam,nbParam);
+
+ do
+ {
+ Nbpoint=0;
+ MIprec=MI;
+ MI=0;
+
+ zeroProbabilities();
+
+ Warp->computeCoeff(p);
+
+ {
+ for(int point=0;point<(int)templateSize;point++)
+ {
+ vpColVector x1(2),x2(2);
+ double i2,j2;
+ double IW;
+ int cr,ct;
+ double er,et;
+
+ x1[0]=(double)ptTemplate[point].x;
+ x1[1]=(double)ptTemplate[point].y;
+
+ Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
+ Warp->warpX(x1,x2,p);
+
+ j2=x2[0];
+ i2=x2[1];
+
+ if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
+ {
+
+ //if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
+ {
+ Nbpoint++;
+ if(!blur)
+ IW=(double)I.getValue(i2,j2);
+ else
+ IW=BI.getValue(i2,j2);
+
+ ct=ptTemplateSupp[point].ct;
+ et=ptTemplateSupp[point].et;
+ double tmp = IW*(((double)Nc)-1.f)/255.f;
+ cr=(int)tmp;
+ er=tmp-(double)cr;
+
+ if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
+ {
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
+ }
+ else if (ptTemplateSelect[point] || !useTemplateSelect)
+ {
+ if(bspline==3){
+ vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
+ }
+ else{
+ vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
+ }
+ }
+ else{
+ vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
+ }
+ }
+
+ }
+ }
+ }
+
+ if(Nbpoint==0)
+ {
+ diverge=true;
+ MI=0;
+ deletePosEvalRMS();
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
+
+ }
+ else
+ {
+ // computeProba(Nbpoint);
+
+ unsigned int indd, indd2;
+ indd = indd2 = 0;
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ for(unsigned int i=0;i<Ncb_*Ncb_;i++){
+ Prt[i]=Prt[i]/Nbpoint;
+ for(unsigned int j=0;j<nbParam;j++){
+ dPrt[indd]=dPrt[indd]/Nbpoint;
+ indd++;
+ for(unsigned int k=0;k<nbParam;k++){
+ d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
+ indd2++;
+ }
+ }
+ }
+
+ computeMI(MI);
+
+ if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
+ computeHessienNormalized(Hnorm);
+ computeHessien(H);
+ }
+ computeGradient();
+
+ vpMatrix::computeHLM(H,lambda,HLM);
+
+ try
+ {
+ switch(hessianComputation)
+ {
+ case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
+ dp=gain*HLMdesireInverse*G;
+ break;
+ case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
+ if(HLM.cond()>HLMdesire.cond())
+ dp=gain*HLMdesireInverse*G;
+ else
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ default:
+ dp=gain*0.2*HLM.inverseByLU()*G;
+ break;
+ }
+ }
+ catch(vpException &e)
+ {
+ //std::cerr<<"probleme inversion"<<std::endl;
+ throw(e);
+ }
+ }
+
+ switch(minimizationMethod)
+ {
+ case vpTemplateTrackerMIInverseCompositional::USE_LMA:
+ {
+ vpColVector dp_test_LMA(nbParam);
+ vpColVector dpinv_test_LMA(nbParam);
+ vpColVector p_test_LMA(nbParam);
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ dp_test_LMA=-100000.1*dp;
+ else
+ dp_test_LMA=1.*dp;
+ Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
+ Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);
+
+ MI=-getCost(I,p);
+ double MI_LMA=-getCost(I,p_test_LMA);
+ if(MI_LMA>MI)
+ {
+ dp=dp_test_LMA;
+ lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
+ }
+ else
+ {
+ dp=0;
+ lambda=(lambda*10.<1e6)?1e6:lambda*10.;
+ }
+ }
+ break;
+ case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
+ dp=-gain*0.3*G*20;
+ break;
+
+ case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
+ {
+ double s_scal_y;
+ if(iterationGlobale!=0)
+ {
+ vpColVector s_quasi=p-p_prec;
+ vpColVector y_quasi=G-G_prec;
+ s_scal_y=s_quasi.t()*y_quasi;
+ //std::cout<<"mise a jour K"<<std::endl;
+ /*if(s_scal_y!=0)//BFGS
+ KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
+ //if(s_scal_y!=0)//DFP
+ if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
+ {
+ KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
+ //std::cout<<"mise a jour K"<<std::endl;
+ }
+ }
+ dp=gain*KQuasiNewton*G;
+ //std::cout<<KQuasiNewton<<std::endl<<std::endl;
+ p_prec=p;
+ G_prec=G;
+ //p-=1.01*dp;
+ }
+ break;
+
+ default:
+ {
+ if(useBrent)
+ {
+ alpha=2.;
+ computeOptimalBrentGain(I,p,-MI,dp,alpha);
+ dp=alpha*dp;
+ }
+ if(ApproxHessian==HESSIAN_NONSECOND)
+ dp=-1.*dp;
+
+ break;
+ }
+ }
+
+ Warp->getParamInverse(dp,dpinv);
+ Warp->pRondp(p,dpinv,p);
+
+ iteration++;
+ iterationGlobale++;
+
+ computeEvalRMS(p);
+
+ // std::cout << p.t() << std::endl;
+ }
+ while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
+ //while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
+
+ nbIteration=iteration;
+
+ if(diverge)
+ {
+ if(computeCovariance){
+ covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ covarianceMatrix = -1;
+ MI_postEstimation = -1;
+ NMI_postEstimation = -1;
+ }
+ deletePosEvalRMS();
+
+ // throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
+ }
+ else
+ {
+ MI_postEstimation=-getCost(I,p);
+ NMI_postEstimation=-getNormalizedCost(I,p);
+ // std::cout << "MI apres: " << MI_postEstimation << std::endl;
+ // std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
+ if(MI_preEstimation>MI_postEstimation)
+ {
+ p=p_avant_estimation;
+ MI_postEstimation = MI_preEstimation;
+ NMI_postEstimation = NMI_preEstimation;
+ covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ covarianceMatrix = -1;
+ }
+
+ deletePosEvalRMS();
+
+ if(computeCovariance){
+ try{
+ covarianceMatrix = (-H).inverseByLU();
+ // covarianceMatrix = (-Hnorm).inverseByLU();
+ }
+ catch(...){
+ covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ covarianceMatrix = -1;
+ MI_postEstimation = -1;
+ NMI_postEstimation = -1;
+ deletePosEvalRMS();
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS(const vpColVector &pw)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+ x_pos=new double[nb_corners];
+ y_pos=new double[nb_corners];
+
+ Warp->computeCoeff(pw);
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,pw);
+ Warp->warpX(X1,X2,p);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+}
+
+void vpTemplateTrackerMIInverseCompositional::computeEvalRMS(const vpColVector &pw)
+{
+ unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
+
+ Warp->computeCoeff(pw);
+ evolRMS=0;
+ vpTemplateTrackerTriangle triangle;
+
+ for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
+ {
+ zoneTracked->getTriangle(i, triangle);
+ for (unsigned int j=0; j<3; j++) {
+ triangle.getCorner(j, X1[0], X1[1]);
+
+ Warp->computeDenom(X1,pw);
+ Warp->warpX(X1,X2,pw);
+ evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
+ x_pos[i*3+j]=X2[0];
+ y_pos[i*3+j]=X2[1];
+ }
+ }
+ evolRMS=evolRMS/nb_corners;
+}
+
+void vpTemplateTrackerMIInverseCompositional::deletePosEvalRMS()
+{
+ delete[] x_pos;
+ delete[] y_pos;
+}
diff --git a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp
new file mode 100644
index 0000000..7ff4e24
--- /dev/null
+++ b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp
@@ -0,0 +1,791 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Template tracker.
+ *
+ * Authors:
+ * Amaury Dame
+ * Aurelien Yol
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et,int Nc, double val, const int °re)
+{
+ switch(degre)
+ {
+ case 4:
+ PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);break;
+ default:
+ PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);//std::cout<<"DEFAUT"<<std::endl;
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et,int Nc, double val)
+{
+ int sr=0;
+ int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+ double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9];
+ for(int ir=-1;ir<=1;ir++)
+ for(int it=-1;it<=1;it++)
+ {
+ *pt++ +=Bspline3(ir-er)*Bspline3(it-et)*val;
+ //pt++;
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et,int Nc, double val)
+{
+ double Bti[4];
+
+ double Br;
+ double *ptBti=&Bti[0];
+ for(int it=-1;it<=2;it++)
+ {
+ *ptBti++ =Bspline4i(it-et,it);
+ //pt++;
+ }
+ double *pt=&Prt[(cr*Nc+ct)*16];
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=Bspline4i(ir-er,ir);
+ ptBti=&Bti[0];
+ for(int it=-1;it<=2;it++)
+ {
+ *pt++ +=Br* *ptBti++ *val;
+ }
+ }
+}
+
+double vpTemplateTrackerMIBSpline::Bspline3(double diff)
+{
+ //double result;
+ double aDiff=std::fabs(diff);
+ // if(aDiff<0.5)
+ //// return (-(aDiff-0.5)*(aDiff-0.5)+(-aDiff+0.5)+0.5);
+ // return (-(aDiff * aDiff) + 0.75);
+ // else if(aDiff<1.5)
+ // return (0.5*(1.5-aDiff)*(1.5-aDiff));
+
+ if(aDiff<1.5){
+ if(aDiff<0.5)
+ return (-(aDiff * aDiff) + 0.75);
+ return (0.5*(1.5-aDiff)*(1.5-aDiff));
+ }
+
+ return 0;
+}
+
+double vpTemplateTrackerMIBSpline::Bspline4i(double diff,int &interv)
+{
+ switch(interv)
+ {
+ case -1:
+ return ((2.+diff)*(2.+diff)*(2.+diff)/6.);
+ case 0:
+ return (-diff*diff*diff/2.-diff*diff+4./6.);
+ case 1:
+ return (diff*diff*diff/2.-diff*diff+4./6.);
+ case 2:
+ return ((2.-diff)*(2.-diff)*(2.-diff)/6.);
+ default:
+ return 0;
+ }
+}
+
+double vpTemplateTrackerMIBSpline::dBspline3(double diff)
+{
+ if((diff>-1.5)&&(diff<=-0.5))
+ return diff+1.5;
+ else if((diff>-0.5)&&(diff<=0.5))
+ return -2.*diff;
+ else if((diff>0.5)&&(diff<=1.5))
+ return diff-1.5;
+
+ // if(fabs(diff + 1.5) <= (-0.5 + 1.5))
+ // return diff+1.5;
+ // else if(fabs(diff + 0.5) <= (0.5 + 0.5))
+ // return -2.*diff;
+ // else if(fabs(diff - 0.5) <= (1.5 - 0.5))
+ // return diff-1.5;
+
+ return 0;
+}
+
+double vpTemplateTrackerMIBSpline::dBspline4(double diff)
+{
+ if((diff>-2.)&&(diff<=-1.))
+ return (diff+2.)*(diff+2.)/2.;
+ else if((diff>-1.)&&(diff<=0.))
+ return -3.*diff*diff/2.-2.*diff;
+ else if((diff>0.)&&(diff<=1.))
+ return 3.*diff*diff/2.-2.*diff;
+ else if((diff>1.)&&(diff<=2.))
+ return -(diff-2.)*(diff-2.)/2.;
+ else
+ return 0;
+}
+
+double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
+{
+ if((diff>-1.5)&&(diff<=-0.5))
+ return 1.;
+ else if((diff>-0.5)&&(diff<=0.5))
+ return -2.;
+ else if((diff>0.5)&&(diff<=1.5))
+ return 1.;
+ else
+ return 0;
+}
+
+double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
+{
+ if((diff>-2.)&&(diff<=-1.))
+ return (diff+2.);
+ else if((diff>-1.)&&(diff<=0.))
+ return -3.*diff-2.;
+ else if((diff>0.)&&(diff<=1.))
+ return 3.*diff-2.;
+ else if((diff>1.)&&(diff<=2.))
+ return -(diff-2.);
+ else
+ return 0;
+
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam); break;
+ default:
+ PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam); break;
+ default:
+ PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam)
+{
+ short int sr=0;
+ short int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+ double dBti[3];
+ double d2Bti[3];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ double *ptd2Bti=&d2Bti[0];
+
+ for(short int it=1;it>=-1;it--)
+ {
+ *ptBti++ =Bspline3(it+et);
+ *ptdBti++ =dBspline3(it+et);
+ *ptd2Bti++ =d2Bspline3(it+et);
+ }
+
+ double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+(int)(NbParam+NbParam*NbParam))];
+ for(short int ir=-1;ir<=1;++ir)
+ {
+ Br=Bspline3(-ir+er);
+
+ for(short unsigned int it=0;it<=2;++it)
+ {
+ *pt++ += Br * (Bti[it]);
+
+ double v1 = Br * (dBti[it]);
+ for(short unsigned int ip=0;ip<NbParam;++ip)
+ {
+ *pt++ -= v1*val[ip];
+ double v2 = Br * (d2Bti[it]) * val[ip];
+ for(short unsigned int ip2=0;ip2<NbParam;++ip2)
+ *pt++ += v2*val[ip2];
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam)
+{
+ short int sr=0;
+ short int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+ double dBti[3];
+ double d2Bti[3];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ double *ptd2Bti=&d2Bti[0];
+
+ for(short int it=1;it>=-1;it--)
+ {
+ *ptBti++ =Bspline3(it+et);
+ *ptdBti++ =dBspline3(it+et);
+ *ptd2Bti++ =d2Bspline3(it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+ for(short int ir=-1;ir<=1;++ir)
+ {
+ Br=Bspline3(-ir+er);
+ short int irInd = ir + 1;
+ short int ind = (cr+sr+irInd)*Ncb;
+ for(short int it=0;it<=2;++it)
+ {
+ Prt[ind+(ct+st+it)] += Br * (Bti[it]);
+
+ double v1 = Br * (dBti[it]);
+ int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
+ for (int ip=0;ip<NbParam_;++ip)
+ {
+ dPrt[ind1+ip]-= v1*val[ip];
+ double v2 = Br * (d2Bti[it]) * val[ip];
+ int ind2 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_*NbParam_+ip*NbParam_;
+ for(short int ip2=0;ip2<NbParam_;++ip2)
+ d2Prt[ind2+ip2] += v2*val[ip2];
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
+{
+ // double Br;
+ // for(short int ir=-1;ir<=1;++ir)
+ // {
+ // Br=Bspline3(-ir+er);
+
+ // for(short unsigned int it=0;it < size;++it)
+ // *Prt++ += Br * bt[it];
+ // }
+#define LSIZE 12
+
+ double Br;
+ const double *btend;
+ double *bt0 = &bt[0];
+ if(er>0.5){er=er-1.0;}
+
+ for(int ir=-1;ir<=1;++ir)
+ {
+ Br=Bspline3(-ir+er);
+ btend = bt0 + size;
+ bt = bt0;
+
+ if (size >= LSIZE) {
+ btend -= LSIZE - 1;
+ for (; bt < btend; bt += LSIZE){
+ *Prt++ += Br * bt[0];
+ *Prt++ += Br * bt[1];
+ *Prt++ += Br * bt[2];
+ *Prt++ += Br * bt[3];
+ *Prt++ += Br * bt[4];
+ *Prt++ += Br * bt[5];
+ *Prt++ += Br * bt[6];
+ *Prt++ += Br * bt[7];
+ *Prt++ += Br * bt[8];
+ *Prt++ += Br * bt[9];
+ *Prt++ += Br * bt[10];
+ *Prt++ += Br * bt[11];
+ }
+ btend += LSIZE - 1;
+ }
+ for (; bt < btend; *Prt++ += Br * *bt++) {};
+
+ }
+#undef LSIZE
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et,int Nc, double *val, unsigned int &NbParam)
+{
+ double Br;
+
+ double Bti[4];
+ double dBti[4];
+ double d2Bti[4];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ double *ptd2Bti=&d2Bti[0];
+ for(char it=-1;it<=2;it++)
+ {
+ *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ *ptdBti++ =dBspline4(-it+et);
+ *ptd2Bti++ =d2Bspline4(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
+ for(char ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ ptBti=&Bti[0];
+ ptdBti=&dBti[0];
+ ptd2Bti=&d2Bti[0];
+ for(char it=-1;it<=2;it++)
+ {
+ *pt++ +=Br**ptBti;
+ for(short int ip=0;ip<NbParam_;ip++)
+ {
+ *pt++ -=Br**ptdBti*val[ip];
+ for(short int ip2=0;ip2<NbParam_;ip2++)
+ *pt++ +=Br**ptd2Bti*val[ip]*val[ip2];
+ }
+ ptBti++;
+ ptdBti++;
+ ptd2Bti++;
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, double et,int Ncb, double *val, unsigned int &NbParam)
+{
+ double Br;
+
+ double Bti[4];
+ double dBti[4];
+ double d2Bti[4];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ double *ptd2Bti=&d2Bti[0];
+ for(char it=-1;it<=2;it++)
+ {
+ *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ *ptdBti++ =dBspline4(-it+et);
+ *ptd2Bti++ =d2Bspline4(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ int irInd = ir + 1;
+ int ind = (cr+irInd)*Ncb+ct;
+
+ ptBti=&Bti[0];
+ ptdBti=&dBti[0];
+ ptd2Bti=&d2Bti[0];
+
+ for(int it=-1;it<=2;it++)
+ {
+ Prt[ind+it] += Br * *ptBti;
+ int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
+ for(int ip=0;ip<NbParam_;ip++)
+ {
+ dPrt[ind1+ip]-= Br**ptdBti*val[ip];
+ int ind2 = ((cr+irInd)*Ncb+(ct+it))*NbParam_*NbParam_+ip*NbParam_;
+ for(int ip2=0;ip2<NbParam_;ip2++)
+ d2Prt[ind2+ip2] += Br**ptd2Bti*val[ip]*val[ip2];
+ }
+ ptBti++;
+ ptdBti++;
+ ptd2Bti++;
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
+{
+#define LSIZE 12
+ double Br;
+ const double *btend;
+ double *bt0 = &bt[0];
+
+ for(int ir=-1;ir<=2;++ir)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ btend = bt0 + size;
+ bt = bt0;
+
+ if (size >= LSIZE) {
+ btend -= LSIZE - 1;
+ for (; bt < btend; bt += LSIZE){
+ *Prt++ += Br * bt[0];
+ *Prt++ += Br * bt[1];
+ *Prt++ += Br * bt[2];
+ *Prt++ += Br * bt[3];
+ *Prt++ += Br * bt[4];
+ *Prt++ += Br * bt[5];
+ *Prt++ += Br * bt[6];
+ *Prt++ += Br * bt[7];
+ *Prt++ += Br * bt[8];
+ *Prt++ += Br * bt[9];
+ *Prt++ += Br * bt[10];
+ *Prt++ += Br * bt[11];
+ }
+ btend += LSIZE - 1;
+ }
+ for (; bt < btend; *Prt++ += Br * *bt++) {};
+
+ }
+#undef LSIZE
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam); break;
+ default:
+ PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam); break;
+ default:
+ PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam)
+{
+ int sr=0;
+ int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+ double dBti[3];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ for(char it=-1;it<=1;it++)
+ {
+ *ptBti++ =Bspline3(-it+et);
+ *ptdBti++ =dBspline3(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+NbParam_+NbParam_*NbParam_)];
+ for(char ir=-1;ir<=1;ir++)
+ {
+ Br=Bspline3(-ir+er);
+ ptBti=&Bti[0];
+ ptdBti=&dBti[0];
+ for(char it=-1;it<=1;it++)
+ {
+ *pt++ +=Br**ptBti;
+ for(unsigned int ip=0;ip<NbParam;ip++)
+ {
+ *pt++ -=Br**ptdBti*val[ip];
+ pt=pt+NbParam; // Modif AY
+ }
+ // pt=pt+NbParam*NbParam; // Modif AY
+ ptBti++;
+ ptdBti++;
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam)
+{
+ int sr=0;
+ int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+ double dBti[3];
+
+ double *ptBti=&Bti[0];
+ double *ptdBti=&dBti[0];
+ for(char it=-1;it<=1;it++)
+ {
+ *ptBti++ =Bspline3(-it+et);
+ *ptdBti++ =dBspline3(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ for(char ir=-1;ir<=1;ir++)
+ {
+ Br=Bspline3(-ir+er);
+
+ int irInd = ir + 1;
+ int ind = (cr+sr+irInd)*Ncb;
+
+ ptBti=&Bti[0];
+ ptdBti=&dBti[0];
+ for(char it=-1;it<=1;it++)
+ {
+ Prt[ind+(ct+st+it)] += Br**ptBti;
+ int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
+ for(short int ip=0;ip<NbParam_;ip++)
+ {
+ dPrt[ind1+ip]-= Br**ptdBti*val[ip];
+ }
+ ptBti++;
+ ptdBti++;
+ }
+ }
+}
+
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam)
+{
+ double Br;
+
+ double Bti[4];
+ double dBti[4];
+
+ for(char it=-1;it<=2;it++)
+ {
+ Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ dBti[it+1] = dBspline4(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for(int it=0;it<=3;it++)
+ {
+ (*pt++) +=Br*Bti[it];
+
+ for(int ip=0;ip<NbParam_;ip++)
+ {
+ (*pt++) -=Br*dBti[it]*val[ip];
+ pt=pt+NbParam_; // Modif AY
+ }
+ // pt=pt+NbParam*NbParam; // Modif AY
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam)
+{
+ double Br;
+
+ double Bti[4];
+ double dBti[4];
+
+ for(char it=-1;it<=2;it++)
+ {
+ Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ dBti[it+1] = dBspline4(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ int irInd = ir + 1;
+ int ind = (cr+irInd)*Ncb+ct;
+
+ for(int it=0;it<=3;it++)
+ {
+ Prt[ind+it] += Br*Bti[it];
+ int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
+
+ for(int ip=0;ip<NbParam_;ip++)
+ {
+ dPrt[ind1+ip]-= Br*dBti[it]*val[ip];
+ }
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam); break;
+ default:
+ PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
+{
+
+ int sr=0;
+ int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+
+ for(char it=-1;it<=1;it++)
+ {
+ Bti[it+1] = Bspline3(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+ int NbParam_val = NbParam_+NbParam_*NbParam_;
+
+ double *pt=&PrtTout[(unsigned int)(((cr+sr)*Nc+(ct+st))*9*(1+NbParam_val))];
+ for(int ir=-1;ir<=1;ir++)
+ {
+ Br= Bspline3(-ir+er);
+ for(int it=0;it<=2;it++)
+ {
+ (*pt++) +=Br*Bti[it];
+ pt=pt+NbParam_val;
+ }
+ }
+}
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
+{
+
+ double Br;
+
+ double Bti[4];
+
+ for(char it=-1;it<=2;it++)
+ {
+ Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ }
+
+ int NbParam_ = (int)NbParam;
+ int NbParam_val = NbParam_+NbParam_*NbParam_;
+ double *pt=&PrtTout[(unsigned int)((cr*Nc+ct)*16*(1+NbParam_val))];
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for(int it=0;it<=3;it++)
+ {
+ (*pt++) +=Br*Bti[it];
+ pt=pt+NbParam_val;
+ }
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb, unsigned int &NbParam, int °ree)
+{
+ switch(degree)
+ {
+ case 4:
+ PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam); break;
+ default:
+ PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
+ }
+}
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
+{
+
+ int sr=0;
+ int st=0;
+ if(er>0.5){sr=1;er=er-1;}
+ if(et>0.5){st=1;et=et-1;}
+
+ double Br;
+
+ double Bti[3];
+
+ for(char it=-1;it<=1;it++)
+ {
+ Bti[it+1] = Bspline3(-it+et);
+ }
+
+ for(int ir=-1;ir<=1;ir++)
+ {
+ Br= Bspline3(-ir+er);
+
+ int irInd = ir + 1;
+ int ind = (cr+sr+irInd)*Ncb;
+ for(int it=0;it<=2;it++)
+ {
+ Prt[ind+(ct+st+it)] += Br*Bti[it];
+ }
+ }
+}
+
+
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
+{
+
+ double Br;
+
+ double Bti[4];
+
+ for(char it=-1;it<=2;it++)
+ {
+ Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ }
+
+ for(int ir=-1;ir<=2;ir++)
+ {
+ Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ int irInd = ir + 1;
+ int ind = (cr+irInd)*Ncb+ct;
+
+ for(int it=0;it<=3;it++)
+ {
+ Prt[ind+it] += Br*Bti[it];
+ }
+ }
+}
+
+#endif
diff --git a/modules/vision/CMakeLists.txt b/modules/vision/CMakeLists.txt
new file mode 100644
index 0000000..63a1b99
--- /dev/null
+++ b/modules/vision/CMakeLists.txt
@@ -0,0 +1,51 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(vision visp_core OPTIONAL visp_visual_features visp_io)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+
+if (CMAKE_VERSION VERSION_LESS 2.8.12)
+ vp_add_tests(
+ SOURCES_EXCLUDE
+ key-point/testKeyPoint-2.cpp
+ key-point/testKeyPoint-4.cpp
+ DEPENDS_ON visp_mbt visp_gui visp_io)
+else()
+ vp_add_tests(DEPENDS_ON visp_mbt visp_gui visp_io)
+endif()
diff --git a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h
new file mode 100644
index 0000000..4290ad0
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h
@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#ifndef vpBasicKeyPoint_H
+#define vpBasicKeyPoint_H
+
+/*!
+ \file vpBasicKeyPoint.h
+ \brief class that defines what is a Keypoint
+*/
+
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRect.h>
+
+#include <vector>
+
+
+/*!
+ \class vpBasicKeyPoint
+ \ingroup group_vision_keypoints
+
+ \brief class that defines what is a Keypoint. This class provides
+ all the basic elements to implement classes which aims to match
+ points from an image to another.
+*/
+class VISP_EXPORT vpBasicKeyPoint
+{
+ public:
+ vpBasicKeyPoint();
+
+ virtual ~vpBasicKeyPoint() {
+ matchedReferencePoints.resize(0);
+ currentImagePointsList.resize(0);
+ referenceImagePointsList.resize(0);
+ };
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I) =0;
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width) =0;
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpRect& rectangle) =0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I) =0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width) =0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
+ const vpRect& rectangle) =0;
+
+ virtual void display(const vpImage<unsigned char> &Iref,
+ const vpImage<unsigned char> &Icurrent, unsigned int size=3) =0;
+
+ virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
+ 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.
+ */
+ 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.
+ */
+ 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.
+ */
+ inline void getReferencePoint (const unsigned int index, vpImagePoint &referencePoint )
+ {
+ if (index >= referenceImagePointsList.size())
+ {
+ vpTRACE("Index of the reference point out of range");
+ throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ }
+
+ referencePoint.set_ij(referenceImagePointsList[index].get_i(), referenceImagePointsList[index].get_j());
+ }
+
+ /*!
+ 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(const unsigned int index, vpImagePoint &referencePoint, vpImagePoint ¤tPoint) {
+ if (index >= matchedReferencePoints.size())
+ {
+ vpTRACE("Index of the matched points out of range");
+ throw(vpException(vpException::fatalError,"Index of the matched points out of range"));
+ }
+ referencePoint.set_ij(referenceImagePointsList[matchedReferencePoints[index]].get_i(),referenceImagePointsList[matchedReferencePoints[index]].get_j());
+ currentPoint.set_ij(currentImagePointsList[index].get_i(), currentImagePointsList[index].get_j());
+ };
+
+ /*!
+ 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
+ vpKeyPointSurf surf;
+
+ //Here the code to compute the reference points and the current points.
+
+ vpImagePoint referencePoint1;
+ vpImagePoint currentPoint;
+ surf.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points
+
+ vpImagePoint referencePoint2;
+ const vpImagePoint* referencePointsList = surf.getAllPointsInReferenceImage();
+ int index = surf.getIndexInAllReferencePointList(1); //Get the first matched reference point index in the complete reference point list
+ referencePoint2 = referencePointsList[index]; //Get the first matched reference point
+ \endcode
+ */
+ inline unsigned int getIndexInAllReferencePointList ( const unsigned int indexInMatchedPointList ) {
+ if (indexInMatchedPointList >= matchedReferencePoints.size())
+ {
+ vpTRACE("Index of the matched reference point out of range");
+ throw(vpException(vpException::fatalError,"Index of the matched reference point out of range"));
+ }
+ return matchedReferencePoints[indexInMatchedPointList];
+ }
+
+ /*!
+ 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.
+ */
+ 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.
+ */
+ const std::vector<vpImagePoint>& getReferenceImagePointsList() const {return referenceImagePointsList;}
+
+ /*!
+ Return the vector of current image point.
+
+ \warning Should not be modified.
+
+ \return Vector of the current image point.
+ */
+ const std::vector<vpImagePoint>& 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.
+ */
+ const std::vector<unsigned int>& getMatchedReferencePoints() const {return matchedReferencePoints;}
+
+ private:
+ virtual void init()=0;
+
+ protected:
+ /*!
+ List of the points which define the reference.
+ */
+ std::vector<vpImagePoint> referenceImagePointsList;
+
+ /*!
+ List of the points which belong to the current image and have
+ been matched with points belonging to the reference.
+ */
+ std::vector<vpImagePoint> 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.
+ */
+ std::vector<unsigned int> matchedReferencePoints;
+
+ //! flag to indicate if the reference has been built.
+ bool _reference_computed;
+};
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
+
diff --git a/modules/vision/include/visp3/vision/vpCalibration.h b/modules/vision/include/visp3/vision/vpCalibration.h
new file mode 100644
index 0000000..34be3a1
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpCalibration.h
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Camera calibration.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpCalibration.h
+ \brief Tools for camera calibration.
+
+ \author Eric Marchand (INRIA) using code from Francois Chaumette (INRIA)
+
+ \sa the example in calibrate.cpp
+*/
+#ifndef vpCalibration_h
+#define vpCalibration_h
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/vision/vpCalibrationException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <list>
+#include <vector>
+/*!
+ \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{
+ CALIB_LAGRANGE, /*!< Lagrange approach without estimation of the distortion. */
+ CALIB_VIRTUAL_VS, /*!< Virtual visual servoing approach without estimation of the distortion (results are similar to Lowe approach). */
+ CALIB_VIRTUAL_VS_DIST, /*!< Virtual visual servoing approach with estimation of the distortion. */
+ CALIB_LAGRANGE_VIRTUAL_VS, /*!< Lagrange approach first, than virtual visual servoing approach, without estimation of the distortion. */
+ CALIB_LAGRANGE_VIRTUAL_VS_DIST, /*!< Lagrange approach first, than virtual visual servoing approach, with estimation of the distortion. */
+ } vpCalibrationMethodType ;
+
+ vpHomogeneousMatrix cMo ; //!< the pose computed for the model without distortion
+ //!< (as a 3x4 matrix [R T])
+ vpHomogeneousMatrix cMo_dist ; //!< the pose computed for perspective projection
+ //!< 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 relation to the effector
+ vpHomogeneousMatrix eMc_dist;
+
+public:
+ // Constructor
+ vpCalibration() ;
+ vpCalibration(const vpCalibration& c) ;
+
+ // Destructor
+ virtual ~vpCalibration() ;
+
+ // Add a new point in this array
+ int addPoint(double X, double Y, double Z, vpImagePoint &ip) ;
+
+ // = operator
+ vpCalibration& operator=(const vpCalibration& twinCalibration);
+
+ static void calibrationTsai(std::vector<vpHomogeneousMatrix> &cMo,
+ std::vector<vpHomogeneousMatrix> &rMe,
+ vpHomogeneousMatrix &eMc);
+
+ //! Suppress all the point in the array of point
+ int clearPoint() ;
+
+ void computeStdDeviation(double &deviation, double &deviation_dist);
+ int computeCalibration(vpCalibrationMethodType method,
+ vpHomogeneousMatrix &cMo_est,
+ vpCameraParameters &cam_est,
+ bool verbose = false) ;
+ static int computeCalibrationMulti(vpCalibrationMethodType method,
+ std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam,
+ double &globalReprojectionError,
+ bool verbose = false) ;
+
+ static int computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
+ vpHomogeneousMatrix &eMc,
+ vpHomogeneousMatrix &eMc_dist);
+ double computeStdDeviation(const vpHomogeneousMatrix &cMo_est,
+ const vpCameraParameters &camera);
+ double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam);
+ int displayData(vpImage<unsigned char> &I, vpColor color=vpColor::red,
+ unsigned int thickness=1, int subsampling_factor=1) ;
+ int displayGrid(vpImage<unsigned char> &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
+ double getResidual(void) const {return residual;}
+ //!get the residual for perspective projection with distortion (in pixels)
+ double getResidual_dist(void) const {return residual_dist;}
+ //!get the number of points
+ unsigned int get_npt() const {return npt;}
+
+ int init() ;
+
+ int readData(const char *filename) ;
+ static int readGrid(const char *filename,unsigned int &n,
+ std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
+ bool verbose = false);
+
+ //!set the gain for the virtual visual servoing algorithm
+ static void setLambda(const double &lambda){gain = lambda;}
+ int writeData(const char *filename) ;
+
+private:
+ void computePose(const vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
+ void calibLagrange( vpCameraParameters &cam , vpHomogeneousMatrix &cMo) ;
+
+ //! Compute the calibration using virtual visual servoing approach
+ void calibVVS( vpCameraParameters &cam , vpHomogeneousMatrix &cMo,
+ bool verbose = false) ;
+
+ static void calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[],
+ vpCameraParameters &cam, bool verbose = false);
+ static void calibVVSMulti(std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam,
+ double &globalReprojectionError, bool verbose = false) ;
+ void calibVVSWithDistortion( vpCameraParameters &cam,
+ vpHomogeneousMatrix &cMo,
+ bool verbose = false) ;
+ static void calibVVSWithDistortionMulti( unsigned int nbPose,
+ vpCalibration table_cal[],
+ vpCameraParameters &cam,
+ bool verbose = false );
+ static void calibVVSWithDistortionMulti( std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam,
+ double &globalReprojectionError,
+ bool verbose = false);
+
+private:
+ unsigned int npt ; //!< number of points used in calibration computation
+ std::list<double> LoX, LoY, LoZ ; //!< list of points coordinates (3D in meters)
+ std::list<vpImagePoint> Lip ; //!< list of points coordinates (2D in pixels)
+
+ double residual ; //!< residual in pixel for camera model without distortion
+ double residual_dist ; //!< residual in pixel for perspective projection with distortion model
+
+ static double threshold;
+ static unsigned int nbIterMax;
+ static double gain;
+
+} ;
+
+#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
new file mode 100644
index 0000000..38f1d1e
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpCalibrationException.h
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exceptions that can be emited by the vpCalibration class and its derivates.
+ *
+ * Authors:
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+#ifndef __vpCalibrationException_H
+#define __vpCalibrationException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* Classes standards. */
+//
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+
+ \class vpCalibrationException
+ \brief Error that can be emited by the vpCalibration class.
+ */
+class VISP_EXPORT vpCalibrationException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpCalibration member
+ */
+ enum errorCodeEnum
+ {
+ //! error returns by a constructor
+ constructionError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedError,
+ //! index out of range
+ outOfRangeError,
+ //! iterative algorithm doesn't converge
+ convergencyError,
+ forbiddenOperatorError,
+ } ;
+
+ public:
+ vpCalibrationException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpCalibrationException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpCalibrationException (const int id)
+ : vpException(id){ ; }
+};
+
+#endif
diff --git a/modules/vision/include/visp3/vision/vpFernClassifier.h b/modules/vision/include/visp3/vision/vpFernClassifier.h
new file mode 100644
index 0000000..811d00f
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpFernClassifier.h
@@ -0,0 +1,336 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class that implements the Fern classifier and the YAPE detector thanks
+ * to the OpenCV library.
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#ifndef vpFernClassifier_H
+#define vpFernClassifier_H
+
+#include <visp3/core/vpConfig.h>
+
+#include <string>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/features2d/features2d.hpp>
+# include <opencv2/legacy/legacy.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
+# include <cv.h>
+# include <cvaux.hpp>
+#endif
+
+#include <visp3/vision/vpBasicKeyPoint.h>
+
+/*!
+ \class vpFernClassifier
+ \ingroup group_vision_keypoints
+
+ \brief Class that implements the Fern classifier and the YAPE detector thanks
+ to the OpenCV library.
+
+ \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
+
+ This class provides a way to detect and match point using the YAPE and
+ a Fern Classifiers, thanks to the OpenCV library (version >= 2.0)
+
+ This class provides a tool to match points between a model and the current
+ image. The points of interests belonging to the model and the points detected
+ in the current image are given in pixels thanks to the vpImagePoint class.
+
+ For more details about the Ferns Classifier and the point detector,
+ see \cite Ozuysal10 and \cite Lepetit04c.
+
+ To use this class, you first have to detect points in the model and train the
+ associated Fern classifier. Then, for each new grabbed image, You can detect
+ points and try to match them with the model.
+
+ As training can requires up to several minutes, it is possible to save (in a
+ file) and load the classifier.
+
+ The following small example shows how to use the class.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/vision/vpFernClassifier.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
+int main()
+{
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+ vpFernClassifier fern;
+
+ //First grab the reference image Ireference
+
+ //Build the reference points.
+ fern.buildReference(Ireference);
+
+ //Then grab another image which represents the current image Icurrent
+
+ //Match points between the reference points and the points detected in the current image.
+ fern.matchPoint(Icurrent);
+
+ //Display the matched points
+ fern.display(Ireference, Icurrent);
+
+ return (0);
+}
+#else
+int main() {}
+#endif
+ \endcode
+
+ It is also possible to create the reference thanks to only a part of the
+ reference image (not the whole image) and find points to match in only a
+ part of the current image. The small following example shows how to do this.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/vision/vpFernClassifier.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
+int main()
+{
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+ vpFernClassifier fern;
+
+ //First grab the reference image Ireference
+
+ //Select a part of the image by clicking on two points which define a rectangle
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Ireference, corners[i]);
+ }
+
+ //Build the reference points.
+ int nbrRef;
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrRef = fern.buildReference(Ireference, corners[0], height, width);
+
+ //Then grab another image which represents the current image Icurrent
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Icurrent, corners[i]);
+ }
+
+ //Match points between the reference points and the points detected in the current image.
+ int nbrMatched;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrMatched = fern.matchPoint(Icurrent, corners[0], height, width);
+
+ //Display the matched points
+ fern.display(Ireference, Icurrent);
+
+ return(0);
+}
+#else
+int main() {}
+#endif
+ \endcode
+*/
+class VISP_EXPORT vpFernClassifier: public vpBasicKeyPoint
+{
+protected:
+ //! The points of interest detector.
+ cv::LDetector ldetector;
+
+ //! The Fern classifier.
+ cv::FernClassifier fernClassifier;
+
+ //! The patch generator (OpenCV format).
+ cv::PatchGenerator gen;
+
+ //! Flag to indicate whether the classifier has been trained or not.
+ bool hasLearn;
+
+ /* parameters for the key-points detector and the classifier */
+ //! Threshold to accept or reject points (usually around 20)
+ int threshold;
+ //! Number of view to generate for the training
+ int nbView;
+ //! Minimal distance between two points
+ int dist;
+ //! Number of classifier
+ int nbClassfier;
+ //! Size of the classifier
+ int ClassifierSize;
+ //! Number of octave for the multi scale
+ int nbOctave;
+ //! Size of the patch
+ int patchSize;
+ //! Radius for the detector
+ int radius;
+ //! Maximal number of points
+ int nbPoints;
+
+ /* parameters for the filter */
+ //! Flag to specify whether the reference image have to be blurred or not in order to improve the recognition rate
+ bool blurImage;
+ //! Radius of the kernel used to blur the image
+ int radiusBlur;
+ //! Sigma of the kernel used to blur the image
+ int sigmaBlur;
+
+ //! Number of minimum point below which the homography is not estimated (must be at least four)
+ unsigned int nbMinPoint;
+
+ //! The current image in the OpenCV format.
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ cv::Mat curImg;
+#else
+ IplImage* curImg;
+#endif
+
+ //! keypoints detected in the reference image.
+ std::vector<cv::KeyPoint> objKeypoints;
+ //! the ROI in the reference image.
+ cv::Rect modelROI_Ref;
+ //! the ROI for the reference image.
+ cv::Rect modelROI;
+ //! the vector containing the points in the model.
+ std::vector<cv::KeyPoint> modelPoints;
+ //! the vector containing the points in the current image.
+ std::vector<cv::KeyPoint> imgKeypoints;
+ //! vector in the OpenCV format to be used by the detector.
+ std::vector<cv::Point2f> refPt, curPt;
+
+
+public:
+ vpFernClassifier();
+ vpFernClassifier(const std::string& _dataFile, const std::string& _objectName);
+ virtual ~vpFernClassifier();
+
+ /* build reference */
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I);
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width);
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpRect& rectangle);
+
+ /* matching */
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I);
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width);
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
+ const vpRect& rectangle);
+
+ /* display */
+ virtual void display(const vpImage<unsigned char> &Iref,
+ const vpImage<unsigned char> &Icurrent, unsigned int size=3);
+ virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
+ const vpColor &color=vpColor::green);
+
+ /* io methods */
+ void load(const std::string& _dataFile, const std::string& /*_objectName*/);
+ void record(const std::string& _objectName, const std::string& _dataFile);
+
+
+
+ /* accessors */
+ /*!
+ The image is blurred before being processed. This solution can lead to a
+ better recognition rate.
+
+ \param _blur : the new option for the blur
+ */
+ inline void setBlurSetting(const bool _blur){ blurImage = _blur;}
+ inline void setBlurSettings(const bool _blur, int _sigma, int _size);
+
+ /*!
+ Return the blur option.
+ The Blur option is used to activate a filter used to blur the input image.
+ The blurring can improve the robustness of the detection.
+
+ \return the value of the blur option.
+ */
+ inline bool getBlurSetting(){ return blurImage;}
+
+ /*!
+ Return the blur sigma (for the filter) option.
+
+ \return The value of the sigma for the blur filter.
+ */
+ inline int getBlurSigma() { return this->sigmaBlur;}
+
+ /*!
+ return the blur size (for the filter) option
+
+ \return the value of the radius for the blur filter
+ */
+ inline int getBlurSize() { return this->radiusBlur;}
+
+ /*!
+ Return a reference on the vector of reference points in the OpenCV format
+
+ \return the list of reference points
+ */
+ const std::vector<cv::Point2f>& getRefPt() const {return refPt;}
+
+ /*!
+ Return a reference on the vector of current points in the OpenCV format
+
+ \return the list of current points
+ */
+ const std::vector<cv::Point2f>& getCurPt() const {return curPt;}
+
+ /*!
+ Return the region of interest in the OpenCV format
+ */
+ cv::Rect getModelROI() const { return modelROI;}
+
+protected:
+ void setImage(const vpImage<unsigned char>& I);
+ void train();
+ virtual void init();
+};
+
+#endif /* opencv ver > 2.0.0 */
+
+#endif
+
diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h
new file mode 100644
index 0000000..1ef90ab
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpHomography.h
@@ -0,0 +1,403 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography transformation.
+ *
+ * Authors:
+ * Muriel Pressigout
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+\file vpHomography.h
+
+This file defines an homography transformation. This class aims to provide some
+tools for homography computation.
+*/
+
+#ifndef vpHomography_hh
+#define vpHomography_hh
+
+#include <list>
+#include <vector>
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMatrix.h>
+
+/*!
+
+ \class vpHomography
+ \ingroup group_vision_homography
+
+ \brief Implementation of an homography and operations on homographies.
+
+ This class aims to compute the homography wrt. two images.
+
+ The vpHomography class is derived from vpArray2D<double>.
+
+ These two images are both described by a set of points. The 2 sets (one per
+ image) are sets of corresponding points : for a point in a image, there is
+ the corresponding point (image of the same 3D point) in the other image
+ points set. These 2 sets are the only data needed to compute the
+ homography. One method used is the one introduced by Ezio Malis during his
+ PhD \cite TheseMalis. A normalization is carried out on this points in order to improve the
+ conditioning of the problem, what leads to improve the stability of the
+ result.
+
+ Store and compute the homography such that
+ \f[
+ ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}
+ \f]
+
+ with
+ \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
+ { ^b{\bf n}^T}
+ \f]
+
+ The \ref tutorial-homography explains how to use this class.
+
+ The example below shows also how to manipulate this class to first
+ compute a ground truth homography from camera poses, project pixel
+ coordinates points using an homography and lastly estimate an
+ homography from a subset of 4 matched points in frame a and frame b
+ respectively.
+
+ \code
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+int main()
+{
+ // Initialize in the object frame the coordinates in meters of 4 points that
+ // belong to a planar object
+ vpPoint Po[4];
+ Po[0].setWorldCoordinates(-0.1, -0.1, 0);
+ Po[1].setWorldCoordinates( 0.2, -0.1, 0);
+ Po[2].setWorldCoordinates( 0.1, 0.1, 0);
+ Po[3].setWorldCoordinates(-0.1, 0.3, 0);
+
+ // Initialize the pose between camera frame a and object frame o
+ vpHomogeneousMatrix aMo(0, 0, 1, 0, 0, 0); // Camera is 1 meter far
+
+ // Initialize the pose between camera frame a and camera frame
+ // b. These two frames correspond for example to two successive
+ // camera positions
+ vpHomogeneousMatrix aMb(0.2, 0.1, 0, 0, 0, vpMath::rad(2));
+
+ // Compute the pose between camera frame b and object frame
+ vpHomogeneousMatrix bMo = aMb.inverse() * aMo;
+
+ // Initialize camera intrinsic parameters
+ vpCameraParameters cam;
+
+ // Compute the coordinates in pixels of the 4 object points in the
+ // camera frame a
+ vpPoint Pa[4];
+ std::vector<double> xa(4), ya(4); // Coordinates in pixels of the points in frame a
+ for(int i=0 ; i < 4 ; i++) {
+ Pa[i] = Po[i];
+ Pa[i].project(aMo); // Project the points from object frame to
+ // camera frame a
+ vpMeterPixelConversion::convertPoint(cam,
+ Pa[i].get_x(), Pa[i].get_y(),
+ xa[i], ya[i]);
+ }
+
+ // Compute the coordinates in pixels of the 4 object points in the
+ // camera frame b
+ vpPoint Pb[4];
+ std::vector<double> xb(4), yb(4); // Coordinates in pixels of the points in frame b
+ for(int i=0 ; i < 4 ; i++) {
+ Pb[i] = Po[i];
+ Pb[i].project(bMo); // Project the points from object frame to
+ // camera frame a
+ }
+
+ // Compute equation of the 3D plane containing the points in camera frame b
+ vpPlane bP(Pb[0], Pb[1], Pb[2]);
+
+ // Compute the corresponding ground truth homography
+ vpHomography aHb(aMb, bP);
+
+ std::cout << "Ground truth homography aHb: \n" << aHb<< std::endl;
+
+ // Compute the coordinates of the points in frame b using the ground
+ // truth homography and the coordinates of the points in frame a
+ vpHomography bHa = aHb.inverse();
+ for(int i = 0; i < 4 ; i++){
+ double inv_z = 1. / (bHa[2][0] * xa[i] + bHa[2][1] * ya[i] + bHa[2][2]);
+
+ xb[i] = (bHa[0][0] * xa[i] + bHa[0][1] * ya[i] + bHa[0][2]) * inv_z;
+ yb[i] = (bHa[1][0] * xa[i] + bHa[1][1] * ya[i] + bHa[1][2]) * inv_z;
+ }
+
+ // Estimate the homography from 4 points coordinates expressed in pixels
+ vpHomography::DLT(xb, yb, xa, ya, aHb, true);
+ aHb /= aHb[2][2]; // Apply a scale factor to have aHb[2][2] = 1
+
+ std::cout << "Estimated homography aHb: \n" << aHb<< std::endl;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpHomography : public vpArray2D<double>
+{
+ private:
+ static const double sing_threshold; // = 0.0001;
+ static const double threshold_rotation;
+ static const double threshold_displacement;
+ vpHomogeneousMatrix aMb ;
+ // bool isplanar;
+ //! reference plane coordinates expressed in Rb
+ vpPlane bP ;
+
+ private:
+ //! build the homography from aMb and Rb
+ void build() ;
+
+ //! insert a rotation matrix
+ void insert(const vpHomogeneousMatrix &aRb) ;
+ //! insert a rotation matrix
+ void insert(const vpRotationMatrix &aRb) ;
+ //! insert a theta u vector (transformation into a rotation matrix)
+ void insert(const vpThetaUVector &tu) ;
+ //! insert a translation vector
+ void insert(const vpTranslationVector &atb) ;
+ //! insert a translation vector
+ void insert(const vpPlane &bP) ;
+
+ static void initRansac(unsigned int n,
+ double *xb, double *yb,
+ double *xa, double *ya,
+ vpColVector &x) ;
+
+ public:
+ vpHomography() ;
+ vpHomography(const vpHomography &H) ;
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP) ;
+ //! 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
+ vpHomography(const vpThetaUVector &tu,
+ const vpTranslationVector &atb,
+ const vpPlane &bP) ;
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpPoseVector &arb,
+ const vpPlane &bP) ;
+ virtual ~vpHomography() {};
+
+ //! 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
+ void buildFrom(const vpThetaUVector &tu,
+ const vpTranslationVector &atb,
+ const vpPlane &bP) ;
+ //! 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) ;
+ vpMatrix convert() const;
+
+ void computeDisplacement(vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n) ;
+
+ void computeDisplacement(const vpColVector& nd,
+ vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n) ;
+
+ void eye();
+
+ //! invert the homography
+ vpHomography inverse() const ;
+ //! invert the homography
+ void inverse(vpHomography &Hi) const;
+
+ //! Load an homography from a file
+ void load(std::ifstream &f) ;
+
+ // Multiplication by an homography
+ vpHomography operator*(const vpHomography &H) const;
+ // Multiplication by a scalar
+ vpHomography operator*(const double &v) const;
+ vpColVector operator*(const vpColVector &b) const;
+ // Multiplication by a point
+ vpPoint operator*(const vpPoint &H) const;
+
+ // Division by a scalar
+ vpHomography operator/(const double &v) const;
+ vpHomography & operator/=(double v);
+ vpHomography & operator=(const vpHomography &H);
+ vpHomography & operator=(const vpMatrix &H);
+
+ vpImagePoint projection(const vpImagePoint &p);
+
+ /*!
+ 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(const unsigned int nrows, const unsigned int ncols,
+ const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize an homography matrix"));
+ };
+
+ void save(std::ofstream &f) const ;
+
+ static void DLT(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya ,
+ vpHomography &aHb,
+ bool normalization=true);
+
+ static void HLM(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ bool isplanar,
+ vpHomography &aHb) ;
+
+ static bool ransac(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ vpHomography &aHb,
+ std::vector<bool> &inliers,
+ double &residual,
+ unsigned int nbInliersConsensus,
+ double threshold,
+ bool normalization=true);
+
+ static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa);
+ static vpPoint project(const vpHomography &bHa, const vpPoint &Pa);
+
+ static void robust(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ vpHomography &aHb,
+ std::vector<bool> &inlier,
+ double &residual,
+ double weights_threshold=0.4,
+ unsigned int niter=4,
+ bool normalization=true);
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ //! build the homography from aMb and Rb
+ static void build(vpHomography &aHb,
+ const vpHomogeneousMatrix &aMb,
+ const vpPlane &bP) ;
+
+ static void computeDisplacement(const vpHomography &aHb,
+ const vpColVector& nd,
+ vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n) ;
+
+ static void computeDisplacement (const vpHomography &aHb,
+ vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n) ;
+
+ static void computeDisplacement(const vpHomography &H,
+ const double x,
+ const double y,
+ std::list<vpRotationMatrix> & vR,
+ std::list<vpTranslationVector> & vT,
+ std::list<vpColVector> & vN) ;
+ static double computeDisplacement(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpPlane &oN,
+ vpHomogeneousMatrix &c2Mc1,
+ vpHomogeneousMatrix &c1Mo,
+ int userobust
+ ) ;
+ static double computeDisplacement(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpPlane *oN,
+ vpHomogeneousMatrix &c2Mc1,
+ vpHomogeneousMatrix &c1Mo,
+ int userobust
+ ) ;
+ static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d);
+ // VVS
+ static double computeRotation(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpHomogeneousMatrix &c2Mc1,
+ int userobust) ;
+ static void computeTransformation(vpColVector &x,unsigned int *ind, vpColVector &M) ;
+ static bool degenerateConfiguration(vpColVector &x,unsigned int *ind) ;
+ static bool degenerateConfiguration(vpColVector &x,unsigned int *ind, double threshold_area);
+ static bool degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya);
+ static void HartleyNormalization(unsigned int n,
+ const double *x, const double *y,
+ double *xn, double *yn,
+ double &xg, double &yg,
+ double &coef);
+ static void HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
+ std::vector<double> &xn, std::vector<double> &yn,
+ double &xg, double &yg, double &coef);
+ static void HartleyDenormalization(vpHomography &aHbn,
+ vpHomography &aHb,
+ double xg1, double yg1, double coef1,
+ double xg2, double yg2, double coef2 ) ;
+
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ void setIdentity();
+ //@}
+#endif
+
+};
+
+#endif
diff --git a/modules/vision/include/visp3/vision/vpKeyPoint.h b/modules/vision/include/visp3/vision/vpKeyPoint.h
new file mode 100644
index 0000000..a26c95e
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpKeyPoint.h
@@ -0,0 +1,1062 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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 functionalities.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+#ifndef __vpKeyPoint_h__
+#define __vpKeyPoint_h__
+
+#include <algorithm> // std::transform
+#include <vector> // std::vector
+#include <stdlib.h> // srand, rand
+#include <time.h> // time
+#include <fstream> // std::ofstream
+#include <numeric> // std::accumulate
+#include <float.h> // DBL_MAX
+#include <map> // std::map
+#include <limits>
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/vision/vpBasicKeyPoint.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpPose.h>
+#ifdef VISP_HAVE_MODULE_IO
+# include <visp3/io/vpImageIo.h>
+#endif
+#include <visp3/core/vpPolygon.h>
+#include <visp3/vision/vpXmlConfigParserKeyPoint.h>
+#include <visp3/core/vpConvert.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+// Require at least OpenCV >= 2.1.1
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D) // OpenCV >= 3.0.0
+# include <opencv2/xfeatures2d.hpp>
+#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+# include <opencv2/nonfree/nonfree.hpp>
+#endif
+
+#ifdef VISP_HAVE_XML2
+# include <libxml/xmlwriter.h>
+#endif
+
+/*!
+ \class vpKeyPoint
+ \ingroup group_vision_keypoints
+
+ \brief Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV library.
+ This class permits to use different types of detectors, extractors and matchers easily.
+ So, the classical SIFT and SURF keypoints could be used, as well as ORB, FAST, (etc.) keypoints,
+ depending of the version of OpenCV you use.
+
+ \note Due to some patents, SIFT and SURF are packaged in an external module called nonfree module
+ in OpenCV version before 3.0.0 and in xfeatures2d from 3.0.0. You have to check you have the
+ corresponding module to use SIFT and SURF.
+
+ The goal of this class is to provide a tool to match reference keypoints from a
+ reference image (or train keypoints in OpenCV terminology) and detected keypoints from a current image (or query
+ keypoints in OpenCV terminology).
+
+ If you supply the corresponding 3D coordinates corresponding to the 2D coordinates of the reference keypoints,
+ you can also estimate the pose of the object by matching a set of detected keypoints in the current image with
+ the reference keypoints.
+
+ If you use this class, the first thing you have to do is to build
+ the reference keypoints by detecting keypoints in a reference image which contains the
+ object to detect. Then you match keypoints detected in a current image with those detected in a reference image
+ by calling matchPoint() methods.
+ You can access to the lists of matched points thanks to the
+ methods getMatchedPointsInReferenceImage() and
+ getMatchedPointsInCurrentImage(). These two methods return a list of
+ matched points. The nth element of the first list is matched with
+ the nth element of the second list.
+ To provide easy compatibility with OpenCV terminology, getTrainKeyPoints() give you access to the list
+ of keypoints detected in train images (or reference images) and getQueryKeyPoints() give you access to the list
+ of keypoints detected in a query image (or current image).
+ The method getMatches() give you access to a list of cv::DMatch with the correspondence between the index of the
+ train keypoints and the index of the query keypoints.
+
+ The following small example shows how to use the class to do the matching between current and reference keypoints.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/vision/vpKeyPoint.h>
+
+int main()
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020300)
+ vpImage<unsigned char> Irefrence;
+ vpImage<unsigned char> Icurrent;
+
+ vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
+ vpKeyPoint keypoint("ORB", "ORB", "BruteForce-Hamming", filterType);
+
+ // First grab the reference image Irefrence
+ // Add your code to load the reference image in Ireference
+
+ // Build the reference ORB points.
+ keypoint.buildReference(Irefrence);
+
+ // Then grab another image which represents the current image Icurrent
+
+ // Match points between the reference points and the ORB points computed in the current image.
+ keypoint.matchPoint(Icurrent);
+
+ // Display the matched points
+ keypoint.display(Irefrence, Icurrent);
+#endif
+
+ return (0);
+}
+ \endcode
+
+ It is also possible to build the reference keypoints in a region of interest (ROI) of an image
+ and find keypoints to match in only a part of the current image. The small following example shows how to do this:
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/vision/vpKeyPoint.h>
+
+int main()
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020300)
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+
+ vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
+ vpKeyPoint keypoint("ORB", "ORB", "BruteForce-Hamming", filterType);
+
+ //First grab the reference image Irefrence
+ //Add your code to load the reference image in Ireference
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Ireference, corners[i]);
+ }
+
+ //Build the reference ORB points.
+ int nbrRef;
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrRef = keypoint.buildReference(Ireference, corners[0], height, width);
+
+ //Then grab another image which represents the current image Icurrent
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Icurrent, corners[i]);
+ }
+
+ //Match points between the reference points and the ORB points computed in the current image.
+ int nbrMatched;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrMatched = keypoint.matchPoint(Icurrent, corners[0], height, width);
+
+ //Display the matched points
+ keypoint.display(Ireference, Icurrent);
+#endif
+
+ return(0);
+}
+ \endcode
+
+ This class is also described in \ref tutorial-matching.
+*/
+class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint {
+
+public:
+
+ /*! Predefined filtering method identifier. */
+ typedef enum {
+ constantFactorDistanceThreshold, /*!< Keep all the points below a constant factor threshold. */
+ stdDistanceThreshold, /*!< Keep all the points below a minimal distance + the standard deviation. */
+ ratioDistanceThreshold, /*!< Keep all the points enough discriminated (the ratio distance between the two best matches is below the threshold). */
+ stdAndRatioDistanceThreshold, /*!< Keep all the points which fall with the two conditions above. */
+ noFilterMatching /*!< No filtering. */
+ } vpFilterMatchingType;
+
+ /*! Predefined detection method identifier. */
+ typedef enum {
+ detectionThreshold, /*!< The object is present if the average of the descriptor distances is below the threshold. */
+ detectionScore /*!< Same condition than the previous but with a formula taking into account the number of matches,
+ the object is present if the score is above the threshold. */
+ } vpDetectionMethodType;
+
+ /*! Predefined constant for training image format. */
+ typedef enum {
+ jpgImageFormat, /*!< Save training images in JPG format. */
+ pngImageFormat, /*!< Save training images in PNG format. */
+ ppmImageFormat, /*!< Save training images in PPM format. */
+ pgmImageFormat /*!< Save training images in PGM format. */
+ } vpImageFormatType;
+
+
+ vpKeyPoint(const std::string &detectorName="ORB", const std::string &extractorName="ORB",
+ const std::string &matcherName="BruteForce-Hamming", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
+ vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
+ const std::string &matcherName="BruteForce", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
+
+ unsigned int buildReference(const vpImage<unsigned char> &I);
+ unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP, const unsigned int height, const unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect& rectangle);
+
+ void buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoint,
+ std::vector<cv::Point3f> &points3f, const bool append=false, const int class_id=-1);
+ void buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoint,
+ const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
+ const bool append=false, const int class_id=-1);
+
+ static void compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point);
+
+ static void compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point);
+
+ static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<cv::KeyPoint> &candidate, const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors=NULL);
+
+ static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<vpImagePoint> &candidate, const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors=NULL);
+
+ bool computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
+ const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+
+ bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
+ std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+
+ bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
+ std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+
+ void createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
+ void createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
+
+ void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
+ const vpRect& rectangle=vpRect());
+ void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
+ const cv::Mat &mask=cv::Mat());
+
+ void detectExtractAffine(const vpImage<unsigned char> &I, std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
+ std::vector<cv::Mat>& listOfDescriptors,
+ std::vector<vpImage<unsigned char> > *listOfAffineI=NULL);
+
+ void display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size=3);
+ void display(const vpImage<unsigned char> &ICurrent, unsigned int size=3, const vpColor &color=vpColor::green);
+
+ void displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
+ unsigned int crossSize, unsigned int lineThickness=1,
+ const vpColor &color=vpColor::green);
+ void displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
+ const std::vector<vpImagePoint> &ransacInliers=std::vector<vpImagePoint>(), unsigned int crossSize=3,
+ unsigned int lineThickness=1);
+
+ void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints=NULL);
+ void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints=NULL);
+
+ /*!
+ Get the covariance matrix when estimating the pose using the Virtual Visual Servoing approach.
+
+ \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
+
+ \sa setCovarianceComputation
+ */
+ inline vpMatrix getCovarianceMatrix() const {
+ if(!m_computeCovariance) {
+ std::cout << "Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl;
+ return vpMatrix();
+ }
+
+ if(m_computeCovariance && !m_useRansacVVS) {
+ std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
+ << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual Visual Servoing approach."
+ << std::endl;
+ return vpMatrix();
+ }
+
+ return m_covarianceMatrix;
+ }
+
+ /*!
+ Get the elapsed time to compute the keypoint detection.
+
+ \return The elapsed time.
+ */
+ inline double getDetectionTime() const {
+ return m_detectionTime;
+ }
+
+ /*!
+ Get the detector pointer.
+ \param name : Name of the detector.
+
+ \return The detector or NULL if the name passed in parameter does not exist.
+ */
+ inline cv::Ptr<cv::FeatureDetector> getDetector(const std::string &name) const {
+ std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector = m_detectors.find(name);
+ if(findDetector != m_detectors.end()) {
+ return findDetector->second;
+ }
+
+ return cv::Ptr<cv::FeatureDetector>();
+ }
+
+ /*!
+ Get the elapsed time to compute the keypoint extraction.
+
+ \return The elapsed time.
+ */
+ inline double getExtractionTime() const {
+ return m_extractionTime;
+ }
+
+ /*!
+ Get the extractor pointer.
+ \param name : Name of the extractor.
+
+ \return The extractor or NULL if the name passed in parameter does not exist.
+ */
+ inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const std::string &name) const {
+ std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor = m_extractors.find(name);
+ if(findExtractor != m_extractors.end()) {
+ return findExtractor->second;
+ }
+
+ return cv::Ptr<cv::DescriptorExtractor>();
+ }
+
+ /*!
+ Get the image format to use when saving training images.
+
+ \return The image format.
+ */
+ inline vpImageFormatType getImageFormat() const {
+ return m_imageFormat;
+ }
+
+ /*!
+ Get the elapsed time to compute the matching.
+
+ \return The elapsed time.
+ */
+ inline double getMatchingTime() const {
+ return m_matchingTime;
+ }
+
+ /*!
+ Get the matcher pointer.
+
+ \return The matcher pointer.
+ */
+ inline cv::Ptr<cv::DescriptorMatcher> getMatcher() const {
+ return m_matcher;
+ }
+
+ /*!
+ Get the list of matches (correspondences between the indexes of the detected keypoints and the train keypoints).
+
+ \return The list of matches.
+ */
+ inline std::vector<cv::DMatch> getMatches() const {
+ return m_filteredMatches;
+ }
+
+ /*!
+ Get the list of pairs with the correspondence between the matched query and train keypoints.
+
+ \return The list of pairs with the correspondence between the matched query and train keypoints.
+ */
+ inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const {
+ std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > matchQueryToTrainKeyPoints(m_filteredMatches.size());
+ for (size_t i = 0; i < m_filteredMatches.size(); i++) {
+ matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
+ m_queryFilteredKeyPoints[(size_t) m_filteredMatches[i].queryIdx],
+ m_trainKeyPoints[(size_t) m_filteredMatches[i].trainIdx]));
+ }
+ return matchQueryToTrainKeyPoints;
+ }
+
+ /*!
+ Get the number of train images.
+
+ \return The number of train images.
+ */
+ inline unsigned int getNbImages() const {
+ return static_cast<unsigned int>(m_mapOfImages.size());
+ }
+
+ void getObjectPoints(std::vector<cv::Point3f> &objectPoints) const;
+ void getObjectPoints(std::vector<vpPoint> &objectPoints) const;
+
+ /*!
+ Get the elapsed time to compute the pose.
+
+ \return The elapsed time.
+ */
+ inline double getPoseTime() const {
+ return m_poseTime;
+ }
+
+ /*!
+ Get the descriptors matrix for the query keypoints.
+
+ \return Matrix with descriptors values at each row for each query keypoints.
+ */
+ inline cv::Mat getQueryDescriptors() const {
+ return m_queryDescriptors;
+ }
+
+ void getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
+ void getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
+
+ /*!
+ Get the list of Ransac inliers.
+
+ \return The list of Ransac inliers.
+ */
+ inline std::vector<vpImagePoint> getRansacInliers() const {
+ return m_ransacInliers;
+ }
+
+ /*!
+ Get the list of Ransac outliers.
+
+ \return The list of Ransac outliers.
+ */
+ inline std::vector<vpImagePoint> getRansacOutliers() const {
+ return m_ransacOutliers;
+ }
+
+ /*!
+ Get the train descriptors matrix.
+
+ \return : Matrix with descriptors values at each row for each train keypoints (or reference keypoints).
+ */
+ inline cv::Mat getTrainDescriptors() const {
+ return m_trainDescriptors;
+ }
+
+ void getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
+ void getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
+
+ void getTrainPoints(std::vector<cv::Point3f> &points) const;
+ void getTrainPoints(std::vector<vpPoint> &points) const;
+
+ void initMatcher(const std::string &matcherName);
+
+ void insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
+ vpImage<unsigned char> &IMatching);
+ void insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
+
+#ifdef VISP_HAVE_XML2
+ void loadConfigFile(const std::string &configFile);
+#endif
+
+ void loadLearningData(const std::string &filename, const bool binaryMode=false, const bool append=false);
+
+ void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
+ std::vector<cv::DMatch> &matches, double &elapsedTime);
+
+ unsigned int matchPoint(const vpImage<unsigned char> &I);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
+
+ bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
+ double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL,
+ const vpRect& rectangle=vpRect());
+
+ bool matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
+ const bool isPlanarObject=true, std::vector<vpImagePoint> *imPts1=NULL,
+ std::vector<vpImagePoint> *imPts2=NULL, double *meanDescriptorDistance=NULL,
+ double *detectionScore=NULL, const vpRect& rectangle=vpRect());
+
+ bool matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
+ double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
+ bool (*func)(vpHomogeneousMatrix *)=NULL, const vpRect& rectangle=vpRect());
+
+ void reset();
+
+ void saveLearningData(const std::string &filename, const bool binaryMode=false, const bool saveTrainingImages=true);
+
+ /*!
+ Set if the covariance matrix has to be computed in the Virtual Visual Servoing approach.
+
+ \param flag : True if the covariance has to be computed, false otherwise.
+ */
+ inline void setCovarianceComputation(const bool& flag) {
+ m_computeCovariance = flag;
+ if(!m_useRansacVVS) {
+ std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
+ << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual "
+ "Visual Servoing approach." << std::endl;
+ }
+ }
+
+ /*!
+ Set the method to decide if the object is present or not.
+
+ \param method : Detection method (detectionThreshold or detectionScore).
+ */
+ inline void setDetectionMethod(const vpDetectionMethodType &method) {
+ m_detectionMethod = method;
+ }
+
+ /*!
+ Set and initialize a detector denominated by his name \p detectorName.
+
+ \param detectorName : Name of the detector.
+ */
+ inline void setDetector(const std::string &detectorName) {
+ m_detectorNames.clear();
+ m_detectorNames.push_back(detectorName);
+ m_detectors.clear();
+ initDetector(detectorName);
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ /*!
+ Template function to set to a \p parameterName a value for a specific detector named by his \p detectorName.
+
+ \param detectorName : Name of the detector
+ \param parameterName : Name of the parameter
+ \param value : Value to set
+ */
+ template<typename T1, typename T2, typename T3> inline void setDetectorParameter(const T1 detectorName,
+ const T2 parameterName, const T3 value) {
+ if(m_detectors.find(detectorName) != m_detectors.end()) {
+ m_detectors[detectorName]->set(parameterName, value);
+ }
+ }
+#endif
+
+ /*!
+ Set and initialize a list of detectors denominated by their names \p detectorNames.
+
+ \param detectorNames : List of detector names.
+ */
+ inline void setDetectors(const std::vector<std::string> &detectorNames) {
+ m_detectorNames.clear();
+ m_detectors.clear();
+ m_detectorNames = detectorNames;
+ initDetectors(m_detectorNames);
+ }
+
+ /*!
+ Set and initialize an extractor denominated by his name \p extractorName.
+
+ \param extractorName : Name of the extractor.
+ */
+ inline void setExtractor(const std::string &extractorName) {
+ m_extractorNames.clear();
+ m_extractorNames.push_back(extractorName);
+ m_extractors.clear();
+ initExtractor(extractorName);
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ /*!
+ Template function to set to a \p parameterName a value for a specific extractor named by his \p extractorName.
+
+ \param extractorName : Name of the extractor
+ \param parameterName : Name of the parameter
+ \param value : Value to set
+ */
+ template<typename T1, typename T2, typename T3> inline void setExtractorParameter(const T1 extractorName,
+ const T2 parameterName, const T3 value) {
+ if(m_extractors.find(extractorName) != m_extractors.end()) {
+ m_extractors[extractorName]->set(parameterName, value);
+ }
+ }
+#endif
+
+ /*!
+ Set and initialize a list of extractors denominated by their names \p extractorNames.
+
+ \param extractorNames : List of extractor names.
+ */
+ inline void setExtractors(const std::vector<std::string> &extractorNames) {
+ m_extractorNames.clear();
+ m_extractorNames = extractorNames;
+ m_extractors.clear();
+ initExtractors(m_extractorNames);
+ }
+
+ /*!
+ Set the image format to use when saving training images.
+
+ \param imageFormat : The image format.
+ */
+ inline void setImageFormat(const vpImageFormatType &imageFormat) {
+ m_imageFormat = imageFormat;
+ }
+
+ /*!
+ Set and initialize a matcher denominated by his name \p matcherName.
+ The different matchers are:
+ - BruteForce (it uses L2 distance)
+ - BruteForce-L1
+ - BruteForce-Hamming
+ - BruteForce-Hamming(2)
+ - FlannBased
+
+ L1 and L2 norms are preferable choices for SIFT and SURF descriptors, NORM_HAMMING should be used with ORB,
+ BRISK and BRIEF, NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4.
+
+ \param matcherName : Name of the matcher.
+ */
+ inline void setMatcher(const std::string &matcherName) {
+ m_matcherName = matcherName;
+ initMatcher(m_matcherName);
+ }
+
+ /*!
+ Set the filtering method to eliminate false matching.
+ The different methods are:
+ - constantFactorDistanceThreshold (keep matches whose the descriptor distance is below dist_min * factor)
+ - stdDistanceThreshold (keep matches whose the descriptor distance is below dist_min + standard_deviation)
+ - ratioDistanceThreshold (keep matches enough discriminated: the ratio distance between the 2 best matches is below the threshold)
+ - stdAndRatioDistanceThreshold (keep matches that agree with at least one of the two conditions)
+ - noFilterMatching
+
+ \param filterType : Type of the filtering method
+ */
+ inline void setFilterMatchingType(const vpFilterMatchingType &filterType) {
+ m_filterType = filterType;
+
+ //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
+ //So this is useful only for ratioDistanceThreshold method
+ if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
+ m_useKnn = true;
+
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if(m_matcher != NULL && m_matcherName == "BruteForce") {
+ //if a matcher is already initialized, disable the crossCheck
+ //because it will not work with knnMatch
+ m_matcher->set("crossCheck", false);
+ }
+ #endif
+ } else {
+ m_useKnn = false;
+
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if(m_matcher != NULL && m_matcherName == "BruteForce") {
+ //if a matcher is already initialized, set the crossCheck mode if necessary
+ m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
+ }
+ #endif
+ }
+ }
+
+ /*!
+ Set the factor value for the filtering method: constantFactorDistanceThreshold.
+
+ \param factor : Factor value
+ */
+ inline void setMatchingFactorThreshold(const double factor) {
+ if(factor > 0.0) {
+ m_matchingFactorThreshold = factor;
+ } else {
+ throw vpException(vpException::badValue, "The factor must be positive.");
+ }
+ }
+
+ /*!
+ Set the ratio value for the filtering method: ratioDistanceThreshold.
+
+ \param ratio : Ratio value (]0 ; 1])
+ */
+ inline void setMatchingRatioThreshold(const double ratio) {
+ if(ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits<double>::epsilon())) {
+ m_matchingRatioThreshold = ratio;
+ } else {
+ throw vpException(vpException::badValue, "The ratio must be in the interval ]0 ; 1].");
+ }
+ }
+
+ /*!
+ Set the percentage value for defining the cardinality of the consensus group.
+
+ \param percentage : Percentage value (]0 ; 100])
+ */
+ inline void setRansacConsensusPercentage(const double percentage) {
+ if(percentage > 0.0 && (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits<double>::epsilon())) {
+ m_ransacConsensusPercentage = percentage;
+ } else {
+ throw vpException(vpException::badValue, "The percentage must be in the interval ]0 ; 100].");
+ }
+ }
+
+ /*!
+ Set the maximum number of iterations for the Ransac pose estimation method.
+
+ \param nbIter : Maximum number of iterations for the Ransac
+ */
+ inline void setRansacIteration(const int nbIter) {
+ if(nbIter > 0) {
+ m_nbRansacIterations = nbIter;
+ } else {
+ throw vpException(vpException::badValue, "The number of iterations must be greater than zero.");
+ }
+ }
+
+ /*!
+ Set the maximum reprojection error (in pixel) to determine if a point is an inlier or not.
+
+ \param reprojectionError : Maximum reprojection error in pixel (used by OpenCV function)
+ */
+ inline void setRansacReprojectionError(const double reprojectionError) {
+ if(reprojectionError > 0.0) {
+ m_ransacReprojectionError = reprojectionError;
+ } else {
+ throw vpException(vpException::badValue, "The Ransac reprojection threshold must be positive as we deal with distance.");
+ }
+ }
+
+ /*!
+ Set the minimum number of inlier for the Ransac pose estimation method.
+
+ \param minCount : Minimum number of inlier for the consensus
+ */
+ inline void setRansacMinInlierCount(const int minCount) {
+ if(minCount > 0) {
+ m_nbRansacMinInlierCount = minCount;
+ } else {
+ throw vpException(vpException::badValue, "The minimum number of inliers must be greater than zero.");
+ }
+ }
+
+ /*!
+ Set the maximum error (in meter) to determine if a point is an inlier or not.
+
+ \param threshold : Maximum error in meter for ViSP function
+ */
+ inline void setRansacThreshold(const double threshold) {
+ if(threshold > 0.0) {
+ m_ransacThreshold = threshold;
+ } else {
+ throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
+ }
+ }
+
+ /*!
+ Set if multiple affine transformations must be used to detect and extract keypoints.
+
+ \param useAffine : True to use multiple affine transformations, false otherwise
+ */
+ inline void setUseAffineDetection(const bool useAffine) {
+ m_useAffineDetection = useAffine;
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ /*!
+ Set if cross check method must be used to eliminate some false matches with a brute-force matching method.
+
+ \param useCrossCheck : True to use cross check, false otherwise
+ */
+ inline void setUseBruteForceCrossCheck(const bool useCrossCheck) {
+ //Only available with BruteForce and with k=1 (i.e not used with a ratioDistanceThreshold method)
+ if(m_matcher != NULL && !m_useKnn && m_matcherName == "BruteForce") {
+ m_matcher->set("crossCheck", useCrossCheck);
+ } else if(m_matcher != NULL && m_useKnn && m_matcherName == "BruteForce") {
+ std::cout << "Warning, you try to set the crossCheck parameter with a BruteForce matcher but knn is enabled";
+ std::cout << " (the filtering method uses a ratio constraint)" << std::endl;
+ }
+ }
+#endif
+
+ /*!
+ Set if we want to match the train keypoints to the query keypoints.
+
+ \param useMatchTrainToQuery : True to match the train keypoints to the query keypoints
+ */
+ inline void setUseMatchTrainToQuery(const bool useMatchTrainToQuery) {
+ m_useMatchTrainToQuery = useMatchTrainToQuery;
+ }
+
+ /*!
+ Set the flag to choose between a percentage value of inliers for the cardinality of the consensus group
+ or a minimum number.
+
+ \param usePercentage : True to a percentage ratio of inliers, otherwise use a specified number of inliers
+ */
+ inline void setUseRansacConsensusPercentage(const bool usePercentage) {
+ m_useConsensusPercentage = usePercentage;
+ }
+
+ /*!
+ Set the flag to choose between the OpenCV or ViSP Ransac pose estimation function.
+
+ \param ransacVVS : True to use ViSP function, otherwise use OpenCV function
+ */
+ inline void setUseRansacVVS(const bool ransacVVS) {
+ m_useRansacVVS = ransacVVS;
+ }
+
+ /*!
+ Set the flag to filter matches where multiple query keypoints are matched to the same train keypoints.
+
+ \param singleMatchFilter : True to use the single match filter.
+ */
+ inline void setUseSingleMatchFilter(const bool singleMatchFilter) {
+ m_useSingleMatchFilter = singleMatchFilter;
+ }
+
+private:
+ //! If true, compute covariance matrix if the user select the pose estimation method using ViSP
+ bool m_computeCovariance;
+ //! Covariance matrix
+ vpMatrix m_covarianceMatrix;
+ //! Current id associated to the training image used for the learning.
+ int m_currentImageId;
+ //! Method (based on descriptor distances) to decide if the object is present or not.
+ vpDetectionMethodType m_detectionMethod;
+ //! Detection score to decide if the object is present or not.
+ double m_detectionScore;
+ //! Detection threshold based on average of descriptor distances to decide if the object is present or not.
+ double m_detectionThreshold;
+ //! Elapsed time to detect keypoints.
+ double m_detectionTime;
+ //! List of detector names.
+ std::vector<std::string> m_detectorNames;
+ //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) detectors,
+ // with a key based upon the detector name.
+ std::map<std::string, cv::Ptr<cv::FeatureDetector> > m_detectors;
+ //! Elapsed time to extract descriptors for the detected keypoints.
+ double m_extractionTime;
+ //! List of extractor name.
+ std::vector<std::string> m_extractorNames;
+ //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) extractors,
+ // with a key based upon the extractor name.
+ std::map<std::string, cv::Ptr<cv::DescriptorExtractor> > m_extractors;
+ //! List of filtered matches between the detected and the trained keypoints.
+ std::vector<cv::DMatch> m_filteredMatches;
+ //! Chosen method of filtering to eliminate false matching.
+ vpFilterMatchingType m_filterType;
+ //! Image format to use when saving the training images
+ vpImageFormatType m_imageFormat;
+ //! List of k-nearest neighbors for each detected keypoints (if the method chosen is based upon on knn).
+ std::vector<std::vector<cv::DMatch> > m_knnMatches;
+ //! Map of image id to know to which training image is related a training keypoints.
+ std::map<int, int> m_mapOfImageId;
+ //! Map of images to have access to the image buffer according to his image id.
+ std::map<int, vpImage<unsigned char> > m_mapOfImages;
+ //! Smart reference-counting pointer (similar to shared_ptr in Boost) of descriptor matcher (e.g. BruteForce or FlannBased).
+ cv::Ptr<cv::DescriptorMatcher> m_matcher;
+ //! Name of the matcher.
+ std::string m_matcherName;
+ //! List of matches between the detected and the trained keypoints.
+ std::vector<cv::DMatch> m_matches;
+ //! Factor value for the filtering method: constantFactorDistanceThreshold.
+ double m_matchingFactorThreshold;
+ //! Ratio value for the filtering method: ratioDistanceThreshold.
+ double m_matchingRatioThreshold;
+ //! Elapsed time to do the matching.
+ double m_matchingTime;
+ //! List of pairs between the keypoint and the 3D point after the Ransac.
+ std::vector<std::pair<cv::KeyPoint, cv::Point3f> > m_matchRansacKeyPointsToPoints;
+ //! Maximum number of iterations for the Ransac method.
+ int m_nbRansacIterations;
+ //! Minimum number of inliers for the Ransac method.
+ int m_nbRansacMinInlierCount;
+ //! List of 3D points (in the object frame) filtered after the matching to compute the pose.
+ std::vector<cv::Point3f> m_objectFilteredPoints;
+ //! Elapsed time to compute the pose.
+ double m_poseTime;
+ /*! Matrix of descriptors (each row contains the descriptors values for each keypoints
+ detected in the current image). */
+ cv::Mat m_queryDescriptors;
+ //! List of detected keypoints filtered after the matching.
+ std::vector<cv::KeyPoint> m_queryFilteredKeyPoints;
+ //! List of keypoints detected in the current image.
+ std::vector<cv::KeyPoint> m_queryKeyPoints;
+ //! Percentage value to determine the number of inliers for the Ransac method.
+ double m_ransacConsensusPercentage;
+ //! List of inliers.
+ std::vector<vpImagePoint> m_ransacInliers;
+ //! List of outliers.
+ std::vector<vpImagePoint> m_ransacOutliers;
+ //! Maximum reprojection error (in pixel for the OpenCV method) to decide if a point is an inlier or not.
+ double m_ransacReprojectionError;
+ //! Maximum error (in meter for the ViSP method) to decide if a point is an inlier or not.
+ double m_ransacThreshold;
+ //! Matrix of descriptors (each row contains the descriptors values for each keypoints
+ //detected in the train images).
+ cv::Mat m_trainDescriptors;
+ //! List of keypoints detected in the train images.
+ std::vector<cv::KeyPoint> m_trainKeyPoints;
+ //! List of 3D points (in the object frame) corresponding to the train keypoints.
+ std::vector<cv::Point3f> m_trainPoints;
+ //! List of 3D points in vpPoint format (in the object frame) corresponding to the train keypoints.
+ std::vector<vpPoint> m_trainVpPoints;
+ //! If true, use multiple affine transformations to cober the 6 affine parameters
+ bool m_useAffineDetection;
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ //! If true, some false matches will be eliminate by keeping only pairs (i,j) such that for i-th
+ //! query descriptor the j-th descriptor in the matcher’s collection is the nearest and vice versa.
+ bool m_useBruteForceCrossCheck;
+#endif
+ //! Flag set if a percentage value is used to determine the number of inliers for the Ransac method.
+ bool m_useConsensusPercentage;
+ //! Flag set if a knn matching method must be used.
+ bool m_useKnn;
+ //! Flag set if we want to match the train keypoints to the query keypoints, useful when there is only one train image
+ //! because it reduces the number of possible false matches (by default it is the inverse because normally there are multiple
+ //! train images of different views of the object)
+ bool m_useMatchTrainToQuery;
+ //! Flag set if a Ransac VVS pose estimation must be used.
+ bool m_useRansacVVS;
+ //! If true, keep only pairs of keypoints where each train keypoint is matched to a single query keypoint
+ bool m_useSingleMatchFilter;
+
+
+ void affineSkew(double tilt, double phi, cv::Mat& img, cv::Mat& mask, cv::Mat& Ai);
+
+ double computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est);
+
+ void filterMatches();
+
+ void init();
+ void initDetector(const std::string &detectorNames);
+ void initDetectors(const std::vector<std::string> &detectorNames);
+
+ void initExtractor(const std::string &extractorName);
+ void initExtractors(const std::vector<std::string> &extractorNames);
+
+ inline size_t myKeypointHash(const cv::KeyPoint &kp) {
+ size_t _Val = 2166136261U, scale = 16777619U;
+ Cv32suf u;
+ u.f = kp.pt.x; _Val = (scale * _Val) ^ u.u;
+ u.f = kp.pt.y; _Val = (scale * _Val) ^ u.u;
+ u.f = kp.size; _Val = (scale * _Val) ^ u.u;
+ //As the keypoint angle can be computed for certain type of keypoint only when extracting
+ //the corresponding descriptor, the angle field is not taking into account for the hash
+// u.f = kp.angle; _Val = (scale * _Val) ^ u.u;
+ u.f = kp.response; _Val = (scale * _Val) ^ u.u;
+ _Val = (scale * _Val) ^ ((size_t) kp.octave);
+ _Val = (scale * _Val) ^ ((size_t) kp.class_id);
+ return _Val;
+ }
+
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ /*
+ * Adapts a detector to detect points over multiple levels of a Gaussian
+ * pyramid. Useful for detectors that are not inherently scaled.
+ * From OpenCV 2.4.11 source code.
+ */
+ class PyramidAdaptedFeatureDetector: public cv::FeatureDetector {
+ public:
+ // maxLevel - The 0-based index of the last pyramid layer
+ PyramidAdaptedFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detector, int maxLevel = 2);
+
+ // TODO implement read/write
+ virtual bool empty() const;
+
+ protected:
+ virtual void detect(cv::InputArray image,
+ CV_OUT std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask =
+ cv::noArray());
+ virtual void detectImpl(const cv::Mat& image,
+ std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask =
+ cv::Mat()) const;
+
+ cv::Ptr<cv::FeatureDetector> detector;
+ int maxLevel;
+ };
+
+ /*
+ * A class filters a vector of keypoints.
+ * Because now it is difficult to provide a convenient interface for all usage scenarios of the keypoints filter class,
+ * it has only several needed by now static methods.
+ */
+ class KeyPointsFilter {
+ public:
+ KeyPointsFilter() {
+ }
+
+ /*
+ * Remove keypoints within borderPixels of an image edge.
+ */
+ static void runByImageBorder(std::vector<cv::KeyPoint>& keypoints,
+ cv::Size imageSize, int borderSize);
+ /*
+ * Remove keypoints of sizes out of range.
+ */
+ static void runByKeypointSize(std::vector<cv::KeyPoint>& keypoints,
+ float minSize, float maxSize = FLT_MAX);
+ /*
+ * Remove keypoints from some image by mask for pixels of this image.
+ */
+ static void runByPixelsMask(std::vector<cv::KeyPoint>& keypoints,
+ const cv::Mat& mask);
+ /*
+ * Remove duplicated keypoints.
+ */
+ static void removeDuplicated(std::vector<cv::KeyPoint>& keypoints);
+
+ /*
+ * Retain the specified number of the best keypoints (according to the response)
+ */
+ static void retainBest(std::vector<cv::KeyPoint>& keypoints, int npoints);
+ };
+
+#endif
+};
+
+#endif
+#endif
diff --git a/modules/vision/include/visp3/vision/vpKeyPointSurf.h b/modules/vision/include/visp3/vision/vpKeyPointSurf.h
new file mode 100644
index 0000000..0de6dbb
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpKeyPointSurf.h
@@ -0,0 +1,301 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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 Surf.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#ifndef vpKeyPointSurf_H
+#define vpKeyPointSurf_H
+
+/*!
+ \file vpKeyPointSurf.h
+
+ \brief Class that implements the SURF key points and technics thanks
+ to the OpenCV library.
+*/
+
+#include <visp3/vision/vpBasicKeyPoint.h>
+
+#include <list>
+#include <vector>
+
+#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 1.1.0 < 3.0.0
+# include <opencv2/core/core.hpp>
+# include <opencv2/features2d/features2d.hpp>
+# include <opencv2/legacy/compat.hpp>
+# include <opencv2/nonfree/nonfree.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/features2d/features2d.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x010100) // Require opencv >= 1.1.0
+# include <cxcore.h>
+# include <cv.h>
+#endif
+
+/*!
+ \class vpKeyPointSurf
+ \ingroup group_vision_keypoints
+
+ \brief Class that implements the SURF key points and technics thanks
+ to OpenCV library.
+
+ \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
+ You should rather use vpKeyPoint class that is more generic.
+
+ The goal of this class is to provide a tool to match points from a
+ model and points belonging to an image in which the model appears.
+ The coordinates of the different reference points and matched points
+ are given in pixel thanks to the class vpImagePoint. In this
+ documentation we do not explain the SURF technics. So if you want to
+ learn more about it you can refer to the following article :
+ Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
+ Robust Features", Proceedings of the 9th European Conference on
+ Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
+ 2006.
+
+ If you use this class the first things you have to do is to create
+ the reference thanks to a reference image which contains the
+ interesting object to detect. Then you have to grab other images
+ containing the object. After calling the specific method to match
+ points you can access to the lists of matched points thanks to the
+ methods getMatchedPointsInReferenceImage() and
+ getMatchedPointsInCurrentImage(). These two methods return a list of
+ matched points. The nth element of the first list is matched with
+ the nth element of the second list. The following small example show
+ how to use the class.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/vision/vpKeyPointSurf.h>
+
+int main()
+{
+#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+ vpKeyPointSurf surf;
+
+ // First grab the reference image Ireference
+
+ // Build the reference SURF points.
+ surf.buildReference(Ireference);
+
+ // Then grab another image which represents the current image Icurrent
+
+ // Match points between the reference points and the SURF points computed in the current image.
+ surf.matchPoint(Icurrent);
+
+ // Display the matched points
+ surf.display(Ireference, Icurrent);
+
+ return (0);
+#endif
+}
+ \endcode
+
+ It is also possible to create the reference thanks to only a part of the
+ reference image (not the whole image) and find points to match in only a
+ part of the current image. The small following example shows how to this
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/vision/vpKeyPointSurf.h>
+
+int main()
+{
+#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+ vpKeyPointSurf surf;
+
+ //First grab the reference image Ireference
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Ireference, corners[i]);
+ }
+
+ //Build the reference SURF points.
+ int nbrRef;
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrRef = surf.buildReference(Ireference, corners[0], height, width);
+
+ //Then grab another image which represents the current image Icurrent
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Icurrent, corners[i]);
+ }
+
+ //Match points between the reference points and the SURF points computed in the current image.
+ int nbrMatched;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrMatched = surf.matchPoint(Icurrent, corners[0], height, width);
+
+ //Display the matched points
+ surf.display(Ireference, Icurrent);
+
+ return(0);
+#endif
+}
+ \endcode
+
+ This class is also described in \ref tutorial-matching.
+*/
+
+class VISP_EXPORT vpKeyPointSurf : public vpBasicKeyPoint
+{
+ public:
+ /*!
+ This enumerate enables to set the detail level of the
+ descriptors.
+ */
+ typedef enum
+ {
+ basicDescriptor, /*<! basicDescriptor means that the descriptors are
+ composed by 64 elements floating-point vector. */
+ extendedDescriptor /*<! Means that the descriptors are composed by
+ 128 elements floating-point vector. */
+ } vpDescriptorType;
+
+ public:
+ vpKeyPointSurf();
+
+ virtual ~vpKeyPointSurf();
+
+ unsigned int buildReference(const vpImage<unsigned char> &I);
+ unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpRect& rectangle);
+ unsigned int matchPoint(const vpImage<unsigned char> &I);
+ unsigned int matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP, const unsigned int height, const unsigned int width);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
+ void display(const vpImage<unsigned char> &Iref,
+ const vpImage<unsigned char> &Icurrent, unsigned int size=3);
+ void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
+ const vpColor &color=vpColor::green);
+ std::list<int*>* matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList);
+ float* getDescriptorReferencePoint (const int index);
+ int getLaplacianReferencePoint (const int index);
+ void getDescriptorParamReferencePoint (const int index, int& size, float& dir);
+ /*!
+
+ Sets the value of the hessian threhold. Note that during the
+ computation of the hessian for each potential points, only the
+ points which have a hessian value higher than the threshold are
+ keeped. Fore more details about the threshold see the article
+ Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
+ Robust Features", Proceedings of the 9th European Conference on
+ Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
+ 2006.
+
+ \param hessian_threshold : Desired hessian threshold value.
+ */
+ void setHessianThreshold (double hessian_threshold) {
+ this->hessianThreshold = hessian_threshold;
+ params = cvSURFParams(this->hessianThreshold, this->descriptorType);
+ } ;
+
+ /*!
+
+ Sets the type of descriptors to use.
+
+ \param descriptor_type : Type of descriptor to use.
+ */
+ void setDescriptorType (vpDescriptorType descriptor_type) {
+ this->descriptorType = descriptor_type;
+ params = cvSURFParams(this->hessianThreshold, this->descriptorType);
+ } ;
+
+ /*!
+ Gets the value of the hessian threhold.
+
+ \return the hessian threshold value.
+ */
+ double getHessianThreshold () {return hessianThreshold;}
+
+ /*!
+ Gets the type of descriptor used.
+
+ \return the type of descriptor used.
+ */
+ vpDescriptorType getDescriptorType () {return descriptorType;}
+
+ private:
+ void init();
+
+ private:
+ //OpenCV Parameters
+ CvMemStorage* storage;
+ CvSURFParams params;
+ CvMemStorage* storage_cur;
+
+ CvSeq* image_keypoints;
+ CvSeq* image_descriptors;
+
+ CvSeq* ref_keypoints;
+ CvSeq* ref_descriptors;
+
+ /*!
+ only features with keypoint.hessian larger than that are extracted.
+ Good default value is ~300-500 (can depend on the average
+ local contrast and sharpness of the image).
+ User can further filter out some features based on their hessian values
+ and other characteristics.
+ */
+ double hessianThreshold;
+ vpDescriptorType descriptorType;
+
+};
+
+
+#endif
+
+#endif
+
+
diff --git a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h
new file mode 100644
index 0000000..ff71da8
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Levenberg Marquartd.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+
+#ifndef vpLevenbergMarquartd_h
+#define vpLevenbergMarquartd_h
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMath.h>
+
+#include <stdio.h>
+#include <errno.h>
+#include <math.h>
+#include <stdlib.h>
+#include <float.h>
+
+int VISP_EXPORT
+qrsolv (int n, double *r, int ldr, int *ipvt, double *diag,
+ double *qtb, double *x, double *sdiag, double *wa) ;
+
+double VISP_EXPORT
+enorm (const double *x, int n);
+
+int VISP_EXPORT
+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);
+
+double VISP_EXPORT
+pythag (double a, double b);
+
+int VISP_EXPORT
+qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
+ int lipvt, double *rdiag, double *acnorm, double *wa);
+
+int VISP_EXPORT
+qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
+ double *x, double *sdiag, double *wa);
+
+int VISP_EXPORT
+lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
+ double *jac, int ldfjac, int iflag),
+ int m, int n, double *x,
+ double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
+ double gtol, unsigned int maxfev, double *diag, int mode,
+ const double factor, int nprint, int *info, unsigned int *nfev,
+ int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
+ double *wa3, double *wa4);
+
+int VISP_EXPORT
+lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
+ double *jac, int ldfjac, int iflag),
+ int m, int n, double *x, double *fvec, double *fjac,
+ int ldfjac, double tol, int *info, 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
new file mode 100644
index 0000000..f1d056f
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h
@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Planar surface detection tool.
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#ifndef VPPLANAROBJECTDETECTOR_H_
+#define VPPLANAROBJECTDETECTOR_H_
+
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+# include <opencv2/imgproc/imgproc.hpp>
+# include <opencv2/features2d/features2d.hpp>
+# include <opencv2/calib3d/calib3d.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
+# include <cv.h>
+# include <cvaux.hpp>
+#endif
+
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/vision/vpFernClassifier.h>
+
+/*!
+ \class vpPlanarObjectDetector
+ \ingroup group_vision_keypoints
+
+ \brief Class used to detect a planar surface.
+
+ \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
+
+ This class allows to learn and recognise a surface in an image based on the
+ Fern Classifier or any other point of interest matching class.
+
+ It uses the class vpFernClassifier to extract points of interest in a
+ reference image. These points are recorded and a classifier is trained to
+ recognises them.
+
+ In this class the points detected are assumed to belong to a planar surface.
+ Therefore an homography can be computed between the reference image and the
+ current image if the object is detected in the image.
+
+ A robust method (RANSAC) is used to remove outliers in the matching process.
+
+ The following example shows how to use the class.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/vision/vpPlanarObjectDetector.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Surf Fern classifier only available since 2.1.0
+int main()
+{
+ vpImage<unsigned char> Ireference;
+ vpImage<unsigned char> Icurrent;
+ vpPlanarObjectDetector planar;
+
+ //First grab the reference image Ireference
+
+ //Select a part of the image by clincking on two points which define a rectangle
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Ireference, corners[i]);
+ }
+
+ //Build the reference points (and train the classifier).
+ int nbrRef;
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ nbrRef = planar.buildReference(Ireference, corners[0], height, width);
+
+ //Then grab another image which represents the current image Icurrent
+
+ //Match points between the reference points and the current points computed in the current image.
+ bool isDetected;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+ isDetected = planar.matchPoint(Icurrent, corners[0], height, width);
+
+ //Display the matched points
+ if(isDetected){
+ planar.display(Ireference, Icurrent);
+ vpHomography homography;
+ planar.getHomography(homography);
+ }
+ else{
+ std::cerr << "planar surface not detected in the current image" << std::endl;
+ }
+
+ return(0);
+}
+#else
+int main() {}
+#endif
+ \endcode
+
+*/
+class VISP_EXPORT vpPlanarObjectDetector{
+protected:
+ //! Fern Classifier used to match the points between a reference image and the current image.
+ vpFernClassifier fern;
+
+ //! Computed homography in the ViSP format.
+ vpHomography homography;
+ //! Computed homography in the OpenCV format.
+ cv::Mat H;
+
+ //! The estimated new coordinates of the corners (reprojected using the homography).
+ std::vector<cv::Point2f> dst_corners;
+
+ //! Flag to indicate wether the last computed homography is correct or not.
+ bool isCorrect;
+
+ //! The corners in the reference image
+ std::vector<cv::Point2f> ref_corners;
+
+ //! The ROI for the reference image.
+ cv::Rect modelROI;
+
+ //! Vector of the image point in the current image that match after the deletion of the outliers with the RANSAC.
+ std::vector<vpImagePoint> currentImagePoints;
+ //! Vector of the image point in the reference image that match after the deletion of the outliers with the RANSAC.
+ std::vector<vpImagePoint> refImagePoints;
+
+ //! Minimal number of point to after the ransac needed to suppose that the homography has been correctly computed.
+ unsigned int minNbMatching;
+
+public:
+
+ // constructors and destructors
+ vpPlanarObjectDetector();
+ vpPlanarObjectDetector(const std::string& dataFile, const std::string& objectName);
+ virtual ~vpPlanarObjectDetector();
+
+ // main functions
+ // creation of reference
+ unsigned int buildReference(const vpImage<unsigned char> &I);
+ unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ unsigned int height, unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I,
+ const vpRect rectangle);
+
+ // matching
+ bool matchPoint(const vpImage<unsigned char> &I);
+ bool matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP, const unsigned int height, const unsigned int width);
+ bool matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle);
+ // database management
+ void recordDetector(const std::string& objectName, const std::string& dataFile);
+ void load(const std::string& dataFilename, const std::string& objName);
+
+
+ // display
+ void display(vpImage<unsigned char> &I, bool displayKpts = false);
+ void display(vpImage<unsigned char> &Iref,
+ vpImage<unsigned char> &Icurrent, bool displayKpts = false);
+
+ /*!
+ Return the positions of the detected corners.
+
+ \return a vector of vpImagePoint containing the position of the corners of
+ the planar surface in the current image.
+ */
+ std::vector<vpImagePoint> getDetectedCorners() const;
+
+ /*!
+ Return a reference to the classifier.
+
+ \return The fern classifier.
+ */
+ vpFernClassifier& getFernClassifier() {return this->fern;}
+
+ /*!
+ Return the computed homography between the reference image and the current
+ image.
+
+ \param _H : The computed homography.
+ */
+ inline void getHomography(vpHomography& _H) const { _H = this->homography;}
+
+ /*!
+ Return the number of reference points
+
+ \return Number of reference points.
+ */
+ inline unsigned int getNbRefPoints() {return (unsigned int)currentImagePoints.size() ;}
+
+ /*!
+ Get the i-th reference point.
+
+ \throw vpException if _i is out if bound.
+
+ \param _i : index of the point to get
+ \param _imPoint : image point returned by the
+ */
+ void getReferencePoint(const unsigned int _i, vpImagePoint& _imPoint);
+
+ /*!
+ 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.
+ */
+ void getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint);
+
+ /*!
+ Set the threshold for the minimal number of point to validate the homography.
+ Default value is 10.
+
+ \param _min : the new threshold.
+ */
+ void setMinNbPointValidation(const unsigned int _min){ this->minNbMatching = _min;}
+
+
+ /*!
+ Get the threshold for the minimal number of point to validate the homography.
+ Default value is 10.
+
+ \return : the current threshold.
+ */
+ unsigned int getMinNbPointValidation() const { return this->minNbMatching;}
+
+protected:
+
+ virtual void init();
+ void computeRoi(vpImagePoint* ip, const unsigned int nbpt);
+ void initialiseRefCorners(const cv::Rect& _modelROI);
+};
+
+#endif
+
+#endif /* VPPLANAROBJECTDETECTOR_H_ */
+
+
+
+
+
diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h
new file mode 100644
index 0000000..3565d6f
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpPose.h
@@ -0,0 +1,230 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPose.h
+ \brief Tools for pose computation (pose from point only).
+
+ \author Eric Marchand (INRIA) using code from Francois Chaumette (INRIA)
+ \date April, 6 1999 (first issue)
+*/
+
+#ifndef vpPOSE_HH
+#define vpPOSE_HH
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpRGBa.h>
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+# include <visp3/core/vpList.h>
+#endif
+
+#include <math.h>
+#include <list>
+#include <vector>
+
+/*!
+ \class vpPose
+ \ingroup group_vision_pose
+ \brief Class used for pose computation from N points (pose from point only).
+
+ \note It is also possible to estimate a pose from other features using vpPoseFeatures class.
+
+ To see how to use this class you can follow the \ref tutorial-pose-estimation.
+*/
+
+
+class VISP_EXPORT vpPose
+{
+public:
+ typedef enum
+ {
+ LAGRANGE ,
+ DEMENTHON ,
+ LOWE ,
+ RANSAC ,
+ LAGRANGE_LOWE ,
+ DEMENTHON_LOWE ,
+ VIRTUAL_VS ,
+ DEMENTHON_VIRTUAL_VS,
+ LAGRANGE_VIRTUAL_VS
+ } vpPoseMethodType;
+
+ unsigned int npt ; //!< number of point used in pose computation
+ std::list<vpPoint> listP ; //!< array of point (use here class vpPoint)
+
+ double residual ; //!< compute the residual in meter
+
+protected :
+ double lambda ;//!< parameters use for the virtual visual servoing approach
+
+private:
+ int vvsIterMax ; //! define the maximum number of iteration in VVS
+ //! variable used in the Dementhon approach
+ std::vector<vpPoint> c3d ;
+ //! Flag used to specify if the covariance matrix has to be computed or not.
+ bool computeCovariance;
+ //! Covariance matrix
+ vpMatrix covarianceMatrix;
+
+ unsigned int ransacNbInlierConsensus;
+ int ransacMaxTrials;
+ std::vector<vpPoint> ransacInliers;
+ std::vector<unsigned int> ransacInlierIndex;
+ double ransacThreshold;
+
+protected:
+ double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;
+
+ // method used in poseDementhonPlan()
+ int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;
+
+public:
+ // constructor
+ vpPose() ;
+ //! destructor
+ virtual ~vpPose() ;
+ //! Add a new point in this array
+ void addPoint(const vpPoint& P) ;
+ //! suppress all the point in the array of point
+ void clearPoint() ;
+
+ //! compute the pose for a given method
+ bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
+ //! compute the residual (i.e., the quality of the result)
+ //! compute the residual (in meter for pose M)
+ double computeResidual(const vpHomogeneousMatrix &cMo) const ;
+ //! test the coplanarity of the points
+ bool coplanar(int &coplanar_plane_type) ;
+ void displayModel(vpImage<unsigned char> &I,
+ vpCameraParameters &cam,
+ vpColor col=vpColor::none) ;
+ void displayModel(vpImage<vpRGBa> &I,
+ vpCameraParameters &cam,
+ vpColor col=vpColor::none) ;
+ double distanceToPlaneForCoplanarityTest ;
+ void init() ;
+ //! compute the pose using Dementhon approach (planar object)
+ void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
+ //! compute the pose using Dementhon approach (non planar object)
+ void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
+ //! compute the pose using Lagrange approach (planar object)
+ void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
+ //! compute the pose using Lagrange approach (non planar object)
+ void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
+ //! compute the pose using the Lowe approach (i.e., using the
+ //! Levenberg Marquartd non linear minimization approach)
+ void poseLowe(vpHomogeneousMatrix & cMo) ;
+ //! compute the pose using the Ransac approach
+ bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
+ //! compute the pose using a robust virtual visual servoing approach
+ void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
+ //! compute the pose using virtual visual servoing approach
+ void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
+ void printPoint() ;
+ void setDistanceToPlaneForCoplanarityTest(double d) ;
+ void setLambda(double a) { lambda = a ; }
+ void setVvsIterMax(int nb) { vvsIterMax = nb ; }
+
+ void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
+ void setRansacThreshold(const double &t) {
+ //Test whether or not t is > 0
+ if(t > 0) {
+ ransacThreshold = t;
+ } else {
+ throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
+ }
+ }
+ void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
+ unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
+ std::vector<unsigned int> getRansacInlierIndex() const{ return ransacInlierIndex; }
+ std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
+
+ /*!
+ Set if the covaraince matrix has to be computed in the Virtual Visual Servoing approach.
+
+ \param flag : True if the covariance has to be computed, false otherwise.
+ */
+ void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
+
+ /*!
+ Get the covariance matrix computed in the Virtual Visual Servoing approach.
+
+ \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
+
+ \sa setCovarianceComputation
+ */
+ vpMatrix getCovarianceMatrix() const {
+ if(!computeCovariance)
+ vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
+
+ return covarianceMatrix;
+ }
+
+ static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
+ vpCameraParameters &cam, double size,
+ vpColor col=vpColor::none) ;
+ static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
+ vpCameraParameters &cam, double size,
+ vpColor col=vpColor::none) ;
+ static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
+ vpPoint &p3,vpPoint &p4,
+ double lx, vpCameraParameters & cam,
+ vpHomogeneousMatrix & cMo) ;
+
+ static void findMatch(std::vector<vpPoint> &p2D,
+ std::vector<vpPoint> &p3D,
+ const unsigned int &numberOfInlierToReachAConsensus,
+ const double &threshold,
+ unsigned int &ninliers,
+ std::vector<vpPoint> &listInliers,
+ vpHomogeneousMatrix &cMo,
+ const int &maxNbTrials = 10000);
+} ;
+
+
+#endif
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vision/include/visp3/vision/vpPoseException.h b/modules/vision/include/visp3/vision/vpPoseException.h
new file mode 100644
index 0000000..436a515
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpPoseException.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Error that can be emited by the vpPose class and its derivates
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpPoseException_H
+#define __vpPoseException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* Classes standards. */
+
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpPoseException
+ \ingroup group_vision_pose
+ \brief Error that can be emited by the vpPose class and its derivates.
+ */
+class VISP_EXPORT vpPoseException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpPose member
+ */
+ enum errorCodeEnum
+ {
+ poseError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedERR,
+ //! index out of range
+ outOfRangeError,
+ notEnoughPointError
+ } ;
+
+ public:
+ vpPoseException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpPoseException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpPoseException (const 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
new file mode 100644
index 0000000..78da391
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpPoseFeatures.h
@@ -0,0 +1,736 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation from any features.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPose.h
+ \brief Tools for pose computation from any feature.
+
+ \author Aurelien Yol
+ \date June, 5 2012
+*/
+
+#ifndef vpPoseFeatures_HH
+#define vpPoseFeatures_HH
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpSphere.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpCylinder.h>
+
+#include <vector>
+#include <iostream>
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+#include <tuple>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+//#################################################
+//## Call a function with a tuple as parameters
+//#################################################
+template < unsigned int N >
+struct vpDesiredFeatureBuilderWithTuple
+{
+ template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureWithTuple( featureType &feature,
+ RetType (*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& t,
+ Args &&... args )
+ {
+ vpDesiredFeatureBuilderWithTuple<N-1>::buildDesiredFeatureWithTuple( feature, f, t, std::get<N-1>( t ), args... );
+ }
+};
+
+template <>
+struct vpDesiredFeatureBuilderWithTuple<0>
+{
+ template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureWithTuple( featureType &/* feature */,
+ RetType (*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& /* t */,
+ Args&&... args )
+ {
+ f( args... );
+ }
+};
+
+template <>
+struct vpDesiredFeatureBuilderWithTuple<1>
+{
+ template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureWithTuple( featureType &feature,
+ RetType (*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& t,
+ Args&&... args )
+ {
+ vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple( feature, f, t, feature, args... );
+ }
+};
+
+template < typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
+void buildDesiredFeatureWithTuple( featureType &feature,
+ RetType (*f)(ArgsFunc...),
+ std::tuple<Args...> const& t )
+{
+ vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple( feature, f, t );
+}
+
+//#################################################
+//## Call a function with a tuple as parameters
+//## Object Mode
+//#################################################
+
+template < unsigned int N >
+struct vpDesiredFeatureBuilderObjectWithTuple
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
+ RetType (objType::*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& t,
+ Args &&... args )
+ {
+ vpDesiredFeatureBuilderObjectWithTuple<N-1>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, std::get<N-1>( t ), args... );
+ }
+};
+
+template <>
+struct vpDesiredFeatureBuilderObjectWithTuple<0>
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType & /*feature*/,
+ RetType (objType::*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& /* t */,
+ Args&&... args )
+ {
+ (obj->*f)( args... );
+ }
+};
+
+template <>
+struct vpDesiredFeatureBuilderObjectWithTuple<1>
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
+ static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
+ RetType (objType::*f)( ArgsF... ),
+ const std::tuple<ArgsT...>& t,
+ Args&&... args )
+ {
+ vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, feature, args... );
+ }
+};
+
+template < typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
+void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
+ RetType (objType::*f)(ArgsFunc...),
+ std::tuple<Args...> const& t )
+{
+ vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t );
+}
+
+//#####################################################
+//## Call un function with a tuple as parameters
+//## Track all the parameters with the cMo
+//## Except the first one (must be de "BasicFeature"
+//#####################################################
+
+template < unsigned int N >
+struct vpCurrentFeatureBuilderWithTuple
+{
+ template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureWithTuple( featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (*f)(ArgsF...), std::tuple<ArgsTuple...>& t,
+ ArgsDecomposed &&... args )
+ {
+ auto proj = std::get<N-1>( t );
+ proj.track(cMo);
+ vpCurrentFeatureBuilderWithTuple<N-1>::buildCurrentFeatureWithTuple( feature, cMo, f, t, proj, args... );
+ }
+};
+
+template <>
+struct vpCurrentFeatureBuilderWithTuple<0>
+{
+ template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureWithTuple( featureType &/*feature*/,
+ const vpHomogeneousMatrix & /*cMo*/,
+ RetType (*f)(ArgsF...),
+ std::tuple<ArgsTuple...>&,
+ ArgsDecomposed &&... args )
+ {
+ f( args... );
+ }
+};
+
+template <>
+struct vpCurrentFeatureBuilderWithTuple<1>
+{
+ template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureWithTuple( featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (*f)(ArgsF...),
+ std::tuple<ArgsTuple...>&t,
+ ArgsDecomposed &&... args )
+ {
+ vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple( feature, cMo, f, t, feature, args... );
+ }
+};
+
+template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
+void buildCurrentFeatureWithTuple( featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (*f)(ArgsFunc...),
+ std::tuple<ArgsTuple...> &t )
+{
+ vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple( feature, cMo, f, t );
+}
+
+//#####################################################
+//## Call un function with a tuple as parameters
+//## Track all the parameters with the cMo
+//## Except the first one (must be de "BasicFeature"
+//## Object Mode
+//#####################################################
+
+template < unsigned int N >
+struct vpCurrentFeatureBuilderObjectWithTuple
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsF...),
+ std::tuple<ArgsTuple...>& t,
+ ArgsDecomposed &&... args )
+ {
+ auto proj = std::get<N-1>( t );
+ proj.track(cMo);
+ vpCurrentFeatureBuilderObjectWithTuple<N-1>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, proj, args... );
+ }
+};
+
+template <>
+struct vpCurrentFeatureBuilderObjectWithTuple<0>
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &/*feature*/,
+ const vpHomogeneousMatrix &/*cMo*/,
+ RetType (objType::*f)(ArgsF...),
+ std::tuple<ArgsTuple...>&,
+ ArgsDecomposed &&... args )
+ {
+ (obj->*f)( args... );
+ }
+};
+
+template <>
+struct vpCurrentFeatureBuilderObjectWithTuple<1>
+{
+ template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
+ static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsF...),
+ std::tuple<ArgsTuple...>&t,
+ ArgsDecomposed &&... args )
+ {
+ vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, feature, args... );
+ }
+};
+
+template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
+void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
+ const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsFunc...),
+ std::tuple<ArgsTuple...> &t )
+{
+ vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t );
+}
+
+//#################################################
+//## Call that will be used in our vpPoseFeatures
+//## to store the specific features.
+//#################################################
+/*!
+ \class vpPoseSpecificFeature
+ \ingroup group_vision_pose
+ \brief Class used to define specific features that could be considered in pose estimation from visual features implemented in vpPoseFeatures.
+*/
+class VISP_EXPORT vpPoseSpecificFeature
+{
+public:
+ vpPoseSpecificFeature(){}
+ virtual ~vpPoseSpecificFeature(){};
+
+ virtual vpColVector error() = 0;
+ virtual vpMatrix currentInteraction() = 0;
+ virtual void createDesired() = 0;
+ virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
+};
+
+//#################################################
+//## Template for all kind of specific features
+//#################################################
+
+/*!
+ \class vpPoseSpecificFeatureTemplate
+ \ingroup group_vision_pose
+ \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
+*/
+template< typename featureType, typename RetType, typename ...Args >
+class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
+{
+private:
+ featureType desiredFeature;
+ featureType currentFeature;
+ std::tuple<Args...> *tuple;
+ RetType (*func_ptr)(Args...);
+
+public:
+ vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
+ {
+ func_ptr = f_ptr; //std::move(f_ptr);
+ tuple = new std::tuple<Args...>(args...);
+ }
+
+ virtual ~vpPoseSpecificFeatureTemplate()
+ {
+ delete tuple;
+ };
+
+ virtual void createDesired(){
+ buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple);
+ }
+
+ virtual vpColVector error(){
+ //std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() << std::endl;
+ return currentFeature.error(desiredFeature);
+ }
+
+ virtual vpMatrix currentInteraction(){
+ return currentFeature.interaction();
+ }
+
+ virtual void createCurrent(const vpHomogeneousMatrix &cMo){
+ buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
+ }
+};
+
+//#################################################
+//## Template for all kind of specific features
+//## Object Mode
+//#################################################
+
+/*!
+ \class vpPoseSpecificFeatureTemplateObject
+ \ingroup group_vision_pose
+ \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
+*/
+template< typename ObjectType, typename featureType, typename RetType, typename ...Args >
+class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
+{
+private:
+ featureType desiredFeature;
+ featureType currentFeature;
+ std::tuple<Args...> *tuple;
+ RetType (ObjectType::*func_ptr)(Args...);
+ ObjectType* obj;
+
+public:
+ vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
+ {
+ func_ptr = f_ptr; //std::move(f_ptr);
+ tuple = new std::tuple<Args...>(args...);
+ obj = o;
+ }
+
+ virtual ~vpPoseSpecificFeatureTemplateObject()
+ {
+ delete tuple;
+ };
+
+ virtual void createDesired(){
+ buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple);
+ }
+
+ virtual vpColVector error(){
+ return currentFeature.error(desiredFeature);
+ }
+
+ virtual vpMatrix currentInteraction(){
+ return currentFeature.interaction();
+ }
+
+ virtual void createCurrent(const vpHomogeneousMatrix &cMo){
+ buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
+ }
+};
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+#endif //VISP_HAVE_CPP11_COMPATIBILITY
+
+/*!
+ \class vpPoseFeatures
+ \brief Tools for pose computation from any feature.
+ \ingroup group_vision_pose
+
+ This class allows to estimate a pose by virtual visual servoing from visual features. The features that are considered are points, segments, lines, ellipses.
+ If the compiler is compatible with C++ 11, it is possible to introduce specific features that are not directly implemented in ViSP.
+ */
+class VISP_EXPORT vpPoseFeatures
+{
+public:
+ /*!
+ Method that will be used to estimate the pose from visual features.
+ */
+ typedef enum
+ {
+ VIRTUAL_VS, /*!< Virtual visual servoing approach. */
+ ROBUST_VIRTUAL_VS /*!< Robust virtual visual servoing approach. */
+ } vpPoseFeaturesMethodType;
+
+private:
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ template<typename FeatureType, typename FirstParamType>
+ struct vpDuo{
+ FeatureType *desiredFeature;
+ FirstParamType firstParam;
+ vpDuo() : desiredFeature(NULL), firstParam() {}
+ };
+
+ template<typename FeatureType, typename FirstParamType, typename SecondParamType>
+ struct vpTrio{
+ FeatureType *desiredFeature;
+ FirstParamType firstParam;
+ SecondParamType secondParam;
+
+ vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
+ };
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ unsigned int maxSize;
+ unsigned int totalSize;
+ unsigned int vvsIterMax;
+ double lambda;
+
+ bool verbose;
+
+ bool computeCovariance;
+ vpMatrix covarianceMatrix;
+
+ //vpFeaturePoint
+ std::vector<vpDuo<vpFeaturePoint,vpPoint> > featurePoint_Point_list;
+ //vpFeaturePoint3D
+ std::vector<vpDuo<vpFeaturePoint3D,vpPoint> > featurePoint3D_Point_list;
+ //vpFeatureVanishingPoint
+ std::vector<vpDuo<vpFeatureVanishingPoint,vpPoint> > featureVanishingPoint_Point_list;
+ std::vector<vpTrio<vpFeatureVanishingPoint,vpLine,vpLine> > featureVanishingPoint_DuoLine_list;
+ //vpFeatureEllipse
+ std::vector<vpDuo<vpFeatureEllipse,vpSphere> > featureEllipse_Sphere_list;
+ std::vector<vpDuo<vpFeatureEllipse,vpCircle> > featureEllipse_Circle_list;
+ //vpFeatureLine
+ std::vector<vpDuo<vpFeatureLine,vpLine> > featureLine_Line_list;
+ std::vector<vpTrio<vpFeatureLine,vpCylinder,int> > featureLine_DuoLineInt_List;
+ //vpFeatureSegment
+ std::vector<vpTrio<vpFeatureSegment,vpPoint,vpPoint> > featureSegment_DuoPoints_list;
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ //Specific features
+ std::vector<vpPoseSpecificFeature*> featureSpecific_list;
+#endif
+
+public:
+
+ vpPoseFeatures();
+ virtual ~vpPoseFeatures();
+
+ // ! Features addition
+ void addFeaturePoint(const vpPoint&);
+
+ void addFeaturePoint3D(const vpPoint&);
+
+ void addFeatureVanishingPoint(const vpPoint&);
+ void addFeatureVanishingPoint(const vpLine&, const vpLine&);
+
+ void addFeatureEllipse(const vpCircle&);
+ void addFeatureEllipse(const vpSphere&);
+
+ void addFeatureLine(const vpLine&);
+ void addFeatureLine(const vpCylinder&, const int &line);
+
+ void addFeatureSegment(vpPoint &, vpPoint&);
+
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ template<typename RetType, typename ...ArgsFunc, typename ...Args>
+ void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args);
+
+ template<typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
+ void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args);
+#endif
+
+ void clear();
+
+ // ! Pose computation
+ void computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
+
+ /*!
+ Get the covariance matrix of the pose parameters computed by virtual visual servoing.
+
+ \warning By default, the covariance matrix is not computed. To enable the computation, use setCovarianceComputation().
+ */
+ vpMatrix getCovarianceMatrix() const {
+ if(!computeCovariance)
+ vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
+
+ return covarianceMatrix;
+ }
+
+ /*!
+ Get the gain that is used to compute the pose with the control law \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
+
+ \return Value of \f$\lambda\f$, the gain of the control law.
+ */
+ double getLambda(){ return lambda; }
+
+ /*!
+ Get the maximum number of iterations of the virtual visual servoing (VVS) scheme implemented in computePose().
+
+ \return Maximum number of iterations used during VVS minimization.
+ */
+ unsigned int getVVSIterMax(){ return vvsIterMax; }
+
+ /*!
+ Enable or disable covariance computation of the pose parameters.
+
+ \param flag : True if the covariance has to be computed, false otherwise.
+ */
+ void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
+
+ /*!
+ Set the gain used in the virtual visual servoing scheme : \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
+
+ \param val : Value of the gain \f$\lambda\f$.
+ */
+ void setLambda(const double &val){ lambda = val; }
+
+ /*!
+ Set the maximum number of iterations used in computePose().
+
+ \param val : Maximum number of iteration used in the VVS scheme.
+ */
+ void setVVSIterMax(const unsigned int &val){ vvsIterMax = val; }
+
+ /*!
+ Turn the verbose mode ON / OFF.
+
+ \param mode : new verbose state. True to turn ON, false otherwise.
+ */
+ void setVerbose(const bool &mode){ verbose = mode; }
+
+
+private:
+ void error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L);
+
+ void computePoseVVS(vpHomogeneousMatrix & cMo);
+ void computePoseRobustVVS(vpHomogeneousMatrix & cMo);
+};
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+/*!
+ Add a specific feature for the pose computation.
+
+ \param fct_ptr : pointer on the function used to create the feature.
+ \param args : List of function parameters;
+ First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
+ others are supposed to be derived from vpForwardProjection (redefine track() function)
+
+ \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/vision/vpPoseFeatures.h>
+
+void vp_createPoint(vpFeaturePoint &fp,const vpPoint &p){
+ vpFeatureBuilder::create(fp,p);
+}
+
+void vp_createTwoPoint(vpFeaturePoint &fp,const vpPoint &p, const vpPoint&p2){
+ vpFeatureBuilder::create(fp,p);
+ vpFeatureBuilder::create(fp,p2);
+}
+
+void vp_createLine(vpFeatureLine &fp,const vpLine &l){
+ vpFeatureBuilder::create(fp,l);
+}
+
+int main()
+{
+ vpPoseFeatures pose;
+
+ vpPoint pts[4];
+ vpLine line;
+
+ //... Projection of the points and line
+
+ vpFeaturePoint fp;
+ vpFeatureLine fl;
+ void (*ptr)(vpFeaturePoint&, const vpPoint&) = &vpFeatureBuilder::create;
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ pose.addSpecificFeature(ptr, fp, pts[0]);
+ pose.addSpecificFeature(&vp_createPoint, fp, pts[1]);
+ pose.addSpecificFeature(&vp_createTwoPoint, fp, pts[2], pts[3]);
+ pose.addSpecificFeature(&vp_createLine, fl, line);
+#endif
+
+ //... Pose Computation
+
+ return 0;
+}
+ \endcode
+*/
+template< typename RetType, typename ...ArgsFunc, typename ...Args>
+void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args)
+{
+ typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
+ typedef typename std::remove_reference<featureTypeReference>::type featureType;
+ featureSpecific_list.push_back(
+ new vpPoseSpecificFeatureTemplate< featureType, RetType, ArgsFunc... >(fct_ptr,std::forward<ArgsFunc>(args)...)
+ );
+
+ featureSpecific_list.back()->createDesired();
+
+ totalSize++;
+ if(featureSpecific_list.size() > maxSize)
+ maxSize = featureSpecific_list.size();
+}
+
+/*!
+ Add a specific feature for the pose computation.
+
+ \param obj : object used to call the function defined by fct_ptr.
+ \param fct_ptr : pointer on the function used to create the feature.
+ \param args : List of function parameters;
+ First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
+ others are supposed to be derived from vpForwardProjection (redefine track() function)
+
+ \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
+
+ \code
+#include <visp3/core/vpConfig.h>
+#include <visp3/vision/vpPoseFeatures.h>
+
+class vp_createClass{
+public:
+ vp_createClass(){}
+
+ int vp_createPoint(vpFeaturePoint &fp,const vpPoint &p){
+ vpFeatureBuilder::create(fp,p);
+ return 2;
+ }
+
+ void vp_createTwoPoint(vpFeaturePoint &fp,const vpPoint &p, const vpPoint &p2){
+ vpFeatureBuilder::create(fp,p);
+ vpFeatureBuilder::create(fp,p2);
+ }
+
+ void vp_createLine(vpFeatureLine &fp,const vpLine &l){
+ vpFeatureBuilder::create(fp,l);
+ }
+};
+
+int main()
+{
+ vpPoseFeatures pose;
+
+ vpPoint pts[3];
+ vpLine line;
+
+ //... Projection of the points and line
+
+ vpFeaturePoint fp;
+ vpFeatureLine fl;
+
+ vp_createClass cpClass;
+ int (vp_createClass::*ptrClassPoint)(vpFeaturePoint&, const vpPoint&) = &vp_createClass::vp_createPoint;
+ void (vp_createClass::*ptrClassTwoPoint)(vpFeaturePoint&, const vpPoint&, const vpPoint&) = &vp_createClass::vp_createTwoPoint;
+ void (vp_createClass::*ptrClassLine)(vpFeatureLine &, const vpLine &) = &vp_createClass::vp_createLine;
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ pose.addSpecificFeature(&cpClass, ptrClassPoint, fp, pts[0]);
+ pose.addSpecificFeature(&cpClass, ptrClassTwoPoint, fp, pts[1], pts[2]);
+ pose.addSpecificFeature(&cpClass, ptrClassLine, fl, line);
+#endif
+
+ //... Pose Computation
+
+ return 0;
+}
+ \endcode
+*/
+template< typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
+void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args)
+{
+ typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
+ typedef typename std::remove_reference<featureTypeReference>::type featureType;
+ featureSpecific_list.push_back(
+ new vpPoseSpecificFeatureTemplateObject< ObjType, featureType, RetType, ArgsFunc... >(obj, fct_ptr,std::forward<ArgsFunc>(args)...)
+ );
+
+ featureSpecific_list.back()->createDesired();
+
+ totalSize++;
+ if(featureSpecific_list.size() > maxSize)
+ maxSize = featureSpecific_list.size();
+}
+#endif
+
+#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
+
+#endif
diff --git a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h
new file mode 100644
index 0000000..9447c0d
--- /dev/null
+++ b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load configuration for vpKeyPoint class.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpXmlConfigParserKeyPoint.cpp
+ \brief Definition of the vpXmlConfigParserKeyPoint class member functions.
+ Class vpXmlConfigParserKeyPoint allows to load configuration defined in a XML file for vpKeyPoint class.
+
+*/
+
+#ifndef __vpXmlConfigParserKeyPoint_h__
+#define __vpXmlConfigParserKeyPoint_h__
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <stdlib.h>
+
+#include <libxml/xmlmemory.h> // Functions of libxml.
+
+#include <visp3/core/vpXmlParser.h>
+
+/*!
+ \class vpXmlConfigParserKeyPoint
+ \ingroup group_vision_keypoints
+*/
+class VISP_EXPORT vpXmlConfigParserKeyPoint: public vpXmlParser
+{
+
+public:
+ /*! Predefined xml node identifier. */
+ typedef enum {
+ conf, /*!< Identifier associated to the root tag. */
+ detector, /*!< Identifier associated to the detector tag. */
+ extractor, /*!< Identifier associated to the extractor tag. */
+ matcher, /*!< Identifier associated to the matcher tag. */
+ name, /*!< Identifier associated to the name tag. */
+ matching_method, /*!< Identifier associated to the matching_method tag. */
+ constant_factor_distance_threshold, /*!< Identifier associated to the constant_factor_distance_threshold tag. */
+ std_distance_threshold, /*!< Identifier associated to the std_distance_threshold tag. */
+ ratio_distance_threshold, /*!< Identifier associated to the ratio_distance_threshold tag. */
+ std_and_ratio_distance_threshold, /*!< Identifier associated to the std_and_ratio_distance_threshold tag. */
+ no_filter_matching, /*!< Identifier associated to the no_filter_matching tag. */
+ matching_factor_threshold, /*!< Identifier associated to the matching_factor_threshold tag. */
+ matching_ratio_threshold, /*!< Identifier associated to the matching_ratio_threshold tag. */
+ ransac, /*!< Identifier associated to the ransac tag. */
+ use_ransac_vvs, /*!< Identifier associated to the use_ransac_vvs tag. */
+ use_ransac_consensus_percentage, /*!< Identifier associated to the use_ransac_consensus_percentage tag. */
+ nb_ransac_iterations, /*!< Identifier associated to the nb_ransac_iterations tag. */
+ ransac_reprojection_error, /*!< Identifier associated to the ransac_reprojection_error tag. */
+ nb_ransac_min_inlier_count, /*!< Identifier associated to the nb_ransac_min_inlier_count tag. */
+ ransac_threshold, /*!< Identifier associated to the ransac_threshold tag. */
+ ransac_consensus_percentage /*!< Identifier associated to the ransac_consensus_percentage tag. */
+ } vpNodeIdentifier;
+
+ /*! Enumerator for the different filtering matching method. */
+ typedef enum {
+ constantFactorDistanceThreshold, /*!< Keep all the points below a constant factor threshold. */
+ stdDistanceThreshold, /*!< Keep all the points below a minimal distance + the standard deviation. */
+ ratioDistanceThreshold, /*!< Keep all the points enough discriminated. */
+ stdAndRatioDistanceThreshold, /*!< Keep all the points which fall with the two conditions. */
+ noFilterMatching /*!< No filtering. */
+ } vpMatchingMethodEnum;
+
+private :
+ //! Name of the keypoint detector.
+ std::string m_detectorName;
+ //! Name of the keypoint extractor.
+ std::string m_extractorName;
+ //! Name of the keypoint matcher.
+ std::string m_matcherName;
+ //! Factor value for filtering method: constantFactorDistanceThreshold.
+ double m_matchingFactorThreshold;
+ //! Filtering method.
+ vpMatchingMethodEnum m_matchingMethod;
+ //! Ratio value for filtering method: ratioDistanceThreshold.
+ double m_matchingRatioThreshold;
+ //! Maximum number of iterations for the Ransac method.
+ int m_nbRansacIterations;
+ //! Minimum number of inliers for the Ransac method.
+ int m_nbRansacMinInlierCount;
+ //! Percentage value of inliers compared to outliers for the Ransac method.
+ double m_ransacConsensusPercentage;
+ //! Maximum reprojection error (in pixel for OpenCV method) to consider a point as an inlier.
+ double m_ransacReprojectionError;
+ //! Maximum error (in meter for ViSP method) to consider a point as an inlier.
+ double m_ransacThreshold;
+ //! If true, the cardinality of the consensus is based upon a percentage, otherwise
+ //it is based on a fixed number.
+ bool m_useRansacConsensusPercentage;
+ //! If true, use ViSP Ransac VVS pose estimation method, otherwise use OpenCV method.
+ bool m_useRansacVVS;
+
+
+public:
+
+ vpXmlConfigParserKeyPoint();
+
+ /*!
+ Get the detector name.
+
+ \return The detector name.
+ */
+ inline std::string getDetectorName() const {
+ return m_detectorName;
+ }
+
+ /*!
+ Get the extractor name.
+
+ \return The extractor name.
+ */
+ inline std::string getExtractorName() const {
+ return m_extractorName;
+ }
+ /*!
+ Get the matcher name.
+
+ \return The detector name.
+ */
+ inline std::string getMatcherName() const {
+ return m_matcherName;
+ }
+ /*!
+ Get the factor value.
+
+ \return The factor value for the filtering method: constantFactorDistanceThreshold.
+ */
+ inline double getMatchingFactorThreshold() const {
+ return m_matchingFactorThreshold;
+ }
+
+ /*!
+ Get the filtering method.
+
+ \return The filtering method.
+ */
+ inline vpMatchingMethodEnum getMatchingMethod() const {
+ return m_matchingMethod;
+ }
+
+ /*!
+ Get the ratio value.
+
+ \return The factor value for the filtering method: ratioDistanceThreshold.
+ */
+ inline double getMatchingRatioThreshold() const {
+ return m_matchingRatioThreshold;
+ }
+
+ /*!
+ Get the maximum number of iterations for the Ransac method.
+
+ \return The maximum number of iterations for the Ransac method.
+ */
+ inline int getNbRansacIterations() const {
+ return m_nbRansacIterations;
+ }
+
+ /*!
+ Get the minimum number of inliers for the Ransac method.
+
+ \return The minimum number of inliers for the Ransac method.
+ */
+ inline int getNbRansacMinInlierCount() const {
+ return m_nbRansacMinInlierCount;
+ }
+
+ /*!
+ Get the percentage value of inliers for the Ransac method.
+
+ \return The percentage value of inliers for the Ransac method.
+ */
+ inline double getRansacConsensusPercentage() const {
+ return m_ransacConsensusPercentage;
+ }
+
+ /*!
+ Get the maximum reprojection error for a candidate inlier for the Ransac method.
+
+ \return The maximum reprojection error for the Ransac method.
+ */
+ inline double getRansacReprojectionError() const {
+ return m_ransacReprojectionError;
+ }
+
+ /*!
+ Get the maximum error for a candidate inlier for the Ransac method.
+
+ \return The maximum error for the Ransac method.
+ */
+ inline double getRansacThreshold() const {
+ return m_ransacThreshold;
+ }
+
+ /*!
+ Get the flag state to choose between a percentage of inliers or a fixed number.
+
+ \return True to use a percentage value for inliers, false otherwise.
+ */
+ inline bool getUseRansacConsensusPercentage() const {
+ return m_useRansacConsensusPercentage;
+ }
+
+ /*!
+ Get the flag state to choose between OpenCV Ransac pose estimation or ViSP
+ Ransac VVS pose estimation.
+
+ \return True to use ViSP method, false otherwise.
+ */
+ inline bool getUseRansacVVSPoseEstimation() const {
+ return m_useRansacVVS;
+ }
+
+ /*!
+ Parse an XML file to get configuration value for vpKeyPoint class.
+
+ \param filename : filename of the XML file to parse
+ */
+ void parse(const std::string &filename);
+
+
+private:
+
+ void init();
+ void read_detector(xmlDocPtr doc, xmlNodePtr node);
+ void read_extractor(xmlDocPtr doc, xmlNodePtr node);
+ void read_matcher(xmlDocPtr doc, xmlNodePtr node);
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+ void read_ransac(xmlDocPtr doc, xmlNodePtr node);
+ virtual void writeMainClass(xmlNodePtr ){};
+
+};
+#endif //VISP_HAVE_XML2
+#endif
diff --git a/modules/vision/src/calibration/vpCalibration.cpp b/modules/vision/src/calibration/vpCalibration.cpp
new file mode 100644
index 0000000..4a91380
--- /dev/null
+++ b/modules/vision/src/calibration/vpCalibration.cpp
@@ -0,0 +1,831 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Camera calibration.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ * Anthony Saunier
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpCalibration.cpp
+ \brief Tools for camera calibration.
+*/
+
+#include <visp3/vision/vpCalibration.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+
+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 ;
+
+ residual = residual_dist = 1000.;
+
+ LoX.clear() ;
+ LoY.clear() ;
+ LoZ.clear() ;
+ Lip.clear() ;
+
+ return 0 ;
+}
+
+/*!
+ Default constructor.
+ */
+vpCalibration::vpCalibration()
+ : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
+ npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), residual_dist(1000.)
+{
+ init() ;
+}
+/*!
+ Copy constructor.
+ */
+vpCalibration::vpCalibration(const vpCalibration &c)
+ : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
+ npt(0), LoX(), LoY(), LoZ(), 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 ;
+ LoX = twinCalibration.LoX ;
+ LoY = twinCalibration.LoY ;
+ LoZ = twinCalibration.LoZ ;
+ Lip = twinCalibration.Lip ;
+
+ residual = twinCalibration.residual;
+ cMo = twinCalibration.cMo;
+ residual_dist = twinCalibration.residual_dist;
+ cMo_dist = twinCalibration.cMo_dist ;
+
+ cam = twinCalibration.cam ;
+ cam_dist = twinCalibration.cam_dist ;
+
+ rMe = twinCalibration.rMe;
+
+ eMc = twinCalibration.eMc;
+ eMc_dist = twinCalibration.eMc_dist;
+
+ return (*this);
+}
+
+
+/*!
+ Delete the array of points.
+*/
+int vpCalibration::clearPoint()
+{
+ LoX.clear() ;
+ LoY.clear() ;
+ LoZ.clear() ;
+ Lip.clear() ;
+ npt = 0 ;
+
+ 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);
+ LoY.push_back(Y);
+ LoZ.push_back(Z);
+
+ Lip.push_back(ip);
+
+ npt++ ;
+
+ return 0 ;
+}
+
+/*!
+ Compute the pose cMo
+ \param camera : camera intrinsic parameters used for computation.
+ \param cMo_est : computed pose
+ */
+void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
+{
+ // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
+ vpPose pose ;
+ // the list of point is cleared (if that's not done before)
+ pose.clearPoint() ;
+ // we set the 3D points coordinates (in meter !) in the object/world frame
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ for (unsigned int i =0 ; i < npt ; i++)
+ {
+ vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
+ double x=0,y=0 ;
+ vpPixelMeterConversion::convertPoint(camera, *it_Lip, x,y) ;
+ P.set_x(x) ;
+ P.set_y(y) ;
+
+ pose.addPoint(P);
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+ }
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
+ vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
+
+ // compute the initial pose using Lagrange method followed by a non linear
+ // minimisation method
+ // Pose by Lagrange it provides an initialization of the pose
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ double residual_lagrange = pose.computeResidual(cMo_lagrange);
+
+ // compute the initial pose using Dementhon method followed by a non linear
+ // minimisation method
+ // Pose by Dementhon it provides an initialization of the pose
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ double residual_dementhon = pose.computeResidual(cMo_dementhon);
+
+ //we keep the better initialization
+ if (residual_lagrange < residual_dementhon)
+ cMo_est = cMo_lagrange;
+ else
+ cMo_est = cMo_dementhon;
+
+ // the pose is now refined using the virtual visual servoing approach
+ // Warning: cMo needs to be initialized otherwise it may diverge
+ pose.computePose(vpPose::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 ;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ double u0 = camera.get_u0() ;
+ double v0 = camera.get_v0() ;
+ double px = camera.get_px() ;
+ double py = camera.get_py() ;
+ vpImagePoint ip;
+
+ for (unsigned int i =0 ; i < npt ; i++)
+ {
+ double oX = *it_LoX;
+ double oY = *it_LoY;
+ double oZ = *it_LoZ;
+
+ double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
+ double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
+ double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
+
+ double x = cX/cZ ;
+ double y = cY/cZ ;
+
+ ip = *it_Lip;
+ double u = ip.get_u() ;
+ double v = ip.get_v() ;
+
+ double xp = u0 + x*px;
+ double yp = v0 + y*py;
+
+ residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+ }
+ 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 ;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ double u0 = camera.get_u0() ;
+ double v0 = camera.get_v0() ;
+ double px = camera.get_px() ;
+ double py = camera.get_py() ;
+ double kud = camera.get_kud() ;
+ double kdu = camera.get_kdu() ;
+
+ double inv_px = 1/px;
+ double inv_py = 1/px;
+ vpImagePoint ip;
+
+ for (unsigned int i =0 ; i < npt ; i++)
+ {
+ double oX = *it_LoX;
+ double oY = *it_LoY;
+ double oZ = *it_LoZ;
+
+ double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
+ double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
+ double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
+
+ double x = cX/cZ ;
+ double y = cY/cZ ;
+
+ ip = *it_Lip;
+ double u = ip.get_u() ;
+ double v = ip.get_v() ;
+
+ double r2ud = 1+kud*(vpMath::sqr(x)+vpMath::sqr(y)) ;
+
+ double xp = u0 + x*px*r2ud;
+ double yp = v0 + y*py*r2ud;
+
+ residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+
+ double r2du = (vpMath::sqr((u-u0)*inv_px)+vpMath::sqr((v-v0)*inv_py)) ;
+
+ xp = u0 + x*px - kdu*(u-u0)*r2du;
+ yp = v0 + y*py - kdu*(v-v0)*r2du;
+
+ residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+ }
+ residual_ /=2;
+
+ this->residual_dist = residual_;
+ 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 0 if the calibration computation succeed.
+*/
+int vpCalibration::computeCalibration(vpCalibrationMethodType method,
+ vpHomogeneousMatrix &cMo_est,
+ vpCameraParameters &cam_est,
+ bool verbose)
+{
+ try{
+ computePose(cam_est,cMo_est);
+ switch (method)
+ {
+ case CALIB_LAGRANGE :
+ case CALIB_LAGRANGE_VIRTUAL_VS :
+ {
+ calibLagrange(cam_est, cMo_est);
+ }
+ break;
+ case CALIB_VIRTUAL_VS:
+ case CALIB_VIRTUAL_VS_DIST:
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
+ default:
+ break;
+ }
+
+ switch (method)
+ {
+ case CALIB_VIRTUAL_VS:
+ case CALIB_VIRTUAL_VS_DIST:
+ case CALIB_LAGRANGE_VIRTUAL_VS:
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
+ {
+ if (verbose){std::cout << "start calibration without distortion"<< std::endl;}
+ calibVVS(cam_est, cMo_est, verbose);
+ }
+ break ;
+ case CALIB_LAGRANGE:
+ default:
+ break;
+ }
+ this->cMo = cMo_est;
+ this->cMo_dist = cMo_est;
+
+ //Print camera parameters
+ if(verbose){
+ // std::cout << "Camera parameters without distortion :" << std::endl;
+ cam_est.printParameters();
+ }
+
+ this->cam = cam_est;
+
+ switch (method)
+ {
+ case CALIB_VIRTUAL_VS_DIST:
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
+ {
+ if (verbose){std::cout << "start calibration with distortion"<< std::endl;}
+ calibVVSWithDistortion(cam_est, cMo_est, verbose);
+ }
+ break ;
+ case CALIB_LAGRANGE:
+ case CALIB_VIRTUAL_VS:
+ case CALIB_LAGRANGE_VIRTUAL_VS:
+ default:
+ break;
+ }
+ //Print camera parameters
+ if(verbose){
+ // std::cout << "Camera parameters without distortion :" << std::endl;
+ this->cam.printParameters();
+ // std::cout << "Camera parameters with distortion :" << std::endl;
+ cam_est.printParameters();
+ }
+
+ this->cam_dist = cam_est ;
+
+ this->cMo_dist = cMo_est;
+ return 0 ;
+ }
+ 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 0 if the computation was managed succeed.
+*/
+int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method,
+ std::vector<vpCalibration> &table_cal,
+ vpCameraParameters& cam_est,
+ double &globalReprojectionError,
+ bool verbose)
+{
+ try{
+ unsigned int nbPose = (unsigned int) table_cal.size();
+ for(unsigned int i=0;i<nbPose;i++){
+ if(table_cal[i].get_npt()>3)
+ table_cal[i].computePose(cam_est,table_cal[i].cMo);
+ }
+ switch (method) {
+ case CALIB_LAGRANGE : {
+ if(nbPose > 1){
+ std::cout << "this calibration method is not available in" << std::endl
+ << "vpCalibration::computeCalibrationMulti()" << std::endl;
+ return -1 ;
+ }
+ else {
+ table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
+ table_cal[0].cam = cam_est ;
+ table_cal[0].cam_dist = cam_est ;
+ table_cal[0].cMo_dist = table_cal[0].cMo ;
+ }
+ break;
+ }
+ case CALIB_LAGRANGE_VIRTUAL_VS :
+ 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;
+ return -1 ;
+ }
+ else {
+ table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
+ table_cal[0].cam = cam_est ;
+ table_cal[0].cam_dist = cam_est ;
+ table_cal[0].cMo_dist = table_cal[0].cMo ;
+ }
+ calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
+ break ;
+ }
+ case CALIB_VIRTUAL_VS:
+ case CALIB_VIRTUAL_VS_DIST: {
+ calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
+ break ;
+ }
+ }
+ //Print camera parameters
+ if(verbose){
+ // std::cout << "Camera parameters without distortion :" << std::endl;
+ cam_est.printParameters();
+ }
+
+ switch (method)
+ {
+ case CALIB_LAGRANGE :
+ case CALIB_LAGRANGE_VIRTUAL_VS :
+ case CALIB_VIRTUAL_VS:
+ verbose = false ;
+ break;
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST :
+ case CALIB_VIRTUAL_VS_DIST:
+ {
+ if(verbose)
+ std::cout << "Compute camera parameters with distortion"<<std::endl;
+
+ calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
+ }
+ break ;
+ }
+ //Print camera parameters
+ if(verbose){
+ // std::cout << "Camera parameters without distortion :" << std::endl;
+ table_cal[0].cam.printParameters();
+ // std::cout << "Camera parameters with distortion:" << std::endl;
+ cam_est.printParameters();
+ std::cout<<std::endl;
+ }
+ return 0 ;
+ }
+ catch(...){ throw; }
+}
+
+/*!
+ \brief Compute the multi-image calibration of effector-camera from R. Tsai and R. Lenz \cite Tsai89a.
+
+ Compute extrinsic camera parameters : the constant transformation from
+ the end-effector to the camera frame \f${^e}{\bf M}_c\f$ considering the camera model with or without distortion.
+
+ \param[in] table_cal : Vector of vpCalibration that contains for each index a couple of
+ \f${^r}{\bf M}_e\f$ (world to end-effector) and \f${^c}{\bf M}_o\f$ (camera to object)
+ transformations.
+ \param[out] eMc : Estimated pose of the camera in relation to the end-effector
+ considering the camera model without distortion.
+ \param[out] eMc_dist : Estimated pose of the camera in relation to the
+ end-effector considering the model with distortion.
+ \return 0 if the computation managed, -1 if less than three poses are provides as input.
+*/
+int vpCalibration::computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
+ vpHomogeneousMatrix& eMc,
+ vpHomogeneousMatrix& eMc_dist)
+{
+ try{
+ unsigned int nbPose = (unsigned int)table_cal.size();
+ if (nbPose > 2){
+ std::vector<vpHomogeneousMatrix> table_cMo(nbPose);
+ std::vector<vpHomogeneousMatrix> table_cMo_dist(nbPose);
+ std::vector<vpHomogeneousMatrix> table_rMe(nbPose);
+
+ for(unsigned int i=0;i<nbPose;i++){
+ table_cMo[i] = table_cal[i].cMo;
+ table_cMo_dist[i] = table_cal[i].cMo_dist;
+ table_rMe[i] = table_cal[i].rMe;
+ }
+ calibrationTsai(table_cMo, table_rMe, eMc);
+ calibrationTsai(table_cMo_dist, table_rMe, eMc_dist);
+
+ return 0;
+ }
+ else{
+ vpERROR_TRACE("Three images are needed to compute Tsai calibration !\n");
+ return -1;
+ }
+ }
+ 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)
+{
+ std::ofstream f(filename) ;
+ vpImagePoint ip;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ f.precision(10);
+ f.setf(std::ios::fixed,std::ios::floatfield);
+ f << LoX.size() << std::endl ;
+
+ for (unsigned int i =0 ; i < LoX.size() ; i++)
+ {
+
+ double oX = *it_LoX;
+ double oY = *it_LoY;
+ double oZ = *it_LoZ;
+
+ ip = *it_Lip;
+ double u = ip.get_u() ;
+ double v = ip.get_v() ;
+
+ f << oX <<" " << oY << " " << oZ << " " << u << " " << v <<std::endl ;
+
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+
+ }
+
+ f.close() ;
+ 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)
+{
+ vpImagePoint ip;
+ std::ifstream f;
+ f.open(filename);
+ if (! f.fail()){
+ unsigned int n ;
+ f >> n ;
+ std::cout << "There are "<< n <<" point on the calibration grid " << std::endl ;
+
+ clearPoint() ;
+
+ if (n > 100000)
+ throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
+
+ for (unsigned int i=0 ; i < n ; i++)
+ {
+ double x, y, z, u, v ;
+ f >> x >> y >> z >> u >> v ;
+ std::cout << x <<" "<<y <<" "<<z<<" "<<u<<" "<<v <<std::endl ;
+ ip.set_u( u );
+ ip.set_v( v );
+ addPoint(x, y, z, ip) ;
+ }
+
+ f.close() ;
+ return 0 ;
+ }
+ 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<double> &oX, std::list<double> &oY, std::list<double> &oZ, bool verbose)
+{
+ try{
+ std::ifstream f;
+ f.open(filename);
+ if (! f.fail()){
+
+ f >> n ;
+ if(verbose)
+ std::cout << "There are "<< n <<" points on the calibration grid " << std::endl ;
+ int no_pt;
+ double x,y,z;
+
+ // clear the list
+ oX.clear();
+ oY.clear();
+ oZ.clear();
+
+ if (n > 100000)
+ throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
+
+ for (unsigned int i=0 ; i < n ; i++)
+ {
+ f >> no_pt >> x >> y >> z ;
+ if(verbose){
+ std::cout << no_pt <<std::endl ;
+ std::cout << x <<" "<< y <<" "<< z <<std::endl ;
+ }
+ oX.push_back(x) ;
+ oY.push_back(y) ;
+ oZ.push_back(z) ;
+ }
+
+ f.close() ;
+ }
+ else{
+ return -1;
+ }
+ }
+ catch(...){return -1;}
+ return 0 ;
+}
+
+/*!
+ 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 vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color,
+ unsigned int thickness, int subsampling_factor)
+{
+
+ for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
+ vpImagePoint ip = *it;
+ if (subsampling_factor > 1.) {
+ ip.set_u( ip.get_u() / subsampling_factor);
+ ip.set_v( ip.get_v() / subsampling_factor);
+ }
+ vpDisplay::displayCross(I, ip, 12, color, thickness) ;
+ }
+ 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<unsigned char> &I, vpColor color,
+ unsigned int thickness, int subsampling_factor)
+{
+ double u0_dist = cam_dist.get_u0() / subsampling_factor;
+ double v0_dist = cam_dist.get_v0() / subsampling_factor;
+ double px_dist = cam_dist.get_px() / subsampling_factor;
+ double py_dist = cam_dist.get_py() / subsampling_factor;
+ double kud_dist = cam_dist.get_kud() ;
+ // double kdu_dist = cam_dist.get_kdu() ;
+
+ // double u0 = cam.get_u0() ;
+ // double v0 = cam.get_v0() ;
+ // double px = cam.get_px() ;
+ // double py = cam.get_py() ;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+
+ for (unsigned int i =0 ; i < npt ; i++)
+ {
+ double oX = *it_LoX;
+ 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) ;
+
+ cX = oX*cMo_dist[0][0]+oY*cMo_dist[0][1]+oZ*cMo_dist[0][2]+cMo_dist[0][3];
+ cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
+ cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
+
+ x = cX/cZ ;
+ y = cY/cZ ;
+
+ double r2 = 1+kud_dist*(vpMath::sqr(x)+vpMath::sqr(y)) ;
+
+ vpImagePoint ip;
+ ip.set_u( u0_dist + x*px_dist*r2 );
+ ip.set_v( v0_dist + y*py_dist*r2 );
+
+ vpDisplay::displayCross(I, ip, 6, color, thickness) ;
+ ///////////////////////////////////////
+
+ // std::cout << oX << " " << oY << " " <<oZ << std::endl ;
+ // I.getClick() ;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ }
+ return 0;
+}
diff --git a/modules/vision/src/calibration/vpCalibrationTools.cpp b/modules/vision/src/calibration/vpCalibrationTools.cpp
new file mode 100644
index 0000000..2a60a3a
--- /dev/null
+++ b/modules/vision/src/calibration/vpCalibrationTools.cpp
@@ -0,0 +1,1837 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Camera calibration.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+#include <visp3/vision/vpCalibration.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#undef MAX
+#undef MIN
+
+void
+vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est)
+{
+
+ vpMatrix A(2*npt,3) ;
+ vpMatrix B(2*npt,9) ;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ vpImagePoint ip;
+
+ for (unsigned int i = 0 ; i < npt ; i++)
+ {
+
+ double x0 = *it_LoX;
+ double y0 = *it_LoY;
+ double z0 = *it_LoZ;
+
+ ip = *it_Lip;
+
+ double xi = ip.get_u() ;
+ double yi = ip.get_v() ;
+
+ A[2*i][0] = x0*xi;
+ A[2*i][1] = y0*xi;
+ A[2*i][2] = z0*xi;
+ B[2*i][0] = -x0;
+ B[2*i][1] = -y0;
+ B[2*i][2] = -z0;
+ B[2*i][3] = 0.0;
+ B[2*i][4] = 0.0;
+ B[2*i][5] = 0.0;
+ B[2*i][6] = -1.0;
+ B[2*i][7] = 0.0;
+ B[2*i][8] = xi;
+ A[2*i+1][0] = x0*yi;
+ A[2*i+1][1] = y0*yi;
+ A[2*i+1][2] = z0*yi;
+ B[2*i+1][0] = 0.0;
+ B[2*i+1][1] = 0.0;
+ B[2*i+1][2] = 0.0;
+ B[2*i+1][3] = -x0;
+ B[2*i+1][4] = -y0;
+ B[2*i+1][5] = -z0;
+ B[2*i+1][6] = 0.0;
+ B[2*i+1][7] = -1.0;
+ B[2*i+1][8] = yi;
+
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+ }
+
+ vpMatrix BtB ; /* compute B^T B */
+ BtB = B.t() * B ;
+
+ /* compute (B^T B)^(-1) */
+ /* input : btb (dimension 9 x 9) = (B^T B) */
+ /* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
+
+ vpMatrix BtBinv ;
+ BtBinv = BtB.pseudoInverse(1e-16) ;
+
+ vpMatrix BtA ;
+ BtA = B.t()*A ; /* compute B^T A */
+
+
+ vpMatrix r ;
+ r = BtBinv*BtA ; /* compute (B^T B)^(-1) B^T A */
+
+ vpMatrix e ; /* compute - A^T B (B^T B)^(-1) B^T A*/
+ e = -(A.t()*B)*r ;
+
+ vpMatrix AtA ; /* compute A^T A */
+ AtA = A.AtA() ;
+
+ e += AtA ; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
+
+ vpColVector x1(3) ;
+ vpColVector x2 ;
+
+ e.svd(x1,AtA) ;// destructive on e
+ // eigenvector computation of E corresponding to the min eigenvalue.
+ /* SVmax computation*/
+ double svm = 0.0;
+ unsigned int imin = 1;
+ for (unsigned int i=0;i<x1.getRows();i++)
+ {
+ if (x1[i] > svm)
+ {
+ svm = x1[i];
+ imin = i;
+ }
+ }
+
+ svm *= 0.1; /* for the rank */
+
+ for (unsigned int i=0;i<x1.getRows();i++)
+ {
+ if (x1[i] < x1[imin]) imin = i;
+ }
+
+ for (unsigned int i=0;i<x1.getRows();i++)
+ x1[i] = AtA[i][imin];
+
+ x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
+
+
+ vpColVector sol(12) ;
+ vpColVector resul(7) ;
+ for (unsigned int i=0;i<3;i++) sol[i] = x1[i]; /* X_1 */
+ for (unsigned int i=0;i<9;i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
+ {
+ sol[i+3] = x2[i];
+ }
+
+ if (sol[11] < 0.0) for (unsigned int i=0;i<12;i++) sol[i] = -sol[i]; /* since Z0 > 0 */
+
+ resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2]; /* u0 */
+
+ resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2]; /* v0 */
+
+ resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5] /* px */
+ -resul[0]*resul[0]);
+ resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8] /* py */
+ -resul[1]*resul[1]);
+
+ cam_est.initPersProjWithoutDistortion(resul[2],resul[3],resul[0],resul[1]);
+
+ resul[4] = (sol[9]-sol[11]*resul[0])/resul[2]; /* X0 */
+ resul[5] = (sol[10]-sol[11]*resul[1])/resul[3]; /* Y0 */
+ resul[6] = sol[11]; /* Z0 */
+
+ vpMatrix rd(3,3) ;
+ /* fill rotation matrix */
+ for (unsigned int i=0;i<3;i++) rd[0][i] = (sol[i+3]-sol[i]*resul[0])/resul[2];
+ for (unsigned int i=0;i<3;i++) rd[1][i] = (sol[i+6]-sol[i]*resul[1])/resul[3];
+ for (unsigned int i=0;i<3;i++) rd[2][i] = sol[i];
+
+ // std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
+ // std::cout << rd*rd.t() ;
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ for (unsigned int j=0 ; j < 3 ; j++)
+ cMo_est[i][j] = rd[i][j];
+ }
+ for (unsigned int i=0 ; i < 3 ; i++) cMo_est[i][3] = resul[i+4] ;
+
+ this->cMo = cMo_est ;
+ this->cMo_dist = cMo_est;
+
+ double deviation,deviation_dist;
+ this->computeStdDeviation(deviation,deviation_dist);
+
+}
+
+
+void
+vpCalibration::calibVVS(vpCameraParameters &cam_est,
+ vpHomogeneousMatrix &cMo_est,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int n_points = npt ;
+
+ vpColVector oX(n_points), cX(n_points) ;
+ vpColVector oY(n_points), cY(n_points) ;
+ vpColVector oZ(n_points), cZ(n_points) ;
+ vpColVector u(n_points) ;
+ vpColVector v(n_points) ;
+
+ vpColVector P(2*n_points) ;
+ vpColVector Pd(2*n_points) ;
+
+ vpImagePoint ip;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ for (unsigned int i =0 ; i < n_points ; i++)
+ {
+ oX[i] = *it_LoX;
+ oY[i] = *it_LoY;
+ oZ[i] = *it_LoZ;
+
+ ip = *it_Lip;
+
+ u[i] = ip.get_u() ;
+ v[i] = ip.get_v() ;
+
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+ }
+
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+ iter++ ;
+ residu_1 = r ;
+
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
+
+ r = 0 ;
+
+ for (unsigned int i=0 ; i < n_points; i++)
+ {
+ cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
+ cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
+ cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
+
+ Pd[2*i] = u[i] ;
+ Pd[2*i+1] = v[i] ;
+
+ P[2*i] = cX[i]/cZ[i]*px + u0 ;
+ P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
+
+ r += ((vpMath::sqr(P[2*i]-Pd[2*i]) + vpMath::sqr(P[2*i+1]-Pd[2*i+1]))) ;
+ }
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/n_points ;
+
+ vpMatrix L(n_points*2,10) ;
+ for (unsigned int i=0 ; i < n_points; i++)
+ {
+ double x = cX[i] ;
+ double y = cY[i] ;
+ double z = cZ[i] ;
+ double inv_z = 1/z;
+
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ //---------------
+ {
+ L[2*i][0] = px * (-inv_z) ;
+ L[2*i][1] = 0 ;
+ L[2*i][2] = px*X*inv_z ;
+ L[2*i][3] = px*X*Y ;
+ L[2*i][4] = -px*(1+X*X) ;
+ L[2*i][5] = px*Y ;
+ }
+ {
+ L[2*i][6]= 1 ;
+ L[2*i][7]= 0 ;
+ L[2*i][8]= X ;
+ L[2*i][9]= 0;
+ }
+ {
+ L[2*i+1][0] = 0 ;
+ L[2*i+1][1] = py*(-inv_z) ;
+ L[2*i+1][2] = py*(Y*inv_z) ;
+ L[2*i+1][3] = py* (1+Y*Y) ;
+ L[2*i+1][4] = -py*X*Y ;
+ L[2*i+1][5] = -py*X ;
+ }
+ {
+ L[2*i+1][6]= 0 ;
+ L[2*i+1][7]= 1 ;
+ L[2*i+1][8]= 0;
+ L[2*i+1][9]= Y ;
+ }
+ } // end interaction
+ vpMatrix Lp ;
+ Lp = L.pseudoInverse(1e-10) ;
+
+ vpColVector e ;
+ e = Lp*error ;
+
+ vpColVector Tc, Tc_v(6) ;
+ Tc = -e*gain ;
+
+ // Tc_v =0 ;
+ for (unsigned int i=0 ; i <6 ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithoutDistortion(px+Tc[8],py+Tc[9],
+ u0+Tc[6],v0+Tc[7]) ;
+
+ cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+ this->cMo = cMo_est;
+ this->cMo_dist = cMo_est;
+ this->residual = r;
+ this->residual_dist = r;
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+
+void
+vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam_est,
+ double &globalReprojectionError,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int nbPoint[256]; //number of points by image
+ unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPose = (unsigned int)table_cal.size();
+ unsigned int nbPose6 = 6*nbPose;
+
+ for (unsigned int i=0; i<nbPose ; i++)
+ {
+ nbPoint[i] = table_cal[i].npt;
+ nbPointTotal += nbPoint[i];
+ }
+
+ if (nbPointTotal < 4) {
+ //vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError,
+ "Not enough point to calibrate")) ;
+ }
+
+ vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
+ vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
+ vpColVector u(nbPointTotal) ;
+ vpColVector v(nbPointTotal) ;
+
+ vpColVector P(2*nbPointTotal) ;
+ vpColVector Pd(2*nbPointTotal) ;
+ vpImagePoint ip;
+
+ unsigned int curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
+ std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
+ std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
+
+ for (unsigned int i =0 ; i < nbPoint[p] ; i++)
+ {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
+
+ ip = *it_Lip;
+ u[curPoint] = ip.get_u() ;
+ v[curPoint] = ip.get_v() ;
+
+ ++ it_LoX;
+ ++ it_LoY;
+ ++ it_LoZ;
+ ++ it_Lip;
+
+ curPoint++;
+ }
+ }
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+
+ iter++ ;
+ residu_1 = r ;
+
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
+
+ r = 0 ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint2 = 2*curPoint;
+
+ cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
+ +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
+ +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
+ +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+
+ Pd[curPoint2] = u[curPoint] ;
+ Pd[curPoint2+1] = v[curPoint] ;
+
+ P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
+ P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
+
+ r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
+ + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
+ curPoint++;
+ }
+ }
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/nbPointTotal ;
+
+ vpMatrix L(nbPointTotal*2,nbPose6+4) ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ unsigned int q = 6*p;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint2 = 2*curPoint;
+ unsigned int curPoint21 = curPoint2 + 1;
+
+ double x = cX[curPoint] ;
+ double y = cY[curPoint] ;
+ double z = cZ[curPoint] ;
+
+ double inv_z = 1/z;
+
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ //---------------
+ {
+ {
+ L[curPoint2][q] = px * (-inv_z) ;
+ L[curPoint2][q+1] = 0 ;
+ L[curPoint2][q+2] = px*(X*inv_z) ;
+ L[curPoint2][q+3] = px*X*Y ;
+ L[curPoint2][q+4] = -px*(1+X*X) ;
+ L[curPoint2][q+5] = px*Y ;
+ }
+ {
+ L[curPoint2][nbPose6]= 1 ;
+ L[curPoint2][nbPose6+1]= 0 ;
+ L[curPoint2][nbPose6+2]= X ;
+ L[curPoint2][nbPose6+3]= 0;
+ }
+ {
+ L[curPoint21][q] = 0 ;
+ L[curPoint21][q+1] = py*(-inv_z) ;
+ L[curPoint21][q+2] = py*(Y*inv_z) ;
+ L[curPoint21][q+3] = py* (1+Y*Y) ;
+ L[curPoint21][q+4] = -py*X*Y ;
+ L[curPoint21][q+5] = -py*X ;
+ }
+ {
+ L[curPoint21][nbPose6]= 0 ;
+ L[curPoint21][nbPose6+1]= 1 ;
+ L[curPoint21][nbPose6+2]= 0;
+ L[curPoint21][nbPose6+3]= Y ;
+ }
+
+ }
+ curPoint++;
+ } // end interaction
+ }
+ vpMatrix Lp ;
+ Lp = L.pseudoInverse(1e-10) ;
+
+ vpColVector e ;
+ e = Lp*error ;
+
+ vpColVector Tc, Tc_v(nbPose6) ;
+ Tc = -e*gain ;
+
+ // Tc_v =0 ;
+ for (unsigned int i = 0 ; i < nbPose6 ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
+ py+Tc[nbPose6+3],
+ u0+Tc[nbPose6],
+ v0+Tc[nbPose6+1]) ;
+
+ // cam.setKd(get_kd() + Tc[10]) ;
+ vpColVector Tc_v_Tmp(6) ;
+
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ for (unsigned int i = 0 ; i < 6 ; i++)
+ Tc_v_Tmp[i] = Tc_v[6*p + i];
+
+ table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
+ * table_cal[p].cMo;
+ }
+
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ table_cal[p].cMo_dist = table_cal[p].cMo ;
+ table_cal[p].cam = cam_est;
+ table_cal[p].cam_dist = cam_est;
+ double deviation,deviation_dist;
+ table_cal[p].computeStdDeviation(deviation,deviation_dist);
+ }
+ globalReprojectionError = sqrt(r/nbPointTotal);
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+
+void
+vpCalibration::calibVVSWithDistortion(vpCameraParameters& cam_est,
+ vpHomogeneousMatrix& cMo_est,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int n_points =npt ;
+
+ vpColVector oX(n_points), cX(n_points) ;
+ vpColVector oY(n_points), cY(n_points) ;
+ vpColVector oZ(n_points), cZ(n_points) ;
+ vpColVector u(n_points) ;
+ vpColVector v(n_points) ;
+
+ vpColVector P(4*n_points) ;
+ vpColVector Pd(4*n_points) ;
+
+ std::list<double>::const_iterator it_LoX = LoX.begin();
+ std::list<double>::const_iterator it_LoY = LoY.begin();
+ std::list<double>::const_iterator it_LoZ = LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
+
+ vpImagePoint ip;
+
+ for (unsigned int i =0 ; i < n_points ; i++)
+ {
+ oX[i] = *it_LoX;
+ oY[i] = *it_LoY;
+ oZ[i] = *it_LoZ;
+
+ ip = *it_Lip;
+ u[i] = ip.get_u();
+ v[i] = ip.get_v();
+
+ ++ it_LoX;
+ ++ it_LoY;
+ ++ it_LoZ;
+ ++ it_Lip;
+ }
+
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+ iter++ ;
+ residu_1 = r ;
+
+ r = 0 ;
+ double u0 = cam_est.get_u0() ;
+ double v0 = cam_est.get_v0() ;
+
+ double px = cam_est.get_px() ;
+ double py = cam_est.get_py() ;
+
+ double inv_px = 1/px ;
+ double inv_py = 1/py ;
+
+ double kud = cam_est.get_kud() ;
+ double kdu = cam_est.get_kdu() ;
+
+ double k2ud = 2*kud;
+ double k2du = 2*kdu;
+ vpMatrix L(n_points*4,12) ;
+
+ for (unsigned int i=0 ; i < n_points; i++)
+ {
+ unsigned int i4 = 4*i;
+ unsigned int i41 = 4*i+1;
+ unsigned int i42 = 4*i+2;
+ unsigned int i43 = 4*i+3;
+
+ cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
+ cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
+ cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
+
+ double x = cX[i] ;
+ double y = cY[i] ;
+ double z = cZ[i] ;
+ double inv_z = 1/z;
+
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ double X2 = X*X;
+ double Y2 = Y*Y;
+ double XY = X*Y;
+
+ double up = u[i] ;
+ double vp = v[i] ;
+
+ Pd[i4] = up ;
+ Pd[i41] = vp ;
+
+ double up0 = up - u0;
+ double vp0 = vp - v0;
+
+ double xp0 = up0 * inv_px;
+ double xp02 = xp0 *xp0 ;
+
+ double yp0 = vp0 * inv_py;
+ double yp02 = yp0 * yp0;
+
+ double r2du = xp02 + yp02 ;
+ double kr2du = kdu * r2du;
+
+ P[i4] = u0 + px*X - kr2du *(up0) ;
+ P[i41] = v0 + py*Y - kr2du *(vp0) ;
+
+ double r2ud = X2 + Y2 ;
+ double kr2ud = 1 + kud * r2ud;
+
+ double Axx = px*(kr2ud+k2ud*X2);
+ double Axy = px*k2ud*XY;
+ double Ayy = py*(kr2ud+k2ud*Y2);
+ double Ayx = py*k2ud*XY;
+
+ Pd[i42] = up ;
+ Pd[i43] = vp ;
+
+ P[i42] = u0 + px*X*kr2ud ;
+ P[i43] = v0 + py*Y*kr2ud ;
+
+ r += (vpMath::sqr(P[i4]-Pd[i4]) +
+ vpMath::sqr(P[i41]-Pd[i41]) +
+ vpMath::sqr(P[i42]-Pd[i42]) +
+ vpMath::sqr(P[i43]-Pd[i43]))*0.5;
+
+ //--distorted to undistorted
+ {
+ {
+ L[i4][0] = px * (-inv_z) ;
+ L[i4][1] = 0 ;
+ L[i4][2] = px*X*inv_z ;
+ L[i4][3] = px*X*Y ;
+ L[i4][4] = -px*(1+X2) ;
+ L[i4][5] = px*Y ;
+ }
+ {
+ L[i4][6]= 1 + kr2du + k2du*xp02 ;
+ L[i4][7]= k2du*up0*yp0*inv_py ;
+ L[i4][8]= X + k2du*xp02*xp0 ;
+ L[i4][9]= k2du*up0*yp02*inv_py ;
+ L[i4][10] = -(up0)*(r2du) ;
+ L[i4][11] = 0 ;
+ }
+ {
+ L[i41][0] = 0 ;
+ L[i41][1] = py*(-inv_z) ;
+ L[i41][2] = py*Y*inv_z ;
+ L[i41][3] = py* (1+Y2) ;
+ L[i41][4] = -py*XY ;
+ L[i41][5] = -py*X ;
+ }
+ {
+ L[i41][6]= k2du*xp0*vp0*inv_px ;
+ L[i41][7]= 1 + kr2du + k2du*yp02;
+ L[i41][8]= k2du*vp0*xp02*inv_px;
+ L[i41][9]= Y + k2du*yp02*yp0;
+ L[i41][10] = -vp0*r2du ;
+ L[i41][11] = 0 ;
+ }
+ //---undistorted to distorted
+ {
+ L[i42][0] = Axx*(-inv_z) ;
+ L[i42][1] = Axy*(-inv_z) ;
+ L[i42][2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
+ L[i42][3] = Axx*X*Y + Axy*(1+Y2);
+ L[i42][4] = -Axx*(1+X2) - Axy*XY;
+ L[i42][5] = Axx*Y -Axy*X;
+ }
+ {
+ L[i42][6]= 1 ;
+ L[i42][7]= 0 ;
+ L[i42][8]= X*kr2ud ;
+ L[i42][9]= 0;
+ L[i42][10] = 0 ;
+ L[i42][11] = px*X*r2ud ;
+ }
+ {
+ L[i43][0] = Ayx*(-inv_z) ;
+ L[i43][1] = Ayy*(-inv_z) ;
+ L[i43][2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
+ L[i43][3] = Ayx*XY + Ayy*(1+Y2) ;
+ L[i43][4] = -Ayx*(1+X2) -Ayy*XY ;
+ L[i43][5] = Ayx*Y -Ayy*X;
+ }
+ {
+ L[i43][6]= 0 ;
+ L[i43][7]= 1;
+ L[i43][8]= 0;
+ L[i43][9]= Y*kr2ud ;
+ L[i43][10] = 0 ;
+ L[i43][11] = py*Y*r2ud ;
+ }
+ } // end interaction
+ } // end interaction
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/n_points ;
+
+ vpMatrix Lp ;
+ Lp = L.pseudoInverse(1e-10) ;
+
+ vpColVector e ;
+ e = Lp*error ;
+
+ vpColVector Tc, Tc_v(6) ;
+ Tc = -e*gain ;
+
+ for (unsigned int i=0 ; i <6 ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9],
+ u0 + Tc[6], v0 + Tc[7],
+ kud + Tc[11],
+ kdu + Tc[10]);
+
+ cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+ this->residual_dist = r;
+ this->cMo_dist = cMo_est ;
+ this->cam_dist = cam_est ;
+
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+
+
+void
+vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam_est, double &globalReprojectionError,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int nbPoint[1024]; //number of points by image
+ unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPose = (unsigned int)table_cal.size();
+ unsigned int nbPose6 = 6*nbPose;
+ for (unsigned int i=0; i<nbPose ; i++)
+ {
+ nbPoint[i] = table_cal[i].npt;
+ nbPointTotal += nbPoint[i];
+ }
+
+ if (nbPointTotal < 4)
+ {
+ //vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError,
+ "Not enough point to calibrate")) ;
+ }
+
+ vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
+ vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
+ vpColVector u(nbPointTotal) ;
+ vpColVector v(nbPointTotal) ;
+
+ vpColVector P(4*nbPointTotal) ;
+ vpColVector Pd(4*nbPointTotal) ;
+ vpImagePoint ip;
+
+ unsigned int curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
+ std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
+ std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
+
+ for (unsigned int i =0 ; i < nbPoint[p] ; i++)
+ {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
+
+ ip = *it_Lip;
+ u[curPoint] = ip.get_u() ;
+ v[curPoint] = ip.get_v() ;
+
+ ++ it_LoX;
+ ++ it_LoY;
+ ++ it_LoZ;
+ ++ it_Lip;
+ curPoint++;
+ }
+ }
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+ iter++ ;
+ residu_1 = r ;
+
+ r = 0 ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
+ +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
+ +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
+ +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+
+ curPoint++;
+ }
+ }
+
+
+ vpMatrix L(nbPointTotal*4,nbPose6+6) ;
+ curPoint = 0 ; //current point indice
+ double px = cam_est.get_px() ;
+ double py = cam_est.get_py() ;
+ double u0 = cam_est.get_u0() ;
+ double v0 = cam_est.get_v0() ;
+
+ double inv_px = 1/px ;
+ double inv_py = 1/py ;
+
+ double kud = cam_est.get_kud() ;
+ double kdu = cam_est.get_kdu() ;
+
+ double k2ud = 2*kud;
+ double k2du = 2*kdu;
+
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ unsigned int q = 6*p;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint4 = 4*curPoint;
+ double x = cX[curPoint] ;
+ double y = cY[curPoint] ;
+ double z = cZ[curPoint] ;
+
+ double inv_z = 1/z;
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ double X2 = X*X;
+ double Y2 = Y*Y;
+ double XY = X*Y;
+
+ double up = u[curPoint] ;
+ double vp = v[curPoint] ;
+
+ Pd[curPoint4] = up ;
+ Pd[curPoint4+1] = vp ;
+
+ double up0 = up - u0;
+ double vp0 = vp - v0;
+
+ double xp0 = up0 * inv_px;
+ double xp02 = xp0 *xp0 ;
+
+ double yp0 = vp0 * inv_py;
+ double yp02 = yp0 * yp0;
+
+ double r2du = xp02 + yp02 ;
+ double kr2du = kdu * r2du;
+
+ P[curPoint4] = u0 + px*X - kr2du *(up0) ;
+ P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
+
+ double r2ud = X2 + Y2 ;
+ double kr2ud = 1 + kud * r2ud;
+
+ double Axx = px*(kr2ud+k2ud*X2);
+ double Axy = px*k2ud*XY;
+ double Ayy = py*(kr2ud+k2ud*Y2);
+ double Ayx = py*k2ud*XY;
+
+ Pd[curPoint4+2] = up ;
+ Pd[curPoint4+3] = vp ;
+
+ P[curPoint4+2] = u0 + px*X*kr2ud ;
+ P[curPoint4+3] = v0 + py*Y*kr2ud ;
+
+ r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
+ vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
+ vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
+ vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
+
+ unsigned int curInd = curPoint4;
+ //---------------
+ {
+ {
+ L[curInd][q] = px * (-inv_z) ;
+ L[curInd][q+1] = 0 ;
+ L[curInd][q+2] = px*X*inv_z ;
+ L[curInd][q+3] = px*X*Y ;
+ L[curInd][q+4] = -px*(1+X2) ;
+ L[curInd][q+5] = px*Y ;
+ }
+ {
+ L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
+ L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
+ L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
+ L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
+ L[curInd][nbPose6+4] = -(up0)*(r2du) ;
+ L[curInd][nbPose6+5] = 0 ;
+ }
+ curInd++;
+ {
+ L[curInd][q] = 0 ;
+ L[curInd][q+1] = py*(-inv_z) ;
+ L[curInd][q+2] = py*Y*inv_z ;
+ L[curInd][q+3] = py* (1+Y2) ;
+ L[curInd][q+4] = -py*XY ;
+ L[curInd][q+5] = -py*X ;
+ }
+ {
+ L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
+ L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
+ L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
+ L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
+ L[curInd][nbPose6+4] = -vp0*r2du ;
+ L[curInd][nbPose6+5] = 0 ;
+ }
+ curInd++;
+ //---undistorted to distorted
+ {
+ L[curInd][q] = Axx*(-inv_z) ;
+ L[curInd][q+1] = Axy*(-inv_z) ;
+ L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
+ L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
+ L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
+ L[curInd][q+5] = Axx*Y -Axy*X;
+ }
+ {
+ L[curInd][nbPose6]= 1 ;
+ L[curInd][nbPose6+1]= 0 ;
+ L[curInd][nbPose6+2]= X*kr2ud ;
+ L[curInd][nbPose6+3]= 0;
+ L[curInd][nbPose6+4] = 0 ;
+ L[curInd][nbPose6+5] = px*X*r2ud ;
+ }
+ curInd++;
+ {
+ L[curInd][q] = Ayx*(-inv_z) ;
+ L[curInd][q+1] = Ayy*(-inv_z) ;
+ L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
+ L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
+ L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
+ L[curInd][q+5] = Ayx*Y -Ayy*X;
+ }
+ {
+ L[curInd][nbPose6]= 0 ;
+ L[curInd][nbPose6+1]= 1;
+ L[curInd][nbPose6+2]= 0;
+ L[curInd][nbPose6+3]= Y*kr2ud ;
+ L[curInd][nbPose6+4] = 0 ;
+ L[curInd][nbPose6+5] = py*Y*r2ud ;
+ }
+ } // end interaction
+ curPoint++;
+ } // end interaction
+ }
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/nbPointTotal ;
+
+ vpMatrix Lp ;
+ /*double rank =*/
+ L.pseudoInverse(Lp,1e-10) ;
+ vpColVector e ;
+ e = Lp*error ;
+ vpColVector Tc, Tc_v(6*nbPose) ;
+ Tc = -e*gain ;
+ for (unsigned int i = 0 ; i < 6*nbPose ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
+ u0+Tc[nbPose6], v0+Tc[nbPose6+1],
+ kud + Tc[nbPose6+5],
+ kdu + Tc[nbPose6+4]);
+
+ vpColVector Tc_v_Tmp(6) ;
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ for (unsigned int i = 0 ; i < 6 ; i++)
+ Tc_v_Tmp[i] = Tc_v[6*p + i];
+
+ table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
+ * table_cal[p].cMo_dist;
+ }
+ if (verbose)
+ std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
+ //std::cout << " residual: " << r << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+
+ double perViewError;
+ double totalError = 0;
+ int totalPoints = 0;
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ table_cal[p].cam_dist = cam_est ;
+ perViewError = table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
+ totalError += perViewError*perViewError * table_cal[p].npt;
+ totalPoints += (int)table_cal[p].npt;
+ }
+ globalReprojectionError = sqrt(r/(nbPointTotal));
+
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+
+/*!
+ \brief calibration method of effector-camera from R. Tsai and R. Lorenz \cite Tsai89a.
+
+ Compute extrinsic camera parameters : the constant transformation from
+ the effector to the camera coordinates (eMc).
+
+ \param cMo : vector of homogeneous matrices representing the transformation
+ between the camera and the scene (input)
+ \param rMe : vector of homogeneous matrices representing the transformation
+ between the effector (where the camera is fixed) and the reference coordinates
+ (base of the manipulator) (input). Must be the same size as cMo.
+ \param eMc : homogeneous matrix representing the transformation
+ between the effector and the camera (output)
+*/
+void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
+ std::vector<vpHomogeneousMatrix>& rMe,
+ vpHomogeneousMatrix &eMc){
+
+ vpColVector x ;
+ unsigned int nbPose = (unsigned int)cMo.size();
+ if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
+ {
+ vpMatrix A ;
+ vpColVector B ;
+ unsigned int k = 0 ;
+ // for all couples ij
+ for (unsigned int i=0 ; i < nbPose ; i++)
+ {
+ vpRotationMatrix rRei, ciRo ;
+ rMe[i].extract(rRei) ;
+ cMo[i].extract(ciRo) ;
+ //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
+
+ for (unsigned int j=0 ; j < nbPose ; j++)
+ {
+ if (j>i) // we don't use two times same couples...
+ {
+ vpRotationMatrix rRej, cjRo ;
+ rMe[j].extract(rRej) ;
+ cMo[j].extract(cjRo) ;
+ //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
+
+ vpRotationMatrix rReij = rRej.t() * rRei;
+
+ vpRotationMatrix cijRo = cjRo * ciRo.t();
+
+ vpThetaUVector rPeij(rReij);
+
+ double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
+ + rPeij[2]*rPeij[2]);
+
+ for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
+
+ vpThetaUVector cijPo(cijRo) ;
+ theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
+ + cijPo[2]*cijPo[2]);
+ for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
+
+ vpMatrix As;
+ vpColVector b(3) ;
+
+ As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
+
+ b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
+
+ if (k==0)
+ {
+ A = As ;
+ B = b ;
+ }
+ else
+ {
+ A = vpMatrix::stack(A,As) ;
+ B = vpColVector::stack(B,b) ;
+ }
+ k++ ;
+ }
+ }
+ }
+
+ // the linear system is defined
+ // x = AtA^-1AtB is solved
+ vpMatrix AtA = A.AtA() ;
+
+ vpMatrix Ap ;
+ AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
+ x = Ap*A.t()*B ;
+
+// {
+// // Residual
+// vpColVector residual;
+// residual = A*x-B;
+// std::cout << "Residual: " << std::endl << residual << std::endl;
+
+// double res = 0;
+// for (int i=0; i < residual.getRows(); i++)
+// res += residual[i]*residual[i];
+// res = sqrt(res/residual.getRows());
+// printf("Mean residual = %lf\n",res);
+// }
+
+ // extraction of theta and U
+ double theta ;
+ double d=x.sumSquare() ;
+ for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
+ theta = sqrt(x.sumSquare())/2 ;
+ theta = 2*asin(theta) ;
+ //if (theta !=0)
+ if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
+ {
+ for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
+ }
+ else
+ x = 0 ;
+ }
+
+ // Building of the rotation matrix eRc
+ vpThetaUVector xP(x[0],x[1],x[2]);
+ vpRotationMatrix eRc(xP);
+
+ {
+ vpMatrix A ;
+ vpColVector B ;
+ // Building of the system for the translation estimation
+ // for all couples ij
+ vpRotationMatrix I3 ;
+ I3.eye() ;
+ int k = 0 ;
+ for (unsigned int i=0 ; i < nbPose ; i++)
+ {
+ vpRotationMatrix rRei, ciRo ;
+ vpTranslationVector rTei, ciTo ;
+ rMe[i].extract(rRei) ;
+ cMo[i].extract(ciRo) ;
+ rMe[i].extract(rTei) ;
+ cMo[i].extract(ciTo) ;
+
+
+ for (unsigned int j=0 ; j < nbPose ; j++)
+ {
+ if (j>i) // we don't use two times same couples...
+ {
+
+ vpRotationMatrix rRej, cjRo ;
+ rMe[j].extract(rRej) ;
+ cMo[j].extract(cjRo) ;
+
+ vpTranslationVector rTej, cjTo ;
+ rMe[j].extract(rTej) ;
+ cMo[j].extract(cjTo) ;
+
+ vpRotationMatrix rReij = rRej.t() * rRei ;
+
+ vpTranslationVector rTeij = rTej+ (-rTei);
+
+ rTeij = rRej.t()*rTeij ;
+
+ vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
+
+ vpTranslationVector b ;
+ b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
+
+ if (k==0)
+ {
+ A = a ;
+ B = b ;
+ }
+ else
+ {
+ A = vpMatrix::stack(A,a) ;
+ B = vpColVector::stack(B,b) ;
+ }
+ k++ ;
+ }
+ }
+ }
+
+ // the linear system is solved
+ // x = AtA^-1AtB is solved
+ vpMatrix AtA = A.AtA() ;
+ vpMatrix Ap ;
+ vpColVector AeTc ;
+ AtA.pseudoInverse(Ap, 1e-6) ;
+ AeTc = Ap*A.t()*B ;
+
+// {
+// // residual
+// vpColVector residual;
+// residual = A*AeTc-B;
+// std::cout << "Residual: " << std::endl << residual << std::endl;
+// double res = 0;
+// for (int i=0; i < residual.getRows(); i++)
+// res += residual[i]*residual[i];
+// res = sqrt(res/residual.getRows());
+// printf("mean residual = %lf\n",res);
+// }
+
+ vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
+
+ eMc.insert(eTc) ;
+ eMc.insert(eRc) ;
+ }
+}
+
+
+void
+vpCalibration::calibVVSMulti(unsigned int nbPose,
+ vpCalibration table_cal[],
+ vpCameraParameters &cam_est,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int nbPoint[256]; //number of points by image
+ unsigned int nbPointTotal = 0; //total number of points
+
+ unsigned int nbPose6 = 6*nbPose;
+
+ for (unsigned int i=0; i<nbPose ; i++)
+ {
+ nbPoint[i] = table_cal[i].npt;
+ nbPointTotal += nbPoint[i];
+ }
+
+ if (nbPointTotal < 4)
+ {
+ //vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError,
+ "Not enough point to calibrate")) ;
+ }
+
+ vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
+ vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
+ vpColVector u(nbPointTotal) ;
+ vpColVector v(nbPointTotal) ;
+
+ vpColVector P(2*nbPointTotal) ;
+ vpColVector Pd(2*nbPointTotal) ;
+ vpImagePoint ip;
+
+ unsigned int curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
+ std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
+ std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
+
+ for (unsigned int i =0 ; i < nbPoint[p] ; i++)
+ {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
+
+ ip = *it_Lip;
+ u[curPoint] = ip.get_u() ;
+ v[curPoint] = ip.get_v() ;
+
+ ++ it_LoX;
+ ++ it_LoY;
+ ++ it_LoZ;
+ ++ it_Lip;
+
+ curPoint++;
+ }
+ }
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+
+ iter++ ;
+ residu_1 = r ;
+
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
+
+ r = 0 ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint2 = 2*curPoint;
+
+ cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
+ +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
+ +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
+ +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+
+ Pd[curPoint2] = u[curPoint] ;
+ Pd[curPoint2+1] = v[curPoint] ;
+
+ P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
+ P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
+
+ r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
+ + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
+ curPoint++;
+ }
+ }
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/nbPointTotal ;
+
+ vpMatrix L(nbPointTotal*2,nbPose6+4) ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ unsigned int q = 6*p;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint2 = 2*curPoint;
+ unsigned int curPoint21 = curPoint2 + 1;
+
+ double x = cX[curPoint] ;
+ double y = cY[curPoint] ;
+ double z = cZ[curPoint] ;
+
+ double inv_z = 1/z;
+
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ //---------------
+ {
+ {
+ L[curPoint2][q] = px * (-inv_z) ;
+ L[curPoint2][q+1] = 0 ;
+ L[curPoint2][q+2] = px*(X*inv_z) ;
+ L[curPoint2][q+3] = px*X*Y ;
+ L[curPoint2][q+4] = -px*(1+X*X) ;
+ L[curPoint2][q+5] = px*Y ;
+ }
+ {
+ L[curPoint2][nbPose6]= 1 ;
+ L[curPoint2][nbPose6+1]= 0 ;
+ L[curPoint2][nbPose6+2]= X ;
+ L[curPoint2][nbPose6+3]= 0;
+ }
+ {
+ L[curPoint21][q] = 0 ;
+ L[curPoint21][q+1] = py*(-inv_z) ;
+ L[curPoint21][q+2] = py*(Y*inv_z) ;
+ L[curPoint21][q+3] = py* (1+Y*Y) ;
+ L[curPoint21][q+4] = -py*X*Y ;
+ L[curPoint21][q+5] = -py*X ;
+ }
+ {
+ L[curPoint21][nbPose6]= 0 ;
+ L[curPoint21][nbPose6+1]= 1 ;
+ L[curPoint21][nbPose6+2]= 0;
+ L[curPoint21][nbPose6+3]= Y ;
+ }
+
+ }
+ curPoint++;
+ } // end interaction
+ }
+ vpMatrix Lp ;
+ Lp = L.pseudoInverse(1e-10) ;
+
+ vpColVector e ;
+ e = Lp*error ;
+
+ vpColVector Tc, Tc_v(nbPose6) ;
+ Tc = -e*gain ;
+
+ // Tc_v =0 ;
+ for (unsigned int i = 0 ; i < nbPose6 ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
+ py+Tc[nbPose6+3],
+ u0+Tc[nbPose6],
+ v0+Tc[nbPose6+1]) ;
+
+ // cam.setKd(get_kd() + Tc[10]) ;
+ vpColVector Tc_v_Tmp(6) ;
+
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ for (unsigned int i = 0 ; i < 6 ; i++)
+ Tc_v_Tmp[i] = Tc_v[6*p + i];
+
+ table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
+ * table_cal[p].cMo;
+ }
+ if (verbose)
+ std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ table_cal[p].cMo_dist = table_cal[p].cMo ;
+ table_cal[p].cam = cam_est;
+ table_cal[p].cam_dist = cam_est;
+ double deviation,deviation_dist;
+ table_cal[p].computeStdDeviation(deviation,deviation_dist);
+ }
+ if (verbose)
+ std::cout << " Global std dev " << sqrt(r/nbPointTotal) << std::endl;
+
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
+
+
+void
+vpCalibration::calibVVSWithDistortionMulti(
+ unsigned int nbPose,
+ vpCalibration table_cal[],
+ vpCameraParameters &cam_est,
+ bool verbose)
+{
+ std::ios::fmtflags original_flags( std::cout.flags() );
+ std::cout.precision(10);
+ unsigned int nbPoint[1024]; //number of points by image
+ unsigned int nbPointTotal = 0; //total number of points
+
+ unsigned int nbPose6 = 6*nbPose;
+ for (unsigned int i=0; i<nbPose ; i++)
+ {
+ nbPoint[i] = table_cal[i].npt;
+ nbPointTotal += nbPoint[i];
+ }
+
+ if (nbPointTotal < 4)
+ {
+ //vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError,
+ "Not enough point to calibrate")) ;
+ }
+
+ vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
+ vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
+ vpColVector u(nbPointTotal) ;
+ vpColVector v(nbPointTotal) ;
+
+ vpColVector P(4*nbPointTotal) ;
+ vpColVector Pd(4*nbPointTotal) ;
+ vpImagePoint ip;
+
+ unsigned int curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
+ std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
+ std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
+ std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
+
+ for (unsigned int i =0 ; i < nbPoint[p] ; i++)
+ {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
+
+ ip = *it_Lip;
+ u[curPoint] = ip.get_u() ;
+ v[curPoint] = ip.get_v() ;
+
+ ++ it_LoX;
+ ++ it_LoY;
+ ++ it_LoZ;
+ ++ it_Lip;
+ curPoint++;
+ }
+ }
+ // double lambda = 0.1 ;
+ unsigned int iter = 0 ;
+
+ double residu_1 = 1e12 ;
+ double r =1e12-1;
+ while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
+ {
+ iter++ ;
+ residu_1 = r ;
+
+ r = 0 ;
+ curPoint = 0 ; //current point indice
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
+ +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
+ +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
+ +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+
+ curPoint++;
+ }
+ }
+
+
+ vpMatrix L(nbPointTotal*4,nbPose6+6) ;
+ curPoint = 0 ; //current point indice
+ double px = cam_est.get_px() ;
+ double py = cam_est.get_py() ;
+ double u0 = cam_est.get_u0() ;
+ double v0 = cam_est.get_v0() ;
+
+ double inv_px = 1/px ;
+ double inv_py = 1/py ;
+
+ double kud = cam_est.get_kud() ;
+ double kdu = cam_est.get_kdu() ;
+
+ double k2ud = 2*kud;
+ double k2du = 2*kdu;
+
+ for (unsigned int p=0; p<nbPose ; p++)
+ {
+ unsigned int q = 6*p;
+ for (unsigned int i=0 ; i < nbPoint[p]; i++)
+ {
+ unsigned int curPoint4 = 4*curPoint;
+ double x = cX[curPoint] ;
+ double y = cY[curPoint] ;
+ double z = cZ[curPoint] ;
+
+ double inv_z = 1/z;
+ double X = x*inv_z ;
+ double Y = y*inv_z ;
+
+ double X2 = X*X;
+ double Y2 = Y*Y;
+ double XY = X*Y;
+
+ double up = u[curPoint] ;
+ double vp = v[curPoint] ;
+
+ Pd[curPoint4] = up ;
+ Pd[curPoint4+1] = vp ;
+
+ double up0 = up - u0;
+ double vp0 = vp - v0;
+
+ double xp0 = up0 * inv_px;
+ double xp02 = xp0 *xp0 ;
+
+ double yp0 = vp0 * inv_py;
+ double yp02 = yp0 * yp0;
+
+ double r2du = xp02 + yp02 ;
+ double kr2du = kdu * r2du;
+
+ P[curPoint4] = u0 + px*X - kr2du *(up0) ;
+ P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
+
+ double r2ud = X2 + Y2 ;
+ double kr2ud = 1 + kud * r2ud;
+
+ double Axx = px*(kr2ud+k2ud*X2);
+ double Axy = px*k2ud*XY;
+ double Ayy = py*(kr2ud+k2ud*Y2);
+ double Ayx = py*k2ud*XY;
+
+ Pd[curPoint4+2] = up ;
+ Pd[curPoint4+3] = vp ;
+
+ P[curPoint4+2] = u0 + px*X*kr2ud ;
+ P[curPoint4+3] = v0 + py*Y*kr2ud ;
+
+ r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
+ vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
+ vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
+ vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
+
+ unsigned int curInd = curPoint4;
+ //---------------
+ {
+ {
+ L[curInd][q] = px * (-inv_z) ;
+ L[curInd][q+1] = 0 ;
+ L[curInd][q+2] = px*X*inv_z ;
+ L[curInd][q+3] = px*X*Y ;
+ L[curInd][q+4] = -px*(1+X2) ;
+ L[curInd][q+5] = px*Y ;
+ }
+ {
+ L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
+ L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
+ L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
+ L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
+ L[curInd][nbPose6+4] = -(up0)*(r2du) ;
+ L[curInd][nbPose6+5] = 0 ;
+ }
+ curInd++;
+ {
+ L[curInd][q] = 0 ;
+ L[curInd][q+1] = py*(-inv_z) ;
+ L[curInd][q+2] = py*Y*inv_z ;
+ L[curInd][q+3] = py* (1+Y2) ;
+ L[curInd][q+4] = -py*XY ;
+ L[curInd][q+5] = -py*X ;
+ }
+ {
+ L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
+ L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
+ L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
+ L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
+ L[curInd][nbPose6+4] = -vp0*r2du ;
+ L[curInd][nbPose6+5] = 0 ;
+ }
+ curInd++;
+ //---undistorted to distorted
+ {
+ L[curInd][q] = Axx*(-inv_z) ;
+ L[curInd][q+1] = Axy*(-inv_z) ;
+ L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
+ L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
+ L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
+ L[curInd][q+5] = Axx*Y -Axy*X;
+ }
+ {
+ L[curInd][nbPose6]= 1 ;
+ L[curInd][nbPose6+1]= 0 ;
+ L[curInd][nbPose6+2]= X*kr2ud ;
+ L[curInd][nbPose6+3]= 0;
+ L[curInd][nbPose6+4] = 0 ;
+ L[curInd][nbPose6+5] = px*X*r2ud ;
+ }
+ curInd++;
+ {
+ L[curInd][q] = Ayx*(-inv_z) ;
+ L[curInd][q+1] = Ayy*(-inv_z) ;
+ L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
+ L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
+ L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
+ L[curInd][q+5] = Ayx*Y -Ayy*X;
+ }
+ {
+ L[curInd][nbPose6]= 0 ;
+ L[curInd][nbPose6+1]= 1;
+ L[curInd][nbPose6+2]= 0;
+ L[curInd][nbPose6+3]= Y*kr2ud ;
+ L[curInd][nbPose6+4] = 0 ;
+ L[curInd][nbPose6+5] = py*Y*r2ud ;
+ }
+ } // end interaction
+ curPoint++;
+ } // end interaction
+ }
+
+ vpColVector error ;
+ error = P-Pd ;
+ //r = r/nbPointTotal ;
+
+ vpMatrix Lp ;
+ /*double rank =*/
+ L.pseudoInverse(Lp,1e-10) ;
+ vpColVector e ;
+ e = Lp*error ;
+ vpColVector Tc, Tc_v(6*nbPose) ;
+ Tc = -e*gain ;
+ for (unsigned int i = 0 ; i < 6*nbPose ; i++)
+ Tc_v[i] = Tc[i] ;
+
+ cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
+ u0+Tc[nbPose6], v0+Tc[nbPose6+1],
+ kud + Tc[nbPose6+5],
+ kdu + Tc[nbPose6+4]);
+
+ vpColVector Tc_v_Tmp(6) ;
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ for (unsigned int i = 0 ; i < 6 ; i++)
+ Tc_v_Tmp[i] = Tc_v[6*p + i];
+
+ table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
+ * table_cal[p].cMo_dist;
+ }
+ if (verbose)
+ std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
+ //std::cout << " residual: " << r << std::endl;
+
+ }
+ if (iter == nbIterMax)
+ {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError,
+ "Maximum number of iterations reached")) ;
+ }
+
+ for (unsigned int p = 0 ; p < nbPose ; p++)
+ {
+ table_cal[p].cam_dist = cam_est ;
+ table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
+ }
+ if (verbose)
+ std::cout <<" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;
+
+ // Restore ostream format
+ std::cout.flags(original_flags);
+}
diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp
new file mode 100644
index 0000000..041251e
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomography.cpp
@@ -0,0 +1,815 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography transformation.
+ *
+ * Authors:
+ * Muriel Pressigout
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpHomography.cpp
+ \brief Definition de la classe vpHomography. Class that consider
+ the particular case of homography
+*/
+
+#include <stdio.h>
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRobust.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+
+/*!
+ \brief initialize an homography as Identity
+*/
+vpHomography::vpHomography()
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ setIdentity();
+}
+
+
+/*!
+ \brief initialize an homography from another homography
+*/
+
+vpHomography::vpHomography(const vpHomography &H)
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ *this = H;
+}
+
+/*!
+ \brief initialize an homography from another homography
+*/
+vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p)
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ buildFrom(M, p) ;
+}
+
+vpHomography::vpHomography(const vpThetaUVector &tu,
+ const vpTranslationVector &atb,
+ const vpPlane &p)
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ buildFrom(tu, atb, p) ;
+}
+
+vpHomography::vpHomography(const vpRotationMatrix &aRb,
+ const vpTranslationVector &atb,
+ const vpPlane &p)
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ buildFrom(aRb, atb, p) ;
+}
+
+vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p)
+ : vpArray2D<double>(3,3), aMb(), bP()
+{
+ buildFrom(arb, p) ;
+}
+
+void
+vpHomography::buildFrom(const vpHomogeneousMatrix &M,
+ const vpPlane &p)
+{
+ insert(M) ;
+ insert(p) ;
+ build() ;
+}
+
+void
+vpHomography::buildFrom(const vpThetaUVector &tu,
+ const vpTranslationVector &atb,
+ const vpPlane &p)
+{
+ insert(tu) ;
+ insert(atb) ;
+ insert(p) ;
+ build() ;
+}
+
+void
+vpHomography::buildFrom(const vpRotationMatrix &aRb,
+ const vpTranslationVector &atb,
+ const vpPlane &p)
+{
+ insert(aRb) ;
+ insert(atb) ;
+ insert(p) ;
+ build() ;
+}
+
+void
+vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
+{
+ aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
+ insert(p) ;
+ build() ;
+}
+
+
+
+/*********************************************************************/
+
+
+/*!
+ \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;
+}
+
+/*!
+ \brief Invert the homography
+
+ \return \f$\bf H^{-1}\f$
+*/
+vpHomography
+vpHomography::inverse() const
+{
+ vpMatrix M = (*this).convert();
+ vpMatrix Minv;
+ M.pseudoInverse(Minv, 1e-16);
+
+ vpHomography H;
+
+ for(unsigned int i=0; i<3; i++)
+ for(unsigned int j=0; j<3; j++)
+ H[i][j] = Minv[i][j];
+
+ 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())
+ {
+ f << *this ;
+ }
+ else
+ {
+ throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")) ;
+ }
+}
+
+/*!
+
+ 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;
+ for(unsigned int i = 0; i < 3; i++) {
+ for(unsigned int j = 0; j < 3; j++) {
+ double s = 0.;
+ for(unsigned int k = 0; k < 3; k ++) {
+ s += (*this)[i][k] * H[k][j];
+ }
+ Hp[i][j] = s;
+ }
+ }
+ return Hp;
+}
+
+/*!
+ Operation a = aHb * b.
+
+ \param b : 3 dimension vector.
+*/
+vpColVector
+vpHomography::operator*(const vpColVector &b) const
+{
+ if (b.size() != 3)
+ throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));
+
+ vpColVector a(3);
+ for(unsigned int i=0; i<3; i++) {
+ a[i] = 0.;
+ for(unsigned int j=0; j<3; j++)
+ a[i] += (*this)[i][j] * b[j];
+ }
+
+ 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;
+
+ for (unsigned int i=0; i < 9; i ++) {
+ H.data[i] = this->data[i] * v;
+ }
+
+ 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 ;
+ vpColVector v(3),v1(3) ;
+
+ v[0] = b_P.get_x() ;
+ v[1] = b_P.get_y() ;
+ v[2] = b_P.get_w() ;
+
+ v1[0] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2] ;
+ v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2] ;
+ v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2] ;
+
+ // v1 = M*v ;
+ a_P.set_x(v1[0]) ;
+ a_P.set_y(v1[1]) ;
+ a_P.set_w(v1[2]) ;
+
+ 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;
+ if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
+
+ double vinv = 1/v ;
+
+ for (unsigned int i=0; i < 9; i ++) {
+ H.data[i] = this->data[i] * vinv;
+ }
+
+ return H;
+}
+
+//! Divide all the element of the homography matrix by v : Hij = Hij / v
+vpHomography & vpHomography::operator/=(double v)
+{
+ //if (x == 0)
+ if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
+
+ double vinv = 1/v ;
+
+ for (unsigned int i=0;i<9;i++)
+ data[i] *= vinv;
+
+ 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++)
+ for (unsigned int j=0; j< 3; j++)
+ (*this)[i][j] = H[i][j];
+
+ 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)
+ throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
+
+ for (unsigned int i=0; i< 3; i++)
+ for (unsigned int j=0; j< 3; j++)
+ (*this)[i][j] = H[i][j];
+
+ 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())
+ {
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3 ; j++)
+ {
+ f >> (*this)[i][j] ;
+ }
+ }
+ else
+ {
+ throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
+ }
+}
+
+/*!
+ \brief Compute aHb such that
+
+ \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
+ { ^b{\bf n}^T}
+ \f]
+*/
+void
+vpHomography::build()
+{
+ vpColVector n(3) ;
+ vpColVector atb(3) ;
+ vpMatrix aRb(3,3);
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ atb[i] = aMb[i][3];
+ for (unsigned int j=0 ; j < 3 ; j++)
+ aRb[i][j] = aMb[i][j];
+ }
+
+ bP.getNormal(n) ;
+
+ double d = bP.getD() ;
+ vpMatrix aHb = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
+ // plane equation. So if the plane is described by
+ // Ax+By+Cz+D=0, d=-D
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3 ; j++)
+ (*this)[i][j] = aHb[i][j];
+}
+
+/*!
+ \brief Compute aHb such that
+
+ \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
+ { ^b{\bf n}^T}
+ \f]
+ //note d => -d verifier
+*/
+void
+vpHomography::build(vpHomography &aHb,
+ const vpHomogeneousMatrix &aMb,
+ const vpPlane &bP)
+{
+ vpColVector n(3) ;
+ vpColVector atb(3) ;
+ vpMatrix aRb(3,3);
+ for (unsigned int i=0 ; i < 3 ; i++)
+ {
+ atb[i] = aMb[i][3] ;
+ for (unsigned int j=0 ; j < 3 ; j++)
+ aRb[i][j] = aMb[i][j];
+ }
+
+ bP.getNormal(n) ;
+
+ double d = bP.getD() ;
+ vpMatrix aHb_ = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
+ // plane equation. So if the plane is described by
+ // Ax+By+Cz+D=0, d=-D
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3 ; j++)
+ aHb[i][j] = aHb_[i][j];
+}
+
+/*!
+ 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++)
+ for (unsigned int j=0 ; j < 3; j++)
+ if (i==j)
+ (*this)[i][j] = 1.0 ;
+ else
+ (*this)[i][j] = 0.0;
+}
+
+#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+/*!
+ \deprecated You should rather use eye().
+
+ Set the homography as identity transformation.
+ \sa eye()
+*/
+void
+vpHomography::setIdentity()
+{
+ eye();
+}
+#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
+
+/*!
+ Given \c iPa a point with coordinates \f$(u_a,v_a)\f$ expressed in pixel in image a, and the homography \c bHa that
+ links image a and b, computes the coordinates of the point \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();
+ double ya = iPa.get_v();
+ vpMatrix H = cam.get_K() * bHa.convert() * cam.get_K_inverse();
+ double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
+ double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
+ double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
+
+ vpImagePoint iPb(yb, xb);
+
+ return iPb;
+}
+
+/*!
+ Given \c Pa a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the image plane a, and the homography \c bHa 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();
+ double ya = Pa.get_y();
+ double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
+ double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
+ double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
+
+ vpPoint Pb;
+ Pb.set_x(xb);
+ Pb.set_y(yb);
+
+ 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<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ vpHomography &aHb,
+ std::vector<bool> &inliers,
+ double &residual,
+ double weights_threshold,
+ unsigned int niter,
+ bool normalization)
+{
+ unsigned int n = (unsigned int) xb.size();
+ if (yb.size() != n || xa.size() != n || ya.size() != n)
+ throw(vpException(vpException::dimensionError,
+ "Bad dimension for robust homography estimation"));
+
+ // 4 point are required
+ if(n<4)
+ throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
+
+ try{
+ std::vector<double> xan, yan, xbn, ybn;
+
+ double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
+
+ vpHomography aHbn;
+
+ if (normalization) {
+ vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
+ vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
+ }
+ else {
+ xbn = xb;
+ ybn = yb;
+ xan = xa;
+ yan = ya;
+ }
+
+ unsigned int nbLinesA = 2;
+ vpMatrix A(nbLinesA*n,8);
+ vpColVector X(8);
+ vpColVector Y(nbLinesA*n);
+ vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix
+
+ vpColVector w(nbLinesA*n) ;
+
+ // All the weights are set to 1 at the beginning to use a classical least square scheme
+ w = 1;
+ // Update the square matrix associated to the weights
+ for (unsigned int i=0; i < nbLinesA*n; i ++) {
+ W[i][i] = w[i];
+ }
+
+ // build matrix A
+ for(unsigned int i=0; i<n;i++) {
+ A[nbLinesA*i][0]=xbn[i];
+ A[nbLinesA*i][1]=ybn[i];
+ A[nbLinesA*i][2]=1;
+ A[nbLinesA*i][3]=0 ;
+ A[nbLinesA*i][4]=0 ;
+ A[nbLinesA*i][5]=0;
+ A[nbLinesA*i][6]=-xbn[i]*xan[i] ;
+ A[nbLinesA*i][7]=-ybn[i]*xan[i];
+
+ A[nbLinesA*i+1][0]=0 ;
+ A[nbLinesA*i+1][1]=0;
+ A[nbLinesA*i+1][2]=0;
+ A[nbLinesA*i+1][3]=xbn[i];
+ A[nbLinesA*i+1][4]=ybn[i];
+ A[nbLinesA*i+1][5]=1;
+ A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
+ A[nbLinesA*i+1][7]=-ybn[i]*yan[i];
+
+ Y[nbLinesA*i] = xan[i];
+ Y[nbLinesA*i+1] = yan[i];
+ }
+
+ vpMatrix WA;
+ vpMatrix WAp;
+ unsigned int iter = 0;
+ vpRobust r(nbLinesA*n) ; // M-Estimator
+
+ while (iter < niter) {
+ WA = W * A;
+
+ X = WA.pseudoInverse(1e-26)*W*Y;
+ vpColVector residu;
+ residu = Y - A * X;
+
+ // Compute the weights using the Tukey biweight M-Estimator
+ r.setIteration(iter) ;
+ r.MEstimator(vpRobust::TUKEY, residu, w) ;
+
+ // Update the weights matrix
+ for (unsigned int i=0; i < n*nbLinesA; i ++) {
+ W[i][i] = w[i];
+ }
+ // Build the homography
+ for(unsigned int i=0;i<8;i++)
+ aHbn.data[i]= X[i];
+ aHbn[2][2] = 1;
+ {
+ vpMatrix aHbnorm = aHbn.convert();
+ aHbnorm /= aHbnorm[2][2] ;
+ }
+
+ iter ++;
+ }
+ inliers.resize(n);
+ unsigned int nbinliers = 0;
+ for(unsigned int i=0; i< n; i++) {
+ if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
+ inliers[i] = false;
+ else {
+ inliers[i] = true;
+ nbinliers ++;
+ }
+ }
+
+ if (normalization) {
+ // H after denormalization
+ vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
+ }
+ else {
+ aHb = aHbn;
+ }
+
+ residual = 0 ;
+ vpColVector a(3), b(3), c(3);
+ for (unsigned int i=0 ; i < n ; i++) {
+ if (inliers[i]) {
+ a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
+ b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
+
+ c = aHb*b ; c /= c[2] ;
+ residual += (a-c).sumSquare();
+ }
+ }
+
+ residual = sqrt(residual/nbinliers);
+ }
+ catch(...)
+ {
+ throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
+ }
+}
+
+/*!
+ 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
+vpHomography::projection(const vpImagePoint &ipb)
+{
+ vpImagePoint ipa;
+ double i = ipb.get_i();
+ double j = ipb.get_j();
+
+ double i_a = (*this)[0][0] * i + (*this)[0][1] * j + (*this)[0][2];
+ double j_a = (*this)[1][0] * i + (*this)[1][1] * j + (*this)[1][2];
+ double k_a = (*this)[2][0] * i + (*this)[2][1] * j + (*this)[2][2];
+
+ if(std::fabs(k_a) > std::numeric_limits<double>::epsilon()){
+ ipa.set_i(i_a / k_a);
+ ipa.set_j(j_a / k_a);
+ }
+
+ return ipa;
+}
+
+/*!
+ Converts an homography to a matrix.
+ \return The 3x3 matrix corresponding to the homography.
+ */
+vpMatrix vpHomography::convert() const
+{
+ vpMatrix M(3, 3);
+ for(unsigned int i=0; i<3; i++)
+ for(unsigned int j=0; j<3; j++)
+ M[i][j] = (*this)[i][j];
+
+ return M;
+}
diff --git a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp
new file mode 100644
index 0000000..9146cc2
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp
@@ -0,0 +1,386 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography estimation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpHomographyDLT.cpp
+
+ This file implements the fonctions related with the homography
+ estimation using the DLT algorithm
+*/
+
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpMatrixException.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+void
+vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
+ std::vector<double> &xn, std::vector<double> &yn,
+ double &xg, double &yg, double &coef)
+{
+ if (x.size() != y.size())
+ throw(vpException(vpException::dimensionError,
+ "Hartley normalization require that x and y vector have the same dimension"));
+
+ unsigned int n = (unsigned int) x.size();
+ if (xn.size() != n)
+ xn.resize(n);
+ if (yn.size() != n)
+ yn.resize(n);
+
+ xg = 0 ;
+ yg = 0 ;
+
+ for (unsigned int i =0 ; i < n ; i++)
+ {
+ xg += x[i] ;
+ yg += y[i] ;
+ }
+ xg /= n ;
+ yg /= n ;
+
+ // Changement d'origine : le centre de gravite doit correspondre
+ // a l'origine des coordonnees
+ double distance=0;
+ for(unsigned int i=0; i<n;i++)
+ {
+ double xni=x[i]-xg;
+ double yni=y[i]-yg;
+ xn[i] = xni ;
+ yn[i] = yni ;
+ distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
+ }//for translation sur tous les points
+
+ //Changement d'echelle
+ distance/=n;
+ //calcul du coef de changement d'echelle
+ //if(distance ==0)
+ if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
+ coef=1;
+ else
+ coef=sqrt(2.0)/distance;
+
+ for(unsigned int i=0; i<n;i++)
+ {
+ xn[i] *= coef;
+ yn[i] *= coef;
+ }
+}
+
+void
+vpHomography::HartleyNormalization(unsigned int n,
+ const double *x, const double *y,
+ double *xn, double *yn,
+ double &xg, double &yg,
+ double &coef)
+{
+ unsigned int i;
+ xg = 0 ;
+ yg = 0 ;
+
+ for (i =0 ; i < n ; i++)
+ {
+ xg += x[i] ;
+ yg += y[i] ;
+ }
+ xg /= n ;
+ yg /= n ;
+
+ //Changement d'origine : le centre de gravite doit correspondre
+ // a l'origine des coordonnees
+ double distance=0;
+ for(i=0; i<n;i++)
+ {
+ double xni=x[i]-xg;
+ double yni=y[i]-yg;
+ xn[i] = xni ;
+ yn[i] = yni ;
+ distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
+ }//for translation sur tous les points
+
+ //Changement d'echelle
+ distance/=n;
+ //calcul du coef de changement d'echelle
+ //if(distance ==0)
+ if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
+ coef=1;
+ else
+ coef=sqrt(2.0)/distance;
+
+ for(i=0; i<n;i++)
+ {
+ xn[i] *= coef;
+ yn[i] *= coef;
+ }
+}
+
+//---------------------------------------------------------------------------------------
+
+void
+vpHomography::HartleyDenormalization(vpHomography &aHbn,
+ vpHomography &aHb,
+ double xg1, double yg1, double coef1,
+ double xg2, double yg2, double coef2 )
+{
+
+ //calcul des transformations a appliquer sur M_norm pour obtenir M
+ //en fonction des deux normalisations effectuees au debut sur
+ //les points: aHb = T2^ aHbn T1
+ vpMatrix T1(3,3);
+ vpMatrix T2(3,3);
+ vpMatrix T2T(3,3);
+
+ T1.eye();
+ T2.eye();
+ T2T.eye();
+
+ T1[0][0]=T1[1][1]=coef1;
+ T1[0][2]=-coef1*xg1 ;
+ T1[1][2]=-coef1*yg1 ;
+
+ T2[0][0]=T2[1][1]=coef2;
+ T2[0][2]=-coef2*xg2 ;
+ T2[1][2]=-coef2*yg2 ;
+
+ T2T=T2.pseudoInverse(1e-16) ;
+
+ vpMatrix aHbn_(3,3);
+ for(unsigned int i=0; i<3; i++)
+ for(unsigned int j=0; j<3; j++)
+ aHbn_[i][j] = aHbn[i][j];
+
+ vpMatrix maHb=T2T*aHbn_*T1;
+
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3 ; j++)
+ aHb[i][j] = maHb[i][j] ;
+}
+
+#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 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]
+ <b>h</b> is the column of <b>V</b> associated with the smalest singular value of <b>A
+ </b>
+
+ \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<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya ,
+ vpHomography &aHb,
+ bool normalization)
+{
+ unsigned int n = (unsigned int) xb.size();
+ if (yb.size() != n || xa.size() != n || ya.size() != n)
+ throw(vpException(vpException::dimensionError,
+ "Bad dimension for DLT homography estimation"));
+
+ // 4 point are required
+ if(n<4)
+ throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
+
+ try{
+ std::vector<double> xan, yan, xbn, ybn;
+
+ double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
+
+ vpHomography aHbn;
+
+ if (normalization) {
+ vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
+ vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
+ }
+ else {
+ xbn = xb;
+ ybn = yb;
+ xan = xa;
+ yan = ya;
+ }
+
+ vpMatrix A(2*n,9);
+ vpColVector h(9);
+ vpColVector D(9);
+ vpMatrix V(9,9);
+
+ // We need here to compute the SVD on a (n*2)*9 matrix (where n is
+ // the number of points). if n == 4, the matrix has more columns
+ // than rows. This kind of matrix is not supported by GSL for
+ // SVD. The solution is to add an extra line with zeros
+ if (n == 4)
+ A.resize(2*n+1,9);
+
+ // build matrix A
+ for(unsigned int i=0; i<n;i++)
+ {
+ A[2*i][0]=0;
+ A[2*i][1]=0;
+ A[2*i][2]=0;
+ A[2*i][3]=-xbn[i] ;
+ A[2*i][4]=-ybn[i] ;
+ A[2*i][5]=-1;
+ A[2*i][6]=xbn[i]*yan[i] ;
+ A[2*i][7]=ybn[i]*yan[i];
+ A[2*i][8]=yan[i];
+
+
+ A[2*i+1][0]=xbn[i] ;
+ A[2*i+1][1]=ybn[i] ;
+ A[2*i+1][2]=1;
+ A[2*i+1][3]=0;
+ A[2*i+1][4]=0;
+ A[2*i+1][5]=0;
+ A[2*i+1][6]=-xbn[i]*xan[i];
+ A[2*i+1][7]=-ybn[i]*xan[i];
+ A[2*i+1][8]=-xan[i] ;
+ }
+
+ // Add an extra line with zero.
+ if (n == 4) {
+ for (unsigned int i=0; i < 9; i ++) {
+ A[2*n][i] = 0;
+ }
+ }
+
+ // solve Ah = 0
+ // SVD Decomposition A = UDV^T (destructive wrt A)
+ A.svd(D,V);
+
+ // on en profite pour effectuer un controle sur le rang de la matrice :
+ // pas plus de 2 valeurs singulieres quasi=0
+ int rank=0;
+ for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
+ if(rank <7)
+ {
+ vpTRACE(" Rank is : %d, should be 8", rank);
+ throw(vpMatrixException(vpMatrixException::rankDeficient,
+ "\n\t\t Matrix rank is deficient")) ;
+ }
+ //h = is the column of V associated with the smallest singular value of A
+
+ // since we are not sure that the svd implemented sort the
+ // singular value... we seek for the smallest
+ double smallestSv = 1e30 ;
+ unsigned int indexSmallestSv = 0 ;
+ for (unsigned int i=0 ; i < 9 ; i++)
+ if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
+
+ h=V.getCol(indexSmallestSv);
+
+ // build the homography
+ for(unsigned int i =0;i<3;i++)
+ {
+ for(unsigned int j=0;j<3;j++)
+ aHbn[i][j]=h[3*i+j];
+ }
+
+ if (normalization) {
+ // H after denormalization
+ vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
+ }
+ else {
+ aHb = aHbn;
+ }
+
+ }
+ catch(vpMatrixException me)
+ {
+ vpTRACE("Matrix Exception ") ;
+ throw(me) ;
+ }
+ catch(vpException me)
+ {
+ vpERROR_TRACE("caught another error") ;
+ std::cout <<std::endl << me << std::endl ;
+ throw(me) ;
+ }
+}
diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp
new file mode 100644
index 0000000..96adefb
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp
@@ -0,0 +1,1331 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography transformation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMath.h>
+#include <math.h>
+
+//#define DEBUG_Homographie 0
+
+/* ---------------------------------------------------------------------- */
+/* --- STATIC ------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+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)
+{
+
+
+ vpColVector nd(3) ;
+ nd[0]=0;nd[1]=0;nd[2]=1;
+
+ 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)
+{
+ computeDisplacement(*this,nd, aRb,atb,n);
+}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+
+ Compute the camera displacement between two images from the homography \f$
+ {^a}{\bf H}_b \f$.
+
+ Camera displacement between \f$ {^a}{\bf p} \f$ and \f$ {^b}{\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 aHb : Input homography \f$ {^a}{\bf H}_b \f$.
+
+ \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 vpHomography &aHb,
+ const vpColVector &nd,
+ vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n)
+{
+ /**** Declarations des variables ****/
+
+ vpMatrix aRbint(3,3) ;
+ vpColVector svTemp(3), sv(3);
+ vpMatrix mX(3,2) ;
+ vpColVector aTbp(3), normaleEstimee(3);
+ double distanceFictive;
+ double sinusTheta, cosinusTheta, signeSinus= 1;
+ double cosinusDesireeEstimee, cosinusAncien;
+ double s, determinantU, determinantV;
+ unsigned int i, j, k, w;
+ unsigned int vOrdre[3];
+
+ //vpColVector normaleDesiree(3) ;
+ //normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
+ vpColVector normaleDesiree(nd);
+
+ /**** Corps de la focntion ****/
+#ifdef DEBUG_Homographie
+ printf ("debut : Homographie_EstimationDeplacementCamera\n");
+#endif
+
+ /* Allocation des matrices */
+ vpMatrix mTempU(3,3) ;
+ vpMatrix mTempV(3,3) ;
+ vpMatrix mU(3,3) ;
+ vpMatrix mV(3,3) ;
+ vpMatrix aRbp(3,3) ;
+
+
+ vpMatrix mH(3,3) ;
+ for (i=0 ; i < 3 ; i++)
+ for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
+
+ /* Preparation au calcul de la SVD */
+ mTempU = mH ;
+
+ /*****
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
+ *****/
+ mTempU.svd(svTemp,mTempV) ;
+
+ /* On va mettre les valeurs singulieres en ordre decroissant : */
+
+ /* Determination de l'ordre des valeurs */
+ if (svTemp[0] >= svTemp[1]) {
+ if (svTemp[0] >= svTemp[2]) {
+ if (svTemp[1] > svTemp[2]) {
+ vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ }
+ } else {
+ vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ }
+ } else {
+ if (svTemp[1] >= svTemp[2]){
+ if (svTemp[0] > svTemp[2])
+ { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
+ else
+ { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
+ } else {
+ vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ }
+ }
+ /*****
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ *****/
+
+
+ for (i = 0; i < 3; i++) {
+ sv[i] = svTemp[vOrdre[i]];
+ for (j = 0; j < 3; j++) {
+ mU[i][j] = mTempU[i][vOrdre[j]];
+ mV[i][j] = mTempV[i][vOrdre[j]];
+ }
+ }
+
+#ifdef DEBUG_Homographie
+ printf("U : \n") ; std::cout << mU << std::endl ;
+ printf("V : \n") ; std::cout << mV << std::endl ;
+ printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
+#endif
+
+ /* A verifier si necessaire!!! */
+ determinantV = mV.det();
+ determinantU = mU.det();
+
+ s = determinantU * determinantV;
+
+#ifdef DEBUG_Homographie
+ printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
+#endif
+ if (s < 0) mV *=-1 ;
+
+
+ /* d' = d2 */
+ distanceFictive = sv[1];
+#ifdef DEBUG_Homographie
+ printf ("d = %f\n",distanceFictive);
+#endif
+ n.resize(3) ;
+
+ if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
+ {
+ //#ifdef DEBUG_Homographie
+ // printf ("\nPure rotation\n");
+ //#endif
+ /*****
+ Cas ou le deplacement est une rotation pure
+ et la normale reste indefini
+ sv[0] = sv[1]= sv[2]
+ *****/
+ aTbp[0] = 0;
+ aTbp[1] = 0;
+ aTbp[2] = 0;
+
+ n[0] = normaleDesiree[0];
+ n[1] = normaleDesiree[1];
+ n[2] = normaleDesiree[2];
+ }
+ else
+ {
+#ifdef DEBUG_Homographie
+ printf("\nCas general\n");
+#endif
+ /* Cas general */
+
+ /*****
+ test pour determiner quelle est la bonne solution on teste
+ d'abord quelle solution est plus proche de la perpendiculaire
+ au plan de la cible constuction de la normale n
+ *****/
+
+ /*****
+ Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
+ dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
+ *****/
+ mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
+ / (sv[0] * sv[0] - sv[2] * sv[2]));
+ mX[1][0] = 0.0;
+ mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
+ / (sv[0] * sv[0] - sv[2] * sv[2]));
+
+ mX[0][1] = -mX[0][0];
+ mX[1][1] = mX[1][0];
+ mX[2][1] = mX[2][0];
+
+ /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
+ cosinusAncien = 0.0;
+ for (w = 0; w < 2; w++) { /* Pour les 2 cas */
+ for (k = 0; k < 2; k++) { /* Pour le signe */
+
+ /* Calcul de la normale estimee : n = V.n' */
+ for (i = 0; i < 3; i++) {
+ normaleEstimee[i] = 0.0;
+ for (j = 0; j < 3; j++) {
+ normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
+ }
+ }
+
+
+ /* Calcul du cosinus de l'angle entre la normale reelle et desire */
+ cosinusDesireeEstimee = 0.0;
+ for (i = 0; i < 3; i++)
+ cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
+
+ /*****
+ Si la solution est meilleur
+ Remarque : On ne teste pas le cas oppose (cos<0)
+ *****/
+ if (cosinusDesireeEstimee > cosinusAncien)
+ {
+ cosinusAncien = cosinusDesireeEstimee;
+
+ /* Affectation de la normale qui est retourner */
+ for (j = 0; j < 3; j++)
+ n[j] = normaleEstimee[j];
+
+ /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
+ aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
+ aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
+ aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
+
+ /* si c'est la deuxieme solution */
+ if (w == 1)
+ signeSinus = -1; /* car esp1*esp3 = -1 */
+ else
+ signeSinus = 1;
+ } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
+ } /* fin k */
+ } /* fin w */
+ } /* fin else */
+
+
+ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
+ for (i = 0; i < 3; i++) {
+ atb[i] = 0.0;
+ for (j = 0; j < 3; j++) {
+ atb[i] += mU[i][j] * aTbp[j];
+ }
+ atb[i] /= distanceFictive;
+ }
+
+
+#ifdef DEBUG_Homographie
+ printf("t' : ") ; std::cout << aTbp.t() ;
+ printf("t/d : ") ; std::cout << atb.t() ;
+ printf("n : ") ; std::cout << n.t() ;
+#endif
+
+
+ /* Calcul de la matrice de rotation R */
+
+ /*****
+ Calcul du sinus(theta) et du cosinus(theta)
+ Remarque : sinus(theta) pourra changer de signe en fonction
+ de la solution retenue (cf. ci-dessous)
+ *****/
+ sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
+ *(sv[1]*sv[1] -sv[2]*sv[2]))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
+
+ /* construction de la matrice de rotation R' */
+ aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
+ aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
+ aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
+
+
+
+ /* multiplication Rint = U R' */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ aRbint[i][j] = 0.0;
+ for (k = 0; k < 3; k++) {
+ aRbint[i][j] += mU[i][k] * aRbp[k][j];
+ }
+ }
+ }
+
+ /* multiplication R = Rint . V^T */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ aRb[i][j] = 0.0;
+ for (k = 0; k < 3; k++) {
+ aRb[i][j] += aRbint[i][k] * mV[j][k];
+ }
+ }
+ }
+ /*transpose_carre(aRb,3); */
+#ifdef DEBUG_Homographie
+ printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
+#endif
+
+}
+
+/*!
+
+ Compute the camera displacement between two images from the homography \f$
+ {^a}{\bf H}_b \f$.
+
+ 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 aHb : Input homography \f$ {^a}{\bf H}_b \f$.
+
+ \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 vpHomography &aHb,
+ vpRotationMatrix &aRb,
+ vpTranslationVector &atb,
+ vpColVector &n)
+{
+ /**** Declarations des variables ****/
+
+ vpMatrix aRbint(3,3) ;
+ vpColVector svTemp(3), sv(3);
+ vpMatrix mX(3,2) ;
+ vpColVector aTbp(3), normaleEstimee(3);
+ double distanceFictive;
+ double sinusTheta, cosinusTheta, signeSinus= 1;
+ double cosinusDesireeEstimee, cosinusAncien;
+ double s, determinantU, determinantV;
+ unsigned int i, j, k, w;
+ unsigned int vOrdre[3];
+
+ vpColVector normaleDesiree(3) ;
+ normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
+
+ /**** Corps de la focntion ****/
+#ifdef DEBUG_Homographie
+ printf ("debut : Homographie_EstimationDeplacementCamera\n");
+#endif
+
+ /* Allocation des matrices */
+ vpMatrix mTempU(3,3) ;
+ vpMatrix mTempV(3,3) ;
+ vpMatrix mU(3,3) ;
+ vpMatrix mV(3,3) ;
+ vpMatrix aRbp(3,3) ;
+
+
+ vpMatrix mH(3,3) ;
+ for (i=0 ; i < 3 ; i++)
+ for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
+
+ /* Preparation au calcul de la SVD */
+ mTempU = mH ;
+
+ /*****
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
+ *****/
+ mTempU.svd(svTemp,mTempV) ;
+
+ /* On va mettre les valeurs singulieres en ordre decroissant : */
+
+ /* Determination de l'ordre des valeurs */
+ if (svTemp[0] >= svTemp[1]) {
+ if (svTemp[0] >= svTemp[2]) {
+ if (svTemp[1] > svTemp[2]) {
+ vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ }
+ } else {
+ vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ }
+ } else {
+ if (svTemp[1] >= svTemp[2]){
+ if (svTemp[0] > svTemp[2])
+ { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
+ else
+ { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
+ } else {
+ vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ }
+ }
+ /*****
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ *****/
+
+
+ for (i = 0; i < 3; i++) {
+ sv[i] = svTemp[vOrdre[i]];
+ for (j = 0; j < 3; j++) {
+ mU[i][j] = mTempU[i][vOrdre[j]];
+ mV[i][j] = mTempV[i][vOrdre[j]];
+ }
+ }
+
+#ifdef DEBUG_Homographie
+ printf("U : \n") ; std::cout << mU << std::endl ;
+ printf("V : \n") ; std::cout << mV << std::endl ;
+ printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
+#endif
+
+ /* A verifier si necessaire!!! */
+ determinantV = mV.det();
+ determinantU = mU.det();
+
+ s = determinantU * determinantV;
+
+#ifdef DEBUG_Homographie
+ printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
+#endif
+ if (s < 0) mV *=-1 ;
+
+
+ /* d' = d2 */
+ distanceFictive = sv[1];
+#ifdef DEBUG_Homographie
+ printf ("d = %f\n",distanceFictive);
+#endif
+ n.resize(3) ;
+
+ if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
+ {
+ //#ifdef DEBUG_Homographie
+ // printf ("\nPure rotation\n");
+ //#endif
+ /*****
+ Cas ou le deplacement est une rotation pure
+ et la normale reste indefini
+ sv[0] = sv[1]= sv[2]
+ *****/
+ aTbp[0] = 0;
+ aTbp[1] = 0;
+ aTbp[2] = 0;
+
+ n[0] = normaleDesiree[0];
+ n[1] = normaleDesiree[1];
+ n[2] = normaleDesiree[2];
+ }
+ else
+ {
+#ifdef DEBUG_Homographie
+ printf("\nCas general\n");
+#endif
+ /* Cas general */
+
+ /*****
+ test pour determiner quelle est la bonne solution on teste
+ d'abord quelle solution est plus proche de la perpendiculaire
+ au plan de la cible constuction de la normale n
+ *****/
+
+ /*****
+ Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
+ dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
+ *****/
+ mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
+ / (sv[0] * sv[0] - sv[2] * sv[2]));
+ mX[1][0] = 0.0;
+ mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
+ / (sv[0] * sv[0] - sv[2] * sv[2]));
+
+ mX[0][1] = -mX[0][0];
+ mX[1][1] = mX[1][0];
+ mX[2][1] = mX[2][0];
+
+ /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
+ cosinusAncien = 0.0;
+ for (w = 0; w < 2; w++) { /* Pour les 2 cas */
+ for (k = 0; k < 2; k++) { /* Pour le signe */
+
+ /* Calcul de la normale estimee : n = V.n' */
+ for (i = 0; i < 3; i++) {
+ normaleEstimee[i] = 0.0;
+ for (j = 0; j < 3; j++) {
+ normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
+ }
+ }
+
+
+ /* Calcul du cosinus de l'angle entre la normale reelle et desire */
+ cosinusDesireeEstimee = 0.0;
+ for (i = 0; i < 3; i++)
+ cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
+
+ /*****
+ Si la solution est meilleur
+ Remarque : On ne teste pas le cas oppose (cos<0)
+ *****/
+ if (cosinusDesireeEstimee > cosinusAncien)
+ {
+ cosinusAncien = cosinusDesireeEstimee;
+
+ /* Affectation de la normale qui est retourner */
+ for (j = 0; j < 3; j++)
+ n[j] = normaleEstimee[j];
+
+ /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
+ aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
+ aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
+ aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
+
+ /* si c'est la deuxieme solution */
+ if (w == 1)
+ signeSinus = -1; /* car esp1*esp3 = -1 */
+ else
+ signeSinus = 1;
+ } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
+ } /* fin k */
+ } /* fin w */
+ } /* fin else */
+
+
+ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
+ for (i = 0; i < 3; i++) {
+ atb[i] = 0.0;
+ for (j = 0; j < 3; j++) {
+ atb[i] += mU[i][j] * aTbp[j];
+ }
+ atb[i] /= distanceFictive;
+ }
+
+
+#ifdef DEBUG_Homographie
+ printf("t' : ") ; std::cout << aTbp.t() ;
+ printf("t/d : ") ; std::cout << atb.t() ;
+ printf("n : ") ; std::cout << n.t() ;
+#endif
+
+
+ /* Calcul de la matrice de rotation R */
+
+ /*****
+ Calcul du sinus(theta) et du cosinus(theta)
+ Remarque : sinus(theta) pourra changer de signe en fonction
+ de la solution retenue (cf. ci-dessous)
+ *****/
+ sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
+ *(sv[1]*sv[1] -sv[2]*sv[2]))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
+
+ /* construction de la matrice de rotation R' */
+ aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
+ aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
+ aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
+
+
+
+ /* multiplication Rint = U R' */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ aRbint[i][j] = 0.0;
+ for (k = 0; k < 3; k++) {
+ aRbint[i][j] += mU[i][k] * aRbp[k][j];
+ }
+ }
+ }
+
+ /* multiplication R = Rint . V^T */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ aRb[i][j] = 0.0;
+ for (k = 0; k < 3; k++) {
+ aRb[i][j] += aRbint[i][k] * mV[j][k];
+ }
+ }
+ }
+ /*transpose_carre(aRb,3); */
+#ifdef DEBUG_Homographie
+ printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
+#endif
+
+}
+
+void vpHomography::computeDisplacement(const vpHomography &H,
+ const double x,
+ const double y,
+ std::list<vpRotationMatrix> & vR,
+ std::list<vpTranslationVector> & vT,
+ std::list<vpColVector> & vN)
+{
+
+#ifdef DEBUG_Homographie
+ printf ("debut : Homographie_EstimationDeplacementCamera\n");
+#endif
+
+ vR.clear();
+ vT.clear();
+ vN.clear();
+
+ /**** Declarations des variables ****/
+ int cas1 =1, cas2=2, cas3=3, cas4=4;
+ int cas =0;
+ bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
+
+ double tmp,determinantU,determinantV,s,distanceFictive;
+ vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
+
+ vpRotationMatrix Rprim1,R1;
+ vpRotationMatrix Rprim2,R2;
+ vpRotationMatrix Rprim3,R3;
+ vpRotationMatrix Rprim4,R4;
+ vpTranslationVector Tprim1, T1;
+ vpTranslationVector Tprim2, T2;
+ vpTranslationVector Tprim3, T3;
+ vpTranslationVector Tprim4, T4;
+ vpColVector Nprim1(3), N1(3);
+ vpColVector Nprim2(3), N2(3);
+ vpColVector Nprim3(3), N3(3);
+ vpColVector Nprim4(3), N4(3);
+
+ vpColVector svTemp(3),sv(3);
+ unsigned int vOrdre[3];
+ vpColVector vTp(3);
+
+
+ /* Preparation au calcul de la SVD */
+ mTempU = H.convert();
+
+ /*****
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
+ *****/
+
+ // cette fonction ne renvoit pas d'erreur
+ mTempU.svd(svTemp, mTempV);
+
+ /* On va mettre les valeurs singulieres en ordre decroissant : */
+ /* Determination de l'ordre des valeurs */
+ if (svTemp[0] >= svTemp[1]) {
+ if (svTemp[0] >= svTemp[2])
+ {
+ if (svTemp[1] > svTemp[2])
+ {
+ vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
+ }
+ else
+ {
+ vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ }
+ }
+ else
+ {
+ vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ }
+ } else {
+ if (svTemp[1] >= svTemp[2]){
+ if (svTemp[0] > svTemp[2]) {
+ vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
+ }
+ } else {
+ vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ }
+ }
+ /*****
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ *****/
+ for (unsigned int i = 0; i < 3; i++) {
+ sv[i] = svTemp[vOrdre[i]];
+ for (unsigned int j = 0; j < 3; j++) {
+ U[i][j] = mTempU[i][vOrdre[j]];
+ V[i][j] = mTempV[i][vOrdre[j]];
+ }
+ }
+
+#ifdef DEBUG_Homographie
+ printf("U : \n") ; Affiche(U) ;
+ printf("V : \n") ; affiche(V) ;
+ printf("Valeurs singulieres : ") ; affiche(sv);
+#endif
+
+ // calcul du determinant de U et V
+ determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
+ U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
+ U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
+
+ determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
+ V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
+ V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
+
+ s = determinantU * determinantV;
+
+#ifdef DEBUG_Homographie
+ printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
+#endif
+
+ // deux cas sont a traiter :
+ // distance Fictive = sv[1]
+ // distance fictive = -sv[1]
+
+ // pour savoir quelle est le bon signe,
+ // on utilise le point qui appartient au plan
+ // la contrainte est :
+ // (h_31x_1 + h_32x_2 + h_33)/d > 0
+ // et d = sd'
+
+ tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
+
+ if ((tmp/(sv[1] *s)) > 0)
+ distanceFictive = sv[1];
+ else
+ distanceFictive = -sv[1];
+
+ // il faut ensuite considerer l'ordre de multiplicite de chaque variable
+ // 1er cas : d1<>d2<>d3
+ // 2eme cas : d1=d2 <> d3
+ // 3eme cas : d1 <>d2 =d3
+ // 4eme cas : d1 =d2=d3
+
+ if ((sv[0] - sv[1]) < sing_threshold)
+ {
+ if ((sv[1] - sv[2]) < sing_threshold)
+ cas = cas4;
+ else
+ cas = cas2;
+ }
+ else
+ {
+ if ((sv[1] - sv[2]) < sing_threshold)
+ cas = cas3;
+ else
+ cas = cas1;
+ }
+
+ Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
+
+ // on filtre ensuite les diff'erentes normales possibles
+ // condition : nm/d > 0
+ // avec d = sd'
+ // et n = Vn'
+
+ // les quatres cas sont : ++, +-, -+ , --
+ // dans tous les cas, Normale[1] = 0;
+ Nprim1[1] = 0.0;
+ Nprim2[1] = 0.0;
+ Nprim3[1] = 0.0;
+ Nprim4[1] = 0.0;
+
+ // on calcule les quatres cas de normale
+
+ if (cas == cas1)
+ {
+ // quatre normales sont possibles
+ Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1] )/
+ (sv[0] * sv[0] - sv[2] * sv[2] ));
+
+ Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2] )/
+ (sv[0] * sv[0] - sv[2] * sv[2] ));
+
+ Nprim2[0] = Nprim1[0]; Nprim2[2] = - Nprim1[2];
+ Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
+ Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
+ }
+ if (cas == cas2)
+ {
+ // 2 normales sont possibles
+ // x3 = +-1
+ Nprim1[2] = 1;
+ Nprim2[2] = -1;
+ }
+
+ if (cas == cas3)
+ {
+ // 2 normales sont possibles
+ // x1 = +-1
+ Nprim1[0] = 1;
+ Nprim2[0] = -1;
+ }
+ // si on est dans le cas 4,
+ // on considere que x reste indefini
+
+ // on peut maintenant filtrer les solutions avec la contrainte
+ // n^tm / d > 0
+ // attention, il faut travailler avec la bonne normale,
+ // soit Ni et non pas Nprimi
+ if (cas == cas1)
+ {
+ N1 = V *Nprim1;
+
+ tmp = N1[0] * x + N1[1] * y + N1[2];
+ tmp/= (distanceFictive *s);
+ norm1ok = (tmp>0);
+
+ N2 = V *Nprim2;
+ tmp = N2[0] * x + N2[1] *y+ N2[2];
+ tmp/= (distanceFictive*s);
+ norm2ok = (tmp>0);
+
+ N3 = V *Nprim3;
+ tmp = N3[0] * x + N3[1]*y+ N3[2];
+ tmp/= (distanceFictive*s);
+ norm3ok = (tmp>0);
+
+ N4 = V *Nprim4;
+ tmp = N4[0] * x + N4[1] * y + N4[2];
+ tmp/= (distanceFictive*s);
+ norm4ok = (tmp>0);
+ }
+
+ if (cas == cas2)
+ {
+ N1 = V *Nprim1;
+ tmp = N1[0] * x + N1[1] * y + N1[2];
+ tmp/= (distanceFictive*s);
+ norm1ok = (tmp>0);
+
+ N2 = V *Nprim2;
+ tmp = N2[0] * x + N2[1] * y + N2[2];
+ tmp/= (distanceFictive*s);
+ norm2ok = (tmp>0);
+ }
+
+ if (cas == cas3)
+ {
+ N1 = V *Nprim1;
+ tmp = N1[0] * x + N1[1] * y + N1[2];
+ tmp/= (distanceFictive*s);
+ norm1ok = (tmp>0);
+
+ N2 = V *Nprim2;
+ tmp = N2[0] * x + N2[1] * y + N2[2];
+ tmp/= (distanceFictive*s);
+ norm2ok = (tmp>0);
+ }
+
+ // on a donc maintenant les differents cas possibles
+ // on peut ensuite remonter aux deplacements
+ // on separe encore les cas 1,2,3
+ // la, deux choix se posent suivant le signe de d
+ if (distanceFictive>0)
+ {
+ if (cas == cas1)
+ {
+ if (norm1ok)
+ {
+
+ //cos theta
+ Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
+ ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim1[2][2] = Rprim1[0][0];
+
+ // sin theta
+ Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim1[0][2] = -Rprim1[2][0];
+
+ Rprim1[1][1] =1.0;
+
+ Tprim1[0] = Nprim1[0];
+ Tprim1[1] = 0.0;
+ Tprim1[2] = -Nprim1[2];
+
+ Tprim1*=(sv[0] - sv[2]);
+
+ }
+
+ if (norm2ok)
+ {
+
+ //cos theta
+ Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
+ ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim2[2][2] = Rprim2[0][0];
+
+ // sin theta
+ Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim2[0][2] = -Rprim2[2][0];
+
+ Rprim2[1][1] =1.0;
+
+ Tprim2[0] = Nprim2[0];
+ Tprim2[1] = 0.0;
+ Tprim2[2] = -Nprim2[2];
+
+ Tprim2*=(sv[0] - sv[2]);
+
+ }
+
+ if (norm3ok)
+ {
+
+ //cos theta
+ Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
+ ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim3[2][2] = Rprim3[0][0];
+
+ // sin theta
+ Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim3[0][2] = -Rprim3[2][0];
+
+ Rprim3[1][1] =1.0;
+
+ Tprim3[0] = Nprim3[0];
+ Tprim3[1] = 0.0;
+ Tprim3[2] = -Nprim3[2];
+
+ Tprim3*=(sv[0] - sv[2]);
+
+ }
+
+ if (norm4ok)
+ {
+
+ //cos theta
+ Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
+ ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim4[2][2] = Rprim4[0][0];
+
+ // sin theta
+ Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] + sv[2]) * sv[1]);
+
+ Rprim4[0][2] = -Rprim4[2][0];
+
+ Rprim4[1][1] =1.0;
+
+ Tprim4[0] = Nprim4[0];
+ Tprim4[1] = 0.0;
+ Tprim4[2] = -Nprim4[2];
+
+ Tprim4*=(sv[0] - sv[2]);
+
+ }
+ }
+
+ if (cas == cas2)
+ {
+ // 2 normales sont potentiellement candidates
+
+ if (norm1ok)
+ {
+ Rprim1.eye();
+
+ Tprim1 = Nprim1[0];
+ Tprim1*= (sv[2] - sv[0]);
+ }
+
+ if (norm2ok)
+ {
+ Rprim2.eye();
+
+ Tprim2 = Nprim2[1];
+ Tprim2*= (sv[2] - sv[0]);
+ }
+ }
+ if (cas == cas3)
+ {
+ if (norm1ok)
+ {
+ Rprim1.eye();
+
+ Tprim1 = Nprim1[0];
+ Tprim1*= (sv[0] - sv[1]);
+ }
+
+ if (norm2ok)
+ {
+ Rprim2.eye();
+
+ Tprim2 = Nprim2[1];
+ Tprim2*= (sv[0] - sv[1]);
+ }
+ }
+ if (cas == cas4)
+ {
+ // on ne connait pas la normale dans ce cas la
+ Rprim1.eye();
+ Tprim1 = 0.0;
+ }
+ }
+
+ if (distanceFictive <0)
+ {
+
+ if (cas == cas1)
+ {
+ if (norm1ok)
+ {
+
+
+ //cos theta
+ Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
+ ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim1[2][2] = -Rprim1[0][0];
+
+ // sin theta
+ Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim1[0][2] = Rprim1[2][0];
+
+ Rprim1[1][1] = -1.0;
+
+ Tprim1[0] = Nprim1[0];
+ Tprim1[1] = 0.0;
+ Tprim1[2] = Nprim1[2];
+
+ Tprim1*=(sv[0] + sv[2]);
+
+ }
+
+ if (norm2ok)
+ {
+
+ //cos theta
+ Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
+ ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim2[2][2] = -Rprim2[0][0];
+
+ // sin theta
+ Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim2[0][2] = Rprim2[2][0];
+
+ Rprim2[1][1] = - 1.0;
+
+ Tprim2[0] = Nprim2[0];
+ Tprim2[1] = 0.0;
+ Tprim2[2] = Nprim2[2];
+
+ Tprim2*=(sv[0] + sv[2]);
+
+ }
+
+ if (norm3ok)
+ {
+
+ //cos theta
+ Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
+ ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim3[2][2] = -Rprim3[0][0];
+
+ // sin theta
+ Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim3[0][2] = Rprim3[2][0];
+
+ Rprim3[1][1] = -1.0;
+
+ Tprim3[0] = Nprim3[0];
+ Tprim3[1] = 0.0;
+ Tprim3[2] = Nprim3[2];
+
+ Tprim3*=(sv[0] + sv[2]);
+
+ }
+
+ if (norm4ok)
+ {
+ //cos theta
+ Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
+
+ Rprim4[2][2] = -Rprim4[0][0];
+
+ // sin theta
+ Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
+ (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
+ / ((sv[0] - sv[2]) * sv[1]);
+
+ Rprim4[0][2] = Rprim4[2][0];
+
+ Rprim4[1][1] = - 1.0;
+
+ Tprim4[0] = Nprim4[0];
+ Tprim4[1] = 0.0;
+ Tprim4[2] = Nprim4[2];
+
+ Tprim4*=(sv[0] + sv[2]);
+ }
+ }
+ if (cas == cas2)
+ {
+ // 2 normales sont potentiellement candidates
+
+ if (norm1ok)
+ {
+ Rprim1.eye();
+ Rprim1[0][0] = -1;
+ Rprim1[1][1] = -1;
+
+ Tprim1 = Nprim1[0];
+ Tprim1*= (sv[2] + sv[0]);
+ }
+
+ if (norm2ok)
+ {
+ Rprim2.eye();
+ Rprim2[0][0] = -1;
+ Rprim2[1][1] = -1;
+
+ Tprim2 = Nprim2[1];
+ Tprim2*= (sv[2] + sv[0]);
+ }
+ }
+ if (cas == cas3)
+ {
+ if (norm1ok)
+ {
+ Rprim1.eye();
+ Rprim1[2][2] = -1;
+ Rprim1[1][1] = -1;
+
+ Tprim1 = Nprim1[0];
+ Tprim1*= (sv[2] + sv[0]);
+ }
+
+ if (norm2ok)
+ {
+ Rprim2.eye();
+ Rprim2[2][2] = -1;
+ Rprim2[1][1] = -1;
+
+ Tprim2 = Nprim2[1];
+ Tprim2*= (sv[0] + sv[2]);
+ }
+ }
+
+ // ON NE CONSIDERE PAS LE CAS NUMERO 4
+ }
+ // tous les Rprim et Tprim sont calcules
+ // on peut maintenant recuperer la
+ // rotation, et la translation
+ // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
+ if ((distanceFictive>0) || (cas != cas4))
+ {
+ // on controle juste si les normales sont ok
+
+ if (norm1ok)
+ {
+ R1 = s * U * Rprim1 * V.t();
+ T1 = U * Tprim1;
+ T1 /= (distanceFictive *s);
+ N1 = V *Nprim1;
+
+ // je rajoute le resultat
+ vR.push_back(R1);
+ vT.push_back(T1);
+ vN.push_back(N1);
+ }
+ if (norm2ok)
+ {
+ R2 = s * U * Rprim2 * V.t();
+ T2 = U * Tprim2;
+ T2 /= (distanceFictive *s);
+ N2 = V *Nprim2;
+
+ // je rajoute le resultat
+ vR.push_back(R2);
+ vT.push_back(T2);
+ vN.push_back(N2);
+ }
+ if (norm3ok)
+ {
+ R3 = s * U * Rprim3 * V.t();
+ T3 = U * Tprim3;
+ T3 /= (distanceFictive *s);
+ N3 = V *Nprim3;
+ // je rajoute le resultat
+ vR.push_back(R3);
+ vT.push_back(T3);
+ vN.push_back(N3);
+ }
+ if (norm4ok)
+ {
+ R4 = s * U * Rprim4 * V.t();
+ T4 = U * Tprim4;
+ T4 /= (distanceFictive *s);
+ N4 = V *Nprim4;
+
+ // je rajoute le resultat
+ vR.push_back(R4);
+ vT.push_back(T4);
+ vN.push_back(N4);
+ }
+ }
+ else
+ {
+ std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
+ }
+
+ // on peut ensuite afficher les resultats...
+ /* std::cout << "Analyse des resultats : "<< std::endl; */
+ /* if (cas==cas1) */
+ /* std::cout << "On est dans le cas 1" << std::endl; */
+ /* if (cas==cas2) */
+ /* std::cout << "On est dans le cas 2" << std::endl; */
+ /* if (cas==cas3) */
+ /* std::cout << "On est dans le cas 3" << std::endl; */
+ /* if (cas==cas4) */
+ /* std::cout << "On est dans le cas 4" << std::endl; */
+
+ /* if (distanceFictive < 0) */
+ /* std::cout << "d'<0" << std::endl; */
+ /* else */
+ /* std::cout << "d'>0" << std::endl; */
+
+#ifdef DEBUG_Homographie
+ printf("fin : Homographie_EstimationDeplacementCamera\n");
+#endif
+}
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp
new file mode 100644
index 0000000..04907ab
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp
@@ -0,0 +1,720 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography estimation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file vpHomographyMalis.cpp
+
+ This file implements the fonctions related with the homography
+ estimation from non planar points using the Malis algorithms \cite Malis00b.
+
+ The algorithm for 2D scene implemented in this file is described in Ezio
+ Malis PhD thesis \cite TheseMalis.
+
+*/
+
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMatrixException.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+const double eps = 1e-6 ;
+
+/**************************************************************************
+ * NOM :
+ * changeFrame
+ *
+ * DESCRIPTION :
+ * Changement de repere Euclidien.
+ *
+ ****************************************************************************
+ * 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
+ *
+ *
+ * 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 ..
+ *
+ *
+ ****************************************************************************
+ */
+
+void changeFrame(unsigned int *pts_ref,
+ unsigned int nb_pts,
+ vpMatrix &pd, vpMatrix &p,
+ vpMatrix &pnd, vpMatrix &pn,
+ vpMatrix &M, vpMatrix &Mdp);
+void HLM2D(unsigned int nb_pts,
+ vpMatrix &points_des,
+ vpMatrix &points_cour,
+ vpMatrix &H);
+void HLM3D(unsigned int nb_pts,
+ vpMatrix &pd,
+ vpMatrix &p,
+ vpMatrix &H);
+void HLM(unsigned int q_cible,
+ unsigned int nbpt,
+ double *xm, double *ym,
+ double *xmi, double *ymi,
+ vpMatrix &H);
+
+void HLM(unsigned int q_cible,
+ const std::vector<double> &xm, const std::vector<double> &ym,
+ const std::vector<double> &xmi, const std::vector<double> &ymi,
+ vpMatrix &H);
+
+void changeFrame(unsigned int *pts_ref,
+ unsigned int nb_pts,
+ vpMatrix &pd, vpMatrix &p,
+ vpMatrix &pnd, vpMatrix &pn,
+ vpMatrix &M, vpMatrix &Mdp)
+{
+ unsigned int i,j, k ;
+ unsigned int cont_pts; /* */
+ double lamb_des[3]; /* */
+ double lamb_cour[3] ; /* */
+
+
+
+ /* Construction des matrices de changement de repere */
+ vpMatrix Md(3,3) ;
+ vpMatrix Mp(3,3) ;
+
+ for (i=0;i<3;i++) {
+ for (j=0;j<3;j++) {
+ M[j][i] = p[pts_ref[i]][j] ;
+ Md[j][i] = pd[pts_ref[i]][j] ;
+ }
+ }
+
+ /*calcul de la pseudo inverse */
+ Mp= M.pseudoInverse(1e-16) ;
+ Mdp = Md.pseudoInverse(1e-16) ;
+
+ if (pts_ref[3] > 0) {
+ for (i=0;i<3;i++) {
+ for (j=0;j<3;j++) {
+ lamb_cour[i] = Mp[i][j]*p[pts_ref[3]][j] ;
+ lamb_des[i] = Mdp[i][j]*pd[pts_ref[3]][j] ;
+ }
+ }
+
+ for (i=0;i<3;i++) {
+ for (j=0;j<3;j++) {
+ M[i][j] = M[i][j]*lamb_cour[j] ;
+ Md[i][j] = Md[i][j]*lamb_des[j] ;
+ }
+ }
+
+ Mdp = Md.pseudoInverse(1e-16);
+ }
+
+
+ /* changement de repere pour tous les points autres que
+ les trois points de reference */
+
+ cont_pts = 0 ;
+ for (k=0;k<nb_pts;k++) {
+ if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) {
+ for (i=0;i<3;i++) {
+ pn[cont_pts][i] = 0.0 ;
+ pnd[cont_pts][i] = 0.0 ;
+ for (j=0;j<3;j++) {
+ pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j]*p[k][j] ;
+ pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j]*pd[k][j] ;
+ }
+ }
+ cont_pts = cont_pts + 1;
+ }
+ }
+
+
+}
+
+
+/**************************************************************************
+ * NOM :
+ * Homographie_CrvMafEstHomoPointsCible2D
+ *
+ * DESCRIPTION :
+ * Calcul de l'homographie entre une image courante et une image desiree dans le
+ * cas particulier d'une cible planaire de points (cible pi).
+ * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
+ *
+ ****************************************************************************
+ * ENTREES :
+ * 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
+ *
+ ****************************************************************************
+ * AUTEUR : BOSSARD Nicolas. INSA Rennes 5eme annee.
+ *
+ * DATE DE CREATION : 02/12/98
+ *
+ * DATES DE MISE A JOUR :
+ *
+ ****************************************************************************/
+void
+HLM2D(unsigned int nb_pts,
+ vpMatrix &points_des,
+ vpMatrix &points_cour,
+ vpMatrix &H)
+{
+ unsigned int i,j ;
+
+ double vals_inf ;
+ unsigned int contZeros, vect;
+
+ /** allocation des matrices utilisees uniquement dans la procedure **/
+ vpMatrix M(3*nb_pts,9) ;
+ vpMatrix V(9,9) ;
+ vpColVector sv(9) ;
+
+ /** construction de la matrice M des coefficients dans le cas general **/
+ for (j=0; j<nb_pts ;j++) {
+ M[3*j][0] = 0 ;
+ M[3*j][1] = 0 ;
+ M[3*j][2] = 0 ;
+ M[3*j][3] = -points_des[j][0]*points_cour[j][2] ;
+ M[3*j][4] = -points_des[j][1]*points_cour[j][2] ;
+ M[3*j][5] = -points_des[j][2]*points_cour[j][2] ;
+ M[3*j][6] = points_des[j][0]*points_cour[j][1] ;
+ M[3*j][7] = points_des[j][1]*points_cour[j][1] ;
+ M[3*j][8] = points_des[j][2]*points_cour[j][1] ;
+
+ M[3*j+1][0] = points_des[j][0]*points_cour[j][2] ;
+ M[3*j+1][1] = points_des[j][1]*points_cour[j][2] ;
+ M[3*j+1][2] = points_des[j][2]*points_cour[j][2] ;
+ M[3*j+1][3] = 0 ;
+ M[3*j+1][4] = 0 ;
+ M[3*j+1][5] = 0 ;
+ M[3*j+1][6] = -points_des[j][0]*points_cour[j][0] ;
+ M[3*j+1][7] = -points_des[j][1]*points_cour[j][0] ;
+ M[3*j+1][8] = -points_des[j][2]*points_cour[j][0] ;
+
+ M[3*j+2][0] = -points_des[j][0]*points_cour[j][1] ;
+ M[3*j+2][1] = -points_des[j][1]*points_cour[j][1] ;
+ M[3*j+2][2] = -points_des[j][2]*points_cour[j][1] ;
+ M[3*j+2][3] = points_des[j][0]*points_cour[j][0] ;
+ M[3*j+2][4] = points_des[j][1]*points_cour[j][0] ;
+ M[3*j+2][5] = points_des[j][2]*points_cour[j][0] ;
+ M[3*j+2][6] = 0 ;
+ M[3*j+2][7] = 0 ;
+ M[3*j+2][8] = 0 ;
+ }
+
+ /** calcul de la pseudo-inverse V de M et des valeurs singulieres **/
+ M.svd(sv,V);
+
+ /*****
+ La meilleure solution est le vecteur de V associe
+ 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
+ de 2 valeurs singulieres quasi=0
+ *****/
+ vals_inf = fabs(sv[0]) ;
+ vect = 0 ;
+ contZeros = 0;
+ if (fabs(sv[0]) < eps) {
+ contZeros = contZeros + 1 ;
+ }
+ for (j=1; j<9; j++) {
+ if (fabs(sv[j]) < vals_inf) {
+ vals_inf = fabs(sv[j]);
+ vect = j ;
+ }
+ if (fabs(sv[j]) < eps) {
+ contZeros = contZeros + 1 ;
+ }
+ }
+
+
+ /** cas d'erreur : plus de 2 valeurs singulieres =0 **/
+ if (contZeros > 2) {
+ //vpERROR_TRACE("matrix is rank deficient");
+ throw (vpMatrixException(vpMatrixException::matrixError,
+ "matrix is rank deficient"));
+ }
+
+ H.resize(3,3) ;
+ /** construction de la matrice H **/
+ for (i=0; i<3; i++) {
+ for (j=0; j<3; j++){
+ H[i][j] = V[3*i+j][vect];
+ }
+ }
+
+
+}
+
+
+/**************************************************************************
+ * NOM :
+ * Homographie_CrvMafEstHomoPointsC3DEzio
+ *
+ * DESCRIPTION :
+ * Calcul de l'homographie entre une image courante et une image desiree dans le
+ * cas particulier d'une cible non planaire de points (cible pi).
+ * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
+ *
+ *
+ ****************************************************************************
+ * ENTREES :
+ * 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
+ *
+ ****************************************************************************
+ **/
+void
+HLM3D(unsigned int nb_pts,
+ vpMatrix &pd,
+ vpMatrix &p,
+ vpMatrix &H)
+{
+ unsigned int i,j,k,ii,jj ;
+ unsigned int cont_pts; /* Pour compter le nombre de points dans l'image */
+ //unsigned int nl; /*** Nombre de lignes ***/
+ unsigned int nc ; /*** Nombre de colonnes ***/
+ unsigned int pts_ref[4]; /*** definit lesquels des points de
+ l'image sont les points de reference***/
+ /*** ***/
+ unsigned int perm; /*** Compte le nombre de permutations, quand le nombre
+ de permutations =0 arret de l'ordonnancement **/
+ int cont_zeros; /*** pour compter les valeurs quasi= a zero ***/
+ unsigned int cont;
+ unsigned int vect;
+
+ //int prob;
+
+ /***** Corps de la fonction *****/
+
+ /* allocation des matrices utilisees uniquement dans la procedure */
+ //prob=0;
+
+ vpMatrix M(3,3) ;
+ vpMatrix Mdp(3,3) ;
+ vpMatrix c(8,2) ; // matrice des coeff C
+
+ vpColVector d(8) ;
+ vpMatrix cp(2,8) ; //matrice pseudo-inverse de C
+
+
+ vpMatrix H_int(3,3) ;
+ vpMatrix pn((nb_pts-3),3) ; //points courant nouveau repere
+
+
+ vpMatrix pnd((nb_pts-3),3) ; //points derives nouveau repere
+
+ /* preparation du changement de repere */
+ /****
+ comme plan de reference on choisit pour le moment
+ arbitrairement le plan contenant les points 1,2,3 du cinq
+ ****/
+ pts_ref[0] = 0 ;
+ pts_ref[1] = 1 ;
+ pts_ref[2] = 2 ;
+ pts_ref[3] = 0 ;
+
+ /* changement de repere pour tous les points autres que les trois points de reference */
+
+ changeFrame(pts_ref,nb_pts,pd,p,pnd,pn,M,Mdp);
+
+
+ cont_pts = nb_pts - 3 ;
+
+ if (cont_pts < 5)
+ {
+ //vpERROR_TRACE(" not enough point to compute the homography ... ");
+ throw (vpMatrixException(vpMatrixException::matrixError,
+ "Not enough point to compute the homography"));
+ }
+
+ //nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ;
+ nc = 7 ;
+
+ /* Allocation matrice CtC */
+ vpMatrix CtC(nc,nc) ;
+
+ /* Initialisation matrice CtC */
+ for (i=0;i<nc;i++) for (j=0;j<nc;j++) CtC[i][j] = 0.0;
+
+
+ /* Allocation matrice M */
+ vpColVector C(nc) ; //Matrice des coefficients
+
+ /* construction de la matrice M des coefficients dans le cas general */
+ /****
+ inconnues du nouveau algorithme
+ x1 = a ; x2 = b ; x3 = c ;
+ x4 = ex ; x5 = ey ; x6 = ez ;
+ qui deviennent apres changement :
+ v1 = x1*x6 ; v2 = x1*x5 ;
+ v3 = x2*x4 ; v4 = x2*x6 ;
+ v5 = x3*x5 ; v6 = x3*x4 ;
+ ****/
+ cont = 0 ;
+ for (i=0 ; i<nb_pts-5; i++) {
+ for (j = i+1 ; j<nb_pts-4; j++) {
+ for (k = j+1 ; k<nb_pts-3; k ++) {
+ /* coeff a^2*b */
+ C[0] = pn[i][2]*pn[j][2]*pn[k][1]*pnd[k][0] //
+ * (pnd[j][0]*pnd[i][1] - pnd[j][1]*pnd[i][0])//
+ + pn[i][2]*pn[k][2]*pn[j][1]*pnd[j][0]//
+ *(pnd[i][0]*pnd[k][1] - pnd[i][1]*pnd[k][0])//
+ + pn[j][2]*pn[k][2]*pn[i][1]*pnd[i][0] //
+ *(pnd[k][0]*pnd[j][1] - pnd[k][1]*pnd[j][0]) ; //
+ /* coeff a*b^2 */
+ C[1] = pn[i][2]*pn[j][2]*pn[k][0]*pnd[k][1]//
+ *(pnd[i][0]*pnd[j][1] - pnd[i][1]*pnd[j][0])//
+ + pn[i][2]*pn[k][2]*pn[j][0]*pnd[j][1]//
+ *(pnd[k][0]*pnd[i][1] - pnd[k][1]*pnd[i][0])//
+ + pn[j][2]*pn[k][2]*pn[i][0]*pnd[i][1]//
+ *(pnd[j][0]*pnd[k][1] - pnd[j][1]*pnd[k][0]) ;//
+ /* coeff a^2 */
+ C[2] = + pn[i][1]*pn[k][1]*pn[j][2]*pnd[j][0]//
+ *(pnd[k][2]*pnd[i][0] - pnd[k][0]*pnd[i][2])//
+ +pn[i][1]*pn[j][1]*pn[k][2]*pnd[k][0] //
+ *(pnd[i][2]*pnd[j][0] - pnd[i][0]*pnd[j][2])
+ + pn[j][1]*pn[k][1]*pn[i][2]*pnd[i][0] //
+ *(pnd[j][2]*pnd[k][0] - pnd[j][0]*pnd[k][2]) ; //
+
+
+
+ /* coeff b^2 */
+ C[3] = pn[i][0]*pn[j][0]*pn[k][2]*pnd[k][1] //
+ *(pnd[i][2]*pnd[j][1] - pnd[i][1]*pnd[j][2]) //
+ + pn[i][0]*pn[k][0]*pn[j][2]*pnd[j][1] //
+ *(pnd[k][2]*pnd[i][1] - pnd[k][1]*pnd[i][2]) //
+ + pn[j][0]*pn[k][0]*pn[i][2]*pnd[i][1] //
+ *(pnd[j][2]*pnd[k][1] - pnd[j][1]*pnd[k][2]) ; //
+
+ /* coeff a */
+ C[5] = pn[i][1]*pn[j][1]*pn[k][0]*pnd[k][2]//
+ *(pnd[i][0]*pnd[j][2] - pnd[i][2]*pnd[j][0])//
+ + pn[i][1]*pn[k][1]*pn[j][0]*pnd[j][2] //
+ *(pnd[k][0]*pnd[i][2] - pnd[k][2]*pnd[i][0])//
+ + pn[j][1]*pn[k][1]*pn[i][0]*pnd[i][2]//
+ *(pnd[j][0]*pnd[k][2] - pnd[j][2]*pnd[k][0]) ;//
+ /* coeff b */
+ C[6] = pn[i][0]*pn[j][0]*pn[k][1]*pnd[k][2] //
+ *(pnd[i][1]*pnd[j][2] - pnd[i][2]*pnd[j][1])//
+ + pn[i][0]*pn[k][0]*pn[j][1]*pnd[j][2]//
+ *(pnd[k][1]*pnd[i][2] - pnd[k][2]*pnd[i][1])//
+ + pn[j][0]*pn[k][0]*pn[i][1]*pnd[i][2]//
+ *(pnd[j][1]*pnd[k][2] - pnd[j][2]*pnd[k][1]) ;//
+ /* coeff a*b */
+ C[4] = pn[i][0]*pn[k][1]*pn[j][2] //
+ *(pnd[k][0]*pnd[j][1]*pnd[i][2] - pnd[j][0]*pnd[i][1]*pnd[k][2])//
+ + pn[k][0]*pn[i][1]*pn[j][2]//
+ *(pnd[j][0]*pnd[k][1]*pnd[i][2] - pnd[i][0]*pnd[j][1]*pnd[k][2])//
+ + pn[i][0]*pn[j][1]*pn[k][2]//
+ *(pnd[k][0]*pnd[i][1]*pnd[j][2] - pnd[j][0]*pnd[k][1]*pnd[i][2])//
+ + pn[j][0]*pn[i][1]*pn[k][2]//
+ *(pnd[i][0]*pnd[k][1]*pnd[j][2] - pnd[k][0]*pnd[j][1]*pnd[i][2])//
+ + pn[k][0]*pn[j][1]*pn[i][2]//
+ *(pnd[j][0]*pnd[i][1]*pnd[k][2] - pnd[i][0]*pnd[k][1]*pnd[j][2])//
+ + pn[j][0]*pn[k][1]*pn[i][2]//
+ *(pnd[i][0]*pnd[j][1]*pnd[k][2] - pnd[k][0]*pnd[i][1]*pnd[j][2]) ;//
+
+ cont = cont+1 ;
+ /* construction de la matrice CtC */
+ for (ii=0;ii<nc;ii++) {
+ for (jj=ii;jj<nc;jj++) {
+ CtC[ii][jj] = CtC[ii][jj] + C[ii]*C[jj];
+ }
+ }
+
+ }
+ }
+ }
+
+
+
+ /* calcul de CtC */
+ for (i=0; i<nc ;i++) {
+ for (j=i+1; j<nc ;j++) CtC[j][i] = CtC[i][j];
+ }
+
+ //nl = cont ; /* nombre de lignes */
+ nc = 7 ; /* nombre de colonnes */
+
+ /* Creation de matrice CtC termine */
+ /* Allocation matrice V */
+ vpMatrix V(nc,nc) ;
+ /*****
+ Preparation au calcul de la svd (pseudo-inverse)
+ pour obtenir une solution il faut au moins 5 equations independantes
+ donc il faut au moins la mise en correspondence de 3+5 points
+ *****/
+ vpColVector sv(nc) ; //Vecteur contenant les valeurs singulieres
+
+ CtC.svd(sv,V) ;
+
+ /*****
+ Il faut un controle sur le rang de la matrice !!
+ La meilleure solution est le vecteur de V associe
+ a la valeur singuliere la plus petite en valeur
+ absolu
+ *****/
+
+ vpColVector svSorted(nc) ; // sorted singular value
+
+ // sorting the singular value
+ for (i=0; i < nc ;i++) svSorted[i] = sv[i] ;
+ perm = 1 ;
+ double v_temp;
+ while (perm != 0) {
+ perm = 0;
+ for (i=1; i < nc ;i++) {
+ if (svSorted[i-1] > svSorted[i]) {
+ v_temp = svSorted[i-1] ;
+ svSorted[i-1] = svSorted[i] ;
+ svSorted[i] = v_temp ;
+ perm = perm + 1;
+ }
+ }
+ }
+
+ /*****
+ Parcours de la matrice ordonnee des valeurs singulieres
+ On note "cont_zeros" le nbre de valeurs quasi= a 0.
+ On note "vect" le rang de la plus petite valeur singliere
+ en valeur absolu
+ *****/
+
+ vect = 0 ; cont_zeros = 0 ; cont = 0 ;
+ for (j=0; j < nc; j++) {
+ //if (fabs(sv[j]) == svSorted[cont]) vect = j ;
+ if (std::fabs(sv[j]-svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j],svSorted[cont]))) vect = j ;
+ if (std::fabs(sv[j]/svSorted[nc-1]) < eps) cont_zeros = cont_zeros + 1 ;
+ }
+
+ if (cont_zeros > 5) {
+ // printf("erreur dans le rang de la matrice: %d \r\n ",7-cont_zeros);
+ HLM2D(nb_pts,pd,p,H);
+ }
+ else
+ {
+
+ // 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 ;
+ d[0] = V[2][vect] ;
+ d[1] = V[4][vect] ;
+ d[2] = V[1][vect] ;
+ d[3] = V[0][vect] ;
+ d[4] = V[3][vect] ;
+ d[5] = V[4][vect] ;
+ d[6] = V[0][vect] ;
+ d[7] = V[1][vect] ;
+
+ c[0][0] = V[5][vect] ; c[0][1] = 0.0 ;
+ c[1][0] = V[6][vect] ; c[1][1] = 0.0 ;
+ c[2][0] = V[3][vect] ; c[2][1] = 0.0 ;
+ c[3][0] = V[4][vect] ; c[3][1] = 0.0
+ ;
+ c[4][0] = 0.0 ; c[4][1] = V[6][vect] ;
+ c[5][0] = 0.0 ; c[5][1] = V[5][vect] ;
+ c[6][0] = 0.0 ; c[6][1] = V[2][vect] ;
+ c[7][0] = 0.0 ; c[7][1] = V[4][vect] ;
+
+
+
+ /// Calcul de la pseudo-inverse de C
+ cp = c.pseudoInverse(1e-6) ;
+
+
+ vpColVector H_nr(3), temp ; // Homographie diagonale
+ // Multiplication de la matrice H_nr par le vecteur cp
+ temp = cp * d;
+
+ H_nr[0] = temp[0] ; H_nr[1] = temp[1] ;
+ H_nr[2] = 1.0 ;
+
+ vpMatrix T(9,3) ; T =0 ;
+ T[0][0] = -V[1][vect] ; T[0][1] = V[0][vect] ;
+ T[1][0] = V[4][vect] ; T[1][2] = -V[2][vect] ;
+ T[2][0] = -V[6][vect] ; T[2][1] = V[2][vect] ;
+ T[3][0] = V[6][vect] ; T[3][2] = -V[0][vect] ;
+ T[4][0] = -V[3][vect] ; T[4][1] = V[6][vect] ;
+ T[5][0] = V[3][vect] ; T[5][2] = -V[1][vect] ;
+ T[6][0] = -V[5][vect] ; T[6][1] = V[4][vect] ;
+ T[7][0] = V[5][vect] ; T[7][2] = -V[6][vect] ;
+ T[8][1] = -V[5][vect] ; T[8][2] = V[2][vect] ;
+
+
+ vpMatrix Hd(3,3) ; // diag(gu,gv,gw)
+ for (i=0 ; i < 3 ; i++) Hd[i][i] = H_nr[i] ;
+
+ // H = M diag(gu,gv,gw) M*-1
+ H = M*Hd*Mdp ;
+
+
+
+ }
+}
+
+void
+HLM(unsigned int q_cible,
+ const std::vector<double> &xm, const std::vector<double> &ym,
+ const std::vector<double> &xmi, const std::vector<double> &ymi,
+ vpMatrix &H)
+{
+ unsigned int nbpt = (unsigned int)xm.size();
+
+ /****
+ on regarde si il y a au moins un point mais pour l'homographie
+ il faut au moins quatre points
+ ****/
+ vpMatrix pd(nbpt,3) ;
+ vpMatrix p(nbpt,3) ;
+
+ for (unsigned int i=0;i<nbpt;i++) {
+ /****
+ on assigne les points fournies par la structure robot
+ pour la commande globale
+ ****/
+ pd[i][0] = xmi[i];
+ pd[i][1] = ymi[i];
+ pd[i][2] = 1.0 ;
+ p[i][0] = xm[i];
+ p[i][1] = ym[i];
+ p[i][2] = 1.0 ;
+ }
+
+ switch (q_cible) {
+ case (1):
+ case (2):
+ /* La cible est planaire de type points */
+
+ HLM2D(nbpt,pd,p,H);
+
+ break;
+ case (3) : /* cible non planaire : chateau */
+ /* cible non planaire de type points */
+ HLM3D(nbpt,pd,p,H);
+ break;
+ } /* fin switch */
+
+
+
+} /* fin procedure calcul_homogaphie */
+
+#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<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ bool isplanar,
+ vpHomography &aHb)
+{
+ unsigned int n = (unsigned int) xb.size();
+ if (yb.size() != n || xa.size() != n || ya.size() != n)
+ throw(vpException(vpException::dimensionError,
+ "Bad dimension for HLM shomography estimation"));
+
+ // 4 point are required
+ if(n<4)
+ throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
+
+ // The reference plane is the plane build from the 3 first points.
+ unsigned int q_cible;
+ vpMatrix H; // matrice d'homographie en metre
+
+ if (isplanar)
+ q_cible =1;
+ else
+ q_cible =3;
+
+ ::HLM(q_cible, xa, ya, xb, yb, H) ;
+
+ aHb = H;
+}
+
diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp
new file mode 100644
index 0000000..3b2262e
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp
@@ -0,0 +1,540 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography estimation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRansac.h>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+#define vpEps 1e-6
+
+/*!
+ \file vpHomographyRansac.cpp
+ \brief function used to estimate an homography using the Ransac algorithm
+*/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+bool iscolinear(double *x1, double *x2, double *x3);
+bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
+
+bool
+iscolinear(double *x1, double *x2, double *x3)
+{
+ vpColVector p1(3), p2(3), p3(3);
+ p1 << x1 ;
+ p2 << x2 ;
+ p3 << x3 ;
+ //vpColVector v;
+ //vpColVector::cross(p2-p1, p3-p1, v);
+ //return (v.sumSquare() < vpEps);
+ // Assume inhomogeneous coords, or homogeneous coords with equal
+ // scale.
+ return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
+}
+bool
+isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
+{
+ return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
+}
+
+
+bool
+vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind,
+ double threshold_area)
+{
+
+ unsigned int i, j, k;
+
+ for (i=1 ; i < 4 ; i++)
+ for (j=0 ; j<i ; j++)
+ if (ind[i]==ind[j]) return true ;
+
+ unsigned int n = x.getRows()/4 ;
+ double pa[4][3] ;
+ double pb[4][3] ;
+
+
+
+ for(i = 0 ; i < 4 ; i++)
+ {
+ pb[i][0] = x[2*ind[i]] ;
+ pb[i][1] = x[2*ind[i]+1] ;
+ pb[i][2] = 1;
+
+ pa[i][0] = x[2*n+2*ind[i]] ;
+ pa[i][1] = x[2*n+2*ind[i]+1] ;
+ pa[i][2] = 1;
+ }
+
+ i = 0, j = 1, k = 2;
+
+ double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
+ pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
+ -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+
+ i = 0; j = 1, k = 3;
+ double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
+ pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
+ -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+
+ i = 0; j = 2, k = 3;
+ double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
+ pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
+ -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+
+ i = 1; j = 2, k = 3;
+ double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
+ pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
+ -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+
+ double sum_area = area012 + area013 + area023 + area123;
+
+ return ((sum_area < threshold_area) ||
+ (iscolinear(pa[0],pa[1],pa[2]) ||
+ iscolinear(pa[0],pa[1],pa[3]) ||
+ iscolinear(pa[0],pa[2],pa[3]) ||
+ iscolinear(pa[1],pa[2],pa[3]) ||
+ iscolinear(pb[0],pb[1],pb[2]) ||
+ iscolinear(pb[0],pb[1],pb[3]) ||
+ iscolinear(pb[0],pb[2],pb[3]) ||
+ iscolinear(pb[1],pb[2],pb[3])));
+}
+ /*
+\brief
+Function to determine if a set of 4 pairs of matched points give rise
+to a degeneracy in the calculation of a homography as needed by RANSAC.
+This involves testing whether any 3 of the 4 points in each set is
+colinear.
+
+point are coded this way
+x1b,y1b, x2b, y2b, ... xnb, ynb
+x1a,y1a, x2a, y2a, ... xna, yna
+leading to 2*2*n
+*/
+bool
+vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
+{
+ for (unsigned int i = 1; i < 4 ; i++)
+ for (unsigned int j = 0 ;j < i ; j++)
+ if (ind[i] == ind[j]) return true ;
+
+ unsigned int n = x.getRows()/4;
+ double pa[4][3];
+ double pb[4][3];
+ unsigned int n2 = 2 * n;
+ unsigned int ind2;
+ for(unsigned int i = 0; i < 4 ;i++)
+ {
+ ind2 = 2 * ind[i];
+ pb[i][0] = x[ind2];
+ pb[i][1] = x[ind2+1];
+ pb[i][2] = 1;
+
+ pa[i][0] = x[n2+ind2] ;
+ pa[i][1] = x[n2+ind2+1] ;
+ pa[i][2] = 1;
+ }
+ return ( iscolinear(pa[0],pa[1],pa[2]) ||
+ iscolinear(pa[0],pa[1],pa[3]) ||
+ iscolinear(pa[0],pa[2],pa[3]) ||
+ iscolinear(pa[1],pa[2],pa[3]) ||
+ iscolinear(pb[0],pb[1],pb[2]) ||
+ iscolinear(pb[0],pb[1],pb[3]) ||
+ iscolinear(pb[0],pb[2],pb[3]) ||
+ iscolinear(pb[1],pb[2],pb[3]));
+}
+bool
+vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya)
+{
+ unsigned int n = (unsigned int)xb.size();
+ if (n < 4)
+ throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
+
+ std::vector<vpColVector> pa(n), pb(n);
+ for (unsigned i=0; i<n;i++) {
+ pa[i].resize(3);
+ pa[i][0] = xa[i];
+ pa[i][1] = ya[i];
+ pa[i][2] = 1;
+ pb[i].resize(3);
+ pb[i][0] = xb[i];
+ pb[i][1] = yb[i];
+ pb[i][2] = 1;
+ }
+
+ for (unsigned int i = 0; i < n-2; i++) {
+ for (unsigned int j = i+1; j < n-1; j++) {
+ for (unsigned int k = j+1; k < n ; k++)
+ {
+ if (isColinear(pa[i], pa[j], pa[k])) {
+ return true;
+ }
+ if (isColinear(pb[i], pb[j], pb[k])){
+ return true;
+ }
+ }
+ }
+ }
+ return false;
+}
+// Fit model to this random selection of data points.
+void
+vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
+{
+ unsigned int i ;
+ unsigned int n = x.getRows()/4 ;
+ std::vector<double> xa(4), xb(4);
+ std::vector<double> ya(4), yb(4);
+ unsigned int n2 = n * 2;
+ unsigned int ind2;
+ for(i=0 ; i < 4 ; i++)
+ {
+ ind2 = 2 * ind[i];
+ xb[i] = x[ind2] ;
+ yb[i] = x[ind2+1] ;
+
+ xa[i] = x[n2+ind2] ;
+ ya[i] = x[n2+ind2+1] ;
+ }
+
+ vpHomography aHb ;
+ try {
+ vpHomography::HLM(xb, yb, xa, ya, true, aHb);
+ }
+ catch(...)
+ {
+ aHb.setIdentity();
+ }
+
+ M.resize(9);
+ for (i=0 ; i <9 ; i++)
+ {
+ M[i] = aHb.data[i] ;
+ }
+ aHb /= aHb[2][2] ;
+}
+
+
+// Evaluate distances between points and model.
+double
+vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
+{
+ unsigned int i ;
+ unsigned int n = x.getRows()/4 ;
+ unsigned int n2 = n * 2;
+ unsigned int i2;
+ vpColVector *pa;
+ vpColVector *pb;
+
+ pa = new vpColVector [n];
+ pb = new vpColVector [n];
+
+ for( i=0 ; i < n ; i++)
+ {
+ i2 = 2 * i;
+ pb[i].resize(3) ;
+ pb[i][0] = x[i2] ;
+ pb[i][1] = x[i2+1] ;
+ pb[i][2] = 1;
+
+ pa[i].resize(3) ;
+ pa[i][0] = x[n2+i2] ;
+ pa[i][1] = x[n2+i2+1] ;
+ pa[i][2] = 1;
+ }
+
+ vpMatrix aHb(3,3) ;
+
+ for (i=0 ; i <9 ; i++)
+ {
+ aHb.data[i] = M[i];
+ }
+
+ aHb /= aHb[2][2];
+
+ d.resize(n);
+
+ vpColVector Hpb ;
+ for (i=0 ; i <n ; i++)
+ {
+ Hpb = aHb*pb[i] ;
+ Hpb /= Hpb[2] ;
+ d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
+ }
+
+ delete [] pa;
+ delete [] pb;
+
+ return 0 ;
+}
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+
+void
+vpHomography::initRansac(unsigned int n,
+ double *xb, double *yb,
+ double *xa, double *ya,
+ vpColVector &x)
+{
+ x.resize(4*n) ;
+ unsigned int n2 = n * 2;
+ unsigned int i2;
+ for (unsigned int i=0 ; i < n ; i++)
+ {
+ i2 = 2 * i;
+ x[i2] = xb[i] ;
+ x[i2+1] = yb[i] ;
+ x[n2+i2] = xa[i] ;
+ x[n2+i2+1] = ya[i] ;
+ }
+}
+
+/*!
+
+ 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<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya,
+ vpHomography &aHb,
+ std::vector<bool> &inliers,
+ double &residual,
+ unsigned int nbInliersConsensus,
+ double threshold,
+ bool normalization)
+{
+ unsigned int n = (unsigned int)xb.size();
+ if (yb.size() != n || xa.size() != n || ya.size() != n)
+ throw(vpException(vpException::dimensionError,
+ "Bad dimension for robust homography estimation"));
+
+ // 4 point are required
+ if(n<4)
+ throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
+
+ vpUniRand random((const long)time(NULL)) ;
+
+ std::vector<unsigned int> best_consensus;
+ std::vector<unsigned int> cur_consensus;
+ std::vector<unsigned int> cur_outliers;
+ std::vector<unsigned int> cur_randoms;
+
+ std::vector<unsigned int> rand_ind;
+
+ unsigned int nbMinRandom = 4 ;
+ unsigned int ransacMaxTrials = 1000;
+ unsigned int maxDegenerateIter = 1000;
+
+ unsigned int nbTrials = 0;
+ unsigned int nbDegenerateIter = 0;
+ unsigned int nbInliers = 0;
+
+ bool foundSolution = false;
+
+ std::vector<double> xa_rand(nbMinRandom);
+ std::vector<double> ya_rand(nbMinRandom);
+ std::vector<double> xb_rand(nbMinRandom);
+ std::vector<double> yb_rand(nbMinRandom);
+
+ if (inliers.size() != n)
+ inliers.resize(n);
+
+ while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
+ {
+ cur_outliers.clear();
+ cur_randoms.clear();
+
+ bool degenerate = true;
+ while(degenerate == true){
+ std::vector<bool> usedPt(n, false);
+
+ rand_ind.clear();
+ for(unsigned int i = 0; i < nbMinRandom; i++)
+ {
+ // Generate random indicies in the range 0..n
+ unsigned int r = (unsigned int)ceil(random()*n) -1;
+ while(usedPt[r]) {
+ r = (unsigned int)ceil(random()*n) -1;
+ }
+ usedPt[r] = true;
+ rand_ind.push_back(r);
+
+ xa_rand[i] = xa[r];
+ ya_rand[i] = ya[r];
+ xb_rand[i] = xb[r];
+ yb_rand[i] = yb[r];
+ }
+
+ try{
+ if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
+ vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
+ degenerate = false;
+ }
+ }
+ catch(...){
+ degenerate = true;
+ }
+
+ nbDegenerateIter ++;
+
+ if (nbDegenerateIter > maxDegenerateIter){
+ vpERROR_TRACE("Unable to select a nondegenerate data set");
+ throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
+ }
+ }
+
+ aHb /= aHb[2][2] ;
+
+ // Computing Residual
+ double r = 0;
+ vpColVector a(3), b(3), c(3) ;
+ for (unsigned int i=0 ; i < nbMinRandom ; i++) {
+ a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
+ b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
+
+ c = aHb*b; c /= c[2] ;
+ r += (a-c).sumSquare() ;
+ //cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
+ }
+
+ // Finding inliers & ouliers
+ r = sqrt(r/nbMinRandom);
+ //std::cout << "Candidate residual: " << r << std::endl;
+ if (r < threshold)
+ {
+ unsigned int nbInliersCur = 0;
+ for (unsigned int i = 0; i < n ; i++)
+ {
+ a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
+ b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
+
+ c = aHb*b ; c /= c[2] ;
+ double error = sqrt((a-c).sumSquare()) ;
+ if(error <= threshold){
+ nbInliersCur++;
+ cur_consensus.push_back(i);
+ inliers[i] = true;
+ }
+ else{
+ cur_outliers.push_back(i);
+ inliers[i] = false;
+ }
+ }
+ //std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
+ if(nbInliersCur > nbInliers)
+ {
+ foundSolution = true;
+ best_consensus = cur_consensus;
+ nbInliers = nbInliersCur;
+ }
+
+ cur_consensus.clear();
+ }
+
+ nbTrials++;
+ if(nbTrials >= ransacMaxTrials){
+ vpERROR_TRACE("Ransac reached the maximum number of trials");
+ foundSolution = true;
+ }
+ }
+
+ if(foundSolution){
+ if(nbInliers >= nbInliersConsensus)
+ {
+ std::vector<double> xa_best(best_consensus.size());
+ std::vector<double> ya_best(best_consensus.size());
+ std::vector<double> xb_best(best_consensus.size());
+ std::vector<double> yb_best(best_consensus.size());
+
+ for(unsigned i = 0 ; i < best_consensus.size(); i++)
+ {
+ xa_best[i] = xa[best_consensus[i]];
+ ya_best[i] = ya[best_consensus[i]];
+ xb_best[i] = xb[best_consensus[i]];
+ yb_best[i] = yb[best_consensus[i]];
+ }
+
+ vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
+ aHb /= aHb[2][2];
+
+ residual = 0 ;
+ vpColVector a(3), b(3), c(3);
+ for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
+ a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
+ b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
+
+ c = aHb*b ; c /= c[2] ;
+ residual += (a-c).sumSquare() ;
+ }
+
+ residual = sqrt(residual/best_consensus.size());
+ return true;
+ }
+ else {
+ return false;
+ }
+ }
+ else {
+ return false;
+ }
+}
diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp
new file mode 100644
index 0000000..46772d9
--- /dev/null
+++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp
@@ -0,0 +1,733 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Homography transformation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+//#include <computeHomography.h>
+//#include <utilsHomography.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpPlane.h>
+#include <iostream>
+#include <visp3/core/vpExponentialMap.h>
+
+
+const double vpHomography::threshold_rotation = 1e-7;
+const double vpHomography::threshold_displacement = 1e-18;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+static void
+updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
+{
+ unsigned int i,j;
+
+ double sinu,cosi,mcosi,u[3], s;
+ vpRotationMatrix rd ;
+
+
+ s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
+ if (s > 1.e-25)
+ {
+
+ for (i=0;i<3;i++) u[i] = dx[i]/s;
+ sinu = sin(s);
+ cosi = cos(s);
+ mcosi = 1-cosi;
+ rd[0][0] = cosi + mcosi*u[0]*u[0];
+ rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
+ rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
+ rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
+ rd[1][1] = cosi + mcosi*u[1]*u[1];
+ rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
+ rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
+ rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
+ rd[2][2] = cosi + mcosi*u[2]*u[2];
+ }
+ else
+ {
+ for (i=0;i<3;i++)
+ {
+ for (j=0;j<3;j++) rd[i][j] = 0.0;
+ rd[i][i] = 1.0;
+ }
+
+ }
+
+ vpHomogeneousMatrix Delta ;
+ Delta.insert(rd) ;
+
+ mati = Delta.inverse() * mati ;
+}
+
+double
+vpHomography::computeRotation(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpHomogeneousMatrix &c2Mc1,
+ int userobust
+ )
+{
+ vpColVector e(2) ;
+ double r_1 = -1 ;
+
+ vpColVector p2(3) ;
+ vpColVector p1(3) ;
+ vpColVector Hp2(3) ;
+ vpColVector Hp1(3) ;
+
+ vpMatrix H2(2,3) ;
+ vpColVector e2(2) ;
+ vpMatrix H1(2,3) ;
+ vpColVector e1(2) ;
+
+ int only_1 = 0 ;
+ int only_2 = 0 ;
+ int iter = 0 ;
+
+ unsigned int n=0 ;
+ for (unsigned int i=0 ; i < nbpoint ; i++) {
+ // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
+ if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
+ &&
+ (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
+ {
+ n++ ;
+ }
+ }
+ //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
+ if ( (! only_1) && (! only_2) )
+ n *=2 ;
+
+ vpRobust robust(n);
+ vpColVector res(n) ;
+ vpColVector w(n) ;
+ w =1 ;
+ robust.setThreshold(0.0000) ;
+ vpMatrix W(2*n,2*n) ;
+ W = 0 ;
+ vpMatrix c2Rc1(3,3) ;
+ double r =0 ;
+ while (vpMath::equal(r_1,r,threshold_rotation) == false )
+ {
+
+ r_1 =r ;
+ // compute current position
+
+ //Change frame (current)
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int j=0 ; j < 3 ; j++)
+ c2Rc1[i][j] = c2Mc1[i][j] ;
+
+ vpMatrix L(2,3), Lp ;
+ int k =0 ;
+ for (unsigned int i=0 ; i < nbpoint ; i++) {
+ //if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
+ if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
+ &&
+ (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
+ {
+ p2[0] = c2P[i].get_x() ;
+ p2[1] = c2P[i].get_y() ;
+ p2[2] = 1.0 ;
+ p1[0] = c1P[i].get_x() ;
+ p1[1] = c1P[i].get_y() ;
+ p1[2] = 1.0 ;
+
+ Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
+ Hp1 = c2Rc1*p1 ; // p1 = Hp2
+
+ Hp2 /= Hp2[2] ; // normalisation
+ Hp1 /= Hp1[2] ;
+
+
+ // set up the interaction matrix
+ double x = Hp2[0] ;
+ double y = Hp2[1] ;
+
+ H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
+ H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
+ H2 *=-1 ;
+ H2 = H2*c2Rc1.t() ;
+
+ // Set up the error vector
+ e2[0] = Hp2[0] - c1P[i].get_x() ;
+ e2[1] = Hp2[1] - c1P[i].get_y() ;
+
+ // set up the interaction matrix
+ x = Hp1[0] ;
+ y = Hp1[1] ;
+
+ H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
+ H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
+
+ // Set up the error vector
+ e1[0] = Hp1[0] - c2P[i].get_x() ;
+ e1[1] = Hp1[1] - c2P[i].get_y() ;
+
+ if (only_2==1)
+ {
+ if (k == 0) { L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ }
+ else
+ if (only_1==1)
+ {
+ if (k == 0) { L = H1 ; e= e1 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+ }
+ else
+ {
+ if (k == 0) {L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+
+ k++ ;
+ }
+ }
+
+ if (userobust)
+ {
+ robust.setIteration(0);
+
+ for (unsigned int l=0 ; l < n ; l++)
+ {
+ res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
+ }
+ robust.MEstimator(vpRobust::TUKEY, res, w);
+
+
+ // compute the pseudo inverse of the interaction matrix
+ for (unsigned int l=0 ; l < n ; l++)
+ {
+ W[2*l][2*l] = w[l] ;
+ W[2*l+1][2*l+1] = w[l] ;
+ }
+ }
+ else
+ {
+ for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
+ }
+ // CreateDiagonalMatrix(w, W) ;
+ (L).pseudoInverse(Lp, 1e-6) ;
+ // Compute the camera velocity
+ vpColVector c2rc1, v(6) ;
+
+ c2rc1 = -2*Lp*W*e ;
+ for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2rc1[i] ;
+
+ // only for simulation
+
+ updatePoseRotation(c2rc1, c2Mc1) ;
+ r =e.sumSquare() ;
+
+ if ((W*e).sumSquare() < 1e-10) break ;
+ if (iter>25) break ;
+ iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
+
+ }
+
+ // std::cout << c2Mc1 <<std::endl ;
+ return (W*e).sumSquare() ;
+}
+
+
+static void
+getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
+{
+ double A1 = cMo[0][0]*oN.getA()+ cMo[0][1]*oN.getB() + cMo[0][2]*oN.getC();
+ double B1 = cMo[1][0]*oN.getA()+ cMo[1][1]*oN.getB() + cMo[1][2]*oN.getC();
+ double C1 = cMo[2][0]*oN.getA()+ cMo[2][1]*oN.getB() + cMo[2][2]*oN.getC();
+ double D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
+
+ cN.resize(3) ;
+ cN[0] = A1 ;
+ cN[1] = B1 ;
+ cN[2] = C1 ;
+ cd = -D1 ;
+}
+
+double
+vpHomography::computeDisplacement(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpPlane &oN,
+ vpHomogeneousMatrix &c2Mc1,
+ vpHomogeneousMatrix &c1Mo,
+ int userobust
+ )
+{
+ vpColVector e(2) ;
+ double r_1 = -1 ;
+
+ vpColVector p2(3) ;
+ vpColVector p1(3) ;
+ vpColVector Hp2(3) ;
+ vpColVector Hp1(3) ;
+
+ vpMatrix H2(2,6) ;
+ vpColVector e2(2) ;
+ vpMatrix H1(2,6) ;
+ vpColVector e1(2) ;
+
+ int only_1;
+ int only_2;
+ int iter = 0 ;
+ unsigned int n=0 ;
+ n = nbpoint ;
+ only_1 = 1 ;
+ only_2 = 0 ;
+
+ //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
+ // Since only_1 is initialized to 1, the next 2 lines connt be reached. We remove them
+ //if ( (! only_1) && (! only_2) )
+ // n *=2 ;
+
+ vpRobust robust(n);
+ vpColVector res(n) ;
+ vpColVector w(n) ;
+ w =1 ;
+ robust.setThreshold(0.0000) ;
+ vpMatrix W(2*n,2*n) ;
+ W = 0 ;
+
+ vpColVector N1(3), N2(3) ;
+ double d1, d2 ;
+
+ double r =1e10 ;
+ iter =0 ;
+ while (vpMath::equal(r_1,r,threshold_displacement) == false )
+ {
+ r_1 =r ;
+ // compute current position
+
+ //Change frame (current)
+ vpHomogeneousMatrix c1Mc2, c2Mo ;
+ vpRotationMatrix c1Rc2, c2Rc1 ;
+ vpTranslationVector c1Tc2, c2Tc1 ;
+ c1Mc2 = c2Mc1.inverse() ;
+ c2Mc1.extract(c2Rc1) ;
+ c2Mc1.extract(c2Tc1) ;
+ c2Mc1.extract(c1Rc2) ;
+ c1Mc2.extract(c1Tc2) ;
+
+ c2Mo = c2Mc1*c1Mo ;
+
+ getPlaneInfo(oN, c1Mo, N1, d1) ;
+ getPlaneInfo(oN, c2Mo, N2, d2) ;
+
+ vpMatrix L(2,3), Lp ;
+ int k =0 ;
+ for (unsigned int i=0 ; i < nbpoint ; i++)
+ {
+ p2[0] = c2P[i].get_x() ;
+ p2[1] = c2P[i].get_y() ;
+ p2[2] = 1.0 ;
+ p1[0] = c1P[i].get_x() ;
+ p1[1] = c1P[i].get_y() ;
+ p1[2] = 1.0 ;
+
+ vpMatrix H(3,3) ;
+
+ Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
+ Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
+
+ Hp2 /= Hp2[2] ; // normalisation
+ Hp1 /= Hp1[2] ;
+
+
+ // set up the interaction matrix
+ double x = Hp2[0] ;
+ double y = Hp2[1] ;
+ double Z1 ;
+
+ Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
+
+
+ H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
+ H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
+ H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
+ H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
+ H2 *=-1 ;
+
+ vpMatrix c1CFc2(6,6) ;
+ {
+ vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
+ for (unsigned int k_=0 ; k_ < 3 ; k_++)
+ for (unsigned int l=0 ; l<3 ; l++)
+ {
+ c1CFc2[k_][l] = c1Rc2[k_][l] ;
+ c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
+ c1CFc2[k_][l+3] = sTR[k_][l] ;
+ }
+ }
+ H2 = H2*c1CFc2 ;
+
+ // Set up the error vector
+ e2[0] = Hp2[0] - c1P[i].get_x() ;
+ e2[1] = Hp2[1] - c1P[i].get_y() ;
+
+ x = Hp1[0] ;
+ y = Hp1[1] ;
+
+ Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
+
+ H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
+ H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
+ H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
+ H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
+
+ // Set up the error vector
+ e1[0] = Hp1[0] - c2P[i].get_x() ;
+ e1[1] = Hp1[1] - c2P[i].get_y() ;
+
+ if (only_2==1)
+ {
+ if (k == 0) { L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ }
+ else
+ if (only_1==1)
+ {
+ if (k == 0) { L = H1 ; e= e1 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+ }
+ else
+ {
+ if (k == 0) {L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+
+
+ k++ ;
+ }
+
+ if (userobust)
+ {
+ robust.setIteration(0);
+ for (unsigned int l=0 ; l < n ; l++)
+ {
+ res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
+ }
+ robust.MEstimator(vpRobust::TUKEY, res, w);
+
+ // compute the pseudo inverse of the interaction matrix
+ for (unsigned int l=0 ; l < n ; l++)
+ {
+ W[2*l][2*l] = w[l] ;
+ W[2*l+1][2*l+1] = w[l] ;
+ }
+ }
+ else
+ {
+ for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
+ }
+ (W*L).pseudoInverse(Lp, 1e-16) ;
+ // Compute the camera velocity
+ vpColVector c2Tcc1 ;
+
+ c2Tcc1 = -1*Lp*W*e ;
+
+ // only for simulation
+
+ c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
+ // UpdatePose2(c2Tcc1, c2Mc1) ;
+ r =(W*e).sumSquare() ;
+
+ if (r < 1e-15) {break ; }
+ if (iter>1000){break ; }
+ if (r>r_1) { break ; }
+ iter++ ;
+ }
+
+ return (W*e).sumSquare() ;
+
+}
+
+
+double
+vpHomography::computeDisplacement(unsigned int nbpoint,
+ vpPoint *c1P,
+ vpPoint *c2P,
+ vpPlane *oN,
+ vpHomogeneousMatrix &c2Mc1,
+ vpHomogeneousMatrix &c1Mo,
+ int userobust
+ )
+{
+
+
+ vpColVector e(2) ;
+ double r_1 = -1 ;
+
+ vpColVector p2(3) ;
+ vpColVector p1(3) ;
+ vpColVector Hp2(3) ;
+ vpColVector Hp1(3) ;
+
+ vpMatrix H2(2,6) ;
+ vpColVector e2(2) ;
+ vpMatrix H1(2,6) ;
+ vpColVector e1(2) ;
+
+
+ int only_1;
+ int only_2;
+ int iter = 0 ;
+ unsigned int i ;
+ unsigned int n=0 ;
+ only_1 = 1 ;
+ only_2 = 0 ;
+ n = nbpoint ;
+
+ // Remove next 2 lines, since when only_1 is initialized to 1 (as it is), we cannot reach the code
+ //if ( (! only_1) && (! only_2) )
+ // n *=2 ;
+
+ vpRobust robust(n);
+ vpColVector res(n) ;
+ vpColVector w(n) ;
+ w =1 ;
+ robust.setThreshold(0.0000) ;
+ vpMatrix W(2*n,2*n) ;
+ W = 0 ;
+
+ vpColVector N1(3), N2(3) ;
+ double d1, d2 ;
+
+ double r =1e10 ;
+ iter =0 ;
+ while (vpMath::equal(r_1,r,threshold_displacement) == false )
+ {
+
+ r_1 =r ;
+ // compute current position
+
+
+ //Change frame (current)
+ vpHomogeneousMatrix c1Mc2, c2Mo ;
+ vpRotationMatrix c1Rc2, c2Rc1 ;
+ vpTranslationVector c1Tc2, c2Tc1 ;
+ c1Mc2 = c2Mc1.inverse() ;
+ c2Mc1.extract(c2Rc1) ;
+ c2Mc1.extract(c2Tc1) ;
+ c2Mc1.extract(c1Rc2) ;
+ c1Mc2.extract(c1Tc2) ;
+
+ c2Mo = c2Mc1*c1Mo ;
+
+
+
+ vpMatrix L(2,3), Lp ;
+ int k =0 ;
+ for (i=0 ; i < nbpoint ; i++)
+ {
+ getPlaneInfo(oN[i], c1Mo, N1, d1) ;
+ getPlaneInfo(oN[i], c2Mo, N2, d2) ;
+ p2[0] = c2P[i].get_x() ;
+ p2[1] = c2P[i].get_y() ;
+ p2[2] = 1.0 ;
+ p1[0] = c1P[i].get_x() ;
+ p1[1] = c1P[i].get_y() ;
+ p1[2] = 1.0 ;
+
+ vpMatrix H(3,3) ;
+
+ Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
+ Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
+
+ Hp2 /= Hp2[2] ; // normalisation
+ Hp1 /= Hp1[2] ;
+
+
+ // set up the interaction matrix
+ double x = Hp2[0] ;
+ double y = Hp2[1] ;
+ double Z1 ;
+
+ Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
+
+
+ H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
+ H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
+ H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
+ H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
+ H2 *=-1 ;
+
+ vpMatrix c1CFc2(6,6) ;
+ {
+ vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
+ for (unsigned int k_=0 ; k_ < 3 ; k_++)
+ for (unsigned int l=0 ; l<3 ; l++)
+ {
+ c1CFc2[k_][l] = c1Rc2[k_][l] ;
+ c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
+ c1CFc2[k_][l+3] = sTR[k_][l] ;
+ }
+ }
+ H2 = H2*c1CFc2 ;
+
+ // Set up the error vector
+ e2[0] = Hp2[0] - c1P[i].get_x() ;
+ e2[1] = Hp2[1] - c1P[i].get_y() ;
+
+ x = Hp1[0] ;
+ y = Hp1[1] ;
+
+ Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
+
+ H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
+ H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
+ H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
+ H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
+
+ // Set up the error vector
+ e1[0] = Hp1[0] - c2P[i].get_x() ;
+ e1[1] = Hp1[1] - c2P[i].get_y() ;
+
+
+ if (only_2==1)
+ {
+ if (k == 0) { L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ }
+ else
+ if (only_1==1)
+ {
+ if (k == 0) { L = H1 ; e= e1 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+ }
+ else
+ {
+ if (k == 0) {L = H2 ; e = e2 ; }
+ else
+ {
+ L = vpMatrix::stack(L,H2) ;
+ e = vpColVector::stack(e,e2) ;
+ }
+ L = vpMatrix::stack(L,H1) ;
+ e = vpColVector::stack(e,e1) ;
+ }
+
+
+ k++ ;
+ }
+
+ if (userobust)
+ {
+ robust.setIteration(0);
+ for (unsigned int k_=0 ; k_ < n ; k_++)
+ {
+ res[k_] = vpMath::sqr(e[2*k_]) + vpMath::sqr(e[2*k_+1]) ;
+ }
+ robust.MEstimator(vpRobust::TUKEY, res, w);
+
+
+ // compute the pseudo inverse of the interaction matrix
+ for (unsigned int k_=0 ; k_ < n ; k_++)
+ {
+ W[2*k_][2*k_] = w[k_] ;
+ W[2*k_+1][2*k_+1] = w[k_] ;
+ }
+ }
+ else
+ {
+ for (unsigned int k_=0 ; k_ < 2*n ; k_++) W[k_][k_] = 1 ;
+ }
+ (W*L).pseudoInverse(Lp, 1e-16) ;
+ // Compute the camera velocity
+ vpColVector c2Tcc1 ;
+
+ c2Tcc1 = -1*Lp*W*e ;
+
+ // only for simulation
+
+ c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
+ // UpdatePose2(c2Tcc1, c2Mc1) ;
+ r =(W*e).sumSquare() ;
+
+
+
+ if (r < 1e-15) {break ; }
+ if (iter>1000){break ; }
+ if (r>r_1) { break ; }
+ iter++ ;
+ }
+
+ return (W*e).sumSquare() ;
+
+}
+
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/vision/src/key-point/vpBasicKeyPoint.cpp b/modules/vision/src/key-point/vpBasicKeyPoint.cpp
new file mode 100644
index 0000000..b5660ee
--- /dev/null
+++ b/modules/vision/src/key-point/vpBasicKeyPoint.cpp
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#include <visp3/vision/vpBasicKeyPoint.h>
+
+/*!
+ 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
new file mode 100644
index 0000000..4c348a2
--- /dev/null
+++ b/modules/vision/src/key-point/vpFernClassifier.cpp
@@ -0,0 +1,583 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Class that implements the Fern classifier and the YAPE detector thanks
+ * to the OpenCV library.
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+
+#include <visp3/vision/vpFernClassifier.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
+
+/*!
+ Basic constructor
+
+*/
+vpFernClassifier::vpFernClassifier()
+ : vpBasicKeyPoint(),
+ ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
+ hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
+ nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
+ sigmaBlur(1), nbMinPoint(10),
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ curImg(),
+ #else
+ curImg(NULL),
+ #endif
+ objKeypoints(), modelROI_Ref(), modelROI(),
+ modelPoints(), imgKeypoints(), refPt(), curPt()
+{
+}
+
+/*!
+ Constructor using the provided data file to load the object given by its name.
+
+ If the name of the object does not correspond in the file, a
+ vpException::ioError is thrown.
+
+ \param _dataFile : the name of the file saved after a training (with the
+ record methods).
+ \param _objectName : the name of the object to load
+*/
+vpFernClassifier::vpFernClassifier(const std::string& _dataFile, const std::string& _objectName)
+ : vpBasicKeyPoint(),
+ ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
+ hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
+ nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
+ sigmaBlur(1), nbMinPoint(10),
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ curImg(),
+ #else
+ curImg(NULL),
+ #endif
+ objKeypoints(), modelROI_Ref(), modelROI(),
+ modelPoints(), imgKeypoints(), refPt(), curPt()
+{
+ this->load(_dataFile, _objectName);
+}
+
+/*!
+ Basic destructor
+
+*/
+vpFernClassifier::~vpFernClassifier()
+{
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ if(curImg != NULL){
+ if(curImg->width % 8 == 0){
+ curImg->imageData = NULL;
+ cvReleaseImageHeader(&curImg);
+ }else{
+ cvReleaseImage(&curImg);
+ }
+ curImg = NULL;
+ }
+#endif
+}
+
+/*!
+ initialise any OpenCV parameters
+
+ The classifier need to be trained again or to be loaded from an external file.
+*/
+void
+vpFernClassifier::init()
+{
+ hasLearn = false;
+ nbClassfier = 100;
+ ClassifierSize = 11;
+ nbPoints = 200;
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ curImg = NULL;
+#endif
+ blurImage = true;
+ radiusBlur = 7;
+ sigmaBlur = 1;
+ patchSize = 32;
+ radius = 7;
+ threshold = 20;
+ nbOctave = 2;
+ nbView = 2000;
+ dist = 2;
+ nbMinPoint = 10;
+}
+
+
+
+
+/*!
+ Train the classifier.
+*/
+void
+vpFernClassifier::train()
+{
+ // initialise detector
+ cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
+
+ //blur
+ cv::Mat obj = (cv::Mat)curImg;
+
+ if(this->getBlurSetting()){
+ cv::GaussianBlur(obj, obj, cv::Size(getBlurSize(), getBlurSize()), getBlurSigma(), getBlurSigma());
+ }
+
+ // build pyramid
+ std::vector<cv::Mat> objpyr;
+ cv::buildPyramid(obj, objpyr, d.nOctaves-1);
+
+ // getPoints
+ d.getMostStable2D(obj, objKeypoints, 100, gen);
+
+ ldetector = d;
+
+ // train classifier
+ modelROI = cv::Rect(0, 0, objpyr[0].cols, objpyr[0].rows);
+ ldetector.getMostStable2D(objpyr[0], modelPoints, 100, gen);
+
+ fernClassifier.trainFromSingleView(objpyr[0], modelPoints,
+ patchSize, (int)modelPoints.size(), 100, 11, 10000,
+ cv::FernClassifier::COMPRESSION_NONE, gen);
+
+ /* from OpenCV format to ViSP format */
+ referenceImagePointsList.resize(0);
+ for (unsigned int i = 0; i < modelPoints.size(); i += 1){
+ vpImagePoint ip(
+ modelPoints[i].pt.y + modelROI_Ref.y,
+ modelPoints[i].pt.x + modelROI_Ref.x);
+ referenceImagePointsList.push_back(ip);
+ }
+
+ // set flag
+ hasLearn = true;
+}
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made all over the image I. It also includes the training of the fern
+ classifier.
+
+ \warning thie method can take up to several minutes depending on the
+ parameters of the classifier and on the size of the image.
+
+ \param _I : The gray scaled image where the reference points are computed.
+
+ \return the number of reference points.
+*/
+unsigned int
+vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
+{
+ this->setImage(_I);
+
+ train();
+
+ _reference_computed = true;
+ return (unsigned int)objKeypoints.size();
+}
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made only on a part of the image. This part is a rectangle defined
+ by its top left corner, its height and its width. The parameters of
+ this rectangle must be given in pixel. It also includes the training of the
+ fern classifier.
+
+ \warning the method can take up to several minutes depending on the
+ parameters of the classifier and on the size of the image.
+
+ \param _I : The gray scaled image where the reference points are computed.
+ \param _iP : The top left corner of the rectangle.
+ \param _height : height of the rectangle (in pixel).
+ \param _width : width of the rectangle (in pixel).
+
+ \return the number of reference points.
+*/
+unsigned int
+vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
+ const vpImagePoint &_iP,
+ const unsigned int _height, const unsigned int _width)
+{
+ if((_iP.get_i()+_height) >= _I.getHeight()
+ || (_iP.get_j()+_width) >= _I.getWidth()) {
+ vpTRACE("Bad size for the subimage");
+ throw(vpException(vpImageException::notInTheImage ,
+ "Bad size for the subimage"));
+ }
+
+ vpImage<unsigned char> subImage;
+ vpImageTools::createSubImage(_I,
+ (unsigned int)_iP.get_i(),
+ (unsigned int)_iP.get_j(),
+ _height, _width, subImage);
+ this->setImage(subImage);
+
+ /* initialise a structure containing the region of interest used in the
+ reference image */
+ modelROI_Ref.x = (int)_iP.get_u();
+ modelROI_Ref.y = (int)_iP.get_v();
+ modelROI_Ref.width = (int)_width;
+ modelROI_Ref.height = (int)_height;
+
+ train();
+
+ return (unsigned int)objKeypoints.size();
+}
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made only on a part of the image. This part is a rectangle. The
+ parameters of this rectangle must be given in pixel. It also includes the
+ training of the fern classifier.
+
+ \warning the method can take up to several minutes depending on the
+ parameters of the classifier and on the size of the image.
+
+ \param _I : The gray scaled image where the reference points are computed.
+ \param _rectangle : The rectangle which defines the interesting part
+ of the image.
+
+ \return The number of reference points.
+*/
+unsigned int
+vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
+ const vpRect& _rectangle)
+{
+ vpImagePoint iP;
+ iP.set_i(_rectangle.getTop());
+ iP.set_j(_rectangle.getLeft());
+ return (this->buildReference(_I, iP,
+ (unsigned int)_rectangle.getHeight(),
+ (unsigned int)_rectangle.getWidth()));
+}
+
+/*!
+ Compute the points of interest in the current image and try to match them
+ using the trained classifier. The matched pairs can be found with the
+ getMatchedPointByRef function.
+
+ \exception vpException::notInitialised if the classifier has not been trained.
+
+ \param _I : The gray scaled image where the points are computed.
+
+ \return The number of point which have been matched.
+*/
+unsigned int
+vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
+{
+ if(!hasLearn){
+ vpERROR_TRACE("The object has not been learned. ");
+ throw vpException(vpException::notInitialized , "object is not learned, load database or build the reference ");
+ }
+
+ setImage(_I);
+ // resize image
+// cv::resize(_image, image, Size(), 1./imgscale, 1./imgscale, INTER_CUBIC);
+ cv::Mat img = this->curImg;
+
+ if(this->getBlurSetting()){
+ cv::GaussianBlur(img, img, cv::Size(this->getBlurSize(), this->getBlurSize()), this->getBlurSigma(), this->getBlurSigma());
+ }
+
+ std::vector<cv::Mat> imgPyr;
+ cv::buildPyramid(img, imgPyr, ldetector.nOctaves-1);
+
+
+ ldetector(imgPyr, imgKeypoints, 500);
+
+ unsigned int m = (unsigned int)modelPoints.size();
+ unsigned int n = (unsigned int)imgKeypoints.size();
+ std::vector<int> bestMatches(m, -1);
+ std::vector<float> maxLogProb(m, -FLT_MAX);
+ std::vector<float> signature;
+ unsigned int totalMatch = 0;
+
+
+ /* part of code from OpenCV planarObjectDetector */
+ currentImagePointsList.resize(0);
+ matchedReferencePoints.resize(0);
+
+ for(unsigned int i = 0; i < n; i++ ){
+ cv::KeyPoint kpt = imgKeypoints[i];
+ kpt.pt.x /= (float)(1 << kpt.octave);
+ kpt.pt.y /= (float)(1 << kpt.octave);
+ int k = fernClassifier(imgPyr[(unsigned int)kpt.octave], kpt.pt, signature);
+ if( k >= 0 && (bestMatches[(unsigned int)k] < 0 || signature[(unsigned int)k] > maxLogProb[(unsigned int)k]) ){
+ maxLogProb[(unsigned int)k] = signature[(unsigned int)k];
+ bestMatches[(unsigned int)k] = (int)i;
+ totalMatch++;
+
+ vpImagePoint ip_cur( imgKeypoints[i].pt.y, imgKeypoints[i].pt.x);
+
+ currentImagePointsList.push_back(ip_cur);
+ matchedReferencePoints.push_back((unsigned int)k);
+
+ }
+ }
+
+ refPt.resize(0);
+ curPt.resize(0);
+ for(unsigned int i = 0; i < m; i++ ){
+ if( bestMatches[i] >= 0 ){
+ refPt.push_back(modelPoints[i].pt);
+ curPt.push_back(imgKeypoints[(unsigned int)bestMatches[i]].pt);
+ }
+ }
+
+ return totalMatch;
+}
+
+
+
+/*!
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs can
+ be found with the getMatchedPointByRef function. The homography between the
+ two planar surfaces is also computed.
+
+ \param _I : The gray scaled image where the points are computed.
+ \param _iP : the top left corner of the rectangle defining the ROI
+ \param _height : the height of the ROI
+ \param _width : the width of the ROI
+
+ \return the number of point which have been matched.
+*/
+unsigned int
+vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
+ const vpImagePoint &_iP,
+ const unsigned int _height, const unsigned int _width)
+{
+ if((_iP.get_i()+_height) >= _I.getHeight()
+ || (_iP.get_j()+_width) >= _I.getWidth()) {
+ vpTRACE("Bad size for the subimage");
+ throw(vpException(vpImageException::notInTheImage ,
+ "Bad size for the subimage"));
+ }
+
+ vpImage<unsigned char> subImage;
+
+ vpImageTools::createSubImage(_I,
+ (unsigned int)_iP.get_i(),
+ (unsigned int)_iP.get_j(),
+ _height, _width, subImage);
+
+ return this->matchPoint(subImage);
+}
+
+/*!
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs can
+ be found with the getMatchedPointByRef function. The homography between the
+ two planar surfaces is also computed.
+
+ \param _I : The gray scaled image where the points are computed.
+ \param _rectangle : the rectangle defining the ROI
+
+ \return the number of point which have been matched.
+*/
+unsigned int
+vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
+ const vpRect& _rectangle)
+{
+ vpImagePoint iP;
+ iP.set_i(_rectangle.getTop());
+ iP.set_j(_rectangle.getLeft());
+ return (this->matchPoint(_I, iP,
+ (unsigned int)_rectangle.getHeight(),
+ (unsigned int)_rectangle.getWidth()));
+}
+
+
+/*!
+ This function displays the matched reference points and the matched
+ points computed in the current image. The reference points are
+ displayed in the image Ireference and the matched points coming from
+ the current image are displayed in the image Icurrent. It is
+ possible to set Ireference and Icurrent with the same image when
+ calling the method.
+
+ \param _Iref : The image where the matched reference points are
+ displayed.
+
+ \param _Icurrent : The image where the matched points computed in the
+ current image are displayed.
+
+ \param size : Size in pixels of the cross that is used to display matched points.
+*/
+void
+vpFernClassifier::display(const vpImage<unsigned char> &_Iref,
+ const vpImage<unsigned char> &_Icurrent, unsigned int size)
+{
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
+ vpDisplay::displayCross (_Iref, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
+ vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, vpColor::green);
+ }
+
+}
+
+
+/*!
+ This function displays only the matched points computed in the
+ current image. They are displayed in the image Icurrent.
+
+ \param _Icurrent : the gray scaled image for the display
+
+ \param size : Size in pixels of the cross that is used to display matched points.
+
+ \param color : Color used to display the matched points.
+*/
+void
+vpFernClassifier::display(const vpImage<unsigned char> &_Icurrent, unsigned int size,
+ const vpColor &color)
+{
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
+ vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, color);
+ }
+}
+
+/* IO METHODS */
+
+/*!
+ \brief load the Fern classifier
+
+ Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
+ can take up to sevral seconds.
+
+ \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
+*/
+void
+vpFernClassifier::load(const std::string& _dataFile, const std::string& /*_objectName*/)
+{
+ std::cout << " > Load data for the planar object detector..." << std::endl;
+
+ /* part of code from OpenCV planarObjectDetector */
+ cv::FileStorage fs(_dataFile, cv::FileStorage::READ);
+ cv::FileNode node = fs.getFirstTopLevelNode();
+
+ cv::FileNodeIterator it = node["model-roi"].begin(), it_end;
+ it >> modelROI.x >> modelROI.y >> modelROI.width >> modelROI.height;
+
+ ldetector.read(node["detector"]);
+ fernClassifier.read(node["fern-classifier"]);
+
+ const cv::FileNode node_ = node["model-points"];
+ cv::read(node_, modelPoints);
+
+ cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
+ ldetector = d;
+ hasLearn = true;
+}
+
+
+/*!
+ \brief record the Ferns classifier in the text file
+
+ \param _objectName : The name of the object to store in the data file.
+ \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
+*/
+void
+vpFernClassifier::record(const std::string& _objectName, const std::string& _dataFile )
+{
+ /* part of code from OpenCV planarObjectDetector */
+ cv::FileStorage fs(_dataFile, cv::FileStorage::WRITE);
+
+ cv::WriteStructContext ws(fs, _objectName, CV_NODE_MAP);
+
+ {
+ cv::WriteStructContext wsroi(fs, "model-roi", CV_NODE_SEQ + CV_NODE_FLOW);
+ cv::write(fs, modelROI_Ref.x);
+ cv::write(fs, modelROI_Ref.y);
+ cv::write(fs, modelROI_Ref.width);
+ cv::write(fs, modelROI_Ref.height);
+ }
+
+ ldetector.write(fs, "detector");
+ cv::write(fs, "model-points", modelPoints);
+ fernClassifier.write(fs, "fern-classifier");
+}
+
+
+
+/*!
+ Set the current image. This method allows to convert a image from the ViSP
+ format to the OpenCV one.
+
+ \param I : the input image.
+*/
+void
+vpFernClassifier::setImage(const vpImage<unsigned char>& I)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ vpImageConvert::convert(I, curImg);
+#else
+ if(curImg != NULL){
+ cvResetImageROI(curImg);
+ if((curImg->width % 8) == 0){
+ curImg->imageData = NULL;
+ cvReleaseImageHeader(&curImg);
+ }else{
+ cvReleaseImage(&curImg);
+ }
+ curImg = NULL;
+ }
+ if((I.getWidth()%8) == 0){
+ curImg = cvCreateImageHeader(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
+ if(curImg != NULL){
+ curImg->imageData = (char*)I.bitmap;
+ }else{
+ throw vpException(vpException::memoryAllocationError, "Could not create the image in the OpenCV format.");
+ }
+ }else{
+ vpImageConvert::convert(I, curImg);
+ }
+ if(curImg == NULL){
+ std::cout << "!> conversion failed" << std::endl;
+ throw vpException(vpException::notInitialized , "conversion failed");
+ }
+#endif
+
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_vision.a(vpFernClassifier.cpp.o) has no symbols
+void dummy_vpFernClassifier() {};
+#endif
+
diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp
new file mode 100644
index 0000000..03386ac
--- /dev/null
+++ b/modules/vision/src/key-point/vpKeyPoint.cpp
@@ -0,0 +1,3928 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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 functionalities.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <limits>
+#include <iomanip>
+#include <stdint.h> //uint32_t ; works also with >= VS2010 / _MSC_VER >= 1600
+
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpIoTools.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+# include <opencv2/calib3d/calib3d.hpp>
+#endif
+
+
+//TODO: test saveLearningData with little / big endian platform
+//Specific Type transformation functions
+///*!
+// Convert a list of cv::DMatch to a cv::DMatch (extract the first cv::DMatch, the nearest neighbor).
+//
+// \param knnMatches : List of cv::DMatch.
+// \return The nearest neighbor.
+// */
+inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches) {
+ if(knnMatches.size() > 0) {
+ return knnMatches[0];
+ }
+
+ return cv::DMatch();
+}
+
+///*!
+// Convert a cv::DMatch to an index (extract the train index).
+//
+// \param match : Point to convert in ViSP type.
+// \return The train index.
+// */
+inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
+ return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
+}
+
+bool isBigEndian() {
+ union {
+ uint32_t i;
+ char c[4];
+ } bint = { 0x01020304 };
+
+ return bint.c[0] == 1;
+}
+
+uint16_t reverse16bits(const uint16_t n) {
+ unsigned char *np = (unsigned char *) &n;
+
+ return ((uint16_t) np[0] << 8) | (uint16_t) np[1];
+}
+
+uint32_t reverse32bits(const uint32_t n) {
+ unsigned char *np = (unsigned char *) &n;
+
+ return ((uint32_t) np[0] << 24) | ((uint32_t) np[1] << 16)
+ | ((uint32_t) np[2] << 8) | (uint32_t) np[3];
+}
+
+float reverseFloat(const float f) {
+ union {
+ float f;
+ unsigned char b[4];
+ } dat1, dat2;
+
+ dat1.f = f;
+ dat2.b[0] = dat1.b[3];
+ dat2.b[1] = dat1.b[2];
+ dat2.b[2] = dat1.b[1];
+ dat2.b[3] = dat1.b[0];
+ return dat2.f;
+}
+
+double reverseDouble(const double d) {
+ union {
+ double d;
+ unsigned char b[8];
+ } dat1, dat2;
+
+ dat1.d = d;
+ dat2.b[0] = dat1.b[7];
+ dat2.b[1] = dat1.b[6];
+ dat2.b[2] = dat1.b[5];
+ dat2.b[3] = dat1.b[4];
+ dat2.b[4] = dat1.b[3];
+ dat2.b[5] = dat1.b[2];
+ dat2.b[6] = dat1.b[1];
+ dat2.b[7] = dat1.b[0];
+ return dat2.d;
+}
+
+void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ uint16_t reverse_ushort = reverse16bits(ushort_value);
+ file.write((char *)(&reverse_ushort), sizeof(reverse_ushort));
+ } else {
+ file.write((char *)(&ushort_value), sizeof(ushort_value));
+ }
+}
+
+void writeBinaryShortLE(std::ofstream &file, const short short_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ uint16_t reverse_short = reverse16bits((uint16_t) short_value);
+ file.write((char *)(&reverse_short), sizeof(reverse_short));
+ } else {
+ file.write((char *)(&short_value), sizeof(short_value));
+ }
+}
+
+void writeBinaryUIntLE(std::ofstream &file, const unsigned int uint_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ if(sizeof(uint_value) == 4) {
+ uint32_t reverse_uint = reverse32bits(uint_value);
+ file.write((char *)(&reverse_uint), sizeof(reverse_uint));
+ } else {
+ uint16_t reverse_uint = reverse16bits(uint_value);
+ file.write((char *)(&reverse_uint), sizeof(reverse_uint));
+ }
+ } else {
+ file.write((char *)(&uint_value), sizeof(uint_value));
+ }
+}
+
+void writeBinaryIntLE(std::ofstream &file, const int int_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ if(sizeof(int_value) == 4) {
+ uint32_t reverse_int = reverse32bits((uint32_t) int_value);
+ file.write((char *)(&reverse_int), sizeof(reverse_int));
+ } else {
+ uint16_t reverse_int = reverse16bits((uint16_t) int_value);
+ file.write((char *)(&reverse_int), sizeof(reverse_int));
+ }
+ } else {
+ file.write((char *)(&int_value), sizeof(int_value));
+ }
+}
+
+void writeBinaryFloatLE(std::ofstream &file, const float float_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ float reverse_float = reverseFloat(float_value);
+ file.write((char *)(&reverse_float), sizeof(reverse_float));
+ } else {
+ file.write((char *)(&float_value), sizeof(float_value));
+ }
+}
+
+void writeBinaryDoubleLE(std::ofstream &file, const double double_value) {
+ if(isBigEndian()) {
+ //Reverse bytes order to little endian
+ double reverse_double = reverseDouble(double_value);
+ file.write((char *)(&reverse_double), sizeof(reverse_double));
+ } else {
+ file.write((char *)(&double_value), sizeof(double_value));
+ }
+}
+
+/*!
+ Constructor to initialize specified detector, extractor, matcher and filtering method.
+
+ \param detectorName : Name of the detector.
+ \param extractorName : Name of the extractor.
+ \param matcherName : Name of the matcher.
+ \param filterType : Filtering matching method chosen.
+ */
+vpKeyPoint::vpKeyPoint(const std::string &detectorName, const std::string &extractorName,
+ const std::string &matcherName, const vpFilterMatchingType &filterType)
+ : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
+ m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(),
+ m_detectors(), m_extractionTime(0.), m_extractorNames(), m_extractors(), m_filteredMatches(), m_filterType(filterType),
+ m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
+ m_matcher(), m_matcherName(matcherName),
+ m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
+ m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
+ m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
+ m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
+ m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
+ m_trainVpPoints(), m_useAffineDetection(false),
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ m_useBruteForceCrossCheck(true),
+ #endif
+ m_useConsensusPercentage(false),
+ m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
+{
+ //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
+ //So this is useful only for ratioDistanceThreshold method
+ if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
+ m_useKnn = true;
+ }
+
+ m_detectorNames.push_back(detectorName);
+ m_extractorNames.push_back(extractorName);
+
+ init();
+}
+
+/*!
+ Constructor to initialize specified detector, extractor, matcher and filtering method.
+
+ \param detectorNames : List of name detector for allowing multiple detectors.
+ \param extractorNames : List of name extractor for allowing multiple extractors.
+ \param matcherName : Name of the matcher.
+ \param filterType : Filtering matching method chosen.
+ */
+vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
+ const std::string &matcherName, const vpFilterMatchingType &filterType)
+ : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
+ m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(detectorNames),
+ m_detectors(), m_extractionTime(0.), m_extractorNames(extractorNames), m_extractors(), m_filteredMatches(),
+ m_filterType(filterType), m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
+ m_matcher(),
+ m_matcherName(matcherName), m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
+ m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
+ m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
+ m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
+ m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
+ m_trainVpPoints(), m_useAffineDetection(false),
+ #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ m_useBruteForceCrossCheck(true),
+ #endif
+ m_useConsensusPercentage(false),
+ m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
+{
+ //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
+ //So this is useful only for ratioDistanceThreshold method
+ if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
+ m_useKnn = true;
+ }
+
+ init();
+}
+
+/*!
+ Apply an affine and skew transformation to an image.
+ \param tilt : Tilt value in the direction of x
+ \param phi : Rotation value
+ \param img : Modified image after the transformation
+ \param mask : Mask containing the location of the image pixels after the transformation
+ \param Ai : Inverse affine matrix
+ */
+void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
+ cv::Mat& mask, cv::Mat& Ai) {
+ int h = img.rows;
+ int w = img.cols;
+
+ mask = cv::Mat(h, w, CV_8UC1, cv::Scalar(255));
+
+ cv::Mat A = cv::Mat::eye(2, 3, CV_32F);
+
+ //if (phi != 0.0) {
+ if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
+ phi *= M_PI / 180.;
+ double s = sin(phi);
+ double c = cos(phi);
+
+ A = (cv::Mat_<float>(2, 2) << c, -s, s, c);
+
+ cv::Mat corners = (cv::Mat_<float>(4, 2) << 0, 0, w, 0, w, h, 0, h);
+ cv::Mat tcorners = corners * A.t();
+ cv::Mat tcorners_x, tcorners_y;
+ tcorners.col(0).copyTo(tcorners_x);
+ tcorners.col(1).copyTo(tcorners_y);
+ std::vector<cv::Mat> channels;
+ channels.push_back(tcorners_x);
+ channels.push_back(tcorners_y);
+ cv::merge(channels, tcorners);
+
+ cv::Rect rect = cv::boundingRect(tcorners);
+ A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);
+
+ cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
+ cv::INTER_LINEAR, cv::BORDER_REPLICATE);
+ }
+ //if (tilt != 1.0) {
+ if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
+ double s = 0.8 * sqrt(tilt * tilt - 1);
+ cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
+ cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
+ A.row(0) = A.row(0) / tilt;
+ }
+ //if (tilt != 1.0 || phi != 0.0) {
+ if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
+ h = img.rows;
+ w = img.cols;
+ cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
+ }
+ cv::invertAffineTransform(A, Ai);
+}
+
+/*!
+ Build the reference keypoints list.
+
+ \param I : Input reference image.
+ \return The number of detected keypoints in the image \p I.
+ */
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I) {
+ return buildReference(I, vpRect());
+}
+
+/*!
+ Build the reference keypoints list in a region of interest in the image.
+
+ \param I : Input reference image
+ \param iP : Position of the top-left corner of the region of interest.
+ \param height : Height of the region of interest.
+ \param width : Width of the region of interest.
+ \return The number of detected keypoints in the current image I.
+ */
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width) {
+
+ return buildReference(I, vpRect(iP, width, height));
+}
+
+/*!
+ Build the reference keypoints list in a region of interest in the image.
+
+ \param I : Input image.
+ \param rectangle : Rectangle of the region of interest.
+ \return The number of detected keypoints in the current image I.
+ */
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
+ const vpRect &rectangle) {
+ //Reset variables used when dealing with 3D models
+ //So as no 3D point list is passed, we dont need this variables
+ m_trainPoints.clear();
+ m_mapOfImageId.clear();
+ m_mapOfImages.clear();
+ m_currentImageId = 1;
+
+ if(m_useAffineDetection) {
+ std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
+ std::vector<cv::Mat> listOfTrainDescriptors;
+
+ //Detect keypoints and extract descriptors on multiple images
+ detectExtractAffine(I, listOfTrainKeyPoints, listOfTrainDescriptors);
+
+ //Flatten the different train lists
+ m_trainKeyPoints.clear();
+ for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
+ it != listOfTrainKeyPoints.end(); ++it) {
+ m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
+ }
+
+ bool first = true;
+ for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
+ if(first) {
+ first = false;
+ it->copyTo(m_trainDescriptors);
+ } else {
+ m_trainDescriptors.push_back(*it);
+ }
+ }
+ } else {
+ detect(I, m_trainKeyPoints, m_detectionTime, rectangle);
+ extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
+ }
+
+ //Save the correspondence keypoint class_id with the training image_id in a map
+ //Used to display the matching with all the training images
+ for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
+ m_mapOfImageId[it->class_id] = m_currentImageId;
+ }
+
+ //Save the image in a map at a specific image_id
+ m_mapOfImages[m_currentImageId] = I;
+
+ //Convert OpenCV type to ViSP type for compatibility
+ vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
+
+ _reference_computed = true;
+
+ //Add train descriptors in matcher object
+ m_matcher->clear();
+ m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
+
+ return static_cast<unsigned int>(m_trainKeyPoints.size());
+}
+
+/*!
+ Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
+
+ \param I : Input image
+ \param trainKeyPoints : List of the train keypoints.
+ \param points3f : Output list of the 3D position corresponding of the keypoints locations.
+ \param append : If true, append the supply train keypoints with those already present.
+ \param class_id : The class id to be set to the input cv::KeyPoint if != -1.
+ */
+void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
+ std::vector<cv::Point3f> &points3f, const bool append, const int class_id) {
+ cv::Mat trainDescriptors;
+ //Copy the input list of keypoints
+ std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
+
+ extract(I, trainKeyPoints, trainDescriptors, m_extractionTime, &points3f);
+
+ if(trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
+ //Keypoints have been removed
+ //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ std::map<size_t, size_t> mapOfKeypointHashes;
+ size_t cpt = 0;
+ for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it, cpt++) {
+ mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
+ }
+
+ std::vector<cv::Point3f> trainPoints_tmp;
+ for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
+ if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]);
+ }
+ }
+
+ //Copy trainPoints_tmp to points3f
+ points3f = trainPoints_tmp;
+ }
+
+ buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, class_id);
+}
+
+/*!
+ Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
+
+ \param I : Input image
+ \param trainKeyPoints : List of the train keypoints.
+ \param points3f : List of the 3D position corresponding of the keypoints locations.
+ \param trainDescriptors : List of the train descriptors.
+ \param append : If true, append the supply train keypoints with those already present.
+ \param class_id : The class id to be set to the input cv::KeyPoint if != -1.
+ */
+void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
+ const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
+ const bool append, const int class_id) {
+ if(!append) {
+ m_currentImageId = 0;
+ m_mapOfImageId.clear();
+ m_mapOfImages.clear();
+ this->m_trainKeyPoints.clear();
+ this->m_trainPoints.clear();
+ }
+
+ m_currentImageId++;
+
+ std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
+ //Set class_id if != -1
+ if(class_id != -1) {
+ for(std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
+ it->class_id = class_id;
+ }
+ }
+
+ //Save the correspondence keypoint class_id with the training image_id in a map
+ //Used to display the matching with all the training images
+ for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
+ m_mapOfImageId[it->class_id] = m_currentImageId;
+ }
+
+ //Save the image in a map at a specific image_id
+ m_mapOfImages[m_currentImageId] = I;
+
+ //Append reference lists
+ this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints_tmp.begin(), trainKeyPoints_tmp.end());
+ if(!append) {
+ trainDescriptors.copyTo(this->m_trainDescriptors);
+ } else {
+ this->m_trainDescriptors.push_back(trainDescriptors);
+ }
+ this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());
+
+
+ //Convert OpenCV type to ViSP type for compatibility
+ vpConvert::convertFromOpenCV(this->m_trainKeyPoints, referenceImagePointsList);
+ vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
+
+ //Add train descriptors in matcher object
+ m_matcher->clear();
+ m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
+
+ _reference_computed = true;
+}
+
+/*!
+ Compute the 3D coordinate given the 2D image coordinate and under the assumption that the point is located on a plane
+ whose the plane equation is known in the camera frame.
+ The Z-coordinate is retrieved according to the proportional relation between the plane equation expressed in the
+ normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
+
+ \param candidate : Keypoint we want to compute the 3D coordinate.
+ \param roi : List of 3D points representing a planar face.
+ \param cam : Camera parameters.
+ \param cMo : Homogeneous matrix between the world and the camera frames.
+ \param point : 3D coordinate computed.
+ */
+void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) {
+ /* compute plane equation */
+ std::vector<vpPoint>::const_iterator it_roi = roi.begin();
+ vpPoint pts[3];
+ pts[0] = *it_roi;
+ ++it_roi;
+ pts[1] = *it_roi;
+ ++it_roi;
+ pts[2] = *it_roi;
+ vpPlane Po(pts[0], pts[1], pts[2]);
+ double xc = 0.0, yc = 0.0;
+ vpPixelMeterConversion::convertPoint(cam, candidate.pt.x, candidate.pt.y, xc, yc);
+ double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
+ double X = xc * Z;
+ double Y = yc * Z;
+ vpColVector point_cam(4);
+ point_cam[0] = X;
+ point_cam[1] = Y;
+ point_cam[2] = Z;
+ point_cam[3] = 1;
+ vpColVector point_obj(4);
+ point_obj = cMo.inverse() * point_cam;
+ point = cv::Point3f((float) point_obj[0], (float) point_obj[1], (float) point_obj[2]);
+}
+
+/*!
+ Compute the 3D coordinate given the 2D image coordinate and under the assumption that the point is located on a plane
+ whose the plane equation is known in the camera frame.
+ The Z-coordinate is retrieved according to the proportional relation between the plane equation expressed in the
+ normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
+
+ \param candidate : vpImagePoint we want to compute the 3D coordinate.
+ \param roi : List of 3D points representing a planar face.
+ \param cam : Camera parameters.
+ \param cMo : Homogeneous matrix between the world and the camera frames.
+ \param point : 3D coordinate computed.
+ */
+void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) {
+ /* compute plane equation */
+ std::vector<vpPoint>::const_iterator it_roi = roi.begin();
+ vpPoint pts[3];
+ pts[0] = *it_roi;
+ ++it_roi;
+ pts[1] = *it_roi;
+ ++it_roi;
+ pts[2] = *it_roi;
+ vpPlane Po(pts[0], pts[1], pts[2]);
+ double xc = 0.0, yc = 0.0;
+ vpPixelMeterConversion::convertPoint(cam, candidate, xc, yc);
+ double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
+ double X = xc * Z;
+ double Y = yc * Z;
+ vpColVector point_cam(4);
+ point_cam[0] = X;
+ point_cam[1] = Y;
+ point_cam[2] = Z;
+ point_cam[3] = 1;
+ vpColVector point_obj(4);
+ point_obj = cMo.inverse() * point_cam;
+ point.setWorldCoordinates(point_obj);
+}
+
+/*!
+ Keep only keypoints located on faces and compute for those keypoints the 3D coordinate given the 2D image coordinate
+ and under the assumption that the point is located on a plane.
+
+ \param cMo : Homogeneous matrix between the world and the camera frames.
+ \param cam : Camera parameters.
+ \param candidates : In input, list of keypoints detected in the whole image, in output, list of keypoints only located
+ on planes.
+ \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
+ \param roisPt : List of faces.
+ \param points : Output list of computed 3D coordinates of keypoints located only on faces.
+ \param descriptors : Optional parameter, pointer to the descriptors to filter
+ */
+void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<cv::KeyPoint> &candidates, const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
+
+ std::vector<cv::KeyPoint> candidatesToCheck = candidates;
+ candidates.clear();
+ points.clear();
+ vpImagePoint imPt;
+ cv::Point3f pt;
+ cv::Mat desc;
+
+ std::vector<std::pair<cv::KeyPoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
+ for(size_t i = 0; i < candidatesToCheck.size(); i++) {
+ pairOfCandidatesToCheck[i] = std::pair<cv::KeyPoint, size_t>(candidatesToCheck[i], i);
+ }
+
+ size_t cpt1 = 0;
+ std::vector<vpPolygon> polygons_tmp = polygons;
+ for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
+ std::vector<std::pair<cv::KeyPoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
+
+ while(it2 != pairOfCandidatesToCheck.end()) {
+ imPt.set_ij(it2->first.pt.y, it2->first.pt.x);
+ if (it1->isInside(imPt)) {
+ candidates.push_back(it2->first);
+ vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
+ points.push_back(pt);
+
+ if(descriptors != NULL) {
+ desc.push_back(descriptors->row((int) it2->second));
+ }
+
+ //Remove candidate keypoint which is located on the current polygon
+ it2 = pairOfCandidatesToCheck.erase(it2);
+ } else {
+ ++it2;
+ }
+ }
+ }
+
+ if(descriptors != NULL) {
+ desc.copyTo(*descriptors);
+ }
+}
+
+/*!
+ Keep only keypoints located on faces and compute for those keypoints the 3D coordinate given the 2D image coordinate
+ and under the assumption that the point is located on a plane.
+
+ \param cMo : Homogeneous matrix between the world and the camera frames.
+ \param cam : Camera parameters.
+ \param candidates : In input, list of vpImagePoint located in the whole image, in output, list of vpImagePoint only located
+ on planes.
+ \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
+ \param roisPt : List of faces.
+ \param points : Output list of computed 3D coordinates of vpImagePoint located only on faces.
+ \param descriptors : Optional parameter, pointer to the descriptors to filter
+ */
+void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<vpImagePoint> &candidates, const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors) {
+
+ std::vector<vpImagePoint> candidatesToCheck = candidates;
+ candidates.clear();
+ points.clear();
+ vpPoint pt;
+ cv::Mat desc;
+
+ std::vector<std::pair<vpImagePoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
+ for(size_t i = 0; i < candidatesToCheck.size(); i++) {
+ pairOfCandidatesToCheck[i] = std::pair<vpImagePoint, size_t>(candidatesToCheck[i], i);
+ }
+
+ size_t cpt1 = 0;
+ std::vector<vpPolygon> polygons_tmp = polygons;
+ for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
+ std::vector<std::pair<vpImagePoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
+
+ while(it2 != pairOfCandidatesToCheck.end()) {
+ if (it1->isInside(it2->first)) {
+ candidates.push_back(it2->first);
+ vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
+ points.push_back(pt);
+
+ if(descriptors != NULL) {
+ desc.push_back(descriptors->row((int) it2->second));
+ }
+
+ //Remove candidate keypoint which is located on the current polygon
+ it2 = pairOfCandidatesToCheck.erase(it2);
+ } else {
+ ++it2;
+ }
+ }
+ }
+}
+
+/*!
+ Compute the pose using the correspondence between 2D points and 3D points using OpenCV function with RANSAC method.
+
+ \param imagePoints : List of 2D points corresponding to the location of the detected keypoints.
+ \param objectPoints : List of the 3D points in the object frame matched.
+ \param cam : Camera parameters.
+ \param cMo : Homogeneous matrix between the object frame and the camera frame.
+ \param inlierIndex : List of indexes of inliers.
+ \param elapsedTime : Elapsed time.
+ \param func : Function pointer to filter the final pose returned by OpenCV pose estimation method.
+ \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
+ */
+bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
+ const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ double t = vpTime::measureTimeMs();
+
+ if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
+
+ return false;
+ }
+
+ cv::Mat cameraMatrix =
+ (cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
+ cv::Mat rvec, tvec;
+
+
+ //Bug with OpenCV < 2.4.0 when zero distorsion is provided by an empty array.
+ //http://code.opencv.org/issues/1700 ; http://code.opencv.org/issues/1718
+ //what(): Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector in function cvProjectPoints2
+ //Fixed in OpenCV 2.4.0 (r7558)
+// cv::Mat distCoeffs;
+ cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);
+
+ try {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ //OpenCV 3.0.0 (2014/12/12)
+ cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
+ rvec, tvec, false, m_nbRansacIterations, (float) m_ransacReprojectionError,
+ 0.99,//confidence=0.99 (default) – The probability that the algorithm produces a useful result.
+ inlierIndex,
+ cv::SOLVEPNP_ITERATIVE);
+ //SOLVEPNP_ITERATIVE (default): Iterative method is based on Levenberg-Marquardt optimization.
+ //In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances
+ //between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
+ //SOLVEPNP_P3P: Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification
+ //for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
+ //SOLVEPNP_EPNP: Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient
+ //Perspective-n-Point Camera Pose Estimation”.
+ //SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS)
+ //Method for PnP”.
+ //SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer.
+ //“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also
+ //estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the
+ //estimated focal length.
+#else
+ int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
+ if(m_useConsensusPercentage) {
+ nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (double) m_queryFilteredKeyPoints.size());
+ }
+
+ cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
+ rvec, tvec, false, m_nbRansacIterations,
+ (float) m_ransacReprojectionError, nbInlierToReachConsensus,
+ inlierIndex);
+#endif
+ } catch (cv::Exception &e) {
+ std::cerr << e.what() << std::endl;
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ return false;
+ }
+ vpTranslationVector translationVec(tvec.at<double>(0),
+ tvec.at<double>(1), tvec.at<double>(2));
+ vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1),
+ rvec.at<double>(2));
+ cMo = vpHomogeneousMatrix(translationVec, thetaUVector);
+
+ if(func != NULL) {
+ //Check the final pose returned by solvePnPRansac to discard
+ //solutions which do not respect the pose criterion.
+ if(!func(&cMo)) {
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ return false;
+ }
+ }
+
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ return true;
+}
+
+/*!
+ Compute the pose using the correspondence between 2D points and 3D points using ViSP function with RANSAC method.
+
+ \param objectVpPoints : List of vpPoint with coordinates expressed in the object and in the camera frame.
+ \param cMo : Homogeneous matrix between the object frame and the camera frame.
+ \param inliers : List of inliers.
+ \param elapsedTime : Elapsed time.
+ \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
+ \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
+ the poses which do not respect some criterion
+ */
+bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
+ std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ std::vector<unsigned int> inlierIndex;
+ return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func);
+}
+
+/*!
+ Compute the pose using the correspondence between 2D points and 3D points using ViSP function with RANSAC method.
+
+ \param objectVpPoints : List of vpPoint with coordinates expressed in the object and in the camera frame.
+ \param cMo : Homogeneous matrix between the object frame and the camera frame.
+ \param inliers : List of inlier points.
+ \param inlierIndex : List of inlier index.
+ \param elapsedTime : Elapsed time.
+ \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
+ \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
+ the poses which do not respect some criterion
+ */
+bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
+ std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ double t = vpTime::measureTimeMs();
+
+ if(objectVpPoints.size() < 4) {
+ elapsedTime = (vpTime::measureTimeMs() - t);
+// std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
+
+ return false;
+ }
+
+ vpPose pose;
+
+ for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
+ pose.addPoint(*it);
+ }
+
+ unsigned int nbInlierToReachConsensus = (unsigned int) m_nbRansacMinInlierCount;
+ if(m_useConsensusPercentage) {
+ nbInlierToReachConsensus = (unsigned int) (m_ransacConsensusPercentage / 100.0 *
+ (double) m_queryFilteredKeyPoints.size());
+ }
+
+ pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
+ pose.setRansacThreshold(m_ransacThreshold);
+ pose.setRansacMaxTrials(m_nbRansacIterations);
+
+ bool isRansacPoseEstimationOk = false;
+ try {
+ pose.setCovarianceComputation(m_computeCovariance);
+ isRansacPoseEstimationOk = pose.computePose(vpPose::RANSAC, cMo, func);
+ inliers = pose.getRansacInliers();
+ inlierIndex = pose.getRansacInlierIndex();
+
+ if(m_computeCovariance) {
+ m_covarianceMatrix = pose.getCovarianceMatrix();
+ }
+ } catch(vpException &e) {
+ std::cerr << "e=" << e.what() << std::endl;
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ return false;
+ }
+
+// if(func != NULL && isRansacPoseEstimationOk) {
+// //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
+// //we can converge toward a final cMo that does not respect the pose criterion even
+// //if the 4 minimal points picked to respect the pose criterion.
+// if(!func(&cMo)) {
+// elapsedTime = (vpTime::measureTimeMs() - t);
+// return false;
+// }
+// }
+
+ elapsedTime = (vpTime::measureTimeMs() - t);
+ return isRansacPoseEstimationOk;
+}
+
+/*!
+ Compute the pose estimation error, the mean square error (in pixel) between the location of the detected keypoints
+ and the location of the projection of the 3D model with the estimated pose.
+
+ \param matchKeyPoints : List of pairs between the detected keypoints and the corresponding 3D points.
+ \param cam : Camera parameters.
+ \param cMo_est : Estimated pose of the object.
+
+ \return The mean square error (in pixel) between the location of the detected keypoints
+ and the location of the projection of the 3D model with the estimated pose.
+ */
+double vpKeyPoint::computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) {
+ if(matchKeyPoints.size() == 0) {
+ //return std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ return DBL_MAX;
+ }
+
+ std::vector<double> errors(matchKeyPoints.size());
+ size_t cpt = 0;
+ vpPoint pt;
+ for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
+ it != matchKeyPoints.end(); ++it, cpt++) {
+ pt.set_oX(it->second.x);
+ pt.set_oY(it->second.y);
+ pt.set_oZ(it->second.z);
+ pt.project(cMo_est);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
+ errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
+ }
+
+ return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
+}
+
+/*!
+ Initialize the size of the matching image (case with a matching side by side between IRef and ICurrent).
+
+ \param IRef : Reference image.
+ \param ICurrent : Current image.
+ \param IMatching : Image matching.
+ */
+void vpKeyPoint::createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
+ vpImage<unsigned char> &IMatching) {
+ //Image matching side by side
+ unsigned int width = IRef.getWidth() + ICurrent.getWidth();
+ unsigned int height = (std::max)(IRef.getHeight(), ICurrent.getHeight());
+
+ IMatching = vpImage<unsigned char>(height, width);
+}
+
+/*!
+ Initialize the size of the matching image with appropriate size according to the number of training images.
+ Used to display the matching of keypoints detected in the current image with those detected in multiple
+ training images.
+
+ \param ICurrent : Current image.
+ \param IMatching : Image initialized with appropriate size.
+ */
+void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
+ //Nb images in the training database + the current image we want to detect the object
+ unsigned int nbImg = (unsigned int) (m_mapOfImages.size() + 1);
+
+ if(m_mapOfImages.empty()) {
+ std::cerr << "There is no training image loaded !" << std::endl;
+ return;
+ }
+
+ if(nbImg == 2) {
+ //Only one training image, so we display them side by side
+ createImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
+ } else {
+ //Multiple training images, display them as a mosaic image
+ //(unsigned int) std::floor(std::sqrt((double) nbImg) + 0.5);
+ unsigned int nbImgSqrt = (unsigned int) vpMath::round(std::sqrt((double) nbImg));
+
+ //Number of columns in the mosaic grid
+ unsigned int nbWidth = nbImgSqrt;
+ //Number of rows in the mosaic grid
+ unsigned int nbHeight = nbImgSqrt;
+
+ //Deals with non square mosaic grid and the total number of images
+ if(nbImgSqrt * nbImgSqrt < nbImg) {
+ nbWidth++;
+ }
+
+ unsigned int maxW = ICurrent.getWidth();
+ unsigned int maxH = ICurrent.getHeight();
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
+ if(maxW < it->second.getWidth()) {
+ maxW = it->second.getWidth();
+ }
+
+ if(maxH < it->second.getHeight()) {
+ maxH = it->second.getHeight();
+ }
+ }
+
+ IMatching = vpImage<unsigned char>(maxH * nbHeight, maxW * nbWidth);
+ }
+}
+
+/*!
+ Detect keypoints in the image.
+
+ \param I : Input image.
+ \param keyPoints : Output list of the detected keypoints.
+ \param elapsedTime : Elapsed time.
+ \param rectangle : Optional rectangle of the region of interest.
+ */
+void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
+ const vpRect &rectangle) {
+ cv::Mat matImg;
+ vpImageConvert::convert(I, matImg, false);
+ cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);
+
+ if(rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
+ cv::Point leftTop((int) rectangle.getLeft(), (int) rectangle.getTop()), rightBottom((int) rectangle.getRight(),
+ (int) rectangle.getBottom());
+ cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
+ } else {
+ mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U) * 255;
+ }
+
+ detect(matImg, keyPoints, elapsedTime, mask);
+}
+
+/*!
+ Detect keypoints in the image.
+
+ \param matImg : Input image.
+ \param keyPoints : Output list of the detected keypoints.
+ \param elapsedTime : Elapsed time.
+ \param mask : Optional mask to detect only where mask[i][j] == 1.
+ */
+void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
+ const cv::Mat &mask) {
+ double t = vpTime::measureTimeMs();
+ keyPoints.clear();
+
+ for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
+ std::vector<cv::KeyPoint> kp;
+ it->second->detect(matImg, kp, mask);
+ keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
+ }
+
+ elapsedTime = vpTime::measureTimeMs() - t;
+}
+
+/*!
+ Display the reference and the detected keypoints in the images.
+
+ \param IRef : Input reference image.
+ \param ICurrent : Input current image.
+ \param size : Size of the displayed cross.
+ */
+void vpKeyPoint::display(const vpImage<unsigned char> &IRef,
+ const vpImage<unsigned char> &ICurrent, unsigned int size) {
+ std::vector<vpImagePoint> vpQueryImageKeyPoints;
+ getQueryKeyPoints(vpQueryImageKeyPoints);
+ std::vector<vpImagePoint> vpTrainImageKeyPoints;
+ getTrainKeyPoints(vpTrainImageKeyPoints);
+
+ for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ vpDisplay::displayCross(IRef, vpTrainImageKeyPoints[(size_t)(it->trainIdx)], size, vpColor::red);
+ vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, vpColor::green);
+ }
+}
+
+/*!
+ Display the reference keypoints.
+
+ \param ICurrent : Input current image.
+ \param size : Size of the displayed crosses.
+ \param color : Color of the crosses.
+ */
+void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color) {
+ std::vector<vpImagePoint> vpQueryImageKeyPoints;
+ getQueryKeyPoints(vpQueryImageKeyPoints);
+
+ for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ vpDisplay::displayCross (ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
+ }
+}
+
+/*!
+ Display the matching lines between the detected keypoints with those detected in one training image.
+
+ \param IRef : Reference image, used to have the x-offset.
+ \param IMatching : Resulting image matching.
+ \param crossSize : Size of the displayed crosses.
+ \param lineThickness : Thickness of the displayed lines.
+ \param color : Color to use, if none, we pick randomly a color for each pair of matching.
+ */
+void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
+ unsigned int crossSize, unsigned int lineThickness, const vpColor &color) {
+ bool randomColor = (color == vpColor::none);
+ srand((unsigned int) time(NULL));
+ vpColor currentColor = color;
+
+ std::vector<vpImagePoint> queryImageKeyPoints;
+ getQueryKeyPoints(queryImageKeyPoints);
+ std::vector<vpImagePoint> trainImageKeyPoints;
+ getTrainKeyPoints(trainImageKeyPoints);
+
+ vpImagePoint leftPt, rightPt;
+ for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ if(randomColor) {
+ currentColor = vpColor((rand() % 256), (rand() % 256), (rand() % 256));
+ }
+
+ leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
+ rightPt = vpImagePoint(queryImageKeyPoints[(size_t)(it->queryIdx)].get_i(),
+ queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.getWidth());
+ vpDisplay::displayCross(IMatching, leftPt, crossSize, currentColor);
+ vpDisplay::displayCross(IMatching, rightPt, crossSize, currentColor);
+ vpDisplay::displayLine(IMatching, leftPt, rightPt, currentColor, lineThickness);
+ }
+}
+
+/*!
+ Display matching between keypoints detected in the current image and with those detected in the multiple training
+ images. Display also RANSAC inliers if the list is supplied.
+
+ \param ICurrent : Current image.
+ \param IMatching : Resulting matching image.
+ \param ransacInliers : List of Ransac inliers or empty list if not available.
+ \param crossSize : Size of the displayed crosses.
+ \param lineThickness : Thickness of the displayed line.
+ */
+void vpKeyPoint::displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
+ const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize, unsigned int lineThickness) {
+ if(m_mapOfImages.empty() || m_mapOfImageId.empty()) {
+ //No training images so return
+ std::cerr << "There is no training image loaded !" << std::endl;
+ return;
+ }
+
+ //Nb images in the training database + the current image we want to detect the object
+ int nbImg = (int) (m_mapOfImages.size() + 1);
+
+ if(nbImg == 2) {
+ //Only one training image, so we display the matching result side-by-side
+ displayMatching(m_mapOfImages.begin()->second, IMatching, crossSize);
+ } else {
+ //Multiple training images, display them as a mosaic image
+ int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
+ int nbWidth = nbImgSqrt;
+ int nbHeight = nbImgSqrt;
+
+ if(nbImgSqrt * nbImgSqrt < nbImg) {
+ nbWidth++;
+ }
+
+ std::map<int, int> mapOfImageIdIndex;
+ int cpt = 0;
+ unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
+ mapOfImageIdIndex[it->first] = cpt;
+
+ if(maxW < it->second.getWidth()) {
+ maxW = it->second.getWidth();
+ }
+
+ if(maxH < it->second.getHeight()) {
+ maxH = it->second.getHeight();
+ }
+ }
+
+ //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
+ int medianI = nbHeight / 2;
+ int medianJ = nbWidth / 2;
+ int medianIndex = medianI * nbWidth + medianJ;
+ for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
+ vpImagePoint topLeftCorner;
+ int current_class_id_index = 0;
+ if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
+ current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
+ } else {
+ //Shift of one unity the index of the training images which are after the current image
+ current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
+ }
+
+ int indexI = current_class_id_index / nbWidth;
+ int indexJ = current_class_id_index - (indexI * nbWidth);
+ topLeftCorner.set_ij((int)maxH*indexI, (int)maxW*indexJ);
+
+ //Display cross for keypoints in the learning database
+ vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
+ crossSize, vpColor::red);
+ }
+
+ vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
+ for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
+ //Display cross for keypoints detected in the current image
+ vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
+ crossSize, vpColor::red);
+ }
+ for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
+ it != ransacInliers.end(); ++it) {
+ //Display green circle for RANSAC inliers
+ vpDisplay::displayCircle(IMatching, (int) (it->get_v() + topLeftCorner.get_i()), (int) (it->get_u() +
+ topLeftCorner.get_j()), 4, vpColor::green);
+ }
+ for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
+ //Display red circle for RANSAC outliers
+ vpDisplay::displayCircle(IMatching, (int) (it->get_i() + topLeftCorner.get_i()), (int) (it->get_j() +
+ topLeftCorner.get_j()), 4, vpColor::red);
+ }
+
+ for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ int current_class_id = 0;
+ if(mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] < medianIndex) {
+ current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]];
+ } else {
+ //Shift of one unity the index of the training images which are after the current image
+ current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] + 1;
+ }
+
+ int indexI = current_class_id / nbWidth;
+ int indexJ = current_class_id - (indexI * nbWidth);
+
+ vpImagePoint end((int)maxH*indexI + m_trainKeyPoints[(size_t) it->trainIdx].pt.y,
+ (int)maxW*indexJ + m_trainKeyPoints[(size_t) it->trainIdx].pt.x);
+ vpImagePoint start((int)maxH*medianI + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.y,
+ (int)maxW*medianJ + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.x);
+
+ //Draw line for matching keypoints detected in the current image and those detected
+ //in the training images
+ vpDisplay::displayLine(IMatching, start, end, vpColor::green, lineThickness);
+ }
+ }
+}
+
+/*!
+ Extract the descriptors for each keypoints of the list.
+
+ \param I : Input image.
+ \param keyPoints : List of keypoints we want to extract their descriptors.
+ \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
+ \param elapsedTime : Elapsed time.
+ \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
+ the corresponding 3D point.
+ */
+void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
+ cv::Mat matImg;
+ vpImageConvert::convert(I, matImg, false);
+ extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
+}
+
+/*!
+ Extract the descriptors for each keypoints of the list.
+
+ \param matImg : Input image.
+ \param keyPoints : List of keypoints we want to extract their descriptors.
+ \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
+ \param elapsedTime : Elapsed time.
+ \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
+ the corresponding 3D point.
+ */
+void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
+ double t = vpTime::measureTimeMs();
+ bool first = true;
+
+ for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
+ itd != m_extractors.end(); ++itd) {
+ if(first) {
+ first = false;
+ //Check if we have 3D object points information
+ if(trainPoints != NULL && !trainPoints->empty()) {
+ //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
+ std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
+
+ //Extract descriptors for the given list of keypoints
+ itd->second->compute(matImg, keyPoints, descriptors);
+
+ if(keyPoints.size() != keyPoints_tmp.size()) {
+ //Keypoints have been removed
+ //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ std::map<size_t, size_t> mapOfKeypointHashes;
+ size_t cpt = 0;
+ for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
+ mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
+ }
+
+ std::vector<cv::Point3f> trainPoints_tmp;
+ for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
+ if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
+ }
+ }
+
+ //Copy trainPoints_tmp to m_trainPoints
+ *trainPoints = trainPoints_tmp;
+ }
+ } else {
+ //Extract descriptors for the given list of keypoints
+ itd->second->compute(matImg, keyPoints, descriptors);
+ }
+ } else {
+ //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
+ std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
+
+ cv::Mat desc;
+ //Extract descriptors for the given list of keypoints
+ itd->second->compute(matImg, keyPoints, desc);
+
+ if(keyPoints.size() != keyPoints_tmp.size()) {
+ //Keypoints have been removed
+ //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ std::map<size_t, size_t> mapOfKeypointHashes;
+ size_t cpt = 0;
+ for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
+ mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
+ }
+
+ std::vector<cv::Point3f> trainPoints_tmp;
+ cv::Mat descriptors_tmp;
+ for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
+ if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ if(trainPoints != NULL && !trainPoints->empty()) {
+ trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
+ }
+
+ if(!descriptors.empty()) {
+ descriptors_tmp.push_back(descriptors.row((int) mapOfKeypointHashes[myKeypointHash(*it)]));
+ }
+ }
+ }
+
+ if(trainPoints != NULL) {
+ //Copy trainPoints_tmp to m_trainPoints
+ *trainPoints = trainPoints_tmp;
+ }
+ //Copy descriptors_tmp to descriptors
+ descriptors_tmp.copyTo(descriptors);
+ }
+
+ //Merge descriptors horizontally
+ if(descriptors.empty()) {
+ desc.copyTo(descriptors);
+ } else {
+ cv::hconcat(descriptors, desc, descriptors);
+ }
+ }
+ }
+
+ if(keyPoints.size() != (size_t) descriptors.rows) {
+ std::cerr << "keyPoints.size() != (size_t) descriptors.rows" << std::endl;
+ }
+ elapsedTime = vpTime::measureTimeMs() - t;
+}
+
+/*!
+ Filter the matches using the desired filtering method.
+ */
+void vpKeyPoint::filterMatches() {
+ std::vector<cv::KeyPoint> queryKpts;
+ std::vector<cv::Point3f> trainPts;
+ std::vector<cv::DMatch> m;
+
+ if(m_useKnn) {
+ double max_dist = 0;
+ //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ double min_dist = DBL_MAX;
+ double mean = 0.0;
+ std::vector<double> distance_vec(m_knnMatches.size());
+
+ if(m_filterType == stdAndRatioDistanceThreshold) {
+ for(size_t i = 0; i < m_knnMatches.size(); i++) {
+ double dist = m_knnMatches[i][0].distance;
+ mean += dist;
+ distance_vec[i] = dist;
+
+ if (dist < min_dist) {
+ min_dist = dist;
+ }
+ if (dist > max_dist) {
+ max_dist = dist;
+ }
+ }
+ mean /= m_queryDescriptors.rows;
+ }
+
+ double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
+ double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
+ double threshold = min_dist + stdev;
+
+ for(size_t i = 0; i < m_knnMatches.size(); i++) {
+ if(m_knnMatches[i].size() >= 2) {
+ //Calculate ratio of the descriptor distance between the two nearest neighbors of the keypoint
+ float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
+// float ratio = std::sqrt((vecMatches[i][0].distance * vecMatches[i][0].distance)
+// / (vecMatches[i][1].distance * vecMatches[i][1].distance));
+ double dist = m_knnMatches[i][0].distance;
+
+ if(ratio < m_matchingRatioThreshold || (m_filterType == stdAndRatioDistanceThreshold && dist < threshold)) {
+ m.push_back(cv::DMatch((int) queryKpts.size(), m_knnMatches[i][0].trainIdx, m_knnMatches[i][0].distance));
+
+ if(!m_trainPoints.empty()) {
+ trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]);
+ }
+ queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]);
+ }
+ }
+ }
+ } else {
+ double max_dist = 0;
+ // create an error under Windows. To fix it we have to add #undef max
+ //double min_dist = std::numeric_limits<double>::max();
+ double min_dist = DBL_MAX;
+ double mean = 0.0;
+ std::vector<double> distance_vec(m_matches.size());
+ for(size_t i = 0; i < m_matches.size(); i++) {
+ double dist = m_matches[i].distance;
+ mean += dist;
+ distance_vec[i] = dist;
+
+ if (dist < min_dist) {
+ min_dist = dist;
+ }
+ if (dist > max_dist) {
+ max_dist = dist;
+ }
+ }
+ mean /= m_queryDescriptors.rows;
+
+ double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
+ double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
+
+ //Define a threshold where we keep all keypoints whose the descriptor distance falls below a factor of the
+ //minimum descriptor distance (for all the query keypoints)
+ //or below the minimum descriptor distance + the standard deviation (calculated on all the query descriptor distances)
+ double threshold = m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;
+
+ for (size_t i = 0; i < m_matches.size(); i++) {
+ if(m_matches[i].distance <= threshold) {
+ m.push_back(cv::DMatch((int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
+
+ if(!m_trainPoints.empty()) {
+ trainPts.push_back(m_trainPoints[(size_t)m_matches[i].trainIdx]);
+ }
+ queryKpts.push_back(m_queryKeyPoints[(size_t)m_matches[i].queryIdx]);
+ }
+ }
+ }
+
+ if(m_useSingleMatchFilter) {
+ //Eliminate matches where multiple query keypoints are matched to the same train keypoint
+ std::vector<cv::DMatch> mTmp;
+ std::vector<cv::Point3f> trainPtsTmp;
+ std::vector<cv::KeyPoint> queryKptsTmp;
+
+ std::map<int, int> mapOfTrainIdx;
+ //Count the number of query points matched to the same train point
+ for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
+ mapOfTrainIdx[it->trainIdx]++;
+ }
+
+ //Keep matches with only one correspondence
+ for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
+ if(mapOfTrainIdx[it->trainIdx] == 1) {
+ mTmp.push_back(cv::DMatch((int) queryKptsTmp.size(), it->trainIdx, it->distance));
+
+ if(!m_trainPoints.empty()) {
+ trainPtsTmp.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ }
+ queryKptsTmp.push_back(queryKpts[(size_t) it->queryIdx]);
+ }
+ }
+
+ m_filteredMatches = mTmp;
+ m_objectFilteredPoints = trainPtsTmp;
+ m_queryFilteredKeyPoints = queryKptsTmp;
+ } else {
+ m_filteredMatches = m;
+ m_objectFilteredPoints = trainPts;
+ m_queryFilteredKeyPoints = queryKpts;
+ }
+}
+
+/*!
+ Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
+ of the keypoints detected in the current image after the matching).
+
+ \param objectPoints : List of 3D coordinates in the object frame.
+ */
+void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const {
+ objectPoints = m_objectFilteredPoints;
+}
+
+/*!
+ Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
+ of the keypoints detected in the current image after the matching).
+
+ \param objectPoints : List of 3D coordinates in the object frame.
+ */
+void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const {
+ vpConvert::convertFromOpenCV(m_objectFilteredPoints, objectPoints);
+}
+
+/*!
+ Get the query keypoints list in OpenCV type.
+
+ \param keyPoints : List of query keypoints (or keypoints detected in the current image).
+ */
+void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
+ keyPoints = m_queryFilteredKeyPoints;
+}
+
+/*!
+ Get the query keypoints list in ViSP type.
+
+ \param keyPoints : List of query keypoints (or keypoints detected in the current image).
+ */
+void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
+ keyPoints = currentImagePointsList;
+}
+
+/*!
+ Get the train keypoints list in OpenCV type.
+
+ \param keyPoints : List of train keypoints (or reference keypoints).
+ */
+void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
+ keyPoints = m_trainKeyPoints;
+}
+
+/*!
+ Get the train keypoints list in ViSP type.
+
+ \param keyPoints : List of train keypoints (or reference keypoints).
+ */
+void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
+ keyPoints = referenceImagePointsList;
+}
+
+/*!
+ Get the train points (the 3D coordinates in the object frame) list in OpenCV type.
+
+ \param points : List of train points (or reference points).
+ */
+void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const {
+ points = m_trainPoints;
+}
+
+/*!
+ Get the train points (the 3D coordinates in the object frame) list in ViSP type.
+
+ \param points : List of train points (or reference points).
+ */
+void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const {
+ points = m_trainVpPoints;
+}
+
+/*!
+ Initialize method for RANSAC parameters and for detectors, extractors and matcher, and for others parameters.
+ */
+void vpKeyPoint::init() {
+ // Require 2.4.0 <= opencv < 3.0.0
+#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ //The following line must be called in order to use SIFT or SURF
+ if (!cv::initModule_nonfree()) {
+ std::cerr << "Cannot init module non free, SIFT or SURF cannot be used."
+ << std::endl;
+ }
+#endif
+
+ initDetectors(m_detectorNames);
+ initExtractors(m_extractorNames);
+ initMatcher(m_matcherName);
+}
+
+/*!
+ Initialize a keypoint detector based on its name.
+
+ \param detectorName : Name of the detector (e.g FAST, SIFT, SURF, etc.).
+ */
+void vpKeyPoint::initDetector(const std::string &detectorName) {
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);
+
+ if(m_detectors[detectorName] == NULL) {
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the detector: " << detectorName << " or it is not available in OpenCV version: "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
+ throw vpException(vpException::fatalError, ss_msg.str());
+ }
+#else
+ std::string detectorNameTmp = detectorName;
+ std::string pyramid = "Pyramid";
+ std::size_t pos = detectorName.find(pyramid);
+ bool usePyramid = false;
+ if(pos != std::string::npos) {
+ detectorNameTmp = detectorName.substr(pos + pyramid.size());
+ usePyramid = true;
+ }
+
+ if(detectorNameTmp == "SIFT") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ cv::Ptr<cv::FeatureDetector> siftDetector = cv::xfeatures2d::SIFT::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = siftDetector;
+ } else {
+ std::cerr << "Kind of non sense to use SIFT with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(siftDetector);
+ }
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the detector: SIFT. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(detectorNameTmp == "SURF") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ cv::Ptr<cv::FeatureDetector> surfDetector = cv::xfeatures2d::SURF::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = surfDetector;
+ } else {
+ std::cerr << "Kind of non sense to use SURF with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(surfDetector);
+ }
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the detector: SURF. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(detectorNameTmp == "FAST") {
+ cv::Ptr<cv::FeatureDetector> fastDetector = cv::FastFeatureDetector::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = fastDetector;
+ } else {
+// m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::FastFeatureDetector::create());
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
+ }
+ } else if(detectorNameTmp == "MSER") {
+ cv::Ptr<cv::FeatureDetector> fastDetector = cv::MSER::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = fastDetector;
+ } else {
+// m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::MSER::create());
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
+ }
+ } else if(detectorNameTmp == "ORB") {
+ cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = orbDetector;
+ } else {
+ std::cerr << "Kind of non sense to use ORB with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(orbDetector);
+ }
+ } else if(detectorNameTmp == "BRISK") {
+ cv::Ptr<cv::FeatureDetector> briskDetector = cv::BRISK::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = briskDetector;
+ } else {
+ std::cerr << "Kind of non sense to use BRISK with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(briskDetector);
+ }
+ } else if(detectorNameTmp == "KAZE") {
+ cv::Ptr<cv::FeatureDetector> kazeDetector = cv::KAZE::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = kazeDetector;
+ } else {
+ std::cerr << "Kind of non sense to use KAZE with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(kazeDetector);
+ }
+ } else if(detectorNameTmp == "AKAZE") {
+ cv::Ptr<cv::FeatureDetector> akazeDetector = cv::AKAZE::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = akazeDetector;
+ } else {
+ std::cerr << "Kind of non sense to use AKAZE with Pyramid !" << std::endl;
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(akazeDetector);
+ }
+ } else if(detectorNameTmp == "GFTT") {
+ cv::Ptr<cv::FeatureDetector> gfttDetector = cv::GFTTDetector::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = gfttDetector;
+ } else {
+// m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::GFTTDetector::create());
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(gfttDetector);
+ }
+ } else if(detectorNameTmp == "SimpleBlob") {
+ cv::Ptr<cv::FeatureDetector> simpleBlobDetector = cv::SimpleBlobDetector::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = simpleBlobDetector;
+ } else {
+// m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::SimpleBlobDetector::create());
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(simpleBlobDetector);
+ }
+ } else if(detectorNameTmp == "STAR") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ cv::Ptr<cv::FeatureDetector> starDetector = cv::xfeatures2d::StarDetector::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = starDetector;
+ } else {
+// m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::xfeatures2d::StarDetector::create());
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(starDetector);
+ }
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the detector: STAR. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(detectorNameTmp == "AGAST") {
+ cv::Ptr<cv::FeatureDetector> agastDetector = cv::AgastFeatureDetector::create();
+ if(!usePyramid) {
+ m_detectors[detectorNameTmp] = agastDetector;
+ } else {
+ m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(agastDetector);
+ }
+ } else {
+ std::cerr << "The detector:" << detectorNameTmp << " is not available." << std::endl;
+ }
+
+ bool detectorInitialized = false;
+ if(!usePyramid) {
+ detectorInitialized = (m_detectors[detectorNameTmp] != NULL);
+ } else {
+ detectorInitialized = (m_detectors[detectorName] != NULL);
+ }
+
+ if(!detectorInitialized) {
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the detector: " << detectorNameTmp << " or it is not available in OpenCV version: "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
+ throw vpException(vpException::fatalError, ss_msg.str());
+ }
+
+#endif
+}
+
+/*!
+ Initialize a list of keypoints detectors if we want to concatenate multiple detectors.
+
+ \param detectorNames : List of detector names.
+ */
+void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
+ for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
+ initDetector(*it);
+ }
+}
+
+/*!
+ Initialize a descriptor extractor based on its name.
+
+ \param extractorName : Name of the extractor (e.g SIFT, SURF, ORB, etc.).
+ */
+void vpKeyPoint::initExtractor(const std::string &extractorName) {
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
+#else
+ if(extractorName == "SIFT") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: SIFT. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "SURF") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ //Use extended set of SURF descriptors (128 instead of 64)
+ m_extractors[extractorName] = cv::xfeatures2d::SURF::create(100, 4, 3, true);
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: SURF. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "ORB") {
+ m_extractors[extractorName] = cv::ORB::create();
+ } else if(extractorName == "BRISK") {
+ m_extractors[extractorName] = cv::BRISK::create();
+ } else if(extractorName == "FREAK") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: FREAK. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "BRIEF") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: BRIEF. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "KAZE") {
+ m_extractors[extractorName] = cv::KAZE::create();
+ } else if(extractorName == "AKAZE") {
+ m_extractors[extractorName] = cv::AKAZE::create();
+ } else if(extractorName == "DAISY") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::DAISY::create();
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: DAISY. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "LATCH") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::LATCH::create();
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: LATCH. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else if(extractorName == "LUCID") {
+#ifdef VISP_HAVE_OPENCV_XFEATURES2D
+ m_extractors[extractorName] = cv::xfeatures2d::LUCID::create(1, 2);
+#else
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: LUCID. OpenCV version "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ throw vpException(vpException::fatalError, ss_msg.str());
+#endif
+ } else {
+ std::cerr << "The extractor:" << extractorName << " is not available." << std::endl;
+ }
+#endif
+
+ if(m_extractors[extractorName] == NULL) {
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the extractor: " << extractorName << " or it is not available in OpenCV version: "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
+ throw vpException(vpException::fatalError, ss_msg.str());
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if(extractorName == "SURF") {
+ //Use extended set of SURF descriptors (128 instead of 64)
+ m_extractors[extractorName]->set("extended", 1);
+ }
+#endif
+}
+
+/*!
+ Initialize a list of descriptor extractors if we want to concatenate multiple extractors.
+
+ \param extractorNames : List of extractor names.
+ */
+void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames) {
+ for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
+ initExtractor(*it);
+ }
+
+ int descriptorType = CV_32F;
+ bool firstIteration = true;
+ for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
+ it != m_extractors.end(); ++it) {
+ if(firstIteration) {
+ firstIteration = false;
+ descriptorType = it->second->descriptorType();
+ } else {
+ if(descriptorType != it->second->descriptorType()) {
+ throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
+ }
+ }
+ }
+}
+
+/*!
+ Initialize a matcher based on its name.
+
+ \param matcherName : Name of the matcher (e.g BruteForce, FlannBased).
+ */
+void vpKeyPoint::initMatcher(const std::string &matcherName) {
+ int descriptorType = CV_32F;
+ bool firstIteration = true;
+ for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
+ it != m_extractors.end(); ++it) {
+ if(firstIteration) {
+ firstIteration = false;
+ descriptorType = it->second->descriptorType();
+ } else {
+ if(descriptorType != it->second->descriptorType()) {
+ throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
+ }
+ }
+ }
+
+ if(matcherName == "FlannBased") {
+ if(m_extractors.empty()) {
+ std::cout << "Warning: No extractor initialized, by default use floating values (CV_32F) "
+ "for descriptor type !" << std::endl;
+ }
+
+ if(descriptorType == CV_8U) {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
+#else
+ m_matcher = new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(12, 20, 2));
+#endif
+ } else {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::KDTreeIndexParams>());
+#else
+ m_matcher = new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams());
+#endif
+ }
+ } else {
+ m_matcher = cv::DescriptorMatcher::create(matcherName);
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if(m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
+ m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
+ }
+#endif
+
+ if(m_matcher == NULL) {
+ std::stringstream ss_msg;
+ ss_msg << "Fail to initialize the matcher: " << matcherName << " or it is not available in OpenCV version: "
+ << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
+ throw vpException(vpException::fatalError, ss_msg.str());
+ }
+}
+
+/*!
+ Insert a reference image and a current image side-by-side.
+
+ \param IRef : Reference image.
+ \param ICurrent : Current image.
+ \param IMatching : Matching image for displaying all the matching between the query keypoints and those
+ detected in the training images.
+ */
+void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
+ vpImage<unsigned char> &IMatching) {
+ vpImagePoint topLeftCorner(0, 0);
+ IMatching.insert(IRef, topLeftCorner);
+ topLeftCorner = vpImagePoint(0, IRef.getWidth());
+ IMatching.insert(ICurrent, topLeftCorner);
+}
+
+/*!
+ Insert the different training images in the matching image.
+
+ \param ICurrent : Current image.
+ \param IMatching : Matching image for displaying all the matching between the query keypoints and those
+ detected in the training images
+ */
+void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
+ //Nb images in the training database + the current image we want to detect the object
+ int nbImg = (int) (m_mapOfImages.size() + 1);
+
+ if(m_mapOfImages.empty()) {
+ std::cerr << "There is no training image loaded !" << std::endl;
+ return;
+ }
+
+ if(nbImg == 2) {
+ //Only one training image, so we display them side by side
+ insertImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
+ } else {
+ //Multiple training images, display them as a mosaic image
+ int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
+ int nbWidth = nbImgSqrt;
+ int nbHeight = nbImgSqrt;
+
+ if(nbImgSqrt * nbImgSqrt < nbImg) {
+ nbWidth++;
+ }
+
+ unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
+ if(maxW < it->second.getWidth()) {
+ maxW = it->second.getWidth();
+ }
+
+ if(maxH < it->second.getHeight()) {
+ maxH = it->second.getHeight();
+ }
+ }
+
+ //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
+ int medianI = nbHeight / 2;
+ int medianJ = nbWidth / 2;
+ int medianIndex = medianI * nbWidth + medianJ;
+
+ int cpt = 0;
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
+ int local_cpt = cpt;
+ if(cpt >= medianIndex) {
+ //Shift of one unity the index of the training images which are after the current image
+ local_cpt++;
+ }
+ int indexI = local_cpt / nbWidth;
+ int indexJ = local_cpt - (indexI * nbWidth);
+ vpImagePoint topLeftCorner((int)maxH*indexI, (int)maxW*indexJ);
+
+ IMatching.insert(it->second, topLeftCorner);
+ }
+
+ vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
+ IMatching.insert(ICurrent, topLeftCorner);
+ }
+}
+
+#ifdef VISP_HAVE_XML2
+/*!
+ Load configuration parameters from an XML config file.
+
+ \param configFile : Path to the XML config file.
+ */
+void vpKeyPoint::loadConfigFile(const std::string &configFile) {
+ vpXmlConfigParserKeyPoint xmlp;
+
+ try {
+ //Reset detector and extractor
+ m_detectorNames.clear();
+ m_extractorNames.clear();
+ m_detectors.clear();
+ m_extractors.clear();
+
+ std::cout << " *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
+ xmlp.parse(configFile);
+
+ m_detectorNames.push_back(xmlp.getDetectorName());
+ m_extractorNames.push_back(xmlp.getExtractorName());
+ m_matcherName = xmlp.getMatcherName();
+
+ switch(xmlp.getMatchingMethod()) {
+ case vpXmlConfigParserKeyPoint::constantFactorDistanceThreshold:
+ m_filterType = constantFactorDistanceThreshold;
+ break;
+
+ case vpXmlConfigParserKeyPoint::stdDistanceThreshold:
+ m_filterType = stdDistanceThreshold;
+ break;
+
+ case vpXmlConfigParserKeyPoint::ratioDistanceThreshold:
+ m_filterType = ratioDistanceThreshold;
+ break;
+
+ case vpXmlConfigParserKeyPoint::stdAndRatioDistanceThreshold:
+ m_filterType = stdAndRatioDistanceThreshold;
+ break;
+
+ case vpXmlConfigParserKeyPoint::noFilterMatching:
+ m_filterType = noFilterMatching;
+ break;
+
+ default:
+ break;
+ }
+
+ m_matchingFactorThreshold = xmlp.getMatchingFactorThreshold();
+ m_matchingRatioThreshold = xmlp.getMatchingRatioThreshold();
+
+ m_useRansacVVS = xmlp.getUseRansacVVSPoseEstimation();
+ m_useConsensusPercentage = xmlp.getUseRansacConsensusPercentage();
+ m_nbRansacIterations = xmlp.getNbRansacIterations();
+ m_ransacReprojectionError = xmlp.getRansacReprojectionError();
+ m_nbRansacMinInlierCount = xmlp.getNbRansacMinInlierCount();
+ m_ransacThreshold = xmlp.getRansacThreshold();
+ m_ransacConsensusPercentage = xmlp.getRansacConsensusPercentage();
+
+ if(m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) {
+ m_useKnn = true;
+ } else {
+ m_useKnn = false;
+ }
+
+ init();
+ }
+ catch(...) {
+ throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
+ }
+}
+#endif
+
+/*!
+ Load learning data saved on disk.
+
+ \param filename : Path of the learning file.
+ \param binaryMode : If true, the learning file is in a binary mode, otherwise it is in XML mode.
+ \param append : If true, concatenate the learning data, otherwise reset the variables.
+ */
+void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) {
+ int startClassId = 0;
+ int startImageId = 0;
+ if(!append) {
+ m_trainKeyPoints.clear();
+ m_trainPoints.clear();
+ m_mapOfImageId.clear();
+ m_mapOfImages.clear();
+ } else {
+ //In append case, find the max index of keypoint class Id
+ for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
+ if(startClassId < it->first) {
+ startClassId = it->first;
+ }
+ }
+
+ //In append case, find the max index of images Id
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
+ if(startImageId < it->first) {
+ startImageId = it->first;
+ }
+ }
+ }
+
+ //Get parent directory
+ std::string parent = vpIoTools::getParent(filename);
+ if(!parent.empty()) {
+ parent += "/";
+ }
+
+ if(binaryMode) {
+ std::ifstream file(filename.c_str(), std::ifstream::binary);
+ if(!file.is_open()){
+ throw vpException(vpException::ioError, "Cannot open the file.");
+ }
+
+ //Read info about training images
+ int nbImgs = 0;
+ file.read((char *)(&nbImgs), sizeof(nbImgs));
+
+#if !defined(VISP_HAVE_MODULE_IO)
+ if(nbImgs > 0) {
+ std::cout << "Warning: The learning file contains image data that will not be loaded as visp_io module "
+ "is not available !" << std::endl;
+ }
+#endif
+
+ for(int i = 0; i < nbImgs; i++) {
+ //Read image_id
+ int id = 0;
+ file.read((char *)(&id), sizeof(id));
+
+ int length = 0;
+ file.read((char *)(&length), sizeof(length));
+ //Will contain the path to the training images
+ char* path = new char[length + 1];//char path[length + 1];
+
+ for(int cpt = 0; cpt < length; cpt++) {
+ char c;
+ file.read((char *)(&c), sizeof(c));
+ path[cpt] = c;
+ }
+ path[length] = '\0';
+
+ vpImage<unsigned char> I;
+#ifdef VISP_HAVE_MODULE_IO
+ if(vpIoTools::isAbsolutePathname(std::string(path))) {
+ vpImageIo::read(I, path);
+ } else {
+ vpImageIo::read(I, parent + path);
+ }
+
+ //Add the image previously loaded only if VISP_HAVE_MODULE_IO
+ m_mapOfImages[id + startImageId] = I;
+#endif
+
+ //Delete path
+ delete[] path;
+ }
+
+ //Read if 3D point information are saved or not
+ int have3DInfoInt = 0;
+ file.read((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
+ bool have3DInfo = have3DInfoInt != 0;
+
+ //Read the number of descriptors
+ int nRows = 0;
+ file.read((char *)(&nRows), sizeof(nRows));
+
+ //Read the size of the descriptor
+ int nCols = 0;
+ file.read((char *)(&nCols), sizeof(nCols));
+
+ //Read the type of the descriptor
+ int descriptorType = 5; //CV_32F
+ file.read((char *)(&descriptorType), sizeof(descriptorType));
+
+ cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
+ for(int i = 0; i < nRows; i++) {
+ //Read information about keyPoint
+ float u, v, size, angle, response;
+ int octave, class_id, image_id;
+ file.read((char *)(&u), sizeof(u));
+ file.read((char *)(&v), sizeof(v));
+ file.read((char *)(&size), sizeof(size));
+ file.read((char *)(&angle), sizeof(angle));
+ file.read((char *)(&response), sizeof(response));
+ file.read((char *)(&octave), sizeof(octave));
+ file.read((char *)(&class_id), sizeof(class_id));
+ file.read((char *)(&image_id), sizeof(image_id));
+ cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
+ m_trainKeyPoints.push_back(keyPoint);
+
+ if(image_id != -1) {
+#ifdef VISP_HAVE_MODULE_IO
+ //No training images if image_id == -1
+ m_mapOfImageId[class_id] = image_id + startImageId;
+#endif
+ }
+
+ if(have3DInfo) {
+ //Read oX, oY, oZ
+ float oX, oY, oZ;
+ file.read((char *)(&oX), sizeof(oX));
+ file.read((char *)(&oY), sizeof(oY));
+ file.read((char *)(&oZ), sizeof(oZ));
+ m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
+ }
+
+ for(int j = 0; j < nCols; j++) {
+ //Read the value of the descriptor
+ switch(descriptorType) {
+ case CV_8U:
+ {
+ unsigned char value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<unsigned char>(i, j) = value;
+ }
+ break;
+
+ case CV_8S:
+ {
+ char value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<char>(i, j) = value;
+ }
+ break;
+
+ case CV_16U:
+ {
+ unsigned short int value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
+ }
+ break;
+
+ case CV_16S:
+ {
+ short int value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<short int>(i, j) = value;
+ }
+ break;
+
+ case CV_32S:
+ {
+ int value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<int>(i, j) = value;
+ }
+ break;
+
+ case CV_32F:
+ {
+ float value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<float>(i, j) = value;
+ }
+ break;
+
+ case CV_64F:
+ {
+ double value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<double>(i, j) = value;
+ }
+ break;
+
+ default:
+ {
+ float value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<float>(i, j) = value;
+ }
+ break;
+ }
+ }
+ }
+
+ if(!append || m_trainDescriptors.empty()) {
+ trainDescriptorsTmp.copyTo(m_trainDescriptors);
+ } else {
+ cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
+ }
+
+ file.close();
+ } else {
+#ifdef VISP_HAVE_XML2
+ xmlDocPtr doc = NULL;
+ xmlNodePtr root_element = NULL;
+
+ /*
+ * this initialize the library and check potential ABI mismatches
+ * between the version it was compiled for and the actual shared
+ * library used.
+ */
+ LIBXML_TEST_VERSION
+
+ /*parse the file and get the DOM */
+ doc = xmlReadFile(filename.c_str(), NULL, 0);
+
+ if (doc == NULL) {
+ throw vpException(vpException::ioError, "Error with file " + filename);
+ }
+
+ root_element = xmlDocGetRootElement(doc);
+
+ xmlNodePtr first_level_node = NULL;
+ char *pEnd = NULL;
+
+ int descriptorType = CV_32F; //float
+ int nRows = 0, nCols = 0;
+ int i = 0, j = 0;
+
+ cv::Mat trainDescriptorsTmp;
+
+ for (first_level_node = root_element->children; first_level_node;
+ first_level_node = first_level_node->next) {
+
+ std::string name((char *) first_level_node->name);
+ if (first_level_node->type == XML_ELEMENT_NODE && name == "TrainingImageInfo") {
+ xmlNodePtr image_info_node = NULL;
+
+ for (image_info_node = first_level_node->children; image_info_node; image_info_node =
+ image_info_node->next) {
+ name = std::string ((char *) image_info_node->name);
+
+ if(name == "trainImg") {
+ //Read image_id
+ xmlChar *image_id_property = xmlGetProp(image_info_node, BAD_CAST "image_id");
+ int id = 0;
+ if(image_id_property) {
+ id = std::atoi((char *) image_id_property);
+ }
+ xmlFree(image_id_property);
+
+ vpImage<unsigned char> I;
+#ifdef VISP_HAVE_MODULE_IO
+ std::string path((char *) image_info_node->children->content);
+ //Read path to the training images
+ if(vpIoTools::isAbsolutePathname(std::string(path))) {
+ vpImageIo::read(I, path);
+ } else {
+ vpImageIo::read(I, parent + path);
+ }
+
+ //Add the image previously loaded only if VISP_HAVE_MODULE_IO
+ m_mapOfImages[id + startImageId] = I;
+#endif
+ }
+ }
+ } else if(first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
+ xmlNodePtr descriptors_info_node = NULL;
+ for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
+ descriptors_info_node->next) {
+ if (descriptors_info_node->type == XML_ELEMENT_NODE) {
+ name = std::string ((char *) descriptors_info_node->name);
+
+ if(name == "nrows") {
+ nRows = std::atoi((char *) descriptors_info_node->children->content);
+ } else if(name == "ncols") {
+ nCols = std::atoi((char *) descriptors_info_node->children->content);
+ } else if(name == "type") {
+ descriptorType = std::atoi((char *) descriptors_info_node->children->content);
+ }
+ }
+ }
+
+ trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
+ } else if (first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorInfo") {
+ xmlNodePtr point_node = NULL;
+ double u = 0.0, v = 0.0, size = 0.0, angle = 0.0, response = 0.0;
+ int octave = 0, class_id = 0, image_id = 0;
+ double oX = 0.0, oY = 0.0, oZ = 0.0;
+
+ std::stringstream ss;
+
+ for (point_node = first_level_node->children; point_node; point_node =
+ point_node->next) {
+ if (point_node->type == XML_ELEMENT_NODE) {
+ name = std::string ((char *) point_node->name);
+
+ //Read information about keypoints
+ if(name == "u") {
+ u = std::strtod((char *) point_node->children->content, &pEnd);
+ } else if(name == "v") {
+ v = std::strtod((char *) point_node->children->content, &pEnd);
+ } else if(name == "size") {
+ size = std::strtod((char *) point_node->children->content, &pEnd);
+ } else if(name == "angle") {
+ angle = std::strtod((char *) point_node->children->content, &pEnd);
+ } else if(name == "response") {
+ response = std::strtod((char *) point_node->children->content, &pEnd);
+ } else if(name == "octave") {
+ octave = std::atoi((char *) point_node->children->content);
+ } else if(name == "class_id") {
+ class_id = std::atoi((char *) point_node->children->content);
+ cv::KeyPoint keyPoint(cv::Point2f((float) u, (float) v), (float) size,
+ (float) angle, (float) response, octave, (class_id + startClassId));
+ m_trainKeyPoints.push_back(keyPoint);
+ } else if(name == "image_id") {
+ image_id = std::atoi((char *) point_node->children->content);
+ if(image_id != -1) {
+#ifdef VISP_HAVE_MODULE_IO
+ //No training images if image_id == -1
+ m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
+#endif
+ }
+ } else if (name == "oX") {
+ oX = std::atof((char *) point_node->children->content);
+ } else if (name == "oY") {
+ oY = std::atof((char *) point_node->children->content);
+ } else if (name == "oZ") {
+ oZ = std::atof((char *) point_node->children->content);
+ m_trainPoints.push_back(cv::Point3f((float) oX, (float) oY, (float) oZ));
+ } else if (name == "desc") {
+ xmlNodePtr descriptor_value_node = NULL;
+ j = 0;
+
+ for (descriptor_value_node = point_node->children;
+ descriptor_value_node; descriptor_value_node =
+ descriptor_value_node->next) {
+
+ if (descriptor_value_node->type == XML_ELEMENT_NODE) {
+ //Read descriptors values
+ std::string parseStr((char *) descriptor_value_node->children->content);
+ ss.clear();
+ ss.str(parseStr);
+
+ if(!ss.fail()) {
+ switch(descriptorType) {
+ case CV_8U:
+ {
+ //Parse the numeric value [0 ; 255] to an int
+ int parseValue;
+ ss >> parseValue;
+ trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char) parseValue;
+ }
+ break;
+
+ case CV_8S:
+ //Parse the numeric value [-128 ; 127] to an int
+ int parseValue;
+ ss >> parseValue;
+ trainDescriptorsTmp.at<char>(i, j) = (char) parseValue;
+ break;
+
+ case CV_16U:
+ ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
+ break;
+
+ case CV_16S:
+ ss >> trainDescriptorsTmp.at<short int>(i, j);
+ break;
+
+ case CV_32S:
+ ss >> trainDescriptorsTmp.at<int>(i, j);
+ break;
+
+ case CV_32F:
+ ss >> trainDescriptorsTmp.at<float>(i, j);
+ break;
+
+ case CV_64F:
+ ss >> trainDescriptorsTmp.at<double>(i, j);
+ break;
+
+ default:
+ ss >> trainDescriptorsTmp.at<float>(i, j);
+ break;
+ }
+ } else {
+ std::cerr << "Error when converting:" << ss.str() << std::endl;
+ }
+
+ j++;
+ }
+ }
+ }
+ }
+ }
+ i++;
+ }
+ }
+
+ if(!append || m_trainDescriptors.empty()) {
+ trainDescriptorsTmp.copyTo(m_trainDescriptors);
+ } else {
+ cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
+ }
+
+ /*free the document */
+ xmlFreeDoc(doc);
+
+ /*
+ *Free the global variables that may
+ *have been allocated by the parser.
+ */
+ xmlCleanupParser();
+#else
+ std::cout << "Error: libxml2 is required !" << std::endl;
+#endif
+ }
+
+ //Convert OpenCV type to ViSP type for compatibility
+ vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
+ vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
+
+ //Add train descriptors in matcher object
+ m_matcher->clear();
+ m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
+
+ //Set _reference_computed to true as we load learning file
+ _reference_computed = true;
+}
+
+/*!
+ Match keypoints based on distance between their descriptors.
+
+ \param trainDescriptors : Train descriptors (or reference descriptors).
+ \param queryDescriptors : Query descriptors.
+ \param matches : Output list of matches.
+ \param elapsedTime : Elapsed time.
+ */
+void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
+ std::vector<cv::DMatch> &matches, double &elapsedTime) {
+ double t = vpTime::measureTimeMs();
+
+ if(m_useKnn) {
+ m_knnMatches.clear();
+
+ if(m_useMatchTrainToQuery) {
+ std::vector<std::vector<cv::DMatch> > knnMatchesTmp;
+
+ //Match train descriptors to query descriptors
+ cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
+ matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2);
+
+ for(std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) {
+ std::vector<cv::DMatch> tmp;
+ for(std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
+ tmp.push_back(cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance));
+ }
+ m_knnMatches.push_back(tmp);
+ }
+
+ matches.resize(m_knnMatches.size());
+ std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
+ } else {
+ //Match query descriptors to train descriptors
+ m_matcher->knnMatch(queryDescriptors, m_knnMatches, 2);
+ matches.resize(m_knnMatches.size());
+ std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
+ }
+ } else {
+ matches.clear();
+
+ if(m_useMatchTrainToQuery) {
+ std::vector<cv::DMatch> matchesTmp;
+ //Match train descriptors to query descriptors
+ cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
+ matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp);
+
+ for(std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
+ matches.push_back(cv::DMatch(it->trainIdx, it->queryIdx, it->distance));
+ }
+ } else {
+ //Match query descriptors to train descriptors
+ m_matcher->match(queryDescriptors, matches);
+ }
+ }
+ elapsedTime = vpTime::measureTimeMs() - t;
+}
+
+/*!
+ Match keypoints detected in the image with those built in the reference list.
+
+ \param I : Input current image.
+ \return The number of matched keypoints.
+ */
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) {
+ return matchPoint(I, vpRect());
+}
+
+/*!
+ Match keypoints detected in a region of interest of the image with those
+ built in the reference list.
+
+ \param I : Input image
+ \param iP : Coordinate of the top-left corner of the region of interest
+ \param height : Height of the region of interest
+ \param width : Width of the region of interest
+ \return The number of matched keypoints
+ */
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width) {
+ return matchPoint(I, vpRect(iP, width, height));
+}
+
+/*!
+ Match keypoints detected in a region of interest of the image with those
+ built in the reference list.
+
+ \param I : Input image
+ \param rectangle : Rectangle of the region of interest
+ \return The number of matched keypoints
+ */
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
+ const vpRect& rectangle) {
+ if(m_trainDescriptors.empty()) {
+ std::cerr << "Reference is empty." << std::endl;
+ if(!_reference_computed) {
+ std::cerr << "Reference is not computed." << std::endl;
+ }
+ std::cerr << "Matching is not possible." << std::endl;
+
+ return 0;
+ }
+
+ if(m_useAffineDetection) {
+ std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
+ std::vector<cv::Mat> listOfQueryDescriptors;
+
+ //Detect keypoints and extract descriptors on multiple images
+ detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
+
+ //Flatten the different train lists
+ m_queryKeyPoints.clear();
+ for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
+ it != listOfQueryKeyPoints.end(); ++it) {
+ m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
+ }
+
+ bool first = true;
+ for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
+ if(first) {
+ first = false;
+ it->copyTo(m_queryDescriptors);
+ } else {
+ m_queryDescriptors.push_back(*it);
+ }
+ }
+ } else {
+ detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
+ extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
+ }
+
+ match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
+
+ if(m_filterType != noFilterMatching) {
+ m_queryFilteredKeyPoints.clear();
+ m_objectFilteredPoints.clear();
+ m_filteredMatches.clear();
+
+ filterMatches();
+ } else {
+ if(m_useMatchTrainToQuery) {
+ //Add only query keypoints matched with a train keypoints
+ m_queryFilteredKeyPoints.clear();
+ m_filteredMatches.clear();
+ for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
+ m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
+ }
+ } else {
+ m_queryFilteredKeyPoints = m_queryKeyPoints;
+ m_filteredMatches = m_matches;
+ }
+
+ if(!m_trainPoints.empty()) {
+ m_objectFilteredPoints.clear();
+ //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
+ // matches to the same train object
+ for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ //m_matches is normally ordered following the queryDescriptor index
+ m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ }
+ }
+ }
+
+ //Convert OpenCV type to ViSP type for compatibility
+ vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
+ vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
+
+ return static_cast<unsigned int>(m_filteredMatches.size());
+}
+
+/*!
+ Match keypoints detected in the image with those built in the reference list and compute the pose.
+
+ \param I : Input image
+ \param cam : Camera parameters
+ \param cMo : Homogeneous matrix between the object frame and the camera frame
+ \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
+ the estimated pose
+ \param elapsedTime : Time to detect, extract, match and compute the pose
+ \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
+ the poses which do not respect some criterion
+ \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
+ \return True if the matching and the pose estimation are OK, false otherwise
+ */
+bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
+ double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
+ //Check if we have training descriptors
+ if(m_trainDescriptors.empty()) {
+ std::cerr << "Reference is empty." << std::endl;
+ if(!_reference_computed) {
+ std::cerr << "Reference is not computed." << std::endl;
+ }
+ std::cerr << "Matching is not possible." << std::endl;
+
+ return false;
+ }
+
+ if(m_useAffineDetection) {
+ std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
+ std::vector<cv::Mat> listOfQueryDescriptors;
+
+ //Detect keypoints and extract descriptors on multiple images
+ detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
+
+ //Flatten the different train lists
+ m_queryKeyPoints.clear();
+ for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
+ it != listOfQueryKeyPoints.end(); ++it) {
+ m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
+ }
+
+ bool first = true;
+ for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
+ if(first) {
+ first = false;
+ it->copyTo(m_queryDescriptors);
+ } else {
+ m_queryDescriptors.push_back(*it);
+ }
+ }
+ } else {
+ detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
+ extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
+ }
+
+ match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
+
+ elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;
+
+ if(m_filterType != noFilterMatching) {
+ m_queryFilteredKeyPoints.clear();
+ m_objectFilteredPoints.clear();
+ m_filteredMatches.clear();
+
+ filterMatches();
+ } else {
+ if(m_useMatchTrainToQuery) {
+ //Add only query keypoints matched with a train keypoints
+ m_queryFilteredKeyPoints.clear();
+ m_filteredMatches.clear();
+ for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
+ m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
+ }
+ } else {
+ m_queryFilteredKeyPoints = m_queryKeyPoints;
+ m_filteredMatches = m_matches;
+ }
+
+ if(!m_trainPoints.empty()) {
+ m_objectFilteredPoints.clear();
+ //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
+ // matches to the same train object
+ for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ //m_matches is normally ordered following the queryDescriptor index
+ m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ }
+ }
+ }
+
+ //Convert OpenCV type to ViSP type for compatibility
+ vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
+ vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
+
+ //error = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ error = DBL_MAX;
+ m_ransacInliers.clear();
+ m_ransacOutliers.clear();
+
+ if(m_useRansacVVS) {
+ std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
+ size_t cpt = 0;
+ //Create a list of vpPoint with 2D coordinates (current keypoint location) + 3D coordinates (world/object coordinates)
+ for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
+ ++it, cpt++) {
+ vpPoint pt;
+ pt.setWorldCoordinates(it->x, it->y, it->z);
+
+ vpImagePoint imP(m_queryFilteredKeyPoints[cpt].pt.y, m_queryFilteredKeyPoints[cpt].pt.x);
+
+ double x = 0.0, y = 0.0;
+ vpPixelMeterConversion::convertPoint(cam, imP, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+
+ objectVpPoints[cpt] = pt;
+ }
+
+ std::vector<vpPoint> inliers;
+ std::vector<unsigned int> inlierIndex;
+
+ bool res = computePose(objectVpPoints, cMo, inliers, inlierIndex, m_poseTime, func);
+
+ std::map<unsigned int, bool> mapOfInlierIndex;
+ m_matchRansacKeyPointsToPoints.clear();
+
+ for (std::vector<unsigned int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
+ m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
+ m_objectFilteredPoints[(size_t)(*it)]));
+ mapOfInlierIndex[*it] = true;
+ }
+
+ for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
+ if(mapOfInlierIndex.find((unsigned int) i) == mapOfInlierIndex.end()) {
+ m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
+ }
+ }
+
+ error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
+
+ m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
+ std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
+ matchRansacToVpImage);
+
+ elapsedTime += m_poseTime;
+
+ return res;
+ } else {
+ std::vector<cv::Point2f> imageFilteredPoints;
+ cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints);
+ std::vector<int> inlierIndex;
+ bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime);
+
+ std::map<int, bool> mapOfInlierIndex;
+ m_matchRansacKeyPointsToPoints.clear();
+
+ for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
+ m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
+ m_objectFilteredPoints[(size_t)(*it)]));
+ mapOfInlierIndex[*it] = true;
+ }
+
+ for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
+ if(mapOfInlierIndex.find((int) i) == mapOfInlierIndex.end()) {
+ m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
+ }
+ }
+
+ error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
+
+ m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
+ std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
+ matchRansacToVpImage);
+
+ elapsedTime += m_poseTime;
+
+ return res;
+ }
+}
+
+/*!
+ Match keypoints detected in the image with those built in the reference list and return the bounding box and the center
+ of gravity.
+
+ \param I : Input image
+ \param boundingBox : Bounding box that contains the good matches
+ \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
+ the bounding box)
+ \param isPlanarObject : If the object is planar, the homography matrix is estimated to eliminate outliers, otherwise
+ it is the fundamental matrix which is estimated
+ \param imPts1 : Pointer to the list of reference keypoints if not null
+ \param imPts2 : Pointer to the list of current keypoints if not null
+ \param meanDescriptorDistance : Pointer to the value of the average distance of the descriptors if not null
+ \param detection_score : Pointer to the value of the detection score if not null
+ \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
+ \return True if the object is present, false otherwise
+ */
+bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
+ const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
+ double *meanDescriptorDistance, double *detection_score, const vpRect& rectangle) {
+ if(imPts1 != NULL && imPts2 != NULL) {
+ imPts1->clear();
+ imPts2->clear();
+ }
+
+ matchPoint(I, rectangle);
+
+ double meanDescriptorDistanceTmp = 0.0;
+ for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ meanDescriptorDistanceTmp += (double) it->distance;
+ }
+
+ meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
+ double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;
+
+ if(meanDescriptorDistance != NULL) {
+ *meanDescriptorDistance = meanDescriptorDistanceTmp;
+ }
+ if(detection_score != NULL) {
+ *detection_score = score;
+ }
+
+ if(m_filteredMatches.size() >= 4) {
+ //Training / Reference 2D points
+ std::vector<cv::Point2f> points1(m_filteredMatches.size());
+ //Query / Current 2D points
+ std::vector<cv::Point2f> points2(m_filteredMatches.size());
+
+ for(size_t i = 0; i < m_filteredMatches.size(); i++) {
+ points1[i] = cv::Point2f(m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx].pt);
+ points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx].pt);
+ }
+
+ std::vector<vpImagePoint> inliers;
+ if(isPlanarObject) {
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
+#else
+ cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
+#endif
+
+ for(size_t i = 0; i < m_filteredMatches.size(); i++ ) {
+ //Compute reprojection error
+ cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
+ realPoint.at<double>(0,0) = points1[i].x;
+ realPoint.at<double>(1,0) = points1[i].y;
+ realPoint.at<double>(2,0) = 1.f;
+
+ cv::Mat reprojectedPoint = homographyMatrix * realPoint;
+ double err_x = (reprojectedPoint.at<double>(0,0) / reprojectedPoint.at<double>(2,0)) - points2[i].x;
+ double err_y = (reprojectedPoint.at<double>(1,0) / reprojectedPoint.at<double>(2,0)) - points2[i].y;
+ double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);
+
+ if(reprojectionError < 6.0) {
+ inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ if(imPts1 != NULL) {
+ imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
+ }
+
+ if(imPts2 != NULL) {
+ imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ }
+ }
+ }
+ } else if(m_filteredMatches.size() >= 8) {
+ cv::Mat fundamentalInliers;
+ cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);
+
+ for(size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
+ if(fundamentalInliers.at<uchar>((int) i, 0)) {
+ inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+
+ if(imPts1 != NULL) {
+ imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
+ }
+
+ if(imPts2 != NULL) {
+ imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ }
+ }
+ }
+ }
+
+ if(!inliers.empty()) {
+ //Build a polygon with the list of inlier keypoints detected in the current image to get the bounding box
+ vpPolygon polygon(inliers);
+ boundingBox = polygon.getBoundingBox();
+
+ //Compute the center of gravity
+ double meanU = 0.0, meanV = 0.0;
+ for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
+ meanU += it->get_u();
+ meanV += it->get_v();
+ }
+
+ meanU /= (double) inliers.size();
+ meanV /= (double) inliers.size();
+
+ centerOfGravity.set_u(meanU);
+ centerOfGravity.set_v(meanV);
+ }
+ } else {
+ //Too few matches
+ return false;
+ }
+
+ if(m_detectionMethod == detectionThreshold) {
+ return meanDescriptorDistanceTmp < m_detectionThreshold;
+ } else {
+ return score > m_detectionScore;
+ }
+}
+
+/*!
+ Match keypoints detected in the image with those built in the reference list, compute the pose and return also
+ the bounding box and the center of gravity.
+
+ \param I : Input image
+ \param cam : Camera parameters
+ \param cMo : Homogeneous matrix between the object frame and the camera frame
+ \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
+ the estimated pose
+ \param elapsedTime : Time to detect, extract, match and compute the pose
+ \param boundingBox : Bounding box that contains the good matches
+ \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
+ the bounding box)
+ \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
+ the poses which do not respect some criterion
+ \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
+ \return True if the matching and the pose estimation are OK, false otherwise.
+ */
+bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
+ double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
+ bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
+ bool isMatchOk = matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
+ if(isMatchOk) {
+ //Use the pose estimated to project the model points in the image
+ vpPoint pt;
+ vpImagePoint imPt;
+ std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
+ size_t cpt = 0;
+ for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
+ pt = *it;
+ pt.project(cMo);
+ vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), imPt);
+ modelImagePoints[cpt] = imPt;
+ }
+
+ //Build a polygon with the list of model image points to get the bounding box
+ vpPolygon polygon(modelImagePoints);
+ boundingBox = polygon.getBoundingBox();
+
+ //Compute the center of gravity of the current inlier keypoints
+ double meanU = 0.0, meanV = 0.0;
+ for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
+ ++it) {
+ meanU += it->get_u();
+ meanV += it->get_v();
+ }
+
+ meanU /= (double) m_ransacInliers.size();
+ meanV /= (double) m_ransacInliers.size();
+
+ centerOfGravity.set_u(meanU);
+ centerOfGravity.set_v(meanV);
+ }
+
+ return isMatchOk;
+}
+
+/*!
+ Apply a set of affine transormations to the image, detect keypoints and
+ reproject them into initial image coordinates.
+ See http://www.ipol.im/pub/algo/my_affine_sift/ for the details.
+ See https://github.com/Itseez/opencv/blob/master/samples/python2/asift.py for the Python implementation by Itseez
+ and Matt Sheckells for the current implementation in C++.
+ \param I : Input image
+ \param listOfKeypoints : List of detected keypoints in the multiple images after affine transformations
+ \param listOfDescriptors : Corresponding list of descriptors
+ \param listOfAffineI : Optional parameter, list of images after affine transformations
+ */
+void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
+ std::vector<cv::Mat>& listOfDescriptors, std::vector<vpImage<unsigned char> > *listOfAffineI) {
+#if 0
+ cv::Mat img;
+ vpImageConvert::convert(I, img);
+ listOfKeypoints.clear();
+ listOfDescriptors.clear();
+
+ for (int tl = 1; tl < 6; tl++) {
+ double t = pow(2, 0.5 * tl);
+ for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
+ std::vector<cv::KeyPoint> keypoints;
+ cv::Mat descriptors;
+
+ cv::Mat timg, mask, Ai;
+ img.copyTo(timg);
+
+ affineSkew(t, phi, timg, mask, Ai);
+
+
+ if(listOfAffineI != NULL) {
+ cv::Mat img_disp;
+ bitwise_and(mask, timg, img_disp);
+ vpImage<unsigned char> tI;
+ vpImageConvert::convert(img_disp, tI);
+ listOfAffineI->push_back(tI);
+ }
+#if 0
+ cv::Mat img_disp;
+ cv::bitwise_and(mask, timg, img_disp);
+ cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
+ cv::imshow( "Skew", img_disp );
+ cv::waitKey(0);
+#endif
+
+ for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
+ it != m_detectors.end(); ++it) {
+ std::vector<cv::KeyPoint> kp;
+ it->second->detect(timg, kp, mask);
+ keypoints.insert(keypoints.end(), kp.begin(), kp.end());
+ }
+
+ double elapsedTime;
+ extract(timg, keypoints, descriptors, elapsedTime);
+
+ for(unsigned int i = 0; i < keypoints.size(); i++) {
+ cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
+ cv::Mat kpt_t = Ai * cv::Mat(kpt);
+ keypoints[i].pt.x = kpt_t.at<float>(0, 0);
+ keypoints[i].pt.y = kpt_t.at<float>(1, 0);
+ }
+
+ listOfKeypoints.push_back(keypoints);
+ listOfDescriptors.push_back(descriptors);
+ }
+ }
+
+#else
+ cv::Mat img;
+ vpImageConvert::convert(I, img);
+
+ //Create a vector for storing the affine skew parameters
+ std::vector<std::pair<double, int> > listOfAffineParams;
+ for (int tl = 1; tl < 6; tl++) {
+ double t = pow(2, 0.5 * tl);
+ for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
+ listOfAffineParams.push_back(std::pair<double, int>(t, phi));
+ }
+ }
+
+ listOfKeypoints.resize(listOfAffineParams.size());
+ listOfDescriptors.resize(listOfAffineParams.size());
+
+ if(listOfAffineI != NULL) {
+ listOfAffineI->resize(listOfAffineParams.size());
+ }
+
+#ifdef VISP_HAVE_OPENMP
+ #pragma omp parallel for
+#endif
+ for(int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
+ std::vector<cv::KeyPoint> keypoints;
+ cv::Mat descriptors;
+
+ cv::Mat timg, mask, Ai;
+ img.copyTo(timg);
+
+ affineSkew(listOfAffineParams[(size_t) cpt].first, listOfAffineParams[(size_t) cpt].second, timg, mask, Ai);
+
+
+ if(listOfAffineI != NULL) {
+ cv::Mat img_disp;
+ bitwise_and(mask, timg, img_disp);
+ vpImage<unsigned char> tI;
+ vpImageConvert::convert(img_disp, tI);
+ (*listOfAffineI)[(size_t) cpt] = tI;
+ }
+
+#if 0
+ cv::Mat img_disp;
+ cv::bitwise_and(mask, timg, img_disp);
+ cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
+ cv::imshow( "Skew", img_disp );
+ cv::waitKey(0);
+#endif
+
+ for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
+ it != m_detectors.end(); ++it) {
+ std::vector<cv::KeyPoint> kp;
+ it->second->detect(timg, kp, mask);
+ keypoints.insert(keypoints.end(), kp.begin(), kp.end());
+ }
+
+ double elapsedTime;
+ extract(timg, keypoints, descriptors, elapsedTime);
+
+ for(size_t i = 0; i < keypoints.size(); i++) {
+ cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
+ cv::Mat kpt_t = Ai * cv::Mat(kpt);
+ keypoints[i].pt.x = kpt_t.at<float>(0, 0);
+ keypoints[i].pt.y = kpt_t.at<float>(1, 0);
+ }
+
+ listOfKeypoints[(size_t) cpt] = keypoints;
+ listOfDescriptors[(size_t) cpt] = descriptors;
+ }
+#endif
+}
+
+/*!
+ Reset the instance as if we would declare another vpKeyPoint variable.
+ */
+void vpKeyPoint::reset() {
+ //vpBasicKeyPoint class
+ referenceImagePointsList.clear(); currentImagePointsList.clear(); matchedReferencePoints.clear(); _reference_computed = false;
+
+
+ m_computeCovariance = false; m_covarianceMatrix = vpMatrix(); m_currentImageId = 0; m_detectionMethod = detectionScore;
+ m_detectionScore = 0.15; m_detectionThreshold = 100.0; m_detectionTime = 0.0; m_detectorNames.clear();
+ m_detectors.clear(); m_extractionTime = 0.0; m_extractorNames.clear(); m_extractors.clear(); m_filteredMatches.clear();
+ m_filterType = ratioDistanceThreshold;
+ m_imageFormat = jpgImageFormat; m_knnMatches.clear(); m_mapOfImageId.clear(); m_mapOfImages.clear();
+ m_matcher = cv::Ptr<cv::DescriptorMatcher>(); m_matcherName = "BruteForce-Hamming";
+ m_matches.clear(); m_matchingFactorThreshold = 2.0; m_matchingRatioThreshold = 0.85; m_matchingTime = 0.0;
+ m_matchRansacKeyPointsToPoints.clear(); m_nbRansacIterations = 200; m_nbRansacMinInlierCount = 100;
+ m_objectFilteredPoints.clear();
+ m_poseTime = 0.0; m_queryDescriptors = cv::Mat(); m_queryFilteredKeyPoints.clear(); m_queryKeyPoints.clear();
+ m_ransacConsensusPercentage = 20.0; m_ransacInliers.clear(); m_ransacOutliers.clear(); m_ransacReprojectionError = 6.0;
+ m_ransacThreshold = 0.01; m_trainDescriptors = cv::Mat(); m_trainKeyPoints.clear(); m_trainPoints.clear();
+ m_trainVpPoints.clear(); m_useAffineDetection = false;
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ m_useBruteForceCrossCheck = true;
+#endif
+ m_useConsensusPercentage = false;
+ m_useKnn = true; //as m_filterType == ratioDistanceThreshold
+ m_useMatchTrainToQuery = false; m_useRansacVVS = true; m_useSingleMatchFilter = true;
+
+ m_detectorNames.push_back("ORB");
+ m_extractorNames.push_back("ORB");
+
+ init();
+}
+
+/*!
+ Save the learning data in a file in XML or binary mode.
+
+ \param filename : Path of the save file
+ \param binaryMode : If true, the data are saved in binary mode, otherwise in XML mode
+ \param saveTrainingImages : If true, save also the training images on disk
+ */
+void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) {
+ std::string parent = vpIoTools::getParent(filename);
+ if(!parent.empty()) {
+ vpIoTools::makeDirectory(parent);
+ }
+
+ std::map<int, std::string> mapOfImgPath;
+ if(saveTrainingImages) {
+#ifdef VISP_HAVE_MODULE_IO
+ //Save the training image files in the same directory
+ int cpt = 0;
+
+ for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
+ if(cpt > 999) {
+ throw vpException(vpException::fatalError, "The number of training images to save is too big !");
+ }
+
+ char buffer[4];
+ sprintf(buffer, "%03d", cpt);
+ std::stringstream ss;
+ ss << "train_image_" << buffer;
+
+ switch(m_imageFormat) {
+ case jpgImageFormat:
+ ss << ".jpg";
+ break;
+
+ case pngImageFormat:
+ ss << ".png";
+ break;
+
+ case ppmImageFormat:
+ ss << ".ppm";
+ break;
+
+ case pgmImageFormat:
+ ss << ".pgm";
+ break;
+
+ default:
+ ss << ".png";
+ break;
+ }
+
+ std::string imgFilename = ss.str();
+ mapOfImgPath[it->first] = imgFilename;
+ vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + imgFilename);
+ }
+#else
+ std::cout << "Warning: in vpKeyPoint::saveLearningData() training images are not saved because "
+ "visp_io module is not available !" << std::endl;
+#endif
+ }
+
+ bool have3DInfo = m_trainPoints.size() > 0;
+ if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
+ throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !");
+ }
+
+ if(binaryMode) {
+ //Save the learning data into little endian binary file.
+ std::ofstream file(filename.c_str(), std::ofstream::binary);
+ if(!file.is_open()) {
+ throw vpException(vpException::ioError, "Cannot create the file.");
+ }
+
+ //Write info about training images
+ int nbImgs = (int) mapOfImgPath.size();
+// file.write((char *)(&nbImgs), sizeof(nbImgs));
+ writeBinaryIntLE(file, nbImgs);
+
+#ifdef VISP_HAVE_MODULE_IO
+ for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
+ //Write image_id
+ int id = it->first;
+// file.write((char *)(&id), sizeof(id));
+ writeBinaryIntLE(file, id);
+
+ //Write image path
+ std::string path = it->second;
+ int length = (int) path.length();
+// file.write((char *)(&length), sizeof(length));
+ writeBinaryIntLE(file, length);
+
+ for(int cpt = 0; cpt < length; cpt++) {
+ file.write((char *) (&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
+ }
+ }
+#endif
+
+ //Write if we have 3D point information
+ int have3DInfoInt = have3DInfo ? 1 : 0;
+// file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
+ writeBinaryIntLE(file, have3DInfoInt);
+
+
+ int nRows = m_trainDescriptors.rows,
+ nCols = m_trainDescriptors.cols;
+ int descriptorType = m_trainDescriptors.type();
+
+ //Write the number of descriptors
+// file.write((char *)(&nRows), sizeof(nRows));
+ writeBinaryIntLE(file, nRows);
+
+ //Write the size of the descriptor
+// file.write((char *)(&nCols), sizeof(nCols));
+ writeBinaryIntLE(file, nCols);
+
+ //Write the type of the descriptor
+// file.write((char *)(&descriptorType), sizeof(descriptorType));
+ writeBinaryIntLE(file, descriptorType);
+
+ for (int i = 0; i < nRows; i++) {
+ unsigned int i_ = (unsigned int) i;
+ //Write u
+ float u = m_trainKeyPoints[i_].pt.x;
+// file.write((char *)(&u), sizeof(u));
+ writeBinaryFloatLE(file, u);
+
+ //Write v
+ float v = m_trainKeyPoints[i_].pt.y;
+// file.write((char *)(&v), sizeof(v));
+ writeBinaryFloatLE(file, v);
+
+ //Write size
+ float size = m_trainKeyPoints[i_].size;
+// file.write((char *)(&size), sizeof(size));
+ writeBinaryFloatLE(file, size);
+
+ //Write angle
+ float angle = m_trainKeyPoints[i_].angle;
+// file.write((char *)(&angle), sizeof(angle));
+ writeBinaryFloatLE(file, angle);
+
+ //Write response
+ float response = m_trainKeyPoints[i_].response;
+// file.write((char *)(&response), sizeof(response));
+ writeBinaryFloatLE(file, response);
+
+ //Write octave
+ int octave = m_trainKeyPoints[i_].octave;
+// file.write((char *)(&octave), sizeof(octave));
+ writeBinaryIntLE(file, octave);
+
+ //Write class_id
+ int class_id = m_trainKeyPoints[i_].class_id;
+// file.write((char *)(&class_id), sizeof(class_id));
+ writeBinaryIntLE(file, class_id);
+
+ //Write image_id
+#ifdef VISP_HAVE_MODULE_IO
+ std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
+ int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1;
+// file.write((char *)(&image_id), sizeof(image_id));
+ writeBinaryIntLE(file, image_id);
+#else
+ int image_id = -1;
+// file.write((char *)(&image_id), sizeof(image_id));
+ writeBinaryIntLE(file, image_id);
+#endif
+
+ if(have3DInfo) {
+ float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
+ //Write oX
+// file.write((char *)(&oX), sizeof(oX));
+ writeBinaryFloatLE(file, oX);
+
+ //Write oY
+// file.write((char *)(&oY), sizeof(oY));
+ writeBinaryFloatLE(file, oY);
+
+ //Write oZ
+// file.write((char *)(&oZ), sizeof(oZ));
+ writeBinaryFloatLE(file, oZ);
+ }
+
+ for (int j = 0; j < nCols; j++) {
+ //Write the descriptor value
+ switch(descriptorType) {
+ case CV_8U:
+ file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)), sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
+ break;
+
+ case CV_8S:
+ file.write((char *)(&m_trainDescriptors.at<char>(i, j)), sizeof(m_trainDescriptors.at<char>(i, j)));
+ break;
+
+ case CV_16U:
+// file.write((char *)(&m_trainDescriptors.at<unsigned short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned short int>(i, j)));
+ writeBinaryUShortLE(file, m_trainDescriptors.at<unsigned short int>(i, j));
+ break;
+
+ case CV_16S:
+// file.write((char *)(&m_trainDescriptors.at<short int>(i, j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
+ writeBinaryShortLE(file, m_trainDescriptors.at<short int>(i, j));
+ break;
+
+ case CV_32S:
+// file.write((char *)(&m_trainDescriptors.at<int>(i, j)), sizeof(m_trainDescriptors.at<int>(i, j)));
+ writeBinaryIntLE(file, m_trainDescriptors.at<int>(i, j));
+ break;
+
+ case CV_32F:
+// file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
+ writeBinaryFloatLE(file, m_trainDescriptors.at<float>(i, j));
+ break;
+
+ case CV_64F:
+// file.write((char *)(&m_trainDescriptors.at<double>(i, j)), sizeof(m_trainDescriptors.at<double>(i, j)));
+ writeBinaryDoubleLE(file, m_trainDescriptors.at<double>(i, j));
+ break;
+
+ default:
+ throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
+ break;
+ }
+ }
+ }
+
+
+ file.close();
+ } else {
+#ifdef VISP_HAVE_XML2
+ xmlDocPtr doc = NULL;
+ xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
+ descriptor_node = NULL, desc_node = NULL;
+
+ /*
+ * this initialize the library and check potential ABI mismatches
+ * between the version it was compiled for and the actual shared
+ * library used.
+ */
+ LIBXML_TEST_VERSION
+
+ doc = xmlNewDoc(BAD_CAST "1.0");
+ if (doc == NULL) {
+ throw vpException(vpException::ioError, "Error with file " + filename);
+ }
+
+ root_node = xmlNewNode(NULL, BAD_CAST "LearningData");
+ xmlDocSetRootElement(doc, root_node);
+
+ std::stringstream ss;
+
+ //Write the training images info
+ image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL);
+
+#ifdef VISP_HAVE_MODULE_IO
+ for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
+ image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg",
+ BAD_CAST it->second.c_str());
+ ss.str("");
+ ss << it->first;
+ xmlNewProp(image_info_node, BAD_CAST "image_id", BAD_CAST ss.str().c_str());
+ }
+#endif
+
+ //Write information about descriptors
+ descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorsInfo", NULL);
+
+ int nRows = m_trainDescriptors.rows,
+ nCols = m_trainDescriptors.cols;
+ int descriptorType = m_trainDescriptors.type();
+
+ //Write the number of rows
+ ss.str("");
+ ss << nRows;
+ xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", BAD_CAST ss.str().c_str());
+
+ //Write the number of cols
+ ss.str("");
+ ss << nCols;
+ xmlNewChild(descriptors_info_node, NULL, BAD_CAST "ncols", BAD_CAST ss.str().c_str());
+
+ //Write the descriptors type
+ ss.str("");
+ ss << descriptorType;
+ xmlNewChild(descriptors_info_node, NULL, BAD_CAST "type", BAD_CAST ss.str().c_str());
+
+ for (int i = 0; i < nRows; i++) {
+ unsigned int i_ = (unsigned int) i;
+ descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo",
+ NULL);
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.x;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "u",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.y;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "v",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].size;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "size",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].angle;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "angle",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].response;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "response",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ ss << m_trainKeyPoints[i_].octave;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "octave",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ ss << m_trainKeyPoints[i_].class_id;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "class_id",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+#ifdef VISP_HAVE_MODULE_IO
+ std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
+ ss << ((saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1);
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
+ BAD_CAST ss.str().c_str());
+#else
+ ss << -1;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
+ BAD_CAST ss.str().c_str());
+#endif
+
+ if (have3DInfo) {
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].x;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "oX",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].y;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "oY",
+ BAD_CAST ss.str().c_str());
+
+ ss.str("");
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].z;
+ xmlNewChild(descriptor_node, NULL, BAD_CAST "oZ",
+ BAD_CAST ss.str().c_str());
+ }
+
+ desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST "desc", NULL);
+
+ for (int j = 0; j < nCols; j++) {
+ ss.str("");
+
+ switch(descriptorType) {
+ case CV_8U:
+ {
+ //Promote an unsigned char to an int
+ //val_tmp holds the numeric value that will be written
+ //We save the value in numeric form otherwise libxml2 will not be able to parse
+ //A better solution could be possible
+ int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
+ ss << val_tmp;
+ }
+ break;
+
+ case CV_8S:
+ {
+ //Promote a char to an int
+ //val_tmp holds the numeric value that will be written
+ //We save the value in numeric form otherwise libxml2 will not be able to parse
+ //A better solution could be possible
+ int val_tmp = m_trainDescriptors.at<char>(i, j);
+ ss << val_tmp;
+ }
+ break;
+
+ case CV_16U:
+ ss << m_trainDescriptors.at<unsigned short int>(i, j);
+ break;
+
+ case CV_16S:
+ ss << m_trainDescriptors.at<short int>(i, j);
+ break;
+
+ case CV_32S:
+ ss << m_trainDescriptors.at<int>(i, j);
+ break;
+
+ case CV_32F:
+ //max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<float>(i, j);
+ break;
+
+ case CV_64F:
+ //max_digits10 == 17 for double
+ ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<double>(i, j);
+ break;
+
+ default:
+ throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
+ break;
+ }
+ xmlNewChild(desc_node, NULL, BAD_CAST "val",
+ BAD_CAST ss.str().c_str());
+ }
+ }
+
+ xmlSaveFormatFileEnc(filename.c_str(), doc, "UTF-8", 1);
+
+ /*free the document */
+ xmlFreeDoc(doc);
+
+ /*
+ *Free the global variables that may
+ *have been allocated by the parser.
+ */
+ xmlCleanupParser();
+#else
+ std::cerr << "Error: libxml2 is required !" << std::endl;
+#endif
+ }
+}
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+//From OpenCV 2.4.11 source code.
+struct KeypointResponseGreaterThanThreshold {
+ KeypointResponseGreaterThanThreshold(float _value) :
+ value(_value) {
+ }
+ inline bool operator()(const cv::KeyPoint& kpt) const {
+ return kpt.response >= value;
+ }
+ float value;
+};
+
+struct KeypointResponseGreater {
+ inline bool operator()(const cv::KeyPoint& kp1,
+ const cv::KeyPoint& kp2) const {
+ return kp1.response > kp2.response;
+ }
+};
+
+// takes keypoints and culls them by the response
+void vpKeyPoint::KeyPointsFilter::retainBest(
+ std::vector<cv::KeyPoint>& keypoints, int n_points) {
+ //this is only necessary if the keypoints size is greater than the number of desired points.
+ if (n_points >= 0 && keypoints.size() > (size_t) n_points) {
+ if (n_points == 0) {
+ keypoints.clear();
+ return;
+ }
+ //first use nth element to partition the keypoints into the best and worst.
+ std::nth_element(keypoints.begin(), keypoints.begin() + n_points,
+ keypoints.end(), KeypointResponseGreater());
+ //this is the boundary response, and in the case of FAST may be ambiguous
+ float ambiguous_response = keypoints[(size_t) (n_points - 1)].response;
+ //use std::partition to grab all of the keypoints with the boundary response.
+ std::vector<cv::KeyPoint>::const_iterator new_end = std::partition(
+ keypoints.begin() + n_points, keypoints.end(),
+ KeypointResponseGreaterThanThreshold(ambiguous_response));
+ //resize the keypoints, given this new end point. nth_element and partition reordered the points inplace
+ keypoints.resize((size_t) (new_end - keypoints.begin()));
+ }
+}
+
+struct RoiPredicate {
+ RoiPredicate(const cv::Rect& _r) :
+ r(_r) {
+ }
+
+ bool operator()(const cv::KeyPoint& keyPt) const {
+ return !r.contains(keyPt.pt);
+ }
+
+ cv::Rect r;
+};
+
+void vpKeyPoint::KeyPointsFilter::runByImageBorder(
+ std::vector<cv::KeyPoint>& keypoints, cv::Size imageSize, int borderSize) {
+ if (borderSize > 0) {
+ if (imageSize.height <= borderSize * 2 || imageSize.width <= borderSize * 2)
+ keypoints.clear();
+ else
+ keypoints.erase(
+ std::remove_if(keypoints.begin(), keypoints.end(),
+ RoiPredicate(
+ cv::Rect(cv::Point(borderSize, borderSize),
+ cv::Point(imageSize.width - borderSize,
+ imageSize.height - borderSize)))), keypoints.end());
+ }
+}
+
+struct SizePredicate {
+ SizePredicate(float _minSize, float _maxSize) :
+ minSize(_minSize), maxSize(_maxSize) {
+ }
+
+ bool operator()(const cv::KeyPoint& keyPt) const {
+ float size = keyPt.size;
+ return (size < minSize) || (size > maxSize);
+ }
+
+ float minSize, maxSize;
+};
+
+void vpKeyPoint::KeyPointsFilter::runByKeypointSize(
+ std::vector<cv::KeyPoint>& keypoints, float minSize, float maxSize) {
+ CV_Assert(minSize >= 0);
+ CV_Assert(maxSize >= 0);
+ CV_Assert(minSize <= maxSize);
+
+ keypoints.erase(
+ std::remove_if(keypoints.begin(), keypoints.end(),
+ SizePredicate(minSize, maxSize)), keypoints.end());
+}
+
+class MaskPredicate {
+public:
+ MaskPredicate(const cv::Mat& _mask) :
+ mask(_mask) {
+ }
+ bool operator()(const cv::KeyPoint& key_pt) const {
+ return mask.at<uchar>((int) (key_pt.pt.y + 0.5f),
+ (int) (key_pt.pt.x + 0.5f)) == 0;
+ }
+
+private:
+ const cv::Mat mask;
+ MaskPredicate& operator=(const MaskPredicate&);
+};
+
+void vpKeyPoint::KeyPointsFilter::runByPixelsMask(
+ std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) {
+ if (mask.empty())
+ return;
+
+ keypoints.erase(
+ std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)),
+ keypoints.end());
+}
+
+struct KeyPoint_LessThan {
+ KeyPoint_LessThan(const std::vector<cv::KeyPoint>& _kp) :
+ kp(&_kp) {
+ }
+ bool operator()(/*int i, int j*/ size_t i, size_t j) const {
+ const cv::KeyPoint& kp1 = (*kp)[/*(size_t)*/ i];
+ const cv::KeyPoint& kp2 = (*kp)[/*(size_t)*/ j];
+ if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.x != kp2.pt.x) {
+ return kp1.pt.x < kp2.pt.x;
+ }
+
+ if (!vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.y != kp2.pt.y) {
+ return kp1.pt.y < kp2.pt.y;
+ }
+
+ if (!vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon())) { //if (kp1.size != kp2.size) {
+ return kp1.size > kp2.size;
+ }
+
+ if (!vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) { //if (kp1.angle != kp2.angle) {
+ return kp1.angle < kp2.angle;
+ }
+
+ if (!vpMath::equal(kp1.response, kp2.response, std::numeric_limits<float>::epsilon())) { //if (kp1.response != kp2.response) {
+ return kp1.response > kp2.response;
+ }
+
+ if (kp1.octave != kp2.octave) {
+ return kp1.octave > kp2.octave;
+ }
+
+ if (kp1.class_id != kp2.class_id) {
+ return kp1.class_id > kp2.class_id;
+ }
+
+ return i < j;
+ }
+ const std::vector<cv::KeyPoint>* kp;
+};
+
+void vpKeyPoint::KeyPointsFilter::removeDuplicated(
+ std::vector<cv::KeyPoint>& keypoints) {
+ size_t i, j, n = keypoints.size();
+ std::vector<size_t> kpidx(n);
+ std::vector<uchar> mask(n, (uchar) 1);
+
+ for (i = 0; i < n; i++) {
+ kpidx[i] = i;
+ }
+ std::sort(kpidx.begin(), kpidx.end(), KeyPoint_LessThan(keypoints));
+ for (i = 1, j = 0; i < n; i++) {
+ cv::KeyPoint& kp1 = keypoints[kpidx[i]];
+ cv::KeyPoint& kp2 = keypoints[kpidx[j]];
+// if (kp1.pt.x != kp2.pt.x || kp1.pt.y != kp2.pt.y || kp1.size != kp2.size || kp1.angle != kp2.angle) {
+ if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon()) ||
+ !vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon()) ||
+ !vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon()) ||
+ !vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) {
+ j = i;
+ } else {
+ mask[kpidx[i]] = 0;
+ }
+ }
+
+ for (i = j = 0; i < n; i++) {
+ if (mask[i]) {
+ if (i != j) {
+ keypoints[j] = keypoints[i];
+ }
+ j++;
+ }
+ }
+ keypoints.resize(j);
+}
+
+/*
+ * PyramidAdaptedFeatureDetector
+ */
+vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(
+ const cv::Ptr<cv::FeatureDetector>& _detector, int _maxLevel) :
+ detector(_detector), maxLevel(_maxLevel) {
+}
+
+bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const {
+ return detector.empty() || (cv::FeatureDetector*) detector->empty();
+}
+
+void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( cv::InputArray image, CV_OUT std::vector<cv::KeyPoint>& keypoints,
+ cv::InputArray mask ) {
+ detectImpl(image.getMat(), keypoints, mask.getMat());
+}
+
+void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
+ std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const {
+ cv::Mat src = image;
+ cv::Mat src_mask = mask;
+
+ cv::Mat dilated_mask;
+ if (!mask.empty()) {
+ cv::dilate(mask, dilated_mask, cv::Mat());
+ cv::Mat mask255(mask.size(), CV_8UC1, cv::Scalar(0));
+ mask255.setTo(cv::Scalar(255), dilated_mask != 0);
+ dilated_mask = mask255;
+ }
+
+ for (int l = 0, multiplier = 1; l <= maxLevel; ++l, multiplier *= 2) {
+ // Detect on current level of the pyramid
+ std::vector<cv::KeyPoint> new_pts;
+ detector->detect(src, new_pts, src_mask);
+ std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end =
+ new_pts.end();
+ for (; it != end; ++it) {
+ it->pt.x *= multiplier;
+ it->pt.y *= multiplier;
+ it->size *= multiplier;
+ it->octave = l;
+ }
+ keypoints.insert(keypoints.end(), new_pts.begin(), new_pts.end());
+
+ // Downsample
+ if (l < maxLevel) {
+ cv::Mat dst;
+ pyrDown(src, dst);
+ src = dst;
+
+ if (!mask.empty())
+ resize(dilated_mask, src_mask, src.size(), 0, 0, CV_INTER_AREA);
+ }
+ }
+
+ if (!mask.empty())
+ vpKeyPoint::KeyPointsFilter::runByPixelsMask(keypoints, mask);
+}
+#endif
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work around to avoid warning: libvisp_vision.a(vpKeyPoint.cpp.o) has no symbols
+void dummy_vpKeyPoint() {};
+#endif
diff --git a/modules/vision/src/key-point/vpKeyPointSurf.cpp b/modules/vision/src/key-point/vpKeyPointSurf.cpp
new file mode 100644
index 0000000..6ec647f
--- /dev/null
+++ b/modules/vision/src/key-point/vpKeyPointSurf.cpp
@@ -0,0 +1,722 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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 Surf.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#include <visp3/vision/vpKeyPointSurf.h>
+
+#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x010100) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 1.1.0 < 3.0.0
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpDebug.h>
+
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <list>
+
+double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
+int naiveNearestNeighbor( const float *vec, int laplacian,
+ const CvSeq *model_keypoints,
+ const CvSeq *model_descriptors );
+int naiveNearestNeighbor( const float *vec,
+ const CvSeq *ref_keypoints,
+ const CvSeq *ref_descriptors );
+void findPairs( const CvSeq* objectKeypoints,
+ const CvSeq* objectDescriptors,
+ const CvSeq* imageKeypoints,
+ const CvSeq* imageDescriptors,
+ std::vector<int>& ptpairs );
+
+// Compare two surf descriptors.
+double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
+{
+ double total_cost = 0;
+ int i;
+ assert( length % 4 == 0 );
+ for( i = 0; i < length; i += 4 )
+ {
+ double t0 = d1[i] - d2[i];
+ double t1 = d1[i+1] - d2[i+1];
+ double t2 = d1[i+2] - d2[i+2];
+ double t3 = d1[i+3] - d2[i+3];
+ total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
+ if( total_cost > best )
+ break;
+ }
+ return total_cost;
+}
+
+
+//Find for a point candidate the most similar point in the reference point list.
+int naiveNearestNeighbor( const float *vec, int laplacian,
+ const CvSeq *model_keypoints,
+ const CvSeq *model_descriptors )
+{
+ int length = (int)(model_descriptors->elem_size/(int)sizeof(float));
+ int i, neighbor = -1;
+ double d, dist1 = 1e6, dist2 = 1e6;
+ CvSeqReader reader, kreader;
+ cvStartReadSeq( model_keypoints, &kreader, 0 );
+ cvStartReadSeq( model_descriptors, &reader, 0 );
+
+ for( i = 0; i < model_descriptors->total; i++ )
+ {
+ const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
+ const float* mvec = (const float*)reader.ptr;
+ CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ if( laplacian != kp->laplacian )
+ continue;
+ d = compareSURFDescriptors( vec, mvec, dist2, length );
+ if( d < dist1 )
+ {
+ dist2 = dist1;
+ dist1 = d;
+ neighbor = i;
+ }
+ else if ( d < dist2 )
+ dist2 = d;
+ }
+ if ( dist1 < 0.6*dist2 )
+ return neighbor;
+ return -1;
+}
+
+
+//Find for a point candidate the most similar point in the reference point list.
+int naiveNearestNeighbor( const float *vec,
+ const CvSeq *ref_keypoints,
+ const CvSeq *ref_descriptors )
+{
+ int length = (int)(ref_descriptors->elem_size/(int)sizeof(float));
+ int i, neighbor = -1;
+ double d, dist1 = 1e6, dist2 = 1e6;
+ CvSeqReader reader, kreader;
+ cvStartReadSeq( ref_keypoints, &kreader, 0 );
+ cvStartReadSeq( ref_descriptors, &reader, 0 );
+
+ for( i = 0; i < ref_descriptors->total; i++ )
+ {
+ const float* mvec = (const float*)reader.ptr;
+ CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ d = compareSURFDescriptors( vec, mvec, dist2, length );
+ if( d < dist1 )
+ {
+ dist2 = dist1;
+ dist1 = d;
+ neighbor = i;
+ }
+ else if ( d < dist2 )
+ dist2 = d;
+ }
+ if ( dist1 < 0.6*dist2 )
+ return neighbor;
+ return -1;
+}
+
+
+
+//Find all the matched points
+void findPairs( const CvSeq* objectKeypoints,
+ const CvSeq* objectDescriptors,
+ const CvSeq* imageKeypoints,
+ const CvSeq* imageDescriptors,
+ std::vector<int>& ptpairs )
+{
+ int i;
+ CvSeqReader reader, kreader;
+ cvStartReadSeq( objectKeypoints, &kreader );
+ cvStartReadSeq( objectDescriptors, &reader );
+ ptpairs.clear();
+
+ for( i = 0; i < objectDescriptors->total; i++ )
+ {
+ const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
+ const float* descriptor = (const float*)reader.ptr;
+ CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ int nearest_neighbor = naiveNearestNeighbor( descriptor,
+ kp->laplacian,
+ imageKeypoints,
+ imageDescriptors );
+ if( nearest_neighbor >= 0 )
+ {
+ ptpairs.push_back(i);
+ ptpairs.push_back(nearest_neighbor);
+ }
+ }
+}
+
+
+
+/*!
+
+ Basic constructor. It sets by default the hessian threshold to 500
+ (a good default value is between 300 and 500) and the descriptor
+ type to extended.
+
+*/
+vpKeyPointSurf::vpKeyPointSurf()
+ : vpBasicKeyPoint(),
+ storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
+ ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
+{
+ init();
+}
+
+/*!
+ Initialize any OpenCV parameters.
+*/
+void vpKeyPointSurf::init()
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
+ cv::initModule_nonfree();
+#endif
+
+ storage = cvCreateMemStorage(0);
+ params = cvSURFParams(hessianThreshold, descriptorType);
+}
+
+/*!
+ Basic Destructor
+
+*/
+vpKeyPointSurf::~vpKeyPointSurf()
+{
+ cvReleaseMemStorage(&storage);
+ if(storage_cur!=NULL){
+ cvReleaseMemStorage(&storage_cur);
+ }
+}
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made all over the image I.
+
+ \param I : The gray scaled image where the reference points are computed.
+
+ \return the number of reference points.
+*/
+unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
+{
+ IplImage* model = NULL;
+
+ if((I.getWidth() % 8) == 0){
+ int height = (int)I.getHeight();
+ int width = (int)I.getWidth();
+ CvSize size = cvSize(width, height);
+ model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
+ model->imageData = (char*)I.bitmap;
+ }else{
+ vpImageConvert::convert(I,model);
+ }
+
+ cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
+
+ const unsigned int nbPoints = (unsigned int)ref_keypoints->total;
+
+ referenceImagePointsList.resize(nbPoints);
+
+ for(unsigned int i = 0; i < nbPoints; i++ )
+ {
+ CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (int)i);
+
+ referenceImagePointsList[i].set_i(r1->pt.y);
+ referenceImagePointsList[i].set_j(r1->pt.x);
+ }
+
+ if((I.getWidth() % 8) == 0){
+ model->imageData = NULL;
+ cvReleaseImageHeader(&model);
+ }else{
+ cvReleaseImage(&model);
+ }
+
+ _reference_computed = true;
+ return nbPoints;
+}
+
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made only on a part of the image. This part is a rectangle defined
+ by its top left corner, its height and its width. The parameters of
+ this rectangle must be given in pixel.
+
+ \param I : The gray scaled image where the reference points are computed.
+
+ \param iP : The top left corner of the rectangle.
+
+ \param height : height of the rectangle (in pixel).
+
+ \param width : width of the rectangle (in pixel).
+
+ \return the number of reference points.
+*/
+unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
+{
+ if((iP.get_i()+height) >= I.getHeight()
+ || (iP.get_j()+width) >= I.getWidth())
+ {
+ vpTRACE("Bad size for the subimage");
+ throw(vpException(vpImageException::notInTheImage ,
+ "Bad size for the subimage"));
+ }
+
+ vpImage<unsigned char> subImage;
+
+ vpImageTools::createSubImage(I,
+ (unsigned int)iP.get_i(),
+ (unsigned int)iP.get_j(),
+ height, width, subImage);
+
+ unsigned int nbRefPoint = this->buildReference(subImage);
+
+ for(unsigned int k = 0; k < nbRefPoint; k++)
+ {
+ (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i()
+ + iP.get_i());
+ (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j()
+ + iP.get_j());
+ }
+ return(nbRefPoint);
+}
+
+
+/*!
+
+ Build the list of reference points. The computation of the points is
+ made only on a part of the image. This part is a rectangle. The
+ parameters of this rectangle must be given in pixel.
+
+ \param I : The gray scaled image where the reference points are computed.
+
+ \param rectangle : The rectangle which defines the interesting part
+ of the image.
+
+ \return the number of reference points.
+*/
+unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
+ const vpRect& rectangle)
+{
+ vpImagePoint iP;
+ iP.set_i(rectangle.getTop());
+ iP.set_j(rectangle.getLeft());
+ return (this->buildReference(I, iP,
+ (unsigned int)rectangle.getHeight(),
+ (unsigned int)rectangle.getWidth()));
+}
+
+
+/*!
+
+ Computes the SURF points in the current image I and try to matched
+ them with the points in the reference list. Only the matched points
+ are stored.
+
+ \param I : The gray scaled image where the points are computed.
+
+ \return the number of point which have been matched.
+*/
+unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
+{
+ IplImage* currentImage = NULL;
+
+ if((I.getWidth() % 8) == 0){
+ int height = (int)I.getHeight();
+ int width = (int)I.getWidth();
+ CvSize size = cvSize(width, height);
+ currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
+ currentImage->imageData = (char*)I.bitmap;
+ }else{
+ vpImageConvert::convert(I,currentImage);
+ }
+
+ /* we release the memory storage for the current points (it has to be kept
+ allocated for the get descriptor points, ...) */
+ if(storage_cur != NULL){
+ cvReleaseMemStorage(&storage_cur);
+ storage_cur = NULL;
+ }
+ storage_cur = cvCreateMemStorage(0);
+
+ cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
+ storage_cur, params );
+
+ CvSeqReader reader, kreader;
+ cvStartReadSeq( ref_keypoints, &kreader );
+ cvStartReadSeq( ref_descriptors, &reader );
+
+
+ std::list<int> indexImagePair;
+ std::list<int> indexReferencePair;
+
+
+ unsigned int nbrPair = 0;
+
+ for(int i = 0; i < ref_descriptors->total; i++ )
+ {
+ const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
+ const float* descriptor = (const float*)reader.ptr;
+ CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ int nearest_neighbor = naiveNearestNeighbor( descriptor,
+ kp->laplacian,
+ image_keypoints,
+ image_descriptors );
+ if( nearest_neighbor >= 0 )
+ {
+ indexReferencePair.push_back(i);
+ indexImagePair.push_back(nearest_neighbor);
+ nbrPair++;
+ }
+ }
+
+ std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
+ std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
+
+ matchedReferencePoints.resize(0);
+
+ if (nbrPair == 0)
+ return (0);
+
+ currentImagePointsList.resize(nbrPair);
+ matchedReferencePoints.resize(nbrPair);
+
+ for (unsigned int i = 0; i < nbrPair; i++)
+ {
+ int index = *indexImagePairIter;
+
+ CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
+
+ currentImagePointsList[i].set_i(r1->pt.y);
+ currentImagePointsList[i].set_j(r1->pt.x);
+
+ matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
+
+
+ ++indexImagePairIter;
+ ++indexReferencePairIter;
+ }
+
+ if((I.getWidth() % 8) == 0){
+ currentImage->imageData = NULL;
+ cvReleaseImageHeader(¤tImage);
+ }else{
+ cvReleaseImage(¤tImage);
+ }
+
+ return nbrPair;
+}
+
+
+/*!
+
+ Computes the SURF points in only a part of the current image I and
+ try to matched them with the points in the reference list. The part
+ of the image is a rectangle defined by its top left corner, its
+ height and its width. The parameters of this rectangle must be given
+ in pixel. Only the matched points are stored.
+
+ \param I : The gray scaled image where the points are computed.
+
+ \param iP : The top left corner of the rectangle.
+
+ \param height : height of the rectangle (in pixel).
+
+ \param width : width of the rectangle (in pixel).
+
+ \return the number of point which have been matched.
+*/
+unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
+{
+ if((iP.get_i()+height) >= I.getHeight()
+ || (iP.get_j()+width) >= I.getWidth())
+ {
+ vpTRACE("Bad size for the subimage");
+ throw(vpException(vpImageException::notInTheImage ,
+ "Bad size for the subimage"));
+ }
+
+ vpImage<unsigned char> subImage;
+
+ vpImageTools::createSubImage(I,
+ (unsigned int)iP.get_i(),
+ (unsigned int)iP.get_j(),
+ height, width, subImage);
+
+ unsigned int nbMatchedPoint = this->matchPoint(subImage);
+
+ for(unsigned int k = 0; k < nbMatchedPoint; k++)
+ {
+ (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i()
+ + iP.get_i());
+ (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j()
+ + iP.get_j());
+ }
+
+ return(nbMatchedPoint);
+}
+
+
+/*!
+
+ Computes the SURF points in only a part of the current image I and
+ try to matched them with the points in the reference list. The part
+ of the image is a rectangle. The parameters of this rectangle must
+ be given in pixel. Only the matched points are stored.
+
+ \param I : The gray scaled image where the points are computed.
+
+ \param rectangle : The rectangle which defines the interesting part
+ of the image.
+
+ \return the number of point which have been matched.
+*/
+unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
+ const vpRect& rectangle)
+{
+ vpImagePoint iP;
+ iP.set_i(rectangle.getTop());
+ iP.set_j(rectangle.getLeft());
+ return (this->matchPoint(I, iP,
+ (unsigned int)rectangle.getHeight(),
+ (unsigned int)rectangle.getWidth()));
+}
+
+
+/*!
+
+ This function displays the matched reference points and the matched
+ points computed in the current image. The reference points are
+ displayed in the image Ireference and the matched points coming from
+ the current image are displayed in the image Icurrent. It is
+ possible to set Ireference and Icurrent with the same image when
+ calling the method.
+
+ \param Ireference : The image where the matched reference points are
+ displayed.
+
+ \param Icurrent : The image where the matched points computed in the
+ current image are displayed.
+
+ \param size : Size in pixels of the cross that is used to display matched points.
+
+*/
+void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference,
+ const vpImage<unsigned char> &Icurrent, unsigned int size)
+{
+// matchedPointsCurrentImageList.front();
+// matchedPointsReferenceImageList.front();
+
+// if (matchedPointsCurrentImageList.nbElements()
+// != matchedPointsReferenceImageList.nbElements())
+// {
+// vpTRACE("Numbers of points mismatch");
+// throw(vpException(vpException::fatalError,"Numbers of points mismatch"));
+// }
+
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
+ {
+ vpDisplay::displayCross (Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
+ vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, vpColor::green);
+// matchedPointsReferenceImageList.next();
+// matchedPointsCurrentImageList.next();
+ }
+}
+
+
+/*!
+
+ This function displays only the matched points computed in the
+ current image. They are displayed in the image Icurrent.
+
+ \param Icurrent : The image where the matched points computed in the
+ current image are displayed.
+
+ \param size : Size in pixels of the cross that is used to display matched points.
+
+ \param color : Color used to display the matched points.
+*/
+void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
+{
+// matchedPointsCurrentImageList.front();
+//
+// vpImagePoint ipCur;
+//
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
+ {
+ vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, color);
+ }
+}
+
+std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList)
+{
+ std::list<int*>* pairPoints = new std::list<int*>;
+
+ if(descriptorList.size() != laplacianList.size()){
+ vpTRACE("Error, the two lists have different number of element");
+ return pairPoints;
+ }
+
+ CvSeqReader reader;
+ cvStartReadSeq( ref_descriptors, &reader );
+
+ std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
+ std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
+ descriptorList.front();
+ int indexList = 0;
+ while(descriptorListIter != descriptorList.end()){
+ float* descriptor = *descriptorListIter;
+
+ int nearest_neighbor = naiveNearestNeighbor( descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
+
+ if(nearest_neighbor >= 0){
+ int* tab;
+ tab = new int[2];
+ tab[0] = nearest_neighbor;
+ tab[1] = indexList;
+ pairPoints->push_back(tab);
+ }
+ indexList++;
+ ++descriptorListIter;
+ ++laplacianListIter;
+ }
+
+ return pairPoints;
+}
+
+/*!
+ Get the descriptor of the nth reference point.
+
+ \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
+*/
+float* vpKeyPointSurf::getDescriptorReferencePoint (const int index)
+{
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ vpTRACE("Index of the reference point out of range");
+ throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ }
+
+ float* descriptor = NULL;
+
+ CvSeqReader reader;
+ cvStartReadSeq( ref_descriptors, &reader );
+
+ for(int j = 0; j < ref_descriptors->total; j++ ){
+ if(j== index){
+ descriptor = (float*)reader.ptr;
+ break;
+ }
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ }
+
+ return descriptor;
+}
+
+/*!
+ Get the laplacian of the nth reference point.
+
+ \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
+*/
+int vpKeyPointSurf::getLaplacianReferencePoint (const int index)
+{
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ vpTRACE("Index of the reference point out of range");
+ throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ }
+
+ CvSeqReader reader;
+ cvStartReadSeq( ref_keypoints, &reader );
+
+ int laplacian = 0;/* normally only -1, 0, +1 are possible */
+
+ for(int j = 0; j < ref_keypoints->total; j++ ){
+ if(j== index){
+ const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
+ laplacian = kp->laplacian;
+ break;
+ }
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ }
+
+ return laplacian;
+}
+
+/*!
+ Get the parameters of the descriptor of the nth reference point.
+
+ \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
+ \param size : The size of the point used to compute the descriptor.
+ \param dir : The orientation of the descriptor (in degree).
+*/
+void vpKeyPointSurf::getDescriptorParamReferencePoint (const int index, int& size, float& dir)
+{
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ vpTRACE("Index of the reference point out of range");
+ throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ }
+
+ CvSeqReader reader;
+ cvStartReadSeq( ref_keypoints, &reader );
+
+ for(int j = 0; j < ref_keypoints->total; j++ ){
+ if(j== index){
+ const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
+ size = kp->size;
+ dir = kp->dir;
+ break;
+ }
+ CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ }
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_vision.a(vpKeyPointSurf.cpp.o) has no symbols
+void dummy_vpKeyPointSurf() {};
+#endif
+
diff --git a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp
new file mode 100644
index 0000000..46f1c81
--- /dev/null
+++ b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp
@@ -0,0 +1,540 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Planar surface detection tool.
+ *
+ * Authors:
+ * Romain Tallonneau
+ *
+ *****************************************************************************/
+
+#include <visp3/vision/vpPlanarObjectDetector.h>
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImageTools.h>
+
+#include <vector>
+#include <iostream>
+
+/*!
+
+ Basic constructor
+
+*/
+vpPlanarObjectDetector::vpPlanarObjectDetector()
+ : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
+ refImagePoints(), minNbMatching(10)
+{
+}
+
+/*!
+
+ Basic constructor, load data from a file.
+
+ \param _dataFile : the name of the file.
+ \param _objectName : the name of the object to load.
+
+*/
+vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
+ : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
+ refImagePoints(), minNbMatching(10)
+{
+ load(_dataFile, _objectName);
+}
+
+/*!
+ Initialise stuff. For the moment does nothing.
+*/
+void
+vpPlanarObjectDetector::init()
+{
+}
+
+/*!
+
+ Basic destructor
+
+*/
+vpPlanarObjectDetector::~vpPlanarObjectDetector()
+{
+
+}
+
+/*!
+ Compute the rectangular ROI from at least 4 points and set the region of
+ interest on the current image.
+
+ \param ip : the list of image point.
+ \param nbpt : the number of point.
+*/
+void
+vpPlanarObjectDetector::computeRoi(vpImagePoint* ip, const unsigned int nbpt)
+{
+ if(nbpt < 3){
+ throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
+ }
+
+ std::vector < vpImagePoint > ptsx(nbpt);
+ std::vector < vpImagePoint > ptsy(nbpt);
+ for(unsigned int i=0; i<nbpt; i++){
+ ptsx[i] = ptsy[i] = ip[i];
+ }
+
+ for(unsigned int i=0; i<nbpt; i++){
+ for(unsigned int j=0; j<nbpt-1; j++){
+ if(ptsx[j].get_j() > ptsx[j+1].get_j()){
+ double tmp = ptsx[j+1].get_j();
+ ptsx[j+1].set_j(ptsx[j].get_j());
+ ptsx[j].set_j(tmp);
+ }
+ }
+ }
+ for(unsigned int i=0; i<nbpt; i++){
+ for(unsigned int j=0; j<nbpt-1; j++){
+ if(ptsy[j].get_i() > ptsy[j+1].get_i()){
+ double tmp = ptsy[j+1].get_i();
+ ptsy[j+1].set_i(ptsy[j].get_i());
+ ptsy[j].set_i(tmp);
+ }
+ }
+ }
+
+}
+
+
+/*!
+ Train the classifier from the entire image (it is therefore assumed that the
+ image is planar).
+
+ Depending on the parameters, the training can take up to several minutes.
+
+ \param _I : The image use to train the classifier.
+
+ \return The number of reference points.
+*/
+unsigned int
+vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
+{
+ modelROI.x = 0;
+ modelROI.y = 0;
+ modelROI.width = (int)_I.getWidth();
+ modelROI.height = (int)_I.getHeight();
+
+ initialiseRefCorners(modelROI);
+
+ return fern.buildReference(_I);
+}
+
+
+/*!
+ Train the classifier on a region of the entire image. The region is a
+ rectangle defined by its top left corner, its height and its width. The
+ parameters of this rectangle must be given in pixel. It also includes the
+ training of the fern classifier.
+
+ \param _I : The image use to train the classifier.
+ \param _iP : The top left corner of the rectangle defining the region of interest (ROI).
+ \param _height : The height of the ROI.
+ \param _width : The width of the ROI.
+
+ \return the number of reference points
+*/
+unsigned int
+vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
+ const vpImagePoint &_iP,
+ unsigned int _height, unsigned int _width)
+{
+ unsigned int res = fern.buildReference(_I, _iP, _height, _width);
+ modelROI.x = (int)_iP.get_u();
+ modelROI.y = (int)_iP.get_v();
+ modelROI.width = (int)_width;
+ modelROI.height = (int)_height;
+
+ initialiseRefCorners(modelROI);
+
+ return res;
+}
+
+
+
+/*!
+ Train the classifier on a region of the entire image. The region is a
+ rectangle. The parameters of this rectangle must be given in pixel. It also
+ includes the training of the fern classifier.
+
+ \param _I : The image use to train the classifier.
+ \param _rectangle : The rectangle defining the region of interest (ROI).
+
+ \return The number of reference points.
+*/
+unsigned int
+vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
+ const vpRect _rectangle)
+{
+ unsigned int res = fern.buildReference(_I, _rectangle);
+
+ vpImagePoint iP = _rectangle.getTopLeft();
+
+ modelROI.x = (int)iP.get_u();
+ modelROI.y = (int)iP.get_v();
+ modelROI.width = (int)_rectangle.getWidth();
+ modelROI.height = (int)_rectangle.getHeight();
+
+ initialiseRefCorners(modelROI);
+
+ return res;
+}
+
+
+/*!
+ Compute the points of interest in the current image and try to recognise them
+ using the trained classifier. The matched pairs can be found with the
+ getMatchedPointByRef function. The homography between the two planar
+ surfaces is also computed.
+
+ \param I : The gray scaled image where the points are computed.
+
+ \return True if the surface has been found.
+*/
+bool
+vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
+{
+ fern.matchPoint(I);
+
+ /* compute homography */
+ std::vector<cv::Point2f> refPts = fern.getRefPt();
+ std::vector<cv::Point2f> curPts = fern.getCurPt();
+
+ for(unsigned int i=0; i<refPts.size(); ++i){
+ refPts[i].x += modelROI.x;
+ refPts[i].y += modelROI.y;
+ }
+ for(unsigned int i=0; i<curPts.size(); ++i){
+ curPts[i].x += modelROI.x;
+ curPts[i].y += modelROI.y;
+ }
+
+ if(curPts.size() < 4){
+ for (unsigned int i = 0; i < 3; i += 1){
+ for (unsigned int j = 0; j < 3; j += 1){
+ if(i == j){
+ homography[i][j] = 1;
+ }
+ else{
+ homography[i][j] = 0;
+ }
+ }
+ }
+ return false;
+ }
+
+ /* part of code from OpenCV planarObjectDetector */
+ std::vector<unsigned char> mask;
+ H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
+
+ if( H.data )
+ {
+ const cv::Mat_<double>& H_tmp = H;
+ dst_corners.resize(4);
+ for(unsigned int i = 0; i < 4; i++ )
+ {
+ cv::Point2f pt = ref_corners[i];
+
+ double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
+ dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
+ (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
+ }
+
+ double* ptr = (double*)H_tmp.data;
+ for(unsigned int i=0; i<9; i++){
+ this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
+ }
+ isCorrect = true;
+ }
+ else{
+ isCorrect = false;
+ }
+
+
+ currentImagePoints.resize(0);
+ refImagePoints.resize(0);
+ for (unsigned int i = 0; i < mask.size(); i += 1){
+ if(mask[i] != 0){
+ vpImagePoint ip;
+ ip.set_i(curPts[i].y);
+ ip.set_j(curPts[i].x);
+ currentImagePoints.push_back(ip);
+ ip.set_i(refPts[i].y);
+ ip.set_j(refPts[i].x);
+ refImagePoints.push_back(ip);
+ }
+ }
+
+ if(currentImagePoints.size() < minNbMatching){
+ isCorrect = false;
+ }
+
+ return isCorrect;
+}
+
+
+
+/*!
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs can
+ be found with the getMatchedPointByRef function. The homography between the
+ two planar surfaces is also computed.
+
+ \param I : The gray scaled image where the points are computed.
+ \param iP : the top left corner of the rectangle defining the ROI
+ \param height : the height of the ROI
+ \param width : the width of the ROI
+
+ \return true if the surface has been found.
+*/
+bool
+vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I,
+ const vpImagePoint &iP, const unsigned int height, const unsigned int width)
+{
+ if((iP.get_i()+height) >= I.getHeight()
+ || (iP.get_j()+width) >= I.getWidth()) {
+ vpTRACE("Bad size for the subimage");
+ throw(vpException(vpImageException::notInTheImage ,
+ "Bad size for the subimage"));
+ }
+
+ vpImage<unsigned char> subImage;
+
+ vpImageTools::createSubImage(I,
+ (unsigned int)iP.get_i(),
+ (unsigned int)iP.get_j(),
+ height, width, subImage);
+
+ return this->matchPoint(subImage);
+}
+
+/*!
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs can
+ be found with the getMatchedPointByRef function. The homography between the
+ two planar surfaces is also computed.
+
+ \param I : The gray scaled image where the points are computed.
+ \param rectangle : The rectangle defining the ROI.
+
+ \return True if the surface has been found.
+*/
+bool
+vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle)
+{
+ vpImagePoint iP;
+ iP.set_i(rectangle.getTop());
+ iP.set_j(rectangle.getLeft());
+ return (this->matchPoint(I, iP,
+ (unsigned int)rectangle.getHeight(),
+ (unsigned int)rectangle.getWidth()));
+}
+
+
+/*!
+ Display the result of the matching. The plane is displayed in red and the
+ points of interests detected in the image are shown by a red dot surrounded by
+ a green circle (the radius of the circle depends on the octave at which it has
+ been detected).
+
+ \param I : The gray scaled image for the display.
+ \param displayKpts : The flag to display keypoints in addition to the surface.
+*/
+void
+vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
+{
+ for(unsigned int i=0; i<dst_corners.size(); i++){
+ vpImagePoint ip1(
+ dst_corners[i].y - modelROI.y,
+ dst_corners[i].x - modelROI.x);
+ vpImagePoint ip2(
+ dst_corners[(i+1)%dst_corners.size()].y - modelROI.y,
+ dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ }
+
+ if(displayKpts){
+ for(unsigned int i=0; i<currentImagePoints.size(); ++i){
+ vpImagePoint ip(
+ currentImagePoints[i].get_i() - modelROI.y,
+ currentImagePoints[i].get_j() - modelROI.x);
+ vpDisplay::displayCross(I, ip, 5, vpColor::red);
+ }
+ }
+}
+
+
+/*!
+ This function displays the matched reference points and the matched
+ points computed in the current image. The reference points are
+ displayed in the image Ireference and the matched points coming from
+ the current image are displayed in the image Icurrent. It is
+ possible to set Ireference and Icurrent with the same image when
+ calling the method.
+
+ \param Iref : The image where the matched reference points are
+ displayed.
+
+ \param Icurrent : The image where the matched points computed in the
+ current image are displayed.
+
+ \param displayKpts : The flag to display keypoints in addition to the surface.
+*/
+void
+vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref,
+ vpImage<unsigned char> &Icurrent, bool displayKpts)
+{
+ display(Icurrent, displayKpts);
+
+ if(displayKpts){
+ for(unsigned int i=0; i<refImagePoints.size(); ++i){
+ vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
+ }
+ }
+}
+
+
+/*!
+ \brief Load the Fern classifier.
+
+ Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
+ can take up to sevral seconds.
+
+ \param dataFilename : The name of the data filename (very large text file).
+ It can have any file extension.
+ \param objName : The name of the object.
+*/
+void
+vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
+{
+ fern.load(dataFilename, objName);
+ modelROI = fern.getModelROI();
+ initialiseRefCorners(modelROI);
+}
+
+
+/*!
+ \brief Record the Ferns classifier in the text file.
+
+ \param objectName : The name of the object to store in the data file.
+ \param dataFile : The name of the data filename (very large text file).
+ It can have any file extension.
+*/
+void
+vpPlanarObjectDetector::recordDetector(const std::string& objectName, const std::string& dataFile )
+{
+ fern.record(objectName, dataFile);
+}
+
+
+
+/*!
+ Return the last positions of the detected corners.
+
+ \return The vectors of corners' postions.
+*/
+std::vector<vpImagePoint>
+vpPlanarObjectDetector::getDetectedCorners() const{
+ vpImagePoint ip;
+ std::vector <vpImagePoint> corners;
+ corners.clear();
+ for(unsigned int i=0; i<dst_corners.size(); i++){
+ ip.set_uv( dst_corners[i].x, dst_corners[i].y);
+ corners.push_back(ip);
+ }
+
+ return corners;
+}
+
+/*!
+ Initialise the internal reference corners from the rectangle.
+
+ \param _modelROI : The rectangle defining the region of interest.
+*/
+void
+vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect& _modelROI)
+{
+ cv::Point2f ip;
+
+ ip.y = (float)_modelROI.y;
+ ip.x = (float)_modelROI.x;
+ ref_corners.push_back(ip);
+
+ ip.y = (float)(_modelROI.y+_modelROI.height);
+ ip.x = (float)_modelROI.x;
+ ref_corners.push_back(ip);
+
+ ip.y = (float)(_modelROI.y+_modelROI.height);
+ ip.x = (float)(_modelROI.x+_modelROI.width);
+ ref_corners.push_back(ip);
+
+ ip.y = (float)_modelROI.y;
+ ip.x = (float)(_modelROI.x+_modelROI.width);
+ ref_corners.push_back(ip);
+}
+
+
+void
+vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint& _imPoint)
+{
+ if(_i >= refImagePoints.size()){
+ throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
+ }
+ _imPoint = refImagePoints[_i];
+}
+
+void
+vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
+{
+// fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
+ if(_index >= currentImagePoints.size()){
+ throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
+ }
+
+ _referencePoint = refImagePoints[_index];
+ _currentPoint = currentImagePoints[_index];
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
+void dummy_vpPlanarObjectDetector() {};
+#endif
+
diff --git a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp
new file mode 100644
index 0000000..6b57c87
--- /dev/null
+++ b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp
@@ -0,0 +1,429 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * XML parser to load configuration for vpKeyPoint class.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpXmlConfigParserKeyPoint.cpp
+ \brief Definition of the vpXmlConfigParserKeyPoint class member functions.
+ Class vpXmlConfigParserKeyPoint permits to load configuration defined in a XML file for vpKeyPoint class.
+
+*/
+
+#include <iostream>
+
+#include <visp3/vision/vpXmlConfigParserKeyPoint.h>
+
+#ifdef VISP_HAVE_XML2
+
+vpXmlConfigParserKeyPoint::vpXmlConfigParserKeyPoint() : m_detectorName("ORB"), m_extractorName("ORB"),
+ m_matcherName("BruteForce-Hamming"), m_matchingFactorThreshold(2.0), m_matchingMethod(ratioDistanceThreshold),
+ m_matchingRatioThreshold(0.85), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100),
+ m_ransacConsensusPercentage(20.0), m_ransacReprojectionError(6.0), m_ransacThreshold(0.01),
+ m_useRansacConsensusPercentage(false), m_useRansacVVS(true)
+{
+ init();
+}
+
+/*!
+ Initialize the nodeMap for the node parsing.
+*/
+void
+vpXmlConfigParserKeyPoint::init()
+{
+ setMainTag("conf");
+
+ nodeMap["conf"] = conf;
+ nodeMap["detector"] = detector;
+ nodeMap["extractor"] = extractor;
+ nodeMap["matcher"] = matcher;
+ nodeMap["name"] = name;
+ nodeMap["matching_method"] = matching_method;
+ nodeMap["constantFactorDistanceThreshold"] = constant_factor_distance_threshold;
+ nodeMap["stdDistanceThreshold"] = std_distance_threshold;
+ nodeMap["ratioDistanceThreshold"] = ratio_distance_threshold;
+ nodeMap["stdAndRatioDistanceThreshold"] = std_and_ratio_distance_threshold;
+ nodeMap["noFilterMatching"] = no_filter_matching;
+ nodeMap["matchingFactorThreshold"] = matching_factor_threshold;
+ nodeMap["matchingRatioThreshold"] = matching_ratio_threshold;
+ nodeMap["ransac"] = ransac;
+ nodeMap["useRansacVVS"] = use_ransac_vvs;
+ nodeMap["useRansacConsensusPercentage"] = use_ransac_consensus_percentage;
+ nodeMap["nbRansacIterations"] = nb_ransac_iterations;
+ nodeMap["ransacReprojectionError"] = ransac_reprojection_error;
+ nodeMap["nbRansacMinInlierCount"] = nb_ransac_min_inlier_count;
+ nodeMap["ransacThreshold"] = ransac_threshold;
+ nodeMap["ransacConsensusPercentage"] = ransac_consensus_percentage;
+}
+
+/*!
+ Parse an XML file to load configuration for vpKeyPoint class.
+ \param filename : filename of the XML file to parse.
+*/
+void
+vpXmlConfigParserKeyPoint::parse(const std::string &filename)
+{
+ vpXmlParser::parse(filename);
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void
+vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool detector_node = false;
+ bool extractor_node = false;
+ bool matcher_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE){
+ std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
+ if(iter_data != nodeMap.end()){
+ switch (iter_data->second){
+ case detector:
+ this->read_detector(doc, dataNode);
+ detector_node = true;
+ break;
+
+ case extractor:
+ this->read_extractor(doc, dataNode);
+ extractor_node = true;
+ break;
+
+ case matcher:
+ this->read_matcher(doc, dataNode);
+ matcher_node = true;
+ break;
+
+ case ransac:
+ this->read_ransac(doc, dataNode);
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if(!detector_node) {
+ std::cout << "detector: name: "<< m_detectorName << " (default)" << std::endl;
+ }
+
+ if(!extractor_node) {
+ std::cout << "extractor: name: "<< m_extractorName << " (default)" << std::endl;
+ }
+
+ if(!matcher_node) {
+ std::cout << "matcher: name: "<< m_matcherName << " (default)" << std::endl;
+ }
+}
+
+/*!
+ Parse detector tag part.
+
+ \param doc : Document to parse.
+ \param node : Detector node.
+*/
+void
+vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool detector_name_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch(iter_data->second) {
+ case name:
+ m_detectorName = xmlReadStringChild(doc, dataNode);
+ detector_name_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if(!detector_name_node)
+ std::cout << "detector : Name : "<< m_detectorName << " (default)" << std::endl;
+ else
+ std::cout << "detector : Name : "<< m_detectorName << std::endl;
+}
+
+/*!
+ Parse extractor tag part.
+
+ \param doc : Document to parse.
+ \param node : Extractor node.
+*/
+void
+vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool extractor_name_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch(iter_data->second) {
+ case name:
+ m_extractorName = xmlReadStringChild(doc, dataNode);
+ extractor_name_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if(!extractor_name_node)
+ std::cout << "extractor : Name : "<< m_extractorName << " (default)" << std::endl;
+ else
+ std::cout << "extractor : Name : "<< m_extractorName << std::endl;
+}
+
+/*!
+ Parse matcher tag part.
+
+ \param doc : Document to parse.
+ \param node : Matcher node.
+*/
+void
+vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool matcher_name_node = false;
+ bool matching_method_node = false;
+ std::string matchingMethodName = "ratioDistanceThreshold";
+ bool matching_factor_threshold_node = false;
+ bool matching_ratio_threshold_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ if(iter_data != nodeMap.end()) {
+ switch(iter_data->second) {
+ case name:
+ m_matcherName = xmlReadStringChild(doc, dataNode);
+ matcher_name_node = true;
+ break;
+
+ case matching_method:
+ {
+ matchingMethodName = xmlReadStringChild(doc, dataNode);
+
+ std::map<std::string, int>::iterator iter_data2 = nodeMap.find(matchingMethodName);
+ if(iter_data2 != nodeMap.end()) {
+ matching_method_node = true;
+ switch(iter_data2->second) {
+ case constant_factor_distance_threshold:
+ m_matchingMethod = constantFactorDistanceThreshold;
+ break;
+
+ case std_distance_threshold:
+ m_matchingMethod = stdDistanceThreshold;
+ break;
+
+ case ratio_distance_threshold:
+ m_matchingMethod = ratioDistanceThreshold;
+ break;
+
+ case std_and_ratio_distance_threshold:
+ m_matchingMethod = stdAndRatioDistanceThreshold;
+ break;
+
+ case no_filter_matching:
+ m_matchingMethod = noFilterMatching;
+ break;
+
+ default:
+ matching_method_node = false;
+ break;
+ }
+ }
+ break;
+ }
+
+ case matching_factor_threshold:
+ m_matchingFactorThreshold = xmlReadDoubleChild(doc, dataNode);
+ matching_factor_threshold_node = true;
+ break;
+
+ case matching_ratio_threshold:
+ m_matchingRatioThreshold = xmlReadDoubleChild(doc, dataNode);
+ matching_ratio_threshold_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if(!matcher_name_node)
+ std::cout << "matcher : Name : "<< m_matcherName << " (default)" << std::endl;
+ else
+ std::cout << "matcher : Name : "<< m_matcherName <<std::endl;
+
+ if(!matching_method_node)
+ std::cout << "matcher : Filter method : "<< matchingMethodName << " (default)" << std::endl;
+ else
+ std::cout << "matcher : Filter method : "<< matchingMethodName << std::endl;
+
+ if(!matching_factor_threshold_node)
+ std::cout << "matcher : matching factor threshold : "<< m_matchingFactorThreshold << " (default)" << std::endl;
+ else
+ std::cout << "matcher : matching factor threshold : "<< m_matchingFactorThreshold << std::endl;
+
+ if(!matching_ratio_threshold_node)
+ std::cout << "matcher : matching ratio threshold : "<< m_matchingRatioThreshold << " (default)" << std::endl;
+ else
+ std::cout << "matcher : matching ratio threshold : "<< m_matchingRatioThreshold << std::endl;
+}
+
+/*!
+ Parse ransac tag part.
+
+ \param doc : Document to parse.
+ \param node : Ransac node.
+*/
+void
+vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool use_ransac_vvs_node = false;
+ bool use_ransac_consensus_percentage_node = false;
+ bool nb_ransac_iterations_node = false;
+ bool ransac_reprojection_error_node = false;
+ bool nb_ransac_min_inlier_count_node = false;
+ bool ransac_threshold_node = false;
+ bool ransac_consensus_percentage_node = false;
+
+ for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if(dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ if(iter_data != nodeMap.end()) {
+ switch(iter_data->second) {
+ case use_ransac_vvs:
+ m_useRansacVVS = xmlReadIntChild(doc, dataNode) != 0;
+ use_ransac_vvs_node = true;
+ break;
+
+ case use_ransac_consensus_percentage:
+ m_useRansacConsensusPercentage = xmlReadIntChild(doc, dataNode) != 0;
+ use_ransac_consensus_percentage_node = true;
+ break;
+
+ case nb_ransac_iterations:
+ m_nbRansacIterations = xmlReadIntChild(doc, dataNode);
+ nb_ransac_iterations_node = true;
+ break;
+
+ case ransac_reprojection_error:
+ m_ransacReprojectionError = xmlReadDoubleChild(doc, dataNode);
+ ransac_reprojection_error_node = true;
+ break;
+
+ case nb_ransac_min_inlier_count:
+ m_nbRansacMinInlierCount = xmlReadIntChild(doc, dataNode);
+ nb_ransac_min_inlier_count_node = true;
+ break;
+
+ case ransac_threshold:
+ m_ransacThreshold = xmlReadDoubleChild(doc, dataNode);
+ ransac_threshold_node = true;
+ break;
+
+ case ransac_consensus_percentage:
+ m_ransacConsensusPercentage = xmlReadDoubleChild(doc, dataNode);
+ ransac_consensus_percentage_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if(!use_ransac_vvs_node)
+ std::cout << "ransac: use ransac vvs pose estimation: "<< m_useRansacVVS << " (default)" << std::endl;
+ else
+ std::cout << "ransac: use ransac vvs pose estimation: "<< m_useRansacVVS <<std::endl;
+
+ if(!use_ransac_consensus_percentage_node)
+ std::cout << "ransac: use consensus percentage: "<< m_useRansacConsensusPercentage << " (default)" << std::endl;
+ else
+ std::cout << "ransac: use consensus percentage: "<< m_useRansacConsensusPercentage <<std::endl;
+
+ if(!nb_ransac_iterations_node)
+ std::cout << "ransac: nb ransac iterations: "<< m_nbRansacIterations << " (default)" << std::endl;
+ else
+ std::cout << "ransac: nb ransac iterations: "<< m_nbRansacIterations <<std::endl;
+
+ if(!ransac_reprojection_error_node)
+ std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError << " (default)" << std::endl;
+ else
+ std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError <<std::endl;
+
+ if(!nb_ransac_min_inlier_count_node)
+ std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount << " (default)" << std::endl;
+ else
+ std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount <<std::endl;
+
+ if(!ransac_threshold_node)
+ std::cout << "ransac: ransac threshold in meter (for ViSP function): "<< m_ransacThreshold << " (default)" << std::endl;
+ else
+ std::cout << "ransac: ransac threshold in meter (for ViSP function): "<< m_ransacThreshold <<std::endl;
+
+ if(!ransac_consensus_percentage_node)
+ std::cout << "ransac: consensus percentage: "<< m_ransacConsensusPercentage << " (default)" << std::endl;
+ else
+ std::cout << "ransac: consensus percentage: "<< m_ransacConsensusPercentage <<std::endl;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_vision.a(vpXmlConfigParserKeyPoint.cpp.o) has no symbols
+void dummy_vpXmlConfigParserKeyPoint() {};
+#endif //VISP_HAVE_XML2
diff --git a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp
new file mode 100644
index 0000000..a492276
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp
@@ -0,0 +1,1663 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Levenberg Marquartd.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <iostream>
+#include <algorithm> // std::min
+
+#include <visp3/vision/vpLevenbergMarquartd.h>
+#include <visp3/core/vpMath.h>
+
+#define SIGN(x) ((x) < 0 ? -1 : 1)
+#define SWAP(a,b,c) {(c) = (a); (a) = (b); (b) = (c);}
+#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
+#define TRUE 1
+#define FALSE 0
+
+/*
+ * PROCEDURE : enorm
+ *
+ * ENTREE :
+ *
+ * x Vecteur de taille "n"
+ * n Taille du vecteur "x"
+ *
+ * DESCRIPTION :
+ * La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n"
+ * La norme euclidienne est calculee par accumulation de la somme des carres
+ * dans les trois directions. Les sommes des carres pour les petits et grands
+ * elements sont mis a echelle afin d'eviter les overflows. Des underflows non
+ * destructifs sont autorisee. Les underflows et overflows sont evites dans le
+ * calcul des sommes des carres non encore mis a echelle par les elements
+ * intermediaires. La definition des elements petit, intermediaire et grand
+ * depend de deux constantes : rdwarf et rdiant. Les restrictions principales
+ * sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est
+ * pas en overflow. Les constantes donnees ici conviennent pour la plupart des
+ * pc connus.
+ *
+ * RETOUR :
+ * En cas de succes, la valeur retournee est la norme euclidienne du vecteur
+ * Sinon, la valeur -1 est retournee et la variable globale "errno" est
+ * initialisee pour indiquee le type de l'erreur.
+ *
+ */
+double enorm (const double *x, int n)
+{
+ const double rdwarf = 3.834e-20;
+ const double rgiant = 1.304e19;
+
+ int i;
+ double agiant, floatn;
+ double norm_eucl = 0.0;
+ double s1 = 0.0, s2 = 0.0, s3 = 0.0;
+ double xabs;
+ double x1max = 0.0 , x3max = 0.0;
+
+ floatn = (double) n;
+ agiant = rgiant / floatn;
+
+ for (i = 0; i < n; i++)
+ {
+ xabs = std::fabs(x[i]);
+ if ((xabs > rdwarf) && (xabs < agiant))
+ {
+ /*
+ * somme pour elements intemediaires.
+ */
+ s2 += xabs * xabs;
+ }
+
+ else if (xabs <= rdwarf)
+ {
+ /*
+ * somme pour elements petits.
+ */
+ if (xabs <= x3max)
+ {
+ //if (xabs != 0.0)
+ if (xabs > std::numeric_limits<double>::epsilon())
+ s3 += (xabs / x3max) * (xabs / x3max);
+ }
+ else
+ {
+ s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
+ x3max = xabs;
+ }
+ }
+
+ else
+ {
+ /*
+ * somme pour elements grand.
+ */
+ if (xabs <= x1max)
+ {
+ s1 += (xabs / x1max) * (xabs / x1max);
+ }
+ else
+ {
+ s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
+ x1max = xabs;
+ }
+ }
+ }
+
+ /*
+ * calcul de la norme.
+ */
+ //if (s1 == 0.0)
+ if (std::fabs(s1) <= std::numeric_limits<double>::epsilon())
+ {
+ //if (s2 == 0.0)
+ if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
+ norm_eucl = x3max * sqrt(s3);
+ else if (s2 >= x3max)
+ norm_eucl = sqrt (s2 * (1.0 + ( x3max / s2) * (x3max * s3)));
+ else if (s2 < x3max)
+ norm_eucl = sqrt (x3max * ((s2 / x3max) + (x3max * s3)));
+ }
+ else
+ norm_eucl = x1max * sqrt (s1 + (s2 / x1max) / x1max);
+
+ return (norm_eucl);
+}
+
+/* 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 triangulaire
+ * superieure de "r".
+ *
+ * ldr Taille maximum de la matrice "r". "ldr" >= "n".
+ *
+ * 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".
+ *
+ * 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.
+ *
+ * par Estimee initiale du parametre de Levenberg-Marquardt.
+ * wa1, wa2 Vecteurs de taille "n" de travail.
+ *
+ * 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 ,
+ *
+ * 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 ,
+ * ou "par" est positif et abs(dxnorm-delta) <= 0.1 * delta.
+ * Cette procedure complete la solution du probleme si on lui fourni les infos
+ * nessaires de la factorisation qr, avec pivotage de colonnes de a.
+ * Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
+ * de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
+ * avec les elements diagonaux classes par ordre decroissant de leur valeur, lmpar
+ * attend une matrice triangulaire superieure complete, la matrice de permutation
+ * "p" et les "n" premiers elements de (q transpose) * b. En sortie, la procedure
+ * lmpar fournit aussi une matrice triangulaire superieure "s" telle que
+ *
+ * t t t
+ * p * (a * a + par * d * d )* p = s * s .
+ *
+ * "s" est utilise a l'interieure de lmpar et peut etre d'un interet separe.
+ *
+ * Seulement quelques iterations sont necessaire pour la convergence de
+ * 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 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".
+ *
+ * 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, tol001 = 0.001; /* tolerance a 0.1 et a 0.001 */
+
+ int l, jm1;
+ unsigned int jp1;
+ unsigned int iter; /* compteur d'iteration */
+ int nsing; /* nombre de singularite de la matrice */
+ double dxnorm, fp, gnorm, parc;
+ double parl, paru; /* borne inf et sup de par */
+ double sum, temp;
+ 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.
+ */
+ nsing = n;
+
+ for (int i = 0; i < n; i++)
+ {
+ wa1[i] = qtb[i];
+ double *pt = MIJ(r, i, i, ldr);
+ //if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
+ if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
+ nsing = i - 1;
+
+ if (nsing < n)
+ wa1[i] = 0.0;
+ }
+
+ if (nsing >= 0)
+ {
+ for (int k = 0; k < nsing; k++)
+ {
+ int i = nsing - 1 - k;
+ wa1[i] /= *MIJ(r, i, i, ldr);
+ temp = wa1[i];
+ jm1 = i - 1;
+
+ if (jm1 >= 0)
+ {
+ for (unsigned int j = 0; j <= (unsigned int)jm1; j++)
+ wa1[j] -= *MIJ(r, i, j, ldr) * temp;
+ }
+ }
+ }
+
+ for (int j = 0; j < n; j++)
+ {
+ l = ipvt[j];
+ x[l] = wa1[j];
+ }
+
+ /*
+ * initialisation du compteur d'iteration.
+ * evaluation de la fonction a l'origine, et test
+ * d'acceptation de la direction de Gauss-Newton.
+ */
+ iter = 0;
+
+ for (int i = 0; i < n; i++)
+ wa2[i] = diag[i] * x[i];
+
+ dxnorm = enorm(wa2, n);
+
+ fp = dxnorm - *delta;
+
+ 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.
+ */
+ parl = 0.0;
+
+ if (nsing >= n)
+ {
+ for (int i = 0; i < n; i++)
+ {
+ l = ipvt[i];
+ wa1[i] = diag[l] * (wa2[l] / dxnorm);
+ }
+
+ for (int i = 0; i < n; i++)
+ {
+ long im1;
+ sum = 0.0;
+ im1 = (i - 1L);
+
+ if (im1 >= 0)
+ {
+ for (unsigned int j = 0; j <= (unsigned int)im1; j++)
+ sum += (*MIJ(r, i, j, ldr) * wa1[j]);
+ }
+ wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
+ }
+
+ temp = enorm(wa1, n);
+ parl = ((fp / *delta) / temp) / temp;
+ }
+
+ /*
+ * calcul d'une limite superieure, paru, pour le zero de la
+ * fonction.
+ */
+ for (int i = 0; i < n; i++)
+ {
+ sum = 0.0;
+
+ for (int j = 0; j <= i; j++)
+ sum += *MIJ(r, i, j, ldr) * qtb[j];
+
+ l = ipvt[i];
+ wa1[i] = sum / diag[l];
+ }
+
+ gnorm = enorm(wa1, n);
+ paru = gnorm / *delta;
+
+ //if (paru == 0.0)
+ if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
+ paru = dwarf / vpMath::minimum(*delta, tol1);
+
+ /*
+ * Si "par" en entree tombe hors de l'intervalle (parl,paru),
+ * on le prend proche du point final.
+ */
+
+ *par = vpMath::maximum(*par, parl);
+ *par = vpMath::maximum(*par, paru);
+
+ //if (*par == 0.0)
+ if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() )
+ *par = gnorm / dxnorm;
+
+ /*
+ * debut d'une iteration.
+ */
+ for(;;) //iter >= 0)
+ {
+ iter++;
+
+ /*
+ * evaluation de la fonction a la valeur courant
+ * de "par".
+ */
+ //if (*par == 0.0)
+ if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() )
+ *par = vpMath::maximum(dwarf, (tol001 * paru));
+
+ temp = sqrt(*par);
+
+ for (int i = 0; i < n; i++)
+ wa1[i] = temp * diag[i];
+
+ qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
+
+ for (int i = 0; i < n; i++)
+ wa2[i] = diag[i] * x[i];
+
+ dxnorm = enorm(wa2, n);
+ temp = fp;
+ 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.
+ */
+ //if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <= temp)
+ // && (temp < 0.0)) || (iter == 10))
+ if ((std::fabs(fp) <= tol1 * (*delta)) || ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp)
+ && (temp < 0.0)) || (iter == 10))
+ {
+ // terminaison.
+
+ // Remove the two next lines since this is a dead code
+ /* if (iter == 0)
+ *par = 0.0; */
+
+ return (0);
+ }
+
+ /*
+ * calcul de la correction de Newton.
+ */
+
+ for (int i = 0; i < n; i++)
+ {
+ l = ipvt[i];
+ wa1[i] = diag[l] * (wa2[l] / dxnorm);
+ }
+
+ for (unsigned int i = 0; i < (unsigned int)n; i++)
+ {
+ wa1[i] = wa1[i] / sdiag[i];
+ temp = wa1[i];
+ jp1 = i + 1;
+ if ( (unsigned int) n >= jp1)
+ {
+ for (unsigned int j = jp1; j < (unsigned int)n; j++)
+ wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
+ }
+ }
+
+ temp = enorm(wa1, n);
+ parc = ((fp / *delta) / temp) / temp;
+
+ /*
+ * selon le signe de la fonction, mise a jour
+ * de parl ou paru.
+ */
+ if (fp > 0.0)
+ parl = vpMath::maximum(parl, *par);
+
+ if (fp < 0.0)
+ paru = vpMath::minimum(paru, *par);
+
+ /*
+ * calcul d'une estimee ameliree de "par".
+ */
+ *par = vpMath::maximum(parl, (*par + parc));
+ }/* fin boucle sur iter */
+ }/* fin fp > tol1 * delta */
+
+ /*
+ * terminaison.
+ */
+ if (iter == 0)
+ *par = 0.0;
+
+ return (0);
+}
+
+/*
+ * PROCEDURE : pythag
+ *
+ * ENTREES :
+ * a, b Variables dont on veut la racine carre de leur somme de carre
+ *
+ * DESCRIPTION :
+ * La procedure calcule la racine carre de la somme des carres de deux nombres
+ * en evitant l'overflow ou l'underflow destructif.
+ *
+ * RETOUR :
+ * La procedure retourne la racine carre de a^2 + b^2.
+ *
+ */
+double pythag (double a, double b)
+{
+ double pyth, p, r, s, t, u;
+
+ p = vpMath::maximum(std::fabs(a), std::fabs(b));
+
+ //if (p == 0.0)
+ if (std::fabs(p) <= std::numeric_limits<double>::epsilon() )
+ {
+ pyth = p;
+ return (pyth);
+ }
+
+ r = (std::min(std::fabs(a), std::fabs(b)) / p) * (std::min(std::fabs(a), std::fabs(b)) / p);
+ t = 4.0 + r;
+
+ //while (t != 4.0)
+ while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t,4.0)) * std::numeric_limits<double>::epsilon() )
+ {
+ s = r / t;
+ u = 1.0 + 2.0 * s;
+ p *= u;
+ r *= (s / u) * (s / u);
+ t = 4.0 + r;
+ }
+
+ pyth = p;
+ return (pyth);
+}
+
+/*
+ * 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 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 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.
+ *
+ * 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 taille "m" x "n".
+ * La procedure determine une matrice orthogonale "q", une matrice de permutation
+ * "p" et une matrice trapesoidale superieure "r" dont les elements diagonaux
+ * sont ordonnes dans l'ordre decroissant de leurs 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
+ *
+ * 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 "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.
+ *
+ */
+int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
+ int /* lipvt */, double *rdiag, double *acnorm, double *wa)
+{
+ const double tolerance = 0.05;
+
+ int i, j, ip1, k, kmax, minmn;
+ double ajnorm, epsmch;
+ double sum, temp, tmp;
+
+ /*
+ * epsmch est la precision machine.
+ */
+ epsmch = std::numeric_limits<double>::epsilon();
+
+ /*
+ * 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);
+ rdiag[i] = acnorm[i];
+ wa[i] = rdiag[i];
+
+ if (pivot)
+ ipvt[i] = i;
+ }
+ /*
+ * reduction de "a" en "r" avec les tranformations de Householder.
+ */
+ minmn = vpMath::minimum(m, n);
+ for (i = 0; i < minmn; i++)
+ {
+ if (pivot)
+ {
+ /*
+ * met la ligne de plus grande norme en position
+ * de pivot.
+ */
+ kmax = i;
+ for (k = i; k < m; k++)
+ {
+ if (rdiag[k] > rdiag[kmax])
+ kmax = k;
+ }
+
+ if (kmax != i)
+ {
+ for (j = 0; j < n; j++)
+ SWAP(*MIJ(a, i, j, lda),
+ *MIJ(a, kmax, j, lda), tmp);
+
+ rdiag[kmax] = rdiag[i];
+ wa[kmax] = wa[i];
+
+ SWAP( ipvt[i], ipvt[kmax], k);
+ }
+ }
+
+ /*
+ * calcul de al transformationde Householder afin de reduire
+ * la jeme ligne de "a" a un multiple du jeme vecteur unite.
+ */
+ ajnorm = enorm(MIJ(a, i, i, lda), n - i);
+
+ //if (ajnorm != 0.0)
+ if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon() )
+ {
+ if (*MIJ(a, i, i, lda) < 0.0)
+ ajnorm = -ajnorm;
+
+ for (j = i; j < n; j++)
+ *MIJ(a, i, j, lda) /= ajnorm;
+ *MIJ(a, i, i, lda) += 1.0;
+
+ /*
+ * application de la tranformation aux lignes
+ * restantes et mise a jour des normes.
+ */
+ ip1 = i + 1;
+
+ if (m >= ip1)
+ {
+ for (k = ip1; k < m; k++)
+ {
+ sum = 0.0;
+ for (j = i; j < n; j++)
+ sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
+
+ temp = sum / *MIJ(a, i, i, lda);
+
+ for (j = i; j < n; j++)
+ *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
+
+ //if (pivot && rdiag[k] != 0.0)
+ if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon()) )
+ {
+ temp = *MIJ (a, k, i, lda) / rdiag[k];
+ rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
+
+ if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch)
+ {
+ rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n -1 - (int) i));
+ wa[k] = rdiag[k];
+ }
+ }
+ }/* fin boucle for k */
+
+ }
+
+ } /* fin if (ajnorm) */
+
+ rdiag[i] = -ajnorm;
+ } /* fin for (i = 0; i < minmn; i++) */
+ return (0);
+}
+
+/* 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 superieure
+ * complete de "r".
+ * 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".
+ *
+ * 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 ,
+ *
+ * Au sens des moindres carres.
+ *
+ * Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de "q"
+ * sont orthogonales et "r" est une matrice traingulaire superieure dont les
+ * elements diagonaux sont classes de l'ordre decroissant de leur valeur. Cette
+ * procedure attend donc la matrice triangulaire superieure remplie "r", 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
+ *
+ * t t
+ * r * z = q * b , p * d * p * z = 0 ,
+ *
+ * Ou x = p * z. Si ce systeme ne possede pas de rangee pleine, alors une
+ * solution au moindre carre est obtenue. En sortie, la procedure fournit aussi
+ * une matrice triangulaire superieure "s" tel que
+ *
+ * t t t
+ * p * (a * a + d * d) * p = s * s .
+ *
+ * "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 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" 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, jp1, k, kp1, l; /* compteur de boucle */
+ int nsing;
+ double cosi, cotg, qtbpj, sinu, sum, 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".
+ */
+ for (i = 0; i < n; i++)
+ {
+ for (j = i; j < n; j++)
+ *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
+
+ x[i] = *MIJ(r, i, i, ldr);
+ wa[i] = qtb[i];
+ }
+
+ /*
+ * 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.
+ */
+ l = ipvt[i];
+
+ //if (diag[l] != 0.0)
+ if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon())
+ {
+ for (k = i; k < n; k++)
+ sdiag[k] = 0.0;
+
+ 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.
+ */
+
+ qtbpj = 0.0;
+
+ for (k = i; k < n; k++)
+ {
+ /*
+ * determination d'une rotation qui elimine
+ * les elements appropriees dans la colonne
+ * courante de d.
+ */
+
+ //if (sdiag[k] != 0.0)
+ if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon())
+ {
+ if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k]))
+ {
+ tg = sdiag[k] / *MIJ(r, k, k, ldr);
+ cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
+ sinu = cosi * tg;
+ }
+ else
+ {
+ cotg = *MIJ(r, k, k, ldr) / sdiag[k];
+ sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
+ cosi = sinu * cotg;
+ }
+
+ /*
+ * 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;
+ qtbpj = -sinu * wa[k] + cosi * qtbpj;
+ wa[k] = temp;
+
+ /*
+ * accumulation des tranformations dans
+ * les lignes de s.
+ */
+
+ kp1 = k + 1;
+
+ if ( n >= kp1)
+ {
+ for (j = kp1; j < n; j++)
+ {
+ temp = cosi * *MIJ(r, k, j, ldr) +
+ sinu * sdiag[j];
+ sdiag[j] = - sinu * *MIJ(r, k, j, ldr) +
+ cosi * sdiag[j];
+ *MIJ(r, k, j, ldr) = temp;
+ }
+ }
+ }/* fin if diag[] !=0 */
+ } /* fin boucle for k -> n */
+ }/* fin if diag =0 */
+
+ /*
+ * 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 */
+
+ /*
+ * resolution du systeme triangulaire pour z. Si le systeme est
+ * singulier, on obtient une solution au moindres carres.
+ */
+ nsing = n;
+
+ for (i = 0; i < n; i++)
+ {
+ //if (sdiag[i] == 0.0 && nsing == n)
+ if ( (std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
+ nsing = i - 1;
+
+ if (nsing < n)
+ wa[i] = 0.0;
+ }
+
+ if (nsing >= 0)
+ {
+ for (k = 0; k < nsing; k++)
+ {
+ i = nsing - 1 - k;
+ sum = 0.0;
+ jp1 = i + 1;
+
+ if (nsing >= jp1)
+ {
+ for (j = jp1; j < nsing; j++)
+ sum += *MIJ(r, i, j, ldr) * wa[j];
+ }
+ wa[i] = (wa[i] - sum) / sdiag[i];
+ }
+ }
+ /*
+ * permutation arriere des composants de z et des componants de x.
+ */
+
+ for (j = 0; j < n; j++)
+ {
+ l = ipvt[j];
+ x[l] = wa[j];
+
+ }
+ return (0);
+}
+
+/*
+ * 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 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 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 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 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 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. ou a "factor" lui meme. Dans la plupart des cas,
+ * "factor" doit se trouve dans l'intervalle (1, 100); ou 100 est
+ * la valeur recommandee.
+ * nprint Controle de l'impression des iterees (si valeur positive).
+ * Dans ce cas, fcn est appelle avec iflag = 0 au debut de la
+ * premiere iteration et 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 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 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 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 :
+ * 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
+ * etre appele comme suit :
+ * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
+ *
+ * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
+ * 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.
+ *
+ * RETOUR :
+ * En cas de succes, la valeur zero est retournee.
+ * Sinon la valeur -1 est retournee.
+ */
+int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
+ double *jac, int ldfjac, int iflag), int m, int n, double *x,
+ double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
+ double gtol, unsigned int maxfev, double *diag, int mode,
+ const double factor, int nprint, int *info, unsigned int *nfev,
+ int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
+ double *wa3, double *wa4)
+{
+ const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
+ int oncol = TRUE;
+ int iflag, iter;
+ int count = 0;
+ int i, j, l;
+ double actred, delta, dirder, epsmch, fnorm, fnorm1, gnorm;
+ double ratio = std::numeric_limits<double>::epsilon();
+ double par, pnorm, prered;
+ double sum, temp, temp1, temp2, xnorm = 0.0;
+
+ /* epsmch est la precision machine. */
+ epsmch = std::numeric_limits<double>::epsilon();
+
+ *info = 0;
+ iflag = 0;
+ *nfev = 0;
+ *njev = 0;
+
+ /* verification des parametres d'entree. */
+
+ /*if (n <= 0)
+ return 0;*/
+ if (m < n)
+ return 0;
+ if (ldfjac < m)
+ return 0;
+ if (ftol < 0.0)
+ return 0;
+ if (xtol < 0.0)
+ return 0;
+ if (gtol < 0.0)
+ return 0;
+ if (maxfev == 0)
+ return 0;
+ if (factor <= 0.0)
+ return 0;
+ if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0)
+ || (gtol < 0.0) || (maxfev == 0) || (factor <= 0.0))
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+
+ if (mode == 2)
+ {
+ for (j = 0; j < n; j++)
+ {
+ if (diag[j] <= 0.0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+ }
+ }
+
+ /*
+ * evaluation de la fonction au point de depart
+ * et calcul de sa norme.
+ */
+ iflag = 1;
+
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+
+ *nfev = 1;
+
+ if (iflag < 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+
+ fnorm = enorm(fvec, m);
+
+ /*
+ * initialisation du parametre de Levenberg-Marquardt
+ * et du conteur d'iteration.
+ */
+
+ par = 0.0;
+ iter = 1;
+
+ /*
+ * debut de la boucle la plus externe.
+ */
+ while (count < (int)maxfev)
+ {
+ count++;
+ /*
+ * calcul de la matrice jacobienne.
+ */
+
+ iflag = 2;
+
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ (*njev) ++;
+
+ if (iflag < 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+
+ /*
+ * si demandee, appel de fcn pour impression des iterees.
+ */
+ if (nprint > 0)
+ {
+ iflag = 0;
+ if ((iter-1) % nprint == 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ if (iflag < 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+ }
+
+ /*
+ * calcul de la factorisation qr du jacobien.
+ */
+ 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.
+ */
+
+ if (iter == 1)
+ {
+ if (mode != 2)
+ {
+ for (j = 0; j < n; j++)
+ {
+ diag[j] = wa2[j];
+ //if (wa2[j] == 0.0)
+ if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
+ diag[j] = 1.0;
+ }
+ }
+
+ /*
+ * 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++)
+ wa3[j] = diag[j] * x[j];
+
+ xnorm = enorm (wa3, n);
+ delta = factor * xnorm;
+
+ //if (delta == 0.0)
+ if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
+ delta = factor;
+ }
+
+ /*
+ * formation de (q transpose) * fvec et stockage des n premiers
+ * composants dans qtf.
+ */
+ for (i = 0; i < m; i++)
+ wa4[i] = fvec[i];
+
+ for (i = 0; i < n; i++)
+ {
+ double *pt = MIJ(fjac, i, i, ldfjac);
+ //if (*MIJ(fjac, i, i, ldfjac) != 0.0)
+ if (std::fabs(*pt) > std::numeric_limits<double>::epsilon() )
+ {
+ sum = 0.0;
+
+ for (j = i; j < m; j++)
+ sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
+
+ temp = - sum / *MIJ(fjac, i, i, ldfjac);
+
+ for (j = i; j < m; j++)
+ wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
+
+ }
+
+ *MIJ(fjac, i, i, ldfjac) = wa1[i];
+ qtf[i] = wa4[i];
+ }
+
+ /*
+ * calcul de la norme du gradient mis a l'echelle.
+ */
+
+ gnorm = 0.0;
+
+ //if (fnorm != 0.0)
+ if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon())
+ {
+ for (i = 0; i < n; i++)
+ {
+ l = ipvt[i];
+ //if (wa2[l] != 0.0)
+ if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon())
+ {
+ sum = 0.0;
+ for (j = 0; j <= i; j++)
+ sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
+
+ gnorm = vpMath::maximum(gnorm, std::fabs(sum / wa2[l]));
+ }
+ }
+ }
+
+ /*
+ * test pour la convergence de la norme du gradient .
+ */
+
+ if (gnorm <= gtol)
+ *info = 4;
+
+ if (*info != 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+
+ /*
+ * remise a l'echelle si necessaire.
+ */
+
+ if (mode != 2)
+ {
+ for (j = 0; j < n; j++)
+ diag[j] = vpMath::maximum(diag[j], wa2[j]);
+ }
+
+ /*
+ * debut de la boucle la plus interne.
+ */
+ ratio = 0.0;
+ while (ratio < tol0001)
+ {
+
+ /*
+ * determination du parametre de Levenberg-Marquardt.
+ */
+ lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1,
+ wa2, wa3, wa4);
+
+ /*
+ * stockage de la direction p et x + p. calcul de la norme de p.
+ */
+
+ for (j = 0; j < n; j++)
+ {
+ wa1[j] = - wa1[j];
+ wa2[j] = x[j] + wa1[j];
+ wa3[j] = diag[j] * wa1[j];
+ }
+
+ pnorm = enorm(wa3, n);
+
+ /*
+ * a la premiere iteration, ajustement de la premiere limite de
+ * l'etape.
+ */
+
+ if (iter == 1)
+ delta = vpMath::minimum(delta, pnorm);
+
+ /*
+ * evaluation de la fonction en x + p et calcul de leur norme.
+ */
+
+ iflag = 1;
+ (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
+
+ (*nfev)++;
+
+ if (iflag < 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+
+ fnorm1 = enorm(wa4, m);
+
+ /*
+ * calcul de la reduction reelle mise a l'echelle.
+ */
+
+ actred = - 1.0;
+
+ if ((tol1 * fnorm1) < fnorm)
+ actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
+
+ /*
+ * calcul de la reduction predite mise a l'echelle et
+ * de la derivee directionnelle mise a l'echelle.
+ */
+
+ for (i = 0; i < n; i++)
+ {
+ wa3[i] = 0.0;
+ l = ipvt[i];
+ temp = wa1[l];
+ for (j = 0; j <= i; j++)
+ wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
+ }
+
+ temp1 = enorm(wa3, n) / fnorm;
+ temp2 = (sqrt(par) * pnorm) / fnorm;
+ prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
+ dirder = - ((temp1 * temp1) + (temp2 * temp2));
+
+ /*
+ * calcul du rapport entre la reduction reel et predit.
+ */
+
+ ratio = 0.0;
+
+ //if (prered != 0.0)
+ if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
+ ratio = actred / prered;
+
+ /*
+ * mise a jour de la limite de l'etape.
+ */
+
+ if (ratio > tol25)
+ {
+ //if ((par == 0.0) || (ratio <= tol75))
+ if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75))
+ {
+ delta = pnorm / tol5;
+ par *= tol5;
+ }
+ }
+ else
+ {
+ if (actred >= 0.0)
+ temp = tol5;
+
+ else
+ temp = tol5 * dirder / (dirder + tol5 * actred);
+
+ if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
+ temp = tol1;
+
+ delta = temp * vpMath::minimum(delta, (pnorm / tol1));
+ par /= temp;
+ }
+
+ /*
+ * test pour une iteration reussie.
+ */
+ if (ratio >= tol0001)
+ {
+ /*
+ * iteration reussie. mise a jour de x, de fvec, et de
+ * leurs normes.
+ */
+
+ for (j = 0; j < n; j++)
+ {
+ x[j] = wa2[j];
+ wa2[j] = diag[j] * x[j];
+ }
+
+ for (i = 0; i < m; i++)
+ fvec[i] = wa4[i];
+
+ xnorm = enorm(wa2, n);
+ fnorm = fnorm1;
+ iter++;
+ }
+
+ /*
+ * tests pour convergence.
+ */
+
+ if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
+ *info = 1;
+
+ if (delta <= xtol * xnorm)
+ *info = 2;
+
+ if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)
+ && *info == 2)
+ *info = 3;
+
+ if (*info != 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m,n,x,fvec,fjac,ldfjac, iflag);
+
+ return (count);
+ }
+ /*
+ * tests pour termination et
+ * verification des tolerances.
+ */
+
+ if (*nfev >= maxfev)
+ *info = 5;
+
+ if ((std::fabs(actred) <= epsmch) && (prered <= epsmch)
+ && (tol5 * ratio <= 1.0))
+ *info = 6;
+
+ if (delta <= epsmch * xnorm)
+ *info = 7;
+
+ if (gnorm <= epsmch)
+ *info = 8;
+
+ if (*info != 0)
+ {
+ /*
+ * termination, normal ou imposee par l'utilisateur.
+ */
+ if (iflag < 0)
+ *info = iflag;
+
+ iflag = 0;
+
+ if (nprint > 0)
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
+
+ return (count);
+ }
+ }/* fin while ratio >=tol0001 */
+ }/*fin while 1*/
+
+ return 0 ;
+}
+
+
+
+/*
+ * 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 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 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 identite.
+ *
+ * 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.
+ * Cette procedure appele la procedure generale au moindre carre lmder.
+ *
+ * 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
+ * etre appele comme suit :
+ * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
+ *
+ * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
+ * 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.
+ *
+ * RETOUR :
+ * En cas de succes, la valeur zero est retournee.
+ * Sinon, la valeur -1.
+ *
+ */
+int lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
+ double *jac, int ldfjac, int iflag),
+ int m, int n, double *x, double *fvec, double *fjac,
+ int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa)
+{
+ const double factor = 100.0;
+ unsigned int maxfev, nfev;
+ int mode, njev, nprint;
+ double ftol, gtol, xtol;
+
+ *info = 0;
+
+ /* verification des parametres en entree qui causent des erreurs */
+
+
+ if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) ||
+ (lwa < (5 * n + m)) )
+ {
+ printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0) , (lwa < (5 * n + m))) ;
+ return (-1);
+ }
+
+ /* appel a lmder */
+
+ maxfev = (unsigned int)(100 * (n + 1));
+ ftol = tol;
+ xtol = tol;
+ gtol = 0.0;
+ mode = 1;
+ nprint = 0;
+
+ lmder (ptr_fcn , m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa,
+ mode, factor, nprint, info, &nfev, &njev, ipvt, &wa[n], &wa[2 * n],
+ &wa[3 * n], &wa[4 * n], &wa[5 * n]);
+
+ if (*info == 8)
+ *info = 4;
+
+ return (0);
+}
+
+#undef TRUE
+#undef FALSE
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp
new file mode 100644
index 0000000..d491384
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPose.cpp
@@ -0,0 +1,743 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+
+/*!
+\file vpPose.cpp
+\brief Fichier contenant la classe vpPose (tout ce qui est necessaire
+pour faire du calcul de pose par difference methode
+*/
+
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/vision/vpPoseException.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpMath.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+#define DEBUG_LEVEL1 0
+/*!
+\brief basic initialisation (called by the constructors)
+*/
+void
+vpPose::init()
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::Init() " << std::endl ;
+#endif
+ npt = 0 ;
+ listP.clear() ;
+ c3d.clear();
+
+ lambda = 0.25 ;
+
+ vvsIterMax = 200 ;
+
+ distanceToPlaneForCoplanarityTest = 0.001 ;
+
+ computeCovariance = false;
+
+ ransacMaxTrials = 1000;
+ ransacThreshold = 0.0001;
+ ransacNbInlierConsensus = 4;
+
+ residual = 0;
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::Init() " << std::endl ;
+#endif
+
+}
+
+/*! Defaukt constructor. */
+vpPose::vpPose()
+ : npt(0), listP(), residual(0), lambda(0.25), vvsIterMax(200), c3d(),
+ computeCovariance(false), covarianceMatrix(),
+ ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
+ distanceToPlaneForCoplanarityTest(0.001)
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::vpPose() " << std::endl ;
+#endif
+
+ init() ;
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::vpPose() " << std::endl ;
+#endif
+
+}
+
+/*!
+ Destructor that deletes the array of point (freed the memory).
+*/
+vpPose::~vpPose()
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::~vpPose() " << std::endl ;
+#endif
+
+ listP.clear() ;
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::~vpPose() " << std::endl ;
+#endif
+}
+/*!
+ Delete the array of point
+*/
+void
+vpPose::clearPoint()
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::ClearPoint() " << std::endl ;
+#endif
+
+ listP.clear() ;
+ npt = 0 ;
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::ClearPoint() " << std::endl ;
+#endif
+}
+
+/*!
+\brief Add a new point in the array of point.
+\param newP : New point to add in the array of point.
+\warning Considering a point from the class vpPoint, X, Y, and Z will
+represent the 3D information and x and y its 2D information.
+These 5 fields must be initialized to be used within this library
+*/
+void
+vpPose::addPoint(const vpPoint& newP)
+{
+
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::AddPoint(Dot) " << std::endl ;
+#endif
+
+ listP.push_back(newP);
+ npt ++ ;
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::AddPoint(Dot) " << std::endl ;
+#endif
+}
+
+
+void
+vpPose::setDistanceToPlaneForCoplanarityTest(double d)
+{
+ distanceToPlaneForCoplanarityTest = d ;
+}
+
+/*!
+\brief test the coplanarity of the set of points
+
+\param coplanar_plane_type:
+ 1: if plane x=cst
+ 2: if plane y=cst
+ 3: if plane z=cst
+ 4: if the points are collinear.
+ 0: any other plane
+\return true if points are coplanar
+false if not
+*/
+bool
+vpPose::coplanar(int &coplanar_plane_type)
+{
+ coplanar_plane_type = 0;
+ if (npt <2)
+ {
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+
+ if (npt ==3) return true ;
+
+ double x1=0,x2=0,x3=0,y1=0,y2=0,y3=0,z1=0,z2=0,z3=0 ;
+
+ std::list<vpPoint>::const_iterator it = listP.begin();
+
+ vpPoint P1, P2, P3 ;
+
+ // Get three 3D points that are not collinear and that is not at origin
+ bool degenerate = true;
+ bool not_on_origin = true;
+ std::list<vpPoint>::const_iterator it_tmp;
+
+ std::list<vpPoint>::const_iterator it_i, it_j, it_k;
+ for (it_i=listP.begin(); it_i != listP.end(); ++it_i) {
+ if (degenerate == false) {
+ //std::cout << "Found a non degenerate configuration" << std::endl;
+ break;
+ }
+ P1 = *it_i;
+ // Test if point is on origin
+ if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ not_on_origin = false;
+ }
+ else {
+ not_on_origin = true;
+ }
+ if (not_on_origin) {
+ it_tmp = it_i; it_tmp ++; // j = i+1
+ for (it_j=it_tmp; it_j != listP.end(); ++it_j) {
+ if (degenerate == false) {
+ //std::cout << "Found a non degenerate configuration" << std::endl;
+ break;
+ }
+ P2 = *it_j;
+ if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ not_on_origin = false;
+ }
+ else {
+ not_on_origin = true;
+ }
+ if (not_on_origin) {
+ it_tmp = it_j; it_tmp ++; // k = j+1
+ for (it_k=it_tmp; it_k != listP.end(); ++it_k) {
+ P3 = *it_k;
+ if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon())
+ && (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ not_on_origin = false;
+ }
+ else {
+ not_on_origin = true;
+ }
+ if (not_on_origin) {
+ x1 = P1.get_oX() ;
+ x2 = P2.get_oX() ;
+ x3 = P3.get_oX() ;
+
+ y1 = P1.get_oY() ;
+ y2 = P2.get_oY() ;
+ y3 = P3.get_oY() ;
+
+ z1 = P1.get_oZ() ;
+ z2 = P2.get_oZ() ;
+ z3 = P3.get_oZ() ;
+
+ vpColVector a_b(3), b_c(3), cross_prod;
+ a_b[0] = x1-x2; a_b[1] = y1-y2; a_b[2] = z1-z2;
+ b_c[0] = x2-x3; b_c[1] = y2-y3; b_c[2] = z2-z3;
+
+ cross_prod = vpColVector::crossProd(a_b, b_c);
+ if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
+ degenerate = true; // points are collinear
+ else
+ degenerate = false;
+ }
+ if (degenerate == false)
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ if (degenerate) {
+ coplanar_plane_type = 4; // points are collinear
+ return true;
+ }
+
+ double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
+ double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
+ double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
+ double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
+
+ //std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d << std::endl;
+ if (std::fabs(b) <= std::numeric_limits<double>::epsilon()
+ && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
+ coplanar_plane_type = 1; // ax=d
+ } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
+ && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
+ coplanar_plane_type = 2; // by=d
+ } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
+ && std::fabs(b) <= std::numeric_limits<double>::epsilon() ) {
+ coplanar_plane_type = 3; // cz=d
+ }
+
+ double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
+
+ double dist;
+
+ for(it=listP.begin(); it != listP.end(); ++it)
+ {
+ P1 = *it ;
+ dist = (a*P1.get_oX() + b*P1.get_oY()+c*P1.get_oZ()+d)/D ;
+ //std::cout << "dist= " << dist << std::endl;
+
+ if (fabs(dist) > distanceToPlaneForCoplanarityTest)
+ {
+ vpDEBUG_TRACE(10," points are not coplanar ") ;
+ // TRACE(" points are not coplanar ") ;
+ return false ;
+ }
+ }
+
+ vpDEBUG_TRACE(10," points are coplanar ") ;
+ // vpTRACE(" points are coplanar ") ;
+
+ return true ;
+}
+
+/*!
+\brief Compute and return the residual expressed in meter for
+the pose matrix 'cMo'.
+
+\param cMo : Input pose. The matrix that defines the pose to be tested.
+
+\return The value of he residual in meter.
+
+*/
+double
+vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const
+{
+ double residual_ = 0 ;
+ vpPoint P ;
+ for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ double x = P.get_x() ;
+ double y = P.get_y() ;
+
+ P.track(cMo) ;
+
+ residual_ += vpMath::sqr(x-P.get_x()) + vpMath::sqr(y-P.get_y()) ;
+ }
+ return residual_ ;
+}
+
+
+
+/*!
+\brief Compute the pose according to the desired method
+
+the different method are
+
+LAGRANGE Lagrange approach
+(test is done to switch between planar and
+non planar algorithm)
+
+DEMENTHON Dementhon approach
+(test is done to switch between planar and
+non planar algorithm)
+
+VIRTUAL_VS Virtual visual servoing approach
+
+DEMENTHON_VIRTUAL_VS Virtual visual servoing approach initialized using
+Dementhon approach
+
+LAGRANGE_VIRTUAL_VS Virtual visual servoing initialized using
+Lagrange approach
+
+*/
+bool
+vpPose::computePose(vpPoseMethodType methode, vpHomogeneousMatrix& cMo, bool (*func)(vpHomogeneousMatrix *))
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::ComputePose()" << std::endl ;
+#endif
+
+ if (npt <4)
+ {
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "No enough point ")) ;
+ }
+
+ switch (methode)
+ {
+ case DEMENTHON :
+ case DEMENTHON_VIRTUAL_VS :
+ case DEMENTHON_LOWE :
+ {
+
+ if (npt <4)
+ {
+ vpERROR_TRACE("Dementhon method cannot be used in that case ") ;
+ vpERROR_TRACE("(at least 4 points are required)") ;
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+
+ // test si les point 3D sont coplanaires
+ int coplanar_plane_type = 0;
+ bool plan = coplanar(coplanar_plane_type) ;
+ if (plan == true)
+ {
+ //std::cout << "Plan" << std::endl;
+ try{
+ poseDementhonPlan(cMo);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ //std::cout << "Fin Plan" << std::endl;
+ }
+ else
+ {
+ //std::cout << "No Plan" << std::endl;
+ try{
+ poseDementhonNonPlan(cMo) ;
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ //std::cout << "Fin No Plan" << std::endl;
+ }
+ }
+ break ;
+ case LAGRANGE :
+ case LAGRANGE_VIRTUAL_VS :
+ case LAGRANGE_LOWE :
+ {
+ // test si les point 3D sont coplanaires
+ int coplanar_plane_type;
+ bool plan = coplanar(coplanar_plane_type) ;
+
+ if (plan == true && coplanar_plane_type > 0) // only plane oX=d, oY=d or oZ=d
+ {
+
+ if (coplanar_plane_type == 4)
+ {
+ vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
+ vpERROR_TRACE("(points are collinear)") ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Points are collinear ")) ;
+ }
+ if (npt <4)
+ {
+ vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
+ vpERROR_TRACE("(at least 4 points are required)") ;
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+ try {
+ poseLagrangePlan(cMo, coplanar_plane_type);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ }
+ else
+ {
+ if (npt <4)
+ {
+ vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
+ vpERROR_TRACE("(at least 4 points are required)") ;
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+ try {
+ poseLagrangeNonPlan(cMo);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ }
+ }
+ break;
+ case RANSAC:
+ if (npt <4)
+ {
+ vpERROR_TRACE("Ransac method cannot be used in that case ") ;
+ vpERROR_TRACE("(at least 4 points are required)") ;
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+ try {
+ return poseRansac(cMo, func);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ break;
+ case LOWE :
+ case VIRTUAL_VS:
+ break ;
+ }
+
+ switch (methode)
+ {
+ case LAGRANGE :
+ case DEMENTHON :
+ case RANSAC :
+ break ;
+ case VIRTUAL_VS:
+ case LAGRANGE_VIRTUAL_VS:
+ case DEMENTHON_VIRTUAL_VS:
+ {
+ try
+ {
+ poseVirtualVS(cMo);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ }
+ break ;
+ case LOWE:
+ case LAGRANGE_LOWE:
+ case DEMENTHON_LOWE:
+ {
+ try
+ {
+ poseLowe(cMo);
+ }
+ catch(...)
+ {
+// vpERROR_TRACE(" ") ;
+ throw ;
+ }
+ }
+ break ;
+ }
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::ComputePose()" << std::endl ;
+#endif
+
+ //If here, there was no exception thrown so return true
+ return true;
+}
+
+
+
+void
+vpPose::printPoint()
+{
+ vpPoint P;
+ for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it ;
+
+ std::cout << "3D oP " << P.oP.t() ;
+ std::cout << "3D cP " << P.cP.t() ;
+ std::cout << "2D " << P.p.t() ;
+ }
+}
+
+
+void
+vpPose::display(vpImage<unsigned char> &I,
+ vpHomogeneousMatrix &cMo,
+ vpCameraParameters &cam,
+ double size,
+ vpColor col)
+{
+ vpDisplay::displayFrame(I, cMo, cam, size, col);
+}
+
+
+void
+vpPose::display(vpImage<vpRGBa> &I,
+ vpHomogeneousMatrix &cMo,
+ vpCameraParameters &cam,
+ double size,
+ vpColor col)
+{
+ vpDisplay::displayFrame(I,cMo,cam, size,col);
+}
+
+/*
+\brief display the 3D model in image I
+\warning the 2D coordinate of the point have to be initialized
+(lispP has to be modified)
+*/
+void
+vpPose::displayModel(vpImage<unsigned char> &I,
+ vpCameraParameters &cam,
+ vpColor col)
+{
+ vpPoint P ;
+ vpImagePoint ip;
+ for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
+ vpDisplay::displayCross(I, ip, 5, col) ;
+ // std::cout << "3D oP " << P.oP.t() ;
+ // std::cout << "3D cP " << P.cP.t() ;
+ // std::cout << "2D " << P.p.t() ;
+ }
+}
+
+/*
+\brief display the 3D model in image I
+\warning the 2D coordinate of the point have to be initialized
+(lispP has to be modified)
+*/
+void
+vpPose::displayModel(vpImage<vpRGBa> &I,
+ vpCameraParameters &cam,
+ vpColor col)
+{
+ vpPoint P ;
+ vpImagePoint ip;
+ for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
+ vpDisplay::displayCross(I, ip, 5, col) ;
+ // std::cout << "3D oP " << P.oP.t() ;
+ // std::cout << "3D cP " << P.cP.t() ;
+ // std::cout << "2D " << P.p.t() ;
+ }
+}
+
+
+/*!
+\brief Carries out the camera pose the image of a rectangle and
+the intrinsec parameters, the length on x axis is known but the
+proprtion of the rectangle are unknown.
+
+This method is taken from "Markerless Tracking using Planar Structures
+in the Scene" by Gilles Simon. The idea is to compute the homography H
+giving the image point of the rectangle by associating them with the
+coordinates (0,0)(1,0)(1,1/s)(0,1/s) (the rectangle is on the Z=0 plane).
+If K is the intrinsec parameters matrix, we have s = ||Kh1||/ ||Kh2||. s
+gives us the proportion of the rectangle
+
+\param p1,p2,p3,p4: the image of the corners of the rectangle
+(respectively the image of (0,0),(lx,0),(lx,lx/s) and (0,lx/s)) (input)
+\param cam: the camera used (input)
+\param lx: the rectangle size on the x axis (input)
+\param cMo: the camera pose (output)
+\return int : OK if no pb occurs
+*/
+double
+vpPose::poseFromRectangle(vpPoint &p1,vpPoint &p2,
+ vpPoint &p3,vpPoint &p4,
+ double lx, vpCameraParameters & cam,
+ vpHomogeneousMatrix & cMo)
+{
+
+ std::vector<double> rectx(4) ;
+ std::vector<double> recty(4) ;
+ rectx[0]= 0 ;
+ recty[0]=0 ;
+ rectx[1]=1 ;
+ recty[1]=0 ;
+ rectx[2]=1 ;
+ recty[2]=1 ;
+ rectx[3]=0 ;
+ recty[3]=1 ;
+ std::vector<double> irectx(4) ;
+ std::vector<double> irecty(4) ;
+ irectx[0]=(p1.get_x()) ;
+ irecty[0]=(p1.get_y()) ;
+ irectx[1]=(p2.get_x()) ;
+ irecty[1]=(p2.get_y()) ;
+ irectx[2]=(p3.get_x()) ;
+ irecty[2]=(p3.get_y()) ;
+ irectx[3]=(p4.get_x()) ;
+ irecty[3]=(p4.get_y()) ;
+
+ //calcul de l'homographie
+ vpMatrix H(3,3);
+ vpHomography hom;
+
+ // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
+ vpHomography::HLM(rectx,recty,irectx,irecty,1,hom);
+ for (unsigned int i=0 ; i < 3 ; i++)
+ for(unsigned int j=0 ; j < 3 ; j++)
+ H[i][j] = hom[i][j] ;
+ //calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y axis)
+ vpColVector kh1(3);
+ vpColVector kh2(3);
+ vpMatrix K(3,3);
+ K = cam.get_K();
+ K.eye();
+ vpMatrix Kinv =K.pseudoInverse();
+
+ vpMatrix KinvH =Kinv*H;
+ kh1=KinvH.getCol(0);
+ kh2=KinvH.getCol(1);
+
+ double s= sqrt(kh1.sumSquare())/sqrt(kh2.sumSquare());
+
+ vpMatrix D(3,3);
+ D.eye();
+ D[1][1]=1/s;
+ vpMatrix cHo=H*D;
+
+ //Calcul de la rotation et de la translation
+ // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
+ p1.setWorldCoordinates(0,0,0) ;
+ p2.setWorldCoordinates(lx,0,0) ;
+ p3.setWorldCoordinates(lx,lx/s,0) ;
+ p4.setWorldCoordinates(0,lx/s,0) ;
+
+
+ vpPose P ;
+ P.addPoint(p1) ;
+ P.addPoint(p2) ;
+ P.addPoint(p3) ;
+ P.addPoint(p4) ;
+
+ P.computePose(vpPose::DEMENTHON_LOWE,cMo) ;
+ return lx/s ;
+}
diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp
new file mode 100644
index 0000000..3ccfd4a
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp
@@ -0,0 +1,712 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpMath.h>
+
+#define DEBUG_LEVEL1 0
+#define DEBUG_LEVEL2 0
+#define DEBUG_LEVEL3 0
+
+/* FC
+#ifndef DEG
+#define DEG (180.0/M_PI)
+#endif
+*/
+
+/*!
+\brief Compute the pose using Dementhon approach for non planar objects
+this is a direct implementation of the algorithm proposed by
+Dementhon and Davis in their 1995 paper \cite Dementhon95.
+
+*/
+
+void
+vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
+{
+ double normI = 0., normJ = 0.;
+ double Z0 = 0.;
+ double seuil=1.0;
+ double f=1.;
+
+ vpPoint p0 = listP.front() ;
+
+ c3d.clear();
+ vpPoint P;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = (*it);
+ P.set_oX(P.get_oX()-p0.get_oX()) ;
+ P.set_oY(P.get_oY()-p0.get_oY()) ;
+ P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
+ c3d.push_back(P) ;
+ }
+
+ vpMatrix a(npt,3) ;
+
+ for (unsigned int i=0 ; i < npt ; i++)
+ {
+ a[i][0]=c3d[i].get_oX();
+ a[i][1]=c3d[i].get_oY();
+ a[i][2]=c3d[i].get_oZ();
+ }
+
+ //std::cout << a << std::endl ;
+ // calcul a^T a
+ vpMatrix ata ;
+ ata = a.t()*a ;
+
+ // calcul (a^T a)^-1 par decomposition LU
+ vpMatrix ata1 ;
+ ata1 = ata.pseudoInverse(1e-6) ; //InverseByLU() ;
+
+ vpMatrix b ;
+ b = (a*ata1).t() ;
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "a" << std::endl <<a<<std::endl ;
+ std::cout << "ata" << std::endl <<ata<<std::endl ;
+ std::cout << "ata1" << std::endl <<ata1<<std::endl ;
+ std::cout<< " ata*ata1" << std::endl << ata*ata1 ;
+ std::cout<< " b" << std::endl << (a*ata1).t() ;
+
+ }
+#endif
+
+ // calcul de la premiere solution
+
+ vpColVector eps(npt) ;
+ eps =0 ;
+
+ int cpt = 0 ;
+ vpColVector I, J, k ;
+ I.resize(3) ;
+ J.resize(3) ;
+ k.resize(3) ;
+
+ while(cpt < 20)
+ {
+ I = 0 ;
+ J = 0 ;
+
+ vpColVector xprim(npt) ;
+ vpColVector yprim(npt) ;
+ for (unsigned int i=0;i<npt;i++)
+ {
+ xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].get_x();
+ yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].get_y();
+ }
+ I = b*xprim ;
+ J = b*yprim ;
+ normI = sqrt(I.sumSquare()) ;
+ normJ = sqrt(J.sumSquare()) ;
+ I = I/normI ;
+ J = J/normJ ;
+
+ if (normI+normJ < 1e-10)
+ {
+ //vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
+ throw(vpException(vpException::divideByZeroError,
+ "Division by zero in Dementhon pose computation: normI+normJ = 0")) ;
+ }
+
+ k = vpColVector::cross(I,J) ;
+ Z0=2*f/(normI+normJ);
+ cpt=cpt+1; seuil=0.0;
+ for (unsigned int i=0; i<npt; i++)
+ {
+ double epsi_1 = eps[i] ;
+ eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
+ seuil+=fabs(eps[i]-epsi_1);
+ }
+ if (npt==0)
+ {
+ //vpERROR_TRACE( " npt = 0, division par zero ");
+ throw(vpException(vpException::divideByZeroError,
+ "Division by zero in Dementhon pose computation: no points")) ;
+ }
+ seuil/=npt;
+ }
+ k.normalize();
+ J = vpColVector::cross(k,I) ;
+ /*matrice de passage*/
+
+ cMo[0][0]=I[0];
+ cMo[0][1]=I[1];
+ cMo[0][2]=I[2];
+ cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
+
+ cMo[1][0]=J[0];
+ cMo[1][1]=J[1];
+ cMo[1][2]=J[2];
+ cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
+
+ cMo[2][0]=k[0];
+ cMo[2][1]=k[1];
+ cMo[2][2]=k[2];
+ cMo[2][3]=Z0;
+
+ cMo[0][3] -= (p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2]);
+ cMo[1][3] -= (p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2]);
+ cMo[2][3] -= (p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2]);
+}
+
+
+#define DMIN 0.01 /* distance min entre la cible et la camera */
+#define EPS 0.0000001
+#define EPS_DEM 0.001
+
+static void
+calculRTheta(double s, double c, double &r, double &theta)
+{
+ if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
+ {
+ r = sqrt(sqrt(s*s+c*c));
+ theta = atan2(s,c)/2.0;
+ }
+ else
+ {
+ if (fabs(c) > fabs(s))
+ {
+ r = fabs(c);
+ if (c >= 0.0)
+ theta = M_PI/2;
+ else
+ theta = -M_PI/2;
+ }
+ else
+ {
+ r = fabs(s);
+ if (s >= 0.0)
+ theta = M_PI/4.0;
+ else
+ theta = -M_PI/4.0;
+ }
+ }
+}
+
+static
+void calculSolutionDementhon(double xi0, double yi0,
+ vpColVector &I, vpColVector &J,
+ vpHomogeneousMatrix &cMo )
+{
+
+#if (DEBUG_LEVEL1)
+ std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
+#endif
+
+ double normI, normJ, normk, Z0;
+ vpColVector k(3);
+
+ // normalisation de I et J
+ normI = sqrt(I.sumSquare()) ;
+ normJ = sqrt(J.sumSquare()) ;
+
+ I/=normI;
+ J/=normJ;
+
+
+ k = vpColVector::cross(I,J) ; // k = I^I
+
+ Z0=2.0/(normI+normJ);
+
+ normk = sqrt(k.sumSquare()) ;
+ k /= normk ;
+
+ J = vpColVector::cross(k,I) ;
+
+ //calcul de la matrice de passage
+ cMo[0][0]=I[0];
+ cMo[0][1]=I[1];
+ cMo[0][2]=I[2];
+ cMo[0][3]=xi0*Z0;
+
+ cMo[1][0]=J[0];
+ cMo[1][1]=J[1];
+ cMo[1][2]=J[2];
+ cMo[1][3]=yi0*Z0;
+
+ cMo[2][0]=k[0];
+ cMo[2][1]=k[1];
+ cMo[2][2]=k[2];
+ cMo[2][3]=Z0;
+
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
+#endif
+
+}
+
+int
+vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
+ vpHomogeneousMatrix &cMo)
+{
+
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
+#endif
+
+ unsigned int i, k;
+ int erreur = 0;
+ unsigned int cpt;
+ double s,c,si,co;
+ double smin,smin_old, s1,s2;
+ double r, theta;
+ vpHomogeneousMatrix cMo1,cMo2,cMo_old;
+
+ unsigned int iter_max = 20;
+ vpMatrix eps(iter_max+1,npt) ;
+
+
+ // on test si tous les points sont devant la camera
+ for(i = 0; i < npt; i++)
+ {
+ double z ;
+ z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
+ if (z <= 0.0) erreur = -1;
+ }
+
+ smin = sqrt(computeResidualDementhon(cMo)/npt) ;
+
+ vpColVector xi(npt) ;
+ vpColVector yi(npt) ;
+
+ if (erreur==0)
+ {
+ k=0;
+ for(i = 0; i < npt; i++)
+ {
+ xi[k] = c3d[i].get_x();
+ yi[k] = c3d[i].get_y();
+
+ if (k != 0)
+ { // On ne prend pas le 1er point
+ eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
+ cMo[2][1]*c3d[i].get_oY() +
+ cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
+ }
+ k++;
+ }
+
+
+ vpColVector I0(3) ;
+ vpColVector J0(3) ;
+ vpColVector I(3) ;
+ vpColVector J(3) ;
+
+ smin_old = 2*smin ;
+
+ cpt = 0;
+ while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
+ {
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "cpt " << cpt << std::endl ;
+ std::cout << "smin_old " << smin_old << std::endl ;
+ std::cout << "smin " << smin << std::endl ;
+ }
+#endif
+
+ smin_old = smin;
+ cMo_old = cMo;
+
+ I0 = 0 ;
+ J0 = 0 ;
+
+ for (i=1;i<npt;i++)
+ {
+ s = (1.0+eps[cpt][i])*xi[i] - xi[0];
+ I0[0] += b[0][i-1] * s;
+ I0[1] += b[1][i-1] * s;
+ I0[2] += b[2][i-1] * s;
+ s = (1.0+eps[cpt][i])*yi[i] - yi[0];
+ J0[0] += b[0][i-1] * s;
+ J0[1] += b[1][i-1] * s;
+ J0[2] += b[2][i-1] * s;
+ }
+
+ s = -2.0*(vpColVector::dotProd(I0,J0));
+ c = J0.sumSquare() - I0.sumSquare() ;
+
+ calculRTheta(s,c,r,theta);
+ co = cos(theta);
+ si = sin(theta);
+
+ /* 1ere branche */
+ I = I0 + U*r*co ;
+ J = J0 + U*r*si ;
+
+#if (DEBUG_LEVEL3)
+ {
+ std::cout << "I " << I.t() ;
+ std::cout << "J " << J.t() ;
+ }
+#endif
+
+ calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
+ s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
+#if (DEBUG_LEVEL3)
+ std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
+#endif
+
+ /* 2eme branche */
+ I = I0 - U*r*co ;
+ J = J0 - U*r*si ;
+#if (DEBUG_LEVEL3)
+ {
+ std::cout << "I " << I.t() ;
+ std::cout << "J " << J.t() ;
+ }
+#endif
+
+ calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
+ s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
+#if (DEBUG_LEVEL3)
+ std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
+#endif
+
+ cpt ++;
+ if (s1 <= s2)
+ {
+ smin = s1;
+ k = 0;
+ for(i = 0; i < npt; i++)
+ {
+ if (k != 0) { // On ne prend pas le 1er point
+ eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
+ + cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
+ }
+ k++;
+ }
+ cMo = cMo1 ;
+ }
+ else
+ {
+ smin = s2;
+ k = 0;
+ for(i = 0; i < npt; i++)
+ {
+ if (k != 0) { // On ne prend pas le 1er point
+ eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
+ + cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
+ }
+ k++;
+ }
+ cMo = cMo2 ;
+ }
+
+ if (smin > smin_old)
+ {
+#if (DEBUG_LEVEL2)
+ std::cout << "Divergence " << std::endl ;
+#endif
+
+ cMo = cMo_old ;
+ }
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "s1 = " << s1 << std::endl ;
+ std::cout << "s2 = " << s2 << std::endl ;
+ std::cout << "smin = " << smin << std::endl ;
+ std::cout << "smin_old = " << smin_old << std::endl ;
+ }
+#endif
+ }
+ }
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
+#endif
+
+ return erreur ;
+}
+
+/*!
+\brief Compute the pose using Dementhon approach for planar objects
+this is a direct implementation of the algorithm proposed by
+Dementhon in his PhD.
+
+\author Francois Chaumette (simplified by Eric Marchand)
+*/
+
+void
+vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
+#endif
+
+ unsigned int i,j,k ;
+
+ vpPoint p0 = listP.front() ;
+
+ vpPoint P ;
+ c3d.clear();
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ P.set_oX(P.get_oX()-p0.get_oX()) ;
+ P.set_oY(P.get_oY()-p0.get_oY()) ;
+ P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
+ c3d.push_back(P);
+ }
+
+ vpMatrix a ;
+ try
+ {
+ a.resize(npt-1,3) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+
+
+ for (i=1 ; i < npt ; i++)
+ {
+ a[i-1][0]=c3d[i].get_oX();
+ a[i-1][1]=c3d[i].get_oY();
+ a[i-1][2]=c3d[i].get_oZ();
+ }
+
+ // calcul a^T a
+ vpMatrix ata ;
+ ata = a.t()*a ;
+
+ /* essai FC pour debug SVD */
+ /*
+ vpMatrix ata_old ;
+ ata_old = a.t()*a ;
+
+ vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
+ for (i=0;i<ata.getRows();i++)
+ for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
+ */
+ vpMatrix ata_sav;
+ ata_sav = ata;
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "a" << std::endl <<a<<std::endl ;
+ std::cout << "ata" << std::endl <<ata<<std::endl ;
+ }
+#endif
+
+ // calcul (a^T a)^-1
+ vpMatrix ata1(ata.getRows(),ata.getCols()) ;
+ vpMatrix v(ata.getRows(),ata.getCols());
+ vpColVector sv(ata.getRows());
+ // ata1 = ata.i() ;
+ unsigned int imin = 0;
+ double s = 0.0;
+
+ //calcul de ata^-1
+ ata.svd(sv,v) ;
+
+ unsigned int nc = sv.getRows() ;
+ for (i=0; i < nc ; i++)
+ if (sv[i] > s) s = sv[i];
+
+ s *= 0.0002;
+ int irank = 0;
+ for (i=0;i<nc;i++)
+ if (sv[i] > s ) irank++;
+
+ double svm = 100.0;
+ for (i = 0; i < nc; i++)
+ if (sv[i] < svm) { imin = i; svm = sv[i]; }
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "rang: " << irank << std::endl ;;
+ std::cout <<"imin = " << imin << std::endl ;
+ std::cout << "sv " << sv.t() << std::endl ;
+ }
+#endif
+
+ for (i=0 ; i < ata.getRows() ; i++)
+ for (j=0 ; j < ata.getCols() ; j++)
+ {
+ ata1[i][j] = 0.0;
+ for (k=0 ; k < nc ; k++)
+ if (sv[k] > s)
+ ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
+ }
+
+
+
+ vpMatrix b ; // b=(at a)^-1*at
+ b = ata1*a.t() ;
+
+ //calcul de U
+ vpColVector U(3) ;
+ U = ata.getCol(imin) ;
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "a" << std::endl <<a<<std::endl ;
+ std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
+ std::cout << "ata1" << std::endl <<ata1<<std::endl ;
+ std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
+ std::cout << "b" << std::endl << b ;
+ std::cout << "U " << U.t() << std::endl ;
+ }
+#endif
+
+ vpColVector xi(npt) ;
+ vpColVector yi(npt) ;
+ //calcul de la premiere solution
+ for (i = 0; i < npt; i++)
+ {
+ xi[i] = c3d[i].get_x() ;
+ yi[i] = c3d[i].get_y() ;
+
+ }
+
+ vpColVector I0(3) ; I0 = 0 ;
+ vpColVector J0(3) ; J0 = 0 ;
+ vpColVector I(3) ;
+ vpColVector J(3) ;
+
+ for (i=1;i<npt;i++)
+ {
+ I0[0] += b[0][i-1] * (xi[i]-xi[0]);
+ I0[1] += b[1][i-1] * (xi[i]-xi[0]);
+ I0[2] += b[2][i-1] * (xi[i]-xi[0]);
+
+ J0[0] += b[0][i-1] * (yi[i]-yi[0]);
+ J0[1] += b[1][i-1] * (yi[i]-yi[0]);
+ J0[2] += b[2][i-1] * (yi[i]-yi[0]);
+ }
+
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "I0 "<<I0.t() ;
+ std::cout << "J0 "<<J0.t() ;
+ }
+#endif
+
+ s = -2.0*vpColVector::dotProd(I0,J0);
+ double c = J0.sumSquare() - I0.sumSquare() ;
+
+ double r,theta,si,co ;
+ calculRTheta(s, c, r, theta);
+ co = cos(theta);
+ si = sin(theta);
+
+ // calcul de la premiere solution
+ I = I0 + U*r*co ;
+ J = J0 + U*r*si ;
+
+ vpHomogeneousMatrix cMo1f ;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
+
+
+ int erreur1 = calculArbreDementhon(b, U, cMo1f);
+
+ // calcul de la deuxieme solution
+ I = I0 - U*r*co ;
+ J = J0 - U*r*si ;
+
+ vpHomogeneousMatrix cMo2f;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
+
+ int erreur2 = calculArbreDementhon(b, U, cMo2f);
+
+ if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
+ if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
+ if ((erreur1 == 0) && (erreur2 == 0))
+ {
+ double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
+ double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
+
+ if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
+ }
+
+ cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
+ cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
+ cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
+#endif
+}
+
+#undef DMIN
+#undef EPS
+#undef EPS_DEM
+
+
+/*!
+\brief Compute and return the residual expressed in meter for the pose matrix
+'pose'.
+
+\param cMo : the matrix that defines the pose to be tested.
+
+\return the value of he residual in meter
+*/
+double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo)
+{
+ double residual_ = 0 ;
+
+ residual_ =0 ;
+ for (unsigned int i =0 ; i < npt ; i++)
+ {
+
+ double X = c3d[i].get_oX()*cMo[0][0]+c3d[i].get_oY()*cMo[0][1]+c3d[i].get_oZ()*cMo[0][2] + cMo[0][3];
+ double Y = c3d[i].get_oX()*cMo[1][0]+c3d[i].get_oY()*cMo[1][1]+c3d[i].get_oZ()*cMo[1][2] + cMo[1][3];
+ double Z = c3d[i].get_oX()*cMo[2][0]+c3d[i].get_oY()*cMo[2][1]+c3d[i].get_oZ()*cMo[2][2] + cMo[2][3];
+
+ double x = X/Z ;
+ double y = Y/Z ;
+
+ residual_ += vpMath::sqr(x-c3d[i].get_x()) + vpMath::sqr(y-c3d[i].get_y()) ;
+ }
+ return residual_ ;
+}
+
+
+#undef DEBUG_LEVEL1
+#undef DEBUG_LEVEL2
+#undef DEBUG_LEVEL3
+
diff --git a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp
new file mode 100644
index 0000000..4a4838e
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp
@@ -0,0 +1,575 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation from any features.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+#include <visp3/vision/vpPoseFeatures.h>
+
+#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
+
+/*!
+ Default constructor.
+*/
+vpPoseFeatures::vpPoseFeatures()
+ : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
+ covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
+ featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
+ featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
+{
+}
+
+/*!
+ Destructor that deletes the array of features and projections.
+*/
+vpPoseFeatures::~vpPoseFeatures()
+{
+ clear();
+}
+
+/*!
+ Clear all the features
+*/
+void vpPoseFeatures::clear()
+{
+ for(int i = (int)featurePoint_Point_list.size()-1 ; i >= 0 ; i--)
+ delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
+ featurePoint_Point_list.clear();
+
+ for(int i = (int)featurePoint3D_Point_list.size()-1 ; i >= 0 ; i--)
+ delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
+ featurePoint3D_Point_list.clear();
+
+ for(int i = (int)featureVanishingPoint_Point_list.size()-1 ; i >= 0 ; i--)
+ delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
+ featureVanishingPoint_Point_list.clear();
+
+ for(int i = (int)featureVanishingPoint_DuoLine_list.size()-1 ; i >= 0 ; i--)
+ delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
+ featureVanishingPoint_DuoLine_list.clear();
+
+ for(int i = (int)featureEllipse_Sphere_list.size()-1 ; i >= 0 ; i--)
+ delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
+ featureEllipse_Sphere_list.clear();
+
+ for(int i = (int)featureEllipse_Circle_list.size()-1 ; i >= 0 ; i--)
+ delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
+ featureEllipse_Circle_list.clear();
+
+ for(int i = (int)featureLine_Line_list.size()-1 ; i >= 0 ; i--)
+ delete featureLine_Line_list[(unsigned int)i].desiredFeature;
+ featureLine_Line_list.clear();
+
+ for(int i = (int)featureLine_DuoLineInt_List.size()-1 ; i >= 0 ; i--)
+ delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
+ featureLine_DuoLineInt_List.clear();
+
+ for(int i = (int)featureSegment_DuoPoints_list.size()-1 ; i >= 0 ; i--)
+ delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
+ featureSegment_DuoPoints_list.clear();
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ for(int i = (int)featureSpecific_list.size()-1 ; i >= 0 ; i--)
+ delete featureSpecific_list[(unsigned int)i];
+ featureSpecific_list.clear();
+#endif
+
+ maxSize = 0;
+ totalSize = 0;
+}
+
+/*!
+ Add a point feature to the list of features to be considered in the pose computation.
+
+ \param p : Point projection expressed as a vpPoint.
+*/
+void vpPoseFeatures::addFeaturePoint(const vpPoint &p)
+{
+ featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint,vpPoint>());
+ featurePoint_Point_list.back().firstParam = p;
+ featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
+ vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature,p);
+
+ totalSize++;
+ if(featurePoint_Point_list.size() > maxSize)
+ maxSize = (unsigned int)featurePoint_Point_list.size();
+}
+
+/*!
+ Add a point 3D feature to the list of features to be considered in the pose computation.
+
+ \param p : Projection expressed as a vpPoint.
+*/
+void vpPoseFeatures::addFeaturePoint3D(const vpPoint &p)
+{
+ featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D,vpPoint>());
+ featurePoint3D_Point_list.back().firstParam = p;
+ featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
+ vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature,p);
+
+ totalSize++;
+ if(featurePoint3D_Point_list.size() > maxSize)
+ maxSize = (unsigned int)featurePoint3D_Point_list.size();
+}
+
+/*!
+ Add a vanishing point feature to the list of features to be considered in the pose computation.
+
+ \param p : Projection expressed as a vpPoint.
+*/
+void vpPoseFeatures::addFeatureVanishingPoint(const vpPoint &p)
+{
+ featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint,vpPoint>());
+ featureVanishingPoint_Point_list.back().firstParam = p;
+ featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
+ vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature,p);
+
+ totalSize++;
+ if(featureVanishingPoint_Point_list.size() > maxSize)
+ maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
+}
+
+/*!
+ Add a vanishing point feature to the list of features to be considered in the pose computation.
+
+ \param l1 : First line used to create the feature.
+ \param l2 : Second line used to create the feature.
+*/
+void vpPoseFeatures::addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2)
+{
+ featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint,vpLine,vpLine>());
+ featureVanishingPoint_DuoLine_list.back().firstParam = l1;
+ featureVanishingPoint_DuoLine_list.back().secondParam = l2;
+ featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
+ vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature,l1,l2);
+
+ totalSize++;
+ if(featureVanishingPoint_DuoLine_list.size() > maxSize)
+ maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
+}
+
+/*!
+ Add an ellipse feature to the list of features to be considered in the pose computation.
+
+ \param s : Ellipse projection expressed as a vpSphere.
+*/
+void vpPoseFeatures::addFeatureEllipse(const vpSphere &s)
+{
+ featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse,vpSphere>());
+ featureEllipse_Sphere_list.back().firstParam = s;
+ featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
+ vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature,s);
+
+ totalSize++;
+ if(featureEllipse_Sphere_list.size() > maxSize)
+ maxSize = (unsigned int)featureEllipse_Sphere_list.size();
+}
+
+/*!
+ Add an ellipse feature to the list of features to be considered in the pose computation.
+
+ \param c : Ellipse projection expressed as a vpCircle.
+*/
+void vpPoseFeatures::addFeatureEllipse(const vpCircle &c)
+{
+ featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse,vpCircle>());
+ featureEllipse_Circle_list.back().firstParam = c;
+ featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
+ vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature,c);
+
+ totalSize++;
+ if(featureEllipse_Circle_list.size() > maxSize)
+ maxSize = (unsigned int)featureEllipse_Circle_list.size();
+}
+
+/*!
+ Add a line feature to the list of features to be considered in the pose computation.
+
+ \param l : Line projection expressed as a vpLine.
+*/
+void vpPoseFeatures::addFeatureLine(const vpLine &l)
+{
+ featureLine_Line_list.push_back(vpDuo<vpFeatureLine,vpLine>());
+ featureLine_Line_list.back().firstParam = l;
+ featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
+ vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature,l);
+
+ totalSize++;
+ if(featureLine_Line_list.size() > maxSize)
+ maxSize = (unsigned int)featureLine_Line_list.size();
+}
+
+/*!
+ Add a line feature to the list of features to be considered in the pose computation.
+
+ \param c : Line projection expressed as a vpCylinder.
+ \param line : Integer id that indicates which limb of the cylinder is to consider.
+ It can be vpCylinder::line1 or vpCylinder::line2.
+*/
+void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
+{
+ featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine,vpCylinder,int>());
+ featureLine_DuoLineInt_List.back().firstParam = c;
+ featureLine_DuoLineInt_List.back().secondParam = line;
+ featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
+ vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature,c,line);
+
+ totalSize++;
+ if(featureLine_DuoLineInt_List.size() > maxSize)
+ maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
+}
+
+/*!
+ Add a segment feature to the list of features to be considered in the pose computation.
+
+ \param P1 : First extremity projection.
+ \param P2 : Second extremity projection.
+*/
+void vpPoseFeatures::addFeatureSegment(vpPoint &P1, vpPoint &P2)
+{
+ featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment,vpPoint,vpPoint>());
+ featureSegment_DuoPoints_list.back().firstParam = P1;
+ featureSegment_DuoPoints_list.back().secondParam = P2;
+ featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
+ vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature,P1,P2);
+
+ totalSize++;
+ if(featureSegment_DuoPoints_list.size() > maxSize)
+ maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
+}
+
+/*!
+ Get the error vector and L matrix from all the features.
+
+ \param cMo : Current Pose.
+ \param err : Resulting error vector.
+ \param L : Resulting interaction matrix.
+*/
+void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L)
+{
+ err = vpColVector();
+ L = vpMatrix();
+
+ for(unsigned int i = 0 ; i < maxSize ; i++)
+ {
+ //--------------vpFeaturePoint--------------
+ //From vpPoint
+ if( i < featurePoint_Point_list.size() ){
+ vpFeaturePoint fp;
+ vpPoint p(featurePoint_Point_list[i].firstParam);
+ p.track(cMo);
+ vpFeatureBuilder::create(fp, p);
+ err.stack(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
+ L.stack(fp.interaction());
+ }
+
+ //--------------vpFeaturePoint3D--------------
+ //From vpPoint
+ if( i < featurePoint3D_Point_list.size() ){
+ vpFeaturePoint3D fp3D;
+ vpPoint p(featurePoint3D_Point_list[i].firstParam);
+ p.track(cMo);
+ vpFeatureBuilder::create(fp3D, p);
+ err.stack(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
+ L.stack(fp3D.interaction());
+ }
+
+ //--------------vpFeatureVanishingPoint--------------
+ //From vpPoint
+ if( i < featureVanishingPoint_Point_list.size() ){
+ vpFeatureVanishingPoint fvp;
+ vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
+ p.track(cMo);
+ vpFeatureBuilder::create(fvp, p);
+ err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
+ L.stack(fvp.interaction());
+ }
+ //From Duo of vpLines
+ if( i < featureVanishingPoint_DuoLine_list.size() ){
+ vpFeatureVanishingPoint fvp;
+ vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
+ vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
+ l1.track(cMo);
+ l2.track(cMo);
+ vpFeatureBuilder::create(fvp, l1, l2);
+ err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
+ L.stack(fvp.interaction());
+ }
+
+ //--------------vpFeatureEllipse--------------
+ //From vpSphere
+ if( i < featureEllipse_Sphere_list.size() ){
+ vpFeatureEllipse fe;
+ vpSphere s(featureEllipse_Sphere_list[i].firstParam);
+ s.track(cMo);
+ vpFeatureBuilder::create(fe, s);
+ err.stack(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
+ L.stack(fe.interaction());
+ }
+ //From vpCircle
+ if( i < featureEllipse_Circle_list.size() ){
+ vpFeatureEllipse fe;
+ vpCircle c(featureEllipse_Circle_list[i].firstParam);
+ c.track(cMo);
+ vpFeatureBuilder::create(fe, c);
+ err.stack(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
+ L.stack(fe.interaction());
+ }
+
+ //--------------vpFeatureLine--------------
+ //From vpLine
+ if( i < featureLine_Line_list.size() ){
+ vpFeatureLine fl;
+ vpLine l(featureLine_Line_list[i].firstParam);
+ l.track(cMo);
+ vpFeatureBuilder::create(fl, l);
+ err.stack(fl.error(*(featureLine_Line_list[i].desiredFeature)));
+ L.stack(fl.interaction());
+ }
+ //From Duo of vpCylinder / Integer
+ if( i < featureLine_DuoLineInt_List.size() ){
+ vpFeatureLine fl;
+ vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
+ c.track(cMo);
+ vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
+ err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
+ L.stack(fl.interaction());
+ }
+
+ //--------------vpFeatureSegment--------------
+ //From Duo of vpPoints
+ if( i < featureSegment_DuoPoints_list.size() ){
+ vpFeatureSegment fs;
+ vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
+ vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
+ p1.track(cMo);
+ p2.track(cMo);
+ vpFeatureBuilder::create(fs, p1, p2);
+ err.stack(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
+ L.stack(fs.interaction());
+ }
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ //--------------Specific Feature--------------
+ if( i < featureSpecific_list.size() ){
+ featureSpecific_list[i]->createCurrent(cMo);
+ err.stack(featureSpecific_list[i]->error());
+ L.stack(featureSpecific_list[i]->currentInteraction());
+ }
+#endif
+ }
+}
+
+
+/*!
+ Compute the pose according to the desired method (virtual visual servoing, or robust virtual visual servoing approach).
+
+ \param cMo : Computed pose.
+
+ \param type : Method to use for the pose computation.
+
+ - The virtual visual servoing approach is described in \cite Marchand02c.
+
+ - The robust virtual visual servoing approach is described in \cite Comport06b.
+
+*/
+void vpPoseFeatures::computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type)
+{
+ switch(type)
+ {
+ case VIRTUAL_VS:
+ computePoseVVS(cMo);
+ break;
+ case ROBUST_VIRTUAL_VS:
+ computePoseRobustVVS(cMo);
+ break;
+ default:
+ break;
+ }
+}
+
+/*!
+ Compute the pose thanks to the virtual visual servoing approach.
+
+ This approach is described in:
+
+ E. Marchand, F. Chaumette. Virtual Visual Servoing: a framework for real-time
+ augmented reality. In EUROGRAPHICS 2002 Conference Proceeding, G. Drettakis,
+ H.-P. Seidel (eds.), Computer Graphics Forum, Volume 21(3), Pages 289-298,
+ Sarrebruck, Allemagne, 2002.
+
+ \param cMo : Computed pose.
+*/
+void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix & cMo)
+{
+ try
+ {
+ double residu_1 = 1e8 ;
+ double r =1e8-1;
+ // we stop the minimization when the error is bellow 1e-8
+
+ vpMatrix L;
+ vpColVector err;
+ vpColVector v ;
+
+ unsigned int iter = 0;
+
+ while((int)((residu_1 - r)*1e12) != 0 )
+ {
+ residu_1 = r ;
+
+ // Compute the interaction matrix and the error
+ error_and_interaction(cMo,err,L);
+
+ // compute the residual
+ r = err.sumSquare() ;
+
+ // compute the pseudo inverse of the interaction matrix
+ vpMatrix Lp ;
+ unsigned int rank = L.pseudoInverse(Lp,1e-16) ;
+
+ if(rank < 6){
+ if(verbose)
+ vpTRACE("Rank must be at least 6 ! cMo not computed.");
+
+ break;
+ }
+
+ // compute the VVS control law
+ v = -lambda*Lp*err ;
+
+ cMo = vpExponentialMap::direct(v).inverse()*cMo ;
+ if (iter++>vvsIterMax){
+ vpTRACE("Max iteration reached") ;
+ break ;
+ }
+ }
+
+ if(computeCovariance)
+ covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("vpPoseFeatures::computePoseVVS") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Compute the pose thanks to the robust virtual visual servoing approach described in \cite Comport06b.
+
+ \param cMo : Computed pose.
+*/
+void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix & cMo)
+{
+ try{
+ double residu_1 = 1e8 ;
+ double r =1e8-1;
+
+ // we stop the minimization when the error is bellow 1e-8
+ vpMatrix L, W;
+ vpColVector w, res;
+ vpColVector v ;
+ vpColVector error ; // error vector
+
+ vpRobust robust(2*totalSize) ;
+ robust.setThreshold(0.0000) ;
+
+ unsigned int iter = 0 ;
+
+ while((int)((residu_1 - r)*1e12) !=0)
+ {
+ residu_1 = r ;
+
+ // Compute the interaction matrix and the error
+ error_and_interaction(cMo,error,L);
+
+ // compute the residual
+ r = error.sumSquare() ;
+
+ if(iter == 0){
+ res = vpColVector(error.getRows()/2);
+ W = vpMatrix(error.getRows(),error.getRows());
+ w = vpColVector(error.getRows()/2);
+ w = 1;
+ }
+
+ for(unsigned int k=0 ; k < error.getRows()/2 ; k++)
+ {
+ res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
+ }
+ robust.setIteration(0);
+ robust.MEstimator(vpRobust::TUKEY, res, w);
+
+ // compute the pseudo inverse of the interaction matrix
+ for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
+ {
+ W[2*k][2*k] = w[k] ;
+ W[2*k+1][2*k+1] = w[k] ;
+ }
+ // compute the pseudo inverse of the interaction matrix
+ vpMatrix Lp ;
+ vpMatrix LRank;
+ (W*L).pseudoInverse(Lp,1e-6) ;
+ unsigned int rank = L.pseudoInverse(LRank,1e-6) ;
+
+ if(rank < 6){
+ if(verbose)
+ vpTRACE("Rank must be at least 6 ! cMo not computed.");
+
+ break;
+ }
+
+ // compute the VVS control law
+ v = -lambda*Lp*W*error ;
+
+ cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
+ if (iter++>vvsIterMax){
+ vpTRACE("Max iteration reached") ;
+ break ;
+ }
+ }
+
+ if(computeCovariance)
+ covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS") ;
+ throw ;
+ }
+}
+
+#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp
new file mode 100644
index 0000000..458f864
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp
@@ -0,0 +1,603 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+
+#include <visp3/vision/vpPose.h>
+
+#define DEBUG_LEVEL1 0
+#define DEBUG_LEVEL2 0
+
+/**********************************************************************/
+/* FONCTION : CalculTranslation */
+/* ROLE : Calcul de la translation entre la */
+/* camera et l'outil connaissant la */
+/* rotation */
+/**********************************************************************/
+
+static
+void
+calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
+ unsigned int nc3, vpColVector &x1, vpColVector &x2)
+{
+
+ try
+ {
+ unsigned int i,j;
+
+ vpMatrix ct(3,nl) ;
+ for (i=0 ; i < 3 ; i++)
+ {
+ for (j=0 ; j < nl ; j++)
+ ct[i][j] = b[j][i+nc3] ;
+ }
+
+ vpMatrix c ;
+ c = ct.t() ;
+
+ vpMatrix ctc ;
+ ctc = ct*c ;
+
+ vpMatrix ctc1 ; // (C^T C)^(-1)
+ ctc1 = ctc.inverseByLU() ;
+
+ vpMatrix cta ;
+ vpMatrix ctb ;
+ cta = ct*a ; /* C^T A */
+ ctb = ct*b ; /* C^T B */
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout <<"ctc " << std::endl << ctc ;
+ std::cout <<"cta " << std::endl << cta ;
+ std::cout <<"ctb " << std::endl << ctb ;
+ }
+#endif
+
+ vpColVector X2(nc3) ;
+ vpMatrix CTB(nc1,nc3) ;
+ for (i=0 ; i < nc1 ; i++)
+ {
+ for (j=0 ; j < nc3 ; j++)
+ CTB[i][j] = ctb[i][j] ;
+ }
+
+ for (j=0 ; j < nc3 ; j++)
+ X2[j] = x2[j] ;
+
+ vpColVector sv ; // C^T A X1 + C^T B X2)
+ sv = cta*x1 + CTB*X2 ;// C^T A X1 + C^T B X2)
+
+#if (DEBUG_LEVEL2)
+ std::cout << "sv " << sv.t() ;
+#endif
+
+ vpColVector X3 ; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
+ X3 = -ctc1*sv ;
+
+#if (DEBUG_LEVEL2)
+ std::cout << "x3 " << X3.t() ;
+#endif
+
+ for (i=0 ; i < nc1 ; i++)
+ x2[i+nc3] = X3[i] ;
+ }
+ catch(...)
+ {
+
+ // en fait il y a des dizaines de raisons qui font que cette fonction
+ // rende une erreur (matrice pas inversible, pb de memoire etc...)
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+
+
+}
+
+
+//*********************************************************************
+// 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
+//*********************************************************************
+
+//#define EPS 1.e-5
+
+static
+void
+lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
+#endif
+
+ try{
+ unsigned int i,imin;
+
+ vpMatrix ata ; // A^T A
+ ata = a.t()*a ;
+ vpMatrix btb ; // B^T B
+ btb = b.t()*b ;
+
+ vpMatrix bta ; // B^T A
+ bta = b.t()*a ;
+
+ vpMatrix btb1 ; // (B^T B)^(-1)
+
+ if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
+ else btb1 = btb.pseudoInverse();
+
+#if (DEBUG_LEVEL1)
+ {
+ std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
+ std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
+ }
+#endif
+
+ vpMatrix r ; // (B^T B)^(-1) B^T A
+ r = btb1*bta ;
+
+ vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
+ e = - (a.t()*b) *r ;
+
+ e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
+
+#if (DEBUG_LEVEL1)
+ {
+ std::cout << " E :" << std::endl << e << std::endl;
+ }
+#endif
+
+ // vpColVector sv ;
+ // vpMatrix v ;
+ e.svd(x1,ata) ;// destructif sur e
+ // calcul du vecteur propre de E correspondant a la valeur propre min.
+
+ /* calcul de SVmax */
+ imin = 0;
+ // FC : Pourquoi calculer SVmax ??????
+ // double svm = 0.0;
+ // for (i=0;i<x1.getRows();i++)
+ // {
+ // if (x1[i] > svm) { svm = x1[i]; imin = i; }
+ // }
+ // svm *= EPS; /* pour le rang */
+
+ for (i=0;i<x1.getRows();i++)
+ if (x1[i] < x1[imin]) imin = i;
+
+#if (DEBUG_LEVEL1)
+ {
+ printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
+ std::cout << " i_min " << imin << std::endl;
+ }
+#endif
+ for (i=0;i<x1.getRows();i++)
+ x1[i] = ata[i][imin];
+
+ x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
+
+#if (DEBUG_LEVEL1)
+ {
+ std::cout << " X1 : " << x1.t() << std::endl;
+ std::cout << " V : " << std::endl << ata << std::endl;
+ }
+#endif
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+#if (DEBUG_LEVEL1)
+ std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
+#endif
+}
+
+//#undef EPS
+
+/*!
+\brief Compute the pose of a planar object using Lagrange approach.
+
+\param cMo : Estimated pose. No initialisation is requested to estimate cMo.
+\param coplanar_plane_type : Type of coplanar plane:
+ 1: if plane x=cst
+ 2: if plane y=cst
+ 3: if plane z=cst
+ 0: any other plane
+*/
+void
+vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
+{
+
+#if (DEBUG_LEVEL1)
+ std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
+#endif
+ try
+ {
+ double s;
+ unsigned int i;
+
+ unsigned int k=0;
+ unsigned int nl=npt*2;
+
+
+ vpMatrix a(nl,3) ;
+ vpMatrix b(nl,6);
+ vpPoint P ;
+ i=0 ;
+
+ if (coplanar_plane_type == 1) { // plane ax=d
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it ;
+ a[k][0] = -P.get_oY();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oY()*P.get_x();
+
+ a[k+1][0] = 0.0;
+ a[k+1][1] = -P.get_oY();
+ a[k+1][2] = P.get_oY()*P.get_y();
+
+ b[k][0] = -P.get_oZ();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oZ()*P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k+1][0] = 0.0;
+ b[k+1][1] = -P.get_oZ();
+ b[k+1][2] = P.get_oZ()*P.get_y();
+ b[k+1][3] = 0.0;
+ b[k+1][4] = -1.0;
+ b[k+1][5] = P.get_y();
+
+ k += 2;
+ }
+
+ }
+ else if (coplanar_plane_type == 2) { // plane by=d
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it ;
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX()*P.get_x();
+
+ a[k+1][0] = 0.0;
+ a[k+1][1] = -P.get_oX();
+ a[k+1][2] = P.get_oX()*P.get_y();
+
+ b[k][0] = -P.get_oZ();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oZ()*P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k+1][0] = 0.0;
+ b[k+1][1] = -P.get_oZ();
+ b[k+1][2] = P.get_oZ()*P.get_y();
+ b[k+1][3] = 0.0;
+ b[k+1][4] = -1.0;
+ b[k+1][5] = P.get_y();
+
+ k += 2;
+ }
+
+ }
+ else { // plane cz=d or any other
+
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it ;
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX()*P.get_x();
+
+ a[k+1][0] = 0.0;
+ a[k+1][1] = -P.get_oX();
+ a[k+1][2] = P.get_oX()*P.get_y();
+
+ b[k][0] = -P.get_oY();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oY()*P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k+1][0] = 0.0;
+ b[k+1][1] = -P.get_oY();
+ b[k+1][2] = P.get_oY()*P.get_y();
+ b[k+1][3] = 0.0;
+ b[k+1][4] = -1.0;
+ b[k+1][5] = P.get_y();
+
+ k += 2;
+ }
+ }
+ vpColVector X1(3) ;
+ vpColVector X2(6) ;
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout <<"a " << a << std::endl ;
+ std::cout <<"b " << b << std::endl ;
+ }
+#endif
+
+ lagrange(a,b,X1,X2);
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
+ std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
+ }
+#endif
+
+ if (X2[5] < 0.0)
+ { /* car Zo > 0 */
+ for (i=0;i<3;i++) X1[i] = -X1[i];
+ for (i=0;i<6;i++) X2[i] = -X2[i];
+ }
+ s = 0.0;
+ for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
+ for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
+
+ //s = 0.0;
+ //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
+ s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
+
+ if (s<1e-10)
+ {
+// std::cout << "Points that produce an error: " << std::endl;
+// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+// {
+// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
+// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
+// }
+ throw(vpException(vpException::divideByZeroError,
+ "Division by zero in Lagrange pose computation (planar plane case)")) ;
+ }
+
+ s = 1.0/sqrt(s);
+ for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
+
+
+ calculTranslation (a, b, nl, 3, 3, X1, X2) ;
+
+ // if (err != OK)
+ {
+ // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
+ // PrintError(err) ;
+ // return err ;
+ }
+
+ if (coplanar_plane_type == 1) { // plane ax=d
+ cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
+ cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
+ cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+
+ for (i=0;i<3;i++)
+ { /* calcul de la matrice de passage */
+ cMo[i][1] = X1[i];
+ cMo[i][2] = X2[i];
+ cMo[i][3] = X2[i+3];
+ }
+
+ }
+ else if (coplanar_plane_type == 2) { // plane by=d
+ cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
+ cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
+ cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+
+ for (i=0;i<3;i++)
+ { /* calcul de la matrice de passage */
+ cMo[i][0] = X1[i];
+ cMo[i][2] = X2[i];
+ cMo[i][3] = X2[i+3];
+ }
+ }
+ else { // plane cz=d or any other
+
+ cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
+ cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
+ cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+
+ for (i=0;i<3;i++)
+ { /* calcul de la matrice de passage */
+ cMo[i][0] = X1[i];
+ cMo[i][1] = X2[i];
+ cMo[i][3] = X2[i+3];
+ }
+ }
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
+#endif
+ // return(OK);
+}
+
+
+void
+vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
+{
+
+#if (DEBUG_LEVEL1)
+ std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
+#endif
+ try{
+ double s;
+ unsigned int i;
+
+ unsigned int k=0;
+ unsigned int nl=npt*2;
+
+ vpMatrix a(nl,3) ;
+ vpMatrix b(nl,9);
+ b =0 ;
+
+ vpPoint P ;
+ i=0 ;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX()*P.get_x();
+
+ a[k+1][0] = 0.0;
+ a[k+1][1] = -P.get_oX();
+ a[k+1][2] = P.get_oX()*P.get_y();
+
+ b[k][0] = -P.get_oY();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oY()*P.get_x();
+
+ b[k][3] = -P.get_oZ();
+ b[k][4] = 0.0;
+ b[k][5] = P.get_oZ()*P.get_x();
+
+ b[k][6] = -1.0;
+ b[k][7] = 0.0;
+ b[k][8] = P.get_x();
+
+ b[k+1][0] = 0.0;
+ b[k+1][1] = -P.get_oY();
+ b[k+1][2] = P.get_oY()*P.get_y();
+
+ b[k+1][3] = 0.0;
+ b[k+1][4] = -P.get_oZ();
+ b[k+1][5] = P.get_oZ()*P.get_y();
+
+ b[k+1][6] = 0.0;
+ b[k+1][7] = -1.0;
+ b[k+1][8] = P.get_y();
+
+ k += 2;
+ }
+ vpColVector X1(3) ;
+ vpColVector X2(9) ;
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout <<"a " << a << std::endl ;
+ std::cout <<"b " << b << std::endl ;
+ }
+#endif
+
+ lagrange(a,b,X1,X2);
+ // if (err != OK)
+ {
+ // std::cout << "in (CLagrange.cc)Lagrange returns " ;
+ // PrintError(err) ;
+ // return err ;
+ }
+
+
+#if (DEBUG_LEVEL2)
+ {
+ std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
+ std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
+ }
+#endif
+
+ if (X2[8] < 0.0)
+ { /* car Zo > 0 */
+ X1 *= -1 ;
+ X2 *= -1 ;
+ }
+ s = 0.0;
+ for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
+ for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
+
+ //s = 0.0;
+ //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
+ s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
+
+ if (s<1e-10)
+ {
+// std::cout << "Points that produce an error: " << std::endl;
+// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+// {
+// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
+// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
+// }
+ //vpERROR_TRACE(" division par zero " ) ;
+ throw(vpException(vpException::divideByZeroError,
+ "Division by zero in Lagrange pose computation (non planar plane case)")) ;
+ }
+
+ s = 1.0/sqrt(s);
+ for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
+
+ X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
+ X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
+ X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+
+ calculTranslation (a, b, nl, 3, 6, X1, X2) ;
+
+ for (i=0 ; i<3 ; i++)
+ {
+ cMo[i][0] = X1[i];
+ cMo[i][1] = X2[i];
+ cMo[i][2] = X2[i+3];
+ cMo[i][3] = X2[i+6];
+ }
+
+ }
+ catch(vpException &e)
+ {
+ throw e;
+ }
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
+#endif
+}
+
+
+#undef DEBUG_LEVEL1
+#undef DEBUG_LEVEL2
diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp
new file mode 100644
index 0000000..5a2a964
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp
@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Francois Chaumette
+ *
+ *****************************************************************************/
+
+#include <math.h>
+#include <float.h>
+#include <string.h>
+#include <limits> // numeric_limits
+
+// besoin de la librairie mathematique, en particulier des
+// fonctions de minimisation de Levenberg Marquartd
+#include <visp3/vision/vpLevenbergMarquartd.h>
+#include <visp3/vision/vpPose.h>
+
+#define NBR_PAR 6
+#define X3_SIZE 3
+#define MINIMUM 0.000001
+
+#define DEBUG_LEVEL1 0
+
+// ------------------------------------------------------------------------
+// FONCTION LOWE :
+// ------------------------------------------------------------------------
+// Calcul de la pose pour un objet 3D
+// ------------------------------------------------------------------------
+
+/*
+* MACRO : MIJ
+*
+* 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 :
+* 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 :
+* L'adresse de m[i][j] est retournee.
+*
+* HISTORIQUE :
+* 1.00 - 11/02/93 - Original.
+*/
+#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
+#define NBPTMAX 50
+
+// Je hurle d'horreur devant ces variable globale...
+static double XI[NBPTMAX],YI[NBPTMAX];
+static double XO[NBPTMAX],YO[NBPTMAX],ZO[NBPTMAX];
+
+
+#define MINI 0.001
+#define MINIMUM 0.000001
+
+void eval_function(int npt,double *xc,double *f);
+void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
+
+void eval_function(int npt,double *xc,double *f)
+{
+ int i;
+ double x,y,z,u[3];
+
+ u[0] = xc[3]; /* Rx */
+ u[1] = xc[4]; /* Ry */
+ u[2] = xc[5]; /* Rz */
+
+ vpRotationMatrix rd(u[0],u[1],u[2]) ;
+ // rot_mat(u,rd); /* matrice de rotation correspondante */
+ for (i=0;i<npt;i++)
+ {
+ x = rd[0][0]*XO[i] + rd[0][1]*YO[i] + rd[0][2]*ZO[i] + xc[0];
+ y = rd[1][0]*XO[i] + rd[1][1]*YO[i] + rd[1][2]*ZO[i] + xc[1];
+ z = rd[2][0]*XO[i] + rd[2][1]*YO[i] + rd[2][2]*ZO[i] + xc[2];
+ f[i] = x/z - XI[i];
+ f[npt+i] = y/z - YI[i];
+ // std::cout << f[i] << " " << f[i+1] << std::endl ;
+ }
+}
+
+
+/*
+* 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.
+*
+* SORTIE :
+* jac Jacobien de la fonction.
+*
+* 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.
+* Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
+* stocke dans "fjac" et "fvecc" reste inchange.
+*
+* HISTORIQUE :
+* 1.00 - xx/xx/xx - Original.
+* 1.01 - 06/07/95 - Modifications.
+* 2.00 - 24/10/95 - Tableau jac monodimensionnel.
+*/
+void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
+{
+ double u[X3_SIZE], rx, ry, rz;// rd[X3_SIZE][X3_SIZE],
+ double tt, mco, co, si, u1, u2, u3, x, y, z;
+ double drxt, drxu1, drxu2, drxu3;
+ double dryt, dryu1, dryu2, dryu3;
+ double drzt, drzu1, drzu2, drzu3;
+ double dxit, dxiu1, dxiu2, dxiu3;
+ double dyit, dyiu1, dyiu2, dyiu3;
+
+ vpRotationMatrix rd ;
+ int i, npt;
+
+ if (m < n) printf("pas assez de points\n");
+ npt = m / 2;
+
+ if (iflag == 1) eval_function (npt, xc, fvecc);
+ else if (iflag == 2)
+ {
+ u[0] =xc[3];
+ u[1]= xc[4];
+ u[2]= xc[5];
+
+ rd.buildFrom(u[0],u[1],u[2]) ;
+ /* a partir de l'axe de rotation, calcul de la matrice de rotation. */
+ // rot_mat(u, rd);
+
+ tt = sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */
+ if (tt >= MINIMUM)
+ {
+ u1 = u[0] / tt;
+ u2 = u[1] / tt; /* axe de rotation unitaire */
+ u3 = u[2] / tt;
+ }
+ else u1 = u2 = u3 = 0.0;
+ co = cos(tt);
+ mco = 1.0 - co;
+ si = sin(tt);
+
+ for (i = 0; i < npt; i++)
+ {
+ x = XO[i];
+ y = YO[i]; /* coordonnees du point i */
+ z = ZO[i];
+
+ /* coordonnees du point i dans le repere camera */
+ rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
+ ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
+ rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
+
+ /* derive des fonctions rx, ry et rz par rapport
+ * a tt, u1, u2, u3.
+ */
+ drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y
+ + (si * u1 * u1 - si) * x;
+ drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
+ drxu2 = si * z + mco * u1 * y;
+ drxu3 = mco * u1 * z - si * y;
+
+ dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y
+ + (co * u3 + si * u1 * u2) * x;
+ dryu1 = mco * u2 * x - si * z;
+ dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
+ dryu3 = mco * u2 * z + si * x;
+
+ drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y
+ + (si * u1 * u3 - co * u2) * x;
+ drzu1 = si * y + mco * u3 * x;
+ drzu2 = mco * u3 * y - si * x;
+ drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
+
+ /* derive de la fonction representant le modele de la
+ * camera (sans distortion) par rapport a tt, u1, u2 et u3.
+ */
+ dxit = drxt / rz - rx * drzt / (rz * rz);
+
+ dyit = dryt / rz - ry * drzt / (rz * rz);
+
+ dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
+ dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
+
+ dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
+ dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
+
+ dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
+ dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
+
+ /* calcul du jacobien : le jacobien represente la
+ * derivee de la fonction representant le modele de la
+ * camera par rapport aux parametres.
+ */
+ *MIJ(jac, 0, i, ldfjac) = 1 / rz;
+ *MIJ(jac, 1, i, ldfjac) = 0.0;
+ *MIJ(jac, 2, i, ldfjac) = - rx / (rz * rz);
+ if (tt >= MINIMUM)
+ {
+ *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt
+ - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
+ *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt
+ + (1 - u2 * u2) * dxiu2 / tt- u2 * u3 * dxiu3 / tt;
+
+ *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt
+ + (1 - u3 * u3) * dxiu3 / tt;
+ }
+ else
+ {
+ *MIJ(jac, 3, i, ldfjac) = 0.0;
+ *MIJ(jac, 4, i, ldfjac) = 0.0;
+ *MIJ(jac, 5, i, ldfjac) = 0.0;
+ }
+ *MIJ(jac, 0, npt + i, ldfjac) = 0.0;
+ *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
+ *MIJ(jac, 2, npt + i, ldfjac) = - ry / (rz * rz);
+ if (tt >= MINIMUM)
+ {
+ *MIJ(jac, 3, npt + i, ldfjac) = u1 * dyit + (1 - u1 * u1) * dyiu1 / tt
+ - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
+ *MIJ(jac, 4, npt + i, ldfjac) = u2 * dyit - u1 * u2 * dyiu1 / tt
+ + (1 - u2 * u2) * dyiu2 / tt- u2 * u3 * dyiu3 / tt;
+ *MIJ(jac, 5, npt + i, ldfjac) = u3 * dyit - u1 * u3 * dyiu1 / tt
+ - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
+ }
+ else
+ {
+ *MIJ(jac, 3, npt + i, ldfjac) = 0.0;
+ *MIJ(jac, 4, npt + i, ldfjac) = 0.0;
+ *MIJ(jac, 5, npt + i, ldfjac) = 0.0;
+ }
+ }
+ } /* fin else if iflag ==2 */
+}
+
+/*!
+\brief Compute the pose using the Lowe non linear approach
+it consider the minimization of a residual using
+the levenberg marquartd approach.
+
+The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
+
+*/
+void
+vpPose::poseLowe(vpHomogeneousMatrix & cMo)
+{
+#if (DEBUG_LEVEL1)
+ std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
+#endif
+ int n, m; /* nombre d'elements dans la matrice jac */
+ int lwa; /* taille du vecteur wa */
+ int ldfjac; /* taille maximum d'une ligne de jac */
+ int info, ipvt[NBR_PAR];
+ int tst_lmder;
+ double f[2 * NBPTMAX], sol[NBR_PAR];
+ double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
+ // double 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<double>::epsilon(); /* critere d'arret */
+
+ // c = cam ;
+ // for (i=0;i<3;i++)
+ // for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
+ // mat_rot(rd,u);
+ vpRotationMatrix cRo ;
+ cMo.extract(cRo) ;
+ vpThetaUVector u(cRo) ;
+ for (unsigned int i=0;i<3;i++)
+ {
+ sol[i] = cMo[i][3];
+ sol[i+3] = u[i];
+ }
+
+ vpPoint P ;
+ unsigned int i_=0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ XI[i_] = P.get_x();//*cam.px + cam.xc ;
+ YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
+ XO[i_] = P.get_oX();
+ YO[i_] = P.get_oY();
+ ZO[i_] = P.get_oZ();
+ ++i_;
+ }
+ 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";
+ // return FATAL_ERROR ;
+ }
+
+ for (unsigned int i = 0; i < 3; i++)
+ u[i] = sol[i + 3];
+
+ for (unsigned int i=0;i<3;i++)
+ {
+ cMo[i][3] = sol[i];
+ u[i] = sol[i+3];
+ }
+
+ vpRotationMatrix rd(u) ;
+ cMo.insert(rd) ;
+ // rot_mat(u,rd);
+ // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
+
+#if (DEBUG_LEVEL1)
+ std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
+#endif
+ // return OK ;
+}
+
+
+#undef MINI
+#undef MINIMUM
+
+
+#undef DEBUG_LEVEL1
+
+
+/*
+* Local variables:
+* c-basic-offset: 2
+* End:
+*/
diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp
new file mode 100644
index 0000000..70ee46b
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp
@@ -0,0 +1,467 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ * Aurelien Yol
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpPoseRansac.cpp
+ \brief function used to estimate a pose using the Ransac algorithm
+*/
+
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
+#include <algorithm> // std::count
+#include <float.h> // DBL_MAX
+
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRansac.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/core/vpList.h>
+#include <visp3/vision/vpPoseException.h>
+#include <visp3/core/vpMath.h>
+
+#define eps 1e-6
+
+
+/*!
+ Compute the pose using the Ransac approach.
+
+ \param cMo : Computed pose
+ \param func : Pointer to a function that takes in parameter a vpHomogeneousMatrix
+ and returns true if the pose check is OK or false otherwise
+ \return True if we found at least 4 points with a reprojection error below ransacThreshold.
+*/
+bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *))
+{
+ ransacInliers.clear();
+ ransacInlierIndex.clear();
+
+ srand(0); //Fix seed here so we will have the same pseudo-random series at each run.
+ std::vector<unsigned int> best_consensus;
+ std::vector<unsigned int> cur_consensus;
+ std::vector<unsigned int> cur_outliers;
+ std::vector<unsigned int> cur_randoms;
+ unsigned int size = (unsigned int)listP.size();
+ int nbTrials = 0;
+ unsigned int nbMinRandom = 4 ;
+ unsigned int nbInliers = 0;
+ double r, r_lagrange, r_dementhon;
+
+ vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
+
+ if (size < 4) {
+ //vpERROR_TRACE("Not enough point to compute the pose");
+ throw(vpPoseException(vpPoseException::notInitializedError,
+ "Not enough point to compute the pose")) ;
+ }
+
+ bool foundSolution = false;
+
+ while (nbTrials < ransacMaxTrials && nbInliers < (unsigned)ransacNbInlierConsensus)
+ {
+ //Hold the list of the index of the inliers (points in the consensus set)
+ cur_consensus.clear();
+
+ //Use a temporary variable because if not, the cMo passed in parameters will be modified when
+ // we compute the pose for the minimal sample sets but if the pose is not correct when we pass
+ // a function pointer we do not want to modify the cMo passed in parameters
+ vpHomogeneousMatrix cMo_tmp;
+ cur_outliers.clear();
+ cur_randoms.clear();
+
+ //Vector of used points, initialized at false for all points
+ std::vector<bool> usedPt(size, false);
+
+ vpPose poseMin;
+ for(unsigned int i = 0; i < nbMinRandom;)
+ {
+ if((size_t) std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
+ //All points was picked once, break otherwise we stay in an infinite loop
+ break;
+ }
+
+ //Pick a point randomly
+ unsigned int r_ = (unsigned int) rand() % size;
+ while(usedPt[r_]) {
+ //If already picked, pick another point randomly
+ r_ = (unsigned int) rand() % size;
+ }
+ //Mark this point as already picked
+ usedPt[r_] = true;
+
+ std::list<vpPoint>::const_iterator iter = listP.begin();
+ std::advance(iter, r_);
+ vpPoint pt = *iter;
+
+ bool degenerate = false;
+ for(std::list<vpPoint>::const_iterator it = poseMin.listP.begin(); it != poseMin.listP.end(); ++it){
+ vpPoint ptdeg = *it;
+ if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
+ ((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
+ degenerate = true;
+ break;
+ }
+ }
+ if(!degenerate) {
+ poseMin.addPoint(pt);
+ cur_randoms.push_back(r_);
+ //Increment the number of points picked
+ i++;
+ }
+ }
+
+ if(poseMin.npt < nbMinRandom) {
+ nbTrials++;
+ continue;
+ }
+
+ //Flags set if pose computation is OK
+ bool is_valid_lagrange = false;
+ bool is_valid_dementhon = false;
+
+ //Set maximum value for residuals
+ r_lagrange = DBL_MAX;
+ r_dementhon = DBL_MAX;
+
+ try {
+ poseMin.computePose(vpPose::LAGRANGE, cMo_lagrange);
+ r_lagrange = poseMin.computeResidual(cMo_lagrange);
+ is_valid_lagrange = true;
+ } catch(/*vpException &e*/...) {
+// std::cerr << e.what() << std::endl;
+ }
+
+ try {
+ poseMin.computePose(vpPose::DEMENTHON, cMo_dementhon);
+ r_dementhon = poseMin.computeResidual(cMo_dementhon);
+ is_valid_dementhon = true;
+ } catch(/*vpException &e*/...) {
+// std::cerr << e.what() << std::endl;
+ }
+
+ //If residual returned is not a number (NAN), set valid to false
+ if(vpMath::isNaN(r_lagrange)) {
+ is_valid_lagrange = false;
+ r_lagrange = DBL_MAX;
+ }
+
+ if(vpMath::isNaN(r_dementhon)) {
+ is_valid_dementhon = false;
+ r_dementhon = DBL_MAX;
+ }
+
+ //If at least one pose computation is OK,
+ //we can continue, otherwise pick another random set
+ if(is_valid_lagrange || is_valid_dementhon) {
+ if (r_lagrange < r_dementhon) {
+ r = r_lagrange;
+// cMo = cMo_lagrange;
+ cMo_tmp = cMo_lagrange;
+ }
+ else {
+ r = r_dementhon;
+// cMo = cMo_dementhon;
+ cMo_tmp = cMo_dementhon;
+ }
+ r = sqrt(r) / (double) nbMinRandom;
+
+ //Filter the pose using some criterion (orientation angles, translations, etc.)
+ bool isPoseValid = true;
+ if(func != NULL) {
+ isPoseValid = func(&cMo_tmp);
+ if(isPoseValid) {
+ cMo = cMo_tmp;
+ }
+ } else {
+ //No post filtering on pose, so copy cMo_temp to cMo
+ cMo = cMo_tmp;
+ }
+
+ if (isPoseValid && r < ransacThreshold)
+ {
+ unsigned int nbInliersCur = 0;
+ unsigned int iter = 0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ vpPoint pt = *it;
+ vpPoint p(pt) ;
+ p.track(cMo) ;
+
+ double d = vpMath::sqr(p.get_x() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.get_y()) ;
+ double error = sqrt(d) ;
+ if(error < ransacThreshold) {
+ // the point is considered as inlier if the error is below the threshold
+ // But, we need also to check if it is not a degenerate point
+ bool degenerate = false;
+
+ for(unsigned int it_inlier_index = 0; it_inlier_index< cur_consensus.size(); it_inlier_index++){
+ std::list<vpPoint>::const_iterator it_point = listP.begin();
+ std::advance(it_point, cur_consensus[it_inlier_index]);
+ pt = *it_point;
+
+ vpPoint ptdeg = *it;
+ if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
+ ((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
+ degenerate = true;
+ break;
+ }
+ }
+
+ if(!degenerate) {
+ nbInliersCur++;
+ cur_consensus.push_back(iter);
+ }
+ else {
+ cur_outliers.push_back(iter);
+ }
+ }
+ else {
+ cur_outliers.push_back(iter);
+ }
+
+ iter++;
+ }
+
+ if(nbInliersCur > nbInliers)
+ {
+ foundSolution = true;
+ best_consensus = cur_consensus;
+ nbInliers = nbInliersCur;
+ }
+
+ nbTrials++;
+
+ if(nbTrials >= ransacMaxTrials) {
+// vpERROR_TRACE("Ransac reached the maximum number of trials");
+ foundSolution = true;
+ }
+ }
+ else {
+ nbTrials++;
+
+ if(nbTrials >= ransacMaxTrials) {
+// vpERROR_TRACE("Ransac reached the maximum number of trials");
+ }
+ }
+ } else {
+ nbTrials++;
+
+ if(nbTrials >= ransacMaxTrials) {
+// vpERROR_TRACE("Ransac reached the maximum number of trials");
+ }
+ }
+ }
+
+ if(foundSolution) {
+// std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
+
+ //Display the random picked points
+ /*
+ std::cout << "Randoms : ";
+ for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
+ std::cout << cur_randoms[i] << " ";
+ std::cout << std::endl;
+ */
+
+ //Display the outliers
+ /*
+ std::cout << "Outliers : ";
+ for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
+ std::cout << cur_outliers[i] << " ";
+ std::cout << std::endl;
+ */
+
+ //Even if the cardinality of the best consensus set is inferior to ransacNbInlierConsensus,
+ //we want to refine the solution with data in best_consensus and return this pose.
+ //This is an approach used for example in p118 in Multiple View Geometry in Computer Vision, Hartley, R.~I. and Zisserman, A.
+ if(nbInliers >= nbMinRandom) //if(nbInliers >= (unsigned)ransacNbInlierConsensus)
+ {
+ //Refine the solution using all the points in the consensus set and with VVS pose estimation
+ vpPose pose ;
+ for(unsigned i = 0 ; i < best_consensus.size(); i++)
+ {
+ std::list<vpPoint>::const_iterator iter = listP.begin();
+ std::advance(iter, best_consensus[i]);
+ vpPoint pt = *iter;
+
+ pose.addPoint(pt) ;
+ ransacInliers.push_back(pt);
+ }
+
+ //Update the list of inlier index
+ ransacInlierIndex = best_consensus;
+
+ //Flags set if pose computation is OK
+ bool is_valid_lagrange = false;
+ bool is_valid_dementhon = false;
+
+ //Set maximum value for residuals
+ r_lagrange = DBL_MAX;
+ r_dementhon = DBL_MAX;
+
+ try {
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
+ r_lagrange = pose.computeResidual(cMo_lagrange);
+ is_valid_lagrange = true;
+ } catch(/*vpException &e*/...) {
+// std::cerr << e.what() << std::endl;
+ }
+
+ try {
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
+ r_dementhon = pose.computeResidual(cMo_dementhon);
+ is_valid_dementhon = true;
+ } catch(/*vpException &e*/...) {
+// std::cerr << e.what() << std::endl;
+ }
+
+ //If residual returned is not a number (NAN), set valid to false
+ if(vpMath::isNaN(r_lagrange)) {
+ is_valid_lagrange = false;
+ r_lagrange = DBL_MAX;
+ }
+
+ if(vpMath::isNaN(r_dementhon)) {
+ is_valid_dementhon = false;
+ r_dementhon = DBL_MAX;
+ }
+
+ if(is_valid_lagrange || is_valid_dementhon) {
+ if (r_lagrange < r_dementhon) {
+ cMo = cMo_lagrange;
+ }
+ else {
+ cMo = cMo_dementhon;
+ }
+
+ pose.setCovarianceComputation(computeCovariance);
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ //In some rare cases, the final pose could not respect the pose criterion even
+ //if the 4 minimal points picked respect the pose criterion.
+ if(func != NULL && !func(&cMo)) {
+ return false;
+ }
+
+ if(computeCovariance) {
+ covarianceMatrix = pose.covarianceMatrix;
+ }
+ }
+ } else {
+ return false;
+ }
+ }
+
+ return foundSolution;
+}
+
+/*!
+ Match a vector p2D of 2D point (x,y) and a vector p3D of 3D points
+ (X,Y,Z) using the Ransac algorithm.
+
+ At least numberOfInlierToReachAConsensus of true correspondance are required
+ to validate the pose
+
+ The inliers are given in a vector of vpPoint listInliers.
+
+ The pose is returned in cMo.
+
+ \param p2D : Vector of 2d points (x and y attributes are used).
+ \param p3D : Vector of 3d points (oX, oY and oZ attributes are used).
+ \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
+ to consider a trial as correct.
+ \param threshold : The maximum error allowed between the 2d points and the
+ reprojection of its associated 3d points by the current pose (in meter).
+ \param ninliers : Number of inliers found for the best solution.
+ \param listInliers : Vector of points (2d and 3d) that are inliers for the best solution.
+ \param cMo : The computed pose (best solution).
+ \param maxNbTrials : Maximum number of trials before considering a solution
+ fitting the required \e numberOfInlierToReachAConsensus and \e threshold
+ cannot be found.
+*/
+void vpPose::findMatch(std::vector<vpPoint> &p2D,
+ std::vector<vpPoint> &p3D,
+ const unsigned int &numberOfInlierToReachAConsensus,
+ const double &threshold,
+ unsigned int &ninliers,
+ std::vector<vpPoint> &listInliers,
+ vpHomogeneousMatrix &cMo,
+ const int &maxNbTrials )
+{
+ vpPose pose;
+
+ int nbPts = 0;
+ for(unsigned int i = 0 ; i < p2D.size() ; i++)
+ {
+ for(unsigned int j = 0 ; j < p3D.size() ; j++)
+ {
+ vpPoint pt(p3D[j].getWorldCoordinates());
+ pt.set_x(p2D[i].get_x());
+ pt.set_y(p2D[i].get_y());
+ pose.addPoint(pt);
+ nbPts++;
+ }
+ }
+
+ if (pose.listP.size() < 4)
+ {
+ vpERROR_TRACE("Ransac method cannot be used in that case ") ;
+ vpERROR_TRACE("(at least 4 points are required)") ;
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ",pose.listP.size()) ;
+ throw(vpPoseException(vpPoseException::notEnoughPointError,
+ "Not enough points ")) ;
+ }
+ else
+ {
+ pose.setRansacMaxTrials(maxNbTrials);
+ pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
+ pose.setRansacThreshold(threshold);
+ pose.computePose(vpPose::RANSAC, cMo);
+ ninliers = pose.getRansacNbInliers();
+ listInliers = pose.getRansacInliers();
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp
new file mode 100644
index 0000000..21e4955
--- /dev/null
+++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp
@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pose computation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPoseVirtualVisualServoing.cpp
+ \brief Compute the pose using virtual visual servoing approach
+*/
+
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpRobust.h>
+
+/*!
+ \brief Compute the pose using virtual visual servoing approach
+
+ This approach is described in \cite Marchand02c.
+
+*/
+
+void
+vpPose::poseVirtualVS(vpHomogeneousMatrix & cMo)
+{
+ try
+ {
+
+ double residu_1 = 1e8 ;
+ double r =1e8-1;
+
+ // we stop the minimization when the error is bellow 1e-8
+
+ int iter = 0 ;
+
+ unsigned int nb = (unsigned int)listP.size() ;
+ vpMatrix L(2*nb,6) ;
+ vpColVector err(2*nb) ;
+ vpColVector sd(2*nb),s(2*nb) ;
+ vpColVector v ;
+
+ vpPoint P;
+ std::list<vpPoint> lP ;
+
+ // create sd
+ unsigned int k =0 ;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ sd[2*k] = P.get_x() ;
+ sd[2*k+1] = P.get_y() ;
+ lP.push_back(P);
+ k ++;
+ }
+
+ vpHomogeneousMatrix cMoPrev = cMo;
+ while((int)((residu_1 - r)*1e12) !=0)
+ {
+ residu_1 = r ;
+
+ // Compute the interaction matrix and the error
+ k =0 ;
+ for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
+ {
+ P = *it;
+ // forward projection of the 3D model for a given pose
+ // change frame coordinates
+ // perspective projection
+ P.track(cMo) ;
+
+ double x = s[2*k] = P.get_x(); /* point projected from cMo */
+ double y = s[2*k+1] = P.get_y();
+ double Z = P.get_Z() ;
+ L[2*k][0] = -1/Z ;
+ L[2*k][1] = 0 ;
+ L[2*k][2] = x/Z ;
+ L[2*k][3] = x*y ;
+ L[2*k][4] = -(1+x*x) ;
+ L[2*k][5] = y ;
+
+ L[2*k+1][0] = 0 ;
+ L[2*k+1][1] = -1/Z ;
+ L[2*k+1][2] = y/Z ;
+ L[2*k+1][3] = 1+y*y ;
+ L[2*k+1][4] = -x*y ;
+ L[2*k+1][5] = -x ;
+
+ k+=1 ;
+ }
+ err = s - sd ;
+
+ // compute the residual
+ r = err.sumSquare() ;
+
+ // compute the pseudo inverse of the interaction matrix
+ vpMatrix Lp ;
+ L.pseudoInverse(Lp,1e-16) ;
+
+ // compute the VVS control law
+ v = -lambda*Lp*err ;
+
+ //std::cout << "r=" << r <<std::endl ;
+ // update the pose
+
+ cMoPrev = cMo;
+ cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
+ if (iter++>vvsIterMax) break ;
+ }
+
+ if(computeCovariance)
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
+ }
+
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+}
+
+/*!
+ \brief Compute the pose using virtual visual servoing approach and
+ a robust control law
+
+ This approach is described in \cite Comport06b.
+
+*/
+void
+vpPose::poseVirtualVSrobust(vpHomogeneousMatrix & cMo)
+{
+ try{
+
+ double residu_1 = 1e8 ;
+ double r =1e8-1;
+
+ // we stop the minimization when the error is bellow 1e-8
+ vpMatrix W ;
+ vpRobust robust((unsigned int)(2*listP.size())) ;
+ robust.setThreshold(0.0000) ;
+ vpColVector w,res ;
+
+ unsigned int nb = (unsigned int)listP.size() ;
+ vpMatrix L(2*nb,6) ;
+ vpColVector error(2*nb) ;
+ vpColVector sd(2*nb),s(2*nb) ;
+ vpColVector v ;
+
+ listP.front() ;
+ vpPoint P;
+ std::list<vpPoint> lP ;
+
+ // create sd
+ unsigned int k_ =0 ;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
+ {
+ P = *it;
+ sd[2*k_] = P.get_x() ;
+ sd[2*k_+1] = P.get_y() ;
+ lP.push_back(P) ;
+ k_ ++;
+ }
+ int iter = 0 ;
+ res.resize(s.getRows()/2) ;
+ w.resize(s.getRows()/2) ;
+ W.resize(s.getRows(), s.getRows()) ;
+ w =1 ;
+
+ while((int)((residu_1 - r)*1e12) !=0)
+ {
+ residu_1 = r ;
+
+ // Compute the interaction matrix and the error
+ k_ =0 ;
+ for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
+ {
+ P = *it;
+ // forward projection of the 3D model for a given pose
+ // change frame coordinates
+ // perspective projection
+ P.track(cMo) ;
+
+ double x = s[2*k_] = P.get_x(); // point projected from cMo
+ double y = s[2*k_+1] = P.get_y();
+ double Z = P.get_Z() ;
+ L[2*k_][0] = -1/Z ;
+ L[2*k_][1] = 0 ;
+ L[2*k_][2] = x/Z ;
+ L[2*k_][3] = x*y ;
+ L[2*k_][4] = -(1+x*x) ;
+ L[2*k_][5] = y ;
+
+ L[2*k_+1][0] = 0 ;
+ L[2*k_+1][1] = -1/Z ;
+ L[2*k_+1][2] = y/Z ;
+ L[2*k_+1][3] = 1+y*y ;
+ L[2*k_+1][4] = -x*y ;
+ L[2*k_+1][5] = -x ;
+
+ k_ ++;
+
+ }
+ error = s - sd ;
+
+ // compute the residual
+ r = error.sumSquare() ;
+
+ for(unsigned int k=0 ; k <error.getRows()/2 ; k++)
+ {
+ res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
+ }
+ robust.setIteration(0);
+ robust.MEstimator(vpRobust::TUKEY, res, w);
+
+ // compute the pseudo inverse of the interaction matrix
+ for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
+ {
+ W[2*k][2*k] = w[k] ;
+ W[2*k+1][2*k+1] = w[k] ;
+ }
+ // compute the pseudo inverse of the interaction matrix
+ vpMatrix Lp ;
+ (W*L).pseudoInverse(Lp,1e-6) ;
+
+ // compute the VVS control law
+ v = -lambda*Lp*W*error ;
+
+ cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
+ if (iter++>vvsIterMax) break ;
+ }
+
+ if(computeCovariance)
+ covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
+ }
+ catch(...)
+ {
+ vpERROR_TRACE(" ") ;
+ throw ;
+ }
+
+}
+
+
diff --git a/modules/vision/test/homography/testDisplacement.cpp b/modules/vision/test/homography/testDisplacement.cpp
new file mode 100644
index 0000000..cfcec29
--- /dev/null
+++ b/modules/vision/test/homography/testDisplacement.cpp
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Tests transformation within various representations of rotation.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file testDisplacement.cpp
+ \brief Tests transformation within various representations of rotation
+*/
+
+/*!
+ \example testDisplacement.cpp
+*/
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+bool test(const std::string &s, const vpHomography &H, const std::vector<double> &bench)
+{
+ static unsigned int cpt = 0;
+ std::cout << "** Test " << ++cpt << std::endl;
+ std::cout << s << "(" << H.getRows() << "," << H.getCols() << ") = \n[" << H << "]" << std::endl;
+ if(bench.size() != H.size()) {
+ std::cout << "Test fails: bad size wrt bench" << std::endl;
+ return false;
+ }
+ for (unsigned int i=0; i<H.size(); i++) {
+ if (std::fabs(H.data[i]-bench[i]) > std::fabs(H.data[i])*std::numeric_limits<double>::epsilon()) {
+ std::cout << "Test fails: bad content" << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+int main()
+{
+ try {
+ int err = 1;
+ {
+ vpHomography H;
+ H.eye();
+ std::vector<double> bench(9, 0);
+ bench[0] = bench[4] = bench[8] = 1.;
+ if (test("H", H, bench) == false)
+ return err;
+ if (test("H", H/H[2][2], bench) == false)
+ return err;
+ }
+ {
+ vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
+
+ std::cout << "Initialization " <<std::endl ;
+ // std::cout << tu << std::endl ;
+
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
+ vpRotationMatrix R(tu) ;
+
+ // pure rotation
+ vpHomogeneousMatrix M ;
+ M.insert(R) ;
+
+ std::cout << "M" <<std::endl <<M << std::endl ;
+ vpPlane p(0,0,1,1) ;
+
+ vpHomography H(M,p) ;
+
+ std::cout << "H" <<std::endl <<H << std::endl ;
+
+ vpColVector n ;
+ vpTranslationVector T ;
+
+ H.computeDisplacement(R,T,n) ;
+
+ std::cout << "R" <<std::endl << R ;
+ std::cout << "T" <<std::endl << T.t() << std::endl;
+ std::cout << "n" <<std::endl << n.t() << std::endl;
+ }
+ std::cout <<"------------------------------------------------------" << std::endl ;
+
+ {
+ vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
+
+ std::cout << "Initialization " << std::endl ;
+ // std::cout << tu << std::endl ;
+
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
+ vpRotationMatrix R(tu) ;
+
+ // pure rotation
+ vpHomogeneousMatrix M ;
+ M.insert(R) ;
+
+ M[0][3] = 0.21 ;
+ M[1][3] = 0.31 ;
+ M[2][3] = 0.5 ;
+
+ std::cout << "M" << std::endl << M << std::endl ;
+ vpPlane p(0,0,1,1) ;
+
+ vpHomography H(M,p) ;
+
+ std::cout << "H" << std::endl << H << std::endl ;
+
+ vpColVector n ;
+ vpTranslationVector T ;
+
+ H.computeDisplacement(R,T,n) ;
+
+ std::cout << "R" <<std::endl << R ;
+ std::cout << "T" <<std::endl << T.t() << std::endl;
+ std::cout << "n" <<std::endl << n.t() << std::endl;
+ }
+
+ std::cout <<"------------------------------------------------------" << std::endl ;
+ {
+ vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45)) ;
+
+ vpRotationMatrix R(tu) ;
+
+ // pure rotation
+ vpHomogeneousMatrix M ;
+ M.insert(R) ;
+
+ M[0][3] = 0.21 ;
+ M[1][3] = -0.31 ;
+ M[2][3] = 0.5 ;
+
+ std::cout << "M" << std::endl << M << std::endl ;
+ vpPlane p(0.4,-0.5,0.5,1) ;
+
+ vpHomography H(M,p) ;
+
+ std::cout << "H" << std::endl << H << std::endl ;
+
+ vpColVector n ;
+ vpTranslationVector T ;
+ H.computeDisplacement(R,T,n) ;
+
+ std::cout << "R" <<std::endl << R ;
+ std::cout << "T" <<std::endl << T.t() << std::endl;
+ std::cout << "n" <<std::endl << n.t() << std::endl;
+
+ vpPlane p1(n[0],n[1],n[2],1.0) ;
+ H.buildFrom(R,T,p1) ;
+ std::cout << "H" << std::endl << H << std::endl ;
+ }
+ std::cout << "All tests succeed" << std::endl;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vision/test/key-point/testKeyPoint-2.cpp b/modules/vision/test/key-point/testKeyPoint-2.cpp
new file mode 100644
index 0000000..5a24654
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-2.cpp
@@ -0,0 +1,437 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test keypoint matching and pose estimation.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoints matching.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint-2.cpp
+
+ \brief Test keypoint matching and pose estimation.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> I;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+
+ //Build the name of the image files
+ std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
+ vpImageIo::read(I, filenameRef);
+ std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+
+ if (opt_display) {
+ display.init(I, 0, 0, "ORB keypoints matching and pose estimation");
+ }
+
+ vpCameraParameters cam;
+ vpMbEdgeTracker tracker;
+ //Load config for tracker
+ std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+
+ bool usexml = false;
+#ifdef VISP_HAVE_XML2
+ tracker.loadConfigFile(tracker_config_file);
+ tracker.getCameraParameters(cam);
+
+ usexml = true;
+#endif
+ if (! usexml) {
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ me.setNbTotalSample(250);
+ tracker.setMovingEdge(me);
+ cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
+ tracker.setCameraParameters(cam);
+ tracker.setNearClippingDistance(0.01);
+ tracker.setFarClippingDistance(100.0);
+ tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
+ }
+
+ tracker.setAngleAppear(vpMath::rad(89));
+ tracker.setAngleDisappear(vpMath::rad(89));
+
+ //Load CAO model
+ std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.cao");
+ tracker.loadModel(cao_model_file);
+
+ //Initialize the pose
+ std::string init_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.init");
+ if (opt_display && opt_click_allowed) {
+ tracker.initClick(I, init_file);
+ }
+ else
+ {
+ vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
+ tracker.initFromPose(I, cMoi);
+ }
+
+ //Get the init pose
+ vpHomogeneousMatrix cMo;
+ tracker.getPose(cMo);
+
+ //Init keypoints
+ vpKeyPoint keypoints("ORB", "ORB", "BruteForce-Hamming");
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
+ //Bug when using LSH index with FLANN and OpenCV 2.3.1.
+ //see http://code.opencv.org/issues/1741 (Bug #1741)
+ keypoints.setMatcher("FlannBased");
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ keypoints.setDetectorParameter("ORB", "nLevels", 1);
+#else
+ cv::Ptr<cv::ORB> orb_detector = keypoints.getDetector("ORB").dynamicCast<cv::ORB>();
+ if(orb_detector != NULL) {
+ orb_detector->setNLevels(1);
+ }
+#endif
+#endif
+
+ //Detect keypoints on the current image
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ double elapsedTime;
+ keypoints.detect(I, trainKeyPoints, elapsedTime);
+
+ //Keep only keypoints on the cube
+ std::vector<vpPolygon> polygons;
+ std::vector<std::vector<vpPoint> > roisPt;
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(false);
+ polygons = pair.first;
+ roisPt = pair.second;
+
+ //Compute the 3D coordinates
+ std::vector<cv::Point3f> points3f;
+ vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
+
+ //Build the reference keypoints
+ keypoints.buildReference(I, trainKeyPoints, points3f, false, 1);
+
+
+ //Read image 150
+ filenameRef = vpIoTools::createFilePath(dirname, "image0150.pgm");
+ vpImageIo::read(I, filenameRef);
+
+ //Init pose at image 150
+ cMo.buildFrom(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828);
+ tracker.initFromPose(I, cMo);
+
+ //Detect keypoints on the image 150
+ keypoints.detect(I, trainKeyPoints, elapsedTime);
+
+ //Keep only keypoints on the cube
+ pair = tracker.getPolygonFaces(false);
+ polygons = pair.first;
+ roisPt = pair.second;
+
+ //Compute the 3D coordinates
+ vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
+
+ //Build the reference keypoints
+ keypoints.buildReference(I, trainKeyPoints, points3f, true, 2);
+
+
+ //Read image 200
+ filenameRef = vpIoTools::createFilePath(dirname, "image0200.pgm");
+ vpImageIo::read(I, filenameRef);
+
+ //Init pose at image 200
+ cMo.buildFrom(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025);
+ tracker.initFromPose(I, cMo);
+
+ //Detect keypoints on the image 200
+ keypoints.detect(I, trainKeyPoints, elapsedTime);
+
+ //Keep only keypoints on the cube
+ pair = tracker.getPolygonFaces(false);
+ polygons = pair.first;
+ roisPt = pair.second;
+
+ //Compute the 3D coordinates
+ vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
+
+ //Build the reference keypoints
+ keypoints.buildReference(I, trainKeyPoints, points3f, true, 3);
+
+
+ //Init reader for getting the input image sequence
+ vpVideoReader g;
+ g.setFileName(filenameCur);
+ g.open(I);
+ g.acquire(I);
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display2;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display2;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display2;
+#else
+ vpDisplayOpenCV display2;
+#endif
+
+ vpImage<unsigned char> IMatching;
+
+ keypoints.createImageMatching(I, IMatching);
+
+ if (opt_display) {
+ display2.init(IMatching, 0, (int)I.getHeight() + 80, "IMatching");
+ }
+
+ bool opt_click = false;
+ double error;
+ vpMouseButton::vpMouseButtonType button;
+ while((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) {
+ g.acquire(I);
+
+ if(opt_display) {
+ vpDisplay::display(I);
+
+ //Display image matching
+ keypoints.insertImageMatching(I, IMatching);
+
+ vpDisplay::display(IMatching);
+ }
+
+ //Match keypoints and estimate the pose
+ if(keypoints.matchPoint(I, cam, cMo, error, elapsedTime)) {
+ tracker.setCameraParameters(cam);
+ tracker.setPose(I, cMo);
+
+ if(opt_display) {
+ tracker.display(I, cMo, cam, vpColor::red, 2);
+ vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
+
+ std::vector<vpImagePoint> ransacInliers = keypoints.getRansacInliers();
+ std::vector<vpImagePoint> ransacOutliers = keypoints.getRansacOutliers();
+
+ for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
+ vpDisplay::displayCircle(I, *it, 4, vpColor::green);
+ vpImagePoint imPt(*it);
+ imPt.set_u(imPt.get_u() + I.getWidth());
+ imPt.set_v(imPt.get_v() + I.getHeight());
+ vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::green);
+ }
+
+ for(std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
+ vpDisplay::displayCircle(I, *it, 4, vpColor::red);
+ vpImagePoint imPt(*it);
+ imPt.set_u(imPt.get_u() + I.getWidth());
+ imPt.set_v(imPt.get_v() + I.getHeight());
+ vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::red);
+ }
+
+ keypoints.displayMatching(I, IMatching);
+
+ //Display model in the correct sub-image in IMatching
+ vpCameraParameters cam2;
+ cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(),
+ cam.get_u0() + I.getWidth(), cam.get_v0() + I.getHeight());
+ tracker.setCameraParameters(cam2);
+ tracker.setPose(IMatching, cMo);
+ tracker.display(IMatching, cMo, cam2, vpColor::red, 2);
+ vpDisplay::displayFrame(IMatching, cMo, cam2, 0.025, vpColor::none, 3);
+ }
+ }
+
+ if(opt_display) {
+ vpDisplay::flush(I);
+ vpDisplay::flush(IMatching);
+ }
+
+ if (opt_click_allowed && opt_display) {
+ //Click requested to process next image
+ if(opt_click) {
+ vpDisplay::getClick(I, button, true);
+ if(button == vpMouseButton::button3) {
+ opt_click = false;
+ }
+ } else {
+ //Use right click to enable/disable step by step tracking
+ if(vpDisplay::getClick(I, button, false)) {
+ if (button == vpMouseButton::button3) {
+ opt_click = true;
+ }
+ else if(button == vpMouseButton::button1) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint-2 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint-3.cpp b/modules/vision/test/key-point/testKeyPoint-3.cpp
new file mode 100644
index 0000000..5af6d40
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-3.cpp
@@ -0,0 +1,287 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test keypoint matching with mostly OpenCV functions calls
+ * to detect potential memory leaks in testKeyPoint.cpp.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoints matching.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint-3.cpp
+
+ \brief Test keypoint matching with mostly OpenCV functions calls
+ to detect potential memory leaks in testKeyPoint.cpp.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> Iref, Icur, Imatch;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+
+ //Build the name of the image files
+ std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
+ vpImageIo::read(Iref, filenameRef);
+ std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
+
+ //Init keypoints
+ cv::Ptr<cv::FeatureDetector> detector;
+ cv::Ptr<cv::DescriptorExtractor> extractor;
+ cv::Ptr<cv::DescriptorMatcher> matcher;
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ detector = cv::ORB::create();
+ extractor = cv::ORB::create();
+#else
+ detector = cv::FeatureDetector::create("ORB");
+ extractor = cv::DescriptorExtractor::create("ORB");
+#endif
+ matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
+
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ cv::Mat matImg, trainDescriptors;
+ vpImageConvert::convert(Iref, matImg);
+ detector->detect(matImg, trainKeyPoints);
+ extractor->compute(matImg, trainKeyPoints, trainDescriptors);
+
+ vpVideoReader g;
+ g.setFileName(filenameCur);
+ g.open(Icur);
+ g.acquire(Icur);
+
+ Imatch.resize(Icur.getHeight(), 2*Icur.getWidth());
+ Imatch.insert(Iref, vpImagePoint(0,0));
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+
+ if (opt_display) {
+ display.init(Imatch, 0, 0, "ORB keypoints matching");
+ }
+
+ bool opt_click = false;
+ vpMouseButton::vpMouseButtonType button;
+ while(!g.end()) {
+ g.acquire(Icur);
+ Imatch.insert(Icur, vpImagePoint(0, Icur.getWidth()));
+
+ if(opt_display) {
+ vpDisplay::display(Imatch);
+ }
+
+ vpImageConvert::convert(Icur, matImg);
+ std::vector<cv::KeyPoint> queryKeyPoints;
+ detector->detect(matImg, queryKeyPoints);
+
+ cv::Mat queryDescriptors;
+ extractor->compute(matImg, queryKeyPoints, queryDescriptors);
+
+ std::vector<std::vector<cv::DMatch> > knn_matches;
+ std::vector<cv::DMatch> matches;
+ matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
+ for(std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end(); ++it) {
+ if(it->size() > 1) {
+ double ratio = (*it)[0].distance / (*it)[1].distance;
+ if(ratio < 0.85) {
+ matches.push_back((*it)[0]);
+ }
+ }
+ }
+
+ if(opt_display) {
+ for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpImagePoint leftPt(trainKeyPoints[(size_t) it->trainIdx].pt.y, trainKeyPoints[(size_t) it->trainIdx].pt.x);
+ vpImagePoint rightPt(queryKeyPoints[(size_t) it->queryIdx].pt.y, queryKeyPoints[(size_t) it->queryIdx].pt.x
+ + Iref.getWidth());
+ vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
+ }
+
+ vpDisplay::flush(Imatch);
+ }
+
+ //Click requested to process next image
+ if (opt_click_allowed && opt_display) {
+ if(opt_click) {
+ vpDisplay::getClick(Imatch, button, true);
+ if(button == vpMouseButton::button3) {
+ opt_click = false;
+ }
+ } else {
+ //Use right click to enable/disable step by step tracking
+ if(vpDisplay::getClick(Imatch, button, false)) {
+ if (button == vpMouseButton::button3) {
+ opt_click = true;
+ }
+ else if(button == vpMouseButton::button1) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint-3 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint-4.cpp b/modules/vision/test/key-point/testKeyPoint-4.cpp
new file mode 100644
index 0000000..c8d3ed9
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-4.cpp
@@ -0,0 +1,406 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test keypoint matching and pose estimation with mostly OpenCV functions calls
+ * to detect potential memory leaks in testKeyPoint-2.cpp.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vision/vpKeyPoint.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoints matching.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint-4.cpp
+
+ \brief Test keypoint matching and pose estimation with mostly OpenCV functions calls
+ to detect potential memory leaks in testKeyPoint-2.cpp.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> I, Imatch, Iref;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+
+ //Build the name of the image files
+ std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
+ vpImageIo::read(I, filenameRef);
+ Iref = I;
+ std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display, display2;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display, display2;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display, display2;
+#else
+ vpDisplayOpenCV display, display2;
+#endif
+
+ if (opt_display) {
+ display.init(I, 0, 0, "ORB keypoints matching");
+ Imatch.resize(I.getHeight(), 2*I.getWidth());
+ Imatch.insert(I, vpImagePoint(0, 0));
+ display2.init(Imatch, 0, (int)I.getHeight() + 70, "ORB keypoints matching");
+ }
+
+ vpCameraParameters cam;
+ vpMbEdgeTracker tracker;
+ //Load config for tracker
+ std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+
+ bool usexml = false;
+#ifdef VISP_HAVE_XML2
+ tracker.loadConfigFile(tracker_config_file);
+ tracker.getCameraParameters(cam);
+
+ usexml = true;
+#endif
+ if (! usexml) {
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ me.setNbTotalSample(250);
+ tracker.setMovingEdge(me);
+ cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
+ tracker.setCameraParameters(cam);
+ tracker.setNearClippingDistance(0.01);
+ tracker.setFarClippingDistance(100.0);
+ tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
+ }
+
+ tracker.setAngleAppear(vpMath::rad(89));
+ tracker.setAngleDisappear(vpMath::rad(89));
+
+ //Load CAO model
+ std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.cao");
+ tracker.loadModel(cao_model_file);
+
+ //Initialize the pose
+ std::string init_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.init");
+ if (opt_display && opt_click_allowed) {
+ tracker.initClick(I, init_file);
+ }
+ else
+ {
+ vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
+ tracker.initFromPose(I, cMoi);
+ }
+
+ //Get the init pose
+ vpHomogeneousMatrix cMo;
+ tracker.getPose(cMo);
+
+ //Init keypoints
+ cv::Ptr<cv::FeatureDetector> detector;
+ cv::Ptr<cv::DescriptorExtractor> extractor;
+ cv::Ptr<cv::DescriptorMatcher> matcher;
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ detector = cv::ORB::create(500, 1.2f, 1);
+ extractor = cv::ORB::create(500, 1.2f, 1);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+ detector = cv::FeatureDetector::create("ORB");
+ extractor = cv::DescriptorExtractor::create("ORB");
+#endif
+ matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ detector->set("nLevels", 1);
+#endif
+
+ //Detect keypoints on the current image
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ cv::Mat matImg;
+ vpImageConvert::convert(I, matImg);
+ detector->detect(matImg, trainKeyPoints);
+
+
+ //Keep only keypoints on the cube
+ std::vector<vpPolygon> polygons;
+ std::vector<std::vector<vpPoint> > roisPt;
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(false);
+ polygons = pair.first;
+ roisPt = pair.second;
+
+ //Compute the 3D coordinates
+ std::vector<cv::Point3f> points3f;
+ vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
+
+
+ //Extract descriptors
+ cv::Mat trainDescriptors;
+ extractor->compute(matImg, trainKeyPoints, trainDescriptors);
+
+ if(trainKeyPoints.size() != (size_t) trainDescriptors.rows || trainKeyPoints.size() != points3f.size()) {
+ std::cerr << "Problem with training data size !" << std::endl;
+ return -1;
+ }
+
+
+ //Init reader for getting the input image sequence
+ vpVideoReader g;
+ g.setFileName(filenameCur);
+ g.open(I);
+ g.acquire(I);
+
+ bool opt_click = false;
+ vpMouseButton::vpMouseButtonType button;
+ while((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) {
+ g.acquire(I);
+
+ vpImageConvert::convert(I, matImg);
+ std::vector<cv::KeyPoint> queryKeyPoints;
+ detector->detect(matImg, queryKeyPoints);
+
+ cv::Mat queryDescriptors;
+ extractor->compute(matImg, queryKeyPoints, queryDescriptors);
+
+ std::vector<std::vector<cv::DMatch> > knn_matches;
+ std::vector<cv::DMatch> matches;
+ matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
+ for(std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end(); ++it) {
+ if(it->size() > 1) {
+ double ratio = (*it)[0].distance / (*it)[1].distance;
+ if(ratio < 0.85) {
+ matches.push_back((*it)[0]);
+ }
+ }
+ }
+
+ vpPose estimated_pose;
+ for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpPoint pt(points3f[(size_t)(it->trainIdx)].x,
+ points3f[(size_t)(it->trainIdx)].y,
+ points3f[(size_t)(it->trainIdx)].z);
+
+ double x = 0.0, y = 0.0;
+ vpPixelMeterConversion::convertPoint(cam, queryKeyPoints[(size_t)(it->queryIdx)].pt.x, queryKeyPoints[(size_t)(it->queryIdx)].pt.y, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+
+ estimated_pose.addPoint(pt);
+ }
+
+ bool is_pose_estimated = false;
+ if(estimated_pose.npt >= 4) {
+ try {
+ unsigned int nb_inliers = (unsigned int) (0.6 * estimated_pose.npt);
+ estimated_pose.setRansacNbInliersToReachConsensus(nb_inliers);
+ estimated_pose.setRansacThreshold(0.01);
+ estimated_pose.setRansacMaxTrials(500);
+ estimated_pose.computePose(vpPose::RANSAC, cMo);
+ is_pose_estimated = true;
+ } catch(...) {
+ is_pose_estimated = false;
+ }
+ }
+
+ if(opt_display) {
+ vpDisplay::display(I);
+
+ Imatch.insert(I, vpImagePoint(0, Iref.getWidth()));
+ vpDisplay::display(Imatch);
+ for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpImagePoint leftPt(trainKeyPoints[(size_t) it->trainIdx].pt.y, trainKeyPoints[(size_t) it->trainIdx].pt.x);
+ vpImagePoint rightPt(queryKeyPoints[(size_t) it->queryIdx].pt.y, queryKeyPoints[(size_t) it->queryIdx].pt.x
+ + Iref.getWidth());
+ vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
+ }
+
+ if(is_pose_estimated) {
+ tracker.setPose(I, cMo);
+ tracker.display(I, cMo, cam, vpColor::red);
+ vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
+ }
+
+ vpDisplay::flush(Imatch);
+ vpDisplay::flush(I);
+ }
+
+ //Click requested to process next image
+ if (opt_click_allowed && opt_display) {
+ if(opt_click) {
+ vpDisplay::getClick(I, button, true);
+ if(button == vpMouseButton::button3) {
+ opt_click = false;
+ }
+ } else {
+ //Use right click to enable/disable step by step tracking
+ if(vpDisplay::getClick(I, button, false)) {
+ if (button == vpMouseButton::button3) {
+ opt_click = true;
+ }
+ else if(button == vpMouseButton::button1) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint-4 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint-5.cpp b/modules/vision/test/key-point/testKeyPoint-5.cpp
new file mode 100644
index 0000000..f254cab
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-5.cpp
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test keypoints detection with OpenCV, specially the Pyramid implementation
+ * feature misssing in OpenCV 3.0.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpKeyPoint.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoints detection.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint-5.cpp
+
+ \brief Test keypoints detection with OpenCV, specially the Pyramid implementation
+ feature missing in OpenCV 3.0.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> I;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+
+ //Build the name of the image files
+ std::string filename = vpIoTools::createFilePath(dirname, "/Klimt.png");
+ vpImageIo::read(I, filename);
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+
+ if(opt_display) {
+ display.init(I, 0, 0, "KeyPoints detection.");
+ }
+
+ vpKeyPoint keyPoints;
+
+ //Will test the different types of keypoints detection to see if there is a problem
+ //between OpenCV versions, modules or constructors
+ std::vector<std::string> detectorNames;
+ detectorNames.push_back("PyramidFAST");
+ detectorNames.push_back("FAST");
+ detectorNames.push_back("PyramidMSER");
+ detectorNames.push_back("MSER");
+ detectorNames.push_back("PyramidGFTT");
+ detectorNames.push_back("GFTT");
+ detectorNames.push_back("PyramidSimpleBlob");
+ detectorNames.push_back("SimpleBlob");
+ //In contrib modules
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ detectorNames.push_back("PyramidSTAR");
+ detectorNames.push_back("STAR");
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ detectorNames.push_back("PyramidAGAST");
+ detectorNames.push_back("AGAST");
+#endif
+ detectorNames.push_back("PyramidORB");
+ detectorNames.push_back("ORB");
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
+ detectorNames.push_back("PyramidBRISK");
+ detectorNames.push_back("BRISK");
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ detectorNames.push_back("PyramidKAZE");
+ detectorNames.push_back("KAZE");
+ detectorNames.push_back("PyramidAKAZE");
+ detectorNames.push_back("AKAZE");
+#endif
+
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ detectorNames.push_back("PyramidSIFT");
+ detectorNames.push_back("SIFT");
+ detectorNames.push_back("PyramidSURF");
+ detectorNames.push_back("SURF");
+#endif
+
+ for(std::vector<std::string>::const_iterator itd = detectorNames.begin(); itd != detectorNames.end(); ++itd) {
+ keyPoints.setDetector(*itd);
+
+ std::vector<cv::KeyPoint> kpts;
+ double elapsedTime;
+ keyPoints.detect(I, kpts, elapsedTime);
+ std::cout << "Nb keypoints detected: " << kpts.size() << " for " << *itd << " method." << std::endl;
+ if(kpts.empty()) {
+ std::cerr << "No keypoints detected with " << *itd << " and image: " << filename << "." << std::endl;
+ return -1;
+ }
+
+ if (opt_display) {
+ vpDisplay::display(I);
+
+ for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ vpImagePoint imPt;
+ imPt.set_uv(it->pt.x, it->pt.y);
+
+ vpDisplay::displayCross(I, imPt, 4, vpColor::red);
+ }
+
+ vpDisplay::flush(I);
+
+ if(opt_click_allowed) {
+ vpDisplay::getClick(I);
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint-5 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint-6.cpp b/modules/vision/test/key-point/testKeyPoint-6.cpp
new file mode 100644
index 0000000..a042b1b
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-6.cpp
@@ -0,0 +1,333 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test descriptor computation.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpKeyPoint.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoint descriptor extraction.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ Get the string OpenCV type.
+
+ \param type : OpenCV type.
+
+ \return The string OpenCV type.
+
+*/
+std::string getOpenCVType(const int type) {
+ std::string type_string = "";
+
+ switch(type) {
+ case CV_8U:
+ type_string = "CV_8U";
+ break;
+
+ case CV_8S:
+ type_string = "CV_8S";
+ break;
+
+ case CV_16U:
+ type_string = "CV_16U";
+ break;
+
+ case CV_16S:
+ type_string = "CV_16S";
+ break;
+
+ case CV_32S:
+ type_string = "CV_32S";
+ break;
+
+ case CV_32F:
+ type_string = "CV_32F";
+ break;
+
+ case CV_64F:
+ type_string = "CV_64F";
+ break;
+
+ default:
+ type_string = "Problem with type !";
+ break;
+ }
+
+ return type_string;
+}
+
+/*!
+ \example testKeyPoint-6.cpp
+
+ \brief Test descriptor extraction.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> I;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+
+ //Build the name of the image files
+ std::string filename = vpIoTools::createFilePath(dirname, "/Klimt.png");
+ vpImageIo::read(I, filename);
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+
+ if(opt_display) {
+ display.init(I, 0, 0, "KeyPoints detection.");
+ }
+
+ vpKeyPoint keyPoints;
+
+ std::vector<std::string> descriptorNames;
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ descriptorNames.push_back("SIFT");
+ descriptorNames.push_back("SURF");
+#endif
+ descriptorNames.push_back("ORB");
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
+ descriptorNames.push_back("BRISK");
+#endif
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D) || (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ descriptorNames.push_back("BRIEF");
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020402)
+ descriptorNames.push_back("FREAK");
+#endif
+#endif
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ descriptorNames.push_back("DAISY");
+ descriptorNames.push_back("LATCH");
+// descriptorNames.push_back("LUCID"); //Problem
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ descriptorNames.push_back("KAZE");
+ descriptorNames.push_back("AKAZE");
+#endif
+
+ std::string detectorName = "FAST";
+ keyPoints.setDetector(detectorName);
+ std::vector<cv::KeyPoint> kpts;
+ double elapsedTime;
+ keyPoints.detect(I, kpts, elapsedTime);
+ std::cout << "Nb keypoints detected: " << kpts.size() << " for " << detectorName << " method." << std::endl;
+ if(kpts.empty()) {
+ std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl;
+ return -1;
+ }
+
+ for(std::vector<std::string>::const_iterator itd = descriptorNames.begin(); itd != descriptorNames.end(); ++itd) {
+ if(*itd == "KAZE") {
+ detectorName = "KAZE";
+ keyPoints.setDetector(detectorName);
+ keyPoints.detect(I, kpts, elapsedTime);
+ std::cout << "Nb keypoints detected: " << kpts.size() << " for " << detectorName << " method." << std::endl;
+ if(kpts.empty()) {
+ std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl;
+ return -1;
+ }
+ } else if(*itd == "AKAZE") {
+ detectorName = "AKAZE";
+ keyPoints.setDetector(detectorName);
+ keyPoints.detect(I, kpts, elapsedTime);
+ std::cout << "Nb keypoints detected: " << kpts.size() << " for " << detectorName << " method." << std::endl;
+ if(kpts.empty()) {
+ std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl;
+ return -1;
+ }
+ }
+
+ keyPoints.setExtractor(*itd);
+
+ double t = vpTime::measureTimeMs();
+ cv::Mat descriptor;
+ keyPoints.extract(I, kpts, descriptor, elapsedTime);
+ t = vpTime::measureTimeMs() - t;
+
+ std::cout << "Descriptor: " << descriptor.rows << "x" << descriptor.cols << " (rows x cols) ; type=" <<
+ getOpenCVType(descriptor.type()) << " for " << *itd << " method in " << t << " ms." << std::endl;
+ if(descriptor.empty()) {
+ std::cerr << "No descriptor extracted with " << *itd << " and image:" << filename << "." << std::endl;
+ return -1;
+ }
+
+ if (opt_display) {
+ vpDisplay::display(I);
+
+ for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ vpImagePoint imPt;
+ imPt.set_uv(it->pt.x, it->pt.y);
+
+ vpDisplay::displayCross(I, imPt, 4, vpColor::red);
+ }
+
+ vpDisplay::flush(I);
+
+ if(opt_click_allowed) {
+ vpDisplay::getClick(I);
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint-6 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint-7.cpp b/modules/vision/test/key-point/testKeyPoint-7.cpp
new file mode 100644
index 0000000..f9df3f1
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint-7.cpp
@@ -0,0 +1,684 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test saving / loading learning files for vpKeyPoint class.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+#include <iomanip>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpException.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdo:h"
+
+void usage(const char *name, const char *badparam, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &opath, std::string user);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test save / load learning files for vpKeyPoint class.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ learning files will be written.\n\
+\n\
+ -h\n\
+ Print the help.\n",
+ opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': break; //not used, to avoid error with default arguments ctest
+ case 'd': break; //not used, to avoid error with default arguments ctest
+ case 'o': opath = optarg_; break;
+ case 'h': usage(argv[0], NULL, opath, user); return false; break;
+
+ default:
+ usage(argv[0], optarg_, opath, user); return false; break;
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ Compare two vectors of cv::KeyPoint.
+
+ \param keypoints1 : First vectors of cv::KeyPoint.
+ \param keypoints2 : Second vectors of cv::KeyPoint.
+
+ \return True if the two vectors are identical, false otherwise.
+*/
+bool compareKeyPoints(const std::vector<cv::KeyPoint> &keypoints1, const std::vector<cv::KeyPoint> &keypoints2) {
+ if(keypoints1.size() != keypoints2.size()) {
+ return false;
+ }
+
+ for(size_t cpt = 0; cpt < keypoints1.size(); cpt++) {
+ if(!vpMath::equal(keypoints1[cpt].angle, keypoints2[cpt].angle, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].angle=" << keypoints1[cpt].angle <<
+ " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle << std::endl;
+ return false;
+ }
+
+ if(keypoints1[cpt].class_id != keypoints2[cpt].class_id) {
+ std::cerr << "keypoints1[cpt].class_id=" << keypoints1[cpt].class_id << " ; keypoints2[cpt].class_id=" <<
+ keypoints2[cpt].class_id << std::endl;
+ return false;
+ }
+
+ if(keypoints1[cpt].octave != keypoints2[cpt].octave) {
+ std::cerr << "keypoints1[cpt].octave=" << keypoints1[cpt].octave << " ; keypoints2[cpt].octave=" <<
+ keypoints2[cpt].octave << std::endl;
+ return false;
+ }
+
+ if(!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.x=" << keypoints1[cpt].pt.x <<
+ " ; keypoints2[cpt].pt.x=" << keypoints2[cpt].pt.x << std::endl;
+ return false;
+ }
+
+ if(!vpMath::equal(keypoints1[cpt].pt.y, keypoints2[cpt].pt.y, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.y=" << keypoints1[cpt].pt.y <<
+ " ; keypoints2[cpt].pt.y=" << keypoints2[cpt].pt.y << std::endl;
+ return false;
+ }
+
+ if(!vpMath::equal(keypoints1[cpt].response, keypoints2[cpt].response, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].response=" << keypoints1[cpt].response <<
+ " ; keypoints2[cpt].response=" << keypoints2[cpt].response << std::endl;
+ return false;
+ }
+
+ if(!vpMath::equal(keypoints1[cpt].size, keypoints2[cpt].size, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].size=" << keypoints1[cpt].size <<
+ " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl;
+ return false;
+ }
+ }
+
+ return true;
+}
+
+/*!
+ Compare two descriptors.
+
+ \param descriptors1 : First descriptor.
+ \param descriptors2 : Second descriptor.
+
+ \return True if the two vectors are identical, false otherwise.
+*/
+bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2) {
+ if(descriptors1.rows != descriptors2.rows || descriptors1.cols != descriptors2.cols ||
+ descriptors1.type() != descriptors2.type()) {
+ return false;
+ }
+
+ for(int i = 0; i < descriptors1.rows; i++) {
+ for(int j = 0; j < descriptors1.cols; j++) {
+ switch(descriptors1.type()) {
+ case CV_8U:
+ if(descriptors1.at<unsigned char>(i,j) != descriptors2.at<unsigned char>(i,j)) {
+ std::cerr << "descriptors1.at<unsigned char>(i,j)=" << descriptors1.at<unsigned char>(i,j) <<
+ " ; descriptors2.at<unsigned char>(i,j)=" << descriptors2.at<unsigned char>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_8S:
+ if(descriptors1.at<char>(i,j) != descriptors2.at<char>(i,j)) {
+ std::cerr << "descriptors1.at<char>(i,j)=" << descriptors1.at<char>(i,j) <<
+ " ; descriptors2.at<char>(i,j)=" << descriptors2.at<char>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_16U:
+ if(descriptors1.at<unsigned short>(i,j) != descriptors2.at<unsigned short>(i,j)) {
+ std::cerr << "descriptors1.at<unsigned short>(i,j)=" << descriptors1.at<unsigned short>(i,j) <<
+ " ; descriptors2.at<unsigned short>(i,j)=" << descriptors2.at<unsigned short>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_16S:
+ if(descriptors1.at<short>(i,j) != descriptors2.at<short>(i,j)) {
+ std::cerr << "descriptors1.at<short>(i,j)=" << descriptors1.at<short>(i,j) <<
+ " ; descriptors2.at<short>(i,j)=" << descriptors2.at<short>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_32S:
+ if(descriptors1.at<int>(i,j) != descriptors2.at<int>(i,j)) {
+ std::cerr << "descriptors1.at<int>(i,j)=" << descriptors1.at<int>(i,j) <<
+ " ; descriptors2.at<int>(i,j)=" << descriptors2.at<int>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_32F:
+ if(!vpMath::equal(descriptors1.at<float>(i,j), descriptors2.at<float>(i,j), std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "descriptors1.at<float>(i,j)=" << descriptors1.at<float>(i,j)
+ << " ; descriptors2.at<float>(i,j)=" << descriptors2.at<float>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ case CV_64F:
+ if(!vpMath::equal(descriptors1.at<double>(i,j), descriptors2.at<double>(i,j), std::numeric_limits<double>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(17) << "descriptors1.at<double>(i,j)=" << descriptors1.at<double>(i,j)
+ << " ; descriptors2.at<double>(i,j)=" << descriptors2.at<double>(i,j) << std::endl;
+ return false;
+ }
+ break;
+
+ default:
+ return false;
+ break;
+ }
+ }
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint-7.cpp
+
+ \brief Test saving / loading learning file.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ std::string opt_opath;
+ std::string username;
+ std::string opath;
+ std::string filename;
+
+ //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ throw vpException(vpException::ioError, "Please set the VISP_INPUT_IMAGE_PATH environment variable value.");
+ }
+
+ // Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_opath, username) == false) {
+ throw vpException(vpException::fatalError, "getOptions(argc, argv, opt_opath, username) == false");
+ }
+
+ // Get the option values
+ if (!opt_opath.empty()) {
+ opath = opt_opath;
+ }
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ }
+ catch (...) {
+ usage(argv[0], NULL, opt_opath, username);
+ std::stringstream ss;
+ ss << std::endl << "ERROR:" << std::endl;
+ ss << " Cannot create " << opath << std::endl;
+ ss << " Check your -o " << opt_opath << " option " << std::endl;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+ }
+
+ vpImage<unsigned char> I;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+
+ //Build the name of the image files
+ std::string img_filename = vpIoTools::createFilePath(dirname, "/Klimt.ppm");
+ vpImageIo::read(I, img_filename);
+
+ vpKeyPoint keyPoints;
+
+ //Test with binary descriptor
+ {
+ std::string keypointName = "ORB";
+ keyPoints.setDetector(keypointName);
+ keyPoints.setExtractor(keypointName);
+
+ keyPoints.buildReference(I);
+
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ keyPoints.getTrainKeyPoints(trainKeyPoints);
+ cv::Mat trainDescriptors = keyPoints.getTrainDescriptors();
+ if(trainKeyPoints.empty() || trainDescriptors.empty() || (int) trainKeyPoints.size() != trainDescriptors.rows) {
+ throw vpException(vpException::fatalError, "Problem when detecting keypoints or when computing descriptors !");
+ }
+
+ //Save in binary with training images
+ filename = vpIoTools::createFilePath(opath, "bin_with_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin");
+ keyPoints.saveLearningData(filename, true, true);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint1;
+ read_keypoint1.loadLearningData(filename, true);
+ std::vector<cv::KeyPoint> trainKeyPoints_read;
+ read_keypoint1.getTrainKeyPoints(trainKeyPoints_read);
+ cv::Mat trainDescriptors_read = read_keypoint1.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved "
+ "in binary with train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "binary with train images saved !");
+ }
+
+
+ //Save in binary with no training images
+ filename = vpIoTools::createFilePath(opath, "bin_without_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin");
+ keyPoints.saveLearningData(filename, true, false);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint2;
+ read_keypoint2.loadLearningData(filename, true);
+ trainKeyPoints_read.clear();
+ read_keypoint2.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint2.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "binary without train images !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "binary without train images !");
+ }
+
+
+#if defined(VISP_HAVE_XML2)
+ //Save in xml with training images
+ filename = vpIoTools::createFilePath(opath, "xml_with_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml");
+ keyPoints.saveLearningData(filename, false, true);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint3;
+ read_keypoint3.loadLearningData(filename, false);
+ trainKeyPoints_read.clear();
+ read_keypoint3.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint3.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "xml with train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "xml with train images saved !");
+ }
+
+
+ //Save in xml without training images
+ filename = vpIoTools::createFilePath(opath, "xml_without_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml");
+ keyPoints.saveLearningData(filename, false, false);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint4;
+ read_keypoint4.loadLearningData(filename, false);
+ trainKeyPoints_read.clear();
+ read_keypoint4.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint4.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "xml without train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "xml without train images saved !");
+ }
+#endif
+
+ std::cout << "Saving / loading learning files with binary descriptor are ok !" << std::endl;
+ }
+
+
+ //Test with floating point descriptor
+#if defined(VISP_HAVE_OPENCV_NONFREE) || ( (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(VISP_HAVE_OPENCV_XFEATURES2D) )
+ {
+ std::string keypointName = "SIFT";
+ keyPoints.setDetector(keypointName);
+ keyPoints.setExtractor(keypointName);
+
+ keyPoints.buildReference(I);
+
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ keyPoints.getTrainKeyPoints(trainKeyPoints);
+ cv::Mat trainDescriptors = keyPoints.getTrainDescriptors();
+ if(trainKeyPoints.empty() || trainDescriptors.empty() || (int) trainKeyPoints.size() != trainDescriptors.rows) {
+ throw vpException(vpException::fatalError, "Problem when detecting keypoints or when computing descriptors (SIFT) !");
+ }
+
+ //Save in binary with training images
+ filename = vpIoTools::createFilePath(opath, "bin_with_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin");
+ keyPoints.saveLearningData(filename, true, true);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint1;
+ read_keypoint1.loadLearningData(filename, true);
+ std::vector<cv::KeyPoint> trainKeyPoints_read;
+ read_keypoint1.getTrainKeyPoints(trainKeyPoints_read);
+ cv::Mat trainDescriptors_read = read_keypoint1.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "binary with train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "binary with train images saved !");
+ }
+
+
+ //Save in binary with no training images
+ filename = vpIoTools::createFilePath(opath, "bin_without_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin");
+ keyPoints.saveLearningData(filename, true, false);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint2;
+ read_keypoint2.loadLearningData(filename, true);
+ trainKeyPoints_read.clear();
+ read_keypoint2.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint2.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "binary without train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "binary without train images saved !");
+ }
+
+
+#if defined(VISP_HAVE_XML2)
+ //Save in xml with training images
+ filename = vpIoTools::createFilePath(opath, "xml_with_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml");
+ keyPoints.saveLearningData(filename, false, true);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint3;
+ read_keypoint3.loadLearningData(filename, false);
+ trainKeyPoints_read.clear();
+ read_keypoint3.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint3.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "xml with train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "xml with train images saved !");
+ }
+
+
+ //Save in xml without training images
+ filename = vpIoTools::createFilePath(opath, "xml_without_img");
+ vpIoTools::makeDirectory(filename);
+ filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml");
+ keyPoints.saveLearningData(filename, false, false);
+
+ //Test if save is ok
+ if(!vpIoTools::checkFilename(filename)) {
+ std::stringstream ss;
+ ss << "Problem when saving file=" << filename;
+ throw vpException(vpException::ioError, ss.str().c_str());
+ }
+
+ //Test if read is ok
+ vpKeyPoint read_keypoint4;
+ read_keypoint4.loadLearningData(filename, false);
+ trainKeyPoints_read.clear();
+ read_keypoint4.getTrainKeyPoints(trainKeyPoints_read);
+ trainDescriptors_read = read_keypoint4.getTrainDescriptors();
+
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
+ "xml without train images saved !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
+ "xml without train images saved !");
+ }
+#endif
+
+ std::cout << "Saving / loading learning files with floating point descriptor are ok !" << std::endl;
+
+
+ //Test vpKeyPoint::reset()
+ vpKeyPoint keypoint_reset;
+
+ keypointName = "ORB";
+ keypoint_reset.setDetector(keypointName);
+ keypoint_reset.setExtractor(keypointName);
+
+ keypoint_reset.buildReference(I);
+
+ //reset
+ keypoint_reset.reset();
+
+ keypointName = "SIFT";
+ keypoint_reset.setDetector(keypointName);
+ keypoint_reset.setExtractor(keypointName);
+
+ keypoint_reset.buildReference(I);
+
+ std::vector<cv::KeyPoint> trainKeyPoints_reset;
+ keypoint_reset.getTrainKeyPoints(trainKeyPoints_reset);
+ cv::Mat trainDescriptors_reset = keypoint_reset.getTrainDescriptors();
+
+ //If reset is ok, we should get the same keypoints and the same descriptors
+ if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_reset)) {
+ throw vpException(vpException::fatalError, "Problem with vpKeyPoint::reset() and trainKeyPoints !");
+ }
+
+ if(!compareDescriptors(trainDescriptors, trainDescriptors_reset)) {
+ throw vpException(vpException::fatalError, "Problem with vpKeyPoint::reset() and trainDescriptors !");
+ }
+
+ std::cout << "vpKeyPoint::reset() is ok with trainKeyPoints and trainDescriptors !" << std::endl;
+ }
+#endif
+
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "Saving / loading learning files are ok !" << std::endl;
+ std::cout << "testKeyPoint-7 is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testKeyPoint.cpp b/modules/vision/test/key-point/testKeyPoint.cpp
new file mode 100644
index 0000000..775d707
--- /dev/null
+++ b/modules/vision/test/key-point/testKeyPoint.cpp
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test keypoint matching.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+*/
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test keypoints matching.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n");
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'h': usage(argv[0], NULL); return false; break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+/*!
+ \example testKeyPoint.cpp
+
+ \brief Test keypoint matching.
+*/
+int main(int argc, const char ** argv) {
+ try {
+ std::string env_ipath;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ if(env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ return -1;
+ }
+
+ vpImage<unsigned char> Iref, Icur, Imatch;
+
+ //Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+
+ //Build the name of the image files
+ std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
+ vpImageIo::read(Iref, filenameRef);
+ std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
+
+ //Init keypoints
+ vpKeyPoint keypoints("ORB", "ORB", "BruteForce-Hamming");
+ std::cout << "Build " << keypoints.buildReference(Iref) << " reference points." << std::endl;
+
+ vpVideoReader g;
+ g.setFileName(filenameCur);
+ g.open(Icur);
+ g.acquire(Icur);
+
+ Imatch.resize(Icur.getHeight(), 2*Icur.getWidth());
+ Imatch.insert(Iref, vpImagePoint(0,0));
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+
+ if (opt_display) {
+ display.init(Imatch, 0, 0, "ORB keypoints matching");
+ }
+
+ bool opt_click = false;
+ vpMouseButton::vpMouseButtonType button;
+ while(!g.end()) {
+ g.acquire(Icur);
+ Imatch.insert(Icur, vpImagePoint(0, Icur.getWidth()));
+
+ if(opt_display) {
+ vpDisplay::display(Imatch);
+ }
+
+ //Match keypoints
+ keypoints.matchPoint(Icur);
+ //Display image with keypoints matched
+ keypoints.displayMatching(Iref, Imatch);
+
+ if(opt_display) {
+ vpDisplay::flush(Imatch);
+ }
+
+ //Click requested to process next image
+ if (opt_click_allowed && opt_display) {
+ if(opt_click) {
+ vpDisplay::getClick(Imatch, button, true);
+ if(button == vpMouseButton::button3) {
+ opt_click = false;
+ }
+ } else {
+ //Use right click to enable/disable step by step tracking
+ if(vpDisplay::getClick(Imatch, button, false)) {
+ if (button == vpMouseButton::button3) {
+ opt_click = true;
+ }
+ else if(button == vpMouseButton::button1) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ } catch(vpException &e) {
+ std::cerr << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "testKeyPoint is ok !" << std::endl;
+ return 0;
+}
+#else
+int main() {
+ std::cerr << "You need OpenCV library." << std::endl;
+
+ return 0;
+}
+
+#endif
diff --git a/modules/vision/test/key-point/testSurfKeyPoint.cpp b/modules/vision/test/key-point/testSurfKeyPoint.cpp
new file mode 100644
index 0000000..bb0477d
--- /dev/null
+++ b/modules/vision/test/key-point/testSurfKeyPoint.cpp
@@ -0,0 +1,376 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Test auto detection of dots.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sstream>
+#include <iomanip>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if (defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) // Require opencv >= 1.1.0 < 3.0.0
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/vision/vpKeyPointSurf.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testSurfKeyPoint.cpp
+
+ \brief Test matching points thanks to the surf key points.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:h"
+
+void usage(const char *name, const char *badparam, std::string ipath);
+bool getOptions(int argc, const char **argv, std::string &ipath,
+ bool &click_allowed, bool &display);
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+
+*/
+void usage(const char *name, const char *badparam, std::string ipath)
+{
+ fprintf(stdout, "\n\
+Test dot tracking.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read image \n\
+ \"ViSP-images/ellipse/ellipse.pgm\"\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -h\n\
+ Print the help.\n",
+ ipath.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+
+}
+/*!
+
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath : Input image path.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath,
+ bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'c': click_allowed = false; break;
+ case 'd': display = false; break;
+ case 'i': ipath = optarg_; break;
+ case 'h': usage(argv[0], NULL, ipath); return false; break;
+
+ default:
+ usage(argv[0], optarg_, ipath);
+ return false; break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+
+int
+main(int argc, const char ** argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string dirname;
+ std::string filenameRef;
+ std::string filenameCur;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (! env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath,
+ opt_click_allowed, opt_display) == false) {
+ exit (-1);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl
+ << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()){
+ usage(argv[0], NULL, ipath);
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
+ << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl << std::endl;
+ exit(-1);
+ }
+
+
+ // 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<unsigned char> Iref ;
+ vpImage<unsigned char> Icur ;
+
+ // Set the path location of the image sequence
+ dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+
+ // Build the name of the image file
+ filenameRef = vpIoTools::createFilePath(dirname, "image.0000.pgm");
+ filenameCur = vpIoTools::createFilePath(dirname, "image.0079.pgm");
+
+ // Read the PGM image named "filename" on the disk, and put the
+ // bitmap into the image structure I. I is initialized to the
+ // correct size
+ //
+ // exception readPGM may throw various exception if, for example,
+ // the file does not exist, or if the memory cannot be allocated
+ try{
+ std::cout << "Load: " << filenameRef << std::endl;
+
+ vpImageIo::read(Iref, filenameRef) ;
+
+ std::cout << "Load: " << filenameCur << std::endl;
+
+ vpImageIo::read(Icur, filenameCur) ;
+ }
+ catch(...)
+ {
+ // an exception is throwned if an exception from readPGM has been catched
+ // here this will result in the end of the program
+ // Note that another error message has been printed from readPGM
+ // to give more information about the error
+ std::cerr << std::endl
+ << "ERROR:" << std::endl;
+ std::cerr << " Cannot read " << filenameRef << "or" << filenameCur <<std::endl;
+ std::cerr << " Check your -i " << ipath << " option " << std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable."
+ << std::endl;
+ exit(-1);
+ }
+
+ // We open a window using either X11, GTK or GDI.
+#if defined VISP_HAVE_X11
+ vpDisplayX display[2];
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display[2];
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display[2];
+#else
+ vpDisplayOpenCV display[2];
+#endif
+
+ if (opt_display) {
+ // Display size is automatically defined by the image (I) size
+ display[0].init(Iref, 100, 100, "Reference image") ;
+ // Display the image
+ // The image class has a member that specify a pointer toward
+ // the display that has been initialized in the display declaration
+ // therefore is is no longuer necessary to make a reference to the
+ // display variable.
+ vpDisplay::display(Iref) ;
+ //Flush the display
+ vpDisplay::flush(Iref) ;
+ }
+
+ vpKeyPointSurf surf;
+ unsigned int nbrRef;
+
+ if (opt_click_allowed && opt_display)
+ {
+ std::cout << "Select a part of the image where the reference points will be computed. This part is a rectangle." << std::endl;
+ std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Iref, corners[i]);
+ }
+
+ vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::red);
+ vpDisplay::flush(Iref);
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+
+ //Computes the reference points
+ nbrRef = surf.buildReference(Iref, corners[0], height, width);
+ }
+
+ else
+ {
+ nbrRef = surf.buildReference(Iref);
+ }
+
+ if(nbrRef < 1)
+ {
+ std::cerr << "No reference point" << std::endl;
+ exit(-1);
+ }
+
+ unsigned int nbrPair;
+ if (opt_display) {
+ display[1].init(Icur, (int)(100+Iref.getWidth()), 100, "Current image") ;
+ // display variable.
+ vpDisplay::display(Icur) ;
+ //Flush the display
+ vpDisplay::flush(Icur) ;
+ }
+
+ if (opt_click_allowed && opt_display)
+ {
+ std::cout << "Select a part of the current image where the reference will be search. This part is a rectangle." << std::endl;
+ std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
+ vpImagePoint corners[2];
+ for (int i=0 ; i < 2 ; i++)
+ {
+ vpDisplay::getClick(Icur, corners[i]);
+ }
+ vpDisplay::displayRectangle(Icur, corners[0], corners[1], vpColor::green);
+ vpDisplay::flush(Icur);
+ unsigned int height, width;
+ height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
+ width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
+
+ //Computes the reference points
+ nbrPair = surf.matchPoint(Icur, corners[0], height, width);
+ }
+
+ else
+ {
+ nbrPair = surf.matchPoint(Icur);
+ }
+
+ if(nbrPair < 1)
+ {
+ std::cout << "No point matched" << std::endl;
+ }
+
+ if (opt_display)
+ {
+ surf.display(Iref, Icur, 7);
+ vpDisplay::flush(Iref) ;
+ vpDisplay::flush(Icur) ;
+ if (opt_click_allowed)
+ {
+ std::cout << "A click on the reference image to exit..." << std::endl;
+ vpDisplay::getClick(Iref);
+ }
+ }
+ return (0);
+ }
+ catch(vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return (1);
+ }
+}
+#else
+int
+main()
+{
+ std::cerr << "You do not have 1.1.0 <= OpenCV < 2.3.0 that contains opencv_nonfree component..." << std::endl;
+}
+
+#endif
diff --git a/modules/vision/test/pose/testFindMatch.cpp b/modules/vision/test/pose/testFindMatch.cpp
new file mode 100644
index 0000000..86b500e
--- /dev/null
+++ b/modules/vision/test/pose/testFindMatch.cpp
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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 <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#define L 0.1
+
+
+/*!
+ \example testFindMatch.cpp
+
+ Find Matches using Ransac.
+
+*/
+
+int
+main()
+{
+ try {
+ std::cout << "Find Matches using Ransac" << std::endl;
+ std::vector<vpPoint> P;
+
+ P.push_back( vpPoint(-L,-L, 0 ) );
+ P.push_back( vpPoint(L,-L, 0 ) );
+ P.push_back( vpPoint(L,L, 0 ) );
+ P.push_back( vpPoint(-L,L, 0 ) );
+ P.push_back( vpPoint(-0,L/2., L ) );
+
+ vpHomogeneousMatrix cMo_ref(0, 0.2, 1, vpMath::rad(3), vpMath::rad(-2), vpMath::rad(10)) ;
+
+ std::vector<vpPoint> p( P.size() );
+ for(unsigned int i=0 ; i < P.size() ; i++)
+ {
+ vpPoint pt = P[i];
+ pt.project(cMo_ref);
+ p[i].set_x(pt.get_x());
+ p[i].set_y(pt.get_y());
+ }
+
+ unsigned int ninliers ;
+ std::vector<vpPoint> inliers;
+ double threshold = 1e-6;
+ unsigned int nbInlierToReachConsensus = (unsigned int)(P.size());
+
+ vpHomogeneousMatrix cMo ;
+
+ vpPose::findMatch(p,P,nbInlierToReachConsensus,threshold,ninliers,inliers,cMo);
+
+ std::cout << "Inliers: " << std::endl;
+ for (unsigned int i = 0; i < inliers.size() ; i++)
+ {
+ inliers[i].print() ;
+ std::cout << std::endl;
+ }
+
+ std::cout << "cMo :\n" << vpPoseVector(cMo).t() << std::endl << std::endl;
+
+ vpPoseVector pose_ref = vpPoseVector(cMo_ref);
+ vpPoseVector pose_est = vpPoseVector(cMo);
+
+ std::cout << std::endl;
+ std::cout << "reference cMo :\n" << pose_ref.t() << std::endl << std::endl;
+ std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
+
+ int test_fail = 0;
+ for(unsigned int i=0; i<6; i++) {
+ if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ test_fail = 1;
+ }
+
+ std::cout << "Matching is " << (test_fail ? "badly" : "well") << " performed" << std::endl;
+
+ return test_fail;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vision/test/pose/testPose.cpp b/modules/vision/test/pose/testPose.cpp
new file mode 100644
index 0000000..00db7f7
--- /dev/null
+++ b/modules/vision/test/pose/testPose.cpp
@@ -0,0 +1,207 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Compute the pose of a 3D object using the Dementhon, Lagrange and
+ * Non-Linear approach.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDebug.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#define L 0.035
+
+/*!
+ \example testPose.cpp
+
+ Compute the pose of a 3D object using the Dementhon, Lagrange and
+ Non-Linear approach.
+
+*/
+
+void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
+int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
+ const std::string &legend);
+
+// print the resulting estimated pose
+void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
+{
+ vpPoseVector cpo = vpPoseVector(cMo);
+
+ std::cout << std::endl << legend << "\n "
+ << "tx = " << cpo[0] << "\n "
+ << "ty = " << cpo[1] << "\n "
+ << "tz = " << cpo[2] << "\n "
+ << "tux = vpMath::rad(" << vpMath::deg(cpo[3]) << ")\n "
+ << "tuy = vpMath::rad(" << vpMath::deg(cpo[4]) << ")\n "
+ << "tuz = vpMath::rad(" << vpMath::deg(cpo[5]) << ")\n"
+ << std::endl;
+}
+
+// test if pose is well estimated
+int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
+ const std::string &legend)
+{
+ vpPoseVector pose_ref = vpPoseVector(cMo_ref);
+ vpPoseVector pose_est = vpPoseVector(cMo_est);
+
+ int fail = 0;
+
+ // Test done on the 3D pose
+ for(unsigned int i=0; i<6; i++) {
+ if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ fail = 1;
+ }
+
+ std::cout << "Based on 3D parameters " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
+
+ // Test done on the residual
+ double r = pose.computeResidual(cMo_est);
+ if (pose.listP.size() < 4) {
+ fail = 1;
+ std::cout << "Not enough point" << std::endl;
+ return fail;
+ }
+ r = sqrt(r)/pose.listP.size();
+ //std::cout << "Residual on each point (meter): " << r << std::endl;
+ fail = (r > 0.1) ? 1 : 0;
+ std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
+ return fail;
+}
+
+int main()
+{
+ try {
+ vpPoint P[5] ; // Point to be tracked
+ vpPose pose ;
+ pose.clearPoint() ;
+
+ P[0].setWorldCoordinates(-L,-L, 0 ) ;
+ P[1].setWorldCoordinates(L,-L, 0 ) ;
+ P[2].setWorldCoordinates(L,L, 0 ) ;
+ P[3].setWorldCoordinates(-2*L, 3*L, 0 ) ;
+ P[4].setWorldCoordinates(-L,L, 0.01 ) ;
+
+ int test_fail = 0, fail = 0;
+ vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0,vpMath::rad(10));
+ vpHomogeneousMatrix cMo_ref(cpo_ref) ;
+ vpHomogeneousMatrix cMo ; // will contain the estimated pose
+
+ for(int i=0 ; i < 5 ; i++) {
+ P[i].project(cMo_ref) ;
+ //P[i].print();
+ pose.addPoint(P[i]) ; // and added to the pose computation class
+ }
+
+ // Let's go ...
+ print_pose(cMo_ref, std::string("Reference pose")); // print the reference pose
+
+ std::cout <<"-------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::LAGRANGE, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Lagrange"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
+ test_fail |= fail;
+
+ std::cout <<"--------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::DEMENTHON, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Dementhon"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
+ test_fail |= fail;
+
+ std::cout <<"--------------------------------------------------"<<std::endl ;
+ pose.setRansacNbInliersToReachConsensus(4);
+ pose.setRansacThreshold(0.01);
+ pose.computePose(vpPose::RANSAC, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Ransac"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
+ test_fail |= fail;
+
+ std::cout <<"--------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::LAGRANGE_LOWE, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Lagrange than Lowe"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than Lowe");
+ test_fail |= fail;
+
+ std::cout <<"--------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::DEMENTHON_LOWE, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Dementhon than Lowe"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than Lowe");
+ test_fail |= fail;
+
+ // Now Virtual Visual servoing
+
+ std::cout <<"--------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by VVS"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
+ test_fail |= fail;
+
+ std::cout <<"-------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Dementhon than by VVS"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than by VVS");
+ test_fail |= fail;
+
+ std::cout <<"-------------------------------------------------"<<std::endl ;
+ pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
+
+ print_pose(cMo, std::string("Pose estimated by Lagrange than by VVS"));
+ fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than by VVS");
+ test_fail |= fail;
+
+ std::cout << "\nGlobal pose estimation test " << (test_fail ? "fail" : "is ok") << std::endl;
+
+ return test_fail;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vision/test/pose/testPoseFeatures.cpp b/modules/vision/test/pose/testPoseFeatures.cpp
new file mode 100644
index 0000000..b273caa
--- /dev/null
+++ b/modules/vision/test/pose/testPoseFeatures.cpp
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Compute the pose from visual features by virtual visual servoing.
+ *
+ * Authors:
+ * Aurelien Yol
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/vision/vpPoseFeatures.h>
+#include <visp3/vision/vpPose.h>
+#include <iostream>
+#include <vector>
+#include <visp3/vision/vpPose.h>
+#include <limits>
+
+/*!
+ \example testPoseFeatures.cpp
+
+ Compute the pose from different features.
+
+*/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+class vp_createPointClass{
+public:
+ int value;
+
+ vp_createPointClass() : value(0){}
+
+ int vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
+ value += 1;
+ vpFeatureBuilder::create(fp,v);
+ return value;
+ }
+};
+
+void vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
+ vpFeatureBuilder::create(fp,v);
+}
+
+void vp_createLine(vpFeatureLine &fp,const vpLine &v){
+ vpFeatureBuilder::create(fp,v);
+}
+#endif
+#endif
+
+int main()
+{
+ try {
+ vpImage<unsigned char> I(600,600);
+
+ vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
+ vpPoseVector pose_ref = vpPoseVector(cMo_ref);
+
+ std::cout << "Reference pose used to create the visual features : " << std::endl;
+ std::cout << pose_ref.t() << std::endl;
+
+ vpPoseFeatures pose;
+
+ std::vector<vpPoint> pts;
+
+ double val = 0.25;
+ double val2 = 0.0;
+
+ //2D Point Feature
+ pts.push_back( vpPoint(0.0,-val,val2) );
+ pts.push_back( vpPoint(0.0,val,val2) );
+ pts.push_back( vpPoint(-val,val,val2) );
+
+ //Segment Feature
+ pts.push_back( vpPoint(-val,-val/2.0,val2) );
+ pts.push_back( vpPoint(val,val/2.0,val2) );
+
+ //3D point Feature
+ pts.push_back( vpPoint(0.0,0.0,-1.5) );
+
+ //Line Feature
+ vpLine line;
+ line.setWorldCoordinates(0.0,1.0,0.0,.0,
+ 0.0,0.0,1.0,0.0);
+
+ //Vanishing Point Feature
+ vpLine l1;
+ l1.setWorldCoordinates(0.0,1.0,0.2,0.0,
+ 1.0,0.0,0.0,-0.25);
+
+ vpLine l2;
+ l2.setWorldCoordinates(0.0,1.0,0.2,0.0,
+ -1.0,0.0,0.0,-0.25);
+
+ //Ellipse Feature
+ vpCircle circle;
+ circle.setWorldCoordinates(0.0, 0.0, 1.0 , 0.0, 0.0, 0.0, 0.25);
+
+ pts[0].project(cMo_ref);
+ pts[1].project(cMo_ref);
+ pts[2].project(cMo_ref);
+
+ pts[3].project(cMo_ref);
+ pts[4].project(cMo_ref);
+
+ pts[5].project(cMo_ref);
+
+ line.project(cMo_ref);
+
+ l1.project(cMo_ref);
+ l2.project(cMo_ref);
+
+ circle.project(cMo_ref);
+
+ pose.addFeaturePoint(pts[0]);
+ // pose.addFeaturePoint(pts[1]);
+ pose.addFeaturePoint(pts[2]);
+
+ pose.addFeaturePoint3D(pts[5]);
+
+ pose.addFeatureVanishingPoint(l1,l2);
+
+ // pose.addFeatureSegment(pts[3],pts[4]);
+ //
+ // pose.addFeatureLine(line);
+
+ pose.addFeatureEllipse(circle);
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ vpFeaturePoint fp;
+ vpFeatureLine fl;
+ vpFeatureSegment fs;
+ void (*ptr)(vpFeatureSegment&, vpPoint&, vpPoint&) = &vpFeatureBuilder::create;
+ vp_createPointClass cpClass;
+ int (vp_createPointClass::*ptrClass)(vpFeaturePoint&, const vpPoint&) = &vp_createPointClass::vp_createPoint;
+ pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
+ pose.addSpecificFeature(&vp_createLine, fl, line);
+ pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
+#endif
+
+ pose.setVerbose(true);
+ pose.setLambda(0.6);
+ pose.setVVSIterMax(200);
+ pose.setCovarianceComputation(true);
+
+ vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
+ vpPoseVector pose_est = vpPoseVector(cMo_est);
+ std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
+ std::cout << pose_est.t() << std::endl;
+
+ pose.computePose(cMo_est);
+ // pose.computePose(cMo_est, vpPoseFeatures::ROBUST_VIRTUAL_VS);
+
+
+ std::cout << "\nEstimated pose from visual features : " << std::endl;
+ pose_est.buildFrom(cMo_est);
+ std::cout << pose_est.t() << std::endl;
+
+ std::cout << "\nResulting covariance (Diag): " << std::endl;
+ vpMatrix covariance = pose.getCovarianceMatrix();
+ std::cout << covariance[0][0] << " " << covariance[1][1] << " "
+ << covariance[2][2] << " " << covariance[3][3] << " "
+ << covariance[4][4] << " " << covariance[5][5] << " " << std::endl;
+
+ int test_fail = 0;
+ for(unsigned int i=0; i<6; i++) {
+ if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ test_fail = 1;
+ }
+
+ std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
+
+ return test_fail;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vision/test/pose/testPoseRansac.cpp b/modules/vision/test/pose/testPoseRansac.cpp
new file mode 100644
index 0000000..fc1537c
--- /dev/null
+++ b/modules/vision/test/pose/testPoseRansac.cpp
@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 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 <visp3/vision/vpPose.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#define L 0.1
+
+
+/*!
+ \example testPoseRansac.cpp
+
+ Compute the pose of a 3D object using the Ransac method.
+
+*/
+
+int
+main()
+{
+ try {
+ std::cout << "Pose computation with matched points" << std::endl;
+ std::vector<vpPoint> P; // Point to be tracked
+
+ P.push_back( vpPoint(-L,-L, 0 ) );
+ P.push_back( vpPoint(L,-L, 0 ) );
+ P.push_back( vpPoint(L,L, 0 ) );
+ P.push_back( vpPoint(-L,L, 0 ) );
+
+ double L2 = L*3.0;
+ P.push_back( vpPoint(0,-L2, 0 ) );
+ P.push_back( vpPoint(L2,0, 0 ) );
+ P.push_back( vpPoint(0,L2, 0 ) );
+ P.push_back( vpPoint(-L2,0, 0 ) );
+
+ vpHomogeneousMatrix cMo_ref(0, 0.2, 1, 0, 0, 0) ;
+ for(size_t i=0 ; i < P.size(); i++)
+ {
+ P[i].project(cMo_ref) ;
+ P[i].print() ;
+ std::cout << std::endl;
+ }
+
+ //Introduce an error
+ double error = 0.01;
+ P[3].set_y(P[3].get_y() + 2*error);
+ P[6].set_x(P[6].get_x() + error);
+
+ vpPose pose;
+ for(size_t i=0 ; i < P.size() ; i++)
+ pose.addPoint(P[i]);
+
+ unsigned int nbInlierToReachConsensus = (unsigned int)(75.0 * (double)(P.size()) / 100.0);
+ double threshold = 0.001;
+
+ pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
+ pose.setRansacThreshold(threshold);
+
+ vpHomogeneousMatrix cMo ;
+ //vpPose::ransac(lp,lP, 5, 1e-6, ninliers, lPi, cMo) ;
+ pose.computePose(vpPose::RANSAC, cMo);
+
+ std::vector<vpPoint> inliers = pose.getRansacInliers();
+
+ std::cout << "Inliers: " << std::endl;
+ for (unsigned int i = 0; i < inliers.size() ; i++)
+ {
+ inliers[i].print() ;
+ std::cout << std::endl;
+ }
+
+ vpPoseVector pose_ref = vpPoseVector(cMo_ref);
+ vpPoseVector pose_est = vpPoseVector(cMo);
+
+ std::cout << std::endl;
+ std::cout << "reference cMo :\n" << pose_ref.t() << std::endl << std::endl;
+ std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
+
+ int test_fail = 0;
+ for(unsigned int i=0; i<6; i++) {
+ if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ test_fail = 1;
+ }
+
+ std::cout << "Pose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
+ return test_fail;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/visual_features/CMakeLists.txt b/modules/visual_features/CMakeLists.txt
new file mode 100644
index 0000000..b826b59
--- /dev/null
+++ b/modules/visual_features/CMakeLists.txt
@@ -0,0 +1,42 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(visual_features visp_core OPTIONAL visp_blob visp_me)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+vp_add_tests()
diff --git a/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h
new file mode 100644
index 0000000..1a9c86b
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpBasicFeature_H
+#define vpBasicFeature_H
+
+/*!
+ \file vpBasicFeature.h
+ \brief class that defines what is a visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+// Display Issue
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+
+//Color / image / display
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+
+// #define FEATURE_ALL 0xff
+
+// #define FEATURE_LINE1 0x1
+// #define FEATURE_LINE2 0x2
+// #define FEATURE_LINE3 0x4
+// #define FEATURE_LINE4 0x8
+// #define FEATURE_LINE5 0x10
+// #define FEATURE_LINE6 0x20
+// #define FEATURE_LINE7 0x40
+// #define FEATURE_LINE8 0x80
+
+
+/*!
+ \class vpBasicFeature
+ \ingroup group_core_features
+ \brief class that defines what is a visual feature
+*/
+class VISP_EXPORT vpBasicFeature
+{
+public: // Public constantes
+ static const unsigned int FEATURE_LINE [32];
+
+ enum {
+ FEATURE_ALL = 0xffff
+ };
+
+protected:
+ //! State of the visual feature.
+ vpColVector s ;
+ //! Dimension of the visual feature.
+ unsigned int dim_s ;
+ //int featureLine[8] ;
+ //! Ensure that all the parameters needed to compute the iteraction matrix are set.
+ bool *flags;
+ //! Number of parameters needed to compute the interaction matrix.
+ unsigned int nbParameters;
+
+public:
+ /*! Return the dimension of the feature vector \f$\bf s\f$. */
+ unsigned int dimension_s() { return dim_s ; }
+
+public:
+
+ virtual void init() = 0 ;
+
+ vpBasicFeature() ;
+ vpBasicFeature(const vpBasicFeature &f) ;
+ vpBasicFeature &operator=(const vpBasicFeature &f) ;
+ virtual ~vpBasicFeature();
+ //! Return element \e i in the state vector (usage : x = s[i] )
+ virtual inline double operator[](const unsigned int i) const { return s[i]; }
+
+ //! Select all the features.
+ static unsigned int selectAll() { return FEATURE_ALL ; }
+
+ // Get the feature vector.
+ vpColVector get_s(unsigned int select=FEATURE_ALL) const;
+
+ // Get the feature vector dimension.
+ unsigned int getDimension(const unsigned int select=FEATURE_ALL) const;
+ //! Compute the interaction matrix from a subset of the possible features.
+ virtual vpMatrix interaction(const unsigned int select = FEATURE_ALL) = 0;
+ virtual vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select= FEATURE_ALL);
+ //! Print the name of the feature.
+ virtual void print(const unsigned int select= FEATURE_ALL) const = 0 ;
+
+ virtual vpBasicFeature *duplicate() const = 0 ;
+
+public:
+ virtual void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const = 0;
+ virtual void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const = 0;
+
+ void setFlags();
+
+protected:
+ void resetFlags();
+ // memory issue (used by the vpServo class)
+public:
+
+ /*!
+ \enum vpBasicFeatureDeallocatorType
+ Indicates who should deallocate the feature.
+
+ */
+ typedef enum
+ {
+ user,
+ vpServo
+ } vpBasicFeatureDeallocatorType;
+
+protected:
+ vpBasicFeatureDeallocatorType deallocate ;
+public:
+ void setDeallocate(vpBasicFeatureDeallocatorType d) { deallocate = d ; }
+ vpBasicFeatureDeallocatorType getDeallocate() { return deallocate ; }
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h
new file mode 100644
index 0000000..7001bf4
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+#ifndef vpFeatureBuilder_H
+#define vpFeatureBuilder_H
+
+/*!
+ \file vpFeatureBuilder.h
+ \brief class that defines conversion between tracker and visual feature
+*/
+#include <visp3/core/vpConfig.h>
+
+// tracker
+#ifdef VISP_HAVE_MODULE_BLOB
+# include <visp3/blob/vpDot.h>
+# include <visp3/blob/vpDot2.h>
+#endif
+
+#ifdef VISP_HAVE_MODULE_ME
+# include <visp3/me/vpMeLine.h>
+# include <visp3/me/vpMeEllipse.h>
+#endif
+
+// forward projection tracker
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpSphere.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpCylinder.h>
+
+// visual feature
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/visual_features/vpFeatureVanishingPoint.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+
+// others
+#include <visp3/core/vpImagePoint.h>
+
+//pixel / meter conversion
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+
+
+/*!
+ \class vpFeatureBuilder
+
+ \ingroup group_visual_features_builder
+ \brief Class that defines conversion between trackers and visual features.
+*/
+class VISP_EXPORT vpFeatureBuilder
+{
+public:
+ // create vpFeaturePoint feature
+#ifdef VISP_HAVE_MODULE_BLOB
+ static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
+ const vpDot &d) ;
+ static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
+ const vpDot2 &d) ;
+#endif
+ static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
+ const vpImagePoint &t) ;
+ static void create(vpFeaturePoint &s, const vpPoint &p) ;
+ static void create(vpFeaturePoint &s,
+ const vpCameraParameters &goodCam,
+ const vpCameraParameters &wrongCam,
+ const vpPoint &p) ;
+
+#ifdef VISP_HAVE_MODULE_BLOB
+ static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2 ) ;
+ static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2) ;
+#endif
+ static void create(vpFeatureSegment &s, const vpCameraParameters &cam,
+ const vpImagePoint &ip1, const vpImagePoint &ip2);
+ static void create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2);
+
+ // create vpFeaturePointPolar feature
+#ifdef VISP_HAVE_MODULE_BLOB
+ static void create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpDot &dot) ;
+ static void create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpDot2 &dot) ;
+#endif
+ static void create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpImagePoint &iP) ;
+ static void create(vpFeaturePointPolar &s, const vpPoint &p) ;
+ static void create(vpFeaturePointPolar &s,
+ const vpCameraParameters &goodCam,
+ const vpCameraParameters &wrongCam,
+ const vpPoint &p) ;
+
+ // create vpFeaturePoint3D feature
+ static void create(vpFeaturePoint3D &s, const vpPoint &p ) ;
+
+ // create vpFeatureLine feature
+ static void create(vpFeatureLine &s, const vpLine &l ) ;
+ static void create(vpFeatureLine &s, const vpCylinder &c, const int line) ;
+
+#ifdef VISP_HAVE_MODULE_ME
+ static void create(vpFeatureLine &s,
+ const vpCameraParameters &cam,
+ const vpMeLine &mel) ;
+#endif
+
+ //! create vpFeatureEllipse feature
+ static void create(vpFeatureEllipse &s, const vpCircle &c) ;
+ static void create(vpFeatureEllipse &s, const vpSphere &sphere) ;
+#ifdef VISP_HAVE_MODULE_BLOB
+ static void create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpDot &d ) ;
+ static void create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpDot2 &d ) ;
+#endif
+
+#ifdef VISP_HAVE_MODULE_ME
+ static void create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpMeEllipse &d ) ;
+#endif
+
+ /*!
+ create vpFeatureVanishingPoint feature from the 2D coordinates of a point
+ in the image plane
+ */
+ static void create(vpFeatureVanishingPoint &s, const vpPoint &p);
+ /*!
+ create vpFeatureVanishingPoint feature from 2 FeatureLine, ie lines in
+ the image plane (error if the 2 lines are parallel)
+ */
+ static void create(vpFeatureVanishingPoint &s, const vpFeatureLine &l1, const vpFeatureLine &l2 );
+ /*!
+ create vpFeatureVanishingPoint feature from 2 Lines, (error if the 2
+ lines are parallel in the image plane)
+ */
+ static void create(vpFeatureVanishingPoint &s, const vpLine &l1, const vpLine &l2 );
+} ;
+
+#endif
+
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h b/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h
new file mode 100644
index 0000000..493578a
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point visual feature.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureDepth_H
+#define vpFeatureDepth_H
+
+/*!
+ \file vpFeatureDepth.h
+ \brief Class that defines 3D point visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeatureDepth
+ \ingroup group_visual_features
+
+ \brief Class that defines a 3D point visual feature \f$ s\f$ which
+ is composed by one parameters that is \f$ log( \frac{Z}{Z^*}) \f$
+ that defines the current depth relative to the desired depth. Here
+ \f$ Z \f$ represents the current depth and \f$ Z^* \f$ the desired
+ depth.
+
+ In this class \f$ x \f$ and \f$ y \f$ are the 2D coordinates in the
+ camera frame and are given in meter. \f$ x \f$, \f$ y \f$ and \f$ Z
+ \f$ are needed during the computation of the interaction matrix \f$
+ L \f$.
+
+ The visual features can be set easily thanks to the buildFrom() method.
+
+ As the visual feature \f$ s \f$ represents the current depth
+ relative to the desired depth, the desired visual feature \f$ s^*
+ \f$ is set to zero. Once the value of the visual feature is set, the
+ interaction() method allows to compute the interaction matrix \f$ L
+ \f$ associated to the visual feature, while the error() method
+ computes the error vector \f$(s - s^*)\f$ between the current visual
+ feature and the desired one which is here set to zero.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 3D depth feature \f$ log( \frac{Z}{Z^*}) \f$ that
+ corresponds to the current depth relative to the desired depth. To
+ control six degrees of freedom, at least five other features must be
+ considered. First we create a current (\f$s\f$) 3D depth
+ feature. Then we set the task to use the interaction matrix
+ associated to the current feature \f$L_s\f$. And finally we compute
+ the camera velocity \f$v=-\lambda \; L_s^+ \; (s-s^*)\f$. The
+ current feature \f$s\f$ is updated in the while() loop.
+
+ \code
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpFeatureDepth s; //The current point feature.
+ //Set the current parameters x, y, Z and the desired depth Zs
+ double x; //You have to compute the value of x.
+ double y; //You have to compute the value of y.
+ double Z; //You have to compute the value of Z.
+ double Zs; //You have to define the desired depth Zs.
+ //Set the point feature thanks to the current parameters.
+ s.buildfrom(x, y, Z, log(Z/Zs));
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the desired visual features sd
+ task.setInteractionMatrixType(vpServo::CURRENT);
+
+ // Add the 3D depth feature to the task
+ task.addFeature(s); // s* is here considered as zero
+
+ // Control loop
+ for ( ; ; ) {
+ // The new parameters x, y and Z must be computed here.
+
+ // Update the current point visual feature
+ s.buildfrom(x, y, Z, log(Z/Zs));
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+ return 0;
+}
+ \endcode
+
+ If you want to build your own control law, this other example shows how
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
+ build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpColVector.h>
+
+int main()
+{
+ vpFeatureDepth s; //The current point feature.
+ //Set the current parameters x, y, Z and the desired depth Zs
+ double x; //You have to compute the value of x.
+ double y; //You have to compute the value of y.
+ double Z; //You have to compute the value of Z.
+ double Zs; //You have to define the desired depth Zs.
+ //Set the point feature thanks to the current parameters.
+ s.buildfrom(x, y, Z, log(Z/Zs));
+
+ // Compute the interaction matrix L_s for the current point feature
+ vpMatrix L = s.interaction();
+
+ // Compute the error vector (s-s*) for the point feature with s* considered as 0.
+ vpColVector s_star(1); //the dimension is 1.
+ s_star(1) = 0; //The value of s* is 0.
+ s.error(s_star);
+}
+ \endcode
+*/
+
+
+class VISP_EXPORT vpFeatureDepth : public vpBasicFeature
+{
+
+private:
+ //! The \f$ x \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ double x;
+ //! The \f$ y \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ double y;
+ //! The \f$ Z \f$ 3D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ double Z;
+
+public:
+ void init() ;
+ vpFeatureDepth() ;
+ //! Destructor.
+ virtual ~vpFeatureDepth() {}
+
+ /*
+ section Set coordinates
+ */
+
+ void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar) ;
+
+ void set_x(const double x) ;
+
+ void set_y(const double y) ;
+
+ void set_Z(const double Z) ;
+
+ void set_LogZoverZstar(const double LogZoverZstar);
+
+ void set_xyZLogZoverZstar(const double x, const double y, const double Z, const double logZZs) ;
+
+ double get_x() const ;
+
+ double get_y() const ;
+
+ double get_Z() const ;
+
+ double get_LogZoverZstar() const ;
+
+
+ /*
+ vpBasicFeature method instantiation
+ */
+
+
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ vpFeatureDepth *duplicate() const ;
+
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h b/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h
new file mode 100644
index 0000000..1dfd20f
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D ellipse visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpFeatureEllipse_H
+#define vpFeatureEllipse_H
+
+/*!
+ \file vpFeatureEllipse.h
+ \brief Class that defines 2D ellipse visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeatureEllipse
+ \ingroup group_visual_features
+ \brief Class that defines 2D ellipse visual feature.
+*/
+class VISP_EXPORT vpFeatureEllipse : public vpBasicFeature
+{
+ /*
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities are useful but not mandatory
+ */
+private:
+ //! FeatureEllipse depth (required to compute the interaction matrix)
+ //! default Z = 1m
+ double A,B,C ;
+
+
+public:
+ //! Default initialization.
+ void init() ;
+ //! Default constructor.
+ vpFeatureEllipse() ;
+ //! Destructor.
+ virtual ~vpFeatureEllipse() { }
+
+public:
+ /*!
+ \section Set coordinates
+ */
+ //! basic constructor
+ vpFeatureEllipse(const double x, const double y,
+ const double mu20, const double mu11, const double mu02);
+
+ // void buildFrom(const vpEllipse &p) ;
+ void buildFrom(const double x, const double y,
+ const double mu20, const double mu11, const double mu02) ;
+ void buildFrom(const double x, const double y,
+ const double mu20, const double mu11, const double mu02,
+ const double A, const double B, const double C) ;
+ void set_x(const double x);
+ void set_y(const double y);
+ void set_xy(const double x,const double y);
+ void setABC(const double A, const double B, const double C) ;
+ void setMu(const double mu20, const double mu11, const double mu02) ;
+
+ double get_x() const {return s[0];}
+ double get_y() const {return s[1];}
+ double getMu20() const {return s[2];}
+ double getMu11() const {return s[3];}
+ double getMu02() const {return s[4];}
+
+public:
+ /*!
+ vpBasicFeature method instantiation
+ */
+
+ // feature selection
+ static unsigned int selectX();
+ static unsigned int selectY();
+ static unsigned int selectMu20();
+ static unsigned int selectMu11();
+ static unsigned int selectMu02();
+
+ //! compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ //! compute the error between two visual features from a subset
+ //! a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ //! compute the error between a visual features and zero
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+ //! print the name of the feature
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ //! feature duplication
+ vpFeatureEllipse *duplicate() const ;
+
+
+public:
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h
new file mode 100644
index 0000000..1325cfc
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception that can be emited by the vpFeature class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef __vpFeatureException_H
+#define __vpFeatureException_H
+
+
+/* ------------------------------------------------------------------------- */
+/* --- INCLUDE ------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+
+/* \file vpFeatureException.h
+ \brief error that can be emited by the vpFeature class and its derivates
+ */
+/* Classes standards. */
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpException.h>
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpFeatureException
+ \ingroup group_visual_features
+ \brief Error that can be emited by the vpBasicFeature class and its derivates.
+ */
+class VISP_EXPORT vpFeatureException : public vpException
+{
+ public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpFeature member
+ */
+ enum errorFeatureCodeEnum
+ {
+ //! feature list or desired feature list is empty
+ badErrorVectorError,
+ sizeMismatchError,
+ notInitializedError,
+ badInitializationError
+ } ;
+
+ public:
+ vpFeatureException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpFeatureException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpFeatureException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h
new file mode 100644
index 0000000..92ee73f
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h
@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D line visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureLine_H
+#define vpFeatureLine_H
+
+/*!
+ \file vpFeatureLine.h
+ \brief Class that defines 2D line visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeatureLine
+ \ingroup group_visual_features
+
+ \brief Class that defines a 2D line visual feature \f$ s\f$ which is
+ composed by two parameters that are \f$ \rho \f$ and \f$ \theta \f$,
+ the polar coordinates of a line.
+
+ In this class, the equation of the line in the image plane is given by :
+ \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f] Here
+ \f$ x \f$ and \f$ y \f$ are the coordinates of a point belonging to
+ the line and they are given in meter. The following image shows the
+ meanings of the distance \f$\rho\f$ and the angle \f$\theta\f$.
+
+ \image html vpFeatureLine.gif
+ \image latex vpFeatureLine.ps width=10cm
+
+ You have to note that the \f$ \theta \f$ angle has its value between
+ \f$ -\pi \f$ and \f$ \pi \f$ and that the \f$ \rho \f$ distance can
+ be positive or negative. The conventions are illustrated by the
+ image above.
+
+ The visual features can be set easily from an instance of the
+ classes vpLine, vpMeLine or vpCylinder. For more precision see the
+ class vpFeatureBuilder.
+
+ Once the values of the visual features are set, the interaction()
+ method allows to compute the interaction matrix \f$ L \f$ associated
+ to the visual feature, while the error() method computes the error
+ vector \f$(s - s^*)\f$ between the current visual feature and the
+ desired one.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 2D line feature \f$(\rho,\theta)\f$ that correspond to
+ the 2D equation of a line in the image plan. To control six
+ degrees of freedom, at least four other features must be considered
+ like two other line features for example. First we create a current
+ (\f$s\f$) 2D line feature. Then we set the task to use the
+ interaction matrix associated to the current feature \f$L_s\f$. And
+ finally we compute the camera velocity \f$v=-\lambda \; L_s^+ \;
+ (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
+ loop.
+
+ \code
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpFeatureLine sd; //The desired line feature.
+ //Sets the desired features rho and theta
+ double rhod = 0;
+ double thetad = 0;
+ //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
+ //The line described by the features belongs to this plan.
+ //Normally two plans are needed to describe a line. But to compute the interaction matrix only
+ //one equation of the two plans is needed.
+ //Notes that the Dd value must not be equal to zero !
+ double Ad = 0;
+ double Bd = 0;
+ double Cd = 1;
+ double Dd = -1;
+ //Set the line feature thanks to the desired parameters.
+ sd.buildfrom(rhod, thetad, Ad,Bd, Cd, Dd);
+
+ vpFeatureLine s; //The current line feature.
+ //Sets the current features rho and theta
+ double rho; //You have to compute the value of rho.
+ double theta; //You have to compute the value of theta.
+ //Set the line feature thanks to the current parameters.
+ s.buildfrom(rho, theta);
+ //In this case the parameters A, B, C, D are not needed because the interaction matrix is computed
+ //with the desired visual feature.
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the desired visual features sd
+ task.setInteractionMatrixType(vpServo::DESIRED);
+
+ // Add the 2D line feature to the task
+ task.addFeature(s, sd);
+
+ // Control loop
+ for ( ; ; ) {
+ // The new parameters rho and theta must be computed here.
+
+ // Update the current line visual feature
+ s.buildfrom(rho, theta);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+ return 0;
+}
+ \endcode
+
+ If you want to build your own control law, this other example shows how to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D line visual feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpFeatureLine sd; //The desired line feature.
+ //Sets the desired features rho and theta
+ double rhod = 0;
+ double thetad = 0;
+ //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
+ double Ad = 0;
+ double Bd = 0;
+ double Cd = 1;
+ double Dd = -1;
+ //Set the line feature thanks to the desired parameters.
+ sd.buildfrom(rhod, thetad, Ad,Bd, Cd, Dd);
+
+ vpFeatureLine s; //The current line feature.
+ //Sets the current features rho and theta
+ double rho; //You have to compute the value of rho.
+ double theta; //You have to compute the value of theta.
+ //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
+ double A; //You have to compute the value of A.
+ double B; //You have to compute the value of B.
+ double C; //You have to compute the value of C.
+ double D; //You have to compute the value of D. D must not be equal to zero !
+ //Set the line feature thanks to the current parameters.
+ s.buildfrom(rho, theta, A, B, C, D);
+
+ // Compute the interaction matrix L_s for the current line feature
+ vpMatrix L = s.interaction();
+ // You can also compute the interaction matrix L_s for the desired line feature
+ // The corresponding line of code is : vpMatrix L = sd.interaction();
+
+ // Compute the error vector (s-sd) for the line feature
+ s.error(s_star);
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpFeatureLine : public vpBasicFeature
+{
+ /*!
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities ar useful but not mandatory
+ */
+private:
+ //! FeatureLine depth (required to compute the interaction matrix)
+ //! equation of a plane
+ double A,B,C,D ;
+
+public:
+ void init() ;
+ vpFeatureLine() ;
+ //! Destructor.
+ virtual ~vpFeatureLine() {}
+
+public:
+
+ void setRhoTheta(const double rho, const double theta) ;
+ void setABCD(const double A, const double B,
+ const double C, const double D) ;
+
+ // void buildFrom(const vpLine &l) ;
+ // void buildFrom(const vpCylinder &c, const int l) ;
+ void buildFrom(const double rho, const double theta) ;
+ void buildFrom(const double rho, const double theta,
+ const double A, const double B,
+ const double C, const double D) ;
+
+
+ /*!
+ Return the \f$ \rho \f$ subset value of the visual feature \f$ s \f$.
+ */
+ double getRho() const { return s[0] ; }
+
+ /*!
+ Return the \f$ \theta \f$ subset value of the visual feature \f$ s \f$.
+ */
+ double getTheta() const { return s[1] ; }
+
+
+public:
+
+ static unsigned int selectRho();
+ static unsigned int selectTheta();
+
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+
+ //vpColVector error(const int select = FEATURE_ALL) ;
+
+ void print(const unsigned int select= FEATURE_ALL) const ;
+
+ vpFeatureLine *duplicate() const ;
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h
new file mode 100644
index 0000000..3f976d6
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Luminance based feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpFeatureLuminance_h
+#define vpFeatureLuminance_h
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpImage.h>
+
+
+/*!
+ \file vpFeatureLuminance.h
+ \brief Class that defines the image luminance visual feature
+
+ For more details see \cite Collewet08c.
+*/
+
+
+/*!
+ \class vpLuminance
+ \brief Class that defines the luminance and gradient of a point
+
+ \sa vpFeatureLuminance
+*/
+
+
+class VISP_EXPORT vpLuminance
+{
+ public:
+ double x, y; // point coordinates (in meter)
+ double I ; // pixel intensity
+ double Ix,Iy ; // pixel gradient
+ double Z; // pixel depth
+
+};
+
+
+/*!
+ \class vpFeatureLuminance
+ \ingroup group_visual_features
+ \brief Class that defines the image luminance visual feature
+
+ For more details see \cite Collewet08c.
+*/
+
+class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature
+{
+ protected:
+ //! FeaturePoint depth (required to compute the interaction matrix)
+ //! default Z = 1m
+ double Z ;
+
+ //! Number of rows.
+ unsigned int nbr ;
+ //! Number of column.
+ unsigned int nbc ;
+ //! Border size.
+ unsigned int bord ;
+
+ //! Store the image (as a vector with intensity and gradient I, Ix, Iy)
+ vpLuminance *pixInfo ;
+ int firstTimeIn ;
+
+ public:
+ void buildFrom(vpImage<unsigned char> &I) ;
+
+public:
+
+ void init() ;
+ void init(unsigned int _nbr, unsigned int _nbc, double _Z) ;
+
+ vpFeatureLuminance() ;
+ vpFeatureLuminance(const vpFeatureLuminance& f) ;
+ vpFeatureLuminance &operator=(const vpFeatureLuminance& f) ;
+
+ //! Destructor.
+ virtual ~vpFeatureLuminance() ;
+
+ public:
+ vpCameraParameters cam ;
+ void setCameraParameters(vpCameraParameters &_cam) ;
+ /*
+ section Set/get Z
+ */
+
+
+ void set_Z(const double Z) ;
+ double get_Z() const ;
+
+
+ /*
+ vpBasicFeature method instantiation
+ */
+
+
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void interaction(vpMatrix &L);
+
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ void error(const vpBasicFeature &s_star,
+ vpColVector &e) ;
+
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ vpFeatureLuminance *duplicate() const ;
+
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
+
+ //! Compute the error between a visual features and zero
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+} ;
+
+
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h
new file mode 100644
index 0000000..73723ba
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h
@@ -0,0 +1,246 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base for all moment features
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+\file vpFeatureMoment.h
+\brief Base class for moment features.
+
+Handles common system operations like selection, duplication. Functionality is computed in derived classes.
+*/
+#ifndef __FEATUREMOMENT_H__
+#define __FEATUREMOMENT_H__
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpException.h>
+#include <vector>
+
+class vpMomentObject;
+class vpMomentDatabase;
+class vpFeatureMomentDatabase;
+class vpMoment;
+
+/*!
+\class vpFeatureMoment
+
+\ingroup group_visual_features
+
+\brief This class defines shared system methods/attributes for 2D moment features but no functional code.
+It is used to compute interaction matrices for moment features.
+
+While vpMoment-type classes do only compute moment values and can by used for almost anything, vpFeatureMoment-type classes
+are specifically designed for visual servoing. More importantly, a vpFeatureMoment is used to compute the interaction matrix associated to it's moment primitive.
+
+This class is virtual and cannot be used directly. It defines the following characteristics common to all moment features:
+- Plane orientation parameters (A,B,C):
+Each camera frame corresponds to a physical planar object contained in a plane. This plane's equation has the following form:
+\f$ A \times x+B \times y + C = \frac{1}{Z} \f$.
+These parameters can be updated anytime.
+- Get corresponding moment primitive: for example a vpFeatureMomentCInvariant will provide access to a vpMomentCInvariant instance.
+- Provide access to a feature database (vpFeatureMomentDatabase).
+- All interaction matrices (different from vpBasicFeature::interaction which selects the required interaction matrix).
+
+Like vpMoment, vpFeatureMoment provides a vpFeatureMoment::update() method. But unlike vpMoment::update() which only acknowledges the new object,
+the vpFeatureMoment::update() acknowledges the new plane parameters AND computes the interaction matrices associated with the feature.
+
+A vpFeatureMoment will be often part of a vpFeatureMomentDatabase in the same way a vpMoment is part of a vpMomentDatabase. This database is specified inside the
+vpFeatureMoment::vpFeatureMoment() constructor.
+As a result, a vpFeatureMoment will be able to access other vpFeatureMoments through this database.
+
+A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
+is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
+
+Note that you can use vpFeatureMoment to do visual servoing but it is not it's only purpose. You may compute your interaction matrices with vpFeatureMoment::update()
+and use them for any purpose.
+
+\attention - A vpFeatureMoment is not responsible for updating the moment primitives it depends on. Make sure your vpMoments are all up to date before computing an interaction matrix using vpFeatureMoment.
+
+\attention - Be careful with orders. Often, computing a feature of order n requires vpMoment primitives of order n+1.
+Make sure to check the documentation of the specialised vpFeatureMoment classes when deciding to which order you want to initialize the object.
+An object of order 6 should be sufficient for all classic implementations of vpFeatureMoment.
+
+Here is an example of how to use a vpFeatureMoment (in this case vpFeatureMomentBasic).
+\code
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+
+ //////////////////////////////REFERENCE VALUES////////////////////////////////
+ vpMomentObject obj(3); // Init object of order 3 because we need
+ // vpFeatureMomentBasic of order 2 which
+ // implies third-order moment primitives
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
+ obj.fromVector(vec_p);
+
+ vpMomentDatabase mdb; //database for moment primitives. This will
+ //only contain the basic moment.
+ vpMomentBasic bm; //basic moment (this particular moment is nothing
+ //more than a shortcut to the vpMomentObject)
+ bm.linkTo(mdb); //add basic moment to moment database
+
+ vpFeatureMomentBasic fmb(mdb,0,0,1,NULL);
+
+ //update and compute the vpMoment BEFORE doing any operations with vpFeatureMoment
+ bm.update(obj);
+ bm.compute();
+
+ fmb.update(0,0,1); //update the vpFeatureMoment with a plane
+ //configuration
+ std::cout << fmb.interaction(1,1) << std::endl;
+}
+\endcode
+*/
+class VISP_EXPORT vpFeatureMoment : public vpBasicFeature
+{
+protected:
+ const vpMoment* moment;
+ const vpMoment& getMoment() const {return *moment;}
+ vpMomentDatabase& moments;
+ vpFeatureMomentDatabase* featureMomentsDataBase;
+ std::vector<vpMatrix> interaction_matrices;
+
+ double A;
+ double B;
+ double C;
+ char _name[255];
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpFeatureMoment(const vpFeatureMoment &fm)
+// : vpBasicFeature(), moment(NULL), moments(fm.moments), featureMomentsDataBase(NULL),
+// interaction_matrices(), A(0), B(0), C(0)
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpFeatureMoment &operator=(const vpFeatureMoment &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane, feature database and matrix size.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database
+ \param nbmatrices : If you want to create a new vpFeatureMoment implementation, your feature will often have a matrix size of n lines. You can specify the number of lines by this parameter.
+ */
+ vpFeatureMoment(vpMomentDatabase& data_base,double A_=0.0, double B_=0.0, double C_=0.0,
+ vpFeatureMomentDatabase* featureMoments=NULL,unsigned int nbmatrices=1)
+ : vpBasicFeature(), moment(NULL),
+ moments(data_base),
+ featureMomentsDataBase(featureMoments),
+ interaction_matrices(nbmatrices),
+ A(A_),B(B_),C(C_)
+ {}
+
+ virtual ~vpFeatureMoment();
+
+ virtual void compute_interaction (void);
+ vpBasicFeature* duplicate () const;
+ void display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
+ const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
+ void display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
+ const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
+
+ int getDimension (unsigned int select=FEATURE_ALL) const;
+ void init (void);
+ vpMatrix interaction (const unsigned int select=FEATURE_ALL) ;
+ void linkTo(vpFeatureMomentDatabase& featureMoments);
+
+ /*!
+ Name of the moment corresponding to the feature. This allows to locate the moment
+ associated with the feature in the provided database.
+ */
+ virtual const char* momentName() const = 0;
+ /*!
+ Name of the feature used to locate it in the database of features.
+ */
+ virtual const char* name() const = 0;
+ void print (const unsigned int select=FEATURE_ALL) const ;
+
+ void update (double A, double B, double C);
+
+ friend VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM);
+ virtual void printDependencies(std::ostream& os) const;
+};
+
+/*!
+\class vpMomentGenericFeature
+
+\ingroup group_visual_features
+
+\brief This class defines a generic feature used for moment feature duplication.
+
+A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
+is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
+The reason for this is that a vpMomentGenericFeature can refer to anything therefore it has no specific name.
+
+Duplication is mostly used internally in ViSP.
+
+*/
+class VISP_EXPORT vpMomentGenericFeature : public vpFeatureMoment{
+public:
+ vpMomentGenericFeature(vpMomentDatabase& data_base,double A_, double B_, double C_,
+ vpFeatureMomentDatabase* featureMoments, const vpMoment* p_moment)
+ : vpFeatureMoment(data_base,A_,B_,C_,featureMoments){this->moment = p_moment;}
+ /*!
+ No specific moment name.
+ */
+ const char* momentName() const { return NULL;}
+ /*!
+ No specific feature name.
+ */
+ virtual const char* name() const { return NULL;}
+};
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h
new file mode 100644
index 0000000..24e1402
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentAlpha.h
+ \brief Implementation of the interaction matrix computation for vpMomentAlpha.
+*/
+
+#ifndef __FEATUREMOMENTALPHA_H__
+#define __FEATUREMOMENTALPHA_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/core/vpColVector.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+
+
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentAlpha
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$. Computes the interaction matrix associated with vpMomentAlpha.
+
+ The interaction matrix for the feature can be deduced from \cite Tahri05z.
+
+ This class allows to compute the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
+
+ The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
+
+ This feature is often used in moment-based visual servoing to control the planar rotation parameter.
+
+ Minimum vpMomentObject order needed to compute this feature: 4.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered.
+*/
+class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentAlpha(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(moments,A,B,C,featureMoments,1)
+ {}
+
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentAlpha";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentAlpha";}
+};
+#else
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentAlpha
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$: computes the interaction matrix associated with vpMomentAlpha.
+
+ This class computes the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
+
+ The interaction matrix for the feature has the following form:
+ \f[{
+ \left[ \begin {array}{c} {\frac {\mu_{{1,1}}{\it DA}\,A}{d}}+{\frac {
+ \left( {\it DA}\,\mu_{{0,2}}+1/2\,d-1/2\,{{\it DA}}^{2} \right) B}{d}
+}\\ \noalign{\medskip}{\frac { \left( {\it DA}\,\mu_{{0,2}}-1/2\,d-1/2
+\,{{\it DA}}^{2} \right) A}{d}}-{\frac {B\mu_{{1,1}}{\it DA}}{d}}
+\\ \noalign{\medskip}Bw_{{x}}-Aw_{{y}}\\ \noalign{\medskip}{\frac {
+\beta\, \left( \mu_{{1,2}} \left( \mu_{{2,0}}-\mu_{{0,2}} \right) +\mu
+_{{1,1}} \left( \mu_{{0,3}}-\mu_{{2,1}} \right) \right) +\gamma\,x_{{
+g}} \left( \mu_{{0,2}} \left( \mu_{{2,0}}-\mu_{{0,2}} \right) -2\,{\mu
+_{{1,1}}}^{2} \right) +\gamma\,y_{{g}}\mu_{{1,1}} \left( \mu_{{2,0}}+
+\mu_{{0,2}} \right) }{d}}\\ \noalign{\medskip}{\frac {\beta\, \left(
+\mu_{{2,1}} \left( \mu_{{0,2}}-\mu_{{2,0}} \right) +\mu_{{1,1}}
+ \left( \mu_{{3,0}}-\mu_{{1,2}} \right) \right) +\gamma\,x_{{g}}\mu_{
+{1,1}} \left( \mu_{{2,0}}+\mu_{{0,2}} \right) +\gamma\,y_{{g}} \left(
+\mu_{{2,0}} \left( \mu_{{0,2}}-\mu_{{2,0}} \right) -2\,{\mu_{{1,1}}}^{
+2} \right) }{d}}\\ \noalign{\medskip}-1\end {array} \right]
+}^t
+\f]
+with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} = DA^2+4{\mu_{1,1}}^2\f$.
+
+ - In the discrete case:
+ \f$beta = 4\f$,\f$gamma = 2\f$.
+ - In the dense case:
+ \f$beta = 5\f$,\f$gamma = 1\f$.
+
+
+ The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
+
+ This feature is often used in moment-based visual servoing to control the planar rotation parameter.
+
+ Minimum vpMomentObject order needed to compute this feature: 4.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentGravityCenter
+*/
+class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentAlpha(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1)
+ {}
+
+ void compute_interaction();
+ /*!
+ Associated moment name.
+ */
+ const char* momentName() const { return "vpMomentAlpha";}
+ /*!
+ Feature name.
+ */
+ const char* name() const { return "vpFeatureMomentAlpha";}
+
+ vpColVector error (const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL);
+};
+#endif
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h
new file mode 100644
index 0000000..3a32d44
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Definition of vpFeatureMomentArea associated to vpMomentArea
+ *
+ * Authors:
+ * Manikandan Bakthavatchalam
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentArea.h
+ \brief Implementation of the interaction matrix computation for vpMomentArea.
+*/
+#ifndef __FEATUREMOMENTAREA_H__
+#define __FEATUREMOMENTAREA_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+
+class vpMomentDatabase;
+
+/*!
+ \class vpFeatureMomentArea
+
+ \ingroup group_visual_features
+
+ \brief Surface moment feature. Computes the interaction matrix associated with vpMomentArea.
+
+*/
+
+class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentArea(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
+
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentArea";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentArea";}
+
+};
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h
new file mode 100644
index 0000000..ce2afa5
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentAreaNormalized.h
+ \brief Implementation of the interaction matrix computation for vpMomentAreaNormalized.
+*/
+#ifndef __FEATUREMOMENTAREANORMALIZED_H__
+#define __FEATUREMOMENTAREANORMALIZED_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+class vpMomentDatabase;
+
+/*!
+ \class vpFeatureMomentAreaNormalized
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
+
+ The interaction matrix for the moment feature can be deduced from \cite Tahri05z.
+
+ To do so, one must derive it and obtain a combination of interaction matrices by using (1).
+ It allows to compute the interaction matrix for \f$ a_n \f$.
+
+ The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
+
+ This feature is often used in moment-based visual servoing to control the depth parameter.
+
+ Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentAreaNormalized
+ - vpFeatureMomentBasic
+
+*/
+class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentAreaNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
+ : vpFeatureMoment(database,A_,B_,C_,featureMoments,1){}
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentAreaNormalized";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentAreaNormalized";}
+
+};
+
+#else
+class vpMomentDatabase;
+
+/*!
+ \class vpFeatureMomentAreaNormalized
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
+
+ The interaction matrix for the moment has the following form:
+ - In the discrete case:
+ \f[
+ L_{a_n} =
+{
+ \left[ \begin {array}{c} a_{{n}}Ae_{{2,0}}+a_{{n}}Be_{{1,1}}
+\\ \noalign{\medskip}a_{{n}}Ae_{{1,1}}+a_{{n}}Be_{{0,2}}
+\\ \noalign{\medskip}-a_{{n}}C+Bw_{{x}}-Aw_{{y}}\\ \noalign{\medskip}-
+ \left( e_{{2,0}}+2\,e_{{0,2}} \right) y_{{g}}-e_{{2,1}}-x_{{g}}e_{{1,
+1}}+\eta_{{1,1}}e_{{1,0}}-e_{{0,3}}+\eta_{{0,2}}e_{{0,1}}
+\\ \noalign{\medskip} \left( 2\,e_{{2,0}}+e_{{0,2}} \right) x_{{g}}+e_
+{{3,0}}+y_{{g}}e_{{1,1}}-\eta_{{2,0}}e_{{1,0}}+e_{{1,2}}-\eta_{{1,1}}e
+_{{0,1}}\\ \noalign{\medskip}0\end {array} \right]
+
+}^t
+
+ \f]
+ - In the dense case:
+ \f[
+ L_{a_n} =
+{
+ \left[ \begin {array}{c} 1/2\,a_{{n}}A\\ \noalign{\medskip}1/2\,a_{{n
+}}B\\ \noalign{\medskip}-a_{{n}}C-3/2\,Ax_{{n}}-3/2\,By_{{n}}
+\\ \noalign{\medskip}-3/2\,y_{{n}}\\ \noalign{\medskip}3/2\,x_{{n}}
+\\ \noalign{\medskip}0\end {array} \right]
+
+}^t
+
+ \f]
+with:
+ - \f$e_{i,j}=\frac{\mu_{i,j}}{NA}\f$
+ - \f$NA=\mu_{2,0}+\mu_{0,2}\f$
+ - \f$\eta\f$ is the centered and normalized moment.
+ To do so, one must derive it and obtain a combination of interaction matrices by using (1).
+ It allows to compute the interaction matrix for \f$ a_n \f$.
+
+ The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
+
+ This feature is often used in moment-based visual servoing to control the depth parameter.
+
+ Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentAreaNormalized
+ - vpMomentGravityCenter
+
+*/
+class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentAreaNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentAreaNormalized";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentAreaNormalized";}
+
+};
+#endif
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h
new file mode 100644
index 0000000..b343932
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentBasic.h
+ \brief Implementation of the interaction matrix computation for vpMomentBasic.
+*/
+
+#ifndef __FEATUREMOMENTBASIC_H__
+#define __FEATUREMOMENTBASIC_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentObject.h>
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentBasic
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for basic moment feature. Computes the interaction matrix associated with vpMomentBasic.
+
+ The interaction matrix for the basic moment feature is defined in \cite Tahri05z, equation (13).
+ This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
+ The interaction matrix \f$ L_{m_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ m_{ij} \f$ obtained by vpMomentBasic::get (i,j).
+ vpFeatureMomentBasic computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
+ \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
+ For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
+ \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
+
+ You can see an example of vpFeatureMomentBasic by looking at the documentation of the vpFeatureMoment class.
+
+ This feature depends on:
+ - vpMomentBasic
+
+*/
+class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment{
+protected:
+ unsigned int order;
+ public:
+ vpFeatureMomentBasic(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
+ void compute_interaction();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ /* Add function due to pure virtual definition in vpBasicFeature.h */
+ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
+ throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+ }
+#endif
+
+ vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
+ /*!
+ Associated moment name.
+ */
+ const char* momentName() const { return "vpMomentBasic";}
+ /*!
+ Feature name.
+ */
+ const char* name() const { return "vpFeatureMomentBasic";}
+
+};
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h
new file mode 100644
index 0000000..c35520d
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentCInvariant.h
+ \brief Implementation of the interaction matrix computation for vpMomentCInvariant.
+*/
+#ifndef __FEATUREMOMENTCINVARIANT_H__
+#define __FEATUREMOMENTCINVARIANT_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+/*!
+ \class vpFeatureMomentCInvariant
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
+
+ The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
+ To do so, one must derive them and obtain a combination of interaction matrices by using (1).
+ It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
+ The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
+ For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
+ Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
+ Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
+
+ These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
+
+ Be careful about the nature of your object when selecting the right features.
+ Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
+
+ Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentCInvariant
+ - vpFeatureMomentBasic
+
+ An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
+
+*/
+class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentCInvariant(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(moments,A,B,C,featureMoments,16){}
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentCInvariant";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentCInvariant";}
+
+ /*!
+ Shortcut selector for \f$C_1\f$.
+ */
+ static unsigned int selectC1(){ return 1 << 0; }
+ /*!
+ Shortcut selector for \f$C_2\f$.
+ */
+ static unsigned int selectC2(){ return 1 << 1; }
+ /*!
+ Shortcut selector for \f$C_3\f$.
+ */
+ static unsigned int selectC3(){ return 1 << 2; }
+ /*!
+ Shortcut selector for \f$C_4\f$.
+ */
+ static unsigned int selectC4(){ return 1 << 3; }
+ /*!
+ Shortcut selector for \f$C_5\f$.
+ */
+ static unsigned int selectC5(){ return 1 << 4; }
+ /*!
+ Shortcut selector for \f$C_6\f$.
+ */
+ static unsigned int selectC6(){ return 1 << 5; }
+ /*!
+ Shortcut selector for \f$C_7\f$.
+ */
+ static unsigned int selectC7(){ return 1 << 6; }
+ /*!
+ Shortcut selector for \f$C_8\f$.
+ */
+ static unsigned int selectC8(){ return 1 << 7; }
+ /*!
+ Shortcut selector for \f$C_9\f$.
+ */
+ static unsigned int selectC9(){ return 1 << 8; }
+ /*!
+ Shortcut selector for \f$C_{10}\f$.
+ */
+ static unsigned int selectC10(){ return 1 << 9; }
+ /*!
+ Shortcut selector for \f$S_x\f$.
+ */
+ static unsigned int selectSx(){ return 1 << 10; }
+ /*!
+ Shortcut selector for \f$S_y\f$.
+ */
+ static unsigned int selectSy(){ return 1 << 11; }
+ /*!
+ Shortcut selector for \f$P_x\f$.
+ */
+ static unsigned int selectPx(){ return 1 << 12; }
+ /*!
+ Shortcut selector for \f$P_y\f$.
+ */
+ static unsigned int selectPy(){ return 1 << 13; }
+
+};
+
+#else
+class vpMomentDatabase;
+
+/*!
+ \class vpFeatureMomentCInvariant
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
+
+ The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
+ To do so, one must derive them and obtain a combination of interaction matrices by using (1).
+ It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
+ The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
+ For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
+ Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
+ Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
+
+ These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
+
+ Be careful about the nature of your object when selecting the right features.
+ Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
+
+ Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
+
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentCInvariant
+ - vpFeatureMomentBasic
+
+ An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
+
+*/
+class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
+ private:
+ std::vector<vpMatrix> LI;
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentCInvariant(vpMomentDatabase& data_base,double A_, double B_, double C_, vpFeatureMomentDatabase* featureMoments=NULL)
+ : vpFeatureMoment(data_base,A_,B_,C_,featureMoments,16), LI(16)
+ {}
+
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentCInvariant";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentCInvariant";}
+
+ /*!
+ Shortcut selector for \f$C_1\f$.
+ */
+ static unsigned int selectC1(){ return 1 << 0; }
+ /*!
+ Shortcut selector for \f$C_2\f$.
+ */
+ static unsigned int selectC2(){ return 1 << 1; }
+ /*!
+ Shortcut selector for \f$C_3\f$.
+ */
+ static unsigned int selectC3(){ return 1 << 2; }
+ /*!
+ Shortcut selector for \f$C_4\f$.
+ */
+ static unsigned int selectC4(){ return 1 << 3; }
+ /*!
+ Shortcut selector for \f$C_5\f$.
+ */
+ static unsigned int selectC5(){ return 1 << 4; }
+ /*!
+ Shortcut selector for \f$C_6\f$.
+ */
+ static unsigned int selectC6(){ return 1 << 5; }
+ /*!
+ Shortcut selector for \f$C_7\f$.
+ */
+ static unsigned int selectC7(){ return 1 << 6; }
+ /*!
+ Shortcut selector for \f$C_8\f$.
+ */
+ static unsigned int selectC8(){ return 1 << 7; }
+ /*!
+ Shortcut selector for \f$C_9\f$.
+ */
+ static unsigned int selectC9(){ return 1 << 8; }
+ /*!
+ Shortcut selector for \f$C_{10}\f$.
+ */
+ static unsigned int selectC10(){ return 1 << 9; }
+ /*!
+ Shortcut selector for \f$S_x\f$.
+ */
+ static unsigned int selectSx(){ return 1 << 10; }
+ /*!
+ Shortcut selector for \f$S_y\f$.
+ */
+ static unsigned int selectSy(){ return 1 << 11; }
+ /*!
+ Shortcut selector for \f$P_x\f$.
+ */
+ static unsigned int selectPx(){ return 1 << 12; }
+ /*!
+ Shortcut selector for \f$P_y\f$.
+ */
+ static unsigned int selectPy(){ return 1 << 13; }
+
+ /*!
+ Print all the interaction matrices of the moment invariants
+ */
+ void printLsofInvariants(std::ostream& os) const;
+
+ /*!
+ Print all the interaction matrices of visual features
+ */
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCInvariant& featcinv);
+
+};
+#endif
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h
new file mode 100644
index 0000000..378129e
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ * Manikandan Bakthavatchalam
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentCentered.h
+ \brief Implementation of the interaction matrix computation for vpMomentCentered.
+*/
+#ifndef __FEATUREMOMENTCENTERED_H__
+#define __FEATUREMOMENTCENTERED_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentCentered
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for centered moment feature. Computes the interaction matrix associated with vpMomentCentered.
+
+ The interaction matrix for the feature is defined in \cite Tahri05z, equation (17).
+ This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
+ The interaction matrix \f$ L_{\mu_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ \mu_{ij} \f$ obtained by vpFeatureMomentCentered::get (i,j).
+
+ vpFeatureMomentCentered computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
+ \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
+ For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
+ \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
+
+ This feature depends on:
+ - vpFeatureMomentBasic
+ - vpFeatureMomentGravityCenter
+ - vpMomentGravityCenter
+*/
+class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment{
+
+protected:
+
+ unsigned int order;
+ /*!
+ Core computation of interaction matrix for moment m_pq
+ */
+ vpMatrix
+ compute_Lmu_pq(const unsigned int& p, const unsigned int& q, const double& xg, const double& yg,
+ const vpMatrix& L_xg, const vpMatrix& L_yg,
+ const vpMomentBasic& m, const vpFeatureMomentBasic& feature_moment_m) const;
+
+ public:
+ vpFeatureMomentCentered(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
+ void compute_interaction();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ /* Add function due to pure virtual definition in vpBasicFeature.h */
+ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
+ throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+ }
+#endif
+ /*!
+ Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
+ \param select_one : first index (i)
+ \param select_two : second index (j)
+ \return Interaction matrix corresponding to the moment
+ */
+ vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
+
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentCentered";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentCentered";}
+
+friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCentered& v);
+};
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h
new file mode 100644
index 0000000..1dee1a4
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h
@@ -0,0 +1,267 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pre-filled pseudo-database used to handle dependencies between common moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpFeatureMomentCommon.h
+ \brief Pre-filled pseudo-database used to handle dependencies between common moment features.
+*/
+
+#ifndef __FEATUREMOMENTCOMMON_H__
+#define __FEATUREMOMENTCOMMON_H__
+#include <visp3/visual_features/vpFeatureMomentAlpha.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentArea.h>
+
+class vpMomentDatabase;
+class vpServo;
+/*!
+ \class vpFeatureMomentCommon
+
+ \ingroup group_visual_features
+
+ \brief This class allows to access common vpFeatureMoments in a pre-filled database.
+
+ It is a vpMomentDatabase filled with the following moments:
+ - vpFeatureMomentGravityCenter
+ - vpFeatureMomentGravityCenterNormalized
+ - vpFeatureMomentAreaNormalized
+ - vpFeatureMomentCInvariant
+ - vpFeatureMomentAlpha
+ - vpFeatureMomentCentered
+ - vpFeatureMomentBasic
+
+
+ There is no need to do the linkTo operations manually nor is it necessary to care about the order of feature computation.
+
+ This class has an vpMomentCommon::updateAll method capable of updating the plane parameters AND computing interaction matrices inside the features.
+
+ The moment features computed by this class are classical moments
+ features used in moment-based visual servoing. For more
+ information see \cite Tahri05z.
+
+ To initialize this feature set, the user needs to supply a vpMomentDatabase containing at least the contents of vpMomentCommon.
+
+ The features can be retrieved like from a normal vpFeatureMomentDatabase. However, some shortcuts to retrieve the features are provided.
+
+ \attention Make sure your object is at least of order 6 when using this pre-filled database.
+
+ The following code demonstrates the construction of a 6x6 interaction matrix as described in [1].
+ \code
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/vs/vpServo.h>
+#include <iostream>
+#include <vector>
+
+int main()
+{
+ // Define source polygon
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+
+ p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+ p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
+ vec_p.push_back(p);
+
+
+ vpMomentObject src(6); // Create a source moment object with 6 as maximum order
+ src.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
+ src.fromVector(vec_p); // Init the dense object with the source polygon
+ vec_p.clear();
+
+ //Define destination polygon. This is the source polygon translated
+ //of 0.1 on x-axis
+ p.set_x(-0.1); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(+0.4); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+ p.set_x(+0.3); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
+ vec_p.push_back(p);
+ p.set_x(-0.1); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
+ vec_p.push_back(p);
+ p.set_x(-0.1); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
+ vec_p.push_back(p);
+
+ vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
+ dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
+ dst.fromVector(vec_p); // Init the dense object with the destination
+ // polygon
+
+
+ //init classic moment primitives (for source)
+ vpMomentCommon mdb_src(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.);
+ //Init classic features
+ vpFeatureMomentCommon fmdb_src(mdb_src);
+
+ ////init classic moment primitives (for destination)
+ vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.);
+ //Init classic features
+ vpFeatureMomentCommon fmdb_dst(mdb_dst);
+
+ //update+compute moment primitives from object (for source)
+ mdb_src.updateAll(src);
+ //update+compute features (+interaction matrices) from plane
+ fmdb_src.updateAll(0.,0.,1.);
+
+ //update+compute moment primitives from object (for destination)
+ mdb_dst.updateAll(dst);
+ //update+compute features (+interaction matrices) from plane
+ fmdb_dst.updateAll(0.,0.,1.);
+
+ //define visual servoing task
+ vpServo task;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT);
+
+ //Add all classic features to the task
+ //In this example, source and destination features are translated by 0.1
+ //will produce a movement of 0.1 on x-axis.
+ task.addFeature(fmdb_src.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
+ task.addFeature(fmdb_src.getFeatureAn(),fmdb_dst.getFeatureAn());
+ //the object is NOT symmetric
+ //select C4 and C6
+ task.addFeature(fmdb_src.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),(1 << 3) | (1 << 5));
+ task.addFeature(fmdb_src.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
+
+ task.setLambda(1) ;
+ vpColVector v = task.computeControlLaw() ;
+
+ task.print();
+
+ task.kill();
+
+ return 0;
+}
+ \endcode
+This code produces the following output:
+\code
+Visual servoing task:
+Type of control law
+Eye-in-hand configuration
+Control in the camera frame
+List of visual features : s
+0.0166667,-0.00833333,
+1,
+-0.312148,0.0249916,
+-1.43449,
+List of desired visual features : s*
+0.116667,-0.00833333,
+1,
+-0.312148,0.0249916,
+-1.43449,
+Interaction Matrix Ls
+-1 0 -6.938893904e-18 0.007291666667 -1.06875 -0.008333333333
+0 -1 3.469446952e-18 1.0171875 -0.007291666667 -0.01666666667
+0 0 -1 0.0125 0.025 0
+0 0 -4.585529113e-15 -0.2983860943 0.5832596643 -4.376751552e-16
+0 0 -3.58244462e-15 0.08633028234 -0.2484618767 3.63421192e-16
+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
+
+\endcode
+*/
+class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase{
+private:
+ vpFeatureMomentGravityCenter featureGravity;
+ vpFeatureMomentGravityCenterNormalized featureGravityNormalized;
+ vpFeatureMomentAreaNormalized featureAn;
+ vpFeatureMomentCInvariant featureCInvariant;
+ vpFeatureMomentAlpha featureAlpha;
+ vpFeatureMomentCentered featureCentered;
+ vpFeatureMomentBasic featureMomentBasic;
+ vpFeatureMomentArea feature_moment_area;
+
+public:
+ vpFeatureMomentCommon(vpMomentDatabase& moments,double A=0.0,double B=0.0,double C=1.0);
+ void updateAll(double A,double B,double C);
+ /*!
+ Returns alpha.
+ */
+ vpFeatureMomentAlpha& getFeatureAlpha(){ return featureAlpha;}
+
+ /*!
+ Returns normalized surface.
+ */
+ vpFeatureMomentAreaNormalized& getFeatureAn(){ return featureAn;}
+ /*!
+ Returns basic moment.
+ */
+ vpFeatureMomentBasic& getFeatureMomentBasic(){ return featureMomentBasic;}
+ /*!
+ Returns centered moments.
+ */
+ vpFeatureMomentCentered& getFeatureCentered(){ return featureCentered;}
+
+ /*!
+ Returns non-symmetric invariants.
+ */
+ vpFeatureMomentCInvariant& getFeatureCInvariant(){ return featureCInvariant;}
+ /*!
+ Returns normalized gravity center.
+ */
+ vpFeatureMomentGravityCenterNormalized& getFeatureGravityNormalized(){ return featureGravityNormalized;}
+ /*!
+ Returns the area
+ */
+ vpFeatureMomentArea& getFeatureArea(){ return feature_moment_area;}
+ /*!
+ Returns gravity center
+ */
+ vpFeatureMomentGravityCenter& getFeatureGravityCenter(){ return featureGravity;}
+};
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h
new file mode 100644
index 0000000..f7910d5
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pseudo-database used to handle dependencies between moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpFeatureMomentDatabase.h
+ \brief Pseudo-database used to handle dependencies between moment features.
+*/
+
+#ifndef __FEATUREMOMENTDATABASE_H__
+#define __FEATUREMOMENTDATABASE_H__
+
+#include <visp3/core/vpConfig.h>
+#include <map>
+#include <iostream>
+#include <cstring>
+
+class vpFeatureMoment;
+class vpMomentObject;
+/*!
+ \class vpFeatureMomentDatabase
+
+ \ingroup group_visual_features
+
+ \brief This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
+
+ Like moments (implemented in vpMoment... classes), a vpFeatureMoment needs to have access to other vpFeatureMoment's values to be computed.
+ In most cases, a vpFeatureMoment needs both: vpMoments and vpFeatureMoments which explains the two databases (see vpFeatureMoment::vpFeatureMoment).
+ For example vpFeatureMomentAlpha needs additionnal information about centered moments vpMomentCentered AND their interaction matrices obtained
+ by vpFeatureMomentCentered in order to compute the moment's value from a vpMomentObject.
+ Like the vpMomentCentered is stored in a vpMomentDatabase, the vpFeatureMomentCentered should be stored in a vpFeatureMomentDatabase.
+
+ All moment features in a database can access each other freely at any time. They can also verify if a moment feature is present in the database or not.
+ This code illustrates the use of both databases to handle dependencies between moment primitives and moment features:
+ \code
+#include <visp3/core/vpPoint.h>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <iostream>
+#include <vector>
+
+int main()
+{
+ try{
+
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+
+ //////////////////////////////REFERENCE VALUES////////////////////////////////
+ vpMomentObject obj(6); // Init object of order 6 because we are
+ // computing C-invariants
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
+ obj.fromVector(vec_p);
+
+
+ vpMomentDatabase mdb; //database for moment primitives. This will
+ //only contain the basic moment.
+ vpMomentCentered mc; //Centered moment
+ vpMomentBasic bm; //Basic moment
+ vpMomentGravityCenter gc; //gravity center
+ vpMomentCInvariant ci; //C-type invariant
+
+ bm.linkTo(mdb); //add basic moment to moment database
+ mc.linkTo(mdb); //add centered moment to moment database
+ gc.linkTo(mdb); //add gravity center to moment database
+ ci.linkTo(mdb); //add C-invariant to moment database
+
+ vpFeatureMomentDatabase fmdb; //feature moment database to store
+ //feature dependencies
+
+ //Declare and link moments to database
+ vpFeatureMomentBasic fmb(mdb,0.,0.,1.,&fmdb); fmb.linkTo(fmdb);
+ vpFeatureMomentCentered fmc(mdb,0.,0.,1.,&fmdb); fmc.linkTo(fmdb);
+ vpFeatureMomentCInvariant fci(mdb,0.,0.,1.,&fmdb); fci.linkTo(fmdb);
+
+
+ //update the whole moment database
+ mdb.updateAll(obj);
+
+ //Compute moments in the correct order with the object
+ bm.compute();
+ gc.compute();
+ mc.compute();
+ ci.compute();
+
+ //update the whole feature moment database with a plane
+ fmb.update(0.,0.,1.);
+ fmc.update(0.,0.,1.);
+ fci.update(0.,0.,1.);
+
+ std::cout << fci.interaction(vpFeatureMomentCInvariant::selectC1()) << std::endl;
+ }catch(vpException e){
+ std::cout << e.getMessage() << std::endl;
+ }
+
+ return 0;
+}
+\endcode
+*/
+class VISP_EXPORT vpFeatureMomentDatabase{
+ private:
+ struct cmp_str {
+ 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<const char*,vpFeatureMoment*,cmp_str> featureMomentsDataBase;
+ void add(vpFeatureMoment& featureMoment,char* name);
+ public:
+ /*!
+ Default constructor.
+ */
+ vpFeatureMomentDatabase() : featureMomentsDataBase() {}
+ /*!
+ Virtual destructor that does nothing.
+ */
+ virtual ~vpFeatureMomentDatabase() {}
+ virtual void updateAll(double A=0.0, double B=0.0, double C=1.0);
+
+ vpFeatureMoment& get(const char* type, bool& found);
+
+ friend VISP_EXPORT std::ostream & operator<<(std::ostream& os, const vpFeatureMomentDatabase& m);
+ friend class vpFeatureMoment;
+};
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h
new file mode 100644
index 0000000..14c9ac7
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h
@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentGravityCenter.h
+ \brief Implementation of the interaction matrix computation for vpMomentGravityCenter.
+*/
+#ifndef __FEATUREMOMENTGRAVITYCENTER_H__
+#define __FEATUREMOMENTGRAVITYCENTER_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentGravityCenter
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
+
+ The interaction matrix for the is defined in \cite Tahri05z, equation (16).
+ It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
+ The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
+ The following code demonstrates a selection of \f$ L_{y_{g}} \f$:
+
+ \code
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentBasic.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <iostream>
+#include <vector>
+
+int main()
+{
+ vpPoint p;
+ std::vector<vpPoint> vec_p; // vector that contains the vertices
+
+ p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ vec_p.push_back(p);
+ p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ vec_p.push_back(p);
+
+ //////////////////////////////REFERENCE VALUES////////////////////////////////
+ vpMomentObject obj(2); // Init object of order 2 because we need
+ // vpFeatureMomentBasic of order 1 (for vpFeatureMomentGravityCenter) which
+ // implies third-order moment primitives
+ obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
+ obj.fromVector(vec_p);
+
+
+ vpMomentDatabase mdb; //database for moment primitives. This will
+ //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
+
+ bm.linkTo(mdb); //add basic moment to moment database
+ gc.linkTo(mdb); //add gravity center to moment database
+
+ vpFeatureMomentDatabase fmdb; //feature moment database to store
+ //feature dependencies
+
+ //Declare and link moments to database
+ vpFeatureMomentBasic fmb(mdb,0.,0.,1.,&fmdb); fmb.linkTo(fmdb);
+ vpFeatureMomentGravityCenter fgc(mdb,0.,0.,1.,&fmdb); fgc.linkTo(fmdb);
+
+ //update and compute the vpMomentBasic before computing vpMomentGravityCenter
+ bm.update(obj);
+ bm.compute();
+ //update and compute the vpMomentGravityCenter before computing vpFeatureMomentBasic
+ gc.update(obj);
+ gc.compute();
+
+ fmb.update(0.,0.,1.); //update the vpFeatureMoment with a plane
+ //configuration and compute interaction matrix
+
+ fgc.update(0.,0.,1.); //update the plane configuration for gravity
+ //center feature and compute it's associated matrix.
+
+ std::cout << fgc.interaction(1 << 1) << std::endl;
+
+ return 0;
+}
+
+ \endcode
+
+ This code produces the following output:
+ \code
+0 -1 1.5 3.5 -2.5 -1.5
+ \endcode
+
+ You can also use the shortcut selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
+
+ \code
+ task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
+ \endcode
+ This feature depends on:
+ - vpFeatureMomentBasic
+
+ Minimum vpMomentObject order needed to compute this feature: 2.
+*/
+class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+ */
+ vpFeatureMomentGravityCenter(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
+ : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
+ {}
+ void compute_interaction();
+ /*!
+ Associated moment name.
+ */
+ const char* momentName() const { return "vpMomentGravityCenter";}
+ /*!
+ Feature name.
+ */
+ const char* name() const { return "vpFeatureMomentGravityCenter";}
+
+ /*!
+ Shortcut selector for \f$x_g\f$.
+ */
+ static unsigned int selectXg(){ return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_g\f$.
+ */
+ static unsigned int selectYg(){ return 1 << 1; }
+
+};
+
+#else
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentGravityCenter
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
+
+ The interaction matrix for the is defined in \cite Tahri05z, equation (16).
+ It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
+ The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
+
+ You can use the selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
+
+ \code
+ task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
+ \endcode
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentGravityCenter
+
+ Minimum vpMomentObject order needed to compute this feature: 2.
+*/
+class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+ */
+ vpFeatureMomentGravityCenter(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
+ vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
+ {}
+ void compute_interaction();
+ /*!
+ Associated moment name.
+ */
+ const char* momentName() const { return "vpMomentGravityCenter";}
+ /*!
+ Feature name.
+ */
+ const char* name() const { return "vpFeatureMomentGravityCenter";}
+
+ /*!
+ Shortcut selector for \f$x_g\f$.
+ */
+ static unsigned int selectXg(){ return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_g\f$.
+ */
+ static unsigned int selectYg(){ return 1 << 1; }
+
+};
+
+#endif
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h
new file mode 100644
index 0000000..a9d37ba
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h
@@ -0,0 +1,251 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+/*!
+ \file vpFeatureMomentGravityCenterNormalized.h
+ \brief Implementation of the interaction matrix computation for vpMomentGravityCenterNormalized.
+*/
+#ifndef __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
+#define __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
+#include <visp3/visual_features/vpFeatureMoment.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentGravityCenterNormalized
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
+
+ The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equation (19).
+ To do so, one must derive it and obtain a combination of interaction matrices by using (1).
+ It allows to compute the interaction matrices for \f$ (x_n,y_n) \f$.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
+ The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
+ You can use these shortcut selectors as follows:
+
+ \code
+ task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
+ \endcode
+
+ The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
+
+ This feature is often used in moment-based visual servoing to control the planar translation parameters.
+
+ Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
+
+ This feature depends on:
+ - vpFeatureMomentGravityCenter
+ - vpMomentGravityCenter
+ - vpMomentAreaNormalized
+ - vpFeatureMomentAreaNormalized
+
+*/
+class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
+ : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
+ {}
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentGravityCenterNormalized";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
+
+ /*!
+ Shortcut selector for \f$x_n\f$.
+ */
+ static unsigned int selectXn(){ return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_n\f$.
+ */
+ static unsigned int selectYn(){ return 1 << 1; }
+
+};
+
+#else
+class vpMomentDatabase;
+/*!
+ \class vpFeatureMomentGravityCenterNormalized
+
+ \ingroup group_visual_features
+
+ \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
+
+ It computes the interaction matrices for \f$ (x_n,y_n) \f$.
+ The interaction matrix for the moment feature has the following expression:
+ - In the discrete case:
+ \f[
+ L_{x_n} =
+{
+ \left[ \begin {array}{c} -Ax_{{n}}\theta+ \left( x_{{n}}e_{{1,1}}-y_{
+{n}} \right) B-a_{{n}}C\\ \noalign{\medskip}Ax_{{n}}e_{{1,1}}+Bx_{{n}}
+\theta\\ \noalign{\medskip} \left( -a_{{n}}-w_{{y}} \right) A+Bw_{{x}}
+\\ \noalign{\medskip}a_{{n}}e_{{1,1}}{\it NA}+ \left( \eta_{{1,0}}e_{{
+1,1}}+\eta_{{0,1}}-e_{{2,1}}-x_{{g}}e_{{1,1}}+\eta_{{0,1}}\theta
+ \right) x_{{n}}+ \left( \eta_{{1,0}}-x_{{g}}\theta \right) y_{{n}}-{
+\frac {x_{{n}}\eta_{{0,3}}}{{\it NA}}}\\ \noalign{\medskip} \left( -1+
+\theta \right) a_{{n}}{\it NA}+ \left( e_{{1,2}}+x_{{g}}-\eta_{{0,1}}e
+_{{1,1}}-2\,\eta_{{1,0}}+e_{{3,0}}+ \left( -x_{{g}}+\eta_{{1,0}}
+ \right) \theta \right) x_{{n}}+e_{{1,1}}x_{{g}}y_{{n}}-a_{{n}}
+\\ \noalign{\medskip}y_{{n}}\end {array} \right]
+}^t
+
+ L_{y_n} =
+
+
+{
+ \left[ \begin {array}{c} \left( 1-\theta \right) y_{{n}}A+y_{{n}}e_{
+{1,1}}B\\ \noalign{\medskip} \left( -x_{{n}}+y_{{n}}e_{{1,1}} \right)
+A+ \left( -1+\theta \right) y_{{n}}B-a_{{n}}C\\ \noalign{\medskip}-Aw_
+{{y}}+ \left( -a_{{n}}+w_{{x}} \right) B\\ \noalign{\medskip}\theta\,a
+_{{n}}{\it NA}+ \left( -e_{{2,1}}+\eta_{{1,0}}e_{{1,1}}+\eta_{{0,1}}-x
+_{{g}}e_{{1,1}}+ \left( \eta_{{0,1}}-y_{{g}} \right) \theta \right) y_
+{{n}}+a_{{n}}-{\frac {y_{{n}}\eta_{{0,3}}}{{\it NA}}}
+\\ \noalign{\medskip}-a_{{n}}e_{{1,1}}{\it NA}-x_{{n}}\eta_{{0,1}}+
+ \left( e_{{1,2}}+y_{{g}}e_{{1,1}}-\eta_{{0,1}}e_{{1,1}}+x_{{g}}+e_{{3
+,0}}-2\,\eta_{{1,0}}+ \left( -x_{{g}}+\eta_{{1,0}} \right) \theta
+ \right) y_{{n}}\\ \noalign{\medskip}-x_{{n}}\end {array} \right]
+
+}^t
+ \f]
+ - In the dense case:
+ \f[
+ L_{x_n} =
+{
+\left[ \begin {array}{c} -a_{{n}}C-1/2\,Ax_{{n}}-By_{{n}}
+\\ \noalign{\medskip}1/2\,Bx_{{n}}\\ \noalign{\medskip} \left( -a_{{n}
+}-w_{{y}} \right) A+Bw_{{x}}\\ \noalign{\medskip} \left( 4\,\eta_{{1,0
+}}-1/2\,x_{{g}} \right) y_{{n}}+4\,a_{{n}}\eta_{{1,1}}+4\,x_{{n}}\eta_
+{{0,1}}\\ \noalign{\medskip} \left( -4\,\eta_{{1,0}}+1/2\,x_{{g}}
+ \right) x_{{n}}+ \left( -1-4\,\eta_{{2,0}} \right) a_{{n}}
+\\ \noalign{\medskip}y_{{n}}\end {array} \right]
+
+
+}^t
+
+ L_{y_n} =
+{
+ \left[ \begin {array}{c} 1/2\,Ay_{{n}}\\ \noalign{\medskip}-1/2\,By_{
+{n}}-a_{{n}}C-Ax_{{n}}\\ \noalign{\medskip}-Aw_{{y}}+ \left( -a_{{n}}+
+w_{{x}} \right) B\\ \noalign{\medskip}4\,\theta\,a_{{n}}{\it NA}+
+ \left( 4\,\eta_{{0,1}}-1/2\,y_{{g}} \right) y_{{n}}+a_{{n}}
+\\ \noalign{\medskip} \left( -4\,\eta_{{1,0}}+1/2\,x_{{g}} \right) y_{
+{n}}-4\,a_{{n}}\eta_{{1,1}}-4\,x_{{n}}\eta_{{0,1}}
+\\ \noalign{\medskip}-x_{{n}}\end {array} \right]
+
+}^t
+ \f]
+with:
+ - \f$e_{i,j}=\frac{\mu_{i,j}}{NA}\f$
+ - \f$NA=\mu_{2,0}+\mu_{0,2}\f$
+ - \f$\theta=\frac{\eta_{0,2}}{NA}\f$
+ - \f$\eta\f$ is the centered and normalized moment.
+
+ These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
+ The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
+ You can use these shortcut selectors as follows:
+
+ \code
+ task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
+ \endcode
+
+ The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
+
+ This feature is often used in moment-based visual servoing to control the planar translation parameters.
+
+ Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
+
+ This feature depends on:
+ - vpFeatureMomentGravityCenter
+ - vpMomentGravityCenter
+ - vpMomentAreaNormalized
+ - vpFeatureMomentAreaNormalized
+
+*/
+class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
+ public:
+ /*!
+ Initializes the feature with information about the database of moment primitives, the object plane and feature database.
+ \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
+ \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param featureMoments : Feature database.
+
+ */
+ vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,
+ vpFeatureMomentDatabase* featureMoments=NULL)
+ : vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
+ {}
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char* momentName() const { return "vpMomentGravityCenterNormalized";}
+ /*!
+ feature name
+ */
+ const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
+
+ /*!
+ Shortcut selector for \f$x_n\f$.
+ */
+ static unsigned int selectXn(){ return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_n\f$.
+ */
+ static unsigned int selectYn(){ return 1 << 1; }
+
+};
+#endif
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h
new file mode 100644
index 0000000..7cf837b
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h
@@ -0,0 +1,269 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeaturePoint_H
+#define vpFeaturePoint_H
+
+/*!
+ \file vpFeaturePoint.h
+ \brief Class that defines 2D point visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpPoint.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeaturePoint
+ \ingroup group_visual_features
+
+ \brief Class that defines a 2D point visual feature \f$ s\f$ which
+ is composed by two parameters that are the cartesian coordinates \f$
+ x \f$ and \f$ y \f$.
+
+ In this class \f$ x \f$ and \f$ y \f$ are the 2D coordinates in the
+ image plane and are given in meter. \f$ Z \f$ which is the 3D
+ coordinate representing the depth is also a parameter of the
+ point. It is needed during the computation of the interaction matrix
+ \f$ L \f$.
+
+ The visual features can be set easily from an instance of the
+ classes vpPoint, vpDot or vpDot2. For more precision see the
+ vpFeatureBuilder class.
+
+ Once the values of the visual features are set, the interaction()
+ method allows to compute the interaction matrix \f$ L \f$ associated
+ to the visual feature, while the error() method computes the error
+ vector \f$(s - s^*)\f$ between the current visual feature and the
+ desired one.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 2D point feature \f$(x,y)\f$ that correspond to the 2D
+ coordinates of a point in the image plane. To control six degrees
+ of freedom, at least four other features must be considered like two
+ other point features for example. First we create a current
+ (\f$s\f$) 2D point feature. Then we set the task to use the
+ interaction matrix associated to the current feature \f$L_s\f$. And
+ finally we compute the camera velocity \f$v=-\lambda \; L_s^+ \;
+ (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
+ loop.
+
+ \code
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpFeaturePoint sd; //The desired point feature.
+ //Set the desired features x and y
+ double xd = 0;
+ double yd = 0;
+ //Set the depth of the point in the camera frame.
+ double Zd = 1;
+ //Set the point feature thanks to the desired parameters.
+ sd.buildFrom(xd, yd, Zd);
+
+ vpFeaturePoint s; //The current point feature.
+ //Set the current features x and y
+ double x; //You have to compute the value of x.
+ double y; //You have to compute the value of y.
+ double Z; //You have to compute the value of Z.
+ //Set the point feature thanks to the current parameters.
+ s.buildFrom(x, y, Z);
+ //In this case the parameter Z is not necessary because the interaction matrix is computed
+ //with the desired visual feature.
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the desired visual features sd
+ task.setInteractionMatrixType(vpServo::DESIRED);
+
+ // Add the 2D point feature to the task
+ task.addFeature(s, sd);
+
+ // Control loop
+ for ( ; ; ) {
+ // The new parameters x and y must be computed here.
+
+ // Update the current point visual feature
+ s.buildFrom(x, y, Z);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+ return 0;
+}
+ \endcode
+
+ If you want to build your own control law, this other example shows how
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
+ build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpFeaturePoint sd; //The desired point feature.
+ //Set the desired features x and y
+ double xd = 0;
+ double yd = 0;
+ //Set the depth of the point in the camera frame.
+ double Zd = 1;
+ //Set the point feature thanks to the desired parameters.
+ sd.buildFrom(xd, yd, Zd);
+
+ vpFeaturePoint s; //The current point feature.
+ //Set the current features x and y
+ double x; //You have to compute the value of x.
+ double y; //You have to compute the value of y.
+ double Z; //You have to compute the value of Z.
+ //Set the point feature thanks to the current parameters.
+ s.buildFrom(x, y, Z);
+
+ // Compute the interaction matrix L_s for the current point feature
+ vpMatrix L = s.interaction();
+ // You can also compute the interaction matrix L_s for the desired point feature
+ // The corresponding line of code is : vpMatrix L = sd.interaction();
+
+ // Compute the error vector (s-sd) for the point feature
+ s.error(s_star);
+}
+ \endcode
+
+ An other fully explained example is given in the \ref tutorial-ibvs.
+
+*/
+
+
+class VISP_EXPORT vpFeaturePoint : public vpBasicFeature
+{
+private:
+ //! FeaturePoint depth (required to compute the interaction matrix)
+ //! default Z = 1m
+ double Z ;
+
+public:
+
+ void init() ;
+
+ vpFeaturePoint() ;
+ //! Destructor.
+ virtual ~vpFeaturePoint() {}
+
+
+ /*
+ section Set coordinates
+ */
+
+ void buildFrom(const double x, const double y, const double Z) ;
+
+ void set_x(const double x) ;
+
+ void set_y(const double y) ;
+
+ void set_Z(const double Z) ;
+
+ void set_xyZ(const double x, const double y, const double Z) ;
+
+ double get_x() const ;
+
+ double get_y() const ;
+
+ double get_Z() const ;
+
+
+ /*
+ vpBasicFeature method instantiation
+ */
+
+ // feature selection
+ static unsigned int selectX();
+ static unsigned int selectY();
+
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ vpFeaturePoint *duplicate() const ;
+
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+
+ /*!
+ @name Deprecated functions
+ */
+ typedef enum
+ {
+ X = 1, // x coordinates
+ Y = 2 // y coordinates
+ } vpFeaturePointType;
+ //! Compute the error between a visual features and zero
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h
new file mode 100644
index 0000000..5af21a6
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h
@@ -0,0 +1,287 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 3D point visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeaturePoint3d_H
+#define vpFeaturePoint3d_H
+
+/*!
+ \file vpFeaturePoint3D.h
+ \brief class that defines the 3D point visual feature.
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpPoint.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+/*!
+ \class vpFeaturePoint3D
+ \ingroup group_visual_features
+ \brief Class that defines the 3D point visual feature.
+
+ A 3D point visual feature corresponds to a 3D point with \f$
+ {\bf X} = (X,Y,Z)\f$ coordinates in the camera frame.
+
+ This class is intended to manipulate the 3D point visual feature
+ \f$ s = (X,Y,Z) \f$. The interaction matrix related to \f$ s \f$ is given by:
+ \f[
+ L = \left[
+ \begin{array}{rrrrrr}
+ -1 & 0 & 0 & 0 & -Z & Y \\
+ 0 & -1 & 0 & Z & 0 & -X \\
+ 0 & 0 & -1 & -Y & X & 0 \\
+ \end{array}
+ \right]
+ \f]
+
+ Two ways are allowed to initialize the feature.
+
+ - The first way by setting the feature values \f$(X,Y,Z)\f$ using
+ vpFeaturePoint3D member fonctions like set_X(), set_Y(), set_Z(),
+ or also buildFrom().
+
+ - The second by using the feature builder functionalities to
+ initialize the feature from a point structure like
+ vpFeatureBuilder::create (vpFeaturePoint3D &, const vpPoint &).
+
+ The interaction() method allows to compute the interaction matrix
+ \f$ L\f$ associated to the 3D point visual feature, while the
+ error() method computes the error vector \f$ (s - s^*)\f$ between the
+ current visual feature and the desired one.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 3D point feature \f$(X,Y,Z)\f$ that correspond to the
+ 3D point coordinates in the camera frame. To control six degrees of
+ freedom, at least three other features must be considered like
+ vpFeatureThetaU visual features. First we create a current (\f$s\f$)
+ and desired (\f$s^*\f$) 3D point feature, set the task to use the
+ interaction matrix associated to the desired feature \f$L_{s^*}\f$
+ and than compute the camera velocity \f$v=-\lambda \; {L_{s^*}}^+ \;
+ (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
+ loop while \f$s^*\f$ is set to \f$Z^*=1\f$.
+
+ \code
+#include <iostream>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ // Set the 3D point coordinates in the object frame: oP
+ vpPoint point(0.1, -0.1, 0);
+
+ vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
+ cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
+ // ... cMo need here to be computed from a pose estimation
+
+ point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
+
+ // Creation of the current feature s
+ vpFeaturePoint3D s;
+ s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame: s=(X,Y,Z)
+ s.print();
+
+ // Creation of the desired feature s*.
+ vpFeaturePoint3D s_star;
+ s_star.buildFrom(0, 0, 1); // Z*=1 meter
+ s_star.print();
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the desired visual features s*
+ task.setInteractionMatrixType(vpServo::DESIRED);
+ // Set the constant gain
+ double lambda = 0.8;
+ task.setLambda(lambda);
+
+ // Add the 3D point feature to the task
+ task.addFeature(s, s_star);
+
+ // Control loop
+ for ( ; ; ) {
+ // ... cMo need here to be estimated from for example a pose estimation.
+ point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
+
+ // Update the current 3D point visual feature
+ s.buildFrom(point);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+}
+ \endcode
+
+ If you want to deal only with the \f$(X,Y)\f$ subset feature from the 3D
+ point feature, you have just to modify the addFeature() call in
+ the previous example by the following line. In that case, the dimension
+ of \f$s\f$ is two.
+
+ \code
+ // Add the (X,Y) subset feature from the 3D point visual feature to the task
+ task.addFeature(s, s_star, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
+ \endcode
+
+ If you want to build your own control law, this other example shows
+ how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
+ point visual feature, compute the corresponding error
+ vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <iostream>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ // Set the 3D point coordinates in the object frame: oP
+ vpPoint point(0.1, -0.1, 0);
+
+ vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
+ cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
+ // ... cMo need here to be computed from a pose estimation
+
+ point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
+
+ // Creation of the current feature s
+ vpFeaturePoint3D s;
+ s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame
+ s.print();
+
+ // Creation of the desired feature s*.
+ vpFeaturePoint3D s_star;
+ s_star.buildFrom(0, 0, 1); // Z*=1 meter
+ s_star.print();
+
+ // Compute the L_s interaction matrix associated to the current feature
+ vpMatrix L = s.interaction();
+ std::cout << "L: " << L << std::endl;
+
+ // Compute the error vector (s-s*) for the 3D point feature
+ vpColVector e = s.error(s_star); // e = (s-s*)
+
+ std::cout << "e: " << e << std::endl;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpFeaturePoint3D : public vpBasicFeature
+
+{
+
+public:
+ // basic construction
+ void init() ;
+ // basic constructor
+ vpFeaturePoint3D() ;
+ //! Destructor. Does nothing.
+ virtual ~vpFeaturePoint3D() {}
+
+ /*
+ /section Set coordinates
+ */
+
+ // build feature from a point (vpPoint)
+ void buildFrom(const vpPoint &p) ;
+ // set the point XY and Z-coordinates
+ void buildFrom(const double X, const double Y, const double Z) ;
+
+ // set the point X-coordinates
+ void set_X(const double X) ;
+ // set the point Y-coordinates
+ void set_Y(const double Y) ;
+ // set the point depth (camera frame)
+ void set_Z(const double Z) ;
+ // set the point XY and Z-coordinates
+ void set_XYZ(const double X, const double Y, const double Z) ;
+
+ // get the point X-coordinates
+ double get_X() const ;
+ // get the point Y-coordinates
+ double get_Y() const ;
+ // get the point depth (camera frame)
+ double get_Z() const ;
+
+
+ /*
+ vpBasicFeature method instantiation
+ */
+
+ static unsigned int selectX();
+ static unsigned int selectY();
+ static unsigned int selectZ();
+
+ // compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ // compute the error between two visual features from a subset
+ // a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ // print the name of the feature
+ void print(const unsigned int select=FEATURE_ALL ) const ;
+
+ // feature duplication
+ vpFeaturePoint3D *duplicate() const ;
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h
new file mode 100644
index 0000000..9cfb67d
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h
@@ -0,0 +1,342 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point with polar coordinates visual feature.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeaturePointPolar_H
+#define vpFeaturePointPolar_H
+
+/*!
+ \file vpFeaturePointPolar.h
+ \brief Class that defines a 2D point visual feature with polar coordinates.
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpPoint.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeaturePointPolar
+ \ingroup group_visual_features
+
+ \brief Class that defines 2D image point visual feature with
+ polar coordinates \f$(\rho,\theta)\f$ described in \cite Corke09a.
+
+ Let us denote \f$(\rho,\theta)\f$ the polar coordinates of an image
+ point, with \f$\rho\f$ the radius of the feature point with respect
+ to the optical center and \f$\theta\f$ the angle. From cartesian
+ coordinates \f$(x,y)\f$ of a image point, polar coordinates are
+ obtained by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ From polar coordinates, cartesian coordinates of the feature point
+ can be obtained by:
+
+ \f[x = \rho \cos\theta \hbox{,}\; \; y = \rho \sin\theta\f]
+
+ This class is intended to manipulate the 2D image point visual
+ feature in polar coordinates \f$ s = (\rho, \theta) \f$. The
+ interaction matrix related to \f$ s \f$ is given by:
+
+ \f[
+ L = \left[
+ \begin{array}{l}
+ L_{\rho} \\
+ \; \\
+ L_{\theta}\\
+ \end{array}
+ \right]
+ =
+ \left[
+ \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]
+
+ where \f$Z\f$ is the 3D depth of the considered point in the camera frame.
+
+ Two ways are allowed to initialize the feature.
+
+ - The first way by setting the feature values \f$(\rho,\theta,Z)\f$
+ using vpFeaturePointPolar members like set_rho(), set_theta(),
+ set_Z(), or set_rhoThetaZ(), or also buildFrom().
+
+ - The second way by using the feature builder functionalities to
+ initialize the feature from a dot tracker, like
+ vpFeatureBuilder::create (vpFeaturePointPolar &, const
+ vpCameraParameters &, const vpDot &) or vpFeatureBuilder::create
+ (vpFeaturePointPolar &, const vpCameraParameters &, const vpDot2
+ &). Be aware, that in that case only \f$(\rho,\theta)\f$ are
+ initialized. You may also initialize the 3D depth \f$Z\f$. It is
+ also possible to initialize the feature from a point structure,
+ like vpFeatureBuilder::create(vpFeaturePointPolar &, const vpPoint
+ &) or vpFeatureBuilder::create(vpFeaturePointPolar &, const
+ vpCameraParameters &, const vpCameraParameters &, const vpPoint
+ &). In that case all the feature parameters \f$(\rho,\theta,Z)\f$
+ would be initialized.
+
+ The interaction() method allows to compute the interaction matrix
+ \f$L\f$ associated to the visual feature, while the error() method
+ computes the error vector \f$(s - s^*)\f$ between the current visual
+ feature and the desired one.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using four 2D point features with polar coordinates. First we
+ create four current features \f$s\f$ (p var name in the code) and
+ four desired \f$s^*\f$ (pd var name in the code) point features with
+ polar coordinates, set the task to use the interaction matrix
+ associated to the current feature \f$L_{s}\f$ and than compute the
+ camera velocity \f$v=-\lambda \; {L_{s}}^+ \; (s-s^*)\f$. The
+ current feature \f$s\f$ is updated in the while() loop, while
+ \f$s^*\f$ is initialized at the beginning.
+
+ \code
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+
+ // Create 4 points to specify the object of interest
+ vpPoint point[4];
+
+ // Set the 3D point coordinates in the object frame: oP
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates( 0.1, -0.1, 0);
+ point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
+
+ // Initialize the desired pose between the camera and the object frame
+ vpHomogeneousMatrix cMod;
+ cMod.buildFrom(0, 0, 1, 0, 0, 0);
+
+ // Compute the desired position of the point
+ for (int i = 0 ; i < 4 ; i++) {
+ // Compute the 3D point coordinates in the camera frame cP = cMod * oP
+ point[i].changeFrame(cMod);
+ // Compute the perspective projection to set (x,y)
+ point[i].projection();
+ }
+
+ // Create 4 desired visual features as 2D points with polar coordinates
+ vpFeaturePointPolar pd[4];
+ // Initialize the desired visual feature from the desired point positions
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
+
+ // Initialize the current pose between the camera and the object frame
+ vpHomogeneousMatrix cMo;
+ cMo.buildFrom(0, 0, 1.2, 0, 0, M_PI);
+ // ... cMo need here to be computed from a pose estimation
+
+ for (int i = 0 ; i < 4 ; i++) {
+ // Compute the 3D point coordinates in the camera frame cP = cMo * oP
+ point[i].changeFrame(cMo);
+ // Compute the perspective projection to set (x,y)
+ point[i].projection();
+ }
+ // Create 4 current visual features as 2D points with polar coordinates
+ vpFeaturePointPolar p[4];
+ // Initialize the current visual feature from the current point positions
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(p[i], point[i]);
+
+ // Visual servo task initialization
+ vpServo task;
+ // - Camera is monted on the robot end-effector and velocities are
+ // computed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // - Interaction matrix is computed with the current visual features s
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ // - Set the contant gain to 1
+ task.setLambda(1);
+ // - Add current and desired features
+ for (int i = 0 ; i < 4 ; i++)
+ task.addFeature(p[i], pd[i]);
+
+ // Control loop
+ for ( ; ; ) {
+ // ... cMo need here to be estimated from for example a pose estimation.
+ // Computes the point coordinates in the camera frame and its 2D
+ // coordinates in the image plane
+ for (int i = 0 ; i < 4 ; i++)
+ point[i].track(cMo) ;
+
+ // Update the current 2D point visual feature with polar coordinates
+ for (int i = 0 ; i < 4 ; i++)
+ vpFeatureBuilder::create(p[i], point[i]);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+
+ task.kill();
+}
+ \endcode
+
+ If you want to deal only with the \f$\rho\f$ subset feature from the 2D
+ point feature set, you have just to modify the addFeature() call in the
+ previous example by the following line. In that case, the dimension of
+ \f$s\f$ is four.
+
+ \code
+ // Add the rho subset feature from the 2D point polar coordinates visual features
+ task.addFeature(p[i], pd[i], vpFeaturePointPolar::selectRho());
+ \endcode
+
+ If you want to build your own control law, this other example shows how
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature with polar coordinates, compute the corresponding error vector
+ \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ // Creation of the current feature s
+ vpFeaturePointPolar s;
+ // Initialize the current feature
+ s.buildFrom(0.1, M_PI, 1); // rho=0.1m, theta=pi, Z=1m
+
+ // Creation of the desired feature s
+ vpFeaturePointPolar s_star;
+ // Initialize the desired feature
+ s.buildFrom(0.15, 0, 0.8); // rho=0.15m, theta=0, Z=0.8m
+
+ // Compute the interaction matrix L_s for the current feature
+ vpMatrix L = s.interaction();
+
+ // Compute the error vector (s-s*) for the point feature with polar coordinates
+ s.error(s_star);
+
+ return 0;
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpFeaturePointPolar : public vpBasicFeature
+{
+private:
+ //! FeaturePoint depth (required to compute the interaction matrix)
+ //! default Z = 1m
+ double Z ;
+
+public:
+ // basic construction
+ void init() ;
+ // basic constructor
+ vpFeaturePointPolar() ;
+ //! Destructor. Does nothing.
+ virtual ~vpFeaturePointPolar() { }
+
+ void buildFrom(const double rho, const double theta, const double Z) ;
+
+ /*
+ Set coordinates
+ */
+
+ // set the point rho-coordinates
+ void set_rho(const double rho) ;
+ // set the point theta-coordinates
+ void set_theta(const double theta) ;
+ // set the point depth (camera frame)
+ void set_Z(const double Z) ;
+ // set the point rho, theta polar coordinates and Z coordinate
+ void set_rhoThetaZ(const double rho, const double theta, const double Z) ;
+
+ // get the point rho-coordinates
+ double get_rho() const ;
+ // get the point theta-coordinates
+ double get_theta() const ;
+ // get the point depth (camera frame)
+ double get_Z() const ;
+
+ /*
+ vpBasicFeature method instantiation
+ */
+ static unsigned int selectRho();
+ static unsigned int selectTheta();
+
+ // compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ // compute the error between two visual features from a subset
+ // a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ // print the name of the feature
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ // feature duplication
+ vpFeaturePointPolar *duplicate() const ;
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+ /*!
+ @name Deprecated functions
+ */
+ //! compute the error between a visual features and zero
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h
new file mode 100644
index 0000000..f8ba092
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h
@@ -0,0 +1,302 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Segment visual feature.
+ *
+ * Authors:
+ * Filip Novotny
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureSegment_H
+#define vpFeatureSegment_H
+
+/*!
+ \file vpFeatureSegment.h
+ \brief class that defines the Segment visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+/*!
+ \class vpFeatureSegment
+ \ingroup group_visual_features
+
+ \brief Class that defines a 2D segment visual features.
+ This class allow to consider two sets of visual features:
+ - the non normalised features \f${\bf s} = (x_c, y_c, l, \alpha)\f$ where \f$(x_c,y_c)\f$
+ are the coordinates of the segment center, \f$ l \f$ the segment length
+ and \f$ \alpha \f$ the orientation of the segment with respect to the \f$ x \f$ axis.
+ - or the normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ with \f$x_n = x_c/l\f$,
+ \f$y_n = y_c/l\f$ and \f$l_n = 1/l\f$.
+
+
+
+ The selection of the feature set is done either during construction using vpFeatureSegment(bool),
+ or by setNormalized(bool).
+
+*/
+class VISP_EXPORT vpFeatureSegment : public vpBasicFeature
+{
+public:
+ //empty constructor
+ vpFeatureSegment(bool normalized=false);
+
+ //! Destructor. Does nothing.
+ ~vpFeatureSegment() {}
+ // change values of the segment
+ void buildFrom(const double x1, const double y1, const double Z1, const double x2, const double y2, const double Z2);
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ //! Feature duplication.
+ vpFeatureSegment *duplicate() const ;
+ // compute the error between two visual features from a subset
+ // a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+
+ /*!
+ Get the x coordinate of the segment center in the image plane.
+
+ \return If normalized features are used, return \f$ x_n = x_c / l \f$. Otherwise return \f$ x_c \f$.
+ */
+ inline double getXc() const { return s[0] ; }
+
+ /*!
+ Get the y coordinate of the segment center in the image plane.
+
+ \return If normalized features are used, return \f$ y_n = y_c / l \f$. Otherwise return \f$ y_c \f$.
+ */
+ inline double getYc() const { return s[1] ; }
+
+ /*!
+ Get the length of the segment.
+
+ \return If normalized features are used, return \f$ l_n = 1 / l \f$. Otherwise return \f$ l \f$.
+
+ */
+ inline double getL() const { return s[2] ; }
+
+ /*!
+ Get the value of \f$ \alpha \f$ which represents the orientation of the segment.
+
+ \return The value of \f$ \alpha \f$.
+ */
+ inline double getAlpha() const { return s[3] ;}
+
+ /*!
+ Get the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
+ of the 3D point that corresponds to the segment first point.
+
+ \return The value of the depth \f$ Z_1 \f$.
+ */
+ inline double getZ1() const { return Z1_ ;}
+
+ /*!
+ Get the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
+ of the 3D point that corresponds to the segment second point.
+
+ \return The value of the depth \f$ Z_2 \f$.
+ */
+ inline double getZ2() const { return Z2_ ;}
+
+ // Basic construction.
+ void init() ;
+
+ // compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+
+ void print(const unsigned int select= FEATURE_ALL) const ;
+
+ /*!
+ Indicates if the normalized features are considered.
+ */
+ bool isNormalized() { return normalized_; };
+
+ static unsigned int selectXc();
+ static unsigned int selectYc();
+ static unsigned int selectL();
+ static unsigned int selectAlpha();
+
+ /*!
+ Set the king of feature to consider.
+ \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
+ If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
+ */
+ void setNormalized(bool normalized) { normalized_ = normalized; };
+ /*!
+
+ Set the value of the x coordinate of the segment center
+ in the image plane. It is one parameter of the visual feature \f$ s \f$.
+
+ \param val : Value to set, that is either equal to \f$ x_n = x_c/l \f$ when normalized features
+ are considered, or equal to \f$ x_c \f$ otherwise.
+ */
+ inline void setXc(const double val){
+ s[0] = xc_ = val;
+ flags[0] = true;
+ }
+ /*!
+
+ Set the value of the y coordinate of the segment center
+ in the image plane. It is one parameter of the visual feature \f$ s \f$.
+
+ \param val : Value to set, that is either equal to \f$ y_n = y_c/l \f$ when normalized features
+ are considered, or equal to \f$ y_c \f$ otherwise.
+ */
+ inline void setYc(const double val){
+ s[1] = yc_ = val;
+ flags[1] = true;
+ }
+ /*!
+
+ Set the value of the segment length in the image plane. It is one parameter of the visual feature \f$ s \f$.
+
+ \param val : Value to set, that is either equal to \f$l_n= 1/l \f$ when normalized features
+ are considered, or equal to \f$ l \f$ otherwise.
+ */
+ inline void setL(const double val){
+ s[2] = l_ = val;
+ flags[2] = true;
+ }
+ /*!
+
+ Set the value of \f$ \alpha \f$ which represents the orientation of the segment
+ in the image plane. It is one parameter of the visual feature \f$ s \f$.
+
+ \param val : \f$ \alpha \f$ value to set.
+ */
+ inline void setAlpha(const double val){
+ s[3] = alpha_ = val;
+ cos_a_ = cos(val);
+ sin_a_ = sin(val);
+ flags[3] = true;
+ }
+
+ /*!
+
+ Set the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
+ of the 3D point that corresponds to the segment first point.
+
+ This value is requested to compute the interaction matrix.
+
+ \param val : \f$ Z_1 \f$ value to set.
+
+ \exception vpFeatureException::badInitializationError : If Z1 is behind the camera or equal to zero.
+ */
+ inline void setZ1(const double val)
+ {
+ Z1_ = val;
+
+ if (Z1_ < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z1 = " << Z1_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z1 is behind the camera ")) ;
+ }
+
+ if (fabs(Z1_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z1 coordinates is null ") ;
+ std::cout <<"Z1 = " << Z1_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z1 coordinates is null")) ;
+ }
+
+ flags[4] = true;
+ }
+
+ /*!
+
+ Set the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
+ of the 3D point that corresponds to the segment second point.
+
+ This value is requested to compute the interaction matrix.
+
+ \param val : \f$ Z_2 \f$ value to set.
+
+ \exception vpFeatureException::badInitializationError : If Z2 is behind the camera or equal to zero.
+ */
+ inline void setZ2(const double val)
+ {
+ Z2_ = val;
+
+ if (Z2_ < 0)
+ {
+ vpERROR_TRACE("Point Z2 is behind the camera ") ;
+ std::cout <<"Z2 = " << Z2_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z2 is behind the camera ")) ;
+ }
+
+ if (fabs(Z2_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z2 coordinates is null ") ;
+ std::cout <<"Z2 = " << Z2_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z2 coordinates is null")) ;
+ }
+
+ flags[5] = true;
+ }
+
+
+private:
+ double xc_;
+ double yc_;
+ double l_;
+ double alpha_;
+ double Z1_;
+ double Z2_;
+ double cos_a_;
+ double sin_a_;
+ bool normalized_;
+} ;
+
+#endif
+
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h
new file mode 100644
index 0000000..0f9b365
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h
@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * ThetaU visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureThetaU_H
+#define vpFeatureThetaU_H
+
+/*!
+ \file vpFeatureThetaU.h
+ \brief class that defines the ThetaU visual feature
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeatureThetaU
+ \ingroup group_visual_features
+
+ \brief Class that defines a 3D visual feature \f$ s\f$ from a \f$ \theta
+ u \f$ axis/angle parametrization that represent the rotation between
+ to frames.
+
+ Let us denote \f$ \theta u = (\theta u_x, \theta u_y, \theta u_z)\f$ .
+
+ It is convenient to consider two coordinate frames: the current
+ camera frame \f$ {\cal{F}}_c \f$ and the desired camera frame \f$
+ {\cal{F}}_{c^*} \f$.
+
+ Let \f$^{c^*}R_c \f$ be the rotation matrix that gives the
+ orientation of the current camera frame relative to the desired camera
+ frame. Let \f$ \theta u_{^{c^*}R_c} \f$ to corresponding axis/angle
+ representation of this rotation.
+
+ Furthermore, let \f$^{c}R_{c^*} \f$ the rotation matrix that gives the
+ orientation of the desired camera frame relative to the current
+ camera frame. Let \f$ \theta u_{^{c}R_{c^*}} \f$ to corresponding
+ axis/angle representation of this rotation.
+
+ This class can be used to manipulate two kind of visual features:
+
+ - \f$ s = \theta u_{^{c^*}R_c} \f$ if the orientation of current
+ camera frame relative to the desired frame has to be
+ considered. The desired visual feature \f$ s^* \f$ is equal to
+ zero. The corresponding error is than equal to \f$ e=(s-s^*) =
+ \theta u_{^{c^*}R_c} \f$. In this case, the interaction matrix
+ related to \f$ s \f$ is given by \f[ L = \left[ \begin{array}{cc}
+ 0_3 & L_{\theta u} \end{array} \right] \f] with \f[
+ L_{\theta u} = I_3 + \frac{\theta}{2} \; [u]_\times +
+ \left(1 - \frac{sinc \theta}{sinc^2 \frac{\theta}{2}}\right)
+ [u]^2_\times \f] where \f$ 0_3 \f$ is a \f$ 3 \times 3 \f$ nul
+ matrix, \f$ I_3 \f$ is the \f$3 \times 3\f$ identity matrix, and
+ for more readability \f$ \theta \f$ and \f$ u \f$ respectively the
+ angle and the axis coordinates of the \f$ \theta u_{^{c^*}R_c} \f$
+ representation.
+
+ - \f$ s = \theta u_{^{c}R_{c^*}} \f$ if it is more the orientation
+ of the desired camera frame relative to the current frame that has
+ to be considered. The desired visual feature \f$ s^* \f$ is equal
+ to zero. The corresponding error is than equal to \f$e=(s-s^*) =
+ \theta u_{^{c}R_{c^*}} \f$. In this case, the interaction matrix
+ related to \f$ s \f$ is given by \f[ L = \left[ \begin{array}{cc}
+ 0_3 & L_{\theta u} \end{array} \right] \f] with \f[
+ L_{\theta u} = -I_3 + \frac{\theta}{2} \; [u]_\times
+ - \left(1 - \frac{sinc \theta}{sinc^2 \frac{\theta}{2}}\right)
+ [u]^2_\times \f] where \f$ 0_3 \f$ is a \f$ 3 \times 3 \f$ nul
+ matrix, \f$ I_3 \f$ is the \f$3 \times 3\f$ identity matrix, and
+ for more readability \f$ \theta \f$ and \f$ u \f$ respectively the
+ angle and the axis coordinates of the \f$ \theta u_{^{c}R_{c^*}}
+ \f$ representation.
+
+ The kind of visual feature is to set during the construction of the
+ vpFeatureThetaU() object by using the selector
+ vpFeatureThetaU::vpFeatureThetaURotationRepresentationType.
+
+ To initialize the feature \f$(\theta u_x, \theta u_y, \theta u_z)\f$
+ you may use vpFeatureThetaU member fonctions like set_TUx(),
+ set_TUy(), set_TUz(), or also buildFrom() fonctions.
+
+ Depending on the choice of the visual feature representation, the
+ interaction() method allows to compute the interaction matrix \f$
+ L \f$ associated to the visual feature, while the error()
+ method computes the error vector \f$(s - s^*)\f$ between the current
+ visual feature and the desired one.
+
+ To know more on the \f$ \theta u \f$ axis/angle representation for a
+ 3D rotation see the vpThetaUVector class.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 3D \f$\theta u\f$ feature \f$(\theta u_x,\theta u_y,
+ \theta u_z)\f$ that correspond to the 3D rotation between the
+ current camera frame and the desired camera frame. To control six
+ degrees of freedom, at least three other features must be considered
+ like vpFeatureTranslation visual features. First we create a current
+ (\f$s\f$) 3D \f$\theta u\f$ feature, than set the
+ task to use the interaction matrix associated to the current feature
+ \f$L_s\f$ and than compute the camera velocity \f$v=-\lambda \;
+ L_s^+ \; (s-s^*)\f$. The current feature \f$s\f$ is updated in the
+ while() loop while \f$s^*\f$ is considered as zero.
+
+ \code
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpHomogeneousMatrix cMcd;
+ // ... cMcd need here to be initialized from for example a pose estimation.
+
+ // Creation of the current feature s that correspond to the rotation
+ // in angle/axis parametrization between the current camera frame
+ // and the desired camera frame
+ vpFeatureThetaU s(vpFeatureThetaU::cRcd);
+ s.buildFrom(cMcd); // Initialization of the feature
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the current visual features s
+ task.setInteractionMatrixType(vpServo::CURRENT);
+
+ // Add the 3D ThetaU feature to the task
+ task.addFeature(s); // s* is here considered as zero
+
+ // Control loop
+ for ( ; ; ) {
+ // ... cMcd need here to be initialized from for example a pose estimation.
+
+ // Update the current ThetaU visual feature
+ s.buildFrom(cMcd);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+}
+ \endcode
+
+ If you want to deal only with the \f$(\theta u_x,\theta u_y)\f$ subset
+ feature from the 3D \f$\theta u\f$ , you have just to modify the
+ addFeature() call in the previous example by the following line. In
+ that case, the dimension of \f$s\f$ is two.
+
+ \code
+ // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
+ // rotation to the task
+ task.addFeature(s, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
+ \endcode
+
+ If you want to build your own control law, this other example shows
+ how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
+ \f$\theta u\f$ visual feature, compute the corresponding error
+ vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpHomogeneousMatrix cdMc;
+ // ... cdMc need here to be initialized from for example a pose estimation.
+
+ // Creation of the current feature s
+ vpFeatureThetaU s(vpFeatureThetaU::cdRc);
+ s.buildFrom(cdMc); // Initialization of the feature
+
+ // Creation of the desired feature s*. By default this feature is
+ // initialized to zero
+ vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
+
+ // Compute the interaction matrix L_s for the current ThetaU feature
+ vpMatrix L = s.interaction();
+
+ // Compute the error vector (s-s*) for the ThetaU feature
+ s.error(s_star);
+}
+ \endcode
+
+
+*/
+class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature
+{
+public:
+ typedef enum
+ {
+ TUx = 1, /*!< Select the subset \f$ \theta u_x \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ TUy = 2, /*!< Select the subset \f$ \theta u_y \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ TUz = 4 /*!< Select the subset \f$ \theta u_z \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ } vpFeatureThetaUType;
+ typedef enum
+ {
+ cdRc, /*!< Selector used to manipulate the visual feature \f$ s
+ = \theta u_{^{c^*}R_c} \f$. This visual feature
+ represent the orientation of the current camera frame
+ relative to the desired camera frame. */
+ cRcd /*!< Selector used to manipulate the visual feature \f$ s = \theta
+ u_{^{c}R_{c^*}} \f$. This visual feature
+ represent the orientation of the desired camera frame
+ relative to the current camera frame. */
+ } vpFeatureThetaURotationRepresentationType;
+ /*
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities are useful but not mandatory
+ */
+
+public:
+ // Basic construction.
+ void init() ;
+ // Basic constructor.
+ vpFeatureThetaU() ;
+ vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) ;
+ vpFeatureThetaU(vpThetaUVector &tu,
+ vpFeatureThetaURotationRepresentationType r) ;
+ vpFeatureThetaU(vpRotationMatrix &R,
+ vpFeatureThetaURotationRepresentationType r) ;
+ vpFeatureThetaU(vpHomogeneousMatrix &M,
+ vpFeatureThetaURotationRepresentationType r) ;
+ void buildFrom(vpThetaUVector &tu) ;
+ // build from a rotation matrix
+ void buildFrom(const vpRotationMatrix &R) ;
+ // build from an homogeneous matrix
+ void buildFrom(const vpHomogeneousMatrix &M) ;
+
+ //! Destructor. Does nothing.
+ virtual ~vpFeatureThetaU() {}
+
+public:
+
+ void set_TUx(const double tu_x) ;
+ void set_TUy(const double tu_y) ;
+ void set_TUz(const double tu_z) ;
+
+ void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r);
+
+ vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const;
+
+ double get_TUx() const;
+ double get_TUy() const;
+ double get_TUz() const;
+
+
+public:
+ /*
+ vpBasicFeature method instantiation
+ */
+ static unsigned int selectTUx();
+ static unsigned int selectTUy();
+ static unsigned int selectTUz();
+
+ // compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ // compute the error between two visual features from a subset
+ // a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ void print(const unsigned int select= FEATURE_ALL) const ;
+
+ //! Feature duplication.
+ vpFeatureThetaU *duplicate() const ;
+
+public:
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+ private:
+ vpFeatureThetaURotationRepresentationType rotation;
+} ;
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h
new file mode 100644
index 0000000..f411f5f
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h
@@ -0,0 +1,364 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 3D translation visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureTranslation_H
+#define vpFeatureTranslation_H
+
+/*!
+ \file vpFeatureTranslation.h
+ \brief class that defines the translation visual feature.
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+/*!
+ \class vpFeatureTranslation
+ \ingroup group_visual_features
+
+ \brief Class that defines the translation visual feature
+ \f$s=(t_x,t_y,t_z)\f$.
+
+ It is convenient to consider two coordinate frames noted here \f$ {\cal{F}}_1 \f$ and \f$
+ {\cal{F}}_{2} \f$.
+
+ Let \f$^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ be the homogeneous matrix that gives the
+ orientation and the translation of the frame \f$ {\cal{F}}_1 \f$ with respect to the frame \f$ {\cal{F}}_2 \f$.
+
+ \f[
+ ^{{\cal{F}}_2}M_{{\cal{F}}_1} = \left(\begin{array}{cc}
+ ^{{\cal{F}}_2}R_{{\cal{F}}_1} & ^{{\cal{F}}_2}t_{{\cal{F}}_1} \\
+ {\bf 0}_{1\times 3} & 1
+ \end{array}
+ \right)
+ \f]
+
+ with \f$^{{\cal{F}}_2}R_{{\cal{F}}_1} \f$ the rotation matrix that gives the orientation
+ of the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$ and
+ \f$^{{\cal{F}}_2}t_{{\cal{F}}_1} \f$ the translation vector that gives the position of
+ the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$. To
+ know more about homogeneous matrices see vpHomogeneousMatrix
+ documentation.
+
+ This class can be used to manipulate three kind of visual features:
+
+ - This class can be used to manipulate the translation visual feature
+ \f$s= ^{c^*}t_c\f$ which gives the position of
+ the current camera frame relative to the desired camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
+ visual feature \f$ s^* \f$ is equal to zero. The corresponding error
+ is than equal to \f$ e=(s-s^*) = ^{c^*}t_c \f$. In this case, the
+ interaction matrix related to \f$ s \f$ is given by \f[ L = [
+ ^{c^*}R_c \;\; 0_3] \f]
+
+ - This class can also be used to manipulate the translation visual feature
+ \f$s= ^{c}t_{c^*}\f$ which gives the position of
+ the desired camera frame relative to the current camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
+ visual feature \f$ s^* \f$ is equal to zero. The corresponding error
+ is than equal to \f$ e=(s-s^*) = ^{c}t_{c^*} \f$. In this case, the
+ interaction matrix related to \f$ s \f$ is given by \f[ L = [
+ -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
+
+ - Actually, this class can also be used to manipulate the
+ translation visual feature \f$s= ^{c}t_o\f$ which gives the position
+ of the object frame relative to the current camera frame. It is
+ composed by the three components \f$(t_x,t_y,t_z)\f$ too. The
+ desired visual feature \f$ s^* \f$ is the translation visual feature
+ \f$s^*= ^{c^*}t_o\f$ which gives the position of the object frame
+ relative to the desired camera frame. The corresponding error is
+ than equal to \f$ e=(s-s^*) = ^{c}t_o - ^{c^*}t_o \f$. In this case,
+ the interaction matrix related to \f$ s \f$ is given by \f[ L = [
+ -I_3 \;\; [^{c}t_o]_\times] \f]
+
+ To initialize the feature \f$(t_x, t_y, t_z)\f$ you may use member
+ fonctions like set_Tx(), set_Ty(), set_Tz(), or also buildFrom()
+ fonctions.
+
+ The interaction() method allows to compute the interaction matrix
+ \f$ L\f$ associated to the translation visual feature, while the
+ error() method computes the error vector \f$(s - s^*)\f$ between the
+ current visual feature and the desired one.
+
+ The code below shows how to create a eye-in hand visual servoing
+ task using a 3D translation feature \f$(t_x,t_y,t_z)\f$ that
+ correspond to the 3D translation between the desired camera frame
+ and the current camera frame. To control six degrees of freedom, at
+ least three other features must be considered like vpFeatureThetaU
+ visual features. First we create a current (\f$s\f$) and desired
+ (\f$s^*\f$) 3D translation feature, set the task to use the
+ interaction matrix associated to the current feature \f$L_s\f$ and
+ than compute the camera velocity \f$v=-\lambda \; L_s^+ \;
+ (s-s^*)\f$. The current feature \f$s\f$ is updated in the while() loop
+ while \f$s^*\f$ is set to zero.
+
+ \code
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpHomogeneousMatrix cdMc;
+ // ... cdMc need here to be initialized from for example a pose estimation.
+
+ // Creation of the current visual feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc);
+ s.buildFrom(cdMc); // Initialization of the current feature s=(tx,ty,tz)
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the current visual features s
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ // Set the constant gain
+ double lambda = 0.8;
+ task.setLambda(lambda);
+
+ // Add the 3D translation feature to the task
+ task.addFeature(s); // s* is here considered as zero
+
+ // Control loop
+ for ( ; ; ) {
+ // ... cdMc need here to be initialized from for example a pose estimation.
+
+ // Update the current 3D translation visual feature
+ s.buildFrom(cdMc);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+}
+ \endcode
+
+ If you want to deal only with the \f$(t_x,t_y)\f$ subset feature from the 3D
+ translation, you have just to modify the addFeature() call in
+ the previous example by the following line. In that case, the dimension
+ of \f$s\f$ is two.
+
+ \code
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(s, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ If you want to build your own control law, this other example shows
+ how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
+ translation visual feature, compute the corresponding error
+ vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+
+ \code
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpHomogeneousMatrix cdMc;
+ // ... cdMc need here to be initialized from for example a pose estimation.
+
+ // Creation of the current feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc);
+ s.buildFrom(cdMc); // Initialization of the feature
+
+ // Creation of the desired feature s*. By default this feature is
+ // initialized to zero
+ vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
+
+ // Compute the interaction matrix for the translation feature
+ vpMatrix L = s.interaction();
+
+ // Compute the error vector (s-s*) for the translation feature
+ vpColVector e = s.error(s_star); // e = (s-s*)
+}
+ \endcode
+
+ The code below shows how to create an eye-in hand visual servoing
+ task using a 3D translation feature \f$(t_x,t_y,t_z)\f$ that
+ correspond to the 3D translation between the current camera frame
+ and the object frame. Like with the previous examples, to
+ control six degrees of freedom, at least three other features must be
+ considered like vpFeatureThetaU visual features. The way to initialize
+ the visual features is quite the same as before. The difference is that
+ the cMo method must be precised and the desired feature is note
+ necessary equal to zero.
+
+ \code
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ vpHomogeneousMatrix cdMo;
+ // ... cdMo need here to be initialized from for example a pose estimation.
+
+ // Creation of the desired visual feature s*
+ vpFeatureTranslation s_star(vpFeatureTranslation::cMo);
+ s_star.buildFrom(cdMo); // Initialization of the desired feature s*=(tx*,ty*,tz*)
+
+ vpHomogeneousMatrix cMo;
+ // ... cMo need here to be computed.
+
+ // Creation of the current visual feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cMo);
+ s.buildFrom(cMo); // Initialization of the current feature s=(tx,ty,tz)
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the current visual features s
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ // Set the constant gain
+ double lambda = 0.8;
+ task.setLambda(lambda);
+
+ // Add the 3D translation feature to the task
+ task.addFeature(s, s_star); // s* is here considered as zero
+
+ // Control loop
+ for ( ; ; ) {
+ // ... cMo need here to be computed from for example a pose estimation.
+
+ // Update the current 3D translation visual feature
+ s.buildFrom(cMo);
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+}
+ \endcode
+
+*/
+class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature
+{
+public:
+
+ /*!
+ \enum vpFeatureTranslationRepresentationType
+ Kind of implemented 3D translation feature.
+ */
+ typedef enum {
+ /*! Selector used to manipulate the visual feature \f$s=
+ ^{c^*}t_c\f$ which gives the position of the current camera frame
+ relative to the desired camera frame.*/
+ cdMc,
+ /*! Selector used to manipulate the visual feature \f$s=
+ ^{c}t_{c^*}\f$ which gives the position of the desired camera frame
+ relative to the current camera frame.*/
+ cMcd,
+ /*! Selector used to manipulate the visual feature \f$s=
+ ^{c}t_o\f$ which gives the position of the object frame relative to
+ the current camera frame. */
+ cMo
+ } vpFeatureTranslationRepresentationType;
+
+ // basic construction
+ void init() ;
+ // basic contructor
+ vpFeatureTranslation() ;
+ // basic constructor specifying the type of translation feature
+ vpFeatureTranslation(vpFeatureTranslationRepresentationType r) ;
+ // constructor : build from an homogeneous matrix
+ // cdMc is the displacement that the camera has to realize
+ vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, vpFeatureTranslationRepresentationType r) ;
+ //! Destructor. Does nothing.
+ virtual ~vpFeatureTranslation() {}
+
+ // build from an homogeneous matrix
+ // cdMc is the displacement that the camera has to realize
+ void buildFrom(const vpHomogeneousMatrix &f2Mf1) ;
+
+ void set_Tx(const double t_x) ;
+ void set_Ty(const double t_y) ;
+ void set_Tz(const double t_z) ;
+
+ void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r);
+
+ vpFeatureTranslationRepresentationType getFeatureTranslationType() const;
+
+ double get_Tx() const ;
+ double get_Ty() const ;
+ double get_Tz() const ;
+
+ // feature selection
+ static unsigned int selectTx();
+ static unsigned int selectTy();
+ static unsigned int selectTz();
+
+ // compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ // compute the error between two visual features from a subset
+ // a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ // print the name of the feature
+ void print(const unsigned int select= FEATURE_ALL) const ;
+
+ //! Feature duplication
+ vpFeatureTranslation *duplicate() const ;
+
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+private:
+ //! displacement that the camera has to realize
+ vpHomogeneousMatrix f2Mf1;
+ vpFeatureTranslationRepresentationType translation;
+} ;
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h
new file mode 100644
index 0000000..8da9822
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
+ *
+ * Authors:
+ * Odile Bourquardez
+ *
+ *****************************************************************************/
+
+
+#ifndef vpFeatureVanishingPoint_H
+#define vpFeatureVanishingPoint_H
+
+
+
+/*!
+ \file vpFeatureVanishingPoint.h \brief Class that defines 2D vanishing
+ point visual feature (Z coordinate in 3D space is infinity)
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpPoint.h>
+
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRGBa.h>
+
+
+
+/*!
+ \class vpFeatureVanishingPoint
+ \ingroup group_visual_features
+
+ \brief Class that defines 2D vanishing point visual feature (Z
+ coordinate in 3D space is infinity).
+*/
+class VISP_EXPORT vpFeatureVanishingPoint : public vpBasicFeature
+{
+public:
+ typedef enum
+ {
+ X = 1, // x coordinates
+ Y = 2 // y coordinates
+ } vpFeatureVanishingPointType;
+
+ /*
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities ar useful but not mandatory
+ */
+ //no Z required
+
+public:
+ //! Default initialization.
+ void init() ;
+ //! Default constructor.
+ vpFeatureVanishingPoint() ;
+ //! Destructor.
+ virtual ~vpFeatureVanishingPoint() {}
+
+
+public:
+ /*
+ /section Set coordinates
+ */
+
+
+ //! set the point x-coordinates
+ void set_x(const double _x) ;
+ //! get the point x-coordinates
+ double get_x() const ;
+ //! set the point y-coordinates
+ void set_y(const double _y) ;
+ //! get the point y-coordinates
+ double get_y() const ;
+ //! set the point xy coordinates
+ void set_xy(const double _x, const double _y) ;
+ // void buildFrom(const vpPoint &p) ;
+ void buildFrom(const double _x, const double _y) ;
+
+
+
+ public:
+ /*
+ vpBasicFeature method instantiation
+ */
+ // feature selection
+ static unsigned int selectX();
+ static unsigned int selectY();
+
+
+ //! compute the interaction matrix from a subset a the possible features
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ //! compute the error between two visual features from a subset
+ //! a the possible features
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+ //! compute the error between a visual features and zero
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+ //! print the name of the feature
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ //! feature duplication
+ vpFeatureVanishingPoint *duplicate() const ;
+
+public:
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const;
+
+} ;
+
+
+
+#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h
new file mode 100644
index 0000000..94325b1
--- /dev/null
+++ b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic feature (used to create new feature not implemented in ViSP).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+#ifndef vpGenericFeature_hh
+#define vpGenericFeature_hh
+
+/*!
+ \file vpGenericFeature.h
+ \brief class that defines what is a generic feature (used to create new
+ feature not implemented in ViSP2
+ */
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/core/vpRGBa.h>
+
+#include <math.h>
+
+/*!
+ \class vpGenericFeature
+ \ingroup group_core_features
+
+ \brief Class that enables to define a feature or a set of features which are not implemented in ViSP as a specific class. It is indeed possible to create its own features, to use the corresponding interaction matrix, and to compute an error between the current and the desired feature. Moreover the created features can be mixed with features already implemented.
+
+ The following example shows how to use the vpGenericFeature class to create and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The interaction matrix corresponding to this feature is \f[ L = \left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f].
+ \code
+#include <visp3/core/vpGenericFeature.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ //First we have to define the desired feature log(Z*) corresponding to the desired point.
+ double xd = 0; //The x coordinate of the desired point.
+ double yd = 0; //The y coordinate of the desired point.
+ double Zd = 1; //The depth of the desired point.
+ vpGenericFeature logZd(1); //The dimension of the feature is 1.
+ logZd.set_s( log(Zd) );
+
+ //Then we have to define the current feature log(Z) corresponding to the current point.
+ double x = 1; //The x coordinate of the current point.
+ double y = 1; //The y coordinate of the current point.
+ double Z = 2; //The depth of the current point.
+ vpGenericFeature logZ(1); //The dimension of the feature is 1.
+ logZ.set_s( log(Z) );
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the current visual features sd
+ task.setInteractionMatrixType(vpServo::CURRENT);
+
+ // Add the point feature to the task
+ task.addFeature(logZ, logZd);
+
+ // Control loop
+ for ( ; ; ) {
+ // The new parameters x, y and Z must be computed here.
+
+ // Update the current point visual feature
+ logZ.set_s( log(Z) ) ;
+
+ // We have to compute the interaction matrix corresponding to the feature.
+ vpMatrix LlogZ(1,6) ;
+ LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
+ LlogZ[0][2] = -1/Z;
+ LlogZ[0][3] = -y;
+ LlogZ[0][4] = x;
+ logZ.setInteractionMatrix(LlogZ) ;
+
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+ return 0;
+}
+ \endcode
+
+The second example shows how to create and use a feature whose specificity is to have a desired feature fixed to zero. It is the case for the feature \f$ log( \frac{Z}{Z^*}) \f$.
+
+ \code
+#include <visp3/core/vpGenericFeature.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ vpServo task; // Visual servoing task
+
+ //First we have to define the desired feature log(Z*) corresponding to the desired point.
+ double xd = 0; //The x coordinate of the desired point.
+ double yd = 0; //The y coordinate of the desired point.
+ double Zd = 1; //The depth of the desired point.
+
+ //Then we have to define the current feature log(Z) corresponding to the current point.
+ double x = 1; //The x coordinate of the current point.
+ double y = 1; //The y coordinate of the current point.
+ double Z = 2; //The depth of the current point.
+ vpGenericFeature logZ(1); //The dimension of the feature is 1.
+ logZ.set_s( log(Z/Zd) );
+
+ // Set eye-in-hand control law.
+ // The computed velocities will be expressed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // Interaction matrix is computed with the current visual features sd
+ task.setInteractionMatrixType(vpServo::CURRENT);
+
+ // Add the point feature to the task
+ task.addFeature(logZ);
+
+ // Control loop
+ for ( ; ; ) {
+ // The new parameters x, y and Z must be computed here.
+
+ // Update the current point visual feature
+ logZ.set_s( log(Z/Zd) ) ;
+
+ // We have to compute the interaction matrix corresponding to the feature.
+ vpMatrix LlogZ(1,6) ;
+ LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
+ LlogZ[0][2] = -1/Z;
+ LlogZ[0][3] = -y;
+ LlogZ[0][4] = x;
+ logZ.setInteractionMatrix(LlogZ) ;
+
+
+ // compute the control law
+ vpColVector v = task.computeControlLaw(); // camera velocity
+ }
+ return 0;
+}
+ \endcode
+
+If the feature needs to be use with other features, the example servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it.
+ */
+class VISP_EXPORT vpGenericFeature : public vpBasicFeature
+{
+private:
+ vpGenericFeature() ;
+public:
+ void init() ;
+ vpGenericFeature(unsigned int dim) ;
+ virtual ~vpGenericFeature() ;
+public:
+
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+
+ vpColVector error(const vpBasicFeature &s_star,
+ const unsigned int select = FEATURE_ALL) ;
+
+ vpColVector error(const unsigned int select = FEATURE_ALL) ;
+
+ void print(const unsigned int select = FEATURE_ALL ) const ;
+
+ vpGenericFeature *duplicate() const ;
+
+private:
+ typedef enum
+ {
+ errorNotInitalized,
+ errorInitialized,
+ errorHasToBeUpdated
+ } vpGenericFeatureErrorType;
+
+ vpMatrix L ;
+ vpColVector err ;
+ vpGenericFeatureErrorType errorStatus ;
+
+public:
+ void setInteractionMatrix(const vpMatrix &L) ;
+ vpMatrix getInteractionMatrix() const { return L ; }
+ void setError(const vpColVector &error_vector) ;
+ void set_s(const vpColVector &s) ;
+ void set_s(const double s0) ;
+ void set_s(const double s0, const double s1) ;
+ void set_s(const double s0, const double s1, const double s2) ;
+
+ void get_s(vpColVector &s) const;
+ void get_s(double &s0) const;
+ void get_s(double &s0, double &s1) const;
+ void get_s(double &s0, double &s1, double &s2) const;
+
+public:
+ void display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color=vpColor::green,
+ unsigned int thickness=1) const ;
+
+
+} ;
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp
new file mode 100644
index 0000000..d4696ca
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp
@@ -0,0 +1,330 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature ellipse.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderEllipse.cpp
+ \brief conversion between tracker
+ and visual feature Ellipse
+*/
+
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+
+
+
+/*!
+ Initialize an ellipse feature thanks to a vpCircle.
+ The vpFeatureEllipse is initialized thanks to the parameters of the circle in the camera frame and in the image plan.
+ All the parameters are given in meter.
+
+ \warning To be sure that the vpFeatureEllipse is well initialized,
+ you have to be sure that at least the circle coordinates in the image
+ plan and in the camera frame are computed and stored in the vpCircle.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpCircle used to create the vpFeatureEllipse.
+*/
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCircle &t )
+{
+ try
+ {
+
+ // 3D data
+ double alpha = t.cP[0] ;
+ double beta = t.cP[1] ;
+ double gamma = t.cP[2] ;
+
+ double X0 = t.cP[3] ;
+ double Y0 = t.cP[4] ;
+ double Z0 = t.cP[5] ;
+
+ // equation p 318 prior eq (39)
+ double d = alpha*X0 + beta*Y0 + gamma*Z0 ;
+
+ double A = alpha / d ;
+ double B = beta / d ;
+ double C = gamma / d ;
+
+ s.setABC(A,B,C) ;
+
+
+ //2D data
+ s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+}
+
+
+/*!
+ Initialize an ellipse feature thanks to a vpSphere.
+ The vpFeatureEllipse is initialized thanks to the parameters
+ of the sphere in the camera frame and in the image plan.
+ All the parameters are given in meter.
+
+ \warning To be sure that the vpFeatureEllipse is well initialized,
+ you have to be sure that at least the sphere coordinates in the image
+ plan and in the camera frame are computed and stored in the vpSphere.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpSphere used to create the vpFeatureEllipse.
+*/
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t)
+{
+ try
+ {
+
+ // 3D data
+ double X0 = t.cP[0] ;
+ double Y0 = t.cP[1] ;
+ double Z0 = t.cP[2] ;
+ double R = t.cP[3] ;
+
+ double d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) -
+ vpMath::sqr(R) ;
+
+
+ double A = X0 / d ;
+ double B = Y0 / d ;
+ double C = Z0 / d ;
+
+ s.setABC(A,B,C) ;
+
+ //2D data
+ s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
+
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+#ifdef VISP_HAVE_MODULE_BLOB
+/*!
+ Initialize an ellipse feature thanks to a vpDot and camera parameters.
+ The vpFeatureEllipse is initialized thanks to the parameters
+ of the dot given in pixel. The camera parameters are used to convert the
+ pixel parameters to parameters given in meter.
+
+ \warning With a vpDot there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
+ initialized them outside the method.
+
+ \param s : Visual feature to initialize.
+
+ \param cam : The parameters of the camera used to acquire the image containing the vpDot.
+
+ \param t : The vpDot used to create the vpFeatureEllipse.
+*/
+void vpFeatureBuilder::create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpDot &t )
+{
+ try
+ {
+
+ unsigned int order = 3 ;
+ vpMatrix mp(order,order) ; mp =0 ;
+ vpMatrix m(order,order) ; m = 0 ;
+
+ mp[0][0] = t.m00 ;
+ mp[1][0] = t.m10;
+ mp[0][1] = t.m01 ;
+ mp[2][0] = t.m20 ;
+ mp[1][1] = t.m11 ;
+ mp[0][2] = t.m02 ;
+
+ vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
+
+ double m00 = m[0][0] ;
+ double m01 = m[0][1] ;
+ double m10 = m[1][0] ;
+ double m02 = m[0][2] ;
+ double m11 = m[1][1] ;
+ double m20 = m[2][0] ;
+
+ double xc = m10/m00 ; // sum j /S
+ double yc = m01/m00 ; // sum i /S
+
+ double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
+ double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
+ double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+ Initialize an ellipse feature thanks to a vpDot2 and camera parameters.
+ The vpFeatureEllipse is initialized thanks to the parameters
+ of the dot given in pixel. The camera parameters are used to convert the
+ pixel parameters to parameters given in meter.
+
+ \warning With a vpDot2 there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
+ initialized them outside the method.
+
+ \param s : Visual feature to initialize.
+
+ \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
+
+ \param t : The vpDot2 used to create the vpFeatureEllipse.
+*/
+void vpFeatureBuilder::create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpDot2 &t )
+{
+ try
+ {
+
+ unsigned int order = 3 ;
+ vpMatrix mp(order,order) ; mp =0 ;
+ vpMatrix m(order,order) ; m = 0 ;
+
+ mp[0][0] = t.m00 ;
+ mp[1][0] = t.m10;
+ mp[0][1] = t.m01 ;
+ mp[2][0] = t.m20 ;
+ mp[1][1] = t.m11 ;
+ mp[0][2] = t.m02 ;
+
+ vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
+
+ double m00 = m[0][0] ;
+ double m01 = m[0][1] ;
+ double m10 = m[1][0] ;
+ double m02 = m[0][2] ;
+ double m11 = m[1][1] ;
+ double m20 = m[2][0] ;
+
+ double xc = m10/m00 ; // sum j /S
+ double yc = m01/m00 ; // sum i /S
+
+ double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
+ double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
+ double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+#endif //#ifdef VISP_HAVE_MODULE_BLOB
+
+#ifdef VISP_HAVE_MODULE_ME
+/*!
+ Initialize an ellipse feature thanks to a vpMeEllipse and camera parameters.
+ The vpFeatureEllipse is initialized thanks to the parameters
+ of the ellipse given in pixel. The camera parameters are used to convert the
+ pixel parameters to parameters given in meter.
+
+ \warning With a vpMeEllipse there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
+ initialized them outside the method.
+
+ \param s : Visual feature to initialize.
+
+ \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
+
+ \param t : The vpMeEllipse used to create the vpFeatureEllipse.
+*/
+void vpFeatureBuilder::create(vpFeatureEllipse &s,
+ const vpCameraParameters &cam,
+ const vpMeEllipse &t )
+{
+ try
+ {
+
+ unsigned int order = 3 ;
+ vpMatrix mp(order,order) ; mp =0 ;
+ vpMatrix m(order,order) ; m = 0 ;
+
+ //The opposite of vpDot and vpDot2 because moments in vpMeEllipse
+ //are computed in the ij coordinate system whereas the moments in vpDot and vpDot2
+ //are computed in the uv coordinate system
+ mp[0][0] = t.get_m00() ;
+ mp[1][0] = t.get_m01();
+ mp[0][1] = t.get_m10() ;
+ mp[2][0] = t.get_m02() ;
+ mp[1][1] = t.get_m11() ;
+ mp[0][2] = t.get_m20() ;
+
+ vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
+
+ double m00 = m[0][0] ;
+ double m01 = m[0][1] ;
+ double m10 = m[1][0] ;
+ double m02 = m[0][2] ;
+ double m11 = m[1][1] ;
+ double m20 = m[2][0] ;
+
+ double xc = m10/m00 ; // sum j /S
+ double yc = m01/m00 ; // sum i /S
+
+ double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
+ double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
+ double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+#endif //#ifdef VISP_HAVE_MODULE_ME
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp
new file mode 100644
index 0000000..73ad08f
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp
@@ -0,0 +1,280 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature line.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderLine.cpp
+ \brief conversion between tracker
+ and visual feature Line
+*/
+
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+
+
+
+/*!
+ Initialize a line feature thanks to a vpLine.
+ A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
+ It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. In vpLine there are the parameters of two plans,
+ but the one which have the biggest D parameter is copied in the vpFeatureLine parameters.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpLine used to create the vpFeatureLine.
+*/
+void vpFeatureBuilder::create(vpFeatureLine &s, const vpLine &t )
+{
+ try
+ {
+ double A,B,C,D ;
+ s.setRhoTheta(t.getRho(),t.getTheta()) ;
+
+ if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
+ {
+ A = t.cP[0] ;
+ B = t.cP[1] ;
+ C = t.cP[2] ;
+ D = t.cP[3] ;
+ }
+ else
+ {
+ A = t.cP[4] ;
+ B = t.cP[5] ;
+ C = t.cP[6] ;
+ D = t.cP[7] ;
+ }
+
+
+ s.setABCD(A,B,C,D) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+
+}
+
+/*!
+ Initialize a line feature thanks to a vpCylinder.
+ A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
+ It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are computed
+ thanks to the parameters that are contained in vpCylinder. It is possible to choose which edge of the cylinder to use to
+ initialize the vpFeatureLine.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpLine used to create the vpFeatureLine.
+
+ \param line : The cylinder edge used to create the line feature.
+ It can be vpCylinder::line1 or vpCylinder::line2.
+*/
+void vpFeatureBuilder::create(vpFeatureLine &s,
+ const vpCylinder &t,
+ const int line)
+{
+ try
+ {
+
+ double a = t.getA() ;
+ double b = t.getB() ;
+ double c = t.getC() ;
+
+ double x0 = t.getX() ;
+ double y0 = t.getY() ;
+ double z0 = t.getZ() ;
+
+ double R = t.getR() ;
+
+ double D =
+ vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0)
+ - vpMath::sqr(R)
+ - vpMath::sqr(a*x0 + b*y0 + c*z0);
+
+ double alpha1 = (1 - a*a)*x0 - a*b*y0 - a*c*z0;
+ double beta1 = -a*b*x0 + (1 - b*b)*y0 - b*c*z0;
+ double gamma1 = -a*c*x0 - b*c*y0 + (1 - c*c)*z0;
+
+ D*=-1 ;
+
+ if (D<0)
+ {
+ alpha1*=-1 ;
+ beta1*=-1 ;
+ gamma1*=-1 ;
+ D*=-1 ;
+ }
+
+ s.setABCD(alpha1,beta1,gamma1,D) ;
+
+ if (line==vpCylinder::line1)
+ {
+
+ s.setRhoTheta(t.getRho1(),t.getTheta1()) ;
+
+ }
+ else
+ {
+
+ s.setRhoTheta(t.getRho2(),t.getTheta2()) ;
+ }
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+#ifdef VISP_HAVE_MODULE_ME
+/*!
+ Initialize a line feature thanks to a vpMeLine and the parameters of the camera.
+ A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
+ In vpMeLine these parameters are given in pixel. The conversion is done thanks to the camera parameters.
+
+ \warning vpFeatureLine also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are needed to
+ compute the interaction matrix but can not be computed thanks to a vpMeLine. You have to compute and set these parameters
+ outside the function.
+
+ \param s : Visual feature to initialize.
+
+ \param cam : The parameters of the camera used to acquire the image containing the line.
+
+ \param t : The vpLine used to create the vpFeatureLine.
+
+ The code below shows how to initialize a vpFeatureLine visual
+ feature. First, we initialize the \f$(\rho,\theta)\f$, and lastly we
+ set the parameters of the plane which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpMeLine line; // Moving-edges line tracker
+
+ vpFeatureLine s; // Point feature
+ ...
+ // Tracking on the dot
+ line.track(I);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, line);
+
+ // A pose estimation is requested to initialize A, B, C and D the
+ //parameters of the equation plan.
+ double A = 1;
+ double B = 1;
+ double C = 1;
+ double D = 1;
+ ....
+ s.setABCD(A,B,C,D);
+ \endcode
+*/
+void
+vpFeatureBuilder::create(vpFeatureLine &s,
+ const vpCameraParameters &cam,
+ const vpMeLine &t)
+{
+ double rhop ;
+ double thetap ;
+
+ double rho ;
+ double theta ;
+
+ try{
+ rhop = t.getRho() ;
+ thetap = t.getTheta();
+
+ //Gives the rho and theta coordinates in the (u,v) coordinate system.
+ if (thetap >= 0 && thetap < M_PI/2)
+ {
+ thetap = M_PI/2 - thetap;
+ }
+
+ else if (thetap >= M_PI/2 && thetap < 3*M_PI/2)
+ {
+ thetap = 3*M_PI/2 + M_PI - thetap;
+ }
+
+ else if (thetap >= 3*M_PI/2 && thetap <= 2*M_PI)
+ {
+ thetap = M_PI/2 + 2*M_PI - thetap;
+ }
+
+ //while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
+ //while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
+
+ // vpTRACE("pixel %f %f",rhop, thetap) ;
+ vpPixelMeterConversion::convertLine(cam,rhop,thetap, rho,theta) ;
+
+ while (theta > M_PI) { theta -= 2*M_PI ; }
+ 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 ;
+ }
+}
+#endif //#ifdef VISP_HAVE_MODULE_ME
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp
new file mode 100644
index 0000000..bf37d1f
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp
@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature point.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderPoint.cpp
+ \brief conversion between tracker
+ and visual feature Point
+*/
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/core/vpException.h>
+
+#ifdef VISP_HAVE_MODULE_BLOB
+/*!
+ Create a vpFeaturePoint thanks to a vpDot and the parameters of the camera.
+ The vpDot contains only the pixel coordinates of the point in an image.
+ Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
+ Those coordinates are stored in the vpFeaturePoint.
+
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot.
+ This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+
+ \param s : Visual feature \f$(x, y)\f$ to initialize. Be
+ aware, the 3D depth \f$Z\f$ requested to compute the interaction
+ matrix is not initialized by this function.
+ \param cam : The parameters of the camera used to acquire the image containing the vpDot.
+ \param d : The vpDot used to create the vpFeaturePoint.
+
+ The code below shows how to initialize a vpFeaturePoint visual
+ feature. First, we initialize the \f$x,y\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpDot dot; // Dot tracker
+
+ vpFeaturePoint s; // Point feature
+ ...
+ // Tracking on the dot
+ dot.track(I);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, dot);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+*/
+void vpFeatureBuilder::create(vpFeaturePoint &s,
+ const vpCameraParameters &cam,
+ const vpDot &d)
+{
+ try
+ {
+ double x=0, y=0;
+
+ vpImagePoint cog;
+ cog = d.getCog();
+
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+
+ s.set_x(x) ;
+ s.set_y(y) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Create a vpFeaturePoint thanks to a vpDot2 and the parameters of the camera.
+ The vpDot2 contains only the pixel coordinates of the point in an image.
+ Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
+ Those coordinates are stored in the vpFeaturePoint.
+
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot2.
+ This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+
+ \param s : The feature point.
+ \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
+ \param d : The vpDot2 used to create the vpFeaturePoint.
+
+ The code below shows how to initialize a vpFeaturePoint visual
+ feature. First, we initialize the \f$x,y\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpDot2 dot; // Dot tracker
+
+ vpFeaturePoint s; // Point feature
+ ...
+ // Tracking on the dot
+ dot.track(I);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, dot);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+*/
+void vpFeatureBuilder::create(vpFeaturePoint &s,
+ const vpCameraParameters &cam,
+ const vpDot2 &d)
+{
+ try
+ {
+ double x=0, y=0;
+
+ vpImagePoint cog;
+ cog = d.getCog();
+
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+
+ s.set_x(x) ;
+ s.set_y(y) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+#endif //#ifdef VISP_HAVE_MODULE_BLOB
+
+
+/*!
+ Create a vpFeaturePoint thanks to a vpImagePoint and the parameters of the camera.
+ The vpImagePoint contains only the pixel coordinates of the point in an image.
+ Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
+ Those coordinates are stored in the vpFeaturePoint.
+
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpImagePoint.
+ This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+
+ \param s : The feature point.
+ \param cam : The parameters of the camera used to acquire the image containing the point.
+ \param ip : The vpImagePoint used to create the vpFeaturePoint.
+
+ The code below shows how to initialize a vpFeaturePoint visual
+ feature. First, we initialize the \f$x,y\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpImagePoint iP; // the point in the image
+
+ vpFeaturePoint s; // Point feature
+ ...
+ // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
+ iP.set_i(0);
+ iP.set_j(0);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, iP);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+*/
+void vpFeatureBuilder::create(vpFeaturePoint &s,
+ const vpCameraParameters &cam,
+ const vpImagePoint &ip)
+{
+ try
+ {
+ double x=0, y=0;
+
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
+
+ s.set_x(x) ;
+ s.set_y(y) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Create a vpFeaturePoint thanks to a vpPoint.
+ This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
+ The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
+
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
+
+ \param s : The feature point.
+ \param p : The vpPoint used to create the vpFeaturePoint.
+*/
+void
+vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p)
+{
+ try
+ {
+ s.set_x( p.get_x()) ;
+ s.set_y( p.get_y()) ;
+
+ s.set_Z( p.cP[2]/p.cP[3]) ;
+
+ if (s.get_Z() < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << s.get_Z() << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(s.get_Z()) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << s.get_Z() << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Create a vpFeaturePoint thanks to a vpPoint. In this method noise is introduced during the initialization of the vpFeaturePoint.
+ This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
+ The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
+
+ This function intends to introduce noise after the initialization of the parameters. Cartesian \f$(x,y)\f$ coordinates
+ are first converted in pixel coordinates in the image using \e
+ goodCam camera parameters. Then, the pixels coordinates of the point
+ are converted back to cartesian coordinates \f$(x^{'},y^{'})\f$ using
+ the noisy camera parameters \e wrongCam. These last parameters are stored in the vpFeaturePoint.
+
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
+
+ \param s : The feature point.
+
+ \param goodCam : Camera parameters used to introduce noise. These
+ parameters are used to convert cartesian coordinates of the point \e
+ p in the image plane in pixel coordinates.
+
+ \param wrongCam : Camera parameters used to introduce noise. These
+ parameters are used to convert pixel coordinates of the point in
+ cartesian coordinates of the point in the image plane.
+
+ \param p : The vpPoint used to create the vpFeaturePoint.
+*/
+void
+vpFeatureBuilder::create(vpFeaturePoint &s,
+ const vpCameraParameters &goodCam,
+ const vpCameraParameters &wrongCam,
+ const vpPoint &p)
+{
+ try
+ {
+ double x = p.p[0] ;
+ double y = p.p[1] ;
+
+ s.set_Z( p.cP[2]/p.cP[3]) ;
+
+ double u=0, v=0;
+ vpMeterPixelConversion::convertPoint(goodCam,x,y,u,v) ;
+ vpPixelMeterConversion::convertPoint(wrongCam,u,v,x,y) ;
+
+
+ s.set_x(x) ;
+ s.set_y(y) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp
new file mode 100644
index 0000000..0a6d796
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature 3D Point.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderPoint3D.cpp
+ \brief conversion between tracker
+ and visual feature 3D Point
+*/
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/core/vpException.h>
+
+
+/*!
+
+ Initialize a 3D point feature using the coordinates of the point
+ \f$(X,Y,Z)\f$ in the camera frame. The values of \f$(X,Y,Z)\f$ are
+ expressed in meters.
+
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point
+ coordinates in the camera frame are computed and stored in the vpPoint.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpPoint used to create the vpFeaturePoint.
+*/
+void
+vpFeatureBuilder::create(vpFeaturePoint3D &s, const vpPoint &t )
+{
+ try
+ {
+
+
+
+ s.set_X( t.cP[0]/t.cP[3]) ;
+ s.set_Y( t.cP[1]/t.cP[3]) ;
+ s.set_Z( t.cP[2]/t.cP[3]) ;
+
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp
new file mode 100644
index 0000000..774974a
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp
@@ -0,0 +1,400 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature point with
+ * polar coordinates.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderPointPolar.cpp
+
+ \brief Conversion between tracker and visual feature point with
+ polar coordinates.
+*/
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/core/vpException.h>
+
+#ifdef VISP_HAVE_MODULE_BLOB
+/*!
+
+ Initialize a point feature with polar coordinates
+ \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
+ obtained by image processing. This point is the center
+ of gravity of a dot tracked using vpDot. Using the camera
+ parameters, the pixels coordinates of the dot are first
+ converted in cartesian \f$(x,y)\f$ coordinates in meter in the
+ camera frame and than in polar coordinates by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ \warning This function does not initialize \f$Z\f$ which is
+ requested to compute the interaction matrix by
+ vpfeaturePointPolar::interaction().
+
+ \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
+ aware, the 3D depth \f$Z\f$ requested to compute the interaction
+ matrix is not initialized by this function.
+
+ \param cam : Camera parameters.
+
+ \param dot : Tracked dot. The center of gravity corresponds to the
+ coordinates of the point in the image plane.
+
+ The code below shows how to initialize a vpFeaturePointPolar visual
+ feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpDot2 dot; // Dot tracker
+
+ vpFeaturePointPolar s; // Point feature with polar coordinates
+ ...
+ // Tracking on the dot
+ dot.track(I);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, dot);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+
+*/
+void vpFeatureBuilder::create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpDot &dot)
+{
+ try {
+ double x=0, y=0;
+
+ vpImagePoint cog;
+ cog = dot.getCog();
+
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+
+ double rho = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+
+ s.set_rho(rho) ;
+ s.set_theta(theta) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Initialize a point feature with polar coordinates
+ \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
+ obtained by image processing. This point is the center
+ of gravity of a dot tracked using vpDot2. Using the camera
+ parameters, the pixels coordinates of the dot are first
+ converted in cartesian \f$(x,y)\f$ coordinates in meter in the
+ camera frame and than in polar coordinates by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ \warning This function does not initialize \f$Z\f$ which is
+ requested to compute the interaction matrix by
+ vpfeaturePointPolar::interaction().
+
+ \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
+ aware, the 3D depth \f$Z\f$ requested to compute the interaction
+ matrix is not initialized by this function.
+
+ \param cam : Camera parameters.
+
+ \param dot : Tracked dot. The center of gravity corresponds to the
+ coordinates of the point in the image plane.
+
+ The code below shows how to initialize a vpFeaturePointPolar visual
+ feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpDot2 dot; // Dot tracker
+
+ vpFeaturePointPolar s; // Point feature with polar coordinates
+ ...
+ // Tracking on the dot
+ dot.track(I);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, dot);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+
+*/
+void vpFeatureBuilder::create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpDot2 &dot)
+{
+ try {
+ double x=0, y=0;
+
+ vpImagePoint cog;
+ cog = dot.getCog();
+
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+
+ double rho = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+
+ s.set_rho(rho) ;
+ s.set_theta(theta) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+#endif //#ifdef VISP_HAVE_MODULE_BLOB
+
+
+/*!
+
+ Initialize a point feature with polar coordinates
+ \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
+ obtained by image processing. The points coordinates are stored in a vpImagePoint. Using the camera
+ parameters, the pixels coordinates of the point are first
+ converted in cartesian \f$(x,y)\f$ coordinates in meter in the
+ camera frame and than in polar coordinates by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ \warning This function does not initialize \f$Z\f$ which is
+ requested to compute the interaction matrix by
+ vpfeaturePointPolar::interaction().
+
+ \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
+ aware, the 3D depth \f$Z\f$ requested to compute the interaction
+ matrix is not initialized by this function.
+
+ \param cam : Camera parameters.
+
+ \param iP : The vpImagePoint used to create the vpFeaturePoint.
+
+ The code below shows how to initialize a vpFeaturePointPolar visual
+ feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
+ set the 3D depth \f$Z\f$ of the point which is generally the result
+ of a pose estimation.
+
+ \code
+ vpImage<unsigned char> I; // Image container
+ vpCameraParameters cam; // Default intrinsic camera parameters
+ vpImagePoint iP; // the point in the image
+
+ vpFeaturePointPolar s; // Point feature with polar coordinates
+ ...
+ // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
+ iP.set_i(0);
+ iP.set_j(0);
+
+ // Initialize rho,theta visual feature
+ vpFeatureBuilder::create(s, cam, iP);
+
+ // A pose estimation is requested to initialize Z, the depth of the
+ // point in the camera frame.
+ double Z = 1; // Depth of the point in meters
+ ....
+ s.set_Z(Z);
+ \endcode
+
+*/
+void vpFeatureBuilder::create(vpFeaturePointPolar &s,
+ const vpCameraParameters &cam,
+ const vpImagePoint &iP)
+{
+ try {
+ double x=0, y=0;
+
+ vpPixelMeterConversion::convertPoint(cam,iP,x,y) ;
+
+ double rho = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+
+ s.set_rho(rho) ;
+ s.set_theta(theta) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+
+ Initialize a point feature with polar coordinates
+ \f$(\rho,\theta)\f$ using the coordinates of the point
+ \f$(x,y,Z)\f$, where \f$(x,y)\f$ correspond to the perspective
+ projection of the point in the image plane and \f$Z\f$ the 3D depth
+ of the point in the camera frame. The values of \f$(x,y,Z)\f$ are
+ expressed in meters. From the coordinates in the image plane, the
+ polar coordinates are computed by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ \param s : Visual feature \f$(\rho,\theta)\f$ and \f$Z\f$ to initialize.
+
+ \param p : A point with \f$(x,y)\f$ cartesian coordinates in the
+ image plane corresponding to the camera perspective projection, and
+ with 3D depth \f$Z\f$.
+*/
+void
+vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p)
+{
+ try {
+
+ double x = p.get_x();
+ double y = p.get_y();
+
+ double rho = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+
+ s.set_rho(rho) ;
+ s.set_theta(theta) ;
+
+ s.set_Z( p.get_Z() ) ;
+
+
+ if (s.get_Z() < 0) {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << s.get_Z() << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(s.get_Z()) < 1e-6) {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << s.get_Z() << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Initialize a point feature with polar coordinates
+ \f$(\rho,\theta)\f$ using the coordinates of the point
+ \f$(x,y,Z)\f$, where \f$(x,y)\f$ correspond to the perspective
+ projection of the point in the image plane and \f$Z\f$ the 3D depth
+ of the point in the camera frame. The values of \f$(x,y,Z)\f$ are
+ expressed in meters.
+
+ This function intends to introduce noise in the conversion from
+ cartesian to polar coordinates. Cartesian \f$(x,y)\f$ coordinates
+ are first converted in pixel coordinates in the image using \e
+ goodCam camera parameters. Then, the pixels coordinates of the point
+ are converted back to cartesian coordinates \f$(x^{'},y^{'})\f$ using
+ the noisy camera parameters \e wrongCam. From these new coordinates
+ in the image plane, the polar coordinates are computed by:
+
+ \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
+
+ \param s : Visual feature \f$(\rho,\theta)\f$ and \f$Z\f$ to initialize.
+
+ \param goodCam : Camera parameters used to introduce noise. These
+ parameters are used to convert cartesian coordinates of the point \e
+ p in the image plane in pixel coordinates.
+
+ \param wrongCam : Camera parameters used to introduce noise. These
+ parameters are used to convert pixel coordinates of the point in
+ cartesian coordinates of the point in the image plane.
+
+ \param p : A point with \f$(x,y)\f$ cartesian coordinates in the
+ image plane corresponding to the camera perspective projection, and
+ with 3D depth \f$Z\f$.
+*/
+void
+vpFeatureBuilder::create(vpFeaturePointPolar &s,
+ const vpCameraParameters &goodCam,
+ const vpCameraParameters &wrongCam,
+ const vpPoint &p)
+{
+ try {
+ double x = p.get_x();
+ double y = p.get_y();
+
+ s.set_Z( p.get_Z() );
+
+ double u=0, v=0;
+ vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
+ vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
+
+ double rho = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+
+ s.set_rho(rho) ;
+ s.set_theta(theta) ;
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp
new file mode 100644
index 0000000..3b1fc93
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Segment creation out of dots.
+ *
+ * Authors:
+ * Filip Novotny
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderSegment.cpp
+ \brief Segment creation out of dots.
+*/
+
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/core/vpMath.h>
+
+#ifdef VISP_HAVE_MODULE_BLOB
+
+/*!
+ Initialize a segment feature out of vpDots and camera parameters.
+
+ \param s : Visual feature to initialize.
+ \param cam : The parameters of the camera used to acquire the image containing the point.
+ \param d1 : The dot corresponding to the first point of the segment.
+ \param d2 : The dot corresponding to the second point of the segment.
+
+*/
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
+ const vpDot &d1, const vpDot &d2 )
+{
+ double x1=0, y1=0, x2=0, y2=0;
+
+ vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
+ vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
+
+ double xc = (x1+x2)/2.;
+ double yc = (y1+y2)/2.;
+ double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
+
+ if (s.isNormalized())
+ {
+ s.setXc( xc/l );
+ s.setYc( yc/l );
+ s.setL ( 1/l );
+ }
+ else
+ {
+ s.setXc( xc );
+ s.setYc( yc );
+ s.setL ( l );
+ }
+
+ s.setAlpha(atan2(y1-y2,x1-x2));
+}
+
+/*!
+ Initialize a segment feature out of vpDots and camera parameters.
+
+ \param s : Visual feature to initialize.
+ \param cam : The parameters of the camera used to acquire the image containing the point.
+ \param d1 : The dot corresponding to the first point of the segment.
+ \param d2 : The dot corresponding to the second point of the segment.
+
+*/
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
+ const vpDot2 &d1, const vpDot2 &d2)
+{
+ double x1=0, y1=0, x2=0, y2=0;
+
+ vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
+ vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
+
+ double xc = (x1+x2)/2.;
+ double yc = (y1+y2)/2.;
+ double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
+
+ if (s.isNormalized())
+ {
+ s.setXc( xc/l );
+ s.setYc( yc/l );
+ s.setL ( 1/l );
+ }
+ else
+ {
+ s.setXc( xc );
+ s.setYc( yc );
+ s.setL ( l );
+ }
+
+ s.setAlpha(atan2(y1-y2,x1-x2));
+}
+#endif //#ifdef VISP_HAVE_MODULE_BLOB
+
+/*!
+ Initialize a segment feature out of image points and camera parameters.
+
+ \param s : Visual feature to initialize.
+ \param cam : The parameters of the camera used to acquire the image containing the point.
+ \param ip1 : The image point corresponding to the first point of the segment.
+ \param ip2 : The image point corresponding to the second point of the segment.
+
+*/
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
+ const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ double x1=0, y1=0, x2=0, y2=0;
+
+ vpPixelMeterConversion::convertPoint(cam, ip1, x1, y1) ;
+ vpPixelMeterConversion::convertPoint(cam, ip2, x2, y2) ;
+
+ double xc = (x1+x2)/2.;
+ double yc = (y1+y2)/2.;
+ double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
+
+ if (s.isNormalized())
+ {
+ s.setXc( xc/l );
+ s.setYc( yc/l );
+ s.setL ( 1/l );
+ }
+ else
+ {
+ s.setXc( xc );
+ s.setYc( yc );
+ s.setL ( l );
+ }
+
+ s.setAlpha(atan2(y1-y2,x1-x2));
+}
+
+/*!
+
+ Build a segment visual feature from two points.
+
+ \param s : Visual feature to initialize.
+ \param P1, P2 : Two points defining the segment. These points must contain the 3D coordinates
+ in the camera frame (cP) and the projected coordinates in the image plane (p).
+
+*/
+void vpFeatureBuilder::create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2)
+{
+ double x1 = P1.get_x();
+ double y1 = P1.get_y();
+ double x2 = P2.get_x();
+ double y2 = P2.get_y();
+
+ double Z1 = P1.cP[2]/P1.cP[3];
+ double Z2 = P2.cP[2]/P2.cP[3];
+
+ s.buildFrom(x1, y1, Z1, x2, y2, Z2);
+}
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp
new file mode 100644
index 0000000..a6bb0bb
--- /dev/null
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Conversion between tracker and visual feature vanishing point.
+ *
+ * Authors:
+ * Odile Bourquardez
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureBuilderVanishingPoint.cpp
+ \brief conversion between vpPoint
+ and visual feature vanishing point.
+*/
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/core/vpException.h>
+
+
+/*!
+ Initialize a vpFeatureVanishingPoint thanks to a vpPoint.
+ The vpFeatureVanishingPoint is initialized thanks to the parameters of the point in the image plan.
+ All the parameters are given in meter.
+
+ \param s : Visual feature to initialize.
+
+ \param t : The vpPoint used to create the vpFeatureVanishingPoint.
+*/
+void
+vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &t)
+{
+ try
+ {
+ s.set_x( t.get_x()) ;
+ s.set_y( t.get_y()) ;
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Cannot create vanishing point feature") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Initialize a vpFeatureVanishingPoint thanks to two vpFeatureLine.
+ The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
+ All the parameters are given in meter.
+
+ \warning An exception is thrown if the two lines are parallels
+
+ \param s : Visual feature to initialize.
+
+ \param L1 : The first vpFeatureLine.
+
+ \param L2 : The second vpFeatureLine.
+*/
+void
+vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, const vpFeatureLine &L2 )
+{
+ double rho_l;
+ double rho_r;
+ double theta_l;
+ double theta_r;
+ double c_l;
+ double s_l;
+ double c_r;
+ double s_r;
+
+ rho_l = L1.getRho();
+ rho_r = L2.getRho();
+ theta_l = L1.getTheta();
+ theta_r = L2.getTheta();
+ c_l = cos(theta_l);
+ c_r = cos(theta_r);
+ s_l = sin(theta_l);
+ s_r = sin(theta_r);
+
+
+ double x,y;
+
+ double min = 0.0001;
+ if(fabs(theta_r-theta_l)<min || fabs(fabs(theta_r-theta_l)-M_PI)<min \
+ || fabs(fabs(theta_r-theta_l)-2*M_PI)<min)
+ {
+ vpCERROR<<"There is no vanishing point : the lines are parallel in the image plane"<<std::endl;
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "There is no vanishing point : the lines are parallel in the image plane")) ;
+ }
+
+ y = (rho_r *c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l );
+ x = (rho_r *s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l );
+
+ s.set_x ( x );
+ s.set_y ( y );
+}
+
+
+
+/*!
+ Initialize a vpFeatureVanishingPoint thanks to two vpLine.
+ The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
+ All the parameters are given in meter.
+
+ \warning An exception is thrown if the two lines are parallels
+
+ \param s : Visual feature to initialize.
+
+ \param L1 : The first vpLine.
+
+ \param L2 : The second vpLine.
+*/
+void
+vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2)
+{
+ vpFeatureLine l1,l2 ;
+ vpFeatureBuilder::create (l1,L1) ;
+ vpFeatureBuilder::create (l2,L2) ;
+
+ vpFeatureBuilder::create (s, l1, l2) ;
+
+}
diff --git a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp
new file mode 100644
index 0000000..ba032eb
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp
@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+
+const unsigned int vpBasicFeature::FEATURE_LINE [32] =
+ {
+ (unsigned int)(1 << 0), (unsigned int)(1 << 1),
+ (unsigned int)(1 << 2), (unsigned int)(1 << 3),
+ (unsigned int)(1 << 4), (unsigned int)(1 << 5),
+ (unsigned int)(1 << 6), (unsigned int)(1 << 7),
+ (unsigned int)(1 << 8), (unsigned int)(1 << 9),
+ (unsigned int)(1 << 10), (unsigned int)(1 << 11),
+ (unsigned int)(1 << 12), (unsigned int)(1 << 13),
+ (unsigned int)(1 << 14), (unsigned int)(1 << 15),
+ (unsigned int)(1 << 16), (unsigned int)(1 << 17),
+ (unsigned int)(1 << 18), (unsigned int)(1 << 19),
+ (unsigned int)(1 << 20), (unsigned int)(1 << 21),
+ (unsigned int)(1 << 22), (unsigned int)(1 << 23),
+ (unsigned int)(1 << 24), (unsigned int)(1 << 25),
+ (unsigned int)(1 << 26), (unsigned int)(1 << 27),
+ (unsigned int)(1 << 28), (unsigned int)(1 << 29),
+ (unsigned int)(1 << 30), (unsigned int)(1 << 31)
+ };
+
+/*!
+ \file vpBasicFeature.cpp
+ \brief Class that defines what is a visual feature.
+*/
+/*!
+ Default constructor.
+*/
+vpBasicFeature::vpBasicFeature()
+ : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user)
+{
+}
+
+/*!
+ Destructor that free allocated memory.
+*/
+vpBasicFeature::~vpBasicFeature()
+{
+ if (flags != NULL) {
+ delete [] flags;
+ flags = NULL;
+ }
+}
+
+/*!
+ Copy constructor.
+*/
+vpBasicFeature::vpBasicFeature(const vpBasicFeature &f)
+ : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user)
+{
+ *this = f;
+}
+
+/*!
+ Copy operator.
+*/
+vpBasicFeature &vpBasicFeature::operator=(const vpBasicFeature &f)
+{
+ s = f.s;
+ dim_s = f.dim_s;
+ nbParameters = f.nbParameters;
+ deallocate = f.deallocate;
+ if (flags)
+ delete [] flags;
+ flags = new bool [nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = f.flags[i];
+
+ return (*this);
+}
+
+//! Get the feature vector dimension.
+unsigned int
+vpBasicFeature::getDimension(unsigned int select) const
+{
+ unsigned int dim = 0 ;
+ 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 ;
+ }
+ return dim ;
+}
+
+//! Get the feature vector \f$\bf s\f$.
+vpColVector
+vpBasicFeature::get_s(const unsigned int select) const
+{
+ vpColVector state(0), stateLine(1);
+ // if s is higher than the possible selections (photometry), send back the whole vector
+ if(dim_s > 31)
+ return s;
+
+ for(unsigned int i=0;i<dim_s;++i)
+ {
+ if(FEATURE_LINE[i] & select)
+ {
+ stateLine[0] = s[i];
+ state.stack(stateLine);
+ }
+ }
+ return state ;
+}
+
+void vpBasicFeature::resetFlags()
+{
+ if (flags != NULL)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+ }
+}
+
+//! Set feature flags to true to prevent warning when re-computing the interaction matrix without having updated the feature.
+void vpBasicFeature::setFlags()
+{
+ if (flags != NULL)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
+ }
+}
+
+//! Compute the error between two visual features from a subset of the possible features.
+vpColVector vpBasicFeature::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0),eLine(1);
+ if (dim_s <= 31)
+ {
+ for(unsigned int i=0;i<dim_s;++i){
+ if(FEATURE_LINE[i] & select)
+ {
+ eLine[0] = s[i] - s_star[i];
+ e.stack(eLine);
+ //std::cout << "dim_s <= 31"<<std::endl;
+ }
+ }
+ }
+ else
+ {
+ e.resize(dim_s);
+ vpColVector sd = s_star.get_s();
+ e = s - sd;
+ }
+
+ return e ;
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 4
+ * End:
+ */
diff --git a/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp
new file mode 100644
index 0000000..f25beee
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp
@@ -0,0 +1,505 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point visual feature.
+ *
+ * Authors:
+ * Nicolas Melchior
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureDepth.cpp
+ \brief Class that defines 2D point visual feature
+*/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+
+/*
+
+
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities ar useful but not mandatory
+
+
+
+
+
+*/
+
+/*!
+ Initialize the memory space requested for 3D depth visual feature.
+*/
+void
+vpFeatureDepth::init()
+{
+ //feature dimension
+ dim_s = 1 ;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+ x = y = 0.;
+ Z = 1.;
+}
+
+
+/*!
+ Default constructor that build a visual feature.
+*/
+vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
+{
+ init() ;
+}
+
+
+/*!
+ Set the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+
+ \param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
+*/
+void
+vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
+{
+ s[0] = LogZoverZstar ;
+}
+
+
+/*!
+ Get the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+
+ \return The value of \f$ log(\frac{Z}{Z^*}) \f$.
+*/
+double
+vpFeatureDepth::get_LogZoverZstar() const
+{
+ return s[0] ;
+}
+
+
+/*!
+ Set the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
+
+ \param x_ : \f$ x \f$ value to set.
+*/
+void
+vpFeatureDepth::set_x(const double x_)
+{
+ this->x = x_ ;
+ flags[0] = true;
+}
+
+
+/*!
+ Get the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
+
+ \return The value of \f$ x \f$.
+*/
+double
+vpFeatureDepth::get_x() const
+{
+ return x ;
+}
+
+
+/*!
+ Set the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
+
+ \param y_ : \f$ y \f$ value to set.
+*/
+void
+vpFeatureDepth::set_y(const double y_)
+{
+ this->y = y_ ;
+ flags[1] = true;
+}
+
+
+/*!
+ Get the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
+
+ \return The value of \f$ y \f$.
+*/
+double
+vpFeatureDepth::get_y() const
+{
+ return y ;
+}
+
+/*!
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \param Z_ : \f$ Z \f$ value to set.
+*/
+void
+vpFeatureDepth::set_Z(const double Z_)
+{
+ this->Z = Z_ ;
+ flags[2] = true;
+}
+
+
+/*!
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \return The value of \f$ Z \f$.
+*/
+double
+vpFeatureDepth::get_Z() const
+{
+ return Z ;
+}
+
+
+/*!
+ Set the value of \f$ x \f$, \f$ y \f$, \f$ Z \f$ and \f$ log(\frac{Z}{Z^*}) \f$. \f$ x \f$ and \f$ y \f$ represent the coordinates of the point in the camera frame. \f$ Z \f$ is the 3D coordinate representing the depth. \f$ log(\frac{Z}{Z^*}) \f$ represents the logarithm of the current depth relative to the desired depth.
+
+ \param x_ : \f$ x \f$ value to set.
+ \param y_ : \f$ y \f$ value to set.
+ \param Z_ : \f$ Z \f$ value to set.
+ \param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
+*/
+void
+vpFeatureDepth::set_xyZLogZoverZstar(const double x_,
+ const double y_,
+ const double Z_,
+ const double LogZoverZstar)
+{
+ set_x(x_) ;
+ set_y(y_) ;
+ set_Z(Z_) ;
+ set_LogZoverZstar(LogZoverZstar) ;
+ for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+ Compute and return the interaction matrix \f$ L \f$. The computation is made thanks to the values of the point coordinates \f$ x \f$ and \f$ y \f$ and the depth \f$ Z \f$.
+
+ \f[ L =
+ \left[\begin{array}{cccccc}
+ 0 & 0 & -1/Z & -y & x & 0
+ \end{array}\right]\f]
+
+ \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+
+ \return The interaction matrix computed from the point feature.
+
+ The code below shows how to compute the interaction matrix associated to the visual feature \f$ s = log(\frac{Z}{Z^*}) \f$.
+ \code
+ // Creation of the current feature s
+ vpFeatureDepth s;
+ s.buildFrom(0, 0, 5, log(5/1)); //The current depth is 5 metters and the desired is 1 metter.
+
+ vpMatrix L_x = s.interaction();
+ \endcode
+*/
+vpMatrix
+vpFeatureDepth::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but z was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ L.resize(1,6) ;
+
+ double x_ = get_x();
+ double y_ = get_y();
+ double Z_ = get_Z();
+
+ if (Z_ < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ if (FEATURE_ALL & select)
+ {
+ L = 0;
+ L[0][0] = 0;
+ L[0][1] = 0;
+ L[0][2] = -1/Z_;
+ L[0][3] = -y_;
+ L[0][4] = x_;
+ L[0][5] = 0;
+ }
+
+ return L ;
+}
+
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ Since this visual feature \f$ s \f$ represent the current depth relative to the desired depth, the desired visual
+ feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
+ the current visual feature \f$ s \f$.
+
+ \param s_star : Desired visual visual feature that should be equal to zero.
+ \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ \exception vpFeatureException::badInitializationError : If the
+ desired visual feature \f$ s^* \f$ is not equal to zero.
+
+ The code below shows how to use this method:
+
+ \code
+ // Creation of the current feature s
+ vpFeatureDepth s;
+
+ // Creation of the desired feature s^*. By default this feature is
+ // initialized to zero
+ vpFeatureDepth s_star;
+ s_star.setLogZoverZstar(0)
+
+ // Compute the interaction matrix for the ThetaU_z feature
+ vpMatrix L_z = s.interaction();
+
+ // Compute the error vector (s-s*) for the ThetaU_z feature
+ s.error(s_star);
+ \endcode
+*/
+vpColVector
+vpFeatureDepth::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+
+ if (fabs(s_star.get_s().sumSquare()) > 1e-6)
+ {
+ vpERROR_TRACE("s* should be zero ! ") ;
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "s* should be zero !")) ;
+ }
+
+ vpColVector e(1) ;
+ if(FEATURE_ALL & select)
+ {
+ e[0] = s[0];
+ }
+
+ return e ;
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+
+ \code
+ vpFeatureDepth s; // Current visual feature s
+
+ // Creation of the current feature s
+ s.buildFrom(0, 0, 5, log(5/1));
+
+ s.print(); // print all the 2 components of the feature
+ \endcode
+*/
+void
+vpFeatureDepth::print(const unsigned int select ) const
+{
+ if (FEATURE_ALL & select)
+ {
+ std::cout <<"Point: x=" << get_x() ;
+ std::cout <<" Point: y=" << get_y() ;
+ std::cout <<" Point: Z=" << get_Z() ;
+
+ std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;
+
+ std::cout <<std::endl ;
+ }
+}
+
+
+/*!
+ Build a 3D depth visual feature from the point coordinates \f$ x \f$ and \f$ y \f$ given in the camera frame, \f$ Z \f$ which describes the depth and \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+
+ \param x_ : The \f$ x \f$ parameter.
+ \param y_ : The \f$ y \f$ parameter.
+ \param Z_ : The \f$ Z \f$ parameter.
+ \param LogZoverZstar : The \f$ log(\frac{Z}{Z^*}) \f$ parameter.
+*/
+void
+vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
+{
+
+ s[0] = LogZoverZstar;
+
+ this->x = x_ ;
+ this->y = y_ ;
+ this->Z = Z_ ;
+
+ if (Z < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+
+}
+
+
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureDepth s;
+ s_star = s.duplicate(); // s_star is now a vpFeatureDepth
+ \endcode
+
+*/
+vpFeatureDepth *vpFeatureDepth::duplicate() const
+{
+ vpFeatureDepth *feature = new vpFeatureDepth;
+ return feature ;
+}
+
+/*!
+
+ Not implemented.
+
+*/
+void
+vpFeatureDepth::display(const vpCameraParameters &/* cam */,
+ const vpImage<unsigned char> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+/*!
+
+ Not implemented.
+
+ */
+void
+vpFeatureDepth::display(const vpCameraParameters &/* cam */,
+ const vpImage<vpRGBa> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp
new file mode 100644
index 0000000..95762ce
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp
@@ -0,0 +1,473 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D ellipse visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureEllipse.cpp
+ \brief Class that defines 2D ellipse visual feature
+*/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+/*
+
+
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities ar useful but not mandatory
+
+
+
+
+
+*/
+
+void
+vpFeatureEllipse::init()
+{
+ //feature dimension
+ dim_s = 5 ;
+ nbParameters = 8;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //default depth values
+ A = B = 0;
+ C =1 ;
+
+}
+
+vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0)
+{
+ init() ;
+}
+
+
+
+//! compute the interaction matrix from a subset a the possible features
+vpMatrix
+vpFeatureEllipse::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but mu20 was not set yet");
+ break;
+ case 3:
+ vpTRACE("Warning !!! The interaction matrix is computed but mu11 was not set yet");
+ break;
+ case 4:
+ vpTRACE("Warning !!! The interaction matrix is computed but mu02 was not set yet");
+ break;
+ case 5:
+ vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
+ break;
+ case 6:
+ vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
+ break;
+ case 7:
+ vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ double xc = s[0] ;
+ double yc = s[1] ;
+ double mu20 = s[2] ;
+ double mu11 = s[3] ;
+ double mu02 = s[4] ;
+
+ //eq 39
+ double Z = 1/(A*xc + B*yc + C) ;
+
+
+
+ if (vpFeatureEllipse::selectX() & select )
+ {
+ vpMatrix H(1,6) ; H = 0;
+
+
+ H[0][0] = -1/Z;
+ H[0][1] = 0 ;
+ H[0][2] = xc/Z + A*mu20 + B*mu11;
+ H[0][3] = xc*yc + mu11;
+ H[0][4] = -1-vpMath::sqr(xc)-mu20;
+ H[0][5] = yc;
+
+
+ L = vpMatrix::stack(L,H) ;
+ }
+
+ if (vpFeatureEllipse::selectY() & select )
+ {
+ vpMatrix H(1,6) ; H = 0;
+
+
+ H[0][0] = 0 ;
+ H[0][1] = -1/Z;
+ H[0][2] = yc/Z + A*mu11 + B*mu02;
+ H[0][3] = 1+vpMath::sqr(yc)+mu02;
+ H[0][4] = -xc*yc - mu11;
+ H[0][5] = -xc;
+
+ L = vpMatrix::stack(L,H) ;
+ }
+
+ if (vpFeatureEllipse::selectMu20() & select )
+ {
+ vpMatrix H(1,6) ; H = 0;
+
+ H[0][0] = -2*(A*mu20+B*mu11);
+ H[0][1] = 0 ;
+ H[0][2] = 2*((1/Z+A*xc)*mu20+B*xc*mu11) ;
+ H[0][3] = 2*(yc*mu20+xc*mu11);
+ H[0][4] = -4*mu20*xc;
+ H[0][5] = 2*mu11;
+
+ L = vpMatrix::stack(L,H) ;
+ }
+
+ if (vpFeatureEllipse::selectMu11() & select )
+ {
+ vpMatrix H(1,6) ; H = 0;
+
+ H[0][0] = -A*mu11-B*mu02;
+ H[0][1] = -A*mu20-B*mu11;
+ H[0][2] = A*yc*mu20+(3/Z-C)*mu11+B*xc*mu02;
+ H[0][3] = 3*yc*mu11+xc*mu02;
+ H[0][4] = -yc*mu20-3*xc*mu11;
+ H[0][5] = mu02-mu20;
+
+ L = vpMatrix::stack(L,H) ;
+ }
+
+ if (vpFeatureEllipse::selectMu02() & select )
+ {
+ vpMatrix H(1,6) ; H = 0;
+
+ H[0][0] = 0 ;
+ H[0][1] = -2*(A*mu11+B*mu02);
+ H[0][2] = 2*((1/Z+B*yc)*mu02+A*yc*mu11);
+ H[0][3] = 4*yc*mu02;
+ H[0][4] = -2*(yc*mu11 +xc*mu02) ;
+ H[0][5] = -2*mu11 ;
+ L = vpMatrix::stack(L,H) ;
+ }
+
+
+ return L ;
+}
+
+//! compute the error between two visual features from a subset
+//! a the possible features
+vpColVector
+vpFeatureEllipse::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeatureEllipse::selectX() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeatureEllipse::selectY() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1] - s_star[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ if (vpFeatureEllipse::selectMu20() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[2] - s_star[2] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeatureEllipse::selectMu11() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[3] - s_star[3] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ if (vpFeatureEllipse::selectMu02() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[4] - s_star[4] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ }
+ catch(...) {
+ throw;
+ }
+
+ return e ;
+
+}
+
+
+void
+vpFeatureEllipse::print(const unsigned int select ) const
+{
+
+ std::cout <<"Ellipse: " << std::endl ;
+ if (vpFeatureEllipse::selectX() & select )
+ std::cout << " x=" << s[0] <<std::endl ;;
+ if (vpFeatureEllipse::selectY() & select )
+ std::cout << " y=" << s[1] <<std::endl ;
+ if (vpFeatureEllipse::selectMu20() & select )
+ std::cout << " mu20=" << s[2] <<std::endl ;
+ if (vpFeatureEllipse::selectMu11() & select )
+ std::cout << " mu11=" << s[3] <<std::endl ;
+ if (vpFeatureEllipse::selectMu02() & select )
+ std::cout << " mu02=" << s[4] <<std::endl ;
+ std::cout << "A = "<<A <<" B = "<<B <<" C = "<<C << std::endl ;
+}
+
+
+void
+vpFeatureEllipse::buildFrom(const double x, const double y,
+ const double mu20, const double mu11,
+ const double mu02)
+{
+
+ s[0] = x ;
+ s[1] = y ;
+ s[2] = mu20 ;
+ s[3] = mu11 ;
+ s[4] = mu02 ;
+
+ for( int i = 0; i < 5; i++) flags[i] = true;
+
+}
+
+void
+vpFeatureEllipse::buildFrom(const double x, const double y,
+ const double mu20, const double mu11,
+ const double mu02,
+ const double a, const double b, const double c)
+{
+
+ s[0] = x ;
+ s[1] = y ;
+ s[2] = mu20 ;
+ s[3] = mu11 ;
+ s[4] = mu02 ;
+
+ this->A = a ;
+ this->B = b ;
+ this->C = c ;
+
+ for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+void
+vpFeatureEllipse::set_x(const double x)
+{
+ s[0] = x ;
+ flags[0] = true;
+}
+
+void
+vpFeatureEllipse::set_y(const double y)
+{
+ s[1] = y ;
+ flags[1] = true;
+}
+
+void
+vpFeatureEllipse::set_xy(const double x,const double y)
+{
+ s[0] = x ;
+ s[1] = y ;
+ for( int i = 0; i < 2; i++) flags[i] = true;
+}
+
+void
+vpFeatureEllipse::setABC(const double a, const double b, const double c)
+{
+ this->A = a ;
+ this->B = b ;
+ this->C = c ;
+ for( unsigned int i = 5; i < nbParameters; i++) flags[i] = true;
+}
+
+
+void
+vpFeatureEllipse::setMu(const double mu20, const double mu11,
+ const double mu02)
+{
+
+ s[2] = mu20 ;
+ s[3] = mu11 ;
+ s[4] = mu02 ;
+ for( int i = 2; i < 5; i++) flags[i] = true;
+
+}
+
+
+
+/*!
+
+ Display ellipse feature.
+
+ \param cam : Camera parameters.
+ \param I : Image on which features have to be displayed.
+ \param color : Color used to display the feature.
+ \param thickness : Thickness of the feature representation.
+*/
+void
+vpFeatureEllipse::display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness ) const
+{
+ try{
+ double x = s[0] ;
+ double y = s[1] ;
+
+ double mu20 = s[2] ;
+ double mu11 = s[3] ;
+ double mu02 = s[4] ;
+
+ vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
+ cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display ellipse feature.
+
+ \param cam : Camera parameters.
+ \param I : Color image on which features have to be displayed.
+ \param color : Color used to display the feature.
+ \param thickness : Thickness of the feature representation.
+*/
+void
+vpFeatureEllipse::display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness ) const
+{
+ try{
+ double x = s[0] ;
+ double y = s[1] ;
+
+ double mu20 = s[2] ;
+ double mu11 = s[3] ;
+ double mu02 = s[4] ;
+
+ vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
+ cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+//! for memory issue (used by the vpServo class only)
+vpFeatureEllipse *vpFeatureEllipse::duplicate() const
+{
+ vpFeatureEllipse *feature = new vpFeatureEllipse ;
+ return feature ;
+}
+
+unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2] ; }
+unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3] ; }
+unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureLine.cpp b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp
new file mode 100644
index 0000000..b63aa55
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp
@@ -0,0 +1,576 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D line visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeatureLine.cpp
+ \brief Class that defines 2D line visual feature
+*/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// simple math function (round)
+#include <visp3/core/vpMath.h>
+
+// Display Issue
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+
+//Color / image / display
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+/*
+
+
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities ar useful but not mandatory
+
+
+
+
+
+*/
+
+/*!
+ Initialize the memory space requested for 2D line visual feature.
+*/
+void
+vpFeatureLine::init()
+{
+ //feature dimension
+ dim_s = 2 ;
+ nbParameters = 6;
+
+ // memory allocation
+ // x cos(theta) + y sin(theta) - rho = 0
+ // s[0] = rho
+ // s[1] = theta
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ A = B = C = D = 0.0 ;
+}
+
+
+/*!
+ Default constructor that build a visual feature.
+*/
+vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0)
+{
+ init() ;
+}
+
+
+/*!
+ Sets the values of \f$ \rho \f$ and \f$ \theta \f$ which represent the parameters of the 2D line feature.
+
+ \param rho : \f$ \rho \f$ value to set.
+ \param theta : \f$ \theta \f$ value to set.
+*/
+void
+vpFeatureLine::setRhoTheta(const double rho, const double theta)
+{
+ s[0] = rho ;
+ s[1] = theta ;
+ for( int i = 0; i < 2; i++) flags[i] = true;
+}
+
+
+/*!
+ Sets the values of A, B, C and D which represent the parameters used to describe the equation of a plan in the camera frame.
+ \f[ AX + BY + CZ + D = 0 \f]
+ Those parameters are needed to compute the interaction matrix associated to a visual feature. Normally, two plans are needed to describe a line (the intersection of those two plans). But to compute the interaction matrix only one plan equation is required. The only one restrictions is that the value of D must not be equal to zero !
+
+ \param A_ : A value to set.
+ \param B_ : B value to set.
+ \param C_ : C value to set.
+ \param D_ : D value to set.
+*/
+void
+vpFeatureLine::setABCD(const double A_, const double B_,
+ const double C_, const double D_)
+{
+ this->A = A_ ;
+ this->B = B_ ;
+ this->C = C_ ;
+ this->D = D_ ;
+ for(unsigned int i = 2; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L \f$. The computation
+ is made thanks to the values of the line feature \f$ \rho \f$ and
+ \f$ \theta \f$ and the equation of a plan to which the line belongs.
+
+ \f[ L = \left[\begin{array}{c}L_{\rho} \\ L_{\theta}\end{array}\right] =
+ \left[\begin{array}{cccccc}
+ \lambda_{\rho}cos(\theta) & \lambda_{\rho}sin(\theta) & -\lambda_{\rho}\rho & (1+\rho^2)sin(\theta) & -(1+\rho^2)cos(\theta) & 0 \\
+ \lambda_{\theta}cos(\theta) & \lambda_{\theta}sin(\theta) & -\lambda_{\theta}\rho & -\rho cos(\theta) & -\rho sin(\theta) & -1
+ \end{array}\right]\f]
+
+ Where :
+ \f[ \lambda_{\rho} = (A \; \rho \; cos(\theta) + B \; \rho \; sin(\theta) + C) / D \f]
+ \f[ \lambda_{\theta} = (A \; sin(\theta) - B \; cos(\theta)) / D \f]
+
+ \param select : Selection of a subset of the possible line features.
+ - To compute the interaction matrix for all the two line features
+ use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
+ interaction matrix is \f$ [2 \times 6] \f$
+ - To compute the interaction matrix for only one of the line
+ component feature (\f$ \rho, \theta \f$) use one of the
+ corresponding function selectRho() or selectTheta(). In that case
+ the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
+
+ \return The interaction matrix computed from the line features.
+
+ The code below shows how to compute the interaction matrix
+ associated to the visual feature \f$ s = \theta \f$.
+ \code
+ // Creation of the current feature s
+ vpFeatureLine s;
+ s.buildFrom(0, 0, 0, 0, 1, -1);
+
+ vpMatrix L_theta = s.interaction( vpFeatureLine::selectTheta() );
+ \endcode
+
+ The code below shows how to compute the interaction matrix
+ associated to the visual feature \f$ s = (\rho, \theta) \f$.
+
+ \code
+ // Creation of the current feature s
+ vpFeatureLine s;
+ s.buildFrom(0, 0, 0, 0, 1, -1);
+
+ vpMatrix L_theta = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+*/
+vpMatrix
+vpFeatureLine::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
+ break;
+ case 3:
+ vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
+ break;
+ case 4:
+ vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
+ break;
+ case 5:
+ vpTRACE("Warning !!! The interaction matrix is computed but D was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+ double rho = s[0] ;
+ double theta = s[1] ;
+
+
+ double co = cos(theta);
+ double si = sin(theta);
+
+ if (fabs(D) < 1e-6)
+ {
+ vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f",D) ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Incorrect plane coordinates D")) ;
+ }
+
+ double lambda_theta =( A*si - B*co) /D;
+ double lambda_rho = (C + rho*A*co + rho*B*si)/D;
+
+ if (vpFeatureLine::selectRho() & select )
+ {
+ vpMatrix Lrho(1,6) ;
+
+
+ Lrho[0][0]= co*lambda_rho;
+ Lrho[0][1]= si*lambda_rho;
+ Lrho[0][2]= -rho*lambda_rho;
+ Lrho[0][3]= si*(1.0 + rho*rho);
+ Lrho[0][4]= -co*(1.0 + rho*rho);
+ Lrho[0][5]= 0.0;
+
+ L = vpMatrix::stack(L,Lrho) ;
+ }
+
+ if (vpFeatureLine::selectTheta() & select )
+ {
+ vpMatrix Ltheta(1,6) ;
+
+ Ltheta[0][0] = co*lambda_theta;
+ Ltheta[0][1] = si*lambda_theta;
+ Ltheta[0][2] = -rho*lambda_theta;
+ Ltheta[0][3] = -rho*co;
+ Ltheta[0][4] = -rho*si;
+ Ltheta[0][5] = -1.0;
+
+ L = vpMatrix::stack(L,Ltheta) ;
+ }
+ return L ;
+}
+
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ \param s_star : Desired visual feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible line features.
+ - To compute the error for all the two line features use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2
+ dimension column vector.
+ - To compute the error for only one of the line component feature
+ (\f$ \rho, \theta \f$) use one of the corresponding function
+ selectRho() or selectTheta(). In that case the error vector is a 1
+ dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ The code below shows how to use this method to manipulate the \f$
+ \theta \f$ subset:
+
+ \code
+ // Creation of the current feature s
+ vpFeatureLine s;
+ s.buildFrom(0, 0, 0, 0, 1, -1);
+
+ // Creation of the desired feature s*
+ vpFeatureLine s_star;
+ s_star.buildFrom(0, 0, 0, 0, 1, -5);
+
+ // Compute the interaction matrix for the theta feature
+ vpMatrix L_theta = s.interaction( vpFeatureLine::selectTheta() );
+
+ // Compute the error vector (s-s*) for the Theta feature
+ s.error(s_star, vpFeatureLine::selectTheta());
+ \endcode
+*/
+vpColVector
+vpFeatureLine::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeatureLine::selectRho() & select )
+ {
+ vpColVector erho(1) ;
+ erho[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,erho) ;
+ }
+
+ if (vpFeatureLine::selectTheta() & select )
+ {
+
+ double err = s[1] - s_star[1] ;
+ while (err < -M_PI) err += 2*M_PI ;
+ while (err > M_PI) err -= 2*M_PI ;
+
+ vpColVector etheta(1) ;
+ etheta[0] = err ;
+ e = vpColVector::stack(e,etheta) ;
+ }
+ }
+ catch(...) {
+ throw ;
+ }
+
+ return e ;
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible line features.
+ - To print all the two line features use vpBasicFeature::FEATURE_ALL.
+ - To print only one of the line component feature (\f$ \rho, \theta \f$)
+ use one of the corresponding function selectRho() or selectTheta().
+
+ \code
+ vpFeatureLine s; // Current visual feature s
+
+ // Creation of the current feature s
+ s.buildFrom(0, 0);
+
+ s.print(); // print all the 2 components of the feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeatureLine::selectRho()); // print only the rho component
+ \endcode
+*/
+
+void
+vpFeatureLine::print(const unsigned int select ) const
+{
+
+ std::cout <<"Line:\t " << A <<"X+" << B <<"Y+" << C <<"Z +" << D <<"=0" <<std::endl ;;
+ if (vpFeatureLine::selectRho() & select )
+ std::cout << " \trho=" << s[0] ;
+ if (vpFeatureLine::selectTheta() & select )
+ std::cout << " \ttheta=" << s[1] ;
+ std::cout <<std::endl ;
+}
+
+
+/*!
+
+ Build a 2D line visual feature from the line equation parameters \f$
+ \rho \f$ and \f$ \theta \f$ given in the image plan.
+
+ \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f]
+
+ See the vpFeatureLine class description for more details about \f$
+ \rho \f$ and \f$ \theta \f$.
+
+ \param rho : The \f$ \rho \f$ parameter.
+ \param theta : The \f$ \theta \f$ parameter.
+
+*/
+void
+vpFeatureLine::buildFrom(const double rho, const double theta)
+{
+ s[0] = rho ;
+ s[1] = theta ;
+ for( int i = 0; i < 2; i++) flags[i] = true;
+}
+
+
+/*!
+
+ Build a 2D line visual feature from the line equation parameters \f$
+ \rho \f$ and \f$ \theta \f$ given in the image plan. The
+ parameters A, B, C and D which describe the equation of a plan to
+ which the line belongs, are set in the same time.
+
+ \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f]
+ \f[ AX + BY + CZ + D = 0 \f]
+
+ See the vpFeatureLine class description for more details about \f$
+ \rho \f$ and \f$ \theta \f$.
+
+ The A, B, C, D parameters are needed to compute the interaction
+ matrix associated to a visual feature. Normally, two plans are
+ needed to describe a line (the intersection of those two plans). But
+ to compute the interaction matrix only one plan equation is
+ required. The only one restrictions is that the value of D must not
+ be equal to zero !
+
+ \param rho : The \f$ \rho \f$ parameter.
+ \param theta : The \f$ \theta \f$ parameter.
+ \param A_ : A parameter of the plan equation.
+ \param B_ : B parameter of the plan equation.
+ \param C_ : C parameter of the plan equation.
+ \param D_ : D parameter of the plan equation.
+
+*/
+void vpFeatureLine::buildFrom(const double rho, const double theta,
+ const double A_, const double B_,
+ const double C_, const double D_)
+{
+ s[0] = rho ;
+ s[1] = theta ;
+ this->A = A_ ;
+ this->B = B_ ;
+ this->C = C_ ;
+ this->D = D_ ;
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureLine s;
+ s_star = s.duplicate(); // s_star is now a vpFeatureLine
+ \endcode
+
+*/
+vpFeatureLine *vpFeatureLine::duplicate() const
+{
+ vpFeatureLine *feature = new vpFeatureLine ;
+ return feature ;
+}
+
+
+
+/*!
+
+ Display line feature.
+
+ \param cam : Camera parameters.
+ \param I : Image on which features have to be displayed.
+ \param color : Color used to display the feature.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeatureLine::display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double rho,theta ;
+ rho = getRho() ;
+ theta = getTheta() ;
+
+ vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display line feature.
+
+ \param cam : Camera parameters.
+ \param I : Color image on which features have to be displayed.
+ \param color : Color used to display the feature.
+ \param thickness : Thickness of the feature representation.
+
+ */
+void
+vpFeatureLine::display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double rho,theta ;
+ rho = getRho() ;
+ theta = getTheta() ;
+
+ vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Function used to select the \f$ \rho \f$ subset of the line visual feature.
+
+ This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \rho \f$.
+
+ This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeatureLine s;
+ vpServo task;
+ ...
+ // Add the (rho) subset features from the 2D line
+ task.addFeature(s, vpFeatureLine::selectRho());
+ \endcode
+*/
+unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0] ; }
+
+/*!
+
+ Function used to select the \f$ \theta \f$ subset of the line visual feature.
+
+ This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \theta \f$.
+
+ This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeatureLine s;
+ vpServo task;
+ ...
+ // Add the (rho) subset features from the 2D line
+ task.addFeature(s, vpFeatureLine::selectTheta());
+ \endcode
+*/
+unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp
new file mode 100644
index 0000000..98935ea
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp
@@ -0,0 +1,422 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Luminance feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpException.h>
+
+#include <visp3/visual_features/vpFeatureLuminance.h>
+
+
+/*!
+ \file vpFeatureLuminance.cpp
+ \brief Class that defines the image luminance visual feature
+
+ For more details see \cite Collewet08c.
+*/
+
+
+
+/*!
+ Initialize the memory space requested for vpFeatureLuminance visual feature.
+*/
+void
+vpFeatureLuminance::init()
+{
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //default value Z (1 meters)
+ Z = 1;
+
+ firstTimeIn =0 ;
+
+ nbr = nbc = 0;
+}
+
+
+void
+vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
+{
+ init() ;
+
+ nbr = _nbr ;
+ nbc = _nbc ;
+
+ if((nbr < 2*bord) || (nbc < 2*bord)){
+ throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
+ }
+
+ // number of feature = nb column x nb lines in the images
+ dim_s = (nbr-2*bord)*(nbc-2*bord) ;
+
+ s.resize(dim_s) ;
+
+ if (pixInfo != NULL)
+ delete [] pixInfo;
+
+ pixInfo = new vpLuminance[dim_s] ;
+
+ Z = _Z ;
+}
+
+/*!
+ Default constructor that build a visual feature.
+*/
+vpFeatureLuminance::vpFeatureLuminance()
+ : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
+{
+ nbParameters = 1;
+ dim_s = 0 ;
+ flags = NULL;
+
+ init() ;
+}
+
+/*!
+ Copy constructor.
+ */
+vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance& f)
+ : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
+{
+ *this = f;
+}
+
+/*!
+ Copy operator.
+ */
+vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance& f)
+{
+ Z = f.Z;
+ nbr = f.nbr;
+ nbc = f.nbc;
+ bord = f.bord;
+ firstTimeIn = f.firstTimeIn;
+ cam = f.cam;
+ if (pixInfo)
+ delete [] pixInfo;
+ pixInfo = new vpLuminance[dim_s] ;
+ for(unsigned int i=0; i< dim_s; i++)
+ pixInfo[i] = f.pixInfo[i];
+ return (*this);
+}
+
+/*!
+ Destructor that free allocated memory.
+*/
+vpFeatureLuminance::~vpFeatureLuminance()
+{
+ if (pixInfo != NULL) delete [] pixInfo ;
+}
+
+/*!
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \param Z_ : \f$ Z \f$ value to set.
+*/
+void
+vpFeatureLuminance::set_Z(const double Z_)
+{
+ this->Z = Z_ ;
+ flags[0] = true;
+}
+
+
+/*!
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \return The value of \f$ Z \f$.
+*/
+double
+vpFeatureLuminance::get_Z() const
+{
+ return Z ;
+}
+
+
+void
+vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam)
+{
+ cam = _cam ;
+}
+
+
+/*!
+
+ Build a luminance feature directly from the image
+*/
+
+void
+vpFeatureLuminance::buildFrom(vpImage<unsigned char> &I)
+{
+ unsigned int l = 0;
+ double Ix,Iy ;
+
+ double px = cam.get_px() ;
+ double py = cam.get_py() ;
+
+
+ if (firstTimeIn==0)
+ {
+ firstTimeIn=1 ;
+ l =0 ;
+ for (unsigned int i=bord; i < nbr-bord ; i++)
+ {
+ // cout << i << endl ;
+ for (unsigned int j = bord ; j < nbc-bord; j++)
+ { double x=0,y=0;
+ vpPixelMeterConversion::convertPoint(cam,
+ j,i,
+ x,y) ;
+
+ pixInfo[l].x = x;
+ pixInfo[l].y = y;
+
+ pixInfo[l].Z = Z ;
+
+ l++;
+ }
+ }
+ }
+
+ l= 0 ;
+ for (unsigned int i=bord; i < nbr-bord ; i++)
+ {
+ // cout << i << endl ;
+ for (unsigned int j = bord ; j < nbc-bord; j++)
+ {
+ // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
+ Ix = px * vpImageFilter::derivativeFilterX(I,i,j) ;
+ Iy = py * vpImageFilter::derivativeFilterY(I,i,j) ;
+
+ // Calcul de Z
+
+ pixInfo[l].I = I[i][j] ;
+ s[l] = I[i][j] ;
+ pixInfo[l].Ix = Ix;
+ pixInfo[l].Iy = Iy;
+
+ l++;
+ }
+ }
+
+}
+
+
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L_I \f$. The computation is made
+ thanks to the values of the luminance features \f$ I \f$
+*/
+void
+vpFeatureLuminance::interaction(vpMatrix &L)
+{
+ double x,y,Ix,Iy,Zinv;
+
+ L.resize(dim_s,6) ;
+
+ for(unsigned int m = 0; m< L.getRows(); m++)
+ {
+ Ix = pixInfo[m].Ix;
+ Iy = pixInfo[m].Iy;
+
+ x = pixInfo[m].x ;
+ y = pixInfo[m].y ;
+ Zinv = 1 / pixInfo[m].Z;
+
+ {
+ L[m][0] = Ix * Zinv;
+ L[m][1] = Iy * Zinv;
+ L[m][2] = -(x*Ix+y*Iy)*Zinv;
+ L[m][3] = -Ix*x*y-(1+y*y)*Iy;
+ L[m][4] = (1+x*x)*Ix + Iy*x*y;
+ L[m][5] = Iy*x-Ix*y;
+ }
+ }
+}
+
+/*!
+ Compute and return the interaction matrix \f$ L_I \f$. The computation is made
+ thanks to the values of the luminance features \f$ I \f$
+*/
+vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
+{
+ /* static */ vpMatrix L ; // warning C4640: 'L' : construction of local static object is not thread-safe
+ interaction(L) ;
+ return L ;
+}
+
+
+/*!
+ Compute the error \f$ (I-I^*)\f$ between the current and the desired
+
+ \param s_star : Desired visual feature.
+ \param e : Error between the current and the desired features.
+
+*/
+void
+vpFeatureLuminance::error(const vpBasicFeature &s_star,
+ vpColVector &e)
+{
+ e.resize(dim_s) ;
+
+ for (unsigned int i =0 ; i < dim_s ; i++)
+ {
+ e[i] = s[i] - s_star[i] ;
+ }
+}
+
+
+
+/*!
+ Compute the error \f$ (I-I^*)\f$ between the current and the desired
+
+ \param s_star : Desired visual feature.
+ \param select : Not used.
+
+*/
+vpColVector
+vpFeatureLuminance::error(const vpBasicFeature &s_star,
+ const unsigned int /* select */)
+{
+ /* static */ vpColVector e ; // warning C4640: 'e' : construction of local static object is not thread-safe
+
+ error(s_star, e) ;
+
+ return e ;
+
+}
+
+
+
+
+/*!
+
+ Not implemented.
+
+ */
+void
+vpFeatureLuminance::print(const unsigned int /* select */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+ }
+
+
+
+/*!
+
+ Not implemented.
+
+ */
+void
+vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
+ const vpImage<unsigned char> & /* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*!
+
+ Not implemented.
+
+ */
+void
+vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
+ const vpImage<vpRGBa> & /* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureLuminance s;
+ s_star = s.duplicate(); // s_star is now a vpFeatureLuminance
+ \endcode
+
+*/
+vpFeatureLuminance *vpFeatureLuminance::duplicate() const
+{
+ vpFeatureLuminance *feature = new vpFeatureLuminance ;
+ return feature ;
+}
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp
new file mode 100644
index 0000000..3ee8270
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Base for all moment features
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/core/vpMoment.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+#include <visp3/core/vpDebug.h>
+#include <vector>
+
+class vpBasicFeature;
+
+/*!
+ Initialize common parameters for moment features.
+*/
+void vpFeatureMoment::init (){
+ //feature dimension
+ /*
+ * The dimension of the visual feature is set according to the size of the vpMoment associated to it.
+ * This partly explains why vpFeatureMomentBasic cannot be used directly as a visual feature.
+ */
+ if(this->moment!=NULL)
+ dim_s = (unsigned int)this->moment->get().size();
+ else
+ dim_s = 0;
+
+ nbParameters = 1;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ for(unsigned int i=0;i<dim_s;i++)
+ s[i] = 0;
+
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+}
+
+
+/*!
+ Feature's dimension according to selection.
+*/
+int vpFeatureMoment::getDimension (unsigned int select) const{
+ int dim=0;
+
+ for(unsigned int i=0;i<dim_s;++i)
+ if(vpBasicFeature::FEATURE_LINE[i] & select)
+ dim++;
+
+ return dim;
+}
+
+
+/*!
+ Outputs the content of the feature: it's corresponding selected moments.
+*/
+void vpFeatureMoment::print (unsigned int select) const{
+ for(unsigned int i=0;i<dim_s;++i){
+ if(vpBasicFeature::FEATURE_LINE[i] & select){
+ std::cout << s[i] << ",";
+ }
+ }
+
+ std::cout << std::endl;
+}
+
+/*!
+ Not implemented since visual representation of a moment doesn't often make sense.
+*/
+void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
+ const vpColor &color, unsigned int thickness) const
+{
+ //visual representation of a moment doesn't often make sense
+ (void)cam;
+ (void)I;
+ (void)color;
+ (void)thickness;
+}
+
+/*!
+ Not implemented since visual representation of a moment doesn't often make sense.
+*/
+void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
+ const vpColor &color, unsigned int thickness) const
+{
+ (void)cam;
+ (void)I;
+ (void)color;
+ (void)thickness;
+}
+
+/*!
+ Updates the interaction matrices with the image plane the camera is facing. The plane must be in the format: \f$ \frac{1}{Z}=Ax+By+C \f$ .
+ The moment primitives MUST be updated before calling this function.
+
+ This method also computes the interaction matrix. Therefore, you must call vpFeatureMoment::update before calling vpFeatureMoment::interaction.
+
+ \attention The behaviour of this method is not the same as vpMoment::update which only acknowledges the new object. This method also computes the interaction matrices.
+
+ \param A_ : A coefficient of the plane.
+ \param B_ : B coefficient of the plane.
+ \param C_ : C coefficient of the plane.
+*/
+void vpFeatureMoment::update (double A_, double B_, double C_){
+ this->A = A_;
+ this->B = B_;
+ this->C = C_;
+
+ if(moment==NULL){
+ bool found;
+ this->moment = &(moments.get(momentName(),found));
+ if(!found) throw vpException(vpException::notInitialized,"Moment not found for feature");
+ }
+ nbParameters = 1;
+ if(this->moment!=NULL){
+ dim_s = (unsigned int)this->moment->get().size();
+
+ s.resize(dim_s);
+
+ for(unsigned int i=0;i<dim_s;i++)
+ s[i] = this->moment->get()[i];
+
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+ }else
+ dim_s = 0;
+
+ compute_interaction();
+}
+
+/*!
+ Retrieves the interaction matrix. No computation is done.
+
+ \param select : Feature selector.
+
+ \return The corresponding interaction matrix.
+
+ There is no rule about the format of the feature selector. It may be
+ different for different features. For example, for
+ vpFeatureMomentBasic or vpFeatureMomentCentered features, select may
+ refer to the \f$ (i,j) \f$ couple in the \f$ j \times order + i \f$
+ format, but for vpFeatureMomentCInvariant the selector allows to
+ select couples \f$ (i,j,k,l...) \f$ in the following format: 1 << i
+ + 1 << j + 1 << k + 1 << l.
+*/
+vpMatrix vpFeatureMoment::interaction (unsigned int select){
+ vpMatrix L(0,0);
+
+ for(unsigned int i=0;i<dim_s;++i){
+ if(vpBasicFeature::FEATURE_LINE[i] & select){
+ L.stack(interaction_matrices[i]);
+ }
+ }
+
+ return L;
+}
+
+/*! Duplicates the feature into a vpGenericFeature harbouring the
+ same properties. The resulting feature is of vpMomentGenericFeature
+ type. While it still can compute interaction matrices and has acces
+ to it's moment primitive, it has lost all precise information about
+ its precise type and therefore cannot be used in a feature database.
+
+ \return The corresponding feature.
+*/
+vpBasicFeature* vpFeatureMoment::duplicate () const
+{
+ vpFeatureMoment* feat = new vpMomentGenericFeature(moments,A,B,C,featureMomentsDataBase,moment);
+ feat->interaction_matrices = interaction_matrices;
+ feat->dim_s = dim_s;
+ feat->nbParameters = nbParameters;
+ // memory allocation
+ feat->s.resize(dim_s) ;
+ for(unsigned int i=0;i<dim_s;i++)
+ feat->s[i] = this->s[i];
+
+ feat->flags = new bool[(unsigned int)nbParameters];
+ for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
+ feat->flags[i] = flags[i];
+
+ return feat;
+}
+
+/*!
+ Links the feature to the feature's database. NB: The feature's database is different from the moment's database.
+ \param featureMoments : database in which the moment features are stored.
+
+*/
+void vpFeatureMoment::linkTo(vpFeatureMomentDatabase& featureMoments){
+ if (strlen( name() ) >= 255) {
+ throw(vpException(vpException::memoryAllocationError,
+ "Not enough memory to intialize the moment name"));
+ }
+
+ std::strcpy(_name,name());
+ this->featureMomentsDataBase=&featureMoments;
+
+ featureMoments.add(*this,_name);
+}
+
+
+void vpFeatureMoment::compute_interaction (){
+
+}
+
+vpFeatureMoment::~vpFeatureMoment (){
+}
+
+VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM) {
+ /*
+ A const_cast is forced here since interaction() defined in vpBasicFeature() is not const
+ But introducing const in vpBasicFeature() can break a lot of client code
+ */
+ vpMatrix Lcomplete((unsigned int)featM.getDimension(), 6); // 6 corresponds to 6velocities in standard interaction matrix
+ Lcomplete = const_cast<vpFeatureMoment&>(featM).interaction(vpBasicFeature::FEATURE_ALL);
+ Lcomplete.matlabPrint(os);
+ return os;
+}
+
+/*!
+Interface function to display the moments and other interaction matrices
+on which a particular vpFeatureMoment is dependent upon
+Not made pure to maintain compatibility
+Recommended : Types inheriting from vpFeatureMoment should implement this function
+*/
+void
+vpFeatureMoment::printDependencies(std::ostream& os) const{
+ os << " WARNING : Falling back to base class version of printDependencies() in vpFeatureMoment. To prevent that, this has to be implemented in the derived classes!" << std::endl;
+}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp
new file mode 100644
index 0000000..a560ee9
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for alpha moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentAlpha.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+#include <vector>
+#include <limits>
+
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+
+/*!
+ Computes interaction matrix for alpha moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+*/
+void vpFeatureMomentAlpha::compute_interaction(){
+ bool found_moment_centered;
+ bool found_FeatureMoment_centered;
+
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
+ vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
+
+ double multiplier = -1./(momentCentered.get(2,0)*momentCentered.get(2,0)-2*momentCentered.get(0,2)*momentCentered.get(2,0)+4*momentCentered.get(1,1)*momentCentered.get(1,1)+momentCentered.get(0,2)*momentCentered.get(0,2));
+
+ interaction_matrices[0].resize(1,6);
+ interaction_matrices[0] = multiplier*(momentCentered.get(1,1)*featureMomentCentered.interaction(2,0)+(momentCentered.get(0,2)-momentCentered.get(2,0))*featureMomentCentered.interaction(1,1)-momentCentered.get(1,1)*featureMomentCentered.interaction(0,2));
+}
+
+#else
+
+/*!
+ Computes interaction matrix for alpha moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentGravityCenter
+*/
+void vpFeatureMomentAlpha::compute_interaction(){
+ bool found_moment_centered;
+ bool found_moment_gravity;
+
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ const vpMomentObject& momentObject = moment->getObject();
+
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ double mu11 = momentCentered.get(1,1);
+ double mu20 = momentCentered.get(2,0);
+ double mu02 = momentCentered.get(0,2);
+ double mu12 = momentCentered.get(1,2);
+ double mu21 = momentCentered.get(2,1);
+ double mu03 = momentCentered.get(0,3);
+ double mu30 = momentCentered.get(3,0);
+
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double Avx,Avy,Avz,Awx,Awy;
+ double beta,gamma;
+
+ if (momentObject.getType()==vpMomentObject::DISCRETE){
+ beta = 4;
+ gamma = 2;
+ } else {
+ beta = 5;
+ gamma = 1;
+ }
+
+ double d = (mu20-mu02)*(mu20-mu02) + 4 * mu11*mu11;
+ double DA = mu20+mu02;
+ double DA_2 = DA*DA;
+ double mu11_2 = mu11*mu11;
+
+ Avx = mu11*DA*A/d+(DA*mu02+(0.5)*d-(0.5)*DA_2)*B/d;
+ Avy = (DA*mu02-(0.5)*d-(.5)*DA_2)*A/d-B*mu11*DA/d;
+
+ Awx = (beta*(mu12*(mu20-mu02)+mu11*(mu03-mu21))+gamma*Xg*(mu02*(mu20-mu02)-2*mu11_2)+gamma*Yg*mu11*(mu20+mu02))/d;
+ Awy = (beta*(mu21*(mu02-mu20)+mu11*(mu30-mu12))+gamma*Xg*mu11*(mu20+mu02)+gamma*Yg*(mu20*(mu02-mu20)-2*mu11_2))/d;
+
+ Avz = B*Awx-A*Awy;
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1,6);
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Avx;
+ interaction_matrices[0][0][VY] = Avy;
+ interaction_matrices[0][0][VZ] = Avz;
+
+ interaction_matrices[0][0][WX] = Awx;
+ interaction_matrices[0][0][WY] = Awy;
+ interaction_matrices[0][0][WZ] = -1.;
+}
+
+vpColVector vpFeatureMomentAlpha::error (const vpBasicFeature &s_star, const unsigned int /* select */){
+ vpColVector e(0) ;
+ double err = s[0] - s_star[0] ;
+
+ if (err < -M_PI) err += 2*M_PI ;
+ if (err > M_PI) err -= 2*M_PI ;
+
+ vpColVector ecv(1) ;
+ ecv[0] = err ;
+ e = vpColVector::stack(e,ecv) ;
+
+ return e;
+}
+#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp
new file mode 100644
index 0000000..b0857cb
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation of vpFeatureMomentArea associated to vpMomentArea
+ *
+ * Authors:
+ * Manikandan Bakthavatchalam
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+#include <vector>
+#include <limits>
+#include <cassert>
+#include <limits> // numeric_limits
+
+#include <visp3/visual_features/vpFeatureMomentArea.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentArea.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+/*!
+ Computes interaction matrix for the normalized surface moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentGravityCenter
+ - vpMomentArea
+*/
+void vpFeatureMomentArea::compute_interaction(){
+
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1,6);
+
+ // Retreive the moment object associated with this feature
+ vpMomentObject mobj = moment->getObject();
+ if (mobj.getType()==vpMomentObject::DISCRETE) {
+ /*
+ * The interaction matrix for the discrete case is zero
+ * since the feature m00 is constant.
+ * Refer thesis of Omar Tahri 2005 [Section 3.4.22]
+ */
+ interaction_matrices[0][0][0] = 0.;
+ interaction_matrices[0][0][1] = 0.;
+ interaction_matrices[0][0][2] = 0.;
+ interaction_matrices[0][0][3] = 0.;
+ interaction_matrices[0][0][4] = 0.;
+ interaction_matrices[0][0][5] = 0.;
+ }
+ else {
+ // Get Xg and Yg
+ bool found_xgyg;
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_xgyg));
+ if (!found_xgyg) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ bool found_m00;
+ const vpMomentArea& areamoment = static_cast<const vpMomentArea&>(moments.get("vpMomentArea", found_m00));
+ if (!found_m00) throw vpException(vpException::notInitialized,"vpMomentArea not found");
+
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double a = areamoment.get()[0]; // Area scalar
+
+ assert(std::fabs(a-mobj.get(0,0)) < a*std::numeric_limits<double>::epsilon());
+
+ interaction_matrices[0][0][0] = -a*A;
+ interaction_matrices[0][0][1] = -a*B;
+ interaction_matrices[0][0][2] = (3*a)*(A*Xg+B*Yg)+(2*C*a);
+ interaction_matrices[0][0][3] = 3*a*Yg;
+ interaction_matrices[0][0][4] = -3*a*Xg;
+ interaction_matrices[0][0][5] = 0.;
+ }
+}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
new file mode 100644
index 0000000..7af7d53
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+
+/*!
+ Computes interaction matrix for the normalized surface moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentAreaNormalized
+ - vpFeatureMomentBasic
+*/
+void vpFeatureMomentAreaNormalized::compute_interaction(){
+ bool found_moment_centered;
+ bool found_moment_surface_normalized;
+ bool found_FeatureMoment_centered;
+
+ bool found_featuremoment_basic;
+ vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
+
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
+ const vpMomentObject& momentObject = moment->getObject();
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
+ vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
+
+ if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
+ if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1,6);
+ double normalized_multiplier;
+ double a;
+ vpMatrix La;
+ if(momentObject.getType()==vpMomentObject::DISCRETE){
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ La = featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2);
+ }else{
+ a = momentObject.get(0,0);
+ La = featureMomentBasic.interaction(0,0);
+ }
+
+ normalized_multiplier = (-momentSurfaceNormalized.getDesiredDepth()/(2*a))*sqrt(momentSurfaceNormalized.getDesiredSurface()/a);
+ interaction_matrices[0] = normalized_multiplier*La;
+
+}
+
+#else
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+
+/*!
+ Computes interaction matrix for the normalized surface moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentAreaNormalized
+ - vpMomentGravityCenter
+*/
+void vpFeatureMomentAreaNormalized::compute_interaction(){
+ bool found_moment_centered;
+ bool found_moment_surface_normalized;
+ bool found_moment_gravity;
+
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ const vpMomentObject& momentObject = moment->getObject();
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
+
+ if (!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if (!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if (!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1,6);
+ double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
+ double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
+ double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double An=momentSurfaceNormalized.get()[0];
+
+ double Xn = An*Xg;
+ double Yn = An*Yg;
+
+ double Anvx,Anvy,Anvz,Anwx,Anwy;
+
+ if (momentObject.getType()==vpMomentObject::DISCRETE) {
+ double a = momentCentered.get(2,0)+momentCentered.get(0,2);
+
+ double e01 = momentCentered.get(0,1)/a;
+ double e10 = momentCentered.get(1,0)/a;
+ double e11 = momentCentered.get(1,1)/a;
+ double e02 = momentCentered.get(0,2)/a;
+ double e20 = momentCentered.get(2,0)/a;
+ double e12 = momentCentered.get(1,2)/a;
+ double e21 = momentCentered.get(2,1)/a;
+ double e03 = momentCentered.get(0,3)/a;
+ double e30 = momentCentered.get(3,0)/a;
+
+ Anvx = An*A*e20+An*B*e11;
+ Anvy = An*A*e11+An*B*e02;
+
+ Anwx = (n02*e01+n11*e10-e03-e21)*An-Xn*e11+(-1-e02)*Yn;
+ Anwy = (e12+e30-n11*e01-n20*e10)*An+(2-e02)*Xn+Yn*e11;
+
+ Anvz = -An*C+B*Anwx-A*Anwy;
+
+ } else {
+ Anvx = A*An/2.;
+ Anvy = B*An/2.;
+ Anvz = -An*C-(3./2.)*A*Xn-(3./2.)*B*Yn;
+
+ Anwx = -(3./2.)*Yn;
+ Anwy = (3./2.)*Xn;
+ }
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Anvx;
+ interaction_matrices[0][0][VY] = Anvy;
+ interaction_matrices[0][0][VZ] = Anvz;
+
+ interaction_matrices[0][0][WX] = Anwx;
+ interaction_matrices[0][0][WY] = Anwy;
+ interaction_matrices[0][0][WZ] = 0.;
+}
+
+
+#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
new file mode 100644
index 0000000..b0b997a
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <vector>
+#include <limits>
+/*!
+ Default constructor.
+ \param data_base : Database of moment primitives.
+ \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param featureMoments : Database of features.
+*/
+vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments) :
+ vpFeatureMoment(data_base,A_,B_,C_,featureMoments), order(0)
+{
+}
+
+/*!
+ Computes interaction matrix for basic moment. Called internally.
+ The moment primitives must be computed before calling this.
+*/
+void vpFeatureMomentBasic::compute_interaction(){
+ int delta;
+ const vpMomentObject& m = moment->getObject();
+ order = m.getOrder()+1;
+ interaction_matrices.resize(order*order);
+ for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end();i++)
+ i->resize(1,6);
+ if (m.getType()==vpMomentObject::DISCRETE){
+ delta=0;
+ } else {
+ delta=1;
+ }
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ //i=0;j=0
+ interaction_matrices[0][0][VX] = -delta*A*m.get(0, 0);
+ interaction_matrices[0][0][VY] = -delta*B*m.get(0, 0);
+ interaction_matrices[0][0][VZ] = 3*delta*(A*m.get(1, 0)+B*m.get(0, 1)+C*m.get(0, 0))-delta*C*m.get(0, 0);
+
+ interaction_matrices[0][0][WX] = 3*delta*m.get(0, 1);
+ interaction_matrices[0][0][WY] = -3*delta*m.get(1, 0);
+ interaction_matrices[0][0][WZ] = 0;
+
+ // int i=0;
+ for(int j=1;j<(int)order-1;j++){
+ unsigned int j_ = (unsigned int) j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+
+ interaction_matrices[j_*order][0][VX] = -delta*A*m.get(0, j_);
+ interaction_matrices[j_*order][0][VY] = -j*(A*m.get(1,jm1_)+B*m.get(0,j_)+C*m.get(0,jm1_))-delta*B*m.get(0,j_);
+ interaction_matrices[j_*order][0][VZ] = (j+3*delta)*(A*m.get(1,j_)+B*m.get(0,jp1_)+C*m.get(0,j_))-delta*C*m.get(0,j_);
+
+ interaction_matrices[j_*order][0][WX] = (j+3*delta)*m.get(0,jp1_)+j*m.get(0,jm1_);
+ interaction_matrices[j_*order][0][WY] = -(j+3*delta)*m.get(1,j_);
+ interaction_matrices[j_*order][0][WZ] = -j*m.get(1,jm1_);
+ }
+
+ //int j=0;
+ for(int i=1;i<(int)order-1;i++){
+ unsigned int i_ = (unsigned int) i;
+ unsigned int im1_ = i_ - 1;
+ unsigned int ip1_ = i_ + 1;
+
+ interaction_matrices[i_][0][VX] = -i*(A*m.get(i_, 0)+B*m.get(im1_, 1)+C*m.get(im1_, 0))-delta*A*m.get(i_, 0);
+ interaction_matrices[i_][0][VY] = -delta*B*m.get(i_, 0);
+ interaction_matrices[i_][0][VZ] = (i+3*delta)*(A*m.get(ip1_, 0)+B*m.get(i_, 1)+C*m.get(i_, 0))-delta*C*m.get(i_, 0);
+
+ interaction_matrices[i_][0][WX] = (i+3*delta)*m.get(i_, 1);
+ interaction_matrices[i_][0][WY] = -(i+3*delta)*m.get(ip1_, 0)-i*m.get(im1_, 0);
+ interaction_matrices[i_][0][WZ] = i*m.get(im1_, 1);
+ }
+
+ for(int j=1; j<(int)order-1; j++){
+ unsigned int j_ = (unsigned int) j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+
+ for(int i=1; i<(int)order-j-1; i++){
+ unsigned int i_ = (unsigned int) i;
+ unsigned int im1_ = i_ - 1;
+ unsigned int ip1_ = i_ + 1;
+
+ interaction_matrices[j_*order+i_][0][VX] = -i*(A*m.get(i_, j_)+B*m.get(im1_, jp1_)+C*m.get(im1_,j_))-delta*A*m.get(i_, j_);
+ interaction_matrices[j_*order+i_][0][VY] = -j*(A*m.get(ip1_, jm1_)+B*m.get(i_, j_)+C*m.get(i_,jm1_))-delta*B*m.get(i_, j_);
+ interaction_matrices[j_*order+i_][0][VZ] = (i+j+3*delta)*(A*m.get(ip1_, j_)+B*m.get(i_,jp1_)+C*m.get(i_, j_))-delta*C*m.get(i_,j_);
+
+ interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*m.get(i_, jp1_)+j*m.get(i_, jm1_);
+ interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*m.get(ip1_, j_)-i*m.get(im1_, j_);
+ interaction_matrices[j_*order+i_][0][WZ] = i*m.get(im1_,jp1_)-j*m.get(ip1_, jm1_);
+ }
+ }
+}
+
+/*!
+Interaction matrix corresponding to \f$ m_{ij} \f$ moment.
+\param select_one : first index (i).
+\param select_two : second index (j).
+\return Interaction matrix \f$ L_{m_{ij}} \f$ corresponding to the moment.
+*/
+vpMatrix vpFeatureMomentBasic::interaction (unsigned int select_one,unsigned int select_two) const {
+ if(select_one+select_two>moment->getObject().getOrder())
+ throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
+ return interaction_matrices[select_two*order+select_one];
+}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp
new file mode 100644
index 0000000..3edaff4
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp
@@ -0,0 +1,420 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+#include <visp3/core/vpMomentObject.h>
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+#include <vector>
+#include <limits>
+
+
+/*!
+ Computes interaction matrix for space-scale-rotation invariants. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentCInvariant
+ - vpFeatureMomentBasic
+*/
+void vpFeatureMomentCInvariant::compute_interaction(){
+ std::vector<vpMatrix> LI(16);
+ bool found_moment_centered;
+ bool found_moment_cinvariant;
+ bool found_FeatureMoment_centered;
+ bool found_featuremoment_basic;
+
+ const vpMomentObject& momentObject = moment->getObject();
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
+ const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
+ vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
+
+ vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
+
+ if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
+
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
+ if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
+
+ vpMatrix zeros(1,6);
+ for(int i=0;i<6;i++) zeros[0][i]=0;
+
+
+ LI[1]=-featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) - momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) + 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+
+
+ LI[2]=2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) * (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) + 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+
+
+ LI[3]=2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) + 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) * (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
+
+
+ LI[4]=2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) + 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) * (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
+
+ LI[5]=-2*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)-4*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-12*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interac [...]
+
+
+ LI[6]=6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+2*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)-12*moment [...]
+
+
+ LI[7] = -3*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-2*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(3,0)-3*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+3*mome [...]
+
+ LI[8]=-2 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * featureMomentCentered.interaction(3, 0) + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) - 6 * featureMomentCentered.interaction(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) * [...]
+
+ LI[9] = 4*pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(3,0)+18*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+12*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*m [...]
+
+
+ LI[10]=featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) - 4 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) - 4 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) + 6 * momentCentered.get(2, 2) * featureMomentCentered.interaction(2, 2);
+
+ LI[11]=-3 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) - 3 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) - 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) - 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(3, 1) * featureMomentCentered.interaction(3, 1) + 2 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 2 * momentCentered.get(3, 1) * featureMome [...]
+
+
+ LI[12]=6 * momentCentered.get(4, 0) * featureMomentCentered.interaction(4, 0) + 12 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) + 12 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) + 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 16 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 16 * momentCentered.get(3, 1) * featureM [...]
+
+
+ LI[13]=2 * (momentCentered.get(5, 0) + 2 * momentCentered.get(3, 2) + momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) + 2 * featureMomentCentered.interaction(3, 2) + featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) + 2 * momentCentered.get(2, 3) + momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) + 2 * featureMomentCentered.interaction(2, 3) + featureMomentCentered.interaction(4, 1));
+
+ LI[14]=2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) - 3 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) - 3 * featureMomentCentered.interaction(4, 1));
+
+ LI[15]=2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) + 5 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) + 5 * featureMomentCentered.interaction(4, 1));
+
+ double s3 = momentCInvariant.getS(3);
+ double s2 = momentCInvariant.getS(2);
+ double c3 = momentCInvariant.getC(3);
+ double c2 = momentCInvariant.getC(2);
+ double I1 = momentCInvariant.getII(1);
+ double I2 = momentCInvariant.getII(2);
+ double I3 = momentCInvariant.getII(3);
+
+ vpMatrix Lc2 = featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1);
+ vpMatrix Ls2 = featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2);
+ vpMatrix Lc3 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) - 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
+ vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1,1) * (momentCentered.get(2,0) - momentCentered.get(0,2)) + 4 * momentCentered.get(1,1) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2));
+ vpMatrix LI1 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) + 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
+ vpMatrix LI2 = 2 * (momentCentered.get(0,3) - 3 * momentCentered.get(2,1)) * (featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1)) + 2 * (momentCentered.get(3,0) - 3 * momentCentered.get(1,2)) * (featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2));
+ vpMatrix LI3 = featureMomentCentered.interaction(2,0) + featureMomentCentered.interaction(0,2);
+
+
+ vpMatrix La(1,6);
+ double a;
+ if(momentObject.getType()==vpMomentObject::DISCRETE){
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
+ }else{
+ a = momentObject.get(0,0);
+ La = featureMomentBasic.interaction(0,0);
+ }
+ interaction_matrices.resize(14);
+
+
+ interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
+ interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
+
+ interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
+
+ interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
+
+ interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
+
+ interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
+
+
+ interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
+
+ interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
+
+ interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
+
+ interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
+
+
+ interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
+ interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
+}
+
+#else
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+#include <vector>
+#include <iostream>
+#include <limits>
+#include <cmath>
+
+/*!
+ Computes interaction matrix for space-scale-rotation invariants. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpFeatureMomentCentered
+ - vpMomentCInvariant
+ - vpFeatureMomentBasic
+*/
+void vpFeatureMomentCInvariant::compute_interaction(){
+
+ //std::vector<vpMatrix> LI(16);
+ LI.resize(16); // LI made class member
+
+ bool found_moment_centered;
+ bool found_moment_cinvariant;
+ bool found_FeatureMoment_centered;
+ bool found_featuremoment_basic;
+
+ const vpMomentObject& momentObject = moment->getObject();
+ const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
+ const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
+
+ vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
+
+ vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
+
+ if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
+ if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
+
+ vpMatrix zeros(1,6);
+ for(int i=0;i<6;i++) zeros[0][i]=0;
+
+ double mu30 = momentCentered.get(3,0);
+ double mu30_2 = mu30*mu30;
+ double mu30_3 = mu30_2*mu30;
+
+ double mu03 = momentCentered.get(0,3);
+ double mu03_2 = mu03*mu03;
+ double mu03_3 = mu03*mu03_2;
+
+ double mu20 = momentCentered.get(2,0);
+ double mu02 = momentCentered.get(0,2);
+ double mu50 = momentCentered.get(5,0);
+ double mu32 = momentCentered.get(3,2);
+ double mu14 = momentCentered.get(1,4);
+ double mu05 = momentCentered.get(0,5);
+ double mu23 = momentCentered.get(2,3);
+ double mu41 = momentCentered.get(4,1);
+ double mu40 = momentCentered.get(4,0);
+ double mu04 = momentCentered.get(0,4);
+ double mu31 = momentCentered.get(3,1);
+ double mu13 = momentCentered.get(1,3);
+ double mu22 = momentCentered.get(2,2);
+ double mu21 = momentCentered.get(2,1);
+ double mu12 = momentCentered.get(1,2);
+ double mu11 = momentCentered.get(1,1);
+
+ double mu12_2 = mu12*mu12;
+ double mu21_2 = mu21*mu21;
+ double mu21_3 = mu21*mu21_2;
+ double mu12_3 = mu12_2*mu12;
+
+ vpMatrix Lmu20 = featureMomentCentered.interaction(2,0);
+ vpMatrix Lmu02 = featureMomentCentered.interaction(0,2);
+ vpMatrix Lmu11 = featureMomentCentered.interaction(1,1);
+ vpMatrix Lmu12 = featureMomentCentered.interaction(1,2);
+ vpMatrix Lmu03 = featureMomentCentered.interaction(0,3);
+ vpMatrix Lmu21 = featureMomentCentered.interaction(2,1);
+ vpMatrix Lmu30 = featureMomentCentered.interaction(3,0);
+ vpMatrix Lmu40 = featureMomentCentered.interaction(4,0);
+ vpMatrix Lmu04 = featureMomentCentered.interaction(0,4);
+ vpMatrix Lmu31 = featureMomentCentered.interaction(3,1);
+ vpMatrix Lmu13 = featureMomentCentered.interaction(1,3);
+ vpMatrix Lmu22 = featureMomentCentered.interaction(2,2);
+ vpMatrix Lmu05 = featureMomentCentered.interaction(0,5);
+ vpMatrix Lmu14 = featureMomentCentered.interaction(1,4);
+ vpMatrix Lmu23 = featureMomentCentered.interaction(2,3);
+ vpMatrix Lmu32 = featureMomentCentered.interaction(3,2);
+ vpMatrix Lmu41 = featureMomentCentered.interaction(4,1);
+ vpMatrix Lmu50 = featureMomentCentered.interaction(5,0);
+
+ LI[1]= -Lmu20*mu02-mu20*Lmu02+2*mu11*Lmu11;
+
+ LI[2]= (-2*mu20+2*mu02)*Lmu02+8*mu11*Lmu11+(2*mu20-2*mu02)*Lmu20;
+
+ LI[3]= (-6*mu21+2*mu03)*Lmu03+(-6*mu30+18*mu12)*Lmu12+(18*mu21-6*mu03)*Lmu21+(2*mu30-6*mu12)*Lmu30;
+
+ LI[4]= (2*mu21+2*mu03)*Lmu03+(2*mu30+2*mu12)*Lmu12+(2*mu21+2*mu03)*Lmu21+(2*mu30+2*mu12)*Lmu30;
+
+ LI[5]= (-2*mu30_2*mu03+6*mu30*mu21*mu12-4*mu21_3)*Lmu03+(6*mu30*mu21*mu03-12*mu30*mu12_2+6*mu21_2*mu12)*Lmu12+(6*mu30*mu12*mu03-12*mu21_2*mu03+6*mu21*mu12_2)*Lmu21+(-2*mu30*mu03_2-4*mu12_3+6*mu21*mu12*mu03)*Lmu30;
+
+ LI[6]= (-6*mu30*mu21*mu12-6*mu21*mu12_2+6*mu21_2*mu03+2*mu21_3+4*mu30_2*mu03)*Lmu03+(-6*mu30*mu21_2-6*mu30*mu21*mu03+12*mu12_3+6*mu30_2*mu12-12*mu21*mu12*mu03+6*mu30*mu12_2)*Lmu12+(6*mu21*mu03_2+6*mu21_2*mu03-6*mu30*mu12*mu03+12*mu21_3-12*mu30*mu21*mu12-6*mu12_2*mu03)*Lmu21+(6*mu30*mu12_2+2*mu12_3+4*mu30*mu03_2-6*mu21_2*mu12-6*mu21*mu12*mu03)*Lmu30;
+
+ LI[7] = (-6*mu21_2*mu12+3*mu30*mu03_2-mu30_3-3*mu30*mu21_2-6*mu21*mu12*mu03+3*mu30*mu12_2+2*mu12_3)*Lmu03+(-3*mu21*mu03_2+12*mu30*mu21*mu12+6*mu30*mu12*mu03+3*mu30_2*mu21+9*mu21*mu12_2-6*mu21_2*mu03-3*mu21_3+6*mu12_2*mu03)*Lmu12+(3*mu30_2*mu12-9*mu21_2*mu12-12*mu21*mu12*mu03-6*mu30*mu21*mu03-6*mu30*mu21_2+6*mu30*mu12_2+3*mu12_3-3*mu12*mu03_2)*Lmu21+(6*mu21*mu12_2+6*mu30*mu21*mu12-3*mu30_2*mu03+3*mu12_2*mu03-3*mu21_2*mu03-2*mu21_3+mu03_3)*Lmu30;
+
+ LI[8]= (6*mu21_3-2*mu30*mu12*mu03+2*mu12_2*mu03+3*mu21*mu12_2-6*mu30*mu21*mu12-mu30_2*mu21-4*mu21_2*mu03-3*mu21*mu03_2)*Lmu03+(2*mu12*mu03_2-4*mu30_2*mu12+9*mu30*mu12_2-mu30*mu03_2-6*mu30*mu21*mu03+3*mu30*mu21_2+6*mu21*mu12*mu03-mu30_3)*Lmu12+(18*mu21_2*mu03+6*mu30*mu21*mu12-4*mu21*mu03_2-mu03_3-mu30_2*mu03-6*mu30*mu12*mu03+3*mu12_2*mu03+2*mu30_2*mu21)*Lmu21+(-6*mu21*mu12*mu03-4*mu30*mu12_2-2*mu30*mu21*mu03+2*mu30*mu21_2+3*mu12_3+3*mu21_2*mu12-3*mu30_2*mu12-mu12*mu03_2)*Lmu30;
+
+ LI[9] = (2*(2*mu03+3*mu21))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu03+6*mu30*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu12+6*mu03*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu21+(2*(2*mu30+3*mu12))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu30;
+
+ LI[10]= Lmu40*mu04+mu40*Lmu04-4*Lmu31*mu13-4*mu31*Lmu13+6*mu22*Lmu22;
+
+ LI[11]= (-2*mu40-3*mu22)*Lmu04+(2*mu31+6*mu13)*Lmu13+(-3*mu04-3*mu40)*Lmu22+(2*mu13+6*mu31)*Lmu31+(-3*mu22-2*mu04)*Lmu40;
+
+ LI[12]= (2*mu40+12*mu22+6*mu04)*Lmu04+16*mu31*Lmu13+(12*mu40+12*mu04)*Lmu22+16*Lmu31*mu13+(6*mu40+12*mu22+2*mu04)*Lmu40;
+
+ LI[13]=(2*mu05+4*mu23+2*mu41)*Lmu05+(2*mu50+4*mu32+2*mu14)*Lmu14+(4*mu05+8*mu23+4*mu41)*Lmu23+(4*mu50+8*mu32+4*mu14)*Lmu32+(2*mu05+4*mu23+2*mu41)*Lmu41+(2*mu50+4*mu32+2*mu14)*Lmu50 ;
+
+ LI[14]= (2*mu05-4*mu23-6*mu41)*Lmu05+(-6*mu50+12*mu32+18*mu14)*Lmu14+(-4*mu05+8*mu23+12*mu41)*Lmu23+(-4*mu50+8*mu32+12*mu14)*Lmu32+(-6*mu05+12*mu23+18*mu41)*Lmu41+(2*mu50-4*mu32-6*mu14)*Lmu50;
+
+ LI[15]= (2*mu05-20*mu23+10*mu41)*Lmu05+(10*mu50-100*mu32+50*mu14)*Lmu14+(-20*mu05+200*mu23-100*mu41)*Lmu23+(-20*mu50+200*mu32-100*mu14)*Lmu32+(10*mu05-100*mu23+50*mu41)*Lmu41+(2*mu50-20*mu32+10*mu14)*Lmu50;
+
+ double s3 = momentCInvariant.getS(3);
+ double s2 = momentCInvariant.getS(2);
+ double c3 = momentCInvariant.getC(3);
+ double c2 = momentCInvariant.getC(2);
+
+ double I1 = momentCInvariant.getII(1);
+ double I2 = momentCInvariant.getII(2);
+ double I3 = momentCInvariant.getII(3);
+
+ vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
+ double mu20__mu02 = mu20 - mu02;
+ vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
+ vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
+ vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02) - 8. * mu11 * Lmu11;
+ vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
+ vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
+ vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
+ vpMatrix LI3 = Lmu20 + Lmu02;
+
+ vpMatrix La(1,6);
+ double a;
+ if(momentObject.getType()==vpMomentObject::DISCRETE){
+ a = momentCentered.get(2,0)+momentCentered.get(0,2);
+ La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
+ }else{
+ a = momentObject.get(0,0);
+ La = featureMomentBasic.interaction(0,0);
+ }
+
+ interaction_matrices.resize(14);
+
+ /*
+ momentCInvariant.printInvariants(std::cout);
+ printLsofInvariants(std::cout);
+ */
+
+ interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
+
+ interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
+
+ interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
+
+ interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
+
+ interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
+
+ interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
+
+ interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
+
+ interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
+
+ interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
+
+ interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
+
+ interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
+
+ interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
+
+ /*
+ std::cout << (*this);
+ vpTRACE("Done.");
+ std::exit(-1);
+ */
+}
+
+/*!
+ Print out all invariants that were computed
+ There are 15 of them, as in [Point-based and region based.ITRO05]
+ \cite Tahri05z
+ */
+void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream& os) const{
+ for (unsigned int i = 1; i < 15; ++i){
+ os << "LI[" << i << "] = ";
+ LI[i].matlabPrint(os);
+ os << std::endl;
+ }
+}
+
+VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMomentCInvariant& featcinv)
+{
+ //Print L for c1 .. c10
+ for (unsigned int i = 0; i < 10; ++i){
+ os << "L_c[" << i << "] = ";
+ featcinv.interaction_matrices[i].matlabPrint(os);
+ os << std::endl;
+ }
+
+ // sx, sy
+ os << "L_sx = ";
+ featcinv.interaction_matrices[10].matlabPrint(os);
+ os << std::endl;
+ os << "L_sy = ";
+ featcinv.interaction_matrices[11].matlabPrint(os);
+ os << std::endl;
+ // Px,Py
+ os << "L_Px = ";
+ featcinv.interaction_matrices[12].matlabPrint(os);
+ os << std::endl;
+ os << "L_Py = ";
+ featcinv.interaction_matrices[13].matlabPrint(os);
+ os << std::endl;
+
+ return os;
+
+}
+#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp
new file mode 100644
index 0000000..8155ad6
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp
@@ -0,0 +1,310 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ * Manikandan Bakthavatchalam
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+
+
+/*!
+ Default constructor
+ \param moments_ : Database of moment primitives.
+ \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
+ \param featureMoments : Database of features.
+*/
+vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase& moments_,
+ double A_, double B_, double C_,
+ vpFeatureMomentDatabase* featureMoments)
+ : vpFeatureMoment(moments_, A_, B_, C_, featureMoments), order(0)
+{
+}
+
+/*!
+Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
+\param select_one : first index (i)
+\param select_two : second index (j)
+\return Interaction matrix corresponding to the moment
+*/
+vpMatrix vpFeatureMomentCentered::interaction (unsigned int select_one,unsigned int select_two) const {
+ if(select_one+select_two>moment->getObject().getOrder())
+ throw vpException(vpException::badValue,
+ "The requested value has not been computed, you should specify a higher order.");
+ return interaction_matrices[select_two*order+select_one];
+}
+
+/*!
+ * Core function for the interaction matrix computation for moment m_pq
+ * Given its dependent moment and interaction matrices, computes the interaction matrix of centred moments
+ */
+vpMatrix
+vpFeatureMomentCentered::compute_Lmu_pq(const unsigned int& p, const unsigned int& q, const double& xg, const double& yg,
+ const vpMatrix& L_xg, const vpMatrix& L_yg,
+ const vpMomentBasic& m, const vpFeatureMomentBasic& feature_moment_m) const
+{
+ // term1, term2 and Lterm3 (matrix) will be repeatedly computed inside the innermost loop
+ double term1 = 0.0;
+ double term2 = 0.0;
+ vpMatrix Lterm3(1,6);
+
+ double pcombk = 0.0;
+ double qcombl = 0.0;
+ double pcombkqcombl = 0.0;
+
+ double mkl = 0.0;
+ vpMatrix L_mkl;
+
+ int pmk = 0; // p-k
+ int qml = 0; // q-l
+ double minus1pow = 0.; // (-1)^(p+q-k-l)
+ double pintom = 0.;
+
+ for (unsigned int k = 0; k <=p; ++k)
+ {
+ pmk = (int)p-(int)k;
+ pcombk = static_cast<double>(vpMath::comb(p,k));
+ for (unsigned int l = 0; l <= q; ++l)
+ {
+ qml = (int)q - (int)l;
+ qcombl = static_cast<double>(vpMath::comb(q,l));
+ minus1pow = pow((double)-1, (double)(pmk + qml));
+ pcombkqcombl = pcombk * qcombl;
+ mkl = m.get(k, l);
+ pintom = pcombkqcombl * mkl;
+ L_mkl = feature_moment_m.interaction(k, l);
+ if(pmk>0)
+ term1 += pintom * pmk * pow(xg, pmk-1) * pow(yg, qml) * minus1pow;
+ if(qml>0)
+ term2 += pintom * qml * pow(xg, pmk) * pow(yg, qml-1) * minus1pow;
+ Lterm3 += pcombkqcombl * pow(xg, pmk) * pow(yg, qml) * L_mkl * minus1pow;
+ }
+ }
+
+ // L_xg and L_yg stay constant with respect to the above loops. Lterm3 is summed over
+ vpMatrix L_mupq = L_xg*term1 + L_yg*term2 + Lterm3;
+ return L_mupq;
+}
+
+/*!
+ Interface to the interaction matrix computation for centered moments. Called internally.
+ Calls compute_Lmu_pq() for main computation moments (upto order-1)
+Dependencies:
+ Moment classes
+ - vpMomentBasic
+ Interaction matrix classes
+ - vpMomentGravityCenter
+ - vpFeatureMomentBasic
+ - vpFeatureMomentGravityCenter
+*/
+void vpFeatureMomentCentered::compute_interaction() {
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+ const vpMomentObject& momentObject = moment->getObject();
+ order = momentObject.getOrder()+1;
+ interaction_matrices.resize(order*order);
+ for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end(); ++i)
+ i->resize(1,6);
+
+ bool found_moment_gravity;
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ double xg = momentGravity.get()[0];
+ double yg = momentGravity.get()[1];
+
+ bool found_feature_gravity_center;
+ vpFeatureMomentGravityCenter& featureMomentGravityCenter= (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_feature_gravity_center)));
+ if(!found_feature_gravity_center) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
+ vpMatrix Lxg = featureMomentGravityCenter.interaction(1<<0);
+ vpMatrix Lyg = featureMomentGravityCenter.interaction(1<<1);
+
+ bool found_moment_basic;
+ const vpMomentBasic& momentbasic = static_cast<const vpMomentBasic&>(moments.get("vpMomentBasic",found_moment_basic));
+ if(!found_moment_basic) throw vpException(vpException::notInitialized,"vpMomentBasic not found");
+
+ bool found_featuremoment_basic;
+ vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
+ if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
+
+ // Calls the main compute_Lmu_pq function for moments upto order-1
+ for(int i=0;i<(int)order-1;i++){
+ for(int j=0;j<(int)order-1-i;j++){
+ interaction_matrices[(unsigned int)j*order+(unsigned int)i] = compute_Lmu_pq(i, j, xg, yg, Lxg, Lyg, momentbasic, featureMomentBasic);
+ }
+ }
+#else // #ifdef VISP_MOMENTS_COMBINE_MATRICES
+ bool found_moment_centered;
+ bool found_moment_gravity;
+
+ const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ int delta;
+ int epsilon;
+ const vpMomentObject& momentObject = moment->getObject();
+ order = momentObject.getOrder()+1;
+ interaction_matrices.resize(order*order);
+ for (std::vector< vpMatrix >::iterator i=interaction_matrices.begin(); i!=interaction_matrices.end(); ++i)
+ i->resize(1,6);
+ if (momentObject.getType()==vpMomentObject::DISCRETE) {
+ delta=0;
+ epsilon=1;
+ } else {
+ delta=1;
+ epsilon=4;
+ }
+ double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
+ double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
+ double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+ double mu00 = momentCentered.get(0,0);
+
+ unsigned int VX = 0;
+ unsigned int VY = 1;
+ unsigned int VZ = 2;
+ unsigned int WX = 3;
+ unsigned int WY = 4;
+ unsigned int WZ = 5;
+
+ interaction_matrices[0][0][VX] = -(delta)*A*mu00;
+ interaction_matrices[0][0][VY] = -(delta)*B*mu00;
+
+ // Since mu10=0 and mu01=0
+ // interaction_matrices[0][0][WX] = (3*delta)*MU(0,1)+(3*delta)*Yg*mu00;
+ // interaction_matrices[0][0][WY] = -(3*delta)*MU(1,0)-(3*delta)*Xg*mu00;
+ // we get the simplification:
+ interaction_matrices[0][0][WX] = (3*delta)*Yg*mu00;
+ interaction_matrices[0][0][WY] = -(3*delta)*Xg*mu00;
+ interaction_matrices[0][0][VZ] = -A*interaction_matrices[0][0][WY]+B*interaction_matrices[0][0][WX]+(2*delta)*C*mu00;
+ interaction_matrices[0][0][WZ] = 0.;
+
+ for (int i=1; i<(int)order-1; i++){
+ unsigned int i_ = (unsigned int) i;
+ unsigned int im1_ = i_ - 1;
+ unsigned int ip1_ = i_ + 1;
+
+ double mu_im10 = momentCentered.get(im1_,0);
+ double mu_ip10 = momentCentered.get(ip1_,0);
+ double mu_im11 = momentCentered.get(im1_,1);
+ double mu_i0 = momentCentered.get(i_,0);
+ double mu_i1 = momentCentered.get(i_,1);
+
+ interaction_matrices[i_][0][VX] = -(i+delta)*A*mu_i0-(i*B*mu_im11);
+ interaction_matrices[i_][0][VY] = -(delta)*B*mu_i0;
+
+ interaction_matrices[i_][0][WX] = (i+3*delta)*mu_i1+(i+3*delta)*Yg*mu_i0+i*Xg*mu_im11-i*epsilon*n11*mu_im10;
+ interaction_matrices[i_][0][WY] = -(i+3*delta)*mu_ip10-(2*i+3*delta)*Xg*mu_i0+i*epsilon*n20*mu_im10;
+ interaction_matrices[i_][0][VZ] = -A*interaction_matrices[i_][0][WY]+B*interaction_matrices[i_][0][WX]+(i+2*delta)*C*mu_i0;
+ interaction_matrices[i_][0][WZ] = i*mu_im11;
+ }
+
+ for(int j=1;j<(int)order-1;j++){
+ unsigned int j_ = (unsigned int) j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+
+ double mu_0jm1 = momentCentered.get(0,jm1_);
+ double mu_0jp1 = momentCentered.get(0,jp1_);
+ double mu_1jm1 = momentCentered.get(1,jm1_);
+ double mu_0j = momentCentered.get(0,j_);
+ double mu_1j = momentCentered.get(1,j_);
+
+ interaction_matrices[j_*order][0][VX] = -(delta)*A*mu_0j;
+ interaction_matrices[j_*order][0][VY] = -j*A*mu_1jm1-(j+delta)*B*mu_0j;
+
+ interaction_matrices[j_*order][0][WX] = (j+3*delta)*mu_0jp1+(2*j+3*delta)*Yg*mu_0j-j*epsilon*n02*mu_0jm1;
+ interaction_matrices[j_*order][0][WY] = -(j+3*delta)*mu_1j-(j+3*delta)*Xg*mu_0j-j*Yg*mu_1jm1+j*epsilon*n11*mu_0jm1;
+ interaction_matrices[j_*order][0][VZ] = -A*interaction_matrices[j_*order][0][WY]+B*interaction_matrices[j_*order][0][WX]+(j+2*delta)*C*mu_0j;
+ interaction_matrices[j_*order][0][WZ] = -j*mu_1jm1;
+ }
+
+ for(int j=1; j<(int)order-1; j++) {
+ unsigned int j_ = (unsigned int) j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+ for(int i=1; i<(int)order-j-1; i++) {
+ unsigned int i_ = (unsigned int) i;
+ unsigned int im1_ = i_ - 1;
+ unsigned int ip1_ = i_ + 1;
+
+ double mu_ijm1 = momentCentered.get(i_,jm1_);
+ double mu_ij = momentCentered.get(i_,j_);
+ double mu_ijp1 = momentCentered.get(i_,jp1_);
+ double mu_im1j = momentCentered.get(im1_,j_);
+ double mu_im1jp1 = momentCentered.get(im1_,jp1_);
+ double mu_ip1jm1 = momentCentered.get(ip1_,jm1_);
+ double mu_ip1j = momentCentered.get(ip1_,j_);
+
+ interaction_matrices[j_*order+i_][0][VX] = -(i+delta)*A*mu_ij-i*B*mu_im1jp1;
+ interaction_matrices[j_*order+i_][0][VY] = -j*A*mu_ip1jm1-(j+delta)*B*mu_ij;
+
+ interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*mu_ijp1+(i+2*j+3*delta)*Yg*mu_ij
+ +i*Xg*mu_im1jp1-i*epsilon*n11*mu_im1j-j*epsilon*n02*mu_ijm1;
+ interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*mu_ip1j-(2*i+j+3*delta)*Xg*mu_ij
+ -j*Yg*mu_ip1jm1+i*epsilon*n20*mu_im1j+j*epsilon*n11*mu_ijm1;
+ interaction_matrices[j_*order+i_][0][VZ] = -A*interaction_matrices[j_*order+i_][0][WY]+B*interaction_matrices[j_*order+i_][0][WX]+(i+j+2*delta)*C*mu_ij;
+ interaction_matrices[j_*order+i_][0][WZ] = i*mu_im1jp1-j*mu_ip1jm1;
+ }
+ }
+#endif // #ifdef VISP_MOMENTS_COMBINE_MATRICES
+}
+
+
+std::ostream& operator<<(std::ostream & os, const vpFeatureMomentCentered& mu){
+ vpTRACE(" << Ls - CENTRED MOMENTS >>");
+ unsigned int order_m_1 = (unsigned int)(mu.order - 1);
+ for(unsigned int i=0; i<order_m_1; i++){
+ for(unsigned int j=0; j<order_m_1-i; j++){
+ os << "L_mu[" << i << "," << j << "] = ";
+ mu.interaction(i,j).matlabPrint(os);
+ }
+ }
+ return os;
+}
+
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp
new file mode 100644
index 0000000..8eb0c06
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pre-filled pseudo-database used to handle dependencies between common moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/core/vpMomentDatabase.h>
+
+/*!
+ Constructor which initializes and links all common features in the database
+ \param moments : database for moment primitives
+ \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+*/
+vpFeatureMomentCommon::vpFeatureMomentCommon(vpMomentDatabase& moments,double A,double B,double C) :
+ featureGravity(moments,A,B,C),
+ featureGravityNormalized(moments,A,B,C),
+ featureAn(moments,A,B,C),
+ featureCInvariant(moments,A,B,C),
+ featureAlpha(moments,A,B,C),
+ featureCentered(moments,A,B,C),
+ featureMomentBasic(moments,A,B,C),
+ feature_moment_area(moments,A,B,C)
+
+{
+ featureGravity.linkTo(*this);
+ featureGravityNormalized.linkTo(*this);
+ featureAn.linkTo(*this);
+ featureCInvariant.linkTo(*this);
+ featureAlpha.linkTo(*this);
+ featureMomentBasic.linkTo(*this);
+ featureCentered.linkTo(*this);
+ feature_moment_area.linkTo(*this);
+}
+
+/*!
+ Update all moment features in the database with plane coefficients
+ \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+*/
+void vpFeatureMomentCommon::updateAll(double A,double B,double C){
+ featureMomentBasic.update(A,B,C);
+ featureGravity.update(A,B,C);
+ featureCentered.update(A,B,C);
+ featureAn.update(A,B,C);
+ featureGravityNormalized.update(A,B,C);
+ featureCInvariant.update(A,B,C);
+ featureAlpha.update(A,B,C);
+ feature_moment_area.update(A,B,C);
+}
+
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp
new file mode 100644
index 0000000..af16de9
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Pseudo-database used to handle dependencies between moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <typeinfo>
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+/*!
+ Add a moment and it's corresponding name to the database
+ \param featureMoment : database for moment features
+ \param name : the feature's name, usually the string naming it's class. Each name must be unique
+*/
+void vpFeatureMomentDatabase::add(vpFeatureMoment& featureMoment,char* name){
+ featureMomentsDataBase.insert(std::pair<const char*,vpFeatureMoment*>((const char*)name,&featureMoment));
+}
+
+/*!
+ Retrieves a moment feature from the database
+ \param type : the name of the feature, the one specified when using add
+ \param found : true if the type string is found inside the database, false otherwise
+
+ \return the moment feature corresponding to the type string
+*/
+vpFeatureMoment& vpFeatureMomentDatabase::get(const char* type, bool& found){
+ std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator it = featureMomentsDataBase.find(type);
+
+ found = (it!=featureMomentsDataBase.end());
+ return *(it->second);
+}
+
+/*!
+ Update all moment features in the database with plane coefficients
+ \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+*/
+void vpFeatureMomentDatabase::updateAll(double A, double B, double C)
+{
+ std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
+#ifdef VISP_HAVE_OPENMP
+ std::vector<vpFeatureMoment*> values;
+ values.reserve(featureMomentsDataBase.size());
+ for(itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); itr++){
+ values.push_back((*itr).second);
+ }
+ unsigned int i_;
+ #pragma omp parallel for shared(A,B,C)
+ for(int i=0;i<(int)values.size();i++){
+ i_ = static_cast<unsigned int>(i);
+ values[i_]->update(A,B,C);
+ }
+#else
+ for(itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); itr++){
+ (*itr).second->update(A,B,C);
+ }
+#endif
+}
+
+/*
+std::ostream & operator<<(std::ostream & os, const vpFeatureMomentDatabase& m){
+ std::map<const char*,vpMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
+ os << "{";
+
+ for(itr = m.featureMoments.begin(); itr != m.featureMoments.end(); itr++){
+ os << (*itr).first << ": [" << *((*itr).second) << "],";
+ }
+ os << "}";
+
+ return os;
+}*/
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp
new file mode 100644
index 0000000..fef4f28
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp
@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+
+/*!
+ Computes interaction matrix for gravity center moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpFeatureMomentBasic
+
+ Minimum vpMomentObject order needed to compute this feature: 2.
+*/
+void vpFeatureMomentGravityCenter::compute_interaction(){
+ bool found_featuremoment_basic;
+
+ vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
+ const vpMomentObject& momentObject = moment->getObject();
+
+ if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
+
+
+ interaction_matrices[0].resize(1,6);
+ interaction_matrices[1].resize(1,6);
+
+ interaction_matrices[0] = featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) - momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
+ interaction_matrices[1] = featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) - momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
+
+
+}
+
+#else
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
+
+
+/*!
+ Computes interaction matrix for gravity center moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentGravityCenter
+
+ Minimum vpMomentObject order needed to compute this feature: 2.
+*/
+void vpFeatureMomentGravityCenter::compute_interaction(){
+ bool found_moment_centered;
+ bool found_moment_gravity;
+
+ const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+
+
+ const vpMomentObject& momentObject = moment->getObject();
+
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+
+ interaction_matrices[0].resize(1,6);
+ interaction_matrices[1].resize(1,6);
+ int epsilon;
+ if (momentObject.getType()==vpMomentObject::DISCRETE) {
+ epsilon=1;
+ } else {
+ epsilon=4;
+ }
+ double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
+ double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
+ double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+ double Zg_inv = A*Xg+B*Yg+C;
+ double Xgvz = Xg*Zg_inv+A*epsilon*n20+B*epsilon*n11;
+ double Ygvz = Yg*Zg_inv+A*epsilon*n11+B*epsilon*n02;
+ double Xgwx = Xg*Yg+epsilon*n11;
+ double Ygwy = -Xgwx;
+ double Xgwy = -(1+Xg*Xg+epsilon*n20);
+ double Ygwx = 1+Yg*Yg+epsilon*n02;
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = -Zg_inv;
+ interaction_matrices[0][0][VY] = 0;
+ interaction_matrices[0][0][VZ] = Xgvz;
+
+ interaction_matrices[0][0][WX] = Xgwx;
+ interaction_matrices[0][0][WY] = Xgwy;
+ interaction_matrices[0][0][WZ] = Yg;
+
+ interaction_matrices[1][0][VX] = 0;
+ interaction_matrices[1][0][VY] = -Zg_inv;
+ interaction_matrices[1][0][VZ] = Ygvz;
+
+ interaction_matrices[1][0][WX] = Ygwx;
+ interaction_matrices[1][0][WY] = Ygwy;
+ interaction_matrices[1][0][WZ] = -Xg;
+
+}
+
+#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
new file mode 100644
index 0000000..a974cf2
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Implementation for all supported moment features.
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_MOMENTS_COMBINE_MATRICES
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+/*!
+ Computes interaction matrix for centered and normalized moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpFeatureMomentGravityCenter
+ - vpMomentGravityCenter
+ - vpMomentAreaNormalized
+ - vpFeatureMomentAreaNormalized
+*/
+void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
+
+ bool found_featuremoment_gravity;
+ bool found_featuremoment_surfacenormalized;
+
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ vpFeatureMomentGravityCenter& featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_featuremoment_gravity)));
+ vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized&>(featureMomentsDataBase->get("vpFeatureMomentAreaNormalized",found_featuremoment_surfacenormalized)));
+
+ if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+
+ if(!found_featuremoment_gravity) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
+ if(!found_featuremoment_surfacenormalized) throw vpException(vpException::notInitialized,"vpFeatureMomentAreaNormalized not found");
+
+
+ interaction_matrices[0].resize(1,6);
+ interaction_matrices[1].resize(1,6);
+
+ interaction_matrices[0] = momentGravity.get()[0]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(1);
+ interaction_matrices[1] = momentGravity.get()[1]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(2);
+}
+
+#else
+
+#include <vector>
+#include <limits>
+
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+
+
+/*!
+ Computes interaction matrix for centered and normalized moment. Called internally.
+ The moment primitives must be computed before calling this.
+ This feature depends on:
+ - vpMomentCentered
+ - vpMomentAreaNormalized
+ - vpMomentGravityCenter
+*/
+void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
+
+ bool found_moment_surface_normalized;
+ bool found_moment_gravity;
+ bool found_moment_centered;
+
+ const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
+ const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
+
+ if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
+ if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+
+ const vpMomentObject& momentObject = moment->getObject();
+ interaction_matrices[0].resize(1,6);
+ interaction_matrices[1].resize(1,6);
+
+
+ double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
+ double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
+ double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
+ double n10 = momentCentered.get(1,0)/momentObject.get(0,0);
+ double n01 = momentCentered.get(0,1)/momentObject.get(0,0);
+ double n03 = momentCentered.get(0,3)/momentObject.get(0,0);
+
+
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double An=momentSurfaceNormalized.get()[0];
+
+ double Xn = An*Xg;
+ double Yn = An*Yg;
+
+
+ double Xnvx,Xnvy,Xnvz,Xnwx,Xnwy;
+ double Ynvx,Ynvy,Ynvz,Ynwx,Ynwy;
+
+ if(momentObject.getType()==vpMomentObject::DISCRETE){
+ double a = momentCentered.get(2,0)+momentCentered.get(0,2);
+
+ double e11 = momentCentered.get(1,1)/a;
+ double e12 = momentCentered.get(1,2)/a;
+ double e21 = momentCentered.get(2,1)/a;
+ double e30 = momentCentered.get(3,0)/a;
+
+ double NA = n20+n02;
+
+
+ Xnvx = B*Xn*e11-Yn*B-An*C-n02*A*Xn/NA;
+ Xnvy = A*Xn*e11+n02*B*Xn/NA;
+
+ Xnwx = An*e11*NA+Yn*n10-Xn*Xg*e11+Xn*n01+Xn*n10*e11-Xn*e21+(-Xn*n03+(Xn*n01-Yn*Xg)*n02)/NA;
+ Xnwy = -An*NA+Xn*e12+Xn*Xg-An+e11*Xg*Yn-Xn*n01*e11-2*Xn*n10+Xn*e30+n02*An+(-Xn*Xg+Xn*n10)*n02/NA;
+
+
+
+ Ynvx = (Yn-n02*Yn/NA)*A+Yn*e11*B;
+ Ynvy = (-Xn+e11*Yn)*A+(-Yn+n02*Yn/NA)*B-An*C;
+
+ Ynwx = n02*An+Yn*n10*e11-e11*Xg*Yn+An-Yn*e21+Yn*n01+(-Yn*n03+(Yn*n01-Yn*Yg)*n02)/NA;
+ Ynwy = -An*e11*NA+Yn*e11*Yg-Yn*n01*e11+Yn*Xg+Yn*e12+Yn*e30-Xn*n01-2*Yn*n10+(Yn*n10-Yn*Xg)*n02/NA;
+
+
+ }else{
+ Xnvx = -An*C-A*Xn-Yn*B;
+ Xnvy = (0.5)*B*Xn;
+
+ Xnwx = (0.5*(8.*n10-Xg))*Yn+4.*An*n11+4.*n01*Xn;
+ Xnwy = (0.5*(-2.-8.*n20))*An+(0.5)*Xn*(-8.*n10+Xg);
+
+ Ynvx = (0.5)*A*Yn;
+ Ynvy = -(0.5)*B*Yn-C*An-A*Xn;
+
+ Ynwx = (0.5)*Yn*(8.*n01-Yg)+(.5*(2.+8.*n02))*An;
+ Ynwy = (0.5*(-8.*n10+Xg))*Yn-4.*An*n11-4.*n01*Xn;
+
+ }
+
+ Ynvz = -A*Ynwy+(-An+Ynwx)*B;
+ Xnvz = -A*An-A*Xnwy+B*Xnwx;
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Xnvx;
+ interaction_matrices[0][0][VY] = Xnvy;
+ interaction_matrices[0][0][VZ] = Xnvz;
+
+ interaction_matrices[0][0][WX] = Xnwx;
+ interaction_matrices[0][0][WY] = Xnwy;
+ interaction_matrices[0][0][WZ] = Yn;
+
+ interaction_matrices[1][0][VX] = Ynvx;
+ interaction_matrices[1][0][VY] = Ynvy;
+ interaction_matrices[1][0][VZ] = Ynvz;
+
+ interaction_matrices[1][0][WX] = Ynwx;
+ interaction_matrices[1][0][WY] = Ynwy;
+ interaction_matrices[1][0][WZ] = -Xn;
+
+}
+#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp
new file mode 100644
index 0000000..e229e45
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp
@@ -0,0 +1,572 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeaturePoint.cpp
+ \brief Class that defines 2D point visual feature
+*/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+
+/*
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities ar useful but not mandatory
+
+*/
+
+/*!
+ Initialize the memory space requested for 2D point visual feature.
+*/
+void
+vpFeaturePoint::init()
+{
+ //feature dimension
+ dim_s = 2 ;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //default value Z (1 meters)
+ Z = 1;
+}
+
+/*!
+ Default constructor that build a visual feature.
+*/
+vpFeaturePoint::vpFeaturePoint() : Z(1.)
+{
+ init() ;
+}
+
+
+/*!
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \param Z_ : \f$ Z \f$ value to set.
+*/
+void
+vpFeaturePoint::set_Z(const double Z_)
+{
+ this->Z = Z_ ;
+ flags[2] = true;
+}
+
+
+/*!
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+
+ \return The value of \f$ Z \f$.
+*/
+double
+vpFeaturePoint::get_Z() const
+{
+ return Z ;
+}
+
+
+/*!
+
+ Set the value of \f$ x \f$ which represents the x coordinate of the point
+ in image plan. It is one parameter of the visual feature \f$ s \f$.
+
+ \param x : \f$ x \f$ value to set.
+*/
+void
+vpFeaturePoint::set_x(const double x)
+{
+ s[0] = x ;
+ flags[0] = true;
+}
+
+
+/*!
+ Get the value of \f$ x \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+
+ \return The value of \f$ x \f$.
+*/
+double
+vpFeaturePoint::get_x() const
+{
+ return s[0] ;
+}
+
+
+/*!
+ Set the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+
+ \param y : \f$ y \f$ value to set.
+*/
+void
+vpFeaturePoint::set_y(const double y)
+{
+ s[1] = y ;
+ flags[1] = true;
+}
+
+
+/*!
+ Get the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+
+ \return The value of \f$ y \f$.
+*/
+double
+vpFeaturePoint::get_y() const
+{
+ return s[1] ;
+}
+
+
+/*!
+
+ Set the value of \f$ x \f$, \f$ y \f$ and \f$ Z \f$. \f$ x \f$ and \f$ y \f$
+ represent the coordinates of the point in the image plan and are the
+ parameters of the visual feature \f$ s \f$. \f$ Z \f$ is the 3D coordinate in the camera frame
+ representing the depth.
+
+ \param x_ : \f$ x \f$ value to set.
+ \param y_ : \f$ y \f$ value to set.
+ \param Z_ : \f$ Z \f$ value to set.
+*/
+void
+vpFeaturePoint::set_xyZ(const double x_,
+ const double y_,
+ const double Z_)
+{
+ set_x(x_) ;
+ set_y(y_) ;
+ set_Z(Z_) ;
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L \f$. The computation is made
+ thanks to the values of the point features \f$ x \f$ and \f$ y \f$ and the
+ depth \f$ Z \f$.
+
+ \f[ L = \left[\begin{array}{c}L_{x} \\ L_{y}\end{array}\right] =
+ \left[\begin{array}{cccccc}
+ -1/Z & 0 & x/Z & xy & -(1+x^2) & y \\
+ 0 & -1/Z & y/Z & 1+y^2 & -xy & -x
+ \end{array}\right]\f]
+
+ \param select : Selection of a subset of the possible point features.
+ - To compute the interaction matrix for all the two point features use
+ vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
+ matrix is \f$ [2 \times 6] \f$
+ - To compute the interaction matrix for only one of the point component
+ feature (\f$ x, y \f$) use one of the corresponding function selectX() or
+ selectY(). In that case the returned interaction matrix is \f$ [1 \times 6]
+ \f$ dimension.
+
+ \return The interaction matrix computed from the point features.
+
+ The code below shows how to compute the interaction matrix associated to the
+ visual feature \f$ s = x \f$.
+ \code
+ // Creation of the current feature s
+ vpFeaturePoint s;
+ s.buildFrom(0, 0, 1);
+
+ vpMatrix L_x = s.interaction( vpFeaturePoint::selectX() );
+ \endcode
+
+ The code below shows how to compute the interaction matrix associated to the
+ visual feature \f$ s = (x, y) \f$.
+ \code
+ // Creation of the current feature s
+ vpFeaturePoint s;
+ s.buildFrom(0, 0, 1);
+
+ vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+*/
+vpMatrix
+vpFeaturePoint::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ double x_ = get_x() ;
+ double y_ = get_y() ;
+ double Z_ = get_Z() ;
+
+ if (Z_ < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ if (vpFeaturePoint::selectX() & select )
+ {
+ vpMatrix Lx(1,6) ; Lx = 0;
+
+ Lx[0][0] = -1/Z_ ;
+ Lx[0][1] = 0 ;
+ Lx[0][2] = x_/Z_ ;
+ Lx[0][3] = x_*y_ ;
+ Lx[0][4] = -(1+x_*x_) ;
+ Lx[0][5] = y_ ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeaturePoint::selectY() & select )
+ {
+ vpMatrix Ly(1,6) ; Ly = 0;
+
+ Ly[0][0] = 0 ;
+ Ly[0][1] = -1/Z_ ;
+ Ly[0][2] = y_/Z ;
+ Ly[0][3] = 1+y_*y_ ;
+ Ly[0][4] = -x_*y_ ;
+ Ly[0][5] = -x_ ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+ return L ;
+}
+
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ \param s_star : Desired visual feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible point features.
+ - To compute the error for all the two point features use vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2 dimension column vector.
+ - To compute the error for only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY(). In that case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired visual feature.
+
+ The code below shows how to use this method to manipulate the \f$ x \f$ subset:
+ \code
+ // Creation of the current feature s
+ vpFeaturePoint s;
+ s.buildFrom(0, 0, 1);
+
+ // Creation of the desired feature s*
+ vpFeaturePoint s_star;
+ s_star.buildFrom(1, 1, 1);
+
+ // Compute the interaction matrix for the x feature
+ vpMatrix L_x = s.interaction( vpFeaturePoint::selectX() );
+
+ // Compute the error vector (s-s*) for the x feature
+ s.error(s_star, vpFeaturePoint::selectX());
+ \endcode
+*/
+vpColVector
+vpFeaturePoint::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeaturePoint::selectX() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeaturePoint::selectY() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1] - s_star[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+ }
+ catch(...) {
+ throw ;
+ }
+
+
+ return e ;
+
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible point features.
+ - To print all the two point features use vpBasicFeature::FEATURE_ALL.
+ - To print only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY().
+
+ \code
+ vpFeaturePoint s; // Current visual feature s
+
+ // Creation of the current feature s
+ s.buildFrom(0, 0, 1);
+
+ s.print(); // print all the 2 components of the feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeaturePoint::selectX()); // print only the x component
+ \endcode
+*/
+void
+vpFeaturePoint::print(const unsigned int select ) const
+{
+
+ std::cout <<"Point: Z=" << get_Z() ;
+ if (vpFeaturePoint::selectX() & select )
+ std::cout << " x=" << get_x() ;
+ if (vpFeaturePoint::selectY() & select )
+ std::cout << " y=" << get_y() ;
+ std::cout <<std::endl ;
+}
+
+
+/*!
+ Build a 2D point visual feature from the point coordinates in the image plan \f$ x \f$ and \f$ y \f$. The parameter Z which describes the depth, is set in the same time.
+
+ See the vpFeaturePoint class description for more details about \f$ x \f$ and \f$ y \f$.
+
+ \param x_ : The \f$ x \f$ parameter.
+ \param y_ : The \f$ y \f$ parameter.
+ \param Z_ : The \f$ Z \f$ parameter.
+*/
+void
+vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_)
+{
+
+ s[0] = x_ ;
+ s[1] = y_ ;
+
+ this->Z = Z_ ;
+
+ if (Z_ < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+
+}
+
+
+/*!
+
+ Display point feature.
+
+ \param cam : Camera parameters.
+ \param I : Image.
+ \param color : Color to use for the display.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeaturePoint::display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double x,y ;
+ x = get_x() ;
+ y = get_y() ;
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display point feature.
+
+ \param cam : Camera parameters.
+ \param I : color Image.
+ \param color : Color to use for the display.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeaturePoint::display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double x,y ;
+ x = get_x() ;
+ y = get_y() ;
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeaturePoint s;
+ s_star = s.duplicate(); // s_star is now a vpFeaturePoint
+ \endcode
+
+*/
+vpFeaturePoint *vpFeaturePoint::duplicate() const
+{
+ vpFeaturePoint *feature = new vpFeaturePoint ;
+ return feature ;
+}
+
+/*!
+
+ Function used to select the \f$ x \f$ subset of the point visual feature.
+
+ This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ x \f$.
+
+ This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePoint s;
+ vpServo task;
+ ...
+ // Add the (x) subset features from the 2D point
+ task.addFeature(s, vpFeaturePoint::selectX());
+ \endcode
+*/
+unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0] ; }
+
+/*!
+
+ Function used to select the \f$ y \f$ subset of the point visual feature.
+
+ This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ y \f$.
+
+ This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePoint s;
+ vpServo task;
+ ...
+ // Add the (y) subset features from the 2D point
+ task.addFeature(s, vpFeaturePoint::selectY());
+ \endcode
+*/
+unsigned int vpFeaturePoint::selectY() { return FEATURE_LINE[1] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp
new file mode 100644
index 0000000..35ae5ae
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp
@@ -0,0 +1,688 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 3D point visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+
+
+
+
+/*
+
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities are useful but not mandatory
+
+*/
+
+/*!
+
+ Initialise the memory space requested for a 3D point visual
+ feature.
+
+ By default this feature is initialized to \f${\bf X} = (0, 0, 1)\f$.
+*/
+void
+vpFeaturePoint3D::init()
+{
+ //feature dimension
+ dim_s = 3 ;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //default value XYZ
+ s[0] = 0;
+ s[1] = 0;
+ s[2] = 1;
+
+}
+
+/*!
+
+ Default constructor that build a 3D point visual feature and
+ initialize it to \f${\bf X} = (0, 0, 1)\f$.
+
+*/
+vpFeaturePoint3D::vpFeaturePoint3D()
+{
+ init() ;
+}
+
+
+/*!
+
+ Initialise the \f$X\f$ coordinate in the camera frame of the 3D Point
+ visual feature \f${\bf X} = (X,Y,Z)\f$.
+
+ \param X : \f$X\f$ coordinate of the visual feature.
+ \sa get_X()
+
+*/
+void
+vpFeaturePoint3D::set_X(const double X)
+{
+ s[0] = X ;
+ flags[0] = true;
+}
+
+/*!
+
+ Initialise the \f$Y\f$ coordinate in the camera frame of the 3D Point
+ visual feature \f${\bf X} = (X,Y,Z)\f$.
+
+ \param Y : \f$Y\f$ coordinate of the visual feature.
+ \sa get_Y()
+
+*/
+void
+vpFeaturePoint3D::set_Y(const double Y)
+{
+ s[1] = Y ;
+ flags[1] = true;
+}
+
+/*!
+
+ Initialise the \f$Z\f$ coordinate in the camera frame of the 3D Point
+ visual feature \f${\bf X} = (X,Y,Z)\f$.
+
+ \param Z : \f$Z\f$ coordinate or depth of the visual feature.
+ \sa get_Z()
+
+*/
+void
+vpFeaturePoint3D::set_Z(const double Z)
+{
+ s[2] = Z ;
+ flags[2] = true;
+}
+
+/*!
+ Initialize the 3D point coordinates.
+
+ \param X,Y,Z : \f$(X,Y,Z)\f$ coordinates in the camera frame of the
+ 3D point visual feature.
+
+ \sa set_X(), set_Y(), set_Z()
+*/
+void
+vpFeaturePoint3D::set_XYZ(const double X,
+ const double Y,
+ const double Z)
+{
+ set_X(X) ;
+ set_Y(Y) ;
+ set_Z(Z) ;
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+//! Return the \f$X\f$ coordinate in the camera frame of the 3D point.
+double
+vpFeaturePoint3D::get_X() const
+{
+ return s[0] ;
+}
+
+//! Return the \f$Y\f$ coordinate in the camera frame of the 3D point.
+double
+vpFeaturePoint3D::get_Y() const
+{
+ return s[1] ;
+}
+
+//! Return the \f$Z\f$ coordinate in the camera frame of the 3D point.
+double
+vpFeaturePoint3D::get_Z() const
+{
+ return s[2] ;
+}
+
+/*!
+ Compute and return the interaction matrix \f$ L \f$ associated to a subset
+ of the possible 3D point features \f$(X,Y,Z)\f$ that
+ represent the 3D point coordinates expressed in the camera frame.
+
+ \f[
+ L = \left[
+ \begin{array}{rrrrrr}
+ -1 & 0 & 0 & 0 & -Z & Y \\
+ 0 & -1 & 0 & Z & 0 & -X \\
+ 0 & 0 & -1 & -Y & X & 0 \\
+ \end{array}
+ \right]
+ \f]
+
+
+ \param select : Selection of a subset of the possible 3D point coordinate
+ features.
+ - To compute the interaction matrix for all the three
+ subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
+ that case the dimension of the interaction matrix is \f$ [3 \times
+ 6] \f$
+ - To compute the interaction matrix for only one of the
+ subset (\f$X, Y,Z\f$) use
+ one of the corresponding function selectX(), selectY() or
+ selectZ(). In that case the returned interaction matrix is \f$ [1
+ \times 6] \f$ dimension.
+
+ \return The interaction matrix computed from the 3D point coordinate
+ features.
+
+ The code below shows how to compute the interaction matrix
+ associated to the visual feature \f$s = X \f$.
+
+ \code
+ vpPoint point;
+ ...
+ // Creation of the current feature s
+ vpFeaturePoint3D s;
+ s.buildFrom(point);
+
+ vpMatrix L_X = s.interaction( vpFeaturePoint3D::selectX() );
+ \endcode
+
+ The code below shows how to compute the interaction matrix
+ associated to the \f$s = (X,Y) \f$
+ subset visual feature:
+
+ \code
+ vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY() );
+ \endcode
+
+ L_XY is here now a 2 by 6 matrix. The first line corresponds to
+ the \f$ X \f$ visual feature while the second one to the \f$
+ Y \f$ visual feature.
+
+ It is also possible to build the interaction matrix from all the
+ 3D point coordinates by:
+
+ \code
+ vpMatrix L_XYZ = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+
+ In that case, L_XYZ is a 3 by 6 interaction matrix where the last
+ line corresponds to the \f$ Z \f$ visual feature.
+
+*/
+vpMatrix
+vpFeaturePoint3D::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ double X = get_X() ;
+ double Y = get_Y() ;
+ double Z = get_Z() ;
+
+ if (vpFeaturePoint3D::selectX() & select )
+ {
+ vpMatrix Lx(1,6) ; Lx = 0;
+
+ Lx[0][0] = -1 ;
+ Lx[0][1] = 0 ;
+ Lx[0][2] = 0 ;
+ Lx[0][3] = 0 ;
+ Lx[0][4] = -Z ;
+ Lx[0][5] = Y ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeaturePoint3D::selectY() & select )
+ {
+ vpMatrix Ly(1,6) ; Ly = 0;
+
+ Ly[0][0] = 0 ;
+ Ly[0][1] = -1 ;
+ Ly[0][2] = 0 ;
+ Ly[0][3] = Z ;
+ Ly[0][4] = 0 ;
+ Ly[0][5] = -X ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+ if (vpFeaturePoint3D::selectZ() & select )
+ {
+ vpMatrix Lz(1,6) ; Lz = 0;
+
+ Lz[0][0] = 0 ;
+ Lz[0][1] = 0 ;
+ Lz[0][2] = -1 ;
+ Lz[0][3] = -Y ;
+ Lz[0][4] = X ;
+ Lz[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Lz) ;
+ }
+ return L ;
+}
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ \param s_star : Desired 3D point visual feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible 3D point coordinate features.
+ - To compute the error for all the three coordinates use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ dimension column vector.
+ - To compute the error for only one of the coordinate
+ feature \f$(X,Y, or Z)\f$ use one of the
+ corresponding function selectX(), selectY() or selectZ(). In
+ that case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ The code below shows how to use this method to manipulate the \f$
+ Z \f$ subset:
+
+ \code
+ // Creation of the current feature s
+ vpFeaturePoint3D s;
+ s.set_Z(0.8); // Initialization of the current Z feature
+
+ // Creation of the desired feature s*.
+ vpFeatureTranslation s_star;
+ s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter
+
+ // Compute the interaction matrix for the Z coordinate feature
+ vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );
+
+ // Compute the error vector (s-s*) for the Z feature
+ s.error(s_star, vpFeaturePoint3D::selectZ());
+ \endcode
+
+ To manipulate the subset features \f$s=(Y, Z)\f$,
+ the code becomes:
+ \code
+ // Compute the interaction matrix for the Y, Z feature coordinates
+ vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );
+
+ // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
+ vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
+ \endcode
+
+*/
+vpColVector
+vpFeaturePoint3D::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeaturePoint3D::selectX() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeaturePoint3D::selectY() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1] - s_star[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ if (vpFeaturePoint3D::selectZ() & select )
+ {
+ vpColVector ez(1) ;
+ ez[0] = s[2] - s_star[2] ;
+ e = vpColVector::stack(e,ez) ;
+ }
+ }
+ catch(...) {
+ throw ;
+ }
+
+ return e ;
+
+}
+
+/*!
+
+ Build a 3D point visual feature from the camera frame coordinates
+ \f$(X,Y,Z)\f$ of a point.
+
+ \param p : A point with camera frame coordinates \f${^c}P=(X,Y,Z)\f$
+ up to date (see vpPoint class).
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is negative. That means that the 3D point is
+ behind the camera which is not possible.
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is null. That means that the 3D point is
+ on the camera which is not possible.
+*/
+void
+vpFeaturePoint3D::buildFrom(const vpPoint &p)
+{
+
+ // cP is expressed in homogeneous coordinates
+ // we devide by the fourth coordinate
+ s[0] = p.cP[0]/p.cP[3] ;
+ s[1] = p.cP[1]/p.cP[3] ;
+ s[2] = p.cP[2]/p.cP[3] ;
+
+ double Z = s[2] ;
+ if (Z < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+
+}
+
+/*!
+
+ Build a 3D point visual feature from the camera frame coordinates
+ \f$(X,Y,Z)\f$ of a point.
+
+ \param X,Y,Z : Camera frame coordinates \f$(X,Y,Z)\f$ of a 3D point.
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is negative. That means that the 3D point is
+ on the camera which is not possible.
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is null. That means that the 3D point is
+ on the camera which is not possible.
+
+*/
+void
+vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
+{
+
+ s[0] = X ;
+ s[1] = Y ;
+ s[2] = Z ;
+
+ if (Z < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible 3D point
+ feature coordinates.
+ - To print all the three coordinates used as features use
+ vpBasicFeature::FEATURE_ALL.
+ - To print only one of the coordinate
+ feature \f$(X,Y,Z)\f$ use one of the
+ corresponding function selectX(), selectX() or selectZ().
+
+ \code
+ vpPoint point;
+
+ // Creation of the current feature s
+ vpFeaturePoint3D s;
+ s.buildFrom(point);
+
+ s.print(); // print all the 3 components of the translation feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeaturePoint3D::selectZ()); // print only the Z component
+ \endcode
+*/
+void
+vpFeaturePoint3D::print(const unsigned int select ) const
+{
+
+ std::cout <<"Point3D: " ;
+ if (vpFeaturePoint3D::selectX() & select )
+ std::cout << " X=" << get_X() ;
+ if (vpFeaturePoint3D::selectY() & select )
+ std::cout << " Y=" << get_Y() ;
+ if (vpFeaturePoint3D::selectZ() & select )
+ std::cout << " Z=" << get_Z() ;
+ std::cout <<std::endl ;
+}
+
+
+/*!
+
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeaturePoint3D s;
+ s_star = s.duplicate(); // s_star is now a vpFeaturePoint3D
+ \endcode
+
+*/
+vpFeaturePoint3D *vpFeaturePoint3D::duplicate() const
+{
+ vpFeaturePoint3D *feature = new vpFeaturePoint3D ;
+ return feature ;
+}
+
+/*!
+
+ Not implemented.
+*/
+void
+vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
+ const vpImage<unsigned char> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*!
+
+ Not implemented.
+ */
+void
+vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
+ const vpImage<vpRGBa> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+/*!
+
+ Function used to select the \f$ X\f$ subset coordinate of the 3D point
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ X\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePoint3D p;
+ vpServo task;
+ ...
+ // Add the (X,Y) subset coordinates features from a 3D point to the task
+ task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
+ \endcode
+
+ \sa selectY(), selectZ()
+
+*/
+unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0] ; }
+
+/*!
+
+ Function used to select the \f$ Y\f$ subset coordinate of the 3D point
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ Y\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePoint3D p;
+ vpServo task;
+ ...
+ // Add the (X,Y) subset coordinates features from a 3D point to the task
+ task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
+ \endcode
+
+ \sa selectX(), selectZ()
+
+*/
+unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1] ; }
+
+/*!
+
+ Function used to select the \f$ Z\f$ subset coordinate of the 3D point
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ Z\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePoint3D p;
+ vpServo task;
+ ...
+ // Add the (Z) subset coordinate feature from a 3D point to the task
+ task.addFeature(p, vpFeaturePoint3D::selectZ());
+ \endcode
+
+ \sa selectX(), selectY()
+
+*/
+unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp
new file mode 100644
index 0000000..74281e5
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp
@@ -0,0 +1,676 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D point visual feature.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpFeaturePointPolar.cpp
+ \brief Class that defines a 2D point visual feature with polar coordinates..
+*/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+
+
+/*
+
+ attributes and members directly related to the vpBasicFeature needs
+ other functionalities ar useful but not mandatory
+
+*/
+
+/*!
+
+ Initialise the memory space requested for a 2D point visual
+ feature with polar coordinates.
+
+ By default this feature is initialized to \f$(\rho, \theta) = (0,
+ 0)\f$. The 3D depth of the point requested in the interaction
+ matrix (see interaction()) is initialized to \f$Z=1\f$.
+*/
+void
+vpFeaturePointPolar::init()
+{
+ //feature dimension
+ dim_s = 2 ;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //default value Z (1 meters)
+ Z = 1;
+
+}
+
+/*!
+
+ Default constructor that build a 2D point visual feature with
+ polar coordinates and initialize it to \f$(\rho, \theta) = (0,
+ 0)\f$.
+
+ The 3D depth of the point requested in the interaction
+ matrix (see interaction()) is initialized to \f$Z=1\f$.
+
+*/
+vpFeaturePointPolar::vpFeaturePointPolar() : Z(1.)
+{
+ init() ;
+}
+
+
+/*!
+ Set the image point \f$\rho\f$ polar coordinate.
+
+ \sa set_theta()
+*/
+void
+vpFeaturePointPolar::set_rho(const double rho)
+{
+ s[0] = rho ;
+ flags[0] = true;
+}
+/*!
+ Set the image point \f$\theta\f$ polar coordinate.
+
+ \sa set_rho()
+*/
+void
+vpFeaturePointPolar::set_theta(const double theta)
+{
+ s[1] = theta ;
+ flags[1] = true;
+}
+
+/*!
+ Set the 3D point depth in the camera frame.
+
+*/
+void
+vpFeaturePointPolar::set_Z(const double Z_)
+{
+ this->Z = Z_ ;
+ flags[2] = true;
+}
+
+/*!
+ Initialize the image point visual feature with polar coordinates.
+
+ \param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
+ the image point.
+
+ \param Z_ : 3D depth of the point in the camera frame.
+
+ \sa set_rho(), set_theta(), set_Z()
+*/
+void
+vpFeaturePointPolar::set_rhoThetaZ(const double rho,
+ const double theta,
+ const double Z_)
+{
+ set_rho(rho) ;
+ set_theta(theta) ;
+ set_Z(Z_) ;
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+/*!
+ Get the image point \f$\rho\f$ polar coordinate.
+
+ \sa get_theta()
+*/
+double
+vpFeaturePointPolar::get_rho() const
+{
+ return s[0] ;
+}
+
+/*!
+ Get the image point \f$\theta\f$ polar coordinate.
+
+ \sa get_rho()
+*/
+double
+vpFeaturePointPolar::get_theta() const
+{
+ return s[1] ;
+}
+/*!
+ Get the 3D point depth in the camera frame.
+
+*/
+double
+vpFeaturePointPolar::get_Z() const
+{
+ return this->Z ;
+}
+
+
+
+/*!
+ Compute and return the interaction matrix \f$ L \f$ associated to a
+ subset of the possible 2D image point features with polar
+ coordinates \f$(\rho,\theta)\f$.
+
+ \f[
+ L = \left[
+ \begin{array}{l}
+ L_{\rho} \\
+ \; \\
+ L_{\theta}\\
+ \end{array}
+ \right]
+ =
+ \left[
+ \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]
+
+ where \f$Z\f$ is the 3D depth of the considered point.
+
+ \param select : Selection of a subset of the possible polar
+ point coordinate features.
+ - To compute the interaction matrix for all the two
+ subset features \f$(\rho,\theta)\f$ use vpBasicFeature::FEATURE_ALL. In
+ that case the dimension of the interaction matrix is \f$ [2 \times
+ 6] \f$
+ - To compute the interaction matrix for only one of the subset
+ (\f$\rho,\theta\f$) use one of the corresponding function
+ selectRho() or selectTheta(). In that case the returned
+ interaction matrix is \f$ [1 \times 6] \f$ dimension.
+
+ \return The interaction matrix computed from the 2D point
+ polar coordinate features.
+
+ \exception vpFeatureException::badInitializationError : If the point
+ is behind the camera \f$(Z < 0)\f$, or if the 3D depth is null \f$(Z
+ = 0)\f$, or if the \f$\rho\f$ polar coordinate of the point is null.
+
+ The code below shows how to compute the interaction matrix associated to
+ the visual feature \f$s = (\rho,\theta)\f$.
+ \code
+ vpFeaturePointPolar s;
+ double rho = 0.3;
+ double theta = M_PI;
+ double Z = 1;
+ // Creation of the current feature s
+ s.buildFrom(rho, theta, Z);
+ // Build the interaction matrix L_s
+ vpMatrix L = s.interaction();
+ \endcode
+
+ The interaction matrix could also be build by:
+ \code
+ vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+
+ In both cases, L is a 2 by 6 matrix. The first line corresponds to
+ the \f$\rho\f$ visual feature while the second one to the
+ \f$\theta\f$ visual feature.
+
+ It is also possible to build the interaction matrix associated to
+ one of the possible features. The code below shows how to consider
+ only the \f$\theta\f$ component.
+
+ \code
+ vpMatrix L_theta = s.interaction( vpFeaturePointPolar::selectTheta() );
+ \endcode
+
+ In that case, L_theta is a 1 by 6 matrix.
+*/
+vpMatrix
+vpFeaturePointPolar::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ double rho = get_rho() ;
+ double theta = get_theta() ;
+ double Z_ = get_Z() ;
+
+ double c_ = cos(theta);
+ double s_ = sin(theta);
+
+ double rho2 = rho*rho;
+
+ if (fabs(rho) < 1e-6) {
+ vpERROR_TRACE("rho polar coordinate of the point is null") ;
+ std::cout <<"rho = " << rho << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "rho polar coordinate of the point is null")) ;
+ }
+
+ if (Z_ < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z_) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z_ << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ if (vpFeaturePointPolar::selectRho() & select )
+ {
+ vpMatrix Lrho(1,6) ; Lrho = 0;
+
+ Lrho[0][0] = -c_/Z_ ;
+ Lrho[0][1] = -s_/Z_ ;
+ Lrho[0][2] = rho/Z_ ;
+ Lrho[0][3] = (1+rho2)*s_ ;
+ Lrho[0][4] = -(1+rho2)*c_ ;
+ Lrho[0][5] = 0 ;
+
+// printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
+// std::cout << "Lrho: " << Lrho << std::endl;
+
+ L = vpMatrix::stack(L,Lrho) ;
+ }
+
+ if (vpFeaturePointPolar::selectTheta() & select )
+ {
+ vpMatrix Ltheta(1,6) ; Ltheta = 0;
+
+ Ltheta[0][0] = s_/(rho*Z_) ;
+ Ltheta[0][1] = -c_/(rho*Z_) ;
+ Ltheta[0][2] = 0 ;
+ Ltheta[0][3] = c_/rho ;
+ Ltheta[0][4] = s_/rho ;
+ Ltheta[0][5] = -1 ;
+
+// printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
+// std::cout << "Ltheta: " << Ltheta << std::endl;
+ L = vpMatrix::stack(L,Ltheta) ;
+ }
+ return L ;
+}
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ For the angular component \f$\theta\f$, we define the error as
+ \f$\theta \ominus \theta^*\f$, where \f$\ominus\f$ is modulo \f$2\pi\f$
+ substraction.
+
+ \param s_star : Desired 2D image point visual feature with
+ polar coordinates.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible 2D point polar coordinate features.
+ - To compute the error for all the three coordinates use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ dimension column vector.
+ - To compute the error for only one of the polar coordinate
+ feature \f$(\rho,\theta)\f$ use one of the
+ corresponding function selectRho() or selectTheta(). In
+ that case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ The code below shows how to use this method to manipulate the
+ \f$\rho\f$ component.
+
+ \code
+ vpFeaturePointPolar s;
+ // Creation of the current feature s
+ s.buildFrom(0.2, ..., 1); // rho and Z need to be set
+ // Build the interaction matrix L associated to rho component
+ vpMatrix L_rho = s.interaction( vpFeaturePointPolar::selectRho() );
+
+ vpFeaturePointPolar s_star;
+ // Creation of the desired feature s*
+ s_star.buildFrom(0.45, ..., 1.2); // rho and Z need to be set
+
+ // Compute the error vector (s-s*) for the rho feature
+ vpColVector e = s.error(s_star, vpFeaturePointPolar::selectRho());
+ \endcode
+*/
+vpColVector
+vpFeaturePointPolar::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeaturePointPolar::selectRho() & select )
+ {
+ vpColVector erho(1) ;
+ erho[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,erho) ;
+ }
+
+ if (vpFeaturePointPolar::selectTheta() & select )
+ {
+
+ // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
+ double err = s[1] - s_star[1] ;
+
+ // printf("Error: %f ", err );
+ while (err < -M_PI) err += 2*M_PI ;
+ while (err > M_PI) err -= 2*M_PI ;
+ // printf(" modif %f \n", err );
+
+ vpColVector etheta(1) ;
+ etheta[0] = err ;
+ e = vpColVector::stack(e,etheta) ;
+ }
+ }
+ catch(...) {
+ throw ;
+ }
+
+
+ return e ;
+
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature.
+
+ \param select : Selection of a subset of the possible 2D image point
+ feature coordinates.
+ - To print all the two polar coordinates \f$(\rho,\theta)\f$ used as
+ features use vpBasicFeature::FEATURE_ALL.
+ - To print only one of the polar coordinate
+ feature \f$(\rho,\theta)\f$ use one of the
+ corresponding function selectRho() or selectTheta().
+
+ \code
+ // Creation of the current feature s
+ vpFeaturePointPolar s;
+ s.buildFrom(0.1, M_PI_2, 1.3);
+
+ s.print(); // print all the 2 components of the image point feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeaturePointPolar::selectRho()); // print only the rho component
+ \endcode
+*/
+void
+vpFeaturePointPolar::print(const unsigned int select ) const
+{
+
+ std::cout <<"Point: Z=" << get_Z() ;
+ if (vpFeaturePointPolar::selectRho() & select )
+ std::cout << " rho=" << get_rho() ;
+ if (vpFeaturePointPolar::selectTheta() & select )
+ std::cout << " theta=" << get_theta() ;
+ std::cout <<std::endl ;
+}
+
+
+/*!
+
+ Build a 2D image point visual feature with polar coordinates.
+
+ \param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
+ the image point.
+
+ \param Z_ : 3D depth of the point in the camera frame.
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is negative. That means that the 3D point is
+ behind the camera which is not possible.
+
+ \exception vpFeatureException::badInitializationError: If the depth
+ (\f$Z\f$ coordinate) is null. That means that the 3D point is
+ on the camera which is not possible.
+*/
+void
+vpFeaturePointPolar::buildFrom(const double rho, const double theta,
+ const double Z_)
+{
+
+ s[0] = rho ;
+ s[1] = theta ;
+
+ this->Z = Z_ ;
+
+ if (Z < 0)
+ {
+ vpERROR_TRACE("Point is behind the camera ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point is behind the camera ")) ;
+ }
+
+ if (fabs(Z) < 1e-6)
+ {
+ vpERROR_TRACE("Point Z coordinates is null ") ;
+ std::cout <<"Z = " << Z << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "Point Z coordinates is null")) ;
+ }
+
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+
+ Display image point feature.
+
+ \param cam : Camera parameters.
+ \param I : Image.
+ \param color : Color to use for the display
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeaturePointPolar::display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try {
+ double rho,theta;
+ rho = get_rho();
+ theta = get_theta();
+
+ double x,y;
+ x = rho*cos(theta);
+ y = rho*sin(theta);
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Display image point feature.
+
+ \param cam : Camera parameters.
+ \param I : color Image.
+ \param color : Color to use for the display
+ \param thickness : Thickness of the feature representation.
+
+ */
+void
+vpFeaturePointPolar::display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try {
+ double rho,theta;
+ rho = get_rho();
+ theta = get_theta();
+
+ double x,y;
+ x = rho*cos(theta);
+ y = rho*sin(theta);
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
+
+ }
+ catch(...) {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+/*!
+
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeaturePointPolar s;
+ s_star = s.duplicate(); // s_star is now a vpFeaturePointPolar
+ \endcode
+
+*/
+vpFeaturePointPolar *vpFeaturePointPolar::duplicate() const
+{
+ vpFeaturePointPolar *feature = new vpFeaturePointPolar;
+ return feature ;
+}
+
+/*!
+
+ Function used to select the \f$\rho\f$ subset polar
+ coordinate of the image point visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$\rho\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePointPolar p;
+ vpServo task;
+ ...
+ // Add only the rho subset coordinate feature from an image point to the task
+ task.addFeature(p, vpFeaturePointPolar::selectRho());
+ \endcode
+
+ \sa selectTheta()
+*/
+unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0] ; }
+
+
+/*!
+
+ Function used to select the \f$\theta\f$ subset polar
+ coordinate of the image point visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$\theta\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeaturePointPolar p;
+ vpServo task;
+ ...
+ // Add only the theta subset coordinate feature from an image point to the task
+ task.addFeature(p, vpFeaturePointPolar::selectTheta());
+ \endcode
+
+ \sa selectRho()
+*/
+unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp
new file mode 100644
index 0000000..f133a15
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp
@@ -0,0 +1,696 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Segment visual feature.
+ *
+ * Authors:
+ * Filip Novotny
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDisplay.h>
+#include <cmath>
+
+// Exception
+#include <visp3/core/vpException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpFeatureSegment.cpp
+ \brief class that defines the vpFeatureSegment visual feature
+*/
+
+/*!
+
+ Initialise the memory space requested for segment visual
+ feature.
+
+*/
+void
+vpFeatureSegment::init()
+{
+ //feature dimension
+ dim_s = 4 ;
+ nbParameters = 6;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+}
+
+/*!
+ Default constructor that builds an empty segment visual feature.
+
+ \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
+ If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
+*/
+vpFeatureSegment::vpFeatureSegment(bool normalized)
+ : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
+{
+ init();
+}
+
+/*!
+ Compute and return the interaction matrix \f$ L \f$ associated to a
+ subset of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
+ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
+
+ The interaction matrix of the non normalized feature set is of the following form:
+ \f[
+ {\bf L} = \left[
+ \begin{array}{c}
+ L_{x_c} \\
+ L_{y_c} \\
+ L_{l} \\
+ L_{\alpha}
+ \end{array}
+ \right] =
+ \left[
+ \begin{array}{cccccc}
+ -\lambda_2 & 0 & \lambda_2 x_c - \lambda_1 l \frac{\cos \alpha}{4} &
+ x_c y_c + l^2 \frac{\cos \alpha \sin \alpha}{4} &
+ -(1 + {x_{c}}^{2} + l^2 \frac{\cos^2\alpha}{4}) &
+ y_c \\
+ 0 & -\lambda_2 & \lambda_2 y_c - \lambda_1 l \frac{\sin \alpha}{4} &
+ 1 + {y_{c}}^{2} + l^2 \frac{\sin^2 \alpha}{4} &
+ -x_c y_c-l^2 \frac{\cos \alpha \sin \alpha}{4} &
+ -x_c \\
+ \lambda_1 \cos \alpha & \lambda_1 \sin \alpha &
+ \lambda_2 l - \lambda_1 (x_c \cos \alpha + y_c \sin \alpha) &
+ l (x_c \cos \alpha \sin \alpha + y_c (1 + \sin^2 \alpha)) &
+ -l (x_c (1 + \cos^2 \alpha)+y_c \cos \alpha \sin \alpha) &
+ 0 \\
+ -\lambda_1 \frac{\sin \alpha}{l} & \lambda_1 \frac{\cos \alpha}{l} &
+ \lambda_1 \frac{x_c \sin \alpha - y_c \cos \alpha}{l} &
+ -x_c \sin^2 \alpha + y_c \cos \alpha \sin \alpha &
+ x_c \cos \alpha \sin \alpha - y_c \cos^2 \alpha &
+ -1
+ \end{array}
+ \right]
+ \f]
+
+ with \f$ \lambda_1 = \frac{Z_1 - Z_2}{Z_1 Z_2}\f$ and \f$ \lambda_2 = \frac{Z_1 + Z_2}{2 Z_1 Z_2}\f$
+ where \f$Z_i\f$ are the depths of the points.
+
+
+ \param select : Selection of a subset of the possible segment features.
+ - To compute the interaction matrix for all the four
+ subset features \f$(x_c \f$, \f$ y_c \f$, \f$ l \f$, \f$ \alpha)\f$ or
+ \f$(x_n \f$, \f$ y_n \f$, \f$ l_n \f$, \f$ \alpha)\f$ use vpBasicFeature::FEATURE_ALL. In
+ that case the dimension of the interaction matrix is \f$ [4 \times 6] \f$.
+ - To compute the interaction matrix for only one of the subset
+ use one of the following functions:
+ selectXc(), selectYc(), selectL(), selectAlpha(). In that case, the returned
+ interaction matrix is of dimension \f$ [1 \times 6] \f$ .
+
+ \return The interaction matrix computed from the segment features.
+
+ The code below shows how to compute the interaction matrix associated to
+ the visual feature \f${\bf s} = (x_c, y_c, l, \alpha)\f$.
+ \code
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpPoint.h>
+
+int main()
+{
+ // Define two 3D points in the object frame
+ vpPoint p1(.1, .1, 0.), p2(.3, .2, 0.);
+
+ // Define the camera pose wrt the object
+ vpHomogeneousMatrix cMo (0, 0, 1, 0, 0, 0); // Z=1 meter
+ // Compute the coordinates of the points in the camera frame
+ p1.changeFrame(cMo);
+ p2.changeFrame(cMo);
+ // Compute the coordinates of the points in the image plane by perspective projection
+ p1.project();
+ p2.project();
+
+ // Build the segment visual feature
+ vpFeatureSegment s;
+ s.buildFrom(p1.get_x(), p1.get_y(), p1.get_Z(), p2.get_x(), p2.get_y(), p2.get_Z());
+
+ // Compute the interaction matrix
+ vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
+}
+ \endcode
+
+ In this case, L is a 4 by 6 matrix.
+
+ It is also possible to build the interaction matrix associated to
+ one of the possible features. The code below shows how to modify the previous code to consider as
+ visual feature \f$s = (l, \alpha)\f$.
+
+ \code
+ vpMatrix L = s.interaction( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() );
+ \endcode
+
+ In that case, L is a 2 by 6 matrix.
+*/
+vpMatrix
+vpFeatureSegment::interaction( const unsigned int select )
+{
+
+ vpMatrix L ;
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but xc was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but Yc was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but l was not set yet");
+ break;
+ case 3:
+ vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
+ break;
+ case 4:
+ vpTRACE("Warning !!! The interaction matrix is computed but Z1 was not set yet");
+ break;
+ case 5:
+ vpTRACE("Warning !!! The interaction matrix is computed but Z2 was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ }
+
+ //This version is a simplification
+ double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
+ double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
+
+ if (normalized_)
+ {
+ // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
+ double xn = xc_;
+ double yn = yc_;
+ double ln = l_;
+ double lambda = -lambda1 * ln;
+ double Zn_inv = lambda2 * ln;
+ double lc = cos_a_ / ln;
+ double ls = sin_a_ / ln;
+ double xnalpha = xn*cos_a_+yn*sin_a_;
+ double lnc = cos_a_ * ln;
+ double lns = sin_a_ * ln;
+
+ if (vpFeatureSegment::selectXc() & select ){
+ vpMatrix Lxn(1,6);
+ Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
+ Lxn[0][1] = lambda * xn * sin_a_ ;
+ Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
+ Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
+ Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
+ Lxn[0][5] = yn ;
+ L = vpMatrix::stack(L, Lxn) ;
+ }
+
+ if (vpFeatureSegment::selectYc() & select ){
+ vpMatrix Lyn(1,6);
+ Lyn[0][0] = lambda*yn*cos_a_ ;
+ Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
+ Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
+ Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
+ Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
+ Lyn[0][5] = -xn ;
+ L = vpMatrix::stack(L, Lyn) ;
+ }
+
+ if (vpFeatureSegment::selectL() & select ){
+ vpMatrix Lln(1,6);
+ Lln[0][0] = lambda * lnc ;
+ Lln[0][1] = lambda * lns ;
+ Lln[0][2] = -(Zn_inv + lambda*xnalpha);
+ Lln[0][3] = -yn-xnalpha*sin_a_ ;
+ Lln[0][4] = xn + xnalpha*cos_a_ ;
+ Lln[0][5] = 0 ;
+ L = vpMatrix::stack(L, Lln) ;
+ }
+ if (vpFeatureSegment::selectAlpha() & select ){
+ // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
+ vpMatrix Lalpha(1,6);
+ Lalpha[0][0] = -lambda1*sin_a_*l_ ;
+ Lalpha[0][1] = lambda1*cos_a_*l_ ;
+ Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_);
+ Lalpha[0][3] = (-xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_)/l_;
+ Lalpha[0][4] = (xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_)/l_ ;
+ Lalpha[0][5] = -1 ;
+ L = vpMatrix::stack(L,Lalpha) ;
+ }
+ }
+ else
+ {
+ if (vpFeatureSegment::selectXc() & select ){
+ vpMatrix Lxc(1,6);
+ Lxc[0][0] = -lambda2 ;
+ Lxc[0][1] = 0. ;
+ Lxc[0][2] = lambda2*xc_ - lambda1*l_*cos_a_/4.;
+ Lxc[0][3] = xc_*yc_ + l_*l_*cos_a_*sin_a_/4. ;
+ Lxc[0][4] = -(1+xc_*xc_+l_*l_*cos_a_*cos_a_/4.) ;
+ Lxc[0][5] = yc_ ;
+ L = vpMatrix::stack(L,Lxc) ;
+ }
+
+ if (vpFeatureSegment::selectYc() & select ){
+ vpMatrix Lyc(1,6);
+ Lyc[0][0] = 0. ;
+ Lyc[0][1] = -lambda2 ;
+ Lyc[0][2] = lambda2*yc_ - lambda1*l_*sin_a_/4.;
+ Lyc[0][3] = 1+yc_*yc_+l_*l_*sin_a_*sin_a_/4. ;
+ Lyc[0][4] = -xc_*yc_-l_*l_*cos_a_*sin_a_/4. ;
+ Lyc[0][5] = -xc_ ;
+ L = vpMatrix::stack(L,Lyc) ;
+ }
+
+ if (vpFeatureSegment::selectL() & select ){
+ vpMatrix Ll(1,6);
+ Ll[0][0] = lambda1*cos_a_ ;
+ Ll[0][1] = lambda1*sin_a_ ;
+ Ll[0][2] = lambda2*l_-lambda1*(xc_*cos_a_+yc_*sin_a_);
+ Ll[0][3] = l_*(xc_*cos_a_*sin_a_ + yc_*(1+sin_a_*sin_a_)) ;
+ Ll[0][4] = -l_*(xc_*(1+cos_a_*cos_a_)+yc_*cos_a_*sin_a_) ;
+ Ll[0][5] = 0 ;
+ L = vpMatrix::stack(L,Ll) ;
+ }
+ if (vpFeatureSegment::selectAlpha() & select ){
+ vpMatrix Lalpha(1,6);
+ Lalpha[0][0] = -lambda1*sin_a_/l_ ;
+ Lalpha[0][1] = lambda1*cos_a_/l_ ;
+ Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_)/l_;
+ Lalpha[0][3] = -xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_;
+ Lalpha[0][4] = xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_ ;
+ Lalpha[0][5] = -1 ;
+ L = vpMatrix::stack(L,Lalpha) ;
+ }
+ }
+
+ return L ;
+}
+
+/*!
+ Computes the error between the current and the desired visual features from a subset
+ of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
+ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
+
+ For the angular component \f$\alpha\f$, we define the error as
+ \f$\alpha \ominus \alpha^*\f$, where \f$\ominus\f$ is modulo \f$2\pi\f$
+ substraction.
+
+ \param s_star : Desired 2D segment feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible segment features.
+ - To compute the error for all the four parameters use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 4
+ dimension column vector.
+ - To compute the error for only one subfeature of
+ \f${\bf s} = (x_c, y_c, l, \alpha)\f$ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ feature set
+ use one of the following functions: selectXc(), selectYc(), selectL(), selectAlpha().
+
+ \return The error between the current and the desired
+ visual feature.
+
+*/
+vpColVector
+vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select )
+{
+ vpColVector e(0) ;
+
+ if (vpFeatureSegment::selectXc() & select ){
+ vpColVector exc(1) ;
+ exc[0] = xc_-s_star[0];
+ e = vpColVector::stack(e,exc) ;
+ }
+
+ if (vpFeatureSegment::selectYc() & select ){
+ vpColVector eyc(1) ;
+ eyc[0] = yc_ - s_star[1];
+ e = vpColVector::stack(e,eyc) ;
+ }
+
+ if (vpFeatureSegment::selectL() & select ){
+ vpColVector eL(1) ;
+ eL[0] = l_ - s_star[2];
+ e = vpColVector::stack(e,eL) ;
+ }
+
+ if (vpFeatureSegment::selectAlpha() & select ){
+ vpColVector eAlpha(1) ;
+ eAlpha[0] = alpha_ - s_star[3];
+ while (eAlpha[0] < -M_PI) eAlpha[0] += 2*M_PI ;
+ while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
+ e = vpColVector::stack(e,eAlpha) ;
+ }
+ return e ;
+}
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible segement features (\f$ x_c \f$,\f$ y_c \f$,\f$ l \f$,\f$ \alpha \f$).
+
+ \code
+ s.print();
+ \endcode
+
+ produces the following output:
+
+ \code
+ vpFeatureSegment: (xc = -0.255634; yc = -0.13311; l = 0.105005; alpha = 92.1305 deg)
+ \endcode
+
+ while
+ \code
+ s.print( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() );
+ \endcode
+
+ produces the following output:
+
+ \code
+ vpFeatureSegment: (l = 0.105005; alpha = 92.1305 deg)
+ \endcode
+*/
+void
+vpFeatureSegment::print( const unsigned int select ) const
+{
+ std::cout <<"vpFeatureSegment: (";
+ if (vpFeatureSegment::selectXc() & select ) {
+ if (normalized_)
+ std::cout << "xn = ";
+ else
+ std::cout << "xc = ";
+ std::cout << s[0] << "; ";
+ }
+ if (vpFeatureSegment::selectYc() & select ) {
+ if (normalized_)
+ std::cout << "yn = ";
+ else
+ std::cout << "yc = ";
+ std::cout << s[1] << "; ";
+ }
+ if (vpFeatureSegment::selectL() & select ) {
+ if (normalized_)
+ std::cout << "ln = ";
+ else
+ std::cout << "l = ";
+ std::cout << s[2] << "; ";
+ }
+ if (vpFeatureSegment::selectAlpha() & select ) {
+ std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
+ }
+ std::cout << ")" << std::endl;
+}
+
+/*!
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureSegment s;
+ s_star = s.duplicate(); // s_star is now a vpFeatureSegment
+ \endcode
+
+*/
+vpFeatureSegment *vpFeatureSegment::duplicate() const
+{
+ vpFeatureSegment *feature;
+
+ feature = new vpFeatureSegment(*this) ;
+ return feature ;
+}
+
+/*!
+
+ Displays a segment representing the feature on a greyscale image.
+ The two limiting points are displayed in cyan and yellow.
+
+ \param cam : Camera parameters.
+ \param I : Image.
+ \param color : Color to use for the segment.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeatureSegment::display(const vpCameraParameters &cam ,
+ const vpImage<unsigned char> & I,
+ const vpColor &color,
+ unsigned int thickness ) const
+{
+ double l, x, y;
+ if (normalized_) {
+ l = 1./l_;
+ x = xc_ * l;
+ y = yc_ * l;
+ }
+ else {
+ l = l_;
+ x = xc_;
+ y = yc_;
+ }
+
+ double x1 = x - (l/2.)*cos_a_;
+ double x2 = x + (l/2.)*cos_a_;
+
+ double y1 = y - (l/2.)*sin_a_;
+ double y2 = y + (l/2.)*sin_a_;
+ vpImagePoint ip1,ip2;
+
+ vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
+ vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ vpDisplay::displayCircle(I, ip1, 5, color, true);
+ vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
+}
+
+/*!
+
+ Displays a segment representing the feature on a RGBa image.
+ The two limiting points are displayed in cyan and yellow.
+
+ \param cam : Camera parameters.
+ \param I : Image.
+ \param color : Color to use for the segment.
+ \param thickness : Thickness of the feature representation.
+*/
+void
+vpFeatureSegment::display(const vpCameraParameters & cam ,
+ const vpImage<vpRGBa> & I,
+ const vpColor &color,
+ unsigned int thickness ) const
+{
+ double l, x, y;
+ if (normalized_) {
+ l = 1./l_;
+ x = xc_ * l;
+ y = yc_ * l;
+ }
+ else {
+ l = l_;
+ x = xc_;
+ y = yc_;
+ }
+
+ double x1 = x - (l/2.)*cos_a_;
+ double x2 = x + (l/2.)*cos_a_;
+
+ double y1 = y - (l/2.)*sin_a_;
+ double y2 = y + (l/2.)*sin_a_;
+ vpImagePoint ip1,ip2;
+
+ vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
+ vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
+ vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
+ vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
+}
+
+/*!
+
+ Build a segment visual feature from two points and their Z coordinates.
+
+ \param x1, y1 : coordinates of the first point in the image plane.
+ \param Z1 : depth of the first point in the camera frame.
+
+ \param x2, y2 : coordinates of the second point in the image plane.
+ \param Z2 : depth of the second point in the camera frame.
+
+ Depending on the feature set that is considered, the features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
+ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ are computed from the two points using the
+ following formulae:
+ \f[ x_c = \frac{x_1 + x_2}{2} \f]
+ \f[ y_c = \frac{y_1 + y_2}{2} \f]
+ \f[ l = \sqrt{{x_1 - x_2}^2 + {y_1 - y_2}^2} \f]
+ \f[ \alpha = arctan(\frac{y_1 - y_2}{x_1 - x_2}) \f]
+*/
+void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
+ const double x2, const double y2, const double Z2)
+{
+ double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
+ double x_c = (x1+x2)/2.;
+ double y_c = (y1+y2)/2.;
+ double alpha = atan2(y1-y2,x1-x2);
+
+ if (normalized_) {
+ setXc( x_c / l );
+ setYc( y_c / l );
+ setL( 1/l );
+ setAlpha( alpha );
+
+ setZ1(Z1);
+ setZ2(Z2);
+ }
+ else {
+ setXc( x_c );
+ setYc( y_c );
+ setL( l );
+ setAlpha( alpha );
+
+ setZ1(Z1);
+ setZ2(Z2);
+ }
+}
+
+/*!
+
+ Function used to select the \f$x_c\f$ or \f$x_n\f$ subfeature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$x_c\f$ or \f$x_n\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+vpFeatureSegment s, s_star; // Current and desired visual feature
+vpServo task;
+...
+// Add only the xc subset feature from a segment to the task
+task.addFeature(s, s_star, vpFeatureSegment::selectXc());
+ \endcode
+
+ \sa selectYc(), selectL(), selectAlpha()
+*/
+unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0] ; }
+
+/*!
+
+ Function used to select the \f$y_c\f$ or \f$y_n\f$ subfeature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$y_c\f$ or \f$y_n\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+vpFeatureSegment s, s_star; // Current and desired visual feature
+vpServo task;
+...
+// Add only the yc subset feature from a segment to the task
+task.addFeature(s, s_star, vpFeatureSegment::selectYc());
+ \endcode
+
+ \sa selectXc(), selectL(), selectAlpha()
+*/
+unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1] ; }
+
+/*!
+
+ Function used to select the \f$l\f$ or \f$l_n\f$ subfeature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$l\f$ or \f$l_n\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+vpFeatureSegment s, s_star; // Current and desired visual feature
+vpServo task;
+...
+// Add only the l subset feature from a segment to the task
+task.addFeature(s, s_star, vpFeatureSegment::selectL());
+ \endcode
+
+ \sa selectXc(), selectYc(), selectAlpha()
+*/
+unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2] ; }
+
+/*!
+
+ Function used to select the \f$\alpha\f$ subfeature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$\alpha\f$ feature.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+vpFeatureSegment s, s_star; // Current and desired visual feature
+vpServo task;
+...
+// Add only the alpha subset feature from a segment to the task
+task.addFeature(s, s_star, vpFeatureSegment::selectAlpha());
+ \endcode
+
+ \sa selectXc(), selectYc(), selectL()
+*/
+
+unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp
new file mode 100644
index 0000000..de6fea1
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp
@@ -0,0 +1,810 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * ThetaU visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/core/vpMath.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpFeatureThetaU.cpp
+ \brief class that defines the ThetaU visual feature
+*/
+/*
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities are useful but not mandatory
+
+*/
+
+/*!
+
+ Initialise the memory space requested for 3D \f$ \theta u \f$ visual
+ feature.
+*/
+void
+vpFeatureThetaU::init()
+{
+ //feature dimension
+ dim_s = 3 ;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+}
+
+/*!
+ Constructor that builds a visual feature and initialize it to zero.
+ The rotation representation of the \f$ \theta u\f$ visual feature
+ will be vpFeatureThetaU::cdRc by default. Use the function
+ setFeatureThetaURotationType() to set the desired type of feature.
+
+*/
+vpFeatureThetaU::vpFeatureThetaU()
+ : rotation(vpFeatureThetaU::cdRc)
+{
+ //vpTRACE("0x%x", this);
+ init() ;
+}
+
+/*!
+ Constructor that builds a visual feature and initialize it to zero.
+
+ \param r [in] : The rotation representation of the \f$ \theta u\f$
+ visual feature.
+
+*/
+vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r)
+ : rotation(r)
+{
+ //vpTRACE("0x%x", this);
+ init() ;
+
+ // kind of rotation representation
+ rotation = r;
+}
+
+/*!
+
+ Constructor that build a 3D visual feature from a \f$ \theta u \f$
+ vector that represent the rotation \f$ R \f$ the camera has to
+ achieve.
+
+ \param tu [in] : Rotation that the camera has to achieve in \f$
+ \theta u \f$ angle/axis representation. Depending on the rotation
+ representation type
+ (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
+ construct this object, the parameter \e tu represents either the
+ rotation that the camera has to achieve to move from the desired
+ camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
+ that the camera has to achieve to move from the current camera frame
+ to the desired one (\f$ ^{c}R_{c^*}\f$).
+
+ \param r [in] : The rotation representation of \f$ \theta u \f$.
+*/
+
+vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu,
+ vpFeatureThetaURotationRepresentationType r)
+ : rotation(r)
+{
+ init() ;
+
+ buildFrom(tu) ;
+}
+
+/*!
+
+ Constructor that build a 3D \f$ \theta u \f$ visual feature from a
+ rotation matrix \f$ R \f$ that represent the rotation that
+ the camera has to achieve.
+
+ \param R [in] : Rotation that the camera has to achieve. Depending
+ on r this rotation represent either the rotation that the camera has
+ to achieve to move from the desired camera frame to the current one
+ (\f$ ^{c^*}R_c\f$), or the rotation that the camera has to achieve to
+ move from the current camera frame to the desired one (\f$
+ ^{c}R_{c^*}\f$).
+
+ \param r [in] : The rotation representation of R.
+
+*/
+
+vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
+ vpFeatureThetaURotationRepresentationType r)
+ : rotation(r)
+{
+ init() ;
+
+ vpThetaUVector tu(R) ;
+ buildFrom(tu) ;
+}
+
+/*!
+
+ Constructor that build a 3D \f$ \theta u \f$ visual feature from an
+ homogeneous matrix \f$ M \f$ that represent the displacement that
+ the camera has to achieve.
+
+ \param M [in] : Homogeneous transformation that describe the
+ movement that the camera has to achieve. Only the rotational part of
+ this homogeneous transformation is taken into
+ consideration. Depending on r the rotation represent either the
+ rotation that the camera has to achieve to move from the desired
+ camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
+ that the camera has to achieve to move from the current camera frame
+ to the desired one (\f$ ^{c}R_{c^*}\f$).
+
+ \param r [in] : The rotation representation of M.
+
+
+*/
+vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
+ vpFeatureThetaURotationRepresentationType r)
+ : rotation(r)
+{
+ init() ;
+ vpRotationMatrix R ;
+ M.extract(R) ;
+ vpThetaUVector tu(R) ;
+ buildFrom(tu) ;
+}
+
+/*!
+
+ Build a 3D visual feature from a \f$ \theta u \f$ vector that
+ represent the rotation \f$ R \f$ the camera has to achieve.
+
+ \param tu [in] : Rotation that the camera has to achieve in \f$
+ \theta u \f$ angle/axis representation. Depending on the rotation
+ representation type
+ (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
+ construct this object, the parameter \e tu represents either the
+ rotation that the camera has to achieve to move from the desired
+ camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
+ that the camera has to achieve to move from the current camera frame
+ to the desired one (\f$ ^{c}R_{c^*}\f$).
+
+*/
+void
+vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
+{
+ s[0] = tu[0] ;
+ s[1] = tu[1] ;
+ s[2] = tu[2] ;
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+/*!
+ Build a 3D \f$ \theta u \f$ visual feature from a
+ rotation matrix \f$ R \f$ that represent the rotation that
+ the camera has to achieve.
+
+ \param R [in] : Rotation that the camera has to achieve. Depending
+ on the rotation representation type
+ (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
+ construct this object, the parameter \e R represents either the
+ rotation that the camera has to achieve to move from the desired
+ camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
+ that the camera has to achieve to move from the current camera frame
+ to the desired one (\f$ ^{c}R_{c^*}\f$).
+
+*/
+void
+vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
+{
+ vpThetaUVector tu(R) ;
+ buildFrom(tu) ;
+}
+
+/*!
+ Build a 3D \f$ \theta u \f$ visual feature from an
+ homogeneous matrix \f$ M \f$ that represent the displacement that
+ the camera has to achieve.
+
+ \param M [in] : Homogeneous transformation that describe the
+ movement that the camera has to achieve. Only the rotational part of
+ this homogeneous transformation is taken into consideration.
+ Depending on the rotation representation type
+ (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
+ construct this object, the parameter \e M represents either the
+ rotation that the camera has to achieve to move from the desired
+ camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
+ that the camera has to achieve to move from the current camera frame
+ to the desired one (\f$ ^{c}R_{c^*}\f$).
+
+
+*/
+void
+vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
+{
+ vpRotationMatrix R ;
+ M.extract(R) ;
+ vpThetaUVector tu(R) ;
+ buildFrom(tu) ;
+}
+
+/*!
+
+ Set the type of rotation feature.
+
+ \param r : type of feature. It can be vpFeatureThetaU::cdRc or vpFeatureThetaU::cRcd.
+ \sa getFeatureThetaURotationType()
+
+*/
+void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r)
+{
+ rotation = r;
+}
+
+/*!
+
+ Initialise the \f$\theta u_x \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param tu_x : \f$\theta u_x \f$ subset value to initialize.
+ \sa get_TUz()
+*/
+void vpFeatureThetaU::set_TUx(const double tu_x)
+{
+ s[0] = tu_x ;
+ flags[0] = true;
+}
+/*!
+
+ Initialise the \f$\theta u_y \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param tu_y : \f$\theta u_y \f$ subset value to initialize.
+ \sa get_TUy()
+*/
+void vpFeatureThetaU::set_TUy(const double tu_y)
+{
+ s[1] = tu_y ;
+ flags[1] = true;
+}
+/*!
+
+ Initialise the \f$\theta u_z \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param tu_z : \f$\theta u_z \f$ subset value to initialize.
+ \sa get_TUz()
+*/
+void
+vpFeatureThetaU::set_TUz(const double tu_z)
+{
+ s[2] = tu_z ;
+ flags[2] = true;
+}
+
+/*!
+
+ Get the type of rotation feature.
+
+ \return Type of rotation feature. It can be vpFeatureThetaU::cdRc or vpFeatureThetaU::cRcd.
+ \sa setFeatureThetaURotationType()
+
+*/
+vpFeatureThetaU::vpFeatureThetaURotationRepresentationType vpFeatureThetaU::getFeatureThetaURotationType() const
+{
+ return rotation;
+}
+
+/*!
+
+ Return the \f$\theta u_x \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double vpFeatureThetaU::get_TUx() const
+{
+ return s[0] ;
+}
+
+/*!
+ Return the \f$\theta u_y \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double vpFeatureThetaU::get_TUy() const
+{
+ return s[1] ;
+}
+
+
+/*!
+ Return the \f$\theta u_z \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double
+vpFeatureThetaU::get_TUz() const
+{
+ return s[2] ;
+}
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L \f$ from
+ a subset (\f$ \theta u_x, \theta u_y, \theta u_z\f$) of the possible
+ \f$ \theta u \f$ features that represent the 3D rotation
+ \f$^{c^*}R_c\f$ or \f$^{c}R_{c^*}\f$, with
+
+ \f[ L = [ 0_3 \; L_{\theta u}] \f]
+
+ See the vpFeatureThetaU class description for the equations of
+ \f$L_{\theta u}\f$.
+
+ \param select : Selection of a subset of the possible \f$ \theta u \f$
+ features.
+ - To compute the interaction matrix for all the three \f$ \theta u \f$
+ features use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
+ interaction matrix is \f$ [3 \times 6] \f$
+ - To compute the interaction matrix for only one of the \f$ \theta u \f$
+ component feature (\f$\theta u_x, \theta u_y, \theta u_z\f$) use one of the
+ corresponding function selectTUx(), selectTUy() or selectTUz(). In
+ that case the returned interaction matrix is \f$ [1 \times 6] \f$
+ dimension.
+
+ \return The interaction matrix computed from the \f$ \theta u \f$
+ features that represent either the rotation \f$^{c^*}R_c\f$ or the
+ rotation \f$^{c}R_{c^*}\f$.
+
+ The code below shows how to compute the interaction matrix
+ associated to the visual feature \f$s = \theta u_x \f$.
+
+ \code
+ vpRotationMatrix cdMc;
+
+ // Creation of the current feature s
+ vpFeatureThetaU s(vpFeatureThetaU::cdRc);
+ s.buildFrom(cdMc);
+
+ vpMatrix L_x = s.interaction( vpFeatureThetaU::selectTUx() );
+ \endcode
+
+ The code below shows how to compute the interaction matrix
+ associated to the \f$s = (\theta u_x, \theta u_y) \f$
+ subset visual feature:
+
+ \code
+ vpMatrix L_xy = s.interaction( vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy() );
+ \endcode
+
+ L_xy is here now a 2 by 6 matrix. The first line corresponds to
+ the \f$ \theta u_x \f$ visual feature while the second one to the \f$
+ \theta u_y \f$ visual feature.
+
+ It is also possible to build the interaction matrix from all the \f$
+ \theta u \f$ components by:
+
+ \code
+ vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+
+ In that case, L_xyz is a 3 by 6 interaction matrix where the last
+ line corresponds to the \f$ \theta u_z \f$ visual feature.
+
+*/
+vpMatrix
+vpFeatureThetaU::interaction(const unsigned int select)
+{
+
+ vpMatrix L ;
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
+ break;
+ case 2:
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
+ vpColVector u(3) ;
+ for (unsigned int i=0 ; i < 3 ; i++) {
+ u[i] = s[i]/2.0 ;
+ }
+
+ vpMatrix Lw(3,3) ;
+ Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
+
+ vpMatrix U2(3,3) ;
+ U2.eye() ;
+
+ double theta = sqrt(s.sumSquare()) ;
+ if (theta >= 1e-6) {
+ for (unsigned int i=0 ; i < 3 ; i++)
+ u[i] = s[i]/theta ;
+
+ vpMatrix skew_u ;
+ skew_u = vpColVector::skew(u) ;
+ U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
+ }
+
+ if (rotation == cdRc) {
+ Lw += U2;
+ }
+ else {
+ Lw -= U2;
+ }
+
+ //This version is a simplification
+ if (vpFeatureThetaU::selectTUx() & select )
+ {
+ vpMatrix Lx(1,6) ;
+
+ Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
+ for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
+
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeatureThetaU::selectTUy() & select )
+ {
+ vpMatrix Ly(1,6) ;
+
+ Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
+ for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+
+ if (vpFeatureThetaU::selectTUz() & select )
+ {
+ vpMatrix Lz(1,6) ;
+
+ Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
+ for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
+
+ L = vpMatrix::stack(L,Lz) ;
+ }
+
+ return L ;
+}
+
+/*!
+
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ Since this visual feature \f$ s \f$ represent either the rotation
+ from the desired camera frame to the current camera frame
+ \f$^{c^*}R_{c} \f$, or the rotation from the current camera frame to
+ the desired camera frame \f$^{c}R_{c^*} \f$, the desired visual
+ feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
+ the current visual feature \f$ s \f$.
+
+ \param s_star : Desired visual visual feature that should be equal to zero.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible \f$ \theta u \f$ features.
+ - To compute the error for all the three \f$ \theta u \f$ features use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ dimension column vector.
+ - To compute the error for only one of the \f$ \theta u \f$ component
+ feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
+ corresponding function selectTUx(), selectTUy() or selectTUz(). In
+ that case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ \exception vpFeatureException::badInitializationError : If the
+ desired visual feature \f$ s^* \f$ is not equal to zero.
+
+ The code below shows how to use this method to manipulate the
+ \f$\theta u_z \f$ subset:
+
+ \code
+ // Creation of the current feature s
+ vpFeatureThetaU s(vpFeatureThetaU::cdRc);
+ s.set_TUz(0.3);
+
+ // Creation of the desired feature s^*. By default this feature is
+ // initialized to zero
+ vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
+
+ // Compute the interaction matrix for the ThetaU_z feature
+ vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
+
+ // Compute the error vector (s-s*) for the ThetaU_z feature
+ s.error(s_star, vpFeatureThetaU::selectTUz());
+ \endcode
+
+ To manipulate the subset features \f$s=(\theta u_y, \theta u_z)\f$,
+ the code becomes:
+ \code
+ // Compute the interaction matrix for the ThetaU_y, ThetaU_z features
+ vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );
+
+ // Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
+ vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
+ \endcode
+
+*/
+vpColVector
+vpFeatureThetaU::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+
+ if (fabs(s_star.get_s().sumSquare()) > 1e-6)
+ {
+ vpERROR_TRACE("s* should be zero ! ") ;
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "s* should be zero !")) ;
+ }
+
+ vpColVector e(0) ;
+
+
+ if (vpFeatureThetaU::selectTUx() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0] ;
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeatureThetaU::selectTUy() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ if (vpFeatureThetaU::selectTUz() & select )
+ {
+ vpColVector ez(1) ;
+ ez[0] = s[2] ;
+ e = vpColVector::stack(e,ez) ;
+ }
+ return e ;
+}
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible \f$ \theta u
+ \f$ features.
+ - To print all the three \f$ \theta u \f$ features use
+ vpBasicFeature::FEATURE_ALL.
+ - To print only one of the \f$ \theta u \f$ component
+ feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
+ corresponding function selectTUx(), selectTUy() or selectTUz().
+
+ \code
+ vpThetaUVector tu; // Current visual feature s
+ tu[0] = 0.1;
+ tu[1] = 0.2;
+ tu[2] = 0.3;
+
+ // Creation of the current feature s
+ vpFeatureThetaU s(vpFeatureThetaU::cdRc);
+ s.buildFrom(tu);
+
+ s.print(); // print all the 3 components of the feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeatureThetaU::selectTUz()); // print only the ThetaU_z component
+ \endcode
+*/
+void
+vpFeatureThetaU::print(const unsigned int select) const
+{
+ std::cout <<"ThetaU: ";
+ if (vpFeatureThetaU::selectTUx() & select ) {
+ std::cout << s[0] << " ";
+ }
+ if (vpFeatureThetaU::selectTUy() & select ) {
+ std::cout << s[1] << " ";
+ }
+ if (vpFeatureThetaU::selectTUz() & select ) {
+ std::cout << s[2] << " ";
+ }
+ std::cout << std::endl;
+}
+
+/*!
+
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureThetaU s;
+ s_star = s.duplicate(); // s_star is now a vpFeatureThetaU
+ \endcode
+
+*/
+vpFeatureThetaU *vpFeatureThetaU::duplicate() const
+{
+ vpFeatureThetaU *feature;
+ if (rotation == cdRc)
+ feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
+ else //if (rotation == cRcd
+ feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
+
+ return feature ;
+}
+
+/*!
+
+ Not implemented.
+
+*/
+void
+vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
+ const vpImage<unsigned char> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+/*!
+
+ Not implemented.
+
+ */
+void
+vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
+ const vpImage<vpRGBa> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*!
+
+ Function used to select the \f$ \theta u_x\f$ subset of the \f$
+ \theta u \f$ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ \theta u_x\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeatureThetaU tu;
+ vpServo task;
+ ...
+ // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
+ // rotation to the task
+ task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
+ \endcode
+
+ \sa selectTUy(), selectTUz()
+*/
+unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0] ; }
+/*!
+
+ Function used to select the \f$ \theta u_y\f$ subset of the \f$
+ \theta u \f$ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ \theta u_y\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeatureThetaU tu;
+ vpServo task;
+ ...
+ // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
+ // rotation to the task
+ task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
+ \endcode
+
+ \sa selectTUx(), selectTUz()
+*/
+unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1] ; }
+/*!
+
+ Function used to select the \f$ \theta u_z\f$ subset of the \f$
+ \theta u \f$ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ \theta u_z\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ \code
+ vpFeatureThetaU tu;
+ vpServo task;
+ ...
+ // Add the (ThetaU_z) subset feature from the 3D ThetaU
+ // rotation to the task
+ task.addFeature(tu, vpFeatureThetaU::selectTUz());
+ \endcode
+
+ \sa selectTUx(), selectTUy()
+*/
+unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp
new file mode 100644
index 0000000..7af9c8f
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp
@@ -0,0 +1,784 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 3D translation visual feature.
+ *
+ * Authors:
+ * Eric Marchand
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+
+#include <visp3/core/vpMath.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+
+/*!
+ \file vpFeatureTranslation.cpp
+ \brief class that defines 3D translation visual feature
+*/
+/*
+
+attributes and members directly related to the vpBasicFeature needs
+other functionalities are useful but not mandatory
+
+*/
+
+/*!
+
+ Initialise the memory space requested for 3D translation visual
+ feature.
+*/
+void
+vpFeatureTranslation::init()
+{
+ //feature dimension
+ dim_s = 3 ;
+ nbParameters = 1;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+}
+
+/*!
+ Default constructor that builds a visual feature and initialize it to zero.
+ The type of the translation feature will be vpFeatureTranslation::cdMc
+ by default. Use the function setFeatureTranslationType() to set the
+ desired type of feature.
+*/
+vpFeatureTranslation::vpFeatureTranslation()
+ : f2Mf1(), translation(vpFeatureTranslation::cdMc)
+{
+ init() ;
+}
+
+
+/*!
+ Default constructor that builds a visual feature and initialize it to zero specifying the type.
+
+ \param r : Type of considered 3D translation feature.
+
+*/
+vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r)
+ : f2Mf1(), translation(r)
+{
+ init() ;
+}
+
+
+/*!
+
+ Constructor that builds a 3D visual feature from an homogeneous
+ matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
+
+ \param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
+ move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
+ \param r : type of feature. It can be vpFeature::cdMc or vpFeature::cMo.
+
+*/
+vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1_, vpFeatureTranslationRepresentationType r)
+ : f2Mf1(), translation(r)
+{
+ init() ;
+
+ buildFrom(f2Mf1_) ;
+}
+
+/*!
+ Build a 3D translation visual feature from an homogeneous
+ matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
+
+ \param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
+ move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
+*/
+void
+vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_)
+{
+ this->f2Mf1 = f2Mf1_ ;
+ s[0] = f2Mf1[0][3] ;
+ s[1] = f2Mf1[1][3] ;
+ s[2] = f2Mf1[2][3] ;
+
+ flags[0] = true;
+}
+
+/*!
+
+ Set the type of translation feature.
+
+ \param r : type of translation feature. It can be vpFeatureTranslation::cdMc, vpFeatureTranslation::cMcd
+ or vpFeatureTranslation::cMo.
+ \sa getFeatureTranslationType()
+
+*/
+void vpFeatureTranslation::setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
+{
+ translation = r;
+}
+
+/*!
+
+ Initialise the \f$t_x \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param t_x : \f$t_x \f$ subset value to initialize.
+ \sa get_Tx()
+
+*/
+void vpFeatureTranslation::set_Tx(const double t_x)
+{
+ s[0] = t_x ;
+}
+/*!
+
+ Initialise the \f$t_y \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param t_y : \f$t_y \f$ subset value to initialize.
+ \sa get_Ty()
+
+*/
+void vpFeatureTranslation::set_Ty(const double t_y)
+{
+ s[1] = t_y ;
+}
+/*!
+
+ Initialise the \f$t_z \f$ subset value of the 3D
+ visual feature \f$ s\f$.
+
+ \param t_z : \f$t_z \f$ subset value to initialize.
+ \sa get_Tz()
+
+*/
+void
+vpFeatureTranslation::set_Tz(const double t_z)
+{
+ s[2] = t_z ;
+}
+
+
+/*!
+
+ Get the type of translation feature.
+
+ \return Type of translation feature. It can be vpFeatureTranslation::cdMc, vpFeatureTranslation::cMcd
+ or vpFeatureTranslation::cMo.
+ \sa setFeatureTranslationType()
+
+*/
+vpFeatureTranslation::vpFeatureTranslationRepresentationType vpFeatureTranslation::getFeatureTranslationType() const
+{
+ return translation ;
+}
+
+/*!
+ Return the \f$t_x \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double vpFeatureTranslation::get_Tx() const
+{
+ return s[0] ;
+}
+
+
+/*!
+ Return the \f$t_y \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double vpFeatureTranslation::get_Ty() const
+{
+ return s[1] ;
+}
+
+
+/*!
+ Return the \f$t_z \f$ subset value of the visual feature
+ \f$s\f$.
+
+*/
+double
+vpFeatureTranslation::get_Tz() const
+{
+ return s[2] ;
+}
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L \f$ from a subset
+ \f$(t_x, t_y, t_z)\f$ of the possible translation features that
+ represent the 3D transformation \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$.
+
+ As it exists three different features, the computation of the
+ interaction matrix is diferent for each one.
+
+ - With the feature type cdMc:
+
+ \f[ L = [ ^{c^*}R_c \;\; 0_3] \f]
+
+ where \f$^{c^*}R_c\f$ is the rotation the camera has to achieve to
+ move from the desired camera frame to the current camera frame.
+
+ - With the feature type cMcd:
+
+ \f[ L = [ -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
+
+ where \f$^{c}R_{c^*}\f$ is the rotation the camera has to achieve to
+ move from the current camera frame to the desired camera frame.
+
+ - With the feature type cMo:
+
+ \f[ L = [ -I_3 \;\; [^{c}t_o]_\times] \f]
+
+ where \f$^{c}t_o \f$ is the position of
+ the object frame relative to the current camera frame.
+
+ \param select : Selection of a subset of the possible translation
+ features.
+ - To compute the interaction matrix for all the three translation
+ subset features \f$(t_x,t_y,t_y)\f$ use vpBasicFeature::FEATURE_ALL. In
+ that case the dimension of the interaction matrix is \f$ [3 \times
+ 6] \f$
+ - To compute the interaction matrix for only one of the translation
+ subset (\f$t_x, t_y, t_z\f$) use
+ one of the corresponding function selectTx(), selectTy() or
+ selectTz(). In that case the returned interaction matrix is \f$ [1
+ \times 6] \f$ dimension.
+
+ \return The interaction matrix computed from the translation
+ features.
+
+ The code below shows how to compute the interaction matrix
+ associated to the visual feature \f$s = t_x \f$ using the cdMc feature type.
+
+ \code
+ vpHomogeneousMatrix cdMc;
+ ...
+ // Creation of the current feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc);
+ s.buildFrom(cdMc);
+
+ vpMatrix L_x = s.interaction( vpFeatureTranslation::selectTx() );
+ \endcode
+
+ The code below shows how to compute the interaction matrix
+ associated to the \f$s = (t_x, t_y) \f$
+ subset visual feature:
+
+ \code
+ vpMatrix L_xy = s.interaction( vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy() );
+ \endcode
+
+ L_xy is here now a 2 by 6 matrix. The first line corresponds to
+ the \f$ t_x \f$ visual feature while the second one to the \f$
+ t_y \f$ visual feature.
+
+ It is also possible to build the interaction matrix from all the
+ translation components by:
+
+ \code
+ vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
+ \endcode
+
+ In that case, L_xyz is a 3 by 6 interaction matrix where the last
+ line corresponds to the \f$ t_z \f$ visual feature.
+
+*/
+vpMatrix
+vpFeatureTranslation::interaction(const unsigned int select)
+{
+
+ vpMatrix L ;
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ if (translation == cdMc) {
+ //This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select ) {
+ vpMatrix Lx(1,6) ;
+
+ for (int i=0 ; i < 3 ; i++)
+ Lx[0][i] = f2Mf1[0][i] ;
+ Lx[0][3] = 0 ; Lx[0][4] = 0 ; Lx[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeatureTranslation::selectTy() & select ) {
+ vpMatrix Ly(1,6) ;
+
+ for (int i=0 ; i < 3 ; i++)
+ Ly[0][i] = f2Mf1[1][i] ;
+ Ly[0][3] = 0 ; Ly[0][4] = 0 ; Ly[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+
+ if (vpFeatureTranslation::selectTz() & select ) {
+ vpMatrix Lz(1,6) ;
+
+ for (int i=0 ; i < 3 ; i++)
+ Lz[0][i] = f2Mf1[2][i] ;
+ Lz[0][3] = 0 ; Lz[0][4] = 0 ; Lz[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Lz) ;
+ }
+ }
+ if (translation == cMcd) {
+ //This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select ) {
+ vpMatrix Lx(1,6) ;
+ Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
+ Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeatureTranslation::selectTy() & select ) {
+ vpMatrix Ly(1,6) ;
+ Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
+ Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+
+ if (vpFeatureTranslation::selectTz() & select ) {
+ vpMatrix Lz(1,6) ;
+ Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
+ Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Lz) ;
+ }
+ }
+
+ if (translation == cMo) {
+ //This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select ) {
+ vpMatrix Lx(1,6) ;
+ Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
+ Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeatureTranslation::selectTy() & select ) {
+ vpMatrix Ly(1,6) ;
+ Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
+ Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+
+ if (vpFeatureTranslation::selectTz() & select ) {
+ vpMatrix Lz(1,6) ;
+ Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
+ Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
+
+ L = vpMatrix::stack(L,Lz) ;
+ }
+ }
+
+ return L ;
+}
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ - With the feature type cdMc:
+ Since this visual feature \f$ s \f$ represent the 3D translation from the desired
+ camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
+ visual feature \f$ s^* \f$ should be zero. Thus, the error is here
+ equal to the current visual feature \f$ s \f$.
+
+ - With the feature type cMo:
+ In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.
+
+ \param s_star : Desired visual feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible translation features.
+ - To compute the error for all the three translation vector coordinates use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ dimension column vector.
+ - To compute the error for only one of the translation vector coordinate
+ feature \f$(t_x, t_y, t_z)\f$ use one of the
+ corresponding function selectTx(), selectTy() or selectTz(). In
+ that case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ \exception vpFeatureException::badInitializationError : If the
+ desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.
+
+ The code below shows how to use this method to manipulate the \f$
+ t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.
+
+ \code
+ // Creation of the current feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc);
+ s.set_TUz(0.3); // Initialization of the feature
+
+ // Creation of the desired feature s*. By default this feature is
+ // initialized to zero
+ vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
+
+ // Compute the interaction matrix for the t_z translation feature
+ vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );
+
+ // Compute the error vector (s-s*) for the t_z feature
+ s.error(s_star, vpFeatureTranslation::selectTz());
+ \endcode
+
+ To manipulate the subset features \f$s=(t_y, t_z)\f$,
+ the code becomes:
+ \code
+ // Compute the interaction matrix for the t_y, t_z features
+ vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz() );
+
+ // Compute the error vector e = (s-s*) for the t_y, t_z feature
+ vpColVector e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz());
+ \endcode
+
+*/
+vpColVector
+vpFeatureTranslation::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ if(translation == cdMc || translation == cMcd)
+ {
+ if (s_star.get_s().sumSquare() > 1e-6)
+ {
+ vpERROR_TRACE("s* should be zero ! ") ;
+ throw(vpFeatureException(vpFeatureException::badInitializationError,
+ "s* should be zero !")) ;
+ }
+ }
+
+
+ if (vpFeatureTranslation::selectTx() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0]-s_star[0] ;
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeatureTranslation::selectTy() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1]-s_star[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+
+ if (vpFeatureTranslation::selectTz() & select )
+ {
+ vpColVector ez(1) ;
+ ez[0] = s[2]-s_star[2] ;
+ e = vpColVector::stack(e,ez) ;
+ }
+
+ return e ;
+}
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible translation features.
+ - To print all the three translation vector coordinates used as features use
+ vpBasicFeature::FEATURE_ALL.
+ - To print only one of the translation coordinate
+ feature \f$(t_x, t_y, t_z)\f$ use one of the
+ corresponding function selectTx(), selectTy() or selectTz().
+
+ \code
+ vpHomogeneousMatrix cdMc; // Homogeneous transformation between the desired camera frame and the current camera frame.
+
+ // Creation of the current feature s
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc);
+ s.buildFrom(cdMc);
+
+ s.print(); // print all the 3 components of the translation feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpFeatureTranslation::selectTz()); // print only the t_z component
+ \endcode
+*/
+void
+vpFeatureTranslation::print(const unsigned int select) const
+{
+ std::cout <<"Translation 3D: ";
+ if (vpFeatureTranslation::selectTx() & select ) {
+ std::cout << s[0] << " ";
+ }
+ if (vpFeatureTranslation::selectTy() & select ) {
+ std::cout << s[1] << " ";
+ }
+ if (vpFeatureTranslation::selectTz() & select ) {
+ std::cout << s[2] << " ";
+ }
+ std::cout << std::endl;
+}
+
+
+/*!
+
+ Create an object with the same type.
+
+ \code
+ vpBasicFeature *s_star;
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc); //or vpFeatureTranslation s(vpFeatureTranslation::cMo);
+ s_star = s.duplicate(); // s_star is now a vpFeatureTranslation
+ \endcode
+
+*/
+vpFeatureTranslation *vpFeatureTranslation::duplicate() const
+{
+ vpFeatureTranslation *feature = NULL;
+ if (translation == cdMc)
+ feature = new vpFeatureTranslation(cdMc) ;
+ if (translation == cMo)
+ feature = new vpFeatureTranslation(cMo) ;
+ if (translation == cMcd)
+ feature = new vpFeatureTranslation(cMcd) ;
+ return feature ;
+}
+
+
+/*!
+
+ Not implemented.
+
+*/
+void
+vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
+ const vpImage<unsigned char> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+/*!
+
+ Not implemented.
+
+*/
+void
+vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
+ const vpImage<vpRGBa> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*!
+
+ Function used to select the \f$ t_x\f$ subset of the translation
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ t_x\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ - With the feature type cdMc:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cdMc);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ - With the feature type cMcd:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMcd);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ - With the feature type cMo:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMo);
+ vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ \sa selectTy(), selectTz()
+
+*/
+unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0] ; }
+
+/*!
+
+ Function used to select the \f$ t_y\f$ subset of the translation
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ t_y\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ - With the feature type cdMc:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cdMc);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ - With the feature type cMcd:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMcd);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ - With the feature type cMo:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMo);
+ vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
+ vpServo task;
+ ...
+ // Add the (tx,ty) subset features from 3D translation to the task
+ task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
+ \endcode
+
+ \sa selectTx(), selectTz()
+*/
+unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1] ; }
+
+/*!
+
+ Function used to select the \f$ t_z\f$ subset of the translation
+ visual feature.
+
+ This function is to use in conjunction with interaction() in order
+ to compute the interaction matrix associated to \f$ t_z\f$.
+
+ See the interaction() method for an usage example.
+
+ This function is also useful in the vpServo class to indicate that
+ a subset of the visual feature is to use in the control law:
+
+ - With the feature type cdMc:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cdMc);
+ vpServo task;
+ ...
+ // Add the (tz) subset feature from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTz());
+ \endcode
+
+ - With the feature type cMcd:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMcd);
+ vpServo task;
+ ...
+ // Add the (tz) subset feature from 3D translation to the task
+ task.addFeature(t, vpFeatureTranslation::selectTz());
+ \endcode
+
+ - With the feature type cMo:
+ \code
+ vpFeatureTranslation t(vpFeatureTranslation::cMo);
+ vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
+ vpServo task;
+ ...
+ // Add the (tz) subset feature from 3D translation to the task
+ task.addFeature(t, t_star, vpFeatureTranslation::selectTz());
+ \endcode
+
+ \sa selectTx(), selectTy()
+*/
+unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2] ; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp
new file mode 100644
index 0000000..a140974
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp
@@ -0,0 +1,309 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
+ *
+ * Authors:
+ * Odile Bourquardez
+ *
+ *****************************************************************************/
+
+
+/*! \file vpFeatureVanishingPoint.cpp
+ \brief Class that defines 2D vanishing
+ point visual feature (Z coordinate in 3D space is infinity)
+*/
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureVanishingPoint.h>
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+// math
+#include <visp3/core/vpMath.h>
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+void
+vpFeatureVanishingPoint::init()
+{
+ //feature dimension
+ dim_s = 2 ;
+ nbParameters = 2;
+
+ // memory allocation
+ s.resize(dim_s) ;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+
+ //Z not required (infinity)
+ //set_Z(1) ;
+
+}
+vpFeatureVanishingPoint::vpFeatureVanishingPoint()
+{
+ init() ;
+}
+
+
+//! set the point x-coordinates
+void
+vpFeatureVanishingPoint::set_x(const double _x)
+{
+ s[0] = _x ;
+ flags[0] = true;
+}
+//! get the point x-coordinates
+double
+vpFeatureVanishingPoint::get_x() const
+{
+ return s[0] ;
+}
+
+//! set the point y-coordinates
+void
+vpFeatureVanishingPoint::set_y(const double _y)
+{
+ s[1] = _y ;
+ flags[1] = true;
+}
+//! get the point y-coordinates
+double
+vpFeatureVanishingPoint::get_y() const
+{
+ return s[1] ;
+}
+
+
+//! set the point xy coordinates
+void
+vpFeatureVanishingPoint::set_xy(const double _x,
+ const double _y)
+{
+ set_x(_x) ;
+ set_y(_y) ;
+}
+
+
+//! compute the interaction matrix from a subset of the possible features
+vpMatrix
+vpFeatureVanishingPoint::interaction(const unsigned int select)
+{
+ vpMatrix L ;
+
+ L.resize(0,6) ;
+
+ if (deallocate == vpBasicFeature::user)
+ {
+ for (unsigned int i = 0; i < nbParameters; i++)
+ {
+ if (flags[i] == false)
+ {
+ switch(i){
+ case 0:
+ vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
+ break;
+ case 1:
+ vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
+ break;
+ default:
+ vpTRACE("Problem during the reading of the variable flags");
+ }
+ }
+ }
+ resetFlags();
+ }
+
+ double x = get_x() ;
+ double y = get_y() ;
+
+ if (vpFeatureVanishingPoint::selectX() & select )
+ {
+ vpMatrix Lx(1,6) ; Lx = 0;
+
+ Lx[0][0] = 0. ;
+ Lx[0][1] = 0. ;
+ Lx[0][2] = 0. ;
+ Lx[0][3] = x*y ;
+ Lx[0][4] = -(1+x*x) ;
+ Lx[0][5] = y ;
+
+ L = vpMatrix::stack(L,Lx) ;
+ }
+
+ if (vpFeatureVanishingPoint::selectY() & select )
+ {
+ vpMatrix Ly(1,6) ; Ly = 0;
+
+ Ly[0][0] = 0 ;
+ Ly[0][1] = 0. ;
+ Ly[0][2] = 0. ;
+ Ly[0][3] = 1+y*y ;
+ Ly[0][4] = -x*y ;
+ Ly[0][5] = -x ;
+
+ L = vpMatrix::stack(L,Ly) ;
+ }
+ return L ;
+}
+
+
+/*! compute the error between two visual features from a subset of the possible
+ features
+ */
+vpColVector
+vpFeatureVanishingPoint::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try{
+ if (vpFeatureVanishingPoint::selectX() & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[0] - s_star[0] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ if (vpFeatureVanishingPoint::selectY() & select )
+ {
+ vpColVector ey(1) ;
+ ey[0] = s[1] - s_star[1] ;
+ e = vpColVector::stack(e,ey) ;
+ }
+ }
+ catch(...) {
+ throw ;
+ }
+ return e ;
+}
+
+
+
+void
+vpFeatureVanishingPoint::print(const unsigned int select ) const
+{
+
+ std::cout <<"Point: " <<std::endl;
+ if (vpFeatureVanishingPoint::selectX() & select )
+ std::cout << " x=" << get_x() ;
+ if (vpFeatureVanishingPoint::selectY() & select )
+ std::cout << " y=" << get_y() ;
+ std::cout <<std::endl ;
+}
+
+
+void
+vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
+{
+ s[0] = _x ;
+ s[1] = _y ;
+ for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+}
+
+
+/*!
+
+ Display vanishing point feature.
+
+ \param cam : Camera parameters.
+ \param I : Image.
+ \param color : Color to use for the display.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double x,y ;
+ x = get_x() ;
+ y = get_y() ;
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+/*!
+
+ Display vanishing point feature.
+
+ \param cam : Camera parameters.
+ \param I : color Image.
+ \param color : Color to use for the display.
+ \param thickness : Thickness of the feature representation.
+
+*/
+void
+vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ const vpColor &color,
+ unsigned int thickness) const
+{
+ try{
+ double x,y ;
+ x = get_x() ;
+ y = get_y() ;
+
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+
+/*! for memory issue (used by the vpServo class only)
+ */
+vpFeatureVanishingPoint *vpFeatureVanishingPoint::duplicate() const
+{
+ vpFeatureVanishingPoint *feature = new vpFeatureVanishingPoint ;
+ return feature ;
+}
+
+unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1] ; }
diff --git a/modules/visual_features/src/visual-feature/vpGenericFeature.cpp b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp
new file mode 100644
index 0000000..39fd8b7
--- /dev/null
+++ b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp
@@ -0,0 +1,754 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Generic feature (used to create new feature not implemented in ViSP).
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+#include <visp3/visual_features/vpGenericFeature.h>
+
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/visual_features/vpFeatureException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+
+
+/*!
+ \file vpGenericFeature.cpp
+ Class that defines what is a generic feature. This class could be used to create new
+ features not implemented in ViSP.
+*/
+
+vpGenericFeature::~vpGenericFeature()
+{
+
+}
+
+void vpGenericFeature::init()
+{
+ s = 0 ;
+}
+
+
+/*!
+
+ Default constructor. You are not allowed to use this
+ constructor. Please use the vpGenericFeature::vpGenericFeature(int
+ _dim) constructor.
+
+ \exception vpException::cannotUseConstructorError : If the use call
+ this constructor.
+
+*/
+vpGenericFeature::vpGenericFeature()
+ : L(), err(), errorStatus(errorNotInitalized)
+{
+ /*
+ vpERROR_TRACE("You are not allow to use this constructor ") ;
+ vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
+ "constructor") ;
+ vpERROR_TRACE("And provide the dimension of the visual feature ") ;
+ throw(vpException(vpException::cannotUseConstructorError,
+ "You are not allow to use this constructor ")) ;
+ */
+}
+
+
+/*!
+ Constructor of the class you have to use. The feature table is initilialized with the good dimension.
+
+ \param dimension_gen_s : Dimension of the generic feature. It corresponds to the number of features you want to create.
+*/
+vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s)
+ : L(), err(), errorStatus(errorNotInitalized)
+{
+ this->dim_s = dimension_gen_s ;
+ s.resize(dimension_gen_s) ;
+}
+
+/*!
+
+ Set the error vector \f$(s-s*)\f$.
+
+ \param error_vector : Error vector \f$(s-s*)\f$.
+
+ \exception vpFeatureException::sizeMismatchError : If the size of
+ the error vector is bad.
+*/
+void
+vpGenericFeature::setError(const vpColVector &error_vector)
+{
+ if (error_vector.getRows() != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between error dimension"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between error dimension"
+ "and feature dimension"));
+
+ }
+ errorStatus = errorInitialized ;
+ err = error_vector ;
+}
+
+
+/*!
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features.
+
+ \exception if errorHasBeenInitialized is true (that is if
+ vpGenericFeature::setError have been used) then s_star is useless. In that
+ since the error HAS TO BE recomputed at each iteration
+ errorHasBeenInitialized is set to errHasToBeUpdated if
+ vpGenericFeature::serError is not used in the loop then an exception is
+ thrown
+
+ obviously if vpGenericFeature::setError is not used then s_star is considered
+ and this warning is meaningless.
+
+ \param s_star : Desired visual feature.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible features.
+ - To compute the error for all the features use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector column
+ vector whose dimension is equal to the number of features.
+ - To compute the error for only one of the component feature you
+ have to say which one you want to take into account. If it is the
+ first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
+ the second one set select to vpBasicFeature::FEATURE_LINE[1], and
+ so on. In that case the error vector is a 1 dimension column
+ vector.
+ - To compute the error for only two of the component feature you
+ have to say which ones you want to take into account. If it is the
+ first one and the second one set select to
+ vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
+ that case the error vector is a 2 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature.
+
+ The code below shows how to use this method to manipulate the two
+ visual features over three:
+
+ \code
+ // Creation of the current feature s
+ vpGenericFeature s(3);
+ s.set_s(0, 0, 0);
+
+ // Creation of the desired feature s*
+ vpGenericFeature s_star(3);
+ s_star.set_s(1, 1, 1);
+
+ // Here you have to compute the interaction matrix L
+ s.setInteractionMatrix(L);
+
+ // Compute the error vector (s-s*) for the two first features
+ s.error(s_star, vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
+ \endcode
+*/
+vpColVector
+vpGenericFeature::error(const vpBasicFeature &s_star,
+ const unsigned int select)
+{
+ if (s_star.get_s().getRows() != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between s* dimension "
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between s* dimension "
+ "and feature dimension"));
+
+ }
+
+ vpColVector e(0) ;
+
+ try
+ {
+ if (errorStatus == errorHasToBeUpdated)
+ {
+ vpERROR_TRACE("Error has no been updated since last iteration"
+ "you should have used vpGenericFeature::setError"
+ "in you visual servoing loop") ;
+ throw(vpFeatureException(vpFeatureException::badErrorVectorError,
+ "Error has no been updated since last iteration"));
+ }
+ else
+ if (errorStatus == errorInitialized)
+ {
+ vpDEBUG_TRACE(25,"Error init: e=e.");
+ errorStatus = errorHasToBeUpdated ;
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ vpColVector ex(1) ;
+ ex[i] = err[i] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+ }
+ else
+ {
+ vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
+
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ vpColVector ex(1) ;
+ ex[0] = s[i] - s_star[i] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ }
+ }
+ catch(...) {
+ throw;
+ }
+ return e ;
+
+}
+
+
+
+/*!
+
+ Compute the error \f$ (s-s^*)\f$ between the current and the desired
+ visual features from a subset of the possible features. But in this
+ case the desired feature is considered as set to 0.
+
+ \param select : The error can be computed for a selection of a
+ subset of the possible features.
+ - To compute the error for all the features use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector column
+ vector whose dimension is equal to the number of features.
+ - To compute the error for only one of the component feature you
+ have to say which one you want to take into account. If it is the
+ first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
+ the second one set select to vpBasicFeature::FEATURE_LINE[1], and
+ so on. In that case the error vector is a 1 dimension column
+ vector.
+ - To compute the error for only two of the component feature you
+ have to say which ones you want to take into account. If it is the
+ first one and the second one set select to
+ vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
+ that case the error vector is a 2 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired
+ visual feature which is automatically set to zero.
+
+ The code below shows how to use this method to manipulate the two
+ visual features over three:
+ \code
+ // Creation of the current feature s
+ vpGenericFeature s(3);
+ s.set_s(0, 0, 0);
+
+ // Here you have to compute the interaction matrix L
+ s.setInteractionMatrix(L);
+
+ // Compute the error vector (s-s*) for the two first features
+ s.error(vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
+ \endcode
+*/
+vpColVector
+vpGenericFeature::error( const unsigned int select)
+{
+ vpColVector e(0) ;
+
+ try
+ {
+ if (errorStatus == errorHasToBeUpdated)
+ {
+ vpERROR_TRACE("Error has no been updated since last iteration"
+ "you should have used vpGenericFeature::setError"
+ "in you visual servoing loop") ;
+ throw(vpFeatureException(vpFeatureException::badErrorVectorError,
+ "Error has no been updated since last iteration"));
+ }
+ else
+ if (errorStatus == errorInitialized)
+ {
+ errorStatus = errorHasToBeUpdated ;
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ vpColVector ex(1) ;
+ ex[i] = err[i] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+ }
+ else
+ {
+
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ vpColVector ex(1) ;
+ ex[i] = s[i] ;
+
+ e = vpColVector::stack(e,ex) ;
+ }
+
+ }
+ }
+ catch(...) {
+ throw;
+ }
+
+ return e ;
+
+}
+
+
+/*!
+
+ Compute and return the interaction matrix \f$ L \f$ for the whole
+ features or a part of them.
+
+ \param select : Selection of a subset of the possible features.
+ - To compute the interaction matrix for all the features use
+ vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
+ matrix is \f$ [number of features \times 6] \f$
+ - To compute the interaction matrix for only one of the component
+ feature you have to say which one you want to take into
+ account. If it is the first one set select to
+ vpBasicFeature::FEATURE_LINE[0], if it is the second one set
+ select to vpBasicFeature::FEATURE_LINE[1], and so on. In that case
+ the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
+ - To compute the interaction matrix for only two of the component
+ features you have to say which ones you want to take into
+ account. If it is the first one and the second one set select to
+ vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
+ that case the returned interaction matrix is \f$ [2 \times 6] \f$
+ dimension.
+
+ \return The interaction matrix computed from the features.
+
+ The code below shows how to compute the interaction matrix associated to the
+ first visual feature.
+ \code
+ // Creation of the current feature s
+ vpGenericFeature s(3);
+ s.set_s(0, 0, 0);
+
+ // Here you have to compute the interaction matrix L for all the three features
+ s.setInteractionMatrix(L);
+
+ vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0] );
+ \endcode
+
+ The code below shows how to compute the interaction matrix associated to two
+ visual features over three.
+ \code
+ // Creation of the current feature s
+ vpGenericFeature s(3);
+ s.set_s(0, 0, 0);
+
+ // Here you have to compute the interaction matrix L
+ s.setInteractionMatrix(L);
+
+ vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0]|vpBasicFeature::FEATURE_LINE[1] );
+ \endcode
+*/
+vpMatrix
+vpGenericFeature::interaction(const unsigned int select)
+{
+ if (L.getRows() == 0)
+ {
+ std::cout << "interaction matrix " << L << std::endl ;
+ vpERROR_TRACE("Interaction has not been initialized");
+ std::cout << "A possible reason (may be) is that you have set" << std::endl ;
+ std::cout << "the interaction matrix for s and compute a control " << std::endl ;
+ std::cout << "with Ls=s* (default) or vice versa" << std::endl ;
+
+ throw(vpFeatureException(vpFeatureException::notInitializedError,
+ "size mismatch between s* dimension "
+ "and feature dimension"));
+
+ }
+
+ vpMatrix Ls ;
+
+ Ls.resize(0,6) ;
+
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ vpMatrix Lx(1,6) ; Lx = 0;
+
+ for (int j=0 ; j < 6 ; j++)
+ Lx[0][j] = L[i][j] ;
+
+ Ls = vpMatrix::stack(Ls,Lx) ;
+ }
+
+ return Ls ;
+}
+
+
+/*!
+ \brief set the value of the interaction matrix.
+
+ \param L_ : The matrix corresponding to the interaction matrix you computed.
+
+ \exception an exception is thrown if the number of row of the interaction
+ matrix is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
+{
+ if (L_.getRows() != dim_s)
+ {
+ std::cout << L_.getRows() <<" " << dim_s << std::endl ;;
+ vpERROR_TRACE("size mismatch between interaction matrix size "
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between interaction matrix size "
+ "and feature dimension"));
+ }
+
+ this->L = L_ ;
+}
+
+/*!
+ \brief set the value of all the features.
+
+ \param s_vector : It is a vector containing the value of the visual features.
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::set_s(const vpColVector &s_vector)
+{
+
+ if (s_vector.getRows() != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between s dimension"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between s dimension"
+ "and feature dimension"));
+ }
+ this->s = s_vector ;
+}
+
+
+/*!
+ \brief get the value of all the features.
+
+ \param s_vector : It is a vector which will contain the value of the visual features.
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::get_s(vpColVector &s_vector) const
+{
+ if (s_vector.getRows() != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between s dimension"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between s dimension"
+ "and feature dimension"));
+ }
+ s_vector = this->s ;
+}
+
+
+/*!
+ \brief set the value of three features if the number of feature is equal to 3.
+
+ \param s0 : value of the first visual feature
+
+ \param s1 : value of the second visual feature
+
+ \param s2 : value of the third visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::set_s(const double s0, const double s1, const double s2)
+{
+
+ if (3 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
+}
+
+
+/*!
+ \brief get the value of three features if the number of feature is equal to 3.
+
+ \param s0 : value of the first visual feature
+
+ \param s1 : value of the second visual feature
+
+ \param s2 : value of the third visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
+{
+
+ if (3 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
+}
+
+
+/*!
+ \brief set the value of two features if the number of feature is equal to 2.
+
+ \param s0 : value of the first visual feature
+
+ \param s1 : value of the second visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::set_s(const double s0, const double s1)
+{
+
+ if (2 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s[0] = s0 ; s[1] = s1 ;
+}
+
+
+/*!
+ \brief get the value of two features if the number of feature is equal to 2.
+
+ \param s0 : value of the first visual feature
+
+ \param s1 : value of the second visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::get_s(double &s0, double &s1) const
+{
+
+ if (2 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s0 = s[0] ; s1 = s[1] ;
+}
+
+
+/*!
+ \brief set the value of one feature if the number of feature is equal to 1.
+
+ \param s0 : value of the visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::set_s(const double s0)
+{
+
+ if (1 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s[0] = s0 ;
+}
+
+
+/*!
+ \brief get the value of one feature if the number of feature is equal to 1.
+
+ \param s0 : value of the visual feature
+
+ \exception an exception is thrown if the number of row of the vector s
+ is different from the dimension of the visual feature as specified
+ in the constructor
+*/
+void
+vpGenericFeature::get_s(double &s0) const
+{
+
+ if (1 != dim_s)
+ {
+ vpERROR_TRACE("size mismatch between number of parameters"
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError,
+ "size mismatch between number of parameters"
+ "and feature dimension"));
+
+ }
+ s0 = s[0];
+}
+
+
+/*!
+ Print to stdout the values of the current visual feature \f$ s \f$.
+
+ \param select : Selection of a subset of the possible features.
+ - To print all the features use vpBasicFeature::FEATURE_ALL.
+ - To print only one of the component features you have to say which
+ one you want to take into account. If it is the first one set
+ select to vpBasicFeature::FEATURE_LINE[0], if it is the second one
+ set select to vpBasicFeature::FEATURE_LINE[1], and so on.
+
+ \code
+ vpGenericFeature s; // Current visual feature s
+
+ // Creation of the current feature s
+ s.set_s(0, 0, 0);
+
+ s.print(); // print all components of the feature
+ s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
+ s.print(vpBasicFeature::FEATURE_LINE[0]); // print only the first component
+ \endcode
+*/
+void
+vpGenericFeature::print(const unsigned int select) const
+{
+
+ std::cout <<"Generic Feature: " ;
+ for (unsigned int i=0 ; i < dim_s ; i++)
+ if (FEATURE_LINE[i] & select )
+ {
+ std::cout << " s["<<i << "]=" << s[i] ;
+ }
+
+ std::cout <<std::endl ;
+}
+
+vpGenericFeature *vpGenericFeature::duplicate() const
+{
+ vpGenericFeature *feature= new vpGenericFeature(dim_s) ;
+
+ vpTRACE("dims = %d",dim_s) ;
+ return feature ;
+}
+
+/*!
+ Not implemented.
+*/
+void
+vpGenericFeature::display(const vpCameraParameters &/* cam */,
+ const vpImage<unsigned char> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+/*!
+ Not implemented.
+ */
+void
+vpGenericFeature::display(const vpCameraParameters &/* cam */,
+ const vpImage<vpRGBa> &/* I */,
+ const vpColor &/* color */,
+ unsigned int /* thickness */) const
+{
+ static int firsttime =0 ;
+
+ if (firsttime==0)
+ {
+ firsttime=1 ;
+ vpERROR_TRACE("not implemented") ;
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
+}
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/visual_features/test/feature/testPoint.cpp b/modules/visual_features/test/feature/testPoint.cpp
new file mode 100644
index 0000000..960675f
--- /dev/null
+++ b/modules/visual_features/test/feature/testPoint.cpp
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Performs various tests on the point class.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+
+/*!
+ \file testPoint.cpp
+ \brief Performs various tests on the the point class.
+*/
+
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+int main()
+{
+ try {
+ vpHomogeneousMatrix cMo ;
+ cMo[0][3] = 0.1 ;
+ cMo[1][3] = 0.2 ;
+ cMo[2][3] = 2 ;
+
+ vpPoint point ;
+ vpTRACE("set point coordinates in the world frame ") ;
+ point.setWorldCoordinates(0,0,0) ;
+
+ std::cout <<"------------------------------------------------------"<<std::endl ;
+ vpTRACE("test the projection ") ;
+ point.track(cMo) ;
+
+ vpTRACE("coordinates in the world frame ") ;
+ std::cout << point.oP.t() << std::endl ;
+ vpTRACE("coordinates in the camera frame ") ;
+ std::cout << point.cP.t() << std::endl ;
+
+ vpTRACE("2D coordinates ") ;
+ std::cout<< point.get_x() << " " << point.get_y() << std::endl ;
+
+ std::cout <<"------------------------------------------------------"<<std::endl ;
+ vpTRACE("test the interaction matrix ") ;
+
+ vpFeaturePoint p ;
+ vpFeatureBuilder::create(p,point) ;
+
+ vpMatrix L ;
+ L = p.interaction() ;
+ std::cout << L << std::endl ;
+
+ vpTRACE("test the interaction matrix select") ;
+ vpTRACE("\t only X") ;
+ L = p.interaction(vpFeaturePoint::selectX()) ;
+ std::cout << L << std::endl ;
+
+ vpTRACE("\t only Y") ;
+ L = p.interaction(vpFeaturePoint::selectY()) ;
+ std::cout << L << std::endl ;
+
+ vpTRACE("\t X & Y") ;
+ L = p.interaction(vpFeaturePoint::selectX() |
+ vpFeaturePoint::selectY()) ;
+ std::cout << L << std::endl ;
+
+ vpTRACE("\t selectAll") ;
+ L = p.interaction(vpFeaturePoint::selectAll() ) ;
+ std::cout << L << std::endl ;
+
+ std::cout <<"------------------------------------------------------"<<std::endl ;
+ vpTRACE("test the error ") ;
+
+ try{
+ vpFeaturePoint pd ;
+ pd.set_x(0) ;
+ pd.set_y(0) ;
+
+ pd.print() ; std::cout << std::endl ;
+ vpColVector e ;
+ e = p.error(pd) ;
+ std::cout << e << std::endl ;
+
+ vpTRACE("test the interaction matrix select") ;
+ vpTRACE("\t only X") ;
+ e = p.error(pd,vpFeaturePoint::selectX()) ;
+ std::cout << e << std::endl ;
+
+ vpTRACE("\t only Y") ;
+ e = p.error(pd,vpFeaturePoint::selectY()) ;
+ std::cout << e << std::endl ;
+
+ vpTRACE("\t X & Y") ;
+ e = p.error(pd,vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
+ std::cout << e << std::endl ;
+
+ vpTRACE("\t selectAll") ;
+ e = p.error(pd,vpFeaturePoint::selectAll() ) ;
+ std::cout << e << std::endl ;
+ }
+ catch(vpFeatureException me){ std::cout << me << std::endl ; }
+ catch(vpException me){ std::cout << me << std::endl ; }
+ std::cout <<"------------------------------------------------------"<<std::endl ;
+ vpTRACE("test the dimension") ;
+ unsigned int dim ;
+ dim = p.getDimension() ;
+ std::cout << "Dimension = " << dim << std::endl ;
+
+ vpTRACE("test the dimension with select") ;
+ vpTRACE("\t only X") ;
+ dim = p.getDimension(vpFeaturePoint::selectX()) ;
+ std::cout << "Dimension = " << dim << std::endl ;
+
+ vpTRACE("\t only Y") ;
+ dim = p.getDimension(vpFeaturePoint::selectY()) ;
+ std::cout << "Dimension = " << dim << std::endl ;
+
+ vpTRACE("\t X & Y") ;
+ dim = p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
+ std::cout << "Dimension = " << dim << std::endl ;
+
+ vpTRACE("\t selectAll") ;
+ dim = p.getDimension(vpFeaturePoint::selectAll() ) ;
+ std::cout << "Dimension = " << dim << std::endl ;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vs/CMakeLists.txt b/modules/vs/CMakeLists.txt
new file mode 100644
index 0000000..65d0fb7
--- /dev/null
+++ b/modules/vs/CMakeLists.txt
@@ -0,0 +1,43 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# ("GPL") version 2 as published by the Free Software Foundation.
+# 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 http://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 at 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:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+# visp_robot is optional to run testFeatureSegment.cpp
+vp_add_module(vs visp_core visp_visual_features OPTIONAL visp_robot visp_io)
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+vp_add_tests(DEPENDS_ON visp_blob visp_io visp_gui)
diff --git a/modules/vs/include/visp3/vs/vpAdaptiveGain.h b/modules/vs/include/visp3/vs/vpAdaptiveGain.h
new file mode 100644
index 0000000..9d00ec2
--- /dev/null
+++ b/modules/vs/include/visp3/vs/vpAdaptiveGain.h
@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Adaptive gain.
+ *
+ * Authors:
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+/*!
+\file vpAdaptiveGain.h
+\brief Adaptive gain
+*/
+
+#ifndef __VP_ADAPTIVE_GAIN_H
+#define __VP_ADAPTIVE_GAIN_H
+
+#include <visp3/core/vpConfig.h>
+#include <iostream>
+
+class vpColVector;
+/*!
+ \class vpAdaptiveGain
+
+ \ingroup group_task
+
+ \brief Adaptive gain computation.
+
+ The formula used to compute the gain is the following :
+
+ \f[ \lambda (x) = a * exp (-b*x) + c \f]
+
+ where \f$ a \f$, \f$ b \f$ and \f$ c \f$ are constant parameters and \f$ x \f$ is the entry to consider.
+
+ The parameters \f$a,b,c\f$ are not set directly. They are computed from three other parameters
+ \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ that are more intuitive to tune:
+ \f[ a = \lambda(0) - \lambda(\infty) \f]
+ \f[ b = {\dot \lambda}(0) / a \f]
+ \f[ c = \lambda(\infty) \f]
+
+ where \f$ \lambda(0)\f$ represents the gain when \f$x=0\f$, \f$ \lambda(\infty)\f$ represents the gain when \f$x=\infty\f$
+ and \f$ {\dot \lambda}(0)\f$ represents the slope of \f$\lambda(x)\f$ when \f$x=0\f$.
+
+*/
+
+class VISP_EXPORT vpAdaptiveGain
+{
+
+public: /* constantes */
+
+ static const double DEFAULT_LAMBDA_ZERO;
+ static const double DEFAULT_LAMBDA_INFINITY;
+ static const double DEFAULT_LAMBDA_SLOPE;
+
+
+private: /* Attributs*/
+ /* Coefficient de la fonction de calcul de lambda.
+ * lambda (x) = a * exp (-b*x) + c. */
+ double coeff_a;
+ double coeff_b;
+ double coeff_c;
+
+ /* Derniere valeur calculee. */
+ mutable double lambda;
+
+
+
+public: /* Methodes*/
+
+ /* --- CONSTRUCTOR -------------------------------------------------------- */
+
+ vpAdaptiveGain ();
+ vpAdaptiveGain (double c);
+ vpAdaptiveGain (double gain_at_zero,
+ double gain_at_infinity,
+ double slope_at_zero);
+
+
+ /* --- INIT --------------------------------------------------------------- */
+ void initFromConstant (double c);
+ void initFromVoid (void);
+ void initStandard (double gain_at_zero,
+ double gain_at_infinity,
+ double slope_at_zero);
+
+
+ /* --- MODIFIORS ---------------------------------------------------------- */
+ double setConstant (void);
+
+
+ /* --- COMPUTE ------------------------------------------------------------ */
+ /* \brief Calcule la valeur de lambda au point courrant.
+ *
+ * Determine la valeur du lambda adaptatif en fonction de la valeur
+ * de la norme de la fonction de tache e par extrapolation exponentielle.
+ * La fonction est : (en_infini - en_zero) * exp (-pente * ||e|| ) + en_infini.
+ * On a bien :
+ * - lambda(10^5) = en_infini ;
+ * - lambda(0) = en_zero ;
+ * - lambda(x ~ 0) ~ - pente * x + en_zero.
+ * \param val_e: valeur de la norme de l'erreur.
+ * \return: valeur de gain au point courrant.
+ */
+ double value_const (double x) const;
+
+ /* \brief Calcule la valeur de lambda au point courrant et stockage du
+ * resultat.
+ *
+ * La fonction calcule la valeur de lambda d'apres la valeur de la norme
+ * de l'erreur, comme le fait la fonction valeur_const.
+ * La fonction non constante stocke de plus le resultat dans this ->lambda.
+ * \param val_e: valeur de la norme de l'erreur.
+ * \return: valeur de gain au point courrant.
+ */
+ double value (double x) const;
+
+ double limitValue_const (void) const;
+
+ double limitValue (void) const;
+
+ /* --- ACCESSORS ---------------------------------------------------------- */
+
+ /*!
+ Gets the last adaptive gain value which was stored in the class.
+
+ \return It returns the last adaptive gain value which was stored in the class.
+ */
+ inline double getLastValue (void) const {return this ->lambda;}
+
+ double operator() (double x) const;
+
+ /* \brief Lance la fonction valeur avec la norme INFINIE du vecteur. */
+ double operator() (const vpColVector & x) const;
+
+ /* \brief Idem function limitValue. */
+ double operator() (void) const;
+
+
+ /* --- IOSTREAM ----------------------------------------------------------- */
+
+ friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda);
+};
+
+#endif /* __VP_ADAPTIVE_GAIN_H */
diff --git a/modules/vs/include/visp3/vs/vpServo.h b/modules/vs/include/visp3/vs/vpServo.h
new file mode 100644
index 0000000..4a37bf3
--- /dev/null
+++ b/modules/vs/include/visp3/vs/vpServo.h
@@ -0,0 +1,612 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual servoing control law.
+ *
+ * Authors:
+ * Eric Marchand
+ * Nicolas Mansard
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#ifndef vpServo_H
+#define vpServo_H
+
+/*!
+ \file vpServo.h
+ \brief Class required to compute the visual servoing control law
+*/
+
+#include <list>
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/vs/vpServoException.h>
+#include <visp3/vs/vpAdaptiveGain.h>
+
+
+/*!
+ \class vpServo
+
+ \ingroup group_task
+ Class required to compute the visual servoing control law descbribed
+ in \cite Chaumette06a and \cite Chaumette07a.
+
+ \warning To avoid potential memory leaks, it is mendatory to call
+ explicitly the kill() function to destroy the task. Otherwise, the
+ destructor ~vpServo() launch an exception
+ vpServoException::notKilledProperly.
+
+ To learn how to use this class, we suggest first to follow the \ref tutorial-ibvs.
+ The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs are also useful
+ for advanced usage of this class.
+
+ The example below shows how to build a position-based visual servo
+ from 3D visual features \f$s=({^{c^*}}t_c,\theta u)\f$. In that
+ case, we have \f$s^* = 0\f$. Let us denote \f$\theta u\f$ the angle/axis
+ parametrization of the rotation \f${^{c^*}}R_c\f$. Moreover,\f$
+ {^{c^*}}t_c\f$ and \f${^{c^*}}R_c\f$ represent respectively the
+ translation and the rotation between the desired camera frame and
+ the current one obtained by pose estimation (see vpPose class).
+
+ \code
+#include <visp3/core/vpColVector.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/vs/vpServo.h>
+
+int main()
+{
+ // Creation of an homogeneous matrix that represent the displacement
+ // the camera has to achieve to move from the desired camera frame
+ // and the current one
+ vpHomogeneousMatrix cdMc;
+
+ // ... cdMc is here the result of a pose estimation
+
+ // Creation of the current visual feature s = (c*_t_c, ThetaU)
+ vpFeatureTranslation s_t(vpFeatureTranslation::cdMc);
+ vpFeatureThetaU s_tu(vpFeatureThetaU::cdRc);
+ // Set the initial values of the current visual feature s = (c*_t_c, ThetaU)
+ s_t.buildFrom(cdMc);
+ s_tu.buildFrom(cdMc);
+
+ // Build the desired visual feature s* = (0,0)
+ vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // Default initialization to zero
+ vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc);// Default initialization to zero
+
+ vpColVector v; // Camera velocity
+ double error; // Task error
+
+ // Creation of the visual servo task.
+ vpServo task;
+
+ // Visual servo task initialization
+ // - Camera is monted on the robot end-effector and velocities are
+ // computed in the camera frame
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // - Interaction matrix is computed with the current visual features s
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ // - Set the contant gain to 1
+ task.setLambda(1);
+ // - Add current and desired translation feature
+ task.addFeature(s_t, s_star_t);
+ // - Add current and desired ThetaU feature for the rotation
+ task.addFeature(s_tu, s_star_tu);
+
+ // Visual servoing loop. The objective is here to update the visual
+ // features s = (c*_t_c, ThetaU), compute the control law and apply
+ // it to the robot
+ do {
+ // ... cdMc is here the result of a pose estimation
+
+ // Update the current visual feature s
+ s_t.buildFrom(cdMc); // Update translation visual feature
+ s_tu.buildFrom(cdMc); // Update ThetaU visual feature
+
+ v = task.computeControlLaw(); // Compute camera velocity skew
+ error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2
+ } while (error > 0.0001); // Stop the task when current and desired visual features are close
+
+ // A call to kill() is requested here to destroy properly the current
+ // and desired feature lists.
+ task.kill();
+}
+ \endcode
+
+*/
+
+class VISP_EXPORT vpServo
+{
+ /*
+ Choice of the visual servoing control law
+ */
+public:
+ typedef enum
+ {
+ NONE,
+ /*!< No control law is specified. */
+ EYEINHAND_CAMERA,
+ /*!< Eye in hand visual servoing with the following control law
+ \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f]
+ where camera velocities are computed. */
+ EYEINHAND_L_cVe_eJe,
+ /*!< Eye in hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
+ where joint velocities are computed. */
+ EYETOHAND_L_cVe_eJe,
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
+ where joint velocities are computed. */
+ EYETOHAND_L_cVf_fVe_eJe,
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
+ where joint velocities are computed. */
+ EYETOHAND_L_cVf_fJe
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf J}_e} \right)^{+} {\bf e}\f]
+ where joint velocities are computed. */
+ } vpServoType;
+
+ typedef enum
+ {
+ CURRENT,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_s \f$computed using the current features \f$\bf s\f$. */
+ DESIRED,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired features \f${\bf s}^*\f$. */
+ MEAN,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2 \f$. */
+ USER_DEFINED
+ /*!< In the control law (see vpServo::vpServoType), uses an interaction matrix set by the user. */
+ } vpServoIteractionMatrixType;
+
+ typedef enum
+ {
+ TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the transpose instead of the pseudo inverse. */
+ PSEUDO_INVERSE /*!< In the control law (see vpServo::vpServoType), uses the pseudo inverse. */
+ } vpServoInversionType;
+
+ typedef enum
+ {
+ ALL, /*!< Print all the task information. */
+ CONTROLLER, /*!< Print the type of controller law. */
+ ERROR_VECTOR, /*!< Print the error vector \f$\bf e = (s-s^*)\f$. */
+ FEATURE_CURRENT, /*!< Print the current features \f$\bf s\f$. */
+ FEATURE_DESIRED, /*!< Print the desired features \f${\bf s}^*\f$. */
+ GAIN, /*!< Print the gain \f$\lambda\f$. */
+ INTERACTION_MATRIX, /*!< Print the interaction matrix. */
+ MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */
+ } vpServoPrintType;
+
+//private:
+//#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// vpServo(const vpServo &)
+// : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
+// rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
+// interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
+// cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
+// errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
+// forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
+// {
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// }
+// vpServo &operator=(const vpServo &){
+// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+// return *this;
+// }
+//#endif
+
+public:
+ // default constructor
+ vpServo();
+ // constructor with Choice of the visual servoing control law
+ vpServo(vpServoType servoType) ;
+ // destructor
+ virtual ~vpServo() ;
+
+ // create a new ste of two visual features
+ void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
+ const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
+ // create a new ste of two visual features
+ void addFeature(vpBasicFeature& s,
+ const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
+
+ // compute the desired control law
+ vpColVector computeControlLaw() ;
+ // compute the desired control law
+ vpColVector computeControlLaw(double t) ;
+ vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
+
+ // compute the error between the current set of visual features and
+ // the desired set of visual features
+ vpColVector computeError() ;
+ // compute the interaction matrix related to the set of visual features
+ vpMatrix computeInteractionMatrix() ;
+
+ // Return the task dimension.
+ unsigned int getDimension() const ;
+ /*!
+ Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual features
+ \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
+ The error vector is updated after a call of computeError() or computeControlLaw().
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpColVector e = task.getError(); // Get the error vector
+\endcode
+ */
+ inline vpColVector getError() const
+ {
+ return error ;
+ }
+ /*
+ Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
+ The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
+
+ \code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
+ \endcode
+ \sa getTaskJacobian()
+ */
+ inline vpMatrix getInteractionMatrix() const
+ {
+ return L;
+ }
+
+ vpMatrix getI_WpW() const;
+ /*!
+ Return the visual servo type.
+ */
+ inline vpServoType getServoType() const
+ {
+ return servoType;
+ }
+
+ vpMatrix getLargeP() const;
+
+ vpMatrix getTaskJacobian() const;
+ vpMatrix getTaskJacobianPseudoInverse() const;
+ unsigned int getTaskRank() const;
+
+ /*!
+ Get task singular values.
+
+ \return Singular values that relies on the task jacobian pseudo inverse.
+ */
+ inline vpColVector getTaskSingularValues() const
+ {
+ return sv;
+ }
+
+ vpMatrix getWpW() const;
+
+ /*!
+ Return the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ */
+ vpVelocityTwistMatrix get_cVe() const { return cVe; }
+ /*!
+ Return the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ */
+ vpVelocityTwistMatrix get_cVf() const { return cVf; }
+ /*!
+ Return the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ */
+ vpVelocityTwistMatrix get_fVe() const { return fVe; }
+ /*!
+ Return the robot jacobian expressed in the end-effector frame.
+ */
+ vpMatrix get_eJe() const { return eJe; }
+ /*!
+ Return the robot jacobian expressed in the robot fixed frame (also called world or base frame).
+ */
+ vpMatrix get_fJe() const { return fJe; }
+
+ // destruction (memory deallocation if required)
+ void kill() ;
+
+ void print(const vpServo::vpServoPrintType display_level=ALL,
+ std::ostream &os = std::cout) ;
+
+ // Add a secondary task.
+ vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false) ;
+ // Add a secondary task.
+ vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt, const bool &useLargeProjectionOperator = false) ;
+ // Add a secondary task to avoid the joint limit.
+ vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector & jointMin,
+ const vpColVector & jointMax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7) const;
+
+
+ /*!
+ Set a variable which enables to compute the interaction matrix at each iteration.
+
+ When the interaction matrix is computed from the desired features \f${\bf s}^*\f$ which are in general constant,
+ the interaction matrix \f${\widehat {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo loop.
+ Sometimes, when the desired features are time dependent \f${{\bf s}(t)}^*\f$ or varying, the interaction matrix
+ need to be computed at each iteration of the servo loop. This method allows to force the computation of
+ \f${\widehat {\bf L}}\f$ in this particular case.
+
+ \param force_computation: If true it forces the interaction matrix computation even if it is already done.
+
+ */
+ void setForceInteractionMatrixComputation(bool force_computation)
+ {
+ this->forceInteractionMatrixComputation = force_computation;
+ }
+
+ /*!
+ Set the interaction matrix type (current, desired, mean or user defined) and how its inverse is computed.
+ \param interactionMatrixType : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for
+ more details.
+ \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for
+ more details.
+ */
+ void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
+ const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
+
+ /*!
+ Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as constant.
+
+ The usage of an adaptive gain allows to reduce the convergence time, see setLambda(const vpAdaptiveGain&).
+
+ \param c : Constant gain. Values are in general between 0.1 and 1. Higher is the gain, higher are the velocities
+ that may be applied to the robot.
+ */
+ void setLambda(double c)
+ {
+ lambda .initFromConstant (c) ;
+ }
+
+ /*!
+ Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
+ Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
+
+ The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
+
+ \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
+ \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
+ \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+
+ For more details on these parameters see vpAdaptiveGain class.
+ */
+ void setLambda(const double gain_at_zero,
+ const double gain_at_infinity,
+ const double slope_at_zero)
+ {
+ lambda .initStandard (gain_at_zero, gain_at_infinity, slope_at_zero) ;
+ }
+ /*!
+ Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
+ Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
+
+ The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
+ \sa vpAdaptiveGain
+ */
+ void setLambda(const vpAdaptiveGain& l){lambda=l;}
+ /*!
+ Set the value of the parameter \f$\mu\f$ used to ensure the continuity of the velocities
+ computed using computeControlLaw(double).
+
+ A recommended value is 4.
+ */
+ void setMu(double mu_){this->mu=mu_;}
+ // Choice of the visual servoing control law
+ void setServo(const vpServoType &servo_type) ;
+
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ */
+ void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_ ; init_cVe = true ; }
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ */
+ void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ */
+ void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_ ; init_cVf = true ; }
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ */
+ void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ */
+ void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_ ; init_fVe = true ; }
+ /*!
+ Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ */
+ void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
+
+ /*!
+ Set the robot jacobian expressed in the end-effector frame.
+ */
+ void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_ ; init_eJe = true ; }
+ /*!
+ Set the robot jacobian expressed in the robot fixed frame (also called world or base frame).
+ */
+ void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_ ; init_fJe = true ; }
+
+ /*!
+ Test if all the initialization are correct. If true, the control law can be computed.
+ */
+ bool testInitialization() ;
+ /*!
+ Test if all the update are correct. If true control law can be computed.
+ */
+ bool testUpdated() ;
+
+ protected:
+ //! Basic initialization.
+ void init() ;
+
+ /*!
+ Compute the classic projetion operator and the large projection operator.
+ */
+ void computeProjectionOperators();
+
+ public:
+ //! Interaction matrix
+ vpMatrix L ;
+ //! Error \f$(s - s^*)\f$ between the current set of visual features
+ //! \f$s\f$ and the desired set of visual features \f$s^*\f$.
+ //! This vector is updated after a call of computeError() or computeControlLaw().
+ vpColVector error ;
+ //! Task Jacobian \f$J_1 = L {^c}V_a {^a}J_e\f$.
+ vpMatrix J1 ;
+ //! Pseudo inverse \f${J_1}^{+}\f$ of the task Jacobian.
+ vpMatrix J1p ;
+
+ //! Current state of visual features \f$s\f$.
+ //! This vector is updated after a call of computeError() or computeControlLaw().
+ vpColVector s ;
+ //! Desired state of visual features \f$s^*\f$.
+ //! This vector is updated after a call of computeError() or computeControlLaw().
+ vpColVector sStar ;
+
+ //! Primary task \f$e_1 = {J_1}^{+}(s-s*)\f$
+ vpColVector e1 ;
+ //! Task \f$e = e_1 + (I-{J_1}^{+} J_1) e_2\f$
+ vpColVector e ;
+
+ //! Articular velocity
+ vpColVector q_dot ;
+ //! Camera velocity
+ vpColVector v ;
+
+ //! Chosen visual servoing control law
+ vpServoType servoType;
+
+ //! Rank of the task Jacobian
+ unsigned int rankJ1 ;
+
+ //! List of current visual features \f$\bf s\f$.
+ std::list<vpBasicFeature *> featureList ;
+ //! List of desired visual features \f$\bf s^*\f$.
+ std::list<vpBasicFeature *> desiredFeatureList ;
+ //! List of selection among visual features
+ //! used for selection of a subset of each visual feature if required.
+ std::list<unsigned int> featureSelectionList ;
+
+ //! Gain used in the control law.
+ vpAdaptiveGain lambda ;
+
+ //! Sign of the interaction +/- 1 (1 for eye-in-hand, -1 for
+ //! eye-to-hand configuration)
+ int signInteractionMatrix ;
+ //! Type of the interaction matrox (current, mean, desired, user)
+ vpServoIteractionMatrixType interactionMatrixType ;
+ //! Indicates if the transpose or the pseudo inverse of the
+ //! interaction matrix should be used to compute the task.
+ vpServoInversionType inversionType ;
+
+ protected:
+ /*
+ Twist transformation matrix
+ */
+
+ //! Twist transformation matrix between Re and Rc.
+ vpVelocityTwistMatrix cVe ;
+ bool init_cVe ;
+ //! Twist transformation matrix between Rf and Rc.
+ vpVelocityTwistMatrix cVf ;
+ bool init_cVf ;
+ //! Twist transformation matrix between Re and Rf.
+ vpVelocityTwistMatrix fVe ;
+ bool init_fVe ;
+
+ /*
+ Jacobians
+ */
+
+ //! Jacobian expressed in the end-effector frame.
+ vpMatrix eJe ;
+ bool init_eJe ;
+ //! Jacobian expressed in the robot reference frame.
+ vpMatrix fJe ;
+ bool init_fJe ;
+
+ /*
+ Task building
+ */
+
+ //! true if the error has been computed.
+ bool errorComputed ;
+ //! true if the interaction matrix has been computed.
+ bool interactionMatrixComputed ;
+ //! Dimension of the task updated during computeControlLaw().
+ unsigned int dim_task ;
+ //! Flag to indicate if the task was killed
+ bool taskWasKilled;
+ //! Force the interaction matrix computation even if it is already done.
+ bool forceInteractionMatrixComputation;
+
+ //! Projection operators \f$\bf WpW\f$.
+ vpMatrix WpW ;
+ //! Projection operators \f$\bf I-WpW\f$.
+ vpMatrix I_WpW ;
+ /*!
+ New Large projection operator (see equation(24) in the paper \cite Marey:2010). This projection operator allows
+ performing secondary task even when the main task is full rank.
+ \f[
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left
+ ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
+ \f]
+
+ with
+
+ \f[
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }
+ {\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
+ \f]
+
+ */
+ vpMatrix P;
+
+
+
+ //! Singular values from the pseudo inverse.
+ vpColVector sv ;
+
+ double mu;
+
+ vpColVector e1_initial;
+
+} ;
+
+#endif
diff --git a/modules/vs/include/visp3/vs/vpServoData.h b/modules/vs/include/visp3/vs/vpServoData.h
new file mode 100644
index 0000000..f92bae7
--- /dev/null
+++ b/modules/vs/include/visp3/vs/vpServoData.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Save data during the task execution.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpServoData_H
+#define vpServoData_H
+
+/*!
+ \file vpServoData.h
+ \brief save data during the task execution
+*/
+
+
+
+// Servo
+#include <visp3/vs/vpServo.h>
+
+#include <iostream>
+
+/*!
+ \class vpServoData
+ \ingroup group_task
+ \brief Save data during the task execution.
+*/
+class VISP_EXPORT vpServoData
+{
+
+private:
+ char baseDirectory[FILENAME_MAX] ;
+
+ std::ofstream velocityFile ;
+ std::ofstream errorFile ;
+ std::ofstream errorNormFile ;
+ std::ofstream sFile ;
+ std::ofstream sStarFile ;
+ std::ofstream vNormFile ;
+
+ //! flag to known if velocity should be output in cm and degrees (true)
+ //! or in m/rad
+ bool cmDeg ;
+
+
+
+public:
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ vpServoData(const vpServoData &sd)
+ : velocityFile(), errorFile(), errorNormFile(), sFile(),
+ sStarFile(), vNormFile(), cmDeg(false) {
+ *this = sd;
+ }
+ vpServoData & operator=(const vpServoData &) {
+ throw vpException(vpException::functionNotImplementedError,"Not implemented!");
+ }
+#endif
+
+ vpServoData() : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), vNormFile(), cmDeg(false) { ; }
+ virtual ~vpServoData() { ; }
+
+ //! velocity output in cm and deg
+ void setCmDeg() ;
+ //! velocity output in meter and deg (default)
+ void setMeterRad() ;
+
+ void save(const vpServo &task) ;
+ void open(const char *baseDirectory) ;
+ void close() ;
+
+ void empty() ;
+ void push() ;
+ void display(vpImage<unsigned char> &I) ;
+
+} ;
+
+
+
+#endif
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vs/include/visp3/vs/vpServoDisplay.h b/modules/vs/include/visp3/vs/vpServoDisplay.h
new file mode 100644
index 0000000..0af2ec6
--- /dev/null
+++ b/modules/vs/include/visp3/vs/vpServoDisplay.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef vpServoDisplay_H
+#define vpServoDisplay_H
+
+/*!
+ \file vpServoDisplay.h
+ \brief interface with the image for feature display
+*/
+
+// Servo
+#include <visp3/vs/vpServo.h>
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+
+//Color / image / display
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
+/*!
+ \class vpServoDisplay
+ \ingroup group_task
+ \brief Interface with the image for feature display.
+*/
+class VISP_EXPORT vpServoDisplay
+{
+public:
+ static void display(const vpServo &s,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ vpColor currentColor = vpColor::green,
+ vpColor desiredColor = vpColor::red,
+ unsigned int thickness=1) ;
+ static void display(const vpServo &s,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ vpColor currentColor = vpColor::green,
+ vpColor desiredColor = vpColor::red,
+ unsigned int thickness=1) ;
+} ;
+
+#endif
+
diff --git a/modules/vs/include/visp3/vs/vpServoException.h b/modules/vs/include/visp3/vs/vpServoException.h
new file mode 100644
index 0000000..258b645
--- /dev/null
+++ b/modules/vs/include/visp3/vs/vpServoException.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Exception that can be emited by the vpServo class and its derivates.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+#ifndef __vpServoException_H
+#define __vpServoException_H
+
+/*!
+ \file vpServoException.h
+ \brief error that can be emited by the vpServo class and its derivates
+*/
+
+#include <visp3/core/vpException.h>
+
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
+
+/* ------------------------------------------------------------------------- */
+/* --- CLASS --------------------------------------------------------------- */
+/* ------------------------------------------------------------------------- */
+
+/*!
+ \class vpServoException
+ \brief Error that can be emited by the vpServo class and its derivates.
+ \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
+ */
+class VISP_EXPORT vpServoException : public vpException
+{
+ public:
+ /*!
+
+ \brief Lists the possible error than can be emmited 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,
+ //! Task was not killed properly
+ notKilledProperly,
+ //! Other exception
+ servoError
+ } ;
+
+ public:
+ vpServoException (const int id, const char* format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end (args);
+ }
+ vpServoException (const int id, const std::string & msg)
+ : vpException(id, msg){ ; }
+ vpServoException (const int id)
+ : vpException(id){ ; }
+
+};
+
+#endif
diff --git a/modules/vs/src/vpAdaptiveGain.cpp b/modules/vs/src/vpAdaptiveGain.cpp
new file mode 100644
index 0000000..68324d7
--- /dev/null
+++ b/modules/vs/src/vpAdaptiveGain.cpp
@@ -0,0 +1,362 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Adaptive gain.
+ *
+ * Authors:
+ * Nicolas Mansard
+ *
+ *****************************************************************************/
+/*!
+\file vpAdaptiveGain.cpp
+*/
+
+/* --- VISP --- */
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/vs/vpAdaptiveGain.h>
+
+#include <iostream>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+
+const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
+const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
+const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
+
+/* -------------------------------------------------------------------------- */
+/* --- CONSTRUCTION --------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ Basic constructor which initializes all the parameters with their default value:
+ - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
+ - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
+ - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
+
+*/
+vpAdaptiveGain::vpAdaptiveGain ()
+ :
+ coeff_a (),
+ coeff_b (),
+ coeff_c (),
+ lambda(1.)
+{
+ vpDEBUG_TRACE (10, "# Entree constructeur par default.");
+ this ->initFromVoid ();
+
+ vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
+ return;
+}
+
+/*!
+ Constructor that initializes the gain as constant. In that case \f$\lambda(x) = c\f$.
+
+ \param c : Value of the constant gain.
+*/
+vpAdaptiveGain::vpAdaptiveGain (double c)
+ :
+ coeff_a (),
+ coeff_b (),
+ coeff_c (),
+ lambda(1.)
+{
+ initFromConstant(c);
+}
+
+/*!
+ Constructor that initializes the gain as adaptive.
+
+ \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
+ \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
+ \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+
+*/
+vpAdaptiveGain::vpAdaptiveGain (double gain_at_zero, double gain_at_infinity, double slope_at_zero)
+ :
+ coeff_a (),
+ coeff_b (),
+ coeff_c (),
+ lambda(1.)
+{
+ initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
+}
+
+/* -------------------------------------------------------------------------- */
+/* --- INIT ----------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ Initializes the parameters to have a constant gain. In that case \f$\lambda(x) = c\f$.
+
+ \param c : Value of the constant gain.
+*/
+void vpAdaptiveGain::initFromConstant (const double c)
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ this ->coeff_a = 0;
+ this ->coeff_b = 1;
+ this ->coeff_c = c;
+
+ vpDEBUG_TRACE (10, "# Sortie.");
+ return;
+}
+
+
+/*!
+ Initializes the parameters with the default value :
+ - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
+ - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
+ - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
+*/
+void vpAdaptiveGain::initFromVoid (void)
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
+ vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
+ vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);
+
+ vpDEBUG_TRACE (10, "# Sortie.");
+ return;
+}
+
+
+/*!
+ Set the parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ used to compute \f$\lambda(x)\f$.
+
+ \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
+ \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
+ \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+*/
+void vpAdaptiveGain::initStandard (const double gain_at_zero,
+ const double gain_at_infinity,
+ const double slope_at_zero)
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ this ->coeff_a = gain_at_zero - gain_at_infinity;
+ //if (0 == this ->coeff_a)
+ if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
+ {
+ this ->coeff_b = 0;
+ }
+ else
+ {
+ this ->coeff_b = slope_at_zero / ( this ->coeff_a);
+ }
+ this ->coeff_c = gain_at_infinity;
+
+ vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
+ this ->coeff_a, this ->coeff_b, this ->coeff_c);
+ return;
+}
+
+
+
+/* -------------------------------------------------------------------------- */
+/* --- MODIFICATOR ---------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ Sets the internal parameters \f$a,b,c\f$ in order to obtain a constant gain equal to
+ the gain in 0 set through the parameter \f$\lambda(0)\f$.
+
+ \return It returns the value of the constant gain \f$\lambda(0)\f$.
+*/
+double vpAdaptiveGain::setConstant (void)
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ double res = this ->coeff_a + this ->coeff_c;
+
+ this ->coeff_a = 0;
+ this ->coeff_b = 1;
+ this ->coeff_c = res;
+
+ vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
+ return res;
+}
+
+/* -------------------------------------------------------------------------- */
+/* --- VALEUR --------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+/*!
+ Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
+
+ \f[\lambda(x) = a * exp(-b*x) + c\f]
+
+ \param x : Input value to consider. During a visual servo this value can be the euclidian
+ norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
+
+ \return It returns the value of the computed gain.
+*/
+double vpAdaptiveGain::value_const (const double x) const
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ double res = this ->coeff_a * exp (- this ->coeff_b * x) + this ->coeff_c;
+
+ vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
+ return res;
+}
+
+/*!
+ Gets the value of the gain at infinity (ie the value of \f$ \lambda(\infty) = c \f$).
+ Similar to limitValue() except that here the value is not stored as a parameter of the class.
+
+ \return It returns the value of the gain at infinity.
+ */
+double vpAdaptiveGain::limitValue_const (void) const
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ double res = this ->coeff_c;
+
+ vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
+ return res;
+}
+
+/*!
+ Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
+
+ \f[\lambda(x) = a * exp(-b*x) + c\f]
+
+ This value is stored as a parameter of the class.
+
+ \param x : Input value to consider. During a visual servo this value can be the euclidian
+ norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
+
+ \return It returns the value of the computed gain.
+ */
+double vpAdaptiveGain::value (const double x) const
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ this ->lambda = this ->value_const (x);
+
+ vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
+ return lambda;
+}
+
+
+/*!
+ Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$) and stores it
+ as a parameter of the class.
+
+ \return It returns the value of the gain at infinity.
+ */
+double vpAdaptiveGain:: limitValue (void) const
+{
+ vpDEBUG_TRACE (10, "# Entree.");
+
+ this ->lambda = this ->limitValue_const ();
+
+ vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
+ return lambda;
+}
+
+/* -------------------------------------------------------------------------- */
+/* --- ACCESSORS ------------------------------------------------------------ */
+/* -------------------------------------------------------------------------- */
+
+
+
+// double vpAdaptiveGain::
+// getLastValue (void) const
+// {
+// return this ->lambda;
+// }
+
+/*!
+ Operator that computes \f$\lambda(x)\f$.
+
+ \param x : Input value to consider. During a visual servo this value can be the euclidian
+ norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
+
+ \return It returns the value of the computed gain.
+
+ \sa value()
+*/
+double vpAdaptiveGain::operator() (const double x) const
+{
+ return this ->value (x);
+}
+
+/*!
+ Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$).
+
+ \return It returns the value of the gain at infinity.
+
+ \sa limitValue()
+ */
+double vpAdaptiveGain::operator() (void) const
+{
+ return this ->limitValue ();
+}
+
+/*!
+ Operator which computes \f$\lambda({||x||}_{\infty})\f$.
+
+ \param x : Input vector to consider.
+
+ \return It returns the value of the computed gain.
+*/
+double vpAdaptiveGain::operator() (const vpColVector & x) const
+{
+ return this ->value (x .infinityNorm());
+}
+
+
+// double operator() (double val_e) const;
+// double operator() (const CColVector & e) const;
+
+/* -------------------------------------------------------------------------- */
+/* --- OUTPUT --------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+
+
+/*!
+ Prints the adaptive gain parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$.
+
+ \param os : The stream where to print the adaptive gain parameters.
+ \param lambda : The adaptive gain containing the parameters to print.
+*/
+VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda)
+{
+ os << "Zero= " << lambda .coeff_a + lambda .coeff_c
+ << "\tInf= " << lambda .coeff_c
+ << "\tDeriv= " << lambda .coeff_a * lambda .coeff_b;
+
+ return os;
+}
diff --git a/modules/vs/src/vpServo.cpp b/modules/vs/src/vpServo.cpp
new file mode 100644
index 0000000..3ee40f3
--- /dev/null
+++ b/modules/vs/src/vpServo.cpp
@@ -0,0 +1,1817 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual servoing control law.
+ *
+ * Authors:
+ * Eric Marchand
+ * Nicolas Mansard
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/vs/vpServo.h>
+
+#include <sstream>
+
+// Exception
+#include <visp3/core/vpException.h>
+
+// Debug trace
+#include <visp3/core/vpDebug.h>
+
+/*!
+ \file vpServo.cpp
+ \brief Class required to compute the visual servoing control law
+*/
+
+
+/*!
+ Default constructor that initializes the following settings:
+ - No control law is specified. The user has to call setServo() to specify the control law.
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
+ features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
+ interaction matrix with the current visual features, or from the mean
+ \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
+ allows to use the transpose instead.
+
+*/
+vpServo::vpServo()
+ : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
+ rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
+ interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
+ cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
+ errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
+ forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
+{
+}
+/*!
+ Constructor that allows to choose the visual servoing control law.
+ \param servo_type : Visual servoing control law.
+
+ The other settings are the following:
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
+ features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
+ interaction matrix with the current visual features, or from the mean
+ \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
+ allows to use the transpose instead.
+
+ */
+vpServo::vpServo(vpServoType servo_type)
+ : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type),
+ rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
+ interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
+ cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
+ errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
+ forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial()
+{
+}
+
+/*!
+ Destructor.
+
+ In fact, it does nothing. You have to call kill() to destroy the
+ current and desired feature lists.
+
+ \sa kill()
+*/
+// \exception vpServoException::notKilledProperly : Task was not killed
+// properly. That means that you should explitly call kill().
+
+vpServo::~vpServo()
+{
+ if (taskWasKilled == false) {
+ vpTRACE("--- Begin Warning Warning Warning Warning Warning ---");
+ vpTRACE("--- You should explicitly call vpServo.kill()... ---");
+ vpTRACE("--- End Warning Warning Warning Warning Warning ---");
+ // throw(vpServoException(vpServoException::notKilledProperly,
+ // "Task was not killed properly"));
+ }
+}
+
+
+/*!
+ Initialize the servo with the following settings:
+
+ - No control law is specified. The user has to call setServo() to specify the control law.
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
+ features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
+ interaction matrix with the current visual features, or from the mean
+ \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
+ allows to use the transpose instead.
+
+
+*/
+void vpServo::init()
+{
+ // type of visual servoing
+ servoType = vpServo::NONE ;
+
+ // Twist transformation matrix
+ init_cVe = false ;
+ init_cVf = false ;
+ init_fVe = false ;
+ // Jacobians
+ init_eJe = false ;
+ init_fJe = false ;
+
+ dim_task = 0 ;
+
+ featureList.clear() ;
+ desiredFeatureList.clear() ;
+ featureSelectionList.clear();
+
+ signInteractionMatrix = 1 ;
+
+ interactionMatrixType = DESIRED ;
+ inversionType=PSEUDO_INVERSE ;
+
+ interactionMatrixComputed = false ;
+ errorComputed = false ;
+
+ taskWasKilled = false;
+
+ forceInteractionMatrixComputation = false;
+
+ rankJ1 = 0;
+}
+
+/*!
+ Task destruction. Kill the current and desired visual feature lists.
+
+ It is mendatory to call explicitly this function to avoid potential
+ memory leaks.
+
+ \code
+ vpServo task ;
+ vpFeatureThetaU s;
+ ...
+ task.addFeature(s); // Add current ThetaU feature
+
+ task.kill(); // A call to kill() is requested here
+ \endcode
+
+*/
+void vpServo::kill()
+{
+ if (taskWasKilled == false) {
+ // kill the current and desired feature lists
+
+ // current list
+ for(std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
+ if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
+ delete (*it) ;
+ (*it) = NULL ;
+ }
+ }
+ //desired list
+ for(std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
+ if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
+ delete (*it) ;
+ (*it) = NULL ;
+ }
+ }
+
+ featureList.clear() ;
+ desiredFeatureList.clear() ;
+ taskWasKilled = true;
+ }
+}
+
+/*!
+ Set the visual servoing control law.
+ \param servo_type : Control law that will be considered.
+ See vpServo::vpServoType to see the possible values.
+ */
+void vpServo::setServo(const vpServoType &servo_type)
+{
+ this->servoType = servo_type ;
+
+ if ((servoType==EYEINHAND_CAMERA) ||(servoType==EYEINHAND_L_cVe_eJe))
+ signInteractionMatrix = 1 ;
+ else
+ signInteractionMatrix = -1 ;
+
+ // when the control is directly compute in the camera frame
+ // we relieve the end-user to initialize cVa and aJe
+ if (servoType==EYEINHAND_CAMERA)
+ {
+ vpVelocityTwistMatrix _cVe ; set_cVe(_cVe) ;
+
+ vpMatrix _eJe ;
+ _eJe.eye(6) ;
+ set_eJe(_eJe) ;
+ };
+}
+
+
+/*!
+
+ Prints on \e os stream information about the task:
+
+ \param displayLevel : Indicates which are the task informations to print. See vpServo::vpServoPrintType for more details.
+
+ \param os : Output stream.
+*/
+void
+vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
+{
+ switch (displayLevel)
+ {
+ case vpServo::ALL:
+ {
+ os << "Visual servoing task: " <<std::endl ;
+
+ os << "Type of control law " <<std::endl ;
+ switch( servoType )
+ {
+ case NONE :
+ os << "Type of task have not been chosen yet ! " << std::endl ;
+ break ;
+ case EYEINHAND_CAMERA :
+ os << "Eye-in-hand configuration " << std::endl ;
+ os << "Control in the camera frame " << std::endl ;
+ break ;
+ case EYEINHAND_L_cVe_eJe :
+ os << "Eye-in-hand configuration " << std::endl ;
+ os << "Control in the articular frame " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVe_eJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVf_fJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
+ break ;
+ }
+
+ os << "List of visual features : s" <<std::endl ;
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<vpBasicFeature *>::const_iterator it_s_star;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
+ {
+ os << "" ;
+ (*it_s)->print( (*it_select) ) ;
+ }
+
+ os << "List of desired visual features : s*" <<std::endl ;
+ for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
+ {
+ os << "" ;
+ (*it_s_star)->print( (*it_select) ) ;
+ }
+
+ os <<"Interaction Matrix Ls "<<std::endl ;
+ if (interactionMatrixComputed)
+ {
+ os << L << std::endl;
+ }
+ else
+ {
+ os << "not yet computed "<<std::endl ;
+ }
+
+ os <<"Error vector (s-s*) "<<std::endl ;
+ if (errorComputed)
+ {
+ os << error.t() << std::endl;
+ }
+ else
+ {
+ os << "not yet computed "<<std::endl ;
+ }
+
+ os << "Gain : " << lambda <<std::endl ;
+
+ break;
+ }
+
+ case vpServo::CONTROLLER:
+ {
+ os << "Type of control law " <<std::endl ;
+ switch( servoType )
+ {
+ case NONE :
+ os << "Type of task have not been chosen yet ! " << std::endl ;
+ break ;
+ case EYEINHAND_CAMERA :
+ os << "Eye-in-hand configuration " << std::endl ;
+ os << "Control in the camera frame " << std::endl ;
+ break ;
+ case EYEINHAND_L_cVe_eJe :
+ os << "Eye-in-hand configuration " << std::endl ;
+ os << "Control in the articular frame " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVe_eJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
+ break ;
+ case EYETOHAND_L_cVf_fJe :
+ os << "Eye-to-hand configuration " << std::endl ;
+ os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
+ break ;
+ }
+ break;
+ }
+
+ case vpServo::FEATURE_CURRENT:
+ {
+ os << "List of visual features : s" <<std::endl ;
+
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
+ {
+ os << "" ;
+ (*it_s)->print( (*it_select) ) ;
+ }
+ break;
+ }
+ case vpServo::FEATURE_DESIRED:
+ {
+ os << "List of desired visual features : s*" <<std::endl ;
+
+ std::list<vpBasicFeature *>::const_iterator it_s_star;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
+ {
+ os << "" ;
+ (*it_s_star)->print( (*it_select) ) ;
+ }
+ break;
+ }
+ case vpServo::GAIN:
+ {
+ os << "Gain : " << lambda <<std::endl ;
+ break;
+ }
+ case vpServo::INTERACTION_MATRIX:
+ {
+ os <<"Interaction Matrix Ls "<<std::endl ;
+ if (interactionMatrixComputed) {
+ os << L << std::endl;
+ }
+ else {
+ os << "not yet computed "<<std::endl ;
+ }
+ break;
+ }
+
+ case vpServo::ERROR_VECTOR:
+ case vpServo::MINIMUM:
+
+ {
+ os <<"Error vector (s-s*) "<<std::endl ;
+ if (errorComputed) {
+ os << error.t() << std::endl;
+ }
+ else {
+ os << "not yet computed "<<std::endl ;
+ }
+
+ break;
+ }
+ }
+}
+
+/*!
+ Add a new set of 2 features \f$\bf s\f$ and \f${\bf s}^*\f$ in the task.
+
+ \param s_cur : Current visual feature denoted \f$\bf s\f$.
+ \param s_star : Desired visual feature denoted \f${\bf s}^*\f$.
+ \param select : Feature selector. By default all the features in \e s and \e s_star are used,
+ but is is possible to specify which one is used in case of multiple features.
+
+ The following sample code explain how to use this method to add a visual feature point \f$(x,y)\f$:
+ \code
+ vpFeaturePoint s, s_star;
+ ...
+ vpServo task;
+ task.addFeature(s, s_star);
+ \endcode
+
+ For example to use only the \f$x\f$ visual feature, the previous code becomes:
+ \code
+ vpFeaturePoint s, s_star;
+ ...
+ vpServo task;
+ task.addFeature(s, s_star, vpFeaturePoint::selectX());
+ \endcode
+
+ */
+void vpServo::addFeature(vpBasicFeature& s_cur, vpBasicFeature &s_star, unsigned int select)
+{
+ featureList.push_back( &s_cur );
+ desiredFeatureList.push_back( &s_star );
+ featureSelectionList.push_back( select );
+}
+
+/*!
+ Add a new features \f$\bf s\f$ in the task. The desired visual feature denoted \f${\bf s}^*\f$ is equal to zero.
+
+ \param s_cur : Current visual feature denoted \f$\bf s\f$.
+ \param select : Feature selector. By default all the features in \e s are used,
+ but is is possible to specify which one is used in case of multiple features.
+
+ The following sample code explain how to use this method to add a \f$\theta {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature:
+ \code
+ vpFeatureThetaU s(vpFeatureThetaU::cRcd);
+ ...
+ vpServo task;
+ task.addFeature(s);
+ \endcode
+
+ For example to use only the \f$\theta u_x\f$ feature, the previous code becomes:
+ \code
+ vpFeatureThetaU s(vpFeatureThetaU::cRcd);
+ ...
+ vpServo task;
+ task.addFeature(s, vpFeatureThetaU::selectTUx);
+ \endcode
+ */
+void vpServo::addFeature(vpBasicFeature& s_cur, unsigned int select)
+{
+ featureList.push_back( &s_cur );
+
+ // in fact we have a problem with s_star that is not defined
+ // by the end user.
+
+ // If the same user want to compute the interaction at the desired position
+ // this "virtual feature" must exist
+
+ // a solution is then to duplicate the current feature (s* = s)
+ // and reinitialized s* to 0
+
+ // it remains the deallocation issue therefore a flag that stipulates that
+ // the feature has been allocated in vpServo is set
+
+ // vpServo must deallocate the memory (see ~vpServo and kill() )
+
+ vpBasicFeature *s_star ;
+ s_star = s_cur.duplicate() ;
+
+ s_star->init() ;
+ s_star->setDeallocate(vpBasicFeature::vpServo) ;
+
+ desiredFeatureList.push_back( s_star );
+ featureSelectionList.push_back( select );
+}
+
+//! Return the task dimension.
+unsigned int vpServo::getDimension() const
+{
+ unsigned int dim = 0 ;
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
+ {
+ dim += (*it_s)->getDimension(*it_select) ;
+ }
+
+ return dim;
+}
+
+void vpServo::setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrix_type,
+ const vpServoInversionType &interactionMatrixInversion)
+{
+ this->interactionMatrixType = interactionMatrix_type ;
+ this->inversionType = interactionMatrixInversion ;
+}
+
+
+static void computeInteractionMatrixFromList (const std::list<vpBasicFeature *> & featureList,
+ const std::list<unsigned int> & featureSelectionList,
+ vpMatrix & L)
+{
+ if (featureList.empty())
+ {
+ vpERROR_TRACE("feature list empty, cannot compute Ls") ;
+ throw(vpServoException(vpServoException::noFeatureError,
+ "feature list empty, cannot compute Ls")) ;
+ }
+
+ /* The matrix dimension is not known before the affectation loop.
+ * It thus should be allocated on the flight, in the loop.
+ * The first assumption is that the size has not changed. A double
+ * reallocation (realloc(dim*2)) is done if necessary. In particular,
+ * [log_2(dim)+1] reallocations are done for the first matrix computation.
+ * If the allocated size is too large, a correction is done after the loop.
+ * The algorithmic cost is linear in affectation, logarithmic in allocation
+ * numbers and linear in allocation size.
+ */
+
+ /* First assumption: matrix dimensions have not changed. If 0, they are
+ * initialized to dim 1.*/
+ unsigned int rowL = L.getRows();
+ const unsigned int colL = 6;
+ if (0 == rowL) {
+ rowL = 1;
+ L .resize(rowL, colL);
+ }
+
+ /* vectTmp is used to store the return values of functions get_s() and
+ * error(). */
+ vpMatrix matrixTmp;
+ unsigned int rowMatrixTmp, colMatrixTmp;
+
+ /* The cursor are the number of the next case of the vector array to
+ * be affected. A memory reallocation should be done when cursor
+ * is out of the vector-array range.*/
+ unsigned int cursorL = 0;
+
+ std::list<vpBasicFeature *>::const_iterator it;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select)
+ {
+ /* Get s. */
+ matrixTmp = (*it)->interaction( *it_select );
+ rowMatrixTmp = matrixTmp .getRows();
+ colMatrixTmp = matrixTmp .getCols();
+
+ /* Check the matrix L size, and realloc if needed. */
+ while (rowMatrixTmp + cursorL > rowL) {
+ rowL *= 2;
+ L.resize (rowL,colL,false);
+ vpDEBUG_TRACE(15,"Realloc!");
+ }
+
+ /* Copy the temporarily matrix into L. */
+ for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
+ for (unsigned int j = 0; j < colMatrixTmp; ++j) {
+ L[cursorL][j] = matrixTmp[k][j];
+ }
+ }
+ }
+
+ L.resize (cursorL,colL,false);
+
+ return ;
+}
+
+
+/*!
+
+ Compute and return the interaction matrix related to the set of visual features.
+
+ \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the control law specified using setServo().
+*/
+vpMatrix vpServo::computeInteractionMatrix()
+{
+ try {
+
+ switch (interactionMatrixType)
+ {
+ case CURRENT:
+ {
+ try
+ {
+ computeInteractionMatrixFromList(this ->featureList,
+ this ->featureSelectionList,
+ L);
+ dim_task = L.getRows() ;
+ interactionMatrixComputed = true ;
+ }
+
+ catch(vpException me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+ break ;
+ case DESIRED:
+ {
+ try
+ {
+ if (interactionMatrixComputed == false || forceInteractionMatrixComputation == true)
+ {
+ computeInteractionMatrixFromList(this ->desiredFeatureList,
+ this ->featureSelectionList, L);
+
+ dim_task = L.getRows() ;
+ interactionMatrixComputed = true ;
+ }
+
+ }
+ catch(vpException me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ }
+ break ;
+ case MEAN:
+ {
+ vpMatrix Lstar (L.getRows(), L.getCols());
+ try
+ {
+ computeInteractionMatrixFromList(this ->featureList,
+ this ->featureSelectionList, L);
+ computeInteractionMatrixFromList(this ->desiredFeatureList,
+ this ->featureSelectionList, Lstar);
+ }
+ catch(vpException me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ L = (L+Lstar)/2;
+
+ dim_task = L.getRows() ;
+ interactionMatrixComputed = true ;
+ }
+ break ;
+ case USER_DEFINED:
+ // dim_task = L.getRows() ;
+ interactionMatrixComputed = false ;
+ break;
+ }
+
+ }
+ catch(vpException me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ return L ;
+}
+
+/*!
+
+ Compute the error \f$\bf e =(s - s^*)\f$ between the current set of visual
+ features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
+
+ \return The error vector \f$\bf e\f$.
+
+*/
+vpColVector vpServo::computeError()
+{
+ if (featureList.empty())
+ {
+ vpERROR_TRACE("feature list empty, cannot compute Ls") ;
+ throw(vpServoException(vpServoException::noFeatureError,
+ "feature list empty, cannot compute Ls")) ;
+ }
+ if (desiredFeatureList.empty())
+ {
+ vpERROR_TRACE("feature list empty, cannot compute Ls") ;
+ throw(vpServoException(vpServoException::noFeatureError,
+ "feature list empty, cannot compute Ls")) ;
+ }
+
+ try {
+ vpBasicFeature *current_s ;
+ vpBasicFeature *desired_s ;
+
+ /* The vector dimensions are not known before the affectation loop.
+ * They thus should be allocated on the flight, in the loop.
+ * The first assumption is that the size has not changed. A double
+ * reallocation (realloc(dim*2)) is done if necessary. In particular,
+ * [log_2(dim)+1] reallocations are done for the first error computation.
+ * If the allocated size is too large, a correction is done after the loop.
+ * The algorithmic cost is linear in affectation, logarithmic in allocation
+ * numbers and linear in allocation size.
+ * No assumptions are made concerning size of each vector: they are
+ * not said equal, and could be different.
+ */
+
+ /* First assumption: vector dimensions have not changed. If 0, they are
+ * initialized to dim 1.*/
+ unsigned int dimError = error .getRows();
+ unsigned int dimS = s .getRows();
+ unsigned int dimSStar = sStar .getRows();
+ if (0 == dimError) { dimError = 1; error .resize(dimError);}
+ if (0 == dimS) { dimS = 1; s .resize(dimS);}
+ if (0 == dimSStar) { dimSStar = 1; sStar .resize(dimSStar);}
+
+ /* vectTmp is used to store the return values of functions get_s() and
+ * error(). */
+ vpColVector vectTmp;
+ unsigned int dimVectTmp;
+
+ /* The cursor are the number of the next case of the vector array to
+ * be affected. A memory reallocation should be done when cursor
+ * is out of the vector-array range.*/
+ unsigned int cursorS = 0;
+ unsigned int cursorSStar = 0;
+ unsigned int cursorError = 0;
+
+ /* For each cell of the list, copy value of s, s_star and error. */
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<vpBasicFeature *>::const_iterator it_s_star;
+ std::list<unsigned int>::const_iterator it_select;
+
+ for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
+ it_s != featureList.end();
+ ++it_s, ++it_s_star, ++it_select)
+ {
+ current_s = (*it_s);
+ desired_s = (*it_s_star);
+ unsigned int select = (*it_select);
+
+ /* Get s, and store it in the s vector. */
+ vectTmp = current_s->get_s(select);
+ dimVectTmp = vectTmp .getRows();
+ while (dimVectTmp + cursorS > dimS)
+ { dimS *= 2; s .resize (dimS,false); vpDEBUG_TRACE(15,"Realloc!"); }
+ for (unsigned int k = 0; k < dimVectTmp; ++k) { s[cursorS++] = vectTmp[k]; }
+
+ /* Get s_star, and store it in the s vector. */
+ vectTmp = desired_s->get_s(select);
+ dimVectTmp = vectTmp .getRows();
+ while (dimVectTmp + cursorSStar > dimSStar) {
+ dimSStar *= 2;
+ sStar.resize (dimSStar,false);
+ }
+ for (unsigned int k = 0; k < dimVectTmp; ++k) {
+ sStar[cursorSStar++] = vectTmp[k];
+ }
+
+ /* Get error, and store it in the s vector. */
+ vectTmp = current_s->error(*desired_s, select) ;
+ dimVectTmp = vectTmp .getRows();
+ while (dimVectTmp + cursorError > dimError) {
+ dimError *= 2;
+ error.resize (dimError,false);
+ }
+ for (unsigned int k = 0; k < dimVectTmp; ++k) {
+ error[cursorError++] = vectTmp[k];
+ }
+ }
+
+ /* If too much memory has been allocated, realloc. */
+ s .resize(cursorS,false);
+ sStar .resize(cursorSStar,false);
+ error .resize(cursorError,false);
+
+ /* Final modifications. */
+ dim_task = error.getRows() ;
+ errorComputed = true ;
+ }
+ catch(vpException me)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+ catch(...)
+ {
+ throw ;
+ }
+ return error ;
+}
+
+bool vpServo::testInitialization()
+{
+ switch (servoType)
+ {
+ case NONE:
+ vpERROR_TRACE("No control law have been yet defined") ;
+ throw(vpServoException(vpServoException::servoError,
+ "No control law have been yet defined")) ;
+ break ;
+ case EYEINHAND_CAMERA:
+ return true ;
+ break ;
+ case EYEINHAND_L_cVe_eJe:
+ case EYETOHAND_L_cVe_eJe:
+ if (!init_cVe) vpERROR_TRACE("cVe not initialized") ;
+ if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
+ return (init_cVe && init_eJe) ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
+ if (!init_fVe) vpERROR_TRACE("fVe not initialized") ;
+ if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
+ return (init_cVf && init_fVe && init_eJe) ;
+ break ;
+
+ case EYETOHAND_L_cVf_fJe :
+ if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
+ if (!init_fJe) vpERROR_TRACE("fJe not initialized") ;
+ return (init_cVf && init_fJe) ;
+ break ;
+ }
+
+ return false ;
+}
+bool vpServo::testUpdated()
+{
+ switch (servoType)
+ {
+ case NONE:
+ vpERROR_TRACE("No control law have been yet defined") ;
+ throw(vpServoException(vpServoException::servoError,
+ "No control law have been yet defined")) ;
+ break ;
+ case EYEINHAND_CAMERA:
+ return true ;
+ case EYEINHAND_L_cVe_eJe:
+ if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
+ return (init_eJe) ;
+ break ;
+ case EYETOHAND_L_cVe_eJe:
+ if (!init_cVe) vpERROR_TRACE("cVe not updated") ;
+ if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
+ return (init_cVe && init_eJe) ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ if (!init_fVe) vpERROR_TRACE("fVe not updated") ;
+ if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
+ return (init_fVe && init_eJe) ;
+ break ;
+
+ case EYETOHAND_L_cVf_fJe :
+ if (!init_fJe) vpERROR_TRACE("fJe not updated") ;
+ return (init_fJe) ;
+ break ;
+ }
+
+ return false ;
+}
+/*!
+
+ Compute the control law specified using setServo(). See vpServo::vpServoType for more
+ details concerning the control laws that are available. The \ref tutorial-ibvs and \ref tutorial-boost-vs
+ are also useful to illustrate the usage of this function.
+
+ The general form of the control law is the following:
+
+ \f[
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e}
+ \f]
+
+ where :
+ - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the robot.
+ - the sign of the control law depends on the eye in hand or eye to hand configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
+ Jacobian.
+ - \f$\bf e = (s-s^*)\f$ is the error to regulate.
+
+ To ensure continuous sequencing the computeControlLaw(double) function can be used. It will ensure that
+ the velocities that are computed are continuous.
+*/
+vpColVector vpServo::computeControlLaw()
+{
+ static int iteration =0;
+
+ try
+ {
+ vpVelocityTwistMatrix cVa ; // Twist transformation matrix
+ vpMatrix aJe ; // Jacobian
+
+ if (iteration==0)
+ {
+ if (testInitialization() == false) {
+ vpERROR_TRACE("All the matrices are not correctly initialized") ;
+ throw(vpServoException(vpServoException::servoError,
+ "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized")) ;
+ }
+ }
+ if (testUpdated() == false) {
+ vpERROR_TRACE("All the matrices are not correctly updated") ;
+ }
+
+ // test if all the required initialization have been done
+ switch (servoType)
+ {
+ case NONE :
+ vpERROR_TRACE("No control law have been yet defined") ;
+ throw(vpServoException(vpServoException::servoError,
+ "No control law have been yet defined")) ;
+ break ;
+ case EYEINHAND_CAMERA:
+ case EYEINHAND_L_cVe_eJe:
+ case EYETOHAND_L_cVe_eJe:
+
+ cVa = cVe ;
+ aJe = eJe ;
+
+ init_cVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf*fVe ;
+ aJe = eJe ;
+ init_fVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fJe :
+ cVa = cVf ;
+ aJe = fJe ;
+ init_fJe = false ;
+ break ;
+ }
+
+ computeInteractionMatrix() ;
+ computeError() ;
+
+ // compute task Jacobian
+ J1 = L*cVa*aJe ;
+
+ // handle the eye-in-hand eye-to-hand case
+ J1 *= signInteractionMatrix ;
+
+ // pseudo inverse of the task Jacobian
+ // and rank of the task Jacobian
+ // the image of J1 is also computed to allows the computation
+ // of the projection operator
+ vpMatrix imJ1t, imJ1 ;
+ bool imageComputed = false ;
+
+ if (inversionType==PSEUDO_INVERSE)
+ {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+
+ imageComputed = true ;
+ }
+ else
+ J1p = J1.t() ;
+
+ if (rankJ1 == J1.getCols())
+ {
+ /* if no degrees of freedom remains (rank J1 = ndof)
+ WpW = I, multiply by WpW is useless
+ */
+ e1 = J1p*error ;// primary task
+
+ WpW.eye(J1.getCols(), J1.getCols()) ;
+ }
+ else
+ {
+ if (imageComputed!=true)
+ {
+ vpMatrix Jtmp ;
+ // image of J1 is computed to allows the computation
+ // of the projection operator
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ imageComputed = true ;
+ }
+ WpW = imJ1t*imJ1t.t() ;
+
+#ifdef DEBUG
+ std::cout << "rank J1 " << rankJ1 <<std::endl ;
+ std::cout << "imJ1t"<<std::endl << imJ1t ;
+ std::cout << "imJ1"<<std::endl << imJ1 ;
+
+ std::cout << "WpW" <<std::endl <<WpW ;
+ std::cout << "J1" <<std::endl <<J1 ;
+ std::cout << "J1p" <<std::endl <<J1p ;
+#endif
+ e1 = WpW*J1p*error ;
+ }
+ e = - lambda(e1) * e1 ;
+
+ vpMatrix I ;
+
+ I.eye(J1.getCols(),J1.getCols()) ;
+
+ computeProjectionOperators();
+
+ }
+ catch(...) {
+ throw;
+ }
+
+ iteration++ ;
+ return e ;
+}
+
+/*!
+ Compute the control law specified using setServo(). See vpServo::vpServoType for more
+ details concerning the control laws that are available. The \ref tutorial-boost-vs is also useful to
+ illustrate the usage of this function.
+
+ To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
+ the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
+ continuous velocities by avoiding abrupt changes in the command.
+
+ The form of the control law considered here is the following:
+
+ \f[
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t)
+ \f]
+
+ where :
+ - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
+ - the sign of the control law depends on the eye in hand or eye to hand configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
+ Jacobian.
+ - \f$\bf e = (s-s^*)\f$ is the error to regulate.
+ - \f$t\f$ is the time given as parameter of this method.
+ - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
+ - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
+ This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
+
+ \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
+*/
+vpColVector vpServo::computeControlLaw(double t)
+{
+ static int iteration =0;
+ //static vpColVector e1_initial;
+
+ try
+ {
+ vpVelocityTwistMatrix cVa ; // Twist transformation matrix
+ vpMatrix aJe ; // Jacobian
+
+ if (iteration==0)
+ {
+ if (testInitialization() == false) {
+ vpERROR_TRACE("All the matrices are not correctly initialized") ;
+ throw(vpServoException(vpServoException::servoError,
+ "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized")) ;
+ }
+ }
+ if (testUpdated() == false) {
+ vpERROR_TRACE("All the matrices are not correctly updated") ;
+ }
+
+ // test if all the required initialization have been done
+ switch (servoType)
+ {
+ case NONE :
+ vpERROR_TRACE("No control law have been yet defined") ;
+ throw(vpServoException(vpServoException::servoError,
+ "No control law have been yet defined")) ;
+ break ;
+ case EYEINHAND_CAMERA:
+ case EYEINHAND_L_cVe_eJe:
+ case EYETOHAND_L_cVe_eJe:
+
+ cVa = cVe ;
+ aJe = eJe ;
+
+ init_cVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf*fVe ;
+ aJe = eJe ;
+ init_fVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fJe :
+ cVa = cVf ;
+ aJe = fJe ;
+ init_fJe = false ;
+ break ;
+ }
+
+ computeInteractionMatrix() ;
+ computeError() ;
+
+ // compute task Jacobian
+ J1 = L*cVa*aJe ;
+
+ // handle the eye-in-hand eye-to-hand case
+ J1 *= signInteractionMatrix ;
+
+ // pseudo inverse of the task Jacobian
+ // and rank of the task Jacobian
+ // the image of J1 is also computed to allows the computation
+ // of the projection operator
+ vpMatrix imJ1t, imJ1 ;
+ bool imageComputed = false ;
+
+ if (inversionType==PSEUDO_INVERSE)
+ {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+
+ imageComputed = true ;
+ }
+ else
+ J1p = J1.t() ;
+
+ if (rankJ1 == J1.getCols())
+ {
+ /* if no degrees of freedom remains (rank J1 = ndof)
+ WpW = I, multiply by WpW is useless
+ */
+ e1 = J1p*error ;// primary task
+
+ WpW.eye(J1.getCols(), J1.getCols()) ;
+ }
+ else
+ {
+ if (imageComputed!=true)
+ {
+ vpMatrix Jtmp ;
+ // image of J1 is computed to allows the computation
+ // of the projection operator
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ imageComputed = true ;
+ }
+ WpW = imJ1t*imJ1t.t() ;
+
+#ifdef DEBUG
+ std::cout << "rank J1 " << rankJ1 <<std::endl ;
+ std::cout << "imJ1t"<<std::endl << imJ1t ;
+ std::cout << "imJ1"<<std::endl << imJ1 ;
+
+ std::cout << "WpW" <<std::endl <<WpW ;
+ std::cout << "J1" <<std::endl <<J1 ;
+ std::cout << "J1p" <<std::endl <<J1p ;
+#endif
+ e1 = WpW*J1p*error ;
+ }
+
+ // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
+ if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
+ e1_initial = e1;
+ }
+ // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
+ if (e1_initial.getRows() != e1.getRows())
+ e1_initial = e1;
+
+ e = - lambda(e1) * e1 + lambda(e1) * e1_initial*exp(-mu*t);
+
+ vpMatrix I ;
+
+ I.eye(J1.getCols(), J1.getCols()) ;
+
+ computeProjectionOperators() ;
+ }
+ catch(...) {
+ throw;
+ }
+
+ iteration++ ;
+ return e ;
+}
+
+/*!
+ Compute the control law specified using setServo(). See vpServo::vpServoType for more
+ details concerning the control laws that are available.
+
+ To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
+ the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
+ continuous velocities by avoiding abrupt changes in the command.
+
+ The form of the control law considered here is the following:
+
+ \f[
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu t)
+ \f]
+
+ where :
+ - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
+ - the sign of the control law depends on the eye in hand or eye to hand configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
+ Jacobian.
+ - \f$\bf e = (s-s^*)\f$ is the error to regulate.
+ - \f$t\f$ is the time given as parameter of this method.
+ - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
+ - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
+ This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
+
+ \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
+ \param e_dot_init : Initial value of \f${\bf \dot e}(0)\f$.
+*/
+vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init)
+{
+ static int iteration =0;
+
+ try
+ {
+ vpVelocityTwistMatrix cVa ; // Twist transformation matrix
+ vpMatrix aJe ; // Jacobian
+
+ if (iteration==0)
+ {
+ if (testInitialization() == false) {
+ vpERROR_TRACE("All the matrices are not correctly initialized") ;
+ throw(vpServoException(vpServoException::servoError,
+ "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized")) ;
+ }
+ }
+ if (testUpdated() == false) {
+ vpERROR_TRACE("All the matrices are not correctly updated") ;
+ }
+
+ // test if all the required initialization have been done
+ switch (servoType)
+ {
+ case NONE :
+ vpERROR_TRACE("No control law have been yet defined") ;
+ throw(vpServoException(vpServoException::servoError,
+ "No control law have been yet defined")) ;
+ break ;
+ case EYEINHAND_CAMERA:
+ case EYEINHAND_L_cVe_eJe:
+ case EYETOHAND_L_cVe_eJe:
+
+ cVa = cVe ;
+ aJe = eJe ;
+
+ init_cVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf*fVe ;
+ aJe = eJe ;
+ init_fVe = false ;
+ init_eJe = false ;
+ break ;
+ case EYETOHAND_L_cVf_fJe :
+ cVa = cVf ;
+ aJe = fJe ;
+ init_fJe = false ;
+ break ;
+ }
+
+ computeInteractionMatrix() ;
+ computeError() ;
+
+ // compute task Jacobian
+ J1 = L*cVa*aJe ;
+
+ // handle the eye-in-hand eye-to-hand case
+ J1 *= signInteractionMatrix ;
+
+ // pseudo inverse of the task Jacobian
+ // and rank of the task Jacobian
+ // the image of J1 is also computed to allows the computation
+ // of the projection operator
+ vpMatrix imJ1t, imJ1 ;
+ bool imageComputed = false ;
+
+ if (inversionType==PSEUDO_INVERSE)
+ {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+
+ imageComputed = true ;
+ }
+ else
+ J1p = J1.t() ;
+
+ if (rankJ1 == J1.getCols())
+ {
+ /* if no degrees of freedom remains (rank J1 = ndof)
+ WpW = I, multiply by WpW is useless
+ */
+ e1 = J1p*error ;// primary task
+
+ WpW.eye(J1.getCols(), J1.getCols()) ;
+ }
+ else
+ {
+ if (imageComputed!=true)
+ {
+ vpMatrix Jtmp ;
+ // image of J1 is computed to allows the computation
+ // of the projection operator
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ imageComputed = true ;
+ }
+ WpW = imJ1t*imJ1t.t() ;
+
+#ifdef DEBUG
+ std::cout << "rank J1 " << rankJ1 <<std::endl ;
+ std::cout << "imJ1t"<<std::endl << imJ1t ;
+ std::cout << "imJ1"<<std::endl << imJ1 ;
+
+ std::cout << "WpW" <<std::endl <<WpW ;
+ std::cout << "J1" <<std::endl <<J1 ;
+ std::cout << "J1p" <<std::endl <<J1p ;
+#endif
+ e1 = WpW*J1p*error ;
+ }
+
+ // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
+ if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
+ e1_initial = e1;
+ }
+ // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
+ if (e1_initial.getRows() != e1.getRows())
+ e1_initial = e1;
+
+ e = - lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial)*exp(-mu*t);
+
+ vpMatrix I ;
+
+ I.eye(J1.getCols(), J1.getCols()) ;
+
+ computeProjectionOperators();
+ }
+ catch(...) {
+ throw;
+ }
+
+ iteration++ ;
+ return e ;
+}
+
+void vpServo::computeProjectionOperators()
+{
+ // Initialization
+ unsigned int n = J1.getCols();
+ P.resize(n,n);
+
+ vpMatrix I;
+ I.eye(n);
+
+ //Compute classical projection operator
+ I_WpW = (I - WpW) ;
+
+ // Compute gain depending by the task error to ensure a smooth change between the operators.
+ double e0_ = 0.1;
+ double e1_ = 0.7;
+ double sig = 0.0;
+
+ double norm_e = error.euclideanNorm() ;
+ if (norm_e > e1_)
+ sig = 1.0;
+ else if (e0_ <= norm_e && norm_e <= e1_ )
+ sig = 1.0 / (1.0 + exp(-12.0 * ( (norm_e-e0_)/((e1_-e0_))) + 6.0 ) );
+ else
+ sig = 0.0;
+
+ vpMatrix J1t = J1.transpose();
+
+ double pp = (error.t() * (J1 * J1t) * error);
+
+ vpMatrix ee_t(n,n);
+ ee_t = error * error.t();
+
+ vpMatrix P_norm_e(n,n);
+ P_norm_e = I - (1.0 / pp ) * J1t * ee_t * J1;
+
+ P = sig * P_norm_e + (1 - sig) * I_WpW;
+
+ return;
+}
+
+/*!
+ Compute and return the secondary task vector according to the classic projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper \cite Marchand05b)
+ or the new large projection operator (see equation(24) in the paper \cite Marey:2010).
+
+ \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
+ \param useLargeProjectionOperator : if true will be use the large projection operator, if false the classic one (default).
+
+ \return The secondary task vector.
+
+ If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
+
+ \f[
+ ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
+ \f]
+
+ Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
+ \f[
+ -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
+ \f]
+
+ Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
+
+ \f[
+ {\bf P}\frac{\partial {\bf e_2}}{\partial t}
+ \f]
+
+ where
+
+ \f[
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
+ \f]
+
+ with
+
+ \f[
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
+ \f]
+
+ \warning computeControlLaw() must be call prior to this function since it updates the projection operators.
+
+ The following sample code shows how to use this method to compute a secondary task using the classic projection operator:
+ \code
+ vpColVector v; // Velocity applied to the robot
+ vpColVector de2dt;
+ vpServo task;
+ ...
+ v = task.computeControlLaw(); // Compute the primary task
+ v += task.secondaryTask(de2dt) // Compute and add the secondary task using the classical projection operator
+ \endcode
+
+ The following sample code shows how to use this method to compute a secondary task using the large projection operator:
+ \code
+ vpColVector v; // Velocity applied to the robot
+ vpColVector de2dt;
+ vpServo task;
+ ...
+ v = task.computeControlLaw(); // Compute the primary task
+ v += task.secondaryTask(de2dt, true) // Compute and add the secondary task using the large projection operator
+ \endcode
+
+ \sa computeControlLaw()
+*/
+vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
+{
+ vpColVector sec ;
+
+ if (!useLargeProjectionOperator)
+ {
+ if (rankJ1 == J1.getCols())
+ {
+ vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
+ throw(vpServoException(vpServoException::noDofFree,
+ "no degree of freedom is free, cannot use secondary task")) ;
+ }
+ else
+ {
+#if 0
+ // computed in computeControlLaw()
+ vpMatrix I ;
+
+ I.resize(J1.getCols(),J1.getCols()) ;
+ I.setIdentity() ;
+ I_WpW = (I - WpW) ;
+#endif
+ // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
+ sec = I_WpW*de2dt ;
+ }
+ }
+
+ else
+ sec = P*de2dt;
+
+ return sec ;
+}
+
+/*!
+ Compute and return the secondary task vector according to the classic projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper \cite Marchand05b)
+ or the new large projection operator (see equation(24) in the paper \cite Marey:2010).
+
+ \param e2 : Value of the secondary task \f${\bf e}_2\f$.
+ \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
+ \param useLargeProjectionOperator: if true will be use the large projection operator, if false the classic one (default).
+
+ \return The secondary task vector.
+
+ If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
+
+ \f[
+ -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
+ \f]
+
+ Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
+ \f[
+ -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
+ \f]
+
+
+ Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
+
+ \f[
+ -\lambda {\bf P} {\bf e_2} + {\bf P}\frac{\partial {\bf e_2}}{\partial t}
+ \f]
+
+ where
+
+ \f[
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
+ \f]
+
+ with
+
+ \f[
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
+ \f]
+
+ \warning computeControlLaw() must be call prior to this function since it updates the projection operators.
+
+ The following sample code shows how to use this method to compute a secondary task using the classical projection operator:
+ \code
+ vpColVector v; // Velocity applied to the robot
+ vpColVector e2;
+ vpColVector de2dt;
+ vpServo task;
+ ...
+ v = task.computeControlLaw(); // Compute the primary task
+ v += task.secondaryTask(e2, de2dt) // Compute and add the secondary task using the classical projection operator
+ \endcode
+
+ The following sample code shows how to use this method to compute a secondary task using the large projection operator:
+ \code
+ vpColVector v; // Velocity applied to the robot
+ vpColVector e2;
+ vpColVector de2dt;
+ vpServo task;
+ ...
+ v = task.computeControlLaw(); // Compute the primary task
+ v += task.secondaryTask(e2, de2dt, true) // Compute and add the secondary task using the large projection operator
+ \endcode
+
+
+ \sa computeControlLaw()
+*/
+vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2dt, const bool &useLargeProjectionOperator)
+{
+ vpColVector sec ;
+
+ if (!useLargeProjectionOperator)
+ {
+ if (rankJ1 == J1.getCols())
+ {
+ vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
+ throw(vpServoException(vpServoException::noDofFree,
+ "no degree of freedom is free, cannot use secondary task")) ;
+ }
+ else
+ {
+
+#if 0
+ // computed in computeControlLaw()
+ vpMatrix I ;
+
+ I.resize(J1.getCols(),J1.getCols()) ;
+ I.setIdentity() ;
+
+ I_WpW = (I - WpW) ;
+#endif
+
+ // To be coherent with the primary task the gain must be the same between
+ // primary and secondary task.
+ sec = -lambda(e1) *I_WpW*e2 + I_WpW *de2dt ;
+
+
+ }
+ }
+ else
+ sec = -lambda(e1) * P *e2 + P *de2dt ;
+
+
+ return sec ;
+}
+
+/*!
+ Compute and return the secondary task vector for joint limit avoidance \cite Marey:2010b using the
+ new large projection operator (see equation(24) in the paper \cite Marey:2010). The robot avoids the
+ joint limits very smoothly even when the main task constrains all the robot degrees of freedom.
+
+ \param q : Actual joint positions vector
+ \param dq : Actual joint velocities vector
+ \param qmin : Vector containing the low limit value of each joint in the chain.
+ \param qmax : Vector containing the high limit value of each joint in the chain.
+ \param rho : tuning paramenter \f${\left [ 0,\frac{1}{2} \right ]}\f$ used to define the safe configuration for the joint. When
+ the joint angle value cross the max or min boundaries (\f${ q_{l_{0}}^{max} }\f$ and \f${q_{l_{0}}^{min}}\f$) the secondary
+ task is actived gradually.
+ \param rho1 : tuning paramenter \f${\left ] 0,1 \right ]}\f$ to compute the external boundaries (\f${q_{l_{1}}^{max}}\f$ and
+ \f${q_{l_{1}}^{min}}\f$) for the joint limits. Here the secondary task it completely activated with the highest gain.
+ \param lambda_tune : value \f${\left [ 0,1 \right ]}\f$ used to tune the difference in magnitude between the absolute value of the
+ elements of the primary task and the elements of the secondary task. (See equation (17) \cite Marey:2010b )
+
+\code
+vpServo task;
+vpColVector qmin;
+vpColVector qmax;
+vpColVector q;
+vpColVector dq;
+// Fill vector qmin and qmax with min and max limits of the joints (same joint order than vector q).
+// Update vector of joint position q and velocities dq;
+...
+vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+v += task.secondaryTaskJointLimitAvoidance(q, dq, qmin, qmax) // Compute and add the secondary task for the joint
+limit avoidance using the large projection operator
+\endcode
+
+ */
+vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq,
+ const vpColVector &qmin, const vpColVector &qmax,
+ const double &rho, const double &rho1, const double &lambda_tune) const
+{
+ unsigned int const n = J1.getCols();
+
+ if (qmin.size() != n || qmax.size() != n )
+ {
+ std::stringstream msg;
+ msg << "Dimension vector qmin (" << qmin.size() << ") or qmax () does not correspond to the number of jacobian columns";
+ msg << "qmin size: " << qmin.size() << std::endl;
+ throw(vpServoException(vpServoException::dimensionError,msg.str())) ;
+ }
+ if (q.size() != n || dq.size() != n )
+ {
+ vpERROR_TRACE("Dimension vector q or dq does not correspont to the number of jacobian columns") ;
+ throw(vpServoException(vpServoException::dimensionError,
+ "Dimension vector q or dq does not correspont to the number of jacobian columns")) ;
+ }
+
+ double lambda_l = 0.0;
+
+ vpColVector q2 (n);
+
+ vpColVector q_l0_min(n);
+ vpColVector q_l0_max(n);
+ vpColVector q_l1_min(n);
+ vpColVector q_l1_max(n);
+
+ // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
+ vpMatrix g(n,n);
+ vpColVector q2_i(n);
+
+ for(unsigned int i = 0; i < n; i++)
+ {
+ q_l0_min[i] = qmin[i] + rho *(qmax[i] - qmin[i]);
+ q_l0_max[i] = qmax[i] - rho *(qmax[i] - qmin[i]);
+
+ q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
+ q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
+
+ if (q[i] < q_l0_min[i] )
+ g[i][i] = -1;
+ else if (q[i] > q_l0_max[i] )
+ g[i][i] = 1;
+ else
+ g[i][i]= 0;
+ }
+
+ for(unsigned int i = 0; i < n; i++)
+ {
+ if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
+ q2_i = 0 * q2_i;
+
+ else
+ {
+ vpColVector Pg_i(n);
+ Pg_i = (P * g.getCol(i));
+ double b = ( vpMath::abs(dq[i]) )/( vpMath::abs( Pg_i[i] ) );
+
+ if (b < 1.) // If the ratio b is big we don't activate the joint avoidance limit for the joint.
+ {
+ if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i] )
+ q2_i = - (1 + lambda_tune) * b * Pg_i;
+
+ else
+ {
+ if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i] )
+ lambda_l = 1 / (1 + exp(-12 *( (q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i]) ) + 6 ) );
+
+ else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
+ lambda_l = 1 / (1 + exp(-12 *( (q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i]) ) + 6 ) );
+
+ q2_i = - lambda_l * (1 + lambda_tune)* b * Pg_i;
+ }
+ }
+ }
+ q2 = q2 + q2_i;
+ }
+ return q2;
+}
+
+/*!
+ Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This operator is updated
+ after a call of computeControlLaw().
+
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator
+\endcode
+ \sa getWpW()
+ */
+vpMatrix vpServo::getI_WpW() const
+{
+ return I_WpW;
+}
+
+
+/*!
+ Return the large projection operator. This operator is updated
+ after a call of computeControlLaw().
+
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix P = task.getP(); // Get the large projection operator
+\endcode
+ \sa getP()
+ */
+vpMatrix vpServo::getLargeP() const
+{
+ return P;
+}
+
+
+/*!
+ Return the task jacobian \f$J\f$. The task jacobian is updated after a call of computeControlLaw().
+
+ In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix J = task.getTaskJacobian(); // Get the task jacobian used to compute v
+\endcode
+ \sa getTaskJacobianPseudoInverse(), getInteractionMatrix()
+ */
+vpMatrix vpServo::getTaskJacobian() const
+{
+ return J1;
+}
+/*!
+ Return the pseudo inverse of the task jacobian \f$J\f$.
+
+ In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
+
+ The task jacobian and its pseudo inverse are updated after a call of computeControlLaw().
+
+ \return Pseudo inverse \f${J}^{+}\f$ of the task jacobian.
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v
+\endcode
+
+ \sa getTaskJacobian()
+ */
+vpMatrix vpServo::getTaskJacobianPseudoInverse() const
+{
+ return J1p;
+}
+/*!
+ Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw().
+
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ unsigned int rank = task.getTaskRank(); // Get the rank of the task jacobian
+\endcode
+ */
+unsigned int vpServo::getTaskRank() const
+{
+ return rankJ1;
+}
+
+/*!
+ Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is updated
+ after a call of computeControlLaw().
+
+ When the dimension of the task is equal to the number of degrees of freedom available
+ \f${\bf W^+W = I}\f$.
+
+\code
+ vpServo task;
+ ...
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
+ vpMatrix WpW = task.getWpW(); // Get the projection operator
+\endcode
+ \sa getI_WpW()
+ */
+vpMatrix vpServo::getWpW() const
+{
+ return WpW;
+}
diff --git a/modules/vs/src/vpServoData.cpp b/modules/vs/src/vpServoData.cpp
new file mode 100644
index 0000000..d9ce2f5
--- /dev/null
+++ b/modules/vs/src/vpServoData.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Save data during the task execution.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+
+/*!
+ \file vpServoData.cpp
+ \brief save data during the task execution
+*/
+
+// Servo
+#include <visp3/vs/vpServo.h>
+
+#include <visp3/vs/vpServoData.h>
+#include <visp3/core/vpIoException.h>
+#include <visp3/core/vpIoTools.h>
+
+void
+vpServoData::open(const char *directory)
+{
+ try
+ {
+ if (vpIoTools::checkDirectory(directory) == false)
+ vpIoTools::makeDirectory(directory);
+
+ char s[FILENAME_MAX] ;
+
+ sprintf(s,"%s/vel.dat",directory) ;
+ velocityFile.open(s) ;
+ sprintf(s,"%s/error.dat",directory) ;
+ errorFile.open(s) ;
+ sprintf(s,"%s/errornorm.dat",directory) ;
+ errorNormFile.open(s) ;
+ sprintf(s,"%s/s.dat",directory) ;
+ sFile.open(s) ;
+ sprintf(s,"%s/sStar.dat",directory) ;
+ sStarFile.open(s) ;
+
+ }
+ catch(...)
+ {
+ vpERROR_TRACE("Error caught") ;
+ throw ;
+ }
+}
+
+void vpServoData::setCmDeg()
+{
+ cmDeg = true ;
+}
+void vpServoData::setMeterRad()
+{
+ cmDeg = false ;
+}
+void vpServoData::save(const vpServo &task)
+{
+ if (cmDeg==false) velocityFile << task.q_dot.t() ;
+ else
+ {
+ for (unsigned int i=0 ; i < 3 ; i++)
+ velocityFile << task.q_dot[i]*100 <<" " ;
+ for (unsigned int i=4 ; i < 6 ; i++)
+ velocityFile << vpMath::deg(task.q_dot[i]) <<" " ;
+ velocityFile << std::endl ;
+ }
+ errorFile << ( task.getError() ).t() ;
+ errorNormFile << ( task.getError() ).sumSquare() << std::endl ;
+ vNormFile << task.q_dot.sumSquare() << std::endl ;
+
+ sFile <<task.s.t() ;
+ sStarFile << task.sStar.t();
+}
+
+
+
+void vpServoData::close()
+{
+ velocityFile.close() ;
+ errorFile.close() ;
+ errorNormFile.close() ;
+ sFile.close() ;
+ sStarFile.close() ;
+}
+
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vs/src/vpServoDisplay.cpp b/modules/vs/src/vpServoDisplay.cpp
new file mode 100644
index 0000000..37bfe0a
--- /dev/null
+++ b/modules/vs/src/vpServoDisplay.cpp
@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Interface with the image for feature display.
+ *
+ * Authors:
+ * Eric Marchand
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpServoDisplay.cpp
+ \brief interface with the image for feature display
+*/
+
+// Servo
+#include <visp3/vs/vpServo.h>
+
+// Meter/pixel conversion
+#include <visp3/core/vpCameraParameters.h>
+
+//Color / image / display
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+
+#include <visp3/core/vpDisplay.h>
+
+#include <visp3/vs/vpServoDisplay.h>
+
+#include <visp3/visual_features/vpBasicFeature.h>
+
+/*!
+
+ Display the current and the desired features in the image I.
+
+ \warning To effectively display the dot graphics a call to
+ vpDisplay::flush() is needed.
+
+ \param s : Visual servoing control law.
+ \param cam : Camera parameters.
+ \param I : Image on which features have to be displayed.
+
+ \param currentColor : Color for the current features. If vpColor::none,
+ current features display is turned off.
+
+ \param desiredColor : Color for the desired features. If vpColor::none,
+ desired features display is turned off.
+
+ \param thickness : Thickness of the feature representation.
+
+*/
+void vpServoDisplay::display(const vpServo &s,
+ const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I,
+ vpColor currentColor,
+ vpColor desiredColor,
+ unsigned int thickness)
+{
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<vpBasicFeature *>::const_iterator it_s_star;
+
+ for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
+ it_s != s.featureList.end();
+ ++it_s, ++it_s_star)
+ {
+ if (desiredColor != vpColor::none) {
+ // desired list
+ (*it_s_star)->display(cam, I, desiredColor, thickness ) ;
+ }
+ if (currentColor != vpColor::none) {
+ // current list
+ (*it_s)->display(cam, I, currentColor, thickness ) ;
+ }
+ }
+ // vpDisplay::flush(I) ;
+}
+
+/*!
+
+ Display the current and the desired features in the image I.
+
+ \warning To effectively display the dot graphics a call to
+ vpDisplay::flush() is needed.
+
+ \param s : Visual servoing control law.
+ \param cam : Camera parameters.
+ \param I : Color image on which features have to be displayed.
+
+ \param currentColor : Color for the current features. If vpColor::none,
+ current features display is turned off.
+
+ \param desiredColor : Color for the desired features. If vpColor::none,
+ desired features display is turned off.
+
+ \param thickness : Thickness of the feature representation.
+
+ */
+void
+vpServoDisplay::display(const vpServo &s,
+ const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I,
+ vpColor currentColor,
+ vpColor desiredColor,
+ unsigned int thickness)
+{
+ std::list<vpBasicFeature *>::const_iterator it_s;
+ std::list<vpBasicFeature *>::const_iterator it_s_star;
+
+ for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
+ it_s != s.featureList.end();
+ ++it_s, ++it_s_star)
+ {
+ if (desiredColor != vpColor::none) {
+ // desired list
+ (*it_s_star)->display(cam, I, desiredColor, thickness ) ;
+ }
+ if (currentColor != vpColor::none) {
+ // current list
+ (*it_s)->display(cam, I, currentColor, thickness ) ;
+ }
+ }
+ // vpDisplay::flush(I) ;
+}
diff --git a/modules/vs/test/visual-feature/testFeature.cpp b/modules/vs/test/visual-feature/testFeature.cpp
new file mode 100644
index 0000000..f9ede73
--- /dev/null
+++ b/modules/vs/test/visual-feature/testFeature.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature manipulation.
+ *
+ * Author:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpCameraParameters.h>
+
+#include <iostream>
+
+/*!
+
+ \example testFeature.cpp
+
+ Shows how to build a task with a \f$ \theta u \f$ visual feature.
+
+*/
+int main()
+{
+ try {
+ for (int i=0; i < 3; i++) {
+ vpServo task ;
+
+ // Creation od a Theta U vector that represent the rotation
+ // between the desired camera frame and the current one.
+ vpThetaUVector tu_cdRc; // Current visual feature s
+ tu_cdRc[0] =0.1;
+ tu_cdRc[1] =0.2;
+ tu_cdRc[2] =0.3;
+
+ // Creation of the current feature s
+ vpFeatureThetaU s(vpFeatureThetaU::cdRc);
+ s.buildFrom(tu_cdRc);
+ s.print();
+ task.addFeature(s); // Add current ThetaU feature
+
+ // Creation of the desired feature s^*
+ vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); // init to zero
+
+ // Compute the interaction matrix for the ThetaU_z feature
+ vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
+ // Compute the error vector (s-s^*) for the ThetaU_z feature
+ s.error(s_star, vpFeatureThetaU::selectTUz());
+
+ // A call to kill() is requested here to destroy properly the current
+ // and desired feature lists.
+ task.kill();
+
+ std::cout << "End, call vpServo destructors..." << std::endl;
+ }
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
diff --git a/modules/vs/test/visual-feature/testFeatureMoment.cpp b/modules/vs/test/visual-feature/testFeatureMoment.cpp
new file mode 100644
index 0000000..dc8d788
--- /dev/null
+++ b/modules/vs/test/visual-feature/testFeatureMoment.cpp
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Example of visual servoing with moments using a polygon as object container
+ *
+ * Authors:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpPlane.h>
+
+#include <limits>
+#include <iostream>
+
+//initialize scene in the interface
+void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
+ vpMomentObject &src, vpMomentObject &dst);
+
+vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
+ vpMomentObject &src, vpMomentObject &dst); //launch the test
+void planeToABC(const vpPlane& pl, double& A,double& B, double& C);
+int test(double x,double y,double z,double alpha);
+
+//Compute a set of parallel positions and check if the matrix is in the right form;
+int main()
+{
+ try {
+ int sum=0;
+ for(double i=-0.2;i<0.2;i+=0.1){
+ for(double j=-0.2;j<0.2;j+=0.1){
+ for(double k=-vpMath::rad(30);k<vpMath::rad(30);k+=vpMath::rad(10)){
+ for(double l=0.5;l<1.5;l+=0.1){
+ sum+=test(i,j,l,k);
+ }
+ }
+
+ }
+ }
+ if(sum<0) return -1;
+ else return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+int test(double x,double y,double z,double alpha){
+ //intial pose
+ vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
+ //Desired pose
+ vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
+
+ //source and destination objects for moment manipulation
+ vpMomentObject src(6);
+ vpMomentObject dst(6);
+
+ //init and run the simulation
+ initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
+
+ vpMatrix mat = execute(cMo, cdMo, src, dst);
+
+ if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+
+ return 0;
+}
+
+void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
+ vpMomentObject &src, vpMomentObject &dst)
+{
+ std::vector<vpPoint> src_pts;
+ std::vector<vpPoint> dst_pts;
+
+ double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
+ double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
+ int nbpoints = 4;
+
+ for (int i = 0 ; i < nbpoints ; i++){
+ vpPoint p(x[i],y[i],0.0);
+ p.track(cMo) ;
+ src_pts.push_back(p);
+ }
+
+ src.setType(vpMomentObject::DENSE_POLYGON);
+ src.fromVector(src_pts);
+ for (int i = 0 ; i < nbpoints ; i++){
+ vpPoint p(x[i],y[i],0.0);
+ p.track(cdMo) ;
+ dst_pts.push_back(p);
+ }
+ dst.setType(vpMomentObject::DENSE_POLYGON);
+ dst.fromVector(dst_pts);
+}
+
+
+vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
+ vpMomentObject &src, vpMomentObject &dst)
+{
+ vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired
+
+ vpServo task;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ //A,B,C parameters of source and destination plane
+ double A; double B; double C;
+ double Ad; double Bd; double Cd;
+ //init main object: using moments up to order 6
+
+ //Initializing values from regular plane (with ax+by+cz=d convention)
+ vpPlane pl;
+ pl.setABCD(0,0,1.0,0);
+ pl.changeFrame(cMo);
+ planeToABC(pl,A,B,C);
+
+ pl.setABCD(0,0,1.0,0);
+ pl.changeFrame(cdMo);
+ planeToABC(pl,Ad,Bd,Cd);
+
+ //extracting initial position (actually we only care about Zdst)
+ vpTranslationVector vec;
+ cdMo.extract(vec);
+
+ ///////////////////////////// initializing moments and features /////////////////////////////////
+ //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
+ vpMomentCommon moments (vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
+ vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
+ //same thing with common features
+ vpFeatureMomentCommon featureMoments(moments);
+ vpFeatureMomentCommon featureMomentsDes(momentsDes);
+
+ moments.updateAll(src);
+ momentsDes.updateAll(dst);
+
+ featureMoments.updateAll(A,B,C);
+ featureMomentsDes.updateAll(Ad,Bd,Cd);
+
+ //setup the interaction type
+ task.setInteractionMatrixType(interaction_type) ;
+ //////////////////////////////////add useful features to task//////////////////////////////
+ task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
+ task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
+ //the moments are different in case of a symmetric object
+ task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
+ task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
+
+ task.setLambda(0.4) ;
+
+ task.computeControlLaw();
+ vpMatrix mat = task.computeInteractionMatrix();
+ task.kill();
+ return mat;
+}
+
+
+void planeToABC(const vpPlane& pl, double& A,double& B, double& C)
+{
+ A=-pl.getA()/pl.getD();
+ B=-pl.getB()/pl.getD();
+ C=-pl.getC()/pl.getD();
+}
+
diff --git a/modules/vs/test/visual-feature/testFeatureSegment.cpp b/modules/vs/test/visual-feature/testFeatureSegment.cpp
new file mode 100644
index 0000000..9e9db76
--- /dev/null
+++ b/modules/vs/test/visual-feature/testFeatureSegment.cpp
@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * ("GPL") version 2 as published by the Free Software Foundation.
+ * 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 http://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 at 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:
+ * Visual feature manipulation (segment).
+ *
+ * Author:
+ * Filip Novotny
+ *
+ *****************************************************************************/
+
+#include <fstream>
+#include <iostream>
+#include <vector>
+#include <numeric>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_MODULE_ROBOT
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/vs/vpServo.h> //visual servoing task
+
+/*!
+
+ \example testFeatureSegment.cpp
+
+ Shows how to build a task with a segment visual feature.
+
+*/
+int main(int argc, const char **argv)
+{
+ try {
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ int opt_no_display = 0;
+ int opt_curves = 1;
+#endif
+ int opt_normalized = 1;
+
+ // Parse the command line to set the variables
+ vpParseArgv::vpArgvInfo argTable[] =
+ {
+ #if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ {"-d", vpParseArgv::ARGV_CONSTANT, 0, (char *) &opt_no_display,
+ "Disable display and graphics viewer."},
+ #endif
+ {"-normalized", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &opt_normalized,
+ "1 to use normalized features, 0 for non normalized."},
+ {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
+ "Print the help."},
+ {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
+ } ;
+
+ // Read the command line options
+ if(vpParseArgv::parse(&argc, argv, argTable,
+ vpParseArgv::ARGV_NO_LEFTOVERS |
+ vpParseArgv::ARGV_NO_ABBREV |
+ vpParseArgv::ARGV_NO_DEFAULTS)) {
+ return (false);
+ }
+
+ std::cout << "Used options: " << std::endl;
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ opt_curves = (opt_no_display == 0) ? 1 : 0;
+ std::cout << " - no display: " << opt_no_display << std::endl;
+ std::cout << " - curves : " << opt_curves << std::endl;
+#endif
+ std::cout << " - normalized: " << opt_normalized << std::endl;
+
+ vpCameraParameters cam(640.,480.,320.,240.);
+
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
+ vpDisplay *display = NULL;
+ if (!opt_no_display) {
+#if defined(VISP_HAVE_X11)
+ display = new vpDisplayX;
+#elif defined VISP_HAVE_GDI
+ display = new vpDisplayGDI;
+#endif
+ }
+#endif
+ vpImage<unsigned char> I(480,640,0);
+
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ if (!opt_no_display)
+ display->init(I);
+#endif
+
+ vpHomogeneousMatrix wMo; // Set to indentity. Robot world frame is equal to object frame
+ vpHomogeneousMatrix cMo (-0.5, 0.5, 4., vpMath::rad(10), vpMath::rad(20), vpMath::rad(90));
+ vpHomogeneousMatrix cdMo(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
+ vpHomogeneousMatrix wMc; // Camera location in the robot world frame
+
+ vpPoint P[4]; // 4 points in the object frame
+ P[0].setWorldCoordinates( .1, .1, 0.);
+ P[1].setWorldCoordinates(-.1, .1, 0.);
+ P[2].setWorldCoordinates(-.1, -.1, 0.);
+ P[3].setWorldCoordinates( .1, -.1, 0.);
+
+ vpPoint Pd[4]; // 4 points in the desired camera frame
+ for (int i=0; i<4; i++) {
+ Pd[i] = P[i];
+ Pd[i].project(cdMo);
+ }
+ vpPoint Pc[4]; // 4 points in the current camera frame
+ for (int i=0; i<4; i++) {
+ Pc[i] = P[i];
+ Pc[i].project(cMo);
+ }
+
+ vpFeatureSegment seg_cur[2], seg_des[2]; // Current and desired features
+ for (int i=0; i <2; i++)
+ {
+ if (opt_normalized) {
+ seg_cur[i].setNormalized(true);
+ seg_des[i].setNormalized(true);
+ }
+ else {
+ seg_cur[i].setNormalized(false);
+ seg_des[i].setNormalized(false);
+ }
+ vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
+ vpFeatureBuilder::create(seg_des[i], Pd[i*2], Pd[i*2+1]);
+ seg_cur[i].print();
+ seg_des[i].print();
+ }
+
+ //define visual servoing task
+ vpServo task;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setLambda(1) ;
+
+ for (int i=0; i <2; i++)
+ task.addFeature(seg_cur[i], seg_des[i]);
+
+#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+ if (!opt_no_display) {
+ vpDisplay::display(I);
+ for (int i=0; i <2; i++) {
+ seg_cur[i].display(cam, I, vpColor::red);
+ seg_des[i].display(cam, I, vpColor::green);
+ vpDisplay::flush(I);
+ }
+ }
+#endif
+
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ vpPlot *graph = NULL;
+ if (opt_curves)
+ {
+ //Create a window (700 by 700) at position (100, 200) with two graphics
+ graph = new vpPlot(2, 500, 500, 700, 10, "Curves...");
+
+ //The first graphic contains 3 curve and the second graphic contains 3 curves
+ graph->initGraph(0,6);
+ graph->initGraph(1,8);
+ // graph->setTitle(0, "Velocities");
+ // graph->setTitle(1, "Error s-s*");
+ }
+#endif
+
+ //param robot
+ vpSimulatorCamera robot;
+ float sampling_time = 0.010f; // Sampling period in seconds
+ robot.setSamplingTime(sampling_time);
+ wMc = wMo * cMo.inverse();
+ robot.setPosition(wMc);
+ int iter=0;
+
+ do {
+ double t = vpTime::measureTimeMs();
+ wMc = robot.getPosition();
+ cMo = wMc.inverse() * wMo;
+ for (int i=0; i <4; i++)
+ Pc[i].project(cMo);
+
+ for (int i=0; i <2; i++)
+ vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
+
+#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+ if (!opt_no_display) {
+ vpDisplay::display(I);
+ for (int i=0; i <2; i++) {
+ seg_cur[i].display(cam, I, vpColor::red);
+ seg_des[i].display(cam, I, vpColor::green);
+ vpDisplay::flush(I);
+ }
+ }
+#endif
+
+ vpColVector v = task.computeControlLaw();
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ if (opt_curves)
+ {
+ graph->plot(0, iter, v); // plot velocities applied to the robot
+ graph->plot(1, iter, task.getError()); // plot error vector
+ }
+#endif
+
+ vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
+ iter ++;
+
+ } while(( task.getError() ).sumSquare() > 0.0005);
+
+ // A call to kill() is requested here to destroy properly the current
+ // and desired feature lists.
+ task.kill();
+
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ if (graph != NULL)
+ delete graph;
+#endif
+#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+ if (!opt_no_display && display != NULL)
+ delete display;
+#endif
+
+ std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
+ return 0;
+ }
+ catch(vpException e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return 1;
+ }
+}
+
+#else
+int main()
+{
+ std::cout << "Test empty since visp_robot module is not available.\n" << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index b1cd872..0000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,133 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4971 2014-11-16 13:01:59Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# This file creates the ViSP library.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-#----------------------------------------------------------------------
-# Include project files
-#----------------------------------------------------------------------
-# include all the ViSP project .cpp files
-include(${VISP_SOURCE_DIR}/CMakeSourceFileList.cmake)
-# include all the ViSP project .h files
-include(${VISP_SOURCE_DIR}/CMakeHeaderFileList.cmake)
-
-#----------------------------------------------------------------------
-# Create rule to copy all the headers from src to include/visp
-#----------------------------------------------------------------------
-# For each header, we create a rule
-set(HEADER_IN_INCLUDE_DIR "")
-foreach(header_ ${HEADER_ALL})
- get_filename_component(headerName ${header_} NAME)
- add_custom_command(
- OUTPUT ${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR}/visp/${headerName}
- COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/${header_} ${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR}/visp/${headerName}
- DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/${header_}
- )
- set(HEADER_IN_INCLUDE_DIR ${HEADER_IN_INCLUDE_DIR} ${VISP_BINARY_DIR}/${CMAKE_INSTALL_INCLUDEDIR}/visp/${headerName}
- )
-endforeach()
-
-#----------------------------------------------------------------------
-# Create a top level rule to copy all the headers from src to include/visp
-#----------------------------------------------------------------------
-add_custom_target(header ALL
- DEPENDS ${HEADER_IN_INCLUDE_DIR}
- )
-
-#----------------------------------------------------------------------
-# build rule for the library
-#----------------------------------------------------------------------
-
-if(POLICY CMP0042)
- cmake_policy(SET CMP0042 NEW)
-endif()
-
-add_library(${VISP_INTERN_LIBRARY}
- ${SRC_ALL}
- ${HEADER_ALL} ${VISP_INCLUDE_DIR}/vpConfig.h)
-
-# create the headers in include/visp before compiling the lib
-add_dependencies(${VISP_INTERN_LIBRARY} header)
-
-# extra target
-add_dependencies(visp_library ${VISP_INTERN_LIBRARY})
-
-# Append the library version information to the library target
-# properties.
-set_target_properties(${VISP_INTERN_LIBRARY} PROPERTIES
- VERSION ${VISP_VERSION}
- SOVERSION ${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}
- PUBLIC_HEADER "${HEADER_ALL};${VISP_INCLUDE_DIR}/vpConfig.h"
- OUTPUT_NAME "${VISP_INTERN_LIBRARY}${VISP_DLLVERSION}"
- DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
- RUNTIME_OUTPUT_DIRECTORY "${BINARY_OUTPUT_PATH}"
- LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}"
- ARCHIVE_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}"
- )
-set_property(TARGET ${VISP_INTERN_LIBRARY} APPEND PROPERTY
- INTERFACE_INCLUDE_DIRECTORIES ${VISP_EXTERN_INCLUDE_DIRS}
-)
-if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${VISP_INTERN_LIBRARY} PROPERTIES FOLDER "library")
- set_target_properties(header PROPERTIES FOLDER "library")
-endif()
-
-
-
-# Link with external libs especially under windows for winmm.lib
-# needed to create visp.dll. Why ? Because for binaries, winmm.lib
-# is considered in the link stage. Probably a CMake bug !
-#IF(WIN32 AND BUILD_SHARED_LIBS AND HAVE_LIBWINMM)
-# TARGET_LINK_LIBRARIES(${VISP_INTERN_LIBRARY} "winmm")
-#ENDIF(WIN32 AND BUILD_SHARED_LIBS AND HAVE_LIBWINMM)
-target_link_libraries(${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
-#----------------------------------------------------------------------
-# customize install target
-#----------------------------------------------------------------------
-install(
- TARGETS ${VISP_INTERN_LIBRARY}
- EXPORT VISPTargets
- # install rule for all the headers
- PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/visp
- # Destination is either lib or lib64 on Unix 64 bits architectures
- RUNTIME DESTINATION ${VISP_BIN_INSTALL_PATH} COMPONENT libraries
- LIBRARY DESTINATION ${VISP_LIB_INSTALL_PATH} COMPONENT libraries
- ARCHIVE DESTINATION ${VISP_LIB_INSTALL_PATH} COMPONENT libraries
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
-)
diff --git a/src/camera/calibration/vpCalibration.cpp b/src/camera/calibration/vpCalibration.cpp
deleted file mode 100644
index 48403fc..0000000
--- a/src/camera/calibration/vpCalibration.cpp
+++ /dev/null
@@ -1,1056 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCalibration.cpp 5233 2015-01-30 13:50:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Camera calibration.
- *
- * Authors:
- * Eric Marchand
- * Francois Chaumette
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-/*!
- \file vpCalibration.cpp
- \brief Tools for camera calibration.
-*/
-
-#include <visp/vpCalibration.h>
-#include <visp/vpDebug.h>
-#include <visp/vpPose.h>
-#include <visp/vpPixelMeterConversion.h>
-
-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 ;
-
- residual = residual_dist = 1000.;
-
- LoX.clear() ;
- LoY.clear() ;
- LoZ.clear() ;
- Lip.clear() ;
-
- return 0 ;
-}
-
-/*!
- Default constructor.
- */
-vpCalibration::vpCalibration()
- : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
- npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), residual_dist(1000.)
-{
- init() ;
-}
-/*!
- Copy constructor.
- */
-vpCalibration::vpCalibration(const vpCalibration &c)
- : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
- npt(0), LoX(), LoY(), LoZ(), 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 ;
- LoX = twinCalibration.LoX ;
- LoY = twinCalibration.LoY ;
- LoZ = twinCalibration.LoZ ;
- Lip = twinCalibration.Lip ;
-
- residual = twinCalibration.residual;
- cMo = twinCalibration.cMo;
- residual_dist = twinCalibration.residual_dist;
- cMo_dist = twinCalibration.cMo_dist ;
-
- cam = twinCalibration.cam ;
- cam_dist = twinCalibration.cam_dist ;
-
- rMe = twinCalibration.rMe;
-
- eMc = twinCalibration.eMc;
- eMc_dist = twinCalibration.eMc_dist;
-
- return (*this);
-}
-
-
-/*!
- Delete the array of points.
-*/
-int vpCalibration::clearPoint()
-{
- LoX.clear() ;
- LoY.clear() ;
- LoZ.clear() ;
- Lip.clear() ;
- npt = 0 ;
-
- 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);
- LoY.push_back(Y);
- LoZ.push_back(Z);
-
- Lip.push_back(ip);
-
- npt++ ;
-
- return 0 ;
-}
-
-/*!
- Compute the pose cMo
- \param camera : camera intrinsic parameters used for computation.
- \param cMo_est : computed pose
- */
-void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
-{
- // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
- vpPose pose ;
- // the list of point is cleared (if that's not done before)
- pose.clearPoint() ;
- // we set the 3D points coordinates (in meter !) in the object/world frame
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- for (unsigned int i =0 ; i < npt ; i++)
- {
- vpPoint P;
- P.setWorldCoordinates(*it_LoX, *it_LoY, *it_LoZ);
- double x=0,y=0 ;
- vpPixelMeterConversion::convertPoint(camera, *it_Lip, x,y) ;
- P.set_x(x) ;
- P.set_y(y) ;
-
- pose.addPoint(P);
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
- }
- vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
- vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
-
- // compute the initial pose using Lagrange method followed by a non linear
- // minimisation method
- // Pose by Lagrange it provides an initialization of the pose
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
- double residual_lagrange = pose.computeResidual(cMo_lagrange);
-
- // compute the initial pose using Dementhon method followed by a non linear
- // minimisation method
- // Pose by Dementhon it provides an initialization of the pose
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
- double residual_dementhon = pose.computeResidual(cMo_dementhon);
-
- //we keep the better initialization
- if (residual_lagrange < residual_dementhon)
- cMo_est = cMo_lagrange;
- else
- cMo_est = cMo_dementhon;
-
- // the pose is now refined using the virtual visual servoing approach
- // Warning: cMo needs to be initialized otherwise it may diverge
- pose.computePose(vpPose::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 ;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- double u0 = camera.get_u0() ;
- double v0 = camera.get_v0() ;
- double px = camera.get_px() ;
- double py = camera.get_py() ;
- vpImagePoint ip;
-
- for (unsigned int i =0 ; i < npt ; i++)
- {
- double oX = *it_LoX;
- double oY = *it_LoY;
- double oZ = *it_LoZ;
-
- double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
- double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
- double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
-
- double x = cX/cZ ;
- double y = cY/cZ ;
-
- ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
-
- double xp = u0 + x*px;
- double yp = v0 + y*py;
-
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
-
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
- }
- 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 ;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- double u0 = camera.get_u0() ;
- double v0 = camera.get_v0() ;
- double px = camera.get_px() ;
- double py = camera.get_py() ;
- double kud = camera.get_kud() ;
- double kdu = camera.get_kdu() ;
-
- double inv_px = 1/px;
- double inv_py = 1/px;
- vpImagePoint ip;
-
- for (unsigned int i =0 ; i < npt ; i++)
- {
- double oX = *it_LoX;
- double oY = *it_LoY;
- double oZ = *it_LoZ;
-
- double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
- double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
- double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
-
- double x = cX/cZ ;
- double y = cY/cZ ;
-
- ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
-
- double r2ud = 1+kud*(vpMath::sqr(x)+vpMath::sqr(y)) ;
-
- double xp = u0 + x*px*r2ud;
- double yp = v0 + y*py*r2ud;
-
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
-
- double r2du = (vpMath::sqr((u-u0)*inv_px)+vpMath::sqr((v-v0)*inv_py)) ;
-
- xp = u0 + x*px - kdu*(u-u0)*r2du;
- yp = v0 + y*py - kdu*(v-v0)*r2du;
-
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
- }
- residual_ /=2;
-
- this->residual_dist = residual_;
- 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 0 if the calibration computation succeed.
-*/
-int vpCalibration::computeCalibration(vpCalibrationMethodType method,
- vpHomogeneousMatrix &cMo_est,
- vpCameraParameters &cam_est,
- bool verbose)
-{
- try{
- computePose(cam_est,cMo_est);
- switch (method)
- {
- case CALIB_LAGRANGE :
- case CALIB_LAGRANGE_VIRTUAL_VS :
- {
- calibLagrange(cam_est, cMo_est);
- }
- break;
- case CALIB_VIRTUAL_VS:
- case CALIB_VIRTUAL_VS_DIST:
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
- default:
- break;
- }
-
- switch (method)
- {
- case CALIB_VIRTUAL_VS:
- case CALIB_VIRTUAL_VS_DIST:
- case CALIB_LAGRANGE_VIRTUAL_VS:
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
- {
- if (verbose){std::cout << "start calibration without distortion"<< std::endl;}
- calibVVS(cam_est, cMo_est, verbose);
- }
- break ;
- case CALIB_LAGRANGE:
- default:
- break;
- }
- this->cMo = cMo_est;
- this->cMo_dist = cMo_est;
-
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- cam_est.printParameters();
- }
-
- this->cam = cam_est;
-
- switch (method)
- {
- case CALIB_VIRTUAL_VS_DIST:
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
- {
- if (verbose){std::cout << "start calibration with distortion"<< std::endl;}
- calibVVSWithDistortion(cam_est, cMo_est, verbose);
- }
- break ;
- case CALIB_LAGRANGE:
- case CALIB_VIRTUAL_VS:
- case CALIB_LAGRANGE_VIRTUAL_VS:
- default:
- break;
- }
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- this->cam.printParameters();
- // std::cout << "Camera parameters with distortion :" << std::endl;
- cam_est.printParameters();
- }
-
- this->cam_dist = cam_est ;
-
- this->cMo_dist = cMo_est;
- return 0 ;
- }
- 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 0 if the computation was managed succeed.
-*/
-int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method,
- std::vector<vpCalibration> &table_cal,
- vpCameraParameters& cam_est,
- double &globalReprojectionError,
- bool verbose)
-{
- try{
- unsigned int nbPose = (unsigned int) table_cal.size();
- for(unsigned int i=0;i<nbPose;i++){
- if(table_cal[i].get_npt()>3)
- table_cal[i].computePose(cam_est,table_cal[i].cMo);
- }
- switch (method) {
- case CALIB_LAGRANGE : {
- if(nbPose > 1){
- std::cout << "this calibration method is not available in" << std::endl
- << "vpCalibration::computeCalibrationMulti()" << std::endl;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
- }
- break;
- }
- case CALIB_LAGRANGE_VIRTUAL_VS :
- 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;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
- }
- calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
- break ;
- }
- case CALIB_VIRTUAL_VS:
- case CALIB_VIRTUAL_VS_DIST: {
- calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
- break ;
- }
- }
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- cam_est.printParameters();
- }
-
- switch (method)
- {
- case CALIB_LAGRANGE :
- case CALIB_LAGRANGE_VIRTUAL_VS :
- case CALIB_VIRTUAL_VS:
- verbose = false ;
- break;
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST :
- case CALIB_VIRTUAL_VS_DIST:
- {
- if(verbose)
- std::cout << "Compute camera parameters with distortion"<<std::endl;
-
- calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
- }
- break ;
- }
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- table_cal[0].cam.printParameters();
- // std::cout << "Camera parameters with distortion:" << std::endl;
- cam_est.printParameters();
- std::cout<<std::endl;
- }
- return 0 ;
- }
- catch(...){ throw; }
-}
-
-/*!
- \brief Compute the multi-image calibration of effector-camera from R. Tsai and R. Lenz \cite Tsai89a.
-
- Compute extrinsic camera parameters : the constant transformation from
- the end-effector to the camera frame \f${^e}{\bf M}_c\f$ considering the camera model with or without distortion.
-
- \param[in] table_cal : Vector of vpCalibration that contains for each index a couple of
- \f${^r}{\bf M}_e\f$ (world to end-effector) and \f${^c}{\bf M}_o\f$ (camera to object)
- transformations.
- \param[out] eMc : Estimated pose of the camera in relation to the end-effector
- considering the camera model without distortion.
- \param[out] eMc_dist : Estimated pose of the camera in relation to the
- end-effector considering the model with distortion.
- \return 0 if the computation managed, -1 if less than three poses are provides as input.
-*/
-int vpCalibration::computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
- vpHomogeneousMatrix& eMc,
- vpHomogeneousMatrix& eMc_dist)
-{
- try{
- unsigned int nbPose = (unsigned int)table_cal.size();
- if (nbPose > 2){
- std::vector<vpHomogeneousMatrix> table_cMo(nbPose);
- std::vector<vpHomogeneousMatrix> table_cMo_dist(nbPose);
- std::vector<vpHomogeneousMatrix> table_rMe(nbPose);
-
- for(unsigned int i=0;i<nbPose;i++){
- table_cMo[i] = table_cal[i].cMo;
- table_cMo_dist[i] = table_cal[i].cMo_dist;
- table_rMe[i] = table_cal[i].rMe;
- }
- calibrationTsai(table_cMo, table_rMe, eMc);
- calibrationTsai(table_cMo_dist, table_rMe, eMc_dist);
-
- return 0;
- }
- else{
- vpERROR_TRACE("Three images are needed to compute Tsai calibration !\n");
- return -1;
- }
- }
- 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)
-{
- std::ofstream f(filename) ;
- vpImagePoint ip;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- f.precision(10);
- f.setf(std::ios::fixed,std::ios::floatfield);
- f << LoX.size() << std::endl ;
-
- for (unsigned int i =0 ; i < LoX.size() ; i++)
- {
-
- double oX = *it_LoX;
- double oY = *it_LoY;
- double oZ = *it_LoZ;
-
- ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
-
- f << oX <<" " << oY << " " << oZ << " " << u << " " << v <<std::endl ;
-
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
-
- }
-
- f.close() ;
- 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)
-{
- vpImagePoint ip;
- std::ifstream f;
- f.open(filename);
- if (! f.fail()){
- unsigned int n ;
- f >> n ;
- std::cout << "There are "<< n <<" point on the calibration grid " << std::endl ;
-
- clearPoint() ;
-
- if (n > 100000)
- throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
-
- for (unsigned int i=0 ; i < n ; i++)
- {
- double x, y, z, u, v ;
- f >> x >> y >> z >> u >> v ;
- std::cout << x <<" "<<y <<" "<<z<<" "<<u<<" "<<v <<std::endl ;
- ip.set_u( u );
- ip.set_v( v );
- addPoint(x, y, z, ip) ;
- }
-
- f.close() ;
- return 0 ;
- }
- 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<double> &oX, std::list<double> &oY, std::list<double> &oZ, bool verbose)
-{
- try{
- std::ifstream f;
- f.open(filename);
- if (! f.fail()){
-
- f >> n ;
- if(verbose)
- std::cout << "There are "<< n <<" points on the calibration grid " << std::endl ;
- int no_pt;
- double x,y,z;
-
- // clear the list
- oX.clear();
- oY.clear();
- oZ.clear();
-
- if (n > 100000)
- throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
-
- for (unsigned int i=0 ; i < n ; i++)
- {
- f >> no_pt >> x >> y >> z ;
- if(verbose){
- std::cout << no_pt <<std::endl ;
- std::cout << x <<" "<< y <<" "<< z <<std::endl ;
- }
- oX.push_back(x) ;
- oY.push_back(y) ;
- oZ.push_back(z) ;
- }
-
- f.close() ;
- }
- else{
- return -1;
- }
- }
- catch(...){return -1;}
- return 0 ;
-}
-
-/*!
- 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 vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color,
- unsigned int thickness, int subsampling_factor)
-{
-
- for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
- vpImagePoint ip = *it;
- if (subsampling_factor > 1.) {
- ip.set_u( ip.get_u() / subsampling_factor);
- ip.set_v( ip.get_v() / subsampling_factor);
- }
- vpDisplay::displayCross(I, ip, 12, color, thickness) ;
- }
- 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<unsigned char> &I, vpColor color,
- unsigned int thickness, int subsampling_factor)
-{
- double u0_dist = cam_dist.get_u0() / subsampling_factor;
- double v0_dist = cam_dist.get_v0() / subsampling_factor;
- double px_dist = cam_dist.get_px() / subsampling_factor;
- double py_dist = cam_dist.get_py() / subsampling_factor;
- double kud_dist = cam_dist.get_kud() ;
- // double kdu_dist = cam_dist.get_kdu() ;
-
- // double u0 = cam.get_u0() ;
- // double v0 = cam.get_v0() ;
- // double px = cam.get_px() ;
- // double py = cam.get_py() ;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
-
- for (unsigned int i =0 ; i < npt ; i++)
- {
- double oX = *it_LoX;
- 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) ;
-
- cX = oX*cMo_dist[0][0]+oY*cMo_dist[0][1]+oZ*cMo_dist[0][2]+cMo_dist[0][3];
- cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
- cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
-
- x = cX/cZ ;
- y = cY/cZ ;
-
- double r2 = 1+kud_dist*(vpMath::sqr(x)+vpMath::sqr(y)) ;
-
- vpImagePoint ip;
- ip.set_u( u0_dist + x*px_dist*r2 );
- ip.set_v( v0_dist + y*py_dist*r2 );
-
- vpDisplay::displayCross(I, ip, 6, color, thickness) ;
- ///////////////////////////////////////
-
- // std::cout << oX << " " << oY << " " <<oZ << std::endl ;
- // I.getClick() ;
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- }
- return 0;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- vpCalibration::computeCalibrationMulti(vpCalibrationMethodType, std::vector<vpCalibration> &,
- vpCameraParameters &, bool)
-
- Compute the multi-images calibration according to the desired method using many poses.
-
- \param method : Method used to estimate the camera parameters.
- \param nbPose : number of images used to compute multi-images calibration
- \param table_cal : array of vpCalibration.
- \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 0 if the computation was managed succeed.
-*/
-int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method,
- unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters& cam_est,
- bool verbose)
-{
- try{
- for(unsigned int i=0;i<nbPose;i++){
- if(table_cal[i].get_npt()>3)
- table_cal[i].computePose(cam_est,table_cal[i].cMo);
- }
- switch (method) {
- case CALIB_LAGRANGE : {
- if(nbPose > 1){
- std::cout << "this calibration method is not available in" << std::endl
- << "vpCalibration::computeCalibrationMulti()" << std::endl;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
- }
- break;
- }
- case CALIB_LAGRANGE_VIRTUAL_VS :
- 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;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
- }
- calibVVSMulti(nbPose, table_cal, cam_est, verbose);
- break ;
- }
- case CALIB_VIRTUAL_VS:
- case CALIB_VIRTUAL_VS_DIST: {
- calibVVSMulti(nbPose, table_cal, cam_est, verbose);
- break ;
- }
- }
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- cam_est.printParameters();
- }
-
- switch (method)
- {
- case CALIB_LAGRANGE :
- case CALIB_LAGRANGE_VIRTUAL_VS :
- case CALIB_VIRTUAL_VS:
- verbose = false ;
- break;
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST :
- case CALIB_VIRTUAL_VS_DIST:
- {
- if(verbose)
- std::cout << "Compute camera parameters with distortion"<<std::endl;
-
- calibVVSWithDistortionMulti(nbPose, table_cal, cam_est, verbose);
- }
- break ;
- }
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
- table_cal[0].cam.printParameters();
- // std::cout << "Camera parameters with distortion:" << std::endl;
- cam_est.printParameters();
- std::cout<<std::endl;
- }
- return 0 ;
- }
- catch(...){ throw; }
-}
-
-/*!
- \deprecated This method is deprecated. You should use
- vpCalibration::computeCalibrationTsai(std::vector<vpCalibration> &, vpHomogeneousMatrix &,
- vpHomogeneousMatrix &)
-
- \brief Compute the multi-image calibration of effector-camera from R. Tsai and R. Lenz \cite Tsai89a.
-
- Compute extrinsic camera parameters : the constant transformation from
- the effector to the camera coordinates (eMc).
-
- \param nbPose : number of images used to compute multi-images calibration
- \param table_cal : array of vpCalibration. All the vpHomogeneousMatrix cMo
- and rMe of each vpCalibration have to be initialized.
- \param eMc : output: estimated pose of the camera in relation to the effector
- (camera support) with the camera model without distortion.
- \param eMc_dist : output: estimated pose of the camera in relation to the
- effector (camera support) with the model with distortion.
- \return 0 if the computation managed, -1 if less than three poses are provides as input.
-*/
-int vpCalibration::computeCalibrationTsai(unsigned int nbPose,
- vpCalibration table_cal[],
- vpHomogeneousMatrix& eMc,
- vpHomogeneousMatrix& eMc_dist)
-{
- try{
- if (nbPose > 2){
- vpHomogeneousMatrix* table_cMo = new vpHomogeneousMatrix[nbPose];
- vpHomogeneousMatrix* table_cMo_dist = new vpHomogeneousMatrix[nbPose];
- vpHomogeneousMatrix* table_rMe = new vpHomogeneousMatrix[nbPose];
-
- for(unsigned int i=0;i<nbPose;i++){
- table_cMo[i] = table_cal[i].cMo;
- table_cMo_dist[i] = table_cal[i].cMo_dist;
- table_rMe[i] = table_cal[i].rMe;
- }
- calibrationTsai(nbPose,table_cMo,table_rMe,eMc);
- calibrationTsai(nbPose,table_cMo_dist,table_rMe,eMc_dist);
-
- delete [] table_cMo;
- delete [] table_cMo_dist;
- delete [] table_rMe;
-
- return 0;
- }
- else{
- vpERROR_TRACE("Three images are needed to compute Tsai calibration !\n");
- return -1;
- }
- }
- catch(...){
- throw;
- }
-}
-
-/*!
- \deprecated This method is deprecated. You should use
- vpCalibration::readGrid(const char*,unsigned int &, vpList<double> &, vpList<double> &, vpList<double> &, bool) instead.
-
- 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
-*/
-int vpCalibration::readGrid(const char* filename,unsigned int &n,
- vpList<double> &oX,vpList<double> &oY,vpList<double> &oZ, bool verbose)
-{
- try{
- std::ifstream f;
- f.open(filename);
- if (! f.fail()){
-
- f >> n ;
- if(verbose)
- std::cout << "There are "<< n <<" points on the calibration grid " << std::endl ;
- int no_pt;
- double x,y,z;
-
- oX.front();
- oY.front();
- oZ.front();
-
- if (n > 100000)
- throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
-
- for (unsigned int i=0 ; i < n ; i++)
- {
- f >> no_pt >> x >> y >> z ;
- if(verbose){
- std::cout << no_pt <<std::endl ;
- std::cout << x <<" "<< y <<" "<< z <<std::endl ;
- }
- oX.addRight(x) ;
- oY.addRight(y) ;
- oZ.addRight(z) ;
- }
-
- f.close() ;
- }
- else{
- return -1;
- }
- }
- catch(...){return -1;}
- return 0 ;
-}
-
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
diff --git a/src/camera/calibration/vpCalibration.h b/src/camera/calibration/vpCalibration.h
deleted file mode 100644
index 7be48a6..0000000
--- a/src/camera/calibration/vpCalibration.h
+++ /dev/null
@@ -1,237 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCalibration.h 4921 2014-10-09 08:19:29Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Camera calibration.
- *
- * Authors:
- * Eric Marchand
- * Francois Chaumette
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-/*!
- \file vpCalibration.h
- \brief Tools for camera calibration.
-
- \author Eric Marchand (INRIA) using code from Francois Chaumette (INRIA)
-
- \sa the example in calibrate.cpp
-*/
-#ifndef vpCalibration_h
-#define vpCalibration_h
-
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpMath.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpImage.h>
-#include <visp/vpCalibrationException.h>
-#include <visp/vpImagePoint.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-#include <list>
-#include <vector>
-/*!
- \class vpCalibration
-
- \ingroup Calibration
-
- \brief Tools for perspective camera calibration.
-
-*/
-class VISP_EXPORT vpCalibration
-{
-public:
- /*!
- 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 of the distortion (results are similar to Lowe approach). */
- CALIB_VIRTUAL_VS_DIST, /*!< Virtual visual servoing approach with estimation of the distortion. */
- CALIB_LAGRANGE_VIRTUAL_VS, /*!< Lagrange approach first, than virtual visual servoing approach, without estimation of the distortion. */
- CALIB_LAGRANGE_VIRTUAL_VS_DIST, /*!< Lagrange approach first, than virtual visual servoing approach, with estimation of the distortion. */
- } vpCalibrationMethodType ;
-
- vpHomogeneousMatrix cMo ; //!< the pose computed for the model without distortion
- //!< (as a 3x4 matrix [R T])
- vpHomogeneousMatrix cMo_dist ; //!< the pose computed for perspective projection
- //!< 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 relation to the effector
- vpHomogeneousMatrix eMc_dist;
-
-public:
- // Constructor
- vpCalibration() ;
- vpCalibration(const vpCalibration& c) ;
-
- // Destructor
- virtual ~vpCalibration() ;
-
- // Add a new point in this array
- int addPoint(double X, double Y, double Z, vpImagePoint &ip) ;
-
- // = operator
- vpCalibration& operator=(const vpCalibration& twinCalibration);
-
- static void calibrationTsai(std::vector<vpHomogeneousMatrix> &cMo,
- std::vector<vpHomogeneousMatrix> &rMe,
- vpHomogeneousMatrix &eMc);
-
- //! Suppress all the point in the array of point
- int clearPoint() ;
-
- void computeStdDeviation(double &deviation, double &deviation_dist);
- int computeCalibration(vpCalibrationMethodType method,
- vpHomogeneousMatrix &cMo_est,
- vpCameraParameters &cam_est,
- bool verbose = false) ;
- static int computeCalibrationMulti(vpCalibrationMethodType method,
- std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError,
- bool verbose = false) ;
-
- static int computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
- vpHomogeneousMatrix &eMc,
- vpHomogeneousMatrix &eMc_dist);
- double computeStdDeviation(const vpHomogeneousMatrix &cMo_est,
- const vpCameraParameters &camera);
- double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam);
- int displayData(vpImage<unsigned char> &I, vpColor color=vpColor::red,
- unsigned int thickness=1, int subsampling_factor=1) ;
- int displayGrid(vpImage<unsigned char> &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
- double getResidual(void) const {return residual;}
- //!get the residual for perspective projection with distortion (in pixels)
- double getResidual_dist(void) const {return residual_dist;}
- //!get the number of points
- unsigned int get_npt() const {return npt;}
-
- int init() ;
-
- int readData(const char *filename) ;
- static int readGrid(const char *filename,unsigned int &n,
- std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
- bool verbose = false);
-
- //!set the gain for the virtual visual servoing algorithm
- static void setLambda(const double &lambda){gain = lambda;}
- int writeData(const char *filename) ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- static void calibrationTsai(unsigned int nbPose, vpHomogeneousMatrix cMo[],
- vpHomogeneousMatrix rMe[],
- vpHomogeneousMatrix &eMc);
- static int computeCalibrationMulti(vpCalibrationMethodType method,unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam,
- bool verbose = false) ;
- vp_deprecated static int computeCalibrationTsai(unsigned int nbPose,
- vpCalibration table_cal[],
- vpHomogeneousMatrix &eMc,
- vpHomogeneousMatrix &eMc_dist);
- vp_deprecated static int readGrid(const char *filename,unsigned int &n,
- vpList<double> &oX,vpList<double> &oY,vpList<double> &oZ,
- bool verbose = false);
-#endif
-
-private:
- void computePose(const vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
- void calibLagrange( vpCameraParameters &cam , vpHomogeneousMatrix &cMo) ;
-
- //! Compute the calibration using virtual visual servoing approach
- void calibVVS( vpCameraParameters &cam , vpHomogeneousMatrix &cMo,
- bool verbose = false) ;
-
- static void calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[],
- vpCameraParameters &cam, bool verbose = false);
- static void calibVVSMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError, bool verbose = false) ;
- void calibVVSWithDistortion( vpCameraParameters &cam,
- vpHomogeneousMatrix &cMo,
- bool verbose = false) ;
- static void calibVVSWithDistortionMulti( unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam,
- bool verbose = false );
- static void calibVVSWithDistortionMulti( std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError,
- bool verbose = false);
-
-private:
- unsigned int npt ; //!< number of points used in calibration computation
- std::list<double> LoX, LoY, LoZ ; //!< list of points coordinates (3D in meters)
- std::list<vpImagePoint> Lip ; //!< list of points coordinates (2D in pixels)
-
- double residual ; //!< residual in pixel for camera model without distortion
- double residual_dist ; //!< residual in pixel for perspective projection with distortion model
-
- static double threshold;
- static unsigned int nbIterMax;
- static double gain;
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/camera/calibration/vpCalibrationException.h b/src/camera/calibration/vpCalibrationException.h
deleted file mode 100644
index 1772420..0000000
--- a/src/camera/calibration/vpCalibrationException.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCalibrationException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpCalibration class and its derivates.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#ifndef __vpCalibrationException_H
-#define __vpCalibrationException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* Classes standards. */
-//
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- \class vpCalibrationException
-
- \ingroup Exception
-
- \brief Error that can be emited by the vpCalibration class.
- */
-class VISP_EXPORT vpCalibrationException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpCalibration member
- */
- enum errorCodeEnum
- {
- //! error returns by a constructor
- constructionError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedError,
- //! index out of range
- outOfRangeError,
- //! iterative algorithm doesn't converge
- convergencyError,
- forbiddenOperatorError,
- } ;
-
- public:
- vpCalibrationException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpCalibrationException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpCalibrationException (const int id)
- : vpException(id){ ; }
-};
-
-#endif
diff --git a/src/camera/calibration/vpCalibrationTools.cpp b/src/camera/calibration/vpCalibrationTools.cpp
deleted file mode 100644
index 3723060..0000000
--- a/src/camera/calibration/vpCalibrationTools.cpp
+++ /dev/null
@@ -1,2009 +0,0 @@
-#include <visp/vpCalibration.h>
-#include <visp/vpMath.h>
-#include <visp/vpPose.h>
-#include <visp/vpPixelMeterConversion.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#undef MAX
-#undef MIN
-
-void
-vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est)
-{
-
- vpMatrix A(2*npt,3) ;
- vpMatrix B(2*npt,9) ;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- vpImagePoint ip;
-
- for (unsigned int i = 0 ; i < npt ; i++)
- {
-
- double x0 = *it_LoX;
- double y0 = *it_LoY;
- double z0 = *it_LoZ;
-
- ip = *it_Lip;
-
- double xi = ip.get_u() ;
- double yi = ip.get_v() ;
-
- A[2*i][0] = x0*xi;
- A[2*i][1] = y0*xi;
- A[2*i][2] = z0*xi;
- B[2*i][0] = -x0;
- B[2*i][1] = -y0;
- B[2*i][2] = -z0;
- B[2*i][3] = 0.0;
- B[2*i][4] = 0.0;
- B[2*i][5] = 0.0;
- B[2*i][6] = -1.0;
- B[2*i][7] = 0.0;
- B[2*i][8] = xi;
- A[2*i+1][0] = x0*yi;
- A[2*i+1][1] = y0*yi;
- A[2*i+1][2] = z0*yi;
- B[2*i+1][0] = 0.0;
- B[2*i+1][1] = 0.0;
- B[2*i+1][2] = 0.0;
- B[2*i+1][3] = -x0;
- B[2*i+1][4] = -y0;
- B[2*i+1][5] = -z0;
- B[2*i+1][6] = 0.0;
- B[2*i+1][7] = -1.0;
- B[2*i+1][8] = yi;
-
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
- }
-
- vpMatrix BtB ; /* compute B^T B */
- BtB = B.t() * B ;
-
- /* compute (B^T B)^(-1) */
- /* input : btb (dimension 9 x 9) = (B^T B) */
- /* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
-
- vpMatrix BtBinv ;
- BtBinv = BtB.pseudoInverse(1e-16) ;
-
- vpMatrix BtA ;
- BtA = B.t()*A ; /* compute B^T A */
-
-
- vpMatrix r ;
- r = BtBinv*BtA ; /* compute (B^T B)^(-1) B^T A */
-
- vpMatrix e ; /* compute - A^T B (B^T B)^(-1) B^T A*/
- e = -(A.t()*B)*r ;
-
- vpMatrix AtA ; /* compute A^T A */
- AtA = A.AtA() ;
-
- e += AtA ; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
-
- vpColVector x1(3) ;
- vpColVector x2 ;
-
- e.svd(x1,AtA) ;// destructive on e
- // eigenvector computation of E corresponding to the min eigenvalue.
- /* SVmax computation*/
- double svm = 0.0;
- unsigned int imin = 1;
- for (unsigned int i=0;i<x1.getRows();i++)
- {
- if (x1[i] > svm)
- {
- svm = x1[i];
- imin = i;
- }
- }
-
- svm *= 0.1; /* for the rank */
-
- for (unsigned int i=0;i<x1.getRows();i++)
- {
- if (x1[i] < x1[imin]) imin = i;
- }
-
- for (unsigned int i=0;i<x1.getRows();i++)
- x1[i] = AtA[i][imin];
-
- x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
-
-
- vpColVector sol(12) ;
- vpColVector resul(7) ;
- for (unsigned int i=0;i<3;i++) sol[i] = x1[i]; /* X_1 */
- for (unsigned int i=0;i<9;i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
- {
- sol[i+3] = x2[i];
- }
-
- if (sol[11] < 0.0) for (unsigned int i=0;i<12;i++) sol[i] = -sol[i]; /* since Z0 > 0 */
-
- resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2]; /* u0 */
-
- resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2]; /* v0 */
-
- resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5] /* px */
- -resul[0]*resul[0]);
- resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8] /* py */
- -resul[1]*resul[1]);
-
- cam_est.initPersProjWithoutDistortion(resul[2],resul[3],resul[0],resul[1]);
-
- resul[4] = (sol[9]-sol[11]*resul[0])/resul[2]; /* X0 */
- resul[5] = (sol[10]-sol[11]*resul[1])/resul[3]; /* Y0 */
- resul[6] = sol[11]; /* Z0 */
-
- vpMatrix rd(3,3) ;
- /* fill rotation matrix */
- for (unsigned int i=0;i<3;i++) rd[0][i] = (sol[i+3]-sol[i]*resul[0])/resul[2];
- for (unsigned int i=0;i<3;i++) rd[1][i] = (sol[i+6]-sol[i]*resul[1])/resul[3];
- for (unsigned int i=0;i<3;i++) rd[2][i] = sol[i];
-
- // std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
- // std::cout << rd*rd.t() ;
-
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- for (unsigned int j=0 ; j < 3 ; j++)
- cMo_est[i][j] = rd[i][j];
- }
- for (unsigned int i=0 ; i < 3 ; i++) cMo_est[i][3] = resul[i+4] ;
-
- this->cMo = cMo_est ;
- this->cMo_dist = cMo_est;
-
- double deviation,deviation_dist;
- this->computeStdDeviation(deviation,deviation_dist);
-
-}
-
-
-void
-vpCalibration::calibVVS(vpCameraParameters &cam_est,
- vpHomogeneousMatrix &cMo_est,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int n_points = npt ;
-
- vpColVector oX(n_points), cX(n_points) ;
- vpColVector oY(n_points), cY(n_points) ;
- vpColVector oZ(n_points), cZ(n_points) ;
- vpColVector u(n_points) ;
- vpColVector v(n_points) ;
-
- vpColVector P(2*n_points) ;
- vpColVector Pd(2*n_points) ;
-
- vpImagePoint ip;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- for (unsigned int i =0 ; i < n_points ; i++)
- {
- oX[i] = *it_LoX;
- oY[i] = *it_LoY;
- oZ[i] = *it_LoZ;
-
- ip = *it_Lip;
-
- u[i] = ip.get_u() ;
- v[i] = ip.get_v() ;
-
- ++it_LoX;
- ++it_LoY;
- ++it_LoZ;
- ++it_Lip;
- }
-
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
-
- double px = cam_est.get_px();
- double py = cam_est.get_py();
- double u0 = cam_est.get_u0();
- double v0 = cam_est.get_v0();
-
- r = 0 ;
-
- for (unsigned int i=0 ; i < n_points; i++)
- {
- cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
- cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
- cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
-
- Pd[2*i] = u[i] ;
- Pd[2*i+1] = v[i] ;
-
- P[2*i] = cX[i]/cZ[i]*px + u0 ;
- P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
-
- r += ((vpMath::sqr(P[2*i]-Pd[2*i]) + vpMath::sqr(P[2*i+1]-Pd[2*i+1]))) ;
- }
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/n_points ;
-
- vpMatrix L(n_points*2,10) ;
- for (unsigned int i=0 ; i < n_points; i++)
- {
- double x = cX[i] ;
- double y = cY[i] ;
- double z = cZ[i] ;
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- //---------------
- {
- L[2*i][0] = px * (-inv_z) ;
- L[2*i][1] = 0 ;
- L[2*i][2] = px*X*inv_z ;
- L[2*i][3] = px*X*Y ;
- L[2*i][4] = -px*(1+X*X) ;
- L[2*i][5] = px*Y ;
- }
- {
- L[2*i][6]= 1 ;
- L[2*i][7]= 0 ;
- L[2*i][8]= X ;
- L[2*i][9]= 0;
- }
- {
- L[2*i+1][0] = 0 ;
- L[2*i+1][1] = py*(-inv_z) ;
- L[2*i+1][2] = py*(Y*inv_z) ;
- L[2*i+1][3] = py* (1+Y*Y) ;
- L[2*i+1][4] = -py*X*Y ;
- L[2*i+1][5] = -py*X ;
- }
- {
- L[2*i+1][6]= 0 ;
- L[2*i+1][7]= 1 ;
- L[2*i+1][8]= 0;
- L[2*i+1][9]= Y ;
- }
- } // end interaction
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
-
- vpColVector e ;
- e = Lp*error ;
-
- vpColVector Tc, Tc_v(6) ;
- Tc = -e*gain ;
-
- // Tc_v =0 ;
- for (unsigned int i=0 ; i <6 ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithoutDistortion(px+Tc[8],py+Tc[9],
- u0+Tc[6],v0+Tc[7]) ;
-
- cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
- if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
- this->cMo = cMo_est;
- this->cMo_dist = cMo_est;
- this->residual = r;
- this->residual_dist = r;
- if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-
-void
-vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam_est,
- double &globalReprojectionError,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int nbPoint[256]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
- unsigned int nbPose = (unsigned int)table_cal.size();
- unsigned int nbPose6 = 6*nbPose;
-
- for (unsigned int i=0; i<nbPose ; i++)
- {
- nbPoint[i] = table_cal[i].npt;
- nbPointTotal += nbPoint[i];
- }
-
- if (nbPointTotal < 4) {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
- }
-
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
-
- vpColVector P(2*nbPointTotal) ;
- vpColVector Pd(2*nbPointTotal) ;
- vpImagePoint ip;
-
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
- std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
- std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
-
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
-
- ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
-
- curPoint++;
- }
- }
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
-
- iter++ ;
- residu_1 = r ;
-
- double px = cam_est.get_px();
- double py = cam_est.get_py();
- double u0 = cam_est.get_u0();
- double v0 = cam_est.get_v0();
-
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
-
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
-
- Pd[curPoint2] = u[curPoint] ;
- Pd[curPoint2+1] = v[curPoint] ;
-
- P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
- P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
-
- r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
- + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
- curPoint++;
- }
- }
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
-
- vpMatrix L(nbPointTotal*2,nbPose6+4) ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
- unsigned int curPoint21 = curPoint2 + 1;
-
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
-
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- //---------------
- {
- {
- L[curPoint2][q] = px * (-inv_z) ;
- L[curPoint2][q+1] = 0 ;
- L[curPoint2][q+2] = px*(X*inv_z) ;
- L[curPoint2][q+3] = px*X*Y ;
- L[curPoint2][q+4] = -px*(1+X*X) ;
- L[curPoint2][q+5] = px*Y ;
- }
- {
- L[curPoint2][nbPose6]= 1 ;
- L[curPoint2][nbPose6+1]= 0 ;
- L[curPoint2][nbPose6+2]= X ;
- L[curPoint2][nbPose6+3]= 0;
- }
- {
- L[curPoint21][q] = 0 ;
- L[curPoint21][q+1] = py*(-inv_z) ;
- L[curPoint21][q+2] = py*(Y*inv_z) ;
- L[curPoint21][q+3] = py* (1+Y*Y) ;
- L[curPoint21][q+4] = -py*X*Y ;
- L[curPoint21][q+5] = -py*X ;
- }
- {
- L[curPoint21][nbPose6]= 0 ;
- L[curPoint21][nbPose6+1]= 1 ;
- L[curPoint21][nbPose6+2]= 0;
- L[curPoint21][nbPose6+3]= Y ;
- }
-
- }
- curPoint++;
- } // end interaction
- }
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
-
- vpColVector e ;
- e = Lp*error ;
-
- vpColVector Tc, Tc_v(nbPose6) ;
- Tc = -e*gain ;
-
- // Tc_v =0 ;
- for (unsigned int i = 0 ; i < nbPose6 ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
- py+Tc[nbPose6+3],
- u0+Tc[nbPose6],
- v0+Tc[nbPose6+1]) ;
-
- // cam.setKd(get_kd() + Tc[10]) ;
- vpColVector Tc_v_Tmp(6) ;
-
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
- * table_cal[p].cMo;
- }
-
- if (verbose)
- std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cMo_dist = table_cal[p].cMo ;
- table_cal[p].cam = cam_est;
- table_cal[p].cam_dist = cam_est;
- double deviation,deviation_dist;
- table_cal[p].computeStdDeviation(deviation,deviation_dist);
- }
- globalReprojectionError = sqrt(r/nbPointTotal);
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-
-void
-vpCalibration::calibVVSWithDistortion(vpCameraParameters& cam_est,
- vpHomogeneousMatrix& cMo_est,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int n_points =npt ;
-
- vpColVector oX(n_points), cX(n_points) ;
- vpColVector oY(n_points), cY(n_points) ;
- vpColVector oZ(n_points), cZ(n_points) ;
- vpColVector u(n_points) ;
- vpColVector v(n_points) ;
-
- vpColVector P(4*n_points) ;
- vpColVector Pd(4*n_points) ;
-
- std::list<double>::const_iterator it_LoX = LoX.begin();
- std::list<double>::const_iterator it_LoY = LoY.begin();
- std::list<double>::const_iterator it_LoZ = LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
- vpImagePoint ip;
-
- for (unsigned int i =0 ; i < n_points ; i++)
- {
- oX[i] = *it_LoX;
- oY[i] = *it_LoY;
- oZ[i] = *it_LoZ;
-
- ip = *it_Lip;
- u[i] = ip.get_u();
- v[i] = ip.get_v();
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
- }
-
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
-
- r = 0 ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
-
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
-
- double inv_px = 1/px ;
- double inv_py = 1/py ;
-
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
-
- double k2ud = 2*kud;
- double k2du = 2*kdu;
- vpMatrix L(n_points*4,12) ;
-
- for (unsigned int i=0 ; i < n_points; i++)
- {
- unsigned int i4 = 4*i;
- unsigned int i41 = 4*i+1;
- unsigned int i42 = 4*i+2;
- unsigned int i43 = 4*i+3;
-
- cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
- cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
- cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
-
- double x = cX[i] ;
- double y = cY[i] ;
- double z = cZ[i] ;
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
-
- double up = u[i] ;
- double vp = v[i] ;
-
- Pd[i4] = up ;
- Pd[i41] = vp ;
-
- double up0 = up - u0;
- double vp0 = vp - v0;
-
- double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
-
- double yp0 = vp0 * inv_py;
- double yp02 = yp0 * yp0;
-
- double r2du = xp02 + yp02 ;
- double kr2du = kdu * r2du;
-
- P[i4] = u0 + px*X - kr2du *(up0) ;
- P[i41] = v0 + py*Y - kr2du *(vp0) ;
-
- double r2ud = X2 + Y2 ;
- double kr2ud = 1 + kud * r2ud;
-
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
-
- Pd[i42] = up ;
- Pd[i43] = vp ;
-
- P[i42] = u0 + px*X*kr2ud ;
- P[i43] = v0 + py*Y*kr2ud ;
-
- r += (vpMath::sqr(P[i4]-Pd[i4]) +
- vpMath::sqr(P[i41]-Pd[i41]) +
- vpMath::sqr(P[i42]-Pd[i42]) +
- vpMath::sqr(P[i43]-Pd[i43]))*0.5;
-
- //--distorted to undistorted
- {
- {
- L[i4][0] = px * (-inv_z) ;
- L[i4][1] = 0 ;
- L[i4][2] = px*X*inv_z ;
- L[i4][3] = px*X*Y ;
- L[i4][4] = -px*(1+X2) ;
- L[i4][5] = px*Y ;
- }
- {
- L[i4][6]= 1 + kr2du + k2du*xp02 ;
- L[i4][7]= k2du*up0*yp0*inv_py ;
- L[i4][8]= X + k2du*xp02*xp0 ;
- L[i4][9]= k2du*up0*yp02*inv_py ;
- L[i4][10] = -(up0)*(r2du) ;
- L[i4][11] = 0 ;
- }
- {
- L[i41][0] = 0 ;
- L[i41][1] = py*(-inv_z) ;
- L[i41][2] = py*Y*inv_z ;
- L[i41][3] = py* (1+Y2) ;
- L[i41][4] = -py*XY ;
- L[i41][5] = -py*X ;
- }
- {
- L[i41][6]= k2du*xp0*vp0*inv_px ;
- L[i41][7]= 1 + kr2du + k2du*yp02;
- L[i41][8]= k2du*vp0*xp02*inv_px;
- L[i41][9]= Y + k2du*yp02*yp0;
- L[i41][10] = -vp0*r2du ;
- L[i41][11] = 0 ;
- }
- //---undistorted to distorted
- {
- L[i42][0] = Axx*(-inv_z) ;
- L[i42][1] = Axy*(-inv_z) ;
- L[i42][2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[i42][3] = Axx*X*Y + Axy*(1+Y2);
- L[i42][4] = -Axx*(1+X2) - Axy*XY;
- L[i42][5] = Axx*Y -Axy*X;
- }
- {
- L[i42][6]= 1 ;
- L[i42][7]= 0 ;
- L[i42][8]= X*kr2ud ;
- L[i42][9]= 0;
- L[i42][10] = 0 ;
- L[i42][11] = px*X*r2ud ;
- }
- {
- L[i43][0] = Ayx*(-inv_z) ;
- L[i43][1] = Ayy*(-inv_z) ;
- L[i43][2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[i43][3] = Ayx*XY + Ayy*(1+Y2) ;
- L[i43][4] = -Ayx*(1+X2) -Ayy*XY ;
- L[i43][5] = Ayx*Y -Ayy*X;
- }
- {
- L[i43][6]= 0 ;
- L[i43][7]= 1;
- L[i43][8]= 0;
- L[i43][9]= Y*kr2ud ;
- L[i43][10] = 0 ;
- L[i43][11] = py*Y*r2ud ;
- }
- } // end interaction
- } // end interaction
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/n_points ;
-
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
-
- vpColVector e ;
- e = Lp*error ;
-
- vpColVector Tc, Tc_v(6) ;
- Tc = -e*gain ;
-
- for (unsigned int i=0 ; i <6 ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9],
- u0 + Tc[6], v0 + Tc[7],
- kud + Tc[11],
- kdu + Tc[10]);
-
- cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
- if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
- this->residual_dist = r;
- this->cMo_dist = cMo_est ;
- this->cam_dist = cam_est ;
-
- if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
-
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-
-
-void
-vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam_est, double &globalReprojectionError,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int nbPoint[1024]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
- unsigned int nbPose = (unsigned int)table_cal.size();
- unsigned int nbPose6 = 6*nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
- nbPoint[i] = table_cal[i].npt;
- nbPointTotal += nbPoint[i];
- }
-
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
- }
-
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
-
- vpColVector P(4*nbPointTotal) ;
- vpColVector Pd(4*nbPointTotal) ;
- vpImagePoint ip;
-
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
- std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
- std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
-
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
-
- ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
- curPoint++;
- }
- }
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
-
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
-
- curPoint++;
- }
- }
-
-
- vpMatrix L(nbPointTotal*4,nbPose6+6) ;
- curPoint = 0 ; //current point indice
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
-
- double inv_px = 1/px ;
- double inv_py = 1/py ;
-
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
-
- double k2ud = 2*kud;
- double k2du = 2*kdu;
-
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint4 = 4*curPoint;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
-
- double inv_z = 1/z;
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
-
- double up = u[curPoint] ;
- double vp = v[curPoint] ;
-
- Pd[curPoint4] = up ;
- Pd[curPoint4+1] = vp ;
-
- double up0 = up - u0;
- double vp0 = vp - v0;
-
- double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
-
- double yp0 = vp0 * inv_py;
- double yp02 = yp0 * yp0;
-
- double r2du = xp02 + yp02 ;
- double kr2du = kdu * r2du;
-
- P[curPoint4] = u0 + px*X - kr2du *(up0) ;
- P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
-
- double r2ud = X2 + Y2 ;
- double kr2ud = 1 + kud * r2ud;
-
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
-
- Pd[curPoint4+2] = up ;
- Pd[curPoint4+3] = vp ;
-
- P[curPoint4+2] = u0 + px*X*kr2ud ;
- P[curPoint4+3] = v0 + py*Y*kr2ud ;
-
- r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
- vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
- vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
- vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
-
- unsigned int curInd = curPoint4;
- //---------------
- {
- {
- L[curInd][q] = px * (-inv_z) ;
- L[curInd][q+1] = 0 ;
- L[curInd][q+2] = px*X*inv_z ;
- L[curInd][q+3] = px*X*Y ;
- L[curInd][q+4] = -px*(1+X2) ;
- L[curInd][q+5] = px*Y ;
- }
- {
- L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
- L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
- L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
- L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
- L[curInd][nbPose6+4] = -(up0)*(r2du) ;
- L[curInd][nbPose6+5] = 0 ;
- }
- curInd++;
- {
- L[curInd][q] = 0 ;
- L[curInd][q+1] = py*(-inv_z) ;
- L[curInd][q+2] = py*Y*inv_z ;
- L[curInd][q+3] = py* (1+Y2) ;
- L[curInd][q+4] = -py*XY ;
- L[curInd][q+5] = -py*X ;
- }
- {
- L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
- L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
- L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
- L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
- L[curInd][nbPose6+4] = -vp0*r2du ;
- L[curInd][nbPose6+5] = 0 ;
- }
- curInd++;
- //---undistorted to distorted
- {
- L[curInd][q] = Axx*(-inv_z) ;
- L[curInd][q+1] = Axy*(-inv_z) ;
- L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
- L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
- L[curInd][q+5] = Axx*Y -Axy*X;
- }
- {
- L[curInd][nbPose6]= 1 ;
- L[curInd][nbPose6+1]= 0 ;
- L[curInd][nbPose6+2]= X*kr2ud ;
- L[curInd][nbPose6+3]= 0;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = px*X*r2ud ;
- }
- curInd++;
- {
- L[curInd][q] = Ayx*(-inv_z) ;
- L[curInd][q+1] = Ayy*(-inv_z) ;
- L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
- L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
- L[curInd][q+5] = Ayx*Y -Ayy*X;
- }
- {
- L[curInd][nbPose6]= 0 ;
- L[curInd][nbPose6+1]= 1;
- L[curInd][nbPose6+2]= 0;
- L[curInd][nbPose6+3]= Y*kr2ud ;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = py*Y*r2ud ;
- }
- } // end interaction
- curPoint++;
- } // end interaction
- }
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
-
- vpMatrix Lp ;
- /*double rank =*/
- L.pseudoInverse(Lp,1e-10) ;
- vpColVector e ;
- e = Lp*error ;
- vpColVector Tc, Tc_v(6*nbPose) ;
- Tc = -e*gain ;
- for (unsigned int i = 0 ; i < 6*nbPose ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
- u0+Tc[nbPose6], v0+Tc[nbPose6+1],
- kud + Tc[nbPose6+5],
- kdu + Tc[nbPose6+4]);
-
- vpColVector Tc_v_Tmp(6) ;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
- * table_cal[p].cMo_dist;
- }
- if (verbose)
- std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
- //std::cout << " residual: " << r << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
-
- double perViewError;
- double totalError = 0;
- int totalPoints = 0;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cam_dist = cam_est ;
- perViewError = table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
- totalError += perViewError*perViewError * table_cal[p].npt;
- totalPoints += (int)table_cal[p].npt;
- }
- globalReprojectionError = sqrt(r/(nbPointTotal));
-
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated Calibration method of effector-camera from R. Tsai and R. Lorenz \cite Tsai89a.
-
- Compute extrinsic camera parameters : the constant transformation from
- the effector to the camera coordinates (eMc).
-
- \param nbPose : number of different positions (input)
- \param cMo : table of homogeneous matrices representing the transformation
- between the camera and the scene (input)
- \param rMe : table of homogeneous matrices representing the transformation
- between the effector (where the camera is fixed) and the reference coordinates
- (base of the manipulator) (input)
- \param eMc : homogeneous matrix representing the transformation
- between the effector and the camera (output)
-*/
-void
-vpCalibration::calibrationTsai(unsigned int nbPose,
- vpHomogeneousMatrix cMo[],
- vpHomogeneousMatrix rMe[],
- vpHomogeneousMatrix &eMc)
-{
-
- vpColVector x ;
- {
- vpMatrix A ;
- vpColVector B ;
- unsigned int k = 0 ;
- // for all couples ij
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
- {
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
- //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
-
- vpRotationMatrix rReij = rRej.t() * rRei;
-
- vpRotationMatrix cijRo = cjRo * ciRo.t();
-
- vpThetaUVector rPeij(rReij);
-
- double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
- + rPeij[2]*rPeij[2]);
-
- for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
-
- vpThetaUVector cijPo(cijRo) ;
- theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
- + cijPo[2]*cijPo[2]);
- for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
-
- vpMatrix As;
- vpColVector b(3) ;
-
- As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
-
- b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
-
- if (k==0)
- {
- A = As ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stackMatrices(A,As) ;
- B = vpMatrix::stackMatrices(B,b) ;
- }
- k++ ;
- }
- }
- }
-
- // the linear system is defined
- // x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
-
- vpMatrix Ap ;
- AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
- x = Ap*A.t()*B ;
-
-// {
-// // Residual
-// vpColVector residual;
-// residual = A*x-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
-
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("Mean residual = %lf\n",res);
-// }
-
- // extraction of theta and U
- double theta ;
- double d=x.sumSquare() ;
- for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
- theta = sqrt(x.sumSquare())/2 ;
- theta = 2*asin(theta) ;
- //if (theta !=0)
- if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
- {
- for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
- }
- else
- x = 0 ;
- }
-
- // Building of the rotation matrix eRc
- vpThetaUVector xP(x[0],x[1],x[2]);
- vpRotationMatrix eRc(xP);
-
- {
- vpMatrix A ;
- vpColVector B ;
- // Building of the system for the translation estimation
- // for all couples ij
- vpRotationMatrix I3 ;
- I3.setIdentity() ;
- int k = 0 ;
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- vpTranslationVector rTei, ciTo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- rMe[i].extract(rTei) ;
- cMo[i].extract(ciTo) ;
-
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
- {
-
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
-
- vpTranslationVector rTej, cjTo ;
- rMe[j].extract(rTej) ;
- cMo[j].extract(cjTo) ;
-
- vpRotationMatrix rReij = rRej.t() * rRei ;
-
- vpTranslationVector rTeij = rTej+ (-rTei);
-
- rTeij = rRej.t()*rTeij ;
-
- vpMatrix a ;
- a = (vpMatrix)rReij - (vpMatrix)I3 ;
-
- vpTranslationVector b ;
- b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
-
- if (k==0)
- {
- A = a ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stackMatrices(A,a) ;
- B = vpMatrix::stackMatrices(B,b) ;
- }
- k++ ;
- }
- }
- }
-
- // the linear system is solved
- // x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
- vpMatrix Ap ;
- vpColVector AeTc ;
- AtA.pseudoInverse(Ap, 1e-6) ;
- AeTc = Ap*A.t()*B ;
-
-// {
-// // residual
-// vpColVector residual;
-// residual = A*AeTc-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("mean residual = %lf\n",res);
-// }
-
- vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
-
- eMc.insert(eTc) ;
- eMc.insert(eRc) ;
- }
-}
-#endif
-
-/*!
- \brief calibration method of effector-camera from R. Tsai and R. Lorenz \cite Tsai89a.
-
- Compute extrinsic camera parameters : the constant transformation from
- the effector to the camera coordinates (eMc).
-
- \param cMo : vector of homogeneous matrices representing the transformation
- between the camera and the scene (input)
- \param rMe : vector of homogeneous matrices representing the transformation
- between the effector (where the camera is fixed) and the reference coordinates
- (base of the manipulator) (input). Must be the same size as cMo.
- \param eMc : homogeneous matrix representing the transformation
- between the effector and the camera (output)
-*/
-void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
- std::vector<vpHomogeneousMatrix>& rMe,
- vpHomogeneousMatrix &eMc){
-
- vpColVector x ;
- unsigned int nbPose = (unsigned int)cMo.size();
- if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
- {
- vpMatrix A ;
- vpColVector B ;
- unsigned int k = 0 ;
- // for all couples ij
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
- {
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
- //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
-
- vpRotationMatrix rReij = rRej.t() * rRei;
-
- vpRotationMatrix cijRo = cjRo * ciRo.t();
-
- vpThetaUVector rPeij(rReij);
-
- double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
- + rPeij[2]*rPeij[2]);
-
- for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
-
- vpThetaUVector cijPo(cijRo) ;
- theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
- + cijPo[2]*cijPo[2]);
- for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
-
- vpMatrix As;
- vpColVector b(3) ;
-
- As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
-
- b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
-
- if (k==0)
- {
- A = As ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stackMatrices(A,As) ;
- B = vpMatrix::stackMatrices(B,b) ;
- }
- k++ ;
- }
- }
- }
-
- // the linear system is defined
- // x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
-
- vpMatrix Ap ;
- AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
- x = Ap*A.t()*B ;
-
-// {
-// // Residual
-// vpColVector residual;
-// residual = A*x-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
-
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("Mean residual = %lf\n",res);
-// }
-
- // extraction of theta and U
- double theta ;
- double d=x.sumSquare() ;
- for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
- theta = sqrt(x.sumSquare())/2 ;
- theta = 2*asin(theta) ;
- //if (theta !=0)
- if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
- {
- for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
- }
- else
- x = 0 ;
- }
-
- // Building of the rotation matrix eRc
- vpThetaUVector xP(x[0],x[1],x[2]);
- vpRotationMatrix eRc(xP);
-
- {
- vpMatrix A ;
- vpColVector B ;
- // Building of the system for the translation estimation
- // for all couples ij
- vpRotationMatrix I3 ;
- I3.setIdentity() ;
- int k = 0 ;
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- vpTranslationVector rTei, ciTo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- rMe[i].extract(rTei) ;
- cMo[i].extract(ciTo) ;
-
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
- {
-
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
-
- vpTranslationVector rTej, cjTo ;
- rMe[j].extract(rTej) ;
- cMo[j].extract(cjTo) ;
-
- vpRotationMatrix rReij = rRej.t() * rRei ;
-
- vpTranslationVector rTeij = rTej+ (-rTei);
-
- rTeij = rRej.t()*rTeij ;
-
- vpMatrix a ;
- a = (vpMatrix)rReij - (vpMatrix)I3 ;
-
- vpTranslationVector b ;
- b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
-
- if (k==0)
- {
- A = a ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stackMatrices(A,a) ;
- B = vpMatrix::stackMatrices(B,b) ;
- }
- k++ ;
- }
- }
- }
-
- // the linear system is solved
- // x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
- vpMatrix Ap ;
- vpColVector AeTc ;
- AtA.pseudoInverse(Ap, 1e-6) ;
- AeTc = Ap*A.t()*B ;
-
-// {
-// // residual
-// vpColVector residual;
-// residual = A*AeTc-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("mean residual = %lf\n",res);
-// }
-
- vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
-
- eMc.insert(eTc) ;
- eMc.insert(eRc) ;
- }
-}
-
-
-void
-vpCalibration::calibVVSMulti(unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam_est,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int nbPoint[256]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
-
- unsigned int nbPose6 = 6*nbPose;
-
- for (unsigned int i=0; i<nbPose ; i++)
- {
- nbPoint[i] = table_cal[i].npt;
- nbPointTotal += nbPoint[i];
- }
-
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
- }
-
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
-
- vpColVector P(2*nbPointTotal) ;
- vpColVector Pd(2*nbPointTotal) ;
- vpImagePoint ip;
-
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
- std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
- std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
-
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
-
- ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
-
- curPoint++;
- }
- }
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
-
- iter++ ;
- residu_1 = r ;
-
- double px = cam_est.get_px();
- double py = cam_est.get_py();
- double u0 = cam_est.get_u0();
- double v0 = cam_est.get_v0();
-
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
-
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
-
- Pd[curPoint2] = u[curPoint] ;
- Pd[curPoint2+1] = v[curPoint] ;
-
- P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
- P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
-
- r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
- + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
- curPoint++;
- }
- }
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
-
- vpMatrix L(nbPointTotal*2,nbPose6+4) ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
- unsigned int curPoint21 = curPoint2 + 1;
-
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
-
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- //---------------
- {
- {
- L[curPoint2][q] = px * (-inv_z) ;
- L[curPoint2][q+1] = 0 ;
- L[curPoint2][q+2] = px*(X*inv_z) ;
- L[curPoint2][q+3] = px*X*Y ;
- L[curPoint2][q+4] = -px*(1+X*X) ;
- L[curPoint2][q+5] = px*Y ;
- }
- {
- L[curPoint2][nbPose6]= 1 ;
- L[curPoint2][nbPose6+1]= 0 ;
- L[curPoint2][nbPose6+2]= X ;
- L[curPoint2][nbPose6+3]= 0;
- }
- {
- L[curPoint21][q] = 0 ;
- L[curPoint21][q+1] = py*(-inv_z) ;
- L[curPoint21][q+2] = py*(Y*inv_z) ;
- L[curPoint21][q+3] = py* (1+Y*Y) ;
- L[curPoint21][q+4] = -py*X*Y ;
- L[curPoint21][q+5] = -py*X ;
- }
- {
- L[curPoint21][nbPose6]= 0 ;
- L[curPoint21][nbPose6+1]= 1 ;
- L[curPoint21][nbPose6+2]= 0;
- L[curPoint21][nbPose6+3]= Y ;
- }
-
- }
- curPoint++;
- } // end interaction
- }
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
-
- vpColVector e ;
- e = Lp*error ;
-
- vpColVector Tc, Tc_v(nbPose6) ;
- Tc = -e*gain ;
-
- // Tc_v =0 ;
- for (unsigned int i = 0 ; i < nbPose6 ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
- py+Tc[nbPose6+3],
- u0+Tc[nbPose6],
- v0+Tc[nbPose6+1]) ;
-
- // cam.setKd(get_kd() + Tc[10]) ;
- vpColVector Tc_v_Tmp(6) ;
-
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
- * table_cal[p].cMo;
- }
- if (verbose)
- std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cMo_dist = table_cal[p].cMo ;
- table_cal[p].cam = cam_est;
- table_cal[p].cam_dist = cam_est;
- double deviation,deviation_dist;
- table_cal[p].computeStdDeviation(deviation,deviation_dist);
- }
- if (verbose)
- std::cout << " Global std dev " << sqrt(r/nbPointTotal) << std::endl;
-
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-
-
-void
-vpCalibration::calibVVSWithDistortionMulti(
- unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam_est,
- bool verbose)
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- std::cout.precision(10);
- unsigned int nbPoint[1024]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
-
- unsigned int nbPose6 = 6*nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
- nbPoint[i] = table_cal[i].npt;
- nbPointTotal += nbPoint[i];
- }
-
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
- }
-
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
-
- vpColVector P(4*nbPointTotal) ;
- vpColVector Pd(4*nbPointTotal) ;
- vpImagePoint ip;
-
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
- std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
- std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
- std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
-
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
-
- ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
- curPoint++;
- }
- }
- // double lambda = 0.1 ;
- unsigned int iter = 0 ;
-
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
-
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
-
- curPoint++;
- }
- }
-
-
- vpMatrix L(nbPointTotal*4,nbPose6+6) ;
- curPoint = 0 ; //current point indice
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
-
- double inv_px = 1/px ;
- double inv_py = 1/py ;
-
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
-
- double k2ud = 2*kud;
- double k2du = 2*kdu;
-
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint4 = 4*curPoint;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
-
- double inv_z = 1/z;
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
-
- double up = u[curPoint] ;
- double vp = v[curPoint] ;
-
- Pd[curPoint4] = up ;
- Pd[curPoint4+1] = vp ;
-
- double up0 = up - u0;
- double vp0 = vp - v0;
-
- double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
-
- double yp0 = vp0 * inv_py;
- double yp02 = yp0 * yp0;
-
- double r2du = xp02 + yp02 ;
- double kr2du = kdu * r2du;
-
- P[curPoint4] = u0 + px*X - kr2du *(up0) ;
- P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
-
- double r2ud = X2 + Y2 ;
- double kr2ud = 1 + kud * r2ud;
-
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
-
- Pd[curPoint4+2] = up ;
- Pd[curPoint4+3] = vp ;
-
- P[curPoint4+2] = u0 + px*X*kr2ud ;
- P[curPoint4+3] = v0 + py*Y*kr2ud ;
-
- r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
- vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
- vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
- vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
-
- unsigned int curInd = curPoint4;
- //---------------
- {
- {
- L[curInd][q] = px * (-inv_z) ;
- L[curInd][q+1] = 0 ;
- L[curInd][q+2] = px*X*inv_z ;
- L[curInd][q+3] = px*X*Y ;
- L[curInd][q+4] = -px*(1+X2) ;
- L[curInd][q+5] = px*Y ;
- }
- {
- L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
- L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
- L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
- L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
- L[curInd][nbPose6+4] = -(up0)*(r2du) ;
- L[curInd][nbPose6+5] = 0 ;
- }
- curInd++;
- {
- L[curInd][q] = 0 ;
- L[curInd][q+1] = py*(-inv_z) ;
- L[curInd][q+2] = py*Y*inv_z ;
- L[curInd][q+3] = py* (1+Y2) ;
- L[curInd][q+4] = -py*XY ;
- L[curInd][q+5] = -py*X ;
- }
- {
- L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
- L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
- L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
- L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
- L[curInd][nbPose6+4] = -vp0*r2du ;
- L[curInd][nbPose6+5] = 0 ;
- }
- curInd++;
- //---undistorted to distorted
- {
- L[curInd][q] = Axx*(-inv_z) ;
- L[curInd][q+1] = Axy*(-inv_z) ;
- L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
- L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
- L[curInd][q+5] = Axx*Y -Axy*X;
- }
- {
- L[curInd][nbPose6]= 1 ;
- L[curInd][nbPose6+1]= 0 ;
- L[curInd][nbPose6+2]= X*kr2ud ;
- L[curInd][nbPose6+3]= 0;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = px*X*r2ud ;
- }
- curInd++;
- {
- L[curInd][q] = Ayx*(-inv_z) ;
- L[curInd][q+1] = Ayy*(-inv_z) ;
- L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
- L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
- L[curInd][q+5] = Ayx*Y -Ayy*X;
- }
- {
- L[curInd][nbPose6]= 0 ;
- L[curInd][nbPose6+1]= 1;
- L[curInd][nbPose6+2]= 0;
- L[curInd][nbPose6+3]= Y*kr2ud ;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = py*Y*r2ud ;
- }
- } // end interaction
- curPoint++;
- } // end interaction
- }
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
-
- vpMatrix Lp ;
- /*double rank =*/
- L.pseudoInverse(Lp,1e-10) ;
- vpColVector e ;
- e = Lp*error ;
- vpColVector Tc, Tc_v(6*nbPose) ;
- Tc = -e*gain ;
- for (unsigned int i = 0 ; i < 6*nbPose ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
- u0+Tc[nbPose6], v0+Tc[nbPose6+1],
- kud + Tc[nbPose6+5],
- kdu + Tc[nbPose6+4]);
-
- vpColVector Tc_v_Tmp(6) ;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
- * table_cal[p].cMo_dist;
- }
- if (verbose)
- std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
- //std::cout << " residual: " << r << std::endl;
-
- }
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
- }
-
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cam_dist = cam_est ;
- table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
- }
- if (verbose)
- std::cout <<" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;
-
- // Restore ostream format
- std::cout.flags(original_flags);
-}
diff --git a/src/camera/vpCameraParameters.cpp b/src/camera/vpCameraParameters.cpp
deleted file mode 100644
index 8900c03..0000000
--- a/src/camera/vpCameraParameters.cpp
+++ /dev/null
@@ -1,632 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCameraParameters.cpp 5215 2015-01-27 19:03:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Camera intrinsic parameters.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-/*!
- \file vpCameraParameters.cpp
- \brief Definition of the vpCameraParameters class member functions.
- Class vpCameraParameters define the camera intrinsic parameters
-
-*/
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpRotationMatrix.h>
-#include <cmath>
-#include <limits>
-#include <iostream>
-#include <sstream>
-#include <iomanip>
-
-const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
-const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
-const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
-const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
-const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
-const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
-const vpCameraParameters::vpCameraParametersProjType
- vpCameraParameters::DEFAULT_PROJ_TYPE =
- vpCameraParameters::perspectiveProjWithoutDistortion;
-
-/*!
- Default constructor.
- By default, a perspective projection without distortion model is set.
-
- \sa init()
-*/
-vpCameraParameters::vpCameraParameters()
- :
- px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
- u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
- kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
- width(0), height(0),
- isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
- inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
- projModel(DEFAULT_PROJ_TYPE)
-{
- init() ;
-}
-
-/*!
- Copy constructor
- */
-vpCameraParameters::vpCameraParameters(const vpCameraParameters &c)
- :
- px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
- u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
- kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
- width(0), height(0),
- isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
- inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
- projModel(DEFAULT_PROJ_TYPE)
-{
- init(c) ;
-}
-
-/*!
- Constructor for perspective projection without distortion model
-
- \param cam_px,cam_py : pixel size
- \param cam_u0,cam_v0 : principal points
-
- */
-vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
- const double cam_u0, const double cam_v0)
- :
- px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
- u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
- kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
- width(0), height(0),
- isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
- inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
- projModel(DEFAULT_PROJ_TYPE)
-{
- initPersProjWithoutDistortion(cam_px,cam_py,cam_u0,cam_v0) ;
-}
-
-/*!
- Constructor for perspective projection with distortion model
-
- \param cam_px,cam_py : pixel size
- \param cam_u0,cam_v0 : principal points
- \param cam_kud : undistorted to distorted radial distortion
- \param cam_kdu : distorted to undistorted radial distortion
-
- */
-vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
- const double cam_u0, const double cam_v0,
- const double cam_kud, const double cam_kdu)
- :
- px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
- u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
- kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
- width(0), height(0),
- isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
- inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
- projModel(DEFAULT_PROJ_TYPE)
-{
- initPersProjWithDistortion(cam_px,cam_py,cam_u0,cam_v0,cam_kud,cam_kdu) ;
-}
-
-/*!
- \brief basic initialization with the default parameters
-*/
-void
-vpCameraParameters::init()
-{
- if (fabs(this->px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- if (fabs(this->py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- this->inv_px = 1./this->px;
- this->inv_py = 1./this->py;
-}
-
-/*!
- Initialization with specific parameters using perpective projection without
- distortion model.
- \param cam_px,cam_py : the ratio between the focal length and the size of a pixel.
- \param cam_u0,cam_v0 : principal point coordinates in pixels.
-
- The following sample code shows how to use this function:
- \code
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
- double u0 = I.getWidth() / 2.;
- double v0 = I.getHeight() / 2.;
- double px = 600;
- double py = 600;
- vpCameraParameters cam;
- cam.initPersProjWithoutDistortion(px, py, u0, v0);
- cam.computeFov(I.getWidth(), I.getHeight());
- std::cout << cam << std::endl;
- std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
- << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
-}
- \endcode
- It produces the following output:
- \code
-Camera parameters for perspective projection without distortion:
- px = 600 py = 600
- u0 = 320 v0 = 240
-
-Field of view (horizontal: 56.145 and vertical: 43.6028 degrees)
- \endcode
-
- */
-void
-vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py,
- const double cam_u0, const double cam_v0)
-{
- this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion ;
-
- this->px = cam_px ;
- this->py = cam_py ;
- this->u0 = cam_u0 ;
- this->v0 = cam_v0 ;
- this->kud = 0 ;
- this->kdu = 0 ;
-
- if (fabs(px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- if (fabs(py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- this->inv_px = 1./px;
- this->inv_py = 1./py;
-}
-
-/*!
- Initialization with specific parameters using perpective projection with
- distortion model.
- \param cam_px,cam_py : the ratio between the focal length and the size of a pixel.
- \param cam_u0,cam_v0 : principal points coordinates in pixels.
- \param cam_kud : undistorted to distorted radial distortion.
- \param cam_kdu : distorted to undistorted radial distortion.
-
- The following sample code shows how to use this function:
- \code
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
- double u0 = I.getWidth() / 2.;
- double v0 = I.getHeight() / 2.;
- double px = 600;
- double py = 600;
- double kud = -0.19;
- double kdu = 0.20;
- vpCameraParameters cam;
- cam.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
- cam.computeFov(I.getWidth(), I.getHeight());
- std::cout << cam << std::endl;
- std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
- << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
-}
- \endcode
- It produces the following output:
- \code
-Camera parameters for perspective projection with distortion:
- px = 600 py = 600
- u0 = 320 v0 = 240
- kud = -0.19
- kdu = 0.2
-
-Field of view (horizontal: 56.14497387 and vertical: 43.60281897 degrees) \endcode
-*/
-void
-vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double cam_py,
- const double cam_u0, const double cam_v0,
- const double cam_kud, const double cam_kdu)
-{
- this->projModel = vpCameraParameters::perspectiveProjWithDistortion ;
-
- this->px = cam_px ;
- this->py = cam_py ;
- this->u0 = cam_u0 ;
- this->v0 = cam_v0 ;
- this->kud = cam_kud ;
- this->kdu = cam_kdu ;
-
- if (fabs(px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- if (fabs(py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
- }
- this->inv_px = 1./px;
- this->inv_py = 1./py;
-}
-
-/*!
- destructor
-
- nothing much to destroy...
-*/
-vpCameraParameters::~vpCameraParameters()
-{
-}
-
-/*!
- initialization from another vpCameraParameters object
-*/
-void
-vpCameraParameters::init(const vpCameraParameters &c)
-{
- *this = c ;
-}
-
-
-/*!
- initialise the camera from a calibration matrix.
- Using a calibration matrix leads to a camera without distortion
-
- The K matrix in parameters must be like:
-
- \f$ K = \left(\begin{array}{ccc}
- p_x & 0 & u_0 \\
- 0 & p_y & v_0 \\
- 0 & 0 & 1
- \end{array} \right) \f$
-
- \param _K : the 3by3 calibration matrix
-*/
-void
-vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
-{
- if(_K.getRows() != 3 || _K.getCols() != 3 ){
- throw vpException(vpException::dimensionError, "bad size for calibration matrix");
- }
- if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
- throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
- }
- initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
-}
-
-/*!
- Initialize the camera model without distorsion from the image dimension and the camera field of view.
- \param w : Image width.
- \param h : Image height.
- \param hfov : Camera horizontal field of view angle expressed in radians.
- \param vfov : Camera vertical field of view angle expressed in radians.
-
- The following sample code shows how to use this function:
- \code
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
- vpCameraParameters cam;
- double hfov = vpMath::rad(56);
- double vfov = vpMath::rad(43);
- cam.initFromFov(I.getWidth(), I.getHeight(), hfov, vfov);
-
- std::cout << cam << std::endl;
- std::cout << "Field of view (horizontal: " << vpMath::deg(cam.getHorizontalFovAngle())
- << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle()) << " degrees)" << std::endl;
-}
- \endcode
- It produces the following output:
- \code
-Camera parameters for perspective projection without distortion:
- px = 601.832 py = 609.275
- u0 = 320 v0 = 240
-
-Field of view (horizontal: 56 and vertical: 43 degrees)
- \endcode
- */
-void
-vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
-{
- projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
- u0 = (double)w/2.;
- v0 = (double)h/2.;
- px = u0 / tan(hfov/2);
- py = v0 / tan(vfov/2);
- kud = 0;
- kdu = 0;
- inv_px = 1./px;
- inv_py = 1./py;
- computeFov(w, h);
-}
-
-/*!
- copy operator
- */
-vpCameraParameters&
- vpCameraParameters::operator=(const vpCameraParameters& cam)
-{
- projModel = cam.projModel ;
- px = cam.px ;
- py = cam.py ;
- u0 = cam.u0 ;
- v0 = cam.v0 ;
- kud = cam.kud ;
- kdu = cam.kdu ;
-
- inv_px = cam.inv_px;
- inv_py = cam.inv_py;
-
- isFov = cam.isFov;
- m_hFovAngle = cam.m_hFovAngle;
- m_vFovAngle = cam.m_vFovAngle;
- width = cam.width;
- height = cam.height;
- fovNormals = cam.fovNormals;
-
- return *this ;
-}
-
-/*!
- Compute angles and normals of the FOV.
-
- \param w : Width of the image
- \param h : Height of the image.
-*/
-void
-vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
-{
- if( !isFov && w != width && h != height && w != 0 && h != 0){
- fovNormals = std::vector<vpColVector>(4);
-
- isFov = true;
-
- double half_hFovAngle = atan((double)w / ( 2.0 * px ));
- double half_vFovAngle = atan((double)h / ( 2.0 * py ));
-
- width = w;
- height = h;
-
- vpColVector n(3);
- n = 0;
- n[0] = 1.0;
-
- vpRotationMatrix Rleft (0,-half_hFovAngle,0);
- vpRotationMatrix Rright(0, half_hFovAngle,0);
-
- vpColVector nLeft, nRight;
-
- nLeft = Rleft * (-n);
- fovNormals[0] = nLeft.normalize();
-
- nRight = Rright * n;
- fovNormals[1] = nRight.normalize();
-
- n = 0;
- n[1] = 1.0;
-
- vpRotationMatrix Rup ( half_vFovAngle,0,0);
- vpRotationMatrix Rdown(-half_vFovAngle,0,0);
-
- vpColVector nUp, nDown;
-
- nUp = Rup * (-n);
- fovNormals[2] = nUp.normalize();
-
- nDown = Rdown * n;
- fovNormals[3] = nDown.normalize();
-
- m_hFovAngle = 2 * half_hFovAngle;
- m_vFovAngle = 2 * half_vFovAngle;
- }
-}
-
-
-/*!
- Return the calibration matrix \f$K\f$.
-
- \f$K\f$ is 3x3 matrix given by:
-
- \f$ K = \left(\begin{array}{ccc}
- p_x & 0 & u_0 \\
- 0 & p_y & v_0 \\
- 0 & 0 & 1
- \end{array} \right) \f$
-
- \warning : this function is useful only in the case of perspective
- projection without distortion.
-
- \sa get_K_inverse()
-*/
-vpMatrix
-vpCameraParameters::get_K() const
-{
- vpMatrix K;
- switch(projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- K.resize(3,3) ;
- K = 0.0 ;
- K[0][0] = px ;
- K[1][1] = py ;
- K[0][2] = u0 ;
- K[1][2] = v0 ;
- K[2][2] = 1.0 ;
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- default :
- vpERROR_TRACE("\n\t getting K matrix in the case of projection \
- with distortion has no sense");
- throw(vpException(vpException::notImplementedError,
- "\n\t getting K matrix in the case of projection \
- with distortion has no sense"));
- }
- return K;
-}
-/*!
- Return the calibration matrix \f$K^{-1}\f$.
-
- \f$K^{-1}\f$ is 3x3 matrix given by:
-
- \f$ K^{-1} = \left(\begin{array}{ccc}
- 1/p_x & 0 & -u_0/p_x \\
- 0 & 1/p_y & -v_0/p_y \\
- 0 & 0 & 1
- \end{array} \right) \f$
-
- \warning : this function is useful only in the case of perspective
- projection without distortion.
-
- \sa get_K()
-*/
-vpMatrix
-vpCameraParameters::get_K_inverse() const
-{
- vpMatrix K_inv;
- switch(projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- K_inv.resize(3,3) ;
- K_inv = 0.0 ;
- K_inv[0][0] = inv_px ;
- K_inv[1][1] = inv_py ;
- K_inv[0][2] = -u0*inv_px ;
- K_inv[1][2] = -v0*inv_py ;
- K_inv[2][2] = 1.0 ;
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- default :
- vpERROR_TRACE("\n\t getting K^-1 matrix in the case of projection \
- with distortion has no sense");
- throw(vpException(vpException::notImplementedError,
- "\n\t getting K matrix in the case of projection \
- with distortion has no sense"));
- }
- return K_inv;
-}
-
-
-/*!
- Print the camera parameters on the standard output
-
- \sa operator <<
-*/
-void
-vpCameraParameters::printParameters()
-{
- std::ios::fmtflags original_flags( std::cout.flags() );
- switch(projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- std::cout.precision(10);
- std::cout << "Camera parameters for perspective projection without distortion:"
- << std::endl ;
- std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
- std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- std::cout.precision(10);
- std::cout << "Camera parameters for perspective projection with distortion:"
- << std::endl ;
- std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
- std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
- std::cout << " kud = " << kud << std::endl ;
- std::cout << " kdu = " << kdu << std::endl ;
- break;
- }
- // Restore ostream format
- std::cout.flags(original_flags);
-}
-/*!
-
- Print on the output stream \e os the camera parameters.
-
- \param os : Output stream.
- \param cam : Camera parameters.
-*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpCameraParameters &cam)
-{
- switch(cam.get_projModel()){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- os << "Camera parameters for perspective projection without distortion:"
- << std::endl ;
- os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
- << std::endl ;
- os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
- << std::endl ;
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- std::ios_base::fmtflags original_flags = os.flags();
- os.precision(10);
- os << "Camera parameters for perspective projection with distortion:"
- << std::endl ;
- os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
- << std::endl ;
- os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
- << std::endl ;
- os << " kud = " << cam.get_kud() << std::endl ;
- os << " kdu = " << cam.get_kdu() << std::endl ;
-
- os.flags(original_flags); // restore os to standard state
- break;
- }
- return os;
-}
-
-
diff --git a/src/camera/vpCameraParameters.h b/src/camera/vpCameraParameters.h
deleted file mode 100644
index 2890687..0000000
--- a/src/camera/vpCameraParameters.h
+++ /dev/null
@@ -1,367 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCameraParameters.h 5207 2015-01-26 09:05:28Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Camera intrinsic parameters.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpCameraParameters.h
- \brief Declaration of the vpCameraParameters class.
- Class vpCameraParameters define the camera intrinsic parameters
-
-*/
-
-#ifndef vpCameraParameters_H
-#define vpCameraParameters_H
-
-#include <vector>
-
-#include <visp/vpConfig.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-
-/*!
- \class vpCameraParameters
-
- \ingroup CameraModelTransformation CameraModel
-
- \brief Generic class defining intrinsic camera parameters.
-
- Two kinds of camera modelisation are implemented:
- - Camera parameters for a perspective projection without distortion model,
- - Camera parameters for a perspective projection with distortion model.
-
- The main intrinsic camera parameters are \f$(p_x, p_y)\f$ the ratio
- between the focal length and the size of a pixel, and \f$(u_0,
- v_0)\f$ the coordinates of the principal point in pixel. The lens
- distortion can also be considered by two additional parameters
- \f$(k_{ud}, k_{du})\f$.
-
- <b>1. Camera parameters for a perspective projection without distortion model</b>
-
- In this modelisation, only \f$u_0,v_0,p_x,p_y\f$ parameters are
- used. If we denote \f$(u,v)\f$ the position of a pixel in the
- digitized image, this position is related to the corresponding
- coordinates \f$(x,y)\f$ in the normalized space (in meter) by:
-
- \f[
- \left\{ \begin{array}{l}
- u = u_0 + p_x x \\
- v = v_0 + p_y y
- \end{array}\right.
- \f]
-
- The initialization of such a model can be done using:
- - initPersProjWithoutDistortion() that allows to set \f$u_0,v_0,p_x,p_y\f$ parameters;
- - initFromFov() that computes the parameters from an image size and a camera field of view.
-
- <b>2. Camera parameters for a perspective projection with distortion model</b>
-
- In this modelisation, \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$
- parameters are used. If a model with distortion is considered, we
- got:
-
- \f[
- \left\{ \begin{array}{l}
- u = u_0 + p_x x + \delta_u \\
- v = v_0 + p_y y + \delta_v
- \end{array}\right.
- \f]
-
- where \f$\delta_u\f$ and \f$\delta_v\f$ are
- geometrical distortions introduced in the camera model. These distortions are
- due to imperfections in the lenses design and assembly there usually are some
- positional errors that have to be taken into account.
- \f$\delta_u\f$ and \f$\delta_v\f$ can be modeled as follow:
- - with an undistorted to distorted model
- \f[
- \left\{ \begin{array}{l}
- \delta_u(x,y) = k_{ud} \;r^2\; p_x x \\
- \\
- \delta_v(x,y) = k_{ud}\; r^2\; p_y y
- \end{array}\right.
- \f]
- with \f[ r^2 = x^2 + y^2 \f]
- This model is useful to convert meter to pixel coordinates because in this
- case :
- \f[
- \left\{ \begin{array}{l}
- u = f_u(x,y) = u_0 + p_x x\left(1+k_{ud}\left(x^2 + y^2\right)\right) \\
- \\
- v = f_v(x,y) = v_0 + p_y y\left(1+k_{ud}\left(x^2 + y^2\right)\right)
- \end{array}\right.
- \f]
- The conversion from normalized coordinates \f$(x,y)\f$ into pixel
- \f$(u,v)\f$ is implemented in vpMeterPixelConversion.
-
- - or with a distorted to undistorted model
- \f[
- \left\{ \begin{array}{l}
- \delta_u(u,v) = -k_{du} \;r^2\; \left(u-u_0\right) \\
- \\
- \delta_v(u,v) = -k_{du}\; r^2\; \left(v-v_0\right)
- \end{array}\right.
- \f]
- with \f[ r^2 = {\left(\frac{u-u_0}{p_{x}}\right)}^2 + {\left(\frac{v-v_0}{p_{y}}\right)}^2 \f]
- This model is useful to convert pixel to meter coordinates because in this
- case :
- \f[
- \left\{ \begin{array}{l}
- x = f_x(u,v) = \frac{u-u_0}{p_x}\left(1+k_{du}\left( {\left(\frac{u-u_0}{p_{x}}\right)}^2 + {\left(\frac{v-v_0}{p_{y}}\right)}^2 \right)\right) \\
- \\
- y = f_y(u,v) = \frac{v-v_0}{p_y}\left(1+k_{du}\left( {\left(\frac{u-u_0}{p_{x}}\right)}^2 + {\left(\frac{v-v_0}{p_{y}}\right)}^2 \right)\right)
- \end{array}\right.
- \f]
-
- The initialization of such a model can be done using:
- - initPersProjWithDistortion() that allows to set \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$ parameters;
-
- \note The conversion from pixel coordinates \f$(u,v)\f$ in the normalized
- space \f$(x,y)\f$ is implemented in vpPixelMeterConversion, whereas
- the conversion from normalized coordinates into pixel is implemented
- in vpMeterPixelConversion.
-
- The selection of one of these modelisations is done during
- vpCameraParameters initialisation.
-
- Here an example of camera initialisation, for a model without distortion. A complete example is given in initPersProjWithoutDistortion().
- \code
- double px = 600;
- double py = 600;
- double u0 = 320;
- double v0 = 240;
-
- // Create a camera parameter container
- vpCameraParameters cam;
- // Camera initialization with a perspective projection without distortion model
- cam.initPersProjWithoutDistortion(px,py,u0,v0);
- // It is also possible to print the current camera parameters
- std::cout << cam << std::endl;
- \endcode
-
- Here an example of camera initialisation, for a model with distortion. A complete example is given in initPersProjWithDistortion().
- \code
- double px = 600;
- double py = 600;
- double u0 = 320;
- double v0 = 240;
- double kud = -0.19;
- double kdu = 0.20;
-
- // Create a camera parameter container
- vpCameraParameters cam;
-
- // Camera initialization with a perspective projection without distortion model
- cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
- \endcode
-
- The code below shows how to know the currently used projection model:
- \code
- vpCameraParameters cam;
- ...
- vpCameraParameters::vpCameraParametersProjType projModel;
- projModel = cam.get_projModel(); // Get the projection model type
- \endcode
-
- An XML parser for camera parameters is also provided in vpXmlParserCamera.
-
- Note also that the \ref tutorial-calibration shows how to calibrate a camera
- to obtain the parameters corresponding to both models implemented in this class.
-*/
-class VISP_EXPORT vpCameraParameters
-{
- friend class vpMeterPixelConversion;
- friend class vpPixelMeterConversion;
-public :
- typedef enum{
- perspectiveProjWithoutDistortion, //!< Perspective projection without distortion model
- perspectiveProjWithDistortion //!< Perspective projection with distortion model
- } vpCameraParametersProjType ;
-
-
- //generic functions
- vpCameraParameters() ;
- vpCameraParameters(const vpCameraParameters &c) ;
- vpCameraParameters(const double px, const double py,
- const double u0, const double v0) ;
- vpCameraParameters(const double px, const double py,
- const double u0, const double v0,
- const double kud, const double kdu) ;
-
- vpCameraParameters& operator =(const vpCameraParameters &c) ;
- virtual ~vpCameraParameters() ;
-
- void init() ;
- void init(const vpCameraParameters &c) ;
- void initFromCalibrationMatrix(const vpMatrix& _K);
- void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov);
- void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0);
- void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0,
- const double kud,const double kdu) ;
-
- /*!
- Specify if the fov has been computed.
-
- \sa computeFov()
-
- \return True if the fov has been computed, False otherwise.
- */
- inline bool isFovComputed() const { return isFov; }
-
- void computeFov(const unsigned int &w, const unsigned int &h);
-
- /*!
- Get the horizontal angle in radian of the field of view.
-
- \return FOV horizontal angle computed with px and width.
-
- \sa computeFov(), getVerticalFovAngle()
- */
- inline double getHorizontalFovAngle() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() won't be significant.");
- return m_hFovAngle;
- }
-
- /*!
- Get the vertical angle in radian of the field of view.
-
- \return FOV vertical angle computed with py and height.
-
- \sa computeFov(), getHorizontalFovAngle()
- */
- inline double getVerticalFovAngle() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't be significant.");
- return m_vFovAngle;
- }
-
- /*!
- Get the list of the normals corresponding to planes describing the field of view.
- - vector[0] : Left Normal.
- - vector[1] : Right Normal.
- - vector[2] : Up Normal.
- - vector[3] : Down Normal.
-
- \sa computeFov()
-
- \return List of the normals.
- */
- inline std::vector<vpColVector> getFovNormals() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be significant.");
- return fovNormals;
- }
-
- inline double get_px() const { return px; }
- inline double get_px_inverse() const {return inv_px; }
- inline double get_py_inverse() const {return inv_py; }
- inline double get_py() const { return py; }
- inline double get_u0() const { return u0; }
- inline double get_v0() const { return v0; }
- inline double get_kud() const { return kud; }
- inline double get_kdu() const { return kdu; }
-
- inline vpCameraParametersProjType get_projModel() const {
- return projModel;
- }
-
- vpMatrix get_K() const;
- vpMatrix get_K_inverse() const;
-
- void printParameters() ;
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpCameraParameters &cam);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- \deprecated This function is deprecated. Use rather getHorizontalFovAngle().
- Get the horizontal angle of the field of view.
-
- \sa computeFov()
-
- \return AngleX computed with px and width.
- */
- vp_deprecated inline double getFovAngleX() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getFovAngleX() won't be significant.");
- return m_hFovAngle;
- }
-
- /*!
- \deprecated This function is deprecated. Use rather getVerticalFovAngle().
- Get the vertical angle in radian of the field of view.
-
- \sa computeFov()
-
- \return FOV vertical angle computed with py and height.
- */
- vp_deprecated inline double getFovAngleY() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getFovAngleY() won't be significant.");
- return m_hFovAngle;
- }
-
-#endif
-
-private:
- static const double DEFAULT_U0_PARAMETER;
- static const double DEFAULT_V0_PARAMETER;
- static const double DEFAULT_PX_PARAMETER;
- static const double DEFAULT_PY_PARAMETER;
- static const double DEFAULT_KUD_PARAMETER;
- static const double DEFAULT_KDU_PARAMETER;
- static const vpCameraParametersProjType DEFAULT_PROJ_TYPE;
-
-
- double px, py ; //!< pixel size
- double u0, v0 ; //!< principal point
- double kud ; //!< radial distortion (from undistorted to distorted)
- double kdu ; //!< radial distortion (from distorted to undistorted)
-
- unsigned int width ; //!< Width of the image used for the fov computation
- unsigned int height ; //!< Height of the image used for the fov computation
- bool isFov ; //!< Boolean to specify if the fov has been computed
- double m_hFovAngle ; //!< Field of view horizontal angle
- double m_vFovAngle ; //!< Field of view vertical angle
- std::vector<vpColVector> fovNormals ; //!< Normals of the planes describing the fov
-
- double inv_px, inv_py;
-
- vpCameraParametersProjType projModel ; //!< used projection model
-} ;
-
-#endif
diff --git a/src/camera/vpMeterPixelConversion.cpp b/src/camera/vpMeterPixelConversion.cpp
deleted file mode 100644
index a7ab876..0000000
--- a/src/camera/vpMeterPixelConversion.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeterPixelConversion.cpp 4740 2014-05-26 07:15:44Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Meter to pixel conversion.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-/*!
- \file vpMeterPixelConversion.cpp
- \brief meter to pixel conversion
-*/
-
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-
-//! Line coordinates conversion (rho,theta).
-void
-vpMeterPixelConversion::convertLine(const vpCameraParameters &cam,
- const double &rho_m, const double &theta_m,
- double &rho_p, double &theta_p)
-{
- double co = cos(theta_m) ;
- double si = sin(theta_m) ;
- double d = sqrt(vpMath::sqr(cam.py*co) + vpMath::sqr(cam.px*si)) ;
-
- if (fabs(d)<1e-6)
- {
- vpERROR_TRACE("division by zero") ;
- throw(vpException(vpException::divideByZeroError,
- "division by zero")) ;
- }
-
- theta_p = atan2(cam.px*si, cam.py*co) ;
- rho_p = (cam.px*cam.py*rho_m + cam.u0*cam.py*co + cam.v0*cam.px*si) ;
- rho_p /= d ;
-}
-
-/*!
- Converts an ellipse with parameters expressed in meter in the image
- plane in parameters expressed in the image in pixels.
-
- The ellipse is here represented by its parameters \f$x_c,y_c,\mu_{20}, \mu_{11}, \mu_{02}\f$.
-
- \param cam [in]: Intrinsic camera parameters.
- \param circle [in]: 3D circle with parameters circle.p[] expressed in meters in the image plane.
- \param center [out]: Center of the corresponding ellipse in the image with coordinates
- expressed in pixels.
- \param mu20_p,mu11_p,mu02_p [out]: Centered moments expressed in pixels.
-
- The following code shows how to use this function:
- \code
- vpCircle circle;
- double mu20_p, mu11_p, mu02_p;
- circle.changeFrame(cMo);
- circle.projection();
- vpMeterPixelConversion::convertEllipse(cam, circle, center_p, mu20_p, mu11_p, mu02_p);
- vpDisplay::displayEllipse(I, center_p, mu20_p, mu11_p, mu02_p);
- \endcode
- */
-void
-vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam,
- const vpCircle &circle, vpImagePoint ¢er,
- double &mu20_p, double &mu11_p, double &mu02_p)
-{
- // Get the parameters of the ellipse in the image plane
- double xc_m = circle.p[0];
- double yc_m = circle.p[1];
- double mu20_m = circle.p[2];
- double mu11_m = circle.p[3];
- double mu02_m = circle.p[4];
-
- // Convert from meter to pixels
- vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center);
- mu20_p = mu20_m*vpMath::sqr(cam.get_px());
- mu11_p = mu11_m*cam.get_px()*cam.get_py();
- mu02_p = mu02_m*vpMath::sqr(cam.get_py());
-}
-
diff --git a/src/camera/vpMeterPixelConversion.h b/src/camera/vpMeterPixelConversion.h
deleted file mode 100644
index 405f7c0..0000000
--- a/src/camera/vpMeterPixelConversion.h
+++ /dev/null
@@ -1,247 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeterPixelConversion.h 4726 2014-04-23 11:58:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Meter to pixel conversion.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-#ifndef vpMeterPixelConversion_H
-#define vpMeterPixelConversion_H
-
-
-/*!
- \file vpMeterPixelConversion.h
- \brief meter to pixel conversion
-
-*/
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpCircle.h>
-#include <visp/vpException.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-
-/*!
- \class vpMeterPixelConversion
-
- \ingroup CameraModelTransformation CameraModel
-
- \brief Conversion from normalized coordinates \f$(x,y)\f$ in meter
- to pixel coordinates \f$(u,v)\f$.
-
- This class relates to vpCameraParameters.
-
-*/
-class VISP_EXPORT vpMeterPixelConversion
-{
-public:
- static void convertEllipse(const vpCameraParameters &cam,
- const vpCircle &circle, vpImagePoint ¢er,
- double &mu20_p, double &mu11_p, double &mu02_p);
-
- static void convertLine(const vpCameraParameters &cam,
- const double &rho_m, const double &theta_m,
- double &rho_p, double &theta_p) ;
-
-/*!
-
- \brief Point coordinates conversion from normalized coordinates
- \f$(x,y)\f$ in meter to pixel coordinates \f$(u,v)\f$.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param x,y : input coordinates in meter.
- \param u,v : output coordinates in pixels.
-
- \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
- perspective projection without distortion.
-
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
- distortion.
-*/
-
- inline static void
- convertPoint(const vpCameraParameters &cam,
- const double &x, const double &y,
- double &u, double &v)
- {
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,x,y,u,v);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,x,y,u,v);
- break;
- }
- }
-
-/*!
-
- \brief Point coordinates conversion from normalized coordinates
- \f$(x,y)\f$ in meter to pixel coordinates.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param x,y : input coordinates in meter.
- \param iP : output coordinates in pixels.
-
- In the frame (u,v) the result is given by:
-
- \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
- perspective projection without distortion.
-
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
- distortion.
-*/
-
- inline static void
- convertPoint(const vpCameraParameters &cam,
- const double &x, const double &y,
- vpImagePoint &iP)
- {
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,x,y,iP);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,x,y,iP);
- break;
- }
- }
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /*!
-
- \brief Point coordinates conversion without distortion from
- normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates
- \f$(u,v)\f$.
-
- \f$ u = x*p_x+u_0 \f$ and \f$ v = y*p_y+v_0 \f$
- */
-
- inline static void
- convertPointWithoutDistortion(const vpCameraParameters &cam,
- const double &x, const double &y,
- double &u, double &v)
- {
- u = x * cam.px + cam.u0 ;
- v = y * cam.py + cam.v0 ;
- }
-
- /*!
-
- \brief Point coordinates conversion without distortion from
- normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates.
-
- In the frame (u,v) the result is given by:
-
- \f$ u = x*p_x+u_0 \f$ and \f$ v = y*p_y+v_0 \f$
- */
-
- inline static void
- convertPointWithoutDistortion(const vpCameraParameters &cam,
- const double &x, const double &y,
- vpImagePoint &iP)
- {
- iP.set_u( x * cam.px + cam.u0 );
- iP.set_v( y * cam.py + cam.v0 );
- }
-
- /*!
-
- \brief Point coordinates conversion with distortion from
- normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates
- \f$(u,v)\f$.
-
- \param cam : camera parameters.
- \param x,y : input coordinates in meter.
- \param u,v : output coordinates in pixels.
-
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and
- \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$
- */
- inline static void
- convertPointWithDistortion(const vpCameraParameters &cam,
- const double &x, const double &y,
- double &u, double &v)
- {
- double r2 = 1.+cam.kud*(x*x+y*y);
- u = cam.u0 + cam.px*x*r2;
- v = cam.v0 + cam.py*y*r2;
- }
-
- /*!
-
- \brief Point coordinates conversion with distortion from
- normalized coordinates \f$(x,y)\f$ in meter to pixel coordinates.
-
- \param cam : camera parameters.
- \param x,y : input coordinates in meter.
- \param iP : output coordinates in pixels.
-
- In the frame (u,v) the result is given by:
-
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and
- \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$
- */
- inline static void
- convertPointWithDistortion(const vpCameraParameters &cam,
- const double &x, const double &y,
- vpImagePoint &iP)
- {
- double r2 = 1.+cam.kud*(x*x+y*y);
- iP.set_u( cam.u0 + cam.px*x*r2 );
- iP.set_v( cam.v0 + cam.py*y*r2 );
- }
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-} ;
-
-#endif
-
-
diff --git a/src/camera/vpPixelMeterConversion.cpp b/src/camera/vpPixelMeterConversion.cpp
deleted file mode 100644
index d05735b..0000000
--- a/src/camera/vpPixelMeterConversion.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPixelMeterConversion.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pixel to meter conversion.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-/*!
- \file vpPixelMeterConversion.cpp
- \brief Pixel to meter conversion.
-*/
-#include<visp/vpPixelMeterConversion.h>
-#include<visp/vpCameraParameters.h>
-#include<visp/vpException.h>
-#include<visp/vpMath.h>
-#include<visp/vpDebug.h>
-
-
-//! line coordinates conversion (rho,theta)
-void
-vpPixelMeterConversion::convertLine(const vpCameraParameters &cam,
- const double &rho_p, const double &theta_p,
- double &rho_m, double &theta_m)
-{
- double co = cos(theta_p) ;
- double si = sin(theta_p) ;
- double d = vpMath::sqr(cam.px*co)+vpMath::sqr(cam.py*si) ;
-
- if (fabs(d)<1e-6)
- {
- vpERROR_TRACE("division by zero") ;
- throw(vpException(vpException::divideByZeroError,
- "division by zero")) ;
- }
- theta_m = atan2(si*cam.py, co*cam.px) ;
- rho_m = (rho_p - cam.u0*co-cam.v0*si)/sqrt(d) ;
-}
-
-
-void
-vpPixelMeterConversion::convertMoment(const vpCameraParameters &cam,
- unsigned int order,
- const vpMatrix &moment_pixel,
- vpMatrix &moment_meter)
-{
-
- vpMatrix m(order, order);
- double yc = -cam.v0 ;
- double xc = -cam.u0 ;
-
- for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
- {
- for (unsigned int p=0 ; p<order; p++) // iteration en X
- for (unsigned int q=0; q<order; q++) // iteration en Y
- if (p+q==k) // on est bien dans la matrice triangulaire superieure
- {
- m[p][q] = 0; // initialisation e 0
- for(unsigned int r=0; r<=p; r++) // somme externe
- for(unsigned int t=0; t<=q; t++) // somme interne
- {
- m[p][q] +=
- static_cast<double>(vpMath::comb(p, r))
- * static_cast<double>(vpMath::comb(q, t))
- * pow(xc, (int)(p-r)) * pow(yc, (int)(q-t))
- * moment_pixel[r][t];
-
- }
- }
-
- }
-
- for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
- for (unsigned int p=0 ; p<order; p++)
- for (unsigned int q=0; q<order; q++)
- if (p+q==k)
- {
- m[p][q] *= pow(cam.inv_px,(int)(1+p)) * pow(cam.inv_py,(int)(1+q));
- }
-
- for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
- for (unsigned int p=0 ; p<order; p++)
- for (unsigned int q=0; q<order; q++)
- if (p+q==k)
- {
- moment_meter[p][q] = m[p][q];
- }
-
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/camera/vpPixelMeterConversion.h b/src/camera/vpPixelMeterConversion.h
deleted file mode 100644
index f93f24f..0000000
--- a/src/camera/vpPixelMeterConversion.h
+++ /dev/null
@@ -1,255 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPixelMeterConversion.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pixel to meter conversion.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#ifndef vpPixelMeterConversion_H
-#define vpPixelMeterConversion_H
-
-
-/*!
- \file vpPixelMeterConversion.h
- \brief pixel to meter conversion
-
-*/
-#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-#include <visp/vpImagePoint.h>
-
-/*!
- \class vpPixelMeterConversion
-
- \ingroup CameraModelTransformation CameraModel
-
- \brief Conversion from pixel coordinates \f$(u,v)\f$ to normalized
- coordinates \f$(x,y)\f$ in meter.
-
- This class relates to vpCameraParameters.
-
-*/
-class VISP_EXPORT vpPixelMeterConversion
-{
-public:
-
-/*!
- \brief Point coordinates conversion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param u,v : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
- perspective projection without distortion.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
- projection with distortion.
-*/
-inline static void
-convertPoint(const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,u,v,x,y);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,u,v,x,y);
- break;
- }
-}
-
-/*!
- \brief Point coordinates conversion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
- perspective projection without distortion.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
- projection with distortion.
-*/
-inline static void
-convertPoint(const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,iP,x,y);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,iP,x,y);
- break;
- }
-}
-
-/*!
- \brief Point coordinates conversion without distortion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param u,v : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
-*/
-inline static void
-convertPointWithoutDistortion(
- const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- x = (u - cam.u0)*cam.inv_px ;
- y = (v - cam.v0)*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion without distortion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
-*/
-inline static void
-convertPointWithoutDistortion(
- const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- x = (iP.get_u() - cam.u0)*cam.inv_px ;
- y = (iP.get_v() - cam.v0)*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion with distortion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param u,v : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
-*/
-inline static void
-convertPointWithDistortion(
- const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- double r2 = 1.+cam.kdu*(vpMath::sqr((u - cam.u0)*cam.inv_px) +
- vpMath::sqr((v-cam.v0)*cam.inv_py));
- x = (u - cam.u0)*r2*cam.inv_px ;
- y = (v - cam.v0)*r2*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion with distortion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x,y : output coordinates in meter.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
-*/
-inline static void
-convertPointWithDistortion(
- const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- double r2 = 1.+cam.kdu*(vpMath::sqr((iP.get_u() - cam.u0)*cam.inv_px) +
- vpMath::sqr((iP.get_v()-cam.v0)*cam.inv_py));
- x = (iP.get_u() - cam.u0)*r2*cam.inv_px ;
- y = (iP.get_v() - cam.v0)*r2*cam.inv_py ;
-}
-
- //! line coordinates conversion (rho,theta)
- static void convertLine(const vpCameraParameters &cam,
- const double &rho_p, const double &theta_p,
- double &rho_m, double &theta_m) ;
-
-
- static void convertMoment(const vpCameraParameters &cam,
- unsigned int order,
- const vpMatrix &moment_pixel,
- vpMatrix &moment_meter) ;
-} ;
-
-#endif
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/camera/vpXmlParserCamera.cpp b/src/camera/vpXmlParserCamera.cpp
deleted file mode 100644
index d47716f..0000000
--- a/src/camera/vpXmlParserCamera.cpp
+++ /dev/null
@@ -1,1101 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParserCamera.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load and save camera intrinsic parameters.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-/*!
- \file vpXmlParserCamera.cpp
- \brief Definition of the vpXmlParserCamera class member functions.
- Class vpXmlParserCamera allowed to load and save intrinsic camera parameters
-
-*/
-#include <visp/vpXmlParserCamera.h>
-#ifdef VISP_HAVE_XML2
-
-#include <stdlib.h>
-#include <string.h>
-
-#include <visp/vpDebug.h>
-/* -------------------------------------------------------------------------- */
-/* --- LABEL XML ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-#define LABEL_XML_ROOT "root"
-#define LABEL_XML_CAMERA "camera"
-#define LABEL_XML_CAMERA_NAME "name"
-#define LABEL_XML_WIDTH "image_width"
-#define LABEL_XML_HEIGHT "image_height"
-#define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
-#define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
-#define LABEL_XML_FULL_WIDTH "full_width"
-#define LABEL_XML_FULL_HEIGHT "full_height"
-#define LABEL_XML_MODEL "model"
-#define LABEL_XML_MODEL_TYPE "type"
-#define LABEL_XML_U0 "u0"
-#define LABEL_XML_V0 "v0"
-#define LABEL_XML_PX "px"
-#define LABEL_XML_PY "py"
-#define LABEL_XML_KUD "kud"
-#define LABEL_XML_KDU "kdu"
-
-#define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
-#define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
-/*!
- Default constructor
-*/
-vpXmlParserCamera::vpXmlParserCamera()
- : vpXmlParser(),
- camera(), camera_name(), image_width(0), image_height(0),
- subsampling_width(0), subsampling_height(0), full_width(0), full_height(0)
-{
-}
-/*!
- Copy constructor
- \param twinParser : parser object to copy
-*/
-vpXmlParserCamera::vpXmlParserCamera(vpXmlParserCamera& twinParser)
- : vpXmlParser(twinParser),
- camera(), camera_name(), image_width(0), image_height(0),
- subsampling_width(0), subsampling_height(0), full_width(0), full_height(0)
-
-{
- this->camera = twinParser.camera;
- this->camera_name = twinParser.camera_name;
- this->image_width = twinParser.image_width;
- this->image_height = twinParser.image_height;
- this->subsampling_width = twinParser.subsampling_width;
- this->subsampling_height = twinParser.subsampling_height;
- this->full_width = twinParser.full_width;
- this->full_height = twinParser.full_height;
-}
-
-/*!
- Copy operator
- \param twinParser : parser object to copy
- \return a copy of the input.
-*/
-vpXmlParserCamera&
-vpXmlParserCamera::operator =(const vpXmlParserCamera& twinParser) {
- this->camera = twinParser.camera;
- this->camera_name = twinParser.camera_name;
- this->image_width = twinParser.image_width;
- this->image_height = twinParser.image_height;
- this->subsampling_width = twinParser.subsampling_width;
- this->subsampling_height = twinParser.subsampling_height;
- this->full_width = twinParser.full_width;
- this->full_height = twinParser.full_height;
- return *this ;
-}
-
-/*!
- Parse an xml file to load camera parameters.
- \param cam : camera parameters to fill.
- \param filename : name of the xml file to parse
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param projModel : camera projection model needed.
- \param im_width : image width on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : image height on which camera calibration was performed.
- Set as 0 if not ambiguous.
-
- \return error code.
-*/
-int
-vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename,
- const std::string& cam_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int im_width,
- const unsigned int im_height)
-{
- xmlDocPtr doc;
- xmlNodePtr node;
-
- doc = xmlParseFile(filename.c_str());
- if (doc == NULL)
- {
- return SEQUENCE_ERROR;
- }
-
- node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- int ret = this ->read (doc, node, cam_name, projModel, im_width, im_height);
-
- cam = camera ;
-
- xmlFreeDoc(doc);
-
- return ret;
-}
-
-/*!
- Save camera parameters in an xml file.
- \param cam : camera parameters to save.
- \param filename : name of the xml file to fill.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration was performed.
- Set as 0 if not ambiguous.
-
- \return error code.
-*/
-int
-vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filename,
- const std::string& cam_name,
- const unsigned int im_width,
- const unsigned int im_height)
-{
- xmlDocPtr doc;
- xmlNodePtr node;
- xmlNodePtr nodeCamera = NULL;
-
- doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR
- + XML_PARSE_NOBLANKS);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
- xmlDocSetRootElement(doc,node);
- xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
- "This file stores intrinsic camera parameters used\n"
- " in the vpCameraParameters Class of ViSP available\n"
- " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
- " It can be read with the parse method of\n"
- " the vpXmlParserCamera class.");
- xmlAddChild(node,node_tmp);
- }
-
- node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- this->camera = cam;
-
- int nbCamera = count(doc, node, cam_name,cam.get_projModel(),
- im_width, im_height);
- if( nbCamera > 0){
-// vpCERROR << nbCamera
-// << " set(s) of camera parameters is(are) already "<< std::endl
-// << "available in the file with your specifications : "<< std::endl
-// << "precise the grabber parameters or delete manually"<< std::endl
-// << "the previous one."<<std::endl;
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- nodeCamera = find_camera(doc, node, cam_name, im_width, im_height);
- if(nodeCamera == NULL){
- write(node, cam_name, im_width, im_height);
- }
- else{
- write_camera(nodeCamera);
- }
-
- xmlSaveFormatFile(filename.c_str(), doc, 1);
- xmlFreeDoc(doc);
-
- return SEQUENCE_OK;
-}
-
-
-
-/*!
- Read camera parameters from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : subsampling of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : subsampling of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return error code.
- */
-int
-vpXmlParserCamera::read (xmlDocPtr doc, xmlNodePtr node,
- const std::string& cam_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int im_width,
- const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- // char * val_char;
- vpXmlCodeType prop;
-
- vpXmlCodeSequenceType back = SEQUENCE_OK;
- unsigned int nbCamera = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
- /*
- switch (prop)
- {
- case CODE_XML_CAMERA:
- if (SEQUENCE_OK == this->read_camera (doc, node, camera_name, projModel,
- image_width, image_height, subsampling_width, subsampling_height)){
- nbCamera++;
- }
- break;
- default:
- back = SEQUENCE_ERROR;
- break;
- }
- */
- if (prop == CODE_XML_CAMERA){
- if (SEQUENCE_OK == this->read_camera (doc, node, cam_name, projModel,
- im_width, im_height, subsampl_width, subsampl_height))
- nbCamera++;
- }
- else back = SEQUENCE_ERROR;
- }
-
- if (nbCamera == 0){
- back = SEQUENCE_ERROR;
- vpCERROR << "No camera parameters is available" << std::endl
- << "with your specifications" << std::endl;
- }
- else if(nbCamera > 1){
- back = SEQUENCE_ERROR;
- vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
- << "with your specifications : " << std::endl
- << "precise your choice..." << std::endl;
- }
-
- return back;
-}
-/*!
- Read camera parameters from a XML file and count the number of available
- sets of camera parameters corresponding with inputs.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : subsampling of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : subsampling of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return number of available camera parameters corresponding with inputs.
- */
-int
-vpXmlParserCamera::count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& cam_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int im_width,
- const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- // char * val_char;
- vpXmlCodeType prop;
- int nbCamera = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- }
- /*
- switch (prop)
- {
- case CODE_XML_CAMERA:
- if (SEQUENCE_OK == this->read_camera (doc, node, camera_name, projModel,
- image_width, image_height,
- subsampling_width, subsampling_height)){
- nbCamera++;
- }
- break;
- default:
- break;
- }
- */
- if (prop== CODE_XML_CAMERA) {
- if (SEQUENCE_OK == this->read_camera (doc, node, cam_name, projModel,
- im_width, im_height,
- subsampl_width, subsampl_height))
- nbCamera++;
- }
- }
-
- return nbCamera;
-}
-/*!
- Read camera headers from a XML file and return the last available
- node pointeur in the xml tree corresponding with inputs.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : subsampling of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : subsampling of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return number of available camera parameters corresponding with inputs.
- */
-xmlNodePtr
-vpXmlParserCamera::find_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& cam_name,
- const unsigned int im_width,
- const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- // char * val_char;
- vpXmlCodeType prop;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- }
- /*
- switch (prop)
- {
- case CODE_XML_CAMERA:
- if (SEQUENCE_OK == this->read_camera_header(doc, node, camera_name,
- image_width, image_height,
- subsampling_width, subsampling_height)){
- return node;
- }
- break;
- default:
- break;
- }
- */
- if(prop == CODE_XML_CAMERA){
- if (SEQUENCE_OK == this->read_camera_header(doc, node, cam_name,
- im_width, im_height,
- subsampl_width, subsampl_height))
- return node;
- }
- }
- return NULL;
-}
-
-/*!
- Read camera fields from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : scale of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : scale of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return error code.
-
- */
-int
-vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& cam_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int im_width,
- const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- vpXmlCodeType prop;
- /* read value in the XML file. */
- std::string camera_name_tmp = "";
- unsigned int image_height_tmp = 0 ;
- unsigned int image_width_tmp = 0 ;
- unsigned int subsampling_width_tmp = 0;
- unsigned int subsampling_height_tmp = 0;
- // unsigned int full_width_tmp = 0;
- // unsigned int full_height_tmp = 0;
- vpCameraParameters cam_tmp;
- vpCameraParameters cam_tmp_model;
- bool projModelFound = false;
- vpXmlCodeSequenceType back = SEQUENCE_OK;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
-
- switch (prop)
- {
- case CODE_XML_CAMERA_NAME: {
- char * val_char = xmlReadCharChild(doc, node);
- camera_name_tmp = val_char;
- std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
- xmlFree(val_char);
- break;
- }
- case CODE_XML_WIDTH:
- image_width_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
-
- case CODE_XML_HEIGHT:
- image_height_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- case CODE_XML_SUBSAMPLING_WIDTH:
- subsampling_width_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- case CODE_XML_SUBSAMPLING_HEIGHT:
- subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- // case CODE_XML_FULL_WIDTH:
- // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
-
- // case CODE_XML_FULL_HEIGHT:
- // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
-
- case CODE_XML_MODEL:
- back = read_camera_model(doc, node, cam_tmp_model);
- if(cam_tmp_model.get_projModel() == projModel){
- cam_tmp = cam_tmp_model;
- projModelFound = true;
- }
- break;
-
- case CODE_XML_BAD:
- case CODE_XML_OTHER:
- case CODE_XML_CAMERA:
- case CODE_XML_FULL_HEIGHT:
- case CODE_XML_FULL_WIDTH:
- case CODE_XML_MODEL_TYPE:
- case CODE_XML_U0:
- case CODE_XML_V0:
- case CODE_XML_PX:
- case CODE_XML_PY:
- case CODE_XML_KUD:
- case CODE_XML_KDU:
- default:
- back = SEQUENCE_ERROR;
- break;
- }
-
- }
- // Create a specific test for subsampling_width and subsampling_height to ensure that division by zero is not possible in the next test
- bool test_subsampling_width = true;
- bool test_subsampling_height = true;
-
- if (subsampling_width) {
- test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) < (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
- }
- if (subsampling_height) {
- test_subsampling_height = (abs((int)subsampl_height - (int)subsampling_height_tmp) < (allowedPixelDiffOnImageSize * (int)(subsampling_height_tmp / subsampling_height)));
- }
- if( !((projModelFound == true) && (cam_name == camera_name_tmp) &&
- (abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
- (abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
- (test_subsampling_width)&&
- (test_subsampling_height))){
- back = SEQUENCE_ERROR;
- }
- else{
- this->camera = cam_tmp;
- this->camera_name = camera_name_tmp;
- this->image_width = image_width_tmp;
- this->image_height = image_height_tmp;
- this->subsampling_width = subsampling_width_tmp;
- this->subsampling_height = subsampling_height_tmp;
- this->full_width = subsampling_width_tmp * image_width_tmp;
- this->full_height = subsampling_height_tmp * image_height_tmp;
- }
- return back;
-}
-/*!
- Read camera header fields from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : scale of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : scale of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return error code.
-
- */
-int
-vpXmlParserCamera::
-read_camera_header (xmlDocPtr doc, xmlNodePtr node,
- const std::string& cam_name,
- const unsigned int im_width,
- const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- vpXmlCodeType prop;
- /* read value in the XML file. */
- std::string camera_name_tmp = "";
- unsigned int image_height_tmp = 0 ;
- unsigned int image_width_tmp = 0 ;
- unsigned int subsampling_width_tmp = 0;
- unsigned int subsampling_height_tmp = 0;
- // unsigned int full_width_tmp = 0;
- // unsigned int full_height_tmp = 0;
- vpXmlCodeSequenceType back = SEQUENCE_OK;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
-
- switch (prop)
- {
- case CODE_XML_CAMERA_NAME:{
- char * val_char = xmlReadCharChild(doc, node);
- camera_name_tmp = val_char;
- xmlFree(val_char);
- }break;
-
- case CODE_XML_WIDTH:
- image_width_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
-
- case CODE_XML_HEIGHT:
- image_height_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- case CODE_XML_SUBSAMPLING_WIDTH:
- subsampling_width_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- case CODE_XML_SUBSAMPLING_HEIGHT:
- subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
- break;
- // case CODE_XML_FULL_WIDTH:
- // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
-
- // case CODE_XML_FULL_HEIGHT:
- // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
-
- case CODE_XML_MODEL:
- break;
-
- case CODE_XML_BAD:
- case CODE_XML_OTHER:
- case CODE_XML_CAMERA:
- case CODE_XML_FULL_HEIGHT:
- case CODE_XML_FULL_WIDTH:
- case CODE_XML_MODEL_TYPE:
- case CODE_XML_U0:
- case CODE_XML_V0:
- case CODE_XML_PX:
- case CODE_XML_PY:
- case CODE_XML_KUD:
- case CODE_XML_KDU:
- default:
- back = SEQUENCE_ERROR;
- break;
- }
- }
- if( !((cam_name == camera_name_tmp) &&
- (im_width == image_width_tmp || im_width == 0) &&
- (im_height == image_height_tmp || im_height == 0) &&
- (subsampl_width == subsampling_width_tmp ||
- subsampl_width == 0)&&
- (subsampl_height == subsampling_height_tmp ||
- subsampl_height == 0))){
- back = SEQUENCE_ERROR;
- }
- return back;
-}
-
-/*!
- Read camera model fields from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param cam_tmp : camera parameters to fill with read data (output).
-
- \return error code.
-
- */
-vpXmlParserCamera::vpXmlCodeSequenceType
-vpXmlParserCamera::read_camera_model (xmlDocPtr doc, xmlNodePtr node,
- vpCameraParameters &cam_tmp)
-{
- // counter of the number of read parameters
- int nb = 0;
- vpXmlCodeType prop;
- /* read value in the XML file. */
-
- char* model_type = NULL;
- double u0 = cam_tmp.get_u0();
- double v0 = cam_tmp.get_v0();
- double px = cam_tmp.get_px();
- double py = cam_tmp.get_py();
- double kud = cam_tmp.get_kud();
- double kdu = cam_tmp.get_kdu();
- vpXmlCodeSequenceType back = SEQUENCE_OK;
- int validation = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
- switch (prop)
- {
- case CODE_XML_MODEL_TYPE:{
- if(model_type != NULL){
- xmlFree(model_type);
- }
- model_type = xmlReadCharChild(doc, node);
- nb++;
- validation = validation | 0x01;
- }break;
- case CODE_XML_U0:
- u0 = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x02;
- break;
- case CODE_XML_V0:
- v0 = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x04;
- break;
- case CODE_XML_PX:
- px = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x08;
- break;
- case CODE_XML_PY:
- py = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x10;
- break;
- case CODE_XML_KUD:
- kud = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x20;
- break;
- case CODE_XML_KDU:
- kdu = xmlReadDoubleChild(doc, node);
- nb++;
- validation = validation | 0x40;
- break;
- case CODE_XML_BAD:
- case CODE_XML_OTHER:
- case CODE_XML_CAMERA:
- case CODE_XML_CAMERA_NAME:
- case CODE_XML_HEIGHT:
- case CODE_XML_WIDTH:
- case CODE_XML_SUBSAMPLING_WIDTH:
- case CODE_XML_SUBSAMPLING_HEIGHT:
- case CODE_XML_FULL_HEIGHT:
- case CODE_XML_FULL_WIDTH:
- case CODE_XML_MODEL:
- default:
- back = SEQUENCE_ERROR;
- break;
- }
- }
-
- if(model_type == NULL) {
- vpERROR_TRACE("projection model type doesn't match with any known model !");
- return SEQUENCE_ERROR;
- }
-
- if( !strcmp(model_type,LABEL_XML_MODEL_WITHOUT_DISTORTION)){
- if (nb != 5 || validation != 0x1F)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
- vpCERROR << "it must contain 5 parameters\n";
- xmlFree(model_type);
-
- return SEQUENCE_ERROR;
- }
- cam_tmp.initPersProjWithoutDistortion(px,py,u0,v0) ;
- }
- else if( !strcmp(model_type,LABEL_XML_MODEL_WITH_DISTORTION)){
- if (nb != 7 || validation != 0x7F)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
- vpCERROR << "it must contain 7 parameters\n";
- xmlFree(model_type);
-
- return SEQUENCE_ERROR;
- }
- cam_tmp.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
- }
- else{
- vpERROR_TRACE("projection model type doesn't match with any known model !");
- xmlFree(model_type);
-
- return SEQUENCE_ERROR;
- }
- xmlFree(model_type);
-
- return back;
-}
-
-/*!
- Write camera parameters in an XML Tree.
-
- \param node : XML tree, pointing on a marker equipement.
- \param cam_name : name of the camera : useful if the xml file has multiple
- camera parameters. Set as "" if the camera name is not ambiguous.
- \param im_width : width of image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration
- was performed. Set as 0 if not ambiguous.
- \param subsampl_width : subsampling of the image width sent by the camera.
- Set as 0 if not ambiguous.
- \param subsampl_height : subsampling of the image height sent by the camera.
- Set as 0 if not ambiguous.
-
- \return error code.
- */
-int vpXmlParserCamera::
-write (xmlNodePtr node, const std::string& cam_name,
- const unsigned int im_width, const unsigned int im_height,
- const unsigned int subsampl_width,
- const unsigned int subsampl_height)
-{
- int back = SEQUENCE_OK;
-
- xmlNodePtr node_tmp;
- xmlNodePtr node_camera;
-
- // <camera>
- node_camera = xmlNewNode(NULL,(xmlChar*)LABEL_XML_CAMERA);
- xmlAddChild(node,node_camera);
- {
- //<name>
-
- if(!cam_name.empty()){
- node_tmp = xmlNewComment((xmlChar*)"Name of the camera");
- xmlAddChild(node_camera,node_tmp);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_CAMERA_NAME,
- (xmlChar*)cam_name.c_str());
- }
-
- if(im_width != 0 || im_height != 0){
- char str[11];
- //<image_width>
- node_tmp = xmlNewComment((xmlChar*)"Size of the image on which camera calibration was performed");
- xmlAddChild(node_camera,node_tmp);
-
- sprintf(str,"%u",im_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_WIDTH,(xmlChar*)str);
- //<image_height>
-
- sprintf(str,"%u",im_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_HEIGHT,(xmlChar*)str);
- if(subsampling_width != 0 || subsampling_height != 0){
- node_tmp = xmlNewComment((xmlChar*)"Subsampling used to obtain the current size of the image.");
- xmlAddChild(node_camera,node_tmp);
-
- //<subsampling_width>
- sprintf(str,"%u",subsampl_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_WIDTH,
- (xmlChar*)str);
- //<subsampling_height>
- sprintf(str,"%u",subsampl_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_HEIGHT,
- (xmlChar*)str);
- node_tmp = xmlNewComment((xmlChar*)"The full size is the sensor size actually used to grab the image. full_width = subsampling_width * image_width");
- xmlAddChild(node_camera,node_tmp);
-
- //<full_width>
- sprintf(str,"%u",im_width*subsampl_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_WIDTH,
- (xmlChar*)str);
- //<full_height>
- sprintf(str,"%u",im_height*subsampl_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_HEIGHT,
- (xmlChar*)str);
- }
- }
-
- node_tmp = xmlNewComment((xmlChar*)"Intrinsic camera parameters computed for each projection model");
-
- xmlAddChild(node_camera,node_tmp);
-
- back = write_camera(node_camera);
- }
- return back;
-}
-/*!
- Write camera parameters in an XML Tree.
-
- \param node_camera : XML pointer node, pointing on a camera node.
-
- \return error code.
- */
-int vpXmlParserCamera::
-write_camera(xmlNodePtr node_camera){
- xmlNodePtr node_model;
- xmlNodePtr node_tmp;
-
- int back = SEQUENCE_OK;
- switch(camera.get_projModel()){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- //<model>
- node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
- xmlAddChild(node_camera,node_model);
- {
- char str[21];
- node_tmp = xmlNewComment((xmlChar*)"Projection model type");
- xmlAddChild(node_model,node_tmp);
-
- //<type>without_distortion</type>
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
- (xmlChar*)LABEL_XML_MODEL_WITHOUT_DISTORTION);
-
- node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
- xmlAddChild(node_model,node_tmp);
- //<px>
- sprintf(str,"%.10f",camera.get_px());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
- //<py>
- sprintf(str,"%.10f",camera.get_py());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
-
- node_tmp = xmlNewComment((xmlChar*)"Principal point");
- xmlAddChild(node_model,node_tmp);
-
- //<u0>
- sprintf(str,"%.10f",camera.get_u0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
- //<v0>
- sprintf(str,"%.10f",camera.get_v0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
- }
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- //<model>
- node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
- xmlAddChild(node_camera,node_model);
- {
- char str[21];
- node_tmp = xmlNewComment((xmlChar*)"Projection model type");
- xmlAddChild(node_model,node_tmp);
- //<type>with_distortion</type>
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
- (xmlChar*)LABEL_XML_MODEL_WITH_DISTORTION);
-
- node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
- xmlAddChild(node_model,node_tmp);
- //<px>
- sprintf(str,"%.10f",camera.get_px());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
- //<py>
- sprintf(str,"%.10f",camera.get_py());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
-
- node_tmp = xmlNewComment((xmlChar*)"Principal point");
- xmlAddChild(node_model,node_tmp);
- //<u0>
- sprintf(str,"%.10f",camera.get_u0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
- //<v0>
- sprintf(str,"%.10f",camera.get_v0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
-
- //<kud>
- node_tmp = xmlNewComment((xmlChar*)"Undistorted to distorted distortion parameter");
- xmlAddChild(node_model,node_tmp);
- sprintf(str,"%.10f",camera.get_kud());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KUD,(xmlChar*)str);
-
- //<kud>
- node_tmp = xmlNewComment((xmlChar*)"Distorted to undistorted distortion parameter");
- xmlAddChild(node_model,node_tmp);
- sprintf(str,"%.10f",camera.get_kdu());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KDU,(xmlChar*)str);
- }
- break;
- }
- return back;
-}
-
-/*!
- Translate a string (label) to a xml code.
- \param str : string to translate.
- \param res : resulting code.
-
- \return error code.
-*/
-
-vpXmlParserCamera::vpXmlCodeSequenceType
-vpXmlParserCamera::str2xmlcode (char * str, vpXmlCodeType & res)
-{
- vpXmlCodeType val_int = CODE_XML_BAD;
- vpXmlCodeSequenceType back = vpXmlParserCamera::SEQUENCE_OK;
-
- // DEBUG_TRACE (9, "# Entree :str=%s.", str);
-
- if (! strcmp (str, LABEL_XML_CAMERA))
- {
- val_int = CODE_XML_CAMERA;
- }
- else if (! strcmp (str, LABEL_XML_CAMERA_NAME))
- {
- val_int = CODE_XML_CAMERA_NAME;
- }
- else if (! strcmp (str, LABEL_XML_MODEL))
- {
- val_int = CODE_XML_MODEL;
- }
- else if (! strcmp (str, LABEL_XML_MODEL_TYPE))
- {
- val_int = CODE_XML_MODEL_TYPE;
- }
- else if (! strcmp (str, LABEL_XML_WIDTH))
- {
- val_int = CODE_XML_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_HEIGHT))
- {
- val_int = CODE_XML_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_SUBSAMPLING_WIDTH))
- {
- val_int = CODE_XML_SUBSAMPLING_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_SUBSAMPLING_HEIGHT))
- {
- val_int = CODE_XML_SUBSAMPLING_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_FULL_WIDTH))
- {
- val_int = CODE_XML_FULL_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_FULL_HEIGHT))
- {
- val_int = CODE_XML_FULL_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_U0))
- {
- val_int = CODE_XML_U0;
- }
- else if (! strcmp (str, LABEL_XML_V0))
- {
- val_int = CODE_XML_V0;
- }
- else if (! strcmp (str, LABEL_XML_PX))
- {
- val_int = CODE_XML_PX;
- }
- else if (! strcmp (str, LABEL_XML_PY))
- {
- val_int = CODE_XML_PY;
- }
- else if (! strcmp (str, LABEL_XML_KUD))
- {
- val_int = CODE_XML_KUD;
- }
- else if (! strcmp (str, LABEL_XML_KDU))
- {
- val_int = CODE_XML_KDU;
- }
- else
- {
- val_int = CODE_XML_OTHER;
- }
- res = val_int;
-
- return back;
-}
-#endif //VISP_HAVE_XML2
diff --git a/src/camera/vpXmlParserCamera.h b/src/camera/vpXmlParserCamera.h
deleted file mode 100644
index 26a9d99..0000000
--- a/src/camera/vpXmlParserCamera.h
+++ /dev/null
@@ -1,359 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParserCamera.h 5006 2014-11-24 15:57:45Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load and save camera intrinsic parameters.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpXmlParserCamera.h
- \brief Declaration of the vpXmlParserCamera class.
- Class vpXmlParserCamera allowed to load and save intrinsic camera parameters
-
-*/
-
-
-#ifndef vpXMLPARSERCAMERA_H
-#define vpXMLPARSERCAMERA_H
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <string>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpXmlParser.h>
-#include <libxml/xmlmemory.h> /* Functions of libxml. */
-
-/*!
- \class vpXmlParserCamera
-
- \ingroup CameraModelTransformation CameraModel
-
- \brief XML parser to load and save intrinsic camera parameters.
-
- To have a complete description of the camera parameters and the
- corresponding projection model implemented in ViSP, see
- vpCameraParameters.
-
- Example of an XML file "myXmlFile.xml" containing intrinsic camera
- parameters:
-
- \code
- <?xml version="1.0"?>
- <root>
- <camera>
- <name>myCamera</name>
- <image_width>640</image_width>
- <image_height>480</image_height>
- <model>
- <type>perspectiveProjWithoutDistortion</type>
- <px>1129.0</px>
- <py>1130.6</py>
- <u0>317.9</u0>
- <v0>229.1</v0>
- </model>
- <model>
- <type>perspectiveProjWithDistortion</type>
- <px>1089.9</px>
- <py>1090.1</py>
- <u0>326.1</u0>
- <v0>230.5</v0>
- <kud>-0.196</kud>
- <kdu>0.204</kdu>
- </model>
- </camera>
- </root>
- \endcode
-
- Example of loading existing camera parameters from an XML file:
- \code
-#include <visp/vpCameraParameters.h>
-#include <visp/vpXmlParserCamera.h>
-
-int main()
-{
- vpCameraParameters cam; // Create a camera parameter container
-
-#ifdef VISP_HAVE_XML2
- vpXmlParserCamera p; // Create a XML parser
- vpCameraParameters::vpCameraParametersProjType projModel; // Projection model
- // Use a perspective projection model without distortion
- projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
- // Parse the xml file "myXmlFile.xml" to find the intrinsic camera
- // parameters of the camera named "myCamera" for the image sizes 640x480,
- // for the projection model projModel. The size of the image is optional
- // if camera parameters are given only for one image size.
- if (p.parse(cam, "myXmlFile.xml", "myCamera", projModel,640,480) != vpXmlParserCamera::SEQUENCE_OK) {
- std::cout << "Cannot found myCamera" << std::endl;
- }
-
- // cout the parameters
- cam.printParameters();
-
- // Get the camera parameters for the model without distortion
- double px = cam.get_px();
- double py = cam.get_py();
- double u0 = cam.get_u0();
- double v0 = cam.get_v0();
-
- // Now we modify the principal point (u0,v0) for example to add noise
- u0 *= 0.9;
- v0 *= 0.8;
-
- // Set the new camera parameters
- cam.initPersProjWithoutDistortion(px, py, u0, v0);
-
- // Save the parameters in a new file "myXmlFileWithNoise.xml"
- p.save(cam,"myXmlFileWithNoise.xml",p.getCameraName(),p.getWidth(),p.getHeight());
-
- // Clean up memory allocated by the xml library
- vpXmlParser::cleanup();
-#endif
-}
- \endcode
-
- Example of writing an XML file containing intrinsic camera parameters:
- \code
-#include <visp/vpCameraParameters.h>
-#include <visp/vpXmlParserCamera.h>
-
-int main()
-{
- // Create a camera parameter container. We want to set these parameters
- // for a 320x240 image, and we want to use the perspective projection
- // modelisation without distortion.
- vpCameraParameters cam;
-
- // Set the principal point coordinates (u0,v0)
- double u0 = 162.3;
- double v0 = 122.4;
- // Set the pixel ratio (px, py)
- double px = 563.2;
- double py = 564.1;
-
- // Set the camera parameters for a model without distortion
- cam.initPersProjWithoutDistortion(px, py, u0, v0);
-
-#ifdef VISP_HAVE_XML2
- // Create a XML parser
- vpXmlParserCamera p;
- // Save the camera parameters in an XML file.
- if (p.save(cam, "myNewXmlFile.xml", "myNewCamera", 320, 240) != vpXmlParserCamera::SEQUENCE_OK) {
- std::cout << "Cannot save camera parameters" << std::endl;
- }
-
- // Clean up memory allocated by the xml library
- vpXmlParser::cleanup();
-#endif
-}
- \endcode
-*/
-
-class VISP_EXPORT vpXmlParserCamera: public vpXmlParser
-{
-
-public:
-
- /* --- XML Code------------------------------------------------------------ */
- typedef enum
- {
- CODE_XML_BAD = -1,
- CODE_XML_OTHER,
- CODE_XML_CAMERA,
- CODE_XML_CAMERA_NAME,
- CODE_XML_HEIGHT,
- CODE_XML_WIDTH,
- CODE_XML_SUBSAMPLING_WIDTH,
- CODE_XML_SUBSAMPLING_HEIGHT,
- CODE_XML_FULL_HEIGHT,
- CODE_XML_FULL_WIDTH,
- CODE_XML_MODEL,
- CODE_XML_MODEL_TYPE,
- CODE_XML_U0,
- CODE_XML_V0,
- CODE_XML_PX,
- CODE_XML_PY,
- CODE_XML_KUD,
- CODE_XML_KDU
- } vpXmlCodeType;
-
- typedef enum
- {
- SEQUENCE_OK ,
- SEQUENCE_ERROR
- } vpXmlCodeSequenceType;
-
-private :
-
- vpCameraParameters camera;
- std::string camera_name;
- unsigned int image_width;
- unsigned int image_height;
- unsigned int subsampling_width;
- unsigned int subsampling_height;
- unsigned int full_width;
- unsigned int full_height;
-
- //! Allowed size difference between input image and data from the xml parser to handle minor differences (ex. FORMAT7 can creates 648*488 images).
- static const int allowedPixelDiffOnImageSize = 15;
-
-public:
-
- vpXmlParserCamera();
- vpXmlParserCamera(vpXmlParserCamera& twinParser);
- vpXmlParserCamera& operator =(const vpXmlParserCamera& twinparser);
- ~vpXmlParserCamera(){}
-
- int parse(vpCameraParameters &cam, const std::string &filename,
- const std::string &camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0, const unsigned int image_height = 0);
- int save(const vpCameraParameters &cam, const std::string &filename,
- const std::string &camera_name,
- const unsigned int image_width = 0, const unsigned int image_height = 0);
-
- // get/set functions
- std::string getCameraName(){return this->camera_name;}
- unsigned int getWidth(){ return this->image_width; }
- unsigned int getHeight(){ return this->image_height; }
- unsigned int getSubsampling_width(){return this->subsampling_width;}
- unsigned int getSubsampling_height(){return this->subsampling_height;}
- vpCameraParameters getCameraParameters(){return this->camera;}
-
- void setCameraName(const std::string& name){
- this->camera_name = name;
- }
- void setWidth(const unsigned int width){ this->image_width = width ; }
- void setHeight(const unsigned int height){ this->image_height = height ; }
- void setSubsampling_width(const unsigned int subsampling){
- this->subsampling_width = subsampling ;
- }
- void setSubsampling_height(const unsigned int subsampling){
- this->subsampling_height = subsampling ;
- }
-
-private:
- int read (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- int count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- int read_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- xmlNodePtr find_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- vpXmlCodeSequenceType read_camera_model (xmlDocPtr doc, xmlNodePtr node,
- vpCameraParameters &camera);
-
- int read_camera_header (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
- void myXmlReadIntChild (xmlDocPtr doc,
- xmlNodePtr node,
- int &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadDoubleChild (xmlDocPtr doc,
- xmlNodePtr node,
- double &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadCharChild (xmlDocPtr doc,
- xmlNodePtr node,
- char **res);
- int write (xmlNodePtr node, const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
- int write_camera(xmlNodePtr node_camera);
-
-private:
-
- /*!
-
- \param 2doc : a pointer representing the document
- \param node : the root node of the document
- */
- virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
-
- /*!
-
-
- \param node2 : the root node of the document
- */
- virtual void writeMainClass(xmlNodePtr ){};
-
-};
-#endif //VISP_HAVE_XML2
-#endif
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/computer-vision/homography-estimation/vpHomography.cpp b/src/computer-vision/homography-estimation/vpHomography.cpp
deleted file mode 100644
index cb4ee30..0000000
--- a/src/computer-vision/homography-estimation/vpHomography.cpp
+++ /dev/null
@@ -1,768 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomography.cpp 4661 2014-02-10 19:34:58Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography transformation.
- *
- * Authors:
- * Muriel Pressigout
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpHomography.cpp
- \brief Definition de la classe vpHomography. Class that consider
- the particular case of homography
-*/
-
-#include <stdio.h>
-
-#include <visp/vpDebug.h>
-#include <visp/vpHomography.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpRobust.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-/*!
- \brief initialize an homography as Identity
-*/
-vpHomography::vpHomography() : data(NULL), aMb(), bP()
-{
- data = new double [9];
- setIdentity();
-}
-
-
-/*!
- \brief initialize an homography from another homography
-*/
-
-vpHomography::vpHomography(const vpHomography &H) : data(NULL), aMb(), bP()
-{
- data = new double [9];
-
- *this = H;
-}
-
-/*!
- \brief initialize an homography from another homography
-*/
-vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : data(NULL), aMb(), bP()
-{
- data = new double [9];
- buildFrom(M, p) ;
-}
-
-vpHomography::vpHomography(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &p) : data(NULL), aMb(), bP()
-{
- data = new double [9];
- buildFrom(tu, atb, p) ;
-}
-
-vpHomography::vpHomography(const vpRotationMatrix &aRb,
- const vpTranslationVector &atb,
- const vpPlane &p) : data(NULL), aMb(), bP()
-{
- data = new double [9];
- buildFrom(aRb, atb, p) ;
-}
-
-vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : data(NULL), aMb(), bP()
-{
- data = new double [9];
- buildFrom(arb, p) ;
-}
-
-vpHomography::~vpHomography()
-{
- if (data)
- delete [] data;
- data = NULL;
-}
-
-void
-vpHomography::buildFrom(const vpHomogeneousMatrix &M,
- const vpPlane &p)
-{
- insert(M) ;
- insert(p) ;
- build() ;
-}
-
-void
-vpHomography::buildFrom(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &p)
-{
- insert(tu) ;
- insert(atb) ;
- insert(p) ;
- build() ;
-}
-
-void
-vpHomography::buildFrom(const vpRotationMatrix &aRb,
- const vpTranslationVector &atb,
- const vpPlane &p)
-{
- insert(aRb) ;
- insert(atb) ;
- insert(p) ;
- build() ;
-}
-
-void
-vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
-{
- aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
- insert(p) ;
- build() ;
-}
-
-
-
-/*********************************************************************/
-
-
-/*!
- \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;
-}
-
-/*!
- \brief Invert the homography
-
- \return \f$\bf H^{-1}\f$
-*/
-vpHomography
-vpHomography::inverse() const
-{
- vpMatrix M = (*this);
- vpMatrix Minv;
- M.pseudoInverse(Minv, 1e-16);
-
- vpHomography H;
-
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
- H[i][j] = Minv[i][j];
-
- return H;
-}
-
-/*!
- \brief Invert the homography.
-
- \param bHa : \f$\bf H^{-1}\f$ with H = *this.
-*/
-void
-vpHomography::inverse(vpHomography &bHa) const
-{
- bHa = inverse() ;
-}
-
-
-void
-vpHomography::save(std::ofstream &f) const
-{
- if (! f.fail())
- {
- f << *this ;
- }
- else
- {
- throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")) ;
- }
-}
-
-/*!
-
- 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;
- for(unsigned int i = 0; i < 3; i++) {
- for(unsigned int j = 0; j < 3; j++) {
- double s = 0.;
- for(unsigned int k = 0; k < 3; k ++) {
- s += (*this)[i][k] * H[k][j];
- }
- Hp[i][j] = s;
- }
- }
- return Hp;
-}
-
-/*!
- Operation a = aHb * b.
-
- \param b : 3 dimension vector.
-*/
-vpColVector
-vpHomography::operator*(const vpColVector &b) const
-{
- if (b.size() != 3)
- throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));
-
- vpColVector a(3);
- for(unsigned int i=0; i<3; i++) {
- a[i] = 0.;
- for(unsigned int j=0; j<3; j++)
- a[i] += (*this)[i][j] * b[j];
- }
-
- 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;
-
- for (unsigned int i=0; i < 9; i ++) {
- H.data[i] = this->data[i] * v;
- }
-
- return H;
-}
-
-/*!
-
- 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;
- if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
-
- double vinv = 1/v ;
-
- for (unsigned int i=0; i < 9; i ++) {
- H.data[i] = this->data[i] * vinv;
- }
-
- return H;
-}
-
-//! Divide all the element of the homography matrix by v : Hij = Hij / v
-vpHomography & vpHomography::operator/=(double v)
-{
- //if (x == 0)
- if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
-
- double vinv = 1/v ;
-
- for (unsigned int i=0;i<9;i++)
- data[i] *= vinv;
-
- 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++)
- for (unsigned int j=0; j< 3; j++)
- (*this)[i][j] = H[i][j];
-
- 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)
- throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
-
- for (unsigned int i=0; i< 3; i++)
- for (unsigned int j=0; j< 3; j++)
- (*this)[i][j] = H[i][j];
-
- return *this;
-}
-
-/*!
- Read an homography in a file, verify if it is really an homogeneous
- matrix.
-
- \param f : the file.
-*/
-void
-vpHomography::load(std::ifstream &f)
-{
- if (! f.fail())
- {
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- {
- f >> (*this)[i][j] ;
- }
- }
- else
- {
- throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
- }
-}
-
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-//! Print the homography as a matrix.
-void
-vpHomography::print()
-{
- std::cout <<*this << std::endl ;
-}
-#endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- \brief Compute aHb such that
-
- \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
- { ^b{\bf n}^T}
- \f]
-*/
-void
-vpHomography::build()
-{
- vpColVector n(3) ;
- vpColVector atb(3) ;
- vpMatrix aRb(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- atb[i] = aMb[i][3];
- for (unsigned int j=0 ; j < 3 ; j++)
- aRb[i][j] = aMb[i][j];
- }
-
- bP.getNormal(n) ;
-
- double d = bP.getD() ;
- vpMatrix aHb = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
- // plane equation. So if the plane is described by
- // Ax+By+Cz+D=0, d=-D
-
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- (*this)[i][j] = aHb[i][j];
-}
-
-/*!
- \brief Compute aHb such that
-
- \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
- { ^b{\bf n}^T}
- \f]
- //note d => -d verifier
-*/
-void
-vpHomography::build(vpHomography &aHb,
- const vpHomogeneousMatrix &aMb,
- const vpPlane &bP)
-{
- vpColVector n(3) ;
- vpColVector atb(3) ;
- vpMatrix aRb(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- atb[i] = aMb[i][3] ;
- for (unsigned int j=0 ; j < 3 ; j++)
- aRb[i][j] = aMb[i][j];
- }
-
- bP.getNormal(n) ;
-
- double d = bP.getD() ;
- vpMatrix aHb_ = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
- // plane equation. So if the plane is described by
- // Ax+By+Cz+D=0, d=-D
-
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- aHb[i][j] = aHb_[i][j];
-}
-
-/*!
- Set the homography as identity transformation.
-*/
-void
-vpHomography::setIdentity()
-{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
- else
- (*this)[i][j] = 0.0;
-}
-
-/*!
- Given \c iPa a point with coordinates \f$(u_a,v_a)\f$ expressed in pixel in image a, and the homography \c bHa that
- links image a and b, computes the coordinates of the point \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();
- double ya = iPa.get_v();
- vpMatrix H = cam.get_K() * bHa * cam.get_K_inverse();
- double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
- double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
- double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
-
- vpImagePoint iPb(yb, xb);
-
- return iPb;
-}
-/*!
- Given \c Pa a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the image plane a, and the homography \c bHa 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();
- double ya = Pa.get_y();
- double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
- double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
- double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
-
- vpPoint Pb;
- Pb.set_x(xb);
- Pb.set_y(yb);
-
- 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<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- double weights_threshold,
- unsigned int niter,
- bool normalization)
-{
- unsigned int n = (unsigned int) xb.size();
- if (yb.size() != n || xa.size() != n || ya.size() != n)
- throw(vpException(vpException::dimensionError,
- "Bad dimension for robust homography estimation"));
-
- // 4 point are required
- if(n<4)
- throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
-
- try{
- std::vector<double> xan, yan, xbn, ybn;
-
- double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
-
- vpHomography aHbn;
-
- if (normalization) {
- vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
- vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
- }
- else {
- xbn = xb;
- ybn = yb;
- xan = xa;
- yan = ya;
- }
-
- unsigned int nbLinesA = 2;
- vpMatrix A(nbLinesA*n,8);
- vpColVector X(8);
- vpColVector Y(nbLinesA*n);
- vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix
-
- vpColVector w(nbLinesA*n) ;
-
- // All the weights are set to 1 at the beginning to use a classical least square scheme
- w = 1;
- // Update the square matrix associated to the weights
- for (unsigned int i=0; i < nbLinesA*n; i ++) {
- W[i][i] = w[i];
- }
-
- // build matrix A
- for(unsigned int i=0; i<n;i++) {
- A[nbLinesA*i][0]=xbn[i];
- A[nbLinesA*i][1]=ybn[i];
- A[nbLinesA*i][2]=1;
- A[nbLinesA*i][3]=0 ;
- A[nbLinesA*i][4]=0 ;
- A[nbLinesA*i][5]=0;
- A[nbLinesA*i][6]=-xbn[i]*xan[i] ;
- A[nbLinesA*i][7]=-ybn[i]*xan[i];
-
- A[nbLinesA*i+1][0]=0 ;
- A[nbLinesA*i+1][1]=0;
- A[nbLinesA*i+1][2]=0;
- A[nbLinesA*i+1][3]=xbn[i];
- A[nbLinesA*i+1][4]=ybn[i];
- A[nbLinesA*i+1][5]=1;
- A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
- A[nbLinesA*i+1][7]=-ybn[i]*yan[i];
-
- Y[nbLinesA*i] = xan[i];
- Y[nbLinesA*i+1] = yan[i];
- }
-
- vpMatrix WA;
- vpMatrix WAp;
- unsigned int iter = 0;
- vpRobust r(nbLinesA*n) ; // M-Estimator
-
- while (iter < niter) {
- WA = W * A;
-
- X = WA.pseudoInverse(1e-26)*W*Y;
- vpColVector residu;
- residu = Y - A * X;
-
- // Compute the weights using the Tukey biweight M-Estimator
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY, residu, w) ;
-
- // Update the weights matrix
- for (unsigned int i=0; i < n*nbLinesA; i ++) {
- W[i][i] = w[i];
- }
- // Build the homography
- for(unsigned int i=0;i<8;i++)
- aHbn.data[i]= X[i];
- aHbn[2][2] = 1;
- {
- vpMatrix aHbnorm = aHbn;
- aHbnorm /= aHbnorm[2][2] ;
- }
-
- iter ++;
- }
- inliers.resize(n);
- unsigned int nbinliers = 0;
- for(unsigned int i=0; i< n; i++) {
- if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
- inliers[i] = false;
- else {
- inliers[i] = true;
- nbinliers ++;
- }
- }
-
- if (normalization) {
- // H after denormalization
- vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
- }
- else {
- aHb = aHbn;
- }
-
- residual = 0 ;
- vpColVector a(3), b(3), c(3);
- for (unsigned int i=0 ; i < n ; i++) {
- if (inliers[i]) {
- a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
- b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- residual += (a-c).sumSquare();
- }
- }
-
- residual = sqrt(residual/nbinliers);
- }
- catch(...)
- {
- throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
- }
-}
-
-/*!
-\brief std::cout a matrix
-*/
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHomography &H)
-{
- std::ios_base::fmtflags original_flags = s.flags();
-
- s.precision(10) ;
- for (unsigned int i=0;i<H.getRows();i++) {
- for (unsigned int j=0;j<H.getCols();j++){
- s << H[i][j] << " ";
- }
- // We don't add a \n char on the end of the last matrix line
- if (i < H.getRows()-1)
- s << std::endl;
- }
-
- s.flags(original_flags); // restore s to standard state
-
- return s;
-}
diff --git a/src/computer-vision/homography-estimation/vpHomography.h b/src/computer-vision/homography-estimation/vpHomography.h
deleted file mode 100644
index c7c8853..0000000
--- a/src/computer-vision/homography-estimation/vpHomography.h
+++ /dev/null
@@ -1,442 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomography.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography transformation.
- *
- * Authors:
- * Muriel Pressigout
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
-\file vpHomography.h
-
-This file defines an homography transformation. This class aims to provide some
-tools for homography computation.
-*/
-
-#ifndef vpHomography_hh
-#define vpHomography_hh
-
-#include <list>
-#include <vector>
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPlane.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-/*!
-
- \class vpHomography
- \ingroup Homography HomographyTransformation
- \brief This class aims to compute the homography wrt.two images.
-
- These two images are both described by a set of points. The 2 sets (one per
- image) are sets of corresponding points : for a point in a image, there is
- the corresponding point (image of the same 3D point) in the other image
- points set. These 2 sets are the only data needed to compute the
- homography. One method used is the one introduced by Ezio Malis during his
- PhD \cite TheseMalis. A normalization is carried out on this points in order to improve the
- conditioning of the problem, what leads to improve the stability of the
- result.
-
- Store and compute the homography such that
- \f[
- ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}
- \f]
-
- with
- \f[ ^a{\bf H}_b = ^a{\bf R}_b + \frac{^a{\bf t}_b}{^bd}
- { ^b{\bf n}^T}
- \f]
-
- The \ref tutorial-homography explains how to use this class.
-
- The example below shows also how to manipulate this class to first
- compute a ground truth homography from camera poses, project pixel
- coordinates points using an homography and lastly estimate an
- homography from a subset of 4 matched points in frame a and frame b
- respectively.
-
- \code
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-
-int main()
-{
- // Initialize in the object frame the coordinates in meters of 4 points that
- // belong to a planar object
- vpPoint Po[4];
- Po[0].setWorldCoordinates(-0.1, -0.1, 0);
- Po[1].setWorldCoordinates( 0.2, -0.1, 0);
- Po[2].setWorldCoordinates( 0.1, 0.1, 0);
- Po[3].setWorldCoordinates(-0.1, 0.3, 0);
-
- // Initialize the pose between camera frame a and object frame o
- vpHomogeneousMatrix aMo(0, 0, 1, 0, 0, 0); // Camera is 1 meter far
-
- // Initialize the pose between camera frame a and camera frame
- // b. These two frames correspond for example to two successive
- // camera positions
- vpHomogeneousMatrix aMb(0.2, 0.1, 0, 0, 0, vpMath::rad(2));
-
- // Compute the pose between camera frame b and object frame
- vpHomogeneousMatrix bMo = aMb.inverse() * aMo;
-
- // Initialize camera intrinsic parameters
- vpCameraParameters cam;
-
- // Compute the coordinates in pixels of the 4 object points in the
- // camera frame a
- vpPoint Pa[4];
- std::vector<double> xa(4), ya(4); // Coordinates in pixels of the points in frame a
- for(int i=0 ; i < 4 ; i++) {
- Pa[i] = Po[i];
- Pa[i].project(aMo); // Project the points from object frame to
- // camera frame a
- vpMeterPixelConversion::convertPoint(cam,
- Pa[i].get_x(), Pa[i].get_y(),
- xa[i], ya[i]);
- }
-
- // Compute the coordinates in pixels of the 4 object points in the
- // camera frame b
- vpPoint Pb[4];
- std::vector<double> xb(4), yb(4); // Coordinates in pixels of the points in frame b
- for(int i=0 ; i < 4 ; i++) {
- Pb[i] = Po[i];
- Pb[i].project(bMo); // Project the points from object frame to
- // camera frame a
- }
-
- // Compute equation of the 3D plane containing the points in camera frame b
- vpPlane bP(Pb[0], Pb[1], Pb[2]);
-
- // Compute the corresponding ground truth homography
- vpHomography aHb(aMb, bP);
-
- std::cout << "Ground truth homography aHb: \n" << aHb<< std::endl;
-
- // Compute the coordinates of the points in frame b using the ground
- // truth homography and the coordinates of the points in frame a
- vpHomography bHa = aHb.inverse();
- for(int i = 0; i < 4 ; i++){
- double inv_z = 1. / (bHa[2][0] * xa[i] + bHa[2][1] * ya[i] + bHa[2][2]);
-
- xb[i] = (bHa[0][0] * xa[i] + bHa[0][1] * ya[i] + bHa[0][2]) * inv_z;
- yb[i] = (bHa[1][0] * xa[i] + bHa[1][1] * ya[i] + bHa[1][2]) * inv_z;
- }
-
- // Estimate the homography from 4 points coordinates expressed in pixels
- vpHomography::DLT(xb, yb, xa, ya, aHb, true);
- aHb /= aHb[2][2]; // Apply a scale factor to have aHb[2][2] = 1
-
- std::cout << "Estimated homography aHb: \n" << aHb<< std::endl;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpHomography
-{
- public:
- //! Data array
- double *data;
-
- private:
- static const double sing_threshold; // = 0.0001;
- static const double threshold_rotation;
- static const double threshold_displacement;
- vpHomogeneousMatrix aMb ;
- // bool isplanar;
- //! reference plane coordinates expressed in Rb
- vpPlane bP ;
-
- private:
- //! build the homography from aMb and Rb
- void build() ;
-
- //! insert a rotation matrix
- void insert(const vpHomogeneousMatrix &aRb) ;
- //! insert a rotation matrix
- void insert(const vpRotationMatrix &aRb) ;
- //! insert a theta u vector (transformation into a rotation matrix)
- void insert(const vpThetaUVector &tu) ;
- //! insert a translation vector
- void insert(const vpTranslationVector &atb) ;
- //! insert a translation vector
- void insert(const vpPlane &bP) ;
-
- static void initRansac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya,
- vpColVector &x) ;
-
- public:
- vpHomography() ;
- vpHomography(const vpHomography &H) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpHomogeneousMatrix &aMb,
- const vpPlane &bP) ;
- //! 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
- vpHomography(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpPoseVector &arb,
- const vpPlane &bP) ;
- virtual ~vpHomography();
-
- //! 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
- void buildFrom(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! 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) ;
-
- void computeDisplacement(vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- void computeDisplacement(const vpColVector& nd,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- //! Return the number of rows of the homography matrix.
- inline unsigned int getRows() const { return 3;}
- //! Return the number of columns of the homography matrix.
- inline unsigned int getCols() const { return 3; }
-
- //! invert the homography
- vpHomography inverse() const ;
- //! invert the homography
- void inverse(vpHomography &Hi) const;
-
- //! Load an homography from a file
- void load(std::ifstream &f) ;
-
- //! Write elements Hij (usage : H[i][j] = x )
- inline double *operator[](unsigned int i) { return &data[i*3]; }
- //! Read elements Hij (usage : x = H[i][j] )
- inline double *operator[](unsigned int i) const {return &data[i*3];}
-
- // Multiplication by an homography
- vpHomography operator*(const vpHomography &H) const;
-
- // Multiplication by a scalar
- vpHomography operator*(const double &v) const;
- vpColVector operator*(const vpColVector &b) const;
-
- // Division by a scalar
- vpHomography operator/(const double &v) const;
- vpHomography & operator/=(double v);
- vpHomography & operator=(const vpHomography &H);
- vpHomography & operator=(const vpMatrix &H);
-
- //! Save an homography in a file
- void save(std::ofstream &f) const ;
-
- void setIdentity();
-
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s,const vpHomography &H);
-
- static void DLT(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya ,
- vpHomography &aHb,
- bool normalization=true);
-
- static void HLM(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- bool isplanar,
- vpHomography &aHb) ;
-
- static bool ransac(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- unsigned int nbInliersConsensus,
- double threshold,
- bool normalization=true);
-
- static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa);
- static vpPoint project(const vpHomography &bHa, const vpPoint &Pa);
-
- static void robust(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inlier,
- double &residual,
- double weights_threshold=0.4,
- unsigned int niter=4,
- bool normalization=true);
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- //! build the homography from aMb and Rb
- static void build(vpHomography &aHb,
- const vpHomogeneousMatrix &aMb,
- const vpPlane &bP) ;
-
- static void computeDisplacement(const vpHomography &aHb,
- const vpColVector& nd,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- static void computeDisplacement (const vpHomography &aHb,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- static void computeDisplacement(const vpMatrix &H,
- const double x,
- const double y,
- std::list<vpRotationMatrix> & vR,
- std::list<vpTranslationVector> & vT,
- std::list<vpColVector> & vN) ;
- static double computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane &oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- ) ;
- static double computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane *oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- ) ;
- static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d);
- // VVS
- static double computeRotation(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpHomogeneousMatrix &c2Mc1,
- int userobust) ;
- static void computeTransformation(vpColVector &x,unsigned int *ind, vpColVector &M) ;
- static bool degenerateConfiguration(vpColVector &x,unsigned int *ind) ;
- static bool degenerateConfiguration(vpColVector &x,unsigned int *ind, double threshold_area);
- static bool degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya);
- static void HartleyNormalization(unsigned int n,
- const double *x, const double *y,
- double *xn, double *yn,
- double &xg, double &yg,
- double &coef);
- static void HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
- std::vector<double> &xn, std::vector<double> &yn,
- double &xg, double &yg, double &coef);
- static void HartleyDenormalization(vpHomography &aHbn,
- vpHomography &aHb,
- double xg1, double yg1, double coef1,
- double xg2, double yg2, double coef2 ) ;
-
-#endif // DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated static void computeDisplacement(const vpMatrix H,
- const double x,
- const double y,
- vpList<vpRotationMatrix> & vR,
- vpList<vpTranslationVector> & vT,
- vpList<vpColVector> & vN);
-
- static void DLT(unsigned int n,
- double *xb, double *yb ,
- double *xa, double *ya,
- vpHomography &aHb) ;
- vp_deprecated static void HartleyDLT(unsigned int n,
- double *xb, double *yb ,
- double *xa, double *ya,
- vpHomography &aHb) ;
-
- static void HLM(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- bool isplan,
- vpHomography &aHb) ;
-
- //! Print the matrix
- vp_deprecated void print() ;
-
- vp_deprecated static bool ransac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &aHb,
- int consensus = 1000,
- double threshold = 1e-6) ;
-
- vp_deprecated static bool ransac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &aHb,
- vpColVector& inliers,
- double residual = 0.1, // Not used
- int consensus = 1000,
- double threshold = 1e-6,
- double areaThreshold = 0.0);
-
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
-} ;
-
-
-
-#endif
diff --git a/src/computer-vision/homography-estimation/vpHomographyDLT.cpp b/src/computer-vision/homography-estimation/vpHomographyDLT.cpp
deleted file mode 100644
index 10f4ff5..0000000
--- a/src/computer-vision/homography-estimation/vpHomographyDLT.cpp
+++ /dev/null
@@ -1,631 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomographyDLT.cpp 5052 2014-12-11 14:18:49Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography estimation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpHomographyDLT.cpp
-
- This file implements the fonctions related with the homography
- estimation using the DLT algorithm
-*/
-
-#include <visp/vpHomography.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpMatrixException.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void
-vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
- std::vector<double> &xn, std::vector<double> &yn,
- double &xg, double &yg, double &coef)
-{
- if (x.size() != y.size())
- throw(vpException(vpException::dimensionError,
- "Hartley normalization require that x and y vector have the same dimension"));
-
- unsigned int n = (unsigned int) x.size();
- if (xn.size() != n)
- xn.resize(n);
- if (yn.size() != n)
- yn.resize(n);
-
- xg = 0 ;
- yg = 0 ;
-
- for (unsigned int i =0 ; i < n ; i++)
- {
- xg += x[i] ;
- yg += y[i] ;
- }
- xg /= n ;
- yg /= n ;
-
- // Changement d'origine : le centre de gravite doit correspondre
- // a l'origine des coordonnees
- double distance=0;
- for(unsigned int i=0; i<n;i++)
- {
- double xni=x[i]-xg;
- double yni=y[i]-yg;
- xn[i] = xni ;
- yn[i] = yni ;
- distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
- }//for translation sur tous les points
-
- //Changement d'echelle
- distance/=n;
- //calcul du coef de changement d'echelle
- //if(distance ==0)
- if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
- coef=1;
- else
- coef=sqrt(2.0)/distance;
-
- for(unsigned int i=0; i<n;i++)
- {
- xn[i] *= coef;
- yn[i] *= coef;
- }
-}
-
-void
-vpHomography::HartleyNormalization(unsigned int n,
- const double *x, const double *y,
- double *xn, double *yn,
- double &xg, double &yg,
- double &coef)
-{
- unsigned int i;
- xg = 0 ;
- yg = 0 ;
-
- for (i =0 ; i < n ; i++)
- {
- xg += x[i] ;
- yg += y[i] ;
- }
- xg /= n ;
- yg /= n ;
-
- //Changement d'origine : le centre de gravite doit correspondre
- // a l'origine des coordonnees
- double distance=0;
- for(i=0; i<n;i++)
- {
- double xni=x[i]-xg;
- double yni=y[i]-yg;
- xn[i] = xni ;
- yn[i] = yni ;
- distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
- }//for translation sur tous les points
-
- //Changement d'echelle
- distance/=n;
- //calcul du coef de changement d'echelle
- //if(distance ==0)
- if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
- coef=1;
- else
- coef=sqrt(2.0)/distance;
-
- for(i=0; i<n;i++)
- {
- xn[i] *= coef;
- yn[i] *= coef;
- }
-}
-
-//---------------------------------------------------------------------------------------
-
-void
-vpHomography::HartleyDenormalization(vpHomography &aHbn,
- vpHomography &aHb,
- double xg1, double yg1, double coef1,
- double xg2, double yg2, double coef2 )
-{
-
- //calcul des transformations a appliquer sur M_norm pour obtenir M
- //en fonction des deux normalisations effectuees au debut sur
- //les points: aHb = T2^ aHbn T1
- vpMatrix T1(3,3);
- vpMatrix T2(3,3);
- vpMatrix T2T(3,3);
-
- T1.setIdentity();
- T2.setIdentity();
- T2T.setIdentity();
-
- T1[0][0]=T1[1][1]=coef1;
- T1[0][2]=-coef1*xg1 ;
- T1[1][2]=-coef1*yg1 ;
-
- T2[0][0]=T2[1][1]=coef2;
- T2[0][2]=-coef2*xg2 ;
- T2[1][2]=-coef2*yg2 ;
-
- T2T=T2.pseudoInverse(1e-16) ;
-
- vpMatrix aHbn_(3,3);
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
- aHbn_[i][j] = aHbn[i][j];
-
- vpMatrix maHb=T2T*aHbn_*T1;
-
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- aHb[i][j] = maHb[i][j] ;
-}
-
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
-
- \deprecated You should rather use
- vpHomography::DLT(const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, vpHomography &, bool)
-
- \brief Computes the homography matrix using the DLT (Direct Linear
- Transform) algorithm on normalized data.
-
- Normalizes data, computes H wrt. these normalized data and denormalizes
- the result. The normalization carried out is the one preconized by Hartley .
- At least 4 correspondant points couples are needed.
-
- \sa DLT()
-
- \exception vpMatrixException::rankDeficient : When the rank of the matrix
- that should be 8 is deficient.
-
-*/
-void
-vpHomography::HartleyDLT(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &aHb)
-{
- try{
- //initialise les donnees initiales
- // code_retour =InitialData(n, p1,p2);
-
- // normalize points
- double *xbn;
- double *ybn;
- xbn = new double [n];
- ybn = new double [n];
-
- double xg1, yg1, coef1 ;
- vpHomography::HartleyNormalization(n,
- xb,yb,
- xbn,ybn,
- xg1, yg1,coef1);
-
- double *xan;
- double *yan;
- xan = new double [n];
- yan = new double [n];
-
- double xg2, yg2, coef2 ;
- vpHomography::HartleyNormalization(n,
- xa,ya,
- xan,yan,
- xg2, yg2,coef2);
-
- vpHomography aHbn ;
- //compute the homography using the DLT from normalized data
- vpHomography::DLT(n,xbn,ybn,xan,yan,aHbn);
-
- //H denormalisee
- vpHomography::HartleyDenormalization(aHbn,aHb,xg1,yg1,coef1,xg2,yg2, coef2);
-
- delete [] xbn;
- delete [] ybn;
- delete [] xan;
- delete [] yan;
-
- }
- catch(...)
- {
- vpTRACE(" ") ;
- throw ;
- }
-}
-#endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- \deprecated You should rather use
- vpHomography::DLT(const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, vpHomography &, bool)
-
- \brief Computes the homography matrix wrt. the
- data using the DLT (Direct Linear Transform) algorithm.
-
- Computes H such as \f[
- ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}
- \f]
-
- To do so, we use the DLT algorithm on the data,
- ie we resolve the linear system by SDV : \f$\bf{Ah} =0\f$.
- \f$\bf{h}\f$ is the vector with the terms of \f$\mathbf{H}\f$,
-
- \f$\mathbf{A}\f$ depends on the points coordinates.
-
- At least 4 correspondant points couples are needed.
-
- For each point, in homogeneous coordinates we have:
- \f[
- \mathbf{p}_{a}= \mathbf{H}\mathbf{p}_{b}
- \f]
- which is equivalent to:
- \f[
- \mathbf{p}_{a} \times \mathbf{H}\mathbf{p}_{b} =0
- \f]
- If we note \f$\mathbf{h}_j^T\f$ the \f$j^{\textrm{th}}\f$ line of \f$\mathbf{H}\f$, we can write:
- \f[
- \mathbf{H}\mathbf{p}_{b} = \left( \begin{array}{c}\mathbf{h}_1^T\mathbf{p}_{b} \\\mathbf{h}_2^T\mathbf{p}_{b} \\\mathbf{h}_3^T \mathbf{p}_{b} \end{array}\right)
- \f]
-
- Setting \f$\mathbf{p}_{a}=(x_{a},y_{a},w_{a})\f$, the cross product can be rewritten by:
- \f[
- \mathbf{p}_{a} \times \mathbf{H}\mathbf{p}_{b} =\left( \begin{array}{c}y_{a}\mathbf{h}_3^T\mathbf{p}_{b}-w_{a}\mathbf{h}_2^T\mathbf{p}_{b} \\w_{a}\mathbf{h}_1^T\mathbf{p}_{b} -x_{a}\mathbf{h}_3^T \mathbf{p}_{b} \\x_{a}\mathbf{h}_2^T \mathbf{p}_{b}- y_{a}\mathbf{h}_1^T\mathbf{p}_{b}\end{array}\right)
- \f]
-
- \f[
- \underbrace{\left( \begin{array}{ccc}\mathbf{0}^T & -w_{a} \mathbf{p}_{b}^T
- & y_{a} \mathbf{p}_{b}^T \\ w_{a}
- \mathbf{p}_{b}^T&\mathbf{0}^T & -x_{a} \mathbf{p}_{b}^T \\
- -y_{a} \mathbf{p}_{b}^T & x_{a} \mathbf{p}_{b}^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 solve: \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]
- <b>h</b> is the column of <b>V</b> associated with the smalest singular value of <b>A
- </b>
-
- \exception vpMatrixException::rankDeficient : When the rank of the matrix
- that should be 8 is deficient.
-*/
-void vpHomography::DLT(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &aHb)
-{
-
- // 4 point are required
- if(n<4)
- {
- vpTRACE("there must be at least 4 points in the both images\n") ;
- throw ;
- }
-
- try{
- vpMatrix A(2*n,9);
- vpColVector h(9);
- vpColVector D(9);
- vpMatrix V(9,9);
-
- // We need here to compute the SVD on a (n*2)*9 matrix (where n is
- // the number of points). if n == 4, the matrix has more columns
- // than rows. This kind of matrix is not supported by GSL for
- // SVD. The solution is to add an extra line with zeros
- if (n == 4)
- A.resize(2*n+1,9);
-
- // build matrix A
- for(unsigned int i=0; i<n;i++)
- {
- A[2*i][0]=0;
- A[2*i][1]=0;
- A[2*i][2]=0;
- A[2*i][3]=-xb[i] ;
- A[2*i][4]=-yb[i] ;
- A[2*i][5]=-1;
- A[2*i][6]=xb[i]*ya[i] ;
- A[2*i][7]=yb[i]*ya[i];
- A[2*i][8]=ya[i];
-
-
- A[2*i+1][0]=xb[i] ;
- A[2*i+1][1]=yb[i] ;
- A[2*i+1][2]=1;
- A[2*i+1][3]=0;
- A[2*i+1][4]=0;
- A[2*i+1][5]=0;
- A[2*i+1][6]=-xb[i]*xa[i];
- A[2*i+1][7]=-yb[i]*xa[i];
- A[2*i+1][8]=-xa[i] ;
- }
-
- // Add an extra line with zero.
- if (n == 4) {
- for (unsigned int i=0; i < 9; i ++) {
- A[2*n][i] = 0;
- }
- }
-
- // solve Ah = 0
- // SVD Decomposition A = UDV^T (destructive wrt A)
- A.svd(D,V);
-
- // on en profite pour effectuer un controle sur le rang de la matrice :
- // pas plus de 2 valeurs singulieres quasi=0
- int rank=0;
- for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
- if(rank <7)
- {
- vpTRACE(" Rank is : %d, should be 8", rank);
- throw(vpMatrixException(vpMatrixException::rankDeficient,
- "\n\t\t Matrix rank is deficient")) ;
- }
- //h = is the column of V associated with the smallest singular value of A
-
- // since we are not sure that the svd implemented sort the
- // singular value... we seek for the smallest
- double smallestSv = 1e30 ;
- unsigned int indexSmallestSv = 0 ;
- for (unsigned int i=0 ; i < 9 ; i++)
- if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
-
-
- h=V.getCol(indexSmallestSv);
-
- // build the homography
- for(unsigned int i =0;i<3;i++)
- {
- for(unsigned int j=0;j<3;j++)
- aHb[i][j]=h[3*i+j];
- }
-
- }
- catch(vpMatrixException me)
- {
- vpTRACE("Matrix Exception ") ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
-}
-#endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- 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]
- <b>h</b> is the column of <b>V</b> associated with the smalest singular value of <b>A
- </b>
-
- \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<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya ,
- vpHomography &aHb,
- bool normalization)
-{
- unsigned int n = (unsigned int) xb.size();
- if (yb.size() != n || xa.size() != n || ya.size() != n)
- throw(vpException(vpException::dimensionError,
- "Bad dimension for DLT homography estimation"));
-
- // 4 point are required
- if(n<4)
- throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
-
- try{
- std::vector<double> xan, yan, xbn, ybn;
-
- double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
-
- vpHomography aHbn;
-
- if (normalization) {
- vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
- vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
- }
- else {
- xbn = xb;
- ybn = yb;
- xan = xa;
- yan = ya;
- }
-
- vpMatrix A(2*n,9);
- vpColVector h(9);
- vpColVector D(9);
- vpMatrix V(9,9);
-
- // We need here to compute the SVD on a (n*2)*9 matrix (where n is
- // the number of points). if n == 4, the matrix has more columns
- // than rows. This kind of matrix is not supported by GSL for
- // SVD. The solution is to add an extra line with zeros
- if (n == 4)
- A.resize(2*n+1,9);
-
- // build matrix A
- for(unsigned int i=0; i<n;i++)
- {
- A[2*i][0]=0;
- A[2*i][1]=0;
- A[2*i][2]=0;
- A[2*i][3]=-xbn[i] ;
- A[2*i][4]=-ybn[i] ;
- A[2*i][5]=-1;
- A[2*i][6]=xbn[i]*yan[i] ;
- A[2*i][7]=ybn[i]*yan[i];
- A[2*i][8]=yan[i];
-
-
- A[2*i+1][0]=xbn[i] ;
- A[2*i+1][1]=ybn[i] ;
- A[2*i+1][2]=1;
- A[2*i+1][3]=0;
- A[2*i+1][4]=0;
- A[2*i+1][5]=0;
- A[2*i+1][6]=-xbn[i]*xan[i];
- A[2*i+1][7]=-ybn[i]*xan[i];
- A[2*i+1][8]=-xan[i] ;
- }
-
- // Add an extra line with zero.
- if (n == 4) {
- for (unsigned int i=0; i < 9; i ++) {
- A[2*n][i] = 0;
- }
- }
-
- // solve Ah = 0
- // SVD Decomposition A = UDV^T (destructive wrt A)
- A.svd(D,V);
-
- // on en profite pour effectuer un controle sur le rang de la matrice :
- // pas plus de 2 valeurs singulieres quasi=0
- int rank=0;
- for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
- if(rank <7)
- {
- vpTRACE(" Rank is : %d, should be 8", rank);
- throw(vpMatrixException(vpMatrixException::rankDeficient,
- "\n\t\t Matrix rank is deficient")) ;
- }
- //h = is the column of V associated with the smallest singular value of A
-
- // since we are not sure that the svd implemented sort the
- // singular value... we seek for the smallest
- double smallestSv = 1e30 ;
- unsigned int indexSmallestSv = 0 ;
- for (unsigned int i=0 ; i < 9 ; i++)
- if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
-
- h=V.getCol(indexSmallestSv);
-
- // build the homography
- for(unsigned int i =0;i<3;i++)
- {
- for(unsigned int j=0;j<3;j++)
- aHbn[i][j]=h[3*i+j];
- }
-
- if (normalization) {
- // H after denormalization
- vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
- }
- else {
- aHb = aHbn;
- }
-
- }
- catch(vpMatrixException me)
- {
- vpTRACE("Matrix Exception ") ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-}
diff --git a/src/computer-vision/homography-estimation/vpHomographyExtract.cpp b/src/computer-vision/homography-estimation/vpHomographyExtract.cpp
deleted file mode 100644
index 8937d63..0000000
--- a/src/computer-vision/homography-estimation/vpHomographyExtract.cpp
+++ /dev/null
@@ -1,1995 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomographyExtract.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography transformation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpHomography.h>
-#include <visp/vpMath.h>
-#include <math.h>
-
-//#define DEBUG_Homographie 0
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------ */
-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)
-{
-
-
- vpColVector nd(3) ;
- nd[0]=0;nd[1]=0;nd[2]=1;
-
- 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 homogenous 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)
-{
- computeDisplacement(*this,nd, aRb,atb,n);
-}
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
-
- Compute the camera displacement between two images from the homography \f$
- {^a}{\bf H}_b \f$.
-
- Camera displacement between \f$ {^a}{\bf p} \f$ and \f$ {^b}{\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 homogenous matrix can be build
- (vpHomogeneousMatrix).
-
- \param aHb : Input homography \f$ {^a}{\bf H}_b \f$.
-
- \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 vpHomography &aHb,
- const vpColVector &nd,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n)
-{
- /**** Declarations des variables ****/
-
- vpMatrix aRbint(3,3) ;
- vpColVector svTemp(3), sv(3);
- vpMatrix mX(3,2) ;
- vpColVector aTbp(3), normaleEstimee(3);
- double distanceFictive;
- double sinusTheta, cosinusTheta, signeSinus= 1;
- double cosinusDesireeEstimee, cosinusAncien;
- double s, determinantU, determinantV;
- unsigned int i, j, k, w;
- unsigned int vOrdre[3];
-
- //vpColVector normaleDesiree(3) ;
- //normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
- vpColVector normaleDesiree(nd);
-
- /**** Corps de la focntion ****/
-#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
-#endif
-
- /* Allocation des matrices */
- vpMatrix mTempU(3,3) ;
- vpMatrix mTempV(3,3) ;
- vpMatrix mU(3,3) ;
- vpMatrix mV(3,3) ;
- vpMatrix aRbp(3,3) ;
-
-
- vpMatrix mH(3,3) ;
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
-
- /* Preparation au calcul de la SVD */
- mTempU = mH ;
-
- /*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
- *****/
- mTempU.svd(svTemp,mTempV) ;
-
- /* On va mettre les valeurs singulieres en ordre decroissant : */
-
- /* Determination de l'ordre des valeurs */
- if (svTemp[0] >= svTemp[1]) {
- if (svTemp[0] >= svTemp[2]) {
- if (svTemp[1] > svTemp[2]) {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
- } else {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
- }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
- }
- } else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2])
- { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
- else
- { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
- }
- }
- /*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
- *****/
-
-
- for (i = 0; i < 3; i++) {
- sv[i] = svTemp[vOrdre[i]];
- for (j = 0; j < 3; j++) {
- mU[i][j] = mTempU[i][vOrdre[j]];
- mV[i][j] = mTempV[i][vOrdre[j]];
- }
- }
-
-#ifdef DEBUG_Homographie
- printf("U : \n") ; std::cout << mU << std::endl ;
- printf("V : \n") ; std::cout << mV << std::endl ;
- printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
-#endif
-
- /* A verifier si necessaire!!! */
- determinantV = mV.det();
- determinantU = mU.det();
-
- s = determinantU * determinantV;
-
-#ifdef DEBUG_Homographie
- printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
-#endif
- if (s < 0) mV *=-1 ;
-
-
- /* d' = d2 */
- distanceFictive = sv[1];
-#ifdef DEBUG_Homographie
- printf ("d = %f\n",distanceFictive);
-#endif
- n.resize(3) ;
-
- if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
- {
- //#ifdef DEBUG_Homographie
- // printf ("\nPure rotation\n");
- //#endif
- /*****
- Cas ou le deplacement est une rotation pure
- et la normale reste indefini
- sv[0] = sv[1]= sv[2]
- *****/
- aTbp[0] = 0;
- aTbp[1] = 0;
- aTbp[2] = 0;
-
- n[0] = normaleDesiree[0];
- n[1] = normaleDesiree[1];
- n[2] = normaleDesiree[2];
- }
- else
- {
-#ifdef DEBUG_Homographie
- printf("\nCas general\n");
-#endif
- /* Cas general */
-
- /*****
- test pour determiner quelle est la bonne solution on teste
- d'abord quelle solution est plus proche de la perpendiculaire
- au plan de la cible constuction de la normale n
- *****/
-
- /*****
- Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
- dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
- *****/
- mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
- / (sv[0] * sv[0] - sv[2] * sv[2]));
- mX[1][0] = 0.0;
- mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
- / (sv[0] * sv[0] - sv[2] * sv[2]));
-
- mX[0][1] = -mX[0][0];
- mX[1][1] = mX[1][0];
- mX[2][1] = mX[2][0];
-
- /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
- cosinusAncien = 0.0;
- for (w = 0; w < 2; w++) { /* Pour les 2 cas */
- for (k = 0; k < 2; k++) { /* Pour le signe */
-
- /* Calcul de la normale estimee : n = V.n' */
- for (i = 0; i < 3; i++) {
- normaleEstimee[i] = 0.0;
- for (j = 0; j < 3; j++) {
- normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
- }
- }
-
-
- /* Calcul du cosinus de l'angle entre la normale reelle et desire */
- cosinusDesireeEstimee = 0.0;
- for (i = 0; i < 3; i++)
- cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
-
- /*****
- Si la solution est meilleur
- Remarque : On ne teste pas le cas oppose (cos<0)
- *****/
- if (cosinusDesireeEstimee > cosinusAncien)
- {
- cosinusAncien = cosinusDesireeEstimee;
-
- /* Affectation de la normale qui est retourner */
- for (j = 0; j < 3; j++)
- n[j] = normaleEstimee[j];
-
- /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
- aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
- aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
- aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
-
- /* si c'est la deuxieme solution */
- if (w == 1)
- signeSinus = -1; /* car esp1*esp3 = -1 */
- else
- signeSinus = 1;
- } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
- } /* fin k */
- } /* fin w */
- } /* fin else */
-
-
- /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
- for (i = 0; i < 3; i++) {
- atb[i] = 0.0;
- for (j = 0; j < 3; j++) {
- atb[i] += mU[i][j] * aTbp[j];
- }
- atb[i] /= distanceFictive;
- }
-
-
-#ifdef DEBUG_Homographie
- printf("t' : ") ; std::cout << aTbp.t() ;
- printf("t/d : ") ; std::cout << atb.t() ;
- printf("n : ") ; std::cout << n.t() ;
-#endif
-
-
- /* Calcul de la matrice de rotation R */
-
- /*****
- Calcul du sinus(theta) et du cosinus(theta)
- Remarque : sinus(theta) pourra changer de signe en fonction
- de la solution retenue (cf. ci-dessous)
- *****/
- sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
- *(sv[1]*sv[1] -sv[2]*sv[2]))
- / ((sv[0] + sv[2]) * sv[1]);
-
- cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
-
- /* construction de la matrice de rotation R' */
- aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
- aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
- aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
-
-
-
- /* multiplication Rint = U R' */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- aRbint[i][j] = 0.0;
- for (k = 0; k < 3; k++) {
- aRbint[i][j] += mU[i][k] * aRbp[k][j];
- }
- }
- }
-
- /* multiplication R = Rint . V^T */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- aRb[i][j] = 0.0;
- for (k = 0; k < 3; k++) {
- aRb[i][j] += aRbint[i][k] * mV[j][k];
- }
- }
- }
- /*transpose_carre(aRb,3); */
-#ifdef DEBUG_Homographie
- printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
-#endif
-
-}
-
-/*!
-
- Compute the camera displacement between two images from the homography \f$
- {^a}{\bf H}_b \f$.
-
- 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 homogenous matrix can be build
- (vpHomogeneousMatrix).
-
- \param aHb : Input homography \f$ {^a}{\bf H}_b \f$.
-
- \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 vpHomography &aHb,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n)
-{
- /**** Declarations des variables ****/
-
- vpMatrix aRbint(3,3) ;
- vpColVector svTemp(3), sv(3);
- vpMatrix mX(3,2) ;
- vpColVector aTbp(3), normaleEstimee(3);
- double distanceFictive;
- double sinusTheta, cosinusTheta, signeSinus= 1;
- double cosinusDesireeEstimee, cosinusAncien;
- double s, determinantU, determinantV;
- unsigned int i, j, k, w;
- unsigned int vOrdre[3];
-
- vpColVector normaleDesiree(3) ;
- normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
-
- /**** Corps de la focntion ****/
-#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
-#endif
-
- /* Allocation des matrices */
- vpMatrix mTempU(3,3) ;
- vpMatrix mTempV(3,3) ;
- vpMatrix mU(3,3) ;
- vpMatrix mV(3,3) ;
- vpMatrix aRbp(3,3) ;
-
-
- vpMatrix mH(3,3) ;
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
-
- /* Preparation au calcul de la SVD */
- mTempU = mH ;
-
- /*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
- *****/
- mTempU.svd(svTemp,mTempV) ;
-
- /* On va mettre les valeurs singulieres en ordre decroissant : */
-
- /* Determination de l'ordre des valeurs */
- if (svTemp[0] >= svTemp[1]) {
- if (svTemp[0] >= svTemp[2]) {
- if (svTemp[1] > svTemp[2]) {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
- } else {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
- }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
- }
- } else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2])
- { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
- else
- { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
- }
- }
- /*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
- *****/
-
-
- for (i = 0; i < 3; i++) {
- sv[i] = svTemp[vOrdre[i]];
- for (j = 0; j < 3; j++) {
- mU[i][j] = mTempU[i][vOrdre[j]];
- mV[i][j] = mTempV[i][vOrdre[j]];
- }
- }
-
-#ifdef DEBUG_Homographie
- printf("U : \n") ; std::cout << mU << std::endl ;
- printf("V : \n") ; std::cout << mV << std::endl ;
- printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
-#endif
-
- /* A verifier si necessaire!!! */
- determinantV = mV.det();
- determinantU = mU.det();
-
- s = determinantU * determinantV;
-
-#ifdef DEBUG_Homographie
- printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
-#endif
- if (s < 0) mV *=-1 ;
-
-
- /* d' = d2 */
- distanceFictive = sv[1];
-#ifdef DEBUG_Homographie
- printf ("d = %f\n",distanceFictive);
-#endif
- n.resize(3) ;
-
- if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold)
- {
- //#ifdef DEBUG_Homographie
- // printf ("\nPure rotation\n");
- //#endif
- /*****
- Cas ou le deplacement est une rotation pure
- et la normale reste indefini
- sv[0] = sv[1]= sv[2]
- *****/
- aTbp[0] = 0;
- aTbp[1] = 0;
- aTbp[2] = 0;
-
- n[0] = normaleDesiree[0];
- n[1] = normaleDesiree[1];
- n[2] = normaleDesiree[2];
- }
- else
- {
-#ifdef DEBUG_Homographie
- printf("\nCas general\n");
-#endif
- /* Cas general */
-
- /*****
- test pour determiner quelle est la bonne solution on teste
- d'abord quelle solution est plus proche de la perpendiculaire
- au plan de la cible constuction de la normale n
- *****/
-
- /*****
- Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
- dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
- *****/
- mX[0][0] = sqrt ((sv[0] * sv[0] - sv[1] * sv[1])
- / (sv[0] * sv[0] - sv[2] * sv[2]));
- mX[1][0] = 0.0;
- mX[2][0] = sqrt ((sv[1] * sv[1] - sv[2] * sv[2])
- / (sv[0] * sv[0] - sv[2] * sv[2]));
-
- mX[0][1] = -mX[0][0];
- mX[1][1] = mX[1][0];
- mX[2][1] = mX[2][0];
-
- /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
- cosinusAncien = 0.0;
- for (w = 0; w < 2; w++) { /* Pour les 2 cas */
- for (k = 0; k < 2; k++) { /* Pour le signe */
-
- /* Calcul de la normale estimee : n = V.n' */
- for (i = 0; i < 3; i++) {
- normaleEstimee[i] = 0.0;
- for (j = 0; j < 3; j++) {
- normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
- }
- }
-
-
- /* Calcul du cosinus de l'angle entre la normale reelle et desire */
- cosinusDesireeEstimee = 0.0;
- for (i = 0; i < 3; i++)
- cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
-
- /*****
- Si la solution est meilleur
- Remarque : On ne teste pas le cas oppose (cos<0)
- *****/
- if (cosinusDesireeEstimee > cosinusAncien)
- {
- cosinusAncien = cosinusDesireeEstimee;
-
- /* Affectation de la normale qui est retourner */
- for (j = 0; j < 3; j++)
- n[j] = normaleEstimee[j];
-
- /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */
- aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
- aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
- aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
-
- /* si c'est la deuxieme solution */
- if (w == 1)
- signeSinus = -1; /* car esp1*esp3 = -1 */
- else
- signeSinus = 1;
- } /* fin if (cosinusDesireeEstimee > cosinusAncien) */
- } /* fin k */
- } /* fin w */
- } /* fin else */
-
-
- /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
- for (i = 0; i < 3; i++) {
- atb[i] = 0.0;
- for (j = 0; j < 3; j++) {
- atb[i] += mU[i][j] * aTbp[j];
- }
- atb[i] /= distanceFictive;
- }
-
-
-#ifdef DEBUG_Homographie
- printf("t' : ") ; std::cout << aTbp.t() ;
- printf("t/d : ") ; std::cout << atb.t() ;
- printf("n : ") ; std::cout << n.t() ;
-#endif
-
-
- /* Calcul de la matrice de rotation R */
-
- /*****
- Calcul du sinus(theta) et du cosinus(theta)
- Remarque : sinus(theta) pourra changer de signe en fonction
- de la solution retenue (cf. ci-dessous)
- *****/
- sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
- *(sv[1]*sv[1] -sv[2]*sv[2]))
- / ((sv[0] + sv[2]) * sv[1]);
-
- cosinusTheta = ( sv[1] * sv[1] + sv[0] * sv[2] ) / ((sv[0] + sv[2]) * sv[1]);
-
- /* construction de la matrice de rotation R' */
- aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
- aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
- aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
-
-
-
- /* multiplication Rint = U R' */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- aRbint[i][j] = 0.0;
- for (k = 0; k < 3; k++) {
- aRbint[i][j] += mU[i][k] * aRbp[k][j];
- }
- }
- }
-
- /* multiplication R = Rint . V^T */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- aRb[i][j] = 0.0;
- for (k = 0; k < 3; k++) {
- aRb[i][j] += aRbint[i][k] * mV[j][k];
- }
- }
- }
- /*transpose_carre(aRb,3); */
-#ifdef DEBUG_Homographie
- printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
-#endif
-
-}
-
-void vpHomography::computeDisplacement(const vpMatrix &H,
- const double x,
- const double y,
- std::list<vpRotationMatrix> & vR,
- std::list<vpTranslationVector> & vT,
- std::list<vpColVector> & vN)
-{
-
-#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
-#endif
-
- vR.clear();
- vT.clear();
- vN.clear();
-
- /**** Declarations des variables ****/
- int cas1 =1, cas2=2, cas3=3, cas4=4;
- int cas =0;
- bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
-
- double tmp,determinantU,determinantV,s,distanceFictive;
- vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
-
- vpRotationMatrix Rprim1,R1;
- vpRotationMatrix Rprim2,R2;
- vpRotationMatrix Rprim3,R3;
- vpRotationMatrix Rprim4,R4;
- vpTranslationVector Tprim1, T1;
- vpTranslationVector Tprim2, T2;
- vpTranslationVector Tprim3, T3;
- vpTranslationVector Tprim4, T4;
- vpColVector Nprim1(3), N1(3);
- vpColVector Nprim2(3), N2(3);
- vpColVector Nprim3(3), N3(3);
- vpColVector Nprim4(3), N4(3);
-
- vpColVector svTemp(3),sv(3);
- unsigned int vOrdre[3];
- vpColVector vTp(3);
-
-
- /* Preparation au calcul de la SVD */
- mTempU = H;
-
- /*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
- *****/
-
- // cette fonction ne renvoit pas d'erreur
- mTempU.svd(svTemp, mTempV);
-
- /* On va mettre les valeurs singulieres en ordre decroissant : */
- /* Determination de l'ordre des valeurs */
- if (svTemp[0] >= svTemp[1]) {
- if (svTemp[0] >= svTemp[2])
- {
- if (svTemp[1] > svTemp[2])
- {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
- }
- else
- {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
- }
- }
- else
- {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
- }
- } else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2]) {
- vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
- } else {
- vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
- }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
- }
- }
- /*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
- *****/
- for (unsigned int i = 0; i < 3; i++) {
- sv[i] = svTemp[vOrdre[i]];
- for (unsigned int j = 0; j < 3; j++) {
- U[i][j] = mTempU[i][vOrdre[j]];
- V[i][j] = mTempV[i][vOrdre[j]];
- }
- }
-
-#ifdef DEBUG_Homographie
- printf("U : \n") ; Affiche(U) ;
- printf("V : \n") ; affiche(V) ;
- printf("Valeurs singulieres : ") ; affiche(sv);
-#endif
-
- // calcul du determinant de U et V
- determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
- U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
- U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
-
- determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
- V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
- V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
-
- s = determinantU * determinantV;
-
-#ifdef DEBUG_Homographie
- printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
-#endif
-
- // deux cas sont a traiter :
- // distance Fictive = sv[1]
- // distance fictive = -sv[1]
-
- // pour savoir quelle est le bon signe,
- // on utilise le point qui appartient au plan
- // la contrainte est :
- // (h_31x_1 + h_32x_2 + h_33)/d > 0
- // et d = sd'
-
- tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
-
- if ((tmp/(sv[1] *s)) > 0)
- distanceFictive = sv[1];
- else
- distanceFictive = -sv[1];
-
- // il faut ensuite considerer l'ordre de multiplicite de chaque variable
- // 1er cas : d1<>d2<>d3
- // 2eme cas : d1=d2 <> d3
- // 3eme cas : d1 <>d2 =d3
- // 4eme cas : d1 =d2=d3
-
- if ((sv[0] - sv[1]) < sing_threshold)
- {
- if ((sv[1] - sv[2]) < sing_threshold)
- cas = cas4;
- else
- cas = cas2;
- }
- else
- {
- if ((sv[1] - sv[2]) < sing_threshold)
- cas = cas3;
- else
- cas = cas1;
- }
-
- Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
-
- // on filtre ensuite les diff'erentes normales possibles
- // condition : nm/d > 0
- // avec d = sd'
- // et n = Vn'
-
- // les quatres cas sont : ++, +-, -+ , --
- // dans tous les cas, Normale[1] = 0;
- Nprim1[1] = 0.0;
- Nprim2[1] = 0.0;
- Nprim3[1] = 0.0;
- Nprim4[1] = 0.0;
-
- // on calcule les quatres cas de normale
-
- if (cas == cas1)
- {
- // quatre normales sont possibles
- Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1] )/
- (sv[0] * sv[0] - sv[2] * sv[2] ));
-
- Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2] )/
- (sv[0] * sv[0] - sv[2] * sv[2] ));
-
- Nprim2[0] = Nprim1[0]; Nprim2[2] = - Nprim1[2];
- Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
- Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
- }
- if (cas == cas2)
- {
- // 2 normales sont possibles
- // x3 = +-1
- Nprim1[2] = 1;
- Nprim2[2] = -1;
- }
-
- if (cas == cas3)
- {
- // 2 normales sont possibles
- // x1 = +-1
- Nprim1[0] = 1;
- Nprim2[0] = -1;
- }
- // si on est dans le cas 4,
- // on considere que x reste indefini
-
- // on peut maintenant filtrer les solutions avec la contrainte
- // n^tm / d > 0
- // attention, il faut travailler avec la bonne normale,
- // soit Ni et non pas Nprimi
- if (cas == cas1)
- {
- N1 = V *Nprim1;
-
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive *s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] *y+ N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
-
- N3 = V *Nprim3;
- tmp = N3[0] * x + N3[1]*y+ N3[2];
- tmp/= (distanceFictive*s);
- norm3ok = (tmp>0);
-
- N4 = V *Nprim4;
- tmp = N4[0] * x + N4[1] * y + N4[2];
- tmp/= (distanceFictive*s);
- norm4ok = (tmp>0);
- }
-
- if (cas == cas2)
- {
- N1 = V *Nprim1;
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
- }
-
- if (cas == cas3)
- {
- N1 = V *Nprim1;
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
- }
-
- // on a donc maintenant les differents cas possibles
- // on peut ensuite remonter aux deplacements
- // on separe encore les cas 1,2,3
- // la, deux choix se posent suivant le signe de d
- if (distanceFictive>0)
- {
- if (cas == cas1)
- {
- if (norm1ok)
- {
-
- //cos theta
- Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim1[2][2] = Rprim1[0][0];
-
- // sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim1[0][2] = -Rprim1[2][0];
-
- Rprim1[1][1] =1.0;
-
- Tprim1[0] = Nprim1[0];
- Tprim1[1] = 0.0;
- Tprim1[2] = -Nprim1[2];
-
- Tprim1*=(sv[0] - sv[2]);
-
- }
-
- if (norm2ok)
- {
-
- //cos theta
- Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim2[2][2] = Rprim2[0][0];
-
- // sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim2[0][2] = -Rprim2[2][0];
-
- Rprim2[1][1] =1.0;
-
- Tprim2[0] = Nprim2[0];
- Tprim2[1] = 0.0;
- Tprim2[2] = -Nprim2[2];
-
- Tprim2*=(sv[0] - sv[2]);
-
- }
-
- if (norm3ok)
- {
-
- //cos theta
- Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim3[2][2] = Rprim3[0][0];
-
- // sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim3[0][2] = -Rprim3[2][0];
-
- Rprim3[1][1] =1.0;
-
- Tprim3[0] = Nprim3[0];
- Tprim3[1] = 0.0;
- Tprim3[2] = -Nprim3[2];
-
- Tprim3*=(sv[0] - sv[2]);
-
- }
-
- if (norm4ok)
- {
-
- //cos theta
- Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim4[2][2] = Rprim4[0][0];
-
- // sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim4[0][2] = -Rprim4[2][0];
-
- Rprim4[1][1] =1.0;
-
- Tprim4[0] = Nprim4[0];
- Tprim4[1] = 0.0;
- Tprim4[2] = -Nprim4[2];
-
- Tprim4*=(sv[0] - sv[2]);
-
- }
- }
-
- if (cas == cas2)
- {
- // 2 normales sont potentiellement candidates
-
- if (norm1ok)
- {
- Rprim1.setIdentity();
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] - sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] - sv[0]);
- }
- }
- if (cas == cas3)
- {
- if (norm1ok)
- {
- Rprim1.setIdentity();
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[0] - sv[1]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] - sv[1]);
- }
- }
- if (cas == cas4)
- {
- // on ne connait pas la normale dans ce cas la
- Rprim1.setIdentity();
- Tprim1 = 0.0;
- }
- }
-
- if (distanceFictive <0)
- {
-
- if (cas == cas1)
- {
- if (norm1ok)
- {
-
-
- //cos theta
- Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim1[2][2] = -Rprim1[0][0];
-
- // sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim1[0][2] = Rprim1[2][0];
-
- Rprim1[1][1] = -1.0;
-
- Tprim1[0] = Nprim1[0];
- Tprim1[1] = 0.0;
- Tprim1[2] = Nprim1[2];
-
- Tprim1*=(sv[0] + sv[2]);
-
- }
-
- if (norm2ok)
- {
-
- //cos theta
- Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim2[2][2] = -Rprim2[0][0];
-
- // sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim2[0][2] = Rprim2[2][0];
-
- Rprim2[1][1] = - 1.0;
-
- Tprim2[0] = Nprim2[0];
- Tprim2[1] = 0.0;
- Tprim2[2] = Nprim2[2];
-
- Tprim2*=(sv[0] + sv[2]);
-
- }
-
- if (norm3ok)
- {
-
- //cos theta
- Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim3[2][2] = -Rprim3[0][0];
-
- // sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim3[0][2] = Rprim3[2][0];
-
- Rprim3[1][1] = -1.0;
-
- Tprim3[0] = Nprim3[0];
- Tprim3[1] = 0.0;
- Tprim3[2] = Nprim3[2];
-
- Tprim3*=(sv[0] + sv[2]);
-
- }
-
- if (norm4ok)
- {
- //cos theta
- Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
-
- Rprim4[2][2] = -Rprim4[0][0];
-
- // sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim4[0][2] = Rprim4[2][0];
-
- Rprim4[1][1] = - 1.0;
-
- Tprim4[0] = Nprim4[0];
- Tprim4[1] = 0.0;
- Tprim4[2] = Nprim4[2];
-
- Tprim4*=(sv[0] + sv[2]);
- }
- }
- if (cas == cas2)
- {
- // 2 normales sont potentiellement candidates
-
- if (norm1ok)
- {
- Rprim1.setIdentity();
- Rprim1[0][0] = -1;
- Rprim1[1][1] = -1;
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
- Rprim2[0][0] = -1;
- Rprim2[1][1] = -1;
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] + sv[0]);
- }
- }
- if (cas == cas3)
- {
- if (norm1ok)
- {
- Rprim1.setIdentity();
- Rprim1[2][2] = -1;
- Rprim1[1][1] = -1;
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
- Rprim2[2][2] = -1;
- Rprim2[1][1] = -1;
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] + sv[2]);
- }
- }
-
- // ON NE CONSIDERE PAS LE CAS NUMERO 4
- }
- // tous les Rprim et Tprim sont calcules
- // on peut maintenant recuperer la
- // rotation, et la translation
- // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
- if ((distanceFictive>0) || (cas != cas4))
- {
- // on controle juste si les normales sont ok
-
- if (norm1ok)
- {
- R1 = s * U * Rprim1 * V.t();
- T1 = U * Tprim1;
- T1 /= (distanceFictive *s);
- N1 = V *Nprim1;
-
- // je rajoute le resultat
- vR.push_back(R1);
- vT.push_back(T1);
- vN.push_back(N1);
- }
- if (norm2ok)
- {
- R2 = s * U * Rprim2 * V.t();
- T2 = U * Tprim2;
- T2 /= (distanceFictive *s);
- N2 = V *Nprim2;
-
- // je rajoute le resultat
- vR.push_back(R2);
- vT.push_back(T2);
- vN.push_back(N2);
- }
- if (norm3ok)
- {
- R3 = s * U * Rprim3 * V.t();
- T3 = U * Tprim3;
- T3 /= (distanceFictive *s);
- N3 = V *Nprim3;
- // je rajoute le resultat
- vR.push_back(R3);
- vT.push_back(T3);
- vN.push_back(N3);
- }
- if (norm4ok)
- {
- R4 = s * U * Rprim4 * V.t();
- T4 = U * Tprim4;
- T4 /= (distanceFictive *s);
- N4 = V *Nprim4;
-
- // je rajoute le resultat
- vR.push_back(R4);
- vT.push_back(T4);
- vN.push_back(N4);
- }
- }
- else
- {
- std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
- }
-
- // on peut ensuite afficher les resultats...
- /* std::cout << "Analyse des resultats : "<< std::endl; */
- /* if (cas==cas1) */
- /* std::cout << "On est dans le cas 1" << std::endl; */
- /* if (cas==cas2) */
- /* std::cout << "On est dans le cas 2" << std::endl; */
- /* if (cas==cas3) */
- /* std::cout << "On est dans le cas 3" << std::endl; */
- /* if (cas==cas4) */
- /* std::cout << "On est dans le cas 4" << std::endl; */
-
- /* if (distanceFictive < 0) */
- /* std::cout << "d'<0" << std::endl; */
- /* else */
- /* std::cout << "d'>0" << std::endl; */
-
-#ifdef DEBUG_Homographie
- printf("fin : Homographie_EstimationDeplacementCamera\n");
-#endif
-}
-#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- computeDisplacement(const vpMatrix &, const double, const double, std::list<vpRotationMatrix> &, std::list<vpTranslationVector> &, std::list<vpColVector> &) instead.
-*/
-void vpHomography::computeDisplacement(const vpMatrix H,
- const double x,
- const double y,
- vpList<vpRotationMatrix> & vR,
- vpList<vpTranslationVector> & vT,
- vpList<vpColVector> & vN)
-{
-
-#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
-#endif
-
- vR.kill();
- vT.kill();
- vN.kill();
-
- /**** Declarations des variables ****/
- int cas1 =1, cas2=2, cas3=3, cas4=4;
- int cas =0;
- bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
-
- double tmp,determinantU,determinantV,s,distanceFictive;
- vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
-
- vpRotationMatrix Rprim1,R1;
- vpRotationMatrix Rprim2,R2;
- vpRotationMatrix Rprim3,R3;
- vpRotationMatrix Rprim4,R4;
- vpTranslationVector Tprim1, T1;
- vpTranslationVector Tprim2, T2;
- vpTranslationVector Tprim3, T3;
- vpTranslationVector Tprim4, T4;
- vpColVector Nprim1(3), N1(3);
- vpColVector Nprim2(3), N2(3);
- vpColVector Nprim3(3), N3(3);
- vpColVector Nprim4(3), N4(3);
-
- vpColVector svTemp(3),sv(3);
- unsigned int vOrdre[3];
- vpColVector vTp(3);
-
-
- /* Preparation au calcul de la SVD */
- mTempU = H;
-
- /*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
- *****/
-
- // cette fonction ne renvoit pas d'erreur
- mTempU.svd(svTemp, mTempV);
-
- /* On va mettre les valeurs singulieres en ordre decroissant : */
- /* Determination de l'ordre des valeurs */
- if (svTemp[0] >= svTemp[1]) {
- if (svTemp[0] >= svTemp[2])
- {
- if (svTemp[1] > svTemp[2])
- {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
- }
- else
- {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
- }
- }
- else
- {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
- }
- } else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2]) {
- vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
- } else {
- vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
- }
- } else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
- }
- }
- /*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
- *****/
- for (unsigned int i = 0; i < 3; i++) {
- sv[i] = svTemp[vOrdre[i]];
- for (unsigned int j = 0; j < 3; j++) {
- U[i][j] = mTempU[i][vOrdre[j]];
- V[i][j] = mTempV[i][vOrdre[j]];
- }
- }
-
-#ifdef DEBUG_Homographie
- printf("U : \n") ; Affiche(U) ;
- printf("V : \n") ; affiche(V) ;
- printf("Valeurs singulieres : ") ; affiche(sv);
-#endif
-
- // calcul du determinant de U et V
- determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
- U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
- U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
-
- determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
- V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
- V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
-
- s = determinantU * determinantV;
-
-#ifdef DEBUG_Homographie
- printf ("s = det(U) * det(V) = %f * %f = %f\n",determinantU,determinantV,s);
-#endif
-
- // deux cas sont a traiter :
- // distance Fictive = sv[1]
- // distance fictive = -sv[1]
-
- // pour savoir quelle est le bon signe,
- // on utilise le point qui appartient au plan
- // la contrainte est :
- // (h_31x_1 + h_32x_2 + h_33)/d > 0
- // et d = sd'
-
- tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
-
- if ((tmp/(sv[1] *s)) > 0)
- distanceFictive = sv[1];
- else
- distanceFictive = -sv[1];
-
- // il faut ensuite considerer l'ordre de multiplicite de chaque variable
- // 1er cas : d1<>d2<>d3
- // 2eme cas : d1=d2 <> d3
- // 3eme cas : d1 <>d2 =d3
- // 4eme cas : d1 =d2=d3
-
- if ((sv[0] - sv[1]) < sing_threshold)
- {
- if ((sv[1] - sv[2]) < sing_threshold)
- cas = cas4;
- else
- cas = cas2;
- }
- else
- {
- if ((sv[1] - sv[2]) < sing_threshold)
- cas = cas3;
- else
- cas = cas1;
- }
-
- Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
-
- // on filtre ensuite les diff'erentes normales possibles
- // condition : nm/d > 0
- // avec d = sd'
- // et n = Vn'
-
- // les quatres cas sont : ++, +-, -+ , --
- // dans tous les cas, Normale[1] = 0;
- Nprim1[1] = 0.0;
- Nprim2[1] = 0.0;
- Nprim3[1] = 0.0;
- Nprim4[1] = 0.0;
-
- // on calcule les quatres cas de normale
-
- if (cas == cas1)
- {
- // quatre normales sont possibles
- Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1] )/
- (sv[0] * sv[0] - sv[2] * sv[2] ));
-
- Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2] )/
- (sv[0] * sv[0] - sv[2] * sv[2] ));
-
- Nprim2[0] = Nprim1[0]; Nprim2[2] = - Nprim1[2];
- Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
- Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
- }
- if (cas == cas2)
- {
- // 2 normales sont possibles
- // x3 = +-1
- Nprim1[2] = 1;
- Nprim2[2] = -1;
- }
-
- if (cas == cas3)
- {
- // 2 normales sont possibles
- // x1 = +-1
- Nprim1[0] = 1;
- Nprim2[0] = -1;
- }
- // si on est dans le cas 4,
- // on considere que x reste indefini
-
- // on peut maintenant filtrer les solutions avec la contrainte
- // n^tm / d > 0
- // attention, il faut travailler avec la bonne normale,
- // soit Ni et non pas Nprimi
- if (cas == cas1)
- {
- N1 = V *Nprim1;
-
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive *s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] *y+ N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
-
- N3 = V *Nprim3;
- tmp = N3[0] * x + N3[1]*y+ N3[2];
- tmp/= (distanceFictive*s);
- norm3ok = (tmp>0);
-
- N4 = V *Nprim4;
- tmp = N4[0] * x + N4[1] * y + N4[2];
- tmp/= (distanceFictive*s);
- norm4ok = (tmp>0);
- }
-
- if (cas == cas2)
- {
- N1 = V *Nprim1;
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
- }
-
- if (cas == cas3)
- {
- N1 = V *Nprim1;
- tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
-
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
- }
-
- // on a donc maintenant les differents cas possibles
- // on peut ensuite remonter aux deplacements
- // on separe encore les cas 1,2,3
- // la, deux choix se posent suivant le signe de d
- if (distanceFictive>0)
- {
- if (cas == cas1)
- {
- if (norm1ok)
- {
-
- //cos theta
- Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim1[2][2] = Rprim1[0][0];
-
- // sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim1[0][2] = -Rprim1[2][0];
-
- Rprim1[1][1] =1.0;
-
- Tprim1[0] = Nprim1[0];
- Tprim1[1] = 0.0;
- Tprim1[2] = -Nprim1[2];
-
- Tprim1*=(sv[0] - sv[2]);
-
- }
-
- if (norm2ok)
- {
-
- //cos theta
- Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim2[2][2] = Rprim2[0][0];
-
- // sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim2[0][2] = -Rprim2[2][0];
-
- Rprim2[1][1] =1.0;
-
- Tprim2[0] = Nprim2[0];
- Tprim2[1] = 0.0;
- Tprim2[2] = -Nprim2[2];
-
- Tprim2*=(sv[0] - sv[2]);
-
- }
-
- if (norm3ok)
- {
-
- //cos theta
- Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim3[2][2] = Rprim3[0][0];
-
- // sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim3[0][2] = -Rprim3[2][0];
-
- Rprim3[1][1] =1.0;
-
- Tprim3[0] = Nprim3[0];
- Tprim3[1] = 0.0;
- Tprim3[2] = -Nprim3[2];
-
- Tprim3*=(sv[0] - sv[2]);
-
- }
-
- if (norm4ok)
- {
-
- //cos theta
- Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
- ((sv[0] + sv[2]) * sv[1]);
-
- Rprim4[2][2] = Rprim4[0][0];
-
- // sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
-
- Rprim4[0][2] = -Rprim4[2][0];
-
- Rprim4[1][1] =1.0;
-
- Tprim4[0] = Nprim4[0];
- Tprim4[1] = 0.0;
- Tprim4[2] = -Nprim4[2];
-
- Tprim4*=(sv[0] - sv[2]);
-
- }
- }
-
- if (cas == cas2)
- {
- // 2 normales sont potentiellement candidates
-
- if (norm1ok)
- {
- Rprim1.setIdentity();
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] - sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] - sv[0]);
- }
- }
- if (cas == cas3)
- {
- if (norm1ok)
- {
- Rprim1.setIdentity();
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[0] - sv[1]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] - sv[1]);
- }
- }
- if (cas == cas4)
- {
- // on ne connait pas la normale dans ce cas la
- Rprim1.setIdentity();
- Tprim1 = 0.0;
- }
- }
-
- if (distanceFictive <0)
- {
-
- if (cas == cas1)
- {
- if (norm1ok)
- {
-
-
- //cos theta
- Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim1[2][2] = -Rprim1[0][0];
-
- // sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim1[0][2] = Rprim1[2][0];
-
- Rprim1[1][1] = -1.0;
-
- Tprim1[0] = Nprim1[0];
- Tprim1[1] = 0.0;
- Tprim1[2] = Nprim1[2];
-
- Tprim1*=(sv[0] + sv[2]);
-
- }
-
- if (norm2ok)
- {
-
- //cos theta
- Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim2[2][2] = -Rprim2[0][0];
-
- // sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim2[0][2] = Rprim2[2][0];
-
- Rprim2[1][1] = - 1.0;
-
- Tprim2[0] = Nprim2[0];
- Tprim2[1] = 0.0;
- Tprim2[2] = Nprim2[2];
-
- Tprim2*=(sv[0] + sv[2]);
-
- }
-
- if (norm3ok)
- {
-
- //cos theta
- Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
-
- Rprim3[2][2] = -Rprim3[0][0];
-
- // sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim3[0][2] = Rprim3[2][0];
-
- Rprim3[1][1] = -1.0;
-
- Tprim3[0] = Nprim3[0];
- Tprim3[1] = 0.0;
- Tprim3[2] = Nprim3[2];
-
- Tprim3*=(sv[0] + sv[2]);
-
- }
-
- if (norm4ok)
- {
- //cos theta
- Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
-
- Rprim4[2][2] = -Rprim4[0][0];
-
- // sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
-
- Rprim4[0][2] = Rprim4[2][0];
-
- Rprim4[1][1] = - 1.0;
-
- Tprim4[0] = Nprim4[0];
- Tprim4[1] = 0.0;
- Tprim4[2] = Nprim4[2];
-
- Tprim4*=(sv[0] + sv[2]);
- }
- }
- if (cas == cas2)
- {
- // 2 normales sont potentiellement candidates
-
- if (norm1ok)
- {
- Rprim1.setIdentity();
- Rprim1[0][0] = -1;
- Rprim1[1][1] = -1;
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
- Rprim2[0][0] = -1;
- Rprim2[1][1] = -1;
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] + sv[0]);
- }
- }
- if (cas == cas3)
- {
- if (norm1ok)
- {
- Rprim1.setIdentity();
- Rprim1[2][2] = -1;
- Rprim1[1][1] = -1;
-
- Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
- }
-
- if (norm2ok)
- {
- Rprim2.setIdentity();
- Rprim2[2][2] = -1;
- Rprim2[1][1] = -1;
-
- Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] + sv[2]);
- }
- }
-
- // ON NE CONSIDERE PAS LE CAS NUMERO 4
- }
- // tous les Rprim et Tprim sont calcules
- // on peut maintenant recuperer la
- // rotation, et la translation
- // IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
- if ((distanceFictive>0) || (cas != cas4))
- {
- // on controle juste si les normales sont ok
-
- if (norm1ok)
- {
- R1 = s * U * Rprim1 * V.t();
- T1 = U * Tprim1;
- T1 /= (distanceFictive *s);
- N1 = V *Nprim1;
-
- // je rajoute le resultat
- vR.addRight(R1);
- vT.addRight(T1);
- vN.addRight(N1);
- }
- if (norm2ok)
- {
- R2 = s * U * Rprim2 * V.t();
- T2 = U * Tprim2;
- T2 /= (distanceFictive *s);
- N2 = V *Nprim2;
-
- // je rajoute le resultat
- vR.addRight(R2);
- vT.addRight(T2);
- vN.addRight(N2);
- }
- if (norm3ok)
- {
- R3 = s * U * Rprim3 * V.t();
- T3 = U * Tprim3;
- T3 /= (distanceFictive *s);
- N3 = V *Nprim3;
- // je rajoute le resultat
- vR.addRight(R3);
- vT.addRight(T3);
- vN.addRight(N3);
- }
- if (norm4ok)
- {
- R4 = s * U * Rprim4 * V.t();
- T4 = U * Tprim4;
- T4 /= (distanceFictive *s);
- N4 = V *Nprim4;
-
- // je rajoute le resultat
- vR.addRight(R4);
- vT.addRight(T4);
- vN.addRight(N4);
- }
- }
- else
- {
- std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
- }
-
- // on peut ensuite afficher les resultats...
- /* std::cout << "Analyse des resultats : "<< std::endl; */
- /* if (cas==cas1) */
- /* std::cout << "On est dans le cas 1" << std::endl; */
- /* if (cas==cas2) */
- /* std::cout << "On est dans le cas 2" << std::endl; */
- /* if (cas==cas3) */
- /* std::cout << "On est dans le cas 3" << std::endl; */
- /* if (cas==cas4) */
- /* std::cout << "On est dans le cas 4" << std::endl; */
-
- /* if (distanceFictive < 0) */
- /* std::cout << "d'<0" << std::endl; */
- /* else */
- /* std::cout << "d'>0" << std::endl; */
-
-#ifdef DEBUG_Homographie
- printf("fin : Homographie_EstimationDeplacementCamera\n");
-#endif
-}
-
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
diff --git a/src/computer-vision/homography-estimation/vpHomographyMalis.cpp b/src/computer-vision/homography-estimation/vpHomographyMalis.cpp
deleted file mode 100644
index 0423c11..0000000
--- a/src/computer-vision/homography-estimation/vpHomographyMalis.cpp
+++ /dev/null
@@ -1,855 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomographyMalis.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography estimation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpHomographyMalis.cpp
-
- This file implements the fonctions related with the homography
- estimation from non planar points using the Malis algorithms \cite Malis00b.
-
- The algorithm for 2D scene implemented in this file is described in Ezio
- Malis PhD thesis \cite TheseMalis.
-
-*/
-
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpMatrixException.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-const double eps = 1e-6 ;
-
-/**************************************************************************
- * NOM :
- * changeFrame
- *
- * DESCRIPTION :
- * Changement de repere Euclidien.
- *
- ****************************************************************************
- * 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
- *
- *
- * 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 ..
- *
- *
- ****************************************************************************
- */
-
-void changeFrame(unsigned int *pts_ref,
- unsigned int nb_pts,
- vpMatrix &pd, vpMatrix &p,
- vpMatrix &pnd, vpMatrix &pn,
- vpMatrix &M, vpMatrix &Mdp);
-void HLM2D(unsigned int nb_pts,
- vpMatrix &points_des,
- vpMatrix &points_cour,
- vpMatrix &H);
-void HLM3D(unsigned int nb_pts,
- vpMatrix &pd,
- vpMatrix &p,
- vpMatrix &H);
-void HLM(unsigned int q_cible,
- unsigned int nbpt,
- double *xm, double *ym,
- double *xmi, double *ymi,
- vpMatrix &H);
-
-void HLM(unsigned int q_cible,
- const std::vector<double> &xm, const std::vector<double> &ym,
- const std::vector<double> &xmi, const std::vector<double> &ymi,
- vpMatrix &H);
-
-void changeFrame(unsigned int *pts_ref,
- unsigned int nb_pts,
- vpMatrix &pd, vpMatrix &p,
- vpMatrix &pnd, vpMatrix &pn,
- vpMatrix &M, vpMatrix &Mdp)
-{
- unsigned int i,j, k ;
- unsigned int cont_pts; /* */
- double lamb_des[3]; /* */
- double lamb_cour[3] ; /* */
-
-
-
- /* Construction des matrices de changement de repere */
- vpMatrix Md(3,3) ;
- vpMatrix Mp(3,3) ;
-
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- M[j][i] = p[pts_ref[i]][j] ;
- Md[j][i] = pd[pts_ref[i]][j] ;
- }
- }
-
- /*calcul de la pseudo inverse */
- Mp= M.pseudoInverse(1e-16) ;
- Mdp = Md.pseudoInverse(1e-16) ;
-
- if (pts_ref[3] > 0) {
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- lamb_cour[i] = Mp[i][j]*p[pts_ref[3]][j] ;
- lamb_des[i] = Mdp[i][j]*pd[pts_ref[3]][j] ;
- }
- }
-
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- M[i][j] = M[i][j]*lamb_cour[j] ;
- Md[i][j] = Md[i][j]*lamb_des[j] ;
- }
- }
-
- Mdp = Md.pseudoInverse(1e-16);
- }
-
-
- /* changement de repere pour tous les points autres que
- les trois points de reference */
-
- cont_pts = 0 ;
- for (k=0;k<nb_pts;k++) {
- if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) {
- for (i=0;i<3;i++) {
- pn[cont_pts][i] = 0.0 ;
- pnd[cont_pts][i] = 0.0 ;
- for (j=0;j<3;j++) {
- pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j]*p[k][j] ;
- pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j]*pd[k][j] ;
- }
- }
- cont_pts = cont_pts + 1;
- }
- }
-
-
-}
-
-
-/**************************************************************************
- * NOM :
- * Homographie_CrvMafEstHomoPointsCible2D
- *
- * DESCRIPTION :
- * Calcul de l'homographie entre une image courante et une image desiree dans le
- * cas particulier d'une cible planaire de points (cible pi).
- * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
- *
- ****************************************************************************
- * ENTREES :
- * 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
- *
- ****************************************************************************
- * AUTEUR : BOSSARD Nicolas. INSA Rennes 5eme annee.
- *
- * DATE DE CREATION : 02/12/98
- *
- * DATES DE MISE A JOUR :
- *
- ****************************************************************************/
-void
-HLM2D(unsigned int nb_pts,
- vpMatrix &points_des,
- vpMatrix &points_cour,
- vpMatrix &H)
-{
- unsigned int i,j ;
-
- double vals_inf ;
- unsigned int contZeros, vect;
-
- /** allocation des matrices utilisees uniquement dans la procedure **/
- vpMatrix M(3*nb_pts,9) ;
- vpMatrix V(9,9) ;
- vpColVector sv(9) ;
-
- /** construction de la matrice M des coefficients dans le cas general **/
- for (j=0; j<nb_pts ;j++) {
- M[3*j][0] = 0 ;
- M[3*j][1] = 0 ;
- M[3*j][2] = 0 ;
- M[3*j][3] = -points_des[j][0]*points_cour[j][2] ;
- M[3*j][4] = -points_des[j][1]*points_cour[j][2] ;
- M[3*j][5] = -points_des[j][2]*points_cour[j][2] ;
- M[3*j][6] = points_des[j][0]*points_cour[j][1] ;
- M[3*j][7] = points_des[j][1]*points_cour[j][1] ;
- M[3*j][8] = points_des[j][2]*points_cour[j][1] ;
-
- M[3*j+1][0] = points_des[j][0]*points_cour[j][2] ;
- M[3*j+1][1] = points_des[j][1]*points_cour[j][2] ;
- M[3*j+1][2] = points_des[j][2]*points_cour[j][2] ;
- M[3*j+1][3] = 0 ;
- M[3*j+1][4] = 0 ;
- M[3*j+1][5] = 0 ;
- M[3*j+1][6] = -points_des[j][0]*points_cour[j][0] ;
- M[3*j+1][7] = -points_des[j][1]*points_cour[j][0] ;
- M[3*j+1][8] = -points_des[j][2]*points_cour[j][0] ;
-
- M[3*j+2][0] = -points_des[j][0]*points_cour[j][1] ;
- M[3*j+2][1] = -points_des[j][1]*points_cour[j][1] ;
- M[3*j+2][2] = -points_des[j][2]*points_cour[j][1] ;
- M[3*j+2][3] = points_des[j][0]*points_cour[j][0] ;
- M[3*j+2][4] = points_des[j][1]*points_cour[j][0] ;
- M[3*j+2][5] = points_des[j][2]*points_cour[j][0] ;
- M[3*j+2][6] = 0 ;
- M[3*j+2][7] = 0 ;
- M[3*j+2][8] = 0 ;
- }
-
- /** calcul de la pseudo-inverse V de M et des valeurs singulieres **/
- M.svd(sv,V);
-
- /*****
- La meilleure solution est le vecteur de V associe
- 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
- de 2 valeurs singulieres quasi=0
- *****/
- vals_inf = fabs(sv[0]) ;
- vect = 0 ;
- contZeros = 0;
- if (fabs(sv[0]) < eps) {
- contZeros = contZeros + 1 ;
- }
- for (j=1; j<9; j++) {
- if (fabs(sv[j]) < vals_inf) {
- vals_inf = fabs(sv[j]);
- vect = j ;
- }
- if (fabs(sv[j]) < eps) {
- contZeros = contZeros + 1 ;
- }
- }
-
-
- /** cas d'erreur : plus de 2 valeurs singulieres =0 **/
- if (contZeros > 2) {
- //vpERROR_TRACE("matrix is rank deficient");
- throw (vpMatrixException(vpMatrixException::matrixError,
- "matrix is rank deficient"));
- }
-
- H.resize(3,3) ;
- /** construction de la matrice H **/
- for (i=0; i<3; i++) {
- for (j=0; j<3; j++){
- H[i][j] = V[3*i+j][vect];
- }
- }
-
-
-}
-
-
-/**************************************************************************
- * NOM :
- * Homographie_CrvMafEstHomoPointsC3DEzio
- *
- * DESCRIPTION :
- * Calcul de l'homographie entre une image courante et une image desiree dans le
- * cas particulier d'une cible non planaire de points (cible pi).
- * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
- *
- *
- ****************************************************************************
- * ENTREES :
- * 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
- *
- ****************************************************************************
- **/
-void
-HLM3D(unsigned int nb_pts,
- vpMatrix &pd,
- vpMatrix &p,
- vpMatrix &H)
-{
- unsigned int i,j,k,ii,jj ;
- unsigned int cont_pts; /* Pour compter le nombre de points dans l'image */
- //unsigned int nl; /*** Nombre de lignes ***/
- unsigned int nc ; /*** Nombre de colonnes ***/
- unsigned int pts_ref[4]; /*** definit lesquels des points de
- l'image sont les points de reference***/
- /*** ***/
- unsigned int perm; /*** Compte le nombre de permutations, quand le nombre
- de permutations =0 arret de l'ordonnancement **/
- int cont_zeros; /*** pour compter les valeurs quasi= a zero ***/
- unsigned int cont;
- unsigned int vect;
-
- //int prob;
-
- /***** Corps de la fonction *****/
-
- /* allocation des matrices utilisees uniquement dans la procedure */
- //prob=0;
-
- vpMatrix M(3,3) ;
- vpMatrix Mdp(3,3) ;
- vpMatrix c(8,2) ; // matrice des coeff C
-
- vpColVector d(8) ;
- vpMatrix cp(2,8) ; //matrice pseudo-inverse de C
-
-
- vpMatrix H_int(3,3) ;
- vpMatrix pn((nb_pts-3),3) ; //points courant nouveau repere
-
-
- vpMatrix pnd((nb_pts-3),3) ; //points derives nouveau repere
-
- /* preparation du changement de repere */
- /****
- comme plan de reference on choisit pour le moment
- arbitrairement le plan contenant les points 1,2,3 du cinq
- ****/
- pts_ref[0] = 0 ;
- pts_ref[1] = 1 ;
- pts_ref[2] = 2 ;
- pts_ref[3] = 0 ;
-
- /* changement de repere pour tous les points autres que les trois points de reference */
-
- changeFrame(pts_ref,nb_pts,pd,p,pnd,pn,M,Mdp);
-
-
- cont_pts = nb_pts - 3 ;
-
- if (cont_pts < 5)
- {
- //vpERROR_TRACE(" not enough point to compute the homography ... ");
- throw (vpMatrixException(vpMatrixException::matrixError,
- "Not enough point to compute the homography"));
- }
-
- //nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ;
- nc = 7 ;
-
- /* Allocation matrice CtC */
- vpMatrix CtC(nc,nc) ;
-
- /* Initialisation matrice CtC */
- for (i=0;i<nc;i++) for (j=0;j<nc;j++) CtC[i][j] = 0.0;
-
-
- /* Allocation matrice M */
- vpColVector C(nc) ; //Matrice des coefficients
-
- /* construction de la matrice M des coefficients dans le cas general */
- /****
- inconnues du nouveau algorithme
- x1 = a ; x2 = b ; x3 = c ;
- x4 = ex ; x5 = ey ; x6 = ez ;
- qui deviennent apres changement :
- v1 = x1*x6 ; v2 = x1*x5 ;
- v3 = x2*x4 ; v4 = x2*x6 ;
- v5 = x3*x5 ; v6 = x3*x4 ;
- ****/
- cont = 0 ;
- for (i=0 ; i<nb_pts-5; i++) {
- for (j = i+1 ; j<nb_pts-4; j++) {
- for (k = j+1 ; k<nb_pts-3; k ++) {
- /* coeff a^2*b */
- C[0] = pn[i][2]*pn[j][2]*pn[k][1]*pnd[k][0] //
- * (pnd[j][0]*pnd[i][1] - pnd[j][1]*pnd[i][0])//
- + pn[i][2]*pn[k][2]*pn[j][1]*pnd[j][0]//
- *(pnd[i][0]*pnd[k][1] - pnd[i][1]*pnd[k][0])//
- + pn[j][2]*pn[k][2]*pn[i][1]*pnd[i][0] //
- *(pnd[k][0]*pnd[j][1] - pnd[k][1]*pnd[j][0]) ; //
- /* coeff a*b^2 */
- C[1] = pn[i][2]*pn[j][2]*pn[k][0]*pnd[k][1]//
- *(pnd[i][0]*pnd[j][1] - pnd[i][1]*pnd[j][0])//
- + pn[i][2]*pn[k][2]*pn[j][0]*pnd[j][1]//
- *(pnd[k][0]*pnd[i][1] - pnd[k][1]*pnd[i][0])//
- + pn[j][2]*pn[k][2]*pn[i][0]*pnd[i][1]//
- *(pnd[j][0]*pnd[k][1] - pnd[j][1]*pnd[k][0]) ;//
- /* coeff a^2 */
- C[2] = + pn[i][1]*pn[k][1]*pn[j][2]*pnd[j][0]//
- *(pnd[k][2]*pnd[i][0] - pnd[k][0]*pnd[i][2])//
- +pn[i][1]*pn[j][1]*pn[k][2]*pnd[k][0] //
- *(pnd[i][2]*pnd[j][0] - pnd[i][0]*pnd[j][2])
- + pn[j][1]*pn[k][1]*pn[i][2]*pnd[i][0] //
- *(pnd[j][2]*pnd[k][0] - pnd[j][0]*pnd[k][2]) ; //
-
-
-
- /* coeff b^2 */
- C[3] = pn[i][0]*pn[j][0]*pn[k][2]*pnd[k][1] //
- *(pnd[i][2]*pnd[j][1] - pnd[i][1]*pnd[j][2]) //
- + pn[i][0]*pn[k][0]*pn[j][2]*pnd[j][1] //
- *(pnd[k][2]*pnd[i][1] - pnd[k][1]*pnd[i][2]) //
- + pn[j][0]*pn[k][0]*pn[i][2]*pnd[i][1] //
- *(pnd[j][2]*pnd[k][1] - pnd[j][1]*pnd[k][2]) ; //
-
- /* coeff a */
- C[5] = pn[i][1]*pn[j][1]*pn[k][0]*pnd[k][2]//
- *(pnd[i][0]*pnd[j][2] - pnd[i][2]*pnd[j][0])//
- + pn[i][1]*pn[k][1]*pn[j][0]*pnd[j][2] //
- *(pnd[k][0]*pnd[i][2] - pnd[k][2]*pnd[i][0])//
- + pn[j][1]*pn[k][1]*pn[i][0]*pnd[i][2]//
- *(pnd[j][0]*pnd[k][2] - pnd[j][2]*pnd[k][0]) ;//
- /* coeff b */
- C[6] = pn[i][0]*pn[j][0]*pn[k][1]*pnd[k][2] //
- *(pnd[i][1]*pnd[j][2] - pnd[i][2]*pnd[j][1])//
- + pn[i][0]*pn[k][0]*pn[j][1]*pnd[j][2]//
- *(pnd[k][1]*pnd[i][2] - pnd[k][2]*pnd[i][1])//
- + pn[j][0]*pn[k][0]*pn[i][1]*pnd[i][2]//
- *(pnd[j][1]*pnd[k][2] - pnd[j][2]*pnd[k][1]) ;//
- /* coeff a*b */
- C[4] = pn[i][0]*pn[k][1]*pn[j][2] //
- *(pnd[k][0]*pnd[j][1]*pnd[i][2] - pnd[j][0]*pnd[i][1]*pnd[k][2])//
- + pn[k][0]*pn[i][1]*pn[j][2]//
- *(pnd[j][0]*pnd[k][1]*pnd[i][2] - pnd[i][0]*pnd[j][1]*pnd[k][2])//
- + pn[i][0]*pn[j][1]*pn[k][2]//
- *(pnd[k][0]*pnd[i][1]*pnd[j][2] - pnd[j][0]*pnd[k][1]*pnd[i][2])//
- + pn[j][0]*pn[i][1]*pn[k][2]//
- *(pnd[i][0]*pnd[k][1]*pnd[j][2] - pnd[k][0]*pnd[j][1]*pnd[i][2])//
- + pn[k][0]*pn[j][1]*pn[i][2]//
- *(pnd[j][0]*pnd[i][1]*pnd[k][2] - pnd[i][0]*pnd[k][1]*pnd[j][2])//
- + pn[j][0]*pn[k][1]*pn[i][2]//
- *(pnd[i][0]*pnd[j][1]*pnd[k][2] - pnd[k][0]*pnd[i][1]*pnd[j][2]) ;//
-
- cont = cont+1 ;
- /* construction de la matrice CtC */
- for (ii=0;ii<nc;ii++) {
- for (jj=ii;jj<nc;jj++) {
- CtC[ii][jj] = CtC[ii][jj] + C[ii]*C[jj];
- }
- }
-
- }
- }
- }
-
-
-
- /* calcul de CtC */
- for (i=0; i<nc ;i++) {
- for (j=i+1; j<nc ;j++) CtC[j][i] = CtC[i][j];
- }
-
- //nl = cont ; /* nombre de lignes */
- nc = 7 ; /* nombre de colonnes */
-
- /* Creation de matrice CtC termine */
- /* Allocation matrice V */
- vpMatrix V(nc,nc) ;
- /*****
- Preparation au calcul de la svd (pseudo-inverse)
- pour obtenir une solution il faut au moins 5 equations independantes
- donc il faut au moins la mise en correspondence de 3+5 points
- *****/
- vpColVector sv(nc) ; //Vecteur contenant les valeurs singulieres
-
- CtC.svd(sv,V) ;
-
- /*****
- Il faut un controle sur le rang de la matrice !!
- La meilleure solution est le vecteur de V associe
- a la valeur singuliere la plus petite en valeur
- absolu
- *****/
-
- vpColVector svSorted(nc) ; // sorted singular value
-
- // sorting the singular value
- for (i=0; i < nc ;i++) svSorted[i] = sv[i] ;
- perm = 1 ;
- double v_temp;
- while (perm != 0) {
- perm = 0;
- for (i=1; i < nc ;i++) {
- if (svSorted[i-1] > svSorted[i]) {
- v_temp = svSorted[i-1] ;
- svSorted[i-1] = svSorted[i] ;
- svSorted[i] = v_temp ;
- perm = perm + 1;
- }
- }
- }
-
- /*****
- Parcours de la matrice ordonnee des valeurs singulieres
- On note "cont_zeros" le nbre de valeurs quasi= a 0.
- On note "vect" le rang de la plus petite valeur singliere
- en valeur absolu
- *****/
-
- vect = 0 ; cont_zeros = 0 ; cont = 0 ;
- for (j=0; j < nc; j++) {
- //if (fabs(sv[j]) == svSorted[cont]) vect = j ;
- if (std::fabs(sv[j]-svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j],svSorted[cont]))) vect = j ;
- if (std::fabs(sv[j]/svSorted[nc-1]) < eps) cont_zeros = cont_zeros + 1 ;
- }
-
- if (cont_zeros > 5) {
- // printf("erreur dans le rang de la matrice: %d \r\n ",7-cont_zeros);
- HLM2D(nb_pts,pd,p,H);
- }
- else
- {
-
- // 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 ;
- d[0] = V[2][vect] ;
- d[1] = V[4][vect] ;
- d[2] = V[1][vect] ;
- d[3] = V[0][vect] ;
- d[4] = V[3][vect] ;
- d[5] = V[4][vect] ;
- d[6] = V[0][vect] ;
- d[7] = V[1][vect] ;
-
- c[0][0] = V[5][vect] ; c[0][1] = 0.0 ;
- c[1][0] = V[6][vect] ; c[1][1] = 0.0 ;
- c[2][0] = V[3][vect] ; c[2][1] = 0.0 ;
- c[3][0] = V[4][vect] ; c[3][1] = 0.0
- ;
- c[4][0] = 0.0 ; c[4][1] = V[6][vect] ;
- c[5][0] = 0.0 ; c[5][1] = V[5][vect] ;
- c[6][0] = 0.0 ; c[6][1] = V[2][vect] ;
- c[7][0] = 0.0 ; c[7][1] = V[4][vect] ;
-
-
-
- /// Calcul de la pseudo-inverse de C
- cp = c.pseudoInverse(1e-6) ;
-
-
- vpColVector H_nr(3), temp ; // Homographie diagonale
- // Multiplication de la matrice H_nr par le vecteur cp
- temp = cp * d;
-
- H_nr[0] = temp[0] ; H_nr[1] = temp[1] ;
- H_nr[2] = 1.0 ;
-
- vpMatrix T(9,3) ; T =0 ;
- T[0][0] = -V[1][vect] ; T[0][1] = V[0][vect] ;
- T[1][0] = V[4][vect] ; T[1][2] = -V[2][vect] ;
- T[2][0] = -V[6][vect] ; T[2][1] = V[2][vect] ;
- T[3][0] = V[6][vect] ; T[3][2] = -V[0][vect] ;
- T[4][0] = -V[3][vect] ; T[4][1] = V[6][vect] ;
- T[5][0] = V[3][vect] ; T[5][2] = -V[1][vect] ;
- T[6][0] = -V[5][vect] ; T[6][1] = V[4][vect] ;
- T[7][0] = V[5][vect] ; T[7][2] = -V[6][vect] ;
- T[8][1] = -V[5][vect] ; T[8][2] = V[2][vect] ;
-
-
- vpMatrix Hd(3,3) ; // diag(gu,gv,gw)
- for (i=0 ; i < 3 ; i++) Hd[i][i] = H_nr[i] ;
-
- // H = M diag(gu,gv,gw) M*-1
- H = M*Hd*Mdp ;
-
-
-
- }
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/**************************************************************************
- * NOM :
- * Homographie_CrvMafCalculHomographie
- *
- * DESCRIPTION :
- * Calcul de l'homographie, en fonction de la cible desiree et de la cible
- * en cours. C'est une estimation lineaire.
- * Cette procedure n'effectue pas elle-meme le calcul de l'homographie :
- * elle se contente d'appeler la bonne sous-procedure.
- * Cette procedure est appellee par "crv_maf_calcul_tomographie".
- *
- ****************************************************************************
- * ENTREES :
- * STR_CIBLE_ASSER *cible_asser Pointeur sur structure contenant les
- * commandes du robot, les donnees de la
- * carte...
- * Voir "cvda/edixaa/param/robot.h"
- * STR_VITESSE_ROBOT *data_common Pointeur sur la structure decrivant la
- * cible d'asservissement.
- * Voir "cvda/edixia/param/param.h"
- * STR_MACH_DIV *machine_div Pointeur sur structure contenant divers
- * parametres de configuration du robot.
- * Voir "cvda/edixia/param/param.h"
- *
- * SORTIES :
- *
- * double **H matrice d'homographie
-
- *
- ****************************************************************************
- * AUTEUR : BOSSARD Nicolas. INSA Rennes 5eme annee.
- *
- * DATE DE CREATION : 01/12/98
- *
- * DATES DE MISE A JOUR :
- *
- ****************************************************************************/
-void
-HLM(unsigned int q_cible,
- unsigned int nbpt,
- double *xm, double *ym,
- double *xmi, double *ymi,
- vpMatrix &H)
-{
- unsigned int i;
-
- /****
- on regarde si il y a au moins un point mais pour l'homographie
- il faut au moins quatre points
- ****/
- vpMatrix pd(nbpt,3) ;
- vpMatrix p(nbpt,3) ;
-
- for (i=0;i<nbpt;i++) {
- /****
- on assigne les points fournies par la structure robot
- pour la commande globale
- ****/
- pd[i][0] = xmi[i];
- pd[i][1] = ymi[i];
- pd[i][2] = 1.0 ;
- p[i][0] = xm[i];
- p[i][1] = ym[i];
- p[i][2] = 1.0 ;
- }
-
-
- switch (q_cible) {
- case (1):
- case (2):
- /* La cible est planaire de type points */
-
- HLM2D(nbpt,pd,p,H);
-
- break;
- case (3) : /* cible non planaire : chateau */
- /* cible non planaire de type points */
- HLM3D(nbpt,pd,p,H);
- break;
- } /* fin switch */
-
-
-
-} /* fin procedure calcul_homogaphie */
-#endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-void
-HLM(unsigned int q_cible,
- const std::vector<double> &xm, const std::vector<double> &ym,
- const std::vector<double> &xmi, const std::vector<double> &ymi,
- vpMatrix &H)
-{
- unsigned int nbpt = (unsigned int)xm.size();
-
- /****
- on regarde si il y a au moins un point mais pour l'homographie
- il faut au moins quatre points
- ****/
- vpMatrix pd(nbpt,3) ;
- vpMatrix p(nbpt,3) ;
-
- for (unsigned int i=0;i<nbpt;i++) {
- /****
- on assigne les points fournies par la structure robot
- pour la commande globale
- ****/
- pd[i][0] = xmi[i];
- pd[i][1] = ymi[i];
- pd[i][2] = 1.0 ;
- p[i][0] = xm[i];
- p[i][1] = ym[i];
- p[i][2] = 1.0 ;
- }
-
- switch (q_cible) {
- case (1):
- case (2):
- /* La cible est planaire de type points */
-
- HLM2D(nbpt,pd,p,H);
-
- break;
- case (3) : /* cible non planaire : chateau */
- /* cible non planaire de type points */
- HLM3D(nbpt,pd,p,H);
- break;
- } /* fin switch */
-
-
-
-} /* fin procedure calcul_homogaphie */
-
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- \brief Computes the homography matrix from planar \cite TheseMalis or non planar points
- using Ezio Malis linear method (HLM) \cite Malis00b.
-
- Computes H such as \f[
- ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}
- \f]
-
- The algorithm for 2D scene implemented in this file is described in Ezio
- Malis PhD thesis.
-
- If the boolean isplanar is true the point is assumed to be in a plane
- otherwise there are assumed to be planar.
-
- The reference planar is the plane build from the 3 first points.
-
-*/
-void vpHomography::HLM(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- bool isplanar,
- vpHomography &aHb)
-{
- unsigned int i,j;
- unsigned int q_cible;
- vpMatrix H; // matrice d'homographie en metre
-
- if (isplanar)
- q_cible =1;
- else
- q_cible =3;
-
- ::HLM(q_cible,n, xa,ya,xb,yb,H) ;
-
- for(i=0;i<3;i++)
- for(j=0;j<3;j++)
- aHb[i][j] = H[i][j];
-}
-
-#endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- 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<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- bool isplanar,
- vpHomography &aHb)
-{
- unsigned int n = (unsigned int) xb.size();
- if (yb.size() != n || xa.size() != n || ya.size() != n)
- throw(vpException(vpException::dimensionError,
- "Bad dimension for HLM shomography estimation"));
-
- // 4 point are required
- if(n<4)
- throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
-
- // The reference plane is the plane build from the 3 first points.
- unsigned int q_cible;
- vpMatrix H; // matrice d'homographie en metre
-
- if (isplanar)
- q_cible =1;
- else
- q_cible =3;
-
- ::HLM(q_cible, xa, ya, xb, yb, H) ;
-
- aHb = H;
-}
-
diff --git a/src/computer-vision/homography-estimation/vpHomographyRansac.cpp b/src/computer-vision/homography-estimation/vpHomographyRansac.cpp
deleted file mode 100644
index 777c0ee..0000000
--- a/src/computer-vision/homography-estimation/vpHomographyRansac.cpp
+++ /dev/null
@@ -1,618 +0,0 @@
-
-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Lagadic / IRISA-INRIA Rennes, 2005
- * www : http://www.irisa.fr/lagadic
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File: vpHomographyRansac.cpp
- * Project: ViSP 2.0
- * Author: Eric Marchand
- * From: vpHomographyRansac.cpp, ViSP 2
- *
- * Version control
- * ===============
- *
- * $Id: vpHomographyRansac.cpp 4649 2014-02-07 14:57:11Z fspindle $
- * optimized by Tran to improve speed.
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#include <visp/vpHomography.h>
-#include <visp/vpColVector.h>
-#include <visp/vpRansac.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpMeterPixelConversion.h>
-
-#define vpEps 1e-6
-
-/*!
- \file vpHomographyRansac.cpp
- \brief function used to estimate an homography using the Ransac algorithm
-*/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-bool iscolinear(double *x1, double *x2, double *x3);
-bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
-
-bool
-iscolinear(double *x1, double *x2, double *x3)
-{
- vpColVector p1(3), p2(3), p3(3);
- p1 << x1 ;
- p2 << x2 ;
- p3 << x3 ;
- //vpColVector v;
- //vpColVector::cross(p2-p1, p3-p1, v);
- //return (v.sumSquare() < vpEps);
- // Assume inhomogeneous coords, or homogeneous coords with equal
- // scale.
- return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
-}
-bool
-isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
-{
- return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
-}
-
-
-bool
-vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind,
- double threshold_area)
-{
-
- unsigned int i, j, k;
-
- for (i=1 ; i < 4 ; i++)
- for (j=0 ; j<i ; j++)
- if (ind[i]==ind[j]) return true ;
-
- unsigned int n = x.getRows()/4 ;
- double pa[4][3] ;
- double pb[4][3] ;
-
-
-
- for(i = 0 ; i < 4 ; i++)
- {
- pb[i][0] = x[2*ind[i]] ;
- pb[i][1] = x[2*ind[i]+1] ;
- pb[i][2] = 1;
-
- pa[i][0] = x[2*n+2*ind[i]] ;
- pa[i][1] = x[2*n+2*ind[i]+1] ;
- pa[i][2] = 1;
- }
-
- i = 0, j = 1, k = 2;
-
- double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
-
- i = 0; j = 1, k = 3;
- double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
-
- i = 0; j = 2, k = 3;
- double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
-
- i = 1; j = 2, k = 3;
- double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
-
- double sum_area = area012 + area013 + area023 + area123;
-
- return ((sum_area < threshold_area) ||
- (iscolinear(pa[0],pa[1],pa[2]) ||
- iscolinear(pa[0],pa[1],pa[3]) ||
- iscolinear(pa[0],pa[2],pa[3]) ||
- iscolinear(pa[1],pa[2],pa[3]) ||
- iscolinear(pb[0],pb[1],pb[2]) ||
- iscolinear(pb[0],pb[1],pb[3]) ||
- iscolinear(pb[0],pb[2],pb[3]) ||
- iscolinear(pb[1],pb[2],pb[3])));
-}
- /*
-\brief
-Function to determine if a set of 4 pairs of matched points give rise
-to a degeneracy in the calculation of a homography as needed by RANSAC.
-This involves testing whether any 3 of the 4 points in each set is
-colinear.
-
-point are coded this way
-x1b,y1b, x2b, y2b, ... xnb, ynb
-x1a,y1a, x2a, y2a, ... xna, yna
-leading to 2*2*n
-*/
-bool
-vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
-{
- for (unsigned int i = 1; i < 4 ; i++)
- for (unsigned int j = 0 ;j < i ; j++)
- if (ind[i] == ind[j]) return true ;
-
- unsigned int n = x.getRows()/4;
- double pa[4][3];
- double pb[4][3];
- unsigned int n2 = 2 * n;
- unsigned int ind2;
- for(unsigned int i = 0; i < 4 ;i++)
- {
- ind2 = 2 * ind[i];
- pb[i][0] = x[ind2];
- pb[i][1] = x[ind2+1];
- pb[i][2] = 1;
-
- pa[i][0] = x[n2+ind2] ;
- pa[i][1] = x[n2+ind2+1] ;
- pa[i][2] = 1;
- }
- return ( iscolinear(pa[0],pa[1],pa[2]) ||
- iscolinear(pa[0],pa[1],pa[3]) ||
- iscolinear(pa[0],pa[2],pa[3]) ||
- iscolinear(pa[1],pa[2],pa[3]) ||
- iscolinear(pb[0],pb[1],pb[2]) ||
- iscolinear(pb[0],pb[1],pb[3]) ||
- iscolinear(pb[0],pb[2],pb[3]) ||
- iscolinear(pb[1],pb[2],pb[3]));
-}
-bool
-vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya)
-{
- unsigned int n = (unsigned int)xb.size();
- if (n < 4)
- throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
-
- std::vector<vpColVector> pa(n), pb(n);
- for (unsigned i=0; i<n;i++) {
- pa[i].resize(3);
- pa[i][0] = xa[i];
- pa[i][1] = ya[i];
- pa[i][2] = 1;
- pb[i].resize(3);
- pb[i][0] = xb[i];
- pb[i][1] = yb[i];
- pb[i][2] = 1;
- }
-
- for (unsigned int i = 0; i < n-2; i++) {
- for (unsigned int j = i+1; j < n-1; j++) {
- for (unsigned int k = j+1; k < n ; k++)
- {
- if (isColinear(pa[i], pa[j], pa[k])) {
- return true;
- }
- if (isColinear(pb[i], pb[j], pb[k])){
- return true;
- }
- }
- }
- }
- return false;
-}
-// Fit model to this random selection of data points.
-void
-vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
-{
- unsigned int i ;
- unsigned int n = x.getRows()/4 ;
- std::vector<double> xa(4), xb(4);
- std::vector<double> ya(4), yb(4);
- unsigned int n2 = n * 2;
- unsigned int ind2;
- for(i=0 ; i < 4 ; i++)
- {
- ind2 = 2 * ind[i];
- xb[i] = x[ind2] ;
- yb[i] = x[ind2+1] ;
-
- xa[i] = x[n2+ind2] ;
- ya[i] = x[n2+ind2+1] ;
- }
-
- vpHomography aHb ;
- try {
- vpHomography::HLM(xb, yb, xa, ya, true, aHb);
- }
- catch(...)
- {
- aHb.setIdentity();
- }
-
- M.resize(9);
- for (i=0 ; i <9 ; i++)
- {
- M[i] = aHb.data[i] ;
- }
- aHb /= aHb[2][2] ;
-}
-
-
-// Evaluate distances between points and model.
-double
-vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
-{
- unsigned int i ;
- unsigned int n = x.getRows()/4 ;
- unsigned int n2 = n * 2;
- unsigned int i2;
- vpColVector *pa;
- vpColVector *pb;
-
- pa = new vpColVector [n];
- pb = new vpColVector [n];
-
- for( i=0 ; i < n ; i++)
- {
- i2 = 2 * i;
- pb[i].resize(3) ;
- pb[i][0] = x[i2] ;
- pb[i][1] = x[i2+1] ;
- pb[i][2] = 1;
-
- pa[i].resize(3) ;
- pa[i][0] = x[n2+i2] ;
- pa[i][1] = x[n2+i2+1] ;
- pa[i][2] = 1;
- }
-
- vpMatrix aHb(3,3) ;
-
- for (i=0 ; i <9 ; i++)
- {
- aHb.data[i] = M[i];
- }
-
- aHb /= aHb[2][2];
-
- d.resize(n);
-
- vpColVector Hpb ;
- for (i=0 ; i <n ; i++)
- {
- Hpb = aHb*pb[i] ;
- Hpb /= Hpb[2] ;
- d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
- }
-
- delete [] pa;
- delete [] pb;
-
- return 0 ;
-}
-#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-void
-vpHomography::initRansac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya,
- vpColVector &x)
-{
- x.resize(4*n) ;
- unsigned int n2 = n * 2;
- unsigned int i2;
- for (unsigned int i=0 ; i < n ; i++)
- {
- i2 = 2 * i;
- x[i2] = xb[i] ;
- x[i2+1] = yb[i] ;
- x[n2+i2] = xa[i] ;
- x[n2+i2+1] = ya[i] ;
- }
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- \deprecated This method is deprecated. You should rather use
- vpHomography::ransac(const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, vpHomography &, std::vector<bool> &, double &, unsigned int, double, bool)
- */
-bool
-vpHomography::ransac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &aHb,
- int consensus,
- double threshold
- )
-{
- vpColVector x ;
- vpHomography::initRansac(n, xb, yb, xa, ya, x) ;
-
- vpColVector M ;
- vpColVector inliers(n) ;
-
-
- bool ransacable = vpRansac<vpHomography>::ransac(n, x, 4, threshold, M, inliers, consensus);
-
- if(ransacable)
- {
- for (unsigned int i = 0 ;i < 9 ;i++)
- {
- aHb.data[i] = M[i];
- }
- aHb /= aHb[2][2];
- }
- return ransacable;
-}
-
-/*!
- \deprecated This method is deprecated. You should rather use
- vpHomography::ransac(const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, const std::vector<double> &, vpHomography &, std::vector<bool> &, double &, unsigned int, double, bool)
-
- Computes homography matrix \f$ b^H_a \f$ such as \f$X_b = b^H_a X_a \f$ with
- \f$ X_a = (xa, ya)^t \f$ and \f$ X_b = (xb, yb)^t \f$.
-
- \param n : Number of points.
- \param xb, yb : Coordinates of the points in \f$ X_b \f$ vector.
- \param xa, ya : Coordinates of the points in \f$ X_a \f$ vector.
-
- \param bHa : Homography matrix computed from \f$ X_a \f$ and \f$ X_b \f$
- vectors.
-
- \param inliers : n dimention vector indicating if a point is an inlier
- (value 1.0) or an outlier (value 0). Matches are stocked in inliers vector
- column.
-
- \param residual : Residual. Not used.
-
- \param consensus : Minimal number of points (less than n) fitting the model.
-
- \param threshold : Threshold for outlier removing.
-
- \param areaThreshold : Ensure that the area formed by every 3 points within
- the 4 points used to compute the homography is greater than this
- threshold. If the area is smaller, we are in a degenerate case.
-
- \return true if the homography could be computed from 4 non-degenerated
- points.
-
-*/
-bool vpHomography::ransac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya ,
- vpHomography &bHa,
- vpColVector &inliers,
- double /* residual */,
- int consensus,
- double threshold,
- double areaThreshold)
-{
- vpColVector x ;
- vpHomography::initRansac(n, xb, yb, xa, ya, x);
-
- vpColVector M ;
-
- bool ransacable= vpRansac<vpHomography>::ransac(n, x, 4,
- threshold, M,
- inliers, consensus,
- areaThreshold);
-
- for (unsigned int i = 0 ;i < 9 ;i++)
- {
- bHa.data[i] = M[i];
- }
-
- bHa /= bHa[2][2];
- return ransacable;
-}
-#endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
-
- 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<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- unsigned int nbInliersConsensus,
- double threshold,
- bool normalization)
-{
- unsigned int n = (unsigned int)xb.size();
- if (yb.size() != n || xa.size() != n || ya.size() != n)
- throw(vpException(vpException::dimensionError,
- "Bad dimension for robust homography estimation"));
-
- // 4 point are required
- if(n<4)
- throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
-
- vpUniRand random((const long)time(NULL)) ;
-
- std::vector<unsigned int> best_consensus;
- std::vector<unsigned int> cur_consensus;
- std::vector<unsigned int> cur_outliers;
- std::vector<unsigned int> cur_randoms;
-
- std::vector<unsigned int> rand_ind;
-
- unsigned int nbMinRandom = 4 ;
- unsigned int ransacMaxTrials = 1000;
- unsigned int maxDegenerateIter = 1000;
-
- unsigned int nbTrials = 0;
- unsigned int nbDegenerateIter = 0;
- unsigned int nbInliers = 0;
-
- bool foundSolution = false;
-
- std::vector<double> xa_rand(nbMinRandom);
- std::vector<double> ya_rand(nbMinRandom);
- std::vector<double> xb_rand(nbMinRandom);
- std::vector<double> yb_rand(nbMinRandom);
-
- if (inliers.size() != n)
- inliers.resize(n);
-
- while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
- {
- cur_outliers.clear();
- cur_randoms.clear();
-
- bool degenerate = true;
- while(degenerate == true){
- std::vector<bool> usedPt(n, false);
-
- rand_ind.clear();
- for(unsigned int i = 0; i < nbMinRandom; i++)
- {
- // Generate random indicies in the range 0..n
- unsigned int r = (unsigned int)ceil(random()*n) -1;
- while(usedPt[r]) {
- r = (unsigned int)ceil(random()*n) -1;
- }
- usedPt[r] = true;
- rand_ind.push_back(r);
-
- xa_rand[i] = xa[r];
- ya_rand[i] = ya[r];
- xb_rand[i] = xb[r];
- yb_rand[i] = yb[r];
- }
-
- try{
- if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
- vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
- degenerate = false;
- }
- }
- catch(...){
- degenerate = true;
- }
-
- nbDegenerateIter ++;
-
- if (nbDegenerateIter > maxDegenerateIter){
- vpERROR_TRACE("Unable to select a nondegenerate data set");
- throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
- }
- }
-
- aHb /= aHb[2][2] ;
-
- // Computing Residual
- double r = 0;
- vpColVector a(3), b(3), c(3) ;
- for (unsigned int i=0 ; i < nbMinRandom ; i++) {
- a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
- b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
-
- c = aHb*b; c /= c[2] ;
- r += (a-c).sumSquare() ;
- //cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
- }
-
- // Finding inliers & ouliers
- r = sqrt(r/nbMinRandom);
- //std::cout << "Candidate residual: " << r << std::endl;
- if (r < threshold)
- {
- unsigned int nbInliersCur = 0;
- for (unsigned int i = 0; i < n ; i++)
- {
- a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
- b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- double error = sqrt((a-c).sumSquare()) ;
- if(error <= threshold){
- nbInliersCur++;
- cur_consensus.push_back(i);
- inliers[i] = true;
- }
- else{
- cur_outliers.push_back(i);
- inliers[i] = false;
- }
- }
- //std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
- if(nbInliersCur > nbInliers)
- {
- foundSolution = true;
- best_consensus = cur_consensus;
- nbInliers = nbInliersCur;
- }
-
- cur_consensus.clear();
- }
-
- nbTrials++;
- if(nbTrials >= ransacMaxTrials){
- vpERROR_TRACE("Ransac reached the maximum number of trials");
- foundSolution = true;
- }
- }
-
- if(foundSolution){
- if(nbInliers >= nbInliersConsensus)
- {
- std::vector<double> xa_best(best_consensus.size());
- std::vector<double> ya_best(best_consensus.size());
- std::vector<double> xb_best(best_consensus.size());
- std::vector<double> yb_best(best_consensus.size());
-
- for(unsigned i = 0 ; i < best_consensus.size(); i++)
- {
- xa_best[i] = xa[best_consensus[i]];
- ya_best[i] = ya[best_consensus[i]];
- xb_best[i] = xb[best_consensus[i]];
- yb_best[i] = yb[best_consensus[i]];
- }
-
- vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
- aHb /= aHb[2][2];
-
- residual = 0 ;
- vpColVector a(3), b(3), c(3);
- for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
- a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
- b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- residual += (a-c).sumSquare() ;
- }
-
- residual = sqrt(residual/best_consensus.size());
- return true;
- }
- else {
- return false;
- }
- }
- else {
- return false;
- }
-}
diff --git a/src/computer-vision/homography-estimation/vpHomographyVVS.cpp b/src/computer-vision/homography-estimation/vpHomographyVVS.cpp
deleted file mode 100644
index 9d8688d..0000000
--- a/src/computer-vision/homography-estimation/vpHomographyVVS.cpp
+++ /dev/null
@@ -1,735 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomographyVVS.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homography transformation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-//#include <computeHomography.h>
-//#include <utilsHomography.h>
-#include <visp/vpRobust.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpMath.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPlane.h>
-#include <iostream>
-#include <visp/vpExponentialMap.h>
-
-
-const double vpHomography::threshold_rotation = 1e-7;
-const double vpHomography::threshold_displacement = 1e-18;
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-static void
-updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
-{
- unsigned int i,j;
-
- double sinu,cosi,mcosi,u[3], s;
- vpRotationMatrix rd ;
-
-
- s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
- if (s > 1.e-25)
- {
-
- for (i=0;i<3;i++) u[i] = dx[i]/s;
- sinu = sin(s);
- cosi = cos(s);
- mcosi = 1-cosi;
- rd[0][0] = cosi + mcosi*u[0]*u[0];
- rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
- rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
- rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
- rd[1][1] = cosi + mcosi*u[1]*u[1];
- rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
- rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
- rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
- rd[2][2] = cosi + mcosi*u[2]*u[2];
- }
- else
- {
- for (i=0;i<3;i++)
- {
- for (j=0;j<3;j++) rd[i][j] = 0.0;
- rd[i][i] = 1.0;
- }
-
- }
-
- vpHomogeneousMatrix Delta ;
- Delta.insert(rd) ;
-
- mati = Delta.inverse() * mati ;
-}
-
-double
-vpHomography::computeRotation(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpHomogeneousMatrix &c2Mc1,
- int userobust
- )
-{
- vpColVector e(2) ;
- double r_1 = -1 ;
-
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
-
- vpMatrix H2(2,3) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,3) ;
- vpColVector e1(2) ;
-
- int only_1 = 0 ;
- int only_2 = 0 ;
- int iter = 0 ;
-
- unsigned int n=0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++) {
- // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
- if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
- &&
- (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
- {
- n++ ;
- }
- }
- //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
- if ( (! only_1) && (! only_2) )
- n *=2 ;
-
- vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
- vpMatrix c2Rc1(3,3) ;
- double r =0 ;
- while (vpMath::equal(r_1,r,threshold_rotation) == false )
- {
-
- r_1 =r ;
- // compute current position
-
- //Change frame (current)
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- c2Rc1[i][j] = c2Mc1[i][j] ;
-
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++) {
- //if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
- if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
- &&
- (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
- {
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
-
- Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
- Hp1 = c2Rc1*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
-
-
- // set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
-
- H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
- H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
- H2 *=-1 ;
- H2 = H2*c2Rc1.t() ;
-
- // Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
-
- // set up the interaction matrix
- x = Hp1[0] ;
- y = Hp1[1] ;
-
- H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
- H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
-
- // Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
- if (only_2==1)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- }
- else
- if (only_1==1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
- }
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
-
- k++ ;
- }
- }
-
- if (userobust)
- {
- robust.setIteration(0);
-
- for (unsigned int l=0 ; l < n ; l++)
- {
- res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
- }
- robust.MEstimator(vpRobust::TUKEY, res, w);
-
-
- // compute the pseudo inverse of the interaction matrix
- for (unsigned int l=0 ; l < n ; l++)
- {
- W[2*l][2*l] = w[l] ;
- W[2*l+1][2*l+1] = w[l] ;
- }
- }
- else
- {
- for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
- }
- // CreateDiagonalMatrix(w, W) ;
- (L).pseudoInverse(Lp, 1e-6) ;
- // Compute the camera velocity
- vpColVector c2rc1, v(6) ;
-
- c2rc1 = -2*Lp*W*e ;
- for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2rc1[i] ;
-
- // only for simulation
-
- updatePoseRotation(c2rc1, c2Mc1) ;
- r =e.sumSquare() ;
-
- if ((W*e).sumSquare() < 1e-10) break ;
- if (iter>25) break ;
- iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
-
- }
-
- // std::cout << c2Mc1 <<std::endl ;
- return (W*e).sumSquare() ;
-}
-
-
-static void
-getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
-{
- double A1 = cMo[0][0]*oN.getA()+ cMo[0][1]*oN.getB() + cMo[0][2]*oN.getC();
- double B1 = cMo[1][0]*oN.getA()+ cMo[1][1]*oN.getB() + cMo[1][2]*oN.getC();
- double C1 = cMo[2][0]*oN.getA()+ cMo[2][1]*oN.getB() + cMo[2][2]*oN.getC();
- double D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
-
- cN.resize(3) ;
- cN[0] = A1 ;
- cN[1] = B1 ;
- cN[2] = C1 ;
- cd = -D1 ;
-}
-
-double
-vpHomography::computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane &oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- )
-{
- vpColVector e(2) ;
- double r_1 = -1 ;
-
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
-
- vpMatrix H2(2,6) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,6) ;
- vpColVector e1(2) ;
-
- int only_1;
- int only_2;
- int iter = 0 ;
- unsigned int n=0 ;
- n = nbpoint ;
- only_1 = 1 ;
- only_2 = 0 ;
-
- //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
- if ( (! only_1) && (! only_2) )
- n *=2 ;
-
- vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
-
- vpColVector N1(3), N2(3) ;
- double d1, d2 ;
-
- double r =1e10 ;
- iter =0 ;
- while (vpMath::equal(r_1,r,threshold_displacement) == false )
- {
- r_1 =r ;
- // compute current position
-
- //Change frame (current)
- vpHomogeneousMatrix c1Mc2, c2Mo ;
- vpRotationMatrix c1Rc2, c2Rc1 ;
- vpTranslationVector c1Tc2, c2Tc1 ;
- c1Mc2 = c2Mc1.inverse() ;
- c2Mc1.extract(c2Rc1) ;
- c2Mc1.extract(c2Tc1) ;
- c2Mc1.extract(c1Rc2) ;
- c1Mc2.extract(c1Tc2) ;
-
- c2Mo = c2Mc1*c1Mo ;
-
- getPlaneInfo(oN, c1Mo, N1, d1) ;
- getPlaneInfo(oN, c2Mo, N2, d2) ;
-
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++)
- {
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
-
- vpMatrix H(3,3) ;
-
- Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
- Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
-
-
- // set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
- double Z1 ;
-
- Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
-
-
- H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
- H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
- H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
- H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
- H2 *=-1 ;
-
- vpMatrix c1CFc2(6,6) ;
- {
- vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
- for (unsigned int k_=0 ; k_ < 3 ; k_++)
- for (unsigned int l=0 ; l<3 ; l++)
- {
- c1CFc2[k_][l] = c1Rc2[k_][l] ;
- c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
- c1CFc2[k_][l+3] = sTR[k_][l] ;
- }
- }
- H2 = H2*c1CFc2 ;
-
- // Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
-
- x = Hp1[0] ;
- y = Hp1[1] ;
-
- Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
-
- H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
- H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
- H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
- H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
-
- // Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
- if (only_2==1)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- }
- else
- if (only_1==1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
- }
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
-
-
- k++ ;
- }
-
- if (userobust)
- {
- robust.setIteration(0);
- for (unsigned int l=0 ; l < n ; l++)
- {
- res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
- }
- robust.MEstimator(vpRobust::TUKEY, res, w);
-
- // compute the pseudo inverse of the interaction matrix
- for (unsigned int l=0 ; l < n ; l++)
- {
- W[2*l][2*l] = w[l] ;
- W[2*l+1][2*l+1] = w[l] ;
- }
- }
- else
- {
- for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
- }
- (W*L).pseudoInverse(Lp, 1e-16) ;
- // Compute the camera velocity
- vpColVector c2Tcc1 ;
-
- c2Tcc1 = -1*Lp*W*e ;
-
- // only for simulation
-
- c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
- // UpdatePose2(c2Tcc1, c2Mc1) ;
- r =(W*e).sumSquare() ;
-
- if (r < 1e-15) {break ; }
- if (iter>1000){break ; }
- if (r>r_1) { break ; }
- iter++ ;
- }
-
- return (W*e).sumSquare() ;
-
-}
-
-
-double
-vpHomography::computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane *oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- )
-{
-
-
- vpColVector e(2) ;
- double r_1 = -1 ;
-
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
-
- vpMatrix H2(2,6) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,6) ;
- vpColVector e1(2) ;
-
-
- int only_1;
- int only_2;
- int iter = 0 ;
- unsigned int i ;
- unsigned int n=0 ;
- only_1 = 1 ;
- only_2 = 0 ;
- n = nbpoint ;
- // if ((only_1==1) || (only_2==1)) ; else n *=2 ;
- if ( (! only_1) && (! only_2) )
- n *=2 ;
-
- vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
-
- vpColVector N1(3), N2(3) ;
- double d1, d2 ;
-
- double r =1e10 ;
- iter =0 ;
- while (vpMath::equal(r_1,r,threshold_displacement) == false )
- {
-
- r_1 =r ;
- // compute current position
-
-
- //Change frame (current)
- vpHomogeneousMatrix c1Mc2, c2Mo ;
- vpRotationMatrix c1Rc2, c2Rc1 ;
- vpTranslationVector c1Tc2, c2Tc1 ;
- c1Mc2 = c2Mc1.inverse() ;
- c2Mc1.extract(c2Rc1) ;
- c2Mc1.extract(c2Tc1) ;
- c2Mc1.extract(c1Rc2) ;
- c1Mc2.extract(c1Tc2) ;
-
- c2Mo = c2Mc1*c1Mo ;
-
-
-
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (i=0 ; i < nbpoint ; i++)
- {
- getPlaneInfo(oN[i], c1Mo, N1, d1) ;
- getPlaneInfo(oN[i], c2Mo, N2, d2) ;
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
-
- vpMatrix H(3,3) ;
-
- Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
- Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
-
-
- // set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
- double Z1 ;
-
- Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
-
-
- H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
- H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
- H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
- H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
- H2 *=-1 ;
-
- vpMatrix c1CFc2(6,6) ;
- {
- vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
- for (unsigned int k_=0 ; k_ < 3 ; k_++)
- for (unsigned int l=0 ; l<3 ; l++)
- {
- c1CFc2[k_][l] = c1Rc2[k_][l] ;
- c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
- c1CFc2[k_][l+3] = sTR[k_][l] ;
- }
- }
- H2 = H2*c1CFc2 ;
-
- // Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
-
- x = Hp1[0] ;
- y = Hp1[1] ;
-
- Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
-
- H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
- H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
- H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
- H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
-
- // Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
-
- if (only_2==1)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- }
- else
- if (only_1==1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
- }
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stackMatrices(L,H2) ;
- e = vpMatrix::stackMatrices(e,e2) ;
- }
- L = vpMatrix::stackMatrices(L,H1) ;
- e = vpMatrix::stackMatrices(e,e1) ;
- }
-
-
- k++ ;
- }
-
- if (userobust)
- {
- robust.setIteration(0);
- for (unsigned int k_=0 ; k_ < n ; k_++)
- {
- res[k_] = vpMath::sqr(e[2*k_]) + vpMath::sqr(e[2*k_+1]) ;
- }
- robust.MEstimator(vpRobust::TUKEY, res, w);
-
-
- // compute the pseudo inverse of the interaction matrix
- for (unsigned int k_=0 ; k_ < n ; k_++)
- {
- W[2*k_][2*k_] = w[k_] ;
- W[2*k_+1][2*k_+1] = w[k_] ;
- }
- }
- else
- {
- for (unsigned int k_=0 ; k_ < 2*n ; k_++) W[k_][k_] = 1 ;
- }
- (W*L).pseudoInverse(Lp, 1e-16) ;
- // Compute the camera velocity
- vpColVector c2Tcc1 ;
-
- c2Tcc1 = -1*Lp*W*e ;
-
- // only for simulation
-
- c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
- // UpdatePose2(c2Tcc1, c2Mc1) ;
- r =(W*e).sumSquare() ;
-
-
-
- if (r < 1e-15) {break ; }
- if (iter>1000){break ; }
- if (r>r_1) { break ; }
- iter++ ;
- }
-
- return (W*e).sumSquare() ;
-
-}
-
-#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/src/computer-vision/pose-estimation/vpLevenbergMarquartd.cpp b/src/computer-vision/pose-estimation/vpLevenbergMarquartd.cpp
deleted file mode 100644
index 47c2479..0000000
--- a/src/computer-vision/pose-estimation/vpLevenbergMarquartd.cpp
+++ /dev/null
@@ -1,1667 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLevenbergMarquartd.cpp 5107 2015-01-05 07:47:13Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Levenberg Marquartd.
- *
- * Authors:
- * Eric Marchand
- * Francois Chaumette
- *
- *****************************************************************************/
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <iostream>
-#include <algorithm> // std::min
-
-#include <visp/vpLevenbergMarquartd.h>
-#include <visp/vpMath.h>
-
-#define SIGN(x) ((x) < 0 ? -1 : 1)
-#define SWAP(a,b,c) {(c) = (a); (a) = (b); (b) = (c);}
-#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
-#define TRUE 1
-#define FALSE 0
-
-/*
- * PROCEDURE : enorm
- *
- * ENTREE :
- *
- * x Vecteur de taille "n"
- * n Taille du vecteur "x"
- *
- * DESCRIPTION :
- * La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n"
- * La norme euclidienne est calculee par accumulation de la somme des carres
- * dans les trois directions. Les sommes des carres pour les petits et grands
- * elements sont mis a echelle afin d'eviter les overflows. Des underflows non
- * destructifs sont autorisee. Les underflows et overflows sont evites dans le
- * calcul des sommes des carres non encore mis a echelle par les elements
- * intermediaires. La definition des elements petit, intermediaire et grand
- * depend de deux constantes : rdwarf et rdiant. Les restrictions principales
- * sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est
- * pas en overflow. Les constantes donnees ici conviennent pour la plupart des
- * pc connus.
- *
- * RETOUR :
- * En cas de succes, la valeur retournee est la norme euclidienne du vecteur
- * Sinon, la valeur -1 est retournee et la variable globale "errno" est
- * initialisee pour indiquee le type de l'erreur.
- *
- */
-double enorm (const double *x, int n)
-{
- const double rdwarf = 3.834e-20;
- const double rgiant = 1.304e19;
-
- int i;
- double agiant, floatn;
- double norm_eucl = 0.0;
- double s1 = 0.0, s2 = 0.0, s3 = 0.0;
- double xabs;
- double x1max = 0.0 , x3max = 0.0;
-
- floatn = (double) n;
- agiant = rgiant / floatn;
-
- for (i = 0; i < n; i++)
- {
- xabs = std::fabs(x[i]);
- if ((xabs > rdwarf) && (xabs < agiant))
- {
- /*
- * somme pour elements intemediaires.
- */
- s2 += xabs * xabs;
- }
-
- else if (xabs <= rdwarf)
- {
- /*
- * somme pour elements petits.
- */
- if (xabs <= x3max)
- {
- //if (xabs != 0.0)
- if (xabs > std::numeric_limits<double>::epsilon())
- s3 += (xabs / x3max) * (xabs / x3max);
- }
- else
- {
- s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
- x3max = xabs;
- }
- }
-
- else
- {
- /*
- * somme pour elements grand.
- */
- if (xabs <= x1max)
- {
- s1 += (xabs / x1max) * (xabs / x1max);
- }
- else
- {
- s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
- x1max = xabs;
- }
- }
- }
-
- /*
- * calcul de la norme.
- */
- //if (s1 == 0.0)
- if (std::fabs(s1) <= std::numeric_limits<double>::epsilon())
- {
- //if (s2 == 0.0)
- if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
- norm_eucl = x3max * sqrt(s3);
- else if (s2 >= x3max)
- norm_eucl = sqrt (s2 * (1.0 + ( x3max / s2) * (x3max * s3)));
- else if (s2 < x3max)
- norm_eucl = sqrt (x3max * ((s2 / x3max) + (x3max * s3)));
- }
- else
- norm_eucl = x1max * sqrt (s1 + (s2 / x1max) / x1max);
-
- return (norm_eucl);
-}
-
-/* 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 triangulaire
- * superieure de "r".
- *
- * ldr Taille maximum de la matrice "r". "ldr" >= "n".
- *
- * 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".
- *
- * 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.
- *
- * par Estimee initiale du parametre de Levenberg-Marquardt.
- * wa1, wa2 Vecteurs de taille "n" de travail.
- *
- * 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 ,
- *
- * 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 ,
- * ou "par" est positif et abs(dxnorm-delta) <= 0.1 * delta.
- * Cette procedure complete la solution du probleme si on lui fourni les infos
- * nessaires de la factorisation qr, avec pivotage de colonnes de a.
- * Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
- * de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
- * avec les elements diagonaux classes par ordre decroissant de leur valeur, lmpar
- * attend une matrice triangulaire superieure complete, la matrice de permutation
- * "p" et les "n" premiers elements de (q transpose) * b. En sortie, la procedure
- * lmpar fournit aussi une matrice triangulaire superieure "s" telle que
- *
- * t t t
- * p * (a * a + par * d * d )* p = s * s .
- *
- * "s" est utilise a l'interieure de lmpar et peut etre d'un interet separe.
- *
- * Seulement quelques iterations sont necessaire pour la convergence de
- * 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 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".
- *
- * 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, tol001 = 0.001; /* tolerance a 0.1 et a 0.001 */
-
- int l, jm1;
- unsigned int jp1;
- unsigned int iter; /* compteur d'iteration */
- int nsing; /* nombre de singularite de la matrice */
- double dxnorm, fp, gnorm, parc;
- double parl, paru; /* borne inf et sup de par */
- double sum, temp;
- 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.
- */
- nsing = n;
-
- for (int i = 0; i < n; i++)
- {
- wa1[i] = qtb[i];
- double *pt = MIJ(r, i, i, ldr);
- //if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
- if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
- nsing = i - 1;
-
- if (nsing < n)
- wa1[i] = 0.0;
- }
-
- if (nsing >= 0)
- {
- for (int k = 0; k < nsing; k++)
- {
- int i = nsing - 1 - k;
- wa1[i] /= *MIJ(r, i, i, ldr);
- temp = wa1[i];
- jm1 = i - 1;
-
- if (jm1 >= 0)
- {
- for (unsigned int j = 0; j <= (unsigned int)jm1; j++)
- wa1[j] -= *MIJ(r, i, j, ldr) * temp;
- }
- }
- }
-
- for (int j = 0; j < n; j++)
- {
- l = ipvt[j];
- x[l] = wa1[j];
- }
-
- /*
- * initialisation du compteur d'iteration.
- * evaluation de la fonction a l'origine, et test
- * d'acceptation de la direction de Gauss-Newton.
- */
- iter = 0;
-
- for (int i = 0; i < n; i++)
- wa2[i] = diag[i] * x[i];
-
- dxnorm = enorm(wa2, n);
-
- fp = dxnorm - *delta;
-
- 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.
- */
- parl = 0.0;
-
- if (nsing >= n)
- {
- for (int i = 0; i < n; i++)
- {
- l = ipvt[i];
- wa1[i] = diag[l] * (wa2[l] / dxnorm);
- }
-
- for (int i = 0; i < n; i++)
- {
- long im1;
- sum = 0.0;
- im1 = (i - 1L);
-
- if (im1 >= 0)
- {
- for (unsigned int j = 0; j <= (unsigned int)im1; j++)
- sum += (*MIJ(r, i, j, ldr) * wa1[j]);
- }
- wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
- }
-
- temp = enorm(wa1, n);
- parl = ((fp / *delta) / temp) / temp;
- }
-
- /*
- * calcul d'une limite superieure, paru, pour le zero de la
- * fonction.
- */
- for (int i = 0; i < n; i++)
- {
- sum = 0.0;
-
- for (int j = 0; j <= i; j++)
- sum += *MIJ(r, i, j, ldr) * qtb[j];
-
- l = ipvt[i];
- wa1[i] = sum / diag[l];
- }
-
- gnorm = enorm(wa1, n);
- paru = gnorm / *delta;
-
- //if (paru == 0.0)
- if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
- paru = dwarf / vpMath::minimum(*delta, tol1);
-
- /*
- * Si "par" en entree tombe hors de l'intervalle (parl,paru),
- * on le prend proche du point final.
- */
-
- *par = vpMath::maximum(*par, parl);
- *par = vpMath::maximum(*par, paru);
-
- //if (*par == 0.0)
- if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() )
- *par = gnorm / dxnorm;
-
- /*
- * debut d'une iteration.
- */
- for(;;) //iter >= 0)
- {
- iter++;
-
- /*
- * evaluation de la fonction a la valeur courant
- * de "par".
- */
- //if (*par == 0.0)
- if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() )
- *par = vpMath::maximum(dwarf, (tol001 * paru));
-
- temp = sqrt(*par);
-
- for (int i = 0; i < n; i++)
- wa1[i] = temp * diag[i];
-
- qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
-
- for (int i = 0; i < n; i++)
- wa2[i] = diag[i] * x[i];
-
- dxnorm = enorm(wa2, n);
- temp = fp;
- 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.
- */
- //if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <= temp)
- // && (temp < 0.0)) || (iter == 10))
- if ((std::fabs(fp) <= tol1 * (*delta)) || ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp)
- && (temp < 0.0)) || (iter == 10))
- {
- // terminaison.
-
- // Remove the two next lines since this is a dead code
- /* if (iter == 0)
- *par = 0.0; */
-
- return (0);
- }
-
- /*
- * calcul de la correction de Newton.
- */
-
- for (int i = 0; i < n; i++)
- {
- l = ipvt[i];
- wa1[i] = diag[l] * (wa2[l] / dxnorm);
- }
-
- for (unsigned int i = 0; i < (unsigned int)n; i++)
- {
- wa1[i] = wa1[i] / sdiag[i];
- temp = wa1[i];
- jp1 = i + 1;
- if ( (unsigned int) n >= jp1)
- {
- for (unsigned int j = jp1; j < (unsigned int)n; j++)
- wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
- }
- }
-
- temp = enorm(wa1, n);
- parc = ((fp / *delta) / temp) / temp;
-
- /*
- * selon le signe de la fonction, mise a jour
- * de parl ou paru.
- */
- if (fp > 0.0)
- parl = vpMath::maximum(parl, *par);
-
- if (fp < 0.0)
- paru = vpMath::minimum(paru, *par);
-
- /*
- * calcul d'une estimee ameliree de "par".
- */
- *par = vpMath::maximum(parl, (*par + parc));
- }/* fin boucle sur iter */
- }/* fin fp > tol1 * delta */
-
- /*
- * terminaison.
- */
- if (iter == 0)
- *par = 0.0;
-
- return (0);
-}
-
-/*
- * PROCEDURE : pythag
- *
- * ENTREES :
- * a, b Variables dont on veut la racine carre de leur somme de carre
- *
- * DESCRIPTION :
- * La procedure calcule la racine carre de la somme des carres de deux nombres
- * en evitant l'overflow ou l'underflow destructif.
- *
- * RETOUR :
- * La procedure retourne la racine carre de a^2 + b^2.
- *
- */
-double pythag (double a, double b)
-{
- double pyth, p, r, s, t, u;
-
- p = vpMath::maximum(std::fabs(a), std::fabs(b));
-
- //if (p == 0.0)
- if (std::fabs(p) <= std::numeric_limits<double>::epsilon() )
- {
- pyth = p;
- return (pyth);
- }
-
- r = (std::min(std::fabs(a), std::fabs(b)) / p) * (std::min(std::fabs(a), std::fabs(b)) / p);
- t = 4.0 + r;
-
- //while (t != 4.0)
- while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t,4.0)) * std::numeric_limits<double>::epsilon() )
- {
- s = r / t;
- u = 1.0 + 2.0 * s;
- p *= u;
- r *= (s / u) * (s / u);
- t = 4.0 + r;
- }
-
- pyth = p;
- return (pyth);
-}
-
-/*
- * 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 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 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.
- *
- * 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 taille "m" x "n".
- * La procedure determine une matrice orthogonale "q", une matrice de permutation
- * "p" et une matrice trapesoidale superieure "r" dont les elements diagonaux
- * sont ordonnes dans l'ordre decroissant de leurs 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
- *
- * 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 "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.
- *
- */
-int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
- int /* lipvt */, double *rdiag, double *acnorm, double *wa)
-{
- const double tolerance = 0.05;
-
- int i, j, ip1, k, kmax, minmn;
- double ajnorm, epsmch;
- double sum, temp, tmp;
-
- /*
- * epsmch est la precision machine.
- */
- epsmch = std::numeric_limits<double>::epsilon();
-
- /*
- * 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);
- rdiag[i] = acnorm[i];
- wa[i] = rdiag[i];
-
- if (pivot)
- ipvt[i] = i;
- }
- /*
- * reduction de "a" en "r" avec les tranformations de Householder.
- */
- minmn = vpMath::minimum(m, n);
- for (i = 0; i < minmn; i++)
- {
- if (pivot)
- {
- /*
- * met la ligne de plus grande norme en position
- * de pivot.
- */
- kmax = i;
- for (k = i; k < m; k++)
- {
- if (rdiag[k] > rdiag[kmax])
- kmax = k;
- }
-
- if (kmax != i)
- {
- for (j = 0; j < n; j++)
- SWAP(*MIJ(a, i, j, lda),
- *MIJ(a, kmax, j, lda), tmp);
-
- rdiag[kmax] = rdiag[i];
- wa[kmax] = wa[i];
-
- SWAP( ipvt[i], ipvt[kmax], k);
- }
- }
-
- /*
- * calcul de al transformationde Householder afin de reduire
- * la jeme ligne de "a" a un multiple du jeme vecteur unite.
- */
- ajnorm = enorm(MIJ(a, i, i, lda), n - i);
-
- //if (ajnorm != 0.0)
- if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon() )
- {
- if (*MIJ(a, i, i, lda) < 0.0)
- ajnorm = -ajnorm;
-
- for (j = i; j < n; j++)
- *MIJ(a, i, j, lda) /= ajnorm;
- *MIJ(a, i, i, lda) += 1.0;
-
- /*
- * application de la tranformation aux lignes
- * restantes et mise a jour des normes.
- */
- ip1 = i + 1;
-
- if (m >= ip1)
- {
- for (k = ip1; k < m; k++)
- {
- sum = 0.0;
- for (j = i; j < n; j++)
- sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
-
- temp = sum / *MIJ(a, i, i, lda);
-
- for (j = i; j < n; j++)
- *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
-
- //if (pivot && rdiag[k] != 0.0)
- if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon()) )
- {
- temp = *MIJ (a, k, i, lda) / rdiag[k];
- rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
-
- if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch)
- {
- rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n -1 - (int) i));
- wa[k] = rdiag[k];
- }
- }
- }/* fin boucle for k */
-
- }
-
- } /* fin if (ajnorm) */
-
- rdiag[i] = -ajnorm;
- } /* fin for (i = 0; i < minmn; i++) */
- return (0);
-}
-
-/* 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 superieure
- * complete de "r".
- * 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".
- *
- * 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 ,
- *
- * Au sens des moindres carres.
- *
- * Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de "q"
- * sont orthogonales et "r" est une matrice traingulaire superieure dont les
- * elements diagonaux sont classes de l'ordre decroissant de leur valeur. Cette
- * procedure attend donc la matrice triangulaire superieure remplie "r", 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
- *
- * t t
- * r * z = q * b , p * d * p * z = 0 ,
- *
- * Ou x = p * z. Si ce systeme ne possede pas de rangee pleine, alors une
- * solution au moindre carre est obtenue. En sortie, la procedure fournit aussi
- * une matrice triangulaire superieure "s" tel que
- *
- * t t t
- * p * (a * a + d * d) * p = s * s .
- *
- * "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 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" 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, jp1, k, kp1, l; /* compteur de boucle */
- int nsing;
- double cosi, cotg, qtbpj, sinu, sum, 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".
- */
- for (i = 0; i < n; i++)
- {
- for (j = i; j < n; j++)
- *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
-
- x[i] = *MIJ(r, i, i, ldr);
- wa[i] = qtb[i];
- }
-
- /*
- * 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.
- */
- l = ipvt[i];
-
- //if (diag[l] != 0.0)
- if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon())
- {
- for (k = i; k < n; k++)
- sdiag[k] = 0.0;
-
- 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.
- */
-
- qtbpj = 0.0;
-
- for (k = i; k < n; k++)
- {
- /*
- * determination d'une rotation qui elimine
- * les elements appropriees dans la colonne
- * courante de d.
- */
-
- //if (sdiag[k] != 0.0)
- if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon())
- {
- if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k]))
- {
- tg = sdiag[k] / *MIJ(r, k, k, ldr);
- cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
- sinu = cosi * tg;
- }
- else
- {
- cotg = *MIJ(r, k, k, ldr) / sdiag[k];
- sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
- cosi = sinu * cotg;
- }
-
- /*
- * 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;
- qtbpj = -sinu * wa[k] + cosi * qtbpj;
- wa[k] = temp;
-
- /*
- * accumulation des tranformations dans
- * les lignes de s.
- */
-
- kp1 = k + 1;
-
- if ( n >= kp1)
- {
- for (j = kp1; j < n; j++)
- {
- temp = cosi * *MIJ(r, k, j, ldr) +
- sinu * sdiag[j];
- sdiag[j] = - sinu * *MIJ(r, k, j, ldr) +
- cosi * sdiag[j];
- *MIJ(r, k, j, ldr) = temp;
- }
- }
- }/* fin if diag[] !=0 */
- } /* fin boucle for k -> n */
- }/* fin if diag =0 */
-
- /*
- * 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 */
-
- /*
- * resolution du systeme triangulaire pour z. Si le systeme est
- * singulier, on obtient une solution au moindres carres.
- */
- nsing = n;
-
- for (i = 0; i < n; i++)
- {
- //if (sdiag[i] == 0.0 && nsing == n)
- if ( (std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
- nsing = i - 1;
-
- if (nsing < n)
- wa[i] = 0.0;
- }
-
- if (nsing >= 0)
- {
- for (k = 0; k < nsing; k++)
- {
- i = nsing - 1 - k;
- sum = 0.0;
- jp1 = i + 1;
-
- if (nsing >= jp1)
- {
- for (j = jp1; j < nsing; j++)
- sum += *MIJ(r, i, j, ldr) * wa[j];
- }
- wa[i] = (wa[i] - sum) / sdiag[i];
- }
- }
- /*
- * permutation arriere des composants de z et des componants de x.
- */
-
- for (j = 0; j < n; j++)
- {
- l = ipvt[j];
- x[l] = wa[j];
-
- }
- return (0);
-}
-
-/*
- * 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 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 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 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 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 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. ou a "factor" lui meme. Dans la plupart des cas,
- * "factor" doit se trouve dans l'intervalle (1, 100); ou 100 est
- * la valeur recommandee.
- * nprint Controle de l'impression des iterees (si valeur positive).
- * Dans ce cas, fcn est appelle avec iflag = 0 au debut de la
- * premiere iteration et 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 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 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 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 :
- * 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
- * etre appele comme suit :
- * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
- *
- * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
- * 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.
- *
- * RETOUR :
- * En cas de succes, la valeur zero est retournee.
- * Sinon la valeur -1 est retournee.
- */
-int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
- double *jac, int ldfjac, int iflag), int m, int n, double *x,
- double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
- double gtol, unsigned int maxfev, double *diag, int mode,
- const double factor, int nprint, int *info, unsigned int *nfev,
- int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
- double *wa3, double *wa4)
-{
- const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
- int oncol = TRUE;
- int iflag, iter;
- int count = 0;
- int i, j, l;
- double actred, delta, dirder, epsmch, fnorm, fnorm1, gnorm;
- double ratio = std::numeric_limits<double>::epsilon();
- double par, pnorm, prered;
- double sum, temp, temp1, temp2, xnorm = 0.0;
-
- /* epsmch est la precision machine. */
- epsmch = std::numeric_limits<double>::epsilon();
-
- *info = 0;
- iflag = 0;
- *nfev = 0;
- *njev = 0;
-
- /* verification des parametres d'entree. */
-
- /*if (n <= 0)
- return 0;*/
- if (m < n)
- return 0;
- if (ldfjac < m)
- return 0;
- if (ftol < 0.0)
- return 0;
- if (xtol < 0.0)
- return 0;
- if (gtol < 0.0)
- return 0;
- if (maxfev == 0)
- return 0;
- if (factor <= 0.0)
- return 0;
- if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0)
- || (gtol < 0.0) || (maxfev == 0) || (factor <= 0.0))
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
-
- if (mode == 2)
- {
- for (j = 0; j < n; j++)
- {
- if (diag[j] <= 0.0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
- }
- }
-
- /*
- * evaluation de la fonction au point de depart
- * et calcul de sa norme.
- */
- iflag = 1;
-
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
-
- *nfev = 1;
-
- if (iflag < 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
-
- fnorm = enorm(fvec, m);
-
- /*
- * initialisation du parametre de Levenberg-Marquardt
- * et du conteur d'iteration.
- */
-
- par = 0.0;
- iter = 1;
-
- /*
- * debut de la boucle la plus externe.
- */
- while (count < (int)maxfev)
- {
- count++;
- /*
- * calcul de la matrice jacobienne.
- */
-
- iflag = 2;
-
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- (*njev) ++;
-
- if (iflag < 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
-
- /*
- * si demandee, appel de fcn pour impression des iterees.
- */
- if (nprint > 0)
- {
- iflag = 0;
- if ((iter-1) % nprint == 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- if (iflag < 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
- }
-
- /*
- * calcul de la factorisation qr du jacobien.
- */
- 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.
- */
-
- if (iter == 1)
- {
- if (mode != 2)
- {
- for (j = 0; j < n; j++)
- {
- diag[j] = wa2[j];
- //if (wa2[j] == 0.0)
- if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
- diag[j] = 1.0;
- }
- }
-
- /*
- * 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++)
- wa3[j] = diag[j] * x[j];
-
- xnorm = enorm (wa3, n);
- delta = factor * xnorm;
-
- //if (delta == 0.0)
- if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
- delta = factor;
- }
-
- /*
- * formation de (q transpose) * fvec et stockage des n premiers
- * composants dans qtf.
- */
- for (i = 0; i < m; i++)
- wa4[i] = fvec[i];
-
- for (i = 0; i < n; i++)
- {
- double *pt = MIJ(fjac, i, i, ldfjac);
- //if (*MIJ(fjac, i, i, ldfjac) != 0.0)
- if (std::fabs(*pt) > std::numeric_limits<double>::epsilon() )
- {
- sum = 0.0;
-
- for (j = i; j < m; j++)
- sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
-
- temp = - sum / *MIJ(fjac, i, i, ldfjac);
-
- for (j = i; j < m; j++)
- wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
-
- }
-
- *MIJ(fjac, i, i, ldfjac) = wa1[i];
- qtf[i] = wa4[i];
- }
-
- /*
- * calcul de la norme du gradient mis a l'echelle.
- */
-
- gnorm = 0.0;
-
- //if (fnorm != 0.0)
- if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon())
- {
- for (i = 0; i < n; i++)
- {
- l = ipvt[i];
- //if (wa2[l] != 0.0)
- if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon())
- {
- sum = 0.0;
- for (j = 0; j <= i; j++)
- sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
-
- gnorm = vpMath::maximum(gnorm, std::fabs(sum / wa2[l]));
- }
- }
- }
-
- /*
- * test pour la convergence de la norme du gradient .
- */
-
- if (gnorm <= gtol)
- *info = 4;
-
- if (*info != 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
-
- /*
- * remise a l'echelle si necessaire.
- */
-
- if (mode != 2)
- {
- for (j = 0; j < n; j++)
- diag[j] = vpMath::maximum(diag[j], wa2[j]);
- }
-
- /*
- * debut de la boucle la plus interne.
- */
- ratio = 0.0;
- while (ratio < tol0001)
- {
-
- /*
- * determination du parametre de Levenberg-Marquardt.
- */
- lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1,
- wa2, wa3, wa4);
-
- /*
- * stockage de la direction p et x + p. calcul de la norme de p.
- */
-
- for (j = 0; j < n; j++)
- {
- wa1[j] = - wa1[j];
- wa2[j] = x[j] + wa1[j];
- wa3[j] = diag[j] * wa1[j];
- }
-
- pnorm = enorm(wa3, n);
-
- /*
- * a la premiere iteration, ajustement de la premiere limite de
- * l'etape.
- */
-
- if (iter == 1)
- delta = vpMath::minimum(delta, pnorm);
-
- /*
- * evaluation de la fonction en x + p et calcul de leur norme.
- */
-
- iflag = 1;
- (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
-
- (*nfev)++;
-
- if (iflag < 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
-
- fnorm1 = enorm(wa4, m);
-
- /*
- * calcul de la reduction reelle mise a l'echelle.
- */
-
- actred = - 1.0;
-
- if ((tol1 * fnorm1) < fnorm)
- actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
-
- /*
- * calcul de la reduction predite mise a l'echelle et
- * de la derivee directionnelle mise a l'echelle.
- */
-
- for (i = 0; i < n; i++)
- {
- wa3[i] = 0.0;
- l = ipvt[i];
- temp = wa1[l];
- for (j = 0; j <= i; j++)
- wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
- }
-
- temp1 = enorm(wa3, n) / fnorm;
- temp2 = (sqrt(par) * pnorm) / fnorm;
- prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
- dirder = - ((temp1 * temp1) + (temp2 * temp2));
-
- /*
- * calcul du rapport entre la reduction reel et predit.
- */
-
- ratio = 0.0;
-
- //if (prered != 0.0)
- if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
- ratio = actred / prered;
-
- /*
- * mise a jour de la limite de l'etape.
- */
-
- if (ratio > tol25)
- {
- //if ((par == 0.0) || (ratio <= tol75))
- if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75))
- {
- delta = pnorm / tol5;
- par *= tol5;
- }
- }
- else
- {
- if (actred >= 0.0)
- temp = tol5;
-
- else
- temp = tol5 * dirder / (dirder + tol5 * actred);
-
- if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
- temp = tol1;
-
- delta = temp * vpMath::minimum(delta, (pnorm / tol1));
- par /= temp;
- }
-
- /*
- * test pour une iteration reussie.
- */
- if (ratio >= tol0001)
- {
- /*
- * iteration reussie. mise a jour de x, de fvec, et de
- * leurs normes.
- */
-
- for (j = 0; j < n; j++)
- {
- x[j] = wa2[j];
- wa2[j] = diag[j] * x[j];
- }
-
- for (i = 0; i < m; i++)
- fvec[i] = wa4[i];
-
- xnorm = enorm(wa2, n);
- fnorm = fnorm1;
- iter++;
- }
-
- /*
- * tests pour convergence.
- */
-
- if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
- *info = 1;
-
- if (delta <= xtol * xnorm)
- *info = 2;
-
- if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)
- && *info == 2)
- *info = 3;
-
- if (*info != 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m,n,x,fvec,fjac,ldfjac, iflag);
-
- return (count);
- }
- /*
- * tests pour termination et
- * verification des tolerances.
- */
-
- if (*nfev >= maxfev)
- *info = 5;
-
- if ((std::fabs(actred) <= epsmch) && (prered <= epsmch)
- && (tol5 * ratio <= 1.0))
- *info = 6;
-
- if (delta <= epsmch * xnorm)
- *info = 7;
-
- if (gnorm <= epsmch)
- *info = 8;
-
- if (*info != 0)
- {
- /*
- * termination, normal ou imposee par l'utilisateur.
- */
- if (iflag < 0)
- *info = iflag;
-
- iflag = 0;
-
- if (nprint > 0)
- (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
- return (count);
- }
- }/* fin while ratio >=tol0001 */
- }/*fin while 1*/
-
- return 0 ;
-}
-
-
-
-/*
- * 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 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 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 identite.
- *
- * 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.
- * Cette procedure appele la procedure generale au moindre carre lmder.
- *
- * 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
- * etre appele comme suit :
- * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
- *
- * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
- * 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.
- *
- * RETOUR :
- * En cas de succes, la valeur zero est retournee.
- * Sinon, la valeur -1.
- *
- */
-int lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
- double *jac, int ldfjac, int iflag),
- int m, int n, double *x, double *fvec, double *fjac,
- int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa)
-{
- const double factor = 100.0;
- unsigned int maxfev, nfev;
- int mode, njev, nprint;
- double ftol, gtol, xtol;
-
- *info = 0;
-
- /* verification des parametres en entree qui causent des erreurs */
-
-
- if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) ||
- (lwa < (5 * n + m)) )
- {
- printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0) , (lwa < (5 * n + m))) ;
- return (-1);
- }
-
- /* appel a lmder */
-
- maxfev = (unsigned int)(100 * (n + 1));
- ftol = tol;
- xtol = tol;
- gtol = 0.0;
- mode = 1;
- nprint = 0;
-
- lmder (ptr_fcn , m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa,
- mode, factor, nprint, info, &nfev, &njev, ipvt, &wa[n], &wa[2 * n],
- &wa[3 * n], &wa[4 * n], &wa[5 * n]);
-
- if (*info == 8)
- *info = 4;
-
- return (0);
-}
-
-#undef TRUE
-#undef FALSE
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/computer-vision/pose-estimation/vpLevenbergMarquartd.h b/src/computer-vision/pose-estimation/vpLevenbergMarquartd.h
deleted file mode 100644
index a746f59..0000000
--- a/src/computer-vision/pose-estimation/vpLevenbergMarquartd.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLevenbergMarquartd.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Levenberg Marquartd.
- *
- * Authors:
- * Eric Marchand
- * Francois Chaumette
- *
- *****************************************************************************/
-
-
-#ifndef vpLevenbergMarquartd_h
-#define vpLevenbergMarquartd_h
-
-#include <visp/vpConfig.h>
-#include <visp/vpMath.h>
-
-#include <stdio.h>
-#include <errno.h>
-#include <math.h>
-#include <stdlib.h>
-#include <float.h>
-
-int VISP_EXPORT
-qrsolv (int n, double *r, int ldr, int *ipvt, double *diag,
- double *qtb, double *x, double *sdiag, double *wa) ;
-
-double VISP_EXPORT
-enorm (const double *x, int n);
-
-int VISP_EXPORT
-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);
-
-double VISP_EXPORT
-pythag (double a, double b);
-
-int VISP_EXPORT
-qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
- int lipvt, double *rdiag, double *acnorm, double *wa);
-
-int VISP_EXPORT
-qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
- double *x, double *sdiag, double *wa);
-
-int VISP_EXPORT
-lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
- double *jac, int ldfjac, int iflag),
- int m, int n, double *x,
- double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
- double gtol, unsigned int maxfev, double *diag, int mode,
- const double factor, int nprint, int *info, unsigned int *nfev,
- int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
- double *wa3, double *wa4);
-
-int VISP_EXPORT
-lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
- double *jac, int ldfjac, int iflag),
- int m, int n, double *x, double *fvec, double *fjac,
- int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa);
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/computer-vision/pose-estimation/vpPose.cpp b/src/computer-vision/pose-estimation/vpPose.cpp
deleted file mode 100644
index ab65258..0000000
--- a/src/computer-vision/pose-estimation/vpPose.cpp
+++ /dev/null
@@ -1,751 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpPose.cpp 5212 2015-01-26 17:45:45Z strinh $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Pose computation.
-*
-* Authors:
-* Eric Marchand
-* Francois Chaumette
-*
-*****************************************************************************/
-
-
-/*!
-\file vpPose.cpp
-\brief Fichier contenant la classe vpPose (tout ce qui est necessaire
-pour faire du calcul de pose par difference methode
-*/
-
-#include <visp/vpPose.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpPoseException.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpMath.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#define DEBUG_LEVEL1 0
-/*!
-\brief basic initialisation (called by the constructors)
-*/
-void
-vpPose::init()
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::Init() " << std::endl ;
-#endif
- npt = 0 ;
- listP.clear() ;
- c3d.clear();
-
- lambda = 0.25 ;
-
- vvsIterMax = 200 ;
-
- distanceToPlaneForCoplanarityTest = 0.001 ;
-
- computeCovariance = false;
-
- ransacMaxTrials = 1000;
- ransacThreshold = 0.0001;
- ransacNbInlierConsensus = 4;
-
- residual = 0;
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::Init() " << std::endl ;
-#endif
-
-}
-
-/*! Defaukt constructor. */
-vpPose::vpPose()
- : npt(0), listP(), residual(0), lambda(0.25), vvsIterMax(200), c3d(),
- computeCovariance(false), covarianceMatrix(),
- ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacThreshold(0.0001),
- distanceToPlaneForCoplanarityTest(0.001)
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::vpPose() " << std::endl ;
-#endif
-
- init() ;
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::vpPose() " << std::endl ;
-#endif
-
-}
-
-/*!
- Destructor that deletes the array of point (freed the memory).
-*/
-vpPose::~vpPose()
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::~vpPose() " << std::endl ;
-#endif
-
- listP.clear() ;
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::~vpPose() " << std::endl ;
-#endif
-}
-/*!
- Delete the array of point
-*/
-void
-vpPose::clearPoint()
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::ClearPoint() " << std::endl ;
-#endif
-
- listP.clear() ;
- npt = 0 ;
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::ClearPoint() " << std::endl ;
-#endif
-}
-
-/*!
-\brief Add a new point in the array of point.
-\param newP : New point to add in the array of point.
-\warning Considering a point from the class vpPoint, X, Y, and Z will
-represent the 3D information and x and y its 2D information.
-These 5 fields must be initialized to be used within this library
-*/
-void
-vpPose::addPoint(const vpPoint& newP)
-{
-
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::AddPoint(Dot) " << std::endl ;
-#endif
-
- listP.push_back(newP);
- npt ++ ;
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::AddPoint(Dot) " << std::endl ;
-#endif
-}
-
-
-void
-vpPose::setDistanceToPlaneForCoplanarityTest(double d)
-{
- distanceToPlaneForCoplanarityTest = d ;
-}
-
-/*!
-\brief test the coplanarity of the set of points
-
-\param coplanar_plane_type:
- 1: if plane x=cst
- 2: if plane y=cst
- 3: if plane z=cst
- 4: if the points are collinear.
- 0: any other plane
-\return true if points are coplanar
-false if not
-*/
-bool
-vpPose::coplanar(int &coplanar_plane_type)
-{
- coplanar_plane_type = 0;
- if (npt <2)
- {
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
-
- if (npt ==3) return true ;
-
- double x1=0,x2=0,x3=0,y1=0,y2=0,y3=0,z1=0,z2=0,z3=0 ;
-
- std::list<vpPoint>::const_iterator it = listP.begin();
-
- vpPoint P1, P2, P3 ;
-
- // Get three 3D points that are not collinear and that is not at origin
- bool degenerate = true;
- bool not_on_origin = true;
- std::list<vpPoint>::const_iterator it_tmp;
-
- std::list<vpPoint>::const_iterator it_i, it_j, it_k;
- for (it_i=listP.begin(); it_i != listP.end(); ++it_i) {
- if (degenerate == false) {
- //std::cout << "Found a non degenerate configuration" << std::endl;
- break;
- }
- P1 = *it_i;
- // Test if point is on origin
- if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
- not_on_origin = false;
- }
- else {
- not_on_origin = true;
- }
- if (not_on_origin) {
- it_tmp = it_i; it_tmp ++; // j = i+1
- for (it_j=it_tmp; it_j != listP.end(); ++it_j) {
- if (degenerate == false) {
- //std::cout << "Found a non degenerate configuration" << std::endl;
- break;
- }
- P2 = *it_j;
- if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
- not_on_origin = false;
- }
- else {
- not_on_origin = true;
- }
- if (not_on_origin) {
- it_tmp = it_j; it_tmp ++; // k = j+1
- for (it_k=it_tmp; it_k != listP.end(); ++it_k) {
- P3 = *it_k;
- if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
- not_on_origin = false;
- }
- else {
- not_on_origin = true;
- }
- if (not_on_origin) {
- x1 = P1.get_oX() ;
- x2 = P2.get_oX() ;
- x3 = P3.get_oX() ;
-
- y1 = P1.get_oY() ;
- y2 = P2.get_oY() ;
- y3 = P3.get_oY() ;
-
- z1 = P1.get_oZ() ;
- z2 = P2.get_oZ() ;
- z3 = P3.get_oZ() ;
-
- vpColVector a_b(3), b_c(3), cross_prod;
- a_b[0] = x1-x2; a_b[1] = y1-y2; a_b[2] = z1-z2;
- b_c[0] = x2-x3; b_c[1] = y2-y3; b_c[2] = z2-z3;
-
- cross_prod = vpColVector::crossProd(a_b, b_c);
- if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
- degenerate = true; // points are collinear
- else
- degenerate = false;
- }
- if (degenerate == false)
- break;
- }
- }
- }
- }
- }
-
- if (degenerate) {
- coplanar_plane_type = 4; // points are collinear
- return true;
- }
-
- double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
- double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
- double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
- double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
-
- //std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d << std::endl;
- if (std::fabs(b) <= std::numeric_limits<double>::epsilon()
- && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
- coplanar_plane_type = 1; // ax=d
- } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
- && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
- coplanar_plane_type = 2; // by=d
- } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
- && std::fabs(b) <= std::numeric_limits<double>::epsilon() ) {
- coplanar_plane_type = 3; // cz=d
- }
-
- double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
-
- double dist;
-
- for(it=listP.begin(); it != listP.end(); ++it)
- {
- P1 = *it ;
- dist = (a*P1.get_oX() + b*P1.get_oY()+c*P1.get_oZ()+d)/D ;
- //std::cout << "dist= " << dist << std::endl;
-
- if (fabs(dist) > distanceToPlaneForCoplanarityTest)
- {
- vpDEBUG_TRACE(10," points are not coplanar ") ;
- // TRACE(" points are not coplanar ") ;
- return false ;
- }
- }
-
- vpDEBUG_TRACE(10," points are coplanar ") ;
- // vpTRACE(" points are coplanar ") ;
-
- return true ;
-}
-
-/*!
-\brief Compute and return the residual expressed in meter for
-the pose matrix 'cMo'.
-
-\param cMo : Input pose. The matrix that defines the pose to be tested.
-
-\return The value of he residual in meter.
-
-*/
-double
-vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const
-{
- double residual_ = 0 ;
- vpPoint P ;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- double x = P.get_x() ;
- double y = P.get_y() ;
-
- P.track(cMo) ;
-
- residual_ += vpMath::sqr(x-P.get_x()) + vpMath::sqr(y-P.get_y()) ;
- }
- return residual_ ;
-}
-
-
-
-/*!
-\brief Compute the pose according to the desired method
-
-the different method are
-
-LAGRANGE Lagrange approach
-(test is done to switch between planar and
-non planar algorithm)
-
-DEMENTHON Dementhon approach
-(test is done to switch between planar and
-non planar algorithm)
-
-VIRTUAL_VS Virtual visual servoing approach
-
-DEMENTHON_VIRTUAL_VS Virtual visual servoing approach initialized using
-Dementhon approach
-
-LAGRANGE_VIRTUAL_VS Virtual visual servoing initialized using
-Lagrange approach
-
-*/
-bool
-vpPose::computePose(vpPoseMethodType methode, vpHomogeneousMatrix& cMo, bool (*func)(vpHomogeneousMatrix *))
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::ComputePose()" << std::endl ;
-#endif
-
- if (npt <4)
- {
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "No enough point ")) ;
- }
-
- switch (methode)
- {
- case DEMENTHON :
- case DEMENTHON_VIRTUAL_VS :
- case DEMENTHON_LOWE :
- {
-
- if (npt <4)
- {
- vpERROR_TRACE("Dementhon method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
-
- // test si les point 3D sont coplanaires
- int coplanar_plane_type = 0;
- bool plan = coplanar(coplanar_plane_type) ;
- if (plan == true)
- {
- //std::cout << "Plan" << std::endl;
- try{
- poseDementhonPlan(cMo);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- //std::cout << "Fin Plan" << std::endl;
- }
- else
- {
- //std::cout << "No Plan" << std::endl;
- try{
- poseDementhonNonPlan(cMo) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- //std::cout << "Fin No Plan" << std::endl;
- }
- }
- break ;
- case LAGRANGE :
- case LAGRANGE_VIRTUAL_VS :
- case LAGRANGE_LOWE :
- {
- // test si les point 3D sont coplanaires
- int coplanar_plane_type;
- bool plan = coplanar(coplanar_plane_type) ;
-
- if (plan == true && coplanar_plane_type > 0) // only plane oX=d, oY=d or oZ=d
- {
-
- if (coplanar_plane_type == 4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(points are collinear)") ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Points are collinear ")) ;
- }
- if (npt <4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- try {
- poseLagrangePlan(cMo, coplanar_plane_type);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- }
- else
- {
- if (npt <4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- try {
- poseLagrangeNonPlan(cMo);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- }
- }
- break;
- case RANSAC:
- if (npt <4)
- {
- vpERROR_TRACE("Ransac method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- try {
- return poseRansac(cMo, func);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- break;
- case LOWE :
- case VIRTUAL_VS:
- break ;
- }
-
- switch (methode)
- {
- case LAGRANGE :
- case DEMENTHON :
- case RANSAC :
- break ;
- case VIRTUAL_VS:
- case LAGRANGE_VIRTUAL_VS:
- case DEMENTHON_VIRTUAL_VS:
- {
- try
- {
- poseVirtualVS(cMo);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- }
- break ;
- case LOWE:
- case LAGRANGE_LOWE:
- case DEMENTHON_LOWE:
- {
- try
- {
- poseLowe(cMo);
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- }
- break ;
- }
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::ComputePose()" << std::endl ;
-#endif
-
- //If here, there was no exception thrown so return true
- return true;
-}
-
-
-
-void
-vpPose::printPoint()
-{
- vpPoint P;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
-
- std::cout << "3D oP " << P.oP.t() ;
- std::cout << "3D cP " << P.cP.t() ;
- std::cout << "2D " << P.p.t() ;
- }
-}
-
-
-void
-vpPose::display(vpImage<unsigned char> &I,
- vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam,
- double size,
- vpColor col)
-{
- vpDisplay::displayFrame(I, cMo, cam, size, col);
-}
-
-
-void
-vpPose::display(vpImage<vpRGBa> &I,
- vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam,
- double size,
- vpColor col)
-{
- vpDisplay::displayFrame(I,cMo,cam, size,col);
-}
-
-/*
-\brief display the 3D model in image I
-\warning the 2D coordinate of the point have to be initialized
-(lispP has to be modified)
-*/
-void
-vpPose::displayModel(vpImage<unsigned char> &I,
- vpCameraParameters &cam,
- vpColor col)
-{
- vpPoint P ;
- vpImagePoint ip;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
- vpDisplay::displayCross(I, ip, 5, col) ;
- // std::cout << "3D oP " << P.oP.t() ;
- // std::cout << "3D cP " << P.cP.t() ;
- // std::cout << "2D " << P.p.t() ;
- }
-}
-
-/*
-\brief display the 3D model in image I
-\warning the 2D coordinate of the point have to be initialized
-(lispP has to be modified)
-*/
-void
-vpPose::displayModel(vpImage<vpRGBa> &I,
- vpCameraParameters &cam,
- vpColor col)
-{
- vpPoint P ;
- vpImagePoint ip;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
- vpDisplay::displayCross(I, ip, 5, col) ;
- // std::cout << "3D oP " << P.oP.t() ;
- // std::cout << "3D cP " << P.cP.t() ;
- // std::cout << "2D " << P.p.t() ;
- }
-}
-
-
-/*!
-\brief Carries out the camera pose the image of a rectangle and
-the intrinsec parameters, the length on x axis is known but the
-proprtion of the rectangle are unknown.
-
-This method is taken from "Markerless Tracking using Planar Structures
-in the Scene" by Gilles Simon. The idea is to compute the homography H
-giving the image point of the rectangle by associating them with the
-coordinates (0,0)(1,0)(1,1/s)(0,1/s) (the rectangle is on the Z=0 plane).
-If K is the intrinsec parameters matrix, we have s = ||Kh1||/ ||Kh2||. s
-gives us the proportion of the rectangle
-
-\param p1,p2,p3,p4: the image of the corners of the rectangle
-(respectively the image of (0,0),(lx,0),(lx,lx/s) and (0,lx/s)) (input)
-\param cam: the camera used (input)
-\param lx: the rectangle size on the x axis (input)
-\param cMo: the camera pose (output)
-\return int : OK if no pb occurs
-*/
-double
-vpPose::poseFromRectangle(vpPoint &p1,vpPoint &p2,
- vpPoint &p3,vpPoint &p4,
- double lx, vpCameraParameters & cam,
- vpHomogeneousMatrix & cMo)
-{
-
- std::vector<double> rectx(4) ;
- std::vector<double> recty(4) ;
- rectx[0]= 0 ;
- recty[0]=0 ;
- rectx[1]=1 ;
- recty[1]=0 ;
- rectx[2]=1 ;
- recty[2]=1 ;
- rectx[3]=0 ;
- recty[3]=1 ;
- std::vector<double> irectx(4) ;
- std::vector<double> irecty(4) ;
- irectx[0]=(p1.get_x()) ;
- irecty[0]=(p1.get_y()) ;
- irectx[1]=(p2.get_x()) ;
- irecty[1]=(p2.get_y()) ;
- irectx[2]=(p3.get_x()) ;
- irecty[2]=(p3.get_y()) ;
- irectx[3]=(p4.get_x()) ;
- irecty[3]=(p4.get_y()) ;
-
- //calcul de l'homographie
- vpMatrix H(3,3);
- vpHomography hom;
-
- // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
- vpHomography::HLM(rectx,recty,irectx,irecty,1,hom);
- for (unsigned int i=0 ; i < 3 ; i++)
- for(unsigned int j=0 ; j < 3 ; j++)
- H[i][j] = hom[i][j] ;
- //calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y axis)
- vpColVector kh1(3);
- vpColVector kh2(3);
- vpMatrix K(3,3);
- K = cam.get_K();
- K.setIdentity();
- vpMatrix Kinv =K.pseudoInverse();
-
- vpMatrix KinvH =Kinv*H;
- kh1=KinvH.getCol(0);
- kh2=KinvH.getCol(1);
-
-
- double s= sqrt(kh1.sumSquare())/sqrt(kh2.sumSquare());
-
-
- vpMatrix D(3,3);
- D.setIdentity();
- D[1][1]=1/s;
- vpMatrix cHo=H*D;
-
- //Calcul de la rotation et de la translation
- // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
- p1.setWorldCoordinates(0,0,0) ;
- p2.setWorldCoordinates(lx,0,0) ;
- p3.setWorldCoordinates(lx,lx/s,0) ;
- p4.setWorldCoordinates(0,lx/s,0) ;
-
-
- vpPose P ;
- P.addPoint(p1) ;
- P.addPoint(p2) ;
- P.addPoint(p3) ;
- P.addPoint(p4) ;
-
-
- P.computePose(vpPose::DEMENTHON_LOWE,cMo) ;
- return lx/s ;
-
-}
diff --git a/src/computer-vision/pose-estimation/vpPose.h b/src/computer-vision/pose-estimation/vpPose.h
deleted file mode 100644
index 191aea1..0000000
--- a/src/computer-vision/pose-estimation/vpPose.h
+++ /dev/null
@@ -1,280 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPose.h 5211 2015-01-26 17:44:35Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pose computation.
- *
- * Authors:
- * Eric Marchand
- * Francois Chaumette
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpPose.h
- \brief Tools for pose computation (pose from point only).
-
- \author Eric Marchand (INRIA) using code from Francois Chaumette (INRIA)
- \date April, 6 1999 (first issue)
-*/
-
-#ifndef vpPOSE_HH
-#define vpPOSE_HH
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpPoint.h>
-#include <visp/vpRGBa.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-#include <math.h>
-#include <list>
-#include <vector>
-
-/*!
- \class vpPose
- \ingroup Pose
- \brief Class used for pose computation from N points (pose from point only).
-
- \note It is also possible to estimate a pose from other features using vpPoseFeatures class.
-
- To see how to use this class you can follow the \ref tutorial-pose-estimation.
-*/
-
-
-class VISP_EXPORT vpPose
-{
-public:
- typedef enum
- {
- LAGRANGE ,
- DEMENTHON ,
- LOWE ,
- RANSAC ,
- LAGRANGE_LOWE ,
- DEMENTHON_LOWE ,
- VIRTUAL_VS ,
- DEMENTHON_VIRTUAL_VS,
- LAGRANGE_VIRTUAL_VS
- } vpPoseMethodType;
-
- unsigned int npt ; //!< number of point used in pose computation
- std::list<vpPoint> listP ; //!< array of point (use here class vpPoint)
-
- double residual ; //!< compute the residual in meter
-
-protected :
- double lambda ;//!< parameters use for the virtual visual servoing approach
-
-private:
- int vvsIterMax ; //! define the maximum number of iteration in VVS
- //! variable used in the Dementhon approach
- std::vector<vpPoint> c3d ;
- //! Flag used to specify if the covariance matrix has to be computed or not.
- bool computeCovariance;
- //! Covariance matrix
- vpMatrix covarianceMatrix;
-
- unsigned int ransacNbInlierConsensus;
- int ransacMaxTrials;
- std::vector<vpPoint> ransacInliers;
- double ransacThreshold;
-
-protected:
- double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;
-
- // method used in poseDementhonPlan()
- int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;
-
-public:
- // constructor
- vpPose() ;
- //! destructor
- virtual ~vpPose() ;
- //! Add a new point in this array
- void addPoint(const vpPoint& P) ;
- //! suppress all the point in the array of point
- void clearPoint() ;
-
- //! compute the pose for a given method
- bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
- //! compute the residual (i.e., the quality of the result)
- //! compute the residual (in meter for pose M)
- double computeResidual(const vpHomogeneousMatrix &cMo) const ;
- //! test the coplanarity of the points
- bool coplanar(int &coplanar_plane_type) ;
- void displayModel(vpImage<unsigned char> &I,
- vpCameraParameters &cam,
- vpColor col=vpColor::none) ;
- void displayModel(vpImage<vpRGBa> &I,
- vpCameraParameters &cam,
- vpColor col=vpColor::none) ;
- double distanceToPlaneForCoplanarityTest ;
- void init() ;
- //! compute the pose using Dementhon approach (planar object)
- void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
- //! compute the pose using Dementhon approach (non planar object)
- void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
- //! compute the pose using Lagrange approach (planar object)
- void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
- //! compute the pose using Lagrange approach (non planar object)
- void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
- //! compute the pose using the Lowe approach (i.e., using the
- //! Levenberg Marquartd non linear minimization approach)
- void poseLowe(vpHomogeneousMatrix & cMo) ;
- //! compute the pose using the Ransac approach
- bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
- //! compute the pose using a robust virtual visual servoing approach
- void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
- //! compute the pose using virtual visual servoing approach
- void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
- void printPoint() ;
- void setDistanceToPlaneForCoplanarityTest(double d) ;
- void setLambda(double a) { lambda = a ; }
- void setVvsIterMax(int nb) { vvsIterMax = nb ; }
-
- void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
- void setRansacThreshold(const double &t) {
- //Test whether or not t is > 0
- if(t > 0) {
- ransacThreshold = t;
- } else {
- throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
- }
- }
- void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
- unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
- std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
-
- /*!
- Set if the covaraince matrix has to be computed in the Virtual Visual Servoing approach.
-
- \param flag : True if the covariance has to be computed, false otherwise.
- */
- void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
-
- /*!
- Get the covariance matrix computed in the Virtual Visual Servoing approach.
-
- \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
-
- \sa setCovarianceComputation
- */
- vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
- }
-
- static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam, double size,
- vpColor col=vpColor::none) ;
- static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam, double size,
- vpColor col=vpColor::none) ;
- static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
- vpPoint &p3,vpPoint &p4,
- double lx, vpCameraParameters & cam,
- vpHomogeneousMatrix & cMo) ;
-
- static void findMatch(std::vector<vpPoint> &p2D,
- std::vector<vpPoint> &p3D,
- const unsigned int &numberOfInlierToReachAConsensus,
- const double &threshold,
- unsigned int &ninliers,
- std::vector<vpPoint> &listInliers,
- vpHomogeneousMatrix &cMo,
- const int &maxNbTrials = 10000);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-private:
- /*!
- @name Deprecated functions
- */
- static void initRansac(const unsigned int n,
- const double *x, const double *y,
- const unsigned int m,
- const double *X, const double *Y, const double *Z,
- vpColVector &data) ;
-
-public:
- static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) ;
-
- static double computeResidual(const vpColVector &x, const vpColVector &M, vpColVector &d) ;
-
- static bool degenerateConfiguration(vpColVector &x, unsigned int *ind) ;
-
- static void ransac(const unsigned int n,
- const double *x, const double *y,
- const unsigned int m,
- const double *X, const double *Y, const double *Z,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- vpColVector &xi, vpColVector &yi,
- vpColVector &Xi, vpColVector &Yi, vpColVector &Zi,
- vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
-
- vp_deprecated static void ransac(const unsigned int n,
- const vpPoint *p,
- const unsigned int m,
- const vpPoint *P,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- std::list<vpPoint> &Pi,
- vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
-
- vp_deprecated static void ransac(std::list<vpPoint> &p,
- std::list<vpPoint> &P,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- std::list<vpPoint> &lPi,
- vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
-#endif
-} ;
-
-
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/computer-vision/pose-estimation/vpPoseDementhon.cpp b/src/computer-vision/pose-estimation/vpPoseDementhon.cpp
deleted file mode 100644
index 58edca4..0000000
--- a/src/computer-vision/pose-estimation/vpPoseDementhon.cpp
+++ /dev/null
@@ -1,716 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpPoseDementhon.cpp 5198 2015-01-23 17:32:04Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Pose computation.
-*
-* Authors:
-* Eric Marchand
-* Francois Chaumette
-*
-*****************************************************************************/
-
-
-
-#include <visp/vpPose.h>
-#include <visp/vpMath.h>
-
-#define DEBUG_LEVEL1 0
-#define DEBUG_LEVEL2 0
-#define DEBUG_LEVEL3 0
-
-/* FC
-#ifndef DEG
-#define DEG (180.0/M_PI)
-#endif
-*/
-
-/*!
-\brief Compute the pose using Dementhon approach for non planar objects
-this is a direct implementation of the algorithm proposed by
-Dementhon and Davis in their 1995 paper \cite Dementhon95.
-
-*/
-
-void
-vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
-{
- double normI = 0., normJ = 0.;
- double Z0 = 0.;
- double seuil=1.0;
- double f=1.;
-
- vpPoint p0 = listP.front() ;
-
- c3d.clear();
- vpPoint P;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = (*it);
- P.set_oX(P.get_oX()-p0.get_oX()) ;
- P.set_oY(P.get_oY()-p0.get_oY()) ;
- P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
- c3d.push_back(P) ;
- }
-
- vpMatrix a(npt,3) ;
-
- for (unsigned int i=0 ; i < npt ; i++)
- {
- a[i][0]=c3d[i].get_oX();
- a[i][1]=c3d[i].get_oY();
- a[i][2]=c3d[i].get_oZ();
- }
-
- //std::cout << a << std::endl ;
- // calcul a^T a
- vpMatrix ata ;
- ata = a.t()*a ;
-
- // calcul (a^T a)^-1 par decomposition LU
- vpMatrix ata1 ;
- ata1 = ata.pseudoInverse(1e-6) ; //InverseByLU() ;
-
- vpMatrix b ;
- b = (a*ata1).t() ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata<<std::endl ;
- std::cout << "ata1" << std::endl <<ata1<<std::endl ;
- std::cout<< " ata*ata1" << std::endl << ata*ata1 ;
- std::cout<< " b" << std::endl << (a*ata1).t() ;
-
- }
-#endif
-
- // calcul de la premiere solution
-
- vpColVector eps(npt) ;
- eps =0 ;
-
- int cpt = 0 ;
- vpColVector I, J, k ;
- I.resize(3) ;
- J.resize(3) ;
- k.resize(3) ;
-
- while(cpt < 20)
- {
- I = 0 ;
- J = 0 ;
-
- vpColVector xprim(npt) ;
- vpColVector yprim(npt) ;
- for (unsigned int i=0;i<npt;i++)
- {
- xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].get_x();
- yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].get_y();
- }
- I = b*xprim ;
- J = b*yprim ;
- normI = sqrt(I.sumSquare()) ;
- normJ = sqrt(J.sumSquare()) ;
- I = I/normI ;
- J = J/normJ ;
-
- if (normI+normJ < 1e-10)
- {
- //vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Dementhon pose computation: normI+normJ = 0")) ;
- }
-
- k = vpColVector::cross(I,J) ;
- Z0=2*f/(normI+normJ);
- cpt=cpt+1; seuil=0.0;
- for (unsigned int i=0; i<npt; i++)
- {
- double epsi_1 = eps[i] ;
- eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
- seuil+=fabs(eps[i]-epsi_1);
- }
- if (npt==0)
- {
- //vpERROR_TRACE( " npt = 0, division par zero ");
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Dementhon pose computation: no points")) ;
- }
- seuil/=npt;
- }
- k.normalize();
- J = vpColVector::cross(k,I) ;
- /*matrice de passage*/
-
- cMo[0][0]=I[0];
- cMo[0][1]=I[1];
- cMo[0][2]=I[2];
- cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
-
- cMo[1][0]=J[0];
- cMo[1][1]=J[1];
- cMo[1][2]=J[2];
- cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
-
- cMo[2][0]=k[0];
- cMo[2][1]=k[1];
- cMo[2][2]=k[2];
- cMo[2][3]=Z0;
-
- cMo[0][3] -= (p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2]);
- cMo[1][3] -= (p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2]);
- cMo[2][3] -= (p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2]);
-}
-
-
-#define DMIN 0.01 /* distance min entre la cible et la camera */
-#define EPS 0.0000001
-#define EPS_DEM 0.001
-
-static void
-calculRTheta(double s, double c, double &r, double &theta)
-{
- if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
- {
- r = sqrt(sqrt(s*s+c*c));
- theta = atan2(s,c)/2.0;
- }
- else
- {
- if (fabs(c) > fabs(s))
- {
- r = fabs(c);
- if (c >= 0.0)
- theta = M_PI/2;
- else
- theta = -M_PI/2;
- }
- else
- {
- r = fabs(s);
- if (s >= 0.0)
- theta = M_PI/4.0;
- else
- theta = -M_PI/4.0;
- }
- }
-}
-
-static
-void calculSolutionDementhon(double xi0, double yi0,
- vpColVector &I, vpColVector &J,
- vpHomogeneousMatrix &cMo )
-{
-
-#if (DEBUG_LEVEL1)
- std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
-#endif
-
- double normI, normJ, normk, Z0;
- vpColVector k(3);
-
- // normalisation de I et J
- normI = sqrt(I.sumSquare()) ;
- normJ = sqrt(J.sumSquare()) ;
-
- I/=normI;
- J/=normJ;
-
-
- k = vpColVector::cross(I,J) ; // k = I^I
-
- Z0=2.0/(normI+normJ);
-
- normk = sqrt(k.sumSquare()) ;
- k /= normk ;
-
- J = vpColVector::cross(k,I) ;
-
- //calcul de la matrice de passage
- cMo[0][0]=I[0];
- cMo[0][1]=I[1];
- cMo[0][2]=I[2];
- cMo[0][3]=xi0*Z0;
-
- cMo[1][0]=J[0];
- cMo[1][1]=J[1];
- cMo[1][2]=J[2];
- cMo[1][3]=yi0*Z0;
-
- cMo[2][0]=k[0];
- cMo[2][1]=k[1];
- cMo[2][2]=k[2];
- cMo[2][3]=Z0;
-
-
-#if (DEBUG_LEVEL1)
- std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
-#endif
-
-}
-
-int
-vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
- vpHomogeneousMatrix &cMo)
-{
-
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
-#endif
-
- unsigned int i, k;
- int erreur = 0;
- unsigned int cpt;
- double s,c,si,co;
- double smin,smin_old, s1,s2;
- double r, theta;
- vpHomogeneousMatrix cMo1,cMo2,cMo_old;
-
- unsigned int iter_max = 20;
- vpMatrix eps(iter_max+1,npt) ;
-
-
- // on test si tous les points sont devant la camera
- for(i = 0; i < npt; i++)
- {
- double z ;
- z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
- if (z <= 0.0) erreur = -1;
- }
-
- smin = sqrt(computeResidualDementhon(cMo)/npt) ;
-
- vpColVector xi(npt) ;
- vpColVector yi(npt) ;
-
- if (erreur==0)
- {
- k=0;
- for(i = 0; i < npt; i++)
- {
- xi[k] = c3d[i].get_x();
- yi[k] = c3d[i].get_y();
-
- if (k != 0)
- { // On ne prend pas le 1er point
- eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
- cMo[2][1]*c3d[i].get_oY() +
- cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
- }
- k++;
- }
-
-
- vpColVector I0(3) ;
- vpColVector J0(3) ;
- vpColVector I(3) ;
- vpColVector J(3) ;
-
- smin_old = 2*smin ;
-
- cpt = 0;
- while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
- {
-#if (DEBUG_LEVEL2)
- {
- std::cout << "cpt " << cpt << std::endl ;
- std::cout << "smin_old " << smin_old << std::endl ;
- std::cout << "smin " << smin << std::endl ;
- }
-#endif
-
- smin_old = smin;
- cMo_old = cMo;
-
- I0 = 0 ;
- J0 = 0 ;
-
- for (i=1;i<npt;i++)
- {
- s = (1.0+eps[cpt][i])*xi[i] - xi[0];
- I0[0] += b[0][i-1] * s;
- I0[1] += b[1][i-1] * s;
- I0[2] += b[2][i-1] * s;
- s = (1.0+eps[cpt][i])*yi[i] - yi[0];
- J0[0] += b[0][i-1] * s;
- J0[1] += b[1][i-1] * s;
- J0[2] += b[2][i-1] * s;
- }
-
- s = -2.0*(vpColVector::dotProd(I0,J0));
- c = J0.sumSquare() - I0.sumSquare() ;
-
- calculRTheta(s,c,r,theta);
- co = cos(theta);
- si = sin(theta);
-
- /* 1ere branche */
- I = I0 + U*r*co ;
- J = J0 + U*r*si ;
-
-#if (DEBUG_LEVEL3)
- {
- std::cout << "I " << I.t() ;
- std::cout << "J " << J.t() ;
- }
-#endif
-
- calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
- s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
-#if (DEBUG_LEVEL3)
- std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
-#endif
-
- /* 2eme branche */
- I = I0 - U*r*co ;
- J = J0 - U*r*si ;
-#if (DEBUG_LEVEL3)
- {
- std::cout << "I " << I.t() ;
- std::cout << "J " << J.t() ;
- }
-#endif
-
- calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
- s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
-#if (DEBUG_LEVEL3)
- std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
-#endif
-
- cpt ++;
- if (s1 <= s2)
- {
- smin = s1;
- k = 0;
- for(i = 0; i < npt; i++)
- {
- if (k != 0) { // On ne prend pas le 1er point
- eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
- + cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
- }
- k++;
- }
- cMo = cMo1 ;
- }
- else
- {
- smin = s2;
- k = 0;
- for(i = 0; i < npt; i++)
- {
- if (k != 0) { // On ne prend pas le 1er point
- eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
- + cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
- }
- k++;
- }
- cMo = cMo2 ;
- }
-
- if (smin > smin_old)
- {
-#if (DEBUG_LEVEL2)
- std::cout << "Divergence " << std::endl ;
-#endif
-
- cMo = cMo_old ;
- }
-#if (DEBUG_LEVEL2)
- {
- std::cout << "s1 = " << s1 << std::endl ;
- std::cout << "s2 = " << s2 << std::endl ;
- std::cout << "smin = " << smin << std::endl ;
- std::cout << "smin_old = " << smin_old << std::endl ;
- }
-#endif
- }
- }
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
-#endif
-
- return erreur ;
-}
-
-/*!
-\brief Compute the pose using Dementhon approach for planar objects
-this is a direct implementation of the algorithm proposed by
-Dementhon in his PhD.
-
-\author Francois Chaumette (simplified by Eric Marchand)
-*/
-
-void
-vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
-#endif
-
- unsigned int i,j,k ;
-
- vpPoint p0 = listP.front() ;
-
- vpPoint P ;
- c3d.clear();
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- P.set_oX(P.get_oX()-p0.get_oX()) ;
- P.set_oY(P.get_oY()-p0.get_oY()) ;
- P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
- c3d.push_back(P);
- }
-
- vpMatrix a ;
- try
- {
- a.resize(npt-1,3) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-
-
- for (i=1 ; i < npt ; i++)
- {
- a[i-1][0]=c3d[i].get_oX();
- a[i-1][1]=c3d[i].get_oY();
- a[i-1][2]=c3d[i].get_oZ();
- }
-
- // calcul a^T a
- vpMatrix ata ;
- ata = a.t()*a ;
-
- /* essai FC pour debug SVD */
- /*
- vpMatrix ata_old ;
- ata_old = a.t()*a ;
-
- vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
- for (i=0;i<ata.getRows();i++)
- for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
- */
- vpMatrix ata_sav;
- ata_sav = ata;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata<<std::endl ;
- }
-#endif
-
- // calcul (a^T a)^-1
- vpMatrix ata1(ata.getRows(),ata.getCols()) ;
- vpMatrix v(ata.getRows(),ata.getCols());
- vpColVector sv(ata.getRows());
- // ata1 = ata.i() ;
- unsigned int imin = 0;
- double s = 0.0;
-
- //calcul de ata^-1
- ata.svd(sv,v) ;
-
- unsigned int nc = sv.getRows() ;
- for (i=0; i < nc ; i++)
- if (sv[i] > s) s = sv[i];
-
- s *= 0.0002;
- int irank = 0;
- for (i=0;i<nc;i++)
- if (sv[i] > s ) irank++;
-
- double svm = 100.0;
- for (i = 0; i < nc; i++)
- if (sv[i] < svm) { imin = i; svm = sv[i]; }
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "rang: " << irank << std::endl ;;
- std::cout <<"imin = " << imin << std::endl ;
- std::cout << "sv " << sv.t() << std::endl ;
- }
-#endif
-
- for (i=0 ; i < ata.getRows() ; i++)
- for (j=0 ; j < ata.getCols() ; j++)
- {
- ata1[i][j] = 0.0;
- for (k=0 ; k < nc ; k++)
- if (sv[k] > s)
- ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
- }
-
-
-
- vpMatrix b ; // b=(at a)^-1*at
- b = ata1*a.t() ;
-
- //calcul de U
- vpColVector U(3) ;
- U = ata.getCol(imin) ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
- std::cout << "ata1" << std::endl <<ata1<<std::endl ;
- std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
- std::cout << "b" << std::endl << b ;
- std::cout << "U " << U.t() << std::endl ;
- }
-#endif
-
- vpColVector xi(npt) ;
- vpColVector yi(npt) ;
- //calcul de la premiere solution
- for (i = 0; i < npt; i++)
- {
- xi[i] = c3d[i].get_x() ;
- yi[i] = c3d[i].get_y() ;
-
- }
-
- vpColVector I0(3) ; I0 = 0 ;
- vpColVector J0(3) ; J0 = 0 ;
- vpColVector I(3) ;
- vpColVector J(3) ;
-
- for (i=1;i<npt;i++)
- {
- I0[0] += b[0][i-1] * (xi[i]-xi[0]);
- I0[1] += b[1][i-1] * (xi[i]-xi[0]);
- I0[2] += b[2][i-1] * (xi[i]-xi[0]);
-
- J0[0] += b[0][i-1] * (yi[i]-yi[0]);
- J0[1] += b[1][i-1] * (yi[i]-yi[0]);
- J0[2] += b[2][i-1] * (yi[i]-yi[0]);
- }
-
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "I0 "<<I0.t() ;
- std::cout << "J0 "<<J0.t() ;
- }
-#endif
-
- s = -2.0*vpColVector::dotProd(I0,J0);
- double c = J0.sumSquare() - I0.sumSquare() ;
-
- double r,theta,si,co ;
- calculRTheta(s, c, r, theta);
- co = cos(theta);
- si = sin(theta);
-
- // calcul de la premiere solution
- I = I0 + U*r*co ;
- J = J0 + U*r*si ;
-
- vpHomogeneousMatrix cMo1f ;
- calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
-
-
- int erreur1 = calculArbreDementhon(b, U, cMo1f);
-
- // calcul de la deuxieme solution
- I = I0 - U*r*co ;
- J = J0 - U*r*si ;
-
- vpHomogeneousMatrix cMo2f;
- calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
-
- int erreur2 = calculArbreDementhon(b, U, cMo2f);
-
- if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
- if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
- if ((erreur1 == 0) && (erreur2 == 0))
- {
- double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
- double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
-
- if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
- }
-
- cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
- cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
- cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
-
-#if (DEBUG_LEVEL1)
- std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
-#endif
-}
-
-#undef DMIN
-#undef EPS
-#undef EPS_DEM
-
-
-/*!
-\brief Compute and return the residual expressed in meter for the pose matrix
-'pose'.
-
-\param cMo : the matrix that defines the pose to be tested.
-
-\return the value of he residual in meter
-*/
-double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo)
-{
- double residual_ = 0 ;
-
- residual_ =0 ;
- for (unsigned int i =0 ; i < npt ; i++)
- {
-
- double X = c3d[i].get_oX()*cMo[0][0]+c3d[i].get_oY()*cMo[0][1]+c3d[i].get_oZ()*cMo[0][2] + cMo[0][3];
- double Y = c3d[i].get_oX()*cMo[1][0]+c3d[i].get_oY()*cMo[1][1]+c3d[i].get_oZ()*cMo[1][2] + cMo[1][3];
- double Z = c3d[i].get_oX()*cMo[2][0]+c3d[i].get_oY()*cMo[2][1]+c3d[i].get_oZ()*cMo[2][2] + cMo[2][3];
-
- double x = X/Z ;
- double y = Y/Z ;
-
- residual_ += vpMath::sqr(x-c3d[i].get_x()) + vpMath::sqr(y-c3d[i].get_y()) ;
- }
- return residual_ ;
-}
-
-
-#undef DEBUG_LEVEL1
-#undef DEBUG_LEVEL2
-#undef DEBUG_LEVEL3
-
diff --git a/src/computer-vision/pose-estimation/vpPoseException.h b/src/computer-vision/pose-estimation/vpPoseException.h
deleted file mode 100644
index 23c4c54..0000000
--- a/src/computer-vision/pose-estimation/vpPoseException.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Error that can be emited by the vpPose class and its derivates
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpPoseException_H
-#define __vpPoseException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* Classes standards. */
-
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpPoseException
- \ingroup Exception
- \brief Error that can be emited by the vpPose class and its derivates.
- */
-class VISP_EXPORT vpPoseException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpPose member
- */
- enum errorCodeEnum
- {
- poseError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedERR,
- //! index out of range
- outOfRangeError,
- notEnoughPointError
- } ;
-
- public:
- vpPoseException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpPoseException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpPoseException (const int id)
- : vpException(id){ ; }
- // vpPoseException() : vpException() { ;}
-};
-
-#endif
diff --git a/src/computer-vision/pose-estimation/vpPoseFeatures.cpp b/src/computer-vision/pose-estimation/vpPoseFeatures.cpp
deleted file mode 100644
index 132c074..0000000
--- a/src/computer-vision/pose-estimation/vpPoseFeatures.cpp
+++ /dev/null
@@ -1,582 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseFeatures.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pose computation from any features.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-#include <visp/vpPoseFeatures.h>
-
-/*!
- Default constructor.
-*/
-vpPoseFeatures::vpPoseFeatures()
- : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
- covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
- featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
- featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
-{
-}
-
-/*!
- Destructor that deletes the array of features and projections.
-*/
-vpPoseFeatures::~vpPoseFeatures()
-{
- clear();
-}
-
-/*!
- Clear all the features
-*/
-void vpPoseFeatures::clear()
-{
- for(int i = (int)featurePoint_Point_list.size()-1 ; i >= 0 ; i--)
- delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
- featurePoint_Point_list.clear();
-
- for(int i = (int)featurePoint3D_Point_list.size()-1 ; i >= 0 ; i--)
- delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
- featurePoint3D_Point_list.clear();
-
- for(int i = (int)featureVanishingPoint_Point_list.size()-1 ; i >= 0 ; i--)
- delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
- featureVanishingPoint_Point_list.clear();
-
- for(int i = (int)featureVanishingPoint_DuoLine_list.size()-1 ; i >= 0 ; i--)
- delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
- featureVanishingPoint_DuoLine_list.clear();
-
- for(int i = (int)featureEllipse_Sphere_list.size()-1 ; i >= 0 ; i--)
- delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
- featureEllipse_Sphere_list.clear();
-
- for(int i = (int)featureEllipse_Circle_list.size()-1 ; i >= 0 ; i--)
- delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
- featureEllipse_Circle_list.clear();
-
- for(int i = (int)featureLine_Line_list.size()-1 ; i >= 0 ; i--)
- delete featureLine_Line_list[(unsigned int)i].desiredFeature;
- featureLine_Line_list.clear();
-
- for(int i = (int)featureLine_DuoLineInt_List.size()-1 ; i >= 0 ; i--)
- delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
- featureLine_DuoLineInt_List.clear();
-
- for(int i = (int)featureSegment_DuoPoints_list.size()-1 ; i >= 0 ; i--)
- delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
- featureSegment_DuoPoints_list.clear();
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- for(int i = (int)featureSpecific_list.size()-1 ; i >= 0 ; i--)
- delete featureSpecific_list[(unsigned int)i];
- featureSpecific_list.clear();
-#endif
-
- maxSize = 0;
- totalSize = 0;
-}
-
-/*!
- Add a point feature to the list of features to be considered in the pose computation.
-
- \param p : Point projection expressed as a vpPoint.
-*/
-void vpPoseFeatures::addFeaturePoint(const vpPoint &p)
-{
- featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint,vpPoint>());
- featurePoint_Point_list.back().firstParam = p;
- featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
- vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature,p);
-
- totalSize++;
- if(featurePoint_Point_list.size() > maxSize)
- maxSize = (unsigned int)featurePoint_Point_list.size();
-}
-
-/*!
- Add a point 3D feature to the list of features to be considered in the pose computation.
-
- \param p : Projection expressed as a vpPoint.
-*/
-void vpPoseFeatures::addFeaturePoint3D(const vpPoint &p)
-{
- featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D,vpPoint>());
- featurePoint3D_Point_list.back().firstParam = p;
- featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
- vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature,p);
-
- totalSize++;
- if(featurePoint3D_Point_list.size() > maxSize)
- maxSize = (unsigned int)featurePoint3D_Point_list.size();
-}
-
-/*!
- Add a vanishing point feature to the list of features to be considered in the pose computation.
-
- \param p : Projection expressed as a vpPoint.
-*/
-void vpPoseFeatures::addFeatureVanishingPoint(const vpPoint &p)
-{
- featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint,vpPoint>());
- featureVanishingPoint_Point_list.back().firstParam = p;
- featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
- vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature,p);
-
- totalSize++;
- if(featureVanishingPoint_Point_list.size() > maxSize)
- maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
-}
-
-/*!
- Add a vanishing point feature to the list of features to be considered in the pose computation.
-
- \param l1 : First line used to create the feature.
- \param l2 : Second line used to create the feature.
-*/
-void vpPoseFeatures::addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2)
-{
- featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint,vpLine,vpLine>());
- featureVanishingPoint_DuoLine_list.back().firstParam = l1;
- featureVanishingPoint_DuoLine_list.back().secondParam = l2;
- featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
- vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature,l1,l2);
-
- totalSize++;
- if(featureVanishingPoint_DuoLine_list.size() > maxSize)
- maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
-}
-
-/*!
- Add an ellipse feature to the list of features to be considered in the pose computation.
-
- \param s : Ellipse projection expressed as a vpSphere.
-*/
-void vpPoseFeatures::addFeatureEllipse(const vpSphere &s)
-{
- featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse,vpSphere>());
- featureEllipse_Sphere_list.back().firstParam = s;
- featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
- vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature,s);
-
- totalSize++;
- if(featureEllipse_Sphere_list.size() > maxSize)
- maxSize = (unsigned int)featureEllipse_Sphere_list.size();
-}
-
-/*!
- Add an ellipse feature to the list of features to be considered in the pose computation.
-
- \param c : Ellipse projection expressed as a vpCircle.
-*/
-void vpPoseFeatures::addFeatureEllipse(const vpCircle &c)
-{
- featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse,vpCircle>());
- featureEllipse_Circle_list.back().firstParam = c;
- featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
- vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature,c);
-
- totalSize++;
- if(featureEllipse_Circle_list.size() > maxSize)
- maxSize = (unsigned int)featureEllipse_Circle_list.size();
-}
-
-/*!
- Add a line feature to the list of features to be considered in the pose computation.
-
- \param l : Line projection expressed as a vpLine.
-*/
-void vpPoseFeatures::addFeatureLine(const vpLine &l)
-{
- featureLine_Line_list.push_back(vpDuo<vpFeatureLine,vpLine>());
- featureLine_Line_list.back().firstParam = l;
- featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
- vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature,l);
-
- totalSize++;
- if(featureLine_Line_list.size() > maxSize)
- maxSize = (unsigned int)featureLine_Line_list.size();
-}
-
-/*!
- Add a line feature to the list of features to be considered in the pose computation.
-
- \param c : Line projection expressed as a vpCylinder.
- \param line : Integer id that indicates which limb of the cylinder is to consider.
- It can be vpCylinder::line1 or vpCylinder::line2.
-*/
-void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
-{
- featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine,vpCylinder,int>());
- featureLine_DuoLineInt_List.back().firstParam = c;
- featureLine_DuoLineInt_List.back().secondParam = line;
- featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
- vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature,c,line);
-
- totalSize++;
- if(featureLine_DuoLineInt_List.size() > maxSize)
- maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
-}
-
-/*!
- Add a segment feature to the list of features to be considered in the pose computation.
-
- \param P1 : First extremity projection.
- \param P2 : Second extremity projection.
-*/
-void vpPoseFeatures::addFeatureSegment(vpPoint &P1, vpPoint &P2)
-{
- featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment,vpPoint,vpPoint>());
- featureSegment_DuoPoints_list.back().firstParam = P1;
- featureSegment_DuoPoints_list.back().secondParam = P2;
- featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
- vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature,P1,P2);
-
- totalSize++;
- if(featureSegment_DuoPoints_list.size() > maxSize)
- maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
-}
-
-/*!
- Get the error vector and L matrix from all the features.
-
- \param cMo : Current Pose.
- \param err : Resulting error vector.
- \param L : Resulting interaction matrix.
-*/
-void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L)
-{
- err = vpColVector();
- L = vpMatrix();
-
- for(unsigned int i = 0 ; i < maxSize ; i++)
- {
- //--------------vpFeaturePoint--------------
- //From vpPoint
- if( i < featurePoint_Point_list.size() ){
- vpFeaturePoint fp;
- vpPoint p(featurePoint_Point_list[i].firstParam);
- p.track(cMo);
- vpFeatureBuilder::create(fp, p);
- err.stackMatrices(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
- L.stackMatrices(fp.interaction());
- }
-
- //--------------vpFeaturePoint3D--------------
- //From vpPoint
- if( i < featurePoint3D_Point_list.size() ){
- vpFeaturePoint3D fp3D;
- vpPoint p(featurePoint3D_Point_list[i].firstParam);
- p.track(cMo);
- vpFeatureBuilder::create(fp3D, p);
- err.stackMatrices(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
- L.stackMatrices(fp3D.interaction());
- }
-
- //--------------vpFeatureVanishingPoint--------------
- //From vpPoint
- if( i < featureVanishingPoint_Point_list.size() ){
- vpFeatureVanishingPoint fvp;
- vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
- p.track(cMo);
- vpFeatureBuilder::create(fvp, p);
- err.stackMatrices(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
- L.stackMatrices(fvp.interaction());
- }
- //From Duo of vpLines
- if( i < featureVanishingPoint_DuoLine_list.size() ){
- vpFeatureVanishingPoint fvp;
- vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
- vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
- l1.track(cMo);
- l2.track(cMo);
- vpFeatureBuilder::create(fvp, l1, l2);
- err.stackMatrices(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
- L.stackMatrices(fvp.interaction());
- }
-
- //--------------vpFeatureEllipse--------------
- //From vpSphere
- if( i < featureEllipse_Sphere_list.size() ){
- vpFeatureEllipse fe;
- vpSphere s(featureEllipse_Sphere_list[i].firstParam);
- s.track(cMo);
- vpFeatureBuilder::create(fe, s);
- err.stackMatrices(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
- L.stackMatrices(fe.interaction());
- }
- //From vpCircle
- if( i < featureEllipse_Circle_list.size() ){
- vpFeatureEllipse fe;
- vpCircle c(featureEllipse_Circle_list[i].firstParam);
- c.track(cMo);
- vpFeatureBuilder::create(fe, c);
- err.stackMatrices(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
- L.stackMatrices(fe.interaction());
- }
-
- //--------------vpFeatureLine--------------
- //From vpLine
- if( i < featureLine_Line_list.size() ){
- vpFeatureLine fl;
- vpLine l(featureLine_Line_list[i].firstParam);
- l.track(cMo);
- vpFeatureBuilder::create(fl, l);
- err.stackMatrices(fl.error(*(featureLine_Line_list[i].desiredFeature)));
- L.stackMatrices(fl.interaction());
- }
- //From Duo of vpCylinder / Integer
- if( i < featureLine_DuoLineInt_List.size() ){
- vpFeatureLine fl;
- vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
- c.track(cMo);
- vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
- err.stackMatrices(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
- L.stackMatrices(fl.interaction());
- }
-
- //--------------vpFeatureSegment--------------
- //From Duo of vpPoints
- if( i < featureSegment_DuoPoints_list.size() ){
- vpFeatureSegment fs;
- vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
- vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
- p1.track(cMo);
- p2.track(cMo);
- vpFeatureBuilder::create(fs, p1, p2);
- err.stackMatrices(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
- L.stackMatrices(fs.interaction());
- }
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- //--------------Specific Feature--------------
- if( i < featureSpecific_list.size() ){
- featureSpecific_list[i]->createCurrent(cMo);
- err.stackMatrices(featureSpecific_list[i]->error());
- L.stackMatrices(featureSpecific_list[i]->currentInteraction());
- }
-#endif
- }
-}
-
-
-/*!
- Compute the pose according to the desired method (virtual visual servoing, or robust virtual visual servoing approach).
-
- \param cMo : Computed pose.
-
- \param type : Method to use for the pose computation.
-
- - The virtual visual servoing approach is described in \cite Marchand02c.
-
- - The robust virtual visual servoing approach is described in \cite Comport06b.
-
-*/
-void vpPoseFeatures::computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type)
-{
- switch(type)
- {
- case VIRTUAL_VS:
- computePoseVVS(cMo);
- break;
- case ROBUST_VIRTUAL_VS:
- computePoseRobustVVS(cMo);
- break;
- default:
- break;
- }
-}
-
-/*!
- Compute the pose thanks to the virtual visual servoing approach.
-
- This approach is described in:
-
- E. Marchand, F. Chaumette. Virtual Visual Servoing: a framework for real-time
- augmented reality. In EUROGRAPHICS 2002 Conference Proceeding, G. Drettakis,
- H.-P. Seidel (eds.), Computer Graphics Forum, Volume 21(3), Pages 289-298,
- Sarrebruck, Allemagne, 2002.
-
- \param cMo : Computed pose.
-*/
-void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix & cMo)
-{
- try
- {
- double residu_1 = 1e8 ;
- double r =1e8-1;
- // we stop the minimization when the error is bellow 1e-8
-
- vpMatrix L;
- vpColVector err;
- vpColVector v ;
-
- unsigned int iter = 0;
-
- while((int)((residu_1 - r)*1e12) != 0 )
- {
- residu_1 = r ;
-
- // Compute the interaction matrix and the error
- error_and_interaction(cMo,err,L);
-
- // compute the residual
- r = err.sumSquare() ;
-
- // compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- unsigned int rank = L.pseudoInverse(Lp,1e-16) ;
-
- if(rank < 6){
- if(verbose)
- vpTRACE("Rank must be at least 6 ! cMo not computed.");
-
- break;
- }
-
- // compute the VVS control law
- v = -lambda*Lp*err ;
-
- cMo = vpExponentialMap::direct(v).inverse()*cMo ;
- if (iter++>vvsIterMax){
- vpTRACE("Max iteration reached") ;
- break ;
- }
- }
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
-
- }
- catch(...)
- {
- vpERROR_TRACE("vpPoseFeatures::computePoseVVS") ;
- throw ;
- }
-}
-
-
-/*!
- Compute the pose thanks to the robust virtual visual servoing approach described in \cite Comport06b.
-
- \param cMo : Computed pose.
-*/
-void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix & cMo)
-{
- try{
- double residu_1 = 1e8 ;
- double r =1e8-1;
-
- // we stop the minimization when the error is bellow 1e-8
- vpMatrix L, W;
- vpColVector w, res;
- vpColVector v ;
- vpColVector error ; // error vector
-
- vpRobust robust(2*totalSize) ;
- robust.setThreshold(0.0000) ;
-
- unsigned int iter = 0 ;
-
- while((int)((residu_1 - r)*1e12) !=0)
- {
- residu_1 = r ;
-
- // Compute the interaction matrix and the error
- error_and_interaction(cMo,error,L);
-
- // compute the residual
- r = error.sumSquare() ;
-
- if(iter == 0){
- res = vpColVector(error.getRows()/2);
- W = vpMatrix(error.getRows(),error.getRows());
- w = vpColVector(error.getRows()/2);
- w = 1;
- }
-
- for(unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
- }
- robust.setIteration(0);
- robust.MEstimator(vpRobust::TUKEY, res, w);
-
- // compute the pseudo inverse of the interaction matrix
- for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- W[2*k][2*k] = w[k] ;
- W[2*k+1][2*k+1] = w[k] ;
- }
- // compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- vpMatrix LRank;
- (W*L).pseudoInverse(Lp,1e-6) ;
- unsigned int rank = L.pseudoInverse(LRank,1e-6) ;
-
- if(rank < 6){
- if(verbose)
- vpTRACE("Rank must be at least 6 ! cMo not computed.");
-
- break;
- }
-
- // compute the VVS control law
- v = -lambda*Lp*W*error ;
-
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax){
- vpTRACE("Max iteration reached") ;
- break ;
- }
- }
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
- }
- catch(...)
- {
- vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS") ;
- throw ;
- }
-}
-
-
-
-
-
-
-
diff --git a/src/computer-vision/pose-estimation/vpPoseFeatures.h b/src/computer-vision/pose-estimation/vpPoseFeatures.h
deleted file mode 100644
index 787ed4b..0000000
--- a/src/computer-vision/pose-estimation/vpPoseFeatures.h
+++ /dev/null
@@ -1,735 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseFeatures.h 4660 2014-02-09 14:13:27Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pose computation from any features.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpPose.h
- \brief Tools for pose computation from any feature.
-
- \author Aurelien Yol
- \date June, 5 2012
-*/
-
-#ifndef vpPoseFeatures_HH
-#define vpPoseFeatures_HH
-
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpRobust.h>
-#include <visp/vpForwardProjection.h>
-#include <visp/vpPoint.h>
-#include <visp/vpCircle.h>
-#include <visp/vpSphere.h>
-#include <visp/vpLine.h>
-#include <visp/vpCylinder.h>
-
-#include <vector>
-#include <iostream>
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
-#include <tuple>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-//#################################################
-//## Call a function with a tuple as parameters
-//#################################################
-template < unsigned int N >
-struct vpDesiredFeatureBuilderWithTuple
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args &&... args )
- {
- vpDesiredFeatureBuilderWithTuple<N-1>::buildDesiredFeatureWithTuple( feature, f, t, std::get<N-1>( t ), args... );
- }
-};
-
-template <>
-struct vpDesiredFeatureBuilderWithTuple<0>
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &/* feature */,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& /* t */,
- Args&&... args )
- {
- f( args... );
- }
-};
-
-template <>
-struct vpDesiredFeatureBuilderWithTuple<1>
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args&&... args )
- {
- vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple( feature, f, t, feature, args... );
- }
-};
-
-template < typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
-void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)(ArgsFunc...),
- std::tuple<Args...> const& t )
-{
- vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple( feature, f, t );
-}
-
-//#################################################
-//## Call a function with a tuple as parameters
-//## Object Mode
-//#################################################
-
-template < unsigned int N >
-struct vpDesiredFeatureBuilderObjectWithTuple
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args &&... args )
- {
- vpDesiredFeatureBuilderObjectWithTuple<N-1>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, std::get<N-1>( t ), args... );
- }
-};
-
-template <>
-struct vpDesiredFeatureBuilderObjectWithTuple<0>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType & /*feature*/,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& /* t */,
- Args&&... args )
- {
- (obj->*f)( args... );
- }
-};
-
-template <>
-struct vpDesiredFeatureBuilderObjectWithTuple<1>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args&&... args )
- {
- vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, feature, args... );
- }
-};
-
-template < typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
-void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)(ArgsFunc...),
- std::tuple<Args...> const& t )
-{
- vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t );
-}
-
-//#####################################################
-//## Call un function with a tuple as parameters
-//## Track all the parameters with the cMo
-//## Except the first one (must be de "BasicFeature"
-//#####################################################
-
-template < unsigned int N >
-struct vpCurrentFeatureBuilderWithTuple
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsF...), std::tuple<ArgsTuple...>& t,
- ArgsDecomposed &&... args )
- {
- auto proj = std::get<N-1>( t );
- proj.track(cMo);
- vpCurrentFeatureBuilderWithTuple<N-1>::buildCurrentFeatureWithTuple( feature, cMo, f, t, proj, args... );
- }
-};
-
-template <>
-struct vpCurrentFeatureBuilderWithTuple<0>
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &/*feature*/,
- const vpHomogeneousMatrix & /*cMo*/,
- RetType (*f)(ArgsF...),
- std::tuple<ArgsTuple...>&,
- ArgsDecomposed &&... args )
- {
- f( args... );
- }
-};
-
-template <>
-struct vpCurrentFeatureBuilderWithTuple<1>
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsF...),
- std::tuple<ArgsTuple...>&t,
- ArgsDecomposed &&... args )
- {
- vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple( feature, cMo, f, t, feature, args... );
- }
-};
-
-template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
-void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsFunc...),
- std::tuple<ArgsTuple...> &t )
-{
- vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple( feature, cMo, f, t );
-}
-
-//#####################################################
-//## Call un function with a tuple as parameters
-//## Track all the parameters with the cMo
-//## Except the first one (must be de "BasicFeature"
-//## Object Mode
-//#####################################################
-
-template < unsigned int N >
-struct vpCurrentFeatureBuilderObjectWithTuple
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>& t,
- ArgsDecomposed &&... args )
- {
- auto proj = std::get<N-1>( t );
- proj.track(cMo);
- vpCurrentFeatureBuilderObjectWithTuple<N-1>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, proj, args... );
- }
-};
-
-template <>
-struct vpCurrentFeatureBuilderObjectWithTuple<0>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &/*feature*/,
- const vpHomogeneousMatrix &/*cMo*/,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>&,
- ArgsDecomposed &&... args )
- {
- (obj->*f)( args... );
- }
-};
-
-template <>
-struct vpCurrentFeatureBuilderObjectWithTuple<1>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>&t,
- ArgsDecomposed &&... args )
- {
- vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, feature, args... );
- }
-};
-
-template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
-void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsFunc...),
- std::tuple<ArgsTuple...> &t )
-{
- vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t );
-}
-
-//#################################################
-//## Call that will be used in our vpPoseFeatures
-//## to store the specific features.
-//#################################################
-/*!
- \class vpPoseSpecificFeature
- \ingroup Pose
- \brief Class used to define specific features that could be considered in pose estimation from visual features implemented in vpPoseFeatures.
-*/
-class VISP_EXPORT vpPoseSpecificFeature
-{
-public:
- vpPoseSpecificFeature(){}
- virtual ~vpPoseSpecificFeature(){};
-
- virtual vpColVector error() = 0;
- virtual vpMatrix currentInteraction() = 0;
- virtual void createDesired() = 0;
- virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
-};
-
-//#################################################
-//## Template for all kind of specific features
-//#################################################
-
-/*!
- \class vpPoseSpecificFeatureTemplate
- \ingroup Pose
- \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
-*/
-template< typename featureType, typename RetType, typename ...Args >
-class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
-{
-private:
- featureType desiredFeature;
- featureType currentFeature;
- std::tuple<Args...> *tuple;
- RetType (*func_ptr)(Args...);
-
-public:
- vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
- {
- func_ptr = f_ptr; //std::move(f_ptr);
- tuple = new std::tuple<Args...>(args...);
- }
-
- virtual ~vpPoseSpecificFeatureTemplate()
- {
- delete tuple;
- };
-
- virtual void createDesired(){
- buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple);
- }
-
- virtual vpColVector error(){
- //std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() << std::endl;
- return currentFeature.error(desiredFeature);
- }
-
- virtual vpMatrix currentInteraction(){
- return currentFeature.interaction();
- }
-
- virtual void createCurrent(const vpHomogeneousMatrix &cMo){
- buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
- }
-};
-
-//#################################################
-//## Template for all kind of specific features
-//## Object Mode
-//#################################################
-
-/*!
- \class vpPoseSpecificFeatureTemplateObject
- \ingroup Pose
- \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
-*/
-template< typename ObjectType, typename featureType, typename RetType, typename ...Args >
-class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
-{
-private:
- featureType desiredFeature;
- featureType currentFeature;
- std::tuple<Args...> *tuple;
- RetType (ObjectType::*func_ptr)(Args...);
- ObjectType* obj;
-
-public:
- vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
- {
- func_ptr = f_ptr; //std::move(f_ptr);
- tuple = new std::tuple<Args...>(args...);
- obj = o;
- }
-
- virtual ~vpPoseSpecificFeatureTemplateObject()
- {
- delete tuple;
- };
-
- virtual void createDesired(){
- buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple);
- }
-
- virtual vpColVector error(){
- return currentFeature.error(desiredFeature);
- }
-
- virtual vpMatrix currentInteraction(){
- return currentFeature.interaction();
- }
-
- virtual void createCurrent(const vpHomogeneousMatrix &cMo){
- buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
- }
-};
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-#endif //VISP_HAVE_CPP11_COMPATIBILITY
-
-/*!
- \class vpPoseFeatures
- \brief Tools for pose computation from any feature.
- \ingroup Pose
-
- This class allows to estimate a pose by virtual visual servoing from visual features. The features that are considered are points, segments, lines, ellipses.
- If the compiler is compatible with C++ 11, it is possible to introduce specific features that are not directly implemented in ViSP.
- */
-class VISP_EXPORT vpPoseFeatures
-{
-public:
- /*!
- Method that will be used to estimate the pose from visual features.
- */
- typedef enum
- {
- VIRTUAL_VS, /*!< Virtual visual servoing approach. */
- ROBUST_VIRTUAL_VS /*!< Robust virtual visual servoing approach. */
- } vpPoseFeaturesMethodType;
-
-private:
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- template<typename FeatureType, typename FirstParamType>
- struct vpDuo{
- FeatureType *desiredFeature;
- FirstParamType firstParam;
- vpDuo() : desiredFeature(NULL), firstParam() {}
- };
-
- template<typename FeatureType, typename FirstParamType, typename SecondParamType>
- struct vpTrio{
- FeatureType *desiredFeature;
- FirstParamType firstParam;
- SecondParamType secondParam;
-
- vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
- };
-#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
- unsigned int maxSize;
- unsigned int totalSize;
- unsigned int vvsIterMax;
- double lambda;
-
- bool verbose;
-
- bool computeCovariance;
- vpMatrix covarianceMatrix;
-
- //vpFeaturePoint
- std::vector<vpDuo<vpFeaturePoint,vpPoint> > featurePoint_Point_list;
- //vpFeaturePoint3D
- std::vector<vpDuo<vpFeaturePoint3D,vpPoint> > featurePoint3D_Point_list;
- //vpFeatureVanishingPoint
- std::vector<vpDuo<vpFeatureVanishingPoint,vpPoint> > featureVanishingPoint_Point_list;
- std::vector<vpTrio<vpFeatureVanishingPoint,vpLine,vpLine> > featureVanishingPoint_DuoLine_list;
- //vpFeatureEllipse
- std::vector<vpDuo<vpFeatureEllipse,vpSphere> > featureEllipse_Sphere_list;
- std::vector<vpDuo<vpFeatureEllipse,vpCircle> > featureEllipse_Circle_list;
- //vpFeatureLine
- std::vector<vpDuo<vpFeatureLine,vpLine> > featureLine_Line_list;
- std::vector<vpTrio<vpFeatureLine,vpCylinder,int> > featureLine_DuoLineInt_List;
- //vpFeatureSegment
- std::vector<vpTrio<vpFeatureSegment,vpPoint,vpPoint> > featureSegment_DuoPoints_list;
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- //Specific features
- std::vector<vpPoseSpecificFeature*> featureSpecific_list;
-#endif
-
-public:
-
- vpPoseFeatures();
- virtual ~vpPoseFeatures();
-
- // ! Features addition
- void addFeaturePoint(const vpPoint&);
-
- void addFeaturePoint3D(const vpPoint&);
-
- void addFeatureVanishingPoint(const vpPoint&);
- void addFeatureVanishingPoint(const vpLine&, const vpLine&);
-
- void addFeatureEllipse(const vpCircle&);
- void addFeatureEllipse(const vpSphere&);
-
- void addFeatureLine(const vpLine&);
- void addFeatureLine(const vpCylinder&, const int &line);
-
- void addFeatureSegment(vpPoint &, vpPoint&);
-
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- template<typename RetType, typename ...ArgsFunc, typename ...Args>
- void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args);
-
- template<typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
- void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args);
-#endif
-
- void clear();
-
- // ! Pose computation
- void computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
-
- /*!
- Get the covariance matrix of the pose parameters computed by virtual visual servoing.
-
- \warning By default, the covariance matrix is not computed. To enable the computation, use setCovarianceComputation().
- */
- vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
- }
-
- /*!
- Get the gain that is used to compute the pose with the control law \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
-
- \return Value of \f$\lambda\f$, the gain of the control law.
- */
- double getLambda(){ return lambda; }
-
- /*!
- Get the maximum number of iterations of the virtual visual servoing (VVS) scheme implemented in computePose().
-
- \return Maximum number of iterations used during VVS minimization.
- */
- unsigned int getVVSIterMax(){ return vvsIterMax; }
-
- /*!
- Enable or disable covariance computation of the pose parameters.
-
- \param flag : True if the covariance has to be computed, false otherwise.
- */
- void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
-
- /*!
- Set the gain used in the virtual visual servoing scheme : \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
-
- \param val : Value of the gain \f$\lambda\f$.
- */
- void setLambda(const double &val){ lambda = val; }
-
- /*!
- Set the maximum number of iterations used in computePose().
-
- \param val : Maximum number of iteration used in the VVS scheme.
- */
- void setVVSIterMax(const unsigned int &val){ vvsIterMax = val; }
-
- /*!
- Turn the verbose mode ON / OFF.
-
- \param mode : new verbose state. True to turn ON, false otherwise.
- */
- void setVerbose(const bool &mode){ verbose = mode; }
-
-
-private:
- void error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L);
-
- void computePoseVVS(vpHomogeneousMatrix & cMo);
- void computePoseRobustVVS(vpHomogeneousMatrix & cMo);
-};
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
-/*!
- Add a specific feature for the pose computation.
-
- \param fct_ptr : pointer on the function used to create the feature.
- \param args : List of function parameters;
- First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
- others are supposed to be derived from vpForwardProjection (redefine track() function)
-
- \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpPoseFeatures.h>
-
-void vp_createPoint(vpFeaturePoint &fp,const vpPoint &p){
- vpFeatureBuilder::create(fp,p);
-}
-
-void vp_createTwoPoint(vpFeaturePoint &fp,const vpPoint &p, const vpPoint&p2){
- vpFeatureBuilder::create(fp,p);
- vpFeatureBuilder::create(fp,p2);
-}
-
-void vp_createLine(vpFeatureLine &fp,const vpLine &l){
- vpFeatureBuilder::create(fp,l);
-}
-
-int main()
-{
- vpPoseFeatures pose;
-
- vpPoint pts[4];
- vpLine line;
-
- //... Projection of the points and line
-
- vpFeaturePoint fp;
- vpFeatureLine fl;
- void (*ptr)(vpFeaturePoint&, const vpPoint&) = &vpFeatureBuilder::create;
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- pose.addSpecificFeature(ptr, fp, pts[0]);
- pose.addSpecificFeature(&vp_createPoint, fp, pts[1]);
- pose.addSpecificFeature(&vp_createTwoPoint, fp, pts[2], pts[3]);
- pose.addSpecificFeature(&vp_createLine, fl, line);
-#endif
-
- //... Pose Computation
-
- return 0;
-}
- \endcode
-*/
-template< typename RetType, typename ...ArgsFunc, typename ...Args>
-void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args)
-{
- typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
- typedef typename std::remove_reference<featureTypeReference>::type featureType;
- featureSpecific_list.push_back(
- new vpPoseSpecificFeatureTemplate< featureType, RetType, ArgsFunc... >(fct_ptr,std::forward<ArgsFunc>(args)...)
- );
-
- featureSpecific_list.back()->createDesired();
-
- totalSize++;
- if(featureSpecific_list.size() > maxSize)
- maxSize = featureSpecific_list.size();
-}
-
-/*!
- Add a specific feature for the pose computation.
-
- \param obj : object used to call the function defined by fct_ptr.
- \param fct_ptr : pointer on the function used to create the feature.
- \param args : List of function parameters;
- First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
- others are supposed to be derived from vpForwardProjection (redefine track() function)
-
- \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpPoseFeatures.h>
-
-class vp_createClass{
-public:
- vp_createClass(){}
-
- int vp_createPoint(vpFeaturePoint &fp,const vpPoint &p){
- vpFeatureBuilder::create(fp,p);
- return 2;
- }
-
- void vp_createTwoPoint(vpFeaturePoint &fp,const vpPoint &p, const vpPoint &p2){
- vpFeatureBuilder::create(fp,p);
- vpFeatureBuilder::create(fp,p2);
- }
-
- void vp_createLine(vpFeatureLine &fp,const vpLine &l){
- vpFeatureBuilder::create(fp,l);
- }
-};
-
-int main()
-{
- vpPoseFeatures pose;
-
- vpPoint pts[3];
- vpLine line;
-
- //... Projection of the points and line
-
- vpFeaturePoint fp;
- vpFeatureLine fl;
-
- vp_createClass cpClass;
- int (vp_createClass::*ptrClassPoint)(vpFeaturePoint&, const vpPoint&) = &vp_createClass::vp_createPoint;
- void (vp_createClass::*ptrClassTwoPoint)(vpFeaturePoint&, const vpPoint&, const vpPoint&) = &vp_createClass::vp_createTwoPoint;
- void (vp_createClass::*ptrClassLine)(vpFeatureLine &, const vpLine &) = &vp_createClass::vp_createLine;
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- pose.addSpecificFeature(&cpClass, ptrClassPoint, fp, pts[0]);
- pose.addSpecificFeature(&cpClass, ptrClassTwoPoint, fp, pts[1], pts[2]);
- pose.addSpecificFeature(&cpClass, ptrClassLine, fl, line);
-#endif
-
- //... Pose Computation
-
- return 0;
-}
- \endcode
-*/
-template< typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
-void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args)
-{
- typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
- typedef typename std::remove_reference<featureTypeReference>::type featureType;
- featureSpecific_list.push_back(
- new vpPoseSpecificFeatureTemplateObject< ObjType, featureType, RetType, ArgsFunc... >(obj, fct_ptr,std::forward<ArgsFunc>(args)...)
- );
-
- featureSpecific_list.back()->createDesired();
-
- totalSize++;
- if(featureSpecific_list.size() > maxSize)
- maxSize = featureSpecific_list.size();
-}
-#endif
-
-
-#endif
diff --git a/src/computer-vision/pose-estimation/vpPoseLagrange.cpp b/src/computer-vision/pose-estimation/vpPoseLagrange.cpp
deleted file mode 100644
index 8491fae..0000000
--- a/src/computer-vision/pose-estimation/vpPoseLagrange.cpp
+++ /dev/null
@@ -1,607 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpPoseLagrange.cpp 5198 2015-01-23 17:32:04Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Pose computation.
-*
-* Authors:
-* Eric Marchand
-* Francois Chaumette
-*
-*****************************************************************************/
-
-
-#include <visp/vpPose.h>
-
-#define DEBUG_LEVEL1 0
-#define DEBUG_LEVEL2 0
-
-/**********************************************************************/
-/* FONCTION : CalculTranslation */
-/* ROLE : Calcul de la translation entre la */
-/* camera et l'outil connaissant la */
-/* rotation */
-/**********************************************************************/
-
-static
-void
-calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
- unsigned int nc3, vpColVector &x1, vpColVector &x2)
-{
-
- try
- {
- unsigned int i,j;
-
- vpMatrix ct(3,nl) ;
- for (i=0 ; i < 3 ; i++)
- {
- for (j=0 ; j < nl ; j++)
- ct[i][j] = b[j][i+nc3] ;
- }
-
- vpMatrix c ;
- c = ct.t() ;
-
- vpMatrix ctc ;
- ctc = ct*c ;
-
- vpMatrix ctc1 ; // (C^T C)^(-1)
- ctc1 = ctc.inverseByLU() ;
-
- vpMatrix cta ;
- vpMatrix ctb ;
- cta = ct*a ; /* C^T A */
- ctb = ct*b ; /* C^T B */
-
-#if (DEBUG_LEVEL2)
- {
- std::cout <<"ctc " << std::endl << ctc ;
- std::cout <<"cta " << std::endl << cta ;
- std::cout <<"ctb " << std::endl << ctb ;
- }
-#endif
-
- vpColVector X2(nc3) ;
- vpMatrix CTB(nc1,nc3) ;
- for (i=0 ; i < nc1 ; i++)
- {
- for (j=0 ; j < nc3 ; j++)
- CTB[i][j] = ctb[i][j] ;
- }
-
- for (j=0 ; j < nc3 ; j++)
- X2[j] = x2[j] ;
-
- vpColVector sv ; // C^T A X1 + C^T B X2)
- sv = cta*x1 + CTB*X2 ;// C^T A X1 + C^T B X2)
-
-#if (DEBUG_LEVEL2)
- std::cout << "sv " << sv.t() ;
-#endif
-
- vpColVector X3 ; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
- X3 = -ctc1*sv ;
-
-#if (DEBUG_LEVEL2)
- std::cout << "x3 " << X3.t() ;
-#endif
-
- for (i=0 ; i < nc1 ; i++)
- x2[i+nc3] = X3[i] ;
- }
- catch(...)
- {
-
- // en fait il y a des dizaines de raisons qui font que cette fonction
- // rende une erreur (matrice pas inversible, pb de memoire etc...)
- vpERROR_TRACE(" ") ;
- throw ;
- }
-
-
-}
-
-
-//*********************************************************************
-// 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
-//*********************************************************************
-
-//#define EPS 1.e-5
-
-static
-void
-lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
-#endif
-
- try{
- unsigned int i,imin;
-
- vpMatrix ata ; // A^T A
- ata = a.t()*a ;
- vpMatrix btb ; // B^T B
- btb = b.t()*b ;
-
- vpMatrix bta ; // B^T A
- bta = b.t()*a ;
-
- vpMatrix btb1 ; // (B^T B)^(-1)
-
- if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
- else btb1 = btb.pseudoInverse();
-
-#if (DEBUG_LEVEL1)
- {
- std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
- std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
- }
-#endif
-
- vpMatrix r ; // (B^T B)^(-1) B^T A
- r = btb1*bta ;
-
- vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
- e = - (a.t()*b) *r ;
-
- e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
-
-#if (DEBUG_LEVEL1)
- {
- std::cout << " E :" << std::endl << e << std::endl;
- }
-#endif
-
- // vpColVector sv ;
- // vpMatrix v ;
- e.svd(x1,ata) ;// destructif sur e
- // calcul du vecteur propre de E correspondant a la valeur propre min.
-
- /* calcul de SVmax */
- imin = 0;
- // FC : Pourquoi calculer SVmax ??????
- // double svm = 0.0;
- // for (i=0;i<x1.getRows();i++)
- // {
- // if (x1[i] > svm) { svm = x1[i]; imin = i; }
- // }
- // svm *= EPS; /* pour le rang */
-
- for (i=0;i<x1.getRows();i++)
- if (x1[i] < x1[imin]) imin = i;
-
-#if (DEBUG_LEVEL1)
- {
- printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
- std::cout << " i_min " << imin << std::endl;
- }
-#endif
- for (i=0;i<x1.getRows();i++)
- x1[i] = ata[i][imin];
-
- x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
-
-#if (DEBUG_LEVEL1)
- {
- std::cout << " X1 : " << x1.t() << std::endl;
- std::cout << " V : " << std::endl << ata << std::endl;
- }
-#endif
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-#if (DEBUG_LEVEL1)
- std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
-#endif
-}
-
-//#undef EPS
-
-/*!
-\brief Compute the pose of a planar object using Lagrange approach.
-
-\param cMo : Estimated pose. No initialisation is requested to estimate cMo.
-\param coplanar_plane_type : Type of coplanar plane:
- 1: if plane x=cst
- 2: if plane y=cst
- 3: if plane z=cst
- 0: any other plane
-*/
-void
-vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
-{
-
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
-#endif
- try
- {
- double s;
- unsigned int i;
-
- unsigned int k=0;
- unsigned int nl=npt*2;
-
-
- vpMatrix a(nl,3) ;
- vpMatrix b(nl,6);
- vpPoint P ;
- i=0 ;
-
- if (coplanar_plane_type == 1) { // plane ax=d
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oY();
- a[k][1] = 0.0;
- a[k][2] = P.get_oY()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oY();
- a[k+1][2] = P.get_oY()*P.get_y();
-
- b[k][0] = -P.get_oZ();
- b[k][1] = 0.0;
- b[k][2] = P.get_oZ()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oZ();
- b[k+1][2] = P.get_oZ()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
-
- k += 2;
- }
-
- }
- else if (coplanar_plane_type == 2) { // plane by=d
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
-
- b[k][0] = -P.get_oZ();
- b[k][1] = 0.0;
- b[k][2] = P.get_oZ()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oZ();
- b[k+1][2] = P.get_oZ()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
-
- k += 2;
- }
-
- }
- else { // plane cz=d or any other
-
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
-
- b[k][0] = -P.get_oY();
- b[k][1] = 0.0;
- b[k][2] = P.get_oY()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oY();
- b[k+1][2] = P.get_oY()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
-
- k += 2;
- }
- }
- vpColVector X1(3) ;
- vpColVector X2(6) ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout <<"a " << a << std::endl ;
- std::cout <<"b " << b << std::endl ;
- }
-#endif
-
- lagrange(a,b,X1,X2);
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
- std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
- }
-#endif
-
- if (X2[5] < 0.0)
- { /* car Zo > 0 */
- for (i=0;i<3;i++) X1[i] = -X1[i];
- for (i=0;i<6;i++) X2[i] = -X2[i];
- }
- s = 0.0;
- for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
- for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
-
- //s = 0.0;
- //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
- s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
-
- if (s<1e-10)
- {
-// std::cout << "Points that produce an error: " << std::endl;
-// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
-// {
-// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
-// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
-// }
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Lagrange pose computation (planar plane case)")) ;
- }
-
- s = 1.0/sqrt(s);
- for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
-
-
- calculTranslation (a, b, nl, 3, 3, X1, X2) ;
-
- // if (err != OK)
- {
- // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
- // PrintError(err) ;
- // return err ;
- }
-
- if (coplanar_plane_type == 1) { // plane ax=d
- cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
-
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
- cMo[i][1] = X1[i];
- cMo[i][2] = X2[i];
- cMo[i][3] = X2[i+3];
- }
-
- }
- else if (coplanar_plane_type == 2) { // plane by=d
- cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
-
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
- cMo[i][0] = X1[i];
- cMo[i][2] = X2[i];
- cMo[i][3] = X2[i+3];
- }
- }
- else { // plane cz=d or any other
-
- cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
-
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
- cMo[i][0] = X1[i];
- cMo[i][1] = X2[i];
- cMo[i][3] = X2[i+3];
- }
- }
- }
- catch(vpException &e)
- {
- throw e;
- }
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
-#endif
- // return(OK);
-}
-
-
-void
-vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
-{
-
-#if (DEBUG_LEVEL1)
- std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
-#endif
- try{
- double s;
- unsigned int i;
-
- unsigned int k=0;
- unsigned int nl=npt*2;
-
- vpMatrix a(nl,3) ;
- vpMatrix b(nl,9);
- b =0 ;
-
- vpPoint P ;
- i=0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
-
- b[k][0] = -P.get_oY();
- b[k][1] = 0.0;
- b[k][2] = P.get_oY()*P.get_x();
-
- b[k][3] = -P.get_oZ();
- b[k][4] = 0.0;
- b[k][5] = P.get_oZ()*P.get_x();
-
- b[k][6] = -1.0;
- b[k][7] = 0.0;
- b[k][8] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oY();
- b[k+1][2] = P.get_oY()*P.get_y();
-
- b[k+1][3] = 0.0;
- b[k+1][4] = -P.get_oZ();
- b[k+1][5] = P.get_oZ()*P.get_y();
-
- b[k+1][6] = 0.0;
- b[k+1][7] = -1.0;
- b[k+1][8] = P.get_y();
-
- k += 2;
- }
- vpColVector X1(3) ;
- vpColVector X2(9) ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout <<"a " << a << std::endl ;
- std::cout <<"b " << b << std::endl ;
- }
-#endif
-
- lagrange(a,b,X1,X2);
- // if (err != OK)
- {
- // std::cout << "in (CLagrange.cc)Lagrange returns " ;
- // PrintError(err) ;
- // return err ;
- }
-
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
- std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
- }
-#endif
-
- if (X2[8] < 0.0)
- { /* car Zo > 0 */
- X1 *= -1 ;
- X2 *= -1 ;
- }
- s = 0.0;
- for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
- for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
-
- //s = 0.0;
- //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
- s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
-
- if (s<1e-10)
- {
-// std::cout << "Points that produce an error: " << std::endl;
-// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
-// {
-// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
-// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
-// }
- //vpERROR_TRACE(" division par zero " ) ;
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Lagrange pose computation (non planar plane case)")) ;
- }
-
- s = 1.0/sqrt(s);
- for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
-
- X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
-
- calculTranslation (a, b, nl, 3, 6, X1, X2) ;
-
- for (i=0 ; i<3 ; i++)
- {
- cMo[i][0] = X1[i];
- cMo[i][1] = X2[i];
- cMo[i][2] = X2[i+3];
- cMo[i][3] = X2[i+6];
- }
-
- }
- catch(vpException &e)
- {
- throw e;
- }
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
-#endif
-}
-
-
-#undef DEBUG_LEVEL1
-#undef DEBUG_LEVEL2
diff --git a/src/computer-vision/pose-estimation/vpPoseLowe.cpp b/src/computer-vision/pose-estimation/vpPoseLowe.cpp
deleted file mode 100644
index 0e08327..0000000
--- a/src/computer-vision/pose-estimation/vpPoseLowe.cpp
+++ /dev/null
@@ -1,374 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpPoseLowe.cpp 5107 2015-01-05 07:47:13Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Pose computation.
-*
-* Authors:
-* Eric Marchand
-* Francois Chaumette
-*
-*****************************************************************************/
-
-#include <math.h>
-#include <float.h>
-#include <string.h>
-#include <limits> // numeric_limits
-
-// besoin de la librairie mathematique, en particulier des
-// fonctions de minimisation de Levenberg Marquartd
-#include <visp/vpLevenbergMarquartd.h>
-#include <visp/vpPose.h>
-
-#define NBR_PAR 6
-#define X3_SIZE 3
-#define MINIMUM 0.000001
-
-#define DEBUG_LEVEL1 0
-
-// ------------------------------------------------------------------------
-// FONCTION LOWE :
-// ------------------------------------------------------------------------
-// Calcul de la pose pour un objet 3D
-// ------------------------------------------------------------------------
-
-/*
-* MACRO : MIJ
-*
-* 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 :
-* 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 :
-* L'adresse de m[i][j] est retournee.
-*
-* HISTORIQUE :
-* 1.00 - 11/02/93 - Original.
-*/
-#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
-#define NBPTMAX 50
-
-// Je hurle d'horreur devant ces variable globale...
-static double XI[NBPTMAX],YI[NBPTMAX];
-static double XO[NBPTMAX],YO[NBPTMAX],ZO[NBPTMAX];
-
-
-#define MINI 0.001
-#define MINIMUM 0.000001
-
-void eval_function(int npt,double *xc,double *f);
-void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
-
-void eval_function(int npt,double *xc,double *f)
-{
- int i;
- double x,y,z,u[3];
-
- u[0] = xc[3]; /* Rx */
- u[1] = xc[4]; /* Ry */
- u[2] = xc[5]; /* Rz */
-
- vpRotationMatrix rd(u[0],u[1],u[2]) ;
- // rot_mat(u,rd); /* matrice de rotation correspondante */
- for (i=0;i<npt;i++)
- {
- x = rd[0][0]*XO[i] + rd[0][1]*YO[i] + rd[0][2]*ZO[i] + xc[0];
- y = rd[1][0]*XO[i] + rd[1][1]*YO[i] + rd[1][2]*ZO[i] + xc[1];
- z = rd[2][0]*XO[i] + rd[2][1]*YO[i] + rd[2][2]*ZO[i] + xc[2];
- f[i] = x/z - XI[i];
- f[npt+i] = y/z - YI[i];
- // std::cout << f[i] << " " << f[i+1] << std::endl ;
- }
-}
-
-
-/*
-* 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.
-*
-* SORTIE :
-* jac Jacobien de la fonction.
-*
-* 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.
-* Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
-* stocke dans "fjac" et "fvecc" reste inchange.
-*
-* HISTORIQUE :
-* 1.00 - xx/xx/xx - Original.
-* 1.01 - 06/07/95 - Modifications.
-* 2.00 - 24/10/95 - Tableau jac monodimensionnel.
-*/
-void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
-{
- double u[X3_SIZE], rx, ry, rz;// rd[X3_SIZE][X3_SIZE],
- double tt, mco, co, si, u1, u2, u3, x, y, z;
- double drxt, drxu1, drxu2, drxu3;
- double dryt, dryu1, dryu2, dryu3;
- double drzt, drzu1, drzu2, drzu3;
- double dxit, dxiu1, dxiu2, dxiu3;
- double dyit, dyiu1, dyiu2, dyiu3;
-
- vpRotationMatrix rd ;
- int i, npt;
-
- if (m < n) printf("pas assez de points\n");
- npt = m / 2;
-
- if (iflag == 1) eval_function (npt, xc, fvecc);
- else if (iflag == 2)
- {
- u[0] =xc[3];
- u[1]= xc[4];
- u[2]= xc[5];
-
- rd.buildFrom(u[0],u[1],u[2]) ;
- /* a partir de l'axe de rotation, calcul de la matrice de rotation. */
- // rot_mat(u, rd);
-
- tt = sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */
- if (tt >= MINIMUM)
- {
- u1 = u[0] / tt;
- u2 = u[1] / tt; /* axe de rotation unitaire */
- u3 = u[2] / tt;
- }
- else u1 = u2 = u3 = 0.0;
- co = cos(tt);
- mco = 1.0 - co;
- si = sin(tt);
-
- for (i = 0; i < npt; i++)
- {
- x = XO[i];
- y = YO[i]; /* coordonnees du point i */
- z = ZO[i];
-
- /* coordonnees du point i dans le repere camera */
- rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
- ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
- rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
-
- /* derive des fonctions rx, ry et rz par rapport
- * a tt, u1, u2, u3.
- */
- drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y
- + (si * u1 * u1 - si) * x;
- drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
- drxu2 = si * z + mco * u1 * y;
- drxu3 = mco * u1 * z - si * y;
-
- dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y
- + (co * u3 + si * u1 * u2) * x;
- dryu1 = mco * u2 * x - si * z;
- dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
- dryu3 = mco * u2 * z + si * x;
-
- drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y
- + (si * u1 * u3 - co * u2) * x;
- drzu1 = si * y + mco * u3 * x;
- drzu2 = mco * u3 * y - si * x;
- drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
-
- /* derive de la fonction representant le modele de la
- * camera (sans distortion) par rapport a tt, u1, u2 et u3.
- */
- dxit = drxt / rz - rx * drzt / (rz * rz);
-
- dyit = dryt / rz - ry * drzt / (rz * rz);
-
- dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
- dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
-
- dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
- dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
-
- dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
- dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
-
- /* calcul du jacobien : le jacobien represente la
- * derivee de la fonction representant le modele de la
- * camera par rapport aux parametres.
- */
- *MIJ(jac, 0, i, ldfjac) = 1 / rz;
- *MIJ(jac, 1, i, ldfjac) = 0.0;
- *MIJ(jac, 2, i, ldfjac) = - rx / (rz * rz);
- if (tt >= MINIMUM)
- {
- *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt
- - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
- *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt
- + (1 - u2 * u2) * dxiu2 / tt- u2 * u3 * dxiu3 / tt;
-
- *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt
- + (1 - u3 * u3) * dxiu3 / tt;
- }
- else
- {
- *MIJ(jac, 3, i, ldfjac) = 0.0;
- *MIJ(jac, 4, i, ldfjac) = 0.0;
- *MIJ(jac, 5, i, ldfjac) = 0.0;
- }
- *MIJ(jac, 0, npt + i, ldfjac) = 0.0;
- *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
- *MIJ(jac, 2, npt + i, ldfjac) = - ry / (rz * rz);
- if (tt >= MINIMUM)
- {
- *MIJ(jac, 3, npt + i, ldfjac) = u1 * dyit + (1 - u1 * u1) * dyiu1 / tt
- - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
- *MIJ(jac, 4, npt + i, ldfjac) = u2 * dyit - u1 * u2 * dyiu1 / tt
- + (1 - u2 * u2) * dyiu2 / tt- u2 * u3 * dyiu3 / tt;
- *MIJ(jac, 5, npt + i, ldfjac) = u3 * dyit - u1 * u3 * dyiu1 / tt
- - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
- }
- else
- {
- *MIJ(jac, 3, npt + i, ldfjac) = 0.0;
- *MIJ(jac, 4, npt + i, ldfjac) = 0.0;
- *MIJ(jac, 5, npt + i, ldfjac) = 0.0;
- }
- }
- } /* fin else if iflag ==2 */
-}
-
-/*!
-\brief Compute the pose using the Lowe non linear approach
-it consider the minimization of a residual using
-the levenberg marquartd approach.
-
-The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
-
-*/
-void
-vpPose::poseLowe(vpHomogeneousMatrix & cMo)
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
-#endif
- int n, m; /* nombre d'elements dans la matrice jac */
- int lwa; /* taille du vecteur wa */
- int ldfjac; /* taille maximum d'une ligne de jac */
- int info, ipvt[NBR_PAR];
- int tst_lmder;
- double f[2 * NBPTMAX], sol[NBR_PAR];
- double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
- // double 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<double>::epsilon(); /* critere d'arret */
-
- // c = cam ;
- // for (i=0;i<3;i++)
- // for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
- // mat_rot(rd,u);
- vpRotationMatrix cRo ;
- cMo.extract(cRo) ;
- vpThetaUVector u(cRo) ;
- for (unsigned int i=0;i<3;i++)
- {
- sol[i] = cMo[i][3];
- sol[i+3] = u[i];
- }
-
- vpPoint P ;
- unsigned int i_=0;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- XI[i_] = P.get_x();//*cam.px + cam.xc ;
- YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
- XO[i_] = P.get_oX();
- YO[i_] = P.get_oY();
- ZO[i_] = P.get_oZ();
- ++i_;
- }
- 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";
- // return FATAL_ERROR ;
- }
-
- for (unsigned int i = 0; i < 3; i++)
- u[i] = sol[i + 3];
-
- for (unsigned int i=0;i<3;i++)
- {
- cMo[i][3] = sol[i];
- u[i] = sol[i+3];
- }
-
- vpRotationMatrix rd(u) ;
- cMo.insert(rd) ;
- // rot_mat(u,rd);
- // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
-
-#if (DEBUG_LEVEL1)
- std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
-#endif
- // return OK ;
-}
-
-
-#undef MINI
-#undef MINIMUM
-
-
-#undef DEBUG_LEVEL1
-
-
-/*
-* Local variables:
-* c-basic-offset: 2
-* End:
-*/
diff --git a/src/computer-vision/pose-estimation/vpPoseRansac.cpp b/src/computer-vision/pose-estimation/vpPoseRansac.cpp
deleted file mode 100644
index 9e74a48..0000000
--- a/src/computer-vision/pose-estimation/vpPoseRansac.cpp
+++ /dev/null
@@ -1,857 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseRansac.cpp 5211 2015-01-26 17:44:35Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pose computation.
- *
- * Authors:
- * Eric Marchand
- * Aurelien Yol
- * Souriya Trinh
- *
- *****************************************************************************/
-
-
-/*!
- \file vpPoseRansac.cpp
- \brief function used to estimate a pose using the Ransac algorithm
-*/
-
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <stdlib.h>
-#include <algorithm> // std::count
-#include <float.h> //DBL_MAX
-
-#include <visp/vpPose.h>
-#include <visp/vpColVector.h>
-#include <visp/vpRansac.h>
-#include <visp/vpTime.h>
-#include <visp/vpList.h>
-#include <visp/vpPoseException.h>
-
-#define eps 1e-6
-
-
-/*!
- Compute the pose using the Ransac approach.
-
- \param cMo : Computed pose
- \param func : Pointer to a function that takes in parameter a vpHomogeneousMatrix
- and returns true if the pose check is OK or false otherwise
- \return True if we found at least 4 points with a reprojection error below ransacThreshold.
-*/
-bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *))
-{
- srand(0); //Fix seed here so we will have the same pseudo-random series at each run.
- std::vector<unsigned int> best_consensus;
- std::vector<unsigned int> cur_consensus;
- std::vector<unsigned int> cur_outliers;
- std::vector<unsigned int> cur_randoms;
- unsigned int size = (unsigned int)listP.size();
- int nbTrials = 0;
- unsigned int nbMinRandom = 4 ;
- unsigned int nbInliers = 0;
- double r, r_lagrange, r_dementhon;
-
- vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
-
- if (size < 4) {
- //vpERROR_TRACE("Not enough point to compute the pose");
- throw(vpPoseException(vpPoseException::notInitializedError,
- "Not enough point to compute the pose")) ;
- }
-
- bool foundSolution = false;
-
- while (nbTrials < ransacMaxTrials && nbInliers < (unsigned)ransacNbInlierConsensus)
- {
- //Hold the list of the index of the inliers (points in the consensus set)
- cur_consensus.clear();
-
- //Use a temporary variable because if not, the cMo passed in parameters will be modified when
- // we compute the pose for the minimal sample sets but if the pose is not correct when we pass
- // a function pointer we do not want to modify the cMo passed in parameters
- vpHomogeneousMatrix cMo_tmp;
- cur_outliers.clear();
- cur_randoms.clear();
-
- //Vector of used points, initialized at false for all points
- std::vector<bool> usedPt(size, false);
-
- vpPose poseMin;
- for(unsigned int i = 0; i < nbMinRandom;)
- {
- if((size_t) std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
- //All points was picked once, break otherwise we stay in an infinite loop
- break;
- }
-
- //Pick a point randomly
- unsigned int r_ = (unsigned int) rand() % size;
- while(usedPt[r_]) {
- //If already picked, pick another point randomly
- r_ = (unsigned int) rand() % size;
- }
- //Mark this point as already picked
- usedPt[r_] = true;
-
- std::list<vpPoint>::const_iterator iter = listP.begin();
- std::advance(iter, r_);
- vpPoint pt = *iter;
-
- bool degenerate = false;
- for(std::list<vpPoint>::const_iterator it = poseMin.listP.begin(); it != poseMin.listP.end(); ++it){
- vpPoint ptdeg = *it;
- if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
- ((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
- degenerate = true;
- break;
- }
- }
- if(!degenerate) {
- poseMin.addPoint(pt);
- cur_randoms.push_back(r_);
- //Increment the number of points picked
- i++;
- }
- }
-
- if(poseMin.npt < nbMinRandom) {
- nbTrials++;
- continue;
- }
-
- //Flags set if pose computation is OK
- bool is_valid_lagrange = false;
- bool is_valid_dementhon = false;
-
- //Set maximum value for residuals
- r_lagrange = DBL_MAX;
- r_dementhon = DBL_MAX;
-
- try {
- poseMin.computePose(vpPose::LAGRANGE, cMo_lagrange);
- r_lagrange = poseMin.computeResidual(cMo_lagrange);
- is_valid_lagrange = true;
- } catch(/*vpException &e*/...) {
-// std::cerr << e.what() << std::endl;
- }
-
- try {
- poseMin.computePose(vpPose::DEMENTHON, cMo_dementhon);
- r_dementhon = poseMin.computeResidual(cMo_dementhon);
- is_valid_dementhon = true;
- } catch(/*vpException &e*/...) {
-// std::cerr << e.what() << std::endl;
- }
-
- //If at least one pose computation is OK,
- //we can continue, otherwise pick another random set
- if(is_valid_lagrange || is_valid_dementhon) {
- if (r_lagrange < r_dementhon) {
- r = r_lagrange;
-// cMo = cMo_lagrange;
- cMo_tmp = cMo_lagrange;
- }
- else {
- r = r_dementhon;
-// cMo = cMo_dementhon;
- cMo_tmp = cMo_dementhon;
- }
- r = sqrt(r) / (double) nbMinRandom;
-
- //Filter the pose using some criterion (orientation angles, translations, etc.)
- bool isPoseValid = true;
- if(func != NULL) {
- isPoseValid = func(&cMo_tmp);
- if(isPoseValid) {
- cMo = cMo_tmp;
- }
- } else {
- //No post filtering on pose, so copy cMo_temp to cMo
- cMo = cMo_tmp;
- }
-
- if (isPoseValid && r < ransacThreshold)
- {
- unsigned int nbInliersCur = 0;
- unsigned int iter = 0;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- vpPoint pt = *it;
- vpPoint p(pt) ;
- p.track(cMo) ;
-
- double d = vpMath::sqr(p.get_x() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.get_y()) ;
- double error = sqrt(d) ;
- if(error < ransacThreshold) {
- // the point is considered as inlier if the error is below the threshold
- // But, we need also to check if it is not a degenerate point
- bool degenerate = false;
-
- for(unsigned int it_inlier_index = 0; it_inlier_index< cur_consensus.size(); it_inlier_index++){
- std::list<vpPoint>::const_iterator it_point = listP.begin();
- std::advance(it_point, cur_consensus[it_inlier_index]);
- pt = *it_point;
-
- vpPoint ptdeg = *it;
- if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
- ((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
- degenerate = true;
- break;
- }
- }
-
- if(!degenerate) {
- nbInliersCur++;
- cur_consensus.push_back(iter);
- }
- else {
- cur_outliers.push_back(iter);
- }
- }
- else {
- cur_outliers.push_back(iter);
- }
-
- iter++;
- }
-
- if(nbInliersCur > nbInliers)
- {
- foundSolution = true;
- best_consensus = cur_consensus;
- nbInliers = nbInliersCur;
- }
-
- nbTrials++;
-
- if(nbTrials >= ransacMaxTrials) {
- vpERROR_TRACE("Ransac reached the maximum number of trials");
- foundSolution = true;
- }
- }
- else {
- nbTrials++;
-
- if(nbTrials >= ransacMaxTrials) {
- vpERROR_TRACE("Ransac reached the maximum number of trials");
- }
- }
- } else {
- nbTrials++;
-
- if(nbTrials >= ransacMaxTrials) {
- vpERROR_TRACE("Ransac reached the maximum number of trials");
- }
- }
- }
-
- if(foundSolution) {
- //std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
-
- //Display the random picked points
- /*
- std::cout << "Randoms : ";
- for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
- std::cout << cur_randoms[i] << " ";
- std::cout << std::endl;
- */
-
- //Display the outliers
- /*
- std::cout << "Outliers : ";
- for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
- std::cout << cur_outliers[i] << " ";
- std::cout << std::endl;
- */
-
- //Even if the cardinality of the best consensus set is inferior to ransacNbInlierConsensus,
- //we want to refine the solution with data in best_consensus and return this pose.
- //This is an approach used for example in p118 in Multiple View Geometry in Computer Vision, Hartley, R.~I. and Zisserman, A.
- if(nbInliers >= nbMinRandom) //if(nbInliers >= (unsigned)ransacNbInlierConsensus)
- {
- //Refine the solution using all the points in the consensus set and with VVS pose estimation
- vpPose pose ;
- for(unsigned i = 0 ; i < best_consensus.size(); i++)
- {
- std::list<vpPoint>::const_iterator iter = listP.begin();
- std::advance(iter, best_consensus[i]);
- vpPoint pt = *iter;
-
- pose.addPoint(pt) ;
- ransacInliers.push_back(pt);
- }
-
- //Flags set if pose computation is OK
- bool is_valid_lagrange = false;
- bool is_valid_dementhon = false;
-
- //Set maximum value for residuals
- r_lagrange = DBL_MAX;
- r_dementhon = DBL_MAX;
-
- try {
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
- r_lagrange = pose.computeResidual(cMo_lagrange);
- is_valid_lagrange = true;
- } catch(/*vpException &e*/...) {
-// std::cerr << e.what() << std::endl;
- }
-
- try {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
- r_dementhon = pose.computeResidual(cMo_dementhon);
- is_valid_dementhon = true;
- } catch(/*vpException &e*/...) {
-// std::cerr << e.what() << std::endl;
- }
-
- if(is_valid_lagrange || is_valid_dementhon) {
- if (r_lagrange < r_dementhon) {
- cMo = cMo_lagrange;
- }
- else {
- cMo = cMo_dementhon;
- }
-
- pose.setCovarianceComputation(computeCovariance);
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
- if(computeCovariance) {
- covarianceMatrix = pose.covarianceMatrix;
- }
- }
- } else {
- return false;
- }
- }
-
- return foundSolution;
-}
-
-/*!
- Match a vector p2D of 2D point (x,y) and a vector p3D of 3D points
- (X,Y,Z) using the Ransac algorithm.
-
- At least numberOfInlierToReachAConsensus of true correspondance are required
- to validate the pose
-
- The inliers are given in a vector of vpPoint listInliers.
-
- The pose is returned in cMo.
-
- \param p2D : Vector of 2d points (x and y attributes are used).
- \param p3D : Vector of 3d points (oX, oY and oZ attributes are used).
- \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
- to consider a trial as correct.
- \param threshold : The maximum error allowed between the 2d points and the
- reprojection of its associated 3d points by the current pose (in meter).
- \param ninliers : Number of inliers found for the best solution.
- \param listInliers : Vector of points (2d and 3d) that are inliers for the best solution.
- \param cMo : The computed pose (best solution).
- \param maxNbTrials : Maximum number of trials before considering a solution
- fitting the required \e numberOfInlierToReachAConsensus and \e threshold
- cannot be found.
-*/
-void vpPose::findMatch(std::vector<vpPoint> &p2D,
- std::vector<vpPoint> &p3D,
- const unsigned int &numberOfInlierToReachAConsensus,
- const double &threshold,
- unsigned int &ninliers,
- std::vector<vpPoint> &listInliers,
- vpHomogeneousMatrix &cMo,
- const int &maxNbTrials )
-{
- vpPose pose;
-
- int nbPts = 0;
- for(unsigned int i = 0 ; i < p2D.size() ; i++)
- {
- for(unsigned int j = 0 ; j < p3D.size() ; j++)
- {
- vpPoint pt;
- pt.set_x(p2D[i].get_x());
- pt.set_y(p2D[i].get_y());
- pt.setWorldCoordinates(p3D[j].getWorldCoordinates());
- pose.addPoint(pt);
- nbPts++;
- }
- }
-
- if (pose.listP.size() < 4)
- {
- vpERROR_TRACE("Ransac method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",pose.listP.size()) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- else
- {
- pose.setRansacMaxTrials(maxNbTrials);
- pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
- pose.setRansacThreshold(threshold);
- pose.computePose(vpPose::RANSAC, cMo);
- ninliers = pose.getRansacNbInliers();
- listInliers = pose.getRansacInliers();
- }
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*
-\brief
-
-determines if there more than one the same point in the chosen set of point
-
-point are coded this way
-x1,y1, X1, Y1, Z1
-xnm, ynm, Xnm, Ynm, Znm
-
-leading to 5*n*m
-*/
-bool
-vpPose::degenerateConfiguration(vpColVector &x, unsigned int *ind)
-{
-
- // vpTRACE("%p %p %d",&x, ind, x.getRows()) ;
- for (int i=1 ; i < 4 ; i++)
- for (int j=0 ; j<i ; j++)
- {
- unsigned int indi = 5*ind[i] ;
- unsigned int indj = 5*ind[j] ;
-
- if ((fabs(x[indi] - x[indj]) < 1e-6) && (fabs(x[indi+1] - x[indj+1]) < 1e-6))
- { return true ; }
-
- if ((fabs(x[indi+2] - x[indj+2]) < 1e-6) && (fabs(x[indi+3] - x[indj+3]) < 1e-6) && (fabs(x[indi+4] - x[indj+4]) < 1e-6))
- { return true ; }
- }
-
- return false ;
-}
-/*!
- \deprecated Fit model to this random selection of data points. \n \n
- We chose the Dementhon algorithm to compute the pose.
-*/
-void
-vpPose::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
-{
- unsigned int i ;
-
- vpPoint p[4] ;
-
- vpPose pose ;
- pose.clearPoint() ;
- for(i=0 ; i < 4 ; i++)
- {
-
- unsigned int index = 5*ind[i] ;
-
- p[i].set_x(x[index]) ;
- p[i].set_y(x[index+1]) ;
-
- p[i].setWorldCoordinates(x[index+2],x[index+3], x[index+4]) ;
- pose.addPoint(p[i]) ;
- }
-
-
- // pose.printPoint() ;
- vpHomogeneousMatrix cMo ;
- try {
- pose.computePose(vpPose::DEMENTHON, cMo) ;
- // std::cout << cMo << std::endl ;
- }
- catch(...)
- {
- cMo.setIdentity() ;
- }
-
- M.resize(16) ;
- for (i=0 ; i <16 ; i++)
- {
- M[i] = cMo.data[i] ;
- }
-
-}
-
-
-/*!
- \deprecated Evaluate distances between points and model. \n \n
- This function can certainly be optimized...
-*/
-
-double
-vpPose::computeResidual(const vpColVector &x, const vpColVector &M, vpColVector &d)
-{
-
- unsigned int i ;
- unsigned int n = x.getRows()/5 ;
-
- vpPoint *p;
- p = new vpPoint [n] ;
- {
- // firsttime=1 ;
- for( i=0 ; i < n ; i++)
- {
- p[i].setWorldCoordinates(x[5*i+2],x[5*i+3], x[5*i+4]) ;
- }
- }
-
- vpHomogeneousMatrix cMo ;
- for (i=0 ; i <16 ; i++)
- {
- cMo.data[i] = M[i];
- }
-
-
- d.resize(n) ;
- vpColVector cP, xy ;
-
- for( i=0 ; i < n ; i++)
- {
- p[i].changeFrame(cMo,cP) ;
- p[i].projection(cP,xy) ;
- d[i] = sqrt(vpMath::sqr(x[5*i]-xy[0])+vpMath::sqr(x[5*i+1]-xy[1])) ;
- }
-
- delete [] p;
-
- return 0 ;
-}
-
-void
-vpPose::initRansac(const unsigned int n,
- const double *x, const double *y,
- const unsigned int m,
- const double *X, const double *Y, const double *Z,
- vpColVector &data)
-{
- data.resize(5*n*m) ;
- unsigned int k =0 ;
- for (unsigned int i=0 ; i < n ; i++)
- {
- for (unsigned int j=0 ; j < m ; j++)
- {
- data[k] = x[i] ;
- data[k+1] = y[i] ;
- data[k+2] = X[j] ;
- data[k+3] = Y[j] ;
- data[k+4] = Z[j] ;
-
- k+=5 ;
- }
- }
-}
-
-
-/*!
- \deprecated
- Compute the pose from a set of n 2D point (x,y) and m 3D points
- (X,Y,Z) using the Ransac algorithm. It is not assumed that
- the 2D and 3D points are registred (there is nm posibilities). \n \n
- At least numberOfInlierToReachAConsensus of true correspondance are required
- to validate the pose. \n \n
- The inliers are given in xi, yi, Xi, Yi, Zi. \n
- The pose is returned in cMo.
-
- \param n : Number of 2d points.
- \param x : Array (of size \e n) of the x coordinates of the 2d points.
- \param y : Array (of size \e n) of the y coordinates of the 2d points.
- \param m : Number of 3d points.
- \param X : Array (of size \e m) of the oX coordinates of the 3d points.
- \param Y : Array (of size \e m) of the oY coordinates of the 3d points.
- \param Z : Array (of size \e m) of the oZ coordinates of the 3d points.
- \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
- to consider a trial as correct.
- \param threshold : The maximum error allowed between the 2d points and the
- reprojection of its associated 3d points by the current pose (in meter).
- \param ninliers : Number of inliers found for the best solution.
- \param xi : Array (of size \e ninliers) of the x coordinates of the inliers.
- \param yi : Array (of size \e ninliers) of the y coordinates of the inliers.
- \param Xi : Array (of size \e ninliers) of the oX coordinates of the inliers.
- \param Yi : Array (of size \e ninliers) of the oY coordinates of the inliers.
- \param Zi : Array (of size \e ninliers) of the oZ coordinates of the inliers.
- \param cMo : The computed pose (best solution).
- \param maxNbTrials : Maximum number of trials before considering a solution
- fitting the required \e numberOfInlierToReachAConsensus and \e threshold
- cannot be found.
- */
-void
-vpPose::ransac(const unsigned int n,
- const double *x, const double *y,
- const unsigned int m,
- const double *X, const double *Y, const double *Z,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- vpColVector &xi, vpColVector &yi,
- vpColVector &Xi, vpColVector &Yi, vpColVector &Zi,
- vpHomogeneousMatrix &cMo,
- const int maxNbTrials)
-{
-
-
- double tms = vpTime::measureTimeMs() ;
- vpColVector data ;
- unsigned int i;
- vpPose::initRansac(n,x,y,m,X,Y,Z, data) ;
-
- vpColVector M(16) ;
- vpColVector inliers(n*m) ;
- vpRansac<vpPose>::ransac(n*m,data,4,
- threshold, M,inliers,
- numberOfInlierToReachAConsensus, 0.0, maxNbTrials) ;
-
-
- // we count the number of inliers
- ninliers = 0 ;
- for(i=0 ; i < n*m ; i++)
- {
- //if (inliers[i]==1)
- if (std::fabs(inliers[i]-1) <= std::fabs(vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
- {
- ninliers++ ;
- }
- }
-
- xi.resize(ninliers) ;
- yi.resize(ninliers) ;
- Xi.resize(ninliers) ;
- Yi.resize(ninliers) ;
- Zi.resize(ninliers) ;
-
- unsigned int k =0 ;
- for(i=0 ; i < n*m ; i++)
- {
- //if (inliers[i]==1)
- if (std::fabs(inliers[i]-1) <= std::fabs(vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
- {
- xi[k] = data[5*i] ;
- yi[k] = data[5*i+1] ;
- Xi[k] = data[5*i+2] ;
- Yi[k] = data[5*i+3] ;
- Zi[k] = data[5*i+4] ;
- k++ ;
- }
- }
-
- for (i=0 ; i <16 ; i++)
- {
- cMo.data[i] = M[i];
- }
-
- std::cout << vpTime::measureTimeMs() - tms << "ms" << std::endl ;
-
-}
-
-/*!
- \deprecated
- Compute the pose from a set of n 2D point (x,y) in p and m 3D points
- (X,Y,Z) in P using the Ransac algorithm. It is not assumed that
- the 2D and 3D points are registred (there is nm posibilities). \n \n
- At least numberOfInlierToReachAConsensus of true correspondance are required
- to validate the pose. \n \n
- The inliers are given in a list of vpPoint. \n
- The pose is returned in cMo.
-
- \param n : Number of 2d points.
- \param p : Array (of size n) of 2d points (x and y attributes are used).
- \param m : Number of 3d points.
- \param P : Array of size m of 3d points (oX, oY and oZ attributes are used).
- \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
- to consider a trial as correct.
- \param threshold : The maximum error allowed between the 2d points and the
- reprojection of its associated 3d points by the current pose (in meter).
- \param ninliers : Number of inliers found for the best solution.
- \param lPi : List of points (2d and 3d) that are inliers for the best solution.
- \param cMo : The computed pose (best solution).
- \param maxNbTrials : Maximum number of trials before considering a solution
- fitting the required \e numberOfInlierToReachAConsensus and \e threshold
- cannot be found.
- */
-void
-vpPose::ransac(const unsigned int n,
- const vpPoint *p,
- const unsigned int m,
- const vpPoint *P,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- std::list<vpPoint> &lPi,
- vpHomogeneousMatrix &cMo,
- const int maxNbTrials)
-{
- double *x, *y;
- x = new double [n];
- y = new double [n] ;
- for (unsigned int i=0 ; i < n ; i++)
- {
- x[i] = p[i].get_x() ;
- y[i] = p[i].get_y() ;
- }
- double *X, *Y, *Z;
- X = new double [m];
- Y = new double [m];
- Z = new double [m];
- for (unsigned int i=0 ; i < m ; i++)
- {
- X[i] = P[i].get_oX() ;
- Y[i] = P[i].get_oY() ;
- Z[i] = P[i].get_oZ() ;
- }
-
- vpColVector xi,yi,Xi,Yi,Zi ;
-
- ransac(n,x,y,
- m,X,Y,Z, numberOfInlierToReachAConsensus,
- threshold,
- ninliers,
- xi,yi,Xi,Yi,Zi,
- cMo, maxNbTrials) ;
-
- for(unsigned int i=0 ; i < ninliers ; i++)
- {
- vpPoint Pi ;
- Pi.setWorldCoordinates(Xi[i], Yi[i], Zi[i]) ;
- Pi.set_x(xi[i]) ;
- Pi.set_y(yi[i]) ;
- lPi.push_back(Pi) ;
- }
-
- delete [] x;
- delete [] y;
- delete [] X;
- delete [] Y;
- delete [] Z;
-}
-
-/*!
- \deprecated
- Compute the pose from a list lp of 2D point (x,y) and a list lP 3D points
- (X,Y,Z) in P using the Ransac algorithm. It is not assumed that
- the 2D and 3D points are registred. \n \n
- At least numberOfInlierToReachAConsensus of true correspondance are required
- to validate the pose. \n \n
- The inliers are given in a list of vpPoint lPi. \n
- The pose is returned in cMo.
-
- \param lp : List of 2d points (x and y attributes are used).
- \param lP : List of 3d points (oX, oY and oZ attributes are used).
- \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
- to consider a trial as correct.
- \param threshold : The maximum error allowed between the 2d points and the
- reprojection of its associated 3d points by the current pose (in meter).
- \param ninliers : Number of inliers found for the best solution.
- \param lPi : List of points (2d and 3d) that are inliers for the best solution.
- \param cMo : The computed pose (best solution).
- \param maxNbTrials : Maximum number of trials before considering a solution
- fitting the required \e numberOfInlierToReachAConsensus and \e threshold
- cannot be found.
- */
-void
-vpPose::ransac(std::list<vpPoint> &lp,
- std::list<vpPoint> &lP,
- const int numberOfInlierToReachAConsensus,
- const double threshold,
- unsigned int &ninliers,
- std::list<vpPoint> &lPi,
- vpHomogeneousMatrix &cMo,
- const int maxNbTrials)
-{
- unsigned int i;
- unsigned int n = (unsigned int)lp.size() ;
- unsigned int m = (unsigned int)lP.size() ;
-
- double *x, *y;
- x = new double [n];
- y = new double [n];
-
- vpPoint pin ;
- i = 0;
- for (std::list<vpPoint>::const_iterator it = lp.begin(); it != lp.end(); ++it)
- {
- pin = *it;
- x[i] = pin.get_x() ;
- y[i] = pin.get_y() ;
- ++ i;
- }
-
- double *X, *Y, *Z;
- X = new double [m];
- Y = new double [m];
- Z = new double [m];
- i = 0;
- for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
- {
- pin = *it;
- X[i] = pin.get_oX() ;
- Y[i] = pin.get_oY() ;
- Z[i] = pin.get_oZ() ;
- ++i;
- }
-
- vpColVector xi,yi,Xi,Yi,Zi ;
-
- ransac(n,x,y,
- m,X,Y,Z, numberOfInlierToReachAConsensus,
- threshold,
- ninliers,
- xi,yi,Xi,Yi,Zi,
- cMo, maxNbTrials) ;
-
- for( i=0 ; i < ninliers ; i++)
- {
- vpPoint Pi ;
- Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
- Pi.set_x(xi[i]) ;
- Pi.set_y(yi[i]) ;
- lPi.push_back(Pi);
- }
-
- delete [] x;
- delete [] y;
- delete [] X;
- delete [] Y;
- delete [] Z;
-
-}
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/computer-vision/pose-estimation/vpPoseVirtualVisualServoing.cpp b/src/computer-vision/pose-estimation/vpPoseVirtualVisualServoing.cpp
deleted file mode 100755
index fa9477c..0000000
--- a/src/computer-vision/pose-estimation/vpPoseVirtualVisualServoing.cpp
+++ /dev/null
@@ -1,278 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseVirtualVisualServoing.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pose computation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpPoseVirtualVisualServoing.cpp
- \brief Compute the pose using virtual visual servoing approach
-*/
-
-#include <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpRobust.h>
-
-/*!
- \brief Compute the pose using virtual visual servoing approach
-
- This approach is described in \cite Marchand02c.
-
-*/
-
-void
-vpPose::poseVirtualVS(vpHomogeneousMatrix & cMo)
-{
- try
- {
-
- double residu_1 = 1e8 ;
- double r =1e8-1;
-
- // we stop the minimization when the error is bellow 1e-8
-
- int iter = 0 ;
-
- unsigned int nb = (unsigned int)listP.size() ;
- vpMatrix L(2*nb,6) ;
- vpColVector err(2*nb) ;
- vpColVector sd(2*nb),s(2*nb) ;
- vpColVector v ;
-
- vpPoint P;
- std::list<vpPoint> lP ;
-
- // create sd
- unsigned int k =0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- sd[2*k] = P.get_x() ;
- sd[2*k+1] = P.get_y() ;
- lP.push_back(P);
- k ++;
- }
-
- while((int)((residu_1 - r)*1e12) !=0)
- {
- residu_1 = r ;
-
- // Compute the interaction matrix and the error
- k =0 ;
- for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
- {
- P = *it;
- // forward projection of the 3D model for a given pose
- // change frame coordinates
- // perspective projection
- P.track(cMo) ;
-
- double x = s[2*k] = P.get_x(); /* point projected from cMo */
- double y = s[2*k+1] = P.get_y();
- double Z = P.get_Z() ;
- L[2*k][0] = -1/Z ;
- L[2*k][1] = 0 ;
- L[2*k][2] = x/Z ;
- L[2*k][3] = x*y ;
- L[2*k][4] = -(1+x*x) ;
- L[2*k][5] = y ;
-
- L[2*k+1][0] = 0 ;
- L[2*k+1][1] = -1/Z ;
- L[2*k+1][2] = y/Z ;
- L[2*k+1][3] = 1+y*y ;
- L[2*k+1][4] = -x*y ;
- L[2*k+1][5] = -x ;
-
- k+=1 ;
- }
- err = s - sd ;
-
- // compute the residual
- r = err.sumSquare() ;
-
- // compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- L.pseudoInverse(Lp,1e-16) ;
-
- // compute the VVS control law
- v = -lambda*Lp*err ;
-
- //std::cout << "r=" << r <<std::endl ;
- // update the pose
-
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax) break ;
- }
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
- }
-
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-/*!
- \brief Compute the pose using virtual visual servoing approach and
- a robust cotrol law
-
- This approach is described in \cite Comport06b.
-
-*/
-void
-vpPose::poseVirtualVSrobust(vpHomogeneousMatrix & cMo)
-{
- try{
-
- double residu_1 = 1e8 ;
- double r =1e8-1;
-
- // we stop the minimization when the error is bellow 1e-8
- vpMatrix W ;
- vpRobust robust((unsigned int)(2*listP.size())) ;
- robust.setThreshold(0.0000) ;
- vpColVector w,res ;
-
- unsigned int nb = (unsigned int)listP.size() ;
- vpMatrix L(2*nb,6) ;
- vpColVector error(2*nb) ;
- vpColVector sd(2*nb),s(2*nb) ;
- vpColVector v ;
-
- listP.front() ;
- vpPoint P;
- std::list<vpPoint> lP ;
-
- // create sd
- unsigned int k_ =0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it;
- sd[2*k_] = P.get_x() ;
- sd[2*k_+1] = P.get_y() ;
- lP.push_back(P) ;
- k_ ++;
- }
- int iter = 0 ;
- res.resize(s.getRows()/2) ;
- w.resize(s.getRows()/2) ;
- W.resize(s.getRows(), s.getRows()) ;
- w =1 ;
-
- while((int)((residu_1 - r)*1e12) !=0)
- {
- residu_1 = r ;
-
- // Compute the interaction matrix and the error
- k_ =0 ;
- for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
- {
- P = *it;
- // forward projection of the 3D model for a given pose
- // change frame coordinates
- // perspective projection
- P.track(cMo) ;
-
- double x = s[2*k_] = P.get_x(); // point projected from cMo
- double y = s[2*k_+1] = P.get_y();
- double Z = P.get_Z() ;
- L[2*k_][0] = -1/Z ;
- L[2*k_][1] = 0 ;
- L[2*k_][2] = x/Z ;
- L[2*k_][3] = x*y ;
- L[2*k_][4] = -(1+x*x) ;
- L[2*k_][5] = y ;
-
- L[2*k_+1][0] = 0 ;
- L[2*k_+1][1] = -1/Z ;
- L[2*k_+1][2] = y/Z ;
- L[2*k_+1][3] = 1+y*y ;
- L[2*k_+1][4] = -x*y ;
- L[2*k_+1][5] = -x ;
-
- k_ ++;
-
- }
- error = s - sd ;
-
- // compute the residual
- r = error.sumSquare() ;
-
- for(unsigned int k=0 ; k <error.getRows()/2 ; k++)
- {
- res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
- }
- robust.setIteration(0);
- robust.MEstimator(vpRobust::TUKEY, res, w);
-
- // compute the pseudo inverse of the interaction matrix
- for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- W[2*k][2*k] = w[k] ;
- W[2*k+1][2*k+1] = w[k] ;
- }
- // compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- (W*L).pseudoInverse(Lp,1e-6) ;
-
- // compute the VVS control law
- v = -lambda*Lp*W*error ;
-
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax) break ;
- }
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-
-}
-
-
diff --git a/src/data-structure/vpList.h b/src/data-structure/vpList.h
deleted file mode 100644
index 485acb0..0000000
--- a/src/data-structure/vpList.h
+++ /dev/null
@@ -1,839 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpList.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * List data structure.
- *
- * Authors:
- * Eric Marchand
- * Nicolas Mansard : Toward const-specification respect
- *
- *****************************************************************************/
-
-
-#ifndef VP_LIST_H
-#define VP_LIST_H
-
-
-/*!
- \file vpList.h
- \brief Definition of the list managment class
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <stdio.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
- \class vpListElement
- \brief Each element of a list
-*/
-template <class type>
-class vpListElement
-{
- public:
- vpListElement() : prev(NULL), next(NULL), val() {};
- vpListElement<type> *prev; //<! pointer to the previous element in the list
- vpListElement<type> *next; //<! pointer to the next element in the list
- type val; //<! value of the element
-} ;
-
-#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-
-
-/*!
- \class vpList
- \ingroup List
- \brief Provide simple list management
-
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-
- \verbatim
- Data structure:
- each object vpListElement contains its own value and
- two pointers to the next element of the list and to the previous one
-
- A list is mainly a pointer to three of these elements
- - one (cur) points the current element
- - one (first) points to a virtual element located at the
- beginning of the list
- - one (last) points to a virtual element located at the
- end of the list
-
- first, last and cur are used internally and are not to be considered
- by "basic" user.
-
- Notation:
- In this documentation we note such a list as
- [*, a, b, c, *]
- f ^ l
- where
- - the * are the two virtual elements (first (f) and last (l))
- - the ^ denotes the position of the current element
-
- \endverbatim
-*/
-
-
-
-template <class type>
-class vpList
-{
- private:
- void init() ;
- public:
-
- unsigned int nb; //<! number of items in the List
- /*!
- \brief the first virtual item in the list
- \verbatim
- [*, a, b, c, *]
- f
- \endverbatim
- */
- vpListElement<type> *first;
- /*!
- \brief the last virtualitem in the list
- \verbatim
- [*, a, b, c, *]
- l
- \endverbatim
- */
- vpListElement<type> *last;
- /*!
- \brief the current item in the list
- \verbatim
- [*, a, b, c, *]
- ^
- \endverbatim
- */
- vpListElement<type> *cur; // the current element
- public:
- vpList() ; // constr.
- vpList(const vpList& l); // cloning
- virtual ~vpList(); // destr.
-
- inline void next(void) ; // current element's successor ( cur = cur->next )
- inline void previous(void) ; // current element's predecessor ( cur = cur->pred )
- inline void front(void) ; // go to the front of the List (cur = first)
- inline void end(void) ; // go back to the end of the List ( cur = last )
- inline bool outside(void) const; // test whether we are outside the List
-
- bool empty(void) const; // tests whether the List is empty
-
- inline type& value(void); // returns the current element value
- inline const type& value(void) const; // returns the current element value
-
- void suppress(void); // deletes the current item
- void kill(); // empties the List
-
- void display() ; // displays the content of the list
- void print() {display() ;} // displays the content of the list
-
-
- inline void addRight(const type& el); // inserts an element on the right
- inline void addLeft(const type& el); // inserts an element on the left
- inline void modify(const type& el); // modifies thevalue field of the curr. el.
- inline void addRight(type& el); // inserts an element on the right
- inline void addLeft(type& el); // inserts an element on the left
- inline void swapLeft(); // Switch the current element with the element on the left
- inline void swapRight(); // Switch the current element with the element on the right
- inline unsigned int nbElement(void); // returns the number of items currently in the list
- inline unsigned int nbElements(void); // returns the number of items currently in the list
-
- vpList<type>& operator=(const vpList<type>& l);
- inline void operator+=(vpList<type>& l);
- inline void operator+=(const type& l);
-
- // Other non fundamental member (may be somehow useful)
- bool nextOutside(void) const; // test whether we are outside the List
- bool previousOutside(void) const;// test whether we are outside the List
-
-
- type& previousValue(void); // returns the previous element value
- type& nextValue(void); // returns the next element value
- type& firstValue(void) ;
- type& lastValue(void) ;
-
-
-};
-
-
-/*!
- \brief initialization, Create an empty list
- \verbatim
- init() --> [*, *]
- \endverbatim */
-template<class type>
-void vpList<type>::init()
-{
- vpListElement<type> *x=new vpListElement<type>;
- vpListElement<type> *y=new vpListElement<type> ;
-
- first = x ;
- last = y ;
-
- x->prev = NULL ;
- x->next = y ;
- y->prev = x ;
- y->next =NULL ;
-
- cur = x ;
- nb = 0 ;
-}
-
-/*!
- \brief Basic constructor, initialization, Create an empty list
- \verbatim
- init() --> [*, *]
- \endverbatim
- \sa init()
- */
-template<class type>
-vpList<type>::vpList() : nb(0), first(NULL), last(NULL), cur(NULL)
-{
- init() ;
-}
-
-
-
-/*!
- \brief vpList destructor
- \sa kill()
- */
-template<class type>
-vpList<type>::~vpList()
-{
- kill() ;
-
- /*if (first != NULL) */ delete first ;
- /*if (last != NULL) */ delete last ;
-}
-
-/*!
- \brief return the number of element in the list
- */
-template<class type>
-unsigned int vpList<type>::nbElement(void)
-{
- return(nb) ;
-}
-
-/*!
- \brief return the number of element in the list
- */
-template<class type>
-unsigned int vpList<type>::nbElements(void)
-{
- return(nb) ;
-}
-
-
-/*!
- \brief position the current element on the next one
- \verbatim
- [*, a, b, c, d, *] --> next() --> [*, a, b, c, d, *]
- ^ ^
- \endverbatim
-*/
-template<class type>
-void vpList<type>::next(void)
-{
- cur = cur->next ;
-}
-
-
-/*!
- \brief position the current element on the previous one
- \verbatim
- [*, a, b, c, d, *] --> previous() --> [*, a, b, c, d, *]
- ^ ^
- \endverbatim
-*/
-template<class type>
-void vpList<type>::previous(void)
-{
- cur = cur->prev ;
-}
-
-/*!
- \brief return the value of the current element
-
- \verbatim
- [*, a, b, c, *] --> value() return b
- ^
- \endverbatim
- */
-template<class type>
-type& vpList<type>::value(void)
-{
- return(cur->val) ;
-}
-
-/*!
- \brief return the value of the current element using a const ref.
-
- \verbatim
- [*, a, b, c, *] --> value() return b
- ^
- \endverbatim
- */
-template<class type>
-const type& vpList<type>::value(void) const
-{
- return(cur->val) ;
-}
-
-/*!
- \brief return the value of the previous element
-
- \verbatim
- [*, a, b, c, *] --> previousValue() return a
- ^
- \endverbatim
-*/
-template<class type>
-type& vpList<type>::previousValue(void)
-{
- return(cur->prev->val) ;
-}
-
-/*!
- \brief return the value of the next element
- \verbatim
- [*, a, b, c, d, *] --> nextValue() return c
- ^
- \endverbatim
-*/
-template<class type>
-type& vpList<type>::nextValue(void)
-{
- return(cur->next->val) ;
-}
-
-
-
-/*!
- \brief return the first element of the list
- \verbatim
- [*, a, b, c, d, *] --> firstValue() return a
- \endverbatim
- */
-template<class type>
-type& vpList<type>::firstValue(void)
-{
- return(first->next->val) ;
-}
-
-
-
-/*!\brief return the last element of the list
- \verbatim
- [*, a, b, c, d, *] --> lastValue() return d
- \endverbatim
- */
-template<class type>
-type& vpList<type>::lastValue(void)
-{
- return(last->prev->val) ;
-}
-
-
-/*!
- \brief Position the current element on the first element of the list
-
- \verbatim
- [*, a, b, c, d, *] --> front() --> [*, a, b, c, d, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::front(void)
-{
- cur = first->next ;
-}
-
-/*!
- \brief Position the current element on the last element of the list
-
- \verbatim
- [*, a, b, c, d, *] --> end() --> [*, a, b, c, d, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::end(void)
-{
- cur = last->prev ;
-}
-
-/*!
- \brief Test if the list is empty
-
- \verbatim
- [*, a, b, c, d, *] --> empty return false
- [*, *] --> empty return true
- \endverbatim
- */
-template<class type>
-bool vpList<type>::empty(void) const
-{
- return((first->next == last) &&( first == last->prev)) ;
-}
-
-/*!
- \brief Test if the current element is outside the list
- (on the virtual element)
-
- \verbatim
- [*, a, b, c, d, *] --> outside return false
- ^
- [*, a, b, c, d, *] --> outside return true
- ^ or ^
- \endverbatim
- */
-template<class type>
-bool vpList<type>::outside(void) const
-{
-
- return((cur==first)||(cur==last)) ;
-}
-
-/*!
- \brief Test if the next element is outside the list
- (ie if the current element is the last one)
-
- \verbatim
- [*, a, b, c, d, *] --> nextOutside return true
- ^
- \endverbatim
- */
-template<class type>
-bool vpList<type>::nextOutside(void) const
-{
- return((cur->next==first)||(cur->next==last)) ;
-}
-
-
-/*!
- \brief Test if the previous element is outside the list
- (ie if the current element is the firts one)
-
- \verbatim
- [*, a, b, c, d, *] --> nextOutside return true
- ^
- \endverbatim
- */
-template<class type>
-bool vpList<type>::previousOutside(void) const
-{
- return((cur->prev==first)||(cur->prev==last)) ;
-}
-
-
-/*!
- \brief add a new element in the list, at the right of the current one
-
- \warning the new element becomes the current one
-
- \verbatim
- [*, a, b, c, *] --> addRight(i) --> [*, a, b, i, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::addRight(const type& v)
-{
- vpListElement<type> *x=new vpListElement<type>;
-
- x->val = v ;
- if (empty())
- {
- cur = first ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
- }
- cur->next->prev = x ;
- x->next = cur->next ;
- x->prev = cur ;
- cur->next = x ;
- cur = x ;
- nb++ ;
-}
-
-
-/*!
- \brief add a new element in the list, at the left of the current one
-
- \warning the new element becomes the current one
-
- \verbatim
- [*, a, b, c, *] --> addLeft(i) --> [*, a, i, b, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::addLeft(const type& v)
-{
- vpListElement<type> *x=new vpListElement<type>;
-
- x->val = v ;
-
- if (empty())
- {
- cur = last ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
- }
- x->next = cur ;
- x->prev = cur->prev ;
- cur->prev->next = x ;
- cur->prev = x ;
- cur = x ;
- nb++ ;
-
-}
-
-/*!
- \brief add a new element in the list, at the right of the current one
-
- \warning the new element becomes the current one
-
- \verbatim
- [*, a, b, c, *] --> addRight(i) --> [*, a, b, i, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::addRight(type& v)
-{
- vpListElement<type> *x=new vpListElement<type>;
-
- x->val = v ;
- if (empty())
- {
- cur = first ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
- }
- cur->next->prev = x ;
- x->next = cur->next ;
- x->prev = cur ;
- cur->next = x ;
- cur = x ;
- nb++ ;
-}
-
-
-/*!
- \brief add a new element in the list, at the left of the current one
-
- \warning the new element becomes the current one
-
- \verbatim
- [*, a, b, c, *] --> addLeft(i) --> [*, a, i, b, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::addLeft(type& v)
-{
- vpListElement<type> *x=new vpListElement<type>;
-
- x->val = v ;
-
- if (empty())
- {
- cur = last ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
- }
- x->next = cur ;
- x->prev = cur->prev ;
- cur->prev->next = x ;
- cur->prev = x ;
- cur = x ;
- nb++ ;
-
-}
-
-/*!
- \brief Modify the value of the current element
-
- \verbatim
- [*, a, b, c, *] --> modify(i) --> [*, a, i, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::modify(const type& v)
-{
- cur->val = v ;
-}
-
-/*!
- \brief Switch the current element with the element on the left
-
- \verbatim
- [*, a, b, c, *] --> swapLeft --> [*, b, a, c, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::swapLeft()
-{
- if (cur->prev != first)
- {
- cur->prev->prev->next = cur;
- cur->next->prev = cur->prev;
-
- vpListElement<type> *nextTmp;
- vpListElement<type> *prevTmp;
-
- nextTmp = cur->next;
- prevTmp = cur->prev;
-
- cur->next = cur->prev;
- cur->prev = cur->prev->prev;
-
- prevTmp->prev = cur;
- prevTmp->next = nextTmp;
- }
- else
- {
- std::cout << "vpList: previous element is outside (swapLeft) " << std::endl ;
- }
-}
-
-/*!
- \brief Switch the current element with the element on the right
-
- \verbatim
- [*, a, b, c, *] --> swapRight --> [*, a, c, b, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::swapRight()
-{
- if (cur->next != last)
- {
- cur->prev->next = cur->next;
- cur->next->next->prev = cur;
-
- vpListElement<type> *nextTmp;
- vpListElement<type> *prevTmp;
-
- nextTmp = cur->next;
- prevTmp = cur->prev;
-
- cur->next = nextTmp->next;
- cur->prev = nextTmp;
-
- nextTmp->prev = prevTmp;
- nextTmp->next = cur;
- }
- else
- {
- std::cout << "vpList: next element is outside (swapRight) " << std::endl ;
- }
-}
-
-/*!
- \brief Destroy the list
-
- \verbatim
- [*, a, b, c, *] --> kill --> [*, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::kill()
-{
-
- front() ;
- while (!empty())
- {
- suppress() ;
- }
-
-}
-
-
-
-/*!
- \brief suppress the current element
-
- \warning new current element is on the next one
-
- \verbatim
- [*, a, b, c, d, *] --> suppress --> [*, a, c, d, *]
- ^ ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::suppress(void)
-{
- vpListElement<type> *x ;
-
- cur->prev->next = cur->next ;
- cur->next->prev = cur->prev ;
- x = cur ;
- cur = cur->next ;
-
- if (x!=NULL) delete x ;
-
- nb-- ;
-
-
-}
-
-
-
-
-/*!
- \brief Copy constructor const
-
- \param l : the list to copy
- */
-
-template<class type>
-vpList<type>& vpList<type>::operator=(const vpList<type>& l)
-{
- type x ;
- vpListElement<type> *e ;
-
- kill() ;
- e = l.first->next ;
- front() ;
- while (e!=l.last)
- {
- x = e->val ;
- addRight(x) ;
- e = e->next ;
- }
-
- nb = l.nb ;
- cur = first->next ;
-
- return *this;
-}
-
-/*!
- \brief Append two lists
-
- \verbatim
- [*, a, b, *] += [*, c, d, *] --> [*, a, b, c, d, *]
- ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::operator+=(vpList<type>& l)
-{
- type x ;
-
- l.front() ;
- end() ;
- while (!l.outside())
- {
- x = l.value() ;
- addRight(x) ;
- l.next() ;
- }
-}
-
-/*!
- \brief Append an element to a list
-
- \verbatim
- [*, a, b, *] += c --> [*, a, b, c, *]
- ^
- \endverbatim
- */
-template<class type>
-void vpList<type>::operator += (const type& l)
-{
- end() ;
- addRight(l) ;
-}
-
-
-/*!
- \brief copy constructor
-
- \param l : the list to copy
-*/
-template<class type>
-vpList<type>::vpList(const vpList<type>& l)
- : nb(0), first(NULL), last(NULL), cur(NULL)
-{
- init() ;
- *this = l;
-}
-
-/*!
- \brief Print (std::cout) all the element of the list
- */
-template<class type>
-void vpList<type>::display()
-{
- unsigned int k = 1 ;
- front() ;
- while(!outside()) {
- std::cout<<k<<" ---> "<<value()<<std::endl ;
- next() ;
- k++ ;
- }
- std::cout<< std::endl << std::endl ;
-}
-
-#endif /* #ifndef VP_LIST_H */
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/detection/barcode/vpDetectorDataMatrixCode.cpp b/src/detection/barcode/vpDetectorDataMatrixCode.cpp
deleted file mode 100644
index b746697..0000000
--- a/src/detection/barcode/vpDetectorDataMatrixCode.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/****************************************************************************
- *
- * $Id$
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base class for bar code detection.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <assert.h>
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_DMTX
-
-#include <dmtx.h>
-
-#include <visp/vpDetectorDataMatrixCode.h>
-
-/*!
- Default constructor that does nothing.
- */
-vpDetectorDataMatrixCode::vpDetectorDataMatrixCode()
-{
-}
-
-/*!
- Detect datamatrix bar codes in the image. Return true if a bar code is detected, false otherwise.
-
- \param I : Input image.
- */
-bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
-{
- bool detected = false;
- m_message.clear();
- m_polygon.clear();
- m_nb_objects = 0;
- DmtxRegion *reg;
- DmtxDecode *dec;
- DmtxImage *img;
- DmtxMessage *msg;
-
- img = dmtxImageCreate(I.bitmap, (int) I.getWidth(), (int) I.getHeight(), DmtxPack8bppK);
- assert(img != NULL);
-
- dec = dmtxDecodeCreate(img, 1);
- assert(dec != NULL);
-
- bool end = false;
- do {
- reg = dmtxRegionFindNext(dec, 0);
-
- if(reg != NULL) {
- msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
- if(msg != NULL) {
-
- std::vector<vpImagePoint> polygon;
-
- DmtxVector2 p00, p10, p11, p01;
-
- p00.X = p00.Y = p10.Y = p01.X = 0.0;
- p10.X = p01.Y = p11.X = p11.Y = 1.0;
- dmtxMatrix3VMultiplyBy(&p00, reg->fit2raw);
- dmtxMatrix3VMultiplyBy(&p10, reg->fit2raw);
- dmtxMatrix3VMultiplyBy(&p11, reg->fit2raw);
- dmtxMatrix3VMultiplyBy(&p01, reg->fit2raw);
-
- polygon.push_back(vpImagePoint(I.getHeight()-p00.Y, p00.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p10.Y, p10.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p11.Y, p11.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p01.Y, p01.X));
-
- m_polygon.push_back(polygon);
- detected = true;
- m_message.push_back( (const char *)msg->output);
-
- m_nb_objects ++;
- }
- else {
- end = true;
- }
- dmtxMessageDestroy(&msg);
- }
- else {
- end = true;
- }
- dmtxRegionDestroy(®);
-
- } while (!end);
-
- dmtxDecodeDestroy(&dec);
- dmtxImageDestroy(&img);
- return detected;
-}
-
-#endif
diff --git a/src/detection/barcode/vpDetectorDataMatrixCode.h b/src/detection/barcode/vpDetectorDataMatrixCode.h
deleted file mode 100644
index 92a90e0..0000000
--- a/src/detection/barcode/vpDetectorDataMatrixCode.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/****************************************************************************
- *
- * $Id$
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base class for bar code detection.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpDetectorDataMatrixCode_h__
-#define __vpDetectorDataMatrixCode_h__
-
-#include <vector>
-#include <utility>
-#include <string>
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_DMTX
-
-#include <visp/vpDetectorBase.h>
-#include <visp/vpImage.h>
-
-/*!
- \class vpDetectorDataMatrixCode
-
- Base class for bar code detector. This class is a wrapper over libdmtx
- available from http://www.libdmtx.org/
-
- The detect() function allows to detect multiple QR codes in an image. Once detected,
- for each QR code it is possible to retrieve the location of the corners using getPolygon(),
- the encoded message using getMessage(), the bounding box using getBBox() and the center
- of gravity using getCog().
-
- The following sample code shows how to use this class to detect QR codes in an image.
- \code
-#include <visp/vpDetectorDataMatrixCode.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-#ifdef VISP_HAVE_DMTX
- vpImage<unsigned char> I;
- vpImageIo::read(I, "bar-code.pgm");
-
- vpDetectorDataMatrixCode detector;
-
- bool status = detector.detect(I);
- if (status) {
- for(size_t i=0; i < detector.getNbObjects(); i++) {
- std::cout << "Bar code " << i << ":" << std::endl;
- std::vector<vpImagePoint> p = detector.getPolygon(i);
- for(size_t j=0; j < p.size(); j++)
- std::cout << " Point " << j << ": " << p[j] << std::endl;
- std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl;
- }
- }
-#endif
-}
- \endcode
-
- The previous example may produce results like:
- \code
-Bar code 0:
- Point 0: 273.21, 78.9799
- Point 1: 390.016, 85.1014
- Point 2: 388.024, 199.185
- Point 3: 269.23, 192.96
- Message: "datamatrix 1"
-Bar code 1:
- Point 0: 262.23, 396.404
- Point 1: 381.041, 402.631
- Point 2: 378.92, 524.188
- Point 3: 257.916, 519.962
- Message: "datamatrix 2"
- \endcode
-
- Other examples are also provided in tutorial-barcode-detector.cpp and
- tutorial-barcode-detector-live.cpp
-
- */
-class VISP_EXPORT vpDetectorDataMatrixCode : public vpDetectorBase
-{
-public:
- vpDetectorDataMatrixCode();
- virtual ~vpDetectorDataMatrixCode() {};
- bool detect(const vpImage<unsigned char> &I);
-};
-
-#endif
-#endif
diff --git a/src/detection/barcode/vpDetectorQRCode.cpp b/src/detection/barcode/vpDetectorQRCode.cpp
deleted file mode 100644
index 659ec30..0000000
--- a/src/detection/barcode/vpDetectorQRCode.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/****************************************************************************
- *
- * $Id$
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base class for bar code detection.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_ZBAR
-
-#include <visp/vpDetectorQRCode.h>
-
-
-/*!
- Default constructor.
- */
-vpDetectorQRCode::vpDetectorQRCode() : m_scanner()
-{
- // configure the reader
- m_scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
-}
-
-/*!
- Detect QR codes in the image. Return true if a code is detected, false otherwise.
-
- \param I : Input image.
- */
-bool vpDetectorQRCode::detect(const vpImage<unsigned char> &I)
-{
- bool detected = false;
- m_message.clear();
- m_polygon.clear();
- m_nb_objects = 0;
-
- m_scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- // wrap image data
- zbar::Image img(width, height, "Y800", I.bitmap, (unsigned long)(width * height));
-
- // scan the image for barcodes
- m_nb_objects = (size_t) m_scanner.scan(img);
-
- // extract results
- for(zbar::Image::SymbolIterator symbol = img.symbol_begin();
- symbol != img.symbol_end();
- ++symbol) {
- m_message.push_back( symbol->get_data() );
- detected = true;
-
- std::vector<vpImagePoint> polygon;
- for(unsigned int i=0; i < (unsigned int)symbol->get_location_size(); i++){
- polygon.push_back(vpImagePoint(symbol->get_location_y(i), symbol->get_location_x(i)));
- }
- m_polygon.push_back(polygon);
- }
-
- // clean up
- img.set_data(NULL, 0);
-
- return detected;
-}
-
-#endif
diff --git a/src/detection/barcode/vpDetectorQRCode.h b/src/detection/barcode/vpDetectorQRCode.h
deleted file mode 100644
index 3839ea0..0000000
--- a/src/detection/barcode/vpDetectorQRCode.h
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- * $Id$
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base class for bar code detection.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpDetectorQRCode_h__
-#define __vpDetectorQRCode_h__
-
-#include <vector>
-#include <utility>
-#include <string>
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_ZBAR
-
-#include <zbar.h>
-
-#include <visp/vpDetectorBase.h>
-#include <visp/vpImage.h>
-
-/*!
- \class vpDetectorQRCode
-
- Base class for bar code detector. This class is a wrapper over libzbar
- available from http://zbar.sourceforge.net/
-
- The detect() function allows to detect multiple QR codes in an image. Once detected,
- for each QR code it is possible to retrieve the location of the corners using getPolygon(),
- the encoded message using getMessage(), the bounding box using getBBox() and the center
- of gravity using getCog().
-
- The following sample code shows how to use this class to detect QR codes in an image.
- \code
-#include <visp/vpDetectorQRCode.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-#ifdef VISP_HAVE_ZBAR
- vpImage<unsigned char> I;
- vpImageIo::read(I, "bar-code.pgm");
-
- vpDetectorQRCode detector;
-
- bool status = detector.detect(I);
- if (status) {
- for(size_t i=0; i < detector.getNbObjects(); i++) {
- std::cout << "Bar code " << i << ":" << std::endl;
- std::vector<vpImagePoint> p = detector.getPolygon(i);
- for(size_t j=0; j < p.size(); j++)
- std::cout << " Point " << j << ": " << p[j] << std::endl;
- std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl;
- }
- }
-#endif
-}
- \endcode
-
- The previous example may produce results like:
- \code
-Bar code 0:
- Point 0: 48, 212
- Point 1: 57, 84
- Point 2: 188, 92
- Point 3: 183, 220
- Message: "qrcode 2"
-Bar code 1:
- Point 0: 26, 550
- Point 1: 35, 409
- Point 2: 174, 414
- Point 3: 167, 555
- Message: "qrcode 1"
- \endcode
-
- Other examples are also provided in tutorial-barcode-detector.cpp and
- tutorial-barcode-detector-live.cpp
- */
-class VISP_EXPORT vpDetectorQRCode : public vpDetectorBase
-{
-protected:
- zbar::ImageScanner m_scanner; //!< QR code detector.
-
-public:
- vpDetectorQRCode();
- virtual ~vpDetectorQRCode() {};
- bool detect(const vpImage<unsigned char> &I);
-};
-
-#endif
-#endif
diff --git a/src/detection/face/vpDetectorFace.cpp b/src/detection/face/vpDetectorFace.cpp
deleted file mode 100644
index 0f1e172..0000000
--- a/src/detection/face/vpDetectorFace.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Detect faces.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
-
-#include <algorithm>
-
-#include <visp/vpDetectorFace.h>
-#include <visp/vpImageConvert.h>
-
-bool vpSortLargestFace(cv::Rect rect1, cv::Rect rect2)
-{
- return (rect1.area() > rect2.area());
-}
-
-/*!
- Default constructor.
- */
-vpDetectorFace::vpDetectorFace() :
- m_faces(), m_face_cascade(), m_frame_gray()
-{
-}
-
-/*!
- Set the name of the OpenCV cascade classifier file used for face detection.
- \param filename : Full path to access to the file. Such a file can be found in OpenCV. Within the last versions
- it was name "haarcascade_frontalface_alt.xml".
- */
-void vpDetectorFace::setCascadeClassifierFile(const std::string &filename)
-{
- if( ! m_face_cascade.load( filename ) ) {
- throw vpException(vpException::ioError, "Cannot read haar file: %s", filename.c_str());
- }
-}
-
-/*!
- Allows to detect a face in the image. When more than one face is detected, faces are sorted from largest to smallest.
-
- \param I : Input image to process.
- \return true if one or more faces are found, false otherwise.
-
- The number of detected faces is returned using getNbObjects().
- If a face is found the functions getBBox(), getCog() return some information about the location of the face.
-
- The largest face is always available using getBBox(0) or getCog(0).
- */
-bool vpDetectorFace::detect(const vpImage<unsigned char> &I)
-{
- vpImageConvert::convert(I, m_frame_gray);
-
- bool detected = false;
- m_message.clear();
- m_polygon.clear();
- m_nb_objects = 0;
-
- m_faces.clear();
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- m_face_cascade.detectMultiScale( m_frame_gray, m_faces, 1.1, 2, 0, cv::Size(30, 30) );
-#else
- m_face_cascade.detectMultiScale( m_frame_gray, m_faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
-#endif
-
- m_nb_objects = m_faces.size();
-
- std::sort(m_faces.begin(), m_faces.end(), vpSortLargestFace);
-
- if (m_faces.size())
- for( size_t i = 0; i < m_faces.size(); i++ ) {
- std::ostringstream message;
- message << "Face " << i;
- m_message.push_back( message.str() );
-
- detected = true;
-
- std::vector<vpImagePoint> polygon;
- double x = m_faces[i].tl().x;
- double y = m_faces[i].tl().y;
- double w = m_faces[i].size().width;
- double h = m_faces[i].size().height;
-
- polygon.push_back(vpImagePoint(y , x ));
- polygon.push_back(vpImagePoint(y+h, x ));
- polygon.push_back(vpImagePoint(y+h, x+w));
- polygon.push_back(vpImagePoint(y , x+w));
-
- m_polygon.push_back(polygon);
- }
-
- return detected;
-}
-
-
-#endif
diff --git a/src/detection/face/vpDetectorFace.h b/src/detection/face/vpDetectorFace.h
deleted file mode 100644
index d27ba88..0000000
--- a/src/detection/face/vpDetectorFace.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Detect faces.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpDetectorFace_h__
-#define __vpDetectorFace_h__
-
-#include <visp/vpConfig.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
-
-#include <opencv2/objdetect/objdetect.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <visp/vpDetectorBase.h>
-
-/*!
- \class vpDetectorFace
-
- The vpDetectorFace class is a wrapper over OpenCV Haar cascade face detection capabilities.
- To use this class ViSP should be build against OpenCV 2.2.0 or a more recent version.
-
- The following sample code shows how to use this class to detect the largest face in the image.
- The cascade classifier file "haarcascade_frontalface_alt.xml" can be found in ViSP source code or in OpenCV.
- \code
-#include <visp/vpDetectorFace.h>
-
-int main()
-{
- vpImage<unsigned char> I;
- vpDetectorFace face_detector;
- face_detector.setCascadeClassifierFile("haarcascade_frontalface_alt.xml");
-
- while(1) {
- // acquire a new image in I
- bool face_found = face_detector.track(I);
- if (face_found) {
- vpRect face_bbox = face_detector.getBoundingBox(0); // largest face has index 0
- }
- }
-}
- \endcode
-
- A more complete example that works with images acquired from a camera is provided in tutorial-face-detector-live.cpp.
- */
-class VISP_EXPORT vpDetectorFace : public vpDetectorBase
-{
-protected:
- std::vector<cv::Rect> m_faces; //!< Bounding box of each detected face.
- cv::CascadeClassifier m_face_cascade; //!< Haar cascade classifier file name.
- cv::Mat m_frame_gray; //!< OpenCV image used as input for the face detection.
-
-public:
- vpDetectorFace();
- /*!
- Default destructor.
- */
- virtual ~vpDetectorFace() {};
- void setCascadeClassifierFile(const std::string &filename);
-
- bool detect(const vpImage<unsigned char> &I);
-};
-
-#endif
-#endif
diff --git a/src/detection/vpDetectorBase.h b/src/detection/vpDetectorBase.h
deleted file mode 100644
index e0191f4..0000000
--- a/src/detection/vpDetectorBase.h
+++ /dev/null
@@ -1,162 +0,0 @@
-/****************************************************************************
- *
- * $Id$
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base class for object detection.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpDetectorBase_h__
-#define __vpDetectorBase_h__
-
-#include <vector>
-#include <utility>
-#include <string>
-#include <assert.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpRect.h>
-
-/*!
- \class vpDetectorBase
-
- Base class for object detection.
-
- This class is a generic class that can be used to detect:
- - bar codes like QRcodes of Data matrices. The example given in tutorial-barcode-detector.cpp shows
- how to detect one or more bar codes in an image. In tutorial-barcode-detector-live.cpp you will find
- an other example that shows how to use this class to detect bar codes in images acquired by a camera.
- - faces. An example is provided in tutorial-face-detector-live.cpp.
- */
-class VISP_EXPORT vpDetectorBase
-{
-protected:
- std::vector< std::vector<vpImagePoint> > m_polygon; //!< For each object, defines the polygon that contains the object.
- std::vector< std::string > m_message; //!< Message attached to each object.
- size_t m_nb_objects; //!< Number of detected objects.
-
-public:
- /*!
- Default constructor.
- */
- vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0) {}
- /*!
- Default destructor.
- */
- virtual ~vpDetectorBase() {};
-
- /*!
- Detect objects in an image.
- \param I : Image where to detect objects.
- \return true if one or multiple objects are detected, false otherwise.
- */
- virtual bool detect(const vpImage<unsigned char> &I) = 0;
- /*!
- Returns object container box as a vector of points.
- */
- std::vector< std::vector<vpImagePoint> > & getPolygon()
- {
- return m_polygon;
- }
-
- /*!
- Returns ith object container box as a vector of points.
- */
- std::vector<vpImagePoint> & getPolygon(size_t i)
- {
- if (i < m_polygon.size())
- return m_polygon[i];
- else
- throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i, m_polygon.size()));
- }
- /*!
- Returns the contained message of the ith object if there is one.
- */
- std::string & getMessage(size_t i)
- {
- if (i < m_polygon.size())
- return m_message[i];
- else
- throw(vpException(vpException::badValue, "Bad index to retrieve object %d . Only %d objects are detected.", i, m_polygon.size()));
- }
- /*!
- Returns the contained message of the ith object if there is one.
- */
- std::vector< std::string > & getMessage()
- {
- return m_message;
- }
- /*!
- Return the number of objects that are detected.
- */
- size_t getNbObjects() const {return m_nb_objects; }
- /*!
- Return the center of gravity location of the ith object.
- */
- vpImagePoint getCog(size_t i) const
- {
- vpImagePoint cog(0,0);
- for(size_t j=0; j < m_polygon[i].size(); j++) {
- cog += m_polygon[i][j];
- }
- cog /= (double)m_polygon[i].size();
- return cog;
- }
- /*!
- Return the bounding box of the ith object.
- */
- vpRect getBBox(size_t i) const
- {
- assert(m_polygon[i].size() > 2);
-
- double left, right;
- double top, bottom;
- left = right = m_polygon[i][0].get_u();
- top = bottom = m_polygon[i][0].get_v();
- for(size_t j=0; j < m_polygon[i].size(); j++) {
- double u = m_polygon[i][j].get_u();
- double v = m_polygon[i][j].get_v();
- if (u < left) left = u;
- if (u > right) right = u;
- if (v < top) top = v;
- if (v > bottom) bottom = v;
- }
- vpRect roi(vpImagePoint(top, left), vpImagePoint(bottom, right));
- return roi;
- }
-};
-
-#endif
diff --git a/src/device/display/vpDisplay.cpp b/src/device/display/vpDisplay.cpp
deleted file mode 100644
index 6a99f61..0000000
--- a/src/device/display/vpDisplay.cpp
+++ /dev/null
@@ -1,3960 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplay.cpp 5008 2014-11-25 17:59:43Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <limits>
-
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayException.h>
-#include <visp/vpImageConvert.h>
-
-#include <visp/vpPoint.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpMath.h>
-
-
-/*!
- \file vpDisplay.cpp
- \brief Generic class for image display.
-*/
-
-/*!
- Default constructor.
-*/
-vpDisplay::vpDisplay()
- : displayHasBeenInitialized(false), windowXPosition(0), windowYPosition(0), width(0), height(0), title_() {}
-
-/*!
- Copy constructor.
-*/
-vpDisplay::vpDisplay(const vpDisplay &d)
- : displayHasBeenInitialized(false), windowXPosition(0), windowYPosition(0), width(0), height(0), title_()
-{
- displayHasBeenInitialized = d.displayHasBeenInitialized;
- windowXPosition = d.windowXPosition;
- windowYPosition = d.windowYPosition;
-
- width = d.width;
- height = d.height;
-}
-
-/*!
- Destructor that desallocates memory.
-*/
-vpDisplay::~vpDisplay()
-{
- displayHasBeenInitialized = false ;
-}
-
-/*!
- Set the font of a text printed in the display overlay. To print a
- text you may use displayCharString().
-
- \param I : Image associated to the display window.
- \param fontname : The expected font name.
-
- \note Under UNIX, the available fonts are given by
- the "xlsfonts" binary. To choose a font you can also use the
- "xfontsel" binary.
-
- \sa displayCharString()
-*/
-void
-vpDisplay::setFont ( const vpImage<unsigned char> &I,
- const char *fontname )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setFont ( fontname ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Set the windows title.
- \note This functionality is not implemented when vpDisplayOpenCV is used.
-
- \param I : Image associated to the display window.
- \param windowtitle : Window title.
-*/
-void
-vpDisplay::setTitle ( const vpImage<unsigned char> &I,
- const char *windowtitle )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setTitle ( windowtitle ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Set the window position in the screen.
-
- \param I : Image associated to the display window.
- \param winx, winy : Position of the upper-left window's border in the screen.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void
-vpDisplay::setWindowPosition ( const vpImage<unsigned char> &I,
- int winx, int winy )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setWindowPosition ( winx, winy ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Set the window background.
-
- \param I : Image associated to the display window.
- \param color: Background color.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void
-vpDisplay::setBackground(const vpImage<unsigned char> &I, const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->clearDisplay ( color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), close()
-*/
-void
-vpDisplay::display ( const vpImage<unsigned char> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayImage ( I ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-void
-vpDisplay::displayROI(const vpImage<unsigned char> &I, const vpRect &roi)
-{
- vpImagePoint topLeft;
- double top = floor(roi.getTop());
- double left = floor(roi.getLeft());
- double roiheight = floor(roi.getHeight());
- double roiwidth = floor(roi.getWidth());
- double iheight = (double)(I.getHeight());
- double iwidth = (double)(I.getWidth());
-
- if (top < 0 || top > iheight || left < 0 || left > iwidth || top+roiheight > iheight || left+roiwidth > iwidth)
- {
- vpERROR_TRACE ( "Region of interest outside of the image" ) ;
- throw ( vpException ( vpException::dimensionError,"Region of interest outside of the image" ) ) ;
- }
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayImageROI ( I , vpImagePoint(top,left), (unsigned int)roiwidth,(unsigned int)roiheight ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-
-/*!
- Get the window pixmap and put it in vpRGBa image.
-
- The code below shows how to use this method.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayOpenCV.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320); // Create a black grey level image
- vpImage<vpRGBa> Ioverlay;
-
- vpDisplay *d;
-
- // Depending on the detected third party libraries, we instantiate here the
- // first video device which is available
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
-
- // Write the color image on the disk
- std::string ofilename("overlay.ppm");
- vpImageIo::write(Ioverlay, ofilename) ;
-
- // Wait for a click in the display window
- vpDisplay::getClick(I);
-
-#ifdef VISP_HAVE_DISPLAY
- delete d;
-#endif
-}
- \endcode
-
-*/
-void
-vpDisplay::getImage ( const vpImage<unsigned char> &Isrc,
- vpImage<vpRGBa> &Idest )
-{
-
- try
- {
- if ( Isrc.display != NULL )
- {
- ( Isrc.display )->getImage ( Idest ) ;
- }
- else
- {
- vpImageConvert::convert(Isrc, Idest);
-// vpERROR_TRACE ( "Display not initialized" ) ;
-// throw ( vpDisplayException ( vpDisplayException::notInitializedError,
-// "Display not initialized" ) ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
-
- Display the projection of an object frame represented by 3 arrows in
- the image.
-
- \param I : The image associated to the display.
-
- \param cMo : Homogeneous matrix that gives the transformation
- between the camera frame and the object frame to project in the
- image.
-
- \param cam : Camera intrinsic parameters.
-
- \param size : Size of the object frame.
-
- \param color : Color used to display the frame in the image.
-
- \param thickness : the thickness of the line
-
-*/
-void
-vpDisplay::displayFrame ( const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size,
- const vpColor &color, unsigned int thickness)
-{
- // used by display
- vpPoint o; o.setWorldCoordinates ( 0.0,0.0,0.0 ) ;
- vpPoint x; x.setWorldCoordinates ( size,0.0,0.0 ) ;
- vpPoint y; y.setWorldCoordinates ( 0.0,size,0.0 ) ;
- vpPoint z; z.setWorldCoordinates ( 0.0,0.0,size ) ;
-
- o.track ( cMo ) ;
- x.track ( cMo ) ;
- y.track ( cMo ) ;
- z.track ( cMo ) ;
-
- vpImagePoint ipo, ip1;
-
- if ( color == vpColor::none )
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
-
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, vpColor::red, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, vpColor::green, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I,ipo, ip1, vpColor::blue, 4*thickness, 2*thickness, thickness) ;
- }
- else
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
-
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I,ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- }
-}
-
-
-/*!
-
- Display the projection of an object frame represented by 3 arrows in
- the image.
-
- \param I : The image associated to the display.
-
- \param cMo : Homogeneous matrix that gives the transformation
- between the camera frame and the object frame to project in the
- image.
-
- \param cam : Camera intrinsic parameters.
-
- \param size : Size of the object frame.
-
- \param color : Color used to display the frame in the image.
-
- \param thickness : the thickness of the line
-
-*/
-void
-vpDisplay::displayFrame ( const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color,
- unsigned int thickness )
-{
- // used by display
- vpPoint o; o.setWorldCoordinates ( 0.0,0.0,0.0 ) ;
- vpPoint x; x.setWorldCoordinates ( size,0.0,0.0 ) ;
- vpPoint y; y.setWorldCoordinates ( 0.0,size,0.0 ) ;
- vpPoint z; z.setWorldCoordinates ( 0.0,0.0,size ) ;
-
- o.track ( cMo ) ;
- x.track ( cMo ) ;
- y.track ( cMo ) ;
- z.track ( cMo ) ;
-
- vpImagePoint ipo, ip1;
- if ( color == vpColor::none )
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
-
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, vpColor::red, 4, 2, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, vpColor::green, 4, 2, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I,ipo, ip1, vpColor::blue, 4, 2, thickness) ;
- }
- else
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
-
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I,ipo, ip1, color, 4*thickness, 2*thickness, thickness) ;
-
- }
-}
-
-/*!
-
- Display the projection of an object camera represented by a cone in
- the image.
-
- \param I : The image associated to the display.
-
- \param cMo : Homogeneous matrix that gives the transformation
- between the camera frame and the object frame to project in the
- image.
-
- \param cam : Camera intrinsic parameters.
-
- \param size : Size of the object camera.
-
- \param color : Color used to display the camera in the image.
-
- \param thickness : Thickness of the graphics drawing.
-
-*/
-void
-vpDisplay::displayCamera ( const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color,
- unsigned int thickness)
-{
- // used by display
- double halfSize = size/2.0;
- vpPoint pt[5];
- pt[0].setWorldCoordinates ( -halfSize,-halfSize,0.0 );
- pt[1].setWorldCoordinates ( halfSize,-halfSize,0.0 );
- pt[2].setWorldCoordinates ( halfSize,halfSize,0.0 );
- pt[3].setWorldCoordinates ( -halfSize,halfSize,0.0 );
- pt[4].setWorldCoordinates ( 0.0,0.0,-size );
-
- for (int i = 0; i < 5; i++)
- pt[i].track ( cMo ) ;
-
- vpImagePoint ip, ip_1, ip0;
- vpMeterPixelConversion::convertPoint ( cam, pt[4].p[0], pt[4].p[1], ip0);
-
- for (int i = 0; i < 4; i++)
- {
- vpMeterPixelConversion::convertPoint ( cam, pt[i].p[0], pt[i].p[1], ip_1);
- vpMeterPixelConversion::convertPoint ( cam, pt[(i+1)%4].p[0], pt[(i+1)%4].p[1], ip);
- vpDisplay::displayLine ( I, ip_1, ip, color, thickness);
- vpDisplay::displayLine ( I, ip0, ip_1, color, thickness);
- }
-}
-
-
-/*!
-
- Display the projection of an object camera represented by a cone in
- the image.
-
- \param I : The image associated to the display.
-
- \param cMo : Homogeneous matrix that gives the transformation
- between the camera frame and the object frame to project in the
- image.
-
- \param cam : Camera intrinsic parameters.
-
- \param size : Size of the object camera.
-
- \param color : Color used to display the camera in the image.
-
- \param thickness : Thickness of the graphics drawing.
-
-*/
-void
-vpDisplay::displayCamera( const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color,
- unsigned int thickness)
-{
- // used by display
- double halfSize = size/2.0;
- vpPoint pt[5];
- pt[0].setWorldCoordinates ( -halfSize,-halfSize,0.0 );
- pt[1].setWorldCoordinates ( halfSize,-halfSize,0.0 );
- pt[2].setWorldCoordinates ( halfSize,halfSize,0.0 );
- pt[3].setWorldCoordinates ( -halfSize,halfSize,0.0 );
- pt[4].setWorldCoordinates ( 0.0,0.0,-size );
-
- for (int i = 0; i < 5; i++)
- pt[i].track ( cMo ) ;
-
- vpImagePoint ip, ip_1, ip0;
- vpMeterPixelConversion::convertPoint ( cam, pt[4].p[0], pt[4].p[1], ip0);
-
- for (int i = 0; i < 4; i++)
- {
- vpMeterPixelConversion::convertPoint ( cam, pt[i].p[0], pt[i].p[1], ip_1);
- vpMeterPixelConversion::convertPoint ( cam, pt[(i+1)%4].p[0], pt[(i+1)%4].p[1], ip);
- vpDisplay::displayLine ( I, ip_1, ip, color, thickness);
- vpDisplay::displayLine ( I, ip0, ip_1, color, thickness);
- }
-}
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void
-vpDisplay::displayArrow ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w,unsigned int h,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-
-void
-vpDisplay::displayArrow ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w,unsigned int h,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display an arrow from image point (i1,j1) to image point (i2,j2).
-
- \param I : The image associated to the display.
- \param i1,j1 : Initial image point.
- \param i2,j2 : Final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void
-vpDisplay::displayArrow ( const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int w, unsigned int h,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i(i1);
- ip1.set_j(j1);
- ip2.set_i(i2);
- ip2.set_j(j2);
- ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display an arrow from image point (i1,j1) to image point (i2,j2).
-
- \param I : The image associated to the display.
- \param i1,j1 : Initial image point.
- \param i2,j2 : Final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-
-*/
-void
-vpDisplay::displayArrow ( const vpImage<vpRGBa> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int w, unsigned int h,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i(i1);
- ip1.set_j(j1);
- ip2.set_i(i2);
- ip2.set_j(j2);
-
- ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point \e ip location.
- Use rather displayText() that does the same.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param ip : Upper left image point location of the string in the display.
- \param string : String to display in overlay.
- \param color : String color.
-
- \sa setFont(), displayText()
-
-*/
-void
-vpDisplay::displayCharString ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip, const char *string,
- const vpColor &color )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, string, color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point \e ip location.
- Use rather displayText() that does the same.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param ip : Upper left image point location of the string in the display.
- \param string : String to display in overlay.
- \param color : String color.
-
- \sa setFont(), displayText()
-
-*/
-void
-vpDisplay::displayCharString ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, const char *string,
- const vpColor &color )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, string, color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point (i,j) location.
- Use rather displayText() that does the same.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param i,j : Upper left image point location of the string in the display.
- \param string : String to display in overlay.
- \param color : String color.
-
- \sa setFont(), displayText()
-
-*/
-void
-vpDisplay::displayCharString ( const vpImage<unsigned char> &I,
- int i, int j, const char *string,
- const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
-
- ( I.display )->displayCharString ( ip, string, color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point (i,j) location.
- Use rather displayText() that does the same.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param i,j : Upper left image point location of the string in the display.
- \param string : String to display in overlay.
- \param color : String color.
-
- \sa setFont(), displayText()
-
-*/
-void
-vpDisplay::displayCharString ( const vpImage<vpRGBa> &I,
- int i, int j, const char *string,
- const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- ( I.display )->displayCharString ( ip, string, color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param ip : Upper left image point location of the string in the display.
- \param s : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-
-*/
-void
-vpDisplay::displayText ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip, const std::string &s,
- const vpColor &color )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param ip : Upper left image point location of the string in the display.
- \param s : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-
-*/
-void
-vpDisplay::displayText ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, const std::string &s,
- const vpColor &color )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point (i,j) location.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param i,j : Upper left image point location of the string in the display.
- \param s : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-
-*/
-void
-vpDisplay::displayText ( const vpImage<unsigned char> &I,
- int i, int j, const std::string &s,
- const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
-
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
-
- Display a string at the image point (i,j) location.
-
- To select the font used to display the string, use setFont().
-
- \param I : Image associated to the display.
- \param i,j : Upper left image point location of the string in the display.
- \param s : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-
-*/
-void
-vpDisplay::displayText ( const vpImage<vpRGBa> &I,
- int i, int j, const std::string &s,
- const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a circle.
- \param I : The image associated to the display.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void
-vpDisplay::displayCircle ( const vpImage<unsigned char> &I,
- const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCircle ( center, radius, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a circle.
- \param I : The image associated to the display.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void
-vpDisplay::displayCircle ( const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCircle ( center, radius, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
-
- Display a circle.
- \param I : The image associated to the display.
- \param i,j : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-
-*/
-void
-vpDisplay::displayCircle ( const vpImage<unsigned char> &I,
- int i, int j, unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
-
- ( I.display )->displayCircle ( ip, radius, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
-
- Display a circle.
- \param I : The image associated to the display.
- \param i,j : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-
-*/
-void
-vpDisplay::displayCircle ( const vpImage<vpRGBa> &I,
- int i, int j, unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- ( I.display )->displayCircle ( ip, radius, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a cross at the image point \e ip location.
- \param I : The image associated to the display.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplay::displayCross ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip, unsigned int size,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a cross at the image point \e ip location.
- \param I : The image associated to the display.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplay::displayCross ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, unsigned int size,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a cross at the image point (i,j) location.
- \param I : The image associated to the display.
- \param i,j : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplay::displayCross ( const vpImage<unsigned char> &I,
- int i, int j,
- unsigned int size, const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
-
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a cross at the image point (i,j) location.
- \param I : The image associated to the display.
- \param i,j : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplay::displayCross ( const vpImage<vpRGBa> &I,
- int i, int j,
- unsigned int size, const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Dashed line thickness.
-*/
-void vpDisplay::displayDotLine ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Dashed line thickness.
-*/
-void vpDisplay::displayDotLine ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a dashed line from image point (i1,j1) to image point (i2,j2).
- \param I : The image associated to the display.
- \param i1,j1: Initial image point.
- \param i2,j2: Final image point.
- \param color : Line color.
- \param thickness : Dashed line thickness.
-
-*/
-void vpDisplay::displayDotLine ( const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a dashed line from image point (i1,j1) to image point (i2,j2).
- \param I : The image associated to the display.
- \param i1,j1: Initial image point.
- \param i2,j2: Final image point.
- \param color : Line color.
- \param thickness : Dashed line thickness.
-*/
-void vpDisplay::displayDotLine ( const vpImage<vpRGBa> &I,
- int i1, int j1,
- int i2, int j2,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplay::displayLine ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a line from image point (i1,j1) to image point (i2,j2).
- \param I : The image associated to the display.
- \param i1,j1: Initial image point.
- \param i2,j2: Final image point.
- \param color : Line color.
- \param thickness : Line thickness.
-
-*/
-void vpDisplay::displayLine ( const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
-
- Display a line from image point (i1,j1) to image point (i2,j2).
- \param I : The image associated to the display.
- \param i1,j1: Initial image point.
- \param i2,j2: Final image point.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplay::displayLine ( const vpImage<vpRGBa> &I,
- int i1, int j1,
- int i2, int j2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param I : The image associated to the display.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplay::displayLine ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a point at the image point \e ip location.
- \param I : The image associated to the display.
- \param ip : Point location.
- \param color : Point color.
- \param thickness : Thickness of the point
-*/
-void vpDisplay::displayPoint ( const vpImage<unsigned char> &I,
- const vpImagePoint &ip,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- if (thickness == 1)
- ( I.display )->displayPoint ( ip, color ) ;
- else {
- vpRect rect(0, 0, thickness, thickness);
- rect.moveCenter(ip);
- ( I.display )->displayRectangle ( rect, color, true ) ;
- }
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a point at the image point \e ip location.
- \param I : The image associated to the display.
- \param ip : Point location.
- \param color : Point color.
- \param thickness : Thickness of the point
-*/
-void vpDisplay::displayPoint ( const vpImage<vpRGBa> &I,
- const vpImagePoint &ip,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- if (thickness == 1)
- ( I.display )->displayPoint ( ip, color ) ;
- else {
- vpRect rect(0, 0, thickness, thickness);
- rect.moveCenter(ip);
- ( I.display )->displayRectangle ( rect, color, true ) ;
- }
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a point at the image point (i,j) location.
- \param I : The image associated to the display.
- \param i,j : Point location.
- \param color : Point color.
- \param thickness : Thickness of the point
-*/
-void vpDisplay::displayPoint ( const vpImage<unsigned char> &I,
- int i, int j,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- if (thickness == 1)
- ( I.display )->displayPoint ( ip, color ) ;
- else {
- vpRect rect(0, 0, thickness, thickness);
- rect.moveCenter(ip);
- ( I.display )->displayRectangle ( rect, color, true ) ;
- }
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
- Display a point at the image point (i,j) location.
- \param I : The image associated to the display.
- \param i,j : Point location.
- \param color : Point color.
- \param thickness : Thickness of the point
-
-*/
-void vpDisplay::displayPoint ( const vpImage<vpRGBa> &I,
- int i, int j,
- const vpColor &color,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- if (thickness == 1)
- ( I.display )->displayPoint ( ip, color ) ;
- else {
- vpRect rect(0, 0, thickness, thickness);
- rect.moveCenter(ip);
- ( I.display )->displayRectangle ( rect, color, true ) ;
- }
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a polygon defined by a vector of image points.
- \param I : The image associated to the display.
- \param vip : Vector of image point that define the vertexes of the polygon.
- \param color : Line color.
- \param thickness : Line thickness.
-
-*/
-void
-vpDisplay::displayPolygon(const vpImage<unsigned char> &I,
- const std::vector<vpImagePoint> &vip,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- for (unsigned int i=0; i< vip.size(); i++)
- ( I.display )->displayLine ( vip[i], vip[(i+1)%vip.size()], color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a polygon defined by a vector of image points.
- \param I : The image associated to the display.
- \param vip : Vector of image point that define the vertexes of the polygon.
- \param color : Line color.
- \param thickness : Line thickness.
-
-*/
-void
-vpDisplay::displayPolygon(const vpImage<vpRGBa> &I,
- const std::vector<vpImagePoint> &vip,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- for (unsigned int i=0; i< vip.size(); i++)
- ( I.display )->displayLine ( vip[i], vip[(i+1)%vip.size()], color, thickness );
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param topLeft : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
- const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( topLeft, width, height, color,
- fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
- const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( topLeft, bottomRight, color,
- fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
- const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( rectangle, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
-
- Display a rectangle defined by its center, its orientation (angle)
- and its size.
-
- \param I : Image associated to the display.
- \param center : Rectangle center point.
- \param angle : Angle in radians width an horizontal axis oriented from left
- to right.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
-*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if (I.display != NULL)
- {
- double i = center.get_i();
- double j = center.get_j();
-
- //A, B, C, D, corners of the rectangle clockwise
- vpImagePoint ipa, ipb, ipc, ipd;
- double cosinus = cos(angle);
- double sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
-
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught in displayRectangle") ;
- throw ;
- }
-}
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param topLeft : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
- const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( topLeft, width, height, color,
- fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
- const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( topLeft, bottomRight, color,
- fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
- const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( rectangle, color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
-
- Display a rectangle defined by its center, its orientation (angle)
- and its size.
-
- \param I : Image associated to the display.
- \param center : Rectangle center point.
- \param angle : Angle in radians width an horizontal axis oriented from left
- to right.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
-*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if (I.display != NULL)
- {
- double i = center.get_i();
- double j = center.get_j();
-
- //A, B, C, D, corners of the rectangle clockwise
- vpImagePoint ipa, ipb, ipc, ipd;
- double cosinus = cos(angle);
- double sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
-
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught in displayRectangle") ;
- throw ;
- }
-}
-
-/*!
-
- Display a rectangle with (i,j) as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param i,j : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<unsigned char> &I,
- int i, int j,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint topLeft;
- topLeft.set_i( i );
- topLeft.set_j( j );
-
- ( I.display )->displayRectangle ( topLeft, width, height,
- color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
-
- Display a rectangle defined by its center, its orientation (angle)
- and its size.
-
- \param I : Image associated to the display.
- \param i,j : Rectangle center point.
- \param angle : Angle in radians width an horizontal axis oriented from left
- to right.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
-*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
- unsigned int i, unsigned int j, float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,unsigned int thickness)
-{
- try
- {
- if (I.display != NULL)
- {
- //A, B, C, D, corners of the rectangle clockwise
- vpImagePoint ipa, ipb, ipc, ipd;
- float cosinus = cos(angle);
- float sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
-
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught in displayRectangle") ;
- throw ;
- }
-}
-
-/*!
-
- Display a rectangle with (i,j) as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param I : The image associated to the display.
- \param i,j : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplay::displayRectangle ( const vpImage<vpRGBa> &I,
- int i, int j,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- vpImagePoint topLeft;
- topLeft.set_i( i );
- topLeft.set_j( j );
-
- ( I.display )->displayRectangle ( topLeft, width, height,
- color, fill, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-/*!
-
- Display a rectangle defined by its center, its orientation (angle)
- and its size.
-
- \param I : Image associated to the display.
- \param i,j : Rectangle center point.
- \param angle : Angle in radians width an horizontal axis oriented from left
- to right.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
- unsigned int i, unsigned int j, float angle,
- unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
-{
- try
- {
- if (I.display != NULL)
- {
- //A, B, C, D, corners of the rectangle clockwise
- vpImagePoint ipa, ipb, ipc, ipd;
- float cosinus = cos(angle);
- float sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
-
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught in displayRectangle") ;
- throw ;
- }
-}
-
-/*!
- Flushes the output buffer associated to image \e I display.
- It's necessary to use this function to see the results of any drawing.
-
- \warning This function is particular and must be called
- to show the overlay. Because it's time spending, use it parcimoniously.
-
- \code
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-
-int main() {
- vpImage<unsigned char> I(240, 380);
- vpDisplayGDI d;
- d.init(I);
- vpDisplay::display(I); // display the image
- vpImagePoint center;
- unsigned int radius = 100;
- vpDisplay::displayCircle(I, center, radius, vpColor::red);
-
- vpDisplay::flush(I); // Mendatory to display the requested features.
-}
- \endcode
-
-*/
-void vpDisplay::flush ( const vpImage<unsigned char> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplay() ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-void vpDisplay::flushROI ( const vpImage<unsigned char> &I, const vpRect &roi )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplayROI(roi.getTopLeft(),(unsigned int)roi.getWidth(),(unsigned int)roi.getHeight()) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Close the display attached to I.
-*/
-void vpDisplay::close ( vpImage<unsigned char> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->closeDisplay() ;
- I.display = NULL;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
- Set the windows title.
- \note This functionality is not implemented when vpDisplayOpenCV is used.
-
- \param I : Image associated to the display window.
- \param windowtitle : Window title.
-*/
-void
-vpDisplay::setTitle ( const vpImage<vpRGBa> &I, const char *windowtitle )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setTitle ( windowtitle ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Set the window position in the screen.
-
- \param I : Image associated to the display window.
- \param winx, winy : Position of the upper-left window's border in the screen.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void
-vpDisplay::setWindowPosition ( const vpImage<vpRGBa> &I, int winx, int winy )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setWindowPosition ( winx, winy ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Set the font of a text printed in the display overlay. To print a
- text you may use displayCharString().
-
- \param I : Image associated to the display window.
- \param fontname : The expected font name.
-
- \note Under UNIX, the available fonts are given by
- the "xlsfonts" binary. To choose a font you can also use the
- "xfontsel" binary.
-
- \sa displayCharString()
-*/
-void
-vpDisplay::setFont ( const vpImage<vpRGBa> &I, const char *fontname )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->setFont ( fontname ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Set the window background.
-
- \param I : Image associated to the display window.
- \param color: Background color.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void
-vpDisplay::setBackground(const vpImage<vpRGBa> &I, const vpColor &color)
-{
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->clearDisplay ( color ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), close()
-*/
-void
-vpDisplay::display ( const vpImage<vpRGBa> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayImage ( I ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-void
-vpDisplay::displayROI(const vpImage<vpRGBa> &I, const vpRect &roi)
-{
- vpImagePoint topLeft;
- double top = floor(roi.getTop());
- double left = floor(roi.getLeft());
- double roiheight = floor(roi.getHeight());
- double roiwidth = floor(roi.getWidth());
- double iheight = (double)(I.getHeight());
- double iwidth = (double)(I.getWidth());
-
- if (top < 0 || top >= iheight || left < 0 || left >= iwidth || top+roiheight >= iheight || left+roiwidth >= iwidth)
- {
- vpERROR_TRACE ( "Region of interest outside of the image" ) ;
- throw ( vpException ( vpException::dimensionError,
- "Region of interest outside of the image" ) ) ;
- }
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->displayImageROI ( I , vpImagePoint(top,left), (unsigned int)roiwidth,(unsigned int)roiheight ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-
-/*!
- Get the window pixmap and put it in vpRGBa image.
-
- The code below shows how to use this method.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayOpenCV.h>
-
-int main()
-{
- vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
- vpImage<vpRGBa> Ioverlay;
-
- vpDisplay *d;
-
- // Depending on the detected third party libraries, we instantiate here the
- // first video device which is available
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
-
- // Write the color image on the disk
- std::string ofilename("overlay.ppm");
- vpImageIo::write(Ioverlay, ofilename) ;
-
- // Wait for a click in the display window
- vpDisplay::getClick(I);
-
-#ifdef VISP_HAVE_DISPLAY
- delete d;
-#endif
-}
- \endcode
-
-*/
-void
-vpDisplay::getImage ( const vpImage<vpRGBa> &Isrc, vpImage<vpRGBa> &Idest )
-{
-
- try
- {
- if ( Isrc.display != NULL )
- {
- ( Isrc.display )->getImage ( Idest ) ;
- }
- else {
- Idest = Isrc;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Flushes the output buffer associated to image \e I display.
- It's necessary to use this function to see the results of any drawing.
-
- \warning This function is particular and must be called
- to show the overlay. Because it's time spending, use it parcimoniously.
-
- \code
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpRGBa.h>
-
-int main() {
- vpImage<vpRGBa> I(240, 380);
- vpDisplayGDI d;
- d.init(I);
- vpDisplay::display(I); // display the image
- vpImagePoint center;
- unsigned int radius = 100;
- vpDisplay::displayCircle(I, center, radius, vpColor::red);
-
- vpDisplay::flush(I); // Mendatory to display the requested features.
-}
- \endcode
-
-*/
-void vpDisplay::flush ( const vpImage<vpRGBa> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplay() ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-void vpDisplay::flushROI ( const vpImage<vpRGBa> &I, const vpRect &roi )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplayROI(roi.getTopLeft(),(unsigned int)roi.getWidth(),(unsigned int)roi.getHeight()) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Close the display attached to I.
-*/
-void vpDisplay::close ( vpImage<vpRGBa> &I )
-{
-
- try
- {
- if ( I.display != NULL )
- {
- ( I.display )->closeDisplay() ;
- I.display = NULL;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-}
-
-/*!
- Wait for a click from one of the mouse button.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool vpDisplay::getClick ( const vpImage<unsigned char> &I, bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick(blocking) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
-
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
- vpImagePoint &ip, bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Wait for a mouse button click and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, button, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Wait for a mouse button click and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise.
-
-*/
-bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- vpImagePoint ip;
- return vpDisplay::getClick(I, ip, button, blocking);
-}
-
-/*!
- Wait for a mouse button click and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise.
-
-*/
-bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- vpImagePoint ip;
- return vpDisplay::getClick(I, ip, button, blocking);
-}
-
-/*!
- Wait for a mouse button click release and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The clicked button.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- released and then returns always true.
- - When set to false, returns true only if a mouse button is
- released, otherwise returns false.
-
- \return true if a mouse button is released, false otherwise. If a
- button is released, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool
-vpDisplay::getClickUp ( const vpImage<unsigned char> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClickUp ( ip, button, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Wait for a mouse button click release and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param button [out] : The clicked button.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- released and then returns always true.
- - When set to false, returns true only if a mouse button is
- released, otherwise returns false.
-
- \return true if a mouse button is released, false otherwise.
-
-*/
-bool vpDisplay::getClickUp ( const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- vpImagePoint ip;
- return vpDisplay::getClickUp(I, ip, button, blocking);
-}
-
-/*!
- Wait for a mouse button click release and get the position of the clicked
- image point. The button used to click is also set.
-
- \param I [in] : The displayed image.
-
- \param button [out] : The clicked button.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- released and then returns always true.
- - When set to false, returns true only if a mouse button is
- released, otherwise returns false.
-
- \return true if a mouse button is released, false otherwise.
-
-*/
-bool vpDisplay::getClickUp ( const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- vpImagePoint ip;
- return vpDisplay::getClickUp(I, ip, button, blocking);
-}
-
-/*!
- Get a keyboard event.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- Below you will find an example showing how to use this method.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320); // Create a black image
-
- vpDisplay *d;
-
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#else
- std::cout << "Sorry, no video device is available" << std::endl;
- return -1;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Wait for keyboard event
- std::cout << "Waiting a keyboard event..." << std::endl;
- vpDisplay::getKeyboardEvent(I, true);
- std::cout << "A keyboard event was detected" << std::endl;
-
- // Non blocking keyboard event loop
- int cpt_event = 0;
- bool event;
- char key[10];
- std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
- do {
- event = vpDisplay::getKeyboardEvent(I, &key[0], false);
- if (event) {
- std::cout << "Key detected: " << key << std::endl;
- cpt_event ++;
- }
-
- vpTime::wait(5); // wait 5 ms
- } while(cpt_event < 5);
-
-#ifdef VISP_HAVE_DISPLAY
- delete d;
-#endif
-}
-\endcode
-*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
-
- Get a keyboard event.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- Below you will find an example showing how to use this method.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320); // Create a black image
-
- vpDisplay *d;
-
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#else
- std::cout << "Sorry, no video device is available" << std::endl;
- return -1;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Wait for keyboard event
- std::cout << "Waiting a keyboard event..." << std::endl;
- vpDisplay::getKeyboardEvent(I, true);
- std::cout << "A keyboard event was detected" << std::endl;
-
- // Non blocking keyboard event loop
- int cpt_event = 0;
- bool event;
- char key[10];
- std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
- do {
- event = vpDisplay::getKeyboardEvent(I, &key[0], false);
- if (event) {
- std::cout << "Key detected: " << key << std::endl;
- cpt_event ++;
- }
-
- vpTime::wait(5); // wait 5 ms
- } while(cpt_event < 5);
-
-#ifdef VISP_HAVE_DISPLAY
- delete d;
-#endif
-}
-\endcode
-*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I,
- char *string, bool blocking)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( string, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-/*!
- Get the coordinates of the mouse pointer.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
-*/
-bool
-vpDisplay::getPointerMotionEvent (const vpImage<unsigned char> &I,
- vpImagePoint &ip)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerMotionEvent ( ip ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
-*/
-bool
-vpDisplay::getPointerPosition (const vpImage<unsigned char> &I,
- vpImagePoint &ip)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerPosition ( ip ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false;
-}
-
-/*!
- Wait for a click.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise.
-*/
-bool vpDisplay::getClick ( const vpImage<vpRGBa> &I, bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick(blocking) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false;
-}
-
-
-/*!
- Return true when a mouse button is pressed.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
- vpImagePoint &ip, bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Return true when a mouse button is pressed.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The clicked button.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, button, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Return true when a mouse button is released.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The clicked button.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- released and then returns always true.
- - When set to false, returns true only if a mouse button is
- released, otherwise returns false.
-
- \return true if a mouse button is released, false otherwise. If a
- button is released, the location of the mouse pointer is updated in
- \e ip.
-
-*/
-bool
-vpDisplay::getClickUp ( const vpImage<vpRGBa> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking )
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getClickUp ( ip, button, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
- Get a keyboard event.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- Below you will find an example showing how to use this method.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-int main()
-{
- vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
-
- vpDisplay *d;
-
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#else
- std::cout << "Sorry, no video device is available" << std::endl;
- return -1;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Wait for keyboard event
- std::cout << "Waiting a keyboard event..." << std::endl;
- vpDisplay::getKeyboardEvent(I, true);
- std::cout << "A keyboard event was detected" << std::endl;
-
- // Non blocking keyboard event loop
- int cpt_event = 0;
- bool event;
- char key[10];
- std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
- do {
- event = vpDisplay::getKeyboardEvent(I, &key[0], false);
- if (event) {
- std::cout << "Key detected: " << key << std::endl;
- cpt_event ++;
- }
- vpTime::wait(5); // wait 5 ms
- } while(cpt_event < 5);
-
-#ifdef VISP_HAVE_DISPLAY
- delete d;
-#endif
-}
-\endcode
-*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-
-/*!
-
- Get a keyboard event.
-
- \param I [in] : The displayed image.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- Below you will find an example showing how to use this method.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-int main()
-{
- vpImage<vpRGBa> I(240, 320); // Create a black RGB color image
-
- vpDisplay *d;
-
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#else
- std::cout << "Sorry, no video device is available" << std::endl;
- return -1;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Wait for keyboard event
- std::cout << "Waiting a keyboard event..." << std::endl;
- vpDisplay::getKeyboardEvent(I, true);
- std::cout << "A keyboard event was detected" << std::endl;
-
- // Non blocking keyboard event loop
- int cpt_event = 0;
- bool event;
- char key[10];
- std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl;
- do {
- event = vpDisplay::getKeyboardEvent(I, &key[0], false);
- if (event) {
- std::cout << "Key detected: " << key << std::endl;
- cpt_event ++;
- }
-
- vpTime::wait(5); // wait 5 ms
- } while(cpt_event < 5);
-
- delete d;
-}
-\endcode
-*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I,
- char *string, bool blocking)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( string, blocking ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false ;
-}
-/*!
- Get the coordinates of the mouse pointer.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
-*/
-bool
-vpDisplay::getPointerMotionEvent (const vpImage<vpRGBa> &I, vpImagePoint &ip)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerMotionEvent ( ip ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param I [in] : The displayed image.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-*/
-bool
-vpDisplay::getPointerPosition (const vpImage<vpRGBa> &I, vpImagePoint &ip)
-{
- try
- {
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerPosition ( ip ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- return false;
-}
-
-/*!
- Display an ellipse from its parameters expressed in pixels.
- \param I : Image to consider.
- \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
-
- \f[ \left(\begin{array}{c}
- u_\theta \\
- v_\theta
- \end{array} \right) = \left(\begin{array}{c}
- u_c \\
- v_c
- \end{array} \right) + \left(\begin{array}{cc}
- \cos(e) & -\sin(e) \\
- \sin(e) & \cos(e)
- \end{array} \right) \left(\begin{array}{c}
- a \cos(\theta) \\
- b \sin(\theta)
- \end{array} \right) \f]
-
- with \f$0 \leq \theta \leq 2\pi\f$.
-
- The following example shows how to use for example this function to display the result of a tracking.
- \code
- vpMeEllipse ellipse;
- ...
- vpDisplay::display(I);
- ellipse.track(I) ;
-
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
- true, vpColor::orange, 1);
- vpDisplay::flush(I);
- \endcode
-*/
-void vpDisplay::displayEllipse(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness)
-{
- vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
-}
-
-/*!
- Display an ellipse from its parameters expressed in pixels.
- \param I : Image to consider.
- \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
- If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
-
- \f[ \left(\begin{array}{c}
- u_\theta \\
- v_\theta
- \end{array} \right) = \left(\begin{array}{c}
- u_c \\
- v_c
- \end{array} \right) + \left(\begin{array}{cc}
- \cos(e) & -\sin(e) \\
- \sin(e) & \cos(e)
- \end{array} \right) \left(\begin{array}{c}
- a \cos(\theta) \\
- b \sin(\theta)
- \end{array} \right) \f]
-
- with \f$\theta_1 \leq \theta \leq \theta_2\f$.
-
- The following example shows how to use for example this function to display the result of a tracking.
- \code
- vpMeEllipse ellipse;
- ...
- vpDisplay::display(I);
- ellipse.track(I) ;
-
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
- ellipse.get_mu11(), ellipse.get_mu02(),
- ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
- true, vpColor::orange, 1);
- vpDisplay::flush(I);
- \endcode
-*/
-void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &theta1, const double &theta2, bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- double j1, i1;
- vpImagePoint iP11;
- double j2, i2;
- vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
- double a=0., b=0., e=0.;
-
- double mu20_p = coef1;
- double mu11_p = coef2;
- double mu02_p = coef3;
-
- if (use_centered_moments) {
- if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
-
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
-
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- e = atan(e);
- }
- else {
- a = sqrt(mu20_p);
- b = sqrt(mu02_p);
- e = 0.;
- }
- }
- else {
- a = coef1;
- b = coef2;
- e = coef3;
- }
-
- // Approximation of the circumference of an ellipse:
- // [Ramanujan, S., "Modular Equations and Approximations to ,"
- // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
- double t = (a-b)/(a+b);
- double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
-
- int nbpoints = (int)(floor(circumference/5));
- if (nbpoints < 10)
- nbpoints = 10;
- double incr = 2*M_PI / nbpoints ; // angle increment
-
- double smallalpha = theta1;
- double highalpha = theta2;
- double ce = cos(e);
- double se = sin(e);
-
- double k = smallalpha ;
- j1 = a *cos(k) ; // equation of an ellipse
- i1 = b *sin(k) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + ce *j1 - se *i1 );
- iP11.set_i ( center.get_i() + se *j1 + ce *i1 );
-
- while (k+incr<highalpha+incr)
- {
- j2 = a *cos(k+incr) ; // equation of an ellipse
- i2 = b *sin(k+incr) ; // equation of an ellipse
-
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + ce *j2 - se *i2 );
- iP22.set_i ( center.get_i() + se *j2 + ce *i2 );
-
- ( I.display )->displayLine(iP11, iP22, color, thickness) ;
-
- i1 = i2;
- j1 = j2;
- iP11 = iP22;
-
- k += incr ;
- }
- }
- }
- catch ( vpException &e )
- {
- throw(e) ;
- }
-}
-
-/*!
- Display an ellipse from its parameters expressed in pixels.
- \param I : Image to consider.
- \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
-
- \f[ \left(\begin{array}{c}
- u_\theta \\
- v_\theta
- \end{array} \right) = \left(\begin{array}{c}
- u_c \\
- v_c
- \end{array} \right) + \left(\begin{array}{cc}
- \cos(e) & -\sin(e) \\
- \sin(e) & \cos(e)
- \end{array} \right) \left(\begin{array}{c}
- a \cos(\theta) \\
- b \sin(\theta)
- \end{array} \right) \f]
-
- with \f$0 \leq \theta \leq 2\pi\f$.
-
- The following example shows how to use for example this function to display the result of a tracking.
- \code
- vpMeEllipse ellipse;
- ...
- vpDisplay::display(I);
- ellipse.track(I) ;
-
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
- true, vpColor::orange, 1);
- vpDisplay::flush(I);
- \endcode
-*/
-void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness)
-{
- vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
-}
-
-/*!
- Display an ellipse from its parameters expressed in pixels.
- \param I : Image to consider.
- \param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
- If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
-
- \f[ \left(\begin{array}{c}
- u_\theta \\
- v_\theta
- \end{array} \right) = \left(\begin{array}{c}
- u_c \\
- v_c
- \end{array} \right) + \left(\begin{array}{cc}
- \cos(e) & -\sin(e) \\
- \sin(e) & \cos(e)
- \end{array} \right) \left(\begin{array}{c}
- a \cos(\theta) \\
- b \sin(\theta)
- \end{array} \right) \f]
-
- with \f$\theta_1 \leq \theta \leq \theta_2\f$.
-
- The following example shows how to use for example this function to display the result of a tracking.
- \code
- vpMeEllipse ellipse;
- ...
- vpDisplay::display(I);
- ellipse.track(I) ;
-
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
- ellipse.get_mu11(), ellipse.get_mu02(),
- ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
- true, vpColor::orange, 1);
- vpDisplay::flush(I);
- \endcode
-*/
-void vpDisplay::displayEllipse(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &theta1, const double &theta2, bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- if ( I.display != NULL )
- {
- double j1, i1;
- vpImagePoint iP11;
- double j2, i2;
- vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
- double a=0., b=0., e=0.;
-
- double mu20_p = coef1;
- double mu11_p = coef2;
- double mu02_p = coef3;
-
- if (use_centered_moments) {
- if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
-
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
-
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- e = atan(e);
- }
- else {
- a = sqrt(mu20_p);
- b = sqrt(mu02_p);
- e = 0.;
- }
- }
- else {
- a = coef1;
- b = coef2;
- e = coef3;
- }
-
- // Approximation of the circumference of an ellipse:
- // [Ramanujan, S., "Modular Equations and Approximations to ,"
- // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
- double t = (a-b)/(a+b);
- double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
-
- int nbpoints = (int)(floor(circumference/5));
- if (nbpoints < 10)
- nbpoints = 10;
- double incr = 2*M_PI / nbpoints ; // angle increment
-
- double smallalpha = theta1;
- double highalpha = theta2;
- double ce = cos(e);
- double se = sin(e);
-
- double k = smallalpha ;
- j1 = a *cos(k) ; // equation of an ellipse
- i1 = b *sin(k) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + ce *j1 - se *i1 );
- iP11.set_i ( center.get_i() + se *j1 + ce *i1 );
-
- while (k+incr<highalpha+incr)
- {
- j2 = a *cos(k+incr) ; // equation of an ellipse
- i2 = b *sin(k+incr) ; // equation of an ellipse
-
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + ce *j2 - se *i2 );
- iP22.set_i ( center.get_i() + se *j2 + ce *i2 );
-
- ( I.display )->displayLine(iP11, iP22, color, thickness) ;
-
- i1 = i2;
- j1 = j2;
- iP11 = iP22;
-
- k += incr ;
- }
- }
- }
- catch ( vpException &e )
- {
- throw(e) ;
- }
-}
-
diff --git a/src/device/display/vpDisplay.h b/src/device/display/vpDisplay.h
deleted file mode 100644
index 078b74d..0000000
--- a/src/device/display/vpDisplay.h
+++ /dev/null
@@ -1,936 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplay.h 5089 2014-12-19 07:58:34Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpDisplay_h
-#define vpDisplay_h
-
-#include <string>
-#include <sstream>
-
-#include <visp/vpImage.h>
-#include <visp/vpColor.h>
-#include <visp/vpMouseButton.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpRect.h>
-#include <visp/vpImagePoint.h>
-
-/*!
- \file vpDisplay.h
- \brief Generic class for image display, also provide the interface
- with the image.
-*/
-
-/*!
-
- \class vpDisplay
-
- \ingroup ImageGUI
-
- \brief Class that defines generic functionnalities for display.
-
- The \ref tutorial-getting-started is a good starting point to know
- how to use this class to display an image in a window.
-
- The example below shows how to use this class.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayOpenCV.h>
-
-int main()
-{
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
-#ifdef _WIN32
- vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
-#else
- vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
-#endif
-
- vpDisplay *d;
-
- // Depending on the detected third party libraries, we instantiate here the
- // first video device which is available
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My image");
-
- // To initialize the video device, it is also possible to replace
- // the 3 previous lines by:
- // d->init(I, 400, 100, "My image");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(50);
- topLeftCorner.set_j(10);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
- char key[10]; sprintf(key, "\0");
- bool ret;
- for (int i=0; i< 200; i++) {
- bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
- }
-
- // Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
- ret = vpDisplay::getKeyboardEvent(I, key, true);
- std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
- std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
- // Wait for a click in the display window
- std::cout << "Wait for a button click..." << std::endl;
- vpDisplay::getClick(I);
-
- delete d;
-}
- \endcode
-
- Other examples are available in tutorial-image-viewer.cpp and
- tutorial-viewer.cpp.
-*/
-class VISP_EXPORT vpDisplay
-{
- protected :
- //! display has been initialized
- bool displayHasBeenInitialized ;
- //! display position
- int windowXPosition ;
- //! display position
- int windowYPosition ;
- unsigned int width ;
- unsigned int height ;
- std::string title_;
-
- /*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
- */
- virtual void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4, unsigned int h=2,
- unsigned int thickness=1) =0;
- /*!
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param ip : Upper left image point location of the string in the display.
- \param text : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
- */
- virtual void displayCharString(const vpImagePoint &ip, const char *text,
- const vpColor &color=vpColor::green) =0;
- /*!
- Display a circle.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
- */
- virtual void displayCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1) =0;
- /*!
- Display a cross at the image point \e ip location.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
- */
- virtual void displayCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color,
- unsigned int thickness=1) =0;
- /*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Dashed line thickness.
- */
- virtual void displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) =0;
- /*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
- */
- virtual void displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) =0;
-
- /*!
- Display a point at the image point \e ip location.
- \param ip : Point location.
- \param color : Point color.
- */
- virtual void displayPoint(const vpImagePoint &ip, const vpColor &color) =0;
-
- /*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
- */
- virtual void displayRectangle(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1)=0 ;
- /*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
- */
- virtual void displayRectangle(const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1 )=0;
- /*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
- */
- virtual void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1)=0 ;
-
- public:
- vpDisplay() ;
- vpDisplay(const vpDisplay& d) ;
- virtual ~vpDisplay();
-
- /*!
- Set the window backgroud to \e color.
- \param color : Background color.
- */
- virtual void clearDisplay(const vpColor &color=vpColor::white) =0 ;
- /*!
- Close the window.
- */
- virtual void closeDisplay() =0;
-
- /*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
- */
- virtual void displayImage(const vpImage<unsigned char> &I) =0 ;
- /*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
- */
- virtual void displayImage(const vpImage<vpRGBa> &I) =0 ;
-
- virtual void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
- virtual void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
-
- /*!
- Return the position (along the horizontal axis) on the screen of the display window.
- \sa getWindowYPosition()
- */
- int getWindowXPosition() const {return windowXPosition;};
- /*!
- Return the position (along the vertical axis) on the screen of the display window.
- \sa getWindowXPosition()
- */
- int getWindowYPosition() const {return windowYPosition;};
- /*!
- Flushes the display.
- It's necessary to use this function to see the results of any drawing.
- */
- virtual void flushDisplay() =0;
-
- /*!
- Flushes the display.
- It's necessary to use this function to see the results of any drawing.
- */
- virtual void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0;
-
-
- /* Simple interface with the mouse event */
-
- /*!
- Wait for a click from one of the mouse button.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
- */
- virtual bool getClick(bool blocking=true) =0;
-
- /*!
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
- */
- virtual bool getClick(vpImagePoint &ip,
- bool blocking=true) =0;
- /*!
- Wait for a mouse button click and get the position of the clicked
- pixel. The button used to click is also set.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
- */
- virtual bool getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) =0 ;
- /*!
- Wait for a mouse button click release and get the position of the
- image point were the click release occurs. The button used to click is
- also set. Same method as getClick(unsigned int&, unsigned int&,
- vpMouseButton::vpMouseButtonType &, bool).
-
- \param ip [out] : Position of the clicked image point.
-
- \param button [in] : Button used to click.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
- \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
-
- */
- virtual bool getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) =0;
-
- /*!
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
- */
- virtual bool getKeyboardEvent(bool blocking=true) =0;
- /*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
- */
- virtual bool getKeyboardEvent(char *string, bool blocking=true) =0;
- /*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
- */
- virtual bool getPointerMotionEvent (vpImagePoint &ip) =0;
-
- /*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
- */
- virtual bool getPointerPosition (vpImagePoint &ip) =0;
-
- /*!
- Return the display height.
- \sa getWidth()
- */
- inline unsigned int getHeight() const { return height ; }
- /*!
- Return the display width.
- \sa getHeight()
- */
- inline unsigned int getWidth() const { return width ; }
-
- /*!
- Initialize the display (size, position and title) of a gray level image.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
- */
- virtual void init(vpImage<unsigned char> &I,
- int x=-1, int y=-1,
- const char *title=NULL) =0 ;
- /*!
- Initialize the display (size, position and title) of a color
- image in RGBa format.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
- */
- virtual void init(vpImage<vpRGBa> &I,
- int x=-1, int y=-1,
- const char *title=NULL) =0 ;
-
- /*!
- Initialize the display size, position and title.
-
- \param width, height : Width and height of the window.
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
- */
- virtual void init(unsigned int width, unsigned int height,
- int x=-1, int y=-1 ,
- const char *title=NULL) =0;
-
- /*!
- Check if the display has been initialised
-
- \return True if the display has been initialised, otherwise False
- */
- inline bool isInitialised() { return displayHasBeenInitialized; }
-
- /*!
- Set the font used to display a text in overlay. The display is
- performed using displayCharString().
-
- \param font : The expected font name. The available fonts are given by
- the "xlsfonts" binary. To choose a font you can also use the
- "xfontsel" binary.
-
- \note Under UNIX, to know all the available fonts, use the
- "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
-
- \sa displayCharString()
- */
- virtual void setFont(const char *font) =0;
- /*!
- Set the window title.
- \param title : Window title.
- */
- virtual void setTitle(const char *title) =0;
- /*!
- Set the window position in the screen.
-
- \param winx, winy : Position of the upper-left window's border in
- the screen.
-
- */
- virtual void setWindowPosition(int winx, int winy) = 0 ;
-
- /*!
- @name Display functionalities on gray level images.
- */
- static void close(vpImage<unsigned char> &I) ;
- static void display(const vpImage<unsigned char> &I) ;
- static void displayArrow(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4, unsigned int h=2,
- unsigned int thickness=1) ;
- static void displayArrow(const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color=vpColor::white,
- unsigned int w=4, unsigned int h=2,
- unsigned int thickness=1) ;
- static void displayCamera(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color,
- unsigned int thickness) ;
- static void displayCharString(const vpImage<unsigned char> &I,
- const vpImagePoint &ip, const char *string,
- const vpColor &color) ;
- static void displayCharString(const vpImage<unsigned char> &I,
- int i, int j, const char *string,
- const vpColor &color) ;
- static void displayCircle(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- static void displayCircle(const vpImage<unsigned char> &I,
- int i, int j, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- static void displayCross(const vpImage<unsigned char> &I,
- const vpImagePoint &ip, unsigned int size,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayCross(const vpImage<unsigned char> &I,
- int i, int j, unsigned int size,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayEllipse(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayEllipse(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &theta1, const double &theta2, bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayFrame(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color=vpColor::none,
- unsigned int thickness=1) ;
- static void displayLine(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayLine(const vpImage<unsigned char> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &ip,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPoint(const vpImage<unsigned char> &I,
- int i, int j,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPolygon(const vpImage<unsigned char> &I,
- const std::vector<vpImagePoint> &vip,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayRectangle(const vpImage<unsigned char> &I,
- const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I,
- const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I,
- const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I,
- const vpImagePoint ¢er,
- float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I,
- int i, int j,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I,
- unsigned int i, unsigned int j, float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
- static void displayText(const vpImage<unsigned char> &I,
- const vpImagePoint &ip, const std::string &s,
- const vpColor &color) ;
- static void displayText(const vpImage<unsigned char> &I,
- int i, int j, const std::string &s,
- const vpColor &color) ;
- static void flush(const vpImage<unsigned char> &I) ;
- static void flushROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
-
- static bool getClick(const vpImage<unsigned char> &I, bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I,
- vpImagePoint &ip, bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClickUp(const vpImage<unsigned char> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClickUp(const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static void getImage(const vpImage<unsigned char> &Is, vpImage<vpRGBa> &Id) ;
-
- static bool getKeyboardEvent(const vpImage<unsigned char> &I,
- bool blocking=true);
- static bool getKeyboardEvent(const vpImage<unsigned char> &I,
- char *string, bool blocking=true);
- static bool getPointerMotionEvent (const vpImage<unsigned char> &I,
- vpImagePoint &ip);
- static bool getPointerPosition (const vpImage<unsigned char> &I,
- vpImagePoint &ip);
- static void setBackground(const vpImage<unsigned char> &I, const vpColor &color);
- static void setFont(const vpImage<unsigned char> &I, const char *font);
- static void setTitle(const vpImage<unsigned char> &I,
- const char *windowtitle);
- static void setWindowPosition(const vpImage<unsigned char> &I,
- int winx, int winy);
-
- /*!
- @name Display functionalities on color images.
- */
- static void close(vpImage<vpRGBa> &I) ;
-
- static void display(const vpImage<vpRGBa> &I) ;
- static void displayArrow(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4, unsigned int h=2,
- unsigned int thickness=1) ;
- static void displayArrow(const vpImage<vpRGBa> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color=vpColor::white,
- unsigned int w=4, unsigned int h=2,
- unsigned int thickness=1) ;
- static void displayCamera(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color,
- unsigned int thickness) ;
- static void displayCharString(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, const char *string,
- const vpColor &color) ;
- static void displayCharString(const vpImage<vpRGBa> &I,
- int i, int j, const char *string,
- const vpColor &color) ;
- static void displayCircle(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- static void displayCircle(const vpImage<vpRGBa> &I,
- int i, int j, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- static void displayCross(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, unsigned int size,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayCross(const vpImage<vpRGBa> &I,
- int i, int j, unsigned int size,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<vpRGBa> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayFrame(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- double size, const vpColor &color=vpColor::none,
- unsigned int thickness=1) ;
- static void displayEllipse(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayEllipse(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &angle1, const double &angle2, bool use_centered_moments,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayLine(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayLine(const vpImage<vpRGBa> &I,
- int i1, int j1, int i2, int j2,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPoint(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPoint(const vpImage<vpRGBa> &I,
- int i, int j,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayPolygon(const vpImage<vpRGBa> &I,
- const std::vector<vpImagePoint> &vip,
- const vpColor &color,
- unsigned int thickness=1) ;
- static void displayRectangle(const vpImage<vpRGBa> &I,
- const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I,
- const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I,
- const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I,
- const vpImagePoint ¢er,
- float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I,
- int i, int j,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I,
- unsigned int i, unsigned int j,
- float angle,
- unsigned int width, unsigned int height,
- const vpColor &color,
- unsigned int thickness=1);
- static void displayROI(const vpImage<vpRGBa> &I, const vpRect &roi) ;
- static void displayText(const vpImage<vpRGBa> &I,
- const vpImagePoint &ip, const std::string &s,
- const vpColor &color) ;
- static void displayText(const vpImage<vpRGBa> &I,
- int i, int j, const std::string &s,
- const vpColor &color) ;
-
- static void flush(const vpImage<vpRGBa> &I) ;
- static void flushROI(const vpImage<vpRGBa> &I, const vpRect &roi) ;
- static bool getClick(const vpImage<vpRGBa> &I, bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I,
- vpImagePoint &ip, bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClickUp(const vpImage<vpRGBa> &I,
- vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static bool getClickUp(const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking=true) ;
- static void getImage(const vpImage<vpRGBa> &Is, vpImage<vpRGBa> &Id) ;
-
- static bool getKeyboardEvent(const vpImage<vpRGBa> &I,
- bool blocking=true);
- static bool getKeyboardEvent(const vpImage<vpRGBa> &I,
- char *string, bool blocking=true);
- static bool getPointerMotionEvent (const vpImage<vpRGBa> &I, vpImagePoint &ip);
- static bool getPointerPosition (const vpImage<vpRGBa> &I, vpImagePoint &ip);
-
- static void setBackground(const vpImage<vpRGBa> &I, const vpColor &color);
- static void setFont(const vpImage<vpRGBa> &I, const char *font);
- static void setTitle(const vpImage<vpRGBa> &I, const char *windowtitle);
- static void setWindowPosition(const vpImage<vpRGBa> &I, int winx, int winy);
-
- private:
- //! get the window pixmap and put it in vpRGBa image
- virtual void getImage(vpImage<vpRGBa> &I) = 0;
-
-} ;
-
-#endif
diff --git a/src/device/display/vpDisplayException.h b/src/device/display/vpDisplayException.h
deleted file mode 100644
index 7c62631..0000000
--- a/src/device/display/vpDisplayException.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception that can be emited by the vpDisplay class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef __vpDisplayException_H
-#define __vpDisplayException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpDisplayException.h
- \brief error that can be emited by the vpDisplay class and its derivates
- */
-/* Classes standards. */
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpDisplayException
-
- \ingroup Exception
-
- \brief Error that can be emited by the vpDisplay class and its derivates.
- */
-class VISP_EXPORT vpDisplayException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpDisplay member
- */
- enum errorDisplayCodeEnum
- {
- notInitializedError,
- cannotOpenWindowError,
- connexionError,
- XWindowsError,
- GTKWindowsError,
- colorAllocError,
- depthNotSupportedError
- } ;
-
- public:
- vpDisplayException(const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
-
- vpDisplayException (const int id, const std::string & msg)
- : vpException(id, msg)
- {
- }
-
- vpDisplayException (const int id)
- : vpException(id)
- {
- }
-
-};
-
-#endif
diff --git a/src/device/display/vpDisplayGTK.cpp b/src/device/display/vpDisplayGTK.cpp
deleted file mode 100644
index 43d56c2..0000000
--- a/src/device/display/vpDisplayGTK.cpp
+++ /dev/null
@@ -1,1722 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayGTK.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Christophe Collewet
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpDisplayGTK.cpp
- \brief Define the GTK console to display images.
-*/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GTK) )
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-// Display stuff
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGTK.h>
-
-//debug / exception
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayException.h>
-#include <visp/vpMath.h>
-#include <visp/vpImageTools.h>
-
-/*!
-
- Constructor : initialize a display to visualize a gray level image
- (8 bits).
-
- \param I : Image to be displayed (not that image has to be initialized).
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title)
- : widget(NULL), background(NULL), gc(NULL),
- blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
- lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
- purple(), lightGray(), gray(), darkGray(),
- colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
-{
- init(I, x, y, title) ;
-}
-
-
-/*!
- Constructor : initialize a display to visualize a RGBa level image
- (32 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title)
- : widget(NULL), background(NULL), gc(NULL),
- blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
- lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
- purple(), lightGray(), gray(), darkGray(),
- colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
-{
- init(I, x, y, title) ;
-}
-
-
-
-/*!
-
- Constructor that just initialize the display position in the screen
- and the display title.
-
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
- To initialize the display size, you need to call init().
-
- \code
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayGTK d(100, 200, "My display");
- vpImage<unsigned char> I(240,384);
- d.init(I);
-}
- \endcode
-*/
-vpDisplayGTK::vpDisplayGTK(int x, int y, const char *title)
- : widget(NULL), background(NULL), gc(NULL),
- blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
- lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
- purple(), lightGray(), gray(), darkGray(),
- colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
-{
- windowXPosition = x ;
- windowYPosition = y ;
-
- if(title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-}
-
-/*!
- Basic constructor.
-
- To initialize the window position, title and size you may call
- init(vpImage<unsigned char> &, int, int, const char *) or
- init(vpImage<vpRGBa> &, int, int, const char *).
-
- \code
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayGTK d;
- vpImage<unsigned char> I(240,384);
- d.init(I, 100, 200, "My display");
-}
- \endcode
-*/
-vpDisplayGTK::vpDisplayGTK()
- : vpDisplay(), widget(NULL), background(NULL), gc(NULL),
- blue(), red(), yellow(), green(), cyan(), orange(), white(), black(), gdkcolor(),
- lightBlue(), darkBlue(), lightRed(), darkRed(),lightGreen(), darkGreen(),
- purple(), lightGray(), gray(), darkGray(),
- colormap(NULL), font(NULL), vectgtk(NULL), col(NULL), ncol(0), nrow(0)
-
-{
-}
-
-/*!
- Destructor.
-*/
-vpDisplayGTK::~vpDisplayGTK()
-{
- closeDisplay() ;
-}
-
-/*!
- Initialize the display (size, position and title) of a gray level image.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayGTK::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
-
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
-
- init (I.getWidth(), I.getHeight(), windowXPosition, windowYPosition, title) ;
- I.display = this ;
- displayHasBeenInitialized = true ;
-}
-
-/*!
- Initialize the display (size, position and title) of a color
- image in RGBa format.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayGTK::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
-
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
-
- init (I.getWidth(), I.getHeight(), windowXPosition, windowYPosition, title) ;
- I.display = this ;
- displayHasBeenInitialized = true ;
-}
-/*!
- Initialize the display size, position and title.
-
- \param w, h : Width and height of the window.
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayGTK::init(unsigned int w, unsigned int h,
- int x, int y,
- const char *title)
-{
- /* Initialisation of thegdk et gdk_rgb library */
- int *argc=NULL ;
- char **argv ;
- gtk_init(argc,&argv);
-
- this->width = w;
- this->height = h;
-
- /* Create the window*/
- widget = gtk_window_new(GTK_WINDOW_TOPLEVEL);
-
- gtk_widget_add_events(widget, GDK_BUTTON_PRESS_MASK | GDK_BUTTON_RELEASE_MASK
- | GDK_POINTER_MOTION_MASK);
-
- gtk_window_set_default_size(GTK_WINDOW(widget), (gint)width, (gint)height);
-
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
-
- gtk_window_move(GTK_WINDOW(widget), windowXPosition, windowYPosition);
-
- gtk_widget_show(widget);
-
- gdk_rgb_init();
-
- /* Create background pixmap */
- background = gdk_pixmap_new(widget->window, (gint)width, (gint)height, -1);
-
- /* Create graphic context */
- gc = gdk_gc_new(widget->window);
-
- /* get the colormap */
- colormap = gdk_window_get_colormap(widget->window);
-
- col = new GdkColor *[vpColor::id_unknown] ; // id_unknown = number of predefined colors
-
- /* Create color */
- gdk_color_parse("light blue",&lightBlue);
- gdk_colormap_alloc_color(colormap,&lightBlue,FALSE,TRUE);
- col[vpColor::id_lightBlue] = &lightBlue ;
-
- gdk_color_parse("blue",&blue);
- gdk_colormap_alloc_color(colormap,&blue,FALSE,TRUE);
- col[vpColor::id_blue] = &blue ;
-
- gdk_color_parse("dark blue",&darkBlue);
- gdk_colormap_alloc_color(colormap,&darkBlue,FALSE,TRUE);
- col[vpColor::id_darkBlue] = &darkBlue ;
-
- gdk_color_parse("#FF8C8C",&lightRed);
- gdk_colormap_alloc_color(colormap,&lightRed,FALSE,TRUE);
- col[vpColor::id_lightRed] = &lightRed ;
-
- gdk_color_parse("red",&red);
- gdk_colormap_alloc_color(colormap,&red,FALSE,TRUE);
- col[vpColor::id_red] = &red ;
-
- gdk_color_parse("dark red",&darkRed);
- gdk_colormap_alloc_color(colormap,&darkRed,FALSE,TRUE);
- col[vpColor::id_darkRed] = &darkRed ;
-
- gdk_color_parse("light green",&lightGreen);
- gdk_colormap_alloc_color(colormap,&lightGreen,FALSE,TRUE);
- col[vpColor::id_lightGreen] = &lightGreen ;
-
- gdk_color_parse("green",&green);
- gdk_colormap_alloc_color(colormap,&green,FALSE,TRUE);
- col[vpColor::id_green] = &green ;
-
- gdk_color_parse("dark green",&darkGreen);
- gdk_colormap_alloc_color(colormap,&darkGreen,FALSE,TRUE);
- col[vpColor::id_darkGreen] = &darkGreen ;
-
- gdk_color_parse("yellow",&yellow);
- gdk_colormap_alloc_color(colormap,&yellow,FALSE,TRUE);
- col[vpColor::id_yellow] = &yellow ;
-
- gdk_color_parse("cyan",&cyan);
- gdk_colormap_alloc_color(colormap,&cyan,FALSE,TRUE);
- col[vpColor::id_cyan] = &cyan ;
-
- gdk_color_parse("orange",&orange);
- gdk_colormap_alloc_color(colormap,&orange,FALSE,TRUE);
- col[vpColor::id_orange] = &orange ;
-
- gdk_color_parse("purple",&purple);
- gdk_colormap_alloc_color(colormap,&purple,FALSE,TRUE);
- col[vpColor::id_purple] = &purple ;
-
- gdk_color_parse("white",&white);
- gdk_colormap_alloc_color(colormap,&white,FALSE,TRUE);
- col[vpColor::id_white] = &white ;
-
- gdk_color_parse("black",&black);
- gdk_colormap_alloc_color(colormap,&black,FALSE,TRUE);
- col[vpColor::id_black] = &black ;
-
- gdk_color_parse("#C0C0C0",&lightGray);
- gdk_colormap_alloc_color(colormap,&lightGray,FALSE,TRUE);
- col[vpColor::id_lightGray] = &lightGray ;
-
- gdk_color_parse("#808080",&gray);
- gdk_colormap_alloc_color(colormap,&gray,FALSE,TRUE);
- col[vpColor::id_gray] = &gray ;
-
- gdk_color_parse("#404040",&darkGray);
- gdk_colormap_alloc_color(colormap,&darkGray,FALSE,TRUE);
- col[vpColor::id_darkGray] = &darkGray ;
-
- // Try to load a default font
- font = gdk_font_load("-*-times-medium-r-normal-*-16-*-*-*-*-*-*-*");
- if (font == NULL)
- font = gdk_font_load("-*-courier-bold-r-normal-*-*-140-*-*-*-*-*-*");
- if (font == NULL)
- font = gdk_font_load("-*-courier 10 pitch-medium-r-normal-*-16-*-*-*-*-*-*-*");
-
- if(title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-
- displayHasBeenInitialized = true ;
- gdk_window_set_title(widget->window, title_.c_str());
-}
-
-
-/*!
-
- \warning Not implemented yet.
-
- Set the font used to display a text in overlay. The display is
- performed using displayCharString().
-
- \param fontname : The expected font name.
-
- \note Under UNIX, to know all the available fonts, use the
- "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
-
- \sa displayCharString()
-*/
-void
-vpDisplayGTK::setFont(const char *fontname)
-{
- font = gdk_font_load((const gchar*)fontname);
-}
-
-/*!
- Set the window title.
- \param title : Window title.
-*/
-void
-vpDisplayGTK::setTitle(const char *title)
-{
- if (displayHasBeenInitialized)
- {
- if(title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
- gdk_window_set_title(widget->window, title_.c_str());
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Set the window position in the screen.
-
- \param winx, winy : Position of the upper-left window's border in the screen.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void vpDisplayGTK::setWindowPosition(int winx, int winy)
-{
-
- if (displayHasBeenInitialized) {
- gtk_window_move(GTK_WINDOW(widget), winx, winy);
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayGTK::displayImage(const vpImage<unsigned char> &I)
-{
-
- if (displayHasBeenInitialized)
- {
- /* Copie de l'image dans le pixmap fond */
- gdk_draw_gray_image(background,
- gc, 0, 0, (gint)width, (gint)height,
- GDK_RGB_DITHER_NONE,
- I.bitmap,
- (gint)width);
-
- /* Le pixmap background devient le fond de la zone de dessin */
- gdk_window_set_back_pixmap(widget->window, background, FALSE);
-
- /* Affichage */
- //gdk_window_clear(GTK_WINDOW(widget));
- //gdk_flush();
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Display a selection of the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w : Width of the region of interest
-
- \param h : Height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayGTK::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
-{
- if (displayHasBeenInitialized)
- {
- vpImage<unsigned char> Itemp;
- vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(), h, w,Itemp);
- /* Copie de l'image dans le pixmap fond */
- gdk_draw_gray_image(background,
- gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
- GDK_RGB_DITHER_NONE,
- I.bitmap,
- (gint)w);
-
- /* Le pixmap background devient le fond de la zone de dessin */
- gdk_window_set_back_pixmap(widget->window, background, FALSE);
-
- /* Affichage */
- //gdk_window_clear(GTK_WINDOW(widget));
- //gdk_flush();
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayGTK::displayImage(const vpImage<vpRGBa> &I)
-{
-
- if (displayHasBeenInitialized)
- {
-
- /* Copie de l'image dans le pixmap fond */
- gdk_draw_rgb_32_image(background,
- gc, 0, 0, (gint)width, (gint)height,
- GDK_RGB_DITHER_NONE,
- (unsigned char *)I.bitmap,
- (gint)(4*width));
-
- /* Permet de fermer la fen�tre si besoin (cas des s�quences d'images) */
- //while (g_main_iteration(FALSE));
-
- /* Le pixmap background devient le fond de la zone de dessin */
- gdk_window_set_back_pixmap(widget->window, background, FALSE);
-
- /* Affichage */
- //gdk_window_clear(GTK_WINDOW(widget));
- //flushDisplay() ;
-
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Display a selection of the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w : Width of the region of interest
-
- \param h : Height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayGTK::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
-{
- if (displayHasBeenInitialized)
- {
- vpImage<vpRGBa> Itemp;
- vpImageTools::createSubImage(I,(unsigned int)iP.get_i(), (unsigned int)iP.get_j(), h, w, Itemp);
- /* Copie de l'image dans le pixmap fond */
- gdk_draw_rgb_32_image(background,
- gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
- GDK_RGB_DITHER_NONE,
- (unsigned char *)Itemp.bitmap,
- (gint)(4*w));
-
- /* Permet de fermer la fen�tre si besoin (cas des s�quences d'images) */
- //while (g_main_iteration(FALSE));
-
- /* Le pixmap background devient le fond de la zone de dessin */
- gdk_window_set_back_pixmap(widget->window, background, FALSE);
-
- /* Affichage */
- //gdk_window_clear(GTK_WINDOW(widget));
- //flushDisplay() ;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- \warning Not implemented yet.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayGTK::displayImage(const unsigned char * /* I */)
-{
- vpTRACE(" not implemented ") ;
-}
-
-/*!
- Close the window.
-
- \sa init()
-*/
-void vpDisplayGTK::closeDisplay()
-{
- if (col != NULL)
- {
- delete [] col ; col = NULL ;
- }
-
- if (widget != NULL)
- {
- gdk_window_hide (widget->window);
- gdk_window_destroy(widget->window);
- widget = NULL;
- }
- displayHasBeenInitialized = false;
-}
-
-
-/*!
- Flushes the display buffer.
- It's necessary to use this function to see the results of any drawing.
-*/
-void vpDisplayGTK::flushDisplay()
-{
- if (displayHasBeenInitialized)
- {
- gdk_window_clear(widget->window);
- gdk_flush();
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Flushes the display buffer.
- It's necessary to use this function to see the results of any drawing.
-*/
-void vpDisplayGTK::flushDisplayROI(const vpImagePoint &/*iP*/, const unsigned int /*width*/, const unsigned int /*height*/)
-{
- if (displayHasBeenInitialized)
- {
- gdk_window_clear(widget->window);
- gdk_flush();
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- \warning Not implemented yet.
-*/
-void vpDisplayGTK::clearDisplay(const vpColor & /* color */)
-{
- vpTRACE("Not implemented") ;
-}
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpDisplayGTK::displayArrow ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w, unsigned int h,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- try{
- double a = ip2.get_i() - ip1.get_i() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
-
- //if ((a==0)&&(b==0))
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
- {
- // DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
-
- vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
-
- vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- }
- catch (...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param ip : Upper left image point location of the string in the display.
- \param text : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-*/
-void vpDisplayGTK::displayCharString ( const vpImagePoint &ip,
- const char *text,
- const vpColor &color )
-{
- if (displayHasBeenInitialized)
- {
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
- if (font != NULL)
- gdk_draw_string(background, font, gc,
- vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v() ),
- (const gchar *)text);
- else
- std::cout << "Cannot draw string: no font is selected" << std::endl;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-/*!
- Display a circle.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the circle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void vpDisplayGTK::displayCircle ( const vpImagePoint ¢er,
- unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- gdk_gc_set_line_attributes(gc, (gint)thickness,
- GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
-
- if (fill == false)
- gdk_draw_arc(background, gc, FALSE,
- vpMath::round( center.get_u()-radius ),
- vpMath::round( center.get_v()-radius ),
- (gint)(2*radius), (gint)(2*radius), 23040, 23040) ; /* 23040 = 360*64 */
- else
- gdk_draw_arc(background, gc, TRUE,
- vpMath::round( center.get_u()-radius ),
- vpMath::round( center.get_v()-radius ),
- (gint)(2*radius), (gint)(2*radius), 23040, 23040) ; /* 23040 = 360*64 */
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-/*!
- Display a cross at the image point \e ip location.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplayGTK::displayCross ( const vpImagePoint &ip,
- unsigned int size,
- const vpColor &color,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- try{
- double i = ip.get_i();
- double j = ip.get_j();
- vpImagePoint ip1, ip2;
-
- ip1.set_i( i-size/2 );
- ip1.set_j( j );
- ip2.set_i( i+size/2 );
- ip2.set_j( j );
- displayLine ( ip1, ip2, color, thickness ) ;
-
- ip1.set_i( i );
- ip1.set_j( j-size/2 );
- ip2.set_i( i );
- ip2.set_j( j+size/2 );
-
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- catch (...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
-
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayGTK::displayDotLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- if (displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- gdk_gc_set_line_attributes(gc, (gint)thickness,
- GDK_LINE_ON_OFF_DASH, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- gdk_draw_line(background, gc,
- vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ),
- vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) );
- gdk_gc_set_line_attributes(gc, 0,
- GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayGTK::displayLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- gdk_gc_set_line_attributes(gc, (gint)thickness,
- GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- gdk_draw_line(background, gc,
- vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ),
- vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) );
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Display a point at the image point \e ip location.
- \param ip : Point location.
- \param color : Point color.
-*/
-void vpDisplayGTK::displayPoint ( const vpImagePoint &ip,
- const vpColor &color )
-{
- if (displayHasBeenInitialized)
- {
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- gdk_draw_point(background,gc,
- vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v() ) );
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param w,h : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayGTK::displayRectangle ( const vpImagePoint &topLeft,
- unsigned int w, unsigned int h,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
- gdk_gc_set_line_attributes(gc, (gint)thickness,
- GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
-
- if (fill == false)
- gdk_draw_rectangle(background, gc, FALSE,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- (gint)w-1, (gint)h-1);
- else
- gdk_draw_rectangle(background, gc, TRUE,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- (gint)w, (gint)h);
-
- if (thickness > 1)
- gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Display a rectangle.
-
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayGTK::displayRectangle ( const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- gdk_gc_set_line_attributes(gc, (gint)thickness,
- GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
-
- int w = vpMath::round( bottomRight.get_u() - topLeft.get_u() );
- int h = vpMath::round( bottomRight.get_v() - topLeft.get_v() );
-
- if (fill == false)
- gdk_draw_rectangle(background, gc, FALSE,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- w-1,h-1);
- else
- gdk_draw_rectangle(background, gc, TRUE,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- w, h);
-
- if (thickness > 1)
- gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-/*!
- Display a rectangle.
-
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplayGTK::displayRectangle ( const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if (color.id < vpColor::id_unknown)
- gdk_gc_set_foreground(gc, col[color.id]);
- else {
- gdkcolor.red = 256 * color.R;
- gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
- gdk_gc_set_foreground(gc, &gdkcolor);
- }
-
- if ( thickness == 1 ) thickness = 0;
-
- gdk_gc_set_line_attributes(gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
-
- if (fill == false)
- gdk_draw_rectangle(background, gc, FALSE,
- vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getTop() ),
- vpMath::round( rectangle.getWidth()-1 ),
- vpMath::round( rectangle.getHeight()-1 ) );
-
- else
- gdk_draw_rectangle(background, gc, TRUE,
- vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getTop() ),
- vpMath::round( rectangle.getWidth()-1 ),
- vpMath::round( rectangle.getHeight()-1 ) );
-
- if (thickness > 1)
- gdk_gc_set_line_attributes(gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT,
- GDK_JOIN_BEVEL) ;
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-}
-
-
-/*!
- Wait for a click from one of the mouse button.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayGTK::getClick(bool blocking)
-{
- bool ret = false;
-
- int cpt =0;
- if (displayHasBeenInitialized) {
-
-// flushDisplay() ;
- GdkEvent *ev = NULL;
- do {
- 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 == widget->window && ev->type == GDK_BUTTON_PRESS){
- ret = true ;
- //printf("Click detection\n");
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret;
-}
-
-/*!
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
-*/
-bool
-vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking)
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
-
- GdkEvent *ev = NULL;
- double u, v ;
- do {
- while ((ev = gdk_event_get())!=NULL){
- if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u );
- ip.set_v( v );
- ret = true ;
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret ;
-}
-
-
-/*!
-
- Wait for a mouse button click and get the position of the clicked
- pixel. The button used to click is also set.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-*/
-bool
-vpDisplayGTK::getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
- GdkEvent *ev = NULL;
- double u, v ;
- do {
- while ((ev = gdk_event_get())){
- if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS){
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u );
- ip.set_v( v );
-
- switch ((int)((GdkEventButton *)ev)->button) {
- case 1:
- button = vpMouseButton::button1; break;
- case 2:
- button = vpMouseButton::button2; break;
- case 3:
- button = vpMouseButton::button3; break;
- }
- ret = true ;
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
-
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret;
-}
-
-/*!
-
- Wait for a mouse button click release and get the position of the
- image point were the click release occurs. The button used to click is
- also set. Same method as getClick(unsigned int&, unsigned int&,
- vpMouseButton::vpMouseButtonType &, bool).
-
- \param ip [out] : Position of the clicked image point.
-
- \param button [in] : Button used to click.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
- \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
-
-*/
-bool
-vpDisplayGTK::getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- bool ret = false;
-
- if ( displayHasBeenInitialized ) {
-
- //flushDisplay() ;
- GdkEvent *ev = NULL;
- double u, v ;
- do {
- while ((ev = gdk_event_get())!=NULL){
- if ( ev->any.window == widget->window
- && ev->type == GDK_BUTTON_RELEASE) {
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u );
- ip.set_v( v );
-
- switch ( ( int ) ( ( GdkEventButton * ) ev )->button ) {
- case 1:
- button = vpMouseButton::button1; break;
- case 2:
- button = vpMouseButton::button2; break;
- case 3:
- button = vpMouseButton::button3; break;
- }
- ret = true ;
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
-
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE ( "GTK not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "GTK not initialized" ) ) ;
- }
- return ret;
-}
-
-/*
- \brief gets the displayed image (including the overlay plane)
- and returns an RGBa image
-*/
-void vpDisplayGTK::getImage(vpImage<vpRGBa> &I)
-{
-
-
- // shoudl certainly be optimized.
- // doesn't work
- if (displayHasBeenInitialized)
- {
-
- GdkImage *ImageGtk;
- /*
- */
-
- ImageGtk = gdk_image_get(background, 0, 0, (gint)width, (gint)height);
-
-
- I.resize(height,width) ;
- guchar *pos;
- guint32 pixel;
- gint x,y;
- guchar OctetRouge,OctetVert,OctetBleu,mask;
- mask = 0x000000FF;
-
- pos = (unsigned char *)I.bitmap;
- for (y=0;y<(gint)height;y++)
- {
- for (x=0;x<(gint)width;x++)
- {
- pixel = gdk_image_get_pixel(ImageGtk,x,y);
- OctetBleu = (guchar)pixel & mask;
- OctetVert = (guchar)(pixel>>8) & mask;
- OctetRouge = (guchar)(pixel>>16) & mask;
- *pos++ = OctetRouge;
- *pos++ = OctetVert;
- *pos++ = OctetBleu;
- *pos++ = 0;
- }
- }
-
-
- }
- else
- {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-
-}
-
-/*!
- \brief get the window depth (8,16,24,32)
-
- usualy it 24 bits now...
-*/
-unsigned int vpDisplayGTK::getScreenDepth()
-{
-
- unsigned int depth;
-
- depth = (unsigned int)gdk_window_get_visual(widget->window)->depth ;
-
- return (depth);
-}
-
-/*!
- \brief get the window size
-
- \warning Not implemented
-*/
-void vpDisplayGTK::getScreenSize(unsigned int &w, unsigned int &h)
-{
- vpTRACE("Not implemented") ;
- w = 0;
- h = 0;
-}
-
-/*!
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayGTK::getKeyboardEvent(bool blocking)
-{
- bool ret = false;
-
- int cpt =0;
- if (displayHasBeenInitialized) {
-
- GdkEvent *ev = NULL;
- do {
- 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 == widget->window && ev->type == GDK_KEY_PRESS){
- ret = true ;
- //printf("Key press detection\n");
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret;
-}
-
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayGTK::getKeyboardEvent(char *string, bool blocking)
-{
- bool ret = false;
-
- int cpt =0;
- if (displayHasBeenInitialized) {
-
- GdkEvent *ev = NULL;
- do {
- 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 == widget->window && ev->type == GDK_KEY_PRESS){
- //std::cout << "Key val: \"" << gdk_keyval_name (ev->key.keyval) /*ev->key.string*/ << "\"" << std::endl;
- sprintf(string, "%s", gdk_keyval_name (ev->key.keyval));
- ret = true ;
- //printf("Key press detection\n");
- }
- gdk_event_free(ev) ;
- }
- if (blocking){
- flushDisplay();
- vpTime::wait(100);
- }
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret;
-}
-
-/*!
-
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-
-*/
-bool
-vpDisplayGTK::getPointerMotionEvent ( vpImagePoint &ip)
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
- GdkEvent *ev = NULL;
- double u, v ;
- if ((ev = gdk_event_get())){
- if (ev->any.window == widget->window && ev->type == GDK_MOTION_NOTIFY){
- u = ((GdkEventMotion *)ev)->x ;
- v = ((GdkEventMotion *)ev)->y ;
- ip.set_u( u );
- ip.set_v( v );
-
- ret = true ;
- }
- gdk_event_free(ev) ;
- }
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
- return ret;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayGTK::getPointerPosition ( vpImagePoint &ip)
-{
- if (displayHasBeenInitialized)
- {
- int u,v;
- gdk_window_get_pointer(widget->window, &u, &v, NULL);
- ip.set_u( u );
- ip.set_v( v );
- }
- else {
- vpERROR_TRACE("GTK not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-
- return true;
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/device/display/vpDisplayGTK.h b/src/device/display/vpDisplayGTK.h
deleted file mode 100644
index dc58ec2..0000000
--- a/src/device/display/vpDisplayGTK.h
+++ /dev/null
@@ -1,288 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayGTK.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Christophe Collewet
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpDisplayGTK_h
-#define vpDisplayGTK_h
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_GTK) )
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-
-#include <gtk/gtk.h>
-#include <gdk/gdkrgb.h>
-
-
-/*!
- \file vpDisplayGTK.h
- \brief Define the GTK console to display images.
-*/
-
-
-/*!
-
- \class vpDisplayGTK
-
- \ingroup ImageGUI
-
- \brief The vpDisplayGTK allows to display image using the GTK+ library
- version 1.2.
-
- The GTK+ 1.2 library has to be available on the system.
-
- The example below shows how to display an image with this video device.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
-#if defined(VISP_HAVE_GTK)
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
-#ifdef _WIN32
- vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
-#else
- vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
-#endif
-
- vpDisplayGTK d;
-
- // Initialize the display with the image I. Display and image are
- // now link together.
- d.init(I);
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My GTK display");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(50);
- topLeftCorner.set_j(10);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
- char key[10];
- bool ret;
- for (int i=0; i< 200; i++) {
- bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
- }
-
- // Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
- ret = vpDisplay::getKeyboardEvent(I, key, true);
- std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
- std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
- // Wait for a click in the display window
- std::cout << "Wait for a button click..." << std::endl;
- vpDisplay::getClick(I);
-#endif
-}
- \endcode
-*/
-
-class VISP_EXPORT vpDisplayGTK: public vpDisplay
-{
-private:
- //! true if GTK display is ready to use
- GtkWidget *widget;
- GdkPixmap *background;
- GdkGC *gc;
- GdkColor blue,red,yellow,green,cyan,orange,white, black, gdkcolor,
- lightBlue, darkBlue, lightRed, darkRed,lightGreen, darkGreen,
- purple, lightGray, gray, darkGray;
- GdkColormap *colormap;
-
- GdkFont *font;
- guchar *vectgtk;
- GdkColor **col ;
- int ncol, nrow ;
-
- typedef enum {
- id_black=0,
- id_white,
- id_lightGray,
- id_gray,
- id_darkGray,
- id_lightRed,
- id_red,
- id_darkRed,
- id_lightGreen,
- id_green,
- id_darkGreen,
- id_lightBlue,
- id_blue,
- id_darkBlue,
- id_yellow,
- id_cyan,
- id_orange,
- id_purple,
- id_npredefined // Number of predefined colors
- } vpColorIdentifier;
-
-public:
- vpDisplayGTK() ;
- vpDisplayGTK(int winx, int winy, const char *title=NULL) ;
- vpDisplayGTK(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
- vpDisplayGTK(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- virtual ~vpDisplayGTK() ;
-
- void init(vpImage<unsigned char> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
- void init(vpImage<vpRGBa> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- void init(unsigned int width, unsigned int height,
- int winx=-1, int winy=-1 ,
- const char *title=NULL) ;
-
- unsigned int getScreenDepth();
- void getScreenSize(unsigned int &width, unsigned int &height);
- void getImage(vpImage<vpRGBa> &I) ;
-
-protected:
-
- void setFont( const char *fontname );
- void setTitle(const char *title) ;
- void setWindowPosition(int winx, int winy);
-
- void clearDisplay(const vpColor &color=vpColor::white) ;
-
- void closeDisplay() ;
-
- void displayArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4,unsigned int h=2,
- unsigned int thickness=1) ;
- void displayCharString(const vpImagePoint &ip, const char *text,
- const vpColor &color=vpColor::green) ;
-
- void displayCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- void displayCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) ;
- void displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const unsigned char *I) ;
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
- void displayRectangle(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
-
- void flushDisplay() ;
- void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) ;
-
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true) ;
- bool getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) ;
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(char *string, bool blocking=true);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
-
- inline unsigned int getWidth() const { return width ; }
- inline unsigned int getHeight() const { return height ; }
-} ;
-
-#endif
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/device/display/vpDisplayOpenCV.cpp b/src/device/display/vpDisplayOpenCV.cpp
deleted file mode 100644
index a11addc..0000000
--- a/src/device/display/vpDisplayOpenCV.cpp
+++ /dev/null
@@ -1,2220 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayOpenCV.cpp 5204 2015-01-24 13:18:18Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Christophe Collewet
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpDisplayOpenCV.cpp
- \brief Define the OpenCV console to display images.
-*/
-
-#include <visp/vpConfig.h>
-
-#if defined(VISP_HAVE_OPENCV)
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-// Display stuff
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpMath.h>
-#include <visp/vpImageTools.h>
-
-//debug / exception
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayException.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/core/core_c.h> // for CV_FILLED versus cv::FILLED
-
-# ifndef CV_RGB
-# define CV_RGB( r, g, b ) cv::Scalar( (b), (g), (r), 0 )
-# endif
-#endif
-
-std::vector<std::string> vpDisplayOpenCV::m_listTitles = std::vector<std::string>();
-unsigned int vpDisplayOpenCV::m_nbWindows = 0;
-
-/*!
-
- Constructor. Initialize a display to visualize a gray level image
- (8 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title)
- :
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
- background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
- #endif
- fontHeight(10), ncol(0), nrow(0), x_move(0), y_move(0) , move(false),
- x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
- x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
- x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
- x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
- x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
- x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
-{
- init(I, x, y, title) ;
-}
-
-
-/*!
- Constructor. Initialize a display to visualize a RGBa level image
- (32 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title)
- :
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
- background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
- #endif
- fontHeight(10), ncol(0), nrow(0), x_move(0), y_move(0) , move(false),
- x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
- x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
- x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
- x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
- x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
- x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
-{
- init(I, x, y, title) ;
-}
-
-/*!
-
- Constructor that just initialize the display position in the screen
- and the display title.
-
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
- To initialize the display size, you need to call init().
-
- \code
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayOpenCV d(100, 200, "My display");
- vpImage<unsigned char> I(240, 384);
- d.init(I);
-}
- \endcode
-*/
-vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const char *title )
- :
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
- background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
- #endif
- fontHeight(10), ncol(0), nrow(0), x_move(0), y_move(0) , move(false),
- x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
- x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
- x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
- x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
- x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
- x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
-{
- windowXPosition = x;
- windowYPosition = y;
-
- if(title != NULL){
- title_ = std::string(title);
- }
- else{
- std::ostringstream s;
- s << m_nbWindows++;
- title_ = std::string("Window ") + s.str();
- }
-
- bool isInList;
- do{
- isInList = false;
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(m_listTitles[i] == title_){
- std::ostringstream s;
- s << m_nbWindows++;
- title_ = std::string("Window ") + s.str();
- isInList = true;
- break;
- }
- }
- }
- while(isInList);
-
- m_listTitles.push_back(title_);
-}
-
-/*!
- Basic constructor.
-
- To initialize the window position, title and size you may call
- init(vpImage<unsigned char> &, int, int, const char *) or
- init(vpImage<vpRGBa> &, int, int, const char *).
-
- \code
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayOpenCV d;
- vpImage<unsigned char> I(240, 384);
- d.init(I, 100, 200, "My display");
-}
- \endcode
-*/
-vpDisplayOpenCV::vpDisplayOpenCV()
- :
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
- background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
- #endif
- fontHeight(10), ncol(0), nrow(0), x_move(0), y_move(0) , move(false),
- x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
- x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
- x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
- x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
- x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
- x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
-{
-}
-
-/*!
- Destructor.
-*/
-vpDisplayOpenCV::~vpDisplayOpenCV()
-{
- closeDisplay() ;
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvReleaseImage(&background);
-#endif
-}
-
-/*!
- Initialize the display (size, position and title) of a gray level image.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayOpenCV::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title)
-{
-
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
- displayHasBeenInitialized = true ;
-}
-
-/*!
- Initialize the display (size, position and title) of a color
- image in RGBa format.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayOpenCV::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
-
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
- displayHasBeenInitialized = true ;
-}
-
-/*!
- Initialize the display size, position and title.
-
- \param w, h : Width and height of the window.
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
- \exception vpDisplayException::notInitializedError If OpenCV was not build
- with an available display device suach as Gtk, Cocoa, Carbon, Qt.
-*/
-void
-vpDisplayOpenCV::init(unsigned int w, unsigned int h,
- int x, int y,
- const char *title)
-{
- this->width = w;
- this->height = h;
-
- if (x != -1)
- this->windowXPosition = x;
- if (y != -1)
- this->windowYPosition = y;
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- int flags = CV_WINDOW_AUTOSIZE;
-#else
- int flags = cv::WINDOW_AUTOSIZE;
-#endif
-
- if(title_.empty()){
- if(title != NULL){
- title_ = std::string(title);
- }
- else{
-
- std::ostringstream s;
- s << m_nbWindows++;
- title_ = std::string("Window ") + s.str();
- }
-
- bool isInList;
- do{
- isInList = false;
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(m_listTitles[i] == title_){
- std::ostringstream s;
- s << m_nbWindows++;
- title_ = std::string("Window ") + s.str();
- isInList = true;
- break;
- }
- }
- }
- while(isInList);
-
- m_listTitles.push_back(title_);
- }
-
- /* Create the window*/
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if (cvNamedWindow( this->title_.c_str(), flags ) < 0) {
- vpERROR_TRACE("OpenCV was not built with a display device");
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV was not built with a display device")) ;
- }
-#else
- cv::namedWindow( this->title_, flags );
-#endif
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvMoveWindow( this->title_.c_str(), this->windowXPosition, this->windowYPosition );
-#else
- cv::moveWindow( this->title_.c_str(), this->windowXPosition, this->windowYPosition );
-#endif
- move = false;
- lbuttondown = false;
- mbuttondown = false;
- rbuttondown = false;
- lbuttonup = false;
- mbuttonup = false;
- rbuttonup = false;
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvSetMouseCallback( this->title_.c_str(), on_mouse, this );
- col = new CvScalar[vpColor::id_unknown] ;
-#else
- cv::setMouseCallback( this->title_, on_mouse, this );
- col = new cv::Scalar[vpColor::id_unknown] ;
-#endif
-
- /* Create color */
- vpColor pcolor; // Predefined colors
- pcolor = vpColor::lightBlue;
- col[vpColor::id_lightBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::blue;
- col[vpColor::id_blue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::darkBlue;
- col[vpColor::id_darkBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::lightRed;
- col[vpColor::id_lightRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::red;
- col[vpColor::id_red] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::darkRed;
- col[vpColor::id_darkRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::lightGreen;
- col[vpColor::id_lightGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::green;
- col[vpColor::id_green] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::darkGreen;
- col[vpColor::id_darkGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::yellow;
- col[vpColor::id_yellow] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::cyan;
- col[vpColor::id_cyan] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::orange;
- col[vpColor::id_orange] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::purple;
- col[vpColor::id_purple] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::white;
- col[vpColor::id_white] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::black;
- col[vpColor::id_black] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::lightGray;
- col[vpColor::id_lightGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::gray;
- col[vpColor::id_gray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
- pcolor = vpColor::darkGray;
- col[vpColor::id_darkGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- font = new CvFont;
- cvInitFont( font, CV_FONT_HERSHEY_PLAIN, 0.70f,0.70f);
- CvSize fontSize;
- int baseline;
- cvGetTextSize( "A", font, &fontSize, &baseline );
-#else
- int thickness = 1;
- cv::Size fontSize;
- int baseline;
- fontSize = cv::getTextSize( "A", font, fontScale, thickness, &baseline );
-#endif
-
- fontHeight = fontSize.height + baseline;
- displayHasBeenInitialized = true ;
-}
-
-
-/*!
- \warning This method is not yet implemented.
-
- Set the font used to display a text in overlay. The display is
- performed using displayCharString().
-
- \param font : The expected font name. The available fonts are given by
- the "xlsfonts" binary. To choose a font you can also use the
- "xfontsel" binary.
-
- \note Under UNIX, to know all the available fonts, use the
- "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
-
- \sa displayCharString()
-*/
-void
-vpDisplayOpenCV::setFont(const char * /* font */)
-{
- vpERROR_TRACE("Not yet implemented" ) ;
-}
-
-/*!
- Set the window title.
-
- \warning This method is not implemented yet.
-
- \param title : Window title.
- */
-void
-vpDisplayOpenCV::setTitle(const char * /* title */)
-{
-// static bool warn_displayed = false;
-// if (! warn_displayed) {
-// vpTRACE("Not implemented");
-// warn_displayed = true;
-// }
-}
-
-
-/*!
- Set the window position in the screen.
-
- \param winx, winy : Position of the upper-left window's border in the screen.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void vpDisplayOpenCV::setWindowPosition(int winx, int winy)
-{
- if (displayHasBeenInitialized) {
- this->windowXPosition = winx;
- this->windowYPosition = winy;
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvMoveWindow( this->title_.c_str(), winx, winy );
-#else
- cv::moveWindow( this->title_.c_str(), winx, winy );
-#endif
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-/*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning suppres the overlay drawing
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &I)
-{
- if (displayHasBeenInitialized)
- {
- vpImage<vpRGBa> Ic;
- vpImageConvert::convert(I,Ic);
- vpImageConvert::convert(Ic,background);
- /* Copie de l'image dans le pixmap fond */
- width = I.getWidth();
- height = I.getHeight();
- /* Le pixmap background devient le fond de la zone de dessin */
- }
- else
- {
- vpERROR_TRACE("openCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a selection of the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w, h : Width and height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
-{
- if (displayHasBeenInitialized)
- {
- vpImage<unsigned char> Itemp;
- vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(),h,w,Itemp);
- vpImage<vpRGBa> Ic;
- vpImageConvert::convert(Itemp,Ic);
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- int depth = 8;
- int channels = 3;
- CvSize size = cvSize((int)this->width, (int)this->height);
- if (background != NULL){
- if(background->nChannels != channels || background->depth != depth
- || background->height != (int) I.getHeight() || background->width != (int) I.getWidth()){
- if(background->nChannels != 0) cvReleaseImage(&background);
- background = cvCreateImage( size, depth, channels );
- }
- }
- else background = cvCreateImage( size, depth, channels );
- IplImage* Ip = NULL;
- vpImageConvert::convert(Ic, Ip);
- unsigned char * input = (unsigned char*)Ip->imageData;
- unsigned char * output = (unsigned char*)background->imageData;
-
- unsigned int iwidth = Ic.getWidth();
-
- output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- *(output+3*j) = *(input+j*3);
- *(output+3*j+1) = *(input+j*3+1);
- *(output+3*j+2) = *(input+j*3+2);
- j++;
- }
- input = input + 3*iwidth;
- output = output + 3*this->width;
- i++;
- }
-
- cvReleaseImage(&Ip);
-#else
- int depth = CV_8U;
- int channels = 3;
- cv::Size size((int)this->width, (int)this->height);
- if(background.channels() != channels || background.depth() != depth
- || background.rows != (int) I.getHeight() || background.cols != (int) I.getWidth()){
- background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
- }
-
- cv::Mat Ip;
- vpImageConvert::convert(Ic, Ip);
-
- unsigned char * input = (unsigned char*)Ip.data;
- unsigned char * output = (unsigned char*)background.data;
-
- unsigned int iwidth = Ic.getWidth();
-
- output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- *(output+3*j) = *(input+j*3);
- *(output+3*j+1) = *(input+j*3+1);
- *(output+3*j+2) = *(input+j*3+2);
- j++;
- }
- input = input + 3*iwidth;
- output = output + 3*this->width;
- i++;
- }
-#endif
- }
- else
- {
- vpERROR_TRACE("openCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-
-/*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning suppres the overlay drawing
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &I)
-{
-
- if (displayHasBeenInitialized)
- {
- /* Copie de l'image dans le pixmap fond */
-
- vpImageConvert::convert(I,background);
- /* Copie de l'image dans le pixmap fond */
- width = I.getWidth();
- height = I.getHeight();
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a selection of the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w, h : Width and height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayOpenCV::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
-{
- if (displayHasBeenInitialized)
- {
- vpImage<vpRGBa> Ic;
- vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(),h,w,Ic);
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- CvSize size = cvSize((int)this->width, (int)this->height);
- int depth = 8;
- int channels = 3;
- if (background != NULL){
- if(background->nChannels != channels || background->depth != depth
- || background->height != (int) I.getHeight() || background->width != (int) I.getWidth()){
- if(background->nChannels != 0) cvReleaseImage(&background);
- background = cvCreateImage( size, depth, channels );
- }
- }
- else background = cvCreateImage( size, depth, channels );
-
- IplImage* Ip = NULL;
- vpImageConvert::convert(Ic, Ip);
-
- unsigned char * input = (unsigned char*)Ip->imageData;
- unsigned char * output = (unsigned char*)background->imageData;
-
- unsigned int iwidth = Ic.getWidth();
-
- output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- *(output+3*j) = *(input+j*3);
- *(output+3*j+1) = *(input+j*3+1);
- *(output+3*j+2) = *(input+j*3+2);
- j++;
- }
- input = input + 3*iwidth;
- output = output + 3*this->width;
- i++;
- }
-
- cvReleaseImage(&Ip);
-#else
- int depth = CV_8U;
- int channels = 3;
- cv::Size size((int)this->width, (int)this->height);
- if(background.channels() != channels || background.depth() != depth
- || background.rows != (int) I.getHeight() || background.cols != (int) I.getWidth()){
- background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
- }
- cv::Mat Ip;
- vpImageConvert::convert(Ic, Ip);
-
- unsigned char * input = (unsigned char*)Ip.data;
- unsigned char * output = (unsigned char*)background.data;
-
- unsigned int iwidth = Ic.getWidth();
-
- output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- *(output+3*j) = *(input+j*3);
- *(output+3*j+1) = *(input+j*3+1);
- *(output+3*j+2) = *(input+j*3+2);
- j++;
- }
- input = input + 3*iwidth;
- output = output + 3*this->width;
- i++;
- }
-#endif
- }
- else
- {
- vpERROR_TRACE("openCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-
-/*!
- \warning ot implemented yet
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayOpenCV::displayImage(const unsigned char * /* I */)
-{
- vpTRACE(" not implemented ") ;
-}
-
-/*!
-
- Close the window.
-
- \sa init()
-
-*/
-void vpDisplayOpenCV::closeDisplay()
-{
- if (col != NULL)
- {
- delete [] col ; col = NULL ;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if (font != NULL)
- {
- delete font ;
- font = NULL ;
- }
-#endif
- if (displayHasBeenInitialized) {
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvDestroyWindow( this->title_.c_str() );
-#else
- cv::destroyWindow( this->title_ );
-#endif
-
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(title_ == m_listTitles[i]){
- m_listTitles.erase(m_listTitles.begin()+(long int)i);
- break;
- }
- }
-
- title_.clear();
-
- displayHasBeenInitialized= false;
- }
-}
-
-
-/*!
- Flushes the OpenCV buffer.
- It's necessary to use this function to see the results of any drawing.
-
-*/
-void vpDisplayOpenCV::flushDisplay()
-{
- if (displayHasBeenInitialized)
- {
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvShowImage(this->title_.c_str(), background );
- cvWaitKey(5);
-#else
- cv::imshow(this->title_, background );
- cv::waitKey(5);
-#endif
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Flushes the OpenCV buffer.
- It's necessary to use this function to see the results of any drawing.
-
-*/
-void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint &/*iP*/, const unsigned int /*width*/, const unsigned int /*height*/)
-{
- if (displayHasBeenInitialized)
- {
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvShowImage(this->title_.c_str(), background );
- cvWaitKey(5);
-#else
- cv::imshow(this->title_.c_str(), background );
- cv::waitKey(5);
-#endif
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-
-/*!
- \warning Not implemented yet.
-*/
-void vpDisplayOpenCV::clearDisplay(const vpColor & /* color */)
-{
- static bool warn_displayed = false;
- if (! warn_displayed) {
- vpTRACE("Not implemented");
- warn_displayed = true;
- }
-}
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpDisplayOpenCV::displayArrow ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w, unsigned int h,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- try{
- double a = ip2.get_i() - ip1.get_i() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
-
- //if ((a==0)&&(b==0))
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon())
- &&(std::fabs(b)<= std::numeric_limits<double>::epsilon()))
- {
- // DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
-
- vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
-
- vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- }
- catch (...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param ip : Upper left image point location of the string in the display.
- \param text : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-*/
-void vpDisplayOpenCV::displayCharString( const vpImagePoint &ip,
- const char *text,
- const vpColor &color )
-{
- if (displayHasBeenInitialized)
- {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvPutText( background, text,
- cvPoint( vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v()+fontHeight ) ),
- font, col[color.id] );
-#else
- cv::putText( background, text,
- cv::Point( vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v()+fontHeight ) ),
- font, fontScale, col[color.id] );
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvPutText( background, text,
- cvPoint( vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v()+fontHeight ) ),
- font, cvcolor );
-#else
- cv::putText( background, text,
- cv::Point( vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v()+fontHeight ) ),
- font, fontScale, cvcolor );
-#endif
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-/*!
- Display a circle.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the circle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er,
- unsigned int radius,
- const vpColor &color,
- bool fill ,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- if (fill == false) {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvCircle( background,
- cvPoint( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, col[color.id], (int)thickness);
-#else
- cv::circle( background,
- cv::Point( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, col[color.id], (int)thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvCircle( background,
- cvPoint( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, cvcolor, (int)thickness);
-#else
- cv::circle( background,
- cv::Point( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, cvcolor, (int)thickness);
-#endif
- }
- }
- else {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- int filled = cv::FILLED;
-#else
- int filled = CV_FILLED;
-#endif
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvCircle( background,
- cvPoint( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, col[color.id], filled);
-#else
- cv::circle( background,
- cv::Point( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, col[color.id], filled);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvCircle( background,
- cvPoint( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, cvcolor, filled);
-#else
- cv::circle( background,
- cv::Point( vpMath::round( center.get_u() ),
- vpMath::round( center.get_v() ) ),
- (int)radius, cvcolor, filled);
-#endif
- }
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a cross at the image point \e ip location.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void
-vpDisplayOpenCV::displayCross(const vpImagePoint &ip,
- unsigned int size,
- const vpColor &color,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- vpImagePoint top,bottom,left,right;
- top.set_i(ip.get_i()-size/2);
- top.set_j(ip.get_j());
- bottom.set_i(ip.get_i()+size/2);
- bottom.set_j(ip.get_j());
- left.set_i(ip.get_i());
- left.set_j(ip.get_j()-size/2);
- right.set_i(ip.get_i());
- right.set_j(ip.get_j()+size/2);
- try{
- displayLine(top, bottom, color, thickness) ;
- displayLine(left, right, color, thickness) ;
- }
- catch (...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
-
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-
-}
-
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
-
- \warning Dot lines are not yet implemented in OpenCV. We display a
- normal line instead.
-
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void
-vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness)
-{
-
- if (displayHasBeenInitialized)
- {
- //vpTRACE("Dot lines are not yet implemented");
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvLine( background,
- cvPoint( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cvPoint( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- col[color.id], (int) thickness);
-#else
- cv::line( background,
- cv::Point( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cv::Point( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- col[color.id], (int) thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvLine( background,
- cvPoint( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cvPoint( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- cvcolor, (int) thickness);
-#else
- cv::line( background,
- cv::Point( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cv::Point( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- cvcolor, (int) thickness);
-#endif
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void
-vpDisplayOpenCV::displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvLine( background,
- cvPoint( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cvPoint( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- col[color.id], (int) thickness);
-#else
- cv::line( background,
- cv::Point( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cv::Point( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- col[color.id], (int) thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvLine( background,
- cvPoint( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cvPoint( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- cvcolor, (int) thickness);
-#else
- cv::line( background,
- cv::Point( vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ) ),
- cv::Point( vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) ),
- cvcolor, (int) thickness);
-#endif
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a point at the image point \e ip location.
- \param ip : Point location.
- \param color : Point color.
-*/
-void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip,
- const vpColor &color)
-{
- if (displayHasBeenInitialized)
- {
- displayLine(ip,ip,color,1);
-// if (color.id < vpColor::id_unknown) {
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3 )] = (uchar)col[color.id].val[0];
-//
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3+1 )] = (uchar)col[color.id].val[1];
-//
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3+2 )] = (uchar)col[color.id].val[2];
-// }
-// else {
-// cvcolor = CV_RGB(color.R, color.G, color.B) ;
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3 )] = (uchar)cvcolor.val[0];
-//
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3+1 )] = (uchar)cvcolor.val[1];
-//
-// ((uchar*)(background->imageData
-// + background->widthStep*vpMath::round( ip.get_i() )))
-// [vpMath::round( ip.get_j()*3+2 )] = (uchar)cvcolor.val[2];
-//
-// }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param w,h : Rectangle size in terms of width and height.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft,
- unsigned int w, unsigned int h,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- if (fill == false) {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- col[color.id], (int)thickness);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- col[color.id], (int)thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- cvcolor, (int)thickness);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- cvcolor, (int)thickness);
-#endif
- }
- }
- else {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- int filled = cv::FILLED;
-#else
- int filled = CV_FILLED;
-#endif
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- col[color.id], filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- col[color.id], filled);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- cvcolor, filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( topLeft.get_u()+w ),
- vpMath::round( topLeft.get_v()+h ) ),
- cvcolor, filled);
-#endif
- }
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-/*!
- Display a rectangle.
-
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayOpenCV::displayRectangle ( const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if (displayHasBeenInitialized)
- {
- if (fill == false) {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- col[color.id], (int)thickness);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- col[color.id], (int)thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- cvcolor, (int)thickness);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- cvcolor, (int)thickness);
-#endif
- }
- }
- else {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- int filled = cv::FILLED;
-#else
- int filled = CV_FILLED;
-#endif
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- col[color.id], filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- col[color.id], filled);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cvPoint( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- cvcolor, filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ) ),
- cv::Point( vpMath::round( bottomRight.get_u() ),
- vpMath::round( bottomRight.get_v() ) ),
- cvcolor, filled);
-#endif
- }
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-/*!
- Display a rectangle.
-
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplayOpenCV::displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- if (displayHasBeenInitialized)
- {
- if (fill == false) {
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cvPoint( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- col[color.id], (int)thickness);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cv::Point( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- col[color.id], (int)thickness);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cvPoint( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- cvcolor, (int)thickness);
-
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cv::Point( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- cvcolor, (int)thickness);
-
-#endif
- }
- }
- else {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- int filled = cv::FILLED;
-#else
- int filled = CV_FILLED;
-#endif
- if (color.id < vpColor::id_unknown) {
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cvPoint( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- col[color.id], filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cv::Point( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- col[color.id], filled);
-#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- cvRectangle( background,
- cvPoint( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cvPoint( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- cvcolor, filled);
-#else
- cv::rectangle( background,
- cv::Point( vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getBottom() ) ),
- cv::Point( vpMath::round( rectangle.getRight() ),
- vpMath::round( rectangle.getTop() ) ),
- cvcolor, filled);
-#endif
- }
- }
- }
- else
- {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
-}
-
-
-
-/*!
- Wait for a click from one of the mouse button.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayOpenCV::getClick(bool blocking)
-{
- bool ret = false;
- if (displayHasBeenInitialized) {
- flushDisplay() ;
- if (blocking){
- lbuttondown = false;
- mbuttondown = false;
- rbuttondown = false;
- }
- do {
- if (lbuttondown){
- ret = true ;
- lbuttondown = false;
- }
- if (mbuttondown){
- ret = true ;
- mbuttondown = false;
- }
- if (rbuttondown){
- ret = true ;
- rbuttondown = false;
- }
- if (blocking)
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvWaitKey(10);
-#else
- cv::waitKey(10);
-#endif
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- return ret;
-}
-
-/*!
-
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
-*/
-bool
-vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
- flushDisplay() ;
-
- double u, v;
-
- if (blocking){
- lbuttondown = false;
- mbuttondown = false;
- rbuttondown = false;
- }
- do {
- if (lbuttondown){
- ret = true ;
- u = (unsigned int)x_lbuttondown;
- v = (unsigned int)y_lbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- lbuttondown = false;
- }
- if (mbuttondown){
- ret = true ;
- u = (unsigned int)x_mbuttondown;
- v = (unsigned int)y_mbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- mbuttondown = false;
- }
- if (rbuttondown){
- ret = true ;
- u = (unsigned int)x_rbuttondown;
- v = (unsigned int)y_rbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- rbuttondown = false;
- }
- if (blocking)
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvWaitKey(10);
-#else
- cv::waitKey(10);
-#endif
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- return ret ;
-}
-
-
-/*!
-
- Wait for a mouse button click and get the position of the clicked
- pixel. The button used to click is also set.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-*/
-bool
-vpDisplayOpenCV::getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
- //flushDisplay() ;
- double u, v;
- if (blocking){
- lbuttondown = false;
- mbuttondown = false;
- rbuttondown = false;
- }
- do {
- if (lbuttondown){
- ret = true ;
- u = (unsigned int)x_lbuttondown;
- v = (unsigned int)y_lbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button1;
- lbuttondown = false;
- }
- if (mbuttondown){
- ret = true ;
- u = (unsigned int)x_mbuttondown;
- v = (unsigned int)y_mbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button2;
- mbuttondown = false;
- }
- if (rbuttondown){
- ret = true ;
- u = (unsigned int)x_rbuttondown;
- v = (unsigned int)y_rbuttondown;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button3;
- rbuttondown = false;
- }
- if (blocking)
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvWaitKey(10);
-#else
- cv::waitKey(10);
-#endif
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- return ret;
-}
-
-/*!
-
- Wait for a mouse button click release and get the position of the
- image point were the click release occurs. The button used to click is
- also set. Same method as getClick(unsigned int&, unsigned int&,
- vpMouseButton::vpMouseButtonType &, bool).
-
- \param ip [out] : Position of the clicked image point.
-
- \param button [in] : Button used to click.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
- \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
-
-*/
-bool
-vpDisplayOpenCV::getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- bool ret = false;
- if (displayHasBeenInitialized) {
- //flushDisplay() ;
- double u, v;
- if (blocking){
- lbuttonup = false;
- mbuttonup = false;
- rbuttonup = false;
- }
- do {
- if (lbuttonup){
- ret = true ;
- u = (unsigned int)x_lbuttonup;
- v = (unsigned int)y_lbuttonup;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button1;
- lbuttonup = false;
- }
- if (mbuttonup){
- ret = true ;
- u = (unsigned int)x_mbuttonup;
- v = (unsigned int)y_mbuttonup;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button2;
- mbuttonup = false;
- }
- if (rbuttonup){
- ret = true ;
- u = (unsigned int)x_rbuttonup;
- v = (unsigned int)y_rbuttonup;
- ip.set_u( u );
- ip.set_v( v );
- button = vpMouseButton::button3;
- rbuttonup = false;
- }
- if (blocking)
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvWaitKey(10);
-#else
- cv::waitKey(10);
-#endif
- } while ( ret == false && blocking == true);
- }
- else {
- vpERROR_TRACE ( "OpenCV not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "OpenCV not initialized" ) ) ;
- }
- return ret;
-}
-
-/*
- \brief gets the displayed image (including the overlay plane)
- and returns an RGBa image
-*/
-void vpDisplayOpenCV::getImage(vpImage<vpRGBa> &I)
-{
- vpImageConvert::convert(background,I);
- // should certainly be optimized.
-}
-
-void vpDisplayOpenCV::on_mouse( int event, int x, int y, int /*flags*/, void* display )
-{
- vpDisplayOpenCV* disp = (vpDisplayOpenCV*)display;
- switch ( event )
- {
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_MOUSEMOVE:
-#else
- case cv::EVENT_MOUSEMOVE:
-#endif
- {
- disp->move = true;
- disp->x_move = x;
- disp->y_move = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_LBUTTONDOWN:
-#else
- case cv::EVENT_LBUTTONDOWN:
-#endif
- {
- disp->lbuttondown = true;
- disp->x_lbuttondown = x;
- disp->y_lbuttondown = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_MBUTTONDOWN:
-#else
- case cv::EVENT_MBUTTONDOWN:
-#endif
- {
- disp->mbuttondown = true;
- disp->x_mbuttondown = x;
- disp->y_mbuttondown = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_RBUTTONDOWN:
-#else
- case cv::EVENT_RBUTTONDOWN:
-#endif
- {
- disp->rbuttondown = true;
- disp->x_rbuttondown = x;
- disp->y_rbuttondown = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_LBUTTONUP:
-#else
- case cv::EVENT_LBUTTONUP:
-#endif
- {
- disp->lbuttonup = true;
- disp->x_lbuttonup = x;
- disp->y_lbuttonup = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_MBUTTONUP:
-#else
- case cv::EVENT_MBUTTONUP:
-#endif
- {
- disp->mbuttonup = true;
- disp->x_mbuttonup = x;
- disp->y_mbuttonup = y;
- break;
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- case CV_EVENT_RBUTTONUP:
-#else
- case cv::EVENT_RBUTTONUP:
-#endif
- {
- disp->rbuttonup = true;
- disp->x_rbuttonup = x;
- disp->y_rbuttonup = y;
- break;
- }
-
- default :
- break;
- }
-}
-
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayOpenCV::getKeyboardEvent(bool blocking)
-{
- int key_pressed;
- int delay;
- if (displayHasBeenInitialized) {
- flushDisplay() ;
- if (blocking)
- delay = 0;
- else
- delay = 10;
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- key_pressed = cvWaitKey(delay);
-#else
- key_pressed = cv::waitKey(delay);
-#endif
-
- if (key_pressed == -1)
- return false;
- return true;
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- //return false; // Never reached after throw()
-}
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayOpenCV::getKeyboardEvent(char *string, bool blocking)
-{
- int key_pressed;
- int delay;
- if (displayHasBeenInitialized) {
- flushDisplay() ;
- if (blocking)
- delay = 0;
- else
- delay = 10;
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- key_pressed = cvWaitKey(delay);
-#else
- key_pressed = cv::waitKey(delay);
-#endif
- if (key_pressed == -1)
- return false;
- else {
- //std::cout << "Key pressed: \"" << key_pressed << "\"" << std::endl;
- sprintf(string, "%c", key_pressed);
- }
- return true;
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- //return false; // Never reached after throw()
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \warning Not implemented yet.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayOpenCV::getPointerMotionEvent (vpImagePoint &ip )
-{
- bool ret = false;
-
- if (displayHasBeenInitialized) {
- //flushDisplay() ;
- double u, v;
- if (move){
- ret = true ;
- u = (unsigned int)x_move;
- v = (unsigned int)y_move;
- ip.set_u( u );
- ip.set_v( v );
- move = false;
- }
- }
-
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- return ret;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayOpenCV::getPointerPosition ( vpImagePoint &ip)
-{
- if (displayHasBeenInitialized) {
- //vpTRACE("Not implemented yet");
- bool moved = getPointerMotionEvent(ip);
- if (!moved)
- {
- double u, v;
- u = (unsigned int)x_move;
- v = (unsigned int)y_move;
- ip.set_u( u );
- ip.set_v( v );
- }
- return false;
- }
- else {
- vpERROR_TRACE("OpenCV not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- //return false; // Never reached after throw()
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/device/display/vpDisplayOpenCV.h b/src/device/display/vpDisplayOpenCV.h
deleted file mode 100644
index 4848d61..0000000
--- a/src/device/display/vpDisplayOpenCV.h
+++ /dev/null
@@ -1,284 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayOpenCV.h 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Christophe Collewet
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpDisplayOpenCV_h
-#define vpDisplayOpenCV_h
-
-#include <visp/vpConfig.h>
-#if defined(VISP_HAVE_OPENCV)
-
-#include <visp/vpDisplay.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
-#else
-# include <cv.h>
-# include <highgui.h>
-# include <cxcore.h>
-#endif
-
-/*!
- \file vpDisplayOpenCV.h
- \brief Define the OpenCV console to display images.
-*/
-
-
-/*!
-
- \class vpDisplayOpenCV
-
- \ingroup ImageGUI
-
- \brief The vpDisplayOpenCV allows to display image using the opencv library.
-
- The example below shows how to display an image with this video device.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV)
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
- vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
-
- vpDisplayOpenCV d;
-
- // Initialize the display with the image I. Display and image are
- // now link together.
- d.init(I);
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My OpenCV display");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(10);
- topLeftCorner.set_j(50);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
- char key[10];
- bool ret;
- for (int i=0; i< 200; i++) {
- bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
- }
-
- // Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
- ret = vpDisplay::getKeyboardEvent(I, key, true);
- std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
- std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
- // Wait for a click in the display window
- std::cout << "Wait for a button click..." << std::endl;
- vpDisplay::getClick(I);
-#endif
-}
- \endcode
-*/
-
-class VISP_EXPORT vpDisplayOpenCV: public vpDisplay
-{
-private:
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- //! true if OpenCV display is ready to use
- IplImage* background;
- CvScalar *col ;
- CvScalar cvcolor;
- CvFont *font;
-#else
- cv::Mat background;
- cv::Scalar *col ;
- cv::Scalar cvcolor;
- int font;
- float fontScale;
-#endif
- static std::vector<std::string> m_listTitles;
- static unsigned int m_nbWindows ;
- int fontHeight;
- int ncol, nrow ;
- int x_move;
- int y_move;
- bool move;
- int x_lbuttondown ;
- int y_lbuttondown ;
- bool lbuttondown;
- int x_mbuttondown ;
- int y_mbuttondown ;
- bool mbuttondown;
- int x_rbuttondown ;
- int y_rbuttondown ;
- bool rbuttondown;
- int x_lbuttonup ;
- int y_lbuttonup ;
- bool lbuttonup;
- int x_mbuttonup ;
- int y_mbuttonup ;
- bool mbuttonup;
- int x_rbuttonup ;
- int y_rbuttonup ;
- bool rbuttonup;
-
-public:
- vpDisplayOpenCV() ;
- vpDisplayOpenCV(int winx, int winy, const char *title=NULL) ;
- vpDisplayOpenCV(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
- vpDisplayOpenCV(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- virtual ~vpDisplayOpenCV() ;
-
- void init(vpImage<unsigned char> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
- void init(vpImage<vpRGBa> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- void init(unsigned int width, unsigned int height,
- int winx=-1, int winy=-1 ,
- const char *title=NULL) ;
- void getImage(vpImage<vpRGBa> &I) ;
-
-protected:
- void setFont( const char *font );
- void setTitle(const char *title) ;
- void setWindowPosition(int winx, int winy);
-
- void clearDisplay(const vpColor &color=vpColor::white) ;
-
- void closeDisplay() ;
-
- void displayArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4,unsigned int h=2,
- unsigned int thickness=1) ;
-
- void displayCharString(const vpImagePoint &ip, const char *text,
- const vpColor &color=vpColor::green) ;
-
- void displayCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- void displayCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) ;
- void displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const unsigned char *I) ;
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
- void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
-
- void displayRectangle(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
-
- void flushDisplay() ;
- void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true) ;
- bool getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) ;
-
- inline unsigned int getWidth() const { return width ; }
- inline unsigned int getHeight() const { return height ; }
-
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(char *string, bool blocking=true);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
-
- static void on_mouse( int event, int x, int y, int flags, void* param );
-} ;
-
-#endif
-#endif
diff --git a/src/device/display/vpDisplayX.cpp b/src/device/display/vpDisplayX.cpp
deleted file mode 100644
index f2a9d99..0000000
--- a/src/device/display/vpDisplayX.cpp
+++ /dev/null
@@ -1,3400 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayX.cpp 5214 2015-01-27 18:33:01Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Fabien Spindler
- * Anthony Saunier
- *
- *****************************************************************************/
-
-/*!
- \file vpDisplayX.cpp
- \brief Define the X11 console to display images.
-*/
-
-
-#include <visp/vpConfig.h>
-#ifdef VISP_HAVE_X11
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-// Display stuff
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-
-//debug / exception
-#include <visp/vpDebug.h>
-#include <visp/vpDisplayException.h>
-
-// math
-#include <visp/vpMath.h>
-
-/*!
-
- Constructor : initialize a display to visualize a gray level image
- (8 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-vpDisplayX::vpDisplayX ( vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title )
- : display(NULL), window(), Ximage(NULL), lut(), context(),
- screen(0), planes(0), event(), pixmap(), x_color(NULL),
- screen_depth(8), xcolor(), values(), size(0), ximage_data_init(false),
- RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
-{
- init ( I, x, y, title ) ;
-}
-
-
-
-/*!
- Constructor : initialize a display to visualize a RGBa level image
- (32 bits).
-
- \param I : Image to be displayed (not that image has to be initialized).
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title )
- : display(NULL), window(), Ximage(NULL), lut(), context(),
- screen(0), planes(0), event(), pixmap(), x_color(NULL),
- screen_depth(8), xcolor(), values(), size(0), ximage_data_init(false),
- RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
-{
- init ( I, x, y, title ) ;
-}
-
-/*!
-
- Constructor that just initialize the display position in the screen
- and the display title.
-
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
- To initialize the display size, you need to call init().
-
- \code
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayX d(100, 200, "My display");
- vpImage<unsigned char> I(240, 384);
- d.init(I);
-}
- \endcode
-*/
-vpDisplayX::vpDisplayX ( int x, int y, const char *title )
- : display(NULL), window(), Ximage(NULL), lut(), context(),
- screen(0), planes(0), event(), pixmap(), x_color(NULL),
- screen_depth(8), xcolor(), values(), size(0), ximage_data_init(false),
- RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
-{
- windowXPosition = x ;
- windowYPosition = y ;
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-}
-
-/*!
- Basic constructor.
-
- To initialize the window position, title and size you may call
- init(vpImage<unsigned char> &, int, int, const char *) or
- init(vpImage<vpRGBa> &, int, int, const char *).
-
- \code
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpDisplayX d;
- vpImage<unsigned char> I(240, 384);
- d.init(I, 100, 200, "My display");
-}
- \endcode
-*/
-vpDisplayX::vpDisplayX()
- : display(NULL), window(), Ximage(NULL), lut(), context(),
- screen(0), planes(0), event(), pixmap(), x_color(NULL),
- screen_depth(8), xcolor(), values(), size(0), ximage_data_init(false),
- RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
-{
-}
-
-/*!
- Destructor.
-*/
-vpDisplayX::~vpDisplayX()
-{
- closeDisplay() ;
-}
-
-/*!
- Initialize the display (size, position and title) of a gray level image.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const char *title )
-{
-
- if (x_color == NULL) {
- // id_unknown = number of predefined colors
- x_color= new unsigned long [vpColor::id_unknown];
- }
-
- XSizeHints hints;
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-
- // Positionnement de la fenetre dans l'ecran.
- if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
- {
- hints.flags = 0;
- }
- else
- {
- hints.flags = USPosition;
- hints.x = windowXPosition;
- hints.y = windowYPosition;
- }
-
- // setup X11 --------------------------------------------------
- width = I.getWidth();
- height = I.getHeight();
- display = XOpenDisplay ( NULL );
- if ( display == NULL )
- {
- vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server." ) ) ;
- }
-
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
-
- if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
- windowXPosition, windowYPosition, width, height, 1,
- BlackPixel ( display, screen ),
- WhitePixel ( display, screen ) ) ) == 0 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
- }
-
- //
- // Create color table for 8 and 16 bits screen
- //
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
- xcolor.red = 256 * i;
- xcolor.green = 256 * i;
- xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
- }
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
- }
-
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
- xcolor.pad = 0;
- xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
- }
- colortable[i] = xcolor.pixel;
- }
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
-
- Visual *visual = DefaultVisual (display, screen);
- RMask = visual->red_mask;
- GMask = visual->green_mask;
- BMask = visual->blue_mask;
-
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
- GShift = 15 - getMsb(GMask);
- BShift = 15 - getMsb(BMask);
- }
-
- //
- // Create colors for overlay
- //
- switch ( screen_depth )
- {
- case 8:
- // Color BLACK and WHITE are set properly by default.
-
- // Color LIGHT GRAY.
- x_color[vpColor::id_lightGray] = 254;
- xcolor.pixel = x_color[vpColor::id_lightGray] ;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GRAY.
- x_color[vpColor::id_gray] = 253;
- xcolor.pixel = x_color[vpColor::id_gray] ;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GRAY.
- x_color[vpColor::id_darkGray] = 252;
- xcolor.pixel = x_color[vpColor::id_darkGray] ;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT RED.
- x_color[vpColor::id_lightRed] = 251;
- xcolor.pixel = x_color[vpColor::id_lightRed] ;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
-
- // Color RED.
- x_color[vpColor::id_red] = 250;
- xcolor.pixel = x_color[vpColor::id_red] ;
- xcolor.red = 256 * 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK RED.
- x_color[vpColor::id_darkRed] = 249;
- xcolor.pixel = x_color[vpColor::id_darkRed] ;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT GREEN.
- x_color[vpColor::id_lightGreen] = 248;
- xcolor.pixel = x_color[vpColor::id_lightGreen] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GREEN.
- x_color[vpColor::id_green] = 247;
- xcolor.pixel = x_color[vpColor::id_green];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GREEN.
- x_color[vpColor::id_darkGreen] = 246;
- xcolor.pixel = x_color[vpColor::id_darkGreen] ;
- xcolor.red = 0;
- xcolor.green = 256 * 128;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT BLUE.
- x_color[vpColor::id_lightBlue] = 245;
- xcolor.pixel = x_color[vpColor::id_lightBlue] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color BLUE.
- x_color[vpColor::id_blue] = 244;
- xcolor.pixel = x_color[vpColor::id_blue];
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK BLUE.
- x_color[vpColor::id_darkBlue] = 243;
- xcolor.pixel = x_color[vpColor::id_darkBlue] ;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- // Color YELLOW.
- x_color[vpColor::id_yellow] = 242;
- xcolor.pixel = x_color[vpColor::id_yellow];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color ORANGE.
- x_color[vpColor::id_orange] = 241;
- xcolor.pixel = x_color[vpColor::id_orange];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color CYAN.
- x_color[vpColor::id_cyan] = 240;
- xcolor.pixel = x_color[vpColor::id_cyan];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color PURPLE.
- x_color[vpColor::id_purple] = 239;
- xcolor.pixel = x_color[vpColor::id_purple];
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- break;
-
- case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- // Couleur BLACK.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_black] = xcolor.pixel;
-
- // Couleur WHITE.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 256* 255;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_white] = xcolor.pixel;
-
- // Couleur LIGHT GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGray] = xcolor.pixel;
-
- // Couleur GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_gray] = xcolor.pixel;
-
- // Couleur DARK GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGray] = xcolor.pixel;
-
- // Couleur LIGHT RED.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightRed] = xcolor.pixel;
-
- // Couleur RED.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_red] = xcolor.pixel;
-
- // Couleur DARK RED.
- xcolor.pad = 0;
- xcolor.red = 256* 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkRed] = xcolor.pixel;
-
- // Couleur LIGHT GREEN.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGreen] = xcolor.pixel;
-
- // Couleur GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256*255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_green] = xcolor.pixel;
-
- // Couleur DARK GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256* 128;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGreen] = xcolor.pixel;
-
- // Couleur LIGHT Blue.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightBlue] = xcolor.pixel;
-
- // Couleur BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_blue] = xcolor.pixel;
-
- // Couleur DARK BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256* 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkBlue] = xcolor.pixel;
-
- // Couleur YELLOW.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_yellow] = xcolor.pixel;
-
- // Couleur ORANGE.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_orange] = xcolor.pixel;
-
- // Couleur CYAN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_cyan] = xcolor.pixel;
-
- // Couleur PURPLE.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_purple] = xcolor.pixel;
- break;
- }
- }
-
- XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
- // Selection des evenements.
- XSelectInput ( display, window,
- ExposureMask |
- ButtonPressMask | ButtonReleaseMask |
- KeyPressMask | KeyReleaseMask |
- StructureNotifyMask |
- PointerMotionMask);
-
- // graphic context creation
- values.plane_mask = AllPlanes;
- values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
-
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
-
- }
-
- // Pixmap creation.
- pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
-
- // Hangs when forward X11 is used to send the display to an other computer
-// do
-// XNextEvent ( display, &event );
-// while ( event.xany.type != Expose );
-
- {
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- I.getWidth() , I.getHeight(), XBitmapPad ( display ), 0 );
-
- Ximage->data = ( char * ) malloc ( I.getHeight() * (unsigned int)Ximage->bytes_per_line );
- ximage_data_init = true;
-
- }
- displayHasBeenInitialized = true ;
-
- XStoreName ( display, window, title_.c_str() );
-
- XSync ( display, 1 );
-
- I.display = this ;
-}
-
-/*!
- Initialize the display (size, position and title) of a color
- image in RGBa format.
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void
-vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const char *title )
-{
-
- XSizeHints hints;
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
-
- if (x_color == NULL) {
- // id_unknown = number of predefined colors
- x_color= new unsigned long [vpColor::id_unknown];
- }
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-
- // Positionnement de la fenetre dans l'ecran.
- if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
- {
- hints.flags = 0;
- }
- else
- {
- hints.flags = USPosition;
- hints.x = windowXPosition;
- hints.y = windowYPosition;
- }
-
- // setup X11 --------------------------------------------------
- width = I.getWidth();
- height = I.getHeight();
-
- if ( ( display = XOpenDisplay ( NULL ) ) == NULL )
- {
- vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server." ) ) ;
- }
-
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
-
- vpDEBUG_TRACE ( 1, "Screen depth: %d\n", screen_depth );
-
- if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
- windowXPosition, windowYPosition,
- width, height, 1,
- BlackPixel ( display, screen ),
- WhitePixel ( display, screen ) ) ) == 0 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
- }
-
- //
- // Create color table for 8 and 16 bits screen
- //
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
- xcolor.red = 256 * i;
- xcolor.green = 256 * i;
- xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
- }
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
- }
-
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
- xcolor.pad = 0;
- xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
- }
- colortable[i] = xcolor.pixel;
- }
-
- Visual *visual = DefaultVisual (display, screen);
- RMask = visual->red_mask;
- GMask = visual->green_mask;
- BMask = visual->blue_mask;
-
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
- GShift = 15 - getMsb(GMask);
- BShift = 15 - getMsb(BMask);
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
- }
-
-
- //
- // Create colors for overlay
- //
- switch ( screen_depth )
- {
-
- case 8:
- // Color BLACK and WHITE are set properly.
-
- // Color LIGHT GRAY.
- x_color[vpColor::id_lightGray] = 254;
- xcolor.pixel = x_color[vpColor::id_lightGray] ;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GRAY.
- x_color[vpColor::id_gray] = 253;
- xcolor.pixel = x_color[vpColor::id_gray] ;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GRAY.
- x_color[vpColor::id_darkGray] = 252;
- xcolor.pixel = x_color[vpColor::id_darkGray] ;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT RED.
- x_color[vpColor::id_lightRed] = 251;
- xcolor.pixel = x_color[vpColor::id_lightRed] ;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
-
- // Color RED.
- x_color[vpColor::id_red] = 250;
- xcolor.pixel = x_color[vpColor::id_red] ;
- xcolor.red = 256 * 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK RED.
- x_color[vpColor::id_darkRed] = 249;
- xcolor.pixel = x_color[vpColor::id_darkRed] ;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT GREEN.
- x_color[vpColor::id_lightGreen] = 248;
- xcolor.pixel = x_color[vpColor::id_lightGreen] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GREEN.
- x_color[vpColor::id_green] = 247;
- xcolor.pixel = x_color[vpColor::id_green];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GREEN.
- x_color[vpColor::id_darkGreen] = 246;
- xcolor.pixel = x_color[vpColor::id_darkGreen] ;
- xcolor.red = 0;
- xcolor.green = 256 * 128;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT BLUE.
- x_color[vpColor::id_lightBlue] = 245;
- xcolor.pixel = x_color[vpColor::id_lightBlue] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color BLUE.
- x_color[vpColor::id_blue] = 244;
- xcolor.pixel = x_color[vpColor::id_blue];
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK BLUE.
- x_color[vpColor::id_darkBlue] = 243;
- xcolor.pixel = x_color[vpColor::id_darkBlue] ;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- // Color YELLOW.
- x_color[vpColor::id_yellow] = 242;
- xcolor.pixel = x_color[vpColor::id_yellow];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color ORANGE.
- x_color[vpColor::id_orange] = 241;
- xcolor.pixel = x_color[vpColor::id_orange];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
-
- // Color CYAN.
- x_color[vpColor::id_cyan] = 240;
- xcolor.pixel = x_color[vpColor::id_cyan];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
-
- // Color PURPLE.
- x_color[vpColor::id_purple] = 239;
- xcolor.pixel = x_color[vpColor::id_purple];
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
-
- break;
-
- case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- // Couleur BLACK.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_black] = xcolor.pixel;
-
- // Couleur WHITE.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 256* 255;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_white] = xcolor.pixel;
-
- // Couleur LIGHT GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGray] = xcolor.pixel;
-
- // Couleur GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_gray] = xcolor.pixel;
-
- // Couleur DARK GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGray] = xcolor.pixel;
-
- // Couleur LIGHT RED.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightRed] = xcolor.pixel;
-
- // Couleur RED.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_red] = xcolor.pixel;
-
- // Couleur DARK RED.
- xcolor.pad = 0;
- xcolor.red = 256* 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkRed] = xcolor.pixel;
-
- // Couleur LIGHT GREEN.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGreen] = xcolor.pixel;
-
- // Couleur GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256*255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_green] = xcolor.pixel;
-
- // Couleur DARK GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256* 128;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGreen] = xcolor.pixel;
-
- // Couleur LIGHT Blue.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightBlue] = xcolor.pixel;
-
- // Couleur BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_blue] = xcolor.pixel;
-
- // Couleur DARK BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256* 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkBlue] = xcolor.pixel;
-
- // Couleur YELLOW.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_yellow] = xcolor.pixel;
-
- // Couleur ORANGE.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_orange] = xcolor.pixel;
-
- // Couleur CYAN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_cyan] = xcolor.pixel;
-
- // Couleur PURPLE.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_purple] = xcolor.pixel;
- break;
- }
- }
-
- XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
- // Selection des evenements.
- XSelectInput ( display, window,
- ExposureMask |
- ButtonPressMask | ButtonReleaseMask |
- KeyPressMask | KeyReleaseMask |
- StructureNotifyMask |
- PointerMotionMask);
-
- // Creation du contexte graphique
- values.plane_mask = AllPlanes;
- values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
-
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
- }
-
- // Pixmap creation.
- pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
-
- // Hangs when forward X11 is used to send the display to an other computer
-// do
-// XNextEvent ( display, &event );
-// while ( event.xany.type != Expose );
-
- {
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- I.getWidth() , I.getHeight(), XBitmapPad ( display ), 0 );
-
-
- Ximage->data = ( char * ) malloc ( I.getHeight() * (unsigned int)Ximage->bytes_per_line );
- ximage_data_init = true;
-
- }
- displayHasBeenInitialized = true ;
-
- XSync ( display, true );
-
- XStoreName ( display, window, title_.c_str() );
-
- I.display = this ;
-}
-
-
-/*!
- Initialize the display size, position and title.
-
- \param w, h : Width and height of the window.
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const char *title )
-{
- /* setup X11 ------------------------------------------------------------- */
- this->width = w;
- this->height = h;
-
- XSizeHints hints;
-
- if (x != -1)
- windowXPosition = x ;
- if (y != -1)
- windowYPosition = y ;
- // Positionnement de la fenetre dans l'ecran.
- if ( ( windowXPosition < 0 ) || ( windowYPosition < 0 ) )
- {
- hints.flags = 0;
- }
- else
- {
- hints.flags = USPosition;
- hints.x = windowXPosition;
- hints.y = windowYPosition;
- }
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-
- if ( ( display = XOpenDisplay ( NULL ) ) == NULL )
- {
- vpERROR_TRACE ( "Can't connect display on server %s.\n", XDisplayName ( NULL ) );
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server." ) ) ;
- }
-
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
-
- vpTRACE ( "Screen depth: %d\n", screen_depth );
-
- if ( ( window = XCreateSimpleWindow ( display, RootWindow ( display, screen ),
- windowXPosition, windowYPosition,
- width, height, 1,
- BlackPixel ( display, screen ),
- WhitePixel ( display, screen ) ) ) == 0 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
- }
-
-
- //
- // Create color table for 8 and 16 bits screen
- //
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
- xcolor.red = 256 * i;
- xcolor.green = 256 * i;
- xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
- }
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
- }
-
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
- xcolor.pad = 0;
- xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
- }
- colortable[i] = xcolor.pixel;
- }
-
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
-
- Visual *visual = DefaultVisual (display, screen);
- RMask = visual->red_mask;
- GMask = visual->green_mask;
- BMask = visual->blue_mask;
-
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
- GShift = 15 - getMsb(GMask);
- BShift = 15 - getMsb(BMask);
- }
-
- vpColor pcolor; // predefined colors
-
- //
- // Create colors for overlay
- //
- switch ( screen_depth )
- {
-
- case 8:
- // Color BLACK: default set to 0
-
- // Color WHITE: default set to 255
-
- // Color LIGHT GRAY.
- pcolor = vpColor::lightGray;
- xcolor.pixel = 254 ; // affected to 254
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GRAY.
- pcolor = vpColor::gray;
- xcolor.pixel = 253 ; // affected to 253
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GRAY.
- pcolor = vpColor::darkGray;
- xcolor.pixel = 252 ; // affected to 252
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT RED.
- pcolor = vpColor::lightRed;
- xcolor.pixel = 251 ; // affected to 251
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color RED.
- pcolor = vpColor::red;
- xcolor.pixel = 250 ; // affected to 250
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK RED.
- pcolor = vpColor::darkRed;
- xcolor.pixel = 249 ; // affected to 249
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT GREEN.
- pcolor = vpColor::lightGreen;
- xcolor.pixel = 248 ; // affected to 248
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color GREEN.
- pcolor = vpColor::green;
- xcolor.pixel = 247; // affected to 247
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK GREEN.
- pcolor = vpColor::darkGreen;
- xcolor.pixel = 246 ; // affected to 246
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color LIGHT BLUE.
- pcolor = vpColor::lightBlue;
- xcolor.pixel = 245 ; // affected to 245
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color BLUE.
- pcolor = vpColor::blue;
- xcolor.pixel = 244; // affected to 244
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color DARK BLUE.
- pcolor = vpColor::darkBlue;
- xcolor.pixel = 243 ; // affected to 243
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color YELLOW.
- pcolor = vpColor::yellow;
- xcolor.pixel = 242; // affected to 242
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color ORANGE.
- pcolor = vpColor::orange;
- xcolor.pixel = 241; // affected to 241
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color CYAN.
- pcolor = vpColor::cyan;
- xcolor.pixel = 240; // affected to 240
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- // Color PURPLE.
- pcolor = vpColor::purple;
- xcolor.pixel = 239; // affected to 239
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
-
- break;
-
- case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
-
- // Couleur BLACK.
- pcolor = vpColor::black;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_black] = xcolor.pixel;
-
- // Color WHITE.
- pcolor = vpColor::white;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_white] = xcolor.pixel;
-
- // Color LIGHT GRAY.
- pcolor = vpColor::lightGray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGray] = xcolor.pixel;
-
- // Color GRAY.
- pcolor = vpColor::gray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_gray] = xcolor.pixel;
-
- // Color DARK GRAY.
- pcolor = vpColor::darkGray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGray] = xcolor.pixel;
-
- // Color LIGHT RED.
- pcolor = vpColor::lightRed;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightRed] = xcolor.pixel;
-
- // Color RED.
- pcolor = vpColor::red;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_red] = xcolor.pixel;
-
- // Color DARK RED.
- pcolor = vpColor::darkRed;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkRed] = xcolor.pixel;
-
- // Color LIGHT GREEN.
- pcolor = vpColor::lightGreen;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightGreen] = xcolor.pixel;
-
- // Color GREEN.
- pcolor = vpColor::green;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_green] = xcolor.pixel;
-
- // Color DARK GREEN.
- pcolor = vpColor::darkGreen;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkGreen] = xcolor.pixel;
-
- // Color LIGHT BLUE.
- pcolor = vpColor::lightBlue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_lightBlue] = xcolor.pixel;
-
- // Color BLUE.
- pcolor = vpColor::blue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_blue] = xcolor.pixel;
-
- // Color DARK BLUE.
- pcolor = vpColor::darkBlue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_darkBlue] = xcolor.pixel;
-
- // Color YELLOW.
- pcolor = vpColor::yellow;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_yellow] = xcolor.pixel;
-
- // Color ORANGE.
- pcolor = vpColor::orange;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_orange] = xcolor.pixel;
-
- // Color CYAN.
- pcolor = vpColor::cyan;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_cyan] = xcolor.pixel;
-
- // Color PURPLE.
- pcolor = vpColor::purple;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
- x_color[vpColor::id_purple] = xcolor.pixel;
- break;
- }
- }
-
- XSetStandardProperties ( display, window, this->title_.c_str(), this->title_.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
- // Selection des evenements.
- XSelectInput ( display, window,
- ExposureMask |
- ButtonPressMask | ButtonReleaseMask |
- KeyPressMask | KeyReleaseMask |
- StructureNotifyMask |
- PointerMotionMask);
-
- /* Creation du contexte graphique */
- values.plane_mask = AllPlanes;
- values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
-
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
- }
-
- // Pixmap creation.
- pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
-
- // Hangs when forward X11 is used to send the display to an other computer
-// do
-// XNextEvent ( display, &event );
-// while ( event.xany.type != Expose );
-
- {
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- width, height, XBitmapPad ( display ), 0 );
-
- Ximage->data = ( char * ) malloc ( height * (unsigned int)Ximage->bytes_per_line );
- ximage_data_init = true;
- }
- displayHasBeenInitialized = true ;
-
- XSync ( display, true );
-
- XStoreName ( display, window, title_.c_str() );
-}
-
-/*!
-
- Set the font used to display a text in overlay. The display is
- performed using displayCharString().
-
- \param font : The expected font name. The available fonts are given by
- the "xlsfonts" binary. To choose a font you can also use the
- "xfontsel" binary.
-
- \note Under UNIX, to know all the available fonts, use the
- "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary.
-
- \sa displayCharString()
-*/
-void vpDisplayX::setFont( const char* font )
-{
- if ( displayHasBeenInitialized )
- {
- if (font!=NULL)
- {
- try
- {
- Font stringfont;
- stringfont = XLoadFont (display, font) ; //"-adobe-times-bold-r-normal--18*");
- XSetFont (display, context, stringfont);
- }
- catch(...)
- {
- vpERROR_TRACE ( "Bad font " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,"Bad font" ) ) ;
- }
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Set the window title.
- \param title : Window title.
-*/
-void
-vpDisplayX::setTitle ( const char *title )
-{
- if ( displayHasBeenInitialized )
- {
- if(title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
- XStoreName ( display, window, title_.c_str() );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Set the window position in the screen.
-
- \param winx, winy : Position of the upper-left window's border in the screen.
-
- \exception vpDisplayException::notInitializedError : If the video
- device is not initialized.
-*/
-void vpDisplayX::setWindowPosition(int winx, int winy)
-{
- if ( displayHasBeenInitialized ) {
- XMoveWindow(display, window, winx, winy);
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImage ( const vpImage<unsigned char> &I )
-{
-
- if ( displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 8:
- {
- unsigned char *src_8 = NULL;
- unsigned char *dst_8 = NULL;
- src_8 = ( unsigned char * ) I.bitmap;
- dst_8 = ( unsigned char * ) Ximage->data;
- // Correction de l'image de facon a liberer les niveaux de gris
- // ROUGE, VERT, BLEU, JAUNE
- {
- unsigned int i = 0;
- unsigned int size_ = width * height;
- unsigned char nivGris;
- unsigned char nivGrisMax = 255 - vpColor::id_unknown;
-
- while ( i < size_ )
- {
- nivGris = src_8[i] ;
- if ( nivGris > nivGrisMax )
- dst_8[i] = 255;
- else
- dst_8[i] = nivGris;
- i++ ;
- }
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-// XClearWindow ( display, window );
-// XSync ( display,1 );
- break;
- }
- case 16:
- {
- unsigned short *dst_16 = ( unsigned short* ) Ximage->data;
- unsigned char *dst_8 = NULL;
- unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
- for ( unsigned int i = 0; i < height ; i++ ) {
- dst_8 = (unsigned char*) Ximage->data + i * bytes_per_line;
- dst_16 = (unsigned short *) dst_8;
- for ( unsigned int j=0 ; j < width; j++ )
- {
- * ( dst_16 + j ) = ( unsigned short ) colortable[I[i][j]] ;
- }
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-// XClearWindow ( display, window );
-// XSync ( display,1 );
- break;
- }
-
- case 24:
- default:
- {
- unsigned char *dst_32 = NULL;
- unsigned int size_ = width * height ;
- dst_32 = ( unsigned char* ) Ximage->data;
- unsigned char *bitmap = I.bitmap ;
- unsigned char *n = I.bitmap + size_;
- //for (unsigned int i = 0; i < size; i++) // suppression de l'iterateur i
- while ( bitmap < n )
- {
- unsigned char val = * ( bitmap++ );
- * ( dst_32 ++ ) = val; // Composante Rouge.
- * ( dst_32 ++ ) = val; // Composante Verte.
- * ( dst_32 ++ ) = val; // Composante Bleue.
- * ( dst_32 ++ ) = val;
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-// XClearWindow ( display, window );
-// XSync ( display,1 );
- break;
- }
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-/*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
-{
- if ( displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 16: {
- unsigned short *dst_16 = NULL;
- unsigned char *dst_8 = NULL;
- vpRGBa* bitmap = I.bitmap;
- unsigned int r, g, b;
- unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
-
- for ( unsigned int i = 0; i < height ; i++ ) {
- dst_8 = (unsigned char*) Ximage->data + i * bytes_per_line;
- dst_16 = (unsigned short *) dst_8;
- for ( unsigned int j=0 ; j < width; j++ )
- {
- r = bitmap->R;
- g = bitmap->G;
- b = bitmap->B;
- * ( dst_16 + j ) = (((r << 8) >> RShift) & RMask) |
- (((g << 8) >> GShift) & GMask) |
- (((b << 8) >> BShift) & BMask);
- bitmap++;
- }
- }
-
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-
- break;
- }
- case 24:
- case 32:
- {
- /*
- * 32-bit source, 24/32-bit destination
- */
- unsigned char *dst_32 = NULL;
- dst_32 = ( unsigned char* ) Ximage->data;
- vpRGBa* bitmap = I.bitmap;
- unsigned int sizeI = I.getWidth() * I.getHeight();
- if (XImageByteOrder(display) == 1) {
- // big endian
- for ( unsigned int i = 0; i < sizeI ; i++ ) {
- *(dst_32++) = bitmap->A;
- *(dst_32++) = bitmap->R;
- *(dst_32++) = bitmap->G;
- *(dst_32++) = bitmap->B;
- bitmap++;
- }
- }
- else {
- // little endian
- for ( unsigned int i = 0; i < sizeI; i++ ) {
- *(dst_32++) = bitmap->B;
- *(dst_32++) = bitmap->G;
- *(dst_32++) = bitmap->R;
- *(dst_32++) = bitmap->A;
- bitmap++;
- }
- }
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
- // XClearWindow ( display, window );
- // XSync ( display,1 );
- break;
-
- }
- default:
- vpERROR_TRACE ( "Unsupported depth (%d bpp) for color display",
- screen_depth ) ;
- throw ( vpDisplayException ( vpDisplayException::depthNotSupportedError,
- "Unsupported depth for color display" ) ) ;
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display an image with a reference to the bitmap.
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing.
-
- \param I : Pointer to the image bitmap.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImage ( const unsigned char *I )
-{
- unsigned char *dst_32 = NULL;
-
- if ( displayHasBeenInitialized )
- {
-
- dst_32 = ( unsigned char* ) Ximage->data;
-
- for ( unsigned int i = 0; i < width * height; i++ )
- {
- * ( dst_32 ++ ) = *I; // red component.
- * ( dst_32 ++ ) = *I; // green component.
- * ( dst_32 ++ ) = *I; // blue component.
- * ( dst_32 ++ ) = *I; // luminance component.
- I++;
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-// XClearWindow ( display, window );
-// XSync ( display,1 );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-
-/*!
- Display a selection of the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w, h : Width and height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
-{
- if ( displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 8:
- {
- unsigned char *src_8 = NULL;
- unsigned char *dst_8 = NULL;
- src_8 = ( unsigned char * ) I.bitmap;
- dst_8 = ( unsigned char * ) Ximage->data;
- // Correction de l'image de facon a liberer les niveaux de gris
- // ROUGE, VERT, BLEU, JAUNE
- {
- //int size = width * height;
- unsigned char nivGris;
- unsigned char nivGrisMax = 255 - vpColor::id_unknown;
-
- //unsigned int iwidth = I.getWidth();
- unsigned int iwidth = I.getWidth();
-
- src_8 = src_8 + (int)(iP.get_i()*iwidth+ iP.get_j());
- dst_8 = dst_8 + (int)(iP.get_i()*this->width+ iP.get_j());
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- nivGris = *(src_8+j);
- if ( nivGris > nivGrisMax )
- *(dst_8+j) = 255;
- else
- *(dst_8+j) = nivGris;
- j++;
- }
- src_8 = src_8 + iwidth;
- dst_8 = dst_8 + this->width;
- i++;
- }
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
- // XClearWindow ( display, window );
- // XSync ( display,1 );
- break;
- }
- case 16:
- {
- unsigned short *dst_16 = NULL;
- unsigned char *dst_8 = NULL;
- unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
- for ( unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i()+h) ; i++ ) {
- dst_8 = (unsigned char *) Ximage->data + i * bytes_per_line;
- dst_16 = (unsigned short *) dst_8;
- for ( unsigned int j=(unsigned int)iP.get_j() ; j < (unsigned int)(iP.get_j()+w); j++ )
- {
- * ( dst_16 + j ) = ( unsigned short ) colortable[I[i][j]] ;
- }
- }
-
-// unsigned char *src_8 = (unsigned char *) I.bitmap;
-// unsigned char *dst_8 = (unsigned char *) Ximage->data;
-// unsigned short *dst_16 = NULL;
-
-// unsigned int iwidth = I.getWidth();
-
-// src_8 += (int)(iP.get_i()*iwidth + iP.get_j());
-// dst_8 += (int)(iP.get_i()*Ximage->bytes_per_line + iP.get_j()*Ximage->bits_per_pixel/8);
-// dst_16 = (unsigned short *) dst_8;
-
-// unsigned int i = 0;
-// while (i < h) {
-// unsigned int j = 0;
-
-// while (j < w) {
-// *(dst_16 + j) = ( unsigned short ) colortable[*(src_8+j)];
-// j++;
-// }
-// src_8 = src_8 + iwidth;
-// dst_16 = dst_16 + Ximage->bytes_per_line;
-// i++;
-// }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
- // XClearWindow ( display, window );
- // XSync ( display,1 );
- break;
- }
-
- case 24:
- default:
- {
- unsigned char *dst_32 = NULL;
- //unsigned int size = width * height ;
- dst_32 = ( unsigned char* ) Ximage->data;
- unsigned char *src_8 = I.bitmap ;
- //unsigned char *n = I.bitmap + size;
-
- unsigned int iwidth = I.getWidth();
-
- src_8 = src_8 + (int)(iP.get_i()*iwidth+ iP.get_j());
- dst_32 = dst_32 + (int)(iP.get_i()*4*this->width+ iP.get_j()*4);
-
- unsigned int i = 0;
- while (i < h)
- {
- unsigned int j = 0;
- while (j < w)
- {
- unsigned char val = *(src_8+j);
- *(dst_32+4*j) = val;
- *(dst_32+4*j+1) = val;
- *(dst_32+4*j+2) = val;
- *(dst_32+4*j+3) = val;
- j++;
- }
- src_8 = src_8 + iwidth;
- dst_32 = dst_32 + 4*this->width;
- i++;
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
- // XClearWindow ( display, window );
- // XSync ( display,1 );
- break;
- }
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-
-/*!
- Display a selection of the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param w, h : Width and height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
-{
- if ( displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 16: {
- unsigned short *dst_16 = NULL;
- unsigned char *dst_8 = NULL;
- unsigned int r, g, b;
- unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line;
- for ( unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i()+h) ; i++ ) {
- dst_8 = (unsigned char *) Ximage->data + i * bytes_per_line;
- dst_16 = (unsigned short *) dst_8;
- for ( unsigned int j=(unsigned int)iP.get_j() ; j < (unsigned int)(iP.get_j()+w); j++ )
- {
- r = I[i][j].R;
- g = I[i][j].G;
- b = I[i][j].B;
- * ( dst_16 + j ) = (((r << 8) >> RShift) & RMask) |
- (((g << 8) >> GShift) & GMask) |
- (((b << 8) >> BShift) & BMask);
- }
- }
-
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, width, height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
-
- break;
- }
- case 24:
- case 32:
- {
- /*
- * 32-bit source, 24/32-bit destination
- */
-
- unsigned char *dst_32 = NULL;
- dst_32 = ( unsigned char* ) Ximage->data;
- vpRGBa* src_32 = I.bitmap;
- //unsigned int sizeI = I.getWidth() * I.getHeight();
-
- unsigned int iwidth = I.getWidth();
-
- src_32 = src_32 + (int)(iP.get_i()*iwidth+ iP.get_j());
- dst_32 = dst_32 + (int)(iP.get_i()*4*this->width+ iP.get_j()*4);
-
- unsigned int i = 0;
-
- if (XImageByteOrder(display) == 1) {
- // big endian
- while (i < h) {
- unsigned int j = 0;
- while (j < w) {
- *(dst_32+4*j) = (src_32+j)->A;
- *(dst_32+4*j+1) = (src_32+j)->R;
- *(dst_32+4*j+2) = (src_32+j)->G;
- *(dst_32+4*j+3) = (src_32+j)->B;
-
- j++;
- }
- src_32 = src_32 + iwidth;
- dst_32 = dst_32 + 4*this->width;
- i++;
- }
-
- }
- else {
- // little endian
- while (i < h) {
- unsigned int j = 0;
- while (j < w) {
- *(dst_32+4*j) = (src_32+j)->B;
- *(dst_32+4*j+1) = (src_32+j)->G;
- *(dst_32+4*j+2) = (src_32+j)->R;
- *(dst_32+4*j+3) = (src_32+j)->A;
-
- j++;
- }
- src_32 = src_32 + iwidth;
- dst_32 = dst_32 + 4*this->width;
- i++;
- }
- }
-
- // Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
- // XClearWindow ( display, window );
- // XSync ( display,1 );
- break;
-
- }
- default:
- vpERROR_TRACE ( "Unsupported depth (%d bpp) for color display",
- screen_depth ) ;
- throw ( vpDisplayException ( vpDisplayException::depthNotSupportedError,
- "Unsupported depth for color display" ) ) ;
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
-
- Close the window.
-
- \sa init()
-
-*/
-void vpDisplayX::closeDisplay()
-{
- if ( displayHasBeenInitialized )
- {
- if ( ximage_data_init == true )
- free ( Ximage->data );
-
- Ximage->data = NULL;
- XDestroyImage ( Ximage );
-
- XFreePixmap ( display, pixmap );
-
- XFreeGC ( display, context );
- XDestroyWindow ( display, window );
- XCloseDisplay ( display );
-
- displayHasBeenInitialized = false;
-
- if (x_color != NULL) {
- delete [] x_color;
- x_color = NULL;
- }
- }
-}
-
-
-/*!
- Flushes the X buffer.
- It's necessary to use this function to see the results of any drawing.
-
-*/
-void vpDisplayX::flushDisplay()
-{
- if ( displayHasBeenInitialized )
- {
- XClearWindow ( display, window );
- //XClearArea ( display, window,0,0,100,100,0 );
- XFlush ( display );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Flushes a part of the X buffer.
- It's necessary to use this function to see the results of any drawing.
-
- \param iP : Top left corner of the region of interest
- \param w,h : Width and height of the region of interest
-*/
-void vpDisplayX::flushDisplayROI(const vpImagePoint &iP, const unsigned int w, const unsigned int h)
-{
- if ( displayHasBeenInitialized )
- {
- //XClearWindow ( display, window );
- XClearArea ( display, window,(int)iP.get_u(),(int)iP.get_v(),w,h,0 );
- XFlush ( display );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-
-/*!
- Set the window backgroud to \e color.
- \param color : Background color.
-*/
-void vpDisplayX::clearDisplay ( const vpColor &color )
-{
- if ( displayHasBeenInitialized )
- {
-
- if (color.id < vpColor::id_unknown)
- XSetWindowBackground ( display, window, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XClearWindow ( display, window );
-
- XFreePixmap ( display, pixmap );
- // Pixmap creation.
- pixmap = XCreatePixmap ( display, window, width, height, screen_depth );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpDisplayX::displayArrow ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w, unsigned int h,
- unsigned int thickness)
-{
- if ( displayHasBeenInitialized )
- {
- try
- {
- double a = ip2.get_i() - ip1.get_i() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt ( vpMath::sqr ( a ) + vpMath::sqr ( b ) ) ;
-
- //if ( ( a==0 ) && ( b==0 ) )
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
- {
- // DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
-
- vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
-
- vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
-
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a string at the image point \e ip location.
-
- To select the font used to display the string, use setFont().
-
- \param ip : Upper left image point location of the string in the display.
- \param text : String to display in overlay.
- \param color : String color.
-
- \sa setFont()
-*/
-void vpDisplayX::displayCharString ( const vpImagePoint &ip,
- const char *text,
- const vpColor &color )
-{
- if ( displayHasBeenInitialized )
- {
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
- XDrawString ( display, pixmap, context,
- (int)ip.get_u(), (int)ip.get_v(),
- text, (int)strlen ( text ) );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a circle.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the circle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void vpDisplayX::displayCircle ( const vpImagePoint ¢er,
- unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness )
-{
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
-
- if ( fill == false )
- {
- XDrawArc ( display, pixmap, context,
- vpMath::round( center.get_u()-radius ),
- vpMath::round( center.get_v()-radius ),
- radius*2, radius*2, 0, 23040 ); /* 23040 = 360*64 */
- }
- else
- {
- XFillArc ( display, pixmap, context,
- vpMath::round( center.get_u()-radius ),
- vpMath::round( center.get_v()-radius ),
- radius*2, radius*2, 0, 23040 ); /* 23040 = 360*64 */
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a cross at the image point \e ip location.
- \param ip : Cross location.
- \param cross_size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplayX::displayCross ( const vpImagePoint &ip,
- unsigned int cross_size,
- const vpColor &color,
- unsigned int thickness)
-{
- if ( displayHasBeenInitialized )
- {
- try
- {
- double i = ip.get_i();
- double j = ip.get_j();
- vpImagePoint ip1, ip2;
-
- ip1.set_i( i-cross_size/2 );
- ip1.set_j( j );
- ip2.set_i( i+cross_size/2 );
- ip2.set_j( j );
- displayLine ( ip1, ip2, color, thickness ) ;
-
- ip1.set_i( i );
- ip1.set_j( j-cross_size/2 );
- ip2.set_i( i );
- ip2.set_j( j+cross_size/2 );
-
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
- }
-
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-
-}
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayX::displayDotLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
-
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XSetLineAttributes ( display, context, thickness,
- LineOnOffDash, CapButt, JoinBevel );
-
- XDrawLine ( display, pixmap, context,
- vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ),
- vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayX::displayLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
-
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
-
- XDrawLine ( display, pixmap, context,
- vpMath::round( ip1.get_u() ),
- vpMath::round( ip1.get_v() ),
- vpMath::round( ip2.get_u() ),
- vpMath::round( ip2.get_v() ) );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a point at the image point \e ip location.
- \param ip : Point location.
- \param color : Point color.
-*/
-void vpDisplayX::displayPoint ( const vpImagePoint &ip,
- const vpColor &color )
-{
- if ( displayHasBeenInitialized )
- {
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XDrawPoint ( display, pixmap, context,
- vpMath::round( ip.get_u() ),
- vpMath::round( ip.get_v() ) );
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param w,h : Rectangle size in terms of width and height.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
- unsigned int w, unsigned int h,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
- if ( fill == false )
- {
- XDrawRectangle ( display, pixmap, context,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- w-1, h-1 );
- }
- else
- {
- XFillRectangle ( display, pixmap, context,
- vpMath::round( topLeft.get_u() ),
- vpMath::round( topLeft.get_v() ),
- w, h );
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a rectangle.
-
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-*/
-void
-vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
-
- unsigned int w = (unsigned int)vpMath::round( std::fabs(bottomRight.get_u() - topLeft.get_u()) );
- unsigned int h = (unsigned int)vpMath::round( std::fabs(bottomRight.get_v() - topLeft.get_v()) );
- if ( fill == false )
- {
-
- XDrawRectangle ( display, pixmap, context,
- vpMath::round( topLeft.get_u() < bottomRight.get_u() ? topLeft.get_u() : bottomRight.get_u() ),
- vpMath::round( topLeft.get_v() < bottomRight.get_v() ? topLeft.get_v() : bottomRight.get_v() ),
- w > 0 ? w-1 : 1, h > 0 ? h : 1 );
- }
- else
- {
- XFillRectangle ( display, pixmap, context,
- vpMath::round( topLeft.get_u() < bottomRight.get_u() ? topLeft.get_u() : bottomRight.get_u() ),
- vpMath::round( topLeft.get_v() < bottomRight.get_v() ? topLeft.get_v() : bottomRight.get_v() ),
- w, h );
- }
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Display a rectangle.
-
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
-
- \param thickness : Thickness of the four lines used to display the
- rectangle. This parameter is only useful when \e fill is set to
- false.
-
-*/
-void
-vpDisplayX::displayRectangle ( const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- if ( displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
- if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
- else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
- xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
-
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
-
- if ( fill == false )
- {
- XDrawRectangle ( display, pixmap, context,
- vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getTop() ),
- (unsigned int)vpMath::round( rectangle.getWidth()-1 ),
- (unsigned int)vpMath::round( rectangle.getHeight()-1 ) );
- }
- else
- {
- XFillRectangle ( display, pixmap, context,
- vpMath::round( rectangle.getLeft() ),
- vpMath::round( rectangle.getTop() ),
- (unsigned int)vpMath::round( rectangle.getWidth() ),
- (unsigned int)vpMath::round( rectangle.getHeight() ) );
- }
-
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
-
- Wait for a click from one of the mouse button.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayX::getClick(bool blocking)
-{
-
- bool ret = false;
-
- if ( displayHasBeenInitialized ) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
-
- // Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
- }
-
- if(ret){
- /* Recuperation de la coordonnee du pixel clique. */
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {}
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret;
-}
-
-/*!
-
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool
-vpDisplayX::getClick ( vpImagePoint &ip, bool blocking )
-{
-
- bool ret = false;
- if ( displayHasBeenInitialized ) {
-
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
- // Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
- }
-
- if(ret){
- // Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x );
- ip.set_v( (double)event.xbutton.y );
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
-}
-
-/*!
-
- Wait for a mouse button click and get the position of the clicked
- pixel. The button used to click is also set.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-*/
-bool
-vpDisplayX::getClick ( vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking )
-{
-
- bool ret = false;
- if ( displayHasBeenInitialized ) {
-
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
-
- // Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
- }
-
- if(ret){
- // Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x );
- ip.set_v( (double)event.xbutton.y );
- switch ( event.xbutton.button ) {
- case Button1: button = vpMouseButton::button1; break;
- case Button2: button = vpMouseButton::button2; break;
- case Button3: button = vpMouseButton::button3; break;
- }
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
-}
-
-/*!
-
- Wait for a mouse button click release and get the position of the
- image point were the click release occurs. The button used to click is
- also set. Same method as getClick(unsigned int&, unsigned int&,
- vpMouseButton::vpMouseButtonType &, bool).
-
- \param ip [out] : Position of the clicked image point.
-
- \param button [in] : Button used to click.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
- \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
-
-*/
-bool
-vpDisplayX::getClickUp ( vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType &button,
- bool blocking )
-{
-
- bool ret = false;
- if ( displayHasBeenInitialized ) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
-
- // Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonReleaseMask ,&event );
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonReleaseMask, &event);
- }
-
- if(ret){
- /* Recuperation de la coordonnee du pixel clique. */
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x );
- ip.set_v( (double)event.xbutton.y );
- switch ( event.xbutton.button ) {
- case Button1: button = vpMouseButton::button1; break;
- case Button2: button = vpMouseButton::button2; break;
- case Button3: button = vpMouseButton::button3; break;
- }
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
-}
-
-/*
- Gets the displayed image (including the overlay plane)
- and returns an RGBa image.
-
- \param I : Image to get.
-*/
-void vpDisplayX::getImage ( vpImage<vpRGBa> &I )
-{
- if ( displayHasBeenInitialized )
- {
- XImage *xi ;
-
- XCopyArea (display,window, pixmap, context,
- 0,0, getWidth(), getHeight(), 0, 0);
-
- xi= XGetImage ( display,pixmap, 0,0, getWidth(), getHeight(),
- AllPlanes, ZPixmap ) ;
-
- try
- {
- I.resize ( getHeight(), getWidth() ) ;
- }
- catch ( ... )
- {
- vpERROR_TRACE ( "Error caught" ) ;
- throw ;
- }
-
- unsigned char *src_32 = NULL;
- src_32 = ( unsigned char* ) xi->data;
-
- if (screen_depth == 16) {
- for ( unsigned int i = 0; i < I.getHeight() ; i++ ) {
- size_t i_ = i*I.getWidth();
- for ( unsigned int j = 0; j < I.getWidth() ; j++ ) {
- size_t ij_ = i_+j;
- unsigned long pixel = XGetPixel(xi, (int)j, (int)i);
- I.bitmap[ij_].R = (((pixel & RMask) << RShift) >> 8);
- I.bitmap[ij_].G = (((pixel & GMask) << GShift) >> 8);
- I.bitmap[ij_].B = (((pixel & BMask) << BShift) >> 8);
- I.bitmap[ij_].A = 0;
- }
- }
-
- }
- else {
- if (XImageByteOrder(display) == 1) {
- // big endian
- for ( unsigned int i = 0; i < I.getWidth() * I.getHeight() ; i++ ) {
- I.bitmap[i].A = src_32[i*4] ;
- I.bitmap[i].R = src_32[i*4 + 1] ;
- I.bitmap[i].G = src_32[i*4 + 2] ;
- I.bitmap[i].B = src_32[i*4 + 3] ;
- }
- }
- else {
- // little endian
- for ( unsigned int i = 0; i < I.getWidth() * I.getHeight() ; i++ ) {
- I.bitmap[i].B = src_32[i*4] ;
- I.bitmap[i].G = src_32[i*4 + 1] ;
- I.bitmap[i].R = src_32[i*4 + 2] ;
- I.bitmap[i].A = src_32[i*4 + 3] ;
- }
- }
- }
- XDestroyImage ( xi ) ;
- }
- else
- {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
-}
-
-/*!
- Gets the window depth (8, 16, 24, 32).
-*/
-unsigned int vpDisplayX::getScreenDepth()
-{
- Display *display_;
- int screen_;
- unsigned int depth;
-
- if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
- {
- vpERROR_TRACE ( "Can't connect display on server %s.",
- XDisplayName ( NULL ) );
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server." ) ) ;
- }
- screen_ = DefaultScreen ( display_ );
- depth = (unsigned int)DefaultDepth ( display_, screen_ );
-
- XCloseDisplay ( display_ );
-
- return ( depth );
-}
-
-/*!
- Gets the window size.
- \param w, h : Size of the display in therms of width and height.
- */
-void vpDisplayX::getScreenSize ( unsigned int &w, unsigned int &h )
-{
- Display *display_;
- int screen_;
-
- if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
- {
- vpERROR_TRACE ( "Can't connect display on server %s.",
- XDisplayName ( NULL ) );
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server." ) ) ;
- }
- screen_ = DefaultScreen ( display_ );
- w = (unsigned int)DisplayWidth ( display_, screen_ );
- h = (unsigned int)DisplayHeight ( display_, screen_ );
-
- XCloseDisplay ( display_ );
-}
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-
-*/
-bool
-vpDisplayX::getKeyboardEvent(bool blocking)
-{
-
- bool ret = false;
-
- if ( displayHasBeenInitialized ) {
- // Event testing
- if(blocking){
- XMaskEvent ( display, KeyPressMask ,&event );
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , KeyPressMask, &event);
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret;
-}
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-
-*/
-bool
-vpDisplayX::getKeyboardEvent(char *string, bool blocking)
-{
-
- bool ret = false;
- KeySym keysym;
-// int count;
- XComposeStatus compose_status;
- char buffer;
-
- if ( displayHasBeenInitialized ) {
- // Event testing
- if(blocking){
- XMaskEvent ( display, KeyPressMask ,&event );
- /* count = */ XLookupString ((XKeyEvent *)&event, &buffer, 1,
- &keysym, &compose_status);
- //std::cout <<"count: " << count << " get \"" << buffer << "\"" << std::endl;
- sprintf(string, "%c", buffer);
- ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , KeyPressMask, &event);
- if (ret) {
- /* count = */ XLookupString ((XKeyEvent *)&event, &buffer, 1,
- &keysym, &compose_status);
- sprintf(string, "%c", buffer);
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret;
-}
-/*!
-
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-
-*/
-bool
-vpDisplayX::getPointerMotionEvent ( vpImagePoint &ip)
-{
-
- bool ret = false;
- if ( displayHasBeenInitialized ) {
-
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
- // Event testing
- ret = XCheckMaskEvent(display , PointerMotionMask, &event);
-
- if(ret){
- // Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x );
- ip.set_v( (double)event.xbutton.y );
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayX::getPointerPosition ( vpImagePoint &ip)
-{
-
- bool ret = false;
- if ( displayHasBeenInitialized ) {
-
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
- // Event testing
- ret = true;
-
- if(ret){
- // Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)win_x );
- ip.set_v( (double)win_y );
- }
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
-}
-
-/*!
- Get the position of the most significant bit.
-*/
-int vpDisplayX::getMsb(unsigned int u32val)
-{
- int i;
-
- for (i = 31; i >= 0; --i) {
- if (u32val & 0x80000000L)
- break;
- u32val <<= 1;
- }
- return i;
-}
-
-#endif
-
diff --git a/src/device/display/vpDisplayX.h b/src/device/display/vpDisplayX.h
deleted file mode 100644
index 7296f27..0000000
--- a/src/device/display/vpDisplayX.h
+++ /dev/null
@@ -1,279 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayX.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpDisplayX_h
-#define vpDisplayX_h
-
-#include <visp/vpConfig.h>
-#include <visp/vpDisplay.h>
-#ifdef VISP_HAVE_X11
-
-//namespace X11name
-//{
-#include <X11/Xlib.h>
-#include <X11/Xutil.h>
-//#include <X11/Xatom.h>
-//#include <X11/cursorfont.h>
-//} ;
-
-//using namespace X11name ;
-
-#include <visp/vpImage.h>
-#include <visp/vpRect.h>
-
-
-
-/*!
- \file vpDisplayX.h
- \brief Define the X11 console to display images.
-*/
-
-
-/*!
- \class vpDisplayX
-
- \ingroup ImageGUI
-
- \brief Define the X11 console to display images.
-
- \note Ready to use with a Unix System (tested for Linux and SunOS)
-
- This class define the X11 console to display images
- It also define method to display some geometric feature (point, line, circle)
- in the image.
-
- The example below shows how to display an image with this video device.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
- vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX d;
-
- // Initialize the display with the image I. Display and image are
- // now link together.
- d.init(I);
-#endif
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My X11 display");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(50);
- topLeftCorner.set_j(10);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
- char key[10];
- bool ret;
- for (int i=0; i< 200; i++) {
- bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
- }
-
- // Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
- ret = vpDisplay::getKeyboardEvent(I, key, true);
- std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
- std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
- // Wait for a click in the display window
- std::cout << "Wait for a button click..." << std::endl;
- vpDisplay::getClick(I);
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpDisplayX: public vpDisplay
-{
-private:
- Display *display ;
- Window window ;
- XImage *Ximage ;
- Colormap lut ;
- GC context ;
- int screen ;
- int planes;
- XEvent event;
- Pixmap pixmap;
- unsigned long *x_color; // Array of predefined colors
- unsigned int screen_depth ;
- unsigned short colortable[256];
- XColor xcolor;
- XGCValues values;
- int size ;
- bool ximage_data_init;
- unsigned int RMask, GMask, BMask;
- int RShift, GShift, BShift;
-
-public:
- vpDisplayX() ;
- vpDisplayX(int winx, int winy, const char *title=NULL) ;
- vpDisplayX(vpImage<unsigned char> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
- vpDisplayX(vpImage<vpRGBa> &I, int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- virtual ~vpDisplayX() ;
-
- void init(vpImage<unsigned char> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
- void init(vpImage<vpRGBa> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- void init(unsigned int width, unsigned int height,
- int winx=-1, int winy=-1 ,
- const char *title=NULL) ;
-
- unsigned int getScreenDepth();
- void getScreenSize(unsigned int &width, unsigned int &height);
- void getImage(vpImage<vpRGBa> &I) ;
-
-protected:
-
- void setFont( const char *font );
- void setTitle(const char *title) ;
- void setWindowPosition(int winx, int winy);
-
- void clearDisplay(const vpColor &color=vpColor::white) ;
-
- void closeDisplay() ;
-
- void displayArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4,unsigned int h=2,
- unsigned int thickness=1) ;
-
- void displayCharString(const vpImagePoint &ip, const char *text,
- const vpColor &color=vpColor::green) ;
-
- void displayCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
- void displayCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) ;
- void displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const unsigned char *I) ;
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
- void displayPoint(const vpImagePoint &ip, const vpColor &color) ;
-
- void displayRectangle(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
-
- void flushDisplay() ;
- void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true);
- bool getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true);
-
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(char *string, bool blocking=true);
-
- int getMsb(unsigned int u32val);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
-
- inline unsigned int getWidth() const { return width ; }
- inline unsigned int getHeight() const { return height ; }
-
-
-} ;
-
-
-#endif
-#endif
diff --git a/src/device/display/vpMouseButton.h b/src/device/display/vpMouseButton.h
deleted file mode 100755
index b15e69f..0000000
--- a/src/device/display/vpMouseButton.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMouseButton.h 5232 2015-01-30 11:53:30Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Color definition.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpMouseButton_h
-#define vpMouseButton_h
-
-#include <visp/vpConfig.h>
-
-/*!
- \class vpMouseButton
- \ingroup ImageGUI
- \brief Class that defines mouse button identifiers.
-*/
-class VISP_EXPORT vpMouseButton
-{
-public:
- typedef enum {
- button1 = 1, /*!< Mouse left button. */
- button2 = 2, /*!< Mouse middle button, or roll. */
- button3 = 3, /*!< Mouse right button. */
- none = 0 /*!< No button. */
- } vpMouseButtonType ;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/device/display/windows/vpD3DRenderer.cpp b/src/device/display/windows/vpD3DRenderer.cpp
deleted file mode 100755
index 997c99c..0000000
--- a/src/device/display/windows/vpD3DRenderer.cpp
+++ /dev/null
@@ -1,1217 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpD3DRenderer.cpp 4733 2014-05-19 20:44:05Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * D3D renderer for windows 32 display
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
-
-#include <visp/vpD3DRenderer.h>
-#include <visp/vpColor.h>
-#include <visp/vpMath.h>
-
-/*
- Be careful, when using :
-
- pd3dText->LockRect(0, &d3dLRect, &r, 0)
- ...
- pd3dText->UnlockRect(0, &d3dLRect, &r, 0)
-
- to write directly to the texture's surface,
- the pointer returned in d3dLRect points to
- the beginning of the locked suface and not
- to the beginning of the texture's surface.
- That's why setBufferPixel and other accesses
- to this buffer are done in the locked surface
- coordinates system.
-
- Moreover, when directly writing to a texture's surface,
- you musn't forget to take the pitch of this texture
- into account (see Direct3D documentation).
-
-*/
-
-/*!
- Constructor.
- Initializes the color palettes and the font.
-*/
-vpD3DRenderer::vpD3DRenderer()
-{
- pD3D=NULL;
- pd3dDevice=NULL;
- pd3dText=NULL;
- pd3dVideoText=NULL;
-
- //D3D palette
- vpColor pcolor; // Predefined colors
- pcolor = vpColor::black;
- colors[vpColor::id_black] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightBlue;
- colors[vpColor::id_lightBlue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::blue;
- colors[vpColor::id_blue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkBlue;
- colors[vpColor::id_darkBlue] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::cyan;
- colors[vpColor::id_cyan] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGreen;
- colors[vpColor::id_lightGreen] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::green;
- colors[vpColor::id_green] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGreen;
- colors[vpColor::id_darkGreen] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightRed;
- colors[vpColor::id_lightRed] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::red;
- colors[vpColor::id_red] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkRed;
- colors[vpColor::id_darkRed] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::white;
- colors[vpColor::id_white] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGray;
- colors[vpColor::id_lightGray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::gray;
- colors[vpColor::id_gray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGray;
- colors[vpColor::id_darkGray] = D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::yellow;
- colors[vpColor::id_yellow]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::orange;
- colors[vpColor::id_orange]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::purple;
- colors[vpColor::id_purple]= D3DCOLOR_ARGB(0xFF,pcolor.R, pcolor.G, pcolor.B);
-
- //initialize the GDI palette
- pcolor = vpColor::black;
- colorsGDI[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightBlue;
- colorsGDI[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::blue;
- colorsGDI[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkBlue;
- colorsGDI[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::cyan;
- colorsGDI[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGreen;
- colorsGDI[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::green;
- colorsGDI[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGreen;
- colorsGDI[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightRed;
- colorsGDI[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::red;
- colorsGDI[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkRed;
- colorsGDI[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::white;
- colorsGDI[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGray;
- colorsGDI[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::gray;
- colorsGDI[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGray;
- colorsGDI[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::yellow;
- colorsGDI[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::orange;
- colorsGDI[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::purple;
- colorsGDI[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
-
- //Creates a logical font
- hFont = CreateFont(18, 0, 0, 0, FW_NORMAL, false, false, false,
- DEFAULT_CHARSET, OUT_DEFAULT_PRECIS,
- CLIP_DEFAULT_PRECIS, DEFAULT_QUALITY,
- DEFAULT_PITCH | FF_DONTCARE, NULL);
-}
-
-/*!
- Destructor.
- Releases all the remaining interfaces.
-*/
-vpD3DRenderer::~vpD3DRenderer()
-{
- DeleteObject(hFont);
-
- if(pd3dDevice != NULL)
- pd3dDevice->Release();
- if(pD3D != NULL)
- pD3D->Release();
- if(pd3dText != NULL)
- pd3dText->Release();
- if(pd3dVideoText != NULL)
- pd3dVideoText->Release();
-}
-
-/*!
- Computes the nearest power of 2 superior to n.
- \param n Number whose nearest superior power of 2 we want.
- \return Nearest power of 2 superior to n.
-*/
-unsigned int vpD3DRenderer::supPowerOf2(unsigned int n)
-{
- unsigned int i=0;
- while(n>1)
- {
- n>>=1;
- i++;
- }
- return static_cast<unsigned int>(1<<(i+1));
-}
-
-/*!
- Initialize the Direct3D renderer.
- \param hwnd The window's handle.
- \param width The window's width.
- \param height The window's height.
-
-*/
-bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height)
-{
- //simple stuff
- nbCols = width;
- nbRows = height;
- hWnd = hwnd;
-
- //D3D initialize
- if(NULL == (pD3D = Direct3DCreate9(D3D_SDK_VERSION)))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't initialize D3D!");
-
- D3DDISPLAYMODE d3ddm;
- if(FAILED(pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm)))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't get the adapter's display mode!");
-
-
- D3DPRESENT_PARAMETERS d3dpp;
- ZeroMemory(&d3dpp, sizeof(d3dpp));
- d3dpp.BackBufferCount=1;
- d3dpp.Windowed = TRUE;
- d3dpp.SwapEffect = D3DSWAPEFFECT_DISCARD;
- d3dpp.BackBufferFormat = d3ddm.Format;
-
- //creates a d3d device
- if( FAILED(pD3D->CreateDevice(D3DADAPTER_DEFAULT , D3DDEVTYPE_HAL, hWnd,
- D3DCREATE_SOFTWARE_VERTEXPROCESSING |
- D3DCREATE_MULTITHREADED,
- &d3dpp, &pd3dDevice )))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create the Direct3D device!");
-
-
- //disables scene lightning
- pd3dDevice->SetRenderState(D3DRS_LIGHTING, FALSE);
-
-
- //inits the direct3D view (for 2D rendering)
- initView((float)nbCols,(float)nbRows);
-
-
- //computes texture size (needs to be a power-of-2 large square)
- textWidth = supPowerOf2( (nbCols>nbRows) ? nbCols : nbRows );
-
- //creates the system memory texture (the one we will directly modify)
- //unfortunately, needs to be X8R8G8B8 in order to be able to use GDI drawing
- //functions
- if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, 0,
- D3DFMT_X8R8G8B8, D3DPOOL_SYSTEMMEM , &pd3dText)
- != D3D_OK)
- {
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create memory texture!");
- }
-
- //creates the video memory texture (the one we will display) -
- if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT,
- D3DUSAGE_DYNAMIC ,
- D3DFMT_X8R8G8B8, D3DPOOL_DEFAULT, &pd3dVideoText)
- != D3D_OK)
- {
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create video texture!");
- }
-
- //creates the sprite used to render the texture
- if(D3DXCreateSprite(pd3dDevice, &pSprite) != S_OK)
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create the texture's sprite!");
-
-
-
- return true;
-}
-
-
-
-
-/*!
- Initialize the view (orthogonal ...).
- \param WindowWidth The Width of the window.
- \param WindowHeight The height of the window.
-*/
-void vpD3DRenderer::initView(float WindowWidth, float WindowHeight)
-{
- D3DXMATRIX Ortho2D;
- D3DXMATRIX Identity;
-
- D3DXMatrixOrthoLH(&Ortho2D, WindowWidth, WindowHeight, 0.0f, 1.0f);
- D3DXMatrixIdentity(&Identity);
-
- if( pd3dDevice->SetTransform(D3DTS_PROJECTION, &Ortho2D) != D3D_OK
- || pd3dDevice->SetTransform(D3DTS_WORLD, &Identity) != D3D_OK
- || pd3dDevice->SetTransform(D3DTS_VIEW, &Identity) != D3D_OK)
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't set the view!");
-}
-
-
-/*!
- Converts a ViSP RGBA image to the Direct3D texture format (BGRA).
- \param I Image to convert.
- \param imBuffer Destination buffer.
- \param pitch Pitch of the destination texture.
-*/
-void vpRGBaToTexture(const vpImage<vpRGBa>& I, unsigned char * imBuffer,
- unsigned int pitch)
-{
- unsigned int j = I.getWidth();
-
- unsigned int k=0;
- for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
- {
- if(j==0){
- k += pitch - (I.getWidth() * 4);
- j = I.getWidth();
- }
-
- imBuffer[k+0] = I.bitmap[i].B;
- imBuffer[k+1] = I.bitmap[i].G;
- imBuffer[k+2] = I.bitmap[i].R;
- imBuffer[k+3] = I.bitmap[i].A;// pb in vpimconvert? , 0xFF?
- k+=4;
- j--;
- }
-}
-
-/*!
- Converts a ViSP gray image to the Direct3D texture format (BGRA).
- \param I Image to convert.
- \param imBuffer Destination buffer.
- \param pitch Pitch of the destination texture.
-*/
-void vpGreyToTexture(const vpImage<unsigned char>& I,
- unsigned char * imBuffer,
- unsigned int pitch)
-{
- unsigned int j = I.getWidth();
-
- unsigned int k=0;
- for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
- {
- if(j==0){
- k += pitch - (I.getWidth() * 4);
- j = I.getWidth();
- }
-
- imBuffer[k+0] = imBuffer[k+1] = imBuffer[k+2] = I.bitmap[i];
- imBuffer[k+3] = 0xFF; //full opacity
-
- k+=4;
- j--;
- }
-}
-
-/*!
- Sets the image to display.
- \param im The image to display.
-*/
-void vpD3DRenderer::setImg(const vpImage<vpRGBa>& im)
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top=0; r.left=0;
- r.bottom=static_cast<signed long>(nbRows);
- r.right=static_cast<signed long>(nbCols);
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //fills this texture with the image data (converted to bgra)
- vpRGBaToTexture(im, buf, pitch);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
-
- }
-}
-
-/*!
- Sets the image to display.
- \param im The image to display.
-*/
-void vpD3DRenderer::setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top=(LONG)iP.get_v(); r.left=(LONG)iP.get_u();
- r.bottom=(LONG)(iP.get_v()+height-1); r.right=(LONG)(iP.get_u()+width);
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //fills this texture with the image data (converted to bgra)
- vpRGBaToTexture(im, buf, pitch);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
-
- }
-}
-
-
-/*!
- Sets the image to display.
- \param im The image to display.
-*/
-void vpD3DRenderer::setImg(const vpImage<unsigned char>& im)
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>(nbRows);
- r.right = static_cast<LONG>(nbCols);
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //fills this texture with the image data (converted to bgra)
- vpGreyToTexture(im, buf, pitch);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-
-}
-
-
-/*!
- Sets the image to display.
- \param im The image to display.
-*/
-void vpD3DRenderer::setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top=(LONG)iP.get_v(); r.left=(LONG)iP.get_u();
- r.bottom=(LONG)(iP.get_v()+height-1); r.right=(LONG)(iP.get_u()+width);
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //fills this texture with the image data (converted to bgra)
- vpGreyToTexture(im, buf, pitch);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-
-}
-
-/*!
- Renders the memory texture to the screen.
- \return True.
-
-*/
-bool vpD3DRenderer::render()
-{
- // Clears the back buffer to a blue color
- //pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET, D3DCOLOR_XRGB(0,0,255), 1.0f, 0 );
-
- //Begins the scene.
- pd3dDevice->BeginScene();
-
- //Texture rectangle to display
- RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>(nbRows);
- r.right = static_cast<LONG>(nbCols);
-
- //Updates the video memory texture with the content of the system
- //memory texture
- pd3dDevice->UpdateTexture(pd3dText,pd3dVideoText);
-
- //Displays this texture as a sprite
-
-#if (D3DX_SDK_VERSION <= 9)
- pSprite->Begin(); //
- pSprite->Draw(pd3dVideoText, &r, NULL, NULL, NULL, NULL, 0xFFFFFFFF );
-#else
- pSprite->Begin(0);
- pSprite->Draw(pd3dVideoText, &r, NULL, NULL, 0xFFFFFFFF );
-#endif
- pSprite->End();
-
- //Ends the scene.
- pd3dDevice->EndScene();
- //Presents the backbuffer
- pd3dDevice->Present( NULL, NULL, NULL, NULL );
-
- return true;
-}
-
-
-/*!
- Sets a pixel to color at position (j,i).
-
- \param ip : The pixel coordinates.
- \param color : the color of the point.
-*/
-void vpD3DRenderer::setPixel(const vpImagePoint &iP,
- const vpColor &color)
-{
- if(iP.get_i()<0 || iP.get_j()<0 || iP.get_i()>=(int)nbRows || iP.get_j()>=(int)nbCols)
- {
- return;
- }
-
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
-
- r.top=(LONG)iP.get_i();
- r.left=(LONG)iP.get_j();
- r.bottom=(LONG)iP.get_i()+1;
- r.right=(LONG)iP.get_j()+1;
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //the coordinates are in the locked area base
- setBufferPixel(buf, pitch, 0, 0,color);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-
-}
-
-/*!
- Draws a line.
- \param ip1,ip2 : Initial and final image point.
- \param color the line's color
- \param thickness : Thickness of the line.
- \param style style of the line
-*/
-void vpD3DRenderer::drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness, int style)
-{
-// if(i1<0 || j1<0 || i2<0 || j2<0 || e<0)
-// {
-// vpCERROR<<"Invalid parameters!"<<std::endl;
-// return;
-// }
-
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
-
- //Will contain the texture's surface drawing context
- HDC hDCMem;
-
- //The texture's surface
- IDirect3DSurface9 * pd3dSurf;
- pd3dText->GetSurfaceLevel(0, &pd3dSurf);
-
- //We get its DC
- pd3dSurf->GetDC(&hDCMem);
-
- //create the pen
- HPEN hPen;
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(style, static_cast<int>(thickness), colorsGDI[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
- }
-
- //we don't use the bkColor
- SetBkMode(hDCMem, TRANSPARENT);
-
- //select the pen
- SelectObject(hDCMem, hPen);
-
- //move to the starting point
- MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
- //Draw the line
- LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
-
-
- //Releases the DC
- pd3dSurf->ReleaseDC(hDCMem);
- //Releases the surface's interface
- pd3dSurf->Release();
- //Deletes additional objects
- DeleteObject(hPen);
- }
-}
-
-
-/*!
- Draws a rectangle.
- \param topLeft its top left point's coordinates
- \param width width of the rectangle
- \param height height of the rectangle
- \param color The rectangle's color
- \param fill When set to true fill the rectangle.
- \param thickness : Line thickness
-*/
-void vpD3DRenderer::drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill ,
- unsigned int /*thickness*/)
-{
- if(topLeft.get_i()>(int)nbRows-1 || topLeft.get_j()>(int)nbCols-1|| topLeft.get_i()+height<0 ||topLeft.get_j()+width<0)
- {
- // vpCERROR<<"Invalid parameters!"<<std::endl;
- return;
- }
-
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top= (LONG)((topLeft.get_i()>0)? topLeft.get_i() : 0 );
- r.left=(LONG)((topLeft.get_j()>0)? topLeft.get_j() : 0 );
- r.bottom=(LONG)((topLeft.get_i()+height < (int)nbRows) ? topLeft.get_i()+height : nbRows-1);
- r.right=(LONG)((topLeft.get_j()+width < (int)nbCols) ? topLeft.get_j()+width : nbCols-1);
-
- /* unsigned */ int rectW = r.right - r.left;
- /* unsigned */ int rectH = r.bottom - r.top;
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- /* unsigned */ int x= 0;
- /* unsigned */ int y= 0;
-
- if(fill == false)
- {
- //draws the top horizontal line
- if(topLeft.get_i()>=0)
- for(; x<rectW ; x++)
- setBufferPixel(buf, pitch, x, y, color);
-
- //draws the right vertical line
- if(topLeft.get_j()+width < nbCols)
- for(; y<rectH ; y++)
- setBufferPixel(buf, pitch, x, y, color);
-
- //draws the bottom horizontal line
- if(topLeft.get_i()+height < nbRows)
- for(; x>0 ; x--)
- setBufferPixel(buf, pitch, x, y, color);
-
- //draws the left vertical line
- if(topLeft.get_j()>=0)
- for(; y>0 ; y--)
- setBufferPixel(buf, pitch, x, y, color);
- }
-
- else
- {
- if(topLeft.get_i()>=0 && topLeft.get_j()+width < nbCols && topLeft.get_i()+height < nbRows && topLeft.get_j()>=0)
- {
- for (x = 0; x<rectW; x++)
- {
- for (y = 0; y<rectH; y++)
- setBufferPixel(buf, pitch, x, y, color);
- }
- }
- }
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-}
-
-/*!
- Clears the image to a specific color.
- \param color The color used to fill the image.
-*/
-void vpD3DRenderer::clear(const vpColor &color)
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>( nbRows );
- r.right = static_cast<LONG>( nbCols );
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- long * buf = (long *) ( d3dLRect.pBits );
-
- unsigned long c;
- if (color.id < vpColor::id_unknown)
- c = colors[color.id];
- else {
- c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
- }
- long * end = (long*)((long)buf + (pitch * nbRows));
-
- //fills the whole image
- while (buf < end)
- *buf++ = static_cast<long>( c );
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-}
-
-
-
-//writes current circle pixels using symetry to reduce the algorithm's complexity
-void vpD3DRenderer::subDrawCircle(int i, int j,
- int x, int y,
- vpColor col,
- unsigned char* buf, unsigned int pitch,
- unsigned int maxX, unsigned int maxY)
-{
- if (x == 0) {
- setBufferPixel(buf, pitch, i , j+y, col, maxX, maxY);
- setBufferPixel(buf, pitch, i , j-y, col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y, j, col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y, j, col, maxX, maxY);
- } else
- if (x == y) {
- setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
- } else
- if (x < y) {
- setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y,j+x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y,j+x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y,j-x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y,j-x,col, maxX, maxY);
- }
-
-}
-
-/*!
- Draws a circle.
- \param center its center point's coordinates
- \param radius The circle's radius
- \param color The circle's color
-*/
-void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool /*fill*/, unsigned int /*thickness*/)
-{
- if(radius<1 || vpMath::round(center.get_i()+radius)<0 || vpMath::round(center.get_i()-radius) > (int)nbRows || vpMath::round(center.get_j()+radius)<0 || vpMath::round(center.get_j()-radius) > (int)nbCols)
- return;
-
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT rec;
- int radius_ = static_cast<int>( radius );
- int rleft = (vpMath::round(center.get_j()-radius_) > 0) ? vpMath::round(center.get_j())-radius_ : 0;
- int rtop = (vpMath::round(center.get_i()-radius_) > 0) ? vpMath::round(center.get_i())-radius_ : 0;
-
- rec.top= rtop;
- rec.left= rleft;
- rec.bottom=(LONG)((vpMath::round(center.get_i()+radius_) < (int)nbRows) ? center.get_i()+radius_ : nbRows-1);
- rec.right=(LONG)((vpMath::round(center.get_j()+radius_) < (int)nbCols) ? center.get_j()+radius_ : nbCols-1);
-
- //used as maxX and maxY for setBufferPixel
- unsigned int rectW = static_cast<unsigned int> ( rec.right - rleft );
- unsigned int rectH = static_cast<unsigned int> ( rec.bottom - rtop );
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &rec, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- // Bresenham 's circle algorithm
-
- int x = 0;
- int y = static_cast<int>( radius );
- int p = (3 - (y<<1));
-
- vpImagePoint ip;
- ip.set_i(center.get_i()-rtop);
- ip.set_j(center.get_j()-rleft);
-
- subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH);
- while(x < y){
- x++;
- if (p < 0)
- {
- p += ((x<<1)+1)<<1;
- }
- else
- {
- y--;
- p += (((x-y)<<1)+1)<<1;
- }
- subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH);
- }
-
-
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-}
-
-
-
-/*!
- Draws some text.
- \param ip its top left point's coordinates
- \param text The string to display
- \param color The text's color
-*/
-void vpD3DRenderer::drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color)
-{
- //Will contain the texture's surface drawing context
- HDC hDCMem;
-
- //The texture's surface
- IDirect3DSurface9 * pd3dSurf;
- pd3dText->GetSurfaceLevel(0, &pd3dSurf);
-
- //We get its DC
- pd3dSurf->GetDC(&hDCMem);
-
- //Select the font
- SelectObject(hDCMem, hFont);
-
- //set the text color
- if (color.id < vpColor::id_unknown)
- SetTextColor(hDCMem, colorsGDI[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- SetTextColor(hDCMem, gdicolor);
- }
-
- //we don't use the bkColor
- SetBkMode(hDCMem, TRANSPARENT);
-
- SIZE size;
- int length = (int) strlen(text);
-
- //get the displayed string dimensions
- GetTextExtentPoint32(hDCMem, text, length, &size);
-
- //displays the string
- TextOut(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v()), text, length);
-
- //Releases the DC
- pd3dSurf->ReleaseDC(hDCMem);
- //Releases the surface's interface
- pd3dSurf->Release();
- //Deletes additional objects
- DeleteObject(hFont);
-}
-
-
-/*!
- Draws a cross.
- \param ip its center point's coordinates
- \param size Size of the cross
- \param color The cross' color
- \param thickness width of the cross
-*/
-void vpD3DRenderer::drawCross(const vpImagePoint &ip,
- unsigned int size,
- const vpColor &color, unsigned int thickness)
-{
- if(ip.get_i()<0 || ip.get_j()<0 || ip.get_i()>(int)nbRows || ip.get_j()>(int)nbCols || thickness<=0)
- return;
-
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
- D3DLOCKED_RECT d3dLRect;
-
- RECT rec;
- thickness = (thickness<size)? thickness : size;
- int half_size_ = static_cast<int>( size/2 );
- //if j-size/2 is inferior to 0, use 0
- int rleft = ( (vpMath::round(ip.get_j()) - half_size_) < 0 ) ? 0 : vpMath::round(ip.get_j()) - half_size_;
- //if j-size/2 is inferior to 0, use 0
- int rtop = ( (vpMath::round(ip.get_i()) - half_size_) < 0 ) ? 0 : vpMath::round(ip.get_i()) - half_size_;
-
- rec.top = rtop;
- rec.left = rleft;
- rec.bottom= (LONG)(ip.get_i() + (size/2));
- rec.right = (LONG)(ip.get_j() + (size/2));
-
- //locks the texture to directly access it
- if( pd3dText->LockRect(0, &d3dLRect, &rec, 0) != D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- /* unsigned */ int x; //xpos
-
- //y-coordinate of the line in the locked rectangle base
-
- /* unsigned */ int y =( vpMath::round(ip.get_i()) < half_size_ ) ? vpMath::round(ip.get_i()) : half_size_;
-
- /* unsigned */ int cpt = 0; //number of lines
- unsigned int re = thickness; //remaining "width"
-
- //horizontal lines
- //stops when there is enough line for e
- while(re!=0)
- {
- //draws a line
- for(x=0; x<(rec.right - rec.left); x++)
- setBufferPixel(buf, pitch, x, y, color);
-
- re--;
- cpt++;
-
- //write alternatively a line at the top and a line at the bottom
- //eg : y=4 -> y=5 -> y=3 -> y=6
- y += ( (re&1) != 0u) ? cpt : -cpt;
- }
-
- cpt = 0;
- re = thickness;
-
- //x-coordinate of the line in the locked rectangle base
- x = ( vpMath::round(ip.get_j()) < half_size_ ) ? vpMath::round(ip.get_j()) : half_size_;
-
- //vertical lines
- while(re!=0)
- {
- //draws a vertical line
- for(y=0; y<rec.bottom - rec.top; y++)
- setBufferPixel(buf, pitch, x, y, color);
-
- re--;
- cpt++;
-
- //write alternatively a line on the left and a line on the right
- x += ( (re&1) != 0) ? cpt : -cpt;
- }
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-}
-
-
-/*!
- Draws an arrow.
- \param ip1,ip2 : Initial and final image point.
- \param color The arrow's color
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpD3DRenderer::drawArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w,unsigned int h, unsigned int thickness)
-{
- double a = ip2.get_i() - ip1.get_i();
- double b = ip2.get_j() - ip1.get_j();
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
- int _h = static_cast<int>( h );
-
- //Will contain the texture's surface drawing context
- HDC hDCMem;
-
- //The texture's surface
- IDirect3DSurface9 * pd3dSurf;
- pd3dText->GetSurfaceLevel(0, &pd3dSurf);
-
- //We get its DC
- pd3dSurf->GetDC(&hDCMem);
-
- //create the pen
- HPEN hPen;
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colorsGDI[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
- }
-
- //select the pen
- SelectObject(hDCMem, hPen);
-
- //based on code from other displays
- if ((a==0)&&(b==0))
- {
- // DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
-
- vpImagePoint ip3;
- ip3.set_i( ip2.get_i() - w*a );
- ip3.set_j( ip2.get_j() - w*b );
-
-
- vpImagePoint ip4 ;
-
- ip4.set_i( ip3.get_i() + b*_h );
- ip4.set_j( ip3.get_j() - a*_h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
- MoveToEx(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()), NULL);
- LineTo(hDCMem, vpMath::round(ip4.get_j()), vpMath::round(ip4.get_i()));
- }
-
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
- MoveToEx(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()), NULL);
- LineTo(hDCMem, vpMath::round(ip4.get_j()), vpMath::round(ip4.get_i()));
- }
-
- MoveToEx(hDCMem, vpMath::round(ip1.get_j()), vpMath::round(ip1.get_i()), NULL);
- LineTo(hDCMem, vpMath::round(ip2.get_j()), vpMath::round(ip2.get_i()));
-
- }
- //Deletes the pen
- DeleteObject(hPen);
- //Releases the DC
- pd3dSurf->ReleaseDC(hDCMem);
- //Releases the surface's interface
- pd3dSurf->Release();
-
-
-}
-
-/*!
- Converts the D3D textures to vpImage<vpRGBa>.
- \param I The destination image.
- \param imBuffer The texture's data.
- \param pitch The texture's pitch.
-*/
-void TextureToRGBa(vpImage<vpRGBa>& I, unsigned char * imBuffer,
- unsigned int pitch)
-{
- unsigned int j = I.getWidth();
-
- unsigned int k=0;
- for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
- {
- //go to the next line
- if(j==0){
- k += pitch - (I.getWidth() * 4);
- j = I.getWidth();
- }
-
- //simple conversion from bgra to rgba
- I.bitmap[i].B = imBuffer[k+0];
- I.bitmap[i].G = imBuffer[k+1];
- I.bitmap[i].R = imBuffer[k+2];
- I.bitmap[i].A = imBuffer[k+3];
-
- k+=4;
- j--;
- }
-}
-
-/*!
- Gets the currently displayed image with its overlay.
- \param I The image to fill.
-*/
-void vpD3DRenderer::getImage(vpImage<vpRGBa> &I)
-{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
-
- //resize the destination image as needed
- I.resize(nbRows, nbCols);
-
- D3DLOCKED_RECT d3dLRect;
-
- RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>( nbRows );
- r.right = static_cast<LONG>( nbCols );
-
- //locks the whole texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
- return;
- }
-
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
-
- //fills this image with the texture's data
- TextureToRGBa(I,buf, pitch);
-
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
- }
-}
-
-#endif
-#endif
diff --git a/src/device/display/windows/vpD3DRenderer.h b/src/device/display/windows/vpD3DRenderer.h
deleted file mode 100755
index 5231be1..0000000
--- a/src/device/display/windows/vpD3DRenderer.h
+++ /dev/null
@@ -1,211 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpD3DRenderer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * D3D renderer for windows 32 display
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_D3D9) )
-
-#ifndef VPD3DRENDERER_HH
-#define VPD3DRENDERER_HH
-
-#include <windows.h>
-#include <d3dx9.h>
-#include <visp/vpWin32Renderer.h>
-#include <visp/vpDisplayException.h>
-
-
-#include <iostream>
-
-/*!
- \class vpD3DRenderer.h
-
- \brief Display under windows using Direct3D9.
- Is used by vpD3DDisplay to do the drawing.
-
-*/
-class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer
-{
-
- IDirect3D9 * pD3D;
-
- //The d3d device we will be working with.
- IDirect3DDevice9 * pd3dDevice;
-
- //Sprite used to render the texture.
- ID3DXSprite * pSprite;
-
- //The system memory texture :
- //The one we will be drawing on.
- IDirect3DTexture9 * pd3dText;
-
- //The video memory texture :
- //The one we will use for display.
- IDirect3DTexture9 * pd3dVideoText;
-
- //The texture's width.
- unsigned int textWidth;
-
- //The window's handle.
- HWND hWnd;
-
- //Colors for overlay drawn with d3d directly.
- unsigned long colors[vpColor::id_unknown];
-
- //Colors for overlay drawn with GDI.
- COLORREF colorsGDI[vpColor::id_unknown];
-
- //Font used for text drawing.
- HFONT hFont;
-
- public:
-
- bool init(HWND hwnd, unsigned int width, unsigned int height);
- bool render();
-
- vpD3DRenderer();
- virtual ~vpD3DRenderer();
-
- void setImg(const vpImage<vpRGBa>& im);
- void setImg(const vpImage<unsigned char>& im);
- void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
- void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
-
- void setPixel(const vpImagePoint &iP, const vpColor &color);
-
- void drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness, int style=PS_SOLID);
-
- void drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill=false,
- unsigned int thickness=1);
-
- void clear(const vpColor &color);
-
- void drawCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill=false, unsigned int thickness=1);
-
- void drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color);
-
- void drawCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1);
-
- void drawArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness=1);
-
- void getImage(vpImage<vpRGBa> &I);
-
-
-
- private:
-
- void initView(float, float);
-
- /*!
- Sub function for circle drawing.
- Circle drawing is based on Bresenham 's circle algorithm.
- */
- void subDrawCircle(int i, int j,
- int x, int y,
- vpColor col, unsigned char* buf,
- unsigned int pitch, unsigned int maxX, unsigned int maxY);
-
-
- /*!
- Safe useful inline function to set a pixel in a texture buffer.
- \param buf The texture's buffer.
- \param pitch The image pitch.
- \param x The x-coordinate of the pixel (in the locked rectangle base)
- \param y The y-coordinate of the pixel (in the locked rectangle base)
- \param color The color of the pixel.
- \param maxX The maximum x value (equals to the width of the locked rectangle).
- \param maxY The maximum y value (equals to the height of the locked rectangle).
-
- */
- inline void setBufferPixel(unsigned char* buf, unsigned int pitch,
- int x, int y,
- const vpColor &color,
- unsigned int maxX, unsigned int maxY)
- {
- unsigned long c;
- if (color.id < vpColor::id_unknown)
- c = colors[color.id];
- else {
- c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
- }
-
- if(x>=0 && y>=0 && x<= (int)maxX && y<= (int)maxY)
- *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
- }
- /*!
- Unsafe useful inline function to set a pixel in a texture buffer.
- \param buf The texture's buffer.
- \param pitch The image pitch.
- \param x The x-coordinate of the pixel (in the locked rectangle base)
- \param y The y-coordinate of the pixel (in the locked rectangle base)
- \param color The color of the pixel.
-
- */
- inline void setBufferPixel(unsigned char* buf, unsigned int pitch,
- int x, int y,
- const vpColor &color)
- {
- unsigned long c;
- if (color.id < vpColor::id_unknown)
- c = colors[color.id];
- else {
- c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
- }
-
- *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
- }
-
- unsigned int supPowerOf2(unsigned int n);
-
-};
-#endif
-#endif
-#endif
diff --git a/src/device/display/windows/vpDisplayD3D.cpp b/src/device/display/windows/vpDisplayD3D.cpp
deleted file mode 100755
index 36d4f0c..0000000
--- a/src/device/display/windows/vpDisplayD3D.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayD3D.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * windows 32 display using D3D
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-/*!
-\file vpDisplayD3D.cpp
-\brief windows 32 display using D3D
-*/
-
-#include <visp/vpConfig.h>
-#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
-
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpD3DRenderer.h>
-
-/*!
- \brief Basic constructor.
-*/
-vpDisplayD3D::vpDisplayD3D(): vpDisplayWin32(new vpD3DRenderer()){}
-
-/*!
-
- \brief Constructor : Initialize a display.
-
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
-
-*/
-vpDisplayD3D::vpDisplayD3D(int winx, int winy, const char *title)
- : vpDisplayWin32(new vpD3DRenderer())
-{
- windowXPosition = winx;
- windowYPosition = winy;
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-}
-
-/*!
-
-\brief Constructor : initialize a display to visualize a RGBa image
-(32 bits).
-
-\param I : Image to be displayed (note that image has to be initialized).
-\param winx, winy : The window is set at position x,y (column index, row index).
-\param title : Window's title.
-
-*/
-vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I,
- int winx, int winy,
- const char *title)
- : vpDisplayWin32(new vpD3DRenderer())
-{
- init(I,winx,winy,title);
-}
-
-/*!
-
-\brief Constructor : initialize a display to visualize a grayscale image
-(8 bits).
-
-\param I Image to be displayed (note that image has to be initialized).
-\param winx, winy The window is set at position x,y (column index, row index).
-\param title Window's title.
-
-*/
-vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I,
- int winx, int winy,
- const char *title)
- : vpDisplayWin32(new vpD3DRenderer())
-{
- init(I,winx,winy,title);
-}
-
-/*!
- \brief Basic destructor.
-*/
-vpDisplayD3D::~vpDisplayD3D(){}
-
-#endif
-
diff --git a/src/device/display/windows/vpDisplayD3D.h b/src/device/display/windows/vpDisplayD3D.h
deleted file mode 100755
index dd71b66..0000000
--- a/src/device/display/windows/vpDisplayD3D.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayD3D.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display using D3D
- *
- * Authors:
- * Bruno Renier
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDisplay.h>
-#if ( defined(VISP_HAVE_D3D9) )
-
-#ifndef VPDISPLAYD3D_HH
-#define VPDISPLAYD3D_HH
-
-
-#include <visp/vpDisplayWin32.h>
-
-/*!
- \class vpDisplayD3D
-
- \ingroup ImageGUI
-
- \brief Display for windows using Direct3D.
-
- Direct3D is part of the DirectX API available under Windows
- operating systems.
-
- \warning Requires DirectX9 SDK to compile and DirectX9 DLLs to run.
-
- The example below shows how to display an image with this video device.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayD3D.h>
-
-int main()
-{
-#if defined(VISP_HAVE_D3D9)
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
- vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
-
- vpDisplayD3D d;
-
- // Initialize the display with the image I. Display and image are
- // now link together.
- d.init(I);
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My Direct 3D display");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(10);
- topLeftCorner.set_j(20);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::red, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Wait for a click in the display window
- vpDisplay::getClick(I);
-#endif
-}
- \endcode
-*/
-class VISP_EXPORT vpDisplayD3D : public vpDisplayWin32
-{
-public:
- vpDisplayD3D();
- vpDisplayD3D(int winx, int winy, const char *title=NULL);
- vpDisplayD3D(vpImage<vpRGBa> &I,int winx=-1, int winy=-1, const char *title=NULL);
- vpDisplayD3D(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const char *title=NULL);
-
- virtual ~vpDisplayD3D();
-
-};
-#endif
-#endif
diff --git a/src/device/display/windows/vpDisplayGDI.cpp b/src/device/display/windows/vpDisplayGDI.cpp
deleted file mode 100755
index 6972110..0000000
--- a/src/device/display/windows/vpDisplayGDI.cpp
+++ /dev/null
@@ -1,121 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayGDI.cpp 4642 2014-02-05 12:42:30Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * GDI based Display for windows 32.
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-/*!
-\file vpDisplayGDI.cpp
-\brief GDI based Display for windows 32.
-*/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) )
-
-#include <visp/vpDisplayGDI.h>
-
-//A vpDisplayGDI is just a vpDisplayWin32 which uses a vpGDIRenderer to do the drawing.
-
-/*!
- \brief Basic constructor.
-*/
-vpDisplayGDI::vpDisplayGDI(): vpDisplayWin32(new vpGDIRenderer()){}
-
-/*!
-
- \brief Constructor : Initialize a display.
-
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
-
-*/
-vpDisplayGDI::vpDisplayGDI(int winx, int winy, const char *title)
- : vpDisplayWin32(new vpGDIRenderer())
-{
- windowXPosition = winx;
- windowYPosition = winy;
-
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-}
-
-/*!
-
- \brief Constructor : Initialize a display to visualize a RGBa image
- (32 bits).
-
- \param I : image to be displayed (note that image has to be initialized).
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
-
-*/
-vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I,
- int winx, int winy,
- const char *title)
- : vpDisplayWin32(new vpGDIRenderer())
-{
- init(I,winx,winy,title);
-}
-
-/*!
-
- \brief Constructor : Initialize a display to visualize a grayscale image
- (8 bits).
-
- \param I Image to be displayed (note that image has to be initialized).
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
-
-*/
-vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I,
- int winx, int winy,
- const char *title)
- : vpDisplayWin32(new vpGDIRenderer())
-{
- init(I,winx,winy,title);
-}
-
-/*!
- \brief Basic destructor.
-*/
-vpDisplayGDI::~vpDisplayGDI(){}
-
-
-#endif
diff --git a/src/device/display/windows/vpDisplayGDI.h b/src/device/display/windows/vpDisplayGDI.h
deleted file mode 100755
index ffe1b31..0000000
--- a/src/device/display/windows/vpDisplayGDI.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayGDI.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display using GDI
- *
- * Authors:
- * Bruno Renier
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-#include <visp/vpDisplay.h>
-
-#if ( defined(VISP_HAVE_GDI) )
-
-#ifndef vpDisplayGDI_HH
-#define vpDisplayGDI_HH
-
-#include <visp/vpDisplayWin32.h>
-
-/*!
- \class vpDisplayGDI
-
- \ingroup ImageGUI
-
- \brief Display for windows using GDI (available on any windows 32 platform).
-
- GDI stands for Graphics Device Interface and is a core component of Microsoft
- Windows operating systems used for displaying graphics in a window.
-
- The example below shows how to display an image with this video device.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayGDI.h>
-
-int main()
-{
-#if defined(VISP_HAVE_GDI)
- vpImage<unsigned char> I; // Grey level image
-
- // Read an image in PGM P5 format
-#ifdef _WIN32
- vpImageIo::read(I, "C:/temp/ViSP-images/Klimt/Klimt.pgm");
-#else
- vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
-#endif
-
- vpDisplayGDI d;
-
- // Initialize the display with the image I. Display and image are
- // now link together.
- d.init(I);
-
- // Specify the window location
- vpDisplay::setWindowPosition(I, 400, 100);
-
- // Set the display window title
- vpDisplay::setTitle(I, "My GDI display");
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, vpColor::red, true);
-
- // Draw a red rectangle in the display overlay (foreground)
- vpImagePoint topLeftCorner;
- topLeftCorner.set_i(50);
- topLeftCorner.set_j(10);
- vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- // Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
- char key[10];
- bool ret;
- for (int i=0; i< 200; i++) {
- bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
- }
-
- // Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
- ret = vpDisplay::getKeyboardEvent(I, key, true);
- std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
- std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
- // Wait for a click in the display window
- std::cout << "Wait for a button click..." << std::endl;
- vpDisplay::getClick(I);
-#endif
-}
- \endcode
-*/
-class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32
-{
-public:
- vpDisplayGDI();
- vpDisplayGDI(int winx, int winy, const char *title=NULL);
- vpDisplayGDI(vpImage<vpRGBa> &I,int winx=-1, int winy=-1, const char *title=NULL);
- vpDisplayGDI(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const char *title=NULL);
-
- virtual ~vpDisplayGDI();
-};
-
-#endif
-#endif
-
diff --git a/src/device/display/windows/vpDisplayWin32.cpp b/src/device/display/windows/vpDisplayWin32.cpp
deleted file mode 100755
index fdcbd93..0000000
--- a/src/device/display/windows/vpDisplayWin32.cpp
+++ /dev/null
@@ -1,964 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayWin32.cpp 4661 2014-02-10 19:34:58Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display base class
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-
-#define FLUSH_ROI
-#include <visp/vpDisplayWin32.h>
-#include <visp/vpDisplayException.h>
-#include <string>
-
-
-
-const int vpDisplayWin32::MAX_INIT_DELAY = 5000;
-
-/*!
- Thread entry point.
- Used as a detour to initWindow.
-*/
-void vpCreateWindow(threadParam * param)
-{
- //char* title = param->title;
- (param->vpDisp)->window.initWindow(param->title.c_str(), param->x, param->y,
- param->w, param->h);
- delete param;
-}
-
-/*!
- Constructor.
-*/
-vpDisplayWin32::vpDisplayWin32(vpWin32Renderer * rend) :
- iStatus(false), window(rend)
-{
-}
-
-
-/*!
- Destructor.
-*/
-vpDisplayWin32::~vpDisplayWin32()
-{
- closeDisplay();
-}
-
-
-/*!
-
- Constructor. Initialize a display to visualize a gray level image
- (8 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void vpDisplayWin32::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const char *title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
-
- window.renderer->setImg(I);
-
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
-}
-
-
-/*!
- Constructor. Initialize a display to visualize a RGBa level image
- (32 bits).
-
- \param I : Image to be displayed (not that image has to be initialized)
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-void vpDisplayWin32::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const char *title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- vpERROR_TRACE("Image not initialized " ) ;
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
- }
-
- window.renderer->setImg(I);
-
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
-}
-
-
-/*!
- Initialize the display size, position and title.
-
- \param width, height : Width and height of the window.
- \param x, y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-
-*/
-void vpDisplayWin32::init(unsigned int width, unsigned int height,
- int x, int y,
- const char *title)
-{
- if (title != NULL)
- title_ = std::string(title);
- else
- title_ = std::string(" ");
-
- if (x != -1)
- windowXPosition = x;
- if (y != -1)
- windowYPosition = y;
-
- //we prepare the window's thread creation
- threadParam * param = new threadParam;
- param->x = windowXPosition;
- param->y = windowYPosition;
- param->w = width;
- param->h = height;
- param->vpDisp = this;
- param->title = this->title_;
-
- //creates the window in a separate thread
- hThread = CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)vpCreateWindow,
- param,0,&threadId);
-
- //the initialization worked
- iStatus = (hThread != (HANDLE)NULL);
-
- displayHasBeenInitialized = true;
-}
-
-
-/*!
- If the window is not initialized yet, wait a little (MAX_INIT_DELAY).
- \exception notInitializedError : the window isn't initialized
-*/
-void vpDisplayWin32::waitForInit()
-{
- //if the window is not initialized yet
- if(!window.isInitialized())
- {
- //wait
- if( WAIT_OBJECT_0 != WaitForSingleObject(window.semaInit,MAX_INIT_DELAY))
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Window not initialized")) ;
- //problem : the window is not initialized
- }
-}
-
-
-/*!
- Display the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning suppres the overlay drawing
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayWin32::displayImage(const vpImage<vpRGBa> &I)
-{
- //waits if the window is not initialized
- waitForInit();
-
- //sets the image to render
- window.renderer->setImg(I);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
-}
-
-
-/*!
- Display a selection of the color image \e I in RGBa format (32bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param width : Width of the region of interest
-
- \param height : Height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayWin32::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //waits if the window is not initialized
- waitForInit();
-
- //sets the image to render
- window.renderer->setImgROI(I,iP,width,height);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
-}
-
-
-/*!
- Display the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning suppres the overlay drawing
-
- \param I : Image to display.
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayWin32::displayImage(const vpImage<unsigned char> &I)
-{
- //wait if the window is not initialized
- waitForInit();
-
- //sets the image to render
- window.renderer->setImg(I);
- //sends a message to the window
- //PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
-}
-
-/*!
- Display a selection of the gray level image \e I (8bits).
-
- \warning Display has to be initialized.
-
- \warning Suppress the overlay drawing in the region of interest.
-
- \param I : Image to display.
-
- \param iP : Top left corner of the region of interest
-
- \param width : Width of the region of interest
-
- \param height : Height of the region of interest
-
- \sa init(), closeDisplay()
-*/
-void vpDisplayWin32::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //waits if the window is not initialized
- waitForInit();
-
- //sets the image to render
- window.renderer->setImgROI(I,iP,width,height);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
-}
-
-
-/*!
- Wait for a click from one of the mouse button.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-*/
-bool vpDisplayWin32::getClick( bool blocking)
-{
- //wait if the window is not initialized
- waitForInit();
- bool ret = false;
- //sends a message to the window
-// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
-
- //waits for a button to be pressed
- if(blocking){
- WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0); //to erase previous events
- WaitForSingleObject(window.semaClick, INFINITE);
- ret = true;
- }
- else {
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
- }
-
- return ret;
-}
-
-
-/*!
- Wait for a click from one of the mouse button and get the position
- of the clicked image point.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
-*/
-bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking)
-{
- //wait if the window is not initialized
- waitForInit();
-
- bool ret = false ;
- double u, v;
- //tells the window there has been a getclick demand
-// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
- //waits for a click
- if(blocking){
- WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
- WaitForSingleObject(window.semaClick, INFINITE);
- ret = true;
- }
- else {
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
- }
-
- u = window.clickX;
- v = window.clickY;
- ip.set_u( u );
- ip.set_v( v );
-
- return ret;
-}
-
-
-/*!
- Wait for a mouse button click and get the position of the clicked
- pixel. The button used to click is also set.
-
- \param ip [out] : The coordinates of the clicked image point.
-
- \param button [out] : The button used to click.
-
- \param blocking [in] :
- - When set to true, this method waits until a mouse button is
- pressed and then returns always true.
- - When set to false, returns true only if a mouse button is
- pressed, otherwise returns false.
-
- \return true if a mouse button is pressed, false otherwise. If a
- button is pressed, the location of the mouse pointer is updated in
- \e ip.
-*/
-bool vpDisplayWin32::getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- //wait if the window is not initialized
- waitForInit();
- bool ret = false;
- double u, v;
- //tells the window there has been a getclickup demand
-// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
- //waits for a click
- if(blocking){
- WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
- WaitForSingleObject(window.semaClick, INFINITE);
- ret = true;
- }
- else
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
-
- u = window.clickX;
- v = window.clickY;
- ip.set_u( u );
- ip.set_v( v );
- button = window.clickButton;
-
- return ret;
-}
-
-
-/*!
- Wait for a mouse button click release and get the position of the
- image point were the click release occurs. The button used to click is
- also set. Same method as getClick(unsigned int&, unsigned int&,
- vpMouseButton::vpMouseButtonType &, bool).
-
- \param ip [out] : Position of the clicked image point.
-
- \param button [in] : Button used to click.
-
- \param blocking [in] : true for a blocking behaviour waiting a mouse
- button click, false for a non blocking behaviour.
-
- \return
- - true if a button was clicked. This is always the case if blocking is set
- to \e true.
- - false if no button was clicked. This can occur if blocking is set
- to \e false.
-
- \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
-
-*/
-bool vpDisplayWin32::getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
-{
- //wait if the window is not initialized
- waitForInit();
- bool ret = false;
- double u, v;
- //tells the window there has been a getclickup demand
-// PostMessage(window.getHWnd(), vpWM_GETCLICKUP, 0,0);
-
- //waits for a click release
- if(blocking){
- WaitForSingleObject(window.semaClickUp, 0);
- WaitForSingleObject(window.semaClick, 0);//to erase previous events
- WaitForSingleObject(window.semaClickUp, INFINITE);
- ret = true;
- }
- else
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClickUp, 0));
-
- u = window.clickXUp;
- v = window.clickYUp;
- ip.set_u( u );
- ip.set_v( v );
- button = window.clickButtonUp;
-
- return ret;
-}
-
-/*!
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool vpDisplayWin32::getKeyboardEvent( bool blocking )
-{
- //wait if the window is not initialized
- waitForInit();
-
- bool ret = false ;
- //waits for a keyboard event
- if(blocking){
- WaitForSingleObject(window.semaKey, 0); // key down
- WaitForSingleObject(window.semaKey, 0); // key up
- WaitForSingleObject(window.semaKey, INFINITE);
- ret = true;
- }
- else
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
-
- return ret;
-}
-/*!
-
- Get a keyboard event.
-
- \param blocking [in] : Blocking behavior.
- - When set to true, this method waits until a key is
- pressed and then returns always true.
- - When set to false, returns true only if a key is
- pressed, otherwise returns false.
-
- \param string [out]: If possible, an ISO Latin-1 character
- corresponding to the keyboard key.
-
- \return
- - true if a key was pressed. This is always the case if blocking is set
- to \e true.
- - false if no key was pressed. This can occur if blocking is set
- to \e false.
-*/
-bool vpDisplayWin32::getKeyboardEvent(char *string, bool blocking)
-{
- //wait if the window is not initialized
- waitForInit();
-
- bool ret = false ;
- //waits for a keyboard event
- if(blocking){
- WaitForSingleObject(window.semaKey, 0); // key down
- WaitForSingleObject(window.semaKey, 0); // key up
- WaitForSingleObject(window.semaKey, INFINITE);
- ret = true;
- }
- else {
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
- }
- // printf("key: %ud\n", window.key);
- sprintf(string, "%s", window.lpString);
-
- return ret;
-}
-/*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true if a pointer motion event was received, false otherwise.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayWin32::getPointerMotionEvent (vpImagePoint &ip)
-{
- //wait if the window is not initialized
- waitForInit();
-
- bool ret = false;
-
- ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaMove, 0));
- if (ret)
- {
- double u, v;
- std::cout << "toto";
- //tells the window there has been a getclick demand
- //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
-
- u = window.coordX;
- v = window.coordY;
- ip.set_u( u );
- ip.set_v( v );
- }
-
- return ret;
-}
-
-/*!
- Get the coordinates of the mouse pointer.
-
- \param ip [out] : The coordinates of the mouse pointer.
-
- \return true.
-
- \exception vpDisplayException::notInitializedError : If the display
- was not initialized.
-*/
-bool
-vpDisplayWin32::getPointerPosition (vpImagePoint &ip)
-{
- //wait if the window is not initialized
- waitForInit();
-
- bool ret = true ;
- double u, v;
- //tells the window there has been a getclick demand
- //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
-
- u = window.coordX;
- v = window.coordY;
- ip.set_u( u );
- ip.set_v( v );
-
- return ret;
-}
-
-/*!
- Changes the window's position.
-
- \param winx, winy : Position of the upper-left window's border in the screen.
-
-*/
-void vpDisplayWin32::setWindowPosition(int winx, int winy)
-{
- //wait if the window is not initialized
- waitForInit();
-
- //cahange the window position only
- SetWindowPos(window.hWnd,HWND_TOP, winx, winy, 0, 0,
- SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER |SWP_NOSIZE);
-
-}
-
-
-/*!
- Changes the window's titlebar text
-
- \param windowtitle : Window title.
-*/
-void vpDisplayWin32::setTitle(const char *windowtitle)
-{
- //wait if the window is not initialized
- waitForInit();
- SetWindowText(window.hWnd, windowtitle);
-}
-
-
-/*!
- \brief Set the font used to display text.
- \param fontname : Name of the font.
- */
-
-void vpDisplayWin32::setFont(const char * /* fontname */)
-{
- vpERROR_TRACE("Not yet implemented" ) ;
-}
-
-
-/*!
- \brief flush the Win32 buffer
- It's necessary to use this function to see the results of any drawing
-
-*/
-void vpDisplayWin32::flushDisplay()
-{
- //waits if the window is not initialized
- waitForInit();
-
- //sends a message to the window
- PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
-}
-
-/*!
- \brief flush the Win32 buffer
- It's necessary to use this function to see the results of any drawing
-
-*/
-void vpDisplayWin32::flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height)
-{
- //waits if the window is not initialized
- waitForInit();
-
- /*
- Under windows, flushing an ROI takes more time than
- flushing the whole image.
- Therefore, we update the maximum area even when asked to update a region.
- */
-#ifdef FLUSH_ROI
- typedef struct _half_rect_t{
- unsigned short left_top;
- unsigned short right_bottom;
- } half_rect_t;
-
- half_rect_t hr1;
- half_rect_t hr2;
-
- hr1.left_top = (unsigned short)iP.get_u();
- hr1.right_bottom = (unsigned short)(iP.get_u()+width-1);
-
- hr2.left_top = (unsigned short)iP.get_v();
- hr2.right_bottom = (unsigned short)(iP.get_v()+height-1);
-
- //sends a message to the window
-# if 1 // new version FS
- WPARAM wp = (WPARAM)(hr1.left_top << sizeof(unsigned short)) + hr1.right_bottom;
- LPARAM lp = (hr2.left_top << sizeof(unsigned short)) + hr2.right_bottom;
-# else // produce warnings with MinGW
- WPARAM wp=*((WPARAM*)(&hr1));
- LPARAM lp=*((WPARAM*)(&hr2));
-# endif
- PostMessage(window.getHWnd(), vpWM_DISPLAY_ROI, wp,lp);
-#else
- PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
-#endif
-}
-
-
-/*!
- Display a point at the image point \e ip location.
- \param ip : Point location.
- \param color : Point color.
-*/
-void vpDisplayWin32::displayPoint(const vpImagePoint &ip,
- const vpColor &color )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->setPixel(ip, color);
-}
-
-/*!
- Display a line from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayWin32::displayLine( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawLine(ip1, ip2, color, thickness);
-}
-
-
-/*!
- Display a dashed line from image point \e ip1 to image point \e ip2.
-
- \warning This line is a dashed line only if the thickness is equal to 1.
-
- \param ip1,ip2 : Initial and final image points.
- \param color : Line color.
- \param thickness : Line thickness.
-*/
-void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawLine(ip1,ip2,color,thickness,PS_DASHDOT);
-}
-
-/*!
- Display a rectangle with \e topLeft as the top-left corner and \e
- width and \e height the rectangle size.
-
- \param topLeft : Top-left corner of the rectangle.
- \param width,height : Rectangle size.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
- \warning The thickness can not be set if the display uses the d3d library.
-*/
-void vpDisplayWin32::displayRectangle( const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
-}
-
-
-/*!
- Display a rectangle.
-
- \param topLeft : Top-left corner of the rectangle.
- \param bottomRight : Bottom-right corner of the rectangle.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
- \warning The thickness can not be set if the display uses the d3d library.
-*/
-void vpDisplayWin32::displayRectangle( const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- unsigned int width = static_cast<unsigned int>( bottomRight.get_j() - topLeft.get_j() );
- unsigned int height = static_cast<unsigned int>(bottomRight.get_i() - topLeft.get_i() );
- window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
-}
-
-/*!
- Display a rectangle.
-
- \param rectangle : Rectangle characteristics.
- \param color : Rectangle color.
- \param fill : When set to true fill the rectangle.
- \param thickness : Thickness of the four lines used to display the
- rectangle.
-
- \warning The thickness can not be set if the display uses the d3d library.
-*/
-void vpDisplayWin32::displayRectangle( const vpRect &rectangle,
- const vpColor &color, bool fill,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- vpImagePoint topLeft;
- topLeft.set_i(rectangle.getTop());
- topLeft.set_j(rectangle.getLeft());
- window.renderer->drawRect(topLeft,
- static_cast<unsigned int>( rectangle.getWidth() ),
- static_cast<unsigned int>( rectangle.getHeight() ),
- color, fill, thickness);
-}
-
-
-/*!
- Display a circle.
- \param center : Circle center position.
- \param radius : Circle radius.
- \param color : Circle color.
- \param fill : When set to true fill the circle.
- \param thickness : Thickness of the circle. This parameter is only useful
- when \e fill is set to false.
-*/
-void vpDisplayWin32::displayCircle(const vpImagePoint ¢er,
- unsigned int radius,
- const vpColor &color,
- bool fill,
- unsigned int thickness )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawCircle(center,radius,color,fill,thickness);
-}
-
-/*!
- Displays a string.
- \param ip : its top left point's coordinates
- \param text : The string to display
- \param color : The text's color
-*/
-void vpDisplayWin32::displayCharString(const vpImagePoint &ip,
- const char *text,
- const vpColor &color )
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawText(ip,text,color);
-}
-
-/*!
- Display a cross at the image point \e ip location.
- \param ip : Cross location.
- \param size : Size (width and height) of the cross.
- \param color : Cross color.
- \param thickness : Thickness of the lines used to display the cross.
-*/
-void vpDisplayWin32::displayCross( const vpImagePoint &ip,
- unsigned int size,
- const vpColor &color,
- unsigned int thickness)
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawCross(ip, size, color, thickness);
-}
-
-
-/*!
- Display an arrow from image point \e ip1 to image point \e ip2.
- \param ip1,ip2 : Initial and final image point.
- \param color : Arrow color.
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpDisplayWin32::displayArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w,unsigned int h,
- unsigned int thickness)
-
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->drawArrow(ip1, ip2, color, w, h, thickness);
-}
-
-
-/*!
- Clears the display.
- \param color : the color to fill the display with
-*/
-void vpDisplayWin32::clearDisplay(const vpColor &color){
- //wait if the window is not initialized
- waitForInit();
- window.renderer->clear(color);
-}
-
-
-/*!
- Closes the display.
- Destroys the window.
-*/
-void vpDisplayWin32::closeDisplay()
-{
- if (displayHasBeenInitialized) {
- waitForInit();
- PostMessage(window.getHWnd(), vpWM_CLOSEDISPLAY, 0,0);
- //if the destructor is called for a reason different than a
- //problem in the thread creation
- if (iStatus) {
- //waits for the thread to end
- WaitForSingleObject(hThread, INFINITE);
- CloseHandle(hThread);
- }
- displayHasBeenInitialized = false ;
- window.initialized = false ;
- }
-}
-
-/*!
- Gets the displayed image (if overlay, if any).
- \param I : Image to fill.
-*/
-void vpDisplayWin32::getImage(vpImage<vpRGBa> &I)
-{
- //wait if the window is not initialized
- waitForInit();
- window.renderer->getImage(I);
-}
-
-#endif
diff --git a/src/device/display/windows/vpDisplayWin32.h b/src/device/display/windows/vpDisplayWin32.h
deleted file mode 100755
index 3e6e680..0000000
--- a/src/device/display/windows/vpDisplayWin32.h
+++ /dev/null
@@ -1,225 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDisplayWin32.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display base class
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-
-#ifndef vpDisplayWin32_hh
-#define vpDisplayWin32_hh
-
-#include <string>
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <windows.h>
-#include <visp/vpWin32Window.h>
-#include <visp/vpWin32Renderer.h>
-#include <visp/vpRect.h>
-#include <visp/vpImagePoint.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-/*!
- Used to pass parameters to the window's thread.
-*/
-struct threadParam
-{
- //! Pointer to the display associated with the window.
- vpDisplayWin32 * vpDisp;
-
- //! X position of the window.
- int x;
-
- //! Y position of the window.
- int y;
-
- //! Width of the window's client area.
- unsigned int w;
-
- //! Height of the window's client area.
- unsigned int h;
-
- //! Title of the window.
- std::string title;
-};
-#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-
-/*!
- \class vpDisplayWin32
-
- \brief Base abstract class for Windows 32 displays.
- Implements the window creation in a separate thread
- and the associated event handling functions for
- Windows 32 displays.
- Uses calls to a renderer to do some display.
- (i.e. all display methods are implemented in the renderer)
-
- \author Bruno Renier
-*/
-class VISP_EXPORT vpDisplayWin32 : public vpDisplay
-{
- protected:
- //! Maximum delay for window initialization
- static const int MAX_INIT_DELAY;
-
- //! Handle of the window's thread.
- HANDLE hThread;
-
- //! Id of the window's thread.
- DWORD threadId;
-
- //! Initialization status.
- bool iStatus;
-
- //! The window.
- vpWin32Window window;
-
- //!
- RECT roi;
-
- //! Function used to launch the window in a thread.
- friend void vpCreateWindow(threadParam * param);
-
- public:
-
- vpDisplayWin32(vpWin32Renderer * rend = NULL);
-
- vpDisplayWin32(vpImage<vpRGBa> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL);
-
- vpDisplayWin32(vpImage<unsigned char> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL);
-
- virtual ~vpDisplayWin32();
-
- void setTitle(const char *windowtitle);
- void setFont( const char *fontname );
- void setWindowPosition(int winx, int winy);
- void clearDisplay(const vpColor &color=vpColor::white);
- void closeDisplay();
- void displayImage(const vpImage<vpRGBa> &I);
- void displayImage(const vpImage<unsigned char> &I);
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void flushDisplay();
- void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void init(vpImage<unsigned char> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- void init(vpImage<vpRGBa> &I,
- int winx=-1, int winy=-1,
- const char *title=NULL) ;
-
- void init(unsigned int width, unsigned int height,
- int winx=-1, int winy=-1 ,
- const char *title=NULL) ;
-
- void getImage(vpImage<vpRGBa> &I);
-
- protected:
-
- void displayArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color=vpColor::white,
- unsigned int w=4,unsigned int h=2,
- unsigned int thickness=1) ;
-
- void displayCharString(const vpImagePoint &ip, const char *text,
- const vpColor &color=vpColor::green) ;
-
- void displayCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color,
- bool fill = false,
- unsigned int thickness=1);
-
- void displayCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
-
- void displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1);
-
-
- void displayPoint(const vpImagePoint &ip, const vpColor &color);
-
- void displayRectangle(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1);
- void displayRectangle(const vpImagePoint &topLeft,
- const vpImagePoint &bottomRight,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- unsigned int thickness=1) ;
-
- bool getClick( bool blocking=true);
-
- bool getClick(vpImagePoint &ip, bool blocking=true);
-
- bool getClick(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking=true);
-
- bool getClickUp(vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking=true);
- bool getKeyboardEvent( bool blocking=true);
- bool getKeyboardEvent(char *string, bool blocking);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
-
- void waitForInit();
-
-};
-#endif
-#endif
-
diff --git a/src/device/display/windows/vpGDIRenderer.cpp b/src/device/display/windows/vpGDIRenderer.cpp
deleted file mode 100755
index e67156d..0000000
--- a/src/device/display/windows/vpGDIRenderer.cpp
+++ /dev/null
@@ -1,1163 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpGDIRenderer.cpp 4733 2014-05-19 20:44:05Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * GDI renderer for windows 32 display
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#define GDI_ROBUST
-#if ( defined(VISP_HAVE_GDI) )
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpGDIRenderer.h>
-
-/*!
- Constructor.
-*/
-vpGDIRenderer::vpGDIRenderer()
-{
- //if the screen depth is not 32bpp, throw an exception
- if( GetDeviceCaps(GetDC(NULL),BITSPIXEL) != 32 )
- throw vpDisplayException(vpDisplayException::depthNotSupportedError,
- "Only works in 32bits mode!");
-
- InitializeCriticalSection(&CriticalSection);
-
- //initialize GDI the palette
- vpColor pcolor; // Predefined colors
-
- pcolor = vpColor::black;
- colors[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightBlue;
- colors[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::blue;
- colors[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkBlue;
- colors[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::cyan;
- colors[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGreen;
- colors[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::green;
- colors[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGreen;
- colors[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightRed;
- colors[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::red;
- colors[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkRed;
- colors[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::white;
- colors[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::lightGray;
- colors[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::gray;
- colors[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::darkGray;
- colors[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::yellow;
- colors[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::orange;
- colors[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
- pcolor = vpColor::purple;
- colors[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
-
- nbCols = 0;
- nbRows = 0;
- bmp = NULL;
-}
-
-/*!
- Destructor.
-*/
-vpGDIRenderer::~vpGDIRenderer()
-{
- //Deletes the critical section object
- DeleteCriticalSection(&CriticalSection);
- //Deletes the bitmap
- DeleteObject(bmp);
- //Deletes the font object
- DeleteObject(hFont);
-}
-
-/*!
- Initialiaze the renderer
- \param hWindow Handle of the window we are working with
- \param width The window's width.
- \param height The window's height.
-*/
-bool vpGDIRenderer::init(HWND hWindow, unsigned int width, unsigned int height)
-{
- timelost = 0.;
- hWnd = hWindow;
- nbCols = width;
- nbRows = height;
-
- //creates the font
- hFont = CreateFont(18, 0, 0, 0, FW_NORMAL, false, false, false,
- DEFAULT_CHARSET, OUT_DEFAULT_PRECIS,
- CLIP_DEFAULT_PRECIS, DEFAULT_QUALITY,
- DEFAULT_PITCH | FF_DONTCARE, NULL);
- return true;
-}
-
-/*!
- Sets the image to display.
- \param I : The rgba image to display.
-*/
-void vpGDIRenderer::setImg(const vpImage<vpRGBa>& I)
-{
- //converts the image into a HBITMAP
- convert(I, bmp);
- //updates the size of the image
- nbCols=I.getWidth();
- nbRows=I.getHeight();
-}
-
-/*!
- Sets the image to display.
- \param I : The rgba image to display.
- \param iP : Top left coordinates of the ROI.
- \param width, height : ROI width and height.
-*/
-void vpGDIRenderer::setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //converts the image into a HBITMAP
- convertROI(I, iP, width, height);
- //updates the size of the image
- nbCols=I.getWidth();
- nbRows=I.getHeight();
-}
-
-/*!
- Sets the image to display.
- \param I : The grayscale image to display.
-*/
-void vpGDIRenderer::setImg(const vpImage<unsigned char>& I)
-{
- //converts the image into a HBITMAP
- convert(I, bmp);
- //updates the size of the image
- nbCols=I.getWidth();
- nbRows=I.getHeight();
-}
-
-/*!
- Sets the image to display.
- \param I : The rgba image to display.
- \param iP : Top left coordinates of the ROI.
- \param width, height : ROI width and height.
-*/
-void vpGDIRenderer::setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
-{
- //converts the image into a HBITMAP
- convertROI(I, iP, width, height);
- //updates the size of the image
- nbCols=I.getWidth();
- nbRows=I.getHeight();
-}
-
-/*!
- Render the current image buffer.
-*/
-bool vpGDIRenderer::render()
-{
- //gets the window's DC
- PAINTSTRUCT ps;
- HDC hDCScreen = BeginPaint(hWnd, &ps);
-
- //create a memory DC
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //selects this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
-
- //blits it on the window's DC
- BitBlt(hDCScreen, 0, 0,
- static_cast<int>( nbCols ),
- static_cast<int>( nbRows ),
- hDCMem, 0, 0, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
- //DeleteDC(hDCMem);
- DeleteObject(hDCMem);
-
- EndPaint(hWnd, &ps);
-
- return true;
-}
-
-/*!
- Converts the image form ViSP in GDI's image format (bgra with padding).
- \param I The image to convert.
- \param hBmp The destination image.
-*/
-void vpGDIRenderer::convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp)
-{
- //get the image's width and height
- unsigned int w = I.getWidth();
- unsigned int h = I.getHeight();
-
- //each line of a HBITMAP needs to be word aligned
- //we need padding if the width is an odd number
- bool needPad = ((w%2) == 0) ? false : true;
- unsigned int newW = w;
-
- //in case of padding, the new width is width+1
- newW = (needPad) ? (w+1) : w;
-
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[newW * h * 4];
-
- //if we need padding (width needs to be a multiple of 2)
- if(needPad)
- {
- unsigned int j = 0;
- for(unsigned int i=0, k=0 ; i<newW * h * 4; i+=4, k++)
- {
- //end of a line = padding = inserts 0s
- if(j==w && needPad)
- {
- imBuffer[i+0] = 0;
- imBuffer[i+1] = 0;
- imBuffer[i+2] = 0;
- imBuffer[i+3] = 0;
- j = 0;
- k --;
- }
- else
- {
- //RGBA -> BGRA
- imBuffer[i+0] = I.bitmap[k].B;
- imBuffer[i+1] = I.bitmap[k].G;
- imBuffer[i+2] = I.bitmap[k].R;
- imBuffer[i+3] = I.bitmap[k].A;
- j++;
- }
- }
- }
- else
- //Simple conversion (no padding)
- {
- for(unsigned int i=0, k=0 ; i<w * h * 4 ; i+=4, k++)
- {
- imBuffer[i+0] = I.bitmap[k].B;
- imBuffer[i+1] = I.bitmap[k].G;
- imBuffer[i+2] = I.bitmap[k].R;
- imBuffer[i+3] = I.bitmap[k].A;
- }
- }
-
- //updates the bitmap's pixel data
- updateBitmap(hBmp,imBuffer, newW, h);
-
- //we don't need this buffer anymore
- delete [] imBuffer;
-}
-
-/*!
- Converts the image form ViSP in GDI's image format (bgra with padding).
- \param I : The image to convert.
- \param iP : Top left coordinates of the ROI.
- \param width, height : ROI width and height.
-*/
-void vpGDIRenderer::convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
-{
- //get the image's width and height
- unsigned int w = width;
- unsigned int h = height;
-
- //each line of a HBITMAP needs to be word aligned
- //we need padding if the width is an odd number
- bool needPad = ((w%2) == 0) ? false : true;
- unsigned int newW = w;
-
- //in case of padding, the new width is width+1
- newW = (needPad) ? (w+1) : w;
-
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[newW * h * 4];
-
- vpRGBa* bitmap = I.bitmap;
- unsigned int iwidth = I.getWidth();
- bitmap = bitmap + (int)(iP.get_i()*iwidth+ iP.get_j());
-
- //if we need padding (width needs to be a multiple of 2)
- if(needPad)
- {
- unsigned int j = 0;
- unsigned int k = 0;
- for(unsigned int i=0 ; i<newW * h * 4; i+=4)
- {
- //end of a line = padding = inserts 0s
- if(j==w && needPad)
- {
- imBuffer[i+0] = 0;
- imBuffer[i+1] = 0;
- imBuffer[i+2] = 0;
- imBuffer[i+3] = 0;
- j = 0;
- }
- else
- {
- //RGBA -> BGRA
- imBuffer[i+0] = (bitmap+k)->B;
- imBuffer[i+1] = (bitmap+k)->G;
- imBuffer[i+2] = (bitmap+k)->R;
- imBuffer[i+3] = (bitmap+k)->A;
- //bitmap++;
- j++;
- k++;
- }
- if (k == newW)
- {
- bitmap = bitmap+iwidth;
- k = 0;
- }
- }
- }
- else
- //Simple conversion (no padding)
- {
- unsigned int k = 0;
- for (unsigned int i=0 ; i < w * h * 4 ; i+=4)
- {
- imBuffer[i+0] = (bitmap+k)->B;
- imBuffer[i+1] = (bitmap+k)->G;
- imBuffer[i+2] = (bitmap+k)->R;
- imBuffer[i+3] = (bitmap+k)->A;
- //bitmap++;
- k++;
- if (k == newW)
- {
- bitmap = bitmap+iwidth;
- k = 0;
- }
- }
- }
-
- //updates the bitmap's pixel data
- updateBitmapROI(imBuffer,iP, newW, h);
-
- //we don't need this buffer anymore
- delete [] imBuffer;
-}
-
-
-/*!
- Converts the image form ViSP in GDI's image format (bgra with padding).
- \param I The image to convert.
- \param hBmp The destination image.
-*/
-void vpGDIRenderer::convert(const vpImage<unsigned char> &I, HBITMAP& hBmp)
-{
- //get the image's width and height
- unsigned int w = I.getWidth();
- unsigned int h = I.getHeight();
-
- //each line of a HBITMAP needs to be word aligned
- //we need padding if the width is an odd number
- bool needPad = ((w%2) == 0) ? false : true;
- unsigned int newW = w;
-
- //in case of padding, the new width is width+1
- newW = (needPad) ? (w+1) : w;
-
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[newW * h * 4];
-
- //if we need padding
- if(needPad)
- {
- unsigned int j = 0;
- for(unsigned int i=0, k=0 ; i<newW * h * 4; i+=4, k++)
- {
- //end of a line = padding = inserts 0s
- if(j==w && needPad)
- {
- imBuffer[i+0] = 0;
- imBuffer[i+1] = 0;
- imBuffer[i+2] = 0;
- imBuffer[i+3] = 0;
- j = 0;
- k --;
- }
- else
- {
- imBuffer[i+0] = I.bitmap[k];
- imBuffer[i+1] = I.bitmap[k];
- imBuffer[i+2] = I.bitmap[k];
- imBuffer[i+3] = I.bitmap[k];
- j++;
- }
- }
- }
- else
- //Simple conversion
- {
- for(unsigned int i=0, k=0 ; i<w * h * 4 ; i+=4, k++)
- {
- imBuffer[i+0] = I.bitmap[k];
- imBuffer[i+1] = I.bitmap[k];
- imBuffer[i+2] = I.bitmap[k];
- imBuffer[i+3] = I.bitmap[k];
- }
- }
-
- //updates the bitmap's pixel data
- updateBitmap(hBmp,imBuffer, newW, h);
-
- //we don't need this buffer anymore
- delete [] imBuffer;
-}
-
-
-/*!
- Converts the image form ViSP in GDI's image format (bgra with padding).
- \param I The image to convert.
- \param iP : Top left coordinates of the ROI.
- \param width, height : ROI width and height.
-*/
-void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
-{
- //get the image's width and height
- unsigned int w = width;
- unsigned int h = height;
-
- //each line of a HBITMAP needs to be word aligned
- //we need padding if the width is an odd number
- bool needPad = ((w%2) == 0) ? false : true;
- unsigned int newW = w;
-
- //in case of padding, the new width is width+1
- newW = (needPad) ? (w+1) : w;
-
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[newW * h * 4];
-
- unsigned char* bitmap = I.bitmap;
- unsigned int iwidth = I.getWidth();
- bitmap = bitmap + (int)(iP.get_i()*iwidth+ iP.get_j());
-
- //if we need padding (width needs to be a multiple of 2)
- if(needPad)
- {
- unsigned int j = 0;
- unsigned int k = 0;
- for(unsigned int i=0 ; i<newW * h * 4; i+=4)
- {
- //end of a line = padding = inserts 0s
- if(j==w && needPad)
- {
- imBuffer[i+0] = 0;
- imBuffer[i+1] = 0;
- imBuffer[i+2] = 0;
- imBuffer[i+3] = 0;
- j = 0;
- }
- else
- {
- //RGBA -> BGRA
- imBuffer[i+0] = *(bitmap+k);
- imBuffer[i+1] = *(bitmap+k);
- imBuffer[i+2] = *(bitmap+k);
- imBuffer[i+3] = *(bitmap+k);
- //bitmap++;
- j++;
- k++;
- }
- if (k == newW)
- {
- bitmap = bitmap+iwidth;
- k = 0;
- }
- }
- }
- else
- //Simple conversion (no padding)
- {
- unsigned int k = 0;
- for (unsigned int i=0 ; i < w * h * 4 ; i+=4)
- {
- imBuffer[i+0] = *(bitmap+k);
- imBuffer[i+1] = *(bitmap+k);
- imBuffer[i+2] = *(bitmap+k);
- imBuffer[i+3] = *(bitmap+k);
- //bitmap++;
- k++;
- if (k == newW)
- {
- bitmap = bitmap+iwidth;
- k = 0;
- }
- }
- }
-
- //updates the bitmap's pixel data
- updateBitmapROI(imBuffer,iP, newW, h);
-
- //we don't need this buffer anymore
- delete [] imBuffer;
-}
-
-/*!
- Updates the bitmap to display.
- Contains a critical section.
- \param hBmp The bitmap to update
- \param imBuffer The new pixel data
- \param w The image's width
- \param h The image's height
-
- \return the operation succefulness
-*/
-bool vpGDIRenderer::updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
- unsigned int w, unsigned int h)
-{
- //the bitmap may only be accessed by one thread at the same time
- //that's why we enter critical section
- EnterCriticalSection(&CriticalSection);
-
- //if the existing bitmap object is of the right size
- if( (nbCols==w) && (nbRows==h) )
- {
- //just replace the content
- SetBitmapBits(hBmp, w * h * 4, imBuffer);
- }
- else
- {
- if(hBmp != NULL)
- {
- //delete the old BITMAP
- DeleteObject(hBmp);
- }
- //create a new BITMAP from this buffer
- if( (hBmp = CreateBitmap(static_cast<int>(w),static_cast<int>(h),
- 1,32,(void*)imBuffer)) == NULL)
- return false;
- }
-
- LeaveCriticalSection(&CriticalSection);
- return true;
-}
-
-
-/*!
- Updates the bitmap to display.
- Contains a critical section.
- \param imBuffer The new pixel data
- \param iP The topleft corner of the roi
- \param w The roi's width
- \param h The roi's height
-
- \return the operation succefulness
-*/
-bool vpGDIRenderer::updateBitmapROI(unsigned char * imBuffer, const vpImagePoint &iP,
- unsigned int w, unsigned int h)
-{
- int w_ = static_cast<int>(w);
- int h_ = static_cast<int>(h);
- HBITMAP htmp = CreateBitmap(w_,h_,1,32,(void*)imBuffer);
-
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
- HDC hDCMem2 = CreateCompatibleDC(hDCScreen);
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
- SelectObject(hDCMem2, htmp);
-
- BitBlt(hDCMem,(int)iP.get_u(),(int)iP.get_v(),w_,h_, hDCMem2, 0, 0,SRCCOPY);
- LeaveCriticalSection(&CriticalSection);
-
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- DeleteObject(htmp);
-
- return true;
-}
-
-/*!
- Sets a pixel to color at position (j,i).
-
- \param ip : The pixel coordinates.
- \param color : the color of the point.
-*/
-void vpGDIRenderer::setPixel(const vpImagePoint &iP,
- const vpColor &color)
-{
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- if (color.id < vpColor::id_unknown)
- SetPixel(hDCMem, vpMath::round(iP.get_u()), vpMath::round(iP.get_v()),
- colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- SetPixel(hDCMem, vpMath::round(iP.get_u()), vpMath::round(iP.get_v()),
- gdicolor);
- }
- //display the result (flush)
- // BitBlt(hDCScreen, x, y, 1, 1, hDCMem, x, y, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-}
-
-/*!
- Draws a line.
- \param ip1,ip2 : Initial and final image point.
- \param color the line's color
- \param thickness : Thickness of the line.
- \param style style of the line
-*/
-void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness, int style)
-{
- HDC hDCScreen= NULL, hDCMem = NULL;
- HPEN hPen = NULL;
-#ifdef GDI_ROBUST
- double start = vpTime::measureTimeMs();
- while(vpTime::measureTimeMs()-start<1000){
- hDCScreen = GetDC(hWnd);
- if(!hDCScreen) continue;
- hDCMem = CreateCompatibleDC(hDCScreen);
- if(!hDCMem){
- ReleaseDC(hWnd, hDCScreen);
- continue;
- }
-
- //create the pen
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(style, static_cast<int>(thickness), colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
- }
- if(!hPen){
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- continue;
- }
- if(!SetBkMode(hDCMem, TRANSPARENT)){
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- continue;
- }
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
-
- if(!SelectObject(hDCMem, bmp)){
- LeaveCriticalSection(&CriticalSection);
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- continue;
- }
-
- //select the pen
- if(!SelectObject(hDCMem, hPen)){
- LeaveCriticalSection(&CriticalSection);
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- continue;
- }
- break;
- }
- timelost+=(vpTime::measureTimeMs()-start);
-#else
- //get the window's DC
- hDCScreen = GetDC(hWnd);
- hDCMem = CreateCompatibleDC(hDCScreen);
-
- //create the pen
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(style, static_cast<int>(thickness), colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
- }
- SetBkMode(hDCMem, TRANSPARENT);
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //select the pen
- SelectObject(hDCMem, hPen);
-#endif
- //move to the starting point
- MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
- //Draw the line
- LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
-
- //computes the coordinates of the rectangle to blit
-// int x = (j2 >= j1) ? j1 : j2;
-// int y = (i2 >= i1) ? i1 : i2;
-// int w = (j2 >= j1) ? j2-j1 : j1-j2;
-// int h = (i2 >= i1) ? i2-i1 : i1-i2;
-
- //display the result (flush)
- // BitBlt(hDCScreen, x, y, w, h, hDCMem, x, y, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-
-}
-
-/*!
- Draws a rectangle.
- \param topLeft its top left point's coordinates
- \param width width of the rectangle
- \param height height of the rectangle
- \param color The rectangle's color
- \param fill When set to true fill the rectangle.
- \param thickness : Line thickness
-*/
-void vpGDIRenderer::drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
-{
- if (thickness == 0) thickness = 1;
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //create the pen
- HPEN hPen;
- COLORREF gdicolor = RGB(0,0,0);
-
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
- else {
- gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
- }
-
- //create an hollow or solid brush (depends on boolean fill)
- LOGBRUSH lBrush;
- if(fill) {
- lBrush.lbStyle = BS_SOLID;
- if (color.id < vpColor::id_unknown)
- lBrush.lbColor = colors[color.id];
- else {
- lBrush.lbColor = gdicolor;
- }
- }
- else lBrush.lbStyle = BS_HOLLOW;
- HBRUSH hbrush = CreateBrushIndirect(&lBrush);
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //select the brush
- SelectObject(hDCMem, hbrush);
- //select the pen
- SelectObject(hDCMem, hPen);
-
- //draw the rectangle
- Rectangle(hDCMem, vpMath::round(topLeft.get_u()), vpMath::round(topLeft.get_v()),
- vpMath::round(topLeft.get_u())+static_cast<int>(width),
- vpMath::round(topLeft.get_v())+static_cast<int>(height));
-
- //display the result (flush)
-// BitBlt(hDCScreen, j, i, width, height, hDCMem, j, i, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteObject(hbrush);
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-
-}
-
-/*!
- Clears the image to a specific color.
- \param color The color used to fill the image.
-*/
-void vpGDIRenderer::clear(const vpColor &color)
-{
- vpImagePoint ip;
- ip.set_i(0);
- ip.set_j(0);
- drawRect(ip, nbCols, nbRows, color, true, 0);
-}
-
-
-/*!
- Draws a circle.
- \param center its center point's coordinates
- \param radius The circle's radius
- \param color The circle's color
- \param fill When set to true fill the circle.
- \param thickness : Line thickness
-*/
-void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
-{
-
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //create the pen
- HPEN hPen;
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
- }
-
- //create an hollow brush
- LOGBRUSH lBrush;
- lBrush.lbStyle = BS_HOLLOW;
- HBRUSH hbrush = CreateBrushIndirect(&lBrush);
-
- //computes bounding rectangle
- int radius_ = static_cast<int>(radius);
- int x1 = vpMath::round(center.get_u())-radius_;
- int y1 = vpMath::round(center.get_v())-radius_;
- int x2 = vpMath::round(center.get_u())+radius_;
- int y2 = vpMath::round(center.get_v())+radius_;
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //select the brush
- SelectObject(hDCMem, hbrush);
- //select the pen
- SelectObject(hDCMem, hPen);
-
- //draw the circle
- if (fill==false)
- Ellipse(hDCMem, x1, y1, x2, y2);
-
- else
- {
- while (x2-x1 > 0)
- {
- x1++;
- x2--;
- y1++;
- y2--;
- Ellipse(hDCMem, x1, y1, x2, y2);
- }
- }
-
- //display the result (flush)
- // BitBlt(hDCScreen, x1, y1, x2-x1, y2-y1, hDCMem, x1, y1, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteObject(hbrush);
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-}
-
-
-/*!
- Draws some text.
- \param ip its top left point's coordinates
- \param text The string to display
- \param color The text's color
-*/
-void vpGDIRenderer::drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color)
-{
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //Select the font
- SelectObject(hDCMem, hFont);
-
- //set the text color
- if (color.id < vpColor::id_unknown)
- SetTextColor(hDCMem, colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- SetTextColor(hDCMem, gdicolor);
- }
-
- //we don't use the bkColor
- SetBkMode(hDCMem, TRANSPARENT);
-
- SIZE size;
- int length = (int) strlen(text);
-
- //get the displayed string dimensions
- GetTextExtentPoint32(hDCMem, text, length, &size);
-
- //displays the string
- TextOut(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v()), text, length);
-
- //display the result (flush)
- // BitBlt(hDCScreen, j, i, size.cx, size.cy, hDCMem, j, i, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-}
-
-
-
-/*!
- Draws a cross.
- \param ip its center point's coordinates
- \param size Size of the cross
- \param color The cross' color
- \param thickness width of the cross
-*/
-void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness)
-{
- /* unsigned */ int half_size = static_cast<int>( size/2 );
-
- // if half_size is equal to zero, nothing is displayed with the code
- // just below. So, if half_size is equal to zero we just draw the
- // pixel.
- if (half_size) {
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //create the pen
- HPEN hPen;
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
- }
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //select the pen
- SelectObject(hDCMem, hPen);
-
- //move to the starting point
- MoveToEx(hDCMem, vpMath::round(ip.get_u())-half_size, vpMath::round(ip.get_v()), NULL);
- //Draw the first line (horizontal)
- LineTo(hDCMem, vpMath::round(ip.get_u())+half_size, vpMath::round(ip.get_v()));
-
- //move to the starting point
- MoveToEx(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v())-half_size, NULL);
- //Draw the second line (vertical)
- LineTo(hDCMem, vpMath::round(ip.get_u()), vpMath::round(ip.get_v())+half_size);
-
- //display the result (flush)
- // BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size,
-// hDCMem, j-(size/2), i-(size/2), SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
- }
- else {
- setPixel(ip, color);
- }
-
-
-}
-
-/*!
- Draws an arrow.
- \param ip1,ip2 : Initial and final image point.
- \param color The arrow's color
- \param w,h : Width and height of the arrow.
- \param thickness : Thickness of the lines used to display the arrow.
-*/
-void vpGDIRenderer::drawArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int w,unsigned int h, unsigned int thickness)
-{
- double a = ip2.get_i() - ip1.get_i() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
-
- //computes the coordinates of the rectangle to blit later
-// unsigned int x = (j2 >= j1) ? j1 : j2;
-// unsigned int y = (i2 >= i1) ? i1 : i2;
-// unsigned int w = (j2 >= j1) ? j2-j1 : j1-j2;
-// unsigned int h = (i2 >= i1) ? i2-i1 : i1-i2;
-
- //get the window's DC
- HDC hDCScreen = GetDC(hWnd);
- HDC hDCMem = CreateCompatibleDC(hDCScreen);
-
- //create the pen
- HPEN hPen;
- if (color.id < vpColor::id_unknown)
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), colors[color.id]);
- else {
- COLORREF gdicolor = RGB(color.R, color.G, color.B);
- hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
- }
-
- //select this bmp in memory
- EnterCriticalSection(&CriticalSection);
- SelectObject(hDCMem, bmp);
-
- //select the pen
- SelectObject(hDCMem, hPen);
-
-
- if ((a==0)&&(b==0))
- {
- // DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
-
- vpImagePoint ip3;
- ip3.set_i( ip2.get_i() - w*a );
- ip3.set_j( ip2.get_j() - w*b );
-
-
- vpImagePoint ip4;
-
- //double t = 0 ;
- //while (t<=_l)
- {
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
- MoveToEx(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()), NULL);
- LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v()));
- }
- // t+=0.1 ;
- }
-
- //t = 0 ;
- //while (t>= -_l)
- {
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
-
- if (lg > 2*vpImagePoint::distance(ip2, ip4) ) {
- MoveToEx(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()), NULL);
- LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v()));
- }
-
- // t-=0.1 ;
- }
- MoveToEx(hDCMem, vpMath::round(ip1.get_u()), vpMath::round(ip1.get_v()), NULL);
- LineTo(hDCMem, vpMath::round(ip2.get_u()), vpMath::round(ip2.get_v()));
-
- }
-
-
- //display the result (flush)
-// BitBlt(hDCScreen, x, y, w, h, hDCMem, x, y, SRCCOPY);
-
- LeaveCriticalSection(&CriticalSection);
-
- DeleteObject(hPen);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDCScreen);
-}
-
-/*!
- Gets the currently displayed image.
- \param I Image returned.
-*/
-void vpGDIRenderer::getImage(vpImage<vpRGBa> &I)
-{
- //size of image buffer : nbCols*nbRows*4
- unsigned int size = nbCols*nbRows*4;
- unsigned char * imBuffer = new unsigned char[size];
-
- //gets the hbitmap's bitmap
- GetBitmapBits(bmp, static_cast<LONG>(size), (void *)imBuffer);
-
- //resize the destination image as needed
- I.resize(nbRows, nbCols);
-
- //copy the content
- for(unsigned int i=0 ; i<size ; i+=4)
- {
- I.bitmap[i>>2].R = imBuffer[i+2];
- I.bitmap[i>>2].G = imBuffer[i+1];
- I.bitmap[i>>2].B = imBuffer[i+0];
- I.bitmap[i>>2].A = 0xFF; //255 = maximum opacity
- }
-
- delete [] imBuffer;
-}
-#endif
-#endif
diff --git a/src/device/display/windows/vpGDIRenderer.h b/src/device/display/windows/vpGDIRenderer.h
deleted file mode 100755
index ca9631d..0000000
--- a/src/device/display/windows/vpGDIRenderer.h
+++ /dev/null
@@ -1,150 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpGDIRenderer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * GDI renderer for windows 32 display
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) )
-#ifndef vpGDIRenderer_HH
-#define vpGDIRenderer_HH
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <windows.h>
-
-#include <visp/vpWin32Renderer.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpDisplayException.h>
-
-#include <visp/vpMath.h>
-
-class VISP_EXPORT vpGDIRenderer : public vpWin32Renderer
-{
- //the handle of the associated window
- HWND hWnd;
-
- //the bitmap object to display
- HBITMAP bmp;
-
- //colors for overlay
- COLORREF colors[vpColor::id_unknown];
-
- //font used to draw text
- HFONT hFont;
-
- //used to ensure that only one thread at a time is accessing bmp
- CRITICAL_SECTION CriticalSection;
-
- public:
- double timelost;
- vpGDIRenderer();
- virtual ~vpGDIRenderer();
-
- bool init(HWND hWnd, unsigned int width, unsigned int height);
-
- bool render();
-
- // gets the image's width.
- unsigned int getImageWidth(){ return nbCols; }
-
- // gets the image's height.
- unsigned int getImageHeight(){ return nbRows; }
-
- void setImg(const vpImage<vpRGBa>& I);
- void setImg(const vpImage<unsigned char>& I);
- void setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
- void setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
-
- void setPixel(const vpImagePoint &iP, const vpColor &color);
-
- void drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness, int style=PS_SOLID);
-
- void drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill=false,
- unsigned int thickness=1);
-
- void clear(const vpColor &color);
-
- void drawCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill=false, unsigned int thickness=1);
-
- void drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color);
-
- void drawCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1);
-
- void drawArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness=1);
-
- void getImage(vpImage<vpRGBa> &I);
-
- private:
-
- //updates the renderer hbitmaps.
- bool updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
- unsigned int w, unsigned int h);
- //updates the renderer hbitmaps.
- bool updateBitmapROI(unsigned char * imBuffer, const vpImagePoint &iP,
- unsigned int w, unsigned int h);
-
-
- //converts a vpImage<vpRGBa> into a HBITMAP .
- void convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp);
-
- //converst a vpImage<unsigned char> into a HBITMAP .
- void convert(const vpImage<unsigned char> &I, HBITMAP& hBmp);
-
- //converts a vpImage<vpRGBa> into a HBITMAP .
- void convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- //converst a vpImage<unsigned char> into a HBITMAP .
- void convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
-};
-#endif
-#endif
-#endif
diff --git a/src/device/display/windows/vpWin32API.cpp b/src/device/display/windows/vpWin32API.cpp
deleted file mode 100644
index 6580fed..0000000
--- a/src/device/display/windows/vpWin32API.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWin32API.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * GDI renderer for windows 32 display
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpWin32API.h>
-#include <visp/vpTime.h>
-#include <iostream>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-DWORD vpProcessErrors(const std::string &api_name){
- LPVOID lpMsgBuf;
- DWORD err = GetLastError();
-
- FormatMessage(
- FORMAT_MESSAGE_ALLOCATE_BUFFER |
- FORMAT_MESSAGE_FROM_SYSTEM |
- FORMAT_MESSAGE_IGNORE_INSERTS,
- NULL,
- err,
- MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
- (LPTSTR) &lpMsgBuf,
- 0, NULL );
- std::cout << "call to " << api_name << " failed with the following error code: " << err << "(" << (LPTSTR)lpMsgBuf << ")" << std::endl;
- return err;
-}
-
-BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd){
- BOOL ret = LineTo(hdc, nXEnd, nYEnd);
- if(ret == 0)
- vpProcessErrors("LineTo");
- return ret;
-}
-
-BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint){
- BOOL ret = MoveToEx(hdc, X, Y, lpPoint);
- if(ret == 0)
- vpProcessErrors("MoveToEx");
- return ret;
-}
-
-BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop){
- BOOL ret = BitBlt(hdcDest,nXDest,nYDest, nWidth, nHeight, hdcSrc, nXSrc, nYSrc, dwRop);
- if(ret == 0)
- vpProcessErrors("BitBlt");
- return ret;
-}
-
-BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase){
- BOOL ret = InvalidateRect(hWnd,lpRect,bErase);
- if(ret == 0)
- vpProcessErrors("InvalidateRect");
- return ret;
-}
-
-void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h){
-
- HGDIOBJ ret = SelectObject(hDCMem,h);
- if(ret==NULL){
- vpProcessErrors("SelectObject");
-
- double ms = vpTime::measureTimeMs();
-
- while(ret==NULL && vpTime::measureTimeMs()-ms<5000){
- DeleteObject(h);
- DeleteDC(hDCMem);
- ReleaseDC(hWnd, hDC);
- }
- }
-}
-
-BOOL vpReleaseSemaphore(HANDLE hSemaphore,LONG IReleaseCount,LPLONG lpPreviousCount){
- BOOL ret = ReleaseSemaphore(hSemaphore,IReleaseCount,lpPreviousCount);
-#ifndef __MINGW32__
- if(ret==0){
- vpProcessErrors("ReleaseSemaphore");
- }
-#endif
- return ret;
-}
-
-void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection){
- EnterCriticalSection(lpCriticalSection);
-}
-void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection){
- LeaveCriticalSection(lpCriticalSection);
-}
-
-COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor){
- COLORREF ret = SetPixel(hdc, X, Y, crColor);
- if(ret == 0)
- vpProcessErrors("SetPixel");
- return ret;
-}
-
-HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, UINT cBitsPerPel, const VOID *lpvBits){
- HBITMAP ret = CreateBitmap(nWidth, nHeight, cPlanes, cBitsPerPel, lpvBits);
- if (ret==NULL)
- vpProcessErrors("CreateBitmap");
-
- return ret;
-}
-
-#endif
diff --git a/src/device/display/windows/vpWin32API.h b/src/device/display/windows/vpWin32API.h
deleted file mode 100644
index 9dfd757..0000000
--- a/src/device/display/windows/vpWin32API.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWin32API.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * GDI renderer for windows 32 display
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#ifndef vpWin32API_HH
-#define vpWin32API_HH
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-
-#include <windows.h>
-#include <string>
-
-DWORD vpProcessErrors(const std::string &api_name);
-void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h);
-void vpPrepareImageWithPen(CRITICAL_SECTION* CriticalSection, HWND hWnd,HBITMAP bmp,COLORREF color,unsigned int thickness, int style, HDC& hDCScreen,HDC& hDCMem,HPEN& hPen);
-void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection);
-void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection);
-BOOL vpReleaseSemaphore(HANDLE hSemaphore,LONG IReleaseCount,LPLONG lpPreviousCount);
-BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd);
-BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint);
-BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop);
-BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase);
-COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor);
-HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, UINT cBitsPerPel, const VOID *lpvBits);
-#endif
-#endif
diff --git a/src/device/display/windows/vpWin32Renderer.h b/src/device/display/windows/vpWin32Renderer.h
deleted file mode 100755
index 5602b09..0000000
--- a/src/device/display/windows/vpWin32Renderer.h
+++ /dev/null
@@ -1,187 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWin32Renderer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 renderer base class
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-
-#ifndef vpWin32Renderer_HH
-#define vpWin32Renderer_HH
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpImage.h>
-#include <visp/vpColor.h>
-#include <windows.h>
-#include <visp/vpDebug.h>
-
-class VISP_EXPORT vpWin32Renderer
-{
-
- protected:
- //the size of the display
- unsigned int nbCols;
- unsigned int nbRows;
-
- public:
- //! Destructor.
- virtual ~vpWin32Renderer() {};
-
- //! Returns the image dimensions.
- unsigned int getImageWidth(){ return nbCols; }
- unsigned int getImageHeight(){ return nbRows; }
-
-
-
- //! Inits the display .
- virtual bool init(HWND hWnd, unsigned int w, unsigned int h) =0;
-
- //! Renders the image.
- virtual bool render() =0;
-
-
- /*!
- Sets the image to display.
- \param im The image to display.
- */
- virtual void setImg(const vpImage<vpRGBa>& im) =0;
- virtual void setImg(const vpImage<unsigned char>& im) =0;
- virtual void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
- virtual void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
-
- /*!
- Sets the pixel at (x,y).
- \param y The y coordinate of the pixel.
- \param x The x coordinate of the pixel.
- \param color The color of the pixel.
- */
- virtual void setPixel(const vpImagePoint &iP, const vpColor &color) =0;
-
- /*!
- Draws a line.
- \param i1 its starting point's first coordinate
- \param j1 its starting point's second coordinate
- \param i2 its ending point's first coordinate
- \param j2 its ending point's second coordinate
- \param e line thickness
- \param col the line's color
- \param style style of the line
- */
- virtual void drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness, int style=PS_SOLID) =0;
-
- /*!
- Draws a rectangle.
- \param i its top left point's first coordinate
- \param j its top left point's second coordinate
- \param width width of the rectangle
- \param height height of the rectangle
- \param col The rectangle's color
- \param fill True if it is a filled rectangle
- \param e line thickness
- */
- virtual void drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill=false,
- unsigned int thickness=1) =0;
-
- /*!
- Clears the image to color c.
- \param c The color used to fill the image.
- */
- virtual void clear(const vpColor &color) =0;
-
- /*!
- Draws a circle.
- \param i its center point's first coordinate
- \param j its center point's second coordinate
- \param r The circle's radius
- \param col The circle's color
- */
- virtual void drawCircle(const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness=1) =0;
-
- /*!
- Draws some text.
- \param i its top left point's first coordinate
- \param j its top left point's second coordinate
- \param s The string to display
- \param col The text's color
- */
- virtual void drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color) =0;
-
- /*!
- Draws a cross.
- \param i its center point's first coordinate
- \param j its center point's second coordinate
- \param size Size of the cross
- \param col The cross' color
- \param e width of the cross
- */
- virtual void drawCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) =0;
-
- /*!
- Draws an arrow.
- \param i1 its starting point's first coordinate
- \param j1 its starting point's second coordinate
- \param i2 its ending point's first coordinate
- \param j2 its ending point's second coordinate
- \param color The line's color
- \param L ...
- \param l ...
- */
- virtual void drawArrow(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness) =0;
-
- /*!
- Gets the currently displayed image.
- \param I Image returned.
- */
- virtual void getImage(vpImage<vpRGBa> &I) =0;
-};
-
-#endif
-#endif
-#endif
diff --git a/src/device/display/windows/vpWin32Window.cpp b/src/device/display/windows/vpWin32Window.cpp
deleted file mode 100755
index 700e088..0000000
--- a/src/device/display/windows/vpWin32Window.cpp
+++ /dev/null
@@ -1,348 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWin32Window.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display's window class
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-#include <iostream>
-#include <visp/vpWin32API.h>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-
-#define MAX_SEM_COUNT 2147483647
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpWin32Window.h>
-
-//Should be already defined ...
-#ifndef GET_X_LPARAM
-# define GET_X_LPARAM(lp) ((int)(short)LOWORD(lp))
-#endif
-
-#ifndef GET_Y_LPARAM
-# define GET_Y_LPARAM(lp) ((int)(short)HIWORD(lp))
-#endif
-
-//declares the window as thread local
-//allows multiple displays
-#ifdef __MINGW32__
-vpWin32Window * window;
-#else
-_declspec(thread) vpWin32Window * window;
-#endif
-
-bool vpWin32Window::registered = false;
-/*!
- The message callback
-*/
-LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
-{
- //the first time this callback is executed, put the window in initialized state
- if (window != NULL)
- {
- if (!window->isInitialized())
- {
- window->initialized = true;
- vpReleaseSemaphore(window->semaInit,1,NULL);
- }
- }
-
- switch (message)
- {
- case vpWM_DISPLAY:
- //redraw the whole window
- InvalidateRect(window->getHWnd(), NULL, TRUE);
- UpdateWindow(window->getHWnd());
- break;
-
- case vpWM_DISPLAY_ROI:
- {
- RECT rect;
- typedef struct _half_rect_t{
- unsigned short left_top;
- unsigned short right_bottom;
- } half_rect_t;
-
- half_rect_t hr1;
- half_rect_t hr2;
-
- hr1 = *((half_rect_t*)(&wParam));
- hr2 = *((half_rect_t*)(&lParam));
-
- rect.left = hr1.left_top;
- rect.right = hr1.right_bottom;
-
- rect.top = hr2.left_top;
- rect.bottom = hr2.right_bottom;
-
- InvalidateRect(window->getHWnd(), &rect, TRUE);
- UpdateWindow(window->getHWnd());
- }
- break;
-
- case WM_LBUTTONDOWN:
- {
- window->clickX = GET_X_LPARAM(lParam);
- window->clickY = GET_Y_LPARAM(lParam);
-
- window->clickButton = vpMouseButton::button1;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
-
- case WM_MBUTTONDOWN:
- {
- window->clickX = GET_X_LPARAM(lParam);
- window->clickY = GET_Y_LPARAM(lParam);
-
- window->clickButton = vpMouseButton::button2;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
-
- case WM_RBUTTONDOWN:
- {
- window->clickX = GET_X_LPARAM(lParam);
- window->clickY = GET_Y_LPARAM(lParam);
-
- window->clickButton = vpMouseButton::button3;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
-
- case WM_LBUTTONUP:
- {
- window->clickXUp = GET_X_LPARAM(lParam);
- window->clickYUp = GET_Y_LPARAM(lParam);
-
- window->clickButtonUp = vpMouseButton::button1;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
-
- case WM_MBUTTONUP:
- {
- window->clickXUp = GET_X_LPARAM(lParam);
- window->clickYUp = GET_Y_LPARAM(lParam);
-
- window->clickButtonUp = vpMouseButton::button2;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
-
- case WM_RBUTTONUP:
- {
- window->clickXUp = GET_X_LPARAM(lParam);
- window->clickYUp = GET_Y_LPARAM(lParam);
-
- window->clickButtonUp = vpMouseButton::button3;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
- case WM_MOUSEMOVE:
- {
- window->coordX = GET_X_LPARAM(lParam);
- window->coordY = GET_Y_LPARAM(lParam);
- vpReleaseSemaphore(window->semaMove,1,NULL);
- }
- break;
-
- case WM_SYSKEYDOWN:
- // case WM_SYSKEYUP:
- case WM_KEYDOWN:
- //case WM_KEYUP:
- {
- GetKeyNameText((LONG)lParam, window->lpString, 10); // 10 is the size of lpString
- //window->key = MapVirtualKey(wParam, MAPVK_VK_TO_CHAR);
- vpReleaseSemaphore(window->semaKey,1,NULL);
- break;
- }
-
- case WM_COMMAND:
-
- break;
-
- //we must prevent the window from erasing the background each time a
- //repaint is needed
- case WM_ERASEBKGND:
- return (LRESULT)1;
-
- case WM_PAINT:
- //render the display
- window->renderer->render();
- break;
-
- case vpWM_CLOSEDISPLAY:
- //cleanup code here, if needed
- //destroys the window
- DestroyWindow(hWnd);
- break;
-
- case WM_DESTROY:
- PostQuitMessage(0);
- break;
- default:
- return DefWindowProc(hWnd, message, wParam, lParam);
- }
- return 0;
-}
-
-/*!
- Constructor.
-*/
-vpWin32Window::vpWin32Window(vpWin32Renderer * rend): initialized(false)
-{
- renderer = rend;
-
- //registered is static member class and is initialized at the beginning of this file (registered = false)
-
- //creates the semaphores
- semaInit = CreateSemaphore(NULL,0,1,NULL);
- semaClick = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaClickUp = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaKey = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaMove = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
-
-}
-
-/*!
- Destructor.
-*/
-vpWin32Window::~vpWin32Window()
-{
- delete renderer;
- CloseHandle(semaInit);
- CloseHandle(semaClick);
- CloseHandle(semaClickUp);
- CloseHandle(semaKey);
- CloseHandle(semaMove);
-}
-
-
-/*!
- A standard window creation procedure.
- Initialize the renderer and associate it with this thread
- \param title String to display in the window's titlebar
- \param posx Initial window X position
- \param posy Initial window Y position
- \param w Initial window's width
- \param h Initial window's height
-
-*/
-void vpWin32Window::initWindow(const char* title, int posx, int posy, unsigned int w, unsigned int h)
-{
- //the window's style
- DWORD style = WS_OVERLAPPEDWINDOW | WS_VISIBLE;
- const char g_szClassName[] = "ViSPWindowClass";
-
- RECT rect;
- rect.left=0;
- rect.right=static_cast<int>(w);
- rect.top=0;
- rect.bottom=static_cast<int>(h);
-
- //now we register the window's class
- WNDCLASSEX wcex;
-
- wcex.cbSize = sizeof(WNDCLASSEX);
-
- wcex.style = CS_HREDRAW | CS_VREDRAW | CS_NOCLOSE ;
- wcex.lpfnWndProc = (WNDPROC)WndProc;
- wcex.cbClsExtra = 0;
- wcex.cbWndExtra = 0;
- wcex.hInstance = hInst;
- wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
- wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
- wcex.hbrBackground =(HBRUSH)(COLOR_WINDOW+1);
- wcex.lpszMenuName = NULL;
- wcex.lpszClassName = g_szClassName;
- wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
-
- RegisterClassEx(&wcex);
-
- AdjustWindowRectEx(&rect, style, false, style);
- // std::cout << "win client size (orig)(w,h): " << rect.left << " " << rect.top << " " << rect.right << " " << rect.bottom << std::endl;
-
- //creates the window
- hWnd = CreateWindowEx(WS_EX_APPWINDOW, g_szClassName, title, style,
- posx, posy, rect.right-rect.left, rect.bottom-rect.top, NULL, NULL, hInst, NULL);
- if (hWnd == NULL)
- {
- DWORD err= GetLastError();
- std::cout << "err CreateWindowEx=" << err << std::endl;
- throw vpDisplayException(vpDisplayException::cannotOpenWindowError,
- "Can't create the window!");
- }
- SetWindowPos(hWnd, NULL, 0, 0, rect.right - rect.left, rect.bottom - rect.top, SWP_NOZORDER | SWP_NOMOVE);
-
- //needed if we want to access it from the callback method (message handler)
- window = this;
-
- //initialize the renderer
- renderer->init(hWnd, w, h);
-
- //displays the window
- ShowWindow(hWnd, SW_SHOWDEFAULT);
- //ShowWindow(hWnd, SW_SHOW);
- UpdateWindow(hWnd);
-
- MSG msg;
-
- //starts the message loop
- while (true)
- {
- BOOL val = GetMessage(&msg, NULL, 0, 0);
- if(val==-1){
- std::cout << "GetMessage error:" << GetLastError() << std::endl;
- break;
- }else if(val==0){
- break;
- }else{
- if (!TranslateAccelerator(msg.hwnd, NULL, &msg))
- {
- TranslateMessage(&msg);
- DispatchMessage(&msg);
- }
- }
- }
-}
-
-#endif
-#endif
diff --git a/src/device/display/windows/vpWin32Window.h b/src/device/display/windows/vpWin32Window.h
deleted file mode 100755
index a3ea37c..0000000
--- a/src/device/display/windows/vpWin32Window.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWin32Window.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Windows 32 display's window class
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-#ifndef vpWin32Window_HH
-#define vpWin32Window_HH
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <windows.h>
-#include <visp/vpWin32Renderer.h>
-#include <visp/vpGDIRenderer.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayException.h>
-
-
-//ViSP-defined messages for window's callback function
-#define vpWM_GETCLICK WM_USER+1
-#define vpWM_DISPLAY WM_USER+2
-#define vpWM_GETCLICKUP WM_USER+3
-#define vpWM_CLOSEDISPLAY WM_USER+4
-#define vpWM_GETPOINTERMOTIONEVENT WM_USER+5
-#define vpWM_DISPLAY_ROI WM_USER+6
-
-//No specific mouse button query
-#define vpNO_BUTTON_QUERY -1
-
-class vpDisplayWin32;
-
-class VISP_EXPORT vpWin32Window
-{
-private :
-
- HINSTANCE hInst;
-
- //! Window's handle
- HWND hWnd;
-
- //! Window is initialized
- bool initialized;
- //! Handle for the initialization semaphore
- HANDLE semaInit;
-
- //! Handle for the getClick semaphore
- HANDLE semaClick;
- //! Handle for the getClickUp semaphore
- HANDLE semaClickUp;
- //! Handle for the keyborad event semaphore
- HANDLE semaKey;
- //! Handle for the mouse event semaphore
- HANDLE semaMove;
-
- //! X coordinate of the click
- int clickX;
- int clickXUp;
- //! Y coordinate of the click
- int clickY;
- int clickYUp;
- //! X coordinate of the mouse
- int coordX;
- //! Y coordinate of the mouse
- int coordY;
- // Keyboard key
- char lpString[10];
- //! Button used for the click
- vpMouseButton::vpMouseButtonType clickButton;
- vpMouseButton::vpMouseButtonType clickButtonUp;
-
- //! True if the window's class has already been registered
- static bool registered;
-
- //! The renderer used by the window
- vpWin32Renderer * renderer;
-
-
- public:
-
- vpWin32Window(vpWin32Renderer * rend = NULL);
- virtual ~vpWin32Window();
-
- //! Returns the displayed image's width
- unsigned int getImageWidth(){ return renderer->getImageWidth(); }
- //! Returns the displayed image's height
- unsigned int getImageHeight(){ return renderer->getImageHeight(); }
- //! Returns the window's handle
- HWND getHWnd(){ return hWnd;}
-
- //! Returns true if the window is initialized
- bool isInitialized(){ return initialized; }
-
- //! Initialize the window
- void initWindow(const char* title, int posx, int posy, unsigned int w, unsigned int h);
-
- // Friend classes
- friend class vpDisplayWin32;
- friend class vpDisplayD3D;
- friend class vpDisplayGDI;
-
- //! The message loop
- friend LRESULT CALLBACK WndProc(HWND hWnd, UINT message,
- WPARAM wParam, LPARAM lParam);
-};
-
-#endif
-#endif
-#endif
diff --git a/src/device/framegrabber/1394/vp1394CMUGrabber.cpp b/src/device/framegrabber/1394/vp1394CMUGrabber.cpp
deleted file mode 100644
index 0e7dac0..0000000
--- a/src/device/framegrabber/1394/vp1394CMUGrabber.cpp
+++ /dev/null
@@ -1,734 +0,0 @@
-/****************************************************************************
- *
- * $Id: vp1394CMUGrabber.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
- *
- * Authors:
- * Lucas Lopes Lemos FEMTO-ST, AS2M departement, Besancon
- * Guillaume Laurent FEMTO-ST, AS2M departement, Besancon
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_CMU1394
-
-#include <iostream>
-
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vp1394CMUGrabber.h>
-
-/*!
- Basic constructor.
- */
-vp1394CMUGrabber::vp1394CMUGrabber()
-{
- // public members
- init = false;
-
- // protected members
- width = height = -1;
-
- // private members
- camera = new C1394Camera;
- index = 0; // If a camera was not selected the first one (index = 0) will be used
- _format = _mode = _fps = -1;
- _modeauto=true;
-}
-
-/*!
- Destructor.
- */
-vp1394CMUGrabber::~vp1394CMUGrabber( )
-{
- close();
- // delete camera instance
- if (camera) {
- delete camera;
- camera = NULL;
- }
-}
-
-/*!
- Select the camera on the bus from its index. The first camera found on the bus has index 0.
- \param cam_id : Camera index.
-*/
-void
-vp1394CMUGrabber::selectCamera(int cam_id)
-{
- int camerror;
-
- index = cam_id ;
-
- camerror = camera->SelectCamera(index);
- if ( camerror!= CAM_SUCCESS)
- {
- switch (camerror)
- {
- case CAM_ERROR_PARAM_OUT_OF_RANGE:
- vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is not present") );
- break;
- case CAM_ERROR_BUSY:
- vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
- break;
- case CAM_ERROR:
- vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
- break;
- }
- close();
- }
-} // end camera select
-
-/*!
- Init the selected camera.
- */
-void
-vp1394CMUGrabber::initCamera()
-{
- if (init == false)
- {
- int camerror;
-
- if (camera->CheckLink() != CAM_SUCCESS)
- {
- vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no detected camera") );
- }
-
- camerror = camera->InitCamera();
- if ( camerror != CAM_SUCCESS )
- {
- switch (camerror)
- {
- case CAM_ERROR_NOT_INITIALIZED:
- vpERROR_TRACE("vp1394CMUGrabber error: No camera selected",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no selected camera") );
- break;
- case CAM_ERROR_BUSY:
- vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
- break;
- case CAM_ERROR:
- vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
- break;
- }
- close();
- }
-
- if (camera->Has1394b())
- camera->Set1394b(TRUE);
-
- // Get the current settings
- _format = camera->GetVideoFormat();
- _mode = camera->GetVideoMode();
- _color = getVideoColorCoding();
- //std::cout << "format: " << _format << std::endl;
- //std::cout << "mode: " << _mode << std::endl;
- //std::cout << "color coding: " << _color << std::endl;
-
- // Set trigger off
- camera->GetCameraControlTrigger()->SetOnOff(false);
-
- unsigned long w, h;
- camera->GetVideoFrameDimensions(&w, &h);
- this->width = w;
- this->height = h;
-
- // start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
- }
-
- init = true;
- }
-
-} // end camera init
-
-
-/*!
- Initialization of the grabber using a greyscale image.
- \param I : gray level image.
- */
-void
-vp1394CMUGrabber::open(vpImage<unsigned char> &I)
-{
- initCamera();
- I.resize(this->height, this->width);
-}
-
-/*!
- Initialization of the grabber using a color image.
- \param I : color image.
- */
-void
-vp1394CMUGrabber::open(vpImage<vpRGBa> &I)
-{
- initCamera();
- I.resize(this->height, this->width);
-}
-
-/*!
- Grabs a grayscale image from the selected camera. If the camera color
- coding differs from vp1394CMUGrabber::MONO8, the acquired image is
- converted in a gray level image to match the requested format.
-
- \param I : Acquired gray level image.
- */
-void
-vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
-{
- // get image data
- unsigned long length;
- unsigned char *rawdata = NULL ;
- int dropped;
- unsigned int size;
-
- open(I);
-
- camera->AcquireImageEx(TRUE,&dropped);
- rawdata = camera->GetRawData(&length);
-
- size = I.getSize();
- switch(_color) {
- case vp1394CMUGrabber::MONO8:
- memcpy(I.bitmap, (unsigned char *) rawdata, size);
- break;
- case vp1394CMUGrabber::MONO16:
- vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV411:
- vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV422:
- vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV444:
- vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::RGB8:
- vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
- break;
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
- };
-
- //unsigned short depth = 0;
- //camera->GetVideoDataDepth(&depth);
- //std::cout << "depth: " << depth << " computed: " << (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
-
-
- //memcpy(I.bitmap,rawdata,length);
-
-}
-
-/*!
- Grabs a color image from the selected camera. Since the cameras
- are not able to grab RGBa color coding format, the acquired image is
- converted in a RGBa to match the requested format. This transformation
- could be time consuming.
-
- \param I : Acquired color image in RGBa format.
- */
-void
-vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I)
-{
- // get image data
- unsigned long length;
- unsigned char *rawdata = NULL;
- int dropped;
- unsigned int size;
-
- open(I);
-
- camera->AcquireImageEx(TRUE,&dropped);
- rawdata = camera->GetRawData(&length);
- size = I.getWidth() * I.getHeight();
-
- switch (_color) {
- case vp1394CMUGrabber::MONO8:
- vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::MONO16:
- vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV411:
- vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV422:
- vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV444:
- vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::RGB8:
- size = length / 3;
- vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
- };
-}
-
-/*!
- Stop the acquisition of images and free the camera.
- */
-void
-vp1394CMUGrabber::close()
-{
- // stop acquisition
- if (camera->IsAcquiring()) {
- // stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
- }
- }
-
- init = false;
-}
-
-/*!
- Set the gain and the shutter values.
- \sa setGain(), setShutter()
- */
-void
-vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
-{
- setShutter(shutter);
- setGain(gain);
-}
-
-/*!
- Get the number of connected cameras.
- */
-int
-vp1394CMUGrabber::getNumberOfConnectedCameras() const
-{
- int n_cam = camera->RefreshCameraList();
-
- return n_cam;
-}
-
-/*!
- Get the gain min and max values.
-
- \sa setAutoGain(), setGain()
- */
-void vp1394CMUGrabber::getGainMinMax(unsigned short &min, unsigned short &max)
-{
- initCamera();
-
- C1394CameraControl *Control;
- Control = camera->GetCameraControl(FEATURE_GAIN);
- Control->Inquire();
- Control->GetRange(&min, &max);
-}
-/*!
- Enable auto gain.
-
- \sa setGain()
- */
-void vp1394CMUGrabber::setAutoGain()
-{
- initCamera();
- camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true);
-}
-/*!
- Disable auto gain and set the gain to the requested value.
-
- \sa setAutoGain()
- */
-void vp1394CMUGrabber::setGain(unsigned short gain)
-{
- initCamera();
- _gain = gain;
-
- unsigned short min,max;
- C1394CameraControl *Control;
-
- Control = camera->GetCameraControl(FEATURE_GAIN);
- Control->Inquire();
- Control->GetRange(&min,&max);
-
- if (_gain < min)
- {
- _gain = min;
- std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be less than " << _gain << std::endl;
- }
- else if (_gain > max)
- {
- _gain = max;
- std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be greater than " << _gain << std::endl;
- }
-
- Control->SetAutoMode(false);
- if(Control->SetValue(_gain) != CAM_SUCCESS)
- {
- std::cout << "vp1394CMUGrabber warning: Can't set gain register value of IEEE 1394 camera number " << index << std::endl;
- }
-}
-
-/*!
- Get the shutter min and max values.
-
- \sa setAutoShutter(), setShutter()
- */
-void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max)
-{
- initCamera();
-
- C1394CameraControl *Control;
- Control = camera->GetCameraControl(FEATURE_SHUTTER);
- Control->Inquire();
- Control->GetRange(&min, &max);
-}
-
-/*!
- Enable auto shutter.
-
- \sa setShutter()
- */
-void vp1394CMUGrabber::setAutoShutter()
-{
- initCamera();
- camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true);
-}
-/*!
- Disable auto shutter and set the shutter to the requested value.
-
- \sa setAutoShutter()
- */
-void vp1394CMUGrabber::setShutter(unsigned short shutter)
-{
- initCamera();
-
- _shutter = shutter;
-
- unsigned short min,max;
- C1394CameraControl *Control;
-
- Control = camera->GetCameraControl(FEATURE_SHUTTER);
- Control->Inquire();
- Control->GetRange(&min,&max);
-
- if (_shutter < min)
- {
- _shutter = min;
- std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be less than " << _shutter << std::endl;
- }
- else if (_shutter > max)
- {
- _shutter = max;
- std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be greater than " << _shutter << std::endl;
- }
- Control->SetAutoMode(false);
- if(Control->SetValue(_shutter) != CAM_SUCCESS)
- {
- std::cout << "vp1394CMUGrabber warning: Can't set exposure time register value of IEEE 1394 camera number " << index << std::endl;
- }
-}
-
-/*!
- Display information about the camera on the standard output.
- */
-void
-vp1394CMUGrabber::displayCameraDescription(int cam_id)
-{
- char buf[512];
-
- if( camera->GetNumberCameras() > cam_id )
- {
- camera->GetNodeDescription(cam_id,buf,512);
- std::cout << "Camera " << cam_id << ": " << buf << std::endl ;
-
- }
- else {
- std::cout << "Camera " << cam_id << ": camera not found" << std::endl ;
- }
-}
-
-
-/*!
- Display camera model on the standard output. Call it after open the grabber.
- */
-void
-vp1394CMUGrabber::displayCameraModel()
-{
- char vendor[256] , model[256] , buf[256];
- LARGE_INTEGER ID;
-
- camera->GetCameraName(model,sizeof(model));
- camera->GetCameraVendor(vendor,sizeof(vendor));
- camera->GetCameraUniqueID(&ID);
-
- std::cout << "Vendor: " << vendor << std::endl;
- std::cout << "Model: " << model << std::endl;
-
- sprintf(buf,"%08X%08X",ID.HighPart,ID.LowPart);
- std::cout << "UniqueID: " << buf << std::endl;
-
-}
-
-
-/*!
- Set the camera format and video mode.
- This method has to be called before open().
-
- \param format : Camera video format.
- \param mode : Camera video mode.
-
- See the following table for the correspondances between the input
- format and mode and the resulting video color coding.
-
- <TABLE BORDER="1">
- <TR><TH> Format </TH><TH> Mode </TH><TH> (H) x (W) </TH><TH> Color </TH></TR>
- <TR><TD> 0 </TD><TD> 0 </TD><TD> 160 x 120 </TD><TD> YUV444 </TD></TR>
- <TR><TD> 0 </TD><TD> 1 </TD><TD> 320 x 240 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 0 </TD><TD> 2 </TD><TD> 640 x 480 </TD><TD> YUV411 </TD></TR>
- <TR><TD> 0 </TD><TD> 3 </TD><TD> 640 x 480 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 0 </TD><TD> 4 </TD><TD> 640 x 480 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 0 </TD><TD> 5 </TD><TD> 640 x 480 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 0 </TD><TD> 6 </TD><TD> 640 x 480 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 1 </TD><TD> 0 </TD><TD> 800 x 600 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 1 </TD><TD> 1 </TD><TD> 800 x 600 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 1 </TD><TD> 2 </TD><TD> 800 x 600 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 1 </TD><TD> 3 </TD><TD>1024 x 768 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 1 </TD><TD> 4 </TD><TD>1024 x 768 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 1 </TD><TD> 5 </TD><TD>1024 x 768 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 1 </TD><TD> 6 </TD><TD> 800 x 600 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 1 </TD><TD> 7 </TD><TD>1024 x 768 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 2 </TD><TD> 0 </TD><TD>1280 x 960 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 2 </TD><TD> 1 </TD><TD>1280 x 960 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 2 </TD><TD> 2 </TD><TD>1280 x 960 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 2 </TD><TD> 3 </TD><TD>1600 x 1200</TD><TD> YUV422 </TD></TR>
- <TR><TD> 2 </TD><TD> 4 </TD><TD>1600 x 1200</TD><TD> RGB8 </TD></TR>
- <TR><TD> 2 </TD><TD> 5 </TD><TD>1600 x 1200</TD><TD> MONO8 </TD></TR>
- <TR><TD> 2 </TD><TD> 6 </TD><TD>1280 x 960 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 2 </TD><TD> 7 </TD><TD>1600 x 1200</TD><TD> MONO16 </TD></TR>
- </TABLE>
-
- */
-void
-vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode )
-{
- initCamera();
-
- _format = format ;
- _mode = mode ;
-
- // Set format and mode
- if ((_format != -1) && (_mode != -1))
- {
- if (!camera->HasVideoMode(_format, _mode))
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: The image format is not supported by the IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Video mode not supported") );
- }
-
- if (camera->IsAcquiring()) {
- // stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
- }
- }
-
- if (camera->SetVideoFormat(_format) != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video format") );
- }
-
- if (camera->SetVideoMode(_mode) != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video mode") );
- }
-
- // start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
- }
-
- // Update Image dimension
- unsigned long w, h;
- camera->GetVideoFrameDimensions(&w, &h);
- this->width = w;
- this->height = h;
-
- // Update the color coding
- _color = getVideoColorCoding();
- }
-}
-
-/*!
- Set camera framerate rate. This method has to be called before open().
-
- \param fps : Value between 0 to 7 used to select a specific camera framerate.
- See the following table for the correspondances between the input
- value and the framerate.
-
- <TABLE BORDER="1">
- <TR><TH> Value </TH> <TH> Frame rate </TH></TR>
- <TR><TD> 0 </TD> <TD> 1.875 fps </TD></TR>
- <TR><TD> 1 </TD> <TD> 3.75 fps </TD></TR>
- <TR><TD> 2 </TD> <TD> 7.5 fps </TD></TR>
- <TR><TD> 3 </TD> <TD> 15 fps </TD></TR>
- <TR><TD> 4 </TD> <TD> 30 fps </TD></TR>
- <TR><TD> 5 </TD> <TD> 60 fps </TD></TR>
- <TR><TD> 6 </TD> <TD> 120 fps </TD></TR>
- <TR><TD> 7 </TD> <TD> 240 fps </TD></TR>
- </TABLE>
-
- \sa getFramerate()
- */
-void
-vp1394CMUGrabber::setFramerate(unsigned long fps)
-{
- initCamera();
-
- _fps = fps;
-
- // Set fps
- if (_fps!=-1)
- {
- if (!camera->HasVideoFrameRate(_format,_mode,_fps))
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported by the IEEE 1394 camera number %i for the selected image format",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"The frame rate is not supported") );
- }
-
- if (camera->IsAcquiring()) {
- // stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
- }
- }
- if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video frame rate") );
- }
- // start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
- close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
- }
-
- }
-}
-/*!
- Get the video framerate.
-
- \return Value between 0 to 7 corresponding to a specific camera framerate.
- See the following table for the correspondances between the returned
- value and the framerate.
-
- <TABLE BORDER="1">
- <TR><TH> Value </TH> <TH> Frame rate </TH></TR>
- <TR><TD> 0 </TD> <TD> 1.875 fps </TD></TR>
- <TR><TD> 1 </TD> <TD> 3.75 fps </TD></TR>
- <TR><TD> 2 </TD> <TD> 7.5 fps </TD></TR>
- <TR><TD> 3 </TD> <TD> 15 fps </TD></TR>
- <TR><TD> 4 </TD> <TD> 30 fps </TD></TR>
- <TR><TD> 5 </TD> <TD> 60 fps </TD></TR>
- <TR><TD> 6 </TD> <TD> 120 fps </TD></TR>
- <TR><TD> 7 </TD> <TD> 240 fps </TD></TR>
- </TABLE>
-
- \sa setFramerate()
-*/
-int
-vp1394CMUGrabber::getFramerate()
-{
- initCamera();
- int fps = camera->GetVideoFrameRate();
- return fps;
-}
-
-#endif
-
diff --git a/src/device/framegrabber/1394/vp1394CMUGrabber.h b/src/device/framegrabber/1394/vp1394CMUGrabber.h
deleted file mode 100644
index c34a837..0000000
--- a/src/device/framegrabber/1394/vp1394CMUGrabber.h
+++ /dev/null
@@ -1,303 +0,0 @@
-/****************************************************************************
- *
- * $Id: vp1394CMUGrabber.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
- *
- * Authors:
- * Lucas Lopes Lemos FEMTO-ST, AS2M departement, Besancon
- * Guillaume Laurent FEMTO-ST, AS2M departement, Besancon
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vp1394CMUGrabber_h
-#define vp1394CMUGrabber_h
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_CMU1394
-
-#include <windows.h>
-#include <1394Camera.h> // CMU library
-
-#include <visp/vpImage.h>
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpFrameGrabberException.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \file vp1394CMUGrabber.h
- \brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
-*/
-
-/*!
- \class vp1394CMUGrabber
-
- \ingroup Framegrabber CameraDriver
-
- \brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
-
- This block is based on the CMU 1394 Digital Camera SDK. The CMU 1394 Digital
- Camera Driver must be installed (go to http://www.cs.cmu.edu/~iwan/1394/index.html
- to download it).
- - Parameters:
- - Camera index (0, 1, 2, ... or 10). First camera has index 0.
- - Image format
- - Frame rate. Real frame rate depends on device capacities.
- - Control : shutter speed and gain can be internally set, but it is possible to set manually them.
- * Exposure time register value. Real exposure time depends on device capacities.
- * Gain register value. Real gain depends on device capacities.
-
- This first example available in tutorial-grabber-CMU1394.cpp shows how to grab
- and display images from a firewire camera under Windows.
-
- \include tutorial-grabber-CMU1394.cpp
-
- This other example shows how to consider more than one firewire camera, and how
- to grab and display images from the first camera found on the bus.
-
- \code
-#include <iostream>
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vp1394CMUGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_CMU1394)
- std::cout << "ViSP Image acquisition example" << std::endl;
-
- vpImage<unsigned char> I;
- vp1394CMUGrabber g;
-
- if( g.getNumberOfConnectedCameras() > 1 )
- std::cout << "There are " << g.getNumberOfConnectedCameras() << " connected cameras." << std::endl;
- if( g.getNumberOfConnectedCameras() == 1 )
- std::cout << "There is " << g.getNumberOfConnectedCameras() << " connected camera." << std::endl;
- else
- std::cout << "There is no connected camera." << std::endl;
-
- // Setting camera parameters manually
- g.selectCamera(0);
- g.setGain(0);
- g.setShutter(2000);
- g.setFramerate(3); // 15 FPS
- g.setVideoMode(0, 5); // 640x480 - MONO
-
- g.acquire(I);
-
- // Display camera description
- g.displayCameraDescription(0);
- g.displayCameraModel();
- std::cout << "Height: " << g.getHeight() << " Width: " << g.getWidth() << std::endl;
-
- vpDisplayOpenCV d(I);
- vpDisplay::display(I);
-
- for(;;)
- {
- g.acquire(I);
- vpDisplay::display(I);
- vpDisplay::flush(I);
- if (vpDisplay::getClick(I, false)) // a click to exit
- break;
- }
-
- g.close();
-#endif
- std::cout << "ViSP exiting..." <<std::endl;
- return 0;
-}
- \endcode
-*/
-
-class VISP_EXPORT vp1394CMUGrabber : public vpFrameGrabber
-{
-public:
- /*!
- Enumeration of color codings.
- */
- typedef enum {
- YUV444,
- YUV422,
- YUV411,
- RGB8,
- MONO8,
- MONO16,
- UNKNOWN
- } vpColorCodingType;
-
-private :
- //! Current camera handle
- C1394Camera *camera;
- //! Camera index on the bus
- int index;
- //! Current video format
- unsigned long _format;
- //! Current video mode
- unsigned long _mode ;
- //! Current video frame rate
- unsigned long _fps ;
- //! Current auto mode
- bool _modeauto;
- //! Current gain
- unsigned short _gain;
- //! Current shutter
- unsigned short _shutter;
- //! Color coding
- vpColorCodingType _color;
-
-public:
-
- // Constructor.
- vp1394CMUGrabber();
- // Destructor.
- virtual ~vp1394CMUGrabber();
-
- // Acquire one frame in a greyscale image.
- void acquire(vpImage<unsigned char> &I);
-
- // Acquire one frame in a color image.
- void acquire(vpImage<vpRGBa> &I);
-
- // Stop the acquisition.
- void close();
-
- // Display information about the camera on the standard output.
- void displayCameraDescription(int cam_id);
-
- // Display camera model on the standard output. Call it after open the grabber.
- void displayCameraModel();
-
- // Get the video framerate
- int getFramerate();
-
- // Get the gain min and max values.
- void getGainMinMax(unsigned short &min, unsigned short &max);
-
- // Get the number of connected cameras.
- int getNumberOfConnectedCameras() const ;
-
- // Get the shutter min and max values.
- void getShutterMinMax(unsigned short &min, unsigned short &max);
-
- //! Get the video color coding format.
- vpColorCodingType getVideoColorCoding() const
- {
- vpColorCodingType color = vp1394CMUGrabber::UNKNOWN;
- if (_format == 0)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV444; break;
- case 1: color = vp1394CMUGrabber::YUV422; break;
- case 2: color = vp1394CMUGrabber::YUV411; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
- }
- }
- else if (_format == 1)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV422; break;
- case 1: color = vp1394CMUGrabber::RGB8; break;
- case 2: color = vp1394CMUGrabber::MONO8; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
- case 7: color = vp1394CMUGrabber::MONO16; break;
- }
- }
- else if (_format == 2)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV422; break;
- case 1: color = vp1394CMUGrabber::RGB8; break;
- case 2: color = vp1394CMUGrabber::MONO8; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
- case 7: color = vp1394CMUGrabber::MONO16; break;
- }
- }
-
- return color;
- }
-
- // Initialization of the grabber using a greyscale image.
- void open(vpImage<unsigned char> &I);
-
- // Initialization of the grabber using a color image.
- void open(vpImage<vpRGBa> &I);
-
- // Select the camera on the bus. Call it before open the grabber.
- void selectCamera(int cam_id);
-
- // Enable auto gain
- void setAutoGain();
-
- // Enable auto shutter
- void setAutoShutter();
-
- // Set the gain and the shutter values. Call it before open the grabber
- void setControl(unsigned short gain, unsigned short shutter);
-
- // Set the frame rate. Call it before open the grabber.
- void setFramerate(unsigned long fps);
-
- // Set the shutter value. Call it before open the grabber
- void setShutter(unsigned short shutter);
-
- // Set the gain value. Call it before open the grabber
- void setGain(unsigned short gain);
-
- // Set the video format and mode. Call it before open the grabber.
- void setVideoMode(unsigned long format, unsigned long mode );
-
-private :
-
- void initCamera();
-
-};
-
-#endif
-#endif
diff --git a/src/device/framegrabber/1394/vp1394TwoGrabber.cpp b/src/device/framegrabber/1394/vp1394TwoGrabber.cpp
deleted file mode 100644
index 7784853..0000000
--- a/src/device/framegrabber/1394/vp1394TwoGrabber.cpp
+++ /dev/null
@@ -1,3836 +0,0 @@
-/****************************************************************************
- *
- * $Id: vp1394TwoGrabber.cpp 4728 2014-04-23 12:26:44Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vp1394TwoGrabber.cpp
- \brief member functions for firewire cameras
- \ingroup libdevice
-*/
-#include <iostream>
-
-#include <visp/vpConfig.h>
-
-/*
- * Interface with libdc1394 2.x
- */
-#if defined(VISP_HAVE_DC1394_2)
-#include <unistd.h>
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpFrameGrabberException.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpTime.h>
-
-const char * vp1394TwoGrabber::strVideoMode[DC1394_VIDEO_MODE_NUM]= {
- "MODE_160x120_YUV444",
- "MODE_320x240_YUV422",
- "MODE_640x480_YUV411",
- "MODE_640x480_YUV422",
- "MODE_640x480_RGB8",
- "MODE_640x480_MONO8",
- "MODE_640x480_MONO16",
- "MODE_800x600_YUV422",
- "MODE_800x600_RGB8",
- "MODE_800x600_MONO8",
- "MODE_1024x768_YUV422",
- "MODE_1024x768_RGB8",
- "MODE_1024x768_MONO8",
- "MODE_800x600_MONO16",
- "MODE_1024x768_MONO16",
- "MODE_1280x960_YUV422",
- "MODE_1280x960_RGB8",
- "MODE_1280x960_MONO8",
- "MODE_1600x1200_YUV422",
- "MODE_1600x1200_RGB8",
- "MODE_1600x1200_MONO8",
- "MODE_1280x960_MONO16",
- "MODE_1600x1200_MONO16",
- "MODE_EXIF",
- "MODE_FORMAT7_0",
- "MODE_FORMAT7_1",
- "MODE_FORMAT7_2",
- "MODE_FORMAT7_3",
- "MODE_FORMAT7_4",
- "MODE_FORMAT7_5",
- "MODE_FORMAT7_6",
- "MODE_FORMAT7_7"
-};
-
-const char * vp1394TwoGrabber::strFramerate[DC1394_FRAMERATE_NUM]= {
- "FRAMERATE_1_875",
- "FRAMERATE_3_75",
- "FRAMERATE_7_5",
- "FRAMERATE_15",
- "FRAMERATE_30",
- "FRAMERATE_60",
- "FRAMERATE_120",
- "FRAMERATE_240"
-};
-
-const char * vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM]= {
- "COLOR_CODING_MONO8",
- "COLOR_CODING_YUV411",
- "COLOR_CODING_YUV422",
- "COLOR_CODING_YUV444",
- "COLOR_CODING_RGB8",
- "COLOR_CODING_MONO16",
- "COLOR_CODING_RGB16",
- "COLOR_CODING_MONO16S",
- "COLOR_CODING_RGB16S",
- "COLOR_CODING_RAW8",
- "COLOR_CODING_RAW16",
-};
-
-
-
-/*!
- Default constructor.
-
- By default:
- - the camera is the first found on the bus,
- - the ring buffer size is set to 4,
- - reset the bus attached to the first camera found on the bus.
-
- Current camera settings can be changed using setCamera() to select the active
- camera on the bus and than setVideoMode() or setFramerate() to fix the active
- camera settings. The list of supported video modes and framerates is
- available using respectively getVideoModeSupported() and
- getFramerateSupported(). To change the ring buffer size use setRingBufferSize().
-
- \param reset : If "true", reset the bus attached to the first
- camera found. Bus reset may help to make firewire working if the
- program was not properly stopped by a CTRL-C.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \sa setCamera(), setVideoMode(), setFramerate()
-
-*/
-vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
- : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), camIsOpen(NULL),
- num_buffers(4), // ring buffer size
- isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
- #ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- , d(NULL),
- list(NULL)
- #endif
-{
- // protected members
- width = height = 0;
-
- // private members
- init = false;
-
- reset = false;
- initialize(reset);
-
- // open();
-}
-
-/*!
-
- Destructor.
-
- Close the firewire grabber.
-
- \sa close()
-
-*/
-vp1394TwoGrabber::~vp1394TwoGrabber()
-{
-/* if(num_cameras >= 1){
- delete[] isDataModified;
- delete[] initialShutterMode;
- delete[] dataCam;
- }*/
- close();
-}
-
-/*!
-
- If multiples cameras are connected on the bus, select the camero to dial
- with.
-
- \param cam_id : A camera identifier or GUID. By identifier, we
- mean a value comprised between 0 (the first camera found on the bus)
- and the number of cameras found on the bus and returned by
- getNumCameras() minus 1. If two cameras are connected on the bus,
- setting \e camera_id to one allows to communicate with the second
- one. This identifier is not unique. That is why, it is also possible
- to select a camera by its GUID, which is unique.
-
- \exception vpFrameGrabberException::settingError : If the required camera is
- not reachable.
-
- Here an example of single capture from the last camera found on the bus:
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- unsigned int ncameras; // Number of cameras on the bus
- vpImage<unsigned char> I;
-
- vp1394TwoGrabber g;
- ncameras = g.getNumCameras();
-
- int last = 0;
- if (ncameras > 1)
- last = ncameras-1;
-
- g.setCamera(last); // To dial with the last camera on the bus
- while(1)
- g.acquire(I);// I contains the frame captured by the last camera on the bus
-#endif
-}
- \endcode
-
- If more than one camera is connected, it is also possible to select a specific camera by its GUID:
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I; // Create a gray level image container
- bool reset = false; // Disable bus reset during construction
- vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
-
- unsigned int ncameras; // Number of cameras on the bus
- ncameras = g.getNumCameras();
- std::cout << ncameras << " cameras found:" << std::endl;
-
- for(int i=0; i< ncameras; i++)
- {
- g.setCamera(i);
- uint64_t guid = g.getGuid();
- printf("camera %d with guid 0x%lx\n", i, guid);
- }
-
- // produce:
- // 2 cameras found:
- // camera 0 with guid 0xb09d01009b329c
- // camera 1 with guid 0xb09d01007e0ee7
-
- g.setCamera( 0xb09d01009b329c );
-
- printf("Use camera with GUID: 0x%lx\n", g.getGuid());
- g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
-
- vpImageIo::write(I, "image.pgm"); // Write image on the disk
-#endif
-}
- \endcode
-
- Here an example of multi camera capture:
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- unsigned int ncameras; // Number of cameras on the bus
- vp1394TwoGrabber g;
- ncameras = g.getNumCameras();
- vpImage<unsigned char> *I = new vpImage<unsigned char> [ncameras];
-
- // If the first camera supports vpVIDEO_MODE_640x480_YUV422 video mode
- g.setCamera(0);
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_YUV422);
-
- if (ncameras >= 2) {
- // If the second camera support 30 fps acquisition
- g.setCamera(1);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
- }
-
- while(1) {
- for (unsigned int camera=0; camera < ncameras; camera ++) {
- g.setCamera(camera);
- g.acquire(I[camera]);
- }
- }
- delete [] I;
-#endif
-}
- \endcode
-
- \sa setFormat(), setVideoMode(), setFramerate(), getNumCameras()
-
-*/
-void
-vp1394TwoGrabber::setCamera(uint64_t cam_id)
-{
- // Suppose that if camera_id is a camera GUID, this value is greater
- // than the number of cameras connected to the bus
- if (cam_id >= num_cameras) {
- // Check if camera_id is a camera guid
- bool is_guid = false;
- // check if the camera_id is a guid
- for (unsigned int i=0; i< num_cameras; i++) {
- if (cameras[i]->guid == cam_id) {
- this->camera_id = i;
- is_guid = true;
- break;
- }
- }
- if (is_guid == false) {
- std::cout << "Error: The camera with guid 0x" << std::hex << cam_id << " is not present" << std::endl;
- std::cout << num_cameras << " camera(s) connected" << std::endl;
- for (unsigned int i=0; i< num_cameras; i++) {
- std::cout << " - camera " << i << " with guid 0x" << std::hex << cameras[i]->guid << std::endl;
- }
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required camera is not present") );
- }
- }
- else {
- this->camera_id = (unsigned int) cam_id; // The input cam_id is not a uint64_t guid, but the index of the camera
- }
-
- // create a pointer to the working camera
- camera = cameras[this->camera_id];
-}
-
-
-/*!
-
- Get the active camera identifier on the bus.
-
- \param cam_id : The active camera identifier. The value is
- comprised between 0 (the first camera) and the number of cameras
- found on the bus returned by getNumCameras() minus 1.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera is found.
-
- \sa setCamera(), getNumCameras()
-
-*/
-void
-vp1394TwoGrabber::getCamera(uint64_t &cam_id)
-{
- if (num_cameras) {
- cam_id = this->camera_id;
- }
- else {
- close();
- vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
- }
-}
-
-/*!
-
- Get the active camera identifier on the bus.
-
- \return The active camera identifier. The value is
- comprised between 0 (the first camera) and the number of cameras
- found on the bus returned by getNumCameras() minus 1.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera is found.
-
- \sa setCamera(), getNumCameras()
-
-*/
-uint64_t
-vp1394TwoGrabber::getCamera()
-{
- if (num_cameras) {
- return this->camera_id;
- }
- else {
- close();
- vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
- }
-}
-
-/*!
-
- Return the number of cameras connected on the bus.
-
- \param ncameras : The number of cameras found on the bus.
-
-*/
-void
-vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
-{
- if (! num_cameras) {
- vpCTRACE << "No camera found..."<< std::endl;
- ncameras = 0;
- }
-
- ncameras = num_cameras;
-}
-
-/*!
-
- Return the number of cameras connected on the bus.
-
- \return The number of cameras found on the bus.
-
-*/
-unsigned int
-vp1394TwoGrabber::getNumCameras() const
-{
- unsigned int ncameras = 0;
- if (! num_cameras) {
- vpCTRACE << "No camera found..."<< std::endl;
- ncameras = 0;
- }
-
- ncameras = num_cameras;
- return ncameras;
-}
-
-/*!
-
- Set the camera video capture mode. Image size is than updated with respect to
- the new video capture mode.
-
- The iso transmission (setTransmission()) and the dma capture (see
- setCapture()) are first stopped. Then, the camera video capture mode is
- set. Finaly, the dma capture and the iso transmission are re-started.
-
- \param videomode : The camera video capture mode. The current camera mode is
- given by getVideoMode(). The camera supported modes are given by
- getVideoModeSupported().
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the video mode.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \sa getVideoMode(), getVideoModeSupported(), setCamera()
-
-*/
-void
-vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
-{
- open();
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- if (!isVideoModeSupported(videomode)){
- vpERROR_TRACE("Video mode not supported by camera %d",camera_id);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Video mode not supported") );
- return ;
- }
- // Stop dma capture if started
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
-
- if (dc1394_video_set_mode(camera, (dc1394video_mode_t) videomode) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't set video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't set video mode") );
- }
-
- setCapture(DC1394_ON);
- setTransmission(DC1394_ON);
-
- // Updates image size from new video mode
- if (dc1394_get_image_size_from_video_mode(camera,
- (dc1394video_mode_t) videomode,
- &this->width, &this->height)
- != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't set video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get image size") );
- }
-
-}
-
-/*!
-
- Query the actual capture video mode of the active camera. All
- the active camera supported modes are given by getVideoModeSupported().
-
- \param videomode : The camera capture video mode.
-
- \exception vpFrameGrabberException::initializationError : If the
- required camera is not present.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the camera actual video mode.
-
- \sa setVideoMode(), getVideoModeSupported(), setCamera()
-
-*/
-void
-vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType & videomode)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394video_mode_t _videomode;
- if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current video mode") );
- }
- videomode = (vp1394TwoVideoModeType) _videomode;
-
-}
-
-
-
-/*!
-
- Query the available active camera video modes.
-
-
- \param videomodes : The list of supported camera video modes.
-
- \return The number of supported camera modes, 0 if an error occurs.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- video modes.
-
- \sa setVideoMode(), getVideoMode(), getCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes)
-{
- // Refresh the list of supported modes
- videomodes.clear();
-
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- dc1394video_modes_t _videomodes;
-
- // get video modes:
- if (dc1394_video_get_supported_modes(camera, &_videomodes)!=DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get video modes");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get video modes") );
- }
-
- // parse the video modes to add in the list
- for (unsigned i=0; i < _videomodes.num; i++) {
- vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType) _videomodes.modes[i];
- videomodes.push_back( _mode );
- }
-
- // return the number of available video modes
- return _videomodes.num;
-}
-/*!
- Check for the active camera video mode.
-
- \param videomode : The video mode to check for.
-
- \return true if the active camera supports the desired video mode.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- video modes.
-
- \sa setVideoMode(), getVideoMode(), getCamera()
- */
-bool
-vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- dc1394video_modes_t _videomodes;
-
- // get video modes:
- if (dc1394_video_get_supported_modes(camera, &_videomodes)!=DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get video modes");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get video modes") );
- }
-
- // parse the video modes to check with the desired
- for (unsigned i=0; i < _videomodes.num; i++) {
- if ((vp1394TwoVideoModeType) _videomodes.modes[i] == videomode)
- return true;
- }
- return false;
-}
-
-/*!
-
- Indicates if the video mode is format 7.
-
- \param videomode : The video mode to check for.
-
- \return true : If the video mode is scalable (Format 7).
- \return false : If the video mode is not Format 7 like.
-
- \sa setVideoMode(), getVideoModeSupported(), setCamera()
-
-*/
-bool
-vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
-{
-
- if (dc1394_is_video_mode_scalable((dc1394video_mode_t) videomode))
- return true;
-
- return false;
-}
-
-/*!
-
- Indicates if the active camera is grabbing color or grey images.
-
- We consider color images if the color coding is either YUV (411, 422, 444) or
- RGB (8, 16, 16S). We consider grey images if the color coding is MONO (8,
- 16, 16S) or RAW (8, 16). vp1394TwoColorCodingType gives the supported color
- codings.
-
- \return true : If color images are acquired.
- \return false : If grey images are acquired.
-
- \sa getColorCoding(), setCamera()
-
-*/
-bool
-vp1394TwoGrabber::isColor()
-{
- vp1394TwoColorCodingType coding;
- getColorCoding(coding);
-
- switch (coding) {
- case vpCOLOR_CODING_MONO8:
- case vpCOLOR_CODING_MONO16:
- case vpCOLOR_CODING_MONO16S:
- case vpCOLOR_CODING_RAW8:
- case vpCOLOR_CODING_RAW16:
- return false;
- case vpCOLOR_CODING_YUV411:
- case vpCOLOR_CODING_YUV422:
- case vpCOLOR_CODING_YUV444:
- case vpCOLOR_CODING_RGB8:
- case vpCOLOR_CODING_RGB16:
- case vpCOLOR_CODING_RGB16S:
- return true;
- }
- return false;
-}
-
-/*!
-
- Set the active camera framerate for non scalable video modes.
-
- The iso transmission (setTransmission()) and the dma capture (see
- setCapture()) are first stopped. Then, the camera framerate capture mode is
- set. Finaly, the dma capture and the iso transmission are re-started.
-
- If the current video mode is scalable (Format 7), this function is without
- effect.
-
- \param fps : The camera framerate. The current framerate of the camera is
- given by getFramerate(). The camera supported framerates are given by
- getFramerateSupported().
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the framerate.
-
- \sa getFramerate(), getFramerateSupported() , setCamera()
-
-*/
-void
-vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
-{
- open();
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- vp1394TwoVideoModeType cur_videomode;
- getVideoMode(cur_videomode);
- if (isVideoModeFormat7(cur_videomode))
- return;
-
- if (!isFramerateSupported(cur_videomode,fps)){
- vpERROR_TRACE("Framerate not supported by camera %d",camera_id);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Framerate not supported") );
- return ;
- }
-
- // Stop dma capture if started
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
-
- if (dc1394_video_set_framerate(camera, (dc1394framerate_t) fps) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't set framerate");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't set framerate") );
- }
-
- setCapture(DC1394_ON);
- setTransmission(DC1394_ON);
-}
-
-/*!
-
- Query the actual camera framerate of the active camera. The camera supported
- framerates are given by getFramerateSupported().
-
- \param fps : The camera capture framerate.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the framerate.
-
- \sa setFramerate(), getFramerateSupported(), setCamera()
-
-*/
-void
-vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType & fps)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- dc1394framerate_t _fps;
- if (dc1394_video_get_framerate(camera, &_fps) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current framerate");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current framerate") );
- }
- fps = (vp1394TwoFramerateType) _fps;
-
-}
-
-/*!
-
- Query the available framerates for the given camera video mode (see
- file dc1394/control.h). No framerate is associated to the following
- camera modes :
-
- - vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6),
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 (format 7):
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_1 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_2 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_3 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_4 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_5 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_6 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_7 (format 7)
-
- \param mode : Camera video mode.
-
- \param fps : The list of supported camera framerates for the given camera
- video mode.
-
- \return The number of supported framerates, 0 if no framerate is available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the supported framerates.
-
- \sa setFramerate(), getFramerate(), setCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
- std::list<vp1394TwoFramerateType> & fps)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- // Refresh the list of supported framerates
- fps.clear();
-
- switch (mode) {
- // Framerate not available for:
- // - vpVIDEO_MODE_EXIF ie Format_6
- // - vpVIDEO_MODE_FORMAT7... ie the Format_7
- case vpVIDEO_MODE_EXIF:
- case vpVIDEO_MODE_FORMAT7_0:
- case vpVIDEO_MODE_FORMAT7_1:
- case vpVIDEO_MODE_FORMAT7_2:
- case vpVIDEO_MODE_FORMAT7_3:
- case vpVIDEO_MODE_FORMAT7_4:
- case vpVIDEO_MODE_FORMAT7_5:
- case vpVIDEO_MODE_FORMAT7_6:
- case vpVIDEO_MODE_FORMAT7_7:
- return 0;
- break;
- default:
- {
- dc1394framerates_t _fps;
- if (dc1394_video_get_supported_framerates(camera,
- (dc1394video_mode_t)mode,
- &_fps) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported frametates for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported framerates") );
- }
- if (_fps.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _fps.num; i ++)
- fps.push_back((vp1394TwoFramerateType)_fps.framerates[i]);
-
- return _fps.num;
- }
- break;
- }
-}
-/*!
-
- Check if the desired framerate is supported for the given camera video mode
- (see file dc1394/control.h). No framerate is associated to the following
- camera modes :
-
- - vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6),
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 (format 7):
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_1 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_2 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_3 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_4 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_5 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_6 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_7 (format 7)
-
- \param mode : Camera video mode.
-
- \param fps : The desired camera framerates for the given camera
- video mode.
-
- \return true if the desired framerate is supported by the active camera.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the supported framerates.
-
-\exception vpFrameGrabberException::settingError : If the framerate is not
- supported.
- \sa setFramerate(), getFramerate(), setCamera()
- */
-bool
-vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode,
- vp1394TwoFramerateType fps)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- switch (mode) {
- // Framerate not available for:
- // - vpVIDEO_MODE_EXIF ie Format_6
- // - vpVIDEO_MODE_FORMAT7... ie the Format_7
- case vpVIDEO_MODE_EXIF:
- case vpVIDEO_MODE_FORMAT7_0:
- case vpVIDEO_MODE_FORMAT7_1:
- case vpVIDEO_MODE_FORMAT7_2:
- case vpVIDEO_MODE_FORMAT7_3:
- case vpVIDEO_MODE_FORMAT7_4:
- case vpVIDEO_MODE_FORMAT7_5:
- case vpVIDEO_MODE_FORMAT7_6:
- case vpVIDEO_MODE_FORMAT7_7:
- return 0;
- break;
- default:
- {
- dc1394framerates_t _fps;
- if (dc1394_video_get_supported_framerates(camera,
- (dc1394video_mode_t)mode,
- &_fps) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported frametates for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported framerates") );
- }
- if (_fps.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _fps.num; i ++){
- if (fps==(vp1394TwoFramerateType)_fps.framerates[i]){
- return true;
- }
- }
- return false;
- }
- break;
- }
-}
-
-/*!
-
- Set the active camera Format 7 color coding.
-
- The iso transmission (setTransmission()) and the dma capture (see
- setCapture()) are first stopped. Then, the active camera Format 7 is
- set. Finaly, the dma capture and the iso transmission are re-started.
-
- \warning Setting color coding for non format 7 video mode will be
- without effect.
-
- \param coding : The camera color coding for Format 7 video mode. The
- current color coding of the camera is given by getColorCoding(). The
- camera supported color codings are given by
- getColorCodingSupported().
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the color coding for Format 7 video mode.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \sa getColorCoding(), getColorCodingSupported() , setCamera()
-
-*/
-void
-vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394video_mode_t _videomode;
- if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current video mode") );
- }
-
- if (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode,coding)){
- vpERROR_TRACE("Color coding not supported by camera %d",camera_id);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Color coding not supported") );
- return ;
- }
-
- // Format 7 video mode
- if (dc1394_is_video_mode_scalable(_videomode)) {
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
-
- if (dc1394_format7_set_color_coding(camera, _videomode,
- (dc1394color_coding_t) coding)
- != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't set color coding");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't set color coding") );
- }
-
- setCapture(DC1394_ON);
- setTransmission(DC1394_ON);
- }
-}
-
-/*!
-
- Query the actual color coding of the active camera. The camera supported
- color codings are given by getColorCodingSupported().
-
- \param coding : The camera capture color coding.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the actual color coding. Occurs if current video mode is
- vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6).
-
- \sa setColorCoding(), getColorCodingSupported(), setCamera()
-
-*/
-void
-vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType & coding)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- dc1394video_mode_t _videomode;
- if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current video mode") );
- }
-
- dc1394color_coding_t _coding;
- if (dc1394_is_video_mode_scalable(_videomode)) {
- // Format 7 video mode
- if (dc1394_format7_get_color_coding(camera, _videomode, &_coding)
- != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current color coding");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't query current color coding") );
- }
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)_videomode)) {
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "No color coding for format 6 video mode"));
- }
- else {
- // Not Format 7 and not Format 6 video modes
- if (dc1394_get_color_coding_from_video_mode(camera,
- (dc1394video_mode_t)_videomode,
- &_coding) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color coding for mode %d\n",
- _videomode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't query current color coding"));
- }
- }
- coding = (vp1394TwoColorCodingType) _coding;
-}
-
-/*!
-
- Query the available color codings for the given camera video mode (see
- file dc1394/control.h).
-
- \param mode : Camera video mode.
-
- \param codings : The list of supported color codings for the given camera
- video mode.
-
- \return The number of supported color codings, 0 if no color codings
- is available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the color codingss.
-
- \sa setColorCoding(), getColorCoding(), setCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
- std::list<vp1394TwoColorCodingType> & codings)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- // Refresh the list of supported framerates
- codings.clear();
-
- if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
- // Format 7 video mode
- dc1394color_codings_t _codings;
- if (dc1394_format7_get_color_codings(camera,
- (dc1394video_mode_t)mode,
- &_codings) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color codings for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color codings") );
- }
- if (_codings.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _codings.num; i ++)
- codings.push_back((vp1394TwoColorCodingType)_codings.codings[i]);
-
- return _codings.num;
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
- // Format 6 video mode
- return 0;
- }
- else {
- // Not Format 7 and not Format 6 video modes
- dc1394color_coding_t _coding;
- if (dc1394_get_color_coding_from_video_mode(camera,
- (dc1394video_mode_t)mode,
- &_coding) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color coding for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color coding") );
- }
- codings.push_back((vp1394TwoColorCodingType)_coding);
- return 1;
- }
-}
-/*!
-
- Check if the color coding is supported for the given camera video mode (see
- file dc1394/control.h).
-
- \param mode : Camera video mode.
-
- \param coding : Desired color coding for the given camera
- video mode.
-
- \return true if the color coding is supported.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the color codingss.
- \exception vpFrameGrabberException::settingError : If the color coding is
- not supported.
- \sa setColorCoding(), getColorCoding(), setCamera()
- */
-bool
-vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode,
- vp1394TwoColorCodingType coding)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
-
- if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
- // Format 7 video mode
- dc1394color_codings_t _codings;
- if (dc1394_format7_get_color_codings(camera,
- (dc1394video_mode_t)mode,
- &_codings) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color codings for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color codings") );
- }
- if (_codings.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _codings.num; i ++){
- if (coding==(vp1394TwoColorCodingType)_codings.codings[i])
- return true;
- }
- return false;
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
- // Format 6 video mode
- return false;
- }
- else {
- // Not Format 7 and not Format 6 video modes
- dc1394color_coding_t _coding;
- if (dc1394_get_color_coding_from_video_mode(camera,
- (dc1394video_mode_t)mode,
- &_coding) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color coding for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color coding") );
- return false;
- }
- if (coding==(vp1394TwoColorCodingType)_coding)
- return true;
-
- return false;
- }
-}
-
-
-/*!
-
- Set the grabbed region of interest ie roi position and size for format 7
- video mode.
-
- The iso transmission (setTransmission()) and the dma capture (see
- setCapture()) are first stopped. Then, the format 7 roi is
- set. Finaly, the dma capture and the iso transmission are re-started.
-
- \warning Setting format 7 roi takes only effect if video mode is
- format 7 like.
-
- \param left : Position of the upper left roi corner.
-
- \param top : Position of the upper left roi corner.
-
- \param w : Roi width. If width is set to 0, uses the maximum
- allowed image width.
-
- \param h : Roi height. If width is set to 0, uses the maximum
- allowed image height.
-
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- roi.
-
- \sa isVideoModeFormat7()
-*/
-void
-vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top,
- unsigned int w, unsigned int h)
-{
- open();
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394video_mode_t _videomode;
- if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get current video mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current video mode") );
- }
- if (dc1394_is_video_mode_scalable(_videomode)) {
- // Stop dma capture if started
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
- // Format 7 video mode
- unsigned int max_width, max_height;
- if (dc1394_format7_get_max_image_size(camera, _videomode,
- &max_width, &max_height)
- != DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get format7 max image size");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get format7 max image size") );
- }
-#if 0
- vpTRACE("left: %d top: %d width: %d height: %d", left, top,
- width == 0 ? DC1394_USE_MAX_AVAIL: w,
- height == 0 ? DC1394_USE_MAX_AVAIL : h);
- vpTRACE("max_width: %d max_height: %d", max_width, max_height);
-#endif
-
- if (left > max_width) {
- vpERROR_TRACE("Can't set format7 ROI");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't set format7 ROI") );
- }
- if (top > max_height) {
- vpERROR_TRACE("Can't set format7 ROI");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't set format7 ROI") );
- }
-
- int32_t roi_width;
- int32_t roi_height;
-
- if (w != 0) {
- // Check if roi width is acceptable (ie roi is contained in the image)
- if (w > (max_width - left))
- w = (max_width - left);
- roi_width = (int32_t)w;
- }
- else {
- roi_width = DC1394_USE_MAX_AVAIL;
- }
-
- if (h != 0) {
- // Check if roi height is acceptable (ie roi is contained in the image)
- if (h > (max_height - top))
- h = (max_height - top);
- roi_height = (int32_t)h;
- }
- else {
- roi_height = DC1394_USE_MAX_AVAIL;
- }
-
- if (dc1394_format7_set_roi(camera, _videomode,
- (dc1394color_coding_t) DC1394_QUERY_FROM_CAMERA, // color_coding
- DC1394_USE_MAX_AVAIL/*DC1394_QUERY_FROM_CAMERA*/, // bytes_per_packet
- (int32_t)left, // left
- (int32_t)top, // top
- roi_width,
- roi_height)
- != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Can't set format7 roi");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get current video mode") );
- }
- // Update the image size
- if (dc1394_format7_get_image_size(camera, _videomode,
- &this->width,
- &this->height)
- != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Can't get format7 image size");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get format7 image size") );
- }
-
- setCapture(DC1394_ON);
- setTransmission(DC1394_ON);
- }
-}
-/*!
-
- Open ohci and asign handle to it and get the camera nodes and
- describe them as we find them.
-
- \param reset : If "true", reset the bus attached to the first
- camera found. Bus reset may help to make firewire working if the
- program was not properly stopped by a CTRL-C.
-
- \exception initializationError : If a raw1394 handle can't be aquired,
- or if no camera is found.
-
- \sa close()
- */
-void
-vp1394TwoGrabber::initialize(bool reset)
-{
- if (init == false){
- // Find cameras
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- if (d != NULL)
- dc1394_free (d);
-
- d = dc1394_new ();
- if (dc1394_camera_enumerate (d, &list) != DC1394_SUCCESS) {
- dc1394_camera_free_list (list);
- close();
- vpERROR_TRACE("Failed to enumerate cameras\n");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Failed to enumerate cameras") );
- }
-
- if (list->num == 0) {
- dc1394_camera_free_list (list);
- close();
- vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
- }
-
- if (cameras != NULL)
- delete [] cameras;
-
- cameras = new dc1394camera_t * [list->num];
-
- num_cameras = 0;
-
- for (unsigned int i=0; i < list->num; i ++) {
- cameras[i] = dc1394_camera_new (d, list->ids[i].guid);
- if (!cameras[i]) {
- vpTRACE ("Failed to initialize camera with guid \"%ld\"\n",
- list->ids[i].guid);
- continue;
- }
- // Update the number of working cameras
- num_cameras ++;
- }
-
- if (reset) {
- // Reset the bus to make firewire working if the program was not properly
- // stopped by a CTRL-C. We reset here only the bus attached to the first
- // camera
- dc1394_reset_bus(cameras[0]);
- }
-
- // if (list != NULL)
- dc1394_camera_free_list (list);
- list = NULL;
-
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- if (cameras != NULL)
- free(cameras);
- cameras = NULL;
- int err = dc1394_find_cameras(&cameras, &num_cameras);
-
- if (err!=DC1394_SUCCESS && err != DC1394_NO_CAMERA) {
- close();
- vpERROR_TRACE("Unable to look for cameras\n\n"
- "Please check \n"
- " - if the kernel modules `ieee1394',`raw1394' and `ohci1394' are loaded \n"
- " - if you have read/write access to /dev/raw1394\n\n");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Unable to look for cameras") );
-
- }
-#endif
-
- if (num_cameras == 0) {
- close();
- vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
- }
-
- // allocation for the parameters
- isDataModified = new bool[num_cameras];
- for(unsigned int i=0; i<num_cameras; i++)
- isDataModified[i] = false;
- initialShutterMode = new dc1394feature_mode_t[num_cameras];
- dataCam = new vpDc1394TwoCameraParametersData[num_cameras];
-
- if (camera_id >= num_cameras) {
- // Bad camera id
- close();
- vpERROR_TRACE("Bad camera id: %u", camera_id);
- vpERROR_TRACE("Only %u camera on the bus.", num_cameras);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Bad camera id") );
- }
-
- if (verbose) {
- std::cout << "------ Bus information ------" << std::endl;
- std::cout << "Number of camera(s) on the bus : " << num_cameras <<std::endl;
- std::cout << "-----------------------------" << std::endl;
- }
-
- if (camIsOpen != NULL) delete [] camIsOpen;
- camIsOpen = new bool [num_cameras];
- for (unsigned int i = 0;i<num_cameras;i++){
- camIsOpen[i]=false;
- }
-
- init = true;
- }
-}
-/*!
-
- Start the iso transmission and the dma capture of the current camera.
-
- \exception initializationError : If a raw1394 handle can't be aquired,
- or if no camera is found.
-
- \sa close()
-*/
-void
-vp1394TwoGrabber::open()
-{
- if (init == false) initialize(false);
- if (camIsOpen[camera_id] == false){
- dc1394switch_t status = DC1394_OFF;
-
- //#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- dc1394_video_get_transmission(cameras[camera_id], &status);
- if (status != DC1394_OFF){
- //#endif
- if (dc1394_video_set_transmission(cameras[camera_id],DC1394_OFF)!=DC1394_SUCCESS)
- vpTRACE("Could not stop ISO transmission");
- else {
- vpTime::wait(500);
- if (dc1394_video_get_transmission(cameras[camera_id], &status)!=DC1394_SUCCESS)
- vpTRACE("Could get ISO status");
- else {
- if (status==DC1394_ON) {
- vpTRACE("ISO transmission refuses to stop");
- }
-#ifdef VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- // No yet in the new API
- cameras[camera_id]->is_iso_on=status;
-#endif
- }
- //#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- }
- //#endif
- }
- setCamera(camera_id);
- //setIsoSpeed(DC1394_ISO_SPEED_400);
- setCapture(DC1394_ON);
- setTransmission(DC1394_ON);
- camIsOpen[camera_id] = true;
- }
-}
-/*!
-
- Close the firewire grabber.
-
- Stops the capture and the iso transmission of the active cameras and than
- releases all the cameras.
-
-*/
-void
-vp1394TwoGrabber::close()
-{
- if (init){
- if (num_cameras) {
- for (unsigned int i = 0; i < num_cameras;i++) {
- if (camIsOpen[i]) {
- camera = cameras[i];
- this->camera_id = i;// set camera id for the function updateDataStructToCam
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
- if(isDataModified[i]){
- // reset values
- try{
- updateDataStructToCam();
- }
- catch(...){
- }
- // reset mode (manual, auto, ...)
- if (dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_EXPOSURE, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_SHARPNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_HUE, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_SATURATION, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_GAMMA, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, initialShutterMode[i]) != DC1394_SUCCESS ||
- dc1394_feature_set_mode(camera, DC1394_FEATURE_IRIS, initialShutterMode[i])){
-
- vpERROR_TRACE("Unable to reset the initial mode");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to reset the initial mode"));
- }
- }
- if (dc1394_camera_set_power(camera, DC1394_OFF) != DC1394_SUCCESS)
- std::cout << "Unable to turn camera off" << std::endl;
- }
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- dc1394_camera_free(cameras[i]);
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- dc1394_free_camera(cameras[i]);
-#endif
- }
- }
- if (camIsOpen != NULL) {
- delete [] camIsOpen;
- camIsOpen = NULL;
- }
-
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- if (cameras != NULL) {
- delete [] cameras;
- cameras = NULL;
- }
- if (d != NULL) {
- dc1394_free (d);
- d = NULL;
- }
-
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- if (cameras != NULL) {
- free(cameras);
- cameras = NULL;
- }
-#endif
-
- camIsOpen = NULL;
- num_cameras = 0;
-
- // remove data for the parameters
- if(isDataModified != NULL){
- delete[] isDataModified;
- isDataModified = NULL;
- }
- if(initialShutterMode != NULL){
- delete[] initialShutterMode;
- initialShutterMode = NULL;
- }
- if(dataCam != NULL){
- delete[] dataCam;
- dataCam = NULL;
- }
-
- init = false;
- }
-}
-
-/*!
-
- Set the ring buffer size used for the capture.
- To know the current ring buffer size see getRingBufferSize().
-
- \param size : Ring buffer size.
-
- \exception vpFrameGrabberException::settingError : If ring buffer size is not valid.
-
- \sa getRingBufferSize()
-*/
-void
-vp1394TwoGrabber::setRingBufferSize(unsigned int size)
-{
- if (size < 1) {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not set ring buffer size") );
- }
-
- if (size != num_buffers) {
- // We need to change the ring buffer size
- num_buffers = size;
- if(camIsOpen[camera_id]){
- setCapture(DC1394_OFF);
- setCapture(DC1394_ON);
- }
- }
-}
-
-/*!
-
- Get the current ring buffer size used for the capture. To change the
- ring buffer size see setRingBufferSize().
-
- \return Current ring buffer size.
-
- \sa setRingBufferSize()
-*/
-unsigned int
-vp1394TwoGrabber::getRingBufferSize() const
-{
- return num_buffers;
-}
-
-/*!
- Enable auto shutter. It is also possible to set auto shutter min
- and max exposure time, but only for AVT cameras. In that case
- use setAutoShutter(unsigned int, unsigned int).
-
- \param enable : Flag to enable or disable the auto shutter. If false, set the
- shutter as manual.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(); // Enable auto shutter
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the auto shutter.
-
- \sa setAutoShutter(unsigned int, unsigned int), getAutoShutter()
-*/
-void
-vp1394TwoGrabber::setAutoShutter(bool enable)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394feature_mode_t mode;
- if (enable) {
- mode = DC1394_FEATURE_MODE_AUTO;
- }
- else {
- mode = DC1394_FEATURE_MODE_MANUAL;
- }
-
- if (dc1394_feature_set_power(camera, DC1394_FEATURE_SHUTTER, DC1394_ON)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set shutter on. \n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set shutter on") );
- }
-
- if (dc1394_feature_set_mode(camera,
- DC1394_FEATURE_SHUTTER,
- mode)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set auto shutter. \n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set auto shutter") );
- }
-}
-/*!
- Set auto shutter. If available set min and max exposure time.
-
- \warning Setting min and max exposure time feature is only available
- for AVT cameras.
-
- \param minvalue : Min shutter exposure time.
- \param maxvalue : Max shutter exposure time.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the auto shutter.
-
- \sa setAutoShutter(), getAutoShutter()
-*/
-void
-vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue)
-{
- setAutoShutter();
-
- if (dc1394_avt_set_auto_shutter(camera, minvalue, maxvalue)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set auto shutter min and max values. Is the camera an AVT one?\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set auto shutter min and max values") );
- }
-}
-
-/*!
- Get auto shutter min and max values.
-
- \warning Getting min and max auto shutter values is only available
- for AVT cameras.
-
- \param minvalue : Min shutter exposure time.
- \param maxvalue : Max shutter exposure time.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-*/
-void
-vp1394TwoGrabber::getAutoShutter(unsigned int &minvalue, unsigned int &maxvalue)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- if (dc1394_avt_get_auto_shutter(camera, &minvalue, &maxvalue)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot get auto shutter min and max values. Is the camera an AVT one?\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot get auto shutter min and max values") );
- }
-}
-
-/*!
- Enable auto gain. It is also possible to set the min
- and max gain, but only for AVT cameras. In that case
- use setAutoGain(unsigned int, unsigned int).
-
- \param enable : Flag to enable or disable the auto gain. If false, set the
- gain as manual.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoGain(true); // Enable auto gain
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the auto shutter.
-
- \sa setAutoGain(unsigned int, unsigned int), getAutoGain()
-*/
-void
-vp1394TwoGrabber::setAutoGain(bool enable)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394feature_mode_t mode;
- if (enable) {
- mode = DC1394_FEATURE_MODE_AUTO;
- }
- else {
- mode = DC1394_FEATURE_MODE_MANUAL;
- }
-
- if (dc1394_feature_set_power(camera, DC1394_FEATURE_GAIN, DC1394_ON)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set shutter on. \n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set shutter on") );
- }
-
- if (dc1394_feature_set_mode(camera,
- DC1394_FEATURE_GAIN,
- mode)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set auto gain. \n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set auto gain") );
- }
-}
-/*!
- Enable auto gain. If available set min and max gain values.
-
- \warning Setting min and max gain feature is only available
- for AVT cameras.
-
- \param minvalue : Min gain.
- \param maxvalue : Max gain.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoGain(1600*20-1, 1600*20); // Set gain min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the auto shutter.
-
- \sa setAutoGain(), getAutoGain()
-*/
-void
-vp1394TwoGrabber::setAutoGain(unsigned int minvalue, unsigned int maxvalue)
-{
- setAutoGain();
-
- if (dc1394_avt_set_auto_gain(camera, minvalue, maxvalue)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot set auto gain min and max values. Is the camera an AVT one?\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set auto gain min and max values") );
- }
-}
-
-/*!
- Get auto gain min and max values.
-
- \warning Getting min and max auto gain values is only available
- for AVT cameras.
-
- \param minvalue : Min gain.
- \param maxvalue : Max gain.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-*/
-void
-vp1394TwoGrabber::getAutoGain(unsigned int &minvalue, unsigned int &maxvalue)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- if (dc1394_avt_get_auto_gain(camera, &minvalue, &maxvalue)
- != DC1394_SUCCESS) {
- // vpERROR_TRACE("Cannot get auto gain min and max values. Is the camera an AVT one?\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot get auto gain min and max values") );
- }
-}
-
-
-/*!
-
- Setup camera capture using dma. A ring buffer is used for the
- capture. It's size can be set using setRingBufferSize().
-
- \param _switch : Camera capture switch:
- - DC1394_ON to start dma capture,
- - DC1394_OFF to stop camera capture.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- dma capture.
-
- \sa setRingBufferSize(), setVideoMode(), setFramerate()
-*/
-void
-vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- if (_switch == DC1394_ON) {
- //if (dc1394_capture_setup(camera, num_buffers) != DC1394_SUCCESS) {
- // To be compatible with libdc1394 svn 382 version
- if (dc1394_capture_setup(camera, num_buffers,
- DC1394_CAPTURE_FLAGS_DEFAULT) != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to setup camera capture-\n"
- "make sure that the video mode and framerate are "
- "supported by your camera.\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
- }
- }
- else { // _switch == DC1394_OFF
- dc1394error_t code = dc1394_capture_stop(camera);
-
- if (code != DC1394_SUCCESS && code != DC1394_CAPTURE_IS_NOT_SET) {
- vpERROR_TRACE("Unable to stop camera capture\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
- }
- }
-}
-
-
-/*!
-
- Setup camera transmission.
-
- \param _switch : Transmission switch:
- - DC1394_ON to start iso transmission,
- - DC1394_OFF to stop iso transmission.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the video mode.
-*/
-void
-vp1394TwoGrabber::setTransmission(dc1394switch_t _switch)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394switch_t status = DC1394_OFF;
-
- if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to get transmision status");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
- }
-
- // if (status!=_switch){
- // Start dma capture if halted
- if (dc1394_video_set_transmission(camera, _switch) != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to setup camera capture-\n"
- "make sure that the video mode and framerate are "
- "supported by your camera.\n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
- }
-
- if (_switch == DC1394_ON) {
- status = DC1394_OFF;
-
- int i = 0;
- while ( status == DC1394_OFF && i++ < 5 ) {
- usleep(50000);
- if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to get transmision status");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
- }
- }
- }
- // }
-}
-
-/*!
- Set the camera iso data transmission speed.
- Speeds over 400Mbps are only available in "B" mode.
-
- \param isospeed : Iso data transmission speed.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't set
- the iso speed transmission.
-
- Below you will find an example that shows how to grab images in
- Format7 with a transmission speed set to 800Mbps in 1394b mode.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g(false); // Don't reset the bus
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 );
- g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
- g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b
- while(1)
- g.acquire(I);
-#endif
-}
- \endcode
-*/
-void
-vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394operation_mode_t op_mode;
- dc1394speed_t speed;
-
- // 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") );
- }
-}
-
-/*!
- Exist only for compatibility with other grabbing devices.
-
- Call acquire(vpImage<unsigned char> &I)
-
- \param I : Image data structure (8 bits image)
-
- \sa acquire(vpImage<unsigned char> &I)
-
-*/
-void
-vp1394TwoGrabber::open(vpImage<unsigned char> &I)
-{
- open();
- acquire(I);
-}
-
-/*!
- Exist only for compatibility with other grabbing devices.
-
- Call acquire(vpImage<vpRGBa> &I)
-
- \param I : Image data structure (RGBa format)
-
- \sa acquire(vpImage<vpRGBa> &I)
-
-*/
-void
-vp1394TwoGrabber::open(vpImage<vpRGBa> &I)
-{
- open();
- acquire(I);
-}
-
-
-/*!
-
- Get an image from the active camera frame buffer. This buffer neads to be
- released by enqueue().
-
- \return Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images. The
- dequeue() ensure to get the last image, while the enqueue() frees
- the ring buffer to be sure that the next image is the last one.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vp1394TwoGrabber g;
- dc1394video_frame_t *frame;
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
- while(1) {
- frame = g.dequeue();
- // Current image is now in frame structure
- // Do your stuff
- g.enqueue(frame);
- }
-#endif
-}
- \endcode
-
- \sa enqueue()
-*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue()
-{
-
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- dc1394video_frame_t *frame = NULL;
-
- if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame)
- !=DC1394_SUCCESS) {
- vpERROR_TRACE ("Error: Failed to capture from camera %d\n", camera_id);
- }
-
- return frame;
-}
-
-/*!
-
- Get an image from the active camera frame buffer. This buffer neads to be
- released by enqueue().
-
- \param I : Image data structure (8 bits image).
-
- \return Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images. The
- dequeue() ensure to get the last image, while the enqueue() frees
- the ring buffer to be sure that the next image is the last one.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
- dc1394video_frame_t *frame;
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
- while(1) {
- frame = g.dequeue(I);
- // Current image is now in frame structure and in I
- // Do your stuff
- g.enqueue(frame);
- }
-#endif
-}
- \endcode
-
- \sa enqueue()
-*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I)
-{
- uint64_t timestamp;
- uint32_t id;
-
- dc1394video_frame_t *frame;
-
- frame = dequeue(I, timestamp, id);
-
- return frame;
-}
-
-/*!
-
- Get an image from the active camera frame buffer. This buffer neads to be
- released by enqueue().
-
- \param I : Image data structure (8 bits image).
-
- \param timestamp : The unix time in microseconds
- at which the frame was captured in the ring buffer.
-
- \param id : The frame position in the ring buffer.
-
- \return Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab images. The
- dequeue() ensure to get the last image, while the enqueue() frees
- the ring buffer to be sure that the next image is the last one.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
- dc1394video_frame_t *frame;
- uint64_t timestamp_us; // timestamp in us
- uint32_t id;
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
- while(1) {
- frame = g.dequeue(I, timestamp_us, id); // get the last image
- // Current image is now in frame structure and in I
- // Do your stuff
- g.enqueue(frame);
- }
-#endif
-}
- \endcode
-
- \sa enqueue()
-*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I,
- uint64_t ×tamp,
- uint32_t &id)
-{
-
- open();
-
- dc1394video_frame_t *frame;
-
- frame = dequeue();
-
- // Timeval data structure providing the unix time
- // [microseconds] at which the frame was captured in the ring buffer.
- timestamp = frame->timestamp;
- id = frame->id;
-
- this->width = frame->size[0];
- this->height = frame->size[1];
- unsigned int size = this->width * this->height;
-
- if ((I.getWidth() != this->width)||(I.getHeight() != this->height))
- I.resize(this->height, this->width);
-
- switch(frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- memcpy(I.bitmap, (unsigned char *) frame->image,
- size*sizeof(unsigned char));
- break;
- case DC1394_COLOR_CODING_MONO16:
- case DC1394_COLOR_CODING_RAW16:
- vpImageConvert::MONO16ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToGrey((unsigned char *) frame->image, I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
- };
-
- return frame;
-}
-
-/*!
-
- Get an image from the active camera frame buffer. This buffer neads to be
- released by enqueue().
-
- \param I : Image data structure (32 bits RGBa image).
-
- \return Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab color images. The
- dequeue() ensure to get the last image, while the enqueue() frees
- the ring buffer to be sure that the next image is the last one.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<vpRGBa> I;
- vp1394TwoGrabber g;
- dc1394video_frame_t *frame;
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
- while(1) {
- frame = g.dequeue(I);
- // Current image is now in frame structure and in I
- // Do your stuff
- g.enqueue(frame);
- }
-#endif
-}
- \endcode
-
- \sa enqueue()
-*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I)
-{
- uint64_t timestamp;
- uint32_t id;
-
- dc1394video_frame_t *frame;
-
- frame = dequeue(I, timestamp, id);
-
- return frame;
-}
-
-/*!
-
- Get an image from the active camera frame buffer. This buffer neads to be
- released by enqueue().
-
- \param I : Image data structure (32 bits RGBa image).
-
- \param timestamp : The unix time in microseconds
- at which the frame was captured in the ring buffer.
-
- \param id : The frame position in the ring buffer.
-
- \return Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- Below you will find an example that shows how to grab color images. The
- dequeue() ensure to get the last image, while the enqueue() frees
- the ring buffer to be sure that the next image is the last one.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<vpRGBa> I;
- vp1394TwoGrabber g;
- dc1394video_frame_t *frame;
- uint64_t timestamp_us; // timestamp in us
- uint32_t id;
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
- while(1) {
- frame = g.dequeue(I, timestamp_us, id); // get the last image
- // Current image is now in frame structure and in I
- // Do your stuff
- g.enqueue(frame);
- }
-#endif
-}
- \endcode
-
- \sa enqueue()
-*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I,
- uint64_t ×tamp,
- uint32_t &id)
-{
-
- open();
-
- dc1394video_frame_t *frame;
-
- frame = dequeue();
-
- // Timeval data structure providing the unix time
- // [microseconds] at which the frame was captured in the ring buffer.
- timestamp = frame->timestamp;
- id = frame->id;
-
- this->width = frame->size[0];
- this->height = frame->size[1];
- unsigned int size = this->width * this->height;
-
- if ((I.getWidth() != width)||(I.getHeight() != height))
- I.resize(height, width);
-
- switch (frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_MONO16:
- case DC1394_COLOR_CODING_RAW16:
- vpImageConvert::MONO16ToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
- };
-
- return frame;
-}
-
-/*!
- Release the frame buffer used by the active camera.
-
- \param frame : Pointer to the libdc1394-2.x image data structure.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \sa dequeue()
-*/
-void
-vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
-{
-
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- if (frame)
- dc1394_capture_enqueue(camera, frame);
-}
-
-
-/*!
- Acquire a grey level image from the active camera.
-
- \param I : Image data structure (8 bits image).
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus or if can't get camera settings.
-
- \exception vpFrameGrabberException::otherError : If format
- conversion to return a 8 bits image is not implemented.
-
- \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
-*/
-void
-vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
-{
- uint64_t timestamp;
- uint32_t id;
-
- dc1394video_frame_t *frame;
-
- frame = dequeue(I, timestamp, id);
- enqueue(frame);
-}
-
-/*!
- Acquire a grey level image from the active camera.
-
- \param I : Image data structure (8 bits image).
-
- \param timestamp : The unix time in microseconds
- at which the frame was captured in the ring buffer.
-
- \param id : The frame position in the ring buffer.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus or if can't get camera settings.
-
- \exception vpFrameGrabberException::otherError : If format
- conversion to return a 8 bits image is not implemented.
-
- \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
-*/
-void
-vp1394TwoGrabber::acquire(vpImage<unsigned char> &I,
- uint64_t ×tamp,
- uint32_t &id)
-{
- dc1394video_frame_t *frame;
-
- open();
- frame = dequeue(I, timestamp, id);
- enqueue(frame);
-}
-
-
-
-/*!
- Acquire a color image from the active camera.
-
- \param I : Image data structure (32 bits RGBa image).
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::otherError : If format
- conversion to return a RGBa bits image is not implemented.
-
- \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
-*/
-void
-vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
-{
- uint64_t timestamp;
- uint32_t id;
- dc1394video_frame_t *frame;
-
- open();
- frame = dequeue(I, timestamp, id);
- enqueue(frame);
-}
-
-/*!
- Acquire a color image from the active camera.
-
- \param I : Image data structure (32 bits RGBa image).
-
- \param timestamp : The unix time in microseconds
- at which the frame was captured in the ring buffer.
-
- \param id : The frame position in the ring buffer.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::otherError : If format
- conversion to return a RGBa bits image is not implemented.
-
- \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
-*/
-void
-vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I,
- uint64_t ×tamp,
- uint32_t &id)
-{
- dc1394video_frame_t *frame;
-
- open();
- frame = dequeue();
- // Timeval data structure providing the unix time
- // [microseconds] at which the frame was captured in the ring buffer.
- timestamp = frame->timestamp;
- id = frame->id;
-
- this->width = frame->size[0];
- this->height = frame->size[1];
- unsigned int size = this->width * this->height;
-
- if ((I.getWidth() != width)||(I.getHeight() != height))
- I.resize(height, width);
-
- switch (frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
- };
-
- enqueue(frame);
-
-}
-
-/*!
-
- Get the image width. It depends on the camera video mode setVideoMode(). The
- image size is only available after a call to open() or acquire().
-
- \param w : The image width, zero if the required camera is not available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \warning Has to be called after open() or acquire() to be sure that camera
- settings are send to the camera.
-
- \sa getHeight(), open(), acquire()
-
-*/
-void vp1394TwoGrabber::getWidth(unsigned int &w)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- w = this->width;
-}
-
-/*!
-
- Get the image width. It depends on the camera video mode setVideoMode(). The
- image size is only available after a call to open() or acquire().
-
- \return width : The image width, zero if the required camera is not available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \warning Has to be called after open() or acquire() to be sure that camera
- settings are send to the camera.
-
- \sa getHeight(), open(), acquire()
-
-*/
-unsigned int vp1394TwoGrabber::getWidth()
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- return this->width;
-}
-
-/*!
-
- Get the image height. It depends on the camera vide mode
- setVideoMode(). The image size is only available after a call to
- open() or acquire().
-
- \param h : The image height.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \warning Has to be called after open() or acquire() to be sure that camera
- settings are send to the camera.
-
- \sa getWidth()
-
-*/
-void vp1394TwoGrabber::getHeight(unsigned int &h)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- h = this->height;
-}
-/*!
-
- Get the image height. It depends on the camera vide mode
- setVideoMode(). The image size is only available after a call to
- open() or acquire().
-
- \return The image height.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \warning Has to be called after open() or acquire() to be sure that camera
- settings are send to the camera.
-
- \sa getWidth()
-
-*/
-unsigned int vp1394TwoGrabber::getHeight()
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- return this->height;
-}
-
-/*!
- Display camera information for the active camera.
-
- \sa setCamera()
-*/
-void
-vp1394TwoGrabber::printCameraInfo()
-{
- std::cout << "----------------------------------------------------------"
- << std::endl
- << "----- Information for camera " << camera_id
- << " -----" << std::endl
- << "----------------------------------------------------------" << std::endl;
-
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- dc1394_camera_print_info( camera, stdout);
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- dc1394_print_camera_info( camera);
-#endif
-
- dc1394featureset_t features;
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- if (dc1394_feature_get_all(camera, &features) != DC1394_SUCCESS)
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- if (dc1394_get_camera_feature_set(camera, &features) != DC1394_SUCCESS)
-#endif
- {
- close();
- vpERROR_TRACE("unable to get feature set for camera %d\n", camera_id);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Cannot get camera features") );
-
- } else {
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- dc1394_feature_print_all(&features, stdout);
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- dc1394_print_feature_set(&features);
-#endif
- }
- std::cout << "----------------------------------------------------------" << std::endl;
-}
-
-/*!
-
- Converts the video mode identifier into a string containing the description
- of the mode.
-
- \param videomode : The camera capture video mode.
-
- \return A string describing the mode, an empty string if the mode is not
- supported.
-
- \sa string2videoMode()
-*/
-std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode)
-{
- std::string _str = "";
- dc1394video_mode_t _videomode = (dc1394video_mode_t) videomode;
-
- if ((_videomode >= DC1394_VIDEO_MODE_MIN)
- && (_videomode <= DC1394_VIDEO_MODE_MAX)) {
- _str = strVideoMode[_videomode - DC1394_VIDEO_MODE_MIN];
- }
- else {
- vpCERROR << "The video mode " << (int)videomode
- << " is not supported by the camera" << std::endl;
- }
-
- return _str;
-}
-
-/*!
-
- Converts the framerate identifier into a string containing the description
- of the framerate.
-
- \param fps : The camera capture framerate.
-
- \return A string describing the framerate, an empty string if the framerate
- is not supported.
-
- \sa string2framerate()
-*/
-std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
-{
- std::string _str = "";
- dc1394framerate_t _fps = (dc1394framerate_t) fps;
-
- if ((_fps >= DC1394_FRAMERATE_MIN)
- && (_fps <= DC1394_FRAMERATE_MAX)) {
- _str = strFramerate[_fps - DC1394_FRAMERATE_MIN];
- }
- else {
- vpCERROR << "The framerate " << (int)fps
- << " is not supported by the camera" << std::endl;
- }
-
- return _str;
-}
-
-/*!
-
- Converts the color coding identifier into a string containing the description
- of the color coding.
-
- \param colorcoding : The color coding format.
-
- \return A string describing the color coding, an empty string if the
- color coding is not supported.
-
- \sa string2colorCoding()
-*/
-std::string vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding)
-{
- std::string _str = "";
- dc1394color_coding_t _coding = (dc1394color_coding_t) colorcoding;
-
- if ((_coding >= DC1394_COLOR_CODING_MIN)
- && (_coding <= DC1394_COLOR_CODING_MAX)) {
- _str = strColorCoding[_coding - DC1394_COLOR_CODING_MIN];
-
- }
- else {
- vpCERROR << "The color coding " << (int)colorcoding
- << " is not supported by the camera" << std::endl;
- }
-
- return _str;
-}
-
-/*!
-
- Converts the string containing the description of the vide mode into
- the video mode identifier.
-
- \param videomode : The string describing the video mode.
-
- \return The camera capture video mode identifier.
-
- \exception vpFrameGrabberException::settingError : If the required videomode
- is not valid.
-
- This method returns 0 if the string does not match to a video mode string.
-
- \sa videoMode2string()
-
-*/
-vp1394TwoGrabber::vp1394TwoVideoModeType
-vp1394TwoGrabber::string2videoMode(std::string videomode)
-{
- vp1394TwoVideoModeType _id;
-
- for (int i = DC1394_VIDEO_MODE_MIN; i <= DC1394_VIDEO_MODE_MAX; i ++) {
- _id = (vp1394TwoVideoModeType) i;
- if (videomode.compare(videoMode2string(_id)) == 0)
- return _id;
- };
-
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required videomode is not valid") );
-
- return (vp1394TwoVideoModeType) 0;
-}
-
-
-/*!
-
- Converts the string containing the description of the framerate into the
- framerate identifier.
-
- \param framerate : The string describing the framerate.
-
- \return The camera capture framerate identifier.
-
- \exception vpFrameGrabberException::settingError : If the required framerate
- is not valid.
-
- This method returns 0 if the string does not match to a framerate string.
-
- \sa framerate2string()
-
-*/
-vp1394TwoGrabber::vp1394TwoFramerateType
-vp1394TwoGrabber::string2framerate(std::string framerate)
-{
- vp1394TwoFramerateType _id;
-
- for (int i = DC1394_FRAMERATE_MIN; i <= DC1394_FRAMERATE_MAX; i ++) {
- _id = (vp1394TwoFramerateType) i;
- if (framerate.compare(framerate2string(_id)) == 0)
- return _id;
- };
-
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required framerate is not valid") );
-
- return (vp1394TwoFramerateType) 0;
-}
-
-/*!
-
- Converts the string containing the description of the color coding into the
- color coding identifier.
-
- \param colorcoding : The string describing the color coding format.
-
- \return The camera capture color coding identifier.
-
- \exception vpFrameGrabberException::settingError : If the required
- color coding is not valid.
-
- This method returns 0 if the string does not match to a color coding string.
-
- \sa colorCoding2string()
-
-*/
-vp1394TwoGrabber::vp1394TwoColorCodingType
-vp1394TwoGrabber::string2colorCoding(std::string colorcoding)
-{
- vp1394TwoColorCodingType _id;
-
- for (int i = DC1394_COLOR_CODING_MIN; i <= DC1394_COLOR_CODING_MAX; i ++) {
- _id = (vp1394TwoColorCodingType) i;
- if (colorcoding.compare(colorCoding2string(_id)) == 0)
- return _id;
- };
-
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required color coding is not valid") );
-
- return (vp1394TwoColorCodingType) 0;
-}
-
-/*!
- Resets the IEEE1394 bus which camera is attached to. Calling this function is
- "rude" to other devices because it causes them to re-enumerate on the bus and
- may cause a temporary disruption in their current activities. Thus, use it
- sparingly. Its primary use is if a program shuts down uncleanly and needs to
- free leftover ISO channels or bandwidth. A bus reset will free those things
- as a side effect.
-
- The example below shows how to reset the bus attached to the last
- camera found.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- unsigned int ncameras; // Number of cameras on the bus
- vp1394TwoGrabber g;
- ncameras = g.getNumCameras();
- g.setCamera(ncameras-1); // To dial with the last camera on the bus
- g.resetBus(); // Reset the bus attached to "ncameras-1"
-#endif
-}
- \endcode
-
- \exception vpFrameGrabberException::initializationError : If no
- camera is found.
-
-*/
-void vp1394TwoGrabber::resetBus()
-{
- for (unsigned int i = 0; i < num_cameras;i++) {
- if (camIsOpen[i]) {
- camera = cameras[i];
- setTransmission(DC1394_OFF);
- setCapture(DC1394_OFF);
- }
- }
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- setCamera(camera_id);
- // free the other cameras
- for (unsigned int i=0;i<num_cameras;i++){
- if (i!=camera_id) dc1394_camera_free(cameras[i]);
- }
-
- printf ("Reseting bus...\n");
- dc1394_reset_bus (camera);
-
- dc1394_camera_free (camera);
- dc1394_free (d);
- d = NULL;
- //if (cameras != NULL)
- delete [] cameras;
- cameras = NULL ;
-#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
-
- setCamera(camera_id);
- // free the other cameras
- for (unsigned int i=0;i<num_cameras;i++){
- if (i!=camera_id) dc1394_free_camera(cameras[i]);
- }
- free(cameras);
- cameras = NULL;
-
- dc1394_reset_bus(camera);
- dc1394_free_camera(camera);
-
-#endif
- if (camIsOpen != NULL)
- delete [] camIsOpen;
- camIsOpen = NULL ;
-
- num_cameras = 0;
-
- init = false;
- vpTime::wait(1000);
- initialize(false);
-}
-
-/*!
-
- This method is useful for controlling single or multiple image
- transmission from stereo vision cameras.
-
- The PAN register 0x884 used to control which images are transmitted
- is set to the corresponding \e panControlValue value.
-
- \param panControlValue : Value used to set the PAN register 0x884.
- The Format_7 Mode 0 transmits images from one camera. When set to 0,
- transmit right image. When set to 1, transmit left (Bumblebee2) or
- center (Bumblebee XB3) image and when set to 2 or higher transmit
- left image. The Format_7 Mode 3 allows to transmit two images
- simultaneously. If your camera has two sensors, you can use the PAN
- register to control the order of the images when
- transmitting. Writing a value of 0 to this register transmits images
- in right-left format. A value of 1 transmits images in left-right
- format. If your camera has three sensors, such as the Bumblebee XB3,
- you can adjust the PAN register as follows to specify which two
- sensors are capturing images. When \e panControlValue is set to 0,
- transmit right-left images, when set to 1, transmit right-center
- images.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If the register was not set.
-
- */
-void vp1394TwoGrabber::setPanControl(unsigned int panControlValue)
-{
- open();
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- uint64_t offset = 0x884;
- uint32_t value = 0x82000000 + (uint32_t)panControlValue;
- dc1394error_t err;
- err = dc1394_set_control_register(camera, offset, value);
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to set PAN register");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to set PAN register") );
- }
-}
-
-
-/*!
-
- This method get the value of one of the parameters of the camera.
-
- \param param : The parameter expressing the value to return
-
- \return the parameter's value
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : if the parameter cannot be
- retrieved.
-
- \sa setParameterValue()
-*/
-unsigned int vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- uint32_t value;
- dc1394feature_t feature = (dc1394feature_t)param;
- dc1394error_t err;
- err = dc1394_feature_get_value(camera, feature, &value);
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to get the information");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to get the information") );
- }
- return (unsigned int)value;
-}
-
-
-/*!
-
- This method set the value of one of the parameters of the camera. The initial
- value of the parameter is recorded and reset when the destructor is called.
-
- \warning If the program crashes and the destructor is not called, then the
- modified parameters will remain in the camera.
-
- \param param : The parameters to modify
- \param val : the new value of this parameter
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError if a manual mode is not
- available for the parameter, an exception is thrown.
-
- \sa getParameterValue()
-
-*/
-void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param,
- unsigned int val)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- uint32_t value = (uint32_t)val;
- dc1394feature_t feature = (dc1394feature_t)param;
- dc1394error_t err;
- dc1394bool_t hasManualMode = DC1394_FALSE;
- dc1394feature_modes_t modesAvailable;
-
- // test wether we can set the shutter value (manual mode available or not)
- err = dc1394_feature_get_modes(camera, feature, &modesAvailable);
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to detect the manual mode information");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to detect the manual mode information"));
- }
-
- for(unsigned int i=0; i<modesAvailable.num; i++){
- if(modesAvailable.modes[i] == DC1394_FEATURE_MODE_MANUAL){
- hasManualMode = DC1394_TRUE;
- }
- }
-
- if(hasManualMode == DC1394_TRUE){
-
- if(!isDataModified[camera_id]){// to ensure we save the first mode even after several set
- /* we update the structure */
- updateDataCamToStruct();
- err = dc1394_feature_get_mode(camera, feature, &(initialShutterMode[camera_id]));
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to get the initial mode");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to get the initial mode"));
- }
- isDataModified[camera_id] = true;
- }
-
- dc1394feature_mode_t manualMode = DC1394_FEATURE_MODE_MANUAL;
- err = dc1394_feature_set_mode(camera, feature, manualMode);
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to set the muanual mode");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to set the manual mode") );
- }
- err = dc1394_feature_set_value(camera, feature, value);
- if (err != DC1394_SUCCESS) {
- vpERROR_TRACE("Unable to set the shutter information");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to set the shutter information") );
- }
- }
- else{
- vpERROR_TRACE("The camera does not have a manual mode.\nCannot change the value");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The camera does not have a manual mode"));
- }
-}
-/*!
-
- Query the actual camera GUID.
-
- \sa setCamera(), getCamera()
-
-*/
-void
-vp1394TwoGrabber::getGuid(uint64_t & guid)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- guid = camera->guid;
-}
-
-/*!
-
- Return the actual camera GUID.
-
- \sa setCamera(), getCamera()
-
-*/
-uint64_t
-vp1394TwoGrabber::getGuid()
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- return camera->guid;
-}
-
-
-/*!
- update the data structure used to record the value of the current camera.
-
-*/
-inline void
-vp1394TwoGrabber::updateDataCamToStruct()
-{
- dataCam[camera_id].brightness = getParameterValue(vpFEATURE_BRIGHTNESS);
- dataCam[camera_id].exposure = getParameterValue(vpFEATURE_EXPOSURE);
- dataCam[camera_id].sharpness = getParameterValue(vpFEATURE_SHARPNESS);
- dataCam[camera_id].hue = getParameterValue(vpFEATURE_HUE);
- dataCam[camera_id].saturation = getParameterValue(vpFEATURE_SATURATION);
- dataCam[camera_id].gamma = getParameterValue(vpFEATURE_GAMMA);
- dataCam[camera_id].shutter = getParameterValue(vpFEATURE_SHUTTER);
- dataCam[camera_id].gain = getParameterValue(vpFEATURE_GAIN);
- dataCam[camera_id].iris = getParameterValue(vpFEATURE_IRIS);
-}
-
-/*!
- set the values of several parameters of the current camera with the value
- previously recorded.
-*/
-inline void
-vp1394TwoGrabber::updateDataStructToCam()
-{
- setParameterValue(vpFEATURE_BRIGHTNESS, dataCam[camera_id].brightness);
- setParameterValue(vpFEATURE_EXPOSURE, dataCam[camera_id].exposure);
- setParameterValue(vpFEATURE_SHARPNESS, dataCam[camera_id].sharpness);
- setParameterValue(vpFEATURE_HUE, dataCam[camera_id].hue);
- setParameterValue(vpFEATURE_SATURATION, dataCam[camera_id].saturation);
- setParameterValue(vpFEATURE_GAMMA, dataCam[camera_id].gamma);
- setParameterValue(vpFEATURE_SHUTTER, dataCam[camera_id].shutter);
- setParameterValue(vpFEATURE_GAIN, dataCam[camera_id].gain);
- setParameterValue(vpFEATURE_IRIS, dataCam[camera_id].iris);
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
-
- \deprecated This method is deprecated. You should use getVideoModeSupported(std::list<vp1394TwoVideoModeType> &) instead.
-
- Query the available active camera video modes.
-
- \param videomodes : The list of supported camera video modes.
-
- \return The number of supported camera modes, 0 if an error occurs.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- video modes.
-
- \sa setVideoMode(), getVideoMode(), getCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getVideoModeSupported(vpList<vp1394TwoVideoModeType> & videomodes)
-{
- // Refresh the list of supported modes
- videomodes.kill();
-
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
- dc1394video_modes_t _videomodes;
-
- // get video modes:
- if (dc1394_video_get_supported_modes(camera, &_videomodes)!=DC1394_SUCCESS) {
-
- close();
- vpERROR_TRACE("Can't get video modes");
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Can't get video modes") );
- }
-
- // parse the video modes to add in the list
- for (unsigned i=0; i < _videomodes.num; i++) {
- vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType) _videomodes.modes[i];
- videomodes.addRight( _mode );
- }
-
- // return the number of available video modes
- return _videomodes.num;
-}
-
-/*!
- \deprecated This method is deprecated. You should use getFramerateSupported(vp1394TwoVideoModeType, std::list<vp1394TwoFramerateType> &) instead.
-
- Query the available framerates for the given camera video mode (see
- file dc1394/control.h). No framerate is associated to the following
- camera modes :
-
- - vp1394TwoGrabber::vpVIDEO_MODE_EXIF (format 6),
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 (format 7):
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_1 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_2 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_3 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_4 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_5 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_6 (format 7)
- - vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_7 (format 7)
-
- \param mode : Camera video mode.
-
- \param fps : The list of supported camera framerates for the given camera
- video mode.
-
- \return The number of supported framerates, 0 if no framerate is available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the supported framerates.
-
- \sa setFramerate(), getFramerate(), setCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
- vpList<vp1394TwoFramerateType> & fps)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- // Refresh the list of supported framerates
- fps.kill();
-
- switch (mode) {
- // Framerate not available for:
- // - vpVIDEO_MODE_EXIF ie Format_6
- // - vpVIDEO_MODE_FORMAT7... ie the Format_7
- case vpVIDEO_MODE_EXIF:
- case vpVIDEO_MODE_FORMAT7_0:
- case vpVIDEO_MODE_FORMAT7_1:
- case vpVIDEO_MODE_FORMAT7_2:
- case vpVIDEO_MODE_FORMAT7_3:
- case vpVIDEO_MODE_FORMAT7_4:
- case vpVIDEO_MODE_FORMAT7_5:
- case vpVIDEO_MODE_FORMAT7_6:
- case vpVIDEO_MODE_FORMAT7_7:
- return 0;
- break;
- default:
- {
- dc1394framerates_t _fps;
- if (dc1394_video_get_supported_framerates(camera,
- (dc1394video_mode_t)mode,
- &_fps) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported frametates for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported framerates") );
- }
- if (_fps.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _fps.num; i ++)
- fps.addRight((vp1394TwoFramerateType)_fps.framerates[i]);
-
- return _fps.num;
- }
- break;
- }
-}
-
-/*!
-
- \deprecated This method is deprecated. You should use
- getColorCodingSupported(vp1394TwoVideoModeType, std::list<vp1394TwoColorCodingType> &) instead.
-
- Query the available color codings for the given camera video mode (see
- file dc1394/control.h).
-
- \param mode : Camera video mode.
-
- \param codings : The list of supported color codings for the given camera
- video mode.
-
- \return The number of supported color codings, 0 if no color codings
- is available.
-
- \exception vpFrameGrabberException::initializationError : If no
- camera found on the bus.
-
- \exception vpFrameGrabberException::settingError : If we can't get
- the color codingss.
-
- \sa setColorCoding(), getColorCoding(), setCamera()
-*/
-uint32_t
-vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
- vpList<vp1394TwoColorCodingType> & codings)
-{
- if (! num_cameras) {
- close();
- vpERROR_TRACE("No camera found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
- }
-
- // Refresh the list of supported framerates
- codings.kill();
-
- if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
- // Format 7 video mode
- dc1394color_codings_t _codings;
- if (dc1394_format7_get_color_codings(camera,
- (dc1394video_mode_t)mode,
- &_codings) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color codings for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color codings") );
- }
- if (_codings.num == 0)
- return 0;
-
- for (unsigned int i = 0; i < _codings.num; i ++)
- codings.addRight((vp1394TwoColorCodingType)_codings.codings[i]);
-
- return _codings.num;
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
- // Format 6 video mode
- return 0;
- }
- else {
- // Not Format 7 and not Format 6 video modes
- dc1394color_coding_t _coding;
- if (dc1394_get_color_coding_from_video_mode(camera,
- (dc1394video_mode_t)mode,
- &_coding) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported color coding for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported color coding") );
- }
- codings.addRight((vp1394TwoColorCodingType)_coding);
- return 1;
- }
-}
-#endif
-
-#endif
-
diff --git a/src/device/framegrabber/1394/vp1394TwoGrabber.h b/src/device/framegrabber/1394/vp1394TwoGrabber.h
deleted file mode 100644
index 4422023..0000000
--- a/src/device/framegrabber/1394/vp1394TwoGrabber.h
+++ /dev/null
@@ -1,473 +0,0 @@
-/****************************************************************************
- *
- * $Id: vp1394TwoGrabber.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vp1394TwoGrabber.h
- \brief class for firewire cameras video capture using libdc1394-2.x api
-
- \warning This class needs libdc1394-2 and
- libraw1394 (version greater than 1.1.0). These libraries are available from
- http://sourceforge.net/projects/libdc1394 and
- http://sourceforge.net/projects/libraw1394 .
-
- vp1394TwoGrabber was tested with libdc1394-2.0.0 (svn version) and
- with the following cameras:
- - Marlin F033C
- - Marlin F131B
- - PointGrey Dragonfly 2
- - APPLE iSight extern webcam
-
-*/
-
-#ifndef vp1394TwoGrabber_h
-#define vp1394TwoGrabber_h
-
-#include <visp/vpConfig.h>
-
-#include <list>
-
-#if defined(VISP_HAVE_DC1394_2)
-
-
-/*!
- \class vp1394TwoGrabber
-
- \ingroup Framegrabber CameraDriver
-
- \brief Class for firewire ieee1394 video devices using libdc1394-2.x api.
-
-
- Needs libraw1394-1.2.0 and libdc1394-2.0.0 or more recent versions
- available on http://sourceforge.net.
-
- This class was tested with Marlin F033C and F131B cameras and with
- Point Grey Dragonfly 2, Flea 2 and Flea 3 cameras.
-
- \ingroup libdevice
-
- This grabber allows single or multi camera acquisition.
-
- - Here you will find an example of single capture from the first camera found
- on the bus. This example is available in tutorial-grabber-1394.cpp:
- \include tutorial-grabber-1394.cpp
- A line by line explanation of this example is provided in \ref tutorial-grabber.
- An other example that shows how to use format 7
- and the auto-shutter is provided in vp1394TwoGrabber() constructor:
-
- - If more than one camera is connected, it is also possible to select a specific camera by its GUID:
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I; // Create a gray level image container
- bool reset = false; // Disable bus reset during construction
- vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
-
- unsigned int ncameras; // Number of cameras on the bus
- ncameras = g.getNumCameras();
- std::cout << ncameras << " cameras found:" << std::endl;
-
- for(unsigned int i=0; i< ncameras; i++)
- {
- g.setCamera(i);
- uint64_t guid = g.getGuid();
- printf("camera %d with guid 0x%lx\n", i, (long unsigned int)guid);
- }
-
- // produce:
- // 2 cameras found:
- // camera 0 with guid 0xb09d01009b329c
- // camera 1 with guid 0xb09d01007e0ee7
- g.setCamera( (uint64_t)0xb09d01009b329cULL );
-
- printf("Use camera with GUID: 0x%lx\n", (long unsigned int)g.getGuid());
- g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
-
- vpImageIo::write(I, "image.pgm"); // Write image on the disk
-#endif
-}
- \endcode
-
- - Here an example of multi camera capture. An other example is available in setCamera():
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- bool reset = false; // Disable bus reset during construction
- vp1394TwoGrabber g(reset); // Creation of a grabber instance based on libdc1394-2.x third party lib.
- unsigned int ncameras; // Number of cameras on the bus
- ncameras = g.getNumCameras();
-
- // Create an image container for each camera
- vpImage<unsigned char> *I = new vpImage<unsigned char> [ncameras];
- char filename[FILENAME_MAX];
-
- // If the first camera supports vpVIDEO_MODE_640x480_YUV422 video mode
- g.setCamera(0);
- g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_YUV422);
-
- // If the second camera support 30 fps acquisition
- g.setCamera(1);
- g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
-
- // Acquire an image from each camera
- for (unsigned int camera=0; camera < ncameras; camera ++) {
- g.setCamera(camera);
- g.acquire(I[camera]);
- sprintf(filename, "image-cam%d.pgm", camera);
- vpImageIo::write(I[camera], filename);
- }
- delete [] I;
-#endif
-}
- \endcode
-
- \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
-
-*/
-
-/*
- * Interface with libdc1394 2.x
- */
-#include <string.h>
-
-#include <dc1394/control.h>
-#include <dc1394/utils.h>
-#include <dc1394/vendor/avt.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpRGBa.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-
-class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
-{
-
- public:
- static const char * strVideoMode[DC1394_VIDEO_MODE_NUM];
- static const char * strFramerate[DC1394_FRAMERATE_NUM];
- static const char * strColorCoding[DC1394_COLOR_CODING_NUM];
-
- /*!
- Enumeration of iso speed. See libdc1394 2.x header file
- dc1394/control.h
- */
- typedef enum {
- vpISO_SPEED_100 = DC1394_ISO_SPEED_100,
- vpISO_SPEED_200 = DC1394_ISO_SPEED_200,
- vpISO_SPEED_400 = DC1394_ISO_SPEED_400,
- vpISO_SPEED_800 = DC1394_ISO_SPEED_800,
- vpISO_SPEED_1600 = DC1394_ISO_SPEED_1600,
- vpISO_SPEED_3200 = DC1394_ISO_SPEED_3200
- } vp1394TwoIsoSpeedType;
-
- /*!
- Enumeration of video modes. See libdc1394 2.x header file
- dc1394/control.h
- */
- typedef enum {
- vpVIDEO_MODE_160x120_YUV444 = DC1394_VIDEO_MODE_160x120_YUV444,
- vpVIDEO_MODE_320x240_YUV422 = DC1394_VIDEO_MODE_320x240_YUV422,
- vpVIDEO_MODE_640x480_YUV411 = DC1394_VIDEO_MODE_640x480_YUV411,
- vpVIDEO_MODE_640x480_YUV422 = DC1394_VIDEO_MODE_640x480_YUV422,
- vpVIDEO_MODE_640x480_RGB8 = DC1394_VIDEO_MODE_640x480_RGB8,
- vpVIDEO_MODE_640x480_MONO8 = DC1394_VIDEO_MODE_640x480_MONO8,
- vpVIDEO_MODE_640x480_MONO16 = DC1394_VIDEO_MODE_640x480_MONO16,
- vpVIDEO_MODE_800x600_YUV422 = DC1394_VIDEO_MODE_800x600_YUV422,
- vpVIDEO_MODE_800x600_RGB8 = DC1394_VIDEO_MODE_800x600_RGB8,
- vpVIDEO_MODE_800x600_MONO8 = DC1394_VIDEO_MODE_800x600_MONO8,
- vpVIDEO_MODE_1024x768_YUV422 = DC1394_VIDEO_MODE_1024x768_YUV422,
- vpVIDEO_MODE_1024x768_RGB8 = DC1394_VIDEO_MODE_1024x768_RGB8,
- vpVIDEO_MODE_1024x768_MONO8 = DC1394_VIDEO_MODE_1024x768_MONO8,
- vpVIDEO_MODE_800x600_MONO16 = DC1394_VIDEO_MODE_800x600_MONO16,
- vpVIDEO_MODE_1024x768_MONO16 = DC1394_VIDEO_MODE_1024x768_MONO16,
- vpVIDEO_MODE_1280x960_YUV422 = DC1394_VIDEO_MODE_1280x960_YUV422,
- vpVIDEO_MODE_1280x960_RGB8 = DC1394_VIDEO_MODE_1280x960_RGB8,
- vpVIDEO_MODE_1280x960_MONO8 = DC1394_VIDEO_MODE_1280x960_MONO8,
- vpVIDEO_MODE_1600x1200_YUV422 = DC1394_VIDEO_MODE_1600x1200_YUV422,
- vpVIDEO_MODE_1600x1200_RGB8 = DC1394_VIDEO_MODE_1600x1200_RGB8,
- vpVIDEO_MODE_1600x1200_MONO8 = DC1394_VIDEO_MODE_1600x1200_MONO8,
- vpVIDEO_MODE_1280x960_MONO16 = DC1394_VIDEO_MODE_1280x960_MONO16,
- vpVIDEO_MODE_1600x1200_MONO16 = DC1394_VIDEO_MODE_1600x1200_MONO16,
- vpVIDEO_MODE_EXIF = DC1394_VIDEO_MODE_EXIF,
- vpVIDEO_MODE_FORMAT7_0 = DC1394_VIDEO_MODE_FORMAT7_0,
- vpVIDEO_MODE_FORMAT7_1 = DC1394_VIDEO_MODE_FORMAT7_1,
- vpVIDEO_MODE_FORMAT7_2 = DC1394_VIDEO_MODE_FORMAT7_2,
- vpVIDEO_MODE_FORMAT7_3 = DC1394_VIDEO_MODE_FORMAT7_3,
- vpVIDEO_MODE_FORMAT7_4 = DC1394_VIDEO_MODE_FORMAT7_4,
- vpVIDEO_MODE_FORMAT7_5 = DC1394_VIDEO_MODE_FORMAT7_5,
- vpVIDEO_MODE_FORMAT7_6 = DC1394_VIDEO_MODE_FORMAT7_6,
- vpVIDEO_MODE_FORMAT7_7 = DC1394_VIDEO_MODE_FORMAT7_7
- } vp1394TwoVideoModeType;
-
- /*!
- Enumeration of framerates. See libdc1394 2.x header file
- dc1394/control.h
- */
- typedef enum {
- vpFRAMERATE_1_875 = DC1394_FRAMERATE_1_875,
- vpFRAMERATE_3_75 = DC1394_FRAMERATE_3_75,
- vpFRAMERATE_7_5 = DC1394_FRAMERATE_7_5,
- vpFRAMERATE_15 = DC1394_FRAMERATE_15,
- vpFRAMERATE_30 = DC1394_FRAMERATE_30,
- vpFRAMERATE_60 = DC1394_FRAMERATE_60,
- vpFRAMERATE_120 = DC1394_FRAMERATE_120,
- vpFRAMERATE_240 = DC1394_FRAMERATE_240
- } vp1394TwoFramerateType;
-
- /*!
- Enumeration of color codings. See libdc1394 2.x header file
- dc1394/control.h
- */
- typedef enum {
- vpCOLOR_CODING_MONO8 = DC1394_COLOR_CODING_MONO8,
- vpCOLOR_CODING_YUV411 = DC1394_COLOR_CODING_YUV411,
- vpCOLOR_CODING_YUV422 = DC1394_COLOR_CODING_YUV422,
- vpCOLOR_CODING_YUV444 = DC1394_COLOR_CODING_YUV444,
- vpCOLOR_CODING_RGB8 = DC1394_COLOR_CODING_RGB8,
- vpCOLOR_CODING_MONO16 = DC1394_COLOR_CODING_MONO16,
- vpCOLOR_CODING_RGB16 = DC1394_COLOR_CODING_RGB16,
- vpCOLOR_CODING_MONO16S = DC1394_COLOR_CODING_MONO16S,
- vpCOLOR_CODING_RGB16S = DC1394_COLOR_CODING_RGB16S,
- vpCOLOR_CODING_RAW8 = DC1394_COLOR_CODING_RAW8,
- vpCOLOR_CODING_RAW16 = DC1394_COLOR_CODING_RAW16
- } vp1394TwoColorCodingType;
-
- /*!
- Enumeration of the parameters that can be modified. See libdc1394 2.x header
- file dc1394/control.h
- */
- typedef enum {
- vpFEATURE_BRIGHTNESS = DC1394_FEATURE_BRIGHTNESS,
- vpFEATURE_EXPOSURE = DC1394_FEATURE_EXPOSURE,
- vpFEATURE_SHARPNESS = DC1394_FEATURE_SHARPNESS,
-// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
- vpFEATURE_HUE = DC1394_FEATURE_HUE,
- vpFEATURE_SATURATION = DC1394_FEATURE_SATURATION,
- vpFEATURE_GAMMA = DC1394_FEATURE_GAMMA,
- vpFEATURE_SHUTTER = DC1394_FEATURE_SHUTTER,
- vpFEATURE_GAIN = DC1394_FEATURE_GAIN,
- vpFEATURE_IRIS = DC1394_FEATURE_IRIS,
-// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
-// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
-// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
-// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
-// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
-// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
-// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
-// vpFEATURE_PAN = DC1394_FEATURE_PAN,
-// vpFEATURE_TILT = DC1394_FEATURE_TILT,
-// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
-// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
-// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
- } vp1394TwoParametersType;
-
-
- private:
-
- /*!
- Control structure of the values that can be modified during the execution.
- */
- typedef struct{
- uint32_t brightness ;
- uint32_t exposure;
- uint32_t sharpness;
-// uint32_t whiteBalance;
- uint32_t hue;
- uint32_t saturation;
- uint32_t gamma;
- uint32_t shutter ;
- uint32_t gain ;
- uint32_t iris;
-// uint32_t focus;
-// uint32_t temperature ;
-// uint32_t trigger ;
-// uint32_t triggerDelay ;
-// uint32_t whiteShadding ;
-// uint32_t frameRate ;
-// uint32_t zoom;
-// uint32_t pan;
-// uint32_t tilt ;
-// uint32_t opticalFilter ;
-// uint32_t captureSize;
-// uint32_t captureQuality ;
- } vpDc1394TwoCameraParametersData;
-
-
- public:
- vp1394TwoGrabber(bool reset=true);
- virtual ~vp1394TwoGrabber();
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id);
- void acquire(vpImage<vpRGBa> &I);
- void acquire(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id);
-
- void close();
-
- static std::string colorCoding2string(vp1394TwoColorCodingType colorcoding);
-
- dc1394video_frame_t *dequeue();
- dc1394video_frame_t *dequeue(vpImage<unsigned char> &I);
- dc1394video_frame_t *dequeue(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id);
- dc1394video_frame_t *dequeue(vpImage<vpRGBa> &I);
- dc1394video_frame_t *dequeue(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id);
- void enqueue(dc1394video_frame_t *frame);
-
- static std::string framerate2string(vp1394TwoFramerateType fps);
-
- void getAutoGain(unsigned int &minvalue, unsigned int &maxvalue);
- void getAutoShutter(unsigned int &minvalue, unsigned int &maxvalue);
-
- uint64_t getCamera();
- void getCamera(uint64_t &camera);
- void getColorCoding(vp1394TwoColorCodingType & coding);
- uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode,
- std::list<vp1394TwoColorCodingType> & codings);
- void getFramerate(vp1394TwoFramerateType & fps);
- uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode,
- std::list<vp1394TwoFramerateType> & fps);
- uint64_t getGuid();
- void getGuid(uint64_t &guid);
- void getHeight(unsigned int &height);
- unsigned int getHeight();
- void getNumCameras(unsigned int &ncameras) const;
- unsigned int getNumCameras() const;
- unsigned int getParameterValue(vp1394TwoParametersType param);
- unsigned int getRingBufferSize() const;
- void getVideoMode(vp1394TwoVideoModeType & videomode);
- uint32_t getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes);
- void getWidth(unsigned int &width);
- unsigned int getWidth();
-
- bool isColor();
- bool isColorCodingSupported(vp1394TwoVideoModeType videomode,
- vp1394TwoColorCodingType coding);
- bool isFramerateSupported(vp1394TwoVideoModeType videomode,
- vp1394TwoFramerateType fps);
- bool isVideoModeSupported(vp1394TwoVideoModeType videomode) ;
- bool isVideoModeFormat7(vp1394TwoVideoModeType videomode);
-
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
-
- void printCameraInfo();
-
- void resetBus();
-
- void setAutoGain(bool enable = true);
- void setAutoGain(unsigned int minvalue, unsigned int maxvalue);
- void setAutoShutter(bool enable = true);
- void setAutoShutter(unsigned int minvalue, unsigned int maxvalue);
- void setCamera(uint64_t camera);
- void setColorCoding(vp1394TwoColorCodingType coding);
- void setFormat7ROI(unsigned int left=0, unsigned int top=0,
- unsigned int width=0, unsigned int height=0);
- void setFramerate(vp1394TwoFramerateType fps);
- void setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed);
- void setPanControl(unsigned int panControlValue);
- void setParameterValue(vp1394TwoParametersType param, unsigned int val);
- void setRingBufferSize(unsigned int size);
- void setVideoMode(vp1394TwoVideoModeType videomode);
-
- static vp1394TwoVideoModeType string2videoMode(std::string videomode);
- static vp1394TwoFramerateType string2framerate(std::string fps);
- static vp1394TwoColorCodingType string2colorCoding(std::string colorcoding);
-
- static std::string videoMode2string(vp1394TwoVideoModeType videomode);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode,
- vpList<vp1394TwoColorCodingType> & codings);
- vp_deprecated uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode,
- vpList<vp1394TwoFramerateType> & fps);
- vp_deprecated uint32_t getVideoModeSupported(vpList<vp1394TwoVideoModeType> & videomodes);
-
-#endif
-
- private:
- void open();
- void initialize(bool reset);
- void setCapture(dc1394switch_t _switch);
- void setTransmission(dc1394switch_t _switch);
- inline void updateDataCamToStruct();
- inline void updateDataStructToCam();
-
- private:
- dc1394camera_t *camera, **cameras;
- unsigned int num_cameras;
- unsigned int camera_id;
-
- bool verbose;
- bool *camIsOpen;
- unsigned int num_buffers;
-
- /* parameters for the cameras */
- bool* isDataModified;
- dc1394feature_mode_t* initialShutterMode;
- vpDc1394TwoCameraParametersData* dataCam;
-
-#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE
- dc1394_t * d;
- dc1394camera_list_t * list;
-#endif
-};
-
-
-#endif
-#endif
-
diff --git a/src/device/framegrabber/OpenCV/vpOpenCVGrabber.cpp b/src/device/framegrabber/OpenCV/vpOpenCVGrabber.cpp
deleted file mode 100644
index 55a5765..0000000
--- a/src/device/framegrabber/OpenCV/vpOpenCVGrabber.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpOpenCVGrabber.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Cameras video capture using OpenCV library.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpOpenCVGrabber.cpp
- \brief class for cameras video capture using OpenCV library.
-*/
-
-#include <visp/vpOpenCVGrabber.h>
-
-#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
-
-#include <visp/vpImageConvert.h>
-#include <visp/vpFrameGrabberException.h>
-
-#include <iostream>
-#include <math.h>
-
-/*!
- Basic Constructor.
-*/
-vpOpenCVGrabber::vpOpenCVGrabber()
- : capture(NULL), DeviceType(0), flip(false)
-{
- // public memebers
- init = false;
-
- // protected members
- width = height = 0;
-}
-
-
-/*!
- Basic destructor that calls the close() method.
-
- \sa close()
-*/
-vpOpenCVGrabber::~vpOpenCVGrabber( )
-{
- close();
-}
-
-
-/*!
- Generic initialization of the grabber.
-*/
-void vpOpenCVGrabber::open()
-{
-
- capture = cvCreateCameraCapture(DeviceType);
-
- if (capture != NULL)
- {
- init = true;
- }
-
- else
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done : camera already used or no camera found") );
- }
-}
-
-
-/*!
- Initialization of the grabber.
- Call the generic initialization method.
-
- \param I : Gray level image. This parameter is not used.
-
- \sa open()
-*/
-void vpOpenCVGrabber::open(vpImage<unsigned char> &/*I*/)
-{
- open();
-}
-
-
-/*!
- Initialization of the grabber.
- Call the generic initialization method.
-
- \param I : Color image. This parameter is not used.
-
- \sa open()
-*/
-void vpOpenCVGrabber::open(vpImage<vpRGBa> &/*I*/)
-{
- open();
-}
-
-
-/*!
- Grab a gray level image.
-
- \param I : Acquired gray level image.
-
- \exception vpFrameGrabberException::initializationError If the
- initialization of the grabber was not done previously.
-*/
-void vpOpenCVGrabber::acquire(vpImage<unsigned char> &I)
-{
- IplImage *im;
-
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- cvGrabFrame(capture);
- im = cvRetrieveFrame(capture);
- vpImageConvert::convert(im, I, flip);
-}
-
-/*!
- Grab a color image.
-
- \param I : Acquired color image.
-
- \exception vpFrameGrabberException::initializationError If the
- initialization of the grabber was not done previously.
-*/
-void vpOpenCVGrabber::acquire(vpImage<vpRGBa> &I)
-{
- IplImage *im;
-
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- cvGrabFrame(capture);
- im = cvRetrieveFrame(capture);
- vpImageConvert::convert(im, I, flip);
-}
-
-/*!
- Grab an image direclty in the OpenCV format.
-
- \return Pointer to the image (must not be freed).
-
- \exception vpFrameGrabberException::initializationError If the
- initialization of the grabber was not done previously.
-*/
-IplImage* vpOpenCVGrabber::acquire()
-{
- IplImage *im;
-
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- cvGrabFrame(capture);
- im = cvRetrieveFrame(capture);
- return im;
-}
-
-/*!
- Stop the acquisition of images and free the camera.
-*/
-void vpOpenCVGrabber::close()
-{
- init = false;
- cvReleaseCapture( &capture );
- capture = NULL;
-}
-
-
-/*!
- Gets the capture frame rate.
-
- \param framerate : The value of the framerate is returned here.
-
-*/
-void vpOpenCVGrabber::getFramerate(double & framerate)
-{
- framerate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
-}
-
-
-/*!
- Sets the capture frame rate
-
- \param framerate : The requested value of the capture framerate.
-
-*/
-void vpOpenCVGrabber::setFramerate(const double framerate)
-{
- cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, framerate);
-}
-
-
-/*!
- Sets the captured image width.
-
- \warning This function must be called after open() method.
-
- \param w : The requested value of the captured image width.
-
- \exception vpFrameGrabberException::initializationError If no
- camera was found.
-*/
-void vpOpenCVGrabber::setWidth(const unsigned int w)
-{
- if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w))
- {
- close();
- vpERROR_TRACE("Impossible to set the size of the grabber");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Impossible to set the size of the grabber") );
- }
-
- this->width = w;
-}
-
-/*!
- Sets the captured image height.
-
- \warning This function must be called after open() method.
-
- \param h : The requested value of the captured image height.
-
- \exception vpFrameGrabberException::initializationError If no
- camera was found.
-*/
-void vpOpenCVGrabber::setHeight(const unsigned int h)
-{
- if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h))
- {
- close();
- vpERROR_TRACE("Impossible to set the size of the grabber");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Impossible to set the size of the grabber") );
- }
-
- this->height = h;
-}
-
-/*!
- Set the expected type of device.
-
- \param type : expected type of device
- - CV_CAP_ANY
- - CV_CAP_MIL
- - CV_CAP_VFW
- - CV_CAP_V4L
- - CV_CAP_V4L2
- - CV_CAP_FIREWIRE
- - CV_CAP_IEEE1394
- - CV_CAP_DC1394
- - CV_CAP_CMU_1394
-*/
-void vpOpenCVGrabber::setDeviceType(int type)
-{
- DeviceType = type;
-
- if ( DeviceType != 0 && DeviceType != 100 &&DeviceType != 200 && DeviceType != 300)
- {
- vpTRACE("The expected type of device may be unknown.");
- }
-}
-
-
-/*!
- Set the boolean variable flip to the expected value.
-
- \param flipType : Expected value of the variable flip. True means that the image is flipped during each image acquisition.
-
- \warning This function is only useful under Windows.
-
- \note The aim of this function is to fix a problem which appears under Windows. Indeed with several cameras the aquired images are flipped.
-*/
-void vpOpenCVGrabber::setFlip(bool flipType)
-{
- flip = flipType;
-}
-#endif
diff --git a/src/device/framegrabber/OpenCV/vpOpenCVGrabber.h b/src/device/framegrabber/OpenCV/vpOpenCVGrabber.h
deleted file mode 100644
index 1587a92..0000000
--- a/src/device/framegrabber/OpenCV/vpOpenCVGrabber.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpOpenCVGrabber.h 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Cameras video capture using OpenCV library.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpOpenCVGrabber.h
- \brief class for cameras video capture using OpenCV library.
-*/
-
-#ifndef vpOpenCVGrabber_h
-#define vpOpenCVGrabber_h
-
-#include <visp/vpConfig.h>
-
-#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/highgui/highgui.hpp>
-#else
-# include <highgui.h>
-#endif
-
-#include <visp/vpImage.h>
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpRGBa.h>
-
-/*!
- \class vpOpenCVGrabber
-
- \ingroup Framegrabber CameraDriver
-
- \brief Class for cameras video capture using OpenCV library.
-
- Needs OpenCV available on http://opencv.willowgarage.com/wiki/.
-
- The code below available in tutorial-grabber-opencv.cpp shows how to grab and
- display images using OpenCV wrappers impremented in ViSP.
- \include tutorial-grabber-opencv.cpp
-
- Note that it is also possible to grab images using OpenCV library by using
- directly OpenCV cv::VideoCapture class. The following code corresponding to
- tutorial-grabber-opencv-bis.cpp shows how to grab images in a cv::Mat structure
- and then convert OpenCV images in ViSP images.
- \include tutorial-grabber-opencv-bis.cpp
-
- An other example very close to the previous one available in grabOpenCV-2.cpp
- shows how to grab images in OpenCV IplImage structure and then how to convert
- OpenCV images in ViSP images.
-
- */
-class VISP_EXPORT vpOpenCVGrabber : public vpFrameGrabber
-{
- private:
-
- CvCapture *capture;
- int DeviceType;
- bool flip;
-
- public:
-
- vpOpenCVGrabber();
- ~vpOpenCVGrabber();
-
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
- IplImage* acquire();
-
- void close();
-
- void getFramerate(double & framerate);
- void setFramerate(const double framerate);
-
- void setWidth(const unsigned int width);
- void setHeight(const unsigned int height);
-
- void setDeviceType(int type);
-
- void setFlip(bool flipType);
-};
-
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowDevice.cpp b/src/device/framegrabber/directshow/vpDirectShowDevice.cpp
deleted file mode 100644
index 21de590..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowDevice.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowDevice.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow device description.
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <stdio.h>
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <visp/vpDirectShowDevice.h>
-
-
-/*!
- Initialize the vpDirectShowDevice with the moniker's information
- \param pMoniker The moniker that contains the device's information
- \return Was the operation successfull
-*/
-bool vpDirectShowDevice::init(const CComPtr<IMoniker>& pMoniker)
-{
- HRESULT hr;
-
- //Get the properties
- CComPtr<IPropertyBag> pPropBag;
- hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag,
- (void**)(&pPropBag));
-
- //get the name of the input
- VARIANT varName;
- VARIANT varDesc;
- VARIANT varDevPath;
- VariantInit(&varName);
- VariantInit(&varDesc);
- VariantInit(&varDevPath);
- char tmp[FILENAME_MAX];
-
- hr = pPropBag->Read(L"FriendlyName", &varName, 0);
-
- //successfully got the name
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S", varName.bstrVal);
- name = tmp;
- }
-
- VariantClear(&varName);
-
- hr = pPropBag->Read(L"Description", &varDesc, 0);
-
- //successfully got the description
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S", varDesc.bstrVal);
- desc = tmp;
- }
-
- VariantClear(&varDesc);
-
- hr = pPropBag->Read(L"DevicePath", &varDevPath, 0);
-
- //successfully got the device path
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S",varDevPath.bstrVal);
- devPath = tmp;
- }
-
- VariantClear(&varDevPath);
-
- inUse=false;
-
- return true;
-}
-
-/*!
- Compares the two vpDirectShowDevice.
- \return true if they are equal
-*/
-bool vpDirectShowDevice::operator==(vpDirectShowDevice& dev)
-{
- return name==dev.name
- && desc==dev.desc
- && devPath==dev.devPath;
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpDirectShowDevice& dev)
-{
- return os<<dev.name<<std::endl<<dev.desc<<std::endl<<dev.devPath;
-}
-
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowDevice.h b/src/device/framegrabber/directshow/vpDirectShowDevice.h
deleted file mode 100644
index 6f35fe2..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowDevice.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowDevice.h 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow device description.
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#ifndef vpDirectShowDevice_hh
-#define vpDirectShowDevice_hh
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <atlbase.h>
-#include <iostream>
-#include <string>
-#include <dshow.h>
-
-
-
-class VISP_EXPORT vpDirectShowDevice
-{
-
- std::string name; //the device's name
- std::string desc; //the device's description
- std::string devPath; //the device's device path (unique)
-
- bool inUse; //true if the device is already used by a grabber
-
-public:
- vpDirectShowDevice() : inUse(false){}
- vpDirectShowDevice(const CComPtr<IMoniker>& moniker) : inUse(false){ init(moniker); }
-
- bool init(const CComPtr<IMoniker>& moniker);
-
- bool getState(){ return inUse; }
- void setInUse(){ inUse=true; }
- void resetInUse() { inUse=false; }
-
- std::string& getName(){ return name; }
- std::string& getDesc(){ return desc; }
- std::string& getDevPath(){ return devPath; }
-
- bool operator==(vpDirectShowDevice& dev);
-
- friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpDirectShowDevice& dev);
-};
-#endif
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowGrabber.cpp b/src/device/framegrabber/directshow/vpDirectShowGrabber.cpp
deleted file mode 100644
index 2235568..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowGrabber.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowGrabber.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow framegrabber.
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <visp/vpDirectShowGrabber.h>
-#include <visp/vpDirectShowGrabberImpl.h>
-
-/*!
- Returns the number of rows in the grabbed image
-*/
-unsigned int vpDirectShowGrabber::getHeight(){ return grabber->getHeight(); }
-
-/*!
- Returns the number of colunms in the grabbed image
-*/
-unsigned int vpDirectShowGrabber::getWidth(){ return grabber->getWidth(); }
-
-
-/*!
- Constructor.
- Initializes COM.
-*/
-vpDirectShowGrabber::vpDirectShowGrabber()
-{
- grabber = new vpDirectShowGrabberImpl();
-}
-
-/*!
- Destructor
-*/
-vpDirectShowGrabber::~vpDirectShowGrabber()
-{
- delete grabber;
-}
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabber::open()
-{
- grabber->open();
-}
-
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabber::open(vpImage<unsigned char> &I)
-{
- grabber->open();
-}
-
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabber::open(vpImage<vpRGBa> &I)
-{
- grabber->open();
-}
-
-
-/*!
- Grabs a grayscale image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
-
- \param I The image to fill with the grabbed frame
-
- \exception otherError Can't grab the frame
-*/
-void vpDirectShowGrabber::acquire(vpImage<unsigned char> &I)
-{
- grabber->acquire(I);
-}
-
-/*!
- Grabs a rgba image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
-
- \param I The image to fill with the grabbed frame
-
- \exception otherError Can't grab the frame
-*/
-void vpDirectShowGrabber::acquire(vpImage<vpRGBa> &I)
-{
- grabber->acquire(I);
-}
-
-
-/*!
- Stops the framegrabber
-*/
-void vpDirectShowGrabber::close() { grabber->close(); }
-
-/*!
- Gets the number of capture devices
- \return the number of capture devices
-*/
-unsigned int vpDirectShowGrabber::getDeviceNumber()
-{
- return grabber->getDeviceNumber();
-}
-
-/*!
- Set the device (or camera) from which images will be grabbed.
- \param id : Identifier of the device to use.
-
- \return true if the device is set successfully, false otherwise.
-*/
-bool vpDirectShowGrabber::setDevice(unsigned int id)
-{
- return grabber->setDevice(id);
-}
-
-/*!
- Displays the list of devices on the standard output
-*/
-void vpDirectShowGrabber::displayDevices()
-{
- grabber->displayDevices();
-}
-/*!
- Set image Size
- \param width : Image width
- \param height : Image height
-
- \return true if successful
-*/
-bool vpDirectShowGrabber::setImageSize(unsigned int width,unsigned int height)
-{
- return grabber->setImageSize(width, height);
-}
-/*!
- Set capture framerate
- \param framerate : Framerate in fps
-
- \return true if successful
-*/
-bool vpDirectShowGrabber::setFramerate(double framerate)
-{
- return grabber->setFramerate(framerate);
-}
-/*!
- Set capture format
- \param width : Image width in pixel
- \param height : Image height in pixel
- \param framerate : Framerate in fps
-
- \return true if successful
-*/
-bool vpDirectShowGrabber::setFormat(unsigned int width,unsigned int height,double framerate)
-{
- return grabber->setFormat(width, height, framerate);
-}
-/*
- Get capture format
- \param width : Pointer to a variable that receives the width in pixel
- \param height : Pointer to a variable that receives the height in pixel
- \param framerate : Pointer to a variable that receives the framerate in fps
-*/
-void vpDirectShowGrabber::getFormat(unsigned int &width,unsigned int &height, double &framerate)
-{
- grabber->getFormat(width, height, framerate);
-}
-
-/*!
- Get the available capture formats
-
- \return true if successful
-*/
-bool vpDirectShowGrabber::getStreamCapabilities()
-{
- return grabber->getStreamCapabilities();
-}
-
-/*!
- Set capture MediaType
- \param mediaTypeID : mediaTypeID (available in calling getStreamCapabilities)
-
- \return true if successful
-*/
-bool vpDirectShowGrabber::setMediaType(int mediaTypeID)
-{
- return grabber->setMediaType(mediaTypeID);
-}
-
-/*
- Get current capture MediaType
-
- \return the current mediaTypeID
-*/
-int vpDirectShowGrabber::getMediaType()
-{
- return grabber->getMediaType();
-}
-
-
-#endif
-
diff --git a/src/device/framegrabber/directshow/vpDirectShowGrabber.h b/src/device/framegrabber/directshow/vpDirectShowGrabber.h
deleted file mode 100644
index d360e26..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowGrabber.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowGrabber.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow framegrabber.
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-#ifndef vpDirectShowGrabber_hh
-#define vpDirectShowGrabber_hh
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpFrameGrabber.h>
-
-class vpDirectShowGrabberImpl;
-
-/*!
- \class vpDirectShowGrabber
- \brief class for windows direct show devices
-
- This class' goal is to totally hide the implementation of the framegrabber
- from the rest of the application. This is necessary to avoid conflicts
- between dshow's Qedit.h and Direct3D's includes.
-
- \ingroup Framegrabber CameraDriver
-*/
-
-class VISP_EXPORT vpDirectShowGrabber : public vpFrameGrabber
-{
- vpDirectShowGrabberImpl * grabber;
-
-
- public:
-
- unsigned int getHeight();
- unsigned int getWidth();
-
- vpDirectShowGrabber();
- virtual ~vpDirectShowGrabber();
-
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
-
- void close();
-
- //get the number of capture devices
- unsigned int getDeviceNumber();
-
- //change the capture device
- bool setDevice(unsigned int id);
-
- //displays a list of available devices
- void displayDevices();
-
- //set image size
- bool setImageSize(unsigned int width,unsigned int height);
-
- //set source framerate
- bool setFramerate(double framerate);
-
-
- //set capture format
- bool setFormat(unsigned int width,unsigned int height, double framerate);
-
- //get capture format
- void getFormat(unsigned int &width,unsigned int &height, double &framerate);
-
- //Get the available capture formats
- bool getStreamCapabilities();
-
- //Set capture MediaType
- bool setMediaType(int mediaTypeID);
-
- //Get current capture MediaType
- int getMediaType();
-
-};
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.cpp b/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
deleted file mode 100755
index c2c4893..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
+++ /dev/null
@@ -1,1242 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowGrabberImpl.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow framegrabber implementation.
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <visp/vpDirectShowGrabberImpl.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-vpDirectShowDevice * vpDirectShowGrabberImpl::deviceList = NULL;
-unsigned int vpDirectShowGrabberImpl::nbDevices ;
-
-/*!
- Converts a HRESULT into the corresponding error message
-*/
-void vpDirectShowGrabberImpl::HRtoStr(std::string str)
-{
- TCHAR szErr[MAX_ERROR_TEXT_LEN];
- DWORD res = AMGetErrorText(hr, szErr, MAX_ERROR_TEXT_LEN);
-
- if (res == 0) str = "Unknown Error: 0x%2x";
-
- char msg[MAX_ERROR_TEXT_LEN];
- sprintf(msg,"%s",szErr);
- str = msg;
-}
-
-
-/*!
- Constructor.
- Initializes COM.
-*/
-vpDirectShowGrabberImpl::vpDirectShowGrabberImpl()
-{
- init = false ;
- initCo = false ;
- //COM initialization
- if (FAILED(hr = CoInitializeEx(NULL,COINIT_MULTITHREADED)))
- {
- std::string err;
- HRtoStr(err);
- throw(vpFrameGrabberException(
- vpFrameGrabberException::initializationError,
- "Can't initialize COM\n"+ err));
- }
- initCo = true ;
-
- //create the device list
- if(deviceList == NULL) {
- CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
-
- if(enumerate(pVideoInputEnum)) {
- createDeviceList(pVideoInputEnum);
- }
- //not used anymore, so we release it
- pVideoInputEnum.Release();
- }
-}
-
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabberImpl::open()
-{
- //create the device list
- if(deviceList == NULL) {
- CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
-
- if(enumerate(pVideoInputEnum)) {
- createDeviceList(pVideoInputEnum);
- }
- //not used anymore, so we release it
- pVideoInputEnum.Release();
- }
-
- init = initDirectShow();
- if(! init )
- {
- std::string err;
- HRtoStr(err);
- throw (vpFrameGrabberException(
- vpFrameGrabberException::initializationError, err));
- }
-}
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabberImpl::open(vpImage<unsigned char> &I)
-{
- open();
-}
-
-/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
-*/
-void vpDirectShowGrabberImpl::open(vpImage<vpRGBa> &I)
-{
- open();
-}
-
-/*!
- Initialization method
- Creates the capture filter graph
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::initDirectShow()
-{
-
-
- //get the first working device's filter (unused and getdevice works on it)
- currentDevice = getFirstUnusedDevice(pCapSource);
-
- if(currentDevice == nbDevices)
- return false;
-
- //create the filter graph
- if(!createGraph())
- return false;
-
- //we add the capture source to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
- return false;
-
- //we create a sample grabber
- if(!createSampleGrabber(pGrabberFilter))
- return false;
-
- //we add the grabber to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pGrabberFilter, L"SampleGrabber")))
- return false;
-
- //we connect the pins
- if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
- return false;
-
-
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
-
- //Gets the various graph's interfaces
- CComPtr<IMediaFilter> pMediaFilter;
-
- pGraph->QueryInterface(IID_IMediaFilter, (void **)&pMediaFilter);
- pGraph->QueryInterface(IID_IMediaControl, reinterpret_cast<void**>(&pControl));
- pGraph->QueryInterface(IID_IMediaEvent, (void **)&pEvent);
-
- pMediaFilter->SetSyncSource(NULL);
- pMediaFilter.Release();
-
- return true;
-}
-
-/*!
- Destructor
-*/
-vpDirectShowGrabberImpl::~vpDirectShowGrabberImpl()
-{
- close();
-}
-
-/*!
- Create a video device enumerator.
- \param ppVideoInputEnum The video device enumerator
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum)
-{
- CComPtr<ICreateDevEnum> pDevEnum = NULL;
- bool res = false;
-
- //Enumerate system devices
- hr = pDevEnum.CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER);
-
- //if it is a success
- if (SUCCEEDED(hr))
- {
- //Create a video input device enumerator
- hr = pDevEnum->CreateClassEnumerator(
- CLSID_VideoInputDeviceCategory,
- &ppVideoInputEnum, 0);
-
- if(hr == S_OK) res = true;
- }
-
-
- pDevEnum.Release();
- return res;
-}
-
-
-/*!
- Create the device list by enumerating the video input devices
- \param ppVideoInputEnum A video device enumerator
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum)
-{
- CComPtr<IMoniker> pMoniker[10]; //const max devices
- unsigned long nbMoniker;
-
- ppVideoInputEnum->Reset();
-
- //Enumerates the different inputs
- ppVideoInputEnum->Next(10, reinterpret_cast<IMoniker **>(&pMoniker), &nbMoniker);
-
- //if no input device
- if(nbMoniker == 0) return false;
-
- deviceList = new vpDirectShowDevice[nbMoniker];
-
- nbDevices = (unsigned int)nbMoniker;
-
- //we try to get the properties of each moniker, if it fails, we skip to the next one and
- //decrement the number of valid devices
- unsigned int i=0;
- unsigned int j=0;
- while(i<nbDevices)
- {
- if(!deviceList[i].init(pMoniker[j]))
- {
- //if we can't get the device properties, skip to the next device
- j++;
- nbDevices--;
- }
- else
- {
- i++;
- j++;
- }
- }
-
- //if no working input device
- if(nbDevices == 0) return false;
-
- //we release the monikers
- for(unsigned int i=0 ; i<nbMoniker ;i++)
- {
- pMoniker[i].Release();
- }
-
- return true;
-}
-
-
-/*!
- Gets the filter associated with device n if it exists.
- \param n Number of the device
- \param ppDevice The n-th device
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::getDevice(unsigned int n, CComPtr<IBaseFilter>& ppDevice)
-{
- //if n is invalid, quit
- if(n>=nbDevices)
- return false;
-
- //if the device is already in use, quit
- if(deviceList[n].getState() == true)
- return false;
-
- //if we can't enumerate the devices, quit
- CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
- if(!enumerate(pVideoInputEnum))
- return false;
-
- CComPtr<IMoniker> pMoniker = NULL;
- bool deviceFound = false;
-
- //Enumerates the different inputs
- while (pVideoInputEnum->Next(1, &pMoniker, NULL) == S_OK
- && !deviceFound)
- {
- //implicit conversion should work ...
- if(deviceList[n] == vpDirectShowDevice(pMoniker))
- {
- //we get the filter
- if(SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, (void**)&ppDevice)))
- {
- //now the device is in use
- deviceList[n].setInUse();
- deviceFound = true;
- }
- else{ break; } //we can't get the device's filter, quit
- }
- pMoniker.Release();
- }
-
- pVideoInputEnum.Release();
-
- return deviceFound;
-}
-
-/*!
- Searches for the first unused device.
- \param ppDevice The first device filter's interface
- \return number of the found device. If the operation wasn't successfull, n=nbDevices.
-*/
-unsigned int vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice)
-{
- unsigned int n=0;
- bool found=false;
-
- for(n=0;n<nbDevices && !found; n++)
- {
- //if the device is not being used
- if(!deviceList[n].getState())
- {
- if(getDevice(n,ppDevice))
- {
- found = true;
- deviceList[n].setInUse();
- return n;
- }
- }
- }
-
- return n;
-}
-
-/*!
- Create the capture graph
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::createGraph()
-{
-
- // Create the Capture Graph Builder.
- hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, CLSCTX_INPROC_SERVER);
-
- if (SUCCEEDED(hr))
- {
- // Create the Filter Graph Manager.
- hr = pGraph.CoCreateInstance(CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER);
-
- if (SUCCEEDED(hr))
- {
- // Initialize the Capture Graph Builder.
- pBuild->SetFiltergraph(pGraph);
-
- return true;
- }
- }
-
- return false;
-}
-
-/*!
- Creates the grabber.
- \param ppGrabberFilter The created grabber filter's interface
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter)
-{
- //Creates the sample grabber
- hr = ppGrabberFilter.CoCreateInstance(CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER);
-
- if (FAILED(hr))
- return false;
-
- //gets the SampleGrabber interface in order to configure it later
- hr = ppGrabberFilter->QueryInterface(IID_ISampleGrabber,
- reinterpret_cast<void**>(&pGrabberI));
-
- if (FAILED(hr))
- return false;
-
- //configure the grabber
- AM_MEDIA_TYPE mt;
- ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
-
- mt.majortype = MEDIATYPE_Video;
-
- //ask for a connection
- mt.subtype = MEDIATYPE_NULL;
-
- if(FAILED(hr = pGrabberI->SetMediaType(&mt)))
- return false;
-
- //configure the callback of the grabber
- pGrabberI->SetCallback(&sgCB,1);
-
- //grab only one frame at a time
- pGrabberI->SetOneShot(TRUE);
-
- //no need to bufferize the sample in the grabber
- pGrabberI->SetBufferSamples(false);
-
- return true;
-}
-
-/*!
- Checks the capture filter's ouput pin media type.
- Some formats' orientation is automatically handled by directshow but this isn't the
- case for all the existing formats.
- If the grabbed image is inverted, the source format's orientation may not be
- known by directshow so consider adding an additional handler for this FourCC format.
-
- \return True if the connection media type was a video media type.
-*/
-bool vpDirectShowGrabberImpl::checkSourceType(CComPtr<IPin>& pCapSourcePin)
-{
- //retrieves the connected media type
- AM_MEDIA_TYPE mt;
- if(FAILED (pCapSourcePin->ConnectionMediaType(&mt)))
- return false;
-
- if(mt.majortype != MEDIATYPE_Video)
- return false;
-
- //Known RGB formats
- if(mt.subtype == MEDIASUBTYPE_ARGB32 ||
- mt.subtype == MEDIASUBTYPE_RGB32 ||
- mt.subtype == MEDIASUBTYPE_RGB24 ||
- mt.subtype == MEDIASUBTYPE_RGB555 ||
- mt.subtype == MEDIASUBTYPE_RGB565 ||
- mt.subtype == MEDIASUBTYPE_RGB8 ||
- mt.subtype == MEDIASUBTYPE_RGB4 ||
- mt.subtype == MEDIASUBTYPE_RGB1 )
- {
- //image orientation will be handled "automatically"
- sgCB.specialMediaType = false;
- }
- //Known YUV formats
- else if(mt.subtype == MEDIASUBTYPE_AYUV ||
- mt.subtype == MEDIASUBTYPE_UYVY ||
- mt.subtype == MEDIASUBTYPE_Y411 ||
- mt.subtype == MEDIASUBTYPE_Y41P ||
- mt.subtype == MEDIASUBTYPE_Y211 ||
- mt.subtype == MEDIASUBTYPE_YUY2 ||
- mt.subtype == MEDIASUBTYPE_YVYU ||
- mt.subtype == MEDIASUBTYPE_YUYV ||
- mt.subtype == MEDIASUBTYPE_IF09 ||
- mt.subtype == MEDIASUBTYPE_IYUV ||
- mt.subtype == MEDIASUBTYPE_YV12 ||
- mt.subtype == MEDIASUBTYPE_YVU9 )
- {
- //image orientation will be handled "automatically"
- sgCB.specialMediaType = false;
- }
- //FOURCC formats
- else
- {
- //invertedSource boolean will decide the bitmap orientation
- sgCB.specialMediaType = true;
-
- DWORD format;
- VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(mt.pbFormat);
- BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
-
- //get the fourcc code
- format = ((bmpInfo.biCompression&0xFF000000)>>24) |
- ((bmpInfo.biCompression&0x00FF0000)>>8) |
- ((bmpInfo.biCompression&0x0000FF00)<<8) |
- (bmpInfo.biCompression&0x000000FF)<<24;
-
- std::cout<<"This format is not one of the standard YUV or RGB format supported by DirectShow.\n"
- <<"FourCC : "
- <<(char)(bmpInfo.biCompression&0x000000FF)
- <<(char)((bmpInfo.biCompression&0x0000FF00)>>8)
- <<(char)((bmpInfo.biCompression&0x00FF0000)>>16)
- <<(char)((bmpInfo.biCompression&0xFF000000)>>24)<<std::endl;
-
- //Y800 is top-down oriented so the image doesn't have to be flipped vertically
- if(format == 'Y800')
- {
- sgCB.invertedSource = false;
- }
- //cyuv seems to be the only yuv bottom-up oriented format (image has to be flipped)
- else if(format == 'cyuv')
- {
- sgCB.invertedSource = true;
- }
- //insert code for other fourcc formats here
- //see fourcc.org to know which format is bottom-up oriented and thus needs invertedSource sets to true
- else
- {
- std::cout<<"Unknown FourCC compression type, assuming top-down orientation. Image may be inverted."<<std::endl;
- sgCB.invertedSource = false; //consider that the image is topdown oriented by default
- }
- }
-
- return true;
-}
-
-/*!
- Connects the capture device's output pin to the grabber's input pin
- \param pCapSource The capture device
- \param pGrabber The grabber
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr<IBaseFilter>& _pCapSource, CComPtr<IBaseFilter>& _pGrabberFilter)
-{
- /*
- //get the capture source's output pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
-
- //get the grabber's input pin
- CComPtr<IPin> pGrabberInputPin;
- if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_INPUT, NULL, NULL, false, 0, &pGrabberInputPin)))
- return false;
-
- //connect the two of them
- if(FAILED(pGraph->Connect(pCapSourcePin, pGrabberInputPin)))
- return false;
-
- //not used anymore, we can release it
- pGrabberInputPin.Release();
- */
- if(FAILED(hr = pBuild->RenderStream(NULL,NULL, _pCapSource, NULL, _pGrabberFilter)))
- return false;
-
- /*
- //get the grabber's output pin
- CComPtr<IPin> pGrabberOutputPin;
- if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_OUTPUT, NULL, NULL, false, 0, &pGrabberOutputPin)))
- return false;
- */
- //get the Null renderer
- CComPtr<IBaseFilter> pNull = NULL;
- if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER)))
- return false;
-/*
- //get the null renderer's input pin
- CComPtr<IPin> 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)))
- return false;
-
- //get the capture source's output pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
- //checks the media type of the capture filter
- //and if the image needs to be inverted
- if(!checkSourceType(pCapSourcePin))
- return false;
-
- //release the remaining interfaces
- pCapSourcePin.Release();
- pNull.Release();
-// pGrabberOutputPin.Release();
-// pNullInputPin.Release();
-
- return true;
-}
-
-
-/*!
- Removes all the filters from the filter graph
-
- \return Was the operation successful
-*/
-bool vpDirectShowGrabberImpl::removeAll()
-{
- CComPtr<IEnumFilters> pEnum = NULL;
- CComPtr<IBaseFilter> pFilter;
- ULONG cFetched;
-
- if (FAILED(hr = pGraph->EnumFilters(&pEnum)))
- return false;
-
- while(pEnum->Next(1, &pFilter, &cFetched) == S_OK)
- {
- if(FAILED(hr = pGraph->RemoveFilter(pFilter))) return false;
- pFilter.Release();
- pEnum->Reset();
- }
-
- pEnum.Release();
- return true;
-}
-
-
-/*!
- Grabs a rgba image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
-
- \param I The image to fill with the grabbed frame
-
- \exception otherError Can't grab the frame
-*/
-void vpDirectShowGrabberImpl::acquire(vpImage<vpRGBa> &I)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //set the rgbaIm pointer on I (will be filled on the next framegrabber callback)
- sgCB.rgbaIm = &I;
- //there is an acquire demand (execute copy during callback)
- sgCB.acqRGBaDemand = true;
-
- //Run the graph to grab a frame
- pControl->Run();
-
- // Wait untill it's done
- long ev;
- hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
-
- width = I.getWidth();
- height = I.getHeight();
-
- //wait for the end of the next callback (copy)
- if( WaitForSingleObject(sgCB.copySem,MAX_DELAY) != WAIT_OBJECT_0)
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't grab the frame, callback timeout") );
-
-
-}
-
-/*!
- Grabs a grayscale image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
-
- \param I The image to fill with the grabbed frame
-
- \exception otherError Can't grab the frame
-*/
-void vpDirectShowGrabberImpl::acquire(vpImage<unsigned char> &I)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //set the grayIm pointer on I (will be filled on the next framegrabber callback)
- sgCB.grayIm = &I;
- //there is an acquire demand (execute copy during callback)
- sgCB.acqGrayDemand = true;
-
- //Run the graph to grab a frame
- pControl->Run();
-
- // Wait untill it's done
- long ev;
- hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
-
- width = I.getWidth();
- height = I.getHeight();
-
- //wait for the end of the next callback (copy)
- if( WaitForSingleObject(sgCB.copySem,MAX_DELAY) != WAIT_OBJECT_0)
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't grab the frame, callback timeout") );
-}
-
-/*!
- Set the device (or camera) used by the grabber
- \param id : Identifier of the device to use.
-
- \return true id the device is set successfully, false otherwise.
-*/
-bool vpDirectShowGrabberImpl::setDevice(unsigned int id)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
-
- //if n is invalid, or the device is already in use, quit
- if(id>=nbDevices || deviceList[id].getState()==true)
- return false;
-
- //we stop the graph
- pControl->Stop();
-
- //then we can safely remove all the filters
- if(!removeAll()) return false;
-
-
- //we release the previous source's interface
- pCapSource.Release();
-
- //here reset inUse in the old DSDevice
- deviceList[currentDevice].resetInUse();
-
- //we add the grabber back in the graph
- pGraph->AddFilter(pGrabberFilter,L"SampleGrabber");
-
- //get the n-th device's filter
- if(!getDevice(id, pCapSource))
- return false;
-
- //we add the capture source to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
- return false;
-
- //we connect the pins
- if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
- return false;
-
-
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- {
- return false;
- }
-
- //the device is now in use
- deviceList[id].setInUse();
- currentDevice=id;
-
- return true;
-}
-
-/*!
- Displays the list of devices on the standard output
-*/
-void vpDirectShowGrabberImpl::displayDevices()
-{
- if(deviceList == NULL)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- for(unsigned int i=0 ; i<nbDevices ; i++)
- std::cout<<i<<" : "<< deviceList[i].getName() <<std::endl;
-
- std::cout<<"Current device : "<<currentDevice<<std::endl<<std::endl;
-}
-
-
-/*!
- Stops the framegrabber
-*/
-
-void vpDirectShowGrabberImpl::close()
-{
- //the current device isn't being used anymore
- if (init) {
- deviceList[currentDevice].resetInUse();
- init = false ;
- }
- if (initCo) {
- //uninstalls COM
- CoUninitialize();
- initCo = false;
- }
-}
-/*!
- Set image size
-*/
-bool vpDirectShowGrabberImpl::setImageSize(unsigned int width,unsigned int height)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- return setFormat(width, height,NULL);
-
-}
-
-/*!
- Set capture framerate
-*/
-bool vpDirectShowGrabberImpl::setFramerate(double framerate)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- return setFormat(pVih->bmiHeader.biWidth,pVih->bmiHeader.biHeight,framerate);
-
-}
-/*!
- Set the capture format
-*/
-bool vpDirectShowGrabberImpl::setFormat(unsigned int width,unsigned int height, double framerate)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- bool found=false;
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- //gets the video control interface
- IAMVideoControl *pVideoControl = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMVideoControl, (void**)&pVideoControl)))
- return false;
-
- //get the grabber's input pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return false;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- // Use the video capabilities structure.
-
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- 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 : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<" at "<<10000000 /pVih->AvgTimePerFrame<<std::endl;
-// std::cout<<"compression : "<<pVih->bmiHeader.biCompression<<std::endl;
- if (SUCCEEDED(hr)&& found==false)
- {
- /* Examine the format, and possibly use it. */
- if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
- (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
- (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
- (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
- (pmtConfig->pbFormat != NULL))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
-
- LONG lWidth = pVih->bmiHeader.biWidth;
- LONG lHeight = pVih->bmiHeader.biHeight;
- if(framerate != NULL)
- {
- if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
- {
-
- pVih->AvgTimePerFrame = (LONGLONG)(10000000 / framerate);
- //set the capture media type and the grabber media type
- if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
- FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
- return false;
- //Run the graph to grab a frame
- pControl->Run();
-
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- LONGLONG ActualFrameDuration;
- if(FAILED(hr = pVideoControl->GetCurrentActualFrameRate(pCapSourcePin,&ActualFrameDuration)))
- std::cout<<"Current format (not sure): "<<width <<" x "<< height <<" at "<< 10000000/pVih->AvgTimePerFrame <<" fps"<<std::endl<<std::endl;
- else {
- std::cout<<"Current format : "<<width <<" x "<< height <<" at "<< 10000000/ActualFrameDuration <<" fps"<<std::endl<<std::endl;
- pVih->AvgTimePerFrame = ActualFrameDuration;
- }
- found=true;
- }
- }
- else
- {
- if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
- {
- pVih->AvgTimePerFrame = scc.MinFrameInterval;
- //set the capture media type and the grabber media type
- if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
- FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
- return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- found=true;
- std::cout<<"Current format : "<<width <<" x "<< height <<" at "<<(10000000 /pVih->AvgTimePerFrame) <<" fps"<<std::endl<<std::endl;
- }
-
- }
- }
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- if(!found)
- if(framerate != NULL)
- std::cout << "The "<<width <<" x "<< height <<" at " <<framerate<<" fps source image format is not available. "<<std::endl<<std::endl;
- else
- std::cout << "The "<<width <<" x "<< height <<"source image size is not available. "<<std::endl<<std::endl;
-
- return found;
-}
-/*!
- Get the current capture format and framerate.
- \param width : Image width.
- \param height : Image height.
- \param framerate : Framerate acquisition.
-*/
-void vpDirectShowGrabberImpl::getFormat(unsigned int &width,unsigned int &height, double &framerate)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- width = (unsigned int)pVih->bmiHeader.biWidth;
- height = (unsigned int)pVih->bmiHeader.biHeight;
- framerate = (double)(10000000/pVih->AvgTimePerFrame);
-
-}
-/*!
- Get the available capture formats
-*/
-bool vpDirectShowGrabberImpl::getStreamCapabilities()
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return false;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- std::cout<<"Available MediaTypes : "<<std::endl<<std::endl;
- // Use the video capabilities structure.
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
-
- if (SUCCEEDED(hr))
- {
- /* 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)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_ARGB32"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB32)
- std::cout<<"subtype : MEDIASUBTYPE_RGB32"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB24)
- std::cout<<"subtype : MEDIASUBTYPE_RGB24"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB555)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB555"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB565)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB565"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB8)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB8"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB4)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB4"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB1)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB1"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YV12)
- std::cout<<"subtype : MEDIASUBTYPE_YV12"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YVU9)
- std::cout<<"subtype : MEDIASUBTYPE_YVU9"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YUY2)
- std::cout<<"subtype : MEDIASUBTYPE_YUY2"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YUYV)
- std::cout<<"subtype : MEDIASUBTYPE_YUYV"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YVYU)
- std::cout<<"subtype : MEDIASUBTYPE_YVYU"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_IYUV)
- std::cout<<"subtype : MEDIASUBTYPE_IYUV"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_UYVY)
- std::cout<<"subtype : MEDIASUBTYPE_UYVY"<<std::endl;
- else if((((pVih->bmiHeader.biCompression&0xFF000000)>>24) |
- ((pVih->bmiHeader.biCompression&0x00FF0000)>>8) |
- ((pVih->bmiHeader.biCompression&0x0000FF00)<<8) |
- ((pVih->bmiHeader.biCompression&0x000000FF)<<24)) == 'I420')
- std::cout<<"subtype : I420"<<std::endl;
- else std::cout<<"subtype (not supported) :"
- <<(char)(pVih->bmiHeader.biCompression&0x000000FF)
- <<(char)((pVih->bmiHeader.biCompression&0x0000FF00)>>8)
- <<(char)((pVih->bmiHeader.biCompression&0x00FF0000)>>16)
- <<(char)((pVih->bmiHeader.biCompression&0xFF000000)>>24)<<std::endl;
-
- std::cout<<"image size : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<std::endl;
- std::cout<<"framerate range: ["<< 10000000/scc.MaxFrameInterval<<","<<10000000/scc.MinFrameInterval<<"]"<<std::endl<<std::endl;
-
-/*
- long frameRateNum;
- LONGLONG *frameRateList;
- if(FAILED(hr = pVideoControl->GetFrameRateList(pCapSourcePin,iFormat,dimensions, //inputs
- &frameRateNum, &frameRateList))) //outputs
- return false;
- for(int i=0; i<(int)frameRateNum ; i++)
- {
- std::cout<<(float)(10000000/frameRateList[i])<<" fps"<<std::endl;
- }
- std::cout<<std::endl;
-*/
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- return true;
-}
-/*!
- Set capture Mediatype
-*/
-bool vpDirectShowGrabberImpl::setMediaType(int mediaTypeID)
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(mediaTypeID, &pmtConfig, (BYTE*)&scc);
-
-
- if (SUCCEEDED(hr))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
- pVih->AvgTimePerFrame = scc.MinFrameInterval ;
- //set the capture media type and the grabber media type
- if(FAILED(hr = pGrabberI->SetMediaType(pmtConfig))||
- FAILED(hr = pConfig->SetFormat(pmtConfig)))
- return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- return true;
-}
-
-/*
- Get current capture MediaType
- \return mediaTypeID (-1 if failed)
-*/
-int vpDirectShowGrabberImpl::getMediaType()
-{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- int mediaTypeID = -1;
- VIDEOINFOHEADER *pVihConnected = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return -1;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return -1;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- // Use the video capabilities structure.
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
-
- if (SUCCEEDED(hr))
- {
- /* Examine the format, and possibly use it. */
- if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
- (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
- (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
- (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
- (pmtConfig->pbFormat != NULL))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
- if(pVih->bmiHeader.biWidth == pVihConnected->bmiHeader.biWidth &&
- pVih->bmiHeader.biHeight == pVihConnected->bmiHeader.biHeight)
- mediaTypeID = iFormat ;
- }
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- return mediaTypeID;
-}
-
-
-/*!
- Deletes an allocated AM_MEDIA_TYPE structure, including the format block
-*/
-void vpDirectShowGrabberImpl::MyDeleteMediaType(AM_MEDIA_TYPE *pmt)
-{
- if (pmt != NULL)
- {
- MyFreeMediaType(*pmt); // See FreeMediaType for the implementation.
- CoTaskMemFree(pmt);
- }
-}
-
-
-/*!
- Frees the format block in an AM_MEDIA_TYPE structure.
-*/
-void vpDirectShowGrabberImpl::MyFreeMediaType(AM_MEDIA_TYPE& mt)
-{
- if (mt.cbFormat != 0)
- {
- CoTaskMemFree((PVOID)mt.pbFormat);
- mt.cbFormat = 0;
- mt.pbFormat = NULL;
- }
- if (mt.pUnk != NULL)
- {
- // Unecessary because pUnk should not be used, but safest.
- mt.pUnk->Release();
- mt.pUnk = NULL;
- }
-}
-
-#endif
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.h b/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.h
deleted file mode 100755
index 6f07bde..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowGrabberImpl.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowGrabberImpl.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow framegrabber Implementation.
- *
- * Authors:
- * Bruno Renier
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#ifndef vpDirectShowGrabberImpl_hh
-#define vpDirectShowGrabberImpl_hh
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <atlbase.h>
-#include <qedit.h>
-#include <dshow.h>
-
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpFrameGrabberException.h>
-
-#include <visp/vpDirectShowSampleGrabberI.h>
-#include <visp/vpDirectShowDevice.h>
-#include <visp/vpDebug.h>
-/*!
- \class vpDirectShowGrabberImpl
- \brief class for windows direct show devices - implementation
-
- \ingroup libdevice
-
- This class uses CComPtr which is the best way to be sure that all the interfaces
- are released sooner or later.
-
-*/
-class VISP_EXPORT vpDirectShowGrabberImpl : public vpFrameGrabber
-{
- static const int MAX_DELAY = 10000;
- static const int MAX_DEVICES = 10;
-
-
- public:
-
- /*!
- Enumeration of video subtypes.
-*/
-/*
- typedef enum {
- //Known RGB formats
- vpMEDIASUBTYPE_ARGB32 = MEDIASUBTYPE_ARGB32,
- vpMEDIASUBTYPE_RGB32 = MEDIASUBTYPE_RGB32,
- vpMEDIASUBTYPE_RGB24 = MEDIASUBTYPE_RGB24,
- vpMEDIASUBTYPE_RGB555 = MEDIASUBTYPE_RGB555,
- vpMEDIASUBTYPE_RGB565 = MEDIASUBTYPE_RGB565,
- vpMEDIASUBTYPE_RGB8 = MEDIASUBTYPE_RGB8,
- vpMEDIASUBTYPE_RGB4 = MEDIASUBTYPE_RGB4,
- vpMEDIASUBTYPE_RGB1 = MEDIASUBTYPE_RGB1,
- //Known YUV formats
- vpMEDIASUBTYPE_AYUV = MEDIASUBTYPE_AYUV,
- vpMEDIASUBTYPE_UYVY = MEDIASUBTYPE_UYVY,
- vpMEDIASUBTYPE_Y411 = MEDIASUBTYPE_Y411,
- vpMEDIASUBTYPE_Y41P = MEDIASUBTYPE_Y41P,
- vpMEDIASUBTYPE_Y211 = MEDIASUBTYPE_Y211,
- vpMEDIASUBTYPE_YUY2 = MEDIASUBTYPE_YUY2,
- vpMEDIASUBTYPE_YVYU = MEDIASUBTYPE_YVYU,
- vpMEDIASUBTYPE_YUYV = MEDIASUBTYPE_YUYV,
- vpMEDIASUBTYPE_IF09 = MEDIASUBTYPE_IF09,
- vpMEDIASUBTYPE_IYUV = MEDIASUBTYPE_IYUV,
- vpMEDIASUBTYPE_YV12 = MEDIASUBTYPE_YV12,
- vpMEDIASUBTYPE_YVU9 = MEDIASUBTYPE_YVU9
- } vpDirectShowMediaSubtype;
-*/
-
- vpDirectShowGrabberImpl();
- virtual ~vpDirectShowGrabberImpl();
-
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
-
- void close();
-
- /*!
- Gets the number of capture devices
- */
- unsigned int getDeviceNumber() {return nbDevices;}
-
- //change the capture device
- bool setDevice(unsigned int id);
-
- //displays a list of available devices
- void displayDevices();
-
- //set image size
- bool setImageSize(unsigned int width,unsigned int height);
-
- //set capture framerate
- bool setFramerate(double framerate);
-
- //set capture format
- bool setFormat(unsigned int width,unsigned int height, double framerate);
-
- //get capture format
- void getFormat(unsigned int &width,unsigned int &height, double &framerate);
-
- //set capture MediaType
- bool setMediaType(int mediaTypeID);
-
- //get current capture MediaType
- int getMediaType();
-
- //Get the available capture formats
- bool getStreamCapabilities();
-
-
- private:
-
- CComPtr<IGraphBuilder> pGraph; //our DS filter graph
-
- CComPtr<ICaptureGraphBuilder2> pBuild; //the interface to the capture graph builder
- //used to build the filter graph
-
- CComPtr<IBaseFilter> pCapSource; //the capture source filter
-
- CComPtr<ISampleGrabber> pGrabberI; //the sample grabber's interface and filter
- CComPtr<IBaseFilter> pGrabberFilter;
-
- CComPtr<IMediaControl> pControl; //The DS filter graph control interface
- CComPtr<IMediaEvent> pEvent; //The DS filter graph event interface
-
- vpDirectShowSampleGrabberI sgCB; //Interface used to implement the frame grabber callback
-
- HRESULT hr; //contains the result of the last operation
-
- static vpDirectShowDevice * deviceList; //This contains the list of the available capture devices
- //it is shared by all the DirectShow Grabbers
-
- static unsigned int nbDevices; //the number of available devices
- int currentDevice; //the number of the current device
-
- // flag to manage CoInitialize() and CoUnInitialze()
- bool initCo ;
- //setup the directshow filtergraph with the first available device
- bool initDirectShow();
-
- //enumerates the different video inputs
- bool enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum);
-
- //selects a random video input from the enumeration and returns the associated filter
- bool selectRandomSource(CComPtr<IEnumMoniker>& ppVideoInputEnum, CComPtr<IBaseFilter>& pCapSource);
-
- //creates the filter graph
- bool createGraph();
-
- //creates the sample grabber
- bool createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter);
-
- //checks the capture filter's media type and sets flags as needed
- bool checkSourceType(CComPtr<IPin>& pCapSourcePin);
-
- //connects the filters as needed
- bool connectSourceToGrabber(CComPtr<IBaseFilter>& pCapSource, CComPtr<IBaseFilter>& pGrabberFilter);
-
- //used to convert HRESULT-associated error message to a string
- void HRtoStr(std::string str);
-
- //create the list of the available devices
- bool createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum);
-
- //get the n-th device if it is available
- bool getDevice(unsigned int n, CComPtr<IBaseFilter>& ppCapSource);
-
- //get the first available device if any
- unsigned int getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice);
-
- //removes all the filters in the graph
- bool removeAll();
-
- //Deletes an allocated AM_MEDIA_TYPE structure, including the format block
- void MyDeleteMediaType(AM_MEDIA_TYPE *pmt);
-
- //Frees the format block in an AM_MEDIA_TYPE structure
- void MyFreeMediaType(AM_MEDIA_TYPE& mt);
-
-};
-
-#endif
-#endif
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp b/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
deleted file mode 100644
index 57f753b..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
+++ /dev/null
@@ -1,289 +0,0 @@
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpDirectShowSampleGrabberI.h>
-#include <visp/vpImageConvert.h>
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-
-
-/*!
- Constructor - creates the semaphore
-*/
-vpDirectShowSampleGrabberI::vpDirectShowSampleGrabberI()
- : acqGrayDemand(false), acqRGBaDemand(false), specialMediaType(false), invertedSource(false)
-{
- //semaphore(0), max value = 1
- copySem = CreateSemaphore (NULL,0,1,NULL);
-}
-
-/*!
- Destructor - destroys the semaphore
-*/
-vpDirectShowSampleGrabberI::~vpDirectShowSampleGrabberI()
-{
- //destroys the semaphore
- CloseHandle(copySem);
-}
-
-
-STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, void **ppvObject)
-{
- if (NULL == ppvObject) return E_POINTER;
- if (riid == __uuidof(IUnknown))
- {
- *ppvObject = static_cast<IUnknown*>(this);
- return S_OK;
- }
- if (riid == __uuidof(ISampleGrabberCB))
- {
- *ppvObject = static_cast<ISampleGrabberCB*>(this);
- return S_OK;
- }
- return E_NOTIMPL;
-}
-
-/*!
- The frame grabber callback -
- Called when the input buffer is full.
- Rq : BufferLen == bmpInfo.biWidth*bmpInfo.biHeight*sizeof(vpRGBa)
-*/
-STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, long BufferLen)
-{
- //if there has been a frame demand
- if(acqGrayDemand || acqRGBaDemand)
- {
- //check if the connected media is compatible
- if(connectedMediaType.formattype==FORMAT_VideoInfo)
- {
- //retrieve the image information
- VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(connectedMediaType.pbFormat);
- BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
-
- //if biHeight > 0 and the source is not special
- //then the image needs to be verticaly flipped
- bool flip;
- if(!specialMediaType)
- flip = bmpInfo.biHeight>=0;
- //the source is fourcc and the image is inverted with this compression
- else if(invertedSource)
- flip = true;
- //fourcc and the image doesn't need to be flipped
- else
- flip = false;
-
- //if the buffer contains a RGB24 image (DS RGB24 <=> BGR)
- if(connectedMediaType.subtype==MEDIASUBTYPE_RGB24)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::BGRToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight(), flip);
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::BGRToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth(), grayIm->getHeight(), flip);
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else
- {
- unsigned long FourCC;
- FourCC = ((bmpInfo.biCompression&0xFF000000)>>24) |
- ((bmpInfo.biCompression&0x00FF0000)>>8) |
- ((bmpInfo.biCompression&0x0000FF00)<<8) |
- (bmpInfo.biCompression&0x000000FF)<<24;
- //if the buffer contains a like YUV420 image
- if(connectedMediaType.subtype==MEDIASUBTYPE_IYUV || FourCC == 'I420')
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
-
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YV12)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YV12ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YVU9)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YVU9ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YUY2 ||
- connectedMediaType.subtype==MEDIASUBTYPE_YUYV)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YVYU)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCrCbToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_UYVY)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV422ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV422ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_RGB32)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- //copy(pBuffer ,pBuffer + 4*rgbaIm->getWidth()*rgbaIm->getHeight(),rgbaIm->bitmap);
- memcpy(rgbaIm->bitmap,pBuffer ,4*rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::RGBaToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- }
- }
-
- //increment the semaphore - allows acquire to continue execution
- ReleaseSemaphore(copySem, 1, NULL);
- }
- return S_OK;
-}
-#endif
-
-#endif
diff --git a/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.h b/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.h
deleted file mode 100644
index f233f3c..0000000
--- a/src/device/framegrabber/directshow/vpDirectShowSampleGrabberI.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDirectShowSampleGrabberI.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * DirectShow framegrabber callback.
- *
- * Authors:
- * Bruno Renier
- *
- *****************************************************************************/
-
-#ifndef vpDirectShowSampleGrabberI_hh
-#define vpDirectShowSampleGrabberI_hh
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-#include <qedit.h>
-#include <stdio.h>
-#include <dshow.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-
-/*!
- This class is needed in order to implement a callback function
- associated with the grabber
-*/
-class VISP_EXPORT vpDirectShowSampleGrabberI : public ISampleGrabberCB
-{
- vpDirectShowSampleGrabberI();
- virtual ~vpDirectShowSampleGrabberI();
-
- //needed by the interface
- STDMETHODIMP_(ULONG) AddRef() { return 1; }
- STDMETHODIMP_(ULONG) Release() { return 2; }
-
- STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject);
-
- //not implemented
- STDMETHODIMP SampleCB(double Time, IMediaSample *pSample)
- {
- return E_NOTIMPL;
- }
-
- //our callback function
- STDMETHODIMP BufferCB(double Time, BYTE *pBuffer, long BufferLen);
-
-private:
- //the currently connected media type
- AM_MEDIA_TYPE connectedMediaType;
-
- //true if the source media type is not a standard one
- bool specialMediaType;
- //true if the image needs to be flipped (only for special media types)
- bool invertedSource;
-
- //booleans used to signal a demand from acquire
- bool acqGrayDemand;
- bool acqRGBaDemand;
-
- //pointer on the image to fill during the next callback if there has been a demand
- vpImage<vpRGBa> * rgbaIm;
- vpImage<unsigned char> * grayIm;
-
- //semaphore used to synchronize the productor (callback) and the consumer (acquire)
- HANDLE copySem;
-
- friend class vpDirectShowGrabberImpl;
-};
-
-#endif
-#endif
-#endif
diff --git a/src/device/framegrabber/disk/vpDiskGrabber.cpp b/src/device/framegrabber/disk/vpDiskGrabber.cpp
deleted file mode 100644
index d1c78cd..0000000
--- a/src/device/framegrabber/disk/vpDiskGrabber.cpp
+++ /dev/null
@@ -1,416 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDiskGrabber.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Disk framegrabber.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpDiskGrabber.h>
-
-
-/*!
- Elementary constructor.
-*/
-vpDiskGrabber::vpDiskGrabber()
- : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
-{
- setDirectory("/tmp");
- setBaseName("I");
- setExtension("pgm");
-
- init = false;
-}
-
-
-vpDiskGrabber::vpDiskGrabber(const char *generic_name)
- : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
-{
- setDirectory("/tmp");
- setBaseName("I");
- setExtension("pgm");
-
- init = false;
- if (strlen( generic_name ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the generic name"));
- }
-
- strcpy(this->genericName, generic_name);
- useGenericName = true;
-}
-
-
-/*!
- 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 char *dir, const char *basename,
- long number,
- int step, unsigned int noz,
- const char *ext)
- : image_number(number), image_step(step), number_of_zero(noz), useGenericName(false)
-{
- setDirectory(dir);
- setBaseName(basename);
- setExtension(ext);
-
- init = false;
-}
-
-void
-vpDiskGrabber::open(vpImage<unsigned char> &I)
-{
- long first_number = getImageNumber();
-
- vpDEBUG_TRACE(2, "first %ld", first_number);
-
- acquire(I);
-
- setImageNumber(first_number);
-
- width = I.getWidth();
- height = I.getHeight();
-
- init = true;
-}
-
-/*!
- Read the fist image of the sequence.
- The image number is not incremented.
-
-*/
-void
-vpDiskGrabber::open(vpImage<vpRGBa> &I)
-{
- // First we save the image number, so that it can be reaffected after the
- // acquisition. That means that the first image is readed twice
- long first_number = getImageNumber();
- vpDEBUG_TRACE(2, "first %ld", first_number);
-
- acquire(I);
-
- setImageNumber(first_number);
-
- width = I.getWidth();
- height = I.getHeight();
-
- init = true;
-}
-
-/*!
- Read the fist image of the sequence.
- The image number is not incremented.
-
-*/
-void
-vpDiskGrabber::open(vpImage<float> &I)
-{
- // First we save the image number, so that it can be reaffected after the
- // acquisition. That means that the first image is readed twice
- long first_number = getImageNumber();
- vpDEBUG_TRACE(2, "first %ld", first_number);
-
- acquire(I);
-
- setImageNumber(first_number);
-
- width = I.getWidth();
- height = I.getHeight();
-
- init = true;
-}
-
-/*!
- Acquire an image: read a pgm image from the disk.
- After this call, the image number is incremented considering the step.
-
- \param I the read image
- */
-void
-vpDiskGrabber::acquire(vpImage<unsigned char> &I)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::read(I, name) ;
-
- width = I.getWidth();
- height = I.getHeight();
-}
-
-/*!
- Acquire an image: read a ppm image from the disk.
- After this call, the image number is incremented considering the step.
-
- \param I the read image
- */
-void
-vpDiskGrabber::acquire(vpImage<vpRGBa> &I)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::read(I, name) ;
-
- width = I.getWidth();
- height = I.getHeight();
-
-}
-
-/*!
- Acquire an image: read a pfm image from the disk.
- After this call, the image number is incremented considering the step.
-
- \param I the read image
- */
-void
-vpDiskGrabber::acquire(vpImage<float> &I)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::readPFM(I, name) ;
-
- width = I.getWidth();
- height = I.getHeight();
-
-}
-
-/*!
- Acquire an image: read a pgm image 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<unsigned char> &I, long img_number)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::read(I, name) ;
-
- width = I.getWidth();
- height = I.getHeight();
-}
-
-/*!
- Acquire an image: read a ppm image 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<vpRGBa> &I, long img_number)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::read(I, name) ;
-
- width = I.getWidth();
- height = I.getHeight();
-
-}
-
-
-/*!
- Acquire an image: read a pfm image 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<float> &I, long img_number)
-{
-
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::readPFM(I, name) ;
-
- width = I.getWidth();
- 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 char *dir)
-{
- sprintf(directory, "%s", dir) ;
-}
-
-/*!
- Set the image base name.
-*/
-void
-vpDiskGrabber::setBaseName(const char *name)
-{
- sprintf(base_name, "%s", name) ;
-}
-
-/*!
- Set the image extension.
- */
-void
-vpDiskGrabber::setExtension(const char *ext)
-{
- sprintf(extension, "%s", ext) ;
-}
-
-/*!
- Set the number of the image to be read.
-*/
-void
-vpDiskGrabber::setImageNumber(long number)
-{
- image_number = number ;
- vpDEBUG_TRACE(2, "image number %ld", image_number);
-
-}
-
-/*!
- Set the step between two images.
-*/
-void
-vpDiskGrabber::setStep(int step)
-{
- image_step = step;
-}
-/*!
- Set the step between two images.
-*/
-void
-vpDiskGrabber::setNumberOfZero(unsigned int noz)
-{
- number_of_zero = noz ;
-}
-
-void
-vpDiskGrabber::setGenericName(const char *generic_name)
-{
- if (strlen( generic_name ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the generic name"));
- }
-
- strcpy(this->genericName, generic_name) ;
- useGenericName = true;
-}
diff --git a/src/device/framegrabber/disk/vpDiskGrabber.h b/src/device/framegrabber/disk/vpDiskGrabber.h
deleted file mode 100644
index a85b70c..0000000
--- a/src/device/framegrabber/disk/vpDiskGrabber.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDiskGrabber.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Disk framegrabber.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpDiskGrabber.h
- \brief Class to load image sequence from the disk.
-*/
-#ifndef vpDiskGrabber_hh
-#define vpDiskGrabber_hh
-
-#include <visp/vpImageIo.h>
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpDebug.h>
-
-/*!
- \class vpDiskGrabber
-
- \ingroup Framegrabber
-
- \brief Class to grab (ie. read) images from the disk.
-
- Defined a virtual video device. "Grab" the images from the disk.
- Derived from the vpFrameGrabber class.
-
- \sa vpFrameGrabber
-
- Here an example of capture from the directory
- "/local/soft/ViSP/ViSP-images/cube". We want to acquire 10 images
- from the first named "image.0001.pgm" by steps of 2.
-
-\code
-#include <visp/vpImage.h>
-#include <visp/vpDiskGrabber.h>
-
-int main(){
- vpImage<unsigned char> I; // Grey level image
-
- // Declare a framegrabber able to read a sequence of successive
- // images from the disk
- vpDiskGrabber g;
-
- // Set the path to the directory containing the sequence
- g.setDirectory("/local/soft/ViSP/ViSP-images/cube");
- // Set the image base name. The directory and the base name constitute
- // the constant part of the full filename
- g.setBaseName("image.");
- // Set the step between two images of the sequence
- g.setStep(2);
- // Set the number of digits to build the image number
- g.setNumberOfZero(4);
- // Set the first frame number of the sequence
- g.setImageNumber(1);
- // Set the image file extension
- g.setExtension("pgm");
-
- // Open the framegrabber by loading the first image of the sequence
- g.open(I) ;
-
- unsigned int cpt = 1;
- // this is the loop over the image sequence
- while(cpt ++ < 10)
- {
- // read the image and then increment the image counter so that the next
- // call to acquire(I) will get the next image
- g.acquire(I) ;
- }
-}
-\endcode
-*/
-class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber
-{
-private:
- long image_number ; //!< id of the next image to be read
- int image_step ; //!< increment between two image id
- unsigned int number_of_zero ; //!< number of zero in the image name (image.00000.pgm)
-
- char directory[FILENAME_MAX] ; //!< image location
- char base_name[FILENAME_MAX] ; //!< image base name
- char extension[FILENAME_MAX] ; //!< image extension
-
- bool useGenericName;
- char genericName[FILENAME_MAX];
-
-public:
- vpDiskGrabber();
- vpDiskGrabber(const char *genericName);
- vpDiskGrabber(const char *dir, const char *basename,
- long number, int step, unsigned int noz,
- const char *ext) ;
- virtual ~vpDiskGrabber() ;
-
- void open(vpImage<unsigned char> &I) ;
- void open(vpImage<vpRGBa> &I) ;
- void open(vpImage<float> &I) ;
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
- void acquire(vpImage<float> &I) ;
- void acquire(vpImage<unsigned char> &I, long image_number);
- void acquire(vpImage<vpRGBa> &I, long image_number);
- void acquire(vpImage<float> &I, long image_number) ;
-
- void close();
-
- void setDirectory(const char *dir);
- void setBaseName(const char *name);
- void setImageNumber(long number) ;
- void setStep(int a);
- void setNumberOfZero(unsigned int noz);
- void setExtension(const char *ext);
- void setGenericName(const char *genericName);
-
- /*!
- Return the current image number.
- */
- long getImageNumber() { return image_number; };
-} ;
-
-#endif
-
diff --git a/src/device/framegrabber/generic-framegrabber/vpFrameGrabber.h b/src/device/framegrabber/generic-framegrabber/vpFrameGrabber.h
deleted file mode 100644
index 975adbd..0000000
--- a/src/device/framegrabber/generic-framegrabber/vpFrameGrabber.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFrameGrabber.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Frame grabbing.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpFrameGrabber_hh
-#define vpFrameGrabber_hh
-
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-
-/*!
- \file vpFrameGrabber.h
- \brief Base class for all video devices. It is
- designed to provide a generic front end to video sources.
-*/
-
-/*!
- \class vpFrameGrabber
-
- \ingroup Framegrabber
-
- \brief Base class for all video devices. It is designed to provide a front
- end to video sources.
-
- This class should provide a virtual function that allows the acquisition
- of an image.
-
- The example below shows how to use this class.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vp1394TwoGrabber.h>
-
-int main()
-{
-#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vpFrameGrabber *g; // Generic framegrabber
-
-#if defined( VISP_HAVE_DC1394_2 )
- vp1394TwoGrabber *g_1394_2 = new vp1394TwoGrabber;
- // specific settings for firewire grabber based on libdc1394-2.x version
- g_1394_2->setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_320x240_YUV422);
- g_1394_2->setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
- g = g_1394_2;
-#elif defined( VISP_HAVE_V4L2 )
- vpV4l2Grabber *g_v4l2 = new vpV4l2Grabber;
- // specific settings for Video For Linux Two grabber
- g_v4l2->setInput(2); // Input 2 on the board
- g_v4l2->setFramerate(vpV4l2Grabber::framerate_50fps); // 50 fps
- g_v4l2->setWidth(384); // Acquired images are 768 width
- g_v4l2->setHeight(288); // Acquired images are 576 height
- g_v4l2->setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
- g = g_v4l2;
-#endif
-
- g->open(I); // Open the framegrabber
- g->acquire(I); // Acquire an image
- vpImageIo::write(I, "image.pgm"); // Write image on the disk
-#endif
-}
- \endcode
-
-
- \author Eric Marchand (Eric.Marchand at irisa.fr), Irisa / Inria Rennes
-*/
-class VISP_EXPORT vpFrameGrabber
-{
-public :
- bool init ; //!< Set to true if the frame grabber has been initialized.
-protected:
- unsigned int height ; //!< Number of rows in the image.
- unsigned int width ; //!< Number of columns in the image.
-
-
-public:
- //! Return the number of rows in the image.
- inline unsigned int getHeight() const { return height ; }
- //! Return the number of columns in the image.
- inline unsigned int getWidth() const { return width ; }
-
-public:
- vpFrameGrabber() : init(false), height(0), width(0) {};
- virtual ~vpFrameGrabber() { ; }
-
- virtual void open(vpImage<unsigned char> &I) =0 ;
- virtual void open(vpImage<vpRGBa> &I) =0 ;
-
- virtual void acquire(vpImage<unsigned char> &I) =0 ;
- virtual void acquire(vpImage<vpRGBa> &I) =0 ;
-
-
- /*!
- This virtual function is used to de-allocate
- the memory used by a specific frame grabber
- */
- virtual void close() =0 ;
-
-} ;
-
-#endif
diff --git a/src/device/framegrabber/generic-framegrabber/vpFrameGrabberException.h b/src/device/framegrabber/generic-framegrabber/vpFrameGrabberException.h
deleted file mode 100644
index 1439f7a..0000000
--- a/src/device/framegrabber/generic-framegrabber/vpFrameGrabberException.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFrameGrabberException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpFrameGrabber class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpFrameGrabberException_H
-#define __vpFrameGrabberException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpFrameGrabberException.h
- \brief error that can be emited by the vpFrameGrabber class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- \ingroup Exception
-
- \brief Error that can be emited by the vpFrameGrabber class and its derivates
- */
-class VISP_EXPORT vpFrameGrabberException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpFrameGrabber member
- */
- enum errorFrameGrabberCodeEnum
- {
- settingError,
- initializationError,
- otherError
- } ;
-
- public:
- vpFrameGrabberException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpFrameGrabberException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpFrameGrabberException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif /* #ifndef __vpFrameGrabberException_H */
diff --git a/src/device/framegrabber/v4l2/vpV4l2Grabber.cpp b/src/device/framegrabber/v4l2/vpV4l2Grabber.cpp
deleted file mode 100644
index 57ec4c7..0000000
--- a/src/device/framegrabber/v4l2/vpV4l2Grabber.cpp
+++ /dev/null
@@ -1,1366 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpV4l2Grabber.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Framegrabber based on Video4Linux2 driver.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpV4l2Grabber.cpp
- \brief class for the Video For Linux 2 video device framegrabbing.
- \ingroup libdevice
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_V4L2
-
-#include <stdio.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <sys/time.h>
-#include <sys/ioctl.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <sys/mman.h>
-#include <errno.h>
-#include <iostream>
-
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpFrameGrabberException.h>
-#include <visp/vpImageIo.h>
-
-const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
-const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
-const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
-const __u32 vpV4l2Grabber::MAX_NORM = 16;
-const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
-const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
-const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
-const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
-#define vpCLEAR(x) memset (&(x), 0, sizeof (x))
-
-/*!
- Default constructor.
-
- Setup the Video For Linux Two (V4L2) driver in streaming mode.
-
- Default settings are:
-
- - Device name: /dev/video0: To change it use setDevice()
-
- - Number of ring buffers: 3. To change this value use setNBuffers(). For non
- real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to
- set the number of buffers to 3.
-
- - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
- fps. These framerates are reachable only if enought buffers are set.
-
- - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
- to change it.
-
- - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
- or setWidth() and setHeight to change it.
-
- \code
- vpImage<unsigned char> I; // Grey level image
-
- vpV4l2Grabber g;
- g.setInput(2); // Input 2 on the board
- g.setWidth(768); // Acquired images are 768 width
- g.setHeight(576); // Acquired images are 576 height
- g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
- g.open(I); // Open the grabber
-
- g.acquire(I); // Acquire a 768x576 grey image
-
- \endcode
-
-*/
-vpV4l2Grabber::vpV4l2Grabber()
- : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
- fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
- waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
- m_input(vpV4l2Grabber::DEFAULT_INPUT),
- m_framerate(vpV4l2Grabber::framerate_25fps),
- m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
- m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-{
- setDevice("/dev/video0");
- setNBuffers(3);
- setFramerate(vpV4l2Grabber::framerate_25fps);
- setInput(vpV4l2Grabber::DEFAULT_INPUT);
- setScale(vpV4l2Grabber::DEFAULT_SCALE);
- setPixelFormat(V4L2_YUYV_FORMAT);
-
- init = false;
-}
-
-/*!
- Default constructor.
-
- Setup the Video For Linux Two (V4L2) driver in streaming mode.
-
- \param verbose : If true activates the verbose mode.
-
- Default settings are:
-
- - Device name: /dev/video0: To change it use setDevice()
-
- - Number of ring buffers: 3. To change this value use setNBuffers(). For non
- real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to
- set the number of buffers to 3.
-
- - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
- fps. These framerates are reachable only if enought buffers are set.
-
- - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
- to change it.
-
- - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
- or setWidth() and setHeight to change it.
-
- \code
- vpImage<unsigned char> I; // Grey level image
-
- vpV4l2Grabber g(true); // Activates the verbose mode
- g.setInput(2); // Input 2 on the board
- g.setWidth(768); // Acquired images are 768 width
- g.setHeight(576); // Acquired images are 576 height
- g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
- g.open(I); // Open the grabber
-
- g.acquire(I); // Acquire a 768x576 grey image
-
- \endcode
-
-*/
-vpV4l2Grabber::vpV4l2Grabber(bool verbose)
- : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
- fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
- waiton_cpt(0), index_buffer(0), m_verbose(verbose), m_nbuffers(3), field(0), streaming(false),
- m_input(vpV4l2Grabber::DEFAULT_INPUT),
- m_framerate(vpV4l2Grabber::framerate_25fps),
- m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
- m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-{
- setDevice("/dev/video0");
- setNBuffers(3);
- setFramerate(vpV4l2Grabber::framerate_25fps);
- setInput(vpV4l2Grabber::DEFAULT_INPUT);
- setScale(vpV4l2Grabber::DEFAULT_SCALE);
- setPixelFormat(V4L2_YUYV_FORMAT);
-
- init = false;
-}
-
-
-/*!
- Constructor.
-
- Setup the Video For Linux Two (V4L2) driver in streaming mode.
-
- \param input : Video input port.
- \param scale : Decimation factor.
-
- Default settings are:
-
- - Device name: /dev/video0: To change it use setDevice()
-
- - Number of ring buffers: 3. To change this value use setNBuffers(). For non
- real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to
- set the number of buffers to 3.
-
- - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
- fps. These framerates are reachable only if enought buffers are set.
-
- \code
- vpImage<unsigned char> I; // Grey level image
-
- vpV4l2Grabber g(1, 2); // Select input 1, and half full size resolution images.
- g.open(I); // Open the grabber
-
- g.acquire(I); // Acquire a 384x288 grey image
-
- \endcode
-*/
-vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
- : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
- fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
- waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
- m_input(vpV4l2Grabber::DEFAULT_INPUT),
- m_framerate(vpV4l2Grabber::framerate_25fps),
- m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
- m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-{
- setDevice("/dev/video0");
- setNBuffers(3);
- setFramerate(vpV4l2Grabber::framerate_25fps);
- setInput(input);
- setScale(scale);
-
- init = false;
-}
-
-/*!
- Constructor.
-
- Setup the Video For Linux Two (V4L2) driver in streaming mode.
-
- \param I : Image data structure (grey 8 bits image)
- \param input : Video input port.
- \param scale : Decimation factor.
-
- Default settings are:
-
- - Device name: /dev/video0: To change it use setDevice()
-
- - Number of ring buffers: 3. To change this value use setNBuffers(). For non
- real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to
- set the number of buffers to 3.
-
- - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
- fps. These framerates are reachable only if enought buffers are set.
-
- \code
- vpImage<unsigned char> I; // Grey level image
-
- vpV4l2Grabber g(I, 1, 2); // Select input 1, and half full size resolution
- // images and open the grabber
-
- g.acquire(I); // Acquire a 384x288 grey image
-
- \endcode
-*/
-vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale )
- : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
- fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
- waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
- m_input(vpV4l2Grabber::DEFAULT_INPUT),
- m_framerate(vpV4l2Grabber::framerate_25fps),
- m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
- m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-{
- setDevice("/dev/video0");
- setNBuffers(3);
- setFramerate(vpV4l2Grabber::framerate_25fps);
- setInput(input);
- setScale(scale);
-
- init = false;
-
- open(I);
-}
-
-/*!
- Constructor.
-
- Setup the Video For Linux Two (V4L2) driver in streaming mode.
-
- \param I : Image data structure (Color RGB32 bits image)
- \param input : Video input port.
- \param scale : Decimation factor.
-
- Default settings are:
-
- - Device name: /dev/video0: To change it use setDevice()
-
- - Number of ring buffers: 3. To change this value use setNBuffers(). For non
- real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to
- set the number of buffers to 3.
-
- - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
- fps. These framerates are reachable only if enought buffers are set.
-
- \code
- vpImage<vpRGBa> I; // Color image
-
- vpV4l2Grabber g(I, 1, 2); // Select input 1, and half full size resolution
- // images and open the grabber
-
- g.acquire(I); // Acquire a 384x288 color image
-
- \endcode
-
-*/
-vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale )
- : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
- fps(0), fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
- waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
- m_input(vpV4l2Grabber::DEFAULT_INPUT),
- m_framerate(vpV4l2Grabber::framerate_25fps),
- m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
- m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-{
- setDevice("/dev/video0");
- setNBuffers(3);
- setFramerate(vpV4l2Grabber::framerate_25fps);
- setInput(input);
- setScale(scale);
-
- init = false;
-
- open(I);
-}
-
-/*!
- Destructor.
-
- \sa close()
-*/
-vpV4l2Grabber::~vpV4l2Grabber()
-{
- close() ;
-}
-
-/*!
- Set the video input port on the board.
-*/
-void
-vpV4l2Grabber::setInput(unsigned input)
-{
- this->m_input = input;
-}
-
-/*!
- Set the decimation factor applied to full resolution images (768x576).
-
- \exception vpFrameGrabberException::settingError : Wrong scale (should be
- between 1 and 16).
-
- \param scale : Decimation factor. If scale is set to 2, 384x288 images will
- be acquired.
-
- An other way to specify the image size is to use setWidth() and setHeight().
-
-*/
-void
-vpV4l2Grabber::setScale(unsigned scale)
-{
- if ((scale <1) || (scale >16))
- {
- close();
-
- vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16",scale) ;
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Wrong scale") );
- }
-
- setWidth(640/scale);
- setHeight(480/scale);
-}
-
-/*!
- Initialize image acquisition in grey format.
- Set the pixel format acquisition to vpV4l2Grabber::V4L2_GREY_FORMAT.
-
- \param I : Image data structure (8 bits image). Once the device is open,
- the image is resized to the current acquisition size.
-
- \exception vpFrameGrabberException::settingError : Wrong input channel.
-
-*/
-void
-vpV4l2Grabber::open(vpImage<unsigned char> &I)
-{
- open();
-
- if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
- {
- std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
- }
-
- vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
-
- try {
- setFormat();
-
- startStreaming();
- }
- catch(...) {
- if (m_verbose) {
- std::cout << "Requested pixel format [" << req_pixelformat
- << "] not compatible with camera" << std::endl;
- std::cout << "Try to found a compatible pixel format..." << std::endl;
- }
-
- // try to fing a compatible format
- for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
- if (format == req_pixelformat) {
- continue;
- }
- try {
- setPixelFormat((vpV4l2PixelFormatType)format);
- setFormat();
- startStreaming();
- if (m_verbose)
- std::cout << "This format [" << m_pixelformat
- << "] is compatible with camera" << std::endl;
-
- break;
- }
- catch (...) {
- if (m_verbose)
- std::cout << "This format [" << m_pixelformat
- << "] is not compatible with camera" << std::endl;
- if (format == (int)V4L2_MAX_FORMAT) {
- std::cout << "No pixel format compatible with the camera was found" << std::endl;
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "No pixel format compatible with the camera was found"));
- }
- }
- }
- }
-
- I.resize(height, width) ;
-
- init = true;
-}
-
-/*!
- Initialize image acquisition in color RGB32 format.
- Set the pixel format acquisition to vpV4l2Grabber::V4L2_RGB32_FORMAT.
-
-
- \param I : Image data structure (RGB32 bits image). Once the device is open,
- the image is resized to the current acquisition size.
-
-*/
-void
-vpV4l2Grabber::open(vpImage<vpRGBa> &I)
-{
- open();
-
- if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
- {
- std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
- }
-
- vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
-
- try {
- setFormat();
-
- startStreaming();
- }
- catch(...) {
- if (m_verbose) {
- std::cout << "Requested pixel format [" << m_pixelformat
- << "] not compatible with camera" << std::endl;
- std::cout << "Try to found a compatible pixel format..." << std::endl;
- }
-
- // try to fing a compatible format
- for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
- if (format == req_pixelformat) {
- continue;
- }
- try {
- setPixelFormat((vpV4l2PixelFormatType)format);
- setFormat();
- startStreaming();
- if (m_verbose)
- std::cout << "This format [" << m_pixelformat
- << "] is compatible with camera" << std::endl;
-
- break;
- }
- catch (...) {
- if (m_verbose)
- std::cout << "This format [" << m_pixelformat
- << "] is not compatible with camera" << std::endl;
-
- }
- }
- }
-
- I.resize(height, width) ;
-
- init = true;
-}
-
-
-
-/*!
- Acquire a grey level image.
-
- \param I : Image data structure (8 bits image)
-
- \exception vpFrameGrabberException::initializationError : Frame grabber not
- initialized.
-
- \sa getField()
-*/
-void
-vpV4l2Grabber::acquire(vpImage<unsigned char> &I)
-{
- struct timeval timestamp;
-
- acquire(I, timestamp);
-}
-
-/*!
- Acquire a grey level image.
-
- \param I : Image data structure (8 bits image).
-
- \param timestamp : Timeval data structure providing the unix time
- at which the frame was captured in the ringbuffer.
-
- \exception vpFrameGrabberException::initializationError : Frame grabber not
- initialized.
-
- \sa getField()
-*/
-void
-vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval ×tamp)
-{
- if (init==false)
- {
- open(I);
- }
-
- if (init==false)
- {
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "V4l2 frame grabber not initialized") );
- }
-
- unsigned char *bitmap ;
- bitmap = waiton(index_buffer, timestamp);
-
- if ((I.getWidth() != width)||(I.getHeight() != height))
- I.resize(height, width) ;
-
- switch(m_pixelformat) {
- case V4L2_GREY_FORMAT:
- memcpy(I.bitmap, bitmap, height * width*sizeof(unsigned char));
- break;
- case V4L2_RGB24_FORMAT:
- vpImageConvert::RGBToGrey((unsigned char *) bitmap, I.bitmap, width*height);
-
- break;
- case V4L2_RGB32_FORMAT:
- vpImageConvert::RGBaToGrey((unsigned char *) bitmap, I.bitmap, width*height);
-
- break;
- case V4L2_BGR24_FORMAT:
- vpImageConvert::BGRToGrey( (unsigned char *) bitmap, I.bitmap, width, height, false);
-
- break;
- case V4L2_YUYV_FORMAT:
- vpImageConvert::YUYVToGrey( (unsigned char *) bitmap, I.bitmap, width*height);
-
- break;
- default:
- std::cout << "V4L2 conversion not handled" << std::endl;
- break;
- }
-
- queueAll();
-}
-
-/*!
- Acquire a color image.
-
- \param I : Image data structure (32 bits image)
-
- \exception vpFrameGrabberException::initializationError : Frame grabber not
- initialized.
-
- \sa getField()
-*/
-void
-vpV4l2Grabber::acquire(vpImage<vpRGBa> &I)
-{
- struct timeval timestamp;
-
- acquire(I, timestamp);
-}
-
-/*!
- Acquire a color image.
-
- \param I : Image data structure (32 bits image).
-
- \param timestamp : Timeval data structure providing the unix time
- at which the frame was captured in the ringbuffer.
-
- \exception vpFrameGrabberException::initializationError : Frame grabber not
- initialized.
-
- \sa getField()
-*/
-void
-vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp)
-{
- if (init==false)
- {
- open(I);
- }
-
- if (init==false)
- {
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "V4l2 frame grabber not initialized") );
- }
-
- unsigned char *bitmap ;
- bitmap = waiton(index_buffer, timestamp);
-
- if ((I.getWidth() != width)||(I.getHeight() != height))
- I.resize(height, width) ;
-
- // The framegrabber acquire aRGB format. We just shift the data from 1 byte all the data and initialize the last byte
-
- switch(m_pixelformat) {
- case V4L2_GREY_FORMAT:
- vpImageConvert::GreyToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width*height);
- break;
- case V4L2_RGB24_FORMAT:
- vpImageConvert::RGBToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width*height);
-
- break;
- case V4L2_RGB32_FORMAT:
- // The framegrabber acquire aRGB format. We just shift the data
- // from 1 byte all the data and initialize the last byte
- memcpy(I.bitmap, bitmap + 1, height * width * sizeof(vpRGBa) - 1);
- I[height-1][width-1].A = 0;
- break;
- case V4L2_BGR24_FORMAT:
- vpImageConvert::BGRToRGBa((unsigned char *) bitmap, (unsigned char *) I.bitmap, width, height, false);
- break;
- case V4L2_YUYV_FORMAT:
- vpImageConvert::YUYVToRGBa( (unsigned char *) bitmap, (unsigned char *) I.bitmap, width, height);
- break;
- default:
- std::cout << "V4l2 conversion not handled" << std::endl;
- break;
- }
-
- queueAll();
-}
-/*!
-
- Return the field (odd or even) corresponding to the last acquired
- frame.
-
- This method is to call after acquire() and has only a mean if the acquisition
- framerate is set to 50 fps.
-
- \return Field of the acquired frame (0 if odd field, 1 if even field).
-
- \exception vpFrameGrabberException::otherError : Video device returns a bad
- frame field.
-
- \sa acquire(), setFramerate()
-
-*/
-bool
-vpV4l2Grabber::getField()
-{
- if(field == 2) return 0; //top field
- else if (field == 3) return 1; //bottom field;
- else {
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "V4l2 returns a bad frame field") );
- return false;
- }
-}
-/*!
-
- Set the frame format depending on the framerate acquisition.
-
- \param framerate : The framerate for the acquisition.
- - If vpV4l2Grabber::framerate_25fps use vpV4l2Grabber::V4L2_IMAGE_FORMAT,
- - else if vpV4l2Grabber::framerate_50fps use vpV4l2Grabber::V4L2_FRAME_FORMAT.
- \warning If you want to acquire frames at 25 fps or 50 fps, you have to be
- aware of the number of buffers required for the streaming. A typical value
- could be 3 (see setNBuffers()).
-
- \sa getFramerate(), setNBuffers()
-
-*/
-void
-vpV4l2Grabber::setFramerate(vpV4l2Grabber::vpV4l2FramerateType framerate)
-{
- this->m_framerate = framerate;
-
- if (framerate == vpV4l2Grabber::framerate_25fps)
- setFrameFormat(V4L2_IMAGE_FORMAT);
- else
- setFrameFormat(V4L2_FRAME_FORMAT);
-}
-
-/*!
-
- Return the framerate of the acquisition.
-
- \return The actual framerate of the framegrabber.
-
- \sa setFramerate()
-*/
-
-
-vpV4l2Grabber::vpV4l2FramerateType
-vpV4l2Grabber::getFramerate()
-{
- return m_framerate;
-}
-
-
-/*!
- Close the video device.
-*/
-void
-vpV4l2Grabber::close()
-{
- stopStreaming();
- streaming = false;
-
- if (fd >= 0){
- //vpTRACE("v4l2_close()");
- v4l2_close (fd);
- fd = -1;
- }
-
- if (inp != NULL) { delete [] inp; inp = NULL; }
- if (std != NULL) { delete [] std; std = NULL; }
- if (fmt != NULL) { delete [] fmt; fmt = NULL; }
- if (ctl != NULL) { delete [] ctl; ctl = NULL; }
- if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
- if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
-}
-
-/*!
-
- Open the Video For Linux Two device.
-
- \exception vpFrameGrabberException::initializationError : Can't access to
- video device.
-
- \exception vpFrameGrabberException::otherError : Can't query video
- capabilities.
-
-*/
-void
-vpV4l2Grabber::open()
-{
- /* Open Video Device */
- struct stat st;
-
- if (-1 == stat (device, &st)) {
- fprintf (stderr, "Cannot identify '%s': %d, %s\n",
- device, errno, strerror (errno));
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Cannot identify video device") );
-
- }
-
- if (!S_ISCHR (st.st_mode)) {
- fprintf (stderr, "%s is no device\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No device") );
-
- }
- fd = v4l2_open (device, O_RDWR | O_NONBLOCK, 0);
- if (fd < 0) {
- close();
-
- vpERROR_TRACE ("No video device \"%s\"\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Can't access to video device") );
-
- }
-
- if (inp != NULL) { delete [] inp; inp = NULL; }
- if (std != NULL) { delete [] std; std = NULL; }
- if (fmt != NULL) { delete [] fmt; fmt = NULL; }
- if (ctl != NULL) { delete [] ctl; ctl = NULL; }
- if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
- if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
-
- inp = new struct v4l2_input [vpV4l2Grabber::MAX_INPUTS];
- std = new struct v4l2_standard [vpV4l2Grabber::MAX_NORM];
- fmt = new struct v4l2_fmtdesc [vpV4l2Grabber::MAX_FORMAT];
- ctl = new struct v4l2_queryctrl [vpV4l2Grabber::MAX_CTRL*2];
- buf_v4l2 = new struct v4l2_buffer [vpV4l2Grabber::MAX_BUFFERS];
- buf_me = new struct ng_video_buf [vpV4l2Grabber::MAX_BUFFERS];
-
- /* Querry Video Device Capabilities */
- if ( v4l2_ioctl (fd, VIDIOC_QUERYCAP, &cap) == -1 ) {
- close();
- fprintf (stderr, "%s is no V4L2 device\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Is not a V4L2 device") );
- }
- if (m_verbose) {
- fprintf(stdout, "v4l2 info:\n"
- " device: %s\n"
- " %s %d.%d.%d / %s @ %s\n",
- device,
- cap.driver,
- (cap.version >> 16) & 0xff,
- (cap.version >> 8) & 0xff,
- cap.version & 0xff,
- cap.card, cap.bus_info);
- if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
- fprintf(stdout, " Support overlay\n");
- else
- fprintf(stdout, " Does not support overlay\n");
- if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
- fprintf(stdout, " Support capture\n");
- else
- fprintf(stdout, " Does not support capture\n");
- if (cap.capabilities & V4L2_CAP_TUNER)
- fprintf(stdout, " Support tuning\n");
- else
- fprintf(stdout, " Does not support tuning\n");
- if (cap.capabilities & V4L2_CAP_STREAMING)
- fprintf(stdout, " Support streaming capture.\n");
- else
- fprintf(stdout, " Does not support streaming capture\n");
- if(cap.capabilities & V4L2_CAP_ASYNCIO)
- fprintf(stdout, " Support asynchronous I/O methods\n");
- else
- fprintf(stdout, " Does not support asynchronous I/O methods\n");
- if(cap.capabilities & V4L2_CAP_TIMEPERFRAME)
- fprintf(stdout, " Support time per frame field\n");
- else
- fprintf(stdout, " Does not support time per frame field\n");
- // Get framerate
- streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
- fprintf(stdout, " Current acquisition framerate: %d fps\n",
- streamparm.parm.output.timeperframe.denominator);
- }
- }
-
- getCapabilities();
-}
-
-/*!
-
- Get device capabilities.
-
- \exception vpFrameGrabberException::otherError : Can't get video parameters.
-*/
-void
-vpV4l2Grabber::getCapabilities()
-{
- for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
- inp[ninputs].index = ninputs;
- if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
- break;
- }
- for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
- std[nstds].index = nstds;
- if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
- break;
-
- }
- for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
- fmt[nfmts].index = nfmts;
- fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
- break;
- }
-
- streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1)
- {
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't get video parameters") );
- }
-}
-
-/*!
-
- Set the capture format using the settings:
- - image size : specified by setWidth(), setHeight() or setScale()
- - frame format : specified by setFramerate()
- - pixel format : specified by setPixelFormat().
-
-
- \exception vpFrameGrabberException::settingError : Bad format, probably do to
- a wrong scale.
-
- \exception vpFrameGrabberException::otherError : Can't get video format.
-*/
-void
-vpV4l2Grabber::setFormat()
-{
- fmt_me.width = width;
- fmt_me.height = height;
- //fmt_me.bytesperline = width; // bad (normally width * depth / 8), but works
- // because initialized later by an ioctl call to VIDIOC_S_FMT
-
- switch(m_pixelformat) {
- case V4L2_GREY_FORMAT : fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
- if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
- break;
- case V4L2_RGB24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
- if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
- break;
- case V4L2_RGB32_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
- if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
- break;
- case V4L2_BGR24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
- if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
- break;
- case V4L2_YUYV_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
- if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
- break;
-
- default:
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Bad format, probably do to a wrong scale"));
- }
-
- /* Get Video Format */
- vpCLEAR (fmt_v4l2);
-
- fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
- if (v4l2_ioctl (fd, VIDIOC_G_FMT, &fmt_v4l2) == -1 ) {
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't get video format") );
- }
- fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
- fmt_v4l2.fmt.pix.width = fmt_me.width;
- fmt_v4l2.fmt.pix.height = fmt_me.height;
- //printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
-
-
- switch (m_frameformat) {
- case V4L2_FRAME_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
- if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
- }
- break;
- case V4L2_IMAGE_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
- if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
- }
- break;
- default:
- close();
-
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Unrecognized frame format") );
- }
-
- //height and width of the captured image or frame
- if( m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE )
- {
- height = FRAME_SIZE;
- }
- //printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
-
- if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't set video format") );
- }
-
- if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Bad pixel format") );
- }
-
- /* Buggy driver paranoia. */
- unsigned int min = fmt_v4l2.fmt.pix.width * 2;
- if (fmt_v4l2.fmt.pix.bytesperline < min)
- fmt_v4l2.fmt.pix.bytesperline = min;
- min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
- if (fmt_v4l2.fmt.pix.sizeimage < min)
- fmt_v4l2.fmt.pix.sizeimage = min;
-
- fmt_me.width = fmt_v4l2.fmt.pix.width;
- fmt_me.height = fmt_v4l2.fmt.pix.height;
- fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
-
- if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (%dx%d, %c%c%c%c, %d byte, %d bytes per line)\n",
- fmt_me.width, fmt_me.height,
- fmt_v4l2.fmt.pix.pixelformat & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff,
- fmt_v4l2.fmt.pix.sizeimage,
- fmt_v4l2.fmt.pix.bytesperline);
- }
-
-}
-/*!
-
- Launch the streaming capture mode and map device memory into application
- address space.
-
- \exception vpFrameGrabberException::otherError : If a problem occurs.
-
-*/
-void
-vpV4l2Grabber::startStreaming()
-{
- if (streaming == true) { // Acquisition in process.
- stopStreaming();
- streaming = false;
- }
-
- /* setup buffers */
- memset (&(reqbufs), 0, sizeof (reqbufs));
- reqbufs.count = m_nbuffers;
- reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- reqbufs.memory = V4L2_MEMORY_MMAP;
-
-
- if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1)
- {
- if (EINVAL == errno) {
- fprintf (stderr, "%s does not support "
- "memory mapping\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Does not support memory mapping") );
- }
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't require video buffers") );
- }
-
- for (unsigned i = 0; i < reqbufs.count; i++) {
- // Clear the buffer
- memset (&(buf_v4l2[i]), 0, sizeof (buf_v4l2[i]));
- buf_v4l2[i].index = i;
- buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
- buf_v4l2[i].length = 0;
- if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't query video buffers") );
- }
- 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);
-
- if(buf_me[i].data == MAP_FAILED)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't map memory") );
- }
-
- 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]);
- }
-
- /* queue up all buffers */
- queueAll();
-
- /* Set video stream capture on */
- if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type)<0)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't start streaming") );
- }
-
- streaming = true;
-}
-
-/*!
-
- Stops the streaming capture mode and unmap the device memory.
-
- \exception vpFrameGrabberException::otherError : if can't stop streaming.
-*/
-void
-vpV4l2Grabber::stopStreaming()
-{
- unsigned int i;
-
- //nothing to do if (fd < 0) or if (streaming == false)
- if ((fd >= 0) && (streaming == true)) {
-
- //vpTRACE(" Stop the streaming...");
- /* stop capture */
- fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_STREAMOFF,&fmt_v4l2.type)) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't stop streaming") );
- }
- /* free buffers */
- for (i = 0; i < reqbufs.count; i++) {
- if (m_verbose)
- printBufInfo(buf_v4l2[i]);
- //vpTRACE("v4l2_munmap()");
-
- if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't unmap memory") );
- }
- }
- queue = 0;
- waiton_cpt = 0;
- streaming = false;
- }
-}
-
-/*!
- Fill the next buffer. If all the buffers are filled return NULL.
-
- Update the buffer index. If all the buffers are filled index is set to -1.
-
- \param index : Index in the buffer where image is available.
-
- \param timestamp : Timeval data structure providing the unix time
- [microseconds] at which the frame was captured in the ringbuffer.
-
- \exception vpFrameGrabberException::otherError : If can't access to the
- frame.
-*/
-unsigned char *
-vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp)
-{
- struct v4l2_buffer buf;
- struct timeval tv;
- fd_set rdset;
-
- /* wait for the next frame */
- again:
-
- tv.tv_sec = 30;
- tv.tv_usec = 0;
- FD_ZERO(&rdset);
- FD_SET(static_cast<unsigned int>(fd), &rdset);
- switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
- case -1:
- if (EINTR == errno)
- goto again;
- index = 0;
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't access to the frame") );
- return NULL;
- case 0:
- index = 0;
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't access to the frame: timeout") );
- return NULL;
- }
-
-
- /* get it */
- memset(&buf, 0, sizeof(buf));
- buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
- if (-1 == v4l2_ioctl(fd,VIDIOC_DQBUF, &buf)) {
- index = 0;
- switch(errno)
- {
- case EAGAIN:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_DQBUF: EAGAIN") );
- break;
- case EINVAL:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_DQBUF: EINVAL") );
- break;
- case ENOMEM:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_DQBUF: ENOMEM") );
- break;
- default:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_DQBUF") );
- break;
- }
- return NULL;
- }
-
- waiton_cpt++;
- buf_v4l2[buf.index] = buf;
-
- index = buf.index;
-
- field = buf_v4l2[index].field;
-
- timestamp = buf_v4l2[index].timestamp;
-
- // if(m_verbose)
- // {
- // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
-
- // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
- // }
- return buf_me[buf.index].data;
-}
-
-/*!
-
- Capture helpers.
-
-*/
-int
-vpV4l2Grabber::queueBuffer()
-{
- unsigned int frame = queue % reqbufs.count;
- int rc;
-
-
- if (0 != buf_me[frame].refcount) {
- if (0 != queue - waiton_cpt)
- return -1;
- fprintf(stderr,"v4l2: waiting for a free buffer..............\n");
- //ng_waiton_video_buf(h->buf_me+frame);
- std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
- }
-
- // std::cout << "frame: " << frame << std::endl;
- rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
- if (0 == rc)
- queue++;
- else
- {
- switch(errno)
- {
- case EAGAIN:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_QBUF: EAGAIN") );
- break;
- case EINVAL:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_QBUF: EINVAL") );
- break;
- case ENOMEM:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_QBUF: ENOMEM") );
- break;
- default:
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "VIDIOC_QBUF") );
- break;
- }
- }
- return rc;
-}
-
-/*!
-
- Call the queue buffer private method if needed
-
-*/
-void
-vpV4l2Grabber::queueAll()
-{
- for (;;) {
- if (queue - waiton_cpt >= reqbufs.count) {
- return;
- }
- if (0 != queueBuffer()) {
- return;
- }
- }
-}
-
-/*!
-
- Get device capabilities.
-
-*/
-void
-vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
-{
- char type[40];
-
- switch(buf.type) {
- case V4L2_BUF_TYPE_VIDEO_CAPTURE: sprintf(type, "video-cap"); break;
- case V4L2_BUF_TYPE_VIDEO_OVERLAY: sprintf(type, "video-over"); break;
- case V4L2_BUF_TYPE_VIDEO_OUTPUT: sprintf(type, "video-out"); break;
- case V4L2_BUF_TYPE_VBI_CAPTURE: sprintf(type, "vbi-cap"); break;
- case V4L2_BUF_TYPE_VBI_OUTPUT: sprintf(type, "vbi-out"); break;
- default: sprintf(type, "unknown"); break;
- }
-
- fprintf(stdout,"v4l2: buf %d: %d ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n",
- buf.index, buf.type, buf.m.userptr, buf.m.offset,
- buf.length, buf.length, buf.bytesused);
-
-}
-#endif
diff --git a/src/device/framegrabber/v4l2/vpV4l2Grabber.h b/src/device/framegrabber/v4l2/vpV4l2Grabber.h
deleted file mode 100644
index 08d4ad2..0000000
--- a/src/device/framegrabber/v4l2/vpV4l2Grabber.h
+++ /dev/null
@@ -1,360 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpV4l2Grabber.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Video for linux two framegrabber.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpV4l2Grabber.h
- \brief class for the Video For Linux 2 video device framegrabbing.
- \ingroup libdevice
-*/
-
-#ifndef vpV4l2Grabber_hh
-#define vpV4l2Grabber_hh
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_V4L2
-
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/videodev2.h> // Video For Linux Two interface
-#include <libv4l2.h> // Video For Linux Two interface
-
-#include <visp/vpImage.h>
-#include <visp/vpFrameGrabber.h>
-#include <visp/vpRGBa.h>
-
-
-
-
-/*!
- \class vpV4l2Grabber
-
- \ingroup Framegrabber CameraDriver
-
- \brief Class for the Video4Linux2 video device.
-
- Information about Video4Linux can be found on
- http://linuxtv.org/v4lwiki/index.php/Main_Page
-
- This class was tested with a Pinnacle PCTV Studio/Rave board but
- also with the following webcams (Logitech QuickCam Vision Pro 9000,
- Logitech QuickCam Orbit AF, Dell latitude E6400 internal webcam).
-
- If the grabbing fail with a webcam, it means probably that you don't
- have the read/write permission on the /dev/video%%d device. You can
- set the right permissions by "sudo chmod a+rw /dev/video*".
-
- If the grabbing fails when the camera is attached to a bttv PCI
- card, it means potentially that you have not configured the linux
- bttv kernel module according to your board.
-
- For that, depending on your linux distribution check the card id in
- - /usr/share/doc/kernel-doc-2.4.20/video4linux/bttv/CARDLIST
- - or /usr/share/doc/kernel-doc-2.6.20/Documentation/video4linux/CARDLIST.bttv
-
- For example, the card id of a Pinnacle PCTV Studio/Rave board is 39.
- Once this id is determined, you have to set the bttv driver with, by adding
- \verbatim
- options bttv card=39
- \endverbatim
- in one of theses files :
- - /etc/modules.conf
- - or /etc/modprobe.conf
-
- This first example available in tutorial-grabber-v4l2.cpp shows how to grab
- and display images from an usb camera.
- \include tutorial-grabber-v4l2.cpp
-
- This other example shows how to use this grabber with an analogic camera
- attached to a bttv PCI card.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-#if defined(VISP_HAVE_V4L2)
- vpImage<unsigned char> I;
- vpV4l2Grabber g;
- g.setInput(2); // Input 2 on the board
- g.setFramerate(vpV4l2Grabber::framerate_25fps); // 25 fps
- g.setWidth(768); // Acquired images are 768 width
- g.setHeight(576); // Acquired images are 576 height
- g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
- g.open(I); // Open the grabber
-
- g.acquire(I); // Acquire a 768x576 grey image
- vpImageIo::write(I, "image.pgm"); // Save the image on the disk
-#endif
-}
- \endcode
-
-
- \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
-
-
- \sa vpFrameGrabber
-*/
-class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber
-{
-public:
- static const unsigned int DEFAULT_INPUT;
- static const unsigned int DEFAULT_SCALE;
- static const __u32 MAX_INPUTS;
- static const __u32 MAX_NORM;
- static const __u32 MAX_FORMAT;
- static const unsigned int MAX_CTRL;
- static const unsigned int MAX_BUFFERS;
- static const unsigned int FRAME_SIZE;
-
- /*! \enum vpV4l2FramerateType
- Frame rate type for capture.
- */
- typedef enum
- {
- framerate_50fps, //!< 50 frames per second
- framerate_25fps //!< 25 frames per second
- } vpV4l2FramerateType;
-
- /*! \enum vpV4l2FrameFormatType
- Frame format type for capture.
- */
- typedef enum
- {
- V4L2_FRAME_FORMAT, /*!< a field only */
- V4L2_IMAGE_FORMAT /*!< an interlaced image */
- } vpV4l2FrameFormatType;
-
- /*! \enum vpV4l2PixelFormatType
- Pixel format type for capture.
- */
- typedef enum {
- V4L2_GREY_FORMAT, /*!< 8 Greyscale */
- V4L2_RGB24_FORMAT, /*!< 24 RGB-8-8-8 */
- V4L2_RGB32_FORMAT, /*!< 32 RGB-8-8-8-8 */
- V4L2_BGR24_FORMAT, /*!< 24 BGR-8-8-8 */
- V4L2_YUYV_FORMAT, /*!< 16 YUYV 4:2:2 */
- V4L2_MAX_FORMAT
- } vpV4l2PixelFormatType;
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- struct ng_video_fmt {
- unsigned int pixelformat; /* VIDEO_* */
- unsigned int width;
- unsigned int height;
- unsigned int bytesperline; /* zero for compressed formats */
- };
-
-
- struct ng_video_buf {
- struct ng_video_fmt fmt;
- size_t size;
- unsigned char *data;
- int refcount;
- };
-#endif
-
-public:
- vpV4l2Grabber();
- vpV4l2Grabber(bool verbose);
- vpV4l2Grabber(unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- vpV4l2Grabber(vpImage<unsigned char> &I,
- unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- vpV4l2Grabber(vpImage<vpRGBa> &I,
- unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- virtual ~vpV4l2Grabber() ;
-
-public:
- void open(vpImage<unsigned char> &I) ;
- void open(vpImage<vpRGBa> &I) ;
-
- void acquire(vpImage<unsigned char> &I) ;
- void acquire(vpImage<unsigned char> &I, struct timeval ×tamp) ;
- void acquire(vpImage<vpRGBa> &I) ;
- void acquire(vpImage<vpRGBa> &I, struct timeval ×tamp) ;
- bool getField();
- vpV4l2FramerateType getFramerate();
- /*!
-
- Get the pixel format used for capture.
-
- \return Camera pixel format coding.
-
- */
- inline vpV4l2PixelFormatType getPixelFormat()
- {
- return (this->m_pixelformat);
- }
- /*!
- Activates the verbose mode to print additional information on stdout.
- \param verbose : If true activates the verbose mode.
- */
- void setVerboseMode(bool verbose) {
- this->m_verbose = verbose;
- };
- void setFramerate(vpV4l2FramerateType framerate);
-
- void setInput(unsigned input = vpV4l2Grabber::DEFAULT_INPUT) ;
-
- /*!
- Set image width to acquire.
-
- */
- inline void setWidth(unsigned w)
- {
- this->width = w;
- }
- /*!
- Set image height to acquire.
-
- */
- inline void setHeight(unsigned h)
- {
- this->height = h;
- }
-
- void setScale(unsigned scale = vpV4l2Grabber::DEFAULT_SCALE) ;
-
- /*!
-
- Set the number of buffers required for streaming data.
-
- For non real-time applications the number of buffers should be set to 1. For
- real-time applications to reach 25 fps or 50 fps a good compromise is to set
- the number of buffers to 3.
-
- \param nbuffers : Number of ring buffers.
-
- */
- inline void setNBuffers(unsigned nbuffers)
- {
- this->m_nbuffers = nbuffers;
- }
-
- /*!
- Set the device name.
-
- \param devname : Device name (like /dev/video0).
-
- */
- inline void setDevice(const std::string &devname)
- {
- sprintf(device, "%s", devname.c_str());
- }
- /*!
-
- Set the pixel format for capture.`If the specified pixel format is
- out of range, we set the V4L2_RGB24_FORMAT.
-
- \param pixelformat : Camera pixel format coding.
-
- */
- inline void setPixelFormat(vpV4l2PixelFormatType pixelformat)
- {
- this->m_pixelformat = pixelformat;
- if (this->m_pixelformat >= V4L2_MAX_FORMAT)
- this->m_pixelformat = V4L2_RGB24_FORMAT;
- }
-
- void close();
-
-private:
-
- void setFormat();
- /*!
- Set the frame format.
-
- \param frameformat :
- - vpV4l2Grabber::V4L2_FRAME_FORMAT: capture alternate fields (or frames),
- - vpV4l2Grabber::V4L2_IMAGE_FORMAT: capture interlaced images.
- */
- inline void setFrameFormat(vpV4l2FrameFormatType frameformat)
- {
- this->m_frameformat = frameformat;
- }
- void open();
- void getCapabilities();
- void startStreaming();
- void stopStreaming();
- unsigned char * waiton(__u32 &index, struct timeval ×tamp);
- int queueBuffer();
- void queueAll();
- void printBufInfo(struct v4l2_buffer buf);
-
-
- int fd;
- char device[FILENAME_MAX];
- /* device descriptions */
- struct v4l2_capability cap;
- struct v4l2_streamparm streamparm;
- struct v4l2_input *inp; //[vpV4l2Grabber::MAX_INPUTS];
- struct v4l2_standard *std; //[vpV4l2Grabber::MAX_NORM];
- struct v4l2_fmtdesc *fmt; //[vpV4l2Grabber::MAX_FORMAT];
- struct v4l2_queryctrl *ctl; //[vpV4l2Grabber::MAX_CTRL*2];
-
- /* capture */
- int fps;
- struct v4l2_format fmt_v4l2;
- struct ng_video_fmt fmt_me;
- struct v4l2_requestbuffers reqbufs;
- struct v4l2_buffer *buf_v4l2; //[vpV4l2Grabber::MAX_BUFFERS];
- struct ng_video_buf *buf_me; //[vpV4l2Grabber::MAX_BUFFERS];
- unsigned int queue;
- unsigned int waiton_cpt;
- __u32 index_buffer; //!< index of the buffer in use
-
- bool m_verbose;
- unsigned m_nbuffers;
- unsigned int field;
- bool streaming;
-
- unsigned m_input;
- vpV4l2FramerateType m_framerate;
- vpV4l2FrameFormatType m_frameformat;
- vpV4l2PixelFormatType m_pixelformat;
-} ;
-
-#endif
-#endif
-
diff --git a/src/device/kinect/vpKinect.cpp b/src/device/kinect/vpKinect.cpp
deleted file mode 100644
index 1f032d0..0000000
--- a/src/device/kinect/vpKinect.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKinect.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * API for using a Microsoft Kinect device
- * Requires libfreenect as a third party library
- *
- * Authors:
- * Celine Teuliere
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-// Note that libfreenect needs libusb-1.0 and libpthread
-#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
-
-#include <limits> // numeric_limits
-
-#include <visp/vpKinect.h>
-#include <visp/vpXmlParserCamera.h>
-
-/*!
- Default constructor.
-*/
-vpKinect::vpKinect(freenect_context *ctx, int index)
- : Freenect::FreenectDevice(ctx, index),
- m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(),
- rgbMir(), irMrgb(), DMres(DMAP_LOW_RES),
- hd(240), wd(320),
- dmap(), IRGB(),
- m_new_rgb_frame(false),
- m_new_depth_map(false),
- m_new_depth_image(false),
- height(480), width(640)
-{
- dmap.resize(height, width);
- IRGB.resize(height, width);
- vpPoseVector r(-0.0266,-0.0047,-0.0055,0.0320578,0.0169041,-0.0076519 );//!Those are the parameters found for our Kinect device. Note that they can differ from one device to another.
- rgbMir.buildFrom(r);
- irMrgb = rgbMir.inverse();
-}
-
-/*!
- Destructor.
-*/
-vpKinect::~vpKinect()
-{
-
-}
-
-void vpKinect::start(vpKinect::vpDMResolution res)
-{
- DMres = res;
- height = 480;
- width = 640;
- //!Calibration parameters are the parameters found for our Kinect device. 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;
- }
-
-#if defined(VISP_HAVE_ACCESS_TO_NAS) && defined(VISP_HAVE_XML2)
- vpXmlParserCamera cameraParser;
- char cameraXmlFile[FILENAME_MAX];
- sprintf(cameraXmlFile, "/udd/fspindle/robot/Viper850/Viper850-code/include/const_camera_Viper850.xml");
- cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width, height);
-#else
-// RGBcam.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
-// RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45, 273.27,0.04,-0.04);//old
-// RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
- RGBcam.initPersProjWithDistortion(522.5431816996,522.7191431808,311.4001982614,267.4283562142,0.0477365207,-0.0462326418);//new
-#endif
-
- this->startVideo();
- this->startDepth();
-}
-
-void vpKinect::stop()
-{
- this->stopVideo();
- this->stopDepth();
-}
-
-
-/*!
- Acquire a new RGB image.
-*/
-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<uint8_t*>(rgb);
- for (unsigned i = 0; i< height;i++){
- for (unsigned j = 0 ; j < width ; j++)
- {
- IRGB[i][j].R = rgb_[3*(width*i +j)+0];
- IRGB[i][j].G = rgb_[3*(width*i +j)+1];
- IRGB[i][j].B = rgb_[3*(width*i +j)+2];
- }
- }
-
- m_new_rgb_frame = true;
-}
-
-/*!
- Acquire a new depth image.
-
- Depth value send by the kinect is coded on 11 bits : 10 for the
- value itself (between 0 and 1023) and one for overflow.
-
- In this function this value is converted into a metric depth map and
- stored in dmap. (range : 0.3 - 5m).
-
-*/
-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<uint16_t*>(depth);
- for (unsigned i = 0; i< height;i++){
- for (unsigned j = 0 ; j < width ; j++)
- {
- dmap[i][j] = 0.1236f * tan(depth_[width*i +j] / 2842.5f + 1.1863f);//formula from http://openkinect.org/wiki/Imaging_Information
- if(depth_[width*i +j]>1023){//Depth cannot be computed
- dmap[i][j] = -1;
- }
- }
- }
- m_new_depth_map = true;
- m_new_depth_image = true;
-}
-
-
-/*!
- Get metric depth map (float).
-*/
-bool vpKinect::getDepthMap(vpImage<float>& map)
-{
- vpMutex::vpScopedLock lock(m_depth_mutex);
- if (!m_new_depth_map)
- return false;
- map = this->dmap;
- m_new_depth_map = false;
- return true;
-}
-
-
-/*!
- * Get metric depth map (float) and corresponding image.
- */
-bool vpKinect::getDepthMap(vpImage<float>& map,vpImage<unsigned char>& Imap)
-{
- // vpMutex::vpScopedLock lock(m_depth_mutex);
- vpImage<float> tempMap;
- m_depth_mutex.lock();
- if (!m_new_depth_map && !m_new_depth_image)
- {
- m_depth_mutex.unlock();
- return false;
- }
- tempMap = dmap;
-
- m_new_depth_map = false;
- m_new_depth_image = false;
- m_depth_mutex.unlock();
-
- if ((Imap.getHeight()!=hd )||(map.getHeight()!=hd))
- vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
- if (DMres == DMAP_LOW_RES){
- for(unsigned int i = 0; i < hd; i++)
- for(unsigned int j = 0; j < wd; j++){
- map[i][j] = tempMap[i<<1][j<<1];
- //if (map[i][j] != -1)
- if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
- Imap[i][j] = (unsigned char)(255*map[i][j]/5);
- else
- Imap[i][j] = 255;
- }
- }
- else
- {
- for (unsigned i = 0; i< height;i++)
- for (unsigned j = 0 ; j < width ; j++){
- map[i][j] = tempMap[i][j];
- //if (map[i][j] != -1)
- if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
- Imap[i][j] = (unsigned char)(255*map[i][j]/5);
- else
- Imap[i][j] = 255;
- }
- }
-
- return true;
-}
-
-
-/*!
- Get RGB image
-*/
-bool vpKinect::getRGB(vpImage<vpRGBa>& I_RGB)
-{
- vpMutex::vpScopedLock lock(m_rgb_mutex);
- if (!m_new_rgb_frame)
- return false;
- I_RGB = this->IRGB;
- m_new_rgb_frame = false;
- return true;
-}
-
-/*!
- Warp the RGB frame to the depth camera frame. The size of the resulting IrgbWarped frame is the same as the size of the depth map Idepth
-*/
-void vpKinect::warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped)
-{
- if ((Idepth.getHeight()!=hd )||(Idepth.getWidth()!=wd)){
- vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
- }
- else{
- if((IrgbWarped.getHeight()!=hd )||(IrgbWarped.getWidth()!=wd))
- IrgbWarped.resize(hd, wd);
- IrgbWarped=0;
- double x1=0., y1=0., x2=0., y2=0., Z1, Z2;
- vpImagePoint imgPoint(0,0);
- double u=0., v=0.;
- vpColVector P1(4),P2(4);
-
-// std::cout <<"rgbMir : "<<rgbMir<<std::endl;
-
- for (unsigned int i = 0; i< hd;i++)
- for (unsigned int j = 0 ; j < wd ; j++){
- //! Compute metric coordinates in the ir camera Frame :
- vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
- Z1 = Idepth[i][j];
- //if (Z1!=-1){
- if (std::fabs(Z1+1) <= std::numeric_limits<double>::epsilon()){
- P1[0]=x1*Z1;
- P1[1]=y1*Z1;
- P1[2]=Z1;
- P1[3]=1;
-
- //! Change frame :
- P2 = rgbMir*P1;
- Z2 = P2[2];
- //if (Z2!= 0){
- if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()){
- x2 = P2[0]/P2[2];
- y2 = P2[1]/P2[2];
- }
- else
- std::cout<<"Z2 = 0 !!"<<std::endl;
-
- //! compute pixel coordinates of the corresponding point in the depth image
- vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
-
- unsigned int u_ = (unsigned int)u;
- unsigned int v_ = (unsigned int)v;
- //!Fill warped image value
- if ((u_<width)&&(v_<height)){
- IrgbWarped[i][j] = Irgb[v_][u_];
- }
- else
- IrgbWarped[i][j] = 0;
- }
- }
- }
-}
-
-
-
-
-#endif // VISP_HAVE_LIBFREENECT
-
diff --git a/src/device/kinect/vpKinect.h b/src/device/kinect/vpKinect.h
deleted file mode 100644
index d4b2d99..0000000
--- a/src/device/kinect/vpKinect.h
+++ /dev/null
@@ -1,188 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKinect.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * API for using a Microsoft Kinect device
- * Requires libfreenect as a third party library
- *
- * Authors:
- * Celine Teuliere
- *
- *****************************************************************************/
-
-
-#ifndef __VP_KINECT__
-#define __VP_KINECT__
-
-#include <visp/vpConfig.h>
-// Note that libfreenect needs libusb-1.0 and libpthread
-#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
-
-#include <iostream>
-#include <libfreenect.hpp>
-
-#include <visp/vpMutex.h> // need pthread
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMeterPixelConversion.h>
-
-/*!
-
- \class vpKinect
-
- \ingroup KinectDriver
-
- \brief Driver for the Kinect device.
-
- The following example shows how to use this class to acquire data
- (depth map and color image) from a Kinect.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpKinect.h>
-
-int main() {
-#ifdef VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES
- // Init Kinect device
-#ifdef VISP_HAVE_LIBFREENECT_OLD
- // This is the way to initialize Freenect with an old version of libfreenect
- // package under ubuntu lucid 10.04
- Freenect::Freenect<vpKinect> freenect;
- vpKinect * kinect = &freenect.createDevice(0);
-#else
- Freenect::Freenect freenect;
- vpKinect * kinect = &freenect.createDevice<vpKinect>(0);
-#endif
- kinect->start(); // Start acquisition thread
-
- // Set tilt angle
- float angle = -5;
- kinect->setTiltDegrees(angle);
-
- vpImage<unsigned char> I(480,640);
- vpImage<vpRGBa> Irgb(480,640);
- vpImage<float> dmap(480,640);
-
- // Acquisition loop
- for (int i=0; i<100; i++)
- {
- kinect->getDepthMap(dmap,I);
- kinect->getRGB(Irgb);
- }
- kinect->stop(); // Stop acquisition thread
-#endif
- return 0;
-}
- \endcode
-*/
-class VISP_EXPORT vpKinect : public Freenect::FreenectDevice
-{
- public:
- /*!
- Depth map resolution.
- */
- typedef enum {
- DMAP_LOW_RES, /*!< Depth map has a resolution of 320 by 240. */
- DMAP_MEDIUM_RES /*!< Depth map has a resolution of 640 by 480. */
- } vpDMResolution;
-
- vpKinect(freenect_context *ctx, int index);
- virtual ~vpKinect();
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- vpKinect(const vpKinect &); // Not implemented!
- void operator=(const vpKinect &){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
-
- void start(vpKinect::vpDMResolution res = DMAP_LOW_RES);
- void stop();
-
- bool getDepthMap(vpImage<float>& map);
- bool getDepthMap(vpImage<float>& map, vpImage<unsigned char>& Imap);
- bool getRGB(vpImage<vpRGBa>& IRGB);
-
-
- inline void getIRCamParameters(vpCameraParameters &cam) const {
- cam = IRcam;
- }
- inline void getRGBCamParameters(vpCameraParameters &cam) const {
- cam = RGBcam;
- }
- inline void setIRCamParameters(const vpCameraParameters &cam) {
- IRcam = cam;
- }
- inline void setRGBCamParameters(const vpCameraParameters &cam) {
- RGBcam = cam;
- }
-
- void warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped);//warp the RGB image into the Depth camera frame
-
- private:
- //!Instantiation of Freenect virtual functions
- // Do not call directly even in child
- void VideoCallback(void* rgb, uint32_t timestamp);
-
- // Do not call directly even in child
- void DepthCallback(void* depth, uint32_t timestamp);
-
- private:
- vpMutex m_rgb_mutex;
- vpMutex m_depth_mutex;
-
- vpCameraParameters RGBcam, IRcam;//intrinsic parameters of the two cameras
- vpHomogeneousMatrix rgbMir;//Transformation from IRcam coordinate frame to RGBcam coordinate frame.
- vpHomogeneousMatrix irMrgb;//Transformation from RGBcam coordinate frame to IRcam coordinate frame .
- vpDMResolution DMres;
- unsigned int hd;//height of the depth map
- unsigned int wd;//width of the depth map
-
- //Access protected by a mutex:
- vpImage<float> dmap;
- vpImage<vpRGBa> IRGB;
- bool m_new_rgb_frame;
- bool m_new_depth_map;
- bool m_new_depth_image;
- unsigned int height;//height of the rgb image
- unsigned int width;//width of the rgb image
-
-};
-
-#endif
-
-#endif
diff --git a/src/device/laserscanner/sick/vpSickLDMRS.cpp b/src/device/laserscanner/sick/vpSickLDMRS.cpp
deleted file mode 100644
index 78fdf6b..0000000
--- a/src/device/laserscanner/sick/vpSickLDMRS.cpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSickLDMRS.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Sick LD-MRS laser driver on UNIX platform.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-
-#include <visp/vpSickLDMRS.h>
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-#include <visp/vpTime.h>
-#include <sys/socket.h>
-#include <sys/select.h>
-#include <netinet/in.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <netdb.h>
-#include <string.h>
-//#include <strings.h>
-#include <math.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <limits.h>
-
-
-
-/*!
-
- \file vpSickLDMRS.cpp
-
- \brief Driver for the Sick LD-MRS laser scanner.
-*/
-
-/*!
-
- Default constructor that initialize the Ethernet address to
- "131.254.12.119", set the port to 12002 and allocates memory for the
- body messages.
-*/
-vpSickLDMRS::vpSickLDMRS()
- : socket_fd(-1), body(NULL), vAngle(), time_offset(0),
- isFirstMeasure(true), maxlen_body(104000)
-{
- ip = "131.254.12.119";
- port = 12002;
- body = new unsigned char [maxlen_body];
-
- vAngle.resize(4); // Vertical angle of the 4 layers
- vAngle[0] = vpMath::rad(-1.2);
- vAngle[1] = vpMath::rad(-0.4);
- vAngle[2] = vpMath::rad( 0.4);
- vAngle[3] = vpMath::rad( 1.2);
-}
-
-/*!
- Destructor that deallocate the memory for the body messages.
-*/
-vpSickLDMRS::~vpSickLDMRS()
-{
- if (body)
- delete [] body;
-}
-
-/*!
- Initialize the connexion with the Sick LD-MRS laser scanner.
-
- \param ip_address : Ethernet address of the laser.
- \param com_port : Ethernet port of the laser.
-
- \return true if the device was initialized, false otherwise.
-
-*/
-bool vpSickLDMRS::setup(std::string ip_address, int com_port)
-{
- setIpAddress( ip_address );
- setPort( com_port );
- return ( this->setup() );
-}
-
-/*!
- Initialize the connexion with the Sick LD-MRS laser scanner.
-
- \return true if the device was initialized, false otherwise.
-*/
-bool vpSickLDMRS::setup()
-{
- struct sockaddr_in serv_addr;
- int res;
- struct timeval tv;
- fd_set myset;
-
- // Create the TCP socket
- socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
- if (socket_fd < 0) {
- fprintf(stderr, "Failed to create socket\n");
- return false;
- }
- //bzero(&serv_addr, sizeof(serv_addr));
- memset(&serv_addr, 0, sizeof(serv_addr));
- serv_addr.sin_family = AF_INET; // Internet/IP
- serv_addr.sin_addr.s_addr = inet_addr(ip.c_str()); // IP address
- serv_addr.sin_port = htons(port); // server port
-
- // Establish connection
- res = connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) ;
- if (errno == EINPROGRESS) {
- tv.tv_sec = 3;
- tv.tv_usec = 0;
- FD_ZERO(&myset);
- FD_SET(static_cast<unsigned int>(socket_fd), &myset);
- res = select(socket_fd+1, NULL, &myset, NULL, &tv);
- if (res < 0 && errno != EINTR) {
- fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno));
- return false;
- }
- else if (res > 0) {
- fprintf(stderr,"ok");
- }
- else {
- fprintf(stderr, "Timeout in select() - Cancelling!\n");
- return false;
- }
- }
-
- return true;
-}
-
-/*!
- Get the measures of the four scan layers.
-
- \return true if the measures are retrieven, false otherwise.
-
-*/
-bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
-{
- unsigned int *uintptr;
- unsigned short *ushortptr;
- static unsigned char header[24];
- ushortptr=(unsigned short *)header;
- uintptr=(unsigned int *)header;
-
- assert (sizeof(header) == 24);
- //std::cout << "size " << sizeof(header) << std::endl;
-
- double time_second = 0;
-
- if (isFirstMeasure) {
- time_second = vpTime::measureTimeSecond();
- }
-
- // read the 24 bytes header
- if (recv(socket_fd, header, sizeof(header), MSG_WAITALL) == -1) {
- printf("recv\n");
- perror("recv");
- return false;
- }
-
- if (ntohl(uintptr[0]) != vpSickLDMRS::MagicWordC2) {
- printf("Error, wrong magic number !!!\n");
- return false;
- }
-
- // get the message body
- uint16_t msgtype = ntohs(ushortptr[7]);
- uint32_t msgLength = ntohl(uintptr[2]);
-
- ssize_t len = recv(socket_fd, body, msgLength, MSG_WAITALL);
- if (len != (ssize_t)msgLength){
- printf("Error, wrong msg length: %d of %d bytes.\n", (int)len, msgLength);
- return false;
- }
-
- if (msgtype!=vpSickLDMRS::MeasuredData){
- //printf("The message in not relative to measured data !!!\n");
- return true;
- }
-
- // decode measured data
-
- // get the measurement number
- unsigned short measurementId;
- ushortptr = (unsigned short *) body;
- measurementId = ushortptr[0];
-
- // get the start timestamp
- uintptr=(unsigned int *) (body+6);
- unsigned int seconds = uintptr[1];
- unsigned int fractional=uintptr[0];
- double startTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
-
- // get the end timestamp
- uintptr=(unsigned int *) (body+14);
- seconds = uintptr[1];
- fractional=uintptr[0];
- double endTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
-
- // compute the time offset to bring the measures in the Unix time reference
- if (isFirstMeasure) {
- time_offset = time_second - startTimestamp;
- isFirstMeasure = false;
- }
-
- startTimestamp += time_offset;
- endTimestamp += time_offset;
-
- // get the number of steps per scanner rotation
- unsigned short numSteps = ushortptr[11];
-
- // 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];
-
- int nlayers = 4;
- for (int i=0; i < nlayers; i++) {
- laserscan[i].clear();
- laserscan[i].setMeasurementId(measurementId);
- laserscan[i].setStartTimestamp(startTimestamp);
- laserscan[i].setEndTimestamp(endTimestamp);
- laserscan[i].setNumSteps(numSteps);
- laserscan[i].setStartAngle(startAngle);
- laserscan[i].setStopAngle(stopAngle);
- laserscan[i].setNumPoints(numPoints);
- }
-
- // decode the measured points
- double hAngle; // horizontal angle in rad
- double rDist; // radial distance in meters
- vpScanPoint scanPoint;
-
- if (numPoints > USHRT_MAX-2)
- throw(vpException (vpException::ioError, "Out of range number of point"));
-
- for (int i=0; i < numPoints; i++) {
- ushortptr = (unsigned short *) (body+44+i*10);
- unsigned char layer = ((unsigned char) body[44+i*10])&0x0F;
- unsigned char echo = ((unsigned char) body[44+i*10])>>4;
- //unsigned char flags = (unsigned char) body[44+i*10+1];
-
- if (echo==0) {
- hAngle = (2.f * M_PI / numSteps)*(short) ushortptr[1];
- rDist = 0.01 * ushortptr[2]; // cm to meters conversion
-
- //vpTRACE("layer: %d d: %f hangle: %f", layer, rDist, hAngle);
- scanPoint.setPolar(rDist, hAngle, vAngle[layer]);
- laserscan[layer].addPoint(scanPoint);
- }
- }
- return true;
-}
-
-#endif
diff --git a/src/device/laserscanner/sick/vpSickLDMRS.h b/src/device/laserscanner/sick/vpSickLDMRS.h
deleted file mode 100644
index 062517c..0000000
--- a/src/device/laserscanner/sick/vpSickLDMRS.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSickLDMRS.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Sick LD-MRS laser driver.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef vpSickLDMRS_h
-#define vpSickLDMRS_h
-
-#include <visp/vpConfig.h>
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-
-#include <arpa/inet.h>
-#include <iostream>
-#include <vector>
-#include <string.h>
-
-#include <visp/vpScanPoint.h>
-#include <visp/vpLaserScan.h>
-#include <visp/vpLaserScanner.h>
-#include <visp/vpColVector.h>
-#include <visp/vpException.h>
-
-/*!
-
- \file vpSickLDMRS.h
-
- \brief Driver for the Sick LD-MRS laser scanner.
-*/
-
-/*!
-
- \class vpSickLDMRS
-
- \ingroup LaserDriver
-
- \brief Driver for the Sick LD-MRS laser scanner.
-
- \warning For the moment, this driver works only on UNIX platform.
-
- The code below shows how the four laser scan provided by the Sick
- LD-MRS could be acquired.
-
- \code
-#include "visp/vpSickLDMRS.h"
-
-int main()
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- std::string ip = "131.254.12.119";
-
- vpSickLDMRS laser;
- laser.setIpAddress(ip);
- laser.setup();
-
- vpLaserScan laserscan[4];
- for ( ; ; ) {
- // Get the measured points in the four layers
- laser.measure(laserscan);
-
- // Prints all the measured points
- for (int layer=0; layer<4; layer++) {
- std::vector<vpScanPoint> pointsInLayer = laserscan[layer].getScanPoints();
- vpScanPoint p;
-
- for (unsigned int i=0; i < pointsInLayer.size(); i++) {
- std::cout << pointsInLayer[i] << std::endl;
- }
- }
- }
-#endif
-}
- \endcode
-*/
-class VISP_EXPORT vpSickLDMRS : public vpLaserScanner
-{
- public:
- enum MagicWord {
- MagicWordC2 = 0xAFFEC0C2 ///< The magic word that allows to identify the messages that are sent by the Sick LD-MRS.
- };
- enum DataType {
- MeasuredData = 0x2202 ///< Flag to indicate that the body of a message contains measured data.
- };
- vpSickLDMRS();
- /*! Copy constructor. */
- vpSickLDMRS(const vpSickLDMRS &sick)
- : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), time_offset(0),
- isFirstMeasure(true), maxlen_body(104000)
- {
- *this = sick;
- };
- virtual ~vpSickLDMRS();
- /*! Copy constructor. */
- vpSickLDMRS &operator=(const vpSickLDMRS &sick)
- {
- socket_fd = sick.socket_fd;
- vAngle = sick.vAngle;
- time_offset = sick.time_offset;
- isFirstMeasure = sick.isFirstMeasure;
- maxlen_body = sick.maxlen_body;
- if (body) delete [] body;
- body = new unsigned char [104000];
- memcpy(body, sick.body, maxlen_body);
- return (*this);
- };
-
- bool setup(std::string ip, int port);
- bool setup();
- bool measure(vpLaserScan laserscan[4]);
-
- protected:
-#if defined(_WIN32)
- SOCKET socket_fd;
-#else
- int socket_fd;
-#endif
- unsigned char *body;
- vpColVector vAngle; // constant vertical angle for each layer
- double time_offset;
- bool isFirstMeasure;
- size_t maxlen_body;
- };
-
-#endif
-
-#endif
diff --git a/src/device/laserscanner/vpLaserScan.h b/src/device/laserscanner/vpLaserScan.h
deleted file mode 100644
index b2e57ae..0000000
--- a/src/device/laserscanner/vpLaserScan.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLaserScan.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Laser scan data structure.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef vpLaserScan_h
-#define vpLaserScan_h
-
-/*!
- \file vpLaserScan.h
-
- \brief Implements a laser scan data structure.
-
-*/
-
-#include "visp/vpScanPoint.h"
-
-#include <vector>
-
-/*!
-
- \class vpLaserScan
-
- \brief Implements a laser scan data structure that contains
- especially the list of scanned points that have been recorded for
- this laser scan.
-
- Other data as the start/stop angle, the start/end timestamp are
- also considered.
- */
-class VISP_EXPORT vpLaserScan
-{
- public:
- /*! Default constructor that initialize all the internal variable to zero. */
- vpLaserScan()
- : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
- numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
- {
- }
- /*! Copy constructor. */
- vpLaserScan(const vpLaserScan &scan)
- : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
- numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
- {
- startTimestamp = scan.startTimestamp;
- endTimestamp = scan.endTimestamp;
- measurementId = scan.measurementId;
- numSteps = scan.numSteps;
- startAngle = scan.startAngle;
- stopAngle = scan.stopAngle;
- numPoints = scan.numPoints;
- listScanPoints = scan.listScanPoints;
- }
- /*! Default destructor that does nothing. */
- virtual ~vpLaserScan() {};
- /*! Add the scan point at the end of the list. */
- inline void addPoint(const vpScanPoint &p) {
- listScanPoints.push_back( p );
- }
- /*! Drop the list of points. */
- inline void clear() {
- listScanPoints.clear( );
- }
- /*! Get the list of points. */
- inline std::vector<vpScanPoint> getScanPoints() {
- return listScanPoints;
- }
- /*! Specifies the id of former measurements and increases with
- every measurement. */
- inline void setMeasurementId(const unsigned short &id) {
- this->measurementId = id;
- }
- /*! Start time of measurement. */
- inline void setStartTimestamp(const double &start_timestamp) {
- this->startTimestamp = start_timestamp;
- }
- /*! End time of measurement. */
- inline void setEndTimestamp(const double &end_timestamp) {
- this->endTimestamp = end_timestamp;
- }
- /*! Angular steps per scanner rotation. */
- inline void setNumSteps(const unsigned short &num_steps) {
- this->numSteps = num_steps;
- }
- /*! Start angle of the measurement in angular steps. */
- inline void setStartAngle(const short &start_angle) {
- this->startAngle = start_angle;
- }
- /*! Stop angle of the measurement in angular steps. */
- inline void setStopAngle(const short &stop_angle) {
- this->stopAngle = stop_angle;
- }
- /*! Number of measured points of the measurement. */
- inline void setNumPoints(const unsigned short &num_points) {
- this->numPoints = num_points;
- }
- /*! Return the measurement start time. */
- inline double getStartTimestamp() {
- return startTimestamp;
- }
- /*! Return the measurement end time. */
- inline double getEndTimestamp() {
- return endTimestamp;
- }
-
- private:
- std::vector<vpScanPoint> listScanPoints;
- double startTimestamp;
- double endTimestamp;
- unsigned short measurementId;
- unsigned short numSteps;
- short startAngle;
- short stopAngle;
- unsigned short numPoints;
-
-};
-
-#endif
diff --git a/src/device/laserscanner/vpLaserScanner.h b/src/device/laserscanner/vpLaserScanner.h
deleted file mode 100644
index bc30f0d..0000000
--- a/src/device/laserscanner/vpLaserScanner.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLaserScanner.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic laser scanner.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef vpLaserScanner_h
-#define vpLaserScanner_h
-
-
-#include "visp/vpConfig.h"
-
-/*!
-
- \file vpLaserScanner.h
-
- \brief Implements a generic laser scanner.
- */
-
-/*!
-
- \class vpLaserScanner
-
- \brief Class that defines a generic laser scanner.
- */
-class VISP_EXPORT vpLaserScanner
-{
- public:
- /*! Default constructor that initialize all the internal variable to zero. */
- vpLaserScanner() : ip("null"), port(0) {};
- /*! Copy constructor. */
- vpLaserScanner(const vpLaserScanner &scanner)
- : ip("null"), port(0)
- {
- ip = scanner.ip;
- port = scanner.port;
- };
- /*! Default destructor that does nothing. */
- virtual ~vpLaserScanner() {};
-
- /*! Set the Ethernet address of the laser. */
- void setIpAddress(std::string ip_address) {
- this->ip = ip_address;
- };
-
- /*! Set the communication port. */
- void setPort(int com_port) {
- this->port = com_port;
- };
-
- protected:
- std::string ip;
- int port;
-};
-
-#endif
diff --git a/src/device/laserscanner/vpScanPoint.h b/src/device/laserscanner/vpScanPoint.h
deleted file mode 100644
index cd8533e..0000000
--- a/src/device/laserscanner/vpScanPoint.h
+++ /dev/null
@@ -1,266 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpScanPoint.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Single laser scanner point.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef vpScanPoint_h
-#define vpScanPoint_h
-
-#include <visp/vpMath.h>
-
-#include <ostream>
-#include <sstream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <math.h>
-
-/*!
- \file vpScanPoint.h
-
- \brief Implements a single laser scanner point.
-*/
-
-/*!
-
- \class vpScanPoint
-
- \brief Class that defines a single laser scanner point.
-
- This class stores data of a single scan point as:
-
- - cartesian coordinates in the 3D space that are available throw
- getX(), getY() and getZ() methods.
-
- - polar coordinates that are the native data provided by a laser
- scanner. By polar coordinates we mean here the radial distance and the
- horizontal angle of a point in the scanner layer and an additional
- vertical angle that gives the orientation of the layer.
-
-*/
-class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not be added since this class is complete inline
-{
- public:
- /*! Default constructor. */
- inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {}
- /*! Copy constructor. */
- inline vpScanPoint(const vpScanPoint &scanpoint) : rDist(0), hAngle(0), vAngle(0) {
- this->rDist = scanpoint.rDist;
- this->hAngle = scanpoint.hAngle;
- this->vAngle = scanpoint.vAngle;
- }
- /*!
- Set the polar point coordinates.
- \param r_dist : Radial distance in meter.
- \param h_angle : Horizontal angle in radian.
- \param v_angle : Vertical angle in radian.
- */
- inline vpScanPoint(double r_dist, double h_angle, double v_angle)
- : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
- {
- this->rDist = r_dist;
- this->hAngle = h_angle;
- this->vAngle = v_angle;
- }
- /*! Destructor that does nothing. */
- inline virtual ~vpScanPoint() {};
- /*!
- Set the polar point coordinates.
- \param r_dist : Radial distance in meter.
- \param h_angle : Horizontal angle in radian.
- \param v_angle : Vertical angle in radian.
- */
- inline void setPolar(double r_dist, double h_angle, double v_angle) {
- this->rDist = r_dist;
- this->hAngle = h_angle;
- this->vAngle = v_angle;
- }
- /*!
- Return the radial distance in meter.
- */
- inline double getRadialDist() const {
- return ( this->rDist );
- }
- /*!
- Returns the polar elevation (vertical) angle in radian.
- */
- inline double getVAngle() const {
- return ( this->vAngle );
- }
- /*!
- Returns the polar elevation (vertical) angle in radian.
- */
- inline double getHAngle() const {
- return ( this->hAngle );
- }
- /*!
- Returns the cartesian x coordinate.
-
- The x and y axis define an horizontal plane, where x is oriented
- positive in front of the laser while y on the left side.
-
- */
- inline double getX() const {
- return ( rDist * cos(this->hAngle) * cos(this->vAngle) );
- }
- /*!
- Returns the cartesian y coordinate.
-
- The x and y axis define an horizontal plane, where x is oriented
- positive in front of the laser while y on the left side.
-
- */
- inline double getY() const {
- return ( rDist * sin(this->hAngle) );
- }
- /*!
- Returns the cartesian z coordinate.
-
- The z axis is vertical and oriented in direction of the sky.
-
- */
- inline double getZ() const {
- return ( rDist * cos(this->hAngle) * sin(this->vAngle) );
- }
-
- friend inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p);
-
- /*!
-
- Returns true if sp1 and sp2 are equal; otherwire returns false.
-
- */
- 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();
- double rd2 = sp2.getRadialDist();
- double ha2 = sp2.getHAngle();
- double va2 = sp2.getVAngle();
-
- return ( ( std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
- }
-
- /*!
-
- Returns true if sp1 and sp2 are different; otherwire returns false.
-
- */
- 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();
- double rd2 = sp2.getRadialDist();
- double ha2 = sp2.getHAngle();
- double va2 = sp2.getVAngle();
- return ( ( std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
- }
-
- private:
- double rDist;
- double hAngle;
- double vAngle;
-};
-
-/*!
-
- Print the values of the scan point on the output stream. Data are
- separated by a white space. Data that are print are first the
- polar coordinates, than the cartesian coordinates:
- - the radial distance in meter
- - the horizontal angle in radian
- - the vertical angle in radian
- - the cartesian X coordinate
- - the cartesian Y coordinate
- - the cartesian Z coordinate
-
- The following code
-
- \code
-#include <iostream>
-#include <visp/vpScanPoint.h>
-
-int main()
-{
-vpScanPoint p;
-double radialDistance = 3; // 3 meters
-double horizontalAngle = 1.12; // 1.12 radian
-double verticalAngle = 0; // 0 radian for a horizontal layer
-
-p.setPolar(radialDistance, horizontalAngle, verticalAngle);
-
-std::cout << p << std::endl;
-}
- \endcode
- will produce the prints
- \code
-"3 1.12 0 1.307047339 2.700301327 0"
- \endcode
-
- */
-inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p) {
- std::ios_base::fmtflags original_flags = s.flags();
-
- s.precision(10);
- s << p.getRadialDist() << " "
- << p.getHAngle() << " "
- << p.getVAngle() << " "
- << p.getX() << " "
- << p.getY() << " " << p.getZ();
-
- s.setf(original_flags); // restore s to standard state
-
- return s;
-}
-
-#endif
diff --git a/src/device/light/vpRingLight.cpp b/src/device/light/vpRingLight.cpp
deleted file mode 100644
index 74a5924..0000000
--- a/src/device/light/vpRingLight.cpp
+++ /dev/null
@@ -1,229 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRingLight.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Ring light management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-# include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_PARPORT
-
-# include <sys/types.h>
-# include <sys/stat.h>
-# include <fcntl.h>
-# include <sys/ioctl.h>
-# include <sys/time.h>
-# include <unistd.h>
-
-# include <visp/vpRingLight.h>
-# include <visp/vpDebug.h>
-# include <visp/vpTime.h>
-
-/*!
- \file vpRingLight.cpp
- \brief Ring light management under unix.
-*/
-
-
-/*!
-
- Constructor to acces to the ring light device connected to the parallel port.
-
- Open and initialise the default parallel port device "/dev/parport0" to
- communicate with the ring light.
-
- \exception vpParallelPortException::opening If the device used to access to
- the parallel port can't be opened. A possible reason is that you don't have
- write access.
-
- Turn the ring light off.
-*/
-vpRingLight::vpRingLight() : parport()
-{
- off();
-}
-
-/*!
- Destructor to close the device.
-
- Turn the ring light off.
-
- \exception vpParallelPortException::closing If the device used to access to
- the parallel port can't be closed.
-*/
-vpRingLight::~vpRingLight()
-{
- off();
-}
-
-/*!
- Activates the ring light by sending a pulse throw the parallel port.
-
- The pulse width is 500 us. This pulse activates a NE555 which turns on on the
- light during 10ms.
-
-*/
-void vpRingLight::pulse()
-{
- // Data set by the parallel port:
- // - D1: need to send a 500us pulse width
- // - D2: 0 }
- // - D3: 0 } To control the light throw the NE555
- // D2 and D3 are used to select the multiplexer output.
- // Light must be connected to output 1+,1-
-
- // To activates the light we send a pulse
- int mask_mode_pulse_d2 = 0x00; // D2 is low
- int mask_pulse_d1 = 0x02; // we send a pulse on D1 : L, H, L
- unsigned char data = 0x00;
- // data = parport.getData(); // actual value of the data bus
- // vpTRACE("Actual data 0x%x = %d\n", data, data);
-
- data = data | mask_pulse_d1 | mask_mode_pulse_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data); // send a 0-1 pulse
-
- // Wait 500 micro seconds
- struct timeval ti, tc; // Initial and current time
- struct timeval tempo;
- tempo.tv_usec = 500;
- gettimeofday(&ti,0L);
- do {
- gettimeofday(&tc,0L);
- } while (tc.tv_usec < ti.tv_usec + tempo.tv_usec);
-
- data = data & (~mask_pulse_d1);
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data); // send a 1-0 pulse
-}
-
-/*!
-
- Activates the ring light by sending a pulse throw the parallel port during a
- specified duration.
-
- \param time : Duration in milli-second (ms) during while the light is turned
- on.
-
-*/
-void vpRingLight::pulse(double time)
-{
- // Data set by the parallel port:
- // - D1: a pulse with duration fixed by time
- // - D2: 0 }
- // - D3: 1 } To control the light directly throw the pulse comming from D1
- // D2 and D3 are used to select the multiplexer output.
- // Light must be connected to output 1+,1-
-
- // To activates the light we send a pulse
- int mask_mode_pulse_d3 = 0x08; // D3 is hight, D2 is low
- int mask_pulse_d1 = 0x02; // we send a pulse on D1 : L, H, L
- unsigned char data = 0x00;
- // data = parport.getData(); // actual value of the data bus
- // vpTRACE("Actual data 0x%x = %d\n", data, data);
-
- data = data | mask_pulse_d1 | mask_mode_pulse_d3;
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data); // send a 0-1 pulse
-
- // Wait 500 micro seconds
- struct timeval ti, tc; // Initial and current time
- gettimeofday(&ti,0);
- do {
- gettimeofday(&tc,0);
- } while (tc.tv_usec < ti.tv_usec + time*1000);
-
- data = data & (~mask_pulse_d1);
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data); // send a 1-0 pulse
-}
-
-/*!
- Turn the ring light on.
-
- To turn the ring light off, see off().
-
-*/
-void vpRingLight::on()
-{
- // Data set by the parallel port:
- // - D1: 0 to turn OFF, 1 to turn ON
- // - D2: 1 }
- // - D3: 0 } To control the light throw D1
- // D2 and D3 are used to select the multiplexer output.
- // Light must be connected to output 1+,1-
-
- // To activates the light we send a pulse
- int mask_mode_onoff_d2 = 0x04; // D2 is Hight
- int mask_on_d1 = 0x02; // D1 is Hight to turn the light on
- unsigned char data = 0x00;
- //data = parport.getData(); // actual value of the data bus
-
- data = data | mask_on_d1 | mask_mode_onoff_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data);
-}
-
-/*!
- Turn the ring light off.
-
- To turn the ring light on, see on().
-
-*/
-void vpRingLight::off()
-{
- // Data set by the parallel port:
- // - D1: 0 to turn OFF, 1 to turn ON
- // - D2: 1 }
- // - D3: 0 } To control the light throw D1
- // D2 and D3 are used to select the multiplexer output.
- // Light must be connected to output 1+,1-
-
- // To activates the light we send a pulse
- int mask_mode_onoff_d2 = 0x04; // D2 is Hight
- int mask_off_d1 = 0x00; // D1 is Low to turn the light off
- unsigned char data = 0x00;
- //data = parport.getData(); // actual value of the data bus
-
- data = data | mask_off_d1 | mask_mode_onoff_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
- parport.sendData(data);
-}
-
-#endif
diff --git a/src/device/light/vpRingLight.h b/src/device/light/vpRingLight.h
deleted file mode 100644
index b1784f4..0000000
--- a/src/device/light/vpRingLight.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRingLight.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Ring light management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpRingLight_h
-#define vpRingLight_h
-
-/*!
- \file vpRingLight.h
- \brief Ring light management under unix.
-*/
-
-
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_PARPORT
-
-# include <visp/vpRingLight.h>
-# include <visp/vpParallelPort.h>
-#include <iostream>
-/*!
-
- \class vpRingLight
- \ingroup Afma6
- \brief Ring light management under unix.
-
- \warning This class works only at Irisa with the Edixia's ring light system.
-
- Here is an example showing how to synchronise the framegrabbing with
- the lighting system.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpRingLight.h>
-
-int main()
-{
-#if defined(VISP_HAVE_PARPORT) && defined(VISP_HAVE_DC1394_2)
- vp1394TwoGrabber g; // Firewire framegrabber based on libdc1394-2.x third party lib
- vpImage<unsigned char> I;
-
- vpRingLight light; // Open the device to access to the ring light.
-
- for (int i=0; i < 10; i++) {
- light.pulse(); // Send a pulse to the lighting system
- g.acquire(I); // Acquire an image
- }
-#endif
-}
- \endcode
-
- Here is an example showing how to turn on the lighting during 10 seconds.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRingLight.h>
-#include <visp/vpTime.h>
-
-int main()
-{
-#ifdef VISP_HAVE_PARPORT
- vpRingLight light; // Open the device to access to the ring light.
-
- int nsec = 10; // Time to wait in seconds
- light.on(); // Turn the ring light on
- vpTime::wait(nsec * 1000); // Wait 10 s
- light.off(); // and then turn the ring light off
-#endif
-}
- \endcode
-
-
-
-*/
-class VISP_EXPORT vpRingLight
-{
-
-public:
- vpRingLight();
- ~vpRingLight();
-
- void pulse();
- void pulse(double time);
- void on();
- void off();
-
-private:
- vpParallelPort parport;
-
-} ;
-
-#endif
-
-#endif
diff --git a/src/exceptions/vpException.cpp b/src/exceptions/vpException.cpp
deleted file mode 100644
index c5eda21..0000000
--- a/src/exceptions/vpException.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpException.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception handling.
- *
- * Authors:
- * Nicolas Mansard
- *
- *****************************************************************************/
-
-
-/* \file vpException.cpp
- \brief error that can be emited by the vp class and its derivates
- */
-
-#include <stdio.h>
-#include "visp/vpException.h"
-
-
-/* ------------------------------------------------------------------------- */
-/* --- CONSTRUCTORS -------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-vpException::vpException (int id)
- : code (id), message ()
-{
-}
-
-vpException::vpException (int id, const std::string & msg)
- : code (id), message (msg)
-{
-}
-
-vpException::vpException (int id, const char* format, ...)
- : code (id), message ()
-{
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
-}
-
-vpException::vpException (const 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)
-{
- char buffer[1024];
- vsnprintf (buffer, 1024, format, args);
- std::string msg(buffer);
- message = msg;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- ACCESSORS ---------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-const char *vpException::getMessage (void)
-{
- return (this->message) .c_str();
-}
-
-const std::string &vpException::getStringMessage (void)
-{
- return this->message;
-}
-
-int
-vpException::getCode (void)
-{
- 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/src/exceptions/vpException.h b/src/exceptions/vpException.h
deleted file mode 100644
index d15124f..0000000
--- a/src/exceptions/vpException.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpException.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception handling.
- *
- * Authors:
- * Nicolas Mansard
- *
- *****************************************************************************/
-
-
-/* \file vpException.h
- \brief error that can be emited by the vp class and its derivates
- */
-
-#ifndef __vpException_H
-#define __vpException_H
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <visp/vpConfig.h>
-
-/* Classes standards. */
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-#include <stdarg.h>
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-/*!
- \class vpException
- \brief error that can be emited 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 VISP_EXPORT vpException : public std::exception
-{
- protected:
-
- //! Contains the error code, see the errorCodeEnum table for details.
- int code;
-
- //! Contains an error message (can be empty)
- std::string message;
-
- //! Set the message container
- void setMessage(const char* format, va_list args);
-
- //! forbid the empty constructor (protected)
- vpException(): code(notInitialized), message("") { };
-
- public:
-
- enum generalExceptionEnum
- {
- memoryAllocationError,
- memoryFreeError,
- functionNotImplementedError,
- ioError,
- cannotUseConstructorError,
- notImplementedError,
- divideByZeroError,
- dimensionError,
- fatalError,
- badValue, /*!< Used to indicate that a value is not in the allowed range. */
- notInitialized /*!< Used to indicate that a parameter is not initialized. */
- } ;
-
- vpException (const int code, const char* format, va_list args);
- vpException (const int code, const char* format, ...);
- vpException (const int code, const std::string & msg);
- vpException (const int code);
-
- /*!
- Basic destructor. Do nothing but implemented to fit the inheritance from
- std::exception
- */
- virtual ~vpException() throw() {}
-
- //! send the object code
- int getCode (void);
-
- //! send a reference (constant) related the error message (can be empty)
- const std::string &getStringMessage (void);
- //! send a pointer on the array of \e char related to the error string.
- //!Cannot be \e NULL.
- const char *getMessage (void);
-
- //! 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 */
diff --git a/src/image/vpColor.cpp b/src/image/vpColor.cpp
deleted file mode 100755
index 27598c6..0000000
--- a/src/image/vpColor.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpColor.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Color definition.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpColor.h>
-
-/*!< Predefined black color with R=G=B=0 and identifier vpColor::id_black. */
-vpColor const vpColor::black = vpColor(0, 0, 0, id_black);
-/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white. */
-vpColor const vpColor::white = vpColor(255, 255, 255, id_white);
-/*!< Predefined light gray color with R=G=B=64 and identifier vpColor::id_lightGray. */
-vpColor const vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
-/*!< Predefined gray color with R=G=B=128 and identifier vpColor::id_gray. */
-vpColor const vpColor::gray = vpColor(128, 128, 128, id_gray);
-/*!< Predefined dark gray color with R=G=B=192 and identifier vpColor::id_darkGray. */
-vpColor const vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
-/*!< Predefined light red color with R= 255 and G=B=140 and identifier
- vpColor::id_lightRed. */
-vpColor const vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
-/*!< Predefined red color with R=255 and G=B=0 and identifier
- vpColor::id_red. */
-vpColor const vpColor::red = vpColor(255, 0, 0, id_red);
-/*!< Predefined dark red color with R= 128 and G=B=0 and identifier
- vpColor::id_darkRed. */
-vpColor const vpColor::darkRed = vpColor(128, 0, 0, id_darkRed);
-/*!< Predefined light green color with G= 255 and R=B=140 and identifier
- vpColor::id_lightGreen. */
-vpColor const vpColor::lightGreen = vpColor(140, 255, 140, id_lightGreen);
-/*!< Predefined green color with G=255 and R=B=0 and identifier
- vpColor::id_green. */
-vpColor const vpColor::green = vpColor(0, 255, 0, id_green);
-/*!< Predefined dark green color with G= 128 and R=B=0 and identifier
- vpColor::id_darkGreen. */
-vpColor const vpColor::darkGreen = vpColor(0, 128, 0, id_darkGreen);
-/*!< Predefined light blue color with B= 255 and R=G=140 and identifier
- vpColor::id_lightBlue. */
-vpColor const vpColor::lightBlue = vpColor(140, 140, 255, id_lightBlue);
-/*!< Predefined blue color with R=G=0 and B=255 and identifier
- vpColor::id_blue. */
-vpColor const vpColor::blue = vpColor(0, 0, 255, id_blue);
-/*!< Predefined dark blue color with B= 128 and R=G=0 and identifier
- vpColor::id_darkBlue. */
-vpColor const vpColor::darkBlue = vpColor(0, 0, 128, id_darkBlue);
-/*!< Predefined yellow color with R=G=255 and B=0 and identifier
- vpColor::id_yellow. */
-vpColor const vpColor::yellow = vpColor(255, 255, 0, id_yellow);
-/*!< Predefined cyan color with R=0 and G=B=255 and identifier
- vpColor::id_cyan. */
-vpColor const vpColor::cyan = vpColor(0, 255, 255, id_cyan);
-/*!< Predefined orange color with R=255, G=165 and B=0 and identifier
- vpColor::id_orange. */
-vpColor const vpColor::orange = vpColor(255, 165, 0, id_orange);
-/*!< Predefined purple color with R=128, G=0 and B=128 and identifier
- vpColor::id_purple. */
-vpColor const vpColor::purple = vpColor(128, 0, 128, id_purple);
-
-const unsigned int vpColor::nbColors = 18;
-
-
-vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpColor::orange,vpColor::purple};
-
-/*!< Array of available colors. */
-vpColor const vpColor::allColors[vpColor::nbColors] = {
- vpColor::blue , // 12
- vpColor::green , // 9
- vpColor::red , // 6
- vpColor::cyan , // 15
- vpColor::purple , // 4
- vpColor::yellow , // 14
- vpColor::orange , // 16
- vpColor::lightBlue ,// 11
- vpColor::lightGreen,// 8
- vpColor::lightRed , // 5
- vpColor::darkBlue , // 13
- vpColor::darkGreen ,// 10
- vpColor::darkRed , // 7
- vpColor::lightGray ,// 2
- vpColor::gray , // 3
- vpColor::darkGray , // 4
- vpColor::black , // 0
- vpColor::white}; // 17
-
-/*!< Predefined none color with R=G=B=0 and identifier vpColor::id_unknown. */
-vpColor const vpColor::none = vpColor(0, 0, 0, id_unknown);
-
-/*!
- Compare two colors.
-
- Return true if the R,G,B components are the same.
-
- \param c1,c2 : Color to compare.
-*/
-VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 ) {
- return ( ( c1.R == c2.R ) && ( c1.G == c2.G ) && ( c1.B == c2.B) );
-}
-
-/*!
-
- Compare two colors.
-
- Return true if the R,G,B components are different.
-
- \param c1,c2 : Color to compare.
-*/
-VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 ) {
- return ( ( c1.R != c2.R ) || ( c1.G != c2.G ) || ( c1.B == c2.B) );
-}
diff --git a/src/image/vpColor.h b/src/image/vpColor.h
deleted file mode 100644
index adeaea2..0000000
--- a/src/image/vpColor.h
+++ /dev/null
@@ -1,244 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpColor.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Color definition.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpColor_hh
-#define vpColor_hh
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpRGBa.h>
-
-/*!
-
- \class vpColor
-
- \ingroup ImageGUI
-
- \brief Class to define colors available for display functionnalities.
-
- You may use predefined colors (black, white, red, green, blue, cyan
- and orange) or specify your own color by settings its R,G,B values.
-
- An identifier vpColorIdentifier is associated to each color. This
- identifier is useful to determine if a color is predefined or
- specified by it R,G,B values. In that last case, the identifier is
- always set to vpColor::id_unknown.
-
- The example below shows how to display geometric features in a
- display overlay using predefined colors (here the blue color to draw
- a circle) and a specific brown color (used to draw a rectangle).
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayOpenCV.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320); // Create a black grey level image
-
- vpDisplay *d;
-
- // Depending on the detected third party libraries, we instantiate here the
- // first video device which is available
-#if defined(VISP_HAVE_X11)
- d = new vpDisplayX;
-#elif defined(VISP_HAVE_GTK)
- d = new vpDisplayGTK;
-#elif defined(VISP_HAVE_GDI)
- d = new vpDisplayGDI;
-#elif defined(VISP_HAVE_D3D9)
- d = new vpDisplayD3D;
-#elif defined(VISP_HAVE_OPENCV)
- d = new vpDisplayOpenCV;
-#endif
-
- // Initialize the display with the image I. Display and image are
- // now link together.
-#ifdef VISP_HAVE_DISPLAY
- d->init(I);
-#endif
-
- // Set the display background with image I content
- vpDisplay::display(I);
-
- // Draw a filled circle with the predefined blue color
- vpDisplay::displayCircle(I, 100, 200, 30, vpColor::blue, true);
-
- // Creation of a new brown color with its RGB values
- vpColor color(128, 100, 50);
-
- // Draw a brown rectangle in the display overlay (foreground)
- vpDisplay::displayRectangle(I, 10, 10, 100, 20, color, true);
-
- // Flush the foreground and background display
- vpDisplay::flush(I);
-
- delete d;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpColor : public vpRGBa
-{
- public:
- /*! Predefined colors identifier. */
- typedef enum {
- id_black=0, /*!< Identifier associated to the predefined vpColor::black color. */
- id_white, /*!< Identifier associated to the predefined vpColor::white color. */
- id_lightGray, /*!< Identifier associated to the predefined vpColor::lightGray color. */
- id_gray, /*!< Identifier associated to the predefined vpColor::gray color. */
- id_darkGray, /*!< Identifier associated to the predefined vpColor::darkGray color. */
- id_lightRed, /*!< Identifier associated to the predefined vpColor::lightRed color. */
- id_red, /*!< Identifier associated to the predefined vpColor::red color. */
- id_darkRed, /*!< Identifier associated to the predefined vpColor::darkRed color. */
- id_lightGreen, /*!< Identifier associated to the predefined vpColor::lightGreen color. */
- id_green, /*!< Identifier associated to the predefined vpColor::green color. */
- id_darkGreen, /*!< Identifier associated to the predefined vpColor::darkGreen color. */
- id_lightBlue, /*!< Identifier associated to the predefined vpColor::lightBlue color. */
- id_blue, /*!< Identifier associated to the predefined vpColor::blue color. */
- id_darkBlue, /*!< Identifier associated to the predefined vpColor::darkBlue color. */
- id_yellow, /*!< Identifier associated to the predefined vpColor::yellow color. */
- id_cyan, /*!< Identifier associated to the predefined vpColor::cyan color. */
- id_orange, /*!< Identifier associated to the predefined vpColor::orange color. */
- id_purple, /*!< Identifier associated to the predefined vpColor::purple color. */
-
- id_unknown /*!< Identifier associated to unknowned
- colors. By unknowned, we mean not a predefined
- color. This identifier can also be used to know
- the number of predefined colors. */
-
- } vpColorIdentifier;
-
- vpColorIdentifier id; /*!< Color identifier to indicate if a color
- is predefined or set by the user using its
- RGB values. */
-
- /* Predefined colors. */
- static const vpColor black ;
- static const vpColor white ;
- static const vpColor lightGray;
- static const vpColor gray;
- static const vpColor darkGray;
- static const vpColor lightRed;
- static const vpColor red ;
- static const vpColor darkRed;
- static const vpColor lightGreen;
- static const vpColor green;
- static const vpColor darkGreen;
- static const vpColor lightBlue;
- static const vpColor blue ;
- static const vpColor darkBlue;
- static const vpColor yellow ;
- static const vpColor cyan;
- static const vpColor orange;
- static const vpColor purple;
- static const vpColor none;
-
- static const unsigned int nbColors;
- static const vpColor allColors[];
-
- /*!
- Default constructor. All the colors components are set to zero.
-
- The color identifier is set to vpColor::id_unknown to indicate
- that this color is not a predefined one.
-
- */
- inline vpColor()
- : vpRGBa(), id(id_unknown)
- {};
- /*!
- Construct a color from its RGB values.
-
- \param r : Red component.
- \param g : Green component.
- \param b : Blue component.
-
- \param cid : The color identifier to indicate if this color is or
- not a predefined one.
- */
- inline vpColor(unsigned char r, unsigned char g, unsigned char b,
- vpColor::vpColorIdentifier cid=vpColor::id_unknown)
- : vpRGBa(r, g, b), id(cid)
- {};
- /*! Default destructor. */
- inline virtual ~vpColor() {};
-
- friend VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 );
- friend VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 );
- /*!
- Set a color from its RGB values.
-
- \param r : Red component.
- \param g : Green component.
- \param b : Blue component.
-
- The color identifier is set to vpColor::id_unknown to indicate
- that this color is not a predefined one.
-
- */
- inline void setColor(unsigned char r, unsigned char g, unsigned char b) {
- this->R = r;
- this->G = g;
- this->B = b;
- this->A = 0;
- id = id_unknown;
- };
-
- /*!
-
- Get a predefined color
-
- \param i : color indice
- */
- static inline vpColor getColor(const unsigned int &i) {
- return vpColor::allColors[i % vpColor::nbColors];
- };
-
-};
-
-#endif
diff --git a/src/image/vpImage.h b/src/image/vpImage.h
deleted file mode 100644
index bc17911..0000000
--- a/src/image/vpImage.h
+++ /dev/null
@@ -1,1601 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImage.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image handling.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpImage.h
- \brief Image handling.
-*/
-
-#ifndef vpImage_H
-#define vpImage_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpImageException.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpRGBa.h>
-
-#include <fstream>
-#include <iostream>
-#include <math.h>
-#include <string.h>
-
-class vpDisplay;
-
-/*!
- \class vpImage
-
- \ingroup ImageContainer ImageDataRepresentation
-
- \brief Definition of the vpImage class member functions.
-
- This is a template class, therefore the type of each element of the
- array is not a priori defined.
-
- <h3> Data structure </h3>
-
- Each image is build using two structure (an array bitmap which size
- is [width*height]) and an array of pointer row (which size is [nrow])
- the ith element in the row array row[i] is pointer toward the ith
- "line" of the image (ie, bitmap +i*width )
-
- \image html image-data-structure.gif
- \image latex image-data-structure.ps width=10cm
-
- Such a structure allows a fast acces to each element of the image.
- if i is the ith rows and j the jth columns the value of this pixel
- is given by I[i][j] (that is equivalent to row[i][j]).
-
- <h3>Example</h3>
- The following example available in tutorial-image-manipulation.cpp shows how
- to create gray level and color images and how to access to the pixels.
- \include tutorial-image-manipulation.cpp
-
- <h3>Important remark</h3> To provide high-performance access there
- is no verification to ensure that 0 \f$\le\f$ i < height and 0
- \f$\le\f$ j < width. Since the memory allocated in the bitmap array
- is continuous, that means that if (i, j) is outside the image you
- will manipulate a pixel that is not as expected. To highlight this
- remark, we provide hereafter an example where the considered pixel
- is outside the image:
-
-\code
-unsigned int width = 320;
-unsigned int height = 240;
-vpImage<unsigned char> I(height, width); // Create an 320x240 image
-// Set pixel coordinates that is outside the image
-unsigned int i = 100;
-unsigned int j = 400;
-unsigned char value;
-value = I[i][j]; // Here we will get the pixel value at position (101, 80)
-\endcode
-
-*/
-template<class Type>
-class vpImage
-{
- friend class vpImageConvert;
-
-public:
- Type *bitmap ; //!< points toward the bitmap
- vpDisplay *display ;
-
- //! constructor
- vpImage() ;
- //! copy constructor
- vpImage(const vpImage<Type>&);
- //! constructor set the size of the image
- vpImage(unsigned int height, unsigned int width) ;
- //! constructor set the size of the image and init all the pixel
- vpImage(unsigned int height, unsigned int width, Type value) ;
- //! destructor
- virtual ~vpImage() ;
- //! set the size of the image
- void init(unsigned int height, unsigned int width) ;
- //! set the size of the image
- void init(unsigned int height, unsigned int width, Type value) ;
- //! set the size of the image without initializing it.
- void resize(const unsigned int h, const unsigned int w) ;
- //! set the size of the image and initialize it.
- void resize(const unsigned int h, const unsigned int w, const Type val) ;
- //! destructor
- void destroy() ;
-
- /*!
- Get the image height.
-
- \return The image height.
-
- \sa getWidth()
-
- */
- inline unsigned int getHeight() const { return height; }
- /*!
- Get the image width.
-
- \return The image width.
-
- \sa getHeight()
-
- */
- inline unsigned int getWidth() const { return width; }
-
- /*!
-
- Get the number of rows in the image.
-
- \return The image number of rows, or image height.
-
- \sa getHeight()
- */
- inline unsigned int getRows() const { return height ; }
-
- /*!
- Get the number of columns in the image.
-
- \return The image number of column, or image width.
-
- \sa getWidth()
- */
- inline unsigned int getCols() const { return width ; }
-
- /*!
- Get the image size.
-
- \return The image size = width * height.
-
- \sa getWidth(), getHeight()
- */
- inline unsigned int getSize() const { return width*height ; }
-
-
- // Return the maximum value within the bitmap
- Type getMaxValue() const ;
- // Return the minumum value within the bitmap
- Type getMinValue() const ;
- //Look for the minumum and the maximum value within the bitmap
- void getMinMaxValue(Type &min, Type &max) const;
-
- // Gets the value of a pixel at a location with bilinear interpolation.
- Type getValue(double i, double j) const;
- // Gets the value of a pixel at a location with bilinear interpolation.
- Type getValue(vpImagePoint &ip) const;
-
- /*!
-
- Get the image number of pixels which corresponds to the image
- width multiplied by the image height.
-
- \return The image number of pixels or image size.
-
-
- \sa getWidth(), getHeight()
- */
- inline unsigned int getNumberOfPixel() const{ return npixels; }
-
- //------------------------------------------------------------------
- // Acces to the image
-
-
- //! operator[] allows operation like I[i] = x.
- inline Type *operator[]( const unsigned int i) { return row[i];}
- inline Type *operator[]( const int i) { return row[i];}
-
- //! operator[] allows operation like x = I[i]
- inline const Type *operator[](unsigned int i) const { return row[i];}
- inline const Type *operator[](int i) const { return row[i];}
-
- /*!
- Get the value of an image point.
-
- \param i,j : Image point coordinates; i for the row position, j for the
- column position.
-
- \return Value of the image point (i, j).
-
- */
- inline Type operator()(const unsigned int i, const unsigned int j) const
- {
- return bitmap[i*width+j] ;
- }
- /*!
- Set the value of an image point.
-
- \param i, j: Image point coordinates; i for the row position, j for the
- column position.
-
- \param v : Value to set for image point (i, j).
-
- */
- inline void operator()(const unsigned int i, const unsigned int j,
- const Type &v)
- {
- bitmap[i*width+j] = v ;
- }
- /*!
- Get the value of an image point.
-
- \param ip : An image point with sub-pixel coordinates. Sub-pixel
- coordinates are roughly transformed to insigned int coordinates by cast.
-
- \return Value of the image point \e ip.
-
- \sa getValue(const vpImagePoint &)
-
- */
- inline Type operator()(const vpImagePoint &ip) const
- {
- unsigned int i = (unsigned int) ip.get_i();
- unsigned int j = (unsigned int) ip.get_j();
-
- return bitmap[i*width+j] ;
- }
- /*!
- Set the value of an image point.
-
- \param ip : An image point with sub-pixel coordinates. Sub-pixel
- coordinates are roughly transformed to insigned int coordinates by cast.
-
- \param v : Value to set for the image point.
-
- */
- inline void operator()(const vpImagePoint &ip,
- const Type &v)
- {
- unsigned int i = (unsigned int) ip.get_i();
- unsigned int j = (unsigned int) ip.get_j();
-
- bitmap[i*width+j] = v ;
- }
-
- vpImage<Type> operator-(const vpImage<Type> &B);
-
- //! Copy operator
- vpImage<Type>& operator=(const vpImage<Type> &I) ;
-
- vpImage<Type>& operator=(const Type &v);
- bool operator==(const vpImage<Type> &I);
- bool operator!=(const vpImage<Type> &I);
-
- void insert(const vpImage<Type> &src, const vpImagePoint topLeft);
-
-
- // Returns a new image that's half size of the current image
- void halfSizeImage(vpImage<Type> &res);
-
- // Returns a new image that's a quarter size of the current image
- void quarterSizeImage(vpImage<Type> &res);
-
- // Returns a new image that's double size of the current image
- void doubleSizeImage(vpImage<Type> &res);
-
- void sub(const vpImage<Type> &B, vpImage<Type> &C);
- void sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated void sub(vpImage<Type>* im2, vpImage<Type>* dst);
- // Returns a new image that's half size of the current image
- vp_deprecated void halfSizeImage(vpImage<Type>* res);
- // Returns a new image that's a quarter size of the current image
- vp_deprecated void quarterSizeImage(vpImage<Type>* res);
- // Returns a new image that's double size of the current image
- vp_deprecated void doubleSizeImage(vpImage<Type>* res);
-#endif
-private:
- unsigned int npixels ; //<! number of pixel in the image
- unsigned int width ; //<! number of columns
- unsigned int height ; //<! number of rows
- Type **row ; //!< points the row pointer array
- } ;
-
-
-/*!
- \brief Image initialisation
-
- Allocate memory for an [h x w] image.
-
- \param w : Image width.
- \param h : Image height.
- \param value : Set all the element of the bitmap to \e value.
-
- \exception vpException::memoryAllocationError
-
- \sa vpImage::init(h, w)
-*/
-template<class Type>
-void
-vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
-{
- try
- {
- init(h,w) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-
- for (unsigned int i=0 ; i < npixels ; i++)
- bitmap[i] = value ;
-}
-
-
-/*!
- \brief Image initialization
-
- Allocate memory for an [h x w] image.
-
- \param w : Image width.
- \param h : Image height.
-
- Element of the bitmap are not initialized
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \exception vpException::memoryAllocationError
-
-*/
-template<class Type>
-void
-vpImage<Type>::init(unsigned int h, unsigned int w)
-{
-
- if (h != this->height) {
- if (row != NULL) {
- vpDEBUG_TRACE(10,"Destruction row[]");
- delete [] row;
- row = NULL;
- }
- }
-
- if ((h != this->height) || (w != this->width))
- {
- if (bitmap != NULL) {
- vpDEBUG_TRACE(10,"Destruction bitmap[]") ;
- delete [] bitmap;
- bitmap = NULL;
- }
- }
-
- this->width = w ;
- this->height = h;
-
- npixels=width*height;
-
- if (bitmap == NULL) bitmap = new Type[npixels] ;
-
- // vpERROR_TRACE("Allocate bitmap %p",bitmap) ;
- if (bitmap == NULL)
- {
- vpERROR_TRACE("cannot allocate bitmap ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate bitmap ")) ;
- }
-
- if (row == NULL) row = new Type*[height] ;
-// vpERROR_TRACE("Allocate row %p",row) ;
- if (row == NULL)
- {
- vpERROR_TRACE("cannot allocate row ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate row ")) ;
- }
-
- unsigned int i ;
- for ( i =0 ; i < height ; i++)
- row[i] = bitmap + i*width ;
-}
-
-/*!
- \brief Constructor
-
- Allocate memory for an [h x w] image.
-
- \param w : Image width.
- \param h : Image height.
-
- Element of the bitmap are set to zero.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \exception vpException::memoryAllocationError
-
- \sa vpImage::init(height, width)
-*/
-template<class Type>
-vpImage<Type>::vpImage(unsigned int h, unsigned int w)
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
-{
- try
- {
- init(h,w,0) ;
- }
- catch(...)
- {
- throw ;
- }
-}
-
-/*!
- \brief Constructor
-
- Allocate memory for an [height x width] image.
-
- \param w : Image width.
- \param h : Image height.
- \param value : Set all the element of the bitmap to value.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \return MEMORY_FAULT if memory allocation is impossible, else OK
-
- \sa vpImage::init(height, width, value)
-*/
-template<class Type>
-vpImage<Type>::vpImage (unsigned int h, unsigned int w, Type value)
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
-{
- try
- {
- init(h,w,value) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-/*!
- \brief Constructor
-
- No memory allocation is done
-
- set all the element of the bitmap to value
-
- \sa vpImage::resize(height, width) for memory allocation
-*/
-template<class Type>
-vpImage<Type>::vpImage()
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
-{
-}
-
-/*!
- \brief resize the image : Image initialization
-
- Allocate memory for an [height x width] image.
-
- \warning The image is not initialized.
-
- \param w : Image width.
- \param h : Image height.
-
- Element of the bitmap are not initialized
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \exception vpException::memoryAllocationError
-
- \sa init(unsigned int, unsigned int)
-*/
-template<class Type>
-void
-vpImage<Type>::resize(unsigned int h, unsigned int w)
-{
- try
- {
- init(h, w) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-/*!
- \brief resize the image : Image initialization
-
- Allocate memory for an [height x width] image and initialize the image.
-
- \param w : Image width.
- \param h : Image height.
- \param val : Pixels value.
-
- Element of the bitmap are not initialized
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \exception vpException::memoryAllocationError
-
- \sa init(unsigned int, unsigned int)
-*/
-template<class Type>
-void
-vpImage<Type>::resize(unsigned int h, unsigned int w, const Type val)
-{
- try
- {
- init(h, w, val) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-
-/*!
- \brief Destructor : Memory de-allocation
-
- \warning does not deallocate memory for display and video
-
-*/
-template<class Type>
-void
-vpImage<Type>::destroy()
-{
- // vpERROR_TRACE("Deallocate ") ;
-
-
- if (bitmap!=NULL)
- {
- // vpERROR_TRACE("Deallocate bitmap memory %p",bitmap) ;
-// vpDEBUG_TRACE(20,"Deallocate bitmap memory %p",bitmap) ;
- delete [] bitmap ;
- bitmap = NULL;
- }
-
-
- if (row!=NULL)
- {
- // vpERROR_TRACE("Deallocate row memory %p",row) ;
-// vpDEBUG_TRACE(20,"Deallocate row memory %p",row) ;
- delete [] row ;
- row = NULL;
- }
-
-}
-
-/*!
- \brief Destructor : Memory de-allocation
-
- \warning does not deallocate memory for display and video
-
-*/
-template<class Type>
-vpImage<Type>::~vpImage()
-{
- destroy() ;
-}
-
-
-
-/*!
- Copy constructor
-*/
-template<class Type>
-vpImage<Type>::vpImage(const vpImage<Type>& I)
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
-{
- try
- {
- resize(I.getHeight(),I.getWidth());
- memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
- for (unsigned int i =0 ; i < this->height ; i++) row[i] = bitmap + i*this->width ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-/*!
- \brief Return the maximum value within the bitmap
-
- \sa getMinValue()
-*/
-template<class Type>
-Type vpImage<Type>::getMaxValue() const
-{
- Type m = bitmap[0] ;
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i]>m) m = bitmap[i] ;
- }
- return m ;
-}
-
-/*!
- \brief Return the minimum value within the bitmap
-
- \sa getMaxValue()
-*/
-template<class Type>
-Type vpImage<Type>::getMinValue() const
-{
- Type m = bitmap[0];
- for (unsigned int i=0 ; i < npixels ; i++)
- if (bitmap[i]<m) m = bitmap[i] ;
- return m ;
-}
-
-
-/*!
- \brief Look for the minimum and the maximum value within the bitmap
-
- \sa getMaxValue()
- \sa getMinValue()
-*/
-template<class Type>
-void vpImage<Type>::getMinMaxValue(Type &min, Type &max) const
-{
- min = max = bitmap[0];
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i]<min) min = bitmap[i] ;
- if (bitmap[i]>max) max = bitmap[i] ;
- }
-}
-
-/*!
- \brief Copy operator
-*/
-template<class Type>
-vpImage<Type> & vpImage<Type>::operator=(const vpImage<Type> &I)
-{
- /* we first have to set the initial values of the image because resize function calls init function that test the actual size of the image */
- if(bitmap != NULL){
- delete[] bitmap;
- bitmap = NULL ;
- }
-
- if(row != NULL){
- delete[] row;
- row = NULL ;
- }
- this->width = I.width;
- this->height = I.height;
- this->npixels = I.npixels;
- try
- {
- if(I.npixels != 0)
- {
- if (bitmap == NULL){
- bitmap = new Type[npixels] ;
- }
-
- if (bitmap == NULL){
- vpERROR_TRACE("cannot allocate bitmap ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate bitmap ")) ;
- }
-
- if (row == NULL){
- row = new Type*[height] ;
- }
- if (row == NULL){
- vpERROR_TRACE("cannot allocate row ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate row ")) ;
- }
-
- memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
-
- for (unsigned int i=0; i<this->height; i++){
- row[i] = bitmap + i*this->width;
- }
- }
- }
- catch(vpException me)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- return (* this);
-}
-
-
-/*!
- \brief = operator : Set all the element of the bitmap to a given value \e v.
- \f$ A = v <=> A[i][j] = v \f$
-
- \warning = must be defined for \f$ <\f$ Type \f$ > \f$
-*/
-template<class Type>
-vpImage<Type>& vpImage<Type>::operator=(const Type &v)
-{
- for (unsigned int i=0 ; i < npixels ; i++)
- bitmap[i] = v ;
-
- return *this;
-}
-
-/*!
- Compare two images.
-
- \return true if the images are the same, false otherwise.
-*/
-template<class Type>
-bool vpImage<Type>::operator==(const vpImage<Type> &I)
-{
- if (this->width != I.getWidth())
- return false;
- if (this->height != I.getHeight())
- return false;
-
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i] != I.bitmap[i])
- return false;
- }
- return true ;
-}
-/*!
- Compare two images.
-
- \return true if the images are different, false if they are the same.
-*/
-template<class Type>
-bool vpImage<Type>::operator!=(const vpImage<Type> &I)
-{
- if (this->width != I.getWidth())
- return true;
- if (this->height != I.getHeight())
- return true;
-
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i] == I.bitmap[i])
- return false;
- }
- return true ;
-}
-
-/*!
- Operation A - B (A is unchanged).
-
- \code
-#include <visp/vpImage.h>
-
-int main()
-{
- vpImage<unsigned char> A(288, 384);
- vpImage<unsigned char> B(288, 384);
- vpImage<unsigned char> C;
-
- A = 128;
- B = 120;
-
- // operator-() : C = A - B
- C = A - B;
-
- return 0;
-}
- \endcode
-
- \sa sub(const vpImage<Type> &, const vpImage<Type> &, vpImage<Type> &) to
- avoid matrix allocation for each use.
-*/
-template<class Type>
-vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
-{
- vpImage<Type> C;
- sub(*this,B,C);
- return C;
-}
-
-/*!
- Insert an image into another one.
-
- It is possible to insert the image \f$ src \f$ into the calling vpImage.
- You can set the point in the destination image where the top left corner of the \f$ src \f$ image will belocated.
-
- \param src : Image to insert
- \param topLeft : Coordinates of the \f$ src \f$ image's top left corner in the destination image.
-*/
-template<class Type>
-void vpImage<Type>::insert(const vpImage<Type> &src,
- const vpImagePoint topLeft)
-{
- Type* srcBitmap;
- Type* destBitmap;
-
- int itl = (int)topLeft.get_i();
- int jtl = (int)topLeft.get_j();
-
- int dest_ibegin = 0;
- int dest_jbegin = 0;
- int src_ibegin = 0;
- int src_jbegin = 0;
- int dest_w = (int)this->getWidth();
- int dest_h = (int)this->getHeight();
- int src_w = (int)src.getWidth();
- int src_h = (int)src.getHeight();
- int wsize = (int)src.getWidth();
- int hsize = (int)src.getHeight();
-
- if (itl >= dest_h || jtl >= dest_w)
- return;
-
- if (itl < 0)
- src_ibegin = -itl;
- else
- dest_ibegin = itl;
-
- if (jtl < 0)
- src_jbegin = -jtl;
- else
- dest_jbegin = jtl;
-
- if (src_w - src_jbegin > dest_w - dest_jbegin)
- wsize = dest_w - dest_jbegin;
- else
- wsize = src_w - src_jbegin;
-
- if (src_h - src_ibegin > dest_h - dest_ibegin)
- hsize = dest_h - dest_ibegin;
- else
- hsize = src_h - src_ibegin;
-
- for (int i = 0; i < hsize; i++)
- {
- srcBitmap = src.bitmap + ((src_ibegin+i)*src_w+src_jbegin);
- destBitmap = this->bitmap + ((dest_ibegin+i)*dest_w+dest_jbegin);
-
- memcpy(destBitmap,srcBitmap,wsize*sizeof(Type));
- }
-}
-
-/*!
- Returns a new image that's half size of the current image.
- No filtering is used during the sub sampling.
-
- Used for building pyramid of the image.
- \warning Operator = must be defined for Type.
-
- \param res [out] : Subsampled image that is half size of the current image.
-
- The example below shows how to use this method:
- \code
- vpImage<unsigned char> I; // original image
- vpImageIo::read(I, "myImage.pgm");
- vpImage<unsigned char> I2; // half size image
- I.halfSizeImage(I2);
- vpImageIo::write(I2, "myHalfSizeImage.pgm");
- \endcode
-
- This other example shows how to construct a pyramid of the image:
- \code
- vpImage<unsigned char> I[4]; // pyramid with 4 levels
- vpImageIo::read(I[1], "myImage.pgm"); // Original image at level 1
- // compute the other levels
- I5[1].doubleSizeImage(I5[0]); // double size image at level 0
- I5[1].halfSizeImage(I5[2]); // half size image at level 2
- I5[1].quarterSizeImage(I5[3]); // quarter size image at level 3
- \endcode
-
-*/
-template<class Type>
-void
-vpImage<Type>::halfSizeImage(vpImage<Type> &res)
-{
- unsigned int h = height/2;
- unsigned int w = width/2;
- res.resize(h, w);
- for(unsigned int i = 0; i < h; i++)
- for(unsigned int j = 0; j < w; j++)
- res[i][j] = (*this)[i<<1][j<<1];
-}
-
-/*!
- Returns a new image that's a quarter size of the current image.
- No filtering is used during the sub sampling.
- Used for building a quarter of the image.
- \warning Operator = must be defined for Type.
-
- \param res [out] : Subsampled image that is quarter size of the
- current image.
-
- The example below shows how to use this method:
- \code
- vpImage<unsigned char> I; // original image
- vpImageIo::read(I, "myImage.pgm");
- vpImage<unsigned char> I4; // quarter size image
- I.halfSizeImage(I4);
- vpImageIo::write(I4, "myQuarterSizeImage.pgm");
- \endcode
-
- See halfSizeImage(vpImage<Type> &) for an example of pyramid construction.
-
-*/
-
-template<class Type>
-void
-vpImage<Type>::quarterSizeImage(vpImage<Type> &res)
-{
- unsigned int h = height/4;
- unsigned int w = width/4;
- res.resize(h, w);
- for(unsigned int i = 0; i < h; i++)
- for(unsigned int j = 0; j < w; j++)
- res[i][j] = (*this)[i<<2][j<<2];
-}
-
-/*!
- Returns a new image that's double size of the current image.
- Used (eg. in case of keypoints extraction, we might
- double size of the image in order to have more keypoints).
- The double size image is computed by nearest-neighbour interpolation:
-
- \code
- A B C
- E F G
- H I J
-
- where
- A C H J are pixels from original image
- B E G I are interpolated pixels
- \endcode
-
-
- \warning Operator = must be defined for Type.
-
- \param res [out] : Image that is double size of the current image.
-
- The example below shows how to use this method:
- \code
- vpImage<unsigned char> I; // original image
- vpImageIo::read(I, "myImage.pgm");
- vpImage<unsigned char> I2; // double size image
- I.doubleSizeImage(I2);
- vpImageIo::write(I2, "myDoubleSizeImage.pgm");
- \endcode
-
- See halfSizeImage(vpImage<Type> &) for an example of pyramid construction.
-
-*/
-template<class Type>
-void
-vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
-{
- int h = height*2;
- int w = width*2;
-
- res.resize(h, w);
-
- for(int i = 0; i < h; i++)
- for(int j = 0; j < w; j++)
- res[i][j] = (*this)[i>>1][j>>1];
-
- /*
- A B C
- E F G
- H I J
- A C H J are pixels from original image
- B E G I are interpolated pixels
- */
-
- //interpolate pixels B and I
- for(int i = 0; i < h; i += 2)
- for(int j = 1; j < w - 1; j += 2)
- res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
- + (*this)[i>>1][(j>>1) + 1]));
-
- //interpolate pixels E and G
- for(int i = 1; i < h - 1; i += 2)
- for(int j = 0; j < w; j += 2)
- res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
- + (*this)[(i>>1)+1][j>>1]));
-
- //interpolate pixel F
- for(int i = 1; i < h - 1; i += 2)
- for(int j = 1; j < w - 1; j += 2)
- res[i][j] = (Type)(0.25 * ((*this)[i>>1][j>>1]
- + (*this)[i>>1][(j>>1)+1]
- + (*this)[(i>>1)+1][j>>1]
- + (*this)[(i>>1)+1][(j>>1)+1]));
-}
-
-/*!
-
- \warning This generic method is not implemented. You should rather use the
- instantiated methods for double, unsigned char and vpRGBa images.
-
- \sa vpImage<double>::getValue(double, double)
- \sa vpImage<unsigned char>::getValue(double, double)
- \sa vpImage<vpRGBa>::getValue(double, double)
-
-*/
-template<class Type>
-Type vpImage<Type>::getValue(double /* i */, double /* j */) const
-{
- vpTRACE("Not implemented");
-}
-
-/*!
-
- Retrieves pixel value from an image of unsigned char with sub-pixel accuracy.
-
- Gets the value of a sub-pixel with coordinates (i,j) with bilinear
- interpolation. If location is out of bounds, then return value of
- closest pixel.
-
- \param i : Sub-pixel coordinate along the rows.
- \param j : Sub-pixel coordinate along the columns.
-
- \return Interpolated sub-pixel value from the four neighbours.
-
- \exception vpImageException::notInTheImage : If (i,j) is out
- of the image.
-
-*/
-template<>
-inline unsigned char vpImage<unsigned char>::getValue(double i, double j) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(i);
- jround = (unsigned int)floor(j);
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (i > height - 1)
- i = (double)(height - 1);
-
- if (j > width - 1)
- j = (double)(width - 1);
-
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
- return (unsigned char)vpMath::round(value);
-}
-
-
-/*!
-
- Retrieves pixel value from an image of double with sub-pixel accuracy.
-
- Gets the value of a sub-pixel with coordinates (i,j) with bilinear
- interpolation. If location is out of bounds, then return value of
- closest pixel.
-
- \param i : Sub-pixel coordinate along the rows.
- \param j : Sub-pixel coordinate along the columns.
-
- \return Interpolated sub-pixel value from the four neighbours.
-
- \exception vpImageException::notInTheImage : If (i,j) is out
- of the image.
-
-*/
-template<>
-inline double vpImage<double>::getValue(double i, double j) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(i);
- jround = (unsigned int)floor(j);
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (i > height - 1)
- i = (double)(height - 1);
-
- if (j > width - 1)
- j = (double)(width - 1);
-
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
- return value;
-}
-
-template<>
-inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(i);
- jround = (unsigned int)floor(j);
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (i > height - 1)
- i = (double)(height - 1);
-
- if (j > width - 1)
- j = (double)(width - 1);
-
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
- double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
- + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
- double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
- + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
- double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
- + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
- return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
-}
-
-/*!
-
- \warning This generic method is not implemented. You should rather use the
- instantiated methods for double, unsigned char and vpRGBa images.
-
- \sa vpImage<double>::getValue(vpImagePoint &)
- \sa vpImage<unsigned char>::getValue(vpImagePoint &)
- \sa vpImage<vpRGBa>::getValue(vpImagePoint &)
-
-*/
-template<class Type>
-Type vpImage<Type>::getValue(vpImagePoint & /* ip */) const
-{
- vpTRACE("Not implemented");
-}
-
-template<>
-inline unsigned char vpImage<unsigned char>::getValue(vpImagePoint &ip) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(ip.get_i());
- jround = (unsigned int)floor(ip.get_j());
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (ip.get_i() > height - 1)
- ip.set_i((double)(height - 1));
-
- if (ip.get_j() > width - 1)
- ip.set_j((double)(width - 1));
-
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
- return (unsigned char)vpMath::round(value);
-}
-
-
-template<>
-inline double vpImage<double>::getValue(vpImagePoint &ip) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(ip.get_i());
- jround = (unsigned int)floor(ip.get_j());
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (ip.get_i() > height - 1)
- ip.set_i((double)(height - 1));
-
- if (ip.get_j() > width - 1)
- ip.set_j((double)(width - 1));
-
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
- return value;
-}
-
-template<>
-inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
-{
- unsigned int iround, jround;
- double rfrac, cfrac;
-
- iround = (unsigned int)floor(ip.get_i());
- jround = (unsigned int)floor(ip.get_j());
-
- if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
- }
-
- if (ip.get_i() > height - 1)
- ip.set_i((double)(height - 1));
-
- if (ip.get_j() > width - 1)
- ip.set_j((double)(width - 1));
-
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
-
- rfrac = 1.0f - rratio;
- cfrac = 1.0f - cratio;
-
- double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
- + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
- double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
- + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
- double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
- + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
- return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
-}
-
-/*!
- Operation C = *this - B.
-
- \code
-#include <visp/vpImage.h>
-
-int main()
-{
- vpImage<unsigned char> A(288, 384);
- vpImage<unsigned char> B(288, 384);
- vpImage<unsigned char> C;
-
- A = 128;
- B = 120;
-
- A.sub(B, C); // C = A - B
-
- return 0;
-}
- \endcode
-
- The result is placed in the third parameter C and not returned.
- A new image won't be allocated for every use of the function
- (Speed gain if used many times with the same result matrix size).
-
- \exception vpException::memoryAllocationError If the images size differ.
-
- \sa operator-()
-*/
-template<class Type>
-void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
-{
-
- try
- {
- if ((this->getHeight() != C.getHeight())
- || (this->getWidth() != C.getWidth()))
- C.resize(this->getHeight(), this->getWidth());
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if ( (this->getWidth() != B.getWidth())||(this->getHeight() != B.getHeight()))
- {
- vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
- throw(vpException(vpException::memoryAllocationError,
- "vpImage mismatch in vpImage/vpImage substraction ")) ;
- }
-
- for (unsigned int i=0;i<this->getWidth()*this->getHeight();i++)
- {
- *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i) ;
- }
-}
-
-/*!
- Operation C = A - B.
-
- The result is placed in the third parameter C and not returned.
- A new image won't be allocated for every use of the function
- (Speed gain if used many times with the same result matrix size).
-
- \exception vpException::memoryAllocationError If the images size differ.
-
- \sa operator-()
-*/
-template<class Type>
-void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B,
- vpImage<Type> &C)
-{
-
- try
- {
- if ((A.getHeight() != C.getHeight())
- || (A.getWidth() != C.getWidth()))
- C.resize(A.getHeight(), A.getWidth());
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if ( (A.getWidth() != B.getWidth())||(A.getHeight() != B.getHeight()))
- {
- vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
- throw(vpException(vpException::memoryAllocationError,
- "vpImage mismatch in vpImage/vpImage substraction ")) ;
- }
-
- for (unsigned int i=0;i<A.getWidth()*A.getHeight();i++)
- {
- *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i) ;
- }
-}
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use vpImage<Type>::sub(const vpImage<Type> &, vpImage<Type> &) instead.
-
- Operation C = *this - B.
-
- The result is placed in parameter C.
-
- \sa operator-()
-
- \exception vpException::memoryAllocationError If images pointer is NULL or if the images size differ.
-*/
-template<class Type>
-void
-vpImage<Type>::sub(vpImage<Type>* B, vpImage<Type>* C)
-{
- if (B == NULL || C == NULL) {
- throw(vpException(vpException::memoryAllocationError,
- "Images are not allocated in vpImage<>::sub()")) ;
- }
- if ( (this->getWidth() != B->getWidth())
- || (this->getHeight() != B->getHeight())
- || (this->getWidth() != C->getWidth())
- || (this->getHeight() != C->getHeight()))
- {
- vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
- throw(vpException(vpException::memoryAllocationError,
- "vpImage mismatch in vpImage/vpImage substraction ")) ;
- }
-
- for(unsigned int i = 0; i < height * width; i++)
- C->bitmap[i] = this->bitmap[i] - B->bitmap[i];
-}
-
-/*!
- \deprecated This method is deprecated. You should use halfSizeImage(vpImage<Type> &) instead.
-
- Returns a new image that's half size of the current image.
- No filtering is used during the sub sampling.
- Used for building pyramid of the image.
-
- \warning Operator = must be defined for Type.
-
- \exception vpException::memoryAllocationError If images pointer is NULL or if the images size differ.
-
- */
-
-template<class Type>
-void
-vpImage<Type>::halfSizeImage(vpImage<Type>* res)
-{
- unsigned int r = height/2;
- unsigned int c = width/2;
- if(res == NULL) {
- throw(vpException(vpException::memoryAllocationError,
- "Images are not allocated in vpImage<>::sub()")) ;
- }
- if((res->getWidth() != c) || (res->getHeight()!= r))
- res->resize(r,c);
- for(unsigned int y = 0; y < r; y++)
- for(unsigned int x = 0; x < c; x++)
- (*res)[y][x] = (*this)[y*2][x*2];
-}
-
-/*!
- \deprecated This method is deprecated. You should use quarterSizeImage(vpImage<Type> &) instead.
-
- Returns a new image that's a quarter size of the current image.
- Used for building a quarter of the image.
-
- \warning Operator = must be defined for Type.
-
- \exception vpException::memoryAllocationError If images pointer is NULL or if the images size differ.
- */
-
-template<class Type>
-void
-vpImage<Type>::quarterSizeImage(vpImage<Type>* res)
-{
- unsigned int r = height/4;
- unsigned int c = width/4;
- if(res == NULL) {
- throw(vpException(vpException::memoryAllocationError,
- "Images are not allocated in vpImage<>::sub()")) ;
- }
- if((res->getWidth() != c) || (res->getHeight()!= r))
- res->resize(r,c);
- for(unsigned int y = 0; y < r; y++)
- for(unsigned int x = 0; x < c; x++)
- (*res)[y][x] = (*this)[y*4][x*4];
-}
-
-/*!
- \deprecated This method is deprecated. You should use doubleSizeImage(vpImage<Type> &) instead.
-
- Returns a new image that's double size of the current image.
- Used (eg. in case of keypoints extraction, we might
- double size of the image in order to have more keypoints)
-
- \warning Operator = must be defined for Type.
-
- \exception vpException::memoryAllocationError If images pointer is NULL or if the images size differ.
- */
-template<class Type>
-void
-vpImage<Type>::doubleSizeImage(vpImage<Type>* res)
-{
- int h = height*2;
- int w = width*2;
-
- if(res == NULL) {
- throw(vpException(vpException::memoryAllocationError,
- "Images are not allocated in vpImage<>::sub()")) ;
- }
- if((res->getWidth() != w) || (res->getHeight()!= h))
- res->resize(h, w);
-
- for(int j = 0; j < h; j++)
- for(int i = 0; i < w; i++)
- (*res)[j][i] = (*this)[j/2][i/2];
-
- /*
- A B C
- E F G
- H I J
- A C H J are pixels from original image
- B E G I are interpolated pixels
- */
-
- //interpolate pixels B and I
- for(int j = 0; j < h; j += 2)
- for(int i = 1; i < w - 1; i += 2)
- (*res)[j][i] = (Type)(0.5 * ((*this)[j/2][i/2] + (*this)[j/2][i/2 + 1]));
-
- //interpolate pixels E and G
- for(int j = 1; j < h - 1; j += 2)
- for(int i = 0; i < w; i += 2)
- (*res)[j][i] = (Type)(0.5 * ((*this)[j/2][i/2] + (*this)[j/2+1][i/2]));
-
- //interpolate pixel F
- for(int j = 1; j < h - 1; j += 2)
- for(int i = 1; i < w - 1; i += 2)
- (*res)[j][i] = (Type)(0.25 * ((*this)[j/2][i/2] + (*this)[j/2][i/2+1] +
- (*this)[j/2+1][i/2] + (*this)[j/2+1][i/2+1]));
-}
-
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
-
-#endif
diff --git a/src/image/vpImageConvert.cpp b/src/image/vpImageConvert.cpp
deleted file mode 100644
index 2e6f309..0000000
--- a/src/image/vpImageConvert.cpp
+++ /dev/null
@@ -1,3554 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageConvert.cpp 5204 2015-01-24 13:18:18Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Convert image types.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- * Anthony Saunier
- *
- *****************************************************************************/
-
-/*!
- \file vpImageConvert.cpp
- \brief Convert image types
-*/
-
-
-#include <sstream>
-
-// image
-#include <visp/vpImageConvert.h>
-
-bool vpImageConvert::YCbCrLUTcomputed = false;
-int vpImageConvert::vpCrr[256];
-int vpImageConvert::vpCgb[256];
-int vpImageConvert::vpCgr[256];
-int vpImageConvert::vpCbb[256];
-
-
-/*!
-Convert a vpImage\<vpRGBa\> to a vpImage\<unsigned char\>
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> & dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
-
- GreyToRGBa(src.bitmap, (unsigned char *)dest.bitmap,
- src.getHeight() * src.getWidth() );
-}
-
-/*!
-Convert a vpImage\<unsigned char\> to a vpImage\<vpRGBa\>
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> & dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
-
- RGBaToGrey((unsigned char *)src.bitmap, dest.bitmap,
- src.getHeight() * src.getWidth() );
-}
-
-
-/*!
-Convert a vpImage\<float\> to a vpImage\<unsigend char\> by renormalizing between 0 and 255.
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<float> &src, vpImage<unsigned char> &dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
- unsigned int max_xy = src.getWidth()*src.getHeight();
- float min, max;
-
- src.getMinMaxValue(min,max);
-
- for (unsigned int i = 0; i < max_xy; i++) {
- float val = 255.f * (src.bitmap[i] - min) / (max - min);
- if(val < 0)
- dest.bitmap[i] = 0;
- else if(val > 255)
- dest.bitmap[i] = 255;
- else
- dest.bitmap[i] = (unsigned char)val;
- }
-}
-
-/*!
-Convert a vpImage\<unsigned char\> to a vpImage\<float\> by basic casting.
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<float> &dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
- for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
- dest.bitmap[i] = (float)src.bitmap[i];
-}
-
-/*!
-Convert a vpImage\<double\> to a vpImage\<unsigend char\> by renormalizing between 0 and 255.
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<double> &src, vpImage<unsigned char> &dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
- unsigned int max_xy = src.getWidth()*src.getHeight();
- double min, max;
-
- src.getMinMaxValue(min,max);
-
- for (unsigned int i = 0; i < max_xy; i++) {
- double val = 255. * (src.bitmap[i] - min) / (max - min);
- if(val < 0)
- dest.bitmap[i] = 0;
- else if(val > 255)
- dest.bitmap[i] = 255;
- else
- dest.bitmap[i] = (unsigned char)val;
- }
-}
-
-/*!
-Convert a vpImage\<unsigned char\> to a vpImage\<double\> by basic casting.
-\param src : source image
-\param dest : destination image
-*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<double> &dest)
-{
- dest.resize(src.getHeight(), src.getWidth()) ;
- for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
- dest.bitmap[i] = (double)src.bitmap[i];
-}
-
-#ifdef VISP_HAVE_OPENCV
-// Deprecated: will be removed with OpenCV transcient from C to C++ api
-/*!
- \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
- IplImage structure will be removed with OpenCV transcient from C to C++ api.
-
- Convert an IplImage to a vpImage\<vpRGBa\>.
-
- An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
- image structure. See http://opencvlibrary.sourceforge.net/ for general
- OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
- for the specific IplImage structure documentation.
-
- \warning This function is only available if OpenCV was detected during
- the configuration step.
-
- \param src : Source image in OpenCV format.
- \param dest : Destination image in ViSP format.
- \param flip : Set to true to vertically flip the converted image.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
- vpImage<vpRGBa> Ic; // A color image
- IplImage* Ip;
-
- // Read an image on a disk with openCV library
- Ip = cvLoadImage("image.ppm", CV_LOAD_IMAGE_COLOR);
- // Convert the grayscale IplImage into vpImage<vpRGBa>
- vpImageConvert::convert(Ip, Ic);
-
- // ...
-
- // Release Ip header and data
- cvReleaseImage(&Ip);
-#endif
-}
- \endcode
-*/
-void
-vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
-{
- int nChannel = src->nChannels;
- int depth = src->depth;
- int height = src->height;
- int width = src->width;
- int widthStep = src->widthStep;
- int lineStep = (flip) ? 1 : 0;
-
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width);
-
- //starting source address
- unsigned char* input = (unsigned char*)src->imageData;
- unsigned char* line;
- unsigned char* beginOutput = (unsigned char*)dest.bitmap;
- unsigned char* output = NULL;
-
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) + (1-lineStep) * 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line+2);
- *(output++) = *(line+1);
- *(output++) = *(line);
- *(output++) = 0;
-
- line+=3;
- }
- //go to the next line
- input+=widthStep;
- }
- }
- else if(nChannel == 1 && depth == 8 ){
- dest.resize((unsigned int)height, (unsigned int)width);
- //starting source address
- unsigned char * input = (unsigned char*)src->imageData;
- unsigned char * line;
- unsigned char* beginOutput = (unsigned char*)dest.bitmap;
- unsigned char* output = NULL;
-
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) + (1-lineStep) * 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);;
-
- line++;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-}
-
-/*!
- \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
- IplImage structure will be removed with OpenCV transcient from C to C++ api.
-
- Convert an IplImage to a vpImage\<unsigned char\>.
-
- An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
- image structure. See http://opencvlibrary.sourceforge.net/ for general
- OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
- for the specific IplImage structure documentation.
-
- \warning This function is only available if OpenCV was detected during
- the configuration step.
-
- \param src : Source image in OpenCV format.
- \param dest : Destination image in ViSP format.
- \param flip : Set to true to vertically flip the converted image.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
- vpImage<unsigned char> Ig; // A grayscale image
- IplImage* Ip;
-
- // Read an image on a disk with openCV library
- Ip = cvLoadImage("image.pgm", CV_LOAD_IMAGE_GRAYSCALE);
- // Convert the grayscale IplImage into vpImage<unsigned char>
- vpImageConvert::convert(Ip, Ig);
-
- // ...
-
- // Release Ip header and data
- cvReleaseImage(&Ip);
-#endif
-}
- \endcode
-*/
-void
-vpImageConvert::convert(const IplImage* src, vpImage<unsigned char> &dest, bool flip)
-{
- int nChannel = src->nChannels;
- int depth = src->depth;
- int height = src->height;
- int width = src->width;
- int widthStep = src->widthStep;
- int lineStep = (flip) ? 1 : 0;
-
- if (flip == false)
- {
- if(widthStep == width){
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- memcpy(dest.bitmap, src->imageData,
- (size_t)(height*width));
- }
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- BGRToGrey((unsigned char*)src->imageData,dest.bitmap, (unsigned int)width, (unsigned int)height,false);
- }
- }
- else{
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- for (int i =0 ; i < height ; i++){
- memcpy(dest.bitmap+i*width, src->imageData + i*widthStep,
- (size_t)width);
- }
- }
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- for (int i = 0 ; i < height ; i++){
- BGRToGrey((unsigned char*)src->imageData + i*widthStep,
- dest.bitmap + i*width, (unsigned int)width, 1, false);
- }
- }
- }
- }
- else
- {
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- unsigned char* beginOutput = (unsigned char*)dest.bitmap;
- for (int i =0 ; i < height ; i++){
- memcpy(beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) , src->imageData + i*widthStep,
- (size_t)width);
- }
- }
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- //for (int i = 0 ; i < height ; i++){
- BGRToGrey((unsigned char*)src->imageData /*+ i*widthStep*/,
- dest.bitmap /*+ i*width*/, (unsigned int)width, (unsigned int)height/*1*/, true);
- //}
- }
- }
-}
-
-/*!
- \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
- IplImage structure will be removed with OpenCV transcient from C to C++ api.
-
- Convert a vpImage\<vpRGBa\> to a IplImage.
-
- An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
- image structure. See http://opencvlibrary.sourceforge.net/ for general
- OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
- for the specific IplImage structure documentation.
-
- \warning This function is only available if OpenCV was detected during
- the configuration step.
-
- \param src : source image
- \param dest : destination image
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
- vpImage<vpRGBa> Ic; // A color image
- IplImage* Ip = NULL;
-
- // Read an image on a disk
- vpImageIo::read(Ic, "image.ppm");
- // Convert the vpImage<vpRGBa> in to color IplImage
- vpImageConvert::convert(Ic, Ip);
- // Treatments on IplImage
- //...
- // Save the IplImage on the disk
- cvSaveImage("Ipl.ppm", Ip);
-
- //Release Ip header and data
- cvReleaseImage(&Ip);
-#endif
-}
- \endcode
-*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> & src, IplImage *&dest)
-{
- int height = (int)src.getHeight();
- int width = (int)src.getWidth();
- CvSize size = cvSize(width, height);
- int depth = 8;
- int channels = 3;
- if (dest != NULL){
- if(dest->nChannels != channels || dest->depth != depth
- || dest->height != height || dest->width != width){
- if(dest->nChannels != 0) cvReleaseImage(&dest);
- dest = cvCreateImage( size, depth, channels );
- }
- }
- else dest = cvCreateImage( size, depth, channels );
-
-
- //starting source address
- unsigned char * input = (unsigned char*)src.bitmap;//rgba image
- unsigned char * line;
- unsigned char * output = (unsigned char*)dest->imageData;//bgr image
-
- int j=0;
- int i=0;
- int widthStep = dest->widthStep;
-
- for(i=0 ; i < height ; i++)
- {
- output = (unsigned char*)dest->imageData + i*widthStep;
- line = input;
- for( j=0 ; j < width ; j++)
- {
- *output++ = *(line+2); //B
- *output++ = *(line+1); //G
- *output++ = *(line); //R
-
- line+=4;
- }
- //go to the next line
- input+=4*width;
- }
-}
-
-/*!
- \deprecated Rather then using OpenCV IplImage you should use cv::Mat images.
- IplImage structure will be removed with OpenCV transcient from C to C++ api.
-
- Convert a vpImage\<unsigned char\> to a IplImage.
-
- An IplImage is an OpenCV (Intel's Open source Computer Vision Library)
- image structure. See http://opencvlibrary.sourceforge.net/ for general
- OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
- for the specific IplImage structure documentation.
-
- \warning This function is only available if OpenCV was detected during
- the configuration step.
-
- \param src : source image
- \param dest : destination image
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408)
- vpImage<unsigned char> Ig; // A greyscale image
- IplImage* Ip = NULL;
-
- // Read an image on a disk
- vpImageIo::read(Ig, "image.pgm");
- // Convert the vpImage<unsigned char> in to greyscale IplImage
- vpImageConvert::convert(Ig, Ip);
- // Treatments on IplImage Ip
- //...
- // Save the IplImage on the disk
- cvSaveImage("Ipl.pgm", Ip);
-
- //Release Ip header and data
- cvReleaseImage(&Ip);
-#endif
-}
- \endcode
-*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> & src, IplImage* &dest)
-{
- unsigned int height = src.getHeight();
- unsigned int width = src.getWidth();
- CvSize size = cvSize((int)width, (int)height);
- int depth = 8;
- int channels = 1;
- if (dest != NULL){
- if(dest->nChannels != channels || dest->depth != depth
- || dest->height != (int) height || dest->width != (int) width){
- if(dest->nChannels != 0) cvReleaseImage(&dest);
- dest = cvCreateImage( size, depth, channels );
- }
- }
- else dest = cvCreateImage( size, depth, channels );
-
- unsigned int widthStep = (unsigned int)dest->widthStep;
-
- if ( width == widthStep){
- memcpy(dest->imageData,src.bitmap, width*height);
- }
- else{
- //copying each line taking account of the widthStep
- for (unsigned int i =0 ; i < height ; i++){
- memcpy(dest->imageData + i*widthStep, src.bitmap + i*width,
- width);
- }
- }
-}
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020100
-/*!
- Convert a cv::Mat to a vpImage\<vpRGBa\>.
-
- A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
- the general OpenCV documentation, or
- http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
- for the specific Mat structure documentation.
-
- Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
- method, only Mat with a depth equal to 8 and a channel between 1 and 3 are
- converted.
-
- \warning This function is only available if OpenCV (version 2.1.0 or greater)
- was detected during the configuration step.
-
- \param src : Source image in OpenCV format.
- \param dest : Destination image in ViSP format.
- \param flip : Set to true to vertically flip the converted image.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- vpImage<vpRGBa> Ic; // A color image
- cv::Mat Ip;
-
- // Read an image on a disk with openCV library
- Ip = cv::imread("image.pgm", 1);// second parameter > 0 for a RGB encoding.
- // Convert the grayscale cv::Mat into vpImage<vpRGBa>
- vpImageConvert::convert(Ip, Ic);
-
- // ...
-#endif
-}
- \endcode
-*/
-void
-vpImageConvert::convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool flip)
-{
- if(src.type() == CV_8UC4){
- dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- vpRGBa rgbaVal;
- for(unsigned int i=0; i<dest.getRows(); ++i)
- for(unsigned int j=0; j<dest.getCols(); ++j){
- cv::Vec4b tmp = src.at<cv::Vec4b>((int)i, (int)j);
- rgbaVal.R = tmp[2];
- rgbaVal.G = tmp[1];
- rgbaVal.B = tmp[0];
- rgbaVal.A = tmp[3];
- if(flip)
- dest[dest.getRows()-i-1][j] = rgbaVal;
- else
- dest[i][j] = rgbaVal;
- }
- }else if(src.type() == CV_8UC3){
- dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- vpRGBa rgbaVal;
- rgbaVal.A = 0;
- for(unsigned int i=0; i<dest.getRows(); ++i){
- for(unsigned int j=0; j<dest.getCols(); ++j){
- cv::Vec3b tmp = src.at<cv::Vec3b>((int)i, (int)j);
- rgbaVal.R = tmp[2];
- rgbaVal.G = tmp[1];
- rgbaVal.B = tmp[0];
- if(flip){
- dest[dest.getRows()-i-1][j] = rgbaVal;
- }else{
- dest[i][j] = rgbaVal;
- }
- }
- }
- }else if(src.type() == CV_8UC1){
- dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- vpRGBa rgbaVal;
- for(unsigned int i=0; i<dest.getRows(); ++i){
- for(unsigned int j=0; j<dest.getCols(); ++j){
- rgbaVal = src.at<unsigned char>((int)i, (int)j);
- if(flip){
- dest[dest.getRows()-i-1][j] = rgbaVal;
- }else{
- dest[i][j] = rgbaVal;
- }
- }
- }
- }
-}
-
-/*!
- Convert a cv::Mat to a vpImage\<unsigned char\>.
-
- A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
- the general OpenCV documentation, or
- http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
- for the specific Mat structure documentation.
-
- Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
- method, only Mat with a depth equal to 8 and a channel between 1 and 3 are
- converted.
-
- \warning This function is only available if OpenCV was detected during
- the configuration step.
-
- \param src : Source image in OpenCV format.
- \param dest : Destination image in ViSP format.
- \param flip : Set to true to vertically flip the converted image.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- vpImage<unsigned char> Ig; // A grayscale image
- cv::Mat Ip;
-
- // Read an image on a disk with openCV library
- Ip = cv::imread("image.pgm", 0);// second parameter = 0 for a gray level.
- // Convert the grayscale cv::Mat into vpImage<unsigned char>
- vpImageConvert::convert(Ip, Ig);
-
- // ...
-#endif
-}
-
- \endcode
-*/
-void
-vpImageConvert::convert(const cv::Mat& src, vpImage<unsigned char>& dest, const bool flip)
-{
- if(src.type() == CV_8UC1){
- dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- if(src.isContinuous() && !flip){
- memcpy(dest.bitmap, src.data, (size_t)(src.rows*src.cols));
- }
- else{
- if(flip){
- for(unsigned int i=0; i<dest.getRows(); ++i){
- memcpy(dest.bitmap+i*dest.getCols(), src.data+(dest.getRows()-i-1)*src.step1(), (size_t)src.step);
- }
- }else{
- for(unsigned int i=0; i<dest.getRows(); ++i){
- memcpy(dest.bitmap+i*dest.getCols(), src.data+i*src.step1(), (size_t)src.step);
- }
- }
- }
- }else if(src.type() == CV_8UC3){
- dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- if(src.isContinuous() && !flip){
- BGRToGrey((unsigned char*)src.data, (unsigned char*)dest.bitmap, (unsigned int)src.cols, (unsigned int)src.rows, flip);
- }
- else{
- if(flip){
- for(unsigned int i=0; i<dest.getRows(); ++i){
- BGRToGrey((unsigned char*)src.data+i*src.step1(),
- (unsigned char*)dest.bitmap+(dest.getRows()-i-1)*dest.getCols(),
- (unsigned int)dest.getCols(), 1, false);
- }
- }else{
- for(unsigned int i=0; i<dest.getRows(); ++i){
- BGRToGrey((unsigned char*)src.data+i*src.step1(),
- (unsigned char*)dest.bitmap+i*dest.getCols(),
- (unsigned int)dest.getCols(), 1, false);
- }
- }
- }
- }
-}
-
-
-/*!
- Convert a vpImage\<unsigned char\> to a cv::Mat.
-
- A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
- the general OpenCV documentation, or
- http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
- for the specific Mat structure documentation.
-
- \warning This function is only available if OpenCV version 2.1.0 or greater
- was detected during the configuration step.
-
- \param src : source image (vpRGBa format)
- \param dest : destination image (BGR format)
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- vpImage<unsigned char> Ig; // A greyscale image
- cv::Mat Ip;
-
- // Read an image on a disk
- vpImageIo::read(Ig, "image.pgm");
- // Convert the vpImage<unsigned char> in to color cv::Mat.
- vpImageConvert::convert(Ig, Ip);
- // Treatments on cv::Mat Ip
- //...
- // Save the cv::Mat on the disk
- cv::imwrite("image.pgm", Ip);
-#endif
-}
-
- \endcode
-*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> & src, cv::Mat& dest)
-{
- cv::Mat vpToMat((int)src.getRows(), (int)src.getCols(), CV_8UC4, (void*)src.bitmap);
-
- dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC3);
- cv::Mat alpha((int)src.getRows(), (int)src.getCols(), CV_8UC1);
-
- cv::Mat out[] = {dest, alpha};
- int from_to[] = { 0,2, 1,1, 2,0, 3,3 };
- cv::mixChannels(&vpToMat, 1, out, 2, from_to, 4);
-}
-
-/*!
- Convert a vpImage\<unsigned char\> to a cv::Mat.
-
- A cv::Mat is an OpenCV image class. See http://opencv.willowgarage.com for
- the general OpenCV documentation, or
- http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
- for the specific Mat structure documentation.
-
- \warning This function is only available if OpenCV version 2.1.0 or greater
- was detected during the configuration step.
-
- \param src : source image
- \param dest : destination image
- \param copyData : if true, the image is copied and modification in one object
- will not modified the other.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- vpImage<unsigned char> Ig; // A greyscale image
- cv::Mat Ip;
-
- // Read an image on a disk
- vpImageIo::read(Ig, "image.pgm");
- // Convert the vpImage<unsigned char> in to greyscale cv::Mat
- vpImageConvert::convert(Ig, Ip);
- // Treatments on cv::Mat Ip
- //...
- // Save the cv::Mat on the disk
- cv::imwrite("image-cv.pgm", Ip);
-#endif
-}
-
- \endcode
-*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> & src, cv::Mat& dest, const bool copyData)
-{
- if(copyData){
- cv::Mat tmpMap((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
- dest = tmpMap.clone();
- }else{
- dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
- }
-}
-
-#endif
-#endif
-
-#ifdef VISP_HAVE_YARP
-/*!
- Convert a vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in ViSP format.
- \param dest : Destination image in YARP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- vpImage<unsigned char> I; // A mocochrome image
- // Read an image on a disk
- vpImageIo::read(I, "image.pgm");
-
- yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelMono >();
- // Convert the vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
- vpImageConvert::convert(I, Iyarp);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const vpImage<unsigned char> & src,
- yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData)
-{
- if(copyData)
- {
- dest->resize(src.getWidth(),src.getHeight());
- memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth());
- }
- else
- dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
-}
-
-/*!
- Convert a yarp::sig::ImageOf\<yarp::sig::PixelMono\> to a vpImage\<unsigned char\>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in YARP format.
- \param dest : Destination image in ViSP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-#if defined(VISP_HAVE_YARP)
- #include <yarp/sig/ImageFile.h>
-#endif
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelMono >();
- // Read an image on a disk
- yarp::sig::file::read(*Iyarp, "image.pgm");
-
- // Convert the yarp::sig::ImageOf<yarp::sig::PixelMono> to a vpImage<unsigned char>
- vpImage<unsigned char> I;
- vpImageConvert::convert(Iyarp, I);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
- vpImage<unsigned char> & dest,const bool copyData)
-{
- dest.resize(src->height(),src->width());
- if(copyData)
- memcpy(dest.bitmap, src->getRawImage(), src->height()*src->width()*sizeof(yarp::sig::PixelMono));
- else
- dest.bitmap = src->getRawImage();
-}
-
-/*!
- Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgba>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in ViSP format.
- \param dest : Destination image in YARP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- vpImage<vpRGBa> I; // A color image
- // Read an image on a disk
- vpImageIo::read(I,"image.jpg");
-
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgba >();
- // Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgba>
- vpImageConvert::convert(I,Iyarp);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData)
-{
- if(copyData){
- dest->resize(src.getWidth(),src.getHeight());
- memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth()*sizeof(vpRGBa));
- }
- else
- dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
-}
-
-/*!
- Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgba> to a vpImage\<vpRGBa\>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in YARP format.
- \param dest : Destination image in ViSP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-#if defined(VISP_HAVE_YARP)
- #include <yarp/sig/ImageFile.h>
-#endif
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgba >();
- // Read an image on a disk
- yarp::sig::file::read(*Iyarp,"image.pgm");
-
- // Convert the yarp::sig::ImageOf<yarp::sig::PixelRgba> to a vpImage<vpRGBa>
- vpImage<vpRGBa> I;
- vpImageConvert::convert(Iyarp,I);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
- vpImage<vpRGBa> & dest,const bool copyData)
-{
- dest.resize(src->height(),src->width());
- if(copyData)
- memcpy(dest.bitmap, src->getRawImage(),src->height()*src->width()*sizeof(yarp::sig::PixelRgba));
- else
- dest.bitmap = (vpRGBa*)src->getRawImage();
-}
-
-/*!
- Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgb>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in ViSP format.
- \param dest : Destination image in YARP format.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- vpImage<vpRGBa> I; // A color image
- // Read an image on a disk
- vpImageIo::read(I,"image.jpg");
-
- yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgb >();
- // Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgb>
- vpImageConvert::convert(I,Iyarp);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest)
-{
- dest->resize(src.getWidth(),src.getHeight());
- for(unsigned int i = 0 ; i < src.getRows() ; i++){
- for(unsigned int j = 0 ; j < src.getWidth() ; j++){
- dest->pixel(j,i).r = src[i][j].R;
- dest->pixel(j,i).g = src[i][j].G;
- dest->pixel(j,i).b = src[i][j].B;
- }
- }
-}
-
-/*!
- Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgb> to a vpImage\<vpRGBa\>
-
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
-
- \param src : Source image in YARP format.
- \param dest : Destination image in ViSP format.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-#if defined(VISP_HAVE_YARP)
- #include <yarp/sig/ImageFile.h>
-#endif
-
-int main()
-{
-#if defined(VISP_HAVE_YARP)
- yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgb >();
- // Read an image on a disk
- yarp::sig::file::read(*Iyarp,"image.pgm");
-
- // Convert the yarp::sig::ImageOf<yarp::sig::PixelRgb> to a vpImage<vpRGBa>
- vpImage<vpRGBa> I;
- vpImageConvert::convert(Iyarp,I);
-
- // ...
-#endif
-}
- \endcode
-*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src, vpImage<vpRGBa> & dest)
-{
- dest.resize(src->height(),src->width());
- for(int i = 0 ; i < src->height() ; i++){
- for(int j = 0 ; j < src->width() ; j++){
- dest[i][j].R = src->pixel(j,i).r;
- dest[i][j].G = src->pixel(j,i).g;
- dest[i][j].B = src->pixel(j,i).b;
- dest[i][j].A = 0;
- }
- }
-}
-
-#endif
-
-#if defined(VISP_HAVE_LIBJPEG)
-#if JPEG_LIB_VERSION > 70
-/*!
- Convert a vpImage\<unsigned char> to a JPEG compressed buffer
-
- \param src : Source image in ViSP format.
- \param dest : Destination buffer in JPEG format.
- \param destSize : Size of the destination buffer.
- \param quality : purcentage of the quality of the compressed image.
-*/
-void vpImageConvert::convertToJPEGBuffer(const vpImage<unsigned char> &src,
- unsigned char **dest, long unsigned int &destSize, int quality)
-{
- struct jpeg_compress_struct cinfo;
- struct jpeg_error_mgr jerr;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_compress(&cinfo);
-
- *dest = NULL;
- destSize = 0;
-
- jpeg_mem_dest(&cinfo, dest, &destSize);
-
- unsigned int width = src.getWidth();
- unsigned int height = src.getHeight();
-
- cinfo.image_width = width;
- cinfo.image_height = height;
- cinfo.input_components = 1;
- cinfo.in_color_space = JCS_GRAYSCALE;
- jpeg_set_defaults(&cinfo);
- jpeg_set_quality(&cinfo, quality, TRUE);
-
- jpeg_start_compress(&cinfo,TRUE);
-
- unsigned char *line;
- line = new unsigned char[width];
- unsigned char* input = (unsigned char*)src.bitmap;
- while (cinfo.next_scanline < cinfo.image_height)
- {
- for (unsigned int i = 0; i < width; i++)
- {
- line[i] = *(input);
- input++;
- }
- jpeg_write_scanlines(&cinfo, &line, 1);
- }
-
- jpeg_finish_compress(&cinfo);
- jpeg_destroy_compress(&cinfo);
- delete [] line;
-}
-
-/*!
- Decompress a JPEG buffer in a vpImage\<unsigned char>
-
- \param src : Source buffer in JPEG format.
- \param srcSize : Size of the source buffer.
- \param dest : Destination image in ViSP format.
-*/
-void vpImageConvert::convertToJPEGBuffer(unsigned char *src, long unsigned int srcSize,
- vpImage<unsigned char> &dest)
-{
- struct jpeg_decompress_struct cinfo;
- struct jpeg_error_mgr jerr;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_decompress(&cinfo);
-
- jpeg_mem_src(&cinfo, src, srcSize);
- jpeg_read_header(&cinfo, TRUE);
-
- unsigned int width = cinfo.image_width;
- unsigned int height = cinfo.image_height;
-
- if ( (width != dest.getWidth()) || (height != dest.getHeight()) )
- dest.resize(height,width);
-
- jpeg_start_decompress(&cinfo);
-
- unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
- JSAMPARRAY buf = (*cinfo.mem->alloc_sarray) ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
-
- if (cinfo.out_color_space == JCS_GRAYSCALE)
- {
- unsigned int row;
- while (cinfo.output_scanline<cinfo.output_height)
- {
- row = cinfo.output_scanline;
- jpeg_read_scanlines(&cinfo,buf,1);
- memcpy(dest[row], buf[0], rowbytes);
- }
- }
-
- jpeg_finish_decompress(&cinfo);
- jpeg_destroy_decompress(&cinfo);
-}
-#endif
-#endif // defined(VISP_HAVE_LIBJPEG)
-
-
-#define vpSAT(c) \
- if (c & (~255)) { if (c < 0) c = 0; else c = 255; }
-/*!
- Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32.
- Destination rgba memory area has to be allocated before.
-
- \sa YUV422ToRGBa()
-*/
-void vpImageConvert::YUYVToRGBa(unsigned char* yuyv, unsigned char* rgba,
- unsigned int width, unsigned int height)
-{
- unsigned char *s;
- unsigned char *d;
- int w, h, c;
- int r, g, b, cr, cg, cb, y1, y2;
-
- h = (int)height;
- w = (int)width;
- s = yuyv;
- d = rgba;
- while (h--) {
- c = w >> 1;
- while (c--) {
- y1 = *s++;
- cb = ((*s - 128) * 454) >> 8;
- cg = (*s++ - 128) * 88;
- y2 = *s++;
- cr = ((*s - 128) * 359) >> 8;
- cg = (cg + (*s++ - 128) * 183) >> 8;
-
- r = y1 + cr;
- b = y1 + cb;
- g = y1 - cg;
- vpSAT(r);
- vpSAT(g);
- vpSAT(b);
-
- *d++ = static_cast<unsigned char>(r);
- *d++ = static_cast<unsigned char>(g);
- *d++ = static_cast<unsigned char>(b);
- *d++ = 0;
-
- r = y2 + cr;
- b = y2 + cb;
- g = y2 - cg;
- vpSAT(r);
- vpSAT(g);
- vpSAT(b);
-
- *d++ = static_cast<unsigned char>(r);
- *d++ = static_cast<unsigned char>(g);
- *d++ = static_cast<unsigned char>(b);
- *d++ = 0;
-
- }
- }
-}
-/*!
-
- Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...)
- to RGB24. Destination rgb memory area has to be allocated before.
-
- \sa YUV422ToRGB()
-*/
-void vpImageConvert::YUYVToRGB(unsigned char* yuyv, unsigned char* rgb,
- unsigned int width, unsigned int height)
-{
- unsigned char *s;
- unsigned char *d;
- int h, w, c;
- int r, g, b, cr, cg, cb, y1, y2;
-
- h = (int)height;
- w = (int)width;
- s = yuyv;
- d = rgb;
- while (h--) {
- c = w >> 1;
- while (c--) {
- y1 = *s++;
- cb = ((*s - 128) * 454) >> 8;
- cg = (*s++ - 128) * 88;
- y2 = *s++;
- cr = ((*s - 128) * 359) >> 8;
- cg = (cg + (*s++ - 128) * 183) >> 8;
-
- r = y1 + cr;
- b = y1 + cb;
- g = y1 - cg;
- vpSAT(r);
- vpSAT(g);
- vpSAT(b);
-
- *d++ = static_cast<unsigned char>(r);
- *d++ = static_cast<unsigned char>(g);
- *d++ = static_cast<unsigned char>(b);
-
- r = y2 + cr;
- b = y2 + cb;
- g = y2 - cg;
- vpSAT(r);
- vpSAT(g);
- vpSAT(b);
-
- *d++ = static_cast<unsigned char>(r);
- *d++ = static_cast<unsigned char>(g);
- *d++ = static_cast<unsigned char>(b);
- }
- }
-}
-/*!
-
- Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...)
- to grey. Destination rgb memory area has to be allocated before.
-
- \sa YUV422ToGrey()
-*/
-void vpImageConvert::YUYVToGrey(unsigned char* yuyv, unsigned char* grey, unsigned int size)
-{
- unsigned int i=0,j=0;
-
- while( j < size*2)
- {
- grey[i++] = yuyv[j];
- grey[i++] = yuyv[j+2];
- j+=4;
- }
-}
-
-
-/*!
-
-Convert YUV411 into RGB32
-yuv411 : u y1 y2 v y3 y4
-
-*/
-void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
-{
-#if 1
- // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- for(unsigned int i = size / 4; i; i--) {
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y0 = *yuv++;
- Y1 = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- Y2 = *yuv++;
- Y3 = *yuv++;
- UV = - U - V;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
- }
-#else
- // tres tres lent ....
- unsigned int i=0,j=0;
- unsigned char r, g, b;
- while( j < numpixels*3/2)
- {
-
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
-
- YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
-
- YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
-
- YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
-
- j+=6;
- }
-#endif
-
-}
-
-/*!
- Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB32 images.
- Destination rgba memory area has to be allocated before.
-
- \sa YUYVToRGBa()
-*/
-void vpImageConvert::YUV422ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
-{
-
-#if 1
- // std::cout << "call optimized convertYUV422ToRGBa()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1;
- for( unsigned int i = size / 2; i; i-- ) {
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y0 = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- Y1 = *yuv++;
- UV = - U - V;
-
- //---
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- rgba++;
- }
-
-#else
- // tres tres lent ....
- unsigned int i=0,j=0;
- unsigned char r, g, b;
-
- while( j < size*2)
- {
-
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
-
- YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = 0;
- i+=4;
- j+=4;
-
- }
-#endif
-}
-
-/*!
-
-Convert YUV411 into Grey
-yuv411 : u y1 y2 v y3 y4
-
-*/
-void vpImageConvert::YUV411ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
-{
- unsigned int i=0,j=0;
- while( j < size*3/2)
- {
-
- grey[i ] = yuv[j+1];
- grey[i+1] = yuv[j+2];
- grey[i+2] = yuv[j+4];
- grey[i+3] = yuv[j+5];
-
- i+=4;
-
- j+=6;
- }
-}
-
-/*!
-
- Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB images.
- Destination rgb memory area has to be allocated before.
-
- \sa YUYVToRGB()
-
-*/
-void vpImageConvert::YUV422ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
-{
-#if 1
- // std::cout << "call optimized convertYUV422ToRGB()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1;
- for( unsigned int i = size / 2; i; i-- ) {
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y0 = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- Y1 = *yuv++;
- UV = - U - V;
-
- //---
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- }
-
-#else
- // tres tres lent ....
- unsigned int i=0,j=0;
- unsigned char r, g, b;
-
- while( j < size*2)
- {
-
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
- j+=4;
-
- }
-#endif
-}
-
-/*!
-
- Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into Grey.
- Destination grey memory area has to be allocated before.
-
- \sa YUYVToGrey()
-
-*/
-void vpImageConvert::YUV422ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
-{
- unsigned int i=0,j=0;
-
- while( j < size*2)
- {
- grey[i++] = yuv[j+1];
- grey[i++] = yuv[j+3];
- j+=4;
- }
-}
-
-/*!
-
-Convert YUV411 into RGB
-yuv411 : u y1 y2 v y3 y4
-
-*/
-void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
-{
-#if 1
- // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- for(unsigned int i = size / 4; i; i--) {
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y0 = *yuv++;
- Y1 = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- Y2 = *yuv++;
- Y3 = *yuv++;
- UV = - U - V;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
- }
-#else
- // tres tres lent ....
-
- unsigned int i=0,j=0;
- unsigned char r, g, b;
-
- while( j < size*3/2)
- {
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
- //TRACE("r= %d g=%d b=%d", r, g, b);
-
- j+=6;
- }
-#endif
-
-}
-
-
-
-/*!
-
- Convert YUV420 into RGBa
- yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
-
-*/
-void vpImageConvert::YUV420ToRGBa(unsigned char* yuv, unsigned char* rgba,
- unsigned int width, unsigned int height)
-{
- // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iU = yuv + size;
- unsigned char* iV = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba + 4*width-7;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba -4*width+1;
- }
- yuv+=width;
- rgba+=4*width;
- }
-}
-/*!
-
- Convert YUV420 into RGB
- yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
-
-*/
-void vpImageConvert::YUV420ToRGB(unsigned char* yuv,
- unsigned char* rgb,
- unsigned int width, unsigned int height)
-{
- // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iU = yuv + size;
- unsigned char* iV = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb + 3*width-5;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb -3*width+1;
- }
- yuv+=width;
- rgb+=3*width;
- }
-}
-
-/*!
-
- Convert YUV420 into Grey
- yuv420 : Y(NxM), U(N/2xM/2), V(N/2xM/2)
-
-*/
-void vpImageConvert::YUV420ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
-{
- for(unsigned int i=0 ; i < size ; i++)
- {
- *grey++ = *yuv++;
- }
-
-}
-/*!
-
- Convert YUV444 into RGBa
- yuv444 : u y v
-
-*/
-void vpImageConvert::YUV444ToRGBa(unsigned char* yuv, unsigned char* rgba, unsigned int size)
-{
- register int U, V, R, G, B, V2, U5, UV;
- register int Y;
- for(unsigned int i = 0; i<size; i++)
- {
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
-
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
- }
-}
-/*!
-
- Convert YUV444 into RGB
- yuv444 : u y v
-
-*/
-void vpImageConvert::YUV444ToRGB(unsigned char* yuv, unsigned char* rgb, unsigned int size)
-{
- register int U, V, R, G, B, V2, U5, UV;
- register int Y;
- for(unsigned int i = 0; i<size; i++)
- {
-
- U = (int)((*yuv++ - 128) * 0.354);
- U5 = 5*U;
- Y = *yuv++;
- V = (int)((*yuv++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
- }
-}
-
-/*!
-
- Convert YUV444 into Grey
- yuv444 : u y v
-
-*/
-void vpImageConvert::YUV444ToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
-{
- yuv++;
- for(unsigned int i=0 ; i < size ; i++)
- {
- *grey++ = *yuv;
- yuv = yuv + 3;
- }
-}
-
-/*!
-
- Convert YV12 into RGBa
- yuv420 : Y(NxM), V(N/2xM/2), U(N/2xM/2)
-
-*/
-void vpImageConvert::YV12ToRGBa(unsigned char* yuv, unsigned char* rgba,
- unsigned int width, unsigned int height)
-{
- // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba + 4*width-7;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba -4*width+1;
- }
- yuv+=width;
- rgba+=4*width;
- }
-}
-/*!
-
- Convert YV12 into RGB
- yuv420 : Y(NxM), V(N/2xM/2), U(N/2xM/2)
-
-*/
-void vpImageConvert::YV12ToRGB(unsigned char* yuv, unsigned char* rgb,
- unsigned int height, unsigned int width)
-{
- // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb + 3*width-5;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb -3*width+1;
- }
- yuv+=width;
- rgb+=3*width;
- }
-}
-
-/*!
-
- Convert YVU9 into RGBa
- yuv420 : Y(NxM), V(N/4xM/4), U(N/4xM/4)
-
-*/
-void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
- unsigned int width, unsigned int height)
-{
- // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3,Y4, Y5, Y6, Y7,Y8, Y9, Y10, Y11,Y12, Y13, Y14, Y15;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 17*size/16;
- for(unsigned int i = 0; i<height/4; i++)
- {
- for(unsigned int j = 0; j < width/4 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv++;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv+width-3;
- Y4 = *yuv++;
- Y5 = *yuv++;
- Y6 = *yuv++;
- Y7 = *yuv;
- yuv = yuv+width-3;
- Y8 = *yuv++;
- Y9 = *yuv++;
- Y10 = *yuv++;
- Y11 = *yuv;
- yuv = yuv+width-3;
- Y12 = *yuv++;
- Y13 = *yuv++;
- Y14 = *yuv++;
- Y15 = *yuv;
- yuv = yuv-3*width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba + 4*width-15;
-
- R = Y4 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y4 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y4 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y5 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y5 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y5 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y6 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y6 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y6 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y7 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y7 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y7 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba + 4*width-15;
-
- R = Y8 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y8 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y8 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y9 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y9 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y9 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y10 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y10 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y10 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y11 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y11 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y11 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba + 4*width-15;
-
- R = Y12 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y12 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y12 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y13 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y13 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y13 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y14 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y14 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y14 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba++ = 0;
-
- //---
- R = Y15 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y15 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y15 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgba++ = (unsigned char)R;
- *rgba++ = (unsigned char)G;
- *rgba++ = (unsigned char)B;
- *rgba = 0;
- rgba = rgba -12*width+1;
- }
- yuv+=3*width;
- rgba+=12*width;
- }
-}
-/*!
-
- Convert YV12 into RGB
- yuv420 : Y(NxM), V(N/4xM/4), U(N/4xM/4)
-
-*/
-void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
- unsigned int height, unsigned int width)
-{
- // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl;
- register int U, V, R, G, B, V2, U5, UV;
- register int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 17*size/16;
- for(unsigned int i = 0; i<height/4; i++)
- {
- for(unsigned int j = 0; j < width/4 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv++;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv+width-3;
- Y4 = *yuv++;
- Y5 = *yuv++;
- Y6 = *yuv++;
- Y7 = *yuv;
- yuv = yuv+width-3;
- Y8 = *yuv++;
- Y9 = *yuv++;
- Y10 = *yuv++;
- Y11 = *yuv;
- yuv = yuv+width-3;
- Y12 = *yuv++;
- Y13 = *yuv++;
- Y14 = *yuv++;
- Y15 = *yuv;
- yuv = yuv-3*width+1;
-
- // Original equations
- // R = Y + 1.402 V
- // G = Y - 0.344 U - 0.714 V
- // B = Y + 1.772 U
- R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
-
- R = Y4 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y4 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y4 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y5 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y5 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y5 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y6 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y6 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y6 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y7 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y7 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y7 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
-
- R = Y8 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y8 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y8 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y9 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y9 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y9 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y10 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y10 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y10 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y11 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y11 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y11 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
-
- R = Y12 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y12 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y12 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y13 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y13 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y13 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y14 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y14 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y14 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
-
- //---
- R = Y15 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
-
- G = Y15 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
-
- B = Y15 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
-
- *rgb++ = (unsigned char)R;
- *rgb++ = (unsigned char)G;
- *rgb++ = (unsigned char)B;
- rgb = rgb -9*width+1;
- }
- yuv+=3*width;
- rgb+=9*width;
- }
-}
-
-/*!
-
- Convert RGB into RGBa
-
-*/
-void vpImageConvert::RGBToRGBa(unsigned char* rgb, unsigned char* rgba, unsigned int size)
-{
- unsigned char *pt_input = rgb;
- unsigned char *pt_end = rgb + 3*size;
- unsigned char *pt_output = rgba;
-
- while(pt_input != pt_end) {
- *(pt_output++) = *(pt_input++) ; // R
- *(pt_output++) = *(pt_input++) ; // G
- *(pt_output++) = *(pt_input++) ; // B
- *(pt_output++) = 0 ; // A
- }
-}
-
-/*!
-
- Convert RGB into RGBa
-
-*/
-void vpImageConvert::RGBaToRGB(unsigned char* rgba, unsigned char* rgb, unsigned int size)
-{
- unsigned char *pt_input = rgba;
- unsigned char *pt_end = rgba + 4*size;
- unsigned char *pt_output = rgb;
-
- while(pt_input != pt_end) {
- *(pt_output++) = *(pt_input++) ; // R
- *(pt_output++) = *(pt_input++) ; // G
- *(pt_output++) = *(pt_input++) ; // B
- pt_input++ ;
- }
-}
-/*!
- Weights convert from linear RGB to CIE luminance assuming a
- modern monitor. See Charles Pontyon's Colour FAQ
- http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html
-
-*/
-void vpImageConvert::RGBToGrey(unsigned char* rgb, unsigned char* grey, unsigned int size)
-{
- unsigned char *pt_input = rgb;
- unsigned char* pt_end = rgb + size*3;
- unsigned char *pt_output = grey;
- while(pt_input != pt_end) {
- *pt_output = (unsigned char) (0.2126 * (*pt_input)
- + 0.7152 * (*(pt_input + 1))
- + 0.0722 * (*(pt_input + 2)) );
- pt_input += 3;
- pt_output ++;
- }
-}
-/*!
-
- Weights convert from linear RGBa to CIE luminance assuming a
- modern monitor. See Charles Pontyon's Colour FAQ
- http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html
-
-*/
-void vpImageConvert::RGBaToGrey(unsigned char* rgba, unsigned char* grey, unsigned int size)
-{
- unsigned char *pt_input = rgba;
- unsigned char* pt_end = rgba + size*4;
- unsigned char *pt_output = grey;
-
- while(pt_input != pt_end) {
- *pt_output = (unsigned char) (0.2126 * (*pt_input)
- + 0.7152 * (*(pt_input + 1))
- + 0.0722 * (*(pt_input + 2)) );
- pt_input += 4;
- pt_output ++;
- }
-}
-
-/*!
- Convert from grey to linear RGBa.
-
-*/
-void
-vpImageConvert::GreyToRGBa(unsigned char* grey, unsigned char* rgba, unsigned int size)
-{
- unsigned char *pt_input = grey;
- unsigned char *pt_end = grey + size;
- unsigned char *pt_output = rgba;
-
- while(pt_input != pt_end) {
- unsigned char p = *pt_input ;
- *(pt_output ) = p ; // R
- *(pt_output + 1) = p ; // G
- *(pt_output + 2) = p ; // B
- *(pt_output + 3) = p ; // A
-
- pt_input ++;
- pt_output += 4;
- }
-}
-
-/*!
- Convert from grey to linear RGBa.
-
-*/
-void
-vpImageConvert::GreyToRGB(unsigned char* grey, unsigned char* rgb, unsigned int size)
-{
- unsigned char *pt_input = grey;
- unsigned char* pt_end = grey + size;
- unsigned char *pt_output = rgb;
-
- while(pt_input != pt_end) {
- unsigned char p = *pt_input ;
- *(pt_output ) = p ; // R
- *(pt_output + 1) = p ; // G
- *(pt_output + 2) = p ; // B
-
- pt_input ++;
- pt_output += 3;
- }
-}
-
-
-/*!
- Converts a BGR image to RGBa
- Flips the image verticaly if needed
- assumes that rgba is already resized
-*/
-void
-vpImageConvert::BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip)
-{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? (bgr+(width*height*3)+lineStep) : bgr;
- unsigned char * line;
-
- unsigned int j=0;
- unsigned int i=0;
-
- for(i=0 ; i < height ; i++)
- {
- line = src;
- for( j=0 ; j < width ; j++)
- {
- *rgba++ = *(line+2);
- *rgba++ = *(line+1);
- *rgba++ = *(line+0);
- *rgba++ = 0;
-
- line+=3;
- }
- //go to the next line
- src+=lineStep;
- }
-
-}
-
-/*!
- Converts a BGR image to greyscale
- Flips the image verticaly if needed
- assumes that grey is already resized
-*/
-void
-vpImageConvert::BGRToGrey(unsigned char * bgr, unsigned char * grey,
- unsigned int width, unsigned int height, bool flip)
-{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? bgr+(width*height*3)+lineStep : bgr;
- unsigned char * line;
-
- unsigned int j=0;
- unsigned int i=0;
-
- for(i=0 ; i < height ; i++)
- {
- line = src;
- for( j=0 ; j < width ; j++)
- {
- *grey++ = (unsigned char)( 0.2126 * *(line+2)
- + 0.7152 * *(line+1)
- + 0.0722 * *(line+0)) ;
- line+=3;
- }
-
- //go to the next line
- src+=lineStep;
- }
-}
-/*!
- Converts a RGB image to RGBa
- Flips the image verticaly if needed
- assumes that rgba is already resized
-*/
-void
-vpImageConvert::RGBToRGBa(unsigned char * rgb, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip)
-{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? (rgb+(width*height*3)+lineStep) : rgb;
- unsigned char * line;
-
- unsigned int j=0;
- unsigned int i=0;
-
- for(i=0 ; i < height ; i++)
- {
- line = src;
- for( j=0 ; j < width ; j++)
- {
- *rgba++ = *(line++);
- *rgba++ = *(line++);
- *rgba++ = *(line++);
- *rgba++ = 0;
- }
- //go to the next line
- src+=lineStep;
- }
-}
-
-/*!
- Converts a RGB image to greyscale
- Flips the image verticaly if needed
- assumes that grey is already resized
-*/
-void
-vpImageConvert::RGBToGrey(unsigned char * rgb, unsigned char * grey,
- unsigned int width, unsigned int height, bool flip)
-{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? rgb+(width*height*3)+lineStep : rgb;
- unsigned char * line;
-
- unsigned int j=0;
- unsigned int i=0;
-
- unsigned r,g,b;
-
- for(i=0 ; i < height ; i++)
- {
- line = src;
- for( j=0 ; j < width ; j++)
- {
- r = *(line++);
- g = *(line++);
- b = *(line++);
- *grey++ = (unsigned char)( 0.2126 * r + 0.7152 * g + 0.0722 * b) ;
- }
-
- //go to the next line
- src+=lineStep;
- }
-}
-
-/*!
-
- Compute the look up table useful for YCbCr conversions.
-
-*/
-void vpImageConvert::computeYCbCrLUT()
-{
- if (YCbCrLUTcomputed == false) {
- int index = 256, aux;
-
- while (index-- ) {
-
- aux = index - 128;
- vpImageConvert::vpCrr[index] = (int)( 364.6610 * aux) >> 8;
- vpImageConvert::vpCgb[index] = (int)( -89.8779 * aux) >> 8;
- vpImageConvert::vpCgr[index] = (int)(-185.8154 * aux) >> 8;
- vpImageConvert::vpCbb[index] = (int)( 460.5724 * aux) >> 8;
- }
-
- YCbCrLUTcomputed = true;
- }
-}
-
-
-/*!
-
- Convert an image from YCbCr 4:2:2 (Y0 Cb01 Y1 Cr01 Y2 Cb23 Y3 ...) to RGB
- format. Destination rgb memory area has to be allocated before.
-
- - In YCbCr (4:2:2) format each pixel is coded using 16 bytes.
- Byte 0: YO (Luma for Pixel 0)
- Byte 1: Chroma Blue Cb (Blue Chroma for Pixel 0 and 1)
- Byte 2: Y1 (Luma for Pixel 1)
- Byte 3: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
- Byte 4: Y2 (Luma for Pixel 2)
-
- - In RGB format, each pixel is coded using 24 bytes.
- Byte 0: Red
- Byte 1: Green
- Byte 2: Blue
-
-
-*/
-void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
-{
- unsigned char *cbv;
- unsigned char *crv;
- unsigned char *pt_ycbcr = ycbcr;
- unsigned char *pt_rgb = rgb;
- cbv = pt_ycbcr + 1;
- crv = pt_ycbcr + 3;
-
- vpImageConvert::computeYCbCrLUT();
-
- int col = 0;
-
- while (size--) {
- register int val_r, val_g, val_b;
- if (!(col++ % 2)) {
- cbv = pt_ycbcr + 1;
- crv = pt_ycbcr + 3;
- }
-
- val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
- val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
- val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
-
- vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
-
- *pt_rgb++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgb++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgb++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
-
- pt_ycbcr += 2;
- }
-}
-
-/*!
-
- Convert an image from YCbCr 4:2:2 (Y0 Cb01 Y1 Cr01 Y2 Cb23 Y3...) to
- RGBa format. Destination rgba memory area has to be allocated
- before.
-
- - In YCbCr (4:2:2) format each pixel is coded using 16 bytes.
- Byte 0: YO (Luma for Pixel 0)
- Byte 1: Chroma Blue Cb (Blue Chroma for Pixel 0 and 1)
- Byte 2: Y1 (Luma for Pixel 1)
- Byte 3: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
- Byte 4: Y2 (Luma for Pixel 2)
-
- - In RGBa format, each pixel is coded using 24 bytes.
- Byte 0: Red
- Byte 1: Green
- Byte 2: Blue
- Byte 3: -
-
-
-*/
-void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsigned int size)
-{
- unsigned char *cbv;
- unsigned char *crv;
- unsigned char *pt_ycbcr = ycbcr;
- unsigned char *pt_rgba = rgba;
- cbv = pt_ycbcr + 1;
- crv = pt_ycbcr + 3;
-
- vpImageConvert::computeYCbCrLUT();
-
- int col = 0;
-
- while (size--) {
- register int val_r, val_g, val_b;
- if (!(col++ % 2)) {
- cbv = pt_ycbcr + 1;
- crv = pt_ycbcr + 3;
- }
-
- val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
- val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
- val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
-
- vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
-
- *pt_rgba++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgba++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgba++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
- *pt_rgba++ = 0;
-
- pt_ycbcr += 2;
- }
-}
-
-
-/*!
-
- Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to grey
- format. Destination grey image memory area has to be allocated
- before.
-
- - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
- Byte 0: YO (Luma for Pixel 0)
- Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
- Byte 2: Y1 (Luma for Pixel 1)
- Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
- Byte 4: Y2 (Luma for Pixel 2)
-
- - In grey format, each pixel is coded using 8 bytes.
-
-*/
-void vpImageConvert::YCbCrToGrey(unsigned char* yuv, unsigned char* grey, unsigned int size)
-{
- unsigned int i=0,j=0;
-
- while( j < size*2)
- {
- grey[i++] = yuv[j];
- grey[i++] = yuv[j+2];
- j+=4;
- }
-}
-
-/*!
-
- Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to RGB
- format. Destination rgb memory area has to be allocated before.
-
- - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
- Byte 0: YO (Luma for Pixel 0)
- Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
- Byte 2: Y1 (Luma for Pixel 1)
- Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
- Byte 4: Y2 (Luma for Pixel 2)
-
- - In RGB format, each pixel is coded using 24 bytes.
- Byte 0: Red
- Byte 1: Green
- Byte 2: Blue
-
-*/
-void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsigned int size)
-{
- unsigned char *cbv;
- unsigned char *crv;
- unsigned char *pt_ycbcr = ycrcb;
- unsigned char *pt_rgb = rgb;
- crv = pt_ycbcr + 1;
- cbv = pt_ycbcr + 3;
-
- vpImageConvert::computeYCbCrLUT();
-
- int col = 0;
-
- while (size--) {
- register int val_r, val_g, val_b;
- if (!(col++ % 2)) {
- crv = pt_ycbcr + 1;
- cbv = pt_ycbcr + 3;
- }
-
- val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
- val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
- val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
-
- vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
-
- *pt_rgb++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgb++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgb++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
-
- pt_ycbcr += 2;
- }
-}
-/*!
-
- Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to RGBa
- format. Destination rgba memory area has to be allocated before.
-
- - In YCrCb (4:2:2) format each pixel is coded using 16 bytes.
- Byte 0: YO (Luma for Pixel 0)
- Byte 1: Chroma Red Cr (Red Chroma for Pixel 0 and 1)
- Byte 2: Y1 (Luma for Pixel 1)
- Byte 3: Chroma blue Cb (Blue Chroma for Pixel 0 and 1)
- Byte 4: Y2 (Luma for Pixel 2)
-
- - In RGBa format, each pixel is coded using 24 bytes.
- Byte 0: Red
- Byte 1: Green
- Byte 2: Blue
- Byte 3: -
-
-
-*/
-void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsigned int size)
-{
- unsigned char *cbv;
- unsigned char *crv;
- unsigned char *pt_ycbcr = ycrcb;
- unsigned char *pt_rgba = rgba;
- crv = pt_ycbcr + 1;
- cbv = pt_ycbcr + 3;
-
- vpImageConvert::computeYCbCrLUT();
-
- int col = 0;
-
- while (size--) {
- register int val_r, val_g, val_b;
- if (!(col++ % 2)) {
- crv = pt_ycbcr + 1;
- cbv = pt_ycbcr + 3;
- }
-
- val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv];
- val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv];
- val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv];
-
- vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
-
- *pt_rgba++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgba++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgba++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
- *pt_rgba++ = 0;
-
- pt_ycbcr += 2;
- }
-}
-
-/*!
-
- Split an image from vpRGBa format to monochrome channels.
- \param src : source image.
- \param pR : red channel. Set as NULL if not needed.
- \param pG : green channel. Set as NULL if not needed.
- \param pB : blue channel. Set as NULL if not needed.
- \param pa : alpha channel. Set as NULL if not needed.
-
- Example code using split :
-
- \code
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-
-int main()
-{
- vpImage<vpRGBa> Ic; // A color image
-
- // Load a color image from the disk
- vpImageIo::read(Ic,"image.ppm");
-
- // Only R and B Channels are desired.
- vpImage<unsigned char> R, B;
-
- // Split Ic color image
- // R and B will be resized in split function if needed
- vpImageConvert::split(Ic, &R, NULL, &B, NULL);
-
- // Save the the R Channel.
- vpImageIo::write(R, "RChannel.pgm");
-}
- \endcode
-*/
-void vpImageConvert::split(const vpImage<vpRGBa> &src,
- vpImage<unsigned char>* pR,
- vpImage<unsigned char>* pG,
- vpImage<unsigned char>* pB,
- vpImage<unsigned char>* pa)
-{
- register size_t n = src.getNumberOfPixel();
- unsigned int height = src.getHeight();
- unsigned int width = src.getWidth();
- unsigned char* input;
- unsigned char* dst ;
-
- vpImage<unsigned char>* tabChannel[4];
-
-/* incrsrc[0] = 0; //init
- incrsrc[1] = 0; //step after the first used channel
- incrsrc[2] = 0; //step after the second used channel
- incrsrc[3] = 0;
- incrsrc[4] = 0;
- */
- tabChannel[0] = pR;
- tabChannel[1] = pG;
- tabChannel[2] = pB;
- tabChannel[3] = pa;
-
- register size_t i; /* ordre */
- for(unsigned int j = 0;j < 4;j++){
- if(tabChannel[j]!=NULL){
- if(tabChannel[j]->getHeight() != height ||
- tabChannel[j]->getWidth() != width){
- tabChannel[j]->resize(height,width);
- }
- dst = (unsigned char*)tabChannel[j]->bitmap;
-
- input = (unsigned char*)src.bitmap+j;
- i = 0;
-#if 1 //optimization
- if (n >= 4) { /* boucle deroulee lsize fois */
- n -= 3;
- for (; i < n; i += 4) {
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
- }
- n += 3;
- }
-#endif
- for (; i < n; i++) {
- *dst = *input; input += 4; dst ++;
- }
- }
- }
-}
-
-/*!
-
- Converts a MONO16 grey scale image (each pixel is coded by two bytes) into a
- grey image where each pixels are coded on one byte.
-
- \param grey16 : Input image to convert (two bytes per pixel).
- \param grey : Output image (one byte per pixel)
- \param size : The image size or the number of pixels.
-
-*/
-void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size)
-{
- register int i = (((int)size)<<1)-1;
- register int j = (int)size-1;
- register int y;
-
- while (i >= 0) {
- y = grey16[i--];
- grey[j--] = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
- }
-}
-
-/*!
-
- Converts a MONO16 grey scale image (each pixel is coded by two bytes) into a
- grey image where each pixels are coded on one byte.
-
- \param grey16 : Input image to convert (two bytes per pixel).
- \param rgba : Output image.
- \param size : The image size or the number of pixels.
-
-*/
-void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size)
-{
- register int i = (((int)size)<<1)-1;
- register int j = (int)(size*4-1);
- register int y;
- register unsigned char v;
-
- while (i >= 0) {
- y = grey16[i--];
- v = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
- rgba[j--] = 0;
- rgba[j--] = v;
- rgba[j--] = v;
- rgba[j--] = v;
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/image/vpImageConvert.h b/src/image/vpImageConvert.h
deleted file mode 100644
index a9f548e..0000000
--- a/src/image/vpImageConvert.h
+++ /dev/null
@@ -1,322 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpImageConvert.h 5204 2015-01-24 13:18:18Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Convert image types.
-*
-* Authors:
-* Eric Marchand
-* Fabien Spindler
-* Anthony Saunier
-*
-*****************************************************************************/
-
-/*!
- \file vpImageConvert.h
- \brief Convert image types
-*/
-
-#ifndef vpIMAGECONVERT_H
-#define vpIMAGECONVERT_H
-
-// image
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDebug.h>
-// color
-#include <visp/vpRGBa.h>
-
-#ifdef VISP_HAVE_OPENCV
-# if (VISP_HAVE_OPENCV_VERSION >= 0x030000) // Require opencv >= 3.0.0
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# include <opencv2/imgproc/imgproc_c.h>
-# elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) // Require opencv >= 2.4.8
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/core/core.hpp>
-# include <opencv2/legacy/legacy.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# include <opencv2/highgui/highgui_c.h>
-# else
-# include <highgui.h>
-# endif
-#endif
-
-#ifdef VISP_HAVE_YARP
-# include <yarp/sig/Image.h>
-#endif
-
-#if defined(_WIN32)
-# include <windows.h>
-#endif
-
-#if defined(VISP_HAVE_LIBJPEG)
-# include <jpeglib.h>
-# include <jerror.h>
-#endif
-
-#if defined(VISP_HAVE_LIBPNG)
-# include <png.h>
-#endif
-
-/*!
- \class vpImageConvert
-
- \ingroup ImageConversion
-
- Convert image types.
-
- The following example available in tutorial-image-converter.cpp shows how to
- convert an OpenCV cv::Mat image into a vpImage:
-
- \include tutorial-image-converter.cpp
-
-*/
-class VISP_EXPORT vpImageConvert
-{
-
-public:
- static void convert(const vpImage<unsigned char> &src,
- vpImage<vpRGBa> & dest) ;
- static void convert(const vpImage<vpRGBa> &src,
- vpImage<unsigned char> & dest) ;
-
- static void convert(const vpImage<float> &src,
- vpImage<unsigned char> &dest);
- static void convert(const vpImage<unsigned char> &src,
- vpImage<float> &dest);
-
- static void convert(const vpImage<double> &src,
- vpImage<unsigned char> &dest);
- static void convert(const vpImage<unsigned char> &src,
- vpImage<double> &dest);
-
-#ifdef VISP_HAVE_OPENCV
- // Deprecated: will be removed with OpenCV transcient from C to C++ api
- static void convert(const IplImage* src,
- vpImage<vpRGBa> & dest, bool flip = false) ;
- static void convert(const IplImage* src,
- vpImage<unsigned char> & dest, bool flip = false) ;
- static void convert(const vpImage<vpRGBa> & src,
- IplImage *&dest) ;
- static void convert(const vpImage<unsigned char> & src,
- IplImage* &dest) ;
-# if VISP_HAVE_OPENCV_VERSION >= 0x020100
- static void convert(const cv::Mat& src,
- vpImage<vpRGBa>& dest, const bool flip = false);
- static void convert(const cv::Mat& src,
- vpImage<unsigned char>& dest, const bool flip = false);
- static void convert(const vpImage<vpRGBa> & src,
- cv::Mat& dest) ;
- static void convert(const vpImage<unsigned char> & src,
- cv::Mat& dest, const bool copyData = true) ;
-# endif
-#endif
-
-#ifdef VISP_HAVE_YARP
- static void convert(const vpImage<unsigned char> & src,
- yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData = true) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
- vpImage<unsigned char> & dest,const bool copyData = true ) ;
-
-
- static void convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData = true) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
- vpImage<vpRGBa> & dest,const bool copyData = true) ;
-
- static void convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src,
- vpImage<vpRGBa> & dest) ;
-#endif
-
-#ifdef VISP_HAVE_LIBJPEG
-#if JPEG_LIB_VERSION > 70
- static void convertToJPEGBuffer(const vpImage<unsigned char> &src,
- unsigned char **dest, long unsigned int &destSize, int quality = 100);
-
- static void convertToJPEGBuffer(unsigned char *src, long unsigned int srcSize,
- vpImage<unsigned char> &dest);
-#endif
-#endif
-
- static void split(const vpImage<vpRGBa> &src,
- vpImage<unsigned char>* pR,
- vpImage<unsigned char>* pG,
- vpImage<unsigned char>* pB,
- vpImage<unsigned char>* pa = NULL) ;
-
- /*!
- Converts a yuv pixel value in rgb format.
-
- \param y Y component of a pixel.
- \param u U component of a pixel.
- \param v V component of a pixel.
- \param r Red component from the YUV coding format. This value is computed
- using:
- \f[ r = 0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128) \f]
- \param g Green component from the YUV coding format. This value is computed
- using:
- \f[g = 0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128) \f]
- \param b Blue component from the YUV coding format. This value is computed
- using:
- \f[b = 0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128) \f]
-
- */
- static inline void YUVToRGB(unsigned char y,
- unsigned char u,
- unsigned char v,
- unsigned char &r,
- unsigned char &g,
- unsigned char &b)
- {
- double dr, dg, db;
- dr = floor(0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128));
- dg = floor(0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128));
- db = floor(0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128));
-
- dr = dr < 0. ? 0. : dr;
- dg = dg < 0. ? 0. : dg;
- db = db < 0. ? 0. : db;
- dr = dr > 255. ? 255. : dr;
- dg = dg > 255. ? 255. : dg;
- db = db > 255. ? 255. : db;
-
- r = (unsigned char) dr;
- g = (unsigned char) dg;
- b = (unsigned char) db;
- };
- static void YUYVToRGBa(unsigned char* yuyv, unsigned char* rgba,
- unsigned int width, unsigned int height);
- static void YUYVToRGB(unsigned char* yuyv, unsigned char* rgb,
- unsigned int width, unsigned int height);
- static void YUYVToGrey(unsigned char* yuyv, unsigned char* grey,
- unsigned int size);
- static void YUV411ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV411ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV411ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
- static void YUV422ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV422ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV422ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
- static void YUV420ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YUV420ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void YUV420ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
-
- static void YUV444ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV444ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV444ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
-
- static void YV12ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YV12ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void YVU9ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YVU9ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void RGBToRGBa(unsigned char* rgb,
- unsigned char* rgba, unsigned int size);
- static void RGBaToRGB(unsigned char* rgba,
- unsigned char* rgb, unsigned int size);
-
- static void RGBToGrey(unsigned char* rgb,
- unsigned char* grey, unsigned int size);
- static void RGBaToGrey(unsigned char* rgba,
- unsigned char* grey, unsigned int size);
-
- static void RGBToRGBa(unsigned char * bgr, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip = false);
- static void RGBToGrey(unsigned char * bgr, unsigned char * grey,
- unsigned int width, unsigned int height, bool flip = false);
-
- static void GreyToRGBa(unsigned char* grey,
- unsigned char* rgba, unsigned int size);
- static void GreyToRGB(unsigned char* grey,
- unsigned char* rgb, unsigned int size);
-
- static void BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip=false);
-
- static void BGRToGrey(unsigned char * bgr, unsigned char * grey,
- unsigned int width, unsigned int height, bool flip=false);
-
- static void YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCbCrToRGBa (unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCrCbToRGB(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCrCbToRGBa(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCbCrToGrey(unsigned char *ycbcr, unsigned char *grey,
- unsigned int size);
- static void MONO16ToGrey(unsigned char *grey16, unsigned char *grey,
- unsigned int size);
- static void MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba,
- unsigned int size);
-
-private:
- static void computeYCbCrLUT();
-
-private:
- static bool YCbCrLUTcomputed;
- static int vpCrr[256];
- static int vpCgb[256];
- static int vpCgr[256];
- static int vpCbb[256];
-
-} ;
-
-#endif
-
-
-/*
-* Local variables:
-* c-basic-offset: 2
-* End:
-*/
diff --git a/src/image/vpImageException.h b/src/image/vpImageException.h
deleted file mode 100644
index 79c7fe9..0000000
--- a/src/image/vpImageException.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpImage class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef __vpImageException_H
-#define __vpImageException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpImageException.h
- \brief error that can be emited by the vpImage class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- \class vpImageException
-
- \ingroup Exception
-
- \brief Error that can be emited by the vpImage class and its derivates.
- */
-class VISP_EXPORT vpImageException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpImage member
- */
- enum errorImageCodeEnum
- {
- ioError,
- noFileNameError,
- notInitializedError,
- incorrectInitializationError,
- notInTheImage
- } ;
-
- public:
- vpImageException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpImageException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpImageException (const int id)
- : vpException(id){ ; }
-};
-
-#endif
diff --git a/src/image/vpImageFilter.cpp b/src/image/vpImageFilter.cpp
deleted file mode 100644
index 1739fe8..0000000
--- a/src/image/vpImageFilter.cpp
+++ /dev/null
@@ -1,691 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageFilter.cpp 5200 2015-01-24 08:34:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Various image tools, convolution, ...
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpImageFilter.h>
-#include <visp/vpImageConvert.h>
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-# include <opencv2/imgproc/imgproc.hpp>
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-# include <opencv2/imgproc/imgproc_c.h>
-#elif defined(VISP_HAVE_OPENCV)
-# include <cv.h>
-#endif
-
-/*!
- Apply a filter to an image.
-
- \param I : Image to filter
- \param If : Filtered image.
- \param M : Filter coefficients.
-
-*/
-void
-vpImageFilter::filter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpMatrix& M)
-{
-
- unsigned int size = M.getRows() ;
- unsigned int half_size = size/2 ;
-
- If.resize(I.getHeight(),I.getWidth()) ;
-
- If = 0 ;
-
- for (unsigned int i=half_size ; i < I.getHeight()-half_size ; i++)
- {
- for (unsigned int j=half_size ; j < I.getWidth()-half_size ; j++)
- {
- double conv_x = 0 ;
-
- for(unsigned int a = 0 ; a < size ; a++ )
- for(unsigned int b = 0 ; b < size ; b++ )
- {
- double val = I[i-half_size+a][j-half_size+b] ;
- conv_x += M[a][b] * val ;
- }
- If[i][j] = conv_x ;
- }
- }
-
-}
-
-/*!
- Apply a filter to an image.
-
- \param I : Image to filter
- \param Iu : Filtered image along the horizontal axis (u = columns).
- \param Iv : Filtered image along the vertical axis (v = rows).
- \param M : Separate filter coefficients
-
-*/
-void
-vpImageFilter::filter(const vpImage<double> &I,
- vpImage<double>& Iu,
- vpImage<double>& Iv,
- const vpMatrix& M)
-{
-
- unsigned int size = M.getRows() ;
- unsigned int half_size = size/2 ;
-
- Iu.resize(I.getHeight(),I.getWidth()) ;
- Iv.resize(I.getHeight(),I.getWidth()) ;
-
- Iu = 0 ;
- Iv = 0 ;
- for (unsigned int v=half_size ; v < I.getHeight()-half_size ; v++)
- {
- for (unsigned int u=half_size ; u < I.getWidth()-half_size ; u++)
- {
- double conv_u = 0 ;
- double conv_v = 0 ;
-
- for(unsigned int a = 0 ; a < size ; a++ )
- for(unsigned int b = 0 ; b < size ; b++ )
- {
- double val = I[v-half_size+a][u-half_size+b] ;
- conv_u += M[a][b] * val ;
- conv_v += M[b][a] * val ;
- }
- Iu[v][u] = conv_u ;
- Iv[v][u] = conv_v ;
- }
- }
-
-}
-
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
-/*!
- Apply the Canny edge operator on the image \e Isrc and return the resulting
- image \e Ires.
-
- The following example shows how to use the method:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageFilter.h>
-
-int main()
-{
-#if VISP_HAVE_OPENCV_VERSION >= 0x020100 // Canny uses OpenCV >=2.1.0
- // Constants for the Canny operator.
- const unsigned int gaussianFilterSize = 5;
- const double thresholdCanny = 15;
- const unsigned int apertureSobel = 3;
-
- // Image for the Canny edge operator
- vpImage<unsigned char> Isrc;
- vpImage<unsigned char> Icanny;
-
- //First grab the source image Isrc.
-
- //Apply the Canny edge operator and set the Icanny image.
- vpImageFilter::canny(Isrc, Icanny, gaussianFilterSize, thresholdCanny, apertureSobel);
-#endif
- return (0);
-}
- \endcode
-
- \param Isrc : Image to apply the Canny edge detector to.
- \param Ires : Filtered image (255 means an edge, 0 otherwise).
- \param gaussianFilterSize : The size of the mask of the Gaussian filter to
- apply (an odd number).
- \param thresholdCanny : The threshold for the Canny operator. Only value
- greater than this value are marked as an edge).
- \param apertureSobel : Size of the mask for the Sobel operator (odd number).
-*/
-void
-vpImageFilter:: canny(const vpImage<unsigned char>& Isrc,
- vpImage<unsigned char>& Ires,
- const unsigned int gaussianFilterSize,
- const double thresholdCanny,
- const unsigned int apertureSobel)
-{
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage* img_ipl = NULL;
- vpImageConvert::convert(Isrc, img_ipl);
- IplImage* edges_ipl;
- edges_ipl = cvCreateImage(cvSize(img_ipl->width, img_ipl->height), img_ipl->depth, img_ipl->nChannels);
-
- cvSmooth(img_ipl, img_ipl, CV_GAUSSIAN, (int)gaussianFilterSize, (int)gaussianFilterSize, 0, 0);
- cvCanny(img_ipl, edges_ipl, thresholdCanny, thresholdCanny, (int)apertureSobel);
-
- vpImageConvert::convert(edges_ipl, Ires);
- cvReleaseImage(&img_ipl);
- cvReleaseImage(&edges_ipl);
-#else
- cv::Mat img_cvmat, edges_cvmat;
- vpImageConvert::convert(Isrc, img_cvmat);
- cv::GaussianBlur(img_cvmat, img_cvmat, cv::Size((int)gaussianFilterSize, (int)gaussianFilterSize), 0, 0);
- cv::Canny(img_cvmat, edges_cvmat, thresholdCanny, thresholdCanny, (int)apertureSobel);
- vpImageConvert::convert(edges_cvmat, Ires);
-#endif
-}
-#endif
-
-/*!
- Apply a separable filter.
- */
-void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size)
-{
- vpImage<double> GIx ;
- filterX(I, GIx,filter,size);
- filterY(GIx, GI,filter,size);
- GIx.destroy();
-}
-
-/*!
- Apply a separable filter.
- */
-void vpImageFilter::filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size)
-{
- vpImage<double> GIx ;
- filterX(I, GIx,filter,size);
- filterY(GIx, GI,filter,size);
- GIx.destroy();
-}
-
-void vpImageFilter::filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
-{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
- }
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
- }
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
- }
- }
-}
-void vpImageFilter::filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
-{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
- }
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
- }
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
- }
- }
-}
-void vpImageFilter::filterY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
-{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
- }
- }
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
- }
- }
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
- }
- }
-}
-void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
-{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
- }
- }
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
- }
- }
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
- }
- }
-}
-
-/*!
- Apply a Gaussian blur to an image.
- \param I : Input image.
- \param GI : Filtered image.
- \param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
-
- */
-void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size, double sigma, bool normalize)
-{
- double *fg=new double[(size+1)/2] ;
- vpImageFilter::getGaussianKernel(fg, size, sigma, normalize) ;
- vpImage<double> GIx ;
- vpImageFilter::filterX(I, GIx,fg,size);
- vpImageFilter::filterY(GIx, GI,fg,size);
- GIx.destroy();
- delete[] fg;
-}
-
-/*!
- Return the coefficients of a Gaussian filter.
-
- \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
- The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
- \param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
-*/
-void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, double sigma, bool normalize)
-{
- if (size%2 != 1)
- throw (vpImageException(vpImageException::incorrectInitializationError,
- "Bad Gaussian filter size"));
-
- if (sigma<= 0)
- sigma = (size-1)/6.0;
-
- int middle = (int)(size-1)/2;
- double sigma2 = vpMath::sqr(sigma);
- for( int i=0; i<= middle; i++)
- {
- filter[i] = (1./(sigma*sqrt(2.*M_PI)))*exp(-(i*i)/(2.*sigma2));
- }
- if (normalize) {
- //renormalization
- double sum=0;
- for(int i=1; i<=middle; i++)
- {
- sum += 2*filter[i] ;
- }
- sum += filter[0];
-
- for(int i=0; i<=middle; i++)
- {
- filter[i] = filter[i]/sum;
- }
- }
-}
-
-/*!
- Return the coefficients of a Gaussian derivative filter that may be used to compute spatial image derivatives after applying a Gaussian blur.
-
- \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
- The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
- \param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
-*/
-void vpImageFilter::getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma, bool normalize)
-{
- if (size%2 != 1)
- throw (vpImageException(vpImageException::incorrectInitializationError,
- "Bad Gaussian filter size"));
-
- if (sigma<= 0)
- sigma = (size-1)/6.0;
-
- int middle = (int)(size-1)/2;
- double sigma2 = vpMath::sqr(sigma);
- filter[0] = 0.;
- for(int i=1; i<= middle; i++)
- {
- filter[i] = -(1./(sigma*sqrt(2.*M_PI)))*(exp(-((i+1)*(i+1))/(2.*sigma2))-exp(-((i-1)*(i-1))/(2.*sigma2)))/2.;
- }
-
- if (normalize) {
- double sum=0;
- for(int i=1; i<=middle; i++)
- {
- sum += 2.*(1./(sigma*sqrt(2.*M_PI)))*exp(-(i*i)/(2.*sigma2));
- }
- sum += (1./(sigma*sqrt(2.*M_PI))) ;
-
- for(int i=1; i<=middle; i++)
- {
- filter[i] = filter[i]/sum;
- }
- }
-}
-
-
-void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx)
-{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- //dIx=0;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < 3 ; j++)
- {
- dIx[i][j]=0;
- }
- for (unsigned int j=3 ; j < I.getWidth()-3 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j);
- }
- for (unsigned int j=I.getWidth()-3 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
- }
- }
-}
-
-void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy)
-{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- //dIy=0;
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
- for (unsigned int i=3 ; i < I.getHeight()-3 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j);
- }
- }
- for (unsigned int i=I.getHeight()-3 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
-}
-
-void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
-{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- //#pragma omp parallel for
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=0;
- }
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
- }
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
- }
- }
-}
-void vpImageFilter::getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
-{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- //dIx=0;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=0;
- }
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
- }
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
- }
- }
-}
-
-void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
-{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- //#pragma omp parallel for
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
- //#pragma omp parallel for
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
- }
- }
- //#pragma omp parallel for
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
-}
-
-void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
-{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- //dIy=0;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
- }
- }
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
- }
- }
-}
-
-/*!
- Compute the gradient along X after applying a gaussian filter along Y.
- \param I : Input image
- \param dIx : Gradient along X.
- \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
- \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
- \param size : Size of the Gaussian and Gaussian derivative kernels.
- */
-void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
-{
- vpImage<double> GIy;
- vpImageFilter::filterY(I, GIy, gaussianKernel, size);
- vpImageFilter::getGradX(GIy, dIx, gaussianDerivativeKernel, size);
-}
-
-/*!
- Compute the gradient along Y after applying a gaussian filter along X.
- \param I : Input image
- \param dIy : Gradient along Y.
- \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
- \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
- \param size : Size of the Gaussian and Gaussian derivative kernels.
- */
-void vpImageFilter::getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel,unsigned int size)
-{
- vpImage<double> GIx;
- vpImageFilter::filterX(I, GIx, gaussianKernel, size);
- vpImageFilter::getGradY(GIx, dIy, gaussianDerivativeKernel, size);
-}
-
-//operation pour pyramide gaussienne
-void vpImageFilter::getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
-{
- vpImage<unsigned char> GIx;
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- cv::Mat imgsrc, imgdest;
- vpImageConvert::convert(I, imgsrc);
- cv::pyrDown( imgsrc, imgdest, cv::Size((int)I.getWidth()/2,(int)I.getHeight()/2));
- vpImageConvert::convert(imgdest, GI);
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat imgsrc, imgdest;
- vpImageConvert::convert(I, imgsrc);
- cv::pyrDown( imgsrc, imgdest, cvSize((int)I.getWidth()/2,(int)I.getHeight()/2));
- vpImageConvert::convert(imgdest, GI);
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- IplImage* imgsrc = NULL;//cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
- IplImage* imgdest = NULL;//cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
- imgsrc = cvCreateImage(cvSize((int)I.getWidth(),(int)I.getHeight()), IPL_DEPTH_8U, 1);
- imgdest = cvCreateImage(cvSize((int)I.getWidth()/2,(int)I.getHeight()/2), IPL_DEPTH_8U, 1);
- vpImageConvert::convert(I,imgsrc);
- cvPyrDown( imgsrc, imgdest);
- vpImageConvert::convert(imgdest,GI);
-
- cvReleaseImage(&imgsrc);
- cvReleaseImage(&imgdest);
- //vpImage<unsigned char> sGI;sGI=GI;
-
-#else
- vpImageFilter::getGaussXPyramidal(I,GIx);
- vpImageFilter::getGaussYPyramidal(GIx,GI);
-#endif
-}
-
-void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
-{
-#if 0
- GI.resize(I.getHeight(),(int)((I.getWidth()+1.)/2.)) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- GI[i][0]=I[i][0];
- for (unsigned int j=1 ; j < ((I.getWidth()+1.)/2.)-1 ; j++)
- {
- GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
- }
- GI[i][(int)((I.getWidth()+1.)/2.)-1]=I[i][2*((int)((I.getWidth()+1.)/2.)-1)];
- }
-#else
- unsigned int w = I.getWidth()/2;
-
- GI.resize(I.getHeight(), w) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- GI[i][0]=I[i][0];
- for (unsigned int j=1 ; j < w-1 ; j++)
- {
- GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
- }
- GI[i][w-1]=I[i][2*w-1];
- }
-
-#endif
-}
-void vpImageFilter::getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
-{
-
-#ifdef ORIG
- GI.resize((int)((I.getHeight()+1.)/2.),I.getWidth()) ;
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- GI[0][j]=I[0][j];
- for (unsigned int i=1 ; i < ((I.getHeight()+1.)/2.)-1 ; i++)
- {
- GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
- }
- GI[(int)((I.getHeight()+1.)/2.)-1][j]=I[2*((int)((I.getHeight()+1.)/2.)-1)][j];
- }
-
-#else
- unsigned int h = I.getHeight()/2;
-
- GI.resize(h, I.getWidth()) ;
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- GI[0][j]=I[0][j];
- for (unsigned int i=1 ; i < h-1 ; i++)
- {
- GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
- }
- GI[h-1][j]=I[2*h-1][j];
- }
-#endif
-}
-
-
diff --git a/src/image/vpImageFilter.h b/src/image/vpImageFilter.h
deleted file mode 100644
index 0a43b22..0000000
--- a/src/image/vpImageFilter.h
+++ /dev/null
@@ -1,448 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageFilter.h 4991 2014-11-21 16:03:09Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Various image tools, convolution, ...
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpImageFilter_H
-#define vpImageFilter_H
-
-/*!
- \file vpImageFilter.h
- \brief Various image filter, convolution, etc...
-
-*/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageException.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-
-#include <fstream>
-#include <iostream>
-#include <math.h>
-#include <string.h>
-
-/*!
- \class vpImageFilter
-
- \ingroup ImageFiltering
-
- \brief Various image filter, convolution, etc...
-
-*/
-class VISP_EXPORT vpImageFilter
-{
-
-public:
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- static void canny(const vpImage<unsigned char>& I,
- vpImage<unsigned char>& Ic,
- const unsigned int gaussianFilterSize,
- const double thresholdCanny,
- const unsigned int apertureSobel);
-#endif
-
- /*!
- Apply a 1x3 derivative filter to an image pixel.
-
- \param I : Image to filter
- \param r: coordinates (row) of the pixel
- \param c : coordinates (column) of the pixel
- */
- template<class T>
- static double derivativeFilterX(const vpImage<T> &I,
- const unsigned int r, const unsigned int c)
- {
- return (2047.0 *(I[r][c+1] - I[r][c-1])
- +913.0 *(I[r][c+2] - I[r][c-2])
- +112.0 *(I[r][c+3] - I[r][c-3]))/8418.0;
- }
-
- /*!
- Apply a 3x1 derivative filter to an image pixel.
-
- \param I : Image to filter
- \param r : coordinates (row) of the pixel
- \param c : coordinates (column) of the pixel
- */
- template<class T>
- static double derivativeFilterY(const vpImage<T> &I,
- const unsigned int r, const unsigned int c)
- {
- return (2047.0 *(I[r+1][c] - I[r-1][c])
- +913.0 *(I[r+2][c] - I[r-2][c])
- +112.0 *(I[r+3][c] - I[r-3][c]))/8418.0;
- }
-
- /*!
- Apply a 1 x size Derivative Filter in X to an image pixel.
-
- \param I : Image to filter
- \param r : coordinates (row) of the pixel
- \param c : coordinates (column) of the pixel
- \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
- \param size : size of the filter
-
- \sa vpImageFilter::getGaussianDerivativeKernel()
- */
-
- template<class T>
- static double derivativeFilterX(const vpImage<T> &I,
- const unsigned int r, const unsigned int c,
- const double *filter, const unsigned int size)
- {
- unsigned int i;
- double result;
-
- result = 0;
-
- for(i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] - I[r][c-i]) ;
- }
- return result;
- }
-
-
-
- /*!
- Apply a size x 1 Derivative Filter in Y to an image pixel.
-
- \param I : Image to filter
- \param r : coordinates (row) of the pixel
- \param c : coordinates (column) of the pixel
- \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
- \param size : size of the filter
-
- \sa vpImageFilter::getGaussianDerivativeKernel()
- */
- template<class T>
- static double derivativeFilterY(const vpImage<T> &I,
- const unsigned int r, const unsigned int c,
- const double *filter, const unsigned int size)
- {
- unsigned int i;
- double result;
-
- result = 0;
-
- for(i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] - I[r-i][c]) ;
- }
- return result;
- }
-
- static void filter(const vpImage<double> &I,
- vpImage<double>& Iu,
- vpImage<double>& Iv,
- const vpMatrix& M) ;
-
-
- static void filter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpMatrix& M) ;
-
-
- static void filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size);
- static void filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size);
-
- static inline unsigned char filterGaussXPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
- {
- return (unsigned char)((1.*I[i][j-2]+4.*I[i][j-1]+6.*I[i][j]+4.*I[i][j+1]+1.*I[i][j+2])/16.);
- }
- static inline unsigned char filterGaussYPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
- {
- return (unsigned char)((1.*I[i-2][j]+4.*I[i-1][j]+6.*I[i][j]+4.*I[i+1][j]+1.*I[i+2][j])/16.);
- }
-
- static void filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static void filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
-
- static inline double filterX(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
-
- static inline double filterXLeftBorder(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c>i)
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- else
- result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterXRightBorder(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c+i<I.getWidth())
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- else
- result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterX(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterXLeftBorder(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c>i)
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- else
- result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterXRightBorder(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c+i<I.getWidth())
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
- else
- result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static void filterY(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static void filterY(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static inline double filterY(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- double static inline filterYTopBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r>i)
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- else
- result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- double static inline filterYBottomBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r+i<I.getHeight())
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- else
- result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterYTopBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r>i)
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- else
- result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterYBottomBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r+i<I.getHeight())
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- else
- result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static inline double filterY(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
- {
- double result;
-
- result = 0;
-
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
- }
- return result+filter[0]*I[r][c];
- }
-
- static void gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size=7, double sigma=0., bool normalize=true);
- /*!
- Apply a 5x5 Gaussian filter to an image pixel.
-
- \param fr : Image to filter
- \param r : coordinates (row) of the pixel
- \param c : coordinates (column) of the pixel
- */
- template<class T>
- static double gaussianFilter(const vpImage<T> & fr,
- const unsigned int r, const unsigned int c)
- {
- //filter Gaussien
- return (
- 15.0 * fr[r][c]
- + 12.0 * ( fr[r-1][c] + fr[r][c-1] + fr[r+1][c] + fr[r][c+1] )
- + 9.0 * ( fr[r-1][c-1] + fr[r+1][c-1] + fr[r-1][c+1] + fr[r+1][c+1])
- + 5.0 * ( fr[r-2][c] + fr[r][c-2] + fr[r+2][c] + fr[r][c+2] )
- + 4.0 * ( fr[r-2][c+1] + fr[r-2][c-1] + fr[r-1][c-2] + fr[r+1][c-2] +
- fr[r+2][c-1] + fr[r+2][c+1] + fr[r-1][c+2] + fr[r+1][c+2] )
- + 2.0 * ( fr[r-2][c-2] + fr[r+2][c-2] + fr[r-2][c+2] + fr[r+2][c+2] )
- )
- /159.0;
- }
- //operation pour pyramide gaussienne
- static void getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
- static void getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
- static void getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
-
- static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
- static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
-
- //fonction renvoyant le gradient en X de l'image I pour traitement pyramidal => dimension /2
- static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx);
- static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
- static void getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
- static void getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel,
- const double *gaussianDerivativeKernel, unsigned int size);
-
- //fonction renvoyant le gradient en Y de l'image I
- static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy);
- static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
- static void getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
- static void getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel,
- const double *gaussianDerivativeKernel,unsigned int size);
-
-} ;
-
-
-#endif
diff --git a/src/image/vpImageIo.cpp b/src/image/vpImageIo.cpp
deleted file mode 100644
index 69b470c..0000000
--- a/src/image/vpImageIo.cpp
+++ /dev/null
@@ -1,3329 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageIo.cpp 5249 2015-02-03 13:04:27Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read/write images.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpImageIo.cpp
- \brief Read/write images
-*/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h> //image conversion
-#include <visp/vpIoTools.h>
-
-const int vpImageIo::vpMAX_LEN = 100;
-
-/*!
-
- Open a file with read access.
-
- \param filename : Name of the file to open.
-
- \return File descriptor.
-*/
-FILE *
-vpImageIo::openFileRead(const char *filename)
-{
-
- FILE *fd ;
-
- // Lecture du nom du fichier image.
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,
- "filename empty ")) ;
- }
-
- // Ouverture de l'image.
- if ((fd = fopen(filename, "r")) == NULL)
- {
- vpERROR_TRACE("cannot open file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot open file")) ;
- }
- return fd ;
-}
-
-/*!
-
- Open a file with write access.
-
- \param filename : Name of the file to open.
-
- \param mode : Access mode. By default set to "w" for write
- access. Could be changed to set for example the access mode to "wa"
- to append data in the file.
-
- \return File descriptor.
-*/
-FILE *
-vpImageIo::openFileWrite(const char *filename, const char *mode)
-{
- FILE *fd ;
-
- // Lecture du nom du fichier image.
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,
- "filename empty ")) ;
- }
-
- // Ouverture de l'image.
- if ((fd = fopen(filename, mode)) == NULL)
- {
- vpERROR_TRACE("cannot open file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot open file")) ;
- }
- return fd ;
-}
-
-/*!
-
- Open a file with read access.
-
- \param filename : Name of the file to open.
-
- \return File descriptor.
-*/
-FILE *
-vpImageIo::openFileRead(const std::string filename)
-{
-
- FILE *fd ;
-
- // Lecture du nom du fichier image.
- if (filename.empty()) {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,
- "filename empty ")) ;
- }
-
- // Ouverture de l'image.
- if ((fd = fopen(filename.c_str(), "r")) == NULL)
- {
- vpERROR_TRACE("cannot open file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot open file")) ;
- }
- return fd ;
-}
-
-/*!
-
- Open a file with write access.
-
- \param filename : Name of the file to open.
-
- \param mode : Access mode. By default set to "w" for write
- access. Could be changed to set for example the access mode to "wa"
- to append data in the file.
-
- \return File descriptor.
-*/
-FILE *
-vpImageIo::openFileWrite(const std::string filename,
- const std::string mode)
-{
- FILE *fd ;
-
- // Lecture du nom du fichier image.
- if (filename.empty())
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,
- "filename empty ")) ;
- }
-
- // Ouverture de l'image.
- if ((fd = fopen(filename.c_str(), mode.c_str())) == NULL)
- {
- vpERROR_TRACE("cannot open file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot open file")) ;
- }
- return fd ;
-}
-
-vpImageIo::vpImageFormatType
-vpImageIo::getFormat(const char *filename)
-{
- std::string sfilename(filename);
-
- std::string ext = vpImageIo::getExtension(sfilename);
-
- if (ext.compare(".PGM") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".pgm") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".PPM") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".ppm") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".JPG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".JPEG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpeg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".PNG") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".png") == 0)
- return FORMAT_PNG;
- // Formats supported by opencv
- else if (ext.compare(".TIFF") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".tiff") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".TIF") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".tif") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".BMP") == 0)
- return FORMAT_BMP;
- else if (ext.compare(".bmp") == 0)
- return FORMAT_BMP;
- else if (ext.compare(".DIB") == 0)
- return FORMAT_DIB;
- else if (ext.compare(".dib") == 0)
- return FORMAT_DIB;
- else if (ext.compare(".PBM") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".pbm") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".SR") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".sr") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".RAS") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".ras") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".JP2") == 0)
- return FORMAT_JPEG2000;
- else if (ext.compare(".jp2") == 0)
- return FORMAT_JPEG2000;
- else
- return FORMAT_UNKNOWN;
-}
-
-// return the extension of the file including the dot
-std::string vpImageIo::getExtension(const std::string &filename)
-{
- // extract the extension
- size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
- return ext;
-}
-
-
-/*!
- Read the contents of the image filename, allocate memory for the corresponding
- greyscale image, update its content, and return a reference to the image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::read(vpImage<unsigned char> &I, const char *filename)
-{
- bool exist = vpIoTools::checkFilename(filename);
- if (!exist) {
- std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
- throw (vpImageException(vpImageException::ioError, message));
- }
- bool try_opencv_reader = false;
-
- switch(getFormat(filename)){
- case FORMAT_PGM :
- readPGM(I,filename); break;
- case FORMAT_PPM :
- readPPM(I,filename); break;
- case FORMAT_JPEG :
-#ifdef VISP_HAVE_LIBJPEG
- readJPEG(I,filename);
-#else
- try_opencv_reader = true;
-#endif
- break;
- case FORMAT_PNG :
-#if defined(VISP_HAVE_LIBPNG)
- readPNG(I,filename);
-#else
- try_opencv_reader = true;
-#endif
- break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
- try_opencv_reader = true;
- break;
- }
-
- if (try_opencv_reader) {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- //std::cout << "Use opencv to read the image" << std::endl;
- cv::Mat cvI = cv::imread(filename, cv::IMREAD_GRAYSCALE);
- if (cvI.cols == 0 && cvI.rows == 0) {
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
- }
- vpImageConvert::convert(cvI, I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- //std::cout << "Use opencv to read the image" << std::endl;
- cv::Mat cvI = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if (cvI.cols == 0 && cvI.rows == 0) {
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
- }
- vpImageConvert::convert(cvI, I);
-#else
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
-#endif
- }
-}
-/*!
- Read the contents of the image filename, allocate memory for the corresponding
- greyscale image, update its content, and return a reference to the image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::read(vpImage<unsigned char> &I, const std::string filename)
-{
- read(I,filename.c_str());
-}
-/*!
- Read the contents of the image filename, allocate memory for the corresponding
- color image, update its content, and return a reference to the image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::read(vpImage<vpRGBa> &I, const char *filename)
-{
- bool exist = vpIoTools::checkFilename(filename);
- if (!exist) {
- std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
- throw (vpImageException(vpImageException::ioError, message));
- }
-
- bool try_opencv_reader = false;
-
- switch(getFormat(filename)){
- case FORMAT_PGM :
- readPGM(I,filename); break;
- case FORMAT_PPM :
- readPPM(I,filename); break;
- case FORMAT_JPEG :
-#ifdef VISP_HAVE_LIBJPEG
- readJPEG(I,filename);
-#else
- try_opencv_reader = true;
-#endif
- break;
- case FORMAT_PNG :
-#if defined(VISP_HAVE_LIBPNG)
- readPNG(I,filename);
-#else
- try_opencv_reader = true;
-#endif
- break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
- try_opencv_reader = true;
- break;
- }
-
- if (try_opencv_reader) {
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- // std::cout << "Use opencv to read the image" << std::endl;
- cv::Mat cvI = cv::imread(filename, cv::IMREAD_COLOR);
- if (cvI.cols == 0 && cvI.rows == 0) {
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
- }
- vpImageConvert::convert(cvI, I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- // std::cout << "Use opencv to read the image" << std::endl;
- cv::Mat cvI = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
- if (cvI.cols == 0 && cvI.rows == 0) {
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
- }
- vpImageConvert::convert(cvI, I);
-#else
- std::string message = "Cannot read file \"" + std::string(filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
-#endif
- }
-}
-/*!
- Read the contents of the image filename, allocate memory for the corresponding
- color image, update its content, and return a reference to the image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::read(vpImage<vpRGBa> &I, const std::string filename)
-{
- read(I,filename.c_str());
-}
-
-/*!
- Write the content of the image in the file which name is given by \e
- filename.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to write.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::write(const vpImage<unsigned char> &I, const char *filename)
-{
- bool try_opencv_writer = false;
-
- switch(getFormat(filename)){
- case FORMAT_PGM :
- writePGM(I,filename); break;
- case FORMAT_PPM :
- writePPM(I,filename); break;
- case FORMAT_JPEG :
-#ifdef VISP_HAVE_LIBJPEG
- writeJPEG(I,filename);
-#else
- try_opencv_writer = true;
-#endif
- break;
- case FORMAT_PNG :
-#ifdef VISP_HAVE_LIBPNG
- writePNG(I,filename);
-#else
- try_opencv_writer = true;
-#endif
- break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
- try_opencv_writer = true;
- break;
- }
-
- if (try_opencv_writer) {
-#if VISP_HAVE_OPENCV_VERSION >= 0x020100
- // std::cout << "Use opencv to write the image" << std::endl;
- cv::Mat cvI;
- vpImageConvert::convert(I, cvI);
- cv::imwrite(filename, cvI);
-#else
- vpCERROR << "Cannot write file: Image format not supported..." << std::endl;
- throw (vpImageException(vpImageException::ioError,
- "Cannot write file: Image format not supported")) ;
-#endif
- }
-}
-/*!
- Write the content of the image in the file which name is given by \e
- filename.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to write.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::write(const vpImage<unsigned char> &I, const std::string filename)
-{
- write(I,filename.c_str());
-}
-/*!
- Write the content of the image in the file which name is given by \e
- filename.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to write.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::write(const vpImage<vpRGBa> &I, const char *filename)
-{
- bool try_opencv_writer = false;
-
- switch(getFormat(filename)){
- case FORMAT_PGM :
- writePGM(I,filename); break;
- case FORMAT_PPM :
- writePPM(I,filename); break;
- case FORMAT_JPEG :
-#ifdef VISP_HAVE_LIBJPEG
- writeJPEG(I,filename);
-#else
- try_opencv_writer = true;
-#endif
- break;
- case FORMAT_PNG :
-#ifdef VISP_HAVE_LIBPNG
- writePNG(I,filename);
-#else
- try_opencv_writer = true;
-#endif
- break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
- try_opencv_writer = true;
- break;
- }
-
- if (try_opencv_writer) {
-#if VISP_HAVE_OPENCV_VERSION >= 0x020100
- // std::cout << "Use opencv to write the image" << std::endl;
- cv::Mat cvI;
- vpImageConvert::convert(I, cvI);
- cv::imwrite(filename, cvI);
-#else
- vpCERROR << "Cannot write file: Image format not supported..." << std::endl;
- throw (vpImageException(vpImageException::ioError,
- "Cannot write file: Image format not supported")) ;
-#endif
- }
-}
-/*!
- Write the content of the image in the file which name is given by \e
- filename.
-
- Always supported formats are *.pgm and *.ppm.
- If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
- If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
-
- \param I : Image to write.
- \param filename : Name of the file containing the image.
- */
-void
-vpImageIo::write(const vpImage<vpRGBa> &I, const std::string filename)
-{
- write(I,filename.c_str());
-}
-//--------------------------------------------------------------------------
-// PFM
-//--------------------------------------------------------------------------
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function is built like portable gray pixmap (eg PGM P5) file.
- but considers float image data.
-
- \param I : Image to save as a (PFM P8) file.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::writePFM(const vpImage<float> &I,
- const char *filename)
-{
-
- FILE* fd;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- fd = fopen(filename, "wb");
-
- if (fd == NULL) {
- vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- // Write the head
- fprintf(fd, "P8\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
-
- // Write the bitmap
- size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
-
- ierr = fwrite(I.bitmap, sizeof(float), nbyte, fd) ;
- if (ierr != nbyte) {
- fclose(fd);
- vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
- nbyte, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- fflush(fd);
- fclose(fd);
-
-}
-//--------------------------------------------------------------------------
-// PGM
-//--------------------------------------------------------------------------
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::writePGM(const vpImage<unsigned char> &I,
- const char *filename)
-{
-
- FILE* fd;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- fd = fopen(filename, "wb");
-
- if (fd == NULL) {
- vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- // Write the head
- fprintf(fd, "P5\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
-
- // Write the bitmap
- size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
-
- ierr = fwrite(I.bitmap, sizeof(unsigned char), nbyte, fd) ;
- if (ierr != nbyte) {
- fclose(fd);
- vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
- nbyte, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- fflush(fd);
- fclose(fd);
-
-}
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePGM(const vpImage<short> &I, const char *filename)
-{
- vpImage<unsigned char> Iuc ;
- unsigned int nrows = I.getHeight();
- unsigned int ncols = I.getWidth();
-
- Iuc.resize(nrows, ncols);
-
- for (unsigned int i=0 ; i < nrows * ncols ; i++)
- Iuc.bitmap[i] = (unsigned char)I.bitmap[i] ;
-
- vpImageIo::writePGM(Iuc, filename) ;
-
-
-}
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
- Color image is converted into a grayscale image.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::writePGM(const vpImage<vpRGBa> &I, const char *filename)
-{
-
- FILE* fd;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- fd = fopen(filename, "wb");
-
- if (fd == NULL) {
- vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- // Write the head
- fprintf(fd, "P5\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
-
- // Write the bitmap
- size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
-
-
- vpImage<unsigned char> Itmp ;
- vpImageConvert::convert(I,Itmp) ;
-
- ierr = fwrite(Itmp.bitmap, sizeof(unsigned char), nbyte, fd) ;
- if (ierr != nbyte) {
- fclose(fd);
- vpERROR_TRACE("couldn't write %d bytes to file \"%s\"\n",
- nbyte, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- fflush(fd);
- fclose(fd);
-
-}
-
-/*!
- Read a PFM P8 file and initialize a float image.
-
- Read the contents of the portable gray pixmap (PFM P8) filename, allocate
- memory for the corresponding image, and set the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::readPFM(vpImage<float> &I, const char *filename)
-{
- FILE* fd = NULL; // File descriptor
- int ierr;
- int line;
- int is255;
- char* err ;
- char str[vpMAX_LEN];
- unsigned int w, h;
-
- // Test the filename
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("no filename") ;
- throw (vpImageException(vpImageException::ioError,
- " no filename")) ;
-
- }
-
- // Open the filename
- fd = fopen(filename, "rb");
- if (fd == NULL)
- {
- vpERROR_TRACE("couldn't read file \"%s\"", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "couldn't read file")) ;
- }
-
- // Read the first line with magic number P8
- line = 0;
-
- err = fgets(str, vpMAX_LEN - 1, fd);
- line++;
- if (err == NULL)
- {
- fclose (fd);
- vpERROR_TRACE("couldn't read line %d of file \"%s\"\n", line, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "couldn't read file")) ;
- }
-
- if (strlen(str) < 3)
- {
- fclose (fd);
- vpERROR_TRACE("\"%s\" is not a PFM file\n", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "this is not a PFM file")) ;
- }
-
- str[2] = '\0';
- if (strcmp(str, "P8") != 0)
- {
- fclose (fd);
- vpERROR_TRACE("\"%s\" is not a PFM file\n", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "this is not a PFM file")) ;
- }
-
- // Jump the possible comment, or empty line and read the following line
- do {
- err = fgets(str, vpMAX_LEN - 1, fd);
- line++;
- if (err == NULL) {
- fprintf(stderr, "couldn't read line %d of file \"%s\"\n", line, filename);
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
- } while ((str[0] == '#') || (str[0] == '\n'));
-
- // Extract image size
- ierr = sscanf(str, "%d %d", &w, &h);
- if (w > 100000 || h>100000) {
- fclose (fd);
- throw(vpException(vpException::badValue, "Bad image size"));
- }
-
- if(ierr == 1){// the norm allows to have the two values on two separated lines.
- do {
- err = fgets(str, vpMAX_LEN - 1, fd);
- line++;
- if (err == NULL) {
- fprintf(stderr, "couldn't read line %d of file \"%s\"\n", line, filename);
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
- } while ((str[0] == '#') || (str[0] == '\n'));
- ierr = sscanf(str, "%d", &h);
- }
- if (ierr == EOF)
- {
- fclose (fd);
- vpERROR_TRACE("couldn't read line %d of file \"%s\"\n",line, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
-
- if ((h != I.getHeight())||( w != I.getWidth()))
- {
- try
- {
- I.resize(h,w) ;
- }
- catch(...)
- {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
- }
-
- // Read 255
- err = fgets(str, vpMAX_LEN - 1, fd);
- line++;
- if (err == NULL) {
- fclose (fd);
- vpERROR_TRACE("couldn't read line %d of file \"%s\"\n",line, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
-
- ierr = sscanf(str, "%d", &is255);
- if (ierr == EOF) {
- fclose (fd);
- vpERROR_TRACE("couldn't read line %d of file \"%s\"\n", line, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
-
- if (is255 != 255)
- {
- fclose (fd);
- vpERROR_TRACE("MAX_VAL is not 255 in file \"%s\"\n", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
-
- unsigned int nbyte = I.getHeight()*I.getWidth();
- if (fread (I.bitmap, sizeof(float), nbyte, fd ) != nbyte)
- {
- fclose (fd);
- vpERROR_TRACE("couldn't read %d bytes in file \"%s\"\n", nbyte, filename) ;
- throw (vpImageException(vpImageException::ioError,
- "Cannot read content of PFM file")) ;
- }
-
- fclose (fd);
-}
-
-
-
-/*!
- Read a PGM P5 file and initialize a scalar image.
-
- Read the contents of the portable gray pixmap (PGM P5) filename, allocate
- memory for the corresponding image, and set the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::readPGM(vpImage<unsigned char> &I, const char *filename)
-{
- FILE* fd = NULL; // File descriptor
- int ierr;
- char* err ;
- char str[vpMAX_LEN];
- unsigned int magic=5, w=0, h=0, maxval=255;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- throw (vpImageException(vpImageException::ioError,
- "No filename")) ;
- }
-
- // Open the filename
- if ((fd = fopen(filename, "rb")) == NULL) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot read file \"%s\"", filename)) ;
- }
-
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "P%u %u %u %u", &magic, &w, &h, &maxval)) == 0) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-
- if (magic != 5) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "\"%s\" is not a PGM P5 file", filename));
- }
-
- // Depending on ierr the line may contain:
- // 1 : P5
- // 2 : P5 w
- // 3 : P5 w h
- // 4 : P5 w h maxval
-
- if (ierr == 1) {
-// std::cout << "magic: " << magic << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u %u", &w, &h, &maxval)) == 0) || (ierr != 1 && ierr != 2 && ierr != 3)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- // Depending on ierr the line may contain:
- // 1 : w
- // 2 : w h
- // 3 : w h maxval
- if (ierr == 1) {
-// std::cout << "w: " << w << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (ierr == 1) {
-// std::cout << "h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- }
-// else {
-// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 2) {
-// std::cout << "w: " << w << " h: " << h << std::endl;
-
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else {
-// std::cout << "w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 2) {
-// std::cout << "magic: " << magic << " w: " << w << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (ierr == 1) {
-// std::cout << "h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else {
-// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 3) {
-// std::cout << "magic: " << magic << " w: " << w << " h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else if (ierr == 4) {
-// std::cout << "magic: " << magic << " w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
-// }
-
- if (w > 100000 || h>100000) {
- fclose (fd);
- throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename));
- }
- if (maxval != 255)
- {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Bad maxval in \"%s\"", filename));
- }
-
- if ((h != I.getHeight())||( w != I.getWidth())) {
- I.resize(h,w) ;
- }
-
- unsigned int nbyte = I.getHeight()*I.getWidth();
- size_t n;
- if ((n = fread (I.bitmap, sizeof(unsigned char), nbyte, fd)) != nbyte) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Read only %d of %d bytes in file \"%s\"", n, nbyte, filename));
- }
-
- fclose (fd);
-}
-
-
-/*!
- Read a PGM P5 file and initialize a scalar image.
-
- Read the contents of the portable gray pixmap (PGM P5) filename, allocate
- memory for the corresponding image, and set the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- The gray level image contained in the \e filename is converted in a
- color image in \e I.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::readPGM(vpImage<vpRGBa> &I, const char *filename)
-{
- try
- {
- vpImage<unsigned char> Itmp ;
-
- vpImageIo::readPGM(Itmp, filename) ;
-
- vpImageConvert::convert(Itmp, I) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-
-//--------------------------------------------------------------------------
-// PPM
-//--------------------------------------------------------------------------
-
-/*!
- Read the contents of the portable pixmap (PPM P6) filename, allocate memory
- for the corresponding gray level image, convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. The quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPPM(vpImage<unsigned char> &I, const char *filename)
-{
-
- try
- {
- vpImage<vpRGBa> Itmp ;
-
- vpImageIo::readPPM(Itmp, filename) ;
-
- vpImageConvert::convert(Itmp, I) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-
-/*!
- Read the contents of the portable pixmap (PPM P6) filename,
- allocate memory for the corresponding vpRGBa image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPPM(vpImage<vpRGBa> &I, const char *filename)
-{
- FILE* fd = NULL; // File descriptor
- int ierr;
- char* err ;
- char str[vpMAX_LEN];
- unsigned int magic=5, w=0, h=0, maxval=255;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- throw (vpImageException(vpImageException::ioError,
- "No filename")) ;
- }
-
- // Open the filename
- if ((fd = fopen(filename, "rb")) == NULL) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot read file \"%s\"", filename)) ;
- }
-
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "P%u %u %u %u", &magic, &w, &h, &maxval)) == 0) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-
- if (magic != 6) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "\"%s\" is not a PGM P6 file", filename));
- }
-
- // Depending on ierr the line may contain:
- // 1 : P6
- // 2 : P6 w
- // 3 : P6 w h
- // 4 : P6 w h maxval
-
- if (ierr == 1) {
-// std::cout << "magic: " << magic << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u %u", &w, &h, &maxval)) == 0) || (ierr != 1 && ierr != 2 && ierr != 3)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- // Depending on ierr the line may contain:
- // 1 : w
- // 2 : w h
- // 3 : w h maxval
- if (ierr == 1) {
-// std::cout << "w: " << w << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (ierr == 1) {
-// std::cout << "h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- }
-// else {
-// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 2) {
-// std::cout << "w: " << w << " h: " << h << std::endl;
-
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else {
-// std::cout << "w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 2) {
-// std::cout << "magic: " << magic << " w: " << w << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (((ierr = sscanf(str, "%u %u", &h, &maxval)) == 0) || (ierr != 1 && ierr != 2)) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if (ierr == 1) {
-// std::cout << "h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else {
-// std::cout << "h: " << h << " maxval: " << maxval << std::endl;
-// }
- }
- else if (ierr == 3) {
-// std::cout << "magic: " << magic << " w: " << w << " h: " << h << std::endl;
- while ((err = fgets(str, vpMAX_LEN - 1, fd)) != NULL && ((str[0] == '#') || (str[0] == '\n'))) {};
- if (err == NULL) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
- if ((ierr = sscanf(str, "%u", &maxval)) != 1) {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read header of file \"%s\"", filename));
- }
-// std::cout << "maxval: " << maxval << std::endl;
- }
-// else if (ierr == 4) {
-// std::cout << "magic: " << magic << " w: " << w << " h: " << h << " maxval: " << maxval << std::endl;
-// }
-
- if (w > 100000 || h>100000) {
- fclose (fd);
- throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename));
- }
- if (maxval != 255)
- {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Bad maxval in \"%s\"", filename));
- }
-
- if ((h != I.getHeight())||( w != I.getWidth())) {
- I.resize(h,w) ;
- }
-
- for(unsigned int i=0;i<I.getHeight();i++)
- {
- for(unsigned int j=0;j<I.getWidth();j++)
- {
- vpRGBa v ;
- size_t res = fread(&v.R,sizeof(v.R),1,fd) ;
- res |= fread(&v.G,sizeof(v.G),1,fd) ;
- res |= fread(&v.B,sizeof(v.B),1,fd) ;
- if (res==0)
- {
- fclose (fd);
- throw (vpImageException(vpImageException::ioError,
- "Cannot read bytes in file \"%s\"\n", filename));
- }
- I[i][j] = v ;
- }
- }
-
- fclose (fd);
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PPM P6) file.
- grayscale image is converted into a color image vpRGBa.
-
- \param I : Image to save as a (PPM P6) file.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::writePPM(const vpImage<unsigned char> &I, const char *filename)
-{
-
- try
- {
- vpImage<vpRGBa> Itmp ;
-
- vpImageConvert::convert(I, Itmp) ;
-
- vpImageIo::writePPM(Itmp, filename) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PPM P6) file.
-
- \param I : Image to save as a (PPM P6) file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePPM(const vpImage<vpRGBa> &I, const char *filename)
-{
-
- FILE* f;
-
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- f = fopen(filename, "wb");
-
- if (f == NULL) {
- vpERROR_TRACE("couldn't write to file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
-
-
- fprintf(f,"P6\n"); // Magic number
- fprintf(f,"%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(f,"%d\n",255); // Max level
-
- for(unsigned int i=0;i<I.getHeight();i++)
- {
- for(unsigned int j=0;j<I.getWidth();j++)
- {
- vpRGBa P ;
- size_t res ;
- P = I[i][j] ;
- unsigned char tmp ;
- tmp = P.R ;
- res = fwrite(&tmp,sizeof(tmp),1,f) ;
- if (res==0)
- {
- fclose(f);
- vpERROR_TRACE("couldn't write file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
- tmp = P.G;
- res = fwrite(&tmp,sizeof(tmp),1,f) ;
- if (res==0)
- {
- fclose(f);
- vpERROR_TRACE("couldn't write file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
- tmp = P.B ;
- res = fwrite(&tmp,sizeof(tmp),1,f) ;
- if (res==0)
- {
- fclose(f);
- vpERROR_TRACE("couldn't write file") ;
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
- }
- }
-
- fflush(f);
- fclose(f);
-}
-
-
-/*!
- Read a PGM P5 file and initialize a scalar image.
-
- Read the contents of the portable gray pixmap (PGM P5) filename, allocate
- memory for the corresponding image, and set the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::readPGM(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readPGM(I, filename.c_str());
-}
-
-/*!
- Read a PGM P5 file and initialize a scalar image.
-
- Read the contents of the portable gray pixmap (PGM P5) filename, allocate
- memory for the corresponding image, and set the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readPGM(I, filename.c_str());
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::writePGM(const vpImage<unsigned char> &I,
- const std::string filename)
-{
- vpImageIo::writePGM(I, filename.c_str());
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-*/
-
-void
-vpImageIo::writePGM(const vpImage<short> &I, const std::string filename)
-{
-
- vpImageIo::writePGM(I, filename.c_str());
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PGM P5) file.
- Color image is converted into a grayscale image.
-
- \param I : Image to save as a (PGM P5) file.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::writePGM(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writePGM(I, filename.c_str());
-}
-
-//--------------------------------------------------------------------------
-// PPM
-//--------------------------------------------------------------------------
-
-/*!
- Read the contents of the portable pixmap (PPM P6) filename, allocate memory
- for the corresponding gray level image, convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. The quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPPM(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readPPM(I, filename.c_str());
-}
-
-/*!
- Read the contents of the portable pixmap (PPM P6) filename,
- allocate memory for the corresponding vpRGBa image.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPPM(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readPPM(I, filename.c_str());
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PPM P6) file.
- grayscale image is converted into a color image vpRGBa.
-
- \param I : Image to save as a (PPM P6) file.
- \param filename : Name of the file containing the image.
-
-*/
-
-void
-vpImageIo::writePPM(const vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::writePPM(I, filename.c_str());
-}
-
-/*!
- Write the content of the bitmap in the file which name is given by \e
- filename. This function writes a portable gray pixmap (PPM P6) file.
-
- \param I : Image to save as a (PPM P6) file.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::writePPM(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writePPM(I, filename.c_str());
-}
-
-
-//--------------------------------------------------------------------------
-// JPEG
-//--------------------------------------------------------------------------
-
-#if defined(VISP_HAVE_LIBJPEG)
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const char *filename)
-{
- struct jpeg_compress_struct cinfo;
- struct jpeg_error_mgr jerr;
- FILE *file;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_compress(&cinfo);
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "wb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- jpeg_stdio_dest(&cinfo, file);
-
- cinfo.image_width = width;
- cinfo.image_height = height;
- cinfo.input_components = 1;
- cinfo.in_color_space = JCS_GRAYSCALE;
- jpeg_set_defaults(&cinfo);
-
- jpeg_start_compress(&cinfo,TRUE);
-
- unsigned char *line;
- line = new unsigned char[width];
- unsigned char* input = (unsigned char*)I.bitmap;
- while (cinfo.next_scanline < cinfo.image_height)
- {
- for (unsigned int i = 0; i < width; i++)
- {
- line[i] = *(input);
- input++;
- }
- jpeg_write_scanlines(&cinfo, &line, 1);
- }
-
- jpeg_finish_compress(&cinfo);
- jpeg_destroy_compress(&cinfo);
- delete [] line;
- fclose(file);
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::writeJPEG(I, filename.c_str());
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const char *filename)
-{
- struct jpeg_compress_struct cinfo;
- struct jpeg_error_mgr jerr;
- FILE *file;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_compress(&cinfo);
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "wb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- jpeg_stdio_dest(&cinfo, file);
-
- cinfo.image_width = width;
- cinfo.image_height = height;
- cinfo.input_components = 3;
- cinfo.in_color_space = JCS_RGB;
- jpeg_set_defaults(&cinfo);
-
- jpeg_start_compress(&cinfo,TRUE);
-
- unsigned char *line;
- line = new unsigned char[3*width];
- unsigned char* input = (unsigned char*)I.bitmap;
- while (cinfo.next_scanline < cinfo.image_height)
- {
- for (unsigned int i = 0; i < width; i++)
- {
- line[i*3] = *(input); input++;
- line[i*3+1] = *(input); input++;
- line[i*3+2] = *(input); input++;
- input++;
- }
- jpeg_write_scanlines(&cinfo, &line, 1);
- }
-
- jpeg_finish_compress(&cinfo);
- jpeg_destroy_compress(&cinfo);
- delete [] line;
- fclose(file);
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writeJPEG(I, filename.c_str());
-}
-
-
-/*!
- Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const char *filename)
-{
- struct jpeg_decompress_struct cinfo;
- struct jpeg_error_mgr jerr;
- FILE *file;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_decompress(&cinfo);
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "rb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot read file")) ;
- }
-
- jpeg_stdio_src(&cinfo, file);
- jpeg_read_header(&cinfo, TRUE);
-
- unsigned int width = cinfo.image_width;
- unsigned int height = cinfo.image_height;
-
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
-
- jpeg_start_decompress(&cinfo);
-
- unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
- JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
- ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
-
- if (cinfo.out_color_space == JCS_RGB) {
- vpImage<vpRGBa> Ic(height,width);
- unsigned char* output = (unsigned char*)Ic.bitmap;
- while (cinfo.output_scanline<cinfo.output_height) {
- jpeg_read_scanlines(&cinfo,buffer,1);
- for (unsigned int i = 0; i < width; i++) {
- *(output++) = buffer[0][i*3];
- *(output++) = buffer[0][i*3+1];
- *(output++) = buffer[0][i*3+2];
- *(output++) = 0;
- }
- }
- vpImageConvert::convert(Ic,I) ;
- }
-
- else if (cinfo.out_color_space == JCS_GRAYSCALE)
- {
- unsigned int row;
- while (cinfo.output_scanline<cinfo.output_height)
- {
- row = cinfo.output_scanline;
- jpeg_read_scanlines(&cinfo,buffer,1);
- memcpy(I[row], buffer[0], rowbytes);
- }
- }
-
- jpeg_finish_decompress(&cinfo);
- jpeg_destroy_decompress(&cinfo);
- fclose(file);
-}
-
-
-/*!
- Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readJPEG(I, filename.c_str());
-}
-
-
-/*!
- Read a JPEG file and initialize a scalar image.
-
- Read the contents of the JPEG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const char *filename)
-{
- struct jpeg_decompress_struct cinfo;
- struct jpeg_error_mgr jerr;
- FILE *file;
-
- cinfo.err = jpeg_std_error(&jerr);
- jpeg_create_decompress(&cinfo);
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "rb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot read file")) ;
- }
-
- jpeg_stdio_src(&cinfo, file);
-
- jpeg_read_header(&cinfo, TRUE);
-
- unsigned int width = cinfo.image_width;
- unsigned int height = cinfo.image_height;
-
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
-
- jpeg_start_decompress(&cinfo);
-
- unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
- JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
- ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
-
- if (cinfo.out_color_space == JCS_RGB)
- {
- unsigned char* output = (unsigned char*)I.bitmap;
- while (cinfo.output_scanline<cinfo.output_height)
- {
- jpeg_read_scanlines(&cinfo,buffer,1);
- for (unsigned int i = 0; i < width; i++) {
- *(output++) = buffer[0][i*3];
- *(output++) = buffer[0][i*3+1];
- *(output++) = buffer[0][i*3+2];
- *(output++) = 0;
- }
- }
- }
-
- else if (cinfo.out_color_space == JCS_GRAYSCALE)
- {
- vpImage<unsigned char> Ig(height,width);
-
- unsigned int row;
- while (cinfo.output_scanline<cinfo.output_height)
- {
- row = cinfo.output_scanline;
- jpeg_read_scanlines(&cinfo,buffer,1);
- memcpy(Ig[row], buffer[0], rowbytes);
- }
-
- vpImageConvert::convert(Ig,I) ;
- }
-
- jpeg_finish_decompress(&cinfo);
- jpeg_destroy_decompress(&cinfo);
- fclose(file);
-}
-
-
-/*!
- Read a JPEG file and initialize a scalar image.
-
- Read the contents of the JPEG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readJPEG(I, filename.c_str());
-}
-
-#elif defined(VISP_HAVE_OPENCV)
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip;
- vpImageConvert::convert(I, Ip);
- cv::imwrite(filename, Ip);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(I, Ip);
-
- cvSaveImage(filename, Ip);
-
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::writeJPEG(I, filename.c_str());
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip;
- vpImageConvert::convert(I, Ip);
- cv::imwrite(filename, Ip);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(I, Ip);
-
- cvSaveImage(filename, Ip);
-
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a JPEG file.
-
- \param I : Image to save as a JPEG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writeJPEG(I, filename.c_str());
-}
-
-
-/*!
- Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#else
- IplImage* Ip = NULL;
- Ip = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if (Ip != NULL)
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError,
- "Can't read the image")) ;
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readJPEG(I, filename.c_str());
-}
-
-
-/*!
- Read a JPEG file and initialize a scalar image.
-
- Read the contents of the JPEG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#else
- IplImage* Ip = NULL;
- Ip = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
- if (Ip != NULL)
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Read a JPEG file and initialize a scalar image.
-
- Read the contents of the JPEG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readJPEG(I, filename.c_str());
-}
-
-#endif
-
-
-
-
-
-
-//--------------------------------------------------------------------------
-// PNG
-//--------------------------------------------------------------------------
-
-#if defined(VISP_HAVE_LIBPNG)
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<unsigned char> &I, const char *filename)
-{
- FILE *file;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "wb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- /* create a png info struct */
- png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING,NULL, NULL, NULL);
- if (!png_ptr)
- {
- fclose (file);
- vpERROR_TRACE("Error during png_create_write_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_infop info_ptr = png_create_info_struct(png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, NULL);
- vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- vpERROR_TRACE("Error during init_io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- /* setup libpng for using standard C fwrite() function with our FILE pointer */
- png_init_io (png_ptr, file);
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
- int bit_depth = 8;
- int color_type = PNG_COLOR_TYPE_GRAY;
- /* set some useful information from header */
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- vpERROR_TRACE("Error during write header\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_set_IHDR(png_ptr, info_ptr, width, height,
- bit_depth, color_type, PNG_INTERLACE_NONE,
- PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
-
- png_write_info(png_ptr, info_ptr);
-
- png_bytep* row_ptrs = new png_bytep[height];
- for (unsigned int i = 0; i < height; i++)
- row_ptrs[i] = new png_byte[width];
-
- unsigned char* input = (unsigned char*)I.bitmap;
-
- for (unsigned int i = 0; i < height; i++)
- {
- png_byte* row = row_ptrs[i];
- for(unsigned int j = 0; j < width; j++)
- {
- row[j] = *(input);
- input++;
- }
- }
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
- vpERROR_TRACE("Error during write image\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_write_image(png_ptr, row_ptrs);
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
- vpERROR_TRACE("Error during write end\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_write_end(png_ptr, NULL);
-
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
-
- png_destroy_write_struct (&png_ptr, &info_ptr);
-
- fclose(file);
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::writePNG(I, filename.c_str());
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<vpRGBa> &I, const char *filename)
-{
- FILE *file;
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "wb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't write file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot write file")) ;
- }
-
- /* create a png info struct */
- png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING,NULL, NULL, NULL);
- if (!png_ptr)
- {
- fclose (file);
- vpERROR_TRACE("Error during png_create_write_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_infop info_ptr = png_create_info_struct(png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, NULL);
- vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- vpERROR_TRACE("Error during init_io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- /* setup libpng for using standard C fwrite() function with our FILE pointer */
- png_init_io (png_ptr, file);
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
- int bit_depth = 8;
- int color_type = PNG_COLOR_TYPE_RGB;
- /* set some useful information from header */
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- vpERROR_TRACE("Error during write header\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_set_IHDR(png_ptr, info_ptr, width, height,
- bit_depth, color_type, PNG_INTERLACE_NONE,
- PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
-
- png_write_info(png_ptr, info_ptr);
-
- png_bytep* row_ptrs = new png_bytep[height];
- for (unsigned int i = 0; i < height; i++)
- row_ptrs[i] = new png_byte[3*width];
-
- unsigned char* input = (unsigned char*)I.bitmap;;
-
- for (unsigned int i = 0; i < height; i++)
- {
- png_byte* row = row_ptrs[i];
- for(unsigned int j = 0; j < width; j++)
- {
- row[3*j] = *(input);input++;
- row[3*j+1] = *(input);input++;
- row[3*j+2] = *(input);input++;
- input++;
- }
- }
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
- vpERROR_TRACE("Error during write image\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_write_image(png_ptr, row_ptrs);
-
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
- vpERROR_TRACE("Error during write end\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
- }
-
- png_write_end(png_ptr, NULL);
-
- for(unsigned int j = 0; j < height; j++)
- delete[] row_ptrs[j];
-
- delete[] row_ptrs;
-
- png_destroy_write_struct (&png_ptr, &info_ptr);
-
- fclose(file);
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writePNG(I, filename.c_str());
-}
-
-/*!
- Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const char *filename)
-{
- FILE *file;
- png_byte magic[8];
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "rb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot read file")) ;
- }
-
- /* read magic number */
- if (fread (magic, 1, sizeof (magic), file) != sizeof (magic))
- {
- fclose (file);
- vpERROR_TRACE("couldn't read magic number in file \"%s\"\n", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
- }
-
- /* check for valid magic number */
- if (png_sig_cmp (magic,0, sizeof (magic)))
- {
- fprintf (stderr, "error: \"%s\" is not a valid PNG image!\n",filename);
- fclose (file);
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
- }
-
- /* create a png read struct */
- //printf("version %s\n", PNG_LIBPNG_VER_STRING);
- png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
- if (png_ptr == NULL)
- {
- fprintf (stderr, "error: can't create a png read structure!\n");
- fclose (file);
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
- }
-
- /* create a png info struct */
- png_infop info_ptr = png_create_info_struct (png_ptr);
- if (info_ptr == NULL)
- {
- fprintf (stderr, "error: can't create a png info structure!\n");
- fclose (file);
- png_destroy_read_struct (&png_ptr, NULL, NULL);
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
- }
-
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
- vpERROR_TRACE("Error during init io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
- }
-
- /* setup libpng for using standard C fread() function with our FILE pointer */
- png_init_io (png_ptr, file);
-
- /* tell libpng that we have already read the magic number */
- png_set_sig_bytes (png_ptr, sizeof (magic));
-
- /* read png info */
- png_read_info (png_ptr, info_ptr);
-
- unsigned int width = png_get_image_width(png_ptr, info_ptr);
- unsigned int height = png_get_image_height(png_ptr, info_ptr);
-
- unsigned int bit_depth, channels, color_type;
- /* get some useful information from header */
- bit_depth = png_get_bit_depth (png_ptr, info_ptr);
- channels = png_get_channels(png_ptr, info_ptr);
- color_type = png_get_color_type (png_ptr, info_ptr);
-
- /* convert index color images to RGB images */
- if (color_type == PNG_COLOR_TYPE_PALETTE)
- png_set_palette_to_rgb (png_ptr);
-
- /* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
- if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
- png_set_expand (png_ptr);
-
-// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
-// png_set_tRNS_to_alpha (png_ptr);
-
- if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
- png_set_strip_alpha(png_ptr);
-
- if (bit_depth == 16)
- png_set_strip_16 (png_ptr);
- else if (bit_depth < 8)
- png_set_packing (png_ptr);
-
- /* update info structure to apply transformations */
- png_read_update_info (png_ptr, info_ptr);
-
- channels = png_get_channels(png_ptr, info_ptr);
-
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
-
- png_bytep* rowPtrs = new png_bytep[height];
-
- unsigned int stride = width * bit_depth * channels / 8;
- unsigned char* data = new unsigned char[stride * height];
-
- for (unsigned int i =0; i < height; i++)
- rowPtrs[i] = (png_bytep)data + (i * stride);
-
- png_read_image(png_ptr, rowPtrs);
-
- vpImage<vpRGBa> Ic(height,width);
- unsigned char* output;
-
- switch (channels)
- {
- case 1:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i];
- }
- break;
- case 2:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*2];
- }
- break;
- case 3:
-
- output = (unsigned char*)Ic.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*3];
- *(output++) = data[i*3+1];
- *(output++) = data[i*3+2];
- *(output++) = 0;
- }
- vpImageConvert::convert(Ic,I) ;
- break;
- case 4:
- output = (unsigned char*)Ic.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*4];
- *(output++) = data[i*4+1];
- *(output++) = data[i*4+2];
- *(output++) = data[i*4+3];
- }
- vpImageConvert::convert(Ic,I) ;
- break;
- }
-
- delete [] (png_bytep)rowPtrs;
- delete [] data;
- png_read_end (png_ptr, NULL);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
- fclose(file);
-}
-
-
-/*!
- Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readPNG(I, filename.c_str());
-}
-
-
-/*!
- Read a PNG file and initialize a scalar image.
-
- Read the contents of the PNG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const char *filename)
-{
- FILE *file;
- png_byte magic[8];
-
- // Test the filename
- if (!filename || *filename == '\0') {
- vpERROR_TRACE("no filename\n");
- throw (vpImageException(vpImageException::ioError,
- "no filename")) ;
- }
-
- file = fopen(filename, "rb");
-
- if (file == NULL) {
- vpERROR_TRACE("couldn't read file \"%s\"\n", filename);
- throw (vpImageException(vpImageException::ioError,
- "cannot read file")) ;
- }
-
- /* read magic number */
- if (fread (magic, 1, sizeof (magic), file) != sizeof (magic))
- {
- fclose (file);
- vpERROR_TRACE("couldn't read magic number in file \"%s\"\n", filename) ;
- throw (vpImageException(vpImageException::ioError,
- "error reading pgm file")) ;
- }
-
- /* check for valid magic number */
- if (png_sig_cmp (magic,0, sizeof (magic)))
- {
- fclose (file);
- vpERROR_TRACE("error: \"%s\" is not a valid PNG image!\n",filename);
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
- }
-
- /* create a png read struct */
- png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
- if (!png_ptr)
- {
- fclose (file);
- vpERROR_TRACE("Error during png_create_read_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
- }
-
- /* create a png info struct */
- png_infop info_ptr = png_create_info_struct (png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, NULL, NULL);
- vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
- }
-
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
- vpERROR_TRACE("Error during init io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
- }
-
- /* setup libpng for using standard C fread() function with our FILE pointer */
- png_init_io (png_ptr, file);
-
- /* tell libpng that we have already read the magic number */
- png_set_sig_bytes (png_ptr, sizeof (magic));
-
- /* read png info */
- png_read_info (png_ptr, info_ptr);
-
- unsigned int width = png_get_image_width(png_ptr, info_ptr);
- unsigned int height = png_get_image_height(png_ptr, info_ptr);
-
- unsigned int bit_depth, channels, color_type;
- /* get some useful information from header */
- bit_depth = png_get_bit_depth (png_ptr, info_ptr);
- channels = png_get_channels(png_ptr, info_ptr);
- color_type = png_get_color_type (png_ptr, info_ptr);
-
- /* convert index color images to RGB images */
- if (color_type == PNG_COLOR_TYPE_PALETTE)
- png_set_palette_to_rgb (png_ptr);
-
- /* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
- if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
- png_set_expand (png_ptr);
-
-// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
-// png_set_tRNS_to_alpha (png_ptr);
-
- if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
- png_set_strip_alpha(png_ptr);
-
- if (bit_depth == 16)
- png_set_strip_16 (png_ptr);
- else if (bit_depth < 8)
- png_set_packing (png_ptr);
-
- /* update info structure to apply transformations */
- png_read_update_info (png_ptr, info_ptr);
-
- channels = png_get_channels(png_ptr, info_ptr);
-
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
-
- png_bytep* rowPtrs = new png_bytep[height];
-
- unsigned int stride = width * bit_depth * channels / 8;
- unsigned char* data = new unsigned char[stride * height];
-
-
- for (unsigned int i =0; i < height; i++)
- rowPtrs[i] = (png_bytep)data + (i * stride);
-
- png_read_image(png_ptr, rowPtrs);
-
- vpImage<unsigned char> Ig(height,width);
- unsigned char* output;
-
- switch (channels)
- {
- case 1:
- output = (unsigned char*)Ig.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i];
- }
- vpImageConvert::convert(Ig,I) ;
- break;
- case 2:
- output = (unsigned char*)Ig.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*2];
- }
- vpImageConvert::convert(Ig,I) ;
- break;
- case 3:
-
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*3];
- *(output++) = data[i*3+1];
- *(output++) = data[i*3+2];
- *(output++) = 0;
- }
- break;
- case 4:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*4];
- *(output++) = data[i*4+1];
- *(output++) = data[i*4+2];
- *(output++) = data[i*4+3];
- }
- break;
- }
-
- delete [] (png_bytep)rowPtrs;
- delete [] data;
- png_read_end (png_ptr, NULL);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
- fclose(file);
-}
-
-
-/*!
- Read a PNG file and initialize a scalar image.
-
- Read the contents of the PNG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readPNG(I, filename.c_str());
-}
-
-#elif defined(VISP_HAVE_OPENCV)
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<unsigned char> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip;
- vpImageConvert::convert(I, Ip);
- cv::imwrite(filename, Ip);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(I, Ip);
-
- cvSaveImage(filename, Ip);
-
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::writePNG(I, filename.c_str());
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<vpRGBa> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip;
- vpImageConvert::convert(I, Ip);
- cv::imwrite(filename, Ip);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(I, Ip);
-
- cvSaveImage(filename, Ip);
-
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Write the content of the image bitmap in the file which name is given by \e
- filename. This function writes a PNG file.
-
- \param I : Image to save as a PNG file.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::writePNG(I, filename.c_str());
-}
-
-
-/*!
- Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#else
- IplImage* Ip = NULL;
- Ip = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if (Ip != NULL)
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError,
- "Can't read the image")) ;
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- \param I : Image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-
-*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string filename)
-{
- vpImageIo::readPNG(I, filename.c_str());
-}
-
-
-/*!
- Read a PNG file and initialize a scalar image.
-
- Read the contents of the PNG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const char *filename)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- cv::Mat Ip = cv::imread(filename, cv::IMREAD_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat Ip = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
- if ( ! Ip.empty())
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError, "Can't read the image")) ;
-#else
- IplImage* Ip = NULL;
- Ip = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
- if (Ip != NULL)
- vpImageConvert::convert(Ip, I);
- else
- throw (vpImageException(vpImageException::ioError,
- "Can't read the image")) ;
- cvReleaseImage(&Ip);
-#endif
-}
-
-
-/*!
- Read a PNG file and initialize a scalar image.
-
- Read the contents of the PNG file, allocate
- memory for the corresponding image, and set
- the bitmap whith the content of
- the file.
-
- If the image has been already initialized, memory allocation is done
- only if the new image size is different, else we re-use the same
- memory space.
-
- If the file corresponds to a grayscaled image, a conversion is done to deal
- with \e I which is a color image.
-
- \param I : Color image to set with the \e filename content.
- \param filename : Name of the file containing the image.
-*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string filename)
-{
- vpImageIo::readPNG(I, filename.c_str());
-}
-
-#endif
diff --git a/src/image/vpImageIo.h b/src/image/vpImageIo.h
deleted file mode 100644
index f08f6d8..0000000
--- a/src/image/vpImageIo.h
+++ /dev/null
@@ -1,256 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageIo.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read/write images.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpImageIo.h
- \brief Read/write images
-*/
-
-#ifndef vpIMAGEIO_H
-#define vpIMAGEIO_H
-
-#include <visp/vpImage.h>
-#include <visp/vpDebug.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpImageConvert.h>
-
-#include <stdio.h>
-#include <iostream>
-
-#if defined(_WIN32)
-# include <windows.h>
-#endif
-
-#if defined(VISP_HAVE_LIBJPEG)
-#include <jpeglib.h>
-#include <jerror.h>
-#endif
-
-#if defined(VISP_HAVE_LIBPNG)
-#include <png.h>
-#endif
-
-
-/*!
- \class vpImageIo
-
- \ingroup ImageRW
-
- \brief Read/write images with various image format.
-
- The code below shows how to convert an PPM P6 image file format into
- a PGM P5 image file format. The extension of the filename is here
- used in read() and write() functions to set the image file format
- (".pgm" for PGM P5 and ".ppm" for PPM P6).
-
- \code
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
- vpImage<unsigned char> I;
-#if defined(_WIN32)
- std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
-#else // UNIX
- std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
-#endif
-
- vpImageIo::read(I, filename); // Convert the color image in a gray level image
- vpImageIo::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format
-}
- \endcode
-
- This other example available in tutorial-image-reader.cpp shows how to read/write
- jpeg images. It supposes that \c libjpeg is installed.
- \include tutorial-image-reader.cpp
-*/
-
-class VISP_EXPORT vpImageIo
-{
-
-private:
-
- typedef enum
- {
- FORMAT_PGM,
- FORMAT_PPM,
- FORMAT_JPEG,
- FORMAT_PNG,
- // Formats supported by opencv
- FORMAT_TIFF,
- FORMAT_BMP,
- FORMAT_DIB,
- FORMAT_PBM,
- FORMAT_RASTER,
- FORMAT_JPEG2000,
- FORMAT_UNKNOWN
- } vpImageFormatType;
-
- static const int vpMAX_LEN;
-
- static FILE * openFileRead(const char *filename) ;
- static FILE * openFileWrite(const char *filename, const char *mode="w") ;
-
- static FILE * openFileRead(const std::string filename) ;
- static FILE * openFileWrite(const std::string filename,
- const std::string mode="w") ;
-
- static vpImageFormatType getFormat(const char *filename) ;
- static std::string getExtension(const std::string &filename);
-
-public:
-
- static
- void read(vpImage<unsigned char> &I, const char *filename) ;
- static
- void read(vpImage<unsigned char> &I, const std::string filename) ;
- static
- void read(vpImage<vpRGBa> &I, const char *filename) ;
- static
- void read(vpImage<vpRGBa> &I, const std::string filename) ;
-
- static
- void write(const vpImage<unsigned char> &I, const char *filename) ;
- static
- void write(const vpImage<unsigned char> &I, const std::string filename) ;
- static
- void write(const vpImage<vpRGBa> &I, const char *filename) ;
- static
- void write(const vpImage<vpRGBa> &I, const std::string filename) ;
-
- static
- void readPFM(vpImage<float> &I, const char *filename) ;
-
-
- static
- void readPGM(vpImage<unsigned char> &I, const char *filename) ;
- static
- void readPGM(vpImage<unsigned char> &I, const std::string filename) ;
- static
- void readPGM(vpImage<vpRGBa> &I, const char *filename) ;
- static
- void readPGM(vpImage<vpRGBa> &I, const std::string filename) ;
-
- static
- void readPPM(vpImage<unsigned char> &I, const char *filename) ;
- static
- void readPPM(vpImage<unsigned char> &I, const std::string filename) ;
- static
- void readPPM(vpImage<vpRGBa> &I, const char *filename) ;
- static
- void readPPM(vpImage<vpRGBa> &I, const std::string filename) ;
-
-#if (defined(VISP_HAVE_LIBJPEG) || defined(VISP_HAVE_OPENCV))
- static
- void readJPEG(vpImage<unsigned char> &I, const char *filename) ;
- static
- void readJPEG(vpImage<unsigned char> &I, const std::string filename) ;
- static
- void readJPEG(vpImage<vpRGBa> &I, const char *filename) ;
- static
- void readJPEG(vpImage<vpRGBa> &I, const std::string filename) ;
-#endif
-
-#if (defined(VISP_HAVE_LIBPNG) || defined(VISP_HAVE_OPENCV))
- static
- void readPNG(vpImage<unsigned char> &I, const char *filename) ;
- static
- void readPNG(vpImage<unsigned char> &I, const std::string filename) ;
- static
- void readPNG(vpImage<vpRGBa> &I, const char *filename) ;
- static
- void readPNG(vpImage<vpRGBa> &I, const std::string filename) ;
-#endif
-
- static
- void writePFM(const vpImage<float> &I, const char *filename) ;
-
-
-
- static
- void writePGM(const vpImage<unsigned char> &I, const char *filename) ;
- static
- void writePGM(const vpImage<unsigned char> &I, const std::string filename) ;
- static
- void writePGM(const vpImage<short> &I, const char *filename) ;
- static
- void writePGM(const vpImage<short> &I, const std::string filename) ;
- static
- void writePGM(const vpImage<vpRGBa> &I, const char *filename) ;
- static
- void writePGM(const vpImage<vpRGBa> &I, const std::string filename) ;
-
- static
- void writePPM(const vpImage<unsigned char> &I, const char *filename) ;
- static
- void writePPM(const vpImage<unsigned char> &I, const std::string filename) ;
- static
- void writePPM(const vpImage<vpRGBa> &I, const char *filename) ;
- static
- void writePPM(const vpImage<vpRGBa> &I, const std::string filename) ;
-
-#if (defined(VISP_HAVE_LIBJPEG) || defined(VISP_HAVE_OPENCV))
- static
- void writeJPEG(const vpImage<unsigned char> &I, const char *filename) ;
- static
- void writeJPEG(const vpImage<unsigned char> &I, const std::string filename) ;
- static
- void writeJPEG(const vpImage<vpRGBa> &I, const char *filename) ;
- static
- void writeJPEG(const vpImage<vpRGBa> &I, const std::string filename) ;
-#endif
-
-#if (defined(VISP_HAVE_LIBPNG) || defined(VISP_HAVE_OPENCV))
- static
- void writePNG(const vpImage<unsigned char> &I, const char *filename) ;
- static
- void writePNG(const vpImage<unsigned char> &I, const std::string filename) ;
- static
- void writePNG(const vpImage<vpRGBa> &I, const char *filename) ;
- static
- void writePNG(const vpImage<vpRGBa> &I, const std::string filename) ;
-#endif
-
- } ;
-#endif
-
diff --git a/src/image/vpImageMorphology.h b/src/image/vpImageMorphology.h
deleted file mode 100644
index 076e0dc..0000000
--- a/src/image/vpImageMorphology.h
+++ /dev/null
@@ -1,229 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageMorphology.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Morphology tools.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpImageMorphology_H
-#define vpImageMorphology_H
-
-/*!
- \file vpImageMorphology.h
- \brief Various mathematical morphology tools, erosion, dilatation...
-
-*/
-#include <visp/vpImage.h>
-#include <visp/vpImageException.h>
-#include <visp/vpMatrix.h>
-
-#include <fstream>
-#include <iostream>
-#include <math.h>
-#include <string.h>
-
-/*!
- \class vpImageMorphology
-
- \ingroup ImageFiltering
-
- \brief Various mathematical morphology tools, erosion, dilatation...
-
- \author Fabien Spindler (Fabien.Spindler at irisa.fr) Irisa / Inria Rennes
-
-
-*/
-class vpImageMorphology
-{
-public:
- /*! \enum vpConnexityType
- Type of connexity 4, or 8.
- */
- typedef enum {
- CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
- right, up, down) */
- CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
- right, up, down, and the 4 pixels located on the diagonal) */
- } vpConnexityType;
-
-public:
- template<class Type>
- static void erosion(vpImage<Type> &I, Type value, Type value_out,
- vpConnexityType connexity = CONNEXITY_4);
-
- template<class Type>
- static void dilatation(vpImage<Type> &I, Type value, Type value_out,
- vpConnexityType connexity = CONNEXITY_4);
-
-} ;
-
-/*!
-
- Erode a binary image using a structuring element of size one.
-
- \param I : Image to process.
- \param value : Values of the pixels to erode.
- \param value_out : Value to set if erosion is done.
- \param connexity : Type of connexity: 4 or 8.
-
- To erode a black area in an unsigned char image, set \e value to
- 0 and \e value_out to 255.
-
- To erode a white area in an unsigned char image with one element mask, set
- \e value to 255 and \e value_out to 0.
-
- \sa dilatation()
-*/
-template<class Type>
-void vpImageMorphology::erosion(vpImage<Type> &I,
- Type value,
- Type value_out,
- vpConnexityType connexity)
-{
- vpImage<Type> J(I.getHeight(), I.getWidth()) ;
- J = I;
-
- if (connexity == CONNEXITY_4) {
- for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
- for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
- {
- if (I[i][j] == value)
- {
- // Consider 4 neighbors
- if ((I[i-1][j] == value_out) ||
- (I[i+1][j] == value_out) ||
- (I[i][j-1] == value_out) ||
- (I[i][j+1] == value_out))
- J[i][j] = value_out;
- }
- }
- }
- else {
- for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
- for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
- {
- if (I[i][j] == value)
- {
- // Consider 8 neighbors
- if ((I[i-1][j-1] == value_out) ||
- (I[i-1][j] == value_out) ||
- (I[i-1][j+1] == value_out) ||
- (I[i][j-1] == value_out) ||
- (I[i][j+1] == value_out) ||
- (I[i+1][j+1] == value_out) ||
- (I[i+1][j+1] == value_out) ||
- (I[i+1][j+1] == value_out) )
- J[i][j] = value_out ;
- }
- }
-
-
- }
- I = J ;
-}
-
-/*!
-
- Dilate a binary image using a structuring element of size one.
-
- \param I : Image to process.
- \param value : Values of the pixels to dilate.
- \param value_out : Value to set if dilatation is done.
- \param connexity : Type of connexity: 4 or 8.
-
- To dilate a black area in an unsigned char image with one element mask, set
- \e value to 0 and \e value_out to 255.
-
- To dilate a white area in an unsigned char image with one element mask, set
- \e value to 255 and \e value_out to 0.
-
- \sa erosion()
-*/
-template<class Type>
-void vpImageMorphology::dilatation(vpImage<Type> &I,
- Type value,
- Type value_out,
- vpConnexityType connexity)
-{
- vpImage<Type> J(I.getHeight(), I.getWidth()) ;
- J = I;
- if (connexity == CONNEXITY_4) {
- for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
- for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
- {
- if (I[i][j] == value_out)
- {
- // Consider 4 neighbors
- if ((I[i-1][j] == value) ||
- (I[i+1][j] == value) ||
- (I[i][j-1] == value) ||
- (I[i][j+1] == value))
- J[i][j] = value ;
- }
- }
- }
- else {
- for (unsigned int i=1 ; i < I.getHeight()-1 ; i++)
- for (unsigned int j=1 ; j < I.getWidth()-1 ; j++)
- {
- if (I[i][j] == value_out)
- {
- // Consider 8 neighbors
- if ((I[i-1][j-1] == value) ||
- (I[i-1][j] == value) ||
- (I[i-1][j+1] == value) ||
- (I[i][j-1] == value) ||
- (I[i][j+1] == value) ||
- (I[i+1][j+1] == value) ||
- (I[i+1][j+1] == value) ||
- (I[i+1][j+1] == value) )
- J[i][j] = value ;
- }
- }
- }
-
- I = J ;
-}
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/image/vpImagePoint.cpp b/src/image/vpImagePoint.cpp
deleted file mode 100644
index c0a8339..0000000
--- a/src/image/vpImagePoint.cpp
+++ /dev/null
@@ -1,415 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.cpp 5244 2015-02-03 06:44:17Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point useful for image processing
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpRect.h>
-#include <visp/vpHomography.h>
-
-/*!
-
- Check if an image point belongs to a rectangle.
-
- \param rect : the rectangle.
-
- \return Returns true if the point belongs to the rectangle.
-
-*/
-bool vpImagePoint::inRectangle( const vpRect &rect ) const
-{
- return ( this->i <= rect.getBottom() &&
- this->i >= rect.getTop() &&
- this->j <= rect.getRight() &&
- this->j >= rect.getLeft());
-}
-
-/*!
- Project the current image point (in frame b) into the frame a using the
- homography aHb.
-
- \param aHb : Homography defining the relation between frame a and frame b.
- \return The projected image point in the frame a.
-*/
-vpImagePoint
-vpImagePoint::projection(const vpHomography& aHb)
-{
- vpImagePoint ap;
-
- double i_a = aHb[0][0] * i + aHb[0][1] * j + aHb[0][2];
- double j_a = aHb[1][0] * i + aHb[1][1] * j + aHb[1][2];
- double k_a = aHb[2][0] * i + aHb[2][1] * j + aHb[2][2];
-
- if(std::fabs(k_a) > std::numeric_limits<double>::epsilon()){
- ap.set_i(i_a / k_a);
- ap.set_j(j_a / k_a);
- }
-
- return ap;
-}
-
-/*!
-
- Operator +=.
-
- This operator can be used to compute the center of gravity of a set of image points.
-
- \code
-#include <iostream>
-#include <vector>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
-std::vector<vpImagePoint> ip(2);
-
-ip[0].set_ij(100, 200);
-ip[1].set_ij(300, 400);
-
-vpImagePoint cog(0,0);
-for(unsigned int i=0; i<ip.size(); i++)
- cog += ip[i];
-cog /= ip.size();
-std::cout << "cog: " << cog << std::endl;
-}
- \endcode
-
-*/
-vpImagePoint& vpImagePoint::operator+=(const vpImagePoint &ip) {
- this->i += ip.i;
- this->j += ip.j;
- return *this;
-}
-
-/*!
-
- Operator /=.
-
- This operator can be used to compute the center of gravity of a set of image points.
- \code
-#include <iostream>
-#include <vector>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
-std::vector<vpImagePoint> ip(2);
-
-ip[0].set_ij(100, 200);
-ip[1].set_ij(300, 400);
-
-vpImagePoint cog(0,0);
-for(unsigned int i=0; i<ip.size(); i++)
- cog += ip[i];
-cog /= ip.size();
-std::cout << "cog: " << cog << std::endl;
-}
- \endcode
-
-*/
-vpImagePoint& vpImagePoint::operator/=(const double scale) {
- this->i /= scale;
- this->j /= scale;
- return *this;
-}
-
-/*!
- \relates vpImagePoint
-
- Returns true if ip1 and ip2 are equal; otherwire returns false.
-
-*/
-VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- //return ( ( ip1.get_i() == ip2.get_i() ) && ( ip1.get_j() == ip2.get_j() ) );
-
- double i1 = ip1.get_i();
- double j1 = ip1.get_j();
- double i2 = ip2.get_i();
- double j2 = ip2.get_j();
-
- return (
- ( std::fabs(i1-i2) <= std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(j1-j2) <= std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
- );
-}
-
-/*!
-
- \relates vpImagePoint
-
- Returns true if ip1 and ip2 are different; otherwire returns true.
-
-*/
-VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- //return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() ) );
- double i1 = ip1.get_i();
- double j1 = ip1.get_j();
- double i2 = ip2.get_i();
- double j2 = ip2.get_j();
-
- return (
- ( std::fabs(i1-i2) > std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(j1-j2) > std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
- );
-}
-
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint wich is the sum of \f$ ip1 \f$ and \f$ ip2 \f$.
-
-*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint wich is the sum of \f$ ip1 \f$ and \f$ ip2 \f$.
-
-*/
-VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with an offset added to the two coordinates.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip+10: " << ip+10 << std::endl; // new coordinates (110, 210)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset ) {
- return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with an offset added to the two coordinates.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip+12.34: " << ip+12.34 << std::endl; // new coordinates (112.34, 212.34)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const double offset ) {
- return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
-}
-
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint wich is the difference between \f$ ip1 \f$ and \f$ ip2 \f$.
-
-*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- return ( vpImagePoint(ip1.get_i()-ip2.get_i(), ip1.get_j()-ip2.get_j()));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with an offset substracted to the two coordinates.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip-10: " << ip-10 << std::endl; // new coordinates (90, 190)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const int offset ) {
- return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with an offset substracted to the two coordinates.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip-12.34: " << ip-12.34 << std::endl; // new coordinates (87.66, 187.66)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const double offset ) {
- return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with coordinates multiplied by a scale factor.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip*2: " << ip*2 << std::endl; // new coordinates (200, 400)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator*( const vpImagePoint &ip1, const double scale ) {
- return ( vpImagePoint(ip1.get_i()*scale, ip1.get_j()*scale));
-}
-/*!
-
- \relates vpImagePoint
-
- Returns a vpImagePoint with coordinates divided by a scale factor.
-
- \code
-#include <iostream>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImagePoint ip(100, 200); // Create an image point with coordinates i=100, j=200
- std::cout << "ip: " << ip << std::endl; // coordinates (100, 200)
- std::cout << "ip/2: " << ip/2 << std::endl; // new coordinates (50, 100)
-
- return 0;
-}
- \endcode
-*/
-VISP_EXPORT vpImagePoint operator/( const vpImagePoint &ip1, const double scale ) {
- return ( vpImagePoint(ip1.get_i()/scale, ip1.get_j()/scale));
-}
-
-/*!
-
- \relates vpImagePoint
-
- Writes the image point coordinates \e ip to the stream \e os, and
- returns a reference to the stream. Writes the first coordinate along
- the \e i axis and then the second one along the \e j axis. The
- coordinates are separated by a comma.
-
- The following code
- \code
-#include <iostream>
-
-#include <visp/vpImagePoint.h>
-int main()
-{
- vpImagePoint ip;
-
- ip.set_i(10);
- ip.set_j(11.1);
-
- std::cout << "Image point with coordinates: " << ip << std::endl;
-
- return 0;
-}
- \endcode
-
- The previous sample code produces the output:
- \verbatim
-Image point with coordinates: 10, 11.1
- \endverbatim
-*/
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip)
-{
- os << ip.get_i() << ", " << ip.get_j();
- return os;
-}
-
-/**
- * Computes and returns the bounding box.
- * @param ipVec : Vector of input image points.
- * @return Bounding box of the points.
- */
-vpRect vpImagePoint::getBBox(const std::vector<vpImagePoint>& ipVec)
-{
- vpRect rec(ipVec);
-
- return rec;
-}
diff --git a/src/image/vpImagePoint.h b/src/image/vpImagePoint.h
deleted file mode 100644
index 9fae9de..0000000
--- a/src/image/vpImagePoint.h
+++ /dev/null
@@ -1,315 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 5175 2015-01-15 17:06:07Z nmeriaux $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point useful for image processing
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpImagePoint_H
-#define vpImagePoint_H
-
-/*!
- \file vpImagePoint.h
- \brief Class that defines a 2D point in an image. This class is useful
- for image processing
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpMath.h>
-
-#include <ostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <vector>
-
-class vpHomography;
-class vpRect;
-
-/*!
- \class vpImagePoint
- \ingroup ImageTool
-
- \brief Class that defines a 2D point in an image. This class is
- useful for image processing and stores only the <B>2D coordinates
- given in sub-pixel</B>.
-
- \warning If you want to define a point thanks to its coordinates
- given in meter in the object frame, the camera frame or the image
- plane, you have to use the class vpPoint.
-
- In this class, the 2D coordinates are not necessary integer
- values. It is easy to manipulate the given coordinates in the two
- frames used in ViSP : the (i,j) coordinates and the (u,v)
- coordinates. The two following images illustrate the two coordinate
- systems.
-
- \image html vpImagePoint.gif
- \image latex vpImagePoint.ps width=10cm
-
- \warning <B>An instance of the vpImagePoint class corresponds to a
- particular point. Thus, if you change the point coordinate using the
- method set_i(const double i), it produces the same effect than if
- you used the method set_v(const double v). These two methods change
- the same private attribute. It is also true for the two methods
- set_j(const double j) and set_u(const double u).</B>
-*/
-
-
-class VISP_EXPORT vpImagePoint
-{
- public:
- /*!
- Default constructor that initialize the coordinates of the image
- point to zero.
- */
- inline vpImagePoint() : i(0), j(0) {}
- /*!
- Default constructor that initialize the coordinates of the image
- thanks to the parameters \f$ ii \f$ and \f$ jj \f$.
- */
- inline vpImagePoint(double ii, double jj) : i(ii), j(jj) {}
- /*!
- Copy constructor.
-
- Initialize the coordinates of the image point with \e ip.
-
- \param ip : An image point.
- */
- inline vpImagePoint(const vpImagePoint &ip) : i(ip.i), j(ip.j) {}
- //! Destructor.
- inline virtual ~vpImagePoint() { ; }
-
- /*!
-
- Copy operator.
-
- */
- inline const vpImagePoint& operator=(const vpImagePoint &ip) {
- this->i = ip.i;
- this->j = ip.j;
- return *this;
- }
- vpImagePoint& operator+=(const vpImagePoint &ip);
-
- /*!
-
- Operator -=.
-
- */
- inline vpImagePoint& operator-=(const vpImagePoint &ip) {
- this->i -= ip.i;
- this->j -= ip.j;
- return *this;
- }
- vpImagePoint& operator/=(const double scale);
- /*!
-
- Operator *=.
-*/
- inline vpImagePoint& operator*=(const double scale) {
- this->i *= scale;
- this->j *= scale;
- return *this;
- }
-
- /*!
-
- Sets the point coordinate corresponding to the \f$ i \f$ axes in
- the frame (i,j).
-
- \param ii : The desired value for the coordinate along the \f$ i \f$ axes.
-
- \sa set_j(), set_u(), set_v()
- */
- inline void set_i(const double ii) { this->i = ii ; }
-
- /*!
-
- Sets the point coordinate corresponding to the \f$ j \f$ axes in
- the frame (i,j).
-
- \param jj : The desired value for the coordinate along the \f$ j \f$ axes.
-
- \sa set_i(), set_u(), set_v()
- */
- inline void set_j(const double jj) { this->j = jj ; }
-
- /*!
-
- Sets the point coordinates in the frame (i,j).
-
- \param ii : The desired value for the coordinate along the \f$ i \f$ axes.
- \param jj : The desired value for the coordinate along the \f$ j \f$ axes.
-
- \sa set_i(), set_j(), set_u(), set_v()
- */
- inline void set_ij(const double ii, const double jj) {
- this->i = ii ;
- this->j = jj ;
- }
-
- /*!
-
- Gets the point coordinate corresponding to the \f$ i \f$ axes in
- the frame (i,j).
-
- \return The value of the coordinate along the \f$ i \f$ axes.
-
- \sa get_j(), get_u(), get_v()
- */
- inline double get_i() const { return i ; }
-
- /*!
-
- Gets the point coordinate corresponding to the \f$ j \f$ axes in
- the frame (i,j).
-
- \return The value of the coordinate along the \f$ j \f$ axes.
-
- \sa get_i(), get_u(), get_v()
- */
- inline double get_j() const { return j ; }
-
- /*!
-
- Sets the point coordinate corresponding to the \f$ u \f$ axes in
- the frame (u,v).
-
- \param u : The desired value for the coordinate along the \f$ u \f$ axes.
-
- \sa set_i(), set_j(), set_v()
- */
- inline void set_u(const double u) { j = u ; }
-
- /*!
-
- Sets the point coordinate corresponding to the \f$ v \f$ axes in
- the frame (u,v).
-
- \param v : The desired value for the coordinate along the \f$ v \f$ axes.
-
- \sa set_i(), set_j(), set_u()
- */
- inline void set_v(const double v) { i = v ; }
-
- /*!
-
- Sets the point coordinates in the frame (u,v).
-
- \param u : The desired value for the coordinate along the \f$ u \f$ axes.
- \param v : The desired value for the coordinate along the \f$ v \f$ axes.
-
- \sa set_i(), set_j(), set_u(), set_v()
- */
- inline void set_uv(const double u, const double v) {
- this->i = v ;
- this->j = u ;
- }
-
- /*!
-
- Gets the point coordinate corresponding to the \f$ u \f$ axes in
- the frame (u,v).
-
- \return The value of the coordinate along the \f$ u \f$ axes.
-
- \sa get_i(), get_j(), get_v()
- */
- inline double get_u() const { return j ; }
-
- /*!
-
- Gets the point coordinate corresponding to the \f$ v \f$ axes in
- the frame (u,v).
-
- \return The value of the coordinate along the \f$ v \f$ axes.
-
- \sa get_i(), get_j(), get_u()
- */
- inline double get_v() const { return i; }
-
- /*!
-
- Compute the distance \f$ |iP1 - iP2| = \sqrt{(i_1-i_2)^2+(j_1-j_2)^2} \f$
-
- \param iP1 : First point
- \param iP2 : Second point
-
- \return the distance between the two points.
- */
- static double distance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
- return(sqrt(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j())));}
-
-
- static vpRect getBBox(const std::vector<vpImagePoint>& ipVec);
-
- /*!
-
- Compute the distance \f$ |iP1 - iP2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
-
- \param iP1 : First point
- \param iP2 : Second point
-
- \return the distance between the two points.
- */
- static double sqrDistance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
- return(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j()));}
-
-
- bool inRectangle( const vpRect &rect ) const;
-
- friend VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const double offset );
- friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const int offset );
- friend VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const double offset );
- friend VISP_EXPORT vpImagePoint operator*( const vpImagePoint &ip1, const double scale );
- friend VISP_EXPORT vpImagePoint operator/( const vpImagePoint &ip1, const double scale );
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip);
-
- vpImagePoint projection(const vpHomography& aHb);
-
- private:
- double i,j;
-};
-
-#endif
diff --git a/src/image/vpImageTools.cpp b/src/image/vpImageTools.cpp
deleted file mode 100644
index 091bef3..0000000
--- a/src/image/vpImageTools.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageTools.cpp 5236 2015-01-30 13:51:42Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image tools.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpImageTools.h>
-
-
-/*!
-
- Change the look up table (LUT) of an image. Considering pixel gray
- level values \f$ l \f$ in the range \f$[A, B]\f$, this method allows
- to rescale these values in \f$[A^*, B^*]\f$ by linear interpolation:
-
- \f$
- \left\{ \begin{array}{ll}
- l \in ]-\infty, A] \mbox{, } & l = A^* \\
- l \in [B, \infty[ \mbox{, } & l = B^* \\
- l \in ]A, B[ \mbox{, } & l = A^* + (l-A) * \frac{B^*-A^*}{B-A}
- \end{array}
- \right.
- \f$
-
- \param I : Image to process.
- \param A : Low gray level value of the range to consider.
- \param A_star : New gray level value \f$ A^*\f$ to attribute to pixel
- who's value was A
- \param B : Height gray level value of the range to consider.
- \param B_star : New gray level value \f$ B^*\f$ to attribute to pixel
- who's value was B
- \return The modified image.
-
- \exception vpImageException::incorrectInitializationError If \f$B \leq A\f$.
-
- As shown in the example below, this method can be used to binarize
- an image. For an unsigned char image (in the range 0-255),
- thresholding this image at level 127 can be done by:
-
- \code
-#include <visp/vpImageTools.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
- vpImage<unsigned char> I;
-#ifdef _WIN32
- std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
-#else
- std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
-#endif
-
- // Read an image from the disk
- vpImageIo::read(I, filename);
-
- // Binarize image I:
- // - gray level values less than or equal to 127 are set to 0,
- // - gray level values greater than 128 are set to 255
- vpImageTools::changeLUT(I, 127, 0, 128, 255);
-
- vpImageIo::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format
-}
- \endcode
-
-*/
-void vpImageTools::changeLUT(vpImage<unsigned char>& I,
- unsigned char A,
- unsigned char A_star,
- unsigned char B,
- unsigned char B_star)
-{
- // Test if input values are valid
- if (B <= A) {
- vpERROR_TRACE("Bad gray levels") ;
- throw (vpImageException(vpImageException::incorrectInitializationError ,
- "Bad gray levels")) ;
- }
- unsigned char v;
-
- double factor = (double)(B_star - A_star)/(double)(B - A);
-
- for (unsigned int i=0 ; i < I.getHeight(); i++)
- for (unsigned int j=0 ; j < I.getWidth(); j++) {
- v = I[i][j];
-
- if (v <= A)
- I[i][j] = A_star;
- else if (v >= B)
- I[i][j] = B_star;
- else
- I[i][j] = (unsigned char)(A_star + factor*(v-A));
- }
-}
-
-/*!
- Compute the signed difference between the two images I1 and I2 for
- visualization issue : Idiff = I1-I2
-
- - pixels with a null difference are set to 128.
- - A negative difference implies a pixel value < 128
- - A positive difference implies a pixel value > 128
-
- \param I1 : The first image.
- \param I2 : The second image.
- \param Idiff : The result of the difference.
-*/
-void vpImageTools::imageDifference(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Idiff)
-{
- if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
- {
- throw (vpException(vpException::dimensionError, "The two images have not the same size"));
- }
-
- if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
- Idiff.resize(I1.getHeight(), I1.getWidth());
-
- unsigned int n = I1.getHeight() * I1.getWidth() ;
- int diff ;
- for (unsigned int b = 0; b < n ; b++)
- {
- diff = I1.bitmap[b] - I2.bitmap[b] + 128;
- Idiff.bitmap[b] = (unsigned char)
- (vpMath::maximum(vpMath::minimum(diff, 255), 0));
- }
-}
-
-/*!
- Compute the difference between the two images I1 and I2
- \warning : This is NOT for visualization
- If you want to visualize difference images during servo, please use
- vpImageTools::imageDifference(..,..,..) function.
-
- \param I1 : The first image.
- \param I2 : The second image.
- \param Idiff : The result of the difference.
-*/
-
-void
-vpImageTools::imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Idiff)
-{
- if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
- {
- throw (vpException(vpException::dimensionError, "The two images do not have the same size"));
- }
-
- if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
- Idiff.resize(I1.getHeight(), I1.getWidth());
-
- unsigned int n = I1.getHeight() * I1.getWidth() ;
- int diff ;
- for (unsigned int b = 0; b < n ; b++)
- {
- diff = I1.bitmap[b] - I2.bitmap[b];
- Idiff.bitmap[b] = diff;
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/image/vpImageTools.h b/src/image/vpImageTools.h
deleted file mode 100644
index e972357..0000000
--- a/src/image/vpImageTools.h
+++ /dev/null
@@ -1,611 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImageTools.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Image tools.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpImageTools_H
-#define vpImageTools_H
-
-/*!
- \file vpImageTools.h
-
- \brief Various image tools; sub-image extraction, modification of
- the look up table, binarisation...
-
-*/
-
-#include <visp/vpImage.h>
-
-#ifdef VISP_HAVE_PTHREAD
-# include <pthread.h>
-#endif
-
-#include <visp/vpImageException.h>
-#include <visp/vpMath.h>
-#include <visp/vpRect.h>
-#include <visp/vpCameraParameters.h>
-
-#include <fstream>
-#include <iostream>
-#include <math.h>
-#include <string.h>
-
-/*!
- \class vpImageTools
-
- \ingroup ImageTool
-
- \brief Various image tools; sub-image extraction, modification of
- the look up table, binarisation...
-
-*/
-class VISP_EXPORT vpImageTools
-{
-
-public:
- template<class Type>
- static void createSubImage(const vpImage<Type> &I,
- unsigned int i_sub, unsigned int j_sub,
- unsigned int nrow_sub, unsigned int ncol_sub,
- vpImage<Type> &S);
-
- template<class Type>
- static void createSubImage(const vpImage<Type> &I,
- const vpRect &rect,
- vpImage<Type> &S);
- template<class Type>
- static void binarise(vpImage<Type> &I,
- Type threshold1, Type threshold2,
- Type value1, Type value2, Type value3);
- static void changeLUT(vpImage<unsigned char>& I,
- unsigned char A,
- unsigned char newA,
- unsigned char B,
- unsigned char newB);
- template<class Type>
- static void undistort(const vpImage<Type> &I,
- const vpCameraParameters &cam,
- vpImage<Type> &newI);
-
- template<class Type>
- static void flip(const vpImage<Type> &I,
- vpImage<Type> &newI);
-
- template<class Type>
- static void flip(vpImage<Type> &I);
-
-
- static void imageDifference(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Idiff) ;
-
- static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Idiff);
-} ;
-
-/*!
- Extract a sub part of an image
-
- \param I : Input image from which a sub image will be extracted.
- \param i_sub, j_sub : coordinates of the upper left point of the sub image
- \param nrow_sub, ncol_sub : number of row, column of the sub image
- \param S : Sub-image.
-*/
-template<class Type>
-void vpImageTools::createSubImage(const vpImage<Type> &I,
- unsigned int i_sub, unsigned int j_sub,
- unsigned int nrow_sub, unsigned int ncol_sub,
- vpImage<Type> &S)
-{
- unsigned int i,j ;
- unsigned int imax = i_sub + nrow_sub ;
- unsigned int jmax = j_sub + ncol_sub ;
-
- if (imax > I.getHeight())
- {
- imax = I.getHeight() -1 ;
- nrow_sub = imax-i_sub ;
- }
- if (jmax > I.getWidth())
- {
- jmax = I.getWidth() -1 ;
- ncol_sub = jmax -j_sub ;
- }
-
- S.resize(nrow_sub, ncol_sub) ;
- for (i=i_sub ; i < imax ; i++)
- for (j=j_sub ; j < jmax ; j++)
- {
- S[i-i_sub][j-j_sub] = I[i][j] ;
- }
-}
-/*!
- Extract a sub part of an image
-
- \param I : Input image from which a sub image will be extracted.
-
- \param rect : Rectangle area in the image \e I corresponding to the
- sub part of the image to extract.
-
- \param S : Sub-image.
-*/
-template<class Type>
-void vpImageTools::createSubImage(const vpImage<Type> &I,
- const vpRect &rect,
- vpImage<Type> &S)
-{
- double dleft = rect.getLeft();
- double dtop = rect.getTop();
- double dright = ceil( rect.getRight() );
- double dbottom = ceil( rect.getBottom() );
-
- if (dleft < 0.0) dleft = 0.0;
- else if (dleft >= I.getWidth()) dleft = I.getWidth() - 1;
-
- if (dright < 0.0) dright = 0.0;
- else if (dright >= I.getWidth()) dright = I.getWidth() - 1;
-
- if (dtop < 0.0) dtop = 0.0;
- else if (dtop >= I.getHeight()) dtop = I.getHeight() - 1;
-
- if (dbottom < 0.0) dbottom = 0.0;
- else if (dbottom >= I.getHeight()) dbottom = I.getHeight() - 1;
-
- // Convert the double-precision rectangle coordinates into integer positions
- unsigned int left = (unsigned int) dleft;
- unsigned int top = (unsigned int) dtop;
- unsigned int bottom = (unsigned int) dbottom;
- unsigned int right = (unsigned int) dright;
-
- unsigned int width = right - left + 1;;
- unsigned int height = bottom - top + 1;
-
- S.resize(height, width) ;
- for (unsigned int i=top ; i <= bottom ; i++) {
- for (unsigned int j=left ; j <= right ; j++) {
- S[i-top][j-left] = I[i][j] ;
- }
- }
-}
-/*!
-
- Binarise an image.
-
- - Pixels whose values are less than \e threshold1 are set to \e value1
-
- - Pixels whose values are greater then or equal to \e threshold1 and
- less then or equal to \e threshold2 are set to \e value2
-
- - Pixels whose values are greater than \e threshold2 are set to \e value3
-
-*/
-template<class Type>
-void vpImageTools::binarise(vpImage<Type> &I,
- Type threshold1, Type threshold2,
- Type value1, Type value2, Type value3)
-{
- unsigned char v;
- unsigned char *p = I.bitmap;
- unsigned char *pend = I.bitmap + I.getWidth()*I.getHeight();
- for (; p < pend; p ++) {
- v = *p;
- if (v < threshold1) *p = value1;
- else if (v > threshold2) *p = value3;
- else *p = value2;
- }
-
-}
-
-#ifdef VISP_HAVE_PTHREAD
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-template<class Type>
-class vpUndistortInternalType
-{
-public:
- Type *src;
- Type *dst;
- unsigned int width;
- unsigned int height;
- vpCameraParameters cam;
- unsigned int nthreads;
- unsigned int threadid;
-public:
- vpUndistortInternalType() {
- src = dst = NULL;
- width = height = 0;
- nthreads = threadid = 0;
- };
- vpUndistortInternalType(const vpUndistortInternalType<Type> &u) {
- src = u.src;
- dst = u.dst;
- width = u.width;
- height = u.height;
- cam = u.cam;
- nthreads = u.nthreads;
- threadid = u.threadid;
- };
-
- static void *vpUndistort_threaded(void *arg);
-};
-
-
-template<class Type>
-void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
-{
- vpUndistortInternalType<Type> *undistortSharedData = (vpUndistortInternalType<Type>*)arg;
- int offset = (int)undistortSharedData->threadid;
- int width = (int)undistortSharedData->width;
- int height = (int)undistortSharedData->height;
- int nthreads = (int)undistortSharedData->nthreads;
-
- double u0 = undistortSharedData->cam.get_u0();
- double v0 = undistortSharedData->cam.get_v0();
- double px = undistortSharedData->cam.get_px();
- double py = undistortSharedData->cam.get_py();
- double kud = undistortSharedData->cam.get_kud();
-
- double invpx = 1.0/px;
- double invpy = 1.0/py;
-
- double kud_px2 = kud * invpx * invpx;
- double kud_py2 = kud * invpy * invpy;
-
- Type *dst = undistortSharedData->dst+(height/nthreads*offset)*width;
- Type *src = undistortSharedData->src;
-
- for (double v = height/nthreads*offset;v < height/nthreads*(offset+1) ; v++) {
- double deltav = v - v0;
- //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
- double fr1 = 1.0 + kud_py2 * deltav * deltav;
-
- for (double u = 0 ; u < width ; u++) {
- //computation of u,v : corresponding pixel coordinates in I.
- double deltau = u - u0;
- //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
- double fr2 = fr1 + kud_px2 * deltau * deltau;
-
- double u_double = deltau * fr2 + u0;
- double v_double = deltav * fr2 + v0;
-
- //computation of the bilinear interpolation
-
- //declarations
- int u_round = (int) (u_double);
- int v_round = (int) (v_double);
- if (u_round < 0.f) u_round = -1;
- if (v_round < 0.f) v_round = -1;
- double du_double = (u_double) - (double) u_round;
- double dv_double = (v_double) - (double) v_round;
- Type v01;
- Type v23;
- if ( (0 <= u_round) && (0 <= v_round) &&
- (u_round < ((width) - 1)) && (v_round < ((height) - 1)) ) {
- //process interpolation
- const Type* _mp = &src[v_round*width+u_round];
- v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
- _mp += width;
- v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
- *dst = (Type)(v01 + ((v23 - v01) * dv_double));
- }
- else {
- *dst = 0;
- }
- dst++;
- }
- }
-
- pthread_exit((void*) 0);
- return NULL;
-}
-#endif // DOXYGEN_SHOULD_SKIP_THIS
-#endif // VISP_HAVE_PTHREAD
-
-/*!
- Undistort an image
-
- \param I : Input image to undistort.
-
- \param cam : Parameters of the camera causing distortion.
-
- \param undistI : Undistorted output image. The size of this image
- will be the same than the input image \e I. If the distortion
- parameter \f$K_d\f$ is null (see cam.get_kd_mp()), \e undistI is
- just a copy of \e I.
-
- \warning This function works only with Types authorizing "+,-,
- multiplication by a scalar" operators.
-
- \warning This function is time consuming :
- - On "Rhea"(Intel Core 2 Extreme X6800 2.93GHz, 2Go RAM)
- or "Charon"(Intel Xeon 3 GHz, 2Go RAM) : ~8 ms for a 640x480 image.
-*/
-template<class Type>
-void vpImageTools::undistort(const vpImage<Type> &I,
- const vpCameraParameters &cam,
- vpImage<Type> &undistI)
-{
-#ifdef VISP_HAVE_PTHREAD
- //
- // Optimized version using pthreads
- //
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- undistI.resize(height, width);
-
- double kud = cam.get_kud();
-
- //if (kud == 0) {
- if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
- // There is no need to undistort the image
- undistI = I;
- return;
- }
-
- unsigned int nthreads = 2;
- pthread_attr_t attr;
- pthread_t *callThd = new pthread_t [nthreads];
- pthread_attr_init(&attr);
- pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
- vpUndistortInternalType<Type> *undistortSharedData;
- undistortSharedData = new vpUndistortInternalType<Type> [nthreads];
-
- for(unsigned int i=0;i<nthreads;i++) {
- // Each thread works on a different set of data.
- // vpTRACE("create thread %d", i);
- undistortSharedData[i].src = I.bitmap;
- undistortSharedData[i].dst = undistI.bitmap;
- undistortSharedData[i].width = I.getWidth();
- undistortSharedData[i].height = I.getHeight();
- undistortSharedData[i].cam = cam;
- undistortSharedData[i].nthreads = nthreads;
- undistortSharedData[i].threadid = i;
- pthread_create( &callThd[i], &attr,
- &vpUndistortInternalType<Type>::vpUndistort_threaded,
- &undistortSharedData[i]);
- }
- pthread_attr_destroy(&attr);
- /* Wait on the other threads */
-
- for(unsigned int i=0;i<nthreads;i++) {
- // vpTRACE("join thread %d", i);
- pthread_join( callThd[i], NULL);
- }
-
- delete [] callThd;
- delete [] undistortSharedData;
-#else // VISP_HAVE_PTHREAD
- //
- // optimized version without pthreads
- //
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- undistI.resize(height, width);
-
- double u0 = cam.get_u0();
- double v0 = cam.get_v0();
- double px = cam.get_px();
- double py = cam.get_py();
- double kud = cam.get_kud();
-
- //if (kud == 0) {
- if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
- // There is no need to undistort the image
- undistI = I;
- return;
- }
-
- double invpx = 1.0/px;
- double invpy = 1.0/py;
-
- double kud_px2 = kud * invpx * invpx;
- double kud_py2 = kud * invpy * invpy;
-
- Type *dst = undistI.bitmap;
- for (double v = 0;v < height ; v++) {
- double deltav = v - v0;
- //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
- double fr1 = 1.0 + kud_py2 * deltav * deltav;
-
- for (double u = 0 ; u < width ; u++) {
- //computation of u,v : corresponding pixel coordinates in I.
- double deltau = u - u0;
- //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
- double fr2 = fr1 + kud_px2 * deltau * deltau;
-
- double u_double = deltau * fr2 + u0;
- double v_double = deltav * fr2 + v0;
-
- //printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
-
- //computation of the bilinear interpolation
-
- //declarations
- int u_round = (int) (u_double);
- int v_round = (int) (v_double);
- if (u_round < 0.f) u_round = -1;
- if (v_round < 0.f) v_round = -1;
- double du_double = (u_double) - (double) u_round;
- double dv_double = (v_double) - (double) v_round;
- Type v01;
- Type v23;
- if ( (0 <= u_round) && (0 <= v_round) &&
- (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1)) ) {
- //process interpolation
- const Type* _mp = &I[(unsigned int)v_round][(unsigned int)u_round];
- v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
- _mp += width;
- v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
- *dst = (Type)(v01 + ((v23 - v01) * dv_double));
- //printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
- }
- else {
- *dst = 0;
- }
- dst++;
- }
- }
-#endif // VISP_HAVE_PTHREAD
-
-
-
-
-#if 0
- // non optimized version
- int width = I.getWidth();
- int height = I.getHeight();
-
- undistI.resize(height,width);
-
- double u0 = cam.get_u0();
- double v0 = cam.get_v0();
- double px = cam.get_px();
- double py = cam.get_py();
- double kd = cam.get_kud();
-
- if (kd == 0) {
- // There is no need to undistort the image
- undistI = I;
- return;
- }
-
- for(int v = 0 ; v < height; v++){
- for(int u = 0; u < height; u++){
- double r2 = vpMath::sqr(((double)u - u0)/px) +
- vpMath::sqr(((double)v-v0)/py);
- double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
- double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
- undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double);
- }
- }
-#endif
-}
-
-/*!
- Flip vertically the input image and give the result in the output image.
-
- \param I : Input image to flip.
- \param newI : Output image which is the flipped input image.
-*/
-
-template<class Type>
-void vpImageTools::flip(const vpImage<Type> &I,
- vpImage<Type> &newI)
-{
- unsigned int height = 0, width = 0;
- int i = 0;
-
- height = I.getHeight();
- width = I.getWidth();
- newI.resize(height, width);
-
- for ( i = 0; i < height; i++)
- {
- memcpy(newI.bitmap+i*width, I.bitmap+(height-1-i)*width,
- width*sizeof(Type));
- }
-}
-
-
-/*!
- Flip vertically the input image.
-
- \param I : Input image which is flipped and modified in output.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpImageTools.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
- vpImage<vpRGBa> I;
-#ifdef _WIN32
- std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm");
-#else
- std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm");
-#endif
-
- // Read an image from the disk
- vpImageIo::read(I, filename);
-
- // Flip the image
- vpImageTools::flip(I);
-
- vpImageIo::write(I, "Klimt-flip.ppm"); // Write the image in a PGM P5 image file format
-}
- \endcode
-*/
-template<class Type>
-void vpImageTools::flip(vpImage<Type> &I)
-{
- unsigned int height = 0, width = 0;
- unsigned int i = 0;
- vpImage<Type> Ibuf;
-
- height = I.getHeight();
- width = I.getWidth();
- Ibuf.resize(1, width);
-
- for ( i = 0; i < height/2; i++)
- {
- memcpy(Ibuf.bitmap, I.bitmap+i*width,
- width*sizeof(Type));
-
- memcpy(I.bitmap+i*width, I.bitmap+(height-1-i)*width,
- width*sizeof(Type));
- memcpy(I.bitmap+(height-1-i)*width, Ibuf.bitmap,
- width*sizeof(Type));
- }
-}
-
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/image/vpRGBa.cpp b/src/image/vpRGBa.cpp
deleted file mode 100644
index 297b320..0000000
--- a/src/image/vpRGBa.cpp
+++ /dev/null
@@ -1,268 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRGBa.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * RGBA pixel.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpRGBa.cpp
- \brief Define the object vpRGBa that is used to build color
- images (it define a RGB 32 bits structure, fourth byte is not used - yet -)
-*/
-
-
-#include <visp/vpRGBa.h>
-#include <visp/vpColor.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-
-
-/*!
- Copy operator (from an unsigned char value)
-
- \param v : Input color ( R = G = B = v )
-*/
-vpRGBa &
-vpRGBa::operator=(const unsigned char &v)
-{
- this->R = v;
- this->G = v;
- this->B = v;
- this->A = v;
- return *this;
-}
-
-/*!
- Copy operator.
-*/
-vpRGBa &
-vpRGBa::operator=(const vpRGBa &v)
-{
- this->R = v.R;
- this->G = v.G;
- this->B = v.B;
- this->A = v.A;
- return *this;
-}
-
-/*!
- Cast a vpColVector in a vpRGBa
-
- \param v : Input vector. v[0], v[1], v[2], v[3] are to make into
- relation with respectively R, G, B and A.
-
- \exception vpException::dimensionError : If v is not a 4 four
- dimention vector.
-*/
-vpRGBa &
-vpRGBa::operator=(const vpColVector &v)
-{
- if (v.getRows() != 4) {
- vpERROR_TRACE("Bad vector dimension ") ;
- throw(vpException(vpException::dimensionError, "Bad vector dimension "));
- }
- R = (unsigned char)v[0];
- G = (unsigned char)v[1];
- B = (unsigned char)v[2];
- A = (unsigned char)v[3];
- return *this;
-}
-
-/*!
- Compare two RGBa values.
-
- \return true if the values are the same, false otherwise.
-*/
-bool vpRGBa::operator==(const vpRGBa &v)
-{
- if (R != v.R)
- return false;
- if (G != v.G)
- return false;
- if (B != v.B)
- return false;
- if (A != v.A)
- return false;
-
- return true ;
-}
-/*!
- Compare two color pixels.
-
- \return true if the images are different, false if they are the same.
-*/
-bool vpRGBa::operator!=(const vpRGBa &v)
-{
- if (R == v.R)
- return false;
- if (G == v.G)
- return false;
- if (B == v.B)
- return false;
- if (A == v.A)
- return false;
-
- return true ;
-}
-
-/*!
- Substraction operator : "this" - v.
- \param v : Color to substract to the current object "this".
- \return "this" - v
-*/
-vpColVector
-vpRGBa::operator-(const vpRGBa &v) const
-{
- vpColVector n(4); // new color
- n[0] = (double)R - (double)v.R;
- n[1] = (double)G - (double)v.G;
- n[2] = (double)B - (double)v.B;
- n[3] = (double)A - (double)v.A;
- return n;
-}
-
-/*!
- Addition operator : "this" + v.
- \param v : Color to add to the current object "this".
- \return "this" + v
- \warning in case of overflow : e.g. 128+128 returns 0 for all 4 channels
-*/
-vpRGBa
-vpRGBa::operator+(const vpRGBa &v) const
-{
- vpRGBa n; // new color
- n.R = static_cast<unsigned char>( R + v.R );
- n.G = static_cast<unsigned char>( G + v.G );
- n.B = static_cast<unsigned char>( B + v.B );
- n.A = static_cast<unsigned char>( A + v.A );
- return n;
-}
-
-/*!
- Substraction operator : "this" - v.
- \param v : Color to substract to the current object "this".
- \return "this" - v
-*/
-vpColVector
-vpRGBa::operator-(const vpColVector &v) const
-{
- vpColVector n(4); // new color
- n[0] = R - v[0];
- n[1] = G - v[1];
- n[2] = B - v[2];
- n[3] = A - v[3];
- return n;
-}
-
-/*!
- Addition operator : "this" + v.
- \param v : Color to add to the current object "this".
- \return "this" + v
-*/
-vpColVector
-vpRGBa::operator+(const vpColVector &v) const
-{
- vpColVector n(4); // new color
- n[0] = R + v[0];
- n[1] = G + v[1];
- n[2] = B + v[2];
- n[3] = A + v[3];
- return n;
-}
-
-/*!
- Multiplication operator : v * "this".
- \param v : Value to multiply.
- \return v * "this"
-*/
-vpColVector
-vpRGBa::operator*(const float &v) const
-{
- vpColVector n(4);
- n[0] = R * v;
- n[1] = G * v;
- n[2] = B * v;
- n[3] = A * v;
- return n;
-}
-
-/*!
- Multiplication operator : v * "this".
- \param v : Value to multiply.
- \return v * "this"
-*/
-vpColVector
-vpRGBa::operator*(const double &v) const
-{
- vpColVector n(4);
- n[0] = R * v;
- n[1] = G * v;
- n[2] = B * v;
- n[3] = A * v;
- return n;
-}
-
-bool
-vpRGBa::operator<(const vpRGBa &v) const
-{
- double gray1 = 0.2126*R+0.7152*G+0.0722*B;
- double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
-
- return (gray1 < gray2);
-}
-
-bool
-vpRGBa::operator>(const vpRGBa &v) const
-{
- double gray1 = 0.2126*R+0.7152*G+0.0722*B;
- double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
-
- return (gray1 > gray2);
-}
-
-vpRGBa operator*(const double &x, const vpRGBa &rgb)
-{
- return rgb*x;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/image/vpRGBa.h b/src/image/vpRGBa.h
deleted file mode 100644
index 5566e56..0000000
--- a/src/image/vpRGBa.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRGBa.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * RGBA pixel.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpRGBa_h
-#define vpRGBa_h
-
-/*!
- \file vpRGBa.h
- \brief Define the object vpRGBa that is used to build color
- images (it define a RGB 32 bits structure, fourth byte is not used)
-*/
-
-#include <visp/vpColVector.h>
-
-
-/*!
- \class vpRGBa
-
- \ingroup ImageContainer
-
- \brief Class that defines a RGB 32 bits structure.
-
- Define the object vpRGBa that is used to build color
- images (it define a RGB 32 bits structure, fourth byte is not used)
-
- \sa vpImage
-*/
-class VISP_EXPORT vpRGBa
-{
-public:
- /*!
- Basic constructor.
-
- Build a black value.
-
- */
- inline vpRGBa() : R(0), G(0), B(0), A(0) {};
-
- /*!
- Constructor.
-
- Initialize the color with R, G, B, A values.
-
- \param r : Red value.
- \param g : Green value.
- \param b : Blue value.
- \param a : Additional value.
-
- */
- inline vpRGBa(const unsigned char &r, const unsigned char &g,
- const unsigned char &b, const unsigned char &a=0)
- : R(r), G(g), B(b), A(a) {};
-
-
- /*!
- Constructor.
-
- Initialize all the R, G, B, A components to \e v.
-
- \param v : Value to set.
-
- */
- inline vpRGBa(const unsigned char &v) : R(v), G(v), B(v), A(v) {};
-
-
- /*!
- Copy constructor.
- */
- inline vpRGBa(const vpRGBa &v) : R(v.R), G(v.G), B(v.B), A(v.A) {};
-
- /*!
- Create a RGBa value from a 4 dimension column vector.
-
- R=v[0]
- G=v[1]
- B=v[2]
- A=v[3]
-
- */
- inline vpRGBa(const vpColVector &v) : R(0), G(0), B(0), A(0)
- {
- *this = v;
- };
-
- // We cannot add here the following destructor without changing the hypothesis that the size of this class is 4.
- // With the destructor it becomes 16 that does break a lot of things arround image conversions
- // virtual ~vpRGBa() {}; // Not to implement
-
- vpRGBa & operator=(const unsigned char &v) ;
- vpRGBa & operator=(const vpRGBa &v) ;
- vpRGBa & operator=(const vpColVector &v) ;
- bool operator==(const vpRGBa &v);
- bool operator!=(const vpRGBa &v);
-
- vpColVector operator-(const vpRGBa &v) const;
- vpRGBa operator+(const vpRGBa &v) const;
- vpColVector operator-(const vpColVector &v) const;
- vpColVector operator+(const vpColVector &v) const;
- vpColVector operator*(const float &v) const;
- vpColVector operator*(const double &v) const;
-
- bool operator<(const vpRGBa &v) const;
- bool operator>(const vpRGBa &v) const;
-
- public:
- unsigned char R ; //!< Red component.
- unsigned char G ; //!< Green component.
- unsigned char B ; //!< Blue component.
- unsigned char A ; //!< Additionnal component.
-
- friend VISP_EXPORT vpRGBa operator*(const double &x, const vpRGBa &rgb);
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/key-point/vpBasicKeyPoint.cpp b/src/key-point/vpBasicKeyPoint.cpp
deleted file mode 100644
index 68ad68b..0000000
--- a/src/key-point/vpBasicKeyPoint.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBasicKeyPoint.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#include <visp/vpBasicKeyPoint.h>
-
-/*!
- Basic constructor.
-*/
-vpBasicKeyPoint::vpBasicKeyPoint()
- : referenceImagePointsList(), currentImagePointsList(), matchedReferencePoints(), _reference_computed(false)
-{
-}
-
-
diff --git a/src/key-point/vpBasicKeyPoint.h b/src/key-point/vpBasicKeyPoint.h
deleted file mode 100644
index 375aa74..0000000
--- a/src/key-point/vpBasicKeyPoint.h
+++ /dev/null
@@ -1,259 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBasicKeyPoint.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#ifndef vpBasicKeyPoint_H
-#define vpBasicKeyPoint_H
-
-/*!
- \file vpBasicKeyPoint.h
- \brief class that defines what is a Keypoint
-*/
-
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpRect.h>
-
-#include <vector>
-
-
-/*!
- \class vpBasicKeyPoint
-
- \brief class that defines what is a Keypoint. This class provides
- all the basic elements to implement classes which aims to match
- points from an image to another.
-*/
-
-
-class VISP_EXPORT vpBasicKeyPoint
-{
- public:
- vpBasicKeyPoint();
-
- virtual ~vpBasicKeyPoint() {
- matchedReferencePoints.resize(0);
- currentImagePointsList.resize(0);
- referenceImagePointsList.resize(0);
- };
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I) =0;
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) =0;
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle) =0;
-
- virtual void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3) =0;
-
- virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- 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.
- */
- 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.
- */
- 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.
- */
- inline void getReferencePoint (const unsigned int index, vpImagePoint &referencePoint )
- {
- if (index >= referenceImagePointsList.size())
- {
- vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
- }
-
- referencePoint.set_ij(referenceImagePointsList[index].get_i(), referenceImagePointsList[index].get_j());
- }
-
- /*!
- 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(const unsigned int index, vpImagePoint &referencePoint, vpImagePoint ¤tPoint) {
- if (index >= matchedReferencePoints.size())
- {
- vpTRACE("Index of the matched points out of range");
- throw(vpException(vpException::fatalError,"Index of the matched points out of range"));
- }
- referencePoint.set_ij(referenceImagePointsList[matchedReferencePoints[index]].get_i(),referenceImagePointsList[matchedReferencePoints[index]].get_j());
- currentPoint.set_ij(currentImagePointsList[index].get_i(), currentImagePointsList[index].get_j());
- };
-
- /*!
- 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
- vpKeyPointSurf surf;
-
- //Here the code to compute the reference points and the current points.
-
- vpImagePoint referencePoint1;
- vpImagePoint currentPoint;
- surf.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points
-
- vpImagePoint referencePoint2;
- const vpImagePoint* referencePointsList = surf.getAllPointsInReferenceImage();
- int index = surf.getIndexInAllReferencePointList(1); //Get the first matched reference point index in the complete reference point list
- referencePoint2 = referencePointsList[index]; //Get the first matched reference point
- \endcode
- */
- inline unsigned int getIndexInAllReferencePointList ( const unsigned int indexInMatchedPointList ) {
- if (indexInMatchedPointList >= matchedReferencePoints.size())
- {
- vpTRACE("Index of the matched reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the matched reference point out of range"));
- }
- return matchedReferencePoints[indexInMatchedPointList];
- }
-
- /*!
- 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.
- */
- 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.
- */
- const std::vector<vpImagePoint>& getReferenceImagePointsList() const {return referenceImagePointsList;}
-
- /*!
- Return the vector of current image point.
-
- \warning Should not be modified.
-
- \return Vector of the current image point.
- */
- const std::vector<vpImagePoint>& 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.
- */
- const std::vector<unsigned int>& getMatchedReferencePoints() const {return matchedReferencePoints;}
-
- private:
- virtual void init()=0;
-
- protected:
- /*!
- List of the points which define the reference.
- */
- std::vector<vpImagePoint> referenceImagePointsList;
-
- /*!
- List of the points which belong to the current image and have
- been matched with points belonging to the reference.
- */
- std::vector<vpImagePoint> 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.
- */
- std::vector<unsigned int> matchedReferencePoints;
-
- //! flag to indicate if the reference has been built.
- bool _reference_computed;
-};
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/key-point/vpFernClassifier.cpp b/src/key-point/vpFernClassifier.cpp
deleted file mode 100644
index a14886f..0000000
--- a/src/key-point/vpFernClassifier.cpp
+++ /dev/null
@@ -1,584 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFernClassifier.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.GPL 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class that implements the Fern classifier and the YAPE detector thanks
- * to the OpenCV library.
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-
-#include <visp/vpFernClassifier.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpColor.h>
-#include <visp/vpDisplay.h>
-
-/*!
- Basic constructor
-
-*/
-vpFernClassifier::vpFernClassifier()
- : vpBasicKeyPoint(),
- ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
- hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
- nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
- sigmaBlur(1), nbMinPoint(10),
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- curImg(),
- #else
- curImg(NULL),
- #endif
- objKeypoints(), modelROI_Ref(), modelROI(),
- modelPoints(), imgKeypoints(), refPt(), curPt()
-{
-}
-
-/*!
- Constructor using the provided data file to load the object given by its name.
-
- If the name of the object does not correspond in the file, a
- vpException::ioError is thrown.
-
- \param _dataFile : the name of the file saved after a training (with the
- record methods).
- \param _objectName : the name of the object to load
-*/
-vpFernClassifier::vpFernClassifier(const std::string& _dataFile, const std::string& _objectName)
- : vpBasicKeyPoint(),
- ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
- hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
- nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
- sigmaBlur(1), nbMinPoint(10),
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- curImg(),
- #else
- curImg(NULL),
- #endif
- objKeypoints(), modelROI_Ref(), modelROI(),
- modelPoints(), imgKeypoints(), refPt(), curPt()
-{
- this->load(_dataFile, _objectName);
-}
-
-/*!
- Basic destructor
-
-*/
-vpFernClassifier::~vpFernClassifier()
-{
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(curImg != NULL){
- if(curImg->width % 8 == 0){
- curImg->imageData = NULL;
- cvReleaseImageHeader(&curImg);
- }else{
- cvReleaseImage(&curImg);
- }
- curImg = NULL;
- }
-#endif
-}
-
-/*!
- initialise any OpenCV parameters
-
- The classifier need to be trained again or to be loaded from an external file.
-*/
-void
-vpFernClassifier::init()
-{
- hasLearn = false;
- nbClassfier = 100;
- ClassifierSize = 11;
- nbPoints = 200;
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- curImg = NULL;
-#endif
- blurImage = true;
- radiusBlur = 7;
- sigmaBlur = 1;
- patchSize = 32;
- radius = 7;
- threshold = 20;
- nbOctave = 2;
- nbView = 2000;
- dist = 2;
- nbMinPoint = 10;
-}
-
-
-
-
-/*!
- Train the classifier.
-*/
-void
-vpFernClassifier::train()
-{
- // initialise detector
- cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
-
- //blur
- cv::Mat obj = (cv::Mat)curImg;
-
- if(this->getBlurSetting()){
- cv::GaussianBlur(obj, obj, cv::Size(getBlurSize(), getBlurSize()), getBlurSigma(), getBlurSigma());
- }
-
- // build pyramid
- std::vector<cv::Mat> objpyr;
- cv::buildPyramid(obj, objpyr, d.nOctaves-1);
-
- // getPoints
- d.getMostStable2D(obj, objKeypoints, 100, gen);
-
- ldetector = d;
-
- // train classifier
- modelROI = cv::Rect(0, 0, objpyr[0].cols, objpyr[0].rows);
- ldetector.getMostStable2D(objpyr[0], modelPoints, 100, gen);
-
- fernClassifier.trainFromSingleView(objpyr[0], modelPoints,
- patchSize, (int)modelPoints.size(), 100, 11, 10000,
- cv::FernClassifier::COMPRESSION_NONE, gen);
-
- /* from OpenCV format to ViSP format */
- referenceImagePointsList.resize(0);
- for (unsigned int i = 0; i < modelPoints.size(); i += 1){
- vpImagePoint ip(
- modelPoints[i].pt.y + modelROI_Ref.y,
- modelPoints[i].pt.x + modelROI_Ref.x);
- referenceImagePointsList.push_back(ip);
- }
-
- // set flag
- hasLearn = true;
-}
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made all over the image I. It also includes the training of the fern
- classifier.
-
- \warning thie method can take up to several minutes depending on the
- parameters of the classifier and on the size of the image.
-
- \param _I : The gray scaled image where the reference points are computed.
-
- \return the number of reference points.
-*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
-{
- this->setImage(_I);
-
- train();
-
- _reference_computed = true;
- return (unsigned int)objKeypoints.size();
-}
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made only on a part of the image. This part is a rectangle defined
- by its top left corner, its height and its width. The parameters of
- this rectangle must be given in pixel. It also includes the training of the
- fern classifier.
-
- \warning the method can take up to several minutes depending on the
- parameters of the classifier and on the size of the image.
-
- \param _I : The gray scaled image where the reference points are computed.
- \param _iP : The top left corner of the rectangle.
- \param _height : height of the rectangle (in pixel).
- \param _width : width of the rectangle (in pixel).
-
- \return the number of reference points.
-*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- const unsigned int _height, const unsigned int _width)
-{
- if((_iP.get_i()+_height) >= _I.getHeight()
- || (_iP.get_j()+_width) >= _I.getWidth()) {
- vpTRACE("Bad size for the subimage");
- throw(vpException(vpImageException::notInTheImage ,
- "Bad size for the subimage"));
- }
-
- vpImage<unsigned char> subImage;
- vpImageTools::createSubImage(_I,
- (unsigned int)_iP.get_i(),
- (unsigned int)_iP.get_j(),
- _height, _width, subImage);
- this->setImage(subImage);
-
- /* initialise a structure containing the region of interest used in the
- reference image */
- modelROI_Ref.x = (int)_iP.get_u();
- modelROI_Ref.y = (int)_iP.get_v();
- modelROI_Ref.width = (int)_width;
- modelROI_Ref.height = (int)_height;
-
- train();
-
- return (unsigned int)objKeypoints.size();
-}
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made only on a part of the image. This part is a rectangle. The
- parameters of this rectangle must be given in pixel. It also includes the
- training of the fern classifier.
-
- \warning the method can take up to several minutes depending on the
- parameters of the classifier and on the size of the image.
-
- \param _I : The gray scaled image where the reference points are computed.
- \param _rectangle : The rectangle which defines the interesting part
- of the image.
-
- \return The number of reference points.
-*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
- const vpRect& _rectangle)
-{
- vpImagePoint iP;
- iP.set_i(_rectangle.getTop());
- iP.set_j(_rectangle.getLeft());
- return (this->buildReference(_I, iP,
- (unsigned int)_rectangle.getHeight(),
- (unsigned int)_rectangle.getWidth()));
-}
-
-/*!
- Compute the points of interest in the current image and try to match them
- using the trained classifier. The matched pairs can be found with the
- getMatchedPointByRef function.
-
- \exception vpException::notInitialised if the classifier has not been trained.
-
- \param _I : The gray scaled image where the points are computed.
-
- \return The number of point which have been matched.
-*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
-{
- if(!hasLearn){
- vpERROR_TRACE("The object has not been learned. ");
- throw vpException(vpException::notInitialized , "object is not learned, load database or build the reference ");
- }
-
- setImage(_I);
- // resize image
-// cv::resize(_image, image, Size(), 1./imgscale, 1./imgscale, INTER_CUBIC);
- cv::Mat img = this->curImg;
-
- if(this->getBlurSetting()){
- cv::GaussianBlur(img, img, cv::Size(this->getBlurSize(), this->getBlurSize()), this->getBlurSigma(), this->getBlurSigma());
- }
-
- std::vector<cv::Mat> imgPyr;
- cv::buildPyramid(img, imgPyr, ldetector.nOctaves-1);
-
-
- ldetector(imgPyr, imgKeypoints, 500);
-
- unsigned int m = (unsigned int)modelPoints.size();
- unsigned int n = (unsigned int)imgKeypoints.size();
- std::vector<int> bestMatches(m, -1);
- std::vector<float> maxLogProb(m, -FLT_MAX);
- std::vector<float> signature;
- unsigned int totalMatch = 0;
-
-
- /* part of code from OpenCV planarObjectDetector */
- currentImagePointsList.resize(0);
- matchedReferencePoints.resize(0);
-
- for(unsigned int i = 0; i < n; i++ ){
- cv::KeyPoint kpt = imgKeypoints[i];
- kpt.pt.x /= (float)(1 << kpt.octave);
- kpt.pt.y /= (float)(1 << kpt.octave);
- int k = fernClassifier(imgPyr[(unsigned int)kpt.octave], kpt.pt, signature);
- if( k >= 0 && (bestMatches[(unsigned int)k] < 0 || signature[(unsigned int)k] > maxLogProb[(unsigned int)k]) ){
- maxLogProb[(unsigned int)k] = signature[(unsigned int)k];
- bestMatches[(unsigned int)k] = (int)i;
- totalMatch++;
-
- vpImagePoint ip_cur( imgKeypoints[i].pt.y, imgKeypoints[i].pt.x);
-
- currentImagePointsList.push_back(ip_cur);
- matchedReferencePoints.push_back((unsigned int)k);
-
- }
- }
-
- refPt.resize(0);
- curPt.resize(0);
- for(unsigned int i = 0; i < m; i++ ){
- if( bestMatches[i] >= 0 ){
- refPt.push_back(modelPoints[i].pt);
- curPt.push_back(imgKeypoints[(unsigned int)bestMatches[i]].pt);
- }
- }
-
- return totalMatch;
-}
-
-
-
-/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
- \param _I : The gray scaled image where the points are computed.
- \param _iP : the top left corner of the rectangle defining the ROI
- \param _height : the height of the ROI
- \param _width : the width of the ROI
-
- \return the number of point which have been matched.
-*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- const unsigned int _height, const unsigned int _width)
-{
- if((_iP.get_i()+_height) >= _I.getHeight()
- || (_iP.get_j()+_width) >= _I.getWidth()) {
- vpTRACE("Bad size for the subimage");
- throw(vpException(vpImageException::notInTheImage ,
- "Bad size for the subimage"));
- }
-
- vpImage<unsigned char> subImage;
-
- vpImageTools::createSubImage(_I,
- (unsigned int)_iP.get_i(),
- (unsigned int)_iP.get_j(),
- _height, _width, subImage);
-
- return this->matchPoint(subImage);
-}
-
-/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
- \param _I : The gray scaled image where the points are computed.
- \param _rectangle : the rectangle defining the ROI
-
- \return the number of point which have been matched.
-*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
- const vpRect& _rectangle)
-{
- vpImagePoint iP;
- iP.set_i(_rectangle.getTop());
- iP.set_j(_rectangle.getLeft());
- return (this->matchPoint(_I, iP,
- (unsigned int)_rectangle.getHeight(),
- (unsigned int)_rectangle.getWidth()));
-}
-
-
-/*!
- This function displays the matched reference points and the matched
- points computed in the current image. The reference points are
- displayed in the image Ireference and the matched points coming from
- the current image are displayed in the image Icurrent. It is
- possible to set Ireference and Icurrent with the same image when
- calling the method.
-
- \param _Iref : The image where the matched reference points are
- displayed.
-
- \param _Icurrent : The image where the matched points computed in the
- current image are displayed.
-
- \param size : Size in pixels of the cross that is used to display matched points.
-*/
-void
-vpFernClassifier::display(const vpImage<unsigned char> &_Iref,
- const vpImage<unsigned char> &_Icurrent, unsigned int size)
-{
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
- vpDisplay::displayCross (_Iref, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
- vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, vpColor::green);
- }
-
-}
-
-
-/*!
- This function displays only the matched points computed in the
- current image. They are displayed in the image Icurrent.
-
- \param _Icurrent : the gray scaled image for the display
-
- \param size : Size in pixels of the cross that is used to display matched points.
-
- \param color : Color used to display the matched points.
-*/
-void
-vpFernClassifier::display(const vpImage<unsigned char> &_Icurrent, unsigned int size,
- const vpColor &color)
-{
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
- vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, color);
- }
-}
-
-/* IO METHODS */
-
-/*!
- \brief load the Fern classifier
-
- Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
- can take up to sevral seconds.
-
- \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
-*/
-void
-vpFernClassifier::load(const std::string& _dataFile, const std::string& /*_objectName*/)
-{
- std::cout << " > Load data for the planar object detector..." << std::endl;
-
- /* part of code from OpenCV planarObjectDetector */
- cv::FileStorage fs(_dataFile, cv::FileStorage::READ);
- cv::FileNode node = fs.getFirstTopLevelNode();
-
- cv::FileNodeIterator it = node["model-roi"].begin(), it_end;
- it >> modelROI.x >> modelROI.y >> modelROI.width >> modelROI.height;
-
- ldetector.read(node["detector"]);
- fernClassifier.read(node["fern-classifier"]);
-
- const cv::FileNode node_ = node["model-points"];
- cv::read(node_, modelPoints);
-
- cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
- ldetector = d;
- hasLearn = true;
-}
-
-
-/*!
- \brief record the Ferns classifier in the text file
-
- \param _objectName : The name of the object to store in the data file.
- \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
-*/
-void
-vpFernClassifier::record(const std::string& _objectName, const std::string& _dataFile )
-{
- /* part of code from OpenCV planarObjectDetector */
- cv::FileStorage fs(_dataFile, cv::FileStorage::WRITE);
-
- cv::WriteStructContext ws(fs, _objectName, CV_NODE_MAP);
-
- {
- cv::WriteStructContext wsroi(fs, "model-roi", CV_NODE_SEQ + CV_NODE_FLOW);
- cv::write(fs, modelROI_Ref.x);
- cv::write(fs, modelROI_Ref.y);
- cv::write(fs, modelROI_Ref.width);
- cv::write(fs, modelROI_Ref.height);
- }
-
- ldetector.write(fs, "detector");
- cv::write(fs, "model-points", modelPoints);
- fernClassifier.write(fs, "fern-classifier");
-}
-
-
-
-/*!
- Set the current image. This method allows to convert a image from the ViSP
- format to the OpenCV one.
-
- \param I : the input image.
-*/
-void
-vpFernClassifier::setImage(const vpImage<unsigned char>& I)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- vpImageConvert::convert(I, curImg);
-#else
- if(curImg != NULL){
- cvResetImageROI(curImg);
- if((curImg->width % 8) == 0){
- curImg->imageData = NULL;
- cvReleaseImageHeader(&curImg);
- }else{
- cvReleaseImage(&curImg);
- }
- curImg = NULL;
- }
- if((I.getWidth()%8) == 0){
- curImg = cvCreateImageHeader(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
- if(curImg != NULL){
- curImg->imageData = (char*)I.bitmap;
- }else{
- throw vpException(vpException::memoryAllocationError, "Could not create the image in the OpenCV format.");
- }
- }else{
- vpImageConvert::convert(I, curImg);
- }
- if(curImg == NULL){
- std::cout << "!> conversion failed" << std::endl;
- throw vpException(vpException::notInitialized , "conversion failed");
- }
-#endif
-
-}
-
-#endif
-
diff --git a/src/key-point/vpFernClassifier.h b/src/key-point/vpFernClassifier.h
deleted file mode 100644
index 118105b..0000000
--- a/src/key-point/vpFernClassifier.h
+++ /dev/null
@@ -1,339 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFernClassifier.h 5203 2015-01-24 09:33:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.GPL 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class that implements the Fern classifier and the YAPE detector thanks
- * to the OpenCV library.
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#ifndef vpFernClassifier_H
-#define vpFernClassifier_H
-
-#include <visp/vpConfig.h>
-
-#include <string>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/legacy/legacy.hpp>
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
-# include <cv.h>
-# include <cvaux.hpp>
-#endif
-
-#include <visp/vpBasicKeyPoint.h>
-
-/*!
- \class vpFernClassifier
-
- \brief Class that implements the Fern classifier and the YAPE detector thanks
- to the OpenCV library.
-
- \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
-
- This class provides a way to detect and match point using the YAPE and
- a Fern Classifiers, thanks to the OpenCV library (version >= 2.0)
-
- This class provides a tool to match points between a model and the current
- image. The points of interests belonging to the model and the points detected
- in the current image are given in pixels thanks to the vpImagePoint class.
-
- For more details about the Ferns Classifier and the point detector,
- see \cite Ozuysal10 and \cite Lepetit04c.
-
- To use this class, you first have to detect points in the model and train the
- associated Fern classifier. Then, for each new grabbed image, You can detect
- points and try to match them with the model.
-
- As training can requires up to several minutes, it is possible to save (in a
- file) and load the classifier.
-
- The following small example shows how to use the class.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpFernClassifier.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
-int main()
-{
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
- vpFernClassifier fern;
-
- //First grab the reference image Ireference
-
- //Build the reference points.
- fern.buildReference(Ireference);
-
- //Then grab another image which represents the current image Icurrent
-
- //Match points between the reference points and the points detected in the current image.
- fern.matchPoint(Icurrent);
-
- //Display the matched points
- fern.display(Ireference, Icurrent);
-
- return (0);
-}
-#else
-int main() {}
-#endif
- \endcode
-
- It is also possible to create the reference thanks to only a part of the
- reference image (not the whole image) and find points to match in only a
- part of the current image. The small following example shows how to do this.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpFernClassifier.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
-int main()
-{
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
- vpFernClassifier fern;
-
- //First grab the reference image Ireference
-
- //Select a part of the image by clicking on two points which define a rectangle
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Ireference, corners[i]);
- }
-
- //Build the reference points.
- int nbrRef;
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrRef = fern.buildReference(Ireference, corners[0], height, width);
-
- //Then grab another image which represents the current image Icurrent
-
- //Select a part of the image by clincking on two points which define a rectangle
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Icurrent, corners[i]);
- }
-
- //Match points between the reference points and the points detected in the current image.
- int nbrMatched;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrMatched = fern.matchPoint(Icurrent, corners[0], height, width);
-
- //Display the matched points
- fern.display(Ireference, Icurrent);
-
- return(0);
-}
-#else
-int main() {}
-#endif
- \endcode
-*/
-class VISP_EXPORT vpFernClassifier: public vpBasicKeyPoint
-{
-protected:
- //! The points of interest detector.
- cv::LDetector ldetector;
-
- //! The Fern classifier.
- cv::FernClassifier fernClassifier;
-
- //! The patch generator (OpenCV format).
- cv::PatchGenerator gen;
-
- //! Flag to indicate whether the classifier has been trained or not.
- bool hasLearn;
-
- /* parameters for the key-points detector and the classifier */
- //! Threshold to accept or reject points (usually around 20)
- int threshold;
- //! Number of view to generate for the training
- int nbView;
- //! Minimal distance between two points
- int dist;
- //! Number of classifier
- int nbClassfier;
- //! Size of the classifier
- int ClassifierSize;
- //! Number of octave for the multi scale
- int nbOctave;
- //! Size of the patch
- int patchSize;
- //! Radius for the detector
- int radius;
- //! Maximal number of points
- int nbPoints;
-
- /* parameters for the filter */
- //! Flag to specify whether the reference image have to be blurred or not in order to improve the recognition rate
- bool blurImage;
- //! Radius of the kernel used to blur the image
- int radiusBlur;
- //! Sigma of the kernel used to blur the image
- int sigmaBlur;
-
- //! Number of minimum point below which the homography is not estimated (must be at least four)
- unsigned int nbMinPoint;
-
- //! The current image in the OpenCV format.
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat curImg;
-#else
- IplImage* curImg;
-#endif
-
- //! keypoints detected in the reference image.
- std::vector<cv::KeyPoint> objKeypoints;
- //! the ROI in the reference image.
- cv::Rect modelROI_Ref;
- //! the ROI for the reference image.
- cv::Rect modelROI;
- //! the vector containing the points in the model.
- std::vector<cv::KeyPoint> modelPoints;
- //! the vector containing the points in the current image.
- std::vector<cv::KeyPoint> imgKeypoints;
- //! vector in the OpenCV format to be used by the detector.
- std::vector<cv::Point2f> refPt, curPt;
-
-
-public:
- vpFernClassifier();
- vpFernClassifier(const std::string& _dataFile, const std::string& _objectName);
- virtual ~vpFernClassifier();
-
- /* build reference */
- virtual unsigned int buildReference(const vpImage<unsigned char> &I);
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
-
- /* matching */
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I);
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
-
- /* display */
- virtual void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3);
- virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- const vpColor &color=vpColor::green);
-
- /* io methods */
- void load(const std::string& _dataFile, const std::string& /*_objectName*/);
- void record(const std::string& _objectName, const std::string& _dataFile);
-
-
-
- /* accessors */
- /*!
- The image is blurred before being processed. This solution can lead to a
- better recognition rate.
-
- \param _blur : the new option for the blur
- */
- inline void setBlurSetting(const bool _blur){ blurImage = _blur;}
- inline void setBlurSettings(const bool _blur, int _sigma, int _size);
-
- /*!
- Return the blur option.
- The Blur option is used to activate a filter used to blur the input image.
- The blurring can improve the robustness of the detection.
-
- \return the value of the blur option.
- */
- inline bool getBlurSetting(){ return blurImage;}
-
- /*!
- Return the blur sigma (for the filter) option.
-
- \return The value of the sigma for the blur filter.
- */
- inline int getBlurSigma() { return this->sigmaBlur;}
-
- /*!
- return the blur size (for the filter) option
-
- \return the value of the radius for the blur filter
- */
- inline int getBlurSize() { return this->radiusBlur;}
-
- /*!
- Return a reference on the vector of reference points in the OpenCV format
-
- \return the list of reference points
- */
- const std::vector<cv::Point2f>& getRefPt() const {return refPt;}
-
- /*!
- Return a reference on the vector of current points in the OpenCV format
-
- \return the list of current points
- */
- const std::vector<cv::Point2f>& getCurPt() const {return curPt;}
-
- /*!
- Return the region of interest in the OpenCV format
- */
- cv::Rect getModelROI() const { return modelROI;}
-
-protected:
- void setImage(const vpImage<unsigned char>& I);
- void train();
- virtual void init();
-};
-
-#endif /* opencv ver > 2.0.0 */
-
-#endif
-
diff --git a/src/key-point/vpKeyPoint.cpp b/src/key-point/vpKeyPoint.cpp
deleted file mode 100644
index 18e7d68..0000000
--- a/src/key-point/vpKeyPoint.cpp
+++ /dev/null
@@ -1,3008 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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 functionalities.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-
-#include <limits>
-
-#include <visp/vpKeyPoint.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
-# include <opencv2/calib3d/calib3d.hpp>
-#endif
-
-
-//Specific Type transformation functions
-///*!
-// Convert a list of cv::DMatch to a cv::DMatch (extract the first cv::DMatch, the nearest neighbor).
-//
-// \param knnMatches : List of cv::DMatch.
-// \return The nearest neighbor.
-// */
-inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches) {
- if(knnMatches.size() > 0) {
- return knnMatches[0];
- }
-
- return cv::DMatch();
-}
-
-///*!
-// Convert a cv::DMatch to an index (extract the train index).
-//
-// \param match : Point to convert in ViSP type.
-// \return The train index.
-// */
-inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
- return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
-}
-
-//TODO: Try to implement a pyramidal feature detection
-//#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
-//class MaskPredicate
-//{
-//public:
-// MaskPredicate( const cv::Mat& _mask ) : mask(_mask) {}
-// bool operator() (const cv::KeyPoint& key_pt) const
-// {
-// return mask.at<uchar>( (int)(key_pt.pt.y + 0.5f), (int)(key_pt.pt.x + 0.5f) ) == 0;
-// }
-//
-//private:
-// const cv::Mat mask;
-// MaskPredicate& operator=(const MaskPredicate&);
-//};
-//
-//void vpKeyPoint::runByPixelsMask( std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask )
-//{
-// if( mask.empty() )
-// {
-// return;
-// }
-//
-// keypoints.erase(std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)), keypoints.end());
-//}
-//
-//void vpKeyPoint::pyramidFeatureDetection(cv::Ptr<cv::FeatureDetector> &detector, const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask,
-// const int maxLevel)
-//{
-// cv::Mat src = image;
-// cv::Mat src_mask = mask;
-//
-// cv::Mat dilated_mask;
-// if( !mask.empty() )
-// {
-// dilate( mask, dilated_mask, cv::Mat() );
-// cv::Mat mask255( mask.size(), CV_8UC1, cv::Scalar(0) );
-// mask255.setTo( cv::Scalar(255), dilated_mask != 0 );
-// dilated_mask = mask255;
-// }
-//
-// for( int l = 0, multiplier = 1; l <= maxLevel; ++l, multiplier *= 2 )
-// {
-// // Detect on current level of the pyramid
-// std::vector<cv::KeyPoint> new_pts;
-// detector->detect( src, new_pts, src_mask );
-// std::vector<cv::KeyPoint>::iterator it = new_pts.begin(),
-// end = new_pts.end();
-// for( ; it != end; ++it)
-// {
-// it->pt.x *= multiplier;
-// it->pt.y *= multiplier;
-// it->size *= multiplier;
-// it->octave = l;
-// }
-// keypoints.insert( keypoints.end(), new_pts.begin(), new_pts.end() );
-//
-// // Downsample
-// if( l < maxLevel )
-// {
-// cv::Mat dst;
-// pyrDown( src, dst );
-// src = dst;
-//
-// if( !mask.empty() )
-// resize( dilated_mask, src_mask, src.size(), 0, 0, CV_INTER_AREA );
-// }
-// }
-//
-// if( !mask.empty() )
-// {
-// //KeyPointsFilter::runByPixelsMask( keypoints, mask );
-// }
-//}
-//#endif
-
-/*!
- Constructor to initialize specified detector, extractor, matcher and filtering method.
-
- \param detectorName : Name of the detector.
- \param extractorName : Name of the extractor.
- \param matcherName : Name of the matcher.
- \param filterType : Filtering matching method chosen.
- */
-vpKeyPoint::vpKeyPoint(const std::string &detectorName, const std::string &extractorName,
- const std::string &matcherName, const vpFilterMatchingType &filterType)
- : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
- m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(),
- m_detectors(), m_extractionTime(0.), m_extractorNames(), m_extractors(), m_filteredMatches(), m_filterType(filterType),
- m_knnMatches(), m_mapOfImageId(), m_mapOfImages(), m_matcher(), m_matcherName(matcherName), m_matches(),
- m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
- m_matchQueryToTrainKeyPoints(), m_matchRansacKeyPointsToPoints(), m_matchRansacQueryToTrainKeyPoints(),
- m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
- m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
- m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
- m_ransacThreshold(0.001), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
- m_trainVpPoints(), m_useAffineDetection(false), m_useBruteForceCrossCheck(true),
- m_useConsensusPercentage(false), m_useKnn(false), m_useRansacVVS(false)
-{
- //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
- //So this is useful only for ratioDistanceThreshold method
- if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
- m_useKnn = true;
- }
-
- m_detectorNames.push_back(detectorName);
- m_extractorNames.push_back(extractorName);
-
- init();
-}
-
-/*!
- Constructor to initialize specified detector, extractor, matcher and filtering method.
-
- \param detectorNames : List of name detector for allowing multiple detectors.
- \param extractorNames : List of name extractor for allowing multiple extractors.
- \param matcherName : Name of the matcher.
- \param filterType : Filtering matching method chosen.
- */
-vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
- const std::string &matcherName, const vpFilterMatchingType &filterType)
- : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
- m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(detectorNames),
- m_detectors(), m_extractionTime(0.), m_extractorNames(extractorNames), m_extractors(), m_filteredMatches(),
- m_filterType(filterType), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(), m_matcher(), m_matcherName(matcherName),
- m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
- m_matchQueryToTrainKeyPoints(), m_matchRansacKeyPointsToPoints(), m_matchRansacQueryToTrainKeyPoints(),
- m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
- m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
- m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
- m_ransacThreshold(0.001), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
- m_trainVpPoints(), m_useAffineDetection(false), m_useBruteForceCrossCheck(true),
- m_useConsensusPercentage(false), m_useKnn(false), m_useRansacVVS(false)
-{
- //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
- //So this is useful only for ratioDistanceThreshold method
- if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
- m_useKnn = true;
- }
-
- init();
-}
-
-/*!
- Apply an affine and skew transformation to an image.
- \param tilt : Tilt value in the direction of x
- \param phi : Rotation value
- \param img : Modified image after the transformation
- \param mask : Mask containing the location of the image pixels after the transformation
- \param Ai : Inverse affine matrix
- */
-void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
- cv::Mat& mask, cv::Mat& Ai) {
- int h = img.rows;
- int w = img.cols;
-
- mask = cv::Mat(h, w, CV_8UC1, cv::Scalar(255));
-
- cv::Mat A = cv::Mat::eye(2, 3, CV_32F);
-
- //if (phi != 0.0) {
- if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
- phi *= M_PI / 180.;
- double s = sin(phi);
- double c = cos(phi);
-
- A = (cv::Mat_<float>(2, 2) << c, -s, s, c);
-
- cv::Mat corners = (cv::Mat_<float>(4, 2) << 0, 0, w, 0, w, h, 0, h);
- cv::Mat tcorners = corners * A.t();
- cv::Mat tcorners_x, tcorners_y;
- tcorners.col(0).copyTo(tcorners_x);
- tcorners.col(1).copyTo(tcorners_y);
- std::vector<cv::Mat> channels;
- channels.push_back(tcorners_x);
- channels.push_back(tcorners_y);
- merge(channels, tcorners);
-
- cv::Rect rect = boundingRect(tcorners);
- A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);
-
- cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
- cv::INTER_LINEAR, cv::BORDER_REPLICATE);
- }
- //if (tilt != 1.0) {
- if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
- double s = 0.8 * sqrt(tilt * tilt - 1);
- cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
- cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
- A.row(0) = A.row(0) / tilt;
- }
- //if (tilt != 1.0 || phi != 0.0) {
- if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
- h = img.rows;
- w = img.cols;
- cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
- }
- invertAffineTransform(A, Ai);
-}
-
-/*!
- Build the reference keypoints list.
-
- \param I : Input reference image.
- \return The number of detected keypoints in the image \p I.
- */
-unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I) {
- return buildReference(I, vpRect());
-}
-
-/*!
- Build the reference keypoints list in a region of interest in the image.
-
- \param I : Input reference image
- \param iP : Position of the top-left corner of the region of interest.
- \param height : Height of the region of interest.
- \param width : Width of the region of interest.
- \return The number of detected keypoints in the current image I.
- */
-unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) {
-
- return buildReference(I, vpRect(iP, width, height));
-}
-
-/*!
- Build the reference keypoints list in a region of interest in the image.
-
- \param I : Input image.
- \param rectangle : Rectangle of the region of interest.
- \return The number of detected keypoints in the current image I.
- */
-unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
- const vpRect &rectangle) {
- //Reset variables used when dealing with 3D models
- //So as no 3D point list is passed, we dont need this variables
- m_trainPoints.clear();
- m_mapOfImageId.clear();
- m_mapOfImages.clear();
- m_currentImageId = 1;
-
- if(m_useAffineDetection) {
- std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
- std::vector<cv::Mat> listOfTrainDescriptors;
-
- //Detect keypoints and extract descriptors on multiple images
- detectExtractAffine(I, listOfTrainKeyPoints, listOfTrainDescriptors);
-
- //Flatten the different train lists
- m_trainKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
- it != listOfTrainKeyPoints.end(); ++it) {
- m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
- }
-
- bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
- if(first) {
- first = false;
- it->copyTo(m_trainDescriptors);
- } else {
- m_trainDescriptors.push_back(*it);
- }
- }
- } else {
- detect(I, m_trainKeyPoints, m_detectionTime, rectangle);
- extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
- }
-
- //Save the correspondence keypoint class_id with the training image_id in a map
- //Used to display the matching with all the training images
- for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
- m_mapOfImageId[it->class_id] = m_currentImageId;
- }
-
- //Save the image in a map at a specific image_id
- m_mapOfImages[m_currentImageId] = I;
-
- //Convert OpenCV type to ViSP type for compatibility
- vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
-
- _reference_computed = true;
-
- return static_cast<unsigned int>(m_trainKeyPoints.size());
-}
-
-/*!
- Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
-
- \param I : Input image
- \param trainKeyPoints : List of the train keypoints.
- \param points3f : Output list of the 3D position corresponding of the keypoints locations.
- \param append : If true, append the supply train keypoints with those already present.
- */
-void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
- std::vector<cv::Point3f> &points3f, bool append) {
- cv::Mat trainDescriptors;
- extract(I, trainKeyPoints, trainDescriptors, m_extractionTime);
-
- buildReference(I, trainKeyPoints, trainDescriptors, points3f, append);
-}
-
-/*!
- Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
-
- \param I : Input image
- \param trainKeyPoints : List of the train keypoints.
- \param points3f : List of the 3D position corresponding of the keypoints locations.
- \param trainDescriptors : List of the train descriptors.
- \param append : If true, append the supply train keypoints with those already present.
- */
-void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
- const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f, bool append) {
- if(!append) {
- m_currentImageId = 0;
- m_mapOfImageId.clear();
- m_mapOfImages.clear();
- this->m_trainKeyPoints.clear();
- this->m_trainPoints.clear();
- }
-
- m_currentImageId++;
-
- //Save the correspondence keypoint class_id with the training image_id in a map
- //Used to display the matching with all the training images
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
- m_mapOfImageId[it->class_id] = m_currentImageId;
- }
-
- //Save the image in a map at a specific image_id
- m_mapOfImages[m_currentImageId] = I;
-
- //Append reference lists
- this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints.begin(), trainKeyPoints.end());
- if(!append) {
- trainDescriptors.copyTo(this->m_trainDescriptors);
- } else {
- this->m_trainDescriptors.push_back(trainDescriptors);
- }
- this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());
-
-
- //Convert OpenCV type to ViSP type for compatibility
- vpConvert::convertFromOpenCV(this->m_trainKeyPoints, referenceImagePointsList);
- vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
-
- _reference_computed = true;
-}
-
-/*!
- Compute the 3D coordinate given the 2D image coordinate and under the assumption that the point is located on a plane
- whose the plane equation is known in the camera frame.
- The Z-coordinate is retrieved according to the proportional relation between the plane equation expressed in the
- normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
-
- \param candidate : Keypoint we want to compute the 3D coordinate.
- \param roi : List of 3D points representing a planar face.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param point : 3D coordinate computed.
- */
-void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) {
- /* compute plane equation */
- std::vector<vpPoint>::const_iterator it_roi = roi.begin();
- vpPoint pts[3];
- pts[0] = *it_roi;
- ++it_roi;
- pts[1] = *it_roi;
- ++it_roi;
- pts[2] = *it_roi;
- vpPlane Po(pts[0], pts[1], pts[2]);
- double xc = 0.0, yc = 0.0;
- vpPixelMeterConversion::convertPoint(cam, candidate.pt.x, candidate.pt.y, xc, yc);
- double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
- double X = xc * Z;
- double Y = yc * Z;
- vpColVector point_cam(4);
- point_cam[0] = X;
- point_cam[1] = Y;
- point_cam[2] = Z;
- point_cam[3] = 1;
- vpColVector point_obj(4);
- point_obj = cMo.inverse() * point_cam;
- point = cv::Point3f((float) point_obj[0], (float) point_obj[1], (float) point_obj[2]);
-}
-
-/*!
- Compute the 3D coordinate given the 2D image coordinate and under the assumption that the point is located on a plane
- whose the plane equation is known in the camera frame.
- The Z-coordinate is retrieved according to the proportional relation between the plane equation expressed in the
- normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
-
- \param candidate : vpImagePoint we want to compute the 3D coordinate.
- \param roi : List of 3D points representing a planar face.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param point : 3D coordinate computed.
- */
-void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) {
- /* compute plane equation */
- std::vector<vpPoint>::const_iterator it_roi = roi.begin();
- vpPoint pts[3];
- pts[0] = *it_roi;
- ++it_roi;
- pts[1] = *it_roi;
- ++it_roi;
- pts[2] = *it_roi;
- vpPlane Po(pts[0], pts[1], pts[2]);
- double xc = 0.0, yc = 0.0;
- vpPixelMeterConversion::convertPoint(cam, candidate, xc, yc);
- double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
- double X = xc * Z;
- double Y = yc * Z;
- vpColVector point_cam(4);
- point_cam[0] = X;
- point_cam[1] = Y;
- point_cam[2] = Z;
- point_cam[3] = 1;
- vpColVector point_obj(4);
- point_obj = cMo.inverse() * point_cam;
- point.setWorldCoordinates(point_obj);
-}
-
-/*!
- Keep only keypoints located on faces and compute for those keypoints the 3D coordinate given the 2D image coordinate
- and under the assumption that the point is located on a plane.
-
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param cam : Camera parameters.
- \param candidates : In input, list of keypoints detected in the whole image, in output, list of keypoints only located
- on planes.
- \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
- \param roisPt : List of faces.
- \param points : Output list of computed 3D coordinates of keypoints located only on faces.
- \param descriptors : Optional parameter, pointer to the descriptors to filter
- */
-void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<cv::KeyPoint> &candidates, std::vector<vpPolygon> &polygons, std::vector<std::vector<vpPoint> > &roisPt,
- std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
-
- std::vector<cv::KeyPoint> candidateToCheck = candidates;
- candidates.clear();
- vpImagePoint iPt;
- cv::Point3f pt;
- cv::Mat desc;
-
- size_t cpt1 = 0;
- for (std::vector<vpPolygon>::iterator it1 = polygons.begin(); it1 != polygons.end(); ++it1, cpt1++) {
- int cpt2 = 0;
-
- for (std::vector<cv::KeyPoint>::iterator it2 = candidateToCheck.begin(); it2 != candidateToCheck.end(); cpt2++) {
- iPt.set_ij(it2->pt.y, it2->pt.x);
- if (it1->isInside(iPt)) {
- candidates.push_back(*it2);
- vpKeyPoint::compute3D(*it2, roisPt[cpt1], cam, cMo, pt);
- points.push_back(pt);
-
- if(descriptors != NULL) {
- desc.push_back(descriptors->row(cpt2));
- }
-
- //Remove candidate keypoint which is located on the current polygon
- candidateToCheck.erase(it2);
- } else {
- ++it2;
- }
- }
- }
-
- if(descriptors != NULL) {
- desc.copyTo(*descriptors);
- }
-}
-
-/*!
- Keep only keypoints located on faces and compute for those keypoints the 3D coordinate given the 2D image coordinate
- and under the assumption that the point is located on a plane.
-
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param cam : Camera parameters.
- \param candidates : In input, list of vpImagePoint located in the whole image, in output, list of vpImagePoint only located
- on planes.
- \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
- \param roisPt : List of faces.
- \param points : Output list of computed 3D coordinates of vpImagePoint located only on faces.
- \param descriptors : Optional parameter, pointer to the descriptors to filter
- */
-void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidates, std::vector<vpPolygon> &polygons, std::vector<std::vector<vpPoint> > &roisPt,
- std::vector<vpPoint> &points, cv::Mat *descriptors) {
-
- std::vector<vpImagePoint> candidateToCheck = candidates;
- candidates.clear();
- vpPoint pt;
- cv::Mat desc;
-
- size_t cpt1 = 0;
- for (std::vector<vpPolygon>::iterator it1 = polygons.begin(); it1 != polygons.end(); ++it1, cpt1++) {
- int cpt2 = 0;
-
- for (std::vector<vpImagePoint>::iterator it2 = candidateToCheck.begin(); it2 != candidateToCheck.end(); cpt2++) {
- if (it1->isInside(*it2)) {
- candidates.push_back(*it2);
- vpKeyPoint::compute3D(*it2, roisPt[cpt1], cam, cMo, pt);
- points.push_back(pt);
-
- if(descriptors != NULL) {
- desc.push_back(descriptors->row(cpt2));
- }
-
- //Remove candidate keypoint which is located on the current polygon
- candidateToCheck.erase(it2);
- } else {
- ++it2;
- }
- }
- }
-}
-
-/*!
- Compute the pose using the correspondence between 2D points and 3D points using OpenCV function with RANSAC method.
-
- \param imagePoints : List of 2D points corresponding to the location of the detected keypoints.
- \param objectPoints : List of the 3D points in the object frame matched.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the object frame and the camera frame.
- \param inlierIndex : List of indexes of inliers.
- \param elapsedTime : Elapsed time.
- \param func : Function pointer to filter the final pose returned by OpenCV pose estimation method.
- \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
- */
-bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
- const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
- double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
- double t = vpTime::measureTimeMs();
-
- if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
- elapsedTime = (vpTime::measureTimeMs() - t);
- std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
-
- return false;
- }
-
- cv::Mat cameraMatrix =
- (cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
- cv::Mat rvec, tvec;
- cv::Mat distCoeffs;
-
- try {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- //OpenCV 3.0.0 (2014/12/12)
- cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
- rvec, tvec, false, m_nbRansacIterations, (float) m_ransacReprojectionError,
- 0.99,//confidence=0.99 (default) – The probability that the algorithm produces a useful result.
- inlierIndex,
- cv::SOLVEPNP_ITERATIVE);
- //SOLVEPNP_ITERATIVE (default): Iterative method is based on Levenberg-Marquardt optimization.
- //In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances
- //between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
- //SOLVEPNP_P3P: Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification
- //for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
- //SOLVEPNP_EPNP: Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient
- //Perspective-n-Point Camera Pose Estimation”.
- //SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS)
- //Method for PnP”.
- //SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer.
- //“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also
- //estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the
- //estimated focal length.
-#else
- int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
- if(m_useConsensusPercentage) {
- nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (double) m_queryFilteredKeyPoints.size());
- }
-
- cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
- rvec, tvec, false, m_nbRansacIterations,
- (float) m_ransacReprojectionError, nbInlierToReachConsensus,
- inlierIndex);
-#endif
- } catch (cv::Exception &e) {
- std::cerr << e.what() << std::endl;
- elapsedTime = (vpTime::measureTimeMs() - t);
- return false;
- }
- vpTranslationVector translationVec(tvec.at<double>(0),
- tvec.at<double>(1), tvec.at<double>(2));
- vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1),
- rvec.at<double>(2));
- cMo = vpHomogeneousMatrix(translationVec, thetaUVector);
-
- if(func != NULL) {
- //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
- //we can converge toward a final cMo that does not respect the pose criterion even
- //if the 4 minimal points picked to respect the pose criterion.
- if(!func(&cMo)) {
- elapsedTime = (vpTime::measureTimeMs() - t);
- return false;
- }
- }
-
- elapsedTime = (vpTime::measureTimeMs() - t);
- return true;
-}
-
-/*!
- Compute the pose using the correspondence between 2D points and 3D points using ViSP function with RANSAC method.
-
- \param objectVpPoints : List of vpPoint with coordinates expressed in the object and in the camera frame.
- \param cMo : Homogeneous matrix between the object frame and the camera frame.
- \param inliers : List of inliers.
- \param elapsedTime : Elapsed time.
- \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- */
-bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
- double t = vpTime::measureTimeMs();
-
- if(objectVpPoints.size() < 4) {
- elapsedTime = (vpTime::measureTimeMs() - t);
- std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
-
- return false;
- }
-
- vpPose pose;
- pose.setCovarianceComputation(true);
-
- for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
- pose.addPoint(*it);
- }
-
- unsigned int nbInlierToReachConsensus = (unsigned int) m_nbRansacMinInlierCount;
- if(m_useConsensusPercentage) {
- nbInlierToReachConsensus = (unsigned int) (m_ransacConsensusPercentage / 100.0 *
- (double) m_queryFilteredKeyPoints.size());
- }
-
- pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
- pose.setRansacThreshold(m_ransacThreshold);
- pose.setRansacMaxTrials(m_nbRansacIterations);
-
- bool isRansacPoseEstimationOk = false;
- try {
- pose.setCovarianceComputation(m_computeCovariance);
- isRansacPoseEstimationOk = pose.computePose(vpPose::RANSAC, cMo, func);
- inliers = pose.getRansacInliers();
- if(m_computeCovariance) {
- m_covarianceMatrix = pose.getCovarianceMatrix();
- }
- } catch(vpException &e) {
- std::cerr << "e=" << e.what() << std::endl;
- elapsedTime = (vpTime::measureTimeMs() - t);
- return false;
- }
-
- if(func != NULL && isRansacPoseEstimationOk) {
- //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
- //we can converge toward a final cMo that does not respect the pose criterion even
- //if the 4 minimal points picked to respect the pose criterion.
- if(!func(&cMo)) {
- elapsedTime = (vpTime::measureTimeMs() - t);
- return false;
- }
- }
-
- elapsedTime = (vpTime::measureTimeMs() - t);
- return isRansacPoseEstimationOk;
-}
-
-/*!
- Compute the pose estimation error, the mean square error (in pixel) between the location of the detected keypoints
- and the location of the projection of the 3D model with the estimated pose.
-
- \param matchKeyPoints : List of pairs between the detected keypoints and the corresponding 3D points.
- \param cam : Camera parameters.
- \param cMo_est : Estimated pose of the object.
-
- \return The mean square error (in pixel) between the location of the detected keypoints
- and the location of the projection of the 3D model with the estimated pose.
- */
-double vpKeyPoint::computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) {
- if(matchKeyPoints.size() == 0) {
- //return std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- return DBL_MAX;
- }
-
- std::vector<double> errors(matchKeyPoints.size());
- size_t cpt = 0;
- vpPoint pt;
- for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
- it != matchKeyPoints.end(); ++it, cpt++) {
- pt.set_oX(it->second.x);
- pt.set_oY(it->second.y);
- pt.set_oZ(it->second.z);
- pt.project(cMo_est);
- double u = 0.0, v = 0.0;
- vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
- errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
- }
-
- return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
-}
-
-/*!
- Initialize the size of the matching image (case with a matching side by side between IRef and ICurrent).
-
- \param IRef : Reference image.
- \param ICurrent : Current image.
- \param IMatching : Image matching.
- */
-void vpKeyPoint::createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
- vpImage<unsigned char> &IMatching) {
- //Image matching side by side
- unsigned int width = IRef.getWidth() + ICurrent.getWidth();
- unsigned int height = (std::max)(IRef.getHeight(), ICurrent.getHeight());
-
- IMatching = vpImage<unsigned char>(height, width);
-}
-
-/*!
- Initialize the size of the matching image with appropriate size according to the number of training images.
- Used to display the matching of keypoints detected in the current image with those detected in multiple
- training images.
-
- \param ICurrent : Current image.
- \param IMatching : Image initialized with appropriate size.
- */
-void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
- //Nb images in the training database + the current image we want to detect the object
- unsigned int nbImg = (unsigned int) (m_mapOfImages.size() + 1);
-
- if(m_mapOfImages.empty()) {
- return;
- }
-
- if(nbImg == 2) {
- //Only one training image, so we display them side by side
- createImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
- } else {
- //Multiple training images, display them as a mosaic image
- //round(std::sqrt((double) nbImg)); VC++ compiler does not have round so the next line is used
- //Different implementations of round exist, here round to the closest integer but will not work for negative numbers
- unsigned int nbImgSqrt = (unsigned int) std::floor(std::sqrt((double) nbImg) + 0.5);
-
- //Number of columns in the mosaic grid
- unsigned int nbWidth = nbImgSqrt;
- //Number of rows in the mosaic grid
- unsigned int nbHeight = nbImgSqrt;
-
- //Deals with non square mosaic grid and the total number of images
- if(nbImgSqrt * nbImgSqrt < nbImg) {
- nbWidth++;
- }
-
- unsigned int maxW = ICurrent.getWidth();
- unsigned int maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(maxW < it->second.getWidth()) {
- maxW = it->second.getWidth();
- }
-
- if(maxH < it->second.getHeight()) {
- maxH = it->second.getHeight();
- }
- }
-
- IMatching = vpImage<unsigned char>(maxH * nbHeight, maxW * nbWidth);
- }
-}
-
-/*!
- Detect keypoints in the image.
-
- \param I : Input image.
- \param keyPoints : Output list of the detected keypoints.
- \param elapsedTime : Elapsed time.
- \param rectangle : Optional rectangle of the region of interest.
- */
-void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const vpRect &rectangle) {
- cv::Mat matImg;
- vpImageConvert::convert(I, matImg, false);
- cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);
-
- if(rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
- cv::Point leftTop((int) rectangle.getLeft(), (int) rectangle.getTop()), rightBottom((int) rectangle.getRight(),
- (int) rectangle.getBottom());
- cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
- } else {
- mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U);
- }
-
- detect(matImg, keyPoints, elapsedTime, mask);
-}
-
-/*!
- Detect keypoints in the image.
-
- \param matImg : Input image.
- \param keyPoints : Output list of the detected keypoints.
- \param elapsedTime : Elapsed time.
- \param mask : Optional mask to detect only where mask[i][j] == 1.
- */
-void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const cv::Mat &mask) {
- double t = vpTime::measureTimeMs();
- keyPoints.clear();
-
- for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
- std::vector<cv::KeyPoint> kp;
- it->second->detect(matImg, kp, mask);
- keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
- }
-
- elapsedTime = vpTime::measureTimeMs() - t;
-}
-
-/*!
- Display the reference and the detected keypoints in the images.
-
- \param IRef : Input reference image.
- \param ICurrent : Input current image.
- \param size : Size of the displayed cross.
- */
-void vpKeyPoint::display(const vpImage<unsigned char> &IRef,
- const vpImage<unsigned char> &ICurrent, unsigned int size) {
- std::vector<vpImagePoint> vpQueryImageKeyPoints;
- getQueryKeyPoints(vpQueryImageKeyPoints);
- std::vector<vpImagePoint> vpTrainImageKeyPoints;
- getTrainKeyPoints(vpTrainImageKeyPoints);
-
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- vpDisplay::displayCross(IRef, vpTrainImageKeyPoints[(size_t)(it->trainIdx)], size, vpColor::red);
- vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, vpColor::green);
- }
-}
-
-/*!
- Display the reference keypoints.
-
- \param ICurrent : Input current image.
- \param size : Size of the displayed crosses.
- \param color : Color of the crosses.
- */
-void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color) {
- std::vector<vpImagePoint> vpQueryImageKeyPoints;
- getQueryKeyPoints(vpQueryImageKeyPoints);
-
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- vpDisplay::displayCross (ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
- }
-}
-
-/*!
- Display the matching lines between the detected keypoints with those detected in one training image.
-
- \param IRef : Reference image, used to have the x-offset.
- \param IMatching : Resulting image matching.
- \param crossSize : Size of the displayed crosses.
- \param lineThickness : Thickness of the displayed lines.
- \param color : Color to use, if none, we pick randomly a color for each pair of matching.
- */
-void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
- unsigned int crossSize, unsigned int lineThickness, const vpColor &color) {
- bool randomColor = (color == vpColor::none);
- srand((unsigned int) time(NULL));
- vpColor currentColor = color;
-
- std::vector<vpImagePoint> queryImageKeyPoints;
- getQueryKeyPoints(queryImageKeyPoints);
- std::vector<vpImagePoint> trainImageKeyPoints;
- getTrainKeyPoints(trainImageKeyPoints);
-
- vpImagePoint leftPt, rightPt;
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- if(randomColor) {
- currentColor = vpColor((rand() % 256), (rand() % 256), (rand() % 256));
- }
-
- leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
- rightPt = vpImagePoint(queryImageKeyPoints[(size_t)(it->queryIdx)].get_i(),
- queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.getWidth());
- vpDisplay::displayCross(IMatching, leftPt, crossSize, currentColor);
- vpDisplay::displayCross(IMatching, rightPt, crossSize, currentColor);
- vpDisplay::displayLine(IMatching, leftPt, rightPt, currentColor, lineThickness);
- }
-}
-
-/*!
- Display matching between keypoints detected in the current image and with those detected in the multiple training
- images. Display also RANSAC inliers if the list is supplied.
-
- \param ICurrent : Current image.
- \param IMatching : Resulting matching image.
- \param ransacInliers : List of Ransac inliers or empty list if not available.
- \param crossSize : Size of the displayed crosses.
- \param lineThickness : Thickness of the displayed line.
- */
-void vpKeyPoint::displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
- const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize, unsigned int lineThickness) {
- if(m_mapOfImages.empty()) {
- //No training images so return
- return;
- }
-
- //Nb images in the training database + the current image we want to detect the object
- int nbImg = (int) (m_mapOfImages.size() + 1);
-
- if(nbImg == 2) {
- //Only one training image, so we display the matching result side-by-side
- displayMatching(m_mapOfImages.begin()->second, IMatching, crossSize);
- } else {
- //Multiple training images, display them as a mosaic image
- //round(std::sqrt((double) nbImg)); VC++ compiler does not have round so the next line is used
- //Different implementations of round exist, here round to the closest integer but will not work for negative numbers
- int nbImgSqrt = (int) std::floor(std::sqrt((double) nbImg) + 0.5);
- int nbWidth = nbImgSqrt;
- int nbHeight = nbImgSqrt;
-
- if(nbImgSqrt * nbImgSqrt < nbImg) {
- nbWidth++;
- }
-
- std::map<int, int> mapOfImageIdIndex;
- int cpt = 0;
- unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
- mapOfImageIdIndex[it->first] = cpt;
-
- if(maxW < it->second.getWidth()) {
- maxW = it->second.getWidth();
- }
-
- if(maxH < it->second.getHeight()) {
- maxH = it->second.getHeight();
- }
- }
-
- //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
- int medianI = nbHeight / 2;
- int medianJ = nbWidth / 2;
- int medianIndex = medianI * nbWidth + medianJ;
- for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
- vpImagePoint topLeftCorner;
- int current_class_id_index = 0;
- if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
- current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
- } else {
- //Shift of one unity the index of the training images which are after the current image
- current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
- }
-
- int indexI = current_class_id_index / nbWidth;
- int indexJ = current_class_id_index - (indexI * nbWidth);
- topLeftCorner.set_ij((int)maxH*indexI, (int)maxW*indexJ);
-
- //Display cross for keypoints in the learning database
- vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
- crossSize, vpColor::red);
- }
-
- vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
- for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
- //Display cross for keypoints detected in the current image
- vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
- crossSize, vpColor::red);
- }
- for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
- it != ransacInliers.end(); ++it) {
- //Display green circle for RANSAC inliers
- vpDisplay::displayCircle(IMatching, (int) (it->get_v() + topLeftCorner.get_i()), (int) (it->get_u() +
- topLeftCorner.get_j()), 4, vpColor::green);
- }
- for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
- //Display red circle for RANSAC outliers
- vpDisplay::displayCircle(IMatching, (int) (it->get_i() + topLeftCorner.get_i()), (int) (it->get_j() +
- topLeftCorner.get_j()), 4, vpColor::red);
- }
-
- for(std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> >::const_iterator it = m_matchQueryToTrainKeyPoints.begin();
- it != m_matchQueryToTrainKeyPoints.end(); ++it) {
- int current_class_id = 0;
- if(mapOfImageIdIndex[m_mapOfImageId[it->second.class_id]] < medianIndex) {
- current_class_id = mapOfImageIdIndex[m_mapOfImageId[it->second.class_id]];
- } else {
- //Shift of one unity the index of the training images which are after the current image
- current_class_id = mapOfImageIdIndex[m_mapOfImageId[it->second.class_id]] + 1;
- }
-
- int indexI = current_class_id / nbWidth;
- int indexJ = current_class_id - (indexI * nbWidth);
-
- vpImagePoint end((int)maxH*indexI + it->second.pt.y, (int)maxW*indexJ + it->second.pt.x);
- vpImagePoint start((int)maxH*medianI + it->first.pt.y, (int)maxW*medianJ + it->first.pt.x);
-
- //Draw line for matching keypoints detected in the current image and those detected
- //in the training images
- vpDisplay::displayLine(IMatching, start, end, vpColor::green, lineThickness);
- }
- }
-}
-
-/*!
- Extract the descriptors for each keypoints of the list.
-
- \param I : Input image.
- \param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param elapsedTime : Elapsed time.
- */
-void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime) {
- cv::Mat matImg;
- vpImageConvert::convert(I, matImg, false);
- extract(matImg, keyPoints, descriptors, elapsedTime);
-}
-
-/*!
- Extract the descriptors for each keypoints of the list.
-
- \param matImg : Input image.
- \param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param elapsedTime : Elapsed time.
- */
-void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime) {
- double t = vpTime::measureTimeMs();
- bool first = true;
-
- for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
- it != m_extractors.end(); ++it) {
- if(first) {
- first = false;
- it->second->compute(matImg, keyPoints, descriptors);
- } else {
- cv::Mat desc;
- it->second->compute(matImg, keyPoints, desc);
- if(descriptors.empty()) {
- desc.copyTo(descriptors);
- } else {
- cv::hconcat(descriptors, desc, descriptors);
- }
- }
- }
-
- if(keyPoints.size() != (size_t) descriptors.rows) {
- std::cerr << "keyPoints.size() != (size_t) descriptors.rows" << std::endl;
- }
- elapsedTime = vpTime::measureTimeMs() - t;
-}
-
-/*!
- Filter the matches using the desired filtering method.
- */
-void vpKeyPoint::filterMatches() {
- m_matchQueryToTrainKeyPoints.clear();
-
- std::vector<cv::KeyPoint> queryKpts;
- std::vector<cv::Point3f> trainPts;
- std::vector<cv::DMatch> m;
-
- if(m_useKnn) {
- double max_dist = 0;
- //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- double min_dist = DBL_MAX;
- double mean = 0.0;
- std::vector<double> distance_vec(m_knnMatches.size());
-
- if(m_filterType == stdAndRatioDistanceThreshold) {
- for(size_t i = 0; i < m_knnMatches.size(); i++) {
- double dist = m_knnMatches[i][0].distance;
- mean += dist;
- distance_vec[i] = dist;
-
- if (dist < min_dist) {
- min_dist = dist;
- }
- if (dist > max_dist) {
- max_dist = dist;
- }
- }
- mean /= m_queryDescriptors.rows;
- }
-
- double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
- double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
- double threshold = min_dist + stdev;
-
- for(size_t i = 0; i < m_knnMatches.size(); i++) {
- if(m_knnMatches[i].size() >= 2) {
- //Calculate ratio of the descriptor distance between the two nearest neighbors of the keypoint
- float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
-// float ratio = std::sqrt((vecMatches[i][0].distance * vecMatches[i][0].distance)
-// / (vecMatches[i][1].distance * vecMatches[i][1].distance));
- double dist = m_knnMatches[i][0].distance;
-
- if(ratio < m_matchingRatioThreshold || (m_filterType == stdAndRatioDistanceThreshold && dist < threshold)) {
- m.push_back(cv::DMatch((int) queryKpts.size(), m_knnMatches[i][0].trainIdx, m_knnMatches[i][0].distance));
-
- if(!m_trainPoints.empty()) {
- trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]);
- }
- queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]);
-
-// //Add the pair with the correspondence between the detected keypoints and the one matched in the train keypoints list
-// m_matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
-// m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx],
-// m_trainKeyPoints[(size_t)m_knnMatches[i][0].trainIdx]));
- }
- }
- }
- } else {
- double max_dist = 0;
- //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- double min_dist = DBL_MAX;
- double mean = 0.0;
- std::vector<double> distance_vec(m_matches.size());
- for(size_t i = 0; i < m_matches.size(); i++) {
- double dist = m_matches[i].distance;
- mean += dist;
- distance_vec[i] = dist;
-
- if (dist < min_dist) {
- min_dist = dist;
- }
- if (dist > max_dist) {
- max_dist = dist;
- }
- }
- mean /= m_queryDescriptors.rows;
-
- double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
- double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
-
- //Define a threshold where we keep all keypoints whose the descriptor distance falls below a factor of the
- //minimum descriptor distance (for all the query keypoints)
- //or below the minimum descriptor distance + the standard deviation (calculated on all the query descriptor distances)
- double threshold = m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;
-
- for (size_t i = 0; i < m_matches.size(); i++) {
- if(m_matches[i].distance <= threshold) {
- m.push_back(cv::DMatch((int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
-
- if(!m_trainPoints.empty()) {
- trainPts.push_back(m_trainPoints[(size_t)m_matches[i].trainIdx]);
- }
- queryKpts.push_back(m_queryKeyPoints[(size_t)m_matches[i].queryIdx]);
-
-// //Add the pair with the correspondence between the detected keypoints and the one matched in the train keypoints list
-// m_matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
-// m_queryKeyPoints[(size_t)m_matches[i].queryIdx], m_trainKeyPoints[(size_t)m_matches[i].trainIdx]));
- }
- }
- }
-
- //Eliminate matches where multiple query keypoints are matched to the same train keypoint
- std::vector<cv::DMatch> mTmp;
- std::vector<cv::Point3f> trainPtsTmp;
- std::vector<cv::KeyPoint> queryKptsTmp;
-
- std::map<int, int> mapOfTrainIdx;
- //Count the number of query points matched to the same train point
- for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
- mapOfTrainIdx[it->trainIdx]++;
- }
-
- //Keep matches with only one correspondence
- for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
- if(mapOfTrainIdx[it->trainIdx] == 1) {
- mTmp.push_back(cv::DMatch((int) queryKptsTmp.size(), it->trainIdx, it->distance));
-
- if(!m_trainPoints.empty()) {
- trainPtsTmp.push_back(m_trainPoints[(size_t) it->trainIdx]);
- }
- queryKptsTmp.push_back(queryKpts[(size_t) it->queryIdx]);
-
- //Add the pair with the correspondence between the detected keypoints and the one matched in the train keypoints list
- m_matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
- queryKpts[(size_t) it->queryIdx],
- m_trainKeyPoints[(size_t) it->trainIdx]));
- }
- }
-
- m_filteredMatches = mTmp;
- m_objectFilteredPoints = trainPtsTmp;
- m_queryFilteredKeyPoints = queryKptsTmp;
-}
-
-/*!
- Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
- of the keypoints detected in the current image after the matching).
-
- \param objectPoints : List of 3D coordinates in the object frame.
- */
-void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const {
- objectPoints = m_objectFilteredPoints;
-}
-
-/*!
- Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
- of the keypoints detected in the current image after the matching).
-
- \param objectPoints : List of 3D coordinates in the object frame.
- */
-void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const {
- vpConvert::convertFromOpenCV(m_objectFilteredPoints, objectPoints);
-}
-
-/*!
- Get the query keypoints list in OpenCV type.
-
- \param keyPoints : List of query keypoints (or keypoints detected in the current image).
- */
-void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
- keyPoints = m_queryFilteredKeyPoints;
-}
-
-/*!
- Get the query keypoints list in ViSP type.
-
- \param keyPoints : List of query keypoints (or keypoints detected in the current image).
- */
-void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
- keyPoints = currentImagePointsList;
-}
-
-/*!
- Get the train keypoints list in OpenCV type.
-
- \param keyPoints : List of train keypoints (or reference keypoints).
- */
-void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
- keyPoints = m_trainKeyPoints;
-}
-
-/*!
- Get the train keypoints list in ViSP type.
-
- \param keyPoints : List of train keypoints (or reference keypoints).
- */
-void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
- keyPoints = referenceImagePointsList;
-}
-
-/*!
- Get the train points (the 3D coordinates in the object frame) list in OpenCV type.
-
- \param points : List of train points (or reference points).
- */
-void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const {
- points = m_trainPoints;
-}
-
-/*!
- Get the train points (the 3D coordinates in the object frame) list in ViSP type.
-
- \param points : List of train points (or reference points).
- */
-void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const {
- points = m_trainVpPoints;
-}
-
-/*!
- Initialize method for RANSAC parameters and for detectors, extractors and matcher, and for others parameters.
- */
-void vpKeyPoint::init() {
-#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require 2.4.0 <= opencv < 3.0.0
- //The following line must be called in order to use SIFT or SURF
- if (!cv::initModule_nonfree()) {
- std::cerr << "Cannot init module non free, SIFT or SURF cannot be used."
- << std::endl;
- }
-#endif
-
- initDetectors(m_detectorNames);
- initExtractors(m_extractorNames);
- initMatcher(m_matcherName);
-}
-
-/*!
- Initialize a keypoint detector based on its name.
-
- \param detectorName : Name of the detector (e.g FAST, SIFT, SURF, etc.).
- */
-void vpKeyPoint::initDetector(const std::string &detectorName) {
- // TODO: Add function to process detection in pyramid images with OpenCV 3.0.0
- // since there seems to have no equivalent function (2014/12/11)
- // std::string pyramid = "Pyramid";
- // std::size_t pos = detectorName.find(pyramid);
- // if(pos != std::string::npos) {
- // std::string sub = detectorName.substr(pos + pyramid.size());
- // detectors[detectorName] = cv::Ptr<cv::FeatureDetector>(
- // new cv::PyramidAdaptedFeatureDetector(cv::FeatureDetector::create<cv::FeatureDetector>(sub), 2));
- // } else {
- // detectors[detectorName] = cv::FeatureDetector::create<cv::FeatureDetector>(detectorName);
- // }
-#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
- m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);
-
- if(m_detectors[detectorName] == NULL) {
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: " << detectorName << " or it is not available in OpenCV version: "
- << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
- throw vpException(vpException::fatalError, ss_msg.str());
- }
-#else
- //TODO: Add a pyramidal feature detection
- std::string detectorNameTmp = detectorName;
- std::string pyramid = "Pyramid";
- std::size_t pos = detectorName.find(pyramid);
- if(pos != std::string::npos) {
- detectorNameTmp = detectorName.substr(pos + pyramid.size());
- }
-
- if(detectorNameTmp == "SIFT") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_detectors[detectorNameTmp] = cv::xfeatures2d::SIFT::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: SIFT. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else if(detectorNameTmp == "SURF") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_detectors[detectorNameTmp] = cv::xfeatures2d::SURF::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: SURF. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else if(detectorNameTmp == "FAST") {
- m_detectors[detectorNameTmp] = cv::FastFeatureDetector::create();
- } else if(detectorNameTmp == "MSER") {
- m_detectors[detectorNameTmp] = cv::MSER::create();
- } else if(detectorNameTmp == "ORB") {
- m_detectors[detectorNameTmp] = cv::ORB::create();
- } else if(detectorNameTmp == "BRISK") {
- m_detectors[detectorNameTmp] = cv::BRISK::create();
- } else if(detectorNameTmp == "KAZE") {
- m_detectors[detectorNameTmp] = cv::KAZE::create();
- } else if(detectorNameTmp == "AKAZE") {
- m_detectors[detectorNameTmp] = cv::AKAZE::create();
- } else if(detectorNameTmp == "GFFT") {
- m_detectors[detectorNameTmp] = cv::GFTTDetector::create();
- } else if(detectorNameTmp == "SimpleBlob") {
- m_detectors[detectorNameTmp] = cv::SimpleBlobDetector::create();
- } else if(detectorNameTmp == "STAR") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_detectors[detectorNameTmp] = cv::xfeatures2d::StarDetector::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: STAR. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else {
- std::cerr << "The detector:" << detectorNameTmp << " is not available." << std::endl;
- }
-
- if(m_detectors[detectorNameTmp] == NULL) {
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: " << detectorNameTmp << " or it is not available in OpenCV version: "
- << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
- throw vpException(vpException::fatalError, ss_msg.str());
- }
-// m_detectors[detectorName] = cv::FeatureDetector::create<cv::FeatureDetector>(detectorName);
-#endif
-}
-
-/*!
- Initialize a list of keypoints detectors if we want to concatenate multiple detectors.
-
- \param detectorNames : List of detector names.
- */
-void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
- for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
- initDetector(*it);
- }
-}
-
-/*!
- Initialize a descriptor extractor based on its name.
-
- \param extractorName : Name of the extractor (e.g SIFT, SURF, ORB, etc.).
- */
-void vpKeyPoint::initExtractor(const std::string &extractorName) {
-#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
- m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
-#else
- if(extractorName == "SIFT") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: SIFT. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else if(extractorName == "SURF") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_extractors[extractorName] = cv::xfeatures2d::SURF::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: SURF. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else if(extractorName == "ORB") {
- m_extractors[extractorName] = cv::ORB::create();
- } else if(extractorName == "BRISK") {
- m_extractors[extractorName] = cv::BRISK::create();
- } else if(extractorName == "FREAK") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: FREAK. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else if(extractorName == "BRIEF") {
-#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
-#else
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: BRIEF. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
- throw vpException(vpException::fatalError, ss_msg.str());
-#endif
- } else {
- std::cerr << "The extractor:" << extractorName << " is not available." << std::endl;
- }
-// m_extractors[extractorName] = cv::DescriptorExtractor::create<cv::DescriptorExtractor>(extractorName);
-#endif
-
- if(m_extractors[extractorName] == NULL) {
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: " << extractorName << " or it is not available in OpenCV version: "
- << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
- throw vpException(vpException::fatalError, ss_msg.str());
- }
-}
-
-/*!
- Initialize a list of descriptor extractors if we want to concatenate multiple extractors.
-
- \param extractorNames : List of extractor names.
- */
-void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames) {
- for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
- initExtractor(*it);
- }
-}
-
-/*!
- Initialize a matcher based on its name.
-
- \param matcherName : Name of the matcher (e.g BruteForce, FlannBased).
- */
-void vpKeyPoint::initMatcher(const std::string &matcherName) {
- m_matcher = cv::DescriptorMatcher::create(matcherName);
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
- if(m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
- m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
- }
-#endif
-
- if(m_matcher == NULL) {
- std::stringstream ss_msg;
- ss_msg << "Fail to initialize the matcher: " << matcherName << " or it is not available in OpenCV version: "
- << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
- throw vpException(vpException::fatalError, ss_msg.str());
- }
-}
-
-/*!
- Insert a reference image and a current image side-by-side.
-
- \param IRef : Reference image.
- \param ICurrent : Current image.
- \param IMatching : Matching image for displaying all the matching between the query keypoints and those
- detected in the training images.
- */
-void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
- vpImage<unsigned char> &IMatching) {
- vpImagePoint topLeftCorner(0, 0);
- IMatching.insert(IRef, topLeftCorner);
- topLeftCorner = vpImagePoint(0, IRef.getWidth());
- IMatching.insert(ICurrent, topLeftCorner);
-}
-
-/*!
- Insert the different training images in the matching image.
-
- \param ICurrent : Current image.
- \param IMatching : Matching image for displaying all the matching between the query keypoints and those
- detected in the training images
- */
-void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
- //Nb images in the training database + the current image we want to detect the object
- int nbImg = (int) (m_mapOfImages.size() + 1);
-
- if(nbImg == 2) {
- //Only one training image, so we display them side by side
- insertImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
- } else {
- //Multiple training images, display them as a mosaic image
- //round(std::sqrt((double) nbImg)); VC++ compiler does not have round so the next line is used
- //Different implementations of round exist, here round to the closest integer but will not work for negative numbers
- int nbImgSqrt = (int) std::floor(std::sqrt((double) nbImg) + 0.5);
- int nbWidth = nbImgSqrt;
- int nbHeight = nbImgSqrt;
-
- if(nbImgSqrt * nbImgSqrt < nbImg) {
- nbWidth++;
- }
-
- unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(maxW < it->second.getWidth()) {
- maxW = it->second.getWidth();
- }
-
- if(maxH < it->second.getHeight()) {
- maxH = it->second.getHeight();
- }
- }
-
- //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
- int medianI = nbHeight / 2;
- int medianJ = nbWidth / 2;
- int medianIndex = medianI * nbWidth + medianJ;
-
- int cpt = 0;
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
- int local_cpt = cpt;
- if(cpt >= medianIndex) {
- //Shift of one unity the index of the training images which are after the current image
- local_cpt++;
- }
- int indexI = local_cpt / nbWidth;
- int indexJ = local_cpt - (indexI * nbWidth);
- vpImagePoint topLeftCorner((int)maxH*indexI, (int)maxW*indexJ);
-
- IMatching.insert(it->second, topLeftCorner);
- }
-
- vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
- IMatching.insert(ICurrent, topLeftCorner);
- }
-}
-
-#ifdef VISP_HAVE_XML2
-/*!
- Load configuration parameters from an XML config file.
-
- \param configFile : Path to the XML config file.
- */
-void vpKeyPoint::loadConfigFile(const std::string &configFile) {
- vpXmlConfigParserKeyPoint xmlp;
-
- try {
- //Reset detector and extractor
- m_detectorNames.clear();
- m_extractorNames.clear();
- m_detectors.clear();
- m_extractors.clear();
-
- std::cout << " *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
- xmlp.parse(configFile);
-
- m_detectorNames.push_back(xmlp.getDetectorName());
- m_extractorNames.push_back(xmlp.getExtractorName());
- m_matcherName = xmlp.getMatcherName();
-
- switch(xmlp.getMatchingMethod()) {
- case vpXmlConfigParserKeyPoint::constantFactorDistanceThreshold:
- m_filterType = constantFactorDistanceThreshold;
- break;
-
- case vpXmlConfigParserKeyPoint::stdDistanceThreshold:
- m_filterType = stdDistanceThreshold;
- break;
-
- case vpXmlConfigParserKeyPoint::ratioDistanceThreshold:
- m_filterType = ratioDistanceThreshold;
- break;
-
- case vpXmlConfigParserKeyPoint::stdAndRatioDistanceThreshold:
- m_filterType = stdAndRatioDistanceThreshold;
- break;
-
- case vpXmlConfigParserKeyPoint::noFilterMatching:
- m_filterType = noFilterMatching;
- break;
-
- default:
- break;
- }
-
- m_matchingFactorThreshold = xmlp.getMatchingFactorThreshold();
- m_matchingRatioThreshold = xmlp.getMatchingRatioThreshold();
-
- m_useRansacVVS = xmlp.getUseRansacVVSPoseEstimation();
- m_useConsensusPercentage = xmlp.getUseRansacConsensusPercentage();
- m_nbRansacIterations = xmlp.getNbRansacIterations();
- m_ransacReprojectionError = xmlp.getRansacReprojectionError();
- m_nbRansacMinInlierCount = xmlp.getNbRansacMinInlierCount();
- m_ransacThreshold = xmlp.getRansacThreshold();
- m_ransacConsensusPercentage = xmlp.getRansacConsensusPercentage();
-
- if(m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) {
- m_useKnn = true;
- } else {
- m_useKnn = false;
- }
-
- init();
- }
- catch(...) {
- throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
- }
-}
-#endif
-
-/*!
- Load learning data saved on disk.
-
- \param filename : Path of the learning file.
- \param binaryMode : If true, the learning file is in a binary mode, otherwise it is in XML mode.
- \param append : If true, concatenate the learning data, otherwise reset the variables.
- */
-void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) {
- int startClassId = 0;
- int startImageId = 0;
- if(!append) {
- m_trainKeyPoints.clear();
- m_trainPoints.clear();
- m_mapOfImageId.clear();
- m_mapOfImages.clear();
- } else {
- //In append case, find the max index of keypoint class Id
- for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
- if(startClassId < it->first) {
- startClassId = it->first;
- }
- }
-
- //In append case, find the max index of images Id
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(startImageId < it->first) {
- startImageId = it->first;
- }
- }
- }
-
- //Get parent directory
- std::string parent = vpIoTools::getParent(filename);
- if(!parent.empty()) {
- parent += "/";
- }
-
- if(binaryMode) {
- std::ifstream file(filename.c_str(), std::ifstream::binary);
- if(!file.is_open()){
- throw vpException(vpException::ioError, "Cannot open the file.");
- }
-
- //Read info about training images
- int nbImgs = 0;
- file.read((char *)(&nbImgs), sizeof(nbImgs));
-
- for(int i = 0; i < nbImgs; i++) {
- //Read image_id
- int id = 0;
- file.read((char *)(&id), sizeof(id));
-
- int length = 0;
- file.read((char *)(&length), sizeof(length));
- //Will contain the path to the training images
- char* path = new char[length + 1];//char path[length + 1];
-
- for(int cpt = 0; cpt < length; cpt++) {
- char c;
- file.read((char *)(&c), sizeof(c));
- path[cpt] = c;
- }
- path[length] = '\0';
-
- vpImage<unsigned char> I;
- if(vpIoTools::isAbsolutePathname(std::string(path))) {
- vpImageIo::read(I, path);
- } else {
- vpImageIo::read(I, parent + path);
- }
-
- m_mapOfImages[id + startImageId] = I;
- }
-
- //Read if 3D point information are saved or not
- int have3DInfoInt = 0;
- file.read((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
- bool have3DInfo = have3DInfoInt != 0;
-
- //Read the number of descriptors
- int nRows = 0;
- file.read((char *)(&nRows), sizeof(nRows));
-
- //Read the size of the descriptor
- int nCols = 0;
- file.read((char *)(&nCols), sizeof(nCols));
-
- //Read the type of the descriptor
- int descriptorType = 5; //CV_32F
- file.read((char *)(&descriptorType), sizeof(descriptorType));
-
- cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
- for(int i = 0; i < nRows; i++) {
- //Read information about keyPoint
- float u, v, size, angle, response;
- int octave, class_id, image_id;
- file.read((char *)(&u), sizeof(u));
- file.read((char *)(&v), sizeof(v));
- file.read((char *)(&size), sizeof(size));
- file.read((char *)(&angle), sizeof(angle));
- file.read((char *)(&response), sizeof(response));
- file.read((char *)(&octave), sizeof(octave));
- file.read((char *)(&class_id), sizeof(class_id));
- file.read((char *)(&image_id), sizeof(image_id));
- cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
- m_trainKeyPoints.push_back(keyPoint);
-
- if(image_id != -1) {
- //No training images if image_id == -1
- m_mapOfImageId[class_id] = image_id + startImageId;
- }
-
- if(have3DInfo) {
- //Read oX, oY, oZ
- float oX, oY, oZ;
- file.read((char *)(&oX), sizeof(oX));
- file.read((char *)(&oY), sizeof(oY));
- file.read((char *)(&oZ), sizeof(oZ));
- m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
- }
-
- for(int j = 0; j < nCols; j++) {
- //Read the value of the descriptor
- switch(descriptorType) {
- case CV_8U:
- {
- unsigned char value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<unsigned char>(i, j) = value;
- }
- break;
-
- case CV_8S:
- {
- char value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<char>(i, j) = value;
- }
- break;
-
- case CV_16U:
- {
- unsigned short int value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
- }
- break;
-
- case CV_16S:
- {
- short int value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<short int>(i, j) = value;
- }
- break;
-
- case CV_32S:
- {
- int value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<int>(i, j) = value;
- }
- break;
-
- case CV_32F:
- {
- float value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<float>(i, j) = value;
- }
- break;
-
- case CV_64F:
- {
- double value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<double>(i, j) = value;
- }
- break;
-
- default:
- {
- float value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<float>(i, j) = value;
- }
- break;
- }
- }
- }
-
- if(!append || m_trainDescriptors.empty()) {
- trainDescriptorsTmp.copyTo(m_trainDescriptors);
- } else {
- cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
- }
-
- file.close();
- } else {
-#ifdef VISP_HAVE_XML2
- xmlDocPtr doc = NULL;
- xmlNodePtr root_element = NULL;
-
- /*
- * this initialize the library and check potential ABI mismatches
- * between the version it was compiled for and the actual shared
- * library used.
- */
- LIBXML_TEST_VERSION
-
- /*parse the file and get the DOM */
- doc = xmlReadFile(filename.c_str(), NULL, 0);
-
- if (doc == NULL) {
- throw vpException(vpException::ioError, "Error with file " + filename);
- }
-
- root_element = xmlDocGetRootElement(doc);
-
- xmlNodePtr first_level_node = NULL;
- char *pEnd = NULL;
-
- int descriptorType = CV_32F; //float
- int nRows = 0, nCols = 0;
- int i = 0, j = 0;
-
- cv::Mat trainDescriptorsTmp;
-
- for (first_level_node = root_element->children; first_level_node;
- first_level_node = first_level_node->next) {
-
- std::string name((char *) first_level_node->name);
- if (first_level_node->type == XML_ELEMENT_NODE && name == "TrainingImageInfo") {
- xmlNodePtr image_info_node = NULL;
-
- for (image_info_node = first_level_node->children; image_info_node; image_info_node =
- image_info_node->next) {
- name = std::string ((char *) image_info_node->name);
-
- if(name == "trainImg") {
- //Read image_id
- int id = std::atoi((char *) xmlGetProp(image_info_node, BAD_CAST "image_id"));
-
- vpImage<unsigned char> I;
- std::string path((char *) image_info_node->children->content);
- //Read path to the training images
- if(vpIoTools::isAbsolutePathname(std::string(path))) {
- vpImageIo::read(I, path);
- } else {
- vpImageIo::read(I, parent + path);
- }
-
- m_mapOfImages[id + startImageId] = I;
- }
- }
- } else if(first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
- xmlNodePtr descriptors_info_node = NULL;
- for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
- descriptors_info_node->next) {
- if (descriptors_info_node->type == XML_ELEMENT_NODE) {
- name = std::string ((char *) descriptors_info_node->name);
-
- if(name == "nrows") {
- nRows = std::atoi((char *) descriptors_info_node->children->content);
- } else if(name == "ncols") {
- nCols = std::atoi((char *) descriptors_info_node->children->content);
- } else if(name == "type") {
- descriptorType = std::atoi((char *) descriptors_info_node->children->content);
- }
- }
- }
-
- trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
- } else if (first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorInfo") {
- xmlNodePtr point_node = NULL;
- double u = 0.0, v = 0.0, size = 0.0, angle = 0.0, response = 0.0;
- int octave = 0, class_id = 0, image_id = 0;
- double oX = 0.0, oY = 0.0, oZ = 0.0;
-
- std::stringstream ss;
-
- for (point_node = first_level_node->children; point_node; point_node =
- point_node->next) {
- if (point_node->type == XML_ELEMENT_NODE) {
- name = std::string ((char *) point_node->name);
-
- //Read information about keypoints
- if(name == "u") {
- u = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "v") {
- v = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "size") {
- size = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "angle") {
- angle = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "response") {
- response = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "octave") {
- octave = std::atoi((char *) point_node->children->content);
- } else if(name == "class_id") {
- class_id = std::atoi((char *) point_node->children->content);
- cv::KeyPoint keyPoint(cv::Point2f((float) u, (float) v), (float) size,
- (float) angle, (float) response, octave, (class_id + startClassId));
- m_trainKeyPoints.push_back(keyPoint);
- } else if(name == "image_id") {
- image_id = std::atoi((char *) point_node->children->content);
- if(image_id != -1) {
- //No training images if image_id == -1
- m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
- }
- } else if (name == "oX") {
- oX = std::atof((char *) point_node->children->content);
- } else if (name == "oY") {
- oY = std::atof((char *) point_node->children->content);
- } else if (name == "oZ") {
- oZ = std::atof((char *) point_node->children->content);
- m_trainPoints.push_back(cv::Point3f((float) oX, (float) oY, (float) oZ));
- } else if (name == "desc") {
- xmlNodePtr descriptor_value_node = NULL;
- j = 0;
-
- for (descriptor_value_node = point_node->children;
- descriptor_value_node; descriptor_value_node =
- descriptor_value_node->next) {
-
- if (descriptor_value_node->type == XML_ELEMENT_NODE) {
- //Read descriptors values
- std::string parseStr((char *) descriptor_value_node->children->content);
- ss.clear();
- ss.str(parseStr);
-
- if(!ss.fail()) {
- switch(descriptorType) {
- case CV_8U:
- {
- //Parse the numeric value [0 ; 255] to an int
- int parseValue;
- ss >> parseValue;
- trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char) parseValue;
- }
- break;
-
- case CV_8S:
- //Parse the numeric value [-128 ; 127] to an int
- int parseValue;
- ss >> parseValue;
- trainDescriptorsTmp.at<char>(i, j) = (char) parseValue;
- break;
-
- case CV_16U:
- ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
- break;
-
- case CV_16S:
- ss >> trainDescriptorsTmp.at<short int>(i, j);
- break;
-
- case CV_32S:
- ss >> trainDescriptorsTmp.at<int>(i, j);
- break;
-
- case CV_32F:
- ss >> trainDescriptorsTmp.at<float>(i, j);
- break;
-
- case CV_64F:
- ss >> trainDescriptorsTmp.at<double>(i, j);
- break;
-
- default:
- ss >> trainDescriptorsTmp.at<float>(i, j);
- break;
- }
- } else {
- std::cerr << "Error when converting:" << ss.str() << std::endl;
- }
-
- j++;
- }
- }
- }
- }
- }
- i++;
- }
- }
-
- if(!append || m_trainDescriptors.empty()) {
- trainDescriptorsTmp.copyTo(m_trainDescriptors);
- } else {
- cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
- }
-
- /*free the document */
- xmlFreeDoc(doc);
-
- /*
- *Free the global variables that may
- *have been allocated by the parser.
- */
- xmlCleanupParser();
-#else
- std::cout << "Error: libxml2 is required !" << std::endl;
-#endif
- }
-
- //Convert OpenCV type to ViSP type for compatibility
- vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
- vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
-
- //Set _reference_computed to true as we load learning file
- _reference_computed = true;
-}
-
-/*!
- Match keypoints based on distance between their descriptors.
-
- \param trainDescriptors : Train descriptors (or reference descriptors).
- \param queryDescriptors : Query descriptors.
- \param matches : Output list of matches.
- \param elapsedTime : Elapsed time.
- */
-void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
- std::vector<cv::DMatch> &matches, double &elapsedTime) {
- double t = vpTime::measureTimeMs();
-
- if(m_useKnn) {
- m_knnMatches.clear();
- m_matcher->knnMatch(queryDescriptors, trainDescriptors, m_knnMatches, 2);
- matches.resize(m_knnMatches.size());
- std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
- } else {
- matches.clear();
- m_matcher->match(queryDescriptors, trainDescriptors, matches);
- }
- elapsedTime = vpTime::measureTimeMs() - t;
-}
-
-/*!
- Match keypoints detected in the image with those built in the reference list.
-
- \param I : Input current image.
- \return The number of matched keypoints.
- */
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) {
- return matchPoint(I, vpRect());
-}
-
-/*!
- Match keypoints detected in a region of interest of the image with those
- built in the reference list.
-
- \param I : Input image
- \param iP : Coordinate of the top-left corner of the region of interest
- \param height : Height of the region of interest
- \param width : Width of the region of interest
- \return The number of matched keypoints
- */
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) {
- return matchPoint(I, vpRect(iP, width, height));
-}
-
-/*!
- Match keypoints detected in a region of interest of the image with those
- built in the reference list.
-
- \param I : Input image
- \param rectangle : Rectangle of the region of interest
- \return The number of matched keypoints
- */
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle) {
- if(m_trainDescriptors.empty()) {
- std::cerr << "Reference is empty." << std::endl;
- if(!_reference_computed) {
- std::cerr << "Reference is not computed." << std::endl;
- }
- std::cerr << "Matching is not possible." << std::endl;
-
- return 0;
- }
-
- if(m_useAffineDetection) {
- std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
- std::vector<cv::Mat> listOfQueryDescriptors;
-
- //Detect keypoints and extract descriptors on multiple images
- detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
-
- //Flatten the different train lists
- m_queryKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
- it != listOfQueryKeyPoints.end(); ++it) {
- m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
- }
-
- bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
- if(first) {
- first = false;
- it->copyTo(m_queryDescriptors);
- } else {
- m_queryDescriptors.push_back(*it);
- }
- }
- } else {
- detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
- extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
- }
-
- match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
-
- if(m_filterType != noFilterMatching) {
- m_queryFilteredKeyPoints.clear();
- m_objectFilteredPoints.clear();
- m_filteredMatches.clear();
-
- filterMatches();
- } else {
- m_queryFilteredKeyPoints = m_queryKeyPoints;
- m_objectFilteredPoints = m_trainPoints;
- m_filteredMatches = m_matches;
-
- m_matchQueryToTrainKeyPoints.clear();
- for (size_t i = 0; i < m_matches.size(); i++) {
- m_matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
- m_queryKeyPoints[(size_t) m_matches[i].queryIdx],
- m_trainKeyPoints[(size_t) m_matches[i].trainIdx]));
- }
- }
-
- //Convert OpenCV type to ViSP type for compatibility
- vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
- vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
-
- return static_cast<unsigned int>(m_filteredMatches.size());
-}
-
-/*!
- Match keypoints detected in the image with those built in the reference list and compute the pose.
-
- \param I : Input image
- \param cam : Camera parameters
- \param cMo : Homogeneous matrix between the object frame and the camera frame
- \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
- the estimated pose
- \param elapsedTime : Time to detect, extract, match and compute the pose
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- \return True if the matching and the pose estimation are OK, false otherwise
- */
-bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
- //Check if we have training descriptors
- if(m_trainDescriptors.empty()) {
- std::cerr << "Reference is empty." << std::endl;
- if(!_reference_computed) {
- std::cerr << "Reference is not computed." << std::endl;
- }
- std::cerr << "Matching is not possible." << std::endl;
-
- return false;
- }
-
- if(m_useAffineDetection) {
- std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
- std::vector<cv::Mat> listOfQueryDescriptors;
-
- //Detect keypoints and extract descriptors on multiple images
- detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
-
- //Flatten the different train lists
- m_queryKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
- it != listOfQueryKeyPoints.end(); ++it) {
- m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
- }
-
- bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
- if(first) {
- first = false;
- it->copyTo(m_queryDescriptors);
- } else {
- m_queryDescriptors.push_back(*it);
- }
- }
- } else {
- detect(I, m_queryKeyPoints, m_detectionTime);
- extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
- }
-
- match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
-
- elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;
-
- if(m_filterType != noFilterMatching) {
- m_queryFilteredKeyPoints.clear();
- m_objectFilteredPoints.clear();
- m_filteredMatches.clear();
-
- filterMatches();
- } else {
- m_queryFilteredKeyPoints = m_queryKeyPoints;
- m_objectFilteredPoints = m_trainPoints;
- m_filteredMatches = m_matches;
-
- m_matchQueryToTrainKeyPoints.clear();
- for (size_t i = 0; i < m_matches.size(); i++) {
- m_matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
- m_queryKeyPoints[(size_t) m_matches[i].queryIdx],
- m_trainKeyPoints[(size_t) m_matches[i].trainIdx]));
- }
- }
-
- //Convert OpenCV type to ViSP type for compatibility
- vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
- vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
-
- //error = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- error = DBL_MAX;
- m_ransacInliers.clear();
- m_ransacOutliers.clear();
-
- if(m_useRansacVVS) {
- std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
- size_t cpt = 0;
- //Create a list of vpPoint with 2D coordinates (current keypoint location) + 3D coordinates (world/object coordinates)
- for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
- ++it, cpt++) {
- vpPoint pt;
- pt.setWorldCoordinates(it->x, it->y, it->z);
-
- vpImagePoint imP(m_queryFilteredKeyPoints[cpt].pt.y, m_queryFilteredKeyPoints[cpt].pt.x);
-
- double x = 0.0, y = 0.0;
- vpPixelMeterConversion::convertPoint(cam, imP, x, y);
- pt.set_x(x);
- pt.set_y(y);
-
- objectVpPoints[cpt] = pt;
- }
-
- std::vector<vpPoint> inliers;
- bool res = computePose(objectVpPoints, cMo, inliers, m_poseTime, func);
- m_ransacInliers.resize(inliers.size());
- for(size_t i = 0; i < m_ransacInliers.size(); i++) {
- vpMeterPixelConversion::convertPoint(cam, inliers[i].get_x(), inliers[i].get_y(), m_ransacInliers[i]);
- }
-
- elapsedTime += m_poseTime;
-
- return res;
- } else {
- std::vector<cv::Point2f> imageFilteredPoints;
- cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints);
- std::vector<int> inlierIndex;
- bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime);
-
- std::map<int, bool> mapOfInlierIndex;
- m_matchRansacKeyPointsToPoints.clear();
- m_matchRansacQueryToTrainKeyPoints.clear();
- for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
- m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
- m_objectFilteredPoints[(size_t)(*it)]));
- m_matchRansacQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(m_queryFilteredKeyPoints[(size_t)(*it)],
- m_trainKeyPoints[(size_t)m_matches[(size_t)(*it)].trainIdx]));
- mapOfInlierIndex[*it] = true;
- }
-
- for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
- if(mapOfInlierIndex.find((int) i) == mapOfInlierIndex.end()) {
- m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
- }
- }
-
- error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
-
- m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
- std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
- matchRansacToVpImage);
-
- elapsedTime += m_poseTime;
-
- return res;
- }
-}
-
-/*!
- Match keypoints detected in the image with those built in the reference list and return the bounding box and the center
- of gravity.
-
- \param I : Input image
- \param boundingBox : Bounding box that contains the good matches
- \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
- the bounding box)
- \param isPlanarObject : If the object is planar, the homography matrix is estimated to eliminate outliers, otherwise
- it is the fundamental matrix which is estimated
- \param imPts1 : Pointer to the list of reference keypoints if not null
- \param imPts2 : Pointer to the list of current keypoints if not null
- \param meanDescriptorDistance : Pointer to the value of the average distance of the descriptors if not null
- \param detectionScore : Pointer to the value of the detection score if not null
- \return True if the object is present, false otherwise
- */
-bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
- double *meanDescriptorDistance, double *detectionScore) {
- if(imPts1 != NULL && imPts2 != NULL) {
- imPts1->clear();
- imPts2->clear();
- }
-
- matchPoint(I);
-
- double meanDescriptorDistanceTmp = 0.0;
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- meanDescriptorDistanceTmp += (double) it->distance;
- }
-
- meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
- double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;
-
- if(meanDescriptorDistance != NULL) {
- *meanDescriptorDistance = meanDescriptorDistanceTmp;
- }
- if(detectionScore != NULL) {
- *detectionScore = score;
- }
-
- if(m_filteredMatches.size() >= 4) {
- //Training / Reference 2D points
- std::vector<cv::Point2f> points1(m_filteredMatches.size());
- //Query / Current 2D points
- std::vector<cv::Point2f> points2(m_filteredMatches.size());
-
- for(size_t i = 0; i < m_filteredMatches.size(); i++) {
- points1[i] = cv::Point2f(m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx].pt);
- points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx].pt);
- }
-
- std::vector<vpImagePoint> inliers;
- if(isPlanarObject) {
-#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
- cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
-#else
- cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
-#endif
-
- for(size_t i = 0; i < m_filteredMatches.size(); i++ ) {
- //Compute reprojection error
- cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
- realPoint.at<double>(0,0) = points1[i].x;
- realPoint.at<double>(1,0) = points1[i].y;
- realPoint.at<double>(2,0) = 1.f;
-
- cv::Mat reprojectedPoint = homographyMatrix * realPoint;
- double err_x = (reprojectedPoint.at<double>(0,0) / reprojectedPoint.at<double>(2,0)) - points2[i].x;
- double err_y = (reprojectedPoint.at<double>(1,0) / reprojectedPoint.at<double>(2,0)) - points2[i].y;
- double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);
-
- if(reprojectionError < 6.0) {
- inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
- if(imPts1 != NULL) {
- imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
- }
-
- if(imPts2 != NULL) {
- imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
- }
- }
- }
- } else if(m_filteredMatches.size() >= 8) {
- cv::Mat fundamentalInliers;
- cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);
-
- for(size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
- if(fundamentalInliers.at<uchar>((int) i, 0)) {
- inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
-
- if(imPts1 != NULL) {
- imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
- }
-
- if(imPts2 != NULL) {
- imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
- }
- }
- }
- }
-
- if(!inliers.empty()) {
- //Build a polygon with the list of inlier keypoints detected in the current image to get the bounding box
- vpPolygon polygon(inliers);
- boundingBox = polygon.getBoundingBox();
-
- //Compute the center of gravity
- double meanU = 0.0, meanV = 0.0;
- for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
- meanU += it->get_u();
- meanV += it->get_v();
- }
-
- meanU /= (double) inliers.size();
- meanV /= (double) inliers.size();
-
- centerOfGravity.set_u(meanU);
- centerOfGravity.set_v(meanV);
- }
- } else {
- //Too few matches
- return false;
- }
-
- if(m_detectionMethod == detectionThreshold) {
- return meanDescriptorDistanceTmp < m_detectionThreshold;
- } else {
- return score > m_detectionScore;
- }
-}
-
-/*!
- Match keypoints detected in the image with those built in the reference list, compute the pose and return also
- the bounding box and the center of gravity.
-
- \param I : Input image
- \param cam : Camera parameters
- \param cMo : Homogeneous matrix between the object frame and the camera frame
- \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
- the estimated pose
- \param elapsedTime : Time to detect, extract, match and compute the pose
- \param boundingBox : Bounding box that contains the good matches
- \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
- the bounding box)
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- \return True if the matching and the pose estimation are OK, false otherwise.
- */
-bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- bool (*func)(vpHomogeneousMatrix *)) {
- bool isMatchOk = matchPoint(I, cam, cMo, error, elapsedTime, func);
- if(isMatchOk) {
- //Use the pose estimated to project the model points in the image
- vpPoint pt;
- vpImagePoint imPt;
- std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
- size_t cpt = 0;
- for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
- pt = *it;
- pt.project(cMo);
- vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), imPt);
- modelImagePoints[cpt] = imPt;
- }
-
- //Build a polygon with the list of model image points to get the bounding box
- vpPolygon polygon(modelImagePoints);
- boundingBox = polygon.getBoundingBox();
-
- //Compute the center of gravity of the current inlier keypoints
- double meanU = 0.0, meanV = 0.0;
- for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
- ++it) {
- meanU += it->get_u();
- meanV += it->get_v();
- }
-
- meanU /= (double) m_ransacInliers.size();
- meanV /= (double) m_ransacInliers.size();
-
- centerOfGravity.set_u(meanU);
- centerOfGravity.set_v(meanV);
- }
-
- return isMatchOk;
-}
-
-/*!
- Apply a set of affine transormations to the image, detect keypoints and
- reproject them into initial image coordinates.
- See http://www.ipol.im/pub/algo/my_affine_sift/ for the details.
- See https://github.com/Itseez/opencv/blob/master/samples/python2/asift.py for the Python implementation by Itseez
- and Matt Sheckells for the current implementation in C++.
- \param I : Input image
- \param listOfKeypoints : List of detected keypoints in the multiple images after affine transformations
- \param listOfDescriptors : Corresponding list of descriptors
- \param listOfAffineI : Optional parameter, list of images after affine transformations
- */
-void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
- std::vector<cv::Mat>& listOfDescriptors, std::vector<vpImage<unsigned char> > *listOfAffineI) {
- cv::Mat img;
- vpImageConvert::convert(I, img);
- listOfKeypoints.clear();
- listOfDescriptors.clear();
-
- for (int tl = 1; tl < 6; tl++) {
- double t = pow(2, 0.5 * tl);
- for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
- std::vector<cv::KeyPoint> keypoints;
- cv::Mat descriptors;
-
- cv::Mat timg, mask, Ai;
- img.copyTo(timg);
-
- affineSkew(t, phi, timg, mask, Ai);
-
-
- if(listOfAffineI != NULL) {
- cv::Mat img_disp;
- bitwise_and(mask, timg, img_disp);
- vpImage<unsigned char> tI;
- vpImageConvert::convert(img_disp, tI);
- listOfAffineI->push_back(tI);
- }
-#if 0
- cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
- cv::imshow( "Skew", img_disp );
- cv::waitKey(0);
-#endif
-
- for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
- std::vector<cv::KeyPoint> kp;
- it->second->detect(timg, kp, mask);
- keypoints.insert(keypoints.end(), kp.begin(), kp.end());
- }
-
- double elapsedTime;
- extract(timg, keypoints, descriptors, elapsedTime);
-
- for(unsigned int i = 0; i < keypoints.size(); i++) {
- cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
- cv::Mat kpt_t = Ai * cv::Mat(kpt);
- keypoints[i].pt.x = kpt_t.at<float>(0, 0);
- keypoints[i].pt.y = kpt_t.at<float>(1, 0);
- }
-
- listOfKeypoints.push_back(keypoints);
- listOfDescriptors.push_back(descriptors);
- }
- }
-}
-
-/*!
- Save the learning data in a file in XML or binary mode.
-
- \param filename : Path of the save file
- \param binaryMode : If true, the data are saved in binary mode, otherwise in XML mode
- \param saveTrainingImages : If true, save also the training images on disk
- */
-void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) {
- std::string parent = vpIoTools::getParent(filename);
- if(!parent.empty()) {
- vpIoTools::makeDirectory(parent);
- }
-
- std::map<int, std::string> mapOfImgPath;
- if(saveTrainingImages) {
- //Save the training image files in the same directory
- int cpt = 0;
-
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
- char buffer[4];
- sprintf(buffer, "%03d", cpt);
- std::stringstream ss;
- ss << "train_image_" << buffer << ".jpg";
- std::string filename_ = ss.str();
- mapOfImgPath[it->first] = filename_;
- vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + filename_);
- }
- }
-
- bool have3DInfo = m_trainPoints.size() > 0;
- if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
- throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !");
- }
-
- if(binaryMode) {
- std::ofstream file(filename.c_str(), std::ofstream::binary);
- if(!file.is_open()) {
- throw vpException(vpException::ioError, "Cannot create the file.");
- }
-
- //Write info about training images
- int nbImgs = (int) mapOfImgPath.size();
- file.write((char *)(&nbImgs), sizeof(nbImgs));
-
- for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
- //Write image_id
- int id = it->first;
- file.write((char *)(&id), sizeof(id));
-
- //Write image path
- std::string path = it->second;
- int length = (int) path.length();
- file.write((char *)(&length), sizeof(length));
-
- for(int cpt = 0; cpt < length; cpt++) {
- file.write((char *) (&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
- }
- }
-
- //Write if we have 3D point information
- int have3DInfoInt = have3DInfo ? 1 : 0;
- file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
-
-
- int nRows = m_trainDescriptors.rows,
- nCols = m_trainDescriptors.cols;
- int descriptorType = m_trainDescriptors.type();
-
- //Write the number of descriptors
- file.write((char *)(&nRows), sizeof(nRows));
-
- //Write the size of the descriptor
- file.write((char *)(&nCols), sizeof(nCols));
-
- //Write the type of the descriptor
- file.write((char *)(&descriptorType), sizeof(descriptorType));
-
- for (int i = 0; i < nRows; i++) {
- unsigned int i_ = (unsigned int) i;
- //Write u
- float u = m_trainKeyPoints[i_].pt.x;
- file.write((char *)(&u), sizeof(u));
-
- //Write v
- float v = m_trainKeyPoints[i_].pt.y;
- file.write((char *)(&v), sizeof(v));
-
- //Write size
- float size = m_trainKeyPoints[i_].size;
- file.write((char *)(&size), sizeof(size));
-
- //Write angle
- float angle = m_trainKeyPoints[i_].angle;
- file.write((char *)(&angle), sizeof(angle));
-
- //Write response
- float response = m_trainKeyPoints[i_].response;
- file.write((char *)(&response), sizeof(response));
-
- //Write octave
- int octave = m_trainKeyPoints[i_].octave;
- file.write((char *)(&octave), sizeof(octave));
-
- //Write class_id
- int class_id = m_trainKeyPoints[i_].class_id;
- file.write((char *)(&class_id), sizeof(class_id));
-
- //Write image_id
- int image_id = (saveTrainingImages && m_mapOfImageId.size() > 0) ? m_mapOfImageId[m_trainKeyPoints[i_].class_id] : -1;
- file.write((char *)(&image_id), sizeof(image_id));
-
- if(have3DInfo) {
- float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
- //Write oX
- file.write((char *)(&oX), sizeof(oX));
-
- //Write oY
- file.write((char *)(&oY), sizeof(oY));
-
- //Write oZ
- file.write((char *)(&oZ), sizeof(oZ));
- }
-
- for (int j = 0; j < nCols; j++) {
- //Write the descriptor value
- switch(descriptorType) {
- case CV_8U:
- file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)), sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
- break;
-
- case CV_8S:
- file.write((char *)(&m_trainDescriptors.at<char>(i, j)), sizeof(m_trainDescriptors.at<char>(i, j)));
- break;
-
- case CV_16U:
- file.write((char *)(&m_trainDescriptors.at<unsigned short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned short int>(i, j)));
- break;
-
- case CV_16S:
- file.write((char *)(&m_trainDescriptors.at<short int>(i, j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
- break;
-
- case CV_32S:
- file.write((char *)(&m_trainDescriptors.at<int>(i, j)), sizeof(m_trainDescriptors.at<int>(i, j)));
- break;
-
- case CV_32F:
- file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
- break;
-
- case CV_64F:
- file.write((char *)(&m_trainDescriptors.at<double>(i, j)), sizeof(m_trainDescriptors.at<double>(i, j)));
- break;
-
- default:
- file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
- break;
- }
- }
- }
-
-
- file.close();
- } else {
-#ifdef VISP_HAVE_XML2
- xmlDocPtr doc = NULL;
- xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
- descriptor_node = NULL, desc_node = NULL;
-
- /*
- * this initialize the library and check potential ABI mismatches
- * between the version it was compiled for and the actual shared
- * library used.
- */
- LIBXML_TEST_VERSION
-
- doc = xmlNewDoc(BAD_CAST "1.0");
- if (doc == NULL) {
- throw vpException(vpException::ioError, "Error with file " + filename);
- }
-
- root_node = xmlNewNode(NULL, BAD_CAST "LearningData");
- xmlDocSetRootElement(doc, root_node);
-
- std::stringstream ss;
-
- //Write the training images info
- image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL);
-
- for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
- image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg",
- BAD_CAST it->second.c_str());
- ss.str("");
- ss << it->first;
- xmlNewProp(image_info_node, BAD_CAST "image_id", BAD_CAST ss.str().c_str());
- }
-
- //Write information about descriptors
- descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorsInfo", NULL);
-
- int nRows = m_trainDescriptors.rows,
- nCols = m_trainDescriptors.cols;
- int descriptorType = m_trainDescriptors.type();
-
- //Write the number of rows
- ss.str("");
- ss << nRows;
- xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", BAD_CAST ss.str().c_str());
-
- //Write the number of cols
- ss.str("");
- ss << nCols;
- xmlNewChild(descriptors_info_node, NULL, BAD_CAST "ncols", BAD_CAST ss.str().c_str());
-
- //Write the descriptors type
- ss.str("");
- ss << descriptorType;
- xmlNewChild(descriptors_info_node, NULL, BAD_CAST "type", BAD_CAST ss.str().c_str());
-
- for (int i = 0; i < nRows; i++) {
- unsigned int i_ = (unsigned int) i;
- descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo",
- NULL);
-
- ss.str("");
- ss << m_trainKeyPoints[i_].pt.x;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "u",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].pt.y;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "v",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].size;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "size",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].angle;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "angle",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].response;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "response",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].octave;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "octave",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainKeyPoints[i_].class_id;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "class_id",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << ((saveTrainingImages && m_mapOfImageId.size() > 0) ? m_mapOfImageId[m_trainKeyPoints[i_].class_id] : -1);
- xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
- BAD_CAST ss.str().c_str());
-
- if (have3DInfo) {
- ss.str("");
- ss << m_trainPoints[i_].x;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "oX",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainPoints[i_].y;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "oY",
- BAD_CAST ss.str().c_str());
-
- ss.str("");
- ss << m_trainPoints[i_].z;
- xmlNewChild(descriptor_node, NULL, BAD_CAST "oZ",
- BAD_CAST ss.str().c_str());
- }
-
- desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST "desc", NULL);
-
- for (int j = 0; j < nCols; j++) {
- ss.str("");
-
- switch(descriptorType) {
- case CV_8U:
- {
- //Promote an unsigned char to an int
- //val_tmp holds the numeric value that will be written
- //We save the value in numeric form otherwise libxml2 will not be able to parse
- //A better solution could be possible
- int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
- ss << val_tmp;
- }
- break;
-
- case CV_8S:
- {
- //Promote a char to an int
- //val_tmp holds the numeric value that will be written
- //We save the value in numeric form otherwise libxml2 will not be able to parse
- //A better solution could be possible
- int val_tmp = m_trainDescriptors.at<char>(i, j);
- ss << val_tmp;
- }
- break;
-
- case CV_16U:
- ss << m_trainDescriptors.at<unsigned short int>(i, j);
- break;
-
- case CV_16S:
- ss << m_trainDescriptors.at<short int>(i, j);
- break;
-
- case CV_32S:
- ss << m_trainDescriptors.at<int>(i, j);
- break;
-
- case CV_32F:
- ss << m_trainDescriptors.at<float>(i, j);
- break;
-
- case CV_64F:
- ss << m_trainDescriptors.at<double>(i, j);
- break;
-
- default:
- ss << m_trainDescriptors.at<float>(i, j);
- break;
- }
- xmlNewChild(desc_node, NULL, BAD_CAST "val",
- BAD_CAST ss.str().c_str());
- }
- }
-
- xmlSaveFormatFileEnc(filename.c_str(), doc, "UTF-8", 1);
-
- /*free the document */
- xmlFreeDoc(doc);
-
- /*
- *Free the global variables that may
- *have been allocated by the parser.
- */
- xmlCleanupParser();
-#else
- std::cerr << "Error: libxml2 is required !" << std::endl;
-#endif
- }
-}
-
-#endif
diff --git a/src/key-point/vpKeyPoint.h b/src/key-point/vpKeyPoint.h
deleted file mode 100644
index 7d1e70f..0000000
--- a/src/key-point/vpKeyPoint.h
+++ /dev/null
@@ -1,861 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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 functionalities.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-#ifndef __vpKeyPoint_h__
-#define __vpKeyPoint_h__
-
-#include <algorithm> // std::transform
-#include <vector> // std::vector
-#include <stdlib.h> // srand, rand
-#include <time.h> // time
-#include <fstream> // std::ofstream
-#include <numeric> // std::accumulate
-#include <float.h> // DBL_MAX
-#include <map> // std::map
-#include <limits>
-
-#include <visp/vpConfig.h>
-#include <visp/vpBasicKeyPoint.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpPoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpPlane.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpPose.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpPolygon.h>
-#include <visp/vpXmlConfigParserKeyPoint.h>
-#include <visp/vpConvert.h>
-
-// Require at least OpenCV >= 2.1.1
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/calib3d/calib3d.hpp>
-
-#if defined(VISP_HAVE_OPENCV_XFEATURES2D) // OpenCV >= 3.0.0
-# include <opencv2/xfeatures2d.hpp>
-#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
-# include <opencv2/nonfree/nonfree.hpp>
-#endif
-
-#ifdef VISP_HAVE_XML2
-# include <libxml/xmlwriter.h>
-#endif
-
-/*!
- \class vpKeyPoint
-
- \brief Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV library.
- This class permits to use different types of detectors, extractors and matchers easily.
- So, the classical SIFT and SURF keypoints could be used, as well as ORB, FAST, (etc.) keypoints,
- depending of the version of OpenCV you use.
-
- \note Due to some patents, SIFT and SURF are packaged in an external module called nonfree module
- in OpenCV version before 3.0.0 and in xfeatures2d from 3.0.0. You have to check you have the
- corresponding module to use SIFT and SURF.
-
- The goal of this class is to provide a tool to match reference keypoints from a
- reference image (or train keypoints in OpenCV terminology) and detected keypoints from a current image (or query
- keypoints in OpenCV terminology).
-
- If you supply the corresponding 3D coordinates corresponding to the 2D coordinates of the reference keypoints,
- you can also estimate the pose of the object by matching a set of detected keypoints in the current image with
- the reference keypoints.
-
-
- If you use this class, the first thing you have to do is to build
- the reference keypoints by detecting keypoints in a reference image which contains the
- object to detect. Then you match keypoints detected in a current image with those detected in a reference image
- by calling matchPoint() methods.
- You can access to the lists of matched points thanks to the
- methods getMatchedPointsInReferenceImage() and
- getMatchedPointsInCurrentImage(). These two methods return a list of
- matched points. The nth element of the first list is matched with
- the nth element of the second list.
- To provide easy compatibility with OpenCV terminology, getTrainKeyPoints() give you access to the list
- of keypoints detected in train images (or reference images) and getQueryKeyPoints() give you access to the list
- of keypoints detected in a query image (or current image).
- The method getMatches() give you access to a list of cv::DMatch with the correspondence between the index of the
- train keypoints and the index of the query keypoints.
-
- The following small example shows how to use the class to do the matching between current and reference keypoints.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpKeyPoint.h>
-
-int main()
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020300)
- vpImage<unsigned char> Irefrence;
- vpImage<unsigned char> Icurrent;
-
- vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
- vpKeyPoint keypoint("ORB", "ORB", "BruteForce-Hamming", filterType);
-
- // First grab the reference image Irefrence
- // Add your code to load the reference image in Ireference
-
- // Build the reference ORB points.
- keypoint.buildReference(Irefrence);
-
- // Then grab another image which represents the current image Icurrent
-
- // Match points between the reference points and the ORB points computed in the current image.
- keypoint.matchPoint(Icurrent);
-
- // Display the matched points
- keypoint.display(Irefrence, Icurrent);
-#endif
-
- return (0);
-}
- \endcode
-
- It is also possible to build the reference keypoints in a region of interest (ROI) of an image
- and find keypoints to match in only a part of the current image. The small following example shows how to do this:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpKeyPoint.h>
-
-int main()
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020300)
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
-
- vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
- vpKeyPoint keypoint("ORB", "ORB", "BruteForce-Hamming", filterType);
-
- //First grab the reference image Irefrence
- //Add your code to load the reference image in Ireference
-
- //Select a part of the image by clincking on two points which define a rectangle
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Ireference, corners[i]);
- }
-
- //Build the reference ORB points.
- int nbrRef;
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrRef = keypoint.buildReference(Ireference, corners[0], height, width);
-
- //Then grab another image which represents the current image Icurrent
-
- //Select a part of the image by clincking on two points which define a rectangle
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Icurrent, corners[i]);
- }
-
- //Match points between the reference points and the ORB points computed in the current image.
- int nbrMatched;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrMatched = keypoint.matchPoint(Icurrent, corners[0], height, width);
-
- //Display the matched points
- keypoint.display(Ireference, Icurrent);
-#endif
-
- return(0);
-}
- \endcode
-
- This class is also described in \ref tutorial-matching.
-*/
-class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint {
-
-public:
-
- /*! Predefined filtering method identifier. */
- typedef enum {
- constantFactorDistanceThreshold, /*!< Keep all the points below a constant factor threshold. */
- stdDistanceThreshold, /*!< Keep all the points below a minimal distance + the standard deviation. */
- ratioDistanceThreshold, /*!< Keep all the points enough discriminated (the ratio distance between the two best matches is below the threshold). */
- stdAndRatioDistanceThreshold, /*!< Keep all the points which fall with the two conditions above. */
- noFilterMatching /*!< No filtering. */
- } vpFilterMatchingType;
-
- /*! Predefined detection method identifier. */
- typedef enum {
- detectionThreshold, /*!< The object is present if the average of the descriptor distances is below the threshold. */
- detectionScore /*!< Same condition than the previous but with a formula taking into account the number of matches,
- the object is present if the score is above the threshold. */
- } vpDetectionMethodType;
-
-
- vpKeyPoint(const std::string &detectorName="ORB", const std::string &extractorName="ORB",
- const std::string &matcherName="BruteForce-Hamming", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
- vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
- const std::string &matcherName="BruteForce", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
-
- unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect& rectangle);
-
- void buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoint,
- std::vector<cv::Point3f> &points3f, bool append=false);
- void buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoint,
- const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f, bool append=false);
-
- static void compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point);
-
- static void compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point);
-
- static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<cv::KeyPoint> &candidate, std::vector<vpPolygon> &polygons, std::vector<std::vector<vpPoint> > &roisPt,
- std::vector<cv::Point3f> &points, cv::Mat *descriptors=NULL);
-
- static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidate, std::vector<vpPolygon> &polygons, std::vector<std::vector<vpPoint> > &roisPt,
- std::vector<vpPoint> &points, cv::Mat *descriptors=NULL);
-
- bool computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
- const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex, double &elapsedTime,
- bool (*func)(vpHomogeneousMatrix *)=NULL);
-
- bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
-
- void createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
- void createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
-
- void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const vpRect& rectangle=vpRect());
- void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const cv::Mat &mask=cv::Mat());
-
- void detectExtractAffine(const vpImage<unsigned char> &I, std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
- std::vector<cv::Mat>& listOfDescriptors,
- std::vector<vpImage<unsigned char> > *listOfAffineI=NULL);
-
- void display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size=3);
- void display(const vpImage<unsigned char> &ICurrent, unsigned int size=3, const vpColor &color=vpColor::green);
-
- void displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
- unsigned int crossSize, unsigned int lineThickness=1,
- const vpColor &color=vpColor::green);
- void displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
- const std::vector<vpImagePoint> &ransacInliers=std::vector<vpImagePoint>(), unsigned int crossSize=3,
- unsigned int lineThickness=1);
-
- void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime);
- void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime);
-
- /*!
- Get the covariance matrix when estimating the pose using the Virtual Visual Servoing approach.
-
- \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
-
- \sa setCovarianceComputation
- */
- inline vpMatrix getCovarianceMatrix() const {
- if(!m_computeCovariance) {
- std::cout << "Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl;
- return vpMatrix();
- }
-
- if(m_computeCovariance && !m_useRansacVVS) {
- std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
- << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual Visual Servoing approach."
- << std::endl;
- return vpMatrix();
- }
-
- return m_covarianceMatrix;
- }
-
- /*!
- Get the elapsed time to compute the keypoint detection.
-
- \return The elapsed time.
- */
- inline double getDetectionTime() const {
- return m_detectionTime;
- }
-
- /*!
- Get the elapsed time to compute the keypoint extraction.
-
- \return The elapsed time.
- */
- inline double getExtractionTime() const {
- return m_extractionTime;
- }
-
- /*!
- Get the elapsed time to compute the matching.
-
- \return The elapsed time.
- */
- inline double getMatchingTime() const {
- return m_matchingTime;
- }
-
- /*!
- Get the list of matches (correspondences between the indexes of the detected keypoints and the train keypoints).
-
- \return The list of matches.
- */
- inline std::vector<cv::DMatch> getMatches() const {
- return m_filteredMatches;
- }
-
- /*!
- Get the list of pairs with the correspondence between the matched query and train keypoints.
-
- \return The list of pairs with the correspondence between the matched query and train keypoints.
- */
- inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const {
- return m_matchQueryToTrainKeyPoints;
- }
-
- /*!
- Get the number of train images.
-
- \return The number of train images.
- */
- inline unsigned int getNbImages() const {
- return static_cast<unsigned int>(m_mapOfImages.size());
- }
-
- void getObjectPoints(std::vector<cv::Point3f> &objectPoints) const;
- void getObjectPoints(std::vector<vpPoint> &objectPoints) const;
-
- /*!
- Get the elapsed time to compute the pose.
-
- \return The elapsed time.
- */
- inline double getPoseTime() const {
- return m_poseTime;
- }
-
- /*!
- Get the descriptors matrix for the query keypoints.
-
- \return Matrix with descriptors values at each row for each query keypoints.
- */
- inline cv::Mat getQueryDescriptors() const {
- return m_queryDescriptors;
- }
-
- void getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
- void getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
-
- /*!
- Get the list of Ransac inliers.
-
- \return The list of Ransac inliers.
- */
- inline std::vector<vpImagePoint> getRansacInliers() const {
- return m_ransacInliers;
- }
-
- /*!
- Get the list of Ransac outliers.
-
- \return The list of Ransac outliers.
- */
- inline std::vector<vpImagePoint> getRansacOutliers() const {
- return m_ransacOutliers;
- }
-
- /*!
- Get the train descriptors matrix.
-
- \return : Matrix with descriptors values at each row for each train keypoints (or reference keypoints).
- */
- inline cv::Mat getTrainDescriptors() const {
- return m_trainDescriptors;
- }
-
- void getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
- void getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
-
- void getTrainPoints(std::vector<cv::Point3f> &points) const;
- void getTrainPoints(std::vector<vpPoint> &points) const;
-
- void initMatcher(const std::string &matcherName);
-
- void insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
- vpImage<unsigned char> &IMatching);
- void insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
-
-#ifdef VISP_HAVE_XML2
- void loadConfigFile(const std::string &configFile);
-#endif
-
- void loadLearningData(const std::string &filename, const bool binaryMode=false, const bool append=false);
-
- void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
- std::vector<cv::DMatch> &matches, double &elapsedTime);
-
- unsigned int matchPoint(const vpImage<unsigned char> &I);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
-
- bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
-
- bool matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- const bool isPlanarObject=true, std::vector<vpImagePoint> *imPts1=NULL,
- std::vector<vpImagePoint> *imPts2=NULL, double *meanDescriptorDistance=NULL,
- double *detectionScore=NULL);
-
- bool matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- bool (*func)(vpHomogeneousMatrix *)=NULL);
-
- void saveLearningData(const std::string &filename, const bool binaryMode=false, const bool saveTrainingImages=true);
-
- /*!
- Set if the covariance matrix has to be computed in the Virtual Visual Servoing approach.
-
- \param flag : True if the covariance has to be computed, false otherwise.
- */
- inline void setCovarianceComputation(const bool& flag) {
- m_computeCovariance = flag;
- if(!m_useRansacVVS) {
- std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
- << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual "
- "Visual Servoing approach." << std::endl;
- }
- }
-
- /*!
- Set the method to decide if the object is present or not.
-
- \param method : Detection method (detectionThreshold or detectionScore).
- */
- inline void setDetectionMethod(const vpDetectionMethodType &method) {
- m_detectionMethod = method;
- }
-
- /*!
- Set and initialize a detector denominated by his name \p detectorName.
-
- \param detectorName : Name of the detector.
- */
- inline void setDetector(const std::string &detectorName) {
- m_detectorNames.clear();
- m_detectorNames.push_back(detectorName);
- m_detectors.clear();
- initDetector(detectorName);
- }
-
- /*!
- Template function to set to a \p parameterName a value for a specific detector named by his \p detectorName.
-
- \param detectorName : Name of the detector
- \param parameterName : Name of the parameter
- \param value : Value to set
- */
- template<typename T1, typename T2, typename T3> inline void setDetectorParameter(const T1 detectorName,
- const T2 parameterName, const T3 value) {
- if(m_detectors.find(detectorName) != m_detectors.end()) {
- m_detectors[detectorName]->set(parameterName, value);
- }
- }
-
- /*!
- Set and initialize a list of detectors denominated by their names \p detectorNames.
-
- \param detectorNames : List of detector names.
- */
- inline void setDetectors(const std::vector<std::string> &detectorNames) {
- m_detectorNames.clear();
- m_detectors.clear();
- m_detectorNames = detectorNames;
- initDetectors(m_detectorNames);
- }
-
- /*!
- Set and initialize an extractor denominated by his name \p extractorName.
-
- \param extractorName : Name of the extractor.
- */
- inline void setExtractor(const std::string &extractorName) {
- m_extractorNames.clear();
- m_extractorNames.push_back(extractorName);
- m_extractors.clear();
- initExtractor(extractorName);
- }
-
- /*!
- Template function to set to a \p parameterName a value for a specific extractor named by his \p extractorName.
-
- \param extractorName : Name of the extractor
- \param parameterName : Name of the parameter
- \param value : Value to set
- */
- template<typename T1, typename T2, typename T3> inline void setExtractorParameter(const T1 extractorName,
- const T2 parameterName, const T3 value) {
- if(m_extractors.find(extractorName) != m_extractors.end()) {
- m_extractors[extractorName]->set(parameterName, value);
- }
- }
-
- /*!
- Set and initialize a list of extractors denominated by their names \p extractorNames.
-
- \param extractorNames : List of extractor names.
- */
- inline void setExtractors(const std::vector<std::string> &extractorNames) {
- m_extractorNames.clear();
- m_extractorNames = extractorNames;
- m_extractors.clear();
- initExtractors(m_extractorNames);
- }
-
- /*!
- Set and initialize a matcher denominated by his name \p matcherName.
- The different matchers are:
- - BruteForce (it uses L2 distance)
- - BruteForce-L1
- - BruteForce-Hamming
- - BruteForce-Hamming(2)
- - FlannBased
-
- L1 and L2 norms are preferable choices for SIFT and SURF descriptors, NORM_HAMMING should be used with ORB,
- BRISK and BRIEF, NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4.
-
- \param matcherName : Name of the matcher.
- */
- inline void setMatcher(const std::string &matcherName) {
- m_matcherName = matcherName;
- initMatcher(m_matcherName);
- }
-
- /*!
- Set the filtering method to eliminate false matching.
- The different methods are:
- - constantFactorDistanceThreshold (keep matches whose the descriptor distance is below dist_min * factor)
- - stdDistanceThreshold (keep matches whose the descriptor distance is below dist_min + standard_deviation)
- - ratioDistanceThreshold (keep matches enough discriminated: the ratio distance between the 2 best matches is below the threshold)
- - stdAndRatioDistanceThreshold (keep matches that agree with at least one of the two conditions)
- - noFilterMatching
-
- \param filterType : Type of the filtering method
- */
- inline void setFilterMatchingType(const vpFilterMatchingType &filterType) {
- m_filterType = filterType;
-
- //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
- //So this is useful only for ratioDistanceThreshold method
- if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
- m_useKnn = true;
- } else {
- m_useKnn = false;
- }
- }
-
- /*!
- Set the factor value for the filtering method: constantFactorDistanceThreshold.
-
- \param factor : Factor value
- */
- inline void setMatchingFactorThreshold(const double factor) {
- if(factor > 0.0) {
- m_matchingFactorThreshold = factor;
- } else {
- throw vpException(vpException::badValue, "The factor must be positive.");
- }
- }
-
- /*!
- Set the ratio value for the filtering method: ratioDistanceThreshold.
-
- \param ratio : Ratio value (]0 ; 1])
- */
- inline void setMatchingRatioThreshold(const double ratio) {
- if(ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits<double>::epsilon())) {
- m_matchingRatioThreshold = ratio;
- } else {
- throw vpException(vpException::badValue, "The ratio must be in the interval ]0 ; 1].");
- }
- }
-
- /*!
- Set the percentage value for defining the cardinality of the consensus group.
-
- \param percentage : Percentage value (]0 ; 100])
- */
- inline void setRansacConsensusPercentage(const double percentage) {
- if(percentage > 0.0 && (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits<double>::epsilon())) {
- m_ransacConsensusPercentage = percentage;
- } else {
- throw vpException(vpException::badValue, "The percentage must be in the interval ]0 ; 100].");
- }
- }
-
- /*!
- Set the maximum number of iterations for the Ransac pose estimation method.
-
- \param nbIter : Maximum number of iterations for the Ransac
- */
- inline void setRansacIteration(const int nbIter) {
- if(nbIter > 0) {
- m_nbRansacIterations = nbIter;
- } else {
- throw vpException(vpException::badValue, "The number of iterations must be greater than zero.");
- }
- }
-
- /*!
- Set the maximum reprojection error (in pixel) to determine if a point is an inlier or not.
-
- \param reprojectionError : Maximum reprojection error in pixel (used by OpenCV function)
- */
- inline void setRansacReprojectionError(const double reprojectionError) {
- if(reprojectionError > 0.0) {
- m_ransacReprojectionError = reprojectionError;
- } else {
- throw vpException(vpException::badValue, "The Ransac reprojection threshold must be positive as we deal with distance.");
- }
- }
-
- /*!
- Set the minimum number of inlier for the Ransac pose estimation method.
-
- \param minCount : Minimum number of inlier for the consensus
- */
- inline void setRansacMinInlierCount(const int minCount) {
- if(minCount > 0) {
- m_nbRansacMinInlierCount = minCount;
- } else {
- throw vpException(vpException::badValue, "The minimum number of inliers must be greater than zero.");
- }
- }
-
- /*!
- Set the maximum error (in meter) to determine if a point is an inlier or not.
-
- \param threshold : Maximum error in meter for ViSP function
- */
- inline void setRansacThreshold(const double threshold) {
- if(threshold > 0.0) {
- m_ransacThreshold = threshold;
- } else {
- throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
- }
- }
-
- /*!
- Set if multiple affine transformations must be used to detect and extract keypoints.
-
- \param useAffine : True to use multiple affine transformations, false otherwise
- */
- inline void setUseAffineDetection(const bool useAffine) {
- m_useAffineDetection = useAffine;
- }
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
- /*!
- Set if cross check method must be used to eliminate some false matches with a brute-force matching method.
-
- \param useCrossCheck : True to use cross check, false otherwise
- */
- inline void setUseBruteForceCrossCheck(const bool useCrossCheck) {
- //Only available with BruteForce and with k=1 (i.e not used with a ratioDistanceThreshold method)
- if(m_matcher != NULL && !m_useKnn && m_matcherName == "BruteForce") {
- m_matcher->set("crossCheck", useCrossCheck);
- }
- }
-#endif
-
- /*!
- Set the flag to choose between a percentage value of inliers for the cardinality of the consensus group
- or a minimum number.
-
- \param usePercentage : True to a percentage ratio of inliers, otherwise use a specified number of inliers
- */
- inline void setUseRansacConsensusPercentage(const bool usePercentage) {
- m_useConsensusPercentage = usePercentage;
- }
-
- /*!
- Set the flag to choose between the OpenCV or ViSP Ransac pose estimation function.
-
- \param ransacVVS : True to use ViSP function, otherwise use OpenCV function
- */
- inline void setUseRansacVVS(const bool ransacVVS) {
- m_useRansacVVS = ransacVVS;
- }
-
-private:
- //! If true, compute covariance matrix if the user select the pose estimation method using ViSP
- bool m_computeCovariance;
- //! Covariance matrix
- vpMatrix m_covarianceMatrix;
- //! Current id associated to the training image used for the learning.
- int m_currentImageId;
- //! Method (based on descriptor distances) to decide if the object is present or not.
- vpDetectionMethodType m_detectionMethod;
- //! Detection score to decide if the object is present or not.
- double m_detectionScore;
- //! Detection threshold based on average of descriptor distances to decide if the object is present or not.
- double m_detectionThreshold;
- //! Elapsed time to detect keypoints.
- double m_detectionTime;
- //! List of detector names.
- std::vector<std::string> m_detectorNames;
- //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) detectors,
- // with a key based upon the detector name.
- std::map<std::string, cv::Ptr<cv::FeatureDetector> > m_detectors;
- //! Elapsed time to extract descriptors for the detected keypoints.
- double m_extractionTime;
- //! List of extractor name.
- std::vector<std::string> m_extractorNames;
- //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) extractors,
- // with a key based upon the extractor name.
- std::map<std::string, cv::Ptr<cv::DescriptorExtractor> > m_extractors;
- //! List of filtered matches between the detected and the trained keypoints.
- std::vector<cv::DMatch> m_filteredMatches;
- //! Chosen method of filtering to eliminate false matching.
- vpFilterMatchingType m_filterType;
- //! List of k-nearest neighbors for each detected keypoints (if the method chosen is based upon on knn).
- std::vector<std::vector<cv::DMatch> > m_knnMatches;
- //! Map of image id to know to which training image is related a training keypoints.
- std::map<int, int> m_mapOfImageId;
- //! Map of images to have access to the image buffer according to his image id.
- std::map<int, vpImage<unsigned char> > m_mapOfImages;
- //! Smart reference-counting pointer (similar to shared_ptr in Boost) of descriptor matcher (e.g. BruteForce or FlannBased).
- cv::Ptr<cv::DescriptorMatcher> m_matcher;
- //! Name of the matcher.
- std::string m_matcherName;
- //! List of matches between the detected and the trained keypoints.
- std::vector<cv::DMatch> m_matches;
- //! Factor value for the filtering method: constantFactorDistanceThreshold.
- double m_matchingFactorThreshold;
- //! Ratio value for the filtering method: ratioDistanceThreshold.
- double m_matchingRatioThreshold;
- //! Elapsed time to do the matching.
- double m_matchingTime;
- //! List of pairs between the matched query and train keypoints.
- std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > m_matchQueryToTrainKeyPoints;
- //! List of pairs between the keypoint and the 3D point after the Ransac.
- std::vector<std::pair<cv::KeyPoint, cv::Point3f> > m_matchRansacKeyPointsToPoints;
- //! List of pairs between the matched query and train keypoints after the Ransac.
- std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > m_matchRansacQueryToTrainKeyPoints;
- //! Maximum number of iterations for the Ransac method.
- int m_nbRansacIterations;
- //! Minimum number of inliers for the Ransac method.
- int m_nbRansacMinInlierCount;
- //! List of 3D points (in the object frame) filtered after the matching to compute the pose.
- std::vector<cv::Point3f> m_objectFilteredPoints;
- //! Elapsed time to compute the pose.
- double m_poseTime;
- /*! Matrix of descriptors (each row contains the descriptors values for each keypoints
- detected in the current image). */
- cv::Mat m_queryDescriptors;
- //! List of detected keypoints filtered after the matching.
- std::vector<cv::KeyPoint> m_queryFilteredKeyPoints;
- //! List of keypoints detected in the current image.
- std::vector<cv::KeyPoint> m_queryKeyPoints;
- //! Percentage value to determine the number of inliers for the Ransac method.
- double m_ransacConsensusPercentage;
- //! List of inliers.
- std::vector<vpImagePoint> m_ransacInliers;
- //! List of outliers.
- std::vector<vpImagePoint> m_ransacOutliers;
- //! Maximum reprojection error (in pixel for the OpenCV method) to decide if a point is an inlier or not.
- double m_ransacReprojectionError;
- //! Maximum error (in meter for the ViSP method) to decide if a point is an inlier or not.
- double m_ransacThreshold;
- //! Matrix of descriptors (each row contains the descriptors values for each keypoints
- //detected in the train images).
- cv::Mat m_trainDescriptors;
- //! List of keypoints detected in the train images.
- std::vector<cv::KeyPoint> m_trainKeyPoints;
- //! List of 3D points (in the object frame) corresponding to the train keypoints.
- std::vector<cv::Point3f> m_trainPoints;
- //! List of 3D points in vpPoint format (in the object frame) corresponding to the train keypoints.
- std::vector<vpPoint> m_trainVpPoints;
- //! If true, use multiple affine transformations to cober the 6 affine parameters
- bool m_useAffineDetection;
- //! If true, some false matches will be eliminate by keeping only pairs (i,j) such that for i-th
- //! query descriptor the j-th descriptor in the matcher’s collection is the nearest and vice versa.
- bool m_useBruteForceCrossCheck;
- //! Flag set if a percentage value is used to determine the number of inliers for the Ransac method.
- bool m_useConsensusPercentage;
- //! Flag set if a knn matching method must be used.
- bool m_useKnn;
- //! Flag set if a Ransac VVS pose estimation must be used.
- bool m_useRansacVVS;
-
-
- void affineSkew(double tilt, double phi, cv::Mat& img, cv::Mat& mask, cv::Mat& Ai);
-
- double computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est);
-
- void filterMatches();
-
- void init();
- void initDetector(const std::string &detectorNames);
- void initDetectors(const std::vector<std::string> &detectorNames);
-
- void initExtractor(const std::string &extractorName);
- void initExtractors(const std::vector<std::string> &extractorNames);
-
-//TODO: Try to implement a pyramidal feature detection
-//#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
-// void pyramidFeatureDetection(cv::Ptr<cv::FeatureDetector> &detector, const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask,
-// const int maxLevel=2);
-// void runByPixelsMask(std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask);
-//#endif
-};
-
-#endif
-#endif
diff --git a/src/key-point/vpKeyPointSurf.cpp b/src/key-point/vpKeyPointSurf.cpp
deleted file mode 100644
index f75051d..0000000
--- a/src/key-point/vpKeyPointSurf.cpp
+++ /dev/null
@@ -1,783 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyPointSurf.cpp 5202 2015-01-24 09:29:06Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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 Surf.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#include <visp/vpKeyPointSurf.h>
-
-#if defined (VISP_HAVE_OPENCV_NONFREE)
-#if (VISP_HAVE_OPENCV_VERSION >= 0x010100) && VISP_HAVE_OPENCV_VERSION < 0x030000// Require opencv >= 1.1.0 < 3.0.0
-
-#include <visp/vpImageConvert.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDebug.h>
-
-#include <ctype.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <vector>
-#include <list>
-
-double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
-int naiveNearestNeighbor( const float *vec, int laplacian,
- const CvSeq *model_keypoints,
- const CvSeq *model_descriptors );
-int naiveNearestNeighbor( const float *vec,
- const CvSeq *ref_keypoints,
- const CvSeq *ref_descriptors );
-void findPairs( const CvSeq* objectKeypoints,
- const CvSeq* objectDescriptors,
- const CvSeq* imageKeypoints,
- const CvSeq* imageDescriptors,
- std::vector<int>& ptpairs );
-
-// Compare two surf descriptors.
-double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
-{
- double total_cost = 0;
- int i;
- assert( length % 4 == 0 );
- for( i = 0; i < length; i += 4 )
- {
- double t0 = d1[i] - d2[i];
- double t1 = d1[i+1] - d2[i+1];
- double t2 = d1[i+2] - d2[i+2];
- double t3 = d1[i+3] - d2[i+3];
- total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
- if( total_cost > best )
- break;
- }
- return total_cost;
-}
-
-
-//Find for a point candidate the most similar point in the reference point list.
-int naiveNearestNeighbor( const float *vec, int laplacian,
- const CvSeq *model_keypoints,
- const CvSeq *model_descriptors )
-{
- int length = (int)(model_descriptors->elem_size/(int)sizeof(float));
- int i, neighbor = -1;
- double d, dist1 = 1e6, dist2 = 1e6;
- CvSeqReader reader, kreader;
- cvStartReadSeq( model_keypoints, &kreader, 0 );
- cvStartReadSeq( model_descriptors, &reader, 0 );
-
- for( i = 0; i < model_descriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* mvec = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- if( laplacian != kp->laplacian )
- continue;
- d = compareSURFDescriptors( vec, mvec, dist2, length );
- if( d < dist1 )
- {
- dist2 = dist1;
- dist1 = d;
- neighbor = i;
- }
- else if ( d < dist2 )
- dist2 = d;
- }
- if ( dist1 < 0.6*dist2 )
- return neighbor;
- return -1;
-}
-
-
-//Find for a point candidate the most similar point in the reference point list.
-int naiveNearestNeighbor( const float *vec,
- const CvSeq *ref_keypoints,
- const CvSeq *ref_descriptors )
-{
- int length = (int)(ref_descriptors->elem_size/(int)sizeof(float));
- int i, neighbor = -1;
- double d, dist1 = 1e6, dist2 = 1e6;
- CvSeqReader reader, kreader;
- cvStartReadSeq( ref_keypoints, &kreader, 0 );
- cvStartReadSeq( ref_descriptors, &reader, 0 );
-
- for( i = 0; i < ref_descriptors->total; i++ )
- {
- const float* mvec = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- d = compareSURFDescriptors( vec, mvec, dist2, length );
- if( d < dist1 )
- {
- dist2 = dist1;
- dist1 = d;
- neighbor = i;
- }
- else if ( d < dist2 )
- dist2 = d;
- }
- if ( dist1 < 0.6*dist2 )
- return neighbor;
- return -1;
-}
-
-
-
-//Find all the matched points
-void findPairs( const CvSeq* objectKeypoints,
- const CvSeq* objectDescriptors,
- const CvSeq* imageKeypoints,
- const CvSeq* imageDescriptors,
- std::vector<int>& ptpairs )
-{
- int i;
- CvSeqReader reader, kreader;
- cvStartReadSeq( objectKeypoints, &kreader );
- cvStartReadSeq( objectDescriptors, &reader );
- ptpairs.clear();
-
- for( i = 0; i < objectDescriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* descriptor = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- int nearest_neighbor = naiveNearestNeighbor( descriptor,
- kp->laplacian,
- imageKeypoints,
- imageDescriptors );
- if( nearest_neighbor >= 0 )
- {
- ptpairs.push_back(i);
- ptpairs.push_back(nearest_neighbor);
- }
- }
-}
-
-
-
-/*!
-
- Basic constructor. It sets by default the hessian threshold to 500
- (a good default value is between 300 and 500) and the descriptor
- type to extended.
-
-*/
-vpKeyPointSurf::vpKeyPointSurf()
- : vpBasicKeyPoint(),
- storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
- ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
-{
- init();
-}
-
-/*!
- Initialize any OpenCV parameters.
-*/
-void vpKeyPointSurf::init()
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
- cv::initModule_nonfree();
-#endif
-
- storage = cvCreateMemStorage(0);
- params = cvSURFParams(hessianThreshold, descriptorType);
-}
-
-/*!
- Basic Destructor
-
-*/
-vpKeyPointSurf::~vpKeyPointSurf()
-{
- cvReleaseMemStorage(&storage);
- if(storage_cur!=NULL){
- cvReleaseMemStorage(&storage_cur);
- }
-}
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made all over the image I.
-
- \param I : The gray scaled image where the reference points are computed.
-
- \return the number of reference points.
-*/
-unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
-{
- IplImage* model = NULL;
-
- if((I.getWidth() % 8) == 0){
- int height = (int)I.getHeight();
- int width = (int)I.getWidth();
- CvSize size = cvSize(width, height);
- model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
- model->imageData = (char*)I.bitmap;
- }else{
- vpImageConvert::convert(I,model);
- }
-
- cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
-
- const unsigned int nbPoints = (unsigned int)ref_keypoints->total;
-
- referenceImagePointsList.resize(nbPoints);
-
- for(unsigned int i = 0; i < nbPoints; i++ )
- {
- CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (int)i);
-
- referenceImagePointsList[i].set_i(r1->pt.y);
- referenceImagePointsList[i].set_j(r1->pt.x);
- }
-
- if((I.getWidth() % 8) == 0){
- model->imageData = NULL;
- cvReleaseImageHeader(&model);
- }else{
- cvReleaseImage(&model);
- }
-
- _reference_computed = true;
- return nbPoints;
-}
-
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made only on a part of the image. This part is a rectangle defined
- by its top left corner, its height and its width. The parameters of
- this rectangle must be given in pixel.
-
- \param I : The gray scaled image where the reference points are computed.
-
- \param iP : The top left corner of the rectangle.
-
- \param height : height of the rectangle (in pixel).
-
- \param width : width of the rectangle (in pixel).
-
- \return the number of reference points.
-*/
-unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width)
-{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth())
- {
- vpTRACE("Bad size for the subimage");
- throw(vpException(vpImageException::notInTheImage ,
- "Bad size for the subimage"));
- }
-
- vpImage<unsigned char> subImage;
-
- vpImageTools::createSubImage(I,
- (unsigned int)iP.get_i(),
- (unsigned int)iP.get_j(),
- height, width, subImage);
-
- unsigned int nbRefPoint = this->buildReference(subImage);
-
- for(unsigned int k = 0; k < nbRefPoint; k++)
- {
- (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i()
- + iP.get_i());
- (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j()
- + iP.get_j());
- }
- return(nbRefPoint);
-}
-
-
-/*!
-
- Build the list of reference points. The computation of the points is
- made only on a part of the image. This part is a rectangle. The
- parameters of this rectangle must be given in pixel.
-
- \param I : The gray scaled image where the reference points are computed.
-
- \param rectangle : The rectangle which defines the interesting part
- of the image.
-
- \return the number of reference points.
-*/
-unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle)
-{
- vpImagePoint iP;
- iP.set_i(rectangle.getTop());
- iP.set_j(rectangle.getLeft());
- return (this->buildReference(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
-}
-
-
-/*!
-
- Computes the SURF points in the current image I and try to matched
- them with the points in the reference list. Only the matched points
- are stored.
-
- \param I : The gray scaled image where the points are computed.
-
- \return the number of point which have been matched.
-*/
-unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
-{
- IplImage* currentImage = NULL;
-
- if((I.getWidth() % 8) == 0){
- int height = (int)I.getHeight();
- int width = (int)I.getWidth();
- CvSize size = cvSize(width, height);
- currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
- currentImage->imageData = (char*)I.bitmap;
- }else{
- vpImageConvert::convert(I,currentImage);
- }
-
- /* we release the memory storage for the current points (it has to be kept
- allocated for the get descriptor points, ...) */
- if(storage_cur != NULL){
- cvReleaseMemStorage(&storage_cur);
- storage_cur = NULL;
- }
- storage_cur = cvCreateMemStorage(0);
-
- cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
- storage_cur, params );
-
- CvSeqReader reader, kreader;
- cvStartReadSeq( ref_keypoints, &kreader );
- cvStartReadSeq( ref_descriptors, &reader );
-
-
- std::list<int> indexImagePair;
- std::list<int> indexReferencePair;
-
-
- unsigned int nbrPair = 0;
-
- for(int i = 0; i < ref_descriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* descriptor = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- int nearest_neighbor = naiveNearestNeighbor( descriptor,
- kp->laplacian,
- image_keypoints,
- image_descriptors );
- if( nearest_neighbor >= 0 )
- {
- indexReferencePair.push_back(i);
- indexImagePair.push_back(nearest_neighbor);
- nbrPair++;
- }
- }
-
- std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
- std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
-
- matchedReferencePoints.resize(0);
-
- if (nbrPair == 0)
- return (0);
-
- currentImagePointsList.resize(nbrPair);
- matchedReferencePoints.resize(nbrPair);
-
- for (unsigned int i = 0; i < nbrPair; i++)
- {
- int index = *indexImagePairIter;
-
- CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
-
- currentImagePointsList[i].set_i(r1->pt.y);
- currentImagePointsList[i].set_j(r1->pt.x);
-
- matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
-
-
- ++indexImagePairIter;
- ++indexReferencePairIter;
- }
-
- if((I.getWidth() % 8) == 0){
- currentImage->imageData = NULL;
- cvReleaseImageHeader(¤tImage);
- }else{
- cvReleaseImage(¤tImage);
- }
-
- return nbrPair;
-}
-
-
-/*!
-
- Computes the SURF points in only a part of the current image I and
- try to matched them with the points in the reference list. The part
- of the image is a rectangle defined by its top left corner, its
- height and its width. The parameters of this rectangle must be given
- in pixel. Only the matched points are stored.
-
- \param I : The gray scaled image where the points are computed.
-
- \param iP : The top left corner of the rectangle.
-
- \param height : height of the rectangle (in pixel).
-
- \param width : width of the rectangle (in pixel).
-
- \return the number of point which have been matched.
-*/
-unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width)
-{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth())
- {
- vpTRACE("Bad size for the subimage");
- throw(vpException(vpImageException::notInTheImage ,
- "Bad size for the subimage"));
- }
-
- vpImage<unsigned char> subImage;
-
- vpImageTools::createSubImage(I,
- (unsigned int)iP.get_i(),
- (unsigned int)iP.get_j(),
- height, width, subImage);
-
- unsigned int nbMatchedPoint = this->matchPoint(subImage);
-
- for(unsigned int k = 0; k < nbMatchedPoint; k++)
- {
- (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i()
- + iP.get_i());
- (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j()
- + iP.get_j());
- }
-
- return(nbMatchedPoint);
-}
-
-
-/*!
-
- Computes the SURF points in only a part of the current image I and
- try to matched them with the points in the reference list. The part
- of the image is a rectangle. The parameters of this rectangle must
- be given in pixel. Only the matched points are stored.
-
- \param I : The gray scaled image where the points are computed.
-
- \param rectangle : The rectangle which defines the interesting part
- of the image.
-
- \return the number of point which have been matched.
-*/
-unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle)
-{
- vpImagePoint iP;
- iP.set_i(rectangle.getTop());
- iP.set_j(rectangle.getLeft());
- return (this->matchPoint(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
-}
-
-
-/*!
-
- This function displays the matched reference points and the matched
- points computed in the current image. The reference points are
- displayed in the image Ireference and the matched points coming from
- the current image are displayed in the image Icurrent. It is
- possible to set Ireference and Icurrent with the same image when
- calling the method.
-
- \param Ireference : The image where the matched reference points are
- displayed.
-
- \param Icurrent : The image where the matched points computed in the
- current image are displayed.
-
- \param size : Size in pixels of the cross that is used to display matched points.
-
-*/
-void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference,
- const vpImage<unsigned char> &Icurrent, unsigned int size)
-{
-// matchedPointsCurrentImageList.front();
-// matchedPointsReferenceImageList.front();
-
-// if (matchedPointsCurrentImageList.nbElements()
-// != matchedPointsReferenceImageList.nbElements())
-// {
-// vpTRACE("Numbers of points mismatch");
-// throw(vpException(vpException::fatalError,"Numbers of points mismatch"));
-// }
-
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
- {
- vpDisplay::displayCross (Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
- vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, vpColor::green);
-// matchedPointsReferenceImageList.next();
-// matchedPointsCurrentImageList.next();
- }
-}
-
-
-/*!
-
- This function displays only the matched points computed in the
- current image. They are displayed in the image Icurrent.
-
- \param Icurrent : The image where the matched points computed in the
- current image are displayed.
-
- \param size : Size in pixels of the cross that is used to display matched points.
-
- \param color : Color used to display the matched points.
-*/
-void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
-{
-// matchedPointsCurrentImageList.front();
-//
-// vpImagePoint ipCur;
-//
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
- {
- vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, color);
- }
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
-
- \deprecated This method is deprecated, you should use
- matchPoint(std::list<float*> , std::list<int> )
- instead.
-
- Computes the SURF points given by their descriptor and laplacian and try to match
- them with the points in the reference list. Only the matched points
- are stored. The two lists must have the same number of element while they correspond
- the same unique list of point.
-
- \warning The list returned contains allocated data (2 int per element). Must be deleted to avoid memory leak.
-
- \param descriptorList : The list of the descriptor
-
- \param laplacianList : The list of laplacian
-
- \return the list of the pair, the first element contains the index in the reference sequence and the second element contains the index in the list given in parameter.
-*/
-vp_deprecated vpList<int*>* vpKeyPointSurf::matchPoint(vpList<float*> descriptorList, vpList<int> laplacianList)
-{
- vpList<int*>* pairPoints = new vpList<int*>;
-
- if(descriptorList.nb != laplacianList.nb){
- vpTRACE("Error, the two lists have different number of element");
- return pairPoints;
- }
-
- CvSeqReader reader;
- cvStartReadSeq( ref_descriptors, &reader );
-
- descriptorList.front();
- pairPoints->front();
- laplacianList.front();
- int indexList = 0;
- while(!descriptorList.outside()){
- float* descriptor = descriptorList.value();
-
- int nearest_neighbor = naiveNearestNeighbor( descriptor, laplacianList.value(), ref_keypoints, ref_descriptors);
-
- if(nearest_neighbor >= 0){
- int* tab;
- tab = new int[2];
- tab[0] = nearest_neighbor;
- tab[1] = indexList;
- pairPoints->addRight(tab);
- }
- indexList++;
- descriptorList.next();
- laplacianList.next();
- }
-
- return pairPoints;
-}
-#endif
-
-std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList)
-{
- std::list<int*>* pairPoints = new std::list<int*>;
-
- if(descriptorList.size() != laplacianList.size()){
- vpTRACE("Error, the two lists have different number of element");
- return pairPoints;
- }
-
- CvSeqReader reader;
- cvStartReadSeq( ref_descriptors, &reader );
-
- std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
- std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
- descriptorList.front();
- int indexList = 0;
- while(descriptorListIter != descriptorList.end()){
- float* descriptor = *descriptorListIter;
-
- int nearest_neighbor = naiveNearestNeighbor( descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
-
- if(nearest_neighbor >= 0){
- int* tab;
- tab = new int[2];
- tab[0] = nearest_neighbor;
- tab[1] = indexList;
- pairPoints->push_back(tab);
- }
- indexList++;
- ++descriptorListIter;
- ++laplacianListIter;
- }
-
- return pairPoints;
-}
-
-/*!
- Get the descriptor of the nth reference point.
-
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
-*/
-float* vpKeyPointSurf::getDescriptorReferencePoint (const int index)
-{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
- vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
- }
-
- float* descriptor = NULL;
-
- CvSeqReader reader;
- cvStartReadSeq( ref_descriptors, &reader );
-
- for(int j = 0; j < ref_descriptors->total; j++ ){
- if(j== index){
- descriptor = (float*)reader.ptr;
- break;
- }
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- }
-
- return descriptor;
-}
-
-/*!
- Get the laplacian of the nth reference point.
-
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
-*/
-int vpKeyPointSurf::getLaplacianReferencePoint (const int index)
-{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
- vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
- }
-
- CvSeqReader reader;
- cvStartReadSeq( ref_keypoints, &reader );
-
- int laplacian = 0;/* normally only -1, 0, +1 are possible */
-
- for(int j = 0; j < ref_keypoints->total; j++ ){
- if(j== index){
- const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
- laplacian = kp->laplacian;
- break;
- }
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- }
-
- return laplacian;
-}
-
-/*!
- Get the parameters of the descriptor of the nth reference point.
-
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
- \param size : The size of the point used to compute the descriptor.
- \param dir : The orientation of the descriptor (in degree).
-*/
-void vpKeyPointSurf::getDescriptorParamReferencePoint (const int index, int& size, float& dir)
-{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
- vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
- }
-
- CvSeqReader reader;
- cvStartReadSeq( ref_keypoints, &reader );
-
- for(int j = 0; j < ref_keypoints->total; j++ ){
- if(j== index){
- const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
- size = kp->size;
- dir = kp->dir;
- break;
- }
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- }
-}
-
-
-#endif
-#endif
-
diff --git a/src/key-point/vpKeyPointSurf.h b/src/key-point/vpKeyPointSurf.h
deleted file mode 100644
index 4b7035e..0000000
--- a/src/key-point/vpKeyPointSurf.h
+++ /dev/null
@@ -1,313 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyPointSurf.h 5205 2015-01-26 08:56:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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 Surf.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#ifndef vpKeyPointSurf_H
-#define vpKeyPointSurf_H
-
-/*!
- \file vpKeyPointSurf.h
-
- \brief Class that implements the SURF key points and technics thanks
- to the OpenCV library.
-*/
-
-#include <visp/vpBasicKeyPoint.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-#include <list>
-#include <vector>
-
-#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 1.1.0 < 3.0.0
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/legacy/compat.hpp>
-# include <opencv2/nonfree/nonfree.hpp>
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/features2d/features2d.hpp>
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x010100) // Require opencv >= 1.1.0
-# include <cxcore.h>
-# include <cv.h>
-#endif
-
-/*!
- \class vpKeyPointSurf
-
- \brief Class that implements the SURF key points and technics thanks
- to OpenCV library.
-
- \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
- You should rather use vpKeyPoint class that is more generic.
-
- The goal of this class is to provide a tool to match points from a
- model and points belonging to an image in which the model appears.
- The coordinates of the different reference points and matched points
- are given in pixel thanks to the class vpImagePoint. In this
- documentation we do not explain the SURF technics. So if you want to
- learn more about it you can refer to the following article :
- Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
- Robust Features", Proceedings of the 9th European Conference on
- Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
- 2006.
-
- If you use this class the first things you have to do is to create
- the reference thanks to a reference image which contains the
- interesting object to detect. Then you have to grab other images
- containing the object. After calling the specific method to match
- points you can access to the lists of matched points thanks to the
- methods getMatchedPointsInReferenceImage() and
- getMatchedPointsInCurrentImage(). These two methods return a list of
- matched points. The nth element of the first list is matched with
- the nth element of the second list. The following small example show
- how to use the class.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpKeyPointSurf.h>
-
-int main()
-{
-#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
- vpKeyPointSurf surf;
-
- // First grab the reference image Ireference
-
- // Build the reference SURF points.
- surf.buildReference(Ireference);
-
- // Then grab another image which represents the current image Icurrent
-
- // Match points between the reference points and the SURF points computed in the current image.
- surf.matchPoint(Icurrent);
-
- // Display the matched points
- surf.display(Ireference, Icurrent);
-
- return (0);
-#endif
-}
- \endcode
-
- It is also possible to create the reference thanks to only a part of the
- reference image (not the whole image) and find points to match in only a
- part of the current image. The small following example shows how to this
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpKeyPointSurf.h>
-
-int main()
-{
-#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
- vpKeyPointSurf surf;
-
- //First grab the reference image Ireference
-
- //Select a part of the image by clincking on two points which define a rectangle
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Ireference, corners[i]);
- }
-
- //Build the reference SURF points.
- int nbrRef;
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrRef = surf.buildReference(Ireference, corners[0], height, width);
-
- //Then grab another image which represents the current image Icurrent
-
- //Select a part of the image by clincking on two points which define a rectangle
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Icurrent, corners[i]);
- }
-
- //Match points between the reference points and the SURF points computed in the current image.
- int nbrMatched;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrMatched = surf.matchPoint(Icurrent, corners[0], height, width);
-
- //Display the matched points
- surf.display(Ireference, Icurrent);
-
- return(0);
-#endif
-}
- \endcode
-
- This class is also described in \ref tutorial-matching.
-*/
-
-class VISP_EXPORT vpKeyPointSurf : public vpBasicKeyPoint
-{
- public:
- /*!
- This enumerate enables to set the detail level of the
- descriptors.
- */
- typedef enum
- {
- basicDescriptor, /*<! basicDescriptor means that the descriptors are
- composed by 64 elements floating-point vector. */
- extendedDescriptor /*<! Means that the descriptors are composed by
- 128 elements floating-point vector. */
- } vpDescriptorType;
-
- public:
- vpKeyPointSurf();
-
- virtual ~vpKeyPointSurf();
-
- unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
- unsigned int matchPoint(const vpImage<unsigned char> &I);
- unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
- void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3);
- void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- const vpColor &color=vpColor::green);
- std::list<int*>* matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList);
- float* getDescriptorReferencePoint (const int index);
- int getLaplacianReferencePoint (const int index);
- void getDescriptorParamReferencePoint (const int index, int& size, float& dir);
- /*!
-
- Sets the value of the hessian threhold. Note that during the
- computation of the hessian for each potential points, only the
- points which have a hessian value higher than the threshold are
- keeped. Fore more details about the threshold see the article
- Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
- Robust Features", Proceedings of the 9th European Conference on
- Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
- 2006.
-
- \param hessian_threshold : Desired hessian threshold value.
- */
- void setHessianThreshold (double hessian_threshold) {
- this->hessianThreshold = hessian_threshold;
- params = cvSURFParams(this->hessianThreshold, this->descriptorType);
- } ;
-
- /*!
-
- Sets the type of descriptors to use.
-
- \param descriptor_type : Type of descriptor to use.
- */
- void setDescriptorType (vpDescriptorType descriptor_type) {
- this->descriptorType = descriptor_type;
- params = cvSURFParams(this->hessianThreshold, this->descriptorType);
- } ;
-
- /*!
- Gets the value of the hessian threhold.
-
- \return the hessian threshold value.
- */
- double getHessianThreshold () {return hessianThreshold;} ;
-
- /*!
- Gets the type of descriptor used.
-
- \return the type of descriptor used.
- */
- vpDescriptorType getDescriptorType () {return descriptorType;} ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated vpList<int*>* matchPoint(vpList<float*> descriptorList, vpList<int> laplacianList);
-#endif
-
- private:
- void init();
-
- private:
- //OpenCV Parameters
- CvMemStorage* storage;
- CvSURFParams params;
- CvMemStorage* storage_cur;
-
- CvSeq* image_keypoints;
- CvSeq* image_descriptors;
-
- CvSeq* ref_keypoints;
- CvSeq* ref_descriptors;
-
- /*!
- only features with keypoint.hessian larger than that are extracted.
- Good default value is ~300-500 (can depend on the average
- local contrast and sharpness of the image).
- User can further filter out some features based on their hessian values
- and other characteristics.
- */
- double hessianThreshold;
- vpDescriptorType descriptorType;
-
-};
-
-
-#endif
-
-#endif
-
-
diff --git a/src/key-point/vpPlanarObjectDetector.cpp b/src/key-point/vpPlanarObjectDetector.cpp
deleted file mode 100755
index 818d123..0000000
--- a/src/key-point/vpPlanarObjectDetector.cpp
+++ /dev/null
@@ -1,542 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlanarObjectDetector.cpp 4976 2014-11-18 10:17:38Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Planar surface detection tool.
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#include <visp/vpPlanarObjectDetector.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-
-#include <visp/vpImageConvert.h>
-#include <visp/vpException.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpColor.h>
-#include <visp/vpImageTools.h>
-
-#include <vector>
-#include <iostream>
-
-/*!
-
- Basic constructor
-
-*/
-vpPlanarObjectDetector::vpPlanarObjectDetector()
- : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
- refImagePoints(), minNbMatching(10)
-{
-}
-
-/*!
-
- Basic constructor, load data from a file.
-
- \param _dataFile : the name of the file.
- \param _objectName : the name of the object to load.
-
-*/
-vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
- : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
- refImagePoints(), minNbMatching(10)
-{
- load(_dataFile, _objectName);
-}
-
-/*!
- Initialise stuff. For the moment does nothing.
-*/
-void
-vpPlanarObjectDetector::init()
-{
-}
-
-/*!
-
- Basic destructor
-
-*/
-vpPlanarObjectDetector::~vpPlanarObjectDetector()
-{
-
-}
-
-/*!
- Compute the rectangular ROI from at least 4 points and set the region of
- interest on the current image.
-
- \param ip : the list of image point.
- \param nbpt : the number of point.
-*/
-void
-vpPlanarObjectDetector::computeRoi(vpImagePoint* ip, const unsigned int nbpt)
-{
- if(nbpt < 3){
- throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
- }
-
- std::vector < vpImagePoint > ptsx(nbpt);
- std::vector < vpImagePoint > ptsy(nbpt);
- for(unsigned int i=0; i<nbpt; i++){
- ptsx[i] = ptsy[i] = ip[i];
- }
-
- for(unsigned int i=0; i<nbpt; i++){
- for(unsigned int j=0; j<nbpt-1; j++){
- if(ptsx[j].get_j() > ptsx[j+1].get_j()){
- double tmp = ptsx[j+1].get_j();
- ptsx[j+1].set_j(ptsx[j].get_j());
- ptsx[j].set_j(tmp);
- }
- }
- }
- for(unsigned int i=0; i<nbpt; i++){
- for(unsigned int j=0; j<nbpt-1; j++){
- if(ptsy[j].get_i() > ptsy[j+1].get_i()){
- double tmp = ptsy[j+1].get_i();
- ptsy[j+1].set_i(ptsy[j].get_i());
- ptsy[j].set_i(tmp);
- }
- }
- }
-
-}
-
-
-/*!
- Train the classifier from the entire image (it is therefore assumed that the
- image is planar).
-
- Depending on the parameters, the training can take up to several minutes.
-
- \param _I : The image use to train the classifier.
-
- \return The number of reference points.
-*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
-{
- modelROI.x = 0;
- modelROI.y = 0;
- modelROI.width = (int)_I.getWidth();
- modelROI.height = (int)_I.getHeight();
-
- initialiseRefCorners(modelROI);
-
- return fern.buildReference(_I);
-}
-
-
-/*!
- Train the classifier on a region of the entire image. The region is a
- rectangle defined by its top left corner, its height and its width. The
- parameters of this rectangle must be given in pixel. It also includes the
- training of the fern classifier.
-
- \param _I : The image use to train the classifier.
- \param _iP : The top left corner of the rectangle defining the region of interest (ROI).
- \param _height : The height of the ROI.
- \param _width : The width of the ROI.
-
- \return the number of reference points
-*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- unsigned int _height, unsigned int _width)
-{
- unsigned int res = fern.buildReference(_I, _iP, _height, _width);
- modelROI.x = (int)_iP.get_u();
- modelROI.y = (int)_iP.get_v();
- modelROI.width = (int)_width;
- modelROI.height = (int)_height;
-
- initialiseRefCorners(modelROI);
-
- return res;
-}
-
-
-
-/*!
- Train the classifier on a region of the entire image. The region is a
- rectangle. The parameters of this rectangle must be given in pixel. It also
- includes the training of the fern classifier.
-
- \param _I : The image use to train the classifier.
- \param _rectangle : The rectangle defining the region of interest (ROI).
-
- \return The number of reference points.
-*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
- const vpRect _rectangle)
-{
- unsigned int res = fern.buildReference(_I, _rectangle);
-
- vpImagePoint iP = _rectangle.getTopLeft();
-
- modelROI.x = (int)iP.get_u();
- modelROI.y = (int)iP.get_v();
- modelROI.width = (int)_rectangle.getWidth();
- modelROI.height = (int)_rectangle.getHeight();
-
- initialiseRefCorners(modelROI);
-
- return res;
-}
-
-
-/*!
- Compute the points of interest in the current image and try to recognise them
- using the trained classifier. The matched pairs can be found with the
- getMatchedPointByRef function. The homography between the two planar
- surfaces is also computed.
-
- \param I : The gray scaled image where the points are computed.
-
- \return True if the surface has been found.
-*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
-{
- fern.matchPoint(I);
-
- /* compute homography */
- std::vector<cv::Point2f> refPts = fern.getRefPt();
- std::vector<cv::Point2f> curPts = fern.getCurPt();
-
- for(unsigned int i=0; i<refPts.size(); ++i){
- refPts[i].x += modelROI.x;
- refPts[i].y += modelROI.y;
- }
- for(unsigned int i=0; i<curPts.size(); ++i){
- curPts[i].x += modelROI.x;
- curPts[i].y += modelROI.y;
- }
-
- if(curPts.size() < 4){
- for (unsigned int i = 0; i < 3; i += 1){
- for (unsigned int j = 0; j < 3; j += 1){
- if(i == j){
- homography[i][j] = 1;
- }
- else{
- homography[i][j] = 0;
- }
- }
- }
- return false;
- }
-
- /* part of code from OpenCV planarObjectDetector */
- std::vector<unsigned char> mask;
- H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
-
- if( H.data )
- {
- const cv::Mat_<double>& H_tmp = H;
- dst_corners.resize(4);
- for(unsigned int i = 0; i < 4; i++ )
- {
- cv::Point2f pt = ref_corners[i];
-
- double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
- dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
- (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
- }
-
- double* ptr = (double*)H_tmp.data;
- for(unsigned int i=0; i<9; i++){
- this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
- }
- isCorrect = true;
- }
- else{
- isCorrect = false;
- }
-
-
- currentImagePoints.resize(0);
- refImagePoints.resize(0);
- for (unsigned int i = 0; i < mask.size(); i += 1){
- if(mask[i] != 0){
- vpImagePoint ip;
- ip.set_i(curPts[i].y);
- ip.set_j(curPts[i].x);
- currentImagePoints.push_back(ip);
- ip.set_i(refPts[i].y);
- ip.set_j(refPts[i].x);
- refImagePoints.push_back(ip);
- }
- }
-
- if(currentImagePoints.size() < minNbMatching){
- isCorrect = false;
- }
-
- return isCorrect;
-}
-
-
-
-/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
- \param I : The gray scaled image where the points are computed.
- \param iP : the top left corner of the rectangle defining the ROI
- \param height : the height of the ROI
- \param width : the width of the ROI
-
- \return true if the surface has been found.
-*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width)
-{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth()) {
- vpTRACE("Bad size for the subimage");
- throw(vpException(vpImageException::notInTheImage ,
- "Bad size for the subimage"));
- }
-
- vpImage<unsigned char> subImage;
-
- vpImageTools::createSubImage(I,
- (unsigned int)iP.get_i(),
- (unsigned int)iP.get_j(),
- height, width, subImage);
-
- return this->matchPoint(subImage);
-}
-
-/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
- \param I : The gray scaled image where the points are computed.
- \param rectangle : The rectangle defining the ROI.
-
- \return True if the surface has been found.
-*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle)
-{
- vpImagePoint iP;
- iP.set_i(rectangle.getTop());
- iP.set_j(rectangle.getLeft());
- return (this->matchPoint(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
-}
-
-
-/*!
- Display the result of the matching. The plane is displayed in red and the
- points of interests detected in the image are shown by a red dot surrounded by
- a green circle (the radius of the circle depends on the octave at which it has
- been detected).
-
- \param I : The gray scaled image for the display.
- \param displayKpts : The flag to display keypoints in addition to the surface.
-*/
-void
-vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
-{
- for(unsigned int i=0; i<dst_corners.size(); i++){
- vpImagePoint ip1(
- dst_corners[i].y - modelROI.y,
- dst_corners[i].x - modelROI.x);
- vpImagePoint ip2(
- dst_corners[(i+1)%dst_corners.size()].y - modelROI.y,
- dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
- }
-
- if(displayKpts){
- for(unsigned int i=0; i<currentImagePoints.size(); ++i){
- vpImagePoint ip(
- currentImagePoints[i].get_i() - modelROI.y,
- currentImagePoints[i].get_j() - modelROI.x);
- vpDisplay::displayCross(I, ip, 5, vpColor::red);
- }
- }
-}
-
-
-/*!
- This function displays the matched reference points and the matched
- points computed in the current image. The reference points are
- displayed in the image Ireference and the matched points coming from
- the current image are displayed in the image Icurrent. It is
- possible to set Ireference and Icurrent with the same image when
- calling the method.
-
- \param Iref : The image where the matched reference points are
- displayed.
-
- \param Icurrent : The image where the matched points computed in the
- current image are displayed.
-
- \param displayKpts : The flag to display keypoints in addition to the surface.
-*/
-void
-vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref,
- vpImage<unsigned char> &Icurrent, bool displayKpts)
-{
- display(Icurrent, displayKpts);
-
- if(displayKpts){
- for(unsigned int i=0; i<refImagePoints.size(); ++i){
- vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
- }
- }
-}
-
-
-/*!
- \brief Load the Fern classifier.
-
- Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
- can take up to sevral seconds.
-
- \param dataFilename : The name of the data filename (very large text file).
- It can have any file extension.
- \param objName : The name of the object.
-*/
-void
-vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
-{
- fern.load(dataFilename, objName);
- modelROI = fern.getModelROI();
- initialiseRefCorners(modelROI);
-}
-
-
-/*!
- \brief Record the Ferns classifier in the text file.
-
- \param objectName : The name of the object to store in the data file.
- \param dataFile : The name of the data filename (very large text file).
- It can have any file extension.
-*/
-void
-vpPlanarObjectDetector::recordDetector(const std::string& objectName, const std::string& dataFile )
-{
- fern.record(objectName, dataFile);
-}
-
-
-
-/*!
- Return the last positions of the detected corners.
-
- \return The vectors of corners' postions.
-*/
-std::vector<vpImagePoint>
-vpPlanarObjectDetector::getDetectedCorners() const{
- vpImagePoint ip;
- std::vector <vpImagePoint> corners;
- corners.clear();
- for(unsigned int i=0; i<dst_corners.size(); i++){
- ip.set_uv( dst_corners[i].x, dst_corners[i].y);
- corners.push_back(ip);
- }
-
- return corners;
-}
-
-/*!
- Initialise the internal reference corners from the rectangle.
-
- \param _modelROI : The rectangle defining the region of interest.
-*/
-void
-vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect& _modelROI)
-{
- cv::Point2f ip;
-
- ip.y = (float)_modelROI.y;
- ip.x = (float)_modelROI.x;
- ref_corners.push_back(ip);
-
- ip.y = (float)(_modelROI.y+_modelROI.height);
- ip.x = (float)_modelROI.x;
- ref_corners.push_back(ip);
-
- ip.y = (float)(_modelROI.y+_modelROI.height);
- ip.x = (float)(_modelROI.x+_modelROI.width);
- ref_corners.push_back(ip);
-
- ip.y = (float)_modelROI.y;
- ip.x = (float)(_modelROI.x+_modelROI.width);
- ref_corners.push_back(ip);
-}
-
-
-void
-vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint& _imPoint)
-{
- if(_i >= refImagePoints.size()){
- throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
- }
- _imPoint = refImagePoints[_i];
-}
-
-void
-vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
-{
-// fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
- if(_index >= currentImagePoints.size()){
- throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
- }
-
- _referencePoint = refImagePoints[_index];
- _currentPoint = currentImagePoints[_index];
-}
-
-#endif
-
diff --git a/src/key-point/vpPlanarObjectDetector.h b/src/key-point/vpPlanarObjectDetector.h
deleted file mode 100755
index ad77253..0000000
--- a/src/key-point/vpPlanarObjectDetector.h
+++ /dev/null
@@ -1,286 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlanarObjectDetector.h 5203 2015-01-24 09:33:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Planar surface detection tool.
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#ifndef VPPLANAROBJECTDETECTOR_H_
-#define VPPLANAROBJECTDETECTOR_H_
-
-#include <visp/vpConfig.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/calib3d/calib3d.hpp>
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
-# include <cv.h>
-# include <cvaux.hpp>
-#endif
-
-#include <visp/vpImagePoint.h>
-#include <visp/vpImage.h>
-#include <visp/vpRect.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpHomography.h>
-#include <visp/vpFernClassifier.h>
-
-/*!
- \class vpPlanarObjectDetector
- \ingroup PlanarSurfaceDetector
- \brief Class used to detect a planar surface.
-
- \deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
-
- This class allows to learn and recognise a surface in an image based on the
- Fern Classifier or any other point of interest matching class.
-
- It uses the class vpFernClassifier to extract points of interest in a
- reference image. These points are recorded and a classifier is trained to
- recognises them.
-
- In this class the points detected are assumed to belong to a planar surface.
- Therefore an homography can be computed between the reference image and the
- current image if the object is detected in the image.
-
- A robust method (RANSAC) is used to remove outliers in the matching process.
-
- The following example shows how to use the class.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpPlanarObjectDetector.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Surf Fern classifier only available since 2.1.0
-int main()
-{
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
- vpPlanarObjectDetector planar;
-
- //First grab the reference image Ireference
-
- //Select a part of the image by clincking on two points which define a rectangle
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Ireference, corners[i]);
- }
-
- //Build the reference points (and train the classifier).
- int nbrRef;
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- nbrRef = planar.buildReference(Ireference, corners[0], height, width);
-
- //Then grab another image which represents the current image Icurrent
-
- //Match points between the reference points and the current points computed in the current image.
- bool isDetected;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- isDetected = planar.matchPoint(Icurrent, corners[0], height, width);
-
- //Display the matched points
- if(isDetected){
- planar.display(Ireference, Icurrent);
- vpHomography homography;
- planar.getHomography(homography);
- }
- else{
- std::cerr << "planar surface not detected in the current image" << std::endl;
- }
-
- return(0);
-}
-#else
-int main() {}
-#endif
- \endcode
-
-*/
-class VISP_EXPORT vpPlanarObjectDetector{
-protected:
- //! Fern Classifier used to match the points between a reference image and the current image.
- vpFernClassifier fern;
-
- //! Computed homography in the ViSP format.
- vpHomography homography;
- //! Computed homography in the OpenCV format.
- cv::Mat H;
-
- //! The estimated new coordinates of the corners (reprojected using the homography).
- std::vector<cv::Point2f> dst_corners;
-
- //! Flag to indicate wether the last computed homography is correct or not.
- bool isCorrect;
-
- //! The corners in the reference image
- std::vector<cv::Point2f> ref_corners;
-
- //! The ROI for the reference image.
- cv::Rect modelROI;
-
- //! Vector of the image point in the current image that match after the deletion of the outliers with the RANSAC.
- std::vector<vpImagePoint> currentImagePoints;
- //! Vector of the image point in the reference image that match after the deletion of the outliers with the RANSAC.
- std::vector<vpImagePoint> refImagePoints;
-
- //! Minimal number of point to after the ransac needed to suppose that the homography has been correctly computed.
- unsigned int minNbMatching;
-
-public:
-
- // constructors and destructors
- vpPlanarObjectDetector();
- vpPlanarObjectDetector(const std::string& dataFile, const std::string& objectName);
- virtual ~vpPlanarObjectDetector();
-
- // main functions
- // creation of reference
- unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- unsigned int height, unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect rectangle);
-
- // matching
- bool matchPoint(const vpImage<unsigned char> &I);
- bool matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- bool matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle);
- // database management
- void recordDetector(const std::string& objectName, const std::string& dataFile);
- void load(const std::string& dataFilename, const std::string& objName);
-
-
- // display
- void display(vpImage<unsigned char> &I, bool displayKpts = false);
- void display(vpImage<unsigned char> &Iref,
- vpImage<unsigned char> &Icurrent, bool displayKpts = false);
-
- /*!
- Return the positions of the detected corners.
-
- \return a vector of vpImagePoint containing the position of the corners of
- the planar surface in the current image.
- */
- std::vector<vpImagePoint> getDetectedCorners() const;
-
- /*!
- Return a reference to the classifier.
-
- \return The fern classifier.
- */
- vpFernClassifier& getFernClassifier() {return this->fern;}
-
- /*!
- Return the computed homography between the reference image and the current
- image.
-
- \param _H : The computed homography.
- */
- inline void getHomography(vpHomography& _H) const { _H = this->homography;}
-
- /*!
- Return the number of reference points
-
- \return Number of reference points.
- */
- inline unsigned int getNbRefPoints() {return (unsigned int)currentImagePoints.size() ;}
-
- /*!
- Get the i-th reference point.
-
- \throw vpException if _i is out if bound.
-
- \param _i : index of the point to get
- \param _imPoint : image point returned by the
- */
- void getReferencePoint(const unsigned int _i, vpImagePoint& _imPoint);
-
- /*!
- 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.
- */
- void getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint);
-
- /*!
- Set the threshold for the minimal number of point to validate the homography.
- Default value is 10.
-
- \param _min : the new threshold.
- */
- void setMinNbPointValidation(const unsigned int _min){ this->minNbMatching = _min;}
-
-
- /*!
- Get the threshold for the minimal number of point to validate the homography.
- Default value is 10.
-
- \return : the current threshold.
- */
- unsigned int getMinNbPointValidation() const { return this->minNbMatching;}
-
-protected:
-
- virtual void init();
- void computeRoi(vpImagePoint* ip, const unsigned int nbpt);
- void initialiseRefCorners(const cv::Rect& _modelROI);
-};
-
-#endif
-
-#endif /* VPPLANAROBJECTDETECTOR_H_ */
-
-
-
-
-
diff --git a/src/key-point/vpXmlConfigParserKeyPoint.cpp b/src/key-point/vpXmlConfigParserKeyPoint.cpp
deleted file mode 100644
index 2ee1811..0000000
--- a/src/key-point/vpXmlConfigParserKeyPoint.cpp
+++ /dev/null
@@ -1,426 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load configuration for vpKeyPoint class.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-
-/*!
- \file vpXmlConfigParserKeyPoint.cpp
- \brief Definition of the vpXmlConfigParserKeyPoint class member functions.
- Class vpXmlConfigParserKeyPoint permits to load configuration defined in a XML file for vpKeyPoint class.
-
-*/
-
-#include <iostream>
-
-#include <visp/vpXmlConfigParserKeyPoint.h>
-
-#ifdef VISP_HAVE_XML2
-
-vpXmlConfigParserKeyPoint::vpXmlConfigParserKeyPoint() : m_detectorName("SIFT"), m_extractorName("SIFT"), m_matcherName("BruteForce"),
-m_matchingFactorThreshold(2.0), m_matchingMethod(stdDistanceThreshold), m_matchingRatioThreshold(0.85), m_nbRansacIterations(200),
-m_nbRansacMinInlierCount(100), m_ransacConsensusPercentage(20.0), m_ransacReprojectionError(6.0), m_ransacThreshold(0.001),
-m_useRansacConsensusPercentage(false), m_useRansacVVS(false)
-{
- init();
-}
-
-/*!
- Initialize the nodeMap for the node parsing.
-*/
-void
-vpXmlConfigParserKeyPoint::init()
-{
- setMainTag("conf");
-
- nodeMap["conf"] = conf;
- nodeMap["detector"] = detector;
- nodeMap["extractor"] = extractor;
- nodeMap["matcher"] = matcher;
- nodeMap["name"] = name;
- nodeMap["matching_method"] = matching_method;
- nodeMap["constantFactorDistanceThreshold"] = constant_factor_distance_threshold;
- nodeMap["stdDistanceThreshold"] = std_distance_threshold;
- nodeMap["ratioDistanceThreshold"] = ratio_distance_threshold;
- nodeMap["stdAndRatioDistanceThreshold"] = std_and_ratio_distance_threshold;
- nodeMap["noFilterMatching"] = no_filter_matching;
- nodeMap["matchingFactorThreshold"] = matching_factor_threshold;
- nodeMap["matchingRatioThreshold"] = matching_ratio_threshold;
- nodeMap["ransac"] = ransac;
- nodeMap["useRansacVVS"] = use_ransac_vvs;
- nodeMap["useRansacConsensusPercentage"] = use_ransac_consensus_percentage;
- nodeMap["nbRansacIterations"] = nb_ransac_iterations;
- nodeMap["ransacReprojectionError"] = ransac_reprojection_error;
- nodeMap["nbRansacMinInlierCount"] = nb_ransac_min_inlier_count;
- nodeMap["ransacThreshold"] = ransac_threshold;
- nodeMap["ransacConsensusPercentage"] = ransac_consensus_percentage;
-}
-
-/*!
- Parse an XML file to load configuration for vpKeyPoint class.
- \param filename : filename of the XML file to parse.
-*/
-void
-vpXmlConfigParserKeyPoint::parse(const std::string &filename)
-{
- vpXmlParser::parse(filename);
-}
-
-/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
- \param doc : Document to parse.
- \param node : Root node.
-*/
-void
-vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
-{
- bool detector_node = false;
- bool extractor_node = false;
- bool matcher_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case detector:
- this->read_detector(doc, dataNode);
- detector_node = true;
- break;
-
- case extractor:
- this->read_extractor(doc, dataNode);
- extractor_node = true;
- break;
-
- case matcher:
- this->read_matcher(doc, dataNode);
- matcher_node = true;
- break;
-
- case ransac:
- this->read_ransac(doc, dataNode);
- break;
-
- default:
- break;
- }
- }
- }
- }
-
- if(!detector_node) {
- std::cout << "detector: name: "<< m_detectorName << " (default)" << std::endl;
- }
-
- if(!extractor_node) {
- std::cout << "extractor: name: "<< m_extractorName << " (default)" << std::endl;
- }
-
- if(!matcher_node) {
- std::cout << "matcher: name: "<< m_matcherName << " (default)" << std::endl;
- }
-}
-
-/*!
- Parse detector tag part.
-
- \param doc : Document to parse.
- \param node : Detector node.
-*/
-void
-vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
-{
- bool detector_name_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if (iter_data != nodeMap.end()) {
- switch(iter_data->second) {
- case name:
- m_detectorName = xmlReadStringChild(doc, dataNode);
- detector_name_node = true;
- break;
-
- default:
- break;
- }
- }
- }
- }
-
- if(!detector_name_node)
- std::cout << "detector : Name : "<< m_detectorName << " (default)" << std::endl;
- else
- std::cout << "detector : Name : "<< m_detectorName << std::endl;
-}
-
-/*!
- Parse extractor tag part.
-
- \param doc : Document to parse.
- \param node : Extractor node.
-*/
-void
-vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
-{
- bool extractor_name_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if (iter_data != nodeMap.end()) {
- switch(iter_data->second) {
- case name:
- m_extractorName = xmlReadStringChild(doc, dataNode);
- extractor_name_node = true;
- break;
-
- default:
- break;
- }
- }
- }
- }
-
- if(!extractor_name_node)
- std::cout << "extractor : Name : "<< m_extractorName << " (default)" << std::endl;
- else
- std::cout << "extractor : Name : "<< m_extractorName << std::endl;
-}
-
-/*!
- Parse matcher tag part.
-
- \param doc : Document to parse.
- \param node : Matcher node.
-*/
-void
-vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
-{
- bool matcher_name_node = false;
- bool matching_method_node = false;
- std::string matchingMethodName = "stdDistanceThreshold";
- bool matching_factor_threshold_node = false;
- bool matching_ratio_threshold_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if(iter_data != nodeMap.end()) {
- switch(iter_data->second) {
- case name:
- m_matcherName = xmlReadStringChild(doc, dataNode);
- matcher_name_node = true;
- break;
-
- case matching_method:
- {
- matchingMethodName = xmlReadStringChild(doc, dataNode);
-
- std::map<std::string, int>::iterator iter_data2 = nodeMap.find(matchingMethodName);
- if(iter_data2 != nodeMap.end()) {
- matching_method_node = true;
- switch(iter_data2->second) {
- case constant_factor_distance_threshold:
- m_matchingMethod = constantFactorDistanceThreshold;
- break;
-
- case std_distance_threshold:
- m_matchingMethod = stdDistanceThreshold;
- break;
-
- case ratio_distance_threshold:
- m_matchingMethod = ratioDistanceThreshold;
- break;
-
- case std_and_ratio_distance_threshold:
- m_matchingMethod = stdAndRatioDistanceThreshold;
- break;
-
- case no_filter_matching:
- m_matchingMethod = noFilterMatching;
- break;
-
- default:
- matching_method_node = false;
- break;
- }
- }
- break;
- }
-
- case matching_factor_threshold:
- m_matchingFactorThreshold = xmlReadDoubleChild(doc, dataNode);
- matching_factor_threshold_node = true;
- break;
-
- case matching_ratio_threshold:
- m_matchingRatioThreshold = xmlReadDoubleChild(doc, dataNode);
- matching_ratio_threshold_node = true;
- break;
-
- default:
- break;
- }
- }
- }
- }
-
- if(!matcher_name_node)
- std::cout << "matcher : Name : "<< m_matcherName << " (default)" << std::endl;
- else
- std::cout << "matcher : Name : "<< m_matcherName <<std::endl;
-
- if(!matching_method_node)
- std::cout << "matcher : Filter method : "<< matchingMethodName << " (default)" << std::endl;
- else
- std::cout << "matcher : Filter method : "<< matchingMethodName << std::endl;
-
- if(!matching_factor_threshold_node)
- std::cout << "matcher : matching factor threshold : "<< m_matchingFactorThreshold << " (default)" << std::endl;
- else
- std::cout << "matcher : matching factor threshold : "<< m_matchingFactorThreshold << std::endl;
-
- if(!matching_ratio_threshold_node)
- std::cout << "matcher : matching ratio threshold : "<< m_matchingRatioThreshold << " (default)" << std::endl;
- else
- std::cout << "matcher : matching ratio threshold : "<< m_matchingRatioThreshold << std::endl;
-}
-
-/*!
- Parse ransac tag part.
-
- \param doc : Document to parse.
- \param node : Ransac node.
-*/
-void
-vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node)
-{
- bool use_ransac_vvs_node = false;
- bool use_ransac_consensus_percentage_node = false;
- bool nb_ransac_iterations_node = false;
- bool ransac_reprojection_error_node = false;
- bool nb_ransac_min_inlier_count_node = false;
- bool ransac_threshold_node = false;
- bool ransac_consensus_percentage_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if(iter_data != nodeMap.end()) {
- switch(iter_data->second) {
- case use_ransac_vvs:
- m_useRansacVVS = xmlReadIntChild(doc, dataNode) != 0;
- use_ransac_vvs_node = true;
- break;
-
- case use_ransac_consensus_percentage:
- m_useRansacConsensusPercentage = xmlReadIntChild(doc, dataNode) != 0;
- use_ransac_consensus_percentage_node = true;
- break;
-
- case nb_ransac_iterations:
- m_nbRansacIterations = xmlReadIntChild(doc, dataNode);
- nb_ransac_iterations_node = true;
- break;
-
- case ransac_reprojection_error:
- m_ransacReprojectionError = xmlReadDoubleChild(doc, dataNode);
- ransac_reprojection_error_node = true;
- break;
-
- case nb_ransac_min_inlier_count:
- m_nbRansacMinInlierCount = xmlReadIntChild(doc, dataNode);
- nb_ransac_min_inlier_count_node = true;
- break;
-
- case ransac_threshold:
- m_ransacThreshold = xmlReadDoubleChild(doc, dataNode);
- ransac_threshold_node = true;
- break;
-
- case ransac_consensus_percentage:
- m_ransacConsensusPercentage = xmlReadDoubleChild(doc, dataNode);
- ransac_consensus_percentage_node = true;
- break;
-
- default:
- break;
- }
- }
- }
- }
-
- if(!use_ransac_vvs_node)
- std::cout << "ransac: use ransac vvs pose estimation: "<< m_useRansacVVS << " (default)" << std::endl;
- else
- std::cout << "ransac: use ransac vvs pose estimation: "<< m_useRansacVVS <<std::endl;
-
- if(!use_ransac_consensus_percentage_node)
- std::cout << "ransac: use consensus percentage: "<< m_useRansacConsensusPercentage << " (default)" << std::endl;
- else
- std::cout << "ransac: use consensus percentage: "<< m_useRansacConsensusPercentage <<std::endl;
-
- if(!nb_ransac_iterations_node)
- std::cout << "ransac: nb ransac iterations: "<< m_nbRansacIterations << " (default)" << std::endl;
- else
- std::cout << "ransac: nb ransac iterations: "<< m_nbRansacIterations <<std::endl;
-
- if(!ransac_reprojection_error_node)
- std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError << " (default)" << std::endl;
- else
- std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError <<std::endl;
-
- if(!nb_ransac_min_inlier_count_node)
- std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount << " (default)" << std::endl;
- else
- std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount <<std::endl;
-
- if(!ransac_threshold_node)
- std::cout << "ransac: ransac threshold in meter (for ViSP function): "<< m_ransacThreshold << " (default)" << std::endl;
- else
- std::cout << "ransac: ransac threshold in meter (for ViSP function): "<< m_ransacThreshold <<std::endl;
-
- if(!ransac_consensus_percentage_node)
- std::cout << "ransac: consensus percentage: "<< m_ransacConsensusPercentage << " (default)" << std::endl;
- else
- std::cout << "ransac: consensus percentage: "<< m_ransacConsensusPercentage <<std::endl;
-}
-
-#endif //VISP_HAVE_XML2
diff --git a/src/key-point/vpXmlConfigParserKeyPoint.h b/src/key-point/vpXmlConfigParserKeyPoint.h
deleted file mode 100644
index 3d6e8b5..0000000
--- a/src/key-point/vpXmlConfigParserKeyPoint.h
+++ /dev/null
@@ -1,269 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load configuration for vpKeyPoint class.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-
-/*!
- \file vpXmlConfigParserKeyPoint.cpp
- \brief Definition of the vpXmlConfigParserKeyPoint class member functions.
- Class vpXmlConfigParserKeyPoint allows to load configuration defined in a XML file for vpKeyPoint class.
-
-*/
-
-#ifndef __vpXmlConfigParserKeyPoint_h__
-#define __vpXmlConfigParserKeyPoint_h__
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <iostream>
-#include <stdlib.h>
-
-#include <libxml/xmlmemory.h> // Functions of libxml.
-
-#include <visp/vpXmlParser.h>
-
-
-class VISP_EXPORT vpXmlConfigParserKeyPoint: public vpXmlParser
-{
-
-public:
- /*! Predefined xml node identifier. */
- typedef enum {
- conf, /*!< Identifier associated to the root tag. */
- detector, /*!< Identifier associated to the detector tag. */
- extractor, /*!< Identifier associated to the extractor tag. */
- matcher, /*!< Identifier associated to the matcher tag. */
- name, /*!< Identifier associated to the name tag. */
- matching_method, /*!< Identifier associated to the matching_method tag. */
- constant_factor_distance_threshold, /*!< Identifier associated to the constant_factor_distance_threshold tag. */
- std_distance_threshold, /*!< Identifier associated to the std_distance_threshold tag. */
- ratio_distance_threshold, /*!< Identifier associated to the ratio_distance_threshold tag. */
- std_and_ratio_distance_threshold, /*!< Identifier associated to the std_and_ratio_distance_threshold tag. */
- no_filter_matching, /*!< Identifier associated to the no_filter_matching tag. */
- matching_factor_threshold, /*!< Identifier associated to the matching_factor_threshold tag. */
- matching_ratio_threshold, /*!< Identifier associated to the matching_ratio_threshold tag. */
- ransac, /*!< Identifier associated to the ransac tag. */
- use_ransac_vvs, /*!< Identifier associated to the use_ransac_vvs tag. */
- use_ransac_consensus_percentage, /*!< Identifier associated to the use_ransac_consensus_percentage tag. */
- nb_ransac_iterations, /*!< Identifier associated to the nb_ransac_iterations tag. */
- ransac_reprojection_error, /*!< Identifier associated to the ransac_reprojection_error tag. */
- nb_ransac_min_inlier_count, /*!< Identifier associated to the nb_ransac_min_inlier_count tag. */
- ransac_threshold, /*!< Identifier associated to the ransac_threshold tag. */
- ransac_consensus_percentage /*!< Identifier associated to the ransac_consensus_percentage tag. */
- } vpNodeIdentifier;
-
- /*! Enumerator for the different filtering matching method. */
- typedef enum {
- constantFactorDistanceThreshold, /*!< Keep all the points below a constant factor threshold. */
- stdDistanceThreshold, /*!< Keep all the points below a minimal distance + the standard deviation. */
- ratioDistanceThreshold, /*!< Keep all the points enough discriminated. */
- stdAndRatioDistanceThreshold, /*!< Keep all the points which fall with the two conditions. */
- noFilterMatching /*!< No filtering. */
- } vpMatchingMethodEnum;
-
-private :
- //! Name of the keypoint detector.
- std::string m_detectorName;
- //! Name of the keypoint extractor.
- std::string m_extractorName;
- //! Name of the keypoint matcher.
- std::string m_matcherName;
- //! Factor value for filtering method: constantFactorDistanceThreshold.
- double m_matchingFactorThreshold;
- //! Filtering method.
- vpMatchingMethodEnum m_matchingMethod;
- //! Ratio value for filtering method: ratioDistanceThreshold.
- double m_matchingRatioThreshold;
- //! Maximum number of iterations for the Ransac method.
- int m_nbRansacIterations;
- //! Minimum number of inliers for the Ransac method.
- int m_nbRansacMinInlierCount;
- //! Percentage value of inliers compared to outliers for the Ransac method.
- double m_ransacConsensusPercentage;
- //! Maximum reprojection error (in pixel for OpenCV method) to consider a point as an inlier.
- double m_ransacReprojectionError;
- //! Maximum error (in meter for ViSP method) to consider a point as an inlier.
- double m_ransacThreshold;
- //! If true, the cardinality of the consensus is based upon a percentage, otherwise
- //it is based on a fixed number.
- bool m_useRansacConsensusPercentage;
- //! If true, use ViSP Ransac VVS pose estimation method, otherwise use OpenCV method.
- bool m_useRansacVVS;
-
-
-public:
-
- vpXmlConfigParserKeyPoint();
-
- /*!
- Get the detector name.
-
- \return The detector name.
- */
- inline std::string getDetectorName() const {
- return m_detectorName;
- }
-
- /*!
- Get the extractor name.
-
- \return The extractor name.
- */
- inline std::string getExtractorName() const {
- return m_extractorName;
- }
- /*!
- Get the matcher name.
-
- \return The detector name.
- */
- inline std::string getMatcherName() const {
- return m_matcherName;
- }
- /*!
- Get the factor value.
-
- \return The factor value for the filtering method: constantFactorDistanceThreshold.
- */
- inline double getMatchingFactorThreshold() const {
- return m_matchingFactorThreshold;
- }
-
- /*!
- Get the filtering method.
-
- \return The filtering method.
- */
- inline vpMatchingMethodEnum getMatchingMethod() const {
- return m_matchingMethod;
- }
-
- /*!
- Get the ratio value.
-
- \return The factor value for the filtering method: ratioDistanceThreshold.
- */
- inline double getMatchingRatioThreshold() const {
- return m_matchingRatioThreshold;
- }
-
- /*!
- Get the maximum number of iterations for the Ransac method.
-
- \return The maximum number of iterations for the Ransac method.
- */
- inline int getNbRansacIterations() const {
- return m_nbRansacIterations;
- }
-
- /*!
- Get the minimum number of inliers for the Ransac method.
-
- \return The minimum number of inliers for the Ransac method.
- */
- inline int getNbRansacMinInlierCount() const {
- return m_nbRansacMinInlierCount;
- }
-
- /*!
- Get the percentage value of inliers for the Ransac method.
-
- \return The percentage value of inliers for the Ransac method.
- */
- inline double getRansacConsensusPercentage() const {
- return m_ransacConsensusPercentage;
- }
-
- /*!
- Get the maximum reprojection error for a candidate inlier for the Ransac method.
-
- \return The maximum reprojection error for the Ransac method.
- */
- inline double getRansacReprojectionError() const {
- return m_ransacReprojectionError;
- }
-
- /*!
- Get the maximum error for a candidate inlier for the Ransac method.
-
- \return The maximum error for the Ransac method.
- */
- inline double getRansacThreshold() const {
- return m_ransacThreshold;
- }
-
- /*!
- Get the flag state to choose between a percentage of inliers or a fixed number.
-
- \return True to use a percentage value for inliers, false otherwise.
- */
- inline bool getUseRansacConsensusPercentage() const {
- return m_useRansacConsensusPercentage;
- }
-
- /*!
- Get the flag state to choose between OpenCV Ransac pose estimation or ViSP
- Ransac VVS pose estimation.
-
- \return True to use ViSP method, false otherwise.
- */
- inline bool getUseRansacVVSPoseEstimation() const {
- return m_useRansacVVS;
- }
-
- /*!
- Parse an XML file to get configuration value for vpKeyPoint class.
-
- \param filename : filename of the XML file to parse
- */
- void parse(const std::string &filename);
-
-
-private:
-
- void init();
- void read_detector(xmlDocPtr doc, xmlNodePtr node);
- void read_extractor(xmlDocPtr doc, xmlNodePtr node);
- void read_matcher(xmlDocPtr doc, xmlNodePtr node);
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_ransac(xmlDocPtr doc, xmlNodePtr node);
- virtual void writeMainClass(xmlNodePtr ){};
-
-};
-#endif //VISP_HAVE_XML2
-#endif
diff --git a/src/math/kalman/vpKalmanFilter.cpp b/src/math/kalman/vpKalmanFilter.cpp
deleted file mode 100644
index 8fe3cd9..0000000
--- a/src/math/kalman/vpKalmanFilter.cpp
+++ /dev/null
@@ -1,466 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKalmanFilter.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Kalman filtering.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpKalmanFilter.cpp
- \brief Generic kalman filtering implementation.
-*/
-
-#include <visp/vpKalmanFilter.h>
-
-#include <math.h>
-#include <stdlib.h>
-
-/*!
- Initialize the Kalman filter.
-
- \param size_state_vector : Size of the state vector \f${\bf x}_{k}\f$ for one signal.
-
- \param size_measure_vector : Size of the measure vector \f${\bf z}_{k}\f$
- for one signal.
-
- \param n_signal : Number of signal to filter.
-*/
-void
-vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector,
- unsigned int n_signal)
-{
- this->size_state = size_state_vector;
- this->size_measure = size_measure_vector ;
- this->nsignal = n_signal ;
- F.resize(size_state*nsignal, size_state*nsignal) ;
- H.resize(size_measure*nsignal, size_state*nsignal) ;
-
- R.resize(size_measure*nsignal, size_measure*nsignal) ;
- Q.resize(size_state*nsignal, size_state*nsignal) ;
-
- Xest.resize(size_state*nsignal) ; Xest = 0;
- Xpre.resize(size_state*nsignal) ; Xpre = 0 ;
-
- Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;
-
- I.resize(size_state*nsignal, size_state*nsignal) ;
- // init_done = false ;
- iter = 0 ;
- dt = -1 ;
-}
-
-/*!
- Construct a default Kalman filter.
-
- The verbose mode is by default desactivated.
-
-*/
-vpKalmanFilter::vpKalmanFilter()
- : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
- Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
-{
-}
-
-/*!
- Construct a default Kalman filter by setting the number of signal to filter.
-
- The verbose mode is by default desactivated.
-
- \param n_signal : Number of signal to filter.
-*/
-vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
- : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
- Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
-{
-}
-
-/*!
- Construct a Kalman filter.
-
- The verbose mode is by default desactivated.
-
- \param size_state_vector : Size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
-
- \param size_measure_vector : Size of the measure vector \f${\bf z}_{(k)}\f$
- for one signal.
-
- \param n_signal : Number of signal to filter.
-*/
-vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
- : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
- Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
-{
- init( size_state_vector, size_measure_vector, n_signal) ;
-}
-
-/*!
- Update the Kalman filter by applying the prediction equations.
-
- The predicted state is given by
- \f[
- {{\bf x}}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}
- \f]
- and the state prediction covariance by
- \f[
- {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
- + {\bf Q}_k
- \f]
-
-*/
-
-void
-vpKalmanFilter::prediction()
-{
- if (Xest.getRows() != size_state*nsignal) {
- std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
- <<" " << size_state*nsignal<< std::endl ;
- std::cout << " Error : Filter non initialized " << std::endl;
- exit(1) ;
- }
-
-// if (!init_done) {
-// std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<< std::endl ;
-// std::cout << " Error : Filter non initialized " << std::endl;
-// exit(1) ;
-// return;
-// }
-
- if (verbose_mode) {
- std::cout << "F = " << std::endl << F << std::endl ;
- std::cout << "Xest = "<< std::endl << Xest << std::endl ;
- }
- // Prediction
- // Bar-Shalom 5.2.3.2
- Xpre = F*Xest ;
- if (verbose_mode) {
- std::cout << "Xpre = "<< std::endl << Xpre << std::endl ;
- std::cout << "Q = "<< std::endl << Q << std::endl ;
- std::cout << "Pest " << std::endl << Pest << std::endl ;
- }
- // Bar-Shalom 5.2.3.5
- Ppre = F*Pest*F.t() + Q ;
-
- // Matrice de covariance de l'erreur de prediction
- if (verbose_mode)
- std::cout << "Ppre " << std::endl << Ppre << std::endl ;
-}
-
-/*!
-
- Update the Kalman filter by applying the filtering equations and
- increment the filter iteration (vpKalmanFilter::iter).
-
- \param z : Measure (or observation) \f${\bf z}_k\f$ provided at iteration \f$k\f$.
-
- The filtering equation is given by:
- \f[
- {\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k -
- {\bf H} {\bf x}_{k \mid k-1} \right]
- \f]
- where \f${\bf W_k}\f$ is the filter gain computed using the formula:
- \f[
- {\bf W_k} = {\bf P}_{k \mid k-1} {\bf H}^T
- \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}
- \f]
- and where the updated covariance of the state is given by
- \f[
- {\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k \mid k-1}
- \f]
- or in a symetric form
- \f[
- {\bf P}_{k \mid k} = {\bf P}_{k \mid k-1} - {\bf W}_k {\bf S}_k {\bf W}^T_k
- \f]
- with
- \f[
- {\bf S}_k = {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k
- \f]
-
-*/
-void
-vpKalmanFilter::filtering(vpColVector &z)
-{
- if (verbose_mode)
- std::cout << "z " << std::endl << z << std::endl ;
- // Bar-Shalom 5.2.3.11
- vpMatrix S = H*Ppre*H.t() + R ;
- if (verbose_mode)
- std::cout << "S " << std::endl << S << std::endl ;
-
- W = (Ppre * H.t())* (S).inverseByLU() ;
- if (verbose_mode)
- std::cout << "W " << std::endl << W << std::endl ;
- // Bar-Shalom 5.2.3.15
- Pest = Ppre - W*S*W.t() ;
- if (verbose_mode)
- std::cout << "Pest " << std::endl << Pest << std::endl ;
-
- if (0) {
- // Bar-Shalom 5.2.3.16
- // numeriquement plus stable que 5.2.3.15
- vpMatrix Pestinv ;
- Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU()*H ;
- Pest = Pestinv.inverseByLU() ;
- }
- // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
- Xest = Xpre + (W*(z - (H*Xpre))) ;
- if (verbose_mode)
- std::cout << "Xest " << std::endl << Xest << std::endl ;
-
- iter++ ;
-}
-
-
-#if 0
-
-
-/*!
- \brief Filter initialization for a constant velocity model
-
- \param dt : time between two measures
- \param Vn : Variance of measure noise
- \param Vw : Variance of the state noise
-
- State model:
- \f[ S = \left[\; y \quad \frac{\partial y}{\partial t}\;\right]^T = \left[\; y \quad \dot{ y}\;\right]^T
- \f]
-
- Filter model:
- \f[
- \begin{array}{rclll} \\
- S(t+1) &= & F S(t) + W(t)&~~~~~~~~~~~& S(t+1) \mbox{ est un vecteur} \left[\;
- y\quad \dot y\;\right]^T \\
- X(t) &=& C S(t) + N(t)&& X(t) \mbox{ est un scalaire}
- \end{array}
- \f]
-
-
- Matrix F describes the evolution of the state. This matrix is given by:
- \f[
- F= \left( \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right)
- \f]
-
- The noise \f$W = \left( \begin{array}{c} \;W_1 \quad W_2\; \end{array} \right)^T\f$
- take into account the variations of the constant velocity model due to the accelerations.
-
- Thus we have:
- \f[
- \left\{
- \begin{array}{rcl}
- y(t+1)& =& y(t) + \Delta(t) \dot y(t) + \underbrace{\frac{\Delta t^2}{2} \ddot y(t)}_{W_1} \\
- \dot y(t+1) &=& \dot y(t) + \underbrace{\Delta(t) \ddot y(t)}_{W_2}
- \end{array}
- \right.
- \f]
- et donc
- \f[
- \left\{
- \begin{array}{rcccccc}
- y(t+1)& =& y(t) &+& \Delta(t) \dot y(t) &+& {W_1} \\
- \dot y(t+1) &=& & &\dot y(t) &+& {W_2}
- \end{array}
- \right. \qquad \Leftrightarrow \qquad S(t+1) = F S(t) + W
- \f]
-*/
-void
-vpKalmanFilter::initFilterCteAcceleration(double dt,
- vpColVector &Z0,
- vpColVector &Z1,
- vpColVector &Z2,
- vpColVector &sigma_noise,
- vpColVector &sigma_state )
-{
- this->dt = dt ;
-
- double dt2 = dt*dt ;
- double dt3 = dt2*dt ;
- double dt4 = dt3*dt ;
- double dt5 = dt4*dt ;
-
- //init_done = true ;
-
- Pest =0 ;
- // initialise les matrices decrivant les modeles
- for (int i=0; i < size_measure ; i++ )
- {
- // modele sur l'etat
-
- // | 1 dt dt2/2 |
- // F = | 0 1 dt |
- // | 0 0 1 |
-
- F[3*i][3*i] = 1 ;
- F[3*i][3*i+1] = dt ;
- F[3*i][3*i+2] = dt*dt/2 ;
- F[3*i+1][3*i+1] = 1 ;
- F[3*i+1][3*i+2] = dt ;
- F[3*i+2][3*i+2] = 1 ;
-
-
- // modele sur la mesure
- H[i][3*i] = 1 ;
- H[i][3*i+1] = 0 ;
- H[i][3*i+2] = 0 ;
-
- double sR = sigma_noise[i] ;
- double sQ = sigma_state[i] ;
-
- // bruit de mesure
- R[i][i] = (sR) ;
-
- // bruit d'etat 6.2.3.9
- Q[3*i ][3*i ] = sQ * dt5/20;
- Q[3*i ][3*i+1] = sQ * dt4/8;
- Q[3*i ][3*i+2] = sQ * dt3/6 ;
-
- Q[3*i+1][3*i ] = sQ * dt4/8 ;
- Q[3*i+1][3*i+1] = sQ * dt3/3 ;
- Q[3*i+1][3*i+2] = sQ * dt2/2 ;
-
- Q[3*i+2][3*i ] = sQ * dt3/6 ;
- Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
- Q[3*i+2][3*i+2] = sQ * dt ;
-
-
- // Initialisation pour la matrice de covariance sur l'etat
-
- Pest[3*i ][3*i ] = sR ;
- Pest[3*i ][3*i+1] = 1.5/dt*sR ;
- Pest[3*i ][3*i+2] = sR/(dt2) ;
-
- Pest[3*i+1][3*i ] = 1.5/dt*sR ;
- Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
- Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
-
- Pest[3*i+2][3*i ] = sR/(dt2) ;
- Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
- Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
-
-
- // Initialisation pour l'etat
-
- Xest[3*i] = Z2[i] ;
- Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
- Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
-
- }
-}
-
-void
-vpKalmanFilter::initFilterSinger(double dt,
- double a,
- vpColVector &Z0,
- vpColVector &Z1,
- vpColVector &sigma_noise,
- vpColVector &sigma_state )
-{
- this->dt = dt ;
-
- double dt2 = dt*dt ;
- double dt3 = dt2*dt ;
-
- double a2 = a*a ;
- double a3 = a2*a ;
- double a4 = a3*a ;
-
- //init_done = true ;
-
- Pest =0 ;
- // initialise les matrices decrivant les modeles
- for (int i=0; i < size_measure ; i++ )
- {
- // modele sur l'etat
-
- // | 1 dt dt2/2 |
- // F = | 0 1 dt |
- // | 0 0 1 |
-
- F[3*i][3*i] = 1 ;
- F[3*i][3*i+1] = dt ;
- F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
- F[3*i+1][3*i+1] = 1 ;
- F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
- F[3*i+2][3*i+2] = exp(-a*dt) ;
-
-
- // modele sur la mesure
- H[i][3*i] = 1 ;
- H[i][3*i+1] = 0 ;
- H[i][3*i+2] = 0 ;
-
- double sR = sigma_noise[i] ;
- double sQ = sigma_state[i] ;
-
- R[i][i] = (sR) ; // bruit de mesure 1.5mm
-
- Q[3*i ][3*i ] = sQ/a4*(1-exp(-2*a*dt)+2*a*dt+2*a3/3*dt3-2*a2*dt2-4*a*dt*exp(-a*dt) ) ;
- Q[3*i ][3*i+1] = sQ/a3*(1+exp(-2*a*dt)-2*exp(-a*dt)+2*a*dt*exp(-a*dt)-2*a*dt+a2*dt2 ) ;
- Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
-
- Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
- Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
- Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
-
- Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
- Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
- Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
-
-
- // Initialisation pour la matrice de covariance sur l'etat
- Pest[3*i ][3*i ] = sR ;
- Pest[3*i ][3*i+1] = 1/dt*sR ;
- Pest[3*i ][3*i+2] = 0 ;
-
- Pest[3*i+1][3*i ] = 1/dt*sR ;
- Pest[3*i+1][3*i+1] = 2*sR/dt2 + sQ/(a4*dt2)*(2-a2*dt2+2*a3*dt3/3.0 -2*exp(-a*dt)-2*a*dt*exp(-a*dt));
- Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
-
- Pest[3*i+2][3*i ] = 0 ;
- Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
- Pest[3*i+2][3*i+2] = 0 ;
-
-
- // Initialisation pour l'etat
-
- Xest[3*i] = Z1[i] ;
- Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
- Xest[3*i+2] = 0 ;
-
- }
-}
-
-#endif
diff --git a/src/math/kalman/vpKalmanFilter.h b/src/math/kalman/vpKalmanFilter.h
deleted file mode 100644
index 823dfe8..0000000
--- a/src/math/kalman/vpKalmanFilter.h
+++ /dev/null
@@ -1,225 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKalmanFilter.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Kalman filtering.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpKalmanFilter_h
-#define vpKalmanFilter_h
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-
-#include <math.h>
-
-/*!
- \file vpKalmanFilter.h
- \brief Generic kalman filtering implementation
-*/
-
-/*!
- \class vpKalmanFilter
- \brief This class provides a generic Kalman filtering algorithm along with
- some specific state model (constant velocity, constant acceleration)
- which are implemented in the vpLinearKalmanFilterInstantiation class.
-
- The state evolution equation is given by:
- \f[
- {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
- \f]
- where \f${\bf x}_{k}\f$ is the unknown state at iteration \f$k\f$.
-
- The measurement equation is given by:
- \f[
- {\bf z}_k = {\bf H} {\bf x}_k + {\bf r}_k
- \f]
- where \f${\bf z}_{k}\f$ is the measure (also named observation) at iteration \f$k\f$.
-
- The predicted state is obtained by:
- \f[
- {\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}
- \f]
- \f[
- {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
- + {\bf Q}_k \f]
- where
- <ul>
- <li> \f$ {\bf x}_{k|k-1}\f$ is the prediction of the state,
- <li> \f$ {\bf P}_{k \mid k-1}\f$ is the state prediction covariance matrix.
- </ul>
- Filtering equation are:
- \f[
- {\bf W}_k = {\bf P}_{k \mid k-1} {\bf H}^T
- \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}
- \f]
- \f[
- {\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k -
- {\bf H x}_{k \mid k-1} \right]
- \f]
- \f[
- {\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H} \right) {\bf P}_{k \mid
- k-1} \f]
-
- where \f$ {\bf W}_k \f$ is the filter gain.
-
- Notice that there is a recursion for the inverse covariance
- \f[
- {\bf P}_{k \mid k}^{-1}= {\bf P}_{k \mid k-1}^{-1} + {\bf H}^T {\bf
- R}^{-1} {\bf H}
- \f]
- where \f${\bf P}_{k \mid k}^{-1}\f$ is the inverse of the covariance matrix.
-
- ViSP provides different state evolution models implemented in the
- vpLinearKalmanFilterInstantiation class.
-*/
-class VISP_EXPORT vpKalmanFilter
-{
-protected :
- //bool init_done ;
-
- //! Filter step or iteration. When set to zero, initialize the filter.
- long iter ;
-
- //! Size of the state vector \f${\bf x}_k\f$.
- unsigned int size_state ;
- //! Size of the measure vector \f${\bf z}_k\f$.
- unsigned int size_measure ;
- //! Number of signal to filter.
- unsigned int nsignal ;
-
- //! When set to true, print the content of internal variables during filtering() and prediction().
- bool verbose_mode;
-
-public:
- vpKalmanFilter() ;
- vpKalmanFilter(unsigned int n_signal) ;
- vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
- /*! Destructor that does noting. */
- virtual ~vpKalmanFilter() {};
- /*!
- Set the number of signal to filter.
- */
- void setNumberOfSignal(unsigned int n_signal)
- {
- this->nsignal = n_signal;
- }
-
- // int init() { return init_done ; }
- void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
- void prediction() ;
- void filtering(vpColVector &z) ;
- /*!
- Return the size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
- */
- unsigned int getStateSize() { return size_state; };
- /*!
- Return the size of the measure vector \f${\bf z}_{(k)}\f$ for one signal.
- */
- unsigned int getMeasureSize() { return size_measure; };
- /*!
- Return the number of signal to filter.
- */
- unsigned int getNumberOfSignal() { return nsignal; };
- /*!
- Return the iteration number.
- */
- int getIteration() { return iter ; }
- /*!
- Sets the verbose mode.
- \param on : If true, activates the verbose mode which consists in printing the Kalman
- filter internal values.
- */
- void verbose(bool on) { verbose_mode = on;};
-
-public:
- /*!
- The updated state estimate \f${\bf x}_{k \mid k} \f$ where
- \f${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k
- \left[ {\bf z}_k - {\bf H x}_{k \mid k-1} \right]\f$.
- */
- vpColVector Xest ;
- /*!
- The predicted state \f${\bf x}_{k \mid k-1} \f$ where
- \f$ {\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}\f$.
- */
- vpColVector Xpre ;
- //! Transition matrix \f${\bf F}\f$ that describes the evolution of the state.
- vpMatrix F ;
-
- //! Matrix \f${\bf H}\f$ that describes the evolution of the measurements.
- vpMatrix H ;
-
- //! Measurement noise covariance matrix \f${\bf R}\f$.
- vpMatrix R ;
- //! Process noise covariance matrix \f${\bf Q}\f$.
- vpMatrix Q ;
- /*! Sampling time \f$\Delta t\f$ in second between two succesive
- iterations. Only used in some specific state models implemented
- in vpLinearKalmanFilterInstantiation.*/
- double dt ;
-
-protected:
- /*!
- The state prediction covariance \f${\bf P}_{k \mid k-1} \f$ where
- \f$ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
- + {\bf Q}_k\f$.
- */
- vpMatrix Ppre ;
-
- /*!
- The updated covariance of the state \f${\bf P}_{k \mid k}\f$
- where \f${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H}
- \right) {\bf P}_{k \mid k-1}\f$.
- */
- vpMatrix Pest ;
-
- /*!
- Filter gain \f${\bf W}_k\f$ where \f$ {\bf W}_k = {\bf P}_{k
- \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf
- R}_k \right]^{-1}\f$.
- */
- vpMatrix W ;
-
- //! Identity matrix \f$ \bf I\f$.
- vpMatrix I ;
-} ;
-
-
-
-#endif
diff --git a/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
deleted file mode 100644
index dbd55d4..0000000
--- a/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
+++ /dev/null
@@ -1,857 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLinearKalmanFilterInstantiation.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Kalman filtering.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpLinearKalmanFilterInstantiation.cpp
- \brief Implementation of some specific Kalman filters.
-*/
-
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-
-#include <math.h>
-#include <stdlib.h>
-
-/*!
-
- Initialize the Kalman filter material depending on the selected
- state model set with setStateModel(). This function is provided as a
- wrapper over all the other initializer functions like
- initStateConstVel_MeasurePos(),
- initStateConstVelWithColoredNoise_MeasureVel(),
- initStateConstAccWithColoredNoise_MeasureVel().
-
- \warning It is requiered to set the state model before using this method.
-
- \param n_signal : Number of signal to filter.
-
- \param sigma_state : Vector that contains the variance of the state
- noise. The dimension of this vector is equal to the state vector
- \f${\bf x}\f$ size multiplied by the number of signal to
- filter. Values are used to initialize the \f${\bf Q}\f$ state
- covariance matrix.
-
- \param sigma_measure : Vector that contains the variance of the
- measurement noise. The dimension of this vector is equal to the
- measure vector \f${\bf x}\f$ size multiplied by the number of signal
- to filter. Values are used to initialize the \f${\bf R}\f$ measure
- covariance matrix.
-
- \param rho : Degree of correlation between successive accelerations. Values
- are in [0:1[.
-
- \param delta_t : Sampling time \f$\Delta t\f$ expressed is
- second. Depending on the filter modelization, this value may not be
- used. This is for example the case for the
- vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel model
- implemented in initStateConstVelWithColoredNoise_MeasureVel().
-
- \exception vpException::badValue : Bad rho value wich is not in [0:1[.
-
- \exception vpException::notInitialized : If the state model is not
- initialized. To initialize it you need to call setStateModel().
-
- The example below shows how to initialize the filter for a one
- dimensional signal.
-
- \code
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-
-int main()
-{
- vpLinearKalmanFilterInstantiation kalman;
- // Select a constant velocity state model with colored noise
- // Measures are velocities
- kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
-
- // Initialise the filter for a one dimension signal
- int signal = 1;
- vpColVector sigma_state(2); // State vector size is 2
- vpColVector sigma_measure(1); // Measure vector size is 1
- double rho = 0.9;
- double dummy = 0; // non used parameter for the selected state model
-
- kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy);
-}
- \endcode
-
- The example below shows a more complete example to filter a two
- dimensional target trajectory with an estimation of the target
- velocities from velocity measures.
-
- \code
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-
-int main()
-{
- vpLinearKalmanFilterInstantiation kalman;
-
- // Set the constant velocity state model used for the filtering
- vpLinearKalmanFilterInstantiation::vpStateModel model;
- model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
- kalman.setStateModel(model);
-
- // We are now able to retrieve the size of the state vector and measure vector
- int state_size = kalman.getStateSize();
-
- // Filter the x and y velocities of a target (2 signals are to consider)
- int nsignal = 2;
-
- // Initialize the filter parameters:
- // - Firstly, the state variance
- int size = state_size*nsignal;
- vpColVector sigma_state(size);
- sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
- sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
- vpColVector sigma_measure(nsignal); // 2 velocity measures available
- sigma_measure[0] = 0.03; // Variance on the x velocity measure
- sigma_measure[1] = 0.06; // Variance on the y velocity measure
- // - Thirdly, the correlation between succesive accelerations
- double rho = 0.9;
-
- double dummy = 0; // non used parameter for the selected state model
-
- // Initialize the filter
- // The state model was set before
- kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
-
- // Does the filtering
- vpColVector vm(2); // Measured velocities
- for ( ; ; ) {
- // Get the two dimentional velocity measures
- // vm[0] = ...;
- // vm[1] = ...;
-
- // Compute the filtering and the prediction
- kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
- std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
- std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
- << std::endl;
- // The one step prediction is available in kalman.Xpre variable
- }
-}
- \endcode
- */
-void
-vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double rho, double delta_t)
-{
- switch (model) {
- case stateConstVelWithColoredNoise_MeasureVel:
- initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state,
- sigma_measure, rho);
- break;
- case stateConstVel_MeasurePos:
- initStateConstVel_MeasurePos(n_signal, sigma_state,
- sigma_measure, delta_t);
- break;
- case stateConstAccWithColoredNoise_MeasureVel:
- initStateConstAccWithColoredNoise_MeasureVel(n_signal, sigma_state,
- sigma_measure, rho, delta_t);
- break;
- case unknown:
- vpERROR_TRACE("Kalman state model is not set") ;
- throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
- }
-}
-
-/*!
- Modelisation of a constant speed state model with white noise. The
- measure is assumed to be the position of the target.
-
- The considered state model is the following
-
- \f[
- \left\{
- \begin{array}{rlrl}
- x_{(k+1)} & = x_{(k)} & + \Delta t \; {\dot{x}}_{(k)} & + {w_1}_{(k)} \\
- {\dot{x}}_{(k+1)} & = & {\dot{x}}_{(k)} &+{w_2}_{(k)}
- \end{array}
- \right.
- \f]
-
- The terms \f${w_1}_{(k)}\f$ and \f${w_2}_{(k)}\f$ account for deviations from the assumed
- constant velocity trajectory. They are assumed zero-mean, white,
- mutually uncorrelated, stationary random variable with variance
- \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$.
-
- We recall that the recursive state evolution equation is given by
- \f[
- {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
- \f]
-
- From this state model, the transition matrix \f${\bf F}\f$ and the
- state covariance matrix \f${\bf Q}\f$ are given by:
-
- \f[
- {\bf F} =
- \left[
- \begin{array}{cc}
- 1 & \Delta t\\
- 0 & 1
- \end{array}
- \right]
- \f]
-
- and
-
- \f[
- {\bf Q} = \sigma^2_Q
- \left[
- \begin{array}{cc}
- \frac{1}{3}\Delta t^3 & \frac{1}{2}\Delta t^2\\
- \frac{1}{2}\Delta t^2 & \Delta t
- \end{array}
- \right]
- \f]
-
- The initial value of the state vector at iteration 0 is set to:
- \f[
- {\bf x_{(0)}} =
- \left[
- \begin{array}{c}
- z_{(0)}\\
- 0
- \end{array}
- \right]
- \f]
-
- The value at iteration 1 is set to:
- \f[
- {\bf x_{(1)}} =
- \left[
- \begin{array}{c}
- z_{(1)}\\
- (z_{(1)} - z_{(0)})/ \Delta t
- \end{array}
- \right]
- \f]
-
- The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
-
- \f[
- {\bf P_{(0|0)}} =
- \left[ \begin{array}{cc}
- \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\
- \frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t + \frac{\sigma^2_R}{2 \Delta t^2}
- \end{array}
- \right]
- \f]
-
- \param n_signal : Number of signal to filter.
-
- \param sigma_state : Vector that fix the variance of the state covariance matrix
- \f$[\sigma^2_Q \; 0]^T\f$. The dimension of
- this vector is 2 multiplied by the number of signal to filter.
-
- \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
- noise. The dimension of this vector is equal to the number of signal
- to filter.
-
- \param delta_t : Sampling time \f$\Delta t\f$ expressed is second.
-
-*/
-void
-vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_signal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double delta_t )
-{
- // init_done = true ;
- setStateModel(stateConstVel_MeasurePos);
-
- init(size_state, size_measure, n_signal);
-
- iter = 0;
- Pest = 0;
- Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
- this->dt = delta_t ;
-
- double dt2 = dt*dt ;
- double dt3 = dt2*dt ;
-
- for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
- // State model
- // | 1 dt |
- // F = | |
- // | 0 1 |
-
- F[2*i][2*i] = 1 ;
- F[2*i][2*i+1] = dt ;
- F[2*i+1][2*i+1] = 1 ;
-
- // Measure model
- H[i][2*i] = 1 ;
- H[i][2*i+1] = 0 ;
-
- double sR = sigma_measure[i] ;
- double sQ = sigma_state[2*i] ; // sigma_state[2*i+1] is not used
-
- // Measure noise
- R[i][i] = sR ;
-
- // State covariance matrix 6.2.2.12
- Q[2*i][2*i] = sQ * dt3/3;
- Q[2*i][2*i+1] = sQ * dt2/2;
- Q[2*i+1][2*i] = sQ * dt2/2;
- Q[2*i+1][2*i+1] = sQ * dt;
-
- Pest[2*i][2*i] = sR ;
- Pest[2*i][2*i+1] = sR/(2*dt) ;
- Pest[2*i+1][2*i] = sR/(2*dt) ;
- Pest[2*i+1][2*i+1] = sQ*2*dt/3.0+ sR/(2*dt2) ;
- }
-}
-
-/*!
-
- Modelisation of a constant speed state model with colored noise. The
- measure is assumed to be the velocity of the target.
-
- This state model assume that there is some memory associated with
- noise measurements as acceleration terms. They can be represented as
- remaining correlated (or colored) over succesive time intervals,
- leading to the following state model:
-
- \f[
- \left\{
- \begin{array}{rll}
- x_{(k+1)} & = x_{(k)} + \nu_{(k)} &\\
- \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{(k)}
- \end{array}
- \right.
- \f]
-
- The term \f$w_{(k)}\f$ account for deviations from the assumed
- constant velocity trajectory. It is assumed zero-mean, white,
- mutually uncorrelated, stationary random variable with variance
- \f$\sigma^2_Q\f$. The term \f$\rho\f$ is the degree of correlation
- between successive accelerations. Values can range from 0 to 1.
-
- We recall that the recursive state evolution equation is given by
- \f[
- {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
- \f]
-
- From this state model, the transition matrix \f${\bf F}\f$ and the
- state covariance matrix \f${\bf Q}\f$ are given by:
-
- \f[
- {\bf F} =
- \left[
- \begin{array}{cc}
- 1 & 1\\
- 0 & \rho
- \end{array}
- \right]
- \f]
-
- and
-
- \f[
- {\bf Q} =
- \left[
- \begin{array}{cc}
- 0 & 0\\
- 0 & \sigma^2_Q
- \end{array}
- \right]
- \f]
-
- The measurement model is given by:
- \f[
- z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)}
- \f]
-
- where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the
- velocity and \f$r_{(k)}\f$ is the measurement noise, assumed
- zero-mean, white mutually uncorrelated stationary random variables
- with variance \f$\sigma^2_R\f$, giving the covariance matrix:
-
- \f[
- {\bf R} = \left[\sigma^2_R\right]
- \f]
-
- The initial value of the state vector is set to:
- \f[
- {\bf x_{(0)}} =
- \left[
- \begin{array}{c}
- z_{(0)}\\
- 0
- \end{array}
- \right]
- \f]
-
- The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
-
- \f[
- {\bf P_{(0|0)}} =
- \left[ \begin{array}{cc}
- \sigma^2_R & 0\\
- 0 & \sigma^2_Q/(1-\rho^2)
- \end{array}
- \right]
- \f]
-
- \param n_signal : Number of signal to filter.
-
- \param sigma_state : Vector that fix the variance of the state covariance matrix
- \f$[0 \; \sigma^2_Q]^T\f$. The dimension of
- this vector is 2 multiplied by the number of signal to filter.
-
- \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
- noise. The dimension of this vector is equal to the number of signal
- to filter.
-
- \param rho : Degree of correlation between successive accelerations. Values
- are in [0:1[.
-
- \exception vpException::badValue : Bad rho value wich is not in [0:1[.
-
- The example below shows how to filter a two dimensional target
- trajectory with an estimation of the target velocity from velocity
- measures.
-
- \code
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-
-int main()
-{
- vpLinearKalmanFilterInstantiation kalman;
- // Filter the x and y velocities of a target (2 signals are to consider)
- int nsignal = 2;
-
- // Initialize the filter parameters:
- // - Firstly, the state variance
- vpColVector sigma_state(4); // 4 = 2 for the state size x 2 signal
- sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
- sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
- vpColVector sigma_measure(nsignal); // 2 velocity measures available
- sigma_measure[0] = 0.03; // Variance on the x velocity measure
- sigma_measure[1] = 0.06; // Variance on the y velocity measure
- // - Thirdly, the correlation between succesive accelerations
- double rho = 0.9;
-
- // Initialize the filter
- kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho);
-
- // Does the filtering
- vpColVector vm(2); // Measured velocities
- for ( ; ; ) {
- // Get the two dimentional velocity measures
- // vm[0] = ...;
- // vm[1] = ...;
-
- // Compute the filtering and the prediction
- kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
- std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
- std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
- << std::endl;
- // The one step prediction is available in kalman.Xpre variable
- }
-}
- \endcode
-*/
-void
-vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(unsigned int n_signal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double rho)
-{
- if ((rho < 0) || (rho >= 1)) {
- vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
- throw(vpException(vpException::badValue, "Bad rho value")) ;
- }
-
- setStateModel(stateConstVelWithColoredNoise_MeasureVel);
-
- init(size_state, size_measure, n_signal);
-
- iter = 0;
- Pest = 0;
- Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
-
- for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
- // State model
- // | 1 1 |
- // F = | |
- // | 0 rho |
-
- F[2*i][2*i] = 1 ;
- F[2*i][2*i+1] = 1 ;
- F[2*i+1][2*i+1] = rho ;
-
- // Measure model
- H[i][2*i] = 1 ;
- H[i][2*i+1] = 0 ;
-
- double sR = sigma_measure[i] ;
- double sQ = sigma_state[2*i+1] ; // sigma_state[2*i] is not used
-
- // Measure noise
- R[i][i] = sR ;
-
- // State covariance matrix
- Q[2*i][2*i] = 0 ;
- Q[2*i][2*i+1] = 0;
- Q[2*i+1][2*i] = 0;
- Q[2*i+1][2*i+1] = sQ ;
-
- Pest[2*i][2*i] = sR ;
- Pest[2*i][2*i+1] = 0. ;
- Pest[2*i+1][2*i] = 0 ;
- Pest[2*i+1][2*i+1] = sQ/(1-rho*rho) ;
- }
-}
-
-/*!
-
- Modelisation of a constant acceleration state model with colored noise. The
- measure is assumed to be the velocity of the target.
-
- This state model assume that there is some memory associated with
- noise measurements as acceleration terms. They can be represented as
- remaining correlated (or colored) over succesive time intervals,
- leading to the following state model:
-
- \f[
- \left\{
- \begin{array}{rll}
- x_{(k+1)} & = x_{(k)} + \Delta t \; \dot{x}_{(k)} + \nu_{(k)} &\\
- \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{1(k)} \\
- \dot{x}_{(k+1)} & = \dot{x}_{(k)} &+w_{2(k)}\\
- \end{array}
- \right.
- \f]
-
- The terms \f$w_{1(k)}\f$ and \f$w_{2(k)}\f$ account for deviations
- from the assumed constant acceleration trajectory. They are assumed
- zero-mean, white, mutually uncorrelated, stationary random variable
- with variance \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$. The term
- \f$\rho\f$ is the degree of correlation between successive
- accelerations. Values can range from 0 to 1.
-
- We recall that the recursive state evolution equation is given by
- \f[
- {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
- \f]
-
- From this state model, the transition matrix \f${\bf F}\f$ and the
- state covariance matrix \f${\bf Q}\f$ are given by:
-
- \f[
- {\bf F} =
- \left[
- \begin{array}{ccc}
- 1 & 1 & \Delta t\\
- 0 & \rho & 0 \\
- 0 & 0 & 1
- \end{array}
- \right]
- \f]
-
- and
-
- \f[
- {\bf Q} =
- \left[
- \begin{array}{ccc}
- 0 & 0 & 0\\
- 0 & \sigma^2_{Q_1} & 0\\
- 0 & 0& \sigma^2_{Q_2} \\
- \end{array}
- \right]
- \f]
-
- The measurement model is given by:
- \f[
- z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)}
- \f]
-
- where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the velocity and \f$r_{(k)}\f$
- is the measurement noise, assumed zero-mean, white mutually
- uncorrelated stationary random variables with variance
- \f$\sigma^2_R\f$, giving the covariance matrix:
-
- \f[
- {\bf R} = \left[\sigma^2_R\right]
- \f]
-
- The initial value of the state vector is set to:
- \f[
- {\bf x_{(0)}} =
- \left[
- \begin{array}{c}
- z_{(0)}\\
- 0 \\
- 0
- \end{array}
- \right]
- \f]
-
- The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
-
- \f[
- {\bf P_{(0|0)}} =
- \left[ \begin{array}{ccc}
- \sigma^2_R & 0 & \sigma^2_R / \Delta t\\
- 0 & \sigma^2_{Q_1}/(1-\rho^2) & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t \\
- \sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t & (2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2
- \end{array}
- \right]
- \f]
-
- \param n_signal : Number of signal to filter.
-
- \param sigma_state : Vector that fix the variance of the state
- covariance matrix \f$[0 \; \sigma^2_{Q_1} \;
- \sigma^2_{Q_2}]^T\f$. The dimension of this vector is 3 multiplied
- by the number of signal to filter.
-
- \param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
- noise. The dimension of this vector is equal to the number of signal
- to filter.
-
- \param rho : Degree of correlation between successive accelerations. Values
- are in [0:1[.
-
- \param delta_t : Sampling time \f$\Delta t\f$ expressed is second.
-
- \exception vpException::badValue : Bad rho value wich is not in [0:1[.
-
- The example below shows how to filter a two dimensional target
- trajectory with an estimation of the target velocity from velocity
- measures.
-
- \code
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-
-int main()
-{
- vpLinearKalmanFilterInstantiation kalman;
- // Filter the x and y velocities of a target (2 signals are to consider)
- int nsignal = 2;
-
- // Initialize the filter parameters:
- // - Firstly, the state variance
- vpColVector sigma_state(6); // 6 = 3 for the state size x 2 signal
- sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
- sigma_state[2] = 0.001; // Variance on the acceleration for the 1st signal (x)
- sigma_state[4] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- sigma_state[5] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
- vpColVector sigma_measure(nsignal); // 2 velocity measures available
- sigma_measure[0] = 0.03; // Variance on the x velocity measure
- sigma_measure[1] = 0.06; // Variance on the y velocity measure
- // - Thirdly, the correlation between succesive accelerations
- double rho = 0.9;
- // - Lastly, the sampling time
- double dt = 0.020; // 20 ms
-
- // Initialize the filter
- kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho, dt);
-
- // Does the filtering
- vpColVector vm(2); // Measured velocities
- for ( ; ; ) {
- // Get the two dimentional velocity measures
- // vm[0] = ...;
- // vm[1] = ...;
-
- // Compute the filtering and the prediction
- kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
- std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
- std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
- << std::endl;
- // The one step prediction is available in kalman.Xpre variable
- }
-}
- \endcode
-
-*/
-void
-vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(unsigned int n_signal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double rho,
- double delta_t)
-{
- if ((rho < 0) || (rho >= 1)) {
- vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
- throw(vpException(vpException::badValue, "Bad rho value")) ;
- }
- setStateModel(stateConstAccWithColoredNoise_MeasureVel);
-
- init(size_state, size_measure, n_signal);
-
- iter = 0;
- Pest = 0;
- Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
- this->dt = delta_t;
- // initialise les matrices decrivant les modeles
- for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
- // State model
- // | 1 1 dt |
- // F = | o rho 0 |
- // | 0 0 1 |
-
- F[3*i][3*i] = 1 ;
- F[3*i][3*i+1] = 1 ;
- F[3*i][3*i+2] = dt ;
- F[3*i+1][3*i+1] = rho ;
- F[3*i+2][3*i+2] = 1 ;
-
- // Measure model
- H[i][3*i] = 1 ;
- H[i][3*i+1] = 0 ;
- H[i][3*i+2] = 0 ;
-
- double sR = sigma_measure[i] ;
- double sQ1 = sigma_state[3*i+1] ;
- double sQ2 = sigma_state[3*i+2] ;
-
- // Measure noise
- R[i][i] = sR ;
-
- // State covariance matrix
- Q[3*i+1][3*i+1] = sQ1;
- Q[3*i+2][3*i+2] = sQ2;
-
- Pest[3*i][3*i] = sR ;
- Pest[3*i][3*i+1] = 0. ;
- Pest[3*i][3*i+2] = sR/dt ;
- Pest[3*i+1][3*i+1] = sQ1/(1-rho*rho) ;
- Pest[3*i+1][3*i+2] = -rho*sQ1/((1-rho*rho)*dt) ;
- Pest[3*i+2][3*i+2] = (2*sR+sQ1/(1-rho*rho))/(dt*dt) ;
- // complete the lower triangle
- Pest[3*i+1][3*i] = Pest[3*i][3*i+1];
- Pest[3*i+2][3*i] = Pest[3*i][3*i+2];
- Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2];
- }
-}
-
-/*!
-
- Do the filtering and prediction of the measure signal.
-
- \param z : Measures \f${\bf z}_k\f$ used to initialise the filter. The dimension of
- this vector is equal to the number of signal to filter (given by
- getNumberOfSignal()) multiplied by the size of the measure vector
- (given by getMeasureSize()) .
-
- \exception vpException::notInitialized : If the filter is not
- initialized. To initialize the filter see initFilter().
-
-*/
-void
-vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
-{
- if (nsignal < 1) {
- vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter") ;
- throw(vpException(vpException::notInitialized,
- "Bad signal number")) ;
- }
-
- // Specific initialization of the filter that depends on the state model
- if (iter == 0) {
- Xest = 0;
- switch (model) {
- case stateConstVel_MeasurePos:
- case stateConstVelWithColoredNoise_MeasureVel:
- case stateConstAccWithColoredNoise_MeasureVel:
- for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
- Xest[size_state*i] = z[i] ;
- }
- prediction();
- // init_done = true;
- break;
- case unknown:
- vpERROR_TRACE("Kalman state model is not set") ;
- throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
- break;
- }
- iter ++;
-
- return;
- }
- else if (iter == 1) {
- 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 ;
- }
- prediction();
- iter ++;
-
- return;
- }
- }
-
- filtering(z);
- prediction();
-
-}
diff --git a/src/math/kalman/vpLinearKalmanFilterInstantiation.h b/src/math/kalman/vpLinearKalmanFilterInstantiation.h
deleted file mode 100644
index 5ff208b..0000000
--- a/src/math/kalman/vpLinearKalmanFilterInstantiation.h
+++ /dev/null
@@ -1,179 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLinearKalmanFilterInstantiation.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Kalman filtering.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpLinearKalmanFilterInstantiation_h
-#define vpLinearKalmanFilterInstantiation_h
-
-#include <visp/vpKalmanFilter.h>
-
-#include <math.h>
-
-/*!
- \file vpLinearKalmanFilterInstantiation.h
- \brief Implementation of some specific linear Kalman filters.
-*/
-
-/*!
- \class vpLinearKalmanFilterInstantiation
- \brief This class provides an implementation of some specific linear Kalman filters.
-*/
-class VISP_EXPORT vpLinearKalmanFilterInstantiation : public vpKalmanFilter
-{
- public:
- /*!
- Selector used to set the Kalman filter state model.
- */
- typedef enum {
- /*! Consider the state as a constant velocity model with white
- noise. Measures available are the succesive positions of the
- target. To know more about this state model, see
- initStateConstVel_MeasurePos(). */
- stateConstVel_MeasurePos,
- /*! Consider the state as a constant velocity model with colored noise
- measurements as acceleration terms. Measured available are the
- velocities of the target. To know more about this state model,
- see initStateConstVelWithColoredNoise_MeasureVel(). */
- stateConstVelWithColoredNoise_MeasureVel,
- /*! Consider the state as a constant acceleration model with colored noise
- measurements as acceleration terms. Measured available are the
- velocities of the target. To know more about this state model,
- see initStateConstAccWithColoredNoise_MeasureVel(). */
- stateConstAccWithColoredNoise_MeasureVel,
- /*! Used to indicate that the state model is not initialized. */
- unknown
- } vpStateModel;
-
- /*!
- Default linear Kalman filter.
-
- By default the state model is unknown and set to
- vpLinearKalmanFilterInstantiation::unknown.
- */
- vpLinearKalmanFilterInstantiation() : model(unknown)
- {
- };
-
- /*! Destructor that does nothng. */
- virtual ~vpLinearKalmanFilterInstantiation() {};
- /*!
- Return the current state model.
- */
- inline vpStateModel getStateModel() {
- return model;
- }
- void filter(vpColVector &z);
-
- /*! @name Generic linear filter initializer */
- //@{
- inline void setStateModel(vpStateModel model);
-
- void initFilter(unsigned int nsignal, vpColVector &sigma_state,
- vpColVector &sigma_measure, double rho, double dt);
- //@}
-
- /*! @name Linear filter initializer with constant velocity models */
- //@{
- void initStateConstVel_MeasurePos(unsigned int nsignal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double dt);
- void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double rho);
- //@}
-
- /*! @name Linear filter initializer with constant acceleration models */
- //@{
- void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal,
- vpColVector &sigma_state,
- vpColVector &sigma_measure,
- double rho,
- double dt);
- //@}
-
- protected:
- vpStateModel model;
-
-} ;
-
-/*!
- Set the Kalman state model. Depending on the state model, we set
- the state vector size and the measure vector size.
-
- The example below shows how to use this method and then to get the
- size of the state and measure vectors.
-
- \code
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-
-int main()
-{
- vpLinearKalmanFilterInstantiation kalman;
-
- kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
- std::cout << "State vector size: " << kalman.getStateSize() << std::endl; // Value is 2
- std::cout << "Measure vector size: " << kalman.getMeasureSize() << std::endl; // Value is 1
-}
- \endcode
-*/
-void vpLinearKalmanFilterInstantiation::setStateModel(vpStateModel mdl) {
- this->model = mdl;
- switch(model) {
- case stateConstVel_MeasurePos:
- case stateConstVelWithColoredNoise_MeasureVel:
- size_state = 2;
- size_measure = 1;
- break;
- case stateConstAccWithColoredNoise_MeasureVel:
- size_state = 3;
- size_measure = 1;
- break;
- case unknown:
- size_state = 0;
- size_measure = 0;
- break;
- }
-}
-
-
-#endif
diff --git a/src/math/matrix/vpColVector.cpp b/src/math/matrix/vpColVector.cpp
deleted file mode 100644
index 7f35664..0000000
--- a/src/math/matrix/vpColVector.cpp
+++ /dev/null
@@ -1,863 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpColVector.cpp 5218 2015-01-28 10:27:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Provide some simple operation on column vectors.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpColVector.cpp
- \brief Class that provides a data structure for the column vectors as well
- as a set of operations on these vectors
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpRotationVector.h>
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <string.h> //EM gcc 4.3
-#include <math.h> //EM gcc 4.3
-
- //! operator addition of two vectors
-vpColVector
-vpColVector::operator+(const vpColVector &m) const
-{
- if (getRows() != m.getRows() ) {
- throw(vpException(vpException::dimensionError,
- "Bad size during vpColVector (%dx1) and vpColVector (%dx1) addition",
- getRows(), m.getRows())) ;
- }
- vpColVector v(rowNum);
-
- for (unsigned int i=0;i<rowNum;i++)
- v[i] = (*this)[i] + m[i];
- return v;
-}
-
-//! operator dot product
-double
-vpColVector::operator*(const vpColVector &m) const
-{
- double v = 0 ;
-
- for (unsigned int i=0;i<rowNum;i++)
- v += (*this)[i] * m[i];
- return v;
-}
-
-//! operator dot product
-vpMatrix vpColVector::operator*(const vpRowVector &m) const
-{
-
- return (vpMatrix)*this*(vpMatrix)m;
-}
-
- //! operator substraction of two vectors V = A-v
-vpColVector vpColVector::operator-(const vpColVector &m) const
-{
- if (getRows() != m.getRows() ) {
- throw(vpException(vpException::dimensionError,
- "Bad size during vpColVector (%dx1) and vpColVector (%dx1) substraction",
- getRows(), m.getRows())) ;
- }
- vpColVector v(rowNum);
-
- for (unsigned int i=0;i<rowNum;i++)
- v[i] = (*this)[i] - m[i];
- return v;
-}
-
-vpColVector::vpColVector (vpColVector &m, unsigned int r, unsigned int nrows)
-{
- if ( (r+nrows) > m.getRows() )
- {
- vpERROR_TRACE("\n\t\t SubvpMatrix larger than vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::subMatrixError,
- "\n\t\t SubvpMatrix larger than vpColVector")) ;
- }
- init(m, r, 0, nrows, 1);
-}
-
-
-vpColVector::vpColVector (const vpRotationVector &v){
- resize(v._size);
- memcpy(data, v.r, v._size*sizeof(double)) ;
-}
-
-
- //! Operator A = -A
-vpColVector vpColVector::operator-() const
-{
- vpColVector A ;
- try {
- A.resize(rowNum) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- double *vd = A.data ; double *d = data ;
-
- for (unsigned int i=0; i<rowNum; i++)
- *(vd++)= - (*d++);
-
- return A;
-}
-
-//! operator multiplication by a scalar V = A * x
-vpColVector vpColVector::operator*(double x) const
-{
- vpColVector v;
- try {
- v.resize(rowNum) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- double *vd = v.data ; double *d = data ;
-
- for (unsigned int i=0;i<rowNum;i++)
- *(vd++)= (*d++) * x;
- return v;
-}
-
-/*!
- Copy from a matrix
- \warning Handled with care m should be a 1 row matrix.
-*/
-vpColVector &vpColVector::operator=(const vpMatrix &m)
-{
- if (m.getCols() !=1)
- {
- vpTRACE(" m should be a 1 cols matrix ") ;
- throw (vpException(vpException::dimensionError,"m should be a 1 cols matrix "));
- }
-
- try {
- resize(m.getRows());
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- memcpy(data, m.data, rowNum*sizeof(double)) ;
-
- /*
- double *md = m.data ; double *d = data ;
- for (int i=0;i<rowNum;i++)
- *(d++)= *(md++) ;
- */
- /*
- for (int i=0; i<rowNum; i++) {
- for (int j=0; j<colNum; j++) {
- rowPtrs[i][j] = m.rowPtrs[i][j];
- }
- }*/
- return *this;
-}
-
- //! Copy operator. Allow operation such as A = v
-vpColVector &vpColVector::operator=(const vpColVector &v)
-{
- unsigned int k = v.rowNum ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
- //
-
- memcpy(data, v.data, rowNum*sizeof(double)) ;
- /*
- double *vd = m.data ; double *d = data ;
- for (int i=0;i<rowNum;i++)
- *(d++)= *(vd++) ;
-
-
- for (int i=0; i<rowNum; i++) {
- for (int j=0; j<colNum; j++) {
- rowPtrs[i][j] = v.rowPtrs[i][j];
- }
- }
-*/
- return *this;
-}
-
-/*!
- Copy operator.
- Allows operation such as A << v
- \code
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector A, B(5);
- for (unsigned int i=0; i<B.size(); i++)
- B[i] = i;
- A << B;
- std::cout << "A: \n" << A << std::endl;
-}
- \endcode
- In column vector A we get:
- \code
-A:
-0
-1
-2
-3
-4
- \endcode
- */
-vpColVector & vpColVector::operator<<(const vpColVector &v)
-{
- *this = v;
- return *this;
-}
-
-/*!
- Assigment operator. Allow operation such as A = *v
-
- The following example shows how to use this operator.
- \code
-#include <visp/vpColVector.h>
-
-int main()
-{
- size_t n = 5;
- vpColVector A(n);
- double *B = new double [n];
- for (unsigned int i = 0; i < n; i++)
- B[i] = i;
- A << B;
- std::cout << "A: \n" << A << std::endl;
- delete [] B;
-}
- \endcode
- It produces the following output:
- \code
-A:
-0
-1
-2
-3
-4
- \endcode
- */
-vpColVector & vpColVector::operator<<( double *x )
-{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
- rowPtrs[i][j] = *x++;
- }
- }
- return *this;
-}
-
-//! initialisation each element of the vector is x
-vpColVector & vpColVector::operator=(double x)
-{
- double *d = data ;
-
- for (unsigned int i=0;i<rowNum;i++)
- *(d++)= x ;
- /*
- for (int i=0; i<rowNum; i++) {
- for (int j=0; j<colNum; j++) {
- rowPtrs[i][j] = x;
- }
- }*/
- return *this;
-}
-
-/*
- \brief Transpose the row vector A
-
- A is defined inside the class
-
- \return A^T
-*/
-vpRowVector vpColVector::t() const
-{
- vpRowVector v ;
- try {
- v.resize(rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- memcpy(v.data, data, rowNum*sizeof(double)) ;
- /*
- // premiere optimisation
- double *vd = m.data ; double *d = data ;
- for (int i=0;i<rowNum;i++)
- *(d++)= *(vd++) ;
- */
-
- /*
- // solution brute
- for (int i=0;i<rowNum;i++)
- v[i] = (*this)[i];
- */
- return v;
-}
-/*!
- \relates vpColVector
- \brief multiplication by a scalar Ci = x*Bi
-*/
-vpColVector operator*(const double &x,const vpColVector &B)
-{
- vpColVector v1 ;
- v1 = B*x ;
- return v1 ;
-}
-
-vpColVector::vpColVector (const vpColVector &v) : vpMatrix(v)
-{
-}
-
-vpColVector::vpColVector (vpMatrix &m, unsigned int j) : vpMatrix(m, 0, j, m.getRows(), 1)
-{
-}
-
-
-/*!
- \relates vpColVector
- \brief compute the dot product of two vectors C = a.b
-*/
-double
-vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
-{
- if (a.data==NULL)
- {
- vpERROR_TRACE("vpColVector a non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
- if (b.data==NULL)
- {
- vpERROR_TRACE("vpColVector b non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
- double *ad = a.data ; double *bd = b.data ;
-
- double c = 0 ;
- for (unsigned int i=0 ; i < a.getRows() ; i++)
- c += *(ad++)* *(bd++) ;
- // vpMatrix c = (a.t() * b);
- // return c[0][0];
- return c ;
-}
-
-/*!
- \relates vpColVector
- \brief normalise the vector
-
- \f[
- {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
- \f]
-*/
-// vpColVector &vpColVector::normalize(vpColVector &x) const
-// {
-// x = x/sqrt(x.sumSquare());
-
-// return x;
-// }
-
-
-/*!
- \relates vpColVector
- \brief normalise the vector
-
- \f[
- {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
- \f]
-*/
-vpColVector &vpColVector::normalize()
-{
-
- double sum_square = sumSquare() ;
-
- //if (sum != 0.0)
- if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
- *this /= sqrt(sum_square) ;
-
- // If sum = 0, we have a nul vector. So we return just.
- return *this;
-}
-
-
-
-vpColVector
-vpColVector::invSort(const vpColVector &v)
-{
- if (v.data==NULL)
- {
- vpERROR_TRACE("vpColVector v non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
- vpColVector tab ;
- tab = v ;
- unsigned int nb_permutation = 1 ;
- unsigned int i = 0 ;
- while (nb_permutation !=0 )
- {
- nb_permutation = 0 ;
- for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
- {
- if ((tab[j]>tab[j-1]))
- {
- double tmp = tab[j] ;
- tab[j] = tab[j-1] ;
- tab[j-1] = tmp ;
- nb_permutation++ ;
- }
- }
- i++ ;
- }
-
- return tab ;
-}
-
-vpColVector
-vpColVector::sort(const vpColVector &v)
-{
- if (v.data==NULL) {
- vpERROR_TRACE("vpColVector a non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
- vpColVector tab ;
- tab = v ;
- unsigned int nb_permutation = 1 ;
- unsigned int i = 0 ;
- while (nb_permutation !=0 )
- {
- nb_permutation = 0 ;
- for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
- {
- if ((tab[j]<tab[j-1]))
- {
- double tmp = tab[j] ;
- tab[j] = tab[j-1] ;
- tab[j-1] = tmp ;
- nb_permutation++ ;
- }
- }
- i++ ;
- }
-
- return tab ;
-}
-
-/*!
- Stack column vector with new element.
-
- \param b : Element to stack to the existing one.
-
- \code
- vpColVector A(3);
- double b = 3;
- A.stack(b); // A = [A b]T
- // A is now an 4 dimension column vector
- \endcode
-
- \sa stack(const vpColVector &, const vpColVector &)
- \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
-
-*/
-void vpColVector::stack(const double &b)
-{
- this->resize(rowNum+1,false);
- (*this)[rowNum-1] = b;
-}
-
-/*!
- Stack column vectors.
-
- \param B : Vector to stack to the existing one.
-
- \code
- vpColVector A(3);
- vpColVector B(5);
- A.stack(B); // A = [A B]T
- // A is now an 8 dimension column vector
- \endcode
-
- \sa stack(const vpColVector &, const double &)
- \sa stack(const vpColVector &, const vpColVector &)
- \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
-
-*/
-void vpColVector::stack(const vpColVector &B)
-{
- *this = vpColVector::stack(*this, B);
-}
-
-/*!
- Stack column vectors.
-
- \param A : Initial vector.
- \param B : Vector to stack at the end of A.
- \return Stacked vector \f$[A B]^T\f$.
-
- \code
- vpColVector A(3);
- vpColVector B(5);
- vpColVector C;
- C = vpColVector::stack(A, B); // C = [A B]T
- // C is now an 8 dimension column vector
- \endcode
-
- \sa stack(const vpColVector &)
- \sa stack(const vpColVector &, const vpColVector &, vpColVector &)
-*/
-vpColVector vpColVector::stack(const vpColVector &A, const vpColVector &B)
-{
- vpColVector C;
- vpColVector::stack(A, B, C);
- return C;
-}
-
-/*!
- Stack column vectors.
-
- \param A : Initial vector.
- \param B : Vector to stack at the end of A.
- \param C : Resulting stacked vector \f$C = [A B]^T\f$.
-
- \code
- vpColVector A(3);
- vpColVector B(5);
- vpColVector C;
- vpColVector::stack(A, B, C); // C = [A B]T
- // C is now an 8 dimension column vector
- \endcode
-
- \sa stack(const vpColVector &)
- \sa stack(const vpColVector &, const vpColVector &)
-*/
-void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
-{
- unsigned int nrA = A.getRows();
- unsigned int nrB = B.getRows();
-
- if (nrA == 0 && nrB == 0) {
- C.resize(0);
- return;
- }
-
- if (nrB == 0) {
- C = A;
- return;
- }
-
- if (nrA == 0) {
- C = B;
- return;
- }
-
- // General case
- C.resize(nrA + nrB);
-
- for (unsigned int i=0; i<nrA; i++)
- C[i] = A[i];
-
- for (unsigned int i=0; i<nrB; i++)
- C[nrA+i] = B[i];
-}
-
-/*!
- \brief Compute the mean value of all the element of the vector
-*/
-double vpColVector::mean(const vpColVector &v)
-{
- if (v.data==NULL)
- {
- vpERROR_TRACE("vpColVector v non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
- double mean = 0 ;
- double *vd = v.data ;
- for (unsigned int i=0 ; i < v.getRows() ; i++)
- mean += *(vd++) ;
-
- /*
- for (int i=0 ; i < v.getRows() ; i++)
- {
- mean += v[i] ;
- }
- mean /= v.rowNum ;
- return mean;
- */
- return mean/v.getRows();
-}
-
-/*!
- Compute the median value of all the element of the vector
-*/
-double
-vpColVector::median(const vpColVector &v)
-{
- if (v.data==NULL)
- {
- vpERROR_TRACE("vpColVector v non initialized") ;
- throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
- }
-
- unsigned int i,j;
- unsigned int inf, sup;
- unsigned int n = v.getRows() ;
- vpColVector infsup(n) ;
-
- for (i=0;i<v.getRows();i++)
- {
- // We compute the number of element gretear (sup) than the current
- // value and the number of element smaller (inf) than the current
- // value
- inf = sup = 0;
- for (j=0;j<v.getRows();j++)
- {
- if (i != j)
- {
- if (v[i] <= v[j]) inf++;
- if (v[i] >= v[j]) sup++;
- }
- }
- // We compute then difference between inf and sup
- // the median should be for |inf-sup| = 0 (1 if an even number of element)
- // which means that there are the same number of element in the array
- // that are greater and smaller that this value.
- infsup[i] = fabs((double)(inf-sup)); //EM gcc 4.3
- }
-
- // seek for the smaller value of |inf-sup| (should be 0 or 1)
- unsigned int imin=0 ; // index of the median in the array
- // min cannot be greater than the number of element
- double min = v.getRows();
- for (i=0;i<v.getRows();i++)
- if (infsup[i] < min)
- {
- min = infsup[i];
- imin = i ;
- }
-
- // return the median
- return(v[imin]);
-
-}
-
-/*!
- Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v.
-
- \f[ \mbox{if} \quad {\bf V} = \left( \begin{array}{c} x \\ y \\ z
- \end{array}\right), \quad \mbox{then} \qquad
- [{\bf v}]_\times = \left( \begin{array}{ccc}
- 0 & -z & y \\
- z & 0 & -x \\
- -y & x & 0
- \end{array}\right)
- \f]
-
- \param v : Input vector used to compute the skew symmetric matrix.
-*/
-vpMatrix
-vpColVector::skew(const vpColVector &v)
-{
-
- vpMatrix M ;
- if (v.getRows() != 3)
- {
- vpERROR_TRACE("Cannot compute skew kinematic matrix,"
- "v has not 3 rows") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t Cannot compute skew kinematic matrix"
- "v has not 3 rows")) ;
- }
-
-
- M.resize(3,3) ;
- M[0][0] = 0 ; M[0][1] = -v[2] ; M[0][2] = v[1] ;
- M[1][0] = v[2] ; M[1][1] = 0 ; M[1][2] = -v[0] ;
- M[2][0] = -v[1] ; M[2][1] = v[0] ; M[2][2] = 0 ;
-
-
- return M ;
-}
-
-/*!
- \brief Compute the cross product of two vectors C = a x b
-
- \param a : vpColVector
- \param b : vpColVector
-*/
-vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
-{
-
- if (a.getRows() != 3)
- {
- vpERROR_TRACE("Cannot compute cross product,"
- "a has not 3 rows") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t Cannot compute cross product"
- "a has not 3 rows")) ;
- }
- if (b.getRows() != 3)
- {
-
- vpERROR_TRACE("Cannot compute cross product,"
- "b has not 3 rows") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t Cannot compute cross product"
- "b has not 3 rows")) ;;
- }
-
- return vpColVector::skew(a) * b;
-}
-
-
-/*!
- \brief reshape the colvector in a matrix
- \param nrows : number of rows of the matrix
- \param ncols : number of columns of the matrix
- \return a vpMatrix
-*/
-vpMatrix vpColVector::reshape(const unsigned int &nrows,const unsigned int &ncols){
- vpMatrix m(nrows,ncols);
- reshape(m,nrows,ncols);
- return m;
-}
-
-/*!
- \brief reshape the colvector in a matrix
- \param m : the reshaped Matrix
- \param nrows : number of rows of the matrix
- \param ncols : number of columns of the matrix
-*/
-void vpColVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){
- if(dsize!=nrows*ncols)
- {
- vpERROR_TRACE("\n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;
- }
- try
- {
- if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- for(unsigned int j =0; j< ncols; j++)
- for(unsigned int i =0; i< nrows; i++)
- m[i][j]=data[j*ncols+i];
-}
-
-/*!
- Insert a column vector.
- \param i : Index of the first element to introduce. This index starts from 0.
- \param v : Column vector to insert.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector v(4);
- for (unsigned int i=0; i < v.size(); i++)
- v[i] = i;
- std::cout << "v:\n" << v << std::endl;
-
- vpColVector w(2);
- for (unsigned int i=0; i < w.size(); i++)
- w[i] = i+10;
- std::cout << "w:\n" << w << std::endl;
-
- v.insert(1, w);
- std::cout << "v:\n" << v << std::endl;
-}
- \endcode
- It produces the following output:
- \code
-v:
-0
-1
-2
-3
-
-w:
-10
-11
-
-v:
-0
-10
-11
-3
- \endcode
- */
-void vpColVector::insert(unsigned int i, const vpColVector &v)
-{
- if (i+v.size() > this->size())
- throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
- for (unsigned int j=0; j < v.size(); j++)
- (*this)[i+j] = v[j];
-}
diff --git a/src/math/matrix/vpColVector.h b/src/math/matrix/vpColVector.h
deleted file mode 100644
index b732f11..0000000
--- a/src/math/matrix/vpColVector.h
+++ /dev/null
@@ -1,219 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpColVector.h 5185 2015-01-21 14:36:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Provide some simple operation on column vectors.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpColVector_H
-#define vpColVector_H
-
-#include <visp/vpMatrix.h>
-#include <visp/vpRowVector.h>
-#include <visp/vpMath.h>
-
-class vpMatrix;
-class vpRotationVector;
-
-/*!
- \file vpColVector.h
- \brief definition of column vector class as well
- as a set of operations on these vector
-*/
-
-/*!
- \class vpColVector
- \ingroup Matrix
- \brief Class that provides a data structure for the column vectors
- as well as a set of operations on these vectors.
-
- the vpColVector is derived from vpMatrix.
-
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-
- \warning Note the vector in the class (*this) will be noted A in the comment
-*/
-class VISP_EXPORT vpColVector : public vpMatrix
-{
- friend class vpMatrix;
-protected:
- vpColVector (vpMatrix &m, unsigned int j);
- vpColVector (vpColVector &m, unsigned int r, unsigned int nrows) ;
-
-public:
-
- //! Basic constructor.
- vpColVector() : vpMatrix() {};
- //! Constructor of vector of size n. Each element is set to 0.
- vpColVector(unsigned int n) : vpMatrix(n,1){};
- //! Constructor of vector of size n. Each element is set to \e val.
- vpColVector(unsigned int n, double val) : vpMatrix(n, 1, val){};
- //! Copy constructor.
- vpColVector (const vpColVector &v);
- //! Constructor that initialize a vpColVector from a vpRotationVector.
- vpColVector (const vpRotationVector &v);
-
- void insert(unsigned int i, const vpColVector &v);
-
- /*! Set the size of the column vector.
- \param i : Column vector size.
- \param flagNullify : If true, set the data to zero.
- */
- void resize(const unsigned int i, const bool flagNullify = true)
- {
- vpMatrix::resize(i, 1, flagNullify);
- }
-
- //! Access V[i] = x
- inline double &operator [](unsigned int n) { return *(data + n); }
- //! Access x = V[i]
- inline const double &operator [](unsigned int n) const { return *(data+n); }
- //! Copy operator. Allow operation such as A = v
- vpColVector &operator=(const vpColVector &v);
- // Copy from a matrix.
- vpColVector &operator=(const vpMatrix &m);
- //! Initialize each element of the vector to x
- vpColVector &operator=(double x);
- // Copy operator. Allow operation such as A << v
- vpColVector &operator<<(const vpColVector &v);
- // Assigment operator. Allow operation such as A = *v
- vpColVector &operator<<(double *);
-
- //! Addition of two vectors V = A+v
- vpColVector operator+(const vpColVector &v) const;
- //! Substraction of two vectors V = A-v
- vpColVector operator-(const vpColVector &v) const;
- //! Operator A = -A
- vpColVector operator-() const;
- //! Dot product
- double operator*(const vpColVector &x) const;
- //! Dot product
- vpMatrix operator*(const vpRowVector &x) const;
- //! Multiplication by a scalar V = A * x
- vpColVector operator*(const double x) const;
-
- vpColVector rows(unsigned int first_row, unsigned int last_row)
- {
- return vpColVector(*this, first_row-1, last_row-first_row+1);
- }
-
- //! Reshape methods
- void reshape(vpMatrix & m,
- const unsigned int &nrows, const unsigned int &ncols);
- vpMatrix reshape(const unsigned int &nrows, const unsigned int &ncols);
-
- void stack(const double &b);
- void stack(const vpColVector &B);
- static vpColVector stack(const vpColVector &A, const vpColVector &B);
- static void stack(const vpColVector &A, const vpColVector &B, vpColVector &C);
-
- //! Transpose of a vector
- vpRowVector t() const;
-
- //! Normalise the vector
- vpColVector &normalize() ;
- // normalise the vector
- // vpColVector &normalize(vpColVector &x) const ;
-
- //! Compute the cross product of two vectors C = a x b
- static vpColVector crossProd(const vpColVector &a, const vpColVector &b) ;
-
- // Compute the cross product of two vectors C = a x b
- inline static vpColVector cross(const vpColVector &a, const vpColVector &b){
- return crossProd(a,b);}
-
- // Compute the skew matrix [v]x
- static vpMatrix skew(const vpColVector &v);
- //! Dot Product
-
- static double dotProd(const vpColVector &a, const vpColVector &b) ;
-
- //! sort the elements of vector v
- static vpColVector sort(const vpColVector &v) ;
- //! reverse sorting of the elements of vector v
- static vpColVector invSort(const vpColVector &v) ;
- //! compute the median
- static double median(const vpColVector &v) ;
- //! compute the mean
- static double mean(const vpColVector &v) ;
-
- /*!
-
- Convert a column vector containing angles in radians into
- degrees.
-
- */
- inline void rad2deg() {
- double r2d = 180.0/M_PI;
-
- for (unsigned int i=0; i < rowNum; i++)
- (*this)[i] *= r2d;
- }
- /*!
- Convert a column vector containing angles in degrees into radians.
-
- */
- inline void deg2rad() {
- double d2r = M_PI/180.0;
-
- for (unsigned int i=0; i < rowNum; i++)
- (*this)[i] *= d2r;
- }
-
- /*!
-
- Return the size of the vector in term of number of rows.
-
- */
- inline unsigned int size() const
- {
- return getRows();
- }
-
-};
-
-
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/matrix/vpGEMM.h b/src/math/matrix/vpGEMM.h
deleted file mode 100644
index 0360d0e..0000000
--- a/src/math/matrix/vpGEMM.h
+++ /dev/null
@@ -1,364 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpGEMM.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix generalized multiplication.
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-
-#ifndef __VP_GEMM__
-#define __VP_GEMM__
-
-#include <visp/vpMatrix.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-
-const vpMatrix null(0,0);
-
-/*!
- \brief Enumeration of the operations applied on matrices in vpGEMM function.
-
- Operations are :
- - VP_GEMM_A_T to use the transpose matrix of A instead of the matrix A
- - VP_GEMM_B_T to use the transpose matrix of B instead of the matrix B
- - VP_GEMM_C_T to use the transpose matrix of C instead of the matrix C
-
- \relates vpMatrix
- */
-typedef enum {
- VP_GEMM_A_T=1, //! Use A^T instead of A
- VP_GEMM_B_T=2, //! Use B^T instead of B
- VP_GEMM_C_T=4, //! Use C^T instead of C
-} vpGEMMmethod;
-
-
-
-
-template<unsigned int> inline void GEMMsize(const vpMatrix & /*A*/,const vpMatrix & /*B*/, unsigned int &/*Arows*/, unsigned int &/*Acols*/, unsigned int &/*Brows*/, unsigned int &/*Bcols*/){}
-
- template<> void inline GEMMsize<0>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getRows();
- Bcols= B.getCols();
-}
-
- template<> inline void GEMMsize<1>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getRows();
- Bcols= B.getCols();
-}
- template<> inline void GEMMsize<2>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getCols();
- Bcols= B.getRows();
-}
- template<> inline void GEMMsize<3>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getCols();
- Bcols= B.getRows();
-}
-
- template<> inline void GEMMsize<4>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getRows();
- Bcols= B.getCols();
-}
-
- template<> inline void GEMMsize<5>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getRows();
- Bcols= B.getCols();
-}
-
- template<> inline void GEMMsize<6>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getCols();
- Bcols= B.getRows();
-}
-
- template<> inline void GEMMsize<7>(const vpMatrix & A,const vpMatrix & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols){
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getCols();
- Bcols= B.getRows();
-}
-
-
-
- template<unsigned int> inline void GEMM1(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpMatrix & /*A*/, const vpMatrix & /*B*/, const double & /*alpha*/,vpMatrix &/*D*/){}
-
- template<> inline void GEMM1<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A, const vpMatrix & B, const double & alpha,vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum;
- }
-}
-
- template<> inline void GEMM1<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A, const vpMatrix & B, const double & alpha,vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum;
- }
-}
-
- template<> inline void GEMM1<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha,vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum;
- }
-}
-
- template<> inline void GEMM1<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha,vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum;
- }
-}
-
- template<unsigned int> inline void GEMM2(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpMatrix & /*A*/,const vpMatrix & /*B*/, const double & /*alpha*/, const vpMatrix & /*C*/ , const double &/*beta*/, vpMatrix &/*D*/){}
-
- template<> inline void GEMM2<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
-
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum+C[r][c]*beta;
- }
-}
-
- template<> inline void GEMM2<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum+C[r][c]*beta;
- }
-}
-
- template<> inline void GEMM2<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum+C[r][c]*beta;
- }
-}
-
- template<> inline void GEMM2<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum+C[r][c]*beta;
- }
-}
-
-
- template<> inline void GEMM2<4>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum+C[c][r]*beta;
- }
-}
-
- template<> inline void GEMM2<5>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum+C[c][r]*beta;
- }
-
-}
-
- template<> inline void GEMM2<6>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum+C[c][r]*beta;
- }
-}
-
- template<> inline void GEMM2<7>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpMatrix & A,const vpMatrix & B, const double & alpha, const vpMatrix & C , const double &beta, vpMatrix &D){
- //vpMatrix &D = *dynamic_cast<double***>(Dptr);
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum+C[c][r]*beta;
- }
-}
-
- template<unsigned int T> inline void vpTGEMM(const vpMatrix & A,const vpMatrix & B, const double & alpha ,const vpMatrix & C, const double & beta, vpMatrix & D){
-
- unsigned int Arows;
- unsigned int Acols;
- unsigned int Brows;
- unsigned int Bcols;
-
-// std::cout << T << std::endl;
- GEMMsize<T>(A,B,Arows,Acols,Brows,Bcols);
-// std::cout << Arows<<" " <<Acols << " "<< Brows << " "<< Bcols<<std::endl;
-
- try
- {
- if ((Arows != D.getRows()) || (Bcols != D.getCols())) D.resize(Arows,Bcols);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if (Acols != Brows)
- {
- vpERROR_TRACE("\n\t\tvpMatrix mismatch size in vpGEMM") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"\n\t\tvpMatrix mismatch size in vpGEMM")) ;
- }
-
- if(C.getRows()!=0 && C.getCols()!=0){
-
- if ((Arows != C.getRows()) || (Bcols != C.getCols()))
- {
- vpERROR_TRACE("\n\t\tvpMatrix mismatch size in vpGEMM") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"\n\t\tvpMatrix mismatch size in vpGEMM")) ;
- }
-
-
- GEMM2<T>(Arows,Brows,Bcols,A,B,alpha,C,beta,D);
- }else{
- GEMM1<T>(Arows,Brows,Bcols,A,B,alpha,D);
- }
-
-}
-
-/*!
- \brief This function performs generalized matrix multiplication:
- D = alpha*op(A)*op(B) + beta*op(C), where op(X) is X or X^T.
- Operation on A, B and C matrices is described by enumeration vpGEMMmethod.
-
- For example, to compute alpha*A^T*B^T+beta*C we need to call :
- \code
- vpGEMM(A,B,alpha,C,beta, VP_GEMM_A_T + VP_GEMM_B_T);
- \endcode
-
- If C is not used, vpGEMM must be called using an empty matrix \e null :
- \code
- vpGEMM(A,B,alpha,C, null,0);
- \endcode
-
- \throw vpMatrixException::incorrectMatrixSizeError if the sizes of the matrices
- do not allow the operations.
-
- \param A : a Matrix
- \param B : a Matrix
- \param alpha : a scalar
- \param C : a Matrix
- \param beta : a scalar
- \param D : a Matrix
- \param ops : a scalar describing operation applied on the matrices
-
- \relates vpMatrix
-
-*/
-inline void vpGEMM(const vpMatrix & A,const vpMatrix & B, const double & alpha ,const vpMatrix & C, const double & beta, vpMatrix & D, const unsigned int &ops=0){
- switch(ops){
- case 0 :
- vpTGEMM<0>( A, B, alpha , C, beta, D);
- break;
- case 1 :
- vpTGEMM<1>( A, B, alpha , C, beta, D);
- break;
- case 2 :
- vpTGEMM<2>( A, B, alpha , C, beta, D);
- break;
- case 3 :
- vpTGEMM<3>( A, B, alpha , C, beta, D);
- break;
- case 4 :
- vpTGEMM<4>( A, B, alpha , C, beta, D);
- break;
- case 5 :
- vpTGEMM<5>( A, B, alpha , C, beta, D);
- break;
- case 6 :
- vpTGEMM<6>( A, B, alpha , C, beta, D);
- break;
- case 7 :
- vpTGEMM<7>( A, B, alpha , C, beta, D);
- break;
- default:
- vpERROR_TRACE("\n\t\tvpMatrix mismatch operation in vpGEMM") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"\n\t\tvpMatrix mismatch operation in vpGEMM")) ;
- break;
- }
-}
-
-#endif
diff --git a/src/math/matrix/vpMatrix.cpp b/src/math/matrix/vpMatrix.cpp
deleted file mode 100644
index 99acb53..0000000
--- a/src/math/matrix/vpMatrix.cpp
+++ /dev/null
@@ -1,4349 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpMatrix.cpp 5238 2015-01-30 13:52:25Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Matrix manipulation.
-*
-* Authors:
-* Eric Marchand
-*
-*****************************************************************************/
-
-
-
-/*!
-\file vpMatrix.cpp
-\brief Definition of the vpMatrix class
-*/
-
-/*
-\class vpMatrix
-
-\brief Provide simple Matrices computation
-
-\author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomography.h>
-#include <visp/vpTranslationVector.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <vector>
-#include <sstream>
-#include <algorithm>
-#include <assert.h>
-#include <fstream>
-#include <string>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#ifdef VISP_HAVE_GSL
-#include <gsl/gsl_linalg.h>
-#endif
-
-#define DEBUG_LEVEL1 0
-#define DEBUG_LEVEL2 0
-
-
-//Prototypes of specific functions
-vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
-
-/*!
-\brief initialization of the object matrix.
-Number of columns and rows are zero.
-*/
-
-void
-vpMatrix::init()
-{
-
- rowNum = 0 ;
- colNum = 0 ;
-
- data = NULL ;
- rowPtrs = NULL ;
-
- dsize = 0 ;
- trsize =0 ;
-}
-
-/*!
-\brief basic constructor of the matrix class
-Construction of the object matrix.
-Number of columns and rows are zero.
-*/
-vpMatrix::vpMatrix()
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
-}
-
-
-/*!
-Constructor.
-
-Initialize a matrix with 0.
-
-\param r : Matrix number of rows.
-\param c : Matrix number of columns.
-*/
-vpMatrix::vpMatrix(unsigned int r, unsigned int c)
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
- resize(r, c);
-}
-
-/*!
-Constructor.
-
-Initialize a matrix with \e val.
-
-\param r : Matrix number of rows.
-\param c : Matrix number of columns.
-\param val : Each element of the matrix is set to \e val.
-*/
-vpMatrix::vpMatrix(unsigned int r, unsigned int c, double val)
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
- resize(r, c);
- *this = val;
-}
-
-vpMatrix::vpMatrix(const vpHomography& H)
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
- (*this) = H;
-}
-
-/*!
-\brief submatrix constructor
-*/
-vpMatrix::vpMatrix(const vpMatrix &m,
- unsigned int r, unsigned int c,
- unsigned int nrows, unsigned int ncols)
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
- if (((r + nrows) > m.rowNum) || ((c + ncols) > m.colNum))
- {
- vpERROR_TRACE("\n\t\t SubvpMatrix larger than vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::subMatrixError,
- "\n\t\t SubvpMatrix larger than vpMatrix")) ;
- }
-
- init(m,r,c,nrows,ncols);
-}
-
-//! copie constructor
-vpMatrix::vpMatrix(const vpMatrix& m)
- : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
-{
- resize(m.rowNum,m.colNum);
-
- memcpy(data,m.data,rowNum*colNum*sizeof(double)) ;
-}
-
-
-/*!
-Set the size of the matrix and initialize all the values to zero.
-
-\param nrows : number of rows
-\param ncols : number of column
-\param flagNullify : if true, then the matrix is re-initialized to 0
-after resize. If false, the initial values from the common part of the
-matrix (common part between old and new version of the matrix) are kept.
-Default value is true.
-
-\return OK or MEMORY_FAULT if memory cannot be allocated
-*/
-
-void vpMatrix::resize(const unsigned int nrows, const unsigned int ncols,
- const bool flagNullify)
-{
-
- if ((nrows == rowNum) && (ncols == colNum))
- {
- if (flagNullify && this->data != NULL)
- { memset(this->data,0,this->dsize*sizeof(double)) ;}
- }
- else
- {
- const bool recopyNeeded = (ncols != this ->colNum);
- double * copyTmp = NULL;
- unsigned int rowTmp = 0, colTmp=0;
-
- vpDEBUG_TRACE (25, "Recopy case per case is required iff number of "
- "cols has changed (structure of double array is not "
- "the same in this case.");
- if (recopyNeeded && this->data != NULL)
- {
- copyTmp = new double[this->dsize];
- memcpy (copyTmp, this ->data, sizeof(double)*this->dsize);
- rowTmp=this->rowNum; colTmp=this->colNum;
- }
-
- vpDEBUG_TRACE (25, "Reallocation of this->data array.");
- this->dsize = nrows*ncols;
- this->data = (double*)realloc(this->data, this->dsize*sizeof(double));
- if ((NULL == this->data) && (0 != this->dsize))
- {
- if (copyTmp != NULL) delete [] copyTmp;
- vpERROR_TRACE("\n\t\tMemory allocation error when allocating data") ;
- throw(vpException(vpException::memoryAllocationError,
- "\n\t\t Memory allocation error when "
- "allocating data")) ;
- }
-
- vpDEBUG_TRACE (25, "Reallocation of this->trsize array.");
- this->trsize = nrows;
- this->rowPtrs = (double**)realloc (this->rowPtrs, this->trsize*sizeof(double*));
- if ((NULL == this->rowPtrs) && (0 != this->dsize))
- {
- if (copyTmp != NULL) delete [] copyTmp;
- vpERROR_TRACE("\n\t\tMemory allocation error when allocating rowPtrs") ;
- throw(vpException(vpException::memoryAllocationError,
- "\n\t\t Memory allocation error when "
- "allocating rowPtrs")) ;
- }
-
- vpDEBUG_TRACE (25, "Recomputation this->trsize array values.");
- {
- double **t_= rowPtrs;
- for (unsigned int i=0; i<dsize; i+=ncols) { *t_++ = this->data + i; }
- }
-
- this->rowNum = nrows; this->colNum = ncols;
-
- vpDEBUG_TRACE (25, "Recopy of this->data array values or nullify.");
- if (flagNullify)
- { memset(this->data,0,this->dsize*sizeof(double)) ;}
- else
- {
- if (recopyNeeded && this->rowPtrs != NULL)
- {
- vpDEBUG_TRACE (25, "Recopy...");
- const unsigned int minRow = (this->rowNum<rowTmp)?this->rowNum:rowTmp;
- const unsigned int minCol = (this->colNum<colTmp)?this->colNum:colTmp;
- for (unsigned int i=0; i<this->rowNum; ++i)
- for (unsigned int j=0; j<this->colNum; ++j)
- {
- if ((minRow > i) && (minCol > j))
- {
- (*this)[i][j] = copyTmp [i*colTmp+j];
- vpCDEBUG (25) << i << "x" << j << "<- " << i*colTmp+j
- << "=" << copyTmp [i*colTmp+j] << std::endl;
- }
- else {(*this)[i][j] = 0;}
- }
- }
- else { vpDEBUG_TRACE (25,"Nothing to do: already done by realloc.");}
- }
-
- if (copyTmp != NULL) delete [] copyTmp;
- }
-
-}
-
-/*!
- Initialize the matrix from a part of an input matrix M.
-
- \param M : Input matrix used for initialization.
- \param r : row index in matrix M.
- \param c : column index in matrix M.
- \param nrows : Number of rows of the matrix that should be initialized.
- \param ncols : Number of columns of the matrix that should be initialized.
-
- The sub-matrix starting from M[r][c] element and ending on M[r+nrows-1][c+ncols-1] element
- is used to initialize the matrix.
-
- The following code shows how to use this function:
-\code
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix M(4,5);
- int val = 0;
- for(size_t i=0; i<M.getRows(); i++) {
- for(size_t j=0; j<M.getCols(); j++) {
- M[i][j] = val++;
- }
- }
- M.print (std::cout, 4, "M ");
-
- vpMatrix N;
- N.init(M, 0, 1, 2, 3);
- N.print (std::cout, 4, "N ");
-}
-\endcode
- It produces the following output:
- \code
-M [4,5]=
- 0 1 2 3 4
- 5 6 7 8 9
- 10 11 12 13 14
- 15 16 17 18 19
-N [2,3]=
- 1 2 3
- 6 7 8
- \endcode
- */
-void
-vpMatrix::init(const vpMatrix &M,unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
-{
- unsigned int rnrows = r+nrows ;
- unsigned int cncols = c+ncols ;
-
- if (rnrows > M.getRows())
- throw(vpException(vpException::dimensionError,
- "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, M.getRows()));
- if (cncols > M.getCols())
- throw(vpException(vpException::dimensionError,
- "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, M.getCols()));
- resize(nrows, ncols);
-
- if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
- return; // Noting to do
- for (unsigned int i=r ; i < rnrows; i++)
- for (unsigned int j=c ; j < cncols; j++)
- (*this)[i-r][j-c] = M[i][j] ;
-}
-
-/*!
-\brief Destruction of the matrix (Memory de-allocation)
-*/
-void
-vpMatrix::kill()
-{
- if (data != NULL )
- {
- free(data);
- data=NULL;
- }
-
- if (rowPtrs!=NULL)
- {
- free(rowPtrs);
- rowPtrs=NULL ;
- }
-}
-/*!
-\brief Destructor (Memory de-allocation)
-*/
-vpMatrix::~vpMatrix()
-{
- kill() ;
-}
-
-
-/*!
-\brief Copy operator.
-Allow operation such as A = B
-
-\param B : matrix to be copied.
-*/
-vpMatrix &
-vpMatrix::operator=(const vpMatrix &B)
-{
- try {
- resize(B.rowNum, B.colNum) ;
- // suppress by em 5/12/06
- // *this = 0;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- memcpy(data,B.data,dsize*sizeof(double)) ;
-
- return *this;
-}
-
-/*!
-\brief Copy operator.
-Allow operation such as A = H
-
-\param H : homography matrix to be copied.
-*/
-vpMatrix &
-vpMatrix::operator=(const vpHomography& H)
-{
- init() ;
- resize(3,3);
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
- (*this)[i][j] = H[i][j];
-
- return *this;
-}
-
-//! set all the element of the matrix A to x
-vpMatrix &
-vpMatrix::operator=(double x)
-{
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // {
- // *(data+i) = x;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< std::endl;
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] = x;
-
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
-
-
- return *this;
-}
-
-
-/*!
-\brief Assigment from an array of double
-*/
-vpMatrix &
-vpMatrix::operator<<( double *x )
-{
-
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
- rowPtrs[i][j] = *x++;
- }
- }
- return *this;
-}
-
-
-//---------------------------------
-// Matrix operations.
-//---------------------------------
-
-/*!
-Operation C = A * B.
-
-The result is placed in the third parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator*()
-*/
-void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- try
- {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if (A.colNum != B.rowNum)
- {
- vpERROR_TRACE("\n\t\tvpMatrix mismatch in vpMatrix/vpMatrix multiply") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\tvpMatrix mismatch in "
- "vpMatrix/vpMatrix multiply")) ;
- }
-
- // 5/12/06 some "very" simple optimization to avoid indexation
- unsigned int BcolNum = B.colNum;
- unsigned int BrowNum = B.rowNum;
- unsigned int i,j,k;
- double **BrowPtrs = B.rowPtrs;
- for (i=0;i<A.rowNum;i++)
- {
- double *rowptri = A.rowPtrs[i];
- double *ci = C[i];
- for (j=0;j<BcolNum;j++)
- {
- double s = 0;
- for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
- ci[j] = s;
- }
- }
-}
-
-/*!
-Operation C = A * B (A is unchanged).
-\sa mult2Matrices() to avoid matrix allocation for each use.
-*/
-vpMatrix vpMatrix::operator*(const vpMatrix &B) const
-{
- vpMatrix C;
-
- vpMatrix::mult2Matrices(*this,B,C);
-
- return C;
-}
-/*!
- Allows to multiply a matrix by an homography.
- Operation M = K * H (H is unchanged).
-
-*/
-vpMatrix vpMatrix::operator*(const vpHomography &H) const
-{
- if (colNum != 3)
- throw(vpException(vpMatrixException::dimensionError, "Cannot multiply the matrix by the homography; bad matrix size"));
- vpMatrix M(rowNum, 3);
-
- for (unsigned int i=0;i<rowNum;i++){
- for (unsigned int j=0;j<3;j++) {
- double s = 0;
- for (unsigned int k=0;k<3;k++) s += (*this)[i][k] * H[k][j];
- M[i][j] = s;
- }
- }
-
- return M;
-}
-
-/*!
-Operation C = A*wA + B*wB
-
-The result is placed in the third parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator+()
-*/
-
-void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C){
- try
- {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if ((A.colNum != B.getCols())||(A.rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t vpMatrix mismatch in "
- "vpMatrix/vpMatrix addition")) ;
- }
-
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
-
- for (unsigned int i=0;i<A.rowNum;i++)
- for(unsigned int j=0;j<A.colNum;j++)
- CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];
-
-}
-
-/*!
-Operation C = A + B.
-
-The result is placed in the third parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator+()
-*/
-void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- try
- {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if ((A.colNum != B.getCols())||(A.rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t vpMatrix mismatch in "
- "vpMatrix/vpMatrix addition")) ;
- }
-
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
-
- // double t0,t1;
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<A.dsize;i++)
- // {
- // *(C.data + i) = *(B.data + i) + *(A.data + i) ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< std::endl;
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<A.rowNum;i++)
- for(unsigned int j=0;j<A.colNum;j++)
- {
-
- CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
- }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
-
-}
-
-/*!
-Operation C = A + B (A is unchanged).
-\sa add2Matrices() to avoid matrix allocation for each use.
-*/
-vpMatrix vpMatrix::operator+(const vpMatrix &B) const
-{
-
- vpMatrix C;
- vpMatrix::add2Matrices(*this,B,C);
- return C;
-}
-
-
-/*!
-Operation C = A - B.
-
-The result is placed in the third parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator-()
-*/
-void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- try
- {
- if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix substraction") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t vpMatrix mismatch in "
- "vpMatrix/vpMatrix substraction")) ;
- }
-
-
-
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<A.dsize;i++)
- // {
- // *(C.data + i) = *(A.data + i) - *(B.data + i) ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< std::endl;
- //
- // 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]-BrowPtrs[i][j];
- }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
-
-}
-
-/*!
-Operation C = A - B (A is unchanged).
-\sa sub2Matrices() to avoid matrix allocation for each use.
-*/
-vpMatrix vpMatrix::operator-(const vpMatrix &B) const
-{
- vpMatrix C;
- vpMatrix::sub2Matrices(*this,B,C);
- return C;
-}
-
-//! Operation A = A + B
-
-vpMatrix &vpMatrix::operator+=(const vpMatrix &B)
-{
- if ( (colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix += addition") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t vpMatrix mismatch in "
- "vpMatrix += addition")) ;
-
- }
-
-
- double ** BrowPtrs=B.rowPtrs;
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // {
- // *(data + i) += *(B.data + i) ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< std::endl;
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] += BrowPtrs[i][j];
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
-
-
-
-
- return *this;
-}
-
-//! Operation A = A - B
-
-vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
-{
- if ( (colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix -= substraction") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t vpMatrix mismatch in "
- "vpMatrix -= substraction")) ;
-
- }
-
- double ** BrowPtrs=B.rowPtrs;
-
- // double t0,t1;
-
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // {
- // *(data + i) -= *(B.data + i) ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< " " ;
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] -= BrowPtrs[i][j];
-
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
-
-
- return *this;
-}
-
-/*!
-Operation C = -A.
-
-The result is placed in the second parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator-(void)
-*/
-void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C)
-{
- try
- {
- if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- double ** ArowPtrs=A.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
-
-
- // std::cout << "negate" << std::endl;
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<A.dsize;i++)
- // {
- // *(C.data + i) = -*(A.data + i) ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<< " ";
-
- // 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];
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
-
-}
-
-/*!
-Operation C = -A (A is unchanged).
-\sa negateMatrix() to avoid matrix allocation for each use.
-*/
-vpMatrix vpMatrix::operator-() const //negate
-{
- vpMatrix C;
- vpMatrix::negateMatrix(*this,C);
- return C;
-}
-
-double
-vpMatrix::sumSquare() const
-{
- double sum_square=0.0;
- double x ;
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // double *d = data ;
- // double *n = data+dsize ;
- // while (d < n )
- // {
- // x = *d++ ;
- // sum_square += x*x ;
- // }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" "<< sum << " ";
- //
-
-
- // sum_square= 0.0;
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- {
- x=rowPtrs[i][j];
- sum_square += x*x;
- }
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" "<< sum << std::endl;
-
- return sum_square;
-}
-
-double
-vpMatrix::sum() const
-{
- double s=0.0;
- for (unsigned int i=0;i<rowNum;i++)
- {
- for(unsigned int j=0;j<colNum;j++)
- {
- s += rowPtrs[i][j];
- }
- }
-
- return s;
-}
-
-
-//---------------------------------
-// Matrix/vector operations.
-//---------------------------------
-
-/*!
-Operation c = A * b (c and b are vectors).
-
-The result is placed in the second parameter C and not returned.
-A new matrix won't be allocated for every use of the function
-(Speed gain if used many times with the same result matrix size).
-
-\sa operator*(const vpColVector &b) const
-*/
-void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &b, vpColVector &c)
-{
- if (A.colNum != b.getRows())
- {
- vpERROR_TRACE("vpMatrix mismatch in vpMatrix/vector multiply") ;
- throw(vpMatrixException::incorrectMatrixSizeError) ;
- }
-
- try
- {
- if (A.rowNum != c.rowNum) c.resize(A.rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
-
- c = 0.0;
- for (unsigned int j=0;j<A.colNum;j++)
- {
- double bj = b[j] ; // optimization em 5/12/2006
- for (unsigned int i=0;i<A.rowNum;i++)
- {
- c[i]+=A.rowPtrs[i][j] * bj;
- }
- }
-}
-
-/*!
-Operation c = A * b (A is unchanged, c and b are vectors).
-\sa multMatrixVector() to avoid matrix allocation for each use.
-*/
-vpColVector
-vpMatrix::operator*(const vpColVector &b) const
-{
- vpColVector c;
- vpMatrix::multMatrixVector(*this,b,c);
- return c;
-}
-
-//! Operation c = A * b (A is unchanged, c and b are translation vectors).
-vpTranslationVector
-vpMatrix::operator*(const vpTranslationVector &b) const
-{
- vpTranslationVector c;
-
- if (rowNum != 3 || colNum != 3)
- {
- vpERROR_TRACE("vpMatrix mismatch in vpMatrix::operator*(const vpTranslationVector)") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "vpMatrix mismatch in vpMatrix::operator*()")) ;
- }
-
- for (unsigned int j=0;j<3;j++) c[j]=0 ;
-
- for (unsigned int j=0;j<3;j++) {
- {
- double bj = b[j] ; // optimization em 5/12/2006
- for (unsigned int i=0;i<3;i++) {
- c[i]+=rowPtrs[i][j] * bj;
- }
- }
- }
- return c ;
-}
-
-//---------------------------------
-// Matrix/real operations.
-//---------------------------------
-
-/*!
- \relates vpMatrix
- Multiplication by a scalar Cij = x*Bij.
-*/
-vpMatrix operator*(const double &x,const vpMatrix &B)
-{
- // Modif EM 13/6/03
- vpMatrix C ;
-
- try {
- C.resize(B.getRows(),B.getCols());
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
- // double t0,t1;
-
- unsigned int Brow = B.getRows() ;
- unsigned int Bcol = B.getCols() ;
- // t0=vpTime::measureTimeMicros();
- //
- // for (int i=0;i<Brow; i++)
- // {
- // double *ci = C[i] ;
- // double *Bi = B[i] ;
- // for (int j=0 ; j < Bcol;j++)
- // ci[j] = Bi[j]*x;
- // }
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<Brow;i++)
- for(unsigned int j=0;j<Bcol;j++)
- C[i][j]= B[i][j]*x;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
-
-
- return C ;
-}
-
-//! Cij = Aij * x (A is unchanged)
-vpMatrix vpMatrix::operator*(double x) const
-{
- vpMatrix C;
-
- try {
- C.resize(rowNum,colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
- // double t0,t1;
-
- double ** CrowPtrs=C.rowPtrs;
-
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // *(C.data+i) = *(data+i)*x;
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
- //
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- CrowPtrs[i][j]= rowPtrs[i][j]*x;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
- return C;
-}
-
-//! Cij = Aij / x (A is unchanged)
-vpMatrix vpMatrix::operator/(double x) const
-{
- vpMatrix C;
-
- try {
- C.resize(rowNum,colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- //if (x == 0) {
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
- //vpERROR_TRACE("Divide by zero in method /(double x)") ;
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /(double x)");
- }
-
- double xinv = 1/x ;
-
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // *(C.data+i) = *(data+i)*xinv ;
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
- //
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- C[i][j]=rowPtrs[i][j]*xinv;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
- return C;
-
-}
-
-
-//! Add x to all the element of the matrix : Aij = Aij + x
-vpMatrix & vpMatrix::operator+=(double x)
-{
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // *(data+i) += x;
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
- //
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]+=x;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
- return *this;
-}
-
-
-//! Substract x to all the element of the matrix : Aij = Aij - x
-vpMatrix & vpMatrix::operator-=(double x)
-{
-
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // *(data+i) -= x;
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
- //
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]-=x;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
- return *this;
-}
-
-//! Multiply all the element of the matrix by x : Aij = Aij * x
-vpMatrix & vpMatrix::operator*=(double x)
-{
-
-
- // for (int i=0;i<dsize;i++)
- // *(data+i) *= x;
-
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]*=x;
-
- return *this;
-}
-
-//! Divide all the element of the matrix by x : Aij = Aij / x
-vpMatrix & vpMatrix::operator/=(double x)
-{
- //if (x == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double x)");
-
- double xinv = 1/x ;
-
- // double t0,t1;
- //
- // t0=vpTime::measureTimeMicros();
- // for (int i=0;i<dsize;i++)
- // *(data+i) *= xinv;
- //
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<" ";
-
- // t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]*=xinv;
- // t1=vpTime::measureTimeMicros();
- // std::cout<< t1-t0<<std::endl;
-
- return *this;
-}
-
-//----------------------------------------------------------------
-// Matrix Operation
-//----------------------------------------------------------------
-
-
-/*!
- By default set the matrix to identity.
- More generally set M[i][i] = val.
-*/
-void
-vpMatrix::setIdentity(const double & val)
-{
- if (rowNum != colNum)
- {
- vpERROR_TRACE("non square matrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError)) ;
- }
-
- unsigned int i,j;
- for (i=0;i<rowNum;i++)
- for (j=0;j<colNum;j++)
- if (i==j) (*this)[i][j] = val ; else (*this)[i][j] = 0;
-}
-
-/*!
- Set an n-by-n matrix to identity.
-
- eye(n) is an n-by-n matrix with ones on the diagonal and zeros
- else where.
-
-*/
-void
-vpMatrix::eye(unsigned int n)
-{
- try {
- eye(n,n) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-}
-/*!
- Set an m-by-n matrix to identity.
-
- eye(m,n) is an m-by-n matrix with ones on the diagonal and zeros
- else where.
-
-*/
-void
-vpMatrix::eye(unsigned int m, unsigned int n)
-{
- try {
- resize(m,n) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
-
- for (unsigned int i=0; i<rowNum; i++)
- for (unsigned int j=0; j<colNum; j++)
- if (i == j) (*this)[i][j] = 1;
- else (*this)[i][j] = 0;
-
-}
-
-
-/*!
- Compute and return the transpose of the matrix.
-*/
-vpMatrix vpMatrix::t() const
-{
- vpMatrix At ;
-
- try {
- At.resize(colNum,rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- unsigned int i,j;
- for (i=0;i<rowNum;i++)
- {
- double *coli = (*this)[i] ;
- for (j=0;j<colNum;j++)
- At[j][i] = coli[j];
- }
- return At;
-}
-
-
-/*!
- Compute and return the transpose of the matrix.
-
- \sa t()
-*/
-vpMatrix vpMatrix::transpose()const
-{
- vpMatrix At ;
- transpose(At);
- return At;
-}
-
-/*!
- Compute \e At the transpose of the matrix.
- \param At : Resulting transpose matrix.
- \sa t()
-*/
-void vpMatrix::transpose(vpMatrix & At )const{
-
- try {
- At.resize(colNum,rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- size_t A_step = colNum;
- double ** AtRowPtrs = At.rowPtrs;
-
- for( unsigned int i = 0; i < colNum; i++ )
- {
- double * row_ = AtRowPtrs[i];
- double * col = rowPtrs[0]+i;
- for( unsigned int j = 0; j < rowNum; j++, col+=A_step )
- *(row_++)=*col;
- }
-}
-
-
-/*!
- Computes the \f$AA^T\f$ operation \f$B = A*A^T\f$
- \return \f$A*A^T\f$
- \sa AAt(vpMatrix &) const
-*/
-vpMatrix vpMatrix::AAt() const
-{
- vpMatrix B;
-
- AAt(B);
-
- return B;
-}
-
-/*!
- Compute the AAt operation such as \f$B = A*A^T\f$.
-
- The result is placed in the parameter \e B and not returned.
-
- A new matrix won't be allocated for every use of the function. This
- results in a speed gain if used many times with the same result
- matrix size.
-
- \sa AAt()
-*/
-void vpMatrix::AAt(vpMatrix &B)const {
-
- try {
- if ((B.rowNum != rowNum) || (B.colNum != rowNum)) B.resize(rowNum,rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- // compute A*A^T
- for(unsigned int i=0;i<rowNum;i++){
- for(unsigned int j=i;j<rowNum;j++){
- double *pi = rowPtrs[i];// row i
- double *pj = rowPtrs[j];// row j
-
- // sum (row i .* row j)
- double ssum=0;
- for(unsigned int k=0; k < colNum ;k++)
- ssum += *(pi++)* *(pj++);
-
- B[i][j]=ssum; //upper triangle
- if(i!=j)
- B[j][i]=ssum; //lower triangle
- }
- }
-}
-
-
-
-/*!
- Compute the AtA operation such as \f$B = A^T*A\f$.
-
- The result is placed in the parameter \e B and not returned.
-
- A new matrix won't be allocated for every use of the function. This
- results in a speed gain if used many times with the same result matrix
- size.
-
- \sa AtA()
-*/
-void vpMatrix::AtA(vpMatrix &B) const
-{
- try {
- if ((B.rowNum != colNum) || (B.colNum != colNum)) B.resize(colNum,colNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- unsigned int i,j,k;
- double s;
- double *ptr;
- double *Bi;
- for (i=0;i<colNum;i++)
- {
- Bi = B[i] ;
- for (j=0;j<i;j++)
- {
- ptr=data;
- s = 0 ;
- for (k=0;k<rowNum;k++)
- {
- s +=(*(ptr+i)) * (*(ptr+j));
- ptr+=colNum;
- }
- *Bi++ = s ;
- B[j][i] = s;
- }
- ptr=data;
- s = 0 ;
- for (k=0;k<rowNum;k++)
- {
- s +=(*(ptr+i)) * (*(ptr+i));
- ptr+=colNum;
- }
- *Bi = s;
- }
-}
-
-
-/*!
- Compute the AtA operation such as \f$B = A^T*A\f$
- \return \f$A^T*A\f$
- \sa AtA(vpMatrix &) const
-*/
-vpMatrix vpMatrix::AtA() const
-{
- vpMatrix B;
-
- AtA(B);
-
- return B;
-}
-
-
-/*!
- Stacks columns of a matrix in a vector.
- \param out : a vpColVector.
-*/
-void vpMatrix::stackColumns(vpColVector &out ){
-
- try {
- if ((out.rowNum != colNum*rowNum) || (out.colNum != 1)) out.resize(rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- double *optr=out.data;
- for(unsigned int j =0;j<colNum ; j++){
- for(unsigned int i =0;i<rowNum ; i++){
- *(optr++)=rowPtrs[i][j];
- }
- }
-}
-
-/*!
- Stacks columns of a matrix in a vector.
- \return a vpColVector.
-*/
-vpColVector vpMatrix::stackColumns(){
-
- vpColVector out(colNum*rowNum);
- stackColumns(out);
- return out;
-}
-
-/*!
- Stacks rows of a matrix in a vector
- \param out : a vpRowVector.
-*/
-void vpMatrix::stackRows(vpRowVector &out ){
-
- try {
- if ((out.rowNum != 1) || (out.colNum != colNum*rowNum)) out.resize(rowNum);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- double *mdata=data;
- double *optr=out.data;
- for(unsigned int i =0;i<dsize ; i++){
- *(optr++)=*(mdata++);
- }
-}
-/*!
- Stacks rows of a matrix in a vector.
- \return a vpRowVector.
-*/
-vpRowVector vpMatrix::stackRows(){
-
- vpRowVector out(colNum*rowNum);
- stackRows(out );
- return out;
-}
-
-/*!
- Compute Kronecker product matrix.
- \param m1 : vpMatrix;
- \param m2 : vpMatrix;
- \param out : The kronecker product : \f$ m1 \otimes m2 \f$
-*/
-void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out){
- unsigned int r1= m1.getRows();
- unsigned int c1= m1.getCols();
- unsigned int r2= m2.getRows();
- unsigned int c2= m2.getCols();
-
-
- if (r1*r2 !=out.rowNum || c1*c2!= out.colNum )
- {
- vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Kronecker prodect bad dimension of output vpMatrix"));
- }
-
- for(unsigned int r =0;r<r1 ; r++){
- for(unsigned int c =0;c<c1 ; c++){
- double alpha = m1[r][c];
- double *m2ptr = m2[0];
- unsigned int roffset= r*r2;
- unsigned int coffset= c*c2;
- for(unsigned int rr =0;rr<r2 ; rr++){
- for(unsigned int cc =0;cc<c2 ;cc++){
- out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
- }
- }
- }
- }
-
-}
-
-/*!
- Compute Kronecker product matrix.
- \param m : vpMatrix.
- \param out : If m1.kron(m2) out contains the kronecker product's result : \f$ m1 \otimes m2 \f$.
-*/
-void vpMatrix::kron(const vpMatrix &m , vpMatrix &out){
- kron(*this,m,out);
-}
-
-/*!
- Compute Kronecker product matrix.
- \param m1 : vpMatrix;
- \param m2 : vpMatrix;
- \return The kronecker product : \f$ m1 \otimes m2 \f$
-*/
-vpMatrix vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2 ){
-
- unsigned int r1= m1.getRows();
- unsigned int c1= m1.getCols();
- unsigned int r2= m2.getRows();
- unsigned int c2= m2.getCols();
-
- vpMatrix out(r1*r2,c1*c2);
-
- for(unsigned int r =0;r<r1 ; r++){
- for(unsigned int c =0;c<c1 ; c++){
- double alpha = m1[r][c];
- double *m2ptr = m2[0];
- unsigned int roffset= r*r2;
- unsigned int coffset= c*c2;
- for(unsigned int rr =0;rr<r2 ; rr++){
- for(unsigned int cc =0;cc<c2 ;cc++){
- out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
- }
- }
- }
- }
- return out;
-}
-
-
-/*!
-Compute Kronecker product matrix.
-\param m : vpMatrix;
-\return m1.kron(m2) The kronecker product : \f$ m1 \otimes m2 \f$
-*/
-vpMatrix vpMatrix::kron(const vpMatrix &m){
- return kron(*this,m);
-}
-
-/*!
-
-Solve a linear system \f$ A X = B \f$ using Singular Value
-Decomposition (SVD).
-
-Non destructive wrt. A and B.
-
-\param b : Vector\f$ B \f$.
-
-\param x : Vector \f$ X \f$.
-
-Here an example:
-\code
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(3,3);
-
-A[0][0] = 4.64;
-A[0][1] = 0.288;
-A[0][2] = -0.384;
-
-A[1][0] = 0.288;
-A[1][1] = 7.3296;
-A[1][2] = 2.2272;
-
-A[2][0] = -0.384;
-A[2][1] = 2.2272;
-A[2][2] = 6.0304;
-
-vpColVector X(3), B(3);
-B[0] = 1;
-B[1] = 2;
-B[2] = 3;
-
-A.solveBySVD(B, X);
-
-// Obtained values of X
-// X[0] = 0.2468;
-// X[1] = 0.120782;
-// X[2] = 0.468587;
-
-std::cout << "X:\n" << X << std::endl;
-}
-\endcode
-
-\sa solveBySVD(const vpColVector &)
-*/
-void
-vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const
-{
- x = pseudoInverse(1e-6)*b ;
-}
-
-
-/*!
-
-Solve a linear system \f$ A X = B \f$ using Singular Value
-Decomposition (SVD).
-
-Non destructive wrt. A and B.
-
-\param B : Vector\f$ B \f$.
-
-\return Vector \f$ X \f$.
-
-Here an example:
-\code
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(3,3);
-
-A[0][0] = 4.64;
-A[0][1] = 0.288;
-A[0][2] = -0.384;
-
-A[1][0] = 0.288;
-A[1][1] = 7.3296;
-A[1][2] = 2.2272;
-
-A[2][0] = -0.384;
-A[2][1] = 2.2272;
-A[2][2] = 6.0304;
-
-vpColVector X(3), B(3);
-B[0] = 1;
-B[1] = 2;
-B[2] = 3;
-
-X = A.solveBySVD(B);
-// Obtained values of X
-// X[0] = 0.2468;
-// X[1] = 0.120782;
-// X[2] = 0.468587;
-
-std::cout << "X:\n" << X << std::endl;
-}
-\endcode
-
-\sa solveBySVD(const vpColVector &, vpColVector &)
-*/
-vpColVector vpMatrix::solveBySVD(const vpColVector &B) const
-{
- vpColVector X(colNum);
-
- solveBySVD(B, X);
- return X;
-}
-
-
-/*!
-
-Singular value decomposition (SVD).
-
-\f[ M = U \Sigma V^{\top} \f]
-
-\warning Destructive method wrt. to the matrix \f$ M \f$ to
-decompose. You should make a COPY of that matrix if needed not to
-CHANGE.
-
-\param w : Vector of singular values. \f$ \Sigma = diag(w) \f$.
-
-\param v : Matrix \f$ V \f$.
-
-\return Matrix \f$ U \f$.
-
-\warning If the GNU Scientific Library (GSL) third party library is used to compute the SVD
-decomposition, the singular values \f$ \Sigma_{i,i} \f$ are ordered in decreasing
-fashion in \e w. This is not the case, if the GSL is not detected by ViSP.
-
-Here an example of SVD decomposition of a non square Matrix M.
-
-\code
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix M(3,2);
-M[0][0] = 1;
-M[1][0] = 2;
-M[2][0] = 0.5;
-
-M[0][1] = 6;
-M[1][1] = 8 ;
-M[2][1] = 9 ;
-
-vpMatrix v;
-vpColVector w;
-vpMatrix Mrec;
-vpMatrix Sigma;
-
-M.svd(w, v);
-// Here M is modified and is now equal to U
-
-// Construct the diagonal matrix from the singular values
-Sigma.diag(w);
-
-// Reconstruct the initial matrix M using the decomposition
-Mrec = M * Sigma * v.t();
-
-// Here, Mrec is obtained equal to the initial value of M
-// Mrec[0][0] = 1;
-// Mrec[1][0] = 2;
-// Mrec[2][0] = 0.5;
-// Mrec[0][1] = 6;
-// Mrec[1][1] = 8 ;
-// Mrec[2][1] = 9 ;
-
-std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
-}
-\endcode
-
-*/
-void
-vpMatrix::svd(vpColVector& w, vpMatrix& v)
-{
-#if (DEBUG_LEVEL1 == 0) /* no verification */
- {
- w.resize( this->getCols() );
- v.resize( this->getCols(), this->getCols() );
-
-#if defined (VISP_HAVE_LAPACK)
- svdLapack(w,v);
-#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
- svdOpenCV(w,v);
-#elif defined (VISP_HAVE_GSL) /* be careful of the copy below */
- svdGsl(w,v) ;
-#else
- svdNr(w,v) ;
-#endif
-
- //svdNr(w,v) ;
- }
-#else /* verification of the SVD */
- {
- int pb = 0;
- unsigned int i,j,k,nrows,ncols;
- vpMatrix A, Asvd;
-
- A = (*this); /* copy because svd is destructive */
-
- w.resize( this->getCols() );
- v.resize( this->getCols(), this->getCols() );
-#ifdef VISP_HAVE_GSL /* be careful of the copy above */
- svdGsl(w,v) ;
-#else
- svdNr(w,v) ;
-#endif
- //svdNr(w,v) ;
-
- nrows = A.getRows();
- ncols = A.getCols();
- Asvd.resize(nrows,ncols);
-
- for (i = 0 ; i < nrows ; i++)
- {
- for (j = 0 ; j < ncols ; j++)
- {
- Asvd[i][j] = 0.0;
- for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*this)[i][k]*w[k]*v[j][k];
- }
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in SVD\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Asvd : " << std::endl << Asvd << std::endl;
- }
- // else printf("SVD ok ;-)\n"); /* It's so good... */
- }
-#endif
-}
-/*!
-\brief Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
-\param Ap : The pseudo inverse \f$ A^+ \f$.
-\param svThreshold : Threshold used to test the singular values.
-\return Return the rank of the matrix A
-*/
-
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
-{
- vpColVector sv ;
- return pseudoInverse(Ap, sv, svThreshold) ;
-}
-
-/*!
-\brief Compute and return the pseudo inverse of a n-by-m matrix : \f$ A^+ \f$
-\param svThreshold : Threshold used to test the singular values.
-
-\return Pseudo inverse of the matrix.
-
-Here an example to compute the inverse of a n-by-n matrix. If the
-matrix is n-by-n it is also possible to use inverseByLU().
-
-\code
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(4,4);
-
-A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
-A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
-A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
-A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
-
-// Compute the inverse
-vpMatrix A_1; // A^-1
-A_1 = A.pseudoInverse();
-std::cout << "Inverse by pseudo inverse: \n" << A_1 << std::endl;
-
-std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
-}
-\endcode
-
-\sa inverseByLU()
-
-*/
-vpMatrix
-vpMatrix::pseudoInverse(double svThreshold) const
-{
- vpMatrix Ap ;
- vpColVector sv ;
- pseudoInverse(Ap, sv, svThreshold) ;
- return Ap ;
-}
-
-/*!
-\brief Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
-\param Ap : The pseudo inverse \f$ A^+ \f$.
-\param sv : Singular values.
-\param svThreshold : Threshold used to test the singular values.
-\return Return the rank of the matrix A
-*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
-{
- vpMatrix imA, imAt ;
- return pseudoInverse(Ap, sv, svThreshold, imA, imAt) ;
-}
-
-/*!
-\brief Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
-
-Pseudo inverse, kernel and image are computed using the SVD decomposition.
-
-A is an m x n matrix,
-if m >=n the svd works on A other wise it works on \f$A^T\f$.
-
-Therefore if m>=n we have
-
-\f[
-{\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
-\f]
-\f[
-{\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
-\mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
-\left[
-\begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
-\f]
-where
-Im(A) is an m x r matrix (r is the rank of A) and
-Im(A^T) is an r x n matrix
-
-
-\param Ap : The pseudo inverse \f$ A^+ \f$.
-\param sv : Singular values.
-\param svThreshold : Threshold used to test the singular values.
-\param imAt : Image A^T
-\param imA: Image A
-\return Return the rank of the matrix A
-
-*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &imA,
- vpMatrix &imAt) const
-{
-
- unsigned int i, j, k ;
-
- unsigned int nrows, ncols;
- unsigned int nrows_orig = getRows() ;
- unsigned int ncols_orig = getCols() ;
- Ap.resize(ncols_orig,nrows_orig) ;
-
- if (nrows_orig >= ncols_orig)
- {
- nrows = nrows_orig;
- ncols = ncols_orig;
- }
- else
- {
- nrows = ncols_orig;
- ncols = nrows_orig;
- }
-
- vpMatrix a(nrows,ncols) ;
- vpMatrix a1(ncols,nrows);
- vpMatrix v(ncols,ncols) ;
- sv.resize(ncols) ;
-
- if (nrows_orig >= ncols_orig) a = *this;
- else a = (*this).t();
-
- a.svd(sv,v);
-
- // compute the highest singular value and the rank of h
- double maxsv = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
-
- unsigned int rank = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
-
-
-
- /*------------------------------------------------------- */
- for (i = 0 ; i < ncols ; i++)
- {
- for (j = 0 ; j < nrows ; j++)
- {
- a1[i][j] = 0.0;
-
- for (k=0 ; k < ncols ; k++)
- if (fabs(sv[k]) > maxsv*svThreshold)
- {
- a1[i][j] += v[i][k]*a[j][k]/sv[k];
- }
- }
- }
- if (nrows_orig >= ncols_orig) Ap = a1;
- else Ap = a1.t();
-
- if (nrows_orig >= ncols_orig)
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = v[i][j] ;
-
- // compute dim A
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = a[i][j] ;
- }
- else
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = a[i][j] ;
-
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = v[i][j] ;
-
- }
-
-#if (DEBUG_LEVEL1)
- {
- int pb = 0;
- vpMatrix A, ApA, AAp, AApA, ApAAp ;
-
- nrows = nrows_orig;
- ncols = ncols_orig;
-
- A.resize(nrows,ncols) ;
- A = *this ;
-
- ApA = Ap * A;
- AApA = A * ApA;
- ApAAp = ApA * Ap;
- AAp = A * Ap;
-
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in pseudo inverse\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Ap : " << std::endl << Ap << std::endl;
- std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
- std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
- std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
- std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
- }
- // else printf("Ap OK ;-) \n");
-
- }
-#endif
-
- // std::cout << v << std::endl ;
- return rank ;
-}
-
-
-
-/*!
-\brief Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
-
-Pseudo inverse, kernel and image are computed using the SVD decomposition.
-
-A is an m x n matrix,
-if m >=n the svd works on A other wise it works on \f$A^T\f$.
-
-Therefore if m>=n we have
-
-\f[
-{\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
-\f]
-\f[
-{\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
-\mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
-\left[
-\begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
-\f]
-where
-Im(A) is an m x r matrix (r is the rank of A) and
-Im(A^T) is an r x n matrix
-
-
-\param Ap : The pseudo inverse \f$ A^+ \f$.
-\param sv : Singular values.
-\param svThreshold : Threshold used to test the singular values.
-\param imA: Image A
-\param imAt : Image A^T
-\param kerA : null space of A
-\return Return the rank of the matrix A
-
-*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &imA,
- vpMatrix &imAt,
- vpMatrix &kerA) const
-{
-
- unsigned int i, j, k ;
-
- unsigned int nrows, ncols;
- unsigned int nrows_orig = getRows() ;
- unsigned int ncols_orig = getCols() ;
- Ap.resize(ncols_orig,nrows_orig) ;
-
- if (nrows_orig >= ncols_orig)
- {
- nrows = nrows_orig;
- ncols = ncols_orig;
- }
- else
- {
- nrows = ncols_orig;
- ncols = nrows_orig;
- }
-
- vpMatrix a(nrows,ncols) ;
- vpMatrix a1(ncols,nrows);
- vpMatrix v(ncols,ncols) ;
- sv.resize(ncols) ;
-
- if (nrows_orig >= ncols_orig) a = *this;
- else a = (*this).t();
-
- a.svd(sv,v);
-
- // compute the highest singular value and the rank of h
- double maxsv = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
-
- unsigned int rank = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
-
-
-
- /*------------------------------------------------------- */
- for (i = 0 ; i < ncols ; i++)
- {
- for (j = 0 ; j < nrows ; j++)
- {
- a1[i][j] = 0.0;
-
- for (k=0 ; k < ncols ; k++)
- if (fabs(sv[k]) > maxsv*svThreshold)
- {
- a1[i][j] += v[i][k]*a[j][k]/sv[k];
- }
- }
- }
- if (nrows_orig >= ncols_orig) Ap = a1;
- else Ap = a1.t();
-
- if (nrows_orig >= ncols_orig)
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = v[i][j] ;
-
- // compute dim A
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = a[i][j] ;
- }
- else
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = a[i][j] ;
-
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = v[i][j] ;
-
- }
-
- vpMatrix cons(ncols_orig, ncols_orig);
- cons = 0;
-
- for (j = 0; j < ncols_orig; j++)
- {
- for (i = 0; i < ncols_orig; i++)
- {
- if (fabs(sv[i]) <= maxsv*svThreshold)
- {
- cons[i][j] = v[j][i];
- }
- }
- }
-
- vpMatrix Ker (ncols_orig-rank, ncols_orig);
- k = 0;
- for (j = 0; j < ncols_orig ; j++)
- {
- //if ( cons.row(j+1).sumSquare() != 0)
- if ( std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
- {
- for (i = 0; i < cons.getCols(); i++)
- Ker[k][i] = cons[j][i];
-
- k++;
- }
- }
- kerA = Ker;
-
-#if (DEBUG_LEVEL1)
- {
- int pb = 0;
- vpMatrix A, ApA, AAp, AApA, ApAAp ;
-
- nrows = nrows_orig;
- ncols = ncols_orig;
-
- A.resize(nrows,ncols) ;
- A = *this ;
-
- ApA = Ap * A;
- AApA = A * ApA;
- ApAAp = ApA * Ap;
- AAp = A * Ap;
-
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in pseudo inverse\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Ap : " << std::endl << Ap << std::endl;
- std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
- std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
- std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
- std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
- std::cout << " KerA : " << std::endl << kerA << std::endl;
- }
- // else printf("Ap OK ;-) \n");
-
- }
-#endif
-
- // std::cout << v << std::endl ;
- return rank ;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use getRow().
-
- Return the i-th row of the matrix.
- \warning notice row(1) is the 0th row.
-*/
-vpRowVector
-vpMatrix::row(const unsigned int i)
-{
- vpRowVector c(getCols()) ;
-
- for (unsigned int j =0 ; j < getCols() ; j++) c[j] = (*this)[i-1][j] ;
- return c ;
-}
-
-/*!
- \deprecated This method is deprecated. You should use getCol().
-
- Return the j-th columns of the matrix.
- \warning notice column(1) is the 0-th column.
- \param j : Index of the column to extract.
-*/
-vpColVector
-vpMatrix::column(const unsigned int j)
-{
- vpColVector c(getRows()) ;
-
- for (unsigned int i =0 ; i < getRows() ; i++) c[i] = (*this)[i][j-1] ;
- return c ;
-}
-#endif
-
-
-/*!
- Extract a column vector from a matrix.
- \warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If col=0, the first column is extracted.
- \param i_begin : Index of the row that gives the location of the first element of the column vector to extract.
- \param column_size : Size of the column vector to extract.
- \return The extracted column vector.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- for(unsigned int i=0; i < A.getRows(); i++)
- for(unsigned int j=0; j < A.getCols(); j++)
- A[i][j] = i*A.getCols()+j;
-
- A.print(std::cout, 4);
-
- vpColVector cv = A.getCol(1, 1, 3);
- std::cout << "Column vector: \n" << cv << std::endl;
-}
- \endcode
-It produces the following output:
- \code
-[4,4]=
- 0 1 2 3
- 4 5 6 7
- 8 9 10 11
- 12 13 14 15
-column vector:
-5
-9
-13
- \endcode
- */
-vpColVector
-vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
-{
- if (i_begin + column_size > getRows() || j >= getCols())
- throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
- vpColVector c(column_size);
- for (unsigned int i=0 ; i < column_size ; i++)
- c[i] = (*this)[i_begin+i][j];
- return c;
-}
-
-/*!
- Extract a column vector from a matrix.
- \warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If j=0, the first column is extracted.
- \return The extracted column vector.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- for(unsigned int i=0; i < A.getRows(); i++)
- for(unsigned int j=0; j < A.getCols(); j++)
- A[i][j] = i*A.getCols()+j;
-
- A.print(std::cout, 4);
-
- vpColVector cv = A.getCol(1);
- std::cout << "Column vector: \n" << cv << std::endl;
-}
- \endcode
-It produces the following output:
- \code
-[4,4]=
- 0 1 2 3
- 4 5 6 7
- 8 9 10 11
- 12 13 14 15
-column vector:
-1
-5
-9
-13
- \endcode
- */
-vpColVector
-vpMatrix::getCol(const unsigned int j) const
-{
- if (j >= getCols())
- throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
- unsigned int nb_rows = getRows();
- vpColVector c(nb_rows);
- for (unsigned int i=0 ; i < nb_rows ; i++)
- c[i] = (*this)[i][j];
- return c;
-}
-
-/*!
- Extract a row vector from a matrix.
- \warning All the indexes start from 0 in this function.
- \param i : Index of the row to extract. If i=0, the first row is extracted.
- \return The extracted row vector.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpMatrix.h>
-#include <visp/vpRowVector.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- for(unsigned int i=0; i < A.getRows(); i++)
- for(unsigned int j=0; j < A.getCols(); j++)
- A[i][j] = i*A.getCols()+j;
-
- A.print(std::cout, 4);
-
- vpRowVector rv = A.getRow(1);
- std::cout << "Row vector: \n" << rv << std::endl;
-} \endcode
-It produces the following output:
- \code
-[4,4]=
- 0 1 2 3
- 4 5 6 7
- 8 9 10 11
- 12 13 14 15
-Row vector:
-4 5 6 7
- \endcode
- */
-vpRowVector
-vpMatrix::getRow(const unsigned int i) const
-{
- if (i >= getRows())
- throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
- unsigned int nb_cols = getCols();
- vpRowVector r( nb_cols );
- for (unsigned int j=0 ; j < nb_cols ; j++)
- r[j] = (*this)[i][j];
- return r;
-}
-
-/*!
- Extract a row vector from a matrix.
- \warning All the indexes start from 0 in this function.
- \param i : Index of the row to extract. If i=0, the first row is extracted.
- \param j_begin : Index of the column that gives the location of the first element of the row vector to extract.
- \param row_size : Size of the row vector to extract.
- \return The extracted row vector.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpMatrix.h>
-#include <visp/vpRowVector.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- for(unsigned int i=0; i < A.getRows(); i++)
- for(unsigned int j=0; j < A.getCols(); j++)
- A[i][j] = i*A.getCols()+j;
-
- A.print(std::cout, 4);
-
- vpRowVector rv = A.getRow(1, 1, 3);
- std::cout << "Row vector: \n" << rv << std::endl;
-} \endcode
-It produces the following output:
- \code
-[4,4]=
- 0 1 2 3
- 4 5 6 7
- 8 9 10 11
- 12 13 14 15
-Row vector:
-5 6 7
- \endcode
- */
-vpRowVector
-vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
-{
- if (j_begin + row_size > getCols() || i >= getRows())
- throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
- vpRowVector r(row_size);
- for (unsigned int j=0 ; j < row_size ; j++)
- r[j] = (*this)[i][j_begin+i];
- return r;
-}
-
-/*!
-\brief Stack matrices. "Stack" two matrices C = [ A B ]^T
-
-\f$ C = \left( \begin{array}{c} A \\ B \end{array}\right) \f$
-
-\param A : Upper matrix.
-\param B : Lower matrix.
-\return Stacked matrix C = [ A B ]^T
-
-\warning A and B must have the same number of column.
-*/
-vpMatrix
-vpMatrix::stackMatrices(const vpMatrix &A, const vpMatrix &B)
-{
- vpMatrix C ;
-
- try{
- stackMatrices(A,B, C) ;
- }
- catch(vpMatrixException me)
- {
- vpCERROR << me << std::endl;
- throw ;
- }
-
- return C ;
-}
-
-/*!
-\relates vpMatrix
-\brief stackMatrices. "stack" two matrices C = [ A B ]^T
-
-\f$ C = \left( \begin{array}{c} A \\ B \end{array}\right) \f$
-
-\param A : Upper matrix.
-\param B : Lower matrix.
-\param C : Stacked matrix C = [ A B ]^T
-
-\warning A and B must have the same number of column
-*/
-void
-vpMatrix::stackMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- unsigned int nra = A.getRows() ;
- unsigned int nrb = B.getRows() ;
-
- if (nra !=0)
- if (A.getCols() != B.getCols())
- {
- vpERROR_TRACE("\n\t\t incorrect matrices size") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t incorrect matrices size")) ;
- }
-
- try {
- C.resize(nra+nrb,B.getCols() ) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- unsigned int i,j ;
- for (i=0 ; i < nra ; i++)
- for (j=0 ; j < A.getCols() ; j++)
- C[i][j] = A[i][j] ;
-
-
- for (i=0 ; i < nrb ; i++)
- for (j=0 ; j < B.getCols() ; j++)
- {
- C[i+nra][j] = B[i][j] ;
-
- }
-
-
-}
-
-
-
-
-
-/*!
-\brief Insert matrix B in matrix A at the given position.
-
-
-\param A : Main matrix.
-\param B : Matrix to insert.
-\param r : Index of the row where to add the matrix.
-\param c : Index of the column where to add the matrix.
-\return Matrix with B insert in A.
-
-\warning Throw exception if the sizes of the matrices do not allow the insertion.
-*/
-vpMatrix
-vpMatrix::insert(const vpMatrix &A, const vpMatrix &B,
- const unsigned int r, const unsigned int c)
-{
- vpMatrix C ;
-
- try{
- insert(A,B, C, r, c) ;
- }
- catch(vpMatrixException me)
- {
- vpCERROR << me << std::endl;
- throw me;
- }
-
- return C ;
-}
-
-/*!
-\relates vpMatrix
-Insert matrix B in matrix A at the given position.
-
-\param A : Main matrix.
-\param B : Matrix to insert.
-\param C : Result matrix.
-\param r : Index of the row where to insert matrix B.
-\param c : Index of the column where to insert matrix B.
-
-\warning Throw exception if the sizes of the matrices do not
-allow the insertion.
-*/
-void
-vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C,
- const unsigned int r, const unsigned int c)
-{
- if( ( (r + B.getRows()) <= A.getRows() ) &&
- ( (c + B.getCols()) <= A.getCols() ) ){
- try {
- C.resize(A.getRows(),A.getCols() ) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
- for(unsigned int i=0; i<A.getRows(); i++){
- for(unsigned int j=0; j<A.getCols(); j++){
- if(i >= r && i < (r + B.getRows()) && j >= c && j < (c+B.getCols())){
- C[i][j] = B[i-r][j-c];
- }
- else{
- C[i][j] = A[i][j];
- }
- }
- }
- }
- else{
- throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\tIncorrect size of the matrix to insert data.");
- }
-}
-
-/*!
-Juxtapose to matrices C = [ A B ].
-
-\f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$
-
-\param A : Left matrix.
-\param B : Right matrix.
-\return Juxtaposed matrix C = [ A B ]
-
-\warning A and B must have the same number of column
-*/
-vpMatrix
-vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
-{
- vpMatrix C ;
-
- try{
- juxtaposeMatrices(A,B, C) ;
- }
- catch(vpMatrixException me)
- {
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- return C ;
-}
-
-/*!
-\relates vpMatrix
-Juxtapose to matrices C = [ A B ].
-
-\f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$
-
-\param A : Left matrix.
-\param B : Right matrix.
-\param C : Juxtaposed matrix C = [ A B ]
-
-\warning A and B must have the same number of column
-*/
-void
-vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- unsigned int nca = A.getCols() ;
- unsigned int ncb = B.getCols() ;
-
- if (nca !=0)
- if (A.getRows() != B.getRows())
- {
- vpERROR_TRACE("\n\t\t incorrect matrices size") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t incorrect matrices size")) ;
- }
-
- try {
- C.resize(B.getRows(),nca+ncb) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
-
- unsigned int i,j ;
- for (i=0 ; i < C.getRows(); i++)
- for (j=0 ; j < nca ; j++)
- C[i][j] = A[i][j] ;
-
-
- for (i=0 ; i < C.getRows() ; i++)
- for (j=0 ; j < ncb ; j++)
- {
- C[i][nca+j] = B[i][j] ;
- }
-}
-
-/*!
-
-Create a diagonal matrix with the element of a vector.
-
-\param A : Vector which element will be put in the diagonal.
-
-\sa createDiagonalMatrix()
-
-\code
-#include <iostream>
-
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A;
-vpColVector v(3);
-
-v[0] = 1;
-v[1] = 2;
-v[2] = 3;
-
-A.diag(v);
-
-std::cout << "A:\n" << A << std::endl;
-
-// A is now equal to:
-// 1 0 0
-// 0 2 0
-// 0 0 3
-}
-\endcode
-*/
-
-void
-vpMatrix::diag(const vpColVector &A)
-{
- unsigned int rows = A.getRows() ;
- try {
- this->resize(rows,rows) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
- (*this) = 0 ;
- for (unsigned int i=0 ; i< rows ; i++ )
- (* this)[i][i] = A[i] ;
-}
-/*!
-
-Create a diagonal matrix with the element of a vector \f$ DA_{ii} = A_i \f$.
-
-\param A : Vector which element will be put in the diagonal.
-
-\param DA : Diagonal matrix DA[i][i] = A[i]
-
-\sa diag()
-*/
-
-void
-vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
-{
- unsigned int rows = A.getRows() ;
- try {
- DA.resize(rows,rows) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- vpCERROR << me << std::endl ;
- throw ;
- }
- DA =0 ;
- for (unsigned int i=0 ; i< rows ; i++ )
- DA[i][i] = A[i] ;
-}
-
-//--------------------------------------------------------------------
-// Output
-//--------------------------------------------------------------------
-
-
-/*!
-\brief std::cout a matrix
-*/
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpMatrix &m)
-{
- std::ios_base::fmtflags original_flags = s.flags();
-
- s.precision(10) ;
- for (unsigned int i=0;i<m.getRows();i++) {
- for (unsigned int j=0;j<m.getCols();j++){
- s << m[i][j] << " ";
- }
- // We don't add a \n char on the end of the last matrix line
- if (i < m.getRows()-1)
- s << std::endl;
- }
-
- s.flags(original_flags); // restore s to standard state
-
- return s;
-}
-
-/*!
-
-Pretty print a matrix. The data are tabulated.
-The common widths before and after the decimal point
-are set with respect to the parameter maxlen.
-
-\param s
-Stream used for the printing.
-
-\param length
-The suggested width of each matrix element.
-The actual width grows in order to accomodate the whole integral part,
-and shrinks if the whole extent is not needed for all the numbers.
-\param intro
-The introduction which is printed before the matrix.
-Can be set to zero (or omitted), in which case
-the introduction is not printed.
-
-\return
-Returns the common total width for all matrix elements
-
-\sa std::ostream &operator <<(ostream &s,const vpMatrix &m)
-*/
-int
-vpMatrix::print(std::ostream& s, unsigned int length, char const* intro)
-{
- typedef std::string::size_type size_type;
-
- unsigned int m = getRows();
- unsigned int n = getCols();
-
- std::vector<std::string> values(m*n);
- std::ostringstream oss;
- std::ostringstream ossFixed;
- std::ios_base::fmtflags original_flags = oss.flags();
-
- // ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
-
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
- // one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
- for (unsigned int j=0;j<n;++j){
- oss.str("");
- oss << (*this)[i][j];
- if (oss.str().find("e")!=std::string::npos){
- ossFixed.str("");
- ossFixed << (*this)[i][j];
- oss.str(ossFixed.str());
- }
-
- values[i*n+j]=oss.str();
- size_type thislen=values[i*n+j].size();
- size_type p=values[i*n+j].find('.');
-
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
- // maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
- }
- }
- }
-
- size_type totalLength=length;
- // increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
- // decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
-
- // the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
-
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
-
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- for (unsigned int j=0;j<n;j++){
- size_type p=values[i*n+j].find('.');
- s.setf(std::ios::right, std::ios::adjustfield);
- s.width((std::streamsize)maxBefore);
- s <<values[i*n+j].substr(0,p).c_str();
-
- if (maxAfter>0){
- s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
- s.width((std::streamsize)maxAfter);
- s <<values[i*n+j].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
- s.width((std::streamsize)maxAfter);
- s <<".0";
- }
- }
-
- s <<' ';
- }
- s <<std::endl;
- }
-
- s.flags(original_flags); // restore s to standard state
-
- return (int)(maxBefore+maxAfter);
-}
-
-
-/*!
-\brief Print using matlab syntax, to be put in matlab later.
-
-Print using the following form:
-[ a,b,c;
-d,e,f;
-g,h,i]
-*/
-std::ostream & vpMatrix::
-matlabPrint(std::ostream & os) const
-{
-
- unsigned int i,j;
-
- os << "[ ";
- for (i=0; i < this->getRows(); ++ i)
- {
- for (j=0; j < this ->getCols(); ++ j)
- {
- os << (*this)[i][j] << ", ";
- }
- if (this ->getRows() != i+1) { os << ";" << std::endl; }
- else { os << "]" << std::endl; }
- }
- return os;
-};
-
-/*!
-\brief Print using MAPLE matrix input format.
-
-Print using the following way so that this output can be directly copied into MAPLE:
-([
-[0.939846, 0.0300754, 0.340272, ],
-[0.0300788, 0.984961, -0.170136, ],
-[-0.340272, 0.170136, 0.924807, ],
-])
-*/
-std::ostream & vpMatrix::
-maplePrint(std::ostream & os) const
-{
- unsigned int i,j;
-
- os << "([ " << std::endl;
- for (i=0; i < this->getRows(); ++ i)
- { os << "[";
- for (j=0; j < this->getCols(); ++ j)
- {
- os << (*this)[i][j] << ", ";
- }
- os << "]," << std::endl;
- }
- os << "])" << std::endl;
- return os;
-};
-
-/*!
-\brief Print matrix in csv format.
-
-Print as comma separated values so that this output can be imported into any program which has a csv data import option:
-0.939846, 0.0300754, 0.340272
-0.0300788, 0.984961, -0.170136
--0.340272, 0.170136, 0.924807
-*/
-std::ostream & vpMatrix::
-csvPrint(std::ostream & os) const
-{
- unsigned int i,j;
-
- for (i=0; i < this->getRows(); ++ i)
- {
- for (j=0; j < this->getCols(); ++ j)
- {
- os << (*this)[i][j];
- if (!(j==(this->getCols()-1)))
- os << ", ";
- }
- os << std::endl;
- }
- return os;
-};
-
-
-/*!
-\brief Print to be used as part of a C++ code later.
-
-Print under the following form:
-vpMatrix A(6,4);
-A[0][0] = 1.4;
-A[0][1] = 0.6; ...
-
-\param os: the stream to be printed in.
-\param matrixName: name of the matrix, "A" by default, to be used for
-the line vpMatrix A(6,7) (see example).
-\param octet: if false, print using double, if true, print byte per byte
-each bytes of the double array.
-*/
-std::ostream & vpMatrix::
-cppPrint(std::ostream & os, const char * matrixName, bool octet) const
-{
-
- unsigned int i,j;
- const char defaultName [] = "A";
- if (NULL == matrixName)
- {
- matrixName = defaultName;
- }
- os << "vpMatrix " << defaultName
- << " (" << this ->getRows ()
- << ", " << this ->getCols () << "); " <<std::endl;
-
- for (i=0; i < this->getRows(); ++ i)
- {
- for (j=0; j < this ->getCols(); ++ j)
- {
- if (! octet)
- {
- os << defaultName << "[" << i << "][" << j
- << "] = " << (*this)[i][j] << "; " << std::endl;
- }
- else
- {
- for (unsigned int k = 0; k < sizeof(double); ++ k)
- {
- os << "((unsigned char*)&(" << defaultName
- << "[" << i << "][" << j << "]) )[" << k
- <<"] = 0x" <<std::hex<<
- (unsigned int)((unsigned char*)& ((*this)[i][j])) [k]
- << "; " << std::endl;
- }
- }
- }
- os << std::endl;
- }
- return os;
-};
-
-
-/*!
-Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{x_{ij}^2}} \f$.
-
-\return The Euclidean norm if the matrix is initialized, 0 otherwise.
-
-\sa infinityNorm()
-*/
-double
-vpMatrix::euclideanNorm () const
-{
- double norm=0.0;
- double x ;
- for (unsigned int i=0;i<dsize;i++) {
- x = *(data +i); norm += x*x;
- }
-
- return sqrt(norm);
-}
-
-
-
-/*!
-
-Compute and return the infinity norm \f$ {||x||}_{\infty} =
-max\left(\sum_{j=0}^{n}{\mid x_{ij} \mid}\right) \f$ with \f$i \in
-\{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size.
-
-\return The infinity norm if the matrix is initialized, 0 otherwise.
-
-\sa euclideanNorm()
-*/
-double
-vpMatrix::infinityNorm () const
-{
- double norm=0.0;
- double x ;
- for (unsigned int i=0;i<rowNum;i++){
- x = 0;
- for (unsigned int j=0; j<colNum;j++){
- x += fabs (*(*(rowPtrs + i)+j)) ;
- }
- if (x > norm) {
- norm = x;
- }
- }
- return norm;
-}
-
-/*!
-Compute the determinant of the matrix using the LU Decomposition.
-
-\return The determinant of the matrix if the matrix is square, 0 otherwise.
-
-See the Numerical Recipes in C page 43 for further explanations.
-*/
-
-double vpMatrix::detByLU() const
-{
- double det_ = 0;
-
- // Test wether the matrix is squred
- if (rowNum == colNum)
- {
- // create a temporary matrix that will be modified by LUDcmp
- vpMatrix tmp(*this);
-
- // using th LUdcmp based on NR codes
- // it modified the tmp matrix in a special structure of type :
- // b11 b12 b13 b14
- // a21 b22 b23 b24
- // a21 a32 b33 b34
- // a31 a42 a43 b44
-
- unsigned int * perm = new unsigned int[rowNum]; // stores the permutations
- int d; // +- 1 fi the number of column interchange is even or odd
- tmp.LUDcmp(perm, d);
- delete[]perm;
-
- // compute the determinant that is the product of the eigen values
- det_ = (double) d;
- for(unsigned int i=0;i<rowNum;i++)
- {
- det_*=tmp[i][i];
- }
- }
-
- else {
- vpERROR_TRACE("Determinant Computation : ERR Matrix not squared") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tDeterminant Computation : ERR Matrix not squared")) ;
-
-
- }
- return det_ ;
-}
-
-
-
-/*!
-Stack A at the end of the current matrix, or copy if the matrix has no dimensions : this = [ this A ]^T
-
-Here an example for a robot velocity log :
-\code
-vpMatrix Velocities;
-vpColVector v(6);
-for(unsigned int i = 0;i<100;i++)
-{
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, v);
- Velocities.stackMatrices(v.t());
-}
-\endcode
-*/
-void vpMatrix::stackMatrices(const vpMatrix &A)
-{
- if(rowNum == 0)
- *this = A;
- else
- *this = vpMatrix::stackMatrices(*this, A);
-}
-
-
-/*!
-Insert matrix A at the given position in the current matrix.
-
-\warning Throw vpMatrixException::incorrectMatrixSizeError if the
-dimensions of the matrices do not allow the operation.
-
-\param A : The matrix to insert.
-\param r : The index of the row to begin to insert data.
-\param c : The index of the column to begin to insert data.
-*/
-void vpMatrix::insert(const vpMatrix&A, const unsigned int r,
- const unsigned int c)
-{
- if( (r + A.getRows() ) <= rowNum && (c + A.getCols() ) <= colNum ){
- // recopy matrix A in the current one, does not call static function to avoid initialisation and recopy of matrix
- for(unsigned int i=r; i<(r+A.getRows()); i++){
- for(unsigned int j=c; j<(c+A.getCols()); j++){
- (*this)[i][j] = A[i-r][j-c];
- }
- }
- }
- else{
- throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\tIncorrect size of the matrix to insert data.");
- }
-}
-
-
-/*!
-Compute the eigenvalues of a n-by-n real symmetric matrix.
-
-\return The eigenvalues of a n-by-n real symmetric matrix.
-
-\warning This method is only available if the Gnu Scientific Library
-(GSL) is detected as a third party library.
-
-\exception vpMatrixException::matrixError If the matrix is not
-square or if the matrix is not symmetric.
-
-\exception vpMatrixException::notImplementedError If the GSL library
-is not detected
-
-Here an example:
-\code
-#include <iostream>
-
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(3,3); // A is a symmetric matrix
-A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
-A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.;
-A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.;
-std::cout << "Initial symmetric matrix: \n" << A << std::endl;
-
-// Compute the eigen values
-vpColVector evalue; // Eigenvalues
-evalue = A.eigenValues();
-std::cout << "Eigen values: \n" << evalue << std::endl;
-}
-\endcode
-
-\sa eigenValues(vpColVector &, vpMatrix &)
-
-*/
-vpColVector vpMatrix::eigenValues()
-{
- if (rowNum != colNum) {
- vpERROR_TRACE("Eigen values computation: ERR Matrix not square") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tEigen values computation: ERR Matrix not square")) ;
- }
-
-#ifdef VISP_HAVE_GSL /* be careful of the copy below */
- {
- // Check if the matrix is symetric: At - A = 0
- vpMatrix At_A = (*this).t() - (*this);
- for (unsigned int i=0; i < rowNum; i++) {
- for (unsigned int j=0; j < rowNum; j++) {
- //if (At_A[i][j] != 0) {
- if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
- vpERROR_TRACE("Eigen values computation: ERR Matrix not symmetric") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tEigen values computation: "
- "ERR Matrix not symmetric")) ;
- }
- }
- }
-
-
- vpColVector evalue(rowNum); // Eigen values
-
- gsl_vector *eval = gsl_vector_alloc (rowNum);
- gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
-
- gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
- gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
-
- unsigned int Atda = m->tda ;
- for (unsigned int i=0 ; i < rowNum ; i++){
- unsigned int k = i*Atda ;
- for (unsigned int j=0 ; j < colNum ; j++)
- m->data[k+j] = (*this)[i][j] ;
- }
- gsl_eigen_symmv (m, eval, evec, w);
-
- gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
-
- for (unsigned int i=0; i < rowNum; i++) {
- evalue[i] = gsl_vector_get (eval, i);
- }
-
- gsl_eigen_symmv_free (w);
- gsl_vector_free (eval);
- gsl_matrix_free (m);
- gsl_matrix_free (evec);
-
- return evalue;
- }
-#else
- {
- vpERROR_TRACE("Not implemented since the GSL library is not detected.") ;
- throw(vpMatrixException(vpMatrixException::notImplementedError,
- "\n\t\tEigen values Computation: Not implemented "
- "since the GSL library is not detected")) ;
- }
-#endif
-}
-
-/*!
-Compute the eigenvalues of a n-by-n real symmetric matrix.
-\return The eigenvalues of a n-by-n real symmetric matrix.
-
-\warning This method is only available if the Gnu Scientific Library
-(GSL) is detected as a third party library.
-
-\param evalue : Eigenvalues of the matrix.
-
-\param evector : Eigenvector of the matrix.
-
-\exception vpMatrixException::matrixError If the matrix is not
-square or if the matrix is not symmetric.
-
-\exception vpMatrixException::notImplementedError If the GSL library
-is not detected
-
-Here an example:
-\code
-#include <iostream>
-
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(4,4); // A is a symmetric matrix
-A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
-A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.;
-A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.;
-A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
-std::cout << "Initial symmetric matrix: \n" << A << std::endl;
-
-vpColVector d; // Eigenvalues
-vpMatrix V; // Eigenvectors
-
-// Compute the eigenvalues and eigenvectors
-A.eigenValues(d, V);
-std::cout << "Eigen values: \n" << d << std::endl;
-std::cout << "Eigen vectors: \n" << V << std::endl;
-
-vpMatrix D;
-D.diag(d); // Eigenvalues are on the diagonal
-
-std::cout << "D: " << D << std::endl;
-
-// Verification: A * V = V * D
-std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl;
-}
-\endcode
-
-\sa eigenValues()
-
-*/
-#ifdef VISP_HAVE_GSL /* be careful of the copy below */
-void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector)
-#else
-void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */)
-#endif
-{
- if (rowNum != colNum) {
- vpERROR_TRACE("Eigen values computation: ERR Matrix not square") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tEigen values computation: ERR Matrix not square")) ;
- }
-
-#ifdef VISP_HAVE_GSL /* be careful of the copy below */
- {
- // Check if the matrix is symetric: At - A = 0
- vpMatrix At_A = (*this).t() - (*this);
- for (unsigned int i=0; i < rowNum; i++) {
- for (unsigned int j=0; j < rowNum; j++) {
- //if (At_A[i][j] != 0) {
- if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
- vpERROR_TRACE("Eigen values computation: ERR Matrix not symmetric") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tEigen values computation: "
- "ERR Matrix not symmetric")) ;
- }
- }
- }
-
- // Resize the output matrices
- evalue.resize(rowNum);
- evector.resize(rowNum, colNum);
-
- gsl_vector *eval = gsl_vector_alloc (rowNum);
- gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
-
- gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
- gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
-
- unsigned int Atda = m->tda ;
- for (unsigned int i=0 ; i < rowNum ; i++){
- unsigned int k = i*Atda ;
- for (unsigned int j=0 ; j < colNum ; j++)
- m->data[k+j] = (*this)[i][j] ;
- }
- gsl_eigen_symmv (m, eval, evec, w);
-
- gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
-
- for (unsigned int i=0; i < rowNum; i++) {
- evalue[i] = gsl_vector_get (eval, i);
- }
- Atda = evec->tda ;
- for (unsigned int i=0; i < rowNum; i++) {
- unsigned int k = i*Atda ;
- for (unsigned int j=0; j < rowNum; j++) {
- evector[i][j] = evec->data[k+j];
- }
- }
-
-
- // {
- // int i;
-
- // for (i = 0; i < rowNum; i++)
- // {
- // double eval_i
- // = gsl_vector_get (eval, i);
- // gsl_vector_view evec_i
- // = gsl_matrix_column (evec, i);
-
- // printf ("eigenvalue = %g\n", eval_i);
- // printf ("eigenvector = \n");
- // gsl_vector_fprintf (stdout,
- // &evec_i.vector, "%g");
- // }
- // }
-
- gsl_eigen_symmv_free (w);
- gsl_vector_free (eval);
- gsl_matrix_free (m);
- gsl_matrix_free (evec);
- }
-#else
- {
- vpERROR_TRACE("Not implemented since the GSL library is not detected.") ;
- throw(vpMatrixException(vpMatrixException::notImplementedError,
- "\n\t\tEigen values Computation: Not implemented "
- "since the GSL library is not detected")) ;
- }
-#endif
-}
-
-
-/*!
-Function to compute the null space (the kernel) of the interaction matrix A which is not full rank.
-The null space ( the kernel ) of a matrix A is defined as Null(A) = Ker(M) ={KerA : A*KerA =0}.
-
-\param kerA : The matrix to contain the null space (kernel) of A (A*KerA.t()=0)
-\param svThreshold : Specify the used threshold in the svd(...) function (a function to compute the singular value decomposition)
-
-\return the rank of the matrix.
-*/
-unsigned int
-vpMatrix::kernel(vpMatrix &kerA, double svThreshold)
-{
-#if (DEBUG_LEVEL1)
- std::cout << "begin Kernel" << std::endl ;
-#endif
- unsigned int i, j ;
- unsigned int ncaptor = getRows() ;
- unsigned int ddl = getCols() ;
- vpMatrix C ;
-
- if (ncaptor == 0)
- std::cout << "Erreur Matrice non initialise" << std::endl ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "Interaction matrix L" << std::endl ;
- std::cout << *this ;
- std::cout << "signaux capteurs : " << ncaptor << std::endl ;
- }
-#endif
-
- C.resize(ddl,ncaptor) ;
- unsigned int min ;
-
- if (ncaptor > ddl) min = ddl ; else min = ncaptor ;
-
- // ! the SVDcmp function inthe matrix lib is destructive
-
- vpMatrix a1 ;
- vpMatrix a2 ;
-
- vpColVector sv(ddl) ; // singular values
- vpMatrix v(ddl,ddl) ;
-
- if (ncaptor < ddl)
- {
- a1.resize(ddl,ddl) ;
- }
- else
- {
- a1.resize(ncaptor,ddl) ;
- }
-
- a2.resize(ncaptor,ddl) ;
-
- for (i=0 ; i < ncaptor ; i++)
- for (j=0 ; j < ddl ; j++)
- {
- a1[i][j] = (*this)[i][j] ;
- a2[i][j] = (*this)[i][j] ;
- }
-
-
- if (ncaptor < ddl)
- {
- for (i=ncaptor ; i < ddl ; i++)
- for (j=0 ; j < ddl ; j++)
- {
- a1[i][j] = 0 ;
- }
- a1.svd(sv,v);
- }
- else
- {
- a1.svd(sv,v);
- }
-
- // compute the highest singular value and the rank of h
-
- double maxsv = 0 ;
- for (i=0 ; i < ddl ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
-
-#if (DEBUG_LEVEL2)
- {
- std::cout << "Singular Value : (" ;
- for (i=0 ; i < ddl ; i++) std::cout << sv[i] << " , " ;
- std::cout << ")" << std::endl ;
- }
-#endif
-
- unsigned int rank = 0 ;
- for (i=0 ; i < ddl ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
-
-#if (DEBUG_LEVEL2)
- {
-
- for (i = 0 ; i < ddl ; i++)
- {
- for (j = 0 ; j < ncaptor ; j++)
- {
- unsigned int k=0 ;
- C[i][j] = 0.0;
-
- // modif le 25 janvier 1999 0.001 <-- maxsv*1.e-ndof
- // sinon on peut observer une perte de range de la matrice
- // ( d'ou venait ce 0.001 ??? )
- for (k=0 ; k < ddl ; k++) if (fabs(sv[k]) > maxsv*svThreshold)
- {
- C[i][j] += v[i][k]*a1[j][k]/sv[k];
- }
- }
- }
-
- // cout << v << endl ;
- std::cout << C << std::endl ;
- }
-#endif
- /*------------------------------------------------------- */
- if (rank != ddl)
- {
- // Compute the kernel if wanted
- if (min < ddl)
- {
- vpMatrix ch(ddl,ddl) ;
- ch = C*a2 ;
- ch.svd(sv,v) ;
-
- }
- // if (noyau == 1)
- {
- maxsv = 0 ;
- for (i=0 ; i < ddl ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
- rank = 0 ;
- for (i=0 ; i < ddl ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
- vpMatrix cons(ddl,ddl) ;
-
- cons =0 ;
- for (j = 0 ; j < ddl ; j++)
- {
- for (i = 0 ; i < ddl ; i++)
- // Change Nicolas Mansard 23/4/04
- // was if (fabs(sv[i]) < maxsv*seuilvp)
- if (fabs(sv[i]) <= maxsv*svThreshold)
- {
- cons[i][j] = v[j][i];
- }
- }
-
- vpMatrix Ker(ddl-rank,ddl) ;
- unsigned int k =0 ;
- for (j = 0 ; j < ddl ; j++)
- {
- // cout << cons.Row(j+1) << " = " << cons.Row(j+1).SumSquare() << endl ;
-
- //if (cons.row(j+1).sumSquare() !=0)
- if (std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
- {
- for (i=0 ; i < cons.getCols() ; i++)
- Ker[k][i] = cons[j][i] ;
- // Ker.Row(k+1) = cons.Row(j+1) ;
- k++;
- }
- }
- kerA = Ker ;
- }
- }
-#if (DEBUG_LEVEL1)
- std::cout << "end Kernel" << std::endl ;
-#endif
- return rank ;
-}
-
-/*!
-Compute the determinant of a n-by-n matrix.
-
-\param method : Method used to compute the determinant. Default LU
-decomposition methos is faster than the method based on Gaussian
-elimination.
-
-\return Determinant of the matrix.
-
-\code
-#include <iostream>
-
-#include <visp/vpMatrix.h>
-
-int main()
-{
-vpMatrix A(3,3);
-A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
-A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
-A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
-std::cout << "Initial matrix: \n" << A << std::endl;
-
-// Compute the determinant
-std:: cout << "Determinant by default method : " <<
-A.det() << std::endl;
-std:: cout << "Determinant by LU decomposition: " <<
-A.det(vpMatrix::LU_DECOMPOSITION ) << std::endl;
-}
-\endcode
-*/
-double vpMatrix::det(vpDetMethod method) const
-{
- double det_ = 0;
-
- if ( method == LU_DECOMPOSITION )
- {
- det_ = this->detByLU();
- }
-
- return (det_);
-}
-
-
-/*!
-Save a matrix to a file.
-
-\param filename : Absolute file name.
-\param M : Matrix to be saved.
-\param binary : If true the matrix is saved in a binary file, else a text file.
-\param header : Optional line that will be saved at the beginning of the file.
-
-\return Returns true if no problem happened.
-
-Warning : If you save the matrix as in a text file the precision is
-less than if you save it in a binary file.
-*/
-bool
-vpMatrix::saveMatrix(const char *filename, const vpMatrix &M,
- const bool binary, const char *header)
-{
- std::fstream file;
-
- if (!binary)
- file.open(filename, std::fstream::out);
- else
- file.open(filename, std::fstream::out|std::fstream::binary);
-
- if(!file)
- {
- file.close();
- return false;
- }
-
- else
- {
- if (!binary)
- {
- unsigned int i = 0;
- file << "# ";
- while (header[i] != '\0')
- {
- file << header[i];
- if (header[i] == '\n')
- file << "# ";
- i++;
- }
- file << '\0';
- file << std::endl;
- file << M.getRows() << "\t" << M.getCols() << std::endl;
- file << M << std::endl;
- }
- else
- {
- int headerSize = 0;
- while (header[headerSize] != '\0') headerSize++;
- file.write(header,headerSize+1);
- unsigned int matrixSize;
- matrixSize = M.getRows();
- file.write((char*)&matrixSize,sizeof(int));
- matrixSize = M.getCols();
- file.write((char*)&matrixSize,sizeof(int));
- double value;
- for(unsigned int i = 0; i < M.getRows(); i++)
- {
- for(unsigned int j = 0; j < M.getCols(); j++)
- {
- value = M[i][j];
- file.write((char*)&value,sizeof(double));
- }
- }
- }
- }
-
- file.close();
- return true;
-}
-
-/*!
- Save a matrix in a YAML-formatted file.
-
- \param filename : absolute file name.
- \param M : matrix to be saved in the file.
- \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
-
- \return Returns true if success.
-
- Here is an example of outputs.
-\code
-vpMatrix M(3,4);
-vpMatrix::saveMatrixYAML("matrix.yml", M, "example: a YAML-formatted header");
-vpMatrix::saveMatrixYAML("matrixIndent.yml", M, "example:\n - a YAML-formatted header\n - with inner indentation");
-\endcode
-Content of matrix.yml:
-\code
-example: a YAML-formatted header
-rows: 3
-cols: 4
-data:
- - [0, 0, 0, 0]
- - [0, 0, 0, 0]
- - [0, 0, 0, 0]
-\endcode
-Content of matrixIndent.yml:
-\code
-example:
- - a YAML-formatted header
- - with inner indentation
-rows: 3
-cols: 4
-data:
- - [0, 0, 0, 0]
- - [0, 0, 0, 0]
- - [0, 0, 0, 0]
-\endcode
-
- \sa loadMatrixYAML()
-*/
-bool vpMatrix::saveMatrixYAML(const char *filename, const vpMatrix &M, const char *header)
-{
- std::fstream file;
-
- file.open(filename, std::fstream::out);
-
- if(!file)
- {
- file.close();
- return false;
- }
-
- unsigned int i = 0;
- bool inIndent = false;
- std::string indent = "";
- bool checkIndent = true;
- while (header[i] != '\0')
- {
- file << header[i];
- if(checkIndent)
- {
- if (inIndent)
- {
- if(header[i] == ' ')
- indent += " ";
- else if (indent.length() > 0)
- checkIndent = false;
- }
- if (header[i] == '\n' || (inIndent && header[i] == ' '))
- inIndent = true;
- else
- inIndent = false;
- }
- i++;
- }
-
- if(i != 0)
- file << std::endl;
- file << "rows: " << M.getRows() << std::endl;
- file << "cols: " << M.getCols() << std::endl;
-
- if(indent.length() == 0)
- indent = " ";
-
- file << "data: " << std::endl;
- unsigned int j;
- for(i=0;i<M.getRows();++i)
- {
- file << indent << "- [";
- for(j=0;j<M.getCols()-1;++j)
- file << M[i][j] << ", ";
- file << M[i][j] << "]" << std::endl;
- }
-
- file.close();
- return true;
-}
-
-
-/*!
- Load a matrix from a file.
-
- \param filename : Absolute file name.
- \param M : Matrix to be loaded.
- \param binary : If true the matrix is loaded from a binary file,
- else from a text file.
- \param header : header of the file loaded in this parameter.
-
- \return Returns true if no problem happened.
-*/
-bool
-vpMatrix::loadMatrix(const char *filename, vpMatrix &M, const bool binary,
- char *header)
-{
- std::fstream file;
-
- if (!binary)
- file.open(filename, std::fstream::in);
- else
- file.open(filename, std::fstream::in|std::fstream::binary);
-
- if(!file)
- {
- file.close();
- return false;
- }
-
- else
- {
- if (!binary)
- {
- char c='0';
- std::string h;
- while ((c != '\0') && (c != '\n'))
- {
- file.read(&c,1);
- h+=c;
- }
- if (header != NULL)
- strncpy(header, h.c_str(), h.size() + 1);
-
- unsigned int rows, cols;
- file >> rows;
- file >> cols;
-
- if (rows > std::numeric_limits<unsigned int>::max()
- || cols > std::numeric_limits<unsigned int>::max())
- throw vpException(vpException::badValue, "Matrix exceed the max size.");
-
- M.resize(rows,cols);
-
- double value;
- for(unsigned int i = 0; i < rows; i++)
- {
- for(unsigned int j = 0; j < cols; j++)
- {
- file >> value;
- M[i][j] = value;
- }
- }
- }
- else
- {
- char c='0';
- std::string h;
- while ((c != '\0') && (c != '\n'))
- {
- file.read(&c,1);
- h+=c;
- }
- if (header != NULL)
- strncpy(header, h.c_str(), h.size() + 1);
-
- unsigned int rows, cols;
- file.read((char*)&rows,sizeof(unsigned int));
- file.read((char*)&cols,sizeof(unsigned int));
- M.resize(rows,cols);
-
- double value;
- for(unsigned int i = 0; i < rows; i++)
- {
- for(unsigned int j = 0; j < cols; j++)
- {
- file.read((char*)&value,sizeof(double));
- M[i][j] = value;
- }
- }
- }
- }
-
- file.close();
- return true;
-}
-
-
-/*!
- Load a matrix from a YAML-formatted file.
-
- \param filename : absolute file name.
- \param M : matrix to be loaded from the file.
- \param header : header of the file is loaded in this parameter.
-
- \return Returns true on success.
-
- \sa saveMatrixYAML()
-
-*/
-bool
-vpMatrix::loadMatrixYAML(const char *filename, vpMatrix &M, char *header)
-{
- std::fstream file;
-
- file.open(filename, std::fstream::in);
-
- if(!file)
- {
- file.close();
- return false;
- }
-
- unsigned int rows = 0,cols = 0;
- std::string h;
- std::string line,subs;
- bool inheader = true;
- unsigned int i=0, j;
- unsigned int lineStart = 0;
-
- while ( getline (file,line) )
- {
- if(inheader)
- {
- if(rows == 0 && line.compare(0,5,"rows:") == 0)
- {
- std::stringstream ss(line);
- ss >> subs;
- ss >> rows;
- }
- else if (cols == 0 && line.compare(0,5,"cols:") == 0)
- {
- std::stringstream ss(line);
- ss >> subs;
- ss >> cols;
- }
- else if (line.compare(0,5,"data:") == 0)
- inheader = false;
- else
- h += line + "\n";
- }
- else
- {
- // if i == 0, we just got out of the header: initialize matrix dimensions
- if(i == 0)
- {
- if(rows == 0 || cols == 0)
- {
- file.close();
- return false;
- }
- M.resize(rows, cols);
- // get indentation level which is common to all lines
- lineStart = (unsigned int)line.find("[") + 1;
- }
- std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
- j = 0;
- while(getline(ss, subs, ','))
- M[i][j++] = atof(subs.c_str());
- i++;
- }
- }
-
- if (header != NULL)
- strncpy(header, h.substr(0,h.length()-1).c_str(), h.size());
-
- file.close();
- return true;
-}
-
-/*!
-
-Compute the exponential matrix of a square matrix.
-
-\return Return the exponential matrix.
-
-*/
-vpMatrix
-vpMatrix::expm()
-{
- if(colNum != rowNum)
- {
- vpTRACE("The matrix must be square");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "The matrix must be square" ));
- }
- else
- {
-#ifdef VISP_HAVE_GSL
- size_t size_ = rowNum * colNum;
- double *b = new double [size_];
- for (size_t i=0; i< size_; i++)
- b[i] = 0.;
- gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
- gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
- gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
- //gsl_matrix_fprintf(stdout, &em.matrix, "%g");
- vpMatrix expA(rowNum, colNum);
- memcpy(expA.data, b, size_ * sizeof(double));
-
- delete [] b;
- return expA;
-#else
- vpMatrix _expE(rowNum, colNum);
- vpMatrix _expD(rowNum, colNum);
- vpMatrix _expX(rowNum, colNum);
- vpMatrix _expcX(rowNum, colNum);
- vpMatrix _eye(rowNum, colNum);
-
- _eye.setIdentity();
- vpMatrix exp(*this);
-
- // double f;
- int e;
- double c = 0.5;
- int q = 6;
- int p = 1;
-
- double nA = 0;
- for (unsigned int i = 0; i < rowNum;i++)
- {
- double sum = 0;
- for (unsigned int j=0; j < colNum; j++)
- {
- sum += fabs((*this)[i][j]);
- }
- if (sum>nA||i==0)
- {
- nA=sum;
- }
- }
-
- /* f = */ frexp(nA, &e);
- //double s = (0 > e+1)?0:e+1;
- double s = e+1;
-
- double sca = 1.0 / pow(2.0,s);
- exp=sca*exp;
- _expX=*this;
- _expE=c*exp+_eye;
- _expD=-c*exp+_eye;
- for (int k=2;k<=q;k++)
- {
- c = c * ((double)(q-k+1)) / ((double)(k*(2*q-k+1)));
- _expcX=exp*_expX;
- _expX=_expcX;
- _expcX=c*_expX;
- _expE=_expE+_expcX;
- if (p) _expD=_expD+_expcX;
- else _expD=_expD- _expcX;
- p = !p;
- }
- _expX=_expD.inverseByLU();
- exp=_expX*_expE;
- for (int k=1;k<=s;k++)
- {
- _expE=exp*exp;
- exp=_expE;
- }
- return exp;
-#endif
- }
-}
-
-double
-vpMatrix::getMinValue() const
-{
- double *dataptr = data;
- double min = *dataptr;
- dataptr++;
- for (unsigned int i = 0; i < dsize-1; i++)
- {
- if (*dataptr < min) min = *dataptr;
- dataptr++;
- }
- return min;
-}
-
-double
-vpMatrix::getMaxValue() const
-{
- double *dataptr = data;
- double max = *dataptr;
- dataptr++;
- for (unsigned int i = 0; i < dsize-1; i++)
- {
- if (*dataptr > max) max = *dataptr;
- dataptr++;
- }
- return max;
-}
-
-
-/**************************************************************************************************************/
-/**************************************************************************************************************/
-
-
-//Specific functions
-
-/*
-input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
-
-output:: the complement matrix of the element (rowNo,colNo).
-This is the matrix obtained from M after elimenating the row rowNo and column colNo
-
-example:
-1 2 3
-M = 4 5 6
-7 8 9
-1 3
-subblock(M, 1, 1) give the matrix 7 9
-*/
-vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
-{
- vpMatrix M_comp(M.getRows()-1,M.getCols()-1);
-
- for ( unsigned int i = 0 ; i < col ; i++)
- {
- for ( unsigned int j = 0 ; j < row ; j++)
- M_comp[i][j]=M[i][j];
- for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
- M_comp[i][j-1]=M[i][j];
- }
- for ( unsigned int i = col+1 ; i < M.getCols(); i++)
- {
- for ( unsigned int j = 0 ; j < row ; j++)
- M_comp[i-1][j]=M[i][j];
- for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
- M_comp[i-1][j-1]=M[i][j];
- }
- return M_comp;
-}
-
-/*!
- \return The condition number, the ratio of the largest singular value of the matrix to the smallest.
- */
-double vpMatrix::cond()
-{
- vpMatrix v;
- vpColVector w;
-
- vpMatrix M;
- M = *this;
-
- M.svd(w, v);
- double min=w[0];
- double max=w[0];
- for(unsigned int i=0;i<M.getCols();i++)
- {
- if(min>w[i])min=w[i];
- if(max<w[i])max=w[i];
- }
- return max/min;
-}
-
-/*!
- Compute \f${\bf H} + \alpha * diag({\bf H})\f$
- \param H : input Matrix \f${\bf H}\f$. This matrix should be square.
- \param alpha : Scalar \f$\alpha\f$
- \param HLM : Resulting operation.
- */
-void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
-{
- if(H.getCols() != H.getRows())
- {
- vpTRACE("The matrix must be square");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "The matrix must be square" ));
- }
- HLM.resize(H.getRows(), H.getCols());
-
- for(unsigned int i=0;i<H.getCols();i++)
- {
- for(unsigned int j=0;j<H.getCols();j++)
- {
- HLM[i][j]=H[i][j];
- if(i==j)
- HLM[i][j]+= alpha*H[i][j];
- }
- }
-
-}
-
-#undef DEBUG_LEVEL1
-#undef DEBUG_LEVEL2
-
-/*
-* Local variables:
-* c-basic-offset: 2
-* End:
-*/
diff --git a/src/math/matrix/vpMatrix.h b/src/math/matrix/vpMatrix.h
deleted file mode 100644
index 3f3659d..0000000
--- a/src/math/matrix/vpMatrix.h
+++ /dev/null
@@ -1,592 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix.h 5238 2015-01-30 13:52:25Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix manipulation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpMatrix_H
-#define vpMatrix_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-#include <visp/vpTime.h>
-
-#ifdef VISP_HAVE_GSL
-# include <gsl/gsl_math.h>
-# include <gsl/gsl_eigen.h>
-#endif
-
-#include <iostream>
-#include <math.h>
-
-class vpRowVector;
-class vpColVector;
-class vpTranslationVector;
-
-
-class vpColVector;
-class vpTranslationVector;
-class vpRowVector;
-class vpHomography;
-
-
-
-/*!
- \file vpMatrix.h
-
- \brief Definition of matrix class as well as a set of operations on
- these matrices.
-*/
-
-
-
-/*!
- \class vpMatrix
- \ingroup Matrix
-
- \brief Definition of the vpMatrix class.
-
- vpMatrix class provides a data structure for the matrices as well
- as a set of operations on these matrices
-
- \author Eric Marchand (IRISA - INRIA Rennes)
-
- \warning Note the matrix in the class (*this) will be noted A in the comment
-
- \ingroup libmath
-
- \sa vpRowVector, vpColVector, vpHomogeneousMatrix, vpRotationMatrix,
- vpTwistMatrix, vpHomography
-*/
-class VISP_EXPORT vpMatrix
-{
- public:
- /*!
- Method used to compute the determinant of a square matrix.
- \sa det()
- */
- typedef enum {
- LU_DECOMPOSITION /*!< LU decomposition method. */
- } vpDetMethod;
-
-
-protected:
- //! number of rows
- unsigned int rowNum;
- //! number of columns
- unsigned int colNum;
-
- public:
- //! address of the first element of the data array
- double *data;
- protected:
- //! address of the first element of each rows
- double **rowPtrs;
-
- //! Current size (rowNum * colNum)
- unsigned int dsize;
- //! Total row space
- unsigned int trsize;
-
- public:
- //! Basic constructor
- vpMatrix() ;
- //! Constructor. Initialization of A as an r x c matrix with 0.
- vpMatrix(unsigned int r, unsigned int c) ;
- vpMatrix(unsigned int r, unsigned int c, double val);
- //! sub vpMatrix constructor
- vpMatrix(const vpMatrix &m, unsigned int r, unsigned int c,
- unsigned int nrows, unsigned int ncols) ;
-
- vpMatrix(const vpHomography& H);
-
- //! Destructor (Memory de-allocation)
- virtual ~vpMatrix();
-
- //! Initialization of the object matrix
- void init() ;
-
- //! Destruction of the matrix (Memory de-allocation)
- void kill() ;
-
- // Initialize an identity matrix n-by-n
- void eye(unsigned int n) ;
- // Initialize an identity matrix m-by-n
- void eye(unsigned int m, unsigned int n) ;
- void setIdentity(const double & val=1.0) ;
-
- //---------------------------------
- // Set/get Matrix size
- //---------------------------------
- /** @name Set/get Matrix size */
- //@{
- //! Return the number of rows of the matrix
- inline unsigned int getRows() const { return rowNum ;}
- //! Return the number of columns of the matrix
- inline unsigned int getCols() const { return colNum; }
- //! Return the number of elements of the matrix.
- inline unsigned int size() const { return colNum*rowNum; }
-
- // Set the size of the matrix A, initialization with a zero matrix
- void resize(const unsigned int nrows, const unsigned int ncols,
- const bool nullify = true);
-
- double getMinValue() const;
-
- double getMaxValue() const;
- //@}
-
- //---------------------------------
- // Printing
- //---------------------------------
-
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s,const vpMatrix &m);
- /** @name Printing */
- //@{
-
- int print(std::ostream& s, unsigned int length, char const* intro=0);
- std::ostream & matlabPrint(std::ostream & os) const;
- std::ostream & maplePrint(std::ostream & os) const;
- std::ostream & csvPrint(std::ostream & os) const;
- std::ostream & cppPrint(std::ostream & os, const char * matrixName = NULL, bool octet = false) const;
-
- void printSize() { std::cout << getRows() <<" x " << getCols() <<" " ; }
- //@}
-
-
- //---------------------------------
- // Copy / assignment
- //---------------------------------
- /** @name Copy / assignment */
- //@{
- //! Copy constructor
- vpMatrix (const vpMatrix& m);
-
- // Assignment from an array
- vpMatrix &operator<<(double*);
-
- //! Copy operator. Allow operation such as A = B
- vpMatrix &operator=(const vpMatrix &B);
- //! Copy operator. Allow operation such as A = H
- vpMatrix &operator=(const vpHomography &H);
- //! Set all the element of the matrix A to x
- vpMatrix &operator=(const double x);
- void diag(const vpColVector &A);
- //@}
-
- //---------------------------------
- // Access/modification operators
- //---------------------------------
- /** @name Access/modification operators */
- //@{
- //! write elements Aij (usage : A[i][j] = x )
- inline double *operator[](unsigned int i) { return rowPtrs[i]; }
- //! read elements Aij (usage : x = A[i][j] )
- inline double *operator[](unsigned int i) const {return rowPtrs[i];}
- //@}
-
- //---------------------------------
- // Matrix operations (Static).
- //---------------------------------
-
- static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
- static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C);
- static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b);
- static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &w);
- static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM);
-
- // Create a diagonal matrix with the element of a vector DAii = Ai
- static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA) ;
- // Insert matrix A in the current matrix at the given position (r, c).
- void insert(const vpMatrix&A, const unsigned int r, const unsigned int c);
- // Insert matrix B in matrix A at the given position (r, c).
- static vpMatrix insert(const vpMatrix &A,const vpMatrix &B, const unsigned int r, const unsigned int c) ;
- // Insert matrix B in matrix A (not modified) at the given position (r, c), the result is given in matrix C.
- static void insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c) ;
-
- // Juxtapose to matrices C = [ A B ]
- static vpMatrix juxtaposeMatrices(const vpMatrix &A,const vpMatrix &B) ;
- // Juxtapose to matrices C = [ A B ]
- static void juxtaposeMatrices(const vpMatrix &A,const vpMatrix &B, vpMatrix &C) ;
- // Compute Kronecker product matrix
- static void kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out);
-
- // Compute Kronecker product matrix
- static vpMatrix kron(const vpMatrix &m1, const vpMatrix &m2 );
-
- /*!
- Load a matrix from a file.
-
- \param filename : absolute file name
- \param M : matrix to be loaded
- \param binary :If true the matrix is loaded from a binary file, else from a text file.
- \param Header : Header of the file is loaded in this parameter
-
- \return Returns true if no problem appends.
- */
- static inline bool loadMatrix(std::string filename, vpMatrix &M,
- const bool binary = false, char *Header = NULL)
- {
- return vpMatrix::loadMatrix(filename.c_str(), M, binary, Header);
- }
- static bool loadMatrix(const char *filename, vpMatrix &M, const bool binary = false, char *Header = NULL);
- static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
- static void multMatrixVector(const vpMatrix &A, const vpColVector &b, vpColVector &c);
- static void negateMatrix(const vpMatrix &A, vpMatrix &C);
- /*!
- Save a matrix to a file.
-
- \param filename : absolute file name
- \param M : matrix to be saved
- \param binary :If true the matrix is save in a binary file, else a text file.
- \param Header : optional line that will be saved at the beginning of the file
-
- \return Returns true if no problem appends.
-
- Warning : If you save the matrix as in a text file the precision is less than if you save it in a binary file.
- */
- static inline bool saveMatrix(std::string filename, const vpMatrix &M,
- const bool binary = false,
- const char *Header = "")
- {
- return vpMatrix::saveMatrix(filename.c_str(), M, binary, Header);
- }
- static bool saveMatrix(const char *filename, const vpMatrix &M, const bool binary = false, const char *Header = "");
-
- /*!
- Save a matrix in a YAML-formatted file.
-
- \param filename : absolute file name.
- \param M : matrix to be saved in the file.
- \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
-
- \return Returns true if success.
-
- */
- static inline bool saveMatrixYAML(std::string filename, const vpMatrix &M, const char *header = "")
- {
- return vpMatrix::saveMatrixYAML(filename.c_str(), M, header);
- }
- static bool saveMatrixYAML(const char *filename, const vpMatrix &M, const char *header = "");
- /*!
- Load a matrix from a YAML-formatted file.
-
- \param filename : absolute file name.
- \param M : matrix to be loaded from the file.
- \param header : Header of the file is loaded in this parameter.
-
- \return Returns true if no problem appends.
- */
- static inline bool loadMatrixYAML(std::string filename, vpMatrix &M, char *header = NULL)
- {
- return vpMatrix::loadMatrixYAML(filename.c_str(), M, header);
- }
- static bool loadMatrixYAML(const char *filename, vpMatrix &M, char *header = NULL);
-
-
- // Stack the matrix A below the current one, copy if not initialized this = [ this A ]^T
- void stackMatrices(const vpMatrix &A);
- //! Stack two Matrices C = [ A B ]^T
- static vpMatrix stackMatrices(const vpMatrix &A,const vpMatrix &B) ;
- //! Stack two Matrices C = [ A B ]^T
- static void stackMatrices(const vpMatrix &A,const vpMatrix &B, vpMatrix &C) ;
- static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
-
-
- //---------------------------------
- // Matrix operations.
- //---------------------------------
- /** @name Matrix operations */
- //@{
- // operation A = A + B
- vpMatrix &operator+=(const vpMatrix &B);
- // operation A = A - B
- vpMatrix &operator-=(const vpMatrix &B);
-
- vpMatrix operator*(const vpMatrix &B) const;
- vpMatrix operator*(const vpHomography &H) const;
- vpMatrix operator+(const vpMatrix &B) const;
- vpMatrix operator-(const vpMatrix &B) const;
- vpMatrix operator-() const;
-
- //---------------------------------
- // Matrix/vector operations.
- //---------------------------------
-
- vpColVector operator*(const vpColVector &b) const;
- // operation c = A * b (A is unchanged, c and b are translation vectors)
- vpTranslationVector operator*(const vpTranslationVector &b) const;
- //---------------------------------
- // Matrix/real operations.
- //---------------------------------
-
- //! Add x to all the element of the matrix : Aij = Aij + x
- vpMatrix &operator+=(const double x);
- //! Substract x to all the element of the matrix : Aij = Aij - x
- vpMatrix &operator-=(const double x);
- //! Multiply all the element of the matrix by x : Aij = Aij * x
- vpMatrix &operator*=(const double x);
- //! Divide all the element of the matrix by x : Aij = Aij / x
- vpMatrix &operator/=(double x);
-
- // Cij = Aij * x (A is unchanged)
- vpMatrix operator*(const double x) const;
- // Cij = Aij / x (A is unchanged)
- vpMatrix operator/(const double x) const;
-
- /*!
- Return the sum of all the \f$a_{ij}\f$ elements of the matrix.
-
- \return \f$\sum a_{ij}\f$
- */
- double sum() const;
- /*!
- Return the sum square of all the \f$a_{ij}\f$ elements of the matrix.
-
- \return \f$\sum a_{ij}^{2}\f$
- */
- double sumSquare() const;
-
- // return the determinant of the matrix.
- double det(vpDetMethod method = LU_DECOMPOSITION) const;
-
- //Compute the exponential matrix of a square matrix
- vpMatrix expm();
-
- //-------------------------------------------------
- // Columns, Rows extraction, SubMatrix
- //-------------------------------------------------
- /** @name Columns, Rows extraction, Submatrix */
- //@{
- vpRowVector getRow(const unsigned int i) const;
- vpRowVector getRow(const unsigned int i, const unsigned int j_begin, const unsigned int size) const;
- vpColVector getCol(const unsigned int j) const;
- vpColVector getCol(const unsigned int j, const unsigned int i_begin, const unsigned int size) const;
- void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols);
- //@}
-
- //-------------------------------------------------
- // transpose
- //-------------------------------------------------
- /** @name Transpose, Identity */
- //@{
- // Compute the transpose C = A^T
- vpMatrix t() const;
-
- // Compute the transpose C = A^T
- vpMatrix transpose()const;
- void transpose(vpMatrix & C )const;
-
- vpMatrix AAt() const;
- void AAt(vpMatrix &B) const;
-
- vpMatrix AtA() const;
- void AtA(vpMatrix &B) const;
- //@}
-
-
- //-------------------------------------------------
- // Kronecker product
- //-------------------------------------------------
- /** @name Kronecker product */
- //@{
-
- // Stacks columns of a matrix in a vector
- void stackColumns(vpColVector &out );
-
- // Stacks columns of a matrix in a vector
- vpColVector stackColumns();
-
- // Stacks columns of a matrix in a vector
- void stackRows(vpRowVector &out );
-
- // Stacks columns of a matrix in a vector
- vpRowVector stackRows();
-
- // Compute Kronecker product matrix
- void kron(const vpMatrix &m1 , vpMatrix &out);
-
- // Compute Kronecker product matrix
- vpMatrix kron(const vpMatrix &m1);
- //@}
-
- //-------------------------------------------------
- // Matrix inversion
- //-------------------------------------------------
- /** @name Matrix inversion */
- //@{
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- //! LU Decomposition
- void LUDcmp(unsigned int* perm, int& d);
- //! solve AX = B using the LU Decomposition
- void LUBksb(unsigned int* perm, vpColVector& b);
-
-#endif // doxygen should skip this
- // inverse matrix A using the LU decomposition
- vpMatrix inverseByLU() const;
-#if defined(VISP_HAVE_LAPACK)
- // inverse matrix A using the Cholesky decomposition (only for real symmetric matrices)
- vpMatrix inverseByCholesky() const;
- //lapack implementation of inverse by Cholesky
- vpMatrix inverseByCholeskyLapack() const;
- // inverse matrix A using the QR decomposition
- vpMatrix inverseByQR() const;
- //lapack implementation of inverse by QR
- vpMatrix inverseByQRLapack() const;
-#endif
- //! Compute the pseudo inverse of the matrix using the SVD.
- vpMatrix pseudoInverse(double svThreshold=1e-6) const;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank
- unsigned int pseudoInverse(vpMatrix &Ap, double svThreshold=1e-6) const;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value
- unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold=1e-6) const ;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value, image
- unsigned int pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &ImA,
- vpMatrix &ImAt) const ;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value, image, kernel.
- unsigned int pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &ImA,
- vpMatrix &ImAt,
- vpMatrix &kerA) const ;
- //@}
-
- //-------------------------------------------------
- // SVD decomposition
- //-------------------------------------------------
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- void svdFlake(vpColVector& w, vpMatrix& v);
- void svdNr(vpColVector& w, vpMatrix& v);
-#ifdef VISP_HAVE_GSL
- void svdGsl(vpColVector& w, vpMatrix& v);
-#endif
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
- void svdOpenCV(vpColVector& w, vpMatrix& v);
-#endif
-#ifdef VISP_HAVE_LAPACK
- void svdLapack(vpColVector& W, vpMatrix& V);
-#endif
- //! solve AX=B using the SVD decomposition
- void SVBksb(const vpColVector& w,
- const vpMatrix& v,
- const vpColVector& b, vpColVector& x);
-#endif
-
- /** @name SVD decomposition */
- //@{
- // singular value decomposition SVD
-
- void svd(vpColVector& w, vpMatrix& v);
-
- // solve Ax=B using the SVD decomposition (usage A = solveBySVD(B,x) )
- void solveBySVD(const vpColVector &B, vpColVector &x) const ;
- // solve Ax=B using the SVD decomposition (usage x=A.solveBySVD(B))
- vpColVector solveBySVD(const vpColVector &B) const ;
-
- unsigned int kernel(vpMatrix &KerA, double svThreshold=1e-6);
- double cond();
- //@}
-
- //-------------------------------------------------
- // Eigen values and vectors
- //-------------------------------------------------
-
- /** @name Eigen values */
-
- //@{
- // compute the eigen values using the Gnu Scientific library
- vpColVector eigenValues();
- void eigenValues(vpColVector &evalue, vpMatrix &evector);
- //@}
-
- // -------------------------
- // Norms
- // -------------------------
- /** @name Norms */
- //@{
- // Euclidean norm ||x||=sqrt(sum(x_i^2))
- double euclideanNorm () const;
- // Infinity norm ||x||=max(sum(fabs(x_i)))
- double infinityNorm () const;
- //@}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated vpRowVector row(const unsigned int i);
- vp_deprecated vpColVector column(const unsigned int j);
-#endif
-
- private:
- double detByLU() const;
-
-};
-
-
-//////////////////////////////////////////////////////////////////////////
-
-
-//////////////////////////////////////////////////////////////////////////
-
-
-//! multiplication by a scalar C = x*A
-VISP_EXPORT vpMatrix operator*(const double &x, const vpMatrix &A) ;
-
- //! multiplication by a scalar C = x*A
-VISP_EXPORT vpColVector operator*(const double &x, const vpColVector &A) ;
-
-
-
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/matrix/vpMatrixException.h b/src/math/matrix/vpMatrixException.h
deleted file mode 100644
index 2d23f34..0000000
--- a/src/math/matrix/vpMatrixException.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrixException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpMatrix class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef __vpMatrixException_H
-#define __vpMatrixException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* Classes standards. */
-//
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpMatrixException
- \ingroup Exception
- \brief error that can be emited by the vpMatrix class and its derivates
- */
-class VISP_EXPORT vpMatrixException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpMatrix member
- */
- enum errorCodeEnum
- {
- //! error returns by a constructor
- constructionError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedError,
- //! index out of range
- outOfRangeError,
- //! iterative algorithm doesn't converge (ex SVD)
- convergencyError,
- incorrectMatrixSizeError,
- forbiddenOperatorError,
- subMatrixError,
- matrixError,
- rankDeficient
- } ;
-
- public:
- vpMatrixException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpMatrixException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpMatrixException (const int id)
- : vpException(id){ ; }
- // vpMatrixException() : vpException() { ;}
-};
-
-#endif
diff --git a/src/math/matrix/vpMatrix_cholesky.cpp b/src/math/matrix/vpMatrix_cholesky.cpp
deleted file mode 100644
index ec4af3f..0000000
--- a/src/math/matrix/vpMatrix_cholesky.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix_lu.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix Cholesky decomposition.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-#ifdef VISP_HAVE_LAPACK
-extern "C" void dpotrf_ (char *uplo, int *n, double *a, int *lda, int *info);
-extern "C" int dpotri_(char *uplo, int *n, double *a, int *lda, int *info);
-
-#endif
-
-#ifdef VISP_HAVE_LAPACK
-vpMatrix vpMatrix::inverseByCholeskyLapack() const{
- int rowNum_ = (int)this->getRows();
- int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
- int info;
-
- vpMatrix A = *this;
- dpotrf_((char*)"L",&rowNum_,A.data,&lda,&info);
-
- if(info!=0)
- std::cout << "cholesky:dpotrf_:error" << std::endl;
-
- dpotri_((char*)"L",&rowNum_,A.data,&lda,&info);
- if(info!=0){
- std::cout << "cholesky:dpotri_:error" << std::endl;
- throw vpMatrixException::badValue;
- }
-
- for(unsigned int i=0;i<A.getRows();i++)
- for(unsigned int j=0;j<A.getCols();j++)
- if(i>j) A[i][j] = A[j][i];
-
- return A;
-}
-#endif
-
-/*!
- Compute the inverse of a n-by-n matrix using the Cholesky decomposition.
- The matrix must be real and symmetric.
- Only available if lapack is installed.
-
- \return The inverse matrix.
-
- Here an example:
- \code
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- // Symmetric matrix
- A[0][0] = 1/1.; A[0][1] = 1/5.; A[0][2] = 1/6.; A[0][3] = 1/7.;
- A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
- A[2][0] = 1/6.; A[2][1] = 1/3.; A[2][2] = 1/2.; A[2][3] = 1/6.;
- A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
-
- // Compute the inverse
- vpMatrix A_1; // A^-1
- A_1 = A.inverseByCholesky();
- std::cout << "Inverse by Cholesky: \n" << A_1 << std::endl;
-
- std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
-}
- \endcode
-
- \sa pseudoInverse()
-*/
-
-#if defined(VISP_HAVE_LAPACK)
-vpMatrix
-vpMatrix::inverseByCholesky() const
-{
-
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
- }
-#ifdef VISP_HAVE_LAPACK
- return inverseByCholeskyLapack();
-#endif
-}
-
-#endif
diff --git a/src/math/matrix/vpMatrix_covariance.cpp b/src/math/matrix/vpMatrix_covariance.cpp
deleted file mode 100644
index c849010..0000000
--- a/src/math/matrix/vpMatrix_covariance.cpp
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix_lu.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at inria.fr
- *
- * This file is provided AS IS with NO WARRANTb OF ANb KIND, INCLUDING THE
- * WARRANTb OF DESIGN, MERCHANTABILITb AND FITNESS FOR A PARTICULAR PURPOSE.
- *
- *
- * Description:
- * Covariance matrix computation.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <limits> // numeric_limits
-#include <cmath> // std::fabs()
-
-#include <visp/vpConfig.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMatrixException.h>
-
-
-/*!
- Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
- Ax = b
-
- \param A : Matrix A from Ax = b.
-
- \param x : Vector x from Ax = b corresponding to the parameters to estimate.
-
- \param b : Vector b from Ax = b.
-*/
-vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
-{
-// double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To consider OLS Estimate for sigma
- double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma
-
- if(denom <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
-
-// double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be equivalent to line bellow.
- double sigma2 = (b - (A * x)).t() * (b - (A * x));
-
- sigma2 /= denom;
-
- return (A.t()*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
-}
-
-/*!
- Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
- WAx = Wb
-
- \param A : Matrix A from WAx = Wb.
-
- \param x : Vector x from WAx = Wb corresponding to the parameters to estimate.
-
- \param b : Vector b from WAx = Wb.
-
- \param W : Diagonal weigths matrix from WAx = Wb.
-*/
-vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &W)
-{
- double denom = 0.0;
- vpMatrix W2(W.getCols(),W.getCols());
- for(unsigned int i = 0 ; i < W.getCols() ; i++){
- denom += W[i][i];
- W2[i][i] = W[i][i]*W[i][i];
- }
-
- if(denom <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
-
-// double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should be equivalent to line bellow.
- double sigma2 = (W * b - (W * A * x)).t() * (W*b - (W * A * x));
- sigma2 /= denom;
-
- return (A.t()*(W2)*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
-}
diff --git a/src/math/matrix/vpMatrix_lu.cpp b/src/math/matrix/vpMatrix_lu.cpp
deleted file mode 100644
index 18be650..0000000
--- a/src/math/matrix/vpMatrix_lu.cpp
+++ /dev/null
@@ -1,282 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix_lu.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix LU decomposition.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#define TINY 1.0e-20;
-
-
-/*--------------------------------------------------------------------
- LU Decomposition related functions
--------------------------------------------------------------------- */
-
-/*!
- \brief Performed the LU decomposition
-
- Given a matrix A (n x n), this routine replaces it by the LU decomposition of
- a rowwise permutation of itself. A is output, arranged as in equation
- (2.3.14) of the NRC ; perm (n) is an output vector that records the row
- permutation effected by the partial pivoting; d is output as 1 depending on
- whether the number of row interchanges was even or odd, respectively.
-
- \warning Destructive wrt. A
-
- \sa This routine is used in combination with LUDksb to solve linear equations
- or invert a matrix.
-
- This function is extracted from the NRC
-
- */
-
-void
-vpMatrix::LUDcmp(unsigned int *perm, int& d)
-{
- unsigned int n = rowNum;
-
- unsigned int i,imax=0,j,k;
- double big,dum,sum_,temp;
- vpColVector vv(n);
-
- d=1;
- for (i=0;i<n;i++) {
- big=0.0;
- for (j=0;j<n;j++)
- if ((temp=fabs(rowPtrs[i][j])) > big) big=temp;
- //if (big == 0.0)
- if (std::fabs(big) <= std::numeric_limits<double>::epsilon())
- {
- //vpERROR_TRACE("Singular vpMatrix in LUDcmp") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Singular vpMatrix in LUDcmp")) ;
- }
- vv[i]=1.0/big;
- }
- for (j=0;j<n;j++) {
- for (i=0;i<j;i++) {
- sum_=rowPtrs[i][j];
- for (k=0;k<i;k++) sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
- rowPtrs[i][j]=sum_;
- }
- big=0.0;
- for (i=j;i<n;i++) {
- sum_=rowPtrs[i][j];
- for (k=0;k<j;k++)
- sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
- rowPtrs[i][j]=sum_;
- if ( (dum=vv[i]*fabs(sum_)) >= big) {
- big=dum;
- imax=i;
- }
- }
- if (j != imax) {
- for (k=0;k<n;k++) {
- dum=rowPtrs[imax][k];
- rowPtrs[imax][k]=rowPtrs[j][k];
- rowPtrs[j][k]=dum;
- }
- d *= -1;
- vv[imax]=vv[j];
- }
- perm[j]=imax;
- //if (rowPtrs[j][j] == 0.0)
- if (std::fabs(rowPtrs[j][j]) <= std::numeric_limits<double>::epsilon())
- rowPtrs[j][j]=TINY;
- if (j != n) {
- dum=1.0/(rowPtrs[j][j]);
- for (i=j+1;i<n;i++) rowPtrs[i][j] *= dum;
- }
- }
-}
-
-#undef TINY
-
-/*!
- \brief Solve linear system AX = B using LU decomposition
-
- Solves the set of n linear equations AX = B. Here A (n x n) is input, not
- as the matrix A but rather as its LU decomposition, determined by the routine
- ludcmp. perm (n) is input as the permutation vector returned by
- ludcmp. b (n) is input as the right-hand side vector B, and returns with
- the solution vector X. A and perm are not modified by this routine and can
- be left in place for successive calls with different right-hand sides b. This
- routine takes into account the possibility that b will begin with many zero
- elements, so it is efficient for use in matrix inversion.
-
- \sa This function must be used with LUDcmp
-
- \sa LUDsolve and solveByLUD are more intuitive and direct to use
-
- This function is extracted from the NRC
-
-*/
-void vpMatrix::LUBksb(unsigned int *perm, vpColVector& b)
-{
- unsigned int n = rowNum;
-
- unsigned int ii=0;
- unsigned int ip;
- double sum_;
- bool flag = false;
- unsigned int i;
-
- for (i=0;i<n;i++) {
- ip=perm[i];
- sum_=b[ip];
- b[ip]=b[i];
- if (flag) {
- for (unsigned int j=ii;j<=i-1;j++) sum_ -= rowPtrs[i][j]*b[j];
- }
- //else if (sum_) {
- else if (std::fabs(sum_) > std::numeric_limits<double>::epsilon()) {
- ii=i;
- flag = true;
- }
- b[i]=sum_;
- }
- // for (int i=n-1;i>=0;i--) {
- // sum_=b[i];
- // for (int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
- // b[i]=sum_/rowPtrs[i][i];
- // }
- i=n;
- do {
- i --;
-
- sum_=b[i];
- for (unsigned int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
- b[i]=sum_/rowPtrs[i][i];
- } while(i != 0);
-}
-#endif // doxygen should skip this
-
-/*!
- Compute the inverse of a n-by-n matrix using the LU decomposition.
-
- \return The inverse matrix.
-
- Here an example:
- \code
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
- A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
- A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
- A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
-
- // Compute the inverse
- vpMatrix A_1; // A^-1
- A_1 = A.inverseByLU();
- std::cout << "Inverse by LU: \n" << A_1 << std::endl;
-
- std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
-}
- \endcode
-
- \sa pseudoInverse()
-*/
-vpMatrix
-vpMatrix::inverseByLU() const
-{
- unsigned int i,j;
-
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
- }
-
- vpMatrix B(rowNum, rowNum), X(rowNum, rowNum);
- vpMatrix V(rowNum, rowNum);
- vpColVector W(rowNum);
-
- for (i=0; i<rowNum; i++) {
- for (j=0; j<rowNum; j++) {
- B[i][j] = (i == j) ? 1 : 0;
- }
- }
-
- vpMatrix A(rowNum, rowNum);
- A = *this;
-
- unsigned int *perm = new unsigned int[rowNum];
- int p;
-
- try {
- A.LUDcmp(perm, p);
- }
- catch(vpException e) {
- delete [] perm;
- throw(e);
- }
-
- vpColVector c_tmp(rowNum) ;
- for (j=1; j<=rowNum; j++)
- {
- c_tmp =0 ; c_tmp[j-1] = 1 ;
- A.LUBksb(perm, c_tmp);
- for (unsigned int k=0 ; k < c_tmp.getRows() ; k++)
- B[k][j-1] = c_tmp[k] ;
- }
- delete [] perm;
- return B;
-}
-
diff --git a/src/math/matrix/vpMatrix_qr.cpp b/src/math/matrix/vpMatrix_qr.cpp
deleted file mode 100644
index e56d029..0000000
--- a/src/math/matrix/vpMatrix_qr.cpp
+++ /dev/null
@@ -1,242 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix_lu.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix QR decomposition.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <algorithm> // for std::min and std::max
-#include <visp/vpConfig.h>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-int allocate_work(double** work);
-
-#ifdef VISP_HAVE_LAPACK
-extern "C" int dgeqrf_(int *m, int *n, double*a, int *lda, double *tau, double *work, int *lwork, int *info);
-extern "C" int dormqr_(char *side, char *trans, int *m, int *n,
- int *k, double *a, int *lda, double *tau, double *c__,
- int *ldc, double *work, int *lwork, int *info);
-extern "C" int dorgqr_(int *, int *, int *, double *, int *,
- double *, double *, int *, int *);
-extern "C" int dtrtri_(char *uplo, char *diag, int *n, double *a, int *lda, int *info);
-#endif
-
-int allocate_work(double** work)
-{
- unsigned int dimWork = (unsigned int)((*work)[0]);
- delete[] *work;
- *work = new double[dimWork];
- return (int)dimWork;
-}
-#ifdef VISP_HAVE_LAPACK
-vpMatrix vpMatrix::inverseByQRLapack() const{
- int rowNum_ = (int)this->getRows();
- int colNum_ = (int)this->getCols();
- int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
- int dimTau = std::min(rowNum_,colNum_);
- int dimWork = -1;
- double *tau = new double[dimTau];
- double *work = new double[1];
- int info;
- vpMatrix C;
- vpMatrix A = *this;
-
- try{
- //1) Extract householder reflections (useful to compute Q) and R
- dgeqrf_(
- &rowNum_, //The number of rows of the matrix A. M >= 0.
- &colNum_, //The number of columns of the matrix A. N >= 0.
- A.data, /*On entry, the M-by-N matrix A.
- On exit, the elements on and above the diagonal of the array
- contain the min(M,N)-by-N upper trapezoidal matrix R (R is
- upper triangular if m >= n); the elements below the diagonal,
- with the array TAU, represent the orthogonal matrix Q as a
- product of min(m,n) elementary reflectors.
- */
- &lda, //The leading dimension of the array A. LDA >= max(1,M).
- tau, /*Dimension (min(M,N))
- The scalar factors of the elementary reflectors
- */
- work, //Internal working array. dimension (MAX(1,LWORK))
- &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
- &info //status
- );
-
- if(info != 0){
- std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
- throw vpMatrixException::badValue;
- }
- dimWork = allocate_work(&work);
-
- dgeqrf_(
- &rowNum_, //The number of rows of the matrix A. M >= 0.
- &colNum_, //The number of columns of the matrix A. N >= 0.
- A.data, /*On entry, the M-by-N matrix A.
- On exit, the elements on and above the diagonal of the array
- contain the min(M,N)-by-N upper trapezoidal matrix R (R is
- upper triangular if m >= n); the elements below the diagonal,
- with the array TAU, represent the orthogonal matrix Q as a
- product of min(m,n) elementary reflectors.
- */
- &lda, //The leading dimension of the array A. LDA >= max(1,M).
- tau, /*Dimension (min(M,N))
- The scalar factors of the elementary reflectors
- */
- work, //Internal working array. dimension (MAX(1,LWORK))
- &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
- &info //status
- );
-
-
- if(info != 0){
- std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
- throw vpMatrixException::badValue;
- }
-
- //A now contains the R matrix in its upper triangular (in lapack convention)
- C = A;
-
- //2) Invert R
- dtrtri_((char*)"U",(char*)"N",&dimTau,C.data,&lda,&info);
- if(info!=0){
- if(info < 0)
- std::cout << "dtrtri_:"<< -info << "th element had an illegal value" << std::endl;
- else if(info > 0){
- std::cout << "dtrtri_:R("<< info << "," <<info << ")"<< " is exactly zero. The triangular matrix is singular and its inverse can not be computed." << std::endl;
- std::cout << "R=" << std::endl << C << std::endl;
- }
- throw vpMatrixException::badValue;
- }
-
- //3) Zero-fill R^-1
- //the matrix is upper triangular for lapack but lower triangular for visp
- //we fill it with zeros above the diagonal (where we don't need the values)
- for(unsigned int i=0;i<C.getRows();i++)
- for(unsigned int j=0;j<C.getRows();j++)
- if(j>i) C[i][j] = 0.;
-
- dimWork = -1;
- int ldc = lda;
-
- //4) Transpose Q and left-multiply it by R^-1
- //get R^-1*tQ
- //C contains R^-1
- //A contains Q
- dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
- if(info != 0){
- std::cout << "dormqr_:Preparation"<< -info << "th element had an illegal value" << std::endl;
- throw vpMatrixException::badValue;
- }
- dimWork = allocate_work(&work);
-
- dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
-
- if(info != 0){
- std::cout << "dormqr_:"<< -info << "th element had an illegal value" << std::endl;
- throw vpMatrixException::badValue;
- }
- delete[] tau;
- delete[] work;
- }catch(vpMatrixException&){
- delete[] tau;
- delete[] work;
- throw;
- }
-
- return C;
-
-}
-#endif
-
-/*!
- Compute the inverse of a n-by-n matrix using the QR decomposition.
- Only available if lapack is installed.
-
- \return The inverse matrix.
-
- Here an example:
- \code
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpMatrix A(4,4);
-
- A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
- A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
- A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
- A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
-
- // Compute the inverse
- vpMatrix A_1; // A^-1
- A_1 = A.inverseByQR();
- std::cout << "Inverse by QR: \n" << A_1 << std::endl;
-
- std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
-}
- \endcode
-
- \sa pseudoInverse()
-*/
-
-#if defined(VISP_HAVE_LAPACK)
-vpMatrix
-vpMatrix::inverseByQR() const
-{
-
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
- }
-#ifdef VISP_HAVE_LAPACK
- return inverseByQRLapack();
-#endif
-}
-
-#endif
diff --git a/src/math/matrix/vpMatrix_svd.cpp b/src/math/matrix/vpMatrix_svd.cpp
deleted file mode 100644
index 97ff0d6..0000000
--- a/src/math/matrix/vpMatrix_svd.cpp
+++ /dev/null
@@ -1,717 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMatrix_svd.cpp 4896 2014-09-10 16:41:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Matrix SVD decomposition.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <iostream>
-
-/*---------------------------------------------------------------------
-
-SVD related functions
-
----------------------------------------------------------------------*/
-
-
-static double pythag(double a, double b)
-{
- double absa, absb;
- absa = fabs(a);
- absb = fabs(b);
- if (absa > absb) return absa*sqrt(1.0+vpMath::sqr(absb/absa));
- //else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
- else return (std::fabs(absb) <= std::numeric_limits<double>::epsilon() ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
-}
-
-#ifdef SIGN
-#undef SIGN
-#endif
-#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
-
-/*!
- \brief Singular value decomposition
-
- Given a matrix A (m x n) this routine compute its sngular value decomposition
- A = U W V^T. The matrice U replace A on output. the diagonal matrix of
- singular value is output as a vector W (n). The matrix V (not the transpose
- V^T) is output as V (n x n)
-
-
- \warning Destructive wrt A
- \warning
-
-
- \sa SVD for a more intuitive use
-
-
- This function is extracted from the NRC
-
-*/
-
-#define MAX_ITER_SVD 50
-void vpMatrix::svdNr(vpColVector& W, vpMatrix& V)
-{
-
- unsigned int m = rowNum;
- unsigned int n = colNum;
- double epsilon = 10*std::numeric_limits<double>::epsilon();
-
- unsigned int flag,i,its,j,jj,k,l=0,nm=0;
- double c,f,h,s,x,y,z;
- double anorm=0.0,g=0.0,scale=0.0;
-
- // So that the original NRC code (using 1..n indexing) can be used
- // This should be considered as a temporary fix.
- double **a = new double*[m+1];
- double **v = new double*[n+1];
- // double **w = W.rowPtrs;
- // w--;
-
- double *w = new double[n+1];
- for (i=0;i<n;i++) w[i+1] = 0.0;
-
- for (i=1;i<=m;i++) {
- a[i] = this->rowPtrs[i-1]-1;
- }
- for (i=1;i<=n;i++) {
- v[i] = V.rowPtrs[i-1]-1;
- }
-
- if (m < n)
- {
- delete[] w;
- delete[] a;
- delete[] v;
- vpERROR_TRACE("\n\t\tSVDcmp: You must augment A with extra zero rows") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tSVDcmp: You must augment A with "
- "extra zero rows")) ;
- }
- double* rv1=new double[n+1];
-
- for (i=1;i<=n;i++) {
- l=i+1;
- rv1[i]=scale*g;
- g=s=scale=0.0;
- if (i <= m) {
- for (k=i;k<=m;k++) scale += fabs(a[k][i]);
- //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
- if ((std::fabs(scale) > epsilon)/* || (fabs(scale) > EPS_SVD)*/) {
- for (k=i;k<=m;k++) {
- a[k][i] /= scale;
- s += a[k][i]*a[k][i];
- }
- f=a[i][i];
- g = -SIGN(sqrt(s),f);
- h=f*g-s;
- a[i][i]=f-g;
- if (i != n) {
- for (j=l;j<=n;j++) {
- for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
- f=s/h;
- for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
- }
- }
- for (k=i;k<=m;k++) a[k][i] *= scale;
- }
- }
- w[i]=scale*g;
- g=s=scale=0.0;
- if (i <= m && i != n) {
- for (k=l;k<=n;k++) scale += fabs(a[i][k]);
- //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
- if ((std::fabs(scale) > epsilon) /*|| (fabs(scale) > EPS_SVD)*/) {
- for (k=l;k<=n;k++) {
- a[i][k] /= scale;
- s += a[i][k]*a[i][k];
- }
- f=a[i][l];
- g = -SIGN(sqrt(s),f);
- h=f*g-s;
- a[i][l]=f-g;
- for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
- if (i != m) {
- for (j=l;j<=m;j++) {
- for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
- for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
- }
- }
- for (k=l;k<=n;k++) a[i][k] *= scale;
- }
- }
- anorm=vpMath::maximum(anorm,(fabs(w[i])+fabs(rv1[i])));
- }
- for (i=n;i>=1;i--) {
- if (i < n) {
- //if ((g) || (fabs(g) > EPS_SVD)) {
- if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
- for (j=l;j<=n;j++)
- v[j][i]=(a[i][j]/a[i][l])/g;
- for (j=l;j<=n;j++) {
- for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
- for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
- }
- }
- for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
- }
- v[i][i]=1.0;
- g=rv1[i];
- l=i;
- }
- for (i=n;i>=1;i--) {
- l=i+1;
- g=w[i];
- if (i < n)
- for (j=l;j<=n;j++) a[i][j]=0.0;
- //if ((g != 0.0) || (fabs(g) > EPS_SVD)) {
- if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
- g=1.0/g;
- if (i != n) {
- for (j=l;j<=n;j++) {
- for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
- f=(s/a[i][i])*g;
- for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
- }
- }
- for (j=i;j<=m;j++) a[j][i] *= g;
- } else {
- for (j=i;j<=m;j++) a[j][i]=0.0;
- }
- ++a[i][i];
- }
- for (k=n;k>=1;k--) {
- for (its=1;its<=MAX_ITER_SVD;its++) {
- flag=1;
- for (l=k;l>=1;l--) {
- nm=l-1;
- //if ((fabs(rv1[l])+anorm == anorm) || (fabs(rv1[l]) <= EPS_SVD)) {
- if ((std::fabs(rv1[l]) <= epsilon) /*|| (fabs(rv1[l]) <= EPS_SVD)*/) {
- flag=0;
- break;
- }
- //if ((fabs(w[nm])+anorm == anorm) || (fabs(w[nm]) <= EPS_SVD)) break;
- if ((std::fabs(w[nm]) <= epsilon) /*|| (fabs(w[nm]) <= EPS_SVD)*/) break;
- }
- if (flag) {
- c=0.0;
- s=1.0;
- for (i=l;i<=k;i++) {
- f=s*rv1[i];
- //if ((fabs(f)+anorm != anorm) || (fabs(f) <= EPS_SVD)) {
- if ((std::fabs(f) > epsilon) /*|| (fabs(f) <= EPS_SVD)*/) {
- g=w[i];
- h=pythag(f,g);
- w[i]=h;
- h=1.0/h;
- c=g*h;
- s=(-f*h);
- for (j=1;j<=m;j++) {
- y=a[j][nm];
- z=a[j][i];
- a[j][nm]=y*c+z*s;
- a[j][i]=z*c-y*s;
- }
- }
- }
- }
- z=w[k];
- if (l == k) {
- if (z < 0.0) {
- w[k] = -z;
- for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
- }
- break;
- }
- if (its == MAX_ITER_SVD)
- {
- for (i=0;i<n;i++) W[i] = w[i+1];
-
- vpERROR_TRACE("\n\t\t No convergence in SVDcmp ") ;
- std::cout << *this <<std::endl ;
- // throw(vpMatrixException(vpMatrixException::matrixError,
- // "\n\t\t No convergence in SVDcmp ")) ;
- }
- x=w[l];
- nm=k-1;
- y=w[nm];
- g=rv1[nm];
- h=rv1[k];
- f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
- g=pythag(f,1.0);
- f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
- c=s=1.0;
- for (j=l;j<=nm;j++) {
- i=j+1;
- g=rv1[i];
- y=w[i];
- h=s*g;
- g=c*g;
- z=pythag(f,h);
- rv1[j]=z;
- if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
- c=f/z;
- s=h/z;
- }
- f=x*c+g*s;
- g=g*c-x*s;
- h=y*s;
- y=y*c;
- for (jj=1;jj<=n;jj++) {
- x=v[jj][j];
- z=v[jj][i];
- v[jj][j]=x*c+z*s;
- v[jj][i]=z*c-x*s;
- }
- z=pythag(f,h);
- w[j]=z;
- //if ((z != 0.0) || (fabs(z) > EPS_SVD)) {
- if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
- z=1.0/z;
- c=f*z;
- s=h*z;
- }
- f=(c*g)+(s*y);
- x=(c*y)-(s*g);
- for (jj=1;jj<=m;jj++) {
- y=a[jj][j];
- z=a[jj][i];
- a[jj][j]=y*c+z*s;
- a[jj][i]=z*c-y*s;
- }
- }
- rv1[l]=0.0;
- rv1[k]=f;
- w[k]=x;
- }
- }
- for (i=0;i<n;i++) W[i] = w[i+1];
-
-
- delete[] w;
- delete[] rv1;
- delete[] a;
- delete[] v;
-
-}
-
-#undef SIGN
-#undef PYTHAG
-
-/*!
- \brief solve a linear system AX = B using an SVD decomposition
-
- Solves AX = B for a vector X, where A is am matrix m x n, w a vector (n) and
- v a matrix (n x n) as returned by SVDcmp. m and n are the dimensions of A,
- and will be equal for square matrices. b (m) is the input right-hand
- side. x (n) is the output solution vector. No input quantities are
- destroyed, so the routine may be called sequentially with different b's.
-
- \warning not to be used directly
-
- \sa to be used with svd first
-
- \sa solveBySVD and SVDsolve for a more intuitive solution of AX=B problem
-*/
-
-
-void vpMatrix::SVBksb( const vpColVector& w,
- const vpMatrix& v,
- const vpColVector& b, vpColVector& x)
-{
- unsigned int m = this->rowNum;
- unsigned int n = this->colNum;
- double** u = rowPtrs;
-
- unsigned int jj,j,i;
- double s,*tmp;
-
- tmp=new double[n];
- for (j=0;j<n;j++) {
- s=0.0;
- //if (w[j])
- if (std::fabs(w[j]) > std::numeric_limits<double>::epsilon())
- {
- for (i=0;i<m;i++) s += u[i][j]*b[i];
- s /= w[j];
- }
- tmp[j]=s;
- }
- for (j=0;j<n;j++) {
- s=0.0;
- for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
- x[j]=s;
- }
- delete [] tmp;
-}
-
-#define TOL 1.0e-5
-
-/*!
- \brief Compute the SVD decomposition
-
- Computes the singular value decomposition of the matrix, U.
- The contents of U are replaced such that A = U*S*V' where A represents
- the initial value of U.
- S is understood to have only room for ncol elements.
- The matrix V may be NULL, in which case, no values are returned for V.
-
- This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
- for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
-
- Gary William Flake
- http://research.yahoo.com/~flakeg/nodelib/html/
- http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html (not valid)
-
- \sa SVDcmp and SVDksb
-*/
-
-#define TOLERANCE 1.0e-7
-
-static
-void svd_internal_use(double *U, double *S, double *V,
- unsigned int nRow, unsigned int nCol)
-{
- unsigned int i, j, k, EstColRank, RotCount, SweepCount, slimit;
- double eps, e2, tol, vt, p, x0, y0, q, r, c0, s0, d1, d2;
-
- eps = TOLERANCE;
- slimit = nCol / 4;
- if (slimit < 6.0)
- slimit = 6;
- SweepCount = 0;
- e2 = 10.0 * nRow * eps * eps;
- tol = eps * .1;
- EstColRank = nCol;
- if(V)
- for (i = 0; i < nCol; i++)
- for (j = 0; j < nCol; j++) {
- V[nCol * i + j] = 0.0;
- V[nCol * i + i] = 1.0;
- }
- RotCount = EstColRank * (EstColRank - 1) / 2;
- while (RotCount != 0 && SweepCount <= slimit) {
- RotCount = EstColRank * (EstColRank - 1) / 2;
- SweepCount++;
- for (j = 0; j < EstColRank - 1; j++) {
- for (k = j + 1; k < EstColRank; k++) {
- p = q = r = 0.0;
- for (i = 0; i < nRow; i++) {
- x0 = U[nCol * i + j];
- y0 = U[nCol * i + k];
- p += x0 * y0;
- q += x0 * x0;
- r += y0 * y0;
- }
- S[j] = q;
- S[k] = r;
- if (q >= r) {
- if (q <= e2 * S[0] || fabs(p) <= tol * q)
- RotCount--;
- else {
- p /= q;
- r = 1 - r / q;
- vt = sqrt(4 * p * p + r * r);
- c0 = sqrt(fabs(.5 * (1 + r / vt)));
- s0 = p / (vt * c0);
- for (i = 0; i < nRow; i++) {
- d1 = U[nCol * i + j];
- d2 = U[nCol * i + k];
- U[nCol * i + j] = d1 * c0 + d2 * s0;
- U[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- if(V)
- for (i = 0; i < nCol; i++) {
- d1 = V[nCol * i + j];
- d2 = V[nCol * i + k];
- V[nCol * i + j] = d1 * c0 + d2 * s0;
- V[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- }
- }
- else {
- p /= r;
- q = q / r - 1;
- vt = sqrt(4 * p * p + q * q);
- s0 = sqrt(fabs(.5 * (1 - q / vt)));
- if (p < 0)
- s0 = -s0;
- c0 = p / (vt * s0);
- for (i = 0; i < nRow; i++) {
- d1 = U[nCol * i + j];
- d2 = U[nCol * i + k];
- U[nCol * i + j] = d1 * c0 + d2 * s0;
- U[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- if(V)
- for (i = 0; i < nCol; i++) {
- d1 = V[nCol * i + j];
- d2 = V[nCol * i + k];
- V[nCol * i + j] = d1 * c0 + d2 * s0;
- V[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- }
- }
- }
- while (EstColRank >= 3 && S[(EstColRank - 1)] <= S[0] * tol + tol * tol)
- EstColRank--;
- }
- for(i = 0; i < nCol; i++)
- S[i] = sqrt(S[i]);
- for(i = 0; i < nCol; i++)
- for(j = 0; j < nRow; j++)
- U[nCol * j + i] = U[nCol * j + i] / S[i];
-}
-
-/*!
- \brief Singular value decomposition (other function)
-
- Given a matrix A (m x n) this routine compute its singular value decomposition
- A = U W V^T. The matrice U replace A on output. the diagonal matrix of
- singular value is output as a vector W (n). The matrix V (not the transpose
- V^T) is output as V (n x n)
-
-
- \warning Destructive wrt A
- \warning
-
-
- \sa SVD for a more intuitive use
-
- This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
- for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
-
- http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html
- http://labs.yahoo.com/~flakeg/nodelib/html/svd.html
-
- \sa SVDcmp and SVDksb
-
-*/
-
-void vpMatrix::svdFlake(vpColVector &W, vpMatrix &V)
-{
-
-
- svd_internal_use(data, W.data, V.data, getRows(), getCols());
-}
-
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/core/core.hpp>
-void vpMatrix::svdOpenCV(vpColVector& w, vpMatrix& v){
- int rows = (int)this->getRows();
- int cols = (int)this->getCols();
- cv::Mat m(rows, cols, CV_64F, this->data);
- cv::SVD opencvSVD(m);
- cv::Mat opencvV = opencvSVD.vt;
- cv::Mat opencvW = opencvSVD.w;
- v.resize((unsigned int)opencvV.rows, (unsigned int)opencvV.cols);
- w.resize((unsigned int)(opencvW.rows*opencvW.cols));
-
- memcpy(v.data, opencvV.data, (size_t)(8*opencvV.rows*opencvV.cols));
- v=v.transpose();
- memcpy(w.data, opencvW.data, (size_t)(8*opencvW.rows*opencvW.cols));
- this->resize((unsigned int)opencvSVD.u.rows, (unsigned int)opencvSVD.u.cols);
- memcpy(this->data,opencvSVD.u.data, (size_t)(8*opencvSVD.u.rows*opencvSVD.u.cols));
-}
-
-#endif
-
-#ifdef VISP_HAVE_LAPACK
-extern "C" int dgesdd_(char *jobz, int *m, int *n, double *a, int *lda, double *s, double *u, int *ldu, double *vt, int *ldvt, double *work, int *lwork, int *iwork, int *info);
-#include <stdio.h>
-#include <string.h>
-
-void vpMatrix::svdLapack(vpColVector& W, vpMatrix& V){
- /* unsigned */ int m = static_cast<int>(this->getCols()), n = static_cast<int>(this->getRows()), lda = m, ldu = m, ldvt = std::min(m,n);
- int info, lwork;
-
- double wkopt;
- double* work;
-
- int* iwork = new int[8*static_cast<unsigned int>(std::min(n,m))];
-
- double *s = W.data;
- double* a = new double[static_cast<unsigned int>(lda*n)];
- memcpy(a,this->data,this->getRows()*this->getCols()*sizeof(double));
- double* u = V.data;
- double* vt = this->data;
-
-
-
- lwork = -1;
- dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info );
- lwork = (int)wkopt;
- work = new double[static_cast<unsigned int>(lwork)];
-
- dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info );
-
- if( info > 0 ) {
- vpTRACE("The algorithm computing SVD failed to converge.");
- throw(vpMatrixException(vpMatrixException::fatalError,
- "The algorithm computing SVD failed to converge.")) ;
-
- }
-
- V=V.transpose();
- delete[] work;
- delete[] iwork;
- delete[] a;
-}
-#endif
-
-#ifdef VISP_HAVE_GSL
-#include <gsl/gsl_linalg.h>
-
-void
-vpMatrix::svdGsl(vpColVector& w, vpMatrix& v)
-{
-
-#if 0
- // premier test avec la gsl 1. on recopie...
- int i,j ;
-
- int nc = getCols() ;
- int nr = getRows() ;
- gsl_matrix *A = gsl_matrix_alloc(nr, nc) ;
-
- int Atda = A->tda ;
- for (i=0 ; i < nr ; i++)
- {
- int k = i*Atda ;
- for (j=0 ; j < nc ; j++)
- A->data[k+j] = (*this)[i][j] ;
- }
- // gsl_matrix_set(A,i,j,(*this)[i][j]) ;
-
- gsl_matrix *V = gsl_matrix_alloc(nc, nc) ;
- gsl_vector *S = gsl_vector_alloc(nc) ;
- gsl_vector *work = gsl_vector_alloc(nc) ;
-
- gsl_linalg_SV_decomp(A,V,S, work) ;
-// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
-
-
- //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
- // autremement (surement !)
-
- Atda = A->tda ;
- for (i=0 ; i < nr ; i++)
- for (j=0 ; j < nc ; j++)
- (*this)[i][j] = gsl_matrix_get(A,i,j) ;
-
- int Vtda = V->tda ;
- for (i=0 ; i < nc ; i++)
- {
- int k = i*Vtda ;
- for (j=0 ; j < nc ; j++)
- v[i][j] = V->data[k+j] ;
- }
-
- for (j=0 ; j < nc ; j++)
- w[j] = gsl_vector_get(S,j) ;
-
-
- gsl_matrix_free(V) ;
- gsl_matrix_free(A) ;
- gsl_vector_free(S) ;
- gsl_vector_free(work) ;
-
-#else //optimisation Anthony 20/03/2008
-
- unsigned int nc = getCols() ;
- unsigned int nr = getRows() ;
- gsl_vector *work = gsl_vector_alloc(nc) ;
-
-// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
-
-
- //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
- // autremement (surement !)
-
- gsl_matrix A;
- A.size1 = nr;
- A.size2 = nc;
- A.tda = A.size2;
- A.data = this->data;
- A.owner = 0;
- A.block = 0;
-
- gsl_matrix V;
- V.size1 = nc;
- V.size2 = nc;
- V.tda = V.size2;
- V.data = v.data;
- V.owner = 0;
- V.block = 0;
-
- gsl_vector S;
- S.size = nc;
- S.stride = 1;
- S.data = w.data;
- S.owner = 0;
- S.block = 0;
-
- gsl_linalg_SV_decomp(&A,&V,&S, work) ;
-
- gsl_vector_free(work) ;
-
-#endif
-}
-#endif // # #GSL
-
-
-#undef TOL
-#undef TOLERANCE
-
-#undef MAX_ITER_SVD
-
-#endif // doxygen should skip this
diff --git a/src/math/matrix/vpRowVector.cpp b/src/math/matrix/vpRowVector.cpp
deleted file mode 100644
index 2f8ca5e..0000000
--- a/src/math/matrix/vpRowVector.cpp
+++ /dev/null
@@ -1,392 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRowVector.cpp 5185 2015-01-21 14:36:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Operation on row vectors.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpRowVector.cpp
- \brief Definition of vpRowVector class member
-*/
-
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpRowVector.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <string.h>
-#include <stdlib.h>
-//! Copy operator. Allow operation such as A = v
-vpRowVector & vpRowVector::operator=(const vpRowVector &v)
-{
- if (colNum==0)
- resize(v.getCols());
-
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
- rowPtrs[i][j] = v.rowPtrs[i][j];
- }
- }
- return *this;
-}
-
-/*!
- \brief copy from a matrix
- \warning Handled with care m should be a 1 column matrix
-*/
-vpRowVector & vpRowVector::operator=(const vpMatrix &m)
-{
- if (m.getCols() != colNum)
- resize(m.getCols());
-
- memcpy(data, m.data, colNum*sizeof(double)) ;
- /*
- for (int i=0; i<rowNum; i++) {
- for (int j=0; j<colNum; j++) {
- rowPtrs[i][j] = m.rowPtrs[i][j];
- }
- }*/
- return *this;
-}
-
-//! initialisation each element of the vector is x
-vpRowVector & vpRowVector::operator=(double x)
-{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
- rowPtrs[i][j] = x;
- }
- }
- return *this;
-}
-
-/*!
-
- Multiply a row vector by a column vector.
-
- \param x : Column vector.
-
- \warning The number of elements of the two vectors must be equal.
-
- \exception vpMatrixException::matrixError : If the number of elements of the
- two vectors is not the same.
-
- \return A scalar.
-
-*/
-double vpRowVector::operator*(const vpColVector &x) const
-{
- unsigned int nelements = x.getRows();
- if (getCols() != nelements ) {
- throw(vpException(vpException::dimensionError,
- "Bad size during vpRowVector (1x%d) by vpColVector (%dx1) multiplication",
- colNum, x.getRows())) ;
- }
-
- double scalar = 0.0;
-
- for (unsigned int i=0; i<nelements; i++) {
- scalar += (*this)[i] * x[i];
- }
- return scalar;
-}
-/*!
-
- Multiply a row vector by a Matrix.
-
- \param A : Matrix.
-
- \warning The number of elements of the rowVector must be equal to the number
- of rows of the matrix.
-
- \exception vpMatrixException::matrixError : If the number of elements of the
- rowVector is not equal to the number of rows of the matrix.
-
- \return A vpRowVector.
-
-*/
-vpRowVector vpRowVector::operator*(const vpMatrix &A) const
-{
- vpRowVector c(A.getCols());
-
- if (colNum != A.getRows())
- {
- vpERROR_TRACE("vpMatrix mismatch in vpRowVector/matrix multiply") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "vpMatrix mismatch in vpRowVector/matrix multiply")) ;
- }
-
- c = 0.0;
-
- for (unsigned int i=0;i<colNum;i++) {
- {
- double bi = data[i] ; // optimization em 5/12/2006
- for (unsigned int j=0;j<A.getCols();j++) {
- c[j]+=bi*A[i][j];
- }
- }
- }
-
- return c ;
-}
-
-//! Operator A = -A
-vpRowVector vpRowVector::operator-() const
-{
- vpRowVector A ;
- try {
- A.resize(colNum) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- double *vd = A.data ; double *d = data ;
-
- for (unsigned int i=0; i<colNum; i++)
- *(vd++)= - (*d++);
-
- return A;
-}
-
-//! Substraction of two vectors V = A-v
-vpRowVector vpRowVector::operator-(const vpRowVector &m) const
-{
- if (getCols() != m.getCols() ) {
- throw(vpException(vpException::dimensionError,
- "Bad size during vpRowVector (1x%d) and vpRowVector (1x%d) substraction",
- getCols(), m.getCols())) ;
- }
-
- vpRowVector v(colNum) ;
-
- for (unsigned int i=0;i<colNum;i++)
- v[i] = (*this)[i] - m[i];
- return v;
-}
-
-//! Addition of two vectors V = A-v
-vpRowVector vpRowVector::operator+(const vpRowVector &m) const
-{
- if (getCols() != m.getCols() ) {
- throw(vpException(vpException::dimensionError,
- "Bad size during vpRowVector (1x%d) and vpRowVector (1x%d) substraction",
- getCols(), m.getCols())) ;
- }
-
- vpRowVector v(colNum) ;
-
- for (unsigned int i=0;i<colNum;i++)
- v[i] = (*this)[i] + m[i];
- return v;
-}
-
-/*!
- Copy operator.
- Allows operation such as A << v
- \code
-#include <visp/vpRowVector.h>
-
-int main()
-{
- vpRowVector A, B(5);
- for (unsigned int i=0; i<B.size(); i++)
- B[i] = i;
- A << B;
- std::cout << "A: \n" << A << std::endl;
-}
- \endcode
- In row vector A we get:
- \code
-A:
-0 1 2 3 4
- \endcode
-
- */
-vpRowVector & vpRowVector::operator<<(const vpRowVector &v)
-{
- *this = v;
- return *this;
-}
-
-/*!
- \brief Transpose the row vector A
-
- A is defined inside the class
-
- \return A^T
-*/
-vpColVector vpRowVector::t() const
-{
- vpColVector tmp(colNum);
- memcpy(tmp.data, data, colNum*sizeof(double)) ;
- /*
- for (int i=0;i<colNum;i++)
- tmp[i] = (*this)[i];
- */
- return tmp;
-}
-
-//! copy constructor
-vpRowVector::vpRowVector (const vpRowVector &v) : vpMatrix(v)
-{
-}
-
-//! Constructor (Take line i of matrix m)
-vpRowVector::vpRowVector (vpMatrix &m, unsigned int i) : vpMatrix(m, i, 0, 1, m.getCols())
-{
-}
-
-/*!
- \relates vpRowVector
- \brief normalise the vector
-
- \f[
- {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
- \f]
-*/
-vpRowVector &vpRowVector::normalize(vpRowVector &x) const
-{
- x = x/sqrt(x.sumSquare());
-
- return x;
-}
-
-
-/*!
- \relates vpRowVector
- \brief normalise the vector
-
- \f[
- {\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
- \f]
-*/
-vpRowVector &vpRowVector::normalize()
-{
- double sum_square = sumSquare() ;
- *this /= sum_square ;
-
- return *this;
-}
-
-/*!
- \brief reshape the row vector in a matrix
- \param nrows : number of rows of the matrix
- \param ncols : number of columns of the matrix
- \return a vpMatrix
-*/
-vpMatrix vpRowVector::reshape(const unsigned int &nrows,const unsigned int &ncols){
- vpMatrix m(nrows,ncols);
- reshape(m,nrows,ncols);
- return m;
-}
-
-/*!
- \brief reshape the row vector in a matrix
- \param m : the reshaped Matrix
- \param nrows : number of rows of the matrix
- \param ncols : number of columns of the matrix
-*/
-void vpRowVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){
- if(dsize!=nrows*ncols)
- {
- vpERROR_TRACE("\n\t\t vpSubRowVector mismatch size for reshape vpSubColVector in a vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubRowVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;
- }
- try
- {
- if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
- }
- for(unsigned int i =0; i< nrows; i++)
- for(unsigned int j =0; j< ncols; j++)
- m[i][j]=data[i*nrows+j];
-}
-
-/*!
- Insert a row vector.
- \param i : Index of the first element to introduce. This index starts from 0.
- \param v : Row vector to insert.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpRowVector.h>
-
-int main()
-{
- vpRowVector v(4);
- for (unsigned int i=0; i < v.size(); i++)
- v[i] = i;
- std::cout << "v:\n" << v << std::endl;
-
- vpRowVector w(2);
- for (unsigned int i=0; i < w.size(); i++)
- w[i] = i+10;
- std::cout << "w:\n" << w << std::endl;
-
- v.insert(1, w);
- std::cout << "v:\n" << v << std::endl;
-}
- \endcode
- It produces the following output:
- \code
-v:
-0 1 2 3
-w:
-10 11
-v:
-0 10 11 3
- \endcode
- */
-void vpRowVector::insert(unsigned int i, const vpRowVector &v)
-{
- if (i+v.size() > this->size())
- throw(vpException(vpException::dimensionError, "Unable to insert a row vector"));
- for (unsigned int j=0; j < v.size(); j++)
- (*this)[i+j] = v[j];
-}
diff --git a/src/math/matrix/vpRowVector.h b/src/math/matrix/vpRowVector.h
deleted file mode 100644
index e16a36d..0000000
--- a/src/math/matrix/vpRowVector.h
+++ /dev/null
@@ -1,162 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRowVector.h 5185 2015-01-21 14:36:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Operation on row vectors.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-
-#ifndef vpRowVector_H
-#define vpRowVector_H
-
-#include <visp/vpMatrix.h>
-
-class vpMatrix;
-
-/*!
- \file vpRowVector.h
- \brief definition of row vector class as well
- as a set of operations on these vector
-*/
-
-/*!
- \class vpRowVector
-
- \ingroup Matrix
- \brief Definition of the row vector class.
-
- vpRowVector class provides a data structure for the row vectors as well
- as a set of operations on these vectors
-
- \author Eric Marchand (IRISA - INRIA Rennes)
-
- \warning Note the vector in the class (*this) will be noted A in the comment
-
- \ingroup libmath
-*/
-class VISP_EXPORT vpRowVector : public vpMatrix
-{
- friend class vpMatrix;
-
-
-protected:
- //! Constructor (Take line i of matrix m)
- vpRowVector(vpMatrix &m, unsigned int i);
-
-public:
- //! Basic constructor.
- vpRowVector() : vpMatrix() {};
- //! Constructor of vector of size n.
- vpRowVector(unsigned int n) : vpMatrix(1, n){};
- //! Constructor of vector of size n. Each element is set to \e val.
- vpRowVector(unsigned int n, double val) : vpMatrix(1, n, val){};
- //! Copy constructor.
- vpRowVector(const vpRowVector &v);
-
- void insert(unsigned int i, const vpRowVector &v);
-
- /*! Set the size of the row vector.
- \param i : Column vector size.
- \param flagNullify : If true, set the data to zero.
- */
- inline void resize(const unsigned int i, const bool flagNullify = true)
- {
- vpMatrix::resize(1, i, flagNullify);
- }
- //! Access V[i] = x
- inline double &operator [](unsigned int n) { return *(data+n); }
- //! Access x = V[i]
- inline const double &operator [](unsigned int n) const { return *(data+n) ; }
-
- //! Copy operator. Allow operation such as A = v
- vpRowVector &operator=(const vpRowVector &v);
- //! copy from a matrix
- vpRowVector & operator=(const vpMatrix &m) ;
- //! Initialize each element of the vector to x
- vpRowVector& operator=(const double x);
-
- //!operator dot product
- double operator*(const vpColVector &x) const;
- //!operator dot product
- vpRowVector operator*(const vpMatrix &A) const;
-
- //! Addition of two vectors V = A+v
- vpRowVector operator+(const vpRowVector &v) const;
-
- //! Substraction of two vectors V = A-v
- vpRowVector operator-(const vpRowVector &v) const;
-
- //! Operator A = -A
- vpRowVector operator-() const;
- // Copy operator. Allow operation such as A << v
- vpRowVector &operator<<(const vpRowVector &v);
-
- //! Reshape methods.
- void reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols);
- vpMatrix reshape(const unsigned int &nrows,const unsigned int &ncols);
-
- //! Transpose the vector.
- vpColVector t() const;
-
- //! Normalise the vector.
- vpRowVector &normalize() ;
- //! Normalise the vector.
- vpRowVector &normalize(vpRowVector &x) const ;
-
- /*!
-
- Return the size of the vector in term of number of columns.
-
- */
- inline unsigned int size() const
- {
- return getCols();
- }
-
-};
-
-
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/matrix/vpSubColVector.cpp b/src/math/matrix/vpSubColVector.cpp
deleted file mode 100644
index 4b93b5f..0000000
--- a/src/math/matrix/vpSubColVector.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubColVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpColVector .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-
-#include <visp/vpSubColVector.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-#include <stdlib.h>
-
-vpSubColVector::vpSubColVector()
- : pRowNum(0), parent(NULL)
-{
-}
-
-/*!
- \brief Constructor
- \param v : parent col vector
- \param offset : offset where subColVector start in the parent colVector
- \param nrows : size of the subColVector
-*/
-vpSubColVector::vpSubColVector(vpColVector &v, const unsigned int & offset, const unsigned int & nrows)
- : pRowNum(0), parent(NULL)
-{
- init(v,offset,nrows);
-}
-
-/*!
- \brief Initialisation of a the subColVector
- \param v : parent col vector
- \param offset : offset where subColVector start in the parent colVector
- \param nrows : size of the subColVector
-*/
-void vpSubColVector::init(vpColVector &v,
- const unsigned int & offset,
- const unsigned int & nrows){
-
- if(!v.data){
- vpERROR_TRACE("\n\t\t vpSubColvector parent vpColVector has been destroyed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColvector parent vpColVector has been destroyed")) ;
- }
-
- if(offset+nrows<=v.getRows()){
- data=v.data+offset;
-
- rowNum=nrows;
- colNum = 1;
-
- pRowNum=v.getRows();
- parent=&v;
-
- if(rowPtrs){
- free(rowPtrs);
- }
-
- rowPtrs=(double**)malloc( parent->getRows() * sizeof(double*));
- for(unsigned int i=0;i<nrows;i++)
- rowPtrs[i]=v.data+i+offset;
-
- dsize = rowNum ;
- trsize =0 ;
- }else{
- vpERROR_TRACE("SubColVector cannot be contain in parent ColVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"SubColVector cannot be contain in parent ColVector")) ;
- }
-}
-
-vpSubColVector::~vpSubColVector(){
- data=NULL ;
-}
-
-
-/*!
- \brief This method can be used to detect if the parent colVector
- always exits or its size have not changed and throw an exception is not
-*/
-void vpSubColVector::checkParentStatus(){
- if(!data){
- vpERROR_TRACE("\n\t\t vpSubColvector parent vpColVector has been destroyed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColvector parent vpColVector has been destroyed")) ;
- }
- if(pRowNum!=parent->getRows()){
- vpERROR_TRACE("\n\t\t vpSubColvector size of parent vpColVector has been changed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"\n\t\t \n\t\t vpSubColvector size of parent vpColVector has been changed")) ;
- }
-}
-
-/*!
- \brief Operation A = B
- \param B : a subColvector
-*/
-vpSubColVector & vpSubColVector::operator=(const vpSubColVector &B){
-
- if ( rowNum != B.getRows())
- {
- vpERROR_TRACE("\n\t\t vpSubColVector mismatch in operator vpSubColVector=vpSubColVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubColVector=vpSubColVector")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i];
- return *this;
-}
-
-/*!
- \brief Operation A = B
- \param B : a rowVector
-*/
-vpSubColVector & vpSubColVector::operator=(const vpColVector &B){
- if ( rowNum != B.getRows())
- {
- vpERROR_TRACE("\n\t\t vpSubColVector mismatch in operator vpSubColVector=vpColVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColVector mismatch in operator vpSubColVector=vpColVector")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i];
-
- return *this;
-}
-
-/*!
- \brief Operation A = B
- \param B : a vpMatrix of size nrow x 1
-*/
-vpSubColVector & vpSubColVector::operator=(const vpMatrix &B){
- if ((B.getCols()!=1)||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpSubColVector mismatch in operator vpSubColVector=vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColVector mismatch in operator vpSubColVector=vpMatrix")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i][1];
- return *this;
-}
-
-/*!
- \brief Operation A = x
- \param x : a scalar value
-*/
-vpSubColVector & vpSubColVector::operator=(const double &x){
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = x;
- return *this;
-}
diff --git a/src/math/matrix/vpSubColVector.h b/src/math/matrix/vpSubColVector.h
deleted file mode 100644
index 59cd355..0000000
--- a/src/math/matrix/vpSubColVector.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubColVector.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpColVector .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-#ifndef __VP_SUB_COL_VECTOR__
-#define __VP_SUB_COL_VECTOR__
-
-#include <visp/vpColVector.h>
-
-/*!
- \file vpSubColVector.h
-
- \brief Definition of the vpSubColVector class
-*/
-
-/*!
- \class vpSubColVector
- \ingroup vpMath
- \brief Definition of the vpSubColVector
- vpSubColVector class provides a mask on a vpColVector
- all properties of vpColVector are available with
- a vpSubColVector
-
- \author Jean Laneurit (IRISA - INRIA Rennes)
-
- \sa vpMatrix vpColvector vpRowVector
-*/
-class VISP_EXPORT vpSubColVector : public vpColVector {
-
- private :
- //!Copy constructor
- vpSubColVector(const vpSubColVector& /* m */);
-
- protected :
-
- //!Number of row of parent vpColvector at initialization
- unsigned int pRowNum;
- //!Parent vpColvector
- vpColVector *parent;
-
- public:
-
- //!Default constructor
- vpSubColVector();
- //!Constructor
- vpSubColVector(vpColVector &v, const unsigned int & offset,const unsigned int & nrows);
- //!Destructor
- ~vpSubColVector();
-
- //! Initialisation of vpSubColVector
- void init(vpColVector &v, const unsigned int & offset,const unsigned int & nrows);
-
- //!Check is partent vpColVector has changed since initialization
- void checkParentStatus();
-
- //! Operation such as subA = subB
- vpSubColVector & operator=(const vpSubColVector &B);
- //! Operation such as subA = B
- vpSubColVector & operator=(const vpColVector &B);
- //! Operation such as subA = matrix B with size of B(N,1)
- vpSubColVector & operator=(const vpMatrix &B);
- //! Operation such as subA = x
- vpSubColVector & operator=(const double &x);
-
-};
-
-#endif
diff --git a/src/math/matrix/vpSubMatrix.cpp b/src/math/matrix/vpSubMatrix.cpp
deleted file mode 100644
index 6ba81a2..0000000
--- a/src/math/matrix/vpSubMatrix.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * Copyright (C) 2005 - 2014 Inria. All rights reserved.
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpMatrix .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-#include <visp/vpSubMatrix.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-#include <stdlib.h>
-
-vpSubMatrix::vpSubMatrix()
- : pRowNum(0), pColNum(0), parent(NULL)
-{
-}
-
-/*!
- \brief Constructor
- \param m : parent matrix
- \param row_offset : row offset
- \param col_offset : col offset
- \param nrows : number of rows of the sub matrix
- \param ncols : number of columns of the sub matrix
-*/
-vpSubMatrix::vpSubMatrix(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset,
- const unsigned int & nrows, const unsigned int & ncols)
- : pRowNum(0), pColNum(0), parent(NULL)
-{
- init(m,row_offset,col_offset,nrows,ncols);
-}
-
-/*!
- \brief Initialisation of a sub matrix
- \param m : parent matrix
- \param row_offset : row offset
- \param col_offset : col offset
- \param nrows : number of rows of the sub matrix
- \param ncols : number of columns of the sub matrix
-*/
-void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset , const unsigned int & nrows , const unsigned int & ncols){
-
- if(! m.data){
- vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ;
- throw(vpMatrixException(vpMatrixException::subMatrixError,
- "\n\t\t SubMatrix parent matrix is not allocated")) ;
- }
-
- if(row_offset+nrows <= m.getRows() && col_offset+ncols <= m.getCols()){
- data=m.data;
- parent =&m;
- rowNum = nrows;
- colNum = ncols;
- pRowNum=m.getRows();
- pColNum=m.getCols();
-
- if(rowPtrs)
- free(rowPtrs);
-
- rowPtrs=(double**) malloc(nrows * sizeof(double*));
- for(unsigned int r=0;r<nrows;r++)
- rowPtrs[r]= m.data+col_offset+(r+row_offset)*pColNum;
-
- dsize = pRowNum*pColNum ;
- trsize =0 ;
- }else{
- vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ;
- }
-}
-
-/*!
- \brief This method can be used to detect if the parent matrix
- always exits or its size have not changed and throw an exception is not
-*/
-void vpSubMatrix::checkParentStatus(){
- if(!data){
- vpERROR_TRACE("\n\t\t vpSubMatrix parent vpMatrix has been destroyed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix parent vpMatrix has been destroyed")) ;
- }
- if(pRowNum!=parent->getRows() || pColNum!=parent->getCols()){
- vpERROR_TRACE("\n\t\t vpSubMatrix size of parent vpMatrix has been changed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix size of parent vpMatrix has been changed")) ;
- }
-}
-
-/*!
- \brief Operation A = B
- \param B : a matrix
-*/
-vpSubMatrix & vpSubMatrix::operator=(const vpMatrix &B){
-
- if ((colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] = B[i][j];
-
- return *this;
-}
-
-/*!
- \brief Operation A = B
- \param B : a subMatrix
-*/
-vpSubMatrix & vpSubMatrix::operator=(const vpSubMatrix &B){
-
- if ((colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
- }
-
-
- double ** BrowPtrs=B.rowPtrs;
-
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] = BrowPtrs[i][j];
-
- return *this;
-}
-
-/*!
- \brief Operation A = x
- \param x : a scalar
-*/
-vpSubMatrix & vpSubMatrix::operator=(const double &x){
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j] = x;
-
- return *this;
-}
-
-
-vpSubMatrix::~vpSubMatrix()
-{
- data=NULL;
-}
diff --git a/src/math/matrix/vpSubMatrix.h b/src/math/matrix/vpSubMatrix.h
deleted file mode 100644
index 9dcae3e..0000000
--- a/src/math/matrix/vpSubMatrix.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubMatrix.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpMatrix .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-#ifndef __VP_SUB_MATRIX__
-#define __VP_SUB_MATRIX__
-
-#include <visp/vpMatrix.h>
-
-
-/*!
- \file vpSubMatrix.h
-
- \brief Definition of the vpSubMatrix class
-*/
-
-/*!
- \class vpSubMatrix
- \ingroup vpMath
- \brief Definition of the vpSubMatrix
- vpSubMatrix class provides a mask on a vpMatrix
- all properties of vpMatrix are available with
- a vpSubMatrix
-
- \author Jean Laneurit (IRISA - INRIA Rennes)
-
- \sa vpMatrix vpColvector vpRowVector
-*/
-class VISP_EXPORT vpSubMatrix : public vpMatrix{
-
- private :
- //! Eye method unavailable
- void eye(unsigned int n);
- //! Eye method unavailable
- void eye(unsigned int m, unsigned int n);
- //! Copy constructor unavailable
- vpSubMatrix(const vpSubMatrix& /* m */);
-
- protected :
-
- unsigned int pRowNum;
- unsigned int pColNum;
- vpMatrix *parent;
-
- public:
-
- //!Default constructor
- vpSubMatrix();
- //!Constructor
- vpSubMatrix(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
- //!Destructor
- ~vpSubMatrix();
-
- //! Initialisation of vpMatrix
- void init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
-
- //!Check is parent vpRowVector has changed since initialization
- void checkParentStatus();
-
- //! Operation such as subA = subB
- vpSubMatrix & operator=(const vpSubMatrix &B);
- //! Operation such as subA = B
- vpSubMatrix & operator=(const vpMatrix &B);
- //! Operation such as subA = x
- vpSubMatrix & operator=(const double &x);
-
-};
-
-#endif
diff --git a/src/math/matrix/vpSubRowVector.cpp b/src/math/matrix/vpSubRowVector.cpp
deleted file mode 100644
index 935f0cb..0000000
--- a/src/math/matrix/vpSubRowVector.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubRowVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpRowVector .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-
-#include <visp/vpSubRowVector.h>
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-#include <stdlib.h>
-
-vpSubRowVector::vpSubRowVector()
- : pColNum(0), parent(NULL)
-{
-}
-
-/*!
- \brief Constructor
- \param v : parent row vector
- \param offset : offset where subRowVector start in the parent vector
- \param ncols : size of the subRowVector
-*/
-vpSubRowVector::vpSubRowVector(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols)
- : pColNum(0), parent(NULL)
-{
- init(v, offset, ncols);
-}
-
-/*!
- \brief Initialisation of a the subRowVector
- \param v : parent row vector
- \param offset : offset where subRowVector start in the parent vector
- \param ncols : size of the subRowVector
-*/
-void vpSubRowVector::init(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols){
-
- if(!v.data){
- vpERROR_TRACE("\n\t\t vpSubColvector parent vpRowVector has been destroyed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColvector parent vpRowVector has been destroyed")) ;
- }
-
- if(offset+ncols<=v.getCols()){
- data=v.data+offset;
-
- rowNum=1;
- colNum = ncols;
-
- pColNum=v.getCols();
- parent=&v;
-
- if(rowPtrs)
- free(rowPtrs);
-
- rowPtrs=(double**) malloc(1 * sizeof(double*));
- for(unsigned int i=0;i<1;i++)
- rowPtrs[i]=v.data+i+offset;
-
- dsize = colNum ;
- trsize =0 ;
- }else{
- vpERROR_TRACE("SubRowVector cannot be contain in parent RowVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"SubRowVector cannot be contain in parent RowVector")) ;
- }
-}
-
-vpSubRowVector::~vpSubRowVector(){
- data=NULL ;
-}
-
-/*!
- \brief This method can be used to detect if the parent rowVector
- always exits or its size have not changed and throw an exception is not
-*/
-void vpSubRowVector::checkParentStatus(){
- if(!data){
- vpERROR_TRACE("\n\t\t vpSubColvector parent vpRowVector has been destroyed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColvector parent vpRowVector has been destroyed")) ;
- }
- if(pColNum!=parent->getCols()){
- vpERROR_TRACE("\n\t\t vpSubColvector size of parent vpRowVector has been changed");
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubColvector size of parent vpRowVector has been changed")) ;
- }
-}
-
-/*!
- \brief Operation A = B
- \param B : a subRowvector
-*/
-vpSubRowVector & vpSubRowVector::operator=(const vpSubRowVector &B){
-
- if ( colNum != B.getCols())
- {
- vpERROR_TRACE("\n\t\t vpSubRowVector mismatch in operator vpSubRowVector=vpSubRowVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubRowVector=vpSubRowVector")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i];
-
- return *this;
-}
-
-/*!
- \brief Operation A = B
- \param B : a rowVector
-*/
-vpSubRowVector & vpSubRowVector::operator=(const vpRowVector &B){
- if ( colNum != B.getCols())
- {
- vpERROR_TRACE("\n\t\t vpSubRowVector mismatch in operator vpSubRowVector=vpRowVector") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubRowVector=vpRowVector")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i];
-
- return *this;
-}
-
-/*!
- \brief Operation A = B
- \param B : a vpMatrix of size 1 x ncols
-*/
-vpSubRowVector & vpSubRowVector::operator=(const vpMatrix &B){
- if ((B.getRows()!=1)||(colNum != B.getCols()))
- {
- vpERROR_TRACE("\n\t\t vpSubRowVector mismatch in operator vpSubRowVector=vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubRowVector=vpMatrix")) ;
- }
-
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = B[i][1];
- return *this;
-}
-/*!
- \brief Operation A = x
- \param x : a scalar value
-*/
-vpSubRowVector & vpSubRowVector::operator=(const double &x){
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = x;
- return *this;
-}
diff --git a/src/math/matrix/vpSubRowVector.h b/src/math/matrix/vpSubRowVector.h
deleted file mode 100644
index eb91bfa..0000000
--- a/src/math/matrix/vpSubRowVector.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSubRowVector.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mask on a vpRowVector .
- *
- * Authors:
- * Laneurit Jean
- *
- *****************************************************************************/
-
-#ifndef __VP_SUB_ROW_VECTOR__
-#define __VP_SUB_ROW_VECTOR__
-
-#include <visp/vpRowVector.h>
-
-
-/*!
- \file vpSubRowVector.h
-
- \brief Definition of the vpSubRowVector class
-*/
-
-/*!
- \class vpSubRowVector
- \ingroup vpMath
- \brief Definition of the vpSubRowVector
- vpSubRowVector class provides a mask on a vpRowVector
- all properties of vpRowVector are available with
- a vpSubRowVector
-
- \author Jean Laneurit (IRISA - INRIA Rennes)
-
- \sa vpMatrix vpColvector vpRowVector
-*/
-
-class VISP_EXPORT vpSubRowVector : public vpRowVector {
-
- private :
- //!Copy constructor unavaible
- vpSubRowVector(const vpSubRowVector& /* m */);
-
- protected :
-
- //!Number of row of parent vpColvector at initialization
- unsigned int pColNum;
- //!Parent vpColvector
- vpRowVector *parent;
-
- public:
-
- //!Default constructor
- vpSubRowVector();
- //!Constructor
- vpSubRowVector(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols);
- //!Destructor
- ~vpSubRowVector();
-
- //! Initialisation of vpSubRowVector
- void init(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols);
-
- //!Check is parent vpRowVector has changed since initialization
- void checkParentStatus();
-
- //! Operation such as subA = subB
- vpSubRowVector & operator=(const vpSubRowVector &B);
- //! Operation such as subA = B
- vpSubRowVector & operator=(const vpRowVector &B);
- //! Operation such as subA = matrix B with size of B(N,1)
- vpSubRowVector & operator=(const vpMatrix &B);
- //! Operation such as subA = x
- vpSubRowVector & operator=(const double &x);
-
-};
-
-#endif
diff --git a/src/math/misc/vpHinkley.cpp b/src/math/misc/vpHinkley.cpp
deleted file mode 100755
index c24c168..0000000
--- a/src/math/misc/vpHinkley.cpp
+++ /dev/null
@@ -1,450 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHinkley.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Hinkley's cumulative sum test implementation.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpHinkley.cpp
-
- \brief Definition of the vpHinkley class corresponding to the Hinkley's
- cumulative sum test.
-
-*/
-
-#include <visp/vpHinkley.h>
-#include <visp/vpDebug.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMath.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-/* VP_DEBUG_MODE fixed by configure:
- 1:
- 2:
- 3: Print data
-*/
-
-
-/*!
-
- Constructor.
-
- Call init() to initialise the Hinkley's test and set \f$\alpha\f$
- and \f$\delta\f$ to default values.
-
- By default \f$ \delta = 0.2 \f$ and \f$ \alpha = 0.2\f$. Use
- setDelta() and setAlpha() to modify these values.
-
-*/
-vpHinkley::vpHinkley()
- : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
-{
-}
-
-/*!
-
- Constructor.
-
- Call init() to initialise the Hinkley's test and set \f$\alpha\f$
- and \f$\delta\f$ thresholds.
-
- \param alpha_val : \f$\alpha\f$ is a predefined threshold.
-
- \param delta_val : \f$\delta\f$ denotes the jump minimal magnitude that
- we want to detect.
-
- \sa setAlpha(), setDelta()
-
-*/
-
-vpHinkley::vpHinkley(double alpha_val, double delta_val)
- : dmin2(delta_val/2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0)
-{
-}
-
-/*!
-
- Call init() to initialise the Hinkley's test and set \f$\alpha\f$
- and \f$\delta\f$ thresholds.
-
- \param alpha_val : \f$\alpha\f$ is a predefined threshold.
-
- \param delta_val : \f$\delta\f$ denotes the jump minimal magnitude that
- we want to detect.
-
- \sa setAlpha(), setDelta()
-
-*/
-void
-vpHinkley::init(double alpha_val, double delta_val)
-{
- init();
-
- setAlpha(alpha_val);
- setDelta(delta_val);
-}
-
-/*!
-
- Destructor.
-
-*/
-vpHinkley::~vpHinkley()
-{
-}
-
-/*!
-
- Initialise the Hinkley's test by setting the mean signal value
- \f$m_0\f$ to zero as well as \f$S_k, M_k, T_k, N_k\f$.
-
-*/
-void vpHinkley::init()
-{
- nsignal = 0;
- mean = 0.0;
-
- Sk = 0;
- Mk = 0;
-
- Tk = 0;
- Nk = 0;
-}
-
-/*!
-
- Set the value of \f$\delta\f$, the jump minimal magnetude that we want to
- detect.
-
- \sa setAlpha()
-
-*/
-void vpHinkley::setDelta(double delta)
-{
- dmin2 = delta / 2;
-}
-
-/*!
-
- Set the value of \f$\alpha\f$, a predefined threshold.
-
- \sa setDelta()
-
-*/
-void vpHinkley::setAlpha(double alpha_val)
-{
- this->alpha = alpha_val;
-}
-
-/*!
-
- Perform the Hinkley test. A downward jump is detected if
- \f$ M_k - S_k > \alpha \f$.
-
- \param signal : Observed signal \f$ s(t) \f$.
-
- \sa setDelta(), setAlpha(), testUpwardJump()
-
-*/
-vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal)
-{
-
- vpHinkleyJumpType jump = noJump;
-
- nsignal ++; // Signal length
-
- if (nsignal == 1) mean = signal;
-
- // Calcul des variables cumulees
- computeSk(signal);
-
- computeMk();
-
- vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
- << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk;
-
- // teste si les variables cumulees excedent le seuil
- if ((Mk - Sk) > alpha)
- jump = downwardJump;
-
-#ifdef VP_DEBUG
- if (VP_DEBUG_MODE >=2) {
- switch(jump) {
- case noJump:
- std::cout << "noJump " << std::endl;
- break;
- case downwardJump:
- std::cout << "downWardJump " << std::endl;
- break;
- case upwardJump:
- std::cout << "upwardJump " << std::endl;
- break;
- }
- }
-#endif
-
- computeMean(signal);
-
- if (jump == downwardJump) {
- vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
-
- Sk = 0; Mk = 0; nsignal = 0;
- }
-
- return (jump);
-}
-
-/*!
-
- Perform the Hinkley test. An upward jump is detected if \f$ T_k - N_k >
- \alpha \f$.
-
- \param signal : Observed signal \f$ s(t) \f$.
-
- \sa setDelta(), setAlpha(), testDownwardJump()
-
-*/
-vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
-{
-
- vpHinkleyJumpType jump = noJump;
-
- nsignal ++; // Signal length
-
- if (nsignal == 1) mean = signal;
-
- // Calcul des variables cumulees
- computeTk(signal);
-
- computeNk();
-
- vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
- << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk;
-
- // teste si les variables cumulees excedent le seuil
- if ((Tk - Nk) > alpha)
- jump = upwardJump;
-
-#ifdef VP_DEBUG
- if (VP_DEBUG_MODE >= 2) {
- switch(jump) {
- case noJump:
- std::cout << "noJump " << std::endl;
- break;
- case downwardJump:
- std::cout << "downWardJump " << std::endl;
- break;
- case upwardJump:
- std::cout << "upWardJump " << std::endl;
- break;
- }
- }
-#endif
- computeMean(signal);
-
- if (jump == upwardJump) {
- vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
-
- Tk = 0; Nk = 0; nsignal = 0;
- }
-
- return (jump);
-}
-
-/*!
-
- Perform the Hinkley test. A downward jump is detected if \f$ M_k - S_k >
- \alpha \f$. An upward jump is detected if \f$ T_k - N_k > \alpha \f$.
-
- \param signal : Observed signal \f$ s(t) \f$.
-
- \sa setDelta(), setAlpha(), testDownwardJump(), testUpwardJump()
-
-*/
-vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
-{
-
- vpHinkleyJumpType jump = noJump;
-
- nsignal ++; // Signal length
-
- if (nsignal == 1) mean = signal;
-
- // Calcul des variables cumulees
- computeSk(signal);
- computeTk(signal);
-
- computeMk();
- computeNk();
-
- vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
- << " signal: " << signal
- << " Sk: " << Sk << " Mk: " << Mk
- << " Tk: " << Tk << " Nk: " << Nk << std::endl;
-
- // teste si les variables cumulees excedent le seuil
- if ((Mk - Sk) > alpha)
- jump = downwardJump;
- else if ((Tk - Nk) > alpha)
- jump = upwardJump;
-
-#ifdef VP_DEBUG
- if (VP_DEBUG_MODE >= 2) {
- switch(jump) {
- case noJump:
- std::cout << "noJump " << std::endl;
- break;
- case downwardJump:
- std::cout << "downWardJump " << std::endl;
- break;
- case upwardJump:
- std::cout << "upwardJump " << std::endl;
- break;
- }
- }
-#endif
-
- computeMean(signal);
-
- if ((jump == upwardJump) || (jump == downwardJump)) {
- vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
-
- Sk = 0; Mk = 0; Tk = 0; Nk = 0; nsignal = 0;
- // Debut modif FS le 03/09/2003
- mean = signal;
- // Fin modif FS le 03/09/2003
- }
-
- return (jump);
-}
-
-/*!
-
- Compute the mean value \f$m_0\f$ of the signal. The mean value must be
- computed before the jump is estimated on-line.
-
- \param signal : Observed signal \f$ s(t) \f$.
-
-*/
-void vpHinkley::computeMean(double signal)
-{
- // Debut modif FS le 03/09/2003
- // Lors d'une chute ou d'une remontee lente du signal, pariculierement
- // apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
- // derives de la moyenne, elle n'est remise a jour avec la valeur
- // courante du signal que si un debut de saut potentiel n'est pas detecte.
- //if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
- if ( ( std::fabs(Mk-Sk) <= std::fabs(vpMath::maximum(Mk,Sk))*std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(Tk-Nk) <= std::fabs(vpMath::maximum(Tk,Nk))*std::numeric_limits<double>::epsilon() ) )
- // Fin modif FS le 03/09/2003
-
- // Mise a jour de la moyenne.
- mean = (mean * (nsignal - 1) + signal) / (nsignal);
-
-}
-/*!
-
- Compute \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2})\f$
-
- \param signal : Observed signal \f$ s(t) \f$.
-
-*/
-void vpHinkley::computeSk(double signal)
-{
-
- // Calcul des variables cumulees
- Sk += signal - mean + dmin2;
-}
-/*!
-
- Compute \f$M_k\f$, the maximum value of \f$S_k\f$.
-
-*/
-void vpHinkley::computeMk()
-{
- if (Sk > Mk) Mk = Sk;
-}
-/*!
-
- Compute \f$T_k = \sum_{t=0}^{k} (s(t) - m_0 - \frac{\delta}{2})\f$
-
- \param signal : Observed signal \f$ s(t) \f$.
-*/
-void vpHinkley::computeTk(double signal)
-{
-
- // Calcul des variables cumulees
- Tk += signal - mean - dmin2;
-}
-/*!
-
- Compute \f$N_k\f$, the minimum value of \f$T_k\f$.
-
-*/
-void vpHinkley::computeNk()
-{
- if (Tk < Nk) Nk = Tk;
-}
-
-void vpHinkley::print(vpHinkley::vpHinkleyJumpType jump)
-{
- switch(jump)
- {
- case noJump :
- std::cout << " No jump detected " << std::endl ;
- break ;
- case downwardJump :
- std::cout << " Jump downward detected " << std::endl ;
- break ;
- case upwardJump :
- std::cout << " Jump upward detected " << std::endl ;
- break ;
- default:
- std::cout << " Jump detected " << std::endl ;
- break ;
-
- }
-}
-
diff --git a/src/math/misc/vpHinkley.h b/src/math/misc/vpHinkley.h
deleted file mode 100755
index 97fad83..0000000
--- a/src/math/misc/vpHinkley.h
+++ /dev/null
@@ -1,173 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHinkley.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Hinkley's cumulative sum test implementation.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpHinkley_H
-#define vpHinkley_H
-
-/*!
- \file vpHinkley.h
- \brief class for Hinkley's cumulative test computation.
-*/
-#include <visp/vpConfig.h>
-
-/*!
- \class vpHinkley
-
- \ingroup Hinkley
- \brief This class implements the Hinkley's cumulative sum test.
-
- \author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
-
- The Hinkley's cumulative sum test is designed to detect jump in mean
- of an observed signal \f$ s(t) \f$. It is known to be robust (by
- taking into account all the past of the observed quantity),
- efficient, and inducing a very low computational load. The other
- attractive features of this test are two-fold. First, it can
- straightforwardly and accurately provide the jump instant. Secondly,
- due to its formulation (cumulative sum test), it can simultaneously
- handle both very abrupt and important changes, and gradual smaller
- ones without adapting the involved thresholds.
-
- Two tests are performed in parallel to look for downwards or upwards
- jumps in \f$ s(t) \f$, respectively defined by:
-
- \f[ S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2}) \f]
- \f[ M_k = \max_{0 \leq i \leq k} S_i\f]
- \f[ T_k = \sum_{t=0}^{k} (s(t) - m_0 - \frac{\delta}{2}) \f]
- \f[ N_k = \max_{0 \leq i \leq k} T_i\f]
-
-
- In which \f$m_o\f$ is computed on-line and corresponds to the mean
- of the signal \f$ s(t) \f$ we want to detect a jump. \f$m_o\f$ is
- re-initialized at zero after each jump detection. \f$\delta\f$
- denotes the jump minimal magnitude that we want to detect and
- \f$\alpha\f$ is a predefined threshold. These values are set by
- default to 0.2 in the default constructor vpHinkley(). To modify the
- default values use setAlpha() and setDelta() or the
- vpHinkley(double alpha, double delta) constructor.
-
- A downward jump is detected if \f$ M_k - S_k > \alpha \f$.
- A upward jump is detected if \f$ T_k - N_k > \alpha \f$.
-
- To detect only downward jumps in \f$ s(t) \f$ use
- testDownwardJump().To detect only upward jumps in \f$ s(t) \f$ use
- testUpwardJump(). To detect both, downard and upward jumps use
- testDownUpwardJump().
-
- If a jump is detected, the jump location is given by the last instant
- \f$k^{'}\f$ when \f$ M_{k^{'}} - S_{k^{'}} = 0 \f$, or \f$ T_{k^{'}} -
- N_{k^{'}} = 0 \f$.
-
-*/
-class VISP_EXPORT vpHinkley
-{
- public:
- /*! \enum vpHinkleyJumpType
- Indicates if a jump is detected by the Hinkley test.
- */
- typedef enum {
- noJump, /*!< No jump is detected by the Hinkley test. */
- downwardJump, /*!< A downward jump is detected by the Hinkley test. */
- upwardJump /*!< An upward jump is detected by the Hinkley test. */
- } vpHinkleyJumpType;
-
- public:
- vpHinkley();
- ~vpHinkley();
- vpHinkley(double alpha, double delta);
-
- void init();
- void init(double alpha, double delta) ;
-
- void setDelta(double delta);
- void setAlpha(double alpha);
- vpHinkleyJumpType testDownwardJump(double signal);
- vpHinkleyJumpType testUpwardJump(double signal);
- vpHinkleyJumpType testDownUpwardJump(double signal);
-
- static void print(vpHinkleyJumpType jump) ;
-
- /*!
- \return The mean value \f$m_0\f$ of the signal \f$ s(t) \f$.
-
- */
- inline double getMean() {return mean;}
- /*!
- \return The value of \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2})\f$
-
- */
- inline double getSk() {return Sk;}
- /*!
- \return The value of \f$M_k\f$, the maximum value of \f$S_k\f$.
-
- */
- inline double getMk() {return Mk;}
- /*!
-
- \return The value of \f$T_k = \sum_{t=0}^{k} (s(t) - m_0 -
- \frac{\delta}{2})\f$
-
- */
- inline double getTk() {return Tk;}
- /*!
- \return The value of \f$N_k\f$, the maximum value of \f$T_k\f$.
-
- */
- inline double getNk() {return Nk;}
- private:
- void computeMean(double signal);
- void computeSk(double signal);
- void computeMk();
- void computeTk(double signal);
- void computeNk();
-
- private:
- double dmin2;
- double alpha;
- int nsignal; // Signal length
- double mean; // Signal mean value
- double Sk;
- double Mk;
- double Tk;
- double Nk;
-};
-
-#endif
diff --git a/src/math/misc/vpMath.cpp b/src/math/misc/vpMath.cpp
deleted file mode 100644
index ac637a1..0000000
--- a/src/math/misc/vpMath.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMath.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simple mathematical function not available in the C math library (math.h).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMath.cpp
- \brief Provides simple Math computation that are not available in
- the C mathematics library (math.h)
-*/
-
-#include <visp/vpMath.h>
-
-const double vpMath::ang_min_sinc = 1.0e-8;
-const double vpMath::ang_min_mc = 2.5e-4;
-
-
diff --git a/src/math/misc/vpMath.h b/src/math/misc/vpMath.h
deleted file mode 100644
index 491fcf6..0000000
--- a/src/math/misc/vpMath.h
+++ /dev/null
@@ -1,380 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMath.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simple mathematical function not available in the C math library (math.h).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMath.h
- \brief Provides simple Math computation that are not available in
- the C mathematics library (math.h)
-*/
-
-
-
-#ifndef vpMATH_HH
-#define vpMATH_HH
-
-#include <visp/vpConfig.h>
-
-#include <math.h>
-
-#if defined(_WIN32) // Not defined in Microsoft math.h
-
-# ifndef M_PI
-# define M_PI 3.14159265358979323846f
-# endif
-
-# ifndef M_PI_2
-# define M_PI_2 (M_PI/2.f)
-# endif
-
-# ifndef M_PI_4
-# define M_PI_4 (M_PI/4.f)
-# endif
-
-#endif
-
-
-/*!
- \class vpMath
- \ingroup MathTools
- \brief Provides simple mathematics computation tools that are not
- available in the C mathematics library (math.h)
-
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-*/
-
-class VISP_EXPORT vpMath
-{
- public:
-
- /*!
- Convert an angle in radians into degrees.
-
- \param rad : Angle in radians.
- \return Angle converted in degrees.
- */
- static inline double deg(double rad) { return (rad*180.0)/M_PI ; }
-
- /*!
- Convert an angle in degrees into radian.
- \param deg : Angle in degrees.
- \return Angle converted in radian.
- */
- static inline double rad(double deg) { return (deg*M_PI)/180.0 ; }
-
- /*!
- Compute x square value.
- \return \f$ x^2 \f$.
- */
- static inline double sqr(double x) { return x*x ; }
-
- // factorial of x
- static inline double fact(unsigned int x) ;
-
- // combinaison
- static inline long double comb(unsigned int n, unsigned int p) ;
-
- // round x to the nearest integer
- static inline int round(const double x) ;
-
- // return the sign of x (+-1)
- static inline int sign(double x) ;
-
-
- // test if a number equals 0 (with threshold value)
- static inline bool nul(double x, double s=0.001);
-
- // test if two numbers are equals (with a user defined threshold)
- static inline bool equal(double x, double y, double s=0.001);
-
- // test if a number is greater than another (with a user defined threshold)
- static inline bool greater(double x, double y, double s=0.001);
-
-
- /*!
- Find the maximum between two numbers (or other).
- \param a : First number.
- \param b : Second number.
- \return The maximum of the two numbers.
- */
- template <class Type> static Type maximum(const Type& a, const Type& b)
- {
- return (a > b) ? a : b;
- }
-
- /*!
- Find the minimum between two numbers (or other).
- \param a : First number.
- \param b : Second number.
- \return The minimum of the two numbers.
- */
- template <class Type> static Type minimum(const Type& a, const Type& b)
- {
- return (a < b) ? a : b;
- }
-
- /*!
- Find the absolute value of a number (or other).
- \param x : The number.
- \return The absolute value of x
- */
- template <class Type> static Type abs(const Type& x)
- {
- return (x < 0) ? -x : x;
- }
-
-
- // sinus cardinal
- static inline double sinc(double x) ;
- static inline double sinc(double sinx, double x) ;
- static inline double mcosc(double cosx, double x) ;
- static inline double msinc(double sinx, double x) ;
-
- // sigmoid
- static inline double sigmoid(double x, double x0=0.,double x1=1., double n=12.);
-
- /*!
- Exchange two numbers.
-
- \param a First number to exchange.
- \param b Second number to exchange
- */
- template <class Type> static void swap(Type& a, Type& b)
- {
- Type tmp = b;
- b = a;
- a = tmp;
- }
-
- private:
- static const double ang_min_sinc;
- static const double ang_min_mc;
-};
-
-
-
-//Begining of the inline functions definition
-
-/*!
- Computes and returns x!
- \param x : parameter of factorial function.
-*/
-double vpMath::fact(unsigned int x)
-{
- if ( (x == 1) || (x == 0)) return 1;
- return x * fact(x-1);
-}
-
-/*!
- Computes the number of combination of p elements inside n elements.
-
- \param n : total number of elements.
- \param p : requested number of elements.
-
- \return \f$ n! / ((n-p)! p!) \f$
-*/
-long double vpMath::comb(unsigned int n, unsigned int p)
-{
- if (n == p) return 1;
- return fact(n)/ (fact(n-p) * fact(p));
-}
-
-
-/*!
- Round x to the nearest integer.
-
- \param x : Value to round.
-
- \return Nearest integer of x.
-
-*/
-int vpMath::round(const double x)
-{
- if (sign(x) > 0)
- {
- if ((x-(int)x) <= 0.5) return (int)x ;
- else return (int)x+1 ;
- }
- else
- {
- if (fabs(x-(int)x) <= 0.5) return (int)x ;
- else return (int)x-1 ;
- }
-}
-
-/*!
- Return the sign of x.
-
- \return -1 if x is negative, +1 if positive.
-
-*/
-int vpMath::sign(double x)
-{
- if (fabs(x) < 1e-15) return 0 ;else
- {
- if (x<0) return -1 ; else return 1 ;
- }
-}
-
-/*!
- Compares \f$ | x | \f$ to \f$ s \f$.
- \param x : Value to test.
- \param s : Tolerance threshold
- \return true if \f$ | x | < s \f$.
-
-*/
-bool vpMath::nul(double x, double s)
-{
- return(fabs(x)<s);
-}
-
-/*!
- Compares \f$ | x - y | \f$ to \f$ s \f$.
- \param x : x value.
- \param y : y value.
- \param s : Tolerance threshold.
- \return true if \f$ | x - y | < s \f$.
-*/
-bool vpMath::equal(double x, double y, double s)
-{
- return( nul(x-y, s) );
-}
-
-/*!
- Compares \f$ x \f$ to \f$ y - s \f$.
- \param x : x value.
- \param y : y value.
- \param s : Tolerance threshold.
- \return true if \f$ x > y - s \f$.
-*/
-bool vpMath::greater(double x, double y, double s)
-{
- return(x>(y-s));
-}
-
-/*!
-
- Compute sinus cardinal \f$ \frac{sin(x)}{x} \f$.
-
- \param x : Value of x.
-
- \return Sinus cardinal.
-
-*/
-double vpMath::sinc(double x)
-{
- if (fabs(x) < ang_min_sinc) return 1.0 ;
- else return sin(x)/x ;
-}
-/*!
-
- Compute sinus cardinal \f$ \frac{sin(x)}{x}\f$.
-
- \param sinx : Value of sin(x).
- \param x : Value of x.
-
- \return Sinus cardinal.
-
-*/
-double vpMath::sinc(double sinx, double x)
-{
- if (fabs(x) < ang_min_sinc) return 1.0 ;
- else return (sinx/x) ;
-}
-
-/*!
- Compute \f$ (1-cos(x))/x^2 \f$
-
- \param cosx : Value of cos(x).
- \param x : Value of x.
-
- \return \f$ (1-cosx)/x^2 \f$
-
-*/
-double vpMath::mcosc(double cosx, double x)
-{
- if (fabs(x) < ang_min_mc) return 0.5 ;
- else return ((1.0-cosx)/x/x) ;
-}
-
-/*!
-
- Compute \f$ (1-sinc(x))/x^2 \f$ with \f$ sinc(x) = sinx / x \f$.
-
- \param sinx : value of sin(x).
- \param x : Value of x.
-
- \return \f$ (1-sinc(x))/x^2 \f$
-
-*/
-double vpMath::msinc(double sinx, double x)
-{
- if (fabs(x) < ang_min_mc) return (1./6.0) ;
- else return ((1.0-sinx/x)/x/x) ;
-}
-
-/*!
-
- Sigmoid function between [x0,x1] with \f$ s(x)=0 if x\le x0\f$ and \f$ s(x)=1 if x \ge x1 \f$
- \param x : Value of x.
- \param x0 : Lower bound (default 0).
- \param x1 : Upper bound (default 1).
- \param n : Degree of the exponential (default 12).
-
-\return \f$1/(1+exp(-n*((x-x0)/(x1-x0)-0.5)))\f$
- */
-double vpMath::sigmoid(double x, double x0,double x1, double n)
-{
- if(x < x0)
- return 0.;
- else if(x > x1)
- return 1.;
- double l0 = 1./(1.+exp(0.5*n));
- double l1 = 1./(1.+exp(-0.5*n));
- return (1./(1.+exp(-n*((x-x0)/(x1-x0)-0.5)))-l0)/(l1-l0);
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/misc/vpNoise.cpp b/src/math/misc/vpNoise.cpp
deleted file mode 100644
index 39f9ef7..0000000
--- a/src/math/misc/vpNoise.cpp
+++ /dev/null
@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNoise.cpp 4975 2014-11-17 15:59:42Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generation of random number with uniform and normal probability density.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpNoise.h>
-#include <math.h>
-
-/*!
- \file vpNoise.cpp
- \brief Class for generating random numbers with uniform and normal probability density.
-
- The algorithms and notations used are described in \cite Gentle:2004.
-
-*/
-
-
-/*!
- Minimal random number generator of Park and Miller \cite Park:1988. Returns a
- uniform random deviate between 0.0 and 1.0.
-
-*/
-inline void
-vpUniRand::draw0()
-//minimal standard
-//Park and Miller random number generator
-{
- /*unsigned*/ long k= x/q;//temp value for computing without overflow
- x = a*(x-k*q)-k*r;
- if (x < 0) x += m; //compute x without overflow
-}
-
-/*!
- Bays-Durham Shuffling of Park-Miller generator.
-
- Minimal random number generator of Park and Miller with Bays-Durham
- shuffle. Returns a uniform random deviate between 0.0 and 1.0 (exclusive of
- the endpoint values).
-*/
-double
-vpUniRand::draw1()
-{
- const long ntab = 33; //we work on a 32 elements array.
- //the 33rd one is actually the first value of y.
- const long modulo = ntab-2;
-
- static long y = 0;
- static long T[ntab];
-
- long j; //index of T
-
- //step 0
- if (!y) { //first time
- for(j = 0; j < ntab; j++) {
- draw0();
- T[j]=x;
- } //compute table T
- y=T[ntab-1];
- }
-
- //step 1
- j = y & modulo; //compute modulo ntab+1 (the first element is the 0th)
-
- //step 3
- y=T[j];
- double ans = (double)y/normalizer;
-
- //step 4
- //generate x(k+i) and set y=x(k+i)
- draw0();
-
- //refresh T[j];
- T[j]=x;
-
- return ans;
-}
-
-/*!
- Generate a normal random variable using the Box-Muller generator.
-
- Generate a normal random variable with mean 0 and standard deviation of 1.
- To adjust to some other distribution, multiply by the standard deviation and
- add the mean. Box-Muller method
-*/
-double
-vpGaussRand::gaussianDraw()
-{
- double v1, v2, rsq;
- static bool AlreadyDone = false;
- static double x2;
-
- if (AlreadyDone) {
- AlreadyDone=false;
- return x2;
- }
-
- else {
-
- do {
- v1=2*draw1()-1;
- v2=2*draw1()-1;
- rsq=v1*v1+v2*v2;
- } while (rsq >= 1);
-
- double fac=sqrt(-2*log(rsq)/rsq);
- x2=v2*fac;
- AlreadyDone=true;
- return v1*fac;
- }
-}
-
diff --git a/src/math/misc/vpNoise.h b/src/math/misc/vpNoise.h
deleted file mode 100644
index bb0e058..0000000
--- a/src/math/misc/vpNoise.h
+++ /dev/null
@@ -1,219 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNoise.h 5179 2015-01-16 09:42:33Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generation of random number with uniform and normal probability density.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpNoise_hh
-#define vpNoise_hh
-
-
-/*!
- \file vpNoise.h
- \brief Class for generating random number
- with uniform and normal probability density
-
- The algorithms and notations used are described in \cite Gentle:2004.
-
- */
-
-#include <visp/vpConfig.h>
-
-
-/*!
- \class vpUniRand
-
- \ingroup Random
- \brief Class for generating random numbers with uniform probability density.
-
- The algorithms and notations used are described in \cite Gentle:2004.
-
- */
-class VISP_EXPORT vpUniRand
-{
- /*unsigned*/ long a ;
- /*unsigned*/ long m ; //2^31-1
- /*unsigned*/ long q ; //integer part of m/a
- /*unsigned*/ long r ;//r=m mod a
- double normalizer ; //we use a normalizer > m to ensure ans will never be 1 (it is the case if x = 739806647)
-
- private:
- void draw0();
- protected:
- long x;
- double draw1();
-
- public:
- vpUniRand(const long seed = 0)
- : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), x((seed)? seed : 739806647)
- {
- }
- virtual ~vpUniRand() {};
-
- double operator()() {return draw1();}
-
-};
-
-/*!
- \class vpGaussRand
- \ingroup Random
- \brief Class for generating random number with normal probability density.
-
- The algorithms and notations used are described in \cite Gentle:2004.
-
- The code below shows how to use the random generator to get values that have their mean equal to
- 10 with a standart deviation equal to 0.5.
-
- \code
-#include <iostream>
-#include <visp/vpNoise.h>
-
-int main()
-{
- vpGaussRand noise(0.5, 10);
- for(int i=0; i< 10; i++) {
- std::cout << "noise " << i << ": " << noise() << std::endl;
- }
- return 0;
-}
- \endcode
-
- The previous example produces the following printings:
-\code
-noise 0: 9.43873
-noise 1: 10.1977
-noise 2: 10.8145
-noise 3: 9.13729
-noise 4: 8.86476
-noise 5: 9.83382
-noise 6: 9.43609
-noise 7: 9.34311
-noise 8: 9.62742
-noise 9: 9.37701
-\endcode
-
- Note that the previous example produces always the same "random" results. To produce real random
- values, you need to initialize the random generator with different values using seed(). For example,
- this could be done using the current time. The code becomes:
-
-\code
-#include <iostream>
-#include <visp/vpNoise.h>
-#include <visp/vpTime.h>
-
-int main()
-{
- vpGaussRand noise(0.5, 10);
- long seed = (long)vpTime::measureTimeMs();
-
- noise.seed(seed);
- for(int i=0; i< 10; i++) {
- std::cout << "noise " << i << ": " << noise() << std::endl;
- }
- return 0;
-}
-\endcode
-
- Now if you run the previous example you will always get different values:
- \code
-noise 0: 10.5982
-noise 1: 9.19111
-noise 2: 9.82498
-noise 3: 9.07857
-noise 4: 9.9285
-noise 5: 10.3688
-noise 6: 9.75621
-noise 7: 10.3259
-noise 8: 10.4238
-noise 9: 10.2391
- \endcode
- */
-class VISP_EXPORT vpGaussRand : public vpUniRand
-{
- private :
- double mean;
- double sigma;
- double gaussianDraw();
-
- public:
-
- /*!
- Default noise generator constructor.
- */
- vpGaussRand() : vpUniRand(), mean(0), sigma(0) {}
-
- /*!
- Gaussian noise random generator constructor.
-
- \param sigma_val : Standard deviation.
- \param mean_val : Mean value.
- \param noise_seed : Seed of the noise
- */
- vpGaussRand(const double sigma_val, const double mean_val, const long noise_seed = 0)
- : vpUniRand(noise_seed), mean(mean_val), sigma(sigma_val) {}
-
- /*!
- Set the standard deviation and mean for gaussian noise.
-
- \param sigma_val : New standard deviation sigma.
- \param mean_val : New mean value.
- */
- void setSigmaMean(const double sigma_val, const double mean_val) {
- this->mean = mean_val;
- this->sigma = sigma_val;
- }
-
- /*!
- Set the seed of the noise.
-
- \param seed_val : New seed.
- */
- void seed(const long seed_val) {
- x=seed_val;
- }
-
- /*!
- Return a random value from the Gaussian noise generator.
- */
- double operator()() {
- return sigma*gaussianDraw()+mean;
- }
-};
-
-#endif
diff --git a/src/math/robust/vpRansac.h b/src/math/robust/vpRansac.h
deleted file mode 100644
index b3029d9..0000000
--- a/src/math/robust/vpRansac.h
+++ /dev/null
@@ -1,259 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRansac.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Ransac robust algorithm.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpRansac.h
-
- template class for
-*/
-
-#ifndef vpRANSAC_HH
-#define vpRANSAC_HH
-
-
-
-#include <visp/vpNoise.h> // random number generation
-#include <visp/vpDebug.h> // debug and trace
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-#include <ctime>
-/*!
- \class vpRansac
- \ingroup Robust
-
- \brief This class is a generic implementation of the Ransac algorithm. It
- cannot be used alone.
-
- RANSAC is described in \cite Fischler81 and \cite Hartley01a.
-
- The code of this class is inspired by :
- Peter Kovesi
- School of Computer Science & Software Engineering
- The University of Western Australia
- pk at csse uwa edu au
- http://www.csse.uwa.edu.au/~pk
-
- \sa vpHomography
-
- */
-template <class vpTransformation>
-class vpRansac
-{
-public:
- static bool ransac(unsigned int npts,
- vpColVector &x,
- unsigned int s, double t,
- vpColVector &model,
- vpColVector &inliers,
- int consensus = 1000,
- double areaThreshold = 0.0,
- const int maxNbumbersOfTrials = 10000);
-};
-
-/*!
- \brief
- RANSAC - Robustly fits a model to data with the RANSAC algorithm
-
- \param npts : The number of data points.
-
- \param x : Data sets to which we are seeking to fit a model M It is assumed
- that x is of size [d x Npts] where d is the dimensionality of the data and
- npts is the number of data points.
-
- \param s : The minimum number of samples from x required by fitting fn to
- fit a model. Value should be greater or equal to 4.
-
- \param t : The distance threshold between data point and the model used to
- decide whether a point is an inlier or not.
-
- \param M : The model having the greatest number of inliers.
-
- \param inliers : An array of indices of the elements of x that were the
- inliers for the best model.
-
- \param consensus : Consensus
-
- \param areaThreshold : Not used.
-
- \param maxNbumbersOfTrials : Maximum number of trials. Even if a solution is
- not found, the method is stopped.
-
-*/
-
-template <class vpTransformation>
-bool
-vpRansac<vpTransformation>::ransac(unsigned int npts, vpColVector &x,
- unsigned int s, double t,
- vpColVector &M,
- vpColVector &inliers,
- int consensus,
- double /* areaThreshold */,
- const int maxNbumbersOfTrials)
-{
- /* bool isplanar; */
- /* if (s == 4) isplanar = true; */
- /* else isplanar = false; */
-
- double eps = 1e-6 ;
- double p = 0.99; // Desired probability of choosing at least one sample
- // free from outliers
-
- int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
- int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
- // data set.
-
- if (s<4)
- s = 4;
-
- // Sentinel value allowing detection of solution failure.
- bool solutionFind = false ;
- vpColVector bestM ;
- int trialcount = 0;
- int bestscore = -1;
- double N = 1; // Dummy initialisation for number of trials.
-
- vpUniRand random((const long)time(NULL)) ;
- vpColVector bestinliers ;
- unsigned int *ind = new unsigned int [s] ;
- int numiter = 0;
- int ninliers = 0;
- double residual = 0.0;
- while(( N > trialcount) && (consensus > bestscore))
- {
- // Select at random s datapoints to form a trial model, M.
- // In selecting these points we have to check that they are not in
- // a degenerate configuration.
-
- bool degenerate = true;
- int count = 1;
-
- while ( degenerate == true)
- {
- // Generate s random indicies in the range 1..npts
- for (unsigned int i=0 ; i < s ; i++)
- ind[i] = (unsigned int)ceil(random()*npts) -1;
-
- // Test that these points are not a degenerate configuration.
- degenerate = vpTransformation::degenerateConfiguration(x,ind) ;
- // degenerate = feval(degenfn, x(:,ind));
-
- // Safeguard against being stuck in this loop forever
- count = count + 1;
-
- if (count > maxDataTrials) {
- delete [] ind;
- vpERROR_TRACE("Unable to select a nondegenerate data set");
- throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
- //return false; //Useless after a throw() function
- }
- }
- // Fit model to this random selection of data points.
- vpTransformation::computeTransformation(x,ind, M);
-
- vpColVector d ;
- // Evaluate distances between points and model.
- vpTransformation::computeResidual(x, M, d) ;
-
- // Find the indices of points that are inliers to this model.
- residual = 0.0;
- ninliers =0 ;
- for (unsigned int i=0 ; i < npts ; i++)
- {
- double resid = fabs(d[i]);
- if (resid < t)
- {
- inliers[i] = 1 ;
- ninliers++ ;
- residual += fabs(d[i]);
- }
- else inliers[i] = 0;
- }
-
- if (ninliers > bestscore) // Largest set of inliers so far...
- {
- bestscore = ninliers; // Record data for this model
- bestinliers = inliers;
- bestM = M;
- solutionFind = true ;
-
- // Update estimate of N, the number of trials to ensure we pick,
- // with probability p, a data set with no outliers.
-
- double fracinliers = (double)ninliers / (double)npts;
-
- double pNoOutliers = 1 - pow(fracinliers,static_cast<int>(s));
-
- pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
- pNoOutliers = vpMath::minimum(1-eps, pNoOutliers);// Avoid division by 0.
- N = (log(1-p)/log(pNoOutliers));
- }
-
- trialcount = trialcount+1;
- // Safeguard against being stuck in this loop forever
- if (trialcount > maxTrials)
- {
- vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
- break ;
- }
- numiter++;
- }
-
- if (solutionFind==true) // We got a solution
- {
- M = bestM;
- inliers = bestinliers;
- }
- else
- {
- vpTRACE("ransac was unable to find a useful solution");
- M = 0;
- }
-
- if(ninliers > 0)
- residual /= ninliers;
- delete [] ind;
-
- return true;
-}
-
-
-#endif
diff --git a/src/math/robust/vpRobust.cpp b/src/math/robust/vpRobust.cpp
deleted file mode 100644
index 86c0aab..0000000
--- a/src/math/robust/vpRobust.cpp
+++ /dev/null
@@ -1,1015 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobust.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * M-Estimator and various influence function.
- *
- * Authors:
- * Andrew Comport
- * Jean Laneurit
- *
- *****************************************************************************/
-
-/*!
- \file vpRobust.cpp
-*/
-
-
-
-
-#include <visp/vpDebug.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-#include <visp/vpRobust.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#define vpITMAX 100
-#define vpEPS 3.0e-7
-#define vpCST 1
-
-
-// ===================================================================
-/*!
- \brief Constructor.
- \param n_data : Size of the data vector.
-
-*/
-vpRobust::vpRobust(unsigned int n_data)
- : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(n_data)
-{
- vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
-
- normres.resize(n_data);
- sorted_normres.resize(n_data);
- sorted_residues.resize(n_data);
- // NoiseThreshold=0.0017; //Can not be more accurate than 1 pixel
-}
-
-/*!
- \brief Resize containers.
- \param n_data : size of input data vector.
-
-*/
-void vpRobust::resize(unsigned int n_data){
-
- if(n_data!=size){
- normres.resize(n_data);
- sorted_normres.resize(n_data);
- sorted_residues.resize(n_data);
- size=n_data;
- }
-
-}
-
-// ===================================================================
-/*!
-
- \brief Calculate an Mestimate given a particular loss function using MAD
- (Median Absolute Deviation) as a scale estimate at each iteration.
-
- \pre Requires a column vector of residues.
-
- \post Keeps a copy of the weights so that rejected points are kept at zero
- weight.
-
- \param method : Type of M-Estimator \f$\rho(r_i)\f$:
-
- - TUKEY : \f$ \rho(r_i, C) = \left\{
- \begin{array}{ll}
- \frac{r_i^6}{6} - \frac{C^2r_i^4}{2} +\frac{C^4r_i^2}{2} & \mbox{if} |r_i| < C \\
- \frac{1}{6} C^6 & \mbox{else} \end{array}
- \right.
- \f$
- with influence function \f$ \psi(r_i, C) = \left\{
- \begin{array}{ll} r_i(r_i^2-C^2)^2 & \mbox{if} |r_i| < C \\ 0 & \mbox{else} \end{array} \right. \f$ where \f$C=4.7 \hat{\sigma} \f$ and with \f$ \hat{\sigma} = 1.48{Med}_i(|r_i - {Med}_j(r_j)|) \f$
-
- - CAUCHY :
-
- - HUBER :
-
- \param residues : Residues \f$ r_i \f$ used in the previous formula.
-
- \param weights : Vector of weights \f$w_i =
- \frac{\psi(r_i)}{r_i}\f$. Values are in [0, 1]. A value near zero
- means that the data is an outlier. This vector must have the same size
- residue vector.
-
- \return Returns a Column Vector of weights associated to each residue.
- */
-
-// ===================================================================
-void vpRobust::MEstimator(const vpRobustEstimatorType method,
- const vpColVector &residues,
- vpColVector &weights)
-{
-
- double med=0; // median
- double normmedian=0; // Normalized median
- double sigma=0;// Standard Deviation
-
- // resize vector only if the size of residue vector has changed
- unsigned int n_data = residues.getRows();
- resize(n_data);
-
- sorted_residues = residues;
-
- unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
-
- // Calculate median
- med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
- //residualMedian = med ;
-
- // Normalize residues
- for(unsigned int i=0; i<n_data; i++)
- {
- normres[i] = (fabs(residues[i]- med));
- sorted_normres[i] = (fabs(sorted_residues[i]- med));
-
- }
-
- // Calculate MAD
- normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
- //normalizedResidualMedian = normmedian ;
- // 1.48 keeps scale estimate consistent for a normal probability dist.
- sigma = 1.4826*normmedian; // median Absolute Deviation
-
- // Set a minimum threshold for sigma
- // (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
- }
-
- switch (method)
- {
- case TUKEY :
- {
- psiTukey(sigma, normres,weights);
-
- vpCDEBUG(2) << "Tukey's function computed" << std::endl;
- break ;
-
- }
- case CAUCHY :
- {
- psiCauchy(sigma, normres,weights);
- break ;
- }
- /* case MCLURE :
- {
- psiMcLure(sigma, normres);
- break ;
- }*/
- case HUBER :
- {
- psiHuber(sigma, normres,weights);
- break ;
- }
- }
-}
-
-
-
-void vpRobust::MEstimator(const vpRobustEstimatorType method,
- const vpColVector &residues,
- const vpColVector& all_residues,
- vpColVector &weights)
-{
-
-
- double normmedian=0; // Normalized median
- double sigma=0;// Standard Deviation
-
- unsigned int n_all_data = all_residues.getRows();
- vpColVector all_normres(n_all_data);
-
- // compute median with the residues vector, return all_normres which are the normalized all_residues vector.
- normmedian = computeNormalizedMedian(all_normres,residues,all_residues,weights);
-
-
- // 1.48 keeps scale estimate consistent for a normal probability dist.
- sigma = 1.4826*normmedian; // Median Absolute Deviation
-
- // Set a minimum threshold for sigma
- // (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
- }
-
-
- switch (method)
- {
- case TUKEY :
- {
- psiTukey(sigma, all_normres,weights);
-
- vpCDEBUG(2) << "Tukey's function computed" << std::endl;
- break ;
-
- }
- case CAUCHY :
- {
- psiCauchy(sigma, all_normres,weights);
- break ;
- }
- /* case MCLURE :
- {
- psiMcLure(sigma, all_normres);
- break ;
- }*/
- case HUBER :
- {
- psiHuber(sigma, all_normres,weights);
- break ;
- }
-
-
- };
-}
-
-
-
-double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
- const vpColVector &residues,
- const vpColVector &all_residues,
- const vpColVector & weights
- )
-{
- double med=0;
- double normmedian=0;
-
- unsigned int n_all_data = all_residues.getRows();
- unsigned int n_data = residues.getRows();
-
- // resize vector only if the size of residue vector has changed
- resize(n_data);
-
- sorted_residues = residues;
- vpColVector no_null_weight_residues;
- no_null_weight_residues.resize(n_data);
-
- //all_normres.resize(n_all_data); // Normalized Residue
- //vpColVector sorted_normres(n_data); // Normalized Residue
- //vpColVector sorted_residues = residues;
- //vpColVector sorted_residues;
-
-
- unsigned int index =0;
- for(unsigned int j=0;j<n_data;j++)
- {
- //if(weights[j]!=0)
- if(std::fabs(weights[j]) > std::numeric_limits<double>::epsilon())
- {
- no_null_weight_residues[index]=residues[j];
- index++;
- }
- }
- sorted_residues.resize(index);
- memcpy(sorted_residues.data,no_null_weight_residues.data,index*sizeof(double));
- n_data=index;
-
- vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
- << std::endl;
-
- // Calculate Median
- // Be careful to not use the rejected residues for the
- // calculation.
-
- unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
- med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
-
- unsigned int i;
- // Normalize residues
- for(i=0; i<n_all_data; i++)
- {
- all_normres[i] = (fabs(all_residues[i]- med));
- }
-
- for(i=0; i<n_data; i++)
- {
- sorted_normres[i] = (fabs(sorted_residues[i]- med));
- }
- // MAD calculated only on first iteration
-
- //normmedian = Median(normres, weights);
- //normmedian = Median(normres);
- normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
-
- return normmedian;
-}
-
-// ===================================================================
-/*!
- * \brief Calculate an Mestimate with a simultaneous scale estimate
- * using HUBER's influence function
- * \pre Requires a column vector of residues
- * \post None
- * \return Returns a Column Vector of weights associated to each residue
- */
-// ===================================================================
-vpColVector
-vpRobust::simultMEstimator(vpColVector &residues)
-{
-
- double med=0; // Median
- double normmedian=0; // Normalized Median
- double sigma=0; // Standard Deviation
-
- unsigned int n_data = residues.getRows();
- vpColVector norm_res(n_data); // Normalized Residue
- vpColVector w(n_data);
-
- vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
- << std::endl;
-
- // Calculate Median
- unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
- med = select(residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
-
- // Normalize residues
- for(unsigned int i=0; i<n_data; i++)
- norm_res[i] = (fabs(residues[i]- med));
-
- // Check for various methods.
- // For Huber compute Simultaneous scale estimate
- // For Others use MAD calculated on first iteration
- if(it==0)
- {
- normmedian = select(norm_res, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
- // 1.48 keeps scale estimate consistent for a normal probability dist.
- sigma = 1.4826*normmedian; // Median Absolute Deviation
- }
- else
- {
- // compute simultaneous scale estimate
- sigma = simultscale(residues);
- }
-
- // Set a minimum threshold for sigma
- // (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
- }
-
- vpCDEBUG(2) << "MAD and C computed" << std::endl;
-
- psiHuber(sigma, norm_res,w);
-
- sig_prev = sigma;
-
- return w;
-}
-
-double
-vpRobust::scale(vpRobustEstimatorType method, vpColVector &x)
-{
- unsigned int p = 6; //Number of parameters to be estimated.
- unsigned int n = x.getRows();
- double sigma2=0;
- /* long */ double Expectation=0;
- /* long */ double Sum_chi=0;
- /* long*/ double chiTmp =0;
-
- for(unsigned int i=0; i<n; i++)
- {
-
- chiTmp = constrainedChi(method, x[i]);
- Expectation += chiTmp*(1-erf(chiTmp));
- Sum_chi += chiTmp;
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
- std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
- std::cout << "x[i] = " << x[i] <<std::endl;
- std::cout << "chi = " << chiTmp << std::endl;
- std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
- std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
- //getchar();
- }
-#endif
-#endif
- }
-
-
- sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
- std::cout << "Expectation = " << Expectation << std::endl;
- std::cout << "Sum chi = " << Sum_chi << std::endl;
- std::cout << "sig_prev" << sig_prev << std::endl;
- std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
- }
-#endif
-#endif
-
- return sqrt(fabs(sigma2));
-
-}
-
-
-double
-vpRobust::simultscale(vpColVector &x)
-{
- unsigned int p = 6; //Number of parameters to be estimated.
- unsigned int n = x.getRows();
- double sigma2=0;
- /* long */ double Expectation=0;
- /* long */ double Sum_chi=0;
- /* long */ double chiTmp =0;
-
- for(unsigned int i=0; i<n; i++)
- {
-
- chiTmp = simult_chi_huber(x[i]);
- Expectation += chiTmp*(1-erf(chiTmp));
- Sum_chi += chiTmp;
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
- std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
- std::cout << "x[i] = " << x[i] <<std::endl;
- std::cout << "chi = " << chiTmp << std::endl;
- std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
- std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
- //getchar();
- }
-#endif
-#endif
- }
-
-
- sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
- std::cout << "Expectation = " << Expectation << std::endl;
- std::cout << "Sum chi = " << Sum_chi << std::endl;
- std::cout << "sig_prev" << sig_prev << std::endl;
- std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
- }
-#endif
-#endif
-
- return sqrt(fabs(sigma2));
-
-}
-
-double
-vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
-{
- switch (method)
- {
- case TUKEY :
- return constrainedChiTukey(x);
- case CAUCHY :
- return constrainedChiCauchy(x);
- case HUBER :
- return constrainedChiHuber(x);
- };
-
- return -1;
-
-}
-
-
-double
-vpRobust::constrainedChiTukey(double x)
-{
- double sct=0;
- double a=4.7;
- double s=sig_prev;
- //double epsillon=0.5;
-
- if(fabs(x) <= 4.7*sig_prev)
- {
- //sct = (vpMath::sqr(s*a-x)*vpMath::sqr(s*a+x)*vpMath::sqr(x))/(s*vpMath::sqr(vpMath::sqr(a*vpMath::sqr(s))));
- sct = (vpMath::sqr(s*a)*x-s*vpMath::sqr(s*a)-x*vpMath::sqr(x))*(vpMath::sqr(s*a)*x+s*vpMath::sqr(s*a)-x*vpMath::sqr(x))/s*vpMath::sqr(vpMath::sqr(vpMath::sqr(s)))/vpMath::sqr(vpMath::sqr(a));
- }
- else
- sct = -1/s;
-
- return sct;
-}
-
-
-double
-vpRobust::constrainedChiCauchy(double x)
-{
- double sct = 0;
- //double u = x/sig_prev;
- double s = sig_prev;
- double b = 2.3849;
-
- sct = -1*(vpMath::sqr(x)*b)/(s*(vpMath::sqr(s*b)+vpMath::sqr(x)));
-
- return sct;
-}
-
-double
-vpRobust::constrainedChiHuber(double x)
-{
- double sct=0;
- double u = x/sig_prev;
- double c = 1.2107; //1.345;
-
- if(fabs(u) <= c)
- sct = vpMath::sqr(u);
- else
- sct = vpMath::sqr(c);
-
- return sct;
-}
-
-double
-vpRobust::simult_chi_huber(double x)
-{
- double sct=0;
- double u = x/sig_prev;
- double c = 1.2107; //1.345;
-
- if(fabs(u) <= c)
- {
- //sct = 0.5*vpMath::sqr(u);
- sct = vpMath::sqr(u);
- }
- else
- {
- //sct = 0.5*vpMath::sqr(c);
- sct = vpMath::sqr(c);
- }
-
- return sct;
-}
-
-/*!
- \brief calculation of Tukey's influence function
-
- \param sigma : sigma parameters
- \param x : normalized residue vector
- \param weights : weight vector
-*/
-
-void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
-{
-
- unsigned int n_data = x.getRows();
- double cst_const = vpCST*4.6851;
-
- for(unsigned int i=0; i<n_data; i++)
- {
- //if(sig==0 && weights[i]!=0)
- if(std::fabs(sig) <= std::numeric_limits<double>::epsilon() && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- weights[i]=1;
- continue;
- }
-
- double xi_sig = x[i]/sig;
-
- //if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
- if((std::fabs(xi_sig)<=(cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- weights[i] = vpMath::sqr(1-vpMath::sqr(xi_sig/cst_const));
- //w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
- }
- else
- {
- //Outlier - could resize list of points tracked here?
- weights[i] = 0;
- }
- }
-}
-
-/*!
- \brief calculation of Tukey's influence function
-
- \param sigma : sigma parameters
- \param x : normalized residue vector
- \param weights : weight vector
-*/
-void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
-{
- double c = 1.2107; //1.345;
- unsigned int n_data = x.getRows();
-
- for(unsigned int i=0; i<n_data; i++)
- {
- //if(weights[i]!=0)
- if(std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- double xi_sig = x[i]/sig;
- if(fabs(xi_sig)<=c)
- weights[i] = 1;
- else
- weights[i] = c/fabs(xi_sig);
- }
- }
-}
-
-/*!
- \brief calculation of Cauchy's influence function
-
- \param sigma : sigma parameters
- \param x : normalized residue vector
- \param weights : weight vector
-*/
-
-void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
-{
- unsigned int n_data = x.getRows();
- double const_sig = 2.3849*sig;
-
- //Calculate Cauchy's equation
- for(unsigned int i=0; i<n_data; i++)
- {
- weights[i] = 1/(1+vpMath::sqr(x[i]/(const_sig)));
-
- // If one coordinate is an outlier the other is too!
- // w[i] < 0.01 is a threshold to be set
- /*if(w[i] < 0.01)
- {
- if(i%2 == 0)
- {
- w[i+1] = w[i];
- i++;
- }
- else
- w[i-1] = w[i];
- }*/
- }
-}
-
-
-/*!
- \brief calculation of McLure's influence function
-
- \param sigma : sigma parameters
- \param x : normalized residue vector
- \param weights : weight vector
-*/
-void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
-{
- unsigned int n_data = r.getRows();
-
- //McLure's function
- for(unsigned int i=0; i<n_data; i++)
- {
- weights[i] = 1/(vpMath::sqr(1+vpMath::sqr(r[i]/sig)));
- //w[i] = 2*mad/vpMath::sqr((mad+r[i]*r[i]));//odobez
-
- // If one coordinate is an outlier the other is too!
- // w[i] < 0.01 is a threshold to be set
- /*if(w[i] < 0.01)
- {
- if(i%2 == 0)
- {
- w[i+1] = w[i];
- i++;
- }
- else
- w[i-1] = w[i];
- }*/
- }
-}
-
-
-/*!
- \brief partition function
- \param a : vector to be sorted
- \param l : first value to be considered
- \param r : last value to be considered
-*/
-int
-vpRobust::partition(vpColVector &a, int l, int r)
-{
- int i = l-1;
- int j = r;
- double v = a[(unsigned int)r];
-
- for (;;)
- {
- while (a[(unsigned int)++i] < v) ;
- while (v < a[(unsigned int)--j]) if (j == l) break;
- if (i >= j) break;
- exch(a[(unsigned int)i], a[(unsigned int)j]);
- }
- exch(a[(unsigned int)i], a[(unsigned int)r]);
- return i;
-}
-
-/*!
- \brief sort a part of a vector and select a value of this new vector
- \param a : vector to be sorted
- \param l : first value to be considered
- \param r : last value to be considered
- \param k : value to be selected
-*/
-double
-vpRobust::select(vpColVector &a, int l, int r, int k)
-{
- while (r > l)
- {
- int i = partition(a, l, r);
- if (i >= k) r = i-1;
- if (i <= k) l = i+1;
- }
- return a[(unsigned int)k];
-}
-
-
-double
-vpRobust::erf(double x)
-{
- return x < 0.0 ? -gammp(0.5,x*x) : gammp(0.5,x*x);
-}
-
-double
-vpRobust::gammp(double a, double x)
-{
- double gamser=0.,gammcf=0.,gln;
-
- if (x < 0.0 || a <= 0.0)
- std::cout << "Invalid arguments in routine GAMMP";
- if (x < (a+1.0))
- {
- gser(&gamser,a,x,&gln);
- return gamser;
- }
- else
- {
- gcf(&gammcf,a,x,&gln);
- return 1.0-gammcf;
- }
-}
-
-void
-vpRobust::gser(double *gamser, double a, double x, double *gln)
-{
- double sum,del,ap;
-
- *gln=gammln(a);
- if (x <= 0.0)
- {
- if (x < 0.0)
- std::cout << "x less than 0 in routine GSER";
- *gamser=0.0;
- return;
- }
- else
- {
- ap=a;
- del=sum=1.0/a;
- for (int n=1; n<=vpITMAX; n++)
- {
- ap += 1.0;
- del *= x/ap;
- sum += del;
- if (fabs(del) < fabs(sum)*vpEPS)
- {
- *gamser=sum*exp(-x+a*log(x)-(*gln));
- return;
- }
- }
- std::cout << "a too large, vpITMAX too small in routine GSER";
- return;
- }
-}
-
-void
-vpRobust::gcf(double *gammcf, double a, double x, double *gln)
-{
- double gold=0.0,g,fac=1.0,b1=1.0;
- double b0=0.0,anf,ana,an,a1,a0=1.0;
-
- *gln=gammln(a);
- a1=x;
- for (int n=1; n<=vpITMAX; n++)
- {
- an=(double) n;
- ana=an-a;
- a0=(a1+a0*ana)*fac;
- b0=(b1+b0*ana)*fac;
- anf=an*fac;
- a1=x*a0+anf*a1;
- b1=x*b0+anf*b1;
- //if (a1)
- if (std::fabs(a1) > std::numeric_limits<double>::epsilon())
- {
- fac=1.0/a1;
- g=b1*fac;
- if (fabs((g-gold)/g) < vpEPS)
- {
- *gammcf=exp(-x+a*log(x)-(*gln))*g;
- return;
- }
- gold=g;
- }
- }
- std::cout << "a too large, vpITMAX too small in routine GCF";
-}
-
-double
-vpRobust::gammln(double xx)
-{
- double x,tmp,ser;
- static double cof[6]={76.18009173,-86.50532033,24.01409822,
- -1.231739516,0.120858003e-2,-0.536382e-5};
-
- x=xx-1.0;
- tmp=x+5.5;
- tmp -= (x+0.5)*log(tmp);
- ser=1.0;
- for (int j=0; j<=5; j++)
- {
- x += 1.0;
- ser += cof[j]/x;
- }
- return -tmp+log(2.50662827465*ser);
-}
-
-
-
-// double
-// vpRobust::median(const vpColVector &v)
-// {
-// int i,j;
-// int inf, sup;
-// int n = v.getRows() ;
-// vpColVector infsup(n) ;
-// vpColVector eq(n) ;
-//
-// for (i=0;i<n;i++)
-// {
-// // We compute the number of elements superior to the current value (sup)
-// // the number of elements inferior (inf) to the current value and
-// // the number of elements equal to the current value (eq)
-// inf = sup = 0;
-// for (j=0;j<n;j++)
-// {
-// if (i != j)
-// {
-// if (v[i] <= v[j]) inf++;
-// if (v[i] >= v[j]) sup++;
-// if (v[i] == v[j]) eq[i]++;
-// }
-// }
-// // We compute then difference between inf and sup
-// // the median should be for |inf-sup| = 0 (1 if an even number of element)
-// // which means that there are the same number of element in the array
-// // that are greater and smaller that this value.
-// infsup[i] = abs(inf-sup);
-// }
-//
-// // seek for the smaller value of |inf-sup| (should be 0 or 1)
-// int imin = 0 ; // index of the median in the array
-// //double eqmax = 0 ; // count of equal values
-// // min cannot be greater than the number of element
-// double min = n;
-//
-// // number of medians
-// int mediancount = 0;
-// // array of medians
-// int *medianindex = new int[n];
-//
-// for (i=0; i<n; i++)
-// {
-// if(infsup[i] < min)
-// {
-// min = infsup[i];
-// imin = i ;
-//
-// //reset count of median values
-// mediancount=0;
-// medianindex[mediancount]=i;
-// }
-// else if(infsup[i]==min) //If there is another median
-// {
-// mediancount++;
-// medianindex[mediancount]=i;
-// }
-// }
-//
-// // Choose smalest data to be the median
-// /*for(i=0; i<mediancount+1; i++)
-// {
-// //Choose the value with the greatest count
-// if(eq[medianindex[i]] > eqmax)
-// {
-// eqmax = eq[medianindex[i]];
-// imin = medianindex[i];
-// }
-// //If we have identical counts
-// // Choose smalest data to be the median
-// //if(v[medianindex[i]] < v[imin])
-// // imin = medianindex[i];
-// }*/
-//
-// // return the median
-// delete []medianindex;
-// return(v[imin]);
-// }
-//
-// // Calculate median only for the residues which have
-// // not be rejected. i.e. weight=0
-// double
-// vpRobust::median(const vpColVector &v, vpColVector &weights)
-// {
-// int i,j;
-// int inf, sup;
-// int n = v.getRows() ;
-// vpColVector infsup(n) ;
-// vpColVector eq(n) ;
-//
-// for (i=0;i<n;i++)
-// {
-// if(weights[i]!=0)
-// {
-// // We compute the number of elements superior to the current value (sup)
-// // the number of elements inferior (inf) to the current value and
-// // the number of elements equal to the current value (eq)
-// inf = sup = 0;
-// for (j=0;j<n;j++)
-// {
-// if (weights[j]!=0 && i!=j)
-// {
-// if (v[i] <= v[j]) inf++;
-// if (v[i] >= v[j]) sup++;
-// if (v[i] == v[j]) eq[i]++;
-// }
-// }
-// // We compute then difference between inf and sup
-// // the median should be for |inf-sup| = 0 (1 if an even number of element)
-// // which means that there are the same number of element in the array
-// // that are greater and smaller that this value.
-// infsup[i] = abs(inf-sup);
-// }
-// }
-//
-// // seek for the smaller value of |inf-sup| (should be 0 or 1)
-// int imin = 0 ; // index of the median in the array
-// //double eqmax = 0 ; // count of equal values
-// // min cannot be greater than the number of element
-// double min = n;
-//
-// for (i=0; i<n; i++)
-// {
-// if(weights[i]!=0)
-// {
-// if(infsup[i] < min)
-// {
-// min = infsup[i];
-// imin = i ;
-// }
-// }
-// }
-//
-// // return the median
-// return(v[imin]);
-// }
-
-
-#undef vpITMAX
-#undef vpEPS
-#undef vpCST
diff --git a/src/math/robust/vpRobust.h b/src/math/robust/vpRobust.h
deleted file mode 100644
index 28f3c1d..0000000
--- a/src/math/robust/vpRobust.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobust.h 4796 2014-07-23 15:51:16Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * M-Estimator and various influence function.
- *
- * Authors:
- * Andrew Comport
- * Jean Laneurit
- *
- *****************************************************************************/
-
-/*!
- \file vpRobust.h
-*/
-
-
-
-#ifndef CROBUST_HH
-#define CROBUST_HH
-
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-
-/*!
- \class vpRobust
- \ingroup Robust
- \brief Contains an M-Estimator and various influence function.
-
- Supported methods: M-estimation, Tukey, Cauchy and Huber
-*/
-class VISP_EXPORT vpRobust
-{
-
-public:
- //! Enumeration of influence functions
- typedef enum
- {
- TUKEY,
- CAUCHY,
- // MCLURE,
- HUBER
- } vpRobustEstimatorType;
-
-private:
-
- //!Normalized residue
- vpColVector normres;
- //!Sorted normalized Residues
- vpColVector sorted_normres;
- //!Sorted residues
- vpColVector sorted_residues;
-
- //!Noise threshold
- double NoiseThreshold;
- //!
- double sig_prev;
- //!
- unsigned int it;
- //! Vairiable used in swap method
- double swap;
- //! Size of the containers
- unsigned int size;
-
-public:
-
- //!Default Constructor
- vpRobust(unsigned int n_data);
-
- //!Destructor
- virtual ~vpRobust(){};
-
- //!Resize containers for sort methods
- void resize(unsigned int n_data);
-
- //! Compute the weights according a residue vector and a PsiFunction
- void MEstimator(const vpRobustEstimatorType method,
- const vpColVector &residues,
- vpColVector &weights);
-
- //! Compute the weights according a residue vector and a PsiFunction
- void MEstimator(const vpRobustEstimatorType method,
- const vpColVector &residues,
- const vpColVector& all_residues,
- vpColVector &weights);
-
- //! Simult Mestimator
- vpColVector simultMEstimator(vpColVector &residues);
-
- //! Set iteration
- void setIteration(const unsigned int iter){it=iter;}
-
- /*!
- Set maximal noise threshold.
- \param noise_threshold : Maximal noise threshold.
- */
- inline void setThreshold(const double noise_threshold) {
- NoiseThreshold=noise_threshold;
- }
-
-//public :
-//double residualMedian ;
-//double normalizedResidualMedian ;
-// private:
-// double median(const vpColVector &x);
-// double median(const vpColVector &x, vpColVector &weights);
-
- private:
- //!Compute normalized median
- double computeNormalizedMedian(vpColVector &all_normres,
- const vpColVector &residues,
- const vpColVector &all_residues,
- const vpColVector &weights
- );
-
-
- //! Calculate various scale estimates
- double scale(vpRobustEstimatorType method, vpColVector &x);
- double simultscale(vpColVector &x);
-
-
- //---------------------------------
- // Partial derivative of loss function with respect to the residue
- //---------------------------------
- /** @name PsiFunctions */
- //@{
- //! Tuckey influence function
- void psiTukey(double sigma, vpColVector &x, vpColVector &w);
- //! Caucht influence function
- void psiCauchy(double sigma, vpColVector &x, vpColVector &w);
- //! McLure influence function
- void psiMcLure(double sigma, vpColVector &x, vpColVector &w);
- //! Huber influence function
- void psiHuber(double sigma, vpColVector &x, vpColVector &w);
- //@}
-
- //! Partial derivative of loss function
- //! with respect to the scale
- double simult_chi_huber(double x);
-
- //---------------------------------
- // Constrained Partial derivative of loss function with respect to the scale
- //---------------------------------
- /** @name Constrained Chi Functions */
- //@{
- //! Constrained Chi Function
- double constrainedChi(vpRobustEstimatorType method, double x);
- //! Constrained Chi Tukey Function
- double constrainedChiTukey(double x);
- //! Constrained Chi Cauchy Function
- double constrainedChiCauchy(double x);
- //! Constrained Chi Huber Function
- double constrainedChiHuber(double x);
- //@}
-
- //---------------------------------
- // Mathematic functions used to calculate the Expectation
- //---------------------------------
- /** @name Some math function */
- //@{
- double erf(double x);
- double gammp(double a, double x);
- void gser(double *gamser, double a, double x, double *gln);
- void gcf(double *gammcf, double a, double x, double *gln);
- double gammln(double xx);
- //@}
-
- /** @name Sort function */
- //@{
- //! Swap two value
- void exch(double &A, double &B){swap = A; A = B; B = swap;}
- //! Sort function using partition method
- int partition(vpColVector &a, int l, int r);
- //! Sort the vector and select a value in the sorted vector
- double select(vpColVector &a, int l, int r, int k);
- //@}
-};
-
-#endif
diff --git a/src/math/robust/vpScale.cpp b/src/math/robust/vpScale.cpp
deleted file mode 100644
index d8d4553..0000000
--- a/src/math/robust/vpScale.cpp
+++ /dev/null
@@ -1,306 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpScale.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
- *
- * Authors:
- * Andrew Comport
- *
- *****************************************************************************/
-
-/*!
- \file vpScale.cpp
-*/
-
-
-
-
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-#include <visp/vpScale.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#define DEBUG_LEVEL2 0
-
-
-
-
-//! Constructor
-vpScale::vpScale()
- : bandwidth(0.02), dimension(1), kernel_type(EPANECHNIKOV)
-{
-#if (DEBUG_LEVEL2)
- std::cout << "vpScale constructor reached" << std::endl;
-#endif
-#if (DEBUG_LEVEL2)
- std::cout << "vpScale constructor finished" << std::endl;
-#endif
-
-}
-
-//! Constructor
-vpScale::vpScale(double kernel_bandwidth, unsigned int dim, int type)
- : bandwidth(kernel_bandwidth), dimension(dim), kernel_type(type)
-
-{
-#if (DEBUG_LEVEL2)
- std::cout << "vpScale constructor reached" << std::endl;
-#endif
-#if (DEBUG_LEVEL2)
- std::cout << "vpScale constructor finished" << std::endl;
-#endif
-}
-
-//! Destructor
-vpScale::~vpScale()
-{
-}
-
-// Calculate the modes of the density for the distribution
-// and their associated errors
-double
-vpScale::MeanShift(vpColVector &error)
-{
-
- unsigned int n = error.getRows()/dimension;
- vpColVector density(n);
- vpColVector density_gradient(n);
- vpColVector mean_shift(n);
-
- unsigned int increment=1;
-
- // choose smallest error as start point
- unsigned int i=0;
- while(error[i]<0 && error[i]<error[i+1])
- i++;
-
- // Do mean shift until no shift
- while(increment >= 1 && i<n)
- {
- increment=0;
- density[i] = KernelDensity(error, i);
- density_gradient[i] = KernelDensityGradient(error, i);
- mean_shift[i]=vpMath::sqr(bandwidth)*density_gradient[i]/((dimension+2)*density[i]);
-
- double tmp_shift = mean_shift[i];
-
- // Do mean shift
- while(tmp_shift>0 && tmp_shift>error[i]-error[i+1])
- {
- i++;
- increment++;
- tmp_shift-=(error[i]-error[i-1]);
- }
- }
-
- return error[i];
-
-}
-
-// Calculate the density of each point in the error vector
-// Requires ordered set of errors
-double
-vpScale::KernelDensity(vpColVector &error, unsigned int position)
-{
-
- unsigned int n = error.getRows()/dimension;
- double density=0;
- double Ke = 1;
- unsigned int j=position;
-
- vpColVector X(dimension);
-
-
- // Use each error in the bandwidth to calculate
- // the local density of error i
- // First treat larger errors
- //while(Ke !=0 && j<=n)
- while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j<=n)
- {
- //Create vector of errors corresponding to each dimension of a feature
- for(unsigned int i=0; i<dimension; i++)
- {
- X[i]=(error[position]-error[j])/bandwidth;
- position++;
- j++;
- }
- position-=dimension; // reset position
-
- Ke = KernelDensity_EPANECHNIKOV(X);
- density+=Ke;
- }
-
- Ke = 1;
- j=position;
- // Then treat smaller errors
- //while(Ke !=0 && j>=dimension)
- while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j>=dimension)
- {
- //Create vector of errors corresponding to each dimension of a feature
- for(unsigned int i=0; i<dimension; i++)
- {
- X[i]=(error[position]-error[j])/bandwidth;
- position++;
- j--;
- }
- position-=dimension; // reset position
-
- Ke = KernelDensity_EPANECHNIKOV(X);
- density+=Ke;
- }
-
- density*=1/(n*bandwidth);
-
- return density;
-
-}
-
-double
-vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
-{
-
- unsigned int n = error.getRows()/dimension;
- double density_gradient=0;
- double sum_delta=0;
- double delta=0;
- int nx=0;
-
- double inside_kernel = 1;
- unsigned int j=position;
- // Use each error in the bandwidth to calculate
- // the local density gradient
- // First treat larger errors than current
- //while(inside_kernel !=0 && j<=n)
- while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j<=n)
- {
- delta = error[position]-error[j];
- if(vpMath::sqr(delta/bandwidth)<1)
- {
- inside_kernel = 1;
- sum_delta+=error[j]-error[position];
- j++;
- nx++;
- }
- else
- inside_kernel = 0;
- }
-
- inside_kernel = 1;
- j=position;
- // Then treat smaller errors than current
- //while(inside_kernel !=0 && j>=dimension)
- while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j>=dimension)
- {
- delta = error[position]-error[j];
- if(vpMath::sqr(delta/bandwidth)<1)
- {
- inside_kernel = 1;
- sum_delta+=error[j]-error[position];
- j--;
- nx++;
- }
- else
- inside_kernel = 0;
- }
-
- density_gradient = KernelDensityGradient_EPANECHNIKOV(sum_delta, n);
-
-
- return density_gradient;
-
-}
-
-
-//Epanechnikov_kernel for an d dimensional Euclidian space R^d
-double
-vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X)
-{
-
- double XtX= X*X;
- double c; // Volume of an n dimensional unit sphere
-
- switch (dimension)
- {
- case 1:
- c = 2;
- break;
- case 2:
- c = M_PI;
- break;
- case 3:
- c = 4*M_PI/3;
- break;
- default:
- std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
- exit(1);
- }
-
- if(XtX < 1)
- return 1/(2*c)*(dimension+2)*(1-XtX);
- else
- return 0;
-
-}
-
-
-//Epanechnikov_kernel for an d dimensional Euclidian space R^d
-double
-vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
-{
-
- double c; // Volume of an n dimensional unit sphere
-
- switch (dimension)
- {
- case 1:
- c = 2;
- break;
- case 2:
- c = M_PI;
- break;
- case 3:
- c = 4*M_PI/3;
- break;
- default:
- std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
- exit(1);
- }
-
- //return sumX*(dimension+2)/(n*pow(bandwidth, (double)dimension)*c*vpMath::sqr(bandwidth));
- return sumX*(dimension+2)/(n*bandwidth*c*vpMath::sqr(bandwidth));
-
-}
-
diff --git a/src/math/robust/vpScale.h b/src/math/robust/vpScale.h
deleted file mode 100644
index 97a60bc..0000000
--- a/src/math/robust/vpScale.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpScale.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
- *
- * Authors:
- * Andrew Comport
- *
- *****************************************************************************/
-
-/*!
- \file vpScale.h
-*/
-
-// =========================================================
-/*!
- * \brief Contains various estimators for scale.
- * \n Methods : Median Absolute Deviation (MAD),
- * MPDE, Mean shift kernel density estimation.
- * \author Andrew Comport
- * \date 24/10/03
- */
-// ==========================================================
-
-
-#ifndef VPSCALE_HH
-#define VPSCALE_HH
-
-#define EPANECHNIKOV 0
-
-#include <visp/vpColVector.h>
-#include <math.h>
-
-
-/*!
- \class vpScale
- \ingroup Robust
-
- \brief Contains different methods for estimating the robust scale of
- an error distribution.
-
- Methods of Median Absolute Deviation and Density Gradient estimation
- using the Mean Shift method.
-
- \author Andrew Comport
- \date 24/10/03
- */
-class VISP_EXPORT vpScale
-{
-
-private:
- double bandwidth;
- unsigned int dimension;
- int kernel_type;
-
-public:
-
- //! Constructor
- vpScale();
- vpScale(double kernel_bandwidth, unsigned int dim=1, int type=EPANECHNIKOV);
- //! Destructor
- virtual ~vpScale(void);
-
- double MeanShift(vpColVector &error);
- double KernelDensity(vpColVector &error, unsigned int position);
- double KernelDensityGradient(vpColVector &error, unsigned int position);
-
- double KernelDensity_EPANECHNIKOV(vpColVector &X);
- double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n);
-
-};
-
-#endif
diff --git a/src/math/spline/vpBSpline.cpp b/src/math/spline/vpBSpline.cpp
deleted file mode 100644
index c78e194..0000000
--- a/src/math/spline/vpBSpline.cpp
+++ /dev/null
@@ -1,533 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBSpline.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * This class implements the B-Spline
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpBSpline.h>
-#include <visp/vpDebug.h>
-
-/*!
- Basic constructor.
-
- The degree \f$ p \f$ of the B-Spline basis functions is set to 3 to
- compute cubic B-Spline.
-*/
-vpBSpline::vpBSpline()
- : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
- crossingPoints()
-{
-}
-
-/*!
- Copy constructor.
-
-*/
-vpBSpline::vpBSpline(const vpBSpline &bspline)
- : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
- crossingPoints()
-{
- controlPoints = bspline.controlPoints;
- knots = bspline.knots;
- p = bspline.p;
- crossingPoints = bspline.crossingPoints;
-}
-/*!
- Basic destructor.
-*/
-vpBSpline::~vpBSpline()
-{
-}
-
-/*!
- Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$ l_u \in [u_i, u_{i+1}[ \f$
-
- 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.
-
- \param l_u : The knot whose knot interval is seeked.
- \param l_p : Degree of the B-Spline basis functions.
- \param l_knots : The knot vector
-
- \return the number of the knot interval in which \f$ l_u \f$ lies.
-*/
-unsigned int
-vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
-{
- unsigned int m = (unsigned int)l_knots.size()-1;
-
- if(l_u > l_knots.back())
- {
- //vpTRACE("l_u higher than the maximum value in the knot vector : %lf",l_u);
- return((unsigned int)(m-l_p-1));
- }
-
- //if (l_u == l_knots.back())
- if (std::fabs(l_u - l_knots.back()) <= std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
- return((unsigned int)(m-l_p-1));
-
- double low = l_p;
- double high = m-l_p;
- double middle = (low+high)/2.0;
-
- while (l_u < l_knots[(unsigned int)vpMath::round(middle)] || l_u >= l_knots[(unsigned int)vpMath::round(middle+1)])
- {
- if(l_u < l_knots[(unsigned int)vpMath::round(middle)]) high = middle;
- else low = middle;
- middle = (low+high)/2.0;
- }
-
- return (unsigned int)middle;
-}
-
-
-/*!
- Find the knot interval in which the parameter \f$ u \f$ lies. Indeed \f$ u \in [u_i, u_{i+1}[ \f$
-
- 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$ u \f$ equal to 0.5 the method will retun 1.
- - For \f$ u \f$ equal to 2.5 the method will retun 3.
- - For \f$ u \f$ equal to 3 the method will retun 3.
-
- \param u : The knot whose knot interval is seeked.
-
- \return the number of the knot interval in which \f$ u \f$ lies.
-*/
-unsigned int
-vpBSpline::findSpan(double u)
-{
- return findSpan( u, p, knots);
-}
-
-
-/*!
- Compute the nonvanishing basis functions at \f$ l_u \f$ which is in the \f$ l_i \f$ th knot interval. All the basis functions are stored in an array such as :
-
- N = \f$ N_{l_i,0}(l_u) \f$, \f$ N_{l_i-1,1}(l_u) \f$, \f$ N_{l_i,1}(l_u) \f$, ... , \f$ N_{l_i-k,k}(l_u) \f$, ..., \f$ N_{l_i,k}(l_u) \f$, ... , \f$ N_{l_i-p,p}(l_u) \f$, ... , \f$ N_{l_i,p}(l_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 B-Spline basis functions.
- \param l_knots : The knot vector
-
- \return An array containing the nonvanishing basis functions at \f$ l_u \f$. The size of the array is \f$ l_p +1 \f$.
-*/
-vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots)
-{
- vpBasisFunction* N = new vpBasisFunction[l_p+1];
-
- N[0].value = 1.0;
-
- double *left = new double[l_p+1];
- double *right = new double[l_p+1];
- double saved = 0.0;
- double temp = 0.0;
-
- for(unsigned int j = 1; j <= l_p; j++)
- {
- left[j] = l_u - l_knots[l_i+1-j];
- right[j] = l_knots[l_i+j] - l_u;
- saved = 0.0;
-
- for (unsigned int r = 0; r < j; r++)
- {
- temp = N[r].value / (right[r+1]+left[j-r]);
- N[r].value = saved +right[r+1]*temp;
- saved = left[j-r]*temp;
- }
- N[j].value = saved;
- }
- for(unsigned int j = 0; j < l_p+1; j++)
- {
- N[j].i = l_i-l_p+j;
- N[j].p = l_p;
- N[j].u = l_u;
- N[j].k = 0;
- }
-
- delete[] left;
- delete[] right;
-
- return N;
-}
-
-
-/*!
- Compute the nonvanishing basis functions at \f$ u \f$. All the basis functions are stored in an array such as :
-
- N = \f$ N_{i,0}(u) \f$, \f$ N_{i-1,1}(u) \f$, \f$ N_{i,1}(u) \f$, ... , \f$ N_{i-k,k}(u) \f$, ..., \f$ N_{i,k}(u) \f$, ... , \f$ N_{i-p,p}(u) \f$, ... , \f$ N_{i,p}(u) \f$
-
- where i the number of the knot interval in which \f$ u \f$ lies.
-
- \param u : A real number which is between the extrimities of the knot vector
-
- \return An array containing the nonvanishing basis functions at \f$ u \f$. The size of the array is \f$ p +1 \f$.
-*/
-vpBasisFunction* vpBSpline::computeBasisFuns(double u)
-{
- unsigned int i = findSpan(u);
- return computeBasisFuns(u, i, p ,knots);
-}
-
-
-/*!
- Compute the nonzero basis functions and their derivatives until the \f$ l_der \f$ th derivative. All the functions are computed at l_u.
-
- \warning \f$ l_der \f$ must be under or equal \f$ l_p \f$.
-
- The result is given as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
-
- The formula to compute the kth derivative at \f$ u \f$ is :
-
- \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \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 B-Spline basis functions.
- \param l_der : The last derivative to be computed.
- \param l_knots : The knot vector
-
- \return the basis functions and their derivatives as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
-
- Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
-*/
-vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots)
-{
- vpBasisFunction** N;
- N = new vpBasisFunction*[l_der+1];
- for(unsigned int j = 0; j <= l_der; j++)
- N[j] = new vpBasisFunction[l_p+1];
-
- vpMatrix a(2,l_p+1);
- vpMatrix ndu(l_p+1,l_p+1);
- ndu[0][0] = 1.0;
-
- double *left = new double[l_p+1];
- double *right = new double[l_p+1];
- double saved = 0.0;
- double temp = 0.0;
-
- for(unsigned int j = 1; j <= l_p; j++)
- {
- left[j] = l_u - l_knots[l_i+1-j];
- right[j] = l_knots[l_i+j] - l_u;
- saved = 0.0;
-
- for (unsigned int r = 0; r < j; r++)
- {
- ndu[j][r] = right[r+1]+left[j-r];
- temp = ndu[r][j-1]/ndu[j][r];
- ndu[r][j] = saved + right[r+1]*temp;
- saved = left[j-r]*temp;
- }
- ndu[j][j] = saved;
- }
-
- for(unsigned int j = 0; j <= l_p; j++)
- {
- N[0][j].value = ndu[j][l_p];
- N[0][j].i = l_i-l_p+j;
- N[0][j].p = l_p;
- N[0][j].u = l_u;
- N[0][j].k = 0;
- }
-
- if( l_der > l_p)
- {
- vpTRACE("l_der must be under or equal to l_p");
- l_der = l_p;
- }
-
- unsigned int s1,s2;
- double d;
- int rk;
- unsigned int pk;
- unsigned int j1,j2;
-
- for (unsigned int r = 0; r <= l_p; r++)
- {
- s1 = 0;
- s2 = 1;
- a[0][0] = 1.0;
- for(unsigned int k = 1; k <= l_der; k++)
- {
- d = 0.0;
- rk = (int)(r-k);
- pk = l_p-k;
- if(r >= k)
- {
- a[s2][0] = a[s1][0]/ndu[pk+1][rk];
- d = a[s2][0]*ndu[(unsigned int)rk][pk];
- }
-
- if(rk >= -1)
- j1 = 1;
- else
- j1 = (unsigned int)(-rk);
-
- if(r-1 <= pk)
- j2 = k-1;
- else
- j2 = l_p-r;
-
- for(unsigned int j =j1; j<= j2; j++)
- {
- a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][(unsigned int)rk+j];
- d += a[s2][j]*ndu[(unsigned int)rk+j][pk];
- }
-
- if(r <= pk)
- {
- a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
- d += a[s2][k]*ndu[r][pk];
- }
- N[k][r].value = d;
- N[k][r].i = l_i-l_p+r;
- N[k][r].p = l_p;
- N[k][r].u = l_u;
- N[k][r].k = k;
-
- s1 = (s1+1)%2;
- s2 = (s2+1)%2;
- }
- }
-
- double r = l_p;
- for ( unsigned int k = 1; k <= l_der; k++ )
- {
- for (unsigned int j = 0; j <= l_p; j++)
- N[k][j].value *= r;
- r *= (l_p-k);
- }
-
- delete[] left;
- delete[] right;
-
- return N;
-}
-
-
-/*!
- Compute the nonzero basis functions and their derivatives until the \f$ der \f$ th derivative. All the functions are computed at u.
-
- \warning \f$ der \f$ must be under or equal \f$ p \f$.
-
- The result is given as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
-
- The formula to compute the kth derivative at \f$ u \f$ is :
-
- \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \param u : A real number which is between the extrimities of the knot vector
- \param der : The last derivative to be computed.
-
- \return the basis functions and their derivatives as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
-
- Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
-*/
-vpBasisFunction** vpBSpline::computeDersBasisFuns(double u, unsigned int der)
-{
- unsigned int i = findSpan(u);
- return computeDersBasisFuns(u, i, p , der, knots);
-}
-
-
-/*!
- Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_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 B-Spline basis functions.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
-
- return the coordinates of a point corresponding to the knot \f$ u \f$.
-*/
-vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
-{
- vpBasisFunction* N = computeBasisFuns(l_u, l_i, l_p, l_knots);
- vpImagePoint pt;
-
- double ic = 0;
- double jc = 0;
- for(unsigned int j = 0; j <= l_p; j++)
- {
- ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i();
- jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j();
- }
-
- pt.set_i(ic);
- pt.set_j(jc);
-
- delete[] N;
-
- return pt;
-}
-
-
-/*!
- Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_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 vpBSpline::computeCurvePoint(double u)
-{
- vpBasisFunction* N = computeBasisFuns(u);
- vpImagePoint pt;
-
- double ic = 0;
- double jc = 0;
- for(unsigned int j = 0; j <= p; j++)
- {
- ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i();
- jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j();
- }
-
- pt.set_i(ic);
- pt.set_j(jc);
-
- delete[] N;
-
- return pt;
-}
-
-
-/*!
- 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)P_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \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 B-Spline 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.
-
- \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* vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
-{
- vpImagePoint *derivate = new vpImagePoint[l_der+1];
- vpBasisFunction** N;
- N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
-
- unsigned int du;
- if (l_p < l_der)
- {
- vpTRACE("l_der must be under or equal to l_p");
- du = l_p;
- }
- else du = l_der;
-
- for(unsigned int k = 0; k <= du; k++)
- {
- derivate[k].set_ij(0.0,0.0);
- for(unsigned int j = 0; j<= l_p; j++)
- {
- derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i());
- derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j());
- }
- }
-
-
- for(unsigned int j = 0; j <= l_der; j++)
- delete[] N[j];
- delete[] N;
-
-
- return derivate;
-}
-
-
-/*!
- 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)P_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \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 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* vpBSpline::computeCurveDers(double u, unsigned int der)
-{
- vpImagePoint *derivate = new vpImagePoint[der+1];
- vpBasisFunction** N;
- N = computeDersBasisFuns(u, der);
-
- unsigned int du;
- if (p < der)
- {
- vpTRACE("der must be under or equal to p");
- du = p;
- }
- else du = der;
-
- for(unsigned int k = 0; k <= du; k++)
- {
- derivate[k].set_ij(0.0,0.0);
- for(unsigned int j = 0; j<= p; j++)
- {
- derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i());
- derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j());
- }
- }
-
- for(unsigned int j = 0; j <= der; j++)
- delete[] N[j];
- delete[] N;
-
- return derivate;
-}
diff --git a/src/math/spline/vpBSpline.h b/src/math/spline/vpBSpline.h
deleted file mode 100644
index c1c371f..0000000
--- a/src/math/spline/vpBSpline.h
+++ /dev/null
@@ -1,315 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBSpline.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * This class implements the B-Spline
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpBSpline_H
-#define vpBSpline_H
-
-/*!
- \file vpBSpline.h
- \brief Class that provides tools to compute and manipulate a B-Spline curve.
-*/
-
-#include <visp/vpImagePoint.h>
-
-#include <vector>
-#include <list>
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-#include <visp/vpMatrix.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
- Structure that defines a B-Spline basis function \f$ N_{i,p}^k(u) \f$.
-
- - i is the number of the knot interval in which the basis function is computed.
- - p is the degree of the B-Spline basis function.
- - u is the "point" point of the curve where the basis function is computed.
- - k indicates which kth derivative is computed.
- - value is the numerical value of \f$ N_{i,p}^k(u) \f$.
-*/
-typedef struct vpBasisFunction{
- unsigned int i;
- unsigned int p;
- double u;
- unsigned int k;
- double value;
-} vpBasisFunction;
-#endif
-
-/*!
- \class vpBSpline
- \ingroup MathTools
-
-
- \brief Class that provides tools to compute and manipulate a B-Spline curve.
-
- The different parameters are :
-
- - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1}, i = 0, ...,m \f$.
- To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
-
- - The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
- \f[ N_{i,0}(u) = \left\{\begin{array}{cc}
- 1 & \mbox{if } u_i \leq u \leq u_{i+1} \\ 0 & else
- \end{array}\right.\f]
-
- \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
-
- where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
-
- - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
-
- It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
- \f[ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)\f]
-
- You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
-*/
-
-class VISP_EXPORT vpBSpline
-{
- public/*protected*/:
- //!Vector wich contains the control points
- std::vector<vpImagePoint> controlPoints;
- //! Vector which contain the knots \f$ {u0, ..., um} \f$
- std::vector<double> knots;
- //! Degree of the B-Spline basis functions.
- unsigned int p;
- //! Vector wich contains the points used during the interpolation method.
- std::vector<vpImagePoint> crossingPoints;
-
- public:
-
- vpBSpline();
- vpBSpline(const vpBSpline &bspline);
- virtual ~vpBSpline();
-
- /*!
- Gets the degree of the B-Spline.
-
- \return the degree of the B-Spline.
- */
- inline unsigned int get_p() const {return p;}
-
- /*!
- Gets all the control points.
-
- \param list : A std::list containing the coordinates of the control points.
- */
- inline void get_controlPoints(std::list<vpImagePoint> &list) const {
- list.clear();
- for (unsigned int i = 0; i < controlPoints.size(); i++)
- list.push_back(*(&(controlPoints[0])+i));
- }
-
- /*!
- Gets all the knots.
-
- \param list : A std::list containing the value of the knots.
- */
- inline void get_knots(std::list<double> &list) const {
- list.clear();
- for (unsigned int i = 0; i < knots.size(); i++)
- list.push_back(*(&(knots[0])+i));
- }
-
- /*!
- Gets all the crossing points (used in the interpolation method)
-
- \param list : A std::list containing the coordinates of the crossing points.
- */
- inline void get_crossingPoints(std::list<vpImagePoint> &list) const {
- list.clear();
- for (unsigned int i = 0; i < crossingPoints.size(); i++)
- list.push_back(*(&(crossingPoints[0])+i));
- }
-
-
- /*!
- Sets the degree of the B-Spline.
-
- \param degree : the degree of the B-Spline.
- */
- inline void set_p(unsigned int degree) {this->p = degree;}
-
-
- /*!
- Sets all the control points.
-
- \param list : A std::list containing the coordinates of the control points
- */
- inline void set_controlPoints(const std::list<vpImagePoint> &list) {
- controlPoints.clear();
- for(std::list<vpImagePoint>::const_iterator it = list.begin(); it!=list.end(); ++it){
- controlPoints.push_back(*it);
- }
- }
-
- /*!
- Sets all the knots.
-
- \param list : A std::list containing the value of the knots.
- */
- inline void set_knots(const std::list<double> &list) {
- knots.clear();
- for(std::list<double>::const_iterator it = list.begin(); it!=list.end(); ++it){
- knots.push_back(*it);
- }
- }
-
- /*!
- Sets all the crossing points (used in the interpolation method)
-
- \param list : A std::list containing the coordinates of the crossing points
- */
- inline void set_crossingPoints(const std::list<vpImagePoint> &list) {
- crossingPoints.clear();
- for(std::list<vpImagePoint>::const_iterator it=list.begin(); it!=list.end(); ++it){
- crossingPoints.push_back(*it);
- }
- }
-
- static unsigned int findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots);
- unsigned int findSpan(double u);
-
- static vpBasisFunction* computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots);
- vpBasisFunction* computeBasisFuns(double u);
-
- static vpBasisFunction** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots);
- vpBasisFunction** computeDersBasisFuns(double u, unsigned int der);
-
- static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
- vpImagePoint computeCurvePoint(double u);
-
- static vpImagePoint* computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
- vpImagePoint* computeCurveDers(double u, unsigned int der);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- /*!
- \deprecated This method is deprecated. You should use get_controlPoints(std::list<vpImagePoint> &) const instead. \n \n
- Gets all the control points.
-
- \return list : A vpList containing the coordinates of the control points
- */
- vp_deprecated vpList<vpImagePoint> get_controlPoints() const {
- vpList<vpImagePoint> list;
- for (unsigned int i = 0; i < controlPoints.size(); i++) list.addRight(*(&(controlPoints[0])+i));
- return list; }
-
- /*!
- \deprecated This method is deprecated. You should use get_knots(std::list<double> &) const instead. \n \n
- Gets all the knots.
-
- \return list : A vpList containing the value of the knots.
- */
- vp_deprecated inline vpList<double> get_knots() const {
- vpList<double> list;
- for (unsigned int i = 0; i < knots.size(); i++) list.addRight(*(&(knots[0])+i));
- return list; }
-
- /*!
- \deprecated This method is deprecated. You should use get_crossingPoints(std::list<vpImagePoint> &) const instead. \n \n
- Gets all the crossing points (used in the interpolation method)
-
- \return list : A vpList containing the coordinates of the crossing points
- */
- vp_deprecated inline vpList<vpImagePoint> get_crossingPoints() const {
- vpList<vpImagePoint> list;
- for (unsigned int i = 0; i < crossingPoints.size(); i++) list.addRight(*(&(crossingPoints[0])+i));
- return list; }
-
- /*!
- \deprecated This method is deprecated. You should use set_controlPoints(const std::list<vpImagePoint> &) instead. \n \n
- Sets all the control points.
-
- \param list : A vpList containing the coordinates of the control points
- */
- vp_deprecated inline void set_controlPoints(vpList<vpImagePoint> &list) {
- controlPoints.clear();
- list.front();
- for (unsigned int i = 0; i < list.nbElements(); i++)
- {
- controlPoints.push_back(list.value());
- list.next();
- }
- }
-
- /*!
- \deprecated This method is deprecated. You should use set_knots(const std::list<double> &) instead. \n \n
- Sets all the knots.
-
- \param list : A vpList containing the value of the knots.
- */
- vp_deprecated inline void set_knots(vpList<double> &list) {
- knots.clear();
- list.front();
- for (unsigned int i = 0; i < list.nbElements(); i++)
- {
- knots.push_back(list.value());
- list.next();
- }
- }
-
- /*!
- \deprecated This method is deprecated. You should use set_crossingPoints(const std::list<vpImagePoint> &) instead. \n \n
- Sets all the crossing points (used in the interpolation method)
-
- \param list : A vpList containing the coordinates of the crossing points
- */
- vp_deprecated inline void set_crossingPoints(vpList<vpImagePoint> &list) {
- crossingPoints.clear();
- list.front();
- for (unsigned int i = 0; i < list.nbElements(); i++)
- {
- crossingPoints.push_back(list.value());
- list.next();
- }
- }
-#endif
-
-};
-
-#endif
diff --git a/src/math/spline/vpNurbs.cpp b/src/math/spline/vpNurbs.cpp
deleted file mode 100644
index 63ba0d0..0000000
--- a/src/math/spline/vpNurbs.cpp
+++ /dev/null
@@ -1,1134 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNurbs.cpp 5235 2015-01-30 13:51:21Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * This class implements the Non Uniform Rational B-Spline (NURBS)
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpNurbs.h>
-#include <visp/vpColVector.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-/*
- Compute the distance d = |Pw1-Pw2|
-*/
-inline double distance(const vpImagePoint &iP1, const double w1, const vpImagePoint &iP2, const double w2)
-{
- double distancei = iP1.get_i() - iP2.get_i();
- double distancej = iP1.get_j() - iP2.get_j();
- double distancew = w1 -w2;
- 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()
-{
- weights = nurbs.weights;
-}
-
-/*!
- Basic destructor
-*/
-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<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
-{
- vpBasisFunction* N = NULL;
- N = computeBasisFuns(l_u, l_i, l_p, l_knots);
- vpImagePoint pt;
-
- double ic = 0;
- double jc = 0;
- double wc = 0;
- for(unsigned int j = 0; j <= l_p; j++)
- {
- ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i() * l_weights[l_i-l_p+j];
- jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j() * l_weights[l_i-l_p+j];
- wc = wc + N[j].value * l_weights[l_i-l_p+j];
- }
-
- pt.set_i(ic/wc);
- pt.set_j(jc/wc);
-
- if (N != NULL) delete[] N;
-
- 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;
- N = computeBasisFuns(u);
- vpImagePoint pt;
-
- double ic = 0;
- double jc = 0;
- double wc = 0;
- for(unsigned int j = 0; j <= p; j++)
- {
- ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i() * weights[N[0].i+j]; //N[0].i = findSpan(u)-p
- jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j() * weights[N[0].i+j];
- wc = wc + N[j].value * weights[N[0].i+j];
- }
-
- pt.set_i(ic/wc);
- pt.set_j(jc/wc);
-
- if (N != NULL) delete[] N;
-
- 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<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
-{
- vpMatrix derivate(l_der+1,3);
- vpBasisFunction** N = NULL;
- N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
-
- for(unsigned int k = 0; k <= l_der; k++)
- {
- derivate[k][0] = 0.0;
- derivate[k][1] = 0.0;
- derivate[k][2] = 0.0;
-
- for(unsigned int j = 0; j<= l_p; j++)
- {
- derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
- derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
- derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
- }
- }
-
- if (N!=NULL) {
- for(unsigned int i = 0; i <= l_der; i++)
- delete [] N[i];
- delete[] N;
- }
- 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);
- vpBasisFunction** N = NULL;
- N = computeDersBasisFuns(u, der);
-
- for(unsigned int k = 0; k <= der; k++)
- {
- derivate[k][0] = 0.0;
- derivate[k][1] = 0.0;
- derivate[k][2] = 0.0;
- for(unsigned int j = 0; j<= p; j++)
- {
- derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
- derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
- derivate[k][2] = derivate[k][2] + N[k][j].value*(weights[N[0][0].i-p+j]);
- }
- }
-
- if (N!=NULL) delete[] N;
-
- 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<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
-{
- std::vector<vpImagePoint> A;
- vpImagePoint pt;
- for(unsigned int j = 0; j < l_controlPoints.size(); j++)
- {
- pt = l_controlPoints[j];
- pt.set_i(pt.get_i()*l_weights[j]);
- pt.set_j(pt.get_j()*l_weights[j]);
- A.push_back(pt);
- }
-
- vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights);
-
- vpImagePoint* CK = new vpImagePoint[l_der+1];
- double ic,jc;
-
- for(unsigned int k = 0; k <= l_der; k++)
- {
- ic = Awders[k][0];
- jc = Awders[k][1];
- for(unsigned int j = 1; j <= k; j++)
- {
- double tmpComb = static_cast<double>( vpMath::comb(k,j) );
- ic = ic - tmpComb*Awders[k][2]*(CK[k-j].get_i());
- jc = jc - tmpComb*Awders[j][2]*(CK[k-j].get_j());
- }
- CK[k].set_ij(ic/Awders[0][2],jc/Awders[0][2]);
- }
- 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
-{
- vpMatrix Rw(l_p+1,3);
- std::vector<vpImagePoint>::iterator it1;
- std::vector<double>::iterator it2;
- vpImagePoint pt;
- double w = 0;
-
- for (unsigned int j = 0; j <= l_p-l_s; j++)
- {
- Rw[j][0] = (l_controlPoints[l_k-l_p+j]).get_i() * l_weights[l_k-l_p+j];
- Rw[j][1] = (l_controlPoints[l_k-l_p+j]).get_j() * l_weights[l_k-l_p+j];
- Rw[j][2] = l_weights[l_k-l_p+j];
- }
-
- it1 = l_controlPoints.begin();
- l_controlPoints.insert(it1+(int)l_k-(int)l_s, l_r, pt);
- it2 = l_weights.begin();
- l_weights.insert(it2+(int)l_k-(int)l_s, l_r, w);
-
- unsigned int L=0;
- double alpha;
- for (unsigned int j = 1; j <= l_r; j++)
- {
- L = l_k - l_p +j;
-
- for (unsigned int i = 0; i <=l_p-j-l_s; i++)
- {
- alpha = (l_u - l_knots[L+i])/(l_knots[i+l_k+1] - l_knots[L+i]);
- Rw[i][0]= alpha*Rw[i+1][0]+(1.0-alpha)*Rw[i][0];
- Rw[i][1]= alpha*Rw[i+1][1]+(1.0-alpha)*Rw[i][1];
- Rw[i][2]= alpha*Rw[i+1][2]+(1.0-alpha)*Rw[i][2];
- }
-
- pt.set_ij(Rw[0][0]/Rw[0][2],Rw[0][1]/Rw[0][2]);
- l_controlPoints[L] = pt;
- l_weights[L] = Rw[0][2];
-
- pt.set_ij(Rw[l_p-j-l_s][0]/Rw[l_p-j-l_s][2],Rw[l_p-j-l_s][1]/Rw[l_p-j-l_s][2]);
- l_controlPoints[l_k+l_r-j-l_s] = pt;
- l_weights[l_k+l_r-j-l_s] = Rw[l_p-j-l_s][2];
- }
-
- for(unsigned int j = L+1; j < l_k-l_s; j++)
- {
- pt.set_ij(Rw[j-L][0]/Rw[j-L][2],Rw[j-L][1]/Rw[j-L][2]);
- l_controlPoints[j] = pt;
- l_weights[j] = Rw[j-L][2];
- }
-
- it2 = l_knots.begin();
- 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
-{
- unsigned int a = findSpan(l_x[0], l_p, l_knots);
- unsigned int b = findSpan(l_x[l_r], l_p, l_knots);
- b++;
-
- unsigned int n = (unsigned int)l_controlPoints.size();
- unsigned int m = (unsigned int)l_knots.size();
-
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
- }
-
- std::vector<double> l_knots_tmp(l_knots);
- std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
- std::vector<double> l_weights_tmp(l_weights);
-
- vpImagePoint pt;
- double w = 0;
-
- for (unsigned int j = 0; j <= l_r; j++)
- {
- l_controlPoints.push_back(pt);
- l_weights.push_back(w);
- l_knots.push_back(w);
- }
-
- for(unsigned int j = b+l_p; j <= m-1; j++) l_knots[j+l_r+1] = l_knots_tmp[j];
-
- for (unsigned int j = b-1; j <= n-1; j++)
- {
- l_controlPoints[j+l_r+1] = l_controlPoints_tmp[j];
- l_weights[j+l_r+1] = l_weights_tmp[j];
- }
-
- unsigned int i = b+l_p-1;
- unsigned int k = b+l_p+l_r;
-
- {
- unsigned int j = l_r + 1;
- do{
- j--;
- while(l_x[j] <= l_knots[i] && i > a)
- {
- l_controlPoints[k-l_p-1] = l_controlPoints_tmp[i-l_p-1];
- l_weights[k-l_p-1] = l_weights_tmp[i-l_p-1];
- l_knots[k] = l_knots_tmp[i];
- k--;
- i--;
- }
-
- l_controlPoints[k-l_p-1] = l_controlPoints[k-l_p];
- l_weights[k-l_p-1] = l_weights[k-l_p];
-
- for (unsigned int l = 1; l <= l_p; l++)
- {
- unsigned int ind = k-l_p+l;
- double alpha = l_knots[k+l] - l_x[j];
- //if (vpMath::abs(alpha) == 0.0)
- if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- {
- l_controlPoints[ind-1] = l_controlPoints[ind];
- l_weights[ind-1] = l_weights[ind];
- }
- 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());
- l_controlPoints[ind-1].set_j( alpha * l_controlPoints[ind-1].get_j() + (1.0-alpha) * l_controlPoints[ind].get_j());
- l_weights[ind-1] = alpha * l_weights[ind-1] + (1.0-alpha) * l_weights[ind];
- }
- }
- l_knots[k] = l_x[j];
- k--;
- }while(j != 0);
- }
-
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()/l_weights[j],l_controlPoints[j].get_j()/l_weights[j]);
- }
-}
-
-
-/*!
- 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
-{
- unsigned int n = (unsigned int)l_controlPoints.size();
- unsigned int m = n + l_p + 1;
-
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
- }
-
- unsigned int ord = l_p + 1;
- double fout = (2*l_r-l_s-l_p)/2.;
- unsigned int last = l_r - l_s;
- unsigned int first = l_r - l_p;
- unsigned int tblSize = 2*l_p+1;
- vpImagePoint *tempP = new vpImagePoint[tblSize];
- double *tempW = new double[tblSize];
- vpImagePoint pt;
- unsigned int t = 0;
- double alfi = 0;
- double alfj = 0;
- unsigned int i, j, ii, jj;
-
- for(t = 0; t < l_num; t++)
- {
- unsigned int off = first - 1;
- tempP[0] = l_controlPoints[off];
- tempW[0] = l_weights[off];
- tempP[last+1-off] = l_controlPoints[last+1];
- tempW[last+1-off] = l_weights[last+1];
- i = first;
- j = last;
- ii = 1;
- jj = last -off;
- int remflag = 0;
- while (j-i > t)
- {
- alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
- alfj = (l_u - l_knots[j-t])/(l_knots[j+ord]-l_knots[j-t]);
- pt.set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
- tempP[ii].set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
- tempP[ii].set_j((l_controlPoints[i].get_j()-(1.0-alfi)*tempP[ii-1].get_j())/alfi);
- tempW[ii] = ((l_weights[i]-(1.0-alfi)*tempW[ii-1])/alfi);
- tempP[jj].set_i((l_controlPoints[j].get_i()-alfj*tempP[jj+1].get_i())/(1.0-alfj));
- tempP[jj].set_j((l_controlPoints[j].get_j()-alfj*tempP[jj+1].get_j())/(1.0-alfj));
- tempW[jj] = ((l_weights[j]-alfj*tempW[jj+1])/(1.0-alfj));
- i++;
- j--;
- ii++;
- jj--;
- }
-
- if(j-i < t)
- {
- double distancei = tempP[ii-1].get_i() - tempP[jj+1].get_i();
- double distancej = tempP[ii-1].get_j() - tempP[jj+1].get_j();
- double distancew = tempW[ii-1] - tempW[jj+1];
- double distance = sqrt(vpMath::sqr(distancei)+vpMath::sqr(distancej)+vpMath::sqr(distancew));
- if(distance <= l_TOL) remflag = 1;
- }
- 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());
- double distancej = 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) remflag = 1;
- }
- if (remflag == 0) break;
- else
- {
- i = first;
- j = last;
- while (j-i>t)
- {
- l_controlPoints[i].set_i(tempP[i-off].get_i());
- l_controlPoints[i].set_j(tempP[i-off].get_j());
- l_weights[i] = tempW[i-off];
- l_controlPoints[j].set_i(tempP[j-off].get_i());
- l_controlPoints[j].set_j(tempP[j-off].get_j());
- l_weights[j] = tempW[j-off];
- i++;
- j--;
- }
- }
- first--;
- last++;
- }
- if (t == 0) {
- delete[] tempP;
- delete[] tempW;
- return t;
- }
- for(unsigned int k = l_r+1; k <= m; k++) l_knots[k-t] = l_knots[k];
- j = (unsigned int)fout;
- i = j;
- for(unsigned int k = 1; k< t; k++)
- {
- if (k%2) i++;
- else j--;
- }
- for(unsigned int k = i+1; k <= n; k++)
- {
- l_controlPoints[j].set_i(l_controlPoints[k].get_i());
- l_controlPoints[j].set_j(l_controlPoints[k].get_j());
- l_weights[j] = l_weights[k];
- j++;
- }
- for(unsigned int k = 0; k < t; k++)
- {
- l_knots.erase(l_knots.end()-1);
- l_controlPoints.erase(l_controlPoints.end()-1);
- }
-
- for(unsigned int k = 0; k < l_controlPoints.size(); k++)
- l_controlPoints[k].set_ij(l_controlPoints[k].get_i()/l_weights[k],l_controlPoints[k].get_j()/l_weights[k]);
-
- delete[] tempP;
- delete[] tempW;
- 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)
-{
- return removeCurveKnot(u, r, num, 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<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
-{
- if (l_p == 0) {
- //vpERROR_TRACE("Bad degree of the NURBS basis functions");
- throw(vpException(vpException::badValue,
- "Bad degree of the NURBS basis functions")) ;
- }
-
- l_knots.clear();
- l_controlPoints.clear();
- l_weights.clear();
- unsigned int n = (unsigned int)l_crossingPoints.size()-1;
- unsigned int m = n+l_p+1;
-
- double d = 0;
- for(unsigned int k=1; k<=n; k++)
- d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
-
- //Compute ubar
- std::vector<double> ubar;
- ubar.push_back(0.0);
- for(unsigned int k=1; k<n; k++)
- ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
- ubar.push_back(1.0);
-
- //Compute the knot vector
- for(unsigned int k = 0; k <= l_p; k++)
- l_knots.push_back(0.0);
-
- double sum = 0;
- for(unsigned int k = 1; k <= l_p; k++)
- sum = sum + ubar[k];
-
- //Centripetal method
- for(unsigned int k = 1; k <= n-l_p; k++)
- {
- l_knots.push_back(sum/l_p);
- sum = sum - ubar[k-1] + ubar[l_p+k-1];
- }
-
- for(unsigned int k = m-l_p; k <= m; k++)
- l_knots.push_back(1.0);
-
- vpMatrix A(n+1,n+1);
- vpBasisFunction* N;
-
- for(unsigned int i = 0; i <= n; i++)
- {
- unsigned int span = findSpan(ubar[i], l_p, l_knots);
- N = computeBasisFuns(ubar[i], span, l_p, l_knots);
- for (unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
- delete[] N;
- }
- //vpMatrix Ainv = A.inverseByLU();
- vpMatrix Ainv;
- A.pseudoInverse(Ainv);
- vpColVector Qi(n+1);
- vpColVector Qj(n+1);
- vpColVector Qw(n+1);
- for (unsigned int k = 0; k <= n; k++)
- {
- Qi[k] = l_crossingPoints[k].get_i();
- Qj[k] = l_crossingPoints[k].get_j();
- }
- Qw = 1;
- vpColVector Pi = Ainv*Qi;
- vpColVector Pj = Ainv*Qj;
- vpColVector Pw = Ainv*Qw;
-
- vpImagePoint pt;
- for (unsigned int k = 0; k <= n; k++)
- {
- pt.set_ij(Pi[k],Pj[k]);
- l_controlPoints.push_back(pt);
- l_weights.push_back(Pw[k]);
- }
-}
-
-/*!
- 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<vpMeSite> &l_crossingPoints)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- l_crossingPoints.front();
- vpMeSite s = l_crossingPoints.value();
- vpImagePoint pt(s.ifloat,s.jfloat);
- vpImagePoint pt_1 = pt;
- v_crossingPoints.push_back(pt);
- l_crossingPoints.next();
- while(!l_crossingPoints.outside())
- {
- s = l_crossingPoints.value();
- pt.set_ij(s.ifloat,s.jfloat);
- if (vpImagePoint::distance(pt_1,pt) >= 10)
- {
- v_crossingPoints.push_back(pt);
- pt_1 = pt;
- }
- l_crossingPoints.next();
- }
- 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<vpImagePoint> &l_crossingPoints)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
- v_crossingPoints.push_back(*it);
- }
- 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<vpMeSite> &l_crossingPoints)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- vpMeSite s = l_crossingPoints.front();
- vpImagePoint pt(s.ifloat,s.jfloat);
- vpImagePoint pt_1 = pt;
- v_crossingPoints.push_back(pt);
- std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
- ++it;
- for(; it!=l_crossingPoints.end(); ++it){
- vpImagePoint pt_tmp(it->ifloat, it->jfloat);
- if (vpImagePoint::distance(pt_1, pt_tmp) >= 10){
- v_crossingPoints.push_back(pt_tmp);
- pt_1 = pt_tmp;
- }
- }
- 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<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
-{
- l_knots.clear();
- l_controlPoints.clear();
- l_weights.clear();
- unsigned int m = (unsigned int)l_crossingPoints.size()-1;
-
- double d = 0;
- for(unsigned int k=1; k<=m; k++)
- d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
-
- //Compute ubar
- std::vector<double> ubar;
- ubar.push_back(0.0);
- for(unsigned int k=1; k<m; k++)
- ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
- ubar.push_back(1.0);
-
- //Compute the knot vector
- for(unsigned int k = 0; k <= l_p; k++)
- l_knots.push_back(0.0);
-
- d = (double)(m+1)/(double)(l_n-l_p+1);
-
- double alpha;
- for(unsigned int j = 1; j <= l_n-l_p; j++)
- {
- double i = floor(j*d);
- alpha = j*d-i;
- l_knots.push_back((1.0-alpha)*ubar[(unsigned int)i-1]+alpha*ubar[(unsigned int)i]);
- }
-
- for(unsigned int k = 0; k <= l_p ; k++)
- l_knots.push_back(1.0);
-
- //Compute Rk
- std::vector<vpImagePoint> Rk;
- vpBasisFunction* N;
- for(unsigned int k = 1; k <= m-1; k++)
- {
- unsigned int span = findSpan(ubar[k], l_p, l_knots);
- if (span == l_p && span == l_n)
- {
- N = computeBasisFuns(ubar[k], span, l_p, l_knots);
- vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
- l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
- Rk.push_back(pt);
- delete[] N;
- }
- 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)
- {
- 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
- {
- Rk.push_back(l_crossingPoints[k]);
- }
- }
-
- vpMatrix A(m-1,l_n-1);
- //Compute A
- for(unsigned int i = 1; i <= m-1; i++)
- {
- unsigned int span = findSpan(ubar[i], l_p, l_knots);
- N = computeBasisFuns(ubar[i], span, l_p, l_knots);
- for (unsigned int k = 0; k <= l_p; k++)
- {
- if (N[k].i > 0 && N[k].i < l_n)
- A[i-1][N[k].i-1] = N[k].value;
- }
- delete[] N;
- }
-
- vpColVector Ri(l_n-1);
- vpColVector Rj(l_n-1);
- vpColVector Rw(l_n-1);
- double sum;
- for (unsigned int i = 0; i < l_n-1; i++)
- {
- sum =0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_i();
- Ri[i] = sum;
- sum = 0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_j();
- Rj[i] = sum;
- sum = 0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]; //The crossing points weigths are equal to 1.
- Rw[i] = sum;
- }
-
- vpMatrix AtA = A.AtA();
- vpMatrix AtAinv;
- AtA.pseudoInverse(AtAinv);
-
- vpColVector Pi = AtAinv*Ri;
- vpColVector Pj = AtAinv*Rj;
- vpColVector Pw = AtAinv*Rw;
-
- vpImagePoint pt;
- l_controlPoints.push_back(l_crossingPoints[0]);
- l_weights.push_back(1.0);
- for (unsigned int k = 0; k < l_n-1; k++)
- {
- pt.set_ij(Pi[k],Pj[k]);
- l_controlPoints.push_back(pt);
- l_weights.push_back(Pw[k]);
- }
- l_controlPoints.push_back(l_crossingPoints[m]);
- 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<vpMeSite> &l_crossingPoints, unsigned int n)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- l_crossingPoints.front();
- while(!l_crossingPoints.outside())
- {
- vpMeSite s = l_crossingPoints.value();
- vpImagePoint pt(s.ifloat,s.jfloat);
- v_crossingPoints.push_back(pt);
- l_crossingPoints.next();
- }
- 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<vpImagePoint> &l_crossingPoints, unsigned int n)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
- v_crossingPoints.push_back(*it);
- }
- 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<vpMeSite> &l_crossingPoints, unsigned int n)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpMeSite>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
- vpImagePoint pt(it->ifloat, it->jfloat);
- v_crossingPoints.push_back(pt);
- }
- 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);
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use globalCurveInterp(const std::list<vpImagePoint> &) instead.
-
- 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<vpImagePoint> &l_crossingPoints)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- l_crossingPoints.front();
- while(!l_crossingPoints.outside())
- {
- v_crossingPoints.push_back(l_crossingPoints.value());
- l_crossingPoints.next();
- }
- globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
-}
-
-/*!
- \deprecated This method is deprecated. You should use globalCurveApprox(const std::list<vpImagePoint> &, unsigned int) instead.
-
- 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(vpList<vpImagePoint> &l_crossingPoints, unsigned int n)
-{
- std::vector<vpImagePoint> v_crossingPoints;
- l_crossingPoints.front();
- while(!l_crossingPoints.outside())
- {
- v_crossingPoints.push_back(l_crossingPoints.value());
- l_crossingPoints.next();
- }
- globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
-}
-
-#endif
-
diff --git a/src/math/spline/vpNurbs.h b/src/math/spline/vpNurbs.h
deleted file mode 100644
index f0d5fdb..0000000
--- a/src/math/spline/vpNurbs.h
+++ /dev/null
@@ -1,191 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNurbs.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * This class implements the Non Uniform Rational B-Spline (NURBS)
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpNurbs_H
-#define vpNurbs_H
-
-/*!
- \file vpNurbs.h
- \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
-*/
-
-#include <visp/vpImagePoint.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpBSpline.h>
-#include <visp/vpList.h>
-
-#include <list>
-
-/*!
- \class vpNurbs
- \ingroup MathTools
-
- \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
-
- The different parameters are :
-
- - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1} i = 0, ...,m \f$.
- To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
-
- - The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
- \f[ N_{i,0}(u) = \left\{\begin{array}{cc}
- 1 & \mbox{if } u_i \leq u_{i+1} \\ 0 & else
- \end{array}\right.\f]
-
- \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
-
- where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
-
- - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
-
- - The weight \f$ {w_i} \f$ associated to each control points.The wheights value is upper than 0.
-
- It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
- \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]
-
- You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
-*/
-
-class VISP_EXPORT vpNurbs : public vpBSpline
-{
-protected:
- std::vector<double> weights; //Vector which contains the weights associated to each control Points
-
-
-protected:
- static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- vpMatrix computeCurveDers(double u, unsigned int der);
-
-public:
-
- vpNurbs();
- vpNurbs(const vpNurbs &nurbs);
- virtual ~vpNurbs();
-
- /*!
- Gets all the weights relative to the control points.
-
- \return list : A std::list containing weights relative to the control points.
- */
- inline void get_weights(std::list<double>& list) const {
- list.clear();
- for (unsigned int i = 0; i < weights.size(); i++)
- list.push_back(*(&(weights[0])+i));
- }
-
- /*!
- Sets all the knots.
-
- \param list : A std::list containing the value of the knots.
- */
- inline void set_weights(const std::list<double> &list) {
- weights.clear();
- for(std::list<double>::const_iterator it=list.begin(); it!=list.end(); ++it){
- weights.push_back(*it);
- }
- }
-
- static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- vpImagePoint computeCurvePoint(double u);
-
- static vpImagePoint* computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- vpImagePoint* computeCurveDersPoint(double u, unsigned int der);
-
- static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void curveKnotIns(double u, unsigned int s = 0, unsigned int r = 1);
-
- static void refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void refineKnotVectCurve(double* x, unsigned int r);
-
- 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<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL);
-
- static void globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void globalCurveInterp(vpList<vpMeSite>& l_crossingPoints);
- void globalCurveInterp(const std::list<vpImagePoint>& l_crossingPoints);
- void globalCurveInterp(const std::list<vpMeSite>& l_crossingPoints);
- void globalCurveInterp();
-
- static void globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void globalCurveApprox(vpList<vpMeSite>& l_crossingPoints, unsigned int n);
- void globalCurveApprox(const std::list<vpImagePoint>& l_crossingPoints, unsigned int n);
- void globalCurveApprox(const std::list<vpMeSite>& l_crossingPoints, unsigned int n);
- void globalCurveApprox(unsigned int n);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- /*!
- \deprecated This method is deprecated. You should use get_weights(std::list<double> &) const instead. \n \n
- Gets all the weights relative to the control points.
-
- \return list : A list containing weights relative to the control points.
- */
- vp_deprecated inline vpList<double> get_weights() const {
- vpList<double> list;
- for (unsigned int i = 0; i < weights.size(); i++) list.addRight(*(&(weights[0])+i));
- return list; }
-
- /*!
- \deprecated This method is deprecated. You should use set_weights(const std::list<double>&) instead.\n \n
- Sets all the knots.
-
- \param list : A list containing the value of the knots.
- */
- vp_deprecated inline void set_weights(vpList<double> &list) {
- weights.clear();
- list.front();
- for (unsigned int i = 0; i < list.nbElements(); i++)
- {
- weights.push_back(list.value());
- list.next();
- }
- }
- vp_deprecated void globalCurveApprox(vpList<vpImagePoint>& l_crossingPoints, unsigned int n);
- vp_deprecated void globalCurveInterp(vpList<vpImagePoint>& l_crossingPoints);
-
-#endif
-};
-
-#endif
diff --git a/src/math/transformation/vpExponentialMap.cpp b/src/math/transformation/vpExponentialMap.cpp
deleted file mode 100644
index 128df22..0000000
--- a/src/math/transformation/vpExponentialMap.cpp
+++ /dev/null
@@ -1,321 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpExponentialMap.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exponential map.
- *
- * Authors:
- * Fabien Spindler
- * Francois Chaumette
- *
- *****************************************************************************/
-
-#include <visp/vpExponentialMap.h>
-
-
-/*!
-
- Compute the exponential map. The inverse function is inverse(). The sampling
- time is here set to 1 second. To use an other value you should use
- direct(const vpColVector &, const float &).
-
- \param v : Instantaneous velocity represented by a 6 dimension
- vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf t} \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector). \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- \return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
- velocity \f$ \bf v \f$. If \f$ \bf v \f$ is expressed in frame c, \f$ {\bf M}
- = {^c}{\bf M}_{c_{new}} \f$.
-
- \sa inverse(const vpHomogeneousMatrix &)
-*/
-vpHomogeneousMatrix
-vpExponentialMap::direct(const vpColVector &v)
-{
- return vpExponentialMap::direct(v, 1.0);
-}
-
-/*!
-
- Compute the exponential map. The inverse function is inverse().
-
- \param v : Instantaneous velocity represented by a 6 dimension
- vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf t} \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector). \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- \param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
- velocity v is applied.
-
- \return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
- velocity \f$ \bf v \f$. If \f$ \bf v \f$ is expressed in frame c, \f$ {\bf M}
- = {^c}{\bf M}_{c_{new}} \f$.
-
- \sa inverse(const vpHomogeneousMatrix &, const float &)
-*/
-vpHomogeneousMatrix
-vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
-{
- double theta,si,co,sinc,mcosc,msinc;
- vpThetaUVector u ;
- vpRotationMatrix rd ;
- vpTranslationVector dt ;
-
- vpColVector v_dt = v * delta_t;
-
- u[0] = v_dt[3];
- u[1] = v_dt[4];
- u[2] = v_dt[5];
- rd.buildFrom(u);
-
- theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
- si = sin(theta);
- co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
- msinc = vpMath::msinc(si,theta);
-
- dt[0] = v_dt[0]*(sinc + u[0]*u[0]*msinc)
- + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
- + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
-
- dt[1] = v_dt[0]*(u[0]*u[1]*msinc + u[2]*mcosc)
- + v_dt[1]*(sinc + u[1]*u[1]*msinc)
- + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
-
- dt[2] = v_dt[0]*(u[0]*u[2]*msinc - u[1]*mcosc)
- + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
- + v_dt[2]*(sinc + u[2]*u[2]*msinc);
-
- vpHomogeneousMatrix Delta ;
- Delta.insert(rd) ;
- Delta.insert(dt) ;
-
-
-
- if (0) // test new version wrt old version
- {
- // old version
- unsigned int i,j;
-
- double sinu,cosi,mcosi,s;
- // double u[3];
- // vpRotationMatrix rd ;
- // vpTranslationVector dt ;
-
- s = sqrt(v_dt[3]*v_dt[3] + v_dt[4]*v_dt[4] + v_dt[5]*v_dt[5]);
- if (s > 1.e-15)
- {
- for (i=0;i<3;i++) u[i] = v_dt[3+i]/s;
- sinu = sin(s);
- cosi = cos(s);
- mcosi = 1-cosi;
- rd[0][0] = cosi + mcosi*u[0]*u[0];
- rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
- rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
- rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
- rd[1][1] = cosi + mcosi*u[1]*u[1];
- rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
- rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
- rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
- rd[2][2] = cosi + mcosi*u[2]*u[2];
-
- dt[0] = v_dt[0]*(sinu/s + u[0]*u[0]*(1-sinu/s))
- + v_dt[1]*(u[0]*u[1]*(1-sinu/s)-u[2]*mcosi/s)
- + v_dt[2]*(u[0]*u[2]*(1-sinu/s)+u[1]*mcosi/s);
-
- dt[1] = v_dt[0]*(u[0]*u[1]*(1-sinu/s)+u[2]*mcosi/s)
- + v_dt[1]*(sinu/s + u[1]*u[1]*(1-sinu/s))
- + v_dt[2]*(u[1]*u[2]*(1-sinu/s)-u[0]*mcosi/s);
-
- dt[2] = v_dt[0]*(u[0]*u[2]*(1-sinu/s)-u[1]*mcosi/s)
- + v_dt[1]*(u[1]*u[2]*(1-sinu/s)+u[0]*mcosi/s)
- + v_dt[2]*(sinu/s + u[2]*u[2]*(1-sinu/s));
- }
- else
- {
- for (i=0;i<3;i++)
- {
- for(j=0;j<3;j++) rd[i][j] = 0.0;
- rd[i][i] = 1.0;
- dt[i] = v_dt[i];
- }
- }
- // end old version
-
- // Test of the new version
- vpHomogeneousMatrix Delta_old ;
- Delta_old.insert(rd) ;
- Delta_old.insert(dt) ;
-
- int pb = 0;
- for (i=0;i<4;i++)
- {
- for(j=0;j<4;j++)
- if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb vpHomogeneousMatrix::expMap\n");
- std::cout << " Delta : " << std::endl << Delta << std::endl;
- std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
- }
- // end of the test
- }
-
- return Delta ;
-}
-
-/*!
-
- Compute an instantaneous velocity from an homogeneous matrix. The inverse
- function is the exponential map, see direct().
-
- \param M : An homogeneous matrix corresponding to a displacement.
-
- \return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
- vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
- rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
- vector: \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
- u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- \sa direct(const vpColVector &)
-*/
-vpColVector
-vpExponentialMap::inverse(const vpHomogeneousMatrix &M)
-{
- return vpExponentialMap::inverse(M, 1.0);
-}
-
-/*!
-
- Compute an instantaneous velocity from an homogeneous matrix. The inverse
- function is the exponential map, see direct().
-
- \param M : An homogeneous matrix corresponding to a displacement.
-
- \param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
- displacement is applied.
-
- \return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
- vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
- rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
- vector: \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
- u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- \sa direct(const vpColVector &, const float &)
-*/
-vpColVector
-vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t)
-{
- vpColVector v(6);
- unsigned int i;
- double theta,si,co,sinc,mcosc,msinc,det;
- vpThetaUVector u ;
- vpRotationMatrix Rd,a;
- vpTranslationVector dt ;
-
- M.extract(Rd);
- u.buildFrom(Rd);
- for (i=0;i<3;i++) v[3+i] = u[i];
-
- theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
- si = sin(theta);
- co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
- msinc = vpMath::msinc(si,theta);
-
- // a below is not a pure rotation matrix, even if not so far from
- // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
- // with V = t.U
-
- a[0][0] = sinc + u[0]*u[0]*msinc;
- a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
- a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
-
- a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
- a[1][1] = sinc + u[1]*u[1]*msinc;
- a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
-
- a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
- a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
- a[2][2] = sinc + u[2]*u[2]*msinc;
-
- det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
- + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
- - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
-
- if (fabs(det) > 1.e-5)
- {
- v[0] = (M[0][3]*a[1][1]*a[2][2]
- + M[1][3]*a[2][1]*a[0][2]
- + M[2][3]*a[0][1]*a[1][2]
- - M[2][3]*a[1][1]*a[0][2]
- - M[1][3]*a[0][1]*a[2][2]
- - M[0][3]*a[2][1]*a[1][2])/det;
- v[1] = (a[0][0]*M[1][3]*a[2][2]
- + a[1][0]*M[2][3]*a[0][2]
- + M[0][3]*a[1][2]*a[2][0]
- - a[2][0]*M[1][3]*a[0][2]
- - a[1][0]*M[0][3]*a[2][2]
- - a[0][0]*M[2][3]*a[1][2])/det;
- v[2] = (a[0][0]*a[1][1]*M[2][3]
- + a[1][0]*a[2][1]*M[0][3]
- + a[0][1]*M[1][3]*a[2][0]
- - a[2][0]*a[1][1]*M[0][3]
- - a[1][0]*a[0][1]*M[2][3]
- - a[0][0]*a[2][1]*M[1][3])/det;
- }
- else
- {
- v[0] = M[0][3];
- v[1] = M[1][3];
- v[2] = M[2][3];
- }
-
- // Apply the sampling time to the computed velocity
- v /= delta_t;
-
- return(v);
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpExponentialMap.h b/src/math/transformation/vpExponentialMap.h
deleted file mode 100644
index f8ed340..0000000
--- a/src/math/transformation/vpExponentialMap.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpExponentialMap.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exponential map.
- *
- * Authors:
- * Fabien Spindler
- * Francois Chaumette
- *
- *****************************************************************************/
-
-/*!
- \file vpExponentialMap.h
- \brief Provides exponential map computation
-*/
-
-
-
-#ifndef vpExponentialMap_h
-#define vpExponentialMap_h
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpColVector.h>
-
-/*!
- \class vpExponentialMap
-
- \ingroup ExpMapTransformation
-
- \brief Direct or inverse exponential map computation.
-
- - The direct exponential map allows to determine a displacement from a
- velocity vector applied during a sampling time. With direct() the sampling
- time is set to 1 second. With direct(const vpColVector &, const float &)
- the sampling time can be set to an other value.
-
- - The inverse exponential map allows to compute a velocity vector from a
- displacement measured during a time interval. With inverse() the time
- interval also called sampling time is set to 1 second. With
- inverse(const vpHomogeneousMatrix &, const float &) the sampling time
- can be set to an other value.
-
- The displacement is represented as an homogeneous matrix
- (vpHomogeneousMatrix). Velocities are represented as a \f$ [{\bf t}, {\bf
- \theta u} ]^t \f$ 6 dimension vector where \f$ t \f$ is a translation vector
- (see vpTranslationVector) and \f$ \theta u \f$ a rotation vector (see
- vpThetaUVector).
-
-*/
-class VISP_EXPORT vpExponentialMap
-{
-
-public:
- static vpHomogeneousMatrix direct(const vpColVector &v);
- static vpHomogeneousMatrix direct(const vpColVector &v,
- const double &delta_t);
- static vpColVector inverse(const vpHomogeneousMatrix &M);
- static vpColVector inverse(const vpHomogeneousMatrix &M,
- const double &delta_t);
-
-};
-#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpForceTwistMatrix.cpp b/src/math/transformation/vpForceTwistMatrix.cpp
deleted file mode 100644
index 96b3aca..0000000
--- a/src/math/transformation/vpForceTwistMatrix.cpp
+++ /dev/null
@@ -1,437 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpForceTwistMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Twist transformation matrix that allows to transform forces from one
- * frame to an other.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpForceTwistMatrix.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpForceTwistMatrix.cpp
-
- \brief Definition of the vpForceTwistMatrix. Class that consider the
- particular case of twist transformation matrix that allows to
- transform a force/torque skew from one frame to an other.
-*/
-
-
-/*!
- Copy operator.
-
- \param M : Force/torque twist matrix to copy.
-*/
-vpForceTwistMatrix &
-vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
-{
- init() ;
-
- for (int i=0; i<6; i++) {
- for (int j=0; j<6; j++) {
- rowPtrs[i][j] = M.rowPtrs[i][j];
- }
- }
-
- return *this;
-}
-
-
-/*!
- Initialize the force/torque 6 by 6 twist matrix as identity.
-*/
-
-void
-vpForceTwistMatrix::init()
-{
- unsigned int i,j ;
-
- try {
- resize(6,6) ;
- }
- catch(vpException me) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- for (i=0 ; i < 6 ; i++) {
- for (j=0 ; j < 6; j++) {
- if (i==j)
- (*this)[i][j] = 1.0 ;
- else
- (*this)[i][j] = 0.0;
- }
- }
-}
-
-/*!
- Initialize a force/torque twist transformation matrix as identity.
-*/
-vpForceTwistMatrix::vpForceTwistMatrix() : vpMatrix()
-{
- init() ;
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from another
- force/torque twist matrix.
-
- \param F : Force/torque twist matrix used as initializer.
-*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) : vpMatrix()
-{
- init() ;
- *this = F ;
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from an
- homogeneous matrix. Given the homogenous transformation
-
- \f[ ^a{\bf M}_b = \left(\begin{array}{cc}
- ^a{\bf R}_b & ^a{\bf t}_b\\
- 0 & 1
- \end{array} \right) \f]
-
- the force/torque twist matrix is given by :
-
- \f[
- ^a{\bf F}_b = \left[\begin{array}{cc}
- ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
- {[^a{\bf t}_b]}_{\times} \; ^a{\bf R}_b & ^a{\bf R}_b
- \end{array}
- \right]
- \f]
-
- \param M : Homogeneous matrix \f$^a{\bf M}_b\f$ used to initialize the twist
- transformation matrix.
-
-*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M) : vpMatrix()
-{
- init() ;
- buildFrom(M);
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tv : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
-
-*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &tv,
- const vpThetaUVector &thetau) : vpMatrix()
-{
- init() ;
- buildFrom(tv, thetau) ;
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation matrix R.
-
- \param tv : Translation vector.
-
- \param R : Rotation matrix.
-
-*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &tv,
- const vpRotationMatrix &R)
-{
- init() ;
- buildFrom(tv,R) ;
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tx,ty,tz : Translation vector in meters.
-
- \param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
-*/
-vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz) : vpMatrix()
-{
- init() ;
- vpTranslationVector T(tx,ty,tz) ;
- vpThetaUVector tu(tux,tuy,tuz) ;
- buildFrom(T,tu) ;
-}
-
-/*!
-
- Set the twist transformation matrix to identity.
-
-*/
-void
-vpForceTwistMatrix::setIdentity()
-{
- init() ;
-}
-
-
-/*!
-
- Operator that allows to multiply a skew transformation matrix by an
- other skew transformation matrix.
-
-\code
-#include <visp/vpForceTwistMatrix.h>
-
-int main()
-{
- vpForceTwistMatrix aFb, bFc;
- // ... initialize the force/torque twist transformations aFb and bFc
- // Compute the force/torque transformation from frame a to c
- vpForceTwistMatrix aFc = aFb * bFc;
-}
-\endcode
-
-*/
-vpForceTwistMatrix
-vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const
-{
- vpForceTwistMatrix Fout ;
-
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
- s +=rowPtrs[i][k] * F.rowPtrs[k][j];
- Fout[i][j] = s ;
- }
- }
- return Fout;
-}
-
-/*!
- Operator that allows to multiply a skew transformation matrix by a matrix.
-
- \exception vpMatrixException::incorrectMatrixSizeError If M is not a 6 rows
- dimension matrix.
-*/
-vpMatrix
-vpForceTwistMatrix::operator*(const vpMatrix &M) const
-{
-
- if (6 != M.getRows())
- {
- vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ;
- throw(vpMatrixException::incorrectMatrixSizeError) ;
- }
-
- vpMatrix p(6, M.getCols()) ;
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<M.getCols();j++) {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
- s += rowPtrs[i][k] * M[k][j];
- p[i][j] = s ;
- }
- }
- return p;
-}
-
-/*!
-
- Operator that allows to multiply a skew transformation matrix by a
- column vector.
-
- \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$.
-
- For example, this operator can be used to convert a force/torque skew from sensor
- frame into the probe frame :
-
- \f$^p{\bf H} = ^p{\bf F}_s \; ^s{\bf H}\f$
-
- The example below shows how to handle that transformation.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpForceTwistMatrix.h>
-
-int main()
-{
-
- // Get the force/torque measures from the sensor
- vpColVector sH(6); // Force/torque measures given by the sensor
-
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
- robot.getForceTorque(sH); // Get the force/torque measures
-#endif
-
- // Set the transformation from sensor frame to the probe frame
- vpHomogeneousMatrix pMs;
- pMs[2][3] = -0.262; // tz only
-
- // Set the force/torque twist transformation
- vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame
-
- // Compute the resulting force/torque in the probe frame
- vpColVector pH(6); // Force/torque in the probe frame
- pH = pFs * sH;
-
- return 0;
-}
- \endcode
-
- \exception vpMatrixException::incorrectMatrixSizeError If \f$ \bf H \f$is not a 6
- dimension vector.
-
-*/
-vpColVector
-vpForceTwistMatrix::operator*(const vpColVector &H) const
-{
- vpColVector Hout(6);
-
- if (6 != H.getRows())
- {
- vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vector multiply") ;
- throw(vpMatrixException::incorrectMatrixSizeError) ;
- }
-
- Hout = 0.0;
-
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
- Hout[i]+=rowPtrs[i][j] * H[j];
- }
- }
-
- return Hout ;
-}
-
-
-/*!
-
- Build a force/torque twist transformation matrix from a translation vector
- \e t and a rotation matrix M.
-
- \param tv : Translation vector.
-
- \param R : Rotation matrix.
-
-*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
- const vpRotationMatrix &R)
-{
- unsigned int i, j;
- vpMatrix skewaR = tv.skew(tv)*R ;
-
- for (i=0 ; i < 3 ; i++) {
- for (j=0 ; j < 3 ; j++) {
- (*this)[i][j] = R[i][j] ;
- (*this)[i+3][j+3] = R[i][j] ;
- (*this)[i+3][j] = skewaR[i][j] ;
- }
- }
- return (*this) ;
-}
-
-/*!
-
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tv : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
-
-*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
- const vpThetaUVector &thetau)
-{
- vpRotationMatrix R ;
- R.buildFrom(thetau) ;
- buildFrom(tv,R) ;
- return (*this) ;
-}
-
-
-/*!
-
- Initialize a force/torque twist transformation matrix from an homogeneous matrix
- \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
- \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
-
- \param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
- transformation matrix.
-
-*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
-{
- vpTranslationVector tv ;
- vpRotationMatrix R ;
- M.extract(R) ;
- M.extract(tv) ;
-
- buildFrom(tv, R) ;
- return (*this) ;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpForceTwistMatrix.h b/src/math/transformation/vpForceTwistMatrix.h
deleted file mode 100644
index 33a797a..0000000
--- a/src/math/transformation/vpForceTwistMatrix.h
+++ /dev/null
@@ -1,150 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpForceTwistMatrix.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Twist transformation matrix that allows to transform forces from one
- * frame to an other.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpForceTwistMatrix_h
-#define vpForceTwistMatrix_h
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRotationMatrix.h>
-
-
-/*!
- \class vpForceTwistMatrix
-
- \ingroup TwistTransformation
-
- \brief Class that consider the particular case of twist
- transformation matrix that allows to transform a force/troque vector
- from one frame to an other.
-
- The vpForceTwistMatrix is derived from vpMatrix.
-
- The twist transformation matrix that allows to transform the
- force/torque vector expressed at frame \f${\cal F}_b\f$ into the
- frame \f${\cal F}_a\f$ is a 6 by 6 matrix defined as
-
- \f[
- ^a{\bf F}_b = \left[ \begin{array}{cc}
- ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
- {[^a{\bf t}_b]}_{\times} \; ^a{\bf R}_b & ^a{\bf R}_b
- \end{array}
- \right]
- \f]
-
- \f$ ^a{\bf R}_b \f$ is a rotation matrix and
- \f$ ^a{\bf t}_b \f$ is a translation vector.
-
- The code belows shows for example how to convert a force/torque skew
- from probe frame to a sensor frame.
-
- \code
-#include <visp/vpColVector.h>
-#include <visp/vpForceTwistMatrix.h>
-
-int main()
-{
- vpForceTwistMatrix sFp; // Twist transformation matrix from sensor to probe frame
-
- vpHomogeneousMatrix sMp; // Force/torque sensor frame to probe frame transformation
- // ... sMp need here to be initialized
-
- sFp.buildFrom(sMp);
-
- vpColVector p_H(6); // Force/torque skew in the probe frame: fx,fy,fz,tx,ty,tz
- // ... p_H should here have an initial value
-
- vpColVector s_H(6); // Force/torque skew in the sensor frame: fx,fy,fz,tx,ty,tz
-
- // Compute the value of the force/torque in the sensor frame
- s_H = sFp * p_H;
-}
- \endcode
-*/
-class VISP_EXPORT vpForceTwistMatrix : public vpMatrix
-{
- friend class vpMatrix;
-
- public:
- // basic constructor
- vpForceTwistMatrix() ;
- // copy constructor
- vpForceTwistMatrix(const vpForceTwistMatrix &F) ;
- // constructor from an homogeneous transformation
- vpForceTwistMatrix(const vpHomogeneousMatrix &M) ;
-
- // Construction from Translation and rotation (ThetaU parameterization)
- vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
- // Construction from Translation and rotation (matrix parameterization)
- vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- vpForceTwistMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
-
- // Basic initialisation (identity)
- void init() ;
-
- vpForceTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R);
- vpForceTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpThetaUVector &thetau);
- vpForceTwistMatrix buildFrom(const vpHomogeneousMatrix &M) ;
-
- // Basic initialisation (identity)
- void setIdentity() ;
-
- vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const ;
- vpMatrix operator*(const vpMatrix &M) const ;
-
- vpColVector operator*(const vpColVector &H) const ;
-
- // copy operator from vpMatrix (handle with care)
- vpForceTwistMatrix &operator=(const vpForceTwistMatrix &H);
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpHomogeneousMatrix.cpp b/src/math/transformation/vpHomogeneousMatrix.cpp
deleted file mode 100644
index 10783a9..0000000
--- a/src/math/transformation/vpHomogeneousMatrix.cpp
+++ /dev/null
@@ -1,754 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomogeneousMatrix.cpp 5130 2015-01-06 18:50:43Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homogeneous matrix.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpHomogeneousMatrix.cpp
- \brief Defines vpHomogeneousMatrix class. Class that consider
- the particular case of an homogeneous matrix.
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpQuaternionVector.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- Initialize a 4x4 momogeneous matrix as identity.
-*/
-void
-vpHomogeneousMatrix::init()
-{
- unsigned int i,j ;
-
- try {
- resize(4,4) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-
- for (i=0 ; i < 4 ; i++)
- for (j=0 ; j < 4; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
- else
- (*this)[i][j] = 0.0;
-
-}
-
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &tv,
- const vpQuaternionVector &q) : vpMatrix()
-{
- init();
- buildFrom(tv,q);
-}
-
-/*!
- Initialize an homogeneous matrix as identity.
-*/
-vpHomogeneousMatrix::vpHomogeneousMatrix() : vpMatrix()
-{
- init() ;
-}
-
-
-/*!
- Initialize an homogeneous matrix from another homogeneous matrix.
-*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M) : vpMatrix()
-{
- init() ;
- *this = M ;
-}
-
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &tv,
- const vpThetaUVector &tu) : vpMatrix()
-{
- init() ;
- buildFrom(tv,tu) ;
-}
-
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &tv,
- const vpRotationMatrix &R) : vpMatrix()
-{
- init() ;
- insert(R) ;
- insert(tv) ;
-}
-
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p) : vpMatrix()
-{
-
- init() ;
- buildFrom(p[0],p[1],p[2],p[3],p[4],p[5]) ;
-}
-
-/*!
- Creates an homogeneous matrix from a vector.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- std::vector<float> v(12, 0);
- v[1] = -1.; // ry=-90
- v[4] = 1.; // rx=90
- v[10] = -1.; // rz=-90
- v[3] = 0.3; // tx
- v[7] = 0.4; // ty
- v[11] = 0.5; // tz
-
- std::cout << "v: ";
- for(unsigned int i=0; i<v.size(); i++)
- std::cout << v[i] << " ";
- std::cout << std::endl;
-
- vpHomogeneousMatrix M(v);
- std::cout << "M:\n" << M << std::endl;
-}
- \endcode
-
- It produces the following printings:
- \code
-v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
-M:
-0 -1 0 0.3000000119
-1 0 0 0.400000006
-0 0 -1 0.5
-0 0 0 1
- \endcode
- */
-vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v) : vpMatrix()
-{
- init() ;
- buildFrom(v) ;
-}
-
-/*!
- Creates an homogeneous matrix from a vector.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- std::vector<double> v(12, 0);
- v[1] = -1.; // ry=-90
- v[4] = 1.; // rx=90
- v[10] = -1.; // rz=-90
- v[3] = 0.3; // tx
- v[7] = 0.4; // ty
- v[11] = 0.5; // tz
-
- std::cout << "v: ";
- for(unsigned int i=0; i<v.size(); i++)
- std::cout << v[i] << " ";
- std::cout << std::endl;
-
- vpHomogeneousMatrix M(v);
- std::cout << "M:\n" << M << std::endl;
-}
- \endcode
-
- It produces the following printings:
- \code
-v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
-M:
-0 -1 0 0.3
-1 0 0 0.4
-0 0 -1 0.5
-0 0 0 1
- \endcode
- */
-vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpMatrix()
-{
- init() ;
- buildFrom(v) ;
-}
-
-vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz) : vpMatrix()
-{
- init() ;
- buildFrom(tx, ty, tz,tux, tuy, tuz) ;
-}
-
-void
-vpHomogeneousMatrix::buildFrom(const vpTranslationVector &tv,
- const vpThetaUVector &tu)
-{
- insert(tu) ;
- insert(tv) ;
-}
-
-void
-vpHomogeneousMatrix::buildFrom(const vpTranslationVector &tv,
- const vpRotationMatrix &R)
-{
- init() ;
- insert(R) ;
- insert(tv) ;
-}
-
-
-void
-vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
-{
-
- vpTranslationVector tv(p[0],p[1],p[2]) ;
- vpThetaUVector tu(p[3],p[4],p[5]) ;
-
- insert(tu) ;
- insert(tv) ;
-}
-
-void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &tv,
- const vpQuaternionVector &q)
-{
- insert(tv);
- insert(q);
-}
-
-void
-vpHomogeneousMatrix::buildFrom(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz)
-{
- vpRotationMatrix R(tux, tuy, tuz) ;
- vpTranslationVector tv(tx, ty, tz) ;
-
- insert(R) ;
- insert(tv) ;
-}
-
-/*!
- Converts a vector to an homogeneous matrix.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- std::vector<float> v(12, 0);
- v[1] = -1.; // ry=-90
- v[4] = 1.; // rx=90
- v[10] = -1.; // rz=-90
- v[3] = 0.3; // tx
- v[7] = 0.4; // ty
- v[11] = 0.5; // tz
-
- std::cout << "v: ";
- for(unsigned int i=0; i<v.size(); i++)
- std::cout << v[i] << " ";
- std::cout << std::endl;
-
- vpHomogeneousMatrix M;
- M.buildFrom(v);
- std::cout << "M:\n" << M << std::endl;
-}
- \endcode
-
- It produces the following printings:
- \code
-v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
-M:
-0 -1 0 0.3000000119
-1 0 0 0.400000006
-0 0 -1 0.5
-0 0 0 1
- \endcode
- */
-void
-vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
-{
- if (v.size() != 12 && v.size() != 16) {
- throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
- }
-
- for (unsigned int i=0; i < 12; i++)
- this->data[i] = (double)v[i];
-}
-
-/*!
- Converts a vector to an homogeneous matrix.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
-
- The following example shows how to use this function:
- \code
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- std::vector<double> v(12, 0);
- v[1] = -1.; // ry=-90
- v[4] = 1.; // rx=90
- v[10] = -1.; // rz=-90
- v[3] = 0.3; // tx
- v[7] = 0.4; // ty
- v[11] = 0.5; // tz
-
- std::cout << "v: ";
- for(unsigned int i=0; i<v.size(); i++)
- std::cout << v[i] << " ";
- std::cout << std::endl;
-
- vpHomogeneousMatrix M;
- M.buildFrom(v);
- std::cout << "M:\n" << M << std::endl;
-}
- \endcode
-
- It produces the following printings:
- \code
-v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5
-M:
-0 -1 0 0.3
-1 0 0 0.4
-0 0 -1 0.5
-0 0 0 1
- \endcode
- */
-void
-vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
-{
- if (v.size() != 12 && v.size() != 16) {
- throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
- }
-
- for (unsigned int i=0; i < 12; i++)
- this->data[i] = v[i];
-}
-
-/*!
- Affectation of two homogeneous matrices.
-
- \param M : *this = M
-*/
-vpHomogeneousMatrix &
-vpHomogeneousMatrix::operator=(const vpHomogeneousMatrix &M)
-{
-
- if (rowPtrs != M.rowPtrs) init() ;
-
- for (int i=0; i<4; i++)
- {
- for (int j=0; j<4; j++)
- {
- rowPtrs[i][j] = M.rowPtrs[i][j];
- }
- }
- return *this;
-}
-
-/*!
- Allow homogeneous matrix multiplication.
-
- \code
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- vpHomogeneousMatrix aMb, bMc;
- // Initialize aMb and bMc...
-
- // Compute aMc * bMc
- vpHomogeneousMatrix aMc = aMb * bMc;
-}
- \endcode
-
-*/
-vpHomogeneousMatrix
-vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
-{
- vpHomogeneousMatrix p,p1 ;
-
- vpRotationMatrix R1, R2, R ;
- vpTranslationVector T1, T2 , T;
-
-
- extract(T1) ;
- M.extract(T2) ;
-
- extract (R1) ;
- M.extract (R2) ;
-
- R = R1*R2 ;
-
- T = R1*T2 + T1 ;
-
- p.insert(T) ;
- p.insert(R) ;
-
- return p;
-}
-
-vpColVector
-vpHomogeneousMatrix::operator*(const vpColVector &v) const
-{
- vpColVector p(rowNum);
-
- p = 0.0;
-
- for (unsigned int j=0;j<4;j++) {
- for (unsigned int i=0;i<4;i++) {
- p[i]+=rowPtrs[i][j] * v[j];
- }
- }
-
- return p;
-}
-
-
-/*********************************************************************/
-
-/*!
- Test if the 3x3 rotational part of the homogeneous matrix is really
- a rotation matrix.
-*/
-bool
-vpHomogeneousMatrix::isAnHomogeneousMatrix() const
-{
- vpRotationMatrix R ;
- extract(R) ;
-
- return R.isARotationMatrix() ;
-}
-
-/*!
- Extract the rotational matrix from the homogeneous matrix.
- \param R : rotational component as a rotation matrix.
-*/
-void
-vpHomogeneousMatrix::extract(vpRotationMatrix &R) const
-{
- unsigned int i,j ;
-
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3; j++)
- R[i][j] = (*this)[i][j] ;
-}
-
-/*!
- Extract the translation vector from the homogeneous matrix.
-*/
-void
-vpHomogeneousMatrix::extract(vpTranslationVector &tv) const
-{
- tv[0] = (*this)[0][3] ;
- tv[1] = (*this)[1][3] ;
- tv[2] = (*this)[2][3] ;
-}
-/*!
- Extract the rotation as a Theta U vector.
-*/
-void
-vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
-{
-
- vpRotationMatrix R;
- (*this).extract(R);
- tu.buildFrom(R);
-}
-
-/*!
- Extract the rotation as a quaternion.
-*/
-void
-vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
-{
- vpRotationMatrix R;
- (*this).extract(R);
- q.buildFrom(R);
-}
-
-/*!
- Insert the rotational component of the homogeneous matrix.
-*/
-void
-vpHomogeneousMatrix::insert(const vpRotationMatrix &R)
-{
- unsigned int i,j ;
-
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3; j++)
- (*this)[i][j] = R[i][j] ;
-}
-
-/*!
-
- Insert the rotational component of the homogeneous matrix from a
- theta u rotation vector.
-
-*/
-void
-vpHomogeneousMatrix::insert(const vpThetaUVector &tu)
-{
- vpRotationMatrix R(tu) ;
- insert(R) ;
-}
-
-/*!
- Insert the translational component in a homogeneous matrix.
-*/
-void
-vpHomogeneousMatrix::insert(const vpTranslationVector &T)
-{
- (*this)[0][3] = T[0] ;
- (*this)[1][3] = T[1] ;
- (*this)[2][3] = T[2] ;
-}
-
-/*!
-
- Insert the rotational component of the homogeneous matrix from a
- quaternion rotation vector.
-
-*/
-void
-vpHomogeneousMatrix::insert(const vpQuaternionVector &q){
- insert(vpRotationMatrix(q));
-}
-
-/*!
- Invert the homogeneous matrix
-
- \return \f$\left[\begin{array}{cc}
- {\bf R} & {\bf t} \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right]^{-1} = \left[\begin{array}{cc}
- {\bf R}^T & -{\bf R}^T {\bf t} \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right]\f$
-
-*/
-vpHomogeneousMatrix
-vpHomogeneousMatrix::inverse() const
-{
- vpHomogeneousMatrix Mi ;
-
-
- vpRotationMatrix R ; extract(R) ;
- vpTranslationVector T ; extract(T) ;
-
- vpTranslationVector RtT ; RtT = -(R.t()*T) ;
-
-
- Mi.insert(R.t()) ;
- Mi.insert(RtT) ;
-
- return Mi ;
-}
-
-/*!
- Set transformation to identity.
-*/
-void vpHomogeneousMatrix::eye()
-{
- (*this)[0][0] = 1 ;
- (*this)[1][1] = 1 ;
- (*this)[2][2] = 1 ;
-
- (*this)[0][1] = (*this)[0][2] = 0 ;
- (*this)[1][0] = (*this)[1][2] = 0 ;
- (*this)[2][0] = (*this)[2][1] = 0 ;
-
- (*this)[0][3] = 0 ;
- (*this)[1][3] = 0 ;
- (*this)[2][3] = 0 ;
-}
-
-/*!
- Invert the homogeneous matrix.
-
- \param M : The inverted homogenous matrix: \f$\left[\begin{array}{cc}
- {\bf R} & {\bf t} \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right]^{-1} = \left[\begin{array}{cc}
- {\bf R}^T & -{\bf R}^T {\bf t} \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right]\f$
-
-*/
-void
-vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const
-{
- M = inverse() ;
-}
-
-
-/*!
- Write an homogeneous matrix in an output file stream.
-
- \param f : Output file stream. The homogeneous matrix is saved as a
- 4 by 4 matrix.
-
- The code below shows how to save an homogenous matrix in a file.
-
- \code
- // Contruct an homogeneous matrix
- vpTranslationVector t(1,2,3);
- vpRxyzVector r(M_PI, 0, -M_PI/4.);
- vpRotationMatrix R(r);
- vpHomogeneousMatrix M(t, R);
-
- // Save the content of the matrix in "homogeneous.dat"
- std::ofstream f("homogeneous.dat");
- M.save(f);
- \endcode
-
- \sa load()
-*/
-void
-vpHomogeneousMatrix::save(std::ofstream &f) const
-{
- if (! f.fail())
- {
- f << *this ;
- }
- else
- {
- vpERROR_TRACE("\t\t file not open " );
- throw(vpException(vpException::ioError, "\t\t file not open")) ;
- }
-}
-
-
-/*!
-
- Read an homogeneous matrix from an input file stream. The
- homogeneous matrix is considered as a 4 by 4 matrix.
-
- \param f : Input file stream.
-
- The code below shows how to get an homogenous matrix from a file.
-
- \code
- vpHomogeneousMatrix M;
-
- std::ifstream f("homogeneous.dat");
- M.load(f);
- \endcode
-
- \sa save()
-*/
-void
-vpHomogeneousMatrix::load(std::ifstream &f)
-{
- if (! f.fail())
- {
- for (unsigned int i=0 ; i < 4 ; i++)
- for (unsigned int j=0 ; j < 4 ; j++)
- {
- f >> (*this)[i][j] ;
- }
- }
- else
- {
- vpERROR_TRACE("\t\t file not open " );
- throw(vpException(vpException::ioError, "\t\t file not open")) ;
- }
-}
-
-//! Print the matrix as a vector [T thetaU]
-void
-vpHomogeneousMatrix::print()
-{
- vpPoseVector r(*this) ;
- std::cout << r.t() ;
-}
-//! Basic initialisation (identity)
-void
-vpHomogeneousMatrix::setIdentity()
-{
- init() ;
-}
-
-/*!
- Converts an homogenous matrix to a vector of 12 floats.
- \param M : Converted matrix.
- */
-void vpHomogeneousMatrix::convert(std::vector<float> &M)
-{
- M.resize(12);
- for(unsigned int i=0; i < 12; i++)
- M[i] = (float)(this->data[i]);
-}
-
-/*!
- Converts an homogenous matrix to a vector of 12 doubles.
- \param M : Converted matrix.
- */
-void vpHomogeneousMatrix::convert(std::vector<double> &M)
-{
- M.resize(12);
- for(unsigned int i=0; i < 12; i++)
- M[i] = this->data[i];
-}
diff --git a/src/math/transformation/vpHomogeneousMatrix.h b/src/math/transformation/vpHomogeneousMatrix.h
deleted file mode 100644
index a12a8bc..0000000
--- a/src/math/transformation/vpHomogeneousMatrix.h
+++ /dev/null
@@ -1,220 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHomogeneousMatrix.h 5130 2015-01-06 18:50:43Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Homogeneous matrix.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpHomogeneousMatrix.h
- \brief Definition and computation on the homogeneous matrices
-*/
-
-
-#ifndef VPHOMOGENEOUSMATRIX_HH
-#define VPHOMOGENEOUSMATRIX_HH
-
-class vpPoseVector;
-class vpMatrix;
-class vpRotationMatrix;
-class vpPoseVector;
-class vpThetaUVector;
-
-#include <vector>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpPoseVector.h>
-
-#include <fstream>
-
-class vpMatrix;
-class vpRotationMatrix;
-class vpPoseVector;
-class vpThetaUVector;
-class vpQuaternionVector;
-
-/*!
- \class vpHomogeneousMatrix
-
- \ingroup PoseTransformation
-
- \brief The class provides a data structure for the homogeneous matrices
- as well as a set of operations on these matrices.
-
- The vpHomogeneousMatrix is derived from vpMatrix.
-
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-
-
- An homogeneous matrix is 4x4 matrix defines as
- \f[
- ^a{\bf M}_b = \left(\begin{array}{cc}
- ^a{\bf R}_b & ^a{\bf t}_b \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right)
- \f]
- that defines the position of frame <em>b</em> in frame <em>a</em>
-
- \f$ ^a{\bf R}_b \f$ is a rotation matrix and
- \f$ ^a{\bf t}_b \f$ is a translation vector.
-
-
-*/
-class VISP_EXPORT vpHomogeneousMatrix : public vpMatrix
-{
- public:
- //! Basic constructor.
- vpHomogeneousMatrix() ;
- //! Copy constructor.
- vpHomogeneousMatrix(const vpHomogeneousMatrix &M) ;
- //! Construction from translation vector and rotation matrix.
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- //! Construction from translation vector and theta u rotation vector.
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu) ;
- //! Construction from translation vector and quaternion rotation vector.
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) ;
- /*!
- Construction from translation vector and theta u rotation vector
- defined as a pose vector.
- */
- vpHomogeneousMatrix(const vpPoseVector &p) ;
-
- //! Construction from translation and rotation defined as a theta u vector.
- vpHomogeneousMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
- vpHomogeneousMatrix(const std::vector<float> &v);
- vpHomogeneousMatrix(const std::vector<double> &v);
-
- //! Construction from translation vector and rotation matrix.
- void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- //! Construction from translation vector and theta u rotation vector.
- void buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu) ;
- //! Construction from translation vector and quaternion rotation vector.
- void buildFrom(const vpTranslationVector &t, const vpQuaternionVector& q) ;
- void buildFrom(const std::vector<float> &v) ;
- void buildFrom(const std::vector<double> &v) ;
-
- /*!
- Construction from translation vector and theta u rotation vector
- defined as a pose vector.
- */
- void buildFrom(const vpPoseVector &p) ;
-
- //! Construction from translation and rotation defined as a theta u vector.
- void buildFrom(const double tx,const double ty, const double tz,
- const double tux,const double tuy, const double tuz ) ;
-
- void convert(std::vector<float> &M);
- void convert(std::vector<double> &M);
-
- /*!
- Return the translation vector from the homogeneous transformation matrix.
- */
- vpTranslationVector getTranslationVector()
- {
- vpTranslationVector tr;
- this->extract(tr);
- return tr;
- }
-// /*!
-// Return the rotation matrix from the homogeneous transformation matrix.
-// */
-// vpThetaUVector getThetaUVector()
-// vpRotationMatrix getRotationMatrix()
-// {
-// vpRotationMatrix R;
-// this->extract(R);
-// return R;
-// }
-
- //! Copy operator from vpHomogeneousMatrix.
- vpHomogeneousMatrix &operator=(const vpHomogeneousMatrix &M);
-
- //! Multiply two homogeneous matrices: aMb = aMc*cMb
- vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const;
-
- //! Multiply by a vector ! size 4 !!!
- vpColVector operator*(const vpColVector &v) const;
-
- // Invert the homogeneous matrix.
- vpHomogeneousMatrix inverse() const ;
- // Invert the homogeneous matrix.
- void inverse(vpHomogeneousMatrix &Mi) const;
-
- // Test if the rotational part of the matrix is a rotation matrix.
- bool isAnHomogeneousMatrix() const ;
-
- void insert(const vpRotationMatrix &R) ;
- void insert(const vpThetaUVector &tu) ;
- void insert(const vpTranslationVector &t) ;
- void insert(const vpQuaternionVector &t) ;
-
- void extract( vpRotationMatrix &R) const;
- void extract(vpTranslationVector &t) const;
- void extract(vpThetaUVector &tu) const;
- void extract(vpQuaternionVector& q) const;
-
- // Load an homogeneous matrix from a file
- void load(std::ifstream &f) ;
- // Save an homogeneous matrix in a file
- void save(std::ofstream &f) const ;
-
- // Set to identity
- void eye();
- //! Basic initialisation (identity).
- void init() ;
- // Basic initialisation (identity).
- void setIdentity() ;
-
- //! Print the matrix as a vector [T thetaU]
- void print() ;
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpPoseVector.cpp b/src/math/transformation/vpPoseVector.cpp
deleted file mode 100644
index bb472e2..0000000
--- a/src/math/transformation/vpPoseVector.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseVector.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpPoseVector.cpp
- \brief Pose vector.
-
-*/
-
-#include <visp/vpPoseVector.h>
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpException.h>
-
-/*!
- Set the size of the vector to 6.
-*/
-void
-vpPoseVector::init()
-{
- resize(6) ;
-}
-
-/*!
-
- Default constructor.
-
- Construct a 6 dimension pose vector \f$ [\bf t, \Theta \bf
- u]^\top\f$ where \f$ \Theta \bf u\f$ is a rotation vector \f$[\Theta
- u_x, \Theta u_y, \Theta u_z]^\top\f$ and \f$ \bf t \f$ is a
- translation vector \f$[t_x, t_y, t_z]^\top\f$.
-
- The pose vector is initialized to zero.
-
-*/
-vpPoseVector::vpPoseVector()
-{
- init() ;
-}
-
-/*!
-
- Construct a 6 dimension pose vector \f$ [\bf{t}, \Theta
- \bf{u}]^\top\f$ from 3 translations and 3 \f$ \Theta \bf{u}\f$
- angles.
-
- Translations are expressed in meters, while rotations in radians.
-
- \param tx,ty,tz : Translations \f$[t_x, t_y, t_z]^\top\f$
- respectively along the x, y and z axis (in meters).
-
- \param tux,tuy,tuz : Rotations \f$[\Theta u_x, \Theta u_y, \Theta
- u_z]^\top\f$ respectively around the x, y and z axis (in radians).
-
-*/
-vpPoseVector::vpPoseVector(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz)
-{
- init() ;
-
- (*this)[0] = tx ;
- (*this)[1] = ty ;
- (*this)[2] = tz ;
-
- (*this)[3] = tux ;
- (*this)[4] = tuy ;
- (*this)[5] = tuz ;
-}
-
-/*!
-
- Construct a 6 dimension pose vector \f$ [\bf t, \Theta \bf
- u]^\top\f$ from a translation vector \f$ \bf t \f$ and a \f$\Theta
- \bf u\f$ vector.
-
- \param tv : Translation vector \f$ \bf t \f$.
- \param tu : \f$\Theta \bf u\f$ rotation vector.
-
-*/
-vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
- const vpThetaUVector& tu)
-{
- init() ;
- buildFrom(tv,tu) ;
-}
-
-/*!
-
- Construct a 6 dimension pose vector \f$ [\bf t, \Theta \bf
- u]^\top\f$ from a translation vector \f$ \bf t \f$ and a rotation
- matrix \f$ \bf R \f$.
-
- \param tv : Translation vector \f$ \bf t \f$.
-
- \param R : Rotation matrix \f$ \bf R \f$ from which \f$\Theta \bf
- u\f$ vector is extracted to initialise the pose vector.
-
-*/
-vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
- const vpRotationMatrix& R)
-{
- init() ;
- buildFrom(tv,R) ;
-}
-
-/*!
-
- Construct a 6 dimension pose vector \f$ [\bf t, \Theta \bf
- u]^\top\f$ from an homogeneous matrix \f$ \bf M \f$.
-
- \param M : Homogeneous matrix \f$ \bf M \f$ from which translation
- \f$ \bf t \f$ and \f$\Theta \bf u \f$ vectors are extracted to
- initialize the pose vector.
-
-*/
-vpPoseVector::vpPoseVector(const vpHomogeneousMatrix& M)
-{
- init() ;
- buildFrom(M) ;
-}
-
-/*!
- Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$ from
- an homogeneous matrix \f$ \bf M \f$.
-
- \param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
- \bf t \f$ and \f$\Theta \bf u \f$ vectors are extracted to initialize
- the pose vector.
-
- \return The build pose vector.
-
-*/
-vpPoseVector
-vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
-{
- vpRotationMatrix R ; M.extract(R) ;
- vpTranslationVector tv ; M.extract(tv) ;
- buildFrom(tv,R) ;
- return *this ;
-}
-
-/*!
-
- Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$
- from a translation vector \f$ \bf t \f$ and a \f$\Theta \bf u\f$
- vector.
-
- \param tv : Translation vector \f$ \bf t \f$.
- \param tu : \f$\Theta \bf u\f$ rotation vector.
-
- \return The build pose vector.
-*/
-vpPoseVector
-vpPoseVector::buildFrom(const vpTranslationVector& tv,
- const vpThetaUVector& tu)
-{
- for (unsigned int i =0 ; i < 3 ; i++)
- {
- (*this)[i] = tv[i] ;
- (*this)[i+3] = tu[i] ;
- }
- return *this ;
-}
-
-/*!
-
- Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$
- from a translation vector \f$ \bf t \f$ and a rotation matrix \f$
- \bf R \f$.
-
- \param tv : Translation vector \f$ \bf t \f$.
-
- \param R : Rotation matrix \f$ \bf R \f$ from which \f$\Theta \bf
- u\f$ vector is extracted to initialise the pose vector.
-
- \return The build pose vector.
-*/
-vpPoseVector
-vpPoseVector::buildFrom(const vpTranslationVector& tv,
- const vpRotationMatrix& R)
-{
- vpThetaUVector tu ;
- tu.buildFrom(R) ;
-
- buildFrom(tv,tu) ;
- return *this ;
-}
-
-
-
-/*!
-
- Prints to the standart stream the pose vector.
-
- \warning Values concerning the \f$ \Theta \bf u\f$ rotation are
- converted in degrees.
-
- The following code
- \code
- // Create a pose vector
- vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
- r.print();
- \endcode
- produces the output:
-
- \code
- 1 2 3 180 -180 0
- \endcode
-*/
-void
-vpPoseVector::print()
-{
- for (unsigned int i =0 ; i < 6 ; i++)
- if (i<3) std::cout << (*this)[i] <<" " ;
- else std::cout << vpMath::deg((*this)[i]) <<" " ;
- std::cout <<std::endl ;
-
-}
-
-/*!
-
- Save the pose vector in the output file stream.
-
- \param f : Output file stream. Should be open before entering in this method.
-
- \exception vpException::ioError : If the output stream is not open.
-
- \sa load()
-*/
-void
-vpPoseVector::save(std::ofstream &f) const
-{
- if (! f.fail())
- {
- f << *this ;
- }
- else
- {
- vpERROR_TRACE("\t\t file not open " );
- throw(vpException(vpException::ioError, "\t\t file not open")) ;
- }
-}
-
-
-/*!
- Read a pose vector from an input file stream.
-
- \param f : The input file stream..Should be open before entering in
- this method.
-
- \exception vpException::ioError : If the input file stream is not open.
-
- \sa save()
-*/
-void
-vpPoseVector::load(std::ifstream &f)
-{
- if (! f.fail())
- {
- for (unsigned int i=0 ; i < 6 ; i++)
- {
- f >> (*this)[i] ;
- }
- }
- else
- {
- vpERROR_TRACE("\t\t file not open " );
- throw(vpException(vpException::ioError, "\t\t file not open")) ;
- }
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpPoseVector.h b/src/math/transformation/vpPoseVector.h
deleted file mode 100644
index 607fdcd..0000000
--- a/src/math/transformation/vpPoseVector.h
+++ /dev/null
@@ -1,186 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoseVector.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpPOSEVECTOR_H
-#define vpPOSEVECTOR_H
-
-/*!
- \file vpPoseVector.h
-
- \brief Pose representation. A pose is a 6 dimension vector [t,tu]^T
- where tu is a rotation vector (theta u representation) and t is a
- translation vector.
-*/
-class vpRotationMatrix;
-class vpHomogeneousMatrix;
-class vpTranslationVector;
-class vpThetaUVector;
-
-#include <visp/vpMatrix.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-class vpRotationMatrix;
-class vpHomogeneousMatrix;
-class vpTranslationVector;
-class vpThetaUVector;
-
-
-/*!
- \class vpPoseVector
-
- \ingroup PoseTransformation
-
- \brief The pose is a complete representation of every rigid motion in the
- euclidian space.
-
- It is composed of a translation and a rotation
- minimaly represented by a 6 dimension pose vector as: \f[ ^{a}{\bf
- r}_b = [^{a}{\bf t}_{b},\Theta {\bf u}]^\top \in R^6\f]
-
- where \f$ ^{a}{\bf r}_b \f$ is the pose from frame \f$ a \f$ to
- frame \f$ b \f$, with \f$ ^{a}{\bf t}_{b} \f$ being the translation
- vector between these frames along the x,y,z
- axis and \f$\Theta \bf u \f$, the \f$\Theta \bf u \f$ representation of the
- rotation \f$^{a}\bf{R}_{b}\f$ between these frames.
-
- To know more about the \f$\Theta \bf u\f$ rotation representation,
- see vpThetaUVector documentation.
-
-*/
-class VISP_EXPORT vpPoseVector : public vpColVector
-{
-
- private:
- // initialize a size 6 vector
- void init() ;
-
- public:
- // constructor
- vpPoseVector() ;
- // constructor from 3 angles (in radian)
- vpPoseVector(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
- // constructor convert an homogeneous matrix in a pose
- vpPoseVector(const vpHomogeneousMatrix& M) ;
- // constructor convert a translation and a "thetau" vector into a pose
- vpPoseVector(const vpTranslationVector& t,
- const vpThetaUVector& tu) ;
- // constructor convert a translation and a rotation matrix into a pose
- vpPoseVector(const vpTranslationVector& t,
- const vpRotationMatrix& R) ;
-
-
- // convert an homogeneous matrix in a pose
- vpPoseVector buildFrom(const vpHomogeneousMatrix& M) ;
- // convert a translation and a "thetau" vector into a pose
- vpPoseVector buildFrom(const vpTranslationVector& t,
- const vpThetaUVector& tu) ;
- // convert a translation and a rotation matrix into a pose
- vpPoseVector buildFrom(const vpTranslationVector& t,
- const vpRotationMatrix& R) ;
-
-
- /*!
- Set the value of an element of the pose vector: r[i] = x.
-
- \param i : Pose vector element index
-
- \code
- // Create a pose vector with translation and rotation set to zero
- vpPoseVector r;
-
- // Initialize the pose vector
- r[0] = 1;
- r[1] = 2;
- r[2] = 3;
- r[3] = M_PI;
- r[4] = -M_PI;
- r[5] = 0;
- \endcode
-
- This code produces the same effect:
- \code
- vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
- \endcode
-
- */
- inline double &operator [](unsigned int i) { return *(data + i); }
- /*!
- Get the value of an element of the pose vector: x = r[i].
-
- \param i : Pose vector element index
-
- \code
- vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
-
- double tx,ty,tz; // Translation
- double tux, tuy,tuz; // Theta u rotation
- tx = r[0];
- ty = r[1];
- tz = r[2];
- tux = r[3];
- tuy = r[4];
- tuz = r[5];
- \endcode
- */
- inline const double &operator [](unsigned int i) const { return *(data+i); }
-
- // Load an homogeneous matrix from a file
- void load(std::ifstream &f) ;
- // Save an homogeneous matrix in a file
- void save(std::ofstream &f) const ;
-
- // Print a vector [T thetaU] thetaU in degree
- void print() ;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/math/transformation/vpQuaternionVector.cpp b/src/math/transformation/vpQuaternionVector.cpp
deleted file mode 100644
index f00cab0..0000000
--- a/src/math/transformation/vpQuaternionVector.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpQuaternionVector.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Quaternion vector.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-
-#include <visp/vpQuaternionVector.h>
-#include <visp/vpMath.h>
-#include <stdio.h>
-#include <string.h>
-#include <algorithm>
-
-
-// minimum value of sine
-const double vpQuaternionVector::minimum = 0.0001;
-
-/*!
- \file vpQuaternionVector.cpp
- \brief Defines a quaternion and common operations on it.
-*/
-
-//! Constructor from doubles.
-vpQuaternionVector::vpQuaternionVector(const double x_, const double y_,
- const double z_,const double w_)
- : vpRotationVector(4)
-{
- set(x_, y_, z_, w_);
-}
-
-/*!
- Constructs a quaternion from a rotation matrix.
-
- \param R : Matrix containing a rotation.
-*/
-vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R)
- : vpRotationVector(4)
-{
- buildFrom(R);
-}
-
-/*!
- Manually change values of a quaternion.
- \param x_ : x quaternion parameter.
- \param y_ : y quaternion parameter.
- \param z_ : z quaternion parameter.
- \param w_ : w quaternion parameter.
-*/
-void vpQuaternionVector::set(const double x_, const double y_,
- const double z_,const double w_)
-{
- r[0]=x_;
- r[1]=y_;
- r[2]=z_;
- r[3]=w_;
-}
-
-
-/*!
- Quaternion addition.
-
- Adds two quaternions. Addition is component-wise.
-
- \param q : quaternion to add.
-*/
-vpQuaternionVector vpQuaternionVector::operator+( vpQuaternionVector &q)
-{
- return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
-}
-/*!
- Quaternion substraction.
-
- Substracts a quaternion from another. Substraction is component-wise.
-
- \param q : quaternion to substract.
-*/
-vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)
-{
- return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
-}
-
-//! Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
-vpQuaternionVector vpQuaternionVector::operator-()
-{
- return vpQuaternionVector(-x(), -y(), -z(), -w());
-}
-
-//! Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
-vpQuaternionVector vpQuaternionVector::operator*( double l)
-{
- return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
-}
-
-//! Multiply two quaternions.
-vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {
- return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
- w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
- w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
- w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
-}
-
-/*!
- Constructs a quaternion from a rotation matrix.
-
- \param R : Rotation matrix.
-*/
-void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
-{
- vpThetaUVector tu(R);
- vpColVector u;
- double theta;
- tu.extract(theta, u);
-
- theta *= 0.5;
-
- double sinTheta_2 = sin(theta);
- set( u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta) );
-}
diff --git a/src/math/transformation/vpQuaternionVector.h b/src/math/transformation/vpQuaternionVector.h
deleted file mode 100644
index a41a41f..0000000
--- a/src/math/transformation/vpQuaternionVector.h
+++ /dev/null
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpQuaternionVector.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Quaternion definition.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-
-
-#ifndef __QUATERNIONVECTOR_H__
-#define __QUATERNIONVECTOR_H__
-
-/*!
- \file vpQuaternionVector.h
-
- \brief Class that consider the case of a quaternion and basic
- operations on it.
-
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRotationVector.h>
-
-
-/*!
- \class vpQuaternionVector
-
- \ingroup RotTransformation
-
- \brief Defines a quaternion and its basic operations.
-
- A quaternion is defined by four values: \f${\bf q} = (x, y, z, w)\f$.
-
- This class allows to compute a quaternion from a rotation matrix
- using either vpQuaternionVector(const vpRotationMatrix &) constructor
- or buildFrom() method.
-
- It also defines common operations on a quaternion such as:
- - multiplication (scalar and quaternion)
- - addition
- - substraction.
-
- */
-class VISP_EXPORT vpQuaternionVector : public vpRotationVector
-{
-private:
- static const double minimum;
-public:
-
- /*! Default constructor that initialize all the angles to zero. */
- vpQuaternionVector() : vpRotationVector(4) {}
- /*! Copy constructor. */
- vpQuaternionVector(const vpQuaternionVector &q) : vpRotationVector(q) {}
- vpQuaternionVector(const double x, const double y, const double z,const double w) ;
- vpQuaternionVector(const vpRotationMatrix &R);
-
- void buildFrom(const vpRotationMatrix& R);
-
- void set(const double x, const double y, const double z,const double w) ;
-
- //! Returns x-component of the quaternion.
- inline double x() const {return r[0];}
- //! Returns y-component of the quaternion.
- inline double y() const {return r[1];}
- //! Returns z-component of the quaternion.
- inline double z() const {return r[2];}
- //! Returns w-component of the quaternion.
- inline double w() const {return r[3];}
-
- vpQuaternionVector operator+( vpQuaternionVector &q) ;
- vpQuaternionVector operator-( vpQuaternionVector &q) ;
- vpQuaternionVector operator-() ;
- vpQuaternionVector operator*(const double l) ;
- vpQuaternionVector operator*( vpQuaternionVector &rq) ;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 4
- * End:
- */
diff --git a/src/math/transformation/vpRotationMatrix.cpp b/src/math/transformation/vpRotationMatrix.cpp
deleted file mode 100644
index ee2dbdb..0000000
--- a/src/math/transformation/vpRotationMatrix.cpp
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRotationMatrix.cpp 4900 2014-09-11 09:16:21Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rotation matrix.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpRotationMatrix.cpp
- \brief Class that consider
- the particular case of rotation matrix
-*/
-
-
-#include <visp/vpMath.h>
-#include <visp/vpMatrix.h>
-
-// Rotation classes
-#include <visp/vpRotationMatrix.h>
-
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-#include <math.h>
-const double vpRotationMatrix::threshold = 1e-6;
-const double vpRotationMatrix::minimum = 0.00001;
-
-#define vpDEBUG_LEVEL1 0
-
-
-/*!
- Initializes a 3x3 rotation matrix as identity
-*/
-void
-vpRotationMatrix::init()
-{
- unsigned int i,j ;
-
- try {
- resize(3,3) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
- else
- (*this)[i][j] = 0.0;
-
-}
-
-/*!
- Initializes the rotation matrix as identity.
-
- \sa eye()
-*/
-void
-vpRotationMatrix::setIdentity()
-{
- init() ;
-}
-/*!
- Initialize the rotation matrix as identity.
-
- \sa setIdentity()
-*/
-void
-vpRotationMatrix::eye()
-{
- init() ;
-}
-
-/*!
- Affectation of two rotation matrix.
-
- \param m : *this = m
-*/
-vpRotationMatrix &
-vpRotationMatrix::operator=(const vpRotationMatrix &m)
-{
- for (unsigned int i=0; i<3; i++)
- {
- for (unsigned int j=0; j<3; j++)
- {
- rowPtrs[i][j] = m.rowPtrs[i][j];
- }
- }
-
- return *this;
-}
-
-/*!
- Affectation of two rotation matrix
-
- \param m : *this = m
-*/
-vpRotationMatrix &
-vpRotationMatrix::operator=(const vpMatrix &m)
-{
-
- if ((m.getCols() !=3) &&(m.getRows() !=3))
- {
- vpERROR_TRACE("m is not a rotation matrix !!!!! ") ;
- throw(vpMatrixException(vpMatrixException::forbiddenOperatorError,
- "m is not a rotation matrix !!!!!"));
- }
-
- for (unsigned int i=0; i<3; i++)
- {
- for (unsigned int j=0; j<3; j++)
- {
- (*this)[i][j] = m[i][j];
- }
- }
-
- if (isARotationMatrix() == false)
- {
- vpERROR_TRACE("m is not a rotation matrix !!!!! ") ;
- throw(vpMatrixException(vpMatrixException::forbiddenOperatorError,
- "m is not a rotation matrix !!!!!"));
- }
-
- return *this;
-}
-
-//! operation C = A * B (A is unchanged)
-vpRotationMatrix
-vpRotationMatrix::operator*(const vpRotationMatrix &B) const
-{
- vpRotationMatrix p ;
-
- for (unsigned int i=0;i<3;i++)
- for (unsigned int j=0;j<3;j++)
- {
- double s =0 ;
- for (unsigned int k=0;k<3;k++)
- s +=rowPtrs[i][k] * B.rowPtrs[k][j];
- p[i][j] = s ;
- }
- return p;
-}
-/*!
- Operation C = A * B (A is unchanged).
- Allows for example to multiply a rotation matrix by a skew matrix.
- \code
- vpRotationMatrix A;
- vpTranslationVector t;
- vpMatrix B = t.skew();
- vpMatrix C = A * B;
- \endcode
-
- \exception vpMatrixException::incorrectMatrixSizeError : If B is not
- a 3 by 3 dimension matrix.
-
-*/
-vpMatrix
-vpRotationMatrix::operator*(const vpMatrix &B) const
-{
- if (B.getRows() != 3 || B.getCols() != 3) {
- vpERROR_TRACE("The matrix is not a 3 by 3 dimension matrix") ;
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "The matrix is not a 3 by 3 dimension matrix"));
- }
- vpRotationMatrix p ;
-
- for (unsigned int i=0;i<3;i++)
- for (unsigned int j=0;j<3;j++)
- {
- double s =0 ;
- for (unsigned int k=0;k<3;k++)
- s +=(*this)[i][k] * B[k][j];
- p[i][j] = s ;
- }
- return p;
-}
-
-/*!
-
- Operator that allows to multiply a rotation matrix by a 3 dimension
- column vector.
-
- \param v : Three dimension column vector.
-
- \return The product of the rotation matrix by the column vector
-
- \exception vpMatrixException::incorrectMatrixSizeError If the column
- vector \e v is not a 3 dimension vector.
-
- The code below shows how to use this operator.
-\code
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector p1(3), p2(3);
- vpRotationMatrix R;
-
- p2 = R * p1;
-
- return 0;
-}
-\endcode
-
-*/
-vpColVector
-vpRotationMatrix::operator*(const vpColVector &v) const
-{
- if (v.getRows() != 3) {
- vpERROR_TRACE("The column vector is not a 3 dimension vector") ;
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "The column vector is not a 3 dimension vector"));
- }
- vpColVector c;
- vpMatrix::multMatrixVector(*this, v, c);
- return c;
-}
-
-
-/*! overload + operator (to say it forbidden operation, throw exception)
-
-
- \exception Cannot add two rotation matrices !!!!!
- vpMatrixException::forbiddenOperatorError
- */
-vpRotationMatrix
-vpRotationMatrix::operator+(const vpRotationMatrix &/* B*/) const
-{
- vpERROR_TRACE("Cannot add two rotation matrices !!!!! ") ;
- throw(vpMatrixException(vpMatrixException::forbiddenOperatorError,
- "Cannot add two rotation matrices !!!!!"));
-}
-
-/*! overload - operator (to say it forbidden operation, throw exception)
-
-
- \exception Cannot substract two rotation matrices !!!!!
- vpMatrixException::forbiddenOperatorError
- */
-vpRotationMatrix
-vpRotationMatrix::operator-(const vpRotationMatrix &/* B */) const
-{
- vpERROR_TRACE("Cannot substract two rotation matrices !!!!! ") ;
- throw(vpMatrixException(vpMatrixException::forbiddenOperatorError,
- "Cannot substract two rotation matrices !!!!!"));
-}
-
-//! operation c = A * b (A is unchanged)
-vpTranslationVector
-vpRotationMatrix::operator*(const vpTranslationVector &mat) const
-{
- vpTranslationVector p ;
-
- for (unsigned int j=0;j<3;j++)p[j]=0 ;
-
- for (unsigned int j=0;j<3;j++) {
- for (unsigned int i=0;i<3;i++) {
- p[i]+=rowPtrs[i][j] * mat[j];
- }
- }
-
- return p;
-}
-
-/*********************************************************************/
-
-/*!
- \brief test if the 3x3 rotational part of the rotation matrix is really a rotation matrix
-*/
-
-bool
-vpRotationMatrix::isARotationMatrix() const
-{
- unsigned int i,j ;
- bool isRotation = true ;
-
- // test R^TR = Id ;
- vpRotationMatrix RtR = (*this).t()*(*this) ;
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3 ; j++)
- if (i==j)
- {
- if (fabs(RtR[i][j]-1) > threshold) isRotation = false ;
- }
- else
- {
- if (fabs(RtR[i][j]) > threshold) isRotation = false ;
- }
-
- // test if it is a basis
- // test || Ci || = 1
- for (i=0 ; i < 3 ; i++)
- if ((sqrt(vpMath::sqr(RtR[0][i]) +
- vpMath::sqr(RtR[1][i]) +
- vpMath::sqr(RtR[2][i])) - 1) > threshold) isRotation = false ;
-
- // test || Ri || = 1
- for (i=0 ; i < 3 ; i++)
- if ((sqrt(vpMath::sqr(RtR[i][0]) +
- vpMath::sqr(RtR[i][1]) +
- vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false ;
-
- // test if the basis is orthogonal
- return isRotation ;
-}
-
-
-/*!
- \brief initialize a rotation matrix as Identity
-*/
-vpRotationMatrix::vpRotationMatrix() : vpMatrix()
-{
- init() ;
-}
-
-
-/*!
- \brief initialize a rotation matrix from another rotation matrix
-*/
-vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpMatrix()
-{
- init() ;
- (*this) = M ;
-}
-/*!
- Initialize a rotation matrix from an homogenous matrix.
-*/
-vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpMatrix()
-{
- init() ;
- buildFrom(M);
-}
-
-//! Construction from rotation (Theta U parameterization)
-vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpMatrix()
-{
- init() ;
- buildFrom(tu) ;
-}
-//! Construction from a pose vector.
-vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpMatrix()
-{
- init() ;
- buildFrom(p) ;
-}
-
-
-//! Construction from rotation (Euler parameterization, ie Rzyz parameterization)
-vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpMatrix()
-{
- init() ;
- buildFrom(euler) ;
-}
-
-
-
-//! Construction from rotation Rxyz
-vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpMatrix()
-{
- init() ;
- buildFrom(Rxyz) ;
-}
-
-//! Construction from rotation Rzyx
-vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpMatrix()
-{
- init() ;
- buildFrom(Rzyx) ;
-}
-
-//! Construction from rotation (Theta U parameterization)
-vpRotationMatrix::vpRotationMatrix(const double tux,
- const double tuy,
- const double tuz) : vpMatrix()
-{
- init() ;
- buildFrom(tux, tuy, tuz) ;
-}
-
-//! Construct from rotation in quaternion representation
-vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector& q){
- init();
- buildFrom(q);
-}
-
-
-/*!
- \brief transpose
- R^T
-*/
-vpRotationMatrix
-vpRotationMatrix::t() const
-{
- vpRotationMatrix Rt ;
-
- unsigned int i,j;
- for (i=0;i<3;i++)
- for (j=0;j<3;j++)
- Rt[j][i] = (*this)[i][j];
-
- return Rt;
-
-}
-
-/*!
- \brief inverse the rotation matrix
-
- \f$ R^-1 = R^T \f$
-*/
-vpRotationMatrix vpRotationMatrix::inverse() const
-{
- vpRotationMatrix Ri = (*this).t() ;
-
- return Ri ;
-}
-
-/*!
- \brief inverse the rotation matrix
-
- \f$ R^-1 = R^T \f$
-*/
-void
-vpRotationMatrix::inverse(vpRotationMatrix &M) const
-{
- M = inverse() ;
-}
-
-
-//! std::cout an rotation matrix [thetaU]
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpRotationMatrix &R)
-{
- for (unsigned int i=0; i<3; i++)
- {
- for (unsigned int j=0; j<3; j++)
- std::cout << R[i][j] << " " ;
- std::cout << std::endl ;
- }
-
- return (s);
-}
-
-//! Print the matrix as a vector [thetaU]
-void
-vpRotationMatrix::printVector()
-{
- vpThetaUVector tu(*this) ;
-
- for (unsigned int i=0; i<3; i++)
- std::cout << tu[i] << " " ;
-
- std::cout << std::endl ;
-}
-
-
-/*
- \relates vpRotationMatrix
- Transform a vector vpThetaUVector into a rotation matrix.
-
- Representation theta u (angle and axes of the rotation) is considered for
- the rotation vector.
-
- The rotation is computed using :
- \f[
- R = \cos{ \theta} \; {I}_{3} + (1 - \cos{ \theta}) \; v v^{T} + \sin{ \theta} \; [v]_\times
- \f]
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpThetaUVector &v)
-{
- unsigned int i,j;
- double theta, si, co, sinc, mcosc;
- vpRotationMatrix R;
-
- theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
- si = sin(theta);
- co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
-
- R[0][0] = co + mcosc*v[0]*v[0];
- R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
- R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
- R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
- R[1][1] = co + mcosc*v[1]*v[1];
- R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
- R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
- R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
- R[2][2] = co + mcosc*v[2]*v[2];
-
- for (i=0;i<3;i++) for (j=0;j<3;j++) (*this)[i][j] = R[i][j];
-
-#if (vpDEBUG_LEVEL1) // test new version wrt old version
- {
- // old version
- vpRotationMatrix R_old; // has to be replaced by (*this) if good version
- double sinu,cosi,mcosi,u[3],ang;
-
- ang = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
- if (ang > minimum)
- {
- for (i=0;i<3;i++) u[i] = v[i]/ang;
- sinu = sin(ang);
- cosi = cos(ang);
- mcosi = 1-cosi;
- R_old[0][0] = cosi + mcosi*u[0]*u[0];
- R_old[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
- R_old[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
- R_old[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
- R_old[1][1] = cosi + mcosi*u[1]*u[1];
- R_old[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
- R_old[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
- R_old[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
- R_old[2][2] = cosi + mcosi*u[2]*u[2];
- }
- else
- {
- for (i=0;i<3;i++)
- {
- for(j=0;j<3;j++) R_old[i][j] = 0.0;
- R_old[i][i] = 1.0;
- }
- }
- // end old version
- // test the new version
-
- unsigned int pb = 0;
- for (i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
- if (fabs(R_old[i][j] - R[i][j]) > 1.e-4) pb = 1;
- }
- if (pb == 1)
- {
- printf("vpRotationMatrix::buildFrom(const vpThetaUVector &v)\n");
- std::cout << " theta " << theta << std::endl;
- std::cout << " R : " << std::endl << R << std::endl;
- std::cout << " R_old : " << std::endl << R_old << std::endl;
- }
-
- }
- // end test
-#endif
-
- return *this ;
-}
-
-/*!
- Build a rotation matrix from an homogenous matrix.
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
-{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- (*this)[i][j] = M[i][j] ;
-
- return *this ;
-}
-
-/*
- \relates vpRotationMatrix
- Transform a pose vector into a rotation matrix.
-
- \sa buildFrom(const vpThetaUVector &)
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpPoseVector &p)
-{
- vpThetaUVector tu(p);
- return buildFrom(tu);
-}
-
-/*!
- Transform a vector representing the euler angle
- into a rotation matrix.
- Rzyz = Rot(\f$ z,\phi \f$) Rot(\f$ y,\theta \f$) Rot(\f$ z,\psi \f$)
-
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRzyzVector &v)
-{
- double c0,c1,c2,s0,s1,s2;
-
- c0 = cos(v[0]);
- c1 = cos(v[1]);
- c2 = cos(v[2]);
- s0 = sin(v[0]);
- s1 = sin(v[1]);
- s2 = sin(v[2]);
-
- (*this)[0][0] = c0*c1*c2 - s0*s2;
- (*this)[0][1] = -c0*c1*s2 - s0*c2;
- (*this)[0][2] = c0*s1;
- (*this)[1][0] = s0*c1*c2+c0*s2 ;
- (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
- (*this)[1][2] = s0*s1;
- (*this)[2][0] = -s1*c2;
- (*this)[2][1] = s1*s2;
- (*this)[2][2] = c1;
-
- return (*this) ;
-}
-
-
-/*!
-
- Transform a vector representing the Rxyz angle into a rotation
- matrix.
-
- Rxyz(\f$ \phi,\theta, \psi \f$) = Rot(\f$ x, \psi \f$) Rot(\f$ y, \theta \f$
- ) Rot(\f$ z,\phi \f$)
-
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRxyzVector &v)
-{
- double c0,c1,c2,s0,s1,s2;
-
- c0 = cos(v[0]);
- c1 = cos(v[1]);
- c2 = cos(v[2]);
- s0 = sin(v[0]);
- s1 = sin(v[1]);
- s2 = sin(v[2]);
-
- (*this)[0][0] = c1*c2;
- (*this)[0][1] = -c1*s2;
- (*this)[0][2] = s1;
- (*this)[1][0] = c0*s2+s0*s1*c2;
- (*this)[1][1] = c0*c2-s0*s1*s2;
- (*this)[1][2] = -s0*c1;
- (*this)[2][0] = -c0*s1*c2+s0*s2;
- (*this)[2][1] = c0*s1*s2+c2*s0;
- (*this)[2][2] = c0*c1;
-
- return (*this) ;
-}
-
-
-
-/*!
- Transform a vector representing the Rzyx angle
- into a rotation matrix.
-
- Rxyz(\f$ \phi, \theta , \psi \f$)
- Rot(\f$ z, \psi \f$) Rot(\f$ y, \theta \f$)Rot(\f$ x, \phi \f$)
-*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRzyxVector &v)
-{
- double c0,c1,c2,s0,s1,s2;
-
- c0 = cos(v[0]);
- c1 = cos(v[1]);
- c2 = cos(v[2]);
- s0 = sin(v[0]);
- s1 = sin(v[1]);
- s2 = sin(v[2]);
-
- (*this)[0][0] = c0*c1 ;
- (*this)[0][1] = c0*s1*s2 - s0*c2 ;
- (*this)[0][2] = c0*s1*c2 + s0*s2 ;
-
- (*this)[1][0] = s0*c1 ;
- (*this)[1][1] = s0*s1*s2 + c0*c2 ;
- (*this)[1][2] = s0*s1*c2 - c0*s2 ;
-
- (*this)[2][0] = -s1 ;
- (*this)[2][1] = c1*s2 ;
- (*this)[2][2] = c1*c2 ;
-
- return (*this) ;
-}
-
-
-
-//! Construction from rotation (theta U parameterization)
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const double tux,
- const double tuy,
- const double tuz)
-{
- vpThetaUVector tu(tux, tuy, tuz) ;
- buildFrom(tu) ;
- return *this ;
-}
-
-
-//! Construction from rotation (as quaternion)
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpQuaternionVector& q){
- double a = q.w();
- double b = q.x();
- double c = q.y();
- double d = q.z();
- (*this)[0][0] = a*a+b*b-c*c-d*d;
- (*this)[0][1] = 2*b*c-2*a*d;
- (*this)[0][2] = 2*a*c+2*b*d;
-
- (*this)[1][0] = 2*a*d+2*b*c;
- (*this)[1][1] = a*a-b*b+c*c-d*d;
- (*this)[1][2] = 2*c*d-2*a*b;
-
- (*this)[2][0] = 2*b*d-2*a*c;
- (*this)[2][1] = 2*a*b+2*c*d;
- (*this)[2][2] = a*a-b*b-c*c+d*d;
- return *this;
-}
-#undef vpDEBUG_LEVEL1
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRotationMatrix.h b/src/math/transformation/vpRotationMatrix.h
deleted file mode 100644
index c45f087..0000000
--- a/src/math/transformation/vpRotationMatrix.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRotationMatrix.h 5037 2014-12-05 19:06:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rotation matrix.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpROTATIONMATRIX_H
-#define vpROTATIONMATRIX_H
-
-/*!
- \file vpRotationMatrix.h
- \brief Class that consider the particular case of rotation matrix
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRzyxVector.h>
-#include <visp/vpRzyzVector.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpQuaternionVector.h>
-#include <visp/vpPoseVector.h>
-
-/*!
- \class vpRotationMatrix
-
- \ingroup RotTransformation
-
- \brief The vpRotationMatrix considers the particular case of
- a rotation matrix.
-
- It is derived from vpMatrix.
-
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
-
-*/
-class VISP_EXPORT vpRotationMatrix : public vpMatrix
-{
- friend class vpMatrix;
- friend class vpHomogeneousMatrix;
- friend class vpRxyzVector;
- friend class vpRzyzVector;
- friend class vpRzyxVector;
- friend class vpThetaUVector;
- friend class vpTranslationVector;
- friend class vpPoseVector;
-public:
- //! Basic initialisation (identity)
- void init() ;
-
- //! Basic initialisation (identity)
- void setIdentity() ;
- void eye();
- //! Default constructor.
- vpRotationMatrix() ;
- //! Copy constructor.
- vpRotationMatrix(const vpRotationMatrix &R) ;
- //! Copy constructor.
- vpRotationMatrix(const vpHomogeneousMatrix &M) ;
- //! Construction from rotation (theta U parameterization)
- vpRotationMatrix(const vpThetaUVector &r) ;
- //! Construction from a pose vector.
- vpRotationMatrix(const vpPoseVector &p) ;
- //! Construction from rotation (Euler parameterization)
- vpRotationMatrix(const vpRzyzVector &r) ;
- //! Construction from rotation Rxyz
- vpRotationMatrix(const vpRxyzVector &r) ;
- //! Construction from rotation Rzyx
- vpRotationMatrix(const vpRzyxVector &r) ;
- //! Construction from rotation (theta U parameterization)
- vpRotationMatrix(const double tux, const double tuy, const double tuz) ;
-
- vpRotationMatrix(const vpQuaternionVector& q);
-
-// /*!
-// Return the \f$\theta u\f$ vector that corresponds to tha rotation matrix.
-// */
-// vpThetaUVector getThetaUVector()
-// {
-// vpThetaUVector tu;
-// tu.buildFrom(*this);
-// return tu;
-// }
-
- //! copy operator from vpRotationMatrix
- vpRotationMatrix &operator=(const vpRotationMatrix &R);
- //! copy operator from vpMatrix (handle with care)
- vpRotationMatrix &operator=(const vpMatrix &m) ;
- //! operation c = A * b (A is unchanged)
- vpTranslationVector operator*(const vpTranslationVector &mat) const ;
- //! operation C = A * B (A is unchanged)
- vpRotationMatrix operator*(const vpRotationMatrix &B) const ;
- //! operation C = A * B (A is unchanged)
- vpMatrix operator*(const vpMatrix &B) const ;
- // operation v2 = A * v1 (A is unchanged)
- vpColVector operator*(const vpColVector &v) const ;
- //! overload + operator (to say it forbidden operation, throw exception)
- vpRotationMatrix operator+(const vpRotationMatrix &B) const ;
- //! overload - operator (to say it forbidden operation, throw exception)
- vpRotationMatrix operator-(const vpRotationMatrix &B) const ;
-
- //! transpose
- vpRotationMatrix t() const ;
-
- //! invert the rotation matrix
- vpRotationMatrix inverse() const ;
- //! invert the rotation matrix
- void inverse(vpRotationMatrix &M) const;
-
- //! test if the matrix is an rotation matrix
- bool isARotationMatrix() const ;
-
- //! Print the matrix as a vector [T thetaU]
- void printVector() ;
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpRotationMatrix &m);
-
- //! Build a rotation matrix from an homogeneous matrix.
- vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M) ;
- //! Transform a vector vpThetaUVector into a rotation matrix
- vpRotationMatrix buildFrom(const vpThetaUVector &v) ;
- //! Transform a pose vector into a rotation matrix
- vpRotationMatrix buildFrom(const vpPoseVector &p) ;
- //! Transform a vector reprensenting the euler (Rzyz) angle
- //! into a rotation matrix
- vpRotationMatrix buildFrom(const vpRzyzVector &v) ;
- //! Transform a vector reprensenting the Rxyz angle into a rotation matrix
- vpRotationMatrix buildFrom(const vpRxyzVector &v) ;
- //! Transform a vector reprensenting the Rzyx angle into a rotation matrix
- vpRotationMatrix buildFrom(const vpRzyxVector &v) ;
- //! Construction from rotation (theta U parameterization)
- vpRotationMatrix buildFrom(const double tux,
- const double tuy,
- const double tuz) ;
-
- vpRotationMatrix buildFrom(const vpQuaternionVector& q);
-private:
- static const double threshold;
- static const double minimum; // useful only for debug
- };
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRotationVector.cpp b/src/math/transformation/vpRotationVector.cpp
deleted file mode 100644
index 5829b3e..0000000
--- a/src/math/transformation/vpRotationVector.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRotationVector.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic rotation vector (cannot be used as is !).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpRotationVector.h>
-#include <algorithm>
-#include <math.h>
-/*!
- \file vpRotationVector.cpp
- \brief class that consider the case of a generic rotation vector
- (cannot be used as is !)
-*/
-
-
-
-/*!
- Return the transpose of the rotation vector.
-
-*/
-vpRowVector vpRotationVector::t() const
-{
- vpRowVector v(_size);
-
- for (unsigned int i=0; i< _size; i++)
- v[i] = r[i];
-
- return v;
-}
-
-/*!
- Size of the rotation vector: number of double values describing the rotation.
- Common sizes are 4 for a quaternion and 3 for angle-based rotation vectors.
-*/
-unsigned int vpRotationVector::size() const {
- return _size;
-}
-/*!
-
- Print the values of the three angles on the output stream. Data are
- formatted as a column vector.
-
- \code
-#include <iostream>
-#include <visp/vpRxyzVector.h>
-
-int main()
-{
- vpRxyzVector r; // By default initialized to zero
-
- std::cout << "Rxyz rotation vector: " << std::endl << r << std::endl;
-}
- \endcode
-
- will lead to the following printing on the standart stream:
-
- \code
-Rxyz rotation vector:
-0
-0
-0
- \endcode
-*/
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpRotationVector &m)
-{
- std::ios::fmtflags original_flags( s.flags() );
- s.precision(10) ;
-
- for (unsigned int i=0; i < m.size(); i++)
- s << m.r[i] << "\n";
-
- s << std::endl;
-
- // Restore ostream format
- s.flags(original_flags);
-
- return s;
-}
-
-
-void vpRotationVector::init(const unsigned int vector_size){
- this->_size = vector_size;
- r = new double[this->_size];
- std::fill(r,r+this->_size,0.);
-}
-
-vpRotationVector::~vpRotationVector(){
- delete[] r;
-}
diff --git a/src/math/transformation/vpRotationVector.h b/src/math/transformation/vpRotationVector.h
deleted file mode 100644
index 965ee37..0000000
--- a/src/math/transformation/vpRotationVector.h
+++ /dev/null
@@ -1,167 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRotationVector.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic rotation vector (cannot be used as is !).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpRotationVECTOR_H
-#define vpRotationVECTOR_H
-
-/*!
- \file vpRotationVector.h
- \brief class that consider the case of a generic rotation vector
- (cannot be used as is !)
-*/
-
-
-
-
-#include <visp/vpMath.h>
-#include <visp/vpRowVector.h>
-#include <stdio.h>
-#include <iostream>
-
-
-#include <math.h>
-
-/*!
- \class vpRotationVector
-
- \ingroup RotTransformation
-
- \brief Class that consider the case of a generic rotation vector
- (cannot be used as is !) consisting in three angles.
-
- The code below shows how this class can be used to manipulate a vpRxyzVector.
-
- \code
-#include <iostream>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpRxyzVector r; // By default initialized to zero
- r[0] = vpMath::rad(45); // Rotation around x set to 45 degres converted in radians
- r[1] = M_PI; // Rotation around y set to PI radians
- r[2] = 0; // Rotation around z set to 0 radians
-
- std::cout << "Rxyz rotation vector: " << r << std::endl;
-
- double rx = r[0]; // Get the value of the angle around x axis
- double ry = r[1]; // Get the value of the angle around y axis
- double rz = r[2]; // Get the value of the angle around z axis
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpRotationVector
-{
- friend class vpColVector;
-protected:
- double *r ;
- unsigned int _size;
- void init(const unsigned int size);
-public:
- //! Constructor that constructs a vector of size 3 and initialize all values to zero.
- vpRotationVector()
- : r(NULL), _size(0)
- {
- init(3);
- }
-
- //! Constructor that constructs a vector of size n and initialize all values to zero.
- vpRotationVector(const unsigned int n)
- : r(NULL), _size(n)
- {
- init(n);
- }
- /*!
- Copy operator.
- */
- vpRotationVector(const vpRotationVector &v)
- : r(NULL), _size(0)
- {
- *this = v;
- }
-
- virtual ~vpRotationVector();
-
- /*!
- Operator that allows to set the value of an element of the rotation
- vector: r[i] = value
- */
- inline double &operator [](unsigned int n) { return *(r + n); }
- /*!
- Operator that allows to get the value of an element of the rotation
- vector: value = r[i]
- */
- inline const double &operator [](unsigned int n) const { return *(r+n); }
-
- /*!
- Affectation of two vectors.
- */
- vpRotationVector &operator=(const vpRotationVector &v)
- {
- init(v.size());
- for (unsigned int i=0; i<_size; i++)
- {
- r[i] = v.r[i] ;
- }
- return *this;
- }
-
- /*! Returns the size of the rotation vector
- */
- unsigned int size() const;
-
- // Transpose of the rotation vector.
- vpRowVector t() const;
-
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpRotationVector &m);
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRxyzVector.cpp b/src/math/transformation/vpRxyzVector.cpp
deleted file mode 100644
index cc2218e..0000000
--- a/src/math/transformation/vpRxyzVector.cpp
+++ /dev/null
@@ -1,176 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRxyzVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rxyz angle parameterization for the rotation.
- * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpRxyzVector.h>
-#include <math.h>
-/*!
- \file vpRxyzVector.cpp
- \brief class that consider the case of the Rxyz angle parameterization for the rotation :
- Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
-*/
-
-
-/*!
- Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
- angles from a rotation matrix.
- \param R : Rotation matrix used to initialize the Euler angles.
-*/
-vpRxyzVector::vpRxyzVector(const vpRotationMatrix& R)
-{
- buildFrom(R) ;
-}
-
-/*!
- Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
- angles vector from a \f$\theta u\f$ vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input to initialize the Euler angles.
-*/
-vpRxyzVector::vpRxyzVector(const vpThetaUVector& tu)
-{
- buildFrom(tu) ;
-}
-
-/*!
- Convert a rotation matrix into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
- angles vector.
-
- \param R : Rotation matrix used as input.
- \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRxyzVector
-vpRxyzVector::buildFrom(const vpRotationMatrix& R)
-{
-
- double COEF_MIN_ROT = 1e-6;
- double phi ;
-
- if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
- else phi = atan2(-R[1][2], R[2][2]) ;
-
- double si = sin(phi) ;
- double co = cos(phi) ;
- double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
- double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);
-
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
-
- if (0) // test new version wrt old version
- {
- // old version
-
- double v1;
- double r2[3]; // has to be replaced by r below if good version
-
- v1 = R[0][2];
- if (v1 > 1.0 ) v1 = 1.0;
- if (v1 < -1.0 ) v1 = -1.0;
- r2[1] = asin(v1);
- if ( fabs(fabs(r2[1]) - M_PI_2) < 0.00001)
- {
- r2[0] = 0.0;
- r2[2] = atan2(R[1][0],R[1][1]);
- }
- else
- {
- r2[0] = atan2(-R[1][2],R[2][2]);
- r2[2] = atan2(-R[0][1],R[0][0]);
- }
- // verification of the new version
- int pb = 0;
- int i;
- for (i=0;i<3;i++)
- {
- if (fabs(r[i] - r2[i]) > 1e-5) pb = 1;
- }
- if (pb == 1)
- {
- printf("vpRxyzVector::buildFrom(const vpRotationMatrix& R)\n");
- printf(" r : %lf %lf %lf\n",r[0],r[1],r[2]);
- printf(" r2 : %lf %lf %lf\n",r2[0],r2[1],r2[2]);
- printf(" r - r2 : %lf %lf %lf\n",r[0]-r2[0],r[1]-r2[1],r[2]-r2[2]);
- }
- }
-
- // What is below corresponds to another representation, but which one???
- /* double phi ;
- if ((fabs(R[1][2]) < 1e-6)&&(fabs(R[2][2]) < 1e-6)) phi = 0 ;
- else phi = atan2(R[1][2], R[2][2]) ;
-
- double si = sin(phi) ;
- double co = cos(phi) ;
- double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
- double psi = atan2(co*R[0][1]+si*R[0][2], co*R[1][1]+si*R[1][2]);
-
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;*/
-
- return *this ;
-}
-
-/*!
- Convert a \f$\theta u\f$ vector into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input.
- \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRxyzVector
-vpRxyzVector::buildFrom(const vpThetaUVector& tu)
-{
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
-
- return *this ;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRxyzVector.h b/src/math/transformation/vpRxyzVector.h
deleted file mode 100644
index ec1f258..0000000
--- a/src/math/transformation/vpRxyzVector.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRxyzVector.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rxyz angle parameterization for the rotation.
- * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpRxyzVECTOR_H
-#define vpRxyzVECTOR_H
-
-/*!
- \file vpRxyzVector.h
-
- \brief Class that consider the case of the Rxyz angle
- parameterization for the rotation.
-
- Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpRotationVector.h>
-#include <visp/vpRotationMatrix.h>
-
-class vpRotationMatrix;
-class vpThetaUVector;
-
-/*!
- \class vpRxyzVector
-
- \ingroup RotTransformation
-
- \brief Class that consider the case of the Euler
- \f$(\varphi,\theta,\psi)\f$ angle using the x-y-z convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
- rotation angles around the \f$x\f$, \f$y\f$ and \f$z\f$ axis.
-
- \f[R_{xyz}(\varphi,\theta,\psi) = R_x(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
-
- with
-
- \f[R_{x}(\varphi) = \left(
- \begin{array}{ccc}
- 1 & 0 & 0 \\
- 0 &\cos \varphi & -\sin\varphi \\
- 0 &\sin \varphi & \cos\varphi \\
- \end{array}
- \right) \;
- R_{y}(\theta) = \left(
- \begin{array}{ccc}
- \cos \theta & 0 & \sin\theta\\
- 0 & 1 & 0 \\
- -\sin\theta & 0 &\cos \theta
- \end{array}
- \right) \;
- R_{z}(\psi) = \left(
- \begin{array}{ccc}
- \cos \psi & -\sin\psi & 0\\
- \sin\psi &\cos \psi& 0 \\
- 0 & 0 & 1
- \end{array}
- \right)\f]
-
- The rotation matrix corresponding to the x-y-z convention is given by:
-
- \f[
- R_{xyz}(\varphi,\theta,\psi) = \left(
- \begin{array}{ccc}
- \cos\theta \cos\psi & -\cos\theta \sin\psi & \sin\theta \\
- \sin\varphi \sin\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \sin\theta \sin\psi +\cos\varphi\cos\psi & -\sin\varphi \cos\theta \\
- -\cos\varphi \sin\theta \cos\psi + \sin\varphi\sin\psi & \cos\varphi \sin\theta \sin\psi +\sin\varphi\cos\psi & \cos\varphi \cos\theta
- \end{array}
- \right)
- \f]
-
- The code below shows first how to initialize this representation of
- Euler angles, than how to contruct a rotation matrix from a
- vpRxyzVector and finaly how to extract the vpRxyzVector Euler angles
- from the build rotation matrix.
-
- \code
-#include <iostream>
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRxyzVector.h>
-
-int main()
-{
- vpRxyzVector rxyz;
-
- // Initialise the Euler angles
- rxyz[0] = vpMath::rad( 45.f); // phi angle in rad around x axis
- rxyz[1] = vpMath::rad(-30.f); // theta angle in rad around y axis
- rxyz[2] = vpMath::rad( 90.f); // psi angle in rad around z axis
-
- // Construct a rotation matrix from the Euler angles
- vpRotationMatrix R(rxyz);
-
- // Extract the Euler angles around x,y,z axis from a rotation matrix
- rxyz.buildFrom(R);
-
- // Print the extracted Euler angles. Values are the same than the
- // one used for initialization
- std::cout << rxyz;
-
- // Since the rotation vector is 3 values column vector, the
- // transpose operation produce a row vector.
- vpRowVector rxyz_t = rxyz.t();
-
- // Print the transpose row vector
- std::cout << rxyz_t << std::endl;
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpRxyzVector : public vpRotationVector
-{
- friend class vpRotationMatrix;
- friend class vpThetaUVector;
-
- public:
- /*! Default constructor that initialize all the angles to zero. */
- vpRxyzVector() {}
- /*! Copy constructor. */
- vpRxyzVector(const vpRxyzVector &rxyz) : vpRotationVector(rxyz) {}
-
- /*!
- Constructor from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$x\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
- */
- vpRxyzVector(const double phi, const double theta, const double psi) :
- vpRotationVector (3) { r[0]=phi;r[1]=theta;r[2]=psi; }
-
- // initialize a Rxyz vector from a rotation matrix
- vpRxyzVector(const vpRotationMatrix& R) ;
-
- // initialize a Rxyz vector from a ThetaU vector
- vpRxyzVector(const vpThetaUVector& tu) ;
-
- /*!
- Construction from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$x\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
- */
- void buildFrom(const double phi, const double theta, const double psi)
- {
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
- }
-
- // convert a rotation matrix into Rxyz vector
- vpRxyzVector buildFrom(const vpRotationMatrix& R) ;
-
- // convert a ThetaU vector into a Rxyz vector
- vpRxyzVector buildFrom(const vpThetaUVector& tu) ;
-
-} ;
-
-#endif
diff --git a/src/math/transformation/vpRzyxVector.cpp b/src/math/transformation/vpRzyxVector.cpp
deleted file mode 100644
index abeab6c..0000000
--- a/src/math/transformation/vpRzyxVector.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRzyxVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rzyx angle parameterization for the rotation.
- * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpRzyxVector.h>
-#include <math.h>
-/*!
- \file vpRzyxVector.cpp
- \brief class that consider the case of the Rzyx angle parameterization for the rotation :
- Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta,Rot(x,psi)
-
-*/
-
-/*!
- Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
- angles from a rotation matrix.
- \param R : Rotation matrix used to initialize the Euler angles.
-*/
-vpRzyxVector::vpRzyxVector(const vpRotationMatrix& R)
-{
- buildFrom(R) ;
-}
-
-
-/*!
- Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
- angles vector from a \f$\theta u\f$ vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input to initialize the Euler angles.
-*/
-vpRzyxVector::vpRzyxVector(const vpThetaUVector& tu)
-{
- buildFrom(tu) ;
-}
-
-/*!
- Convert a rotation matrix into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
- angles vector.
-
- Source: R. Paul, Robot Manipulators: Mathematics, Programming, and Control.
- MIT Press, 1981, p. 71
-
- \param R : Rotation matrix used as input.
- \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRzyxVector
-vpRzyxVector::buildFrom(const vpRotationMatrix& R)
-{
- double nx = R[0][0];
- double ny = R[1][0];
-
- double phi = atan2(ny,nx) ;
- double si = sin(phi) ;
- double co = cos(phi) ;
-
- double nz = R[2][0];
- double theta = atan2(-nz, co*nx+si*ny) ;
-
- double ax = R[0][2];
- double ay = R[1][2];
- double ox = R[0][1];
- double oy = R[1][1];
-
- double psi = atan2(si*ax-co*ay,-si*ox+co*oy);
-
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
-
- return *this ;
-}
-
-
-/*!
- Convert a \f$\theta u\f$ vector into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input.
- \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRzyxVector
-vpRzyxVector::buildFrom(const vpThetaUVector& tu)
-{
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
-
- return *this ;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRzyxVector.h b/src/math/transformation/vpRzyxVector.h
deleted file mode 100644
index 329c4b4..0000000
--- a/src/math/transformation/vpRzyxVector.h
+++ /dev/null
@@ -1,204 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRzyxVector.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Rzyx angle parameterization for the rotation.
- * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpRzyxVector_h
-#define vpRzyxVector_h
-
-/*!
- \file vpRzyxVector.h
-
- \brief class that consider the case of the Rzyx angle
- parameterization for the rotation.
-
- Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
-*/
-
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRotationVector.h>
-
-class vpRotationMatrix;
-class vpThetaUVector;
-
-/*!
- \class vpRzyxVector
-
- \ingroup RotTransformation
-
- \brief Class that consider the case of the Euler
- \f$(\varphi,\theta,\psi)\f$ angle using the z-y-x convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
- rotation angles around the \f$z\f$, \f$y\f$ and \f$x\f$ axis.
-
- \f[R_{zyx}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_x(\psi)\f]
-
- with
-
- \f[
- R_{z}(\varphi) = \left(
- \begin{array}{ccc}
- \cos \varphi & -\sin\varphi & 0\\
- \sin\varphi &\cos \varphi& 0 \\
- 0 & 0 & 1
- \end{array}
- \right) \;
- R_{y}(\theta) = \left(
- \begin{array}{ccc}
- \cos \theta & 0 & \sin\theta\\
- 0 & 1 & 0 \\
- -\sin\theta & 0 &\cos \theta
- \end{array}
- \right) \;
- R_{x}(\psi) = \left(
- \begin{array}{ccc}
- 1 & 0 & 0 \\
- 0 &\cos \psi & -\sin\psi \\
- 0 &\sin \psi & \cos\psi \\
- \end{array}
- \right)
- \f]
-
- The rotation matrix corresponding to the z-y-x convention is given by:
-
- \f[
- R_{zyx}(\varphi,\theta,\psi) = \left(
- \begin{array}{ccc}
- \cos\varphi \cos\theta & -\sin\varphi \cos\psi + \cos\varphi\sin\theta\sin\psi & \sin\varphi \sin\psi +\cos\varphi\sin\theta\cos\psi \\
- \sin\varphi \cos\theta & \cos\varphi\cos\psi + \sin\varphi\sin\theta \sin\psi & -\cos\varphi \sin\psi +\sin\varphi\sin\theta\cos\psi \\
- -\sin\theta & \cos\theta \sin\psi & \cos\theta \cos\psi
- \end{array}
- \right)
- \f]
-
- The code below shows first how to initialize this representation of
- Euler angles, than how to contruct a rotation matrix from a
- vpRzyxVector and finaly how to extract the vpRzyxVector Euler angles
- from the build rotation matrix.
-
- \code
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRzyxVector.h>
-
-int main()
-{
- vpRzyxVector rzyx;
-
- // Initialise the Euler angles
- rzyx[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
- rzyx[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
- rzyx[2] = vpMath::rad( 90.f); // psi angle in rad/s around x axis
-
- // Construct a rotation matrix from the Euler angles
- vpRotationMatrix R(rzyx);
-
- // Extract the Euler angles around z,y,x axis from a rotation matrix
- rzyx.buildFrom(R);
-
- // Print the extracted Euler angles. Values are the same than the
- // one used for initialization
- std::cout << rzyx;
-
- // Since the rotation vector is 3 values column vector, the
- // transpose operation produce a row vector.
- vpRowVector rzyx_t = rzyx.t();
-
- // Print the transpose row vector
- std::cout << rzyx_t << std::endl;
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpRzyxVector : public vpRotationVector
-{
- friend class vpRotationMatrix;
- friend class vpThetaUVector;
-
-public:
- /*! Default constructor that initialize all the angles to zero. */
- vpRzyxVector() {}
- /*! Copy constructor. */
- vpRzyxVector(const vpRzyxVector &rzyx) : vpRotationVector(rzyx) {}
-
- /*!
- Constructor from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$x\f$ axis.
- */
- vpRzyxVector(const double phi, const double theta, const double psi) :
- vpRotationVector (3) { r[0]=phi;r[1]=theta;r[2]=psi; }
-
- // initialize a Rzyx vector from a rotation matrix
- vpRzyxVector(const vpRotationMatrix& R) ;
-
- // initialize a Rzyx vector from a ThetaU vector
- vpRzyxVector(const vpThetaUVector& tu) ;
-
- /*!
- Construction from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$x\f$ axis.
- */
- void buildFrom(const double phi, const double theta, const double psi)
- {
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
- }
-
- // convert a rotation matrix into Rzyx vector
- vpRzyxVector buildFrom(const vpRotationMatrix& R) ;
-
- // convert a ThetaU vector into a Rzyx vector
- vpRzyxVector buildFrom(const vpThetaUVector& R) ;
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpRzyzVector.cpp b/src/math/transformation/vpRzyzVector.cpp
deleted file mode 100644
index d0d0eb2..0000000
--- a/src/math/transformation/vpRzyzVector.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRzyzVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Euler angles parameterization for the rotation.
- * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpRzyzVector.h>
-#include <math.h>
-
-/*!
- \file vpRzyzVector.cpp
- \brief class that consider the case of the Rzyz angle parameterization for the rotation :
- Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
-*/
-
-/*!
- Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
- angles from a rotation matrix.
- \param R : Rotation matrix used to initialize the Euler angles.
-*/
-vpRzyzVector::vpRzyzVector(const vpRotationMatrix& R)
-{
- buildFrom(R) ;
-}
-
-/*!
- Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
- angles vector from a \f$\theta u\f$ vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input to initialize the Euler angles.
-*/
-vpRzyzVector::vpRzyzVector(const vpThetaUVector& tu)
-{
- buildFrom(tu) ;
-}
-
-
-/*!
- Convert a rotation matrix into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
- angles vector.
-
- \param R : Rotation matrix used as input.
- \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRzyzVector
-vpRzyzVector::buildFrom(const vpRotationMatrix& R)
-{
- double phi ;
- if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
- phi = 0 ;
- else
- phi = atan2(R[1][2],R[0][2]) ;
- double cphi = cos(phi) ;
- double sphi = sin(phi) ;
-
- double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);
-
- double psi = atan2(-sphi*R[0][0]+cphi*R[1][0],-sphi*R[0][1]+cphi*R[1][1]) ;
-
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
-
- return *this ;
-}
-
-
-/*!
- Convert a \f$\theta u\f$ vector into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta u\f$ representation of a rotation used here as
- input.
- \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
-*/
-vpRzyzVector
-vpRzyzVector::buildFrom(const vpThetaUVector& tu)
-{
-
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
-
- return *this ;
-}
-
-/*!
-
- Initialize each element of the vector to the same angle value \e v.
-
- \param v : Angle value to set for each element of the vector.
-
-\code
-#include <visp/vpMath.h>
-#include <visp/vpRzyzVector.h>
-
-int main()
-{
- vpRzyzVector r;
-
- // Initialise the rotation vector
- r = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
-}
-\endcode
-*/
-vpRzyzVector &vpRzyzVector::operator=(double v)
-{
- for (int i=0; i< 3; i++)
- r[i] = v;
-
- return *this;
-}
-
diff --git a/src/math/transformation/vpRzyzVector.h b/src/math/transformation/vpRzyzVector.h
deleted file mode 100644
index 90865e9..0000000
--- a/src/math/transformation/vpRzyzVector.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRzyzVector.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Euler angles parameterization for the rotation.
- * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpRzyzVector_h
-#define vpRzyzVector_h
-
-/*!
- \file vpRzyzVector.h
- \brief class that consider the case of the Rzyz angles parameterization
- for the rotation
-
- Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
-*/
-
-class vpRotationMatrix;
-class vpThetaUVector;
-
-#include <visp/vpRotationVector.h>
-#include <visp/vpRotationMatrix.h>
-
-/*!
- \class vpRzyzVector
-
- \ingroup RotTransformation
-
- \brief Class that consider the case of the Euler
- \f$(\varphi,\theta,\psi)\f$ angles using the z-y-z convention, where
- \f$(\varphi,\theta,\psi)\f$ are respectively the rotation angles
- around the \f$z\f$, \f$y\f$ and \f$z\f$ axis.
-
- \f[R_{zyz}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
-
- with
-
- \f[
- R_{z}(\varphi) = \left(
- \begin{array}{ccc}
- \cos \varphi & -\sin\varphi & 0\\
- \sin\varphi &\cos \varphi& 0 \\
- 0 & 0 & 1
- \end{array}
- \right) \;
- R_{y}(\theta) = \left(
- \begin{array}{ccc}
- \cos \theta & 0 & \sin\theta\\
- 0 & 1 & 0 \\
- -\sin\theta & 0 &\cos \theta
- \end{array}
- \right) \;
- R_{z}(\psi) = \left(
- \begin{array}{ccc}
- \cos \psi & -\sin\psi & 0\\
- \sin\psi &\cos \psi& 0 \\
- 0 & 0 & 1
- \end{array}
- \right)
- \f]
-
- The rotation matrix corresponding to the z-y-z convention is given by:
-
- \f[
- R_{zyz}(\varphi,\theta,\psi) = \left(
- \begin{array}{ccc}
- \cos\varphi \cos\theta \cos\psi - \sin\varphi\sin\psi & -\cos\varphi \cos\theta \sin\psi -\sin\varphi\cos\psi & \cos\varphi \sin\theta \\
- \sin\varphi \cos\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \cos\theta \sin\psi +\cos\varphi\cos\psi & \sin\varphi \sin\theta \\
- -\sin\theta \cos\psi & \sin\theta \sin\psi & \cos\theta
- \end{array}
- \right)
- \f]
-
- The code below shows first how to initialize this representation of
- Euler angles, than how to contruct a rotation matrix from a
- vpRzyzVector and finaly how to extract the vpRzyzVector Euler angles
- from the build rotation matrix.
-
- \code
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRzyzVector.h>
-
-int main()
-{
- vpRzyzVector rzyz;
-
- // Initialise the Euler angles
- rzyz[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
- rzyz[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
- rzyz[2] = vpMath::rad( 90.f); // psi angle in rad/s around z axis
-
- // Construct a rotation matrix from the Euler angles
- vpRotationMatrix R(rzyz);
-
- // Extract the Euler angles around z,y,z axis from a rotation matrix
- rzyz.buildFrom(R);
-
- // Print the extracted Euler angles. Values are the same than the
- // one used for initialization
- std::cout << rzyz;
-
- // Since the rotation vector is 3 values column vector, the
- // transpose operation produce a row vector.
- vpRowVector rzyz_t = rzyz.t();
-
- // Print the transpose row vector
- std::cout << rzyz_t << std::endl;
-}
- \endcode
-*/
-class VISP_EXPORT vpRzyzVector : public vpRotationVector
-{
- friend class vpRotationMatrix;
- friend class vpThetaUVector;
-
- public:
- /*! Default constructor that initialize all the angles to zero. */
- vpRzyzVector() {}
- /*! Copy constructor. */
- vpRzyzVector(const vpRzyzVector &rzyz) : vpRotationVector(rzyz) {}
-
- /*!
- Constructor from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
- */
- vpRzyzVector(const double phi, const double theta, const double psi) :
- vpRotationVector (3) { r[0]=phi;r[1]=theta;r[2]=psi; }
-
- // initialize a Rzyz vector from a rotation matrix
- vpRzyzVector(const vpRotationMatrix& R);
-
- // initialize a Rzyz vector from a ThetaU vector
- vpRzyzVector(const vpThetaUVector& tu);
-
- /*!
- Construction from 3 angles (in radian).
- \param phi : \f$\varphi\f$ angle around the \f$z\f$ axis.
- \param theta : \f$\theta\f$ angle around the \f$y\f$ axis.
- \param psi : \f$\psi\f$ angle around the \f$z\f$ axis.
- */
- void buildFrom(const double phi, const double theta, const double psi)
- {
- r[0] = phi ;
- r[1] = theta ;
- r[2] = psi ;
- }
-
- // convert a rotation matrix into Rzyz vector
- vpRzyzVector buildFrom(const vpRotationMatrix& R) ;
-
- // convert a ThetaU vector into a Rzyz vector
- vpRzyzVector buildFrom(const vpThetaUVector& R) ;
-
- vpRzyzVector &operator=(double x) ;
-
-} ;
-#endif
-
diff --git a/src/math/transformation/vpThetaUVector.cpp b/src/math/transformation/vpThetaUVector.cpp
deleted file mode 100644
index 3ceb8ed..0000000
--- a/src/math/transformation/vpThetaUVector.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpThetaUVector.cpp 4792 2014-07-18 11:56:02Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Theta U parameterization for the rotation.
-*
-* Authors:
-* Eric Marchand
-*
-*****************************************************************************/
-
-/*!
-\file vpThetaUVector.cpp
-\brief class that consider the case of the Theta U parameterization for the
-rotation
-*/
-
-
-
-#include <visp/vpThetaUVector.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#define vpDEBUG_LEVEL1 0
-
-const double vpThetaUVector::minimum = 0.0001;
-
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from an homogeneous matrix.
-*/
-vpThetaUVector::vpThetaUVector(const vpHomogeneousMatrix& M)
-{
- buildFrom(M) ;
-}
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from a pose vector.
-*/
-vpThetaUVector::vpThetaUVector(const vpPoseVector& p)
-{
- buildFrom(p) ;
-}
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from a rotation matrix.
-*/
-vpThetaUVector::vpThetaUVector(const vpRotationMatrix& R)
-{
- buildFrom(R) ;
-}
-
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
-representation vector.
-*/
-vpThetaUVector::vpThetaUVector(const vpRzyxVector& rzyx)
-{
- buildFrom(rzyx) ;
-}
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from an Euler z-y-z
-representation vector.
-*/
-vpThetaUVector::vpThetaUVector(const vpRzyzVector& rzyz)
-{
- buildFrom(rzyz) ;
-}
-/*!
-Initialize a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
-representation vector.
-*/
-vpThetaUVector::vpThetaUVector(const vpRxyzVector& rxyz)
-{
- buildFrom(rxyz) ;
-}
-
-/*!
-Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
-{
- vpRotationMatrix R;
-
- M.extract(R);
- buildFrom(R);
-
- return *this ;
-}
-/*!
-Converts a pose vector into a \f$\theta {\bf u}\f$ vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpPoseVector& p)
-{
- for(unsigned int i=0; i<3; i++)
- r[i] = p[i+3];
-
- return *this ;
-}
-
-/*!
-Converts a rotation matrix into a \f$\theta {\bf u}\f$ vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRotationMatrix& R)
-{
- double s,c,theta,sinc;
-
- s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
- + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
- + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
- s = sqrt(s)/2.0;
- c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
- theta=atan2(s,c); /* theta in [0, PI] since s > 0 */
-
- // General case when theta != pi. If theta=pi, c=-1
- if ( (1+c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
- {
- sinc = vpMath::sinc(s,theta);
-
- r[0] = (R[2][1]-R[1][2])/(2*sinc);
- r[1] = (R[0][2]-R[2][0])/(2*sinc);
- r[2] = (R[1][0]-R[0][1])/(2*sinc);
- }
- else /* theta near PI */
- {
- if ( (R[0][0]-c) < std::numeric_limits<double>::epsilon() )
- r[0] = 0.;
- else
- r[0] = theta*(sqrt((R[0][0]-c)/(1-c)));
- if ((R[2][1]-R[1][2]) < 0) r[0] = -r[0];
-
- if ( (R[1][1]-c) < std::numeric_limits<double>::epsilon() )
- r[1] = 0.;
- else
- r[1] = theta*(sqrt((R[1][1]-c)/(1-c)));
-
- if ((R[0][2]-R[2][0]) < 0) r[1] = -r[1];
-
- if ( (R[2][2]-c) < std::numeric_limits<double>::epsilon() )
- r[2] = 0.;
- else
- r[2] = theta*(sqrt((R[2][2]-c)/(1-c)));
-
- if ((R[1][0]-R[0][1]) < 0) r[2] = -r[2];
- }
-
-#if (vpDEBUG_LEVEL1) // test new version wrt old version
- {
- // old version
- int i;
- // double s,c;
- double ang;
- double r2[3]; // has to be replaced by r below if good version
-
- s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
- + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
- + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
- s = sqrt(s)/2.0;
- c = (R[0][0]+R[1][1]+R[2][2]-1)/2.0;
- ang=atan2(s,c);
- if (ang > minimum)
- {
- if (s > minimum)
- {
- r2[0] = (R[2][1]-R[1][2])/(2*s);
- r2[1] = (R[0][2]-R[2][0])/(2*s);
- r2[2] = (R[1][0]-R[0][1])/(2*s);
- }
- else
- {
- r2[0] = (sqrt((R[0][0]-c)/(1-c)));
- if ((R[2][1]-R[1][2]) < 0) r2[0] = -r2[0];
- r2[1] = (sqrt((R[1][1]-c)/(1-c)));
- if ((R[0][2]-R[2][0]) < 0) r2[1] = -r2[1];
- r2[2] = (sqrt((R[2][2]-c)/(1-c)));
- if ((R[1][0]-R[0][1]) < 0) r2[2] = -r2[2];
- }
- for (i=0;i<3;i++) r2[i] = r2[i]*ang;
- }
- else
- {
- r2[0] = r2[1] = r2[2] = 0.0;
- }
- // end old version
- // verification of the new version
- int pb = 0;
-
- for (i=0;i<3;i++)
- {
- if (fabs(r[i] - r2[i]) > 1e-5) pb = 1;
- }
- if (pb == 1)
- {
- printf("vpThetaUVector::buildFrom(const vpRotationMatrix& R)\n");
- printf(" r : %lf %lf %lf\n",r[0],r[1],r[2]);
- printf(" r2 : %lf %lf %lf\n",r2[0],r2[1],r2[2]);
- printf(" r - r2 : %lf %lf %lf\n",r[0]-r2[0],r[1]-r2[1],r[2]-r2[2]);
- }
- // end of the verification
- }
-#endif
- return *this ;
-}
-/*!
-Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
-representation vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRzyxVector& rzyx)
-{
- vpRotationMatrix R(rzyx) ;
-
- buildFrom(R) ;
- return *this ;
-}
-/*!
-Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-z
-representation vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRzyzVector& rzyz)
-{
- vpRotationMatrix R(rzyz) ;
-
- buildFrom(R) ;
- return *this ;
-}
-/*!
-Build a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
-representation vector.
-*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRxyzVector& rxyz)
-{
- vpRotationMatrix R(rxyz) ;
-
- buildFrom(R) ;
- return *this ;
-}
-
-/*!
-
-Initialize each element of the \f$\theta {\bf u}\f$ vector to the
-same angle value \e v.
-
-\param v : Angle value to set for each element of the \f$\theta {\bf
-u}\f$ vector.
-
-\code
-#include <visp/vpMath.h>
-#include <visp/vpThetaUVector.h>
-
-int main()
-{
-vpThetaUVector tu;
-
-// Initialise the theta U rotation vector
-tu = vpMath::rad( 45.f); // All the 3 angles are set to 45 degrees
-}
-\endcode
-*/
-vpThetaUVector &vpThetaUVector::operator=(double v)
-{
- for (int i=0; i< 3; i++)
- r[i] = v;
-
- return *this;
-}
-
-/*!
-
-Extract the rotation angle \f$ \theta \f$ and the unit vector
-\f$\bf u \f$ from the \f$ \theta {\bf u} \f$ representation.
-
-\param theta : Rotation angle \f$ \theta \f$.
-
-\param u : Unit vector \f${\bf u} = (u_{x},u_{y},u_{z})^{\top} \f$
-representing the rotation axis.
-
-*/
-void
-vpThetaUVector::extract(double &theta, vpColVector &u) const
-{
- u.resize(3);
-
- theta = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
- //if (theta == 0) {
- if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
- u = 0;
- return;
- }
- for (unsigned int i=0 ; i < 3 ; i++)
- u[i] = r[i] / theta ;
-}
-
-#undef vpDEBUG_LEVEL1
diff --git a/src/math/transformation/vpThetaUVector.h b/src/math/transformation/vpThetaUVector.h
deleted file mode 100644
index 354e7b1..0000000
--- a/src/math/transformation/vpThetaUVector.h
+++ /dev/null
@@ -1,201 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpThetaUVector.h 4792 2014-07-18 11:56:02Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Theta U parameterization for the rotation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpTHETAUVECTOR_H
-#define vpTHETAUVECTOR_H
-
-/*!
- \file vpThetaUVector.h
- \brief class that consider the case of the Theta U parameterization for the
- rotation
-*/
-
-class vpHomogeneousMatrix;
-class vpRotationMatrix;
-class vpRzyxVector;
-class vpRxyzVector;
-class vpRzyzVector;
-
-#include <visp/vpRotationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRzyxVector.h>
-
-
-/*!
- \class vpThetaUVector
-
- \ingroup RotTransformation
-
- \brief Class that consider the case of the \f$\theta {\bf u}\f$
- parameterization for the rotation.
-
- The \f$\theta {\bf u}\f$ representation is one of the minimal
- representation of a rotation matrix, where
- \f${\bf u} = (u_{x} \; u_{y} \; u_{z})^{\top}\f$
- is a unit vector representing the rotation
- axis and \f$\theta\f$ is the rotation angle.
-
- From the \f$\theta {\bf u}\f$ representation it is possible to build the
- rotation matrix \f${\bf R}\f$ using the Rodrigues formula:
-
- \f[
- {\bf R} = {\bf I}_{3} + (1 - \cos{ \theta}) \; {\bf u u}^{\top} + \sin{ \theta} \; [{\bf u}]_{\times}
- \f]
-
- with \f${\bf I}_{3}\f$ the identity matrix of dimension
- \f$3\times3\f$ and \f$[{\bf u}]_{\times}\f$ the skew matrix:
-
- \f[
- [{\bf u}]_{\times} = \left(
- \begin{array}{ccc}
- 0 & -u_{z} & u_{y} \\
- u_{z} & 0 & -u_{x} \\
- -u_{y} & u_{x} & 0
- \end{array}
- \right)
- \f]
- From the implementation point of view, it is nothing more than an
- array of three floats.
-
- The code below shows first how to initialize a \f$\theta {\bf u}\f$
- vector, than how to contruct a rotation matrix from a vpThetaUVector
- and finaly how to extract the theta U angles from the build rotation
- matrix.
-
- \code
-#include <iostream>
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpThetaUVector.h>
-
-int main()
-{
- vpThetaUVector tu;
-
- // Initialise the theta U rotation vector
- tu[0] = vpMath::rad( 45.f);
- tu[1] = vpMath::rad(-30.f);
- tu[2] = vpMath::rad( 90.f);
-
- // Construct a rotation matrix from the theta U angles
- vpRotationMatrix R(tu);
-
- // Extract the theta U angles from a rotation matrix
- tu.buildFrom(R);
-
- // Print the extracted theta U angles. Values are the same than the
- // one used for initialization
- std::cout << tu;
-
- // Since the rotation vector is 3 values column vector, the
- // transpose operation produce a row vector.
- vpRowVector tu_t = tu.t();
-
- // Print the transpose row vector
- std::cout << tu_t << std::endl;
-}
- \endcode
-*/
-class VISP_EXPORT vpThetaUVector : public vpRotationVector
-{
-
-private:
- //! initialize a size 3 vector
- void init() ;
-
- static const double minimum;
-
-public:
-
- /*! Default constructor that initialize all the angles to zero. */
- vpThetaUVector() {}
- /*! Copy constructor. */
- vpThetaUVector(const vpThetaUVector &tu) : vpRotationVector(tu) {}
-
- // constructor initialize a Theta U vector from a homogeneous matrix
- vpThetaUVector(const vpHomogeneousMatrix & M) ;
- // constructor initialize a Theta U vector from a pose vector
- vpThetaUVector(const vpPoseVector & p) ;
- // constructor initialize a Theta U vector from a rotation matrix
- vpThetaUVector(const vpRotationMatrix& R) ;
- // constructor initialize a Theta U vector from a RzyxVector
- vpThetaUVector(const vpRzyxVector& rzyx) ;
- // constructor initialize a Theta U vector from a RzyzVector
- vpThetaUVector(const vpRzyzVector& rzyz) ;
- // constructor initialize a Theta U vector from a RxyzVector
- vpThetaUVector(const vpRxyzVector& rxyz) ;
-
- /*!
- Build a \f$\theta {\bf u}\f$ vector from 3 angles in radian.
- */
- vpThetaUVector(const double tux, const double tuy, const double tuz) :
- vpRotationVector (3) { r[0]=tux;r[1]=tuy;r[2]=tuz; }
-
- // convert an homogeneous matrix into Theta U vector
- vpThetaUVector buildFrom(const vpHomogeneousMatrix& M) ;
- // convert a pose vector into Theta U vector
- vpThetaUVector buildFrom(const vpPoseVector& p) ;
- // convert a rotation matrix into Theta U vector
- vpThetaUVector buildFrom(const vpRotationMatrix& R) ;
- // convert an Rzyx vector into Theta U vector
- vpThetaUVector buildFrom(const vpRzyxVector &rzyx) ;
- // convert an Rzyz vector into Theta U vector
- vpThetaUVector buildFrom(const vpRzyzVector &zyz) ;
- // convert an Rxyz vector into Theta U vector
- vpThetaUVector buildFrom(const vpRxyzVector &xyz) ;
-
- vpThetaUVector &operator=(double x) ;
-
- // extract the angle and the axis from the ThetaU representation
- void extract( double &theta, vpColVector &u) const;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
diff --git a/src/math/transformation/vpTranslationVector.cpp b/src/math/transformation/vpTranslationVector.cpp
deleted file mode 100644
index 00e7945..0000000
--- a/src/math/transformation/vpTranslationVector.cpp
+++ /dev/null
@@ -1,380 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTranslationVector.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Translation vector.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpTranslationVector.h>
-#include <stdio.h>
-#include <string.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-/*!
- \file vpTranslationVector.cpp
- \brief Class that consider the case of a translation vector.
-*/
-
-//! initialize a size 3 vector
-void vpTranslationVector::init()
-{
- resize(3) ;
-}
-
-/*!
- Contruct a translation vector \f$ \bf t \f$ from 3 doubles.
-
- \param tx,ty,tz : Translation respectively along x, y and z axis.
-
-*/
-vpTranslationVector::vpTranslationVector(const double tx,
- const double ty,
- const double tz)
-{
- init() ;
- (*this)[0] = tx ;
- (*this)[1] = ty ;
- (*this)[2] = tz ;
-}
-
-/*!
- Copy constructor.
-
- \param tv : Translation vector to copy.
-
- \code
- vpTranslationVector t1(1,2,3); // Create and initialize a translation vector
-
- vpTranslationVector t2(t1); // t2 is now a copy of t1
- \endcode
-
-*/
-vpTranslationVector::vpTranslationVector (const vpTranslationVector &tv) : vpColVector(tv)
-{
-}
-
-/*!
- Initialize a translation vector from 3 doubles.
-
- \param tx,ty,tz : Translation respectively along x, y and z axis.
-
-*/
-void
-vpTranslationVector::set(const double tx,
- const double ty,
- const double tz)
-{
- (*this)[0] = tx ;
- (*this)[1] = ty ;
- (*this)[2] = tz ;
-}
-
-/*!
- Operator that allows to add two translation vectors.
-
- \param tv : Translation vector to add.
-
- \return The sum of the current translation vector (*this) and the one to add.
- \code
- vpTranslationVector t1(1,2,3);
- vpTranslationVector t2(4,5,6);
- vpTranslationVector t3;
-
- t3 = t2 + t1;
- // t1 and t2 leave unchanged
- // t3 is now equal to : 5, 7, 9
- \endcode
-
-*/
-vpTranslationVector
-vpTranslationVector::operator+(const vpTranslationVector &tv) const
-{
- vpTranslationVector s;
-
- for (unsigned int i=0;i<3;i++) s[i] = (*this)[i]+tv[i] ;
-
- return s;
-}
-
-/*!
- Operator that allows to substract two translation vectors.
-
- \param tv : Translation vector to substract.
-
- \return The substraction of the current translation vector (*this) and the one to substract.
- \code
- vpTranslationVector t1(1,2,3);
- vpTranslationVector t2(4,5,6);
- vpTranslationVector t3;
-
- t3 = t2 - t1;
- // t1 and t2 leave unchanged
- // t3 is now equal to : 3, 3, 3
- \endcode
-
-*/
-vpTranslationVector
-vpTranslationVector::operator-(const vpTranslationVector &tv) const
-{
- vpTranslationVector sub ;
-
- for (unsigned int i=0;i<3;i++) sub[i] = (*this)[i]-tv[i] ;
-
- return sub;
-}
-
-
-/*!
- Operator that allows to negate a translation vector.
-
- \return The negate translation. The current translation vector
- (*this) is unchanged.
-
- \code
- vpTranslationVector t1(1,2,3);
- vpTranslationVector t2;
- t2 = -t1;
- // t1 is unchanged
- // t2 is now equal to : -1, -2, -3
- \endcode
-*/
-vpTranslationVector vpTranslationVector::operator-() const //negate
-{
- vpTranslationVector tv ;
- for (unsigned int i=0;i<dsize;i++)
- {
- *(tv.data + i) = -*(data + i) ;
- }
-
- return tv;
-}
-
-/*!
- Operator that allows to multiply a translation vector by a scalar.
-
- \param x : The scalar.
-
- \return The translation vector multiplied by the scalar. The current
- translation vector (*this) is unchanged.
-
- \code
- vpTranslationVector t1(1,2,3);
- t2 = t1 * 3;
- // t1 is unchanged
- // t2 is now equal to : 3, 6, 9
- \endcode
-*/
-vpTranslationVector vpTranslationVector::operator*(const double x) const
-{
- vpTranslationVector tv ;
- for (unsigned int i=0;i<dsize;i++)
- {
- *(tv.data + i) = (*(data + i)) * x ;
- }
-
- return tv;
-}
-
-/*!
- Copy operator.
- \param tv : Translation vector to copy
- \return A copy of t.
-
- \code
- vpTranslationVector t1(1,2,3);
- vpTranslationVector t2;
- t2 = t1;
- // t1 is unchanged
- // t2 is now equal to t1 : 1, 2, 3
- \endcode
-*/
-vpTranslationVector &vpTranslationVector::operator=(const vpTranslationVector &tv)
-{
-
- unsigned int k = tv.rowNum ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
-
- memcpy(data, tv.data, rowNum*sizeof(double)) ;
-
- return *this;
-}
-
-/*!
- Initialize each element of a translation vector to the same value x.
-
- \param x : Value to set for each element of the translation vector.
-
- \code
- vpTranslationVector t;
- t = 3;
- // Here t is set to 3,3,3
- \endcode
-*/
-vpTranslationVector & vpTranslationVector::operator=(double x)
-{
-
- double *d = data ;
-
- for (int i=0;i<3;i++)
- *(d++)= x ;
-
- return *this;
-}
-
-/*!
- \relates vpMatrix
-
- Compute the skew symmetric matrix \f$M\f$ of translation vector \f$t\f$
- (matrice de pre-produit vectoriel).
-
- \f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
- \end{array}\right), \quad \mbox{then} \qquad
- M = \left( \begin{array}{ccc}
- 0 & -t_z & t_y \\
- t_z & 0 & -t_x \\
- -t_y & t_x & 0
- \end{array}\right)
- \f]
-
- \param t : Translation vector in input used to compute the skew symmetric
- matrix M.
-
- \param M : Skew symmetric matrix of translation vector \f$t\f$.
-*/
-void
-vpTranslationVector::skew(const vpTranslationVector &t,vpMatrix &M)
-{
- M.resize(3,3) ;
- M[0][0] = 0 ; M[0][1] = -t[2] ; M[0][2] = t[1] ;
- M[1][0] = t[2] ; M[1][1] = 0 ; M[1][2] = -t[0] ;
- M[2][0] = -t[1] ; M[2][1] = t[0] ; M[2][2] = 0 ;
-}
-
-/*!
-
- Compute the skew symmetric matrix \f$M\f$ of translation vector
- \f$t\f$ (matrice de pre-produit vectoriel).
-
- \f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
- \end{array}\right), \quad \mbox{then} \qquad
- M = \left( \begin{array}{ccc}
- 0 & -t_z & t_y \\
- t_z & 0 & -t_x \\
- -t_y & t_x & 0
- \end{array}\right)
- \f]
-
- \param t : Translation vector in input.
-
- \return Skew symmetric matrix \f$M\f$ of translation vector
- \f$t\f$
-
-*/
-vpMatrix
-vpTranslationVector::skew(const vpTranslationVector &t)
-{
- vpMatrix M(3, 3);
- skew(t,M);
- return M;
-}
-
-/*!
-
- Compute the skew symmetric matrix \f$M\f$ of the translation vector (matrice
- de pre-produit vectoriel), where
-
- \f[ M = \left( \begin{array}{ccc}
- 0 & -t_z & t_y \\
- t_z & 0 & -t_x \\
- -t_y & t_x & 0
- \end{array}\right)
- \f]
-
- and where \f$(t_x,t_y,t_z)\f$ are the coordinates of the translation
- vector.
-
- \return Skew symmetric matrix \f$M\f$ of the translation vector.
-
-*/
-vpMatrix
-vpTranslationVector::skew() const
-{
- vpMatrix M(3, 3);
- skew(*this,M);
- return M;
-}
-
-
-/*!
-
- Return the cross product of two translation vectors \f$a \times b\f$.
-
- \param a,b : Translation vectors in input.
-
- \return The cross product of two translation vectors \f$a \times
- b\f$.
-*/
-vpTranslationVector
-vpTranslationVector::cross(const vpTranslationVector &a,
- const vpTranslationVector &b)
-{
- vpMatrix skew_a = vpTranslationVector::skew(a) ;
- return (vpTranslationVector)(skew_a * b);
-}
-
-/*
- * Local variables:
- * c-basic-offset: 4
- * End:
- */
diff --git a/src/math/transformation/vpTranslationVector.h b/src/math/transformation/vpTranslationVector.h
deleted file mode 100644
index 16c5d06..0000000
--- a/src/math/transformation/vpTranslationVector.h
+++ /dev/null
@@ -1,144 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTranslationVector.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Translation vector.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-#ifndef vpTRANSLATIONVECTOR_H
-#define vpTRANSLATIONVECTOR_H
-
-/*!
- \file vpTranslationVector.h
- \brief Class that consider the case of a translation vector.
-*/
-
-#include <visp/vpColVector.h>
-
-
-/*!
- \class vpTranslationVector
-
- \ingroup TransTransformation
-
- \brief Class that consider the case of a translation vector.
-
- Let be \f$^{a}{\bf t}_{b} = [t_x,t_y,t_z]^\top\f$ be a translation
- from frame \f$ a \f$ to frame \f$ b \f$. The representation of a
- translation is a column vector of dimension 3.
-
- Translations are expressed in meters.
-
- The code below shows how to use a translation vector to build an
- homogeneous matrix.
-
- \code
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpTranslationVector.h>
-
-int main()
-{
- vpTranslationVector t; // Translation vector
-
- // Initialization of the translation vector
- t[0] = 0.2; // tx = 0.2 meters
- t[1] = -0.1; // ty = -0.1 meters
- t[2] = 1.0; // tz = 1 meters
-
- // Construction of a rotation matrix
- vpRotationMatrix R; // Set to identity by default
-
- // Construction of an homogeneous matrix
- vpHomogeneousMatrix M(t, R);
-}
- \endcode
-
-*/
-class VISP_EXPORT vpTranslationVector : public vpColVector
-{
-private:
- //! initialize a size 3 vector
- void init() ;
-
-public:
-
- /*!
- Default constructor.
- The translation vector is initialized to zero.
- */
- vpTranslationVector() { init() ; }
- // constructor from double in meter
- vpTranslationVector(const double tx, const double ty, const double tz) ;
- // copy constructor
- vpTranslationVector(const vpTranslationVector &t);
- void set(const double tx, const double ty, const double tz) ;
-
- // operators
-
- // translation vectors additions c = a + b (a, b unchanged)
- vpTranslationVector operator+(const vpTranslationVector &t) const ;
- // translation vectors substraction c = a - b (a, b unchanged)
- vpTranslationVector operator-(const vpTranslationVector &t) const ;
- // negate t = -a (t is unchanged)
- vpTranslationVector operator-() const ;
- // b = x * a (x=scalar)
- vpTranslationVector operator*(const double x) const;
- // Copy operator. Allow operation such as A = v
- vpTranslationVector &operator=(const vpTranslationVector &t);
-
- vpTranslationVector &operator=(double x) ;
-
-
- // Skew Symmetric matrix
- vpMatrix skew() const ;
- static vpMatrix skew(const vpTranslationVector &t) ;
- static void skew(const vpTranslationVector &t, vpMatrix &M) ;
- static vpTranslationVector cross(const vpTranslationVector &a,
- const vpTranslationVector &b) ;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 4
- * End:
- */
diff --git a/src/math/transformation/vpVelocityTwistMatrix.cpp b/src/math/transformation/vpVelocityTwistMatrix.cpp
deleted file mode 100644
index add5bec..0000000
--- a/src/math/transformation/vpVelocityTwistMatrix.cpp
+++ /dev/null
@@ -1,460 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpVelocityTwistMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Velocity twist transformation matrix.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpVelocityTwistMatrix.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpVelocityTwistMatrix.cpp
-
- \brief Definition of the vpVelocityTwistMatrix. Class that consider the
- particular case of twist transformation matrix that allows to
- transform a velocity skew from one frame to an other.
-*/
-
-
-/*!
- Copy operator.
-
- \param V : Velocity twist matrix to copy.
-*/
-vpVelocityTwistMatrix &
-vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
-{
- init() ;
-
- for (int i=0; i<6; i++)
- {
- for (int j=0; j<6; j++)
- {
- rowPtrs[i][j] = V.rowPtrs[i][j];
- }
- }
-
- return *this;
-}
-
-
-/*!
- Initialize a 6x6 velocity twist matrix as identity.
-*/
-
-void
-vpVelocityTwistMatrix::init()
-{
- unsigned int i,j ;
-
- try {
- resize(6,6) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- for (i=0 ; i < 6 ; i++)
- for (j=0 ; j < 6; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
- else
- (*this)[i][j] = 0.0;
-}
-
-/*!
- Initialize a velocity twist transformation matrix as identity.
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix() : vpMatrix()
-{
- init() ;
-}
-
-/*!
- Initialize a velocity twist transformation matrix from another velocity twist matrix.
-
- \param V : Velocity twist matrix used as initializer.
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) : vpMatrix()
-{
- init() ;
- *this = V;
-}
-
-/*!
-
- Initialize a velocity twist transformation matrix from an homogeneous matrix
- \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
- \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
-
- \param M : Homogeneous matrix \f$M\f$ used to initialize the velocity twist
- transformation matrix.
-
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M) : vpMatrix()
-{
- init() ;
- buildFrom(M);
-}
-
-/*!
-
- Initialize a velocity twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tv : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
-
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &tv,
- const vpThetaUVector &thetau) : vpMatrix()
-{
- init() ;
- buildFrom(tv, thetau) ;
-}
-
-/*!
-
- Initialize a velocity twist transformation matrix from a translation vector
- \e t and a rotation matrix M.
-
- \param tv : Translation vector.
-
- \param R : Rotation matrix.
-
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &tv,
- const vpRotationMatrix &R)
-{
- init() ;
- buildFrom(tv,R) ;
-}
-
-/*!
-
- Initialize a velocity twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tx,ty,tz : Translation vector in meters.
-
- \param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
-*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz) : vpMatrix()
-{
- init() ;
- vpTranslationVector T(tx,ty,tz) ;
- vpThetaUVector tu(tux,tuy,tuz) ;
- buildFrom(T,tu) ;
-}
-
-/*!
-
- Set the velocity twist transformation matrix to identity.
-
-*/
-void
-vpVelocityTwistMatrix::setIdentity()
-{
- init() ;
-}
-
-
-/*!
-
- Operator that allows to multiply a velocity twist transformation matrix by an
- other velocity twist transformation matrix.
-
-*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const
-{
- vpVelocityTwistMatrix p ;
-
- for (unsigned int i=0;i<6;i++)
- for (unsigned int j=0;j<6;j++)
- {
- double s =0 ;
- for (int k=0;k<6;k++)
- s +=rowPtrs[i][k] * V.rowPtrs[k][j];
- p[i][j] = s ;
- }
- return p;
-}
-
-/*!
- Operator that allows to multiply a velocity twist transformation matrix by a matrix.
-
- As shown in the example below, this operator can be used to compute the corresponding
- camera velocity skew from the joint velocities knowing the robot jacobian.
-
- \code
- #include <visp/vpConfig.h>
- #include <visp/vpRobotAfma6.h>
- #include <visp/vpColVector.h>
- #include <visp/vpVelocityTwistMatrix.h>
-
- #ifdef VISP_HAVE_AFMA6
-
- int main()
- {
- vpRobotAfma6 robot;
-
- vpColVector q_vel(6); // Joint velocity on the 6 joints
- // ... q_vel need here to be initialized
-
- vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
-
- vpVelocityTwistMatrix cVe; // Velocity skew transformation from camera frame to end-effector
- robot.get_cVe(cVe);
-
- vpMatrix eJe; // Robot jacobian
- robot.get_eJe(eJe);
-
- // Compute the velocity in the camera frame
- c_v = cVe * eJe * q_vel;
-
- return 0;
- }
- #endif
- \endcode
-
- \exception vpMatrixException::incorrectMatrixSizeError If M is not a 6 rows
- dimension matrix.
-*/
-vpMatrix
-vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
-{
-
- if (6 != M.getRows())
- {
- vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vpMatrix multiply") ;
- throw(vpMatrixException::incorrectMatrixSizeError) ;
- }
-
- vpMatrix p(6, M.getCols()) ;
- for (unsigned int i=0;i<6;i++)
- for (unsigned int j=0;j<M.getCols();j++)
- {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
- s += rowPtrs[i][k] * M[k][j];
- p[i][j] = s ;
- }
- return p;
-}
-
-/*!
-
- Operator that allows to multiply a twist transformation matrix by a
- column vector.
-
- Operation c = V * v (V, the velocity skew transformation matrix is unchanged,
- c and v are 6 dimension vectors).
-
- \param v : Velocity skew vector.
-
- \exception vpMatrixException::incorrectMatrixSizeError If v is not a 6
- dimension vector.
-
-*/
-vpColVector
-vpVelocityTwistMatrix::operator*(const vpColVector &v) const
-{
- vpColVector c(6);
-
- if (6 != v.getRows())
- {
- vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "Mismatch in vpVelocityTwistMatrix/vector multiply")) ;
- }
-
- c = 0.0;
-
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
- {
- c[i]+=rowPtrs[i][j] * v[j];
- }
- }
- }
-
- return c ;
-}
-
-
-/*!
-
- Build a velocity twist transformation matrix from a translation vector
- \e t and a rotation matrix M.
-
- \param tv : Translation vector.
-
- \param R : Rotation matrix.
-
-*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &tv,
- const vpRotationMatrix &R)
-{
- unsigned int i, j;
- vpMatrix skewaR = tv.skew(tv)*R ;
-
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3 ; j++)
- {
- (*this)[i][j] = R[i][j] ;
- (*this)[i+3][j+3] = R[i][j] ;
- (*this)[i][j+3] = skewaR[i][j] ;
-
- }
- return (*this) ;
-}
-
-/*!
-
- Initialize a velocity twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
-
- \param tv : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
-
-*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &tv,
- const vpThetaUVector &thetau)
-{
- vpRotationMatrix R ;
- R.buildFrom(thetau) ;
- buildFrom(tv,R) ;
- return (*this) ;
-
-}
-
-
-/*!
-
- Initialize a velocity twist transformation matrix from an homogeneous matrix
- \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
- \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
-
- \param M : Homogeneous matrix \f$M\f$ used to initialize the twist
- transformation matrix.
-
-*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
-{
- vpTranslationVector tv ;
- vpRotationMatrix R ;
- M.extract(R) ;
- M.extract(tv) ;
-
- buildFrom(tv, R) ;
- return (*this) ;
-}
-
-
-//! invert the twist matrix
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::inverse() const
-{
- vpVelocityTwistMatrix Wi;
- vpRotationMatrix R;extract(R);
- vpTranslationVector T;extract(T);
- vpTranslationVector RtT ; RtT = -(R.t()*T) ;
-
- Wi.buildFrom(RtT,R.t());
-
- return Wi ;
-}
-
-
-//! invert the twist matrix
-void
-vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &Wi) const
-{
- Wi = inverse();
-}
-
-//! extract the rotational matrix from the twist matrix
-void
-vpVelocityTwistMatrix::extract( vpRotationMatrix &R) const
-{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- R[i][j] = (*this)[i][j];
-}
-
-//! extract the translation vector from the twist matrix
-void
-vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
-{
- vpRotationMatrix R;extract(R);
- vpMatrix skTR(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- skTR[i][j] = (*this)[i][j+3];
-
- vpMatrix skT = skTR*R.t();
- tv[0] = skT[2][1];
- tv[1] = skT[0][2];
- tv[2] = skT[1][0];
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpVelocityTwistMatrix.h b/src/math/transformation/vpVelocityTwistMatrix.h
deleted file mode 100644
index 1ca88bc..0000000
--- a/src/math/transformation/vpVelocityTwistMatrix.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpVelocityTwistMatrix.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Velocity twist transformation matrix.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpVelocityRwistMatrix_h
-#define vpVelocityRwistMatrix_h
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRotationMatrix.h>
-
-
-/*!
- \class vpVelocityTwistMatrix
-
- \ingroup TwistTransformation
-
- \brief Class that consider the particular case of twist
- transformation matrix that allows to transform a velocity skew from
- one frame to an other.
-
- The vpVelocityTwistMatrix is derived from vpMatrix.
-
- A twist transformation matrix is 6x6 matrix defined as
- \f[
- ^a{\bf V}_b = \left[\begin{array}{cc}
- ^a{\bf R}_b & [^a{\bf t}_b]_\times \; ^a{\bf R}_b\\
- {\bf 0}_{3\times 3} & ^a{\bf R}_b
- \end{array}
- \right]
- \f]
- that expressed a velocity in frame <em>a</em> knowing velocity in <em>b</em>.
-
- \f$ ^a{\bf R}_b \f$ is a rotation matrix and
- \f$ ^a{\bf t}_b \f$ is a translation vector.
-
- The code belows shows for example how to convert a velocity skew
- from camera frame to a fix frame.
-
- \code
-#include <visp/vpColVector.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-int main()
-{
- vpVelocityTwistMatrix fVc; // Twist transformation matrix from fix to camera frame
-
- vpHomogeneousMatrix fMc; // Fix to camera frame transformation
- // ... fMc need here to be initialized
-
- fVc.buildFrom(fMc);
-
- vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
- // ... c_v should here have an initial value
-
- vpColVector f_v(6); // Velocity in the fix frame: vx,vy,vz,wx,wy,wz
-
- // Compute the velocity in the fix frame
- f_v = fVc * c_v;
-}
- \endcode
-*/
-class VISP_EXPORT vpVelocityTwistMatrix : public vpMatrix
-{
- friend class vpMatrix;
-
- public:
- // basic constructor
- vpVelocityTwistMatrix() ;
- // copy constructor
- vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) ;
- // constructor from an homogeneous transformation
- vpVelocityTwistMatrix(const vpHomogeneousMatrix &M) ;
-
- // Construction from Translation and rotation (ThetaU parameterization)
- vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
- // Construction from Translation and rotation (matrix parameterization)
- vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- vpVelocityTwistMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
-
- // Basic initialisation (identity)
- void init() ;
-
- vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R);
- vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpThetaUVector &thetau);
- vpVelocityTwistMatrix buildFrom(const vpHomogeneousMatrix &M) ;
-
- // Basic initialisation (identity)
- void setIdentity() ;
-
- vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const ;
- vpMatrix operator*(const vpMatrix &M) const ;
-
- vpColVector operator*(const vpColVector &v) const ;
-
- // copy operator from vpMatrix (handle with care)
- vpVelocityTwistMatrix &operator=(const vpVelocityTwistMatrix &V);
-
- //! invert the twist matrix
- vpVelocityTwistMatrix inverse() const ;
- //! invert the twist matrix
- void inverse(vpVelocityTwistMatrix &Wi) const;
-
- //! extract the rotational matrix from the twist matrix
- void extract( vpRotationMatrix &R) const;
- //! extract the translation vector from the twist matrix
- void extract(vpTranslationVector &t) const;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
deleted file mode 100644
index 2c9befe..0000000
--- a/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
+++ /dev/null
@@ -1,608 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParserHomogeneousMatrix.cpp 4920 2014-10-09 08:18:30Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load and save Homogeneous Matrix in a XML file
- *
- * Authors:
- * Giovanni Claudio
- *
- *****************************************************************************/
-
-
-/*!
- \file vpXmlParserHomogeneousMatrix.cpp
- \brief Definition of the vpXmlParserHomogeneousMatrix class member functions.
- Class vpXmlParserHomogeneousMatrix allowed to load and save an homogeneous matrix in a XML file.
-
-*/
-#include <visp/vpXmlParserHomogeneousMatrix.h>
-#ifdef VISP_HAVE_XML2
-
-#include <stdlib.h>
-#include <string.h>
-
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
-/* -------------------------------------------------------------------------- */
-/* --- LABEL XML ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-#define LABEL_XML_ROOT "root"
-#define LABEL_XML_M "homogeneous_transformation"
-#define LABEL_XML_M_NAME "name"
-#define LABEL_XML_VALUE "values"
-#define LABEL_XML_TRANSLATION "translation"
-#define LABEL_XML_TX "tx"
-#define LABEL_XML_TY "ty"
-#define LABEL_XML_TZ "tz"
-#define LABEL_XML_ROTATION "rotation"
-#define LABEL_XML_TUX "theta_ux"
-#define LABEL_XML_TUY "theta_uy"
-#define LABEL_XML_TUZ "theta_uz"
-
-/*!
- Default constructor
-*/
-vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix()
- : vpXmlParser(), m_M(), m_name()
-{
-}
-/*!
- Copy constructor
- \param twinParser : parser object to copy
-*/
-vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix& twinParser)
- : vpXmlParser(twinParser), m_M(), m_name()
-{
- *this = twinParser;
-}
-
-/*!
- Copy operator
- \param twinParser : parser object to copy
- \return a copy of the input.
-*/
-vpXmlParserHomogeneousMatrix&
-vpXmlParserHomogeneousMatrix::operator =(const vpXmlParserHomogeneousMatrix& twinParser)
-{
- this->m_M = twinParser.m_M;
- this->m_name = twinParser.m_name;
-
- return *this ;
-}
-
-/*!
- Parse an xml file to load an homogeneous matrix
- \param M : homogeneous matrix to fill.
- \param filename : name of the xml file to parse.
- \param name : name of the homogeneous matrix to find in the xml file.
-
- \return error code.
-*/
-int
-vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename,
- const std::string &name)
-{
- xmlDocPtr doc;
- xmlNodePtr node;
-
- doc = xmlParseFile(filename.c_str());
- if (doc == NULL)
- {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " I cannot open the file "<< filename << std::endl;
-
- return SEQUENCE_ERROR;
- }
-
- node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- int ret = this ->read (doc, node, name);
-
- M = m_M ;
-
- xmlFreeDoc(doc);
-
- return ret;
-}
-
-/*!
- Save an homogenous matrix in an xml file.
- \param M : homogenous matrix to save.
- \param filename : name of the xml file to fill.
- \param name : name of the homogenous matrix.
-
- \return error code.
-*/
-int
-vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
- const std::string &name)
-{
- xmlDocPtr doc;
- xmlNodePtr node;
-
- doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR
- + XML_PARSE_NOBLANKS);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
- xmlDocSetRootElement(doc,node);
- xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
- "This file stores homogeneous matrix used\n"
- " in the vpHomogeneousMatrix Class of ViSP available\n"
- " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
- " It can be read with the parse method of\n"
- " the vpXmlParserHomogeneousMatrix class.");
- xmlAddChild(node,node_tmp);
- }
-
- node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- this->m_M = M;
-
- int M_isFound = count(doc, node, name);
-
- if( M_isFound > 0){
- //vpCERROR
- std::cout << "There is already an homogeneous matrix "<< std::endl
- << "available in the file with the input name: "<< name << "."<< std::endl
- << "Please delete it manually from the xml file."<< std::endl;
- xmlFreeDoc(doc);
- return SEQUENCE_ERROR;
- }
-
- write(node, name);
-
- xmlSaveFormatFile(filename.c_str(), doc, 1);
- xmlFreeDoc(doc);
-// std::cout << "Homogeneous matrix '"<< name << "' saved in the file named "<< filename << " correctly." << std::endl;
-
- return SEQUENCE_OK;
-}
-
-
-
-/*!
- Read Homogeneous matrix values from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param name : name of the Homogeneous Matrix
- \return error code.
- */
-int
-vpXmlParserHomogeneousMatrix::read (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name)
-{
- // char * val_char;
- vpXmlCodeType prop;
-
- vpXmlCodeSequenceType back = SEQUENCE_OK;
- unsigned int nbM = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
- if (prop == CODE_XML_M){
- if (SEQUENCE_OK == this->read_matrix (doc, node, name))
- nbM++;
- }
- else back = SEQUENCE_ERROR;
- }
-
- if (nbM == 0){
- back = SEQUENCE_ERROR;
- vpCERROR << "No Homogeneous matrix is available" << std::endl
- << "with name: " << name << std::endl;
- }
- else if(nbM > 1){
- back = SEQUENCE_ERROR;
- vpCERROR << nbM << " There are more Homogeneous matrix" << std::endl
- << "with the same name : " << std::endl
- << "precise your choice..." << std::endl;
- }
-
- return back;
-}
-/*!
- Read homogeneous matrix names from a XML file and read if there is already a homogeneous matrix
- with the same name.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param name : name of the homogeneous matrix.
-
- \return 1 if there is an homogeneous matrix corresponding with the input name, 0 otherwise.
- */
-int
-vpXmlParserHomogeneousMatrix::count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name)
-{
- // char * val_char;
- vpXmlCodeType prop;
- int nbM = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- }
- if (prop== CODE_XML_M) {
- if (SEQUENCE_OK == this->read_matrix (doc, node, name))
- nbM++;
- }
- }
-
- return nbM;
-}
-
-/*!
- Read Homogeneous Matrix fields from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param name : name of the Homogeneous matrix
-
- \return error code.
-
- */
-int
-vpXmlParserHomogeneousMatrix::read_matrix (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name)
-{
- vpXmlCodeType prop;
- /* read value in the XML file. */
- std::string M_name_tmp = "";
- vpHomogeneousMatrix M_tmp;
-
- vpXmlCodeSequenceType back = SEQUENCE_OK;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
-
- switch (prop)
- {
- case CODE_XML_M_NAME: {
- char * val_char = xmlReadCharChild(doc, node);
- M_name_tmp = val_char;
- xmlFree(val_char);
- break;
- }
-
- case CODE_XML_VALUE: //VALUE
- if (name == M_name_tmp)
- {
- std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
- back = read_values(doc, node, M_tmp);
- }
- break;
-
- case CODE_XML_BAD:
- case CODE_XML_OTHER:
- case CODE_XML_M:
- case CODE_XML_TX:
- case CODE_XML_TY:
- case CODE_XML_TZ:
- case CODE_XML_TUX:
- case CODE_XML_TUY:
- case CODE_XML_TUZ:
-
- default:
- back = SEQUENCE_ERROR;
- break;
- }
-
- }
-
- if( !(name == M_name_tmp)){
- back = SEQUENCE_ERROR;
- }
- else{
- this-> m_M = M_tmp;
- //std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
- //std::cout << this-> M << std::endl;
- this-> m_name = M_name_tmp;
-
- }
- return back;
-}
-
-
-/*!
- Read homogeneous matrix fields from a XML file.
-
- \param doc : XML file.
- \param node : XML tree, pointing on a marker equipement.
- \param M_tmp : homogeneous matrix to fill with read data (output).
-
- \return error code.
-
- */
-vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType
-vpXmlParserHomogeneousMatrix::read_values (xmlDocPtr doc, xmlNodePtr node,
- vpHomogeneousMatrix &M)
-{
- // counter of the number of read parameters
- int nb = 0;
- vpXmlCodeType prop;
- /* read value in the XML file. */
-
- double tx_=0.;
- double ty_=0.;
- double tz_=0.;
- double tux_=0.;
- double tuy_=0.;
- double tuz_=0.;
-
- vpXmlCodeSequenceType back = SEQUENCE_OK;
- //int validation = 0;
-
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- // vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
- prop = CODE_XML_OTHER;
- back = SEQUENCE_ERROR;
- }
-
- switch (prop)
- {
-
- case CODE_XML_TX:
- tx_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
- case CODE_XML_TY:
- ty_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
- case CODE_XML_TZ:
- tz_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
- case CODE_XML_TUX:
- tux_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
- case CODE_XML_TUY:
- tuy_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
- case CODE_XML_TUZ:
- tuz_ = xmlReadDoubleChild(doc, node);
- nb++;
- break;
-
- case CODE_XML_BAD:
- case CODE_XML_OTHER:
- case CODE_XML_M:
- case CODE_XML_M_NAME:
- case CODE_XML_VALUE:
-
- default:
- back = SEQUENCE_ERROR;
- break;
- }
- }
-
- if (nb != 6)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
- vpCERROR << "it must contain 6 parameters\n";
-
- return SEQUENCE_ERROR;
- }
-
- // Create the Homogeneous matrix
- M.buildFrom(tx_,ty_,tz_,tux_,tuy_,tuz_);
-
- // std::cout << "Read values from file:" << std::endl;
- // std::cout << "tx:" << tx_<< std::endl;
- // std::cout << "ty:" << ty_<< std::endl;
- // std::cout << "tz:" << tz_<< std::endl;
- // std::cout << "tux:" << tux_<< std::endl;
- // std::cout << "tuy:" << tuy_<< std::endl;
- // std::cout << "tuz:" << tuz_<< std::endl;
-
- return back;
-}
-
-/*!
- Write Homogeneous Matrix in an XML Tree.
-
- \param node : XML tree, pointing on a marker equipement.
- \param name : name of the Homogeneous Matrix.
-
-
- \return error code.
- */
-int vpXmlParserHomogeneousMatrix::
-write (xmlNodePtr node, const std::string& name)
-{
- int back = SEQUENCE_OK;
-
- xmlNodePtr node_tmp;
- xmlNodePtr node_matrix;
- xmlNodePtr node_values;
- char str[11];
-
- // Convert from Rotational matrix to Theta-U vector
- vpRotationMatrix R;
- m_M.extract(R);
-
- vpThetaUVector tu(R);
-
- // <homogenous_transformation>
- node_tmp = xmlNewComment((xmlChar*)"Homogeneous Matrix");
- xmlAddChild(node,node_tmp);
- node_matrix = xmlNewNode(NULL,(xmlChar*)LABEL_XML_M);
- xmlAddChild(node,node_matrix);
- {
- //<name>
-
- if(!name.empty()){
- node_tmp = xmlNewComment((xmlChar*)"Name of the homogeneous matrix");
- xmlAddChild(node_matrix,node_tmp);
- xmlNewTextChild(node_matrix,NULL,(xmlChar*)LABEL_XML_M_NAME, (xmlChar*)name.c_str());
- }
-
- //<values>
-
- node_values = xmlNewNode(NULL,(xmlChar*)LABEL_XML_VALUE);
- xmlAddChild(node_matrix,node_values);
- {
- node_tmp = xmlNewComment((xmlChar*)"Translation vector with values in meters");
- xmlAddChild(node_values,node_tmp);
-
- //<tx>
- sprintf(str,"%f", m_M[0][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TX,(xmlChar*)str);
-
- //<ty>
- sprintf(str,"%f", m_M[1][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TY,(xmlChar*)str);
-
- //<tz>
- sprintf(str,"%f", m_M[2][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TZ,(xmlChar*)str);
-
- node_tmp = xmlNewComment((xmlChar*)"Rotational vector expressed in angle axis representation with values in radians");
- xmlAddChild(node_values,node_tmp);
-
- //<tux>
- sprintf(str,"%f", tu[0]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUX,(xmlChar*)str);
-
- //<tuy>
- sprintf(str,"%f", tu[1]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUY,(xmlChar*)str);
-
- //<tuz>
- sprintf(str,"%f", tu[2]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUZ,(xmlChar*)str);
- }
- }
- return back;
-}
-
-/*!
- Translate a string (label) to a xml code.
- \param str : string to translate.
- \param res : resulting code.
-
- \return error code.
-*/
-
-vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType
-vpXmlParserHomogeneousMatrix::str2xmlcode (char * str, vpXmlCodeType & res)
-{
- vpXmlCodeType val_int = CODE_XML_BAD;
- vpXmlCodeSequenceType back = vpXmlParserHomogeneousMatrix::SEQUENCE_OK;
-
- // DEBUG_TRACE (9, "# Entree :str=%s.", str);
-
- if (! strcmp (str, LABEL_XML_M))
- {
- val_int = CODE_XML_M;
- }
- else if (! strcmp (str, LABEL_XML_M_NAME))
- {
- val_int = CODE_XML_M_NAME;
- }
- else if (! strcmp (str, LABEL_XML_VALUE))
- {
- val_int = CODE_XML_VALUE;
- }
- else if (! strcmp (str, LABEL_XML_TX))
- {
- val_int = CODE_XML_TX;
- }
- else if (! strcmp (str, LABEL_XML_TY))
- {
- val_int = CODE_XML_TY;
- }
- else if (! strcmp (str, LABEL_XML_TZ))
- {
- val_int = CODE_XML_TZ;
- }
- else if (! strcmp (str, LABEL_XML_TUX))
- {
- val_int = CODE_XML_TUX;
- }
- else if (! strcmp (str, LABEL_XML_TUY))
- {
- val_int = CODE_XML_TUY;
- }
- else if (! strcmp (str, LABEL_XML_TUZ))
- {
- val_int = CODE_XML_TUZ;
- }
- else
- {
- val_int = CODE_XML_OTHER;
- }
- res = val_int;
-
- return back;
-}
-#endif //VISP_HAVE_XML2
diff --git a/src/math/transformation/vpXmlParserHomogeneousMatrix.h b/src/math/transformation/vpXmlParserHomogeneousMatrix.h
deleted file mode 100644
index 27d2881..0000000
--- a/src/math/transformation/vpXmlParserHomogeneousMatrix.h
+++ /dev/null
@@ -1,262 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParserHomogeneousMatrix.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * XML parser to load and save Homogeneous Matrix in a XML file
- *
- * Authors:
- * Giovanni Claudio
- *
- *****************************************************************************/
-
-/*!
- \file vpXmlParserHomogeneousMatrix.h
- \brief Declaration of the vpXmlParserHomogeneousMatrix class.
- Class vpXmlParserHomogeneousMatrix allowed to load and save Homogeneous Matrixes in a file XML
-
-*/
-
-#ifndef vpXMLPARSERHOMOGENEOUSMATRIX_H
-#define vpXMLPARSERHOMOGENEOUSMATRIX_H
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <string>
-#include <visp/vpXmlParser.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <libxml/xmlmemory.h> /* Functions of libxml. */
-
-/*!
- \class vpXmlParserHomogeneousMatrix
-
- \ingroup PoseTransformation
-
- \brief XML parser to load and save an homogenous matrix in a file.
-
- To have a complete description of the homogeneous matrix implemented in ViSP, see
- vpHomogeneousMatrix.
-
- Example of an XML file "homogenous_matrixes.xml" containing a Pose vector
- that will be converted in an homogeneous matrix:
-
- \code
-<?xml version="1.0"?>
-<root>
- <homogeneous_transformation>
- <!--Name of the homogeneous matrix-->
- <name>eMc</name>
- <values>
- <!--Translation vector with values in meters -->
- <tx>1.00</tx>
- <ty>1.30</ty>
- <tz>3.50</tz>
- <!--Rotational vector expressed in angle axis representation with values in radians -->
- <theta_ux>0.20</theta_ux>
- <theta_uy>0.30</theta_uy>
- <theta_uz>0.50</theta_uz>
- </values>
- </homogeneous_transformation>
-</root>
- \endcode
-
- Example of loading an existing homogeneous matrix from an XML file.
- \code
-
-#include <iostream>
-#include <string>
-
-#include <visp/vpXmlParserHomogeneousMatrix.h>
-
-int main(int argc, char* argv[])
-{
-#ifdef VISP_HAVE_XML2
- vpHomogeneousMatrix eMc;
-
- // Create a XML parser
- vpXmlParserHomogeneousMatrix p;
-
- // Define the name of the matrix to load
- std::string name = "eMc";
-
- if (p.parse(eMc,"homogenous_matrixes.xml", name) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
- std::cout << "Cannot found the Homogeneous matrix named " << name << "." << std::endl;
- }
- else
- std::cout << "Homogeneous matrix " << name <<": " << std::endl << eMc << std::endl;
-#endif
-
- return 0;
-}
- \endcode
-
- Example of writing an homogenoeus matrix in a XML file.
- \note Before writing an homogeneous matrix check if there
- is already in the xml file a matrix with the same name.
- If you are sure to overwrite it please delete it manually
- from the file before.
-
- \code
-#include <iostream>
-#include <string>
-
-#include <visp/vpXmlParserHomogeneousMatrix.h>
-
-int main(int argc, char* argv[])
-{
-#ifdef VISP_HAVE_XML2
- // Create Pose Vector and convert to homogeneous matrix
- vpPoseVector r(1.0,1.3,3.5,0.2,0.3,0.5);
- vpHomogeneousMatrix M(r);
-
- // Create a XML parser
- vpXmlParserHomogeneousMatrix p;
-
- // Define the name of the matrix
- std::string name_M = "eMe";
-
- // Define name of the file xml to fill
- char filename[FILENAME_MAX];
- sprintf(filename, "%s", "homogenous_matrixes.xml");
-
- if (p.save(M, filename, name_M) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
- std::cout << "Cannot save the Homogeneous matrix" << std::endl;
- }
-
- vpXmlParser::cleanup();
-#endif
- return 0;
-}
- \endcode
-*/
-
-class VISP_EXPORT vpXmlParserHomogeneousMatrix: public vpXmlParser
-{
-
-public:
-
- /* --- XML Code------------------------------------------------------------ */
- typedef enum
- {
- CODE_XML_BAD = -1,
- CODE_XML_OTHER,
- CODE_XML_M,
- CODE_XML_M_NAME,
- CODE_XML_VALUE,
- CODE_XML_TX,
- CODE_XML_TY,
- CODE_XML_TZ,
- CODE_XML_TUX,
- CODE_XML_TUY,
- CODE_XML_TUZ
- } vpXmlCodeType;
-
- typedef enum
- {
- SEQUENCE_OK,
- SEQUENCE_ERROR
- } vpXmlCodeSequenceType;
-
-private :
-
- vpHomogeneousMatrix m_M;
- std::string m_name;
-
-public:
-
- vpXmlParserHomogeneousMatrix();
- vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix& twinParser);
- ~vpXmlParserHomogeneousMatrix(){}
-
- // get/set functions
- vpHomogeneousMatrix getHomogeneousMatrix() const {return this->m_M;}
- std::string getHomogeneousMatrixName() const {return this->m_name;}
-
- vpXmlParserHomogeneousMatrix& operator =(const vpXmlParserHomogeneousMatrix& twinparser);
- int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
-
- int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
-
- void setHomogeneousMatrixName(const std::string& name){
- this->m_name = name;
- }
-
-private:
- int read (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- int count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- int read_matrix (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- vpXmlCodeSequenceType read_values (xmlDocPtr doc, xmlNodePtr node,
- vpHomogeneousMatrix &M);
-
- static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
- void myXmlReadIntChild (xmlDocPtr doc,
- xmlNodePtr node,
- int &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadDoubleChild (xmlDocPtr doc,
- xmlNodePtr node,
- double &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadCharChild (xmlDocPtr doc,
- xmlNodePtr node,
- char **res);
- int write (xmlNodePtr node, const std::string& name);
-
-private:
-
- /*!
-
- \param 2doc : a pointer representing the document
- \param node : the root node of the document
- */
- virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
-
- /*!
-
-
- \param node2 : the root node of the document
- */
- virtual void writeMainClass(xmlNodePtr ){};
-
-};
-#endif //VISP_HAVE_XML2
-#endif
diff --git a/src/network/vpClient.cpp b/src/network/vpClient.cpp
deleted file mode 100644
index 66e5e2a..0000000
--- a/src/network/vpClient.cpp
+++ /dev/null
@@ -1,227 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpClient.cpp 4661 2014-02-10 19:34:58Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Client
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpClient.h>
-
-
-vpClient::vpClient() : vpNetwork(), numberOfAttempts(0)
-{
-}
-
-/*!
- Disconnect the client from all the servers, and close the sockets.
-*/
-vpClient::~vpClient()
-{
- stop();
-}
-
-/*!
- Connect to the server represented by the given hostname, and at a given port.
-
- \sa vpClient::connectToIP();
-
- \param hostname : Hostname of the server.
- \param port_serv : Port used for the connection.
-
- \return True if the connection has been etablished, false otherwise.
-*/
-bool vpClient::connectToHostname(const std::string &hostname, const unsigned int &port_serv)
-{
- // get server host information from hostname
- struct hostent *server = gethostbyname( hostname.c_str() );
-
- if ( server == NULL )
- {
- std::string noSuchHostMessage( "ERROR, " );
- noSuchHostMessage.append( hostname );
- noSuchHostMessage.append( ": no such host\n" );
- vpERROR_TRACE( noSuchHostMessage.c_str(),
- "vpClient::connectToHostname(const std::string &hostname, const int &port_serv)" );
- return false;
- }
-
- vpNetwork::vpReceptor serv;
-
- serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( serv.socketFileDescriptorReceptor < 0){
-#else
- if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
-#endif
- vpERROR_TRACE( "ERROR opening socket",
- "vpClient::connectToHostname()" );
- return false;
- }
-
- memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
- serv.receptorAddress.sin_family = AF_INET;
- memmove( (char *) &serv.receptorAddress.sin_addr.s_addr, (char *) server->h_addr, (unsigned)server->h_length );
- serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
- serv.receptorIP = inet_ntoa(*(in_addr *)server->h_addr);
-
- return connectServer(serv);
-}
-
-/*!
- Connect to the server represented by the given ip, and at a given port.
-
- \sa vpClient::connectToHostname()
-
- \param ip : IP of the server.
- \param port_serv : Port used for the connection.
-
- \return True if the connection has been etablished, false otherwise.
-*/
-bool vpClient::connectToIP(const std::string &ip, const unsigned int &port_serv)
-{
- vpNetwork::vpReceptor serv;
-
- serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( serv.socketFileDescriptorReceptor < 0){
-#else
- if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
-#endif
- vpERROR_TRACE( "ERROR opening socket",
- "vpClient::connectToIP()" );
- return false;
- }
-
- memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
- serv.receptorAddress.sin_family = AF_INET;
- serv.receptorAddress.sin_addr.s_addr = inet_addr(ip.c_str());
- serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
-
- return connectServer(serv);
-}
-
-/*!
- Deconnect from the server at a specific index.
-
- \param index : Index of the server.
-*/
-void vpClient::deconnect(const unsigned int &index)
-{
- if(index < receptor_list.size())
- {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- shutdown( receptor_list[index].socketFileDescriptorReceptor, SHUT_RDWR );
-#else // _WIN32
- shutdown( receptor_list[index].socketFileDescriptorReceptor, SD_BOTH );
-#endif
- receptor_list.erase(receptor_list.begin()+(int)index);
- }
-}
-
-/*!
- Stops the server and close the sockets.
-*/
-void vpClient::stop()
-{
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++){
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- shutdown( receptor_list[i].socketFileDescriptorReceptor, SHUT_RDWR );
-#else // _WIN32
- shutdown( receptor_list[i].socketFileDescriptorReceptor, SD_BOTH );
-#endif
- receptor_list.erase(receptor_list.begin()+(int)i);
- i--;
- }
-}
-
-/*!
- Print the servers.
-*/
-void vpClient::print()
-{
- vpNetwork::print("Server");
-}
-
-//Private function
-bool vpClient::connectServer(vpNetwork::vpReceptor &serv)
-{
- serv.receptorAddressSize = sizeof( serv.receptorAddress );
-
- numberOfAttempts = 15;
- unsigned int ind = 1;
- int connectionResult=-1;
-
- while(ind <= numberOfAttempts){
- std::cout << "Attempt number " << ind << "..." << std::endl;
-
- connectionResult = connect( serv.socketFileDescriptorReceptor,
- (sockaddr*) &serv.receptorAddress,
- serv.receptorAddressSize );
- if(connectionResult >= 0)
- break;
-
- ind++;
- vpTime::wait(1000);
- }
-
- if( connectionResult< 0 )
- {
- vpERROR_TRACE( "ERROR connecting, the server may not be waiting for connection at this port.",
- "vpClient::connectServer()");
-
- return false;
- }
-
- receptor_list.push_back(serv);
-
-#ifdef SO_NOSIGPIPE
- // Mac OS X does not have the MSG_NOSIGNAL flag. It does have this
- // connections based version, however.
- if (serv.socketFileDescriptorReceptor > 0) {
- int set_option = 1;
- if (0 == setsockopt(serv.socketFileDescriptorReceptor, SOL_SOCKET, SO_NOSIGPIPE, &set_option, sizeof(set_option))) {
- } else {
- std::cout << "Failed to set socket signal option" << std::endl;
- }
- }
-#endif // SO_NOSIGPIPE
-
- std::cout << "Connected!" << std::endl;
- return true;
-}
diff --git a/src/network/vpClient.h b/src/network/vpClient.h
deleted file mode 100644
index 0f45779..0000000
--- a/src/network/vpClient.h
+++ /dev/null
@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpClient.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Client
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#ifndef vpClient_H
-#define vpClient_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpRequest.h>
-#include <visp/vpNetwork.h>
-#include <visp/vpTime.h>
-
-
-/*!
- \class vpClient
-
- \ingroup Network
-
- \brief This class represents a Transmission Control Protocol (TCP) client.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
- on one computer to another program on another computer.
-
- Exemple of client's code, receiving and sending basic message
- It corresponds to the client used in the first exemple of vpServer class' documentation:
-
- \code
-#include <visp/vpClient.h>
-#include <iostream>
-
-int main(int argc, char **argv)
-{
- std::string servername = "localhost";
- unsigned int port = 35000;
-
- vpClient client;
- client.connectToHostname(servername, port);
- //client.connectToIP("127.0.0.1",port);
-
- int val = 0;
-
- while(1)
- {
- if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
- std::cout << "Error while sending" << std::endl;
- else
- std::cout << "Sending : " << val << std::endl;
-
- if(client.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
- else
- std::cout << "Received : " << val << std::endl;
- }
-
- return 0;
-}
- \endcode
-
- Exemple of client's code, sending a vpImage on request form.
- It correspond to the server used in the second exemple of vpServer class' documentation.
-
- \code
-#include <visp/vpClient.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <iostream>
-
-#include "vpRequestImage.h" //See vpRequest class documentation
-
-int main(int argc, char **argv)
-{
-#if defined(VISP_HAVE_V4L2)
- std::string servername = "localhost";
- unsigned int port = 35000;
-
- vpImage<unsigned char> I; // Create a gray level image container
-
- // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
- vpV4l2Grabber g;
- g.setScale(1);
- g.setInput(0);
- g.open(I);
-
- // Create an image viewer
-#if defined(VISP_HAVE_X11)
- vpDisplayX d(I, -1, -1, "Camera frame");
-#elif defined(VISP_HAVE_GDI) //Win32
- vpDisplayGDI d(I, -1, -1, "Camera frame");
-#endif
-
- vpClient client;
- client.connectToHostname(servername, port);
- //client.connectToIP("127.0.0.1",port);
-
- vpRequestImage reqImage(&I);
-
- while(1)
- {
- double t = vpTime::measureTimeMs();
- // Acquire a new image
- g.acquire(I);
-
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- client.sendAndEncodeRequest(reqImage);
-
- // A click in the viewer to exit
- if ( vpDisplay::getClick(I, false) )
- break;
- }
-
- return 0;
-#endif
-}
- \endcode
-
- \sa vpClient
- \sa vpRequest
- \sa vpNetwork
-*/
-class VISP_EXPORT vpClient : public vpNetwork
-{
-private:
- //######## PARAMETERS ########
- //# #
- //############################
-
- unsigned int numberOfAttempts;
-
- //######## Private Functions ########
- //# #
- //###################################
-
- bool connectServer(vpNetwork::vpReceptor &serv);
-
-public:
- vpClient();
- virtual ~vpClient();
-
- bool connectToHostname(const std::string &hostname, const unsigned int &port_serv);
- bool connectToIP(const std::string &ip, const unsigned int &port_serv);
-
- void deconnect(const unsigned int &index = 0);
- /*!
- Get the actual number of attempts to connect to the server.
-
- \sa vpClient::setNumberOfAttempts()
-
- \return Actual number of attempts.
- */
- unsigned int getNumberOfAttempts(){ return numberOfAttempts; }
-
- /*!
- Get the number of server that the client is connected on.
-
- \return Number of servers.
- */
- unsigned int getNumberOfServers(){ return (unsigned int)receptor_list.size(); }
-
- void print();
-
- /*!
- Set the number of attempts to connect to the server.
-
- \sa vpClient::getNumberOfAttempts()
-
- \param nb : Number of attempts.
- */
- void setNumberOfAttempts(const unsigned int &nb){ numberOfAttempts = nb; }
-
- void stop();
-};
-
-#endif
diff --git a/src/network/vpNetwork.cpp b/src/network/vpNetwork.cpp
deleted file mode 100644
index 459e6e5..0000000
--- a/src/network/vpNetwork.cpp
+++ /dev/null
@@ -1,793 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNetwork.cpp 4794 2014-07-21 17:43:22Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Network
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-
-#include <visp/vpNetwork.h>
-
-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)
-{
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
-#if defined(_WIN32)
- //Enable the sockets to be used
- //Note that: if we were using "winsock.h" instead of "winsock2.h" we would had to use:
- //WSAStartup(MAKEWORD(1,0), &WSAData);
- WSADATA WSAData;
- WSAStartup(MAKEWORD(2,0), &WSAData);
-#endif
-}
-
-vpNetwork::~vpNetwork()
-{
-#if defined(_WIN32)
- WSACleanup();
-#endif
-}
-
-/*!
- Add a decoding request to the emitter. This request will be used to decode the received messages.
- Each request must have a different id.
-
- \warning vpRequest is a virtual pure class. It has to be implemented according to the way how you want
- to decode the message received.
-
- \sa vpNetwork::removeDecodingRequest()
-
- \param req : Request to add.
-*/
-void vpNetwork::addDecodingRequest(vpRequest *req)
-{
- bool alreadyHas = false;
-
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- if(request_list[i]->getId() == req->getId()){
- alreadyHas = true;
- break;
- }
-
- if(alreadyHas)
- std::cout << "Server already has one request with the similar ID. Request hasn't been added." << std::endl;
- else
- request_list.push_back(req);
-}
-
-/*!
- Delete a decoding request from the emitter.
-
- \sa vpNetwork::addDecodingRequest()
-
- \param id : Id of the request to delete.
-*/
-void vpNetwork::removeDecodingRequest(const char *id)
-{
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- {
- if(request_list[i]->getId() == id)
- {
- request_list.erase(request_list.begin()+(int)i);
- break;
- }
- }
-}
-
-/*!
- Print the receptors.
-
- \param id : Message to display before the receptor's index.
-*/
-void vpNetwork::print(const char *id)
-{
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
- {
- std::cout << id << i << " : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- }
-}
-
-/*!
- Get the receptor index from its name. The name can be either the IP, or its name on the network.
-
- \param name : Name of the receptor.
-
- \return Index of the receptor, or -1 if an error occurs.
-*/
-int vpNetwork::getReceptorIndex(const char *name)
-{
- struct hostent *server = gethostbyname(name);
-
- if ( server == NULL )
- {
- std::string noSuchHostMessage( "ERROR, " );
- noSuchHostMessage.append( name );
- noSuchHostMessage.append( ": no such host\n" );
- vpERROR_TRACE( noSuchHostMessage.c_str(), "vpNetwork::getReceptorIndex()" );
- return -1;
- }
-
- std::string ip = inet_ntoa(*(in_addr *)server->h_addr);
-
- for(int i = 0 ; i < (int)receptor_list.size() ; i++)
- {
- if(receptor_list[(unsigned)i].receptorIP == ip)
- return i;
- }
-
- return -1;
-}
-
-/*!
- Send a request to the first receptor in the list.
-
- \sa vpNetwork::sendRequestTo()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
- \sa vpNetwork::send()
- \sa vpNetwork::sendTo()
-
- \param req : Request to send.
-
- \return The number of bytes that have been sent, -1 if an error occured.
-*/
-int vpNetwork::sendRequest(vpRequest &req)
-{
- return sendRequestTo(req,0);
-}
-
-/*!
- Send a request to a specific receptor.
-
- \sa vpNetwork::sendRequest()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
- \sa vpNetwork::send()
- \sa vpNetwork::sendTo()
-
- \param req : Request to send.
- \param dest : Index of the receptor receiving the request.
-
- \return The number of bytes that have been sent, -1 if an error occured.
-*/
-int vpNetwork::sendRequestTo(vpRequest &req, const unsigned int &dest)
-{
- int size = (int)receptor_list.size();
- int sizeMinusOne = (int)receptor_list.size()-1;
- if(size == 0 || dest > (unsigned)sizeMinusOne)
- {
- if(verboseMode)
- vpTRACE( "Cannot Send Request! Bad Index" );
- return 0;
- }
-
- std::string message = beginning + req.getId() + separator;
-
- if(req.size() != 0){
- message += req[0];
-
- for(unsigned int i = 1 ; i < req.size() ; i++){
- message += param_sep + req[i];
- }
- }
-
- message += end;
-
- int flags = 0;
-//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
-#if defined(__linux__)
- flags = MSG_NOSIGNAL; // Only for Linux
-#endif
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int value = sendto(receptor_list[dest].socketFileDescriptorReceptor, message.c_str(), message.size(), flags,
- (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
-#else
- int value = sendto((unsigned)receptor_list[dest].socketFileDescriptorReceptor, message.c_str(), (int)message.size(), flags,
- (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
-#endif
-
- return value;
-}
-
-/*!
- Send and encode a request to the first receptor in the list.
-
- \sa vpNetwork::sendRequestTo()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
- \sa vpNetwork::send()
- \sa vpNetwork::sendTo()
-
- \param req : Request to send.
-
- \return The number of bytes that have been sent, -1 if an error occured.
-*/
-int vpNetwork::sendAndEncodeRequest(vpRequest &req)
-{
- req.encode();
- return sendRequest(req);
-}
-
-/*!
- Send and encode a request to a specific receptor.
-
- \sa vpNetwork::sendRequest()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
- \sa vpNetwork::send()
- \sa vpNetwork::sendTo()
-
- \param req : Request to send.
- \param dest : Index of the receptor receiving the request.
-
- \return The number of bytes that have been sent, -1 if an error occured.
-*/
-int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest)
-{
- req.encode();
- return sendRequestTo(req,dest);
-}
-
-/*!
- Receive requests untils there is requests to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-*/
-std::vector<int> vpNetwork::receiveRequest()
-{
- _receiveRequest();
- return _handleRequests();
-}
-
-/*!
- Receives requests, from a specific emitter, untils there is request to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message
-*/
-std::vector<int> vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting)
-{
- _receiveRequestFrom(receptorEmitting);
- return _handleRequests();
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::receiveRequestOnce()
-{
- _receiveRequestOnce();
- return _handleFirstRequest();
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message.
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting)
-{
- _receiveRequestOnceFrom(receptorEmitting);
- return _handleFirstRequest();
-}
-
-/*!
- Receives and decode requests untils there is requests to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-*/
-std::vector<int> vpNetwork::receiveAndDecodeRequest()
-{
- std::vector<int> res = receiveRequest();
- for(unsigned int i = 0 ; i < res.size() ; i++)
- if(res[i] != -1)
- request_list[(unsigned)res[i]]->decode();
-
- return res;
-}
-
-/*!
- Receives and decode requests, from a specific emitter, untils there is request to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message
-*/
-std::vector<int> vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting)
-{
- std::vector<int> res = receiveRequestFrom(receptorEmitting);
- for(unsigned int i = 0 ; i < res.size() ; i++)
- if(res[i] != -1)
- request_list[(unsigned)res[i]]->decode();
-
- return res;
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
- If it represents an entire request, it decodes the request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::receiveAndDecodeRequestOnce()
-{
- int res = receiveRequestOnce();
- if(res != -1)
- request_list[(unsigned)res]->decode();
-
- return res;
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
- If it represents an entire request, it decodes the request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message.
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting)
-{
- int res = receiveRequestOnceFrom(receptorEmitting);
- if(res != -1)
- request_list[(unsigned)res]->decode();
-
- return res;
-}
-
-
-//######## Definition of Template Functions ########
-//# #
-//##################################################
-
-
-/*!
- Handle requests until there are requests to handle.
-
- \warning : This function doesn't decode the requests. If it does handle a request that hasn't been ran yet,
- The request's parameters will be replace.
-
- \sa vpNetwork::handleFirstRequest()
-
- \return : The list of index corresponding to the requests that have been handled.
-*/
-std::vector<int> vpNetwork::_handleRequests()
-{
- std::vector<int> resIndex;
- int index = _handleFirstRequest();
-
- while(index != -1)
- {
- resIndex.push_back(index);
- index = _handleFirstRequest();
- }
-
- return resIndex;
-}
-
-/*!
- Handle the first request in the queue.
-
- \warning : This function doesn't run the request. If it does handle a request that hasn't been ran yet,
- The request's parameters will be replace.
-
- \sa vpNetwork::handleRequests()
-
- \return : The index of the request that has been handled.
-*/
-int vpNetwork::_handleFirstRequest()
-{
- size_t indStart = currentMessageReceived.find(beginning);
- size_t indSep = currentMessageReceived.find(separator);
- size_t indEnd = currentMessageReceived.find(end);
-
- if (indStart == std::string::npos && indSep == std::string::npos && indEnd == std::string::npos)
- {
- if(currentMessageReceived.size() != 0)
- currentMessageReceived.clear();
-
- if(verboseMode)
- vpTRACE("Incorrect message");
-
- return -1;
- }
-
- if(indStart == std::string::npos || indSep == std::string::npos || indEnd == std::string::npos)
- return -1;
-
- if(indEnd < indStart)
- {
- if(verboseMode)
- vpTRACE("Incorrect message");
- currentMessageReceived.erase((unsigned)indStart,indEnd+end.size());
- return -1;
- }
-
- size_t indStart2 = currentMessageReceived.find(beginning,indStart+1);
- if(indStart2 != std::string::npos && indStart2 < indEnd)
- {
- if(verboseMode)
- vpTRACE("Incorrect message");
- currentMessageReceived.erase((unsigned)indStart,(unsigned)indStart2);
- return -1;
- }
-
- size_t deb = indStart + beginning.size();
- std::string id = currentMessageReceived.substr((unsigned)deb, indSep - deb);
-
- deb = indSep+separator.size();
- std::string params = currentMessageReceived.substr((unsigned)deb, (unsigned)(indEnd - deb));
-
-// std::cout << "Handling : " << currentMessageReceived.substr(indStart, indEnd+end.size() - indStart) << std::endl;
-
- int indRequest = 0;
- bool hasBeenFound = false;
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- {
- if(id == request_list[i]->getId()){
- hasBeenFound = true;
- request_list[i]->clear();
- indRequest = (int)i;
- break;
- }
- }
-
- if(!hasBeenFound){
- //currentMessageReceived.erase(indStart,indEnd+end.size());
- if(verboseMode)
- vpTRACE("No request corresponds to the received message");
- return -1;
- }
-
- size_t indDebParam = indSep + separator.size();
- size_t indEndParam = currentMessageReceived.find(param_sep,indDebParam);
-
- std::string param;
- while(indEndParam != std::string::npos || indEndParam < indEnd)
- {
- param = currentMessageReceived.substr((unsigned)indDebParam,(unsigned)(indEndParam - indDebParam));
- request_list[(unsigned)indRequest]->addParameter(param);
- indDebParam = indEndParam+param_sep.size();
- indEndParam = currentMessageReceived.find(param_sep,indDebParam);
- }
-
- param = currentMessageReceived.substr((unsigned)indDebParam, indEnd - indDebParam);
- request_list[(unsigned)indRequest]->addParameter(param);
- currentMessageReceived.erase(indStart,indEnd+end.size());
-
- return indRequest;
-}
-
-/*!
- Receive requests untils there is requests to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-*/
-void vpNetwork::_receiveRequest()
-{
- while(_receiveRequestOnce() > 0) {};
-}
-
-/*!
- Receives requests, from a specific emitter, untils there is request to receive.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message
-*/
-void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting)
-{
- while(_receiveRequestOnceFrom(receptorEmitting) > 0) {};
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::_receiveRequestOnce()
-{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No Receptor!" );
- return -1;
- }
-
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
- FD_ZERO(&readFileDescriptor);
-
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(i == 0)
- socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
- if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
- }
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- int numbytes = 0;
-
- if(value == -1){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
- return -1;
- }
- else if(value == 0){
- //Timeout
- return 0;
- }
- 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];
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes=recv(receptor_list[i].socketFileDescriptorReceptor, buf, max_size_message, 0);
-#else
- numbytes=recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, buf, (int)max_size_message, 0);
-#endif
-
- if(numbytes <= 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
- delete [] buf;
- return numbytes;
- }
- else if(numbytes > 0){
- std::string returnVal(buf, (unsigned int)numbytes);
- currentMessageReceived.append(returnVal);
- }
- delete [] buf;
- break;
- }
- }
- }
-
- return numbytes;
-}
-
-/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
- \warning Requests will be received but not decoded.
-
- \sa vpNetwork::receive()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param receptorEmitting : Index of the receptor emitting the message.
-
- \return The number of bytes received, -1 if an error occured.
-*/
-int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting)
-{
- int size = (int)receptor_list.size();
- int sizeMinusOne = (int)receptor_list.size()-1;
- if(size == 0 || receptorEmitting > (unsigned)sizeMinusOne )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index!" );
- return -1;
- }
-
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
- FD_ZERO(&readFileDescriptor);
-
- socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;
- FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- int numbytes = 0;
- if(value == -1){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
- return -1;
- }
- else if(value == 0){
- //Timeout
- return 0;
- }
- 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
- numbytes=recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, max_size_message, 0);
-#else
- numbytes=recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, (int)max_size_message, 0);
-#endif
- if(numbytes <= 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
- delete [] buf;
- return numbytes;
- }
- else if(numbytes > 0){
- std::string returnVal(buf, (unsigned int)numbytes);
- currentMessageReceived.append(returnVal);
- }
- delete [] buf;
- }
- }
-
- return numbytes;
-}
-
-
-
diff --git a/src/network/vpNetwork.h b/src/network/vpNetwork.h
deleted file mode 100644
index 4be624c..0000000
--- a/src/network/vpNetwork.h
+++ /dev/null
@@ -1,499 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpNetwork.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Network
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#ifndef vpNetwork_H
-#define vpNetwork_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpRequest.h>
-
-#include <vector>
-#include <stdio.h>
-#include <string.h>
-#include <iostream>
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
-# include <sys/socket.h>
-# include <netinet/in.h>
-# include <arpa/inet.h>
-# include <netdb.h>
-#else
-# include<io.h>
-//# include<winsock.h>
-# include<winsock2.h>
-//# pragma comment(lib, "ws2_32.lib") // Done by CMake in main CMakeLists.txt
-#endif
-
-
-/*!
- \class vpNetwork
-
- \ingroup Network
-
- \brief This class represents a Transmission Control Protocol (TCP) network.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
- on one computer to another program on another computer.
-
- \warning This class shouldn't be used directly. You better use vpClient and
- vpServer to simulate your network. Some exemples are provided in these classes.
-
- \sa vpServer
- \sa vpNetwork
-*/
-class VISP_EXPORT vpNetwork
-{
-protected:
-
- struct vpReceptor{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketFileDescriptorReceptor;
- socklen_t receptorAddressSize;
-#else
- SOCKET socketFileDescriptorReceptor;
- int receptorAddressSize;
-#endif
- struct sockaddr_in receptorAddress;
- std::string receptorIP;
-
- vpReceptor() : socketFileDescriptorReceptor(0), receptorAddressSize(), receptorAddress(), receptorIP() {}
- };
-
- struct vpEmitter{
- struct sockaddr_in emitterAddress;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketFileDescriptorEmitter;
-#else
- SOCKET socketFileDescriptorEmitter;
-#endif
- vpEmitter() : emitterAddress(), socketFileDescriptorEmitter(0)
- {
- emitterAddress.sin_family = AF_INET;
- emitterAddress.sin_addr.s_addr = INADDR_ANY;
- emitterAddress.sin_port = 0;
- socketFileDescriptorEmitter = 0;
- }
- };
-
- //######## PARAMETERS ########
- //# #
- //############################
-
- vpEmitter emitter;
- std::vector<vpReceptor> receptor_list;
- fd_set readFileDescriptor;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketMax;
-#else
- SOCKET socketMax;
-#endif
-
- //Message Handling
- std::vector<vpRequest*> request_list;
-
- unsigned int max_size_message;
- std::string separator;
- std::string beginning;
- std::string end;
- std::string param_sep;
-
- std::string currentMessageReceived;
-
- struct timeval tv;
- long tv_sec;
- long tv_usec;
-
- bool verboseMode;
-
-private:
-
- std::vector<int> _handleRequests();
- int _handleFirstRequest();
-
- void _receiveRequest();
- void _receiveRequestFrom(const unsigned int &receptorEmitting);
- int _receiveRequestOnce();
- int _receiveRequestOnceFrom(const unsigned int &receptorEmitting);
-
-public:
-
- vpNetwork();
- virtual ~vpNetwork();
-
- void addDecodingRequest(vpRequest *);
-
- int getReceptorIndex(const char *name);
-
- /*!
- Get the Id of the request at the index ind.
-
- \param ind : Index of the request.
-
- \return Id of the request.
- */
- std::string getRequestIdFromIndex(const int &ind){
- if(ind >= (int)request_list.size() || ind < 0)
- return "";
- return request_list[(unsigned)ind]->getId();
- }
-
- /*!
- Get the maximum size that the emitter can receive (in request mode).
-
- \sa vpNetwork::setMaxSizeReceivedMessage()
-
- \return Acutal max size value.
- */
- unsigned int getMaxSizeReceivedMessage(){ return max_size_message; }
-
- void print(const char *id = "");
-
- template<typename T>
- int receive(T* object, const unsigned int &sizeOfObject = sizeof(T));
- template<typename T>
- int receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject = sizeof(T));
-
- std::vector<int> receiveRequest();
- std::vector<int> receiveRequestFrom(const unsigned int &receptorEmitting);
- int receiveRequestOnce();
- int receiveRequestOnceFrom(const unsigned int &receptorEmitting);
-
- std::vector<int> receiveAndDecodeRequest();
- std::vector<int> receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting);
- int receiveAndDecodeRequestOnce();
- int receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting);
-
- void removeDecodingRequest(const char *);
-
- template<typename T>
- int send(T* object, const int unsigned &sizeOfObject = sizeof(T));
- template<typename T>
- int sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject = sizeof(T));
-
- int sendRequest(vpRequest &req);
- int sendRequestTo(vpRequest &req, const unsigned int &dest);
-
- int sendAndEncodeRequest(vpRequest &req);
- int sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest);
-
- /*!
- Change the maximum size that the emitter can receive (in request mode).
-
- \sa vpNetwork::getMaxSizeReceivedMessage()
-
- \param s : new maximum size value.
- */
- void setMaxSizeReceivedMessage(const unsigned int &s){ max_size_message = s;}
-
- /*!
- Change the time the emitter spend to check if he receives a message from a receptor.
- Initially this value is set to 10usec.
-
- \sa vpNetwork::setTimeoutUSec()
-
- \param sec : new value in second.
- */
- void setTimeoutSec(const long &sec){ tv_sec = sec; }
-
- /*!
- Change the time the emitter spend to check if he receives a message from a receptor.
- Initially this value is set to 10usec.
-
- \sa vpNetwork::setTimeoutSec()
-
- \param usec : new value in micro second.
- */
- void setTimeoutUSec(const long &usec){ tv_usec = usec; }
-
- /*!
- Set the verbose mode.
-
- \param mode : Change the verbose mode. True to turn on, False to turn off.
- */
- void setVerbose(const bool &mode){ verboseMode = mode; }
-};
-
-//######## Definition of Template Functions ########
-//# #
-//##################################################
-
-/*!
- Receives a object. The size of the received object is suppose to be the size of the type of the object.
- Note that a received message can correspond to a deconnection signal.
-
- \warning Using this function means that you know what kind of object you are suppose to receive,
- and when you are suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
- \sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
- \sa vpNetwork::receiveAndDecodeRequest()
- \sa vpNetwork::receiveAndDecodeRequestOnce()
-
- \param object : Received object.
- \param sizeOfObject : Size of the received object.
-
- \return the number of bytes received, or -1 if an error occured.
-*/
-template<typename T>
-int vpNetwork::receive(T* object, const unsigned int &sizeOfObject)
-{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No receptor" );
- return -1;
- }
-
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
- FD_ZERO(&readFileDescriptor);
-
- for(unsigned int i=0; i<receptor_list.size(); i++){
- FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
-
- if(i == 0)
- socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
- }
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- int numbytes = 0;
-
- if(value == -1){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
- return -1;
- }
- else if(value == 0){
- //Timeout
- return 0;
- }
- else{
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes = recv(receptor_list[i].socketFileDescriptorReceptor, (char*)(void*)object, sizeOfObject, 0);
-#else
- numbytes = recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, (char*)(void*)object, (int)sizeOfObject, 0);
-#endif
- if(numbytes <= 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
- return numbytes;
- }
-
- break;
- }
- }
- }
-
- return numbytes;
-}
-
-/*!
- Receives a object from a receptor, by specifying its size or not.
- Note that a received message can correspond to a deconnection signal.
-
- \warning Using this function means that you know what kind of object you are suppose to receive,
- and when you are suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" mode.
-
- \sa vpNetwork::getReceptorIndex()
- \sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequestOnceFrom()
- \sa vpNetwork::receiveAndDecodeRequestFrom()
- \sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
- \param object : Received object.
- \param receptorEmitting : Index of the receptor emitting the message.
- \param sizeOfObject : Size of the received object.
-
- \return the number of bytes received, or -1 if an error occured.
-*/
-template<typename T>
-int vpNetwork::receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject)
-{
- if(receptor_list.size() == 0 || receptorEmitting > (int)receptor_list.size()-1 )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index" );
- return -1;
- }
-
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
- FD_ZERO(&readFileDescriptor);
-
- socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;
- FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- int numbytes = 0;
-
- if(value == -1){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
- return -1;
- }
- else if(value == 0){
- //timeout
- return 0;
- }
- else{
- if(FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor)){
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes = recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, sizeOfObject, 0);
-#else
- numbytes = recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, (int)sizeOfObject, 0);
-#endif
- if(numbytes <= 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
- return numbytes;
- }
- }
- }
-
- return numbytes;
-}
-
-/*!
- Send an object. The size of the received object is suppose to be the size of its type.
- Note that sending object containing pointers, virtual methods, etc, won't probably work.
-
- \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
- and when it is suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
- \sa vpNetwork::sendTo()
- \sa vpNetwork::sendRequest()
- \sa vpNetwork::sendRequestTo()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
-
- \param object : Received object.
- \param sizeOfObject : Size of the object
-
- \return The number of bytes sent, or -1 if an error happened.
-*/
-template<typename T>
-int vpNetwork::send(T* object, const unsigned int &sizeOfObject)
-{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No receptor !" );
- return 0;
- }
-
- int flags = 0;
-//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
-#if defined(__linux__)
- flags = MSG_NOSIGNAL; // Only for Linux
-#endif
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- return sendto(receptor_list[0].socketFileDescriptorReceptor, (const char*)(void*)object, sizeOfObject,
- flags, (sockaddr*) &receptor_list[0].receptorAddress,receptor_list[0].receptorAddressSize);
-#else
- return sendto(receptor_list[0].socketFileDescriptorReceptor, (const char*)(void*)object, (int)sizeOfObject,
- flags, (sockaddr*) &receptor_list[0].receptorAddress,receptor_list[0].receptorAddressSize);
-#endif
-
-}
-
-/*!
- Send an object. The size has to be specified.
-
- \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
- and when it is suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
- \sa vpNetwork::getReceptorIndex()
- \sa vpNetwork::send()
- \sa vpNetwork::sendRequest()
- \sa vpNetwork::sendRequestTo()
- \sa vpNetwork::sendAndEncodeRequest()
- \sa vpNetwork::sendAndEncodeRequestTo()
-
- \param object : Object to send.
- \param dest : Index of the receptor that you are sending the object.
- \param sizeOfObject : Size of the object.
-
- \return The number of bytes sent, or -1 if an error happened.
-*/
-template<typename T>
-int vpNetwork::sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject)
-{
- if(receptor_list.size() == 0 || dest > (int)receptor_list.size()-1 )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index." );
- return 0;
- }
-
- int flags = 0;
-//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
-#if defined(__linux__)
- flags = MSG_NOSIGNAL; // Only for Linux
-#endif
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- return sendto(receptor_list[dest].socketFileDescriptorReceptor, (const char*)(void*)object, sizeOfObject,
- flags, (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
-#else
- return sendto(receptor_list[dest].socketFileDescriptorReceptor, (const char*)(void*)object, (int)sizeOfObject,
- flags, (sockaddr*) &receptor_list[dest].receptorAddress,receptor_list[dest].receptorAddressSize);
-#endif
-}
-
-#endif
diff --git a/src/network/vpRequest.cpp b/src/network/vpRequest.cpp
deleted file mode 100644
index 4195c39..0000000
--- a/src/network/vpRequest.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRequest.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Network request.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpRequest.h>
-
-vpRequest::vpRequest()
- : request_id(""), listOfParams()
-{}
-
-vpRequest::~vpRequest()
-{}
-
-/*!
- Add a message as parameter of the request.
-
- \sa vpNetwork::addParameterObject()
-
- \param params : Array of characters representing the message to add.
-*/
-void vpRequest::addParameter(char *params)
-{
- std::string val = params;
- listOfParams.push_back(val);
-}
-
-/*!
- Add a message as parameter of the request.
-
- \sa vpNetwork::addParameterObject()
-
- \param params : std::string representing the message to add.
-*/
-void vpRequest::addParameter(std::string ¶ms)
-{
- listOfParams.push_back(params);
-}
-
-/*!
- Add messages as parameters of the request.
- Each message corresponds to one parameter.
-
- \sa vpNetwork::addParameterObject()
-
- \param listOfparams : Array of std::string representing the messages to add.
-*/
-void vpRequest::addParameter(std::vector< std::string > &listOfparams)
-{
- for(unsigned int i = 0; i < listOfparams.size() ; i++)
- listOfparams.push_back(listOfparams[i]);
-}
diff --git a/src/network/vpRequest.h b/src/network/vpRequest.h
deleted file mode 100644
index a155160..0000000
--- a/src/network/vpRequest.h
+++ /dev/null
@@ -1,241 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRequest.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Network Request.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#ifndef vpRequest_H
-#define vpRequest_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpImageException.h>
-
-#include <string.h>
-#include <vector>
-
-/*!
- \class vpRequest
-
- \ingroup Network
-
- \brief This the request that will transit on the network
-
- Exemple request decoding an image on a specific form.
- First parameter : Height of the image.
- Second parameter : Width of the image.
- Thirs parameter : Bitmap of the image (not compress).
-
- Here is the header of the vpRequestImage class.
-
- \code
-#ifndef vpRequestImage_H
-#define vpRequestImage_H
-
-#include <visp/vpImage.h>
-#include <visp/vpRequest.h>
-
-class vpRequestImage : public vpRequest
-{
-private:
- vpImage<unsigned char> *I;
-
-public:
- vpRequestImage();
- vpRequestImage(vpImage<unsigned char> *);
- ~vpRequestImage();
-
- virtual void encode();
- virtual void decode();
-};
-
-#endif
- \endcode
-
- Here is the definition of the vpRequestImage class.
-
- \code
-#include <vpRequestImage.h>
-
-vpRequestImage::vpRequestImage(){
- request_id = "image";
-}
-
-vpRequestImage::vpRequestImage(vpImage<unsigned char> *Im){
- request_id = "image";
- I = Im;
-}
-
-vpRequestImage::~vpRequestImage(){}
-
-void vpRequestImage::encode(){
- clear();
-
- unsigned int h = I->getHeight();
- unsigned int w = I->getWidth();
-
- addParameterObject(&h);
- addParameterObject(&w);
- addParameterObject(I->bitmap,h*w*sizeof(unsigned char));
-}
-
-void vpRequestImage::decode(){
- if(listOfParams.size() == 3){
- unsigned int w, h;
- memcpy((void*)&h, (void*)listOfParams[0].c_str(), sizeof(unsigned int));
- memcpy((void*)&w, (void*)listOfParams[1].c_str(), sizeof(unsigned int));
-
- I->resize(h,w);
- memcpy((void*)I->bitmap,(void*)listOfParams[2].c_str(),w*h*sizeof(unsigned char));
- }
-}
- \endcode
-
- \sa vpClient
- \sa vpServer
- \sa vpNetwork
-*/
-class VISP_EXPORT vpRequest
-{
-protected:
- std::string request_id;
- std::vector<std::string> listOfParams;
-
-public:
- vpRequest();
- virtual ~vpRequest();
-
- void addParameter(char *params);
- void addParameter(std::string ¶ms);
- void addParameter(std::vector<std::string> &listOfparams);
- template<typename T>
- void addParameterObject(T * params, const int &sizeOfObject = sizeof(T));
-
- /*!
- Decode the parameters of the request (Funtion that has to be redifined).
-
- \sa vpRequest::encode()
- */
- virtual void decode() = 0;
-
- /*!
- Clear the parameters of the request.
- */
- void clear(){ listOfParams.clear(); }
-
- /*!
- Encode the parameters of the request (Funtion that has to be redifined).
-
- \sa vpRequest::decode()
- */
- virtual void encode() = 0;
-
- /*!
- Accessor on the parameters.
-
- \return Parameter at the index i.
- */
- inline std::string& operator[](const unsigned int &i) { return listOfParams[i];}
-
- /*!
- Accessor on the parameters (const).
-
- \return Parameter at the index i (const).
- */
- inline const std::string& operator[](const unsigned int &i) const { return listOfParams[i];}
-
- /*!
- Get the ID of the request.
-
- \sa vpRequest::setId()
-
- \return ID of the request.
- */
- std::string getId(){ return request_id; }
-
- /*!
- Change the ID of the request.
-
- \sa vpRequest::getId()
-
- \param id : new ID.
- */
- void setId(const char *id){ request_id = id; }
-
- /*!
- Get the number of parameters.
-
- \return Number of parameters.
- */
- unsigned int size(){ return (unsigned int)listOfParams.size(); }
-};
-
-
-//######## Definition of Template Functions ########
-//# #
-//##################################################
-
-
-/*!
- Add an object as parameter of the request.
-
- \warning Only simple object can be sent unless you know its size.
- Sending object containing pointers, virtual methods, etc, won't probably work.
- Unless the size is well defined...
-
- \sa vpRequest::addParameter()
-
- \param params : Object to add.
- \param sizeOfObject : Size of the object.
-*/
-template<typename T>
-void vpRequest::addParameterObject(T * params, const int &sizeOfObject)
-{
- if(sizeOfObject != 0){
- char *tempS = new char [sizeOfObject];
- memcpy((void*)tempS, (void*)params, sizeOfObject);
- std::string returnVal(tempS, sizeOfObject);
-
- listOfParams.push_back(returnVal);
-
- delete [] tempS;
- }
-}
-
-#endif
diff --git a/src/network/vpServer.cpp b/src/network/vpServer.cpp
deleted file mode 100644
index 01647ac..0000000
--- a/src/network/vpServer.cpp
+++ /dev/null
@@ -1,285 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServer.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Server
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpServer.h>
-
-
-/*!
- Construct a server on the machine launching it.
-*/
-vpServer::vpServer( ) : adress(), port(0), started(false), max_clients(10)
-{
- int protocol = 0;
- emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if (emitter.socketFileDescriptorEmitter < 0)
-#else
- if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
-#endif
- {
- vpERROR_TRACE( "vpServer::vpServer(), cannot open socket." );
- }
- emitter.emitterAddress.sin_family = AF_INET;
- emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY;
- emitter.emitterAddress.sin_port = 0;
-
- adress = inet_ntoa(emitter.emitterAddress.sin_addr);
- port = emitter.emitterAddress.sin_port;
-}
-
-/*!
- Construct a server on the machine launching it, with a specified port.
-
- \param port_serv : server's port.
-*/
-vpServer::vpServer( const int &port_serv ) : adress(), port(0), started(false), max_clients(10)
-{
- int protocol = 0;
- emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if (emitter.socketFileDescriptorEmitter < 0)
-#else
- if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
-#endif
- {
- vpERROR_TRACE( "vpServer::vpServer(const int &port_serv), cannot open socket." );
- }
- emitter.emitterAddress.sin_family = AF_INET;
- emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY; //inet_addr("127.0.0.1");;
- emitter.emitterAddress.sin_port = htons( (unsigned short)port_serv );
-
- adress = inet_ntoa(emitter.emitterAddress.sin_addr);
- port = port_serv;
-}
-
-/*!
- Construct a server on the machine at a given adress, with a specified port.
-
- \param adress_serv : server's adress.
- \param port_serv : server's port.
-*/
-vpServer::vpServer( const std::string &adress_serv,const int &port_serv )
- : adress(), port(0), started(false), max_clients(10)
-{
- int protocol = 0;
- emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if (emitter.socketFileDescriptorEmitter < 0)
-#else
- if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
-#endif
- {
- vpERROR_TRACE( "vpServer::vpServer(const std::string &adress_serv,const int &port_serv), cannot open socket." );
- }
- emitter.emitterAddress.sin_family = AF_INET;
- emitter.emitterAddress.sin_addr.s_addr = inet_addr(adress_serv.c_str());
- emitter.emitterAddress.sin_port = htons( (unsigned short)port_serv );
-
- adress = adress_serv;
- port = port_serv;
-}
-
-/*!
- Shutdown the server.
-*/
-vpServer::~vpServer()
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- close( emitter.socketFileDescriptorEmitter );
-#else //Win32
- closesocket( (unsigned)emitter.socketFileDescriptorEmitter );
-#endif
-
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- close( receptor_list[i].socketFileDescriptorReceptor );
-#else //Win32
- closesocket( (unsigned)receptor_list[i].socketFileDescriptorReceptor );
-#endif
-
-}
-
-/*!
- Enable the server to wait for clients (on the limit of the maximum limit).
-
- \return True if the server has started, false otherwise.
-*/
-bool vpServer::start()
-{
- int serverStructLength = sizeof(emitter.emitterAddress);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int bindResult = bind( emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, (unsigned)serverStructLength );
-#else //Win32
- int bindResult = bind( (unsigned)emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, serverStructLength );
-#endif
-
-
- if( bindResult < 0 )
- {
- std::cout << "Error id : " << bindResult << std::endl;
- std::string errorMessage( "vpServer::vpServer(), cannot bind to port" );
- char posrtNumberString[32];
- sprintf( posrtNumberString, "%d", port );
- errorMessage += " ";
- errorMessage += posrtNumberString;
- errorMessage += " The port may be already used.";
- vpERROR_TRACE( errorMessage.c_str() );
- return false;
- }
-
-#ifdef SO_NOSIGPIPE
- // Mac OS X does not have the MSG_NOSIGNAL flag. It does have this
- // connections based version, however.
- if (emitter.socketFileDescriptorEmitter > 0) {
- int set_option = 1;
- if (0 == setsockopt(emitter.socketFileDescriptorEmitter, SOL_SOCKET, SO_NOSIGPIPE, &set_option, sizeof(set_option))) {
- } else {
- std::cout << "Failed to set socket signal option" << std::endl;
- }
- }
-#endif // SO_NOSIGPIPE
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- listen( emitter.socketFileDescriptorEmitter, (int)max_clients );
-#else //Win32
- listen( (unsigned)emitter.socketFileDescriptorEmitter, (int)max_clients );
-#endif
-
- std::cout << "Server ready" << std::endl;
-
- started = true;
-
- return true;
-}
-
-/*!
- Check if a client has connected or deconnected the server
-
- \return True if a client connected or deconnected, false otherwise OR server not started yet.
-*/
-bool vpServer::checkForConnections()
-{
- if(!started)
- if(!start()){
- return false;
- }
-
- tv.tv_sec = tv_sec;
- tv.tv_usec = tv_usec;
-
- FD_ZERO(&readFileDescriptor);
-
- socketMax = emitter.socketFileDescriptorEmitter;
- FD_SET((unsigned)emitter.socketFileDescriptorEmitter,&readFileDescriptor);
-
- for(unsigned int i=0; i<receptor_list.size(); i++){
- FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
-
- if(i == 0)
- socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
- }
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- if(value == -1){
- //vpERROR_TRACE( "vpServer::run(), select()" );
- return false;
- }
- else if(value == 0){
- return false;
- }
- else{
- if(FD_ISSET((unsigned int)emitter.socketFileDescriptorEmitter,&readFileDescriptor)){
- vpNetwork::vpReceptor client;
- client.receptorAddressSize = sizeof(client.receptorAddress);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- client.socketFileDescriptorReceptor = accept(emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
-#else //Win32
- client.socketFileDescriptorReceptor = accept((unsigned int)emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
-#endif
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if((client.socketFileDescriptorReceptor) == -1)
-#else
- if((client.socketFileDescriptorReceptor) == INVALID_SOCKET)
-#endif
- vpERROR_TRACE( "vpServer::run(), accept()" );
-
- client.receptorIP = inet_ntoa(client.receptorAddress.sin_addr);
- printf("New client connected : %s\n", inet_ntoa(client.receptorAddress.sin_addr));
- receptor_list.push_back(client);
-
- return true;
- }
- else{
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
- char deco;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int numbytes = recv(receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
-#else //Win32
- int numbytes = recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
-#endif
-
-
- if(numbytes == 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
- return 0;
- }
- }
- }
- }
- }
-
- return false;
-}
-
-/*!
- Print the connected clients.
-*/
-void vpServer::print()
-{
- vpNetwork::print("Client");
-}
-
diff --git a/src/network/vpServer.h b/src/network/vpServer.h
deleted file mode 100644
index b1873e2..0000000
--- a/src/network/vpServer.h
+++ /dev/null
@@ -1,225 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * TCP Server
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#ifndef vpServer_H
-#define vpServer_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpNetwork.h>
-
-
-/*!
- \class vpServer
-
- \ingroup Network
-
- \brief This class represents a Transmission Control Protocol (TCP) server.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
- on one computer to another program on another computer.
-
- Exemple of server's code, receiving and sending basic message.
- It corresponds to the client used in the first exemple of vpClient class'
- documentation.
-
- \code
-#include <visp/vpServer.h>
-#include <iostream>
-
-int main(int argc,const char** argv)
-{
- int port = 35000;
- vpServer serv(port); //Launch the server on localhost
- serv.start();
-
- bool run = true;
- int val;
-
- while(run){
- serv.checkForConnections();
-
- if(serv.getNumberOfClients() > 0)
- {
- if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
- else
- std::cout << "Received : " << val << std::endl;
-
- val = val+1;
- if(serv.send(&val) != sizeof(int)) //Sending the new value to the first client
- std::cout << "Error while sending" << std::endl;
- else
- std::cout << "Sending : " << val << std::endl;
- }
- }
-
- return 0;
-}
- \endcode
-
- Exemple of server's code, receiving a vpImage on request form.
- It correspond to the client used in the second exemple of vpClient class' documentation.
-
- \code
-#include <visp/vpServer.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-
-#include "vpRequestImage.h" //See vpRequest class documentation
-
-int main(int argc,const char** argv)
-{
- int port = 35000;
-
- std::cout << "Port: " << port << std::endl;
- vpServer serv(port);
- serv.start();
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX display;
-#elif defined(VISP_HAVE_GDI) //Win32
- vpDisplayGDI display;
-#endif
-
- vpImage<unsigned char> I;
-
- vpRequestImage reqImage(&I);
- serv.addDecodingRequest(&reqImage);
-
- bool run = true;
-
- while(run){
- serv.checkForConnections();
-
- if(serv.getNumberOfClients() > 0)
- {
- int index = serv.receiveAndDecodeRequestOnce();
- std::string id = serv.getRequestIdFromIndex(index);
-
- if(id == reqImage.getId())
- {
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
- if (! display.isInitialised() )
- display.init(I, -1, -1, "Remote display");
-#endif
-
- vpDisplay::display(I) ;
- vpDisplay::flush(I);
-
- // A click in the viewer to exit
- if ( vpDisplay::getClick(I, false) )
- run = false;
- }
- }
- }
-
- return 0;
-}
- \endcode
-
- \sa vpClient
- \sa vpRequest
- \sa vpNetwork
-*/
-class VISP_EXPORT vpServer : public vpNetwork
-{
-private:
-
- //######## PARAMETERS ########
- //# #
- //############################
- std::string adress;
- int port;
- bool started;
- unsigned int max_clients;
-
-public:
-
- vpServer();
- vpServer(const int &port);
- vpServer(const std::string &adress_serv,const int &port_serv);
-
- virtual ~vpServer();
-
- bool checkForConnections();
-
- /*!
- Check if the server is started.
-
- \sa vpServer::start()
-
- \return True if the server is started, false otherwise.
- */
- bool isStarted(){ return started; }
-
- /*!
- Get the maximum number of clients that can be connected to the server.
-
- \sa vpServer::setMaxNumberOfClients()
-
- \return Maximum number of clients.
- */
- unsigned int getMaxNumberOfClients(){ return max_clients; }
-
- /*!
- Get the number of clients connected to the server.
-
- \return Number of clients connected.
- */
- unsigned int getNumberOfClients(){ return (unsigned int)receptor_list.size(); }
-
- void print();
-
- bool start();
-
- /*!
- Set the maximum number of clients that can be connected to the server.
-
- \sa vpServer::getMaxNumberOfClients()
-
- \param l : Maximum number of clients.
- */
- void setMaxNumberOfClients(unsigned int &l){ max_clients = l; }
-};
-
-#endif
diff --git a/src/robot/real-robot/afma4/vpAfma4.cpp b/src/robot/real-robot/afma4/vpAfma4.cpp
deleted file mode 100644
index 32ee3c9..0000000
--- a/src/robot/real-robot/afma4/vpAfma4.cpp
+++ /dev/null
@@ -1,650 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAfma4.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpAfma4.cpp
-
- Control of Irisa's cylindrical robot named Afma4.
-
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpAfma4.h>
-
-
-/* ----------------------------------------------------------------------- */
-/* --- STATIC ------------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-const unsigned int vpAfma4::njoint = 4;
-
-
-/*!
-
- Default constructor.
-
-*/
-vpAfma4::vpAfma4()
- : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
-{
- // Set the default parameters in case of the config files on the NAS
- // at Inria are not available.
-
- //
- // Geometric model constant parameters
- //
- this->_a1 = 0.205; // distance along x2
- this->_d3 = 0.403; // distance along z2
- this->_d4 = 0.14; // distance along z3
-
- // Maximal value of the joints
- this->_joint_max[0] = 1.8; // rad
- this->_joint_max[1] = 0.9; // meter
- this->_joint_max[2] = 0.9; // rad
- this->_joint_max[3] = 0.76; // rad
- // Minimal value of the joints
- this->_joint_min[0] = -1.5; // rad
- this->_joint_min[1] = -0.9; // meter
- this->_joint_min[2] = -3.5; // rad
- this->_joint_min[3] = -0.76;// rad
-
- // Camera extrinsic parameters: effector to camera frame
- this->_etc[0] = 0.; // Translation
- this->_etc[1] = 0.;
- this->_etc[2] = 0.;
- this->_erc[0] = 0.; // Rotation
- this->_erc[1] = -M_PI/2.;
- this->_erc[2] = 0;
-
- vpRotationMatrix eRc(_erc);
- this->_eMc.buildFrom(_etc, eRc);
-
- init();
-}
-
-
-/*!
-
- Does nothing for the moment.
- */
-void
-vpAfma4::init (void)
-{
- return;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
-
- This method is the same than get_fMc(const vpColVector & q).
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \return The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- camera frame (\f${^f}M_c\f$) with:
- \f[
- {^f}M_c = {^f}M_e * {^e}M_c
- \f]
-
- \sa get_fMc(const vpColVector & q)
- \sa getInverseKinematics()
-
-*/
-vpHomogeneousMatrix
-vpAfma4::getForwardKinematics(const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- fMc = get_fMc(q);
-
- return fMc;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
-
- This method is the same than getForwardKinematics(const vpColVector & q).
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \return The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- camera frame (\f${^f}M_c\f$) with:
- \f[
- {^f}M_c = {^f}M_e * {^e}M_c
- \f]
-
- \sa getForwardKinematics(const vpColVector & q)
-*/
-vpHomogeneousMatrix
-vpAfma4::get_fMc (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- get_fMc(q, fMc);
-
- return fMc;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param fMc : The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- camera frame (\f${^f}M_c\f$) with:
- \f[
- {^f}M_c = {^f}M_e * {^e}M_c
- \f]
-
-*/
-void
-vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
-{
-
- // Compute the direct geometric model: fMe = transformation between
- // fix and end effector frame.
- vpHomogeneousMatrix fMe;
-
- get_fMe(q, fMe);
-
- fMc = fMe * this->_eMc;
-
- return;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the end effector with respect to the base frame given the
- articular positions of all the four variable joints.
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param fMe The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- end effector frame (\f${^f}M_e\f$) with
-
- \f[
- {^f}M_e = \left[\begin{array}{cccc}
- c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
- s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
- -s_5 & -c_5 & d_4+q_2 \\
- 0 & 0 & 0 & 1 \\
- \end{array}
- \right]
- \f]
-
-*/
-void
-vpAfma4::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
-{
- double q1 = q[0]; // rot touret
- double q2 = q[1]; // vertical translation
- double q4 = q[2]; // pan
- double q5 = q[3]; // tilt
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
-
- /* Calcul du modele d'apres les angles. */
- fMe[0][0] = c1*s4*c5 + s1*c4*c5;
- fMe[0][1] = -c1*s4*s5 - s1*c4*s5;
- fMe[0][2] = c1*c4 - s1*s4;
- fMe[0][3] = c1*this->_a1 - s1*(this->_d3);
-
- fMe[1][0] = s1*s4*c5 - c1*c4*c5;
- fMe[1][1] = -s1*s4*s5 + c1*c4*s5;
- fMe[1][2] = s1*c4+c1*s4;
- fMe[1][3] = s1*this->_a1 + c1*(this->_d3);
-
- fMe[2][0] = -s5;
- fMe[2][1] = -c5;
- fMe[2][2] = 0.f;
- fMe[2][3] = this->_d4 + q2;
-
- fMe[3][0] = 0.f;
- fMe[3][1] = 0.f;
- fMe[3][2] = 0.f;
- fMe[3][3] = 1;
-
- // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
-
- return;
-}
-
-/*!
-
- Get the geometric transformation between the camera frame and the
- end-effector frame. This transformation is constant and correspond
- to the extrinsic camera parameters estimated by hand or by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
-vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- cMe = this->_eMc.inverse();
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
-vpAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-
- return;
-}
-
-/*!
-
- Get the twist transformation from camera frame to the reference
- frame. This transformation allows to compute a velocity expressed
- in the reference frame into the camera frame.
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param cVf : Twist transformation.
-
-*/
-void
-vpAfma4::get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const
-{
- vpHomogeneousMatrix fMc, cMf ;
- get_fMc(q, fMc) ;
- cMf = fMc.inverse();
-
- cVf.buildFrom(cMf) ;
-
- return;
-}
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame:
-
- \f[
- {^e}J_e = \left[\begin{array}{cccc}
- -c_5(a_1c_4+d_3s_4) & -s_5 & 0 & 0 \\
- s_5(a_1c_4+d_3s_4) & -c_5 & 0 & 0 \\
- a_1s_4-d_3c_4 & 0 & 0 & 0 \\
- -s_5 & 0 & -s_5 & 0 \\
- -c_5 & 0 & -c_5 & 0 \\
- 0 & 0 & 0 & 1 \\
- \end{array}
- \right]
- \f]
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param eJe : Robot jacobian expressed in the end-effector frame, with:
- \f[
- {^e}J_e = \left[\begin{array}{cc}
- {^f}R_e^T & 0_{3 \times 3} \\
- 0_{3 \times 3} & {^f}R_e^T \\
- \end{array}
- \right] {^f}J_e
- \f]
-
- \sa get_fJe()
-*/
-void
-vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
-{
- double q4 = q[2]; // pan
- double q5 = q[3]; // tilt
-
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
-
- eJe.resize(6, 4);
-
- eJe = 0;
-
- eJe[0][0] = -(this->_a1*c4 + this->_d3*s4)*c5; eJe[0][1] = -s5;
- eJe[1][0] = (this->_a1*c4 + this->_d3*s4)*s5; eJe[1][1] = -c5;
- eJe[2][0] = (this->_a1*s4 - this->_d3*c4);
- eJe[3][0] = eJe[3][2] = -s5;
- eJe[4][0] = eJe[4][2] = -c5;
- eJe[5][3] = 1.;
-}
-
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame:
-
- \f[
- {^f}J_e = \left[\begin{array}{cccc}
- -a_1s_1-d_3c_1 & 0 & 0 & 0 \\
- a_1c_1-d_3s_1 & 0 & 0 & 0 \\
- 0 & 1 & 0 & 0 \\
- 0 & 0 & 0 & c_{14} \\
- 0 & 0 & 0 & s_{14} \\
- 1 & 0 & 1 & 0 \\
- \end{array}
- \right]
- \f]
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param fJe : Robot jacobian expressed in the robot reference frame.
-
- \sa get_eJe() and get_fJe_inverse()
-*/
-
-void
-vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
-{
- fJe.resize(6,4) ;
-
- double q1 = q[0]; // rot touret
- double q4 = q[2]; // pan
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c14 = cos(q1 + q4);
- double s14 = sin(q1 + q4);
-
- fJe = 0;
-
- fJe[0][0] = -s1*this->_a1 - c1*this->_d3;
-
- fJe[1][0] = c1*this->_a1 - s1*this->_d3;
-
- fJe[2][1] = 1.0;
-
- fJe[3][3] = c14;
-
- fJe[4][3] = s14;
-
- fJe[5][0] = fJe[5][2] = 1.0;
-}
-
-/*!
-
- Get the inverse jacobian.
-
- \f[
- {^f}J_e^+ = \left[\begin{array}{cccccc}
- -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\
- 0 & 0 & 1 & 0 & 0 & 0 \\
- (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\
- 0 & 0 & 0 & c_{14} & s_{14} & 0 \\
- \end{array}
- \right]
- \f]
-
- \param q : Articular position of the four joints: q[0] corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q[1] corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
- q[3] are expressed in radians. The translation q[1] is expressed in
- meters.
-
- \param fJe_inverse : Inverse robot jacobian expressed in the robot
- reference frame.
-
- \sa get_eJe() and get_fJe()
-
-*/
-void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
-{
- fJe_inverse.resize(4, 6) ;
- fJe_inverse = 0;
-
- double q1 = q[0]; // rot touret
- double q4 = q[2]; // pan
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c14 = cos(q1 + q4);
- double s14 = sin(q1 + q4);
-
- double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
-
- fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
- fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;
-
- fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
-
- fJe_inverse[2][0] = - fJe_inverse[0][0];
- fJe_inverse[2][1] = - fJe_inverse[0][1];
-
- fJe_inverse[3][3] = c14;
- fJe_inverse[3][4] = s14;
-}
-
-/*!
- Get min joint values.
-
- \return Minimal joint values for the 4 dof
- X, Y, A, B. Translation Y is expressed in meters. Rotations
- X,A and B in radians.
-
-*/
-vpColVector
-vpAfma4::getJointMin() const
-{
- vpColVector qmin(4);
- for (unsigned int i=0; i < 4; i ++)
- qmin[i] = this->_joint_min[i];
- return qmin;
-}
-
-/*!
- Get max joint values.
-
- \return Maximal joint values for the 4 dof
- X, Y, A, B. Translation Y is expressed in meters. Rotations
- X, A and B in radians.
-
-*/
-vpColVector
-vpAfma4::getJointMax() const
-{
- vpColVector qmax(4);
- for (unsigned int i=0; i < 4; i ++)
- qmax[i] = this->_joint_max[i];
- return qmax;
-}
-
-
-
-/*!
-
- Print on the output stream \e os the robot parameters (joint
- min/max, distance between axis 5 and 6, coupling factor between axis
- 5 and 6, hand-to-eye homogeneous matrix.
-
- \param os : Output stream.
- \param afma4 : Robot parameters.
-*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os,
- const vpAfma4 & afma4)
-{
- vpRotationMatrix eRc;
- afma4._eMc.extract(eRc);
- vpRxyzVector rxyz(eRc);
-
- os
- << "Joint Max:" << std::endl
- << "\t" << afma4._joint_max[0]
- << "\t" << afma4._joint_max[1]
- << "\t" << afma4._joint_max[2]
- << "\t" << afma4._joint_max[3]
- << "\t" << std::endl
-
- << "Joint Min: " << std::endl
- << "\t" << afma4._joint_min[0]
- << "\t" << afma4._joint_min[1]
- << "\t" << afma4._joint_min[2]
- << "\t" << afma4._joint_min[3]
- << "\t" << std::endl
-
- << "a1: " << std::endl
- << "\t" << afma4._a1
- << "\t" << std::endl
-
- << "d3: " << std::endl
- << "\t" << afma4._d3
- << "\t" << std::endl
-
- << "d4: " << std::endl
- << "\t" << afma4._d4
- << "\t" << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << afma4._eMc[0][3] << " "
- << afma4._eMc[1][3] << " "
- << afma4._eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
-
- return os;
-}
diff --git a/src/robot/real-robot/afma4/vpAfma4.h b/src/robot/real-robot/afma4/vpAfma4.h
deleted file mode 100644
index 376314a..0000000
--- a/src/robot/real-robot/afma4/vpAfma4.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAfma4.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpAfma4_h
-#define __vpAfma4_h
-
-/*!
-
- \file vpAfma4.h
-
- Modelisation of Irisa's cylindrical robot named Afma4.
-
-*/
-
-/*!
-
- \class vpAfma4
-
- \ingroup Afma4
-
- \brief Modelisation of Irisa's cylindrical robot named Afma4.
-
- This robot has five degrees of freedom, but only four motorized
- joints (joint 3 is not motorized). Joint 2 and 3 are prismatic. The
- other ones are revolute joints.
-
- The non modified Denavit-Hartenberg representation of the robot is
- given in the table below, where \f$q_1^*, q_2^*,q_4^*, q_5^*\f$
- are the variable joint positions.
-
- \f[
- \begin{tabular}{|c|c|c|c|c|}
- \hline
- Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
- \hline
- 1 & 0 & 0 & 0 & $q_1^*$ \\
- 2 & $a_1$ & $q_2^*$ & $-\pi/2$ & 0 \\
- 3 & 0 & $d_3$ & $\pi/2$ & 0 \\
- 4 & 0 & $d_4$ & $-\pi/2$ & $q_4^*-\pi/2$ \\
- 5 & 0 & 0 & 0 & $q_5^*$ \\
- \hline
- \end{tabular}
- \f]
-
- The forward kinematics of the robot is given by the homogeneous
- matrix \f${^f}M_e\f$ which is implemented in get_fMe().
-
- \f[
- {^f}M_e = \left[\begin{array}{cccc}
- c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
- s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
- -s_5 & -c_5 & d_4+q_2 \\
- 0 & 0 & 0 & 1 \\
- \end{array}
- \right]
- \f]
-
- The robot forward jacobian used to compute the cartesian velocities
- from joint ones is given and implemented in get_fJe() and
- get_eJe().
-
- The robot inverse jacobian used to compute the joint velocities from
- cartesian ones are given and implemented in get_fJe_inverse().
-
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-
-class VISP_EXPORT vpAfma4
-{
- public:
- vpAfma4();
- /*! Destructor that does nothing. */
- virtual ~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;
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const;
- void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
- void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
- void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const;
-
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpAfma4 & afma4);
-
- vpColVector getJointMin() const;
- vpColVector getJointMax() const;
-
- public:
-
- static const unsigned int njoint; ///< Number of joint.
-
- protected:
- // Denavit Hartenberg parameters
- double _a1; // distance along x2
- double _d3; // distance along z2
- double _d4; // distance along z3
- double _joint_max[4]; // Maximal value of the joints
- double _joint_min[4]; // Minimal value of the joints
-
- // Minimal representation of _eMc
- vpTranslationVector _etc; // meters
- vpRxyzVector _erc; // radian
-
- vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
-};
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/afma4/vpRobotAfma4.cpp b/src/robot/real-robot/afma4/vpRobotAfma4.cpp
deleted file mode 100644
index 731a357..0000000
--- a/src/robot/real-robot/afma4/vpRobotAfma4.cpp
+++ /dev/null
@@ -1,1868 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotAfma4.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_AFMA4
-
-#include <signal.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <visp/vpRobotException.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpRobotAfma4.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ----------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-bool vpRobotAfma4::robotAlreadyCreated = false;
-
-/*!
-
- Default positioning velocity in percentage of the maximum
- velocity. This value is set to 15. The member function
- setPositioningVelocity() allows to change this value.
-
-*/
-const double vpRobotAfma4::defaultPositioningVelocity = 15.0;
-
-
-/* ---------------------------------------------------------------------- */
-/* --- EMERGENCY STOP --------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Emergency stops the robot if the program is interrupted by a SIGINT
- (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
- or SIGQUIT signal.
-
-*/
-void emergencyStopAfma4(int signo)
-{
- std::cout << "Stop the Afma4 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
- }
- // std::cout << "Emergency stop called\n";
- // PrimitiveESTOP_Afma4();
- PrimitiveSTOP_Afma4();
- std::cout << "Robot was stopped\n";
-
- // Free allocated resources
- // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
-
- fprintf(stdout, "Application ");
- fflush(stdout);
- kill(getpid(), SIGKILL);
- exit(1) ;
-}
-
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- The only available constructor.
-
- This contructor calls init() to initialise the connection with the MotionBox
- or low level controller, power on the robot and wait 1 sec before returning
- to be sure the initialisation is done.
-
- It also set the robot state to vpRobot::STATE_STOP.
-
-*/
-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
- */
-
- signal(SIGINT, emergencyStopAfma4);
- signal(SIGBUS, emergencyStopAfma4) ;
- signal(SIGSEGV, emergencyStopAfma4) ;
- signal(SIGKILL, emergencyStopAfma4);
- signal(SIGQUIT, emergencyStopAfma4);
-
- setVerbose(verbose);
- if (verbose_)
- std::cout << "Open communication with MotionBlox.\n";
- try {
- this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
- // vpERROR_TRACE("Error caught") ;
- throw ;
- }
- positioningVelocity = defaultPositioningVelocity ;
-
- vpRobotAfma4::robotAlreadyCreated = true;
-
- return ;
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INITIALISATION ----------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Initialise the connection with the MotionBox or low level
- controller, power on the
- robot and wait 1 sec before returning to be sure the initialisation
- is done.
-
-*/
-void
-vpRobotAfma4::init (void)
-{
- int stt;
- InitTry;
-
- // Initialise private variables used to compute the measured velocities
- q_prev_getvel.resize(4);
- q_prev_getvel = 0;
- time_prev_getvel = 0;
- first_time_getvel = true;
-
- // Initialise private variables used to compute the measured displacement
- q_prev_getdis.resize(4);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
- // Initialize the firewire connection
- Try( stt = InitializeConnection(verbose_) );
-
- if (stt != SUCCESS) {
- vpERROR_TRACE ("Cannot open connexion with the motionblox.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
-
- // Connect to the servoboard using the servo board GUID
- Try( stt = InitializeNode_Afma4() );
-
- if (stt != SUCCESS) {
- vpERROR_TRACE ("Cannot open connexion with the motionblox.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
- Try( PrimitiveRESET_Afma4() );
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // Print the robot status
- if (verbose_) {
- std::cout << "Robot status: ";
- switch(EStopStatus) {
- case ESTOP_AUTO:
- case ESTOP_MANUAL:
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
- case ESTOP_ACTIVATED:
- std::cout << "Emergency stop is activated" << std::endl;
- break;
- default:
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- }
- std::cout << std::endl;
- }
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
-
- // If an error occur in the low level controller, goto here
- // CatchPrint();
- Catch();
-
- // Test if an error occurs
- if (TryStt == -20001)
- printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
- else if (TryStt == -675)
- printf(" Timeout enabling power...\n");
-
- if (TryStt < 0) {
- // Power off the robot
- PrimitivePOWEROFF_Afma4();
- // Free allocated resources
- ShutDownConnection();
-
- std::cout << "Cannot open connexion with the motionblox..." << std::endl;
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
- return ;
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- DESTRUCTOR --------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Destructor.
-
- Free allocated resources.
-*/
-vpRobotAfma4::~vpRobotAfma4 (void)
-{
- InitTry;
-
- setRobotState(vpRobot::STATE_STOP) ;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
-// if (HIPowerStatus == 1) {
-// fprintf(stdout, "Power OFF the robot\n");
-// fflush(stdout);
-
-// Try( PrimitivePOWEROFF_Afma4() );
-// }
-
- // Free allocated resources
- ShutDownConnection();
-
- vpRobotAfma4::robotAlreadyCreated = false;
-
- CatchPrint();
- return;
-}
-
-
-
-
-
-/*!
-
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
-vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState)
-{
- InitTry;
-
- switch (newState) {
- case vpRobot::STATE_STOP: {
- if (vpRobot::STATE_STOP != getRobotState ()) {
- Try( PrimitiveSTOP_Afma4() );
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Afma4() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
- }
- default:
- break ;
- }
-
- CatchPrint();
-
- return vpRobot::setRobotState (newState);
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- STOP --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Stop the robot and set the robot state to vpRobot::STATE_STOP.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-*/
-void
-vpRobotAfma4::stopMotion(void)
-{
- InitTry;
- Try( PrimitiveSTOP_Afma4() );
- setRobotState (vpRobot::STATE_STOP);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
- }
-}
-
-
-/*!
-
- Power on the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot power on.
-
- \sa powerOff(), getPowerState()
-*/
-void
-vpRobotAfma4::powerOn(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- bool firsttime = true;
- unsigned int nitermax = 10;
-
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- if (EStopStatus == ESTOP_AUTO) {
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
- if (firsttime) {
- std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
- firsttime = false;
- }
- fprintf(stdout, "Remaining time %ds \r", nitermax-i);
- fflush(stdout);
- CAL_Wait(1);
- }
- else {
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- ShutDownConnection();
- exit(0);
- }
- }
-
- if (EStopStatus == ESTOP_ACTIVATED)
- std::cout << std::endl;
-
- if (EStopStatus == ESTOP_ACTIVATED) {
- std::cout << "Sorry, cannot power on the robot." << std::endl;
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power on the robot.");
- }
-
- if (HIPowerStatus == 0) {
- fprintf(stdout, "Power ON the Afma4 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWERON_Afma4() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Power off the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-
- \sa powerOn(), getPowerState()
-*/
-void
-vpRobotAfma4::powerOff(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- fprintf(stdout, "Power OFF the Afma4 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWEROFF_Afma4() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Get the robot power state indication if power is on or off.
-
- \return true if power is on. false if power is off
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error.
-
- \sa powerOn(), powerOff()
-*/
-bool
-vpRobotAfma4::getPowerState(void)
-{
- InitTry;
- bool status = false;
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- status = true;
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
- }
- return status;
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
-vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- vpAfma4::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the twist transformation from camera frame to the reference
- frame. This transformation allows to compute a velocity expressed
- in the reference frame into the camera frame.
-
- \param cVf : Twist transformation.
-
-*/
-void
-vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const
-{
- double position[this->njoint];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(this->njoint);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try {
- vpAfma4::get_cVf(q, cVf) ;
- }
- catch(...) {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }}
-
-/*!
-
- Get the geometric transformation between the camera frame and the
- end-effector frame. This transformation is constant and correspond
- to the extrinsic camera parameters estimated by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
-vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpAfma4::get_cMe(cMe) ;
-}
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame. To have
- acces to the analytic form of this jacobian see vpAfma4::get_eJe().
-
- To compute eJe, we communicate with the low level controller to get
- the articular joint position of the robot.
-
- \param eJe : Robot jacobian expressed in the end-effector frame.
-
- \sa vpAfma4::get_eJe()
-*/
-void
-vpRobotAfma4::get_eJe(vpMatrix &eJe)
-{
-
- double position[this->njoint];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(this->njoint);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try {
- vpAfma4::get_eJe(q, eJe) ;
- }
- catch(...) {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame. To have acces to the analytic form of this
- jacobian see vpAfma4::get_fJe().
-
- To compute fJe, we communicate with the low level controller to get
- the articular joint position of the robot.
-
- \param fJe : Robot jacobian expressed in the reference frame.
-
- \sa vpAfma4::get_fJe()
-*/
-
-void vpRobotAfma4::get_fJe(vpMatrix &fJe)
-{
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try {
- vpAfma4::get_fJe(q, fJe) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-/*!
-
- Set the maximal velocity percentage to use for a position control.
-
- The default positioning velocity is defined by
- vpRobotAfma4::defaultPositioningVelocity. This method allows to
- change this default positioning velocity
-
- \param velocity : Percentage of the maximal velocity. Values should
- be in ]0:100].
-
- \code
- vpColVector q[4]);
- q = 0; // position in meter and rad
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot to the joint position [0,0,0,0]
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- \sa getPositioningVelocity()
-*/
-void
-vpRobotAfma4::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
-
-/*!
- Get the maximal velocity percentage used for a position control.
-
- \sa setPositioningVelocity()
-*/
-double
-vpRobotAfma4::getPositioningVelocity (void)
-{
- return positioningVelocity;
-}
-
-
-/*!
-
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
-
- \warning The position to reach can only be specified in joint space
- coordinates.
-
- This method owerloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param position : The joint positions to reach. position[0]
- corresponds to the first rotation of the turret around the vertical
- axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
- to the vertical translation (joint 2 with value \f$q_2\f$), while
- position[2] and position[3] correspond to the pan and tilt of the
- camera (respectively joint 4 and 5 with values \f$q_4\f$ and
- \f$q_5\f$). Rotations position[0], position[2] and position[3] are
- expressed in radians. The translation q[1] is expressed in meters.
-
- \param frame : Frame in which the position is expressed.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
- // Set positions in the joint space
- vpColVector q[4];
- double q[0] = M_PI/8; // Joint 1, in radian
- double q[1] = 0.2; // Joint 2, in meter
- double q[2] = M_PI/4; // Joint 4, in radian
- double q[3] = M_PI/8; // Joint 5, in radian
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- \exception vpRobotException::lowLevelError : If the requested frame
- (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, or vpRobot::MIXT_FRAME) are
- requested since they are not implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, q);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- \endcode
-
-*/
-
-void
-vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & position )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- int error = 0;
-
- switch(frame) {
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Positionning error. Reference frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Reference frame not implemented.");
- break ;
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Positionning error. Camera frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Camera frame not implemented.");
- break ;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
-
- case vpRobot::ARTICULAR_FRAME: {
- break ;
-
- }
- }
- if (position.getRows() != this->njoint) {
- vpERROR_TRACE ("Positionning error: bad vector dimension.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Positionning error: bad vector dimension.");
- }
-
- InitTry;
-
- Try( PrimitiveMOVE_Afma4(position.data, positioningVelocity) );
- Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
-
- CatchPrint();
- if (TryStt == InvalidPosition || TryStt == -1023)
- std::cout << " : Position out of range.\n";
- else if (TryStt < 0)
- std::cout << " : Unknown error (see Fabien).\n";
- else if (error == -1)
- std::cout << "Position out of range.\n";
-
- if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
-
- return ;
-}
-
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
-
- \warning The position to reach can only be specified in joint space
- coordinates.
-
- This method owerloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param q1, q2, q4, q5 : The four joint positions to reach. q1 corresponds to
- the first rotation (joint 1 with value \f$q_1\f$) of the turret
- around the vertical axis, while q2 corresponds to the vertical
- translation (joint 2 with value \f$q_2\f$), while q4 and q5
- correspond to the pan and tilt of the camera (respectively joint 4
- and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q1, q4 and
- q5 are expressed in radians. The translation q2 is expressed in
- meters.
-
- \param frame : Frame in which the position is expressed.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
- // Set positions in the camera frame
- double q1 = M_PI/8; // Joint 1, in radian
- double q2 = 0.2; // Joint 2, in meter
- double q4 = M_PI/4; // Joint 4, in radian
- double q5 = M_PI/8; // Joint 5, in radian
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q1, q2, q4, q5);
- \endcode
-
- \sa setPosition()
-*/
-void vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
- const double q1,
- const double q2,
- const double q4,
- const double q5)
-{
- try {
- vpColVector position(this->njoint) ;
- position[0] = q1 ;
- position[1] = q2 ;
- position[2] = q4 ;
- position[3] = q5 ;
-
- setPosition(frame, position) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-
-
-
-/*!
-
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
- vpColVector q;
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL)
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-
-*/
-void vpRobotAfma4::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
- Returns the robot controller current time (in second) since last robot power on.
-*/
-double vpRobotAfma4::getTime() const
-{
- double timestamp;
- PrimitiveACQ_TIME_Afma4(×tamp);
- return timestamp;
-}
-
-/*!
-
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 4 dimension vector corresponding to the joint position of
- each dof. position[0]
- corresponds to the first rotation of the turret around the vertical
- axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
- to the vertical translation (joint 2 with value \f$q_2\f$), while
- position[2] and position[3] correspond to the pan and tilt of the
- camera (respectively joint 4 and 5 with values \f$q_4\f$ and
- \f$q_5\f$). Rotations position[0], position[2] and position[3] are
- expressed in radians. The translation q[1] is expressed in meters.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Time in second since last robot power on.
-
- \code
- vpRobotAfma4 robot;
- vpColVector r;
- robot.getPosition(vpRobot::REFERENCE_FRAME, r);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame pose in terms of a homogenous matrix
- vpHomogeneousMatrix fMc(fRc, ftc);
- \endcode
-
- \exception vpRobotException::lowLevelError : If the position cannot
- be get from the low level controller.
-
- \sa setPosition(const vpRobot::vpControlFrameType frame, const
- vpColVector & r)
-
-*/
-void
-vpRobotAfma4::getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector & position, double ×tamp)
-{
-
- InitTry;
-
- position.resize (this->njoint);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- double _q[njoint];
- Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
- for (unsigned int i=0; i < this->njoint; i ++) {
- position[i] = _q[i];
- }
-
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- double _q[njoint];
- Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
-
- vpColVector q(this->njoint);
- for (unsigned int i=0; i < this->njoint; i++)
- q[i] = _q[i];
-
- // Compute fMc
- vpHomogeneousMatrix fMc;
- vpAfma4::get_fMc(q, fMc);
-
- // From fMc extract the pose
- vpRotationMatrix fRc;
- fMc.extract(fRc);
- vpRxyzVector rxyz;
- rxyz.buildFrom(fRc);
-
- for (unsigned int i=0; i < 3; i++) {
- position[i] = fMc[i][3]; // translation x,y,z
- position[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
- break ;
- }
- case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get position.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position.");
- }
-
- return;
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in the joint space or in the camera frame. The reference
- frame and mixt frame are not implemented.
-
- \param vel : Velocity vector. Translation velocities are expressed in m/s
- while rotation velocities in rad/s. The size of this vector may be 4 (the
- number of dof) if frame is set to vpRobot::ARTICULAR_FRAME. The size of this
- vector is 2 when the control is in the camera frame (frame is than set to
- vpRobot::CAMERA_FRAME).
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} t_x, ^{c} t_y, ^{c} t_z,^{c}
- \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t\f$ is expressed in the
- camera frame. Since only four dof are available, in camera frame we
- control control only the camera pan and tilt.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
- // Set joint velocities
- vpColVector q_dot(4);
- q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
- q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
- q_dot[3] = M_PI/8; // Joint 5 velocity, in rad/s
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
- \endcode
-*/
-void
-vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & vel)
-{
-
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- // Check the dimension of the velocity vector to see if it is
- // compatible with the requested frame
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- //if (vel.getRows() != 2) {
- if (vel.getRows() != 6) {
- vpERROR_TRACE ("Bad dimension of the velocity vector in camera frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension of the velocity vector "
- "in camera frame");
- }
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- if (vel.getRows() != this->njoint) {
- vpERROR_TRACE ("Bad dimension of the articular velocity vector");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension of the articular "
- "velocity vector ");
- }
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the mixt frame:"
- "functionality not implemented");
- break ;
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
- }
- }
-
-
- //
- // Velocities saturation with normalization
- //
- vpColVector joint_vel(this->njoint);
-
- // Case of the camera frame where we control only 2 dof
- if (frame == vpRobot::CAMERA_FRAME) {
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++) {
- vel_max[i] = getMaxTranslationVelocity();
- vel_max[i+3] = getMaxRotationVelocity();
- }
-
- vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
-
-#if 0 // ok
- vpMatrix eJe(4,6);
- eJe = 0;
- eJe[2][4] = -1;
- eJe[3][3] = 1;
-
- joint_vel = eJe * velocity; // Compute the articular velocity
-#endif
- vpColVector q;
- getPosition(vpRobot::ARTICULAR_FRAME, q);
- vpMatrix fJe_inverse;
- get_fJe_inverse(q, fJe_inverse);
- vpHomogeneousMatrix fMe;
- get_fMe(q, fMe);
- vpTranslationVector t;
- t=0;
- vpRotationMatrix fRe;
- fMe.extract(fRe);
- vpVelocityTwistMatrix fVe(t, fRe);
- // compute the inverse jacobian in the end-effector frame
- vpMatrix eJe_inverse = fJe_inverse * fVe;
-
- // Transform the velocities from camera to end-effector frame
- vpVelocityTwistMatrix eVc;
- eVc.buildFrom(this->_eMc);
- 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]);
- }
-
- // Case of the joint control where we control all the joints
- else if (frame == vpRobot::ARTICULAR_FRAME) {
-
- vpColVector vel_max(4);
-
- vel_max[0] = getMaxRotationVelocity();
- vel_max[1] = getMaxTranslationVelocity();
- vel_max[2] = getMaxRotationVelocity();
- vel_max[3] = getMaxRotationVelocity();
-
- joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
- }
-
- InitTry;
-
- // Send a joint velocity to the low level controller
- Try( PrimitiveMOVESPEED_Afma4(joint_vel.data) );
-
- Catch();
- if (TryStt < 0) {
- if (TryStt == VelStopOnJoint) {
- UInt32 axisInJoint[njoint];
- PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i])
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- }
- }
- else {
- printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
- if (TryString != NULL) {
- // The statement is in TryString, but we need to check the validity
- printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
- printf("\n");
- }
- }
- }
-
- return;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- GET ---------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param velocity : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Time in second since last robot power on.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \warning The first time this method is called, \e velocity is set to 0. The
- first call is used to intialise the velocity computation for the next call.
-
- \code
- // Set joint velocities
- vpColVector q_dot(4);
- q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
- q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
- q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- vpColVector q_dot_mes; // Measured velocities
-
- // Initialisation of the velocity measurement
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 4, the number of joint
-
- double timestamp;
- while (1) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
- }
- \endcode
-*/
-void
-vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp)
-{
-
- switch (frame) {
- case vpRobot::ARTICULAR_FRAME:
- velocity.resize (this->njoint);
- default:
- velocity.resize (6);
- }
-
- velocity = 0;
-
- double q[4];
- vpColVector q_cur(4);
- vpHomogeneousMatrix fMc_cur;
- vpHomogeneousMatrix cMc; // camera displacement
- double time_cur;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
- time_cur = timestamp;
-
- for (unsigned int i=0; i < this->njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- // Get the camera pose from the direct kinematics
- vpAfma4::get_fMc(q_cur, fMc_cur);
-
- if ( ! first_time_getvel ) {
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
-
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a displacement in the mixt frame:"
- "functionality not implemented");
-
- break ;
- }
- }
- }
- else {
- first_time_getvel = false;
- }
-
- // Memorize the camera pose for the next call
- fMc_prev_getvel = fMc_cur;
-
- // Memorize the joint position for the next call
- q_prev_getvel = q_cur;
-
- // Memorize the time associated to the joint position for the next call
- time_prev_getvel = time_cur;
-
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
-
- */
-void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
-{
- double timestamp;
- getVelocity(frame, velocity, timestamp);
-}
-
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Time in second since last robot power on.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
- // Set joint velocities
- vpColVector q_dot(4);
- q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
- q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
- q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
-
- vpRobotAfma4 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 4, the number of joint
-
- vpColVector q_dot_mes; // Measured velocities
- double timestamp;
- while (1) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
- }
- \endcode
-*/
-vpColVector
-vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- vpColVector velocity;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
-
- */
-vpColVector vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector velocity;
- double timestamp;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
-Read joint positions in a specific Afma4 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#AFMA4 - Position - Version 2.01
-# file: "myposition.pos "
-#
-# R: X Y A B
-# Joint position: X : rotation of the turret in degrees (joint 1)
-# Y : vertical translation in meters (joint 2)
-# A : pan rotation of the camera in degrees (joint 4)
-# B : tilt rotation of the camera in degrees (joint 5)
-#
-
-R: 45 0.3 -20 30
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : Joint positions: X,Y,A,B. Translations Y is
-expressed in meters, while joint rotations X,A,B in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-vpRobotAfma4 robot;
-vpColVector q; // Joint position
-robot.readPosFile("myposition.pos", q); // Set the joint position from the file
-robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
-robot.setPositioningVelocity(5); // Positioning velocity set to 5%
-robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-\endcode
-
-\sa savePosFile()
-*/
-
-bool
-vpRobotAfma4::readPosFile(const char *filename, vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
-// else
-// return (false); // fin fichier sans position robot.
- }
- }
- else {
- return (false); /* fin fichier */
- }
-
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(4);
- sscanf(line, "%s %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2], &q[3]);
-
- // converts rotations from degrees into radians
- q[0] = vpMath::rad(q[0]);
- q[2] = vpMath::rad(q[2]);
- q[3] = vpMath::rad(q[3]);
-
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Save joint (articular) positions in a specific Afma4 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : Joint positions: X,Y,A,B. Translations Y is
- expressed in meters, while joint rotations X,A,B in radians.
-
- \warning The joint rotations X,A,B written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-
-bool
-vpRobotAfma4::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#AFMA4 - Position - Version 2.01\n\
-#\n\
-# R: X Y A B\n\
-# Joint position: X : rotation of the turret in degrees (joint 1)\n\
-# Y : vertical translation in meters (joint 2)\n\
-# A : pan rotation of the camera in degrees (joint 4)\n\
-# B : tilt rotation of the camera in degrees (joint 5)\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- q[1],
- vpMath::deg(q[2]),
- vpMath::deg(q[3]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Moves the robot to the joint position specified in the filename. The
- positioning velocity is set to 10% of the robot maximal velocity.
-
- \param filename: File containing a joint position.
-
- \sa readPosFile
-
-*/
-void
-vpRobotAfma4::move(const char *filename)
-{
- vpColVector q;
-
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotAfma4::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param displacement : The measured articular displacement. The
- dimension of \e displacement is 6 (the number of axis of the
- robot). Translations are expressed in meters, rotations in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma4::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- cartesian space. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call of this
- method. The dimension of \e displacement is always 4, the number of
- joints. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
-
- double q[6];
- vpColVector q_cur(6);
- double timestamp;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- if ( ! first_time_getdis ) {
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/afma4/vpRobotAfma4.h b/src/robot/real-robot/afma4/vpRobotAfma4.h
deleted file mode 100644
index b7d19e0..0000000
--- a/src/robot/real-robot/afma4/vpRobotAfma4.h
+++ /dev/null
@@ -1,296 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotAfma4.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpRobotAfma4_h
-#define vpRobotAfma4_h
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_AFMA4
-
-#include <iostream>
-#include <stdio.h>
-
-#include <visp/vpRobot.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpAfma4.h>
-
-// low level controller api
-extern "C" {
-# include "irisa_Afma4.h"
-# include "trycatch.h"
-}
-
-
-/*!
- \class vpRobotAfma4
-
- \ingroup Afma4 RobotDriver
-
- \brief Control of Irisa's cylindrical robot named Afma4.
-
- Implementation of the vpRobot class in order to control Irisa's
- Afma4 robot. This robot is a cylindrical robot with five degrees of
- freedom but only four are actuated (see vpAfma4 documentation). It
- was manufactured in 1995 by the french Afma-Robots company. In 2008,
- the low level controller change for a more recent Adept technology
- based on the MotionBlox controller. A firewire camera is mounted on
- the end-effector to allow eye-in-hand visual servoing. The control
- of this camera is achieved by the vp1394TwoGrabber class. A
- Servolens lens is attached to the camera. It allows to control the
- focal lens, the iris and the focus throw a serial link. The control
- of the lens is possible using the vpServolens class.
-
- This class allows to control the Afma4 cylindrical robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
-
- Mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame is not implemented.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The Denavit-Hartenberg representation as well as the direct and
- inverse kinematics models are given and implemented in the vpAfma4
- class.
-
- \warning A Ctrl-C, a segmentation fault or other system errors are
- catched by this class to stop the robot.
-
- To communicate with the robot, you may first create an instance of this
- class by calling the default constructor:
-
- \code
- vpRobotAfma4 robot;
- \endcode
-
- This initialize the robot kinematics with the eMc extrinsic camera
- parameters.
-
- To control the robot in position, you may set the controller
- to position control and than send the position to reach in a specific
- frame like here in the joint space:
-
- \code
- vpColVector q(4);
- // Set a joint position
- q[0] = M_PI/2; // X axis, in radian
- q[1] = 0.2; // Y axis, in meter
- q[2] = -M_PI/2; // A axis, in radian
- q[3] = M_PI/8; // B axis, in radian
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- The robot moves to the specified position with the default
- positioning velocity vpRobotAfma4::defaultPositioningVelocity. The
- setPositioningVelocity() method allows to change the maximal
- velocity used to reach the desired position.
-
- \code
- // Set the max velocity to 40%
- robot.setPositioningVelocity(40);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and than send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = M_PI/8; // X axis, in rad/s
- qvel[1] = 0.2; // Y axis, in m/s
- qvel[2] = 0; // A axis, in rad/s
- qvel[3] = M_PI/8; // B axis, in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- while (...) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP)
- \endcode
-
- There is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and writePosFile()
- methods.
-*/
-class VISP_EXPORT vpRobotAfma4
- :
- public vpAfma4,
- public vpRobot
-{
-
-private: /* Not allowed functions. */
-
- /*!
- Copy constructor not allowed.
- */
- vpRobotAfma4 (const vpRobotAfma4 & robot);
-
-private: /* Attributs prives. */
-
- /** \brief Vrai ssi aucun objet de la classe vpRobotAfma4 n'existe.
- *
- * Il ne peut exister simultanement qu'un seul objet de la classe
- * vpRobotAfma4, car il correspond a un seul robot AFMA4. Creer
- * simultanement deux objets peut engendrer des conflits. Le constructeur
- * lance une erreur si le champ n'est pas FAUX puis positionne le champ
- * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
- * alors la creation d'un nouvel objet.
- */
- static bool robotAlreadyCreated;
-
- double positioningVelocity;
-
- // Variables used to compute the measured velocities (see getVelocity() )
- vpColVector q_prev_getvel;
- vpHomogeneousMatrix fMc_prev_getvel;
- double time_prev_getvel;
- bool first_time_getvel;
-
- // Variables used to compute the measured displacement (see
- // getDisplacement() )
- vpColVector q_prev_getdis;
- bool first_time_getdis;
-
-public: /* Constantes */
-
- /* Vitesse maximale par default lors du positionnement du robot.
- * C'est la valeur a la construction de l'attribut prive \a
- * positioningVelocity. Cette valeur peut etre changee par la fonction
- * #setPositioningVelocity.
- */
- static const double defaultPositioningVelocity; // = 20.0;
-
-public: /* Methode publiques */
-
- vpRobotAfma4 (bool verbose=true);
- virtual ~vpRobotAfma4 (void);
-
- void getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position, double ×tamp);
-
- double getPositioningVelocity (void);
- bool getPowerState();
-
- double getTime() const;
-
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
-
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_cVf(vpVelocityTwistMatrix &cVf) const;
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (void);
-
- void move(const char *filename) ;
-
- void powerOn() ;
- void powerOff() ;
-
- static bool readPosFile(const char *filename, vpColVector &q) ;
- static bool savePosFile(const char *filename, const vpColVector &q) ;
-
- /* --- POSITIONNEMENT --------------------------------------------------- */
- void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &position) ;
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double q1, const double q2,
- const double q4, const double q5) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
-
- /* --- ETAT ------------------------------------------------------------- */
-
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
-
- /* --- VITESSE ---------------------------------------------------------- */
-
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
-
- void stopMotion() ;
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
-};
-
-#endif
-#endif /* #ifndef vpRobotAfma4_h */
diff --git a/src/robot/real-robot/afma4/vpServolens.cpp b/src/robot/real-robot/afma4/vpServolens.cpp
deleted file mode 100644
index 1104be2..0000000
--- a/src/robot/real-robot/afma4/vpServolens.cpp
+++ /dev/null
@@ -1,899 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServolens.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Servolens lens attached to the camera fixed on the
- * Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpServolens.cpp
-
- Interface for the Servolens lens attached to the camera fixed on the
- Afma4 robot.
-
-*/
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-
-#include <unistd.h>
-#include <termios.h>
-#include <stdio.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <visp/vpServolens.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpTime.h>
-
-/*!
- Default constructor. Does nothing.
-
- \sa open()
-
-*/
-vpServolens::vpServolens() : remfd(0), isinit(false)
-{
-}
-
-/*!
- Open and initialize the Servolens serial link at 9600 bauds, 7
- data bits, even parity, 1 stop bit. The cariage return mode is not
- active, that means that each character is directly read without
- waitong for a cariage return.
-
- \sa open()
-*/
-vpServolens::vpServolens(const char *port) : remfd(0), isinit(false)
-{
- this->open(port);
-}
-
-/*!
- Destructor.
-
- Close the Servolens serial link.
-
- \sa close()
-*/
-vpServolens::~vpServolens()
-{
- this->close();
-}
-
-/*!
-
- Open and initialize the Servolens serial link at 9600 bauds, 7
- data bits, even parity, 1 stop bit. The cariage return mode is not
- active, that means that each character is directly read without
- waitong for a cariage return.
-
- \param port : Serial device like /dev/ttyS0 or /dev/ttya.
-
- \exception vpRobotException::communicationError : If cannot open
- Servolens serial port or intialize the serial link.
-
- \sa close()
-*/
-void
-vpServolens::open(const char *port)
-{
- if (! isinit) {
- struct termios info;
-
- printf("\nOpen the Servolens serial port \"%s\"\n", port);
-
- if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
- vpERROR_TRACE ("Cannot open Servolens serial port.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot open Servolens serial port.");
- }
-
- // Lecture des parametres courants de la liaison serie.
- if (tcgetattr(this->remfd, &info) < 0) {
- ::close(this->remfd);
- vpERROR_TRACE ("Error using TCGETS in ioctl.");
- throw vpRobotException (vpRobotException::communicationError,
- "Error using TCGETS in ioctl");
- }
-
- //
- // Configuration de la liaison serie:
- // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
- //
-
- // Traitement sur les caracteres recus
- info.c_iflag = 0;
- info.c_iflag |= INLCR;
-
- // Traitement sur les caracteres envoyes sur la RS232.
- info.c_oflag = 0; // idem
-
- // Traitement des lignes
- info.c_lflag = 0;
-
- // Controle materiel de la liaison
- info.c_cflag = 0;
- info.c_cflag |= CREAD; // Validation reception
- info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
-
- // Caracteres immediatement disponibles.
- // info.c_cc[VMIN] = 1;
- // info.c_cc[VTIME] = 0;
-
- if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
- ::close(this->remfd);
- vpERROR_TRACE ("Error using TCGETS in ioctl.");
- throw vpRobotException (vpRobotException::communicationError,
- "Error using TCGETS in ioctl");
- }
-
- // Supprime tous les caracteres recus mais non encore lus par read()
- tcflush(this->remfd, TCIFLUSH);
-
- isinit = true;
-
- this->init();
-
- // Try to get the position of the zoom to check if the lens is really connected
- unsigned int izoom;
- if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
- vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with the servolens. Check if the serial link is connected.");
-
- }
-
- }
-}
-
-/*!
- Close the Servolens serial link.
- \sa open()
-*/
-void
-vpServolens::close()
-{
- if (isinit) {
- printf("\nClose the serial connexion with Servolens\n");
- ::close(this->remfd);
- isinit = false;
- }
-}
-
-/*!
- Reset the Servolens.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-*/
-void
-vpServolens::reset() const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- /* suppression de l'echo */
- sprintf(commande, "SE1");
- this->write(commande);
-
- /* initialisation de l'objectif, idem qu'a la mise sous tension */
- sprintf(commande, "SR0");
- this->write(commande);
-
- vpTime::wait(25000);
-
- this->wait();
-
- /* suppression de l'echo */
- sprintf(commande, "SE0");
- this->write(commande);
-
- /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
- sprintf(commande, "VW0");
- this->write(commande);
-}
-/*!
- Initialize the Servolens lens.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
- \sa open()
-*/
-void
-vpServolens::init() const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
-
- char commande[10];
-
- /* suppression de l'echo */
- sprintf(commande, "SE0");
- this->write(commande);
-
- /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
- sprintf(commande, "VW0");
- this->write(commande);
-
- /* L'experience montre qu'une petite tempo est utile. */
- vpTime::wait(500);
-}
-
-/*!
- Set or remove the Servolens command complete status at the end of servoing.
-
- \param servo : Servolens servo motor.
-
- \param active : true to activate the emission of a command complete
- flag at the end of motion. false to disable this functionality.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-void
-vpServolens::enableCmdComplete(vpServoType servo, bool active) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
- * une information de fin de mouvement (ex: ZF, FF, DF).
- */
- switch(servo) {
- case ZOOM:
- if (active)
- sprintf(commande, "ZF1");
- else
- sprintf(commande, "ZF0");
- break;
- case FOCUS:
- if (active)
- sprintf(commande, "FF1");
- else
- sprintf(commande, "FF0");
- break;
- case IRIS:
- if (active)
- sprintf(commande, "DF1");
- else
- sprintf(commande, "DF0");
- break;
- }
-
- /* envoie de la commande */
- this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
-}
-
-/*!
- Enable or disable the emission of the Servolens prompt "SERVOLENS>".
-
- \param active : true to activate the emission of the prompy. false
- to disable this functionality.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-void
-vpServolens::enablePrompt(bool active) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- /* suppression de l'echo */
- if (active == true)
- sprintf(commande, "SE1");
- else
- sprintf(commande, "SE0");
-
- this->write(commande);
-}
-
-/*!
- Set the controller type.
-
- \param controller : Controller type.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-*/
-void
-vpServolens::setController(vpControllerType controller) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- switch(controller) {
- case AUTO:
- /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
- sprintf(commande, "VW1");
- this->write(commande);
- break;
- case CONTROLLED:
- /* nettoyage : mot d'etat vide 0000 */
- sprintf(commande,"SX0842");
- this->write(commande);
- /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
- sprintf(commande, "VW0");
- this->write(commande);
- break;
- case RELEASED:
- sprintf(commande,"SX1084");
- this->write(commande);
- /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
- sprintf(commande, "VW0");
- this->write(commande);
- break;
- }
-
-}
-
-/*!
- Activates the auto iris mode of the Servolens.
-
- \param enable : true to activate the auto iris.
-
-*/
-void
-vpServolens::setAutoIris(bool enable) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- if (enable)
- sprintf(commande, "DA1");
- else
- sprintf(commande, "DA0");
-
- this->write(commande);
-}
-
-/*!
- Set the Servolens servo to the desired position.
-
- \param servo : Servolens servo motor to actuate.
- \param position : Desired position of the servo.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-void
-vpServolens::setPosition(vpServoType servo, unsigned int position) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- /* attente du prompt pour envoyer une commande */
- /*
- printf("attente prompt\n");
- this->wait();
- */
-
-#ifdef FINSERVO
- /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
- /* une commande de fin de mouvement (ex: ZF, FF, DF). */
- this->enableCommandComplete();
-#endif /* FINSERVO */
-
- // 08/08/00 Fabien S. - Correction de la consigne demandee
- // pour prendre en compte l'erreur entre la consigne demandee
- // et la consigne mesuree.
- // A la consigne du zoom on retranche 1.
- // A la consigne du focus on ajoute 1.
- // A la consigne du iris on ajoute 1.
- switch (servo) {
- case ZOOM:
- //printf("zoom demande: %d ", position);
- position --;
- if (position < ZOOM_MIN) position = ZOOM_MIN;
- //printf("zoom corrige: %d \n", position);
- break;
- case FOCUS:
- //printf("focus demande: %d ", position);
- position ++;
- if (position > FOCUS_MAX) position = FOCUS_MAX;
- //printf("focus corrige: %d \n", position);
- break;
- case IRIS:
- // printf("iris demande: %d ", position);
- position ++;
- if (position > IRIS_MAX) position = IRIS_MAX;
- //printf("iris corrige: %s \n", position);
- break;
- }
-
- /* commande a envoyer aux servomoteurs */
- switch(servo) {
- case ZOOM:
- sprintf(commande, "ZD%d", position);
- break;
- case FOCUS:
- sprintf(commande, "FD%d", position);
- break;
- case IRIS:
- sprintf(commande, "DD%d", position);
- break;
- }
- /* envoie de la commande */
-#ifdef PRINT
- printf("\ncommande: %s", commande);
-#endif
-
- this->write(commande);
-
-#ifdef FINSERVO
- /* on attend la fin du mouvement des objectifs */
- this->wait(servo); /* on attend les codes ZF, FF, DF */
-#endif
-}
-
-/*!
- Get the Servolens current servo position.
-
- \param servo : Servolens servo motor to actuate.
- \param position : Measured position of the servo.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-bool
-vpServolens::getPosition(vpServoType servo, unsigned int &position) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
- char posit[10], *pt_posit;
- char c;
- short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
- short posit_car; /* donne la position du caractere lu */
- short lecture_posit_en_cours; /* indique si on lit la position courante */
-
- /* attente du prompt pour envoyer une commande */
- /*
- this->wait();
- */
- pt_posit = posit;
-
- /* envoie des commandes pour obtenir la position des servo-moteurs. */
- switch (servo) {
- case ZOOM:
- sprintf(commande, "ZD?");
- break;
- case FOCUS:
- sprintf(commande, "FD?");
- break;
- case IRIS:
- sprintf(commande, "DD?");
- break;
- default:
- break;
- }
- /* envoie de la commande */
- // printf("\ncommande: %s", commande);
-
- this->write(commande);
-
- /* on cherche a lire la position du servo-moteur */
- /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
- fin_lect_posit = 0;
- posit_car = 0;
- lecture_posit_en_cours = 0;
- do {
- if (this->read(&c, 1) == true) {
-
- // printf("caractere lu: %c\n", c);
- switch (posit_car){
- /* on lit le 1er caractere; (soit Z, soit F, soit D) */
- case 0:
- /* sauvegarde du pointeur */
- pt_posit = posit;
-
- switch (servo) {
- case ZOOM:
- if( c == 'Z') posit_car = 1;
- break;
- case FOCUS:
- if( c == 'F') posit_car = 1;
- break;
- case IRIS:
- if( c == 'D') posit_car = 1;
- break;
- }
- break;
-
- /* si le 1er caractere est correct, on lit le 2eme caractere */
- /* (toujours D) */
- case 1:
- if( c == 'D') posit_car = 2;
- else posit_car = 0; /* le 2eme caractere n'est pas correct */
- break;
-
- /* si on a lu les 2 premiers caracteres, on peut lire la */
- /* position du servo-moteur */
- case 2:
- if (c >= '0' && c <= '9')
- {
- *pt_posit++ = c; /* sauvegarde de la position */
- lecture_posit_en_cours = 1;
- }
- else if (lecture_posit_en_cours)
- {
- fin_lect_posit = 1;
- *pt_posit = '\0';
- }
- else posit_car = 0;
- break;
- }
-
- }
- else {
- // Timout sur la lecture, on retoure FALSE
- return false;
- }
- }
- while ( !fin_lect_posit );
-
- // printf("\nChaine lue: posit: %s", posit);
-
- /* toilettage de la position courantes lue */
- this->clean(posit, posit);
-
- // printf("\nChaine toilettee: posit: %s", posit);
- position = (unsigned int)atoi(posit);
-
- return(true);
-}
-
-/*!
-
- These parameters are computed from the Dragonfly2 DR2-COL camera sensor pixel
- size (7.4 um) and from the servolens zoom position.
-
- \param I : An image coming from the Dragonfly2 camera attached to the
- servolens.
-
-\code
- #include <visp/vpServolens.h>
-
- int main()
- {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- vpServolens servolens("/dev/ttyS0");
-
- vpImage<unsigned char> I(240, 320);
- vpCameraParameters cam = servolens.getCameraParameters(I);
- std::cout << "Camera parameters: " << cam << std::endl;
-#endif
- }
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-\endcode
-
- */
-vpCameraParameters
-vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- vpCameraParameters cam;
- double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
- double px=1000, py=1000, u0=320, v0=240;
- // Determine if the image is subsampled.
- // Dragonfly2 native images are 640 by 480
- double subsample_factor = 1.;
- double width = I.getWidth();
- double height= I.getHeight();
-
-
- if (width > 300 && width < 340 && height > 220 && height < 260)
- subsample_factor = 2;
- else if (width > 140 && width < 1800 && height > 100 && height < 140)
- subsample_factor = 4;
-
- unsigned zoom;
- getPosition(vpServolens::ZOOM, zoom);
- //std::cout << "Actual zoom value: " << zoom << std::endl;
-
- // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
- // aux parametres de calibration de la camera (donnees constructeur) pour des
- // tailles d'images CCIR (768x576), donc avec scale = 1.
- double focale = zoom * 1.0e-5; // Transformation en metres
- px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
- py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
- u0 = I.getWidth() / 2.;
- v0 = I.getHeight() / 2.;
- cam.initPersProjWithoutDistortion(px, py, u0, v0);
-
- return cam;
-}
-
-/*!
- Waits for the Servolens promt '>'.
-
- \return The prompt character.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-*/
-char
-vpServolens::wait() const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
-
- ssize_t r;
- r = ::write(this->remfd, "\r\n", strlen("\r\n"));
- if (r != (ssize_t)(strlen("\r\n"))) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot write on Servolens.");
- }
- char c;
- do {
- r = ::read(this->remfd, &c, 1);
- c &= 0x7f;
- }
- while (c != '>');
- return c;
-
-}
-
-/*!
-
- Waits the end of motion of the corresponding servo motor.
-
- \param servo : Servolens servo motor.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-void
-vpServolens::wait(vpServoType servo) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
-
- char c;
- char fin_mvt[3];
- bool sortie = false;
-
- switch (servo) {
- case ZOOM:
- sprintf(fin_mvt, "ZF");
- break;
- case FOCUS:
- sprintf(fin_mvt, "FF");
- break;
- case IRIS:
- default:
- sprintf(fin_mvt, "DF");
- break;
-
- }
-
- /* lecture des caracteres recus */
- do {
- /* lecture des caracteres */
- if (::read(this->remfd,&c,1) != 1) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot read on Servolens.");
- }
- c &= 0x7f;
-
- /* tests si fin de mouvement */
- if (c == fin_mvt[0]) {
- /* lecture du caractere suivant */
- if (::read(this->remfd,&c,1) != 1) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot read on Servolens.");
- }
-
- c &= 0x7f;
- if (c == fin_mvt[1]) {
- sortie = true;
- }
- }
- }
- while ( !sortie);
-
- /* printf("\nmouvement fini: chaine lue = %s", chaine); */
-}
-
-/*!
- Read one character form the serial link.
-
- \param c : The character that was read.
- \param timeout_s : Timeout in seconds.
-
- \return true if the reading was successfully achieved, false otherwise.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-*/
-bool
-vpServolens::read(char *c, long timeout_s) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
-
- int n;
- fd_set readfds; /* list of fds for select to listen to */
- struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
-
- FD_ZERO(&readfds);
- FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
-
- if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
- (fd_set *)NULL, &timeout) > 0) {
- n = ::read(this->remfd, c, 1); /* read one character at a time */
- if (n != 1)
- return false;
- *c &= 0x7f;
- //printf("lecture 1 car: %c\n", *c);
- return(true);
- }
-
- return (false);
-}
-
-/*!
- Write a string to the serial link.
-
- \param s : String to send to the Servolens.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-*/
-void
-vpServolens::write(const char *s) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- ssize_t r = 0;
- r = ::write(this->remfd,"\r", strlen("\r"));
- r += ::write(this->remfd, s, strlen(s));
- r += ::write(this->remfd,"\r", strlen("\r"));
- if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot write on Servolens.");
- }
-
-
- /*
- * Une petite tempo pour laisser le temps a la liaison serie de
- * digerer la commande envoyee. En fait, la liaison serie fonctionne
- * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
- * Les plus longues commandes envoyees sur la liaison serie sont du type:
- * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
- * envoyer la commande SX0842.
- * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
- * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
- * Ici on rajoute une marge pour amener la tempo a 20 ms.
- */
- vpTime::wait(20);
-}
-
-/*!
- Suppress all the zero characters on the left hand of \e in string.
-
- \param in : Input string.
- \param out : Output string without zero characters on the left.
-
-*/
-bool
-vpServolens::clean(const char *in, char *out) const
-{
- short nb_car, i=0;
- bool error = false;
-
- nb_car = strlen(in);
-
- /* on se positionne sur le 1er caractere different de zero */
- while( *(in) == '0' && i++ < nb_car ) {
- in++;
- if (i == nb_car)
- {
- error = true; /* la chaine ne contient pas une position */
- *(out++) = '0'; /* mise a zero de la position */
- }
- }
-
- /* copie de la position epuree des zeros de gauche */
- while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
- /* de chaine \0 */
- *(out++) = *(in++);
- }
- return (error);
-}
-
-#endif
diff --git a/src/robot/real-robot/afma4/vpServolens.h b/src/robot/real-robot/afma4/vpServolens.h
deleted file mode 100644
index fcf4e64..0000000
--- a/src/robot/real-robot/afma4/vpServolens.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServolens.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Servolens lens attached to the camera fixed on the
- * Afma4 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpServolens_h
-#define __vpServolens_h
-
-#include <visp/vpConfig.h>
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-
-/*!
-
- \file vpServolens.h
-
- Interface for the Servolens lens attached to the camera fixed on the
- Afma4 robot.
-
-*/
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-
-/*!
-
- \class vpServolens
-
- \ingroup Afma4 RobotDriver
-
- \brief Interface for the Servolens lens attached to the camera fixed on the
- Afma4 robot.
-
- The code below shows how to manipulate this class to get and modify
- the position of the focal lens.
-
- \code
-#include <iostream>
-#include <visp/vpServolens.h>
-
-int main()
-{
- // Open the serial device to communicate with the Servolens lens
- vpServolens servolens("/dev/ttyS0");
-
- // Get the current zoom position
- unsigned zoom;
- servolens.getPosition(vpServolens::ZOOM, zoom);
- std::cout << "Actual zoom value: " << zoom << std::endl;
-
- // Set a new zoom value
- servolens.setPosition(vpServolens::ZOOM, zoom+1000);
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpServolens
-{
- public:
- typedef enum {
- ZOOM = 1,
- FOCUS= 2,
- IRIS = 3
- } vpServoType;
- typedef enum {
- ZOOM_MAX = 10000, // Valeur maxi zoom (mm/100)
- ZOOM_MIN = 1000, // Valeur mini zoom (mm/100)
- FOCUS_MAX = 1500, // Valeur maxi focus (metres/100)
- FOCUS_MIN = 100, // Valeur mini focus (metres/100)
- IRIS_MAX = 1000, // Valeur maxi diaph (ouverture/100)
- IRIS_MIN = 160 // Valeur mini disph (ouverture/100)
- } vpLimitsType;
- typedef enum {
- AUTO = 1,
- CONTROLLED = 2,
- RELEASED = 3
- } vpControllerType;
-
- vpServolens();
- vpServolens(const char *port);
- ~vpServolens();
-
- void open(const char *port="/dev/ttyS0");
- void close();
- void reset() const;
-
- void setController(vpControllerType controller) const;
- void setAutoIris(bool enable) const;
- void setPosition(vpServoType servo, unsigned int position) const;
- bool getPosition(vpServoType servo, unsigned int &position) const;
- vpCameraParameters getCameraParameters(vpImage<unsigned char> &I) const;
-
- void enablePrompt(bool active) const;
-
- private:
-
- void init() const;
-
- void enableCmdComplete(vpServoType servo, bool active) const;
-
- char wait() const;
- void wait(vpServoType servo) const;
-
- bool read(char *c, long timeout_s) const;
- void write(const char *s) const;
-
- bool clean(const char *in, char *out) const;
-
- int remfd; // file pointer of the host's tty
- bool isinit;
-};
-
-#endif
-#endif
-
diff --git a/src/robot/real-robot/afma6/vpAfma6.cpp b/src/robot/real-robot/afma6/vpAfma6.cpp
deleted file mode 100644
index 38008ff..0000000
--- a/src/robot/real-robot/afma6/vpAfma6.cpp
+++ /dev/null
@@ -1,1585 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAfma6.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma6 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpAfma6.cpp
-
- Control of Irisa's gantry robot named Afma6.
-
-*/
-
-
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpXmlParserCamera.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpAfma6.h>
-
-/* ----------------------------------------------------------------------- */
-/* --- STATIC ------------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-static const char *opt_Afma6[] = {"JOINT_MAX","JOINT_MIN","LONG_56","COUPL_56",
- "CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
- NULL};
-
-const char * const vpAfma6::CONST_AFMA6_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf";
-#endif
-
-const char * const vpAfma6::CONST_CAMERA_AFMA6_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Afma6/current/include/const_camera_Afma6.xml";
-#else
-= "/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml";
-#endif
-
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
-const char * const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
-const char * const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
-const char * const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
-const char * const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
-
-const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP;
-
-const unsigned int vpAfma6::njoint = 6;
-
-/*!
-
- Default constructor.
-
-*/
-vpAfma6::vpAfma6()
- : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(),
- tool_current(vpAfma6::defaultTool),
- projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
-{
- // Set the default parameters in case of the config files on the NAS
- // at Inria are not available.
-
- //
- // Geometric model constant parameters
- //
- // coupling between join 5 and 6
- this->_coupl_56 = 0.009091;
- // distance between join 5 and 6
- this->_long_56 = -0.06924;
- // Camera extrinsic parameters: effector to camera frame
- this->_eMc.setIdentity(); // Default values are initialized ...
- // ... in init (vpAfma6::vpAfma6ToolType tool,
- // vpCameraParameters::vpCameraParametersProjType projModel)
- // Maximal value of the joints
- this->_joint_max[0] = 0.7001;
- this->_joint_max[1] = 0.5201;
- this->_joint_max[2] = 0.4601;
- this->_joint_max[3] = 2.7301;
- this->_joint_max[4] = 2.4801;
- this->_joint_max[5] = 1.5901;
- // Minimal value of the joints
- this->_joint_min[0] = -0.6501;
- this->_joint_min[1] = -0.6001;
- this->_joint_min[2] = -0.5001;
- this->_joint_min[3] = -2.7301;
- this->_joint_min[4] = -0.1001;
- this->_joint_min[5] = -1.5901;
-
- init();
-
-}
-
-/*!
-
- Initialize the robot with the default tool vpAfma6::defaultTool.
- */
-void
-vpAfma6::init (void)
-{
- this->init ( vpAfma6::defaultTool);
- return;
-}
-
-/*!
-
- Read files containing the constant parameters related to the robot
- kinematics and to the end-effector to camera transformation.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param paramAfma6 : Filename containing the constant parameters of
- the robot kinematics.
-
- \param paramCamera : Filename containing the camera extrinsic parameters.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpAfma6::init (const char * paramAfma6,
- const char * paramCamera)
-{
- // vpTRACE ("Parsage fichier robot.");
- this->parseConfigFile (paramAfma6);
-
- //vpTRACE ("Parsage fichier camera.");
- this->parseConfigFile (paramCamera);
-
- return ;
-}
-#endif
-
-/*!
-
- Get the constant parameters related to the robot kinematics and to
- the end-effector to camera transformation (eMc) corresponding to the
- camera extrinsic parameters. These last parameters depend on the
- camera and projection model in use.
-
- \param tool : Camera in use.
-
- \param proj_model : Projection model of the camera.
-
-*/
-void
-vpAfma6::init (vpAfma6::vpAfma6ToolType tool,
- vpCameraParameters::vpCameraParametersProjType proj_model)
-{
-
- this->projModel = proj_model;
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- // Read the robot parameters from files
- char filename_eMc [FILENAME_MAX];
- switch (tool) {
- case vpAfma6::TOOL_CCMOP: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- 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;
- }
- }
-
- this->init (vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
-
-#else // VISP_HAVE_ACCESS_TO_NAS
-
- // Use here default values of the robot constant parameters.
- switch (tool) {
- case vpAfma6::TOOL_CCMOP: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- _erc[0] = vpMath::rad(164.35); // rx
- _erc[1] = vpMath::rad( 89.64); // ry
- _erc[2] = vpMath::rad(-73.05); // rz
- _etc[0] = 0.0117; // tx
- _etc[1] = 0.0033; // ty
- _etc[2] = 0.2272; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- _erc[0] = vpMath::rad(33.54); // rx
- _erc[1] = vpMath::rad(89.34); // ry
- _erc[2] = vpMath::rad(57.83); // rz
- _etc[0] = 0.0373; // tx
- _etc[1] = 0.0024; // ty
- _etc[2] = 0.2286; // tz
- break;
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- _erc[0] = vpMath::rad( 88.33); // rx
- _erc[1] = vpMath::rad( 72.07); // ry
- _erc[2] = vpMath::rad( 2.53); // rz
- _etc[0] = 0.0783; // tx
- _etc[1] = 0.1234; // ty
- _etc[2] = 0.1638; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- _erc[0] = vpMath::rad(86.69); // rx
- _erc[1] = vpMath::rad(71.93); // ry
- _erc[2] = vpMath::rad( 4.17); // rz
- _etc[0] = 0.1034; // tx
- _etc[1] = 0.1142; // ty
- _etc[2] = 0.1642; // tz
- break;
- }
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- _erc[0] = vpMath::rad( 90.40); // rx
- _erc[1] = vpMath::rad( 75.11); // ry
- _erc[2] = vpMath::rad( 0.18); // rz
- _etc[0] = 0.0038; // tx
- _etc[1] = 0.1281; // ty
- _etc[2] = 0.1658; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- _erc[0] = vpMath::rad(91.61); // rx
- _erc[1] = vpMath::rad(76.17); // ry
- _erc[2] = vpMath::rad(-0.98); // rz
- _etc[0] = 0.0815; // tx
- _etc[1] = 0.1162; // ty
- _etc[2] = 0.1658; // tz
- break;
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
- // set eMc to identity
- _erc[0] = 0; // rx
- _erc[1] = 0; // ry
- _erc[2] = 0; // rz
- _etc[0] = 0; // tx
- _etc[1] = 0; // ty
- _etc[2] = 0; // tz
- break;
- }
- break;
- }
- }
- vpRotationMatrix eRc(_erc);
- this->_eMc.buildFrom(_etc, eRc);
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
- setToolType(tool);
- return ;
-}
-
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
-
- This method is the same than get_fMc(const vpColVector & q).
-
- \param q : Articular position of the six joints: q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \return The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the base frame and the
- camera frame (fMc).
-
- \sa get_fMc(const vpColVector & q)
- \sa getInverseKinematics()
-
-*/
-vpHomogeneousMatrix
-vpAfma6::getForwardKinematics(const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- fMc = get_fMc(q);
-
- return fMc;
-}
-
-/*!
-
- Compute the inverse kinematics (inverse geometric model).
-
- By inverse kinematics we mean here the six articular values of the joint
- positions given the position and the orientation of the camera frame
- relative to the base frame.
-
- \param fMc : Homogeneous matrix describing the transformation from
- base frame to the camera frame.
-
- \param q : In input, the current articular joint position of the
- robot. In output, the solution of the inverse kinematics. Articular
- position of the six joints: q[0], q[1], q[2] correspond to the first
- 3 translations expressed in meter, while q[3], q[4] and q[5]
- correspond to the 3 succesives rotations expressed in radians.
-
- \param nearest : true to return the nearest solution to q. false to
- return the farest.
-
- \param verbose : Activates printings when no solution is found.
-
- \return The number of solutions (1 or 2) of the inverse geometric
- model. O, if no solution can be found.
-
- The code below shows how to compute the inverse geometric model:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRobotAfma6.h>
-
-int main()
-{
-#ifdef VISP_HAVE_AFMA6
- vpColVector q1(6), q2(6);
- vpHomogeneousMatrix fMc;
-
- vpRobotAfma6 robot;
-
- // Get the current articular position of the robot
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
-
- // Compute the pose of the camera in the reference frame using the
- // direct geometric model
- fMc = robot.getForwardKinematics(q1);
- // this is similar to fMc = robot.get_fMc(q1);
- // or robot.get_fMc(q1, fMc);
-
- // Compute the inverse geometric model
- int nbsol; // number of solutions (0, 1 or 2) of the inverse geometric model
- // get the nearest solution to the current articular position
- nbsol = robot.getInverseKinematics(fMc, q1, true);
-
- if (nbsol == 0)
- std::cout << "No solution of the inverse geometric model " << std::endl;
- else if (nbsol >= 1)
- std::cout << "First solution: " << q1 << std::endl;
-
- if (nbsol == 2) {
- // Compute the other solution of the inverse geometric model
- q2 = q1;
- robot.getInverseKinematics(fMc, q2, false);
- std::cout << "Second solution: " << q2 << std::endl;
- }
-#endif
-}
- \endcode
-
- \sa getForwardKinematics()
-
-*/
-int
-vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
- vpColVector & q, const bool &nearest, const bool &verbose) const
-{
- vpHomogeneousMatrix fMe;
- double q_[2][6],d[2],t;
- int ok[2];
- double cord[6];
-
- int nbsol = 0;
-
- if (q.getRows() != njoint)
- q.resize(6);
-
-
- // for(unsigned int i=0;i<3;i++) {
- // fMe[i][3] = fMc[i][3];
- // for(int j=0;j<3;j++) {
- // fMe[i][j] = 0.0;
- // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
- // fMe[i][3] -= fMe[i][j]*rpi[j][3];
- // }
- // }
-
- // std::cout << "\n\nfMc: " << fMc;
- // std::cout << "\n\neMc: " << _eMc;
-
- fMe = fMc * this->_eMc.inverse();
- // std::cout << "\n\nfMe: " << fMe;
-
- if (fMe[2][2] >= .99999f)
- {
- vpTRACE("singularity\n");
- q_[0][4] = q_[1][4] = M_PI/2.f;
- t = atan2(fMe[0][0],fMe[0][1]);
- q_[1][3] = q_[0][3] = q[3];
- 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 */
- {
- q_[1][5] -= vpMath::rad(10);
- q_[1][3] += vpMath::rad(10);
- }
- while (q_[1][5] <= this->_joint_min[5])
- {
- q_[1][5] += vpMath::rad(10);
- q_[1][3] -= vpMath::rad(10);
- }
- }
- else if (fMe[2][2] <= -.99999)
- {
- vpTRACE("singularity\n");
- q_[0][4] = q_[1][4] = -M_PI/2;
- t = atan2(fMe[1][1],fMe[1][0]);
- 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 */
- {
- q_[1][5] -= vpMath::rad(10);
- q_[1][3] -= vpMath::rad(10);
- }
- while (q_[1][5] <= this->_joint_min[5])
- {
- q_[1][5] += vpMath::rad(10);
- q_[1][3] += vpMath::rad(10);
- }
- }
- else
- {
- q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
- if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
- else q_[1][3] = q_[0][3] + M_PI;
-
- q_[0][4] = asin(fMe[2][2]);
- if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
- else q_[1][4] = -M_PI - q_[0][4];
-
- q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
- if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
- else q_[1][5] = q_[0][5] + M_PI;
- }
- q_[0][0] = fMe[0][3] - this->_long_56*cos(q_[0][3]);
- q_[1][0] = fMe[0][3] - this->_long_56*cos(q_[1][3]);
- q_[0][1] = fMe[1][3] - this->_long_56*sin(q_[0][3]);
- 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 */
- q_[0][5] += this->_coupl_56*q_[0][4];
- q_[1][5] += this->_coupl_56*q_[1][4];
-
- for (int j=0;j<2;j++)
- {
- ok[j] = 1;
- // test is position is reachable
- for (unsigned int i=0;i<6;i++) {
- if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
- if (verbose) {
- if (i < 3)
- std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < " << this->_joint_max[i] << std::endl;
- else
- std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < " << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
- }
- ok[j] = 0;
- }
- }
- }
- if (ok[0] == 0)
- {
- if (ok[1] == 0) {
- std::cout << "No solution..." << std::endl;
- nbsol = 0;
- return nbsol;
- }
- else if (ok[1] == 1) {
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
- nbsol = 1;
- }
- }
- else
- {
- if (ok[1] == 0) {
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
- nbsol = 1;
- }
- else
- {
- nbsol = 2;
- //vpTRACE("2 solutions\n");
- for (int j=0;j<2;j++)
- {
- d[j] = 0.0;
- for (unsigned int i=3;i<6;i++)
- d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
- }
- if (nearest == true)
- {
- if (d[0] <= d[1])
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
- else
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
- }
- else
- {
- if (d[0] <= d[1])
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
- else
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
- }
- }
- }
- for(unsigned int i=0; i<6; i++)
- q[i] = cord[i] ;
-
- return nbsol;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
-
- This method is the same than getForwardKinematics(const vpColVector & q).
-
- \param q : Articular position of the six joints: q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \return The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the base frame and the
- camera frame (fMc).
-
- \sa getForwardKinematics(const vpColVector & q)
-*/
-vpHomogeneousMatrix
-vpAfma6::get_fMc (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- get_fMc(q, fMc);
-
- return fMc;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
-
- \param q : Articular joint position of the robot. q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \param fMc The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- camera frame (fMc).
-
-*/
-void
-vpAfma6::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
-{
-
- // Compute the direct geometric model: fMe = transformation between
- // fix and end effector frame.
- vpHomogeneousMatrix fMe;
-
- get_fMe(q, fMe);
-
- fMc = fMe * this->_eMc;
-
- return;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the end effector with respect to the base frame given the
- articular positions of all the six joints.
-
- \param q : Articular joint position of the robot. q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \param fMe The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- end effector frame (fMe).
-
-*/
-void
-vpAfma6::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
-{
- double q0 = q[0]; // meter
- double q1 = q[1]; // meter
- double q2 = q[2]; // meter
-
- /* Decouplage liaisons 2 et 3. */
- double q5 = q[5] - this->_coupl_56 * q[4];
-
- double c1 = cos(q[3]);
- double s1 = sin(q[3]);
- double c2 = cos(q[4]);
- double s2 = sin(q[4]);
- double c3 = cos(q5);
- double s3 = sin(q5);
-
- // Compute the direct geometric model: fMe = transformation betwee
- // fix and end effector frame.
- fMe[0][0] = s1*s2*c3 + c1*s3;
- fMe[0][1] = -s1*s2*s3 + c1*c3;
- fMe[0][2] = -s1*c2;
- fMe[0][3] = q0 + this->_long_56*c1;
-
- fMe[1][0] = -c1*s2*c3 + s1*s3;
- fMe[1][1] = c1*s2*s3 + s1*c3;
- fMe[1][2] = c1*c2;
- fMe[1][3] = q1 + this->_long_56*s1;
-
- fMe[2][0] = c2*c3;
- fMe[2][1] = -c2*s3;
- fMe[2][2] = s2;
- fMe[2][3] = q2;
-
- fMe[3][0] = 0;
- fMe[3][1] = 0;
- fMe[3][2] = 0;
- fMe[3][3] = 1;
-
- // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
-
- return;
-}
-
-/*!
-
- Get the geometric transformation between the camera frame and the
- end-effector frame. This transformation is constant and correspond
- to the extrinsic camera parameters estimated by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
-vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- cMe = this->_eMc.inverse();
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
-vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-
- return;
-}
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame.
-
- \param q : Articular joint position of the robot. q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \param eJe : Robot jacobian expressed in the end-effector frame.
-
-*/
-void
-vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
-{
-
- eJe.resize(6,6) ;
-
- double s4,c4,s5,c5,s6,c6 ;
-
- s4=sin(q[3]); c4=cos(q[3]);
- s5=sin(q[4]); c5=cos(q[4]);
- s6=sin(q[5]-this->_coupl_56*q[4]); c6=cos(q[5]-this->_coupl_56*q[4]);
-
- eJe = 0;
- eJe[0][0] = s4*s5*c6+c4*s6;
- eJe[0][1] = -c4*s5*c6+s4*s6;
- eJe[0][2] = c5*c6;
- eJe[0][3] = - this->_long_56*s5*c6;
-
- eJe[1][0] = -s4*s5*s6+c4*c6;
- eJe[1][1] = c4*s5*s6+s4*c6;
- eJe[1][2] = -c5*s6;
- eJe[1][3] = this->_long_56*s5*s6;
-
- eJe[2][0] = -s4*c5;
- eJe[2][1] = c4*c5;
- eJe[2][2] = s5;
- eJe[2][3] = this->_long_56*c5;
-
- eJe[3][3] = c5*c6;
- eJe[3][4] = s6;
-
- eJe[4][3] = -c5*s6;
- eJe[4][4] = c6;
-
- eJe[5][3] = s5;
- eJe[5][4] = -this->_coupl_56;
- eJe[5][5] = 1;
-
- return;
-}
-
-
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- \f[
- {^f}J_e = \left(\begin{array}{cccccc}
- 1 & 0 & 0 & -Ls4 & 0 & 0 \\
- 0 & 1 & 0 & Lc4 & 0 & 0 \\
- 0 & 0 & 1 & 0 & 0 & 0 \\
- 0 & 0 & 0 & 0 & c4+\gamma s4c5 & -s4c5 \\
- 0 & 0 & 0 & 0 & s4-\gamma c4c5 & c4c5 \\
- 0 & 0 & 0 & 1 & -gamma s5 & s5 \\
- \end{array}
- \right)
- \f]
- where \f$\gamma\f$ is the coupling factor between join 5 and 6.
-
- \param q : Articular joint position of the robot. q[0], q[1], q[2]
- correspond to the first 3 translations expressed in meter, while
- q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in
- radians.
-
- \param fJe : Robot jacobian expressed in the robot reference frame.
-
-*/
-
-void
-vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
-{
-
- fJe.resize(6,6) ;
-
- // block superieur gauche
- fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
-
- double s4 = sin(q[3]) ;
- double c4 = cos(q[3]) ;
-
-
- // block superieur droit
- fJe[0][3] = - this->_long_56*s4 ;
- fJe[1][3] = this->_long_56*c4 ;
-
-
- double s5 = sin(q[4]) ;
- double c5 = cos(q[4]) ;
- // block inferieur droit
- fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
- fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
- fJe[5][3] = 1 ; fJe[5][5] = s5 ;
-
- // coupling between joint 5 and 6
- fJe[3][4] += this->_coupl_56*s4*c5;
- fJe[4][4] += -this->_coupl_56*c4*c5;
- fJe[5][4] += -this->_coupl_56*s5;
-
- return;
-}
-
-
-/*!
- Get min joint values.
-
- \return Minimal joint values for the 6 dof
- X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
- A,B,C in radians.
-
-*/
-vpColVector
-vpAfma6::getJointMin() const
-{
- vpColVector qmin(6);
- for (unsigned int i=0; i < 6; i ++)
- qmin[i] = this->_joint_min[i];
- return qmin;
-}
-
-/*!
- Get max joint values.
-
- \return Maximal joint values for the 6 dof
- X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation
- A,B,C in radians.
-
-*/
-vpColVector
-vpAfma6::getJointMax() const
-{
- vpColVector qmax(6);
- for (unsigned int i=0; i < 6; i ++)
- qmax[i] = this->_joint_max[i];
- return qmax;
-}
-
-/*!
-
- Return the coupling factor between join 5 and 6.
-
- \return Coupling factor between join 5 and 6.
-*/
-double
-vpAfma6::getCoupl56() const
-{
- return _coupl_56;
-}
-
-/*!
-
- Return the distance between join 5 and 6.
-
- \return Distance between join 5 and 6.
-*/
-double
-vpAfma6::getLong56() const
-{
- return _long_56;
-}
-
-
-/*!
-
- This function gets the robot constant parameters from a file.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param filename : File name containing the robot constant
- parameters, like max/min joint values, distance between 5 and 6 axis,
- coupling factor between axis 5 and 6, and the hand-to-eye homogenous matrix.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpAfma6::parseConfigFile (const char * filename)
-{
- size_t dim;
- int code;
- char Ligne[FILENAME_MAX];
- char namoption[100];
- FILE * fdtask;
- int numLn = 0;
- double rot_eMc[3]; // rotation
- double trans_eMc[3]; // translation
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
-
- //vpTRACE("Read the config file for constant parameters %s.", filename);
- if ((fdtask = fopen(filename, "r" )) == NULL)
- {
- vpERROR_TRACE ("Impossible to read the config file %s.",
- filename);
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file.");
- }
-
- while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
- numLn ++;
- if ('#' == Ligne[0]) { continue; }
- sscanf(Ligne, "%s", namoption);
- dim = strlen(namoption);
-
- for (code = 0;
- NULL != opt_Afma6[code];
- ++ code)
- {
- if (strncmp(opt_Afma6[code], namoption, dim) == 0)
- {
- break;
- }
- }
-
- switch(code)
- {
- case 0:
- sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf",
- namoption,
- &this->_joint_max[0], &this->_joint_max[1],
- &this->_joint_max[2], &this->_joint_max[3],
- &this->_joint_max[4], &this->_joint_max[5]);
- break;
-
- case 1:
- sscanf(Ligne, "%s %lf %lf %lf %lf %lf %lf", namoption,
- &this->_joint_min[0], &this->_joint_min[1],
- &this->_joint_min[2], &this->_joint_min[3],
- &this->_joint_min[4], &this->_joint_min[5]);
- break;
-
- case 2:
- sscanf(Ligne, "%s %lf", namoption, &this->_long_56);
- break;
-
- case 3:
- sscanf(Ligne, "%s %lf", namoption, &this->_coupl_56);
- break;
-
- case 4:
- break; // Nothing to do: camera name
-
- case 5:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &rot_eMc[0],
- &rot_eMc[1],
- &rot_eMc[2]);
-
- // Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
- break;
-
- case 6:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &trans_eMc[0],
- &trans_eMc[1],
- &trans_eMc[2]);
- get_trans_eMc = true;
- break;
-
- default:
- vpERROR_TRACE ("Bad configuration file %s "
- "ligne #%d.", filename, numLn);
- } /* SWITCH */
- } /* WHILE */
-
- fclose (fdtask);
-
- // Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- for (unsigned int i=0; i < 3; i ++) {
- _erc[i] = rot_eMc[i];
- _etc[i] = trans_eMc[i];
- }
-
- vpRotationMatrix eRc(_erc);
- this->_eMc.buildFrom(_etc, eRc);
- }
-
- return;
-}
-#endif
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpAfma6::CONST_CAMERA_AFMA6_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param image_width : Image width used to compute camera calibration.
- \param image_height : Image height used to compute camera calibration.
-
- The code below shows how to get the camera parameters of the camera
- attached to the robot.
-
- \code
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-
- vpRobotAfma6 robot;
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- // Camera parameters are read from
- // /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
- // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined in vpConfig.h file
- try {
- robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-#endif
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-*/
-
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
-{
-#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
- vpXmlParserCamera parser;
- switch (getToolType()) {
- case vpAfma6::TOOL_CCMOP: {
- std::cout << "Get camera parameters for camera \""
- << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
- vpAfma6::CONST_CCMOP_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- std::cout << "Get camera parameters for camera \""
- << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
- vpAfma6::CONST_GRIPPER_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- std::cout << "Get camera parameters for camera \""
- << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
- vpAfma6::CONST_VACUUM_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpAfma6::CONST_CAMERA_AFMA6_FILENAME,
- vpAfma6::CONST_GENERIC_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- 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).");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- break;
- }
- }
-#else
- // Set default parameters
- switch (getToolType()) {
- case vpAfma6::TOOL_CCMOP: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- default:
- vpERROR_TRACE ("This error should not occur!");
- break;
- }
-#endif
- return;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- Camera parameters are read from
- /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
-
- \param cam : In output, camera parameters to fill.
- \param I : A B&W image send by the current camera in use.
-
- \code
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-
- vpRobotAfma6 robot;
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-#endif
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
-/*!
- \brief Get the current intrinsic camera parameters obtained by calibration.
-
- Camera parameters are read from
- /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
-
- \param cam : In output, camera parameters to fill.
- \param I : A color image send by the current camera in use.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
- vpImage<vpRGBa> I;
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-
- vpRobotAfma6 robot;
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-#endif
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
-
-
-/*!
-
- Print on the output stream \e os the robot parameters (joint
- min/max, distance between axis 5 and 6, coupling factor between axis
- 5 and 6, hand-to-eye homogeneous matrix.
-
- \param os : Output stream.
- \param afma6 : Robot parameters.
-*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os,
- const vpAfma6 & afma6)
-{
- vpRotationMatrix eRc;
- afma6._eMc.extract(eRc);
- vpRxyzVector rxyz(eRc);
-
- os
- << "Joint Max:" << std::endl
- << "\t" << afma6._joint_max[0]
- << "\t" << afma6._joint_max[1]
- << "\t" << afma6._joint_max[2]
- << "\t" << afma6._joint_max[3]
- << "\t" << afma6._joint_max[4]
- << "\t" << afma6._joint_max[5]
- << "\t" << std::endl
-
- << "Joint Min: " << std::endl
- << "\t" << afma6._joint_min[0]
- << "\t" << afma6._joint_min[1]
- << "\t" << afma6._joint_min[2]
- << "\t" << afma6._joint_min[3]
- << "\t" << afma6._joint_min[4]
- << "\t" << afma6._joint_min[5]
- << "\t" << std::endl
-
- << "Long 5-6: " << std::endl
- << "\t" << afma6._long_56
- << "\t" << std::endl
-
- << "Coupling 5-6:" << std::endl
- << "\t" << afma6._coupl_56
- << "\t" << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << afma6._eMc[0][3] << " "
- << afma6._eMc[1][3] << " "
- << afma6._eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
-
- return os;
-}
-
diff --git a/src/robot/real-robot/afma6/vpAfma6.h b/src/robot/real-robot/afma6/vpAfma6.h
deleted file mode 100644
index 0b5f173..0000000
--- a/src/robot/real-robot/afma6/vpAfma6.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAfma6.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma6 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpAfma6_h
-#define __vpAfma6_h
-
-/*!
-
- \file vpAfma6.h
-
- Modelisation of Irisa's gantry robot named Afma6.
-
-*/
-
-/*!
-
- \class vpAfma6
-
- \ingroup Afma6
-
- \brief Modelisation of Irisa's gantry robot named Afma6.
-
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-class VISP_EXPORT vpAfma6
-{
- public:
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- //! File where constant parameters in relation with the robot are stored:
- //! joint max, min, coupling factor between 4 ant 5 joint, distance between 5
- //! and 6 joint, tranformation eMc between end-effector and camera frame.
- static const char * const CONST_AFMA6_FILENAME;
- static const char * const CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
- static const char * const CONST_CAMERA_AFMA6_FILENAME;
-#endif
- /*!
- Name of the camera attached to the CCMOP tool (vpAfma6ToolType::TOOL_CCMOP).
- */
- static const char * const CONST_CCMOP_CAMERA_NAME;
- /*!
- Name of the camera attached to the 2 fingers gripper tool
- (vpAfma6ToolType::TOOL_GRIPPER).
- */
- static const char * const CONST_GRIPPER_CAMERA_NAME;
- /*!
- Name of the camera attached to the vacuum gripper tool
- (vpAfma6ToolType::TOOL_VACUUM).
- */
- static const char * const CONST_VACUUM_CAMERA_NAME;
- /*!
- Name of the generic camera attached to the robot hand
- (vpAfma6ToolType::TOOL_GENERIC_CAMERA).
- */
- static const char * const CONST_GENERIC_CAMERA_NAME;
-
- //! List of possible tools that can be attached to the robot end-effector.
- typedef enum
- {
- TOOL_CCMOP, /*!< Pneumatic CCMOP gripper. */
- TOOL_GRIPPER, /*!< Pneumatic gripper with 2 fingers. */
- TOOL_VACUUM, /*!< Pneumatic vaccum gripper. */
- TOOL_GENERIC_CAMERA /*!< A generic camera. */
- } vpAfma6ToolType;
-
- //! Default tool attached to the robot end effector
- static const vpAfma6ToolType defaultTool;
-
- public:
- vpAfma6();
- /*! Destructor that does nothing. */
- virtual ~vpAfma6() {};
-
- void init (void);
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void init (const char * paramAfma6, const char * paramCamera);
-#endif
- void init (vpAfma6::vpAfma6ToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel =
- vpCameraParameters::perspectiveProjWithoutDistortion);
-
- vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
- int getInverseKinematics(const vpHomogeneousMatrix & fMc,
- vpColVector & q, const bool &nearest=true,
- const bool &verbose=false) const;
- 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;
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
- void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void parseConfigFile (const char * filename);
-#endif
-
- //! Get the current tool type
- vpAfma6ToolType getToolType() const {
- return tool_current;
- };
- //! Get the current camera model projection type
- vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const{
- return projModel;
- };
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
-
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpAfma6 & afma6);
-
- vpColVector getJointMin() const;
- vpColVector getJointMax() const;
- double getCoupl56() const;
- double getLong56() const;
-
- protected:
- //! Set the current tool type
- void setToolType(vpAfma6::vpAfma6ToolType tool){
- tool_current = tool;
- };
-
- public:
-
- static const unsigned int njoint; ///< Number of joint.
-
-
- protected:
- double _coupl_56; // coupling between join 5 and 6
- double _long_56; // distance between join 5 and 6
- double _joint_max[6]; // Maximal value of the joints
- double _joint_min[6]; // Minimal value of the joints
- // Minimal representation of _eMc
- vpTranslationVector _etc; // meters
- vpRxyzVector _erc; // radian
-
- vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
-
- protected:
- //! Current tool in use
- vpAfma6ToolType tool_current;
- // Used projection model
- vpCameraParameters::vpCameraParametersProjType projModel;
-
-};
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/afma6/vpRobotAfma6.cpp b/src/robot/real-robot/afma6/vpRobotAfma6.cpp
deleted file mode 100644
index ba60fa5..0000000
--- a/src/robot/real-robot/afma6/vpRobotAfma6.cpp
+++ /dev/null
@@ -1,2245 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotAfma6.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma6 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_AFMA6
-
-#include <signal.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <visp/vpRobotException.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpRotationMatrix.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ----------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-bool vpRobotAfma6::robotAlreadyCreated = false;
-
-/*!
-
- Default positioning velocity in percentage of the maximum
- velocity. This value is set to 15. The member function
- setPositioningVelocity() allows to change this value.
-
-*/
-const double vpRobotAfma6::defaultPositioningVelocity = 15.0;
-
-/* ---------------------------------------------------------------------- */
-/* --- EMERGENCY STOP --------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Emergency stops the robot if the program is interrupted by a SIGINT
- (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
- or SIGQUIT signal.
-
-*/
-void emergencyStopAfma6(int signo)
-{
- std::cout << "Stop the Afma6 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
- }
- //std::cout << "Emergency stop called\n";
- // PrimitiveESTOP_Afma6();
- PrimitiveSTOP_Afma6();
- std::cout << "Robot was stopped\n";
-
- // Free allocated resources
- // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
-
- fprintf(stdout, "Application ");
- fflush(stdout);
- kill(getpid(), SIGKILL);
- exit(1) ;
-}
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- The only available constructor.
-
- This contructor calls init() to initialise
- the connection with the MotionBox or low level controller, send the
- default eMc homogeneous matrix, power on the robot and wait 1 sec
- before returning to be sure the initialisation is done.
-
- It also set the robot state to vpRobot::STATE_STOP.
-
- To set the extrinsic camera parameters related to the eMc matrix
- obtained with a camera perspective projection model including the
- distorsion, use the code below:
-
- \code
- vpRobotAfma6 robot;
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters of the image I
- acquired with the camera, with:
-
- \code
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
- \endcode
-
- \sa vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-
-*/
-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
- */
-
- signal(SIGINT, emergencyStopAfma6);
- signal(SIGBUS, emergencyStopAfma6) ;
- signal(SIGSEGV, emergencyStopAfma6) ;
- signal(SIGKILL, emergencyStopAfma6);
- signal(SIGQUIT, emergencyStopAfma6);
-
- setVerbose(verbose);
- if (verbose_)
- std::cout << "Open communication with MotionBlox.\n";
- try {
- this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
- // vpERROR_TRACE("Error caught") ;
- throw ;
- }
- positioningVelocity = defaultPositioningVelocity ;
-
- vpRobotAfma6::robotAlreadyCreated = true;
-
- return ;
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INITIALISATION ----------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Initialise the connection with the MotionBox or low level
- controller, send the default eMc homogeneous matrix, power on the
- robot and wait 1 sec before returning to be sure the initialisation
- is done.
-
- \warning This method sets the camera extrinsic parameters (matrix
- eMc) to the one obtained by calibration with a camera projection
- model without distorsion by calling
- init(vpAfma6::defaultCameraRobot). If you want to set the extrinsic
- camera parameters to those obtained with a camera perspective model
- including the distorsion you have to call the
- init(vpAfma6::vpAfma6CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType) method.
-
- \sa vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-*/
-void
-vpRobotAfma6::init (void)
-{
- InitTry;
-
- // Initialise private variables used to compute the measured velocities
- q_prev_getvel.resize(6);
- q_prev_getvel = 0;
- time_prev_getvel = 0;
- first_time_getvel = true;
-
- // Initialise private variables used to compute the measured displacement
- q_prev_getdis.resize(6);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
- // Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
-
- // Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Afma6() );
-
- Try( PrimitiveRESET_Afma6() );
-
- // Update the eMc matrix in the low level controller
- init(vpAfma6::defaultTool);
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // Print the robot status
- if (verbose_) {
- std::cout << "Robot status: ";
- switch(EStopStatus) {
- case ESTOP_AUTO:
- case ESTOP_MANUAL:
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
- case ESTOP_ACTIVATED:
- std::cout << "Emergency stop is activated" << std::endl;
- break;
- default:
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- }
- std::cout << std::endl;
- }
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
-
- // If an error occur in the low level controller, goto here
- //CatchPrint();
- Catch();
-
- // Test if an error occurs
- if (TryStt == -20001)
- printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
- else if (TryStt == -675)
- printf(" Timeout enabling power...\n");
-
- if (TryStt < 0) {
- // Power off the robot
- PrimitivePOWEROFF_Afma6();
- // Free allocated resources
- ShutDownConnection();
-
- std::cout << "Cannot open connexion with the motionblox..." << std::endl;
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
- return ;
-}
-
-/*!
-
- Initialize the robot kinematics with the extrinsic calibration
- parameters associated to a specific camera (set the eMc homogeneous
- parameters in the low level controller) and also get the joint
- limits from the low-level controller.
-
- The eMc parameters depend on the camera and the projection model in use.
-
- \param tool : Tool to use.
-
- \param projModel : Projection model associated to the camera.
-
- To set the extrinsic camera parameters related to the eMc matrix
- obtained with a camera perspective projection model including the
- distorsion, use the code below:
-
- \code
- vpRobotAfma6 robot;
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters of the image \e I
- acquired with the camera, with:
-
- \code
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
- \endcode
-
- \sa vpCameraParameters, init()
-*/
-void
-vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel)
-{
-
- InitTry;
- // Read the robot constants from files
- // - joint [min,max], coupl_56, long_56
- // - camera extrinsic parameters relative to eMc
- vpAfma6::init(tool, projModel);
-
- // Set the robot constant (coupl_56, long_56) in the MotionBlox
- Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
-
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = _etc[i]; // translation in meters
- eMc_pose[i+3] = _erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
-
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
-
- setToolType(tool);
-
- CatchPrint();
- return ;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- DESTRUCTOR --------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Destructor.
-
- Free allocated resources.
-*/
-vpRobotAfma6::~vpRobotAfma6 (void)
-{
- InitTry;
-
- setRobotState(vpRobot::STATE_STOP) ;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
-// if (HIPowerStatus == 1) {
-// fprintf(stdout, "Power OFF the robot\n");
-// fflush(stdout);
-
-// Try( PrimitivePOWEROFF_Afma6() );
-// }
-
- // Free allocated resources
- ShutDownConnection();
-
- vpRobotAfma6::robotAlreadyCreated = false;
-
- CatchPrint();
- return;
-}
-
-
-
-
-/*!
-
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
-vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState)
-{
- InitTry;
-
- switch (newState) {
- case vpRobot::STATE_STOP: {
- if (vpRobot::STATE_STOP != getRobotState ()) {
- Try( PrimitiveSTOP_Afma6() );
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Afma6() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
- }
- default:
- break ;
- }
-
- CatchPrint();
-
- return vpRobot::setRobotState (newState);
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- STOP --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Stop the robot and set the robot state to vpRobot::STATE_STOP.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-*/
-void
-vpRobotAfma6::stopMotion(void)
-{
- InitTry;
- Try( PrimitiveSTOP_Afma6() );
- setRobotState (vpRobot::STATE_STOP);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
- }
-}
-
-/*!
-
- Power on the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot power on.
-
- \sa powerOff(), getPowerState()
-*/
-void
-vpRobotAfma6::powerOn(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- bool firsttime = true;
- unsigned int nitermax = 10;
-
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- if (EStopStatus == ESTOP_AUTO) {
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
- if (firsttime) {
- std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
- firsttime = false;
- }
- fprintf(stdout, "Remaining time %ds \r", nitermax-i);
- fflush(stdout);
- CAL_Wait(1);
- }
- else {
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- ShutDownConnection();
- exit(0);
- }
- }
-
- if (EStopStatus == ESTOP_ACTIVATED)
- std::cout << std::endl;
-
- if (EStopStatus == ESTOP_ACTIVATED) {
- std::cout << "Sorry, cannot power on the robot." << std::endl;
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power on the robot.");
- }
-
- if (HIPowerStatus == 0) {
- fprintf(stdout, "Power ON the Afma6 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWERON_Afma6() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Power off the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-
- \sa powerOn(), getPowerState()
-*/
-void
-vpRobotAfma6::powerOff(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- fprintf(stdout, "Power OFF the Afma6 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWEROFF_Afma6() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Get the robot power state indication if power is on or off.
-
- \return true if power is on. false if power is off
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error.
-
- \sa powerOn(), powerOff()
-*/
-bool
-vpRobotAfma6::getPowerState(void)
-{
- InitTry;
- bool status = false;
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- status = true;
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
- }
- return status;
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
-vpRobotAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- vpAfma6::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the geometric transformation between the camera frame and the
- end-effector frame. This transformation is constant and correspond
- to the extrinsic camera parameters estimated by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
-vpRobotAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpAfma6::get_cMe(cMe) ;
-}
-
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame.
-
- To compute eJe, we communicate with the low level controller to get
- the articular joint position of the robot.
-
- \param eJe : Robot jacobian expressed in the end-effector frame.
-
-*/
-void
-vpRobotAfma6::get_eJe(vpMatrix &eJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try
- {
- vpAfma6::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- To compute fJe, we communicate with the low level controller to get
- the articular joint position of the robot.
-
- \f[
- {^f}J_e = \left(\begin{array}{cccccc}
- 1 & 0 & 0 & -Ls4 & 0 & 0 \\
- 0 & 1 & 0 & Lc4 & 0 & 0 \\
- 0 & 0 & 1 & 0 & 0 & 0 \\
- 0 & 0 & 0 & 0 & c4 & -s4c5 \\
- 0 & 0 & 0 & 0 & s4 & c4c5 \\
- 0 & 0 & 0 & 1 & 0 & s5 \\
- \end{array}
- \right)
- \f]
-
- \param fJe : Robot jacobian expressed in the reference frame.
-*/
-
- void
- vpRobotAfma6::get_fJe(vpMatrix &fJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try
- {
- vpAfma6::get_fJe(q, fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Set the maximal velocity percentage to use for a position control.
-
- The default positioning velocity is defined by
- vpRobotAfma6::defaultPositioningVelocity. This method allows to
- change this default positioning velocity
-
- \param velocity : Percentage of the maximal velocity. Values should
- be in ]0:100].
-
- \code
- vpColVector position(6);
- position = 0; // position in meter and rad
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot to the joint position [0,0,0,0,0,0]
- robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
- \endcode
-
- \sa getPositioningVelocity()
-*/
-void
-vpRobotAfma6::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
-
-/*!
- Get the maximal velocity percentage used for a position control.
-
- \sa setPositioningVelocity()
-*/
-double
-vpRobotAfma6::getPositioningVelocity (void)
-{
- return positioningVelocity;
-}
-
-/*!
-
- Move the robot to an absolute cartesian position with a given percent of max
- velocity. The percent of max velocity is to set with
- setPositioningVelocity(). The position to reach can only be specified in
- camera frame or in the reference frame. In joint, an exception is thrown.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pose : A six dimension pose vector corresponding to the position
- to reach. The three first parameters are the translations in meter, the three
- last parameters are the rotations expressed as a theta u vector in
- radians. If the position is out of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the camera and the reference frame, rotations are
- represented by a vpThetaUVector.
-
- - Mixt frame of joint frame is not implemented.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
- and vpRobot::ARTICULAR_FRAME not implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpPoseVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_AFMA6
- vpPoseVector pose;
- // Set positions in the reference frame
- pose[0] = 0.1; // x axis, in meter
- pose[1] = 0.; // y axis, in meter
- pose[2] = 0.3; // z axis, in meter
- pose[3] = M_PI/8; // ThetaU rotation around x axis, in rad
- pose[4] = M_PI/4; // ThetaU rotation around y axis, in rad
- pose[5] = 0.; // ThetaU rotation around z axis, in rad
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::REFERENCE_FRAME, pose);
-
- return 0;
-#endif
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::REFERENCE_FRAME, pose);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- }
- \endcode
-
-*/
-void
-vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
- const vpPoseVector & pose )
-
-{
- vpColVector position(6);
- vpRxyzVector rxyz;
- vpRotationMatrix R;
-
- R.buildFrom(pose[3], pose[4], pose[5]); //thetau
- rxyz.buildFrom(R);
-
- for (unsigned int i=0; i < 3; i++) {
- position[i] = pose[i];
- position[i+3] = rxyz[i];
- }
- if (frame == vpRobot::ARTICULAR_FRAME) {
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Joint frame not implemented for pose positionning.");
- }
- setPosition (frame, position);
-}
-/*!
-
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param position : A six dimension vector corresponding to the
- position to reach. All the positions are expressed in meters for the
- translations and radians for the rotations. If the position is out
- of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X, Y, Z, A, B, C,
- with X,Y,Z the translations, and A,B,C the rotations of the
- end-effector.
-
- - In the camera and the reference frame, rotations are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpColVector.h>
-#include <visp/vpRobotException.h>
-
-int main()
-{
-#ifdef VISP_HAVE_AFMA6
- vpColVector position(6);
- // Set positions in the camera frame
- position[0] = 0.1; // x axis, in meter
- position[1] = 0.2; // y axis, in meter
- position[2] = 0.3; // z axis, in meter
- position[3] = M_PI/8; // rotation around x axis, in rad
- position[4] = M_PI/4; // rotation around y axis, in rad
- position[5] = M_PI/10;// rotation around z axis, in rad
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
-
- return 0;
-#endif
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- }
- \endcode
-
-*/
-
-void
-vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & position )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- double _destination[6];
- int error = 0;
- double timestamp;
-
- InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
-
- vpColVector q(njoint);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = _q[i];
-
- // Get fMc from the inverse kinematics
- vpHomogeneousMatrix fMc;
- vpAfma6::get_fMc(q, fMc);
-
- // Set cMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
-
- // Compute cMc2
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- // Compute the new position to reach: fMc*cMc2
- vpHomogeneousMatrix fMc2 = fMc * cMc2;
-
- // Compute the corresponding joint position from the inverse kinematics
- bool nearest = true;
- int solution = this->getInverseKinematics(fMc2, q, nearest);
- if (solution) { // Position is reachable
- for (unsigned int i=0; i < njoint; i ++) {
- _destination[i] = q[i];
- }
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
-
- break ;
- }
- case vpRobot::ARTICULAR_FRAME: {
- for (unsigned int i=0; i < njoint; i ++) {
- _destination[i] = position[i];
- }
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- break ;
-
- }
- case vpRobot::REFERENCE_FRAME: {
- // Set fMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
- // Compute fMc from the input position
- vpRotationMatrix fRc(rxyz);
- vpHomogeneousMatrix fMc(txyz, fRc);
-
- // Compute the corresponding joint position from the inverse kinematics
- vpColVector q(6);
- bool nearest = true;
- int solution = this->getInverseKinematics(fMc, q, nearest);
- if (solution) { // Position is reachable
- for (unsigned int i=0; i < njoint; i ++) {
- _destination[i] = q[i];
- }
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
-
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt == InvalidPosition || TryStt == -1023)
- std::cout << " : Position out of range.\n";
- else if (TryStt < 0)
- std::cout << " : Unknown error (see Fabien).\n";
- else if (error == -1)
- std::cout << "Position out of range.\n";
-
- if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
-
- return ;
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- This method owerloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
- the position to reach. All the positions are expressed in meters for
- the translations and radians for the rotations.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X (pos1), Y (pos2),
- Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
- translations, and A,B,C the rotations of the end-effector.
-
- - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotAfma6.h>
-
-int main()
-{
-#ifdef VISP_HAVE_AFMA6
- // Set positions in the camera frame
- double pos1 = 0.1; // x axis, in meter
- double pos2 = 0.2; // y axis, in meter
- double pos3 = 0.3; // z axis, in meter
- double pos4 = M_PI/8; // rotation around x axis, in rad
- double pos5 = M_PI/4; // rotation around y axis, in rad
- double pos6 = M_PI; // rotation around z axis, in rad
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
-
- return 0;
-#endif
-}
- \endcode
-
- \sa setPosition()
-*/
-void vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6)
-{
- try{
- vpColVector position(6) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_AFMA6
- vpRobotAfma6 robot;
- vpColVector q; // joint position
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
- return 0;
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-
-*/
-void vpRobotAfma6::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
-
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the articular
- position of each dof, first the 3 translations, then the 3
- articular rotation positions represented by a vpRxyzVector.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Time in second since last robot power on.
-
- \code
- vpRobotAfma6 robot;
- vpColVector position;
- robot.getPosition(vpRobot::REFERENCE_FRAME, position);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame pose in terms of a homogenous matrix
- vpHomogeneousMatrix fMc(ftc, fRc);
- \endcode
-
- \exception vpRobotException::lowLevelError : If the position cannot
- be get from the low level controller.
-
- \sa setPosition(const vpRobot::vpControlFrameType frame, const
- vpColVector & r)
-
-*/
-void
-vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector & position, double ×tamp)
-{
-
- InitTry;
-
- position.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
- position[i] = _q[i];
- }
-
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
-
- vpColVector q(njoint);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = _q[i];
-
- // Compute fMc
- vpHomogeneousMatrix fMc;
- vpAfma6::get_fMc(q, fMc);
-
- // From fMc extract the pose
- vpRotationMatrix fRc;
- fMc.extract(fRc);
- vpRxyzVector rxyz;
- rxyz.buildFrom(fRc);
-
- for (unsigned int i=0; i < 3; i++) {
- position[i] = fMc[i][3]; // translation x,y,z
- position[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
- break ;
- }
- case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get position.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position.");
- }
-
- return;
-}
-/*!
- Returns the robot controller current time (in second) since last robot power on.
-*/
-double vpRobotAfma6::getTime() const
-{
- double timestamp;
- PrimitiveACQ_TIME_Afma6(×tamp);
- return timestamp;
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
-
- Get the current position of the robot.
- Similar as :
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position)
- as the difference the position is returned using a ThetaU representation.
-
-*/
-void
-vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
-{
- vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame, posRxyz, timestamp);
- vpRxyzVector RxyzVect;
- for(unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(RxyzVect);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
-
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
- is always 6.
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
-
- - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
-
- - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
- // Set joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // X axis, in meter/s
- q_dot[1] = 0.2; // Y axis, in meter/s
- q_dot[2] = 0.3; // Z axis, in meter/s
- q_dot[3] = M_PI/8; // A axis, in rad/s
- q_dot[4] = M_PI/4; // B axis, in rad/s
- q_dot[5] = M_PI/16;// C axis, in rad/s
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
- \endcode
-*/
-void
-vpRobotAfma6::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++)
- vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
-
- vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
-
- InitTry;
-
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
- Try( PrimitiveMOVESPEED_Afma6(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6) );
- break ;
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
- }
-
- Catch();
- if (TryStt < 0) {
- if (TryStt == VelStopOnJoint) {
- Int32 axisInJoint[njoint];
- PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i])
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- }
- }
- else {
- printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
- if (TryString != NULL) {
- // The statement is in TryString, but we need to check the validity
- printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
- printf("\n");
- }
- }
- }
-
- return;
-}
-
-
-
-
-
-
-/* ------------------------------------------------------------------------ */
-/* --- GET ---------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param velocity : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Time in second since last robot power on.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \warning The first time this method is called, \e velocity is set to 0. The
- first call is used to intialise the velocity computation for the next call.
-
- \code
- // Set joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // X axis, in meter/s
- q_dot[1] = 0.2; // Y axis, in meter/s
- q_dot[2] = 0.3; // Z axis, in meter/s
- q_dot[3] = M_PI/8; // A axis, in rad/s
- q_dot[4] = M_PI/4; // B axis, in rad/s
- q_dot[5] = M_PI/16;// C axis, in rad/s
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- vpColVector q_dot_mes; // Measured velocities
-
- // Initialisation of the velocity measurement
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- double timestamp;
- while (1) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
- \endcode
-*/
-void
-vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp)
-{
- velocity.resize (6);
- velocity = 0;
-
- double q[6];
- vpColVector q_cur(6);
- vpHomogeneousMatrix fMc_cur;
- vpHomogeneousMatrix cMc; // camera displacement
- double time_cur;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
- time_cur = timestamp;
- for (unsigned int i=0; i < njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- // Get the camera pose from the direct kinematics
- vpAfma6::get_fMc(q_cur, fMc_cur);
-
- if ( ! first_time_getvel ) {
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
-
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the ThetaU representation for the rotation
- vpRotationMatrix cRc;
- cMc.extract(cRc);
- vpThetaUVector thetaU;
- thetaU.buildFrom(cRc);
-
- for (unsigned int i=0; i < 3; i++) {
- // Compute the translation displacement in the reference frame
- velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
- // Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
- }
-
- // Compute the velocity
- velocity /= (time_cur - time_prev_getvel);
- break ;
- }
- }
- }
- else {
- first_time_getvel = false;
- }
-
- // Memorize the camera pose for the next call
- fMc_prev_getvel = fMc_cur;
-
- // Memorize the joint position for the next call
- q_prev_getvel = q_cur;
-
- // Memorize the time associated to the joint position for the next call
- time_prev_getvel = time_cur;
-
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
-
- */
-void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
-{
- double timestamp;
- getVelocity(frame, velocity, timestamp);
-}
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Time in second since last robot power on.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
- // Set joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // X axis, in meter/s
- q_dot[1] = 0.2; // Y axis, in meter/s
- q_dot[2] = 0.3; // Z axis, in meter/s
- q_dot[3] = M_PI/8; // A axis, in rad/s
- q_dot[4] = M_PI/4; // B axis, in rad/s
- q_dot[5] = M_PI/16;// C axis, in rad/s
-
- vpRobotAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- vpColVector q_dot_mes; // Measured velocities
- double timestamp;
- while (1) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
- \endcode
-*/
-vpColVector
-vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- vpColVector velocity;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
-
- */
-vpColVector vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector velocity;
- double timestamp;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
-Read joint positions in a specific Afma6 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#AFMA6 - Position - Version 2.01
-# file: "myposition.pos "
-#
-# R: X Y Z A B C
-# Joint position: X, Y, Z: translations in meters
-# A, B, C: rotations in degrees
-#
-
-R: 0.1 0.3 -0.25 -80.5 80 0
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are
-expressed in meters, while joint rotations A,B,C in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-vpRobotAfma6 robot;
-vpColVector q; // Joint position
-robot.readPosFile("myposition.pos", q); // Set the joint position from the file
-robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
-robot.setPositioningVelocity(5); // Positioning velocity set to 5%
-robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-\endcode
-
-\sa savePosFile()
-*/
-
-bool
-vpRobotAfma6::readPosFile(const char *filename, vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
-// else
-// return (false); // fin fichier sans position robot.
- }
- }
- else {
- return (false); /* fin fichier */
- }
-
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(njoint);
- sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2],
- &q[3], &q[4], &q[5]);
-
- // converts rotations from degrees into radians
- for (unsigned int i=3; i < njoint; i ++)
- q[i] = vpMath::rad(q[i]) ;
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Save joint (articular) positions in a specific Afma6 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : Joint positions [X,Y,Z,A,B,C] to save in the
- filename. Translations X,Y,Z are expressed in meters, while
- rotations A,B,C in radians.
-
- \warning The joint rotations A,B,C written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-
-bool
-vpRobotAfma6::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#AFMA6 - Position - Version 2.01\n\
-#\n\
-# R: X Y Z A B C\n\
-# Joint position: X, Y, Z: translations in meters\n\
-# A, B, C: rotations in degrees\n\
-#\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- q[0],
- q[1],
- q[2],
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Moves the robot to the joint position specified in the filename. The
- positioning velocity is set to 10% of the robot maximal velocity.
-
- \param filename : File containing a joint position to reach.
-
- \sa readPosFile(), move(const char *, const double)
-
-*/
-void
-vpRobotAfma6::move(const char *filename)
-{
- vpColVector q;
-
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Moves the robot to the joint position specified in the \e filename with a
- specified positioning \e velocity.
-
- \param filename : File containing a joint position to reach.
- \param velocity : Percentage of the maximal velocity. Values should
- be in ]0:100].
-
- \sa readPosFile(), move(const char *)
-
-*/
-void
-vpRobotAfma6::move(const char *filename, const double velocity)
-{
- vpColVector q;
-
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- this->setPositioningVelocity(velocity);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Open the pneumatic CCMOP gripper.
-
- \sa closeGripper()
-*/
-void
-vpRobotAfma6::openGripper()
-{
- InitTry;
- Try( PrimitiveGripper_Afma6(1) );
- std::cout << "Open the gripper..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot open the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot open the gripper.");
- }
-}
-
-/*!
-
- Close the pneumatic CCMOP gripper.
-
- \sa openGripper()
-
-*/
-void
-vpRobotAfma6::closeGripper()
-{
- InitTry;
- Try( PrimitiveGripper_Afma6(0) );
- std::cout << "Close the gripper..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot close the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot close the gripper.");
- }
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotAfma6::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param displacement : The measured articular displacement. The
- dimension of \e displacement is 6 (the number of axis of the
- robot). Translations are expressed in meters, rotations in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma6::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- reference and mixt frames. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME) and in the camera frame (vpRobot::CAMERA_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call
- of this method. The dimension of \e displacement is always
- 6. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
-
- double q[6];
- vpColVector q_cur(6);
- vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
- double timestamp;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- // Compute the camera pose in the reference frame
- fMc_cur = get_fMc(q_cur);
-
- if ( ! first_time_getdis ) {
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- // Compute the camera dispacement from the previous pose
- c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
-
- vpTranslationVector t;
- vpRotationMatrix R;
- c_prevMc_cur.extract(t);
- c_prevMc_cur.extract(R);
-
- vpRxyzVector rxyz;
- rxyz.buildFrom(R);
-
- for (unsigned int i=0; i<3; i++) {
- displacement[i] = t[i];
- displacement[i+3] = rxyz[i];
- }
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-
- // Memorize the pose for the next call
- fMc_prev_getdis = fMc_cur;
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Test the joints of the robot to detect if one or more is at its limit.
-
- \param jointsStatus : A vector (size 6) of the status of the joints. For each
- joint, the value is equal to 1 if the joint is at its maximal limit, -1 if the
- joint is at its minimal value and 0 otherwise.
-
- \return false if at least one joint is at one of its limit.
-*/
-bool
-vpRobotAfma6::checkJointLimits(vpColVector& jointsStatus)
-{
- Int32 axisInJoint[njoint];
- bool status = true;
- jointsStatus.resize(6);
- InitTry;
-
- Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
-NULL));
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i]){
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- jointsStatus[i] = axisInJoint[i];
- status = false;
- }
- else{
- jointsStatus[i] = 0;
- }
- }
-
- Catch();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot check joint limits.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot check joint limits.");
- }
-
- return status;
-
-}
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-#endif
-
diff --git a/src/robot/real-robot/afma6/vpRobotAfma6.h b/src/robot/real-robot/afma6/vpRobotAfma6.h
deleted file mode 100644
index f7b48b0..0000000
--- a/src/robot/real-robot/afma6/vpRobotAfma6.h
+++ /dev/null
@@ -1,357 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotAfma6.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Afma6 robot controlled by an Adept MotionBlox.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpRobotAfma6_h
-#define vpRobotAfma6_h
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_AFMA6
-
-#include <iostream>
-#include <stdio.h>
-
-#include <visp/vpRobot.h>
-#include <visp/vpColVector.h>
-#include <visp/vpPoseVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpAfma6.h>
-
-// low level controller api
-extern "C" {
-# include "irisa_Afma6.h"
-# include "trycatch.h"
-}
-
-
-/*!
- \class vpRobotAfma6
-
- \ingroup Afma6 RobotDriver
-
- \brief Control of Irisa's gantry robot named Afma6.
-
- Implementation of the vpRobot class in order to control Irisa's
- Afma6 robot. This robot is a gantry robot with six degrees of
- freedom manufactured in 1992 by the french Afma-Robots company. In
- 2008, the low level controller change for a more recent Adept
- technology based on the MotionBlox controller. A firewire camera is
- mounted on the end-effector to allow eye-in-hand visual
- servoing. The control of this camera is achieved by the
- vp1394TwoGrabber class. A ring light is attached around the
- camera. The control of this ring light is possible throw the
- vpRingLight class. A CCMOP gripper is also mounted on the
- end-effector. The pneumatic control of this gripper is possible
- throw the openGripper() or closeGripper() member functions.
-
- This class allows to control the Afma6 gantry robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The direct and inverse kinematics models are implemented in the
- vpAfma6 class.
-
- \warning A Ctrl-C, a segmentation fault or other system errors are
- catched by this class to stop the robot.
-
- To communicate with the robot, you may first create an instance of this
- class by calling the default constructor:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotAfma6.h>
-
-#ifdef VISP_HAVE_AFMA6
-int main()
-{
- vpRobotAfma6 robot;
-}
-#else
-int main() {}
-#endif
- \endcode
-
- This initialize the robot kinematics with the eMc extrinsic camera
- parameters obtained with a projection model without distortion. To
- set the robot kinematics with the eMc matrix obtained with a camera
- perspective model including distortion you need to initialize the
- robot with:
-
- \code
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distortion parameter
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- You can get the intrinsic camera parameters of the image I
- acquired with the camera, with:
-
- \code
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distortion.
- \endcode
-
- To control the robot in position, you may set the controller
- to position control and than send the position to reach in a specific
- frame like here in the joint space:
-
- \code
- vpColVector q(6);
- // Set a joint position
- q[0] = 0.1; // x axis, in meter
- q[1] = 0.2; // y axis, in meter
- q[2] = 0.3; // z axis, in meter
- q[3] = M_PI/8; // rotation around A axis, in rad
- q[4] = M_PI/4; // rotation around B axis, in rad
- q[5] = M_PI; // rotation around C axis, in rad
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- The robot moves to the specified position with the default
- positioning velocity vpRobotAfma6::defaultPositioningVelocity. The
- setPositioningVelocity() method allows to change the maximal
- velocity used to reach the desired position.
-
- \code
- // Set the max velocity to 40%
- robot.setPositioningVelocity(40);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and than send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // x axis, in m/s
- qvel[1] = 0.2; // y axis, in m/s
- qvel[2] = 0; // z axis, in m/s
- qvel[3] = M_PI/8; // rotation around A axis, in rad/s
- qvel[4] = 0; // rotation around B axis, in rad/s
- qvel[5] = 0; // rotation around C axis, in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- while (...) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP)
- \endcode
-
- There is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and writePosFile()
- methods.
-*/
-class VISP_EXPORT vpRobotAfma6
- :
- public vpAfma6,
- public vpRobot
-{
-
-private: /* Not allowed functions. */
-
- /*!
- Copy constructor not allowed.
- */
- vpRobotAfma6 (const vpRobotAfma6 & robot);
-
-private: /* Attributs prives. */
-
- /** \brief Vrai ssi aucun objet de la classe vpRobotAfma6 n'existe.
- *
- * Il ne peut exister simultanement qu'un seul objet de la classe
- * vpRobotAfma6, car il correspond a un seul robot AFMA6. Creer
- * simultanement deux objets peut engendrer des conflits. Le constructeur
- * lance une erreur si le champ n'est pas FAUX puis positionne le champ
- * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
- * alors la creation d'un nouvel objet.
- */
- static bool robotAlreadyCreated;
-
- double positioningVelocity;
-
- // Variables used to compute the measured velocities (see getVelocity() )
- vpColVector q_prev_getvel;
- vpHomogeneousMatrix fMc_prev_getvel;
- double time_prev_getvel;
- bool first_time_getvel;
-
- // Variables used to compute the measured displacement (see
- // getDisplacement() )
- vpColVector q_prev_getdis;
- bool first_time_getdis;
- vpHomogeneousMatrix fMc_prev_getdis;
-
-
-public: /* Constantes */
-
- /* Vitesse maximale par default lors du positionnement du robot.
- * C'est la valeur a la construction de l'attribut prive \a
- * positioningVelocity. Cette valeur peut etre changee par la fonction
- * #setPositioningVelocity.
- */
- static const double defaultPositioningVelocity; // = 20.0;
-
-public: /* Methode publiques */
-
- vpRobotAfma6 (bool verbose=true);
- virtual ~vpRobotAfma6 (void);
-
- bool checkJointLimits(vpColVector& jointsStatus);
-
- void closeGripper() ;
-
- void getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
-
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position, double ×tamp);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp);
-
- double getPositioningVelocity (void);
- bool getPowerState();
- double getTime() const;
-
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
-
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &_cMe) const;
- void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_eJe(vpMatrix &_eJe);
- void get_fJe(vpMatrix &_fJe);
-
- void init (void);
- void init (vpAfma6::vpAfma6ToolType tool,
- vpCameraParameters::vpCameraParametersProjType
- projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
-
- void move(const char *filename) ;
- void move(const char *filename, const double velocity) ;
-
- void openGripper() ;
-
- void powerOn() ;
- void powerOff() ;
-
- static bool readPosFile(const char *filename, vpColVector &q) ;
- static bool savePosFile(const char *filename, const vpColVector &q) ;
-
- /* --- POSITIONNEMENT --------------------------------------------------- */
- void setPosition (const vpRobot::vpControlFrameType frame,
- const vpPoseVector & pose );
- void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &position) ;
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1, const double pos2, const double pos3,
- const double pos4, const double pos5, const double pos6) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
-
- /* --- ETAT ------------------------------------------------------------- */
-
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
-
-
- /* --- VITESSE ---------------------------------------------------------- */
-
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
-
- void stopMotion() ;
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
-};
-
-
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-#endif /* #ifndef vpRobotAfma6_h */
diff --git a/src/robot/real-robot/biclops/vpBiclops.cpp b/src/robot/real-robot/biclops/vpBiclops.cpp
deleted file mode 100644
index 92a2bae..0000000
--- a/src/robot/real-robot/biclops/vpBiclops.cpp
+++ /dev/null
@@ -1,455 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBiclops.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/* ----------------------------------------------------------------------- */
-/* --- INCLUDE ----------------------------------------------------------- */
-/* ----------------------------------------------------------------------- */
-
-
-
-#include <visp/vpBiclops.h>
-#include <visp/vpDebug.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpMath.h>
-#include <math.h>
-
-
-/* ------------------------------------------------------------------------ */
-/* --- COMPUTE ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------ */
-const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
-const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
-const float vpBiclops::panJointLimit = (float)(M_PI); /*!< Pan range (in rad): from -panJointLimit to + panJointLimit */
-const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5); /*!< Tilt range (in rad): from -tiltJointLimit to + tiltJointLimit */
-
-const float vpBiclops::speedLimit = (float)(M_PI/3.0); /*!< Maximum speed (in rad/s) to perform a displacement */
-
-/*!
- Compute the direct geometric model of the camera: fMc
-
- \warning Provided for compatibilty with previous versions. Use rather
- get_fMc(const vpColVector &, vpHomogeneousMatrix &).
-
- \param q : Articular position for pan and tilt axis.
-
- \param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
-
- \sa get_fMc(const vpColVector &, vpHomogeneousMatrix &)
-*/
-void
-vpBiclops::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
-{
- vpHomogeneousMatrix fMe = get_fMe(q);
- fMc = fMe * cMe_.inverse();
-
- vpCDEBUG (6) << "camera position: " << std::endl << fMc;
-
- return ;
-}
-
-/*!
- Compute the direct geometric model of the camera: fMc
-
- \param q : Articular position for pan and tilt axis.
-
- \param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
-
-*/
-void
-vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
-{
- vpHomogeneousMatrix fMe = get_fMe(q);
- fMc = fMe * cMe_.inverse();
-
- vpCDEBUG (6) << "camera position: " << std::endl << fMc;
-
- return ;
-}
-
-/*!
- Return the direct geometric model of the camera: fMc
-
- \warning Provided for compatibilty with previous versions. Use rather
- get_fMc(const vpColVector &).
-
- \param q : Articular position for pan and tilt axis.
-
- \return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Describes the transformation between the robot reference
- frame (called fixed) and the camera frame.
-
- \sa get_fMc(const vpColVector &)
-*/
-vpHomogeneousMatrix
-vpBiclops::computeMGD (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
-
- computeMGD (q, fMc);
-
- return fMc;
-}
-
-/*!
- Return the direct geometric model of the camera: fMc
-
- \param q : Articular position for pan and tilt axis.
-
- \return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Discribes the transformation between the robot reference
- frame (called fixed) and the camera frame.
-
-*/
-vpHomogeneousMatrix
-vpBiclops::get_fMc (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
-
- get_fMc (q, fMc);
-
- return fMc;
-}
-
-/*!
- Return the direct geometric model of the end effector: fMe
-
- \param q : Articular position for pan and tilt axis.
-
- \return fMe, the homogeneous matrix corresponding to the direct geometric
- model of the end effector. Describes the transformation between the robot reference
- frame (called fixed) and the end effector frame.
-
-*/
-vpHomogeneousMatrix
-vpBiclops::get_fMe (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMe;
-
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for biclops articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
- }
-
- double q1 = q[0]; // pan
- double q2 = q[1]; // tilt
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
-
- if (dh_model_ == DH1)
- {
- fMe[0][0] = -c1*s2;
- fMe[0][1] = -s1;
- fMe[0][2] = c1*c2;
- fMe[0][3] = 0;
-
- fMe[1][0] = -s1*s2;
- fMe[1][1] = c1;
- fMe[1][2] = s1*c2;
- fMe[1][3] = 0;
-
- fMe[2][0] = -c2;
- fMe[2][1] = 0;
- fMe[2][2] = -s2;
- fMe[2][3] = 0;
-
- fMe[3][0] = 0;
- fMe[3][1] = 0;
- fMe[3][2] = 0;
- fMe[3][3] = 1;
- }
- else
- {
- fMe[0][0] = c1*s2;
- fMe[0][1] = -s1;
- fMe[0][2] = c1*c2;
- fMe[0][3] = 0;
-
- fMe[1][0] = s1*s2;
- fMe[1][1] = c1;
- fMe[1][2] = s1*c2;
- fMe[1][3] = 0;
-
- fMe[2][0] = -c2;
- fMe[2][1] = 0;
- fMe[2][2] = s2;
- fMe[2][3] = 0;
-
- fMe[3][0] = 0;
- fMe[3][1] = 0;
- fMe[3][2] = 0;
- fMe[3][3] = 1;
- }
-
- return fMe;
-}
-
-/*!
- Compute the direct geometric model of the camera in terms of pose vector.
-
- \warning Provided for compatibilty with previous versions. Use rather
- get_fMc(const vpColVector &, vpPoseVector &).
-
- \param q : Articular position for pan and tilt axis.
-
- \param fvc : Pose vector corresponding to the transformation between the robot
- reference frame (called fixed) and the camera frame.
-
- \sa get_fMc(const vpColVector &, vpPoseVector &)
-*/
-
-void
-vpBiclops::computeMGD (const vpColVector &q, vpPoseVector &fvc) const
-{
- vpHomogeneousMatrix fMc;
-
- get_fMc (q, fMc);
- fvc.buildFrom(fMc.inverse());
-
- return ;
-}
-
-/*!
- Compute the direct geometric model of the camera in terms of pose vector.
-
- \param q : Articular position for pan and tilt axis.
-
- \param fvc : Pose vector corresponding to the transformation between the robot
- reference frame (called fixed) and the camera frame.
-
-*/
-
-void
-vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc) const
-{
- vpHomogeneousMatrix fMc;
-
- get_fMc (q, fMc);
- fvc.buildFrom(fMc.inverse());
-
- return ;
-}
-
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Default construtor. Call init().
-
-*/
-vpBiclops::vpBiclops (void)
- : dh_model_(DH1), cMe_()
-{
- init();
-}
-/* ---------------------------------------------------------------------- */
-/* --- PRIVATE ---------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-/*!
- Initialization.
- Set the default \f${^c}{\bf M}_e\f$ transformation.
-
-*/
-void
-vpBiclops::init ()
-{
- dh_model_ = DH1;
- set_cMe();
- return ;
-}
-
-
-/* ----------------------------------------------------------------------- */
-/* --- DISPLAY ----------------------------------------------------------- */
-/* ----------------------------------------------------------------------- */
-
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & /*constant*/)
-{
- os
- << "Geometric parameters: " << std::endl
- << "h: "
- << "\t" << vpBiclops::h << std::endl;
-
- return os;
-}
-
-
-/*!
-
- Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cVe : Twist transformation between camera and end effector frame to
- expess a velocity skew from end effector frame in camera frame.
-
-*/
-void
-vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- cVe.buildFrom(cMe_) ;
-}
-
-/*!
-
- Set the default homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
-*/
-void
-vpBiclops::set_cMe()
-{
- vpHomogeneousMatrix eMc ;
-
- eMc[0][0] = 0;
- eMc[0][1] = -1;
- eMc[0][2] = 0;
- eMc[0][3] = h;
-
- eMc[1][0] = 1;
- eMc[1][1] = 0;
- eMc[1][2] = 0;
- eMc[1][3] = 0;
-
- eMc[2][0] = 0;
- eMc[2][1] = 0;
- eMc[2][2] = 1;
- eMc[2][3] = 0;
-
- eMc[3][0] = 0;
- eMc[3][1] = 0;
- eMc[3][2] = 0;
- eMc[3][3] = 1;
-
- cMe_ = eMc.inverse() ;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- \warning Re is not the embedded camera frame. It corresponds to the frame
- associated to the tilt axis (see also get_cMe).
-
- \param q : Articular position for pan and tilt axis.
-
- \param eJe : Jacobian between end effector frame and end effector frame (on
- tilt axis).
-
-*/
-void
-vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
-{
- eJe.resize(6,2) ;
-
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for biclops articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
- }
-
- double s2 = sin(q[1]) ;
- double c2 = cos(q[1]) ;
-
- eJe = 0;
-
- if (dh_model_ == DH1)
- {
- eJe[3][0] = -c2;
- eJe[4][1] = 1;
- eJe[5][0] = -s2;
- }
- else
- {
- eJe[3][0] = -c2;
- eJe[4][1] = -1;
- eJe[5][0] = s2;
-
- }
-
-}
-/*!
- Get the robot jacobian expressed in the robot reference frame
-
- \param q : Articular position for pan and tilt axis.
-
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
-
-*/
-
-void
-vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
-{
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for biclops articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
- }
-
- fJe.resize(6,2) ;
-
- double s1 = sin(q[0]) ;
- double c1 = cos(q[0]) ;
-
- fJe = 0;
-
- if (dh_model_ == DH1)
- {
- fJe[3][1] = -s1;
- fJe[4][1] = c1;
- fJe[5][0] = 1;
- }
- else
- {
- fJe[3][1] = s1;
- fJe[4][1] = -c1;
- fJe[5][0] = 1;
- }
-}
diff --git a/src/robot/real-robot/biclops/vpBiclops.h b/src/robot/real-robot/biclops/vpBiclops.h
deleted file mode 100644
index 2461b4d..0000000
--- a/src/robot/real-robot/biclops/vpBiclops.h
+++ /dev/null
@@ -1,201 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBiclops.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef __vpBiclops_H
-#define __vpBiclops_H
-
-/* ----------------------------------------------------------------------- */
-/* --- INCLUDES -------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* --- ViSP --- */
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPoseVector.h>
-#include <visp/vpMath.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-/* --- GENERAL --- */
-#include <iostream>
-
-/*!
-
- \class vpBiclops
-
- \ingroup Biclops
-
- \brief Jacobian, geometric model functionnalities... for biclops, pan, tilt
- head.
-
- Two different Denavit Hartenberg representations of the robot are implemented.
- As mentionned in vpBiclops::DenavitHartenbergModel they differ in the orientation
- of the tilt axis. Use setDenavitHartenbergModel() to select the representation.
-
- See http://www.traclabs.com/tracbiclops.htm for more details concerning the hardware.
-
-*/
-
-class VISP_EXPORT vpBiclops
-{
-public:
- /*!
- Two different Denavit Hartenberg representations of the robot are implemented.
- They differ in the orientation of the tilt axis.
-
- The first representation, vpBiclops::DH1 is given by:
-
- \f[
- \begin{tabular}{|c|c|c|c|c|}
- \hline
- Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
- \hline
- 1 & 0 & 0 & $-\pi/2$ & $q_1^*$ \\
- 2 & 0 & 0 & $ \pi/2$ & $q_2^* + \pi/2$ \\
- \hline
- \end{tabular}
- \f]
-
- The second one, vpBiclops::DH2 is given by:
-
- \f[
- \begin{tabular}{|c|c|c|c|c|}
- \hline
- Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
- \hline
- 1 & 0 & 0 & $ \pi/2$ & $q_1^*$ \\
- 2 & 0 & 0 & $ -\pi/2$ & $q_2^* - \pi/2$ \\
- \hline
- \end{tabular}
- \f]
-
- where \f$q_1^*, q_2^*\f$ are respectively the pan and tilt joint positions.
-
- In those representations, the pan is oriented from left to right, while the tilt is oriented
- - in vpBiclops::DH1 from down to top,
- - in vpBiclops::DH2 from top to down.
-
-
- */
- typedef enum
- {
- DH1, /*!< First Denavit Hartenberg representation. */
- DH2 /*!< Second Denavit Hartenberg representation. */
- } DenavitHartenbergModel;
-
-public: /* Constants */
-
- static const unsigned int ndof; /*!< Number of dof */
-
- /* Geometric model */
- static const float h;
-
- static const float panJointLimit;
- static const float tiltJointLimit;
- static const float speedLimit;
-
-
-protected:
- DenavitHartenbergModel dh_model_;
- vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
-
-public:
- vpBiclops (void);
- /*! Destructor that does nothing. */
- virtual ~vpBiclops() {};
-
- void init (void);
-
- void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
- vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
- void computeMGD (const vpColVector &q, vpPoseVector &fvc) const;
-
- /*!
- Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame and the end
- effector frame.
- */
- vpHomogeneousMatrix get_cMe() const
- {
- return cMe_;
- }
-
- void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const;
- void get_fMc (const vpColVector &q, vpPoseVector &fvc) const;
- vpHomogeneousMatrix get_fMc (const vpColVector &q) const;
- vpHomogeneousMatrix get_fMe (const vpColVector &q) const;
-
- void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
- void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
-
- /*!
- Return the Denavit Hartenberg representation used to model the head.
- \sa vpBiclops::DenavitHartenbergModel
- */
- inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const
- {
- return dh_model_;
- }
-
- void set_cMe() ;
- /*!
- Set the transformation between the camera frame and the end effector frame.
- */
- void set_cMe(const vpHomogeneousMatrix &cMe)
- {
- cMe_ = cMe;
- }
- /*!
- Set the Denavit Hartenberg representation used to model the head.
-
- \sa vpBiclops::DenavitHartenbergModel
- */
- inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1)
- {
- dh_model_ = m;
- }
-
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & constant);
-};
-
-
-#endif
diff --git a/src/robot/real-robot/biclops/vpRobotBiclops.cpp b/src/robot/real-robot/biclops/vpRobotBiclops.cpp
deleted file mode 100644
index f1ef8fb..0000000
--- a/src/robot/real-robot/biclops/vpRobotBiclops.cpp
+++ /dev/null
@@ -1,1436 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotBiclops.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <signal.h>
-#include <errno.h>
-#include <string.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#include <visp/vpTime.h>
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_BICLOPS
-
-#include <visp/vpBiclops.h>
-#include <visp/vpRobotBiclops.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpExponentialMap.h>
-
-//#define VP_DEBUG // Activate the debug mode
-//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
-#include <visp/vpDebug.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------ */
-
-bool vpRobotBiclops::robotAlreadyCreated = false;
-const double vpRobotBiclops::defaultPositioningVelocity = 10.0;
-
-static pthread_mutex_t vpEndThread_mutex;
-static pthread_mutex_t vpShm_mutex;
-static pthread_mutex_t vpMeasure_mutex;
-
-/* ----------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-
-/*!
-
- Default constructor.
-
- Does nothing more than setting the default configuration file
- to /usr/share/BiclopsDefault.cfg.
-
- As shown in the following example,the turret need to be initialized
- using init() function.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotBiclops.h>
-
-int main()
-{
-#ifdef VISP_HAVE_BICLOPS
- vpRobotBiclops robot; // Use the default config file in /usr/share/BiclopsDefault.cfg"
-
- // Specify the config file location
- robot.setConfigFile("/usr/share/BiclopsDefault.cfg"); // Not mandatory since the file is the default one
-
- // Initialize the head
- robot.init();
-
- // Move the robot to a specified pan and tilt
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- vpColVector q(2);
- q[0] = vpMath::rad(20); // pan
- q[1] = vpMath::rad(40); // tilt
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
- return 0;
-}
- \endcode
-
-*/
-vpRobotBiclops::vpRobotBiclops ()
- :
- vpRobot ()
-{
- vpDEBUG_TRACE (12, "Begin default constructor.");
-
- vpRobotBiclops::robotAlreadyCreated = false;
- controlThreadCreated = false;
- setConfigFile("/usr/share/BiclopsDefault.cfg");
-
- // Initialize the mutex dedicated to she shm protection
- pthread_mutex_init (&vpShm_mutex, NULL);
- pthread_mutex_init (&vpEndThread_mutex, NULL);
- pthread_mutex_init (&vpMeasure_mutex, NULL);
-
- positioningVelocity = defaultPositioningVelocity ;
-}
-
-/*!
-
- Default constructor.
-
- Initialize the biclops pan, tilt head by reading the
- configuration file provided by Traclabs
- and do the homing sequence.
-
- The following example shows how to use the constructor.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotBiclops.h>
-
-int main()
-{
-#ifdef VISP_HAVE_BICLOPS
- // Specify the config file location and initialize the turret
- vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg");
-
- // Move the robot to a specified pan and tilt
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
-
- vpColVector q(2);
- q[0] = vpMath::rad(-20); // pan
- q[1] = vpMath::rad(10); // tilt
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
- return 0;
-}
- \endcode
-
-*/
-vpRobotBiclops::vpRobotBiclops (const char * filename)
- :
- vpRobot ()
-{
- vpDEBUG_TRACE (12, "Begin default constructor.");
-
- vpRobotBiclops::robotAlreadyCreated = false;
- controlThreadCreated = false;
- setConfigFile(filename);
-
- // Initialize the mutex dedicated to she shm protection
- pthread_mutex_init (&vpShm_mutex, NULL);
- pthread_mutex_init (&vpEndThread_mutex, NULL);
- pthread_mutex_init (&vpMeasure_mutex, NULL);
-
- positioningVelocity = defaultPositioningVelocity ;
-
- init();
-
- return ;
-}
-
-/*!
-
- Destructor.
- Wait the end of the control thread.
-
-*/
-
-vpRobotBiclops::~vpRobotBiclops (void)
-{
-
- vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
- setRobotState(vpRobot::STATE_STOP) ;
-
- vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
- pthread_mutex_unlock(&vpEndThread_mutex);
-
- /* wait the end of the control thread */
- int code;
- vpDEBUG_TRACE (12, "Wait end of control thread");
-
- if (controlThreadCreated == true) {
- code = pthread_join(control_thread, NULL);
- if (code != 0) {
- vpCERROR << "Cannot terminate the control thread: " << code
- << " strErr=" << strerror(errno)
- << " strCode=" << strerror(code)
- << std::endl;
- }
- }
-
- pthread_mutex_destroy (&vpShm_mutex);
- pthread_mutex_destroy (&vpEndThread_mutex);
- pthread_mutex_destroy (&vpMeasure_mutex);
-
- vpRobotBiclops::robotAlreadyCreated = false;
-
- vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
- return;
-}
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INITIALISATION ------------------------------------------------------ */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- Set the Biclops config filename.
-
-*/
-void
-vpRobotBiclops::setConfigFile(const char * filename)
-{
- sprintf(configfile, "%s", filename);
-}
-
-/*!
-
- Set the Biclops config filename.
- Check if the config file exists and initialize the head.
-
- \exception vpRobotException::constructionError If the config file cannot be
- oppened.
-
-*/
-void
-vpRobotBiclops::init ()
-{
- // test if the config file exists
- FILE *fd = fopen(configfile, "r");
- if (fd == NULL) {
- vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with biclops");
- }
- fclose(fd);
-
- // Initialize the controller
- controller.init(configfile);
-
- try
- {
- setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- vpRobotBiclops::robotAlreadyCreated = true;
-
- // Initialize previous articular position to manage getDisplacement()
- q_previous.resize(vpBiclops::ndof);
- q_previous = 0;
-
- controlThreadCreated = false;
-
- return ;
-}
-
-/*
- Control loop to manage the biclops joint limits in speed control.
-
- This control loop is running in a seperate thread in order to detect each 5
- ms joint limits during the speed control. If a joint limit is detected the
- axis should be halted.
-
- \warning Velocity control mode is not exported from the top-level Biclops API
- class provided by Traclabs. That means that there is no protection in this
- mode to prevent an axis from striking its hard limit. In position mode,
- Traclabs put soft limits in that keep any command from driving to a position
- too close to the hard limits. In velocity mode this protection does not exist
- in the current API.
-
- \warning With the understanding that hitting the hard limits at full
- speed/power can damage the unit, damage due to velocity mode commanding is
- under user responsibility.
-*/
-void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
-{
- vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
-
- int iter = 0;
-// PMDAxisControl *panAxis = controller->getPanAxis();
-// PMDAxisControl *tiltAxis = controller->getTiltAxis();
- vpRobotBiclopsController::shmType shm;
-
- vpDEBUG_TRACE(10, "Start control loop");
- vpColVector mes_q;
- vpColVector mes_q_dot;
- vpColVector softLimit(vpBiclops::ndof);
- vpColVector q_dot(vpBiclops::ndof);
- bool *new_q_dot = new bool [ vpBiclops::ndof ];
- bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
- bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
- bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
- vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
- double secure = vpMath::rad(2); // add a security angle before joint limit
-
-
- // Set the soft limits
- softLimit[0] = vpBiclops::panJointLimit - secure;
- softLimit[1] = vpBiclops::tiltJointLimit - secure;
- vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
- vpMath::deg(softLimit[0]),
- vpMath::deg(softLimit[1]));
-
- // Initilisation
- vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
- shm = controller->readShm();
-
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- prev_q_dot [i] = shm.q_dot[i];
- new_q_dot [i] = false;
- change_dir [i] = false;
- force_halt [i] = false;
- enable_limit[i] = true;
- }
-
- // Initialize actual position and velocity
- mes_q = controller->getActualPosition();
- mes_q_dot = controller->getActualVelocity();
-
- vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
- shm = controller->readShm();
- // Updates the shm
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- shm.actual_q[i] = mes_q[i];
- shm.actual_q_dot[i] = mes_q_dot[i];
- }
- // Update the actuals positions
- controller->writeShm(shm);
-
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
- pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
-
- while (! controller->isStopRequested()) {
-
- // Get actual position and velocity
- mes_q = controller->getActualPosition();
- mes_q_dot = controller->getActualVelocity();
-
- vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
-
- shm = controller->readShm();
-
- // Updates the shm
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- shm.actual_q[i] = mes_q[i];
- shm.actual_q_dot[i] = mes_q_dot[i];
- }
-
- vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
- vpMath::deg(mes_q[0]),
- vpMath::deg(mes_q[1]));
- vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
- vpMath::deg(mes_q_dot[0]),
- vpMath::deg(mes_q_dot[1]));
- vpDEBUG_TRACE(12, "desired q_dot : %f %f",
- vpMath::deg(shm.q_dot[0]),
- vpMath::deg(shm.q_dot[1]));
- vpDEBUG_TRACE(13, "previous q_dot : %f %f",
- vpMath::deg(prev_q_dot[0]),
- vpMath::deg(prev_q_dot[1]));
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- // test if joint limits are reached
- if (mes_q[i] < -softLimit[i]) {
- vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
- shm.status[i] = vpRobotBiclopsController::STOP;
- shm.jointLimit[i] = true;
- }
- else if (mes_q[i] > softLimit[i]) {
- vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
- shm.status[i] = vpRobotBiclopsController::STOP;
- shm.jointLimit[i] = true;
- }
- else {
- shm.status[i] = vpRobotBiclopsController::SPEED;
- shm.jointLimit[i] = false;
- }
-
- // Test if new a speed is demanded
- //if (shm.q_dot[i] != prev_q_dot[i])
- if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
- new_q_dot[i] = true;
- else
- new_q_dot[i] = false;
-
- // Test if desired speed change of sign
- if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
- change_dir[i] = true;
- else
- change_dir[i] = false;
-
- }
- vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
- vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
- vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
- vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
- vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
- vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
-
-
- bool updateVelocity = false;
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- // Test if a new desired speed is to apply
- if (new_q_dot[i]) {
- // A new desired speed is to apply
- if (shm.status[i] == vpRobotBiclopsController::STOP) {
- // Axis in joint limit
- if (change_dir[i] == false) {
- // New desired speed without change of direction
- // We go in the joint limit
- if (enable_limit[i] == true) { // limit detection active
- // We have to stop this axis
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // We have to apply the desired speed to go away the joint
- // Update the desired speed
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- force_halt[i] = false;
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // New desired speed and change of direction.
- if (enable_limit[i] == true) { // limit detection active
- // Update the desired speed to go away the joint limit
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- force_halt[i] = false;
- enable_limit[i] = false; // Disable joint limit detection
- updateVelocity = true; // We have to send this new speed
- }
- else {
- // We have to stop this axis
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- enable_limit[i] = true; // Joint limit detection must be active
- updateVelocity = true; // We have to send this new speed
- }
- }
- }
- }
- else {
- // Axis not in joint limit
-
- // Update the desired speed
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- enable_limit[i] = true; // Joint limit detection must be active
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // No change of the desired speed. We have to stop the robot in case of
- // joint limit
- if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
- if (enable_limit[i] == true) { // limit detection active
-
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- // We have to stop this axis
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- updateVelocity = true; // We have to send this new speed
- }
- }
- }
- else {
- // No need to stop the robot
- enable_limit[i] = true; // Normal situation, activate limit detection
- }
- }
- }
- // Update the actuals positions
- controller->writeShm(shm);
-
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- if (updateVelocity) {
- vpDEBUG_TRACE(12, "apply q_dot : %f %f",
- vpMath::deg(q_dot[0]),
- vpMath::deg(q_dot[1]));
-
- // Apply the velocity
- controller -> setVelocity( q_dot );
- }
-
-
- // Update the previous speed for next iteration
- for (unsigned int i=0; i < vpBiclops::ndof; i ++)
- prev_q_dot[i] = shm.q_dot[i];
-
- vpDEBUG_TRACE(12, "iter: %d", iter);
-
- //wait 5 ms
- vpTime::wait(5.0);
-
-// if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
-// vpDEBUG_TRACE (12, "Calling thread will end");
-// vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
-// std::cout << "Calling thread will end" << std::endl;
-// std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
-//
-// pthread_mutex_unlock(&vpEndThread_mutex);
-// break;
-// }
-
- iter ++;
- }
- controller->stopRequest(false);
- // Stop the robot
- vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
- q_dot = 0;
- controller -> setVelocity( q_dot );
-
- delete [] new_q_dot;
- delete [] change_dir;
- delete [] force_halt;
- delete [] enable_limit;
- vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
- pthread_mutex_unlock(&vpEndThread_mutex);
-
- vpDEBUG_TRACE (10, "Exit control thread ");
- // pthread_exit(0);
-
- return NULL;
-}
-
-
-/*!
-
- Change the state of the robot either to stop them, or to set position or
- speed control.
-
-*/
-vpRobot::vpRobotStateType
-vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
-{
- switch (newState)
- {
- case vpRobot::STATE_STOP:
- {
- if (vpRobot::STATE_STOP != getRobotState ())
- {
- stopMotion();
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL:
- {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
- {
- vpDEBUG_TRACE (12, "Speed to position control.");
- stopMotion();
- }
-
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL:
- {
-
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
- pthread_mutex_lock(&vpEndThread_mutex);
-
- vpDEBUG_TRACE (12, "Create speed control thread");
- int code;
- code = pthread_create(&control_thread, NULL,
- &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop,
- &controller);
- if (code != 0) {
- vpCERROR << "Cannot create speed biclops control thread: " << code
- << " strErr=" << strerror(errno)
- << " strCode=" << strerror(code)
- << std::endl;
- }
-
- controlThreadCreated = true;
-
- vpDEBUG_TRACE (12, "Speed control thread created");
- }
- break;
- }
- default:
- break ;
- }
-
- return vpRobot::setRobotState (newState);
-}
-
-
-
-/*!
-
- Halt all the axis.
-
-*/
-void
-vpRobotBiclops::stopMotion(void)
-{
- vpColVector q_dot(vpBiclops::ndof);
- q_dot = 0;
- controller.setVelocity(q_dot);
- //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
- controller.stopRequest(true);
-}
-
-/*!
-
- Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cVe : Twist transformation between camera and end effector frame to
- expess a velocity skew from end effector frame in camera frame.
-
-*/
-void
-vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- cMe = vpBiclops::get_cMe() ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cMe : Homogeneous matrix between camera and end effector frame.
-
-*/
-void
-vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- cMe = vpBiclops::get_cMe() ;
-}
-
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- \warning Re is not the embedded camera frame. It corresponds to the frame
- associated to the tilt axis (see also get_cMe).
-
- \param eJe : Jacobian between end effector frame and end effector frame (on
- tilt axis).
-
-*/
-void
-vpRobotBiclops::get_eJe(vpMatrix &eJe)
-{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpBiclops::get_eJe(q,eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-
-/*!
- Get the robot jacobian expressed in the robot reference frame
-
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
-
-*/
-void
-vpRobotBiclops::get_fJe(vpMatrix &fJe)
-{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpBiclops::get_fJe(q,fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-
-}
-
-/*!
-
- Set the velocity used for a position control.
-
- \param velocity : Velocity in % of the maximum velocity between [0,100]. The
- maximum velocity is given vpBiclops::speedLimit.
-*/
-void
-vpRobotBiclops::setPositioningVelocity (const double velocity)
-{
- if (velocity < 0 || velocity > 100) {
- vpERROR_TRACE("Bad positionning velocity");
- throw vpRobotException (vpRobotException::constructionError,
- "Bad positionning velocity");
- }
-
- positioningVelocity = velocity;
-}
-/*!
- Get the velocity in % used for a position control.
-
- \return Positionning velocity in [0, 100.0]. The
- maximum positionning velocity is given vpBiclops::speedLimit.
-
-*/
-double
-vpRobotBiclops::getPositioningVelocity (void)
-{
- return positioningVelocity;
-}
-
-
-/*!
- Move the robot in position control.
-
- \warning This method is blocking. That mean that it waits the end of the
- positionning.
-
- \param frame : Control frame. This biclops head can only be controlled in
- articular.
-
- \param q : The position to set for each axis in radians.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-void
-vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & q )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot move the robot in camera frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in camera frame: "
- "not implemented");
- break;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot move the robot in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in reference frame: "
- "not implemented");
- break;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot move the robot in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in mixt frame: "
- "not implemented");
- break;
- case vpRobot::ARTICULAR_FRAME:
- break ;
- }
-
- // test if position reachable
-// if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
-// (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
-// vpERROR_TRACE ("Positionning error.");
-// throw vpRobotException (vpRobotException::wrongStateError,
-// "Positionning error.");
-// }
-
- vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
- pthread_mutex_lock(&vpEndThread_mutex);
- controller.setPosition( q, positioningVelocity );
- vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
- pthread_mutex_unlock(&vpEndThread_mutex);
- return ;
-}
-
-/*!
- Move the robot in position control.
-
- \warning This method is blocking. That mean that it wait the end of the
- positionning.
-
- \param frame : Control frame. This biclops head can only be controlled in
- articular.
-
- \param q1 : The pan position to set in radians.
- \param q2 : The tilt position to set in radians.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-void vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
- const double &q1, const double &q2)
-{
- try{
- vpColVector q(2) ;
- q[0] = q1 ;
- q[1] = q2 ;
-
- setPosition(frame,q) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Read the content of the position file and moves to head to articular
- position.
-
- \param filename : Position filename
-
- \exception vpRobotException::readingParametersError : If the articular
- position cannot be read from file.
-
- \sa readPositionFile()
-
-*/
-void
-vpRobotBiclops::setPosition(const char *filename)
-{
- vpColVector q ;
- if (readPositionFile(filename, q) == false) {
- vpERROR_TRACE ("Cannot get biclops position from file");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Cannot get biclops position from file");
- }
- setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Return the position of each axis.
- - In positionning control mode, call vpRobotBiclopsController::getPosition()
- - In speed control mode, call vpRobotBiclopsController::getActualPosition()
-
- \param frame : Control frame. This biclops head can only be controlled in
- articular.
-
- \param q : The position of the axis in radians.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-void
-vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector & q)
-{
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in camera frame: "
- "not implemented");
- break;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in reference frame: "
- "not implemented");
- break;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break;
- case vpRobot::ARTICULAR_FRAME:
- break ;
- }
-
- vpRobot::vpRobotStateType state;
- state = vpRobot::getRobotState();
-
- switch (state) {
- case STATE_STOP:
- case STATE_POSITION_CONTROL:
- q = controller.getPosition();
-
- break;
- case STATE_VELOCITY_CONTROL:
- case STATE_ACCELERATION_CONTROL:
- default:
- q.resize(vpBiclops::ndof);
-
- vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
- pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
-
- vpRobotBiclopsController::shmType shm;
-
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
- shm = controller.readShm();
-
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- q[i] = shm.actual_q[i];
- }
-
- vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
-
- vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
- pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
-
- break;
-
- }
-}
-
-
-/*!
-
- Send a velocity on each axis.
-
- \param frame : Control frame. This biclops head can only be controlled in
- articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference
- frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are
- not implemented.
-
- \param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot
- {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the
- camera and \f$ \dot{q}_2\f$ the tilt of the camera.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME or vpRobot::MIXT_FRAME) is
- given.
-
- \warning Velocities could be saturated if one of them exceed the maximal
- autorized speed (see vpRobot::maxRotationVelocity).
-
-*/
-void
-vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & q_dot)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the camera frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- if ( q_dot.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in articular frame");
- }
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the mixt frame:"
- "functionality not implemented");
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
- }
- }
-
- vpDEBUG_TRACE (12, "Velocity limitation.");
- bool norm = false; // Flag to indicate when velocities need to be nomalized
-
- // Saturate articular speed
- double max = vpBiclops::speedLimit;
- vpColVector q_dot_sat(vpBiclops::ndof);
-
- // init q_dot_saturated
- q_dot_sat = q_dot;
-
- for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
- {
- if (fabs (q_dot[i]) > max)
- {
- norm = true;
- max = fabs (q_dot[i]);
- vpERROR_TRACE ("Excess velocity: ROTATION "
- "(axe nr.%d).", i);
- }
- }
- // Rotations velocities normalisation
- if (norm == true) {
- max = vpBiclops::speedLimit / max;
- q_dot_sat = q_dot * max;
- }
-
- vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
-
- vpRobotBiclopsController::shmType shm;
-
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
- shm = controller.readShm();
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++)
- shm.q_dot[i] = q_dot[i];
-
- controller.writeShm(shm);
-
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- return;
-}
-
-
-/* ------------------------------------------------------------------------- */
-/* --- GET ----------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/*!
-
- Get the articular velocity.
-
- \param frame : Control frame. This head can only be controlled in articular.
-
- \param q_dot : The measured articular velocity in rad/s.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-*/
-void
-vpRobotBiclops::getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & q_dot)
-{
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in camera frame: "
- "not implemented");
- break;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in reference frame: "
- "not implemented");
- break;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break;
- case vpRobot::ARTICULAR_FRAME:
- break ;
- }
-
- vpRobot::vpRobotStateType state;
- state = vpRobot::getRobotState();
-
- switch (state) {
- case STATE_STOP:
- case STATE_POSITION_CONTROL:
- q_dot = controller.getVelocity();
-
- break;
- case STATE_VELOCITY_CONTROL:
- case STATE_ACCELERATION_CONTROL:
- default:
- q_dot.resize(vpBiclops::ndof);
-
- vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
- pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
-
- vpRobotBiclopsController::shmType shm;
-
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
- pthread_mutex_lock(&vpShm_mutex);
-
- shm = controller.readShm();
-
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
- pthread_mutex_unlock(&vpShm_mutex);
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- q_dot[i] = shm.actual_q_dot[i];
- }
-
- vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
-
- vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
- pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
-
- break;
-
- }
-}
-
-
-/*!
-
- Return the articular velocity.
-
- \param frame : Control frame. This head can only be controlled in articular.
-
- \return The measured articular velocity in rad/s.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-*/
-vpColVector
-vpRobotBiclops::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector q_dot;
- getVelocity (frame, q_dot);
-
- return q_dot;
-}
-
-/*!
-
- Get an articular position from the position file.
-
- \param filename : Position file.
-
- \param q : The articular position read in the file.
-
- \code
- # Example of biclops position file
- # The axis positions must be preceed by R:
- # First value : pan articular position in degrees
- # Second value: tilt articular position in degrees
- R: 15.0 5.0
- \endcode
-
- \return true if a position was found, false otherwise.
-
-*/
-bool
-vpRobotBiclops::readPositionFile(const char *filename, vpColVector &q)
-{
- FILE * pt_f ;
- pt_f = fopen(filename,"r") ;
-
- if (pt_f == NULL) {
- vpERROR_TRACE ("Can not open biclops position file %s", filename);
- return false;
- }
-
- char line[FILENAME_MAX];
- char head[] = "R:";
- bool end = false;
-
- do {
- // skip lines begining with # for comments
- if (fgets (line, 100, pt_f) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // this line is not a comment
- if ( fscanf (pt_f, "%s", line) != EOF) {
- if ( strcmp (line, head) == 0)
- end = true; // robot position was found
- }
- else {
- fclose(pt_f);
- return (false); // end of file without position
- }
- }
- }
- else {
- fclose(pt_f);
- return (false);// end of file
- }
-
- }
- while ( end != true );
-
- double q1,q2;
- // Read positions
- if (fscanf(pt_f, "%lf %lf", &q1, &q2) == EOF) {
- fclose(pt_f);
- std::cout << "Cannot read joint positions." << std::endl;
- return false;
- }
- q.resize(vpBiclops::ndof) ;
-
- q[0] = vpMath::rad(q1) ; // Rot tourelle
- q[1] = vpMath::rad(q2) ;
-
- fclose(pt_f) ;
- return (true);
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param d The measured displacement in camera frame. The dimension of d is 6
- (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
- radians.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotBiclops::getCameraDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, d);
-
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param d The measured articular displacement. The dimension of d is 2 (the
- number of axis of the robot) with respectively d[0] (pan displacement),
- d[1] (tilt displacement)
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, d);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning The first call of this method gives not a good value for the
- displacement.
-
- \param frame The frame in which the measured displacement is expressed.
-
- \param d The displacement:
-
- - In articular, the dimension of q is 2 (the number of axis of the robot)
- with respectively d[0] (pan displacement), d[1] (tilt displacement).
-
- - In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz).
- Translations are expressed in meters, rotations in radians with the theta U
- representation.
-
- \exception vpRobotException::wrongStateError If a not supported frame type is
- given.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &d)
-{
- vpColVector q_current; // current position
-
- getPosition(vpRobot::ARTICULAR_FRAME, q_current);
-
- switch(frame) {
- case vpRobot::ARTICULAR_FRAME:
- d.resize(vpBiclops::ndof);
- d = q_current - q_previous;
- break ;
-
- case vpRobot::CAMERA_FRAME: {
- d.resize(6);
- vpHomogeneousMatrix fMc_current;
- vpHomogeneousMatrix fMc_previous;
- fMc_current = vpBiclops::get_fMc(q_current);
- fMc_previous = vpBiclops::get_fMc(q_previous);
- vpHomogeneousMatrix c_previousMc_current;
- // fMc_c = fMc_p * c_pMc_c
- // => c_pMc_c = (fMc_p)^-1 * fMc_c
- c_previousMc_current = fMc_previous.inverse() * fMc_current;
-
- // Compute the instantaneous velocity from this homogeneous matrix.
- d = vpExponentialMap::inverse( c_previousMc_current );
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the reference frame:"
- "functionality not implemented");
- break ;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the mixt frame:"
- "functionality not implemented");
- break ;
- }
-
-
- q_previous = q_current; // Update for next call of this method
-
-}
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/biclops/vpRobotBiclops.h b/src/robot/real-robot/biclops/vpRobotBiclops.h
deleted file mode 100644
index cc749a4..0000000
--- a/src/robot/real-robot/biclops/vpRobotBiclops.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotBiclops.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_BICLOPS
-
-#ifndef __vpROBOT_BICLOPS_H
-#define __vpROBOT_BICLOPS_H
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INCLUDES ----------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/* --- GENERAL --- */
-#include <iostream>
-#include <stdio.h>
-#include <pthread.h>
-
-
-/* --- ViSP --- */
-#include <visp/vpRobot.h>
-#include <visp/vpBiclops.h>
-#include <visp/vpColVector.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpRobotBiclopsController.h>
-
-
-/* ------------------------------------------------------------------------ */
-/* --- CLASS -------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-
-/*!
-
-
- \class vpRobotBiclops
-
- \ingroup Biclops RobotDriver
-
- \brief Interface for the biclops, pan, tilt head control.
-
- See http://www.traclabs.com/biclopspt.html for more details.
-
- This class provide a position and a speed control interface for the biclops
- head. To manage the biclops joint limits in speed control, a control loop is
- running in a seperate thread (see vpRobotBiclopsSpeedControlLoop()).
-
- The control of the head is done by vpRobotBiclopsController class.
-
- \warning Velocity control mode is not exported from the top-level Biclops API
- class provided by Traclabs. That means that there is no protection in this
- mode to prevent an axis from striking its hard limit. In position mode,
- Traclabs put soft limits in that keep any command from driving to a position
- too close to the hard limits. In velocity mode this protection does not exist
- in the current API.
-
- \warning With the understanding that hitting the hard limits at full
- speed/power can damage the unit, damage due to velocity mode commanding is
- under user responsibility.
-
-
-*/
-class VISP_EXPORT vpRobotBiclops
- :
- public vpBiclops,
- public vpRobot
-{
-
-private:
-
- /*! \brief No copy constructor allowed. */
- vpRobotBiclops (const vpRobotBiclops & ass);
- void getCameraDisplacement(vpColVector &d);
- void getArticularDisplacement(vpColVector &d);
-
-private:
- static bool robotAlreadyCreated;
- pthread_t control_thread;
-
- char configfile[FILENAME_MAX]; // Biclops config file
-
- vpRobotBiclopsController controller;
-
- double positioningVelocity;
- vpColVector q_previous;
- bool controlThreadCreated;
-
-public:
-
- static const double defaultPositioningVelocity;
-
- vpRobotBiclops (void);
- vpRobotBiclops (const char * filename);
- virtual ~vpRobotBiclops (void);
-
- void init (void);
-
- void get_cMe(vpHomogeneousMatrix &_cMe) const;
- void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_eJe(vpMatrix &_eJe) ;
- void get_fJe(vpMatrix &_fJe) ;
-
- void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d);
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
- double getPositioningVelocity (void);
- void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
-
- bool readPositionFile(const char *filename, vpColVector &q) ;
-
- void setConfigFile (const char * filename="/usr/share/BiclopsDefault.cfg");
- void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) ;
- void setPosition (const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & q_dot);
-
- void stopMotion() ;
-
- static void * vpRobotBiclopsSpeedControlLoop (void * arg);
-};
-
-
-
-#endif /* #ifndef __vpROBOT_BICLOPS_H */
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/robot/real-robot/biclops/vpRobotBiclopsController.cpp b/src/robot/real-robot/biclops/vpRobotBiclopsController.cpp
deleted file mode 100644
index 1be2483..0000000
--- a/src/robot/real-robot/biclops/vpRobotBiclopsController.cpp
+++ /dev/null
@@ -1,494 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotBiclopsController.cpp 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <signal.h>
-#include <string.h>
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
-#endif
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_BICLOPS
-
-/* Headers */
-#include <visp/vpRobotBiclops.h>
-#include <visp/vpRobotBiclopsController.h>
-#include <visp/vpRobotException.h>
-
-//#define VP_DEBUG // Activate the debug mode
-//#define VP_DEBUG_MODE 20 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-
-
-/* ----------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
- Default constructor.
-*/
-vpRobotBiclopsController::vpRobotBiclopsController()
-{
- stopControllerThread_ = false;
- axisMask = Biclops::PanMask
- + Biclops::TiltMask
- /*+ Biclops::VergeMask*/; // add this if you want verge.
-
- panAxis = NULL;
- tiltAxis = NULL;
- vergeAxis = NULL;
-
- // Set Debug level depending on how much info you want to see about
- // the inner workings of the API. Level 2 is highest with 0 being
- // the default (i.e., no messages).
- biclops.SetDebugLevel(0);
-
- // initialize the shared data structure
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- shm.status[i] = STOP;
- shm.q_dot[i] = 0.;
- shm.actual_q[i] = 0.;
- shm.jointLimit[i] = false;
- shm.status[i] = STOP;
- }
-
-}
-
-/*!
-
- Destructor.
-
-*/
-vpRobotBiclopsController::~vpRobotBiclopsController()
-{
-}
-
-/*!
-
- Initialize the biclops by homing all axis.
-
- \param configfile : Biclops configuration file.
-
- \exception vpRobotException::notInitializedError If the biclops head connot
- be initialized. The initialization can failed,
- - if the head is not powered on,
- - if the head is not connected to your computer throw a serial cable,
- - if you try to open a bad serial port. Check you config file to verify
- which is the used serial port.
-*/
-void
-vpRobotBiclopsController::init(const char *configfile)
-{
- vpDEBUG_TRACE (12, "Initialize biclops.");
- bool binit = false;
- for (int i=0; i<1; i++) {
- try {
- std::cout << "Try to initialize biclops head " << std::endl;
- binit = biclops.Initialize(configfile);
- usleep(100000);
- if (binit) {
- // Initialization completed successfully. Close the config file.
- std::cout << "Initialization succeed...\n";
- break;
- }
- else {
- std::cout << "Initialization failed...\n";
- }
- }
- catch(...)
- {
- std::cout << "Initialization failed..."<< std::endl;
- }
- }
-
- if (! binit) {
- std::cout << "Cannot initialize biclops head. " << std::endl;
- std::cout << "Check if the serial cable is connected." << std::endl;
- std::cout << "Check if the robot is powered on." << std::endl;
- std::cout << "Check if you try to open the good serial port." << std::endl;
- std::cout << "Try to power off/on and restart..." << std::endl;
-
- throw vpRobotException (vpRobotException::notInitializedError,
- "Cannot initialize biclops head.");
-
- }
-
-
- vpDEBUG_TRACE(12, "Biclops initialized");
-
- // Get shortcut references to each axis.
- panAxis = biclops.GetAxis(Biclops::Pan);
- tiltAxis = biclops.GetAxis(Biclops::Tilt);
- if ((axisMask & Biclops::VergeMask) != 0)
- vergeAxis = biclops.GetAxis(Biclops::Verge);
-
-#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
- if (!panAxis -> GetHomedState() || !tiltAxis -> GetHomedState()) {
- vpDEBUG_TRACE(12, "Biclops is not homed");
- }
-#else // old API
- if (!panAxis -> IsAlreadyHomed() || !tiltAxis -> IsAlreadyHomed()) {
- vpDEBUG_TRACE(12, "Biclops is not homed");
- }
-#endif
-
- //Execute the homing sequence for all axes.
- vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
- vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
- if ( biclops.HomeAxes(axisMask))
- vpDEBUG_TRACE(12, "Homing sequence succeed.");
- else {
- vpERROR_TRACE("Homing sequence failed. Program is stopped");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with biclops");
- }
-
- // Get the currently defined (default) motion profiles.
- // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
- panAxis->GetProfile(panProfile);
- tiltAxis->GetProfile(tiltProfile);
- if ((axisMask & Biclops::VergeMask) != 0)
- vergeAxis->GetProfile(vergeProfile);
-}
-
-/*!
-
- Set the biclops axis position. The motion of the axis is synchronized to end
- on the same time.
-
- \warning Wait the end of the positionning.
-
- \param q : The position to set for each axis.
-
- \param percentVelocity : The velocity displacement to reach the new position
- in the range [0: 100.0]. 100 % corresponds to the maximal admissible
- speed. The maximal admissible speed is given by vpBiclops::speedLimit.
-
-*/
-
-void
-vpRobotBiclopsController::setPosition(const vpColVector & q,
- const double percentVelocity )
-{
- if (q.getRows() != vpBiclops::ndof )
- {
- vpERROR_TRACE ("Bad dimension for positioning vector.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad dimension for positioning vector.");
- }
-
- panAxis -> SetProfileMode(PMDTrapezoidalProfile);
- tiltAxis -> SetProfileMode(PMDTrapezoidalProfile);
-
- // Create the list of axes we want to coordinate
- PMDUtils::AxisList axisList;
- axisList.push_back(panAxis);
- axisList.push_back(tiltAxis);
-
-#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
- // Get the currently defined (default) motion profiles.
- PMDAxisControl::Profile panProfile,tiltProfile;
- panAxis->GetProfile(panProfile);
- tiltAxis->GetProfile(tiltProfile);
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- panProfile.pos = PMDUtils::RadsToRevs(q[0]);
- panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
- * percentVelocity / 100.);
-
- tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
- tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
- * percentVelocity / 100.);
-
- // Inform the controller of the new desired position.
- panAxis->SetProfile(panProfile);
- tiltAxis->SetProfile(tiltProfile);
-
-#else // old API
-
- PMDAxisControl::CountsProfile desired_profile;
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- panProfile.pos = PMDUtils::RadsToRevs(q[0]);
- panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
- * percentVelocity / 100.);
-
- vpDEBUG_TRACE(12, "Speed percent: %lf",
- vpBiclops::speedLimit * percentVelocity / 100.);
-
- panAxis -> ProfileToCounts(panProfile, desired_profile);
- vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
- vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
-
- panAxis -> SetProfile(desired_profile);
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
- tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
- * percentVelocity / 100.);
-
- tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
- vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
- vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
-
- tiltAxis -> SetProfile(desired_profile);
-#endif
-
- // Coordinate motion
- PMDUtils::Coordinate(axisList);
- biclops.Move(Biclops::PanMask + Biclops::TiltMask/*, 0*/); //
-
-}
-
-/*!
-
- Apply a velocity to each axis of the biclops robot.
-
- \warning This method is non blocking.
-
- \param q_dot : Velocity to apply.
-
-*/
-void
-vpRobotBiclopsController::setVelocity(const vpColVector & q_dot)
-{
- if (q_dot.getRows() != vpBiclops::ndof )
- {
- vpERROR_TRACE ("Bad dimension for velocity vector.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad dimension for velocity vector.");
- }
-
-
-#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
- // Get the currently defined (default) motion profiles.
- PMDAxisControl::Profile panProfile, tiltProfile;
- panAxis->GetProfile(panProfile);
- tiltAxis->GetProfile(tiltProfile);
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
- tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
-
- // Inform the controller of the new desired position.
- panAxis->SetProfile(panProfile);
- tiltAxis->SetProfile(tiltProfile);
-
- panAxis -> SetProfileMode(PMDVelocityContouringProfile);
- tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
-#else // old API
- panAxis -> SetProfileMode(PMDVelocityContouringProfile);
- tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
-\
- PMDAxisControl::CountsProfile desired_profile;
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
-
- panAxis -> ProfileToCounts(panProfile, desired_profile);
- panAxis -> SetProfile(desired_profile);
-
- // Set a position to move to by modifying the respective profiles.
- // NOTE: profile values are in revolutions, so here we convert
- // from degrees (divide by 360) for readability.
- tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
-
- tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
- tiltAxis -> SetProfile(desired_profile);
-#endif
- // Coordinate motion
- biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
-}
-
-/*!
-
- Get the biclops articular position.
-
- \return The axis articular position in radians.
-
-*/
-vpColVector
-vpRobotBiclopsController::getPosition()
-{
- vpDEBUG_TRACE (12, "Start vpRobotBiclopsController::getPosition() ");
- vpColVector q(vpBiclops::ndof);
- PMDint32 panpos, tiltpos;
-
- panAxis -> GetPosition(panpos);
- tiltAxis -> GetPosition(tiltpos);
-
- q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
- q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
-
- vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
- vpDEBUG_TRACE (12, "End vpRobotBiclopsController::getPosition()");
-
- return q;
-}
-
-/*!
-
- Get the biclops actual articular position.
-
- \return The axis actual articular position in radians.
-
-*/
-vpColVector
-vpRobotBiclopsController::getActualPosition()
-{
- vpColVector q(vpBiclops::ndof);
- PMDint32 panpos, tiltpos;
-
- panAxis -> GetActualPosition(panpos);
- tiltAxis -> GetActualPosition(tiltpos);
-
- q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
- q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
-
- return q;
-}
-
-/*!
-
- Get the biclops articular velocity.
-
- \return The axis articular velocity in rad/s.
-
-*/
-vpColVector
-vpRobotBiclopsController::getVelocity()
-{
- vpColVector q_dot(vpBiclops::ndof);
- PMDint32 pan_vel, tilt_vel;
-
- panAxis -> GetVelocity(pan_vel);
- tiltAxis -> GetVelocity(tilt_vel);
-
- q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
- q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
-
- return q_dot;
-}
-
-/*!
-
- Get the biclops actual articular velocity.
-
- \return The axis actual articular velocity in rad/s.
-
-*/
-vpColVector
-vpRobotBiclopsController::getActualVelocity()
-{
- vpColVector q_dot(vpBiclops::ndof);
- PMDint32 pan_vel, tilt_vel;
-
- panAxis -> GetActualVelocity(pan_vel);
- tiltAxis -> GetActualVelocity(tilt_vel);
-
- q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
- q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
-
- return q_dot;
-}
-
-/*!
-
- Update the shared memory.
-
- \param shm_ : Content to write in the shared memory.
-*/
-void
-vpRobotBiclopsController::writeShm(shmType &shm_)
-{
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
- vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
- }
- memcpy(&this->shm, &shm_, sizeof(shmType));
- //this->shm = shm_;
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
- vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
- }
-}
-
-/*!
-
- Get a copy of the shared memory.
-
- \return A copy of the shared memory.
-*/
-vpRobotBiclopsController::shmType
-vpRobotBiclopsController::readShm()
-{
- shmType tmp_shm;
-
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
- vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
- }
- memcpy(&tmp_shm, &this->shm, sizeof(shmType));
- //tmp_shm = shm;
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
- vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
- }
-
- return tmp_shm;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/biclops/vpRobotBiclopsController.h b/src/robot/real-robot/biclops/vpRobotBiclopsController.h
deleted file mode 100644
index b615ccf..0000000
--- a/src/robot/real-robot/biclops/vpRobotBiclopsController.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotBiclopsController.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Biclops robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_BICLOPS
-
-#ifndef __vpROBOT_BICLOPS_CONTROLLER_H
-#define __vpROBOT_BICLOPS_CONTROLLER_H
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INCLUDES ----------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/* --- GENERAL --- */
-#include <iostream> /* Classe std::ostream. */
-#include <stdio.h> /* Classe std::ostream. */
-#include <pthread.h> /* Classe std::ostream. */
-
-#include <visp/vpConfig.h>
-
-#include "Biclops.h" // Contrib for Biclops robot
-#include "PMDUtils.h" // Contrib for Biclops robot
-
-#if defined(_WIN32)
-class VISP_EXPORT Biclops; //needed for dll creation
-#endif
-
-/* ------------------------------------------------------------------------ */
-/* --- CLASS ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- \class vpRobotBiclopsController
-
- \ingroup Biclops
-
- \brief Interface to Biclops, pan, tilt, verge head for computer vision
- applications.
-
- See http://www.traclabs.com/tracbiclops.htm for more details.
-
- This class uses libraries libBiclops.so, libUtils.so and libPMD.so and
- includes Biclops.h and PMDUtils.h provided by Traclabs.
-
-*/
-class VISP_EXPORT vpRobotBiclopsController
-{
-public:
- typedef enum {
- STOP, /*!< Have to stop the robot. */
- SPEED /*!< Can send the desired speed. */
- } vpControllerStatusType;
-
-public:
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- // SHM
- typedef struct /* ControllerShm_struct */ {
- vpControllerStatusType status[2];
- double q_dot[2]; /*!< Desired speed. */
- double actual_q[2]; /*!< Current measured position of each axes. */
- double actual_q_dot[2]; /*!< Current measured velocity of each axes. */
- bool jointLimit[2]; /*!< Indicates if an axe is in joint limit. */
- } shmType;
-#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-
-public:
- vpRobotBiclopsController();
- virtual ~vpRobotBiclopsController();
- void init(const char *configfile);
- void setPosition(const vpColVector & q, const double percentVelocity);
- void setVelocity(const vpColVector & q_dot);
- vpColVector getPosition();
- vpColVector getActualPosition();
- vpColVector getVelocity();
- vpColVector getActualVelocity();
- PMDAxisControl * getPanAxis() { return panAxis; };
- PMDAxisControl * getTiltAxis() { return tiltAxis; };
- PMDAxisControl * getVergeAxis() { return vergeAxis; };
- void writeShm(shmType &shm);
- shmType readShm();
- bool isStopRequested()
- {
- return stopControllerThread_;
- }
-
- void stopRequest(bool stop)
- {
- stopControllerThread_ = stop;
- }
-
-private:
- Biclops biclops; // THE interface to Biclops.
- int axisMask;
-
- // Pointers to each axis (populated once controller is initialized).
- PMDAxisControl *panAxis;
- PMDAxisControl *tiltAxis;
- PMDAxisControl *vergeAxis;
-
- PMDAxisControl::Profile panProfile;
- PMDAxisControl::Profile tiltProfile;
- PMDAxisControl::Profile vergeProfile;
-
- shmType shm;
- bool stopControllerThread_;
-
-};
-
-
-#endif /* #ifndef __vpROBOT_BICLOPS_CONTROLLER_H */
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/robot/real-robot/pioneer/vpPioneer.h b/src/robot/real-robot/pioneer/vpPioneer.h
deleted file mode 100644
index 9dc2727..0000000
--- a/src/robot/real-robot/pioneer/vpPioneer.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPioneer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Common features for Pioneer unicycle mobile robots.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef VPPIONEER_H
-#define VPPIONEER_H
-
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpUnicycle.h>
-
-
-/*!
-
- \class vpPioneer
-
- \ingroup Pioneer
-
- \brief Generic functions for Pioneer mobile robots.
-
- This class provides common features for Pioneer mobile robots.
- This robot has two control velocities \f$(v_x, w_z)\f$, the translational and
- rotational velocities of the mobile platform respectively.
-
- The figure below shows the position of the frames that are used to model the robot.
- The end effector frame is here located at the middle point between the two wheels.
-
- \image html pioneer.png
-
- The robot jacobian at the end effector frame, the point located at the
- middle between the two wheels is given by:
-
- \f[
- {^e}{\bf J}_e = \left(\begin{array}{cc}
- 1 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 1 \\
- \end{array}
- \right)
- \f]
-
- Considering \f$(v_x, w_z)\f$, it is possible to compute \f$\bf v\f$ the six
- dimention velocity skew expressed at the end effector frame by:
-
- \f[
- {\bf v} = {^e}{\bf J}_e \;
- \left(\begin{array}{c}
- v_x \\
- w_z \\
- \end{array}
- \right)
- \f].
-
-*/
-class VISP_EXPORT vpPioneer: public vpUnicycle
-{
-public:
- /*!
- Create a default Pioneer robot.
- */
- vpPioneer() : vpUnicycle()
- {
- set_cMe();
- set_eJe();
- }
-
- /*!
- Destructor that does nothing.
- */
- virtual ~vpPioneer() {};
-
-private:
- /*!
- Set the transformation between the camera frame and the mobile platform
- end effector frame.
- */
- void set_cMe()
- {
- // Position of the camera in the mobile platform frame
- double l = 0.13; // distance between the camera frame and the mobile robot frame
- vpTranslationVector cte; // meters
- vpRxyzVector cre; // radian
- cte.set(0, 0, -l);
- cre.buildFrom(vpMath::rad(90.), 0, vpMath::rad(90.));
- cMe_.buildFrom(cte, vpRotationMatrix(cre));
- }
-
- /*!
- Set the robot jacobian at the end effector frame, the point located at the
- middle between the two wheels.
-
- Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z]\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile robot and \f$\bf v\f$ the six dimention velocity skew expressed at the
- end effector frame, the robot jacobian is given by:
-
- \f[
- {^e}{\bf J}_e = \left(\begin{array}{cc}
- 1 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 0 \\
- 0 & 1 \\
- \end{array}
- \right)
- \f]
-
- */
- void set_eJe()
- {
- eJe_.resize(6, 2); // pioneer jacobian expressed at point M
- eJe_ = 0;
- eJe_[0][0] = 1; // vx
- eJe_[5][1] = 1; // wz
- }
-};
-
-#endif
-
diff --git a/src/robot/real-robot/pioneer/vpPioneerPan.h b/src/robot/real-robot/pioneer/vpPioneerPan.h
deleted file mode 100644
index 5d1f6d9..0000000
--- a/src/robot/real-robot/pioneer/vpPioneerPan.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPioneerPan.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Common features for Pioneer unicycle mobile robots.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef VPPIONEERPAN_H
-#define VPPIONEERPAN_H
-
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpUnicycle.h>
-
-
-/*!
-
- \class vpPioneerPan
-
- \ingroup Pioneer
-
- \brief Generic functions for Pioneer mobile robots equiped with a pan head.
-
- This class provides common features for Pioneer mobile robots equiped with a pan head.
-
- This robot has three control velocities \f$(v_x, w_z, \dot{q_1})\f$, the translational and
- rotational velocities of the mobile platform, the pan head velocity respectively.
-
- The figure below shows the position of the frames that are used to model the robot.
- The end effector frame is here located at the pan axis.
-
- \image html pioneer-pan.png
-
- Considering
- \f[{\bf v} = {^e}{\bf J}_e \;
- \left(\begin{array}{c}
- v_x \\
- w_z \\
- \dot{q_1} \\
- \end{array}
- \right)
- \f]
- with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
- the six dimention velocity skew expressed at point E in frame E,
- the robot jacobian is given by:
-
- \f[
- {^e}{\bf J}_e = \left(\begin{array}{ccc}
- c_1 & -c_1*p_y - s_1*p_x & 0 \\
- 0 & 0 & 0 \\
- s_1 & -s_1*p_y + c_1*p_x & 0 \\
- 0 & 0 & 0 \\
- 0 & -1 & 1 \\
- 0 & 0 & 0 \\
- \end{array}
- \right)
- \f]
-
- with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
- located at the middle point between the two weels.
-
-*/
-class VISP_EXPORT vpPioneerPan: public vpUnicycle
-{
-public:
- /*!
- Create a pioneer mobile robot equiped with a pan head.
- */
- vpPioneerPan() : mMp_(), pMe_()
- {
- double q = 0; // Initial position of the pan axis
- set_mMp();
- set_pMe( q );
- set_cMe();
- set_eJe( q );
- }
-
- /*!
- Destructor that does nothing.
- */
- virtual ~vpPioneerPan() {};
-
- /*!
- Set the robot jacobian expressed at point E the end effector frame located on the pan head.
-
- Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z, \dot{q_1}]\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
- the six dimention velocity skew expressed at point E in frame E,
- the robot jacobian is given by:
-
- \f[
- {^e}{\bf J}_e = \left(\begin{array}{ccc}
- c_1 & -c_1*p_y - s_1*p_x & 0 \\
- 0 & 0 & 0 \\
- s_1 & -s_1*p_y + c_1*p_x & 0 \\
- 0 & 0 & 0 \\
- 0 & -1 & 1 \\
- 0 & 0 & 0 \\
- \end{array}
- \right)
- \f]
-
- with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
- located at the middle point between the two weels.
-
- */
- void set_eJe(double q_pan)
- {
- double px = mMp_[0][3];
- double py = mMp_[1][3];
- double c1 = cos(q_pan);
- double s1 = sin(q_pan);
-
- eJe_.resize(6,3); // robot jacobian expressed at the pan head end effector
-
- eJe_ = 0;
- eJe_[0][0] = c1;
- eJe_[0][1] = -c1*py - s1*px;
-
- eJe_[2][0] = s1;
- eJe_[2][1] = -s1*py + c1*px;
-
- eJe_[4][1] = -1;
- eJe_[4][2] = 1;
- }
-
-protected:
- /*!
- Set the transformation between the camera frame and the pan head end effector frame.
- */
- void set_cMe()
- {
- // Position of the camera in the pan frame
- double cx = 0;
- double cy = -0.065; // distance between camera and tilt axis
- double cz = 0;
- vpTranslationVector etc(cx, cy, cz);
- vpRotationMatrix eRc;
- eRc[0][0] = eRc[1][1] = eRc[2][2] = 0;
- eRc[0][2] = 1;
- eRc[1][1] = 1;
- eRc[2][0] = -1;
-
- vpHomogeneousMatrix eMc;
- eMc.buildFrom(etc, eRc);
-
- cMe_ = eMc.inverse();
- }
-
- /*!
- Set the transformation between the mobile platform frame
- located at the middle point between the two weels and the base frame of the pan head.
- */
- void set_mMp()
- {
- // Position of the pan head in the mobile platform frame
- double px = 0.103; // distance between the pan frame and the robot frame
- double py = 0;
- double pz = 0.27;
- vpTranslationVector mtp;
- mtp.set(px, py, pz);
-
- vpRotationMatrix mRp; // set to Identity
- mRp[1][1] = mRp[2][2] = -1.;
-
- mMp_.insert(mtp);
- mMp_.insert(mRp);
- }
-
- /*!
- Set the transformation between the pan head reference frame and the
- end-effector frame.
-
- \param q : Position in rad of the pan axis.
-
- */
- void set_pMe(const double q)
- {
- vpRotationMatrix pRe;
- pRe[0][0] = cos(q);
- pRe[0][2] = pRe[1][0] = sin(q);
- pRe[1][1] = pRe[2][2] = 0.;
- pRe[2][1] = 1.;
- pRe[1][2] = - pRe[0][0];
-
- pMe_.insert(pRe);
- }
-
-protected:
- vpHomogeneousMatrix mMp_; // constant
- vpHomogeneousMatrix pMe_; // depends on q pan
-
-};
-
-#endif
-
diff --git a/src/robot/real-robot/pioneer/vpRobotPioneer.cpp b/src/robot/real-robot/pioneer/vpRobotPioneer.cpp
deleted file mode 100644
index bd72577..0000000
--- a/src/robot/real-robot/pioneer/vpRobotPioneer.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotPioneer.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for Pioneer robots based on Aria 3rd party library.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpMath.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpRobotPioneer.h>
-
-#ifdef VISP_HAVE_PIONEER
-
-/*!
- Default constructor that initializes Aria.
- */
-vpRobotPioneer::vpRobotPioneer() : vpPioneer(), ArRobot()
-{
- isInitialized = false;
-
- Aria::init();
-}
-
-/*!
- Destructor.
- */
-vpRobotPioneer::~vpRobotPioneer()
-{
-#if 0
- std::cout << "Ending robot thread..." << std::endl;
- stopRunning();
-
- // wait for the thread to stop
- waitForRunExit();
-#endif
-}
-
-/*!
- Set the velocity (frame has to be specified) that will be applied to the robot.
-
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
- rotational velocities are implemented.
-
- \param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
- independently.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
-
- Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
- which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
- the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
- */
-void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
-{
- init();
-
- /*
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- } */
-
- if (vel.size() != 2)
- {
- throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
- }
-
- vpColVector vel_max(2);
- vpColVector vel_sat;
-
- if (frame == vpRobot::REFERENCE_FRAME)
- {
- vel_max[0] = getMaxTranslationVelocity();
- vel_max[1] = getMaxRotationVelocity();
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
- this->lock();
- this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
- this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
- this->unlock();
- }
- else if (frame == vpRobot::ARTICULAR_FRAME)
- {
- vel_max[0] = getMaxTranslationVelocity();
- vel_max[1] = getMaxTranslationVelocity();
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
- this->lock();
- //std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
- this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
- this->unlock();
- }
- else
- {
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send the robot velocity in the specified control frame");
- }
-}
-
-/*!
- Initialize the robot.
- - Sets the robot in asynchronous mode by starting a low level thread. The robot will be stopped
- if there is no connection to the robot at any given point.
- - Enables the motors on the robot, if it is connected.
-
- */
-void vpRobotPioneer::init()
-{
- if ( ! isInitialized )
- {
- // Start the robot processing cycle running in the background.
- // True parameter means that if the connection is lost, then the
- // run loop ends.
- this->runAsync(true);
- this->lock();
- this->enableMotors();
- this->unlock();
-
- isInitialized = true;
- }
-}
-
-/*!
- Gets the current translational velocity of the robot.
-
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
- rotational velocities are implemented.
-
- \param velocity : A two dimension vector that corresponds to the current velocities applied to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
-
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
-
- \sa getVelocity(const vpRobot::vpControlFrameType)
- */
-void vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity)
-{
- init();
- velocity.resize(2);
-
- if (frame == vpRobot::ARTICULAR_FRAME)
- {
- this->lock();
- velocity[0] = this->getLeftVel() / 1000.;
- velocity[1] = this->getRightVel() / 1000;
- this->unlock();
- }
- else if (frame == vpRobot::REFERENCE_FRAME)
- {
- this->lock();
- velocity[0] = this->getVel() / 1000.;
- velocity[1] = vpMath::rad( this->getRotVel() );
- this->unlock();
- }
- else {
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get the robot volocity in the specified control frame");
- }
-}
-
-/*!
- Gets the current translational velocity of the robot.
-
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
- rotational velocities are implemented.
-
- \return A two dimension vector that corresponds to the current velocities applied to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
-
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
-
- \sa getVelocity(const vpRobot::vpControlFrameType, vpColVector &)
- */
-vpColVector vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame)
-{
- vpColVector velocity;
- getVelocity(frame, velocity);
- return velocity;
-}
-
-#endif
-
diff --git a/src/robot/real-robot/pioneer/vpRobotPioneer.h b/src/robot/real-robot/pioneer/vpRobotPioneer.h
deleted file mode 100644
index a18e198..0000000
--- a/src/robot/real-robot/pioneer/vpRobotPioneer.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotPioneer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for Pioneer mobile robots based on Aria 3rd party library.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef VPROBOTPIONEER_H
-#define VPROBOTPIONEER_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpRobot.h>
-#include <visp/vpPioneer.h>
-
-#ifdef VISP_HAVE_PIONEER
-
-#include <Aria.h>
-
-/*!
-
- \class vpRobotPioneer
-
- \ingroup Pioneer RobotDriver
-
- \brief Interface for Pioneer mobile robots based on Aria 3rd party library.
-
- This class provides a position and speed control interface for Pioneer mobile robots.
- It inherits from the Aria ArRobot class. For more information about the model of the robot,
- see vpPioneer documentation.
-
-*/
-class VISP_EXPORT vpRobotPioneer: public vpRobot, public vpPioneer, public ArRobot
-{
-private: /* Not allowed functions. */
-
- /*!
- Copy constructor not allowed.
- */
- vpRobotPioneer(const vpRobotPioneer &robot);
-
-public:
- vpRobotPioneer();
- virtual ~vpRobotPioneer();
-
- /*!
- Get the robot Jacobian expressed at point E, the point located at the
- middle between the two wheels.
-
- \param eJe : Robot jacobian such as \f$(v_x, w_z) = {^e}{\bf J}e \; {\bf v}\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile robot, \f$\bf v\f$ the six dimention velocity skew, and where
-
- \sa get_eJe()
-
- */
- void get_eJe(vpMatrix & eJe)
- {
- eJe = vpUnicycle::get_eJe();
- }
-
-private: // Set as private since not implemented
- /*!
- Get the robot Jacobian expressed in the robot reference (or world) frame.
- \warning Not implemented.
- */
- void get_fJe(vpMatrix & /*fJe*/) {} ;
-
- /*!
- Get a displacement (frame as to ve specified) between two successive position control.
- \warning Not implemented.
- */
- void getDisplacement(const vpRobot::vpControlFrameType /*frame*/, vpColVector &/*q*/) {};
-
-public:
- void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
-
-private: // Set as private since not implemented
- /*!
- Get the robot position (frame has to be specified).
- \warning Not implemented.
- */
- void getPosition(const vpRobot::vpControlFrameType /*frame*/, vpColVector &/*q*/) {};
-
-public:
- void init();
-
-private: // Set as private since not implemented
- /*!
- Set a displacement (frame has to be specified) in position control.
- \warning Not implemented.
- */
- void setPosition(const vpRobot::vpControlFrameType /*frame*/, const vpColVector &/*q*/) {};
-
-public:
- void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel);
-
- /*!
- Enable or disable sonar device usage.
- */
- void useSonar(bool usage)
- {
- this->comInt(ArCommands::SONAR, usage);
- }
-
-protected:
- bool isInitialized;
-};
-
-#endif
-
-#endif // VPROBOTPIONEER_H
-
diff --git a/src/robot/real-robot/pioneer/vpUnicycle.h b/src/robot/real-robot/pioneer/vpUnicycle.h
deleted file mode 100644
index eb4328d..0000000
--- a/src/robot/real-robot/pioneer/vpUnicycle.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpUnicycle.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Common features for unicycle mobile robots.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef VPUNICYCLE_H
-#define VPUNICYCLE_H
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-
-/*!
-
- \class vpUnicycle
-
- \ingroup Pioneer
-
- \brief Generic functions for unicycle mobile robots.
-
- This class provides common features for unicycle mobile robots.
-
-*/
-class VISP_EXPORT vpUnicycle
-{
-public:
- /*!
- Default constructor that does nothing.
- */
- vpUnicycle() : cMe_(), eJe_()
- {
- };
- /*!
- Destructor that does nothing.
- */
- virtual ~vpUnicycle() {};
-
- /*!
- Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame
- and the mobile robot end effector frame.
- */
- vpHomogeneousMatrix get_cMe() const
- {
- return cMe_;
- }
-
- /*!
-
- Return the twist transformation from camera frame to the mobile robot
- end effector frame. This transformation allows to compute a velocity expressed
- in the end effector frame into the camera frame.
- */
- vpVelocityTwistMatrix get_cVe() const
- {
- vpVelocityTwistMatrix cVe;
- cVe.buildFrom(cMe_) ;
- return cVe;
- }
-
- /*!
-
- Return the twist transformation from camera frame to the mobile robot
- end effector frame. This transformation allows to compute a velocity expressed
- in the end effector frame into the camera frame.
-
- \sa get_cVe()
- */
- void get_cVe(vpVelocityTwistMatrix &cVe) const
- {
- cVe = vpUnicycle::get_cVe();
- }
-
- /*!
- Return the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
-
- \return The robot jacobian such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
- \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
- */
- vpMatrix get_eJe() const
- {
- return eJe_;
- }
-
- /*!
- Set the transformation between the camera frame and the end effector frame.
- */
- void set_cMe(const vpHomogeneousMatrix &cMe)
- {
- cMe_ = cMe;
- }
-
- /*!
- Set the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
-
- \param eJe : The robot jacobian to set such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
- \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
- */
- void set_eJe(const vpMatrix &eJe)
- {
- eJe_ = eJe;
- }
-
-protected:
- vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
- vpMatrix eJe_; // Robot jacobian
-};
-
-#endif
-
-
diff --git a/src/robot/real-robot/ptu46/vpPtu46.cpp b/src/robot/real-robot/ptu46/vpPtu46.cpp
deleted file mode 100644
index 2fea9f6..0000000
--- a/src/robot/real-robot/ptu46/vpPtu46.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPtu46.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ptu-46 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-/* ----------------------------------------------------------------------- */
-/* --- INCLUDE ----------------------------------------------------------- */
-/* ----------------------------------------------------------------------- */
-
-
-#include <visp/vpPtu46.h>
-#include <visp/vpDebug.h>
-#include <visp/vpRobotException.h>
-
-
-/* Inclusion des fichiers standards. */
-#include <math.h>
-#include <visp/vpMath.h>
-
-/* ------------------------------------------------------------------------ */
-/* --- COMPUTE ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------ */
-const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
-const float vpPtu46::L = 0.0765f; /*! Horizontal offset along the last joint,
- from last joint to camera frame. */
-const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
- camera frame. */
-
-
-/*!
- Compute the direct geometric model of the camera: fMc
-
- \param q : Articular position for pan and tilt axis.
-
- \param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
-
-*/
-void
-vpPtu46::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
-{
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
- }
-
- double q1 = q[0]; // pan
- double q2 = q[1]; // tilt
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
-
- fMc[0][0] = s1;
- fMc[0][1] = c1*s2;
- fMc[0][2] = c1*c2;
- fMc[0][3] = -h*c1*s2 - L*s1;
-
- fMc[1][0] = -c1;
- fMc[1][1] = s1*s2;
- fMc[1][2] = s1*c2;
- fMc[1][3] = -h*s1*s2 + L*c1;
-
- fMc[2][0] = 0;
- fMc[2][1] = -c2;
- fMc[2][2] = s2;
- fMc[2][3] = h*c2;
-
- fMc[3][0] = 0;
- fMc[3][1] = 0;
- fMc[3][2] = 0;
- fMc[3][3] = 1;
-
- vpCDEBUG (6) << "Position de la camera: " << std::endl << fMc;
-
- return ;
-}
-
-/*!
- Return the direct geometric model of the camera: fMc
-
- \param q : Articular position for pan and tilt axis.
-
- \return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Describes the transformation between the robot reference
- frame (called fixed) and the camera frame.
-
-*/
-vpHomogeneousMatrix
-vpPtu46::computeMGD (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
-
- computeMGD (q, fMc);
-
- return fMc;
-}
-
-/*!
- Compute the direct geometric model of the camera in terms of pose vector.
-
- \param q : Articular position for pan and tilt axis.
-
- \param r : Pose vector corresponding to the transformation between the robot
- reference frame (called fixed) and the camera frame.
-
-*/
-void
-vpPtu46::computeMGD (const vpColVector & q, vpPoseVector & r) const
-{
- vpHomogeneousMatrix fMc;
-
- computeMGD (q, fMc);
- r.buildFrom(fMc.inverse());
-
- return ;
-}
-
-
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Default construtor. Call init().
-
-*/
-vpPtu46::vpPtu46 (void)
-{
- init();
-}
-/* ---------------------------------------------------------------------- */
-/* --- PRIVATE ---------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-
-/*!
- Initialization. Here nothing to do.
-
-*/
-void
-vpPtu46::init ()
-{
- return ;
-}
-
-
-/* ----------------------------------------------------------------------- */
-/* --- DISPLAY ----------------------------------------------------------- */
-/* ----------------------------------------------------------------------- */
-
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & /* constant */)
-{
- os
- << "Geometric parameters: " << std::endl
- << "L: "
- << "\t" << vpPtu46::L << std::endl
- << "h: "
- << "\t" << vpPtu46::h << std::endl;
-
- return os;
-}
-
-
-/*!
-
- Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cVe : Twist transformation between camera and end effector frame to
- expess a velocity skew from end effector frame in camera frame.
-
-*/
-void
-vpPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cMe : Homogeneous matrix between camera and end effector frame.
-
-*/
-void
-vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpHomogeneousMatrix eMc ;
-
- eMc[0][0] = 0;
- eMc[0][1] = -1;
- eMc[0][2] = 0;
- eMc[0][3] = h;
-
- eMc[1][0] = 1;
- eMc[1][1] = 0;
- eMc[1][2] = 0;
- eMc[1][3] = -L;
-
- eMc[2][0] = 0;
- eMc[2][1] = 0;
- eMc[2][2] = 1;
- eMc[2][3] = 0;
-
- eMc[3][0] = 0;
- eMc[3][1] = 0;
- eMc[3][2] = 0;
- eMc[3][3] = 1;
-
- cMe = eMc.inverse() ;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- \warning Re is not the embedded camera frame. It corresponds to the frame
- associated to the tilt axis (see also get_cMe).
-
- \param q : Articular position for pan and tilt axis.
-
- \param eJe : Jacobian between end effector frame and end effector frame (on
- tilt axis).
-
-*/
-void
-vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
-{
-
-
- eJe.resize(6,2) ;
-
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
- }
-
- double s2 = sin(q[1]) ;
- double c2 = cos(q[1]) ;
-
- eJe = 0;
-
- eJe[3][0] = c2;
- eJe[4][1] = 1;
- eJe[5][0] = s2;
-}
-
-/*!
- Get the robot jacobian expressed in the robot reference frame
-
- \param q : Articular position for pan and tilt axis.
-
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
-
-*/
-void
-vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
-{
-
- if (q.getRows() != 2) {
- vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
- throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
- }
-
- fJe.resize(6,2) ;
-
- double s1 = sin(q[0]) ;
- double c1 = cos(q[0]) ;
-
- fJe = 0;
-
- fJe[3][1] = s1;
- fJe[4][1] = -c1;
- fJe[5][0] = 1;
-}
diff --git a/src/robot/real-robot/ptu46/vpPtu46.h b/src/robot/real-robot/ptu46/vpPtu46.h
deleted file mode 100644
index 554fe89..0000000
--- a/src/robot/real-robot/ptu46/vpPtu46.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPtu46.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ptu-46 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifndef __vpPtu46_H
-#define __vpPtu46_H
-
-
-/* ----------------------------------------------------------------------- */
-/* --- INCLUDES -------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* --- GENERAL --- */
-#include <iostream>
-
-/* --- ViSP --- */
-#include <visp/vpConfig.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPoseVector.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-/*!
-
- \class vpPtu46
-
- \ingroup Ptu46
-
- \brief Jacobian, geometric model functionnalities... for ptu46, pan, tilt
- head from Directed Perception.
-
- See http://www.DPerception.com for more details.
-
-*/
-
-
-class VISP_EXPORT vpPtu46
-{
-
-public: /* Constants */
-
- /** Nombre d'articulations du robot. */
- static const unsigned int ndof; /*!< Number of dof */
-
- /** Geometric model */
- static const float L;
- static const float h;
-
-
-public: /* Methodes publiques */
-
- vpPtu46 (void);
- /*! Destructor that does nothing. */
- virtual ~vpPtu46() {};
-
- void init (void);
-
- void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
- vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
- void computeMGD (const vpColVector & q, vpPoseVector & r) const;
-
- void get_cMe(vpHomogeneousMatrix &_cMe) const;
- void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
- void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
-
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & constant);
-};
-
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/robot/real-robot/ptu46/vpRobotPtu46.cpp b/src/robot/real-robot/ptu46/vpRobotPtu46.cpp
deleted file mode 100644
index 0edcb33..0000000
--- a/src/robot/real-robot/ptu46/vpRobotPtu46.cpp
+++ /dev/null
@@ -1,937 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotPtu46.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ptu-46 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <signal.h>
-#include <string.h>
-
-#include <visp/vpConfig.h>
-#ifdef VISP_HAVE_PTU46
-
-/* Headers des fonctions implementees. */
-#include <visp/vpPtu46.h>
-#include <visp/vpRobotPtu46.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------ */
-
-bool vpRobotPtu46::robotAlreadyCreated = false;
-const double vpRobotPtu46::defaultPositioningVelocity = 10.0;
-
-/* ----------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-
-/*!
-
- Default constructor.
-
- Initialize the ptu-46 pan, tilt head by opening the serial port.
-
- \sa init()
-
-*/
-vpRobotPtu46::vpRobotPtu46 (const char *device)
- :
- vpRobot ()
-{
- this->device = new char [FILENAME_MAX];
-
- sprintf(this->device, "%s", device);
-
- vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
- try
- {
- init();
- }
- catch(...)
- {
- delete [] this->device;
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- try
- {
- setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...)
- {
- delete [] this->device;
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- positioningVelocity = defaultPositioningVelocity ;
- return ;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- DESTRUCTOR -------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Destructor.
- Close the serial connection with the head.
-
-*/
-vpRobotPtu46::~vpRobotPtu46 (void)
-{
-
- setRobotState(vpRobot::STATE_STOP) ;
-
- if (0 != ptu.close())
- {
- vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
- }
-
- vpRobotPtu46::robotAlreadyCreated = false;
-
- delete [] device;
-
- return;
-}
-
-
-/* -------------------------------------------------------------------------- */
-/* --- INITIALISATION ------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
-
- Open the serial port.
-
-
- \exception vpRobotException::constructionError : If the device cannot be
- oppened.
-
-*/
-void
-vpRobotPtu46::init ()
-{
-
- vpDEBUG_TRACE (12, "Open connection Ptu-46.");
- if (0 != ptu.init(device) )
- {
- vpERROR_TRACE ("Cannot open connexion with ptu-46.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with ptu-46");
- }
-
- return ;
-}
-
-
-/*!
-
- Change the state of the robot either to stop them, or to set position or
- speed control.
-
-*/
-vpRobot::vpRobotStateType
-vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
-{
- switch (newState)
- {
- case vpRobot::STATE_STOP:
- {
- if (vpRobot::STATE_STOP != getRobotState ())
- {
- ptu.stop();
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL:
- {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
- {
- vpDEBUG_TRACE (12, "Passage vitesse -> position.");
- ptu.stop();
- }
- else
- {
- vpDEBUG_TRACE (1, "Passage arret -> position.");
- }
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL:
- {
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpDEBUG_TRACE (10, "Arret du robot...");
- ptu.stop();
- }
- break;
- }
- default:
- break ;
- }
-
- return vpRobot::setRobotState (newState);
-}
-
-/*!
-
- Halt all the axis.
-
-*/
-void
-vpRobotPtu46::stopMotion(void)
-{
- ptu.stop();
- setRobotState (vpRobot::STATE_STOP);
-}
-
-
-/*!
-
- Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cVe : Twist transformation between camera and end effector frame to
- expess a velocity skew from end effector frame in camera frame.
-
-*/
-void
-vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- vpPtu46::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
-
- \param cMe : Homogeneous matrix between camera and end effector frame.
-
-*/
-void
-vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpPtu46::get_cMe(cMe) ;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- \warning Re is not the embedded camera frame. It corresponds to the frame
- associated to the tilt axis (see also get_cMe).
-
- \param eJe : Jacobian between end effector frame and end effector frame (on
- tilt axis).
-
-*/
-void
-vpRobotPtu46::get_eJe(vpMatrix &eJe)
-{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpPtu46::get_eJe(q,eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-
-/*!
- Get the robot jacobian expressed in the robot reference frame
-
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
-
-*/
-void
-vpRobotPtu46::get_fJe(vpMatrix &fJe)
-{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpPtu46::get_fJe(q,fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-
-}
-
-
-/*!
-
- Set the velocity used for a position control.
-
- \param velocity : Velocity in % of the maximum velocity between [0,100].
-*/
-void
-vpRobotPtu46::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
-/*!
- Get the velocity in % used for a position control.
-
- \return Positionning velocity in [0, 100]
-
-*/
-double
-vpRobotPtu46::getPositioningVelocity (void)
-{
- return positioningVelocity;
-}
-
-
-/*!
- Move the robot in position control.
-
- \warning This method is blocking. That mean that it waits the end of the
- positionning.
-
- \param frame : Control frame. This head can only be controlled in
- articular.
-
- \param q : The position to set for each axis.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-
-void
-vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & q )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot move the robot in camera frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in camera frame: "
- "not implemented");
- break;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot move the robot in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in reference frame: "
- "not implemented");
- break;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot move the robot in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in mixt frame: "
- "not implemented");
- break;
- case vpRobot::ARTICULAR_FRAME:
- break ;
- }
-
- // Interface for the controller
- double artpos[2];
-
- artpos[0] = q[0];
- artpos[1] = q[1];
-
- if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error.");
- }
-
- return ;
-}
-
-
-/*!
- Move the robot in position control.
-
- \warning This method is blocking. That mean that it wait the end of the
- positionning.
-
- \param frame : Control frame. This head can only be controlled in
- articular.
-
- \param q1 : The pan position to set.
- \param q2 : The tilt position to set.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-void
-vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
- const double &q1, const double &q2)
-{
- try{
- vpColVector q(2) ;
- q[0] = q1 ;
- q[1] = q2 ;
-
- setPosition(frame,q) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Read the content of the position file and moves to head to articular
- position.
-
- \param filename : Position filename
-
- \exception vpRobotException::readingParametersError : If the articular
- position cannot be read from file.
-
- \sa readPositionFile()
-
-*/
-void
-vpRobotPtu46::setPosition(const char *filename)
-{
- vpColVector q ;
- if (readPositionFile(filename, q) == false) {
- vpERROR_TRACE ("Cannot get ptu-46 position from file");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Cannot get ptu-46 position from file");
- }
- setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Return the position of each axis.
-
- \param frame : Control frame. This head can only be controlled in
- articular.
-
- \param q : The position of the axis.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-
-*/
-void
-vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector & q)
-{
- vpDEBUG_TRACE (9, "# Entree.");
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in camera frame: "
- "not implemented");
- break;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in reference frame: "
- "not implemented");
- break;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break;
- case vpRobot::ARTICULAR_FRAME:
- break ;
- }
-
- double artpos[2];
-
- if (0 != ptu.getCurrentPosition( artpos ) )
- {
- vpERROR_TRACE ("Error when calling recup_posit_Afma4.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Error when calling recup_posit_Afma4.");
- }
-
- q.resize (vpPtu46::ndof);
-
- q[0] = artpos[0];
- q[1] = artpos[1];
-}
-
-
-/*!
-
- Send a velocity on each axis.
-
- \param frame : Control frame. This head can only be controlled in articular
- and camera frame. Be aware, the reference frame (vpRobot::REFERENCE_FRAME)
- and the mixt frame (vpRobot::MIXT_FRAME) are not implemented.
-
- \param v : The desired velocity of the axis. The size of this vector is
- always 2. Velocitoes are expressed in rad/s.
-
- - In camera frame, \f$ v = [\omega_x, \omega_y]^t \f$.
-
- - In articular, we control the 2 dof, \f$ v = [\dot{q}_1, \dot{q}_2]^t \f$
- with \f$ \dot{q}_1 \f$ the pan of the camera and \f$ \dot{q}_2\f$ the tilt of
- the camera.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- (vpRobot::REFERENCE_FRAME, vpRobot::MIXT_FRAME) is given.
-
- \warning Velocities could be saturated if one of them exceed the maximal
- autorized speed (see vpRobot::maxRotationVelocity).
-*/
-
-void
-vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & v)
-{
- TPtuFrame ptuFrameInterface;
-
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- ptuFrameInterface = PTU_CAMERA_FRAME;
- if ( v.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in camera frame");
- }
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- ptuFrameInterface = PTU_ARTICULAR_FRAME;
- if ( v.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in articular frame");
- }
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the mixt frame:"
- "functionality not implemented");
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
- }
- }
-
- vpDEBUG_TRACE (12, "Velocity limitation.");
- bool norm = false; // Flag to indicate when velocities need to be nomalized
- double ptuSpeedInterface[2];
-
- switch(frame) {
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::CAMERA_FRAME : {
- double max = this ->maxRotationVelocity;
- for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
- {
- if (fabs (v[i]) > max)
- {
- norm = true;
- max = fabs (v[i]);
- vpERROR_TRACE ("Excess velocity: ROTATION "
- "(axe nr.%d).", i);
- }
- }
- // Rotations velocities normalisation
- if (norm == true) {
- max = this ->maxRotationVelocity / max;
- for (unsigned int i = 0 ; i < 2; ++ i)
- ptuSpeedInterface [i] = v[i]*max;
- }
- break;
- }
- default:
- // Should never occur
- break;
-
- }
-
- vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
- << " " << ptuSpeedInterface[1] << std::endl;
- ptu.move(ptuSpeedInterface, ptuFrameInterface);
- return;
-}
-
-
-/* ------------------------------------------------------------------------- */
-/* --- GET ----------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/*!
-
- Get the articular velocity.
-
- \param frame : Control frame. This head can only be controlled in articular.
-
- \param q_dot : The measured articular velocity in rad/s.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-*/
-void
-vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & q_dot)
-{
-
- TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
-
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the camera frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::ARTICULAR_FRAME:
- {
- ptuFrameInterface = PTU_ARTICULAR_FRAME;
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- {
- vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the reference frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
-
- vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the mixt frame:"
- "functionality not implemented");
- break ;
- }
- }
-
- q_dot.resize(vpPtu46::ndof);
- double ptuSpeedInterface[2];
-
- ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
-
- q_dot[0] = ptuSpeedInterface[0];
- q_dot[1] = ptuSpeedInterface[1];
-
-}
-
-
-/*!
-
- Return the articular velocity.
-
- \param frame : Control frame. This head can only be controlled in articular.
-
- \return The measured articular velocity in rad/s.
-
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
-*/
-vpColVector
-vpRobotPtu46::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector q_dot;
- getVelocity (frame, q_dot);
-
- return q_dot;
-}
-
-/*!
-
- Get an articular position from the position file.
-
- \param filename : Position file.
-
- \param q : The articular position read in the file.
-
- \code
- # Example of ptu-46 position file
- # The axis positions must be preceed by R:
- # First value : pan articular position in degrees
- # Second value: tilt articular position in degrees
- R: 15.0 5.0
- \endcode
-
- \return true if a position was found, false otherwise.
-
-*/
-bool
-vpRobotPtu46::readPositionFile(const char *filename, vpColVector &q)
-{
- FILE * pt_f ;
- pt_f = fopen(filename,"r") ;
-
- if (pt_f == NULL) {
- vpERROR_TRACE ("Can not open ptu-46 position file %s", filename);
- return false;
- }
-
- char line[FILENAME_MAX];
- char head[] = "R:";
- bool end = false;
-
- do {
- // skip lines begining with # for comments
- if (fgets (line, 100, pt_f) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // this line is not a comment
- if ( fscanf (pt_f, "%s", line) != EOF) {
- if ( strcmp (line, head) == 0)
- end = true; // robot position was found
- }
- else
- return (false); // end of file without position
- }
- }
- else {
- return (false);// end of file
- }
-
- }
- while ( end != true );
-
- double q1,q2;
- // Read positions
- fscanf(pt_f, "%lf %lf", &q1, &q2);
- q.resize(vpPtu46::ndof) ;
-
- q[0] = vpMath::rad(q1) ; // Rot tourelle
- q[1] = vpMath::rad(q2) ;
-
- fclose(pt_f) ;
- return (true);
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param v The measured displacement in camera frame. The dimension of v is 6
- (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
- radians.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotPtu46::getCameraDisplacement(vpColVector &v)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, v);
-
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param d The measured articular displacement. The dimension of d is 2 (the
- number of axis of the robot) with respectively d[0] (pan displacement),
- d[1] (tilt displacement)
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void vpRobotPtu46::getArticularDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, d);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning The first call of this method gives not a good value for the
- displacement.
-
- \param frame The frame in which the measured displacement is expressed.
-
- \param d The displacement:
- - In articular, the dimension of q is 2 (the number of axis of the robot)
- with respectively d[0] (pan displacement), d[1] (tilt displacement).
- - In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians.
-
- \exception vpRobotException::wrongStateError If a not supported frame type is
- given.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &d)
-{
- double d_[6];
-
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- d.resize (6);
- ptu.measureDpl(d_, PTU_CAMERA_FRAME);
- d[0]=d_[0];
- d[1]=d_[1];
- d[2]=d_[2];
- d[3]=d_[3];
- d[4]=d_[4];
- d[5]=d_[5];
- break ;
- }
- case vpRobot::ARTICULAR_FRAME:
- {
- ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
- d.resize (vpPtu46::ndof);
- d[0]=d_[0]; // pan
- d[1]=d_[1]; // tilt
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- {
- vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a displacement in the reference frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a displacement in the reference frame:"
- "functionality not implemented");
-
- break ;
- }
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/real-robot/ptu46/vpRobotPtu46.h b/src/robot/real-robot/ptu46/vpRobotPtu46.h
deleted file mode 100644
index a5702d0..0000000
--- a/src/robot/real-robot/ptu46/vpRobotPtu46.h
+++ /dev/null
@@ -1,156 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotPtu46.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ptu-46 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#ifdef VISP_HAVE_PTU46
-
-
-#ifndef __vpROBOT_PTU46_H
-#define __vpROBOT_PTU46_H
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INCLUDES ----------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/* --- GENERAL --- */
-#include <iostream>
-#include <stdio.h>
-
-
-/* --- ViSP --- */
-#include <visp/vpRobot.h>
-#include <visp/vpPtu46.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-#include <ptu.h> // Contrib for Ptu-46 robot
-
-/* ------------------------------------------------------------------------ */
-/* --- CLASS ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-/*!
-
- \class vpRobotPtu46
-
- \ingroup Ptu46 RobotDriver
-
- \brief Interface for the Directed Perception ptu-46 pan, tilt head .
-
- See http://www.DPerception.com for more details.
-
- This class provide a position and a speed control interface for the ptu-46
- head.
-
-*/
-class VISP_EXPORT vpRobotPtu46
- :
- public vpPtu46,
- public vpRobot
-{
-
-private:
-
- /*! \brief No copy constructor allowed. */
- vpRobotPtu46 (const vpRobotPtu46 & ass);
-
- /*! Object to control. This is a contribution. */
- Ptu ptu;
-
-private:
- static bool robotAlreadyCreated;
- double positioningVelocity;
- int velocityMesureTempo;
- char *device;
-
-public:
- static const double defaultPositioningVelocity;
-
- vpRobotPtu46 (const char *device="/dev/ttyS0");
- vpRobotPtu46 (vpRobotPtu46 * pub);
- virtual ~vpRobotPtu46 (void);
-
- void get_cMe(vpHomogeneousMatrix &_cMe) const;
- void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_eJe(vpMatrix &_eJe);
- void get_fJe(vpMatrix &_fJe);
-
- void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
- double getPositioningVelocity (void);
- void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
-
- void init (void);
-
- bool readPositionFile(const char *filename, vpColVector &q);
-
- void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) ;
- void setPosition (const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
-
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & q_dot);
-
- void stopMotion() ;
-
-private:
- void getCameraDisplacement(vpColVector &d);
- void getArticularDisplacement(vpColVector &d);
-};
-
-
-
-#endif /* #ifndef __vpROBOT_PTU46_H */
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/robot/real-robot/viper/vpRobotViper650.cpp b/src/robot/real-robot/viper/vpRobotViper650.cpp
deleted file mode 100644
index bf2780b..0000000
--- a/src/robot/real-robot/viper/vpRobotViper650.cpp
+++ /dev/null
@@ -1,2348 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotViper650.cpp 4793 2014-07-21 15:10:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Viper S650 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_VIPER650
-
-#include <signal.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <visp/vpRobotException.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotViper650.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ----------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-bool vpRobotViper650::robotAlreadyCreated = false;
-
-/*!
-
- Default positioning velocity in percentage of the maximum
- velocity. This value is set to 15. The member function
- setPositioningVelocity() allows to change this value.
-
-*/
-const double vpRobotViper650::defaultPositioningVelocity = 15.0;
-
-/* ---------------------------------------------------------------------- */
-/* --- EMERGENCY STOP --------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Emergency stops the robot if the program is interrupted by a SIGINT
- (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
- or SIGQUIT signal.
-
-*/
-void emergencyStopViper650(int signo)
-{
- std::cout << "Stop the Viper650 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
-}
- //std::cout << "Emergency stop called\n";
- // PrimitiveESTOP_Viper650();
- PrimitiveSTOP_Viper650();
- std::cout << "Robot was stopped\n";
-
- // Free allocated resources
- // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
-
- fprintf(stdout, "Application ");
- fflush(stdout);
- kill(getpid(), SIGKILL);
- exit(1) ;
-}
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- The only available constructor.
-
- This contructor calls init() to initialise the connection with the
- MotionBox or low level controller, send the default \f$^e{\bf
- M}_c\f$ homogeneous matrix and power on the robot.
-
- It also set the robot state to vpRobot::STATE_STOP.
-
- To set the extrinsic camera parameters related to the \f$^e{\bf
- M}_c\f$ matrix obtained with a camera perspective projection model
- including the distorsion, use the code below:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters associated to an
- image acquired by the camera attached to the robot, with:
-
- \code
- vpImage<unsigned char> I(480, 640);
-
- // Get an image from the camera attached to the robot
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
- g.acquire(I);
-#endif
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
-#endif
-}
- \endcode
-
- \sa vpCameraParameters, init(vpViper650::vpViper650CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-
-*/
-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
- */
-
- signal(SIGINT, emergencyStopViper650);
- signal(SIGBUS, emergencyStopViper650) ;
- signal(SIGSEGV, emergencyStopViper650) ;
- signal(SIGKILL, emergencyStopViper650);
- signal(SIGQUIT, emergencyStopViper650);
-
- setVerbose(verbose);
- if (verbose_)
- std::cout << "Open communication with MotionBlox.\n";
- try {
- this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
- // vpERROR_TRACE("Error caught") ;
- throw ;
- }
- positioningVelocity = defaultPositioningVelocity ;
-
- maxRotationVelocity_joint6 = maxRotationVelocity;
-
- vpRobotViper650::robotAlreadyCreated = true;
-
- return ;
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INITIALISATION ----------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Initialise the connection with the MotionBox or low level
- controller, send the default eMc homogeneous matrix, power on the
- robot and wait 1 sec before returning to be sure the initialisation
- is done.
-
- \warning This method sets the camera extrinsic parameters (matrix
- eMc) to the one obtained by calibration with a camera projection
- model without distorsion by calling
- init(vpViper650::defaultCameraRobot). If you want to set the extrinsic
- camera parameters to those obtained with a camera perspective model
- including the distorsion you have to call the
- init(vpViper650::vpViper650CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType) method.
-
- \sa vpCameraParameters, init(vpViper650::vpViper650CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-*/
-void
- vpRobotViper650::init (void)
-{
- InitTry;
-
- // Initialise private variables used to compute the measured velocities
- q_prev_getvel.resize(6);
- q_prev_getvel = 0;
- time_prev_getvel = 0;
- first_time_getvel = true;
-
- // Initialise private variables used to compute the measured displacement
- q_prev_getdis.resize(6);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
- // Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
-
- // Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Viper650() );
-
- Try( PrimitiveRESET_Viper650() );
-
- // Enable the joint limits on axis 6
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
-
- // Update the eMc matrix in the low level controller
- init(vpViper650::defaultTool);
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // Print the robot status
- if (verbose_) {
- std::cout << "Robot status: ";
- switch(EStopStatus) {
- case ESTOP_AUTO:
- controlMode = AUTO;
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
-
- case ESTOP_MANUAL:
- controlMode = MANUAL;
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
- case ESTOP_ACTIVATED:
- controlMode = ESTOP;
- std::cout << "Emergency stop is activated" << std::endl;
- break;
- default:
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- }
- std::cout << std::endl;
- }
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
-
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
-
- // If an error occur in the low level controller, goto here
- //CatchPrint();
- Catch();
-
- // Test if an error occurs
- if (TryStt == -20001)
- printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
- else if (TryStt == -675)
- printf(" Timeout enabling power...\n");
-
- if (TryStt < 0) {
- // Power off the robot
- PrimitivePOWEROFF_Viper650();
- // Free allocated resources
- ShutDownConnection();
-
- std::cout << "Cannot open connexion with the motionblox..." << std::endl;
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
- return ;
-}
-
-/*!
-
- Initialize the robot kinematics with the extrinsic calibration
- parameters associated to a specific camera (set the eMc homogeneous
- parameters in the low level controller) and also get the joint
- limits from the low-level controller.
-
- The eMc parameters depend on the camera and the projection model in use.
-
- \param tool : Tool to use.
-
- \param projModel : Projection model associated to the camera.
-
- To set the extrinsic camera parameters related to the \f$^e{\bf
- M}_c\f$ matrix obtained with a camera perspective projection model
- including the distorsion, use the code below:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters associated to an
- image acquired by the camera attached to the robot, with:
-
- \code
- vpImage<unsigned char> I(480, 640);
-
- // Get an image from the camera attached to the robot
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
- g.acquire(I);
-#endif
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
-#endif
-}
- \endcode
-
- \sa vpCameraParameters, init()
-*/
-void
- vpRobotViper650::init (vpViper650::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel)
-{
-
- InitTry;
- // Read the robot constants from files
- // - joint [min,max], coupl_56, long_56
- // - camera extrinsic parameters relative to eMc
- vpViper650::init(tool, projModel);
-
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
-
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
-
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
-
- setToolType(tool);
-
- CatchPrint();
- return ;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- DESTRUCTOR --------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Destructor.
-
- Free allocated resources.
-*/
-vpRobotViper650::~vpRobotViper650 (void)
-{
- InitTry;
-
- setRobotState(vpRobot::STATE_STOP) ;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // if (HIPowerStatus == 1) {
- // fprintf(stdout, "Power OFF the robot\n");
- // fflush(stdout);
-
- // Try( PrimitivePOWEROFF_Viper650() );
- // }
-
- // Free allocated resources
- ShutDownConnection();
-
- vpRobotViper650::robotAlreadyCreated = false;
-
- CatchPrint();
- return;
-}
-
-
-
-
-/*!
-
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
- vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState)
-{
- InitTry;
-
- switch (newState) {
- case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- Try( PrimitiveSTOP_Viper650() );
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Viper650() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
- }
- default:
- break ;
- }
-
- CatchPrint();
-
- return vpRobot::setRobotState (newState);
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- STOP --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Stop the robot and set the robot state to vpRobot::STATE_STOP.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-*/
-void
- vpRobotViper650::stopMotion(void)
-{
- if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
- return;
-
- InitTry;
- Try( PrimitiveSTOP_Viper650() );
- setRobotState (vpRobot::STATE_STOP);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
- }
-}
-
-/*!
-
- Power on the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot power on.
-
- \sa powerOff(), getPowerState()
-*/
-void
- vpRobotViper650::powerOn(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- bool firsttime = true;
- unsigned int nitermax = 10;
-
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- if (EStopStatus == ESTOP_AUTO) {
- controlMode = AUTO;
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
- controlMode = MANUAL;
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
- controlMode = ESTOP;
- if (firsttime) {
- std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
- firsttime = false;
- }
- fprintf(stdout, "Remaining time %ds \r", nitermax-i);
- fflush(stdout);
- CAL_Wait(1);
- }
- else {
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- ShutDownConnection();
- exit(0);
- }
- }
-
- if (EStopStatus == ESTOP_ACTIVATED)
- std::cout << std::endl;
-
- if (EStopStatus == ESTOP_ACTIVATED) {
- std::cout << "Sorry, cannot power on the robot." << std::endl;
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power on the robot.");
- }
-
- if (HIPowerStatus == 0) {
- fprintf(stdout, "Power ON the Viper650 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWERON_Viper650() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Power off the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-
- \sa powerOn(), getPowerState()
-*/
-void
- vpRobotViper650::powerOff(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- fprintf(stdout, "Power OFF the Viper650 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWEROFF_Viper650() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Get the robot power state indication if power is on or off.
-
- \return true if power is on. false if power is off.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error.
-
- \sa powerOn(), powerOff()
-*/
-bool
- vpRobotViper650::getPowerState(void) const
-{
- InitTry;
- bool status = false;
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- status = true;
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
- }
- return status;
-}
-
-/*!
-
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
- end-effector frame. This transformation allows to compute a
- velocity expressed in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
- vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- vpViper650::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the geometric transformation \f$^c{\bf M}_e\f$ between the
- camera frame and the end-effector frame. This transformation is
- constant and correspond to the extrinsic camera parameters estimated
- by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
- vpRobotViper650::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpViper650::get_cMe(cMe) ;
-}
-
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame.
-
- To compute \f$^e{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param eJe : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
- end-effector frame.
-
-*/
-void
- vpRobotViper650::get_eJe(vpMatrix &eJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_J_Viper650(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = vpMath::rad(position[i]);
-
- try
- {
- vpViper650::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- To compute \f$^f{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param fJe : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
- reference frame.
-*/
-
-void
- vpRobotViper650::get_fJe(vpMatrix &fJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Viper650(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try
- {
- vpViper650::get_fJe(q, fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Set the maximal velocity percentage to use for a position control.
-
- The default positioning velocity is defined by
- vpRobotViper650::defaultPositioningVelocity. This method allows to
- change this default positioning velocity
-
- \param velocity : Percentage of the maximal velocity. Values should
- be in ]0:100].
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpColVector position(6);
- position = 0; // position in rad
-
- vpRobotViper650 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot to the joint position [0,0,0,0,0,0]
- robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
-#endif
-}
- \endcode
-
- \sa getPositioningVelocity()
-*/
-void
- vpRobotViper650::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
-
-/*!
- Get the maximal velocity percentage used for a position control.
-
- \sa setPositioningVelocity()
-*/
-double
- vpRobotViper650::getPositioningVelocity (void) const
-{
- return positioningVelocity;
-}
-
-
-/*!
-
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param position : A six dimension vector corresponding to the
- position to reach. All the positions are expressed in meters for the
- translations and radians for the rotations. If the position is out
- of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are the six joint rotations starting
- from the base to the end-effector.
-
- - In the camera and the reference frame, positions are respectively
- X,Y,Z translations and 3 rotations arround the X, Y and Z
- axis. Rotations are represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpColVector position(6);
- // Set positions in the camera frame
- position[0] = 0.1; // x axis, in meter
- position[1] = 0.2; // y axis, in meter
- position[2] = 0.3; // z axis, in meter
- position[3] = M_PI/8; // rotation around x axis, in rad
- position[4] = M_PI/4; // rotation around y axis, in rad
- position[5] = M_PI; // rotation around z axis, in rad
-
- vpRobotViper650 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
-#endif
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- \endcode
-
-*/
-void
- vpRobotViper650::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & position )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- vpColVector destination(njoint);
- int error = 0;
- double timestamp;
-
- InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- vpColVector q(njoint);
- Try( PrimitiveACQ_POS_Viper650(q.data, ×tamp) );
-
- // Convert degrees into rad
- q.deg2rad();
-
- // Get fMc from the inverse kinematics
- vpHomogeneousMatrix fMc;
- vpViper650::get_fMc(q, fMc);
-
- // Set cMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
-
- // Compute cMc2
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- // Compute the new position to reach: fMc*cMc2
- vpHomogeneousMatrix fMc2 = fMc * cMc2;
-
- // Compute the corresponding joint position from the inverse kinematics
- unsigned int solution = this->getInverseKinematics(fMc2, q);
- if (solution) { // Position is reachable
- destination = q;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
- Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
-
- break ;
- }
- case vpRobot::ARTICULAR_FRAME: {
- destination = position;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
-
- //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
- break ;
-
- }
- case vpRobot::REFERENCE_FRAME: {
- // Convert angles from Rxyz representation to Rzyz representation
- vpRxyzVector rxyz(position[3],position[4],position[5]);
- vpRotationMatrix R(rxyz);
- vpRzyzVector rzyz(R);
-
- for (unsigned int i=0; i <3; i++) {
- destination[i] = position[i];
- destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
- }
- int configuration = 0; // keep the actual configuration
-
- //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_C_Viper650(destination.data, configuration,
- positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
-
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt == InvalidPosition || TryStt == -1023)
- std::cout << " : Position out of range.\n";
-
- else if (TryStt == -3019) {
- if (frame == vpRobot::ARTICULAR_FRAME)
- std::cout << " : Joint position out of range.\n";
- else
- std::cout << " : Cartesian position leads to a joint position out of range.\n";
- }
- else if (TryStt < 0)
- std::cout << " : Unknown error (see Fabien).\n";
- else if (error == -1)
- std::cout << "Position out of range.\n";
-
- if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
-
- return ;
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- This method owerloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
- the position to reach. All the positions are expressed in meters for
- the translations and radians for the rotations.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X (pos1), Y (pos2),
- Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
- translations, and A,B,C the rotations of the end-effector.
-
- - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- // Set positions in the camera frame
- double pos1 = 0.1; // x axis, in meter
- double pos2 = 0.2; // y axis, in meter
- double pos3 = 0.3; // z axis, in meter
- double pos4 = M_PI/8; // rotation around x axis, in rad
- double pos5 = M_PI/4; // rotation around y axis, in rad
- double pos6 = M_PI; // rotation around z axis, in rad
-
- vpRobotViper650 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
-#endif
-}
- \endcode
-
- \sa setPosition()
-*/
-void vpRobotViper650::setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6)
-{
- try{
- vpColVector position(6) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpColVector q;
- vpRobotViper650 robot;
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-
-*/
-void vpRobotViper650::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
-
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Time in second since last robot power on.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
- double timestamp;
-
- vpColVector position;
- robot.getPosition(vpRobot::REFERENCE_FRAME, position, timestamp);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (unsigned int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame transformation in terms of a
- // homogenous matrix
- vpHomogeneousMatrix fMc(ftc, fRc);
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : If the position cannot
- be get from the low level controller.
-
- \sa setPosition(const vpRobot::vpControlFrameType frame, const
- vpColVector & r)
-
-*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position,
- double ×tamp)
-{
-
- InitTry;
-
- position.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- Try( PrimitiveACQ_POS_J_Viper650(position.data, ×tamp) );
- //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
- position.deg2rad();
-
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveACQ_POS_C_Viper650(position.data, ×tamp) );
- // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
- // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
- // Convert Euler Rzyz angles from deg to rad
- for (unsigned int i=3; i <6; i++)
- position[i] = vpMath::rad(position[i]);
- // Convert Rzyz angles into Rxyz representation
- vpRzyzVector rzyz(position[3], position[4], position[5]);
- vpRotationMatrix R(rzyz);
- vpRxyzVector rxyz(R);
-
- // Update the position using Rxyz representation
- for (unsigned int i=0; i <3; i++)
- position[i+3] = rxyz[i];
- // vpCTRACE << "Cartesian position Rxyz (deg)"
- // << position[0] << " " << position[1] << " " << position[2] << " "
- // << vpMath::deg(position[3]) << " "
- // << vpMath::deg(position[4]) << " "
- // << vpMath::deg(position[5]) << std::endl;
-
- break ;
- }
- case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get position.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position.");
- }
-
- return;
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
-
- The difference is here that the position is returned using a ThetaU
- representation for the rotation.
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
-*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position,
- double ×tamp)
-{
- vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame, posRxyz, timestamp);
- vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(RxyzVect);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
-
- The difference is here that the timestamp is not returned.
-
-*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
- Returns the robot controller current time (in second) since last robot power on.
-*/
-double vpRobotViper650::getTime() const
-{
- double timestamp;
- PrimitiveACQ_TIME_Viper650(×tamp);
- return timestamp;
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
-
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
- is always 6.
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
-
- - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
-
- - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- while (1) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-#endif
-}
- \endcode
-*/
-void
- vpRobotViper650::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- vpColVector vel_sat(6);
-
- // Velocity saturation
- switch(frame) {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- case vpRobot::MIXT_FRAME : {
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++)
- vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
-
- break;
- }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME : {
- vpColVector vel_max(6);
-
- if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
- for (unsigned int i=0; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
- }
- else {
- for (unsigned int i=0; i<5; i++)
- vel_max[i] = getMaxRotationVelocity();
- vel_max[5] = getMaxRotationVelocityJoint6();
- }
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
-
- }
- }
-
- InitTry;
-
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- // Send velocities in m/s and rad/s
- // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- // Convert all the velocities from rad/s into deg/s
- vel_sat.rad2deg();
- //std::cout << "Vitesse appliquee: " << vel_sat.t();
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
- Try( PrimitiveMOVESPEED_Viper650(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- // Send velocities in m/s and rad/s
- std::cout << "Vitesse ref appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- //Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
- break ;
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
- }
-
- Catch();
- if (TryStt < 0) {
- if (TryStt == VelStopOnJoint) {
- UInt32 axisInJoint[njoint];
- PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i])
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- }
- }
- else {
- printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
- if (TryString != NULL) {
- // The statement is in TryString, but we need to check the validity
- printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
- printf("\n");
- }
- }
- }
-
- return;
-}
-
-
-
-
-
-
-/* ------------------------------------------------------------------------ */
-/* --- GET ---------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param velocity : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Time in second since last robot power on.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \warning The first time this method is called, \e velocity is set to 0. The
- first call is used to intialise the velocity computation for the next call.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpRobotViper650 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- while (1) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-#endif
-}
- \endcode
-*/
-void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp)
-{
- velocity.resize (6);
- velocity = 0;
-
- vpColVector q_cur(6);
- vpHomogeneousMatrix fMc_cur;
- vpHomogeneousMatrix cMc; // camera displacement
- double time_cur;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_J_Viper650(q_cur.data, ×tamp) );
- time_cur = timestamp;
- q_cur.deg2rad();
-
- // Get the camera pose from the direct kinematics
- vpViper650::get_fMc(q_cur, fMc_cur);
-
- if ( ! first_time_getvel ) {
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
-
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the ThetaU representation for the rotation
- vpRotationMatrix cRc;
- cMc.extract(cRc);
- vpThetaUVector thetaU;
- thetaU.buildFrom(cRc);
-
- for (unsigned int i=0; i < 3; i++) {
- // Compute the translation displacement in the reference frame
- velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
- // Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
- }
-
- // Compute the velocity
- velocity /= (time_cur - time_prev_getvel);
- break ;
- }
- }
- }
- else {
- first_time_getvel = false;
- }
-
- // Memorize the camera pose for the next call
- fMc_prev_getvel = fMc_cur;
-
- // Memorize the joint position for the next call
- q_prev_getvel = q_cur;
-
- // Memorize the time associated to the joint position for the next call
- time_prev_getvel = time_cur;
-
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
-
- */
-void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
-{
- double timestamp;
- getVelocity(frame, velocity, timestamp);
-}
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Time in second since last robot power on.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpRobotViper650 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- double timestamp;
- while (1) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-#endif
-}
- \endcode
-*/
-vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- vpColVector velocity;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
-
- */
-vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector velocity;
- double timestamp;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
-Read joint positions in a specific Viper650 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#Viper - Position - Version 1.0
-# file: "myposition.pos "
-#
-# R: A B C D E F
-# Joint position in degrees
-#
-
-R: 0.1 0.3 -0.25 -80.5 80 0
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : The six joint positions. Values are expressed in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- // Enable the position control of the robot
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Get the current robot joint positions
- vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
-
- // Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
-
- // Get the position from a file and move to the registered position
- robot.readPosFile("current.pos", q); // Set the joint position from the file
-
- robot.setPositioningVelocity(5); // Positioning velocity set to 5%
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-#endif
-}
-\endcode
-
-\sa savePosFile()
-*/
-
-bool vpRobotViper650::readPosFile(const char *filename, vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
- // else
- // return (false); // fin fichier sans position robot.
- }
- }
- else {
- return (false); /* fin fichier */
- }
-
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(njoint);
- sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2],
- &q[3], &q[4], &q[5]);
-
- // converts rotations from degrees into radians
- q.deg2rad();
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Save joint (articular) positions in a specific Viper650 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : The six joint positions to save in the
- filename. Values are expressed in radians.
-
- \warning All the six joint rotations written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-
-bool
- vpRobotViper650::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#Viper650 - Position - Version 1.00\n\
-#\n\
-# R: A B C D E F\n\
-# Joint position in degrees\n\
-#\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Moves the robot to the joint position specified in the filename. The
- positioning velocity is set to 10% of the robot maximal velocity.
-
- \param filename: File containing a joint position.
-
- \sa readPosFile
-
-*/
-void
- vpRobotViper650::move(const char *filename)
-{
- vpColVector q;
-
- try {
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
- }
- catch(...) {
- throw;
- }
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
- vpRobotViper650::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper650::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- cartesian space. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call
- of this method. The dimension of \e displacement is always
- 6. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
-
- double q[6];
- vpColVector q_cur(6);
- double timestamp;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Viper650(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- if ( ! first_time_getdis ) {
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Bias the force/torque sensor.
-
- \warning This function waits 500 ms after the bias request to be sure the
- next measures take into account the bias.
-
- \exception vpRobotException::lowLevelError : If the force/torque sensor bias
- cannot be done on the low level controller.
-
- \sa getForceTorque()
-
-*/
-void
- vpRobotViper650::biasForceTorqueSensor() const
-{
- InitTry;
-
- Try( PrimitiveTFS_BIAS_Viper650() );
-
- // Wait 500 ms to be sure the next measures take into account the bias
- vpTime::wait(500);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot bias the force/torque sensor.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot bias the force/torque sensor.");
- }
-}
-
-/*!
-
- Get the rough force/torque sensor measures.
-
- \param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
-
- The code below shows how to get the force/torque measures after a sensor bias.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTime.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpColVector H; // force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz]
-
- vpRobotViper650 robot;
-
- // Bias the force/torque sensor
- robot.biasForceTorqueSensor();
-
- for (unsigned int i=0; i< 10; i++) {
- robot.getForceTorque(H) ;
- std::cout << "Measured force/torque: " << H.t() << std::endl;
- vpTime::wait(5);
- }
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : If the force/torque measures
- cannot be get from the low level controller.
-
- \sa biasForceTorqueSensor()
-
-*/
-void
- vpRobotViper650::getForceTorque(vpColVector &H) const
-{
- InitTry;
-
- H.resize (6);
-
- Try( PrimitiveTFS_ACQ_Viper650(H.data) );
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
- }
-}
-
-/*!
- Enable the joint limits on axis number 6. This is the default.
-
- \sa disbleJoint6Limits()
-*/
-void vpRobotViper650::enableJoint6Limits() const
-{
- InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
- std::cout << "Enable joint limits on axis 6..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot enable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot enable joint limits on axis 6.");
- }
-}
-
-/*!
- \warning Each call to this function should be done carefully.
-
- Disable the joint limits on axis number 6. When joint 6 is outside the limits,
- a call to this function allows to bring the robot to a position inside the limits.
- Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
-
- \sa enableJoint6Limits()
-*/
-void vpRobotViper650::disableJoint6Limits() const
-{
- InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1) );
- std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot disable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot disable joint limits on axis 6.");
- }
-}
-
-/*!
-
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
-
- \param w_max : Maximum rotation velocity expressed in rad/s.
-*/
-
-void
-vpRobotViper650::setMaxRotationVelocity (double w_max)
-{
- vpRobot::setMaxRotationVelocity(w_max);
- setMaxRotationVelocityJoint6(w_max);
-
- return;
-}
-
-/*!
-
- Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
-
- This function affects only the velocities that are sent as joint velocities.
-
- \code
- vpRobotViper650 robot;
- robot.setMaxRotationVelocity( vpMath::rad(20) );
- robot.setMaxRotationVelocityJoint6( vpMath::rad(50) );
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setVelocity(ARTICULAR_FRAME, v);
- \endcode
-
-
- \param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
-*/
-
-void
-vpRobotViper650::setMaxRotationVelocityJoint6 (const double w6_max)
-{
- maxRotationVelocity_joint6 = w6_max;
- return;
-}
-
-/*!
-
- Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
-
- \return Maximum rotation velocity on joint 6 expressed in rad/s.
-*/
-double
-vpRobotViper650::getMaxRotationVelocityJoint6() const
-{
- return maxRotationVelocity_joint6;
-}
-
-#endif
-
diff --git a/src/robot/real-robot/viper/vpRobotViper650.h b/src/robot/real-robot/viper/vpRobotViper650.h
deleted file mode 100644
index d9ad2bf..0000000
--- a/src/robot/real-robot/viper/vpRobotViper650.h
+++ /dev/null
@@ -1,426 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotViper650.h 4793 2014-07-21 15:10:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Viper S650 robot controlled by an Adept MotionBlox.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpRobotViper650_h
-#define vpRobotViper650_h
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_VIPER650
-
-#include <iostream>
-#include <stdio.h>
-
-#include <visp/vpRobot.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpViper650.h>
-
-// low level controller api
-extern "C" {
-# include "irisa_Viper650.h"
-# include "trycatch.h"
-}
-
-
-/*!
- \class vpRobotViper650
-
- \ingroup Viper RobotDriver
-
- \brief Control of Irisa's Viper S650 robot named Viper650.
-
- Implementation of the vpRobot class in order to control Irisa's
- Viper650 robot. This robot is an ADEPT six degrees of freedom arm.
- A firewire camera is mounted on the end-effector to allow
- eye-in-hand visual servoing. The control of this camera is achieved
- by the vp1394TwoGrabber class.
-
- This class allows to control the Viper650 arm robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The direct and inverse kinematics models are implemented in the
- vpViper650 class.
-
- \warning A Ctrl-C, a segmentation fault or other system errors are
- catched by this class to stop the robot.
-
- To communicate with the robot, you may first create an instance of this
- class by calling the default constructor:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-#endif
-}
- \endcode
-
- This initialize the robot kinematics with the \f$^e{\bf M}_c\f$
- extrinsic camera parameters obtained with a projection model without
- distortion. To set the robot kinematics with the \f$^e{\bf M}_c\f$
- transformation obtained with a camera perspective model including
- distortion you need to initialize the robot with:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distortion parameter
- robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
-#endif
-}
- \endcode
-
- You can get the intrinsic camera parameters of an image
- acquired by the camera attached to the robot, with:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
- g.acquire(I);
-
- vpRobotViper650 robot;
-
- // ...
-
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distortion.
-#endif
-}
- \endcode
-
- To control the robot in position, you may set the controller
- to position control and than send the position to reach in a specific
- frame like here in the joint space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- vpColVector q(6);
- // Set a joint position
- q[0] = vpMath::rad(10); // Joint 1 position, in rad
- q[1] = 0.2; // Joint 2 position, in rad
- q[2] = 0.3; // Joint 3 position, in rad
- q[3] = M_PI/8; // Joint 4 position, in rad
- q[4] = M_PI/4; // Joint 5 position, in rad
- q[5] = M_PI; // Joint 6 position, in rad
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- The robot moves to the specified position with the default
- positioning velocity vpRobotViper650::defaultPositioningVelocity. The
- setPositioningVelocity() method allows to change the maximal
- velocity used to reach the desired position.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- vpColVector q(6);
- // Set q[i] with i in [0:5]
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 40%
- robot.setPositioningVelocity(40);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and than send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper650.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-#endif
-}
- \endcode
-
- It is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and savePosFile()
- methods.
-*/
-class VISP_EXPORT vpRobotViper650
- :
- public vpViper650,
- public vpRobot
-{
-
-public: /* Constantes */
-
- /*! \enum vpControlModeType Control mode. */
- typedef enum {
- AUTO, //!< Automatic control mode (default).
- MANUAL, //!< Manual control mode activated when the dead man switch is in use.
- ESTOP //!< Emergency stop activated.
- } vpControlModeType;
-
- /* Vitesse maximale par default lors du positionnement du robot.
- * C'est la valeur a la construction de l'attribut prive \a
- * positioningVelocity. Cette valeur peut etre changee par la fonction
- * #setPositioningVelocity.
- */
- static const double defaultPositioningVelocity; // = 20.0;
-
-private: /* Not allowed functions. */
-
- /*!
- Copy constructor not allowed.
- */
- vpRobotViper650 (const vpRobotViper650 & robot);
-
-private: /* Attributs prives. */
-
- /** \brief Vrai ssi aucun objet de la classe vpRobotViper650 n'existe.
- *
- * Il ne peut exister simultanement qu'un seul objet de la classe
- * vpRobotViper650, car il correspond a un seul robot AFMA6. Creer
- * simultanement deux objets peut engendrer des conflits. Le constructeur
- * lance une erreur si le champ n'est pas FAUX puis positionne le champ
- * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
- * alors la creation d'un nouvel objet.
- */
- static bool robotAlreadyCreated;
-
- double positioningVelocity;
-
- // Variables used to compute the measured velocities (see getVelocity() )
- vpColVector q_prev_getvel;
- vpHomogeneousMatrix fMc_prev_getvel;
- double time_prev_getvel;
- bool first_time_getvel;
-
- // Variables used to compute the measured displacement (see
- // getDisplacement() )
- vpColVector q_prev_getdis;
- bool first_time_getdis;
- vpControlModeType controlMode;
-
-
-public: /* Methode publiques */
-
- vpRobotViper650 (bool verbose=true);
- virtual ~vpRobotViper650 (void);
-
- // Force/Torque control
- void biasForceTorqueSensor() const;
-
- void disableJoint6Limits() const;
- void enableJoint6Limits() const;
-
- /*!
- \return The control mode indicating if the robot is in automatic,
- manual (usage of the dead man switch) or emergnecy stop mode.
- */
- vpControlModeType getControlMode() const {
- return controlMode;
- }
-
- void getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
- void getForceTorque(vpColVector &H) const;
-
- double getMaxRotationVelocityJoint6() const;
-
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position,
- double ×tamp);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position,
- double ×tamp);
-
- double getPositioningVelocity (void) const;
- bool getPowerState() const;
-
- double getTime () const;
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
-
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (void);
- void init (vpViper650::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType
- projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
-
- void move(const char *filename);
-
- void powerOn();
- void powerOff();
-
- static bool readPosFile(const char *filename, vpColVector &q);
- static bool savePosFile(const char *filename, const vpColVector &q);
-
- void setMaxRotationVelocity(double w_max);
- void setMaxRotationVelocityJoint6(double w6_max);
-
- // Position control
- void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &position) ;
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1, const double pos2, const double pos3,
- const double pos4, const double pos5, const double pos6) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
-
- // State
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
- // Velocity control
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
-
- void stopMotion();
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
- double maxRotationVelocity_joint6;
-};
-
-#endif
-#endif /* #ifndef vpRobotViper650_h */
diff --git a/src/robot/real-robot/viper/vpRobotViper850.cpp b/src/robot/real-robot/viper/vpRobotViper850.cpp
deleted file mode 100644
index f68efb5..0000000
--- a/src/robot/real-robot/viper/vpRobotViper850.cpp
+++ /dev/null
@@ -1,2400 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotViper850.cpp 4594 2014-01-20 15:08:07Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Viper S850 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_VIPER850
-
-#include <signal.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <visp/vpRobotException.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotViper850.h>
-
-/* ---------------------------------------------------------------------- */
-/* --- STATIC ----------------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-bool vpRobotViper850::robotAlreadyCreated = false;
-
-/*!
-
- Default positioning velocity in percentage of the maximum
- velocity. This value is set to 15. The member function
- setPositioningVelocity() allows to change this value.
-
-*/
-const double vpRobotViper850::defaultPositioningVelocity = 15.0;
-
-/* ---------------------------------------------------------------------- */
-/* --- EMERGENCY STOP --------------------------------------------------- */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- Emergency stops the robot if the program is interrupted by a SIGINT
- (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
- or SIGQUIT signal.
-
-*/
-void emergencyStopViper850(int signo)
-{
- std::cout << "Stop the Viper850 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
-}
- //std::cout << "Emergency stop called\n";
- // PrimitiveESTOP_Viper850();
- PrimitiveSTOP_Viper850();
- std::cout << "Robot was stopped\n";
-
- // Free allocated resources
- // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
-
- fprintf(stdout, "Application ");
- fflush(stdout);
- kill(getpid(), SIGKILL);
- exit(1) ;
-}
-
-/* ---------------------------------------------------------------------- */
-/* --- CONSTRUCTOR ------------------------------------------------------ */
-/* ---------------------------------------------------------------------- */
-
-/*!
-
- The only available constructor.
-
- This contructor calls init() to initialise the connection with the
- MotionBox or low level controller, send the default \f$^e{\bf
- M}_c\f$ homogeneous matrix and power on the robot.
-
- It also set the robot state to vpRobot::STATE_STOP.
-
- To set the extrinsic camera parameters related to the \f$^e{\bf
- M}_c\f$ matrix obtained with a camera perspective projection model
- including the distorsion, use the code below:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters associated to an
- image acquired by the camera attached to the robot, with:
-
- \code
- vpImage<unsigned char> I(480, 640);
-
- // Get an image from the camera attached to the robot
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
- g.acquire(I);
-#endif
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
-#endif
-}
- \endcode
-
- \sa vpCameraParameters, init(vpViper850::vpViper850CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-
-*/
-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
- */
-
- signal(SIGINT, emergencyStopViper850);
- signal(SIGBUS, emergencyStopViper850) ;
- signal(SIGSEGV, emergencyStopViper850) ;
- signal(SIGKILL, emergencyStopViper850);
- signal(SIGQUIT, emergencyStopViper850);
-
- setVerbose(verbose);
- if (verbose_)
- std::cout << "Open communication with MotionBlox.\n";
- try {
- this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
- // vpERROR_TRACE("Error caught") ;
- throw ;
- }
- positioningVelocity = defaultPositioningVelocity ;
-
- maxRotationVelocity_joint6 = maxRotationVelocity;
-
- vpRobotViper850::robotAlreadyCreated = true;
-
- return ;
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- INITIALISATION ----------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Initialise the connection with the MotionBox or low level
- controller, send the default eMc homogeneous matrix, power on the
- robot and wait 1 sec before returning to be sure the initialisation
- is done.
-
- \warning This method sets the camera extrinsic parameters (matrix
- eMc) to the one obtained by calibration with a camera projection
- model without distorsion by calling
- init(vpViper850::defaultCameraRobot). If you want to set the extrinsic
- camera parameters to those obtained with a camera perspective model
- including the distorsion you have to call the
- init(vpViper850::vpViper850CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType) method.
-
- \sa vpCameraParameters, init(vpViper850::vpViper850CameraRobotType,
- vpCameraParameters::vpCameraParametersProjType)
-*/
-void
- vpRobotViper850::init (void)
-{
- InitTry;
-
- // Initialise private variables used to compute the measured velocities
- q_prev_getvel.resize(6);
- q_prev_getvel = 0;
- time_prev_getvel = 0;
- first_time_getvel = true;
-
- // Initialise private variables used to compute the measured displacement
- q_prev_getdis.resize(6);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
-
- // Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
-
- // Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Viper850() );
-
- Try( PrimitiveRESET_Viper850() );
-
- // Enable the joint limits on axis 6
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
-
- // Update the eMc matrix in the low level controller
- init(vpViper850::defaultTool);
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // Print the robot status
- if (verbose_) {
- std::cout << "Robot status: ";
- switch(EStopStatus) {
- case ESTOP_AUTO:
- controlMode = AUTO;
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
-
- case ESTOP_MANUAL:
- controlMode = MANUAL;
- if (HIPowerStatus == 0)
- std::cout << "Power is OFF" << std::endl;
- else
- std::cout << "Power is ON" << std::endl;
- break;
- case ESTOP_ACTIVATED:
- controlMode = ESTOP;
- std::cout << "Emergency stop is activated" << std::endl;
- break;
- default:
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- }
- std::cout << std::endl;
- }
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
-
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
-
- // If an error occur in the low level controller, goto here
- //CatchPrint();
- Catch();
-
- // Test if an error occurs
- if (TryStt == -20001)
- printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
- else if (TryStt == -675)
- printf(" Timeout enabling power...\n");
-
- if (TryStt < 0) {
- // Power off the robot
- PrimitivePOWEROFF_Viper850();
- // Free allocated resources
- ShutDownConnection();
-
- std::cout << "Cannot open connexion with the motionblox..." << std::endl;
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connexion with the motionblox");
- }
- return ;
-}
-
-/*!
-
- Initialize the robot kinematics with the extrinsic calibration
- parameters associated to a specific camera (set the eMc homogeneous
- parameters in the low level controller) and also get the joint
- limits from the low-level controller.
-
- The eMc parameters depend on the camera and the projection model in use.
-
- \param tool : Tool to use.
-
- \param projModel : Projection model associated to the camera.
-
- To set the extrinsic camera parameters related to the \f$^e{\bf
- M}_c\f$ matrix obtained with a camera perspective projection model
- including the distorsion, use the code below:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distorsion parameter
- robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
-
- Now, you can get the intrinsic camera parameters associated to an
- image acquired by the camera attached to the robot, with:
-
- \code
- vpImage<unsigned char> I(480, 640);
-
- // Get an image from the camera attached to the robot
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
- g.acquire(I);
-#endif
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distorsion.
-#endif
-}
- \endcode
-
- \sa vpCameraParameters, init()
-*/
-void
- vpRobotViper850::init (vpViper850::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel)
-{
-
- InitTry;
- // Read the robot constants from files
- // - joint [min,max], coupl_56, long_56
- // - camera extrinsic parameters relative to eMc
- vpViper850::init(tool, projModel);
-
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
-
- // get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
-
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
-
- setToolType(tool);
-
- CatchPrint();
- return ;
-}
-
-/* ------------------------------------------------------------------------ */
-/* --- DESTRUCTOR --------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Destructor.
-
- Free allocated resources.
-*/
-vpRobotViper850::~vpRobotViper850 (void)
-{
- InitTry;
-
- setRobotState(vpRobot::STATE_STOP) ;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- // if (HIPowerStatus == 1) {
- // fprintf(stdout, "Power OFF the robot\n");
- // fflush(stdout);
-
- // Try( PrimitivePOWEROFF_Viper850() );
- // }
-
- // Free allocated resources
- ShutDownConnection();
-
- vpRobotViper850::robotAlreadyCreated = false;
-
- CatchPrint();
- return;
-}
-
-
-
-
-/*!
-
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
- vpRobotViper850::setRobotState(vpRobot::vpRobotStateType newState)
-{
- InitTry;
-
- switch (newState) {
- case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- Try( PrimitiveSTOP_Viper850() );
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Viper850() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
- }
- default:
- break ;
- }
-
- CatchPrint();
-
- return vpRobot::setRobotState (newState);
-}
-
-
-/* ------------------------------------------------------------------------ */
-/* --- STOP --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-/*!
-
- Stop the robot and set the robot state to vpRobot::STATE_STOP.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-*/
-void
- vpRobotViper850::stopMotion(void)
-{
- if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
- return;
-
- InitTry;
- Try( PrimitiveSTOP_Viper850() );
- setRobotState (vpRobot::STATE_STOP);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
- }
-}
-
-/*!
-
- Power on the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot power on.
-
- \sa powerOff(), getPowerState()
-*/
-void
- vpRobotViper850::powerOn(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- UInt32 EStopStatus;
- bool firsttime = true;
- unsigned int nitermax = 10;
-
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
- if (EStopStatus == ESTOP_AUTO) {
- controlMode = AUTO;
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
- controlMode = MANUAL;
- break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
- controlMode = ESTOP;
- if (firsttime) {
- std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
- firsttime = false;
- }
- fprintf(stdout, "Remaining time %ds \r", nitermax-i);
- fflush(stdout);
- CAL_Wait(1);
- }
- else {
- std::cout << "Sorry there is an error on the emergency chain." << std::endl;
- std::cout << "You have to call Adept for maintenance..." << std::endl;
- // Free allocated resources
- ShutDownConnection();
- exit(0);
- }
- }
-
- if (EStopStatus == ESTOP_ACTIVATED)
- std::cout << std::endl;
-
- if (EStopStatus == ESTOP_ACTIVATED) {
- std::cout << "Sorry, cannot power on the robot." << std::endl;
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power on the robot.");
- }
-
- if (HIPowerStatus == 0) {
- fprintf(stdout, "Power ON the Viper850 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWERON_Viper850() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Power off the robot.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error during robot stopping.
-
- \sa powerOn(), getPowerState()
-*/
-void
- vpRobotViper850::powerOff(void)
-{
- InitTry;
-
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- fprintf(stdout, "Power OFF the Viper850 robot\n");
- fflush(stdout);
-
- Try( PrimitivePOWEROFF_Viper850() );
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
- }
-}
-
-/*!
-
- Get the robot power state indication if power is on or off.
-
- \return true if power is on. false if power is off.
-
- \exception vpRobotException::lowLevelError : If the low level
- controller returns an error.
-
- \sa powerOn(), powerOff()
-*/
-bool
- vpRobotViper850::getPowerState(void) const
-{
- InitTry;
- bool status = false;
- // Look if the power is on or off
- UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
- CAL_Wait(0.1);
-
- if (HIPowerStatus == 1) {
- status = true;
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
- }
- return status;
-}
-
-/*!
-
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
- end-effector frame. This transformation allows to compute a
- velocity expressed in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-
-*/
-void
- vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- vpViper850::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
-
- Get the geometric transformation \f$^c{\bf M}_e\f$ between the
- camera frame and the end-effector frame. This transformation is
- constant and correspond to the extrinsic camera parameters estimated
- by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
-*/
-void
- vpRobotViper850::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpViper850::get_cMe(cMe) ;
-}
-
-
-/*!
-
- Get the robot jacobian expressed in the end-effector frame.
-
- To compute \f$^e{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param eJe : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
- end-effector frame.
-
-*/
-void
- vpRobotViper850::get_eJe(vpMatrix &eJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = vpMath::rad(position[i]);
-
- try
- {
- vpViper850::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-/*!
-
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- To compute \f$^f{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param fJe : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
- reference frame.
-*/
-
-void
- vpRobotViper850::get_fJe(vpMatrix &fJe)
-{
-
- double position[6];
- double timestamp;
-
- InitTry;
- Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
- CatchPrint();
-
- vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
- q[i] = position[i];
-
- try
- {
- vpViper850::get_fJe(q, fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Set the maximal velocity percentage to use for a position control.
-
- The default positioning velocity is defined by
- vpRobotViper850::defaultPositioningVelocity. This method allows to
- change this default positioning velocity
-
- \param velocity : Percentage of the maximal velocity. Values should
- be in ]0:100].
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpColVector position(6);
- position = 0; // position in rad
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot to the joint position [0,0,0,0,0,0]
- robot.setPosition(vpRobot::ARTICULAR_FRAME, position);
-#endif
-}
- \endcode
-
- \sa getPositioningVelocity()
-*/
-void
- vpRobotViper850::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
-
-/*!
- Get the maximal velocity percentage used for a position control.
-
- \sa setPositioningVelocity()
-*/
-double
- vpRobotViper850::getPositioningVelocity (void) const
-{
- return positioningVelocity;
-}
-
-
-/*!
-
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param position : A six dimension vector corresponding to the
- position to reach. All the positions are expressed in meters for the
- translations and radians for the rotations. If the position is out
- of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are the six joint rotations starting
- from the base to the end-effector.
-
- - In the camera and the reference frame, positions are respectively
- X,Y,Z translations and 3 rotations arround the X, Y and Z
- axis. Rotations are represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpColVector position(6);
- // Set positions in the camera frame
- position[0] = 0.1; // x axis, in meter
- position[1] = 0.2; // y axis, in meter
- position[2] = 0.3; // z axis, in meter
- position[3] = M_PI/8; // rotation around x axis, in rad
- position[4] = M_PI/4; // rotation around y axis, in rad
- position[5] = M_PI; // rotation around z axis, in rad
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
-#endif
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- \endcode
-
-*/
-void
- vpRobotViper850::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & position )
-{
-
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- vpColVector destination(njoint);
- int error = 0;
- double timestamp;
-
- InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- vpColVector q(njoint);
- Try( PrimitiveACQ_POS_Viper850(q.data, ×tamp) );
-
- // Convert degrees into rad
- q.deg2rad();
-
- // Get fMc from the inverse kinematics
- vpHomogeneousMatrix fMc;
- vpViper850::get_fMc(q, fMc);
-
- // Set cMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
-
- // Compute cMc2
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- // Compute the new position to reach: fMc*cMc2
- vpHomogeneousMatrix fMc2 = fMc * cMc2;
-
- // Compute the corresponding joint position from the inverse kinematics
- unsigned int solution = this->getInverseKinematics(fMc2, q);
- if (solution) { // Position is reachable
- destination = q;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
- Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
-
- break ;
- }
- case vpRobot::ARTICULAR_FRAME: {
- destination = position;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
-
- //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
- break ;
-
- }
- case vpRobot::REFERENCE_FRAME: {
- // Convert angles from Rxyz representation to Rzyz representation
- vpRxyzVector rxyz(position[3],position[4],position[5]);
- vpRotationMatrix R(rxyz);
- vpRzyzVector rzyz(R);
-
- for (unsigned int i=0; i <3; i++) {
- destination[i] = position[i];
- destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
- }
- int configuration = 0; // keep the actual configuration
-
- //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_C_Viper850(destination.data, configuration,
- positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
-
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt == InvalidPosition || TryStt == -1023)
- std::cout << " : Position out of range.\n";
- else if (TryStt == -3019) {
- if (frame == vpRobot::ARTICULAR_FRAME)
- std::cout << " : Joint position out of range.\n";
- else
- std::cout << " : Cartesian position leads to a joint position out of range.\n";
- }
- else if (TryStt < 0)
- std::cout << " : Unknown error (see Fabien).\n";
- else if (error == -1)
- std::cout << "Position out of range.\n";
-
- if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
-
- return ;
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- This method owerloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
- the position to reach. All the positions are expressed in meters for
- the translations and radians for the rotations.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X (pos1), Y (pos2),
- Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
- translations, and A,B,C the rotations of the end-effector.
-
- - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- // Set positions in the camera frame
- double pos1 = 0.1; // x axis, in meter
- double pos2 = 0.2; // y axis, in meter
- double pos3 = 0.3; // z axis, in meter
- double pos4 = M_PI/8; // rotation around x axis, in rad
- double pos5 = M_PI/4; // rotation around y axis, in rad
- double pos6 = M_PI; // rotation around z axis, in rad
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
-#endif
-}
- \endcode
-
- \sa setPosition()
-*/
-void vpRobotViper850::setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6)
-{
- try{
- vpColVector position(6) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
-
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpColVector q;
- vpRobotViper850 robot;
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-
-*/
-void vpRobotViper850::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
-
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Time in second since last robot power on.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- vpColVector position;
- double timestamp;
- robot.getPosition(vpRobot::REFERENCE_FRAME, position, timestamp);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (unsigned int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame transformation in terms of a
- // homogenous matrix
- vpHomogeneousMatrix fMc(ftc, fRc);
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : If the position cannot
- be get from the low level controller.
-
- \sa setPosition(const vpRobot::vpControlFrameType frame, const
- vpColVector & r)
-
-*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector & position, double ×tamp)
-{
-
- InitTry;
-
- position.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- Try( PrimitiveACQ_POS_J_Viper850(position.data, ×tamp) );
- //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
- position.deg2rad();
-
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveACQ_POS_C_Viper850(position.data, ×tamp) );
- // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
- // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
- // Convert Euler Rzyz angles from deg to rad
- for (unsigned int i=3; i <6; i++)
- position[i] = vpMath::rad(position[i]);
- // Convert Rzyz angles into Rxyz representation
- vpRzyzVector rzyz(position[3], position[4], position[5]);
- vpRotationMatrix R(rzyz);
- vpRxyzVector rxyz(R);
-
- // Update the position using Rxyz representation
- for (unsigned int i=0; i <3; i++)
- position[i+3] = rxyz[i];
- // vpCTRACE << "Cartesian position Rxyz (deg)"
- // << position[0] << " " << position[1] << " " << position[2] << " "
- // << vpMath::deg(position[3]) << " "
- // << vpMath::deg(position[4]) << " "
- // << vpMath::deg(position[5]) << std::endl;
-
- break ;
- }
- case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- break ;
- }
- }
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get position.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position.");
- }
-
- return;
-}
-
-/*!
- Returns the robot controller current time (in second) since last robot power on.
-*/
-double vpRobotViper850::getTime() const
-{
- double timestamp;
- PrimitiveACQ_TIME_Viper850(×tamp);
- return timestamp;
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector & position)
-{
- double timestamp;
- getPosition(frame, position, timestamp);
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
-
- The difference is here that the position is returned using a ThetaU
- representation.
-
-*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position,
- double ×tamp)
-{
- vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame, posRxyz, timestamp);
- vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(RxyzVect);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
-
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
-
- The difference is here that the timestamp is not used.
-
-*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
-{
- vpColVector posRxyz;
- double timestamp;
- //recupere position en Rxyz
- this->getPosition(frame, posRxyz, timestamp);
- vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(RxyzVect);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
-
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
- is always 6.
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
-
- - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
-
- - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- while (1) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-#endif
-}
- \endcode
-*/
-void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- vpColVector vel_sat(6);
-
- // Velocity saturation
- switch(frame) {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- case vpRobot::MIXT_FRAME : {
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++)
- vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
-
- break;
- }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME : {
- vpColVector vel_max(6);
-
- if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
- for (unsigned int i=0; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
- }
- else {
- for (unsigned int i=0; i<5; i++)
- vel_max[i] = getMaxRotationVelocity();
- vel_max[5] = getMaxRotationVelocityJoint6();
- }
-
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
- }
- }
-
- InitTry;
-
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- // Send velocities in m/s and rad/s
- // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- // Convert all the velocities from rad/s into deg/s
- vel_sat.rad2deg();
- //std::cout << "Vitesse appliquee: " << vel_sat.t();
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
- Try( PrimitiveMOVESPEED_Viper850(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- // Send velocities in m/s and rad/s
- std::cout << "Vitesse ref appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- //Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
- break ;
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
- }
-
- Catch();
- if (TryStt < 0) {
- if (TryStt == VelStopOnJoint) {
- UInt32 axisInJoint[njoint];
- PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i])
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- }
- }
- else {
- printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
- if (TryString != NULL) {
- // The statement is in TryString, but we need to check the validity
- printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
- printf("\n");
- }
- }
- }
-
- return;
-}
-
-
-
-
-
-
-/* ------------------------------------------------------------------------ */
-/* --- GET ---------------------------------------------------------------- */
-/* ------------------------------------------------------------------------ */
-
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param velocity : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Time in second since last robot power on.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \warning The first time this method is called, \e velocity is set to 0. The
- first call is used to intialise the velocity computation for the next call.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- while (1) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-#endif
-}
- \endcode
-*/
-void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp)
-{
-
- velocity.resize (6);
- velocity = 0;
-
- vpColVector q_cur(6);
- vpHomogeneousMatrix fMc_cur;
- vpHomogeneousMatrix cMc; // camera displacement
- double time_cur;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp) );
- time_cur = timestamp;
- q_cur.deg2rad();
-
- // Get the camera pose from the direct kinematics
- vpViper850::get_fMc(q_cur, fMc_cur);
-
- if ( ! first_time_getvel ) {
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel)
- / (time_cur - time_prev_getvel);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
-
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
-
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the ThetaU representation for the rotation
- vpRotationMatrix cRc;
- cMc.extract(cRc);
- vpThetaUVector thetaU;
- thetaU.buildFrom(cRc);
-
- for (unsigned int i=0; i < 3; i++) {
- // Compute the translation displacement in the reference frame
- velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
- // Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
- }
-
- // Compute the velocity
- velocity /= (time_cur - time_prev_getvel);
- break ;
- }
- }
- }
- else {
- first_time_getvel = false;
- }
-
- // Memorize the camera pose for the next call
- fMc_prev_getvel = fMc_cur;
-
- // Memorize the joint position for the next call
- q_prev_getvel = q_cur;
-
- // Memorize the time associated to the joint position for the next call
- time_prev_getvel = time_cur;
-
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
-
- */
-void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
-{
- double timestamp;
- getVelocity(frame, velocity, timestamp);
-}
-
-/*!
-
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Time in second since last robot power on.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
- // q_dot_mes is resized to 6, the number of joint
-
- double timestamp;
- while (1) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-#endif
-}
- \endcode
-*/
-vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- vpColVector velocity;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
- Get robot velocities.
-
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
-
- */
-vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector velocity;
- double timestamp;
- getVelocity (frame, velocity, timestamp);
-
- return velocity;
-}
-
-/*!
-
-Read joint positions in a specific Viper850 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#Viper - Position - Version 1.0
-# file: "myposition.pos "
-#
-# R: A B C D E F
-# Joint position in degrees
-#
-
-R: 0.1 0.3 -0.25 -80.5 80 0
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : The six joint positions. Values are expressed in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- // Enable the position control of the robot
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Get the current robot joint positions
- vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
-
- // Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
-
- // Get the position from a file and move to the registered position
- robot.readPosFile("current.pos", q); // Set the joint position from the file
-
- robot.setPositioningVelocity(5); // Positioning velocity set to 5%
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-#endif
-}
-\endcode
-
-\sa savePosFile()
-*/
-
-bool
- vpRobotViper850::readPosFile(const char *filename, vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
- // else
- // return (false); // fin fichier sans position robot.
- }
- }
- else {
- return (false); /* fin fichier */
- }
-
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(njoint);
- sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2],
- &q[3], &q[4], &q[5]);
-
- // converts rotations from degrees into radians
- q.deg2rad();
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Save joint (articular) positions in a specific Viper850 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : The six joint positions to save in the
- filename. Values are expressed in radians.
-
- \warning All the six joint rotations written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-
-bool
- vpRobotViper850::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#Viper850 - Position - Version 1.00\n\
-#\n\
-# R: A B C D E F\n\
-# Joint position in degrees\n\
-#\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
-
- Moves the robot to the joint position specified in the filename. The
- positioning velocity is set to 10% of the robot maximal velocity.
-
- \param filename: File containing a joint position.
-
- \sa readPosFile
-
-*/
-void
- vpRobotViper850::move(const char *filename)
-{
- vpColVector q;
-
- try {
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
- }
- catch(...) {
- throw;
- }
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
- vpRobotViper850::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper850::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- cartesian space. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call
- of this method. The dimension of \e displacement is always
- 6. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
-
- double q[6];
- vpColVector q_cur(6);
- double timestamp;
-
- InitTry;
-
- // Get the current joint position
- Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
- q_cur[i] = q[i];
- }
-
- if ( ! first_time_getdis ) {
- switch (frame) {
- case vpRobot::CAMERA_FRAME: {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
- }
-}
-
-/*!
-
- Bias the force/torque sensor.
-
- \warning This function waits 500 ms after the bias request to be sure the
- next measures take into account the bias.
-
- \exception vpRobotException::lowLevelError : If the force/torque sensor bias
- cannot be done on the low level controller.
-
- \sa getForceTorque()
-
-*/
-void
- vpRobotViper850::biasForceTorqueSensor() const
-{
- InitTry;
-
- Try( PrimitiveTFS_BIAS_Viper850() );
-
- // Wait 500 ms to be sure the next measures take into account the bias
- vpTime::wait(500);
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot bias the force/torque sensor.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot bias the force/torque sensor.");
- }
-}
-
-/*!
-
- Get the rough force/torque sensor measures.
-
- \param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
-
- The code below shows how to get the force/torque measures after a sensor bias.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTime.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpColVector H; // force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz]
-
- vpRobotViper850 robot;
-
- // Bias the force/torque sensor
- robot.biasForceTorqueSensor();
-
- for (unsigned int i=0; i< 10; i++) {
- robot.getForceTorque(H) ;
- std::cout << "Measured force/torque: " << H.t() << std::endl;
- vpTime::wait(5);
- }
-#endif
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : If the force/torque measures
- cannot be get from the low level controller.
-
- \sa biasForceTorqueSensor()
-
-*/
-void
- vpRobotViper850::getForceTorque(vpColVector &H) const
-{
- InitTry;
-
- H.resize (6);
-
- Try( PrimitiveTFS_ACQ_Viper850(H.data) );
-
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
- }
-}
-/*!
-
- Open the electric two fingers Schunk gripper.
-
- \sa closeGripper()
-*/
-void
- vpRobotViper850::openGripper()
-{
- InitTry;
- Try( PrimitiveGripper_Viper850(1) );
- std::cout << "Open the gripper..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot open the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot open the gripper.");
- }
-}
-
-/*!
-
- Close the electric two fingers Schunk gripper.
-
- \sa openGripper()
-
-*/
-void vpRobotViper850::closeGripper() const
-{
- InitTry;
- Try( PrimitiveGripper_Viper850(0) );
- std::cout << "Close the gripper..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot close the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot close the gripper.");
- }
-}
-
-/*!
- Enable the joint limits on axis number 6. This is the default.
-
- \sa disbleJoint6Limits()
-*/
-void vpRobotViper850::enableJoint6Limits() const
-{
- InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
- std::cout << "Enable joint limits on axis 6..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot enable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot enable joint limits on axis 6.");
- }
-}
-
-/*!
- \warning Each call to this function should be done carefully.
-
- Disable the joint limits on axis number 6. When joint 6 is outside the limits,
- a call to this function allows to bring the robot to a position inside the limits.
- Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
-
- \sa enableJoint6Limits()
-*/
-void vpRobotViper850::disableJoint6Limits() const
-{
- InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1) );
- std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
- CatchPrint();
- if (TryStt < 0) {
- vpERROR_TRACE ("Cannot disable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot disable joint limits on axis 6.");
- }
-}
-
-/*!
-
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
-
- \param w_max : Maximum rotation velocity expressed in rad/s.
-*/
-
-void
-vpRobotViper850::setMaxRotationVelocity (double w_max)
-{
- vpRobot::setMaxRotationVelocity(w_max);
- setMaxRotationVelocityJoint6(w_max);
-
- return;
-}
-
-/*!
-
- Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
-
- This function affects only the velocities that are sent as joint velocities.
-
- \code
- vpRobotViper850 robot;
- robot.setMaxRotationVelocity( vpMath::rad(20) );
- robot.setMaxRotationVelocityJoint6( vpMath::rad(50) );
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setVelocity(ARTICULAR_FRAME, v);
- \endcode
-
-
- \param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
-*/
-
-void
-vpRobotViper850::setMaxRotationVelocityJoint6 (const double w6_max)
-{
- maxRotationVelocity_joint6 = w6_max;
- return;
-}
-
-/*!
-
- Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
-
- \return Maximum rotation velocity on joint 6 expressed in rad/s.
-*/
-double
-vpRobotViper850::getMaxRotationVelocityJoint6() const
-{
- return maxRotationVelocity_joint6;
-}
-
-#endif
-
diff --git a/src/robot/real-robot/viper/vpRobotViper850.h b/src/robot/real-robot/viper/vpRobotViper850.h
deleted file mode 100644
index 5d5d7d0..0000000
--- a/src/robot/real-robot/viper/vpRobotViper850.h
+++ /dev/null
@@ -1,440 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotViper850.h 4594 2014-01-20 15:08:07Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the Irisa's Viper S850 robot controlled by an Adept MotionBlox.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpRobotViper850_h
-#define vpRobotViper850_h
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_VIPER850
-
-#include <iostream>
-#include <stdio.h>
-
-#include <visp/vpRobot.h>
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpViper850.h>
-
-// low level controller api
-extern "C" {
-# include "irisa_Viper850.h"
-# include "trycatch.h"
-}
-
-
-/*!
- \class vpRobotViper850
-
- \ingroup Viper RobotDriver
-
- \brief Control of Irisa's Viper S850 robot named Viper850.
-
- Implementation of the vpRobot class in order to control Irisa's
- Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
- A firewire camera is mounted on the end-effector to allow
- eye-in-hand visual servoing. The control of this camera is achieved
- by the vp1394TwoGrabber class.
-
- This class allows to control the Viper850 arm robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The direct and inverse kinematics models are implemented in the
- vpViper850 class.
-
- \warning A Ctrl-C, a segmentation fault or other system errors are
- catched by this class to stop the robot.
-
- To communicate with the robot, you may first create an instance of this
- class by calling the default constructor:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-#endif
-}
- \endcode
-
- This initialize the robot kinematics with the \f$^e{\bf M}_c\f$
- extrinsic camera parameters obtained with a projection model without
- distortion. To set the robot kinematics with the \f$^e{\bf M}_c\f$
- transformation obtained with a camera perspective model including
- distortion you need to initialize the robot with:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- // Set the extrinsic camera parameters obtained with a perpective
- // projection model including a distortion parameter
- robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
-#endif
-}
- \endcode
-
- You can get the intrinsic camera parameters of an image
- acquired by the camera attached to the robot, with:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpCameraParameters.h>
-
-int main()
-{
-#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I;
- vp1394TwoGrabber g;
- g.acquire(I);
-
- vpRobotViper850 robot;
-
- // ...
-
- vpCameraParameters cam;
- robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
- // with distortion.
-#endif
-}
- \endcode
-
- To control the robot in position, you may set the controller
- to position control and than send the position to reach in a specific
- frame like here in the joint space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- vpColVector q(6);
- // Set a joint position
- q[0] = vpMath::rad(10); // Joint 1 position, in rad
- q[1] = 0.2; // Joint 2 position, in rad
- q[2] = 0.3; // Joint 3 position, in rad
- q[3] = M_PI/8; // Joint 4 position, in rad
- q[4] = M_PI/4; // Joint 5 position, in rad
- q[5] = M_PI; // Joint 6 position, in rad
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- The robot moves to the specified position with the default
- positioning velocity vpRobotViper850::defaultPositioningVelocity. The
- setPositioningVelocity() method allows to change the maximal
- velocity used to reach the desired position.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- vpColVector q(6);
- // Set q[i] with i in [0:5]
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 40%
- robot.setPositioningVelocity(40);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-#endif
-}
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and than send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-#endif
-}
- \endcode
-
- It is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and savePosFile()
- methods.
-*/
-class VISP_EXPORT vpRobotViper850
- :
- public vpViper850,
- public vpRobot
-{
-
-public: /* Constantes */
-
- /*! \enum vpControlModeType Control mode. */
- typedef enum {
- AUTO, //!< Automatic control mode (default).
- MANUAL, //!< Manual control mode activated when the dead man switch is in use.
- ESTOP //!< Emergency stop activated.
- } vpControlModeType;
-
- /* Vitesse maximale par default lors du positionnement du robot.
- * C'est la valeur a la construction de l'attribut prive \a
- * positioningVelocity. Cette valeur peut etre changee par la fonction
- * #setPositioningVelocity.
- */
- static const double defaultPositioningVelocity; // = 20.0;
-
-private: /* Not allowed functions. */
-
- /*!
- Copy constructor not allowed.
- */
- vpRobotViper850 (const vpRobotViper850 & robot);
-
-private: /* Attributs prives. */
-
- /** \brief Vrai ssi aucun objet de la classe vpRobotViper850 n'existe.
- *
- * Il ne peut exister simultanement qu'un seul objet de la classe
- * vpRobotViper850, car il correspond a un seul robot AFMA6. Creer
- * simultanement deux objets peut engendrer des conflits. Le constructeur
- * lance une erreur si le champ n'est pas FAUX puis positionne le champ
- * a VRAI. Seul le destructeur repositionne le champ a FAUX, ce qui
- * alors la creation d'un nouvel objet.
- */
- static bool robotAlreadyCreated;
-
- double positioningVelocity;
-
- // Variables used to compute the measured velocities (see getVelocity() )
- vpColVector q_prev_getvel;
- vpHomogeneousMatrix fMc_prev_getvel;
- double time_prev_getvel;
- bool first_time_getvel;
-
- // Variables used to compute the measured displacement (see
- // getDisplacement() )
- vpColVector q_prev_getdis;
- bool first_time_getdis;
- vpControlModeType controlMode;
-
-
-public: /* Methode publiques */
-
- vpRobotViper850 (bool verbose=true);
- virtual ~vpRobotViper850 (void);
-
- // Force/Torque control
- void biasForceTorqueSensor() const;
-
- void closeGripper() const;
-
- void disableJoint6Limits() const;
- void enableJoint6Limits() const;
-
- void getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
- /*!
- \return The control mode indicating if the robot is in automatic,
- manual (usage of the dead man switch) or emergnecy stop mode.
- */
- vpControlModeType getControlMode() const {
- return controlMode;
- }
-
- void getForceTorque(vpColVector &H) const;
-
- double getMaxRotationVelocityJoint6() const;
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector &position, double ×tamp);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position);
- void getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp);
-
- double getPositioningVelocity (void) const;
- bool getPowerState() const;
-
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
-
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- double getTime() const;
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (void);
- void init (vpViper850::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType
- projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
-
- void move(const char *filename) ;
-
- void openGripper();
-
- void powerOn() ;
- void powerOff() ;
-
- static bool readPosFile(const char *filename, vpColVector &q) ;
- static bool savePosFile(const char *filename, const vpColVector &q) ;
-
- void setMaxRotationVelocity(double w_max);
- void setMaxRotationVelocityJoint6(double w6_max);
-
- // Position control
- void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &position) ;
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1, const double pos2, const double pos3,
- const double pos4, const double pos5, const double pos6) ;
- void setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
-
- // State
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
-
- // Velocity control
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
-
- void stopMotion() ;
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
-
- double maxRotationVelocity_joint6;
-};
-
-
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-#endif /* #ifndef vpRobotViper850_h */
diff --git a/src/robot/real-robot/viper/vpViper.cpp b/src/robot/real-robot/viper/vpViper.cpp
deleted file mode 100644
index 250410c..0000000
--- a/src/robot/real-robot/viper/vpViper.cpp
+++ /dev/null
@@ -1,1302 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for a generic ADEPT Viper (either 650 or 850) robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpViper.cpp
-
- Modelisation of the ADEPT Viper 650 or 850 robot.
-
-*/
-
-
-
-#include <visp/vpDebug.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpViper.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-const unsigned int vpViper::njoint = 6;
-
-/*!
-
- Default constructor.
-
-*/
-vpViper::vpViper()
- : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), c56(0), joint_max(), joint_min()
-{
- // Default values are initialized
-
- // Denavit Hartenberg parameters
- a1 = 0.075;
- a2 = 0.365;
- a3 = 0.090;
- d1 = 0.335;
- d4 = 0.405;
- d6 = 0.080;
- c56 = -341.33 / 9102.22;
-
- // Software joint limits in radians
- joint_min.resize(njoint);
- joint_min[0] = vpMath::rad(-170);
- joint_min[1] = vpMath::rad(-190);
- joint_min[2] = vpMath::rad(-29);
- joint_min[3] = vpMath::rad(-190);
- joint_min[4] = vpMath::rad(-120);
- joint_min[5] = vpMath::rad(-360);
- joint_max.resize(njoint);
- joint_max[0] = vpMath::rad(170);
- joint_max[1] = vpMath::rad(45);
- joint_max[2] = vpMath::rad(256);
- joint_max[3] = vpMath::rad(190);
- joint_max[4] = vpMath::rad(120);
- joint_max[5] = vpMath::rad(360);
-
- // End effector to camera transformation
- eMc.setIdentity();
-}
-
-
-
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the six joint positions.
-
- This method is the same than get_fMc(const vpColVector & q).
-
- \param q : A six dimension vector corresponding to the robot joint
- positions expressed in radians.
-
- \return The homogeneous matrix \f$^f{\bf M}_c \f$ corresponding to
- the direct geometric model which expresses the transformation
- between the base frame and the camera frame.
-
- \sa get_fMc(const vpColVector & q)
- \sa getInverseKinematics()
-
-*/
-vpHomogeneousMatrix
-vpViper::getForwardKinematics(const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- fMc = get_fMc(q);
-
- return fMc;
-}
-
-/*!
-
- Convert a joint position by applying modulo \f$2 \pi\f$ to ensure
- that the position is in the joint limits.
-
- \param joint : Joint to consider.
-
- \param q : A joint position.
-
- \param q_mod : The joint position modified by considering modulo
- \f$2 \pi\f$ to be in the joint limits.
-
- \return true if the joint position is in the joint limits. false otherwise.
- */
-bool
-vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose) const
-{
- double eps = 0.01;
- if (q >= joint_min[joint]-eps && q <= joint_max[joint]+eps ) {
- q_mod = q;
- return true;
- }
-
- q_mod = q + 2*M_PI;
- if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
- return true;
- }
-
- q_mod = q - 2*M_PI;
- if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
- return true;
- }
-
- if (verbose) {
- std::cout << "Joint " << joint << " not in limits: "
- << this->joint_min[joint] << " < " << q << " < " << this->joint_max[joint] << std::endl;
-
- }
-
- return false;
-}
-
-/*!
-
- Compute the inverse kinematics (inverse geometric model).
-
- By inverse kinematics we mean here the six joint values given the
- position and the orientation of the camera frame relative to the
- base frame.
-
- \param fMw : Homogeneous matrix \f$^f{\bf M}_w \f$ describing the
- transformation from base frame to the wrist frame.
-
- \param q : In input, a six dimension vector corresponding to the
- current joint positions expressed in radians. In output, the
- solution of the inverse kinematics, ie. the joint positions
- corresponding to \f$^f{\bf M}_w \f$.
-
- \param verbose : Add extra printings.
-
- \return Add printings if no solution was found.
-
- \return The number of solutions (1 to 8) of the inverse geometric
- model. O, if no solution can be found.
-
- The code below shows how to compute the inverse geometric model:
-
- \code
- vpColVector q1(6), q2(6);
- vpHomogeneousMatrix fMw;
-
- vpViper robot;
-
- // Get the current joint position of the robot
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
-
- // Compute the pose of the wrist in the reference frame using the
- // direct geometric model
- robot.get_fMw(q1, fMw);
-
- // Compute the inverse geometric model
- int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
- // get the nearest solution to the current joint position
- nbsol = robot.getInverseKinematicsWrist(fMw, q1);
-
- if (nbsol == 0)
- std::cout << "No solution of the inverse geometric model " << std::endl;
- else if (nbsol >= 1)
- std::cout << "Nearest solution: " << q1 << std::endl;
- \endcode
-
- \sa getForwardKinematics(), getInverseKinematics()
-
-*/
-unsigned int
-vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose) const
-{
- vpColVector q_sol[8];
-
- for (unsigned int i=0; i<8; i++)
- q_sol[i].resize(6);
-
- double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
-
- bool ok[8];
-
- if (q.getRows() != njoint)
- q.resize(6);
-
- for (unsigned int i=0; i< 8; i++)
- ok[i] = true;
-
- double px = fMw[0][3]; // a*c1
- double py = fMw[1][3]; // a*s1
- double pz = fMw[2][3];
-
- // Compute q1
- double a_2 = px*px+py*py;
- //if (a_2 == 0) {// singularity
- if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {// singularity
- c1[0] = cos(q[0]);
- s1[0] = sin(q[0]);
- c1[4] = cos(q[0]+M_PI);
- s1[4] = sin(q[0]+M_PI);
- }
- else {
- double a = sqrt(a_2);
- c1[0] = px/a;
- s1[0] = py/a;
- c1[4] = -px/a;
- s1[4] = -py/a;
- }
-
- double q1_mod;
- for(unsigned int i=0;i<8;i+=4) {
- q_sol[i][0] = atan2(s1[i],c1[i]);
- if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
- q_sol[i][0] = q1_mod;
- for(unsigned int j=1;j<4;j++) {
- c1[i+j] = c1[i];
- s1[i+j] = s1[i];
- q_sol[i+j][0] = q_sol[i][0];
- }
- }
- else {
- for(unsigned int j=1;j<4;j++)
- ok[i+j] = false;
- }
- }
-
- // Compute q3
- double K, q3_mod;
- for(unsigned int i=0; i<8; i+=4) {
- if(ok[i] == true) {
- K = (px*px+py*py+pz*pz+a1*a1-a2*a2-a3*a3+d1*d1-d4*d4
- - 2*(a1*c1[i]*px + a1*s1[i]*py + d1*pz)) / (2*a2);
- double d4_a3_K = d4*d4+a3*a3-K*K;
-
- q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
- q_sol[i+2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
-
- for (unsigned int j=0; j<4; j+=2) {
- if (d4_a3_K < 0) {
- for(unsigned int k=0; k<2; k++)
- ok[i+j+k] = false;
- }
- else {
- if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod, verbose) == true) {
- for(unsigned int k=0; k<2; k++) {
- q_sol[i+j+k][2] = q3_mod;
- c3[i+j+k] = cos(q3_mod);
- s3[i+j+k] = sin(q3_mod);
- }
- }
- else {
- for(unsigned int k=0; k<2; k++)
- ok[i+j+k] = false;
- }
- }
- }
- }
- }
- // std::cout << "ok apres q3: ";
- // for (unsigned int i=0; i< 8; i++)
- // std::cout << ok[i] << " ";
- // std::cout << std::endl;
-
- // Compute q2
- double q23[8], q2_mod;
- for (unsigned int i=0; i<8; i+=2) {
- if (ok[i] == true) {
- // Compute q23 = q2+q3
- c23[i] = (-(a3-a2*c3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(d4+a2*s3[i]))
- / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
- s23[i] = ((d4+a2*s3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(a3-a2*c3[i]))
- / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
- q23[i] = atan2(s23[i],c23[i]);
- //std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] << std::endl;
- // q2 = q23 - q3
- q_sol[i][1] = q23[i] - q_sol[i][2];
-
- if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
- for(unsigned int j=0; j<2; j++) {
- q_sol[i+j][1] = q2_mod;
- c23[i+j] = c23[i];
- s23[i+j] = s23[i];
- }
- }
- else {
- for(unsigned int j=0; j<2; j++)
- ok[i+j] = false;
- }
- }
- }
- // std::cout << "ok apres q2: ";
- // for (unsigned int i=0; i< 8; i++)
- // std::cout << ok[i] << " ";
- // std::cout << std::endl;
-
- // Compute q4 as long as s5 != 0
- double r13 = fMw[0][2];
- double r23 = fMw[1][2];
- double r33 = fMw[2][2];
- double s4s5, c4s5, q4_mod, q5_mod;
- for (unsigned int i=0; i<8; i+=2) {
- if (ok[i] == true) {
- s4s5 = -s1[i]*r13+c1[i]*r23;
- c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
- if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
- // s5 = 0
- c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
- //std::cout << "Singularity: s5 near 0: ";
- if (c5[i] > 0.)
- q_sol[i][4] = 0.0;
- else
- q_sol[i][4] = M_PI;
-
- if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
- for(unsigned int j=0; j<2; j++) {
- q_sol[i+j][3] = q[3]; // keep current q4
- q_sol[i+j][4] = q5_mod;
- c4[i] = cos(q_sol[i+j][3]);
- s4[i] = sin(q_sol[i+j][3]);
- }
- }
- else {
- for(unsigned int j=0; j<2; j++)
- ok[i+j] = false;
- }
- }
- else {
- // s5 != 0
- //if (c4s5 == 0) {
- if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
- // c4 = 0
- // vpTRACE("c4 = 0");
- // q_sol[i][3] = q[3]; // keep current position
- q_sol[i][3] = atan2(s4s5, c4s5);
- }
- else {
- q_sol[i][3] = atan2(s4s5, c4s5);
- }
- if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
- q_sol[i][3] = q4_mod;
- c4[i] = cos(q4_mod);
- s4[i] = sin(q4_mod);
- }
- else {
- ok[i] = false;
- }
- if (q_sol[i][3] > 0.)
- q_sol[i+1][3] = q_sol[i][3] + M_PI;
- else
- q_sol[i+1][3] = q_sol[i][3] - M_PI;
- if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod, verbose) == true) {
- q_sol[i+1][3] = q4_mod;
- c4[i+1] = cos(q4_mod);
- s4[i+1] = sin(q4_mod);
- }
- else {
- ok[i+1] = false;
- }
-
- // Compute q5
- for (unsigned int j=0; j<2; j++) {
- if (ok[i+j] == true) {
- c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
- s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
- +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
-
- q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
- if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod, verbose) == true) {
- q_sol[i+j][4] = q5_mod;
- }
- else {
-
- ok[i+j] = false;
- }
- }
- }
- }
- }
- }
-
- // Compute q6
- // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
- double r12 = fMw[0][1];
- double r22 = fMw[1][1];
- double r32 = fMw[2][1];
- double q6_mod;
- for (unsigned int i=0; i<8; i++) {
- c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
- +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
- s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
- -s1[i]*s4[i]*c5[i])*r12
- -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
- +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
-
- q_sol[i][5] = atan2(s6[i], c6[i]);
- if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
- q_sol[i][5] = q6_mod;
- }
- else {
- ok[i] = false;
- }
- }
-
- // Select the best config in terms of distance from the current position
- unsigned int nbsol = 0;
- unsigned int sol = 0;
- vpColVector dist(8);
- for (unsigned int i=0; i<8; i++) {
- if (ok[i] == true) {
- nbsol ++;
- sol = i;
- // dist[i] = vpColVector::distance(q, q_sol[i]);
- vpColVector weight(6);
- weight = 1;
- weight[0] = 8;
- weight[1] = weight[2] = 4;
- dist[i] = 0;
- for (unsigned int j=0; j< 6; j++) {
- double rought_dist = q[j]- q_sol[i][j];
- double modulo_dist = rought_dist;
- if (rought_dist > 0) {
- if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
- modulo_dist = rought_dist - 2*M_PI;
- }
- else {
- 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) {
- for (unsigned int i=0; i<8; i++) {
- if (ok[i] == true)
- if (dist[i] < dist[sol]) sol = i;
- }
- // 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;
-
-}
-
-/*!
-
- Compute the inverse kinematics (inverse geometric model).
-
- By inverse kinematics we mean here the six joint values given the
- position and the orientation of the camera frame relative to the
- base frame.
-
- \param fMc : Homogeneous matrix \f$^f{\bf M}_c \f$ describing the
- transformation from base frame to the camera frame.
-
- \param q : In input, a six dimension vector corresponding to the
- current joint positions expressed in radians. In output, the
- solution of the inverse kinematics, ie. the joint positions
- corresponding to \f$^f{\bf M}_c \f$.
-
- \param verbose : Add extra printings.
-
- \return Add printings if no solution was found.
-
- \return The number of solutions (1 to 8) of the inverse geometric
- model. O, if no solution can be found.
-
- The code below shows how to compute the inverse geometric model:
-
- \code
- vpColVector q1(6), q2(6);
- vpHomogeneousMatrix fMc;
-
- vpViper robot;
-
- // Get the current joint position of the robot
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
-
- // Compute the pose of the camera in the reference frame using the
- // direct geometric model
- fMc = robot.getForwardKinematics(q1);
- // this is similar to fMc = robot.get_fMc(q1);
- // or robot.get_fMc(q1, fMc);
-
- // Compute the inverse geometric model
- int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
- // get the nearest solution to the current joint position
- nbsol = robot.getInverseKinematics(fMc, q1);
-
- if (nbsol == 0)
- std::cout << "No solution of the inverse geometric model " << std::endl;
- else if (nbsol >= 1)
- std::cout << "Nearest solution: " << q1 << std::endl;
- \endcode
-
- \sa getForwardKinematics(), getInverseKinematicsWrist
-
-*/
-unsigned int
-vpViper::getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose) const
-{
- vpHomogeneousMatrix fMw;
- vpHomogeneousMatrix wMe;
- vpHomogeneousMatrix eMc_;
- this->get_wMe(wMe);
- this->get_eMc(eMc_);
- fMw = fMc * eMc_.inverse() * wMe.inverse();
-
- return (getInverseKinematicsWrist(fMw, q, verbose));
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the joint positions of all
- the six joints.
-
- \f[
- ^f{\bf M}_c = ^f{\bf M}_e \; ^e{\bf M}_c
- \f]
-
- This method is the same than getForwardKinematics(const vpColVector & q).
-
- \param q : Vector of six joint positions expressed in
- radians.
-
- \return The homogeneous matrix corresponding to the direct geometric
- model which expresses the transformation between the base frame and the
- camera frame (fMc).
-
- \sa getForwardKinematics(const vpColVector & q), get_fMe(), get_eMc()
-
-*/
-vpHomogeneousMatrix
-vpViper::get_fMc (const vpColVector & q) const
-{
- vpHomogeneousMatrix fMc;
- get_fMc(q, fMc);
-
- return fMc;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix.
-
- By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the six joint positions.
-
- \f[
- ^f{\bf M}_c = ^f{\bf M}_e \; {^e}{\bf M}_c
- \f]
-
- \param q : Vector of six joint positions expressed in
- radians.
-
- \param fMc The homogeneous matrix \f$^f{\bf M}_c\f$corresponding to
- the direct geometric model which expresses the transformation
- between the fix frame and the camera frame.
-
- \sa get_fMe(), get_eMc()
-*/
-void
-vpViper::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
-{
-
- // Compute the direct geometric model: fMe = transformation between
- // fix and end effector frame.
- vpHomogeneousMatrix fMe;
-
- get_fMe(q, fMe);
-
- fMc = fMe * this->eMc;
-
- return;
-}
-
-/*!
-
- Compute the forward kinematics (direct geometric model) as an
- homogeneous matrix \f${^f}{\bf M}_e\f$.
-
- By forward kinematics we mean here the position and the orientation
- of the end effector with respect to the base frame given the
- motor positions of all the six joints.
-
- \f[
- {^f}M_e = \left(\begin{array}{cccc}
- r_{11} & r_{12} & r_{13} & t_x \\
- r_{21} & r_{22} & r_{23} & t_y \\
- r_{31} & r_{32} & r_{33} & t_z \\
- \end{array}
- \right)
- \f]
-
- with
- \f[
- \begin{array}{l}
- r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\
- r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\
- r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\
- \\
- r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\
- r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\
- r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\
- \\
- r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\
- r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\
- r_{33} = -s23c4s5+c23c5\\
- \\
- t_x = c1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)-s1s4s5d6\\
- t_y = s1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)+c1s4s5d6\\
- t_z = s23(a3-c4s5d6)+c23(c5d6+d4)-a2s2+d1\\
- \end{array}
- \f]
-
- \param q : A 6-dimension vector that contains the 6 joint positions
- expressed in radians.
-
- \param fMe The homogeneous matrix \f${^f}{\bf M}_e\f$ corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- end effector frame.
-
- Note that this transformation can also be computed by considering the wrist
- frame \f${^f}{\bf M}_e = {^f}{\bf M}_w *{^w}{\bf M}_e\f$.
-
- \code
-#include <visp/vpViper.h>
-
-int main()
-{
- vpViper robot;
- vpColVector q(6); // The measured six joint positions
-
- vpHomogeneousMatrix fMe; // Transformation from fix frame to end-effector
- robot.get_fMe(q, fMe); // Get the forward kinematics
-
- // The forward kinematics can also be computed by considering the wrist frame
- vpHomogeneousMatrix fMw; // Transformation from fix frame to wrist frame
- robot.get_fMw(q, fMw);
- vpHomogeneousMatrix wMe; // Transformation from wrist frame to end-effector
- robot.get_wMe(wMe); // Constant transformation
-
- // Compute the forward kinematics
- fMe = fMw * wMe;
-}
- \endcode
-
-*/
-void
-vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
-{
- double q1 = q[0];
- double q2 = q[1];
- double q3 = q[2];
- double q4 = q[3];
- double q5 = q[4];
- double q6 = q[5];
- // We turn off the coupling since the measured positions are joint position
- // taking into account the coupling factor. The coupling factor is relevant
- // 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);
- double s2 = sin(q2);
- //double c3 = cos(q3);
- //double s3 = sin(q3);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
- double c6 = cos(q6);
- double s6 = sin(q6);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
-
- fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
-
- fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
-
- fMe[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMe[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMe[2][2] = -s23*c4*s5+c23*c5;
-
- fMe[0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
- fMe[1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
- fMe[2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
-
- // std::cout << "Effector position fMe: " << std::endl << fMe;
-
- return;
-}
-/*!
-
- Compute the transformation between the fix frame and the wrist frame. The
- wrist frame is located on the intersection of the 3 last rotations.
-
- \param q : A 6-dimension vector that contains the 6 joint positions
- expressed in radians.
-
- \param fMw The homogeneous matrix corresponding to the transformation between
- the fix frame and the wrist frame (fMw).
-
- \f[
- {^f}M_w = \left(\begin{array}{cccc}
- r_{11} & r_{12} & r_{13} & t_x \\
- r_{21} & r_{22} & r_{23} & t_y \\
- r_{31} & r_{32} & r_{33} & t_z \\
- \end{array}
- \right)
- \f]
-
- with
- \f[
- \begin{array}{l}
- r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\
- r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\
- r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\
- \\
- r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\
- r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\
- r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\
- \\
- r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\
- r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\
- r_{33} = -s23c4s5+c23c5\\
- \\
- t_x = c1(-c23a3+s23d4+a1+a2c2)\\
- t_y = s1(-c23a3+s23d4+a1+a2c2)\\
- t_z = s23a3+c23d4-a2s2+d1\\
- \end{array}
- \f]
-
-*/
-void
-vpViper::get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const
-{
- double q1 = q[0];
- double q2 = q[1];
- double q3 = q[2];
- double q4 = q[3];
- double q5 = q[4];
- double q6 = q[5];
- // We turn off the coupling since the measured positions are joint position
- // taking into account the coupling factor. The coupling factor is relevant
- // 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);
- double s2 = sin(q2);
- // double c3 = cos(q3);
- //double s3 = sin(q3);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
- double c6 = cos(q6);
- double s6 = sin(q6);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
-
- fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
-
- fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
-
- fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMw[2][2] = -s23*c4*s5+c23*c5;
-
- fMw[0][3] = c1*(-c23*a3+s23*d4+a1+a2*c2);
- fMw[1][3] = s1*(-c23*a3+s23*d4+a1+a2*c2);
- fMw[2][3] = s23*a3+c23*d4-a2*s2+d1;
-
- //std::cout << "Wrist position fMw: " << std::endl << fMw;
-
- return;
-}
-
-/*!
-
- Return the transformation between the wrist frame and the end-effector. The
- wrist frame is located on the intersection of the 3 last rotations.
-
-
- \param wMe The homogeneous matrix corresponding to the transformation between
- the wrist frame and the end-effector frame (wMe).
-
-*/
-void
-vpViper::get_wMe(vpHomogeneousMatrix & wMe) const
-{
- // Set the rotation as identity
- wMe.setIdentity();
-
- // Set the translation
- wMe[2][3] = d6;
-}
-
-/*!
-
- Get the geometric transformation between the end-effector frame and
- the camera frame. This transformation is constant and correspond to
- the extrinsic camera parameters estimated by calibration.
-
- \param eMc_ : Transformation between the the
- end-effector frame and the camera frame.
-
- \sa get_cMe()
-*/
-void
-vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const
-{
- eMc_ = this->eMc;
-}
-
-/*!
-
- Get the geometric transformation between the camera frame and the
- end-effector frame. This transformation is constant and correspond
- to the extrinsic camera parameters estimated by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-
- \sa get_eMc()
-*/
-void
-vpViper::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- cMe = this->eMc.inverse();
-}
-
-/*!
-
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
- \f[
- ^c{\bf V}_e = \left(\begin{array}{cc}
- ^c{\bf R}_e & [^c{\bf t}_e]_\times ^c{\bf R}_e\\
- {\bf 0}_{3\times 3} & ^c{\bf R}_e
- \end{array}
- \right)
- \f]
- \param cVe : Twist transformation \f$^c{\bf V}_e\f$.
-
-*/
-void
-vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-
- return;
-}
-
-/*!
-
- Get the robot jacobian \f${^e}{\bf J}_e\f$ which gives the velocity
- of the origin of the end-effector frame expressed in end-effector frame.
-
- \f[
- {^e}{\bf J}_e = \left[\begin{array}{cc}
- {^w}{\bf R}_f & {[{^e}{\bf t}_w}]_\times \; {^w}{\bf R}_f \\
- 0_{3\times3} & {^w}{\bf R}_f
- \end{array}
- \right] \;
- {^f}{\bf J}_w
- \f]
-
- \param q : A six-dimension vector that contains the joint positions
- of the robot expressed in radians.
-
- \param eJe : Robot jacobian \f${^e}{\bf J}_e\f$ that express the
- velocity of the end-effector in the robot end-effector frame.
-
- \sa get_fJw()
-*/
-void
-vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
-{
- vpMatrix V(6,6);
- V = 0;
- // Compute the first and last block of V
- vpHomogeneousMatrix fMw;
- get_fMw(q, fMw);
- vpRotationMatrix fRw;
- fMw.extract(fRw);
- vpRotationMatrix wRf;
- wRf = fRw.inverse();
- for (unsigned int i=0; i<3; i++ ) {
- for (unsigned int j=0; j<3; j++ ) {
- V[i][j] = V[i+3][j+3] = wRf[i][j];
- }
- }
- // Compute the second block of V
- vpHomogeneousMatrix wMe;
- get_wMe(wMe);
- vpHomogeneousMatrix eMw;
- eMw = wMe.inverse();
- vpTranslationVector etw;
- eMw.extract(etw);
- vpMatrix block2 = etw.skew()*wRf;
- for (unsigned int i=0; i<3; i++ ) {
- for (unsigned int j=0; j<3; j++ ) {
- V[i][j+3] = block2[i][j];
- }
- }
- // Compute eJe
- vpMatrix fJw;
- get_fJw(q, fJw);
- eJe = V * fJw;
-
- return;
-}
-
-
-/*!
-
- Get the robot jacobian \f${^f}{\bf J}_w\f$ which express the
- velocity of the origin of the wrist frame in the robot reference
- frame also called fix frame.
-
- \f[
- {^f}J_w = \left(\begin{array}{cccccc}
- J_{11} & J_{12} & J_{13} & 0 & 0 & 0 \\
- J_{21} & J_{22} & J_{23} & 0 & 0 & 0 \\
- 0 & J_{32} & J_{33} & 0 & 0 & 0 \\
- 0 & -s1 & -s1 & c1s23 & J_{45} & J_{46} \\
- 0 & c1 & c1 & s1s23 & J_{55} & J_{56} \\
- 1 & 0 & 0 & c23 & s23s4 & J_{56} \\
- \end{array}
- \right)
- \f]
-
- with
- \f[
- \begin{array}{l}
- J_{11} = -s1(-c23a3+s23d4+a1+a2c2) \\
- J_{21} = c1(-c23a3+s23d4+a1+a2c2) \\
- J_{12} = c1(s23a3+c23d4-a2s2) \\
- J_{22} = s1(s23a3+c23d4-a2s2) \\
- J_{32} = c23a3-s23d4-a2c2 \\
- J_{13} = c1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\
- J_{23} = s1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\
- J_{33} = -a3(s2s3-c2c3)-d4(s2c3+c2s3)\\
- J_{45} = -c23c1s4-s1c4\\
- J_{55} = c1c4-c23s1s4\\
- J_{46} = (c1c23c4-s1s4)s5+c1s23c5\\
- J_{56} = (s1c23c4+c1s4)s5+s1s23c5\\
- J_{66} = -s23c4s5+c23c5\\
- \end{array}
- \f]
-
- \param q : A six-dimension vector that contains the joint positions
- of the robot expressed in radians.
-
- \param fJw : Robot jacobian \f${^f}{\bf J}_w\f$ that express the
- velocity of the point \e w (origin of the wrist frame) in the robot
- reference frame.
-
- \sa get_fJe(), get_eJe()
-*/
-
-void
-vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
-{
- double q1 = q[0];
- double q2 = q[1];
- double q3 = q[2];
- double q4 = q[3];
- double q5 = q[4];
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
- double c3 = cos(q3);
- double s3 = sin(q3);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
-
- vpColVector J1(6);
- vpColVector J2(6);
- vpColVector J3(6);
- vpColVector J4(6);
- vpColVector J5(6);
- vpColVector J6(6);
-
- // Jacobian when d6 is set to zero
- J1[0] = -s1*(-c23*a3+s23*d4+a1+a2*c2);
- J1[1] = c1*(-c23*a3+s23*d4+a1+a2*c2);
- J1[2] = 0;
- J1[3] = 0;
- J1[4] = 0;
- J1[5] = 1;
-
- J2[0] = c1*(s23*a3+c23*d4-a2*s2);
- J2[1] = s1*(s23*a3+c23*d4-a2*s2);
- J2[2] = c23*a3-s23*d4-a2*c2;
- J2[3] = -s1;
- J2[4] = c1;
- J2[5] = 0;
-
- J3[0] = c1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
- J3[1] = s1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
- J3[2] = -a3*(s2*s3-c2*c3)-d4*(s2*c3+c2*s3);
- J3[3] = -s1;
- J3[4] = c1;
- J3[5] = 0;
-
- J4[0] = 0;
- J4[1] = 0;
- J4[2] = 0;
- J4[3] = c1*s23;
- J4[4] = s1*s23;
- J4[5] = c23;
-
- J5[0] = 0;
- J5[1] = 0;
- J5[2] = 0;
- J5[3] = -c23*c1*s4-s1*c4;
- J5[4] = c1*c4-c23*s1*s4;
- J5[5] = s23*s4;
-
- J6[0] = 0;
- J6[1] = 0;
- J6[2] = 0;
- J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
- J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
- J6[5] = -s23*c4*s5+c23*c5;
-
- fJw.resize(6,6) ;
- for (unsigned int i=0;i<6;i++) {
- fJw[i][0] = J1[i];
- fJw[i][1] = J2[i];
- fJw[i][2] = J3[i];
- fJw[i][3] = J4[i];
- fJw[i][4] = J5[i];
- fJw[i][5] = J6[i];
- }
- return;
-}
-/*!
-
- Get the robot jacobian \f${^f}{\bf J}_e\f$ which gives the velocity
- of the origin of the end-effector frame expressed in the robot
- reference frame also called fix frame.
-
- \f[
- {^f}{\bf J}_e = \left[\begin{array}{cc}
- I_{3\times3} & [{^f}{\bf R}_w \; {^e}{\bf t}_w]_\times \\
- 0_{3\times3} & I_{3\times3}
- \end{array}
- \right]
- {^f}{\bf J}_w
- \f]
-
- \param q : A six-dimension vector that contains the joint positions
- of the robot expressed in radians.
-
- \param fJe : Robot jacobian \f${^f}{\bf J}_e\f$ that express the
- velocity of the end-effector in the robot reference frame.
-
- \sa get_fJw
-*/
-void
-vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
-{
- vpMatrix V(6,6);
- V = 0;
- // Set the first and last block to identity
- for (unsigned int i=0; i<6; i++ )
- V[i][i] = 1;
-
- // Compute the second block of V
- vpHomogeneousMatrix fMw;
- get_fMw(q, fMw);
- vpRotationMatrix fRw;
- fMw.extract(fRw);
- vpHomogeneousMatrix wMe;
- get_wMe(wMe);
- vpHomogeneousMatrix eMw;
- eMw = wMe.inverse();
- vpTranslationVector etw;
- eMw.extract(etw);
- vpMatrix block2 = (fRw*etw).skew();
- // Set the second block
- for (unsigned int i=0; i<3; i++ )
- for (unsigned int j=0; j<3; j++ )
- V[i][j+3] = block2[i][j];
-
- // Compute fJe
- vpMatrix fJw;
- get_fJw(q, fJw);
- fJe = V * fJw;
-
- return;
-}
-
-
-/*!
- Get minimal joint values.
-
- \return A 6-dimension vector that contains the minimal joint values
- for the 6 dof. All the values are expressed in radians.
-
-*/
-vpColVector
-vpViper::getJointMin() const
-{
- return joint_min;
-}
-
-/*!
- Get maximal joint values.
-
- \return A 6-dimension vector that contains the maximal joint values
- for the 6 dof. All the values are expressed in radians.
-
-*/
-vpColVector
-vpViper::getJointMax() const
-{
- return joint_max;
-}
-
-/*!
-
- Return the coupling factor between join 5 and joint 6.
-
- This factor should be only useful when motor positions are
- considered. Since the positions returned by the robot are joint
- positions which takes into account the coupling factor, it has not to
- be considered in the modelization of the robot.
-
-*/
-double
-vpViper::getCoupl56() const
-{
- return c56;
-}
-
-/*!
-
- Print on the output stream \e os the robot parameters (joint
- min/max, coupling factor between axis 5 and 6, hand-to-eye constant
- homogeneous matrix \f$^e{\bf M}_c \f$.
-
- \param os : Output stream.
- \param viper : Robot parameters.
-*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper)
-{
- vpRotationMatrix eRc;
- viper.eMc.extract(eRc);
- vpRxyzVector rxyz(eRc);
-
- // Convert joint limits in degrees
- vpColVector jmax = viper.joint_max;
- vpColVector jmin = viper.joint_min;
- jmax.rad2deg();
- jmin.rad2deg();
-
- os
- << "Joint Max (deg):" << std::endl
- << "\t" << jmax.t() << std::endl
-
- << "Joint Min (deg): " << std::endl
- << "\t" << jmin.t() << std::endl
-
- << "Coupling 5-6:" << std::endl
- << "\t" << viper.c56 << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << viper.eMc[0][3] << " "
- << viper.eMc[1][3] << " "
- << viper.eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
-
- return os;
-}
diff --git a/src/robot/real-robot/viper/vpViper.h b/src/robot/real-robot/viper/vpViper.h
deleted file mode 100644
index e67984c..0000000
--- a/src/robot/real-robot/viper/vpViper.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper.h 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for a generic ADEPT Viper (either 650 or 850) robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpViper_h
-#define vpViper_h
-
-/*!
-
- \file vpViper.h
-
- Modelisation of the ADEPT Viper 650 or 850 robot.
-
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpRobotException.h>
-
-/*!
-
- \class vpViper
-
- \ingroup Viper
-
- \brief Modelisation of the ADEPT Viper robot
-
- This robot has six degrees of freedom.
-
- The non modified Denavit-Hartenberg representation of the robot is
- given in the table below, where \f$q_1^*, \ldots, q_6^*\f$
- are the variable joint positions.
-
- \f[
- \begin{tabular}{|c|c|c|c|c|}
- \hline
- Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
- \hline
- 1 & $a_1$ & $d_1$ & $-\pi/2$ & $q_1^*$ \\
- 2 & $a_2$ & 0 & 0 & $q_2^*$ \\
- 3 & $a_3$ & 0 & $-\pi/2$ & $q_3^* - \pi$ \\
- 4 & 0 & $d_4$ & $\pi/2$ & $q_4^*$ \\
- 5 & 0 & 0 & $-\pi/2$ & $q_5^*$ \\
- 6 & 0 & 0 & 0 & $q_6^*-\pi$ \\
- 7 & 0 & $d_6$ & 0 & 0 \\
- \hline
- \end{tabular}
- \f]
-
- In this modelisation, different frames have to be considered.
- - \f$ {\cal F}_f \f$: the reference frame, also called world frame,
-
- - \f$ {\cal F}_w \f$: the wrist frame located at the intersection of
- the last three rotations, with \f$ ^f{\bf M}_w = ^0{\bf M}_6 \f$,
-
- - \f$ {\cal F}_e \f$: the end-effector frame, with \f$^f{\bf M}_e =
- 0{\bf M}_7 \f$,
-
- - \f$ {\cal F}_c \f$: the camera frame, with \f$^f{\bf M}_c = ^f{\bf
- M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage.
-
- The forward kinematics of the robot is implemented in get_fMw(),
- get_fMe() and get_fMc().
-
- The robot forward jacobian used to compute the cartesian velocities
- from joint ones is given and implemented in get_fJw(), get_fJe() and
- get_eJe().
-
-*/
-class VISP_EXPORT vpViper
-{
- public:
- vpViper();
- virtual ~vpViper() {};
-
- vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
- unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose=false) const;
- unsigned int getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose=false) const;
- vpHomogeneousMatrix get_fMc (const vpColVector & q) const;
- void get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const;
- void get_wMe(vpHomogeneousMatrix & wMe) const;
- void get_eMc(vpHomogeneousMatrix & eMc) const;
- void get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const;
- void get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const;
-
- void get_cMe(vpHomogeneousMatrix &cMe) const;
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_fJw(const vpColVector &q, vpMatrix &fJw) const;
- void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
- void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
-
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper);
-
- vpColVector getJointMin() const;
- vpColVector getJointMax() const;
- double getCoupl56() const;
-
- private:
- bool convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose=false) const;
-
- public:
- static const unsigned int njoint; ///< Number of joint.
-
- protected:
- vpHomogeneousMatrix eMc; //!< End effector to camera transformation
- // Minimal representation of eMc
- vpTranslationVector etc; // meters
- vpRxyzVector erc; // radian
-
- // Denavit-Hartenberg parameters
- double a1, d1; //!< for joint 1
- double a2; //!< for joint 2
- double a3; //!< for joint 3
- double d4; //!< for joint 4
- double d6; //!< for joint 6
- double c56; //!< Mechanical coupling between joint 5 and joint 6
-
- // Software joint limits in radians
- vpColVector joint_max; // Maximal value of the joints
- vpColVector joint_min; // Minimal value of the joints
-};
-
-#endif
-
diff --git a/src/robot/real-robot/viper/vpViper650.cpp b/src/robot/real-robot/viper/vpViper650.cpp
deleted file mode 100644
index 0e8c2df..0000000
--- a/src/robot/real-robot/viper/vpViper650.cpp
+++ /dev/null
@@ -1,876 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper650.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ADEPT Viper 650 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpViper650.cpp
-
- Modelisation of the ADEPT Viper 650 robot.
-
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpViper650.h>
-#include <visp/vpMath.h>
-#include <visp/vpXmlParserCamera.h>
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
- NULL};
-
-const char * const vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_schunk_gripper_without_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_schunk_gripper_without_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_generic_without_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_generic_without_distortion_Viper650.cnf";
-#endif
-
-const char * const vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_eMc_generic_with_distortion_Viper650.cnf";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_eMc_generic_with_distortion_Viper650.cnf";
-#endif
-
-
-const char * const vpViper650::CONST_CAMERA_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper650/current/include/const_camera_Viper650.xml";
-#else
-= "/udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml";
-#endif
-
-
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
-const char * const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
-const char * const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
-const char * const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
-const char * const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
-
-const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA;
-
-
-
-/*!
-
- Default constructor.
- Sets the specific parameters like the Denavit Hartenberg parameters.
-
-*/
-vpViper650::vpViper650()
- : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
-{
- // Denavit Hartenberg parameters
- a1 = 0.075;
- a2 = 0.270;
- a3 = 0.090;
- d1 = 0.335;
- d4 = 0.295;
- d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
- c56 = -341.33 / 9102.22;
-
- // Software joint limits in radians
- joint_min[0] = vpMath::rad(-170);
- joint_min[1] = vpMath::rad(-190);
- joint_min[2] = vpMath::rad(-29);
- joint_min[3] = vpMath::rad(-190);
- joint_min[4] = vpMath::rad(-120);
- joint_min[5] = vpMath::rad(-360);
- joint_max[0] = vpMath::rad(170);
- joint_max[1] = vpMath::rad(45);
- joint_max[2] = vpMath::rad(256);
- joint_max[3] = vpMath::rad(190);
- joint_max[4] = vpMath::rad(120);
- joint_max[5] = vpMath::rad(360);
-
- init(); // Set the default tool
-}
-
-/*!
-
- Initialize the robot with the default tool vpViper650::defaultTool.
- */
-void
-vpViper650::init (void)
-{
- this->init(vpViper650::defaultTool);
- return;
-}
-
-/*!
-
- Read files containing the constant parameters related to the robot
- tools in order to set the end-effector to camera transformation.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param camera_extrinsic_parameters : Filename containing the camera
- extrinsic parameters.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpViper650::init (const char *camera_extrinsic_parameters)
-{
- //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
- this->parseConfigFile (camera_extrinsic_parameters);
-
- return ;
-}
-#endif
-
-/*!
-
- Set the constant parameters related to the robot kinematics and to
- the end-effector to camera transformation (\f$^e{\bf M}c\f$)
- corresponding to the camera extrinsic parameters. These last
- parameters depend on the camera and projection model in use.
-
- \param tool : Camera in use.
-
- \param proj_model : Projection model of the camera.
-
-*/
-void
-vpViper650::init (vpViper650::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType proj_model)
-{
-
- this->projModel = proj_model;
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- // Read the robot parameters from files
- char filename_eMc [FILENAME_MAX];
- switch (tool) {
- case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper650::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- 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;
- }
- }
-
- this->init (filename_eMc);
-
-#else // VISP_HAVE_ACCESS_TO_NAS
-
- // Use here default values of the robot constant parameters.
- switch (tool) {
- case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
- erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.26); // rx
- erc[1] = vpMath::rad(2.12); // ry
- erc[2] = vpMath::rad(-91.31); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0005; // ty
- etc[2] = 0.1022; // tz
- break;
- }
- break;
- }
- case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.15); // rx
- erc[1] = vpMath::rad(1.28); // ry
- erc[2] = vpMath::rad(-90.8); // rz
- etc[0] = -0.0456; // tx
- etc[1] = -0.0013; // ty
- etc[2] = 0.001; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.72); // rx
- erc[1] = vpMath::rad(2.10); // ry
- erc[2] = vpMath::rad(-90.5); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0012; // ty
- etc[2] = 0.078; // tz
- break;
- }
- break;
- }
- case vpViper650::TOOL_GENERIC_CAMERA: {
- // Set eMc to identity
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = 0; // rx
- erc[1] = 0; // ry
- erc[2] = 0; // rz
- etc[0] = 0; // tx
- etc[1] = 0; // ty
- etc[2] = 0; // tz
- break;
- }
- break;
- }
- }
- vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc, eRc);
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
- setToolType(tool);
- return ;
-}
-
-/*!
-
- This function gets the robot constant parameters from a file.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param filename : File name containing the robot constant
- parameters, like the hand-to-eye transformation.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpViper650::parseConfigFile (const char * filename)
-{
- size_t dim;
- int code;
- char Ligne[FILENAME_MAX];
- char namoption[100];
- FILE * fdtask;
- int numLn = 0;
- double rot_eMc[3]; // rotation
- double trans_eMc[3]; // translation
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
-
- //vpTRACE("Read the config file for constant parameters %s.", filename);
- if ((fdtask = fopen(filename, "r" )) == NULL)
- {
- vpERROR_TRACE ("Impossible to read the config file %s.",
- filename);
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file.");
- }
-
- while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
- numLn ++;
- if ('#' == Ligne[0]) { continue; }
- sscanf(Ligne, "%s", namoption);
- dim = strlen(namoption);
-
- for (code = 0;
- NULL != opt_viper650[code];
- ++ code)
- {
- if (strncmp(opt_viper650[code], namoption, dim) == 0)
- {
- break;
- }
- }
-
- switch(code) {
- case 0:
- break; // Nothing to do: camera name
-
- case 1:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &rot_eMc[0],
- &rot_eMc[1],
- &rot_eMc[2]);
-
- // Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
- break;
-
- case 2:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &trans_eMc[0],
- &trans_eMc[1],
- &trans_eMc[2]);
- get_trans_eMc = true;
- break;
-
- default:
- vpERROR_TRACE ("Bad configuration file %s "
- "ligne #%d.", filename, numLn);
- } /* SWITCH */
- } /* WHILE */
-
- fclose (fdtask);
-
- // Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- for (unsigned int i=0; i < 3; i ++) {
- erc[i] = rot_eMc[i];
- etc[i] = trans_eMc[i];
- }
-
- vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc, eRc);
- }
-
- return;
-}
-#endif
-
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper650::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param image_width : Image width used to compute camera calibration.
- \param image_height : Image height used to compute camera calibration.
-
- The code below shows how to get the camera parameters of the camera
- attached to the robot.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper650.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-#else
- vpViper650 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- // Camera parameters are read from
- // /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
- // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined
- // in vpConfig.h file
- try {
- robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
-{
-#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
- vpXmlParserCamera parser;
- switch (getToolType()) {
- case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_MARLIN_F033C_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper650::TOOL_GENERIC_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_GENERIC_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- 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).");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- break;
- }
- }
-#else
- // Set default parameters
- switch (getToolType()) {
- case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper650::TOOL_GENERIC_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- default:
- vpERROR_TRACE ("This error should not occur!");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- break;
- }
-#endif
- return;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper650::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param I : A B&W image send by the current camera in use.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper650.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-#else
- vpViper650 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
-/*!
- \brief Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper650::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param I : A color image send by the current camera in use.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper650.h>
-#include <visp/vpRobotViper650.h>
-
-int main()
-{
- vpImage<vpRGBa> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER650
- vpRobotViper650 robot;
-#else
- vpViper650 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
diff --git a/src/robot/real-robot/viper/vpViper650.h b/src/robot/real-robot/viper/vpViper650.h
deleted file mode 100644
index b19a7aa..0000000
--- a/src/robot/real-robot/viper/vpViper650.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper650.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ADEPT Viper 650 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpViper650_h
-#define vpViper650_h
-
-/*!
-
- \file vpViper650.h
-
- Modelisation of the ADEPT Viper 650 robot.
-
-*/
-
-/*!
-
- \class vpViper650
-
- \ingroup Viper
-
- \brief Modelisation of the ADEPT Viper 650 robot.
-
-*/
-
-#include <visp/vpViper.h>
-
-
-class VISP_EXPORT vpViper650: public vpViper
-{
- public:
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- //! Files where constant tranformation between end-effector and camera frame
- //! are stored.
- static const char * const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
- static const char * const CONST_CAMERA_FILENAME;
-#endif
- /*!
- Name of the camera attached to the end-effector.
- */
- static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
- static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
- static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
- static const char * const CONST_GENERIC_CAMERA_NAME;
-
- //! List of possible tools that can be attached to the robot end-effector.
- typedef enum {
- TOOL_MARLIN_F033C_CAMERA, /*!< Marlin F033C camera. */
- TOOL_PTGREY_FLEA2_CAMERA, /*!< Point Grey Flea 2 camera. */
- TOOL_SCHUNK_GRIPPER_CAMERA, /*!< Camera attached to the Schunk gripper. */
- TOOL_GENERIC_CAMERA /*!< A generic camera. */
- } vpToolType;
-
- //! Default tool attached to the robot end effector
- static const vpToolType defaultTool;
-
- vpViper650();
- virtual ~vpViper650() {};
-
- void init (void);
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void init(const char *camera_extrinsic_parameters);
-#endif
- void init (vpViper650::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel =
- vpCameraParameters::perspectiveProjWithoutDistortion);
-
-
- //! Get the current camera model projection type
- vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const {
- return projModel;
- };
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
-
- //! Get the current tool type
- vpToolType getToolType() const{
- return tool_current;
- };
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void parseConfigFile (const char * filename);
-#endif
-
- protected:
- //! Set the current tool type
- void setToolType(vpViper650::vpToolType tool){
- tool_current = tool;
- };
-
- protected:
- //! Current tool in use
- vpToolType tool_current;
- // Used projection model
- vpCameraParameters::vpCameraParametersProjType projModel;
-
-};
-
-#endif
-
diff --git a/src/robot/real-robot/viper/vpViper850.cpp b/src/robot/real-robot/viper/vpViper850.cpp
deleted file mode 100644
index 3a29dc4..0000000
--- a/src/robot/real-robot/viper/vpViper850.cpp
+++ /dev/null
@@ -1,877 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper850.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ADEPT Viper 850 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \file vpViper850.cpp
-
- Modelisation of the ADEPT Viper 850 robot.
-
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpViper850.h>
-#include <visp/vpMath.h>
-#include <visp/vpXmlParserCamera.h>
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ",
- NULL};
-
-const char * const vpViper850::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
-#endif
-
-const char * const vpViper850::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
-#endif
-
-
-const char * const vpViper850::CONST_CAMERA_FILENAME
-#if defined(_WIN32)
-= "Z:/robot/Viper850/current/include/const_camera_Viper850.xml";
-#else
-= "/udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml";
-#endif
-
-
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
-const char * const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
-const char * const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
-const char * const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
-const char * const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
-
-const vpViper850::vpToolType vpViper850::defaultTool = vpViper850::TOOL_PTGREY_FLEA2_CAMERA;
-
-
-
-/*!
-
- Default constructor.
- Sets the specific parameters like the Denavit Hartenberg parameters.
-
-*/
-vpViper850::vpViper850()
- : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
-
-{
- // Denavit Hartenberg parameters
- a1 = 0.075;
- a2 = 0.365;
- a3 = 0.090;
- d1 = 0.335;
- d4 = 0.405;
- d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
- c56 = -341.33 / 9102.22;
-
- // Software joint limits in radians
- joint_min[0] = vpMath::rad(-170);
- joint_min[1] = vpMath::rad(-190);
- joint_min[2] = vpMath::rad(-29);
- joint_min[3] = vpMath::rad(-190);
- joint_min[4] = vpMath::rad(-120);
- joint_min[5] = vpMath::rad(-360);
- joint_max[0] = vpMath::rad(170);
- joint_max[1] = vpMath::rad(45);
- joint_max[2] = vpMath::rad(256);
- joint_max[3] = vpMath::rad(190);
- joint_max[4] = vpMath::rad(120);
- joint_max[5] = vpMath::rad(360);
-
- init(); // Set the default tool
-}
-
-/*!
-
- Initialize the robot with the default tool vpViper850::defaultTool.
- */
-void
-vpViper850::init (void)
-{
- this->init(vpViper850::defaultTool);
- return;
-}
-
-/*!
-
- Read files containing the constant parameters related to the robot
- tools in order to set the end-effector to camera transformation.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param camera_extrinsic_parameters : Filename containing the camera
- extrinsic parameters.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpViper850::init (const char *camera_extrinsic_parameters)
-{
- //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
- this->parseConfigFile (camera_extrinsic_parameters);
-
- return ;
-}
-#endif
-
-/*!
-
- Set the constant parameters related to the robot kinematics and to
- the end-effector to camera transformation (\f$^e{\bf M}c\f$)
- corresponding to the camera extrinsic parameters. These last
- parameters depend on the camera and projection model in use.
-
- \param tool : Camera in use.
-
- \param proj_model : Projection model of the camera.
-
-*/
-void
-vpViper850::init (vpViper850::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType proj_model)
-{
-
- this->projModel = proj_model;
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- // Read the robot parameters from files
- char filename_eMc [FILENAME_MAX];
- switch (tool) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- case vpViper850::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME);
-#endif
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#else // _WIN32
- _snprintf(filename_eMc, FILENAME_MAX, "%s",
- CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME);
-#endif
- break;
- }
- break;
- }
- 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;
- }
- }
-
- this->init (filename_eMc);
-
-#else // VISP_HAVE_ACCESS_TO_NAS
-
- // Use here default values of the robot constant parameters.
- switch (tool) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
- erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.26); // rx
- erc[1] = vpMath::rad(2.12); // ry
- erc[2] = vpMath::rad(-91.31); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0005; // ty
- etc[2] = 0.1022; // tz
- break;
- }
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.15); // rx
- erc[1] = vpMath::rad(1.28); // ry
- erc[2] = vpMath::rad(-90.8); // rz
- etc[0] = -0.0456; // tx
- etc[1] = -0.0013; // ty
- etc[2] = 0.001; // tz
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.72); // rx
- erc[1] = vpMath::rad(2.10); // ry
- erc[2] = vpMath::rad(-90.5); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0012; // ty
- etc[2] = 0.078; // tz
- break;
- }
- break;
- }
- case vpViper850::TOOL_GENERIC_CAMERA: {
- // Set eMc to identity
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = 0; // rx
- erc[1] = 0; // ry
- erc[2] = 0; // rz
- etc[0] = 0; // tx
- etc[1] = 0; // ty
- etc[2] = 0; // tz
- break;
- }
- break;
- }
- }
- vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc, eRc);
-#endif // VISP_HAVE_ACCESS_TO_NAS
-
- setToolType(tool);
- return ;
-}
-
-/*!
-
- This function gets the robot constant parameters from a file.
-
- \warning This function is only available if the macro
- VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.
-
- \param filename : File name containing the robot constant
- parameters, like the hand-to-eye transformation.
-
-*/
-#ifdef VISP_HAVE_ACCESS_TO_NAS
-void
-vpViper850::parseConfigFile (const char * filename)
-{
- size_t dim;
- int code;
- char Ligne[FILENAME_MAX];
- char namoption[100];
- FILE * fdtask;
- int numLn = 0;
- double rot_eMc[3]; // rotation
- double trans_eMc[3]; // translation
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
-
- //vpTRACE("Read the config file for constant parameters %s.", filename);
- if ((fdtask = fopen(filename, "r" )) == NULL)
- {
- vpERROR_TRACE ("Impossible to read the config file %s.",
- filename);
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file.");
- }
-
- while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
- numLn ++;
- if ('#' == Ligne[0]) { continue; }
- sscanf(Ligne, "%s", namoption);
- dim = strlen(namoption);
-
- for (code = 0;
- NULL != opt_viper850[code];
- ++ code)
- {
- if (strncmp(opt_viper850[code], namoption, dim) == 0)
- {
- break;
- }
- }
-
- switch(code) {
- case 0:
- break; // Nothing to do: camera name
-
- case 1:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &rot_eMc[0],
- &rot_eMc[1],
- &rot_eMc[2]);
-
- // Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
- break;
-
- case 2:
- sscanf(Ligne, "%s %lf %lf %lf", namoption,
- &trans_eMc[0],
- &trans_eMc[1],
- &trans_eMc[2]);
- get_trans_eMc = true;
- break;
-
- default:
- vpERROR_TRACE ("Bad configuration file %s "
- "ligne #%d.", filename, numLn);
- } /* SWITCH */
- } /* WHILE */
-
- fclose (fdtask);
-
- // Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- for (unsigned int i=0; i < 3; i ++) {
- erc[i] = rot_eMc[i];
- etc[i] = trans_eMc[i];
- }
-
- vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc, eRc);
- }
-
- return;
-}
-#endif
-
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper850::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param image_width : Image width used to compute camera calibration.
- \param image_height : Image height used to compute camera calibration.
-
- The code below shows how to get the camera parameters of the camera
- attached to the robot.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper850.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-#else
- vpViper850 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- // Camera parameters are read from
- // /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
- // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined
- // in vpConfig.h file
- try {
- robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
-{
-#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
- vpXmlParserCamera parser;
- switch (getToolType()) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_MARLIN_F033C_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper850::TOOL_GENERIC_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_GENERIC_CAMERA_NAME,
- projModel,
- image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- 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).");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- break;
- }
- }
-#else
- // Set default parameters
- switch (getToolType()) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- case vpViper850::TOOL_GENERIC_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
- break;
- }
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- }
- break;
- }
- default:
- vpERROR_TRACE ("This error should not occur!");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
- break;
- }
-#endif
- return;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper850::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param I : A B&W image send by the current camera in use.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper850.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
- vpImage<unsigned char> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-#else
- vpViper850 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
-/*!
- \brief Get the current intrinsic camera parameters obtained by calibration.
-
- \warning This method needs XML library to parse the file defined in
- vpViper850::CONST_CAMERA_FILENAME and containing the camera
- parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is
- defined in include/visp/vpConfig.h file.
-
- \warning Thid method needs also an access to the file located on
- Inria's NAS server and containing the camera parameters in XML
- format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is
- defined in include/visp/vpConfig.h file.
-
- - If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined,
- this method gets the camera parameters from
- /udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml
- config file.
-
- - If these two macros are not defined, this method set the camera parameters
- to default one.
-
- \param cam : In output, camera parameters to fill.
- \param I : A color image send by the current camera in use.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpViper850.h>
-#include <visp/vpRobotViper850.h>
-
-int main()
-{
- vpImage<vpRGBa> I(480, 640);
-
-#ifdef VISP_HAVE_DC1394_2
- vp1394TwoGrabber g;
-
- // Acquire an image to update image structure
- g.acquire(I) ;
-#endif
-
-#ifdef VISP_HAVE_VIPER850
- vpRobotViper850 robot;
-#else
- vpViper850 robot;
-#endif
-
- vpCameraParameters cam ;
- // Get the intrinsic camera parameters depending on the image size
- try {
- robot.getCameraParameters (cam, I);
- }
- catch(...) {
- std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
- }
- std::cout << "Camera parameters: " << cam << std::endl;
-}
- \endcode
-
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
-
-*/
-
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
-{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
-}
diff --git a/src/robot/real-robot/viper/vpViper850.h b/src/robot/real-robot/viper/vpViper850.h
deleted file mode 100644
index 8418cf0..0000000
--- a/src/robot/real-robot/viper/vpViper850.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViper850.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface for the ADEPT Viper 850 robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpViper850_h
-#define vpViper850_h
-
-/*!
-
- \file vpViper850.h
-
- Modelisation of the ADEPT Viper 850 robot.
-
-*/
-
-/*!
-
- \class vpViper850
-
- \ingroup Viper
-
- \brief Modelisation of the ADEPT Viper 850 robot.
-
-*/
-
-#include <visp/vpViper.h>
-
-
-class VISP_EXPORT vpViper850: public vpViper
-{
- public:
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- //! Files where constant tranformation between end-effector and camera frame
- //! are stored.
- static const char * const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
- static const char * const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
- static const char * const CONST_CAMERA_FILENAME;
-#endif
- /*!
- Name of the camera attached to the end-effector.
- */
- static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
- static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
- static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
- static const char * const CONST_GENERIC_CAMERA_NAME;
-
- //! List of possible tools that can be attached to the robot end-effector.
- typedef enum {
- TOOL_MARLIN_F033C_CAMERA, /*!< Marlin F033C camera. */
- TOOL_PTGREY_FLEA2_CAMERA, /*!< Point Grey Flea 2 camera. */
- TOOL_SCHUNK_GRIPPER_CAMERA, /*!< Camera attached to the Schunk gripper. */
- TOOL_GENERIC_CAMERA /*!< A generic camera. */
- } vpToolType;
-
- //! Default tool attached to the robot end effector
- static const vpToolType defaultTool;
-
- vpViper850();
- virtual ~vpViper850() {};
-
- void init (void);
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void init(const char *camera_extrinsic_parameters);
-#endif
- void init (vpViper850::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType projModel =
- vpCameraParameters::perspectiveProjWithoutDistortion);
-
-
- //! Get the current camera model projection type
- vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const{
- return projModel;
- };
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
-
- //! Get the current tool type
- vpToolType getToolType() const {
- return tool_current;
- };
-
-#ifdef VISP_HAVE_ACCESS_TO_NAS
- void parseConfigFile (const char * filename);
-#endif
-
- protected:
- //! Set the current tool type
- void setToolType(vpViper850::vpToolType tool){
- tool_current = tool;
- };
-
- protected:
- //! Current tool in use
- vpToolType tool_current;
- // Used projection model
- vpCameraParameters::vpCameraParametersProjType projModel;
-
-};
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
-
diff --git a/src/robot/robot/vpRobot.cpp b/src/robot/robot/vpRobot.cpp
deleted file mode 100644
index e8addbc..0000000
--- a/src/robot/robot/vpRobot.cpp
+++ /dev/null
@@ -1,283 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobot.cpp 5238 2015-01-30 13:52:25Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic virtual robot.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#include <visp/vpRobot.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-
-
-const double vpRobot::maxTranslationVelocityDefault = 0.2;
-const double vpRobot::maxRotationVelocityDefault = 0.7;
-
-/* ------------------------------------------------------------------------- */
-/* --- CONSTRUCTEUR -------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-vpRobot::vpRobot (void)
- :
- stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
- maxTranslationVelocity (maxTranslationVelocityDefault),
- maxRotationVelocity (maxRotationVelocityDefault),
- nDof(0),
- eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
- qmin(NULL), qmax(NULL), verbose_(true)
-{
-}
-
-vpRobot::vpRobot (const vpRobot &robot)
- :
- stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
- maxTranslationVelocity (maxTranslationVelocityDefault),
- maxRotationVelocity (maxRotationVelocityDefault),
- nDof(0),
- eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
- qmin(NULL), qmax(NULL), verbose_(true)
-{
- *this = robot;
-}
-
-/*!
- Destructor that free allocated memory.
- */
-vpRobot::~vpRobot()
-{
- if (qmin != NULL) {
- delete [] qmin;
- qmin = NULL;
- }
- if (qmax != NULL) {
- delete [] qmax;
- qmax = NULL;
- }
-}
-
-/*! Copy operator. */
-vpRobot & vpRobot::operator=(const vpRobot &robot)
-{
- stateRobot = robot.stateRobot;
- frameRobot = robot.frameRobot;
- maxTranslationVelocity = robot.maxTranslationVelocity;
- maxRotationVelocity = robot.maxRotationVelocity;
- nDof = robot.nDof;
- eJe = robot.eJe;
- eJeAvailable = robot.eJeAvailable;
- fJe= robot.fJe;
- fJeAvailable = robot.fJeAvailable;
- areJointLimitsAvailable = robot.areJointLimitsAvailable;
- qmin = new double [nDof];
- qmax = new double [nDof];
- for (int i = 0; i< nDof; i++) {
- qmin[i] = robot.qmin[i];
- qmax[i] = robot.qmax[i];
- }
- verbose_ = robot.verbose_;
-
- return (*this);
-}
-/*!
- Saturate velocities.
-
- \param v_in : Vector of input velocities to saturate. Translation velocities should
- be expressed in m/s while rotation velocities in rad/s.
-
- \param v_max : Vector of maximal allowed velocities. Maximal translation velocities
- should be expressed in m/s while maximal rotation velocities in rad/s.
-
- \param verbose : Print a message indicating which axis causes the saturation.
-
- \return Saturated velocities.
-
- \exception vpRobotException::dimensionError : If the input vectors have different dimensions.
-
- The code below shows how to use this static method in order to saturate a velocity skew vector.
-
- \code
-#include <iostream>
-
-#include <visp/vpRobot.h>
-
-int main()
-{
- // Set a velocity skew vector
- vpColVector v(6);
- v[0] = 0.1; // vx in m/s
- v[1] = 0.2; // vy
- v[2] = 0.3; // vz
- v[3] = vpMath::rad(10); // wx in rad/s
- v[4] = vpMath::rad(-10); // wy
- v[5] = vpMath::rad(20); // wz
-
- // Set the maximal allowed velocities
- vpColVector v_max(6);
- for (int i=0; i<3; i++)
- v_max[i] = 0.3; // in translation (m/s)
- for (int i=3; i<6; i++)
- v_max[i] = vpMath::rad(10); // in rotation (rad/s)
-
- // Compute the saturated velocity skew vector
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
-
- std::cout << "v : " << v.t() << std::endl;
- std::cout << "v max: " << v_max.t() << std::endl;
- std::cout << "v sat: " << v_sat.t() << std::endl;
-
- return 0;
-}
- \endcode
- */
-vpColVector
-vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose)
-{
- unsigned int size = v_in.size();
- if (size != v_max.size())
- throw vpRobotException (vpRobotException::dimensionError, "Velocity vectors should have the same dimension");
-
- double scale = 1; // global scale factor to saturate all the axis
- for (unsigned int i = 0; i < size; i++)
- {
- double v_i = fabs(v_in[i]);
- double v_max_i = fabs(v_max[i]);
- if ( v_i > v_max_i ) // Test if we should saturate the axis
- {
- double scale_i = v_max_i/v_i;
- if (scale_i < scale)
- scale = scale_i;
-
- if (verbose)
- std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl;
- }
- }
-
- vpColVector v_sat(size);
- v_sat = v_in * scale;
-
- return v_sat;
-}
-
-
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
- \file vpRobot.cpp
- \brief class that defines a generic virtual robot
-*/
-vpRobot::vpRobotStateType
-vpRobot::setRobotState (const vpRobot::vpRobotStateType newState)
-{
- stateRobot = newState ;
- return newState ;
-}
-
-vpRobot::vpControlFrameType
-vpRobot::setRobotFrame (vpRobot::vpControlFrameType newFrame)
-{
- frameRobot = newFrame ;
- return newFrame ;
-}
-
-/*!
- Return the current robot position in the specified frame.
-*/
-vpColVector
-vpRobot::getPosition (vpRobot::vpControlFrameType frame)
-{
- vpColVector r;
- this ->getPosition (frame, r);
-
- return r;
-}
-
-/* ------------------------------------------------------------------------- */
-/* --- VELOCITY CONTROL ---------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- Set the maximal translation velocity that can be sent to the robot during a velocity control.
-
- \param v_max : Maximum translation velocity expressed in m/s.
-
-*/
-void
-vpRobot::setMaxTranslationVelocity (const double v_max)
-{
- this ->maxTranslationVelocity = v_max;
- return;
-}
-
-/*!
- Get the maximal translation velocity that can be sent to the robot during a velocity control.
-
- \return Maximum translation velocity expressed in m/s.
-*/
-double
-vpRobot::getMaxTranslationVelocity (void) const
-{
- return this ->maxTranslationVelocity;
-}
-/*!
-
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
-
- \param w_max : Maximum rotation velocity expressed in rad/s.
-*/
-
-void
-vpRobot::setMaxRotationVelocity (const double w_max)
-{
- this ->maxRotationVelocity = w_max;
- return;
-}
-
-/*!
-
- Get the maximal rotation velocity that can be sent to the robot during a velocity control.
-
- \return Maximum rotation velocity expressed in rad/s.
-*/
-double
-vpRobot::getMaxRotationVelocity (void) const
-{
- return this ->maxRotationVelocity;
-}
-
diff --git a/src/robot/robot/vpRobot.h b/src/robot/robot/vpRobot.h
deleted file mode 100644
index 4987af7..0000000
--- a/src/robot/robot/vpRobot.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobot.h 5238 2015-01-30 13:52:25Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic virtual robot.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpRobot_H
-#define vpRobot_H
-
-/*!
- \file vpRobot.h
- \brief class that defines a generic virtual robot
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpPoseVector.h>
-
-
-/*!
- \class vpRobot
- \brief class that defines a generic virtual robot
-*/
-class VISP_EXPORT vpRobot
-{
-public:
- /*!
- Robot control states.
- */
- typedef enum
- {
- STATE_STOP, /*!< Stops robot motion especially in velocity and
- acceleration control. */
- STATE_VELOCITY_CONTROL, //!< Initialize the velocity controller.
- STATE_POSITION_CONTROL, //!< Initialize the position controller.
- STATE_ACCELERATION_CONTROL //!< Initialize the acceleration controller.
- } vpRobotStateType ;
-
- /*!
- Robot control frames.
- */
- typedef enum
- {
- REFERENCE_FRAME, /*!< Corresponds to a fixed reference frame
- attached to the robot structure. */
- ARTICULAR_FRAME, /*!< Corresponds to the joint space. */
- CAMERA_FRAME, /*!< Corresponds to a frame attached to the
- camera mounted on the robot end-effector. */
- MIXT_FRAME /*!< Corresponds to a "virtual" frame where
- translations are expressed in the reference frame, and
- rotations in the camera frame.*/
- } vpControlFrameType ;
-
-private: /* Membres privees */
- vpRobot::vpRobotStateType stateRobot;
- vpRobot::vpControlFrameType frameRobot;
-
-protected:
- double maxTranslationVelocity;
- static const double maxTranslationVelocityDefault;// = 0.2;
- double maxRotationVelocity;
- static const double maxRotationVelocityDefault;// = 0.7;
-
- //! number of degrees of freedom
- int nDof ;
- //! robot Jacobian expressed in the end-effector frame
- vpMatrix eJe ;
- //! is the robot Jacobian expressed in the end-effector frame available
- int eJeAvailable ;
- //! robot Jacobian expressed in the robot reference frame available
- vpMatrix fJe ;
- //! is the robot Jacobian expressed in the robot reference frame available
- int fJeAvailable ;
-
- int areJointLimitsAvailable ;
- double *qmin;
- double *qmax ;
-
- bool verbose_;
-
-public:
- vpRobot (void);
- vpRobot (const vpRobot &robot);
- virtual ~vpRobot();
-
- //---------- Jacobian -----------------------------
- //! Get the robot Jacobian expressed in the end-effector frame
- virtual void get_eJe(vpMatrix &_eJe) = 0 ;
- //! Get the robot Jacobian expressed in the robot reference (or world) frame.
- virtual void get_fJe(vpMatrix &_fJe) = 0 ;
-
- //! Get a displacement (frame as to ve specified) between two successive position control.
- virtual void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &q) = 0 ;
-
- double getMaxTranslationVelocity (void) const ;
- double getMaxRotationVelocity (void) const;
- //! Get the robot position (frame has to be specified).
- virtual void getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &q) = 0 ;
-
- // Return the robot position (frame has to be specified).
- vpColVector getPosition (const vpRobot::vpControlFrameType frame);
- virtual vpRobotStateType getRobotState (void) const { return stateRobot ; }
-
- virtual void init() = 0 ;
-
- vpRobot & operator=(const vpRobot &robot);
-
- static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false);
-
- void setMaxRotationVelocity (const double maxVr);
- void setMaxTranslationVelocity (const double maxVt);
- //! Set a displacement (frame has to be specified) in position control.
- virtual void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &q) = 0 ;
- virtual vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
-
- //! Set the velocity (frame has to be specified) that will be applied to the velocity controller.
- virtual void setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &vel) = 0 ;
- inline void setVerbose(bool verbose) { verbose_ = verbose; };
-
-protected:
- vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame);
- vpControlFrameType getRobotFrame (void) const { return frameRobot ; }
-} ;
-
-#endif
diff --git a/src/robot/robot/vpRobotException.h b/src/robot/robot/vpRobotException.h
deleted file mode 100644
index 7fb9a36..0000000
--- a/src/robot/robot/vpRobotException.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception that can be emited by the vpRobot class and its derivates.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef __vpRobotException_H
-#define __vpRobotException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpRobotException.h
- \brief error that can be emited by the vpRobot class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \ingroup Exception
- \brief Error that can be emited by the vpRobot class and its derivates.
- */
-class VISP_EXPORT vpRobotException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpRobot member
- */
- enum errorRobotCodeEnum
- {
-
- /** Erreur lancee par le 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. */
- uniqueRobotError,
-
- /** Erreur lancee par les fonctions de commande si le
- * robot n'est pas dans le bon etat au moment du passage
- * d'ordre. */
- wrongStateError,
-
- /** Erreur lancee par les fonctions de changement d'etat
- * si le changement demandee n'est pas possible. */
- stateModificationError,
-
- /** Erreur generee lors d'un retour non nulle d'une fonction
- * de communication de la lib Afma6. */
- communicationError,
-
- /** Erreur lancee apres un appel a une fonction de la lib
- * bas-niveau de control de l'afma6 ayant renvoye une erreur. */
- lowLevelError,
-
- /** Erreur lancee par la fonction de parsing des parametres du
- * robot, si le fichier donne en entree n'est pas valide.
- */
- 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. */
- 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'.
- */
- notImplementedError,
- /** Position is out of range.
- */
- positionOutOfRangeError
- } ;
-
- public:
- vpRobotException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpRobotException (const int id, const std::string & msg)
- : vpException(id, msg)
- {
- }
- vpRobotException (const int id)
- : vpException(id)
- {
- }
-
-};
-
-#endif
diff --git a/src/robot/robot/vpRobotTemplate.cpp b/src/robot/robot/vpRobotTemplate.cpp
deleted file mode 100644
index 441da22..0000000
--- a/src/robot/robot/vpRobotTemplate.cpp
+++ /dev/null
@@ -1,176 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotTemplate.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a robot just to show which function you must implement.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpRobotTemplate.cpp
- \brief class that defines a robot just to show which function you must implement
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRobotTemplate.h>
-#include <visp/vpDebug.h>
-
-//! basic initialization
-void vpRobotTemplate::init()
-{
- vpTRACE(" Get the joint limits " ) ;
- std::cout << "Not implemented ! " << std::endl;
-}
-
-//! constructor
-vpRobotTemplate::vpRobotTemplate()
-{
- init() ;
-}
-
-
-//! constructor
-vpRobotTemplate::~vpRobotTemplate()
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-
-/*
-
-AT LEAST ONE OF THESE TWO FUNCTIONS HAS TO BE IMPLEMENTED
-
-get_eJe
-get_fJe
-
-*/
-
-//! get the robot Jacobian expressed in the end-effector frame
-void
-vpRobotTemplate::get_eJe(vpMatrix &/* _eJe */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-
-//! get the robot Jacobian expressed in the robot reference frame
-void
-vpRobotTemplate::get_fJe(vpMatrix &/* _fJe */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-
-
-/*
-
-AT LEAST ONE OF THESE TWO FUNCTIONS HAS TO BE IMPLEMENTED
-
-sendCameraVelocity
-sendArticularVelocity
-
-
-*/
-//! send to the controller a velocity expressed in the camera frame
-void vpRobotTemplate::sendCameraVelocity(const vpColVector &/* v */)
-{
- std::cout << "Not implemented ! " << std::endl;
- std::cout << "To implement me you need : " << std::endl ;
- std::cout << "\t to known the robot jacobian expressed in " ;
- std::cout << "the end-effector frame (eJe) " <<std::endl ;
- std::cout << "\t the frame transformation between camera frame " ;
- std::cout << "and end-effector frame (cMe)" << std::endl ;
-}
-
-//! send to the controller a velocity expressed in the articular frame
-void
-vpRobotTemplate::sendArticularVelocity(const vpColVector &/* qdot */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-//! send to the controller a velocity (frame as to ve specified)
-void
-vpRobotTemplate::setVelocity(const vpRobot::vpControlFrameType /* frame */,
- const vpColVector &/* vel */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-
-
-/*
-
-THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
-
-*/
-
-//! get a position expressed in the robot reference frame
-void
-vpRobotTemplate::getPosition(vpPoseVector &/* q */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-//! get a position expressed in the articular frame
-void
-vpRobotTemplate::getArticularPosition(vpColVector &/* q */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-//! get a displacement (frame as to ve specified)
-void
-vpRobotTemplate::getPosition( const vpRobot::vpControlFrameType/*frame*/,
- vpColVector &/* q */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-//! set a displacement (frame as to ve specified)
-void
-vpRobotTemplate::setPosition( const vpRobot::vpControlFrameType/*frame*/,
- const vpColVector &/* q */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-//! get a displacement (frame as to ve specified)
-void
-vpRobotTemplate::getDisplacement(const vpRobot::vpControlFrameType /* frame */,
- vpColVector &/* q */)
-{
- std::cout << "Not implemented ! " << std::endl;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/robot/robot/vpRobotTemplate.h b/src/robot/robot/vpRobotTemplate.h
deleted file mode 100644
index 8d527b8..0000000
--- a/src/robot/robot/vpRobotTemplate.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotTemplate.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a robot just to show which function you must implement.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpRobotTemplate_H
-#define vpRobotTemplate_H
-
-/*!
- \file vpRobotTemplate.h
- \brief class that defines a robot just to show which function you must implement
-*/
-
-#include <visp/vpRobot.h>
-
-/*!
- \class vpRobotTemplate
- \brief class that defines a robot just to show which function you must implement
-*/
-
-class VISP_EXPORT vpRobotTemplate : public vpRobot
-{
-
-public:
-
- //! basic initialization
- void init() ;
-
- //! constructor
- vpRobotTemplate() ;
- //! destructor
- virtual ~vpRobotTemplate() ;
-
-
- //! get the robot Jacobian expressed in the end-effector frame
- void get_eJe(vpMatrix &_eJe) ;
- //! get the robot Jacobian expressed in the robot reference frame
- void get_fJe(vpMatrix &_fJe) ;
-
- //! send to the controller a velocity expressed in the camera frame
- void sendCameraVelocity(const vpColVector &v) ;
- //! send to the controller a velocity expressed in the articular frame
- void sendArticularVelocity(const vpColVector &qdot) ;
- //! send to the controller a velocity (frame as to be specified)
- void setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &vel) ;
-
- //! get a position expressed in the robot reference frame
- void getPosition(vpPoseVector &q) ;
- //! get a position expressed in the articular frame
- void getArticularPosition(vpColVector &q) ;
- //! get a displacement (frame as to be specified)
- void getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &q) ;
- //! set a displacement (frame as to be specified)
- void setPosition(const vpRobot::vpControlFrameType frame,
- const vpColVector &q) ;
-
- //! get a displacement (frame as to be specified)
- void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &q) ;
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/robot/simulator-robot/arms/CMakeRobotArmsList.cmake b/src/robot/simulator-robot/arms/CMakeRobotArmsList.cmake
deleted file mode 100644
index 4cf0a10..0000000
--- a/src/robot/simulator-robot/arms/CMakeRobotArmsList.cmake
+++ /dev/null
@@ -1,56 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeRobotArmsList.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Default scenes used by the vpSimulatorViper850
-#
-# Authors:
-# Nicolas Melchior
-#
-#############################################################################
-
-SET(ROBOT_ARMS_SCENES
- afma6_gate.bnd
- afma6_arm1.bnd
- afma6_arm2.bnd
- afma6_arm3.bnd
- afma6_arm4.bnd
- afma6_tool_ccmop.bnd
- afma6_tool_gripper.bnd
- afma6_tool_vacuum.bnd
- viper850_arm1.bnd
- viper850_arm2.bnd
- viper850_arm3.bnd
- viper850_arm4.bnd
- viper850_arm5.bnd
- viper850_arm6.bnd
-)
diff --git a/src/robot/simulator-robot/vpRobotCamera.cpp b/src/robot/simulator-robot/vpRobotCamera.cpp
deleted file mode 100644
index b9b206f..0000000
--- a/src/robot/simulator-robot/vpRobotCamera.cpp
+++ /dev/null
@@ -1,283 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotCamera.cpp 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines the simplest robot : a free flying camera.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpRobotCamera.cpp
- \brief class that defines the simplest robot : a free flying camera
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpExponentialMap.h>
-
-
-/*!
- Constructor.
-
- Initialise the robot by a call to init().
-
- Sampling time is set to 40 ms. To change it you should call
- setSamplingTime().
-
- Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
- is set to identity (see get_eJe()).
-
- \code
- vpRobotCamera robot;
-
- robot.setSamplingTime(0.020); // Set the sampling time to 20 ms.
-
- \endcode
-
-*/
-vpRobotCamera::vpRobotCamera()
- : cMw_()
-{
- init() ;
-}
-
-/*!
- Robot initialisation.
-
- Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
- is set to identity (see get_eJe()).
-
-*/
-void vpRobotCamera::init()
-{
- nDof = 6;
- eJe.resize(6,6) ;
- eJe.setIdentity() ;
- eJeAvailable = true;
- fJeAvailable = false;
- areJointLimitsAvailable = false;
- qmin = NULL;
- qmax = NULL;
-
- setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
- setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
-}
-
-
-/*!
- Destructor.
-
-*/
-vpRobotCamera::~vpRobotCamera()
-{
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation. Here this transformation is equal to identity
- since camera frame and end-effector frame are at the same location.
-
-*/
-void
-vpRobotCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpVelocityTwistMatrix cVe_;
- cVe = cVe_;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
- For that simple robot the Jacobian is the identity.
-
- \param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
- J}_e\f$ expressed in the end-effector frame.
-*/
-void
-vpRobotCamera::get_eJe(vpMatrix &eJe_)
-{
- eJe_ = this->eJe ;
-}
-
-/*!
- Send to the controller a velocity.
-
- \param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
- and camera frame (vpRobot::CAMERA_FRAME) are implemented.
-
- \param v : Velocity to apply to the robot.
-
- - In the camera frame, this velocity is represented by a vector of dimension 6
- \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- - In articular, this velocity is represented by a 6 dimension vector \f$
- \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
- u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
- robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
- here set to identity.
-
- We use the exponential map (vpExponentialMap) to update the camera location.
- Sampling time can be set using setSamplingTime().
-
- \sa setSamplingTime()
-
-*/
-void
-vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &v)
-{
- switch (frame)
- {
- case vpRobot::ARTICULAR_FRAME:
- case vpRobot::CAMERA_FRAME: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
-
- vpColVector v_max(6);
-
- for (unsigned int i=0; i<3; i++)
- v_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- v_max[i] = getMaxRotationVelocity();
-
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
-
- this->cMw_ = vpExponentialMap::direct(v_sat, delta_t_).inverse()*this->cMw_ ;
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the reference frame:"
- "functionality not implemented");
- break ;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
-
- break ;
- }
-}
-
-
-/*!
- Get the robot position as the transformation from camera frame to world frame.
-*/
-void
-vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const
-{
- cMw = this->cMw_ ;
-}
-
-/*
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the articular
- position of each dof, first the 3 translations, then the 3
- articular rotation positions represented by a vpRxyzVector.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
-*/
-void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME :
- q = 0;
- break;
-
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::REFERENCE_FRAME : {
- // Convert wMc_ to a position
- // From fMc extract the pose
- vpRotationMatrix cRw;
- this->cMw_.extract(cRw);
- vpRxyzVector rxyz;
- rxyz.buildFrom(cRw);
-
- for (unsigned int i=0; i < 3; i++) {
- q[i] = this->cMw_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
-
- break;
- }
- case vpRobot::MIXT_FRAME :
- std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
- }
-}
-
-/*!
- Set the robot position as the transformation from camera frame to world frame.
-*/
-void
-vpRobotCamera::setPosition(const vpHomogeneousMatrix &cMw)
-{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_POSITION_CONTROL);
- }
-
- this->cMw_ = cMw ;
-}
-
diff --git a/src/robot/simulator-robot/vpRobotCamera.h b/src/robot/simulator-robot/vpRobotCamera.h
deleted file mode 100644
index 4c34837..0000000
--- a/src/robot/simulator-robot/vpRobotCamera.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotCamera.h 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines the simplest robot : a free flying camera.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpRobotCamera_H
-#define vpRobotCamera_H
-
-/*!
- \file vpRobotCamera.h
- \brief class that defines the simplest robot : a free flying camera
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpRobotSimulator.h>
-
-/*!
- \class vpRobotCamera
-
- \ingroup RobotSimuWithoutVisu
-
- \brief Class that defines the simplest robot: a free flying camera.
-
- This free flying camera has 6 dof; 3 in translation and 3 in rotation.
- It evolves as a gentry robot with respect to a world frame. This class
- is similar to vpSimulatorCamera class except that here the position of the robot
- is provided as the transformation from camera frame to world frame; cMw. Since
- the position of the camera frame evolves, this representation is less intuitive
- than the one implemented in vpSimulatorCamera where the transformation from world
- to camera frame is considered; wMc.
-
- \note We recommend to use vpSimulatorCamera rather than vpRobotCamera.
-
- For this particular simulated robot, the end-effector and camera frame are confused.
- That means that the cMe transformation is equal to identity.
-
- The robot jacobian expressed in the end-effector frame
- \f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
-
- The following code shows how to control this robot in position and velocity.
- \code
-#include <visp/vpRobotCamera.h>
-
-int main()
-{
- vpHomogeneousMatrix cMw;
- vpRobotCamera robot;
-
- robot.getPosition(cMw); // Position of the camera in the world frame
- std::cout << "Default position of the camera in the world frame cMw:\n" << cMw << std::endl;
-
- cMw[2][3] = 1.; // World frame is 1 meter along z axis in front of the camera frame
- robot.setPosition(cMw); // Set the new position of the camera wrt the world frame
- std::cout << "New position of the camera wrt the world frame cMw:\n" << cMw << std::endl;
-
- robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
- robot.setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
- robot.setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
-
- vpColVector v(6);
- v = 0;
- v[2] = 1.; // set v_z to 1 m/s
- robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- // The robot has moved from 0.1 meters along the z axis
- robot.getPosition(cMw); // Position of the camera wrt the world frame
- std::cout << "New position of the camera cMw:\n" << cMw << std::endl;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpRobotCamera : public vpRobotSimulator
-{
-protected:
- vpHomogeneousMatrix cMw_; // camera to world
-
-public:
- vpRobotCamera() ;
- virtual ~vpRobotCamera() ;
-
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_eJe(vpMatrix &eJe);
-
- void getPosition(vpHomogeneousMatrix &cMw) const ;
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
-
- void setPosition(const vpHomogeneousMatrix &cMw) ;
- void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) ;
-
-private:
- void init() ;
-
- // Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
-
-#endif
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/robot/simulator-robot/vpRobotSimulator.cpp b/src/robot/simulator-robot/vpRobotSimulator.cpp
deleted file mode 100644
index 92cf1ec..0000000
--- a/src/robot/simulator-robot/vpRobotSimulator.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotSimulator.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic class used to make robot simulators.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#include <visp/vpRobotSimulator.h>
-
-/*!
- Basic constructor that sets the sampling time by default to 40ms.
-*/
-vpRobotSimulator::vpRobotSimulator() : vpRobot(), delta_t_(0.040f)
-{
-}
diff --git a/src/robot/simulator-robot/vpRobotSimulator.h b/src/robot/simulator-robot/vpRobotSimulator.h
deleted file mode 100644
index 7c193b9..0000000
--- a/src/robot/simulator-robot/vpRobotSimulator.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotSimulator.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic class used to make robot simulators.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpRobotSimulator_HH
-#define vpRobotSimulator_HH
-
-/*!
- \file vpRobotSimulator.h
- \brief Basic class used to make robot simulators.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpRobot.h>
-#include <visp/vpTime.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-
-/*!
- \class vpRobotSimulator
-
- \ingroup RobotSimuWithVisu, RobotSimuWithoutVisu
-
- \brief This class aims to be a basis used to create all the
- robot simulators.
-
-*/
-class VISP_EXPORT vpRobotSimulator : public vpRobot
-{
-protected:
- double delta_t_; // sampling time in second
-
-public:
- vpRobotSimulator();
- /*!
- Basic destructor
- */
- virtual ~vpRobotSimulator() {};
-
- /*!
- Return the sampling time.
-
- \return Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
- */
- inline double getSamplingTime() const
- {
- return(this->delta_t_);
- }
-
- /*!
- Set the sampling time.
-
- \param delta_t : Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
-
- */
- virtual inline void setSamplingTime(const double &delta_t)
- {
- this->delta_t_ = delta_t;
- }
-};
-
-#endif
diff --git a/src/robot/simulator-robot/vpRobotWireFrameSimulator.cpp b/src/robot/simulator-robot/vpRobotWireFrameSimulator.cpp
deleted file mode 100644
index e69b9bf..0000000
--- a/src/robot/simulator-robot/vpRobotWireFrameSimulator.cpp
+++ /dev/null
@@ -1,375 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotWireFrameSimulator.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic class used to make robot simulators.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-
-
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-#include <visp/vpRobotWireFrameSimulator.h>
-#include <visp/vpSimulatorViper850.h>
-
-/*!
- Basic constructor
-*/
-vpRobotWireFrameSimulator::vpRobotWireFrameSimulator()
- : vpWireFrameSimulator(), vpRobotSimulator(),
- I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
-#if defined(_WIN32)
-#elif defined(VISP_HAVE_PTHREAD)
- thread(), attr(),
-#endif
- mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
- displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
- cameraParam(),
-#if defined(VISP_HAVE_DISPLAY)
- display(),
-#endif
- displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
- setVelocityCalled(false), verbose_(false)
-{
- setSamplingTime(0.010);
- velocity.resize(6);
- I.resize(480,640);
- I = 255;
-#if defined(VISP_HAVE_DISPLAY)
- display.init(I, 0, 0,"The External view");
-#endif
-
- //pid_t pid = getpid();
- // setpriority (PRIO_PROCESS, pid, 19);
-}
-
-/*!
- Default constructor.
- \param do_display : When true, enables the display of the external view.
- */
-vpRobotWireFrameSimulator::vpRobotWireFrameSimulator(bool do_display)
- : vpWireFrameSimulator(), vpRobotSimulator(),
- I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
-#if defined(_WIN32)
-#elif defined(VISP_HAVE_PTHREAD)
- thread(), attr(),
-#endif
- /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
- displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
- cameraParam(),
-#if defined(VISP_HAVE_DISPLAY)
- display(),
-#endif
- displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false),
- setVelocityCalled(false), verbose_(false)
-{
- setSamplingTime(0.010);
- velocity.resize(6);
- I.resize(480,640);
- I = 255;
-
-#if defined(VISP_HAVE_DISPLAY)
- if (do_display)
- this->display.init(I, 0, 0,"The External view");
-#endif
-
- //pid_t pid = getpid();
- // setpriority (PRIO_PROCESS, pid, 19);
-}
-
-
-
-/*!
- Basic destructor
-*/
-vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator()
-{
-}
-
-/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
-*/
-void
-vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
-{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- vpWireFrameSimulator::initScene(obj, desired_object);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- displayCamera = false;
-}
-
-/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file.
- It is also possible to use a vrml (.wrl) file.
-
- \param obj : Path to the scene file you want to use.
- \param desired_object : Path to the scene file you want to use.
-*/
-void
-vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
-{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- vpWireFrameSimulator::initScene(obj, desired_object);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- displayCamera = false;
-}
-
-/*!
- Initialize the display. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- \param obj : Type of scene used to display the object at the current position.
-*/
-void
-vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
-{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- vpWireFrameSimulator::initScene(obj);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- displayCamera = false;
-}
-
-/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file, or the vrml file.
-
- \param obj : Path to the scene file you want to use.
-*/
-void
-vpRobotWireFrameSimulator::initScene(const char* obj)
-{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- vpWireFrameSimulator::initScene(obj);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- displayCamera = false;
-}
-
-/*!
- Get the view of the camera's robot.
-
- According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
-
- \param I_ : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
-{
-
- if (!sceneInitialized)
- throw;
-
- double u;
- double v;
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_int);
- v = (double)I_.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- }
-
- float o44c[4][4],o44cd[4][4],x,y,z;
- Matrix id = IDENTITY_MATRIX;
-
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
- get_fMi(fMit);
- this->cMo = fMit[size_fMi-1].inverse()*fMo;
- this->cMo = rotz*cMo;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- while (get_displayBusy()) vpTime::wait(2);
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
- x = o44c[2][0] + o44c[3][0];
- y = o44c[2][1] + o44c[3][1];
- z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayObject)
- display_scene(id,this->scene,I_, curColor);
-
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
- x = o44cd[2][0] + o44cd[3][0];
- y = o44cd[2][1] + o44cd[3][1];
- z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
- else display_scene(id,desiredScene,I_, desColor);
- }
- delete[] fMit;
- set_displayBusy(false);
-}
-
-/*!
- Get the view of the camera's robot.
-
- According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
-
- \param I_ : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
-{
-
- if (!sceneInitialized)
- throw;
-
- double u;
- double v;
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- }
-
- float o44c[4][4],o44cd[4][4],x,y,z;
- Matrix id = IDENTITY_MATRIX;
-
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
- get_fMi(fMit);
- this->cMo = fMit[size_fMi-1].inverse()*fMo;
- this->cMo = rotz*cMo;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- while (get_displayBusy()) vpTime::wait(2);
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
- x = o44c[2][0] + o44c[3][0];
- y = o44c[2][1] + o44c[3][1];
- z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayObject)
- {
- display_scene(id,this->scene,I_, curColor);
- }
-
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
- x = o44cd[2][0] + o44cd[3][0];
- y = o44cd[2][1] + o44cd[3][1];
- z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
- else display_scene(id,desiredScene,I_, desColor);
- }
- delete[] fMit;
- set_displayBusy(false);
-}
-
-/*!
- Get the pose between the object and the robot's camera.
-
- \return The pose between the object and the fixed world frame.
-*/
-vpHomogeneousMatrix
-vpRobotWireFrameSimulator::get_cMo()
-{
- vpHomogeneousMatrix cMoTemp;
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
- get_fMi(fMit);
- cMoTemp = fMit[size_fMi-1].inverse()*fMo;
- delete[] fMit;
- return cMoTemp;
-}
-
-#endif
diff --git a/src/robot/simulator-robot/vpRobotWireFrameSimulator.h b/src/robot/simulator-robot/vpRobotWireFrameSimulator.h
deleted file mode 100644
index 7a4435e..0000000
--- a/src/robot/simulator-robot/vpRobotWireFrameSimulator.h
+++ /dev/null
@@ -1,453 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRobotWireFrameSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic class used to make robot simulators.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpRobotWireFrameSimulator_HH
-#define vpRobotWireFrameSimulator_HH
-
-/*!
- \file vpRobotWireFrameSimulator.h
- \brief Basic class used to make robot simulators.
-*/
-
-#include <visp/vpConfig.h>
-
-
-
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if defined(_WIN32)
-# include <windows.h>
-#elif defined(VISP_HAVE_PTHREAD)
-# include <pthread.h>
-#endif
-
-#include <visp/vpWireFrameSimulator.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotSimulator.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-/*!
- \class vpRobotWireFrameSimulator
-
- \ingroup RobotSimuWithVisu
-
- \brief This class aims to be a basis used to create all the
- simulators of robots.
-
- Thus in this class you will find all the parameters and methods
- which are necessary to create a simulator. Several methods are pure
- virtual. In this case it means that they are specific to the each
- robot, for example the computation of the geometrical model.
-
- \warning This class uses threading capabilities. Thus on Unix-like
- platforms, the libpthread third-party library need to be
- installed. On Windows, we use the native threading capabilities.
-*/
-class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
-{
- public:
- vpImage<vpRGBa> I;
-
- typedef enum
- {
- MODEL_3D,
- MODEL_DH
- } vpDisplayRobotType;
-
-
- protected:
- /*! cpu time at the begining of the robot's movement*/
- double tcur;
- /*! cpu time at the end of the last robot's movement*/
- double tprev;
-
- /*! Contains the 3D model of the robot's arms*/
- Bound_scene* robotArms;
-
- /*! Size of the fMi table*/
- unsigned int size_fMi;
- /*! Table containing all the homogeneous matrices between the reference frame of the robot and the frames you used to compute the Denavit-Hartenberg representation
-
- If you use a camera at the end of the effector, the last homogeneous matrix has to be the one between the reference frame and the camera frame (fMc)*/
- vpHomogeneousMatrix* fMi;
-
- /*! The articular coordinates*/
- vpColVector artCoord;
- /*! The articular velocity*/
- vpColVector artVel;
- /*! The velocity in the current frame (articular, camera or reference)*/
- vpColVector velocity;
-
-#if defined(_WIN32)
- HANDLE hThread;
- HANDLE mutex_fMi;
- HANDLE mutex_artVel;
- HANDLE mutex_artCoord;
- HANDLE mutex_velocity;
- HANDLE mutex_display;
-#elif defined(VISP_HAVE_PTHREAD)
- pthread_t thread;
- pthread_attr_t attr;
- pthread_mutex_t mutex_fMi;
- pthread_mutex_t mutex_artVel;
- pthread_mutex_t mutex_artCoord;
- pthread_mutex_t mutex_velocity;
- pthread_mutex_t mutex_display;
-#endif
-
- bool displayBusy;
-
- /*! True if the robot has to be stopped*/
- bool robotStop;
- /*! True if one of the joint reach the limit*/
- bool jointLimit;
- /*! Index of the joint which is in limit*/
- unsigned int jointLimitArt;
- /*! True if the singularity are automatically managed */
- bool singularityManagement;
-
- /*! External camera parameters*/
- vpCameraParameters cameraParam;
-
- #if defined VISP_HAVE_X11
- vpDisplayX display;
- #elif defined VISP_HAVE_GDI
- vpDisplayGDI display;
- #elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV display;
- #elif defined VISP_HAVE_D3D9
- vpDisplayD3D display;
- #elif defined VISP_HAVE_GTK
- vpDisplayGTK display;
- #endif
-
- vpDisplayRobotType displayType;
-
- bool displayAllowed;
- //! Flag used to force the sampling time in the thread computing the robot's displacement to a constant value (\e samplingTime). It may be useful if the main thread (computing the features) is very time consumming. False by default.
- bool constantSamplingTimeMode;
-
- //! Flag used to specify to the thread managing the robot displacements that the setVelocity() method has been called.
- bool setVelocityCalled;
-
- bool verbose_;
-
- public:
- vpRobotWireFrameSimulator();
- vpRobotWireFrameSimulator(bool display);
- virtual ~vpRobotWireFrameSimulator();
-
- /*!
- Get the parameters of the virtual external camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getExternalCameraParameters() const {
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
- /*!
- Get the external camera's position relative to the the world reference frame.
-
- \return the main external camera position relative to the the world reference frame.
- */
- vpHomogeneousMatrix getExternalCameraPosition() const
- {
- return this->vpWireFrameSimulator::getExternalCameraPosition();
- }
-
- void getInternalView(vpImage<vpRGBa> &I);
- void getInternalView(vpImage<unsigned char> &I);
-
- vpHomogeneousMatrix get_cMo();
- /*!
- Get the pose between the object and the fixed world frame.
-
- \return The pose between the object and the fixed world frame.
- */
- vpHomogeneousMatrix get_fMo() const {return fMo;}
-
- /* Display functions */
- void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
- void initScene(const char* obj, const char* desiredObject);
- void initScene (const vpSceneObject &obj);
- void initScene(const char* obj);
-
- /*!
- Set the color used to display the camera in the external view.
-
- \param col : The desired color.
- */
- void setCameraColor(const vpColor col) {camColor = col;}
-
- /*!
- Set the flag used to force the sampling time in the thread computing the
- robot's displacement to a constant value; see setSamplingTime(). It may be
- useful if the main thread (computing the features) is very time consuming.
- False by default.
-
- \param _constantSamplingTimeMode : The new value of the constantSamplingTimeMode flag.
- */
- inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
- constantSamplingTimeMode = _constantSamplingTimeMode;
- }
-
- /*!
- Set the color used to display the object at the current position in the robot's camera view.
-
- \param col : The desired color.
- */
- void setCurrentViewColor(const vpColor col) {curColor = col;}
-
- /*!
- Set the color used to display the object at the desired position in the robot's camera view.
-
- \param col : The desired color.
- */
- void setDesiredViewColor(const vpColor col) {desColor = col;}
-
- /*!
- Set the desired position of the robot's camera relative to the object.
-
- \param cdMo_ : The desired pose of the camera.
- */
- void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo_)
- {
- this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
- }
-
- /*!
- Set the way to draw the robot. Depending on what you choose you can display a 3D wireframe model or a set of lines linking the frames used to compute the geometrical model.
-
- \param dispType : Type of display. Can be MODEL_3D or MODEL_DH.
- */
- inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
- /*!
- Set the external camera point of view.
-
- \param camMf_ : The pose of the external camera relative to the world reference frame.
- */
- void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
- {
- this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
- }
- /*!
- Specify the thickness of the graphics drawings.
- */
- void setGraphicsThickness(unsigned int thickness)
- {
- this->thickness_ = thickness;
- }
-
- /*!
- Set the sampling time.
-
- \param delta_t : Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
-
- Since the wireframe simulator is threaded, the sampling time is set to vpTime::minTimeForUsleepCall / 1000 seconds.
-
- */
- inline void setSamplingTime(const double &delta_t)
- {
- if(delta_t < static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3)){
- this->delta_t_ = static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3);
- } else {
- this->delta_t_ = delta_t;
- }
- }
- /*! Set the parameter which enable or disable the singularity mangement */
- void setSingularityManagement (const bool sm) {singularityManagement = sm;}
-
- /*!
- Activates extra printings when the robot reaches joint limits...
- */
- void setVerbose(bool verbose)
- {
- this->verbose_ = verbose;
- }
-
- /*!
- Set the pose between the object and the fixed world frame.
-
- \param fMo_ : The pose between the object and the fixed world frame.
- */
- void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;}
-
- protected:
- /*!
- Function used to launch the thread which moves the robot.
- */
- #if defined(_WIN32)
- static DWORD WINAPI launcher( LPVOID lpParam )
- {
- ((vpRobotWireFrameSimulator *)lpParam)->updateArticularPosition();
- return 0;
- }
- #elif defined(VISP_HAVE_PTHREAD)
- static void* launcher(void *arg)
- {
- (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
- // pthread_exit((void*) 0);
- return NULL;
- }
- #endif
-
- /* Robot functions */
- void init() {;}
- /*! Method lauched by the thread to compute the position of the robot in the articular frame. */
- virtual void updateArticularPosition() = 0;
- /*! Method used to check if the robot reached a joint limit. */
- virtual int isInJointLimit () = 0;
- /*! Compute the articular velocity relative to the velocity in another frame. */
- virtual void computeArticularVelocity() = 0;
-
- /* Display functions */
- void initDisplay() {;}
- virtual void initArms() = 0;
-
- #if defined(_WIN32)
- vpColVector get_artCoord() const {
- WaitForSingleObject(mutex_artCoord,INFINITE);
- vpColVector artCoordTmp (6);
- artCoordTmp = artCoord;
- ReleaseMutex(mutex_artCoord);
- return artCoordTmp;}
- void set_artCoord(const vpColVector &coord) {
- WaitForSingleObject(mutex_artCoord,INFINITE);
- artCoord = coord;
- ReleaseMutex(mutex_artCoord);}
-
- vpColVector get_artVel() const {
- WaitForSingleObject(mutex_artVel,INFINITE);
- vpColVector artVelTmp (artVel);
- ReleaseMutex(mutex_artVel);
- return artVelTmp;}
- void set_artVel(const vpColVector &vel) {
- WaitForSingleObject(mutex_artVel,INFINITE);
- artVel = vel;
- ReleaseMutex(mutex_artVel);}
-
- vpColVector get_velocity() {
- WaitForSingleObject(mutex_velocity,INFINITE);
- vpColVector velocityTmp = velocity;
- ReleaseMutex(mutex_velocity);
- return velocityTmp;}
- void set_velocity(const vpColVector &vel) {
- WaitForSingleObject(mutex_velocity,INFINITE);
- velocity = vel;
- ReleaseMutex(mutex_velocity);}
-
- void set_displayBusy (const bool &status) {
- WaitForSingleObject(mutex_display,INFINITE);
- displayBusy = status;
- ReleaseMutex(mutex_display);}
- bool get_displayBusy () {
- WaitForSingleObject(mutex_display,INFINITE);
- bool status = displayBusy;
- if (!displayBusy) displayBusy = true;
- ReleaseMutex(mutex_display);
- return status;}
-
- #elif defined(VISP_HAVE_PTHREAD)
- vpColVector get_artCoord() {
- pthread_mutex_lock (&mutex_artCoord);
- vpColVector artCoordTmp (6);
- artCoordTmp = artCoord;
- pthread_mutex_unlock (&mutex_artCoord);
- return artCoordTmp;}
- void set_artCoord(const vpColVector &coord) {
- pthread_mutex_lock (&mutex_artCoord);
- artCoord = coord;
- pthread_mutex_unlock (&mutex_artCoord);}
-
- vpColVector get_artVel() {
- pthread_mutex_lock (&mutex_artVel);
- vpColVector artVelTmp (artVel);
- pthread_mutex_unlock (&mutex_artVel);
- return artVelTmp;}
- void set_artVel(const vpColVector &vel) {
- pthread_mutex_lock (&mutex_artVel);
- artVel = vel;
- pthread_mutex_unlock (&mutex_artVel);}
-
- vpColVector get_velocity() {
- pthread_mutex_lock (&mutex_velocity);
- vpColVector velocityTmp = velocity;
- pthread_mutex_unlock (&mutex_velocity);
- return velocityTmp;}
- void set_velocity(const vpColVector &vel) {
- pthread_mutex_lock (&mutex_velocity);
- velocity = vel;
- pthread_mutex_unlock (&mutex_velocity);}
-
- void set_displayBusy (const bool &status) {
- pthread_mutex_lock (&mutex_display);
- displayBusy = status;
- pthread_mutex_unlock (&mutex_display);}
- bool get_displayBusy () {
- pthread_mutex_lock (&mutex_display);
- bool status = displayBusy;
- if (!displayBusy) displayBusy = true;
- pthread_mutex_unlock (&mutex_display);
- return status;}
- #endif
-
- /*! Get a table of poses between the reference frame and the frames you used to compute the Denavit-Hartenberg representation */
- virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
-};
-
-#endif
-
-#endif
diff --git a/src/robot/simulator-robot/vpSimulatorAfma6.cpp b/src/robot/simulator-robot/vpSimulatorAfma6.cpp
deleted file mode 100644
index 2a38013..0000000
--- a/src/robot/simulator-robot/vpSimulatorAfma6.cpp
+++ /dev/null
@@ -1,2612 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorAfma6.cpp 2595 2010-06-02 08:50:59Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class which provides a simulator for the robot Afma6.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpConfig.h>
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpTime.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpIoTools.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <string>
-const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0;
-
-
-/*!
- Basic constructor
-*/
-vpSimulatorAfma6::vpSimulatorAfma6()
- : vpRobotWireFrameSimulator(), vpAfma6(),
- q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
- zeroPos(), reposPos(), toolCustom(false), arm_dir()
-{
- init();
- initDisplay();
-
- tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
-
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined (VISP_HAVE_PTHREAD)
- pthread_mutex_init(&mutex_fMi, NULL);
- pthread_mutex_init(&mutex_artVel, NULL);
- pthread_mutex_init(&mutex_artCoord, NULL);
- pthread_mutex_init(&mutex_velocity, NULL);
- pthread_mutex_init(&mutex_display, NULL);
-
- pthread_attr_init(&attr);
- pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
- pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
- compute_fMi();
-}
-
-/*!
- Constructor used to enable or disable the external view of the robot.
-
- \param do_display : When true, enables the display of the external view.
-
-*/
-vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
- : vpRobotWireFrameSimulator(do_display),
- q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
- zeroPos(), reposPos(), toolCustom(false), arm_dir()
-{
- init();
- initDisplay();
-
- tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
-
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_init(&mutex_fMi, NULL);
- pthread_mutex_init(&mutex_artVel, NULL);
- pthread_mutex_init(&mutex_artCoord, NULL);
- pthread_mutex_init(&mutex_velocity, NULL);
- pthread_mutex_init(&mutex_display, NULL);
-
- pthread_attr_init(&attr);
- pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
- pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
- compute_fMi();
-}
-
-/*!
- Basic destructor
-*/
-vpSimulatorAfma6::~vpSimulatorAfma6()
-{
- robotStop = true;
-
- #if defined(_WIN32)
- WaitForSingleObject(hThread,INFINITE);
- CloseHandle(hThread);
- CloseHandle(mutex_fMi);
- CloseHandle(mutex_artVel);
- CloseHandle(mutex_artCoord);
- CloseHandle(mutex_velocity);
- CloseHandle(mutex_display);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_attr_destroy(&attr);
- pthread_join(thread, NULL);
- pthread_mutex_destroy(&mutex_fMi);
- pthread_mutex_destroy(&mutex_artVel);
- pthread_mutex_destroy(&mutex_artCoord);
- pthread_mutex_destroy(&mutex_velocity);
- pthread_mutex_destroy(&mutex_display);
- #endif
-
- if (robotArms != NULL)
- {
- for(int i = 0; i < 6; i++)
- free_Bound_scene (&(robotArms[i]));
- }
-
- delete[] robotArms;
- delete[] fMi;
-}
-
-/*!
- Method which initialises the parameters linked to the robot caracteristics.
-
- Set the path to the arm files (*.bnd and *.sln) used by the
- simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is
- not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
- variable that the user has to set.
-*/
-void
-vpSimulatorAfma6::init()
-{
- // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
- if (vpIoTools::checkDirectory(VISP_ROBOT_ARMS_DIR) == true) // directory exists
- arm_dir = VISP_ROBOT_ARMS_DIR;
- else {
- try {
- arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
- std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
- }
- catch (...) {
- std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
- }
- }
-
- this->init(vpAfma6::TOOL_CCMOP);
- toolCustom = false;
-
- size_fMi = 8;
- fMi = new vpHomogeneousMatrix[8];
- artCoord.resize(njoint);
- artVel.resize(njoint);
-
- zeroPos.resize(njoint);
- zeroPos = 0;
- reposPos.resize(njoint);
- reposPos = 0;
- reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
-
- artCoord = zeroPos;
- artVel = 0;
-
- q_prev_getdis.resize(njoint);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
- positioningVelocity = defaultPositioningVelocity ;
-
- setRobotFrame (vpRobot::ARTICULAR_FRAME);
- this->setRobotState(vpRobot::STATE_STOP);
-
- // Software joint limits in radians
- //_joint_min.resize(njoint);
- _joint_min[0] = -0.6501;
- _joint_min[1] = -0.6001;
- _joint_min[2] = -0.5001;
- _joint_min[3] = -2.7301;
- _joint_min[4] = -0.1001;
- _joint_min[5] = -1.5901;
- //_joint_max.resize(njoint);
- _joint_max[0] = 0.7001;
- _joint_max[1] = 0.5201;
- _joint_max[2] = 0.4601;
- _joint_max[3] = 2.7301;
- _joint_max[4] = 2.4801;
- _joint_max[5] = 1.5901;
-}
-
-/*!
- Method which initialises the parameters linked to the display part.
-*/
-void
-vpSimulatorAfma6::initDisplay()
-{
- robotArms = NULL;
- robotArms = new Bound_scene[6];
- initArms();
- setExternalCameraPosition(vpHomogeneousMatrix(0,0,0,0,0,vpMath::rad(180))*vpHomogeneousMatrix(-0.1,0,4,vpMath::rad(90),0,0));
- cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
- setExternalCameraParameters(cameraParam);
- vpCameraParameters tmp;
- getCameraParameters(tmp,640,480);
- px_int = tmp.get_px();
- py_int = tmp.get_py();
- sceneInitialized = true;
-}
-
-
-/*!
-
- Initialize the robot kinematics with the extrinsic calibration
- parameters associated to a specific camera.
-
- The eMc parameters depend on the camera.
-
- \warning Only perspective projection without distortion is available!
-
- \param tool : Tool to use. Note that the generic camera is not handled.
-
- \param proj_model : Projection model associated to the camera.
-
- \sa vpCameraParameters, init()
-*/
-void
-vpSimulatorAfma6::init (vpAfma6::vpAfma6ToolType tool,
- vpCameraParameters::vpCameraParametersProjType proj_model)
-{
- this->projModel = proj_model;
- unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
- if (arm_dir.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
- unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
- if (full_length > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
-
- // Use here default values of the robot constant parameters.
- switch (tool) {
- case vpAfma6::TOOL_CCMOP: {
- _erc[0] = vpMath::rad(164.35); // rx
- _erc[1] = vpMath::rad( 89.64); // ry
- _erc[2] = vpMath::rad(-73.05); // rz
- _etc[0] = 0.0117; // tx
- _etc[1] = 0.0033; // ty
- _etc[2] = 0.2272; // tz
-
- setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_ccmop.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- _erc[0] = vpMath::rad( 88.33); // rx
- _erc[1] = vpMath::rad( 72.07); // ry
- _erc[2] = vpMath::rad( 2.53); // rz
- _etc[0] = 0.0783; // tx
- _etc[1] = 0.1234; // ty
- _etc[2] = 0.1638; // tz
-
- setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_gripper.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- _erc[0] = vpMath::rad( 90.40); // rx
- _erc[1] = vpMath::rad( 75.11); // ry
- _erc[2] = vpMath::rad( 0.18); // rz
- _etc[0] = 0.0038; // tx
- _etc[1] = 0.1281; // ty
- _etc[2] = 0.1658; // tz
-
- setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
-
- char *name_arm = new char [full_length];
-
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_vacuum.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
- }
- }
-
- vpRotationMatrix eRc(_erc);
- this->_eMc.buildFrom(_etc, eRc);
-
- setToolType(tool);
- return ;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param image_width : Image width used to compute camera calibration.
- \param image_height : Image height used to compute camera calibration.
-
- \warning The image size must be : 640x480 !
-*/
-
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height)
-{
- if (toolCustom)
- {
- vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
- }
- // Set default parameters
- switch (getToolType()) {
- case vpAfma6::TOOL_CCMOP: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480)
- {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
- cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- }
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
- cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- }
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA:
- case vpAfma6::TOOL_VACUUM: {
- std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
- break;
- }
- default:
- vpERROR_TRACE ("This error should not occur!");
- break;
- }
- return;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param I_ : A B&W image send by the current camera in use.
-
- \warning The image size must be : 640x480 !
-*/
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I_)
-{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param I_ : A B&W image send by the current camera in use.
-
- \warning The image size must be : 640x480 !
-*/
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I_)
-{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
-}
-
-
-/*!
- Set the intrinsic camera parameters.
-
- \param cam : The desired camera parameters.
-*/
-void
-vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
-{
- px_int = cam.get_px();
- py_int = cam.get_py();
- toolCustom = true;
-}
-
-
-/*!
- Method lauched by the thread to compute the position of the robot in the articular frame.
-*/
-void
-vpSimulatorAfma6::updateArticularPosition()
-{
- double tcur_1 = tcur;// temporary variable used to store the last time since the last command
-
- while (!robotStop)
- {
- //Get current time
- tprev = tcur_1;
- tcur = vpTime::measureTimeMs();
-
- if(setVelocityCalled || !constantSamplingTimeMode){
- setVelocityCalled = false;
-
- computeArticularVelocity();
-
- double ellapsedTime = (tcur - tprev) * 1e-3;
- if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
- ellapsedTime = getSamplingTime(); // in second
- }
-
- vpColVector articularCoordinates = get_artCoord();
- articularCoordinates = get_artCoord();
- vpColVector articularVelocities = get_artVel();
-
- if (jointLimit)
- {
- double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
- if (art <= _joint_min[jointLimitArt-1] || art >= _joint_max[jointLimitArt-1]) {
- if (verbose_) {
- std::cout << "Joint " << jointLimitArt-1
- << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt-1]) << " < "
- << vpMath::deg(art) << " < " << vpMath::deg(_joint_max[jointLimitArt-1]) << std::endl;
- }
-
- articularVelocities = 0.0;
- }
- else
- jointLimit = false;
- }
-
- articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
- articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
- articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
- articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
- articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
- articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
-
- int jl = isInJointLimit();
-
- if (jl != 0 && jointLimit == false)
- {
- if (jl < 0)
- ellapsedTime = (_joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
- else
- ellapsedTime = (_joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
-
- for (unsigned int i = 0; i < 6; i++)
- articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
-
- jointLimit = true;
- jointLimitArt = (unsigned int)fabs((double)jl);
- }
-
- set_artCoord(articularCoordinates);
- set_artVel(articularVelocities);
-
- compute_fMi();
-
- if (displayAllowed)
- {
- vpDisplay::display(I);
- vpDisplay::displayFrame(I,getExternalCameraPosition (),cameraParam,0.2,vpColor::none, thickness_);
- vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[7],cameraParam,0.1,vpColor::none, thickness_);
- }
-
- if (displayType == MODEL_3D && displayAllowed)
- {
- while (get_displayBusy()) vpTime::wait(2);
- vpSimulatorAfma6::getExternalImage(I);
- set_displayBusy(false);
- }
-
-
- if (0/*displayType == MODEL_DH && displayAllowed*/)
- {
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
-
- //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
-
- vpImagePoint iP, iP_1;
- vpPoint pt;
- pt.setWorldCoordinates (0,0,0);
-
- pt.track(getExternalCameraPosition ());
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
- pt.track(getExternalCameraPosition ()*fMit[0]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
- for (unsigned int k = 1; k < 7; k++)
- {
- pt.track(getExternalCameraPosition ()*fMit[k-1]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
-
- pt.track(getExternalCameraPosition ()*fMit[k]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
-
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
- }
- vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
- }
-
- vpDisplay::flush(I);
-
-
- vpTime::wait( tcur, 1000*getSamplingTime() );
- tcur_1 = tcur;
- }else{
- vpTime::wait(tcur, vpTime::minTimeForUsleepCall);
- }
- }
-}
-
-/*!
- Compute the pose between the robot reference frame and the frames used used to compute the Denavit-Hartenberg representation. The last element of the table corresponds to the pose between the reference frame and the camera frame.
-
- To compute the diferent homogeneous matrices, this function needs the articular coordinates as input.
-
- Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,\f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
-*/
-void
-vpSimulatorAfma6::compute_fMi()
-{
- //vpColVector q = get_artCoord();
- vpColVector q(6);//; = get_artCoord();
- q = get_artCoord();
-
- vpHomogeneousMatrix fMit[8];
-
- double q1 = q[0];
- double q2 = q[1];
- double q3 = q[2];
- double q4 = q[3];
- double q5 = q[4];
- double q6 = q[5];
-
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
- double c6 = cos(q6);
- double s6 = sin(q6);
-
- fMit[0][0][0] = 1;
- fMit[0][1][0] = 0;
- fMit[0][2][0] = 0;
- fMit[0][0][1] = 0;
- fMit[0][1][1] = 1;
- fMit[0][2][1] = 0;
- fMit[0][0][2] = 0;
- fMit[0][1][2] = 0;
- fMit[0][2][2] = 1;
- fMit[0][0][3] = q1;
- fMit[0][1][3] = 0;
- fMit[0][2][3] = 0;
-
- fMit[1][0][0] = 1;
- fMit[1][1][0] = 0;
- fMit[1][2][0] = 0;
- fMit[1][0][1] = 0;
- fMit[1][1][1] = 1;
- fMit[1][2][1] = 0;
- fMit[1][0][2] = 0;
- fMit[1][1][2] = 0;
- fMit[1][2][2] = 1;
- fMit[1][0][3] = q1;
- fMit[1][1][3] = q2;
- fMit[1][2][3] = 0;
-
- fMit[2][0][0] = 1;
- fMit[2][1][0] = 0;
- fMit[2][2][0] = 0;
- fMit[2][0][1] = 0;
- fMit[2][1][1] = 1;
- fMit[2][2][1] = 0;
- fMit[2][0][2] = 0;
- fMit[2][1][2] = 0;
- fMit[2][2][2] = 1;
- fMit[2][0][3] = q1;
- fMit[2][1][3] = q2;
- fMit[2][2][3] = q3;
-
- fMit[3][0][0] = s4;
- fMit[3][1][0] = -c4;
- fMit[3][2][0] = 0;
- fMit[3][0][1] = c4;
- fMit[3][1][1] = s4;
- fMit[3][2][1] = 0;
- fMit[3][0][2] = 0;
- fMit[3][1][2] = 0;
- fMit[3][2][2] = 1;
- fMit[3][0][3] = q1;
- fMit[3][1][3] = q2;
- fMit[3][2][3] = q3;
-
- fMit[4][0][0] = s4*s5;
- fMit[4][1][0] = -c4*s5;
- fMit[4][2][0] = c5;
- fMit[4][0][1] = s4*c5;
- fMit[4][1][1] = -c4*c5;
- fMit[4][2][1] = -s5;
- fMit[4][0][2] = c4;
- fMit[4][1][2] = s4;
- fMit[4][2][2] = 0;
- fMit[4][0][3] = c4*this->_long_56+q1;
- fMit[4][1][3] = s4*this->_long_56+q2;
- fMit[4][2][3] = q3;
-
- fMit[5][0][0] = s4*s5*c6+c4*s6;
- fMit[5][1][0] = -c4*s5*c6+s4*s6;
- fMit[5][2][0] = c5*c6;
- fMit[5][0][1] = -s4*s5*s6+c4*c6;
- fMit[5][1][1] = c4*s5*s6+s4*c6;
- fMit[5][2][1] = -c5*s6;
- fMit[5][0][2] = -s4*c5;
- fMit[5][1][2] = c4*c5;
- fMit[5][2][2] = s5;
- fMit[5][0][3] = c4*this->_long_56+q1;
- fMit[5][1][3] = s4*this->_long_56+q2;
- fMit[5][2][3] = q3;
-
- fMit[6][0][0] = fMit[5][0][0];
- fMit[6][1][0] = fMit[5][1][0];
- fMit[6][2][0] = fMit[5][2][0];
- fMit[6][0][1] = fMit[5][0][1];
- fMit[6][1][1] = fMit[5][1][1];
- fMit[6][2][1] = fMit[5][2][1];
- fMit[6][0][2] = fMit[5][0][2];
- fMit[6][1][2] = fMit[5][1][2];
- fMit[6][2][2] = fMit[5][2][2];
- fMit[6][0][3] = fMit[5][0][3];
- fMit[6][1][3] = fMit[5][1][3];
- fMit[6][2][3] = fMit[5][2][3];
-
-// vpHomogeneousMatrix cMe;
-// get_cMe(cMe);
-// cMe = cMe.inverse();
-// fMit[7] = fMit[6] * cMe;
- vpAfma6::get_fMc(q,fMit[7]);
-
- #if defined(_WIN32)
- WaitForSingleObject(mutex_fMi,INFINITE);
- for (int i = 0; i < 8; i++)
- fMi[i] = fMit[i];
- ReleaseMutex(mutex_fMi);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMi[i] = fMit[i];
- pthread_mutex_unlock (&mutex_fMi);
- #endif
-}
-
-
-/*!
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
-vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState)
-{
- switch (newState) {
- case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- stopMotion();
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- stopMotion();
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- break;
- }
- case vpRobot::STATE_ACCELERATION_CONTROL:
- default:
- break ;
- }
-
- return vpRobot::setRobotState (newState);
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
-
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
- is always 6.
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
-
- - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
-
- - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorAfma6 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in m/s
- qvel[1] = 0.1; // Joint 2 velocity in m/s
- qvel[2] = 0.1; // Joint 3 velocity in m/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-}
- \endcode
-*/
-void
-vpSimulatorAfma6::setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- vpColVector vel_sat(6);
-
- double scale_trans_sat = 1;
- double scale_rot_sat = 1;
- double scale_sat = 1;
- double vel_trans_max = getMaxTranslationVelocity();
- double vel_rot_max = getMaxRotationVelocity();
-
- double vel_abs; // Absolute value
-
- // Velocity saturation
- switch(frame)
- {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
-
- for (unsigned int i = 0 ; i < 3; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_trans_max && !jointLimit)
- {
- vel_trans_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
- "(axis nr. %d).", vel[i], i+1);
- }
-
- vel_abs = fabs (vel[i+3]);
- if (vel_abs > vel_rot_max && !jointLimit) {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i+3], i+4);
- }
- }
-
- if (vel_trans_max > getMaxTranslationVelocity())
- scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
-
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
-
- if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
- {
- if (scale_trans_sat < scale_rot_sat)
- scale_sat = scale_trans_sat;
- else
- scale_sat = scale_rot_sat;
- }
- break;
- }
-
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i], i+1);
- }
- }
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-
- set_velocity (vel * scale_sat);
- setRobotFrame (frame);
- setVelocityCalled = true;
-}
-
-
-/*!
- Compute the articular velocity relative to the velocity in another frame.
-*/
-void
-vpSimulatorAfma6::computeArticularVelocity()
-{
- vpRobot::vpControlFrameType frame = getRobotFrame();
-
- double scale_rot_sat = 1;
- double scale_sat = 1;
- double vel_rot_max = getMaxRotationVelocity();
- double vel_abs;
-
- vpColVector articularCoordinates = get_artCoord();
- vpColVector velocityframe = get_velocity();
- vpColVector articularVelocity;
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix eVc(_eMc);
- vpAfma6::get_eJe(articularCoordinates,eJe_);
- eJe_ = eJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,eJe_);
- articularVelocity = eJe_*eVc*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpAfma6::get_fJe(articularCoordinates,fJe_);
- fJe_ = fJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,fJe_);
- articularVelocity = fJe_*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- articularVelocity = velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-
-
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (articularVelocity[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", articularVelocity[i], i+1);
- }
- }
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
-
- set_artVel(articularVelocity * scale_sat);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-}
-
-
-/*!
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param vel : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in m/s
- q_dot[1] = 0.2; // Joint 2 velocity in m/s
- q_dot[2] = 0.3; // Joint 3 velocity in m/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpSimulatorAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
-
- for ( ; ; ) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-}
- \endcode
-*/
-void
-vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
-{
- vel.resize(6);
-
- vpColVector articularCoordinates = get_artCoord();
- vpColVector articularVelocity = get_artVel();
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix cVe(_eMc);
- vpAfma6::get_eJe(articularCoordinates,eJe_);
- vel = cVe*eJe_*articularVelocity;
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- vel = articularVelocity;
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpAfma6::get_fJe(articularCoordinates,fJe_);
- vel = fJe_*articularVelocity;
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
- }
-}
-
-/*!
- Get the robot time stamped velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param vel : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
-*/
-void
-vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getVelocity(frame, vel);
-}
-
-/*!
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpSimulatorAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
-
- for ( ; ; ) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-}
- \endcode
-*/
-vpColVector
-vpSimulatorAfma6::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector vel(6);
- getVelocity (frame, vel);
-
- return vel;
-}
-
-/*!
- Get the time stamped robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \sa getVelocity(vpRobot::vpControlFrameType frame)
-*/
-vpColVector
-vpSimulatorAfma6::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- vpColVector vel(6);
- getVelocity (frame, vel);
-
- return vel;
-}
-
-void
-vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
-{
- double vel_rot_max = getMaxRotationVelocity();
- double velmax = fabs(q[0]);
- for (unsigned int i = 1; i < 6; i++)
- {
- if (velmax < fabs(q[i]))
- velmax = fabs(q[i]);
- }
-
- double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
- q = q * alpha;
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param q : A six dimension vector corresponding to the
- position to reach. All the positions are expressed in meters for the
- translations and radians for the rotations. If the position is out
- of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X, Y, Z, A, B, C,
- with X,Y,Z the translations, and A,B,C the rotations of the
- end-effector.
-
- - In the camera and the reference frame, rotations are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector position(6);
- // Set positions in the camera frame
- position[0] = 0.1; // x axis, in meter
- position[1] = 0.2; // y axis, in meter
- position[2] = 0.3; // z axis, in meter
- position[3] = M_PI/8; // rotation around x axis, in rad
- position[4] = M_PI/4; // rotation around y axis, in rad
- position[5] = M_PI; // rotation around z axis, in rad
-
- vpSimulatorAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- \endcode
-
-*/
-void
-vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q)
-{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- vpColVector articularCoordinates = get_artCoord();
-
- vpColVector error(6);
- double errsqr = 0;
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
-
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- vpHomogeneousMatrix fMc_;
- vpAfma6::get_fMc(articularCoordinates, fMc_);
-
- vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
- }while (errsqr > 1e-8 && nbSol > 0);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- do
- {
- articularCoordinates = get_artCoord();
- error = q - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (q);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }while (errsqr > 1e-8);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
-
- vpRotationMatrix fRc(rxyz);
- vpHomogeneousMatrix fMc_(txyz, fRc);
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }while (errsqr > 1e-8 && nbSol > 0);
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- This method overloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
- the position to reach. All the positions are expressed in meters for
- the translations and radians for the rotations.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X (pos1), Y (pos2),
- Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
- translations, and A,B,C the rotations of the end-effector.
-
- - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-
-int main()
-{
- // Set positions in the camera frame
- double pos1 = 0.1; // x axis, in meter
- double pos2 = 0.2; // y axis, in meter
- double pos3 = 0.3; // z axis, in meter
- double pos4 = M_PI/8; // rotation around x axis, in rad
- double pos5 = M_PI/4; // rotation around y axis, in rad
- double pos6 = M_PI; // rotation around z axis, in rad
-
- vpSimulatorAfma6 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
-}
- \endcode
-
- \sa setPosition()
-*/
-void vpSimulatorAfma6::setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6)
-{
- try{
- vpColVector position(6) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector q;
- vpSimulatorAfma6 robot;
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-*/
-void vpSimulatorAfma6::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param q : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- vpSimulatorAfma6 robot;
-
- vpColVector position;
- robot.getPosition(vpRobot::REFERENCE_FRAME, position);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame transformation in terms of a
- // homogenous matrix
- vpHomogeneousMatrix fMc(fRc, ftc);
-}
- \endcode
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
- \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
-
-*/
-void
-vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize(6);
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- q = 0;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- q = get_artCoord();
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- vpHomogeneousMatrix fMc_;
- vpAfma6::get_fMc (get_artCoord(), fMc_);
-
- vpRotationMatrix fRc;
- fMc_.extract(fRc);
- vpRxyzVector rxyz(fRc);
-
- vpTranslationVector txyz;
- fMc_.extract(txyz);
-
- for (unsigned int i=0; i <3; i++)
- {
- q[i] = txyz[i];
- q[i+3] = rxyz[i];
- }
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-}
-
-/*!
-
- Get the current time stamped position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param q : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
- */
-void
-vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getPosition(frame, q);
-}
-
-
-/*!
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
-
- The difference is here that the position is returned using a ThetaU
- representation.
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
-*/
-void
-vpSimulatorAfma6::getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position)
-{
- vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame,posRxyz);
-
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
-
- Get the current time stamped position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
-
- The difference is here that the position is returned using a ThetaU
- representation.
-
- */
-void
-vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getPosition(frame, position);
-}
-
-/*!
- This method enables to set the minimum and maximum joint limits for all the six axis of the robot. The three first values have to be given in meter and the others in radian.
-
- \param limitMin : The minimum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
- \param limitMax : The maximum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
-*/
-void
-vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
-{
- if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
- {
- vpTRACE("Joint limit vector has not a size of 6 !");
- return;
- }
-
- _joint_min[0] = limitMin[0];
- _joint_min[1] = limitMin[1];
- _joint_min[2] = limitMin[2];
- _joint_min[3] = limitMin[3];
- _joint_min[4] = limitMin[4];
- _joint_min[5] = limitMin[5];
-
- _joint_max[0] = limitMax[0];
- _joint_max[1] = limitMax[1];
- _joint_max[2] = limitMax[2];
- _joint_max[3] = limitMax[3];
- _joint_max[4] = limitMax[4];
- _joint_max[5] = limitMax[5];
-
-}
-
-/*!
- Test to detect if the robot is near one of its singularities.
-
- The goal is to avoid the problems du to such configurations.
-*/
-bool
-vpSimulatorAfma6::singularityTest(const vpColVector q, vpMatrix &J)
-{
- double q5 = q[4];
-
- bool cond = fabs(q5-M_PI/2) < 1e-1;
-
- if(cond)
- {
- J[0][3] = 0;
- J[0][4] = 0;
- J[1][3] = 0;
- J[1][4] = 0;
- J[3][3] = 0;
- J[3][4] = 0;
- J[5][3] = 0;
- J[5][4] = 0;
- return true;
- }
-
- return false;
-}
-
-/*!
- Method used to check if the robot reached a joint limit.
-*/
-int
-vpSimulatorAfma6::isInJointLimit ()
-{
- int artNumb = 0;
- double diff = 0;
- double difft = 0;
-
- vpColVector articularCoordinates = get_artCoord();
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] <= _joint_min[i])
- {
- difft = _joint_min[i] - articularCoordinates[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = -(int)i-1;
- }
- }
- }
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] >= _joint_max[i])
- {
- difft = articularCoordinates[i] - _joint_max[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = (int)(i+1);
- }
- }
- }
-
- if (artNumb != 0)
- std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
-
- return artNumb;
-}
-
-/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-*/
-void
-vpSimulatorAfma6::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-
-/*!
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- cartesian space. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call
- of this method. The dimension of \e displacement is always
- 6. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
- vpColVector q_cur(6);
-
- q_cur = get_artCoord();
-
- if ( ! first_time_getdis )
- {
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else
- {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-}
-
-/*!
-Read joint positions in a specific Afma6 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#AFMA6 - Position - Version 2.01
-# file: "myposition.pos "
-#
-# R: X Y Z A B C
-# Joint position: X, Y, Z: translations in meters
-# A, B, C: rotations in degrees
-#
-
-R: 0.1 0.3 -0.25 -80.5 80 0
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are
-expressed in meters, while joint rotations A,B,C in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-vpSimulatorAfma6 robot;
-vpColVector q; // Joint position
-robot.readPosFile("myposition.pos", q); // Set the joint position from the file
-robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
-robot.setPositioningVelocity(5); // Positioning velocity set to 5%
-robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-\endcode
-
-\sa savePosFile()
-*/
-bool
-vpSimulatorAfma6::readPosFile(const char *filename, vpColVector &q)
-{
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
- // else
- // return (false); // fin fichier sans position robot.
- }
- }
- else {
- fclose(fd) ;
- return (false); /* fin fichier */
- }
-
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(njoint);
- int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
-
- if (ret != 7) {
- fclose(fd) ;
- return false;
- }
-
- // converts rotations from degrees into radians
- //q.deg2rad();
-
- q[3] = vpMath::rad(q[3]);
- q[4] = vpMath::rad(q[4]);
- q[5] = vpMath::rad(q[5]);
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
- Save joint (articular) positions in a specific Afma6 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : Joint positions [X,Y,Z,A,B,C] to save in the
- filename. Translations X,Y,Z are expressed in meters, while
- rotations A,B,C in radians.
-
- \warning The joint rotations A,B,C written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-bool
-vpSimulatorAfma6::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#AFMA6 - Position - Version 2.01\n\
-#\n\
-# R: X Y Z A B C\n\
-# Joint position: X, Y, Z: translations in meters\n\
-# A, B, C: rotations in degrees\n\
-#\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- q[0],
- q[1],
- q[2],
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
- Moves the robot to the joint position specified in the filename.
-
- \param filename: File containing a joint position.
-
- \sa readPosFile
-*/
-void
-vpSimulatorAfma6::move(const char *filename)
-{
- vpColVector q;
-
- try
- {
- this->readPosFile(filename, q);
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
- }
- catch(...)
- {
- throw;
- }
-}
-
-/*!
- Get the geometric transformation \f$^c{\bf M}_e\f$ between the
- camera frame and the end-effector frame. This transformation is
- constant and correspond to the extrinsic camera parameters estimated
- by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-*/
-void
-vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe)
-{
- vpAfma6::get_cMe(cMe) ;
-}
-
-/*!
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
- end-effector frame. This transformation allows to compute a
- velocity expressed in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-*/
-void
-vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
-{
- vpHomogeneousMatrix cMe ;
- vpAfma6::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- To compute \f$^e{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
- end-effector frame.
-*/
-void
-vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
-{
- try
- {
- vpAfma6::get_eJe(get_artCoord(), eJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-
-/*!
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- To compute \f$^f{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
- reference frame.
-*/
-void
-vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
-{
- try
- {
- vpColVector articularCoordinates = get_artCoord();
- vpAfma6::get_fJe(articularCoordinates, fJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
- Stop the robot.
-*/
-void
-vpSimulatorAfma6::stopMotion()
-{
- if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
- return;
-
- vpColVector stop(6);
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpRobot::setRobotState (vpRobot::STATE_STOP);
-}
-
-
-
-/**********************************************************************************/
-/**********************************************************************************/
-/**********************************************************************************/
-/**********************************************************************************/
-
-/*!
- Initialise the display of the robot's arms.
-
- Set the path to the scene files (*.bnd and *.sln) used by the
- simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
- not valid, the path is set from the VISP_SCENES_DIR environment
- variable that the user has to set.
-*/
-void
-vpSimulatorAfma6::initArms()
-{
- // set scene_dir from #define VISP_SCENE_DIR if it exists
- std::string scene_dir_;
- if (vpIoTools::checkDirectory(VISP_SCENES_DIR) == true) // directory exists
- scene_dir_ = VISP_SCENES_DIR;
- else {
- try {
- scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
- std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
- }
- catch (...) {
- std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
- }
- }
-
- unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
- if (scene_dir_.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
- unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
- if (full_length > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
-
- char *name_cam = new char [full_length];
-
- strcpy(name_cam, scene_dir_.c_str());
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
- if (arm_dir.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
- full_length = (unsigned int)arm_dir.size() + name_length;
- if (full_length > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
-
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_gate.bnd");
- set_scene(name_arm, robotArms, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm1.bnd");
- set_scene(name_arm, robotArms+1, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm2.bnd");
- set_scene(name_arm, robotArms+2, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm3.bnd");
- set_scene(name_arm, robotArms+3, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm4.bnd");
- set_scene(name_arm, robotArms+4, 1.0);
-
- vpAfma6::vpAfma6ToolType tool;
- tool = getToolType();
- strcpy(name_arm, arm_dir.c_str());
- switch (tool) {
- case vpAfma6::TOOL_CCMOP: {
- strcat(name_arm,"/afma6_tool_ccmop.bnd");
- break;
- }
- case vpAfma6::TOOL_GRIPPER: {
- strcat(name_arm,"/afma6_tool_gripper.bnd");
- break;
- }
- case vpAfma6::TOOL_VACUUM: {
- strcat(name_arm,"/afma6_tool_vacuum.bnd");
- break;
- }
- case vpAfma6::TOOL_GENERIC_CAMERA: {
- std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
- break;
- }
- }
- set_scene(name_arm, robotArms+5, 1.0);
-
- add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-//
-// sceneInitialized = true;
-// displayObject = true;
- displayCamera = true;
-
- delete [] name_cam;
- delete [] name_arm;
-}
-
-
-void
-vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
-{
- bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I_,changed);
-
- //if (displacement[2][3] != 0)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- camMf2 = camMf2*displacement;
-
- f2Mf = camMf2.inverse()*camMf;
-
- camMf = camMf2* displacement * f2Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_ext);
- v = (double)I_.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- }
-
- float w44o[4][4],w44cext[4][4],x,y,z;
-
- vp2jlc_matrix(camMf.inverse(),w44cext);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
-
- vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
- display_scene(w44o,robotArms[0],I_, curColor);
-
- vp2jlc_matrix(fMit[0],w44o);
- display_scene(w44o,robotArms[1],I_, curColor);
-
- vp2jlc_matrix(fMit[2],w44o);
- display_scene(w44o,robotArms[2],I_, curColor);
-
- vp2jlc_matrix(fMit[3],w44o);
- display_scene(w44o,robotArms[3],I_, curColor);
-
- vp2jlc_matrix(fMit[4],w44o);
- display_scene(w44o,robotArms[4],I_, curColor);
-
- vp2jlc_matrix(fMit[5],w44o);
- display_scene(w44o,robotArms[5],I_, curColor);
-
- if (displayCamera)
- {
- vpHomogeneousMatrix cMe;
- get_cMe(cMe);
- cMe = cMe.inverse();
- cMe = fMit[6] * cMe;
- vp2jlc_matrix(cMe,w44o);
- display_scene(w44o,camera, I_, camColor);
- }
-
- if (displayObject)
- {
- vp2jlc_matrix(fMo,w44o);
- display_scene(w44o,scene,I_, curColor);
- }
-}
-
-/*!
- This method enables to initialise the joint coordinates of the robot in order
- to position the camera relative to the object.
-
- Before using this method it is advised to set the position of the object thanks
- to the set_fMo() method.
-
- In other terms, set the world to camera transformation
- \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}\f$, and from the inverse kinematics
- set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
-
- \param cMo_ : the desired pose of the camera.
-
- \return false if the robot kinematics is not able to reach the cMo position.
-
-*/
-bool
-vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
-{
- vpColVector stop(6);
- bool status = true;
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpHomogeneousMatrix fMc_;
- fMc_ = fMo * cMo_.inverse();
-
- vpColVector articularCoordinates = get_artCoord();
- int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
-
- if (nbSol == 0) {
- status = false;
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }
-
- if (verbose_)
- std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
-
- set_artCoord(articularCoordinates);
-
- compute_fMi();
-
- return status;
-}
-
-/*!
- This method enables to initialise the pose between the object and the reference frame,
- in order to position the object relative to the camera.
-
- Before using this method it is advised to set the articular coordinates of the robot.
-
- In other terms, set the world to object transformation
- \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
- with \f${\bf q}\f$ the robot joint position
-
- \param cMo_ : the desired pose of the camera.
-*/
-void
-vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
-{
- vpColVector stop(6);
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
- fMo = fMit[7] * cMo_;
-}
-
-/*!
- This method enable to move the robot with respect to the initialized object.
- The robot trajectory is a straight line from the current position to the one corresponding to the desired pose (3D visual servoing).
-
- \param cdMo_ : the desired pose of the camera wrt. the object
- \param Iint : pointer to the image where the internal view is displayed
- \param errMax : maximum error to consider the pose is reached
-
- \return True is the pose is reached, False else
-*/
-bool
-vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
-{
- // get rid of max velocity
- double vMax = getMaxTranslationVelocity();
- double wMax = getMaxRotationVelocity();
- setMaxTranslationVelocity(10.*vMax);
- setMaxRotationVelocity(10.*wMax);
-
- vpColVector v(3),w(3),vel(6);
- vpHomogeneousMatrix cdMc;
- vpTranslationVector cdTc;vpRotationMatrix cdRc;vpThetaUVector cdTUc;
- vpColVector err(6);err=1.;
- const double lambda = 5.;
- double t;
-
- vpVelocityTwistMatrix cVe;
-
- unsigned int i,iter=0;
- while((iter++<300) & (err.euclideanNorm()>errMax))
- {
- t = vpTime::measureTimeMs();
-
- // update image
- if(Iint != NULL)
- {
- vpDisplay::display(*Iint);
- getInternalView(*Iint);
- vpDisplay::flush(*Iint);
- }
-
- // update pose error
- cdMc = cdMo_*get_cMo().inverse();
- cdMc.extract(cdRc);
- cdMc.extract(cdTc);
- cdTUc.buildFrom(cdRc);
-
- // compute v,w and velocity
- v = -lambda*cdRc.t()*cdTc;
- w = -lambda*cdTUc;
- for(i=0;i<3;++i)
- {
- vel[i] = v[i];
- vel[i+3] = w[i];
- err[i] = cdTc[i];
- err[i+3] = cdTUc[i];
- }
-
- // update feat
- setVelocity(vpRobot::CAMERA_FRAME,vel);
-
- // wait for it
- vpTime::wait(t,10);
- }
- vel=0.;
- set_velocity(vel);
- set_artVel(vel);
- setMaxTranslationVelocity(vMax);
- setMaxRotationVelocity(wMax);
-
- //std::cout << "setPosition: final error " << err.t() << std::endl;
- return(err.euclideanNorm()<= errMax);
-}
-
-#endif
diff --git a/src/robot/simulator-robot/vpSimulatorAfma6.h b/src/robot/simulator-robot/vpSimulatorAfma6.h
deleted file mode 100644
index c2e8681..0000000
--- a/src/robot/simulator-robot/vpSimulatorAfma6.h
+++ /dev/null
@@ -1,287 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorAfma6.h 2598 2010-06-02 09:20:22Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class which provides a simulator for the robot Afma6.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpSimulatorAfma6_HH
-#define vpSimulatorAfma6_HH
-
-/*!
- \file vpSimulatorAfma6.h
- \brief Class which provides a simulator for the robot Afma6.
-*/
-
-#include <visp/vpRobotWireFrameSimulator.h>
-#include <visp/vpAfma6.h>
-
-#include <string>
-
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-
-/*!
- \class vpSimulatorAfma6
-
- \ingroup Afma6 RobotSimuWithVisu
-
-
- \brief Simulator of Irisa's gantry robot named Afma6.
-
- Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
- Afma6 robot. This robot is a gantry robot with six degrees of
- freedom manufactured in 1992 by the french Afma-Robots company.
-
- \warning This class uses threading capabilities. Thus on Unix-like
- platforms, the libpthread third-party library need to be
- installed. On Windows, we use the native threading capabilities.
-
- This class allows to control the Afma6 gantry robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The direct and inverse kinematics models are implemented in the
- vpAfma6 class.
-
- To control the robot in position, you may set the controller
- to position control and then send the position to reach in a specific
- frame like here in the joint space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorAfma6 robot;
-
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
-
- vpColVector q(6);
- // Set a joint position
- q[0] = 0.1; // Joint 1 position, in meter
- q[1] = 0.2; // Joint 2 position, in meter
- q[2] = 0.3; // Joint 3 position, in meter
- q[3] = M_PI/8; // Joint 4 position, in rad
- q[4] = M_PI/4; // Joint 5 position, in rad
- q[5] = M_PI; // Joint 6 position, in rad
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
- return 0;
-}
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and then send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorAfma6.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorAfma6 robot;
-
- robot.init(vpAfma6::TOOL_GRIPPER, vpCameraParameters::perspectiveProjWithoutDistortion);
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in m/s
- qvel[1] = 0.1; // Joint 2 velocity in m/s
- qvel[2] = 0.1; // Joint 3 velocity in m/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-
- return 0;
-}
- \endcode
-
- It is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and savePosFile()
- methods.
-
- To know how this class can be used to achieve a visual servoing simulation,
- you can follow the \ref tutorial-ibvs.
-
-*/
-
-
-class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
-{
- public:
- static const double defaultPositioningVelocity;
-
- private:
- vpColVector q_prev_getdis;
- bool first_time_getdis;
-
- double positioningVelocity;
-
- vpColVector zeroPos;
- vpColVector reposPos;
-
- bool toolCustom;
- std::string arm_dir;
-
-public:
- vpSimulatorAfma6();
- vpSimulatorAfma6(bool display);
- virtual ~vpSimulatorAfma6();
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height);
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I);
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
- void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
- double getPositioningVelocity (void){return positioningVelocity;}
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe);
- void get_cVe(vpVelocityTwistMatrix &cVe);
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
- bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
- void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
-
- void move(const char *filename) ;
-
- static bool readPosFile(const char *filename, vpColVector &q);
- static bool savePosFile(const char *filename, const vpColVector &q);
- void setCameraParameters(const vpCameraParameters &cam) ;
- void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
-
- void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6);
- void setPosition(const char *filename);
- void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
- bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
-
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
-
- void stopMotion();
-
-
-protected:
- void computeArticularVelocity();
- void compute_fMi();
- void findHighestPositioningSpeed(vpColVector &q);
- void getExternalImage(vpImage<vpRGBa> &I);
- inline void get_fMi(vpHomogeneousMatrix *fMit) {
-#if defined(_WIN32)
- WaitForSingleObject(mutex_fMi,INFINITE);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- ReleaseMutex(mutex_fMi);
-#elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- pthread_mutex_unlock (&mutex_fMi);
-#endif
- }
- void init();
- void initArms();
- void initDisplay();
- int isInJointLimit (void);
- bool singularityTest(const vpColVector q, vpMatrix &J);
- void updateArticularPosition();
-
-private:
- void getCameraDisplacement(vpColVector &displacement);
- void getArticularDisplacement(vpColVector &displacement);
-};
-
-#endif
-
-#endif
diff --git a/src/robot/simulator-robot/vpSimulatorCamera.cpp b/src/robot/simulator-robot/vpSimulatorCamera.cpp
deleted file mode 100644
index f88eda3..0000000
--- a/src/robot/simulator-robot/vpSimulatorCamera.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorCamera.cpp 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines the simplest robot : a free flying camera.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpSimulatorCamera.cpp
- \brief class that defines the simplest robot : a free flying camera
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpSimulatorCamera.h>
-
-
-/*!
- Default constructor that sets the transformation between
- world frame and camera frame to identity.
-
-*/
-vpSimulatorCamera::vpSimulatorCamera() : wMc_()
-{
- init() ;
-}
-
-/*!
- Robot initialisation.
-
- Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
- is set to identity (see get_eJe()).
-
-*/
-void vpSimulatorCamera::init()
-{
- nDof = 6;
- eJe.resize(6,6) ;
- eJe.setIdentity() ;
- eJeAvailable = true;
- fJeAvailable = false;
- areJointLimitsAvailable = false;
- qmin = NULL;
- qmax = NULL;
-
- setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
- setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
-}
-
-
-/*!
- Destructor.
-
-*/
-vpSimulatorCamera::~vpSimulatorCamera()
-{
-}
-
-/*!
-
- Get the twist transformation from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation. Here this transformation is equal to identity
- since camera frame and end-effector frame are at the same location.
-
-*/
-void
-vpSimulatorCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- vpVelocityTwistMatrix cVe_;
- cVe = cVe_;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
- For that simple robot the Jacobian is the identity.
-
- \param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
- J}_e\f$ expressed in the end-effector frame. Yhis matrix is equal to identity.
-*/
-void
-vpSimulatorCamera::get_eJe(vpMatrix &eJe_)
-{
- eJe_ = this->eJe ;
-}
-
-/*!
- Get the robot position in the world frame.
-
-*/
-void
-vpSimulatorCamera::getPosition(vpHomogeneousMatrix &wMc) const
-{
- wMc = this->wMc_ ;
-}
-
-/*
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the articular
- position of each dof, first the 3 translations, then the 3
- articular rotation positions represented by a vpRxyzVector.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
-*/
-void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME :
- q = 0;
- break;
-
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::REFERENCE_FRAME : {
- // Convert wMc_ to a position
- // From fMc extract the pose
- vpRotationMatrix wRc;
- this->wMc_.extract(wRc);
- vpRxyzVector rxyz;
- rxyz.buildFrom(wRc);
-
- for (unsigned int i=0; i < 3; i++) {
- q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
-
- break;
- }
- case vpRobot::MIXT_FRAME :
- std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
- }
-}
-
-/*!
- Send to the controller a velocity.
-
- \param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
- and camera frame (vpRobot::CAMERA_FRAME) are implemented.
-
- \param v : Velocity to apply to the robot.
-
- - In the camera frame, this velocity is represented by a vector of dimension 6
- \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
-
- - In articular, this velocity is represented by a 6 dimension vector \f$
- \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
- u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
- robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
- here set to identity.
-
- We use the exponential map (vpExponentialMap) to update the camera location.
- Sampling time can be set using setSamplingTime().
-
- \sa setSamplingTime()
-
-*/
-void
-vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &v)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
-
- switch (frame)
- {
- case vpRobot::ARTICULAR_FRAME:
- case vpRobot::CAMERA_FRAME: {
- vpColVector v_max(6);
-
- for (unsigned int i=0; i<3; i++)
- v_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- v_max[i] = getMaxRotationVelocity();
-
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
-
- wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
- setRobotFrame(frame);
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the reference frame:"
- "functionality not implemented");
- break ;
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
-
- break ;
- }
-}
-
-/*!
- Set the robot position in the world frame.
-
- \param wMc : Transformation from world frame to camera frame.
-*/
-void vpSimulatorCamera::setPosition(const vpHomogeneousMatrix &wMc)
-{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_POSITION_CONTROL);
- }
-
- this->wMc_ = wMc;
-}
diff --git a/src/robot/simulator-robot/vpSimulatorCamera.h b/src/robot/simulator-robot/vpSimulatorCamera.h
deleted file mode 100644
index 70f22cf..0000000
--- a/src/robot/simulator-robot/vpSimulatorCamera.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorCamera.h 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines the simplest robot : a free flying camera.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpSimulatorCamera_H
-#define vpSimulatorCamera_H
-
-/*!
- \file vpSimulatorCamera.h
- \brief class that defines the simplest robot : a free flying camera
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotSimulator.h>
-
-/*!
- \class vpSimulatorCamera
-
- \ingroup RobotSimuWithoutVisu
-
- \brief Class that defines the simplest robot: a free flying camera.
-
- This free flying camera has 6 dof; 3 in translation and 3 in rotation.
- It evolves as a gentry robot with respect to a world frame. This class
- is similar to vpRobotCamera class except that here the position of the robot
- is provided as the transformation from world frame to camera frame; wMc. This
- representation is more intuitive than the one implemented in vpRobotCamera
- where the transformation from camera to world frame is considered; cMw.
-
- For this particular simulated robot, the end-effector and camera frame are confused.
- That means that the cMe transformation is equal to identity.
-
- The robot jacobian expressed in the end-effector frame
- \f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
-
- The following code shows how to control this robot in position and velocity.
- \code
-#include <visp/vpSimulatorCamera.h>
-
-int main()
-{
- vpHomogeneousMatrix wMc;
- vpSimulatorCamera robot;
-
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
-
- wMc[2][3] = 1.; // Camera frame is 1 meter along z axis in front of the world frame
- robot.setPosition(wMc); // Set the new position of the camera in the world frame
- std::cout << "New position of the camera in the world frame wMc:\n" << wMc << std::endl;
-
- robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
- robot.setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
- robot.setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
-
- vpColVector v(6);
- v = 0;
- v[2] = 1.; // set v_z to 1 m/s
- robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- // The robot has moved from 0.1 meters along the z axis
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
-}
- \endcode
-
- To know how this class can be used to achieve a visual servoing simulation,
- you can follow the \ref tutorial-ibvs.
-*/
-class VISP_EXPORT vpSimulatorCamera : public vpRobotSimulator
-{
-protected:
- vpHomogeneousMatrix wMc_; // world to camera
-
-public:
- vpSimulatorCamera() ;
- virtual ~vpSimulatorCamera() ;
-
-public:
- void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_eJe(vpMatrix &eJe);
-
- void getPosition(vpHomogeneousMatrix &wMc) const;
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void setPosition(const vpHomogeneousMatrix &wMc);
- void setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &vel) ;
-
-private:
- void init() ;
-
- // Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
-
-#endif
diff --git a/src/robot/simulator-robot/vpSimulatorPioneer.cpp b/src/robot/simulator-robot/vpSimulatorPioneer.cpp
deleted file mode 100644
index eb66459..0000000
--- a/src/robot/simulator-robot/vpSimulatorPioneer.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorPioneer.cpp 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pioneer mobile robot simulator without display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpSimulatorPioneer.cpp
- \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
-
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpSimulatorPioneer.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpExponentialMap.h>
-
-
-/*!
- Constructor.
-
-*/
-vpSimulatorPioneer::vpSimulatorPioneer()
- : wMc_(), wMe_(), xm_(0), ym_(0), theta_(0)
-{
- init() ;
-}
-
-/*!
- Robot initialisation.
-
- Sampling time is set to 40 ms. To change it you should call
- setSamplingTime().
-
-*/
-void vpSimulatorPioneer::init()
-{
- xm_ = 0;
- ym_ = 0;
- theta_ = 0;
-
- nDof = 2;
- eJeAvailable = true;
- fJeAvailable = false;
- areJointLimitsAvailable = false;
- qmin = NULL;
- qmax = NULL;
-
- wMc_ = wMe_ * cMe_.inverse();
-}
-
-
-/*!
- Destructor.
-
-*/
-vpSimulatorPioneer::~vpSimulatorPioneer()
-{
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
- The jacobian expression is given in vpPioneer class.
-
- \param _eJe : A 6 by 2 matrix representing the robot jacobian \f$ {^e}{\bf
- J}_e\f$ expressed in the end-effector frame.
-*/
-void
-vpSimulatorPioneer::get_eJe(vpMatrix &_eJe)
-{
- _eJe = vpUnicycle::get_eJe();
-}
-
-
-/*!
- Send to the controller a velocity.
-
- \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
-
- \param v : Velocity vector \f$(v_x, w_z)\f$ to apply to the robot.
-
- Depending on the velocity specified as input, the robot position is updated
- using the sampling time that can be modified using setSamplingTime().
-
- \sa setSamplingTime()
-
-*/
-void
-vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &v)
-{
- switch (frame)
- {
- case vpRobot::ARTICULAR_FRAME: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
- setRobotFrame(frame);
-
- // v is a 2 dimension vector that contains v,w
- if (v.size() != 2) {
- vpERROR_TRACE ("Bad dimension of the control vector");
- throw vpRobotException (vpRobotException::dimensionError,
- "Bad dimension of the control vector");
- }
-
- vpColVector v_max(2);
-
- v_max[0] = getMaxTranslationVelocity();
- v_max[1] = getMaxRotationVelocity();
-
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
-
- xm_ += delta_t_ * v_sat[0] * cos(theta_);
- ym_ += delta_t_ * v_sat[0] * sin(theta_);
- theta_ += delta_t_ * v_sat[1];
-
- vpRotationMatrix wRe(0, 0, theta_);
- vpTranslationVector wte(xm_, ym_, 0);
- wMe_.buildFrom(wte, wRe);
- wMc_ = wMe_ * cMe_.inverse();
-
- break ;
- }
- break ;
- case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the camera frame:"
- "functionality not implemented");
- break ;
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the articular frame:"
- "functionality not implemented");
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
-
- break ;
- }
-}
-
-/*!
- Get the robot position in the world frame.
-
-*/
-void
-vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const
-{
- wMc = this->wMc_ ;
-}
-
-/*
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes (not implemented)
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, this functionality is not implemented.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
-*/
-void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME :
- q = 0;
- break;
-
- case vpRobot::ARTICULAR_FRAME :
- std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
- break;
- case vpRobot::REFERENCE_FRAME : {
- // Convert wMc_ to a position
- // From fMc extract the pose
- vpRotationMatrix wRc;
- this->wMc_.extract(wRc);
- vpRxyzVector rxyz;
- rxyz.buildFrom(wRc);
-
- for (unsigned int i=0; i < 3; i++) {
- q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
-
- break;
- }
- case vpRobot::MIXT_FRAME :
- std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
- }
-}
diff --git a/src/robot/simulator-robot/vpSimulatorPioneer.h b/src/robot/simulator-robot/vpSimulatorPioneer.h
deleted file mode 100644
index 1357e2b..0000000
--- a/src/robot/simulator-robot/vpSimulatorPioneer.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorPioneer.h 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pioneer mobile robot simulator without display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpSimulatorPioneer_H
-#define vpSimulatorPioneer_H
-
-/*!
- \file vpSimulatorPioneer.h
- \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpPioneer.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotSimulator.h>
-
-/*!
- \class vpSimulatorPioneer
-
- \ingroup RobotSimuWithoutVisu
-
- \brief Class that defines the Pioneer mobile robot simulator equipped with a static camera.
-
- It intends to simulate the mobile robot described in vpPioneer class.
- This robot has 2 dof: \f$(v_x, w_z)\f$, the translational and
- rotational velocities that are applied at point E.
-
- The robot position evolves with respect to a world frame; wMc. When a new joint velocity
- is applied to the robot using setVelocity(), the position of the camera wrt the world frame
- is updated.
-
- \image html pioneer.png
-
- The following code shows how to control this robot in position and velocity.
- \code
-#include <visp/vpSimulatorPioneer.h>
-
-int main()
-{
- vpHomogeneousMatrix wMc;
- vpSimulatorPioneer robot;
-
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
-
- robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
- robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s
- robot.setMaxRotationVelocity(vpMath::rad(90)); // wz max set to 90 deg/s
-
- vpColVector v(2); // we control vx and wz dof
- v = 0;
- v[0] = 1.; // set vx to 1 m/s
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
- // The robot has moved from 0.1 meters along the z axis
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
-}
- \endcode
-
- The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
-
-*/
-class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, public vpRobotSimulator
-{
-
-protected:
- // world to camera
- vpHomogeneousMatrix wMc_ ;
- // world to end effector frame which is also the mobile
- // robot frame located between the two wheels
- vpHomogeneousMatrix wMe_ ;
- // cMe_ is a protected member of vpUnicycle
-
- double xm_;
- double ym_;
- double theta_;
-
-public:
- vpSimulatorPioneer() ;
- virtual ~vpSimulatorPioneer();
-
-public:
- void get_eJe(vpMatrix &eJe);
-
- void getPosition(vpHomogeneousMatrix &wMc) const;
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &vel) ;
-
-private:
- void init() ;
-
- // Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-
-} ;
-
-#endif
-
diff --git a/src/robot/simulator-robot/vpSimulatorPioneerPan.cpp b/src/robot/simulator-robot/vpSimulatorPioneerPan.cpp
deleted file mode 100644
index 25c8281..0000000
--- a/src/robot/simulator-robot/vpSimulatorPioneerPan.cpp
+++ /dev/null
@@ -1,265 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorPioneerPan.cpp 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pioneer mobile robot equipped with a pan head simulator without display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpSimulatorPioneerPan.cpp
- \brief class that defines the Pioneer mobile robot simulator equipped
- with a camera able to move in pan.
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpSimulatorPioneerPan.h>
-#include <visp/vpRobotException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpExponentialMap.h>
-
-
-/*!
- Constructor.
-
- Initialise the robot by a call to init().
-
- Sampling time is set to 40 ms. To change it you should call
- setSamplingTime().
-
-*/
-vpSimulatorPioneerPan::vpSimulatorPioneerPan() : wMc_(), wMm_(), xm_(0), ym_(0), theta_(0), q_pan_()
-{
- init() ;
-}
-
-/*!
- Robot initialisation.
-
- Sampling time is set to 40 ms. To change it you should call
- setSamplingTime().
-
- Robot jacobian expressed in the end-effector frame \f$ {^e}{\bf J}_e \f$
- is set to identity (see get_eJe()).
-
-*/
-void vpSimulatorPioneerPan::init()
-{
- xm_ = 0;
- ym_ = 0;
- theta_ = 0;
- q_pan_ = 0;
-
- nDof = 3;
- eJeAvailable = true;
- fJeAvailable = false;
- areJointLimitsAvailable = false;
- qmin = NULL;
- qmax = NULL;
-
- wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
-}
-
-
-/*!
- Destructor.
-
-*/
-vpSimulatorPioneerPan::~vpSimulatorPioneerPan()
-{
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
- The jacobian expression is given in vpPioneerPan class.
-
- \param _eJe : A 6 by 3 matrix representing the robot jacobian \f$ {^e}{\bf
- J}_e\f$ expressed in the end-effector frame.
-*/
-void
-vpSimulatorPioneerPan::get_eJe(vpMatrix &_eJe)
-{
- _eJe = vpUnicycle::get_eJe();
-}
-
-/*!
- Send to the controller a velocity.
-
- \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
-
- \param v : Velocity vector \f$(v_x, w_z, \dot q)\f$ to apply to the robot.
-
- Depending on the velocity specified as input, the robot position is updated
- using the sampling time that can be modified using setSamplingTime().
-
- \sa setSamplingTime()
-
-*/
-void
-vpSimulatorPioneerPan::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &v)
-{
- switch (frame)
- {
- case vpRobot::ARTICULAR_FRAME:{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
-
- setRobotFrame(frame);
-
- // v is a 3 dimension vector that contains vx, wz, qpan
- if (v.size() != 3) {
- vpERROR_TRACE ("Bad dimension of the control vector");
- throw vpRobotException (vpRobotException::dimensionError,
- "Bad dimension of the control vector");
- }
-
- vpColVector v_max(3);
-
- v_max[0] = getMaxTranslationVelocity();
- v_max[1] = getMaxRotationVelocity();
- v_max[2] = getMaxRotationVelocity();
-
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
-
- xm_ += delta_t_ * v_sat[0] * cos(theta_);
- ym_ += delta_t_ * v_sat[0] * sin(theta_);
- theta_ += delta_t_ * v_sat[1];
- q_pan_ += delta_t_ * v_sat[2];
-
- vpRotationMatrix wRm(0, 0, theta_);
- vpTranslationVector wtm(xm_, ym_, 0);
- wMm_.buildFrom(wtm, wRm);
-
- // Update the end effector pose
- set_pMe(q_pan_);
-
- // Update the camera pose
- wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
-
- // Update the jacobian
- set_eJe(q_pan_);
-
- break ;
- }
- case vpRobot::CAMERA_FRAME: {
- vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the camera frame:"
- "functionality not implemented");
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the reference frame:"
- "functionality not implemented");
- break ;
-
- case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
-
- break ;
- }
-}
-
-/*!
- Get the robot position in the world frame.
-
-*/
-void
-vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const
-{
- wMc = this->wMc_ ;
-}
-
-/*
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes (not implemented)
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
-
- \param position : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, this functionality is not implemented.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
-*/
-void vpSimulatorPioneerPan::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize (6);
-
- switch (frame) {
- case vpRobot::CAMERA_FRAME :
- q = 0;
- break;
-
- case vpRobot::ARTICULAR_FRAME :
- std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
- break;
- case vpRobot::REFERENCE_FRAME : {
- // Convert wMc_ to a position
- // From fMc extract the pose
- vpRotationMatrix wRc;
- this->wMc_.extract(wRc);
- vpRxyzVector rxyz;
- rxyz.buildFrom(wRc);
-
- for (unsigned int i=0; i < 3; i++) {
- q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
- }
-
- break;
- }
- case vpRobot::MIXT_FRAME :
- std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
- }
-}
diff --git a/src/robot/simulator-robot/vpSimulatorPioneerPan.h b/src/robot/simulator-robot/vpSimulatorPioneerPan.h
deleted file mode 100644
index d01e5d5..0000000
--- a/src/robot/simulator-robot/vpSimulatorPioneerPan.h
+++ /dev/null
@@ -1,144 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorPioneerPan.h 2456 2010-01-07 10:33:12Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pioneer mobile robot equipped with a pan head simulator without display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpSimulatorPioneerPan_H
-#define vpSimulatorPioneerPan_H
-
-/*!
- \file vpSimulatorPioneerPan.h
- \brief class that defines the Pioneer mobile robot simulator equipped
- with a camera able to move in pan.
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpPioneerPan.h>
-#include <visp/vpRobot.h>
-#include <visp/vpRobotSimulator.h>
-
-/*!
- \class vpSimulatorPioneerPan
-
- \ingroup RobotSimuWithoutVisu
-
- \brief Class that defines the Pioneer mobile robot simulator equipped
- with a camera able to move in pan.
-
- It intends to simulate the mobile robot described in vpPioneerPan class.
- This robot has 3 dof: \f$(v_x, w_z, \dot{q_1})\f$, the translational and
- rotational velocities of the mobile platform, the pan head velocity respectively.
-
- The robot position evolves with respect to a world frame; wMc. When a new joint velocity
- is applied to the robot using setVelocity(), the position of the camera wrt the world frame
- is updated.
-
- \image html pioneer-pan.png
-
- The following code shows how to control this robot in position and velocity.
- \code
-#include <visp/vpSimulatorPioneerPan.h>
-
-int main()
-{
- vpHomogeneousMatrix wMc;
- vpSimulatorPioneerPan robot;
-
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;
-
- robot.setSamplingTime(0.100); // Modify the default sampling time to 0.1 second
- robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s
- robot.setMaxRotationVelocity(vpMath::rad(90)); // wz max set to 90 deg/s
-
- vpColVector v(3); // we control vx, wz and q_pan
- v = 0;
- v[0] = 1.; // set vx to 1 m/s
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
- // The robot has moved from 0.1 meters along the z axis
- robot.getPosition(wMc); // Position of the camera in the world frame
- std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
-}
- \endcode
-
- The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
-
-*/
-class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, public vpRobotSimulator
-{
-
-protected:
- //! robot / camera location in the world frame
- vpHomogeneousMatrix wMc_ ; // world to camera
- vpHomogeneousMatrix wMm_ ; // world to mobile robot frame located between the two weels
- // mMp_ mobile robot to pan frame is a protected member of vpPioneerPan
- // pMe_ pan head to end effector frame is a protected member of vpPioneerPan
- // cMe_ is a protected member of vpUnicycle
-
- double xm_;
- double ym_;
- double theta_;
- double q_pan_;
-
-public:
- vpSimulatorPioneerPan() ;
- virtual ~vpSimulatorPioneerPan();
-
-public:
- void get_eJe(vpMatrix &eJe);
-
- void getPosition(vpHomogeneousMatrix &wMc) const;
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &vel) ;
-
-private:
- void init() ;
-
- // Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
-
-#endif
-
diff --git a/src/robot/simulator-robot/vpSimulatorViper850.cpp b/src/robot/simulator-robot/vpSimulatorViper850.cpp
deleted file mode 100644
index 9286b2a..0000000
--- a/src/robot/simulator-robot/vpSimulatorViper850.cpp
+++ /dev/null
@@ -1,2516 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorViper850.cpp 5171 2015-01-15 15:11:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class which provides a simulator for the robot Viper850.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpTime.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpIoTools.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <string>
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-
-const double vpSimulatorViper850::defaultPositioningVelocity = 25.0;
-
-/*!
- Basic constructor
-*/
-vpSimulatorViper850::vpSimulatorViper850()
- : vpRobotWireFrameSimulator(), vpViper850(),
- q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
- zeroPos(), reposPos(), toolCustom(false), arm_dir()
-{
- init();
- initDisplay();
-
- tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
-
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined (VISP_HAVE_PTHREAD)
- pthread_mutex_init(&mutex_fMi, NULL);
- pthread_mutex_init(&mutex_artVel, NULL);
- pthread_mutex_init(&mutex_artCoord, NULL);
- pthread_mutex_init(&mutex_velocity, NULL);
- pthread_mutex_init(&mutex_display, NULL);
-
- pthread_attr_init(&attr);
- pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
- pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
- compute_fMi();
-}
-
-/*!
- Constructor used to enable or disable the external view of the robot.
-
- \param do_display : When true, enables the display of the external view.
-
-*/
-vpSimulatorViper850::vpSimulatorViper850(bool do_display)
- : vpRobotWireFrameSimulator(do_display),
- q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
- zeroPos(), reposPos(), toolCustom(false), arm_dir()
-{
- init();
- initDisplay();
-
- tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
-
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_init(&mutex_fMi, NULL);
- pthread_mutex_init(&mutex_artVel, NULL);
- pthread_mutex_init(&mutex_artCoord, NULL);
- pthread_mutex_init(&mutex_velocity, NULL);
- pthread_mutex_init(&mutex_display, NULL);
-
- pthread_attr_init(&attr);
- pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
- pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
- compute_fMi();
-}
-
-/*!
- Basic destructor
-*/
-vpSimulatorViper850::~vpSimulatorViper850()
-{
- robotStop = true;
-
- #if defined(_WIN32)
- WaitForSingleObject(hThread,INFINITE);
- CloseHandle(hThread);
- CloseHandle(mutex_fMi);
- CloseHandle(mutex_artVel);
- CloseHandle(mutex_artCoord);
- CloseHandle(mutex_velocity);
- CloseHandle(mutex_display);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_attr_destroy(&attr);
- pthread_join(thread, NULL);
- pthread_mutex_destroy(&mutex_fMi);
- pthread_mutex_destroy(&mutex_artVel);
- pthread_mutex_destroy(&mutex_artCoord);
- pthread_mutex_destroy(&mutex_velocity);
- pthread_mutex_destroy(&mutex_display);
- #endif
-
- if (robotArms != NULL)
- {
- // free_Bound_scene (&(camera));
- for(int i = 0; i < 6; i++)
- free_Bound_scene (&(robotArms[i]));
- }
-
- delete[] robotArms;
- delete[] fMi;
-}
-
-/*!
- Method which initialises the parameters linked to the robot caracteristics.
-
- Set the path to the arm files (*.bnd and *.sln) used by the
- simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is
- not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
- variable that the user has to set.
-*/
-void
-vpSimulatorViper850::init()
-{
- // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
- if (vpIoTools::checkDirectory(VISP_ROBOT_ARMS_DIR) == true) // directory exists
- arm_dir = VISP_ROBOT_ARMS_DIR;
- else {
- try {
- arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
- std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
- }
- catch (...) {
- std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
- }
- }
-
- this->init(vpViper850::defaultTool);
- toolCustom = false;
-
- size_fMi = 8;
- fMi = new vpHomogeneousMatrix[8];
- artCoord.resize(njoint);
- artVel.resize(njoint);
-
- zeroPos.resize(njoint);
- zeroPos = 0;
- zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
- reposPos.resize(njoint);
- reposPos = 0;
- reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
-
- artCoord = reposPos;
- artVel = 0;
-
- q_prev_getdis.resize(njoint);
- q_prev_getdis = 0;
- first_time_getdis = true;
-
- positioningVelocity = defaultPositioningVelocity ;
-
- setRobotFrame (vpRobot::ARTICULAR_FRAME);
- this->setRobotState(vpRobot::STATE_STOP);
-
- // Software joint limits in radians
- //joint_min.resize(njoint);
- joint_min[0] = vpMath::rad(-50);
- joint_min[1] = vpMath::rad(-135);
- joint_min[2] = vpMath::rad(-40);
- joint_min[3] = vpMath::rad(-190);
- joint_min[4] = vpMath::rad(-110);
- joint_min[5] = vpMath::rad(-184);
- //joint_max.resize(njoint);
- joint_max[0] = vpMath::rad(50);
- joint_max[1] = vpMath::rad(-40);
- joint_max[2] = vpMath::rad(215);
- joint_max[3] = vpMath::rad(190);
- joint_max[4] = vpMath::rad(110);
- joint_max[5] = vpMath::rad(184);
-}
-
-/*!
- Method which initialises the parameters linked to the display part.
-*/
-void
-vpSimulatorViper850::initDisplay()
-{
- robotArms = NULL;
- robotArms = new Bound_scene[6];
- initArms();
- setExternalCameraPosition(vpHomogeneousMatrix(0.0,0.5,1.5,vpMath::rad(90),0,0));
- cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
- setExternalCameraParameters(cameraParam);
- vpCameraParameters tmp;
- getCameraParameters(tmp,640,480);
- px_int = tmp.get_px();
- py_int = tmp.get_py();
- sceneInitialized = true;
-}
-
-
-/*!
-
- Initialize the robot kinematics with the extrinsic calibration
- parameters associated to a specific camera.
-
- The eMc parameters depend on the camera.
-
- \warning Only perspective projection without distortion is available!
-
- \param tool : Tool to use.
-
- \param proj_model : Projection model associated to the camera.
-
- \sa vpCameraParameters, init()
-*/
-void
-vpSimulatorViper850::init (vpViper850::vpToolType tool,
- vpCameraParameters::vpCameraParametersProjType proj_model)
-{
- this->projModel = proj_model;
-
- // Use here default values of the robot constant parameters.
- switch (tool) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
- erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
-
- setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- erc[0] = vpMath::rad(0.1527764261); // rx
- erc[1] = vpMath::rad(1.285092455); // ry
- erc[2] = vpMath::rad(-90.75777848); // rz
- etc[0] = -0.04558630174; // tx
- etc[1] = -0.00134326752; // ty
- etc[2] = 0.001000828017; // tz
-
- setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
- break;
- }
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
- case vpViper850::TOOL_GENERIC_CAMERA: {
- std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
- break;
- }
- }
-
- vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc, eRc);
-
- setToolType(tool);
- return ;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param image_width : Image width used to compute camera calibration.
- \param image_height : Image height used to compute camera calibration.
-
- \warning The image size must be : 640x480 !
-*/
-
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height)
-{
- if (toolCustom)
- {
- vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
- }
- // Set default parameters
- switch (getToolType()) {
- case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480)
- {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
- cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- }
- break;
- }
- case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- // Set default intrinsic camera parameters for 640x480 images
- if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
- cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- }
- break;
- }
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
- case vpViper850::TOOL_GENERIC_CAMERA: {
- std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
- break;
- }
- }
- return;
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param I_ : A B&W image send by the current camera in use.
-
- \warning The image size must be : 640x480 !
-*/
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I_)
-{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
-}
-
-/*!
- Get the current intrinsic camera parameters obtained by calibration.
-
- \param cam : In output, camera parameters to fill.
- \param I_ : A B&W image send by the current camera in use.
-
- \warning The image size must be : 640x480 !
-*/
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I_)
-{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
-}
-
-
-/*!
- Set the intrinsic camera parameters.
-
- \param cam : The desired camera parameters.
-*/
-void
-vpSimulatorViper850::setCameraParameters(const vpCameraParameters &cam)
-{
- px_int = cam.get_px();
- py_int = cam.get_py();
- toolCustom = true;
-}
-
-
-/*!
- Method lauched by the thread to compute the position of the robot in the articular frame.
-*/
-void
-vpSimulatorViper850::updateArticularPosition()
-{
- double tcur_1 = tcur;// temporary variable used to store the last time since the last command
-
- while (!robotStop)
- {
- //Get current time
- tprev = tcur_1;
- tcur = vpTime::measureTimeMs();
-
-
- if(setVelocityCalled || !constantSamplingTimeMode){
- setVelocityCalled = false;
- computeArticularVelocity();
-
- double ellapsedTime = (tcur - tprev) * 1e-3;
- if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
- ellapsedTime = getSamplingTime(); // in second
- }
-
- vpColVector articularCoordinates = get_artCoord();
- articularCoordinates = get_artCoord();
- vpColVector articularVelocities = get_artVel();
-
- if (jointLimit)
- {
- double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
- if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
- if (verbose_) {
- std::cout << "Joint " << jointLimitArt-1
- << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
- }
- articularVelocities = 0.0;
- }
- else
- jointLimit = false;
- }
-
- articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
- articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
- articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
- articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
- articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
- articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
-
- int jl = isInJointLimit();
-
- if (jl != 0 && jointLimit == false)
- {
- if (jl < 0)
- ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
- else
- ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
-
- for (unsigned int i = 0; i < 6; i++)
- articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
-
- jointLimit = true;
- jointLimitArt = (unsigned int)fabs((double)jl);
- }
-
- set_artCoord(articularCoordinates);
- set_artVel(articularVelocities);
-
- compute_fMi();
-
- if (displayAllowed)
- {
- vpDisplay::display(I);
- vpDisplay::displayFrame(I,getExternalCameraPosition (),cameraParam,0.2,vpColor::none, thickness_);
- vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[7],cameraParam,0.1,vpColor::none, thickness_);
- }
-
- if (displayType == MODEL_3D && displayAllowed)
- {
- while (get_displayBusy()) vpTime::wait(2);
- vpSimulatorViper850::getExternalImage(I);
- set_displayBusy(false);
- }
-
-
- if (displayType == MODEL_DH && displayAllowed)
- {
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
-
- //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
-
- vpImagePoint iP, iP_1;
- vpPoint pt;
- pt.setWorldCoordinates (0,0,0);
-
- pt.track(getExternalCameraPosition ());
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
- pt.track(getExternalCameraPosition ()*fMit[0]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
- vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
- for (int k = 1; k < 7; k++)
- {
- pt.track(getExternalCameraPosition ()*fMit[k-1]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
-
- pt.track(getExternalCameraPosition ()*fMit[k]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
-
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
- }
- vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
- }
-
- vpDisplay::flush(I);
-
- vpTime::wait( tcur, 1000 * getSamplingTime() );
- tcur_1 = tcur;
- }else{
- vpTime::wait(tcur, vpTime::minTimeForUsleepCall);
- }
- }
-}
-
-/*!
- Compute the pose between the robot reference frame and the frames used to compute the Denavit-Hartenberg
- representation. The last element of the table corresponds to the pose between the reference frame and
- the camera frame.
-
- To compute the different homogeneous matrices, this function needs the articular coordinates as input.
-
- Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,
- \f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
-*/
-void
-vpSimulatorViper850::compute_fMi()
-{
- //vpColVector q = get_artCoord();
- vpColVector q(6);//; = get_artCoord();
- q = get_artCoord();
-
- vpHomogeneousMatrix fMit[8];
-
- double q1 = q[0];
- double q2 = q[1];
- double q3 = q[2];
- double q4 = q[3];
- double q5 = q[4];
- double q6 = q[5];
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
- double c6 = cos(q6);
- double s6 = sin(q6);
-
- fMit[0][0][0] = c1;
- fMit[0][1][0] = s1;
- fMit[0][2][0] = 0;
- fMit[0][0][1] = 0;
- fMit[0][1][1] = 0;
- fMit[0][2][1] = -1;
- fMit[0][0][2] = -s1;
- fMit[0][1][2] = c1;
- fMit[0][2][2] = 0;
- fMit[0][0][3] = a1*c1;
- fMit[0][1][3] = a1*s1;
- fMit[0][2][3] = d1;
-
- fMit[1][0][0] = c1*c2;
- fMit[1][1][0] = s1*c2;
- fMit[1][2][0] = -s2;
- fMit[1][0][1] = -c1*s2;
- fMit[1][1][1] = -s1*s2;
- fMit[1][2][1] = -c2;
- fMit[1][0][2] = -s1;
- fMit[1][1][2] = c1;
- fMit[1][2][2] = 0;
- fMit[1][0][3] = c1*(a2*c2+a1);
- fMit[1][1][3] = s1*(a2*c2+a1);
- fMit[1][2][3] = d1-a2*s2;
-
- double quickcomp1 = a3*c23-a2*c2-a1;
-
- fMit[2][0][0] = -c1*c23;
- fMit[2][1][0] = -s1*c23;
- fMit[2][2][0] = s23;
- fMit[2][0][1] = s1;
- fMit[2][1][1] = -c1;
- fMit[2][2][1] = 0;
- fMit[2][0][2] = c1*s23;
- fMit[2][1][2] = s1*s23;
- fMit[2][2][2] = c23;
- fMit[2][0][3] = -c1*quickcomp1;
- fMit[2][1][3] = -s1*quickcomp1;
- fMit[2][2][3] = a3*s23-a2*s2+d1;
-
- double quickcomp2 = c1*(s23*d4 - quickcomp1);
- double quickcomp3 = s1*(s23*d4 - quickcomp1);
-
- fMit[3][0][0] = -c1*c23*c4+s1*s4;
- fMit[3][1][0] = -s1*c23*c4-c1*s4;
- fMit[3][2][0] = s23*c4;
- fMit[3][0][1] = c1*s23;
- fMit[3][1][1] = s1*s23;
- fMit[3][2][1] = c23;
- fMit[3][0][2] = -c1*c23*s4-s1*c4;
- fMit[3][1][2] = -s1*c23*s4+c1*c4;
- fMit[3][2][2] = s23*s4;
- fMit[3][0][3] = quickcomp2;
- fMit[3][1][3] = quickcomp3;
- fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
-
- fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
- fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
- fMit[4][2][0] = s23*c4*c5+c23*s5;
- fMit[4][0][1] = c1*c23*s4+s1*c4;
- fMit[4][1][1] = s1*c23*s4-c1*c4;
- fMit[4][2][1] = -s23*s4;
- fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
- fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
- fMit[4][2][2] = -s23*c4*s5+c23*c5;
- fMit[4][0][3] = quickcomp2;
- fMit[4][1][3] = quickcomp3;
- fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
-
- fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
- fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
- fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMit[5][2][2] = -s23*c4*s5+c23*c5;
- fMit[5][0][3] = quickcomp2;
- fMit[5][1][3] = quickcomp3;
- fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
-
- fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
- fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
- fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMit[6][2][2] = -s23*c4*s5+c23*c5;
- fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
- fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
- fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
-
- vpHomogeneousMatrix cMe;
- get_cMe(cMe);
- cMe = cMe.inverse();
-// fMit[7] = fMit[6] * cMe;
- vpViper::get_fMc(q,fMit[7]);
-
- #if defined(_WIN32)
- WaitForSingleObject(mutex_fMi,INFINITE);
- for (int i = 0; i < 8; i++)
- fMi[i] = fMit[i];
- ReleaseMutex(mutex_fMi);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMi[i] = fMit[i];
- pthread_mutex_unlock (&mutex_fMi);
- #endif
-}
-
-
-/*!
-Change the robot state.
-
-\param newState : New requested robot state.
-*/
-vpRobot::vpRobotStateType
-vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState)
-{
- switch (newState) {
- case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- stopMotion();
- }
- break;
- }
- case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- stopMotion();
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- break;
- }
- case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- break;
- }
- case vpRobot::STATE_ACCELERATION_CONTROL:
- default:
- break ;
- }
-
- return vpRobot::setRobotState (newState);
-}
-
-/*!
- Apply a velocity to the robot.
-
- \param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
-
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
- is always 6.
-
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
-
- - In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
-
- - In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
-
- - In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
- \exception vpRobotException::wrongStateError : If a the robot is not
- configured to handle a velocity. The robot can handle a velocity only if the
- velocity control mode is set. For that, call setRobotState(
- vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
-
- \warning Velocities could be saturated if one of them exceed the
- maximal autorized speed (see vpRobot::maxTranslationVelocity and
- vpRobot::maxRotationVelocity). To change these values use
- setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-}
- \endcode
-*/
-void
-vpSimulatorViper850::setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
- vpColVector vel_sat(6);
-
- double scale_trans_sat = 1;
- double scale_rot_sat = 1;
- double scale_sat = 1;
- double vel_trans_max = getMaxTranslationVelocity();
- double vel_rot_max = getMaxRotationVelocity();
-
- double vel_abs; // Absolute value
-
- // Velocity saturation
- switch(frame)
- {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
-
- for (unsigned int i = 0 ; i < 3; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_trans_max && !jointLimit)
- {
- vel_trans_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
- "(axis nr. %d).", vel[i], i+1);
- }
-
- vel_abs = fabs (vel[i+3]);
- if (vel_abs > vel_rot_max && !jointLimit) {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i+3], i+4);
- }
- }
-
- if (vel_trans_max > getMaxTranslationVelocity())
- scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
-
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
-
- if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
- {
- if (scale_trans_sat < scale_rot_sat)
- scale_sat = scale_trans_sat;
- else
- scale_sat = scale_rot_sat;
- }
- break;
- }
-
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i], i+1);
- }
- }
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-
- set_velocity (vel * scale_sat);
- setRobotFrame (frame);
- setVelocityCalled = true;
-}
-
-
-/*!
- Compute the articular velocity relative to the velocity in another frame.
-*/
-void
-vpSimulatorViper850::computeArticularVelocity()
-{
- vpRobot::vpControlFrameType frame = getRobotFrame();
-
- double scale_rot_sat = 1;
- double scale_sat = 1;
- double vel_rot_max = getMaxRotationVelocity();
- double vel_abs;
-
- vpColVector articularCoordinates = get_artCoord();
- vpColVector velocityframe = get_velocity();
- vpColVector articularVelocity;
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix eVc(eMc);
- vpViper850::get_eJe(articularCoordinates,eJe_);
- eJe_ = eJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,eJe_);
- articularVelocity = eJe_*eVc*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpViper850::get_fJe(articularCoordinates,fJe_);
- fJe_ = fJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,fJe_);
- articularVelocity = fJe_*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- articularVelocity = velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-
-
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (articularVelocity[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", articularVelocity[i], i+1);
- }
- }
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
-
- set_artVel(articularVelocity * scale_sat);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
- }
-}
-
-
-/*!
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param vel : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpSimulatorViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
-
- for ( ; ; ) {
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-}
- \endcode
-*/
-void
-vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
-{
- vel.resize(6);
-
- vpColVector articularCoordinates = get_artCoord();
- vpColVector articularVelocity = get_artVel();
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix cVe(eMc);
- vpViper850::get_eJe(articularCoordinates,eJe_);
- vel = cVe*eJe_*articularVelocity;
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- vel = articularVelocity;
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpViper850::get_fJe(articularCoordinates,fJe_);
- vel = fJe_*articularVelocity;
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
- }
-}
-
-/*!
- Get the robot time stamped velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param vel : Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
-
- \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
-*/
-void
-vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getVelocity(frame, vel);
-}
-
-/*!
- Get the robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- // Set requested joint velocities
- vpColVector q_dot(6);
- q_dot[0] = 0.1; // Joint 1 velocity in rad/s
- q_dot[1] = 0.2; // Joint 2 velocity in rad/s
- q_dot[2] = 0.3; // Joint 3 velocity in rad/s
- q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
- q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
- q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
-
- vpSimulatorViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- // Moves the joint in velocity
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
-
- // Initialisation of the velocity measurement
- vpColVector q_dot_mes; // Measured velocities
-
- for ( ; ; ) {
- q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
- vpTime::wait(40); // wait 40 ms
- // here q_dot_mes is equal to [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
- }
-}
- \endcode
-*/
-vpColVector
-vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame)
-{
- vpColVector vel(6);
- getVelocity (frame, vel);
-
- return vel;
-}
-
-/*!
- Get the time stamped robot velocities.
-
- \param frame : Frame in wich velocities are mesured.
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \return Measured velocities. Translations are expressed in m/s
- and rotations in rad/s.
-
- \sa getVelocity(vpRobot::vpControlFrameType frame)
-*/
-vpColVector
-vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- vpColVector vel(6);
- getVelocity (frame, vel);
-
- return vel;
-}
-
-void
-vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
-{
- double vel_rot_max = getMaxRotationVelocity();
- double velmax = fabs(q[0]);
- for (unsigned int i = 1; i < 6; i++)
- {
- if (velmax < fabs(q[i]))
- velmax = fabs(q[i]);
- }
-
- double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
- q = q * alpha;
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param q : A six dimension vector corresponding to the
- position to reach. All the positions are expressed in meters for the
- translations and radians for the rotations. If the position is out
- of range, an exception is provided.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X, Y, Z, A, B, C,
- with X,Y,Z the translations, and A,B,C the rotations of the
- end-effector.
-
- - In the camera and the reference frame, rotations are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector position(6);
- // Set positions in the camera frame
- position[0] = 0.1; // x axis, in meter
- position[1] = 0.2; // y axis, in meter
- position[2] = 0.3; // z axis, in meter
- position[3] = M_PI/8; // rotation around x axis, in rad
- position[4] = M_PI/4; // rotation around y axis, in rad
- position[5] = M_PI; // rotation around z axis, in rad
-
- vpSimulatorViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
-}
- \endcode
-
- To catch the exception if the position is out of range, modify the code like:
-
- \code
- try {
- robot.setPosition(vpRobot::CAMERA_FRAME, position);
- }
- catch (vpRobotException e) {
- if (e.getCode() == vpRobotException::positionOutOfRangeError) {
- std::cout << "The position is out of range" << std::endl;
- }
- \endcode
-
-*/
-void
-vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q)
-{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
-
- vpColVector articularCoordinates = get_artCoord();
-
- vpColVector error(6);
- double errsqr = 0;
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- unsigned int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
-
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- vpHomogeneousMatrix fMc_;
- vpViper850::get_fMc(articularCoordinates, fMc_);
-
- vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc2, qdes, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
- }while (errsqr > 1e-8 && nbSol > 0);
-
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- do
- {
- articularCoordinates = get_artCoord();
- error = q - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (q);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }while (errsqr > 1e-8);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- unsigned int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
-
- vpRotationMatrix fRc(rxyz);
- vpHomogeneousMatrix fMc_(txyz, fRc);
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc_, qdes, verbose_);
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }while (errsqr > 1e-8 && nbSol > 0);
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-}
-
-/*!
- Move to an absolute position with a given percent of max velocity.
- The percent of max velocity is to set with setPositioningVelocity().
- The position to reach can be specified in joint coordinates, in the
- camera frame or in the reference frame.
-
- This method overloads setPosition(const
- vpRobot::vpControlFrameType, const vpColVector &).
-
- \warning This method is blocking. It returns only when the position
- is reached by the robot.
-
- \param pos1, pos2, pos3, pos4, pos5, pos6 : The six coordinates of
- the position to reach. All the positions are expressed in meters for
- the translations and radians for the rotations.
-
- \param frame : Frame in which the position is expressed.
-
- - In the joint space, positions are respectively X (pos1), Y (pos2),
- Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the
- translations, and A,B,C the rotations of the end-effector.
-
- - In the camera and the reference frame, rotations [pos4, pos5, pos6] are
- represented by a vpRxyzVector.
-
- - Mixt frame is not implemented. By mixt frame we mean, translations
- expressed in the reference frame, and rotations in the camera
- frame.
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-
-int main()
-{
- // Set positions in the camera frame
- double pos1 = 0.1; // x axis, in meter
- double pos2 = 0.2; // y axis, in meter
- double pos3 = 0.3; // z axis, in meter
- double pos4 = M_PI/8; // rotation around x axis, in rad
- double pos5 = M_PI/4; // rotation around y axis, in rad
- double pos6 = M_PI; // rotation around z axis, in rad
-
- vpRobotViper850 robot;
-
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 20%
- robot.setPositioningVelocity(20);
-
- // Moves the robot in the camera frame
- robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
-}
- \endcode
-
- \sa setPosition()
-*/
-void vpSimulatorViper850::setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6)
-{
- try{
- vpColVector position(6) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
- Move to an absolute joint position with a given percent of max
- velocity. The robot state is set to position control. The percent
- of max velocity is to set with setPositioningVelocity(). The
- position to reach is defined in the position file.
-
- \param filename : Name of the position file to read. The
- readPosFile() documentation shows a typical content of such a
- position file.
-
- This method has the same behavior than the sample code given below;
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpColVector q;
- vpSimulatorViper850 robot;
-
- robot.readPosFile("MyPositionFilename.pos", q);
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
- \endcode
-
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME not
- implemented.
-
- \exception vpRobotException::positionOutOfRangeError : The requested
- position is out of range.
-
- \sa setPositioningVelocity()
-*/
-void vpSimulatorViper850::setPosition(const char *filename)
-{
- vpColVector q;
- bool ret;
-
- ret = this->readPosFile(filename, q);
-
- if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
- }
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition(vpRobot::ARTICULAR_FRAME, q);
-}
-
-/*!
- Get the current position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param q : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- vpColVector position;
- robot.getPosition(vpRobot::REFERENCE_FRAME, position);
-
- vpTranslationVector ftc; // reference frame to camera frame translations
- vpRxyzVector frc; // reference frame to camera frame rotations
-
- // Update the transformation between reference frame and camera frame
- for (int i=0; i < 3; i++) {
- ftc[i] = position[i]; // tx, ty, tz
- frc[i] = position[i+3]; // ry, ry, rz
- }
-
- // Create a rotation matrix from the Rxyz rotation angles
- vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
-
- // Create the camera to fix frame transformation in terms of a
- // homogenous matrix
- vpHomogeneousMatrix fMc(fRc, ftc);
-}
- \endcode
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
- \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
-
-*/
-void
-vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
-{
- q.resize(6);
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- q = 0;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- q = get_artCoord();
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- vpHomogeneousMatrix fMc_;
- vpViper::get_fMc (get_artCoord(), fMc_);
-
- vpRotationMatrix fRc;
- fMc_.extract(fRc);
- vpRxyzVector rxyz(fRc);
-
- vpTranslationVector txyz;
- fMc_.extract(txyz);
-
- for (unsigned int i=0; i <3; i++)
- {
- q[i] = txyz[i];
- q[i+3] = rxyz[i];
- }
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
- }
- }
-}
-
-/*!
-
- Get the current time stamped position of the robot.
-
- \param frame : Control frame type in which to get the position, either :
- - in the camera cartesien frame,
- - joint (articular) coordinates of each axes
- - in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference
- frame, and rotation in camera frame)
-
- \param q : Measured position of the robot:
- - in camera cartesien frame, a 6 dimension vector, set to 0.
-
- - in articular, a 6 dimension vector corresponding to the joint
- position of each dof in radians.
-
- - in reference frame, a 6 dimension vector, the first 3 values correspond to
- the translation tx, ty, tz in meters (like a vpTranslationVector), and the
- last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
- below show how to convert this position into a vpHomogenousMatrix:
-
- \param timestamp : Unix time in second since January 1st 1970.
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
- */
-void
-vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getPosition(frame, q);
-}
-
-
-/*!
- Get the current position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
-
- The difference is here that the position is returned using a ThetaU
- representation.
-
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
-*/
-void
-vpSimulatorViper850::getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
-{
- vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame,posRxyz);
-
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
- }
-}
-
-/*!
-
- Get the current time stamped position of the robot.
-
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
-
- The difference is here that the position is returned using a ThetaU
- representation.
-
- */
-void
-vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
-{
- timestamp = vpTime::measureTimeSecond();
- getPosition(frame, position);
-}
-
-/*!
- This method enables to set the minimum and maximum joint limits for all the six axis of the robot. All the values have to be given in radian.
-
- \param limitMin : The minimum joint limits are given in a vector of size 6. All the value must be in radian.
- \param limitMax : The maximum joint limits are given in a vector of size 6. All the value must be in radian.
-*/
-void
-vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
-{
- if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
- {
- vpTRACE("Joint limit vector has not a size of 6 !");
- return;
- }
-
- joint_min[0] = limitMin[0];
- joint_min[1] = limitMin[1];
- joint_min[2] = limitMin[2];
- joint_min[3] = limitMin[3];
- joint_min[4] = limitMin[4];
- joint_min[5] = limitMin[5];
-
- joint_max[0] = limitMax[0];
- joint_max[1] = limitMax[1];
- joint_max[2] = limitMax[2];
- joint_max[3] = limitMax[3];
- joint_max[4] = limitMax[4];
- joint_max[5] = limitMax[5];
-
-}
-
-/*!
- Test to detect if the robot is near one of its singularities.
-
- The goal is to avoid the problems du to such configurations.
-*/
-bool
-vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
-{
- double q2 = q[1];
- double q3 = q[2];
- double q5 = q[4];
-
- double c2 = cos(q2);
- double c3 = cos(q3);
- double s3 = sin(q3);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
- double s5 = sin(q5);
-
- bool cond1 = fabs(s5) < 1e-1;
- bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
- bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
-
- if(cond1)
- {
- J[3][0] = 0;
- J[5][0] = 0;
- J[3][1] = 0;
- J[5][1] = 0;
- J[3][2] = 0;
- J[5][2] = 0;
- J[3][3] = 0;
- J[5][3] = 0;
- J[3][4] = 0;
- J[5][4] = 0;
- J[3][5] = 0;
- J[5][5] = 0;
- return true;
- }
-
- if(cond2)
- {
- J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
- J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
- J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
- return true;
- }
-
- if(cond3)
- {
- J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
- J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
- }
-
- return false;
-}
-
-/*!
- Method used to check if the robot reached a joint limit.
-*/
-int
-vpSimulatorViper850::isInJointLimit ()
-{
- int artNumb = 0;
- double diff = 0;
- double difft = 0;
-
- vpColVector articularCoordinates = get_artCoord();
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] <= joint_min[i])
- {
- difft = joint_min[i] - articularCoordinates[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = -(int)i-1;
- }
- }
- }
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] >= joint_max[i])
- {
- difft = articularCoordinates[i] - joint_max[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = (int)(i+1);
- }
- }
- }
-
- if (artNumb != 0)
- std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
-
- return artNumb;
-}
-
-/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-*/
-void
-vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-
-/*!
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
- Get the robot displacement since the last call of this method.
-
- \warning This functionnality is not implemented for the moment in the
- cartesian space. It is only available in the joint space
- (vpRobot::ARTICULAR_FRAME).
-
- \param frame : The frame in which the measured displacement is expressed.
-
- \param displacement : The measured displacement since the last call
- of this method. The dimension of \e displacement is always
- 6. Translations are expressed in meters, rotations in radians.
-
- In camera or reference frame, rotations are expressed with the
- Euler Rxyz representation.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
-{
- displacement.resize (6);
- displacement = 0;
- vpColVector q_cur(6);
-
- q_cur = get_artCoord();
-
- if ( ! first_time_getdis )
- {
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- break ;
- }
- }
- }
- else
- {
- first_time_getdis = false;
- }
-
- // Memorize the joint position for the next call
- q_prev_getdis = q_cur;
-}
-
-/*!
-Read joint positions in a specific Viper850 position file.
-
-This position file has to start with a header. The six joint positions
-are given after the "R:" keyword. The first 3 values correspond to the
-joint translations X,Y,Z expressed in meters. The 3 last values
-correspond to the joint rotations A,B,C expressed in degres to be more
-representative for the user. Theses values are then converted in
-radians in \e q. The character "#" starting a line indicates a
-comment.
-
-A typical content of such a file is given below:
-
-\code
-#Viper - Position - Version 1.0
-# file: "myposition.pos "
-#
-# R: A B C D E F
-# Joint position in degrees
-#
-
-R: 0.1 0.3 -0.25 -80.5 80 0
-\endcode
-
-\param filename : Name of the position file to read.
-
-\param q : The six joint positions. Values are expressed in radians.
-
-\return true if the positions were successfully readen in the file. false, if
-an error occurs.
-
-The code below shows how to read a position from a file and move the robot to this position.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- // Enable the position control of the robot
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Get the current robot joint positions
- vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
-
- // Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
-
- // Get the position from a file and move to the registered position
- robot.readPosFile("current.pos", q); // Set the joint position from the file
-
- robot.setPositioningVelocity(5); // Positioning velocity set to 5%
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
-}
-\endcode
-
-\sa savePosFile()
-*/
-bool
-vpSimulatorViper850::readPosFile(const char *filename, vpColVector &q)
-{
- FILE * fd ;
- fd = fopen(filename, "r") ;
- if (fd == NULL)
- return false;
-
- char line[FILENAME_MAX];
- char dummy[FILENAME_MAX];
- char head[] = "R:";
- bool sortie = false;
-
- do {
- // Saut des lignes commencant par #
- if (fgets (line, FILENAME_MAX, fd) != NULL) {
- if ( strncmp (line, "#", 1) != 0) {
- // La ligne n'est pas un commentaire
- if ( strncmp (line, head, sizeof(head)-1) == 0) {
- sortie = true; // Position robot trouvee.
- }
- // else
- // return (false); // fin fichier sans position robot.
- }
- }
- else {
- fclose(fd) ;
- return (false); /* fin fichier */
- }
- }
- while ( sortie != true );
-
- // Lecture des positions
- q.resize(njoint);
- int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
- dummy,
- &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
- if (ret != 7) {
- fclose(fd) ;
- return false;
- }
-
- // converts rotations from degrees into radians
- q.deg2rad();
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
- Save joint (articular) positions in a specific Viper850 position file.
-
- This position file starts with a header on the first line. After
- convertion of the rotations in degrees, the joint position \e q is
- written on a line starting with the keyword "R: ". See readPosFile()
- documentation for an example of such a file.
-
- \param filename : Name of the position file to create.
-
- \param q : The six joint positions to save in the
- filename. Values are expressed in radians.
-
- \warning All the six joint rotations written in the file are converted
- in degrees to be more representative for the user.
-
- \return true if the positions were successfully saved in the file. false, if
- an error occurs.
-
- \sa readPosFile()
-*/
-bool
-vpSimulatorViper850::savePosFile(const char *filename, const vpColVector &q)
-{
-
- FILE * fd ;
- fd = fopen(filename, "w") ;
- if (fd == NULL)
- return false;
-
- fprintf(fd, "\
-#Viper - Position - Version 1.0\n\
-#\n\
-# R: A B C D E F\n\
-# Joint position in degrees\n\
-#\n\
-#\n\n");
-
- // Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
- return (true);
-}
-
-/*!
- Moves the robot to the joint position specified in the filename.
-
- \param filename: File containing a joint position.
-
- \sa readPosFile
-*/
-void
-vpSimulatorViper850::move(const char *filename)
-{
- vpColVector q;
-
- try
- {
- this->readPosFile(filename, q);
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
- }
- catch(...)
- {
- throw;
- }
-}
-
-/*!
- Get the geometric transformation \f$^c{\bf M}_e\f$ between the
- camera frame and the end-effector frame. This transformation is
- constant and correspond to the extrinsic camera parameters estimated
- by calibration.
-
- \param cMe : Transformation between the camera frame and the
- end-effector frame.
-*/
-void
-vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe)
-{
- vpViper850::get_cMe(cMe) ;
-}
-
-/*!
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
- end-effector frame. This transformation allows to compute a
- velocity expressed in the end-effector frame into the camera frame.
-
- \param cVe : Twist transformation.
-*/
-void
-vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
-{
- vpHomogeneousMatrix cMe ;
- vpViper850::get_cMe(cMe) ;
-
- cVe.buildFrom(cMe) ;
-}
-
-/*!
- Get the robot jacobian expressed in the end-effector frame.
-
- To compute \f$^e{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
- end-effector frame.
-*/
-void
-vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
-{
- try
- {
- vpViper850::get_eJe(get_artCoord(), eJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
-}
-
-/*!
- Get the robot jacobian expressed in the robot reference frame also
- called fix frame.
-
- To compute \f$^f{\bf J}_e\f$, we communicate with the low level
- controller to get the joint position of the robot.
-
- \param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
- reference frame.
-*/
-void
-vpSimulatorViper850::get_fJe(vpMatrix &fJe_)
-{
- try
- {
- vpColVector articularCoordinates = get_artCoord();
- vpViper850::get_fJe(articularCoordinates, fJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
-}
-
-/*!
- Stop the robot.
-*/
-void
-vpSimulatorViper850::stopMotion()
-{
- if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
- return;
-
- vpColVector stop(6);
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpRobot::setRobotState (vpRobot::STATE_STOP);
-}
-
-
-
-/**********************************************************************************/
-/**********************************************************************************/
-/**********************************************************************************/
-/**********************************************************************************/
-
-/*!
- Initialise the display of the robot's arms.
-
- Set the path to the scene files (*.bnd and *.sln) used by the
- simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
- not valid, the path is set from the VISP_SCENES_DIR environment
- variable that the user has to set.
-*/
-void
-vpSimulatorViper850::initArms()
-{
- // set scene_dir from #define VISP_SCENE_DIR if it exists
- std::string scene_dir_;
- if (vpIoTools::checkDirectory(VISP_SCENES_DIR) == true) // directory exists
- scene_dir_ = VISP_SCENES_DIR;
- else {
- try {
- scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
- std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
- }
- catch (...) {
- std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
- }
- }
-
- unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
- if (scene_dir_.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
-
- unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
- if (full_length > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
-
- char *name_cam = new char [full_length];
-
- strcpy(name_cam, scene_dir_.c_str());
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
- if (arm_dir.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
- full_length = (unsigned int)arm_dir.size() + name_length;
- if (full_length > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
-
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm1.bnd");
- set_scene(name_arm, robotArms, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm2.bnd");
- set_scene(name_arm, robotArms+1, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm3.bnd");
- set_scene(name_arm, robotArms+2, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm4.bnd");
- set_scene(name_arm, robotArms+3, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm5.bnd");
- set_scene(name_arm, robotArms+4, 1.0);
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm6.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
-
-// set_scene("./arm2.bnd", robotArms+1, 1.0);
-// set_scene("./arm3.bnd", robotArms+2, 1.0);
-// set_scene("./arm4.bnd", robotArms+3, 1.0);
-// set_scene("./arm5.bnd", robotArms+4, 1.0);
-// set_scene("./arm6.bnd", robotArms+5, 1.0);
-
- add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-//
-// sceneInitialized = true;
-// displayObject = true;
- displayCamera = true;
-
- delete [] name_cam;
- delete [] name_arm;
-}
-
-
-void
-vpSimulatorViper850::getExternalImage(vpImage<vpRGBa> &I_)
-{
- bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I_,changed);
-
- //if (displacement[2][3] != 0)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- camMf2 = camMf2*displacement;
-
- f2Mf = camMf2.inverse()*camMf;
-
- camMf = camMf2* displacement * f2Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_ext);
- v = (double)I_.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- }
-
- float w44o[4][4],w44cext[4][4],x,y,z;
-
- vp2jlc_matrix(camMf.inverse(),w44cext);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
-
- vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
- display_scene(w44o,robotArms[0],I_, curColor);
-
- vp2jlc_matrix(fMit[0],w44o);
- display_scene(w44o,robotArms[1],I_, curColor);
-
- vp2jlc_matrix(fMit[1],w44o);
- display_scene(w44o,robotArms[2],I_, curColor);
-
- vp2jlc_matrix(fMit[2],w44o);
- display_scene(w44o,robotArms[3],I_, curColor);
-
- vp2jlc_matrix(fMit[3],w44o);
- display_scene(w44o,robotArms[4],I_, curColor);
-
- vp2jlc_matrix(fMit[6],w44o);
- display_scene(w44o,robotArms[5],I_, curColor);
-
- if (displayCamera)
- {
- vpHomogeneousMatrix cMe;
- get_cMe(cMe);
- cMe = cMe.inverse();
- cMe = fMit[6] * cMe;
- vp2jlc_matrix(cMe,w44o);
- display_scene(w44o,camera, I_, camColor);
- }
-
- if (displayObject)
- {
- vp2jlc_matrix(fMo,w44o);
- display_scene(w44o,scene,I_, curColor);
- }
-}
-
-/*!
- This method enables to initialise the joint coordinates of the robot in order to
- position the camera relative to the object.
-
- Before using this method it is advised to set the position of the object thanks
- to the set_fMo() method.
-
- In other terms, set the world to camera transformation
- \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}{_o})^{-1}\f$, and from the inverse kinematics
- set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
-
- \param cMo_ : the desired pose of the camera.
-
- \return false if the robot kinematics is not able to reach the cMo position.
-*/
-bool
-vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
-{
- vpColVector stop(6);
- bool status = true;
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpHomogeneousMatrix fMc_;
- fMc_ = fMo * cMo_.inverse();
-
- vpColVector articularCoordinates = get_artCoord();
- unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
-
- if (nbSol == 0) {
- status = false;
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }
-
- if (verbose_)
- std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
-
- set_artCoord(articularCoordinates);
-
- compute_fMi();
-
- return status;
-}
-
-/*!
- This method enables to initialise the pose between the object and the reference frame,
- in order to position the object relative to the camera.
-
- Before using this method it is advised to set the articular coordinates of the robot.
-
- In other terms, set the world to object transformation
- \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
- with \f${\bf q}\f$ the robot joint position
-
- \param cMo_ : the desired pose of the camera.
-*/
-void
-vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
-{
- vpColVector stop(6);
- stop = 0;
- set_artVel(stop);
- set_velocity(stop);
- vpHomogeneousMatrix fMit[8];
- get_fMi(fMit);
- fMo = fMit[7] * cMo_;
-}
-
-#endif
diff --git a/src/robot/simulator-robot/vpSimulatorViper850.h b/src/robot/simulator-robot/vpSimulatorViper850.h
deleted file mode 100644
index 6cccdca..0000000
--- a/src/robot/simulator-robot/vpSimulatorViper850.h
+++ /dev/null
@@ -1,314 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorViper850.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class which provides a simulator for the robot Viper850.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpSimulatorViper850_HH
-#define vpSimulatorViper850_HH
-
-/*!
- \file vpSimulatorViper850.h
- \brief Class which provides a simulator for the robot Viper850..
-*/
-
-
-
-#include <visp/vpRobotWireFrameSimulator.h>
-#include <visp/vpViper850.h>
-#include <string>
-#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
-
-/*!
- \class vpSimulatorViper850
-
- \ingroup Viper RobotSimuWithVisu
-
- \brief Simulator of Irisa's Viper S850 robot named Viper850.
-
- Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
- Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
-
- \warning This class uses threading capabilities. Thus on Unix-like
- platforms, the libpthread third-party library need to be
- installed. On Windows, we use the native threading capabilities.
-
- This class allows to control the Viper850 arm robot in position
- and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- - in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame and rotations in the camera frame.
-
- All the translations are expressed in meters for positions and m/s
- for the velocities. Rotations are expressed in radians for the
- positions, and rad/s for the rotation velocities.
-
- The direct and inverse kinematics models are implemented in the
- vpViper850 class.
-
- To control the robot in position, you may set the controller
- to position control and then send the position to reach in a specific
- frame like here in the joint space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- vpColVector q(6);
- // Set a joint position
- q[0] = vpMath::rad(10); // Joint 1 position, in rad
- q[1] = 0.2; // Joint 2 position, in rad
- q[2] = 0.3; // Joint 3 position, in rad
- q[3] = M_PI/8; // Joint 4 position, in rad
- q[4] = M_PI/4; // Joint 5 position, in rad
- q[5] = M_PI; // Joint 6 position, in rad
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
- return 0;
-}
- \endcode
-
- The robot moves to the specified position with the default
- positioning velocity vpRobotViper850::defaultPositioningVelocity. The
- setPositioningVelocity() method allows to change the maximal
- velocity used to reach the desired position.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- vpColVector q(6);
- // Set q[i] with i in [0:5]
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
-
- // Set the max velocity to 40%
- robot.setPositioningVelocity(40);
-
- // Moves the robot in the joint space
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
- return 0;
-}
- \endcode
-
- To control the robot in velocity, you may set the controller to
- velocity control and then send the velocities. To end the velocity
- control and stop the robot you have to set the controller to the
- stop state. Here is an example of a velocity control in the joint
- space:
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpSimulatorViper850.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-
-int main()
-{
- vpSimulatorViper850 robot;
-
- vpColVector qvel(6);
- // Set a joint velocity
- qvel[0] = 0.1; // Joint 1 velocity in rad/s
- qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
- qvel[2] = 0; // Joint 3 velocity in rad/s
- qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
- qvel[4] = 0; // Joint 5 velocity in rad/s
- qvel[5] = 0; // Joint 6 velocity in rad/s
-
- // Initialize the controller to position control
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
-
- for ( ; ; ) {
- // Apply a velocity in the joint space
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
-
- // Compute new velocities qvel...
- }
-
- // Stop the robot
- robot.setRobotState(vpRobot::STATE_STOP);
-
- return 0;
-}
- \endcode
-
- It is also possible to measure the robot current position with
- getPosition() method and the robot current velocities with the getVelocity()
- method.
-
- For convenience, there is also the ability to read/write joint
- positions from a position file with readPosFile() and savePosFile()
- methods.
-
- To know how this class can be used to achieve a visual servoing simulation,
- you can follow the \ref tutorial-ibvs.
-
-*/
-
-
-class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, public vpViper850
-{
- public:
- static const double defaultPositioningVelocity;
-
- private:
- vpColVector q_prev_getdis;
- bool first_time_getdis;
-
- double positioningVelocity;
-
- vpColVector zeroPos;
- vpColVector reposPos;
-
- bool toolCustom;
- std::string arm_dir;
-
- public:
- vpSimulatorViper850();
- vpSimulatorViper850(bool display);
- virtual ~vpSimulatorViper850();
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height);
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I);
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
-
- void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
-
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
- double getPositioningVelocity (void){return positioningVelocity;}
-
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe);
- void get_cVe(vpVelocityTwistMatrix &cVe);
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
- bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
- void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
-
- void move(const char *filename) ;
-
- static bool readPosFile(const char *filename, vpColVector &q);
- static bool savePosFile(const char *filename, const vpColVector &q);
-
- void setCameraParameters(const vpCameraParameters &cam) ;
- void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
- void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
- void setPosition (const vpRobot::vpControlFrameType frame,
- const double pos1,
- const double pos2,
- const double pos3,
- const double pos4,
- const double pos5,
- const double pos6);
- void setPosition(const char *filename);
- void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
-
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
-
- void stopMotion();
-
-protected:
- void computeArticularVelocity();
- void compute_fMi();
- void findHighestPositioningSpeed(vpColVector &q);
- void getExternalImage(vpImage<vpRGBa> &I);
-
- inline void get_fMi(vpHomogeneousMatrix *fMit) {
-#if defined(_WIN32)
- WaitForSingleObject(mutex_fMi,INFINITE);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- ReleaseMutex(mutex_fMi);
-#elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- pthread_mutex_unlock (&mutex_fMi);
-#endif
- }
- void init();
- void initArms();
- void initDisplay();
- int isInJointLimit (void);
- bool singularityTest(const vpColVector q, vpMatrix &J);
- void updateArticularPosition();
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
-};
-
-#endif
-
-#endif
diff --git a/src/servo/vpAdaptiveGain.cpp b/src/servo/vpAdaptiveGain.cpp
deleted file mode 100644
index 6bc54f7..0000000
--- a/src/servo/vpAdaptiveGain.cpp
+++ /dev/null
@@ -1,366 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAdaptiveGain.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Adaptive gain.
- *
- * Authors:
- * Nicolas Mansard
- *
- *****************************************************************************/
-/*!
-\file vpAdaptiveGain.cpp
-*/
-
-/* --- VISP --- */
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpAdaptiveGain.h>
-
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
-const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
-const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
-
-/* -------------------------------------------------------------------------- */
-/* --- CONSTRUCTION --------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
- Basic constructor which initializes all the parameters with their default value:
- - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
-
-*/
-vpAdaptiveGain::vpAdaptiveGain ()
- :
- coeff_a (),
- coeff_b (),
- coeff_c (),
- lambda(1.)
-{
- vpDEBUG_TRACE (10, "# Entree constructeur par default.");
- this ->initFromVoid ();
-
- vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
- return;
-}
-
-/*!
- Constructor that initializes the gain as constant. In that case \f$\lambda(x) = c\f$.
-
- \param c : Value of the constant gain.
-*/
-vpAdaptiveGain::vpAdaptiveGain (double c)
- :
- coeff_a (),
- coeff_b (),
- coeff_c (),
- lambda(1.)
-{
- initFromConstant(c);
-}
-
-/*!
- Constructor that initializes the gain as adaptive.
-
- \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
-
-*/
-vpAdaptiveGain::vpAdaptiveGain (double gain_at_zero, double gain_at_infinity, double slope_at_zero)
- :
- coeff_a (),
- coeff_b (),
- coeff_c (),
- lambda(1.)
-{
- initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
-}
-
-/* -------------------------------------------------------------------------- */
-/* --- INIT ----------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
- Initializes the parameters to have a constant gain. In that case \f$\lambda(x) = c\f$.
-
- \param c : Value of the constant gain.
-*/
-void vpAdaptiveGain::initFromConstant (const double c)
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->coeff_a = 0;
- this ->coeff_b = 1;
- this ->coeff_c = c;
-
- vpDEBUG_TRACE (10, "# Sortie.");
- return;
-}
-
-
-/*!
- Initializes the parameters with the default value :
- - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
-*/
-void vpAdaptiveGain::initFromVoid (void)
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
- vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
- vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);
-
- vpDEBUG_TRACE (10, "# Sortie.");
- return;
-}
-
-
-/*!
- Set the parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ used to compute \f$\lambda(x)\f$.
-
- \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
-*/
-void vpAdaptiveGain::initStandard (const double gain_at_zero,
- const double gain_at_infinity,
- const double slope_at_zero)
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->coeff_a = gain_at_zero - gain_at_infinity;
- //if (0 == this ->coeff_a)
- if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
- {
- this ->coeff_b = 0;
- }
- else
- {
- this ->coeff_b = slope_at_zero / ( this ->coeff_a);
- }
- this ->coeff_c = gain_at_infinity;
-
- vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
- this ->coeff_a, this ->coeff_b, this ->coeff_c);
- return;
-}
-
-
-
-/* -------------------------------------------------------------------------- */
-/* --- MODIFICATOR ---------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
- Sets the internal parameters \f$a,b,c\f$ in order to obtain a constant gain equal to
- the gain in 0 set through the parameter \f$\lambda(0)\f$.
-
- \return It returns the value of the constant gain \f$\lambda(0)\f$.
-*/
-double vpAdaptiveGain::setConstant (void)
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- double res = this ->coeff_a + this ->coeff_c;
-
- this ->coeff_a = 0;
- this ->coeff_b = 1;
- this ->coeff_c = res;
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
- return res;
-}
-
-/* -------------------------------------------------------------------------- */
-/* --- VALEUR --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-/*!
- Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
-
- \f[\lambda(x) = a * exp(-b*x) + c\f]
-
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
-
- \return It returns the value of the computed gain.
-*/
-double vpAdaptiveGain::value_const (const double x) const
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- double res = this ->coeff_a * exp (- this ->coeff_b * x) + this ->coeff_c;
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
- return res;
-}
-
-/*!
- Gets the value of the gain at infinity (ie the value of \f$ \lambda(\infty) = c \f$).
- Similar to limitValue() except that here the value is not stored as a parameter of the class.
-
- \return It returns the value of the gain at infinity.
- */
-double vpAdaptiveGain::limitValue_const (void) const
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- double res = this ->coeff_c;
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
- return res;
-}
-
-/*!
- Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
-
- \f[\lambda(x) = a * exp(-b*x) + c\f]
-
- This value is stored as a parameter of the class.
-
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
-
- \return It returns the value of the computed gain.
- */
-double vpAdaptiveGain::value (const double x) const
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->lambda = this ->value_const (x);
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
- return lambda;
-}
-
-
-/*!
- Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$) and stores it
- as a parameter of the class.
-
- \return It returns the value of the gain at infinity.
- */
-double vpAdaptiveGain:: limitValue (void) const
-{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->lambda = this ->limitValue_const ();
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
- return lambda;
-}
-
-/* -------------------------------------------------------------------------- */
-/* --- ACCESSORS ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-
-
-// double vpAdaptiveGain::
-// getLastValue (void) const
-// {
-// return this ->lambda;
-// }
-
-/*!
- Operator that computes \f$\lambda(x)\f$.
-
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
-
- \return It returns the value of the computed gain.
-
- \sa value()
-*/
-double vpAdaptiveGain::operator() (const double x) const
-{
- return this ->value (x);
-}
-
-/*!
- Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$).
-
- \return It returns the value of the gain at infinity.
-
- \sa limitValue()
- */
-double vpAdaptiveGain::operator() (void) const
-{
- return this ->limitValue ();
-}
-
-/*!
- Operator which computes \f$\lambda({||x||}_{\infty})\f$.
-
- \param x : Input vector to consider.
-
- \return It returns the value of the computed gain.
-*/
-double vpAdaptiveGain::operator() (const vpColVector & x) const
-{
- return this ->value (x .infinityNorm());
-}
-
-
-// double operator() (double val_e) const;
-// double operator() (const CColVector & e) const;
-
-/* -------------------------------------------------------------------------- */
-/* --- OUTPUT --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-
-
-/*!
- Prints the adaptive gain parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$.
-
- \param os : The stream where to print the adaptive gain parameters.
- \param lambda : The adaptive gain containing the parameters to print.
-*/
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda)
-{
- os << "Zero= " << lambda .coeff_a + lambda .coeff_c
- << "\tInf= " << lambda .coeff_c
- << "\tDeriv= " << lambda .coeff_a * lambda .coeff_b;
-
- return os;
-}
diff --git a/src/servo/vpAdaptiveGain.h b/src/servo/vpAdaptiveGain.h
deleted file mode 100644
index 766cc8d..0000000
--- a/src/servo/vpAdaptiveGain.h
+++ /dev/null
@@ -1,175 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAdaptiveGain.h 4641 2014-02-05 12:42:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Adaptive gain.
- *
- * Authors:
- * Nicolas Mansard
- *
- *****************************************************************************/
-/*!
-\file vpAdaptiveGain.h
-\brief Adaptive gain
-*/
-
-#ifndef __VP_ADAPTIVE_GAIN_H
-#define __VP_ADAPTIVE_GAIN_H
-
-#include <visp/vpConfig.h>
-#include <iostream>
-
-class vpColVector;
-/*!
- \class vpAdaptiveGain
-
- \ingroup VsTask
-
- \brief Adaptive gain computation.
-
- The formula used to compute the gain is the following :
-
- \f[ \lambda (x) = a * exp (-b*x) + c \f]
-
- where \f$ a \f$, \f$ b \f$ and \f$ c \f$ are constant parameters and \f$ x \f$ is the entry to consider.
-
- The parameters \f$a,b,c\f$ are not set directly. They are computed from three other parameters
- \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ that are more intuitive to tune:
- \f[ a = \lambda(0) - \lambda(\infty) \f]
- \f[ b = {\dot \lambda}(0) / a \f]
- \f[ c = \lambda(\infty) \f]
-
- where \f$ \lambda(0)\f$ represents the gain when \f$x=0\f$, \f$ \lambda(\infty)\f$ represents the gain when \f$x=\infty\f$
- and \f$ {\dot \lambda}(0)\f$ represents the slope of \f$\lambda(x)\f$ when \f$x=0\f$.
-
-*/
-
-class VISP_EXPORT vpAdaptiveGain
-{
-
-public: /* constantes */
-
- static const double DEFAULT_LAMBDA_ZERO;
- static const double DEFAULT_LAMBDA_INFINITY;
- static const double DEFAULT_LAMBDA_SLOPE;
-
-
-private: /* Attributs*/
- /* Coefficient de la fonction de calcul de lambda.
- * lambda (x) = a * exp (-b*x) + c. */
- double coeff_a;
- double coeff_b;
- double coeff_c;
-
- /* Derniere valeur calculee. */
- mutable double lambda;
-
-
-
-public: /* Methodes*/
-
- /* --- CONSTRUCTOR -------------------------------------------------------- */
-
- vpAdaptiveGain ();
- vpAdaptiveGain (double c);
- vpAdaptiveGain (double gain_at_zero,
- double gain_at_infinity,
- double slope_at_zero);
-
-
- /* --- INIT --------------------------------------------------------------- */
- void initFromConstant (double c);
- void initFromVoid (void);
- void initStandard (double gain_at_zero,
- double gain_at_infinity,
- double slope_at_zero);
-
-
- /* --- MODIFIORS ---------------------------------------------------------- */
- double setConstant (void);
-
-
- /* --- COMPUTE ------------------------------------------------------------ */
- /* \brief Calcule la valeur de lambda au point courrant.
- *
- * Determine la valeur du lambda adaptatif en fonction de la valeur
- * de la norme de la fonction de tache e par extrapolation exponentielle.
- * La fonction est : (en_infini - en_zero) * exp (-pente * ||e|| ) + en_infini.
- * On a bien :
- * - lambda(10^5) = en_infini ;
- * - lambda(0) = en_zero ;
- * - lambda(x ~ 0) ~ - pente * x + en_zero.
- * \param val_e: valeur de la norme de l'erreur.
- * \return: valeur de gain au point courrant.
- */
- double value_const (double x) const;
-
- /* \brief Calcule la valeur de lambda au point courrant et stockage du
- * resultat.
- *
- * La fonction calcule la valeur de lambda d'apres la valeur de la norme
- * de l'erreur, comme le fait la fonction valeur_const.
- * La fonction non constante stocke de plus le resultat dans this ->lambda.
- * \param val_e: valeur de la norme de l'erreur.
- * \return: valeur de gain au point courrant.
- */
- double value (double x) const;
-
- double limitValue_const (void) const;
-
- double limitValue (void) const;
-
- /* --- ACCESSORS ---------------------------------------------------------- */
-
- /*!
- Gets the last adaptive gain value which was stored in the class.
-
- \return It returns the last adaptive gain value which was stored in the class.
- */
- inline double getLastValue (void) const {return this ->lambda;}
-
- double operator() (double x) const;
-
- /* \brief Lance la fonction valeur avec la norme INFINIE du vecteur. */
- double operator() (const vpColVector & x) const;
-
- /* \brief Idem function limitValue. */
- double operator() (void) const;
-
-
- /* --- IOSTREAM ----------------------------------------------------------- */
-
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda);
-};
-
-#endif /* __VP_ADAPTIVE_GAIN_H */
diff --git a/src/servo/vpServo.cpp b/src/servo/vpServo.cpp
deleted file mode 100644
index 04ebe2c..0000000
--- a/src/servo/vpServo.cpp
+++ /dev/null
@@ -1,1595 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServo.cpp 5219 2015-01-28 10:29:21Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual servoing control law.
- *
- * Authors:
- * Eric Marchand
- * Nicolas Mansard
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpServo.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-/*!
- \file vpServo.cpp
- \brief Class required to compute the visual servoing control law
-*/
-
-
-/*!
- Default constructor that initializes the following settings:
- - No control law is specified. The user has to call setServo() to specify the control law.
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
-
-*/
-vpServo::vpServo()
- : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
- rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
- interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
- cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
- errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
- forceInteractionMatrixComputation(false), WpW(), I_WpW(), sv(), mu(4.)
-{
-}
-/*!
- Constructor that allows to choose the visual servoing control law.
- \param servo_type : Visual servoing control law.
-
- The other settings are the following:
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
-
- */
-vpServo::vpServo(vpServoType servo_type)
- : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type),
- rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
- interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
- cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
- errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
- forceInteractionMatrixComputation(false), WpW(), I_WpW(), sv(), mu(4)
-{
-}
-
-/*!
- Destructor.
-
- In fact, it does nothing. You have to call kill() to destroy the
- current and desired feature lists.
-
- \sa kill()
-*/
-// \exception vpServoException::notKilledProperly : Task was not killed
-// properly. That means that you should explitly call kill().
-
-vpServo::~vpServo()
-{
- if (taskWasKilled == false) {
- vpTRACE("--- Begin Warning Warning Warning Warning Warning ---");
- vpTRACE("--- You should explicitly call vpServo.kill()... ---");
- vpTRACE("--- End Warning Warning Warning Warning Warning ---");
- // throw(vpServoException(vpServoException::notKilledProperly,
- // "Task was not killed properly"));
- }
-}
-
-
-/*!
- Initialize the servo with the following settings:
-
- - No control law is specified. The user has to call setServo() to specify the control law.
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
-
-
-*/
-void vpServo::init()
-{
- // type of visual servoing
- servoType = vpServo::NONE ;
-
- // Twist transformation matrix
- init_cVe = false ;
- init_cVf = false ;
- init_fVe = false ;
- // Jacobians
- init_eJe = false ;
- init_fJe = false ;
-
- dim_task = 0 ;
-
- featureList.clear() ;
- desiredFeatureList.clear() ;
- featureSelectionList.clear();
-
- signInteractionMatrix = 1 ;
-
- interactionMatrixType = DESIRED ;
- inversionType=PSEUDO_INVERSE ;
-
- interactionMatrixComputed = false ;
- errorComputed = false ;
-
- taskWasKilled = false;
-
- forceInteractionMatrixComputation = false;
-
- rankJ1 = 0;
-}
-
-/*!
- Task destruction. Kill the current and desired visual feature lists.
-
- It is mendatory to call explicitly this function to avoid potential
- memory leaks.
-
- \code
- vpServo task ;
- vpFeatureThetaU s;
- ...
- task.addFeature(s); // Add current ThetaU feature
-
- task.kill(); // A call to kill() is requested here
- \endcode
-
-*/
-void vpServo::kill()
-{
- if (taskWasKilled == false) {
- // kill the current and desired feature lists
-
- // current list
- for(std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
- if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
- delete (*it) ;
- (*it) = NULL ;
- }
- }
- //desired list
- for(std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
- if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
- delete (*it) ;
- (*it) = NULL ;
- }
- }
-
- featureList.clear() ;
- desiredFeatureList.clear() ;
- taskWasKilled = true;
- }
-}
-
-/*!
- Set the visual servoing control law.
- \param servo_type : Control law that will be considered.
- See vpServo::vpServoType to see the possible values.
- */
-void vpServo::setServo(const vpServoType &servo_type)
-{
- this->servoType = servo_type ;
-
- if ((servoType==EYEINHAND_CAMERA) ||(servoType==EYEINHAND_L_cVe_eJe))
- signInteractionMatrix = 1 ;
- else
- signInteractionMatrix = -1 ;
-
- // when the control is directly compute in the camera frame
- // we relieve the end-user to initialize cVa and aJe
- if (servoType==EYEINHAND_CAMERA)
- {
- vpVelocityTwistMatrix _cVe ; set_cVe(_cVe) ;
-
- vpMatrix _eJe ;
- _eJe.eye(6) ;
- set_eJe(_eJe) ;
- };
-}
-
-
-/*!
-
- Prints on \e os stream information about the task:
-
- \param displayLevel : Indicates which are the task informations to print. See vpServo::vpServoPrintType for more details.
-
- \param os : Output stream.
-*/
-void
- vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
-{
- switch (displayLevel)
- {
- case vpServo::ALL:
- {
- os << "Visual servoing task: " <<std::endl ;
-
- os << "Type of control law " <<std::endl ;
- switch( servoType )
- {
- case NONE :
- os << "Type of task have not been chosen yet ! " << std::endl ;
- break ;
- case EYEINHAND_CAMERA :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the camera frame " << std::endl ;
- break ;
- case EYEINHAND_L_cVe_eJe :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the articular frame " << std::endl ;
- break ;
- case EYETOHAND_L_cVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
- break ;
- }
-
- os << "List of visual features : s" <<std::endl ;
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<vpBasicFeature *>::const_iterator it_s_star;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- os << "" ;
- (*it_s)->print( (*it_select) ) ;
- }
-
- os << "List of desired visual features : s*" <<std::endl ;
- for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
- {
- os << "" ;
- (*it_s_star)->print( (*it_select) ) ;
- }
-
- os <<"Interaction Matrix Ls "<<std::endl ;
- if (interactionMatrixComputed)
- {
- os << L << std::endl;
- }
- else
- {
- os << "not yet computed "<<std::endl ;
- }
-
- os <<"Error vector (s-s*) "<<std::endl ;
- if (errorComputed)
- {
- os << error.t() << std::endl;
- }
- else
- {
- os << "not yet computed "<<std::endl ;
- }
-
- os << "Gain : " << lambda <<std::endl ;
-
- break;
- }
-
- case vpServo::CONTROLLER:
- {
- os << "Type of control law " <<std::endl ;
- switch( servoType )
- {
- case NONE :
- os << "Type of task have not been chosen yet ! " << std::endl ;
- break ;
- case EYEINHAND_CAMERA :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the camera frame " << std::endl ;
- break ;
- case EYEINHAND_L_cVe_eJe :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the articular frame " << std::endl ;
- break ;
- case EYETOHAND_L_cVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
- break ;
- }
- break;
- }
-
- case vpServo::FEATURE_CURRENT:
- {
- os << "List of visual features : s" <<std::endl ;
-
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- os << "" ;
- (*it_s)->print( (*it_select) ) ;
- }
- break;
- }
- case vpServo::FEATURE_DESIRED:
- {
- os << "List of desired visual features : s*" <<std::endl ;
-
- std::list<vpBasicFeature *>::const_iterator it_s_star;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
- {
- os << "" ;
- (*it_s_star)->print( (*it_select) ) ;
- }
- break;
- }
- case vpServo::GAIN:
- {
- os << "Gain : " << lambda <<std::endl ;
- break;
- }
- case vpServo::INTERACTION_MATRIX:
- {
- os <<"Interaction Matrix Ls "<<std::endl ;
- if (interactionMatrixComputed) {
- os << L << std::endl;
- }
- else {
- os << "not yet computed "<<std::endl ;
- }
- break;
- }
-
- case vpServo::ERROR_VECTOR:
- case vpServo::MINIMUM:
-
- {
- os <<"Error vector (s-s*) "<<std::endl ;
- if (errorComputed) {
- os << error.t() << std::endl;
- }
- else {
- os << "not yet computed "<<std::endl ;
- }
-
- break;
- }
- }
-}
-
-/*!
- Add a new set of 2 features \f$\bf s\f$ and \f${\bf s}^*\f$ in the task.
-
- \param s_cur : Current visual feature denoted \f$\bf s\f$.
- \param s_star : Desired visual feature denoted \f${\bf s}^*\f$.
- \param select : Feature selector. By default all the features in \e s and \e s_star are used,
- but is is possible to specify which one is used in case of multiple features.
-
- The following sample code explain how to use this method to add a visual feature point \f$(x,y)\f$:
- \code
- vpFeaturePoint s, s_star;
- ...
- vpServo task;
- task.addFeature(s, s_star);
- \endcode
-
- For example to use only the \f$x\f$ visual feature, the previous code becomes:
- \code
- vpFeaturePoint s, s_star;
- ...
- vpServo task;
- task.addFeature(s, s_star, vpFeaturePoint::selectX());
- \endcode
-
- */
-void vpServo::addFeature(vpBasicFeature& s_cur, vpBasicFeature &s_star, unsigned int select)
-{
- featureList.push_back( &s_cur );
- desiredFeatureList.push_back( &s_star );
- featureSelectionList.push_back( select );
-}
-
-/*!
- Add a new features \f$\bf s\f$ in the task. The desired visual feature denoted \f${\bf s}^*\f$ is equal to zero.
-
- \param s_cur : Current visual feature denoted \f$\bf s\f$.
- \param select : Feature selector. By default all the features in \e s are used,
- but is is possible to specify which one is used in case of multiple features.
-
- The following sample code explain how to use this method to add a \f$\theta {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature:
- \code
- vpFeatureThetaU s(vpFeatureThetaU::cRcd);
- ...
- vpServo task;
- task.addFeature(s);
- \endcode
-
- For example to use only the \f$\theta u_x\f$ feature, the previous code becomes:
- \code
- vpFeatureThetaU s(vpFeatureThetaU::cRcd);
- ...
- vpServo task;
- task.addFeature(s, vpFeatureThetaU::selectTUx);
- \endcode
- */
-void vpServo::addFeature(vpBasicFeature& s_cur, unsigned int select)
-{
- featureList.push_back( &s_cur );
-
- // in fact we have a problem with s_star that is not defined
- // by the end user.
-
- // If the same user want to compute the interaction at the desired position
- // this "virtual feature" must exist
-
- // a solution is then to duplicate the current feature (s* = s)
- // and reinitialized s* to 0
-
- // it remains the deallocation issue therefore a flag that stipulates that
- // the feature has been allocated in vpServo is set
-
- // vpServo must deallocate the memory (see ~vpServo and kill() )
-
- vpBasicFeature *s_star ;
- s_star = s_cur.duplicate() ;
-
- s_star->init() ;
- s_star->setDeallocate(vpBasicFeature::vpServo) ;
-
- desiredFeatureList.push_back( s_star );
- featureSelectionList.push_back( select );
-}
-
-//! Return the task dimension.
-unsigned int vpServo::getDimension() const
-{
- unsigned int dim = 0 ;
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- dim += (*it_s)->getDimension(*it_select) ;
- }
-
- return dim;
-}
-
-void vpServo::setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrix_type,
- const vpServoInversionType &interactionMatrixInversion)
-{
- this->interactionMatrixType = interactionMatrix_type ;
- this->inversionType = interactionMatrixInversion ;
-}
-
-
-static void computeInteractionMatrixFromList (const std::list<vpBasicFeature *> & featureList,
- const std::list<unsigned int> & featureSelectionList,
- vpMatrix & L)
-{
- if (featureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
- }
-
- /* The matrix dimension is not known before the affectation loop.
- * It thus should be allocated on the flight, in the loop.
- * The first assumption is that the size has not changed. A double
- * reallocation (realloc(dim*2)) is done if necessary. In particular,
- * [log_2(dim)+1] reallocations are done for the first matrix computation.
- * If the allocated size is too large, a correction is done after the loop.
- * The algorithmic cost is linear in affectation, logarithmic in allocation
- * numbers and linear in allocation size.
- */
-
- /* First assumption: matrix dimensions have not changed. If 0, they are
- * initialized to dim 1.*/
- unsigned int rowL = L.getRows();
- const unsigned int colL = 6;
- if (0 == rowL) {
- rowL = 1;
- L .resize(rowL, colL);
- }
-
- /* vectTmp is used to store the return values of functions get_s() and
- * error(). */
- vpMatrix matrixTmp;
- unsigned int rowMatrixTmp, colMatrixTmp;
-
- /* The cursor are the number of the next case of the vector array to
- * be affected. A memory reallocation should be done when cursor
- * is out of the vector-array range.*/
- unsigned int cursorL = 0;
-
- std::list<vpBasicFeature *>::const_iterator it;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select)
- {
- /* Get s. */
- matrixTmp = (*it)->interaction( *it_select );
- rowMatrixTmp = matrixTmp .getRows();
- colMatrixTmp = matrixTmp .getCols();
-
- /* Check the matrix L size, and realloc if needed. */
- while (rowMatrixTmp + cursorL > rowL) {
- rowL *= 2;
- L.resize (rowL,colL,false);
- vpDEBUG_TRACE(15,"Realloc!");
- }
-
- /* Copy the temporarily matrix into L. */
- for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
- for (unsigned int j = 0; j < colMatrixTmp; ++j) {
- L[cursorL][j] = matrixTmp[k][j];
- }
- }
- }
-
- L.resize (cursorL,colL,false);
-
- return ;
-}
-
-
-/*!
-
- Compute and return the interaction matrix related to the set of visual features.
-
- \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the control law specified using setServo().
-*/
-vpMatrix vpServo::computeInteractionMatrix()
-{
- try {
-
- switch (interactionMatrixType)
- {
- case CURRENT:
- {
- try
- {
- computeInteractionMatrixFromList(this ->featureList,
- this ->featureSelectionList,
- L);
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
- }
-
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
- break ;
- case DESIRED:
- {
- try
- {
- if (interactionMatrixComputed == false || forceInteractionMatrixComputation == true)
- {
- computeInteractionMatrixFromList(this ->desiredFeatureList,
- this ->featureSelectionList, L);
-
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
- }
-
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- }
- break ;
- case MEAN:
- {
- vpMatrix Lstar (L.getRows(), L.getCols());
- try
- {
- computeInteractionMatrixFromList(this ->featureList,
- this ->featureSelectionList, L);
- computeInteractionMatrixFromList(this ->desiredFeatureList,
- this ->featureSelectionList, Lstar);
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- L = (L+Lstar)/2;
-
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
- }
- break ;
- case USER_DEFINED:
- // dim_task = L.getRows() ;
- interactionMatrixComputed = false ;
- break;
- }
-
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- return L ;
-}
-
-/*!
-
- Compute the error \f$\bf e =(s - s^*)\f$ between the current set of visual
- features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
-
- \return The error vector \f$\bf e\f$.
-
-*/
-vpColVector vpServo::computeError()
-{
- if (featureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
- }
- if (desiredFeatureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
- }
-
- try {
- vpBasicFeature *current_s ;
- vpBasicFeature *desired_s ;
-
- /* The vector dimensions are not known before the affectation loop.
- * They thus should be allocated on the flight, in the loop.
- * The first assumption is that the size has not changed. A double
- * reallocation (realloc(dim*2)) is done if necessary. In particular,
- * [log_2(dim)+1] reallocations are done for the first error computation.
- * If the allocated size is too large, a correction is done after the loop.
- * The algorithmic cost is linear in affectation, logarithmic in allocation
- * numbers and linear in allocation size.
- * No assumptions are made concerning size of each vector: they are
- * not said equal, and could be different.
- */
-
- /* First assumption: vector dimensions have not changed. If 0, they are
- * initialized to dim 1.*/
- unsigned int dimError = error .getRows();
- unsigned int dimS = s .getRows();
- unsigned int dimSStar = sStar .getRows();
- if (0 == dimError) { dimError = 1; error .resize(dimError);}
- if (0 == dimS) { dimS = 1; s .resize(dimS);}
- if (0 == dimSStar) { dimSStar = 1; sStar .resize(dimSStar);}
-
- /* vectTmp is used to store the return values of functions get_s() and
- * error(). */
- vpColVector vectTmp;
- unsigned int dimVectTmp;
-
- /* The cursor are the number of the next case of the vector array to
- * be affected. A memory reallocation should be done when cursor
- * is out of the vector-array range.*/
- unsigned int cursorS = 0;
- unsigned int cursorSStar = 0;
- unsigned int cursorError = 0;
-
- /* For each cell of the list, copy value of s, s_star and error. */
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<vpBasicFeature *>::const_iterator it_s_star;
- std::list<unsigned int>::const_iterator it_select;
-
- for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
- it_s != featureList.end();
- ++it_s, ++it_s_star, ++it_select)
- {
- current_s = (*it_s);
- desired_s = (*it_s_star);
- unsigned int select = (*it_select);
-
- /* Get s, and store it in the s vector. */
- vectTmp = current_s->get_s(select);
- dimVectTmp = vectTmp .getRows();
- while (dimVectTmp + cursorS > dimS)
- { dimS *= 2; s .resize (dimS,false); vpDEBUG_TRACE(15,"Realloc!"); }
- for (unsigned int k = 0; k < dimVectTmp; ++k) { s[cursorS++] = vectTmp[k]; }
-
- /* Get s_star, and store it in the s vector. */
- vectTmp = desired_s->get_s(select);
- dimVectTmp = vectTmp .getRows();
- while (dimVectTmp + cursorSStar > dimSStar) {
- dimSStar *= 2;
- sStar.resize (dimSStar,false);
- }
- for (unsigned int k = 0; k < dimVectTmp; ++k) {
- sStar[cursorSStar++] = vectTmp[k];
- }
-
- /* Get error, and store it in the s vector. */
- vectTmp = current_s->error(*desired_s, select) ;
- dimVectTmp = vectTmp .getRows();
- while (dimVectTmp + cursorError > dimError) {
- dimError *= 2;
- error.resize (dimError,false);
- }
- for (unsigned int k = 0; k < dimVectTmp; ++k) {
- error[cursorError++] = vectTmp[k];
- }
- }
-
- /* If too much memory has been allocated, realloc. */
- s .resize(cursorS,false);
- sStar .resize(cursorSStar,false);
- error .resize(cursorError,false);
-
- /* Final modifications. */
- dim_task = error.getRows() ;
- errorComputed = true ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- catch(...)
- {
- throw ;
- }
- return error ;
-}
-
-bool vpServo::testInitialization()
-{
- switch (servoType)
- {
- case NONE:
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
- case EYEINHAND_CAMERA:
- return true ;
- break ;
- case EYEINHAND_L_cVe_eJe:
- case EYETOHAND_L_cVe_eJe:
- if (!init_cVe) vpERROR_TRACE("cVe not initialized") ;
- if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
- return (init_cVe && init_eJe) ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
- if (!init_fVe) vpERROR_TRACE("fVe not initialized") ;
- if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
- return (init_cVf && init_fVe && init_eJe) ;
- break ;
-
- case EYETOHAND_L_cVf_fJe :
- if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
- if (!init_fJe) vpERROR_TRACE("fJe not initialized") ;
- return (init_cVf && init_fJe) ;
- break ;
- }
-
- return false ;
-}
-bool vpServo::testUpdated()
-{
- switch (servoType)
- {
- case NONE:
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
- case EYEINHAND_CAMERA:
- return true ;
- case EYEINHAND_L_cVe_eJe:
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_eJe) ;
- break ;
- case EYETOHAND_L_cVe_eJe:
- if (!init_cVe) vpERROR_TRACE("cVe not updated") ;
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_cVe && init_eJe) ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- if (!init_fVe) vpERROR_TRACE("fVe not updated") ;
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_fVe && init_eJe) ;
- break ;
-
- case EYETOHAND_L_cVf_fJe :
- if (!init_fJe) vpERROR_TRACE("fJe not updated") ;
- return (init_fJe) ;
- break ;
- }
-
- return false ;
-}
-/*!
-
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available. The \ref tutorial-ibvs and \ref tutorial-boost-vs
- are also useful to illustrate the usage of this function.
-
- The general form of the control law is the following:
-
- \f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e}
- \f]
-
- where :
- - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
- - \f$\bf e = (s-s^*)\f$ is the error to regulate.
-
- To ensure continuous sequencing the computeControlLaw(double) function can be used. It will ensure that
- the velocities that are computed are continuous.
-*/
-vpColVector vpServo::computeControlLaw()
-{
- static int iteration =0;
-
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
-
- if (iteration==0)
- {
- if (testInitialization() == false) {
- vpERROR_TRACE("All the matrices are not correctly initialized") ;
- throw(vpServoException(vpServoException::servoError,
- "Cannot compute control law "
- "All the matrices are not correctly"
- "initialized")) ;
- }
- }
- if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
- }
-
- // test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
- case EYEINHAND_CAMERA:
- case EYEINHAND_L_cVe_eJe:
- case EYETOHAND_L_cVe_eJe:
-
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
- }
-
- computeInteractionMatrix() ;
- computeError() ;
-
- // compute task Jacobian
- J1 = L*cVa*aJe ;
-
- // handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
-
- // pseudo inverse of the task Jacobian
- // and rank of the task Jacobian
- // the image of J1 is also computed to allows the computation
- // of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
-
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
-
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
-
- if (rankJ1 == J1.getCols())
- {
- /* if no degrees of freedom remains (rank J1 = ndof)
- WpW = I, multiply by WpW is useless
- */
- e1 = J1p*error ;// primary task
-
- WpW.resize(J1.getCols(), J1.getCols()) ;
- WpW.setIdentity() ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
- // image of J1 is computed to allows the computation
- // of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
- imageComputed = true ;
- }
- WpW = imJ1t*imJ1t.t() ;
-
-#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
-
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
-#endif
- e1 = WpW*J1p*error ;
- }
- e = - lambda(e1) * e1 ;
-
- vpMatrix I ;
-
- I.resize(J1.getCols(),J1.getCols()) ;
- I.setIdentity() ;
-
- I_WpW = (I - WpW) ;
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("Caught a matrix related error") ;
- std::cout << me << std::endl ;
- throw me;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw me ;
- }
-
- iteration++ ;
- return e ;
-}
-
-/*!
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available. The \ref tutorial-boost-vs is also useful to
- illustrate the usage of this function.
-
- To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
- the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
- continuous velocities by avoiding abrupt changes in the command.
-
- The form of the control law considered here is the following:
-
- \f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t)
- \f]
-
- where :
- - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
- - \f$\bf e = (s-s^*)\f$ is the error to regulate.
- - \f$t\f$ is the time given as parameter of this method.
- - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
- - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
- This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
-
- \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
-*/
-vpColVector vpServo::computeControlLaw(double t)
-{
- static int iteration =0;
- //static vpColVector e1_initial;
-
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
-
- if (iteration==0)
- {
- if (testInitialization() == false) {
- vpERROR_TRACE("All the matrices are not correctly initialized") ;
- throw(vpServoException(vpServoException::servoError,
- "Cannot compute control law "
- "All the matrices are not correctly"
- "initialized")) ;
- }
- }
- if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
- }
-
- // test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
- case EYEINHAND_CAMERA:
- case EYEINHAND_L_cVe_eJe:
- case EYETOHAND_L_cVe_eJe:
-
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
- }
-
- computeInteractionMatrix() ;
- computeError() ;
-
- // compute task Jacobian
- J1 = L*cVa*aJe ;
-
- // handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
-
- // pseudo inverse of the task Jacobian
- // and rank of the task Jacobian
- // the image of J1 is also computed to allows the computation
- // of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
-
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
-
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
-
- if (rankJ1 == J1.getCols())
- {
- /* if no degrees of freedom remains (rank J1 = ndof)
- WpW = I, multiply by WpW is useless
- */
- e1 = J1p*error ;// primary task
-
- WpW.resize(J1.getCols(), J1.getCols()) ;
- WpW.setIdentity() ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
- // image of J1 is computed to allows the computation
- // of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
- imageComputed = true ;
- }
- WpW = imJ1t*imJ1t.t() ;
-
-#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
-
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
-#endif
- e1 = WpW*J1p*error ;
- }
-
- // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
- if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
- e1_initial = e1;
- }
- // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
- if (e1_initial.getRows() != e1.getRows())
- e1_initial = e1;
-
- e = - lambda(e1) * e1 + lambda(e1) * e1_initial*exp(-mu*t);
-
- vpMatrix I ;
-
- I.resize(J1.getCols(), J1.getCols()) ;
- I.setIdentity() ;
-
- I_WpW = (I - WpW) ;
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("Caught a matrix related error") ;
- std::cout << me << std::endl ;
- throw me;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw me ;
- }
-
- iteration++ ;
- return e ;
-}
-
-/*!
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available.
-
- To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
- the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
- continuous velocities by avoiding abrupt changes in the command.
-
- The form of the control law considered here is the following:
-
- \f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu t)
- \f]
-
- where :
- - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
- - \f$\bf e = (s-s^*)\f$ is the error to regulate.
- - \f$t\f$ is the time given as parameter of this method.
- - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
- - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
- This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
-
- \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
- \param e_dot_init : Initial value of \f${\bf \dot e}(0)\f$.
-*/
-vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init)
-{
- static int iteration =0;
-
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
-
- if (iteration==0)
- {
- if (testInitialization() == false) {
- vpERROR_TRACE("All the matrices are not correctly initialized") ;
- throw(vpServoException(vpServoException::servoError,
- "Cannot compute control law "
- "All the matrices are not correctly"
- "initialized")) ;
- }
- }
- if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
- }
-
- // test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
- case EYEINHAND_CAMERA:
- case EYEINHAND_L_cVe_eJe:
- case EYETOHAND_L_cVe_eJe:
-
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
- }
-
- computeInteractionMatrix() ;
- computeError() ;
-
- // compute task Jacobian
- J1 = L*cVa*aJe ;
-
- // handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
-
- // pseudo inverse of the task Jacobian
- // and rank of the task Jacobian
- // the image of J1 is also computed to allows the computation
- // of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
-
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
-
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
-
- if (rankJ1 == J1.getCols())
- {
- /* if no degrees of freedom remains (rank J1 = ndof)
- WpW = I, multiply by WpW is useless
- */
- e1 = J1p*error ;// primary task
-
- WpW.resize(J1.getCols(), J1.getCols()) ;
- WpW.setIdentity() ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
- // image of J1 is computed to allows the computation
- // of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
- imageComputed = true ;
- }
- WpW = imJ1t*imJ1t.t() ;
-
-#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
-
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
-#endif
- e1 = WpW*J1p*error ;
- }
-
- // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
- if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
- e1_initial = e1;
- }
- // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
- if (e1_initial.getRows() != e1.getRows())
- e1_initial = e1;
-
- e = - lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial)*exp(-mu*t);
-
- vpMatrix I ;
-
- I.resize(J1.getCols(), J1.getCols()) ;
- I.setIdentity() ;
-
- I_WpW = (I - WpW) ;
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("Caught a matrix related error") ;
- std::cout << me << std::endl ;
- throw me;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw me ;
- }
-
- iteration++ ;
- return e ;
-}
-
-
-/*!
- Compute and return the secondary task vector according to the projection operator \f${\bf I-W^+W}\f$. For more details,
- see equation(7) in the paper \cite Marchand05b.
-
- \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
-
- \return The secondary task vector: \f[
- ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
- \f]
-
- Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
- \f[
- -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
- \f]
-
- \warning computeControlLaw() must be call prior to this function since it updates the projection operator \f$ \bf W^+W \f$.
-
- The following sample code shows how to use this method:
- \code
- vpColVector v; // Velocity applied to the robot
- vpColVector de2dt;
- vpServo task;
- ...
- v = task.computeControlLaw(); // Compute the primary task
- v += task.secondaryTask(de2dt) // Compute and add the secondary task
- \endcode
-
- \sa computeControlLaw()
-*/
-vpColVector vpServo::secondaryTask(const vpColVector &de2dt)
-{
- if (rankJ1 == J1.getCols())
- {
- vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
- throw(vpServoException(vpServoException::noDofFree,
- "no degree of freedom is free, cannot use secondary task")) ;
- }
- else
- {
- vpColVector sec ;
-#if 0
- // computed in computeControlLaw()
- vpMatrix I ;
-
- I.resize(J1.getCols(),J1.getCols()) ;
- I.setIdentity() ;
- I_WpW = (I - WpW) ;
-#endif
- // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
- sec = I_WpW*de2dt ;
-
- return sec ;
- }
-}
-
-/*!
- Compute and return the secondary task vector according to the projection operator \f${\bf I-W^+W}\f$. For more details,
- see equation(7) in the paper \cite Marchand05b.
-
- \param e2 : Value of the secondary task \f${\bf e}_2\f$.
- \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
-
- \return The secondary task vector:
- \f[
- -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
- \f]
-
- Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
- \f[
- -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
- \f]
-
- \warning computeControlLaw() must be call prior to this function since it updates the projection operator \f$ \bf W^+W \f$.
-
- The following sample code shows how to use this method:
- \code
- vpColVector v; // Velocity applied to the robot
- vpColVector e2;
- vpColVector de2dt;
- vpServo task;
- ...
- v = task.computeControlLaw(); // Compute the primary task
- v += task.secondaryTask(e2, de2dt) // Compute and add the secondary task
- \endcode
-
- \sa computeControlLaw()
-*/
-vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2dt)
-{
- if (rankJ1 == J1.getCols())
- {
- vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
- throw(vpServoException(vpServoException::noDofFree,
- "no degree of freedom is free, cannot use secondary task")) ;
- }
- else
- {
- vpColVector sec ;
-#if 0
- // computed in computeControlLaw()
- vpMatrix I ;
-
- I.resize(J1.getCols(),J1.getCols()) ;
- I.setIdentity() ;
-
- I_WpW = (I - WpW) ;
-#endif
-
- // To be coherent with the primary task the gain must be the same between
- // primary and secondary task.
- sec = -lambda(e1) *I_WpW*e2 + I_WpW *de2dt ;
-
- return sec ;
- }
-}
-
-/*!
- Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This operator is updated
- after a call of computeControlLaw().
-
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator
-\endcode
- \sa getWpW()
- */
-vpMatrix vpServo::getI_WpW() const
-{
- return I_WpW;
-}
-
-/*!
- Return the task jacobian \f$J\f$. The task jacobian is updated after a call of computeControlLaw().
-
- In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix J = task.getTaskJacobian(); // Get the task jacobian used to compute v
-\endcode
- \sa getTaskJacobianPseudoInverse(), getInteractionMatrix()
- */
-vpMatrix vpServo::getTaskJacobian() const
-{
- return J1;
-}
-/*!
- Return the pseudo inverse of the task jacobian \f$J\f$.
-
- In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
-
- The task jacobian and its pseudo inverse are updated after a call of computeControlLaw().
-
- \return Pseudo inverse \f${J}^{+}\f$ of the task jacobian.
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v
-\endcode
-
- \sa getTaskJacobian()
- */
-vpMatrix vpServo::getTaskJacobianPseudoInverse() const
-{
- return J1p;
-}
-/*!
- Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw().
-
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- unsigned int rank = task.getTaskRank(); // Get the rank of the task jacobian
-\endcode
- */
-unsigned int vpServo::getTaskRank() const
-{
- return rankJ1;
-}
-
-/*!
- Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is updated
- after a call of computeControlLaw().
-
- When the dimension of the task is equal to the number of degrees of freedom available
- \f${\bf W^+W = I}\f$.
-
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix WpW = task.getWpW(); // Get the projection operator
-\endcode
- \sa getI_WpW()
- */
-vpMatrix vpServo::getWpW() const
-{
- return WpW;
-}
diff --git a/src/servo/vpServo.h b/src/servo/vpServo.h
deleted file mode 100644
index 9c80a30..0000000
--- a/src/servo/vpServo.h
+++ /dev/null
@@ -1,568 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServo.h 5219 2015-01-28 10:29:21Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual servoing control law.
- *
- * Authors:
- * Eric Marchand
- * Nicolas Mansard
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpServo_H
-#define vpServo_H
-
-/*!
- \file vpServo.h
- \brief Class required to compute the visual servoing control law
-*/
-
-#include <list>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpServoException.h>
-#include <visp/vpAdaptiveGain.h>
-
-
-/*!
- \class vpServo
-
- \ingroup VsTask
- Class required to compute the visual servoing control law descbribed
- in \cite Chaumette06a and \cite Chaumette07a.
-
- \warning To avoid potential memory leaks, it is mendatory to call
- explicitly the kill() function to destroy the task. Otherwise, the
- destructor ~vpServo() launch an exception
- vpServoException::notKilledProperly.
-
- To learn how to use this class, we suggest first to follow the \ref tutorial-ibvs.
- The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs are also useful
- for advanced usage of this class.
-
- The example below shows how to build a position-based visual servo
- from 3D visual features \f$s=({^{c^*}}t_c,\theta u)\f$. In that
- case, we have \f$s^* = 0\f$. Let us denote \f$\theta u\f$ the angle/axis
- parametrization of the rotation \f${^{c^*}}R_c\f$. Moreover,\f$
- {^{c^*}}t_c\f$ and \f${^{c^*}}R_c\f$ represent respectively the
- translation and the rotation between the desired camera frame and
- the current one obtained by pose estimation (see vpPose class).
-
- \code
-#include <visp/vpColVector.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- // Creation of an homogeneous matrix that represent the displacement
- // the camera has to achieve to move from the desired camera frame
- // and the current one
- vpHomogeneousMatrix cdMc;
-
- // ... cdMc is here the result of a pose estimation
-
- // Creation of the current visual feature s = (c*_t_c, ThetaU)
- vpFeatureTranslation s_t(vpFeatureTranslation::cdMc);
- vpFeatureThetaU s_tu(vpFeatureThetaU::cdRc);
- // Set the initial values of the current visual feature s = (c*_t_c, ThetaU)
- s_t.buildFrom(cdMc);
- s_tu.buildFrom(cdMc);
-
- // Build the desired visual feature s* = (0,0)
- vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // Default initialization to zero
- vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc);// Default initialization to zero
-
- vpColVector v; // Camera velocity
- double error; // Task error
-
- // Creation of the visual servo task.
- vpServo task;
-
- // Visual servo task initialization
- // - Camera is monted on the robot end-effector and velocities are
- // computed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // - Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
- // - Set the contant gain to 1
- task.setLambda(1);
- // - Add current and desired translation feature
- task.addFeature(s_t, s_star_t);
- // - Add current and desired ThetaU feature for the rotation
- task.addFeature(s_tu, s_star_tu);
-
- // Visual servoing loop. The objective is here to update the visual
- // features s = (c*_t_c, ThetaU), compute the control law and apply
- // it to the robot
- do {
- // ... cdMc is here the result of a pose estimation
-
- // Update the current visual feature s
- s_t.buildFrom(cdMc); // Update translation visual feature
- s_tu.buildFrom(cdMc); // Update ThetaU visual feature
-
- v = task.computeControlLaw(); // Compute camera velocity skew
- error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2
- } while (error > 0.0001); // Stop the task when current and desired visual features are close
-
- // A call to kill() is requested here to destroy properly the current
- // and desired feature lists.
- task.kill();
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpServo
-{
- /*
- Choice of the visual servoing control law
- */
-public:
- typedef enum
- {
- NONE,
- /*!< No control law is specified. */
- EYEINHAND_CAMERA,
- /*!< Eye in hand visual servoing with the following control law
- \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f]
- where camera velocities are computed. */
- EYEINHAND_L_cVe_eJe,
- /*!< Eye in hand visual servoing with the following control law
- \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVe_eJe,
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVf_fVe_eJe,
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVf_fJe
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- } vpServoType;
-
- typedef enum
- {
- CURRENT,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_s \f$computed using the current features \f$\bf s\f$. */
- DESIRED,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired features \f${\bf s}^*\f$. */
- MEAN,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2 \f$. */
- USER_DEFINED
- /*!< In the control law (see vpServo::vpServoType), uses an interaction matrix set by the user. */
- } vpServoIteractionMatrixType;
-
- typedef enum
- {
- TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the transpose instead of the pseudo inverse. */
- PSEUDO_INVERSE /*!< In the control law (see vpServo::vpServoType), uses the pseudo inverse. */
- } vpServoInversionType;
-
- typedef enum
- {
- ALL, /*!< Print all the task information. */
- CONTROLLER, /*!< Print the type of controller law. */
- ERROR_VECTOR, /*!< Print the error vector \f$\bf e = (s-s^*)\f$. */
- FEATURE_CURRENT, /*!< Print the current features \f$\bf s\f$. */
- FEATURE_DESIRED, /*!< Print the desired features \f${\bf s}^*\f$. */
- GAIN, /*!< Print the gain \f$\lambda\f$. */
- INTERACTION_MATRIX, /*!< Print the interaction matrix. */
- MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */
- } vpServoPrintType;
-
-public:
- // default constructor
- vpServo();
- // constructor with Choice of the visual servoing control law
- vpServo(vpServoType servoType) ;
- // destructor
- virtual ~vpServo() ;
-
- // create a new ste of two visual features
- void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
- const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
- // create a new ste of two visual features
- void addFeature(vpBasicFeature& s,
- const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
-
- // compute the desired control law
- vpColVector computeControlLaw() ;
- // compute the desired control law
- vpColVector computeControlLaw(double t) ;
- vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
-
- // compute the error between the current set of visual features and
- // the desired set of visual features
- vpColVector computeError() ;
- // compute the interaction matrix related to the set of visual features
- vpMatrix computeInteractionMatrix() ;
-
- // Return the task dimension.
- unsigned int getDimension() const ;
- /*!
- Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual features
- \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
- The error vector is updated after a call of computeError() or computeControlLaw().
-\code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpColVector e = task.getError(); // Get the error vector
-\endcode
- */
- inline vpColVector getError() const
- {
- return error ;
- }
- /*
- Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
- The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
-
- \code
- vpServo task;
- ...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
- \endcode
- \sa getTaskJacobian()
- */
- inline vpMatrix getInteractionMatrix() const
- {
- return L;
- }
-
- vpMatrix getI_WpW() const;
- /*!
- Return the visual servo type.
- */
- inline vpServoType getServoType() const
- {
- return servoType;
- }
-
- vpMatrix getTaskJacobian() const;
- vpMatrix getTaskJacobianPseudoInverse() const;
- unsigned int getTaskRank() const;
-
- /*!
- Get task singular values.
-
- \return Singular values that relies on the task jacobian pseudo inverse.
- */
- inline vpColVector getTaskSingularValues() const
- {
- return sv;
- }
-
- vpMatrix getWpW() const;
-
- /*!
- Return the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
- */
- vpVelocityTwistMatrix get_cVe() const { return cVe; }
- /*!
- Return the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
- */
- vpVelocityTwistMatrix get_cVf() const { return cVf; }
- /*!
- Return the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
- */
- vpVelocityTwistMatrix get_fVe() const { return fVe; }
- /*!
- Return the robot jacobian expressed in the end-effector frame.
- */
- vpMatrix get_eJe() const { return eJe; }
- /*!
- Return the robot jacobian expressed in the robot fixed frame (also called world or base frame).
- */
- vpMatrix get_fJe() const { return fJe; }
-
- // destruction (memory deallocation if required)
- void kill() ;
-
- void print(const vpServo::vpServoPrintType display_level=ALL,
- std::ostream &os = std::cout) ;
-
- // Add a secondary task.
- vpColVector secondaryTask(const vpColVector &de2dt) ;
- // Add a secondary task.
- vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt) ;
-
- /*!
- Set a variable which enables to compute the interaction matrix at each iteration.
-
- When the interaction matrix is computed from the desired features \f${\bf s}^*\f$ which are in general constant,
- the interaction matrix \f${\widehat {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo loop.
- Sometimes, when the desired features are time dependent \f${{\bf s}(t)}^*\f$ or varying, the interaction matrix
- need to be computed at each iteration of the servo loop. This method allows to force the computation of
- \f${\widehat {\bf L}}\f$ in this particular case.
-
- \param force_computation: If true it forces the interaction matrix computation even if it is already done.
-
- */
- void setForceInteractionMatrixComputation(bool force_computation)
- {
- this->forceInteractionMatrixComputation = force_computation;
- }
-
- /*!
- Set the interaction matrix type (current, desired, mean or user defined) and how its inverse is computed.
- \param interactionMatrixType : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for
- more details.
- \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for
- more details.
- */
- void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
- const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
-
- /*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as constant.
-
- The usage of an adaptive gain allows to reduce the convergence time, see setLambda(const vpAdaptiveGain&).
-
- \param c : Constant gain. Values are in general between 0.1 and 1. Higher is the gain, higher are the velocities
- that may be applied to the robot.
- */
- void setLambda(double c)
- {
- lambda .initFromConstant (c) ;
- }
-
- /*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
- Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
-
- The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
-
- \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
-
- For more details on these parameters see vpAdaptiveGain class.
- */
- void setLambda(const double gain_at_zero,
- const double gain_at_infinity,
- const double slope_at_zero)
- {
- lambda .initStandard (gain_at_zero, gain_at_infinity, slope_at_zero) ;
- }
- /*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
- Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
-
- The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
- \sa vpAdaptiveGain
- */
- void setLambda(const vpAdaptiveGain& l){lambda=l;}
- /*!
- Set the value of the parameter \f$\mu\f$ used to ensure the continuity of the velocities
- computed using computeControlLaw(double).
-
- A recommended value is 4.
- */
- void setMu(double mu_){this->mu=mu_;}
- // Choice of the visual servoing control law
- void setServo(const vpServoType &servo_type) ;
-
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
- */
- void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_ ; init_cVe = true ; }
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
- */
- void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
- */
- void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_ ; init_cVf = true ; }
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
- */
- void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
- */
- void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_ ; init_fVe = true ; }
- /*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
- */
- void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
-
- /*!
- Set the robot jacobian expressed in the end-effector frame.
- */
- void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_ ; init_eJe = true ; }
- /*!
- Set the robot jacobian expressed in the robot fixed frame (also called world or base frame).
- */
- void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_ ; init_fJe = true ; }
-
- /*!
- Test if all the initialization are correct. If true, the control law can be computed.
- */
- bool testInitialization() ;
- /*!
- Test if all the update are correct. If true control law can be computed.
- */
- bool testUpdated() ;
-
- protected:
- //! Basic initialization.
- void init() ;
-
- public:
- //! Interaction matrix
- vpMatrix L ;
- //! Error \f$(s - s^*)\f$ between the current set of visual features
- //! \f$s\f$ and the desired set of visual features \f$s^*\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector error ;
- //! Task Jacobian \f$J_1 = L {^c}V_a {^a}J_e\f$.
- vpMatrix J1 ;
- //! Pseudo inverse \f${J_1}^{+}\f$ of the task Jacobian.
- vpMatrix J1p ;
-
- //! Current state of visual features \f$s\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector s ;
- //! Desired state of visual features \f$s^*\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector sStar ;
-
- //! Primary task \f$e_1 = {J_1}^{+}(s-s*)\f$
- vpColVector e1 ;
- //! Task \f$e = e_1 + (I-{J_1}^{+} J_1) e_2\f$
- vpColVector e ;
-
- //! Articular velocity
- vpColVector q_dot ;
- //! Camera velocity
- vpColVector v ;
-
- //! Chosen visual servoing control law
- vpServoType servoType;
-
- //! Rank of the task Jacobian
- unsigned int rankJ1 ;
-
- //! List of current visual features \f$\bf s\f$.
- std::list<vpBasicFeature *> featureList ;
- //! List of desired visual features \f$\bf s^*\f$.
- std::list<vpBasicFeature *> desiredFeatureList ;
- //! List of selection among visual features
- //! used for selection of a subset of each visual feature if required.
- std::list<unsigned int> featureSelectionList ;
-
- //! Gain used in the control law.
- vpAdaptiveGain lambda ;
-
- //! Sign of the interaction +/- 1 (1 for eye-in-hand, -1 for
- //! eye-to-hand configuration)
- int signInteractionMatrix ;
- //! Type of the interaction matrox (current, mean, desired, user)
- vpServoIteractionMatrixType interactionMatrixType ;
- //! Indicates if the transpose or the pseudo inverse of the
- //! interaction matrix should be used to compute the task.
- vpServoInversionType inversionType ;
-
- protected:
- /*
- Twist transformation matrix
- */
-
- //! Twist transformation matrix between Re and Rc.
- vpVelocityTwistMatrix cVe ;
- bool init_cVe ;
- //! Twist transformation matrix between Rf and Rc.
- vpVelocityTwistMatrix cVf ;
- bool init_cVf ;
- //! Twist transformation matrix between Re and Rf.
- vpVelocityTwistMatrix fVe ;
- bool init_fVe ;
-
- /*
- Jacobians
- */
-
- //! Jacobian expressed in the end-effector frame.
- vpMatrix eJe ;
- bool init_eJe ;
- //! Jacobian expressed in the robot reference frame.
- vpMatrix fJe ;
- bool init_fJe ;
-
- /*
- Task building
- */
-
- //! true if the error has been computed.
- bool errorComputed ;
- //! true if the interaction matrix has been computed.
- bool interactionMatrixComputed ;
- //! Dimension of the task updated during computeControlLaw().
- unsigned int dim_task ;
- //! Flag to indicate if the task was killed
- bool taskWasKilled;
- //! Force the interaction matrix computation even if it is already done.
- bool forceInteractionMatrixComputation;
-
- //! Projection operators \f$\bf WpW\f$.
- vpMatrix WpW ;
- //! Projection operators \f$\bf I-WpW\f$.
- vpMatrix I_WpW ;
-
- //! Singular values from the pseudo inverse.
- vpColVector sv ;
-
- double mu;
-
- vpColVector e1_initial;
-
-} ;
-
-#endif
diff --git a/src/servo/vpServoData.cpp b/src/servo/vpServoData.cpp
deleted file mode 100644
index 183bcc2..0000000
--- a/src/servo/vpServoData.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServoData.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Save data during the task execution.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpServoData.cpp
- \brief save data during the task execution
-*/
-
-// Servo
-#include <visp/vpServo.h>
-
-#include <visp/vpServoData.h>
-#include <visp/vpIoException.h>
-#include <visp/vpIoTools.h>
-
-void
-vpServoData::open(const char *directory)
-{
- try
- {
- if (vpIoTools::checkDirectory(directory) == false)
- vpIoTools::makeDirectory(directory);
-
- char s[FILENAME_MAX] ;
-
- sprintf(s,"%s/vel.dat",directory) ;
- velocityFile.open(s) ;
- sprintf(s,"%s/error.dat",directory) ;
- errorFile.open(s) ;
- sprintf(s,"%s/errornorm.dat",directory) ;
- errorNormFile.open(s) ;
- sprintf(s,"%s/s.dat",directory) ;
- sFile.open(s) ;
- sprintf(s,"%s/sStar.dat",directory) ;
- sStarFile.open(s) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-void vpServoData::setCmDeg()
-{
- cmDeg = true ;
-}
-void vpServoData::setMeterRad()
-{
- cmDeg = false ;
-}
-void vpServoData::save(const vpServo &task)
-{
- if (cmDeg==false) velocityFile << task.q_dot.t() ;
- else
- {
- for (unsigned int i=0 ; i < 3 ; i++)
- velocityFile << task.q_dot[i]*100 <<" " ;
- for (unsigned int i=4 ; i < 6 ; i++)
- velocityFile << vpMath::deg(task.q_dot[i]) <<" " ;
- velocityFile << std::endl ;
- }
- errorFile << ( task.getError() ).t() ;
- errorNormFile << ( task.getError() ).sumSquare() << std::endl ;
- vNormFile << task.q_dot.sumSquare() << std::endl ;
-
- sFile <<task.s.t() ;
- sStarFile << task.sStar.t();
-}
-
-
-
-void vpServoData::close()
-{
- velocityFile.close() ;
- errorFile.close() ;
- errorNormFile.close() ;
- sFile.close() ;
- sStarFile.close() ;
-}
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/servo/vpServoData.h b/src/servo/vpServoData.h
deleted file mode 100644
index 1f5ab8a..0000000
--- a/src/servo/vpServoData.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServoData.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Save data during the task execution.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpServoData_H
-#define vpServoData_H
-
-/*!
- \file vpServoData.h
- \brief save data during the task execution
-*/
-
-
-
-// Servo
-#include <visp/vpServo.h>
-
-#include <iostream>
-
-/*!
- \class vpServoData
- \ingroup VsTask
- \brief Save data during the task execution.
-*/
-class VISP_EXPORT vpServoData
-{
-
-private:
- char baseDirectory[FILENAME_MAX] ;
-
- std::ofstream velocityFile ;
- std::ofstream errorFile ;
- std::ofstream errorNormFile ;
- std::ofstream sFile ;
- std::ofstream sStarFile ;
- std::ofstream vNormFile ;
-
- //! flag to known if velocity should be output in cm and degrees (true)
- //! or in m/rad
- bool cmDeg ;
-
-
-
-public:
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- vpServoData(const vpServoData &sd)
- : velocityFile(), errorFile(), errorNormFile(), sFile(),
- sStarFile(), vNormFile(), cmDeg(false) {
- *this = sd;
- }
- vpServoData & operator=(const vpServoData &) {
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
-
- vpServoData() : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), vNormFile(), cmDeg(false) { ; }
- virtual ~vpServoData() { ; }
-
- //! velocity output in cm and deg
- void setCmDeg() ;
- //! velocity output in meter and deg (default)
- void setMeterRad() ;
-
- void save(const vpServo &task) ;
- void open(const char *baseDirectory) ;
- void close() ;
-
- void empty() ;
- void push() ;
- void display(vpImage<unsigned char> &I) ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/servo/vpServoDisplay.cpp b/src/servo/vpServoDisplay.cpp
deleted file mode 100644
index b0f3960..0000000
--- a/src/servo/vpServoDisplay.cpp
+++ /dev/null
@@ -1,154 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServoDisplay.cpp 4645 2014-02-05 17:44:06Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpServoDisplay.cpp
- \brief interface with the image for feature display
-*/
-
-// Servo
-#include <visp/vpServo.h>
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-
-//Color / image / display
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-#include <visp/vpDisplay.h>
-
-#include <visp/vpServoDisplay.h>
-
-#include <visp/vpBasicFeature.h>
-
-/*!
-
- Display the current and the desired features in the image I.
-
- \warning To effectively display the dot graphics a call to
- vpDisplay::flush() is needed.
-
- \param s : Visual servoing control law.
- \param cam : Camera parameters.
- \param I : Image on which features have to be displayed.
-
- \param currentColor : Color for the current features. If vpColor::none,
- current features display is turned off.
-
- \param desiredColor : Color for the desired features. If vpColor::none,
- desired features display is turned off.
-
- \param thickness : Thickness of the feature representation.
-
-*/
-void vpServoDisplay::display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- vpColor currentColor,
- vpColor desiredColor,
- unsigned int thickness)
-{
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<vpBasicFeature *>::const_iterator it_s_star;
-
- for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
- it_s != s.featureList.end();
- ++it_s, ++it_s_star)
- {
- if (desiredColor != vpColor::none) {
- // desired list
- (*it_s_star)->display(cam, I, desiredColor, thickness ) ;
- }
- if (currentColor != vpColor::none) {
- // current list
- (*it_s)->display(cam, I, currentColor, thickness ) ;
- }
- }
- // vpDisplay::flush(I) ;
-}
-
-/*!
-
- Display the current and the desired features in the image I.
-
- \warning To effectively display the dot graphics a call to
- vpDisplay::flush() is needed.
-
- \param s : Visual servoing control law.
- \param cam : Camera parameters.
- \param I : Color image on which features have to be displayed.
-
- \param currentColor : Color for the current features. If vpColor::none,
- current features display is turned off.
-
- \param desiredColor : Color for the desired features. If vpColor::none,
- desired features display is turned off.
-
- \param thickness : Thickness of the feature representation.
-
- */
-void
-vpServoDisplay::display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- vpColor currentColor,
- vpColor desiredColor,
- unsigned int thickness)
-{
- std::list<vpBasicFeature *>::const_iterator it_s;
- std::list<vpBasicFeature *>::const_iterator it_s_star;
-
- for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
- it_s != s.featureList.end();
- ++it_s, ++it_s_star)
- {
- if (desiredColor != vpColor::none) {
- // desired list
- (*it_s_star)->display(cam, I, desiredColor, thickness ) ;
- }
- if (currentColor != vpColor::none) {
- // current list
- (*it_s)->display(cam, I, currentColor, thickness ) ;
- }
- }
- // vpDisplay::flush(I) ;
-}
diff --git a/src/servo/vpServoDisplay.h b/src/servo/vpServoDisplay.h
deleted file mode 100644
index 130f843..0000000
--- a/src/servo/vpServoDisplay.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServoDisplay.h 4645 2014-02-05 17:44:06Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpServoDisplay_H
-#define vpServoDisplay_H
-
-/*!
- \file vpServoDisplay.h
- \brief interface with the image for feature display
-*/
-
-// Servo
-#include <visp/vpServo.h>
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-
-//Color / image / display
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-/*!
- \class vpServoDisplay
- \ingroup VsTask
- \brief Interface with the image for feature display.
-*/
-class VISP_EXPORT vpServoDisplay
-{
-public:
- static void display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- vpColor currentColor = vpColor::green,
- vpColor desiredColor = vpColor::red,
- unsigned int thickness=1) ;
- static void display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- vpColor currentColor = vpColor::green,
- vpColor desiredColor = vpColor::red,
- unsigned int thickness=1) ;
-} ;
-
-#endif
-
diff --git a/src/servo/vpServoException.h b/src/servo/vpServoException.h
deleted file mode 100644
index f74fc67..0000000
--- a/src/servo/vpServoException.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpServoException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception that can be emited by the vpServo class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef __vpServoException_H
-#define __vpServoException_H
-
-/*!
- \file vpServoException.h
- \brief error that can be emited by the vpServo class and its derivates
-*/
-
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpServoException
- \ingroup Exception
- \brief Error that can be emited by the vpServo class and its derivates.
- \author Eric Marchand (Eric.Marchand at irisa.fr) Irisa / Inria Rennes
- */
-class VISP_EXPORT vpServoException : public vpException
-{
- public:
- /*!
-
- \brief Lists the possible error than can be emmited 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,
- //! Task was not killed properly
- notKilledProperly,
- //! Other exception
- servoError
- } ;
-
- public:
- vpServoException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpServoException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpServoException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/simulator/coin-simulator/vpAR.cpp b/src/simulator/coin-simulator/vpAR.cpp
deleted file mode 100644
index 48698e2..0000000
--- a/src/simulator/coin-simulator/vpAR.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAR.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Use to display an image behind the internal view of the simulator
- * used for augmented reality application
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpAR.cpp
- \brief class used to display an image behind the internal view of the simulator. Used for augmented reality applications.
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-
-#include <visp/vpAR.h>
-#include <visp/vpTime.h>
-
-
-/* Objets OIV. */
-#include <Inventor/nodes/SoCone.h> /* Objet cone. */
-#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
-#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
-#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
-#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
-#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
-#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
-
-#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
-#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
-#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
-#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
-#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
-#include <Inventor/actions/SoWriteAction.h>
-
-
-
-/*!
- Basic Destructor that calls the kill() method of the vpSimulator class.
-*/
-vpAR::~vpAR()
-{
- kill() ;
-}
-
-
-/*!
- Initialisation of the internal view of the simulator.
-
- \param width : Width of the internal view.
- \param height : Height of the internal view.
- \param type : Type of background image ie gray scaled or color.
-*/
-void
-vpAR::initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type)
-{
-
- vpSimulator::initInternalViewer(width,height) ;
-
-
- // no image is loaded
- background = false ;
-
- if ( image_background != NULL)
- {
- free(image_background) ;
- image_background = NULL ;
- }
-
- typeImage = type;
- if (typeImage == grayImage)
- image_background =(GLubyte *)
- malloc(internal_width*internal_height*sizeof(GLubyte)) ;
- else
- image_background =(GLubyte *)
- malloc(3*internal_width*internal_height*sizeof(GLubyte)) ;
-
-}
-
-
-
-/*!
- Set the background image and turn it to deal with the frame of OpenGL.
-
- \param I : Gray scaled image for the background.
-*/
-// Grey pictures SetBackGroundImage
-void
-vpAR::setImage(vpImage<unsigned char> &I)
-{
-
- if ((internal_width != I.getWidth()) ||
- (internal_height != I.getHeight()))
- {
- vpERROR_TRACE("The image size is different from the view size ");
- throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
- }
-
-
- background = true ;
-
-
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- //le repere image open GL est en bas a gauche donc l'image serait inverse
- image_background[i*I.getWidth()+j] = I[I.getHeight()-i-1][j] ;
-
-}
-
-
-
-/*!
- Set the background image and turn it to deal with the frame of OpenGL.
-
- \param I : Color image for the background.
-*/
-// Color pictures SetBackGroundImage
-void
-vpAR::setImage(vpImage<vpRGBa> &I)
-{
-
- if ((internal_width != I.getWidth()) ||
- (internal_height != I.getHeight()))
- {
- vpERROR_TRACE("The image size is different from the view size ");
- throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
- }
-
-
- background = true ;
-
- unsigned int k =0 ;
- for (unsigned int i=0 ; i <I.getHeight() ; i++)
- {
- k=0;
- for (unsigned int j=0 ; j <I.getWidth() ; j++)
- //le repere image open GL est en bas a gauche donc l'image serait inverse
- {
- image_background[i*I.getWidth()*3+k+0]=I[I.getHeight()-i-1][j].R ;
- image_background[i*I.getWidth()*3+k+1]=I[I.getHeight()-i-1][j].G ;
- image_background[i*I.getWidth()*3+k+2]=I[I.getHeight()-i-1][j].B ;
- k+=3;
- }
- }
-
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/simulator/coin-simulator/vpAR.h b/src/simulator/coin-simulator/vpAR.h
deleted file mode 100644
index 47ca360..0000000
--- a/src/simulator/coin-simulator/vpAR.h
+++ /dev/null
@@ -1,164 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAR.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Use to display an image behind the internal view of the simulator
- * used for augmented reality application
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpAR.h
-
- \brief Class used to display an image behind the internal view of
- the simulator. Used for augmented reality applications.
-
- \warning The content of this file is only available if Coin3D and
- one of the GUI (SoWin, SoXT, SoQt) are installed.
-
-*/
-
-
-#ifndef vpAR_HH
-#define vpAR_HH
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-
-// visp
-#include <visp/vpDebug.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-
-#include <visp/vpSimulator.h>
-
-#include <visp/vpViewer.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpTime.h>
-
-/*!
- \class vpAR
-
- \ingroup RobotSimuWithViz
-
- \brief Implementation of an augmented reality viewer.
-
- This class can be used to display an image behind the internal view
- of the simulator used for augmented reality application.
-
- \warning This class is only available if Coin3D and one of the GUI
- (SoWin, SoXT, SoQt) are installed.
-
- The code below shows how to use the class.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpAR.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-static void *mainloopfunction(void *_simu)
-{
- vpAR *simu = (vpAR *)_simu ;
- simu->initMainApplication() ;
-
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo;
-
- //Your code to compute the pose cMo.
-
- //Set the image to use as background.
- simu->setImage(I) ;
- //Set the camera position thanks to the pose cMo computed before.
- simu->setCameraPosition(cMo) ;
-
- simu->closeMainApplication();
-}
-#endif
-
-int main()
-{
-#ifdef VISP_HAVE_COIN_AND_GUI
- vpAR simu;
- //Camera parameters.
- vpCameraParameters cam(600,600,160,120);
-
- //Initialize the internal view of the simulator.
- simu.initInternalViewer(640,480, vpSimulator::grayImage);
-
- vpTime::wait(300);
-
- //Load the cad model.
- simu.load("./4points.iv"); //4points.iv can be downloaded on the website with the image package
-
- //Initialize the internal camera parameters.
- simu.setInternalCameraParameters(cam);
-
- simu.initApplication(&mainloopfunction);
-
- simu.mainLoop();
-#endif
- return 0;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpAR : public vpSimulator
-{
-
-private:
-
- bool background;
-
- public:
-
-
- virtual ~vpAR() ;
- void initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type = grayImage) ;
- void setImage(vpImage<unsigned char> &I) ;
- void setImage(vpImage<vpRGBa> &I) ;
-
-} ;
-
-
-#endif
-#endif
diff --git a/src/simulator/coin-simulator/vpProjectionDisplay.cpp b/src/simulator/coin-simulator/vpProjectionDisplay.cpp
deleted file mode 100644
index b4fe61b..0000000
--- a/src/simulator/coin-simulator/vpProjectionDisplay.cpp
+++ /dev/null
@@ -1,176 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpProjectionDisplay.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpProjectionDisplay.cpp
- \brief interface with the image for feature display
-*/
-
-#include <visp/vpConfig.h>
-#if defined (VISP_HAVE_DISPLAY)
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-
-//Color / image / display
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-#include <visp/vpDisplay.h>
-
-#include <visp/vpProjectionDisplay.h>
-
-#include <visp/vpBasicFeature.h>
-
-
-void
-vpProjectionDisplay::insert( vpForwardProjection &fp)
-{
- // vpForwardProjection *f ;
- // f = fp.duplicate() ;
- // f->setDeallocate(vpForwardProjection::vpDisplayForwardProjection) ;
-
- listFp.push_back(&fp);
-}
-
-void
-vpProjectionDisplay::init()
-{
- o.setWorldCoordinates(0,0,0) ;
- x.setWorldCoordinates(0.1,0,0) ;
- y.setWorldCoordinates(0,0.1,0) ;
- z.setWorldCoordinates(0,0,0.1) ;
- traj.resize(0,2);
-}
-void
-vpProjectionDisplay::init(const int select)
-{
- if (select & vpProjectionDisplay::internalView())
- {
- Icam.resize(256,256) ;
- dIcam.init(Icam,100,100) ;
- }
- if (select & vpProjectionDisplay::externalView())
- {
- Iext.resize(256,256) ;
- dIext.init(Iext,400,100) ;
- }
-
- init() ;
-}
-
-
-void
-vpProjectionDisplay::close()
-{
-
-}
-
-void
-vpProjectionDisplay::display(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const bool &displayTraj,
- const unsigned int thickness)
-{
-
- for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin() ; it != listFp.end(); ++it )
- {
- vpForwardProjection *fp = *it ;
- fp->display(I,cextMo, cam, color, thickness) ;
- }
-
- if(displayTraj) // display past camera positions
- for(unsigned int i=0;i<traj.getRows();++i)
- vpDisplay::displayCircle(I,(int)traj[i][0],(int)traj[i][1],2,vpColor::green,true);
-
- displayCamera(I, cextMo, cMo, cam, thickness);
-
- if(displayTraj) // store current camera position
- {
- const unsigned int n = traj.getRows();
- traj.resize(n+1, 2, false);
- vpMeterPixelConversion::convertPoint(cam,o.p[0],o.p[1],traj[n][1],traj[n][0]);
- }
-}
-
-
-void
-vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const unsigned int thickness)
-{
- vpHomogeneousMatrix c1Mc ;
- c1Mc = cextMo*cMo.inverse() ;
-
- o.track(c1Mc) ;
-
- if(o.get_Z() < 0) // do not print camera if behind the external camera
- return;
-
- x.track(c1Mc) ;
- y.track(c1Mc) ;
- z.track(c1Mc) ;
-
- vpImagePoint ipo;
- vpImagePoint ipx;
-
- vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
-
- vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;
-
- vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
-}
-
-#endif
diff --git a/src/simulator/coin-simulator/vpProjectionDisplay.h b/src/simulator/coin-simulator/vpProjectionDisplay.h
deleted file mode 100644
index 6c13769..0000000
--- a/src/simulator/coin-simulator/vpProjectionDisplay.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpProjectionDisplay.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpProjectionDisplay_H
-#define vpProjectionDisplay_H
-
-/*!
- \file vpProjectionDisplay.h
- \brief interface with the image for feature display
-*/
-
-#include <visp/vpConfig.h>
-#if defined (VISP_HAVE_DISPLAY)
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpForwardProjection.h>
-
-#include <list>
-
-/*!
- \class vpProjectionDisplay
- \brief interface with the image for feature display
-*/
-class VISP_EXPORT vpProjectionDisplay
-{
-private:
- vpImage<unsigned char> Icam ;
- vpImage<unsigned char> Iext ;
-
-#if defined VISP_HAVE_X11
- vpDisplayX dIcam ;
- vpDisplayX dIext ;
-#elif defined VISP_HAVE_GTK
- vpDisplayGTK dIcam ;
- vpDisplayGTK dIext ;
-#elif defined VISP_HAVE_GDI
- vpDisplayGDI dIcam ;
- vpDisplayGDI dIext ;
-#elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV dIcam ;
- vpDisplayOpenCV dIext ;
-#elif defined(VISP_HAVE_D3D9)
- vpDisplayD3D dIcam ;
- vpDisplayD3D dIext ;
-#endif
-public:
- void init() ;
- void init(int select) ;
- void close() ;
- static int internalView() { return 0x01 ; }
- static int externalView() { return 0x02 ; }
-
- /*! Default constructor. */
- vpProjectionDisplay()
- : Icam(), Iext(),
-#if defined (VISP_HAVE_DISPLAY)
- dIcam(), dIext(),
-#endif
- listFp(), o(), x(), y(), z(), traj()
- {
- init();
- }
- vpProjectionDisplay(int select)
- : Icam(), Iext(),
-#if defined (VISP_HAVE_DISPLAY)
- dIcam(), dIext(),
-#endif
- listFp(), o(), x(), y(), z(), traj()
- {
- init(select) ;
- }
-
- void insert( vpForwardProjection &fp) ;
- void display(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const bool &displayTraj = false,
- const unsigned int thickness=1);
- void displayCamera(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const unsigned int thickness=1) ;
-
-private:
- std::list<vpForwardProjection *> listFp ;
- vpPoint o ;
- vpPoint x ;
- vpPoint y ;
- vpPoint z ;
- vpMatrix traj;
-} ;
-
-
-
-#endif
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/simulator/coin-simulator/vpSimulator.cpp b/src/simulator/coin-simulator/vpSimulator.cpp
deleted file mode 100644
index 684669c..0000000
--- a/src/simulator/coin-simulator/vpSimulator.cpp
+++ /dev/null
@@ -1,1101 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulator.cpp 5263 2015-02-04 13:43:25Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulator based on Coin3d.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-/*!
- \file vpSimulator.cpp
- \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
- The simulator uses the vpViewer class.
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-
-#include <visp/vpSimulator.h>
-#include <visp/vpTime.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-/* Objets OIV. */
-#include <Inventor/nodes/SoCone.h> /* Objet cone. */
-#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
-#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
-#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
-#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
-#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
-#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
-
-#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
-#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
-#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
-#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
-#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
-#include <Inventor/actions/SoWriteAction.h>
-
-
-
-
-// Positions of all of the vertices:
-//
-static float pyramidVertexes [5][3] =
- {
- {0.33f, 0.33f, 0.f},
- {-0.33f, 0.33f, 0.f},
- {-0.33f, -0.33f, 0.f},
- {0.33f, -0.33f, 0.f},
-
- {0.f, 0.f, -1.0f}
- };
-
-
-static int32_t pyramidFaces[] =
- {
- 0, 1, 2, 3, SO_END_FACE_INDEX, // top face
-
- 0, 1, 4, SO_END_FACE_INDEX, // 4 faces about top
- 1, 2, 4, SO_END_FACE_INDEX,
- 2, 3, 4, SO_END_FACE_INDEX,
- 3, 0, 4, SO_END_FACE_INDEX,
- };
-
-
-// Routine to create a scene graph representing a dodecahedron
-SoSeparator *
-makePyramide()
-{
- SoSeparator *result = new SoSeparator;
- result->ref();
-
- // Define coordinates for vertices
- SoCoordinate3 *myCoords = new SoCoordinate3;
- myCoords->point.setValues(0, 5, pyramidVertexes);
- result->addChild(myCoords);
-
- // Define the IndexedFaceSet, with indices into the vertices:
- SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
- myFaceSet->coordIndex.setValues (0, 21, (const int32_t*)pyramidFaces);
- result->addChild (myFaceSet);
-
- result->unrefNoDelete();
- return result;
-}
-
-/* Cree une fleche composee d'un cylindre et d'un cone.
- * La fleche a une hauteur total de <longueur>, dont
- * <proportionFleche>% pour la fleche. Le rayon du cylindre
- * est <radius>, et celui de la fleche <radius> * 5.
- * La fleche est oriente selon l'axe Y.
- */
-static SoSeparator *
-createArrow (float longueur,
- float proportionFleche,
- float radius)
-{
- SoSeparator *fleche = new SoSeparator;
- fleche->ref();
-
- SoTranslation *poseCylindre = new SoTranslation;
- SoCylinder *line = new SoCylinder;
- SoTranslation *posePointe = new SoTranslation;
- SoCone *pointe = new SoCone;
-
- float l_cylindre = longueur * ( 1 - proportionFleche);
- float l_cone = longueur * proportionFleche;
- float radius_cylindre = radius;
- float radius_cone = radius * 5;
-
- line->radius.setValue (radius_cylindre);
- line->height.setValue (l_cylindre);
-
- poseCylindre->translation.setValue (0, l_cylindre / 2, 0);
- posePointe->translation.setValue (0.0, l_cylindre / 2 + l_cone / 2, 0);
-
- pointe->bottomRadius.setValue (radius_cone);
- pointe->height.setValue (l_cone);
-
-
- fleche->addChild (poseCylindre);
- fleche->addChild (line);
- fleche->addChild (posePointe);
- fleche->addChild (pointe);
-
- return fleche;
-}
-
-
-/*
- Cree un objet repere dans un noeud separator, et le renvoie.
- \return : code d'erreur, SIMU_CODE_OK si tout s'est bien passe.
-*/
-#define LONGUEUR_FLECHE 1.0f
-#define RAYON_FLECHE 0.002f
-#define PROPORTION_FLECHE 0.1f
-
-SoSeparator *
-createFrame (float longueurFleche = LONGUEUR_FLECHE ,
- float proportionFleche = PROPORTION_FLECHE,
- float radiusFleche = RAYON_FLECHE)
-{
- vpDEBUG_TRACE (15, "# Entree.");
-
- SoSeparator *frame = new SoSeparator;
- frame-> ref ();
-
- SoRotationXYZ *rotationY_X = new SoRotationXYZ;
- rotationY_X->axis = SoRotationXYZ::Z;
- rotationY_X->angle.setValue ((float)(- M_PI / 2));
-
- SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
- rotationX_Y->axis = SoRotationXYZ::Z;
- rotationX_Y->angle.setValue ((float)(M_PI / 2));
-
- SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
- rotationY_Z->axis = SoRotationXYZ::X;
- rotationY_Z->angle.setValue ((float)(M_PI / 2));
-
- SoMaterial *rouge = new SoMaterial;
- rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
- rouge->emissiveColor.setValue(0.5, 0.0, 0.0);
-
- SoMaterial *vert = new SoMaterial;
- vert->diffuseColor.setValue(0.0, 1.0, 0.0);
- vert->emissiveColor.setValue(0.0, 0.5, 0.0);
-
- SoMaterial *bleu = new SoMaterial;
- bleu->diffuseColor.setValue(0.0, 0.0, 1.0);
- bleu->emissiveColor.setValue(0.0, 0.0, 0.5);
-
- SoSeparator *fleche = createArrow(longueurFleche,
- proportionFleche,
- radiusFleche);
-
- frame->addChild (rouge);
- frame->addChild (rotationY_X);
- frame->addChild (fleche);
- frame->addChild (vert);
- frame->addChild (rotationX_Y);
- frame->addChild (fleche);
- frame->addChild (bleu);
- frame->addChild (rotationY_Z);
- frame->addChild (fleche);
-
- frame-> unrefNoDelete ();
-
- vpDEBUG_TRACE (15, "# Sortie.");
- return frame;
-}
-
-SoSeparator *
-createCameraObject (const float zoomFactor = 1.0)
-{
- vpDEBUG_TRACE (15, "# Entree.");
-
- SoSeparator * cam = new SoSeparator;
- cam->ref ();
-
- SoMaterial *myMaterial = new SoMaterial;
- myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
- myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
-
- SoScale *taille = new SoScale;
- {
- float zoom = 0.1f * zoomFactor;
- taille->scaleFactor.setValue (zoom, zoom, zoom);
- }
-
- SoMaterial *couleurBlanc = new SoMaterial;
- couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
- couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
- SoDrawStyle * filDeFer = new SoDrawStyle;
- filDeFer->style.setValue (SoDrawStyle::LINES);
- filDeFer->lineWidth.setValue (1);
-
- SoSeparator * cone = new SoSeparator;
- cone->ref();
- cone->addChild (makePyramide());
- cone->addChild (couleurBlanc);
- cone->addChild (filDeFer);
- cone->addChild (makePyramide());
- cone->unrefNoDelete();
-
- cam->addChild(myMaterial);
- cam->addChild(taille);
- cam->addChild(cone);
- cam->addChild(createFrame(2.0f,0.1f,0.01f));
-
- // cam->unref() ;
- vpDEBUG_TRACE (15, "# Sortie.");
- return cam;
-}
-
-
-//--------------------------------------------------------------
-void
-vpSimulator::init()
-{
- internal_width = 200;
- internal_height= 200;
- external_width = 200;
- external_height= 200;
-
- mainWindowInitialized = false ;
- internalView = NULL ;
- externalView = NULL ;
- image_background = NULL ;
-
- zoomFactor = 1 ;
- cameraPositionInitialized = false ;
-
- // write image process
- realtime=NULL ;
- offScreenRenderer = NULL ;
- bufferView = NULL;
- get = 1 ;
- typeImage = grayImage;
- mainThread = NULL;
- scene = NULL;
- internalRoot = NULL;
- externalRoot = NULL;
- internalCamera = NULL;
- externalCamera = NULL;
- internalCameraPosition = NULL;
- extrenalCameraPosition = NULL;
- internalCameraObject = NULL;
-#if defined(VISP_HAVE_SOWIN)
- // mainWindow = ?;
-#elif defined(VISP_HAVE_SOQT)
- mainWindow = NULL;
-#elif defined(VISP_HAVE_SOXT)
- // mainWindow = ?;
-#endif
-
-}
-void
-vpSimulator::kill()
-{
- if (internalView !=NULL) {delete internalView ; internalView = NULL;}
- if (externalView !=NULL) {delete externalView ; externalView = NULL;}
- if (bufferView!=NULL) {delete[] bufferView ; bufferView = NULL;}
- if (image_background != NULL) {
- free (image_background);
- image_background = NULL;
- }
-
-}
-
-vpSimulator::vpSimulator()
- :
-#if defined(VISP_HAVE_SOWIN)
- mainWindow(),
-#elif defined(VISP_HAVE_SOQT)
- mainWindow(NULL),
-#elif defined(VISP_HAVE_SOXT)
- mainWindow(),
-#endif
- mainWindowInitialized(false), typeImage(vpSimulator::grayImage),
- image_background(NULL), internalView(NULL), externalView(NULL),
- mainThread(NULL), internal_width(0), internal_height(0),
- external_width(0), external_height(0), scene(NULL), internalRoot(NULL),
- externalRoot(NULL), internalCamera(NULL), externalCamera(NULL),
- internalCameraPosition(NULL), extrenalCameraPosition(NULL), internalCameraObject(NULL),
- zoomFactor(0.), cameraPositionInitialized(false), cMf(), internalCameraParameters(),
- externalCameraParameters(), realtime(NULL), offScreenRenderer(NULL), bufferView(NULL),
- get(0)
-{
- vpSimulator::init() ;
-}
-
-vpSimulator::~vpSimulator()
-{
- vpSimulator::kill() ;
-}
-
-void
-vpSimulator::initSoApplication()
-{
- mainWindow = vpViewer::init("");
- mainWindowInitialized = true ;
-}
-
-void
-vpSimulator::initSceneGraph()
-{
- this->scene = new SoSeparator;
- this->internalRoot = new SoSeparator;
- this->externalRoot = new SoSeparator;
-
- this->scene->ref();
- this->internalRoot->ref();
- this->externalRoot->ref();
-
- // define the camera SoPerspectiveCamera
- this->internalCamera = new SoPerspectiveCamera ;
- this->externalCamera = new SoPerspectiveCamera ;
-
-
- this->internalCameraPosition = new SoTransform;
- this->internalCameraObject = createCameraObject(zoomFactor);
-
- internalCamera->farDistance.setValue(100);
- internalCamera->nearDistance.setValue(0.0001f);
-
-
-
- // link between camera and internal root
- this->internalRoot->addChild (this->internalCamera);
- this->internalRoot->addChild (this->scene);
-
- this->externalRoot->addChild (this->externalCamera);
- this->externalRoot->addChild (this->scene);
-
-
- SoSeparator * camera = new SoSeparator;
- camera->ref();
- camera->addChild (this->internalCameraPosition);
- camera->addChild (this->internalCameraObject);
- this->externalRoot->addChild (camera);
-
-
- //this->externalRoot->addChild (internalCameraPosition);
- // this->externalRoot->addChild (internalCameraObject);
- SoCube *cube = new SoCube ;
- cube->width=0.01f ;
- cube->depth=0.01f ;
- cube->height=0.01f ;
-
- this->externalRoot->addChild (cube);
-
- if (realtime==NULL)
- {
-
- SoDB::enableRealTimeSensor(FALSE);
- SoSceneManager::enableRealTimeUpdate(FALSE);
- realtime = (SbTime *) SoDB::getGlobalField("realTime");
- realtime->setValue(0.0);
-
- }
-
-
-}
-
-
-
-/*!
- \brief Define the zoom factor used to define the size of the objects (frame,
- camera, ...)
-
- \param zoom: zoom factor of the objects. By default, 1.
-*/
-void
-vpSimulator::setZoomFactor (const float zoom)
-{
- zoomFactor = zoom;
- static bool firstTime = true;
- if(firstTime){
- SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
- this->scene->addChild(taille);
- firstTime = false;
- }
- else{
- SoScale * taille = (SoScale*)this->scene->getChild(0);
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
- }
-}
-
-/*!
- \brief Change the zoom factor associated to the child given by index.
- In order to create multiple zoom factor for multiple object to display,
- objects loaded the load() function, you have to know the index of the scale
- object associated to.
-
- Usually, if you define the main zoom factor (for example for the frames) and
- then load two differents objects, You can change the zoom factor of all the
- objects using: changeZoomFactor(newZoom, 0)
-
- If you want to change the zoom factor of the first object, use changeZoomFactor(newZoom, 1)
-
- And for the second object, use changeZoomFactor(newZoom, 3)
-
- \param zoomFactor : the new zoom use to specify the apparent size of the object
- \param index : the index of the Scale object to modify (see comments)
-*/
-void
-vpSimulator::changeZoomFactor(const float zoomFactor, const int index)
-{
- SoScale * taille = (SoScale*)this->scene->getChild(index);
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
-// this->setZoomFactor(zoomFactor);
-}
-
-void
-vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height)
-{
- internal_width = width;
- internal_height = height;
-
- if (mainWindowInitialized==false)
- {
- initSoApplication() ;
- initSceneGraph() ;
- }
-
- internalView = new vpViewer(mainWindow, this,vpViewer::internalView);
-
- // set the scene to render from this view
- internalView->setSceneGraph(internalRoot);
-
- // set the title
- internalView->setTitle("Internal camera view") ;
-
- //If the view mode is on, user events will be caught and used to influence
- //the camera position / orientation. in this viewer we do not want that,
- //we set it to false
- internalView->setViewing(false);
-
- // Turn the viewer decorations
- internalView->setDecoration(false) ;
-
- internalView->resize((int)width, (int)height, true) ;
-
- // open the window
- internalView->show();
-
- bufferView = new unsigned char[3*width*height] ;
-
-}
-
-void
-vpSimulator::initExternalViewer(const unsigned int width, const unsigned int height)
-{
-
- external_width = width;
- external_height = height;
-
- if (mainWindowInitialized==false)
- {
- initSoApplication() ;
- initSceneGraph() ;
- }
-
- externalView = new vpViewer(mainWindow,this, vpViewer::externalView);
-
- // set the scene to render this view
- externalView->setSceneGraph(externalRoot);
-
- // set the title
- externalView->setTitle("External View") ;
- externalView->resize((int)width, (int)height, false) ;
- // the goal here is to see all the scene and not to determine
- // a manual viewpoint
- externalView->viewAll ();
-
- // open the window
- externalView->show();
-}
-
-void
-vpSimulator::setInternalCameraParameters(vpCameraParameters &_cam)
-{
- internalCameraParameters = _cam ;
-
-
- float px = (float)_cam.get_px();
- float py = (float)_cam.get_py();
- float v = internal_height/(2.f*py);
-
- internalCamera->ref() ;
- internalCamera->heightAngle = 2*atan(v);
- internalCamera->aspectRatio=(internal_width/internal_height)*(px/py);
- internalCamera->nearDistance = 0.001f ;
-
- internalCamera->farDistance = 1000;
- internalCamera->unrefNoDelete() ;
-}
-
-void
-vpSimulator::setExternalCameraParameters(vpCameraParameters &_cam)
-{
-// SoPerspectiveCamera *camera ;
-// camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
- externalCameraParameters = _cam ;
-
- float px = (float)_cam.get_px();
- float py = (float)_cam.get_py();
- float v = external_height/(2*py);
-
- externalCamera->ref() ;
- externalCamera->heightAngle = 2*atan(v);
- externalCamera->aspectRatio=(external_width/external_height)*(px/py);
- externalCamera->nearDistance = 0.001f ;
- externalCamera->farDistance = 1000;
- externalCamera->unrefNoDelete() ;
-
-}
-
-void
-vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf)
-{
-/* SoCamera *camera ;
- camera = this->externalView->getCamera() ;*/
- SoSFVec3f position = externalCamera->position ;
-
- // get the rotation
- SoSFRotation orientation = externalCamera->orientation;
- SbVec3f axis ; float angle ;
- orientation.getValue(axis,angle) ;
- SbRotation rotation(axis,angle) ;
-
- // get the translation
- SbVec3f t ;
- t = position.getValue() ;
-
- SbMatrix matrix ;
- matrix.setRotate(rotation) ;
-
- vpHomogeneousMatrix fMc ;
- SbMatrix rotX;
- rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
- matrix.multLeft (rotX);
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- fMc[j][i]=matrix[(int)i][(int)j];
- fMc[0][3] = t[0] ;
- fMc[1][3] = t[1] ;
- fMc[2][3] = t[2] ;
-
- cMf = fMc.inverse() ;
-}
-
-
-void
-vpSimulator::setCameraPosition(vpHomogeneousMatrix &_cMf)
-{
- cameraPositionInitialized = true ;
- cMf = _cMf ;
-}
-void
-vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf)
-{
-
- SbMatrix matrix;
- SbRotation rotCam;
- SbMatrix rotX;
- rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- matrix[(int)j][(int)i]=(float)cMf[i][j];
-
- matrix= matrix.inverse();
- matrix.multLeft (rotX);
- rotCam.setValue(matrix);
-
-
- internalCamera->ref() ;
- internalCamera->orientation.setValue(rotCam);
- internalCamera->position.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
- internalCamera->unref() ;
-
- rotX.setRotate (SbRotation (SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
- matrix.multLeft (rotX);
- rotCam.setValue(matrix);
- internalCameraPosition->ref() ;
- internalCameraPosition->rotation.setValue(rotCam);
- internalCameraPosition->translation.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
- internalCameraPosition->unref() ;
-}
-
-
-/*! this function MUST NOT be called from a thread where the vpSimulator and
- its mainloop are not
-*/
-void
-vpSimulator::redraw()
-{
-
- // if (this->cameraPositionInitialized==true)
- {
- if (this->externalView != NULL)
- {
- this->externalView->render() ; //call actualRedraw()
- // vpHomogeneousMatrix c ;
- // getExternalCameraPosition(c) ;
- }
- if (this->internalView != NULL)
- {
- this->moveInternalCamera(this->cMf) ;
- this->internalView->render() ; //call actualRedraw()
- }
- }
-}
-
-// This function is called 20 times each second.
-static void
-timerSensorCallback(void *data , SoSensor *)
-{
- vpSimulator * simulator = (vpSimulator *)data ;
-
- simulator->redraw() ;
-
-}
-
-
-void
-vpSimulator::mainLoop()
-{
- if (mainWindowInitialized==false)
- {
- vpERROR_TRACE("main window is not opened ") ;
- }
-
- vpTime::wait(1000) ;
-
- // Timer sensor
- SoTimerSensor * timer = new SoTimerSensor(timerSensorCallback, (void *)this);
- timer->setInterval(0.01);
- timer->schedule();
- vpViewer::mainLoop() ;
-}
-
-
-//-----------------------------------------------------------------
-// scene stuff
-//-----------------------------------------------------------------
-
-//!loading the virtual scene
-void
-vpSimulator::load(const char *file_name)
-{
-
- SoInput input;
- if (!input.openFile(file_name))
- {
- vpERROR_TRACE("Erreur cannot open file %s",file_name);
- }
-
- SoSeparator *newscene=SoDB::readAll(&input);
- newscene->ref() ;
- if (newscene==NULL)
- {
- vpERROR_TRACE("Error while reading %s",file_name);
- }
-
- SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
-
-// newscene->addChild(taille);
-
-// std::cout << "this->scene->getNumChildren() = " << this->scene->getNumChildren() << std::endl;
-
- this->scene->addChild(taille);
- this->scene->addChild(newscene);
- newscene->unref() ;
-
-}
-
-
-void
-vpSimulator::save(const char *name,bool binary)
-{
- // get a pointer to the object "name"
- SoOutput output ;
- output.openFile(name) ;
-
- if (binary==true) output.setBinary(TRUE) ;
-
- SoWriteAction writeAction(&output) ;
- writeAction.apply(scene) ;
- output.closeFile() ;
-
-}
-
-/*!
- Add the representation of a frame.
- \param fMo : desired position of the frame
- \param zoom : Zoom factor.
-*/
-void
-vpSimulator::addFrame (const vpHomogeneousMatrix &fMo, float zoom)
-{
-
- SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoom, zoom, zoom);
-
- SoSeparator * frame = new SoSeparator;
- frame->ref();
- frame->addChild(taille);
- frame->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom));
- this->addObject(frame, fMo, externalRoot) ;
- // frame->unref();
-}
-
-/*!
- \brief Add the representation of the absolute frame
-
- \param zoom : Zoom factor.
-*/
-void
-vpSimulator::addAbsoluteFrame (float zoom)
-{
- scene->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom)) ;
-}
-
-/*!
- \brief Add a new object in the scene graph
- \param iv_filename : name of.iv file to load
- \param fMo : position of the object wrt the reference frame
-*/
-void
-vpSimulator::load(const char * iv_filename,const vpHomogeneousMatrix &fMo)
-{
-
- SoInput in;
- SoSeparator * newObject;
-
- if (! in.openFile (iv_filename))
- {
- vpERROR_TRACE ("Erreur lors de la lecture du fichier %s.", iv_filename);
- }
-
- newObject = SoDB::readAll (&in);
- if (NULL == newObject)
- {
- vpERROR_TRACE ("Problem reading data for file <%s>.", iv_filename);
- }
-
- try
- {
- this->addObject (newObject, fMo) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error adding object from file <%s> ",iv_filename) ;
- throw ;
- }
-
-}
-
-
-/*!
- \brief Add a new object in the scene graph
- \param newObject : pointer toward the new object
- \param fMo : position of the object wrt the reference frame
-*/
-void
-vpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo)
-{
- try
- {
- this->addObject(newObject, fMo , scene);
- }
- catch(...)
- {
- vpERROR_TRACE("Error adding object in scene graph ") ;
- throw ;
- }
-}
-
-
-
-
-
-/*!
- \brief Add an object in a sub scene graph
- \param object : pointer toward the new object
- \param fMo : position of the object wrt the reference frame
- \param root : pointer toward the subscene graph
-*/
-
-void
-vpSimulator::addObject(SoSeparator * object,
- const vpHomogeneousMatrix &fMo,
- SoSeparator * root)
-{
-
- bool identity = true ;
- for (unsigned int i=0 ; i <4 ;i++){
- for (unsigned int j=0 ; j < 4 ; j++){
- if (i==j){
- if (fabs(fMo[i][j] -1) > 1e-6) identity=false ;
- }
- else{
- if (fabs(fMo[i][j]) > 1e-6) identity=false ;
- }
- }
- }
-
- if (identity==true)
- {
- root->addChild (object);
- }
- else
- {
- SbMatrix matrix;
- SbRotation rotation;
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- matrix[(int)j][(int)i]=(float)fMo[i][j];
-
- // matrix= matrix.inverse();
- rotation.setValue(matrix);
-
- SoTransform *displacement = new SoTransform;
- SoSeparator *newNode = new SoSeparator;
-
- displacement->rotation.setValue(rotation);
- displacement->translation.setValue(matrix[3][0],
- matrix[3][1],
- matrix[3][2]);
-
- root->addChild (newNode);
- newNode->addChild (displacement);
- newNode->addChild (object);
-
- }
-}
-
-
-//! init the main program thread
-void
-vpSimulator::initApplication(void *(*start_routine)(void *))
-{
- //pthread_create (&mainThread, NULL, start_routine, (void *)this);
- mainThread = SbThread::create (start_routine, (void *)this);
-}
-
-/*!
- Set the function used for the simulation loop and the data to pass to this
- function. As the data are represented using a generic pointer, care should be
- taken to ensure there is no memory corruption.
-
- \param start_routine : A pointer to the function used as a main simulation
- loop for the simulation.
- \param data : The data to pass to the main loop.
-*/
-void
-vpSimulator::initApplication(void *(*start_routine)(void *), void* data)
-{
- mainThread = SbThread::create (start_routine, (void *)data);
-}
-
-//! performed some initialization in the main program thread
-//! should be locate at the beginning of the main program
-void
-vpSimulator::initMainApplication()
-{
- //pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
- //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
- vpTime::wait(1000) ;
-}
-//! performed some thread destruction in the main program thread
-//! should be locate at the end of the main program
-void
-vpSimulator::closeMainApplication()
-{
- vpViewer::exitMainLoop() ;
- //pthread_exit (NULL);
-}
-
-
-
-
-
-/* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
- * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
- * enregistre ou passe a l'utilisateur.
- * INPUT:
- * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
- * il faut rendre la vue externe.
- * OUTPUT:
- * - width : largeur de l'image dans le buffer.
- * - height : hauteur de l'image dans le buffer.
- */
-void
-vpSimulator::offScreenRendering(vpSimulatorViewType view, int * width, int * height)
-{
-
- SbVec2s size(320,200);
- SoNode * thisroot;
-
- {
- if (view==vpSimulator::INTERNAL)
- {
- size = this ->internalView ->getViewportRegion().getWindowSize();
- thisroot = this ->internalView->getSceneManager()->getSceneGraph() ;
- }
- else
- {
- size = this ->externalView ->getViewportRegion().getWindowSize();
- thisroot = this ->externalView->getSceneManager()->getSceneGraph() ;
- }
- }
- SbViewportRegion myViewPort(size);
-
- // Creation du rendu si necessaire.
- if (NULL == this ->offScreenRenderer)
- {
- //Init du SoOffscreenRenderer
- this ->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
- }
- else
- {
- // Redefini le view port
- this ->offScreenRenderer ->setViewportRegion (myViewPort);
- }
-
- // Rendu offscreen
- if (! this ->offScreenRenderer ->render(thisroot))
- {
- vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
- delete this ->offScreenRenderer;
- this ->offScreenRenderer = NULL;
- }
- else
- {
-
-
- /*
- if (view==vpSimulator::INTERNAL)
- {
- //Recopie du buffer contenant l'image, dans bufferView
- int length = 3*size [0]*size[1];
- delete [] bufferView;
- bufferView = new unsigned char [length];
- for(int i=0; i<length; i++)
- {
- bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
- }
- }*/
-
- }
-
- // exit(1) ;
- if (NULL != width) { * width = size [0]; }
- if (NULL != height) { * height = size [1]; }
-
-
-}
-
-
-/* Enregistre l'image de vue interne ou externe dans un fichier RGB.
- * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
- * ce buffer au format PS (copie directe).
- * INPUT
- * - fileName: nom du fichier dans lequel placer le resultat.
- * OUTPUT
- * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
- */
-
-void
-vpSimulator::write (const char * fileName)
-{
-
- while (get==0) { vpTRACE("%d ",get); }
- get =2 ;
- /* FILE *fp = fopen(fileName, "w");
- fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
- fwrite(bufferView, sizeof(unsigned char), internal_width*internal_height*3, fp) ;*/
- vpImage<vpRGBa> I(internal_height, internal_width) ;
-
-
- for(unsigned int i=0 ; i < internal_height ; i++)
- for(unsigned int j=0 ; j < internal_width ; j++)
- {
- unsigned char r,g,b ;
- unsigned int index = 3*((internal_height-i-1)* internal_width + j );
- r = *(bufferView+index);
- g = *(bufferView+index+1);
- b = *(bufferView+index+2);
- I[i][j].R =r ;
- I[i][j].G =g ;
- I[i][j].B =b ;
- }
- vpImageIo::write(I,fileName) ;
- // fclose (fp);
- get =1 ;
-}
-
-void
-vpSimulator::getSizeInternalView(int& width, int& height)
-{
- SbVec2s size = this ->internalView ->getViewportRegion().getWindowSize();
- width = size [0];
- height = size[1];
-}
-
-/*!
- Make a copy of the current internal view
- \param I : destination image
- */
-
-void
-vpSimulator::getInternalImage(vpImage<vpRGBa> &I)
-{
- //while (get==0) {;}
- get =2 ;
- I.resize(internal_height,internal_width) ;
- vpImageConvert::RGBToRGBa(bufferView,(unsigned char*)I.bitmap,internal_width,internal_height,true);
- get =1 ;
-}
-
-/*!
- Make a copy of the current internal view
- \param I : destination image
- */
-void
-vpSimulator::getInternalImage(vpImage<unsigned char> &I)
-{
- //while (get==0) {;}
- get =2 ;
- I.resize(internal_height,internal_width) ;
- vpImageConvert::RGBToGrey(bufferView,I.bitmap,internal_width,internal_height,true);
- get =1 ;
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/simulator/coin-simulator/vpSimulator.h b/src/simulator/coin-simulator/vpSimulator.h
deleted file mode 100644
index 6f1c4d4..0000000
--- a/src/simulator/coin-simulator/vpSimulator.h
+++ /dev/null
@@ -1,307 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulator.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulator based on Coin3d.
- *
- * Authors:
- * Eric Marchand
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#ifndef vpSimulator_HH
-#define vpSimulator_HH
-/*!
- \file vpSimulator.h
- \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
- The simulator uses the vpViewer class.
-
- \warning The content of this file is only available if Coin3D and
- one of the GUI (SoWin, SoXT, SoQt) are installed.
-*/
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-
-#include <visp/vpViewer.h>
-
-/* KNOWN ISSUE DEALING WITH X11 and QT
- If you get a strange compiler error on the line with None,
- it's probably because you're also including X11 headers,
- which #define the symbol None. Put the X11 includes after
- the Qt includes to solve this problem.
- */
-
-#include <Inventor/nodes/SoBaseColor.h>
-#include <Inventor/nodes/SoTransform.h>
-#include <Inventor/nodes/SoCone.h>
-#include <Inventor/nodes/SoCube.h>
-#include <Inventor/nodes/SoImage.h>
-#include <Inventor/nodes/SoLightModel.h>
-#include <Inventor/nodes/SoPerspectiveCamera.h>
-#include <Inventor/nodes/SoRotationXYZ.h>
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoTranslation.h>
-#include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/SoOffscreenRenderer.h>
-#include <Inventor/fields/SoSFTime.h>
-#include <Inventor/threads/SbThread.h>
-
-// visp
-#include <visp/vpDebug.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpImageConvert.h>
-
-/*!
- \class vpSimulator
-
- \ingroup RobotSimuWithViz
-
- \brief Implementation of a simulator based on Coin3d (www.coin3d.org).
-
- The simulator uses the vpViewer class.
-
- \warning This class is only available if Coin3D and one of the GUI
- (SoWin, SoXT, SoQt) are installed.
-*/
-
-class VISP_EXPORT vpSimulator
-{
-protected:
- //! perform some initialization
- void init() ;
- //! perform some destruction
- void kill() ;
-
-public:
- //! constructor
- vpSimulator() ;
- virtual ~vpSimulator() ;
-
-protected:
- //! main Widget
-#if defined(VISP_HAVE_SOWIN)
- HWND mainWindow ;
-#elif defined(VISP_HAVE_SOQT)
- QWidget * mainWindow ;
-#elif defined(VISP_HAVE_SOXT)
- Widget mainWindow ;
-#endif
-
- bool mainWindowInitialized ;
-
- //! open the SoGui application
- void initSoApplication() ;
-
-public:
- typedef enum {
- grayImage,
- colorImage
- } vpImageType ;
- vpImageType typeImage ;
-
- GLubyte * image_background;
- //! activate the mainloop
- virtual void mainLoop() ;
-
-protected:
- //! view from the camera
- vpViewer *internalView ;
- //! view from an external camera
- vpViewer *externalView ;
-
-public:
- //! initialize the camera view
- virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol) ;
- //! initialize the external view
- void initExternalViewer(const unsigned int nlig, const unsigned int ncol) ;
-
-protected:
- //! thread with the main program
- SbThread * mainThread;
- //pthread_t mainThread;
-
-public:
- //! begin the main program
- void initApplication(void *(*start_routine)(void *)) ;
- void initApplication(void *(*start_routine)(void *), void* data);
- //! perform some initialization in the main program thread
- void initMainApplication() ;
- void closeMainApplication() ;
-
- //----------------------------------------------------
- // scene description
-protected:
- unsigned int internal_width;
- unsigned int internal_height;
- unsigned int external_width;
- unsigned int external_height;
-
-public:
- /*!
- Return the width of the internal view.
-
- \return The width of the internal view.
- */
- unsigned int getInternalWidth() const { return internal_width; }
- /*!
- Return the height of the internal view.
-
- \return The height of the internal view.
- */
- unsigned int getInternalHeight() const { return internal_height; }
-
-protected:
- //! root node of the scene : contains everything except stuff specific to
- //! each view
- SoSeparator *scene;
- //! root node of the internal view
- SoSeparator *internalRoot;
- //! root node of the external view
- SoSeparator *externalRoot;
-
- //! internal camera
- SoPerspectiveCamera *internalCamera ;
- //! external camera
- SoPerspectiveCamera *externalCamera ;
-
- //! internal camera position
- SoTransform *internalCameraPosition ;
-
- //! external camera position
- SoTransform *extrenalCameraPosition ;
-
- //! representation of the camera in the external view
- SoSeparator *internalCameraObject;
-
- //! initialize the scene graph
- void initSceneGraph() ;
-
- //!Add a new object in the scene graph ad a given location
- void addObject(SoSeparator * object, const vpHomogeneousMatrix &fMo,
- SoSeparator * root) ;
-
-public :
- //!Add a new object in the scene graph ad a given location
- void addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo) ;
-
-public:
- //! display the scene (handle with care)
- void redraw() ;
- //! load an iv file
- void load(const char *file_name) ;
- //! load an iv file, set the location of this scene
- void load(const char *iv_filename,const vpHomogeneousMatrix &fMo) ;
- //! save the scene in an iv file
- void save(const char *name,bool binary=false) ;
-
- //! Add the representation of the absolute frame
- void addAbsoluteFrame (float zoom=1) ;
- //! Add the representation of a frame
- void addFrame (const vpHomogeneousMatrix &fMo, float zoom=1) ;
- //! set the size of the camera/frame
- void setZoomFactor (const float zoom) ;
-
-protected:
- float zoomFactor ;
- //---------------------------------------------------
- // camera description
-protected:
- bool cameraPositionInitialized ;
- //! internal camera position
- vpHomogeneousMatrix cMf ;
- //! internal camera parameters
- vpCameraParameters internalCameraParameters ;
- //! internal camera parameters
- vpCameraParameters externalCameraParameters ;
-
-public:
- //! set the camera position (from an homogeneous matrix)
- void setCameraPosition(vpHomogeneousMatrix &cMf) ;
- //! get the camera position (from an homogeneous matrix)
- void getCameraPosition(vpHomogeneousMatrix &_cMf) { _cMf = cMf ; }
- //! modify the position of the camera in the scene graph
- void moveInternalCamera(vpHomogeneousMatrix &cMf) ;
- //! set internal camera parameters
- void setInternalCameraParameters(vpCameraParameters &cam) ;
- //! set external camera parameters
- void setExternalCameraParameters(vpCameraParameters &cam) ;
- //! get the external camera position
- void getExternalCameraPosition(vpHomogeneousMatrix &cMf) ;
-
- //! get an Image of the internal view
- void getInternalImage(vpImage<unsigned char> &I) ;
- //! get an Image of the internal view
- void getInternalImage(vpImage<vpRGBa> &I) ;
- /* --- Off screen rendering --- */
-
- void changeZoomFactor(const float zoom, const int index);
-public:
- typedef enum { INTERNAL, EXTERNAL } vpSimulatorViewType ;
- void write(const char * fileName);
-protected:
- SbTime * realtime ;
- SoOffscreenRenderer * offScreenRenderer ;
- void offScreenRendering (vpSimulatorViewType view=vpSimulator::EXTERNAL,
- int * width = NULL,
- int * height = NULL);
-public:
- //! image of the internal view
- unsigned char * bufferView;
-
- //! Flag to protect the read and write of the framebuffer (between the simulator and the viewer).
- int get ;
-public:
- //! get the image corresponding to the internal view
- unsigned char* getBufferedOffScreenRenderer()
- {
- return bufferView;
- }
-
- //! get the size of the internal view
- void getSizeInternalView(int& width, int& height);
-
- //! get the intrinsic parameters of the camera
- void getCameraParameters(vpCameraParameters &cam)
- {
- cam = internalCameraParameters;
- }
-} ;
-
-
-#endif
-#endif
diff --git a/src/simulator/coin-simulator/vpSimulatorException.h b/src/simulator/coin-simulator/vpSimulatorException.h
deleted file mode 100644
index 887b749..0000000
--- a/src/simulator/coin-simulator/vpSimulatorException.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSimulatorException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the simulator classes.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpSimulatorException_H
-#define __vpSimulatorException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpSimulatorException.h
- \brief error that can be emited by the vpSimulator class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
-
- \class vpSimulatorException
-
- \ingroup Exception
-
- \brief Error that can be emited by the vpSimulator class and its derivates.
- */
-class VISP_EXPORT vpSimulatorException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpSimulator member
- */
- enum errorSimulatorCodeEnum
- {
- ioError,
- noFileNameError,
- notInitializedError,
- windowSizeNotInitializedError,
- badInitializationError
- } ;
-
- public:
- vpSimulatorException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpSimulatorException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpSimulatorException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/simulator/coin-simulator/vpViewer.cpp b/src/simulator/coin-simulator/vpViewer.cpp
deleted file mode 100644
index 69e66e6..0000000
--- a/src/simulator/coin-simulator/vpViewer.cpp
+++ /dev/null
@@ -1,266 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViewer.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulator based on Coin3d.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-/*!
- \file vpViewer.cpp
- Viewer used by the simulator. Under Windows, the viewer is
- based either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or SoXt.
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-#include <visp/vpViewer.h>
-#include <visp/vpSimulator.h>
-
-
-#include <Inventor/events/SoKeyboardEvent.h>
-#include <Inventor/nodes/SoEventCallback.h>
-
-
-#if defined(VISP_HAVE_SOWIN)
-vpViewer::vpViewer(HWND parent, vpSimulator *_simu, vpViewerType viewerType):
- SoWinExaminerViewer(parent,(char *)NULL,false)
-#elif defined(VISP_HAVE_SOQT)
-vpViewer::vpViewer(QWidget * parent, vpSimulator *_simu, vpViewerType viewerType) :
- SoQtExaminerViewer(parent,(char *)NULL,false)
-#elif defined(VISP_HAVE_SOXT)
-vpViewer::vpViewer(Widget parent, vpSimulator *_simu, vpViewerType viewerType):
- SoXtExaminerViewer(parent,(char *)NULL,false)
-#endif
-{
- this->simu = _simu ;
- this->viewerType = viewerType;
- // Coin should not clear the pixel-buffer, so the background image
- // is not removed.
-
- this->setClearBeforeRender(FALSE, TRUE);
- // this->setAntialiasing(true, 2) ;
- setAutoRedraw(false);
-}
-
-vpViewer::~vpViewer()
-{
-
-}
-
-void
-vpViewer::actualRedraw(void)
-{
-
- {
- const SbViewportRegion vp = this->getViewportRegion();
- SbVec2s origin = vp.getViewportOriginPixels();
- SbVec2s size = vp.getViewportSizePixels();
- glViewport(origin[0], origin[1], size[0], size[1]);
-
- const SbColor col = this->getBackgroundColor();
- glClearColor(col[0], col[1], col[2], 0.0f);
- glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
- // this should be used only with the vpAR:vpSimulator
- // to diplay an image background
- if (simu->image_background != NULL)
- {
- glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
- if (simu->typeImage == vpSimulator::grayImage)
- glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_LUMINANCE,
- GL_UNSIGNED_BYTE,
- simu->image_background );
- else
- glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_RGB,
- GL_UNSIGNED_BYTE,
- simu->image_background );
-
- glEnable(GL_DEPTH_TEST);
- glClear(GL_DEPTH_BUFFER_BIT); // clear the z-buffer
- glClearDepth(100.0); // Profondeur du Z-Buf
- }
- // Render normal scenegraph.
-#if defined(VISP_HAVE_SOWIN)
- SoWinExaminerViewer::actualRedraw();
-#elif defined(VISP_HAVE_SOQT)
- SoQtExaminerViewer::actualRedraw();
-#elif defined(VISP_HAVE_SOXT)
- SoXtExaminerViewer::actualRedraw();
-#endif
- glSwapBuffers() ;
- if(viewerType == vpViewer::internalView){
- simu->get = 0 ;
- glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_RGB,
- GL_UNSIGNED_BYTE,
- simu->bufferView ) ;
- simu->get =1 ;
- }
- }
-
-}
-
-/*!
-
- \param x : width
- \param y : height
- \param fixed : set as true to disable mouse window resizing
-
-*/
-void
-#if defined(VISP_HAVE_SOWIN) || defined (VISP_HAVE_SOQT)
-vpViewer::resize(int x, int y, bool fixed)
-#else
-vpViewer::resize(int x, int y, bool /*fixed*/)
-#endif
-{
- SbVec2s size(x,y) ;
- setSize(size);
- setGLSize(size) ;
-
-#if defined(VISP_HAVE_SOWIN)
- HWND parent = getParentWidget();
-
- RECT rcClient, rcWindow;
- POINT ptDiff;
- GetClientRect(parent, &rcClient);
- GetWindowRect(parent, &rcWindow);
- ptDiff.x = (rcWindow.right - rcWindow.left) - rcClient.right;
- ptDiff.y = (rcWindow.bottom - rcWindow.top) - rcClient.bottom;
- MoveWindow(parent,rcWindow.left, rcWindow.top,
- x + ptDiff.x, y + ptDiff.y, TRUE);
- if(fixed){
- DWORD dwStyle = GetWindowLong(parent, GWL_STYLE);
- dwStyle &= ~(WS_SIZEBOX);
- SetWindowLong(parent, GWL_STYLE,dwStyle);
- }
-#elif defined(VISP_HAVE_SOQT)
- if(fixed){
- QWidget * parent = getParentWidget();
- parent->setFixedSize(x, y);
- }
-#endif
-}
-
-/*!
-\param left : left coordinate
-\param bottom : bottom coordinate
-\param x : width
-\param y : height
-
-*/
-
-SbBool
-vpViewer::processSoEvent(const SoEvent * const event)
-{
- if ( this->isViewing() &&
- event->getTypeId() == SoKeyboardEvent::getClassTypeId() )
- {
- SoKeyboardEvent * kbevent = (SoKeyboardEvent *) event;
- switch ( kbevent->getKey() ) {
- case SoKeyboardEvent::H:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- std::cout << "H : this help "<<std::endl ;
- std::cout << "M : get and save the external camera location (matrix)"<<std::endl;
- std::cout << "V : get and save the external camera location (vector)"<<std::endl;
- std::cout << "M : load camera location (vector)"<<std::endl;
- std::cout << "P : get external camera location and set the internal one"<<std::endl;
- }
- return TRUE;
-
- case SoKeyboardEvent::M:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- std::ofstream f("cMf.dat") ;
- cMf.save(f) ;
- f.close() ;
- }
- return TRUE;
- case SoKeyboardEvent::V:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- vpPoseVector vcMf(cMf) ;
- std::ofstream f("vcMf.dat") ;
- vcMf.save(f) ;
- f.close() ;
- }
- return TRUE;
- case SoKeyboardEvent::L:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpPoseVector vcMf;
- std::ifstream f("vcMf.dat") ;
- vcMf.load(f) ;
- f.close() ;
- vpHomogeneousMatrix cMf(vcMf) ;
- simu->setCameraPosition(cMf) ;
- simu->moveInternalCamera(cMf) ;
- }
- return TRUE;
- case SoKeyboardEvent::P:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- vpPoseVector vcMf(cMf) ;
- vcMf.print() ;
- simu->setCameraPosition(cMf) ;
- simu->moveInternalCamera(cMf) ;
- }
- return TRUE;
- default:
- break;
- }
- }
-#if defined(VISP_HAVE_SOWIN)
- return SoWinExaminerViewer::processSoEvent(event);
-#elif defined(VISP_HAVE_SOQT)
- return SoQtExaminerViewer::processSoEvent(event);
-#elif defined(VISP_HAVE_SOXT)
- return SoXtExaminerViewer::processSoEvent(event);
-#endif
-
-}
-
-#endif
diff --git a/src/simulator/coin-simulator/vpViewer.h b/src/simulator/coin-simulator/vpViewer.h
deleted file mode 100644
index c02bc4d..0000000
--- a/src/simulator/coin-simulator/vpViewer.h
+++ /dev/null
@@ -1,169 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViewer.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Simulator based on Coin3d.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpViewer_HH
-#define vpViewer_HH
-/*!
- \file vpViewer.h
-
- Viewer used by the simulator. Under Windows, the viewer is based
- either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or
- SoXt.
-
- \warning The content of this file is only available if Coin3D and
- one of the GUI (SoWin, SoXT, SoQt) are installed.
-
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_COIN_AND_GUI
-
-#if defined(VISP_HAVE_SOWIN)
-
- #include <Inventor/Win/SoWin.h>
- #include <Inventor/Win/viewers/SoWinExaminerViewer.h>
-
-#elif defined(VISP_HAVE_SOQT)
-
- #include <Inventor/Qt/SoQt.h>
- #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
-
-#elif defined(VISP_HAVE_SOXT)
-
- #include <Inventor/Xt/SoXt.h>
- #include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
-
-#endif
-
-// Coin stuff
-
-#include <Inventor/nodes/SoBaseColor.h>
-#include <Inventor/nodes/SoCone.h>
-#include <Inventor/nodes/SoCube.h>
-#include <Inventor/nodes/SoImage.h>
-#include <Inventor/nodes/SoLightModel.h>
-#include <Inventor/nodes/SoPerspectiveCamera.h>
-#include <Inventor/nodes/SoRotationXYZ.h>
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoTranslation.h>
-#include <Inventor/sensors/SoTimerSensor.h>
-
-#if defined(VISP_HAVE_SOWIN)
-#include <GL/gl.h>
-#elif defined(VISP_HAVE_SOQT)
-#include <qgl.h>
-#elif defined(VISP_HAVE_SOXT)
-#include <GL/gl.h>
-#endif
-
-// visp
-#include <visp/vpDebug.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-
-/*!
- \class vpViewer
-
- \brief Viewer used by the simulator.
-
- Under Windows, the viewer is based either on SoWin or SoQt. Under
- Unix, the viewer is based on SoQt or SoXt.
-
- \warning This class is only available if Coin3D and one of the GUI
- (SoWin, SoXT, SoQt) are installed.
-
-*/
-
-class vpSimulator;
-
-#if defined(VISP_HAVE_SOWIN)
-class VISP_EXPORT vpViewer : public SoWinExaminerViewer
-#elif defined(VISP_HAVE_SOQT)
-class VISP_EXPORT vpViewer : public SoQtExaminerViewer
-#elif defined(VISP_HAVE_SOXT)
-class VISP_EXPORT vpViewer : public SoXtExaminerViewer
-#endif
-{
-
- friend class vpSimulator ;
-
-public:
- typedef enum{
- internalView,
- externalView
- } vpViewerType ;
-#if defined(VISP_HAVE_SOWIN)
- vpViewer(HWND parent, vpSimulator *simu,vpViewerType viewerType);
-#elif defined(VISP_HAVE_SOQT)
- vpViewer(QWidget * parent, vpSimulator *simu,vpViewerType viewerType);
-#elif defined(VISP_HAVE_SOXT)
- vpViewer(Widget parent, vpSimulator *simu,vpViewerType viewerType);
-#endif
-
- virtual ~vpViewer();
- void resize(int x, int y, bool fixed = false) ;
- virtual void actualRedraw(void);
-
-private:
-
- vpViewerType viewerType;
- vpSimulator *simu ;
- SbBool processSoEvent(const SoEvent * const event) ;
-#if defined(VISP_HAVE_SOWIN)
- static HWND init(const char * appname) {return SoWin::init(appname);};
- static void mainLoop() {SoWin::mainLoop();};
- static void exitMainLoop() {SoWin::exitMainLoop();};
-#elif defined(VISP_HAVE_SOQT)
- static QWidget * init(const char * appname) {return SoQt::init(appname);};
- static void mainLoop() { SoQt::mainLoop();};
- static void exitMainLoop() {SoQt::exitMainLoop();};
-#elif defined(VISP_HAVE_SOXT)
- static Widget init(const char * appname) {return SoXt::init(appname);};
- static void mainLoop() { SoXt::mainLoop();};
- static void exitMainLoop() {SoXt::exitMainLoop();};
-#endif
-};
-
-#endif //VISP_HAVE_COIN_AND_GUI
-
-#endif
diff --git a/src/simulator/image-simulator/vpImageSimulator.cpp b/src/simulator/image-simulator/vpImageSimulator.cpp
deleted file mode 100644
index 462a2c7..0000000
--- a/src/simulator/image-simulator/vpImageSimulator.cpp
+++ /dev/null
@@ -1,2070 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPose.h 2453 2010-01-07 10:01:10Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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: Class which enables to project an image in the 3D space
- * and get the view of a virtual camera.
- *
- *
- * Authors:
- * Amaury Dame
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpImageSimulator.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpMbtPolygon.h>
-
-/*!
- Basic constructor.
-
- You can choose if you want to use a colored or gray scaled image.
-
- \param col : Enable to choose the color space to use for the image which is projected.
-
- By default the class uses colored images.
-*/
-vpImageSimulator::vpImageSimulator(const vpColorPlan &col)
- : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
- distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
- euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
- vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
- colorI(col), Ig(), Ic(), rect(), cleanPrevImage(false),
- setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
-{
- for(int i=0;i<4;i++)
- X[i].resize(3);
-
- for(int i=0;i<4;i++)
- X2[i].resize(3);
-
- normal_obj.resize(3);
- visible=false;
- normal_Cam.resize(3);
-
- //Xinter.resize(3);
-
- vbase_u.resize(3);
- vbase_v.resize(3);
-
- focal.resize(3);
- focal=0;
- focal[2]=1;
-
- normal_Cam_optim = new double[3];
- X0_2_optim = new double[3];
- vbase_u_optim = new double[3];
- vbase_v_optim = new double[3];
- Xinter_optim = new double[3];
-
- pt.resize(4);
-}
-
-
-/*!
- Copy constructor
-*/
-vpImageSimulator::vpImageSimulator(const vpImageSimulator &text)
- : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
- distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
- euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
- vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
- colorI(GRAY_SCALED), Ig(), Ic(), rect(), cleanPrevImage(false),
- setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
-{
- pt.resize(4);
- for(unsigned int i=0;i<4;i++)
- {
- X[i] = text.X[i];
- pt[i] = text.pt[i];
- }
-
- for(int i=0;i<4;i++)
- X2[i].resize(3);
-
- Ic = text.Ic;
- Ig = text.Ig;
-
- focal.resize(3);
- focal=0;
- focal[2]=1;
-
- normal_obj = text.normal_obj;
- euclideanNorm_u = text.euclideanNorm_u;
- euclideanNorm_v = text.euclideanNorm_v;
-
- normal_Cam.resize(3);
- vbase_u.resize(3);
- vbase_v.resize(3);
-
-
- normal_Cam_optim = new double[3];
- X0_2_optim = new double[3];
- vbase_u_optim = new double[3];
- vbase_v_optim = new double[3];
- Xinter_optim = new double[3];
-
- colorI = text.colorI;
- interp = text.interp;
- bgColor = text.bgColor;
- cleanPrevImage = text.cleanPrevImage;
- setBackgroundTexture = false;
-
- setCameraPosition(text.cMt);
-}
-
-/*!
- Basic destructor.
-*/
-vpImageSimulator::~vpImageSimulator()
-{
- delete[] normal_Cam_optim;
- delete[] X0_2_optim;
- delete[] vbase_u_optim;
- delete[] vbase_v_optim;
- delete[] Xinter_optim;
-}
-
-
-vpImageSimulator&
-vpImageSimulator::operator=(const vpImageSimulator& sim)
-{
- for(unsigned int i=0;i<4;i++)
- {
- X[i] = sim.X[i];
- pt[i] = sim.pt[i];
- }
-
- Ic = sim.Ic;
- Ig = sim.Ig;
-
- bgColor = sim.bgColor;
- cleanPrevImage = sim.cleanPrevImage;
-
- focal = sim.focal;
-
- normal_obj = sim.normal_obj;
- euclideanNorm_u = sim.euclideanNorm_u;
- euclideanNorm_v = sim.euclideanNorm_v;
-
- colorI = sim.colorI;
- interp = sim.interp;
-
- setCameraPosition(sim.cMt);
-
- return *this;
-}
-
-/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
- \param I : The image used to store the result.
- \param cam : The parameters of the virtual camera.
-*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- const vpCameraParameters &cam)
-{
- int nb_point_dessine = 0;
- if (setBackgroundTexture)
- // The Ig has been set to a previously defined background texture
- I = Ig;
- else
- {
- if (cleanPrevImage)
- {
- unsigned char col = (unsigned char) (0.2126 * bgColor.R
- + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = col;
- }
- }
- }
- }
-
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- unsigned char *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 0;
- if(getPixel(ip,Ipixelplan))
- {
- *(bitmap+i*width+j)=Ipixelplan;
- nb_point_dessine++;
- }
- }
- else if (colorI == COLORED)
- {
- vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
- nb_point_dessine++;
- }
- }
- }
- }
- }
-}
-
-
-/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
-
- \param I : The image used to store the result.
- \param Isrc : The image which is projected into \f$ I \f$.
- \param cam : The parameters of the virtual camera.
-*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- vpImage<unsigned char> &Isrc,
- const vpCameraParameters &cam)
-{
- int nb_point_dessine = 0;
- if (cleanPrevImage)
- {
- unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = col;
- }
- }
- }
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- unsigned char *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- unsigned char Ipixelplan = 0;
- if(getPixel(Isrc,ip,Ipixelplan))
- {
- *(bitmap+i*width+j)=Ipixelplan;
- nb_point_dessine++;
- }
- }
- }
- }
-}
-
-/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
-
- To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
-
- \param I : The image used to store the result.
- \param cam : The parameters of the virtual camera.
- \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
-*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- const vpCameraParameters &cam, vpMatrix &zBuffer)
-{
- if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
-
- int nb_point_dessine = 0;
- if (cleanPrevImage)
- {
- unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = col;
- }
- }
- }
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- unsigned char *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- *(bitmap+i*width+j)=Ipixelplan;
- nb_point_dessine++;
- zBuffer[i][j] = Xinter_optim[2];
- }
- }
- }
- else if (colorI == COLORED)
- {
- vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
- nb_point_dessine++;
- zBuffer[i][j] = Xinter_optim[2];
- }
- }
- }
- }
- }
- }
-}
-
-/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
-
- \param I : The image used to store the result.
- \param cam : The parameters of the virtual camera.
-*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
-{
- int nb_point_dessine = 0;
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = bgColor;
- }
- }
- }
-
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- vpRGBa *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- vpRGBa pixelcolor;
- pixelcolor.R = Ipixelplan;
- pixelcolor.G = Ipixelplan;
- pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
- nb_point_dessine++;
- }
- }
- else if (colorI == COLORED)
- {
- vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- *(bitmap+i*width+j) = Ipixelplan;
- nb_point_dessine++;
- }
- }
- }
- }
- }
-}
-
-
-/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
-
- \param I : The image used to store the result.
- \param Isrc : The image which is projected into \f$ I \f$.
- \param cam : The parameters of the virtual camera.
-*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
- const vpCameraParameters &cam)
-{
- int nb_point_dessine = 0;
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = bgColor;
- }
- }
- }
-
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- vpRGBa *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- vpRGBa Ipixelplan;
- if(getPixel(Isrc,ip,Ipixelplan))
- {
- *(bitmap+i*width+j) = Ipixelplan;
- nb_point_dessine++;
- }
- }
- }
- }
-}
-
-/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
-
- To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
-
- \param I : The image used to store the result.
- \param cam : The parameters of the virtual camera.
- \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
-*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer)
-{
- if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
-
- int nb_point_dessine = 0;
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
- I[i][j] = bgColor;
- }
- }
- }
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
- else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
- double top = rect.getTop();
- double bottom = rect.getBottom();
- double left = rect.getLeft();
- double right= rect.getRight();
-
- vpRGBa *bitmap = I.bitmap;
- unsigned int width = I.getWidth();
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
- {
- for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- vpRGBa pixelcolor;
- pixelcolor.R = Ipixelplan;
- pixelcolor.G = Ipixelplan;
- pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
- nb_point_dessine++;
- zBuffer[i][j] = Xinter_optim[2];
- }
- }
- }
- else if (colorI == COLORED)
- {
- vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- *(bitmap+i*width+j) = Ipixelplan;
- nb_point_dessine++;
- zBuffer[i][j] = Xinter_optim[2];
- }
- }
- }
- }
- }
- }
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated, you should use:
- vpImageSimulator::getImage(vpImage<unsigned char> &,
- std::list<vpImageSimulator> &,
- const vpCameraParameters &) instead.
-
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
-
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
-
-
- The following example shows how to use the method:
-
- \code
- #include <visp/vpImage.h>
- #include <visp/vpImageSimulator.h>
-
- int main()
- {
- vpImage<vpRGBa> Icamera(480,640,0);
- vpImage<vpRGBa> Iimage(60,60);
-
- // Initialise the image which will be projected into the image Icamera
- vpRGBa colorb(0,0,255);
- vpRGBa colorw(255,255,255);
- vpRGBa colorr(255,0,0);
- for(int i = 0; i < 60; i++)
- {
- for(int j = 0; j < 20; j++)
- Iimage[i][j] = colorb;
- for(int j = 20; j < 40; j++)
- Iimage[i][j] = colorw;
- for(int j = 40; j < 60; j++)
- Iimage[i][j] = colorr;
- }
-
- // Initialise the 3D coordinates of the Iimage corners
- vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim;
- sim.init(Iimage, X);
-
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim2;
- sim2.init(Iimage, X);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
- sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
-
- vpList<vpImageSimulator> listSim;
- listSim.addRight(sim);
- listSim.addRight(sim2);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
- vpCameraParameters cam(868.0, 869.0, 320, 240);
-
- vpImageSimulator::getImage(Icamera,listSim,cam);
-
- return 0;
- }
- \endcode
-
- \param I : The image used to store the result
- \param list : List of vpImageSimulator to project
- \param cam : The parameters of the virtual camera
-*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- vpList<vpImageSimulator> &list,
- const vpCameraParameters &cam)
-{
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- unsigned int nbsimList = list.nbElements();
-
- if (nbsimList < 1)
- return;
-
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
-
- double topFinal = height+1;;
- double bottomFinal = -1;
- double leftFinal = width+1;
- double rightFinal = -1;
-
- list.front();
-
- unsigned int unvisible = 0;
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- vpImageSimulator* sim = &(list.value());
- list.next();
- if (sim->visible)
- simList[i] = sim;
- else
- unvisible++;
- }
- nbsimList = nbsimList - unvisible;
-
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
-
-
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
- else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
- }
-
- double zmin = -1;
- int indice = -1;
- unsigned char *bitmap = I.bitmap;
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
- {
- for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
- {
- zmin = -1;
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- *(bitmap+i*width+j)=Ipixelplan;
- }
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
- }
- }
- }
- }
-
- delete[] simList;
-}
-
-
-/*!
- \deprecated This method is deprecated, you should use:
- vpImageSimulator::getImage(vpImage<vpRGBa> &,
- std::list<vpImageSimulator> &,
- const vpCameraParameters &) instead.
-
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
-
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
-
- The following example shows how to use the method:
-
- \code
- #include <visp/vpImage.h>
- #include <visp/vpImageSimulator.h>
-
- int main()
- {
- vpImage<vpRGBa> Icamera(480,640,0);
- vpImage<vpRGBa> Iimage(60,60);
-
- // Initialise the image which will be projected into the image Icamera
- vpRGBa colorb(0,0,255);
- vpRGBa colorw(255,255,255);
- vpRGBa colorr(255,0,0);
- for(int i = 0; i < 60; i++)
- {
- for(int j = 0; j < 20; j++)
- Iimage[i][j] = colorb;
- for(int j = 20; j < 40; j++)
- Iimage[i][j] = colorw;
- for(int j = 40; j < 60; j++)
- Iimage[i][j] = colorr;
- }
-
- // Initialise the 3D coordinates of the Iimage corners
- vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim;
- sim.init(Iimage, X);
-
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim2;
- sim2.init(Iimage, X);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
- sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
-
- vpList<vpImageSimulator> listSim;
- listSim.addRight(sim);
- listSim.addRight(sim2);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
- vpCameraParameters cam(868.0, 869.0, 320, 240);
-
- vpImageSimulator::getImage(Icamera,listSim,cam);
-
- return 0;
- }
- \endcode
-
- \param I : The image used to store the result
- \param list : List of vpImageSimulator to project
- \param cam : The parameters of the virtual camera
-*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpList<vpImageSimulator> &list,
- const vpCameraParameters &cam)
-{
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- unsigned int nbsimList = list.nbElements();
-
- if (nbsimList < 1)
- return;
-
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
-
- double topFinal = height+1;;
- double bottomFinal = -1;
- double leftFinal = width+1;
- double rightFinal = -1;
-
- list.front();
-
- unsigned int unvisible = 0;
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- vpImageSimulator* sim = &(list.value());
- list.next();
- if (sim->visible)
- simList[i] = sim;
- else
- unvisible++;
- }
- nbsimList = nbsimList - unvisible;
-
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
-
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
- else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
- }
-
- double zmin = -1;
- int indice = -1;
- vpRGBa *bitmap = I.bitmap;
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
- {
- for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
- {
- zmin = -1;
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- vpRGBa pixelcolor;
- pixelcolor.R = Ipixelplan;
- pixelcolor.G = Ipixelplan;
- pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
- }
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- //unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B;
- *(bitmap+i*width+j)=Ipixelplan;
- }
- }
- }
- }
-
- delete[] simList;
-}
-#endif
-
-/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
-
- The following example shows how to use the method:
-
- \code
- #include <list>
- #include <visp/vpImage.h>
- #include <visp/vpImageSimulator.h>
-
- int main()
- {
- vpImage<vpRGBa> Icamera(480,640,0);
- vpImage<vpRGBa> Iimage(60,60);
-
- // Initialise the image which will be projected into the image Icamera
- vpRGBa colorb(0,0,255);
- vpRGBa colorw(255,255,255);
- vpRGBa colorr(255,0,0);
- for(int i = 0; i < 60; i++)
- {
- for(int j = 0; j < 20; j++)
- Iimage[i][j] = colorb;
- for(int j = 20; j < 40; j++)
- Iimage[i][j] = colorw;
- for(int j = 40; j < 60; j++)
- Iimage[i][j] = colorr;
- }
-
- // Initialise the 3D coordinates of the Iimage corners
- vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim;
- sim.init(Iimage, X);
-
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim2;
- sim2.init(Iimage, X);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
- sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
-
- std::list<vpImageSimulator> listSim;
- listSim.addRight(sim);
- listSim.addRight(sim2);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
- vpCameraParameters cam(868.0, 869.0, 320, 240);
-
- vpImageSimulator::getImage(Icamera,listSim,cam);
-
- return 0;
- }
- \endcode
-
- \param I : The image used to store the result
- \param list : List of vpImageSimulator to project
- \param cam : The parameters of the virtual camera
-*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam)
-{
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- unsigned int nbsimList = (unsigned int)list.size();
-
- if (nbsimList < 1)
- return;
-
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
-
- double topFinal = height+1;;
- double bottomFinal = -1;
- double leftFinal = width+1;
- double rightFinal = -1;
-
- unsigned int unvisible = 0;
- unsigned int indexSimu=0;
- for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
- vpImageSimulator* sim = &(*it);
- if (sim->visible)
- simList[indexSimu] = sim;
- else
- unvisible++;
- }
- nbsimList = nbsimList - unvisible;
-
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
-
-
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
- else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
- }
-
- double zmin = -1;
- int indice = -1;
- unsigned char *bitmap = I.bitmap;
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
- {
- for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
- {
- zmin = -1;
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- *(bitmap+i*width+j)=Ipixelplan;
- }
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
- }
- }
- }
- }
-
- delete[] simList;
-}
-
-
-/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
-
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
-
- The following example shows how to use the method:
-
- \code
- #include <list>
- #include <visp/vpImage.h>
- #include <visp/vpImageSimulator.h>
-
- int main()
- {
- vpImage<vpRGBa> Icamera(480,640,0);
- vpImage<vpRGBa> Iimage(60,60);
-
- // Initialise the image which will be projected into the image Icamera
- vpRGBa colorb(0,0,255);
- vpRGBa colorw(255,255,255);
- vpRGBa colorr(255,0,0);
- for(int i = 0; i < 60; i++)
- {
- for(int j = 0; j < 20; j++)
- Iimage[i][j] = colorb;
- for(int j = 20; j < 40; j++)
- Iimage[i][j] = colorw;
- for(int j = 40; j < 60; j++)
- Iimage[i][j] = colorr;
- }
-
- // Initialise the 3D coordinates of the Iimage corners
- vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim;
- sim.init(Iimage, X);
-
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 1;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 1;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 1;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 1;
-
- vpImageSimulator sim2;
- sim2.init(Iimage, X);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
- sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));
-
- std::list<vpImageSimulator> listSim;
- listSim.addRight(sim);
- listSim.addRight(sim2);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
- vpCameraParameters cam(868.0, 869.0, 320, 240);
-
- vpImageSimulator::getImage(Icamera,listSim,cam);
-
- return 0;
- }
- \endcode
-
- \param I : The image used to store the result
- \param list : List of vpImageSimulator to project
- \param cam : The parameters of the virtual camera
-*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam)
-{
-
- unsigned int width = I.getWidth();
- unsigned int height = I.getHeight();
-
- unsigned int nbsimList = (unsigned int)list.size();
-
- if (nbsimList < 1)
- return;
-
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
-
- double topFinal = height+1;;
- double bottomFinal = -1;
- double leftFinal = width+1;
- double rightFinal = -1;
-
- unsigned int unvisible = 0;
- unsigned int indexSimu = 0;
- for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
- vpImageSimulator* sim = &(*it);
- if (sim->visible)
- simList[indexSimu] = sim;
- else
- unvisible++;
- }
-
- nbsimList = nbsimList - unvisible;
-
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
-
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
- else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
- }
-
- double zmin = -1;
- int indice = -1;
- vpRGBa *bitmap = I.bitmap;
- vpImagePoint ip;
-
- for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++)
- {
- for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++)
- {
- zmin = -1;
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- vpRGBa pixelcolor;
- pixelcolor.R = Ipixelplan;
- pixelcolor.G = Ipixelplan;
- pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
- }
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- //unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B;
- *(bitmap+i*width+j)=Ipixelplan;
- }
- }
- }
- }
-
- delete[] simList;
-}
-
-/*!
- Enable to set the position of the 3D plane relative to the virtual camera.
-
- \param cMt_ : The pose of the plane relative to the virtual camera.
-*/
-void
-vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
-{
- cMt = cMt_;
- vpRotationMatrix R;
- cMt.extract(R);
- needClipping = false;
-
- normal_Cam = R * normal_obj;
-
- visible_result = vpColVector::dotProd(normal_Cam,focal);
-
- for(unsigned int i = 0; i < 4; i++)
- pt[i].track(cMt);
-
- vpColVector e1(3) ;
- vpColVector e2(3) ;
- vpColVector facenormal(3) ;
-
- e1[0] = pt[1].get_X() - pt[0].get_X() ;
- e1[1] = pt[1].get_Y() - pt[0].get_Y() ;
- e1[2] = pt[1].get_Z() - pt[0].get_Z() ;
-
- e2[0] = pt[2].get_X() - pt[1].get_X() ;
- e2[1] = pt[2].get_Y() - pt[1].get_Y() ;
- e2[2] = pt[2].get_Z() - pt[1].get_Z() ;
-
- facenormal = vpColVector::crossProd(e1,e2) ;
-
- double angle = pt[0].get_X()*facenormal[0] + pt[0].get_Y()*facenormal[1] + pt[0].get_Z()*facenormal[2] ;
-
- if (angle > 0){
- visible=true;
- }
- else {
- visible=false;
- }
-
- if(visible)
- {
- for(unsigned int i = 0; i < 4; i++)
- {
- project(X[i],cMt,X2[i]);
- pt[i].track(cMt);
- if(pt[i].get_Z() < 0)
- needClipping = true;
- }
-
- vbase_u = X2[1]-X2[0];
- vbase_v = X2[3]-X2[0];
-
- distance = vpColVector::dotProd(normal_Cam,X2[1]);
-
-
- if(distance < 0)
- {
- visible = false;
- return;
- }
-
- for(unsigned int i = 0; i < 3; i++)
- {
- normal_Cam_optim[i] = normal_Cam[i];
- X0_2_optim[i] = X2[0][i];
- vbase_u_optim[i] = vbase_u[i];
- vbase_v_optim[i] = vbase_v[i];
- }
-
- std::vector<vpPoint> *ptPtr = &pt;
- if(needClipping){
- vpMbtPolygon::getClippedPolygon(pt,ptClipped,cMt,vpMbtPolygon::NEAR_CLIPPING);
- ptPtr = &ptClipped;
- }
-
- listTriangle.clear();
- for(unsigned int i = 1 ; i < (*ptPtr).size()-1 ; i++){
- vpImagePoint ip1, ip2, ip3;
- ip1.set_j((*ptPtr)[0].get_x());
- ip1.set_i((*ptPtr)[0].get_y());
-
- ip2.set_j((*ptPtr)[i].get_x());
- ip2.set_i((*ptPtr)[i].get_y());
-
- ip3.set_j((*ptPtr)[i+1].get_x());
- ip3.set_i((*ptPtr)[i+1].get_y());
-
- vpTriangle tri(ip1,ip2,ip3);
- listTriangle.push_back(tri);
- }
- }
-}
-
-void
-vpImageSimulator::initPlan(vpColVector* X_)
-{
- for (unsigned int i = 0; i < 4; i++)
- {
- X[i]=X_[i];
- pt[i].setWorldCoordinates(X_[i][0],X_[i][1],X_[i][2]);
- }
-
- normal_obj=vpColVector::crossProd(X[1]-X[0],X[3]-X[0]);
- normal_obj=normal_obj/normal_obj.euclideanNorm();
-
- euclideanNorm_u=(X[1]-X[0]).euclideanNorm();
- euclideanNorm_v=(X[3]-X[0]).euclideanNorm();
-}
-
-/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- The table must have a size of 4!
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param I : The image which is projected.
- \param X_ : table of the 3D coordinates corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const vpImage<unsigned char> &I,vpColVector* X_)
-{
- Ig = I;
- vpImageConvert::convert(I,Ic);
- initPlan(X_);
-}
-
-/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- The table must have a size of 4!
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param I : The image which is projected.
- \param X_ : table of the 3D coordinates corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const vpImage<vpRGBa> &I,vpColVector* X_)
-{
- Ic = I;
- vpImageConvert::convert(I,Ig);
- initPlan(X_);
-}
-
-/*!
- Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- The table must have a size of 4!
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param file_image : The adress of an image file.
- \param X_ : table of the 3D coordinates corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const char* file_image,vpColVector* X_)
-{
- vpImageIo::read(Ig,file_image);
- vpImageIo::read(Ic,file_image);
- initPlan(X_);
-}
-
-/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- \throw vpException::dimensionError if the _X vector is not of size 4.
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param I : The image which is projected.
- \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
- corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint>& X_)
-{
- if(X_.size() != 4){
- throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
- }
- vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
- Xvec[i].resize(3);
- Xvec[i][0] = X_[i].get_oX();
- Xvec[i][1] = X_[i].get_oY();
- Xvec[i][2] = X_[i].get_oZ();
- }
-
- Ig = I;
- vpImageConvert::convert(I,Ic);
- initPlan(Xvec);
-}
-/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- \throw vpException::dimensionError if the X_ vector is not of size 4.
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param I : The image which is projected.
- \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
- corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X_)
-{
- if(X_.size() != 4){
- throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
- }
- vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
- Xvec[i].resize(3);
- Xvec[i][0] = X_[i].get_oX();
- Xvec[i][1] = X_[i].get_oY();
- Xvec[i][2] = X_[i].get_oZ();
- }
-
- Ic = I;
- vpImageConvert::convert(I,Ig);
- initPlan(Xvec);
-}
-/*!
- Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
- \throw vpException::dimensionError if the X_ vector is not of size 4.
-
- - \f$ X[0] \f$ :Top left corner.
- - \f$ X[1] \f$ :Top right corner.
- - \f$ X[2] \f$ :Bottom right corner.
- - \f$ X[3] \f$ :Bottom left corner.
-
- \param file_image : The adress of an image file.
- \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
- corresponding to the image corners.
-*/
-void
-vpImageSimulator::init(const char* file_image, const std::vector<vpPoint>& X_)
-{
- if(X_.size() != 4){
- throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
- }
- vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
- Xvec[i].resize(3);
- Xvec[i][0] = X_[i].get_oX();
- Xvec[i][1] = X_[i].get_oY();
- Xvec[i][2] = X_[i].get_oZ();
- }
-
- vpImageIo::read(Ig,file_image);
- vpImageIo::read(Ic,file_image);
- initPlan(Xvec);
-}
-
-bool
-vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
-{
-// std::cout << "In get Pixel" << std::endl;
- //test si pixel dans zone projetee
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
-//// std::cout << "The pixel is inside the projected area" << std::endl;
-// return false;}
-
- //methoed algebrique
- double z;
-
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
-
- //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])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
- double u = 0, v = 0;
- double diff = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
- }
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
-
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Ig.getHeight()-1);
- j2=u*(Ig.getWidth()-1);
- if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Ig.getValue(i2,j2);
- else if (interp == SIMPLE)
- Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
- return true;
- }
- else
- return false;
-}
-
-bool
-vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc,
- const vpImagePoint &iP, unsigned char &Ipixelplan)
-{
- //test si pixel dans zone projetee
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
-
- //methoed algebrique
- double z;
-
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
-
- //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])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
- double u = 0, v = 0;
- double diff = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
- }
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
-
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Isrc.getHeight()-1);
- j2=u*(Isrc.getWidth()-1);
- if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Isrc.getValue(i2,j2);
- else if (interp == SIMPLE)
- Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
- return true;
- }
- else
- return false;
-}
-
-
-bool
-vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
-{
- //test si pixel dans zone projetee
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
-
- //methoed algebrique
- double z;
-
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
-
- //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])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
- double u = 0, v = 0;
- double diff = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
- }
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
-
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Ic.getHeight()-1);
- j2=u*(Ic.getWidth()-1);
- if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Ic.getValue(i2,j2);
- else if (interp == SIMPLE)
- Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
- return true;
- }
- else
- return false;
-}
-
-bool
-vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
- vpRGBa &Ipixelplan)
-{
- //test si pixel dans zone projetee
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-
- //methoed algebrique
- double z;
-
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
-
- //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])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
- double u = 0, v = 0;
- double diff = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
- }
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
-
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Isrc.getHeight()-1);
- j2=u*(Isrc.getWidth()-1);
- if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Isrc.getValue(i2,j2);
- else if (interp == SIMPLE)
- Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
- return true;
- }
- else
- return false;
-}
-
-bool
-vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan)
-{
- //test si pixel dans zone projetee
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
-
- Zpixelplan = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- return true;
-}
-
-bool
-vpImageSimulator::getPixelVisibility(const vpImagePoint &iP,
- double &Visipixelplan)
-{
- //test si pixel dans zone projetee
- bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
-
- Visipixelplan = visible_result;
- return true;
-}
-
-void
-vpImageSimulator::project(const vpColVector &_vin,
- const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
-{
- vpColVector XH(4);
- getHomogCoord(_vin,XH);
- getCoordFromHomog(_cMt*XH,_vout);
-}
-
-void
-vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
-{
- for(unsigned int i=0;i<3;i++)
- _vH[i]=_v[i];
- _vH[3]=1.;
-}
-
-void
-vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, vpColVector &_v)
-{
- for(unsigned int i=0;i<3;i++)
- _v[i]=_vH[i]/_vH[3];
-}
-
-
-void
-vpImageSimulator::getRoi(const unsigned int &Iwidth,
- const unsigned int &Iheight,
- const vpCameraParameters &cam,
- const std::vector<vpPoint> &point,
- vpRect &rectangle)
-{
- double top = Iheight+1;
- double bottom = -1;
- double right = -1;
- double left= Iwidth+1;
-
- for( unsigned int i = 0; i < point.size(); i++)
- {
- double u=0,v=0;
- vpMeterPixelConversion::convertPoint(cam,point[i].get_x(),point[i].get_y(),u,v);
- if (v < top) top = v;
- if (v > bottom) bottom = v;
- if (u < left) left = u;
- if (u > right) right = u;
- }
- if (top < 0) top = 0;
- if(top >= Iheight) top = Iheight-1;
- if (bottom < 0) bottom = 0;
- if(bottom >= Iheight) bottom = Iheight-1;
- if(left < 0) left = 0;
- if(left >= Iwidth) left = Iwidth-1;
- if(right < 0) right = 0;
- if(right >= Iwidth) right = Iwidth-1;
-
- rectangle.setTop(top);
- rectangle.setBottom(bottom);
- rectangle.setLeft(left);
- rectangle.setRight(right);
-}
-
-std::vector<vpColVector>
-vpImageSimulator::get3DcornersTextureRectangle()
-{
- std::vector<vpColVector> X_;
- for (int i=0; i<4; i++)
- X_.push_back(X[i]);
- return X_;
-}
-
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/)
-{
- os << "";
- return os;
-}
diff --git a/src/simulator/image-simulator/vpImageSimulator.h b/src/simulator/image-simulator/vpImageSimulator.h
deleted file mode 100644
index 054a519..0000000
--- a/src/simulator/image-simulator/vpImageSimulator.h
+++ /dev/null
@@ -1,327 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPose.h 2453 2010-01-07 10:01:10Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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: Class which enables to project an image in the 3D space
- * and get the view of a virtual camera.
- *
- *
- * Authors:
- * Amaury Dame
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpImageSimulator_h
-#define vpImageSimulator_h
-
-/*!
- \class vpImageSimulator
-
- \ingroup ImageSimulator
-
- \brief Class which enables to project an image in the 3D space and get the view of a virtual camera.
-
- The image is represented by a rectangular image whose corners coordinates are known in the 3D frame linked to the 3D rectangle.
-
- The 3D rectangle is positionned relative to a virtual camera (represented by its intrinsic parameters). Indeed, the pose \f$ cMt \f$ has to be given by the user of the class.
-
- And finally, the view of the virtual camera is given by the geImage() method.
-
- You can use a colored or a gray scaled image.
-
- To avoid the aliasing especially when the camera is very near from the image plane, a bilinear interpolation can be done for every pixels which have to be filled in. By default this functionality is not used because it consumes lot of time.
-
- The following example explain how to use the class.
-
- \code
- #include <visp/vpImage.h>
- #include <visp/vpImageSimulator.h>
-
- int main()
- {
- vpImage<vpRGBa> Icamera(480,640,0);
- vpImage<vpRGBa> Iimage(60,60);
-
- // Initialise the image which will be projected into the image Icamera
- vpRGBa colorb(0,0,255);
- vpRGBa colorw(255,255,255);
- vpRGBa colorr(255,0,0);
- for(int i = 0; i < 60; i++)
- {
- for(int j = 0; j < 20; j++)
- Iimage[i][j] = colorb;
- for(int j = 20; j < 40; j++)
- Iimage[i][j] = colorw;
- for(int j = 40; j < 60; j++)
- Iimage[i][j] = colorr;
- }
-
- // Initialise the 3D coordinates of the Iimage corners
- vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
- // Top left corner
- X[0][0] = -1;
- X[0][1] = -1;
- X[0][2] = 0;
-
- // Top right corner
- X[1][0] = 1;
- X[1][1] = -1;
- X[1][2] = 0;
-
- // Bottom right corner
- X[2][0] = 1;
- X[2][1] = 1;
- X[2][2] = 0;
-
- //Bottom left corner
- X[3][0] = -1;
- X[3][1] = 1;
- X[3][2] = 0;
-
- vpImageSimulator sim;
- sim.init(Iimage, X);
-
- sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
- vpCameraParameters cam(868.0, 869.0, 320, 240);
-
- sim.getImage(Icamera,cam);
-
- return 0;
- }
- \endcode
-*/
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpTriangle.h>
-#include <visp/vpRect.h>
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPoint.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-#include <vector>
-#include <list>
-
-class VISP_EXPORT vpImageSimulator
-{
- public:
- typedef enum {
- COLORED,
- GRAY_SCALED
- } vpColorPlan;
-
- typedef enum {
- SIMPLE,
- BILINEAR_INTERPOLATION
- } vpInterpolationType;
-
- private:
- vpColVector X[4];
- vpHomogeneousMatrix cMt;
- vpColVector X2[4];
- std::vector<vpPoint> pt;
- std::vector<vpPoint> ptClipped;
-
- vpInterpolationType interp;
-
- //normal repere objet
- vpColVector normal_obj;
- //normal repere cam
- vpColVector normal_Cam;
- //normal repere cam sous forme de pointeur sinon vpColVector prend trop de temps
- double *normal_Cam_optim;
-
- //distance du plan au centre de camera
- double distance;
- double visible_result;
- bool visible;
-
-
- //point 3D du plan definissant ou sera le pixel (0,0) de l'image
- double *X0_2_optim;
-
- //variable pour intersection algebre : vecteurs de la base 2D et leur norme
- double euclideanNorm_u,euclideanNorm_v;
-
- //2 vecteur 3D (typiquement deux cotes du plan) qui definissent le repere 2D u,v de l'image
- vpColVector vbase_u,vbase_v;
- //version pointeur des vecteurs de base
- double *vbase_u_optim;
- double *vbase_v_optim;
-
- //allocation memoire du point 3D d'intersection entre le vecteur (centre_cam - point_plan_image) et le plan
- double *Xinter_optim;
-
- //triangles de projection du plan
- std::vector<vpTriangle> listTriangle;
-
- //image de texture
- vpColorPlan colorI;
- vpImage<unsigned char> Ig;
- vpImage<vpRGBa> Ic;
-
- vpRect rect;
- bool cleanPrevImage;
- bool setBackgroundTexture; // flag set when the background is to a texture using setBackGroundTexture()
- vpColor bgColor;
-
- vpColVector focal;
-
- //boolean to tell if the points in the camera frame have to be clipped
- bool needClipping;
-
- public:
- vpImageSimulator(const vpColorPlan &col = COLORED);
- vpImageSimulator(const vpImageSimulator &text);
- virtual ~vpImageSimulator();
-
- vpImageSimulator& operator=(const vpImageSimulator& sim);
-
-
- //creation du plan a partir de ses coordonnees 3D ds repere objet et de son image texture
- void init(const vpImage<unsigned char> &I,vpColVector* X);
- void init(const vpImage<vpRGBa> &I,vpColVector* X);
- void init(const char* file_image,vpColVector* X);
- void init(const vpImage<unsigned char> &I, const std::vector<vpPoint>& X);
- void init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X);
- void init(const char* file_image, const std::vector<vpPoint>& X);
-
- //projection du plan par cMo => creation des deux triangles definissant projection du plan sur plan image (coord en metre)
- void setCameraPosition(const vpHomogeneousMatrix &cMt);
-
- void setInterpolationType (const vpInterpolationType interplt) {this->interp = interplt;}
-
- void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam);
- void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam);
-
- void getImage(vpImage<unsigned char> &I, vpImage<unsigned char> &Isrc,
- const vpCameraParameters &cam);
- void getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
- const vpCameraParameters &cam);
-
- void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer);
- void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer);
-
- static void getImage(vpImage<unsigned char> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam);
- static void getImage(vpImage<vpRGBa> &I,
- std::list <vpImageSimulator> &list,
- const vpCameraParameters &cam);
-
- std::vector<vpColVector> get3DcornersTextureRectangle();
-
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/);
-
- /*!
- As it can be time consuming to reset all the image to a default baground value, this function enable to reset only the pixel which changed the previous time.
-
- By default this functionality is disabled. and the background color is white.
-
- \param clean : Enable the reset method.
- \param color : Color of the back ground.
- */
- void setCleanPreviousImage(const bool &clean, const vpColor &color = vpColor::white) {
- cleanPrevImage = clean;
- bgColor = color;
- }
-
- /*!
- This function is to set the background to a texture instead of default black
- The corresponding flag is set
- \param Iback: Image/Texture for the background
- */
- inline void
- setBackGroundTexture(const vpImage<unsigned char>& Iback) {
- setBackgroundTexture = true;
- Ig = Iback;
- }
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated static void getImage(vpImage<unsigned char> &I,
- vpList<vpImageSimulator> &list,
- const vpCameraParameters &cam);
- vp_deprecated static void getImage(vpImage<vpRGBa> &I, vpList<vpImageSimulator> &list,
- const vpCameraParameters &cam);
-#endif
-
- private:
- void initPlan(vpColVector* X);
-
- //result = plan est visible.
- //ie: un plan est oriente dans si normal_plan.focal < 0 => plan est visible sinon invisible.
- bool isVisible() {return visible;}
-
- //function that project a point x,y on the plane, return true if the projection is on the limited plane
- // and in this case return the corresponding image pixel Ipixelplan
- bool getPixel(const vpImagePoint &iP,unsigned char &Ipixelplan);
- bool getPixel(const vpImagePoint &iP,vpRGBa &Ipixelplan);
- bool getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP,
- unsigned char &Ipixelplan);
- bool getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
- vpRGBa &Ipixelplan);
- bool getPixelDepth(const vpImagePoint &iP, double &Zpixelplan);
- bool getPixelVisibility(const vpImagePoint &iP, double &Zpixelplan);
-
- //operation 3D de base :
- void project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt,
- vpColVector &_vout);
- //donne coordonnes homogenes de _v;
- void getHomogCoord(const vpColVector &_v, vpColVector &_vH);
- //donne coordonnes _v en fction coord homogenes _vH;
- void getCoordFromHomog(const vpColVector &_vH, vpColVector &_v);
-
- void getRoi(const unsigned int &Iwidth, const unsigned int &Iheight,
- const vpCameraParameters &cam, const std::vector<vpPoint> &point, vpRect &rect);
-};
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-
diff --git a/src/simulator/ogre-simulator/vpAROgre.cpp b/src/simulator/ogre-simulator/vpAROgre.cpp
deleted file mode 100644
index 074a0e6..0000000
--- a/src/simulator/ogre-simulator/vpAROgre.cpp
+++ /dev/null
@@ -1,1072 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAROgre.cpp 5234 2015-01-30 13:51:02Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Augmented Reality viewer using Ogre3D.
- *
- * Authors:
- * Bertrand Delabarre
- *
- *****************************************************************************/
-
-/*!
- \file vpAROgre.cpp
-
- \brief Class that implements augmented reality viewer based on Ogre3D.
-
- \warning The content of this file is only available if Ogre3D and
- one of the renderer (OpenGL or Direct3Dt) are installed.
-
-*/
-
-#include "visp/vpConfig.h"
-
-#ifdef VISP_HAVE_OGRE
-
-#include "visp/vpAROgre.h"
-#include "visp/vpIoTools.h"
-
-
-/*!
- Constructor.
-
- \param cam : Camera parameters.
-
- \param width : Width of the window.
- \param height : Height of the window.
-
- \param resourcePath : Path to the resources.cfg file telling Ogre where to
- look for resources.
-
- \param pluginsPath : Path to the plugins.cfg of plugins_d.cfg files
- telling Ogre where to look for renderer plugins.
-
-*/
-vpAROgre::vpAROgre(const vpCameraParameters &cam,
- unsigned int width, unsigned int height,
- const char *resourcePath, const char *pluginsPath)
- : name("ViSP - Augmented Reality"),mRoot(0), mCamera(0), mSceneMgr(0), mWindow(0),
- mResourcePath(resourcePath), mPluginsPath(pluginsPath),
-#ifdef VISP_HAVE_OIS
- mInputManager(0), mKeyboard(0),
-#endif
- keepOn(true), // When created no reason to stop displaying
- mImageRGBA(), mImage(), mPixelBuffer(NULL), mBackground(NULL), mBackgroundHeight(0),
- mBackgroundWidth(0), mWindowHeight(height), mWindowWidth(width), windowHidden(false),
- mNearClipping(0.001), mFarClipping(200), mcam(cam), mshowConfigDialog(true),
- mOptionnalResourceLocation()
-{
-}
-
-/*!
- Initialisation of Ogre with a grey level background.
-
- Load the plugins that are specified in the plugins.cfg or
- plugins_d.cfg files. These files are located in
- VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
- Note that plugins.cfg file is always considered under Unix
- platforms. The file plugins_d.cfg is only considered under Windows
- when the build type is Debug.
-
- Load also the resources that are defined in the resources.cfg
- file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
- that is defined in vpConfig.h.
-
- Create also the grey level background used to display the image.
-
- \param I : Image that is displayed in the background.
-
- \param bufferedKeys : If true, use of buffered input for the keyboard (see
- Ogre documentation). Note that this parameter is only useful if OIS is used.
-
- \param hidden : If true, the created window will be hidden.
- Note that this functionnality requires Ogre3D 1.8.1 at least.
-
- \exception vpException::ioError : If the required plugins.cfg /
- plugins_d.cfg or resources.cfg files are not accessible.
-
-*/
-void vpAROgre::init(vpImage<unsigned char> &I,
- bool
-#ifdef VISP_HAVE_OIS
- bufferedKeys
-#endif
- ,bool hidden
- )
-{
- mBackgroundWidth = I.getWidth();
- mBackgroundHeight = I.getHeight();
-
- init(
-#ifdef VISP_HAVE_OIS
- bufferedKeys,
-#else
- false,
-#endif
- hidden
- );
- // Create the background image which will come from the grabber
- createBackground(I);
-}
-
-/*!
- Initialisation of Ogre with a color background.
-
- Load the plugins that are specified in the plugins.cfg or
- plugins_d.cfg files. These files are located in
- VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
- Note that plugins.cfg file is always considered under Unix
- platforms. The file plugins_d.cfg is only considered under Windows
- when the build type is Debug.
-
- Load also the resources that are defined in the resources.cfg
- file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
- that is defined in vpConfig.h.
-
- Create also a color background used to display the image.
-
- \param I : Image that is displayed in the background.
-
- \param bufferedKeys : If true, use of buffered input for the keyboard (see
- Ogre documentation). Note that this parameter is only useful if OIS is used.
-
- \param hidden : If true, the created window will be hidden.
- Note that this functionnality requires Ogre3D 1.8.1 at least.
-
- \exception vpException::ioError : If the required plugins.cfg /
- plugins_d.cfg or resources.cfg files are not accessible.
-
-*/
-void vpAROgre::init(vpImage<vpRGBa> &I,
- bool
-#ifdef VISP_HAVE_OIS
- bufferedKeys
-#endif
- ,bool hidden
- )
-{
- mBackgroundWidth = I.getWidth();
- mBackgroundHeight = I.getHeight();
-
- init(
-#ifdef VISP_HAVE_OIS
- bufferedKeys,
-#else
- false,
-#endif
- hidden
- );
- // Create the background image which will come from the grabber
- createBackground(I);
-}
-
-/*!
- Initialisation of Ogre.
-
- Load the plugins that are specified in the plugins.cfg or
- plugins_d.cfg files. These files are located in
- VISP_HAVE_OGRE_PLUGINS_PATH folder that is defined in vpConfig.h.
- Note that plugins.cfg file is always considered under Unix
- platforms. The file plugins_d.cfg is only considered under Windows
- when the build type is Debug.
-
- Load also the resources that are defined in the resources.cfg
- file. This file is located in VISP_HAVE_OGRE_RESOURCES_PATH folder
- that is defined in vpConfig.h.
-
- \param bufferedKeys : If true, use of buffered input for the keyboard (see
- Ogre documentation). Note that this parameter is only useful if OIS is used.
-
- \param hidden : If true, the created window will be hidden.
- Note that this functionnality requires Ogre3D 1.8.1 at least.
-
- \exception vpException::ioError : If the required plugins.cfg /
- plugins_d.cfg or resources.cfg files are not accessible.
-
-*/
-void vpAROgre::init(bool
-#ifdef VISP_HAVE_OIS
- bufferedKeys
-#endif
- ,bool hidden
- )
-{
- // Create the root
-#if defined(NDEBUG) || !defined(_WIN32)
- std::string pluginFile = mPluginsPath+"/plugins.cfg";
-#else
- std::string pluginFile = mPluginsPath+"/plugins_d.cfg";
-#endif
- if(!vpIoTools::checkFilename(pluginFile)){
- std::string errorMsg = "Error: the requested plugins file \""
- + pluginFile + "\" doesn't exist.";
- std::cout << errorMsg << std::endl;
-
- throw (vpException(vpException::ioError, errorMsg));
- }
- std::cout << "######################### Load plugin file: " << pluginFile << std::endl;
-
- if(Ogre::Root::getSingletonPtr() == NULL)
- mRoot = new Ogre::Root(pluginFile, "ogre.cfg", "Ogre.log");
- else
- mRoot = Ogre::Root::getSingletonPtr();
-
- // Load resource paths from config file
-
- // File format is:
- // [ResourceGroupName]
- // ArchiveType=Path
- // .. repeat
- // For example:
- // [General]
- // FileSystem=media/
- // Zip=packages/level1.zip
- Ogre::ConfigFile cf;
- std::string resourceFile = mResourcePath+"/resources.cfg";
- if(!vpIoTools::checkFilename(resourceFile)){
- std::string errorMsg = "Error: the requested resource file \""
- + resourceFile + "\" doesn't exist.";
- std::cout << errorMsg << std::endl;
-
- throw (vpException(vpException::ioError, errorMsg));
- }
- std::cout << "######################### Load resource file: " << resourceFile << std::endl;
- cf.load(resourceFile);
-
- // Go through all sections & settings in the file
- Ogre::ConfigFile::SectionIterator seci = cf.getSectionIterator();
-
- Ogre::String secName, typeName, archName;
- while (seci.hasMoreElements())
- {
- secName = seci.peekNextKey();
- Ogre::ConfigFile::SettingsMultiMap *settings = seci.getNext();
- Ogre::ConfigFile::SettingsMultiMap::iterator i;
- for (i = settings->begin(); i != settings->end(); ++i)
- {
- typeName = i->first;
- archName = i->second;
- Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
- archName, typeName, secName);
- }
- }
- std::cout << "##################### add resources" << std::endl;
- //Add optionnal resources (given by the user).
- for(std::list<std::string>::const_iterator iter = mOptionnalResourceLocation.begin(); iter != mOptionnalResourceLocation.end(); ++iter){
- Ogre::ResourceGroupManager::getSingleton().addResourceLocation(*iter, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
- }
-
- // Create the window
- bool canInit = true;
- if(mshowConfigDialog){
- mRoot->restoreConfig();
- if(!mRoot->showConfigDialog())
- canInit = false;
- }
- else{
- if(!mRoot->restoreConfig())
- canInit = false;
- }
-
- if(!mRoot->isInitialised()){
- if(!canInit){ //We set the default renderer system
- const Ogre::RenderSystemList& lRenderSystemList = mRoot->getAvailableRenderers();
- if( lRenderSystemList.size() == 0 )
- throw "ConfigDialog aborted"; // Exit the application on cancel
-
- Ogre::RenderSystem *lRenderSystem = lRenderSystemList.at(0);
- std::cout << "Using " << lRenderSystem->getName() << " as renderer." << std::endl;
- mRoot->setRenderSystem(lRenderSystem);
- }
-
- mRoot->initialise(false);
- }
-
- bool fullscreen = false;
- Ogre::NameValuePairList misc;
- Ogre::ConfigOptionMap config = mRoot->getRenderSystem()->getConfigOptions();
- Ogre::ConfigOptionMap::const_iterator it = config.begin();
-
- while( it != config.end() ){
- Ogre::String leftconf = (*it).first;
- Ogre::String rightconf = (*it).second.currentValue;
-
- if(leftconf == "Video Mode"){
- if(canInit) {
- int ret = sscanf(rightconf.c_str(), "%d %*s %d", &mWindowWidth, &mWindowHeight);
- if (ret == 0)
- std::cout << "Cannot read Ogre video mode" << std::endl;
- }
- else{
- if(mWindowWidth == 0 && mWindowHeight == 0){
- mWindowWidth = mBackgroundWidth;
- mWindowHeight = mBackgroundHeight;
- }
- }
- }
- else if( leftconf == "Full Screen" ){
- if(canInit){
- if(rightconf == "Yes") fullscreen = true;
- }
- }
- else
- misc[leftconf] = rightconf;
-
- it++;
- }
-
- // With Ogre version >= 1.8.1 we hide the window
- if( hidden ){
-#if ( OGRE_VERSION >= (1 << 16 | 8 << 8 | 1) )
- misc["hidden"] = "true";
- windowHidden = true;
-#endif
- }
- mWindow = mRoot->createRenderWindow(name, mWindowWidth, mWindowHeight, fullscreen, &misc);
-
- // Initialise resources
- Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
- //-----------------------------------------------------
- // 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
- //-----------------------------------------------------
-
- mSceneMgr = mRoot->createSceneManager(Ogre::ST_GENERIC);
-
- // Create the camera
- createCamera();
-
- // Create a viewport
- Ogre::Viewport* viewPort = mWindow->addViewport(mCamera);
-// Ogre::Viewport* viewPort = mCamera->getViewport();
- viewPort->setClearEveryFrame(true);
- // Set the projection parameters to match the camera intrinsic parameters
- updateCameraProjection();
-
- // Create the 3D scene
- createScene();
-
- // Initialise and register event handlers
- mRoot->addFrameListener(this);
-
- // Register as a Window listener
- Ogre::WindowEventUtilities::addWindowEventListener(mWindow, this);
-
-#ifdef VISP_HAVE_OIS
- // Initialise OIS
- Ogre::LogManager::getSingletonPtr()->logMessage("*** Initializing OIS ***");
- OIS::ParamList pl;
-
- size_t windowHnd = 0;
- std::ostringstream windowHndStr;
- // Initialise window
- mWindow->getCustomAttribute("WINDOW", &windowHnd);
- windowHndStr << windowHnd;
- pl.insert(std::make_pair(std::string("WINDOW"), windowHndStr.str()));
- // Let the user use the keyboard elsewhere
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- pl.insert(std::make_pair(std::string("x11_keyboard_grab"), std::string("false")));
-#endif
-
- mInputManager = OIS::InputManager::createInputSystem( pl );
-
- //Create all devices
- // Here we only consider the keyboard input
- mKeyboard = static_cast<OIS::Keyboard*>(mInputManager->createInputObject( OIS::OISKeyboard, bufferedKeys ));
- if ( !bufferedKeys ) mKeyboard->setEventCallback ( this);
-#endif
-
- // Initialise a render to texture to be able to retrieve a screenshot
- Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
- mWindow->getWidth(),mWindow->getHeight(), 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
-
-
-
-// Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
-// 640,480, 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
- Ogre::RenderTexture* RTarget = Texture->getBuffer()->getRenderTarget();
- /*Ogre::Viewport* Viewport =*/ RTarget->addViewport(mCamera);
- RTarget->getViewport(0)->setClearEveryFrame(true);
- RTarget->getViewport(0)->setOverlaysEnabled(false);
-}
-
-/*!
- Destructor.
-*/
-vpAROgre::~vpAROgre(void)
-{
- // Destroy 3D scene
- destroyScene();
- // Close OIS
- closeOIS();
-
- if ( mWindow) {
- Ogre::WindowEventUtilities::removeWindowEventListener(mWindow, this);
- windowClosed(mWindow);
- }
- // Delete root
- if (mRoot) delete mRoot;
-}
-
-/*!
- Function testing if the program must stop rendering or not.
- \param evt : Frame event to process.
- \return False if the program must be stopped.
-*/
-bool vpAROgre::stopTest(const Ogre::FrameEvent& evt)
-{
- // Always keep this part
- if(keepOn){
- return updateScene(evt);
- }
- else
- return keepOn;
-}
-
-/*!
- Function telling what to do before each frame.
- Calls customframeStarted() and see if the program has to be stopped.
-
- This function is called before every frame is rendered by Ogre.
-
- \param evt : Frame event to process.
- \return True if everything went well.
-*/
-bool vpAROgre::frameStarted(const Ogre::FrameEvent& evt)
-{
- // custom method telling what to do at the beginning of each frame
- bool result = customframeStarted(evt);
-
- // Listen to the window
- Ogre::WindowEventUtilities::messagePump();
- processInputEvent(evt);
-
- // See if we have to stop rendering
- if(result) return stopTest(evt);
- else return result;
-}
-
-
-/*!
- Function telling what to do after each frame.
- Calls customframeEnded() and see if the program is to be stopped.
- \param evt : Frame event to process.
- \return True if everything went well.
-*/
-bool vpAROgre::frameEnded(const Ogre::FrameEvent& evt)
-{
- // custom method telling what to do at the end of each frame
- bool result = customframeEnded(evt);
-
- // See if we have to stop rendering
- if(result) return stopTest(evt);
- else return result;
-}
-
-/*!
-
- Function telling what to do before each frame. This method is called
- before every frame rendered by Ogre.
-
- \param evt : Frame event to process
- \return True if everything went well
-*/
-bool vpAROgre::customframeStarted(const Ogre::FrameEvent& /*evt*/)
-{
- // See if window was closed
- if(mWindow->isClosed()) return false;
-
-#ifdef VISP_HAVE_OIS
- // Get keyboard input
- mKeyboard->capture();
- if(mKeyboard->isKeyDown(OIS::KC_ESCAPE))
- return false;
-#endif
- return true;
-}
-
-
-/*!
- Function telling what to do after each frame.
- \param evt : Frame event to process.
- \return True if everything went well.
-*/
-bool vpAROgre::customframeEnded(const Ogre::FrameEvent& /*evt*/){return true;}
-
-/*!
-
- Unattach OIS (Object-oriented Input System library) before window shutdown
- (very important under Linux).
-
- \warning This method is only available if OIS is installed and detected
- during ViSP configuration stage.
-
- \param rw : RenderWindow to close.
-*/
-void vpAROgre::windowClosed(Ogre::RenderWindow* rw)
-{
- //Only close for window that created OIS (the main window in these demos)
- if( rw == mWindow ) closeOIS();
-}
-
-/*!
- Render one frame.
- \param I : Grey level image to show in background.
- \param cMw : Camera pose as an homogeneous matrix.
-*/
-bool vpAROgre::renderOneFrame(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMw)
-{
- // Update the background to match the situation
- updateBackgroundTexture(I);
-
- // Update the camera parameters to match the grabbed image
- updateCameraParameters(cMw);
-
- // Display on Ogre Window
- return mRoot->renderOneFrame();
-}
-
-/*!
- Render one frame.
- \param I : RGBa image to show in background.
- \param cMw : Camera pose as an homogeneous matrix.
-*/
-bool vpAROgre::renderOneFrame(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMw)
-{
- // Update the background to match the situation
- updateBackgroundTexture(I);
-
- // Update the camera parameters to match the grabbed image
- updateCameraParameters(cMw);
-
- // Display on Ogre Window
- return mRoot->renderOneFrame();
-}
-
-/*!
- Display a frame.
- \param I : Grey level image to show in background.
- \param cMw : Camera pose as an homogeneous matrix.
-*/
-void vpAROgre::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMw)
-{
- // Display on Ogre Window
- if(renderOneFrame(I,cMw)){
- mWindow->update();
- keepOn = true;
- }
- else
- keepOn = false;
-}
-
-/*!
- Display a frame.
- \param I : RGBa image to show in background.
- \param cMw : Camera pose as an homogeneous matrix.
-*/
-void vpAROgre::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw)
-{
- // Display on Ogre Window
- if(renderOneFrame(I,cMw)){
- mWindow->update();
- keepOn = true;
- }
- else
- keepOn = false;
-}
-
-/*!
- Ask the program if we can continue to render.
- \return True if nothing stopped the rendering loop.
-*/
-bool vpAROgre::continueRendering(void)
-{
- return keepOn;
-}
-
-/*!
- Set the camera intrinsic parameters
-*/
-void vpAROgre::setCameraParameters(const vpCameraParameters &cameraP )
-{
- mcam = cameraP;
-}
-
-/*!
- Load a mesh in the 3D world.
- \param name : Name of the Entity and SceneNode to create.
- \param model : 3D model to load.
-*/
-void vpAROgre::load(const std::string &name, const std::string &model)
-{
- Ogre::Entity *newEntity = mSceneMgr->createEntity(name, model);
- Ogre::SceneNode *newNode = mSceneMgr->getRootSceneNode()->createChildSceneNode(name);
- newNode->attachObject(newEntity);
-}
-
-/*!
- Change position of a ScneneNode.
- \param name : Name of the SceneNode to move.
- \param wTo : New position of the node (translation between object frame and
- world frame).
-*/
-void vpAROgre::setPosition(const std::string &name,
- const vpTranslationVector &wTo)
-{
- // Reset the position
- Ogre::SceneNode *node = mSceneMgr->getSceneNode(name);
- node->setPosition((Ogre::Real)wTo[0], (Ogre::Real)wTo[1], (Ogre::Real)wTo[2]);
-}
-
-/*!
- Get position of a SceneNode.
- \param name : Name of the SceneNode in the scene graph.
- \return The position of the node.
-*/
-vpTranslationVector vpAROgre::getPosition(const std::string &name)const
-{
- Ogre::Vector3 translation = mSceneMgr->getSceneNode(name)->getPosition();
- return vpTranslationVector((Ogre::Real)translation[0], (Ogre::Real)translation[1], (Ogre::Real)translation[2]);
-}
-
-/*!
- Set the orientation of a SceneNode.
- \param name : Name of the SceneNode to rotate.
- \param wRo : The rotation matrix representing the rotation to apply.
-*/
-void vpAROgre::setRotation(const std::string &name, const vpRotationMatrix &wRo)
-{
- // Get the node in its original position
- mSceneMgr->getSceneNode(name)->resetOrientation();
- // Apply the new rotation
- Ogre::Matrix3 rotationOgre
- = Ogre::Matrix3( (Ogre::Real)wRo[0][0], (Ogre::Real)wRo[0][1], (Ogre::Real)wRo[0][2],
- (Ogre::Real)wRo[1][0], (Ogre::Real)wRo[1][1], (Ogre::Real)wRo[1][2],
- (Ogre::Real)wRo[2][0], (Ogre::Real)wRo[2][1], (Ogre::Real)wRo[2][2]);
- Ogre::Quaternion q(rotationOgre);
- mSceneMgr->getSceneNode(name)->rotate(q);
-}
-
-/*!
- Add a rotation to a SceneNode.
- \param name : Name of the SceneNode to rotate.
- \param wRo : The rotation matrix representing the rotation to apply.
-*/
-void vpAROgre::addRotation(const std::string &name,
- const vpRotationMatrix &wRo)
-{
- // Apply the new rotation
- Ogre::Matrix3 rotationOgre
- = Ogre::Matrix3( (Ogre::Real)wRo[0][0], (Ogre::Real)wRo[0][1], (Ogre::Real)wRo[0][2],
- (Ogre::Real)wRo[1][0], (Ogre::Real)wRo[1][1], (Ogre::Real)wRo[1][2],
- (Ogre::Real)wRo[2][0], (Ogre::Real)wRo[2][1], (Ogre::Real)wRo[2][2]);
- Ogre::Quaternion q(rotationOgre);
- mSceneMgr->getSceneNode(name)->rotate(q);
-
-
-}
-
-/*!
- Set the position and the orientation of a SceneNode.
- \param name : Name of the SceneNode to rotate.
-
- \param wMo : The homogeneous matrix representing the rotation and
- translation to apply.
-
-*/
-void vpAROgre::setPosition(const std::string &name,
- const vpHomogeneousMatrix &wMo)
-{
- // Extract the position and orientation data
- vpRotationMatrix rotations;
- vpTranslationVector translation;
- wMo.extract(rotations);
- wMo.extract(translation);
- // Apply them to the node
- setPosition(name, translation);
- setRotation(name, rotations);
-}
-
-/*!
- Tell if a SceneNode is shown on the screen or not.
- \param name : Name of the SceneNode.
- \param isVisible : If true we show the node, if false we don't.
-*/
-void vpAROgre::setVisibility(const std::string &name, bool isVisible)
-{
- mSceneMgr->getSceneNode(name)->setVisible(isVisible);
-}
-
-/*!
- Scale a SceneNode.
- \param name : Name of the SceneNode.
- \param factorx : Scale factor along the x-axis.
- \param factory : Scale factor along the x-axis.
- \param factorz : Scale factor along the x-axis.
-*/
-void vpAROgre::setScale(const std::string &name, const float factorx, const float factory, const float factorz)
-{
- // Reset the scale to its original value
- mSceneMgr->getSceneNode(name)->scale(Ogre::Vector3(1,1,1)/mSceneMgr->getSceneNode(name)->getScale());
- // Apply the new scale
- mSceneMgr->getSceneNode(name)->scale(Ogre::Vector3(factorx, factory, factorz));
-}
-
-/*!
- Create the Ogre camera.
-*/
-void vpAROgre::createCamera(void)
-{
- mCamera = mSceneMgr->createCamera("Camera");
-}
-
-/*!
- Create a greylevel background to show the real scene.
-
- \param I : This parameter is here only used to initialize a grey level background.
-*/
-void vpAROgre::createBackground(vpImage<unsigned char> & /* I */)
-{
- // Create a rectangle to show the incoming images from the camera
- mBackground = new Ogre::Rectangle2D(true); // true = textured
- mBackground->setCorners(-1.0, 1.0, 1.0, -1.0); // Spread all over the window
- mBackground->setBoundingBox(Ogre::AxisAlignedBox(-100000.0*Ogre::Vector3::UNIT_SCALE, 100000.0*Ogre::Vector3::UNIT_SCALE)); // To be shown everywhere
-
- // Texture options
- Ogre::MaterialManager::getSingleton().setDefaultTextureFiltering(Ogre::TFO_NONE);
- Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1);
-
- // Dynamic texture
- // If we are using opengl we can boost a little bit performances with a dynamic texture
- if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- Ogre::PF_BYTE_L,
- Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
- }
- else{
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- Ogre::PF_BYTE_L,
- Ogre::TU_DEFAULT);
- }
-
- // Pointer to the dynamic texture
- Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
-//#else
-// ;
-//#endif
- mPixelBuffer = dynTexPtr->getBuffer();
-
- // Material to apply the texture to the background
- Ogre::MaterialPtr Backgroundmaterial
- = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Material>();
-//#else
-// ;
-//#endif
- Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
- Backgroundtechnique->createPass();
- Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
- Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
- Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
- Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
- mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
- mBackground->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND); // To be rendered in Background
-
- // Add the background to the Scene Graph so it will be rendered
- Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode");
- BackgroundNode->attachObject(mBackground);
-}
-
-/*!
- Create a color background to show the real scene.
-
- \param I : This parameter is here only used to initialize a color background.
-*/
-void vpAROgre::createBackground(vpImage<vpRGBa> & /* I */)
-{
- // Create a rectangle to show the incoming images from the camera
- mBackground = new Ogre::Rectangle2D(true); // true = textured
- mBackground->setCorners(-1.0, 1.0, 1.0, -1.0); // Spread all over the window
- mBackground->setBoundingBox(Ogre::AxisAlignedBox(-100000.0*Ogre::Vector3::UNIT_SCALE, 100000.0*Ogre::Vector3::UNIT_SCALE)); // To be shown everywhere
-
- // Texture options
- Ogre::MaterialManager::getSingleton().setDefaultTextureFiltering(Ogre::TFO_NONE);
- Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1);
-
- // Dynamic texture
- // If we are using opengl we can boost a little bit performances with a dynamic texture
- if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- //Ogre::PF_BYTE_RGBA,
- Ogre::PF_BYTE_BGRA,
- Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
- }
- else{ // As that texture does not seem to work properly with direct3D we use a default texture
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- //Ogre::PF_BYTE_RGBA,
- Ogre::PF_BYTE_BGRA,
- Ogre::TU_DEFAULT);
- }
-
-
- // Pointer to the dynamic texture
- Ogre::TexturePtr dynTexPtr =
- Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
-//#else
-// ;
-//#endif
-
- // Get the pixel buffer
- mPixelBuffer = dynTexPtr->getBuffer();
-
- // Material to apply the texture to the background
- Ogre::MaterialPtr Backgroundmaterial
- = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Material>();
-//#else
-// ;
-//#endif
- Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
- Backgroundtechnique->createPass();
- Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
- Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
- Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
- Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
- mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
- mBackground->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND); // To be rendered in Background
-
- // Add the background to the Scene Graph so it will be rendered
- Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode");
- BackgroundNode->attachObject(mBackground);
-}
-
-/*!
- Close the OIS based input manager.
-
- \warning This method is only to call if OIS is installed and detected during
- ViSP configuration stage.
-
-*/
-void vpAROgre::closeOIS(void)
-{
-#ifdef VISP_HAVE_OIS
- if( mInputManager )
- {
- mInputManager->destroyInputObject( mKeyboard );
-
- OIS::InputManager::destroyInputSystem(mInputManager);
- mInputManager = 0;
- }
-#endif
-}
-
-/*!
- Update the projection parameters of the camera.
-*/
-// Note: equation taken from:
-// http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
-void vpAROgre::updateCameraProjection(void)
-{
- if(mCamera != 0){
- Ogre::Real f,n,f_m_n,f_p_n,px,py,u0,v0;
- f = (Ogre::Real)(mFarClipping); // Far clip distance
- n = (Ogre::Real)(mNearClipping); // Near clip distance
- f_m_n = (Ogre::Real)(f-n);
- f_p_n = (Ogre::Real)(f+n);
- px = (Ogre::Real)mcam.get_px();
- py = (Ogre::Real)mcam.get_py();
- u0 = (Ogre::Real)mcam.get_u0();
- v0 = (Ogre::Real)mcam.get_v0();
- Ogre::Matrix4 Projection
- = Ogre::Matrix4( (Ogre::Real)(2.0*px/mBackgroundWidth), 0, (Ogre::Real)(1.0 - 2.0*(u0/mBackgroundWidth)), 0,
- 0, (Ogre::Real)(2.0*py/mBackgroundHeight), (Ogre::Real)(-1.0 + 2.0*(v0/mBackgroundHeight)),0,
- 0, 0, (Ogre::Real)(-1.0*f_p_n/f_m_n), (Ogre::Real)(-2.0*f*n/f_m_n),
- 0, 0, -1.0, 0);
- mCamera->setCustomProjectionMatrix(true, Projection);
- }
-}
-
-/*!
- Update the texture we see in background with a grey level image.
-*/
-void vpAROgre::updateBackgroundTexture(const vpImage<unsigned char> &I)
-{
- // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
- // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
- // performance than HBL_NORMAL
- mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
- // Buffer data
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
- // Fill in the data in the grey level texture
- memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth);
-
- // Unlock the pixel buffer
- mPixelBuffer->unlock();
-}
-
-/*!
- Update the texture we see in background with a RGBa image.
-*/
-void vpAROgre::updateBackgroundTexture(const vpImage<vpRGBa> &I)
-{
- // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
- // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
- // performance than HBL_NORMAL
- mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
- // Buffer data
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
- // Fill in the data in the grey level texture
-#if 1 // if texture in BGRa format
- for(unsigned int i=0; i<mBackgroundHeight; i++){
- for(unsigned int j=0; j<mBackgroundWidth; j++){
- // Color Image
-// *pDest++=I[i][mBackgroundWidth-j].B; // Blue component
-// *pDest++=I[i][mBackgroundWidth-j].G; // Green component
-// *pDest++=I[i][mBackgroundWidth-j].R; // Red component
-
- *pDest++=I[i][j].B; // Blue component
- *pDest++=I[i][j].G; // Green component
- *pDest++=I[i][j].R; // Red component
-
- *pDest++ = 255; // Alpha component
- }
- }
-#else // if texture in RGBa format which is the format of the input image
- memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth*sizeof(vpRGBa));
-#endif
-
- // Unlock the pixel buffer
- mPixelBuffer->unlock();
-}
-
-/*!
- Update Camera parameters from a pose calculation.
-*/
-void vpAROgre::updateCameraParameters (const vpHomogeneousMatrix &cMw)
-{
- // The matrix is given to Ogre with some changes to fit with the world projection
- Ogre::Matrix4 ModelView
-// = Ogre::Matrix4( (Ogre::Real)-cMo[0][0], (Ogre::Real)-cMo[0][1], (Ogre::Real)-cMo[0][2], (Ogre::Real)-cMo[0][3],
- = Ogre::Matrix4( (Ogre::Real)cMw[0][0], (Ogre::Real)cMw[0][1], (Ogre::Real)cMw[0][2], (Ogre::Real)cMw[0][3],
- (Ogre::Real)-cMw[1][0], (Ogre::Real)-cMw[1][1], (Ogre::Real)-cMw[1][2], (Ogre::Real)-cMw[1][3],
- (Ogre::Real)-cMw[2][0], (Ogre::Real)-cMw[2][1], (Ogre::Real)-cMw[2][2], (Ogre::Real)-cMw[2][3],
- (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)1);
- mCamera->setCustomViewMatrix(true, ModelView);
-}
-
-/*!
- Get the result of the rendering loop.
-
- \param I : The image on which to copy the result of the rendering loop.
- \param cMo : The desired camera pose.
-*/
-void vpAROgre::getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo)
-{
- updateCameraParameters(cMo);
- Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("rtf");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();
-//#else
-// ;
-//#endif
- Ogre::RenderTexture* RTarget = dynTexPtr->getBuffer()->getRenderTarget();
- mWindow->update();
- RTarget->update();
- if(I.getHeight() != mWindow->getHeight() || I.getWidth() != mWindow->getWidth()){
- I.resize(mWindow->getHeight(), mWindow->getWidth());
- }
- Ogre::HardwarePixelBufferSharedPtr mPixelBuffer = dynTexPtr->getBuffer();
- mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
- dynTexPtr->getBuffer()->blitToMemory(pixelBox);
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
-#if 1 // if texture in BGRa format
- for(unsigned int i=0; i<I.getHeight(); i++){
- for(unsigned int j=0; j<I.getWidth(); j++){
- // Color Image
- I[i][j].B = *pDest++; // Blue component
- I[i][j].G = *pDest++; // Green component
- I[i][j].R = *pDest++; // Red component
- I[i][j].A = *pDest++; // Alpha component
- }
- }
-#else // if texture in RGBa format which is the format of the input image
- memcpy(I.bitmap, pDest, I.getHeight()*I.getWidth()*sizeof(vpRGBa));
-#endif
-
- // Unlock the pixel buffer
- mPixelBuffer->unlock();
-
-}
-
-
-#endif
-
diff --git a/src/simulator/ogre-simulator/vpAROgre.h b/src/simulator/ogre-simulator/vpAROgre.h
deleted file mode 100644
index 7ce79b5..0000000
--- a/src/simulator/ogre-simulator/vpAROgre.h
+++ /dev/null
@@ -1,397 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAROgre.h 4931 2014-10-09 14:02:52Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Augmented Reality viewer using Ogre3D.
- *
- * Authors:
- * Bertrand Delabarre
- *
- *****************************************************************************/
-
-/*!
- \file vpAROgre.h
-
- \brief Class that implements augmented reality viewer based on Ogre3D.
-
- \warning The content of this file is only available if Ogre3D and
- one of the renderer (OpenGL or DirectX) are installed.
-
-*/
-
-#ifndef __VP_AROGRE__
-#define __VP_AROGRE__
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_OGRE
-#include <list>
-
-#include <visp/vpImage.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpRGBa.h>
-
-#include <visp/vpImageTools.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpRxyzVector.h>
-
-#include <Ogre.h>
-#include <OgreFrameListener.h>
-
-#ifdef VISP_HAVE_OIS
-# include <OIS.h>
-#endif
-
-/*!
- \class vpAROgre
-
- \ingroup RobotSimuWithViz
-
- \brief Implementation of an augmented reality viewer.
-
- This class provides methods to show a 3D scene in a real world. To do that
- you will need to initialise it with the parameters of your camera, then each
- frame you will need to compute a pose for your camera and give it to the
- application. With that information and the image to be shown in background
- it will set up the 3D scene correspondingly.
-
-*/
-class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEventListener
-#ifdef VISP_HAVE_OIS
- , public OIS::KeyListener
-#endif
-{
- public:
- vpAROgre(const vpCameraParameters &cam = vpCameraParameters(),
- unsigned int width = 0, unsigned int height = 0,
- const char* resourcePath =
-#ifdef VISP_HAVE_OGRE_RESOURCES_PATH
- VISP_HAVE_OGRE_RESOURCES_PATH,
-#else
- ".",
-#endif
- const char* pluginsPath =
-#ifdef VISP_HAVE_OGRE_PLUGINS_PATH
- VISP_HAVE_OGRE_PLUGINS_PATH
-#else
- "."
-#endif
- );
-
- virtual ~vpAROgre(void);
-
- /*!
- Add optional resource location. Since a resource file cannot be always
- sufficient to manage multiple location media (depending on the computer and
- the executable path), this method may be used to add such paths.
-
- \warning To be effective, this method must be called before the init() one.
-
- \param resourceLocation : The resource location (it may be a folder or a zip
- file).
- */
- inline void addResource(const std::string& resourceLocation){
- mOptionnalResourceLocation.push_back(resourceLocation);
- }
-
- void addRotation(const std::string &name, const vpRotationMatrix &wRo);
-
- bool continueRendering(void);
-
- virtual bool customframeStarted(const Ogre::FrameEvent& evt);
-
- virtual bool customframeEnded(const Ogre::FrameEvent& evt);
-
- virtual void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMw);
-
- virtual void display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMw);
-
- inline Ogre::Camera* getCamera(){ return mCamera; }
-
- /*!
- Get the far distance for clipping.
-
- \return Far clipping value.
- */
- inline double getFarClippingDistance() const { return mFarClipping; }
-
- /*!
- Get the near distance for clipping.
-
- \return Near clipping value.
- */
- inline double getNearClippingDistance() const { return mNearClipping; }
-
- vpTranslationVector getPosition(const std::string &name)const;
-
- void getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo);
-
- inline Ogre::SceneManager* getSceneManager(){ return mSceneMgr; }
-
- virtual void init(vpImage<unsigned char> &I, bool bufferedKeys=false, bool hidden=false);
- virtual void init(vpImage<vpRGBa> &I, bool bufferedKeys=false, bool hidden=false);
-
- /*!
- Test if the window is hidden or not.
-
- \warning True if the window is hidden, false otherwise.
- */
- bool isWindowHidden(){ return windowHidden; }
-
-#ifdef VISP_HAVE_OIS
- /**
- * Default event handler
- */
- virtual bool keyPressed( const OIS::KeyEvent & /*e*/) { return true; }
- /**
- * Default event handler
- */
- virtual bool keyReleased( const OIS::KeyEvent & /*e*/) { return true; }
-#endif
-
- void load(const std::string &name, const std::string &model);
-
- bool renderOneFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw);
-
- bool renderOneFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw);
-
- void setCameraParameters(const vpCameraParameters &cameraP);
-
- /*!
- Set the far distance for clipping.
-
- \param dist : Far clipping value.
- */
- void setFarClippingDistance(const double &dist){
- mFarClipping = dist;
- updateCameraProjection();
- }
-
- /*!
- Set the near distance for clipping.
-
- \param dist : Near clipping value.
- */
- void setNearClippingDistance(const double &dist){
- mNearClipping = dist;
- updateCameraProjection();
- }
-
- /*!
- Set the plugins path used to locate the plugins.cfg file.
-
- By default, this path is set to VISP_HAVE_OGRE_PLUGINS_PATH defined in vpConfig.h
-
- \warning To be effective, this method must be called before the init() one.
-
- \param pluginsPath : The new plugins path (must not have a terminate /).
- */
- inline void setPluginsPath(const char* pluginsPath){
- mPluginsPath = pluginsPath;
- }
-
- void setPosition(const std::string &name,
- const vpTranslationVector &wTo);
- void setPosition(const std::string &name, const vpHomogeneousMatrix &wMo);
-
- /*!
- Set the resource path used to locate the resource.cfg file.
-
- By default, this path is set to VISP_HAVE_OGRE_RESOURCES_PATH defined in vpConfig.h
-
- \warning To be effective, this method must be called before the init() one.
-
- \param resourcePath : The new resource path (must not have a terminate /).
- */
- inline void setResourcePath(const char* resourcePath){
- mResourcePath = resourcePath;
- }
-
- void setRotation(const std::string &name, const vpRotationMatrix &wRo);
-
- void setScale(const std::string &name, const float factorx, const float factory, const float factorz);
-
- /*!
- Enable/Disable the appearance of the config dialog on startup.
-
- \warning To be effective, this method must be called before the init() one.
-
- \param showConfigDialog : if true, shows the dialog window (used to set the
- display options)
- */
- inline void setShowConfigDialog(const bool showConfigDialog){
- mshowConfigDialog = showConfigDialog;
- }
-
- void setVisibility(const std::string &name, bool isVisible);
-
- /*!
- Set the name of the window.
-
- \warning Has to be called before initialisation.
-
- \param n : Name of the window.
- */
- inline void setWindowName(const Ogre::String &n){
- name = n;
- }
-
- /*!
- Set the window position in the screen.
-
- \throw vpException::notInitialised if the window has not been created (using
- the init() method).
-
- \param win_x : x coordinate of the new top left corner of the window in the
- screen.
- \param win_y : y coordinate of the new top left corner of the window in the
- screen.
- */
- inline void setWindowPosition(const unsigned int win_x, const unsigned int win_y){
- if(mWindow == NULL){
- throw vpException(vpException::notInitialized, "Window not initialised, cannot set its position");
- }
- mWindow->reposition(static_cast<int>(win_x), static_cast<int>(win_y));
- }
-
- virtual void windowClosed(Ogre::RenderWindow* rw);
-
- protected:
-
- virtual void init(bool bufferedKeys=false, bool hidden=false);
- virtual void createCamera(void);
-
- /**
- * Build the 3D scene
- * Override this to show what you want
- */
- virtual void createScene(void){};
-
- virtual void closeOIS(void);
-
- /*!
- Update the 3D scene
-
- \return Always true.
- */
- virtual bool updateScene(const Ogre::FrameEvent & /*evt*/) {return true;};
-
- /*!
- Check for keyboard, mouse and joystick inputs.
-
- \return Always true.
- */
- virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) {return true;};
-
- /*!
- Clean up the 3D scene.
-
- \return Always true.
- */
- virtual bool destroyScene(void) {
- if(!mSceneMgr)
- return false;
-
- mSceneMgr->destroyAllCameras ();
- mSceneMgr->clearScene();
- mRoot->destroySceneManager(mSceneMgr);
- return true;
- }
-
- virtual void updateCameraParameters (const vpHomogeneousMatrix &cMo);
-
- virtual void updateCameraProjection(void);
-
- virtual void updateBackgroundTexture(const vpImage<unsigned char> &I);
-
- virtual void updateBackgroundTexture(const vpImage<vpRGBa> &I);
-
-
- private:
-
- void createBackground(vpImage<unsigned char> &I);
- void createBackground(vpImage<vpRGBa> &I);
-
- bool frameStarted(const Ogre::FrameEvent& evt);
-
- bool frameEnded( const Ogre::FrameEvent& evt);
-
- bool stopTest(const Ogre::FrameEvent& evt);
-
- protected:
- // Attributes
- Ogre::String name; /**Name of th Window*/
-
- // OGRE 3D System
- Ogre::Root* mRoot; /** Application's root */
- Ogre::Camera* mCamera; /** Camera */
- Ogre::SceneManager* mSceneMgr; /** Scene manager */
- Ogre::RenderWindow* mWindow; /** Display window */
- Ogre::String mResourcePath; /** Path to resources.cfg */
- Ogre::String mPluginsPath; /** Path to plugins.cfg */
-
-#ifdef VISP_HAVE_OIS
- // OIS Input manager and devices
- OIS::InputManager* mInputManager;
- OIS::Keyboard* mKeyboard;
-#endif
-
- // ViSP AR System
- bool keepOn; /** Has the application received a signal to stop(false) or not (true) */
- vpImage<vpRGBa> mImageRGBA; /** vpImage to store grabbed image */
- vpImage<unsigned char> mImage; /** vpImage to store grabbed image */
- Ogre::HardwarePixelBufferSharedPtr mPixelBuffer; /** Pointer to the pixel buffer */
- Ogre::Rectangle2D* mBackground; /** Background image */
- unsigned int mBackgroundHeight; /** Height of the acquired image */
- unsigned int mBackgroundWidth; /** Width of the acquired image */
- unsigned int mWindowHeight; /** Height of the window */
- unsigned int mWindowWidth; /** Width of the window */
- bool windowHidden; /** Is window hidden */
-
- // Camera calculations
- double mNearClipping; /** Near Clipping Distance **/
- double mFarClipping; /** Far Clipping Distance **/
- vpCameraParameters mcam; /** The intrinsic camera parameters */
-
- bool mshowConfigDialog; /** if true, shows the dialog window (used to set the display options) */
-
- std::list<std::string> mOptionnalResourceLocation; /** Optional resource location (used to load mesh and material) */
-
-};
-
-#endif // VISP_HAVE_OGRE
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpArit.cpp b/src/simulator/wireframe-simulator/core/vpArit.cpp
deleted file mode 100755
index d4ca6a5..0000000
--- a/src/simulator/wireframe-simulator/core/vpArit.cpp
+++ /dev/null
@@ -1,827 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpArit.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "arit.c" contient les procedures arithmetiques.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <stdio.h>
-#include <math.h>
-#include <string.h>
-
-#if defined(VISP_USE_MSVC)
-# ifndef M_PI
-# define M_PI 3.14159265358979323846
-# endif
-# ifndef M_PI_2
-# define M_PI_2 M_PI / 2
-# endif
-#endif
-
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-/*
- * La procedure "fprintf_matrix" affiche une matrice sur un fichier.
- * Entree :
- * fp Fichier en sortie.
- * m Matrice a ecrire.
- */
-void
-fprintf_matrix (FILE *fp, Matrix m)
-{
- int i;
-
- fprintf (fp, "(matrix\n");
- for (i = 0; i < 4; i++)
- fprintf (fp, "\t%.4f\t%.4f\t%.4f\t%.4f\n",
- m[i][0], m[i][1], m[i][2], m[i][3]);
- fprintf (fp, ")\n");
-}
-
-/*
- * La procedure "ident_matrix" initialise la matrice par la matrice identite.
- * Entree :
- * m Matrice a initialiser.
- */
-void
-ident_matrix (Matrix m)
-{
- static Matrix identity = IDENTITY_MATRIX;
-
- //bcopy ((char *) identity, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) identity, sizeof (Matrix));
-/*
- * Version moins rapide.
- *
- * int i, j;
- *
- * 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.
- */
-void
-premult_matrix (Matrix a, Matrix b)
-{
- Matrix m;
- int i, j;
-
- for (i = 0; i < 4; i++)
- for (j = 0; j < 4; j++)
- m[i][j] = b[i][0] * a[0][j] +
- b[i][1] * a[1][j] +
- b[i][2] * a[2][j] +
- b[i][3] * a[3][j];
- //bcopy ((char *) m, (char *) a, sizeof (Matrix));
- memmove ((char *) a, (char *) m, sizeof (Matrix));
-}
-
-/*
- * 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.
- */
-void
-premult3_matrix (Matrix a, Matrix b)
-{
- Matrix m;
- int i, j;
-
- //bcopy ((char *) a, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) a, sizeof (Matrix));
- for (i = 0; i < 3; i++)
- for (j = 0; j < 4; j++)
- a[i][j] = b[i][0] * m[0][j] +
- b[i][1] * m[1][j] +
- b[i][2] * m[2][j];
-}
-
-/*
- * La procedure "prescale_matrix" premultiplie la matrice par l'homothetie.
- * Entree :
- * m Matrice a multiplier m = vp * m.
- * vp Vecteur d'homothetie.
- */
-void
-prescale_matrix (Matrix m, Vector *vp)
-{
- int i;
-
- for (i = 0; i < 4; i++) {
- m[0][i] *= vp->x;
- m[1][i] *= vp->y;
- m[2][i] *= vp->z;
- }
-}
-
-/*
- * La procedure "pretrans_matrix" premultiplie la matrice par la translation.
- * Entree :
- * m Matrice a multiplier m = vp * m.
- * vp Vecteur de translation.
- */
-void
-pretrans_matrix (Matrix m, Vector *vp)
-{
- int i;
-
- for (i = 0; i < 4; i++)
- m[3][i] += vp->x * m[0][i] + vp->y * m[1][i] + vp->z * m[2][i];
-}
-
-/*
- * 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'.
- */
-void
-postleft_matrix (Matrix m, char axis)
-{
- static char proc_name[] = "postleft_matrix";
-
- int i;
-
- switch (axis) {
- case 'x' :
- for (i = 0; i < 4; i++) m[i][0] = - m[i][0];
- break;
- case 'y' :
- for (i = 0; i < 4; i++) m[i][1] = - m[i][1];
- break;
- case 'z' :
- for (i = 0; i < 4; i++) m[i][2] = - m[i][2];
- break;
- default :
- fprintf (stderr, "%s: axis unknown\n", proc_name);
- break;
- }
-}
-
-/*
- * 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.
- */
-void
-postmult_matrix (Matrix a, Matrix b)
-{
- Matrix m;
- int i, j;
-
- for (i = 0; i < 4; i++)
- for (j = 0; j < 4; j++)
- m[i][j] = a[i][0] * b[0][j] +
- a[i][1] * b[1][j] +
- a[i][2] * b[2][j] +
- a[i][3] * b[3][j];
- //bcopy ((char *) m, (char *) a, sizeof (Matrix));
- memmove ((char *) a, (char *) m, sizeof (Matrix));
-}
-
-/*
- * 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.
- */
-void
-postmult3_matrix (Matrix a, Matrix b)
-{
- Matrix m;
- int i, j;
-
- //bcopy ((char *) a, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) a, sizeof (Matrix));
- for (i = 0; i < 4; i++)
- for (j = 0; j < 3; j++)
- a[i][j] = m[i][0] * b[0][j] +
- m[i][1] * b[1][j] +
- m[i][2] * b[2][j];
-}
-
-/*
- * La procedure "postscale_matrix" post multiplie la matrice par l'homothetie.
- * Entree :
- * m Matrice a multiplier m = m * vp.
- * vp Vecteur d'homothetie.
- */
-void
-postscale_matrix (Matrix m, Vector *vp)
-{
- int i;
-
- for (i = 0; i < 4; i++) {
- m[i][0] *= vp->x;
- m[i][1] *= vp->y;
- m[i][2] *= vp->z;
- }
-}
-
-/*
- * La procedure "posttrans_matrix" post mutiplie la matrice par la translation.
- * Entree :
- * m Matrice a multiplier m = m * vp.
- * vp Vecteur de translation.
- */
-void
-posttrans_matrix (Matrix m, Vector *vp)
-{
- int i;
-
- for (i = 0; i < 4; i++) {
- m[i][0] += m[i][3] * vp->x;
- m[i][1] += m[i][3] * vp->y;
- m[i][2] += m[i][3] * vp->z;
- }
-}
-
-/*
- * La procedure "transpose_matrix" transpose la matrice.
- * Entree :
- * m Matrice a transposer.
- */
-void
-transpose_matrix (Matrix m)
-{
- unsigned int i, j;
- float t;
-
- for (i = 0; i < 4; i++)
- for (j = 0; j < i; j++)
- SWAP(m[i][j],m[j][i],t);
-}
-
-/*
- * 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.
- * Sortie :
- * Angle en radians.
- */
-float
-cosin_to_angle (float ca, float sa)
-{
- float a; /* angle a calculer */
-
- if (FABS(ca) < M_EPSILON) {
- a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2);
- }
- else {
- a = (float) atan((double) (sa / ca));
- if (ca < (float)0.0) a += (sa > (float)0.0) ? (float)M_PI : (float)(- M_PI);
- }
- return (a);
-}
-
-/*
- * 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".
- */
-void
-cosin_to_lut (Index level, float *coslut, float *sinlut)
-{
- int i;
- int i_pi_2 = TWO_POWER(level);
- int quad; /* quadrant courant */
- float ca; /* cosinus 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 */
- quad += i_pi_2;
- coslut[quad] = 0.0; sinlut[quad] = 1.0; /* PI/2 */
- quad += i_pi_2;
- coslut[quad] = -1.0; sinlut[quad] = 0.0; /* PI */
- quad += i_pi_2;
- coslut[quad] = 0.0; sinlut[quad] = -1.0; /* 3PI/2*/
-
- for (i = 1, a = step; i < i_pi_2; i++, a += step) {
- ca = (float) cos (a);
- quad = 0;
- 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) */
- quad += i_pi_2;
- 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) */
- quad += i_pi_2;
- coslut[quad - i] = ca; /* cos(2PI-a) */
- }
-}
-
-/*
- * La procedure "norm_vector" normalise le vecteur.
- * Si la norme est nulle la normalisation n'est pas effectuee.
- * Entree :
- * vp Le vecteur a norme.
- * Sortie :
- * La norme du vecteur.
- */
-float
-norm_vector (Vector *vp)
-{
- static char proc_name[] = "norm_vector";
-
- float norm; /* norme du vecteur */
-
- if ((norm = (float) sqrt ((double) DOT_PRODUCT(*vp,*vp))) > M_EPSILON) {
- vp->x /= norm;
- vp->y /= norm;
- vp->z /= norm;
- }
- else fprintf (stderr, "%s: nul vector\n", proc_name);
- return (norm);
-}
-
-/*
- * 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.
- */
-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(v,*cp,*ap);
- norm_vector (&u);
- norm_vector (&v);
- CROSS_PRODUCT (*np,u,v);
-}
-
-/*
- * 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.
- */
-void
-point_matrix (Point4f *p4, Point3f *p3, Matrix m)
-{
- float x = p3->x, y = p3->y, z = p3->z;
-
- p4->x = COORD3_COL(x,y,z,m,0);
- p4->y = COORD3_COL(x,y,z,m,1);
- p4->z = COORD3_COL(x,y,z,m,2);
- p4->w = COORD3_COL(x,y,z,m,3);
-}
-
-/*
- * 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 resultat.
- */
-void
-point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op)
-{
- Point3f *pend = ip + size; /* borne de ip */
- float x, y, z; /* [xyz] = *ip */
-
- for (; ip < pend; ip++, op++) {
- x = ip->x;
- y = ip->y;
- z = ip->z;
-
- op->x = COORD3_COL(x,y,z,m,0);
- op->y = COORD3_COL(x,y,z,m,1);
- op->z = COORD3_COL(x,y,z,m,2);
- }
-}
-
-/*
- * 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 4D resultat.
- */
-void
-point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4)
-{
- Point3f *pend = p3 + size; /* borne de p3 */
- float x, y, z; /* [xyz] = *p3 */
-
- for (; p3 < pend; p3++, p4++) {
- x = p3->x;
- y = p3->y;
- z = p3->z;
-
- p4->x = COORD3_COL(x,y,z,m,0);
- p4->y = COORD3_COL(x,y,z,m,1);
- p4->z = COORD3_COL(x,y,z,m,2);
- p4->w = COORD3_COL(x,y,z,m,3);
- }
-}
-
-/*
- * 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.
- */
-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 */
-
- n = *axis; /* norme le vecteur directeur */
- norm_vector (&n);
-
- /*
- * Avant rotation, vp vaut :
- * u + v
- * Apres rotation, vp vaut :
- * u + cos(a) * v + sin(a) * (n^vp)
- * = u + cos(a) * v + sin(a) * (n^v)
- * avec u = (vp.n) * n, v = vp-u;
- * ou "u" est la projection de "vp" sur l'axe "axis",
- * et "v" est la composante de "vp" perpendiculaire a "axis".
- */
- f = DOT_PRODUCT(*vp, n);
- u = n;
- MUL_COORD3(u,f,f,f); /* (vp.n) * n */
-
- DIF_COORD3(v,*vp,u); /* calcule "v" */
-
- f = (float) cos ((double) 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) */
-
- SET_COORD3(*vp,
- u.x + v.x + cross.x,
- u.y + v.y + cross.y,
- u.z + v.z + cross.z);
-}
-
-/*
- * La procedure "upright_vector" calcule un vecteur perpendiculaire.
- * Les vecteurs ont un produit scalaire nul.
- * Entree :
- * vp Vecteur origine.
- * Entree/Sortie :
- * up Vecteur perpendiculaire a vp.
- */
-void
-upright_vector (Vector *vp, Vector *up)
-{
- static char proc_name[] = "upright_vector";
-
- 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 */
- 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 */
- up->x = - (vp->y + vp->z) / vp->x;
- up->y = up->z = 1.0;
- }
- else {
- up->x = up->y = up->z = 0.0;
- fprintf (stderr, "%s: nul vector\n", proc_name);
- return;
- }
-}
-
-/*
- * La procedure "Matrix_to_Position" initialise la position par la matrice.
- * 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.
- */
-void
-Matrix_to_Position (Matrix m, AritPosition *pp)
-{
- Matrix_to_Rotate (m, &pp->rotate);
- SET_COORD3(pp->scale,1.0,1.0,1.0);
- SET_COORD3(pp->translate,m[3][0],m[3][1],m[3][2]);
-}
-
-/*
- * 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 |
- *
- * 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
- * Ry rotation autour de Oy d'angle O2
- * 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".
- *
- * Entree :
- * 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)
-{
- float cx, sx; /* cosinus et sinus de la rotation sur l'axe X */
- float cy, sy; /* cosinus et sinus de la rotation sur l'axe Y */
- float cz, sz; /* cosinus et sinus de la rotation sur l'axe Z */
-
- sy = - m[0][2];
- cy = (float) sqrt (1.0 - (double) (sy * sy));
-
- if (FABS(cy) > M_EPSILON) {
- sz = m[0][1] / cy;
- cz = m[0][0] / cy;
-
- sx = m[1][2] / cy;
- 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 */
- 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->y *= (float)180.0 / (float)M_PI;
- vp->z *= (float)180.0 / (float)M_PI;
-}
-
-/*
- * 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.
- */
-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 */
- m[3][1] = pp->translate.y;
- m[3][2] = pp->translate.z;
-}
-
-/*
- * 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 |
- *
- * 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.
- *
- * Entree :
- * 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 */
- 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);
-
- m[0][0] = cy * cz;
- m[1][0] = (sx * sy * cz) - (cx * sz);
- m[2][0] = (cx * sy * cz) + (sx * sz);
-
- m[0][1] = cy * sz;
- m[1][1] = (sx * sy * sz) + (cx * cz);
- m[2][1] = (cx * sy * sz) - (sx * cz);
-
- m[0][2] = - sy;
- m[1][2] = sx * cy;
- m[2][2] = cx * cy;
-
- m[0][3] = m[1][3] = m[2][3] = 0.0;
- m[3][0] = m[3][1] = m[3][2] = 0.0;
- m[3][3] = 1.0;
-}
-
-/*
- * La procedure "Rotaxis_to_Matrix" initialise la matrice par la rotation
- * 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~
- *
- * | 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 |
- * Entree :
- * 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 */
- Vector n; /* vecteur norme*/
- Vector conv; /* verO n */
- Vector tilde; /* sinO n */
-
- 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 */
- norm_vector (&n);
- tilde = conv = n;
- MUL_COORD3(conv,vera,vera,vera);
- MUL_COORD3(tilde,sina,sina,sina);
-
- m[0][0] = conv.x * n.x + cosa;
- m[0][1] = conv.x * n.y + tilde.z;
- m[0][2] = conv.x * n.z - tilde.y;
-
- m[1][0] = conv.y * n.x - tilde.z;
- m[1][1] = conv.y * n.y + cosa;
- m[1][2] = conv.y * n.z + tilde.x;
-
- m[2][0] = conv.z * n.x + tilde.y;
- m[2][1] = conv.z * n.y - tilde.x;
- m[2][2] = conv.z * n.z + cosa;
-
- m[0][3] = m[2][3] = m[1][3] = 0.0;
- m[3][0] = m[3][1] = m[3][2] = 0.0;
- m[3][3] = 1.0;
-}
-
-/*
- * 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.
- */
-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 */
- m[3][1] = tp->y;
- m[3][2] = tp->z;
-}
-
-/*
- * La procedure "Scale_to_Matrix" initialise la matrice par l'homothetie.
- * Entree :
- * vp Vecteur des coordonnees d'homothetie.
- * m Matrice a initialiser.
- */
-void
-Scale_to_Matrix (Vector *vp, Matrix m)
-{
- ident_matrix (m);
- m[0][0] = vp->x;
- m[1][1] = vp->y;
- m[2][2] = vp->z;
-}
-
-/*
- * La procedure "Translate_to_Matrix" initialise la matrice par la translation.
- * Entree :
- * vp Vecteur des coordonnees de translation.
- * m Matrice a initialiser.
- */
-void
-Translate_to_Matrix (Vector *vp, Matrix m)
-{
- ident_matrix (m);
- m[3][0] = vp->x;
- m[3][1] = vp->y;
- m[3][2] = vp->z;
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpArit.h b/src/simulator/wireframe-simulator/core/vpArit.h
deleted file mode 100755
index 6424b1c..0000000
--- a/src/simulator/wireframe-simulator/core/vpArit.h
+++ /dev/null
@@ -1,214 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpArit.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module contient les procedures arithmetiques.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-#ifndef vpArit_h
-#define vpArit_h
-
-#include <visp/vpConfig.h>
-#include <stdio.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#define ADD_COORD2(r,a,b) { (r).x = (a).x + (b).x;\
- (r).y = (a).y + (b).y; }
-
-#define ADD_COORD3(r,a,b) { (r).x = (a).x + (b).x;\
- (r).y = (a).y + (b).y;\
- (r).z = (a).z + (b).z; }
-
-#define INC_COORD2(r,a) { (r).x += (a).x; (r).y += (a).y; }
-
-#define INC_COORD3(r,a) { (r).x += (a).x; (r).y += (a).y;\
- (r).z += (a).z; }
-
-#define CROSS_PRODUCT(r,a,b) { (r).x = (a).y * (b).z - (a).z * (b).y;\
- (r).y = (a).z * (b).x - (a).x * (b).z;\
- (r).z = (a).x * (b).y - (a).y * (b).x; }
-
-#define DIF_COORD2(r,a,b) { (r).x = (a).x - (b).x;\
- (r).y = (a).y - (b).y; }
-
-#define DIF_COORD3(r,a,b) { (r).x = (a).x - (b).x;\
- (r).y = (a).y - (b).y;\
- (r).z = (a).z - (b).z; }
-
-#define DOT_PRODUCT(a,b) ( ((a).x * (b).x) +\
- ((a).y * (b).y) +\
- ((a).z * (b).z) )
-
-#define LENGTH3(a) (sqrt((double) DOT_PRODUCT((a),(a))))
-
-#define MID_COORD3(r,a,b) { (r).x = ((a).x + (b).x) / 2.0;\
- (r).y = ((a).y + (b).y) / 2.0;\
- (r).z = ((a).z + (b).z) / 2.0; }
-
-#define MUL_COORD3(r,a,b,c) { (r).x *= (a); (r).y *= (b); (r).z *= (c); }
-
-#define PAR_COORD3(r,t,a,b) { (r).x = ((b).x - (a).x) * (t) + (a).x;\
- (r).y = ((b).y - (a).y) * (t) + (a).y;\
- (r).z = ((b).z - (a).z) * (t) + (a).z; }
-
-#define SET_COORD2(r,a,b) { (r).x = (a); (r).y = (b); }
-
-#define SET_COORD3(r,a,b,c) { (r).x = (a); (r).y = (b); (r).z = (c); }
-
-#define SUB_COORD2(r,a) { (r).x -= (a).x; (r).y -= (a).y; }
-
-#define SUB_COORD3(r,a) { (r).x -= (a).x; (r).y -= (a).y;\
- (r).z -= (a).z; }
-
-#define COORD3_COL(x,y,z,m,i) ( ((x) * (m)[0][i]) +\
- ((y) * (m)[1][i]) +\
- ((z) * (m)[2][i]) +\
- (m)[3][i] )
-
-#define COORD4_COL(x,y,z,w,m,i) ( ((x) * (m)[0][i]) +\
- ((y) * (m)[1][i]) +\
- ((z) * (m)[2][i]) +\
- ((w) * (m)[3][i]) )
-
-#define M_POLY1(x,a,b) ((a) * (x) + (b))
-#define M_POLY2(x,a,b,c) (M_POLY1((x),(a),(b)) * (x) + (c))
-#define M_POLY3(x,a,b,c,d) (M_POLY2((x),(a),(b),(c)) * (x) + (d))
-
-
-typedef struct {
- int x, y;
-} Point2i;
-
-typedef struct {
- short x, y;
-} Point2s;
-
-typedef struct {
- int x, y, z;
-} Point3i;
-
-typedef struct {
- float x, y, z;
-} Point3f;
-
-typedef struct {
- float x,y,z,w;
-} Point4f;
-
-typedef struct {
- float x,y,z;
-} Vector;
-
-/*
- * MATRIX
- * ______
- *
- * Matrice homogene ou non.
- * | Rotation | 0 |
- * Matrice non homogene = | 3x3 | 0 |
- * |-------------| 0 |
- * | Translation | 1 |
- */
-typedef float Matrix[4][4];
-
-#define IDENTITY_MATRIX { {1.0, 0.0, 0.0, 0.0},\
- {0.0, 1.0, 0.0, 0.0},\
- {0.0, 0.0, 1.0, 0.0},\
- {0.0, 0.0, 0.0, 1.0} }
-
-
-/*
- * 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.
- */
-typedef struct {
- Vector rotate; /* vecteur rotation */
- Vector scale; /* vecteur homothetie */
- Vector translate; /* vecteur translation */
-} AritPosition;
-
-#define IDENTITY_ROTATE { 0.0, 0.0, 0.0 }
-#define IDENTITY_SCALE { 1.0, 1.0, 1.0 }
-#define IDENTITY_TRANSLATE { 0.0, 0.0, 0.0 }
-
-#define IDENTITY_POSITION { IDENTITY_ROTATE,\
- IDENTITY_SCALE,\
- IDENTITY_TRANSLATE }
-
-void fprintf_matrix (FILE *fp, Matrix m);
-void ident_matrix (Matrix m);
-void premult_matrix (Matrix a, Matrix b);
-void premult3_matrix (Matrix a, Matrix b);
-void prescale_matrix (Matrix m, Vector *vp);
-void pretrans_matrix (Matrix m, Vector *vp);
-void postleft_matrix (Matrix m, char axis);
-void postmult_matrix (Matrix a, Matrix b);
-void postmult3_matrix (Matrix a, Matrix b);
-void postscale_matrix (Matrix m, Vector *vp);
-void posttrans_matrix (Matrix m, Vector *vp);
-void transpose_matrix (Matrix m);
-
-float cosin_to_angle (float ca, float sa);
-float norm_vector (Vector *vp);
-void point_matrix (Point4f *p4, Point3f *p3, Matrix m);
-void plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
-void point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op);
-void point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4);
-void rotate_vector (Vector *vp, float a, Vector *axis);
-void upright_vector (Vector *vp, Vector *up);
-
-void Matrix_to_Position (Matrix m, AritPosition *pp);
-void Matrix_to_Rotate (Matrix m, Vector *vp);
-void Position_to_Matrix (AritPosition *pp, Matrix m);
-void Rotate_to_Matrix (Vector *vp, Matrix m);
-void Rotaxis_to_Matrix (float a, Vector *axis, Matrix m);
-void Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m);
-void Scale_to_Matrix (Vector *vp, Matrix m);
-void Translate_to_Matrix (Vector *vp, Matrix m);
-
-void fscanf_Point3f (Point3f *pp);
-void fscanf_Vector (Vector *vp);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpAritio.cpp b/src/simulator/wireframe-simulator/core/vpAritio.cpp
deleted file mode 100755
index 716db6e..0000000
--- a/src/simulator/wireframe-simulator/core/vpAritio.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpAritio.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-#include "visp/vpConfig.h"
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpToken.h>
-#include <visp/vpLex.h>
-
-#include <stdio.h>
-/*
- * La procedure "fprintf_Position" ecrit en ascii un positionnement.
- * Entree :
- * f Fichier en sortie.
- * pp Positionnement a ecrite.
- */
-void fprintf_Position (FILE *f, AritPosition *pp)
-{
- fprintf (f, "%.3f\t%.3f\t%.3f\n%.3f\t%.3f\t%.3f\n%.3f\t%.3f\t%.3f\n",
- pp->rotate.x, pp->rotate.y, pp->rotate.z,
- pp->scale.x, pp->scale.y, pp->scale.z,
- pp->translate.x, pp->translate.y, pp->translate.z);
-}
-
-/*
- * La procedure "fscanf_Point3f" lit en ascii un point flottant 3D.
- * Entree :
- * 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. */
-
- 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. */
-
- 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. */
-
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
- pp->z = (t == T_INT) ? (float) myint : myfloat;
-}
-
-/*
- * La procedure "fscanf_Vector" lit en ascii un vecteur.
- * Entree :
- * vp Vecteur a lire.
- */
-void fscanf_Vector (Vector *vp)
-{
-static const char *err_tbl[] = {
-"float expected (coordinate ",
-" of vector)"
-};
-
- int t;
-
- /* 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. */
-
- 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. */
-
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
- vp->z = (t == T_INT) ? (float) myint : myfloat;
-}
-
-/*
- * La procedure "fscanf_Position" lit en ascii un positionnement.
- * Entree :
- * pp Positionnement a lire.
- */
-void fscanf_Position (AritPosition *pp)
-{
- pusherr ("rotate: ");
- fscanf_Vector (&pp->rotate);
- popuperr ("scale: ");
- fscanf_Vector (&pp->scale);
- popuperr ("translate: ");
- fscanf_Vector (&pp->translate);
- poperr ();
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpBound.cpp b/src/simulator/wireframe-simulator/core/vpBound.cpp
deleted file mode 100755
index 1a87515..0000000
--- a/src/simulator/wireframe-simulator/core/vpBound.cpp
+++ /dev/null
@@ -1,230 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBound.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le fichier "bound.c" contient les procedures de gestion des scenes de modele geometrique surfacique.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include "visp/vpConfig.h"
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpBound.h>
-#include <errno.h>
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-/*
- * La procedure "free_Bound" libere la memoire d'une surface.
- * Les champs "bound.face.edge" ne sont pas utilises.
- * Entree :
- * 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 */
- if (fp->vertex.ptr != fp->vertex.tbl)
- free ((char *) fp->vertex.ptr);
- }
- 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 */
- free ((char *) bp->point.ptr);
- bp->point.ptr = NULL;
- }
-#ifdef face_normal
- if (bp->normal.ptr != NULL) { /* libere les vecteurs */
- free ((char *) bp->normal.ptr);
- bp->normal.ptr = NULL;
- }
-#endif /* face_normal */
- bp->is_display = FALSE;
-}
-
-/*
- * 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.
- */
-void free_huge_Bound (Bound *bp)
-{
- 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.
- */
-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 */
- free_Bound (bp);
- }
- if (bsp->name != NULL) { /* libere le nom */
- free ((char *) bsp->name);
- bsp->name = NULL;
- }
- if (bsp->bound.ptr != NULL) { /* libere le tableau */
- free ((char *) bsp->bound.ptr);
- bsp->bound.ptr = NULL;
- }
-}
-
-/*
- * 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.
- */
-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 */
- bp->face.ptr = NULL;
- else if ((bp->face.ptr = (Face *) malloc (fn * sizeof (Face)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
- 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);
- exit (1);
- }
-
-#ifdef face_normal
- /* 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) {
- perror (proc_name);
- exit (1);
- }
-#endif /* face_normal */
-
- bp->type = type;
- bp->is_display = TRUE;
- bp->is_polygonal = polygonal;
-}
-
-/*
- * La procedure "malloc_huge_Bound" alloue une surface de taille maximale.
- * 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.
- * 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.
- */
-void malloc_huge_Bound (Bound *bp)
-{
- static char proc_name[] = "malloc_Huge_Bound";
-
-#ifdef face_normal
- malloc_Bound (bp, (Type) BND_NULL, FALSE, FACE_NBR, POINT_NBR);
-#else
- malloc_Bound (bp, (Type) BND_NULL, TRUE, FACE_NBR, POINT_NBR);
-#endif /* face_normal */
- if ((bp->face.ptr->vertex.ptr =
- (Index *) malloc (FACE_NBR * VERTEX_NBR * sizeof (Index))) == NULL) {
- perror (proc_name);
- exit (1);
- }
-}
-
-/*
- * La procedure "malloc_Bound_scene" alloue une scene de surfaces.
- * 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.
- */
-void malloc_Bound_scene (Bound_scene *bsp, const char *name, Index bn)
-{
- static char proc_name[] = "malloc_Bound_scene";
-
- if ((bsp->name = (char *) malloc ((strlen (name) + 1) * sizeof (char)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
- if ((bsp->bound.nbr = bn) == 0)
- bsp->bound.ptr = NULL;
- else if ((bsp->bound.ptr = (Bound *) malloc (bn * sizeof (Bound)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
- strcpy (bsp->name, name);
- bsp->bound.nbr = 0;
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpBound.h b/src/simulator/wireframe-simulator/core/vpBound.h
deleted file mode 100755
index d05b2a9..0000000
--- a/src/simulator/wireframe-simulator/core/vpBound.h
+++ /dev/null
@@ -1,182 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBound.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le fichier "bound.h" contient les macros et le types
- * utilises par le modele geometrique surfacique polygonale 3D.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpBound_H
-#define vpBound_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpMy.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-#define START_ARG 0
-#define NEXT_ARG 1
-
-#define BND_NULL (-1)
-
-#define BND_BLOCK 0
-#define BND_CARDIOIDE 1
-#define BND_CONE 2
-#define BND_CYLINDER 3
-#define BND_SPHERE 4
-#define BND_TORUS 5
-#define BND_WEDGE 6
-
-#define BND_F3 7
-#define BND_F4 8
-
-#define BND_GRID 9
-#define BND_PIPE 10
-#define BND_SECTION 11
-
-#define BND_NBR 12
-
-
-#define BOUND_NBR 1024
-#define FACE_NBR 6144 /* Tailles de tableaux */
-#define VERTEX_NBR 16
-#define POINT_NBR 6144
-#ifdef face_normal
-#define VECTOR_NBR 6144
-#endif //face_normal
-
-#ifdef face_edge
-typedef struct {
- 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 */
-} Edge_list;
-#endif //face_edge
-
-#define DEFAULT_VSIZE 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;
- */
-typedef struct {
- Index nbr; /* nombre de sommets */
- Index *ptr; /* liste dynamique */
- Index tbl[DEFAULT_VSIZE];
-} Vertex_list;
-
-typedef struct {
- Index nbr; /* nombre de points */
- Point3f *ptr; /* liste dynamique */
-} Point3f_list;
-
-#ifdef face_normal
-typedef struct {
- Index nbr; /* nombre de vecteurs */
- Vector *ptr; /* liste dynamique */
-} Vector_list;
-#endif //face_normal
-
-typedef struct {
- unsigned is_polygonal:1; /* face polygonale */
- unsigned is_visible :1; /* face affichable */
-#ifdef face_edge
- Edge_list edge; /* liste d'aretes */
-#endif //face_edge
- Vertex_list vertex; /* liste de sommets */
-#ifdef face_normal
- Vector normal; /* vecteur normal */
-#endif //face_normal
-} Face;
-
-typedef struct {
- Index nbr; /* nombre de faces */
- Face *ptr; /* liste dynamique */
-} Face_list;
-
-typedef struct {
- 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 */
-#endif //face_edge
- Face_list face; /* liste de faces */
- Point3f_list point; /* points aux sommets */
-#ifdef face_normal
- Vector_list normal; /* normales aux sommets */
-#endif //face_normal
-} Bound;
-
-typedef struct {
- Index nbr; /* nombre de surfaces */
- Bound *ptr; /* liste dynamique */
-} Bound_list;
-
-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 */
-} Bounding_box;
-
-typedef struct {
- char *name; /* nom de la scene */
- Bound_list bound; /* liste de surfaces */
-} Bound_scene;
-
-typedef struct {
- Index nbr; /* nombre de scenes */
- Bound_scene *ptr; /* liste dynamique */
-} Bound_scene_list;
-
-extern void malloc_huge_Bound (Bound *bp);
-extern void free_huge_Bound (Bound *bp);
-extern void fscanf_Bound (Bound *bp);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpBoundio.cpp b/src/simulator/wireframe-simulator/core/vpBoundio.cpp
deleted file mode 100755
index fe971a0..0000000
--- a/src/simulator/wireframe-simulator/core/vpBoundio.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBoundio.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "boundio.c" contient les procedures d'entree/sortie
- * des types definis dans le module "bound.h".
- * Les entrees non specifiees sont effectuees
- * sur le fichier "source" de "lex.c".
- * Pour les mots cles des "fprintf_..." voir "token.c".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-#include "visp/vpConfig.h"
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpBound.h>
-#include <visp/vpToken.h>
-#include <visp/vpSkipio.h>
-#include <visp/vpLex.h>
-
-#include <errno.h>
-#include <stdio.h>
-#include <stdlib.h>
-void fscanf_Point3f_list (Point3f_list *);
-void fscanf_Face_list (Face_list *);
-
-/*
- * La procedure "fscanf_Bound" lit en ascii une surface.
- * Entree :
- * bp Surface a lire.
- */
-void fscanf_Bound (Bound *bp)
-{
- /* 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. */
-
- 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. */
-
- skip_keyword (T_FACE_LIST, "bound: keyword \"face_list\" expected");
- pusherr ("bound_face_list: ");
- fscanf_Face_list (&bp->face);
- poperr ();
-}
-
-/*
- * La procedure "fscanf_Face_list" lit en ascii une liste de faces.
- * Entree :
- * lp Liste de faces a lire.
- */
-void fscanf_Face_list (Face_list *lp)
-{
- static char proc_name[] = "fscanf_Face_list";
-
- Face *fp; /* face courante */
- Face *fend; /* borne de "fp" */
-
-
- /* 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. */
-
- if (lp->nbr == 0)
- lp->ptr = NULL;
- else if ((lp->ptr = (Face *) malloc (lp->nbr * sizeof (Face)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
- /* Lecture des faces de la liste une a une. */
-
- fp = lp->ptr;
- fend = fp + lp->nbr;
- for (; fp < fend; fp++) {
- Vertex_list *lp = &fp->vertex;
- Index *vp; /* sommet courant */
- Index *vend; /* borne de "vp" */
-
- /* 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. */
-
- if (lex () != T_INT)
- lexerr ("start", "integer expected (number of vertices)", NULL);
- lp->nbr = (Index) myint;
-
- /* Allocation dynamique du polygone de la face. */
-
- if (lp->nbr <= DEFAULT_VSIZE)
- lp->ptr = lp->tbl;
- else if ((lp->ptr = (Index *) malloc (lp->nbr * sizeof (Index)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
- /* Lecture des sommets de la face un a un. */
-
- vp = lp->ptr;
- vend = vp + lp->nbr;
- for (; vp < vend; *vp++ = (Index) myint)
- if (lex () != T_INT)
- lexerr ("start", "integer expected (index of points 3D)", NULL);
- }
-}
-
-/*
- * La procedure "fscanf_Point_list" lit en ascii une liste de points 3D.
- * Entree :
- * lp Liste de points a lire.
- */
-void fscanf_Point3f_list (Point3f_list *lp)
-{
- static const char proc_name[] = "fscanf_Point3f_list";
-
-static const char *err_tbl[] = {
-"float expected (coordinate ",
-" of point)"
-};
- Point3f *pp; /* point courant */
- Point3f *pend; /* borne de "pp" */
-
- /* 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. */
-
- if (lp->nbr == 0)
- lp->ptr = NULL;
- else if ((lp->ptr = (Point3f *) malloc (lp->nbr * sizeof (Point3f)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
- /* Lecture des points de la liste un a un. */
-
- pp = lp->ptr;
- pend = pp + lp->nbr;
- for (; pp < pend; pp++) {
- int t;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "X", err_tbl[1], NULL);
-/* FC printf("X %d %f\n",myint, myfloat); */
- pp->x = (t == T_INT) ? (float) myint : myfloat;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "Y", err_tbl[1], NULL);
-/* FC printf("Y %d %f\n",myint, myfloat); */
- pp->y = (t == T_INT) ? (float) myint : myfloat;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "Z", err_tbl[1], NULL);
-/* FC printf("Z %d %f\n",myint, myfloat); */
- pp->z = (t == T_INT) ? (float) myint : myfloat;
- }
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpCgiconstants.h b/src/simulator/wireframe-simulator/core/vpCgiconstants.h
deleted file mode 100755
index c4b9727..0000000
--- a/src/simulator/wireframe-simulator/core/vpCgiconstants.h
+++ /dev/null
@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCgiconstants.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpCgiconstants_H
-#define vpCgiconstants_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/* errors */
-#define NO_ERROR 0
-#define ENOTCGCL 1
-#define ENOTCGOP 2
-#define ENOTVSOP 3
-#define ENOTVSAC 4
-#define ENOTOPOP 5
-#define EVSIDINV 10
-#define ENOWSTYP 11
-#define EMAXVSOP 12
-#define EVSNOTOP 13
-#define EVSISACT 14
-#define EVSNTACT 15
-#define EINQALTL 16
-#define EBADRCTD 20
-#define EBDVIEWP 21
-#define ECLIPTOL 22
-#define ECLIPTOS 23
-#define EVDCSDIL 24
-#define EBTBUNDL 30
-#define EBBDTBDI 31
-#define EBTUNDEF 32
-#define EBADLINX 33
-#define EBDWIDTH 34
-#define ECINDXLZ 35
-#define EBADCOLX 36
-#define EBADMRKX 37
-#define EBADSIZE 38
-#define EBADFABX 39
-#define EPATARTL 40
-#define EPATSZTS 41
-#define ESTYLLEZ 42
-#define ENOPATNX 43
-#define EPATITOL 44
-#define EBADTXTX 45
-#define EBDCHRIX 46
-#define ETXTFLIN 47
-#define ECEXFOOR 48
-#define ECHHTLEZ 49
-#define ECHRUPVZ 50
-#define ECOLRNGE 51
-#define ENMPTSTL 60
-#define EPLMTWPT 61
-#define EPLMTHPT 62
-#define EGPLISFL 63
-#define EARCPNCI 64
-#define EARCPNEL 65
-#define ECELLATS 66
-#define ECELLPOS 67
-#define ECELLTLS 68
-#define EVALOVWS 69
-#define EPXNOTCR 70
-#define EINDNOEX 80
-#define EINDINIT 81
-#define EINDALIN 82
-#define EINASAEX 83
-#define EINAIIMP 84
-#define EINNTASD 85
-#define EINTRNEX 86
-#define EINNECHO 87
-#define EINECHON 88
-#define EINEINCP 89
-#define EINERVWS 90
-#define EINETNSU 91
-#define EINENOTO 92
-#define EIAEVNEN 93
-#define EINEVNEN 94
-#define EBADDATA 95
-#define ESTRSIZE 96
-#define EINQOVFL 97
-#define EINNTRQE 98
-#define EINNTRSE 99
-#define EINNTQUE 100
-#define EMEMSPAC 110
-#define ENOTCSTD 111
-#define ENOTCCPW 112
-#define EFILACC 113
-#define ECGIWIN 114
-
-/* devices */
-#define BW1DD 1
-#define BW2DD 2
-#define CG1DD 3
-#define BWPIXWINDD 4
-#define CGPIXWINDD 5
-#define GP1DD 6
-#define CG2DD 7
-#define CG4DD 8
-#define PIXWINDD 9
-
-#define VWSURF_NEWFLG 1
-#define _CGI_KEYBORDS 1
-#define _CGI_LOCATORS 4
-#define _CGI_STROKES 3
-#define _CGI_VALUATRS 3
-#define _CGI_CHOICES 3
-#define _CGI_PICKS 3
-#define MAXVWS 5
-#define MAXTRIG 6
-#define MAXASSOC 5
-#define MAXEVENTS 1024
-
-/* limits */
-#define MAXAESSIZE 10 /* maximum number of AES table entries */
-#define MAXNUMPATS 50 /* maximum number of pattern table entries */
-#define MAXPATSIZE 256 /* maximum pattern size */
-
-#define MAXPTS 1024 /* maximum number of pts per polygon */
-#define MAXCHAR 256 /* maximum number of chars in a string */
-
-#define OUTFUNS 67 /* number of output functions */
-#define INFUNS 22 /* number of input functions */
-
-/* attributes */
-
-/* fonts */
-#define ROMAN 0
-#define GREEK 1
-#define SCRIPT 2
-#define OLDENGLISH 3
-#define STICK 4
-#define SYMBOLS 5
-
-#define DEVNAMESIZE 20
-/* Warning: Because of the ; separators,
- * do not use the following macro unless it is surrounded with { }s.
- * Beware interactions with if-else and ?: constructs.
- */
-#define NORMAL_VWSURF(dev,surf) \
- dev.screenname[0] = '\0'; \
- dev.windowname[0] = '\0'; \
- dev.windowfd = 0; \
- dev.retained = 0; \
- dev.dd = surf; \
- dev.cmapsize = 0; \
- dev.cmapname[0] ='\0'; \
- dev.flags = 0; \
- dev.ptr= '\0' ;
-
-
-#endif
-#endif
\ No newline at end of file
diff --git a/src/simulator/wireframe-simulator/core/vpCgidefs.h b/src/simulator/wireframe-simulator/core/vpCgidefs.h
deleted file mode 100755
index 7ed5c3b..0000000
--- a/src/simulator/wireframe-simulator/core/vpCgidefs.h
+++ /dev/null
@@ -1,525 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCgidefs.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpCgidefs_H
-#define vpCgidefs_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpCgiconstants.h> /* defines constants */
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-typedef char Cchar;
-
-typedef float Cfloat;
-
-typedef int Cint;
-
-typedef enum
-{
- ACTIVE, INACTIVE
-} Cactstate;
-
-typedef enum
-{
- CLEAR, NO_OP, RETAIN
-} Cacttype;
-
-
-typedef enum
-{
- INDIVIDUAL, BUNDLED
-} Casptype;
-
-typedef enum
-{
- VALID_DATA,
- TIMED_OUT,
- DISABLED,
- WRONG_STATE,
- NOT_SUPPORTED
-} Cawresult;
-
-typedef enum
-{
- TRANSPARENT, OPAQUE
-} Cbmode;
-
-typedef enum
-{
- BITNOT, BITTRUE
-} Cbitmaptype;
-
-typedef enum
-{
- HOLLOW, SOLIDI, PATTERN, HATCH
-} Cintertype;
-
-typedef enum
-{
- STRING, CHARACTER, STROKE
-} Cprectype;
-
-typedef enum
-{
- SOLID, DOTTED, DASHED, DASHED_DOTTED, DASH_DOT_DOTTED, LONG_DASHED
-} Clintype;
-
-typedef enum
-{
- DOT, PLUS, ASTERISK, CIRCLE, X
-} Cmartype;
-
-typedef struct
-{
- Clintype line_type;
- Cfloat line_width;
- Cint line_color;
- Cmartype marker_type;
- Cfloat marker_size;
- Cint marker_color;
- Cintertype interior_style;
- Cint hatch_index;
- Cint pattern_index;
- Cint fill_color;
- Clintype perimeter_type;
- Cfloat perimeter_width;
- Cint perimeter_color;
- Cint text_font;
- Cprectype text_precision;
- Cfloat character_expansion;
- Cfloat character_spacing;
- Cint text_color;
-} Cbunatt;
-
-typedef struct
-{
- unsigned char *ra;
- unsigned char *ga;
- unsigned char *ba;
- Cint n;
-} Ccentry;
-
-typedef enum
-{
- OPEN, CLOSE
-} Ccflag;
-
-typedef enum
-{
- YES, NO
-} Cchangetype;
-
-typedef enum
-{
- NOCLIP,
- CLIP,
- CLIP_RECTANGLE
-} Cclip;
-
-typedef enum
-{
- CHORD, PIE
-} Cclosetype;
-
-typedef enum
-{
- REPLACE, AND, OR, NOT, XOR
-} Ccombtype;
-
-typedef struct
-{
- Cint x;
- Cint y;
-} Ccoor;
-
-typedef struct
-{
- Ccoor *ptlist;
- Cint n;
-} Ccoorlist;
-
-typedef struct
-{
- Ccoor *upper;
- Ccoor *lower;
-} Ccoorpair;
-
-typedef enum
-{
- IC_LOCATOR,
- IC_STROKE,
- IC_VALUATOR,
- IC_CHOICE,
- IC_STRING,
- IC_PICK
-} Cdevoff;
-
-typedef enum
-{
- E_TRACK,
- E_ECHO,
- E_TRACK_OR_ECHO,
- E_TRACK_AND_ECHO
-} Cechoav;
-
-typedef enum
-{
- ECHO_OFF, ECHO_ON, TRACK_ON
-} Cechostate;
-
-typedef enum
-{
- NO_ECHO, PRINTERS_FIST, HIGHLIGHT, RUBBER_BAND_BOX,
- DOTTED_LINE, SOLID_LINE, STRING_ECHO, XLINE, YLINE
-} Cechotype;
-
-typedef struct
-{
- Cint n;
- Cechoav *elements;
- Cechotype *echos;
-} Cechotypelst;
-
-typedef struct
-{
- Cechostate *echos;
- Cint n;
-} Cechostatelst;
-
-typedef struct
-{
- int segid; /* segment */
- int pickid; /* pick id */
-} Cpick;
-
-typedef struct
-{
- Ccoor *xypt; /* LOCATOR */
- Ccoorlist *points; /* STROKE devices */
- Cfloat val; /* VALUATOR device */
- Cint choice; /* CHOICE devices */
- Cchar *string; /* STRING device */
- Cpick *pick; /* PICK devices */
-} Cinrep;
-
-typedef struct
-{
- Cinrep *echos;
- Cint n;
-} Cechodatalst;
-
-typedef enum
-{
- NATURAL, POINT, BEST_FIT
-} Cendstyle;
-
-typedef enum
-{
- NO_OFLO, OFLO
-} Ceqflow;
-
-typedef Cint Cerror;
-
-typedef enum
-{
- INTERRUPT, NO_ACTION, POLL
-} Cerrtype;
-
-typedef enum
-{
- CLIP_RECT, VIEWPORT, VIEWSURFACE
-} Cexttype;
-
-typedef enum
-{
- OFF, ON
-} Cflag;
-
-typedef struct
-{
- Cintertype style;
- Cflag visible;
- Cint color;
- Cint hatch_index;
- Cint pattern_index;
- Cint index;
- Clintype pstyle;
- Cfloat pwidth;
- Cint pcolor;
-} Cfillatt;
-
-typedef struct
-{
- Cint n;
- Cint *num;
- Casptype *value;
-} Cflaglist;
-
-typedef enum
-{
- FREEZE, REMOVE
-} Cfreeze;
-
-typedef struct
-{
- Clintype style;
- Cfloat width;
- Cint color;
- Cint index;
-} Clinatt;
-
-typedef enum
-{
- L_FALSE, L_TRUE
-} Clogical;
-
-typedef struct pixrect Cpixrect;
-
-typedef enum
-{
- RELEASE, NO_EVENTS, REQUEST_EVENT, RESPOND_EVENT, QUEUE_EVENT
-} Clidstate;
-
-typedef struct
-{
- Cmartype type;
- Cfloat size;
- Cint color;
- Cint index;
-} Cmarkatt;
-
-typedef enum
-{
- NO_INPUT, ALWAYS_ON, SETTABLE, DEPENDS_ON_LID
-} Cinputability;
-
-typedef struct
-{
- Cint cur_index;
- Cint row;
- Cint column;
- Cint *colorlist;
- Ccoor *point;
- Cint dx;
- Cint dy;
-} Cpatternatt;
-
-typedef enum
-{
- PROMPT_OFF, PROMPT_ON
-} Cpromstate;
-
-typedef enum
-{
- ACK_ON, ACK_OFF
-} Cackstate;
-
-
-typedef struct
-{
- Cint n;
- Cdevoff *class;
- Cint *assoc;
-} Cassoclid;
-
-typedef struct
-{
- Clidstate state;
- Cpromstate prompt;
- Cackstate acknowledgement;
- Cinrep *current;
- Cint n;
- Cint *triggers;
- Cechotype echotyp;
- Cechostate echosta;
- Cint echodat;
-} Cstatelist;
-
-typedef struct
-{
- Clogical sample;
- Cchangetype change;
- Cint numassoc;
- Cint *trigassoc;
- Cinputability prompt;
- Cinputability acknowledgement;
- Cechotypelst *echo;
- Cchar *classdep;
- Cstatelist state;
-} Cliddescript;
-
-typedef enum
-{
- SIMULTANEOUS_EVENT_FOLLOWS, SINGLE_EVENT
-} Cmesstype;
-
-typedef enum
-{
- RIGHT, LEFT, UP, DOWN
-} Cpathtype;
-
-typedef enum
-{
- LFT, CNTER, RGHT, NRMAL, CNT
-} Chaligntype;
-
-typedef enum
-{
- TOP, CAP, HALF, BASE, BOTTOM, NORMAL, CONT
-} Cvaligntype;
-
-typedef enum
-{
- NOT_VALID, EMPTY, NON_EMPTY, ALMOST_FULL, FULL
-} Cqtype;
-
-typedef enum
-{
- ABSOLUTE, SCALED
-} Cspecmode;
-
-typedef enum
-{
- NONE, REQUIRED_FUNCTIONS_ONLY, SOME_NON_REQUIRED_FUNCTIONS,
- ALL_NON_REQUIRED_FUNCTIONS
-} Csuptype;
-
-typedef struct
-{
- Cint fontset;
- Cint index;
- Cint current_font;
- Cprectype precision;
- Cfloat exp_factor;
- Cfloat space;
- Cint color;
- Cint height;
- Cfloat basex;
- Cfloat basey;
- Cfloat upx;
- Cfloat upy;
- Cpathtype path;
- Chaligntype halign;
- Cvaligntype valign;
- Cfloat hcalind;
- Cfloat vcalind;
-} Ctextatt;
-
-typedef enum
-{
- NOT_FINAL, FINAL
-} Ctextfinal;
-
-typedef struct
-{
- Cchangetype change;
- Cassoclid *numassoc;
- Cint maxassoc;
- Cpromstate prompt;
- Cackstate acknowledgement;
- Cchar *name;
- Cchar *description;
-} Ctrigdis;
-
-typedef struct
-{
- Cactstate state;
- Cassoclid *assoc;
-} Ctrigstate;
-
-typedef enum
-{
- INTEGER, REAL, BOTH
-} Cvdctype;
-
-typedef struct
-{
- Cint numloc;
- Cint numval;
- Cint numstrk;
- Cint numchoice;
- Cint numstr;
- Cint numtrig;
- Csuptype event_queue;
- Csuptype asynch;
- Csuptype coord_map;
- Csuptype echo;
- Csuptype tracking;
- Csuptype prompt;
- Csuptype acknowledgement;
- Csuptype trigger_manipulation;
-} Ccgidesctab;
-
-typedef struct
-{
- Cchar screenname[DEVNAMESIZE]; /* physical screen */
- Cchar windowname[DEVNAMESIZE]; /* window */
- Cint windowfd; /* window file */
- Cint retained; /* retained flag */
- Cint dd; /* device */
- Cint cmapsize; /* color map size */
- Cchar cmapname[DEVNAMESIZE]; /* color map name */
- Cint flags; /* new flag */
- Cchar **ptr; /* CGI tool descriptor */
-} Cvwsurf;
-
-
-
-/* define abnormal function calls */
-
-Cpixrect *inquire_device_bitmap();
-Clinatt *inquire_line_attributes();
-Cmarkatt *inquire_marker_attributes();
-Cfillatt *inquire_fill_area_attributes();
-Cpatternatt *inquire_pattern_attributes();
-Ctextatt *inquire_text_attributes();
-Cflaglist *inquire_aspect_source_flags();
-
-/* define abnormal CGIPW function calls */
-Clinatt *cgipw_inquire_line_attributes();
-Cmarkatt *cgipw_inquire_marker_attributes();
-Cfillatt *cgipw_inquire_fill_area_attributes();
-Cpatternatt *cgipw_inquire_pattern_attributes();
-Ctextatt *cgipw_inquire_text_attributes();
-Cflaglist *cgipw_inquire_aspect_source_flags();
-#endif
-
-#endif
\ No newline at end of file
diff --git a/src/simulator/wireframe-simulator/core/vpClipping.cpp b/src/simulator/wireframe-simulator/core/vpClipping.cpp
deleted file mode 100755
index 63c3413..0000000
--- a/src/simulator/wireframe-simulator/core/vpClipping.cpp
+++ /dev/null
@@ -1,489 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpClipping.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "clipping.c" contient les procedures de decoupage
- * d'une scene 3D par l'algorithme de Sutherland et Hodgman.
- * Pour plus de reseignements, voir :
- * I. Sutherland, E. Hodgman, W. Gary.
- * "Reentrant Polygon Clipping".
- * Communications of the ACM,
- * Junary 1974, Volume 17, Number 1, pp 32-44.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpBound.h>
-#include <visp/vpView.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <limits>
-#include <cmath>
-
-void open_clipping (void);
-void close_clipping (void);
-static Index clipping (Byte mask, Index vni, Index *pi, Index *po);
-static Index clipping_Face (Face *fi, Face *fo);
-static Index clipping_Face (Face *fi, Face *fo);
-static void inter (Byte mask, Index v0, Index v1);
-static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
-void set_Point4f_code (Point4f *p4, int size, Byte *cp);
-Byte where_is_Point4f (Point4f *p4);
-
-/*
- * Variables utilisees par le decoupage :
- *
- * 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.
- *
- * 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 :
- * 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.
- * Le tableau recoit ensuite un a un les booleans des points 4D d'intersection
- * de la surface avec les 6 plans de decoupage.
- *
- * POINT4F :
- * Tableau de points durant le decoupage.
- * Le tableau est initialise par les points de la surface en entree apres
- * transforation en coordonnees homogenes 4D.
- * 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 */
-
-#if clip_opt
-static Index *poly0, *poly1; /* polygones temporaires*/
-#else
-static Index *poly_tmp; /* polygone temporaire */
-#endif /* clip_opt */
-
-
-/*
- * La procedure "open_clipping" alloue et initialise les variables utilisees
- * par le mode "clipping".
- */
-void open_clipping (void)
-{
- static char proc_name[] = "open_clipping";
-
- /* alloue la surface de travail */
- malloc_huge_Bound (&clip);
-
- /* alloue les tableaux */
- if ((code = (Byte *) malloc (POINT_NBR * sizeof (Byte))) == NULL
- || (point4f = (Point4f *) malloc (POINT_NBR * sizeof (Point4f))) == NULL
-#ifdef clip_opt
- || (poly0 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL
- || (poly1 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL) {
- perror (proc_name);
- exit (1);
- }
-#else
- || (poly_tmp = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL){
- perror (proc_name);
- exit (1);
- }
-#endif /* clip_opt */
-}
-
-/*
- * La procedure "close_clipping" libere les variables utilisees par
- * le mode "clipping".
- */
-void close_clipping (void)
-{
- free_huge_Bound (&clip);
- free ((char *) code);
- free ((char *) point4f);
-#ifdef clip_opt
- free ((char *) poly0);
- free ((char *) poly1);
-#else
- free ((char *) poly_tmp);
-#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.
- * Sortie :
- * 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.
- */
- Index vno = vni; /* nombre de sommets */
- Index vs = pi[vni-1]; /* premier sommet */
- Index vp; /* second sommet */
- Byte ins = code[vs] & mask; /* code de "vs" */
- Byte inp; /* code de "vp" */
-
- while (vni--) { /* pour tous les sommets */
- vp = *pi++; /* second sommet */
- inp = code[vp] & mask; /* code du plan courant */
-
- if (ins == IS_INSIDE) {
- if (inp == IS_INSIDE) { /* arete interieure */
- *po++ = vp;
- }
- else { /* intersection */
- inter (mask, vs, vp);
- *po++ = point4f_nbr++;
- }
- }
- else {
- if (inp == IS_INSIDE) { /* intersection */
- inter (mask, vs, vp);
- *po++ = point4f_nbr++;
- *po++ = vp;
- vno++;
- }
- else { /* arete exterieure */
- vno--;
- }
- }
- vs = vp;
- ins = inp;
- }
- return (vno);
-}
-
-
-/*
- * 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.
- * Sortie :
- * 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 */
-
- if ((vn = clipping (IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0)
- if ((vn = clipping (IS_BELOW, vn, flip, flop)) != 0)
- if ((vn = clipping (IS_RIGHT, vn, flop, flip)) != 0)
- 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 */
- 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 */
- return (vn);
- }
- return (0);
-}
-
-/*
- * La procedure "clipping_Bound" decoupe une surface par rapport aux 6 plans
- * de decoupage de la pyramide tronquee de vision.
- * 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.
- * Sortie :
- * 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 *fend = fi + bp->face.nbr; /* borne de "fi"*/
- Face *fo = clip.face.ptr; /* face clippee */
-
- /* recopie de "bp" dans les tableaux intermediaires */
-
- point4f_nbr = bp->point.nbr;
- point_3D_4D (bp->point.ptr, (int) point4f_nbr, m, point4f);
- set_Point4f_code (point4f, (int) point4f_nbr, code);
-#ifdef face_normal
- if (! (clip.is_polygonal = bp->is_polygonal))
- //bcopy (bp->normal.ptr, clip.normal.ptr,
- // bp->normal.nbr * sizeof (Vector));
- memmove (clip.normal.ptr, bp->normal.ptr,
- bp->normal.nbr * sizeof (Vector));
-#endif /* face_normal */
- for (; fi < fend; fi++) { /* pour toutes les faces*/
- if (clipping_Face (fi, fo) != 0) {
- fo++; /* ajoute la face a "clip" */
- /*
- * Construction a la volee du future polygone.
- * dont l'espace memoire est deja alloue (voir
- * la procedure "malloc_huge_Bound").
- */
- fo->vertex.ptr = (fo-1)->vertex.ptr+(fo-1)->vertex.nbr;
- }
- }
-
- if (fo == clip.face.ptr)
- return (NULL); /* Rien a voir, circulez... */
-
- /* recopie des tableaux intermediaires dans "clip" */
-
- point_4D_3D (point4f, (int) point4f_nbr, code, clip.point.ptr);
- clip.type = bp->type;
- clip.face.nbr = (Index)( fo - clip.face.ptr );
- clip.point.nbr = point4f_nbr;
-#ifdef face_normal
- if (! bp->is_polygonal)
- clip.normal.nbr = point4f_nbr;
-#endif /* face_normal */
- return (&clip);
-}
-
-/*
- * 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.
- */
-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 */
-
- /* 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) - (p1->w - p1->y);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t;
- t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- 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) - (p1->w + p1->y);
- //t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t;
- t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- 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) - (p1->w - p1->x);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t;
- t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- 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) - (p1->w + p1->x);
- //t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t;
- t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- 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) - (p1->w - p1->z);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t;
- t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = p->z; /* propriete du point d'intersection */
- break;
-
- case IS_FRONT :
- /* 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<double>::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 */
- break;
- }
- /* 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" */
-#ifdef face_normal
- if (! clip.is_polygonal) {
- Vector *n0 = clip.normal.ptr + v0;
- Vector *n1 = clip.normal.ptr + v1;
- Vector *n = clip.normal.ptr + point4f_nbr;
-
- 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 */
-}
-
-/*
- * 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.
- * 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.
- */
-static void
-point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3)
-{
- Point4f *pend = p4 + size; /* borne de p4 */
- float w;
-
- for (; p4 < pend; p4++, p3++) {
- if (*cp++ == IS_INSIDE) { /* point visible */
- w = p4->w;
-
- p3->x = p4->x / w; /* projection 4D en 3D */
- p3->y = p4->y / w;
- p3->z = p4->z / w;
- }
- else { /* marque invisible */
- p3->z = -1.0;
- }
- }
-}
-
-/*
- * 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 resultat.
- */
-void
-set_Point4f_code (Point4f *p4, int size, Byte *cp)
-{
- Point4f *pend = p4 + size; /* borne de p4 */
- Byte b; /* code de p4 */
-
- for (; p4 < pend; p4++, *cp++ = b) {
- b = IS_INSIDE;
- if ( p4->w < p4->y) b |= IS_ABOVE;
- else if (- p4->w > p4->y) b |= IS_BELOW;
- if ( p4->w < p4->x) b |= IS_RIGHT;
- else if (- p4->w > p4->x) b |= IS_LEFT;
- if ( p4->w < p4->z) b |= IS_BACK;
- else if ( -0.9 > p4->z) b |= IS_FRONT;
- }
-}
-
-
-/*
- * La procedure "where_is_Point4f" localise un point 4D par rapport aux 6 plans
- * de decoupage de la pyramide tronquee de vision.
- * Entree :
- * p4 Point homogene 4D a localiser.
- * Sortie :
- * Code indiquant le position de "p4" par rapport aux 6 plans.
- */
-Byte
-where_is_Point4f (Point4f *p4)
-{
- Byte b = IS_INSIDE; /* code de "p4" */
-
- if ( p4->w < p4->y) b |= IS_ABOVE;
- else if (- p4->w > p4->y) b |= IS_BELOW;
- if ( p4->w < p4->x) b |= IS_RIGHT;
- else if (- p4->w > p4->x) b |= IS_LEFT;
- if ( p4->w < p4->z) b |= IS_BACK;
- else if ( -0.9 > p4->z) b |= IS_FRONT;
- return (b);
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpCoreDisplay.cpp b/src/simulator/wireframe-simulator/core/vpCoreDisplay.cpp
deleted file mode 100755
index 6cc784b..0000000
--- a/src/simulator/wireframe-simulator/core/vpCoreDisplay.cpp
+++ /dev/null
@@ -1,240 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCoreDisplay.cpp 5294 2015-02-10 09:41:38Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "display.c" contient les procedures de d'affichage
- * des scenes de modele geometrique surfacique.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <stdio.h>
-#include <stdlib.h>
-/*
-#include <suntool/sunview.h>
-*/
-#ifdef suncgi
-#include "cgidefs.h"
-#endif /* suncgi */
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-//#include "graph.h"
-
-#include <visp/vpBound.h>
-#include <visp/vpView.h>
-#include <visp/vpImstack.h>
-#include <visp/vpRfstack.h>
-#include <visp/vpVwstack.h>
-
-
-/*
- * POINT2I :
- * Tableau de points 2D dans l'espace ecran servant a l'affichage fil-de-fer.
- *
- * RENAME :
- * Tableau de renommage des sommets ou tableau de compteurs associes aux points.
- */
-Point2i *point2i = (Point2i *) NULL;
-Point2i *listpoint2i = (Point2i *) NULL;
-static int *rename_jlc = (int *) NULL;
-
-
-/*
- * La procedure "open_display" alloue et initialise les variables utilisees
- * par le mode "display".
- */
-void open_display (void)
-{
- static char proc_name[] = "open_display";
-
- if ((point2i = (Point2i *) malloc (POINT_NBR*sizeof (Point2i))) == NULL
- || (listpoint2i = (Point2i *) malloc (50*sizeof (Point2i))) == NULL
- || (rename_jlc = (int *) malloc (POINT_NBR * sizeof (int))) == NULL)
- {
- perror (proc_name);
- exit (1);
- }
-}
-
-/*
- * La procedure "close_display" libere les variables utilisees par le mode
- * "display".
- */
-void close_display (void)
-{
- free ((char *) point2i);
- free ((char *) listpoint2i);
- free ((char *) rename_jlc);
- point2i = (Point2i *) NULL;
- listpoint2i = (Point2i *) NULL;
- rename_jlc = (int *) NULL;
-}
-
-/*
- * La procedure "point_3D_2D" projette les points 3D du volume canonique
- * dans l'espace image 2D.
- *
- * Volume canonique Espace image
- * ________________ ____________
- *
- * - 1 < X < 1 0 < X < xsize
- * - 1 < Y < 1 0 < Y < ysize
- * 0 < Z < 1
- *
- * 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.
- */
-// static
-void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
-{
- Point3f *pend = p3 + size; /* borne de p3 */
- float xdiv2 = ((float) xsize) / (float)2.0;
- float ydiv2 = ((float) ysize) / (float)2.0;
-
- for (; p3 < pend; p3++, p2++) {
- p2->x = (int) ((1.0 + p3->x) * xdiv2);
- p2->y = (int) ((1.0 - p3->y) * ydiv2);
- }
-}
-
-/*
- * La procedure "set_Bound_face_display" marque les faces affichables
- * de la surface "bp".
- * 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 |
- *
- * 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.
- * Entree :
- * bp Surface a initialiser.
- * b Drapeaux indiquant les faces non affichables.
- */
-void set_Bound_face_display (Bound *bp, Byte b)
-{
- Face *fp = bp->face.ptr;
- Face *fend = fp + bp->face.nbr;
- Point3f *pp = bp->point.ptr;
-
- for (; fp < fend; fp++) {
- Index *vp;
- Point3f *p0; /* premier sommet */
- Point3f *p1; /* second sommet */
- Point3f *p2; /* dernier sommet */
-
- fp->is_visible = TRUE;
- if (b == IS_INSIDE) continue;
- vp = fp->vertex.ptr;
- p0 = pp + *vp;
- p1 = pp + *(vp + 1);
- p2 = pp + *(vp + fp->vertex.nbr - 1);
- if (b & IS_ABOVE) {
- fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
- >= (p1->x - p0->x) * (p2->z - p0->z));
- }
- if (! fp->is_visible) continue;
- if (b & IS_BELOW) {
- fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
- <= (p1->x - p0->x) * (p2->z - p0->z));
- }
- if (! fp->is_visible) continue;
- if (b & IS_RIGHT) {
- fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
- >= (p1->z - p0->z) * (p2->y - p0->y));
- }
- if (! fp->is_visible) continue;
- if (b & IS_LEFT) {
- fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
- <= (p1->z - p0->z) * (p2->y - p0->y));
- }
- if (! fp->is_visible) continue;
- if (b & IS_BACK) {
- fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
- >= (p1->y - p0->y) * (p2->x - p0->x));
- }
- if (! fp->is_visible) continue;
- if (b & IS_FRONT) {
- fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
- <= (p1->y - p0->y) * (p2->x - p0->x));
- }
- }
-}
-
-
-/*
- * La procedure "wireframe_Face" affiche une face "fp" en "fil de fer".
- * sur la fenetre graphique de "suncgi" sur "SUN".
- * 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.
- */
-void wireframe_Face (Face *fp, Point2i *pp)
-{
-// extern Window id_window;
-
- Index *vp = fp->vertex.ptr;
- Index *vend = vp + fp->vertex.nbr;
- Point2i *cp = listpoint2i;
-
- if (fp->vertex.nbr < 2) return;
- if (fp->vertex.nbr > 50)
- {
- printf("pb malloc listpoint2i (display.c)\n"); return;
- }
- for (; vp < vend; vp++, cp++) {
- SET_COORD2(*cp, pp[*vp].x, pp[*vp].y);
- }
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpImstack.h b/src/simulator/wireframe-simulator/core/vpImstack.h
deleted file mode 100755
index 1fa1061..0000000
--- a/src/simulator/wireframe-simulator/core/vpImstack.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImstack.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "imstack.h" contient les macros, les types et
- * les specifications des procedures de gestion de la pile
- * des images (IMage STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-#ifndef vpImstack_H
-#define vpImstack_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void fprintf_imstack (void);
-void load_imstack (void);
-void pop_imstack (void);
-void push_imstack (void);
-void swap_imstack (void);
-void add_imstack (void);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpKeyword.cpp b/src/simulator/wireframe-simulator/core/vpKeyword.cpp
deleted file mode 100755
index d89143a..0000000
--- a/src/simulator/wireframe-simulator/core/vpKeyword.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyword.cpp 5310 2015-02-11 11:57:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "keyword.c" contient les procedures de gestion
- * des mots cles retournes par l'analyseur lexical "lex".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpToken.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void open_keyword (Keyword *kwp);
-static void open_hash (void);
-static void close_hash (void);
-static int hashpjw (const char *str);
-static void insert_keyword (const char *str, Index token);
-Index get_symbol (char *ident, int length);
-
-#ifdef debug
-static void delete_keyword (void);
-static char *get_keyword (void);
-#endif /* debug */
-
-
-#define PRIME 211
-#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 */
-} Bucket;
-
-
-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.
- */
-void open_keyword (Keyword *kwp)
-{
- open_hash ();
- for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */
- insert_keyword (kwp->ident, kwp->token);
-}
-
-/*
- * La procedure "close_keyword" libere les variables utilisees
- * par les procedures de gestion des mots cles.
- */
-void close_keyword (void)
-{
- close_hash ();
-}
-
-/*
- * La procedure "open_hash" alloue et initialise la table de codage.
- */
-static void
-open_hash (void)
-{
- static char proc_name[] = "open_hash";
-
- Bucket **head, **bend;
-
- if ((hash_tbl = (Bucket **) malloc (sizeof (Bucket *) * PRIME))==NULL){
- perror (proc_name);
- exit (1);
- }
- head = hash_tbl;
- bend = head + PRIME;
- for (; head < bend; *head++ = NULL) {};
-}
-
-/*
- * La procedure "close_hash" libere la table de codage et ses elements.
- */
-static void
-close_hash (void)
-{
- Bucket **head = hash_tbl;
- Bucket **bend = head + PRIME;
- Bucket *bp; /* element courant */
- Bucket *next; /* element suivant */
-
- 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 */
-}
-
-/*
- * 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.
- * Entree :
- * str Chaine de caracteres a coder.
- * Sortie :
- * Le code de la chaine.
- */
-static int
-hashpjw (const char *str)
-{
- unsigned h = 0; /* "hash value" */
- unsigned g;
-
- for (; *str != '\0'; str++) {
- h = (h << 4) + *str;
- if ((g = h & ~0xfffffff) != 0) {
- h ^= g >> 24;
- h ^= g;
- }
- }
- return (h % PRIME);
-}
-
-
-/*
- * La procedure "insert_keyword" insere en tete d'un point d'entree
- * 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.
- */
-static void
-insert_keyword (const char *str, Index token)
-{
- static const char proc_name[] = "insert_keyword";
-
- Bucket **head = hash_tbl + hashpjw (str);
- Bucket *bp;
- Byte length;
-
- length = (Byte)( strlen(str) ); // Warning! Overflow possible!
- if ((bp = (Bucket *) malloc (sizeof (Bucket) + length + 1)) == NULL) {
- perror (proc_name);
- exit (1);
- }
- bp->length = length;
- bp->token = token;
- bp->ident = (char *) (bp + 1);
- strcpy (bp->ident, str);
-
- bp->next = *head; /* insere "b" en tete de "head" */
- *head = bp;
-}
-
-/*
- * 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.
- * Note :
- * La chaine "ident" n'est pas terminee par '\0'.
- * Sortie :
- * Valeur du jeton associe si c'est un mot cle, 0 sinon.
- */
-Index get_symbol (char *ident, int length)
-{
- Bucket *bp;
- const char *kwd;
- char *idn = ident;
- int len = length;
-
- { /* calcule le code de hachage (voir "hashpjw") */
- unsigned h = 0; /* "hash value" */
- unsigned g;
-
- for (; len != 0; idn++, len--) {
- h = (h << 4) + *idn;
- if ((g = h & ~0xfffffff) != 0) {
- h ^= g >> 24;
- h ^= g;
- }
- }
- bp = hash_tbl[h % PRIME];
- }
-
- /* recherche le mot cle */
-
- for (; bp != NULL; NEXT(bp)) {
- if (length == bp->length) {
- idn = ident;
- len = length;
- kwd = bp->ident;
- for (; *idn == *kwd; idn++, kwd++)
- if (--len == 0) return (bp->token);
- }
- }
- return (0); /* identificateur */
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpKeyword.h b/src/simulator/wireframe-simulator/core/vpKeyword.h
deleted file mode 100644
index f75bf11..0000000
--- a/src/simulator/wireframe-simulator/core/vpKeyword.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyword.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "keyword.c" contient les procedures de gestion
- * des mots cles retournes par l'analyseur lexical "lex".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpKeyword_H
-#define vpKeyword_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpMy.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-extern Index get_symbol (char *ident, int length);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpLex.cpp b/src/simulator/wireframe-simulator/core/vpLex.cpp
deleted file mode 100755
index 108ac69..0000000
--- a/src/simulator/wireframe-simulator/core/vpLex.cpp
+++ /dev/null
@@ -1,822 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLex.cpp 5310 2015-02-11 11:57:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "lex.c" contient les procedures de gestion
- * de l'analyse lexicale de l'analyseur lexicale "lex"
- * d'un fichier source dont la grammaire possede
- * les symboles terminaux suivants (ecrit en "LEX", UNIX) :
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpToken.h>
-#include <visp/vpKeyword.h>
-
-#include <ctype.h>
-#include <math.h>
-#include <stdio.h>
-//#include <unistd.h>
-#include <fcntl.h>
-
-
-// #include <varargs.h> /* modif pour Fedora */
-#include <stdarg.h>
-
-#include <stdlib.h>
-#include <string.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-static void count (void);
-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 */
-
-#define ASCII_NBR 128 /* nombre de codes ASCII*/
-
-#ifndef EOB
-#define EOB (-2) /* fin de buffer */
-#endif
-#ifndef EOF
-#define EOF (-1) /* fin de fichier */
-#endif
-#ifndef EOL
-#define EOL 10 /* fin de ligne */
-#endif
-
-#define CHAR_NBR 130 /* nombre de caracteres */
-
-/* Tests des drapeaux */
-
-#define isnult(c) (scantbl[c] == _NULT)
-#define iscmtt(c) (scantbl[c] & _CMTT)
-#define isfptt(c) (scantbl[c] & _FPTT)
-#define isidnt(c) (scantbl[c] & _IDNT)
-#define isintt(c) (scantbl[c] & _INTT)
-#define issgnt(c) (scantbl[c] & _SGNT)
-#define isstgt(c) (scantbl[c] & _STGT)
-
-/*
- * Codes des messages d'erreur de l'analyseur lexicale.
- */
-#define E_UNKNOWN 0
-#define E_SYMBOL 1
-#define E_CMT_EOF 2
-#define E_FLOAT 3
-#define E_INT 4
-#define E_KEYWORD 5
-#define E_STG_EOF 6
-#define E_STG_EOL 7
-#define E_STRING 8
-#define E_9 9
-
-
-const char *lex_errtbl[] = { /* table des messages d'erreur */
- "error unknown",
- "symbol undefined",
- "unexpected EOF in comment",
- "float expected",
- "int expected",
- "keyword expected",
- "unexpected EOF in string or char constant",
- "newline in string or char constant",
- "string expected",
- ""
-};
-
-char *mytext = NULL;
-int mylength = 0;
-int mylineno = 1;
-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 Byte *chtbl; /* premiers caracteres des terminaux */
-static Byte *scantbl; /* caracteres suivants des terminaux */
-
-
-/*
- * La procedure "open_lex" alloue et initialise les variables utilisees
- * par l'analyseur lexical "lex".
- */
-void open_lex (void)
-{
- static char proc_name[] = "open_lex";
-
- int i;
-
- if ((chtbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL
- || (scantbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL) {
- perror (proc_name);
- exit (1);
- }
- chtbl += 2; /* 2 sentinelles non affichables */
- scantbl += 2;
-
- /* initialise les premiers caracteres des symboles terminaux */
-
- for (i = 0; i < ASCII_NBR; i++) {
- if (isalpha(i)) chtbl[i] = IDNT;
- else if (isdigit(i)) chtbl[i] = INTT;
- else if (isspace(i)) chtbl[i] = SPCT;
- else switch (i) {
- case '"' : chtbl[i] = STGT; break;
- case '+' :
- case '-' : chtbl[i] = SGNT; break;
- case '.' : chtbl[i] = FPTT; break;
- case '/' : chtbl[i] = CMTT; break;
- case '_' : chtbl[i] = IDNT; break;
- default : chtbl[i] = NULT; break;
- }
- }
-
- /* Initialise les sentinelles comme des terminaux. */
-
- chtbl[EOB] = EOBT;
- chtbl[EOF] = EOFT;
- chtbl[EOL] = EOLT;
-
- /* Initialise les caracteres suivants des symboles terminaux. */
-
- for (i = 0; i < ASCII_NBR; i++) {
- if (isalpha(i)) scantbl[i] = _CMTT|_IDNT|_STGT;
- else if (isdigit(i)) scantbl[i] = _CMTT|_IDNT|_INTT|_STGT;
- else switch (i) {
- case '"' : scantbl[i] = _CMTT; break;
- case '+' :
- case '-' : scantbl[i] = _CMTT|_SGNT|_STGT; break;
- case '.' : scantbl[i] = _CMTT|_FPTT|_STGT; break;
- case '/' : scantbl[i] = _STGT; break;
- case '_' : scantbl[i] = _CMTT|_IDNT|_STGT; break;
- default : scantbl[i] = _CMTT|_STGT; break;
- }
- }
-
- /* Initialise les sentinelles comme des terminaux. */
-
- scantbl[EOB] = _NULT;
- scantbl[EOF] = _NULT;
- scantbl[EOL] = _NULT;
-}
-
-/*
- * La procedure "close_lex" libere les variables utilisees
- * par l'analyseur lexical "lex".
- */
-void close_lex (void)
-{
- 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 */
-
-
-/*
- * La procedure "lex" contient l'analyseur lexical.
- * Note :
- * 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.
- */
-int lex (void)
-{
-lex_loop :
-
- for (; chtbl[(int)CURC] == SPCT; mysptr++) {}; /* saute les espaces */
-
- switch (chtbl[(int)CURC]) {
-
- case NULT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- return (*mytext);
- break;
- case EOBT :
- next_source ();
- goto lex_loop;
- break;
- case EOFT :
- mytext = mysptr; /* sauvegarde le jeton */
- return (T_EOF);
- break;
- case EOLT :
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto lex_loop;
- break;
- case CMTT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (CURC != '*')
- return (*mytext);
- mysptr++;
-comment :
- for (; iscmtt((int)CURC); mysptr++) {};
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto comment;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
- return (T_EOF);
- break;
- case EOLT :
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto comment;
- break;
- case CMTT :
- if (PREVC == '*') { /* veritable fin */
- mysptr++;
- goto lex_loop;
- }
- mysptr++; /* pseudo fin */
- goto comment;
- break;
- }
- break;
- case IDNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- for (; isidnt((int)CURC); mysptr++) {};
- mylength = (int)(mysptr - mytext);
- return (get_symbol (mytext, mylength));
- break;
- case INTT :
- 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 */
-float_part :
- mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
- if (CURC != 'E' && CURC != 'e') {
- myfloat = (float) atof (mytext);
-/* FC
-printf("mytext %s, myfloat %f\n",mytext,myfloat);
-*/
- return (T_FLOAT);
- }
- break;
- case 'E' : /* lecture exposant */
- case 'e' :
- mysptr++;
- if (isintt((int)CURC))
- mysptr++;
- else if (issgnt((int)CURC) && isintt((int)NEXTC))
- mysptr +=2;
- else {
- mysptr--;
- myfloat = (float) atof (mytext);
- return (T_FLOAT);
- }
- for (; isintt((int)CURC); mysptr++) {};
- myfloat = (float) atof (mytext);
- return (T_FLOAT);
- break;
- default :
- if (*mytext == '-')
- myint = - myint;
- return (T_INT);
- break;
- }
- break;
- case FPTT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (! isintt((int)CURC)) /* pas de fraction */
- return (*mytext);
- goto float_part;
- break;
- case SGNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (isintt((int)CURC))
- goto int_part;
- if (isfptt((int)CURC) && isintt((int)NEXTC))
- goto float_part;
- return (*mytext);
- break;
- case STGT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
-string :
- for (; isstgt((int)CURC); mysptr++) {};
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto string;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
- return ('\n');
- break;
- case EOLT :
- lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
- return ('\n');
- break;
- case STGT :
- if (PREVC != '\\') { /* veritable fin */
- mytext++;
- mylength = (int)(mysptr - mytext);
- mysptr++;
- return (T_STRING);
- }
- mysptr++; /* pseudo fin */
- goto string;
- break;
- }
- break;
- default :
- ECHO;
- mysptr++;
- goto lex_loop;
- break;
- }
- return (T_EOF);
-}
-
-/*
- * La procedure "lexecho" contient l'analyseur lexical "lex" :
- * 1 Analyse le fichier source,
- * 2 Affiche le fichier source sur le fichier "f",
- * 3 Stoppe devant le jeton "token".
- * Note :
- * 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.
- * Sortie :
- * Code du symbole terminale analyse.
- */
-int lexecho (FILE *f, int token)
-{
-lex_loop :
- 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 */
- mysptr++;
- if (token != *mytext)
- fwrite (mytext, 1, 1, f);
- return (*mytext);
- break;
- case EOBT :
- next_source ();
- goto lex_loop;
- break;
- case EOFT :
- mytext = mysptr; /* sauvegarde le jeton */
- return (T_EOF);
- break;
- case EOLT :
- fwrite (mysptr, 1, 1, f);
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto lex_loop;
- break;
- case CMTT :
- fwrite (mysptr, 1, 1, f);
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (CURC != '*')
- return (*mytext);
- fwrite (mysptr, 1, 1, f);
- mysptr++;
-comment :
- for (; iscmtt((int)CURC); mysptr++)
- fwrite (mysptr, 1, 1, f);
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto comment;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
- return (T_EOF);
- break;
- case EOLT :
- fwrite (mysptr, 1, 1, f);
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto comment;
- break;
- case CMTT :
- fwrite (mysptr, 1, 1, f);
- if (PREVC == '*') { /* veritable fin */
- mysptr++;
- goto lex_loop;
- }
- mysptr++; /* pseudo fin */
- goto comment;
- break;
- }
- break;
- case IDNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- for (; isidnt((int)CURC); mysptr++) {};
- mylength = (int)(mysptr - mytext);
- if (token != get_symbol (mytext, mylength))
- fwrite (mytext, mylength, 1, f);
- return (get_symbol (mytext, mylength));
- break;
- case INTT :
- mytext = mysptr; /* sauvegarde le jeton */
-int_part :
- mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
- switch (CURC) {
- case '.' : /* lecture fraction */
-float_part :
- mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
- if (CURC != 'E' && CURC != 'e') {
- if (token != T_FLOAT)
- fwrite (mytext, mysptr - mytext, 1, f);
- return (T_FLOAT);
- }
- break;
- case 'E' : /* lecture exposant */
- case 'e' :
- mysptr++;
- if (isintt((int)CURC)) mysptr++;
- else if (issgnt((int)CURC) && isintt((int)NEXTC)) mysptr +=2;
- else {
- mysptr--;
- if (token != T_FLOAT)
- fwrite (mytext, mysptr - mytext, 1, f);
- return (T_FLOAT);
- }
- for (; isintt((int)CURC); mysptr++) {};
- if (token != T_FLOAT)
- fwrite (mytext, mysptr - mytext, 1, f);
- return (T_FLOAT);
- break;
- default :
- if (token != T_INT)
- fwrite (mytext, mysptr - mytext, 1, f);
- return (T_INT);
- break;
- }
- break;
- case FPTT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (! isintt((int)CURC)) { /* pas de fraction */
- if (token != *mytext)
- fwrite (mytext, 1, 1, f);
- return (*mytext);
- }
- goto float_part;
- break;
- case SGNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (isintt((int)CURC)) goto int_part;
- if (isfptt((int)CURC) && isintt((int)NEXTC)) goto float_part;
- if (token != *mytext)
- fwrite (mytext, 1, 1, f);
- return (*mytext);
- break;
- case STGT :
- fwrite (mysptr, 1, 1, f);
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
-string :
- for (; isstgt((int)CURC); mysptr++)
- fwrite (mysptr, 1, 1, f);
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto comment;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
- return (T_EOF);
- break;
- case EOLT :
- lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
- return ('\n');
- break;
- case STGT :
- fwrite (mysptr, 1, 1, f);
- if (PREVC != '\\') { /* veritable fin */
- mytext++;
- mylength = (int)(mysptr - mytext);
- mysptr++;
- return (T_STRING);
- }
- mysptr++; /* pseudo fin */
- goto string;
- break;
- }
- break;
- default :
- fwrite (mysptr, 1, 1, f);
- mysptr++;
- goto lex_loop;
- break;
- }
- return (T_EOF);
-}
-
-
-#undef BUFSIZE
-#undef LINESIZE
-#undef TEXTSIZE
-
-#define BUFSIZE (BUFSIZ << 5)
-#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 */
-
-
-/*
- * La procedure "unlex" recule la tete de lecture devant le dernier jeton.
- */
-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.
- */
-void open_source (FILE *fd, const char *str)
-{
- static char proc_name[] = "open_source";
-
- if ((source = (char *) malloc((strlen(str)+1)*sizeof(char))) == NULL) {
- perror (proc_name);
- exit (1);
- }
- strcpy (source, str);
- if ((botbuf = (char *) malloc (TEXTSIZE * sizeof (char))) == NULL) {
- perror (proc_name);
- exit (1);
- }
- fds = fd;
- buf = botbuf + 1 + LINESIZE;
- topbuf = buf + 1;
- mylineno = 1;
- next_source ();
-}
-
-/*
- * La procedure "close_source" libere les variables utilisees pour la gestion
- * des entrees du programme source.
- */
-void close_source (void)
-{
- free ((char *) source);
- free ((char *) botbuf);
-}
-
-/*
- * La procedure "next_source" remplit le buffer courant.
- */
-static void next_source (void)
-{
- size_t size;
- char *bot = buf;
- char *top = topbuf;
-
- /* recopie la derniere ligne devant "buf" */
-
- *bot = EOL; /* evite le debordement de "buf" */
- while ((*--bot = *--top) != EOL) {};
- myline = mysptr = bot + 1;
-
- size = fread (buf,sizeof (char), BUFSIZE,fds);
- if (size == 0) {
- topbuf = buf + 1;
- *buf = EOF;
- *topbuf = EOB; /* sentinelle de fin de fichier */
- mysptr = buf;
- }
- else {
- topbuf = buf + size;
- *topbuf = EOB; /* sentinelle de fin de buffer */
-
- /* recherche de la derniere ligne */
- top = topbuf;
- while (*--top != EOL) {};
- lastline = top;
- }
-}
-
-
-/*
- * 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".
- */
-#define ERR_STACK_MAX 32
-
-
-static const char *err_stack[ERR_STACK_MAX];
-static int size_stack = 0;
-
-
-/*
- * La procedure "count" calcule la distance en espaces entre
- * le premier caractere "*mytext" et le caractere de debut de ligne "*myline".
- */
-static void count (void)
-{
- char *str;
-
- mycolumno = 0;
- for (str = myline; str <= mytext; str++) {
- (*str == '\t') ? mycolumno += 8 - (mycolumno % 8u) : mycolumno++;
- }
-}
-
-/*
- * La procedure "lexerr" affiche les messages d'erreur.
- * 1 elle affiche la ligne d'erreur du fichier source.
- * 2 elle indique la position de l'erreur dans la ligne.
- * 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.
- */
-
-//lexerr (va_alist)
-//va_dcl
-
-void lexerr (const char* path, ...)
-{
- va_list ap;
- char *cp;
- int i;
-
- /* 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. */
-
- for (i = 0; i < size_stack; i++)
- fprintf (stderr, "%s", err_stack[i]);
-
- /* Affiche les messages d'erreur en parametres. */
-
- va_start(ap,path);
- while ((cp = (char *) va_arg(ap, char *)) != NULL)
- fprintf (stderr, "%s", cp);
- fprintf (stderr,"\n");
- va_end(ap);
-
- exit (1);
-}
-
-/*
- * La procedure "poperr" depile le message d'erreur du sommet de pile.
- */
-void poperr (void)
-{
- static char proc_name[] = "poperr";
-
- if (--size_stack < 0) {
- fprintf (stderr, "%s: error stack underflow\n", proc_name);
- exit (1);
- }
-}
-
-/*
- * La procedure "popup_error" remplace le message d'erreur du sommet de pile.
- */
-void popuperr (const char *str)
-{
- static const char proc_name[] = "popuerr";
-
- if (size_stack <= 0) {
- fprintf (stderr, "%s: error stack underflow\n", proc_name);
- exit (1);
- }
- err_stack[size_stack-1] = str;
-}
-
-/*
- * La procedure "pusherr" empile le message d'erreur.
- */
-void pusherr (const char *str)
-{
- static const char proc_name[] = "pusherr";
-
- if (size_stack >= ERR_STACK_MAX) {
- fprintf (stderr, "%s: error stack overflow\n", proc_name);
- exit (1);
- }
- err_stack[size_stack++] = str;
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpLex.h b/src/simulator/wireframe-simulator/core/vpLex.h
deleted file mode 100644
index 859426d..0000000
--- a/src/simulator/wireframe-simulator/core/vpLex.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLex.h 5297 2015-02-10 11:19:24Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "lex.c" contient les procedures de gestion
- * de l'analyse lexicale de l'analyseur lexicale "lex"
- * d'un fichier source dont la grammaire possede
- * les symboles terminaux suivants (ecrit en "LEX", UNIX) :
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpLex_h
-#define vpLex_h
-
-#include <visp/vpConfig.h>
-#include <stdio.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-int lex(void);
-void unlex (void);
-void lexerr (const char* path, ...);
-void pusherr (const char *str);
-void popuperr (const char *str);
-void poperr (void);
-int lexecho (FILE *f, int token);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpMy.h b/src/simulator/wireframe-simulator/core/vpMy.h
deleted file mode 100755
index ac303f7..0000000
--- a/src/simulator/wireframe-simulator/core/vpMy.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMy.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Module de Macros et de Types de bases en langage "C".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-#ifndef vpMy_H
-#define vpMy_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifndef NULL
-#define NULL 0
-#endif
-#ifndef FALSE
-#define FALSE 0
-#endif
-#ifndef TRUE
-#define TRUE 1
-#endif
-
-#ifndef STDIN
-#define STDIN 0
-#endif
-#ifndef STDOUT
-#define STDOUT 1
-#endif
-#ifndef STDERR
-#define STDERR 2
-#endif
-
-#define NAMESIZE 80
-#define LINESIZE 256
-
-
-#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)) (MIN) = (M);\
- else if ((M) > (MAX)) (MAX) = (M)
-
-#define TWO_POWER(P) (((P) > 0) ? 1 << (P) : 1)
-#define SWAP(A,B,T) { (T) = (A); (A) = (B); (B) = (T); }
-
-typedef unsigned char Byte;
-typedef unsigned short Index;
-typedef char Type;
-
-void fscanf_float (float *fp);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpMyio.cpp b/src/simulator/wireframe-simulator/core/vpMyio.cpp
deleted file mode 100755
index 3eca168..0000000
--- a/src/simulator/wireframe-simulator/core/vpMyio.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMyio.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "myio.c" contient les procedures d'entree/sortie
- * des types definis dans le module "my.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
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpToken.h>
-#include <visp/vpLex.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-extern char *mytext; /* chaine du symbole courant */
-
-
-/*
- * La procedure "fscanf_float" lit en ascii un nombre flottant.
- * Entree :
- * fp Nombre flottant a lire.
- */
-void fscanf_float (float *fp)
-{
- int t;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", "float expected", NULL);
- *fp = (t == T_INT) ? (float) myint : myfloat;
-}
-
-/*
- * La procedure "fscanf_Index" lit en ascii un indice.
- * Entree :
- * ip Indice a lire.
- */
-void fscanf_Index (Index *ip)
-{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = (Index) myint;
-}
-
-/*
- * La procedure "fscanf_int" lit en ascii un nombre entier.
- * Entree :
- * ip Nombre entier a lire.
- */
-void fscanf_int (int *ip)
-{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = myint;
-}
-
-/*
- * La procedure "fscanf_string" lit en ascii une chaine de caracteres.
- * Entree :
- * str Chaine a lire.
- */
-void fscanf_string (char **str)
-{
- if (lex () != T_STRING)
- lexerr ("start", "string expected", NULL);
- if (*str == NULL)
- *str = (char *) malloc ((mylength + 1) * sizeof (char));
- else
- *str = (char *) realloc (*str, (mylength + 1) * sizeof (char));
-
- if (*str == NULL) {
- printf("Unable to read the string: bad memory allocation");
- return;
- }
-
- strncpy (*str, mytext, mylength);
-}
-
-/*
- * La procedure "fscanf_Type" lit en ascii un octet.
- * Entree :
- * ip Type a lire.
- */
-void fscanf_Type (Type *ip)
-{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = (Type ) myint;
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpParser.cpp b/src/simulator/wireframe-simulator/core/vpParser.cpp
deleted file mode 100755
index e5092b6..0000000
--- a/src/simulator/wireframe-simulator/core/vpParser.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParser.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "parser.c" contient les procedures de gestion
- * de l'analyse syntaxique d'un fichier source dont la grammaire
- * possede les symboles terminaux de "lex.c".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpView.h>
-#include <visp/vpBound.h>
-#include <visp/vpToken.h>
-#include <visp/vpLex.h>
-#include <visp/vpSkipio.h>
-
-#include <stdio.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef used
-extern Byte *get_remove (void);
-extern View_parameters *get_view_parameters (void);
-#endif /* used */
-
-/*
- * La procedure "parser" fait l'analyse syntaxique du fichier source.
- * Entree/Sortie :
- * bsp Scene surfacique polygonale a lire.
- */
-void parser (Bound_scene *bsp)
-{
- int token;
-
- while ((token = lex ()) != T_EOF)
- switch (token) {
- case '$' :
- switch (lex ()) {
- case T_IDENT : /* saute la commande inconnue */
- skip_cmd (/* stderr */);
- unlex ();
- break;
- case T_EXIT :
- return;
- break;
- case T_BOUND :
- if (bsp->bound.nbr == BOUND_NBR) {
- fprintf (stderr, "mire: too much bound\n");
- return;
- }
- fscanf_Bound (
- &(bsp->bound.ptr[bsp->bound.nbr++]));
- break;
-#ifdef used
- case T_REMOVE :
- fscanf_Remove (get_remove ());
- break;
- case T_VIEW :
- fscanf_View_parameters (get_view_parameters ());
- set_projection (void);
- break;
-#endif /* used */
- default :
- lexerr ("start", "keyword expected", NULL);
- break;
- }
- break;
- default :
- lexerr ("start", "symbol '$' expected", NULL);
- break;
- }
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpProjection.cpp b/src/simulator/wireframe-simulator/core/vpProjection.cpp
deleted file mode 100755
index f0f2aca..0000000
--- a/src/simulator/wireframe-simulator/core/vpProjection.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpProjection.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "projection.c" contient les procedures de calcul
- * des matrices de projection perspective et parallele.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpView.h>
-#include <stdio.h>
-#include <math.h>
-void set_parallel (View_parameters*, Matrix);
-void set_perspective (View_parameters*, Matrix);
-
-
-
-/*
- * 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.
- */
-void View_to_Matrix (View_parameters *vp, Matrix m)
-{
- static char proc_name[] = "View_to_Matrix";
-
- switch (vp->type) {
- case PARALLEL :
- set_parallel (vp, m);
- break;
- case PERSPECTIVE :
- set_perspective (vp, m);
- break;
- default :
- fprintf (stderr, "%s: bad view type\n", proc_name);
- set_perspective (vp, m);
- break;
- }
-}
-
-
-/*
- * 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.
- * Entree :
- * m Matrice a initialiser.
- * v0 Premier vecteur.
- * v1 Second vecteur.
- */
-static void set_zy (Matrix m, Vector *v0, Vector *v1)
-{
- Vector rx, ry, rz;
-
- SET_COORD3(rz, - v0->x,- v0->y, - v0->z);
- CROSS_PRODUCT(rx, *v0, *v1);
- norm_vector (&rx);
- norm_vector (&rz);
- CROSS_PRODUCT (ry,rz,rx); /* ry est norme */
-
- m[0][0] = rx.x; m[0][1] = ry.x; m[0][2] = rz.x; m[0][3] = 0.0;
- m[1][0] = rx.y; m[1][1] = ry.y; m[1][2] = rz.y; m[1][3] = 0.0;
- m[2][0] = rx.z; m[2][1] = ry.z; m[2][2] = rz.z; m[2][3] = 0.0;
- m[3][0] = 0.0 ; m[3][1] = 0.0 ; m[3][2] = 0.0 ; m[3][3] = 1.0;
-}
-
-/*
- * 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.
- * Entree :
- * vp Parametres de visualisation.
- * wc Matrice a initialiser.
- */
-void set_parallel (View_parameters *vp, Matrix wc)
-{
- Matrix m = IDENTITY_MATRIX;
- Point3f cop;
- Point4f doprim;
- Vector dop, v;
-
- /*
- * 1 : Translation du point de reference VRP a l'origine.
- */
- SET_COORD3(v,- vp->vrp.x, - vp->vrp.y, - vp->vrp.z);
- Translate_to_Matrix (&v, wc);
- /*
- * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
- * 3 : Rotation pour rendre la projection de VUP sur le plan de
- * projection parallele a l'axe Y.
- */
- set_zy (m, &vp->vpn, &vp->vup);
- /*
- * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
- */
- postleft_matrix (m, 'z');
- postmult_matrix (wc, m);
- /*
- * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
- * COP = DOP = Direction of Projection.
- * DOPRIM = DOP * R_TRL
- * Pas de translation dans la matrice R_TRL pour la transformation
- * du vecteur DOP.
- */
- SET_COORD3(dop,
- vp->vrp.x - vp->cop.x,
- vp->vrp.y - vp->cop.y,
- vp->vrp.z - vp->cop.z);
- norm_vector (&dop);
- SET_COORD3(cop,dop.x,dop.y,dop.z);
- point_matrix (&doprim, &cop, m);
- ident_matrix (m);
- m[2][0] = - doprim.x / doprim.z;
- m[2][1] = - doprim.y / doprim.z;
- postmult_matrix (wc, m);
- /*
- * 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
- * je prefere, afin de rester coherent avec la projection perspective,
- * -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));
- posttrans_matrix (wc, &v);
- SET_COORD3(v,
- (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)),
- (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
- (float)(1.0 / (vp->depth.back - vp->depth.front)));
- postscale_matrix (wc, &v);
-}
-
-/*
- * 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.
- * Entree :
- * vp Parametres de visualisation.
- * wc Matrice a initialiser.
- */
-void set_perspective (View_parameters *vp, Matrix wc)
-{
- Matrix m = IDENTITY_MATRIX;
- Point4f vrprim, cw;
- float zmin;
- Vector v;
-
- /*
- * 1 : Translation du centre de projection COP a l'origine.
- */
- SET_COORD3(v,- vp->cop.x, - vp->cop.y, - vp->cop.z);
- Translate_to_Matrix (&v, wc);
- /*
- * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
- * 3 : Rotation pour rendre la projection de VUP sur le plan de
- * projection parallele a l'axe Y.
- */
- set_zy (m, &vp->vpn, &vp->vup);
- postmult_matrix (wc, m);
- /*
- * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
- */
- postleft_matrix (wc, 'z');
- /*
- * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
- */
- point_matrix (&vrprim, &vp->vrp, wc);
- cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
- cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
- cw.z = (float)(vrprim.z);
- ident_matrix (m);
- m[2][0] = - cw.x / cw.z;
- m[2][1] = - cw.y / cw.z;
- postmult_matrix (wc, m);
- /*
- * 6 : Mise a l'echelle de la pyramide.
- */
- SET_COORD3(v,
- (float)((2.0*vrprim.z)/((vp->vwd.umax-vp->vwd.umin)*(vrprim.z+vp->depth.back))),
- (float)((2.0*vrprim.z)/((vp->vwd.vmax-vp->vwd.vmin)*(vrprim.z+vp->depth.back))),
- (float)( 1.0/(vrprim.z+vp->depth.back)));
- postscale_matrix (wc, &v);
- /*
- * 7 : Transformation perspective.
- */
- zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
- ident_matrix (m);
- m[2][2] = (float)(1.0 / (1.0 - zmin)); m[2][3] = 1.0;
- m[3][2] = (float)(- zmin / (1.0 - zmin)); m[3][3] = 0.0;
- postmult_matrix (wc, m);
-}
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpRfstack.cpp b/src/simulator/wireframe-simulator/core/vpRfstack.cpp
deleted file mode 100755
index c784b7d..0000000
--- a/src/simulator/wireframe-simulator/core/vpRfstack.cpp
+++ /dev/null
@@ -1,204 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRfstack.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "rfstack.c" contient les procedures de gestion
- * de la pile d'elimination de faces (Remove Faces STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpView.h>
-#include <visp/vpRfstack.h>
-#include <stdio.h>
-#include <string.h>
-#define STACKSIZE 32
-
-
-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.
- */
-void
-fprintf_rfstack (FILE *fp)
-{
- int flg;
- flg = 0; /* nul si element unique */
-
- if (*sp == IS_INSIDE) {
- fprintf (fp, "(null)\n");
- return;
- }
- fprintf (fp, "(");
- if (*sp & IS_ABOVE) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "above");
- }
- if (*sp & IS_BELOW) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "below");
- }
- if (*sp & IS_RIGHT) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "right");
- }
- if (*sp & IS_LEFT) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "left");
- }
- if (*sp & IS_BACK) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "back");
- }
- if (*sp & IS_FRONT) {
- /*if (flg)*/ fprintf (fp, " ");
- flg ++;
- fprintf (fp, "front");
- }
- fprintf (fp, ")\n");
-}
-
-/*
- * 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.
- */
-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.
- */
-void
-load_rfstack (int i)
-{
- *sp = i;
-}
-
-/*
- * La procedure "pop_rfstack" depile les drapeaux au sommet
- * de la pile des drapeaux d'elimination de faces.
- */
-void
-pop_rfstack (void)
-{
- static char proc_name[] = "pop_rfstack";
-
- if (sp == stack) {
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
-}
-
-/*
- * La procedure "push_rfstack" empile et duplique les drapeaux du sommet
- * de la pile des drapeaux d'elimination de faces.
- */
-void
-push_rfstack (void)
-{
- static char proc_name[] = "push_rfstack";
-
- if (sp == stack + STACKSIZE - 1) {
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- *sp = *(sp - 1);
-}
-
-/*
- * La procedure "swap_rfstack" echange les deux premiers elements
- * de la pile des drapeaux d'elimination de faces.
- */
-void
-swap_rfstack (void)
-{
- int *ip, tmp;
-
- ip = (sp == stack) ? sp + 1 : sp - 1;
- SWAP(*sp, *ip, tmp);
-}
-
-/*
- * La procedure "add_rfstack" ajoute des drapeaux au sommet
- * de la pile des drapeaux d'elimination de faces.
- */
-void
-add_rfstack (int i)
-{
- *sp |= i;
-}
-
-/*
- * La procedure "sub_rfstack" soustrait des drapeaux au sommet
- * de la pile des drapeaux d'elimination de faces.
- */
-void
-sub_rfstack (int i)
-{
- *sp &= ~i;
-}
-
-#endif
-
diff --git a/src/simulator/wireframe-simulator/core/vpRfstack.h b/src/simulator/wireframe-simulator/core/vpRfstack.h
deleted file mode 100755
index 80d6610..0000000
--- a/src/simulator/wireframe-simulator/core/vpRfstack.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRfstack.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "rfstack.h" contient les macros, les types et
- * les specifications des procedures de gestion de la pile
- * des drapeaux d'elimination de faces (Remove Faces STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-#ifndef vpRfstack_h
-#define vpRfstack_h
-
-#include <visp/vpConfig.h>
-#include <stdio.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void fprintf_rfstack (FILE *fp);
-int *get_rfstack (void);
-void load_rfstack (int i);
-void pop_rfstack (void);
-void push_rfstack (void);
-void swap_rfstack (void);
-void add_rfstack (int i);
-void sub_rfstack (int i);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpSkipio.cpp b/src/simulator/wireframe-simulator/core/vpSkipio.cpp
deleted file mode 100755
index 24ee274..0000000
--- a/src/simulator/wireframe-simulator/core/vpSkipio.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSkipio.cpp 5310 2015-02-11 11:57:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "skipio.c" contient les procedures d'analyse
- * syntaxique du fichier "source" qui permettent de traiter
- * les commandes inconnues.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp/vpMy.h>
-#include <visp/vpToken.h>
-#include <visp/vpLex.h>
-#include <visp/vpSkipio.h>
-#include <stdio.h>
-/*
- * La procedure "skip_cmd" saute les structures d'une commande
- * jusqu'a reconnaitre le debut d'une nouvelle commande.
- * Entree :
- * f Fichier en sortie.
- */
-void skip_cmd (void)
-{
- int token;
-
- fprintf (stderr, "\n$ ");
- fwrite (mytext, mylength, 1, stderr);
- while ((token = lexecho (stderr, '$')) != T_EOF && token != '$') {};
- unlex ();
-}
-
-/*
- * 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.
- */
-void skip_keyword (int token, const char *err)
-{
- int t;
-
- switch (t = lex ()) {
- case T_IDENT : /* saute le mot cle inconnu */
- while ((t = lex ()) != 0){
- switch (t) {
- case '$' : /* nouvelle commande */
- case T_EOF : /* fin de fichier */
- lexerr ("start", err, NULL);
- break;
- default :
- if (t == token) return;
- break;
- }
- }
- break;
- default :
- if (t == token) return;
- break;
- }
- lexerr ("start", err, NULL);
-}
-
-#endif
-
diff --git a/src/simulator/wireframe-simulator/core/vpSkipio.h b/src/simulator/wireframe-simulator/core/vpSkipio.h
deleted file mode 100644
index d4d4cc5..0000000
--- a/src/simulator/wireframe-simulator/core/vpSkipio.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSkipio.h 5284 2015-02-09 14:24:10Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "skipio.c" contient les procedures d'analyse
- * syntaxique du fichier "source" qui permettent de traiter
- * les commandes inconnues.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpSkipio_h
-#define vpSkipio_h
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void skip_cmd (void);
-void skip_keyword (int token, const char *err);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpTmstack.cpp b/src/simulator/wireframe-simulator/core/vpTmstack.cpp
deleted file mode 100755
index 61e4461..0000000
--- a/src/simulator/wireframe-simulator/core/vpTmstack.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTmstack.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "tmstack.c" contient les procedures de gestion
- * de la pile de matrices de transformation (Transformation
- * Matrix STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpTmstack.h>
-#include <math.h>
-#include <stdio.h>
-#include <string.h>
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#define STACKSIZE 32
-
-
-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.
- */
-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.
- */
-void
-load_tmstack (Matrix m)
-{
- //bcopy ((char *) m, (char *) *sp, sizeof (Matrix));
- memmove ((char *) *sp, (char *) m, sizeof (Matrix));
-}
-
-/*
- * La procedure "pop_tmstack" depile la matrice au sommet
- * de la pile des matrices de transformation.
- */
-void
-pop_tmstack (void)
-{
- static char proc_name[] = "pop_tmstack";
-
- if (sp == stack) {
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
-}
-
-/*
- * La procedure "push_tmstack" empile et duplique le sommet
- * de la pile des matrices de transformation.
- */
-void
-push_tmstack (void)
-{
- static char proc_name[] = "push_tmstack";
-
- if (sp == stack + STACKSIZE - 1) {
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- //bcopy ((char *) (sp - 1), (char *) sp, sizeof (Matrix));
- memmove ((char *) sp, (char *) (sp - 1), sizeof (Matrix));
-}
-
-/*
- * La procedure "swap_tmstack" echange les deux premieres matrices
- * de la pile des matrices de transformation.
- */
-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));
- memmove ((char *) tmp, (char *) *sp, sizeof (Matrix));
- memmove ((char *) *sp, (char *) *mp, sizeof (Matrix));
- memmove ((char *) *mp, (char *) tmp, sizeof (Matrix));
-}
-
-/*
- * La procedure "postmult_tmstack" postmultiplie la matrice au sommet
- * de la pile des matrices de transformation.
- * Entree :
- * m Matrice multiplicative.
- */
-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.
- */
-void
-postrotate_tmstack (Vector *vp)
-{
- Matrix m;
-
- Rotate_to_Matrix (vp, m);
- postmult3_matrix (*sp, m);
-}
-
-/*
- * La procedure "postscale_tmstack" postmultiplie la matrice au sommet
- * de la pile des matrices de transformation par une homothetie.
- * Entree :
- * vp Vecteur d'homothetie.
- */
-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.
- */
-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.
- */
-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.
- */
-void
-prerotate_tmstack (Vector *vp)
-{
- Matrix m;
-
- Rotate_to_Matrix (vp, m);
- premult3_matrix (*sp, m);
-}
-
-/*
- * La procedure "prescale_tmstack" premultiplie la matrice au sommet
- * de la pile des matrices de transformation par une homothetie.
- * Entree :
- * vp Vecteur d'homothetie.
- */
-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.
- */
-void
-pretranslate_tmstack (Vector *vp)
-{
- pretrans_matrix (*sp, vp);
-}
-
-#endif
-
diff --git a/src/simulator/wireframe-simulator/core/vpTmstack.h b/src/simulator/wireframe-simulator/core/vpTmstack.h
deleted file mode 100755
index af03b72..0000000
--- a/src/simulator/wireframe-simulator/core/vpTmstack.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTmstack.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "tmstack.h" contient les macros, les types et
- * les specifications des procedures de gestion de la pile
- * de matrices de transformation (Transformation Matrix STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpTmstack_h
-#define vpTmstack_h
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-Matrix *get_tmstack (void);
-void load_tmstack (Matrix m);
-void pop_tmstack (void);
-void push_tmstack (void);
-void swap_tmstack (void);
-
-void postmult_tmstack (Matrix m);
-void postrotate_tmstack (Vector *vp);
-void postscale_tmstack (Vector *vp);
-void posttranslate_tmstack (Vector *vp);
-void premult_tmstack (Matrix m);
-void prerotate_tmstack (Vector *vp);
-void prescale_tmstack (Vector *vp);
-void pretranslate_tmstack (Vector *vp);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpToken.cpp b/src/simulator/wireframe-simulator/core/vpToken.cpp
deleted file mode 100755
index 989f942..0000000
--- a/src/simulator/wireframe-simulator/core/vpToken.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpToken.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "token.c" contient la declaration des mots cles.
- * de l'analyseur lexical "lex".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-
-#include <visp/vpMy.h>
-#include <visp/vpToken.h>
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-Keyword keyword_tbl[] = { /* tableau des mots cles termine par NULL*/
- { "above", T_ABOVE },
- { "back", T_BACK },
- { "below", T_BELOW },
- { "bound", T_BOUND },
- { "cop", T_COP },
- { "depth", T_DEPTH },
- { "exit", T_EXIT },
- { "face_list", T_FACE_LIST },
- { "front", T_FRONT },
- { "left", T_LEFT },
- { "none", T_NONE },
- { "parallel", T_PARALLEL },
- { "perspective",T_PERSPECTIVE },
- { "point_list", T_POINT_LIST },
- { "remove", T_REMOVE },
- { "right", T_RIGHT },
- { "type", T_TYPE },
- { "view", T_VIEW },
- { "vpn", T_VPN },
- { "vrp", T_VRP },
- { "vup", T_VUP },
- { "window", T_WINDOW },
- { NULL, NULL }
-};
-
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpToken.h b/src/simulator/wireframe-simulator/core/vpToken.h
deleted file mode 100755
index e7c251d..0000000
--- a/src/simulator/wireframe-simulator/core/vpToken.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpToken.h 5284 2015-02-09 14:24:10Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "token.h" contient les Macros et les Types
- * des jetons de l'analyseur lexicale .
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-#ifndef vpToken_H
-#define vpToken_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-typedef struct {
- const char *ident; /* identifateur */
- Index token; /* code du jeton */
-} Keyword;
-
-#define T_EOF 256
-#define T_FLOAT 257
-#define T_IDENT 258
-#define T_INT 259
-#define T_STRING 260
-
-extern float myfloat;
-extern int myint;
-extern int mylength;
-extern int mylineno;
-extern char *mytext;
-extern Keyword keyword_tbl[];
-
-
-/*
- * Jetons superieurs a 270 (voir "../mylex/token.h").
- */
-#define T_ABOVE 270
-#define T_BACK 271
-#define T_BELOW 272
-#define T_BOUND 273
-#define T_COP 274
-#define T_DEPTH 275
-#define T_EXIT 276
-#define T_FACE_LIST 277
-#define T_FILE 278
-#define T_FRONT 279
-#define T_IMAGE 280
-#define T_LEFT 281
-#define T_NONE 282
-#define T_ORIGIN 283
-#define T_PARALLEL 284
-#define T_PERSPECTIVE 285
-#define T_POINT_LIST 286
-#define T_REMOVE 287
-#define T_RIGHT 288
-#define T_SIZE 289
-#define T_TYPE 290
-#define T_VIEW 291
-#define T_VPN 292
-#define T_VRP 293
-#define T_VUP 294
-#define T_WINDOW 295
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/core/vpView.h b/src/simulator/wireframe-simulator/core/vpView.h
deleted file mode 100755
index 8f95d20..0000000
--- a/src/simulator/wireframe-simulator/core/vpView.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpView.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "view.h" contient les Macros et les types
- * des parametres de visualisation et de transformation 3D.
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpView_H
-#define vpView_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*
- * Macros de numerotation des 6 plans de decoupage :
- * - 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
- */
-#define PLANE_ABOVE 0
-#define PLANE_BELOW 1
-#define PLANE_RIGHT 2
-#define PLANE_LEFT 3
-#define PLANE_BACK 4
-#define PLANE_FRONT 5
-#define PLANE_NBR 6
-
-/*
- * Macros de positionnement des points 4D :
- * Le positionnement d'un point 4D dans l'espace de l'observateur virtuel
- * se fait par rapport aux 6 plans de decoupage.
- * A chaque point 4D on associe 6 bits, un par plan de decoupage.
- */
-#define IS_INSIDE 0x00
-#define IS_ABOVE 0x01
-#define IS_BELOW 0x02
-#define IS_RIGHT 0x04
-#define IS_LEFT 0x08
-#define IS_BACK 0x10
-#define IS_FRONT 0x20
-
-#define vpDEFAULT_REMOVE IS_INSIDE
-
-#define PARALLEL 0
-#define PERSPECTIVE 1
-
-#define vpDEFAULT_EYE { 0.0, 0.0, 1.0 }
-#define vpDEFAULT_TARGET { 0.0, 0.0, 0.0 }
-#define vpDEFAULT_FOCAL 1.0
-#define vpDEFAULT_ANGLE 45.0
-#define vpDEFAULT_TWIST 0.0
-#define vpDEFAULT_SPEED 0.0
-#define vpDEFAULT_CAMERA { vpDEFAULT_EYE, vpDEFAULT_TARGET,\
- vpDEFAULT_FOCAL,vpDEFAULT_ANGLE, vpDEFAULT_TWIST,\
- vpDEFAULT_SPEED }
-
-#define vpDEFAULT_COP { 0.0, 0.0, 1.0 }
-#define vpDEFAULT_VRP { 0.0, 0.0, 0.0 }
-#define vpDEFAULT_VPN { 0.0, 0.0,-1.0 }
-#define vpDEFAULT_VUP { 0.0, 1.0, 0.0 }
-#define vpDEFAULT_VWD {-1.0, 1.0,-1.0, 1.0 }
-#define vpDEFAULT_DEPTH { 0.0, 1.0 }
-#define vpDEFAULT_TYPE PERSPECTIVE
-
-#define vpDEFAULT_VIEW { vpDEFAULT_TYPE,\
- vpDEFAULT_COP, vpDEFAULT_VRP,\
- vpDEFAULT_VPN, vpDEFAULT_VUP,\
- vpDEFAULT_VWD, vpDEFAULT_DEPTH }
-
-#define vpDEFAULT_WC { 1.0, 0.0, 0.0, 0.0,\
- 0.0, 1.0, 0.0, 0.0,\
- 0.0, 0.0, 1.0, 0.0,\
- 0.0, 0.0, 0.0, 1.0 }
-
-
-/*
- * 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).
- */
-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 */
-} Camera_parameters;
-
-typedef struct {
- 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") */
-} 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 */
-} View_parameters;
-
-#endif
-#endif
-
diff --git a/src/simulator/wireframe-simulator/core/vpViewio.cpp b/src/simulator/wireframe-simulator/core/vpViewio.cpp
deleted file mode 100755
index 6c19df0..0000000
--- a/src/simulator/wireframe-simulator/core/vpViewio.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpViewio.cpp 5284 2015-02-09 14:24:10Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "viewio.c" contient les procedures d'entree/sortie
- * des types definis dans le module "view.h".
- * Les entrees non specifiees sont effectuees
- * sur le fichier source de "lex.c".
- * Pour les mots cles des "fprintf_..." voir "token.c".
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <stdio.h>
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpView.h>
-#include <visp/vpToken.h>
-#include <visp/vpLex.h>
-#include <visp/vpSkipio.h>
-
-
-/*
- * La procedure "fscanf_Remove" lit en ascii les parametres d'elimination
- * des faces.
- * Entree :
- * bp Parametres a lire.
- */
-void fscanf_Remove (Byte *bp)
-{
- switch (lex ()) {
- case T_NONE : *bp = IS_INSIDE; break;
- case T_ABOVE : *bp |= IS_ABOVE; break;
- case T_BACK : *bp |= IS_BACK; break;
- case T_BELOW : *bp |= IS_BELOW; break;
- case T_FRONT : *bp |= IS_FRONT; break;
- case T_LEFT : *bp |= IS_LEFT; break;
- case T_RIGHT : *bp |= IS_RIGHT; break;
- default :
- lexerr ("start", "remove: keyword \"none|above|back|below|front|left|right\" expected");
- break;
- }
-}
-
-/*
- * La procedure "fscanf_View_parameters" lit en ascii les parametres
- * de visualisation.
- * Entree :
- * vp Parametres de visualisation a lire.
- */
-void fscanf_View_parameters (View_parameters *vp)
-{
- /* Lecture du type de projection lors de la prise de vue. */
-
- skip_keyword (T_TYPE, "view: keyword \"type\" expected");
- switch (lex ()) {
- case T_PARALLEL :
- vp->type = PARALLEL;
- break;
- case T_PERSPECTIVE :
- vp->type = PERSPECTIVE;
- break;
- default :
- lexerr ("start", "view_type: keyword \"parallel|perspective\" expected");
- break;
- }
-
- /* 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. */
-
- 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. */
-
- 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. */
-
- 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. */
-
- skip_keyword (T_WINDOW, "view: keyword \"window\" expected");
- pusherr ("view_window_umin: ");
- fscanf_float (&vp->vwd.umin);
- popuperr ("view_window_umax: ");
- fscanf_float (&vp->vwd.umax);
- popuperr ("view_window_vmin: ");
- fscanf_float (&vp->vwd.vmin);
- popuperr ("view_window_vmax: ");
- fscanf_float (&vp->vwd.vmax);
- poperr ();
-
- /* Lecture des profondeurs de decoupage avant et arriere. */
-
- skip_keyword (T_DEPTH, "view: keyword \"depth\" expected");
- pusherr ("view_depth_front: ");
- fscanf_float (&vp->depth.front);
- popuperr ("view_depth_back: ");
- fscanf_float (&vp->depth.back);
- poperr ();
-}
-
-#endif
-
-
diff --git a/src/simulator/wireframe-simulator/core/vpVwstack.cpp b/src/simulator/wireframe-simulator/core/vpVwstack.cpp
deleted file mode 100644
index e0dfbc1..0000000
--- a/src/simulator/wireframe-simulator/core/vpVwstack.cpp
+++ /dev/null
@@ -1,281 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpVwstack.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
-* This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "vwstack.c" contient les procedures de gestion
- * de la pile des points de vue (VieW STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <stdio.h>
-#include <string.h>
-#include <limits>
-#include <cmath> // std::fabs()
-
-// #include <varargs.h> /* modif Fedora
-#include <stdarg.h>
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpView.h>
-#include <visp/vpVwstack.h>
-
-
-#define STACKSIZE 4
-
-
-static View_parameters stack[STACKSIZE]= { vpDEFAULT_VIEW };
-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.
- */
-void
-fprintf_vwstack (FILE *fp, char *argv)
-{
- static char proc_name[] = "fprintf_vwstack";
-
- if (argv == NULL || strcmp (argv, "type") == 0) {
- const char *typetoa;
-
- switch (sp->type) {
- case PARALLEL :
- typetoa = "parallel";
- break;
- case PERSPECTIVE :
- typetoa = "perspective";
- break;
- default :
- typetoa = "unknown";
- break;
- }
- fprintf (fp, "(type\t%s)\n", typetoa);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "cop") == 0) {
- fprintf (fp, "(cop\t%.3f\t%.3f\t%.3f)\n",
- sp->cop.x, sp->cop.y, sp->cop.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vrp") == 0) {
- fprintf (fp, "(vrp\t%.3f\t%.3f\t%.3f)\n",
- sp->vrp.x, sp->vrp.y, sp->vrp.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vpn") == 0) {
- fprintf (fp, "(vpn\t%.3f\t%.3f\t%.3f)\n",
- sp->vpn.x, sp->vpn.y, sp->vpn.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vup") == 0) {
- fprintf (fp, "(vup\t%.3f\t%.3f\t%.3f)\n",
- sp->vup.x, sp->vup.y, sp->vup.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "window") == 0) {
- fprintf (fp, "(window\t%.3f\t%.3f\t%.3f\t%.3f)\n",
- sp->vwd.umin,sp->vwd.umax,sp->vwd.vmin,sp->vwd.vmax);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "depth") == 0) {
- fprintf (fp, "(depth\t%.3f\t%.3f)\n",
- sp->depth.front, sp->depth.back);
- if (argv != NULL) return;
- }
- if (argv != NULL)
- fprintf (stderr, "%s: argument unknown\n", proc_name);
-}
-
-/*
- * 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.
- */
-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.
- */
-void
-load_vwstack (View_parameters *vp)
-{
- *sp = *vp;
-}
-
-/*
- * La procedure "pop_vwstack" depile le point de vue au sommet
- * de la pile des points de vue.
- */
-void
-pop_vwstack (void)
-{
- static char proc_name[] = "pop_vwstack";
-
- if (sp == stack) {
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
-}
-
-/*
- * La procedure "push_vwstack" empile et duplique le point de vue au sommet
- * de la pile des points de vue.
- */
-void
-push_vwstack (void)
-{
- static char proc_name[] = "push_vwstack";
-
- if (sp == stack + STACKSIZE - 1) {
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- *sp = *(sp - 1);
-}
-
-/*
- * La procedure "swap_vwstack" echange les deux premiers elements
- * de la pile des points de vue.
- */
-void
-swap_vwstack (void)
-{
- View_parameters *vp, tmp;
-
- vp = (sp == stack) ? sp + 1 : sp - 1;
- SWAP(*sp, *vp, tmp);
-}
-
-/*
- * 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.
- */
-
-void
-add_vwstack (const char* path, ... )
-//add_vwstack (va_alist)
-// va_dcl
-{
- static char proc_name[] = "add_vwstack";
-
- va_list ap;
- char *argv;
-
- va_start (ap,path);
- argv = va_arg (ap, char *);
- if (strcmp (argv, "cop") == 0) {
- /* 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 */
- 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 */
- sp->type = (Type) va_arg (ap, int);
- }
- else if (strcmp (argv, "vpn") == 0) {
- /* 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);
-
- //if (x == 0 && y == 0 && z == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon())
- fprintf (stderr, "%s: bad vpn\n", proc_name);
- else {
- SET_COORD3(sp->vpn,x,y,z);
- }
- }
- else if (strcmp (argv, "vrp") == 0) {
- /* 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 */
- float x = (float) va_arg (ap, double);
- float y = (float) va_arg (ap, double);
- float z = (float) va_arg (ap, double);
-
- //if (x == 0 && y == 0 && z == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon())
- fprintf (stderr, "%s: bad vup\n", proc_name);
- else {
- SET_COORD3(sp->vup,x,y,z);
- }
- }
- else if (strcmp (argv, "window") == 0) {
- /* 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);
- sp->vwd.vmax = (float) va_arg (ap, double);
- }
- else
- fprintf (stderr, "%s: bad argument\n", proc_name);
- va_end (ap);
-}
-
-#endif
-
diff --git a/src/simulator/wireframe-simulator/core/vpVwstack.h b/src/simulator/wireframe-simulator/core/vpVwstack.h
deleted file mode 100755
index 83f2f74..0000000
--- a/src/simulator/wireframe-simulator/core/vpVwstack.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpVwstack.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Le module "vwstack.h" contient les macros, les types et
- * les specifications des procedures de gestion de la pile
- * des points de vue (VieW STACK).
- *
- * Authors:
- * Jean-Luc CORRE
- *
- *****************************************************************************/
-
-#ifndef vpVwstack_H
-#define vpVwstack_H
-
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-void fprintf_vwstack (FILE *fp, char *argv);
-extern View_parameters *get_vwstack (void);
-void load_vwstack (View_parameters *vp);
-void pop_vwstack (void);
-void push_vwstack (void);
-void swap_vwstack (void);
-void add_vwstack (const char* path, ...);
-
-#endif
-#endif
diff --git a/src/simulator/wireframe-simulator/scene/CMakeWireframeScenesList.cmake b/src/simulator/wireframe-simulator/scene/CMakeWireframeScenesList.cmake
deleted file mode 100644
index 5b605fa..0000000
--- a/src/simulator/wireframe-simulator/scene/CMakeWireframeScenesList.cmake
+++ /dev/null
@@ -1,86 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeWireframeScenesList.cmake 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# Default scenes used by the wireframe simulator
-#
-# Authors:
-# Nicolas Melchior
-#
-#############################################################################
-
-SET(WIREFRAME_SCENES
- 3pts.bnd
- 3pts.sld
- camera.bnd
- camera.sld
- circle.bnd
- circle.sld
- circles2.bnd
- circles2.sld
- circle_sq2.bnd
- circle_sq2.sld
- cube.bnd
- cube.sld
- cylinder.bnd
- cylinder.sld
- diamond.bnd
- diamond.sld
- line.bnd
- line.sld
- pipe.bnd
- pipe.sld
- plan.bnd
- plan.sld
- plate.bnd
- plate.sld
- plate_6cm.bnd
- plate_6cm.sld
- plate_8cm.bnd
- plate_8cm.sld
- point.bnd
- point.sld
- point_cloud.bnd
- point_cloud.sld
- rectangle.bnd
- rectangle.sld
- road.bnd
- road.sld
- sphere.bnd
- sphere.sld
- square10cm.bnd
- square10cm.sld
- tool.bnd
- tool.sld
- trapezoid.bnd
- trapezoid.sld
-)
diff --git a/src/simulator/wireframe-simulator/vpWireFrameSimulator.cpp b/src/simulator/wireframe-simulator/vpWireFrameSimulator.cpp
deleted file mode 100644
index e5e38dd..0000000
--- a/src/simulator/wireframe-simulator/vpWireFrameSimulator.cpp
+++ /dev/null
@@ -1,2056 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWireFrameSimulator.cpp 5297 2015-02-10 11:19:24Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Wire frame simulator
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpWireFrameSimulator.cpp
- \brief Implementation of a wire frame simulator.
-*/
-
-#include <visp/vpWireFrameSimulator.h>
-#include <fcntl.h>
-#include <string.h>
-#include <stdio.h>
-#include <vector>
-
-#include <visp/vpSimulatorException.h>
-#include <visp/vpPoint.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPoint.h>
-#include <visp/vpIoTools.h>
-
-//Inventor includes
-#if defined(VISP_HAVE_COIN)
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
-#include <Inventor/actions/SoGetMatrixAction.h>
-#include <Inventor/actions/SoGetPrimitiveCountAction.h>
-#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
-#endif
-
-extern Point2i *point2i;
-extern Point2i *listpoint2i;
-
-typedef enum
-{
- BND_MODEL,
- WRL_MODEL,
- UNKNOWN_MODEL
-} Model_3D;
-
-Model_3D getExtension(const char* file);
-void set_scene_wrl (const char* str, Bound_scene *sc, float factor);
-
-/*
- Get the extension of the file and return it
-*/
-Model_3D
-getExtension(const char* file)
-{
- std::string sfilename(file);
-
- size_t bnd = sfilename.find("bnd");
- size_t BND = sfilename.find("BND");
- size_t wrl = sfilename.find("wrl");
- size_t WRL = sfilename.find("WRL");
-
- size_t size = sfilename.size();
-
- if ((bnd>0 && bnd<size ) || (BND>0 && BND<size))
- return BND_MODEL;
- else if ((wrl>0 && wrl<size) || ( WRL>0 && WRL<size))
- {
-#if defined(VISP_HAVE_COIN)
- return WRL_MODEL;
-#else
- std::cout << "Coin not installed, cannot read VRML files" << std::endl;
- throw std::string("Coin not installed, cannot read VRML files");
-#endif
- }
- return UNKNOWN_MODEL;
-}
-
-/*
- Enable to initialize the scene
-*/
-void set_scene (const char* str, Bound_scene *sc, float factor)
-{
- FILE *fd;
-
- //if ((fd = fopen (str, 0)) == -1)
- if ((fd = fopen (str, "r")) == NULL)
- {
- std::string error = "The file " + std::string(str) + " can not be opened";
-
- throw(vpException(vpSimulatorException::ioError, error.c_str())) ;
- }
- open_keyword (keyword_tbl);
- open_lex ();
- open_source (fd, str);
- malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
- parser (sc);
-
- //if (factor != 1)
- if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
- {
- for (int i = 0; i < sc->bound.nbr; i++)
- {
- for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
- {
- sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
- sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
- sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
- }
- }
- }
-
- close_source ();
- close_lex ();
- close_keyword ();
- fclose(fd);
-}
-
-#if defined(VISP_HAVE_COIN)
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-typedef struct indexFaceSet
-{
- indexFaceSet() : nbPt(0), pt(), nbIndex(0), index() {};
- int nbPt;
- std::vector<vpPoint> pt;
- int nbIndex;
- std::vector<int> index;
-} indexFaceSet;
-#endif // DOXYGEN_SHOULD_SKIP_THIS
-
-void extractFaces(SoVRMLIndexedFaceSet*, indexFaceSet *ifs);
-void ifsToBound (Bound*, std::list<indexFaceSet*> &);
-void destroyIfs(std::list<indexFaceSet*> &);
-
-
-void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
-{
- //Load the sceneGraph
- SoDB::init();
- SoInput in;
- SbBool ok = in.openFile(str);
- SoSeparator *sceneGraph;
- SoVRMLGroup *sceneGraphVRML2;
-
- if (!ok) {
- vpERROR_TRACE("can't open file \"%s\" \n Please check the Marker_Less.ini file", str);
- exit(1);
- }
-
- if(!in.isFileVRML2())
- {
- sceneGraph = SoDB::readAll(&in);
- if (sceneGraph == NULL) { /*return -1;*/ }
- sceneGraph->ref();
-
- SoToVRML2Action tovrml2;
- tovrml2.apply(sceneGraph);
- sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
- sceneGraphVRML2->ref();
- sceneGraph->unref();
- }
- else
- {
- sceneGraphVRML2 = SoDB::readAllVRML(&in);
- if (sceneGraphVRML2 == NULL) {
- /*return -1;*/
- throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
- }
- sceneGraphVRML2->ref();
- }
-
- in.closeFile();
-
- int nbShapes = sceneGraphVRML2->getNumChildren();
-
- SoNode * child;
-
- malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
-
- int iterShapes = 0;
- for (int i = 0; i < nbShapes; i++)
- {
- int nbFaces = 0;
- child = sceneGraphVRML2->getChild(i);
- if (child->getTypeId() == SoVRMLShape::getClassTypeId())
- {
- std::list<indexFaceSet*> ifs_list;
- SoChildList * child2list = child->getChildren();
- for (int j = 0; j < child2list->getLength(); j++)
- {
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
- {
- indexFaceSet *ifs = new indexFaceSet;
- SoVRMLIndexedFaceSet * face_set;
- face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
- extractFaces(face_set,ifs);
- ifs_list.push_back(ifs);
- nbFaces++;
- }
-// if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
-// {
-// std::cout << "> We found a line" << std::endl;
-// SoVRMLIndexedLineSet * line_set;
-// line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
-// extractLines(line_set);
-// }
- }
- sc->bound.nbr++;
- ifsToBound (&(sc->bound.ptr[iterShapes]), ifs_list);
- destroyIfs(ifs_list);
- iterShapes++;
- }
- }
-
- //if (factor != 1)
- if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
- {
- for (int i = 0; i < sc->bound.nbr; i++)
- {
- for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
- {
- sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
- sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
- sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
- }
- }
- }
-}
-
-
-void
-extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
-{
-// vpList<vpPoint> pointList;
-// pointList.kill();
- SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
- int coordSize = coord->point.getNum();
-
- ifs->nbPt = coordSize;
- for (int i = 0; i < coordSize; i++)
- {
- SbVec3f point(0,0,0);
- point[0]=coord->point[i].getValue()[0];
- point[1]=coord->point[i].getValue()[1];
- point[2]=coord->point[i].getValue()[2];
- vpPoint pt;
- pt.setWorldCoordinates(point[0],point[1],point[2]);
- ifs->pt.push_back(pt);
- }
-
- SoMFInt32 indexList = face_set->coordIndex;
- int indexListSize = indexList.getNum();
-
- ifs->nbIndex = indexListSize;
- for (int i = 0; i < indexListSize; i++)
- {
- int index = face_set->coordIndex[i];
- ifs->index.push_back(index);
- }
-}
-
-void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
-{
- int nbPt = 0;
- for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
- nbPt += (*it)->nbPt;
- }
- bptr->point.nbr = (Index)nbPt;
- bptr->point.ptr = (Point3f *) malloc ((unsigned int)nbPt * sizeof (Point3f));
-
- ifs_list.front();
- unsigned int iter = 0;
- for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++)
- {
- bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
- bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
- bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
- iter++;
- }
- }
-
- unsigned int nbFace = 0;
- ifs_list.front();
- std::list<int> indSize;
- int indice = 0;
- for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
- {
- if(ifs->index[j] == -1)
- {
- nbFace++;
- indSize.push_back(indice);
- indice = 0;
- }
- else indice++;
- }
- }
-
- bptr->face.nbr = (Index)nbFace;
- bptr->face.ptr = (Face *) malloc (nbFace * sizeof (Face));
-
-
- std::list<int>::const_iterator iter_indSize = indSize.begin();
- for (unsigned int i = 0; i < indSize.size(); i++)
- {
- bptr->face.ptr[i].vertex.nbr = (Index)*iter_indSize;
- bptr->face.ptr[i].vertex.ptr = (Index *) malloc ((unsigned int)*iter_indSize * sizeof (Index));
- ++iter_indSize;
- }
-
- int offset = 0;
- indice = 0;
- for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
- iter = 0;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
- {
- if(ifs->index[j] != -1)
- {
- bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
- iter++;
- }
- else
- {
- iter = 0;
- indice++;
- }
- }
- offset += ifs->nbPt;
- }
-}
-
-void destroyIfs(std::list<indexFaceSet*> &ifs_list)
-{
- for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
- delete *it;
- }
- ifs_list.clear();
-}
-#else
-void set_scene_wrl (const char* /*str*/, Bound_scene* /*sc*/, float /*factor*/)
-{
-}
-#endif
-
-
-/*
- Convert the matrix format to deal with the one in the simulator
-*/
-void vp2jlc_matrix (const vpHomogeneousMatrix vpM, Matrix &jlcM)
-{
- for (unsigned int i = 0; i < 4; i++) {
- for (unsigned int j = 0; j < 4; j++)
- jlcM[j][i] = (float)vpM[i][j];
- }
-}
-
-/*
- Copy the scene corresponding to the registeresd parameters in the image.
-*/
-void
-vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
-{
- // extern Bound *clipping_Bound ();
- Bound *bp, *bend;
- Bound *clip; /* surface apres clipping */
- Byte b = (Byte) *get_rfstack ();
- Matrix m;
-
- //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
- memmove((char *) m, (char *) mat, sizeof (Matrix));
- View_to_Matrix (get_vwstack (), *(get_tmstack ()));
- postmult_matrix (m, *(get_tmstack ()));
- bp = sc.bound.ptr;
- bend = bp + sc.bound.nbr;
- for (; bp < bend; bp++)
- {
- if ((clip = clipping_Bound (bp, m)) != NULL)
- {
- Face *fp = clip->face.ptr;
- Face *fend = fp + clip->face.nbr;
-
- set_Bound_face_display (clip, b); //regarde si is_visible
-
- point_3D_2D (clip->point.ptr, clip->point.nbr,I.getWidth(),I.getHeight(),point2i);
- for (; fp < fend; fp++)
- {
- if (fp->is_visible)
- {
- wireframe_Face (fp, point2i);
- Point2i *pt = listpoint2i;
- for (int i = 1; i < fp->vertex.nbr; i++)
- {
- vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
- pt++;
- }
- if (fp->vertex.nbr > 2)
- {
- vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
- }
- }
- }
- }
- }
-}
-
-/*
- Copy the scene corresponding to the registeresd parameters in the image.
-*/
-void
-vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color)
-{
- //extern Bound *clipping_Bound ();
-
- Bound *bp, *bend;
- Bound *clip; /* surface apres clipping */
- Byte b = (Byte) *get_rfstack ();
- Matrix m;
-
- //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
- memmove((char *) m, (char *) mat, sizeof (Matrix));
- View_to_Matrix (get_vwstack (), *(get_tmstack ()));
- postmult_matrix (m, *(get_tmstack ()));
- bp = sc.bound.ptr;
- bend = bp + sc.bound.nbr;
- for (; bp < bend; bp++)
- {
- if ((clip = clipping_Bound (bp, m)) != NULL)
- {
- Face *fp = clip->face.ptr;
- Face *fend = fp + clip->face.nbr;
-
- set_Bound_face_display (clip, b); //regarde si is_visible
-
- point_3D_2D (clip->point.ptr, clip->point.nbr,I.getWidth(),I.getHeight(),point2i);
- for (; fp < fend; fp++)
- {
- if (fp->is_visible)
- {
- wireframe_Face (fp, point2i);
- Point2i *pt = listpoint2i;
- for (int i = 1; i < fp->vertex.nbr; i++)
- {
- vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
- pt++;
- }
- if (fp->vertex.nbr > 2)
- {
- vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
- }
- }
- }
- }
- }
-}
-
-/*************************************************************************************************************/
-
-/*!
- Basic constructor.
-
- Set the path to the scene files (*.bnd and *.sln) used by the
- simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
- not valid, the path is set from the VISP_SCENES_DIR environment
- variable that the user has to set.
-*/
-vpWireFrameSimulator::vpWireFrameSimulator()
- : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(),
- refMo(), cMo(), cdMo(), object(PLATE), desiredObject(D_STANDARD),
- camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
- desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true),
- cameraTrajectory(), poseList(), fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(),
- old_iPt(), blockedr(false), blockedz(false), blockedt(false), blocked(false),
- camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
- displayDesiredObject(false), displayCamera(false), displayImageSimulator(false),
- cameraFactor(1.), camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
-{
- // set scene_dir from #define VISP_SCENE_DIR if it exists
- if (vpIoTools::checkDirectory(VISP_SCENES_DIR) == true) // directory exists
- scene_dir = VISP_SCENES_DIR;
- else {
- try {
- scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
- std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
- }
- catch (...) {
- std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
- }
- }
-
- open_display();
- open_clipping();
-
- old_iPr = vpImagePoint(-1,-1);
- old_iPz = vpImagePoint(-1,-1);
- old_iPt = vpImagePoint(-1,-1);
-
- rotz.buildFrom(0,0,0,0,0,vpMath::rad(180));
-
- scene.name = NULL;
- scene.bound.ptr = NULL;
- scene.bound.nbr = 0;
-
- desiredScene.name = NULL;
- desiredScene.bound.ptr = NULL;
- desiredScene.bound.nbr = 0;
-
- camera.name = NULL;
- camera.bound.ptr = NULL;
- camera.bound.nbr = 0;
-}
-
-
-/*!
- Basic destructor
-*/
-vpWireFrameSimulator::~vpWireFrameSimulator()
-{
- if(sceneInitialized)
- {
- if(displayObject)
- free_Bound_scene (&(this->scene));
- if(displayCamera){
- free_Bound_scene (&(this->camera));
- }
- if(displayDesiredObject)
- free_Bound_scene (&(this->desiredScene));
- }
- close_clipping();
- close_display ();
-
- cameraTrajectory.clear();
- poseList.clear();
- fMoList.clear();
-}
-
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
-*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
-{
- char name_cam[FILENAME_MAX];
- char name[FILENAME_MAX];
-
- object = obj;
- this->desiredObject = desired_object;
-
- const char *scene_dir_ = scene_dir.c_str();
- if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
- }
-
- strcpy(name_cam, scene_dir_);
- if (desiredObject != D_TOOL)
- {
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
- }
- else
- {
- strcat(name_cam,"/tool.bnd");
- set_scene(name_cam,&(this->camera),1.0);
- }
-
- strcpy(name, scene_dir_);
- switch (obj)
- {
- case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
- case CUBE : { strcat(name, "/cube.bnd"); break; }
- case PLATE : { strcat(name, "/plate.bnd"); break; }
- case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
- case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
- case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
- case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
- case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
- case THREE_LINES : { strcat(name, "/line.bnd"); break; }
- case ROAD : { strcat(name, "/road.bnd"); break; }
- case TIRE : { strcat(name, "/circles2.bnd"); break; }
- case PIPE : { strcat(name, "/pipe.bnd"); break; }
- case CIRCLE : { strcat(name, "/circle.bnd"); break; }
- case SPHERE : { strcat(name, "/sphere.bnd"); break; }
- case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
- case PLAN: { strcat(name, "/plan.bnd"); break; }
- case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
- }
- set_scene(name,&(this->scene),1.0);
-
- scene_dir_ = scene_dir.c_str();
- if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the desired object name"));
- }
-
- switch (desiredObject)
- {
- case D_STANDARD : { break; }
- case D_CIRCLE : {
- strcpy(name, scene_dir_);
- strcat(name, "/circle_sq2.bnd");
- break; }
- case D_TOOL : {
- strcpy(name, scene_dir_);
- strcat(name, "/tool.bnd");
- break; }
- }
- set_scene(name, &(this->desiredScene), 1.0);
-
- if (obj == PIPE) load_rfstack(IS_INSIDE);
- else add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-
- sceneInitialized = true;
- displayObject = true;
- displayDesiredObject = true;
- displayCamera = true;
- displayImageSimulator = true;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- initScene(vpSceneObject, vpSceneDesiredObject, const std::list<vpImageSimulator> &)
- instead.
-
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(vpSceneObject obj, vpSceneDesiredObject desired_object, vpList<vpImageSimulator> &imObj)
-{
- initScene(obj, desired_object);
- objectImage.clear();
- for(imObj.front(); !imObj.outside(); imObj.next()){
- objectImage.push_back(imObj.value());
- }
- displayImageSimulator = true;
-}
-#endif
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, const std::list<vpImageSimulator> &imObj)
-{
- initScene(obj, desired_object);
- objectImage = imObj;
- displayImageSimulator = true;
-}
-
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- \param obj : Path to the scene file you want to use.
- \param desired_object : Path to the scene file you want to use.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
-{
- char name_cam[FILENAME_MAX];
- char name[FILENAME_MAX];
-
- object = THREE_PTS;
- this->desiredObject = D_STANDARD;
-
- const char *scene_dir_ = scene_dir.c_str();
- if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
- }
-
- strcpy(name_cam, scene_dir_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
- if (strlen(obj) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the name"));
- }
-
- strcpy(name,obj);
- Model_3D model;
- model = getExtension(obj);
- if (model == BND_MODEL)
- set_scene(name,&(this->scene),1.0);
- else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->scene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
- vpERROR_TRACE("Unknown file extension for the 3D model");
- }
-
- if (strlen(desired_object) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
- }
-
- strcpy(name,desired_object);
- model = getExtension(desired_object);
- if (model == BND_MODEL)
- set_scene(name,&(this->desiredScene),1.0);
- else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->desiredScene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
- vpERROR_TRACE("Unknown file extension for the 3D model");
- }
-
- add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-
- sceneInitialized = true;
- displayObject = true;
- displayDesiredObject = true;
- displayCamera = true;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- initScene(const char*, const char*, const std::list<vpImageSimulator> &) instead.
-
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Path to the scene file you want to use.
- \param desired_object : Path to the scene file you want to use.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, vpList<vpImageSimulator> &imObj)
-{
- initScene(obj, desired_object);
- objectImage.clear();
- for(imObj.front(); !imObj.outside(); imObj.next()){
- objectImage.push_back(imObj.value());
- }
- displayImageSimulator = true;
-}
-#endif
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Path to the scene file you want to use.
- \param desired_object : Path to the scene file you want to use.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, const std::list<vpImageSimulator> &imObj)
-{
- initScene(obj, desired_object);
- objectImage = imObj;
- displayImageSimulator = true;
-}
-
-/*!
- Initialize the simulator. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- \param obj : Type of scene used to display the object at the current position.
-*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj)
-{
- char name_cam[FILENAME_MAX];
- char name[FILENAME_MAX];
-
- object = obj;
-
- const char *scene_dir_ = scene_dir.c_str();
- if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
- }
-
- strcpy(name_cam, scene_dir_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
- strcpy(name, scene_dir_);
- switch (obj)
- {
- case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
- case CUBE : { strcat(name, "/cube.bnd"); break; }
- case PLATE : { strcat(name, "/plate.bnd"); break; }
- case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
- case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
- case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
- case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
- case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
- case THREE_LINES : { strcat(name, "/line.bnd"); break; }
- case ROAD : { strcat(name, "/road.bnd"); break; }
- case TIRE : { strcat(name, "/circles2.bnd"); break; }
- case PIPE : { strcat(name, "/pipe.bnd"); break; }
- case CIRCLE : { strcat(name, "/circle.bnd"); break; }
- case SPHERE : { strcat(name, "/sphere.bnd"); break; }
- case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
- case PLAN: { strcat(name, "/plan.bnd"); break; }
- case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
- }
- set_scene(name,&(this->scene),1.0);
-
- if (obj == PIPE) load_rfstack(IS_INSIDE);
- else add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-
- sceneInitialized = true;
- displayObject = true;
- displayCamera = true;
-
- displayDesiredObject = false;
- displayImageSimulator = false;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- initScene(vpSceneObject, const std::list<vpImageSimulator> &) instead.
-
- Initialize the simulator. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Type of scene used to display the object at the current position.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(vpSceneObject obj, vpList<vpImageSimulator> &imObj)
-{
- initScene(obj);
- objectImage.clear();
- for(imObj.front(); !imObj.outside(); imObj.next()){
- objectImage.push_back(imObj.value());
- }
- displayImageSimulator = true;
-}
-#endif
-
-/*!
- Initialize the simulator. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Type of scene used to display the object at the current position.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
-{
- initScene(obj);
- objectImage = imObj;
- displayImageSimulator = true;
-}
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- \param obj : Path to the scene file you want to use.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj)
-{
- char name_cam[FILENAME_MAX];
- char name[FILENAME_MAX];
-
- object = THREE_PTS;
-
- const char *scene_dir_ = scene_dir.c_str();
- if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
- }
-
- strcpy(name_cam, scene_dir_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
- if (strlen(obj) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the name"));
- }
-
- strcpy(name,obj);
- Model_3D model;
- model = getExtension(obj);
- if (model == BND_MODEL)
- set_scene(name,&(this->scene),1.0);
- else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->scene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
- vpERROR_TRACE("Unknown file extension for the 3D model");
- }
-
- add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-
- sceneInitialized = true;
- displayObject = true;
- displayCamera = true;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- initScene(const char*, const std::list<vpImageSimulator> &) instead.
-
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Path to the scene file you want to use.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj, vpList<vpImageSimulator> &imObj)
-{
- initScene(obj);
- objectImage.clear();
- for(imObj.front(); !imObj.outside(); imObj.next()){
- objectImage.push_back(imObj.value());
- }
- displayImageSimulator = true;
-}
-#endif
-
-/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Path to the scene file you want to use.
- \param imObj : A list of vpImageSimulator instances.
-*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulator> &imObj)
-{
- initScene(obj);
- objectImage = imObj;
- displayImageSimulator = true;
-}
-
-/*!
- Get the internal view ie the view of the camera.
-
- \param I : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &I)
-{
- if (!sceneInitialized)
- throw(vpException(vpSimulatorException::notInitializedError,"The scene has to be initialized")) ;
-
- double u;
- double v;
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- float o44c[4][4],o44cd[4][4],x,y,z;
- Matrix id = IDENTITY_MATRIX;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- if (displayImageSimulator)
- {
- I = 255;
-
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
-
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
- x = o44c[2][0] + o44c[3][0];
- y = o44c[2][1] + o44c[3][1];
- z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayObject)
- display_scene(id,this->scene,I, curColor);
-
-
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
- x = o44cd[2][0] + o44cd[3][0];
- y = o44cd[2][1] + o44cd[3][1];
- z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
- else display_scene(id,desiredScene,I, desColor);
- }
-}
-
-
-/*!
- Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
-
- \param I : The image where the external view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-
-void
-vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
-{
- bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I,changed);
-
- //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- camMf2 = camMf2*displacement;
-
- f2Mf = camMf2.inverse()*camMf;
-
- camMf = camMf2* displacement * f2Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
-
- vp2jlc_matrix(camMf.inverse(),w44cext);
- vp2jlc_matrix(fMc,w44c);
- vp2jlc_matrix(fMo,w44o);
-
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if ((object == CUBE) || (object == SPHERE))
- {
- add_vwstack ("start","type", PERSPECTIVE);
- }
-
- if (displayImageSimulator)
- {
- I = 255;
-
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
-
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
-
- if (displayCamera)
- display_scene(w44c,camera, I, camColor);
-
- if (displayCameraTrajectory)
- {
- vpImagePoint iP;
- vpImagePoint iP_1;
- poseList.push_back(cMo);
- fMoList.push_back(fMo);
-
- int iter = 0;
-
- if (changed || extCamChanged)
- {
- cameraTrajectory.clear();
- std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
- std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
-
- while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end()))
- {
- iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
- cameraTrajectory.push_back(iP);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- ++iter_poseList;
- ++iter_fMoList;
- iter++;
- iP_1 = iP;
- }
- extCamChanged = false;
- }
- else
- {
- iP = projectCameraTrajectory(I, cMo, fMo);
- cameraTrajectory.push_back(iP);
-
- for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I, *it, camTrajColor);
- iter++;
- iP_1 = *it;
- }
- }
-
- if (poseList.size() > nbrPtLimit)
- {
- poseList.pop_front();
- }
- if (fMoList.size() > nbrPtLimit)
- {
- fMoList.pop_front();
- }
- if (cameraTrajectory.size() > nbrPtLimit)
- {
- cameraTrajectory.pop_front();
- }
- }
-}
-
-
-/*!
- Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
-
- \param I : The image where the external view is displayed.
- \param cam_Mf : The pose between the point of view and the fixed world frame.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
-{
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
-
- vpHomogeneousMatrix camMft = rotz * cam_Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- vp2jlc_matrix(camMft.inverse(),w44cext);
- vp2jlc_matrix(fMo*cMo.inverse(),w44c);
- vp2jlc_matrix(fMo,w44o);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- if (displayImageSimulator)
- {
- I = 255;
-
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cam_Mf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
-
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
- if (displayCamera)
- display_scene(w44c,camera, I, camColor);
-}
-
-
-/*!
- Get the internal view ie the view of the camera.
-
- \param I : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &I)
-{
- if (!sceneInitialized)
- throw(vpException(vpSimulatorException::notInitializedError,"The scene has to be initialized")) ;
-
- double u;
- double v;
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- float o44c[4][4],o44cd[4][4],x,y,z;
- Matrix id = IDENTITY_MATRIX;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- if (displayImageSimulator)
- {
- I = 255;
-
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
-
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
- x = o44c[2][0] + o44c[3][0];
- y = o44c[2][1] + o44c[3][1];
- z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayObject)
- display_scene(id,this->scene,I, curColor);
-
-
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
- x = o44cd[2][0] + o44cd[3][0];
- y = o44cd[2][1] + o44cd[3][1];
- z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
- else display_scene(id,desiredScene,I, desColor);
- }
-}
-
-
-/*!
- Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
-
- \param I : The image where the external view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-
-void
-vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
-{
- bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I,changed);
-
- //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- camMf2 = camMf2*displacement;
-
- f2Mf = camMf2.inverse()*camMf;
-
- camMf = camMf2* displacement * f2Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
-
- vp2jlc_matrix(camMf.inverse(),w44cext);
- vp2jlc_matrix(fMc,w44c);
- vp2jlc_matrix(fMo,w44o);
-
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if ((object == CUBE) || (object == SPHERE))
- {
- add_vwstack ("start","type", PERSPECTIVE);
- }
-
- if (displayImageSimulator)
- {
- I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
-
- if (displayCamera)
- display_scene(w44c,camera, I, camColor);
-
- if (displayCameraTrajectory)
- {
- vpImagePoint iP;
- vpImagePoint iP_1;
- poseList.push_back(cMo);
- fMoList.push_back(fMo);
-
- int iter = 0;
-
- if (changed || extCamChanged)
- {
- cameraTrajectory.clear();
- std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
- std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
-
- while ((iter_poseList!=poseList.end()) && (iter_fMoList!=fMoList.end()) )
- {
- iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
- cameraTrajectory.push_back(iP);
- //vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- ++iter_poseList;
- ++iter_fMoList;
- iter++;
- iP_1 = iP;
- }
- extCamChanged = false;
- }
- else
- {
- iP = projectCameraTrajectory(I, cMo,fMo);
- cameraTrajectory.push_back(iP);
-
- for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
- if (camTrajType == CT_LINE){
- if (iter != 0) vpDisplay::displayLine(I,iP_1,*it,camTrajColor, thickness_);
- }
- else if(camTrajType == CT_POINT)
- vpDisplay::displayPoint(I, *it, camTrajColor);
- iter++;
- iP_1 = *it;
- }
- }
-
- if (poseList.size() > nbrPtLimit)
- {
- poseList.pop_front();
- }
- if (fMoList.size() > nbrPtLimit)
- {
- fMoList.pop_front();
- }
- if (cameraTrajectory.size() > nbrPtLimit)
- {
- cameraTrajectory.pop_front();
- }
- }
-}
-
-
-/*!
- Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
-
- \param I : The image where the external view is displayed.
- \param cam_Mf : The pose between the point of view and the fixed world frame.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
-*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
-{
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
-
- vpHomogeneousMatrix camMft = rotz * cam_Mf;
-
- double u;
- double v;
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- }
-
- vp2jlc_matrix(camMft.inverse(),w44cext);
- vp2jlc_matrix(fMo*cMo.inverse(),w44c);
- vp2jlc_matrix(fMo,w44o);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
- x = w44cext[2][0] + w44cext[3][0];
- y = w44cext[2][1] + w44cext[3][1];
- z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- if (displayImageSimulator)
- {
- I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cam_Mf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
- }
- if (I.display != NULL)
- vpDisplay::display(I);
- }
-
- if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
- if (displayCamera)
- display_scene(w44c,camera, I, camColor);
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated. You should use
- displayTrajectory(vpImage<unsigned char> &, const std::list<vpHomogeneousMatrix> &, const std::list<vpHomogeneousMatrix> &, const vpHomogeneousMatrix &);
- instead.
-
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
-
- The two lists must have the same size of homogeneous matrices must have the same size.
-
- \param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
-*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix cMf)
-{
- if (list_cMo.nbElements() != list_fMo.nbElements())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
-
- list_cMo.front();
- list_fMo.front();
- vpImagePoint iP;
- vpImagePoint iP_1;
- int iter = 0;
-
- while (!list_cMo.outside() && !list_fMo.outside())
- {
- iP = projectCameraTrajectory(I, rotz * list_cMo.value(), list_fMo.value(), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- list_cMo.next();
- list_fMo.next();
- iter++;
- iP_1 = iP;
- }
-}
-
-/*!
- \deprecated This method is deprecated. You should use
- displayTrajectory (vpImage<vpRGBa> &, const std::list<vpHomogeneousMatrix> &, const std::list<vpHomogeneousMatrix> &, const vpHomogeneousMatrix &);
- instead.
-
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
-
- The two lists must have the same size of homogeneous matrices must have the same size.
-
- \param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
-*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix cMf)
-{
- if (list_cMo.nbElements() != list_fMo.nbElements())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
-
- list_cMo.front();
- list_fMo.front();
- vpImagePoint iP;
- vpImagePoint iP_1;
- int iter = 0;
-
- while (!list_cMo.outside() && !list_fMo.outside())
- {
- iP = projectCameraTrajectory(I, rotz * list_cMo.value(), list_fMo.value(), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- list_cMo.next();
- list_fMo.next();
- iter++;
- iP_1 = iP;
- }
-}
-#endif
-
-
-/*!
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
-
- The two lists must have the same size of homogeneous matrices must have the same size.
-
- \param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
-*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
- const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
-{
- if (list_cMo.size() != list_fMo.size())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
-
- vpImagePoint iP;
- vpImagePoint iP_1;
- int iter = 0;
-
- std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
- std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
-
- while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
- {
- iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- ++it_cMo;
- ++it_fMo;
- iter++;
- iP_1 = iP;
- }
-}
-
-/*!
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
-
- The two lists must have the same size of homogeneous matrices must have the same size.
-
- \param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
-*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
- const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
-{
- if (list_cMo.size() != list_fMo.size())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
-
- vpImagePoint iP;
- vpImagePoint iP_1;
- int iter = 0;
-
- std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
- std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
-
- while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
- {
- iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
- ++it_cMo;
- ++it_fMo;
- iter++;
- iP_1 = iP;
- }
-}
-
-
-/*!
- Enables to change the external camera position.
-*/
-vpHomogeneousMatrix
-vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
-{
- double width = vpMath::minimum(I.getWidth(),I.getHeight());
- vpImagePoint iP;
- vpImagePoint trash;
- bool clicked = false;
- bool clickedUp = false;
- vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
-
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
- changed = false;
-
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
-
- if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
-
- if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
-
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
- if (b == vpMouseButton::button3) blockedt = true;
- blocked = true;
- }
- if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
- blockedr = false;
- }
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
- blockedz = false;
- }
- if (b == vpMouseButton::button3)
- {
- old_iPt = vpImagePoint(-1,-1);
- blockedt = false;
- }
- if (!(blockedr || blockedz || blockedt))
- {
- blocked = false;
- while (vpDisplay::getClick(I,trash,b,false)) {};
- }
- }
-
- vpDisplay::getPointerPosition(I,iP);
-
- double anglei = 0;
- double anglej = 0;
-
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
- double diffi = iP.get_i() - old_iPr.get_i();
- double diffj = iP.get_j() - old_iPr.get_j();
- //cout << "delta :" << diffj << endl;;
- anglei = diffi*360/width;
- anglej = diffj*360/width;
- mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
- changed = true;
- }
-
- if (blockedr) old_iPr = iP;
-
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
- double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
- changed = true;
- }
-
- if (blockedz) old_iPz = iP;
-
- if (old_iPt != vpImagePoint(-1,-1) && blockedt)
- {
- double diffi = iP.get_i() - old_iPt.get_i();
- double diffj = iP.get_j() - old_iPt.get_j();
- mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
- changed = true;
- }
-
- if (blockedt) old_iPt = iP;
-
- return mov;
-}
-
-
-/*!
- Enables to change the external camera position.
-*/
-vpHomogeneousMatrix
-vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
-{
- double width = vpMath::minimum(I.getWidth(),I.getHeight());
- vpImagePoint iP;
- vpImagePoint trash;
- bool clicked = false;
- bool clickedUp = false;
- vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
-
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
- changed = false;
-
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
-
- if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
-
- if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
-
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
- if (b == vpMouseButton::button3) blockedt = true;
- blocked = true;
- }
- if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
- blockedr = false;
- }
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
- blockedz = false;
- }
- if (b == vpMouseButton::button3)
- {
- old_iPt = vpImagePoint(-1,-1);
- blockedt = false;
- }
- if (!(blockedr || blockedz || blockedt))
- {
- blocked = false;
- while (vpDisplay::getClick(I,trash,b,false)) {};
- }
- }
-
- vpDisplay::getPointerPosition(I,iP);
-
- //std::cout << "point : " << iP << std::endl;
-
- double anglei = 0;
- double anglej = 0;
-
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
- double diffi = iP.get_i() - old_iPr.get_i();
- double diffj = iP.get_j() - old_iPr.get_j();
- //cout << "delta :" << diffj << endl;;
- anglei = diffi*360/width;
- anglej = diffj*360/width;
- mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
- changed = true;
- }
-
- if (blockedr) old_iPr = iP;
-
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
- double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
- changed = true;
- }
-
- if (blockedz) old_iPz = iP;
-
- if (old_iPt != vpImagePoint(-1,-1) && blockedt)
- {
- double diffi = iP.get_i() - old_iPt.get_i();
- double diffj = iP.get_j() - old_iPt.get_j();
- mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
- changed = true;
- }
-
- if (blockedt) old_iPt = iP;
-
- return mov;
-}
-
-/*!
- Project the center of the internal camera into the external camera view.
-*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_)
-{
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
-
- point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
-
- vpImagePoint iP;
-
- vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
-
- return iP;
-}
-
-/*!
- Project the center of the internal camera into the external camera view.
-*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_)
-{
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
-
- point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
-
- vpImagePoint iP;
-
- vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
-
- return iP;
-}
-
-/*!
- Project the center of the internal camera into the external camera view.
-*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
-{
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
-
- point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
-
- vpImagePoint iP;
-
- vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
-
- return iP;
-}
-
-/*!
- Project the center of the internal camera into the external camera view.
-*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
-{
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
-
- point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
-
- vpImagePoint iP;
-
- vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
-
- return iP;
-}
-
diff --git a/src/simulator/wireframe-simulator/vpWireFrameSimulator.h b/src/simulator/wireframe-simulator/vpWireFrameSimulator.h
deleted file mode 100644
index 4c30224..0000000
--- a/src/simulator/wireframe-simulator/vpWireFrameSimulator.h
+++ /dev/null
@@ -1,621 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpWireFrameSimulator.h 5297 2015-02-10 11:19:24Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Wire frame simulator
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#ifndef vpWireFrameSimulator_HH
-#define vpWireFrameSimulator_HH
-
-/*!
- \file vpWireFrameSimulator.h
- \brief Implementation of a wire frame simulator.
-*/
-#include <visp/vpConfig.h>
-#include <stdio.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <list>
-#include <string>
-
-#include <visp/vpMy.h>
-#include <visp/vpArit.h>
-#include <visp/vpBound.h>
-#include <visp/vpView.h>
-#include <visp/vpToken.h>
-#include <visp/vpTmstack.h>
-#include <visp/vpVwstack.h>
-#include <visp/vpRfstack.h>
-#include <visp/vpArit.h>
-
-void open_display();
-void close_display();
-void open_clipping();
-void close_clipping();
-void open_keyword (Keyword *kwp);
-void open_lex (void);
-void open_source (FILE *fd, const char *str);
-void malloc_Bound_scene (Bound_scene *bsp, const char *name,Index bn);
-void free_Bound_scene (Bound_scene *bsp);
-void parser (Bound_scene *bsp);
-void close_source (void);
-void close_lex (void);
-void close_keyword (void);
-void display_scene(Matrix mat, Bound_scene sc);
-void View_to_Matrix (View_parameters *vp, Matrix m);
-Bound *clipping_Bound (Bound *bp, Matrix m);
-void set_Bound_face_display (Bound *bp, Byte b);
-void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2);
-void wireframe_Face (Face *fp, Point2i *pp);
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDisplay.h>
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-#include <visp/vpImagePoint.h>
-#include <visp/vpImageSimulator.h>
-
-void set_scene (const char*, Bound_scene *, float);
-void vp2jlc_matrix (const vpHomogeneousMatrix, Matrix&);
-
-
-/*!
- \class vpWireFrameSimulator
-
- \ingroup RobotSimuWithViz
-
- \brief Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not
- require third party libraries to be used.
-
- The simulator uses several frames to display the scene in the image. There are:
-
- - The world frame : This is a fixed frame used to compute the position of the other frames against each other. By default this frame is positionned at the same location as the initial object frame position.
-
- - The object frame : It is the frame linked to the object.
-
- - The camera frame : It is the frame linked to the main camera.
-
- - The external camera frame : It is the frame which corresponds to one external point of view.
-
- The most used homogeneous matrices which describes the displacement between two frames are :
-
- - fMo which is the displacement between the world frame and the object frame.
-
- - cMo which is the displacement between the main camera frame and the object frame.
-
- - cextMf which is the displacement between one external camera frame and the world frame.
-
- The following picture summarize all the previous information.
-
- \image html vpWireFrameSimulator.jpeg
- \image latex vpWireFrameSimulator.ps
-
- The simulator uses .bnd or .wrl files as 3D scene descriptors. Several scenes can be found in the data folder which is in the directory where you build ViSP.
-
- You can move the main external view while clicking in the image. The left click enables to turn, the middle button enables to zoom and the left to translate along x and y.
-
- The simulator is able to take into account to camera parameters. You can set the internal and external cameras parameters thanks to a vpCameraParameters.
-
- The following example shows how it is easy to use.
-
- \code
-
- #include <visp/vpImage.h>
- #include <visp/vpWireFrameSimulator.h>
-
- int main()
- {
- vpWireFrameSimulator sim;
-
- vpImage<vpRGBa> Iint(480,640,255);
- vpImage<vpRGBa> Iext(480,640,255);
-
- //Set the type of scene to use
- sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
-
- //Set the initial pose of the camera
- sim.setCameraPositionRelObj(vpHomogeneousMatrix(0,0,0.5,vpMath::rad(0),vpMath::rad(10),0));
- //Set the desired pose of the camera (for the internal view)
- sim.setDesiredCameraPosition(vpHomogeneousMatrix(0.0,0,0.5,0,0,0));
- //Set the pose of the reference frame (for the external view)
- sim.setExternalCameraPosition(vpHomogeneousMatrix(0.1,0,0.2,0,0,0));
-
- //Set the camera parameters
- vpCameraParameters camera(1000,1000,320,240);
- sim.setInternalCameraParameters(camera);
- sim.setExternalCameraParameters(camera);
-
- //Get the internal view
- sim.getInternalImage(Iint);
-
- //Get the external view
- sim.getExternalImage(Iext);
-
- return 0;
- }
- \endcode
-*/
-
-class VISP_EXPORT vpWireFrameSimulator
-{
-public:
-
- /*!
- Type of scene used to display the object at the current position.
- */
- typedef enum
- {
- THREE_PTS, //!< A 40cm by 40cm plate with 3 points at coordinates (0,0,0), (0.1,0,0), (0,0.1,0). Each point is represented by a circle with 2cm radius.
- CUBE, //!< A 12.5cm size cube.
- PLATE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
- SMALL_PLATE, //!< 4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0), (0.03,0.03,0), (0.03,0.03,0). Each point is represented by a circle with 1cm radius.
- RECTANGLE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0.07,-0.05,0), (-0.07,-0.05,0). Each point is represented by a circle with 2cm radius.
- SQUARE_10CM, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0), (-0.05,-0.05,0). Each point is represented by a circle with 2cm radius.
- DIAMOND, //!< A 40cm by 40cm plate with 4 points at coordinates (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is represented by a circle with 2cm radius.
- TRAPEZOID, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0), (0.025,-0.05,0). Each point is represented by a circle with 2cm radius.
- THREE_LINES, //!< Three parallel lines with equation y=-5, y=0, y=5.
- ROAD, //!< Three parallel lines representing a road.
- TIRE, //!< A tire represented by 2 circles with radius 10cm and 15cm.
- PIPE, //!< A pipe represented by a cylinder of 25 cm length and 15cm radius.
- CIRCLE, //!< A 10cm radius circle.
- SPHERE, //!< A 15cm radius sphere.
- CYLINDER, //!< A cylinder of 80cm length and 10cm radius.
- PLAN, //!< A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.
- POINT_CLOUD, //!< A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
- } vpSceneObject;
-
- /*!
- Type of scene used to display the object at the desired pose (in the internal view).
-
- - D_STANDARD will use the vpSceneObject used to be the object at the current position.
- - D_OUTIL will display a tool which is attached to the camera.
- */
- typedef enum
- {
- D_STANDARD, //!< The object displayed at the desired position is the same than the scene object defined in vpSceneObject.
- D_CIRCLE, //!< The object displayed at the desired position is a circle.
- D_TOOL //!< A cylindrical tool is attached to the camera.
- } vpSceneDesiredObject;
-
- typedef enum
- {
- CT_LINE,
- CT_POINT
- } vpCameraTrajectoryDisplayType;
-
-protected:
- Bound_scene scene;
- Bound_scene desiredScene;
- Bound_scene camera;
- std::list<vpImageSimulator> objectImage;
-
- vpHomogeneousMatrix fMo;
- vpHomogeneousMatrix fMc;
- vpHomogeneousMatrix camMf;
- vpHomogeneousMatrix refMo;
- vpHomogeneousMatrix cMo;
- vpHomogeneousMatrix cdMo;
-
- vpSceneObject object;
- vpSceneDesiredObject desiredObject;
-
- vpColor camColor;
- vpColor camTrajColor;
- vpColor curColor;
- vpColor desColor;
-
- bool sceneInitialized;
-
- bool displayCameraTrajectory;
- std::list<vpImagePoint> cameraTrajectory;
- std::list<vpHomogeneousMatrix> poseList;
- std::list<vpHomogeneousMatrix> fMoList;
- unsigned int nbrPtLimit;
-
- vpImagePoint old_iPr;
- vpImagePoint old_iPz;
- vpImagePoint old_iPt;
- bool blockedr;
- bool blockedz;
- bool blockedt;
- bool blocked;
-
- vpHomogeneousMatrix camMf2;
- vpHomogeneousMatrix f2Mf;
-
- double px_int;
- double py_int;
- double px_ext;
- double py_ext;
-
- bool displayObject;
- bool displayDesiredObject;
- bool displayCamera;
- bool displayImageSimulator;
-
- float cameraFactor;
-
- vpCameraTrajectoryDisplayType camTrajType;
-
- bool extCamChanged;
-
- vpHomogeneousMatrix rotz;
-
- unsigned int thickness_;
-
-private:
- std::string scene_dir;
-
-public:
- vpWireFrameSimulator();
- virtual ~vpWireFrameSimulator();
-
- /*!
- Delete the history of the main camera position which are displayed in the external views.
- */
- inline void deleteCameraPositionHistory() {
- cameraTrajectory.clear();
- poseList.clear();
- fMoList.clear();}
-
- void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
- void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
-
- /*!
- Get the parameters of the virtual external camera.
-
- \param I : The image used to display the view of the camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const {
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
- /*!
- Get the parameters of the virtual external camera.
-
- \param I : The image used to display the view of the camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const {
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
- /*!
- Get the main external camera's position relative to the the world reference frame.
-
- \return the main external camera position relative to the the world reference frame.
- */
- inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf;}
-
- void getExternalImage(vpImage<unsigned char> &I);
- void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
- void getExternalImage(vpImage<vpRGBa> &I);
- void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
-
- /*!
- Get the parameters of the virtual internal camera.
-
- \param I : The image used to display the view of the camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const {
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
- /*!
- Get the parameters of the virtual internal camera.
-
- \param I : The image used to display the view of the camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const {
- //if(px_int != 1 && py_int != 1)
- // we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
-
- void getInternalImage(vpImage<unsigned char> &I);
- void getInternalImage(vpImage<vpRGBa> &I);
-
- /*!
- Get the pose between the object and the camera.
-
- \return The pose between between the object and the camera.
- */
- vpHomogeneousMatrix get_cMo() const {return rotz*cMo;}
-
- /*!
- Get the homogeneous matrices cMo stored to display the camera trajectory.
-
- \param cMo_history : The list of the homogeneous matrices cMo.
- */
- void get_cMo_History(std::list<vpHomogeneousMatrix>& cMo_history) {
- cMo_history.clear();
- for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
- cMo_history.push_back(rotz*(*it));
- }
- }
-
- /*!
- Get the pose between the object and the fixed world frame.
-
- \return The pose between the object and the fixed world frame.
- */
- vpHomogeneousMatrix get_fMo() const {return fMo;}
-
- /*!
- Get the homogeneous matrices fMo stored to display the camera trajectory.
-
- \param fMo_history : The list of the homogeneous matrices fMo.
- */
- void get_fMo_History(std::list<vpHomogeneousMatrix>& fMo_history) {fMo_history = fMoList;}
-
- void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
- void initScene(const char* obj, const char* desiredObject);
- void initScene(const vpSceneObject &obj);
- void initScene(const char* obj);
-
- void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list<vpImageSimulator> &imObj);
- void initScene(const char* obj, const char* desiredObject, const std::list<vpImageSimulator> &imObj);
- void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
- void initScene(const char* obj, const std::list<vpImageSimulator> &imObj);
-
- /*!
- Set the color used to display the camera in the external view.
-
- \param col : The desired color.
- */
- void setCameraColor(const vpColor &col) {camColor = col;}
- /*!
- Set the position of the camera relative to the object.
-
- \param cMo_ : The pose of the camera.
- */
- void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_) {this->cMo = rotz * cMo_; fMc = fMo*this->cMo.inverse();}
-
- /*!
- Set the position of the the world reference frame relative to the camera.
-
- \param fMc_ : The pose of the camera.
- */
- void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_) {this->fMc = fMc_*rotz; cMo = this->fMc.inverse()*fMo;}
-
- /*!
- Set the parameter which enables to choose the size of the main camera in the external camera views. By default this parameter is set to 1.
-
- \param factor : The ration for the camera size.
- */
- inline void setCameraSizeFactor (const float factor) {cameraFactor = factor;}
-
- /*!
- Set the color used to display the camera trajectory in the external view.
-
- \param col : The desired color.
- */
- void setCameraTrajectoryColor(const vpColor &col) {camTrajColor = col;}
-
- /*!
- Set the way to display the history of the main camera trajectory in the main external view. The choice is given between displaying lines and points.
-
- \param camTraj_type : The chosen way to display the camera trajectory.
- */
- inline void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType &camTraj_type) {this->camTrajType = camTraj_type;}
-
- /*!
- Set the color used to display the object at the current position.
-
- \param col : The desired color.
- */
- void setCurrentViewColor(const vpColor &col) {curColor = col;}
- /*!
- Set the desired position of the camera relative to the object.
-
- \param cdMo_ : The desired pose of the camera.
- */
- void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) {this->cdMo = rotz * cdMo_;}
- /*!
- Set the color used to display the object at the desired position.
-
- \param col : The desired color.
- */
- void setDesiredViewColor(const vpColor &col) {desColor = col;}
- /*!
- Enable or disable the displaying of the camera trajectory in the main external camera view.
-
- By default the trajectory is displayed.
-
- \param do_display : Set to true to display the camera trajectory.
- */
- void setDisplayCameraTrajectory (const bool &do_display) {this->displayCameraTrajectory = do_display;}
-
- /*!
- Set the internal camera parameters.
-
- \param cam : The desired camera parameters.
- */
- inline void setExternalCameraParameters(const vpCameraParameters &cam) {
- px_ext = cam.get_px();
- py_ext = cam.get_py();
- }
- /*!
- Set the external camera point of view.
-
- \param cam_Mf : The pose of the external camera relative to the world reference frame.
- */
- void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
- {
- this->camMf = rotz * cam_Mf;
- vpTranslationVector T;
- this->camMf.extract (T);
- this->camMf2.buildFrom(0,0,T[2],0,0,0);
- f2Mf = camMf2.inverse()*this->camMf;
- extCamChanged = true;
- }
-
- /*!
- Specify the thickness of the graphics drawings.
- */
- void setGraphicsThickness(unsigned int thickness)
- {
- this->thickness_ = thickness;
- }
-
- /*!
- Set the internal camera parameters.
-
- \param cam : The desired camera parameters.
- */
- inline void setInternalCameraParameters(const vpCameraParameters &cam) {
- px_int = cam.get_px();
- py_int = cam.get_py();
- }
-
- /*!
- Set the maximum number of main camera's positions which are stored. Those position can be displayed in the external camera field of view. By default this parameter is set to 1000.
-
- \param nbPt : The desired number of position which are saved.
- */
- inline void setNbPtTrajectory(const unsigned int nbPt) {nbrPtLimit = nbPt;}
-
- /*!
- Set the pose between the object and the fixed world frame.
-
- \param fMo_ : The pose between the object and the fixed world frame.
- */
- void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;/*this->cMo = fMc.inverse()*fMo;*/}
-
-protected:
- void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
- void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
- vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
- vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
- vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
- vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
- vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
- vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
-
-public:
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
-
- vp_deprecated void displayTrajectory(const vpImage<unsigned char> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix camMf);
- vp_deprecated void displayTrajectory(const vpImage<vpRGBa> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix camMf);
-
- /*!
- \deprecated This method is deprecated. You should use
- get_cMo_History(std::list<vpHomogeneousMatrix> &) instead. \n \n
- Get the homogeneous matrices cMo stored to display the camera trajectory.
-
- \return Returns the list of the homogeneous matrices cMo.
- */
- vp_deprecated vpList<vpHomogeneousMatrix> get_cMo_History () {
- vpList<vpHomogeneousMatrix> list_cMo;
- vpHomogeneousMatrix tmp_;
- for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
- tmp_ = (rotz*(*it));
- list_cMo.addRight(tmp_);
- }
- return list_cMo;}
- /*!
- \deprecated This method is deprecated. You should use
- get_fMo_History(std::list<vpHomogeneousMatrix> &) instead. \n \n
- Get the homogeneous matrices fMo stored to display the camera trajectory.
-
- \return Returns the list of the homogeneous matrices fMo.
- */
- vp_deprecated vpList<vpHomogeneousMatrix> get_fMo_History () {
- vpList<vpHomogeneousMatrix> fMoHistory;
- for(std::list<vpHomogeneousMatrix>::const_iterator iter = fMoList.begin(); iter != fMoList.end(); ++iter){
- fMoHistory.addRight(*iter);
- }
- return fMoHistory;}
-
-
- vp_deprecated void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject, vpList<vpImageSimulator> &imObj);
- vp_deprecated void initScene(const char* obj, const char* desiredObject, vpList<vpImageSimulator> &imObj);
- vp_deprecated void initScene(vpSceneObject obj, vpList<vpImageSimulator> &imObj);
- vp_deprecated void initScene(const char* obj, vpList<vpImageSimulator> &imObj);
-
-#endif
-};
-
-#endif
diff --git a/src/tools/convert/vpConvert.cpp b/src/tools/convert/vpConvert.cpp
deleted file mode 100644
index a5294c5..0000000
--- a/src/tools/convert/vpConvert.cpp
+++ /dev/null
@@ -1,435 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpIoTools.cpp 5214 2015-01-27 18:33:01Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Directory management.
- *
- * Authors:
- * Fabien Spindler
- * Souriya Trinh
- *
- *****************************************************************************/
-
-/*!
- \file vpConvert.cpp
- \brief Tools for type or general conversion.
-*/
-
-#include <vector> // std::vector
-#include <algorithm> // std::transform
-
-#include <visp/vpConvert.h>
-
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- /**!
- Unary function used to transform a cv::KeyPoint to a vpImagePoint.
- \param keypoint : KeyPoint to convert.
-
- \return A vpImagePoint with the 2D coordinates corresponding to the location of the KeyPoint.
- */
- vpImagePoint vpConvert::keyPointToVpImagePoint(const cv::KeyPoint &keypoint) {
- return vpImagePoint(keypoint.pt.y, keypoint.pt.x);
- }
-
- /*!
- Unary function to convert a cv::Point2f to a vpImagePoint.
- \param point : Point to convert.
-
- \return A vpImagePoint with the 2D coordinates stored in cv::Point2f.
- */
- vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) {
- return vpImagePoint(point.y, point.x);
- }
-
- /*!
- Unary function to convert a cv::Point2d to a vpImagePoint.
- \param point : Point to convert.
-
- \return A vpImagePoint with the 2D coordinates stored in cv::Point2d.
- */
- vpImagePoint vpConvert::point2dToVpImagePoint(const cv::Point2d &point) {
- return vpImagePoint(point.y, point.x);
- }
-
- /*!
- Unary function to convert a cv::Point3f to a vpPoint (object frame).
- \param point3f : Point to convert.
- \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3f.
- */
- vpPoint vpConvert::point3fToVpObjectPoint(const cv::Point3f &point3f) {
- vpPoint pt;
- pt.set_oX(point3f.x);
- pt.set_oY(point3f.y);
- pt.set_oZ(point3f.z);
- pt.set_oW(1.0);
- return pt;
- }
-
- /*!
- Unary function to convert a cv::Point3f to a vpPoint (camera frame).
- \param point3f : Point to convert.
- \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3f.
- */
- vpPoint vpConvert::point3fToVpCamPoint(const cv::Point3f &point3f) {
- vpPoint pt;
- pt.set_X(point3f.x);
- pt.set_Y(point3f.y);
- pt.set_Z(point3f.z);
- pt.set_W(1.0);
- return pt;
- }
-
- /*!
- Unary function to convert a cv::Point3d to a vpPoint (object frame).
- \param point3d : Point to convert.
- \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3d.
- */
- vpPoint vpConvert::point3dToVpObjectPoint(const cv::Point3d &point3d) {
- vpPoint pt;
- pt.set_oX(point3d.x);
- pt.set_oY(point3d.y);
- pt.set_oZ(point3d.z);
- pt.set_oW(1.0);
- return pt;
- }
-
- /*!
- Unary function to convert a cv::Point3d to a vpPoint (camera frame).
- \param point3d : Point to convert.
- \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3d.
- */
- vpPoint vpConvert::point3dToVpCamPoint(const cv::Point3d &point3d) {
- vpPoint pt;
- pt.set_X(point3d.x);
- pt.set_Y(point3d.y);
- pt.set_Z(point3d.z);
- pt.set_W(1.0);
- return pt;
- }
-
- /*!
- Unary function to convert a vpImagePoint to a cv::Point2f.
- \param point : Image point to convert.
-
- \return A cv::Point2f with the 2D coordinates stored in vpImagePoint.
- */
- cv::Point2f vpConvert::vpImagePointToPoint2f(const vpImagePoint &point) {
- return cv::Point2f((float) point.get_u(), (float) point.get_v());
- }
-
- /*!
- Unary function to convert a vpImagePoint to a cv::Point2d.
- \param point : Image point to convert.
-
- \return A cv::Point2d with the 2D coordinates stored in vpImagePoint.
- */
- cv::Point2d vpConvert::vpImagePointToPoint2d(const vpImagePoint &point) {
- return cv::Point2d(point.get_u(), point.get_v());
- }
-
- /*!
- Unary function to convert the 3D coordinates in the camera frame to a cv::Point3f.
- \param point : Point to convert.
-
- \return A cv::Point3f with the 3D coordinates stored in vpPoint in the camera frame.
- */
- cv::Point3f vpConvert::vpCamPointToPoint3f(const vpPoint &point) {
- return cv::Point3f((float) point.get_X(), (float) point.get_Y(), (float) point.get_Z());
- }
-
- /*!
- Unary function to convert the 3D coordinates in the camera frame to a cv::Point3d.
- \param point : Point to convert.
-
- \return A cv::Point3d with the 3D coordinates stored in vpPoint in the camera frame.
- */
- cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point) {
- return cv::Point3d(point.get_X(), point.get_Y(), point.get_Z());
- }
-
- /*!
- Unary function to convert the 3D coordinates in the object frame to a cv::Point3f.
- \param point : Point to convert.
-
- \return A cv::Point3f with the 3D coordinates stored in vpPoint in the object frame.
- */
- cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point) {
- return cv::Point3f((float) point.get_oX(), (float) point.get_oY(), (float) point.get_oZ());
- }
-
- /*!
- Unary function to convert the 3D coordinates in the object frame to a cv::Point3d.
- \param point : Point to convert.
-
- \return A cv::Point3d with the 3D coordinates stored in vpPoint in the object frame.
- */
- cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) {
- return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
- }
-
- /*!
- Unary function to return the train index stored in a cv::DMatch.
- \param match : The cv::DMatch we want to get the train index.
-
- \return The train index stored in a cv::DMatch.
- */
- int vpConvert::dMatchToTrainIndex(const cv::DMatch &match) {
- return match.trainIdx;
- }
-
-
- /*!
- Convert a cv::KeyPoint to a vpImagePoint.
- \param from : cv::KeyPoint to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to) {
- to = keyPointToVpImagePoint(from);
- }
-
- /*!
- Convert a cv::Point2f to a vpImagePoint.
- \param from : cv::Point2f to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to) {
- to = point2fToVpImagePoint(from);
- }
-
- /*!
- Convert a cv::Point2d to a vpImagePoint.
- \param from : cv::Point2d to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) {
- to = point2dToVpImagePoint(from);
- }
-
- /*!
- Convert a cv::Point3f to a vpPoint.
- \param from : cv::Point3f to convert.
- \param to : vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = point3fToVpCamPoint(from);
- } else {
- to = point3fToVpObjectPoint(from);
- }
- }
-
- /*!
- Convert a cv::Point3d to a vpPoint.
- \param from : cv::Point3d to convert.
- \param to : vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = point3dToVpCamPoint(from);
- } else {
- to = point3dToVpObjectPoint(from);
- }
- }
-
- /*!
- Convert a vector of cv::KeyPoint to a vector of vpImagePoint.
- \param from : Vector of cv::KeyPoint to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), keyPointToVpImagePoint);
- }
-
- /*!
- Convert a vector of cv::Point2f to a vector of vpImagePoint.
- \param from : Vector of cv::Point2f to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), point2fToVpImagePoint);
- }
-
- /*!
- Convert a vector of cv::Point2d to a vector of vpImagePoint.
- \param from : Vector of cv::Point2d to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), point2dToVpImagePoint);
- }
-
- /*!
- Convert a vector of cv::Point3f to a vector of vpPoint.
- \param from : Vector of cv::Point3f to convert.
- \param to : Vector of vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), point3fToVpCamPoint);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), point3fToVpObjectPoint);
- }
- }
-
- /*!
- Convert a vector of cv::Point3d to a vector of vpPoint.
- \param from : Vector of cv::Point3d to convert.
- \param to : Vector of vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), point3dToVpCamPoint);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), point3dToVpObjectPoint);
- }
- }
-
- /*!
- Convert a vector of cv::DMatch to a vector of unsigned int (for a query index 0, to[0] ==> train index).
-
- \warning The list of query indexes in DMatch must be ordered in a way that from[i].queryIdx == i.
-
- \param from : Vector of cv::DMatch to convert.
- \param to : Vector of unsigned int converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), dMatchToTrainIndex);
- }
-
- /*!
- Convert a vpImagePoint to a cv::Point2f.
- \param from : vpImagePoint to convert.
- \param to : cv::Point2f converted.
- */
- void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2f &to) {
- to = vpImagePointToPoint2f(from);
- }
-
- /*!
- Convert a vpImagePoint to a cv::Point2d.
- \param from : vpImagePoint to convert.
- \param to : cv::Point2d converted.
- */
- void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) {
- to = vpImagePointToPoint2d(from);
- }
-
- /*!
- Convert a vpPoint to a cv::Point3f.
- \param from : vpPoint to convert.
- \param to : cv::Point3f converted.
- \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = vpCamPointToPoint3f(from);
- } else {
- to = vpObjectPointToPoint3f(from);
- }
- }
-
- /*!
- Convert a vpPoint to a cv::Point3d.
- \param from : vpPoint to convert.
- \param to : cv::Point3d converted.
- \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = vpCamPointToPoint3d(from);
- } else {
- to = vpObjectPointToPoint3d(from);
- }
- }
-
- /*!
- Convert a vector of vpImagePoint to a vector of cv::Point2f.
- \param from : Vector of vpImagePoint to convert.
- \param to : Vector of cv::Point2f converted.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2f);
- }
-
- /*!
- Convert a vector of vpImagePoint to a vector of cv::Point2d.
- \param from : Vector of vpImagePoint to convert.
- \param to : Vector of cv::Point2d converted.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2d);
- }
-
- /*!
- Convert a vector of vpPoint to a vector of cv::Point3f.
- \param from : Vector of vpPoint to convert.
- \param to : Vector of cv::Point3f converted.
- \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3f);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3f);
- }
- }
-
- /*!
- Convert a vector of vpPoint to a vector of cv::Point3d.
- \param from : Vector of vpPoint to convert.
- \param to : Vector of cv::Point3d converted.
- \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3d);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3d);
- }
- }
-#endif
diff --git a/src/tools/convert/vpConvert.h b/src/tools/convert/vpConvert.h
deleted file mode 100644
index 2a871f2..0000000
--- a/src/tools/convert/vpConvert.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpIoTools.h 5197 2015-01-23 17:24:22Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Directory management.
- *
- * Authors:
- * Fabien Spindler
- * Souriya Trinh
- *
- *****************************************************************************/
-
-
-#ifndef __vpConvert_h__
-#define __vpConvert_h__
-
-/*!
- \file vpConvert.h
- \brief Tools for type or general conversion.
- */
-
-#include <visp/vpConfig.h>
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- #include <opencv2/core/core.hpp>
- #include <opencv2/features2d/features2d.hpp>
-#endif
-
-#include <visp/vpImagePoint.h>
-#include <visp/vpPoint.h>
-
-
-class VISP_EXPORT vpConvert
-{
-
-public:
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- static void convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to);
- static void convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to);
- static void convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to);
- static void convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame=false);
- static void convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame=false);
-
- static void convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to);
- static void convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to);
- static void convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to);
- static void convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
- static void convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
- static void convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to);
-
- static void convertToOpenCV(const vpImagePoint &from, cv::Point2f &to);
- static void convertToOpenCV(const vpImagePoint &from, cv::Point2d &to);
- static void convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame=false);
- static void convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame=false);
-
- static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to);
- static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to);
- static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame=false);
- static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame=false);
-
-private:
- static vpImagePoint keyPointToVpImagePoint(const cv::KeyPoint &keypoint);
- static vpImagePoint point2fToVpImagePoint(const cv::Point2f &point);
- static vpImagePoint point2dToVpImagePoint(const cv::Point2d &point);
- static vpPoint point3fToVpObjectPoint(const cv::Point3f &point3f);
- static vpPoint point3dToVpObjectPoint(const cv::Point3d &point3d);
- static vpPoint point3fToVpCamPoint(const cv::Point3f &point3f);
- static vpPoint point3dToVpCamPoint(const cv::Point3d &point3d);
- static int dMatchToTrainIndex(const cv::DMatch &match);
-
- static cv::Point2f vpImagePointToPoint2f(const vpImagePoint &point);
- static cv::Point2d vpImagePointToPoint2d(const vpImagePoint &point);
- static cv::Point3f vpCamPointToPoint3f(const vpPoint &point);
- static cv::Point3d vpCamPointToPoint3d(const vpPoint &point);
- static cv::Point3f vpObjectPointToPoint3f(const vpPoint &point);
- static cv::Point3d vpObjectPointToPoint3d(const vpPoint &point);
-
-#endif
-
-};
-
-#endif
diff --git a/src/tools/geometry/vpPlane.cpp b/src/tools/geometry/vpPlane.cpp
deleted file mode 100644
index 58055c0..0000000
--- a/src/tools/geometry/vpPlane.cpp
+++ /dev/null
@@ -1,396 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlane.cpp 4740 2014-05-26 07:15:44Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Plane geometrical structure.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpPlane.cpp
- \brief definition of the vpPlane class member functions
- \ingroup libtools
-*/
-
-#include <visp/vpPlane.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-/*!
- Copy operator.
-*/
-vpPlane&
-vpPlane::operator =(const vpPlane& p)
-{
- A = p.A ;
- B = p.B ;
- C = p.C ;
- D = p.D ;
-
- return *this ;
-}
-
-/*!
- Basic constructor that set the plane parameters A, B, C, D to zero.
-*/
-vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
-
-/*!
- Plane constructor from A, B, C, D parameters.
-
- A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
- (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal
- vector of the plane.
-
- \param a, b, c, d : Parameters of the plane.
-
-*/
-vpPlane::vpPlane(const double a,const double b,const double c, const double d)
- : A(a), B(b), C(c), D(d) {}
-
-/*!
- Copy constructor.
-*/
-vpPlane::vpPlane(const vpPlane& P)
- : A(0), B(0), C(0), D(0)
-{
- setA(P.getA()) ;
- setB(P.getB()) ;
- setC(P.getC()) ;
- setD(P.getD()) ;
-}
-
-/*!
-
- Plane constructor from a point \e P on the plane and the normal
- \e n to the plane.
-
- A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
- (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal
- vector of the plane.
-
- \param P : A point with coordinates (x,y,z) on the plane. The \e frame parameter indicates
- if the coordinates of this points that are used are expressed in the camera of object frame.
-
- \param n : The normal to the plane.
-
- \param frame: Indicates if the plane should be initialized from the point P
- coordinates expressed in the camera or object frame.
-
-*/
-vpPlane::vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame)
- : A(0), B(0), C(0), D(0)
-{
- //Equation of the plane is given by:
- A = n[0];
- B = n[1];
- C = n[2];
-
- if (frame == vpPlane::camera_frame)
- D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
- else
- D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
-
-}
-
-/*!
- Initialize the plane with the parameters of an other plane \e P.
-
- \param P : Plane used as initializer.
-*/
-void vpPlane::init(const vpPlane& P)
-{
- setA(P.getA()) ;
- setB(P.getB()) ;
- setC(P.getC()) ;
- setD(P.getD()) ;
-}
-
-/*!
- Initialize the plane from a point \e P on the plane and the normal
- \e n to the plane.
-
- \param P : A point with coordinates (x,y,z) on the plane.
- The size of the vector should be 3, with P[0]=x, with P[1]=y, with P[2]=z.
-
- \param n : The normal to the plane.
-
- \sa vpPlane(const vpPoint&, const vpColVector &)
-*/
-void vpPlane::init(const vpColVector & P, const vpColVector &n)
-{
- //Equation of the plane is given by:
- A = n[0];
- B = n[1];
- C = n[2];
-
- D=-(A*P[0]+B*P[1]+C*P[2]);
-}
-
-/*!
- Compute the equation of a plane given three point P, Q, R.
-
- The normal to the plane is given by:
- n = PQ x PR
-
- \param P,Q,R: Three points on the plane.
- \param frame: Indicates if the plane should be initialized from the points
- coordinates expressed in the camera or object frame.
-
-*/
-void
-vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
-{
- vpColVector a(3);
- vpColVector b(3);
- vpColVector n(3);
- if (frame == vpPlane::camera_frame) {
- //Calculate vector corresponding to PQ
- a[0]=P.get_X()-Q.get_X();
- a[1]=P.get_Y()-Q.get_Y();
- a[2]=P.get_Z()-Q.get_Z();
-
- //Calculate vector corresponding to PR
- b[0]=P.get_X()-R.get_X();
- b[1]=P.get_Y()-R.get_Y();
- b[2]=P.get_Z()-R.get_Z();
- }
- else {
- //Calculate vector corresponding to PQ
- a[0]=P.get_oX()-Q.get_oX();
- a[1]=P.get_oY()-Q.get_oY();
- a[2]=P.get_oZ()-Q.get_oZ();
-
- //Calculate vector corresponding to PR
- b[0]=P.get_oX()-R.get_oX();
- b[1]=P.get_oY()-R.get_oY();
- b[2]=P.get_oZ()-R.get_oZ();
- }
- //Calculate normal vector to plane PQ x PR
- n=vpColVector::cross(a,b);
-
- //Equation of the plane is given by:
- A = n[0];
- B = n[1];
- C = n[2];
- if (frame == vpPlane::camera_frame)
- D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
- else
- D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
-
- double norm = sqrt(A*A+B*B+C*C) ;
- A /= norm ;
- B /= norm ;
- C /= norm ;
- D /= norm ;
-}
-
-
-/*!
- Compute the equation of a plane given three point P, Q, R.
-
- The normal to the plane is given by:
- n = PQ x PR
-
- \param P,Q,R: Three points on the plane.
- \param frame: Indicates if the plane should be initialized from the points
- coordinates expressed in the camera or object frame.
-
- \sa init(const vpPoint &, const vpPoint &, const vpPoint &)
-*/
-vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
- : A(0), B(0), C(0), D(0)
-{
- init(P, Q, R, frame) ;
-}
-
-/*!
- Return the normal to the plane.
-
- A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
- (x,y,z) is a point of R^3 and (A,B,C) are the coordinates of the normal.
-
- \sa getNormal(vpColVector &n)
-*/
-vpColVector vpPlane::getNormal() const
-{
- vpColVector n(3);
- n[0] = A ;
- n[1] = B ;
- n[2] = C ;
-
- return n ;
-}
-
-/*!
- Return the normal to the plane.
-
- A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
- (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is normal
- vector of the plane.
-
- \sa getNormal()
-
-*/
-void vpPlane::getNormal(vpColVector &n) const
-{
- n.resize(3) ;
- n[0] = A ;
- n[1] = B ;
- n[2] = C ;
-}
-
-/*!
- Compute the coordinates of the projection of a point on the plane.
-
- \param P : point to be projected on the plane
- \param Pproj : result of the projection (pproj belongs to the plane)
-*/
-void
-vpPlane::projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const
-{
- double x0,y0,z0 ;
- double rho ;
-
- x0 = P.get_X()/P.get_W() ;
- y0 = P.get_Y()/P.get_W() ;
- z0 = P.get_Z()/P.get_W() ;
-
- rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
-
- Pproj.set_X(x0+A*rho) ;
- Pproj.set_Y(y0+B*rho) ;
- Pproj.set_Z(z0+C*rho) ;
- Pproj.set_W(1) ;
-}
-
-
-double
-vpPlane::rayIntersection(const vpPoint &M0,
- const vpPoint &M1,
- vpColVector &H ) const
-{
-
- double k,scal;
- double R[3];
-
- // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
- if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
- || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
- || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
- {
- R[0]= M1.get_X() - M0.get_X();
- R[1]= M1.get_Y() - M0.get_Y();
- R[2]= M1.get_Z() - M0.get_Z();
-
- scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
- //if (scal != 0)
- if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
- else
- k = 0;
-
- H[0] = M0.get_X()+ k*R[0];
- H[1] = M0.get_Y()+ k*R[1];
- H[2] = M0.get_Z()+ k*R[2];
- }
- else
- {
- scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
- //if (scal != 0)
- if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -getD()/scal;
- else
- k=0;
- H[0] = k*M1.get_X();
- H[1] = k*M1.get_Y();
- H[2] = k*M1.get_Z();
- }
-
- return k;
-
-}
-
-double vpPlane::getIntersection(const vpColVector &M1,vpColVector &H )const
-{
-
- double k,scal;
-
- scal = A*M1[0] + B*M1[1] + C*M1[2];
- //if (scal != 0)
- if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -getD()/scal;
- else
- k=0;
- H[0] = k*M1[0];
- H[1] = k*M1[1];
- H[2] = k*M1[2];
-
- return k;
-
-}
-
-/*!
-
- Considering the plane in the Ro frame computes the equation of the
- plane in the Rc frame.
-
- \param cMo : Homogeneous transformation from Rc to Ro frames.
-
-*/
-void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- // Save current plane parameters
- double Ao = A, Bo = B, Co = C, Do =D ;
- A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
- B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
- C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
- D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
-}
-
-/*!
-
- Print the plane parameters as a stream like "(A,B,C,D) " where
- A,B,C and D correspond to the parameters of the plane.
-
-*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
-{
- return (os << "("<<p.getA() << ","<<p.getB()
- << ","<<p.getC()<< ","<<p.getD() <<") ") ;
-} ;
diff --git a/src/tools/geometry/vpPlane.h b/src/tools/geometry/vpPlane.h
deleted file mode 100644
index c55e08f..0000000
--- a/src/tools/geometry/vpPlane.h
+++ /dev/null
@@ -1,177 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlane.h 4702 2014-03-27 15:33:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Plane geometrical structure.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpPlane_hh
-#define vpPlane_hh
-
-
-
-
-/*!
- \class vpPlane
-
- \ingroup GeometryPlane
-
- \brief This class defines the container for a plane geometrical structure.
-
- A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where
- (x,y,z) are the coordinates of a point and where \f$[A,B,C]^T\f$ is a normal
- vector of the plane.
-
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-
-class VISP_EXPORT vpPlane
-{
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- // for backward compatibility
- public:
-#else
- private:
-#endif
- double A,B,C,D ;
-
-public:
- typedef enum {
- object_frame,
- camera_frame
- } vpPlaneFrame;
- vpPlane() ;
- vpPlane(const vpPlane& P) ;
- vpPlane(const double A, const double B,const double C,const double D) ;
- vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame=camera_frame) ;
- vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame) ;
- void init(const vpPoint& P, const vpPoint& Q, const vpPoint& R, vpPlaneFrame frame=camera_frame) ;
- void init(const vpColVector& P, const vpColVector &n) ;
- void init(const vpPlane& P) ;
-
- // SET the parameter
- /*! Set plane parameter A. */
- inline void setA(const double a) { this->A = a ; }
- /*! Set plane parameter B. */
- inline void setB(const double b) { this->B = b ; }
- /*! Set plane parameter C. */
- inline void setC(const double c) { this->C = c ; }
- /*! Set plane parameter D. */
- inline void setD(const double d) { this->D = d ; }
- /*! Set plane parameters A, B, C, D. */
- inline void setABCD(const double a, const double b,
- const double c, const double d)
- {
- this->A = a;
- this->B = b;
- this->C = c;
- this->D = d;
- }
-
- vpPlane& operator =(const vpPlane& f) ;
-
- // GET information
- /*! \return The value of the plane parameter A. */
- double getA() const { return A ; }
- /*! \return The value of the plane parameter B. */
- double getB() const { return B ; }
- /*! \return The value of the plane parameter C. */
- double getC() const { return C ; }
- /*! \return The value of the plane parameter D. */
- double getD() const { return D ; }
-
- /*!
-
- \return Return the four dimension vector \f$[A,B,C,D]^T\f$
- corresponding to the plane parameters.
-
- */
- inline vpColVector getABCD() const {
- vpColVector n(4);
- n[0]=A;
- n[1]=B;
- n[2]=C;
- n[3]=D;
-
- return n;
- }
- /*!
-
- \warning This method is provided for compatibility with the
- previous versions. Users should now use getABCD().
-
- \return Return the four dimension vector \f$[A,B,C,D]^T\f$
- corresponding to the plane parameters.
-
- \sa getABCD()
- */
- inline vpColVector abcd() const {
- vpColVector n(4);
- n[0]=A;
- n[1]=B;
- n[2]=C;
- n[3]=D;
-
- return n;
- }
-
- vpColVector getNormal() const;
- void getNormal(vpColVector &n) const;
-
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p);
-
- // Operation with Plane
- void projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const ;
-
- double rayIntersection(const vpPoint &M0,
- const vpPoint &M1,
- vpColVector &H )const ;
-
- double getIntersection(const vpColVector &M1,vpColVector &H )const ;
- void changeFrame(const vpHomogeneousMatrix &cMo);
-
-} ;
-
-
-
-#endif
diff --git a/src/tools/geometry/vpPolygon.cpp b/src/tools/geometry/vpPolygon.cpp
deleted file mode 100644
index 4cef509..0000000
--- a/src/tools/geometry/vpPolygon.cpp
+++ /dev/null
@@ -1,412 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPolygon.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a generic 2D polygon.
- *
- * Author:
- * Amaury Dame
- * Nicolas Melchior
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpPolygon.h>
-#include <visp/vpException.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpNoise.h>
-#include <set>
-#include <limits>
-/*!
- Basic constructor.
-
- By default, it defines a triangle with the three 2D points coordinates :
- \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
-*/
-vpPolygon::vpPolygon()
- : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
-{
- std::vector<vpImagePoint> corners;
- corners.push_back(vpImagePoint(0,0));
- corners.push_back(vpImagePoint(1,0));
- corners.push_back(vpImagePoint(0,1));
- init(corners);
-}
-
-/*!
- Constructor which initialise the polygon thanks to the given corners.
-
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
- \param corners : The Points defining the corners.
-*/
-vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
- : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
-{
- if(corners.size() < 3){
- _goodPoly = false;
- }
- init(corners);
-}
-
-/*!
- Copy constructor
-
- \param poly : The polygon used for the initialisation.
-*/
-vpPolygon::vpPolygon(const vpPolygon &poly)
- : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
-{
- _corners = poly._corners;
- _center = poly._center;
- _area = poly._area;
- _goodPoly = poly._goodPoly;
- _bbox = poly._bbox;
-}
-
-/*!
- Basic destructor
-*/
-vpPolygon::~vpPolygon()
-{
-}
-
-/*!
- Equal operator.
-
- Assign \e poly to this polygon and return a reference to it.
-*/
-vpPolygon &
-vpPolygon::operator=(const vpPolygon& poly)
-{
- _corners = poly._corners;
- _center = poly._center;
- _area = poly._area;
- _goodPoly = poly._goodPoly;
- return *this;
-};
-
-/*!
- Initialise the triangle thanks to the collection of 2D points (in pixel).
-
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
- \param corners : The corners of the polyon.
-*/
-void
-vpPolygon::buildFrom(const std::vector<vpImagePoint>& corners)
-{
- init(corners);
-}
-
-/*!
- Initialise the triangle thanks to the collection of 2D points (in meter). The
- fields \e x and \e y are used to compute the corresponding coordinates in
- pixel thanks to the camera parameters \e cam.
-
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
- \param corners : The corners of the polyon.
- \param cam : The camera parameters used to convert the coordinates from meter
- to pixel.
-*/
-void
-vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
-{
- std::vector<vpImagePoint> ipCorners(corners.size());
- for(unsigned int i=0; i<corners.size(); ++i){
- vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
- }
- buildFrom(ipCorners);
-}
-
-/*!
- Initialise the polygon by (left-)clicking to add a corners to the polygon.
- A right click is used to stop the addition of new corner.
-
- \param I : The image where to click to initialise the corners.
-*/
-void
-vpPolygon::initClick(const vpImage<unsigned char>& I)
-{
- vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
- vpImagePoint ip;
-
- std::vector<vpImagePoint> cornersClick;
-
- while(button == vpMouseButton::button1){
- bool ret = vpDisplay::getClick(I, ip, button, true);
- if(ret && button == vpMouseButton::button1){
- vpDisplay::displayCross(I, ip, 5, vpColor::red);
- cornersClick.push_back(ip);
- vpDisplay::flush(I);
- }
- }
-
- buildFrom(cornersClick);
-}
-
-
-/*!
- Intialise the polygon using the collection of image points. This method
- compute come internal variables such as center, area, ...
-
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
- \param corners : The corners of the polyon.
-*/
-void
-vpPolygon::init(const std::vector<vpImagePoint>& corners)
-{
- _corners = corners;
-
- updateBoundingBox();
- updateArea();
- updateCenter();
-}
-
-
-
-/*!
- Test if two segments are intersecting.
-
- \throw vpException::divideByZeroError if the two lines are aligned (
- denominator equal to zero).
-
- \param ip1 : The first image point of the first segment.
- \param ip2 : The second image point of the first segment.
- \param ip3 : The first image point of the second segment.
- \param ip4 : The second image point of the second segment.
-*/
-bool
-vpPolygon::testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4)
-{
- double di1 = ip2.get_i() - ip1.get_i();
- double dj1 = ip2.get_j() - ip1.get_j();
-
- double di2 = ip4.get_i() - ip3.get_i();
- double dj2 = ip4.get_j() - ip3.get_j();
-
- double denominator = di1 * dj2 - dj1 * di2;
-
- if(fabs(denominator) < std::numeric_limits<double>::epsilon()){
- throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
- }
-
- double alpha = - ( ( ip1.get_i() - ip3.get_i() ) * dj2 + di2 * ( ip3.get_j() - ip1.get_j())) / denominator;
- if(alpha < 0 || alpha >= 1){
- return false;
- }
-
- double beta = - (di1 * (ip3.get_j() - ip1.get_j() ) + dj1 * (ip1.get_i() - ip3.get_i()) ) / denominator;
- if(beta < 0 || beta >= 1){
- return false;
- }
-
- return true;
-}
-
-/*!
- Check if the 2D point \f$ iP \f$ is inside the polygon.
-
- \param ip : The point which have to be tested.
-
- \return Returns true if the point is inside the triangle, false otherwise.
-*/
-bool
-vpPolygon::isInside(const vpImagePoint& ip)
-{
- vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
- vpUniRand generator;
- infPoint.set_i( infPoint.get_i() + 1000 * generator());
- infPoint.set_j( infPoint.get_j() + 1000 * generator());// we add random since it appears that sometimes infPoint may cause a degenerated case (so realucnch and hope that result will be different).
-
- unsigned int nbinterscetion = 0;
- for(unsigned int i=0; i<_corners.size(); ++i){
- vpImagePoint ip1 = _corners[i];
- vpImagePoint ip2 = _corners[(i+1)%_corners.size()];
- bool intersection = false;
-
- // If the points are the same we continue without trying to found
- // an intersection
- if (ip1 == ip2)
- continue;
-
- try{
- intersection = testIntersectionSegments(ip1, ip2, ip, infPoint );
- }catch(vpException e){
- return isInside(ip);
- }
-
- if(intersection){
- ++nbinterscetion;
- }
- }
-
- return ((nbinterscetion%2)==1);
-}
-
-
-
-/*!
- Update the _area attribute of the polygon using the corners.
-
- The area is computed using the formula:
- \f[
- A = \frac{1}{2} \sum_{i=0}^{n-1} (x_1 y_{i+1} - x_{i+1} y_{i})
- \f]
-
-*/
-void
-vpPolygon::updateArea()
-{
- if(_corners.size() == 0){
- _area = 0;
- _goodPoly = false;
- return;
- }
- _area = 0;
-
- for(unsigned int i=0; i<_corners.size(); ++i){
- unsigned int i_p_1 = ( i+1 ) % _corners.size();
- _area += _corners[i].get_j() * _corners[i_p_1].get_i()
- - _corners[i_p_1].get_j() * _corners[i].get_i();
- }
-
- _area /= 2;
- if(_area < 0){
- _area = - _area;
- }
-}
-
-
-/*!
- Update the _center attribute of the polygon using the corners.
-
- The i coordinates is computed using:
-
- \f[
- i = \frac{1}{6{area}} \sum_{i=0}^{n-1} (i_i + i_{i+1})(i_{i+1} j_i - j_{i+1} i_i)
- \f]
-
- The computation of the j coordinate is similar.
-*/
-void
-vpPolygon::updateCenter()
-{
- if(_corners.size() == 0){
- _center = vpImagePoint(0, 0);
- _goodPoly = false;
- return;
- }
- double i_tmp = 0;
- double j_tmp = 0;
-#if 0
- for(unsigned int i=0; i<(_corners.size()-1); ++i){
- i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
- (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
-
- j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
- (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
- }
-#else
- for(unsigned int i=0; i<_corners.size(); ++i){
- unsigned int i_p_1 = ( i+1 ) % _corners.size();
- i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
- (_corners[i_p_1].get_i() * _corners[i].get_j()
- - _corners[i_p_1].get_j() * _corners[i].get_i());
-
- j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
- (_corners[i_p_1].get_i() * _corners[i].get_j()
- - _corners[i_p_1].get_j() * _corners[i].get_i());
- }
-#endif
-
- if(_area > 0){
- _center.set_i(fabs(i_tmp / (6 * _area)));
- _center.set_j(fabs(j_tmp / (6 * _area)));
- }else{
- _center = _corners[0];
- _goodPoly = false;
- }
-}
-
-void
-vpPolygon::updateBoundingBox()
-{
- if(_corners.size() == 0){
- _bbox.setBottomRight(vpImagePoint(0, 0));
- _bbox.setTopLeft(vpImagePoint(0, 0));
- _goodPoly = false;
- return;
- }
-
- std::set<double> setI;
- std::set<double> setJ;
- for(unsigned int i=0; i<_corners.size(); ++i){
- setI.insert(_corners[i].get_i());
- setJ.insert(_corners[i].get_j());
- }
- vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
- std::set<double>::const_iterator iterI = setI.end();
- std::set<double>::const_iterator iterJ = setJ.end();
- --iterI;
- --iterJ;
- vpImagePoint br(*iterI, *iterJ);
-
- if(tl == br){
- _goodPoly = false;
- }
- _bbox.setTopLeft(tl);
- _bbox.setBottomRight(br);
-}
-
-/*!
- Display the polygon in the image (overlay, so the image is not modified).
- A call to the flush() method is necessary.
-
- \param I : The image where is displayed the polygon.
- \param color : The color of the lines of the polygon.
- \param thickness : The thickness of the lines used to display the polygon.
-*/
-void
-vpPolygon::display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness)
-{
- const unsigned int N = (unsigned int)_corners.size();
- for(unsigned int i=0; i<N; ++i){
- vpDisplay::displayLine(I, _corners[i], _corners[(i+1)%N], color, thickness);
- }
-}
diff --git a/src/tools/geometry/vpPolygon.h b/src/tools/geometry/vpPolygon.h
deleted file mode 100644
index 13cf644..0000000
--- a/src/tools/geometry/vpPolygon.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPolygon.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a generic 2D polygon.
- *
- * Author:
- * Amaury Dame
- * Nicolas Melchior
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#ifndef vpPolygon_h
-#define vpPolygon_h
-
-/*!
- \class vpPolygon
- \brief Defines a generic 2D polygon.
-
- A polygon is internally represented by N 2D points.
-
- By default three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint
- class documentation for more details about the frame) are used \f$ (0,0) \f$,
- \f$ (1,0) \f$ and \f$ (0,1) \f$.
-
- The code bellow shows how to manipulate a polygon.
-\code
-#include <iostream>
-
-#include <visp/vpPolygon.h>
-
-int main()
-{
- std::vector<vpImagePoint> corners;
-
- // Initialize the corners vector with 4 points
- corners.push_back( vpImagePoint( 50, 100) );
- corners.push_back( vpImagePoint( 50, 300) );
- corners.push_back( vpImagePoint(200, 300) );
- corners.push_back( vpImagePoint(200, 100) );
-
- // Initialize a polygon with the corners
- vpPolygon polygon(corners);
-
- // Get the polygon bounding box
- vpRect bbox = polygon.getBoundingBox();
- std::cout << "Bounding box: " << bbox.getTopLeft() << " to " << bbox.getBottomRight() << std::endl;
-
- // Get the polygon surface and center
- std::cout << "Area: " << polygon.getArea() << std::endl;
- std::cout << "Center: " << polygon.getCenter() << std::endl;
-
- // Check if a point is inside the polygon
- vpImagePoint ip(550, 200);
- std::cout << "The point " << ip << " is " << (polygon.isInside(ip) ? "inside":"outside") << " the polygon" << std::endl;
-
- return 0;
-}
-\endcode
-*/
-
-#include <visp/vpImagePoint.h>
-#include <visp/vpRect.h>
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpPoint.h>
-#include <visp/vpCameraParameters.h>
-
-#include <vector>
-
-class VISP_EXPORT vpPolygon
-{
- protected:
- //! Collection of image points containing the corners.
- std::vector<vpImagePoint> _corners;
- //! Center of the polygon. It is automatically computed when the corners are set.
- vpImagePoint _center;
- //! Area of the polygon.
- double _area;
- //! Flag to indicate whether the polygon is a good polygon (ie. it has more than two corners, ...)
- bool _goodPoly;
- //! Boumding box containing the polygon.
- vpRect _bbox;
-
- public:
- vpPolygon();
- vpPolygon(const std::vector<vpImagePoint>& corners);
- vpPolygon(const vpPolygon &poly);
- virtual ~vpPolygon();
-
- vpPolygon &operator=(const vpPolygon& poly);
-
- void buildFrom(const std::vector<vpImagePoint>& corners);
- void buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam);
- void initClick(const vpImage<unsigned char>& I);
-
- bool isInside(const vpImagePoint &iP);
-
- void display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness=1);
-
- /*!
- Get the corners of the polygon.
-
- \return A reference to the corners.
- */
- const std::vector<vpImagePoint>& getCorners() const {
- return _corners;
- }
-
-
- /*!
- Return the area of the polygon.
- The area is computed when the polygon is built from the corners.
-
- \return The area of the polygon.
- */
- inline double getArea() const{
- return this->_area;
- }
-
- /*!
- Return the center of the polygon.
- The center is computed when the polygon is built from the corners.
-
- \return The area of the polygon.
- */
- inline vpImagePoint getCenter() const{
- return this->_center;
- }
-
- /*!
- Return the bounding box. The bounding box is the smallest rectangle
- containing all the polygon.
-
- \return The bounding box of the polygon.
- */
- inline vpRect getBoundingBox() const {
- return _bbox;
- }
-
- protected:
- void init(const std::vector<vpImagePoint>& corners);
- void updateArea();
- void updateCenter();
- void updateBoundingBox();
-
- private:
- bool testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4);
-
-};
-
-#endif
-
diff --git a/src/tools/geometry/vpRect.cpp b/src/tools/geometry/vpRect.cpp
deleted file mode 100755
index 1dba245..0000000
--- a/src/tools/geometry/vpRect.cpp
+++ /dev/null
@@ -1,256 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRect.cpp 4917 2014-10-06 08:35:22Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a rectangle in the plane.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpRect.cpp
- \brief Defines a rectangle in the plane.
- \ingroup libtools
-*/
-
-
-#include <visp/vpRect.h>
-#include <visp/vpDebug.h>
-
-/*!
-
- Constructs a default rectangle with the \e top, \e left corner set to (0,0)
- and \e width and \e height set to 1.
-
-*/
-
-vpRect::vpRect() : left(0), top(0), width(0), height(0) {};
-
-/*!
-
- Constructs a rectangle with the \e top, \e left corner and \e width
- and \e height.
-
- \param l : coordinate of the top/left corner along the horizontal axis.
- \param t : coordinate of the top/left corner along the vertical axis.
- \param w : rectangle width.
- \param h : rectangle height.
-
-*/
-vpRect::vpRect(double l, double t, double w, double h)
- : left(l), top(t), width(w), height(h)
-{
-};
-
-/*!
-
- Constructs a rectangle with \e topLeft the top-left corner location
- and \e width and \e height the rectangle size.
-
- \param topLeft : coordinates of the top/left corner.
- \param w : rectangle width.
- \param h : rectangle height.
-
-*/
-vpRect::vpRect(const vpImagePoint &topLeft, double w, double h)
- : left(topLeft.get_u()), top(topLeft.get_v()), width(w), height(h)
-{
-};
-
-/*!
-
- Constructs a rectangle with \e topLeft the top-left corner location
- and \e bottomRight the bottom-right corner.
-
-*/
-vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
- : left(topLeft.get_u()), top(topLeft.get_v()), width(0), height(0)
-{
- this->left = topLeft.get_u();
- this->top = topLeft.get_v();
-
- setBottom( bottomRight.get_v() );
- setRight( bottomRight.get_u() );
-};
-
-/*!
-
- Constructs a rectangle that is a copy of \e r.
-
-*/
-vpRect::vpRect(const vpRect& r)
- : left(0), top(0), width(0), height(0)
-{
- *this = r;
-};
-
-/*!
-
- Assign \e r to this rectangle and return a reference to this rectangle.
-*/
-vpRect &vpRect::operator=(const vpRect& r)
-{
- this->left = r.left;
- this->top = r.top;
- this->width = r.width;
- this->height = r.height;
- return *this;
-};
-
-/*!
-
- Create a rectangle as the bounding box of a vector of image points.
- \param ip : Vector of image points. At least 1 points is mandatory,
- otherwise an exception is thrown.
-*/
-vpRect::vpRect(const std::vector<vpImagePoint> &ip)
- : left(0), top(0), width(0), height(0)
-{
- set(ip);
-}
-
-/*!
-
- Set the rectangle with the \e top, \e left corner and \e width
- and \e height.
- \param l : coordinate of the top/left corner along the horizontal axis.
- \param t : coordinate of the top/left corner along the vertical axis.
- \param w : rectangle width.
- \param h : rectangle height.
-
-*/
-void vpRect::set(double l, double t, double w, double h)
-{
- left = l;
- top = t;
- width = w;
- height = h;
-};
-
-/*!
-
- Set the rectangle with \e topLeft the top-left corner location
- and \e width and \e height the rectangle size.
-
- \param topLeft : coordinates of the top-left corner.
- \param w : rectangle width.
- \param h : rectangle height.
-*/
-void vpRect::set(const vpImagePoint &topLeft, double w, double h)
-{
- left = topLeft.get_u();
- top = topLeft.get_v();
- width = w;
- height = h;
-};
-
-/*!
-
- Set the rectangle as the bounding box of a vector of image points.
- \param ip : Vector of image points. At least 1 points is mandatory,
- otherwise an exception is thrown.
-*/
-void vpRect::set(const std::vector<vpImagePoint> &ip)
-{
- if (ip.size() < 1)
- throw (vpException(vpException::dimensionError,
- "At least 1 point is requested to build a rectangle"));
- double minu, maxu;
- double minv, maxv;
- minu = maxu = ip[0].get_u();
- minv = maxv = ip[0].get_v();
-
- for(size_t i=1; i<ip.size(); i++) {
- double u = ip[i].get_u();
- double v = ip[i].get_v();
- if ( u < minu ) minu = u;
- else if (u > maxu) maxu = u;
- if ( v < minv ) minv = v;
- else if (v > maxv) maxv = v;
- }
-
- setLeft (minu);
- setTop (minv);
- setRight (maxu);
- setBottom(maxv);
-};
-
-/*!
-
- Set the rectangle with \e topLeft the top-left corner location
- and \e bottomRight the bottom-right corner.
-
- \param topLeft : coordinates of the top-left corner.
- \param bottomRight : coordinates of the bottom-right corner.
-
-*/
-void vpRect::set(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
-{
- this->left = topLeft.get_u();
- this->top = topLeft.get_v();
-
- setBottom( bottomRight.get_v() );
- setRight( bottomRight.get_u() );
-};
-
-/*!
-
- Set the rectangle from \e r.
-
-*/
-void vpRect::set(const vpRect& r)
-{
- *this = r;
-};
-
-/*!
-
- Check if an image point belongs to a rectangle.
-
- \param ip : the image point.
- \param rect : the rectangle.
-
- \return Returns true if the point belongs to the rectangle.
-
-*/
-VISP_EXPORT bool inRectangle( const vpImagePoint &ip, const vpRect &rect ) {
- return ( ip.get_i() <= rect.getBottom() && ip.get_i() >= rect.getTop() && ip.get_j() <= rect.getRight() && ip.get_j() >= rect.getLeft());
-}
-
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRect& r)
- {
- os << r.getLeft() << ", " << r.getTop() << ", " << r.getWidth() << ", " << r.getHeight();
- return os;
-}
diff --git a/src/tools/geometry/vpRect.h b/src/tools/geometry/vpRect.h
deleted file mode 100755
index 9ae88e5..0000000
--- a/src/tools/geometry/vpRect.h
+++ /dev/null
@@ -1,335 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpRect.h 5009 2014-11-25 18:00:13Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a rectangle in the plane.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpRect_h
-#define vpRect_h
-
-
-
-
-/*!
- \class vpRect
-
- \brief Defines a rectangle in the plane.
-
- A rectangle is internally represented as an upper-left corner and a
- width and height, but it is normally expressed as an upper-left
- corner and bottom-right corner.
-
- Note that the size (width and height) of a rectangle might be
- different from what you are used to. If the top-left corner and the
- bottom-right corner are the same, the height and the width of the
- rectangle will both be 1.
-
- Generally, width = right - left + 1 and height = bottom - top +
- 1. We designed it this way to make it correspond to rectangular
- spaces used by drawing functions in which the width and height
- denote a number of pixels. For example, drawing a rectangle with
- width and height 1 draws a single pixel.
-
- The default coordinate system has origin (0, 0) in the top-left
- corner. The positive direction of the y axis is down, and the
- positive x axis is from left to right.
-
- A vpRect can be constructed with a set of left, top, width and
- height double. After creation the dimensions can be changed,
- e.g. with setLeft(), setRight(), setTop() and setBottom(), or by
- setting sizes, e.g. setWidth(), setHeight()
-
-*/
-
-#include <vector>
-#include <visp/vpException.h>
-#include <visp/vpImagePoint.h>
-
-
-class VISP_EXPORT vpRect
-{
-public:
-
- vpRect();
- vpRect(double left, double top, double width, double height);
- vpRect(const vpImagePoint &topLeft, double width, double height);
- vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight);
- vpRect(const vpRect& r);
- vpRect(const std::vector<vpImagePoint> &ip);
-
-
- vpRect &operator=(const vpRect& r);
-
- /*!
- Returns the bottom coordinate of the rectangle.
- \sa getRight()
- */
- inline double getBottom() const { return (this->top + this->height - 1.0); };
- /*!
- Returns the bottom-right coordinate of the rectangle.
- \sa getTopLeft(), getBottom(), getRight()
- */
- inline vpImagePoint getBottomRight() const {
- vpImagePoint bottomRight;
- bottomRight.set_u( getRight() );
- bottomRight.set_v( getBottom() );
-
- return bottomRight;
- };
- /*!
-
- Returns the center point of the rectangle. The center point
- coordinates are (\e x, \e y)
-
- The default coordinate system has origin (0, 0) in the top-left
- corner. The positive direction of the y axis is down, and the
- positive x axis is from left to right.
-
- \sa moveCenter()
- */
- inline void getCenter(double & x, double & y) const {
- x = this->left + this->width / 2.0 - 0.5;
- y = this->top + this->height / 2.0 - 0.5;
- };
- /*!
-
- Returns the center point of the rectangle. The center point
- coordinates are (\e x, \e y)
-
- The default coordinate system has origin (0, 0) in the top-left
- corner. The positive direction of the y axis is down, and the
- positive x axis is from left to right.
-
- \sa moveCenter()
- */
- inline vpImagePoint getCenter() const {
- vpImagePoint center;
- center.set_u( this->left + this->width / 2.0 - 0.5 );
- center.set_v( this->top + this->height / 2.0 - 0.5 );
- return center;
- };
- /*!
-
- Returns the height of the rectangle. The height includes both the
- top and bottom edges, i.e. height = bottom - top + 1.
-
- \sa getWidth()
-
- */
- inline double getHeight() const { return this->height; };
- /*!
- Returns the left coordinate of the rectangle.
-
- \sa getTopLeft(), getRight()
- */
- inline double getLeft() const { return this->left; };
-
- /*!
- Returns the right coordinate of the rectangle.
- \sa getLeft()
- */
- inline double getRight() const { return (this->left + this->width - 1.0); };
-
- /*!
- Returns the size of the rectangle.
- \sa getWidth(), getHeight()
- */
- inline double getSize() const { return (this->width * this->height); };
-
- /*!
- Returns the top coordinate of the rectangle.
-
- \sa getTopLeft(), getBottom()
- */
- inline double getTop() const { return this->top; };
- /*!
- Returns the top-left position of the rectangle.
-
- \sa getBottomRight(), getTop(), getLeft()
- */
- inline vpImagePoint getTopLeft() const {
- vpImagePoint topLeft;
- topLeft.set_u( this->left );
- topLeft.set_v( this->top );
- return topLeft;
- };
- /*!
- Returns the width of the rectangle. The width includes both the
- left and right edges, i.e. width = right - left + 1.
-
- \sa getHeight()
-
- */
- inline double getWidth() const { return this->width; };
-
- friend VISP_EXPORT bool inRectangle( const vpImagePoint &ip, const vpRect &rect );
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRect& r);
- void set(double left, double top, double width, double height);
- void set(const vpImagePoint &topLeft, double width, double height);
- void set(const vpImagePoint &topLeft, const vpImagePoint &bottomRight);
- void set(const vpRect& r);
- void set(const std::vector<vpImagePoint> &ip);
-
- /*!
-
- Sets the bottom edge position of the rectangle to pos. May change
- the height of the rectangle, but will never change the top edge
- position the rectangle.
-
- \sa setTop()
- */
- inline void setBottom(double pos) { this->height = pos - this->top + 1.0; };
- /*!
-
- Sets the bottom-right position of the rectangle. Will never change
- the top-left position the rectangle.
-
- \sa setTopLeft()
- */
- inline void setBottomRight(const vpImagePoint &bottomRight) {
- this->height = bottomRight.get_v() - this->top + 1.0;
- this->width = bottomRight.get_u() - this->left + 1.0;
- };
- /*!
-
- Sets the height of the rectangle to \e h. The top edge is not moved,
- but the bottom edge may be moved.
-
- \sa setWidth()
- */
- inline void setHeight(double h) { this->height = h; };
- /*!
-
- Sets the left edge position of the rectangle to pos. May change the right
- edge position of the rectangle, but will never change the width of the
- rectangle.
-
- \sa setRight()
-
- */
- inline void setLeft(double pos) { this->left = pos; };
- /*!
-
- Sets the coordinates of the rectangle's top left corner to
- (left, top), and its size to (width, height).
-
- \param l,t : (left, top) corner position.
- \param w,h : (width, height) rectangle size.
-
- */
- inline void setRect(double l, double t, double w, double h) {
- this->left = l;
- this->top = t;
- this->width = w;
- this->height = h;
- };
- /*!
-
- Sets the right edge position of the rectangle to pos. May change
- the width of the rectangle, but will never change the left edge
- position of the rectangle.
-
- \sa setLeft()
-
- */
- inline void setRight(double pos) { this->width = pos - this->left + 1.0; };
- /*!
-
- Sets the top edge position of the rectangle to pos. May change the bottom
- edge position of the rectangle, but will never change the height of the
- rectangle.
-
- \sa setBottom()
- */
- inline void setTop(double pos) { this->top = pos; };
- /*!
-
- Sets the top-left position of the rectangle. May change the bottom
- edge position of the rectangle, but will never change the height of the
- rectangle.
-
- \sa setBottomRight()
- */
- inline void setTopLeft(const vpImagePoint &topLeft) {
- this->left = topLeft.get_u();
- this->top = topLeft.get_v();
- };
- /*!
-
- Sets the width of the rectangle to \e w. The right edge is changed,
- but not the left edge.
-
- \sa setHeight()
- */
- inline void setWidth(double w) { this->width = w; }
-
- /*!
-
- Sets the center point of the rectangle to (\e x, \e y), leaving
- the size unchanged.
-
- \sa getCenter()
-
- */
- inline void moveCenter(double x, double y) {
- this->left = x - this->width/2 + 0.5;
- this->top = y - this->height/2 + 0.5;
- };
-
- /*!
-
- Center the rectangle to the image point given as parameter, leaving
- the size unchanged.
-
- \sa getCenter()
-
- */
- inline void moveCenter(const vpImagePoint ¢er) {
- this->left = center.get_u() - this->width/2 + 0.5;
- this->top = center.get_v() - this->height/2 + 0.5;
- };
-
-private:
- double left; // Upper left corner position along the columns axis
- double top; // Upper left corner position along the rows axis
- double width; // Rectangle width
- double height; // Rectangle height
-};
-
-#endif
diff --git a/src/tools/geometry/vpTriangle.cpp b/src/tools/geometry/vpTriangle.cpp
deleted file mode 100644
index 8910371..0000000
--- a/src/tools/geometry/vpTriangle.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTriangle.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a 2D triangle.
- *
- * Author:
- * Amaury Dame
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpTriangle.h>
-#include <visp/vpDebug.h>
-
-/*!
- Basic constructor.
-
- By default, the three 2D points coordinates which define the triangle are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
-*/
-vpTriangle::vpTriangle()
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
-{
- init (vpImagePoint(0,0),vpImagePoint(1,0),vpImagePoint(0,1));
-}
-
-/*!
- Constructor which initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
-
- \param iP1 : The first apex of the triangle.
- \param iP2 : The first apex of the triangle.
- \param iP3 : The first apex of the triangle.
-*/
-vpTriangle::vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
-{
- init(iP1,iP2,iP3);
-}
-
-/*!
- Copy constructor
-
- \param tri : The triangle used for the initialisation.
-*/
-vpTriangle::vpTriangle(const vpTriangle &tri)
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
-{
- *this = tri;
-}
-
-/*!
- Basic destructor
-*/
-vpTriangle::~vpTriangle()
-{
-}
-
-/*!
- Assign \e tri to this triangle and return a reference to this triangle.
-*/
-vpTriangle &
-vpTriangle::operator=(const vpTriangle& tri)
-{
- goodTriange = tri.goodTriange;
- S1 = tri.S1;
- uvinv00 = tri.uvinv00;
- uvinv01 = tri.uvinv01;
- uvinv10 = tri.uvinv10;
- uvinv11 = tri.uvinv11;
- ptempo0 = tri.ptempo0;
- ptempo1 = tri.ptempo1;
- area = tri.area;
- return *this;
-};
-
-/*!
- Initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
-
- \param iP1 : The first apex of the triangle.
- \param iP2 : The first apex of the triangle.
- \param iP3 : The first apex of the triangle.
-*/
-void
-vpTriangle::buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
-{
- init(iP1,iP2,iP3);
-}
-
-
-void
-vpTriangle::init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
-{
- ptempo0 = ptempo1 = 0.;
- apex1 = iP1;
- apex2 = iP2;
- apex3 = iP3;
-
- vpMatrix uv(2,2);
- vpMatrix uvinv(2,2);
-
- uv[0][0] = iP2.get_i() - iP1.get_i();
- uv[1][0] = iP3.get_i() - iP1.get_i();
- uv[0][1] = iP2.get_j() - iP1.get_j();
- uv[1][1] = iP3.get_j() - iP1.get_j();
- try
- {
- uvinv=uv.inverseByLU();
- goodTriange = true;
- }
- catch(...)
- {
- goodTriange = false;
- std::cout<<"Empty triangle"<<std::endl;
- }
-
- uvinv00=uvinv[0][0];
- uvinv01=uvinv[0][1];
- uvinv10=uvinv[1][0];
- uvinv11=uvinv[1][1];
- S1 = iP1;
- area = 0.5 * fabs(uv.det());
-}
-
-
-/*!
- Check if the 2D point \f$ iP \f$ is inside the triangle.
-
- \param iP : The point which coulb be inside the triangle.
- \param threshold : A threshold used to define the accuracy of the computation when the point is very near from the edges of the triangle. 0 is the smallest value.
-
- \return Returns true if the point is inside the triangle. Returns false otherwise.
-*/
-bool
-vpTriangle::inTriangle(const vpImagePoint &iP, double threshold)
-{
- if(!goodTriange)
- return false;
-
- if(threshold < 0)
- threshold = 0;
-
- ptempo0 = iP.get_i() - S1.get_i();
- ptempo1 = iP.get_j() - S1.get_j();
-
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
-
- return (p_ds_uv0+p_ds_uv1<1.+threshold && p_ds_uv0>-threshold && p_ds_uv1>-threshold);
-}
-
diff --git a/src/tools/geometry/vpTriangle.h b/src/tools/geometry/vpTriangle.h
deleted file mode 100644
index 98f1aa9..0000000
--- a/src/tools/geometry/vpTriangle.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTriangle.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Defines a 2D triangle.
- *
- * Author:
- * Amaury Dame
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef vpTriangle_h
-#define vpTriangle_h
-
-/*!
- \class vpTriangle
- \brief Defines a 2D triangle.
-
- A triangle is internally represented by three 2D points.
-
- By default the three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint class documentation for more details about the frame.) are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
-*/
-
-#include <visp/vpImagePoint.h>
-#include <visp/vpMatrix.h>
-
-class VISP_EXPORT vpTriangle
-{
- private:
- bool goodTriange;
- vpImagePoint S1;
- double uvinv00;
- double uvinv01;
- double uvinv10;
- double uvinv11;
- double ptempo0;
- double ptempo1;
- double area;
- vpImagePoint apex1;
- vpImagePoint apex2;
- vpImagePoint apex3;
-
- public:
- vpTriangle();
- vpTriangle(const vpTriangle &tri);
- virtual ~vpTriangle();
-
- vpTriangle &operator=(const vpTriangle& tri);
-
- vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
- void buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
- bool inTriangle(const vpImagePoint &iP, double threshold = 0.00001);
-
- /*!
- Get the apexes of the triangle.
-
- \param iP1 : first apex.
- \param iP2 : second apex.
- \param iP3 : third apex.
- */
- void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, vpImagePoint &iP3) const {
- iP1 = apex1;
- iP2 = apex2;
- iP3 = apex3;}
-
-
- /*!
- Return the area of the triangle.
- The area is computed when the triangle is built from three image points.
-
- \return The area of the triangle.
- */
- inline double getArea() const{
- return this->area;
- }
-
- private:
- void init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
-};
-
-#endif
-
diff --git a/src/tools/histogram/vpHistogram.cpp b/src/tools/histogram/vpHistogram.cpp
deleted file mode 100644
index b45b37a..0000000
--- a/src/tools/histogram/vpHistogram.cpp
+++ /dev/null
@@ -1,1126 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogram.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpHistogram.cpp
- \brief Definition of the vpHistogram class member functions.
-
- Class vpHistogram allows to calculate and manipulate gray level
- image histograms.
-
-*/
-
-#include <visp/vpHistogram.h>
-#include <stdlib.h>
-
-bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second);
-
-// comparison,
-bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second)
-{
- if (first.getValue() > second.getValue()) return true;
- else return false;
-}
-
-
-
-/*!
- Defaut constructor for a gray level histogram.
-*/
-vpHistogram::vpHistogram() : histogram(NULL), size(256)
-{
- init();
-}
-
-/*!
- Copy constructor of a gray level histogram.
-*/
-vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256)
-{
- init(h.size);
- memcpy(histogram, h.histogram, size * sizeof(unsigned));
-}
-
-/*!
- Calculates the histrogram from a gray level image.
-
- \param I : Gray level image.
-
- \sa calculate()
-*/
-vpHistogram::vpHistogram(const vpImage<unsigned char> &I)
- : histogram(NULL), size(256)
-{
- init();
-
- calculate(I);
-}
-
-/*!
- Destructor.
-*/
-vpHistogram::~vpHistogram()
-{
- if (histogram != NULL) {
- // vpTRACE("free: %p", &histogram);
- delete [] histogram;
- histogram = NULL;
- size = 0;
- }
-}
-
-/*!
-
- Copy operator.
- \param h : Histogram to copy.
-
- \code
- vpImage<unsigned char> I;
-
- vpHistogram h(I);
- vpHistogram hcopied = h;
-
- \endcode
-*/
-vpHistogram &
-vpHistogram::operator=(const vpHistogram &h)
-{
- init(h.size);
- memcpy(histogram, h.histogram, size * sizeof(unsigned));
-
- return *this;
-}
-
-/*!
- Initialise the histogram structure.
- - Allocates the array
- - Initialise all the values to zero.
-*/
-void
-vpHistogram::init(unsigned size_)
-{
- if (histogram != NULL) {
- delete [] histogram;
- histogram = NULL;
- }
-
- this->size = size_;
- histogram = new unsigned [size];
-
- memset(histogram, 0, size * sizeof(unsigned));
-
- // vpTRACE("alloc: %p", &histogram);
-}
-
-
-/*!
-
- Calculate the histogram from a gray level image.
-
- \param I : Gray level image.
-
-*/
-void vpHistogram::calculate(const vpImage<unsigned char> &I)
-{
- memset(histogram, 0, size * sizeof(unsigned));
-
- for (unsigned i=0; i < I.getHeight(); i ++) {
- for (unsigned j=0; j < I.getWidth(); j ++) {
- histogram[ I[i][j] ] ++;
- }
- }
-}
-
-/*!
-
- Smoothes the histogram and returns it.
-
- A simple average scheme is used where each value \f$h(i)\f$ in the
- histogram is replaced by the average of itself and the neighbours.
-
- \f[h(i) = \sum_{j=i-\frac{fsize}{2}}^{i+\frac{fsize}{2}} h(j) \f]
-
- \param fsize : Filter size. Corresponds to the number of values
- around each point used to compute the mean value.
-
- \return Smoothed histogram.
-
- \exception vpImageException::notInitializedError : Histogram array
- not initialsed. Means that the histogram was not calculated before.
-
- \sa calculate()
-
-*/
-void
-vpHistogram::smooth(const unsigned int fsize)
-{
- if (histogram == NULL) {
- vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
- }
-
- vpHistogram h;
- h = *this;
-
- int hsize = (int)fsize / 2; // half filter size
- unsigned int sum;
- unsigned int nb;
-
- for (unsigned i=0; i < size; i ++) {
- sum = 0;
- nb = 0;
- for (int j=-hsize; j <= hsize; j ++) {
- // exploitation of the overflow to detect negative value...
- if ( /*(i + j) >= 0 &&*/ (i + (unsigned int)j) < size ) {
- sum += h.histogram[i + (unsigned int)j];
- nb ++;
- }
- }
- histogram[i] = sum / nb;
- }
-
-}
-
-/*!
-
- Build a list of all histogram peaks. This peak list is gray level
- sorted from 0 to 255. That mean that the first peak has a gray level
- less than the second one, etc.
-
- To sort this list from highest peak value to the lowest one, you can
- use sort().
-
- \param peaks : List of peaks.
- \return The number of peaks in the histogram.
-
- \sa sort()
-*/
-unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & peaks)
-{
- if (histogram == NULL) {
- vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
- }
-
- int prev_slope; // Previous histogram inclination
- int next_slope; // Next histogram inclination
- vpHistogramPeak p; // An histogram peak
- unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
-
- peaks.clear();
-
- // Parse the histogram to get the local maxima
- unsigned cpt = 0;
- unsigned sum_level = 0;
- nbpeaks = 0;
- prev_slope = 1;
-
- for (unsigned i = 0; i < size-1; i++) {
- next_slope = (int)histogram[i+1] - (int)histogram[i];
-
-// if ((prev_slope < 0) && (next_slope > 0) ) {
-// sum_level += i;
-// cpt ++;
-// continue;
-// }
-
- if ((prev_slope > 0) && (next_slope == 0) ) {
- sum_level += i + 1;
- cpt ++;
- continue;
- }
-
-
- // Peak detection
- if ( (prev_slope > 0) && (next_slope < 0) ) {
- sum_level += i;
- cpt ++;
-
- unsigned int level = sum_level / cpt;
- p.set((unsigned char)level, histogram[level]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- peaks.push_back(p);
-
- nbpeaks ++;
-
- }
-
- prev_slope = next_slope;
- sum_level = 0;
- cpt = 0;
- }
- if (prev_slope > 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- peaks.push_back(p);
- nbpeaks ++;
- }
-
- return nbpeaks;
-}
-
-/*!
-
- Find the two highest peaks in the histogram. Usually, the first
- one correspond to the image background, the second one to
- the object.
-
- \param dist : Minimal distance between two significative histogram peaks.
-
- \param peak1 : Highest peak in the histogram.
- \param peak2 : Second highest peak in the histogram.
-
- \return The number of highest peaks:
- - 2: If the histogram is bimodal
- - 1: If no second highest peak was found.
- - 0: if no peaks were found.
-
-*/
-unsigned vpHistogram::getPeaks(unsigned char dist,
- vpHistogramPeak & peak1,
- vpHistogramPeak & peak2)
-{
- std::list<vpHistogramPeak> peaks;
- unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
-
- nbpeaks = getPeaks(peaks);
- sort(peaks);
-
- if (nbpeaks == 0) {
- peak1.set(0, 0);
- peak2.set(0, 0);
- return 0;
- }
-
- if (nbpeaks == 1) {
- peak1 = peaks.front();
- peak2.set(0, 0);
- return 1;
- }
-
- // Parse the peaks list to get the peak with a distance greater
- // than dist to the highest
- peak1 = peaks.front();
- for(std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++ it)
- {
- vpHistogramPeak p = *it;
- if (abs(p.getLevel() - peak1.getLevel()) > dist) {
- // The second peak is found
- peak2 = p;
- return 2;
- }
- }
-
- // No second peak found
- peak2.set(0, 0);
- return 1;
-}
-
-
-/*!
-
- Determine the two highest peaks in the histogram and compute a
- threshold to separate the two objects. Here we dont know which is the
- highest peak. It could be \e peakl or \e peakr.
-
- \param dist : Distance between two significative histogram maxima
- \param peakl : Position of the left histogram peak.
- \param peakr : Position of the right histogram peak.
-
- \param valey : Valey between the two peaks \e peakl and \e peakr.
-
- \return true if the histogram is bimodal, false otherwise.
-*/
-
-bool
-vpHistogram::getPeaks(unsigned char dist,
- vpHistogramPeak & peakl,
- vpHistogramPeak & peakr,
- vpHistogramValey & valey)
-{
- unsigned char *peak; // Local maxima values
- int prev_slope; // Previous histogram inclination
- int next_slope; // Next histogram inclination
- unsigned index_highest_peak; // Index in peak[] array of the highest peak
- unsigned index_second_peak; // Index in peak[] array of the second peak
-
- unsigned int prof;
- unsigned int maxprof; // Nb pixels difference between 2 maxi peaks
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
- unsigned int nbpeaks; // Number of peaks in the histogram (ie local maxima)
-
- // Init the valey
- valey.set(0, 0);
-
- // Allocation for the
- peak = new unsigned char [size];
-
- // Parse the histogram to get the local maxima
- nbpeaks = 0;
- prev_slope = 1;
- for (unsigned i = 0; i < size-1; i++) {
- next_slope = (int)histogram[i+1] - (int)histogram[i];
- if (next_slope == 0)
- continue;
- // Peak detection
- if ( (prev_slope > 0) && (next_slope < 0) )
- peak[nbpeaks ++] = (unsigned char)i;
-
- prev_slope = next_slope;
- }
- 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++) {
- if (histogram[ peak[i] ] > histogram[ peak[index_highest_peak] ]) {
- index_highest_peak = i;
- }
- }
-
-// 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;
-
- // Search second local maximum on the left of the global maximum
- for (unsigned i = 0; i < index_highest_peak; i++) {
- if (peak[index_highest_peak] - peak[i] > dist) {
- prof=0;
- for (int j=peak[i]; j <= peak[index_highest_peak]-dist; j++)
- if((histogram[peak[i]] - histogram[j]) > prof)
- prof = histogram[peak[i]] - histogram[j];
-
- if (prof > maxprof) {
- maxprof = prof;
- index_second_peak = i;
- }
- }
- }
-
- // Search second local maximum on the right of the global maximum
- for (unsigned i = index_highest_peak+1; i < nbpeaks; i++) {
- if (peak[i] - peak[index_highest_peak] > dist) {
- prof=0;
- for (int j=peak[index_highest_peak]+dist; j <= peak[i]; j++)
- if((histogram[peak[i]] - histogram[j]) > prof)
- prof = histogram[peak[i]] - histogram[j];
-
- if (prof > maxprof) {
- maxprof = prof;
- index_second_peak = i;
- }
- }
- }
-// 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
- {
- peakl.set(peak[index_second_peak], histogram[ peak[index_second_peak] ]);
- peakr.set(peak[index_highest_peak], histogram[ peak[index_highest_peak]]);
- }
-
- if (peakl == peakr) {
-
- delete [] peak;
-
- return (false); // Not a bimodal histogram
- }
-
- // Search the valey
- mini = peakl.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
- continue;
- }
- if (histogram[i] == mini) {
- sumindmini += i;
- nbmini ++;
- }
- }
- // vpTRACE("nbmini %d", nbmini);
- // vpTRACE("sumindmini %d", sumindmini);
- // vpTRACE("mini: indmini: %d", sumindmini/nbmini);
-
- if (nbmini == 0) {
- // no valey found
- valey.set(0, 0);
-
- delete [] peak;
-
- return false;
- }
- else {
- mini = sumindmini/nbmini; // mean
- valey.set((unsigned char)mini, histogram[mini]);
-
- delete [] peak;
-
- return (true);
- }
-}
-
-/*!
-
- Build a list of all histogram valey. This valey list is gray level
- sorted from 0 to 255. That mean that the first valey has a gray level
- less than the second one, etc.
-
- \param valey : List of valey.
- \return The number of valey in the histogram.
-
- \sa sort()
-*/
-unsigned vpHistogram::getValey(std::list<vpHistogramValey> & valey)
-{
- if (histogram == NULL) {
- vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
- }
-
- int prev_slope; // Previous histogram inclination
- int next_slope; // Next histogram inclination
- vpHistogramValey p; // An histogram valey
- unsigned nbvaley; // Number of valey in the histogram (ie local minima)
-
- valey.clear();
-
- // Parse the histogram to get the local minima
- unsigned cpt = 0;
- unsigned sum_level = 0;
- nbvaley = 0;
- prev_slope = -1;
-
- for (unsigned i = 0; i < size-1; i++) {
- next_slope = (int)histogram[i+1] - (int)histogram[i];
-
- if ((prev_slope < 0) && (next_slope == 0) ) {
- sum_level += i + 1;
- cpt ++;
- continue;
- }
-
- // Valey detection
- if ( (prev_slope < 0) && (next_slope > 0) ) {
- sum_level += i;
- cpt ++;
-
- unsigned int level = sum_level / cpt;
- p.set((unsigned char)level, histogram[level]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- valey.push_back(p);
-
- nbvaley ++;
-
- }
-
- prev_slope = next_slope;
- sum_level = 0;
- cpt = 0;
- }
- if (prev_slope < 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- valey.push_back(p);
- nbvaley ++;
- }
-
- return nbvaley;
-}
-
-/*!
-
- Find the valey between two peaks. It starts at the lowest peak and
- works its way up to the highest peak. Along the way, it looks at
- each point in the histogram to find the location of the smallest
- histogram point which is the valey point.
-
- \param peak1 : A peak in the histogram.
- \param peak2 : A other peak in the histogram.
-
- \param valey : Low point of the valey between two peaks in a histogram.
-
- \return true if a valey was found, false otherwise.
-*/
-bool
-vpHistogram::getValey(const vpHistogramPeak & peak1,
- const vpHistogramPeak & peak2,
- vpHistogramValey & valey)
-{
-
- // Set the left and right peaks
- vpHistogramPeak peakl, peakr;
- if (peak1.getLevel() > peak2.getLevel()) {
- peakl = peak2;
- peakr = peak1;
- }
- else {
- peakl = peak1;
- peakr = peak2;
- }
-
- // Search the valey
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
-
- mini = peakl.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
- continue;
- }
- if (histogram[i] == mini) {
- sumindmini += i;
- nbmini ++;
- }
- }
-
- if (nbmini == 0) {
- // no valey found
- valey.set(0, 0);
-
- return false;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
-
- valey.set((unsigned char)minipos, histogram[minipos]);
- return true;
- }
-}
-/*!
-
- Find the two valey around an histogram peak. It starts at the peak
- position and works its way down and up to find the left and right
- valey around the peak.
-
-
- \param dist : Minimal distance between two significative histogram peaks.
- \param peak : A peak in the histogram.
- \param valeyl : The valey on the left of the peak.
- \param valeyr : The valey on the right of the peak.
-
- \return 0x00 : If no valey was found.
- \return 0x01 : If only the right valey was found.
- \return 0x10 : If only the left valey was found.
- \return 0x11 : If two valeys around the peak were found.
-
-*/
-unsigned
-vpHistogram::getValey(unsigned char dist,
- const vpHistogramPeak & peak,
- vpHistogramValey & valeyl,
- vpHistogramValey & valeyr)
-{
- unsigned int ret = 0x11;
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
- vpHistogramPeak peakr, peakl; // Left and right peaks of peak
- std::list<vpHistogramPeak> peaks; // list of histogram peaks
- // unsigned int nbpeaks=0; // Number of peaks in the histogram (ie local maxima)
-
- if (peak.getLevel() == 0) {
- valeyl.set(0, 0);
- ret &= 0x01;
- }
- if (peak.getLevel() == size -1) {
- valeyr.set((unsigned char)(size-1), 0);
- ret &= 0x10;
- }
-
- if (ret >> 1) // consider the left part of the requested peak
- {
- // If the list of peaks is empty, compute it
- if (peaks.empty())
- {
- /* 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<vpHistogramPeak>::const_iterator it;
- unsigned index = 0;
- for (it = peaks.begin(); it != peaks.end(); ++ it)
- {
- if (peak == *it) {
- // we are on the peak.
- break;
- }
- index ++;
- }
-
- bool found = false;
- if (index == 0) {
- // 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
- std::list<vpHistogramPeak>::const_iterator lit; // left iterator
- for(lit = peaks.begin(); lit != it; ++ lit)
- {
- if (abs((*lit).getLevel() - peak.getLevel()) > dist) {
- // peakl found
- peakl = *lit;
- found = true; // we cannot stop here, since the other peaks on the right may exist
- }
- }
- }
- if ( ! found)
- peakl.set(0,0);
-
- // Search the valey on the left
- mini = peak.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i < peak.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
- continue;
- }
- if (histogram[i] == mini) {
- sumindmini += i;
- nbmini ++;
- }
- }
- if (nbmini == 0) {
- valeyl.set(0, 0);
- ret &= 0x01;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
- valeyl.set((unsigned char)minipos, histogram[minipos]);
- ret &= 0x11;
- }
- }
-
- if (ret << 1) {
- // If the list of peaks is empty, compute it
- if (peaks.empty()) {
- /* nbpeaks = */ getPeaks(peaks);
- }
- // Go to the requested peak in the list
- std::list<vpHistogramPeak>::const_iterator it;
- unsigned index = 0;
- for (it = peaks.begin(); it != peaks.end(); ++ it)
- {
- if (peak == *it) {
- // we are on the peak.
- break;
- }
- index ++;
- }
-
- bool found = false;
- // search for the nearest peak on the right that matches the distance
- std::list<vpHistogramPeak>::const_iterator rit; // right iterator
- for(rit = it; rit != peaks.end(); ++ rit)
-
- if (abs((*rit).getLevel() - peak.getLevel()) > dist) {
- // peakr found
- peakr = *rit;
- found = true;
- break; // we can stop here
- }
-
- if ( ! found)
- peakr.set((unsigned char)(size-1),0);
-
- // Search the valey on the right
- mini = peak.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=(unsigned int)peak.getLevel()+1; i <= (unsigned int)peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
- continue;
- }
- if (histogram[i] == mini) {
- sumindmini += i;
- nbmini ++;
- }
- }
- if (nbmini == 0) {
- valeyr.set((unsigned char)(size-1), 0);
- ret &= 0x10;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
- valeyr.set((unsigned char)minipos, histogram[minipos]);
- ret &= 0x11;
- }
- }
-
- return ret;
-}
-
-/*!
- Sort a list of histogram peaks from highest to the lowest.
-
- \param peaks : List of histogram peaks.
- \return The number of peaks in the histogram.
-
- \sa getPeak()
-*/
-unsigned vpHistogram::sort(std::list<vpHistogramPeak> & peaks)
-{
-
- if ( peaks.empty() ) {
- return 0;
- }
-
- peaks.sort(compare_vpHistogramPeak);
-
- return (unsigned int)peaks.size();
-}
-
-/*!
-
- Write the histogram values in a file.
-
- In the file, on each line you will find first the gray level and
- than the number of pixels which have this gray level.
-
- \param filename : File name to write with histogram values.
-
- \return true if the file could be written, false otherwise.
-*/
-
-bool
-vpHistogram::write(const std::string &filename)
-{
- return ( this->write(filename.c_str()) );
-}
-
-/*!
-
- Write the histogram values in a file.
-
- In the file, on each line you will find first the gray level and
- than the number of pixels which have this gray level.
-
- \param filename : File name to write with histogram values.
-
- \return true if the file could be written, false otherwise.
-*/
-
-bool
-vpHistogram::write(const char *filename)
-{
- std::string opath;
-
- FILE *fd = fopen(filename, "w");
- if (fd == NULL)
- return false;
- for (unsigned i=0; i < size; i ++)
- fprintf(fd, "%d %d\n", i, histogram[i]);
- fclose(fd);
-
- return true;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
-
- \deprecated This method is deprecated. You should use getPeaks(std::list<vpHistogramPeak> &) instead.
-
- Build a list of all histogram peaks. This peak list is gray level
- sorted from 0 to 255. That mean that the first peak has a gray level
- less than the second one, etc.
-
- To sort this list from highest peak value to the lowest one, you can
- use sort().
-
- \param peaks : List of peaks.
- \return The number of peaks in the histogram.
-
- \sa sort()
-*/
-unsigned vpHistogram::getPeaks(vpList<vpHistogramPeak> & peaks)
-{
- if (histogram == NULL) {
- vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
- }
-
- int prev_slope; // Previous histogram inclination
- int next_slope; // Next histogram inclination
- vpHistogramPeak p; // An histogram peak
- unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
-
- if ( ! peaks.empty() )
- peaks.kill();
-
- peaks.front();
-
- // Parse the histogram to get the local maxima
- unsigned cpt = 0;
- unsigned sum_level = 0;
- nbpeaks = 0;
- prev_slope = 1;
-
- for (unsigned i = 0; i < size-1; i++) {
- next_slope = (int)histogram[i+1] - (int)histogram[i];
-
-// if ((prev_slope < 0) && (next_slope > 0) ) {
-// sum_level += i;
-// cpt ++;
-// continue;
-// }
-
- if ((prev_slope > 0) && (next_slope == 0) ) {
- sum_level += i + 1;
- cpt ++;
- continue;
- }
-
-
- // Peak detection
- if ( (prev_slope > 0) && (next_slope < 0) ) {
- sum_level += i;
- cpt ++;
-
- unsigned int level = sum_level / cpt;
- p.set((unsigned char)level, histogram[level]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- peaks.addRight(p);
-
- nbpeaks ++;
-
- }
-
- prev_slope = next_slope;
- sum_level = 0;
- cpt = 0;
- }
- if (prev_slope > 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- peaks.addRight(p);
- nbpeaks ++;
- }
-
- return nbpeaks;
-}
-
-/*!
-
- \deprecated This method is deprecated. You should use getValey(std::list<vpHistogramValey> &) instead.
-
- Build a list of all histogram valey. This valey list is gray level
- sorted from 0 to 255. That mean that the first valey has a gray level
- less than the second one, etc.
-
- \param valey : List of valey.
- \return The number of valey in the histogram.
-
- \sa sort()
-*/
-unsigned vpHistogram::getValey(vpList<vpHistogramValey> & valey)
-{
- if (histogram == NULL) {
- vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
- }
-
- int prev_slope; // Previous histogram inclination
- int next_slope; // Next histogram inclination
- vpHistogramValey p; // An histogram valey
- unsigned nbvaley; // Number of valey in the histogram (ie local minima)
-
- if ( ! valey.empty() )
- valey.kill();
-
- valey.front();
-
- // Parse the histogram to get the local minima
- unsigned cpt = 0;
- unsigned sum_level = 0;
- nbvaley = 0;
- prev_slope = -1;
-
- for (unsigned i = 0; i < size-1; i++) {
- next_slope = (int)histogram[i+1] - (int)histogram[i];
-
- if ((prev_slope < 0) && (next_slope == 0) ) {
- sum_level += i + 1;
- cpt ++;
- continue;
- }
-
- // Valey detection
- if ( (prev_slope < 0) && (next_slope > 0) ) {
- sum_level += i;
- cpt ++;
-
- unsigned int level = sum_level / cpt;
- p.set((unsigned char)level, histogram[level]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- valey.addRight(p);
-
- nbvaley ++;
-
- }
-
- prev_slope = next_slope;
- sum_level = 0;
- cpt = 0;
- }
- if (prev_slope < 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
- // vpTRACE("add %d %d", p.getLevel(), p.getValue());
- valey.addRight(p);
- nbvaley ++;
- }
-
- return nbvaley;
-}
-
-/*!
- \deprecated This method is deprecated. You should use sort(std::list<vpHistogramPeak> &) instead.
-
- Sort a list of histogram peaks from highest to the lowest.
-
- \param peaks : List of histogram peaks.
- \return The number of peaks in the histogram.
-
- \sa getPeak()
-*/
-unsigned vpHistogram::sort(vpList<vpHistogramPeak> & peaks)
-{
-
- vpList<vpHistogramPeak> _peaks;
- vpHistogramPeak p, _p; // A peak
- unsigned nbpeaks = 0;
-
- if ( peaks.empty() ) {
- nbpeaks = 0;
- return nbpeaks;
- }
-
- _peaks = peaks;
- peaks.kill(); // erase the output list
- // Sort the list of peaks from highest to the lowest
- peaks.front();
- _p = _peaks.value();
- // Add first value in the list
- peaks.addRight(_p);
- nbpeaks ++;
- // parse the rest of the list
- _peaks.next();
- while (! _peaks.outside() ) {
- _p = _peaks.value();
- peaks.front();
- while (! peaks.outside() ) {
- p = peaks.value();
- if (_p.getValue() > p.getValue()) {
- peaks.addLeft(_p);
- nbpeaks ++;
- break;
- }
- if( peaks.nextOutside()) {
- peaks.addRight(_p);
- nbpeaks ++;
- break;
- }
-
- peaks.next();
- }
-
- _peaks.next();
- }
- return nbpeaks;
-}
-
-#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tools/histogram/vpHistogram.h b/src/tools/histogram/vpHistogram.h
deleted file mode 100644
index db90687..0000000
--- a/src/tools/histogram/vpHistogram.h
+++ /dev/null
@@ -1,307 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogram.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpHistogram.h
- \brief Declaration of the vpHistogram class.
- Class vpHistogram defines gray level image histograms
-
-*/
-
-#ifndef vpHistogram_h
-#define vpHistogram_h
-
-#include <visp/vpImage.h>
-#include <visp/vpHistogramPeak.h>
-#include <visp/vpHistogramValey.h>
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp/vpList.h>
-#endif
-
-#include <list>
-
-/*!
- \class vpHistogram
- \ingroup Histogram
- \brief Class to compute a gray level image histogram.
-
-
- Here are two examples showing how to use this class to determine the
- threshold which can be used to segment two objects.
-
- The code below:
-
- \code
- vpImage<unsigned char> I;
- ...
- unsigned char dist = 60;
- vpHistogramValey valey;
- vpHistogram h(I);
- h.smooth(); // Filter the histogram values
- vpHistogramPeak peakl, peakr; // Two highest peaks in the histogram
- // - peakl: Peak on the left
- // - peakr: Peak on the right
-
- h.getPeaks(dist, peakl, peakr, valey);
- unsigned char threshold; // Position of the valey between the two peaks
- threshold = valey.getLevel();
- \endcode
-
- has the same behaviour than this one:
-
- \code
- vpImage<unsigned char> I;
- ...
- unsigned char dist = 60;
- vpHistogram h(I);
- h.smooth(); // Filter the histogram values
- vpHistogramPeak peak1, peak2; // Two highest peaks in the histogram
- // - peak1: Highest peak
- // - peakr: Second highest peak
-
- // Get the two highest peaks
- h.getPeaks(dist, peak1, peak2);
-
- // Get the valey between the two highest peaks
- vpHistogramValey valey;
- h.getValey(peak1, peak2, valey);
-
- unsigned char threshold; // Position of the valey between the two peaks
- threshold = valey.getLevel();
- \endcode
-
-*/
-class VISP_EXPORT vpHistogram
-{
-public:
- vpHistogram();
- vpHistogram(const vpHistogram &h);
- vpHistogram(const vpImage<unsigned char> &I);
- virtual ~vpHistogram();
-
- vpHistogram & operator=(const vpHistogram &h);
-
- /*!
-
- Return the number of pixels having the gray \e level.
-
- \param level : Gray level in the histogram.
-
- \return Number of pixels having the gray level.
-
- \code
- vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
- h.calculate(I); // Histogram of the gray level image
-
- // Print the histogram values
- for (int i=0; i < h.getSize(); i ++)
- printf("%d: %d\n", i, h[i]);
- \endcode
-
- */
- inline unsigned operator[](const unsigned char level) const
- {
- return histogram[level];
-
- };
- /*!
-
- Return the number of pixels having the gray \e level.
-
- \param level : Gray level in the histogram.
-
- \return Number of pixels having the gray level.
-
- \code
- vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
- h.calculate(I); // Histogram of the gray level image
-
- // Print the histogram values
- for (int i=0; i < h.getSize(); i ++)
- printf("%d: %d\n", i, h(i));
- \endcode
-
- */
- inline unsigned operator()(const unsigned char level) const
- {
- return histogram[level];
- };
- /*!
-
- Return the number of pixels having the gray \e level.
-
- \param level : Gray level in the histogram.
-
- \return Number of pixels having the gray level.
-
- \code
- vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
- h.calculate(I); // Histogram of the gray level image
-
- // Print the histogram values
- for (int i=0; i < h.getSize(); i ++)
- printf("%d: %d\n", i, h.get(i));
- \endcode
-
- */
- inline unsigned get(const unsigned char level) const
- {
- return histogram[level];
- };
-
- /*!
-
- Set the number of pixels having the gray \e level.
-
- \param level : Gray level in the histogram. Level is in [0:255]
-
- \param value : Number of pixels having the gray level.
-
- \code
- vpHistogram h;
-
- // Set histogram values
- for (int i=0; i < h.getSize(); i ++)
- h.set(i, i*2); // for each level i, set a value of 2*i
- \endcode
-
- */
- inline void set(const unsigned char level, unsigned int value)
- {
- histogram[level] = value;
- };
-
- void calculate(const vpImage<unsigned char> &I);
- void smooth(const unsigned int fsize = 3);
- unsigned getPeaks(std::list<vpHistogramPeak> & peaks);
- unsigned getPeaks(unsigned char dist,
- vpHistogramPeak & peak1,
- vpHistogramPeak & peak2);
- bool getPeaks(unsigned char dist,
- vpHistogramPeak & peakl,
- vpHistogramPeak & peakr,
- vpHistogramValey & valey);
- unsigned getValey(std::list<vpHistogramValey> & valey);
- bool getValey(const vpHistogramPeak & peak1,
- const vpHistogramPeak & peak2,
- vpHistogramValey & valey);
- unsigned getValey(unsigned char dist,
- const vpHistogramPeak & peak,
- vpHistogramValey & valeyl,
- vpHistogramValey & valeyr);
- unsigned sort(std::list<vpHistogramPeak> & peaks);
-
- bool write(const std::string &filename);
- bool write(const char *filename);
-
- /*!
- Get the histogram size.
-
- \return The size of the histogram, or the image maximum gray
- levels numbers.
-
- \sa getValues()
- */
- inline unsigned getSize()
- {
- return size;
- };
-
- /*!
-
- Get the histogram values.
-
- \return A pointer to the array of histogram values. The size of
- this array is given by getSize().
-
- \code
- vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
- h.calculate(I); // Histogram of the gray level image
-
- // Print the histogram values
- unsigned char *values = h.getValues();
- for (int i=0; i < h.getSize(); i ++)
- printf("%d: %d\n", i, values[i]);
- \endcode
-
- \sa getSize()
- */
- inline unsigned * getValues()
- {
- return histogram;
- };
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated unsigned getPeaks(vpList<vpHistogramPeak> & peaks);
- vp_deprecated unsigned getValey(vpList<vpHistogramValey> & valey);
- vp_deprecated unsigned sort(vpList<vpHistogramPeak> & peaks);
-#endif
-
-private:
- void init(unsigned size = 256);
-
- unsigned int *histogram;
- unsigned size; // Histogram size (max allowed 256)
-};
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tools/histogram/vpHistogramPeak.cpp b/src/tools/histogram/vpHistogramPeak.cpp
deleted file mode 100644
index e7d2226..0000000
--- a/src/tools/histogram/vpHistogramPeak.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogramPeak.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpHistogramPeak.cpp
- \brief Declaration of the vpHistogramPeak class.
- Class vpHistogramPeak defines a gray level histogram peak.
-
-*/
-
-#include <visp/vpHistogramPeak.h>
-
-
-/*!
- Defaut constructor for a gray level histogram peak.
-*/
-vpHistogramPeak::vpHistogramPeak() : level(0), value(0) {}
-
-/*!
- Defaut constructor for a gray level histogram peak.
-*/
-vpHistogramPeak::vpHistogramPeak(unsigned char lvl, unsigned val)
- : level(lvl), value(val) {}
-
-/*!
- Copy constructor of a gray level histogram peak.
-*/
-vpHistogramPeak::vpHistogramPeak(const vpHistogramPeak &p)
- : level(0), value(0)
-{
- *this = p;
-}
-
-/*!
-
- Copy operator.
- \param p : Histogram peak to copy.
-
- \code
- vpHistogramPeak p1(0, 255);
- vpHistogramPeak p2 = p1; // Peak p2 is set to 0, 255
- \endcode
-*/
-vpHistogramPeak &
-vpHistogramPeak::operator=(const vpHistogramPeak &p)
-{
- setLevel(p.level);
- setValue(p.value);
-
- return *this;
-}
-
-/*!
-
- Comparison operator.
-
- \param p : Gray level histogram peak to compar.
-
-*/
-bool
-vpHistogramPeak::operator==(const vpHistogramPeak &p) const
-{
- return ( (level == p.level) && (value == p.value) );
-}
-
-/*!
- \brief std::cout a peak
-*/
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHistogramPeak &p)
-{
-
- s << (int)p.getLevel() << " " << p.getValue();
-
- return s;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tools/histogram/vpHistogramPeak.h b/src/tools/histogram/vpHistogramPeak.h
deleted file mode 100644
index 3b1e0ea..0000000
--- a/src/tools/histogram/vpHistogramPeak.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogramPeak.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpHistogramPeak.h
- \brief Declaration of the vpHistogramPeak class.
- Class vpHistogramPeak defines a gray level histogram peak.
-
-*/
-
-#ifndef vpHistogramPeak_h
-#define vpHistogramPeak_h
-
-#include <visp/vpConfig.h>
-
-#include <ostream>
-
-/*!
- \class vpHistogramPeak
-
- \ingroup Histogram
- \brief Declaration of the peak (maximum value) in a gray level image
- histogram.
-
- A peak is internally represented as a gray \e level and a \e value.
- The \e value represents the number of pixels having the gray \e level.
-
-*/
-
-class VISP_EXPORT vpHistogramPeak
-{
-public :
- vpHistogramPeak();
- vpHistogramPeak(unsigned char level, unsigned value);
- vpHistogramPeak(const vpHistogramPeak & p);
-
- /*! Destructor that does nothing. */
- virtual ~vpHistogramPeak() {}
-
- vpHistogramPeak & operator=(const vpHistogramPeak &p);
- bool operator==(const vpHistogramPeak &p) const;
-
- /*!
-
- Set the peak gray \e level. To set the number of pixels having this
- gray level use setValue().
-
- \param lvl : Location of the peak or gray \e level.
-
- \sa setValue(), set()
-
- */
- inline void setLevel(unsigned char lvl)
- {
- this->level = lvl;
- };
- /*!
-
- Set the peak number of pixels having a same gray level. To set the
- gray level of this peak use setLevel().
-
- \param val : Number of pixels having the same location or gray level.
-
- \sa setLevel(), set()
-
- */
- inline void setValue(unsigned val)
- {
- this->value = val;
- };
- /*!
-
- Set the peak gray \e level and number of pixels at this gray level.
-
- \param lvl : Location of the peak or gray level.
- \param val : Number of pixels having the same location or gray level.
-
- \sa setLevel(), setValue()
-
- */
- inline void set(unsigned char lvl, unsigned val)
- {
- this->level = lvl;
- this->value = val;
- };
-
- /*!
-
- Get the peak gray \e level. The number of pixels having this
- gray level is available throw getValue().
-
- \return Location of the peak or gray level.
-
- \sa getValue()
-
- */
- inline unsigned char getLevel() const
- {
- return level;
- };
- /*!
-
- Get the peak number of pixels having a same gray level. The
- corresponding gray level is available throw getLevel().
-
- \return : Number of pixels having the same location or gray level.
-
- \sa getLevel()
-
- */
- inline unsigned getValue() const
- {
- return value;
- };
-
- //---------------------------------
- // Printing
- //---------------------------------
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpHistogramPeak &p);
-
-
-protected :
- unsigned char level; //! Gray level ot the peak.
- unsigned value; //! Number of pixels on the gray level.
-};
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/tools/histogram/vpHistogramValey.cpp b/src/tools/histogram/vpHistogramValey.cpp
deleted file mode 100644
index cfffac7..0000000
--- a/src/tools/histogram/vpHistogramValey.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogramValey.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpHistogramValey.cpp
- \brief Declaration of the vpHistogramValey class.
- Class vpHistogramValey defines a gray level histogram valey.
-
-*/
-
-#include <visp/vpHistogramValey.h>
-
-/*!
-
- Copy operator.
- \param v : Histogram valey to copy.
-
- \code
- vpHistogramValey v1(0, 255);
- vpHistogramValey v2 = v1; // Valey p2 is set to 0, 255
- \endcode
-*/
-vpHistogramValey &
-vpHistogramValey::operator=(const vpHistogramValey &v)
-{
- setLevel(v.level);
- setValue(v.value);
-
- return *this;
-}
-
-/*!
-
- Comparison operator.
-
- \param v : Gray level histogram valey to compar.
-
-*/
-bool
-vpHistogramValey::operator==(const vpHistogramValey &v) const
-{
- return ( (level == v.level) && (value == v.value) );
-}
-
-/*!
- \brief std::cout a valey
-*/
-VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHistogramValey &v)
-{
-
- s << (int)v.getLevel() << " " << v.getValue();
-
- return s;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tools/histogram/vpHistogramValey.h b/src/tools/histogram/vpHistogramValey.h
deleted file mode 100644
index 2c950d1..0000000
--- a/src/tools/histogram/vpHistogramValey.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpHistogramValey.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Gray level histogram manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-/*!
- \file vpHistogramValey.h
- \brief Declaration of the vpHistogramValey class.
- Class vpHistogramValey defines a gray level histogram valey.
-
-*/
-
-#ifndef vpHistogramValey_h
-#define vpHistogramValey_h
-
-#include <visp/vpHistogramPeak.h>
-
-/*!
- \class vpHistogramValey
-
- \ingroup Histogram
- \brief Declaration of the valey (minimum value) in a gray level image
- histogram.
-
- A valey is internally represented as a gray \e level and a \e value.
- The \e value represents the number of pixels having the gray \e level.
-
-*/
-
-class VISP_EXPORT vpHistogramValey : vpHistogramPeak
-{
- public:
- vpHistogramValey() :
- vpHistogramPeak() {};
-
- vpHistogramValey(unsigned char lvl, unsigned val) :
- vpHistogramPeak(lvl, val) {};
-
- vpHistogramValey(const vpHistogramValey & v) :
- vpHistogramPeak(v) {};
-
- /*! Destructor that does nothing. */
- virtual ~vpHistogramValey() {}
-
- vpHistogramValey & operator=(const vpHistogramValey &v);
- bool operator==(const vpHistogramValey &v) const;
-
- /*!
-
- Set the valey gray \e level. To set the number of pixels having this
- gray level use setValue().
-
- \param lvl : Location of the valey or gray level.
-
- \sa setValue(), set()
-
- */
- inline void setLevel(unsigned char lvl)
- {
- this->level = lvl;
- };
- /*!
-
- Set the valey number of pixels having a same gray level. To set the
- gray level of this valey use setLevel().
-
- \param val : Number of pixels having the same location or gray level.
-
- \sa setPosition(), set()
-
- */
- inline void setValue(unsigned val)
- {
- this->value = val;
- };
- /*!
-
- Set the valey gray \e level and number of pixels at this location.
-
- \param lvl : Location of the valey or gray level.
- \param val : Number of pixels having the same location or gray level.
-
- \sa setLevel(), setValue()
-
- */
- inline void set(unsigned char lvl, unsigned val)
- {
- this->level = lvl;
- this->value = val;
- };
-
- /*!
-
- Get the valey gray \e level. The number of pixels having this
- gray level is available through getValue().
-
- \return Location of the valey or gray level.
-
- \sa getValue()
-
- */
- inline unsigned char getLevel() const
- {
- return level;
- };
- /*!
-
- Get the valey number of pixels having a same gray level. The
- corresponding gray level is available through getLevel().
-
- \return Number of pixels having the same location or gray level.
-
- \sa getLevel()
-
- */
- inline unsigned getValue() const
- {
- return value;
- };
-
- //---------------------------------
- // Printing
- //---------------------------------
- friend VISP_EXPORT std::ostream &operator << (std::ostream &s, const vpHistogramValey &v);
-};
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
-#endif
diff --git a/src/tools/io/vpIoException.h b/src/tools/io/vpIoException.h
deleted file mode 100644
index 2c03ed7..0000000
--- a/src/tools/io/vpIoException.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpIoException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpIo class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpIoException_H
-#define __vpIoException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/*!
- \file vpIoException.h
- \brief Error that can be emited by the vpIoTools class and its derivates.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-#include <iostream>
-#include <string>
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpIoException
- \ingroup Exception
- \brief Error that can be emited by the vpIoTools class and its derivates.
- */
-class VISP_EXPORT vpIoException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited 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:
- vpIoException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpIoException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpIoException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/tools/io/vpIoTools.cpp b/src/tools/io/vpIoTools.cpp
deleted file mode 100644
index ae85396..0000000
--- a/src/tools/io/vpIoTools.cpp
+++ /dev/null
@@ -1,1407 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpIoTools.cpp 5214 2015-01-27 18:33:01Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Directory management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpIoTools.cpp
- \brief File and directories basic tools.
-*/
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpIoException.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <fstream>
-#include <string.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <fcntl.h>
-#include <limits>
-#include <cmath>
-#include <algorithm>
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
-#elif defined(_WIN32)
-# include <windows.h>
-# include <direct.h>
-#endif
-#if !defined(_WIN32)
-# include <wordexp.h>
-#endif
-
-std::string vpIoTools::baseName = "";
-std::string vpIoTools::baseDir = "";
-std::string vpIoTools::configFile = "";
-std::vector<std::string> vpIoTools::configVars = std::vector<std::string>();
-std::vector<std::string> vpIoTools::configValues = std::vector<std::string>();
-
-
-/*!
- Get the user name.
-
- - Under unix, get the content of the LOGNAME environment variable. For most
- purposes (especially in conjunction with crontab), it is more useful to use
- the environment variable LOGNAME to find out who the user is, rather than
- the getlogin() function. This is more flexible precisely because the user
- can set LOGNAME arbitrarily.
- - Under windows, uses the GetUserName() function.
-
- \param username : The user name.
-
- \exception vpIoException::cantGetUserName : If this method cannot get the
- user name.
-
- \sa getUserName()
-*/
-void
-vpIoTools::getUserName(std::string &username)
-{
- // With MinGW, UNIX and _WIN32 are defined
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- // Get the user name.
- char *_username = NULL;
- _username = ::getenv("LOGNAME");
- if (_username == NULL) {
- vpERROR_TRACE( "Cannot get the username. Check your LOGNAME environment variable" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
- }
- username = _username;
-#elif defined(_WIN32)
- unsigned int info_buffer_size = 1024;
- TCHAR *infoBuf = new TCHAR [info_buffer_size];
- DWORD bufCharCount = (DWORD) info_buffer_size;
- // Get the user name.
- if( ! GetUserName( infoBuf, &bufCharCount ) ) {
- delete [] infoBuf;
- vpERROR_TRACE( "Cannot get the username" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
-
- }
- username = infoBuf;
- delete [] infoBuf;
-#endif
-}
-/*!
- Get the user name.
-
- - Under unix, get the content of the LOGNAME environment variable. For most
- purposes (especially in conjunction with crontab), it is more useful to use
- the environment variable LOGNAME to find out who the user is, rather than
- the getlogin() function. This is more flexible precisely because the user
- can set LOGNAME arbitrarily.
- - Under windows, uses the GetUserName() function.
-
- \return The user name.
-
- \exception vpIoException::cantGetUserName : If this method cannot get the
- user name.
-
- \sa getUserName(std::string &)
-*/
-std::string
-vpIoTools::getUserName()
-{
- std::string username;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- // Get the user name.
- char *_username = NULL;
- _username = ::getenv("LOGNAME");
- if (_username == NULL) {
- vpERROR_TRACE( "Cannot get the username. Check your LOGNAME environment variable" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
- }
- username = _username;
-#elif defined(_WIN32)
- unsigned int info_buffer_size = 1024;
- TCHAR *infoBuf = new TCHAR [info_buffer_size];
- DWORD bufCharCount = (DWORD) info_buffer_size;
- // Get the user name.
- if( ! GetUserName( infoBuf, &bufCharCount ) ) {
- delete [] infoBuf;
- vpERROR_TRACE( "Cannot get the username" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
-
- }
- username = infoBuf;
- delete [] infoBuf;
-#endif
- return username;
-}
-
-/*!
- Get the content of an environment variable.
-
- \param env : Environment variable name (HOME, LOGNAME...).
- \return Value of the environment variable.
-
- \exception vpIoException::cantGetenv : If an error occur while
- getting the environment variable value.
-
- \code
-#include <iostream>
-#include <string>
-#include <visp/vpIoTools.h>
-
-int main()
-{
- std::string envvalue;
- try {
- envvalue = vpIoTools::getenv("HOME");
- std::cout << "$HOME = \"" << envvalue << "\"" << std::endl;
- }
- catch (vpException &e) {
- std::cout << e.getMessage() << std::endl;
- return -1;
- }
- return 0;
-}
- \endcode
-
- \sa getenv(std::string &)
-*/
-std::string
-vpIoTools::getenv(const char *env)
-{
- std::string value;
- // Get the environment variable value.
- char *_value = NULL;
- _value = ::getenv(env);
- if (_value == NULL) {
- vpERROR_TRACE( "Cannot get the environment variable value" );
- throw(vpIoException(vpIoException::cantGetenv,
- "Cannot get the environment variable value")) ;
- }
- value = _value;
-
- return value;
-}
-
-/*!
- Get the content of an environment variable.
-
- \param env : Environment variable name (HOME, LOGNAME...).
- \return Value of the environment variable
-
- \exception vpIoException::cantGetenv : If an error occur while
- getting the environment variable value.
-
- \code
-#include <iostream>
-#include <string>
-#include <visp/vpIoTools.h>
-
-int main()
-{
- std::string envvalue;
- try {
- std::string env = "HOME";
- envvalue = vpIoTools::getenv(env);
- std::cout << "$HOME = \"" << envvalue << "\"" << std::endl;
- }
- catch (vpException &e) {
- std::cout << e.getMessage() << std::endl;
- return -1;
- }
- return 0;
-}
- \endcode
-
- \sa getenv(const char *)
-*/
-std::string
-vpIoTools::getenv(const std::string &env)
-{
- return (vpIoTools::getenv(env.c_str()));
-}
-
-/*!
- Extract major, minor and patch from a version given as "x.x.x".
- Ex: If version is "1.2.1", major will be 1, minor 2 and patch 1.
-
- \param version : String to extract the values.
- \param major : Extracted major.
- \param minor : Extracted minor.
- \param patch : Extracted patch.
-*/
-void
-vpIoTools::getVersion(const std::string &version, unsigned int &major, unsigned int &minor, unsigned int &patch)
-{
- if(version.size() == 0){
- major = 0;
- minor = 0;
- patch = 0;
- }
- else{
- size_t major_pos = version.find('.');
- std::string major_str = version.substr(0, major_pos);
- major = (unsigned)atoi(major_str.c_str());
-
- if(major_pos != std::string::npos){
- size_t minor_pos = version.find('.', major_pos+1);
- std::string minor_str = version.substr(major_pos+1, (minor_pos - (major_pos+1)));
- minor = (unsigned)atoi(minor_str.c_str());
-
- if(minor_pos != std::string::npos){
- std::string patch_str = version.substr(minor_pos+1);
- patch = (unsigned)atoi(patch_str.c_str());
- }
- else{
- patch = 0;
- }
- }
- else{
- minor = 0;
- patch = 0;
- }
- }
-}
-
-/*!
-
- Check if a directory exists.
-
- \param dirname : Directory to test if it exists. The directory name
- is converted to the current system's format; see path().
-
- \return true : If the directory exists and is accessible with write access.
-
- \return false : If dirname string is null, or is not a directory, or
- has no write access.
-
- \sa checkDirectory(const std::string &)
-*/
-bool
-vpIoTools::checkDirectory(const char *dirname )
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- struct stat stbuf;
-#elif defined(_WIN32)
- struct _stat stbuf;
-#endif
-
- if ( dirname == NULL || dirname[0] == '\0' ) {
- return false;
- }
-
- std::string _dirname = path(dirname);
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _dirname.c_str(), &stbuf ) != 0 )
-#elif defined(_WIN32)
- if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
-#endif
- {
- return false;
- }
- if ( (stbuf.st_mode & S_IFDIR) == 0 ) {
- return false;
- }
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( (stbuf.st_mode & S_IWUSR) == 0 )
-#elif defined(_WIN32)
- if ( (stbuf.st_mode & S_IWRITE) == 0 )
-#endif
- {
- return false;
- }
- return true;
-}
-
-/*!
- Check if a directory exists.
-
- \param dirname : Directory to test if it exists. The directory name
- is converted to the current system's format; see path().
-
- \return true : If the directory exists and is accessible with write access.
-
- \return false : If dirname string is null, or is not a directory, or
- has no write access.
-
- \sa checkDirectory(const char *)
-*/
-bool
-vpIoTools::checkDirectory(const std::string &dirname )
-{
- return vpIoTools::checkDirectory(dirname.c_str());
-}
-/*!
-
- Create a new directory.
-
- \param dirname : Directory to create. The directory name
- is converted to the current system's format; see path().
-
- \exception vpIoException::invalidDirectoryName : The \e dirname is invalid.
-
- \exception vpIoException::cantCreateDirectory : If the directory cannot be
- created.
-
- \sa makeDirectory(const std::string &)
-*/
-void
-vpIoTools::makeDirectory(const char *dirname )
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- struct stat stbuf;
-#elif defined(_WIN32)
- struct _stat stbuf;
-#endif
-
- if ( dirname == NULL || dirname[0] == '\0' ) {
- vpERROR_TRACE( "invalid directory name\n");
- throw(vpIoException(vpIoException::invalidDirectoryName,
- "invalid directory name")) ;
- }
-
- std::string _dirname = path(dirname);
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _dirname.c_str(), &stbuf ) != 0 )
-#elif defined(_WIN32)
- if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
-#endif
- {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
- if ( mkdir( _dirname.c_str(), (mode_t)0755 ) != 0 )
-#elif defined(_WIN32)
- if ( _mkdir( _dirname.c_str()) != 0 )
-#endif
- {
- vpERROR_TRACE("unable to create directory '%s'\n", dirname );
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
- }
- vpDEBUG_TRACE(2,"has created directory '%s'\n", dirname );
- }
-
- if ( checkDirectory( dirname ) == false) {
- vpERROR_TRACE("unable to create directory '%s'\n", dirname );
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
- }
-}
-
-/*!
-
- Create a new directory.
-
- \param dirname : Directory to create. The directory name
- is converted to the current system's format; see path().
-
- \exception vpIoException::cantCreateDirectory : If the directory cannot be
- created.
-
- \sa makeDirectory(const char *)
-*/
-void
-vpIoTools::makeDirectory(const std::string &dirname )
-{
- try {
- vpIoTools::makeDirectory(dirname.c_str());
- }
- catch (...) {
- vpERROR_TRACE("unable to create directory '%s'\n",dirname.c_str());
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
- }
-}
-
-/*!
-
- Check if a file exists.
-
- \param filename : Filename to test if it exists.
-
- \return true : If the filename exists and is accessible with read access.
-
- \return false : If filename string is null, or is not a filename, or
- has no read access.
-
- \sa checkFilename(const std::string &)
-*/
-bool
-vpIoTools::checkFilename(const char *filename)
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- struct stat stbuf;
-#elif defined(_WIN32)
- struct _stat stbuf;
-#endif
-
- if ( filename == NULL || filename[0] == '\0' ) {
- return false;
- }
-
- std::string _filename = path(filename);
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _filename.c_str(), &stbuf ) != 0 )
-#elif defined(_WIN32)
- if ( _stat( _filename.c_str(), &stbuf ) != 0 )
-#endif
- {
- return false;
- }
- if ( (stbuf.st_mode & S_IFREG) == 0 ) {
- return false;
- }
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( (stbuf.st_mode & S_IRUSR) == 0 )
-#elif defined(_WIN32)
- if ( (stbuf.st_mode & S_IREAD) == 0 )
-#endif
- {
- return false;
- }
- return true;
-}
-
-/*!
- Check if a file exists.
-
- \param filename : Filename to test if it exists.
-
- \return true : If the filename exists and is accessible with read access.
-
- \return false : If filename string is null, or is not a filename, or
- has no read access.
-
- \sa checkFilename(const char *)
-*/
-bool
-vpIoTools::checkFilename(const std::string &filename)
-{
- return vpIoTools::checkFilename(filename.c_str());
-}
-
-/*!
-
- Copy a \e src file or directory in \e dst.
-
- \param src : Existing file or directory to copy.
- \param dst : New copied file or directory.
-
- \return true if the file or the directory was copied, false otherwise.
-
- \sa copy(const std::string &, const std::string &)
-
-*/
-bool
-vpIoTools::copy(const char *src, const char *dst)
-{
- char cmd[FILENAME_MAX];
- int ret;
- // Check if we have to consider a file or a directory
- if ( vpIoTools::checkFilename(src) ) {
- //std::cout << "copy file: " << src << " in " << dst << std::endl;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- sprintf(cmd, "cp -p %s %s", src, dst);
-#elif defined(_WIN32)
- std::string src_ = vpIoTools::path(src);
- std::string dst_ = vpIoTools::path(dst);
- sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
-#endif
- ret = system( cmd );
- //std::cout << cmd << " return value: " << ret << std::endl;
- return true;
- }
- else if ( vpIoTools::checkDirectory(src) ) {
- //std::cout << "copy directory: " << src << " in " << dst << std::endl;
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- sprintf(cmd, "cp -p -r %s %s", src, dst);
-#elif defined(_WIN32)
- std::string src_ = vpIoTools::path(src);
- std::string dst_ = vpIoTools::path(dst);
- sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
-#endif
- ret = system( cmd );
- if(ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
- return true;
- }
- else {
- std::cout << "Cannot copy: " << src << " in " << dst << std::endl;
- return false;
- }
-}
-/*!
-
- Copy a \e src file or directory in \e dst.
-
- \param src : Existing file or directory to copy.
- \param dst : New copied file or directory.
-
- \return true if the file or the directory was copied, false otherwise.
-
- \sa copy(const char *, const char *)
-
-*/
-bool
-vpIoTools::copy(const std::string &src, const std::string &dst)
-{
- return vpIoTools::copy(src.c_str(), dst.c_str());
-}
-
-/*!
-
- Remove a file or a directory.
-
- \param file_or_dir : File or directory to remove.
-
- \return true if the file or the directory was removed, false otherwise.
-
- \sa remove(const std::string &)
-*/
-bool
-vpIoTools::remove(const char *file_or_dir)
-{
- // Check if we have to consider a file or a directory
- if ( vpIoTools::checkFilename(file_or_dir) ) {
- //std::cout << "remove file: " << file_or_dir << std::endl;
- if (::remove(file_or_dir) != 0)
- return false;
- else
- return true;
- }
- else if ( vpIoTools::checkDirectory(file_or_dir) ) {
- //std::cout << "remove directory: " << file_or_dir << std::endl;
- char cmd[FILENAME_MAX];
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- sprintf(cmd, "rm -rf %s", file_or_dir);
-#elif defined(_WIN32)
- std::string file_or_dir_ = vpIoTools::path(file_or_dir);
- sprintf(cmd, "rmdir /S /Q %s", file_or_dir_.c_str());
-#endif
- int ret = system( cmd );
- if(ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
- return true;
- }
- else {
- std::cout << "Cannot remove: " << file_or_dir << std::endl;
- return false;
- }
-}
-/*!
-
- Remove a file or a directory.
-
- \param file_or_dir : File or directory to remove.
-
- \return true if the file or the directory was removed, false otherwise.
-
- \sa remove(const char *)
-
-*/
-bool
-vpIoTools::remove(const std::string &file_or_dir)
-{
- return vpIoTools::remove(file_or_dir.c_str());
-}
-
-/*!
-
- Rename an existing file \e oldfilename in \e newfilename.
-
- \param oldfilename : File to rename.
- \param newfilename : New file name.
-
- \return true if the file was renamed, false otherwise.
-
- \sa rename(const std::string &, const std::string &)
-*/
-bool
-vpIoTools::rename(const char *oldfilename, const char *newfilename)
-{
- if (::rename(oldfilename, newfilename) != 0)
- return false;
- else
- return true;
-}
-
-/*!
-
- Rename an existing file \e oldfilename in \e newfilename.
-
- \param oldfilename : File to rename.
- \param newfilename : New file name.
-
- \return true if the file was renamed, false otherwise.
-
- \sa rename(const char *, const char *)
-*/
-bool
-vpIoTools::rename(const std::string &oldfilename, const std::string &newfilename)
-{
- return vpIoTools::rename(oldfilename.c_str(), newfilename.c_str());
-}
-
-
-
-/*!
- Converts a path name to the current system's format.
-
- \param pathname : Path name to convert. Path name to convert. Under
- windows, converts all the "/" characters in the \e pathname string
- into "\\" characters. Under Unix systems converts all the "\\"
- characters in the \e pathname string into "/" characters.
-
- \return The converted path name.
-
- \sa path(const std::string &)
-*/
-std::string
-vpIoTools::path(const char *pathname)
-{
- std::string path(pathname);
-
-#if defined(_WIN32)
- for(unsigned int i=0 ; i<path.length() ; i++)
- if( path[i] == '/') path[i] = '\\';
-#else
- for(unsigned int i=0 ; i<path.length() ; i++)
- if( path[i] == '\\') path[i] = '/';
- wordexp_t exp_result;
- wordexp(path.c_str(), &exp_result, 0);
- path = std::string(exp_result.we_wordv[0]);
- wordfree(&exp_result);
-#endif
-
- return path;
-}
-
-/*!
- Converts a path name to the current system's format.
-
- \param pathname : Path name to convert. Under windows, converts all
- the "/" characters in the \e pathname string into "\\"
- characters. Under Unix systems converts all the "\\" characters in
- the \e pathname string into "/" characters.
-
- \return The converted path name.
-
- \sa path(const char *)
-*/
-std::string
-vpIoTools::path(const std::string &pathname)
-{
- return path(pathname.c_str());
-}
-
-
-/*!
- Reads the configuration file and parses it.
-
- \param confFile : path to the file containing the configuration parameters to parse.
-
- \return true if succeed, false otherwise.
- */
-bool vpIoTools::loadConfigFile(const std::string &confFile)
-{
- configFile = path(confFile);
- configVars.clear();configValues.clear();
- std::ifstream confContent(configFile.c_str(), std::ios::in);
-
- if(confContent.is_open())
- {
- std::string line,var,val;
- long unsigned int k;
- int c;
- std::string stop[3] = {" ", "\t", "#"};
- while(std::getline(confContent, line))
- {
- if((line.find("#",0,1) != 0) && (line.size() > 2))
- {
- try
- {
- // name of the variable
- k = (unsigned long)line.find(" ");
- var = line.substr(0,k);
- // look for the end of the actual value
- c = 200;
- for(unsigned i=0;i<3;++i)
- c = vpMath::minimum(c,(int)line.find(stop[i],k+1));
- if(c==-1)
- c = (int)line.size();
- long unsigned int c_ = (long unsigned int) c;
- val = line.substr(k+1,c_-k-1);
- configVars.push_back(var);
- configValues.push_back(val);
- }
- catch(...){}
- }
- }
- confContent.close();
- }
- else {
- return false;
- }
- return true;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e float.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, float &value)
-{
- bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- if(configValues[k].compare("PI") == 0)
- value = M_PI;
- else if(configValues[k].compare("PI/2") == 0)
- value = M_PI/2;
- else if(configValues[k].compare("-PI/2") == 0)
- value = -M_PI/2;
- else
- value = (float) atof(configValues[k].c_str());
- found = true;
- }
- }
- if(found == false)
- std::cout << var << " not found in config file" << std::endl;
- return found;
-}
-/*!
- Tries to read the parameter named \e var as a \e double.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, double &value)
-{
- bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- if(configValues[k].compare("PI") == 0)
- value = M_PI;
- else if(configValues[k].compare("PI/2") == 0)
- value = M_PI/2;
- else if(configValues[k].compare("-PI/2") == 0)
- value = -M_PI/2;
- else
- value = atof(configValues[k].c_str());
- found = true;
- }
- }
- if(found == false)
- std::cout << var << " not found in config file" << std::endl;
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e int.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, int &value)
-{
- bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- value = atoi(configValues[k].c_str());
- found = true;
- }
- }
- if(found == false)
- std::cout << var << " not found in config file" << std::endl;
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e unsigned int.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, unsigned int &value)
-{
- int v = 0;
- bool found = readConfigVar(var,v);
- value = (unsigned int) v;
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e bool.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, bool &value)
-{
- int v = 0;
- bool found = readConfigVar(var,v);
- value = (v!=0);
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e vpColor.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read. See vpColor.cpp for the color number.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, vpColor &value)
-{
- unsigned int v = 0;
- bool found = readConfigVar(var,v);
- value = vpColor::getColor(v);
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e std::string.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, std::string &value)
-{
- bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- value = configValues[k];
- found = true;
- }
- }
- if(found == false)
- std::cout << var << " not found in config file" << std::endl;
- return found;
-}
-
-/*!
- Tries to read the parameter named \e var as a \e vpMatrix.
- If \e nCols and \e nRows are indicated, will resize the matrix.
- Otherwise, will try to read as many values as indicated by the dimension of \e value.
-
- \param var : Name of the parameter in the configuration file.
- \param value : Value to be read.
- \param nCols : Column dimension if resized.
- \param nRows : Row dimension if resized
-
- \return true if the parameter could be read.
-*/
-bool vpIoTools::readConfigVar(const std::string &var, vpMatrix &value, const unsigned int &nCols, const unsigned int &nRows)
-{
- bool found = false;
- std::string nb;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- found = true;
- // resize or not
- if(nCols != 0 && nRows != 0)
- value.resize(nRows, nCols);
- size_t ind=0,ind2;
- for(unsigned int i=0;i<value.getRows();++i)
- for(unsigned int j=0;j<value.getCols();++j)
- {
- ind2 = configValues[k].find(",",ind);
- nb = configValues[k].substr(ind,ind2-ind);
- if(nb.compare("PI") == 0)
- value[i][j] = M_PI;
- else if(nb.compare("PI/2") == 0)
- value[i][j] = M_PI/2;
- else if(nb.compare("-PI/2") == 0)
- value[i][j] = -M_PI/2;
- else
- value[i][j] = atof(nb.c_str());
- ind = ind2+1;
- }
- }
- }
- if(found == false)
- std::cout << var << " not found in config file" << std::endl;
- return found;
-}
-
-// construct experiment filename & path
-
-/*!
- Augments the prefix of the experiment files by \e strTrue if \e cond is verified, and by \e strFalse otherwise.
-
- \param strTrue : String to add if \e cond is true
- \param cond : Condition managing the file name
- \param strFalse : String to add if \e cond is false (default "")
-*/
-void vpIoTools::addNameElement(const std::string &strTrue, const bool &cond, const std::string &strFalse)
-{
- if(cond)
- baseName += "_" + strTrue;
- else if(strFalse != "")
- baseName += "_" + strFalse;
-}
-
-/*!
- Augments the prefix of the experiment files by \e strTrue followed by \e val.
-
- \param strTrue : String to add
- \param val : Value to add
-
-*/
-void vpIoTools::addNameElement(const std::string &strTrue, const double &val)
-{
- //if(val != 0.)
- if(std::fabs(val) < std::numeric_limits<double>::epsilon())
- {
- char valC[256];
- sprintf(valC, "%.3f", val);
- std::string valS(valC);
- baseName += "_" + strTrue + valS;
- }
-}
-
-/*!
- Creates the directory \e baseDir/baseName. If already exists, empties
- it if \e empty is true.
- Useful to save the images corresponding to a particular experiment.
-
- \param empty : Indicates if the new directory has to be emptied
-
-*/
-void vpIoTools::createBaseNamePath(const bool &empty)
-{
- if(vpIoTools::checkDirectory(baseDir + baseName) == false) {
- vpIoTools::makeDirectory(baseDir + baseName);
- std::cout << "creating directory " + baseDir + baseName << std::endl;
- }
- else {
- if(empty) {
- std::cout << "emptying directory " + baseDir + baseName << std::endl;
- vpIoTools::remove(baseDir + baseName + "/*");
- }
- }
-}
-
-/*!
- Copy the initial configuration file to the experiment directory.
-
- \param actuallySave : If false, do not copy the file.
-
-*/
-void vpIoTools::saveConfigFile(const bool &actuallySave)
-{
- if(actuallySave) {
- std::string dest = baseDir + "/" + baseName + "_config.txt";
- // file copy
- vpIoTools::copy(configFile, dest);
- }
-}
-
-/*!
- Get ViSP images data path. ViSP images data can be installed from Debian or Ubuntu \e visp-images-data package.
- It can be also installed from ViSP-images.zip that can be found on http://team.inria.fr/lagadic/visp/download.html page.
-
- This function returns the path to the folder that contains the data.
- - It checks first if \e visp-images-data package is installed. In that case returns then \e /usr/share/visp-images-data".
- - Then it checks if VISP_INPUT_IMAGE_PATH environment variable that gives the location of the data is set. In that
- case returns the content of this environment var.
-
- If the path is not found, returns an empty string.
- */
-std::string vpIoTools::getViSPImagesDataPath()
-{
- std::string data_path;
- std::string file_to_test("ViSP-images/mbt/cube.cao");
- std::string filename;
-#ifdef UNIX
- // Test if visp-images-data package is u-installed (Ubuntu and Debian)
- data_path = "/usr/share/visp-images-data";
- filename = data_path + "/" + file_to_test;
- if (vpIoTools::checkFilename(filename))
- return data_path;
-#endif
- // Test if VISP_INPUT_IMAGE_PATH env var is set
- try {
- data_path = vpIoTools::getenv("VISP_INPUT_IMAGE_PATH");
- filename = data_path + "/" + file_to_test;
- if (vpIoTools::checkFilename(filename))
- return data_path;
- }
- catch(...) {
- }
- data_path = "";
- return data_path;
-}
-
-/*!
- Returns the extension of the file or an empty string if the file has no extension. If checkFile flag is set,
- it will check first if the pathname denotes a directory and so return an empty string and second it will check
- if the file denoted by the pathanme exists. If so, it will return the extension if present.
- \param pathname : The pathname of the file we want to get the extension.
- \param checkFile : If true, the file must exist otherwise an empty string will be returned.
- \return The extension of the file or an empty string if the file has no extension.
- or if the pathname is empty.
- */
-std::string vpIoTools::getFileExtension(const std::string& pathname, const bool checkFile)
-{
- if(checkFile && (vpIoTools::checkDirectory(pathname) || !vpIoTools::checkFilename(pathname))) {
- return "";
- }
-
- //On Unix, or on the Mac
- std::string sep = "/";
- std::string altsep = "";
- std::string extsep = ".";
-
-#if defined(_WIN32)
- sep = "\\";
- altsep = "/";
- extsep = ".";
-#endif
-
- //Python 2.7.8 module.
-//# Split a path in root and extension.
-//# The extension is everything starting at the last dot in the last
-//# pathname component; the root is everything before that.
-//# It is always true that root + ext == p.
-//
-//# Generic implementation of splitext, to be parametrized with
-//# the separators
-//def _splitext(p, sep, altsep, extsep):
-// """Split the extension from a pathname.
-//
-// Extension is everything from the last dot to the end, ignoring
-// leading dots. Returns "(root, ext)"; ext may be empty."""
-//
-// sepIndex = p.rfind(sep)
-// if altsep:
-// altsepIndex = p.rfind(altsep)
-// sepIndex = max(sepIndex, altsepIndex)
-//
-// dotIndex = p.rfind(extsep)
-// if dotIndex > sepIndex:
-// # skip all leading dots
-// filenameIndex = sepIndex + 1
-// while filenameIndex < dotIndex:
-// if p[filenameIndex] != extsep:
-// return p[:dotIndex], p[dotIndex:]
-// filenameIndex += 1
-//
-// return p, ''
-
- int sepIndex = (int)pathname.rfind(sep);
- if(!altsep.empty()) {
- int altsepIndex = (int)pathname.rfind(altsep);
- sepIndex = (std::max)(sepIndex, altsepIndex);
- }
-
- size_t dotIndex = pathname.rfind(extsep);
- if(dotIndex != std::string::npos) {
- //The extsep character exists
- if((sepIndex != (int)std::string::npos && (int)dotIndex > sepIndex) || sepIndex == (int)std::string::npos) {
- if(sepIndex == (int)std::string::npos) {
- sepIndex = -1;
- }
- size_t filenameIndex = (size_t)(sepIndex + 1);
-
- while(filenameIndex < dotIndex) {
- if(pathname.compare(filenameIndex, 1, extsep) != 0) {
- return pathname.substr(dotIndex);
- }
- filenameIndex++;
- }
- }
- }
-
-
- return "";
-}
-
-/*!
- Returns the name of the file or directory denoted by this pathname.
- \return The name of the file or directory denoted by this pathname, or an
- empty string if this pathname's name sequence is empty.
- */
-std::string vpIoTools::getName(const std::string& pathname)
-{
- if(pathname.size() > 0)
- {
- std::string convertedPathname = vpIoTools::path(pathname);
-
- size_t index = convertedPathname.find_last_of(vpIoTools::separator);
- if(index != std::string::npos) {
- return convertedPathname.substr(index + 1);
- }
-
- return convertedPathname;
- }
-
- return "";
-}
-
-/*!
- Returns the name of the file without extension or directory denoted by this pathname.
- \return The name of the file without extension or directory denoted by this pathname, or an
- empty string if this pathname's name sequence is empty.
- */
-std::string vpIoTools::getNameWE(const std::string& pathname)
-{
- std::string name = vpIoTools::getName(pathname);
- size_t found = name.find_last_of(".");
- std::string name_we = name.substr(0, found);
- return name_we;
-}
-
-/*!
- Returns the pathname string of this pathname's parent.
- \return The pathname string of this pathname's parent, or
- an empty string if this pathname does not name a parent directory.
- */
-std::string vpIoTools::getParent(const std::string& pathname)
-{
- if(pathname.size() > 0)
- {
- std::string convertedPathname = vpIoTools::path(pathname);
-
- size_t index = convertedPathname.find_last_of(vpIoTools::separator);
- if(index != std::string::npos) {
- return convertedPathname.substr(0, index);
- }
- }
-
- return "";
-}
-
-/*!
- Return the file path that corresponds to the concatenated
- \e parent and \e child string files
- by adding the corresponding separator for unix or windows.
-
- The corresponding path is also converted. Under
- windows, all the "/" characters are converted
- into "\\" characters. Under Unix systems all the "\\"
- characters are converted into "/" characters.
- */
-std::string vpIoTools::createFilePath(const std::string& parent, const std::string child)
-{
- if(child.size() == 0 && parent.size() == 0)
- {
- return "";
- }
-
- if(child.size() == 0)
- {
- return vpIoTools::path(parent);
- }
-
- if(parent.size() == 0)
- {
- return vpIoTools::path(child);
- }
-
- std::string convertedParent = vpIoTools::path(parent);
- std::string convertedChild = vpIoTools::path(child);
-
- std::stringstream ss;
- ss << vpIoTools::separator;
- std::string stringSeparator;
- ss >> stringSeparator;
-
- std::string lastConvertedParentChar = convertedParent.substr(convertedParent.size() - 1);
- std::string firstConvertedChildChar = convertedChild.substr(0, 1);
-
- if(lastConvertedParentChar == stringSeparator)
- {
- convertedParent = convertedParent.substr(0, convertedParent.size() - 1);
- }
-
- if(firstConvertedChildChar == stringSeparator)
- {
- convertedChild = convertedChild.substr(1);
- }
-
- return std::string(convertedParent + vpIoTools::separator + convertedChild);
-}
-
-/*!
- Return whether a path is absolute.
-
- \return true if the pathname is absolute, false otherwise.
- */
-bool vpIoTools::isAbsolutePathname(const std::string& pathname)
-{
- //# Inspired by the Python 2.7.8 module.
- //# Return whether a path is absolute.
- //# Trivial in Posix, harder on the Mac or MS-DOS.
- //# For DOS it is absolute if it starts with a slash or backslash (current
- //# volume), or if a pathname after the volume letter and colon / UNC resource
- //# starts with a slash or backslash.
- //
- //def isabs(s):
- // """Test whether a path is absolute"""
- // s = splitdrive(s)[1]
- // return s != '' and s[:1] in '/\\'
- std::string path = splitDrive(pathname).second;
- return path.size() > 0 && (path.substr(0, 1) == "/" || path.substr(0, 1) == "\\");
-}
-
-/*!
- Split a path in a drive specification (a drive letter followed by a colon) and the path specification.
- It is always true that drivespec + pathspec == p
- Inspired by the Python 2.7.8 module.
- \return a pair whose the first element is the drive specification and the second element
- the path specification
- */
-std::pair<std::string, std::string> vpIoTools::splitDrive(const std::string& pathname)
-{
-//# Split a path in a drive specification (a drive letter followed by a
-//# colon) and the path specification.
-//# It is always true that drivespec + pathspec == p
-//def splitdrive(p):
-// """Split a pathname into drive/UNC sharepoint and relative path specifiers.
-// Returns a 2-tuple (drive_or_unc, path); either part may be empty.
-//
-// If you assign
-// result = splitdrive(p)
-// It is always true that:
-// result[0] + result[1] == p
-//
-// If the path contained a drive letter, drive_or_unc will contain everything
-// up to and including the colon. e.g. splitdrive("c:/dir") returns ("c:", "/dir")
-//
-// If the path contained a UNC path, the drive_or_unc will contain the host name
-// and share up to but not including the fourth directory separator character.
-// e.g. splitdrive("//host/computer/dir") returns ("//host/computer", "/dir")
-//
-// Paths cannot contain both a drive letter and a UNC path.
-//
-// """
-// if len(p) > 1:
-// normp = p.replace(altsep, sep)
-// if (normp[0:2] == sep*2) and (normp[2] != sep):
-// # is a UNC path:
-// # vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
-// # \\machine\mountpoint\directory\etc\...
-// # directory ^^^^^^^^^^^^^^^
-// index = normp.find(sep, 2)
-// if index == -1:
-// return '', p
-// index2 = normp.find(sep, index + 1)
-// # a UNC path can't have two slashes in a row
-// # (after the initial two)
-// if index2 == index + 1:
-// return '', p
-// if index2 == -1:
-// index2 = len(p)
-// return p[:index2], p[index2:]
-// if normp[1] == ':':
-// return p[:2], p[2:]
-// return '', p
-
-
- //On Unix, the drive is always empty.
- //On the Mac, the drive is always empty (don't use the volume name -- it doesn't have the same
- //syntactic and semantic oddities as DOS drive letters, such as there being a separate current directory per drive).
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
- return std::pair<std::string, std::string>("", pathname);
-#else
- const std::string sep = "\\";
- const std::string sepsep = "\\\\";
- const std::string altsep = "/";
-
- if(pathname.size() > 1) {
- std::string normPathname = pathname;
- std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), *sep.c_str());
-
- if(normPathname.substr(0, 2) == sepsep && normPathname.substr(2, 1) != sep) {
- // is a UNC path:
- // vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
- // \\machine\mountpoint\directory\etc\...
- // directory ^^^^^^^^^^^^^^^
- size_t index = normPathname.find(sep, 2);
- if(index == std::string::npos) {
- return std::pair<std::string, std::string>("", pathname);
- }
-
- size_t index2 = normPathname.find(sep, index + 1);
- //# a UNC path can't have two slashes in a row
- //# (after the initial two)
- if(index2 == index + 1) {
- return std::pair<std::string, std::string>("", pathname);
- }
-
- if(index2 == std::string::npos) {
- index2 = pathname.size();
- }
-
- return std::pair<std::string, std::string>(pathname.substr(0, index2), pathname.substr(index2));
- }
-
- if(normPathname[1] == ':') {
- return std::pair<std::string, std::string>(pathname.substr(0, 2), pathname.substr(2));
- }
- }
-
- return std::pair<std::string, std::string>("", pathname);
-#endif
-}
diff --git a/src/tools/io/vpIoTools.h b/src/tools/io/vpIoTools.h
deleted file mode 100644
index 7d7ef70..0000000
--- a/src/tools/io/vpIoTools.h
+++ /dev/null
@@ -1,255 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpIoTools.h 5197 2015-01-23 17:24:22Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Directory management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpIoTools_HH
-#define vpIoTools_HH
-
-/*!
- \file vpIoTools.h
- \brief File and directories basic tools.
- */
-
-#include <visp/vpConfig.h>
-
-#include <iostream>
-#include <sstream>
-#include <string>
-#include <stdlib.h>
-#include <vector>
-#include <visp/vpColor.h>
-
-/*!
- \class vpIoTools
- \ingroup FileDirectories
- \brief File and directories basic tools.
-
- The example below shows how to manipulate the functions of this
- class to create first a directory which name corresponds to the user
- name and then create a file in this directory.
-
- \code
-#include <iostream>
-#include <string>
-#include <fstream>
-#include <visp/vpIoTools.h>
-
-int main()
-{
- std::string username;
- vpIoTools::getUserName(username);
-
- // Test if a username directory exist. If no try to create it
- if (vpIoTools::checkDirectory(username) == false) {
- try {
- // Create a directory with name "username"
- vpIoTools::makeDirectory(username);
- }
- catch (...) {
- std::cout << "Cannot create " << username << " directory" << std::endl;
- return false;
- }
- }
- // Create a empty filename with name "username/file.txt"
- std::ofstream f;
- std::string filename = username + "/file.txt";
- filename = vpIoTools::path(filename); // Under Windows converts the filename string into "username\\file.txt"
-
- std::cout << "Create: " << filename << std::endl;
- f.open(filename.c_str());
- f.close();
-
- // Rename the file
- std::string newfilename = username + "/newfile.txt";
- std::cout << "Rename: " << filename << " in: " << newfilename << std::endl;
- if (vpIoTools::rename(filename, newfilename) == false)
- std::cout << "Unable to rename: " << filename << std::endl;
-
- // Remove the file
- std::cout << "Remove: " << newfilename << std::endl;
- if (vpIoTools::remove(newfilename) == false)
- std::cout << "Unable to remove: " << newfilename << std::endl;
-}
- \endcode
-
- The example below shows how to read a configuration file and how to create a name for experiment files.
- We assume the following file "/home/user/demo/config.txt" :
- \code
-expNumber 2
-save 0
-lambda 0.4
-use2D 0
-use3D 1
- \endcode
-
- \code
-#include <iostream>
-#include <string>
-#include <visp/vpIoTools.h>
-
-int main()
-{
- // reading configuration file
- vpIoTools::loadConfigFile("/home/user/demo/config.txt");
- std::string nExp;vpIoTools::readConfigVar("expNumber", nExp); // nExp <- "2"
- double lambda;vpIoTools::readConfigVar("lambda", lambda); // lambda <- 0.4
- bool use2D;vpIoTools::readConfigVar("use2D", use2D); // use2D <- false
- bool use3D;vpIoTools::readConfigVar("use3D", use3D); // use3D <- true
- bool doSave;vpIoTools::readConfigVar("save", doSave); // doSave <- false
-
- // creating name for experiment files
- vpIoTools::setBaseDir("/home/user/data");
- vpIoTools::setBaseName("exp" + nExp); // full name <- "/home/user/data/exp2"
- vpIoTools::addNameElement("2D", use2D); // full name <- "/home/user/data/exp2" since use2D==false
- vpIoTools::addNameElement("3D", use3D); // full name <- "/home/user/data/exp2_3D"
- vpIoTools::addNameElement("lambda", lambda); // full name <- "/home/user/data/exp2_3D_lambda0.4"
-
- // saving file
- vpIoTools::saveConfigFile(doSave); // would copy "/home/user/demo/config.txt" to "/home/user/data/exp2_3D_lambda0.4_config.txt" if doSave was true
-
- // create sub directory
- vpIoTools::createBaseNamePath(); // creates "/home/user/data/exp2_3D_lambda0.4/"
-}
- \endcode
-
- */
-
-class VISP_EXPORT vpIoTools
-{
-
-public:
- static void getUserName(std::string &username);
- static std::string getUserName();
- static std::string getenv(const char *env);
- static std::string getenv(const std::string &env);
- static std::string getViSPImagesDataPath();
- static void getVersion(const std::string &version, unsigned int &major, unsigned int &minor, unsigned int &patch);
- static bool checkDirectory(const char *dirname);
- static bool checkDirectory(const std::string &dirname);
- static bool checkFilename(const char *filename);
- static bool checkFilename(const std::string &filename);
- static bool copy(const char *src, const char *dst);
- static bool copy(const std::string &src, const std::string &dst);
- static void makeDirectory(const char *dirname);
- static void makeDirectory(const std::string &dirname);
- static bool remove(const char *filename);
- static bool remove(const std::string &filename);
- static bool rename(const char *oldfilename, const char *newfilename);
- static bool rename(const std::string &oldfilename, const std::string &newfilename);
-
- static std::string path(const char * pathname);
- static std::string path(const std::string &pathname);
-
- /*!
- Define the directory separator character, backslash ('\') for windows platform or slash ('/') otherwise.
- */
- static const char separator =
- #if defined(_WIN32)
- '\\';
- #else
- '/';
- #endif
-
- static std::string getFileExtension(const std::string &pathname, const bool checkFile=false);
- static std::string getName(const std::string &pathname);
- static std::string getNameWE(const std::string &pathname);
- static std::string getParent(const std::string& pathname);
- static std::string createFilePath(const std::string& parent, const std::string child);
- static bool isAbsolutePathname(const std::string& pathname);
- static std::pair<std::string, std::string> splitDrive(const std::string& pathname);
-
- // read configuration file
- static bool loadConfigFile(const std::string &confFile);
- static bool readConfigVar(const std::string &var, float &value);
- static bool readConfigVar(const std::string &var, double &value);
- static bool readConfigVar(const std::string &var, int &value);
- static bool readConfigVar(const std::string &var, unsigned int &value);
- static bool readConfigVar(const std::string &var, bool &value);
- static bool readConfigVar(const std::string &var, std::string &value);
- static bool readConfigVar(const std::string &var, vpColor &value);
- static bool readConfigVar(const std::string &var, vpMatrix &value,
- const unsigned int &nCols = 0,
- const unsigned int &nRows = 0);
-
- // construct experiment filename & path
- /*!
- Sets the base name (prefix) of the experiment files.
-
- \param s : Prefix of the experiment files.
- */
- inline static void setBaseName(const std::string &s) {baseName = s;}
- /*!
- Sets the base directory of the experiment files.
-
- \param dir : Directory where the data will be saved.
- */
- static inline void setBaseDir(const std::string &dir) {baseDir = dir + "/";}
- static void addNameElement(const std::string &strTrue,
- const bool &cond=true,
- const std::string &strFalse="");
- static void addNameElement(const std::string &strTrue, const double &val);
- /*!
- Gets the base name (prefix) of the experiment files.
-
- \return the base name of the experiment files.
- */
- inline static std::string getBaseName() {return baseName;}
- /*!
- Gets the full path of the experiment files : baseDir/baseName
-
- \return the full path of the experiment files.
- */
- inline static std::string getFullName() {return baseDir + baseName;}
-
- // write files
- static void saveConfigFile(const bool &actuallySave = true);
- static void createBaseNamePath(const bool &empty = false);
-
- protected:
- static std::string baseName;
- static std::string baseDir;
- static std::string configFile;
- static std::vector<std::string> configVars;
- static std::vector<std::string> configValues;
-} ;
-
-
-#endif
diff --git a/src/tools/io/vpKeyboard.cpp b/src/tools/io/vpKeyboard.cpp
deleted file mode 100644
index b18696e..0000000
--- a/src/tools/io/vpKeyboard.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyboard.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Keybord management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-# include <stdio.h>
-# include <visp/vpKeyboard.h>
-
-/*!
- \file vpKeyboard.cpp
- \brief Keybord management under unix.
-*/
-
-
-/*!
- Activates the raw mode to read keys in an non blocking way.
-*/
-vpKeyboard::vpKeyboard() : initial_settings(), new_settings()
-{
- init();
-}
-
-/*!
- Stops the raw mode.
-*/
-vpKeyboard::~vpKeyboard()
-{
- end();
-}
-
-/*!
-
- Get the hit key. kbhit() indicates if a key was hitten.
-*/
-int
-vpKeyboard::getchar()
-{
- int c;
- c = ::getchar();
- return c;
-}
-
-/*!
-
-
- \return 1 : if a key was hit.
-*/
-int
-vpKeyboard::kbhit()
-{
- struct timeval tv = { 0, 0 };
- fd_set readfds;
-
- FD_ZERO(&readfds);
- FD_SET(STDIN_FILENO, &readfds);
-
- return select(STDIN_FILENO + 1, &readfds, NULL, NULL, &tv) == 1;
-}
-
-/*!
-
- Activates the raw mode to read keys in an non blocking way.
-*/
-void
-vpKeyboard::init()
-{
- setRawMode(true);
-}
-
-/*!
-
- Stops the raw mode.
-*/
-void
-vpKeyboard::end()
-{
- setRawMode(false);
-}
-
-/*!
- Turns on/off the 'raw' mode.
-
- \param active : true to activate the raw mode, false to turn it off.
-
- If raw mode is active, there is no need to press return to get a key.
-*/
-void
-vpKeyboard::setRawMode(bool active)
-{
- if (active) {
-
- tcgetattr(STDIN_FILENO, &initial_settings);
-
- new_settings = initial_settings;
- // cfmakeraw(&new_settings);
- new_settings = initial_settings;
- new_settings.c_lflag &= (unsigned int)~ICANON;
- new_settings.c_lflag &= (unsigned int)~ECHO;
- new_settings.c_lflag &= (unsigned int)~ISIG;
- //new_settings.c_oflag &= (unsigned int)~NL0;
- //new_settings.c_oflag &= (unsigned int)~CR0;
- new_settings.c_oflag &= (unsigned int)~TAB0;
- //new_settings.c_oflag &= (unsigned int)~BS0;
- new_settings.c_cc[VMIN] = 1;
- new_settings.c_cc[VTIME] = 0;
- tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
-
- }
- else {
- tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings);
- }
-}
-
-#endif // defined UNIX
diff --git a/src/tools/io/vpKeyboard.h b/src/tools/io/vpKeyboard.h
deleted file mode 100644
index c1097ca..0000000
--- a/src/tools/io/vpKeyboard.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKeyboard.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Keybord management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpKeyboard_h
-#define vpKeyboard_h
-
-/*!
- \file vpKeyboard.h
- \brief Keybord management under unix.
-*/
-
-#include <visp/vpConfig.h>
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-
-# include <iostream>
-# include <termios.h>
-# include <unistd.h>
-# include <stdlib.h>
-
-
-/*!
-
- \class vpKeyboard
- \ingroup Keyboard
- \brief Keybord management under unix.
-
- Gets a key from the keyboard without waiting for the enter key.
-
- \code
- vpKeyboard keyboard; // Turn on keyboard raw mode
- int c;
- while (...) {
- ...
- if (keyboard.kbhit()) { // Detect if a key was pressed
- c = keyboard.getchar (void); // Get the pressed key
- if (c == 'q' || c == 'Q') {
- break; // Quit the while()
- }
- ...
- }
- }
-
- // Keyboard raw mode is turned off by the vpKeyboard destructor
- \endcode
-
-*/
-class VISP_EXPORT vpKeyboard
-{
-
-public:
- vpKeyboard();
- ~vpKeyboard();
- int kbhit();
- int getchar();
-
-private:
- void init();
- void end();
-
- void setRawMode(bool active);
-
- struct termios initial_settings, new_settings;
-} ;
-
-#endif // defined UNIX
-
-#endif
diff --git a/src/tools/io/vpParallelPort.cpp b/src/tools/io/vpParallelPort.cpp
deleted file mode 100644
index 2ff0628..0000000
--- a/src/tools/io/vpParallelPort.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParallelPort.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Parallel port management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_PARPORT
-
-# include <sys/types.h>
-# include <sys/stat.h>
-# include <fcntl.h>
-# include <sys/ioctl.h>
-# include <unistd.h>
-
-# include <visp/vpParallelPort.h>
-
-/*!
- \file vpParallelPort.cpp
- \brief Parallel port management under unix.
-*/
-
-
-
-static unsigned char vpParallelPortData;
-
-/*!
-
- Constructor to acces to a parallel port.
-
- Open and initialise the parallel port by sending 0 to the data bus.
-
- Set the default device to "/dev/parport0".
-
- \exception vpParallelPortException::opening If the device used to access to
- the parallel port can't be opened. A possible reason is that you don't have
- write access.
-
-*/
-vpParallelPort::vpParallelPort() : fd(0), device()
-{
- sprintf(device, "/dev/parport0");
-
- this->open();
-
- unsigned char data = 0;
- this->sendData(data);
-}
-
-/*!
- Destructor to close the parallel port.
-
- \exception vpParallelPortException::closing If the device used to access to
- the parallel port can't be closed.
-*/
-vpParallelPort::~vpParallelPort()
-{
- this->close();
-}
-
-/*!
- Open the parallel port with write access and initialise it.
-
- \exception vpParallelPortException::opening If the device used to access to
- the parallel port can't be opened. A possible reason is that you don't have
- write access.
-
- \sa close()
-*/
-void vpParallelPort::open()
-{
- fd = ::open("/dev/parport0", O_WRONLY);
- if (fd < 0) {
- printf("Can't open /dev/parport0\n");
- printf("Check if you have write access to /dev/parport0\n");
- perror("Open parallel port");
- throw(vpParallelPortException(vpParallelPortException::opening,
- "Can't open /dev/parport0")) ;
- }
-
- int i;
-
- ioctl(fd, PPCLAIM);
- i = PARPORT_MODE_COMPAT;
- ioctl(fd, PPSETMODE, & i);
- i = IEEE1284_MODE_COMPAT;
- ioctl(fd, PPNEGOT, & i);
-}
-
-/*!
- Send a data to the parallel port.
-
- \param data : Value [D7, D6, ... D0] to send to the data bus.
-
- The code bellow shows how to set D0 and D2 to logical level 1:
-
- \code
- vpParallelPort parport;
-
- unsigned char data = 5; // 0x00000101 = 5 in decimal
- parport.sendData(data); // D0 and D2 are set to logical level 1
-
- \endcode
-
- \return true if the device was close, false if an error occurs.
-*/
-void vpParallelPort::sendData(unsigned char &data)
-{
- ioctl(fd, PPWDATA, & data);
-
- // Memorise the last sended data to the static variable
- vpParallelPortData = data;
-}
-
-/*!
-
- Get the last data sent to the parallel port.
-
-*/
-unsigned char vpParallelPort::getData()
-{
- return vpParallelPortData;
-}
-
-/*!
- Close the parallel port.
-
- \exception vpParallelPortException::closing If the device used to access to
- the parallel port can't be closed.
-
-*/
-void vpParallelPort::close()
-{
- ioctl(fd, PPRELEASE);
-
- int err;
- err = ::close(fd);
-
- if (err !=0) {
- printf("Can't close the parallel port\n");
- throw(vpParallelPortException(vpParallelPortException::closing,
- "Can't close the parallel port")) ;
- }
-
-
-}
-
-
-#endif
-
diff --git a/src/tools/io/vpParallelPort.h b/src/tools/io/vpParallelPort.h
deleted file mode 100644
index d778e40..0000000
--- a/src/tools/io/vpParallelPort.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParallelPort.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Parallel port management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpParallelPort_h
-#define vpParallelPort_h
-
-/*!
- \file vpParallelPort.h
- \brief Parallel port management under unix.
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_PARPORT
-
-#include <iostream>
-# include <stdio.h>
-# include <errno.h>
-# include <linux/parport.h>
-# include <linux/ppdev.h>
-
-# include <visp/vpParallelPortException.h>
-
-/*!
-
- \class vpParallelPort
- \ingroup ParallelPort
- \brief Parallel port management under unix.
-
- The code below shows how to send a data over the parallel port.
- \code
-#include <visp/vpConfig.h>
-#include <visp/vpParallelPort.h>
-
-int main()
-{
-#ifdef VISP_HAVE_PARPORT
- vpParallelPort parport;
-
- unsigned char data = 5; // 0x00000101 = 5 in decimal
- parport.sendData(data); // D0 and D2 are set to logical level 1
-#endif
-}
- \endcode
-
-*/
-class VISP_EXPORT vpParallelPort
-{
-
-public:
- vpParallelPort();
- ~vpParallelPort();
-
- void sendData(unsigned char &data);
- unsigned char getData();
-
-private:
- void open();
- void close();
-
-private:
- int fd; // parallel port descriptor
- char device[FILENAME_MAX];
-} ;
-
-#endif
-
-#endif
diff --git a/src/tools/io/vpParallelPortException.h b/src/tools/io/vpParallelPortException.h
deleted file mode 100644
index c022185..0000000
--- a/src/tools/io/vpParallelPortException.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParallelPortException.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpParallelPort class and its derivates.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef __vpParallelPortException_H
-#define __vpParallelPortException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/*!
-
- \file vpParallelPortException.h
-
- \brief Error that can be emited by the vpParallelPort class and its
- derivates.
-
-*/
-
-/* Classes standards. */
-
-#include <visp/vpException.h>
-
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpParallelPortException
- \ingroup Exception
-
- \brief Error that can be emited by the vpParallelPort class and its
- derivates.
- */
-class VISP_EXPORT vpParallelPortException : public vpException
-{
- public:
- /*!
- \brief Lists the possible errors than can be emmited while calling
- vpParallelPort member
- */
- enum error
- {
- opening, /*!< Cannot access to the parallel port device. */
- closing /*!< Cannot close the parallel port device. */
- } ;
-
- public:
- vpParallelPortException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpParallelPortException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpParallelPortException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/tools/io/vpParseArgv.cpp b/src/tools/io/vpParseArgv.cpp
deleted file mode 100644
index a662226..0000000
--- a/src/tools/io/vpParseArgv.cpp
+++ /dev/null
@@ -1,580 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParseArgv.cpp 5311 2015-02-11 17:42:01Z fspindle $
- *
- * This file contains a procedure that handles table-based
- * argv-argc parsing.
- *
- * Copyright 1990 Regents of the University of California
- * Permission to use, copy, modify, and distribute this
- * software and its documentation for any purpose and without
- * fee is hereby granted, provided that the above copyright
- * notice appear in all copies. The University of California
- * makes no representations about the suitability of this
- * software for any purpose. It is provided "as is" without
- * express or implied warranty.
- *
- *
- * This file has been modified to not rely on tcl, tk or X11.
- * Based on tkArgv.c from tk2.3 :
- *
- *
- * Modifications by Peter Neelin (November 27, 1992)
- * Modifications by Fabien Spindler (June 20, 2006)
- */
-
-/*!
- \file vpParseArgv.cpp
- \brief Command line argument parsing.
-*/
-
-
-
-#include <visp/vpParseArgv.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <string.h>
-#include <math.h>
-#include <ctype.h>
-
-/*
- * Default table of argument descriptors. These are normally available
- * in every application.
- */
-
-vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = {
- {"-help", ARGV_HELP, (char *) NULL, (char *) NULL,
- "Print summary of command-line options and abort.\n"},
- {NULL, ARGV_END, (char *) NULL, (char *) NULL,
- (char *) NULL}
-};
-
-int (*handlerProc1)(const char *dst, const char *key, const char *argument);
-int (*handlerProc2)(const char *dst, const char *key, int valargc, const char **argument);
-
-
-/*!
- Process an argv array according to a table of expectedvcommand-line options.
-
- The return value is a boolean value with true indicating an
- error. If an error occurs then an error message is printed on
- stderr. Under normal conditions, both *argcPtr and *argv are
- modified to return the arguments that couldn't be processed here
- (they didn't match the option table, or followed an
- vpParseArgv::ARGV_REST argument).
-
- \param argcPtr: Pointer to the count of command line arguments.
-
- \param argv: Array of command line argument strings.
-
- \param argTable: Array of command-specific argument descriptions.
-
- \param flags: This parameter is to set with vpParseArgv::vpArgvFlags
- values or combinations of these values using the OR operator
- (vpParseArgv::ARGV_NO_LEFTOVERS | vpParseArgv::ARGV_NO_DEFAULTS). If
- the vpParseArgv::ARGV_NO_DEFAULTS bit is set, then
- don't generate information for default options.
-*/
-bool
-vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable,
- int flags)
-
-{
- register vpArgvInfo *infoPtr; /* Pointer to the current entry in the
- * table of argument descriptions. */
- vpArgvInfo *matchPtr; /* Descriptor that matches current argument. */
- const char *curArg; /* Current argument */
- register char c; /* Second character of current arg (used for
- * quick check for matching; use 2nd char.
- * because first char. will almost always
- * be '-'). */
- int srcIndex; /* Location from which to read next argument
- * from argv. */
- int dstIndex; /* Index into argv to which next unused
- * argument should be copied (never greater
- * than srcIndex). */
- int argc; /* # arguments in argv still to process. */
- size_t length; /* Number of characters in current argument. */
- unsigned long long nargs; /* Number of following arguments to get. */
-
-/* Macro to optionally print errors */
-#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
-
- if (flags & ARGV_DONT_SKIP_FIRST_ARG) {
- srcIndex = dstIndex = 0;
- argc = *argcPtr;
- } else {
- srcIndex = dstIndex = 1;
- argc = *argcPtr-1;
- }
-
- while (argc > 0) {
- curArg = argv[srcIndex];
- srcIndex++;
- argc--;
- c = curArg[1];
- length = strlen(curArg);
-
- /*
- * Loop throught the argument descriptors searching for one with
- * the matching key string. If found, leave a pointer to it in
- * matchPtr.
- */
-
- matchPtr = NULL;
- for (unsigned int i = 0; i < 2; i++) {
- if (i == 0) {
- infoPtr = argTable;
- } else {
- infoPtr = defaultTable;
- }
- for (; infoPtr->type != ARGV_END; infoPtr++) {
- if (infoPtr->key == NULL) {
- continue;
- }
- if ((infoPtr->key[1] != c)
- || (strncmp(infoPtr->key, curArg, length) != 0)) {
- continue;
- }
- if (infoPtr->key[length] == 0) {
- matchPtr = infoPtr;
- goto gotMatch;
- }
- if (flags & ARGV_NO_ABBREV) {
- continue;
- }
- if (matchPtr != NULL) {
- FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg);
- return true;
- }
- matchPtr = infoPtr;
- }
- }
- if (matchPtr == NULL) {
-
- /*
- * Unrecognized argument. Just copy it down, unless the caller
- * prefers an error to be registered.
- */
-
- if (flags & ARGV_NO_LEFTOVERS) {
- FPRINTF(stderr, "unrecognized argument \"%s\"\n", curArg);
- }
- argv[dstIndex] = curArg;
- dstIndex++;
- continue;
- }
-
- /*
- * Take the appropriate action based on the option type
- */
- gotMatch:
- infoPtr = matchPtr;
- switch (infoPtr->type) {
- case ARGV_CONSTANT:
- *((long long *) infoPtr->dst) = 1;
- break;
- case ARGV_INT:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr=NULL;
-
- *(((int *) infoPtr->dst)+i) =
- strtol(argv[srcIndex], &endPtr, 0);
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected integer argument for \"%s\" but got \"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_LONG:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr=NULL;
-
- *(((long *) infoPtr->dst)+i) =
- strtol(argv[srcIndex], &endPtr, 0);
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected long argument for \"%s\" but got \"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_STRING:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- *(((const char **)infoPtr->dst)+i) = argv[srcIndex];
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_REST:
- *((int *) infoPtr->dst) = dstIndex;
- goto argsDone;
- case ARGV_FLOAT:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr;
-
- *(((float *) infoPtr->dst)+i) =
- (float)strtod(argv[srcIndex], &endPtr); // Here we use strtod
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected floating-point argument for \"%s\" but got\"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_DOUBLE:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr;
-
- *(((double *) infoPtr->dst)+i) =
- strtod(argv[srcIndex], &endPtr);
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected double-point argument for \"%s\" but got\"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
-
- case ARGV_FUNC: {
- handlerProc1 = (int (*)(const char *dst, const char *key, const char *argument))infoPtr->src;
-
- if ((*handlerProc1)(infoPtr->dst, infoPtr->key, argv[srcIndex]))
- {
- srcIndex += 1;
- argc -= 1;
- }
- break;
- }
- case ARGV_GENFUNC: {
- handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, const char **argument))infoPtr->src;
-
- argc = (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv+srcIndex);
- if (argc < 0) {
- return true;
- }
- break;
- }
-
- case ARGV_HELP:
- printUsage (argTable, flags);
- return true;
- case ARGV_END:
- default:
- FPRINTF(stderr, "bad argument type %d in vpArgvInfo",
- infoPtr->type);
- return true;
- }
- }
-
- /*
- * If we broke out of the loop because of an OPT_REST argument,
- * copy the remaining arguments down.
- */
-
- argsDone:
- while (argc) {
- argv[dstIndex] = argv[srcIndex];
- srcIndex++;
- dstIndex++;
- argc--;
- }
- argv[dstIndex] = (char *) NULL;
- *argcPtr = dstIndex;
- return false;
-
- missingArg:
- FPRINTF(stderr, "\"%s\" option requires an additional argument\n", curArg);
- return true;
-
-#undef FPRINTF
-}
-
-/*!
- Generate a help string describing command-line options.
-
- Prints on stderr (unless vpParseArgv::ARGV_NO_PRINT is specified in
- flags) a help string describing all the options in argTable, plus
- all those in the default table unless vpParseArgv::ARGV_NO_DEFAULTS
- is specified in flags.
-
- \param argTable: Array of command-specific argument.descriptions.
-
- \param flags: If the vpParseArgv::ARGV_NO_DEFAULTS bit is set in
- this word, then don't generate information for default options.
-
-*/
-void
-vpParseArgv::printUsage(vpArgvInfo * argTable, int flags)
-{
- register vpArgvInfo *infoPtr;
- int width;
- int numSpaces;
-#define NUM_SPACES 20
- static char spaces[] = " ";
-/* char tmp[30]; */
- unsigned long long nargs;
-
-/* Macro to optionally print errors */
-#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
-
- /*
- * First, compute the width of the widest option key, so that we
- * can make everything line up.
- */
-
- width = 4;
- for (unsigned int i = 0; i < 2; i++) {
- for (infoPtr = i ? defaultTable : argTable;
- infoPtr->type != ARGV_END; infoPtr++) {
- int length;
- if (infoPtr->key == NULL) {
- continue;
- }
- length = (int)strlen(infoPtr->key);
- if (length > width) {
- width = length;
- }
- }
- }
-
- FPRINTF(stderr, "Command-specific options:");
- for (unsigned int i = 0; ; i++) {
- for (infoPtr = i ? defaultTable : argTable;
- infoPtr->type != ARGV_END; infoPtr++) {
- if ((infoPtr->type == ARGV_HELP) && (infoPtr->key == NULL)) {
- FPRINTF(stderr, "\n%s", infoPtr->help);
- continue;
- }
- FPRINTF(stderr, "\n %s:", infoPtr->key);
- numSpaces = width + 1 - (int)strlen(infoPtr->key);
- while (numSpaces > 0) {
- if (numSpaces >= NUM_SPACES) {
- FPRINTF(stderr, "%s",spaces);
- } else {
- FPRINTF(stderr, "%s",spaces+NUM_SPACES-numSpaces);
- }
- numSpaces -= NUM_SPACES;
- }
- FPRINTF(stderr, "%s",infoPtr->help);
- switch (infoPtr->type) {
- case ARGV_INT: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %d", *(((int *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_LONG: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %ld", *(((long *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_FLOAT: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %f", *(((float *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_DOUBLE: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %g", *(((double *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_STRING: {
- const char *string;
-
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- string = *((const char **) infoPtr->dst);
- if ((nargs==1) && (string == NULL)) break;
- for (unsigned long j=0; j<nargs; j++) {
- string = *(((const char **) infoPtr->dst)+j);
- if (string != NULL) {
- FPRINTF(stderr, " \"%s\"", string);
- }
- }
-
- break;
- }
- case ARGV_END:
- case ARGV_HELP:
- case ARGV_GENFUNC:
- case ARGV_FUNC:
- case ARGV_REST:
- case ARGV_CONSTANT:
- default: {
- break;
- }
- }
- }
-
- if ((flags & ARGV_NO_DEFAULTS) || (i > 0)) {
- break;
- }
- FPRINTF(stderr, "\nGeneric options for all commands:");
- }
-
- FPRINTF(stderr, "\n");
-#undef FPRINTF
-}
-
-/*!
-
- Get next command line option and parameter.
-
- \param argc: Count of command line arguments.
-
- \param argv: Array of command line argument strings.
-
- \param validOpts: String of valid case-sensitive option characters, a
- ':' following a given character means that option can take a parameter.
-
- \param param: Pointer to a pointer to a string for output.
-
- \return If valid option is found, the character value of that option is
- returned, and *param points to the parameter if given, or is NULL if no
- param.
-
- \return If standalone parameter (with no option) is found, 1 is returned, and
- *param points to the standalone parameter
-
- \return If option is found, but it is not in the list of valid options, -1 is
- returned, and *param points to the invalid argument.
-
- \return When end of argument list is reached, 0 is returned, and *param
- is NULL.
-
-*/
-int
-vpParseArgv::parse(int argc, const char** argv, const char* validOpts,
- const char** param)
-{
- static int iArg = 1;
- int chOpt;
- const char* psz = NULL;
- const char* pszParam = NULL;
-
- if (iArg < argc) {
- psz = &(argv[iArg][0]);
- if (*psz == '-') { // || *psz == '/') {
- // we have an option specifier
- chOpt = argv[iArg][1];
- if (isalnum(chOpt) || ispunct(chOpt)) {
- // we have an option character
- psz = strchr(validOpts, chOpt);
- if (psz != NULL) {
- // option is valid, we want to return chOpt
- if (psz[1] == ':') {
- // option can have a parameter
- psz = &(argv[iArg][2]);
- if (*psz == '\0') {
- // must look at next argv for param
- if (iArg+1 < argc) {
- psz = &(argv[iArg+1][0]);
- // next argv is the param
- iArg++;
- pszParam = psz;
- }
- else {
- // reached end of args looking for param
- // option specified without parameter
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
- }
-
- }
- else {
- // param is attached to option
- pszParam = psz;
- }
- }
- else {
- // option is alone, has no parameter
- }
- }
- else {
- // option specified is not in list of valid options
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
- }
- }
- else {
- // though option specifier was given, option character
- // is not alpha or was was not specified
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
- }
- }
- else {
- // standalone arg given with no option specifier
- chOpt = 1;
- pszParam = &(argv[iArg][0]);
- }
- }
- else {
- // end of argument list
- chOpt = 0;
- }
-
- iArg++;
- *param = pszParam;
- return (chOpt);
-}
diff --git a/src/tools/io/vpParseArgv.h b/src/tools/io/vpParseArgv.h
deleted file mode 100644
index c466daa..0000000
--- a/src/tools/io/vpParseArgv.h
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpParseArgv.h 4473 2013-09-27 16:50:44Z fspindle $
- *
- * Declarations for Tk-related things that are visible
- * outside of the Tk module itself.
- *
- * Copyright 1989-1992 Regents of the University of California.
- * Permission to use, copy, modify, and distribute this
- * software and its documentation for any purpose and without
- * fee is hereby granted, provided that the above copyright
- * notice appear in all copies. The University of California
- * makes no representations about the suitability of this
- * software for any purpose. It is provided "as is" without
- * express or implied warranty.
- *
- * 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)
- *
- *****************************************************************************/
-
-/*!
- \file vpParseArgv.h
- \brief Command line argument parsing.
-*/
-
-#ifndef vpParseArgv_h
-#define vpParseArgv_h
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-/*!
- \class vpParseArgv
- \ingroup ArgumentParser
- \brief Command line argument parsing.
-
- The code below shows a first way to parse command line arguments
- using vpParseArgv class. It allows to specify an option
- name with more than one character.
-
- \code
-#include <stdio.h>
-#include <visp/vpParseArgv.h>
-
-// Usage : [-int <integer value>] [-float <float value>] [-double <double value>] [-h]
-int main(int argc, const char ** argv)
-{
- // Variables to set by command line parsing
- int i_val = 0;
- float f_val = 0;
- double d_val = 0;
-
- // Parse the command line to set the variables
- vpParseArgv::vpArgvInfo argTable[] =
- {
- {"-int", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &i_val,
- "An integer value."},
- {"-float", vpParseArgv::ARGV_FLOAT, (char*) NULL, (char *) &f_val,
- "A float value."},
- {"-double", vpParseArgv::ARGV_DOUBLE, (char*) NULL, (char *) &d_val,
- "A double value."},
- {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
- "Print the help."},
- {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
- } ;
-
- // Read the command line options
- if(vpParseArgv::parse(&argc, argv, argTable,
- vpParseArgv::ARGV_NO_LEFTOVERS |
- vpParseArgv::ARGV_NO_ABBREV |
- vpParseArgv::ARGV_NO_DEFAULTS)) {
- return (false);
- }
-
- // i_val, f_val, d_val may have new values
-}
- \endcode
-
- The code below shows an other way to parse command line arguments using
- vpParseArgv class. Here command line options are only one character long.
- \code
-#include <stdio.h>
-#include <stdlib.h>
-#include <visp/vpParseArgv.h>
-
-// List of allowed command line options
-#define GETOPTARGS "d:f:i:h" // double point mean here that the preceding option request an argument
-
-// Usage : [-i <integer value>] [-f <float value>] [-d <double value>] [-h]
-int main(int argc, const char ** argv)
-{
- // Variables to set by command line parsing
- int i_val = 0;
- float f_val = 0;
- double d_val = 0;
-
- // Parse the command line to set the variables
- const char *optarg;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
-
- switch (c) {
- case 'd': d_val = atof(optarg); break;
- case 'f': f_val = (float) atof(optarg); break;
- case 'i': i_val = atoi(optarg); break;
- case 'h': printf("Usage: ...\n"); return true; break;
-
- default:
- printf("Usage: ...\n"); return true; break;
- }
- }
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- printf("Usage: ...\n");
- return false;
- }
-
- // i_val, f_val, d_val may have new values
-}
- \endcode
-
-
-*/
-
-
-class VISP_EXPORT vpParseArgv
-{
- public:
- /*!
- Legal values for the type field of a vpArgvInfo.
- */
- typedef enum {
- ARGV_CONSTANT,/*!< Stand alone argument. */
- ARGV_INT, /*!< Argument is associated to an int. */
- ARGV_LONG, /*!< Argument is associated to a long. */
- ARGV_STRING, /*!< Argument is associated to a char * string. */
- ARGV_REST,
- ARGV_FLOAT, /*!< Argument is associated to a float. */
- ARGV_DOUBLE, /*!< Argument is associated to a double. */
- ARGV_FUNC,
- ARGV_GENFUNC,
- ARGV_HELP, /*!< Argument is for help displaying. */
- ARGV_END /*!< End of the argument list. */
- } vpArgvType;
-
- /*!
- Flag bits.
- */
- typedef enum {
- ARGV_NO_DEFAULTS = 0x1, /*!< No default options like -help. */
- ARGV_NO_LEFTOVERS = 0x2, /*!< Print an error message if an option is not in the argument list. */
- ARGV_NO_ABBREV = 0x4, /*!< No abrevation. Print an error message if an option is abrevated (ie "-i" in place of "-int" which is requested). */
- ARGV_DONT_SKIP_FIRST_ARG = 0x8, /*!< Don't skip first argument. */
- ARGV_NO_PRINT = 0x10 /*!< No printings. */
- } vpArgvFlags;
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /*!
-
- Structure used to specify how to handle argv options.
- */
- typedef struct {
- const char *key; /*!< The key string that flags the option in the
- * argv array. */
- vpArgvType type; /*!< Indicates option type; see below. */
- const char *src; /*!< Value to be used in setting dst; usage
- * depends on type. */
- const char *dst; /*!< Address of value to be modified; usage
- * depends on type. */
- const char *help; /*!< Documentation message describing this option. */
- } vpArgvInfo;
-#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-
-public:
- static vpArgvInfo defaultTable[2];
- static bool parse(int *argcPtr, const char **argv,
- vpArgvInfo *argTable, int flags);
- static int parse(int argc, const char** argv, const char* validOpts, const char** param);
-
- private:
- static void printUsage (vpArgvInfo *argTable, int flags);
-
-
-
-} ;
-
-
-#endif
diff --git a/src/tools/mutex/vpMutex.h b/src/tools/mutex/vpMutex.h
deleted file mode 100644
index c0bfd0f..0000000
--- a/src/tools/mutex/vpMutex.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMutex.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Mutex protection.
- *
- * Authors:
- * Celine Teuliere
- *
- *****************************************************************************/
-
-
-#ifndef __VP_MUTEX__
-#define __VP_MUTEX__
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-#ifdef VISP_HAVE_PTHREAD
-
-#include <pthread.h>
-
-/*!
-
- \class vpMutex
-
- \ingroup Mutex
-
- \brief Class that allows protection by mutex.
-
- \warning This class needs the pthread third-party library.
-*/
-class VISP_EXPORT vpMutex {
-public:
- vpMutex() : m_mutex() {
- pthread_mutex_init( &m_mutex, NULL );
- }
- void lock() {
- pthread_mutex_lock( &m_mutex );
- }
- void unlock() {
- pthread_mutex_unlock( &m_mutex );
- }
-
- /*!
-
- \class vpScopedLock
-
- \ingroup Mutex
-
- \brief Class that allows protection by mutex.
-
- \warning This class needs the pthread third-party library.
- */
- class VISP_EXPORT vpScopedLock
- {
- private:
- vpMutex & _mutex;
- public:
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- void operator=(const vpScopedLock &){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
-
- vpScopedLock(vpMutex & mutex)
- : _mutex(mutex)
- {
- _mutex.lock();
- }
- ~vpScopedLock()
- {
- _mutex.unlock();
- }
- };
-private:
- pthread_mutex_t m_mutex;
-};
-
-#endif
-
-#endif
diff --git a/src/tools/plot/vpPlot.cpp b/src/tools/plot/vpPlot.cpp
deleted file mode 100644
index dcf216c..0000000
--- a/src/tools/plot/vpPlot.cpp
+++ /dev/null
@@ -1,637 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlot.cpp 5226 2015-01-29 17:05:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Plot curves.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpConfig.h>
-
-#if defined(VISP_HAVE_DISPLAY)
-#include <visp/vpPlot.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpPose.h>
-#include <fstream>
-#include <list>
-#include <vector>
-
-/*!
- Default constructor.
-
- Needs then a call to init().
-
-*/
-vpPlot::vpPlot() : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40),
- factori(1.f), factorj(1.)
-{
-}
-/*!
- This constructor creates a new window where the curves
- will be drawn. The number of graphics in the window must be set.
-
- \warning You can modify the default window size, but this is not advised.
-
- \warning Under Unix, when X11 is used to display the curves, the
- default font is set to "-adobe-times-medium-i-normal--10-100-75-75-p-52-iso8859-*".
- Note that you can chose an other one using "xfontsel".
-
- \param graph_nbr : The number of graph in the window.
- \param height : Height of the window.
- \param width : Width of the window.
- \param x,y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-vpPlot::vpPlot(const unsigned int graph_nbr,
- const unsigned int height, const unsigned int width,
- const int x, const int y, const char *title)
- : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40),
- factori(1.f), factorj(1.)
-{
- init(graph_nbr, height, width, x, y, title);
-}
-
-/*!
- Creates a new window where the curves
- will be drawn. The number of graphics in the window must be set.
-
- \warning You can modify the default window size, but this is not advised.
-
- \param graph_nbr : The number of graph in the window.
- \param height : Height of the window.
- \param width : Width of the window.
- \param x,y : The window is set at position x,y (column index, row index).
- \param title : Window title.
-*/
-void vpPlot::init(const unsigned int graph_nbr,
- const unsigned int height, const unsigned int width,
- const int x, const int y, const char *title)
-{
- I.init(height,width,255);
-
-#if defined VISP_HAVE_X11
- display = new vpDisplayX;
-#elif defined VISP_HAVE_GDI
- display = new vpDisplayGDI;
-#elif defined VISP_HAVE_OPENCV
- display = new vpDisplayOpenCV;
-#elif defined VISP_HAVE_GTK
- display = new vpDisplayGTK;
-#elif defined VISP_HAVE_D3D9
- display = new vpDisplayD3D;
-#endif
-
- display->init(I, x, y, title);
-
- vpDisplay::display(I);
-
- factori = height/700.0f;
- factorj = width/700.0f;
-
- initNbGraph(graph_nbr);
-}
-
-/*!
- Basic destructor
-*/
-vpPlot::~vpPlot()
-{
- if (graphList != NULL)
- {
- delete[] graphList;
- graphList = NULL;
- }
- if (display != NULL)
- {
- delete display;
- display = NULL;
- }
-}
-
-/*!
- Function called by the constructor to initialize the window and the basic parameters.
-
- \exception vpException::dimensionError if the parameter exceed the possible
- number of graph (4).
-
- \param graphNbr : The number of graph in the window.
-*/
-void
-vpPlot::initNbGraph (unsigned int nbGraph)
-{
- if(nbGraph > 4){
- throw vpException(vpException::dimensionError, "Cannot create more than 4 graphs");
- }
- graphList = new vpPlotGraph[nbGraph];
- graphNbr = nbGraph;
-
- switch (nbGraph)
- {
- case 1 :
- graphList[0].initSize(vpImagePoint(0,0), (unsigned int)(700*factorj),(unsigned int)(700*factori),margei,margej);
- break;
- case 2 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- break;
- case 3 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- break;
- case 4 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[3].initSize(vpImagePoint((unsigned int)(350*factori),(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- break;
- }
-
- for (unsigned int i = 0; i < graphNbr; i++)
- {
- strcpy(graphList[i].title, "");
- strcpy(graphList[i].unitx, "");
- strcpy(graphList[i].unity, "");
- strcpy(graphList[i].unitz, "");
- }
-}
-
-/*!
- Function which enables to initialize the number of curves which belongs to a 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 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*/,
-// const bool gx, const 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.
-*/
-void
-vpPlot::initRange (const unsigned int graphNum,
- double xmin, double xmax,
- double ymin, double ymax)
-{
- (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,true,true);
-}
-
-/*!
- 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.
-*/
-void
-vpPlot::initRange (const unsigned int graphNum,
- double xmin, double xmax, double ymin,
- double ymax, double zmin, double zmax)
-{
- (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,zmin,zmax,10,true,true);
-}
-
-/*!
- 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
-*/
-void
-vpPlot::setColor (const unsigned int graphNum, const unsigned int curveNum, vpColor color)
-{
- (graphList+graphNum)->setCurveColor(curveNum, color);
-}
-
-/*!
- display the grid for all graphics.
-*/
-void
-vpPlot::displayGrid()
-{
- for (unsigned int i = 0; i < graphNbr; i++)
- graphList[i].displayGrid(I);
-}
-
-/*!
- This function enables you to add a new point in the curve. This point is 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.
-*/
-void
-vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
-{
- (graphList+graphNum)->plot(I,curveNum,x,y);
-}
-
-/*!
- This function enables you to add new points in all curves of a plot. These 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.
-*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpColVector &v_y)
-{
- if((graphList+graphNum)->curveNbr == v_y.getRows())
- {
- for(unsigned int i = 0;i < v_y.getRows();++i)
- this->plot(graphNum, i, x, v_y[i]);
- }
- else
- vpTRACE("error in plot vector : not the right dimension");
-}
-
-
-/*!
- This function enables you to add a new point in the curve. This point is 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.
- \param z : The coordinate of the new point along the z axis and given in the user unit system.
-*/
-vpMouseButton::vpMouseButtonType
-vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z)
-{
- return (graphList+graphNum)->plot(I,curveNum,x,y,z);
-}
-
-/*!
- This function enables you to add new points in all curves of a plot. These 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.
- \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(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z)
-{
- vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
- if((graphList+graphNum)->curveNbr == v_y.getRows() && (graphList+graphNum)->curveNbr == v_z.getRows())
- {
- for(unsigned int i = 0;i < v_y.getRows();++i)
- button = this->plot(graphNum, i, x, v_y[i], v_z[i]);
- }
- else
- vpTRACE("error in plot vector : not the right dimension");
- return button;
-}
-
-/*!
- This method allows to change the point of view with the mouse if you have a 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.
-*/
-void
-vpPlot::navigate()
-{
- vpMouseButton::vpMouseButtonType b = vpMouseButton::none;
-
- bool blocked = false;
- unsigned int iblocked = 0;
- vpImagePoint iP;
-
- while (b != vpMouseButton::button3)
- {
- if (!blocked)
- {
- vpDisplay::getPointerPosition(I,iP);
- for (unsigned int i = 0; i < graphNbr ; i++)
- {
- if (iP.inRectangle((graphList+i)->graphZone))
- {
- iblocked = i;
- break;
- }
- }
- if ((graphList+iblocked)->move(I, b))
- {
- (graphList+iblocked)->replot3D(I);
- }
- blocked = (graphList+iblocked)->blocked;
- }
- else
- {
- if ((graphList+iblocked)->move(I, b))
- {
- (graphList+iblocked)->replot3D(I);
- }
- blocked = (graphList+iblocked)->blocked;
- }
- vpTime::sleepMs(20);
- }
-}
-
-/*!
- This method displays the coordinates of the point pointed by the mouse pointer.
-
- \param block : If true, this method waits a click.
-*/
-void
-vpPlot::getPixelValue(const bool block)
-{
- vpImagePoint iP;
-
- if (block)
- vpDisplay::getClick(I,iP);
- else
- vpDisplay::getPointerPosition(I,iP);
-
- for (unsigned int i = 0; i < graphNbr; i++)
- {
- if ((graphList+i)->getPixelValue(I,iP)) break;
- }
-}
-
-/*!
- Sets the title of a 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 title : The graphic title.
-*/
-void
-vpPlot::setTitle (const unsigned int graphNum, const char *title)
-{
- (graphList+graphNum)->setTitle(title);
-}
-
-/*!
- Sets the unit system of the x axis.
-
- \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.
-*/
-void
-vpPlot::setUnitX (const unsigned int graphNum, const char *unitx)
-{
- (graphList+graphNum)->setUnitX(unitx);
-}
-
-/*!
- Sets the unit system of the y axis.
-
- \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.
-*/
-void
-vpPlot::setUnitY (const unsigned int graphNum, const char *unity)
-{
- (graphList+graphNum)->setUnitY(unity);
-}
-
-/*!
- Sets the unit system of the z axis.
-
- \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.
-*/
-void
-vpPlot::setUnitZ (const unsigned int graphNum, const char *unitz)
-{
- (graphList+graphNum)->setUnitZ(unitz);
-}
-
-/*!
- Sets the legend of a 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 legend : The legend of the curve.
-*/
-void
-vpPlot::setLegend (const unsigned int graphNum, const unsigned int curveNum, const char *legend)
-{
- (graphList+graphNum)->setLegend(curveNum, legend);
-}
-
-/*!
- This method enables to erase the list of points stored for the graphic number \f$ graphNum \f$.
-
- \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.
-*/
-void
-vpPlot::resetPointList (const unsigned int graphNum)
-{
- for (unsigned int i = 0; i < (graphList+graphNum)->curveNbr; i++)
- (graphList+graphNum)->resetPointList(i);
-}
-
-/*!
-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.
- \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
-*/
-void
-vpPlot::setThickness (const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness)
-{
- (graphList+graphNum)->setCurveThickness(curveNum, thickness);
-}
-
-/*!
-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.
- \param thickness : The thickness you want to use
-*/
-void
-vpPlot::setGraphThickness (const unsigned int graphNum, const unsigned int thickness)
-{
- for (unsigned int curveNum=0; curveNum < (graphList+graphNum)->curveNbr; curveNum++)
- (graphList+graphNum)->setCurveThickness(curveNum, thickness);
-}
-
-/*!
- This function enables you to choose the thickness used to draw the grid and the axis of 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.
- \param thickness : The thickness you want to use
-*/
-void
-vpPlot::setGridThickness (const unsigned int graphNum, const unsigned int thickness)
-{
- (graphList+graphNum)->setGridThickness(thickness);
-}
-
-/*!
- This method enables to erase the list of points stored for the curve number \f$ curveNum \f$ contained in the graphic number \f$ graphNum \f$.
-
- \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.
-*/
-void
-vpPlot::resetPointList (const unsigned int graphNum, const unsigned int curveNum)
-{
- (graphList+graphNum)->resetPointList(curveNum);
-}
-
-/*!
- This function enables to save in a text file all the plotted points of a graphic.
-
- The first line of the text file is the graphic title. Then the points coordinates are given. If the graphic has to curves:
- - the first column corresponds to the x axis of the first curve
- - the second column corresponds to the y axis of the first curve
- - the third column corresponds to the z axis of the first curve
- - the fourth column corresponds to the x axis of the second curve
- - the fifth column corresponds to the y axis of the second curve
- - the sixth column corresponds to the z axis of the second curve
-
- The column are delimited thanks to tabultaions.
-
- \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.
-*/
-void vpPlot::saveData(const unsigned int graphNum, const char* dataFile)
-{
- std::ofstream fichier;
- fichier.open(dataFile);
-
- unsigned int ind;
- double *p = new double[3];
- bool end=false;
-
- std::vector< std::list<double>::const_iterator > vec_iter_pointListx((graphList+graphNum)->curveNbr);
- std::vector< std::list<double>::const_iterator > vec_iter_pointListy((graphList+graphNum)->curveNbr);
- std::vector< std::list<double>::const_iterator > vec_iter_pointListz((graphList+graphNum)->curveNbr);
-
- fichier << (graphList+graphNum)->title << std::endl;
-
- for(ind=0;ind<(graphList+graphNum)->curveNbr;ind++)
- {
- vec_iter_pointListx[ind] = (graphList+graphNum)->curveList[ind].pointListx.begin();
- vec_iter_pointListy[ind] = (graphList+graphNum)->curveList[ind].pointListy.begin();
- vec_iter_pointListz[ind] = (graphList+graphNum)->curveList[ind].pointListz.begin();
-// (graphList+graphNum)->curveList[ind].pointListx.front();
-// (graphList+graphNum)->curveList[ind].pointListy.front();
-// (graphList+graphNum)->curveList[ind].pointListz.front();
- }
-
- while (end == false)
- {
- end = true;
- for(ind=0;ind<(graphList+graphNum)->curveNbr;ind++)
- {
-// if (!(graphList+graphNum)->curveList[ind].pointListx.outside()
-// && !(graphList+graphNum)->curveList[ind].pointListy.outside()
-// && !(graphList+graphNum)->curveList[ind].pointListz.outside())
- if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
- && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
- && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
- {
- p[0] = *vec_iter_pointListx[ind];
- p[1] = *vec_iter_pointListy[ind];
- p[2] = *vec_iter_pointListz[ind];
-// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
-// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
-// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
-
- fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
- ++vec_iter_pointListx[ind];
- ++vec_iter_pointListy[ind];
- ++vec_iter_pointListz[ind];
-// (graphList+graphNum)->curveList[ind].pointListx.next();
-// (graphList+graphNum)->curveList[ind].pointListy.next();
-// (graphList+graphNum)->curveList[ind].pointListz.next();
-// if(!(graphList+graphNum)->curveList[ind].pointListx.nextOutside()
-// && !(graphList+graphNum)->curveList[ind].pointListy.nextOutside()
-// && !(graphList+graphNum)->curveList[ind].pointListz.nextOutside())
- if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
- && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
- && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
- end = false;
- }
- else
- {
-// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
-// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
-// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
- p[0] = (graphList+graphNum)->curveList[ind].pointListx.back();
- p[1] = (graphList+graphNum)->curveList[ind].pointListy.back();
- p[2] = (graphList+graphNum)->curveList[ind].pointListz.back();
- fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
- }
- }
- fichier << std::endl;
- }
-
- delete[] p;
- fichier.close();
-}
-
-#endif
diff --git a/src/tools/plot/vpPlot.h b/src/tools/plot/vpPlot.h
deleted file mode 100644
index a5d672b..0000000
--- a/src/tools/plot/vpPlot.h
+++ /dev/null
@@ -1,187 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlot.h 5226 2015-01-29 17:05:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Plot curves.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpPlot.h
- \brief Plot curves.
-*/
-
-#ifndef vpPlot_H
-#define vpPlot_H
-
-#include <visp/vpConfig.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpPlotGraph.h>
-
-/*!
- \class vpPlot
- \ingroup plot
-
- \brief This class enables real time drawing of 2D or 3D graphics. An
- instance of the class open a window which contains between 1 and 4
- graphics. Each one contains a desired number of curves.
-
- \warning This class is only available if one of the display functionalities (X11, GDI, GTK, OpenCV or Direct3D)
- is available. In visp/vpConfig.h header file, you should have VISP_HAVE_DISPLAY define.
-
- The example below shows how to use the vpPlot class. An other example provided in tutoral-ibvs-plotter.cpp
- and described in \ref tutorial-plotter shows how to use this class to plot in real-time some curves during
- an image-based visual servo.
-
- \code
-#include <visp/vpPlot.h>
-
-int main ()
-{
-#if defined(VISP_HAVE_DISPLAY)
- // Create a window (700 by 700) at position (100, 200) with two graphics
- vpPlot A(2, 700, 700, 100, 200, "Curves...");
-
- // The first graphic contains 1 curve and the second graphic contains 2 curves
- A.initGraph(0,1);
- A.initGraph(1,2);
-
- // The color of the curve in the first graphic is red
- A.setColor(0,0,vpColor::red);
- // The first curve in the second graphic is green
- A.setColor(1,0,vpColor::green);
- // The second curve in the second graphic is blue
- A.setColor(1,1,vpColor::blue);
-
- // Add the point (0,0) in the first graphic
- A.plot(0,0,0,0);
-
- // Add the point (0,1) to the first curve of the second graphic
- A.plot(1,0,0,1);
-
- // Add the point (0,2) to the second curve of the second graphic
- A.plot(1,1,0,2);
-
- for (int i = 0; i < 50; i++) {
- // Add the point (i,sin(i*pi/10) in the first graphic
- A.plot(0,0,i,sin(i*M_PI/10));
-
- // Add the point (i,1) to the first curve of the second graphic
- A.plot(1,0,i,1);
-
- // Add the point (i,2) to the second curve of the second graphic
- A.plot(1,1,i,2);
- }
-
- return 0;
-#endif
-}
- \endcode
-*/
-
-#if defined(VISP_HAVE_DISPLAY)
-
-class VISP_EXPORT vpPlot
-{
- public:
- vpImage<unsigned char> I;
-
- private:
- vpDisplay *display;
-
- unsigned int graphNbr;
- vpPlotGraph* graphList;
-
- unsigned int margei;
- unsigned int margej;
-
- float factori;
- float factorj;
-
- public:
- vpPlot();
- vpPlot(const unsigned int nbGraph,
- const unsigned int height=700,
- const unsigned int width=700,
- const int x=-1, const int y=-1, const char *title=NULL);
- ~vpPlot();
- void getPixelValue(const bool block);
- void init(const unsigned int nbGraph,
- const unsigned int height=700,
- const unsigned int width=700,
- const int x=-1, const int y=-1, const char *title=NULL);
- void initGraph (unsigned int graphNum, unsigned int curveNbr);
-
- void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax);
- void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax);
- void navigate (void);
-
- void plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y);
- void plot(const unsigned int graphNum, const double x, const vpColVector &v_y);
- vpMouseButton::vpMouseButtonType plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z);
- vpMouseButton::vpMouseButtonType plot(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z);
-
- void resetPointList (const unsigned int graphNum);
- void resetPointList (const unsigned int graphNum, const unsigned int curveNum);
-
- void saveData(const unsigned int graphNum, const char* dataFile);
- void setColor (const unsigned int graphNum, const unsigned int curveNum, vpColor color);
- void setGraphThickness (const unsigned int graphNum, const unsigned int thickness);
- void setGridThickness (const unsigned int graphNum, const unsigned int thickness);
- /*!
- Set the font of the characters. The display should be initialized before.
-
- To know which font are available, on Unix you can use xfontsel or xlsfonts utilities.
- */
- void setFont(const char *font)
- {
- if (display->isInitialised())
- vpDisplay::setFont(I, font);
- }
- void setLegend (const unsigned int graphNum, const unsigned int curveNum, const char *legend);
- void setTitle (const unsigned int graphNum, const char *title);
- void setUnitX (const unsigned int graphNum, const char *unitx);
- void setUnitY (const unsigned int graphNum, const char *unity);
- void setUnitZ (const unsigned int graphNum, const char *unitz);
- void setThickness (const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness);
-
- private:
- void initNbGraph (unsigned int nbGraph);
- void displayGrid();
-};
-#endif
-
-#endif
diff --git a/src/tools/plot/vpPlotCurve.cpp b/src/tools/plot/vpPlotCurve.cpp
deleted file mode 100644
index 245fb40..0000000
--- a/src/tools/plot/vpPlotCurve.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlotCurve.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Define a curve for the vpPlot class.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpPlotCurve.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayD3D.h>
-
-#if defined(VISP_HAVE_DISPLAY)
-vpPlotCurve::vpPlotCurve() :
- color(vpColor::red), curveStyle(point), thickness(1), nbPoint(0), lastPoint(),
- pointListx(), pointListy(), pointListz(), xmin(0), xmax(0), ymin(0), ymax(0)
-{
-}
-
-vpPlotCurve::~vpPlotCurve()
-{
- pointListx.clear();
- pointListy.clear();
- pointListz.clear();
-}
-
-void
-vpPlotCurve::plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y)
-{
- nbPoint++;
-
- if (nbPoint > 1)
- {
- vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
- }
-#if defined (VISP_HAVE_DISPLAY)
- double top;
- double left;
- double width;
- double height;
-
- if (iP.get_i() <= lastPoint.get_i()) {top = iP.get_i()-5; height = lastPoint.get_i() - top+10;}
- else {top = lastPoint.get_i()-5; height = iP.get_i() - top+10;}
- if (iP.get_j() <= lastPoint.get_j()) {left = iP.get_j()-5; width = lastPoint.get_j() - left+10;}
- else {left = lastPoint.get_j()-5; width = iP.get_j() - left+10;}
- vpDisplay::flushROI(I,vpRect(left,top,width,height));
-#endif
- lastPoint = iP;
- pointListx.push_back(x);
- pointListy.push_back(y);
- pointListz.push_back(0.0);
-}
-
-void
-vpPlotCurve::plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy)
-{
- std::list<double>::const_iterator it_ptListx = pointListx.begin();
- std::list<double>::const_iterator it_ptListy = pointListy.begin();
-
- unsigned int k = 0;
- vpImagePoint iP;
- while (k < nbPoint)
- {
- iP.set_ij(yorg-(zoomy*(*it_ptListy)),xorg+(zoomx*(*it_ptListx)));
-
- if (k > 0)
- vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
-
- lastPoint = iP;
-
- ++it_ptListx;
- ++it_ptListy;
- k++;
- }
-}
-
-#endif
-#endif
diff --git a/src/tools/plot/vpPlotCurve.h b/src/tools/plot/vpPlotCurve.h
deleted file mode 100644
index 32f6eac..0000000
--- a/src/tools/plot/vpPlotCurve.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlotCurve.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Define a curve for the vpPlot class.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifndef vpPlotCurve_H
-#define vpPlotCurve_H
-
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRect.h>
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPoint.h>
-
-#include <list>
-
-#if defined(VISP_HAVE_DISPLAY)
-
-//! Different styles to plot the curve.
-typedef enum
-{
- point,
- line,
- dashed_line,
- marker
-} vpCurveStyle;
-
-class vpPlotCurve
-{
- public:
- vpColor color;
- vpCurveStyle curveStyle;
- unsigned int thickness ;
- //vpMarkerStyle markerStyle;
- //char lineStyle[20];
- //vpList<vpImagePoint> pointList;
- unsigned int nbPoint;
- vpImagePoint lastPoint;
- std::list<double> pointListx;
- std::list<double> pointListy;
- std::list<double> pointListz;
- char legend[256];
- double xmin;
- double xmax;
- double ymin;
- double ymax;
-
- public:
- vpPlotCurve();
- ~vpPlotCurve();
- void plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y);
- void plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy);
-};
-
-#endif
-#endif
-#endif
diff --git a/src/tools/plot/vpPlotGraph.cpp b/src/tools/plot/vpPlotGraph.cpp
deleted file mode 100644
index 49c2fc0..0000000
--- a/src/tools/plot/vpPlotGraph.cpp
+++ /dev/null
@@ -1,1412 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPlotGraph.cpp 5226 2015-01-29 17:05:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Define a graph for the vpPlot class.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-#define FLUSH_ON_PLOT
-
-#include <visp/vpConfig.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpPlotGraph.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpPose.h>
-
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayD3D.h>
-
-#include <cmath> // std::fabs
-#include <visp/vpMath.h>
-#include <limits> // numeric_limits
-
-#if defined(VISP_HAVE_DISPLAY)
-
-int laFonctionSansNom (const double delta);
-void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3);
-
-
-vpPlotGraph::vpPlotGraph()
- : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10),
- xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(), curveNbr(1), curveList(NULL),
- scaleInitialized(false), firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
- width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
- dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
- w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.),
- nbDivisionz(10), zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(),
- blockedr(false), blockedz(false), blocked(false), epsi(5), epsj(6),
- dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
-{
- gridColor.setColor(200,200,200);
-
- old_iPr = vpImagePoint(-1,-1);
- old_iPz = vpImagePoint(-1,-1);
-
- gridThickness = 1;
-}
-
-vpPlotGraph::~vpPlotGraph()
-{
- if (curveList != NULL)
- {
- delete[] curveList;
- curveList = NULL;
- }
-}
-
-void
-vpPlotGraph::initGraph (unsigned int nbCurve)
-{
- curveList = new vpPlotCurve[nbCurve];
- curveNbr = nbCurve;
-
- vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpColor::orange,vpColor::purple};
-
- for (unsigned int i = 0; i < curveNbr; i++)
- {
- (curveList+i)->color = colors[i%6];
- (curveList+i)->curveStyle = line;
- (curveList+i)->pointListx.clear();
- (curveList+i)->pointListy.clear();
- strcpy((curveList+i)->legend,"");
- }
-}
-
-void
-vpPlotGraph::initSize (vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei, unsigned int margej)
-{
- this->topLeft = top_left;
- this->width = w;
- this->height = h;
- graphZone.setTopLeft(topLeft);
- graphZone.setWidth(width);
- graphZone.setHeight(height);
-
- this->dTopLeft = vpImagePoint(topLeft.get_i()+margei,topLeft.get_j()+margej);
- this->dWidth = width-margej-10;
- this->dHeight = height-2*margei;
- dGraphZone.setTopLeft(dTopLeft);
- dGraphZone.setWidth(dWidth+1);
- dGraphZone.setHeight(dHeight+1);
-
- this->dTopLeft3D = vpImagePoint(topLeft.get_i()+margei,topLeft.get_j()+10);
- dGraphZone3D.setTopLeft(dTopLeft3D);
- dGraphZone3D.setWidth(dWidth+1);
- dGraphZone3D.setHeight(dHeight+1);
-
- if (this->dWidth > this->dHeight)
- {
- w_ysize = 1.0;
- w_xsize = this->dWidth/this->dHeight;
- w_zsize = w_xsize;
-
- w_yval = w_ysize/2.0;
- w_xval = w_xsize/2.0;
- w_zval = w_zsize/2.0;
- }
- else if (this->dWidth == this->dHeight)
- {
- w_ysize = 1.0;
- w_xsize = 1.0;
- w_zsize = 1.0;
-
- w_yval = 0.5;
- w_xval = 0.5;
- w_zval = 0.5;
- }
- else if (this->dWidth < this->dHeight)
- {
- w_xsize = 1.0;
- w_ysize = this->dHeight/this->dWidth;
- w_zsize = w_ysize;
-
- w_yval = w_ysize/2.0;
- w_xval = w_xsize/2.0;
- w_zval = w_zsize/2.0;
- }
-
- cam.initPersProjWithoutDistortion(1000,1000,this->dWidth/2.0,this->dHeight/2.0);
-
- findPose();
-
- cMf.buildFrom(0,0,cMo[2][3],0,0,0);
-}
-
-
-void
-vpPlotGraph::findPose()
-{
- vpPoint point[4];
- point[0].setWorldCoordinates(-w_xval,-w_yval,-w_zval);
- point[1].setWorldCoordinates(w_xval,-w_yval,-w_zval);
- point[2].setWorldCoordinates(w_xval,w_yval,-w_zval);
- point[3].setWorldCoordinates(-w_xval,w_yval,-w_zval);
-
- vpImagePoint iP[4];
- iP[0].set_ij(0,0);
- iP[1].set_ij(0,dWidth-1);
- iP[2].set_ij(dHeight-1,dWidth-1);
- iP[3].set_ij(dHeight-1,0);
-
- double x=0, y=0;
- vpPose pose;
- pose.clearPoint();
-
- for (unsigned int i=0 ; i < 4 ; i++)
- {
- vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
- point[i].set_x(x);
- point[i].set_y(y);
- pose.addPoint(point[i]);
- }
-
- pose.computePose(vpPose::LAGRANGE, cMo) ;
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-}
-
-void
-vpPlotGraph::computeGraphParameters()
-{
- zoomx = dWidth/(xmax-xmin);
- zoomy = dHeight/(ymax-ymin);
- xorg = dTopLeft.get_j() - (xmin*zoomx);
- yorg = dTopLeft.get_i() + (ymax*zoomy);
-}
-
-void
-vpPlotGraph::setCurveColor(const unsigned int curveNum, const vpColor color)
-{
- (curveList+curveNum)->color = color;
-}
-
-void
-vpPlotGraph::setTitle (const char *title_)
-{
- if (strlen(title_) >= 256) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the title"));
- }
-
- strcpy(this->title, title_);
- dispTitle = true;
-}
-
-void
-vpPlotGraph::setUnitX (const char *unit_x)
-{
- if (strlen(unit_x) >= 256) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the unit along x axis"));
- }
-
- strcpy(this->unitx, unit_x);
- dispUnit = true;
-}
-
-void
-vpPlotGraph::setUnitY (const char *unit_y)
-{
- if (strlen(unit_y) >= 256) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the unit along y axis"));
- }
-
- strcpy(this->unity, unit_y);
- dispUnit = true;
-}
-
-void
-vpPlotGraph::setUnitZ (const char *unit_z)
-{
- if (strlen(unit_z) >= 256) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the unit along z axis"));
- }
-
- strcpy(this->unitz, unit_z);
- dispUnit = true;
-}
-
-void
-vpPlotGraph::setLegend (const unsigned int curveNum, const char *legend)
-{
- if (strlen(legend) >= 256) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the legend"));
- }
-
- strcpy((curveList+curveNum)->legend,legend);
- dispLegend = true;
-}
-
-void
-vpPlotGraph::setCurveThickness(const unsigned int curveNum, const unsigned int thickness)
-{
- (curveList+curveNum)->thickness = thickness;
-}
-
-int
-laFonctionSansNom (const double delta)
-{
- double d = delta;
- int power = 0;
- if (d < 1)
- {
- while (d < 1)
- {
- d = d * 10;
- power++;
- }
- power--;
- return power;
- }
-
- if (d >= 10)
- {
- while (d > 10)
- {
- d = d / 10;
- power--;
- }
- power--;
- return power;
- }
-
- return 0;
-}
-
-void
-vpPlotGraph::displayGrid (vpImage<unsigned char> &I)
-{
- computeGraphParameters();
-
- xdelt = (xmax-xmin)/nbDivisionx;
- ydelt = (ymax-ymin)/nbDivisiony;
-
- double t;
- char valeur[20];
- int power;
-
- power = laFonctionSansNom(xdelt);
- for(t=xmin;t<=xmax;t=t+xdelt)
- {
- double x = xorg+(zoomx*t);
- if(gridy)
- vpDisplay::displayDotLine(I,vpImagePoint(dTopLeft.get_i(),x), vpImagePoint(dTopLeft.get_i()+dHeight,x), gridColor, gridThickness);
- else
- vpDisplay::displayDotLine(I,vpImagePoint(yorg,x), vpImagePoint(yorg-3,x), vpColor::black, gridThickness);
-
- if (t+xdelt <= xmax+1e-10)
- {
- double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
- sprintf(valeur, "%.2f", ttemp);
-#if defined VISP_HAVE_X11
- vpDisplay::displayCharString(I,vpImagePoint(yorg + 3*epsi,x),valeur, vpColor::black);
-#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- vpDisplay::displayCharString(I,vpImagePoint(yorg + epsi,x),valeur, vpColor::black);
-#endif
- }
- }
- if (power != 0)
- {
- sprintf(valeur, "x10e%d", -power);
-#if defined VISP_HAVE_X11
- vpDisplay::displayCharString(I,vpImagePoint(yorg+4*epsi,dTopLeft.get_j()+dWidth-6*epsj),valeur, vpColor::black);
-#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- vpDisplay::displayCharString(I,vpImagePoint(yorg+4*epsi,dTopLeft.get_j()+dWidth-10*epsj),valeur, vpColor::black);
-#endif
- }
-
- power = laFonctionSansNom(ydelt);
- for(t=ymin;t<=ymax;t=t+ydelt)
- {
- double y = yorg-(zoomy*t);
- if(gridx)
- vpDisplay::displayDotLine(I,vpImagePoint(y,dTopLeft.get_j()), vpImagePoint(y,dTopLeft.get_j()+dWidth), gridColor, gridThickness);
- else
- vpDisplay::displayDotLine(I,vpImagePoint(y,xorg), vpImagePoint(y,xorg+3), vpColor::black, gridThickness);
-
- double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
-
- sprintf(valeur, "%.2f", ttemp);
-#if defined VISP_HAVE_X11
- vpDisplay::displayCharString(I,vpImagePoint(y+epsi,topLeft.get_j()+epsj),valeur, vpColor::black);
-#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- vpDisplay::displayCharString(I,vpImagePoint(y-epsi,topLeft.get_j()+epsj),valeur, vpColor::black);
-#endif
- }
- if (power != 0)
- {
- sprintf(valeur, "x10e%d", -power);
-#if defined VISP_HAVE_X11
- vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i()-3*epsi,dTopLeft.get_j()-6*epsj),valeur, vpColor::black);
-#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i()-3*epsi,dTopLeft.get_j()-6*epsj),valeur, vpColor::black);
-#endif
- }
-
- //Ligne horizontal
- vpDisplay::displayArrow(I,vpImagePoint(yorg,dTopLeft.get_j()), vpImagePoint(yorg,dTopLeft.get_j()+dWidth), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
- //Ligne verticale
- vpDisplay::displayArrow(I, vpImagePoint(dTopLeft.get_i()+dHeight,xorg), vpImagePoint(dTopLeft.get_i(),xorg), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
-
- if (dispUnit)
- displayUnit(I);
- if (dispTitle)
- displayTitle(I);
- if (dispLegend)
- displayLegend(I);
-
- //vpDisplay::flushROI(I,graphZone);
-}
-
-void
-vpPlotGraph::displayUnit (vpImage<unsigned char> &
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- I
-#endif
- )
-{
- unsigned int offsetx = vpMath::minimum<unsigned int>((unsigned int)strlen(unitx), dWidth);
-
-#if defined VISP_HAVE_X11
- vpDisplay::displayCharString(I,vpImagePoint(yorg-2*epsi,dTopLeft.get_j()+dWidth-offsetx*epsj),unitx, vpColor::black);
- vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i(),dTopLeft.get_j()+epsj),unity, vpColor::black);
-#elif defined (VISP_HAVE_GDI) || defined (VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- vpDisplay::displayCharString(I,vpImagePoint(yorg-5*epsi,dTopLeft.get_j()+dWidth-offsetx*epsj),unitx, vpColor::black);
- vpDisplay::displayCharString(I,vpImagePoint(dTopLeft.get_i(),dTopLeft.get_j()+epsj),unity, vpColor::black);
-#endif
-}
-
-void
-vpPlotGraph::displayTitle (vpImage<unsigned char> &I)
-{
- double size = (double)strlen(title);
- size = size/2.0;
- vpDisplay::displayCharString(I,
- vpImagePoint(dTopLeft.get_i()-3*epsi,
- dTopLeft.get_j()+dWidth/2.0-4*size),
- title,
- vpColor::black);
-}
-
-void
-vpPlotGraph::displayLegend (vpImage<unsigned char> &I)
-{
- size_t offsetj = 0;
- for (unsigned int i = 0; i < curveNbr; i++) {
- size_t offset = epsj * strlen((curveList+i)->legend);
- offsetj = vpMath::maximum(offset, offsetj);
- }
- if (offsetj > dWidth) offsetj = dWidth;
-
- for (unsigned int i = 0; i < curveNbr; i++) {
- vpDisplay::displayCharString(I,
- vpImagePoint(dTopLeft.get_i()+i*5*epsi,
- dTopLeft.get_j()+dWidth-offsetj),
- (curveList+i)->legend,
- (curveList+i)->color);
- }
-}
-
-void
-vpPlotGraph::rescalex(unsigned int side, double extremity)
-{
- switch (side)
- {
- case 0:
- xmin = (3*extremity-xmax)/2;
- break;
- case 1:
- xmax = (3*extremity-xmin)/2;
- break;
- }
-
- xdelt = (xmax-xmin)/(double)nbDivisionx;
-}
-
-void
-vpPlotGraph::rescaley(unsigned int side, double extremity)
-{
- switch (side)
- {
- case 0:
- ymin = (3*extremity-ymax)/2;
- break;
- case 1:
- ymax = (3*extremity-ymin)/2;
- break;
- }
-
- ydelt = (ymax-ymin)/(double)nbDivisiony;
-}
-
-void
-vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
- const double y_min, const double y_max, const int nbDivy, const bool gx, const bool gy)
-{
- this->xmin = x_min;
- this->xmax = x_max;
- this->ymin = y_min;
- this->ymax = y_max;
- this->gridx = gx;
- this->gridy = gy;
- this->nbDivisionx = nbDivx;
- this->nbDivisiony = nbDivy;
- computeGraphParameters();
- clearGraphZone(I);
- displayGrid(I);
- vpDisplay::flushROI(I,graphZone);
- scaleInitialized = true;
-}
-
-
-void
-vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
- const double y_min, const double y_max, const int nbDivy,
- const double z_min, const double z_max, const int nbDivz, const bool gx, const bool gy)
-{
- this->xmin = x_min;
- this->xmax = x_max;
- this->ymin = y_min;
- this->ymax = y_max;
- this->zmin = z_min;
- this->zmax = z_max;
- this->gridx = gx;
- this->gridy = gy;
- this->nbDivisionx = nbDivx;
- this->nbDivisiony = nbDivy;
- this->nbDivisionz = nbDivz;
- computeGraphParameters();
- clearGraphZone(I);
- displayGrid(I);
- vpDisplay::flushROI(I,graphZone);
- scaleInitialized = true;
-}
-
-void
-vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y)
-{
- if (!scaleInitialized)
- {
- if (x < 0)
- {
- xmax = 0;
- rescalex(0,x);
- }
- if (x > 0)
- {
- xmin = 0;
- rescalex(1,x);
- }
- if (y < 0)
- {
- ymax = 0;
- rescaley(0,y);
- }
- if (y > 0)
- {
- ymin = 0;
- rescaley(1,y);
- }
- scaleInitialized = true;
- computeGraphParameters();
- clearGraphZone(I);
- displayGrid(I);
- //if (y == 0)
- if (std::fabs(y) <= std::numeric_limits<double>::epsilon())
- scaleInitialized = false;
- }
-
- if (firstPoint)
- {
-// clearGraphZone(I);
-// displayGrid(I);
-// vpDisplay::flushROI(I,graphZone);
- replot(I);
- firstPoint = false;
- }
-
- double i = yorg-(zoomy*y);
- double j = xorg+(zoomx*x);
-
- vpImagePoint iP(i,j);
-
- if (!iP.inRectangle(dGraphZone))
- {
- if (x > xmax) rescalex(1,x);
- else if(x < xmin) rescalex(0,x);
-
- if (y > ymax) rescaley(1,y);
- else if(y < ymin) rescaley(0,y);
-
- computeGraphParameters();
-
- replot(I);
- i = yorg-(zoomy*y);
- j = xorg+(zoomx*x);
-
- iP.set_ij(i,j);
- }
-
- (curveList+curveNb)->plotPoint(I, iP, x, y);
-#if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
- vpDisplay::flushROI(I,graphZone);
- //vpDisplay::flush(I);
-#endif
-}
-
-void
-vpPlotGraph::replot (vpImage<unsigned char> &I)
-{
- clearGraphZone(I);
- displayGrid(I);
- for (unsigned int i = 0; i < curveNbr; i++)
- (curveList+i)->plotList(I,xorg,yorg,zoomx,zoomy);
- vpDisplay::flushROI(I,graphZone);
-}
-
-void
-vpPlotGraph::clearGraphZone(vpImage<unsigned char> &I)
-{
- vpDisplay::displayROI(I,graphZone);
-}
-
-bool
-vpPlotGraph::getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP)
-{
- if (iP.inRectangle(dGraphZone))
- {
- double x = (iP.get_j()-xorg)/zoomx;
- double y = (yorg-iP.get_i())/zoomy;
-
- vpDisplay::displayROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
- char valeur[200];
- sprintf(valeur, " x: %f", x);
- vpDisplay::displayCharString(I,vpImagePoint(topLeft.get_i()+height-2,topLeft.get_j()+5*epsj),valeur, vpColor::black);
- sprintf(valeur, " y: %f", y);
- vpDisplay::displayCharString(I,vpImagePoint(topLeft.get_i()+height-2,topLeft.get_j()+width/2.0),valeur, vpColor::black);
-// vpDisplay::flush(I);
- vpDisplay::flushROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
- return true;
- }
- return false;
-}
-
-void
-vpPlotGraph::resetPointList(const unsigned int curveNum)
-{
- (curveList+curveNum)->pointListx.clear();
- (curveList+curveNum)->pointListy.clear();
- (curveList+curveNum)->pointListz.clear();
- (curveList+curveNum)->nbPoint = 0;
- firstPoint = true;
-}
-
-
-/************************************************************************************************/
-
-bool
-vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
-{
- bool iP1In = iP1.inRectangle(dGraphZone3D);
- bool iP2In = iP2.inRectangle(dGraphZone3D);
-
- if (!iP1In || !iP2In)
- {
- double dTopLeft_i = dTopLeft3D.get_i();
- double dTopLeft_j = dTopLeft3D.get_j();
- double dBottomRight_i = dTopLeft_i+dHeight;
- double dBottomRight_j = dTopLeft_j+dWidth;
-
- //Cas vertical
- if (vpImagePoint::distance(iP1,iP2) < 9)
- return false;
- if (fabs(iP2.get_j()-iP1.get_j()) <=2)
- {
- if (!iP1In && !iP2In)
- {
- if (iP1.get_i() < dTopLeft_i && iP2.get_i() < dTopLeft_i)
- return false;
- if (iP1.get_i() > dBottomRight_i && iP2.get_i() > dBottomRight_i)
- return false;
- if (iP1.get_j() < dTopLeft_j || iP1.get_j() > dBottomRight_j)
- return false;
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- else iP1.set_i(dBottomRight_i);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- else iP2.set_i(dBottomRight_i);
- }
- else if (!iP1In)
- {
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
- return true;
- }
- else if (!iP2In)
- {
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
- return true;
- }
- }
- //cas horizontal
- else if (fabs(iP2.get_i()-iP1.get_i()) <= 2)
- {
- if (!iP1In && !iP2In)
- {
- if (iP1.get_j() < dTopLeft_j && iP2.get_j() < dTopLeft_j)
- return false;
- if (iP1.get_j() > dBottomRight_j && iP2.get_j() > dBottomRight_j)
- return false;
- if (iP1.get_i() < dTopLeft_i || iP1.get_i() > dBottomRight_i)
- return false;
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- else iP1.set_j(dBottomRight_j);
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- else iP2.set_j(dBottomRight_j);
- }
- else if (!iP1In)
- {
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
- return true;
- }
- else if (!iP2In)
- {
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
- return true;
- }
- }
-
- double a = (iP2.get_i()-iP1.get_i())/(iP2.get_j()-iP1.get_j());
- double b = iP1.get_i()-a*iP1.get_j();
-
- //test horizontal
- double jtop = (dTopLeft_i-b)/a;
- double jlow = (dBottomRight_i-b)/a;
- //test vertical
- double ileft = dTopLeft_j*a+b;
- double iright = (dBottomRight_j)*a+b;
-
- vpImagePoint iP[2];
- int n = 0;
-
- if(jtop >= dTopLeft_j && jtop <= dBottomRight_j)
- {
- iP[n].set_ij(dTopLeft_i,jtop);
- n++;
- }
- if(jlow >= dTopLeft_j && jlow <= dBottomRight_j)
- {
- iP[n].set_ij(dBottomRight_i,jlow);
- n++;
- }
- if(ileft >= dTopLeft_i && ileft <= dBottomRight_i && n <2)
- {
- iP[n].set_ij(ileft,dTopLeft_j);
- n++;
- }
- if(iright >= dTopLeft_i && iright <= dBottomRight_i && n <2)
- {
- iP[n].set_ij(iright,dBottomRight_j);
- n++;
- }
-
- if (n < 2)
- return false;
-
- if (!iP1In && !iP2In)
- {
- if (fabs(a) < 1)
- {
- if (vpMath::sign(iP1.get_j()-iP[0].get_j()) == vpMath::sign(iP2.get_j()-iP[0].get_j()))
- return false;
- int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
- if (sign == vpMath::sign(iP[0].get_j()-iP[1].get_j()))
- {
- iP1 = iP[0]; iP2 = iP[1];
- }
- else
- {
- iP1 = iP[1]; iP2 = iP[0];
- }
- }
- else
- {
- if (vpMath::sign(iP1.get_i()-iP[0].get_i()) == vpMath::sign(iP2.get_i()-iP[0].get_i()))
- return false;
- int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
- if (sign == vpMath::sign(iP[0].get_i()-iP[1].get_i()))
- {
- iP1 = iP[0];iP2 = iP[1];
- }
- else
- {
- iP1 = iP[1];iP2 = iP[0];
- }
- }
- }
- else if (!iP1In)
- {
- vpImagePoint iPtemp = iP1;
- if (fabs(a) < 1)
- {
- int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
- if (fabs(iP[0].get_j()-iP2.get_j()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_j()-iP2.get_j())) iP1 = iP[0];
- else iP1 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_j()-iP2.get_j())) iP1 = iP[1];
- else iP1 = iP[0];
- }
- }
- else
- {
- int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
- if (fabs(iP[0].get_i()-iP2.get_i()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_i()-iP2.get_i())) iP1 = iP[0];
- else iP1 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_i()-iP2.get_i())) iP1 = iP[1];
- else iP1 = iP[0];
- }
- }
- if (vpImagePoint::distance(iP1,iP2) < 9)
- {
- iP1 = iPtemp;
- return false;
- }
- }
- else if (!iP2In)
- {
- vpImagePoint iPtemp = iP2;
- if (fabs(a) < 1)
- {
- int sign = vpMath::sign(iP2.get_j() - iP1.get_j());
- if (fabs(iP[0].get_j()-iP1.get_j()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_j()-iP1.get_j())) iP2 = iP[0];
- else iP2 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_j()-iP1.get_j())) iP2 = iP[1];
- else iP2 = iP[0];
- }
- }
- else
- {
- int sign = vpMath::sign(iP2.get_i() - iP1.get_i());
- if (fabs(iP[0].get_i()-iP1.get_i()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_i()-iP1.get_i())) iP2 = iP[0];
- else iP2 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_i()-iP1.get_i())) iP2 = iP[1];
- else iP2 = iP[0];
- }
- }
- if (vpImagePoint::distance(iP1,iP2) < 9)
- {
- iP2 = iPtemp;
- return false;
- }
- }
- }
- return true;
-}
-
-bool
-vpPlotGraph::check3Dpoint(vpImagePoint &iP)
-{
- if (!iP.inRectangle(dGraphZone3D))
- {
- if (iP.get_i() < dTopLeft3D.get_i())
- iP.set_i(dTopLeft3D.get_i());
- else if (iP.get_i() > dTopLeft3D.get_i()+dHeight)
- iP.set_i(dTopLeft3D.get_i()+dHeight-1);
- if (iP.get_j() <dTopLeft3D.get_j())
- iP.set_j(dTopLeft3D.get_j());
- else if (iP.get_j() > dTopLeft3D.get_j()+dWidth)
- iP.set_j(dTopLeft3D.get_j()+dWidth-1);
- return false;
- }
- return true;
-}
-
-void
-vpPlotGraph::computeGraphParameters3D()
-{
- zoomx_3D = w_xsize/(xmax-xmin);
- zoomy_3D = w_ysize/(ymax-ymin);
- zoomz_3D = w_zsize/(zmax-zmin);
- ptXorg = w_xval - zoomx_3D*xmax;
- ptYorg = w_yval + zoomy_3D*ymin;
- ptZorg = w_zval - zoomz_3D*zmax;
-}
-
-void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3)
-{
- if (pente <= 1)
- {
- ip1 = iPunit-vpImagePoint(3,0);
- ip2 = iPunit+vpImagePoint(3,0);
- ip3 = iPunit-vpImagePoint(6,6);
- }
- else
- {
- ip1 = iPunit-vpImagePoint(0,3);
- ip2 = iPunit+vpImagePoint(0,3);
- ip3 = iPunit+vpImagePoint(6,6);
- }
-}
-
-void
-vpPlotGraph::displayGrid3D (vpImage<unsigned char> &I)
-{
- computeGraphParameters3D();
-
- xdelt = (xmax-xmin)/nbDivisionx;
- ydelt = (ymax-ymin)/nbDivisiony;
- zdelt = (zmax-zmin)/nbDivisionz;
-
- vpPoint pt[6];
- pt[0].setWorldCoordinates(-w_xval,ptYorg,ptZorg);
- pt[1].setWorldCoordinates(w_xval,ptYorg,ptZorg);
- pt[2].setWorldCoordinates(ptXorg,-w_yval,ptZorg);
- pt[3].setWorldCoordinates(ptXorg,w_yval,ptZorg);
- pt[4].setWorldCoordinates(ptXorg,ptYorg,-w_zval);
- pt[5].setWorldCoordinates(ptXorg,ptYorg,w_zval);
-
- vpImagePoint iP[6];
- for (unsigned int i = 0; i < 6; i++)
- {
- pt[i].track(cMo);
- double u=0.0,v=0.0;
- vpMeterPixelConversion::convertPoint(cam,pt[i].get_x(),pt[i].get_y(),u,v);
- iP[i].set_uv(u,v);
- iP[i] = iP[i] + dTopLeft3D;
- }
-
- int power;
- double t;
- char valeur[20];
- vpPoint ptunit;
- vpImagePoint iPunit;
- double pente;
- vpImagePoint ip1;
- vpImagePoint ip2;
- vpImagePoint ip3;
- vpImagePoint ip4;
-
- power = laFonctionSansNom(xdelt);
- ptunit.setWorldCoordinates(-w_xval,ptYorg,ptZorg);
- //if (iP[0].get_j()-iP[1].get_j() != 0)
- if (std::fabs(iP[0].get_j()-iP[1].get_j()) >
- vpMath::maximum(std::fabs(iP[0].get_j()), std::fabs(iP[1].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[0].get_i()-iP[1].get_i())/(iP[0].get_j()-iP[1].get_j()));
- else pente = 2;
-
- unsigned int count = 1;
- for(t=xmin;t<=xmax;t=t+xdelt)
- {
- double x = ptXorg+(zoomx_3D*t);
- ptunit.set_oX(x);
- ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
- iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip2, ip1, vpColor::black);
- if (count%2 == 1)
- {
- double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
- sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
- }
- }
- count++;
- }
- if (power != 0)
- {
- ip4 = iP[1] -vpImagePoint(-15,10);
- sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
- }
-
- power = laFonctionSansNom(ydelt);
- ptunit.setWorldCoordinates(ptXorg,-w_yval,ptZorg);
- //if (iP[2].get_j()-iP[3].get_j() != 0)
- if (std::fabs(iP[2].get_j()-iP[3].get_j()) >
- vpMath::maximum(std::fabs(iP[2].get_j()), std::fabs(iP[3].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[2].get_i()-iP[3].get_i())/(iP[2].get_j()-iP[3].get_j()));
- else pente = 2;
- count = 0;
- for(t=ymin;t<=ymax;t=t+ydelt)
- {
- double y = ptYorg-(zoomy_3D*t);
- ptunit.set_oY(y);
- ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
- iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
- if (count%2 == 1)
- {
- double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
- sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
- }
- }
- count++;
- }
- if (power != 0)
- {
- ip4 = iP[2] -vpImagePoint(-15,10);
- sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
- }
-
- power = laFonctionSansNom(zdelt);
- ptunit.setWorldCoordinates(ptXorg,ptYorg,-w_zval);
- //if (iP[4].get_j()-iP[5].get_j() != 0)
- if (std::fabs(iP[4].get_j()-iP[5].get_j()) >
- vpMath::maximum(std::fabs(iP[4].get_j()), std::fabs(iP[5].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[4].get_i()-iP[5].get_i())/(iP[4].get_j()-iP[5].get_j()));
- else pente = 2;
- count = 0;
- for(t=zmin;t<=zmax;t=t+zdelt)
- {
- double z = ptZorg+(zoomz_3D*t);
- ptunit.set_oZ(z);
- ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
- iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
- if (count%2 == 1)
- {
- double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
- sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayCharString(I,ip3,valeur, vpColor::black);
- }
- }
- count++;
- }
- if (power != 0)
- {
- ip4 = iP[5] -vpImagePoint(-15,10);
- sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayCharString(I,ip4,valeur, vpColor::black);
- }
-
-
-//Ligne horizontal
- if (check3Dline(iP[0],iP[1]))
- {
- vpDisplay::displayArrow(I,iP[0],iP[1], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[1].get_i(),iP[1].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayCharString(I,iPunit,unitx, vpColor::black);
- }
- }
- if (check3Dline(iP[3],iP[2]))
- {
- vpDisplay::displayArrow(I,iP[3],iP[2], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[2].get_i(),iP[2].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayCharString(I,iPunit,unity, vpColor::black);
- }
- }
- if (check3Dline(iP[4],iP[5]))
- {
- vpDisplay::displayArrow(I,iP[4],iP[5], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[5].get_i(),iP[5].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayCharString(I,iPunit,unitz, vpColor::black);
- }
- }
-
- if (dispTitle)
- displayTitle(I);
- if (dispLegend)
- displayLegend(I);
-}
-
-vpMouseButton::vpMouseButtonType
-vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z)
-{
- if (!scaleInitialized)
- {
- if (x < 0)
- {
- xmax = 0;
- rescalex(0,x);
- }
- if (x > 0)
- {
- xmin = 0;
- rescalex(1,x);
- }
- if (y < 0)
- {
- ymax = 0;
- rescaley(0,y);
- }
- if (y > 0)
- {
- ymin = 0;
- rescaley(1,y);
- }
- if (z < 0)
- {
- zmax = 0;
- rescalez(0,z);
- }
- if (z > 0)
- {
- zmin = 0;
- rescalez(1,z);
- }
- scaleInitialized = true;
- computeGraphParameters3D();
- clearGraphZone(I);
- displayGrid3D(I);
- //if (std::fabs(y) == 0 || z == 0)
- if (std::fabs(y) <= std::numeric_limits<double>::epsilon() || std::fabs(z) <= std::numeric_limits<double>::epsilon())
- scaleInitialized = false;
- }
-
- if (firstPoint)
- {
- clearGraphZone(I);
- displayGrid3D(I);
- vpDisplay::flushROI(I,graphZone);
- firstPoint = false;
- }
-
- bool changed = false;
- if (x > xmax) {rescalex(1,x); changed = true;}
- else if(x < xmin) {rescalex(0,x);changed = true;}
-
- if (y > ymax) {rescaley(1,y);changed = true;}
- else if(y < ymin) {rescaley(0,y);changed = true;}
-
- if (z > zmax) {rescalez(1,z);changed = true;}
- else if(z < zmin) {rescalez(0,z);changed = true;}
-
- vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
-
- if (changed || move(I, button))
- {
- computeGraphParameters3D();
- replot3D(I);
- }
-
- vpPoint pointPlot;
- pointPlot.setWorldCoordinates(ptXorg+(zoomx_3D*x),ptYorg-(zoomy_3D*y),ptZorg+(zoomz_3D*z));
- pointPlot.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,pointPlot.get_x(),pointPlot.get_y(),u,v);
- vpImagePoint iP;
- iP.set_uv(u,v);
- iP = iP + dTopLeft3D;
-
- (curveList+curveNb)->pointListx.end();
- (curveList+curveNb)->pointListy.end();
- (curveList+curveNb)->pointListz.end();
- if((curveList+curveNb)->nbPoint)
- {
- if (check3Dline((curveList+curveNb)->lastPoint,iP))
- vpDisplay::displayLine(I,(curveList+curveNb)->lastPoint, iP, (curveList+curveNb)->color, (curveList+curveNb)->thickness);
- }
-#if( defined VISP_HAVE_X11 || defined VISP_HAVE_GDI )
- double top;
- double left;
- double width_;
- double height_;
-
- if (iP.get_i() <= (curveList+curveNb)->lastPoint.get_i()) {top = iP.get_i()-5; height_ = (curveList+curveNb)->lastPoint.get_i() - top+10;}
- else {top = (curveList+curveNb)->lastPoint.get_i()-5; height_ = iP.get_i() - top+10;}
- if (iP.get_j() <= (curveList+curveNb)->lastPoint.get_j()) {left = iP.get_j()-5; width_ = (curveList+curveNb)->lastPoint.get_j() - left+10;}
- else {left = (curveList+curveNb)->lastPoint.get_j()-5; width_ = iP.get_j() - left+10;}
- vpDisplay::flushROI(I,vpRect(left,top,width_,height_));
-#endif
-
- (curveList+curveNb)->lastPoint = iP;
- (curveList+curveNb)->pointListx.push_back(x);
- (curveList+curveNb)->pointListy.push_back(y);
- (curveList+curveNb)->pointListz.push_back(z);
- (curveList+curveNb)->nbPoint++;
-
-#if( !defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
- vpDisplay::flushROI(I,graphZone);
-#endif
- return button;
-}
-
-void
-vpPlotGraph::replot3D (vpImage<unsigned char> &I)
-{
- clearGraphZone(I);
- displayGrid3D(I);
-
- for (unsigned int i = 0; i < curveNbr; i++)
- {
- std::list<double>::const_iterator it_ptListx = (curveList+i)->pointListx.begin();
- std::list<double>::const_iterator it_ptListy = (curveList+i)->pointListy.begin();
- std::list<double>::const_iterator it_ptListz = (curveList+i)->pointListz.begin();
-
- unsigned int k = 0;
- vpImagePoint iP;
- vpPoint pointPlot;
- double x,y,z;
- while (k < (curveList+i)->nbPoint)
- {
- x = *it_ptListx;
- y = *it_ptListy;
- z = *it_ptListz;
- pointPlot.setWorldCoordinates(ptXorg+(zoomx_3D*x),ptYorg-(zoomy_3D*y),ptZorg+(zoomz_3D*z));
- pointPlot.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,pointPlot.get_x(),pointPlot.get_y(),u,v);
- iP.set_uv(u,v);
- iP = iP + dTopLeft3D;
-
- //vpDisplay::displayCross(I,iP,3,vpColor::cyan);
- if (k > 0)
- {
- if (check3Dline((curveList+i)->lastPoint,iP))
- vpDisplay::displayLine(I,(curveList+i)->lastPoint, iP, (curveList+i)->color);
- //vpDisplay::displayCross(I,iP,3,vpColor::orange);
- }
-
- (curveList+i)->lastPoint = iP;
-
- ++it_ptListx;
- ++it_ptListy;
- ++it_ptListz;
- k++;
- }
- }
- vpDisplay::flushROI(I,graphZone);
-}
-
-
-
-void
-vpPlotGraph::rescalez(unsigned int side, double extremity)
-{
- switch (side)
- {
- case 0:
- zmin = (3*extremity-zmax)/2;
- break;
- case 1:
- zmax = (3*extremity-zmin)/2;
- break;
- }
-
- zdelt = (zmax-zmin)/(double)nbDivisionz;
-}
-
-/*!
- \param I : Image used to display the graph.
- \param button : Mouse button used during navigation.
- */
-bool
-vpPlotGraph::move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button)
-{
- bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I, changed, button);
-
- //if (displacement[2][3] != 0)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- cMf = cMf*displacement;
- vpHomogeneousMatrix fMo = cMf.inverse()*cMo;
-
- cMo = cMf* displacement * fMo;
- return changed;
-}
-
-vpHomogeneousMatrix
-vpPlotGraph::navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b)
-{
- vpImagePoint iP;
- vpImagePoint trash;
- bool clicked = false;
- bool clickedUp = false;
-
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
- changed = false;
-
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
-
- if(!blocked) clicked = vpDisplay::getClick(I,iP,b,false);
-
- if(blocked) clickedUp = vpDisplay::getClickUp(I,trash, b,false);
-
- if (clicked)
- {
- if (!iP.inRectangle(graphZone))
- return mov;
- }
-
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
- blocked = true;
- }
-
- else if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
- blockedr = false;
- }
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
- blockedz = false;
- }
- if (!(blockedr || blockedz))
- {
- blocked = false;
- //while (vpDisplay::getClick(I,trash,b,false)) {};
- }
- }
-
- vpDisplay::getPointerPosition(I,iP);
-
- double anglei = 0;
- double anglej = 0;
-
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
- double width_ = vpMath::minimum(I.getWidth(),I.getHeight());
-
- double diffi = iP.get_i() - old_iPr.get_i();
- double diffj = iP.get_j() - old_iPr.get_j();
-
- anglei = diffi*360/width_;
- anglej = diffj*360/width_;
- mov.buildFrom(0,0,0,vpMath::rad(anglei),vpMath::rad(-anglej),0);
- changed = true;
- }
-
- if (blockedr) old_iPr = iP;
-
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
- double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
- changed = true;
- }
-
- if (blockedz) old_iPz = iP;
-
- return mov;
-}
-
-#endif
-#endif
diff --git a/src/tools/plot/vpPlotGraph.h b/src/tools/plot/vpPlotGraph.h
deleted file mode 100644
index 2da1a0f..0000000
--- a/src/tools/plot/vpPlotGraph.h
+++ /dev/null
@@ -1,201 +0,0 @@
-
-/****************************************************************************
- *
- * $Id: vpPlotGraph.h 5226 2015-01-29 17:05:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Define a graph for the vpPlot class.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifndef vpPlotGraph_H
-#define vpPlotGraph_H
-
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-#include <visp/vpPlotCurve.h>
-#include <visp/vpMouseButton.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRect.h>
-
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPoint.h>
-
-#if defined(VISP_HAVE_DISPLAY)
-
-class vpPlotGraph
-{
- public:
- double xorg;
- double yorg;
- double zoomx;
- double zoomy;
- double xmax;
- double ymax;
- double xmin;
- double ymin;
- double xdelt;
- double ydelt;
- bool gridx;
- bool gridy;
- vpColor gridColor;
- char title[256];
- char unitx[256];
- char unity[256];
- unsigned int curveNbr;
- vpPlotCurve* curveList;
- bool scaleInitialized;
- bool firstPoint;
-
- int nbDivisionx;
- int nbDivisiony;
-
- //Graph complet
- vpImagePoint topLeft;
- unsigned int width;
- unsigned int height;
- vpRect graphZone;
-
- //Zone d'affichage
- vpImagePoint dTopLeft;
- unsigned int dWidth;
- unsigned int dHeight;
- vpRect dGraphZone;
-
- //Zone d'affichage
- vpImagePoint dTopLeft3D;
-// int dWidth;
-// int dHeight;
- vpRect dGraphZone3D;
-
- //3D part
- vpCameraParameters cam;
- vpHomogeneousMatrix cMo;
- vpHomogeneousMatrix cMf;
- double w_xval;
- double w_xsize;
- double w_yval;
- double w_ysize;
- double w_zval;
- double w_zsize;
- double ptXorg;
- double ptYorg;
- double ptZorg;
- double zoomx_3D;
- double zoomy_3D;
- double zoomz_3D;
-
- int nbDivisionz;
-
- double zorg;
- double zoomz;
- double zmax;
- double zmin;
- double zdelt;
- char unitz[256];
- vpImagePoint old_iPr;
- vpImagePoint old_iPz;
- bool blockedr;
- bool blockedz;
- bool blocked;
-
- unsigned int epsi;
- unsigned int epsj;
-
- bool dispUnit;
- bool dispTitle;
- bool dispLegend;
-
- unsigned int gridThickness;
-
- public:
- vpPlotGraph();
- ~vpPlotGraph();
-
- bool check3Dline(vpImagePoint &iP1, vpImagePoint &iP2);
- bool check3Dpoint(vpImagePoint &iP);
- void clearGraphZone(vpImage<unsigned char> &I);
- void computeGraphParameters();
- void computeGraphParameters3D();
-
- void initGraph (unsigned int nbCurve);
- void initSize (vpImagePoint topLeft, unsigned int width, unsigned int height, unsigned int margei, unsigned int margej);
- void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const bool gx, const bool gy);
- void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const double zmin, const double zmax, const int nbDivz, const bool gx, const bool gy);
-
- void displayGrid (vpImage<unsigned char> &I);
- void displayUnit (vpImage<unsigned char> &I);
- void displayTitle (vpImage<unsigned char> &I);
- void displayLegend (vpImage<unsigned char> &I);
- void displayGrid3D (vpImage<unsigned char> &I);
-
- void findPose();
-
- bool getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP);
-
- bool move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button);
- vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b);
-
- void plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y);
- vpMouseButton::vpMouseButtonType plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z);
- void replot (vpImage<unsigned char> &I);
- void replot3D (vpImage<unsigned char> &I);
-
- void rescalex(unsigned int side, double extremity);
- void rescaley(unsigned int side, double extremity);
- void rescalez(unsigned int side, double extremity);
- //void rescale(double &min, double &max, double &delta, const int nbDiv, int side);
- void resetPointList(const unsigned int curveNum);
-
- void setCurveColor(const unsigned int curveNum, const vpColor color);
- void setCurveThickness(const unsigned int curveNum, const unsigned int thickness);
- void setGridThickness (const unsigned int thickness) {
- this->gridThickness = thickness;
- };
- void setLegend (const unsigned int curveNum, const char *legend);
- void setTitle (const char *title);
- void setUnitX (const char *unitx);
- void setUnitY (const char *unity);
- void setUnitZ (const char *unitz);
-};
-
-#endif
-#endif
-#endif
-
diff --git a/src/tools/time/vpTime.cpp b/src/tools/time/vpTime.cpp
deleted file mode 100644
index e1d5adc..0000000
--- a/src/tools/time/vpTime.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTime.cpp 4622 2014-01-28 17:40:36Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Time management and measurement.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpTime.h>
-#include <visp/vpDebug.h>
-#include <iostream>
-
-
-/*!
- \file vpTime.cpp
- \brief Time management and measurement
-
-*/
-
-
-// Unix depend version
-
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <sys/time.h>
-# include <unistd.h>
-#elif defined(_WIN32)
-# include <windows.h>
-# include <winbase.h>
-#endif
-
-double vpTime::minTimeForUsleepCall = 4; /*! This time is in
- milli-seconds. Threshold to
- activate usleep() in waiting
- methods (see wait()). This
- threshold is needed, because
- usleep() is not accurate on many
- machines. Call sleep() functions
- during the time to wait minus
- vpTime::minTimeForUsleepCall. The
- rest of the time to wait is managed
- by a while loop. */
-
-
-/*!
- Return the time in milliseconds since January 1st 1970.
-
- \sa measureTimeMicros(), measureTimeSecond()
-*/
-double
-vpTime::measureTimeMs()
-{
-#if defined(_WIN32)
- LARGE_INTEGER time, frequency;
- QueryPerformanceFrequency(&frequency);
- if(frequency.QuadPart == 0){
- return(timeGetTime());
- }
- else{
- QueryPerformanceCounter(&time);
- return (double)(1000.0*time.QuadPart/frequency.QuadPart);
- }
-#elif !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- struct timeval tp;
- gettimeofday(&tp,0);
- return(1000.0*tp.tv_sec + tp.tv_usec/1000.0);
-#endif
-}
-
-
-/*!
- Return the time in microseconds since January 1st 1970.
-
- \sa measureTimeMs(), measureTimeSecond()
-*/
-double
-vpTime::measureTimeMicros()
-{
-#if defined(_WIN32)
- LARGE_INTEGER time, frequency;
- QueryPerformanceFrequency(&frequency);
- if(frequency.QuadPart == 0){
- return(timeGetTime());
- }
- else{
- QueryPerformanceCounter(&time);
- return (double)(1000000.0*time.QuadPart/frequency.QuadPart);
- }
-#else
-
- struct timeval tp;
- gettimeofday(&tp,0);
- return(1000000.0*tp.tv_sec + tp.tv_usec);
-#endif
-}
-
-
-
-/*!
-
- Wait t miliseconds after t0 (in ms).
-
- The waiting is done by a call to usleep() if the time to wait is greater than
- vpTime::minTimeForUsleepCall.
-
- \param t0 : reference time (in ms)
- \param t : time to wait (in ms)
-
- \return 0 : The function did wait.
- \return 1 : The time was already over, no need to wait.
-*/
-
-int
-vpTime::wait(double t0, double t)
-{
- double timeCurrent, timeToWait;
- timeCurrent = measureTimeMs();
-
- timeToWait = t0 + t - timeCurrent;
-
- if ( timeToWait <= 0. ) // no need to wait
- return(1);
- else {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if (timeToWait > vpTime::minTimeForUsleepCall) {
- usleep((unsigned long )((timeToWait-vpTime::minTimeForUsleepCall)*1000));
- }
-#elif defined(_WIN32)
- if (timeToWait > vpTime::minTimeForUsleepCall) {
- Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
- }
-#endif
- // Blocking loop to have an accurate waiting
- do {
- timeCurrent = measureTimeMs();
- timeToWait = t0 + t - timeCurrent;
-
- } while (timeToWait > 0.);
-
- return 0;
- }
-}
-
-
-/*!
- Wait t miliseconds from now.
-
- The waiting is done by a call to usleep() if the time to wait is greater than
- vpTime::minTimeForUsleepCall.
-
- \param t : Time to wait in ms.
-
-*/
-void vpTime::wait(double t)
-{
- double t0, timeCurrent, timeToWait;
- t0 = timeCurrent = measureTimeMs();
-
- timeToWait = t;
-
- if ( timeToWait <= 0. ) // no need to wait
- return;
- else {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if (timeToWait > vpTime::minTimeForUsleepCall) {
- usleep((unsigned long )((timeToWait-vpTime::minTimeForUsleepCall)*1000));
- }
-#elif defined(_WIN32)
- if (timeToWait > vpTime::minTimeForUsleepCall) {
- Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
- }
-#endif
- // Blocking loop to have an accurate waiting
- do {
- timeCurrent = measureTimeMs();
- timeToWait = t0 + t - timeCurrent;
-
- } while (timeToWait > 0.);
-
- return;
- }
-}
-
-/*!
-
- Return the measured time in seconds since January 1st 1970.
-
- \sa measureTimeMs()
-*/
-double vpTime::measureTimeSecond()
-{
- return vpTime::measureTimeMs()/1000.0 ;
-}
-
-/*!
- Sleep t miliseconds from now.
-
- \param t : Time to sleep in ms.
-
-*/
-void vpTime::sleepMs(double t)
-{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep((unsigned long )(t*1000));
-#elif defined(_WIN32)
- Sleep((DWORD)(t));
-#endif
-}
-
-
diff --git a/src/tools/time/vpTime.h b/src/tools/time/vpTime.h
deleted file mode 100644
index b21b2f1..0000000
--- a/src/tools/time/vpTime.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTime.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Time management and measurement.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpTime_h
-#define vpTime_h
-
-/*!
- \file vpTime.h
- \brief Time management and measurement
-
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-
-/*!
- \class vpTime
- \ingroup Time
- \brief Time management and measurement.
-
- The example below shows how to synchronize a loop to a given framerate.
-
- \code
-#include <visp/vpTime.h>
-
-int main()
-{
- double t;
- for ( ; ; ) {
- t = vpTime::measureTimeMs();
- ...
- vpTime::wait(t, 40); // Loop time is set to 40 ms, ie 25 Hz
- }
-}
- \endcode
-
-*/
-
-class VISP_EXPORT vpTime
-{
-public:
- static double minTimeForUsleepCall;
-
- static double measureTimeSecond() ;
- static double measureTimeMs() ;
- static double measureTimeMicros() ;
- static void sleepMs(double t);
- static int wait(double t0, double t) ;
- static void wait(double t) ;
-} ;
-
-#endif
diff --git a/src/tools/trace/vpDebug.h b/src/tools/trace/vpDebug.h
deleted file mode 100644
index 4f3970c..0000000
--- a/src/tools/trace/vpDebug.h
+++ /dev/null
@@ -1,554 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDebug.h 4604 2014-01-21 14:15:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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.
- * - 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
- * 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
- *
- *****************************************************************************/
-
-#ifndef __VP_DEBUG_HH
-#define __VP_DEBUG_HH
-
-#include <visp/vpConfig.h>
-#include <stdio.h>
-#include <stdarg.h>
-#include <iostream>
-
-
-#if defined(_WIN32)
-# ifndef __FUNCTION__
-# define __FUNCTION__ " "
-# endif
-#endif
-
-#ifndef VP_DEBUG_MODE
-# define VP_DEBUG_MODE 0
-#endif
-
-
-/*!
- \class vpTraceOutput
-
- \ingroup Debug
-
- \brief This class is used to display debug or error messages.
-
- It needs to be initialized with the file name, function name and
- line, of the place where it is created. It is best used by first
- instanciating the object and directly calling the () operator. This
- is used to mimic variadic macros.
-
- This class is used to define the following macros:
-
- - Macros for tracing: vpTRACE(), vpERROR_TRACE(), vpIN_FCT() and
- vpOUT_FCT() work like printf
- with carreer return at the end of the string, while vpCTRACE() and
- vpCERROR() work like the C++ output streams std::cout and
- std::cerr.
-
- - Macros for debuging: vpDEBUG_TRACE(level) and vpDERROR_TRACE(level)
- work like printf, but print only if the tracing level \e level is
- greater than the debug level VP_DEBUG_MODE macro. vpCDEBUG(level)
- work like the C++ output stream std::cout. vpDEBUG_ENABLE(level) is
- equal to 1 if the debug level level is greater than the debug mode
- VP_DEBUG_MODE, 0 else.
-
- The example below shows how to use these macros.
-
- \code
-#define VP_TRACE // Activate the trace mode
-#define VP_DEBUG // Activate the debug mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- vpIN_FCT("main()");
-
- // Check the active debug levels
- std::cout << "Debug level 1 active: " << vpDEBUG_ENABLE(1) << std::endl;
- std::cout << "Debug level 2 active: " << vpDEBUG_ENABLE(2) << std::endl;
- std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
-
- // C-like debug printings
- vpTRACE("C-like trace"); // stdout
-
- // Printing depend only VP_DEBUG_MODE value is >= 1
- vpTRACE(1, "C-like trace level 1"); // stdout
- vpERROR_TRACE(1, "C-like error trace level 1"); // stderr
-
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
- vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
-
- // C++-like debug printings
- vpCTRACE << "C++-like trace" << std::endl; // stdout
- vpCERROR << "C++-like error trace" << std::endl; // stderr
-
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
-
- vpOUT_FCT("main()");
-}
- \endcode
-
-*/
-class vpTraceOutput
-{
- private:
- const char* currentFile; //Name of the file to use in the displays
- const char* currentFunc; //Name of the function to use in the displays
- int currentLine; //Line to use in the displays
-
- //if true, output to std::cerr/stderr else std::cout/stdout
- bool err;
- //string to display before anything else
- const char* header;
-
- public:
- /*!
- Constructor.
- \param file Should be the name of the file where this constructor was called.
- \param line Should be the line in file where this constructor was called.
- \param func Should be the name of the function where this constructor was called.
- \param error If true, writes to the error stream.
- \param s String to print before any other message (acts like a header).
- \note Call the constructor with something like vpTraceOutput(__FILE__,__LINE__, __FUNCTION__).
- */
- vpTraceOutput(const char* file, int line, const char* func, bool error=false, const char * s=NULL) :
- currentFile(file),
- currentFunc(func),
- currentLine(line),
- err(error),
- header(s)
- {}
-
- /*!
- Displays a string if the debug level is inferior to VP_DEBUG_MODE.
- \param level Level of this message.
- \param format Formating string.
- */
- void operator()(int level, const char* format, ...)
- {
- //if the level is inferior to VP_DEBUG_MODE
- if(VP_DEBUG_MODE >= level)
- {
- //gets the variable list of arguments
- va_list args;
- va_start(args, format);
-
- if (err)
- std::cerr << "(L" << level << ") " ;
- else
- std::cout << "(L" << level << ") " ;
-
- //calls display with it
- display(format, args);
-
- va_end(args);
- }
- }
-
- /*!
- Displays a string.
- \param format Formating string.
- */
- void operator()(const char* format, ...)
- {
- //gets the variable list of arguments
- va_list args;
- va_start(args, format);
-
-#ifdef VP_DEBUG
- std::cout<<"(L0) ";
-#endif
-
- //calls display with it
- display(format, args);
-
- va_end(args);
- }
-
- /*!
-
- Displays a message to either stdout or
- stderr (based on error boolean).
-
- \param format Formating string.
- \param args List of arguments.
-
- */
- void display(const char* format, va_list args)
- {
- //if we want to write to std::cerr/stderr
- if(err)
- {
- //first writes the header if there is one
- if(header != NULL) std::cerr<<header;
- //then writes the recorded namefile, function and line
- std::cerr << "!!\t" << currentFile << ": " <<currentFunc << "(#" << currentLine << ") : " ;
- //and finally writes the message passed to () operator.
- vfprintf (stderr, format, args);
- fprintf (stderr, "\n");
- //flushes the buffer
- fflush (stderr);
- }
- else
- {
- //first writes the header if there is one
- if(header != NULL) std::cout<<header;
- //then writes the recorded namefile, function and line
- std::cout <<currentFile << ": " << currentFunc << "(#" << currentLine << ") : " ;
- //and finally writes the message passed to () operator.
- vprintf (format, args);
- printf ("\n");
- //flushes the buffer
- fflush (stdout);
- }
- }
-
-};
-
-
-/* ------------------------------------------------------------------------- */
-/* --- vpTRACE IN/OUT FONCTION --------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-#ifdef VP_TRACE // Activate the trace mode
-
-/*!
- \ingroup Debug
- Works like vpTRACE() and should be used at the beginning of a function.
-
- \code
-#define VP_TRACE // To activate the trace mode
-#include <visp/vpDebug.h>
-
-int main()
-{
- vpIN_FCT("main()");
- // the body of the main() function
- vpOUT_FCT("main()");
-}
- \endcode
-
- \sa vpOUT_FCT
-*/
-#define vpIN_FCT (vpTraceOutput(__FILE__,__LINE__, __FUNCTION__, false, "begin "))
-
-
-/*!
- \ingroup Debug
- Works like vpTRACE() and should be used at the end of a function.
-
- \code
-#define VP_TRACE // To activate the trace mode
-#include <visp/vpDebug.h>
-
-int main()
-{
- vpIN_FCT("main()");
- // the body of the main() function
- vpOUT_FCT("main()");
-}
- \endcode
-
- \sa vpIN_FCT
-*/
-#define vpOUT_FCT (vpTraceOutput(__FILE__,__LINE__, __FUNCTION__, false, "end "))
-
-#else // #ifdef VP_TRACE
-
-inline void vpIN_FCT (const char * /* a */, ...){}
-inline void vpOUT_FCT (const char * /* a */, ...){}
-
-#endif // #ifdef VP_TRACE
-
-/* -------------------------------------------------------------------------- */
-/* --- vpTRACE -------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-#ifdef VP_TRACE
-
-/*!
- \ingroup Debug
- Used to display trace messages on the standard stream (C++).
- Use like this : vpCTRACE<<"my message"<<std::endl;
-
- \code
-#define VP_TRACE // To activate trace mode
-#define VP_DEBUG // To activate the debug mode
-#define VP_DEBUG_MODE 2 // To activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // C++-like debug printings
- vpCTRACE << "C++-like trace" << std::endl; // stdout
- vpCERROR << "C++-like error trace" << std::endl; // stderr
-
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
-}
- \endcode
-
- \sa vpTRACE(), vpCERROR(), vpCDEBUG()
-*/
-#define vpCTRACE std::cout << "(L0) " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
-
-
-/*!
- \ingroup Debug
- Used to display error messages on the error stream (C++).
- Use like this : vpCERROR<<"my message"<<std::endl;
-
- \code
-#define VP_TRACE // To activate trace mode
-#define VP_DEBUG // To activate the debug mode
-#define VP_DEBUG_MODE 2 // To activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // C++-like debug printings
- vpCTRACE << "C++-like trace" << std::endl; // stdout
- vpCERROR << "C++-like error trace" << std::endl; // stderr
-
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
-}
- \endcode
-
- \sa vpCTRACE(), vpCDEBUG()
-*/
-#define vpCERROR std::cerr << "(L0) " << "!!\t" << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
-
-/*!
- \ingroup Debug
- Used to display error messages on the error stream.
- Prints the name of the file, the function name and the line where
- it was used.
- Use like this : vpERROR_TRACE("my error message number %d", i);
- with any "printf" string.
-
- \code
-#define VP_TRACE // To activate trace mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // Printing depend only VP_DEBUG_MODE value is >= 1
- vpTRACE(1, "C-like trace level 1"); // stdout
- vpERROR_TRACE(1, "C-like error trace level 1"); // stderr
-}
- \endcode
-
- \sa vpTRACE()
-*/
-#define vpERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
-
-/*!
- \ingroup Debug
- Used to display trace messages on the standard stream.
- Prints the name of the file, the function name and the line where
- it was used.
- Use like this : vpTRACE("my debug message number %d", i);
- with any "printf" string.
-
- \code
-#define VP_TRACE // To activate trace mode
-#include <visp/vpDebug.h>
-
-int main()
-{
- // C-like debug printings
- vpTRACE("C-like trace"); // stdout
-}
- \endcode
-
- \sa vpCTRACE(), vpERROR_TRACE()
-*/
-#define vpTRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
-
-#else // #ifdef VP_TRACE
-
-#define vpCTRACE if(false) std::cout // Warning C4127
-#define vpCERROR if(false) std::cerr // Warning C4127
-
-inline void vpERROR_TRACE (const char * /* a */, ...){}
-inline void vpERROR_TRACE (int /* level */, const char * /* a */, ...){}
-inline void vpTRACE (const char * /* a */, ...){}
-inline void vpTRACE (int /* level */, const char * /* a */, ...){}
-
-#endif // #ifdef VP_TRACE
-
-/* ------------------------------------------------------------------------- */
-/* --- VP_DEBUG ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------- */
-
-#ifdef VP_DEBUG
-
-/*!
- \ingroup Debug
- vpDERROR_TRACE works like printf, but prints only if the
- tracing level is smaller than the debug level VP_DEBUG_MODE.
-
- \code
-#define VP_DEBUG // Activate the debug mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
- vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
-}
- \endcode
-
- \sa vpDEBUG_TRACE()
-*/
-#define vpDERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
-
-/*!
- \ingroup Debug
- vpDEBUG_TRACE works like printf, but prints only if the
- tracing level level is greater than the debug level VP_DEBUG_MODE.
-
- \code
-#define VP_DEBUG // Activate the debug mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpDEBUG_TRACE(2, "C-like debug trace level 2"); // stdout
- vpDERROR_TRACE(2, "C-like error trace level 2"); // stderr
-}
- \endcode
-
- \sa vpDERROR_TRACE()
-*/
-#define vpDEBUG_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
-
-/*!
- \ingroup Debug
- vpCDEBUG(level) work like the C++ output stream std::cout.
- \code
-#define VP_DEBUG // Activate the debug mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // C++-like debug printings
- vpCTRACE << "C++-like trace" << std::endl; // stdout
- vpCERROR << "C++-like error trace" << std::endl; // stderr
-
- // Printing if VP_DEBUG defined and VP_DEBUG_MODE value >= 2
- vpCDEBUG(2) << "C++-like debug trace level 2" << std::endl; // stdout
-}
- \endcode
-
- \sa vpCTRACE(), vpCERROR()
-*/
-#define vpCDEBUG(level) if (VP_DEBUG_MODE < level) ; else \
- std::cout << "(L" << level << ") "<< __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
-
-/*!
- \ingroup Debug
-
- vpDEBUG_ENABLE(level) is equal to 1 if the debug level \e level is greater than
- the debug mode VP_DEBUG_MODE, 0 else.
-
- \code
-#define VP_DEBUG // Activate the debug mode
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-
-#include <visp/vpDebug.h>
-
-int main()
-{
- // Check the active debug levels
- std::cout << "Debug level 1 active: " << vpDEBUG_ENABLE(1) << std::endl;
- std::cout << "Debug level 2 active: " << vpDEBUG_ENABLE(2) << std::endl;
- std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
-}
- \endcode
-*/
-#define vpDEBUG_ENABLE(level) (VP_DEBUG_MODE >= level)
-
-#else // #ifdef VP_DEBUG
-
-inline void vpDERROR_TRACE(const char * /* a */, ...){}
-inline void vpDEBUG_TRACE(const char * /* a */, ...){}
-inline void vpDERROR_TRACE(int /* level */, const char * /* a */, ...){}
-inline void vpDEBUG_TRACE(int /* level */, const char * /* a */, ...){}
-
-#define vpCDEBUG(level) if(false) std::cout // Warning C4127
-#define vpDEBUG_ENABLE(level) (false) // Warning C4127
-
-#endif // #ifdef VP_DEBUG
-
-/* -------------------------------------------------------------------------- */
-/* --- DEFENSIF ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-#ifdef VP_DEFENSIF
-#define DEFENSIF(a) (a)
-#else
-#define DEFENSIF(a) (0)
-#endif /*#ifdef DEFENSIF*/
-
-
-#endif /* #ifdef __DEBUG_HH */
diff --git a/src/tools/xml/vpXmlParser.cpp b/src/tools/xml/vpXmlParser.cpp
deleted file mode 100755
index 28a59ec..0000000
--- a/src/tools/xml/vpXmlParser.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParser.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tool to automatise the creation of xml parser based on the libXML2
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#include <visp/vpXmlParser.h>
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <visp/vpException.h>
-#include <visp/vpDebug.h>
-#include <libxml/parser.h>
-
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <typeinfo>
-
-/*!
- Basic constructor.
-
- Initialise the main tag with default value.
-*/
-vpXmlParser::vpXmlParser() : nodeMap(), main_tag("config")
-{
-}
-
-/*!
- Basic destructor that does nothing.
-
- \warning As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
- to clean up memory allocated by the xml2 library itself, the user should call
- xmlCleanupParser() only when the process has finished using the xml2 library.
- In case of doubt abstain from calling this function or do it just before calling exit()
- to avoid leak reports from valgrind ! That's why
- in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
- function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
- just before exit().
-
- \sa cleanup()
-*/
-vpXmlParser::~vpXmlParser()
-{
- //xmlCleanupParser();
-}
-
-/*!
- Cpoy constructor.
-
- \param _twin : The parser to copy.
-*/
-vpXmlParser::vpXmlParser(const vpXmlParser& _twin)
- : nodeMap(), main_tag("config")
-{
- main_tag = _twin.main_tag;
- nodeMap = _twin.nodeMap;
-}
-
-/* utilities functions to read/write data from an xml document */
-
-/*!
- Read an array of character.
-
- \warning The array of characters is allocated and must be explicitly freed to
- avoid memory leak.
-
- \param doc : The main xml document
- \param node : a pointer to the node to read value
-
- \return pointer to an allocated array of character.
-*/
-char*
-vpXmlParser::xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node)
-{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read char*";
- std::cerr << errorMsg << std::endl;
- throw vpException(vpException::fatalError, errorMsg);
- }
- return (char *) xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
-}
-
-/*!
- Read an array of character.
-
-
- \param doc : The main xml document
- \param node : a pointer to the node to read value
-
- \return std::string representing the value.
-*/
-
-std::string
-vpXmlParser::xmlReadStringChild (xmlDocPtr doc, xmlNodePtr node)
-{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read std::string";
- std::cerr << errorMsg << std::endl;
- throw vpException(vpException::fatalError, errorMsg);
- }
- char* dataCh = (char*)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
- std::string dataStr = dataCh;
- xmlFree(dataCh);
- return dataStr;
-}
-
-/*!
- read an int
-
- \warning throw a vpException::ioError if the value cannot be parsed to an integer
-
- \param doc : The main xml document
- \param node : a pointer to the node to read value
-
- \return the integer value in the node
-*/
-int
-vpXmlParser::xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node)
-{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read int";
- std::cerr << errorMsg << std::endl;
- throw vpException(vpException::fatalError, errorMsg);
- }
- char * val_char;
- char * control_convert;
- int val_int;
-
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_int = strtol ((char *)val_char, &control_convert, 10);
-
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
- throw vpException(vpException::ioError, "cannot parse entry to int");
- }
- xmlFree((xmlChar*) val_char);
-
- return val_int;
-}
-
-/*!
- read an int
-
- \warning throw a vpException::ioError if the value cannot be parsed to an integer
-
- \param doc : The main xml document
- \param node : a pointer to the node to read value
-
- \return the unsigned integer value in the node
-*/
-unsigned int
-vpXmlParser::xmlReadUnsignedIntChild (xmlDocPtr doc, xmlNodePtr node)
-{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read unsigned int";
- std::cerr << errorMsg << std::endl;
- throw vpException(vpException::fatalError, errorMsg);
- }
- char * val_char;
- char * control_convert;
- unsigned int val_uint;
-
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_uint = strtoul ((char *)val_char, &control_convert, 10);
-
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
- throw vpException(vpException::ioError, "cannot parse entry to int");
- }
- xmlFree((xmlChar*) val_char);
-
- return val_uint;
-}
-
-
-/*!
- read a double
-
- \warning throw a vpException::ioError if the value cannot be parsed to an integer
-
- \param doc : The main xml document
- \param node : a pointer to the node to read value
-
- \return the double value in the node
-*/
-double
-vpXmlParser::xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node)
-{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read double";
- std::cerr << errorMsg << std::endl;
- throw vpException(vpException::fatalError, errorMsg);
- }
- char * val_char;
- char * control_convert;
- double val_double;
-
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_double = strtod ((char *)val_char, &control_convert);
-
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
- throw vpException(vpException::ioError, "cannot parse entry to double");
- }
- xmlFree((xmlChar*) val_char);
- return val_double;
-}
-
-/*!
- write an array of character.
-
- \param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
- \param value : pointer to the array of character to write
-*/
-void
-vpXmlParser::xmlWriteCharChild(xmlNodePtr node, const char* label, const char* value)
-{
- xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)value);
- xmlAddChild(node, tmp);
-}
-
-/*!
- write an array of character.
-
- \param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
- \param value : std::string to write;
-*/
-void
-vpXmlParser::xmlWriteStringChild(xmlNodePtr node, const char* label, const std::string& value)
-{
- xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)value.c_str());
- xmlAddChild(node, tmp);
-}
-
-
-/*!
- write an integer.
-
- \param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
- \param value : integer to write
-*/
-void
-vpXmlParser::xmlWriteIntChild(xmlNodePtr node, const char* label, const int value)
-{
- char str[100];
- sprintf(str, "%d", value);
- xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
-}
-
-/*!
- write an unsigned integer.
-
- \param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
- \param value : unsigned integer to write
-*/
-void
-vpXmlParser::xmlWriteUnsignedIntChild(xmlNodePtr node, const char* label, const unsigned int value)
-{
- char str[100];
- sprintf(str, "%u", value);
- xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
-}
-
-
-/*!
- write a double.
-
- \param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
- \param value : double to write
-*/
-void
-vpXmlParser::xmlWriteDoubleChild(xmlNodePtr node, const char* label, const double value)
-{
- char str[100];
- sprintf(str, "%lf", value);
- xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
-}
-
-
-/* -------------------------------------------------------------------------- */
-/* MAIN METHODS */
-/* -------------------------------------------------------------------------- */
-
-/*!
- parse the document.
- The data in the file are stored in the attributes of the child class. This
- method calls the readMainClass method which has to be implemented for every
- child class depending on the content to parse.
-
- \param filename : name of the file to parse
-*/
-void
-vpXmlParser::parse(const std::string& filename)
-{
- xmlDocPtr doc;
- xmlNodePtr root_node;
-
- doc = xmlParseFile(filename.c_str());
- if(doc == NULL){
- vpERROR_TRACE("cannot open file");
- throw vpException(vpException::ioError, "cannot open file");
- }
-
- root_node = xmlDocGetRootElement(doc);
- if(root_node == NULL){
- vpERROR_TRACE("cannot get root element");
- throw vpException(vpException::ioError, "cannot get root element");
- }
-
- readMainClass(doc, root_node);
-
- xmlFreeDoc(doc);
-}
-
-/*!
- Save the content of the class in the file given in parameters.
- The data of the class are in the child class.
- This method calls the write_main_class method which has to be implemented for
- every class depending on the data to save.
-
- \param filename : the name of the file used to record the data
- \param append : if true and if the file exists, the data will be added to the
- data already in the file
-*/
-void
-vpXmlParser::save(const std::string& filename, const bool append)
-{
- xmlDocPtr doc;
- xmlNodePtr root_node;
-
- doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR + XML_PARSE_NOBLANKS);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
- xmlDocSetRootElement(doc, root_node);
- }
- else{
- if(!append){
- xmlFreeDoc(doc);
- if (remove(filename.c_str()) != 0)
- throw vpException(vpException::ioError, "Cannot remove existing xml file");
-
- doc = xmlNewDoc ((xmlChar*)"1.0");
- root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
- xmlDocSetRootElement(doc, root_node);
- }
- }
-
- root_node = xmlDocGetRootElement(doc);
- if (root_node == NULL)
- {
- vpERROR_TRACE("problem to get the root node");
- throw vpException(vpException::ioError, "problem to get the root node");
- }
-
- writeMainClass(root_node);
-
- xmlSaveFormatFile(filename.c_str(), doc, 1);
- xmlFreeDoc(doc);
-}
-
-
-
-
-#endif
-
-
-
-
diff --git a/src/tools/xml/vpXmlParser.h b/src/tools/xml/vpXmlParser.h
deleted file mode 100755
index 76d6bff..0000000
--- a/src/tools/xml/vpXmlParser.h
+++ /dev/null
@@ -1,311 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpXmlParser.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tools to automatise the creation of xml parser based on the libXML2
- *
- * Authors:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-#ifndef vpXmlParser_HH
-#define vpXmlParser_HH
-
-/*!
- \file vpXmlParser.h
- \brief Tools to simplify the creation of xml parser based on the libXML2
-*/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <visp/vpException.h>
-
-#include <libxml/parser.h>
-
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <typeinfo>
-#include <map>
-#include <string.h>
-
-
-/*!
- \class vpXmlParser
-
- \brief This class intends to simplify the creation of xml parser based on the
- libxml2 third party library.
-
- This class can be useful to manage external data parameters (for example for
- configuration of an experiment, ...).
-
- \warning This class is only available if libxml2 is installed and detected by ViSP.
-
- In order to use this class, you have to create a new class inheriting from this one.
- In the child class, you have to implement the methods:
- - writeMainClass()
- - readMainClass()
-
- These two methods depends on the data to parse, and must not be directly called
- (they are called from the parse() and the save() methods).
-
- Following is an example of implementation for the document:
-
- \code
- <config>
- <range>5</range>
- <step>7</step>
- <size_filter>3</size_filter>
- </config>
- \endcode
-
- A class to parse this document is declared as follows:
-
- \code
- class vpDataParser: public vpXmlParser
- {
- private:
- int m_range;
- int m_step;
- int m_size_filter
- public:
- typedef enum{
- config,
- range,
- step,
- size_filter
- }dataToParse
-
- vpDataParser(){
- nodeMap["config"] = config;
- nodeMap["range"] = range;
- nodeMap["step"] = step;
- nodeMap["size_filter"] = size_filter;
- }
-
- virtual void writeMainClass(xmlNodePtr node);
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
-
- // additionals methods specific to the data to parse
- // such as: accessors
- }
- \endcode
-
- The readMainClass function implementation is:
-
- \code
- void
- vpDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
- {
- for (xmlNodePtr tmpNode = node->xmlChildrenNode; tmpNode != NULL; tmpNode = tmpNode->next) {
- if(tmpNode->type == XML_ELEMENT_NODE){
-
- std::map<std::string, int>::iterator iter= this->nodeMap.find((char*)tmpNode->name);
- if(iter == nodeMap.end()){
- continue;
- }
-
- switch (iter->second){
- case range:
- this->m_range = xmlReadIntChild(doc, tmpNode);
- break;
- case step:
- this->m_step = xmlReadIntChild(doc, tmpNode);
- break;
- case size_filter:
- this->m_size_filter = xmlReadIntChild(doc, tmpNode);
- break;
- default:
- std::cout << "problem in the readMainClass (" << iter->second << " , " << iter->first << " )" << std::endl;
- break;
- }
- }
- }
- }
- \endcode
-
- Data can now be accessed through the internal variables of the class vpDataParser.
-
- To store the data in a xml file, the function save has to be called. This
- function needs the implementation of the writeMainClass function.
-
- For example,
-
- \code
- void
- vpDataParser::writeMainClass(xmlNodePtr node)
- {
- xmlWriteIntChild(node, "range", m_range);
- xmlWriteIntChild(node, "step", m_step);
- xmlWriteIntChild(node, "size_filter", m_size_filter);
- }
- \endcode
-
-*/
-class VISP_EXPORT vpXmlParser
-{
-protected:
- std::string xmlReadStringChild (xmlDocPtr doc, xmlNodePtr node);
- char* xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node);
- int xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node);
- unsigned int xmlReadUnsignedIntChild (xmlDocPtr doc, xmlNodePtr node);
- double xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node);
-
-
- void xmlWriteStringChild(xmlNodePtr node, const char* label, const std::string& value);
- void xmlWriteCharChild(xmlNodePtr node, const char* label, const char* value);
- void xmlWriteIntChild(xmlNodePtr node, const char* label, const int value);
- void xmlWriteUnsignedIntChild(xmlNodePtr node, const char* label, const unsigned int value);
- void xmlWriteDoubleChild(xmlNodePtr node, const char* label, const double value);
-
- /*!
- The map describing the data to parse
- */
- std::map<std::string, int> nodeMap;
-
- /*!
- The name of the main tag for the file to parse
- */
- std::string main_tag;
-
-public:
-
- vpXmlParser();
- vpXmlParser(const vpXmlParser& _twin);
- virtual ~vpXmlParser();
-
- /*!
- As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
- to clean up memory allocated by the xml2 library itself, the user should call
- xmlCleanupParser() only when the process has finished using the xml2 library.
- In case of doubt abstain from calling this function or do it just before calling exit()
- to avoid leak reports from valgrind ! That's why
- in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
- function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
- just before exit().
- */
- static void cleanup()
- {
- xmlCleanupParser();
- }
-
- /* virtual */ void parse(const std::string& filename);
- /* virtual */ void save(const std::string& filename, const bool append=false);
-
- /*!
- Set the map describing the data to parse. This map stores the name of each
- node and an associated key used to simplify the parsing of the file.
-
- If the following file want to be parsed:
-
- \code
- <config>
- <range>5</range>
- <step>7</step>
- <size_filter>3</size_filter>
- </config>
- \endcode
-
- The following map has to be declared:
-
- \code
- std::map dataToParse;
- dataToParse["config"] = 0;
- dataToParse["range"] = 1;
- dataToParse["step"] = 2;
- dataToParse["size_filter"] = 3;
- \endcode
-
- Or, you can use keyzord instead of number as key but it implies to declare
- in the child class an enumeration type of the name. For example:
-
- \code
- typedef enum{
- config,
- range,
- step,
- size_filter} data_enum;
-
- std::map dataToParse;
- dataToParse["config"] = config;
- dataToParse["range"] = range;
- dataToParse["step"] = step;
- dataToParse["size_filter"] = size_filter;
- \endcode
-
- \param _map : the map describing the data to parse
- */
- void setMap(const std::map<std::string, int>& _map){ nodeMap = _map;}
-
- /*!
- set the name of the main tag
-
- The main tag corresponds to the name of the root node
-
- \param tag : name of the root node of the document
- */
- inline void setMainTag(const std::string& tag){ main_tag = tag;}
-
-protected:
- /*!
- pure virtual method used to read the document.
-
- As the content of the function depends on the structure of the file to read,
- data name, data types and data values, it has to be reimplemented for every
- type of filenam
-
- \param doc : a pointer representing the document
- \param node : the root node of the document
- */
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node)=0;
-
- /*!
- pure virtual method used to write the document.
-
- As the content of the function depends on the structure of the file to read,
- data name and data types, it has to be reimplemented for every
- type of file to parse.
-
- \param node : the root node of the document
- */
- virtual void writeMainClass(xmlNodePtr node)=0;
-
-};
-
-
-#endif /* VISP_HAVE_XML2 */
-
-#endif
diff --git a/src/tracking/dots/vpDot.cpp b/src/tracking/dots/vpDot.cpp
deleted file mode 100644
index 2a926ff..0000000
--- a/src/tracking/dots/vpDot.cpp
+++ /dev/null
@@ -1,972 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDot.cpp 4943 2014-11-03 13:51:09Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Track a white dot.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*
- \file vpDot.cpp
- \brief Track a white dot
-*/
-
-#include <visp/vpDot.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpColor.h>
-#include <visp/vpTrackingException.h>
-
-#include <vector>
-
-/*
- \class vpDot
- \brief Track a white dot
-*/
-
-/* spiral size for the dot search */
-const unsigned int vpDot::SPIRAL_SEARCH_SIZE = 350;
-
-/*!
-
- Initialize the tracker with default parameters.
- - connexityType is set to 4 (see setConnexityType())
- - dot maximal size is set to 25% of the image size (see setMaxDotSize())
-*/
-void vpDot::init()
-{
- cog.set_u(0);
- cog.set_v(0);
-
- compute_moment = false ;
- graphics = false ;
- thickness = 1;
- maxDotSizePercentage = 0.25 ; // 25 % of the image size
-
- mean_gray_level = 0;
- gray_level_min = 128;
- gray_level_max = 255;
- grayLevelPrecision = 0.85;
- gamma = 1.5 ;
-
- m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu02 = mu20 = 0 ;
-
- connexityType = CONNEXITY_4;
-
- u_min = u_max = v_min = v_max = 0;
-
- gray_level_out = 0;
- nbMaxPoint = 0;
-}
-
-vpDot::vpDot()
- : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
- cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
- gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
- gamma(1.5), compute_moment(false), nbMaxPoint(0)
-{
-}
-
-/*!
- \brief Constructor with initialization of the dot location.
-
- \param ip : An image point with sub-pixel coordinates.
- */
-vpDot::vpDot(const vpImagePoint &ip)
- : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
- cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
- gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
- gamma(1.5), compute_moment(false), nbMaxPoint(0)
-{
- cog = ip;
-}
-
-/*!
- \brief Copy constructor.
- */
-vpDot::vpDot(const vpDot& d)
- : vpTracker(d),
- m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
- cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
- gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
- gamma(1.5), compute_moment(false), nbMaxPoint(0)
-{
- *this = d ;
-}
-
-/*!
- \brief Destructor.
- */
-vpDot::~vpDot()
-{
- ip_connexities_list.clear() ;
-}
-
-/*!
- \brief Copy operator.
- */
-vpDot&
-vpDot::operator=(const vpDot& d)
-{
- ip_edges_list = d.ip_edges_list;
- ip_connexities_list = d.ip_connexities_list;
- connexityType = d.connexityType;
- cog = d.getCog();
-
- u_min = d.u_min;
- v_min = d.v_min;
- u_max = d.u_max;
- v_max = d.v_max;
-
- graphics = d.graphics ;
- thickness = d.thickness;
- maxDotSizePercentage = d.maxDotSizePercentage;
- gray_level_out = d.gray_level_out;
- mean_gray_level = d.mean_gray_level ;
- gray_level_min = d.gray_level_min ;
- gray_level_max = d.gray_level_max ;
- grayLevelPrecision = d.grayLevelPrecision;
- gamma = d.gamma;
- compute_moment = d.compute_moment ;
- nbMaxPoint = d.nbMaxPoint;
-
- m00 = d.m00;
- m01 = d.m01;
- m10 = d.m10;
- m11 = d.m11;
- m02 = d.m02;
- m20 = d.m20;
-
- mu11 = d.mu11;
- mu20 = d.mu20;
- mu02 = d.mu02;
-
- return *this ;
-}
-
-bool
-vpDot::operator!=(const vpDot& d)
-{
- return ( cog != d.getCog() );
-}
-
-bool
-vpDot::operator==(const vpDot& d)
-{
- return ( cog == d.getCog() );
-}
-
-/*!
-
- This method choose a gray level (default is 0) used to modify the
- "in" dot level in "out" dot level. This gray level is here needed to
- stop the recursivity . The pixels of the dot are set to this new
- gray level "\out\" when connexe() is called.
-
- \exception vpTrackingException::initializationError : No valid gray
- level "out" can be found. This means that the min gray level is set to 0
- and the max gray level to 255. This should not occur
-*/
-void
-vpDot::setGrayLevelOut()
-{
- if (gray_level_min == 0) {
- if (gray_level_max == 255) {
- // gray_level_min = 0 and gray_level_max = 255: this should not occur
- //vpERROR_TRACE("Unable to choose a good \"out\" level") ;
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Unable to choose a good \"out\" level")) ;
- }
- gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
- }
-}
-
-/*!
- Perform the tracking of a dot by connex components.
-
- \param mean_value : Threshold to use for the next call to track()
- and corresponding to the mean value of the dot intensity.
-
- \warning The content of the image is modified thanks to
- setGrayLevelOut() called before. This method choose a gray level
- (default is 0) used to modify the "in" dot level in "out" dot
- level. This gray level is here needed to stop the recursivity . The
- pixels of the dot are set to this new gray level "\out\".
-
- \return vpDot::out if an error occurs, vpDot::in otherwise.
-
- \sa setGrayLevelOut()
-*/
-
-bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n)
-{
- std::vector<bool> checkTab(I.getWidth()*I.getHeight(),false);
- return connexe(I,u,v,mean_value,u_cog,v_cog,n,checkTab);
-}
-/*!
- Perform the tracking of a dot by connex components.
-
- \param mean_value : Threshold to use for the next call to track()
- and corresponding to the mean value of the dot intensity.
-
- \warning The content of the image is modified thanks to
- setGrayLevelOut() called before. This method choose a gray level
- (default is 0) used to modify the "in" dot level in "out" dot
- level. This gray level is here needed to stop the recursivity . The
- pixels of the dot are set to this new gray level "\out\".
-
- \return vpDot::out if an error occurs, vpDot::in otherwise.
-
- \sa setGrayLevelOut()
-*/
-
-bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab)
-{
-
- unsigned int width = I.getWidth();
- unsigned int height= I.getHeight();
-
- // Test if we are in the image
- if ( (u >= width) || (v >= height) )
- {
- //std::cout << "out of bound" << std::endl;
- return false;
- }
-
- if(checkTab[u + v*I.getWidth()])
- return true;
-
- vpImagePoint ip;
- ip.set_u(u);
- ip.set_v(v);
-
- if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
- {
- checkTab[v*I.getWidth() + u] = true;
-
- ip_connexities_list.push_back(ip);
-
- u_cog += u ;
- v_cog += v ;
- 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() "
- "method.",
- n, n / (I.getWidth() * I.getHeight()),
- nbMaxPoint, maxDotSizePercentage)) ;
- }
-
- // Bounding box update
- if (u < this->u_min) this->u_min = u;
- if (u > this->u_max) this->u_max = u;
- if (v < this->v_min) this->v_min = v;
- if (v > this->v_max) this->v_max = v;
-
- // Mean value of the dot intensities
- mean_value = (mean_value *(n-1) + I[v][u]) / n;
- if (compute_moment==true)
- {
- m00++ ;
- m10 += u ;
- m01 += v ;
- m11 += (u*v) ;
- m20 += u*u ;
- m02 += v*v ;
- }
- }
- else
- {
- //std::cout << "not in" << std::endl;
- return false;
- }
-
- bool edge = false;
-
- //if((int)u-1 >= 0)
- if(u >= 1)
- if(!checkTab[u-1 + v*I.getWidth()])
- if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
- edge = true;
-
- if(u+1 < I.getWidth())
- if(!checkTab[u+1+v*I.getWidth()])
- if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v >= 1)
- if(!checkTab[u+(v-1)*I.getWidth()])
- if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v+1 < I.getHeight())
- if(!checkTab[u+(v+1)*I.getWidth()])
- if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if (connexityType == CONNEXITY_8) {
- if(v >= 1 && u >= 1)
- if(!checkTab[u-1+(v-1)*I.getWidth()])
- if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v >= 1 && u+1 < I.getWidth())
- if(!checkTab[u+1+(v-1)*I.getWidth()])
- if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v+1 < I.getHeight() && u >= 1)
- if(!checkTab[u-1+(v+1)*I.getWidth()])
- if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v+1 < I.getHeight() && u+1 < I.getWidth())
- if(!checkTab[u+1+(v+1)*I.getWidth()])
- if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
- }
-
- if(edge){
- ip_edges_list.push_back(ip);
- if (graphics==true)
- {
- vpImagePoint ip_(ip);
- for(unsigned int t=0; t<thickness; t++) {
- ip_.set_u(ip.get_u() + t);
- vpDisplay::displayPoint(I, ip_, vpColor::red) ;
- }
- //vpDisplay::flush(I);
- }
- }
-
- return true;
-}
-
-/*!
-
- Compute the center of gravity (COG) of the dot using connex
- components. We assume the origin pixel (u, v) is in the dot. If
- not, the dot is seach around this origin using a spiral search.
-
- \param I : Image to process.
- \param u : Starting pixel coordinate along the columns from where the
- dot is searched .
-
- \param v : Starting pixel coordinate along the rows from where the
- dot is searched .
-
- \warning The content of the image is modified.
-
- \exception vpTrackingException::featureLostError : If the tracking fails.
-
- \sa connexe()
-*/
-void
-vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
-{
- // Set the maximal number of points considering the maximal dot size
- // image percentage
- nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
-
- // segmentation de l'image apres seuillage
- // (etiquetage des composante connexe)
- if (compute_moment)
- m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;
-
- double u_cog = 0 ;
- double v_cog = 0 ;
- double npoint = 0 ;
- this->mean_gray_level = 0 ;
-
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- // Initialise the boundig box
- this->u_min = I.getWidth();
- this->u_max = 0;
- this->v_min = I.getHeight();
- this->v_max = 0;
-
-#if 0
- // Original version
- if ( connexe(I, (unsigned int)u, (unsigned int)v,
- gray_level_min, gray_level_max,
- mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
- {
- bool sol = false ;
- unsigned int pas ;
- for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
- {
- for (int k=-1 ; k <=1 ; k++) if (sol==false)
- for (int l=-1 ; l <=1 ; l++) if (sol==false)
- {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
-
- this->mean_gray_level = 0 ;
- if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
- gray_level_min, gray_level_max,
- mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
- {
- sol = true ; u += k*pas ; v += l*pas ;
- }
- }
- }
- if (sol == false)
- {
- //vpERROR_TRACE("Dot has been lost") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "Dot has been lost")) ;
- }
- }
-#else
- // If the dot is not found, search around using a spiral
- if ( !connexe(I,(unsigned int)u,(unsigned int)v, mean_gray_level, u_cog, v_cog, npoint) )
- {
- bool sol = false ;
-
- unsigned int right = 1;
- unsigned int botom = 1;
- unsigned int left = 2;
- unsigned int up = 2;
- double u_ = u, v_ = v;
- unsigned int k;
-
- // Spiral search from the center to find the nearest dot
- while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
- for (k=1; k <= right; k++) if(sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
- if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
- sol = true; u = u_+k; v = v_;
- }
- }
- u_ += k;
- right += 2;
-
- for (k=1; k <= botom; k++) if (sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
- sol = true; u = u_; v = v_+k;
- }
- }
- v_ += k;
- botom += 2;
-
- for (k=1; k <= left; k++) if (sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),mean_gray_level,u_cog, v_cog, npoint) ) {
- sol = true ; u = u_-k; v = v_;
- }
- }
- u_ -= k;
- left += 2;
-
- for (k=1; k <= up; k++) if(sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),mean_gray_level,u_cog, v_cog, npoint) ) {
- sol = true ; u = u_; v = v_-k;
- }
- }
- v_ -= k;
- up += 2;
- }
-
- if (sol == false) {
- //vpERROR_TRACE("Dot has been lost") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "Dot has been lost")) ;
- }
- }
-
-#endif
-/*
- vpImagePoint ip;
- unsigned int i, j;
- std::list<vpImagePoint>::iterator it;
- for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it ++) {
- ip = *it;
- i = (unsigned int) ip.get_i();
- j = (unsigned int) ip.get_j();
- I[i][j] = 255 ;
- }*/
-
- u_cog = u_cog/npoint ;
- v_cog = v_cog/npoint ;
-
- u = u_cog ;
- v = v_cog ;
-
- // Initialize the threshold for the next call to track()
- double Ip = pow((double)this->mean_gray_level/255,1/gamma);
-
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
-
- //vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
- //vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
-
- if (npoint < 5)
- {
- //vpERROR_TRACE("Dot to small") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "Dot to small")) ;
- }
-
- 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 using the setMaxDotSize() method.",
- npoint, npoint / (I.getWidth() * I.getHeight()),
- nbMaxPoint, maxDotSizePercentage)) ;
- }
-}
-
-/*!
- Maximal size of the region to track in terms of image size percentage.
-
- \param percentage : Image size percentage corresponding to the dot
- maximal size. Values should be in ]0 : 1]. If 1, that means that the
- dot to track could take up the whole image.
-
- During the tracking, if the dot size if bigger than the maximal size
- allowed an exception is throwed :
- vpTrackingException::featureLostError.
-
-*/
-void
-vpDot::setMaxDotSize(double percentage)
-{
- if (percentage <= 0.0 || percentage > 1.0) {
- // print a warning. We keep the default percentage
- vpTRACE("Max dot size percentage is requested to be set to %lf.",
- "Value should be in ]0:1]. Value will be set to %lf.",
- percentage, maxDotSizePercentage);
- }
- else {
- maxDotSizePercentage = percentage;
- }
-}
-
-/*!
-
- Initialize the tracking with a mouse click and update the dot
- characteristics (center of gravity, moments).
-
- Wait a user click in a gray area in the image I. The clicked pixel
- will be the starting point from which the dot will be tracked.
-
- The threshold used to segment the dot is set with the grayLevelPrecision
- parameter. See the formula in setGrayLevelPrecision() function.
-
- The sub pixel coordinates of the dot are updated. To get the center
- of gravity coordinates of the dot, use getCog(). To
- compute the moments use setComputeMoments(true) before a call to
- initTracking().
-
- \warning The content of the image modified since we call track() to
- compute the dot characteristics.
-
- \param I : Image to process.
-
- \sa track(), getCog()
-*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I)
-{
- while (vpDisplay::getClick(I, cog) != true) ;
-
- unsigned int i = (unsigned int)cog.get_i();
- unsigned int j = (unsigned int)cog.get_j();
-
- double Ip = pow((double)I[i][j]/255, 1/gamma);
-
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
-
- try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
- }
-}
-
-/*!
-
- Initialize the tracking for a dot supposed to be located at (u,v) and
- updates the dot characteristics (center of gravity, moments).
-
- The threshold used to segment the dot is set to 80 percent of the
- gray level of the pixel (u,v).
-
- The sub pixel coordinates of the dot are updated. To get the center
- of gravity coordinates of the dot, use getCog(). To
- compute the moments use setComputeMoments(true) before a call to
- initTracking().
-
- \warning The content of the image modified since we call track() to
- compute the dot characteristics.
-
- \param I : Image to process.
-
- \param ip : Location of the starting point from which the dot will be
- tracked in the image.
-
- \sa track()
-*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip)
-{
-
- cog = ip ;
-
- unsigned int i = (unsigned int)cog.get_i();
- unsigned int j = (unsigned int)cog.get_j();
-
- double Ip = pow((double)I[i][j]/255, 1/gamma);
-
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
- try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
- }
-}
-
-/*!
-
- Initialize the tracking for a dot supposed to be located at (u,v) and
- updates the dot characteristics (center of gravity, moments).
-
- The sub pixel coordinates of the dot are updated. To get the center
- of gravity coordinates of the dot, use getCog(). To
- compute the moments use setComputeMoments(true) before a call to
- initTracking().
-
- \warning The content of the image modified since we call track() to
- compute the dot characteristics.
-
- \param I : Image to process.
-
- \param ip : Location of the starting point from which the dot will
- be tracked in the image.
-
- \param level_min : Minimum gray level threshold used to segment the dot;
- value comprised between 0 and 255.
-
- \param level_max : Maximum gray level threshold used to segment the
- dot; value comprised between 0 and 255. \e gray_level_max should be
- greater than \e gray_level_min.
-
- \sa track(), getCog()
-*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int level_min, unsigned int level_max)
-{
-
- cog = ip ;
-
- this->gray_level_min = level_min;
- this->gray_level_max = level_max;
-
- try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
- }
-}
-
-
-/*!
- Track and compute the dot characteristics.
-
- To get the center of gravity coordinates of the dot, use
- getCog(). To compute the moments use setComputeMoments(true) before
- a call to initTracking().
-
- \warning The image is modified (all the pixels that belong to the point
- are set to white (ie to 255).
-
- \param I : Image to process.
-
- \sa getCog()
-*/
-void
-vpDot::track(const vpImage<unsigned char> &I)
-{
- try{
- setGrayLevelOut();
- double u = this->cog.get_u();
- double v = this->cog.get_v();
-
- COG( I, u, v ) ;
-
- this->cog.set_u( u );
- this->cog.set_v( v );
-
- if (compute_moment==true)
- {
- mu11 = m11 - u*m01;
- mu02 = m02 - v*m01;
- mu20 = m20 - u*m10;
- }
-
- if (graphics) {
- // display a red cross at the center of gravity's location in the image.
- vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
- }
-
- }
- catch(vpException &e)
- {
- throw(e) ;
- }
-}
-
-/*!
-
- Track and updates the new dot coordinates
-
- To compute the moments use setComputeMoments(true) before a call to
- initTracking() or track().
-
- \warning The image is modified (all the pixels that belong to the point
- are set to white (ie to 255).
-
- \param I : Image to process.
-
- \param ip [out] : Sub pixel coordinate of the tracked dot center of gravity.
-*/
-void
-vpDot::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
-{
- track( I ) ;
-
- ip = this->cog;
-}
-
-/*!
- Display in overlay the dot edges and center of gravity.
-
- \param I : Image.
- \param color : The color used for the display.
- \param thick : Thickness of the displayed cross located at the dot cog.
-*/
-void vpDot::display(const vpImage<unsigned char>& I, vpColor color, unsigned int thick) const
-{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thick);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-/*!
-
- Set the precision of the gray level of the dot.
-
- \param precision : It is a double precision float which value is
- in ]0,1]:
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
- - Values lower or equal to 0 are brought back to an epsion>0
- - Values higher than 1 are brought back to 1
- If the initial gray level is I, the gray levels of the dot will be between :
- \f$Imin=255*\big((\frac{I}{255})^{{\gamma}^{-1}}-(1-grayLevelPrecision)\big)^{\gamma}\f$
- and
- \f$Imax=255*\big((\frac{I}{255})^{{\gamma}^{-1}}+(1-grayLevelPrecision)\big)^{\gamma}\f$
- with \f$\gamma=1.5\f$ .
-
- \sa setWidth(), setHeight(), setGrayLevelMin(), setGrayLevelMax()
-*/
-void vpDot::setGrayLevelPrecision( const double & precision )
-{
- double epsilon = 0.05;
- if( grayLevelPrecision<epsilon )
- {
- this->grayLevelPrecision = epsilon;
- }
- else if( grayLevelPrecision>1 )
- {
- this->grayLevelPrecision = 1.0;
- }
- else
- {
- this->grayLevelPrecision = precision;
- }
-}
-
-/*!
-
- Display the dot center of gravity and its list of edges.
-
- \param I : The image used as background.
-
- \param cog : The center of gravity.
-
- \param edges_list : The list of edges;
-
- \param color : Color used to display the dot.
-
- \param thickness : Thickness of the dot.
-*/
-void vpDot::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
-{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-/*!
-
- Display the dot center of gravity and its list of edges.
-
- \param I : The image used as background.
-
- \param cog : The center of gravity.
-
- \param edges_list : The list of edges;
-
- \param color : Color used to display the dot.
-
- \param thickness : Thickness of the dot.
-*/
-void vpDot::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
-{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-/*!
- Writes the dot center of gravity coordinates in the frame (i,j) (For more details
- about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
- and returns a reference to the stream.
-*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d) {
- return (os << "(" << d.getCog() << ")" ) ;
-} ;
-
-
diff --git a/src/tracking/dots/vpDot.h b/src/tracking/dots/vpDot.h
deleted file mode 100644
index 558deee..0000000
--- a/src/tracking/dots/vpDot.h
+++ /dev/null
@@ -1,425 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDot.h 4943 2014-11-03 13:51:09Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Track a white dot.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpDot.h
- \brief Track a white dot
-*/
-
-#ifndef vpDot_hh
-#define vpDot_hh
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpTracker.h>
-#include <visp/vpRect.h>
-#include <visp/vpImagePoint.h>
-
-#include <math.h>
-#include <fstream>
-#include <list>
-#include <vector>
-
-#ifdef VISP_USE_MSVC
-# pragma comment(linker, "/STACK:256000000") // Increase max recursion depth
-#endif
-
-/*!
- \class vpDot
-
- \ingroup TrackingImageBasic
-
- \brief This tracker is meant to track a dot (connected pixels with same
- gray level) on a vpImage.
-
- The underground algorithm is based on a binarization of the image
- and a connex component segmentation to determine the dot
- characteristics (location, moments, size...).
-
- The following sample code shows how to grab images from a firewire camera,
- track a blob and display the tracking results.
-
- \code
-#include <visp/vpConfig.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot.h>
-#include <visp/vpImage.h>
-
-int main()
-{
-#if defined(VISP_HAVE_DC1394_2)
- vpImage<unsigned char> I; // Create a gray level image container
- vp1394TwoGrabber g(false); // Create a grabber based on libdc1394-2.x third party lib
- g.acquire(I); // Acquire an image
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX d(I, 0, 0, "Camera view");
-#endif
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- vpDot blob;
- blob.initTracking(I);
- blob.setGraphics(true);
-
- while(1) {
- g.acquire(I); // Acquire an image
- vpDisplay::display(I);
- blob.track(I);
-
- vpDisplay::flush(I);
- }
-#endif
-}
- \endcode
-
- \sa vpDot2
-*/
-class VISP_EXPORT vpDot : public vpTracker
-{
-public :
- /*! \enum vpConnexityType
- Type of connexity 4, or 8.
- */
- typedef enum {
- CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
- right, up, down) */
- CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
- right, up, down, and the 4 pixels located on the diagonal) */
- } vpConnexityType;
-
- static const unsigned int SPIRAL_SEARCH_SIZE; /*!< Spiral size for the dot
- search. */
- double m00; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
- with \f$i = j = 0 \f$.
-
- \sa setComputeMoments()
- */
- double m01; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
- obtained with \f$i = 0 \f$ and \f$ j = 1 \f$.
-
- \sa setComputeMoments()
- */
- double m10; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 0 \f$.
-
- \sa setComputeMoments()
- */
- double m11; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double m20; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
- obtained with \f$i = 2 \f$ and \f$ j = 0 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double m02; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
- obtained with \f$i = 0 \f$ and \f$ j = 2 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
- \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
- double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
- \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
-
- \sa setComputeMoments()
- */
- double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
- \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
-
- vpDot() ;
- vpDot(const vpImagePoint &ip) ;
- vpDot(const vpDot& d) ;
- virtual ~vpDot() ;
-
- void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
- unsigned int thickness=1) const;
-
- /*!
-
- Return the dot bounding box.
-
- \sa getWidth(), getHeight()
-
- */
- inline vpRect getBBox() const {
- vpRect bbox;
-
- bbox.setRect(this->u_min,
- this->v_min,
- this->u_max - this->u_min + 1,
- this->v_max - this->v_min + 1);
-
- return (bbox);
- };
- /*!
- Return the location of the dot center of gravity.
-
- \return The coordinates of the center of gravity.
- */
- inline vpImagePoint getCog() const {
- return cog;
- }
-
- /*!
- Return the list of all the image points on the border of the dot.
-
- \warning Doesn't return the image points inside the dot anymore. To get those points see getConnexities().
- */
- inline std::list<vpImagePoint> getEdges() const {
- return this->ip_edges_list;
- };
-
- /*!
-
- Return the list of all the image points inside the dot.
-
- \return The list of all the images points in the dot.
- This list is updated after a call to track().
-
- */
- inline std::list<vpImagePoint> getConnexities() const {
- return this->ip_connexities_list;
- };
-
- inline double getGamma() const {return this->gamma;};
- /*!
-
- Return the precision of the gray level of the dot. It is a double
- precision float witch value is in ]0,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
-
- */
- double getGrayLevelPrecision() const {return grayLevelPrecision;}
- double getMaxDotSize() const {
- return this->maxDotSizePercentage;
- }
- /*!
- Return the mean gray level value of the dot.
- */
- double getMeanGrayLevel() const {
- return (this->mean_gray_level);
- };
-
- /*!
-
- Return the width of the dot.
-
- \sa getHeight()
-
- */
- inline unsigned int getWidth() const {
- return (this->u_max - this->u_min + 1);
- };
-
- /*!
-
- Return the width of the dot.
-
- \sa getHeight()
-
- */
- inline unsigned int getHeight() const {
- return (this->v_max - this->v_min + 1);
- };
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip);
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip,
- unsigned int gray_level_min, unsigned int gray_level_max);
-
- vpDot& operator =(const vpDot& d) ;
- bool operator ==(const vpDot& d);
- bool operator !=(const vpDot& d);
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d);
-
- void print(std::ostream& os) { os << *this << std::endl ; }
-
- /*!
- Initialize the dot coordinates with \e ip.
- */
- inline void setCog(const vpImagePoint &ip) {
- this->cog = ip;
- }
-
- /*!
-
- Activates the dot's moments computation.
-
- \param activate true, if you want to compute the moments. If false, moments
- are not computed.
-
- Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
- vpDot::m20, vpDot::m02.
-
- The coordinates of the region's centroid (u, v) can be computed from the
- moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$.
-
- */
- void setComputeMoments(const bool activate) { compute_moment = activate; }
-
- /*!
- Set the type of connexity: 4 or 8.
- */
- void setConnexity(vpConnexityType type) {this->connexityType = type; };
- void setMaxDotSize(double percentage) ;
- void setGrayLevelMin( const unsigned int &level_min ) {
- this->gray_level_min = level_min;
- };
- void setGrayLevelMax( const unsigned int &level_max ) {
- this->gray_level_max = level_max;
- };
- void setGrayLevelPrecision( const double & grayLevelPrecision );
-
- /*!
- Activates the display of all the pixels of the dot during the tracking.
- The default thickness of the overlayed drawings can be modified using
- setGraphicsThickness().
-
- \warning To effectively display the dot graphics a call to
- vpDisplay::flush() is needed.
-
- \param activate true to activate the display of dot pixels, false to turn
- off the display.
-
- \sa setGraphicsThickness()
- */
- void setGraphics(const bool activate) { graphics = activate ; }
- /*!
- Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
-
- \sa setGraphics()
- */
- void setGraphicsThickness(unsigned int t) {this->thickness = t;};
-
- void track(const vpImage<unsigned char> & I) ;
- void track(const vpImage<unsigned char> & I, vpImagePoint &ip) ;
-
-private:
- //! internal use only
- std::list<vpImagePoint> ip_connexities_list;
-
- //! List of border points
- std::list<vpImagePoint> ip_edges_list;
-
- /*! Type of connexity
-
- \warning In previous version this variable was called connexity
- */
- vpConnexityType connexityType;
-
- //! coordinates of the point center of gravity
- vpImagePoint cog;
-
- // Bounding box
- unsigned int u_min, u_max, v_min, v_max;
-
- // Flag used to allow display
- bool graphics ;
-
- unsigned int thickness; // Graphics thickness
-
- double maxDotSizePercentage;
- unsigned char gray_level_out;
-
- double mean_gray_level; // Mean gray level of the dot
- unsigned int gray_level_min; // left threshold for binarisation
- unsigned int gray_level_max; // right threshold for binarisation
- double grayLevelPrecision; //precision of the gray level of the dot.
- //It is a double precision float witch value is in ]0,1].
- //1 means full precision, whereas values close to 0 show a very bad precision
- double gamma ;
- //! flag : true moment are computed
- bool compute_moment ;
- double nbMaxPoint;
-
- void init() ;
- void setGrayLevelOut();
- bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n);
- bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab);
- void COG(const vpImage<unsigned char> &I,double& u, double& v) ;
-
-//Static Functions
-public:
- static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
-} ;
-
-#endif
-
-
-
diff --git a/src/tracking/dots/vpDot2.cpp b/src/tracking/dots/vpDot2.cpp
deleted file mode 100644
index 5edc64a..0000000
--- a/src/tracking/dots/vpDot2.cpp
+++ /dev/null
@@ -1,2664 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDot2.cpp 2135 2009-04-29 13:51:31Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Track a white dot.
- *
- * Authors:
- * Fabien Spindler
- * Anthony Saunier
- *
- *****************************************************************************/
-
-/*!
- \file vpDot2.cpp
- \brief Track a dot.
-*/
-
-//#define DEBUG
-
-
-#include <visp/vpDisplay.h>
-
-// exception handling
-#include <visp/vpTrackingException.h>
-#include <visp/vpMath.h>
-#include <visp/vpIoTools.h>
-
-#include <visp/vpDot2.h>
-#include <math.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-/******************************************************************************
- *
- * CONSTRUCTORS AND DESTRUCTORS
- *
- *****************************************************************************/
-/*!
-
- Initialize the tracker with default parameters.
-
-*/
-void vpDot2::init()
-{
- cog.set_u(0);
- cog.set_v(0);
-
- width = 0;
- height = 0;
- surface = 0;
- mean_gray_level = 0;
- gray_level_min = 128;
- gray_level_max = 255;
- grayLevelPrecision = 0.80;
- gamma = 1.5 ;
-
- sizePrecision = 0.65;
- ellipsoidShapePrecision = 0.65;
- maxSizeSearchDistancePrecision = 0.65;
- setEllipsoidBadPointsPercentage();
- m00 = m11 = m02 = m20 = m10 = m01 = 0.;
- mu11 = mu02 = mu20 = 0.;
-
- bbox_u_min = bbox_u_max = bbox_v_min = bbox_v_max = 0;
-
- firstBorder_u = 0;
- firstBorder_v = 0;
-
- compute_moment = false ;
- graphics = false;
- thickness = 1;
-}
-
-/*!
- Default constructor. Just do basic default initialization.
-*/
-vpDot2::vpDot2()
- : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
- gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
- sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
- allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
- graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
- firstBorder_u(0), firstBorder_v()
-{
-}
-
-/*!
-
- Constructor initialize the coordinates of the gravity center of the dot to
- the image point \e ip. Rest is the same as the default constructor.
-
- \param ip : An image point with sub-pixel coordinates.
-
-*/
-vpDot2::vpDot2(const vpImagePoint &ip)
- : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
- gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
- sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
- allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
- graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
- firstBorder_u(0), firstBorder_v()
-{
- cog = ip;
-}
-
-/*!
- Copy constructor.
-*/
-vpDot2::vpDot2(const vpDot2& twinDot )
- : vpTracker(twinDot),
- m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
- gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
- sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
- allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
- graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
- firstBorder_u(0), firstBorder_v()
-{
- *this = twinDot;
-}
-
-/*!
- = operator.
-*/
-vpDot2& vpDot2::operator=(const vpDot2& twinDot )
-{
- cog = twinDot.cog;
-
- width = twinDot.width;
- height = twinDot.height;
- surface = twinDot.surface;
- gray_level_min = twinDot.gray_level_min;
- gray_level_max = twinDot.gray_level_max;
- mean_gray_level = twinDot.mean_gray_level;
- grayLevelPrecision = twinDot.grayLevelPrecision;
- gamma = twinDot.gamma; ;
- sizePrecision = twinDot.sizePrecision;
- ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision ;
- maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
- allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
- area = twinDot.area;
-
- direction_list = twinDot.direction_list;
- ip_edges_list = twinDot.ip_edges_list;
-
- compute_moment = twinDot.compute_moment;
- graphics = twinDot.graphics;
- thickness = twinDot.thickness;
-
- bbox_u_min = twinDot.bbox_u_min;
- bbox_u_max = twinDot.bbox_u_max;
- bbox_v_min = twinDot.bbox_v_min;
- bbox_v_max = twinDot.bbox_v_max;
-
- firstBorder_u = twinDot.firstBorder_u;
- firstBorder_v = twinDot.firstBorder_v;
-
- m00 = twinDot.m00;
- m01 = twinDot.m01;
- m11 = twinDot.m11;
- m10 = twinDot.m10;
- m02 = twinDot.m02;
- m20 = twinDot.m20;
-
- mu11 = twinDot.mu11;
- mu20 = twinDot.mu20;
- mu02 = twinDot.mu02;
-
- return (*this);
-}
-
-/*!
- Destructor... do nothing for the moment.
-*/
-vpDot2::~vpDot2(){}
-
-
-/******************************************************************************
- *
- * PUBLIC METHODS
- *****************************************************************************/
-
-/*!
- Display in overlay the dot edges and center of gravity.
-
- \param I : Image.
- \param color : The color used for the display.
- \param t : Thickness of the displayed cross located at the dot cog.
-*/
-void vpDot2::display(const vpImage<unsigned char>& I, vpColor color, unsigned int t) const
-{
- vpDisplay::displayCross(I, cog, 3*t+8, color, t);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-
-/*!
-
- Initialize the tracking with a mouse click on the image and update the dot
- characteristics (center of gravity, moments) by a call to track().
-
- Wait a user click in a white area in the image. The clicked pixel
- will be the starting point from which the dot will be tracked.
-
- To get center of gravity of the dot, see getCog(). To compute the
- moments see setComputeMoments(). To get the width or height of the
- dot, call getWidth() and getHeight(). The area of the dot is
- given by getArea().
-
- \param I : Image.
- \param size : Size of the dot to track.
-
- If no valid dot was found in the window, return an exception.
-
- \exception vpTrackingException::featureLostError : If the dot initialization
- failed. The initialization can fail if the following characteristics are
- not valid;
- - The gray level is between gray level min and gray level max.
-
- - The shape should be ellipsoid if
- setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
- This is the default case. To track a non ellipsoid shape use
- setEllipsoidShapePrecision(0).
-
- \sa track()
-
-*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I, unsigned int size)
-{
- while ( vpDisplay::getClick(I, cog) != true) ;
-
- unsigned int i = (unsigned int)cog.get_i();
- unsigned int j = (unsigned int)cog.get_j();
-
- double Ip = pow((double)I[i][j]/255,1/gamma);
-
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
-
- setWidth(size);
- setHeight(size);
-
- try {
- track( I );
- }
- catch(vpException e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
- }
-}
-
-/*!
-
- Initialize the tracking for a dot supposed to be located at (u,v) and update
- the dot characteristics (center of gravity, moments) by a call to track().
-
- \param I : Image to process.
-
- \param ip : Location of the starting point from which the dot will be
- tracked in the image.
-
- \param size : Size of the dot to track.
-
- To get center of gravity of the dot, see getCog(). To compute the
- moments see setComputeMoments().
-
- If no valid dot was found in the window, return an exception.
-
- \exception vpTrackingException::featureLostError : If the dot initialization
- failed. The initialization can fail if the following characteristics are
- not valid;
- - The gray level is between gray level min and gray level max.
-
- - The shape should be ellipsoid if
- setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
- This is the default case. To track a non ellipsoid shape use
- setEllipsoidShapePrecision(0).
-*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I,
- const vpImagePoint &ip, unsigned int size)
-{
- cog = ip ;
-
- unsigned int i = (unsigned int)cog.get_i();
- unsigned int j = (unsigned int)cog.get_j();
-
- double Ip = pow((double)I[i][j]/255,1/gamma);
-
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
-
- setWidth(size);
- setHeight(size);
-
- try {
- track( I );
- }
- catch(vpException &e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
- }
-}
-
-/*!
-
- Initialize the tracking for a dot supposed to be located at (u,v) and update
- the dot characteristics (center of gravity, moments) by a call to track().
-
- The sub pixel coordinates of the dot are updated. To get the center
- of gravity coordinates of the dot, use getCog(). To
- compute the moments use setComputeMoments(true) before a call to
- initTracking().
-
- \param I : Image to process.
-
- \param ip : Location of the starting point from which the dot will
- be tracked in the image.
-
- \param gray_lvl_min : Minimum gray level threshold used to segment the dot;
- value comprised between 0 and 255.
-
- \param gray_lvl_max : Maximum gray level threshold used to segment the
- dot; value comprised between 0 and 255. \e gray_level_max should be
- greater than \e gray_level_min.
-
- \param size : Size of the dot to track.
-
- If no valid dot was found in the window, return an exception.
-
- \exception vpTrackingException::featureLostError : If the dot initialization
- failed. The initialization can fail if the following characteristics are
- not valid;
- - The gray level is between gray level min and gray level max.
-
- - The shape should be ellipsoid if
- setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
- This is the default case. To track a non ellipsoid shape use
- setEllipsoidShapePrecision(0).
-
- \sa track(), getCog()
-
-*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I,
- const vpImagePoint &ip,
- unsigned int gray_lvl_min,
- unsigned int gray_lvl_max,
- unsigned int size)
-{
- cog = ip ;
-
- this->gray_level_min = gray_lvl_min;
- this->gray_level_max = gray_lvl_max;
-
- setWidth(size);
- setHeight(size);
-
- try {
- track( I );
- }
- catch(vpException &e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
- }
-}
-
-
-
-/*!
-
- Try to locate the dot in the image:
-
- - First, estimate the new position of the dot, using its previous position.
- - Then compute the center of gravity (surface, width height) of the
- tracked entity from the Freeman chain elements.
- - If the dot is lost (estimated point too dark, too much surface change,...),
- search the dot in a window around the previous position.
- - If no valid dot was found in the window, return an exception.
-
- \param I : Image.
-
- \exception vpTrackingException::featureLostError : If the dot tracking
- failed. The tracking can fail if the following characteristics are not
- valid;
- - The gray level is between gray level min and gray level max.
-
- - The size (width or height) and the surface (in terms of number of
- pixels) should not differ to much with the previous dot.
-
- - The shape should be ellipsoid if
- setEllipsoidShapePrecision(ellipsoidShapePrecision) is used.
- This is the default case. To track a non ellipsoid shape use
- setEllipsoidShapePrecision(0).
-
- To get the center of gravity of the dot, call getCog(). To get the
- width or height of the dot, call getWidth() and getHeight(). The area of the
- dot is given by getArea().
-
- To compute all the inertia moments associated to the dot see
- setComputeMoments().
-
- To get the pixels coordinates on the dot boundary, see getList_u() and
- getList_v().
-
-
-*/
-void vpDot2::track(const vpImage<unsigned char> &I)
-{
- m00 = m11 = m02 = m20 = m10 = m01 = 0 ;
-
- // First, we will estimate the position of the tracked point
-
- // Set the search area to the entire image
- setArea(I);
-
- // create a copy of the dot to search
- // This copy can be saw as the previous dot used to check if the current one
- // found with computeParameters() is similar to the previous one (see isValid()
- // function).
- // If the found dot is not similar (or valid), we use this copy to set the current
- // found dot to the previous one (see below).
- vpDot2 wantedDot(*this);
-
- // vpDEBUG_TRACE(0, "Previous dot: ");
- // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
- // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
- bool found = false;
- found = computeParameters(I, cog.get_u(), cog.get_v());
-
- if (found) {
- // test if the found dot is valid (ie similar to the previous one)
- found = isValid( I, wantedDot);
- if (! found) {
- *this = wantedDot;
- //std::cout << "The found dot is not valid" << std::endl;
- }
- }
-
- if (! found) {
- // vpDEBUG_TRACE(0, "Search the dot in a biggest window around the last position");
- // vpDEBUG_TRACE(0, "Bad computed dot: ");
- // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
- // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
-
- // if estimation was wrong (get an error tracking), look for the dot
- // closest from the estimation,
- // i.e. search for dots in an a region of interest around the this dot and get the first
- // element in the area.
-
- // first get the size of the search window from the dot size
- double searchWindowWidth, searchWindowHeight;
- //if( getWidth() == 0 || getHeight() == 0 )
- if( std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon() || std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon() )
- {
- searchWindowWidth = 80.;
- searchWindowHeight = 80.;
- }
- else
- {
- searchWindowWidth = getWidth() * 5;
- searchWindowHeight = getHeight() * 5;
- }
- std::list<vpDot2> candidates;
- searchDotsInArea( I,
- (int)(this->cog.get_u()-searchWindowWidth /2.0),
- (int)(this->cog.get_v()-searchWindowHeight/2.0),
- (unsigned int)searchWindowWidth,
- (unsigned int)searchWindowHeight,
- candidates);
-
- // if the vector is empty, that mean we didn't find any candidate
- // in the area, return an error tracking.
- if( candidates.empty() )
- {
- //vpERROR_TRACE("No dot was found") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "No dot was found")) ;
- }
-
- // otherwise we've got our dot, update this dot's parameters
- vpDot2 movingDot = candidates.front();
-
- setCog( movingDot.getCog() );
- setArea( movingDot.getArea() );
- setWidth( movingDot.getWidth() );
- setHeight( movingDot.getHeight() );
-
- // Update the moments
- m00 = movingDot.m00;
- m01 = movingDot.m01;
- m10 = movingDot.m10;
- m11 = movingDot.m11;
- m20 = movingDot.m20;
- m02 = movingDot.m02;
-
- // Update the bounding box
- bbox_u_min = movingDot.bbox_u_min;
- bbox_u_max = movingDot.bbox_u_max;
- bbox_v_min = movingDot.bbox_v_min;
- bbox_v_max = movingDot.bbox_v_max;
- }
- // 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 ) )
- {
- //vpERROR_TRACE("The center of gravity of the dot is not in the image") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "The center of gravity of the dot is not in the image")) ;
- }
-
- // Get dots center of gravity
- // unsigned int u = (unsigned int) this->cog.get_u();
- // unsigned int v = (unsigned int) this->cog.get_v();
- // Updates the min and max gray levels for the next iteration
- // double Ip = pow((double)I[v][u]/255,1/gamma);
- double Ip = pow(getMeanGrayLevel()/255,1/gamma);
- //printf("current value of gray level center : %i\n", I[v][u]);
-
- //getMeanGrayLevel(I);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
- if (gray_level_min > 255)
- gray_level_min = 255;
- }
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
- if (gray_level_max > 255)
- gray_level_max = 255;
-
- //printf("%i %i \n",gray_level_max,gray_level_min);
- if (graphics) {
- // display a red cross at the center of gravity's location in the image.
-
- vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
- //vpDisplay::flush(I);
- }
-}
-
-/*!
-
- Track and get the new dot coordinates. See track() for a more complete
- description
-
- \param I : Image to process.
-
- \param ip [out] : Sub pixel coordinate of the tracked dot center of gravity.
-
- The behavior of this method is similar to the following code:
- \code
- vpDot2 d;
- d.track(I);
- vpImagePoint cog = d.getCog();
- \endcode
-
- \sa track()
-*/
-void
-vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
-{
- track(I);
-
- ip = this->cog;
-}
-
-///// GET METHODS /////////////////////////////////////////////////////////////
-
-/*!
- Return the width of the dot.
-
- \sa getHeight()
-*/
-double vpDot2::getWidth() const
-{
- return width;
-}
-
-/*!
- Return the height of the dot.
-
- \sa getWidth()
-*/
-double vpDot2::getHeight() const
-{
- return height;
-}
-
-/*!
- Return the area of the dot.
-
- The area of the dot is also given by \f$|m00|\f$.
-*/
-double vpDot2::getArea() const
-{
- return fabs(surface);
-}
-
-/*!
- Return the precision of the gray level of the dot. It is a double
- precision float witch value is in ]0,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
-*/
-double vpDot2::getGrayLevelPrecision() const
-{
- return grayLevelPrecision;
-}
-
-/*!
- Return the precision of the size of the dot. It is a double
- precision float witch value is in ]0,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
-*/
-double vpDot2::getSizePrecision() const
-{
- return sizePrecision;
-}
-
-/*!
- Return the precision of the ellipsoid shape of the dot. It is a double
- precision float witch value is in [0,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
-
- \sa setEllipsoidShapePrecision()
-*/
-double vpDot2::getEllipsoidShapePrecision() const
-{
- return ellipsoidShapePrecision;
-}
-
-/*!
- Return the precision of the search maximum distance to get the starting point on a dot border. It is a double
- precision float witch value is in [0.05,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
-*/
-double vpDot2::getMaxSizeSearchDistancePrecision() const{
- return maxSizeSearchDistancePrecision;
-}
-
-/*!
- Return the distance between the two center of dots.
-*/
-double vpDot2::getDistance( const vpDot2& distantDot ) const
-{
- vpImagePoint cogDistantDot = distantDot.getCog();
- double diff_u = this->cog.get_u() - cogDistantDot.get_u();
- double diff_v = this->cog.get_v() - cogDistantDot.get_v();
- return sqrt( diff_u*diff_u + diff_v*diff_v );
-}
-
-
-///// SET METHODS ////////////////////////////////////////////////////////////
-
-
-/*!
-
- Set the width of the dot. This is meant to be used to search a dot in an
- area.
-
- \param w : Width of a dot to search in a region of interest.
-
- \sa setHeight(), setArea(), setSizePrecision()
-*/
-void vpDot2::setWidth( const double & w )
-{
- this->width = w;
-}
-
-/*!
-
- Set the height of the dot. This is meant to be used to search a dot in an
- area.
-
- \param h : Height of a dot to search in a region of interest.
-
- \sa setWidth(), setArea(), setSizePrecision()
-
-*/
-void vpDot2::setHeight( const double & h )
-{
- this->height = h;
-}
-
-/*!
-
- Set the area of the dot. This is meant to be used to search a dot in a region of interest.
-
- \param a : Area of a dot to search in a region of interest.
-
- \sa setWidth(), setHeight(), setSizePrecision()
-
-*/
-void vpDot2::setArea( const double & a )
-{
- this->surface = a;
-}
-
-/*!
-
- Set the precision of the gray level of the dot.
-
- \param precision : It is a double precision float which value is in ]0,1]:
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
- - Values lower or equal to 0 are brought back to an epsilon>0
- - Values higher than 1 are brought back to 1
- If the initial gray level is I, the gray levels of the dot will be between :
- \f$Imin=255*\big((\frac{I}{255})^{{\gamma}^{-1}}-(1-grayLevelPrecision)\big)^{\gamma}\f$
- and
- \f$Imax=255*\big((\frac{I}{255})^{{\gamma}^{-1}}+(1-grayLevelPrecision)\big)^{\gamma}\f$
- with \f$\gamma=1.5\f$ .
-
- \sa setGrayLevelMin(), setGrayLevelMax()
-*/
-void vpDot2::setGrayLevelPrecision( const double & precision )
-{
- double epsilon = 0.05;
- if( grayLevelPrecision<epsilon )
- {
- this->grayLevelPrecision = epsilon;
- }
- else if( grayLevelPrecision>1 )
- {
- this->grayLevelPrecision = 1.0;
- }
- else
- {
- this->grayLevelPrecision = precision;
- }
-}
-/*!
-
- Set the precision of the size of the dot. Used to test the validity of the dot
-
- \param precision : It is a double precision float which value is in [0,1]:
- - this is the limit ratio between the tested parameter and the measured one.
- minSize = sizePrecision*originalSize ; maxSize = originalSize/sizePrecision ;
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
- - Values lower or equal to 0 are brought back to 0.
- - Values higher than 1 are brought back to 1.
- - To desactivate validity test set sizePrecision to 0
-
- \sa setWidth(), setHeight(), setArea()
-*/
-void vpDot2::setSizePrecision( const double & precision )
-{
- if( sizePrecision<0 )
- {
- this->sizePrecision = 0;
- }
- else if( sizePrecision>1 )
- {
- this->sizePrecision = 1.0;
- }
- else
- {
- this->sizePrecision = precision;
- }
-}
-
-/*!
- Indicates if the dot should have an ellipsoid shape to be valid.
- \param precision : It is a double precision float which value is in [0,1]:
-
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
- - Values lower or equal to 0 are brought back to 0.
- - Values higher than 1 are brought back to 1.
- To track a non ellipsoid shape use setEllipsoidShapePrecision(0).
-
- The following example show how to track a blob with a height constraint on an ellipsoid shape.
- The tracking will fail if the shape is not ellipsoid.
- \code
- vpDot2 dot;
- dot.setEllipsoidShapePrecision(0.9); // to track a blob with a height constraint attendee on a circle shape
- ...
- dot.track();
- \endcode
-
- This other example shows how to remove any constraint on the shape. Here the tracker will be able to track
- any shape, including square or rectangular shapes.
- \code
- vpDot2 dot;
- dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
- ...
- dot.track();
- \endcode
-
- \sa getEllipsoidShapePrecision()
-*/
-void vpDot2::setEllipsoidShapePrecision(const double & precision) {
-
- if( ellipsoidShapePrecision<0 )
- {
- this->ellipsoidShapePrecision = 0;
- }
- else if( ellipsoidShapePrecision>1 )
- {
- this->ellipsoidShapePrecision = 1.0;
- }
- else
- {
- this->ellipsoidShapePrecision = precision;
- }
-}
-
-/*!
-
- Set the precision of the search maximum distance to get the starting point on a dot border. A too low value
- mean a large search area.
-
- \param precision : It is a double precision float which value is in [0.05,1]:
- - this is the limit ratio between the tested parameter and the measured one.
- distance < getWidth()/(getSizePrecision()+epsilon);
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
- - Values lower or equal to 0.05 are brought back to 0.05
- - Values higher than 1 are brought back to 1.
-
-*/
-void vpDot2::setMaxSizeSearchDistancePrecision( const double & precision )
-{
- double epsilon = 0.05;
- if( maxSizeSearchDistancePrecision<epsilon )
- {
- this-> maxSizeSearchDistancePrecision = epsilon;
- }
- else if( maxSizeSearchDistancePrecision >1 )
- {
- this->maxSizeSearchDistancePrecision = 1.0;
- }
- else
- {
- this->maxSizeSearchDistancePrecision = precision;
- }
-}
-
-/*!
-
- Set the parameters of the area in which a dot is search to the image
- dimension.
-
- \param I : Image.
-
-*/
-void
-vpDot2::setArea(const vpImage<unsigned char> &I)
-{
- setArea(I, 0, 0, I.getWidth(), I.getHeight());
-}
-
-/*!
-
- Set the parameters of an area by setting the upper-left corner coordinates
- (u, v), width and height.
-
- \param I : The image we are working with.
- \param u : Area horizontal left coordinate.
- \param v : Area vertical top coordinate.
- \param w : Area width.
- \param h : Area height.
-
-*/
-void
-vpDot2::setArea(const vpImage<unsigned char> &I,
- int u, int v,
- unsigned int w, unsigned int h)
-{
- unsigned int image_w = I.getWidth();
- unsigned int image_h = I.getHeight();
-
- // Bounds the area to the image
- if (u < 0) u = 0;
- else if (u >= (int)image_w) u = (int)image_w - 1;
- if (v < 0) v = 0;
- else if (v >= (int)image_h) v = (int)image_h - 1;
-
- if (((unsigned int)u + w) > image_w) w = image_w - (unsigned int)u - 1;
- if (((unsigned int)v + h) > image_h) h = image_h - (unsigned int)v - 1;
-
- area.setRect(u, v, w, h);
-}
-
-/*!
-
- Set the parameters of the area.
-
- \param a : Area.
-
-*/
-void
- vpDot2::setArea(const vpRect & a)
-{
- area = a;
-}
-
-///// CLASS FUNCTIONALITY ////////////////////////////////////////////////////
-
-/*!
-
- Look for a list of dot matching this dot parameters within the entire
- image.
-
- \warning Allocates memory for the list of dots returned by this method.
- Desallocation has to be done by yourself.
-
- \param I : Image.
-
- \param niceDots: List of the dots that are found.
-
- Before calling this method, dot characteristics to found have to be set like:
-
- \code
- vpDot2 d;
-
- // Set dot characteristics for the auto detection
- d.setWidth(24);
- d.setHeight(23);
- d.setArea(412);
- d.setGrayLevelMin(160);
- d.setGrayLevelMax(255);
- d.setGrayLevelPrecision(0.8);
- d.setSizePrecision(0.65);
- d.setEllipsoidShapePrecision(0.65);
- \endcode
-
- To search dots in the whole image:
- \code
- std::list<vpDot2> list_d;
- d.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), list_d) ;
- \endcode
-
- The number of dots found in the area is given by:
- \code
- std::cout << list_d.size();
- \endcode
-
- To parse all the dots:
- \code
- std::list<vpDot2>::iterator it;
- for (it = list_d.begin(); it != list_d.end(); ++it) {
- vpDot2 tmp_d = *it;
- }
- \endcode
-
- \sa searchDotsInArea(vpImage<unsigned char>&, int, int, unsigned int, unsigned int, std::list<vpDot2> &)
-*/
-void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots)
-{
- searchDotsInArea( I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
-}
-
-/*!
-
- Look for a list of dot matching this dot parameters within a region of interest
- defined by a rectangle in the image. The rectangle upper-left coordinates are given by
- (\e area_u, \e area_v). The size of the rectangle is given by \e area_w and
- \e area_h.
-
- \param I : Image to process.
- \param area_u : Coordinate (column) of the upper-left area corner.
- \param area_v : Coordinate (row) of the upper-left area corner.
-
- \param area_w : Width or the area in which a dot is searched.
- \param area_h : Height or the area in which a dot is searched.
-
- \param niceDots: List of the dots that are found.
-
- \warning Allocates memory for the list of vpDot2 returned by this method.
- Desallocation has to be done by yourself, see searchDotsInArea()
-
- \sa searchDotsInArea(vpImage<unsigned char>& I, std::list<vpDot2> &)
-*/
-void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
- int area_u,
- int area_v,
- unsigned int area_w,
- unsigned int area_h,
- std::list<vpDot2> &niceDots)
-
-{
- // clear the list of nice dots
- niceDots.clear();
-
- // Fit the input area in the image; we keep only the common part between this
- // area and the image.
- setArea(I, area_u, area_v, area_w, area_h);
-
- // compute the size of the search grid
- unsigned int gridWidth;
- unsigned int gridHeight;
- getGridSize( gridWidth, gridHeight );
-
- if (graphics) {
- // Display the area were the dot is search
- vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness);
- //vpDisplay::flush(I);
- }
-
-#ifdef DEBUG
- vpDisplay::displayRectangle(I, area, vpColor::blue);
- vpDisplay::flush(I);
-#endif
- // start the search loop; for all points of the search grid,
- // test if the pixel belongs to a valid dot.
- // if it is so eventually add it to the vector of valid dots.
- std::list<vpDot2> badDotsVector;
- std::list<vpDot2>::iterator itnice;
- std::list<vpDot2>::iterator itbad;
-
- vpDot2* dotToTest = NULL;
- vpDot2 tmpDot;
-
- unsigned int area_u_min = (unsigned int) area.getLeft();
- unsigned int area_u_max = (unsigned int) area.getRight();
- unsigned int area_v_min = (unsigned int) area.getTop();
- unsigned int area_v_max = (unsigned int) area.getBottom();
-
- unsigned int u, v;
- vpImagePoint cogTmpDot;
-
- for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
- {
- for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
- {
- // if the pixel we're in doesn't have the right color (outside the
- // graylevel interval), no need to check further, just get to the
- // next grid intersection.
- if( !hasGoodLevel(I, u, v) ) continue;
-
- // Test if an other germ is inside the bounding box of a dot previously
- // detected
- bool good_germ = true;
-
- itnice = niceDots.begin();
- while( itnice != niceDots.end() && good_germ == true) {
- tmpDot = *itnice;
-
- cogTmpDot = tmpDot.getCog();
- double u0 = cogTmpDot.get_u();
- double v0 = cogTmpDot.get_v();
- double half_w = tmpDot.getWidth() / 2.;
- double half_h = tmpDot.getHeight() / 2.;
-
- if ( u >= (u0-half_w) && u <= (u0+half_w) &&
- v >= (v0-half_h) && v <= (v0+half_h) ) {
- // Germ is in a previously detected dot
- good_germ = false;
- }
- ++ itnice;
- }
-
- if (! good_germ)
- continue;
-
- // Compute the right border position for this possible germ
- unsigned int border_u;
- unsigned int border_v;
- if(findFirstBorder(I, u, v, border_u, border_v) == false){
- // germ is not good.
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
- u = border_u;
- v = border_v;
- continue;
- }
-
- itbad = badDotsVector.begin();
-#define vpBAD_DOT_VALUE (*itbad)
- vpImagePoint cogBadDot;
-
- while( itbad != badDotsVector.end() && good_germ == true) {
- if( (double)u >= vpBAD_DOT_VALUE.bbox_u_min
- && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
- (double)v >= vpBAD_DOT_VALUE.bbox_v_min
- && (double)v <= vpBAD_DOT_VALUE.bbox_v_max){
- std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
- while (it_edges != ip_edges_list.end() && good_germ == true){
- // Test if the germ belong to a previously detected dot:
- // - from the germ go right to the border and compare this
- // position to the list of pixels of previously detected dots
- cogBadDot = *it_edges;
- //if( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) {
- if( (std::fabs(border_u - cogBadDot.get_u()) <= vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u()))*std::numeric_limits<double>::epsilon() )
- &&
- (std::fabs(v - cogBadDot.get_v()) <= vpMath::maximum(std::fabs((double)v), std::fabs(cogBadDot.get_v()))*std::numeric_limits<double>::epsilon() )) {
- good_germ = false;
- }
- ++ it_edges;
- }
- }
- ++itbad;
- }
-#undef vpBAD_DOT_VALUE
-
- if (! good_germ) {
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
- u = border_u;
- v = border_v;
- continue;
- }
-
- vpTRACE(4, "Try germ (%d, %d)", u, v);
-
- vpImagePoint germ;
- germ.set_u( u );
- germ.set_v( v );
-
- // otherwise estimate the width, height and surface of the dot we
- // created, and test it.
- if( dotToTest != NULL ) delete dotToTest;
- dotToTest = getInstance();
- dotToTest->setCog( germ );
- dotToTest->setGrayLevelMin ( getGrayLevelMin() );
- dotToTest->setGrayLevelMax ( getGrayLevelMax() );
- dotToTest->setGrayLevelPrecision( getGrayLevelPrecision() );
- dotToTest->setSizePrecision( getSizePrecision() );
- dotToTest->setGraphics( graphics );
- dotToTest->setGraphicsThickness( thickness );
- dotToTest->setComputeMoments( true );
- dotToTest->setArea( area );
- dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
-
- // first compute the parameters of the dot.
- // if for some reasons this caused an error tracking
- // (dot partially out of the image...), check the next intersection
- if( dotToTest->computeParameters( I ) == false ) {
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
- u = border_u;
- v = border_v;
- continue;
- }
- // if the dot to test is valid,
- if( dotToTest->isValid( I, *this ) )
- {
- vpImagePoint cogDotToTest = dotToTest->getCog();
- // Compute the distance to the center. The center used here is not the
- // area center available by area.getCenter(area_center_u,
- // area_center_v) but the center of the input area which may be
- // partially outside the image.
-
- double area_center_u = area_u + area_w/2.0 - 0.5;
- double area_center_v = area_v + area_h/2.0 - 0.5;
-
- double thisDiff_u = cogDotToTest.get_u() - area_center_u;
- double thisDiff_v = cogDotToTest.get_v() - area_center_v;
- double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
-
- bool stopLoop = false;
- itnice = niceDots.begin();
-
- while( itnice != niceDots.end() && stopLoop == false )
- {
- tmpDot = *itnice;
-
- //double epsilon = 0.001; // detecte +sieurs points
- double epsilon = 3.0;
- // if the center of the dot is the same than the current
- // don't add it, test the next point of the grid
- cogTmpDot = tmpDot.getCog();
-
- if( fabs( cogTmpDot.get_u() - cogDotToTest.get_u() ) < epsilon &&
- fabs( cogTmpDot.get_v() - cogDotToTest.get_v() ) < epsilon )
- {
- stopLoop = true;
- // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
- u = border_u;
- v = border_v;
- continue;
- }
-
- double otherDiff_u = cogTmpDot.get_u() - area_center_u;
- double otherDiff_v = cogTmpDot.get_v() - area_center_v;
- double otherDist = sqrt( otherDiff_u*otherDiff_u +
- otherDiff_v*otherDiff_v );
-
-
- // if the distance of the curent vector element to the center
- // is greater than the distance of this dot to the center,
- // then add this dot before the current vector element.
- if( otherDist > thisDist )
- {
- niceDots.insert(itnice, *dotToTest );
- ++ itnice;
- stopLoop = true;
- // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
- u = border_u;
- v = border_v;
- continue;
- }
- ++itnice;
- }
- vpTRACE(4, "End while (%d, %d)", u, v);
-
- // if we reached the end of the vector without finding the dot
- // or inserting it, insert it now.
- if( itnice == niceDots.end() && stopLoop == false )
- {
- niceDots.push_back( *dotToTest );
- }
- }
- else {
- // Store bad dots
- badDotsVector.push_front( *dotToTest );
- }
- }
- }
- if( dotToTest != NULL ) delete dotToTest;
-}
-
-/*!
-
- Check if the dot is "like" the wanted dot passed in.
-
- Compare the following characteristics of the dot to the wanted dot;
- - the size (width or height)
- - the surface (number of pixels)
- - the geometry; the shape should be ellipsoid if
- setEllipsoidShapePrecision(double ellispoidShapePrecision) is used.
-
- \return If it is so, return true, otherwise return false.
-
- \warning Parameters of the wanted dot (width, height, surface, in level, out
- level, accuracy) must already be set before; see
- searchDotsInArea(vpImage<unsigned char>& I)
-
- \param I : Image.
- \param wantedDot : Wanted dot passed in.
-
-*/
-bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
-{
- double size_precision = wantedDot.getSizePrecision();
- double ellipsoidShape_precision = wantedDot.getEllipsoidShapePrecision();
- double epsilon = 0.001;
-
- //
- // First, check the width, height and surface of the dot. Those parameters
- // must be the same.
- //
- //if ( (wantedDot.getWidth() != 0)
- // && (wantedDot.getHeight() != 0)
- // && (wantedDot.getArea() != 0) )
- if ( (std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon()) )
- // if (size_precision!=0){
- if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()){
-#ifdef DEBUG
- std::cout << "test size precision......................\n";
- std::cout << "wanted dot: " << "w=" << wantedDot.getWidth()
- << " h=" << wantedDot.getHeight()
- << " s=" << wantedDot.getArea()
- << " precision=" << size_precision
- << " epsilon=" << epsilon << std::endl;
- std::cout << "dot found: " << "w=" << getWidth()
- << " h=" << getHeight()
- << " s=" << getArea() << std::endl;
-#endif
- if( ( wantedDot.getWidth()*size_precision-epsilon < getWidth() ) == false )
- {
- vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad width > for dot (%g, %g)\n", cog.get_u(), cog.get_v());
-#endif
- return false;
- }
-
- if( ( getWidth() < wantedDot.getWidth()/(size_precision+epsilon ) )== false )
- {
- vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad width %g > %g for dot (%g, %g)\n",
- getWidth(), wantedDot.getWidth()/(size_precision+epsilon),
- cog.get_u(), cog.get_v());
-#endif
- return false;
- }
-
- if( ( wantedDot.getHeight()*size_precision-epsilon < getHeight() ) == false )
- {
- vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad height %g > %g for dot (%g, %g)\n",
- wantedDot.getHeight()*size_precision-epsilon, getHeight(),
- cog.get_u(), cog.get_v());
-#endif
- return false;
- }
-
- if( ( getHeight() < wantedDot.getHeight()/(size_precision+epsilon )) == false )
- {
- vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad height %g > %g for dot (%g, %g)\n",
- getHeight(), wantedDot.getHeight()/(size_precision+epsilon),
- cog.get_u(), cog.get_v());
-#endif
- return false;
- }
-
- if( ( wantedDot.getArea()*(size_precision*size_precision)-epsilon < getArea() ) == false )
- {
- vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad surface %g > %g for dot (%g, %g)\n",
- wantedDot.getArea()*(size_precision*size_precision)-epsilon,
- getArea(),
- cog.get_u(), cog.get_v());
-#endif
- return false;
- }
-
- if( ( getArea() < wantedDot.getArea()/(size_precision*size_precision+epsilon )) == false )
- {
- vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
-#ifdef DEBUG
- printf("Bad surface %g < %g for dot (%g, %g)\n",
- getArea(), wantedDot.getArea()/(size_precision*size_precision+epsilon),
- cog.get_u(), cog.get_v());
-#endif
- return false;
- }
- }
- //
- // Now we can proceed to more advanced (and costy) checks.
- // First check there is a white (>level) elipse within dot
- // Then check the dot is surrounded by a black ellipse.
- //
- int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid
- int nb_bad_points = 0;
- int nb_max_bad_points = (int)(nb_point_to_test*allowedBadPointsPercentage_);
- double step_angle = 2*M_PI / nb_point_to_test;
-
- // if (ellipsoidShape_precision != 0 && compute_moment) {
- if (std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon() && compute_moment) {
- // std::cout << "test shape precision......................\n";
- // See F. Chaumette. Image moments: a general and useful set of features
- // for visual servoing. IEEE Trans. on Robotics, 20(4):713-723, August 2004.
-
- // mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
- // = m11 - m10 * m01 / m00
- // mu20 = m20 - m00 * xg^2 = m20 - m00 * m10/m00 * m10/m00
- // = m20 - m10^2 / m00
- // mu02 = m02 - m01^2 / m00
- // alpha = 1/2 arctan( 2 * mu11 / (mu20 - mu02) )
- //
- // a1^2 = 2 / m00 * (mu02 + mu20 + sqrt( (mu20 - mu02)^2 + 4mu11^2) )
- //
- // a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) )
-
- //we compute parameters of the estimated ellipse
- double tmp1 = (m01*m01 -m10*m10)/m00+(m20-m02);
- double tmp2 = m11 -m10*m01/m00 ;
- double Sqrt = sqrt(tmp1*tmp1 + 4*tmp2*tmp2);
- double a1 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 + Sqrt));
- double a2 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 - Sqrt));
- double alpha = 0.5*atan2(2*(m11*m00-m10*m01),
- ((m20-m02)*m00-m10*m10+m01*m01));
-
- // to be able to track small dots, minorize the ellipsoid radius for the
- // inner test
- a1 -= 1.0;
- a2 -= 1.0;
-
- double innerCoef = ellipsoidShape_precision ;
- unsigned int u, v;
- double cog_u = this->cog.get_u();
- double cog_v = this->cog.get_v();
-
- vpImagePoint ip;
- nb_bad_points = 0;
- for( double theta = 0. ; theta<2*M_PI ; theta+= step_angle ) {
- 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 cercle pixel (%d, %d) has bad level for dot (%g, %g)",
- // u, v, cog_u, cog_v);
-#ifdef DEBUG
- printf("Inner cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
- u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
-#endif
- //return false;
- nb_bad_points ++;
- }
- if (graphics) {
- for (unsigned int t=0; t< thickness; t++) {
- ip.set_u( u + t );
- ip.set_v( v );
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
- }
- }
-#ifdef DEBUG
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
- vpDisplay::flush(I);
-#endif
- }
- if (nb_bad_points > nb_max_bad_points)
- {
-#ifdef DEBUG
- printf("Inner ellipse has %d bad points. Max allowed is %d\n",
- nb_bad_points, nb_max_bad_points);
-#endif
- return false;
- }
- // to be able to track small dots, maximize the ellipsoid radius for the
- // inner test
- a1 += 2.0;
- a2 += 2.0;
-
- double outCoef = 2-ellipsoidShape_precision; //1.6;
- nb_bad_points = 0;
- for( double theta=0. ; theta<2*M_PI ; theta+= step_angle ) {
- u = (unsigned int) (cog_u + outCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
- v = (unsigned int) (cog_v + outCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
-#ifdef DEBUG
- //vpDisplay::displayRectangle(I, area, vpColor::yellow);
- vpDisplay::displayCross( I, v, u, 7, vpColor::purple ) ;
- vpDisplay::flush(I);
-#endif
- // If outside the area, continue
- if ((double)u < area.getLeft() || (double)u > area.getRight()
- || (double)v < area.getTop() || (double)v > area.getBottom()) {
- continue;
- }
- if( ! this->hasReverseLevel( I, u, v ) ) {
- // vpTRACE("Outside cercle pixel (%d, %d) has bad level for dot (%g, %g)",
- // u, v, cog_u, cog_v);
-#ifdef DEBUG
- printf("Outside cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
- u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
-#endif
- nb_bad_points ++;
- //return false;
- }
- if (graphics) {
- for(unsigned int t=0; t<thickness; t++) {
- ip.set_u( u + t);
- ip.set_v( v );
-
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
- }
- }
- }
- }
- if (nb_bad_points > nb_max_bad_points)
- {
-#ifdef DEBUG
- printf("Outside ellipse has %d bad points. Max allowed is %d\n",
- nb_bad_points, nb_max_bad_points);
-#endif
- return false;
- }
-
- return true;
-}
-
-
-
-/*!
-
- Check if a the pixel of coordinates (u, v) is in the image and has
- a good level to belong to this dot.
-
- \param I : Image.
- \param u : Pixel to test.
- \param v : Pixel to test.
-
- \return true : If the pixel of coordinates (u, v) is in the area and
- has a value between the min and max levels fixed by setGrayLevelMin() and setGrayLevelMax().
-
- \return false : Otherwise
-
- \sa setGrayLevelMin(), setGrayLevelMax()
-
-*/
-bool vpDot2::hasGoodLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const
-{
- if( !isInArea( u, v ) )
- return false;
-
- if( I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
- {
- return true;
- }
- else
- {
- return false;
- }
-}
-
-
-/*!
-
- Check if a the pixel of coordinates (u, v) in the image has a good level to
- be a dark zone around the dot.
-
- \param I : Image.
- \param u : Pixel to test.
- \param v : Pixel to test.
-
- \return true if it is so, and false otherwise.
-
-*/
-bool vpDot2::hasReverseLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const
-{
-
- if( !isInArea( u, v ) )
- return false;
-
- if( I[v][u] < gray_level_min || I[v][u] > gray_level_max)
- {
- return true;
- }
- else
- {
- return false;
- }
-}
-
-
-/*!
- Return a new instance of vpDot2.
-
- Should be used by child classed to return their own instance of vpDot2.
-
- \return An instance of vpDot2.
-
-*/
-vpDot2* vpDot2::getInstance()
-{
- return new vpDot2();
-}
-
-/*!
-
- Returns the list of Freeman chain code used to turn around the dot
- counterclockwise.
-
- \return List of Freeman chain list [0, ..., 7]
- - 0 : right
- - 1 : top right
- - 2 : top
- - 3 : top left
- - 4 : left
- - 5 : down left
- - 6 : down
- - 7 : down right
-*/
-void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const
-{
- freeman_chain = direction_list;
-}
-
-
-
-/******************************************************************************
- *
- * PRIVATE METHODS
- *
- ******************************************************************************/
-
-
-
-/*!
-
- Compute all the parameters of the dot (center, width, height, surface,
- inertia moments...).
-
- This is done the following way:
-
- - First, we check the point (_u, _v) passed in has the right level in the
- image
-
- - Then we cross the tracked entity from left to right until we reach it's
- border.
-
- - We follow this border until we come back to the first point or we get to
- border of the image. Each time we update variables used to compute the
- dot parameters
-
- \param I : The image we are working with.
-
- \param _u : A pixel coordinate inside the dot.
-
- \param _v : A pixel coordinate inside the dot.
-
- \return false : If a dot can't be found around pixel coordinates given as
- parameter
-
- \return true : If a dot was found.
-
- \sa getFirstBorder_u(), getFirstBorder_v()
-
-*/
-bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
- const double &_u,
- const double &_v)
-{
- direction_list.clear();
- ip_edges_list.clear();
-
- double est_u = _u; // estimated
- double est_v = _v;
-
- // if u has default value, set it to the actual center value
- //if( est_u == -1.0 )
- if( std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u),1.)*std::numeric_limits<double>::epsilon() )
- {
- est_u = this->cog.get_u();
- }
-
- // if v has default value, set it to the actual center value
- //if( est_v == -1.0 )
- if( std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v),1.)*std::numeric_limits<double>::epsilon() )
- {
- est_v = this->cog.get_v();
- }
-
- // if the estimated position of the dot is out of the image, not need to continue,
- // return an error tracking
- if( !isInArea( (unsigned int) est_u, (unsigned int) est_v ) )
- {
- vpDEBUG_TRACE(3, "Initial pixel coordinates (%d, %d) for dot tracking are not in the area",
- (int) est_u, (int) est_v) ;
- return false;
- }
-
- bbox_u_min = (int)I.getWidth();
- bbox_u_max = 0;
- bbox_v_min = (int)I.getHeight();
- bbox_v_max = 0;
-
- // if the first point doesn't have the right level then there's no point to
- // continue.
- if( !hasGoodLevel( I, (unsigned int) est_u, (unsigned int) est_v ) )
- {
- vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates",
- (int) est_u, (int) est_v) ;
- return false;
- }
-
- // find the border
-
- if(!findFirstBorder(I, (unsigned int) est_u, (unsigned int) est_v,
- this->firstBorder_u, this->firstBorder_v)) {
-
- vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates",
- (int) est_u, (int) est_v) ;
- return false;
- }
-
- unsigned int dir = 6;
-
- // Determine the first element of the Freeman chain
- computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir);
- unsigned int firstDir = dir;
-
- // if we are now out of the image, return an error tracking
- if( !isInArea( this->firstBorder_u, this->firstBorder_v ) )
- {
- vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area",
- this->firstBorder_u, this->firstBorder_v);
- return false;
- }
-
- // store the new direction and dot border coordinates.
- direction_list.push_back( dir );
- vpImagePoint ip;
- ip.set_u( this->firstBorder_u );
- ip.set_v( this->firstBorder_v );
-
- ip_edges_list.push_back( ip );
-
- 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;
- m10 = 0.0;
- m01 = 0.0;
- m11 = 0.0;
- m20 = 0.0;
- m02 = 0.0;
- // while we didn't come back to the first point, follow the border
- do {
- // if it was asked, show the border
- if (graphics) {
- for(int t=0; t< (int)thickness; t++) {
- ip.set_u ( border_u + t);
- ip.set_v ( border_v );
-
- vpDisplay::displayPoint(I, ip, vpColor::red) ;
- }
- //vpDisplay::flush(I);
- }
-#ifdef DEBUG
- vpDisplay::displayPoint(I, border_v, border_u, vpColor::red);
- vpDisplay::flush(I);
-#endif
- // Determine the increments for the parameters
- computeFreemanParameters(border_u, border_v, dir, du, dv,
- dS, // surface
- dMu, dMv, // first order moments
- dMuv, dMu2, dMv2); // second order moment
-
- // Update the parameters
- border_u += du; // Next position on the border
- border_v += dv;
- m00 += dS; // enclosed area
- m10 += dMu; // First order moment along v axis
- m01 += dMv; // First order moment along u axis
- if (compute_moment) {
- m11 += dMuv; // Second order moment
- m20 += dMu2; // Second order moment along v axis
- m02 += dMv2; // Second order moment along u axis
- }
- // if we are now out of the image, return an error tracking
- if( !isInArea( (unsigned int)border_u, (unsigned int)border_v ) ) {
-
- vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v);
- // Can Occur on a single pixel dot located on the top border
- return false;
- }
-
- // store the new direction and dot border coordinates.
-
- direction_list.push_back( dir );
-
- ip.set_u( border_u );
- ip.set_v( border_v );
- ip_edges_list.push_back( ip );
-
- // vpDisplay::getClick(I);
-
- // update the extreme point of the dot.
- if( border_v < bbox_v_min ) bbox_v_min = border_v;
- if( border_v > bbox_v_max ) bbox_v_max = border_v;
- if( border_u < bbox_u_min ) bbox_u_min = border_u;
- if( border_u > bbox_u_max ) bbox_u_max = border_u;
-
- // move around the tracked entity by following the border.
- if (computeFreemanChainElement(I, (unsigned int)border_u, (unsigned int)border_v, dir) == false) {
- vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)",
- border_u, border_v);
- return false;
- }
-
- // vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v, dir);
-
- }
- while( (getFirstBorder_u() != (unsigned int)border_u
- || getFirstBorder_v() != (unsigned int)border_v
- || firstDir != dir) &&
- isInArea( (unsigned int)border_u, (unsigned int)border_v ) );
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- vpDisplay::flush(I);
-#endif
-#endif
-
- // if the surface is one or zero , the center of gravity wasn't properly
- // detected. Return an error tracking.
- //if( m00 == 0 || m00 == 1 )
- if( std::fabs(m00) <= std::numeric_limits<double>::epsilon()
- || std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.)*std::numeric_limits<double>::epsilon() )
- {
- vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected");
- return false;
- }
- else // compute the center
- {
- // this magic formula gives the coordinates of the center of gravity
- double tmpCenter_u = m10 / m00;
- double tmpCenter_v = m01 / m00;
-
- //Updates the central moments
- if (compute_moment)
- {
- mu11 = m11 - tmpCenter_u*m01;
- mu02 = m02 - tmpCenter_v*m01;
- 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 );
- }
-
- width = bbox_u_max - bbox_u_min + 1;
- height = bbox_v_max - bbox_v_min + 1;
- surface = m00;
-
- computeMeanGrayLevel(I);
- return true;
-}
-
-
-/*!
- Find the starting point on a dot border from an other point in the dot.
- the dot border is computed from this point.
-
- \param I : Image.
- \param u : The row coordinate of a pixel in the dot.
- \param v : The column coordinate of a pixel in the dot.
- \param border_u : The row coordinate of the found starting point.
- \param border_v : The column coordinate of the found starting point.
-
- \return false if the width of this dot was initialised and we already crossed
-the dot on more than the max possible width. Return true if success.
-
- \sa computeParameters()
-*/
-bool
- vpDot2::findFirstBorder(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &border_u,
- unsigned int &border_v)
-{
- // find the border
-
- // NOTE:
- // from here we use int and not double. This is because we don't have
- // rounding problems and it's actually more a trouble than smth else to
- // work with double when navigating around the dot.
- border_u = u;
- border_v = v;
- double epsilon =0.001;
-
-#ifdef DEBUG
- std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl;
-#endif
- while( hasGoodLevel( I, border_u+1, border_v ) &&
- border_u < area.getRight()/*I.getWidth()*/ ) {
- // if the width of this dot was initialised and we already crossed the dot
- // on more than the max possible width, no need to continue, return an
- // error tracking
- if( getWidth() > 0 && ( border_u - u ) > getWidth()/(getMaxSizeSearchDistancePrecision()+epsilon) ) {
- vpDEBUG_TRACE(3, "The found dot (%d, %d, %d) has a greater width than the required one", u, v, border_u);
- return false;
- }
-#ifdef DEBUG
- vpDisplay::displayPoint(I, border_v, border_u+1, vpColor::green);
- vpDisplay::flush(I);
-#endif
-
- border_u++;
- }
- return true;
-}
-
-
-/*!
-
- Considering a pixel (u, v) compute the next element of the Freeman chain
- code.
-
- According to the gray level of pixel (u, v) and his eight neighbors determine
- the next element of the chain in order to turn around the dot
- counterclockwise.
-
- \param I : The image we are working with.
- \param v : The row coordinate of a pixel on a border.
- \param u : The column coordinate of the pixel on a border.
- \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
- with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
-
- \return false if an element cannot be found. Occurs for example with an area
- constituted by a single pixel. Return true if success.
-*/
-bool
- vpDot2::computeFreemanChainElement(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &element)
-{
-
- if (hasGoodLevel( I, u, v )) {
- unsigned int _u = u;
- unsigned int _v = v;
- // get the point on the right of the point passed in
- updateFreemanPosition( _u, _v, (element + 2) %8 );
- if (hasGoodLevel( I, _u, _v )) {
- element = (element + 2) % 8; // turn right
- }
- else {
- unsigned int _u1 = u;
- unsigned int _v1 = v;
- updateFreemanPosition( _u1, _v1, (element + 1) %8 );
-
- if ( hasGoodLevel( I, _u1, _v1 )) {
- element = (element + 1) % 8; // turn diag right
- }
- else {
- unsigned int _u2 = u;
- unsigned int _v2 = v;
- updateFreemanPosition( _u2, _v2, element ); // same direction
-
- if ( hasGoodLevel( I, _u2, _v2 )) {
- //element = element; // keep same dir
- }
- else {
- unsigned int _u3 = u;
- unsigned int _v3 = v;
- updateFreemanPosition( _u3, _v3, (element + 7) %8 ); // diag left
-
- if ( hasGoodLevel( I, _u3, _v3 )) {
- element = (element + 7) %8; // turn diag left
- }
- else {
- unsigned int _u4 = u;
- unsigned int _v4 = v;
- updateFreemanPosition( _u4, _v4, (element + 6) %8 ); // left
-
- if ( hasGoodLevel( I, _u4, _v4 )) {
- element = (element + 6) %8 ; // turn left
- }
- else {
- unsigned int _u5 = u;
- unsigned int _v5 = v;
- updateFreemanPosition( _u5, _v5, (element + 5) %8 ); // left
-
- if ( hasGoodLevel( I, _u5, _v5 )) {
- element = (element + 5) %8 ; // turn diag down
- }
- else {
- unsigned int _u6 = u;
- unsigned int _v6 = v;
- updateFreemanPosition( _u6, _v6, (element + 4) %8 ); // left
-
- if ( hasGoodLevel( I, _u6, _v6 )) {
- element = (element + 4) %8 ; // turn down
- }
- else {
- unsigned int _u7 = u;
- unsigned int _v7 = v;
- updateFreemanPosition( _u7, _v7, (element + 3) %8 ); // diag
-
- if ( hasGoodLevel( I, _u7, _v7 )) {
- element = (element + 3) %8 ; // turn diag right down
- }
- else {
- // No neighbor with a good level
- //
- return false;
- }
- }
- }
- }
- }
- }
- }
- }
- }
-
- else {
- return false;
- }
-
- return true;
-
-}
-
-/*!
-
- Given the previous position of a pixel (u_p, v_p) on the dot border and the
- direction to reach the next pixel on the border, compute Freeman parameters.
-
- \param u_p : Previous value of the row coordinate of a pixel on a border.
- \param v_p : Previous value of the column coordinate of a pixel on a border.
- \param du : Increment to go from previous to next pixel on the dot border.
- \param dv : Increment to go from previous to next pixel on the dot border.
-
- \param dS : Enclosed area increases. Cumulated values of dS gives m00.
-
- \param dMu : First order moment along v axis increases. Cumulated values of
- dMu gives m10.
-
- \param dMv : First order moment along u axis increases. Cumulated values of
- dMv gives m01.
-
- \param dMuv : Moment increases. Cumulated values of dMuv gives m11.
-
- \param dMu2 : Second order moment along v axis increases. Cumulated values of
- dMu2 gives m20.
-
- \param dMv2 : Second order moment along u axis increases. Cumulated values of
- dMv2 gives m02.
-
- Considering the previous coordinates (u_p, v_p) of a pixel on a border, the
- next coordinates (u, v) are given by: u = u_p + du and v = v_p + dv
-
-
-*/
-void
- vpDot2::computeFreemanParameters(const int &u_p,
- const int &v_p,
- unsigned int &element,
- int &du, int &dv,
- float &dS,
- float &dMu, float &dMv,
- float &dMuv,
- float &dMu2, float &dMv2)
-{
- du = 0;
- dv = 0;
- dMuv = 0;
- dMu2 = 0;
- dMv2 = 0;
-
- /*
- 3 2 1
- \ | /
- \|/
- 4 ------- 0
- /|\
- / | \
- 5 6 7
- */
- switch(element) {
- case 0: // go right
- du = 1;
- dS = (float) v_p;
- dMu = 0.0;
- dMv = (float)(0.5 * v_p * v_p);
- if (compute_moment) {
- dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
- dMu2 = 0;
- dMv2 = (float)(1.0/ 3. * v_p * v_p * v_p);
- }
- break;
-
- case 1: // go right top
- du = 1;
- dv = 1;
- dS = (float)(v_p + 0.5);
- dMu = - (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
- dMv = (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
- if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25+half_u_p) + v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
- dMu2 = (float)(-1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)( 1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1./12.0);
- }
- break;
-
- case 2: // go top
- dv = 1;
- dS = 0.0;
- dMu = (float)(- 0.5 * u_p * u_p);
- dMv = 0.0;
- if (compute_moment) {
- dMuv = 0;
- dMu2 = (float)(-1.0/ 3. * u_p * u_p * u_p);
- dMv2 = 0;
- }
- break;
-
- case 3:
- du = -1;
- dv = 1;
- dS = (float)(- v_p - 0.5);
- dMu = - (float)(0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
- dMv = - (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
- if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25-half_u_p) + v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
- dMu2 = (float)(-1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)(-1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1./12.0);
- }
- break;
-
- case 4:
- du = -1;
- dS = (float)(- v_p);
- dMv = (float)(- 0.5 * v_p * v_p);
- dMu = 0.0;
- if (compute_moment) {
- dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
- dMu2 = 0;
- dMv2 = (float)(-1.0/ 3. * v_p * v_p * v_p);
- }
- break;
-
- case 5:
- du = -1;
- dv = -1;
- dS = (float)(- v_p + 0.5);
- dMu = (float)( 0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
- dMv = (float)(- (0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0));
- if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25-half_u_p) - v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
- dMu2 = (float)( 1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)(-1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
- }
- break;
-
- case 6:
- dv = -1;
- dS = 0.0;
- dMu = (float)(0.5 * u_p * u_p);
- dMv = 0.0;
- if (compute_moment) {
- dMuv = 0;
- dMu2 = (float)(1.0/ 3. * u_p * u_p * u_p);
- dMv2 = 0;
- }
- break;
-
- case 7:
- du = 1;
- dv = -1;
- dS = (float)(v_p - 0.5);
- dMu = (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
- dMv = (float)(0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0);
- if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25+half_u_p) - v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
- dMu2 = (float)(1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1./12.0);
- dMv2 = (float)(1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
- }
- break;
- }
-}
-
-
-/*!
-
- From a pixel coordinate and a direction, update the pixel coordinates after
- moving forward.
-
- \param v : The row coordinate of the pixel, updated by this method.
-
- \param u : The column coordinate of the pixel, updated by this method.
-
- \param dir : The direction in the image, 0=right, 1, 2=down, 3, 4=left, 5,
- 6=up and 7.
-
-*/
-void vpDot2::updateFreemanPosition( unsigned int& u, unsigned int& v,
- const unsigned int &dir )
-{
- switch(dir) {
- case 0: u += 1; break;
- case 1: u += 1; v += 1; break;
- case 2: v += 1; break;
- case 3: u -= 1; v += 1; break;
- case 4: u -= 1; break;
- case 5: u -= 1; v -= 1; break;
- case 6: v -= 1; break;
- case 7: u += 1; v -= 1; break;
- }
-}
-
-/*!
-
- Test if a pixel is in the image. Points of the border are not
- considered to be in the image. Call the isInImage( vpImage<unsigned
- char> &I, const vpImagePoint &) method.
-
- \param I : The image.
-
- \return true if the pixel of coordinates (posI, posJ) is in the image and
- false otherwise.
-*/
-bool vpDot2::isInImage(const vpImage<unsigned char> &I) const
-{
- return isInImage( I, cog);
-}
-
-/*!
-
- Test if a pixel is in the image. Points of the border are not considered to
- be in the image.
-
- \param I : The image.
- \param ip : An image point.
-
- \return true if the image point \e ip is in the image and false
- otherwise.
-*/
-bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const
-{
- unsigned int h = I.getHeight();
- unsigned int w = I.getWidth();
- double u = ip.get_u();
- double v = ip.get_v();
-
- if( u < 0 || u >= w ) return false;
- if( v < 0 || v >= h ) return false;
- return true;
-}
-
-/*!
-
- Test if a pixel is in a region of interest. Points of the border are not considered to
- be in the area.
-
- \param u : The column coordinate of the pixel.
- \param v : The row coordinate of the pixel .
-
- \return true if the pixel of coordinates (u, v) is in the image and false
- otherwise.
-*/
-bool vpDot2::isInArea( const unsigned int &u, const unsigned int &v) const
-{
- unsigned int area_u_min = (unsigned int) area.getLeft();
- unsigned int area_u_max = (unsigned int) area.getRight();
- unsigned int area_v_min = (unsigned int) area.getTop();
- unsigned int area_v_max = (unsigned int) area.getBottom();
-
- if( u < area_u_min || u > area_u_max ) return false;
- if( v < area_v_min || v > area_v_max ) return false;
- return true;
-}
-
-
-/*!
-
- Get the search grid size used to found a dot in a region of interest. This grid is used to
- parse only some pixels of the search area.
-
- \param gridWidth : Number of pixels between to vertical lines of the grid
-
- \param gridHeight : Number of pixels between to horizontal lines of the grid
-
-
-*/
-void vpDot2::getGridSize( unsigned int &gridWidth, unsigned int &gridHeight )
-{
- // first get the research grid width and height Note that
- // 1/sqrt(2)=cos(pi/4). The grid squares should be small enough to be
- // contained in the dot. We gent this here if the dot is a perfect disc.
- // More accurate criterium to define the grid should be implemented if
- // necessary
- gridWidth = (unsigned int) (getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.));
- gridHeight = (unsigned int) (getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0));
-
- if( gridWidth == 0 ) gridWidth = 1;
- if( gridHeight == 0 ) gridHeight = 1;
-}
-
-
-
-/*!
-
- Compute an approximation of mean gray level of the dot.
- We compute it by searching the mean of vertical and diagonal points
- which gray is between min and max gray level.
-
- \param I: The image.
-
- \return the mean gray level
-
-
-*/
-void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char>& I)
-{
- int cog_u = (int)cog.get_u();
- int cog_v = (int)cog.get_v();
-
- unsigned int sum_value =0;
- unsigned int nb_pixels =0;
-
- for(unsigned int i=(unsigned int)this->bbox_u_min; i <=(unsigned int)this->bbox_u_max ; i++){
- unsigned int pixel_gray =(unsigned int) I[(unsigned int)cog_v][i];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
- sum_value += pixel_gray;
- nb_pixels ++;
- }
- }
- for(unsigned int i=(unsigned int)this->bbox_v_min; i <=(unsigned int)this->bbox_v_max ; i++){
- unsigned char pixel_gray =I[i][(unsigned int)cog_u];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
- sum_value += pixel_gray;
- nb_pixels ++;
- }
- }
- if(nb_pixels < 10){ //could be good to choose the min nb points from area of dot
- //add diagonals points to have enough point
- int imin,imax;
- if( (cog_u - bbox_u_min) > (cog_v - bbox_v_min)){
- imin=cog_v - bbox_v_min;
- }
- else{ imin = cog_u - bbox_u_min;}
- if( (bbox_u_max - cog_u) > (bbox_v_max - cog_v)){
- imax=bbox_v_max - cog_v;
- }
- else{ imax = bbox_u_max - cog_u;}
- for(int i=-imin; i <=imax ; i++){
- unsigned int pixel_gray =(unsigned int) I[(unsigned int)(cog_v + i)][(unsigned int)(cog_u + i)];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
- sum_value += pixel_gray;
- nb_pixels ++;
- }
- }
-
- if( (cog_u - bbox_u_min) > (bbox_v_max - cog_v)){
- imin = bbox_v_max - cog_v;
- }
- else{ imin = cog_u - bbox_u_min;}
- if( (bbox_u_max - cog_u) > (cog_v - bbox_v_min)){
- imax = cog_v - bbox_v_min;
- }
- else{ imax = bbox_u_max - cog_u;}
-
- for(int i=-imin; i <=imax ; i++){
- unsigned char pixel_gray =I[(unsigned int)(cog_v - i)][(unsigned int)(cog_u + i)];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
- sum_value += pixel_gray;
- nb_pixels ++;
- }
- }
- }
-
- if(nb_pixels== 0){
- //should never happen
- throw(vpTrackingException(vpTrackingException::notEnoughPointError,"No point was found"));
- }
- else{
- mean_gray_level = sum_value/nb_pixels;
- }
-}
-
-
-/*!
- Define a number of dots from a file.
- If the file does not exist, define it by clicking an image, the dots are then saved into the file.
-
- If the dots from the file cannot be tracked in the image, will ask to click them.
-
- \param dot : dot2 array
- \param n : number of dots, array dimension
- \param dotFile : path for the file
- \param I : image
- \param col : color to print the dots (default Blue)
- \param trackDot : if true, tracks the dots in the image, if false simply loads the coordinates (default true)
-
- \return an nx2 matrix with the coordinates of the dots
-*/
-vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col, bool trackDot)
-{
- vpMatrix Cogs(n, 2);
- vpImagePoint cog;
- unsigned int i;
- bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
- if(fromFile)
- {
- vpMatrix::loadMatrix(dotFile, Cogs);
- std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
- }
-
- // test number of cogs in file
- if(Cogs.getRows() < n)
- {
- std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
- fromFile = false;
- }
-
- // read from file and tracks the dots
- if(fromFile)
- {
- try
- {
- for(i=0;i<n;++i)
- {
- cog.set_uv(Cogs[i][0], Cogs[i][1]);
- dot[i].setGraphics(true);
- dot[i].setCog(cog);
- if(trackDot)
- {
- dot[i].initTracking(I,cog);
- dot[i].track(I);
- vpDisplay::displayCross(I, cog, 10, col);
- }
- }
- }
- catch(...)
- {
- std::cout << "Cannot track dots from file" << std::endl;
- fromFile = false;
- }
- vpDisplay::flush(I);
-
- // check that dots are far away ones from the other
- double d;
- for(i=0;i<n && fromFile;++i)
- {
- d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth()));
- for(unsigned int j=0;j<n && fromFile;++j)
- if(j!=i)
- if(dot[i].getDistance(dot[j]) < d)
- {
- fromFile = false;
- std::cout << "Dots from file seem incoherent" << std::endl;
- }
- }
- }
-
- if(!fromFile)
- {
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
- for (i = 0; i < n; i++)
- {
- if(trackDot)
- {
- dot[i].setGraphics(true);
- dot[i].initTracking(I);
- cog = dot[i].getCog();
- }
- else
- {
- vpDisplay::getClick(I, cog);
- dot[i].setCog(cog);
- }
- Cogs[i][0] = cog.get_u();
- Cogs[i][1] = cog.get_v();
- vpDisplay::displayCross(I, cog, 10, col);
- vpDisplay::flush(I);
- }
- }
-
- if (!fromFile & (dotFile != ""))
- {
- vpMatrix::saveMatrix(dotFile, Cogs);
- std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
- }
-
- // back to non graphic mode
- for(i=0;i<n;++i)
- dot[i].setGraphics(false);
-
- return Cogs;
-}
-
-/*!
- Tracks a number of dots in an image and displays their trajectories
-
- \param dot : dot2 array
- \param n : number of dots, array dimension
- \param I : image
- \param cogs : vector of vpImagePoint that will be updated with the new dots, will be displayed in green
- \param cogStar (optional) : array of vpImagePoint indicating the desired position (default NULL), will be displayed in red
-*/
-void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I, std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar)
-{
- unsigned int i;
- // tracking
- for(i=0;i<n;++i)
- {
- dot[i].track(I);
- cogs.push_back(dot[i].getCog());
- }
- // trajectories
- for(i=n;i<cogs.size();++i)
- vpDisplay::displayCircle(I,cogs[i],4,vpColor::green,true);
- // initial position
- for(i=0;i<n;++i)
- vpDisplay::displayCircle(I,cogs[i],4,vpColor::blue,true);
- // if exists, desired position
- if(cogStar != NULL)
- for(i=0;i<n;++i)
- {
- vpDisplay::displayDotLine(I,cogStar[i],dot[i].getCog(),vpColor::red);
- vpDisplay::displayCircle(I,cogStar[i],4,vpColor::red,true);
- }
- vpDisplay::flush(I);
-}
-
-/*!
-
- Display the dot center of gravity and its list of edges.
-
- \param I : The image used as background.
-
- \param cog : The center of gravity.
-
- \param edges_list : The list of edges;
-
- \param color : Color used to display the dot.
-
- \param thickness : Thickness of the dot.
-*/
-void vpDot2::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
-{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-/*!
-
- Display the dot center of gravity and its list of edges.
-
- \param I : The image used as background.
-
- \param cog : The center of gravity.
-
- \param edges_list : The list of edges;
-
- \param color : Color used to display the dot.
-
- \param thickness : Thickness of the dot.
-*/
-void vpDot2::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
-{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
- std::list<vpImagePoint>::const_iterator it;
-
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
- vpDisplay::displayPoint(I, *it, color);
- }
-}
-
-/*!
- Writes the dot center of gravity coordinates in the frame (i,j) (For more details
- about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
- and returns a reference to the stream.
-*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d) {
- return (os << "(" << d.getCog() << ")" ) ;
-} ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated Return the area of the dot.
-
- This function is deprecated since it is mis named. Surface means here area.
- You should rather use getArea().
-
- The area of the dot is also given by \f$|m00|\f$.
-
- \sa getArea()
-*/
-double vpDot2::getSurface() const
-{
- return fabs(surface);
-}
-
-/*!
-
- \deprecated Set the surface of the dot. This is meant to be used to search a dot in a region of interest.
-
- This function is deprecated since it is mis named. You should rather use setArea()
-
- \param s : Here surface means area. Area of a dot to search in a region of interest.
-
- \sa setWidth(), setHeight(), setArea(), setSizePrecision()
-
-*/
-void vpDot2::setSurface( const double & s )
-{
- this->surface = s;
-}
-#endif
diff --git a/src/tracking/dots/vpDot2.h b/src/tracking/dots/vpDot2.h
deleted file mode 100644
index 15a0d30..0000000
--- a/src/tracking/dots/vpDot2.h
+++ /dev/null
@@ -1,566 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpDot2.h 2135 2009-04-29 13:51:31Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Track a white dot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*
- \file vpDot2.h
- \brief This tracker is meant to track some zones on a vpImage.
-*/
-
-#ifndef vpDot2_hh
-#define vpDot2_hh
-
-#include <visp/vpImage.h>
-#include <visp/vpRect.h>
-#include <visp/vpTracker.h>
-#include <visp/vpColor.h>
-#include <visp/vpImagePoint.h>
-
-#include <vector>
-#include <list>
-
-/*!
- \class vpDot2
-
- \ingroup TrackingImageBasic
-
- \brief This tracker is meant to track a blob (connex pixels with same
- gray level) on a vpImage.
-
- The underground algorithm is based on a binarisation of the image
- and then on a contour detection using the Freeman chain coding to
- determine the blob characteristics (location, moments, size...).
-
- The binarisation is done using gray level minimum and maximum values
- that define the admissible gray levels of the blob. You can specify these
- levels by setGrayLevelMin() and setGrayLevelMax(). These levels are also
- set automatically by setGrayLevelPrecision(). The algorithm allows
- to track white objects on a black background and vice versa.
-
- When a blob is found, some tests are done to see if it is valid:
- - A blob is considered by default as ellipsoid. The found blob could
- be rejected if the shape is not ellipsoid. To determine if the shape
- is ellipsoid the algorithm consider an inner and outside ellipse.
- Sampled points on these two ellipses should have the right gray levels.
- Along the inner ellipse the sampled points should have gray levels
- that are in the gray level minimum and maximum bounds, while
- on the outside ellipse, the gray levels should be out of the gray level
- bounds. To set the percentage of the sample points which should have the right
- levels use setEllipsoidBadPointsPercentage(). The distance between the
- inner ellpsoid and the blob contour, as well the distance between the
- blob contour and the outside ellipse is fixed by setEllipsoidShapePrecision().
- If you want to track a non ellipsoid shape, and turn off this validation test,
- you have to call setEllipsoidShapePrecision(0).
- - The width, height and surface of the blob are compared to the
- corresponding values of the previous blob. If they differ to much
- the blob could be rejected. To set the admissible distance you can
- use setSizePrecision().
-
- Note that track() and searchDotsInArea() are the most important features
- of this class.
-
- - track() estimate the current position of the dot using its previous
- position, then try to compute the new parameters of the dot. If everything
- went ok, tracking succeeds, otherwise we search this dot in a window
- around the last position of the dot.
-
- - searchDotsInArea() enable to find dots similar to this dot in a window. It
- is used when there was a problem performing basic tracking of the dot, but
- can also be used to find a certain type of dots in the full image.
-
- The following sample code available in tutorial-blob-tracker-live-firewire.cpp shows how to
- grab images from a firewire camera, track a blob and display the tracking
- results.
-
- \include tutorial-blob-tracker-live-firewire.cpp
- A line by line explanation of the previous example is provided in
- \ref tutorial-tracking-blob.
-
- This other example available in tutorial-blob-auto-tracker.cpp shows firstly
- how to detect in the first image all the blobs that match some characteristics
- in terms of size, area, gray level. Secondly, it shows how to track all the
- dots that are detected.
-
- \include tutorial-blob-auto-tracker.cpp
- A line by line explanation of this last example is also provided in
- \ref tutorial-tracking-blob, section \ref tracking_blob_tracking.
-
- \sa vpDot
-*/
-class VISP_EXPORT vpDot2 : public vpTracker
-{
-public:
- vpDot2();
- vpDot2(const vpImagePoint &ip) ;
- vpDot2(const vpDot2& twinDot );
- virtual ~vpDot2();
-
- static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col = vpColor::blue, bool trackDot = true);
-
- void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
- unsigned int thickness=1) const;
-
- double getArea() const;
- /*!
-
- Return the dot bounding box.
-
- \sa getWidth(), getHeight()
-
- */
- inline vpRect getBBox() const {
- vpRect bbox;
-
- bbox.setRect(this->bbox_u_min,
- this->bbox_v_min,
- this->bbox_u_max - this->bbox_u_min + 1,
- this->bbox_v_max - this->bbox_v_min + 1);
-
- return (bbox);
- };
- /*!
- Return the location of the dot center of gravity.
-
- \return The coordinates of the center of gravity.
- */
- inline vpImagePoint getCog() const {
- return cog;
- }
-
- double getDistance( const vpDot2& distantDot ) const;
- /*!
-
- Return the list of all the image points on the dot
- border.
-
- \param edges_list : The list of all the images points on the dot
- border. This list is update after a call to track().
-
- */
- void getEdges(std::list<vpImagePoint> &edges_list) const {
- edges_list = this->ip_edges_list;
- };
- /*!
-
- Return the list of all the image points on the dot
- border.
-
- \return The list of all the images points on the dot
- border. This list is update after a call to track().
-
- */
- std::list<vpImagePoint> getEdges() const {
- return(this->ip_edges_list);
- };
- /*!
- Get the percentage of sampled points that are considered non conform
- in terms of the gray level on the inner and the ouside ellipses.
-
- \sa setEllipsoidBadPointsPercentage()
- */
- double getEllipsoidBadPointsPercentage() const
- {
- return allowedBadPointsPercentage_;
- }
-
- double getEllipsoidShapePrecision() const;
- void getFreemanChain(std::list<unsigned int> &freeman_chain) const;
-
- inline double getGamma() const {return this->gamma;};
- /*!
- Return the color level of pixels inside the dot.
-
- \sa getGrayLevelMax()
- */
- inline unsigned int getGrayLevelMin() const {
- return gray_level_min;
- };
- /*!
- Return the color level of pixels inside the dot.
-
- \sa getGrayLevelMin()
- */
- inline unsigned int getGrayLevelMax() const {
- return gray_level_max;
- };
- double getGrayLevelPrecision() const;
-
- double getHeight() const;
- double getMaxSizeSearchDistancePrecision() const;
- /*!
- \return The mean gray level value of the dot.
- */
- double getMeanGrayLevel() const {
- return (this->mean_gray_level);
- };
- double getSizePrecision() const;
- double getWidth() const;
-
- void initTracking(const vpImage<unsigned char>& I, unsigned int size = 0);
- void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int size = 0);
- void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int gray_lvl_min, unsigned int gray_lvl_max,
- unsigned int size = 0 );
-
- vpDot2& operator=(const vpDot2& twinDot );
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d);
-
- void print(std::ostream& os) { os << *this << std::endl ; }
- void searchDotsInArea(const vpImage<unsigned char>& I,
- int area_u, int area_v,
- unsigned int area_w, unsigned int area_h, std::list<vpDot2> &niceDots );
-
- void searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots );
-
- void setArea( const double & area );
- /*!
- Initialize the dot coordinates with \e ip.
- */
- inline void setCog(const vpImagePoint &ip) {
- this->cog = ip;
- }
- /*!
-
- Activates the dot's moments computation.
-
- \param activate true, if you want to compute the moments. If false, moments
- are not computed.
-
- Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
- vpDot::m20, vpDot::m02.
-
- The coordinates of the region's centroid (u, v) can be computed from the
- moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$.
-
- */
- void setComputeMoments(const bool activate) { compute_moment = activate; }
-
- /*!
- Set the percentage of sampled points that are considered non conform
- in terms of the gray level on the inner and the ouside ellipses.
- Points located on the inner ellipse should have the same gray level
- than the blob, while points located on the outside ellipse should
- have a different gray level.
-
- \param percentage : Percentage of points sampled with bad gray level
- on the inner and outside ellipses that are admissible. 0 means
- that all the points should have a right level, while a value of 1
- means that all the points can have a bad gray level.
- */
- void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
- {
- if (percentage < 0.)
- allowedBadPointsPercentage_ = 0.;
- else if (percentage > 1.)
- allowedBadPointsPercentage_ = 1.;
- else
- allowedBadPointsPercentage_ = percentage;
- }
-
- void setEllipsoidShapePrecision(const double & ellipsoidShapePrecision);
- /*!
- Activates the display of the border of the dot during the tracking.
- The default thickness of the overlayed drawings can be modified using
- setGraphicsThickness().
-
- \warning To effectively display the dot graphics a call to
- vpDisplay::flush() is needed.
-
- \param activate If true, the border of the dot will be painted. false to
- turn off border painting.
-
- \sa setGraphicsThickness()
- */
- void setGraphics(const bool activate) { graphics = activate ; }
- /*!
- Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
-
- \sa setGraphics()
- */
- void setGraphicsThickness(unsigned int t) {this->thickness = t;};
- /*!
-
- Set the color level of the dot to search a dot in a region of interest. This level will be
- used to know if a pixel in the image belongs to the dot or not. Only pixels
- with higher level can belong to the dot. If the level is lower than the
- minimum level for a dot, set the level to MIN_IN_LEVEL.
-
- \param min : Color level of a dot to search in a region of interest.
-
- \sa setGrayLevelMax(), setGrayLevelPrecision()
-
- */
- inline void setGrayLevelMin( const unsigned int & min ) {
- if (min > 255)
- this->gray_level_min = 255;
- else
- this->gray_level_min = min;
- };
-
- /*!
-
- Set the color level of pixels surrounding the dot. This is meant to be used
- to search a dot in a region of interest.
-
- \param max : Intensity level of a dot to search in a region of interest.
-
- \sa setGrayLevelMin(), setGrayLevelPrecision()
- */
- inline void setGrayLevelMax( const unsigned int & max ) {
- if (max > 255)
- this->gray_level_max = 255;
- else
- this->gray_level_max = max;
- };
- void setGrayLevelPrecision( const double & grayLevelPrecision );
- void setHeight( const double & height );
- void setMaxSizeSearchDistancePrecision(const double & maxSizeSearchDistancePrecision);
- void setSizePrecision( const double & sizePrecision );
- void setWidth( const double & width );
-
- void track(const vpImage<unsigned char> &I);
- void track(const vpImage<unsigned char> &I, vpImagePoint &cog);
-
- static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
- std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar = NULL);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated double getSurface() const;
- vp_deprecated void setSurface( const double & surface );
-#endif
-
-public:
- double m00; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
- with \f$i = j = 0 \f$. This moment corresponds to the dot
- surface.
-
- \sa setComputeMoments()
- */
- double m10; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 0 \f$. \f$ m_{10} \f$
- corresponds to the inertia first order moment along the v axis.
-
- \sa setComputeMoments()
- */
- double m01; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
- obtained with \f$i = 0 \f$ and \f$ j = 1 \f$. \f$ m_{01} \f$
- corresponds to the inertia first order moment along the u axis.
-
- \sa setComputeMoments()
- */
- double m11; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
-
- \sa setComputeMoments()
- */
- double m20; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
- obtained with \f$i = 2 \f$ and \f$ j = 0 \f$. \f$ m_{20} \f$
- corresponds to the inertia second order moment along the v
- axis.
-
- \sa setComputeMoments()
- */
- double m02; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
- obtained with \f$i = 0 \f$ and \f$ j = 2 \f$. \f$ m_{02} \f$
- corresponds to the inertia second order moment along the u
- axis.
-
- \sa setComputeMoments()
- */
- double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
- \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
- double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
- \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
-
- \sa setComputeMoments()
- */
- double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
- \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
-
-private:
- virtual bool isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot);
-
- virtual bool hasGoodLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const;
- virtual bool hasReverseLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const;
-
- virtual vpDot2* getInstance();
-
- void init();
-
- bool computeParameters(const vpImage<unsigned char> &I,
- const double &u = -1.0,
- const double &v = -1.0);
-
-
-
- bool findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u,
- const unsigned int &v, unsigned int &border_u,
- unsigned int &border_v);
- void computeMeanGrayLevel(const vpImage<unsigned char>& I);
-
- /*!
-
- Get the starting point on a dot border. The dot border is
- computed from this point.
-
- \sa getFirstBorder_v()
-
- */
- unsigned int getFirstBorder_u() const {
- return this->firstBorder_u;
- }
- /*!
-
- Get the starting point on a dot border. The dot border is
- computed from this point.
-
- \sa getFirstBorder_u()
-
- */
- unsigned int getFirstBorder_v() const {
- return this->firstBorder_v;
- }
-
- bool computeFreemanChainElement(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &element);
- void computeFreemanParameters(const int &u_p,
- const int &v_p, unsigned int &element,
- int &du, int &dv, float &dS,
- float &dMu, float &dMv,
- float &dMuv,
- float &dMu2, float &dMv2);
- void updateFreemanPosition( unsigned int& u, unsigned int& v,
- const unsigned int &dir );
-
-
- bool isInImage(const vpImage<unsigned char> &I ) const;
- bool isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const;
-
- bool isInArea(const unsigned int &u, const unsigned int &v) const;
-
- void getGridSize( unsigned int &gridWidth, unsigned int &gridHeight );
- void setArea(const vpImage<unsigned char> &I,
- int u, int v, unsigned int w, unsigned int h);
- void setArea(const vpImage<unsigned char> &I);
- void setArea(const vpRect & a);
-
- unsigned char getMeanGrayLevel(vpImage<unsigned char>& I) const;
- //! coordinates (float) of the point center of gravity
- vpImagePoint cog;
-
- double width;
- double height;
- double surface;
- unsigned int gray_level_min; // minumum gray level for the dot.
- // pixel with lower level don't belong
- // to this dot.
-
- unsigned int gray_level_max; // maximum gray level for the dot.
- // pixel with higher level don't belong
- // to this dot.
- double mean_gray_level; // Mean gray level of the dot
- double grayLevelPrecision ;
- double gamma ;
- double sizePrecision ;
- double ellipsoidShapePrecision;
- double maxSizeSearchDistancePrecision;
- double allowedBadPointsPercentage_;
- // Area where the dot is to search
- vpRect area;
-
- // other
- std::list<unsigned int> direction_list;
- std::list<vpImagePoint> ip_edges_list;
-
- // flag
- bool compute_moment ; // true moment are computed
- bool graphics ; // true for graphic overlay display
-
- unsigned int thickness; // Graphics thickness
-
- // Bounding box
- int bbox_u_min, bbox_u_max, bbox_v_min, bbox_v_max;
-
- // The first point coodinate on the dot border
- unsigned int firstBorder_u;
- unsigned int firstBorder_v;
-
-//Static funtions
-public:
- static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
-
-};
-
-#endif
-
-
diff --git a/src/tracking/feature-builder/vpFeatureBuilder.h b/src/tracking/feature-builder/vpFeatureBuilder.h
deleted file mode 100644
index 004bc7b..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilder.h
+++ /dev/null
@@ -1,181 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilder.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#ifndef vpFeatureBuilder_H
-#define vpFeatureBuilder_H
-
-/*!
- \file vpFeatureBuilder.h
- \brief class that defines conversion between tracker and visual feature
-*/
-#include <visp/vpConfig.h>
-
-// tracker
-#include <visp/vpDot.h>
-#include <visp/vpDot2.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpMeEllipse.h>
-
-
-// forward projection tracker
-#include <visp/vpPoint.h>
-#include <visp/vpLine.h>
-#include <visp/vpSphere.h>
-#include <visp/vpCircle.h>
-#include <visp/vpCylinder.h>
-
-// visual feature
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeaturePointPolar.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpFeaturePoint3D.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpFeatureVanishingPoint.h>
-#include <visp/vpFeatureSegment.h>
-
-// others
-#include <visp/vpImagePoint.h>
-
-//pixel / meter conversion
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMeterPixelConversion.h>
-
-
-/*!
- \class vpFeatureBuilder
-
- \ingroup VsFeatureBuilder
- \brief Class that defines conversion between trackers and visual features.
-*/
-class VISP_EXPORT vpFeatureBuilder
-{
-public:
- // create vpFeaturePoint feature
- static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
- const vpDot &d) ;
- static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
- const vpDot2 &d) ;
- static void create(vpFeaturePoint &s, const vpCameraParameters &cam,
- const vpImagePoint &t) ;
- static void create(vpFeaturePoint &s, const vpPoint &p) ;
- static void create(vpFeaturePoint &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p) ;
-
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2 ) ;
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2) ;
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpImagePoint &ip1, const vpImagePoint &ip2);
- static void create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2);
-
- // create vpFeaturePointPolar feature
- static void create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpDot &dot) ;
- static void create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpDot2 &dot) ;
- static void create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpImagePoint &iP) ;
- static void create(vpFeaturePointPolar &s, const vpPoint &p) ;
- static void create(vpFeaturePointPolar &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p) ;
-
- // create vpFeaturePoint3D feature
- static void create(vpFeaturePoint3D &s, const vpPoint &p ) ;
-
- // create vpFeatureLine feature
- static void create(vpFeatureLine &s, const vpLine &l ) ;
- static void create(vpFeatureLine &s, const vpCylinder &c, const int line) ;
-
- static void create(vpFeatureLine &s,
- const vpCameraParameters &cam,
- const vpMeLine &mel) ;
-
- //! create vpFeatureEllipse feature
- static void create(vpFeatureEllipse &s, const vpCircle &c) ;
- static void create(vpFeatureEllipse &s, const vpSphere &sphere) ;
- static void create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot &d ) ;
- static void create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot2 &d ) ;
- static void create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpMeEllipse &d ) ;
-
-
- /*!
- create vpFeatureVanishingPoint feature from the 2D coordinates of a point
- in the image plane
- */
- static void create(vpFeatureVanishingPoint &s, const vpPoint &p);
- /*!
- create vpFeatureVanishingPoint feature from 2 FeatureLine, ie lines in
- the image plane (error if the 2 lines are parallel)
- */
- static void create(vpFeatureVanishingPoint &s, const vpFeatureLine &l1, const vpFeatureLine &l2 );
- /*!
- create vpFeatureVanishingPoint feature from 2 Lines, (error if the 2
- lines are parallel in the image plane)
- */
- static void create(vpFeatureVanishingPoint &s, const vpLine &l1, const vpLine &l2 );
-
-
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderEllipse.cpp b/src/tracking/feature-builder/vpFeatureBuilderEllipse.cpp
deleted file mode 100644
index e85ad42..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderEllipse.cpp
+++ /dev/null
@@ -1,342 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderEllipse.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature ellipse.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderEllipse.cpp
- \brief conversion between tracker
- and visual feature Ellipse
-*/
-
-#include <visp/vpFeatureBuilder.h>
-
-
-#include <visp/vpMath.h>
-
-
-
-/*!
- Initialize an ellipse feature thanks to a vpCircle.
- The vpFeatureEllipse is initialized thanks to the parameters of the circle in the camera frame and in the image plan.
- All the parameters are given in meter.
-
- \warning To be sure that the vpFeatureEllipse is well initialized,
- you have to be sure that at least the circle coordinates in the image
- plan and in the camera frame are computed and stored in the vpCircle.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpCircle used to create the vpFeatureEllipse.
-*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCircle &t )
-{
- try
- {
-
- // 3D data
- double alpha = t.cP[0] ;
- double beta = t.cP[1] ;
- double gamma = t.cP[2] ;
-
- double X0 = t.cP[3] ;
- double Y0 = t.cP[4] ;
- double Z0 = t.cP[5] ;
-
- // equation p 318 prior eq (39)
- double d = alpha*X0 + beta*Y0 + gamma*Z0 ;
-
- double A = alpha / d ;
- double B = beta / d ;
- double C = gamma / d ;
-
- s.setABC(A,B,C) ;
-
-
- //2D data
- s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-
-/*!
- Initialize an ellipse feature thanks to a vpSphere.
- The vpFeatureEllipse is initialized thanks to the parameters
- of the sphere in the camera frame and in the image plan.
- All the parameters are given in meter.
-
- \warning To be sure that the vpFeatureEllipse is well initialized,
- you have to be sure that at least the sphere coordinates in the image
- plan and in the camera frame are computed and stored in the vpSphere.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpSphere used to create the vpFeatureEllipse.
-*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t)
-{
- try
- {
-
- // 3D data
- double X0 = t.cP[0] ;
- double Y0 = t.cP[1] ;
- double Z0 = t.cP[2] ;
- double R = t.cP[3] ;
-
- double d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) -
- vpMath::sqr(R) ;
-
-
- double A = X0 / d ;
- double B = Y0 / d ;
- double C = Z0 / d ;
-
- s.setABC(A,B,C) ;
-
- //2D data
- s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
-
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Initialize an ellipse feature thanks to a vpDot and camera parameters.
- The vpFeatureEllipse is initialized thanks to the parameters
- of the dot given in pixel. The camera parameters are used to convert the
- pixel parameters to parameters given in meter.
-
- \warning With a vpDot there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
-
- \param s : Visual feature to initialize.
-
- \param cam : The parameters of the camera used to acquire the image containing the vpDot.
-
- \param t : The vpDot used to create the vpFeatureEllipse.
-*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot &t )
-{
- try
- {
-
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
-
- mp[0][0] = t.m00 ;
- mp[1][0] = t.m10;
- mp[0][1] = t.m01 ;
- mp[2][0] = t.m20 ;
- mp[1][1] = t.m11 ;
- mp[0][2] = t.m02 ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-
-
-/*!
- Initialize an ellipse feature thanks to a vpDot2 and camera parameters.
- The vpFeatureEllipse is initialized thanks to the parameters
- of the dot given in pixel. The camera parameters are used to convert the
- pixel parameters to parameters given in meter.
-
- \warning With a vpDot2 there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
-
- \param s : Visual feature to initialize.
-
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
-
- \param t : The vpDot2 used to create the vpFeatureEllipse.
-*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot2 &t )
-{
- try
- {
-
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
-
- mp[0][0] = t.m00 ;
- mp[1][0] = t.m10;
- mp[0][1] = t.m01 ;
- mp[2][0] = t.m20 ;
- mp[1][1] = t.m11 ;
- mp[0][2] = t.m02 ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-/*!
- Initialize an ellipse feature thanks to a vpMeEllipse and camera parameters.
- The vpFeatureEllipse is initialized thanks to the parameters
- of the ellipse given in pixel. The camera parameters are used to convert the
- pixel parameters to parameters given in meter.
-
- \warning With a vpMeEllipse there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
-
- \param s : Visual feature to initialize.
-
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
-
- \param t : The vpMeEllipse used to create the vpFeatureEllipse.
-*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpMeEllipse &t )
-{
- try
- {
-
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
-
- //The opposite of vpDot and vpDot2 because moments in vpMeEllipse
- //are computed in the ij coordinate system whereas the moments in vpDot and vpDot2
- //are computed in the uv coordinate system
- mp[0][0] = t.get_m00() ;
- mp[1][0] = t.get_m01();
- mp[0][1] = t.get_m10() ;
- mp[2][0] = t.get_m02() ;
- mp[1][1] = t.get_m11() ;
- mp[0][2] = t.get_m20() ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderLine.cpp b/src/tracking/feature-builder/vpFeatureBuilderLine.cpp
deleted file mode 100644
index 5093da2..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderLine.cpp
+++ /dev/null
@@ -1,288 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderLine.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature line.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderLine.cpp
- \brief conversion between tracker
- and visual feature Line
-*/
-
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpMath.h>
-
-
-
-/*!
- Initialize a line feature thanks to a vpLine.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. In vpLine there are the parameters of two plans,
- but the one which have the biggest D parameter is copied in the vpFeatureLine parameters.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpLine used to create the vpFeatureLine.
-*/
-void vpFeatureBuilder::create(vpFeatureLine &s, const vpLine &t )
-{
- try
- {
- double A,B,C,D ;
- s.setRhoTheta(t.getRho(),t.getTheta()) ;
-
- if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
- {
- A = t.cP[0] ;
- B = t.cP[1] ;
- C = t.cP[2] ;
- D = t.cP[3] ;
- }
- else
- {
- A = t.cP[4] ;
- B = t.cP[5] ;
- C = t.cP[6] ;
- D = t.cP[7] ;
- }
-
-
- s.setABCD(A,B,C,D) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-/*!
- Initialize a line feature thanks to a vpCylinder.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are computed
- thanks to the parameters that are contained in vpCylinder. It is possible to choose which edge of the cylinder to use to
- initialize the vpFeatureLine.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpLine used to create the vpFeatureLine.
-
- \param line : The cylinder edge used to create the line feature.
- It can be vpCylinder::line1 or vpCylinder::line2.
-*/
-void vpFeatureBuilder::create(vpFeatureLine &s,
- const vpCylinder &t,
- const int line)
-{
- try
- {
-
- double a = t.getA() ;
- double b = t.getB() ;
- double c = t.getC() ;
-
- double x0 = t.getX() ;
- double y0 = t.getY() ;
- double z0 = t.getZ() ;
-
- double R = t.getR() ;
-
- double D =
- vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0)
- - vpMath::sqr(R)
- - vpMath::sqr(a*x0 + b*y0 + c*z0);
-
- double alpha1 = (1 - a*a)*x0 - a*b*y0 - a*c*z0;
- double beta1 = -a*b*x0 + (1 - b*b)*y0 - b*c*z0;
- double gamma1 = -a*c*x0 - b*c*y0 + (1 - c*c)*z0;
-
- D*=-1 ;
-
- if (D<0)
- {
- alpha1*=-1 ;
- beta1*=-1 ;
- gamma1*=-1 ;
- D*=-1 ;
- }
-
- s.setABCD(alpha1,beta1,gamma1,D) ;
-
- if (line==vpCylinder::line1)
- {
-
- s.setRhoTheta(t.getRho1(),t.getTheta1()) ;
-
- }
- else
- {
-
- s.setRhoTheta(t.getRho2(),t.getTheta2()) ;
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Initialize a line feature thanks to a vpMeLine and the parameters of the camera.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- In vpMeLine these parameters are given in pixel. The conversion is done thanks to the camera parameters.
-
- \warning vpFeatureLine also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are needed to
- compute the interaction matrix but can not be computed thanks to a vpMeLine. You have to compute and set these parameters
- outside the function.
-
- \param s : Visual feature to initialize.
-
- \param cam : The parameters of the camera used to acquire the image containing the line.
-
- \param t : The vpLine used to create the vpFeatureLine.
-
- The code below shows how to initialize a vpFeatureLine visual
- feature. First, we initialize the \f$(\rho,\theta)\f$, and lastly we
- set the parameters of the plane which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpMeLine line; // Moving-edges line tracker
-
- vpFeatureLine s; // Point feature
- ...
- // Tracking on the dot
- line.track(I);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, line);
-
- // A pose estimation is requested to initialize A, B, C and D the
- //parameters of the equation plan.
- double A = 1;
- double B = 1;
- double C = 1;
- double D = 1;
- ....
- s.setABCD(A,B,C,D);
- \endcode
-*/
-void
-vpFeatureBuilder::create(vpFeatureLine &s,
- const vpCameraParameters &cam,
- const vpMeLine &t)
-{
- double rhop ;
- double thetap ;
-
- double rho ;
- double theta ;
-
- try{
- rhop = t.getRho() ;
- thetap = t.getTheta();
-
- //Gives the rho and theta coordinates in the (u,v) coordinate system.
- if (thetap >= 0 && thetap < M_PI/2)
- {
- thetap = M_PI/2 - thetap;
- }
-
- else if (thetap >= M_PI/2 && thetap < 3*M_PI/2)
- {
- thetap = 3*M_PI/2 + M_PI - thetap;
- }
-
- else if (thetap >= 3*M_PI/2 && thetap <= 2*M_PI)
- {
- thetap = M_PI/2 + 2*M_PI - thetap;
- }
-
- //while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
- //while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
-
- // vpTRACE("pixel %f %f",rhop, thetap) ;
- vpPixelMeterConversion::convertLine(cam,rhop,thetap, rho,theta) ;
-
- while (theta > M_PI) { theta -= 2*M_PI ; }
- 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 ;
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderPoint.cpp b/src/tracking/feature-builder/vpFeatureBuilderPoint.cpp
deleted file mode 100644
index 815376d..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderPoint.cpp
+++ /dev/null
@@ -1,347 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderPoint.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature point.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderPoint.cpp
- \brief conversion between tracker
- and visual feature Point
-*/
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureException.h>
-#include <visp/vpException.h>
-
-
-/*!
- Create a vpFeaturePoint thanks to a vpDot and the parameters of the camera.
- The vpDot contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
-
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
-
- \param s : Visual feature \f$(x, y)\f$ to initialize. Be
- aware, the 3D depth \f$Z\f$ requested to compute the interaction
- matrix is not initialized by this function.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot.
- \param d : The vpDot used to create the vpFeaturePoint.
-
- The code below shows how to initialize a vpFeaturePoint visual
- feature. First, we initialize the \f$x,y\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpDot dot; // Dot tracker
-
- vpFeaturePoint s; // Point feature
- ...
- // Tracking on the dot
- dot.track(I);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, dot);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-*/
-void vpFeatureBuilder::create(vpFeaturePoint &s,
- const vpCameraParameters &cam,
- const vpDot &d)
-{
- try
- {
- double x=0, y=0;
-
- vpImagePoint cog;
- cog = d.getCog();
-
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
-
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Create a vpFeaturePoint thanks to a vpDot2 and the parameters of the camera.
- The vpDot2 contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
-
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot2.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
-
- \param s : The feature point.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
- \param d : The vpDot2 used to create the vpFeaturePoint.
-
- The code below shows how to initialize a vpFeaturePoint visual
- feature. First, we initialize the \f$x,y\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpDot2 dot; // Dot tracker
-
- vpFeaturePoint s; // Point feature
- ...
- // Tracking on the dot
- dot.track(I);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, dot);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-*/
-void vpFeatureBuilder::create(vpFeaturePoint &s,
- const vpCameraParameters &cam,
- const vpDot2 &d)
-{
- try
- {
- double x=0, y=0;
-
- vpImagePoint cog;
- cog = d.getCog();
-
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
-
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Create a vpFeaturePoint thanks to a vpImagePoint and the parameters of the camera.
- The vpImagePoint contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
-
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpImagePoint.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
-
- \param s : The feature point.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param ip : The vpImagePoint used to create the vpFeaturePoint.
-
- The code below shows how to initialize a vpFeaturePoint visual
- feature. First, we initialize the \f$x,y\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpImagePoint iP; // the point in the image
-
- vpFeaturePoint s; // Point feature
- ...
- // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
- iP.set_i(0);
- iP.set_j(0);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, iP);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-*/
-void vpFeatureBuilder::create(vpFeaturePoint &s,
- const vpCameraParameters &cam,
- const vpImagePoint &ip)
-{
- try
- {
- double x=0, y=0;
-
- vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
-
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Create a vpFeaturePoint thanks to a vpPoint.
- This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
- The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
-
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
-
- \param s : The feature point.
- \param p : The vpPoint used to create the vpFeaturePoint.
-*/
-void
-vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p)
-{
- try
- {
- s.set_x( p.get_x()) ;
- s.set_y( p.get_y()) ;
-
- s.set_Z( p.cP[2]/p.cP[3]) ;
-
- if (s.get_Z() < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(s.get_Z()) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Create a vpFeaturePoint thanks to a vpPoint. In this method noise is introduced during the initialization of the vpFeaturePoint.
- This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
- The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
-
- This function intends to introduce noise after the initialization of the parameters. Cartesian \f$(x,y)\f$ coordinates
- are first converted in pixel coordinates in the image using \e
- goodCam camera parameters. Then, the pixels coordinates of the point
- are converted back to cartesian coordinates \f$(x^{'},y^{'})\f$ using
- the noisy camera parameters \e wrongCam. These last parameters are stored in the vpFeaturePoint.
-
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
-
- \param s : The feature point.
-
- \param goodCam : Camera parameters used to introduce noise. These
- parameters are used to convert cartesian coordinates of the point \e
- p in the image plane in pixel coordinates.
-
- \param wrongCam : Camera parameters used to introduce noise. These
- parameters are used to convert pixel coordinates of the point in
- cartesian coordinates of the point in the image plane.
-
- \param p : The vpPoint used to create the vpFeaturePoint.
-*/
-void
-vpFeatureBuilder::create(vpFeaturePoint &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p)
-{
- try
- {
- double x = p.p[0] ;
- double y = p.p[1] ;
-
- s.set_Z( p.cP[2]/p.cP[3]) ;
-
- double u=0, v=0;
- vpMeterPixelConversion::convertPoint(goodCam,x,y,u,v) ;
- vpPixelMeterConversion::convertPoint(wrongCam,u,v,x,y) ;
-
-
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderPoint3D.cpp b/src/tracking/feature-builder/vpFeatureBuilderPoint3D.cpp
deleted file mode 100644
index 2367d43..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderPoint3D.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderPoint3D.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature 3D Point.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderPoint3D.cpp
- \brief conversion between tracker
- and visual feature 3D Point
-*/
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureException.h>
-#include <visp/vpException.h>
-
-
-/*!
-
- Initialize a 3D point feature using the coordinates of the point
- \f$(X,Y,Z)\f$ in the camera frame. The values of \f$(X,Y,Z)\f$ are
- expressed in meters.
-
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point
- coordinates in the camera frame are computed and stored in the vpPoint.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpPoint used to create the vpFeaturePoint.
-*/
-void
-vpFeatureBuilder::create(vpFeaturePoint3D &s, const vpPoint &t )
-{
- try
- {
-
-
-
- s.set_X( t.cP[0]/t.cP[3]) ;
- s.set_Y( t.cP[1]/t.cP[3]) ;
- s.set_Z( t.cP[2]/t.cP[3]) ;
-
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderPointPolar.cpp b/src/tracking/feature-builder/vpFeatureBuilderPointPolar.cpp
deleted file mode 100644
index fddd389..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderPointPolar.cpp
+++ /dev/null
@@ -1,402 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderPointPolar.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature point with
- * polar coordinates.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderPointPolar.cpp
-
- \brief Conversion between tracker and visual feature point with
- polar coordinates.
-*/
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureException.h>
-#include <visp/vpException.h>
-
-/*!
-
- Initialize a point feature with polar coordinates
- \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
- obtained by image processing. This point is the center
- of gravity of a dot tracked using vpDot. Using the camera
- parameters, the pixels coordinates of the dot are first
- converted in cartesian \f$(x,y)\f$ coordinates in meter in the
- camera frame and than in polar coordinates by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- \warning This function does not initialize \f$Z\f$ which is
- requested to compute the interaction matrix by
- vpfeaturePointPolar::interaction().
-
- \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
- aware, the 3D depth \f$Z\f$ requested to compute the interaction
- matrix is not initialized by this function.
-
- \param cam : Camera parameters.
-
- \param dot : Tracked dot. The center of gravity corresponds to the
- coordinates of the point in the image plane.
-
- The code below shows how to initialize a vpFeaturePointPolar visual
- feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpDot2 dot; // Dot tracker
-
- vpFeaturePointPolar s; // Point feature with polar coordinates
- ...
- // Tracking on the dot
- dot.track(I);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, dot);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-
-*/
-void vpFeatureBuilder::create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpDot &dot)
-{
- try {
- double x=0, y=0;
-
- vpImagePoint cog;
- cog = dot.getCog();
-
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
-
- double rho = sqrt(x*x + y*y);
- double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
-
- Initialize a point feature with polar coordinates
- \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
- obtained by image processing. This point is the center
- of gravity of a dot tracked using vpDot2. Using the camera
- parameters, the pixels coordinates of the dot are first
- converted in cartesian \f$(x,y)\f$ coordinates in meter in the
- camera frame and than in polar coordinates by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- \warning This function does not initialize \f$Z\f$ which is
- requested to compute the interaction matrix by
- vpfeaturePointPolar::interaction().
-
- \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
- aware, the 3D depth \f$Z\f$ requested to compute the interaction
- matrix is not initialized by this function.
-
- \param cam : Camera parameters.
-
- \param dot : Tracked dot. The center of gravity corresponds to the
- coordinates of the point in the image plane.
-
- The code below shows how to initialize a vpFeaturePointPolar visual
- feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpDot2 dot; // Dot tracker
-
- vpFeaturePointPolar s; // Point feature with polar coordinates
- ...
- // Tracking on the dot
- dot.track(I);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, dot);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-
-*/
-void vpFeatureBuilder::create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpDot2 &dot)
-{
- try {
- double x=0, y=0;
-
- vpImagePoint cog;
- cog = dot.getCog();
-
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
-
- double rho = sqrt(x*x + y*y);
- double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
-
- Initialize a point feature with polar coordinates
- \f$(\rho,\theta)\f$ using the coordinates of the point in pixels
- obtained by image processing. The points coordinates are stored in a vpImagePoint. Using the camera
- parameters, the pixels coordinates of the point are first
- converted in cartesian \f$(x,y)\f$ coordinates in meter in the
- camera frame and than in polar coordinates by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- \warning This function does not initialize \f$Z\f$ which is
- requested to compute the interaction matrix by
- vpfeaturePointPolar::interaction().
-
- \param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
- aware, the 3D depth \f$Z\f$ requested to compute the interaction
- matrix is not initialized by this function.
-
- \param cam : Camera parameters.
-
- \param iP : The vpImagePoint used to create the vpFeaturePoint.
-
- The code below shows how to initialize a vpFeaturePointPolar visual
- feature. First, we initialize the \f$\rho,\theta\f$, and lastly we
- set the 3D depth \f$Z\f$ of the point which is generally the result
- of a pose estimation.
-
- \code
- vpImage<unsigned char> I; // Image container
- vpCameraParameters cam; // Default intrinsic camera parameters
- vpImagePoint iP; // the point in the image
-
- vpFeaturePointPolar s; // Point feature with polar coordinates
- ...
- // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
- iP.set_i(0);
- iP.set_j(0);
-
- // Initialize rho,theta visual feature
- vpFeatureBuilder::create(s, cam, iP);
-
- // A pose estimation is requested to initialize Z, the depth of the
- // point in the camera frame.
- double Z = 1; // Depth of the point in meters
- ....
- s.set_Z(Z);
- \endcode
-
-*/
-void vpFeatureBuilder::create(vpFeaturePointPolar &s,
- const vpCameraParameters &cam,
- const vpImagePoint &iP)
-{
- try {
- double x=0, y=0;
-
- vpPixelMeterConversion::convertPoint(cam,iP,x,y) ;
-
- double rho = sqrt(x*x + y*y);
- double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
-
- Initialize a point feature with polar coordinates
- \f$(\rho,\theta)\f$ using the coordinates of the point
- \f$(x,y,Z)\f$, where \f$(x,y)\f$ correspond to the perspective
- projection of the point in the image plane and \f$Z\f$ the 3D depth
- of the point in the camera frame. The values of \f$(x,y,Z)\f$ are
- expressed in meters. From the coordinates in the image plane, the
- polar coordinates are computed by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- \param s : Visual feature \f$(\rho,\theta)\f$ and \f$Z\f$ to initialize.
-
- \param p : A point with \f$(x,y)\f$ cartesian coordinates in the
- image plane corresponding to the camera perspective projection, and
- with 3D depth \f$Z\f$.
-*/
-void
-vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p)
-{
- try {
-
- double x = p.get_x();
- double y = p.get_y();
-
- double rho = sqrt(x*x + y*y);
- double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
-
- s.set_Z( p.get_Z() ) ;
-
-
- if (s.get_Z() < 0) {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(s.get_Z()) < 1e-6) {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Initialize a point feature with polar coordinates
- \f$(\rho,\theta)\f$ using the coordinates of the point
- \f$(x,y,Z)\f$, where \f$(x,y)\f$ correspond to the perspective
- projection of the point in the image plane and \f$Z\f$ the 3D depth
- of the point in the camera frame. The values of \f$(x,y,Z)\f$ are
- expressed in meters.
-
- This function intends to introduce noise in the conversion from
- cartesian to polar coordinates. Cartesian \f$(x,y)\f$ coordinates
- are first converted in pixel coordinates in the image using \e
- goodCam camera parameters. Then, the pixels coordinates of the point
- are converted back to cartesian coordinates \f$(x^{'},y^{'})\f$ using
- the noisy camera parameters \e wrongCam. From these new coordinates
- in the image plane, the polar coordinates are computed by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- \param s : Visual feature \f$(\rho,\theta)\f$ and \f$Z\f$ to initialize.
-
- \param goodCam : Camera parameters used to introduce noise. These
- parameters are used to convert cartesian coordinates of the point \e
- p in the image plane in pixel coordinates.
-
- \param wrongCam : Camera parameters used to introduce noise. These
- parameters are used to convert pixel coordinates of the point in
- cartesian coordinates of the point in the image plane.
-
- \param p : A point with \f$(x,y)\f$ cartesian coordinates in the
- image plane corresponding to the camera perspective projection, and
- with 3D depth \f$Z\f$.
-*/
-void
-vpFeatureBuilder::create(vpFeaturePointPolar &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p)
-{
- try {
- double x = p.get_x();
- double y = p.get_y();
-
- s.set_Z( p.get_Z() );
-
- double u=0, v=0;
- vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
- vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
-
- double rho = sqrt(x*x + y*y);
- double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/feature-builder/vpFeatureBuilderSegment.cpp b/src/tracking/feature-builder/vpFeatureBuilderSegment.cpp
deleted file mode 100644
index 7901bf7..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderSegment.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderLine.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Segment creation out of dots.
- *
- * Authors:
- * Filip Novotny
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderSegment.cpp
- \brief Segment creation out of dots.
-*/
-
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpMath.h>
-
-
-/*!
- Initialize a segment feature out of vpDots and camera parameters.
-
- \param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param d1 : The dot corresponding to the first point of the segment.
- \param d2 : The dot corresponding to the second point of the segment.
-
-*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpDot &d1, const vpDot &d2 )
-{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
- }
-
- s.setAlpha(atan2(y1-y2,x1-x2));
-}
-
-/*!
- Initialize a segment feature out of vpDots and camera parameters.
-
- \param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param d1 : The dot corresponding to the first point of the segment.
- \param d2 : The dot corresponding to the second point of the segment.
-
-*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpDot2 &d1, const vpDot2 &d2)
-{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
- }
-
- s.setAlpha(atan2(y1-y2,x1-x2));
-}
-
-/*!
- Initialize a segment feature out of image points and camera parameters.
-
- \param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param ip1 : The image point corresponding to the first point of the segment.
- \param ip2 : The image point corresponding to the second point of the segment.
-
-*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpImagePoint &ip1, const vpImagePoint &ip2)
-{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, ip1, x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, ip2, x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
- }
-
- s.setAlpha(atan2(y1-y2,x1-x2));
-}
-
-/*!
-
- Build a segment visual feature from two points.
-
- \param s : Visual feature to initialize.
- \param P1, P2 : Two points defining the segment. These points must contain the 3D coordinates
- in the camera frame (cP) and the projected coordinates in the image plane (p).
-
-*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2)
-{
- double x1 = P1.get_x();
- double y1 = P1.get_y();
- double x2 = P2.get_x();
- double y2 = P2.get_y();
-
- double Z1 = P1.cP[2]/P1.cP[3];
- double Z2 = P2.cP[2]/P2.cP[3];
-
- s.buildFrom(x1, y1, Z1, x2, y2, Z2);
-}
diff --git a/src/tracking/feature-builder/vpFeatureBuilderVanishingPoint.cpp b/src/tracking/feature-builder/vpFeatureBuilderVanishingPoint.cpp
deleted file mode 100644
index d3d8dbb..0000000
--- a/src/tracking/feature-builder/vpFeatureBuilderVanishingPoint.cpp
+++ /dev/null
@@ -1,155 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureBuilderVanishingPoint.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Conversion between tracker and visual feature vanishing point.
- *
- * Authors:
- * Odile Bourquardez
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureBuilderVanishingPoint.cpp
- \brief conversion between vpPoint
- and visual feature vanishing point.
-*/
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureException.h>
-#include <visp/vpException.h>
-
-
-/*!
- Initialize a vpFeatureVanishingPoint thanks to a vpPoint.
- The vpFeatureVanishingPoint is initialized thanks to the parameters of the point in the image plan.
- All the parameters are given in meter.
-
- \param s : Visual feature to initialize.
-
- \param t : The vpPoint used to create the vpFeatureVanishingPoint.
-*/
-void
-vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &t)
-{
- try
- {
- s.set_x( t.get_x()) ;
- s.set_y( t.get_y()) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Cannot create vanishing point feature") ;
- throw ;
- }
-}
-
-
-/*!
- Initialize a vpFeatureVanishingPoint thanks to two vpFeatureLine.
- The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
- All the parameters are given in meter.
-
- \warning An exception is thrown if the two lines are parallels
-
- \param s : Visual feature to initialize.
-
- \param L1 : The first vpFeatureLine.
-
- \param L2 : The second vpFeatureLine.
-*/
-void
-vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, const vpFeatureLine &L2 )
-{
- double rho_l;
- double rho_r;
- double theta_l;
- double theta_r;
- double c_l;
- double s_l;
- double c_r;
- double s_r;
-
- rho_l = L1.getRho();
- rho_r = L2.getRho();
- theta_l = L1.getTheta();
- theta_r = L2.getTheta();
- c_l = cos(theta_l);
- c_r = cos(theta_r);
- s_l = sin(theta_l);
- s_r = sin(theta_r);
-
-
- double x,y;
-
- double min = 0.0001;
- if(fabs(theta_r-theta_l)<min || fabs(fabs(theta_r-theta_l)-M_PI)<min \
- || fabs(fabs(theta_r-theta_l)-2*M_PI)<min)
- {
- vpCERROR<<"There is no vanishing point : the lines are parallel in the image plane"<<std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "There is no vanishing point : the lines are parallel in the image plane")) ;
- }
-
- y = (rho_r *c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l );
- x = (rho_r *s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l );
-
- s.set_x ( x );
- s.set_y ( y );
-}
-
-
-
-/*!
- Initialize a vpFeatureVanishingPoint thanks to two vpLine.
- The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
- All the parameters are given in meter.
-
- \warning An exception is thrown if the two lines are parallels
-
- \param s : Visual feature to initialize.
-
- \param L1 : The first vpLine.
-
- \param L2 : The second vpLine.
-*/
-void
-vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2)
-{
- vpFeatureLine l1,l2 ;
- vpFeatureBuilder::create (l1,L1) ;
- vpFeatureBuilder::create (l2,L2) ;
-
- vpFeatureBuilder::create (s, l1, l2) ;
-
-}
diff --git a/src/tracking/forward-projection/vpCircle.cpp b/src/tracking/forward-projection/vpCircle.cpp
deleted file mode 100644
index 58a22b1..0000000
--- a/src/tracking/forward-projection/vpCircle.cpp
+++ /dev/null
@@ -1,359 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCircle.cpp 4772 2014-07-10 17:05:53Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature circle.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpCircle.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-void
-vpCircle::init()
-{
-
- oP.resize(7) ;
- cP.resize(7) ;
-
- p.resize(5) ;
-}
-
-/*!
- Set the world coordinates of the circle from the intersection of a plane and a sphere.
- We mean here the coordinates of the circle in the object frame
-
- \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
- oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
- oP[6] corresponds to the radius of the sphere.
-*/
-void
-vpCircle::setWorldCoordinates(const vpColVector& oP_)
-{
- this->oP = oP_ ;
-}
-
-/*!
- Set the world coordinates of the circle from the intersection of a plane and a sphere.
- We mean here the coordinates of the circle in the object frame
-
- \param A : A from the plane equation Ax + By + Cz = 0.
- \param B : B from the plane equation Ax + By + Cz = 0.
- \param C : C from the plane equation Ax + By + Cz = 0.
- \param X0 : X Coordinate of the center of the sphere.
- \param Y0 : Y Coordinate of the center of the sphere.
- \param Z0 : Z Coordinate of the center of the sphere.
- \param R : Radius of the sphere.
-*/
-void
-vpCircle::setWorldCoordinates(const double A, const double B, const double C,
- const double X0, const double Y0, const double Z0,
- const double R)
-{
- oP[0] = A ;
- oP[1] = B ;
- oP[2] = C ;
- oP[3] = X0 ;
- oP[4] = Y0 ;
- oP[5] = Z0 ;
- oP[6] = R ;
-}
-
-
-
-vpCircle::vpCircle()
-{
- init() ;
-}
-
-/*!
- Construct the circle from the intersection of a plane and a sphere.
-
- \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
- oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
- oP[6] corresponds to the radius of the sphere.
-
- \sa setWorldCoordinates()
-*/
-vpCircle::vpCircle(const vpColVector& oP_)
-{
- init() ;
- setWorldCoordinates(oP_) ;
-}
-
-/*!
- Construct the circle from the intersection of a plane and a sphere.
-
- \param A : A from the plane equation Ax + By + Cz = 0.
- \param B : B from the plane equation Ax + By + Cz = 0.
- \param C : C from the plane equation Ax + By + Cz = 0.
- \param X0 : X Coordinate of the center of the sphere.
- \param Y0 : Y Coordinate of the center of the sphere.
- \param Z0 : Z Coordinate of the center of the sphere.
- \param R : Radius of the sphere.
-
- \sa setWorldCoordinates()
-*/
-vpCircle::vpCircle(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R)
-{
- init() ;
- setWorldCoordinates(A, B, C,
- X0, Y0, Z0,
- R) ;
-}
-
-vpCircle::~vpCircle()
-{
-}
-
-
-
-
-/*!
- Perspective projection of the circle.
-
- From the 3D parameters of the circle in the camera frame available in cP, computes the 2D parameters of the ellipse resulting from the perspective projection in the image plane. Those 2D parameters are available in p vector.
-
- See vpCircle::projection(const vpColVector &, vpColVector &) for a more detailed description of the parameters.
- */
-void
-vpCircle::projection()
-{
- projection(cP,p) ;
-}
-
-/*!
- Perspective projection of the circle.
- \param cP_: 3D cercle input parameters. This vector is of dimension 7. It contains the following parameters: A, B, C, X0, Y0, Z0, r where
- - A,B,C are the parameters of the plane with equation Ax+By+Cz+D=0 containing the circle
- - X0,Y0,Z0 are the 3D coordinates of the cercle in the camera frame
- - r is the circle radius.
-
- \param p_: 2D circle output parameters. This is a 5 dimension vector. It contains the following parameters: xc, yc, m20, m11, m02 where:
- - xc,yc are the normalized coordinates of the center of the ellipse (ie the perspective projection of a 3D
- circle becomes a 2D ellipse in the image) in the image plane.
- - mu20,mu11,mu02 are the second order centered moments of the ellipse.
- */
-void
-vpCircle::projection(const vpColVector &cP_, vpColVector &p_)
-{
- vpColVector K(6) ;
- {
- double A = cP_[0] ;
- double B = cP_[1] ;
- double C = cP_[2] ;
-
- double X0 = cP_[3] ;
- double Y0 = cP_[4] ;
- double Z0 = cP_[5] ;
-
- double r = cP_[6];
-
- // projection
- double s = X0*X0 + Y0*Y0 + Z0*Z0 - r*r ;
- double det = A*X0+B*Y0+C*Z0;
- A = A/det ;
- B = B/det ;
- C = C/det ;
-
- K[0] = 1 - 2*A*X0 + A*A*s;
- K[1] = 1 - 2*B*Y0 + B*B*s;
- K[2] = -A*Y0 - B*X0 + A*B*s;
- K[3] = -C*X0 - A*Z0 + A*C*s;
- K[4] = -C*Y0 - B*Z0 + B*C*s;
- K[5] = 1 - 2*C*Z0 + C*C*s;
-
- }
-
-// {
-// std::cout << "K dans vpCircle::projection(): " << std::endl;
-// for (unsigned int i=1; i<6; i++)
-// std::cout << K[i]/K[0] << std::endl;
-// }
- double det = K[2]*K[2] -K[0]*K[1];
- if (fabs(det) < 1e-8)
- {
- vpERROR_TRACE("division par 0") ;
- throw(vpException(vpException::divideByZeroError,
- "division par 0")) ;
- }
-
- double xc = (K[1]*K[3]-K[2]*K[4])/det;
- double yc = (K[0]*K[4]-K[2]*K[3])/det;
-
- double c = sqrt( (K[0]-K[1])*(K[0]-K[1]) + 4*K[2]*K[2] );
- double s = 2*(K[0]*xc*xc + 2*K[2]*xc*yc + K[1]*yc*yc - K[5]);
-
- double A,B,E ;
-
- //if (fabs(K[2])<1e-6)
- if (fabs(K[2])<std::numeric_limits<double>::epsilon())
- {
- E = 0.0;
- if (K[0] > K[1])
- {
- A = sqrt(s/(K[0] + K[1] + c));
- B = sqrt(s/(K[0] + K[1] - c));
- }
- else
- {
- A = sqrt(s/(K[0] + K[1] - c));
- B = sqrt(s/(K[0] + K[1] + c));
- }
- }
- else
- {
- E = (K[1] - K[0] + c)/(2*K[2]);
- if ( fabs(E) > 1.0)
- {
- A = sqrt(s/(K[0] + K[1] + c));
- B = sqrt(s/(K[0] + K[1] - c));
- }
- else
- {
- A = sqrt(s/(K[0] + K[1] - c));
- B = sqrt(s/(K[0] + K[1] + c));
- E = -1.0/E;
- }
- }
-
- det = (1.0 + vpMath::sqr(E));
- double mu20 = (vpMath::sqr(A) + vpMath::sqr(B*E)) /det ;
- double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) *E / det ;
- double mu02 = (vpMath::sqr(B) + vpMath::sqr(A*E)) / det ;
-
- p_[0] = xc ;
- p_[1] = yc ;
- p_[2] = mu20 ;
- p_[3] = mu11 ;
- p_[4] = mu02 ;
-}
-
-//! perspective projection of the circle
-void
-vpCircle::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
-{
-
- double A,B,C ;
- A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
- B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
- C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
-
- double X0,Y0,Z0 ;
- X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
- Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
- Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
- double R = oP[6] ;
-
- cP_[0] = A ;
- cP_[1] = B ;
- cP_[2] = C ;
-
- cP_[3] = X0 ;
- cP_[4] = Y0 ;
- cP_[5] = Z0 ;
-
- cP_[6] = R ;
-
- // vpTRACE("_cP :") ; std::cout << _cP.t() ;
-
-}
-
-//! perspective projection of the circle
-void
-vpCircle::changeFrame(const vpHomogeneousMatrix &cMo)
-{
-
- double A,B,C ;
- A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
- B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
- C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
-
- double X0,Y0,Z0 ;
- X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
- Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
- Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
- double R = oP[6] ;
-
- cP[0] = A ;
- cP[1] = B ;
- cP[2] = C ;
-
- cP[3] = X0 ;
- cP[4] = Y0 ;
- cP[5] = Z0 ;
-
- cP[6] = R ;
-
- // vpTRACE("_cP :") ; std::cout << _cP.t() ;
-
-}
-
-void vpCircle::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
- cam, I, color, thickness) ;
-}
-
-// non destructive wrt. cP and p
-void vpCircle::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
- cam, I, color, thickness) ;
-
-}
-//! for memory issue (used by the vpServo class only)
-vpCircle *vpCircle::duplicate() const
-{
- vpCircle *feature = new vpCircle(*this) ;
- return feature ;
-}
diff --git a/src/tracking/forward-projection/vpCircle.h b/src/tracking/forward-projection/vpCircle.h
deleted file mode 100644
index 86151b2..0000000
--- a/src/tracking/forward-projection/vpCircle.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCircle.h 4702 2014-03-27 15:33:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature circle.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpCircle.h
- \brief class that defines what is a circle
-*/
-
-#ifndef vpCircle_hh
-#define vpCircle_hh
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <math.h>
-#include <visp/vpDebug.h>
-#include <visp/vpMath.h>
-#include <visp/vpForwardProjection.h>
-
-/*!
- \class vpCircle
- \ingroup TrackingFeature GeometryFeature
- \brief Class that defines what is a circle.
-*/
-class VISP_EXPORT vpCircle : public vpForwardProjection
-{
-public:
- void init() ;
- vpCircle() ;
- vpCircle(const vpColVector& oP) ;
- vpCircle(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
- virtual ~vpCircle() ;
-
-
- void setWorldCoordinates(const vpColVector& oP) ;
- void setWorldCoordinates(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
-
-
- double getA() const { return cP[0] ; }
- double getB() const{ return cP[1] ; }
- double getC() const { return cP[2] ; }
-
- double getX() const { return cP[3] ; }
- double getY() const { return cP[4] ; }
- double getZ() const{ return cP[5] ; }
-
- double getR() const { return cP[6] ; }
-
-
-
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- vpCircle *duplicate() const ;
-
-} ;
-
-
-#endif
diff --git a/src/tracking/forward-projection/vpCylinder.cpp b/src/tracking/forward-projection/vpCylinder.cpp
deleted file mode 100644
index 208538c..0000000
--- a/src/tracking/forward-projection/vpCylinder.cpp
+++ /dev/null
@@ -1,429 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCylinder.cpp 4908 2014-09-19 06:55:24Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Cylinder feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpCylinder.h>
-#include <visp/vpFeatureDisplay.h>
-
-
-void
-vpCylinder::init()
-{
-
- oP.resize(7) ;
- cP.resize(7) ;
-
- p.resize(4) ;
-}
-
-/*!
- Set the cylinder parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
-
- \param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
-
- \code
- vpCylinder cylinder;
- double A, B, C, X0, Y0, Z0, R;
- ...
- vpColVector oP(7);
- oP[0] = A;
- ...
- oP[3] = X0;
- ...
- oP[6] = R;
- cylinder.setWorldCoordinates(oP);
- \endcode
-*/
-void
-vpCylinder::setWorldCoordinates(const vpColVector& o_P)
-{
- this->oP = o_P ;
-}
-
-/*!
- Set the cylinder parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
-
- \param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
-
-*/
-void
-vpCylinder::setWorldCoordinates(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R)
-{
- oP[0] = A ;
- oP[1] = B ;
- oP[2] = C ;
- oP[3] = X0 ;
- oP[4] = Y0 ;
- oP[5] = Z0 ;
- oP[6] = R ;
-}
-
-
-/*!
- Default constructor.
-*/
-vpCylinder::vpCylinder()
-{
- init() ;
-}
-
-/*!
- Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
-
- \param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
-
- \code
- vpCylinder cylinder;
- double A, B, C, X0, Y0, Z0, R;
- ...
- vpColVector oP(7);
- oP[0] = A;
- ...
- oP[3] = X0;
- ...
- oP[6] = R;
- vpCylinder cylinder(oP);
- \endcode
- \sa setWorldCoordinates(const vpColVector&)
-*/
-vpCylinder::vpCylinder(const vpColVector& o_P)
-{
- init() ;
- setWorldCoordinates(o_P) ;
-}
-
-/*!
- Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
-
- \param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
-
- \sa setWorldCoordinates(const double,const double,const double,const double,const double,const double,const double)
-*/
-vpCylinder::vpCylinder(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R)
-{
- init() ;
- setWorldCoordinates(A, B, C,
- X0, Y0, Z0,
- R) ;
-}
-
-/*!
- Default constructor.
- */
-vpCylinder::~vpCylinder()
-{
-}
-
-
-/*!
- Perspective projection of the cylinder.
-
- From the parameters of the cylinder in the camera frame, compute the perspective
- projection of the cylinder in the image plane.
-
- \code
- vpCylinder cylinder;
- vpColVector oP(7);
- // Initialize oP[] with A,B,C,X0,X0,Z0,R parameters
- cylinder.setWorldCoordinates(oP); // Set the cylinder world frame parameters
-
- vpHomogeneousMatrix cMo; // Camera to world frame transformation
- cylinder.changeFrame(cMo); // Update internal cP parameters
-
- cylinder.projection(); // Compute the perspective projection
- \endcode
-
- \sa projection(const vpColVector &, vpColVector &)
- */
-void
-vpCylinder::projection()
-{
- projection(cP,p) ;
-}
-
-
-/*!
- Perspective projection of the cylinder.
-
- From the parameters of the cylinder in the camera frame \f$c{\bf P}\f$, compute the perspective
- projection of the cylinder in the image plane.
-
- \param cP_ [in] : Cylinder parameters in the camera frame.
- \param p_ [out] : Parameters of the cylinder in the image plane obtained by perspective projection.
-
- \exception vpException::fatalError : The camera is inside the cylinder.
-
- \code
- vpCylinder cylinder;
- vpColVector oP(7);
- // Initialize oP[] with A,B,C,X0,X0,Z0,R parameters
- cylinder.setWorldCoordinates(oP); // Set the cylinder world frame parameters
-
- vpHomogeneousMatrix cMo; // Camera to world frame transformation
- vpColVector cP(7); // Parameters of the cylinder in the camera frame
- cylinder.changeFrame(cMo, cP); // Update cP parameters
-
- vpColVector p(4); // Parameters of the cylinder in the image plane
- cylinder.projection(cP, p); // Compute the perspective projection and update p
- \endcode
-
- \sa projection()
- */
-void
-vpCylinder::projection(const vpColVector &cP_, vpColVector &p_)
-{
- //calcul de la scene 2-D
-
- double co, si, e, x0, y0, z0;
- double A,B,C, X0, Y0, Z0, R ;
- double s, a, b, c, zero;
-
- A = cP_[0] ;
- B = cP_[1] ;
- C = cP_[2] ;
- X0 = cP_[3] ;
- Y0 = cP_[4] ;
- Z0 = cP_[5] ;
- R= cP_[6] ;
- zero = A*X0 + B*Y0 + C*Z0; // should be zero for a good reprensetation of the cylinder
-
- s = X0*X0 + Y0*Y0 + Z0*Z0 - R*R - zero*zero;
- if (s < 0)
- {
- printf("The camera is inside the cylinder with s=%f !\n", s);
- throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
- }
- s = 1.0/sqrt(s);
- a = X0 - A*zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
- b = Y0 - B*zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
- c = Z0 - C*zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
- x0 = C*Y0 - B*Z0;
- y0 = A*Z0 - C*X0;
- z0 = B*X0 - A*Y0;
-
- // rho1 / theta1
- co = R*a*s-x0;
- si = R*b*s-y0;
- e = sqrt(co*co + si*si);
- p_[0] = -(R*c*s-z0)/e ; // rho1
- p_[1] = atan2(si,co) ; // theta 1
-
- // while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
- // while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
-
- // rho2 / theta2
- co = R*a*s+x0;
- si = R*b*s+y0;
- e = sqrt(co*co + si*si);
- p_[2] = -( R*c*s+z0 )/e ; //rho2
- p_[3] = atan2( si,co ) ; //theta2
-
-
- // while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
- // while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
-
- // std::cout << p.t() << std::endl ;
-}
-
-/*!
- From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
- the cylinder internal parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
-
- \param cMo : Camera to world frame transformation.
-
- \sa changeFrame(const vpHomogeneousMatrix &, vpColVector &)
- */
-void
-vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo,cP) ;
-}
-
-/*!
- From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
- the cylinder parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
-
- \param cMo : Camera to world frame transformation.
- \param cP_ [out] : Parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
-
- \sa changeFrame(const vpHomogeneousMatrix &)
-*/
-void
-vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
-{
- double X1, Y1, Z1;
- double X2, Y2, Z2;
- double s, a, b, c;
-
- double oA,oB,oC, oX0, oY0, oZ0 ;
- oA = oP[0] ;
- oB = oP[1] ;
- oC = oP[2] ;
- oX0 = oP[3] ;
- oY0 = oP[4] ;
- oZ0 = oP[5] ;
-
- X1 = cMo[0][0]*oA + cMo[0][1]*oB + cMo[0][2]*oC ;
- Y1 = cMo[1][0]*oA + cMo[1][1]*oB + cMo[1][2]*oC ;
- Z1 = cMo[2][0]*oA + cMo[2][1]*oB + cMo[2][2]*oC ;
- s = sqrt ( X1*X1 + Y1*Y1 + Z1*Z1 );
- a = X1 / s;
- b = Y1 / s;
- c = Z1 / s;
-
- // set axis coordinates in camera frame
- cP_[0] = a ;
- cP_[1] = b ;
- cP_[2] = c ;
-
- X2 = cMo[0][3] + cMo[0][0]*oX0 + cMo[0][1]*oY0 + cMo[0][2]*oZ0;
- Y2 = cMo[1][3] + cMo[1][0]*oX0 + cMo[1][1]*oY0 + cMo[1][2]*oZ0;
- Z2 = cMo[2][3] + cMo[2][0]*oX0 + cMo[2][1]*oY0 + cMo[2][2]*oZ0;
-
- // adding the constraint X0 is the nearest point to the origin (A^T . X0 = 0)
- // using the projection operator (I - AA^T) orthogonal to A
- cP_[3] = (1-a*a)*X2 - a*b*Y2 - a*c*Z2;
- cP_[4] = -a*b*X2 + (1-b*b)*Y2 - b*c*Z2;
- cP_[5] = -a*c*X2 - b*c*Y2 + (1-c*c)*Z2;
-
- /* old version for the same onstraint
- if ( fabs(a) > 0.25 )
- {
- double xx, yy, zz, xx1, yy1;
-
- xx1 = a*Y2 - b*X2;
- yy1 = a*Z2 - c*X2;
- xx = -( b*xx1 + c*yy1);
- yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
- zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
-
- // set point coordinates in camera frame
- _cP[3] = xx ;
- _cP[4] = yy ;
- _cP[5] = zz ;
- }
- else if ( fabs(b) >0.25 )
- {
- double xx, yy, zz, xx1, yy1;
-
- xx1 = a*Y2 - b*X2;
- yy1 = c*Y2 - b*Z2;
- xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
- yy = a*xx1 + c*yy1;
- zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
-
-
- // set point coordinates in camera frame
- _cP[3] = xx ;
- _cP[4] = yy ;
- _cP[5] = zz ;
- }
- else
- {
- double xx, yy, zz, xx1, yy1;
-
- xx1 = a*Z2 - c*X2;
- yy1 = b*Z2 - c*Y2;
- xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
- yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
- zz = a*xx1 + b*yy1;
-
- // set point coordinates in camera frame
- _cP[3] = xx ;
- _cP[4] = yy ;
- _cP[5] = zz ;
- }
- */
- //radius
- cP_[6] = oP[6] ;
-
-}
-
-//! for memory issue (used by the vpServo class only)
-vpCylinder *vpCylinder::duplicate() const
-{
- vpCylinder *feature = new vpCylinder(*this) ;
- return feature ;
-}
-
-/*!
- Display the projection of the cylinder in the image as two lines.
-*/
-void
-vpCylinder::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
-
- vpColVector _cP(7), _p(4) ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayCylinder(_p[0],_p[1], _p[2], _p[3],
- cam, I, color, thickness) ;
-
-}
-
-/*!
- Display the projection of the cylinder in the image as two lines.
-*/
-void
-vpCylinder::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3],
- cam, I, color, thickness) ;
-}
diff --git a/src/tracking/forward-projection/vpCylinder.h b/src/tracking/forward-projection/vpCylinder.h
deleted file mode 100644
index e8f0ebb..0000000
--- a/src/tracking/forward-projection/vpCylinder.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpCylinder.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Cylinder feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpCylinder.h
- \brief class that defines what is a cylinder
-*/
-
-#ifndef vpCylinder_hh
-#define vpCylinder_hh
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <math.h>
-#include <visp/vpMath.h>
-
-#include <visp/vpForwardProjection.h>
-
-/*!
- \class vpCylinder
- \ingroup TrackingFeature GeometryFeature
- \brief Class that defines what is a cylinder.
-
- A cylinder may be represented by the equation:
- \f$ (X - X_0)^2 + (Y - Y_0)^2 + (Z - Z_0)^2 - (A \; X + B \; Y + C \; Z)^2 - R^2 = 0 \f$
- with
-
- \f$
- \left\{ \begin{array}{l}
- A^2 + B^2 + C^2 = 1 \\
- A \; X_0 + B \; Y_0 + C \; Z_0 = 0
- \end{array} \right.
- \f$
-
- where \f$R\f$ is the radius of the cylinder, \f$A, B, C\f$ are the coordinates of
- its direction vector and \f$X_0, Y_0, Z_0\f$ are the coordinates
- of the nearest point belonging to the cylinder axis from the projection center.
-
- Setting the cylinder parameters is achieved throw the constructors with parameters
- or the setWorldCoordinates() methods.
-
- Considering the set of parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame, cylinder coordinates expressed in the camera
- frame are obtained using changeFrame().
-
- The projection of a cylinder on the image plane is (for
- non-degenerated cases) a set of two straight lines with equation:
-
- \f$
- \left\{ \begin{array}{lll}
- x \;\cos\theta_1 + x \;\sin\theta_1 - \rho_1 = 0 \\
- y \;\cos\theta_2 + y \;\sin\theta_2 - \rho_2 = 0
- \end{array} \right.
- \f$
-
- The projection is achieved using projection() methods. The methods getRho1(), getTheta1() and
- getRho2(), getTheta2() allow to access to the projected line parameters.
-*/
-class VISP_EXPORT vpCylinder : public vpForwardProjection
-{
-public:
- typedef enum
- {
- line1, /*!< First limb of the cylinder. */
- line2 /*!< Second limb of the cylinder. */
- } vpLineCylinderType;
-
- vpCylinder() ;
- vpCylinder(const vpColVector& oP) ;
- vpCylinder(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
- virtual ~vpCylinder() ;
-
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
-
- vpCylinder *duplicate() const ;
-
- /*!
- Return the \f$\rho_1\f$ parameter of the line corresponding to the
- projection of the cylinder in the image plane.
- \sa getTheta1()
- */
- double getRho1() const { return p[0] ; }
- /*!
- Return the \f$\theta_1\f$ parameter of the line corresponding to the
- projection of the cylinder in the image plane.
- \sa getRho1()
- */
- double getTheta1() const { return p[1] ; }
-
- /*!
- Return the \f$\rho_2\f$ parameter of the line corresponding to the
- projection of the cylinder in the image plane.
- \sa getTheta2()
- */
- double getRho2() const { return p[2] ; }
- /*!
- Return the \f$\theta_2\f$ parameter of the line corresponding to the
- projection of the cylinder in the image plane.
- \sa getRho2()
- */
- double getTheta2() const { return p[3] ; }
-
- /*!
- Return cylinder \f$A\f$ parameter expressed in the camera frame.
- */
- double getA() const { return cP[0] ; }
- /*!
- Return cylinder \f$B\f$ parameter expressed in the camera frame.
- */
- double getB() const { return cP[1] ; }
- /*!
- Return cylinder \f$C\f$ parameter expressed in the camera frame.
- */
- double getC() const { return cP[2] ; }
- /*!
- Return cylinder \f$X_0\f$ parameter expressed in the camera frame.
- */
- double getX() const { return cP[3] ; }
- /*!
- Return cylinder \f$Y_0\f$ parameter expressed in the camera frame.
- */
- double getY() const { return cP[4] ; }
- /*!
- Return cylinder \f$Z_0\f$ parameter expressed in the camera frame.
- */
- double getZ() const { return cP[5] ; }
- /*!
- Return cylinder \f$R\f$ parameter corresponding to the cylinder radius.
- */
- double getR() const { return cP[6] ; }
-
- void init() ;
-
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
-
- void setWorldCoordinates(const vpColVector& oP) ;
- void setWorldCoordinates(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
-} ;
-
-
-#endif
diff --git a/src/tracking/forward-projection/vpForwardProjection.cpp b/src/tracking/forward-projection/vpForwardProjection.cpp
deleted file mode 100644
index d1c30f3..0000000
--- a/src/tracking/forward-projection/vpForwardProjection.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpForwardProjection.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Forward projection.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpForwardProjection.h>
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpForwardProjection.cpp
- \brief class that defines what is a point
-*/
-
-
-
-
-
-/*!
- Print to stdout the feature parameters in:
- - the object frame
- - the camera frame
- - the image plane.
-*/
-void
-vpForwardProjection::print() const
-{
- std::cout << "oP : " << oP.t() ;
- std::cout << "cP : " << cP.t() ;
- std::cout << "p : " << p.t() ;
-}
-
-/*!
-
- Compute the feature parameters in the image plane (vpTracker::p)
- from the parameters in the camera frame (vpTracker::cP).
-
- \warning Be careful to update vpTracker::p and vpTracker::cP public
- attributes before the call of this method.
-*/
-void
-vpForwardProjection::project()
-{
- projection(cP, p) ;
-}
-
-/*!
-
- Compute the feature parameters in the camera frame (vpTracker::cP)
- and than compute the projection of these parameters in the image
- plane (vpTracker::p).
-
- \warning The feature parameters in the object frame
- (vpForwardProjection:oP) need to be set prior the use of this
- method. To initialize these parameters see setWorldCoordinates().
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
-*/
-void
-vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
-{
- try{
- changeFrame(cMo) ;
- projection() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
-
- Track the feature parameters in the camera frame (vpTracker::cP)
- and than compute the projection of these parameters in the image
- plane (vpTracker::p).
-
- This method is similar to project(const vpHomogeneousMatrix &).
-
- \warning The feature parameters in the object frame
- (vpForwardProjection:oP) need to be set prior the use of this
- method. To initialize these parameters see setWorldCoordinates().
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
-*/
-void
-vpForwardProjection::track(const vpHomogeneousMatrix &cMo)
-{
- try{
- project(cMo) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/forward-projection/vpForwardProjection.h b/src/tracking/forward-projection/vpForwardProjection.h
deleted file mode 100644
index 6b4a8ea..0000000
--- a/src/tracking/forward-projection/vpForwardProjection.h
+++ /dev/null
@@ -1,241 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpForwardProjection.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Forward projection.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpForwardProjection_H
-#define vpForwardProjection_H
-
-/*!
- \file vpForwardProjection.h
- \brief class that defines what is a generic geometric feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpTracker.h>
-#include <visp/vpColor.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-
-
-/*!
- \class vpForwardProjection
- \brief Class that defines what is a generic geometric feature.
-
- Each geometric feature has parameters expressed:
-
- - in the object frame \e oP. These parameters are located in the public
- attribute vpForwardProjection::oP.
- - in the camera frame \e cP. These parameters are located in the public
- attribute vpTracker::cP.
- - in the image plane \e p. These parameters are located in the public
- attribute vpTracker::p. They correspond to normalized coordinates
- of the feature expressed in meters.
-*/
-class VISP_EXPORT vpForwardProjection : public vpTracker
-{
-public:
- /*!
- Feature coordinates expressed in the object frame, also called world frame.
- */
- vpColVector oP ;
-
-public:
- /*!
- Default initialisation of the feature parameters:
- - in the object frame: \e oP
- - in the camera frame: \e cP
- - in the image plane: \e p.
- */
- virtual void init() = 0;
-
- //! Destructor.
- virtual ~vpForwardProjection() { ; }
-
-public:
- /*!
- Sets the parameters which define the feature in the object frame.
-
- \param oP : Feature parameters expressed in the object frame used
- to set the vpForwardProjection::oP public attribute.
-
- */
- virtual void setWorldCoordinates(const vpColVector &oP) = 0;
-
- /*!
-
- Computes the features parameters in the camera frame (\e cP) thanks
- to the parameters given in the object frame
- (vpForwardProjection::oP) and the homogeneous matrix relative to
- the pose (\e cMo) between the object frame and the camera frame.
-
- To set the parameters in the object frame you need to call
- setWorldCoordinates().
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
- \param cP : The vector which will contain the feature parameters
- expressed in the camera frame.
-
- With this method, the vpTracker::cP public attribute is not updated.
-
- */
- virtual void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)=0;
- /*!
-
- Computes the features parameters in the camera frame (\e cP) thanks
- to the parameters given in the object frame
- (vpForwardProjection::oP) and the homogeneous matrix relative to
- the pose (\e cMo) between the object frame and the camera frame.
-
- To set the parameters in the object frame you need to call
- setWorldCoordinates().
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
- The features parameters in the camera frame (cP) are updated in
- the vpTracker::cP public attribute.
- */
- virtual void changeFrame(const vpHomogeneousMatrix &cMo) = 0 ;
- /*!
-
- Computes the feature parameters in the image plane from the
- parameters expressed in the camera frame.
-
- \param cP [input] : Feature parameters expressed in the camera frame.
-
- \param p [output] : Feature parameters expressed in the image plane.
- */
- virtual void projection(const vpColVector &cP, vpColVector &p) =0 ;
-
- /*!
- Computes the feature parameters in the image plane. These
- parameters are than updated in the vpTracker::p public attribute.
-
- \warning To compute these parameters, the method exploit the
- feature parameters in the camera frame. Thus, vpTracker::cP need
- to be updated before the call of this method. For that, a call to
- changeFrame(const vpHomogeneousMatrix &) is requested.
- */
- virtual void projection() =0 ;
-
- void project() ;
- void project(const vpHomogeneousMatrix &cMo) ;
- void track(const vpHomogeneousMatrix &cMo) ;
-
- /*!
-
- Displays the feature in the image \e I thanks to the 2D feature
- parameters in the image plane (vpTracker::p) and the camera
- parameters which enable to convert the features from meter to pixel.
-
- \param I : The image where the feature must be displayed in overlay.
-
- \param cam : The camera parameters to enable the conversion from
- meter to pixel.
-
- \param color : The desired color to display the line in the image.
- \param thickness : Thickness of the feature representation.
- */
- virtual void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) =0;
-
- /*!
-
- Displays the feature in the image \e I thanks to the features in
- the object frame (vpForwardProjection::oP), the homogeneous matrix relative
- to the pose between the object frame and the camera frame and the
- camera parameters which enable to convert the features from meter
- to pixel.
-
- \param I : The image where the line must be displayed in overlay.
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
- \param cam : The camera parameters to enable the conversion from
- meter to pixel.
-
- \param color : The desired color to display the line in the image.
- \param thickness : Thickness of the feature representation.
- */
- virtual void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) =0;
-
- virtual void print() const ;
-
- /*!
- Create an object with the same type.
- */
- virtual vpForwardProjection *duplicate() const = 0 ;
-
-public:
- /*!
- Used for memory issue especially in the vpServo class.
- */
- typedef enum
- {
- user,
- vpDisplayForwardProjection
- } vpForwardProjectionDeallocatorType;
-
-private:
- vpForwardProjectionDeallocatorType deallocate ;
-public:
- vpForwardProjection() : oP(), deallocate(user) {}
-
- void setDeallocate(vpForwardProjectionDeallocatorType d) { deallocate = d ; }
- vpForwardProjectionDeallocatorType getDeallocate() { return deallocate ; }
-} ;
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/forward-projection/vpLine.cpp b/src/tracking/forward-projection/vpLine.cpp
deleted file mode 100644
index 1b10384..0000000
--- a/src/tracking/forward-projection/vpLine.cpp
+++ /dev/null
@@ -1,572 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLine.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Line feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpLine.h>
-
-#include <visp/vpDebug.h>
-#include <visp/vpMath.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-/*!
- \file vpLine.cpp
- \brief class that defines what is a line
-*/
-
-
-
-/*!
-
- Initialize the memory space requested for the 2D line parameters (\e
- p) in the image plane and for 3D line parameters (\e oP and \e cP)
- respectively in the object frame and the camera frame.
-*/
-void
-vpLine::init()
-{
- oP.resize(8) ;
- cP.resize(8) ;
- p.resize(2) ;
-}
-
-/*!
- Default constructor.
-*/
-vpLine::vpLine()
-{
- init() ;
-}
-
-
-
-/*!
-
- Sets the parameters \e oP which define the line in the object
- frame. As said in the class description, the line is defined as the
- intersection of two planes. The different parameters here define the
- equations of the two planes in the object frame. They are used to set
- the vpForwardProjection::oP public attribute.
-
- \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
- \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
- Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
-
- \param A1, B1, C1, D1 : The parameters used to define the first plane.
- \param A2, B2, C2, D2 : The parameters used to define the second plane.
-
-*/
-void
-vpLine::setWorldCoordinates(const double &A1, const double &B1,
- const double &C1, const double &D1,
- const double &A2, const double &B2,
- const double &C2, const double &D2)
-{
- oP[0] = A1 ;
- oP[1] = B1 ;
- oP[2] = C1 ;
- oP[3] = D1 ;
-
- oP[4] = A2 ;
- oP[5] = B2 ;
- oP[6] = C2 ;
- oP[7] = D2 ;
-}
-
-
-/*!
-
- Sets the parameters \e oP which define the line in the object frame. As
- said in the class description, the line is defined as the
- intersection of two planes. Eight parameters are required to define
- the equations of the two planes in the object frame. They are used to set
- the vpForwardProjection::oP public attribute.
-
- \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
- \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
- Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
-
- \param oP_ : The column vector which contains the eight parameters
- needed to define the equations of the two planes in the object
- frame. \f[ oP = \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \\ A2 \\
- B2 \\ C2 \\ D2 \end{array}\right] \f]
-
-*/
-void
-vpLine::setWorldCoordinates(const vpColVector &oP_)
-{
- if (oP_.getRows() != 8)
- throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
-
- this->oP = oP_ ;
-}
-
-
-/*!
-
- Sets the parameters \e oP which define the line in the object frame. As
- said in the class description, the line is defined as the
- intersection of two planes. Eight parameters are required to define
- the equations of the two planes in the object frame. They are used to set
- the vpForwardProjection::oP public attribute.
-
- \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
- \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
- Here \f$ (X, Y, Z) \f$ are the 3D coordinates in the object frame.
-
- \param oP1 : The column vector which contains the four parameters
- needed to define the equations of the first plane. \f[ oP1 =
- \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \end{array}\right] \f]
-
- \param oP2 : The column vector which contains the four parameters
- needed to define the equations of the second plane. \f[ oP2 =
- \left[\begin{array}{c} A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
-
-*/
-void
-vpLine::setWorldCoordinates(const vpColVector &oP1,
- const vpColVector &oP2)
-{
- if (oP1.getRows() != 4)
- throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
-
- if (oP2.getRows() != 4)
- throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
-
- for (unsigned int i=0 ; i < 4 ; i++)
- {
- oP[i] = oP1[i] ;
- oP[i+4] = oP2[i] ;
- }
-
-}
-
-
-/*!
-
- Computes the 2D parameters \e p of the line in the image plane thanks to
- the 3D line parameters \e cP in the camera frame located in the
- vpTracker::cP public attribute. The parameters \f$p=(\rho,
- \theta)\f$ are updated in the vpTracker::p public attribute.
-
- \warning To compute these parameters \e p, the method exploit the feature
- parameters \e cP in the camera frame. Thus, vpTracker::cP need to be
- updated before the call of this method. For that, a call to
- changeFrame(const vpHomogeneousMatrix &) is requested.
-
- The code below shows how to use this method.
- \code
- //Create the line
- vpLine line;
-
- //Set the coordinates of the line in the object frame in meter.
- line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
- //Here the line is define by the intersection between the plane X = 0.5m and Z = 0.5m
-
- //Create the homogeneous matrix
- vpHomogeneousMatrix cMo;
- //Computes or set here the homogeneous matrix
-
- //Computes the equations of the two planes in the camera frame
- line.changeFrame(cMo);
-
- //Computes the line features in the camera frame( rho and theta)
- line.projection();
- \endcode
-*/
-void
-vpLine::projection()
-{
- projection(cP,p) ;
-}
-
-
-/*!
-
- Computes the 2D parameters \e p of the line in the image plane thanks
- to the 3D line features \e cP expressed in the camera frame. The
- image plane parameters \f$p=(\rho , \theta)\f$ are updated in
- output.
-
- \param cP_ : The vector containing the line features relative to the
- camera frame. \f[ cP = \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1
- \\ A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
-
- \param p_ : The vector which contains the 2D line features expressed
- in the image plane. \f[ p = \left[\begin{array}{c} \rho \\ \theta
- \end{array}\right] \f]
-
- \exception vpException::fatalError : Degenerate case, the image of the straight line is a point.
-*/
-void
-vpLine::projection(const vpColVector &cP_, vpColVector &p_)
-{
- //projection
-
- if (cP.getRows() != 8)
- throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
-
- double A1, A2, B1, B2, C1, C2, D1, D2;
-
- A1=cP_[0] ;
- B1=cP_[1] ;
- C1=cP_[2] ;
- D1=cP_[3] ;
-
- A2=cP_[4] ;
- B2=cP_[5] ;
- C2=cP_[6] ;
- D2=cP_[7] ;
-
- double a, b, c, s;
- a = A2*D1 - A1*D2;
- b = B2*D1 - B1*D2;
- c = C2*D1 - C1*D2;
- s = a*a+b*b;
- if (s <= 1e-8) // seuil pas terrible
- {
- printf("Degenerate case: the image of the straight line is a point!\n");
- throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
- }
- s = 1.0/sqrt(s);
-
- double rho = -c*s ;
- double theta = atan2( b, a);
-
- if (p.getRows() != 2)
- p.resize(2);
-
- p_[0] = rho ;
- p_[1] = theta ;
-}
-
-
-/*!
-
- Computes the line parameters \e cP in the camera frame thanks to the
- line parameters \e oP given in the object frame and the homogeneous matrix
- relative to the pose \e cMo between the object frame and the camera
- frame. Thus the computation gives the equations of the two planes
- needed to define the line in the camera frame thanks to the
- equations of the same two planes in the object frame.
-
- In input of this method, the line parameters \e oP in the object
- frame are those from the vpForwardProjection::oP public attribute.
-
- As a result of this method, line parameters \e cP in the camera
- frame are updated in the vpTracker::cP public attribute.
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
- The code below shows how to use this method.
- \code
- //Create the line
- vpLine line;
-
- //Set the coordinates of the line in the object frame in meter.
- line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
- //The line is define by the intersection between the plane X = 0.5m and Z = 0.5m
-
- //Create the homogeneous matrix
- vpHomogeneousMatrix cMo;
- //Computes or set here the homogeneous matrix
-
- //Computes the equations of the two planes in the camera frame
- line.changeFrame(cMo);
- \endcode
-*/
-void
-vpLine::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo,cP) ;
-}
-
-
-/*!
-
- Computes the line parameters \e cP in the camera frame thanks to the
- line parameters \e oP given in the object frame and the homogeneous
- matrix relative to the pose between the camera frame and the
- object frame. Thus the computation gives the equations of the two
- planes needed to define the line in the desired frame thanks to the
- equations of the same two planes in the object frame.
-
- In input of this method, the line parameters \e oP in the object
- frame are those from the vpForwardProjection::oP public attribute.
-
- \param cMo : The homogeneous matrix relative to the pose
- between the desired frame and the object frame.
-
- \param cP_ : The vector which will contain the parameters of the two
- planes in the camera frame. \f[ cP =
- \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \\ A2 \\ B2 \\ C2 \\ D2
- \end{array}\right] \f]
-
- The code below shows how to use this method.
- \code
- //Create the line
- vpLine line;
-
- //Set the coordinates of the line in the object frame in meter.
- line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
- //The line is define by the intersection between the plane X = 0.5m and Z = 0.5m
-
- //Create the homogeneous matrix
- vpHomogeneousMatrix cMo;
- //Computes or set here the homogeneous matrix
-
- //Creates the vector which will contain the line features
- vpColVector cP(8);
-
- //Computes the equations of the two planes in the camera frame
- line.changeFrame(cMo, cP);
- \endcode
-*/
-
-void
-vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
-{
-
- double a1, a2, b1, b2, c1, c2, d1, d2;
- double A1, A2, B1, B2, C1, C2, D1, D2;
-
- // in case of verification
- // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
-
- if (cP.getRows() != 8)
- cP.resize(8);
-
- a1=oP[0] ;
- b1=oP[1] ;
- c1=oP[2] ;
- d1=oP[3] ;
-
- a2=oP[4] ;
- b2=oP[5] ;
- c2=oP[6] ;
- d2=oP[7] ;
-
- A1 = cMo[0][0]*a1 + cMo[0][1]*b1 + cMo[0][2]*c1;
- B1 = cMo[1][0]*a1 + cMo[1][1]*b1 + cMo[1][2]*c1;
- C1 = cMo[2][0]*a1 + cMo[2][1]*b1 + cMo[2][2]*c1;
- D1 = d1 - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
-
- A2 = cMo[0][0]*a2 + cMo[0][1]*b2 + cMo[0][2]*c2;
- B2 = cMo[1][0]*a2 + cMo[1][1]*b2 + cMo[1][2]*c2;
- C2 = cMo[2][0]*a2 + cMo[2][1]*b2 + cMo[2][2]*c2;
- D2 = d2 - (cMo[0][3]*A2 + cMo[1][3]*B2 + cMo[2][3]*C2);
-
- // in case of verification
- // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
- // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2;
-
- // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
- // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
-
- // Adding constraints on the straight line to have a unique representation
-
- // direction of the straight line = N1 x N2
- a2 = B1*C2 - C1*B2;
- b2 = C1*A2 - A1*C2;
- c2 = A1*B2 - B1*A2;
-
- // Constraint D1 = 0 (the origin belongs to P1)
- a1 = A2*D1 - A1*D2;
- b1 = B2*D1 - B1*D2;
- c1 = C2*D1 - C1*D2;
-
- if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
- {
- A2 = A1;
- B2 = B1;
- C2 = C1;
- D2 = D1;
- }
-
- // Constraint A1^2 + B1^2 + C1^2 = 1
- d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
- cP_[0] = A1 = a1*d1 ;
- cP_[1] = B1 = b1*d1 ;
- cP_[2] = C1 = c1*d1 ;
- cP_[3] = D1 = 0 ;
-
- // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
- // N2_new = (N1 x N2) x N1_new
- a1 = b2*C1 - c2*B1;
- b1 = c2*A1 - a2*C1;
- c1 = a2*B1 - b2*A1;
-
- // Constraint A2^2 + B2^2 + C2^2 = 1
- d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
- a1 *= d1 ;
- b1 *= d1 ;
- c1 *= d1 ;
-
- // D2_new = D2 / (N2^T . N2_new)
- D2 /= (A2*a1 + B2*b1 + C2*c1);
- A2 = a1;
- B2 = b1;
- C2 = c1;
-
- // Constraint D2 < 0
- if (D2 > 0)
- {
- A2 = -A2;
- B2 = -B2;
- C2 = -C2;
- D2 = -D2;
- }
- // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
- // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
-
- cP_[4] = A2;
- cP_[5] = B2;
- cP_[6] = C2;
- cP_[7] = D2;
-
- // in case of verification
- /*
- x = -A2*D2;
- y = -B2*D2;
- z = -C2*D2;
- d1 = ap1*x+bp1*y+cp1*z+dp1;
- d2 = ap2*x+bp2*y+cp2*z+dp2;
- if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
- {
- printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2);
- exit(-1);
- }
- d1 = A1*x+B1*y+C1*z+D1;
- d2 = A2*x+B2*y+C2*z+D2;
- if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
- {
- printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
- exit(-1);
- }
- */
-
-}
-
-
-
-/*!
-
- Displays the line in the image \e I thanks to the 2D parameters of
- the line \e p in the image plane (vpTracker::p) and the camera
- parameters which enable to convert the parameters from meter to
- pixel.
-
- \param I : The image where the line must be displayed.
-
- \param cam : The camera parameters to enable the conversion from
- meter to pixel.
-
- \param color : The desired color to display the line in the image.
-
- \param thickness : Thickness of the feature representation.
-*/
-void vpLine::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness) ;
-}
-
-
-/*!
-
- Displays the line in the image \e I thanks to the parameters in the
- object frame (vpForwardProjection::oP), the homogeneous matrix
- relative to the pose between the camera frame and the object frame
- and the camera parameters which enable to convert the features from
- meter to pixel.
-
- \param I : The image where the line must be displayed in overlay.
-
- \param cMo : The homogeneous matrix corresponding to the pose
- between the camera frame and the object frame.
-
- \param cam : The camera parameters to enable the conversion from
- meter to pixel.
-
- \param color : The desired color to display the line in the image.
-
- \param thickness : Thickness of the feature representation.
-*/
-// non destructive wrt. cP and p
-void vpLine::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayLine(_p[0],_p[1],
- cam, I, color, thickness) ;
-
-}
-
-
-/*!
- Create an object with the same type.
-
- \code
- vpForwardProjection *fp;
- vpLine line;
- fp = line.duplicate(); // fp is now a vpLine
- \endcode
-
-*/
-vpLine *vpLine::duplicate() const
-{
- vpLine *feature = new vpLine(*this) ;
- return feature ;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/forward-projection/vpLine.h b/src/tracking/forward-projection/vpLine.h
deleted file mode 100644
index 780a1a0..0000000
--- a/src/tracking/forward-projection/vpLine.h
+++ /dev/null
@@ -1,219 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpLine.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Line feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpLine_H
-#define vpLine_H
-
-/*!
- \file vpLine.h
- \brief class that defines what is a line
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#include <visp/vpForwardProjection.h>
-
-/*!
- \class vpLine
- \ingroup TrackingFeature GeometryFeature
-
- \brief Class that defines a line in the object frame, the
- camera frame and the image plane. All the parameters
- must be set in meter.
-
- \par Object and camera frame parametrization:
- In the 3D frames, the object frame parameters (\e oP) and the camera
- frame parameters (\e cP), the line is defined as the intersection
- between two plans. Thus, the parameters which define the line are
- the parameters which define the two equations of the two plans. Each
- point which belongs to the line is a solution of those two
- equations:
-
- \par
- \f[ A1 X + B1 Y + C1 Z +D1 = 0 \f]
- \f[ A2 X + B2 Y + C2 Z +D2 = 0 \f]
- Here \f$ (X, Y, Z) \f$ are the 3D coordinates in one of the two 3D frames.
-
- \par
- In this class it is easily possible to compute the parameters (\e cP) of the
- line in the camera frame thanks to its parameters (\e oP) in the
- object frame. But you have to notes that four constraints are
- added in the planes equations.
-
- \par
- \f[ D1 = 0 \f]
- \f[ D2 > 0 \f]
- \f[ A1 A2 + B1 B2 + C1 C2 = 0 \f]
- \f[ || A2 || = 1 \f]
-
-
- \par
- The line parameters \e oP corresponding to the object frame are
- located in the vpForwardProjection::oP public attribute, where \e oP
- is a vector defined as: \f[ oP = \left[\begin{array}{c}A1_o \\ B1_o
- \\ C1_o \\ D1_o \\ A2_o \\ B2_o \\ C2_o \\ D2_o \end{array}\right]
- \f]
-
- \par
- The line parameters corresponding to the camera frame are located
- in the vpTracker::cP public attribute, where \e cP is a vector
- defined as: \f[ cP = \left[\begin{array}{c}A1_c \\ B1_c \\ C1_c \\
- D1_c \\ A2_c \\ B2_c \\ C2_c \\ D2_c \end{array}\right] \f]
-
- \par Image plane parametrization:
- In the image plane, the line is defined thanks to its 2D equation.
- \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f] Here \f$ x
- \f$ and \f$ y \f$ are the coordinates of a point belonging to the
- line in the image plane while \f$ \rho \f$ and \f$ \theta \f$ are
- the parameters used to define the line. The value of \f$ \theta
- \f$ is between \f$ -\pi/2 \f$ and \f$ \pi/2 \f$ and the value of
- \f$ \rho \f$ can be positive or negative. The conventions used to
- choose the sign of \f$ \rho \f$ and the value of \f$ \theta \f$
- are illustrated by the following image.
-
- \par
- \image html vpFeatureLine.gif
- \image latex vpFeatureLine.ps width=10cm
-
- \par
- The line parameters corresponding to the image frame are located
- in the vpTracker::p public attribute, where \e p is a vector defined
- as: \f[ p = \left[\begin{array}{c} \rho \\ \theta \end{array}\right]
- \f]
-*/
-class VISP_EXPORT vpLine : public vpForwardProjection
-{
-
-public:
-
- void init() ;
-
- vpLine() ;
- //! Destructor
- virtual ~vpLine() { ; }
-
- /*!
-
- Sets the \f$ \rho \f$ parameter used to define the line in the
- image plane.
-
- \param rho : The desired value for \f$ \rho \f$.
-
- \sa setTheta()
- */
- void setRho(const double rho) { p[0] = rho ; };
-
- /*!
- Sets the \f$ \theta \f$ angle value used to define the line in the
- image plane.
-
- \param theta : The desired value for \f$ \theta \f$ angle.
-
- \sa setRho()
- */
- void setTheta(const double theta) { p[1] = theta ;};
-
- /*!
-
- Gets the \f$ \theta \f$ angle value corresponding to one of the
- two parameters used to define the line parametrization in the
- image plane.
-
- \return Returns the current value of \f$ \theta \f$.
-
- \sa getRho()
- */
- double getTheta() const { return p[1] ; }
-
- /*!
- Gets the \f$ \rho \f$ value corresponding to one of the
- two parameters used to define the line parametrization in the
- image plane.
-
- \return Returns the current value of \f$ \rho \f$.
-
- \sa getTheta()
- */
- double getRho() const { return p[0] ; }
-
-
-
- void setWorldCoordinates(const double &A1, const double &B1,
- const double &C1, const double &D1,
- const double &A2, const double &B2,
- const double &C2, const double &D2) ;
-
-
- void setWorldCoordinates(const vpColVector &oP1,
- const vpColVector &oP2) ;
-
-
- void setWorldCoordinates(const vpColVector &oP) ;
-
-
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
-
- vpLine *duplicate() const ;
-} ;
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/forward-projection/vpPoint.cpp b/src/tracking/forward-projection/vpPoint.cpp
deleted file mode 100644
index 5c7e2dc..0000000
--- a/src/tracking/forward-projection/vpPoint.cpp
+++ /dev/null
@@ -1,320 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoint.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Point feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpPoint.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFeatureDisplay.h>
-
-#include <visp/vpHomography.h>
-
-/*!
- \file vpPoint.cpp
- \brief class that defines what is a point
-*/
-
-
-
-void
-vpPoint::init()
-{
- p.resize(3) ; p = 0 ; p[2] = 1 ;
- oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
- cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
-
- //default value Z (1 meters)
- set_Z(1) ;
-}
-
-vpPoint::vpPoint()
-{
- init() ;
-}
-
-//! set the point world coordinates
-void
-vpPoint::setWorldCoordinates(const double ox,
- const double oy,
- const double oz)
-{
- oP[0] = ox ;
- oP[1] = oy ;
- oP[2] = oz ;
- oP[3] = 1 ;
-}
-
-
-void
-vpPoint::setWorldCoordinates(const vpColVector &_oP)
-{
- oP[0] = _oP[0] ;
- oP[1] = _oP[1] ;
- oP[2] = _oP[2] ;
- oP[3] = _oP[3] ;
-
- oP /= oP[3] ;
-}
-
-void
-vpPoint::getWorldCoordinates(double& ox,
- double& oy,
- double& oz)
-{
- ox = oP[0] ;
- oy = oP[1] ;
- oz = oP[2] ;
-}
-
-
-void
-vpPoint::getWorldCoordinates(vpColVector &_oP)
-{
- _oP[0] = oP[0] ;
- _oP[1] = oP[1] ;
- _oP[2] = oP[2] ;
- _oP[3] = oP[3] ;
-}
-
-
-vpColVector
-vpPoint::getWorldCoordinates(void)
-{
- return this->oP;
-}
-
-
-
-/*!
- Compute the perspective projection of a point _cP.
-
- \param _cP : Three dimension vector that corresponds to the coordinates of the point in the camera frame.
- \param _p : Coordinates of the point in the image plane obtained by perspective projection.
-*/
-void
-vpPoint::projection(const vpColVector &_cP, vpColVector &_p)
-{
- _p.resize(3) ;
-
- _p[0] = _cP[0]/_cP[2] ;
- _p[1] = _cP[1]/_cP[2] ;
- _p[2] = 1 ;
-}
-
-/*!
- From the 3D coordinates of the point in the object frame set using for example
- setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
- of the point in the camera frame.
-
- \param cMo : Transformation from camera to object frame.
- \param _cP : 3D coordinates of the point in the camera frame.
-*/
-void
-vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
-{
-
- _cP.resize(4) ;
-
- _cP[0] = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
- _cP[1] = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
- _cP[2] = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
- _cP[3] = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
-
- double d = 1/_cP[3] ;
- _cP[0] *=d ;
- _cP[1] *=d ;
- _cP[2] *=d ;
- _cP[3] *=d ; ;
-}
-
-/*!
- From the coordinates of the point in camera frame b and the transformation between
- camera frame a and camera frame b computes the coordinates of the point in camera frame a.
-
- \param aMb : 3D transformation between camera frame a and b.
- \param bP : 3D coordinates of the point in camera frame bP.
-
- \return A point with 3D coordinates in the camera frame a. The coordinates in the world or object
- frame are set to the same coordinates than the one in the camera frame.
-*/
-const vpPoint
-operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
-{
- vpPoint aP ;
-
- vpColVector v(4),v1(4) ;
-
- v[0] = bP.get_X() ;
- v[1] = bP.get_Y() ;
- v[2] = bP.get_Z() ;
- v[3] = bP.get_W() ;
-
- v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
- v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
- v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
- v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
-
- v1 /= v1[3] ;
-
- // v1 = M*v ;
- aP.set_X(v1[0]) ;
- aP.set_Y(v1[1]) ;
- aP.set_Z(v1[2]) ;
- aP.set_W(v1[3]) ;
-
- aP.set_oX(v1[0]) ;
- aP.set_oY(v1[1]) ;
- aP.set_oZ(v1[2]) ;
- aP.set_oW(v1[3]) ;
-
- return aP ;
-}
-
-/*!
- From the coordinates of the point in image plane b and the homography between image
- a and b computes the coordinates of the point in image plane a.
-
- \param aHb : Homography between image a and b.
- \param bP : 2D coordinates of the point in the image plane b.
-
- \return A point with 2D coordinates in the image plane a.
-*/
-const vpPoint
-operator*(const vpHomography &aHb, const vpPoint& bP)
-{
- vpPoint aP ;
- vpColVector v(3),v1(3) ;
-
- v[0] = bP.get_x() ;
- v[1] = bP.get_y() ;
- v[2] = bP.get_w() ;
-
- v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
- v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
- v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
-
- // v1 = M*v ;
- aP.set_x(v1[0]) ;
- aP.set_y(v1[1]) ;
- aP.set_w(v1[2]) ;
-
- return aP ;
-}
-
-//! For memory issue (used by the vpServo class only).
-vpPoint *vpPoint::duplicate() const
-{
- vpPoint *feature = new vpPoint(*this) ;
- return feature ;
-}
-
-/*!
- Display the point in the image.
-*/
-void
-vpPoint::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
-
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
-
- if(_cP[2] < 0) // no display if point is behind the camera
- return;
-
- vpPoint::projection(_cP,_p) ;
- vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
-}
-
-/*!
- Display the point in the image.
-*/
-void
-vpPoint::display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
-
- if(_cP[2] < 0) // no display if point is behind the camera
- return;
-
- vpPoint::projection(_cP,_p) ;
- vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpPoint& /* vpp */)
-{
- return( os<<"vpPoint" );
-}
-
-vpPoint&
-vpPoint::operator=(const vpPoint& vpp )
-{
- p = vpp.p;
- cP = vpp.cP;
- oP = vpp.oP;
- cPAvailable = vpp.cPAvailable;
-
- return *this;
-}
-
-/*!
- Display the point in the image.
-*/
-void
-vpPoint::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness) ;
-}
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/forward-projection/vpPoint.h b/src/tracking/forward-projection/vpPoint.h
deleted file mode 100644
index ebe0a83..0000000
--- a/src/tracking/forward-projection/vpPoint.h
+++ /dev/null
@@ -1,211 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpPoint.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Point feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpPoint_H
-#define vpPoint_H
-
-class vpHomography ;
-
-/*!
- \file vpPoint.h
- \brief class that defines what is a point
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#include <visp/vpForwardProjection.h>
-
-class vpHomography;
-
-/*!
- \class vpPoint
- \ingroup TrackingFeature GeometryFeature
- \brief Class that defines what is a point.
-*/
-class VISP_EXPORT vpPoint : public vpForwardProjection
-{
-
-public:
- //! Basic constructor.
- vpPoint() ;
- //! Destructor.
- virtual ~vpPoint() { ; }
-
-public:
-
- // Compute the 3D coordinates _cP (camera frame)
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) ;
-
- /*!
- From the 3D coordinates of the point in the object frame set using for example
- setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
- of the point in the camera frame.
-
- \param cMo : Transformation from camera to object frame.
-
- */
- inline void changeFrame(const vpHomogeneousMatrix &cMo) {
- double X = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
- double Y = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
- double Z = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
- double W = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
-
- double d = 1/W ;
- cP[0] =X*d ;
- cP[1] =Y*d ;
- cP[2] =Z*d ;
- cP[3] =1 ;
- }
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- vpPoint *duplicate() const ;
-
- // Get coordinates
- //! Get the point X coordinate in the camera frame.
- double get_X() const { return cP[0] ; }
- //! Get the point Y coordinate in the camera frame.
- double get_Y() const { return cP[1] ; }
- //! Get the point Z coordinate in the camera frame.
- double get_Z() const { return cP[2] ; }
- //! Get the point W coordinate in the camera frame.
- double get_W() const { return cP[3] ; }
-
- //! Get the point X coordinate in the object frame.
- double get_oX() const { return oP[0] ; }
- //! Get the point Y coordinate in the object frame.
- double get_oY() const { return oP[1] ; }
- //! Get the point Z coordinate in the object frame.
- double get_oZ() const { return oP[2] ; }
- //! Get the point W coordinate in the object frame.
- double get_oW() const { return oP[3] ; }
-
- //! Get the point x coordinate in the image plane.
- double get_x() const { return p[0] ; }
- //! Get the point y coordinate in the image plane.
- double get_y() const { return p[1] ; }
- //! Get the point w coordinate in the image plane.
- double get_w() const { return p[2] ; }
-
- //! Get the point world coordinates. We mean here the coordinates of the point in the object frame.
- void getWorldCoordinates(double& ox,
- double& oy,
- double& oz) ;
- //! Get the point world coordinates. We mean here the coordinates of the point in the object frame.
- void getWorldCoordinates(vpColVector &_oP) ;
- vpColVector getWorldCoordinates(void) ;
-
- //! Basic construction.
- void init() ;
-
- friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpPoint& vpp);
- vpPoint& operator=(const vpPoint& vpp);
-
- //! Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP, output : the 2D coordinates _p.
- void projection(const vpColVector &_cP, vpColVector &_p) ;
-
- /*!
- Perspective projection of the point.
-
- Projection onto the //image plane of the point. Update the object
- attribute p (2D //homogeneous coordinates) according to object
- attribute cP (current //3D coordinates in the camera frame).
-
- */
- inline void projection() {
- double d = 1/cP[2] ;
- p[0] = cP[0]*d ;
- p[1] = cP[1]*d ;
- p[2] = 1 ;
- }
-
- // Set coordinates
- //! Set the point X coordinate in the camera frame.
- inline void set_X(const double X) { cP[0] = X ; }
- //! Set the point Y coordinate in the camera frame.
- inline void set_Y(const double Y) { cP[1] = Y ; }
- //! Set the point Z coordinate in the camera frame.
- inline void set_Z(const double Z) { cP[2] = Z ; }
- //! Set the point W coordinate in the camera frame.
- inline void set_W(const double W) { cP[3] = W ; }
-
- //! Set the point X coordinate in the object frame.
- inline void set_oX(const double X) { oP[0] = X ; }
- //! Set the point Y coordinate in the object frame.
- inline void set_oY(const double Y) { oP[1] = Y ; }
- //! Set the point Z coordinate in the object frame.
- inline void set_oZ(const double Z) { oP[2] = Z ; }
- //! Set the point W coordinate in the object frame.
- inline void set_oW(const double W) { oP[3] = W ; }
-
- //! Set the point x coordinate in the image plane.
- inline void set_x(const double x) { p[0] = x ; }
- //! Set the point y coordinate in the image plane.
- inline void set_y(const double y) { p[1] = y ; }
- //! Set the point w coordinate in the image plane.
- inline void set_w(const double w) { p[2] = w ; }
-
- //! Set the point world coordinates. We mean here the coordinates of the point in the object frame.
- void setWorldCoordinates(const double ox,
- const double oy,
- const double oz) ;
- //! Set the point world coordinates. We mean here the coordinates of the point in the object frame.
- void setWorldCoordinates(const vpColVector &_oP) ;
-} ;
-
-const vpPoint VISP_EXPORT operator*(const vpHomogeneousMatrix &M, const vpPoint& p) ;
-const vpPoint VISP_EXPORT operator*(const vpHomography &H, const vpPoint& p) ;
-
-#endif
diff --git a/src/tracking/forward-projection/vpSphere.cpp b/src/tracking/forward-projection/vpSphere.cpp
deleted file mode 100644
index 4e380e4..0000000
--- a/src/tracking/forward-projection/vpSphere.cpp
+++ /dev/null
@@ -1,234 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSphere.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Sphere feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpSphere.h>
-#include <visp/vpFeatureDisplay.h>
-
-
-void
-vpSphere::init()
-{
-
- oP.resize(4) ;
- cP.resize(4) ;
-
- p.resize(5) ;
-}
-
-void
-vpSphere::setWorldCoordinates(const vpColVector& oP_)
-{
- this->oP = oP_ ;
-}
-
-void
-vpSphere::setWorldCoordinates(const double X0, const double Y0,
- const double Z0, const double R)
-{
- oP[0] = X0 ;
- oP[1] = Y0 ;
- oP[2] = Z0 ;
- oP[3] = R ;
-}
-
-
-
-vpSphere::vpSphere()
-{
- init() ;
-}
-
-
-vpSphere::vpSphere(const vpColVector& oP_)
-{
- init() ;
- setWorldCoordinates(oP_) ;
-}
-
-vpSphere::vpSphere(const double X0, const double Y0,
- const double Z0, const double R)
-{
- init() ;
- setWorldCoordinates(X0, Y0, Z0, R) ;
-}
-
-vpSphere::~vpSphere()
-{
-}
-
-
-//! perspective projection of the sphere
-void
-vpSphere::projection()
-{
- projection(cP,p) ;
-}
-
-//! Perspective projection of the circle.
-void
-vpSphere::projection(const vpColVector &cP_, vpColVector &p_)
-{
- double x0, y0, z0; //variables intermediaires
-// double k0, k1, k2, k3, k4; //variables intermediaires
- double E, A, B; //variables intermediaires
-
- //calcul des parametres M20, M11, M02 de l'ellipse
- double s, a, b, r, e; //variables intermediaires
- r = cP_[3];
-
- x0 = cP_[0] ;
- y0 = cP_[1] ;
- z0 = cP_[2] ;
-
- s = r*r - y0*y0 -z0*z0;
-
-// k0 = (r*r - x0*x0 -z0*z0)/s;
-// k1 = (x0*y0)/s;
-// k2 = (x0*z0)/s;
-// k3 = (y0*z0)/s;
-// k4 = (r*r - x0*x0 -y0*y0)/s;
-
- if ((s = z0*z0 - r*r) < 0.0)
- {
- vpERROR_TRACE("sphere derriere le plan image\n");
- }
-
- p_[0] = x0*z0/s ; //x
- p_[1] = y0*z0/s ; //y
-
- if (fabs(x0) > 1e-6)
- {
- // vpERROR_TRACE(" %f",r) ;
- e = y0/x0;
- b = r/sqrt(s);
- if ((a = x0*x0 + y0*y0 + z0*z0 - r*r) < 0.0)
- {
- vpERROR_TRACE("sphere derriere le plan image\n");
- }
- a = r*sqrt(a)/s;
- if (fabs(e) <= 1.0)
- {
- E = e;
- A = a;
- B = b;
- }
- else
- {
- E = -1.0/e;
- A = b;
- B = a;
- }
- }
- else
- {
- // vpERROR_TRACE(" %f",r) ;
- E = 0.0;
- A = r/sqrt(s);
- B = r*sqrt(y0*y0+z0*z0-r*r)/s;
- }
-
- p_[2] = ( A*A + B*B * E*E) / (1.0 + E*E); // mu20
- p_[3] = ( A*A - B*B) * E / (1.0 + E*E); // mu11
- p_[4] = ( B*B + A*A * E*E) / (1.0 + E*E); // mu02
-
- // vpERROR_TRACE(" %f",r) ;
-
- // std::cout << p.t() ;
-}
-//! perspective projection of the circle
-void
-vpSphere::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo,cP) ;
-}
-
-//! Perspective projection of the circle.
-void
-vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
-{
- double x0, y0, z0; //variables intermediaires
-
- x0 = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2] + cMo[0][3];
- y0 = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2] + cMo[1][3];
- z0 = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2] + cMo[2][3];
-
- cP_[3] = oP[3];
-
- cP_[0] = x0 ;
- cP_[1] = y0 ;
- cP_[2] = z0 ;
-}
-
-//! for memory issue (used by the vpServo class only)
-vpSphere *vpSphere::duplicate() const
-{
- vpSphere *feature = new vpSphere(*this) ;
- return feature ;
-}
-
-
-
-// non destructive wrt. cP and p
-void vpSphere::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
- cam, I, color, thickness) ;
-
-}
-
-
-
-void vpSphere::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
-{
- vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
- cam, I, color, thickness) ;
-}
diff --git a/src/tracking/forward-projection/vpSphere.h b/src/tracking/forward-projection/vpSphere.h
deleted file mode 100644
index dfef7c9..0000000
--- a/src/tracking/forward-projection/vpSphere.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpSphere.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Sphere feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpSphere.h
- \brief forward projection of a sphere
-*/
-
-#ifndef vpSphere_hh
-#define vpSphere_hh
-
-
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#include <visp/vpForwardProjection.h>
-#include <math.h>
-/*!
- \class vpSphere
- \ingroup TrackingFeature GeometryFeature
- \brief Class that defines what is a sphere.
-
- Forward projection of a sphere.
-*/
-class VISP_EXPORT vpSphere : public vpForwardProjection
-{
-public:
- void init() ;
- vpSphere() ;
- virtual ~vpSphere() ;
-
-public:
- vpSphere(const vpColVector& oP) ;
- vpSphere(const double X0, const double Y0,
- const double Z0,
- const double R) ;
-
-
- void setWorldCoordinates(const vpColVector& oP) ;
- void setWorldCoordinates(const double X0, const double Y0,
- const double Z0,
- const double R) ;
-
- double get_x() const { return p[0] ; }
- double get_y() const { return p[1] ; }
- double get_mu20() const { return p[2] ; }
- double get_mu11() const { return p[3] ; }
- double get_mu02() const { return p[4] ; }
-
- double getX() const { return cP[0] ; }
- double getY() const { return cP[1] ; }
- double getZ() const { return cP[2] ; }
-
- double getR() const { return cP[3] ; }
-
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
-
- vpSphere *duplicate() const ;
-} ;
-
-
-#endif
diff --git a/src/tracking/general-tracking-issues/vpTracker.cpp b/src/tracking/general-tracking-issues/vpTracker.cpp
deleted file mode 100644
index 4e37571..0000000
--- a/src/tracking/general-tracking-issues/vpTracker.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTracker.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic tracker.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpTracker.h>
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpTracker.cpp
- \brief Class that defines what is a generic tracker.
-*/
-
-
-void
-vpTracker::init()
-{
- cPAvailable = false ;
-}
-
-
-
-vpTracker::vpTracker() : p(), cP(), cPAvailable(false) {}
-
-vpTracker::vpTracker(const vpTracker &tracker) : p(), cP(), cPAvailable(false)
-{
- *this = tracker;
-}
-
-
-vpTracker &vpTracker::operator=(const vpTracker &tracker)
-{
- p = tracker.p;
- cP = tracker.cP;
- cPAvailable = tracker.cPAvailable;
-
- return *this;
-}
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/general-tracking-issues/vpTracker.h b/src/tracking/general-tracking-issues/vpTracker.h
deleted file mode 100644
index 3acc3b7..0000000
--- a/src/tracking/general-tracking-issues/vpTracker.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTracker.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic tracker.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpTracker_H
-#define vpTracker_H
-
-/*!
- \file vpTracker.h
- \brief Class that defines what is a generic tracker.
-*/
-
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-
-/*!
- \class vpTracker
- \brief Class that defines what is a feature generic tracker.
-
- A tracker is able to track features with parameters expressed in:
- - in the camera frame \e cP. These parameters are located in the public
- attribute vpTracker::cP.
- - in the image plane \e p. These parameters are located in the public
- attribute vpTracker::p. They correspond to normalized coordinates
- of the feature expressed in meters.
-
-*/
-class VISP_EXPORT vpTracker
-{
-
-
-public:
- /*!
- Feature coordinates expressed in the image plane \e p. They correspond
- to 2D normalized coordinates expressed in meters.
- */
- vpColVector p ;
- /*!
- Feature coordinates expressed in the camera frame \e cP.
- */
- vpColVector cP ;
-
- /*!
- Flag used to indicate if the feature parameters \e cP expressed
- in the camera frame are available.
- */
- bool cPAvailable ;
-
-public:
- //! Default initialization.
- void init() ;
- //! Default constructor.
- vpTracker() ;
- //! Copy constructor.
- vpTracker(const vpTracker &tracker) ;
- //! Copy operator.
- vpTracker &operator=(const vpTracker &tracker);
-
- //! Destructor.
- virtual ~vpTracker() { ; }
-} ;
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/tracking/general-tracking-issues/vpTrackingException.h b/src/tracking/general-tracking-issues/vpTrackingException.h
deleted file mode 100644
index 4528593..0000000
--- a/src/tracking/general-tracking-issues/vpTrackingException.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTrackingException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exceptions that can be emited by the vpTracking class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpTrackingException_H
-#define __vpTrackingException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpTrackingException.h
- \brief error that can be emited by the vpTracker class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpTrackingException
- \ingroup Exception
- \brief Error that can be emited by the vpTracker class and its derivates.
- */
-class VISP_EXPORT vpTrackingException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpTracking member
- */
- enum errorTrackingCodeEnum
- {
- featureLostError,
-
- // Moving edges
- notEnoughPointError,
- initializationError,
- fatalError
- } ;
-
- public:
- vpTrackingException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpTrackingException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpTrackingException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/tracking/klt/vpKltOpencv.cpp b/src/tracking/klt/vpKltOpencv.cpp
deleted file mode 100644
index 787c843..0000000
--- a/src/tracking/klt/vpKltOpencv.cpp
+++ /dev/null
@@ -1,1289 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKltOpencv.cpp 5189 2015-01-21 16:42:18Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
- * with opencv.
- *
- * Authors:
- * Fabien Servant
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpKltOpencv.cpp
-
- \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
- implemented with opencv.
-*/
-
-#include <visp/vpConfig.h>
-
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-
-#include <string>
-
-#include <visp/vpDisplay.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpTrackingException.h>
-
-/*!
- 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), m_pyrMaxLevel(3),
- m_next_points_id(0), m_initial_guess(false)
-{
- m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS, 20, 0.03);
-
-}
-
-/*!
- Copy constructor.
- */
-vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
- : 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), m_pyrMaxLevel(3),
- m_next_points_id(0), m_initial_guess(false)
-{
- *this = copy;
-}
-
-/*!
- Copy operator.
- */
-vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
-{
- m_gray = copy.m_gray;
- m_prevGray = copy.m_prevGray;
- m_points[0] = copy.m_points[0];
- m_points[1] = copy.m_points[1];
- m_points_id = copy.m_points_id;
- m_maxCount = copy.m_maxCount;
- m_termcrit = copy.m_termcrit;
- m_winSize = copy.m_winSize;
- m_qualityLevel = copy.m_qualityLevel;
- m_minDistance = copy.m_minDistance;
- m_minEigThreshold = copy.m_minEigThreshold;
- m_harris_k = copy.m_harris_k;
- m_blockSize = copy.m_blockSize;
- m_useHarrisDetector = copy.m_useHarrisDetector;
- m_pyrMaxLevel = copy.m_pyrMaxLevel;
- m_next_points_id = copy.m_next_points_id;
- m_initial_guess = copy.m_initial_guess;
-
- return *this;
-}
-
-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;
-
- //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
- I.copyTo(m_gray);
-
- for (size_t i=0; i<2; i++) {
- m_points[i].clear();
- }
-
- m_points_id.clear();
-
- cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, 0, m_harris_k);
-
- if(m_points[1].size() > 0){
- cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1,-1), m_termcrit);
-
- for (size_t i=0; i < m_points[1].size(); i++)
- m_points_id.push_back(m_next_points_id++);
- }
-}
-
-/*!
- 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)
- throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
-
- std::vector<float> err;
- int flags = 0;
-
- cv::swap(m_prevGray, m_gray);
-
- if (m_initial_guess) {
- flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
- m_initial_guess = false;
- }
- else {
- std::swap(m_points[1], m_points[0]);
- }
-
- //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
- I.copyTo(m_gray);
-
- if(m_prevGray.empty()){
- m_gray.copyTo(m_prevGray);
- }
-
- std::vector<uchar> status;
-
- cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
- m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);
-
- // Remove points that are lost
- for (int i=(int)status.size()-1; i>=0; i--) {
- if (status[(size_t)i] == 0) { // point is lost
- m_points[0].erase(m_points[0].begin()+i);
- m_points[1].erase(m_points[1].begin()+i);
- m_points_id.erase(m_points_id.begin()+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, int &id, float &x, float &y) const
-{
- if ((size_t)index >= m_points[1].size()){
- throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
- }
-
- x = m_points[1][(size_t)index].x;
- y = m_points[1][(size_t)index].y;
- 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<unsigned char> &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<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, 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<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
- }
-}
-
-/*!
-
- 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
-*/
-void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (size_t i = 0; i < features.size(); i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10, color, thickness);
-
- std::ostringstream id;
- id << featuresid[i];
- ip.set_u( vpMath::round( features[i].x + 5 ) );
- vpDisplay::displayText(I, ip, id.str(), color);
- }
-}
-
-/*!
-
- 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
-*/
-void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10, color, thickness);
-
- std::ostringstream id;
- id << featuresid[i];
- ip.set_u( vpMath::round( features[i].x + 5 ) );
- vpDisplay::displayText(I, ip, id.str(), color);
- }
-}
-
-/*!
- 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 vpKltOpencv::setMaxFeatures(const int maxCount)
-{
- m_maxCount = maxCount;
-}
-
-/*!
- 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 vpKltOpencv::setWindowSize(const int winSize)
-{
- m_winSize = winSize;
-}
-
-/*!
- 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 vpKltOpencv::setQuality(double qualityLevel)
-{
- m_qualityLevel = qualityLevel;
-}
-
-/*!
- 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 vpKltOpencv::setHarrisFreeParameter(double harris_k)
-{
- m_harris_k = harris_k;
-}
-
-/*!
- 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 vpKltOpencv::setUseHarris(const int useHarrisDetector)
-{
- m_useHarrisDetector = useHarrisDetector;
-}
-
-/*!
- 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 vpKltOpencv::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 vpKltOpencv::setMinEigThreshold(double minEigThreshold)
-{
- m_minEigThreshold = minEigThreshold;
-}
-
-/*!
- 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 vpKltOpencv::setBlockSize(const int blockSize)
-{
- m_blockSize = blockSize;
-}
-
-/*!
- 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 vpKltOpencv::setPyramidLevels(const int pyrMaxLevel)
-{
- m_pyrMaxLevel = pyrMaxLevel;
-}
-
-/*!
- 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
-vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
-{
- if(guess_pts.size() != m_points[1].size()){
- throw(vpException(vpException::badValue,
- "Cannot set initial guess: size feature vector [%d] and guess vector [%d] doesn't match",
- m_points[1].size(), guess_pts.size()));
- }
-
- m_points[1] = guess_pts;
- 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<cv::Point2f> &pts)
-{
- m_initial_guess = false;
- m_points[1] = pts;
- m_next_points_id = 0;
- m_points_id.clear();
- for(size_t i=0; i < m_points[1].size(); i++) {
- m_points_id.push_back(m_next_points_id ++);
- }
-
- I.copyTo(m_gray);
-}
-
-void
-vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
-{
- m_initial_guess = false;
- m_points[1] = pts;
- m_points_id.clear();
-
- if(ids.size() != pts.size()){
- m_next_points_id = 0;
- for(size_t i=0; i < m_points[1].size(); i++)
- m_points_id.push_back(m_next_points_id ++);
- }
- else{
- long max = 0;
- for(size_t i=0; i < m_points[1].size(); i++){
- m_points_id.push_back(ids[i]);
- if(ids[i] > max) max = ids[i];
- }
- m_next_points_id = max + 1;
- }
-
- 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);
- m_points[1].push_back(f);
- m_points_id.push_back(m_next_points_id++);
-}
-
-/*!
-
- \deprecated Add a keypoint at the end of the feature list.
- This function is deprecated since it 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);
- m_points[1].push_back(f);
- m_points_id.push_back(id);
- if (id >= m_next_points_id)
- 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()){
- throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
- }
-
- m_points[1].erase(m_points[1].begin()+index);
- m_points_id.erase(m_points_id.begin()+index);
-}
-
-
-#elif defined(VISP_HAVE_OPENCV)
-
-#include <string>
-
-#include <visp/vpKltOpencv.h>
-
-void vpKltOpencv::clean()
-{
- if (image) cvReleaseImage(&image);
- if (prev_image) cvReleaseImage(&prev_image);
- if (pyramid) cvReleaseImage(&pyramid);
- if (prev_pyramid) cvReleaseImage(&prev_pyramid);
-
- image = NULL;
- prev_image = NULL;
- pyramid = NULL;
- prev_pyramid = NULL;
-
- swap_temp = NULL;
- countFeatures = 0;
- countPrevFeatures = 0;
- flags = 0;
- initialized = 0;
- globalcountFeatures = 0;
-}
-
-void vpKltOpencv::cleanAll()
-{
- clean();
- if (features) cvFree(&features);
- if (prev_features) cvFree(&prev_features);
- if (status) cvFree(&status);
- if (lostDuringTrack) cvFree(&lostDuringTrack);
- if (featuresid) cvFree(&featuresid);
- if (prev_featuresid) cvFree(&prev_featuresid);
- features = NULL;
- prev_features = NULL;
- status = NULL;
- lostDuringTrack = 0;
- featuresid = NULL;
- prev_featuresid = NULL;
-}
-
-void vpKltOpencv::reset()
-{
- clean();
-
-}
-
-/*!
- Default constructor.
- */
-vpKltOpencv::vpKltOpencv()
- : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
- min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
- pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
- prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
- features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
- flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
- OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
-{
- features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(features[0]));
- prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(prev_features[0]));
- status = (char*)cvAlloc((size_t)maxFeatures);
- lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
- featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
- prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
-}
-
-/*!
- Copy constructor.
- */
-vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
- : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
- min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
- pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
- prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
- features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
- flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
- OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
-{
- *this = copy;
-}
-
-/*!
- Copy operator.
- */
-vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
-{
- //Shallow copy of primitives
- initialized = copy.initialized;
- maxFeatures = copy.maxFeatures;
- countFeatures = copy.countFeatures;
- countPrevFeatures = copy.countPrevFeatures;
- globalcountFeatures = copy.globalcountFeatures;
- flags = copy.flags;
- win_size = copy.win_size;
- quality = copy.quality;
- min_distance = copy.min_distance;
- harris_free_parameter = copy.harris_free_parameter;
- block_size = copy.block_size;
- use_harris = copy.use_harris;
- pyramid_level = copy.pyramid_level;
- _tid = copy._tid;
-
- OnInitialize = copy.OnInitialize;
- OnFeatureLost = copy.OnFeatureLost;
- OnNewFeature = copy.OnNewFeature;
- OnMeasureFeature = copy.OnMeasureFeature;
- IsFeatureValid = copy.IsFeatureValid;
-
- initial_guess = copy.initial_guess;
- lostDuringTrack = copy.lostDuringTrack;
-
- if (!initialized) {
- status = 0;
- lostDuringTrack = 0;
- countFeatures = 0;
- countPrevFeatures = 0;
- flags = 0;
- initialized = 0;
- globalcountFeatures = 0;
- }
-
- if (copy.image)
- {
- image = cvCreateImage(cvGetSize(copy.image), 8, 1);
- // /*IplImage **/cvCopyImage(copy.image,image);
- cvCopy(copy.image, image, 0);
- }
-
- if (copy.prev_image)
- {
- prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
- // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
- cvCopy(copy.prev_image, prev_image, 0);
- }
-
- if (copy.pyramid)
- {
- pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
- // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
- cvCopy(copy.pyramid, pyramid, 0);
- }
-
- if (copy.prev_pyramid)
- {
- prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
- // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
- cvCopy(copy.prev_pyramid, prev_pyramid, 0);
- }
-
- //Deep copy of arrays
- if (copy.features) {
- /*CvPoint2D32f **/features =
- (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
- for (int i = 0; i < copy.maxFeatures; i++)
- features[i] = copy.features[i];
- }
-
- if (copy.prev_features) {
- /*CvPoint2D32f **/prev_features =
- (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
- for (int i = 0; i < copy.maxFeatures; i++)
- prev_features[i] = copy.prev_features[i];
- }
-
- if (copy.featuresid) {
- /*long **/featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
- for (int i = 0; i < copy.maxFeatures; i++)
- featuresid[i] = copy.featuresid[i];
- }
-
- if (copy.prev_featuresid) {
- /*long **/prev_featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
- for (int i = 0; i < copy.maxFeatures; i++)
- prev_featuresid[i] = copy.prev_featuresid[i];
- }
-
- if (copy.status) {
- /*char **/status = (char*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(char));
- for (int i = 0; i < copy.maxFeatures; i++)
- status[i] = copy.status[i];
- }
-
- if (copy.lostDuringTrack) {
- /*bool **/lostDuringTrack = (bool*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(bool));
- for (int i = 0; i < copy.maxFeatures; i++)
- lostDuringTrack[i] = copy.lostDuringTrack[i];
- }
-
- return *this;
-}
-
-vpKltOpencv::~vpKltOpencv()
-{
- cleanAll();
-}
-
-/*!
- Set the maximum number of features to track in the image.
-
- \warning The tracker must be re-initialised using the method initTracking().
-
- \param input : The new number of maximum features.
-*/
-void vpKltOpencv::setMaxFeatures(const int input) {
- initialized = 0; maxFeatures=input;
-
- if (features) cvFree(&features);
- if (prev_features) cvFree(&prev_features);
- if (status) cvFree(&status);
- if (lostDuringTrack) cvFree(&lostDuringTrack);
- if (featuresid) cvFree(&featuresid);
- if (prev_featuresid) cvFree(&prev_featuresid);
-
-
- features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
- prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
- status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
- lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
- featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
- prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
-}
-
-/*!
- 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 IplImage *I, const IplImage *mask)
-{
- if (!I) {
- throw(vpException(vpTrackingException::initializationError, "Image Not initialized")) ;
- }
-
- if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
- throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
- }
-
- if (mask) {
- if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
- throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
- }
- }
-
- //Creation des buffers
- CvSize Sizeim, SizeI;
- SizeI = cvGetSize(I);
- bool b_imOK = true;
- if(image != NULL){
- Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
- }
- if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
- reset();
- image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
- prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- }else{
- swap_temp = 0;
- countFeatures = 0;
- countPrevFeatures = 0;
- flags = 0;
- initialized = 0;
- globalcountFeatures = 0;
- }
-
- initialized = 1;
-
- //Import
- cvCopy(I, image, 0);
-
- //Recherche de points d'interets
- countFeatures = maxFeatures;
- countPrevFeatures = 0;
- IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
- IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
- cvGoodFeaturesToTrack(image, eig, temp, features,
- &countFeatures, quality, min_distance,
- mask, block_size, use_harris, harris_free_parameter);
- cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
- cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
- CV_TERMCRIT_EPS,20,0.03));
- cvReleaseImage(&eig);
- cvReleaseImage(&temp);
-
- if (OnInitialize)
- OnInitialize(_tid);
-
- //printf("Number of features at init: %d\n", countFeatures);
- for (int boucle=0; boucle<countFeatures;boucle++) {
- featuresid[boucle] = globalcountFeatures;
- globalcountFeatures++;
-
- if (OnNewFeature){
- OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
- features[boucle].y);
- }
- }
-}
-
-/*!
- 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 IplImage *I, CvPoint2D32f *pts, int size)
-{
- if (size > maxFeatures)
- throw(vpException(vpTrackingException::initializationError,
- "Cannot initialize tracker from points"));
-
- //Creation des buffers
- CvSize Sizeim, SizeI;
- SizeI = cvGetSize(I);
- bool b_imOK = true;
- if(image != NULL){
- Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
- }
- if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
- reset();
- image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
- prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- } else {
- flags = 0;
- }
- // Save current features as previous features
- countFeatures = size;
- for (int i=0; i<countFeatures;i++) {
- features[i] = pts[i];
- featuresid[i] = i;
- }
-
- globalcountFeatures = size;
- initialized = 1;
-
- cvCopy(I, image, 0);
-}
-
-void
-vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
-{
- if (size > maxFeatures)
- throw(vpException(vpTrackingException::initializationError,
- "Cannot initialize tracker from points"));
-
- //Creation des buffers
- CvSize Sizeim, SizeI;
- SizeI = cvGetSize(I);
- bool b_imOK = true;
- if(image != NULL){
- Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
- }
- if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
- reset();
- image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
- prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- } else {
- flags = 0;
- }
- // Save current features as previous features
- countFeatures = size;
- long max = 0;
- for (int i=0; i<countFeatures;i++) {
- features[i] = pts[i];
- featuresid[i] = fid[i];
- if (fid[i] > max)
- max = fid[i];
- }
-
- globalcountFeatures = max + 1;
- initialized = 1;
-
- cvCopy(I, image, 0);
-}
-
-void vpKltOpencv::track(const IplImage *I)
-{
- if (!initialized) {
- vpERROR_TRACE("KLT Not initialized") ;
- throw(vpException(vpTrackingException::initializationError,
- "KLT Not initialized")) ;
- }
-
- if (!I) {
- throw(vpException(vpTrackingException::initializationError,
- "Image Not initialized")) ;
- }
-
- if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
- throw(vpException(vpTrackingException::initializationError,
- "Bad Image format")) ;
- }
-
-
-
- CV_SWAP(prev_image, image, swap_temp);
- CV_SWAP(prev_pyramid, pyramid, swap_temp);
-
- cvCopy(I, image, 0);
-
- if(!initial_guess){
- // Save current features as previous features
- countPrevFeatures = countFeatures;
- for (int boucle=0; boucle<countFeatures;boucle++) {
- prev_featuresid[boucle] = featuresid[boucle];
- }
-
- CvPoint2D32f *swap_features = 0;
- CV_SWAP(prev_features, features, swap_features);
- }
-
- if (countFeatures <= 0) return;
-
- cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
- prev_features, features, countFeatures,
- cvSize(win_size, win_size), pyramid_level,
- status, 0, cvTermCriteria(CV_TERMCRIT_ITER
- |CV_TERMCRIT_EPS,20,0.03),
- flags );
-
- if(!initial_guess)
- flags |= CV_LKFLOW_PYR_A_READY;
- else{
- flags = CV_LKFLOW_PYR_A_READY;
- initial_guess = false;
- }
-
- int i,k;
- for (i = k = 0; i < countFeatures ; i++) {
- if (!status[i]) {
- lostDuringTrack[i] = 1;
- if (OnFeatureLost)
- OnFeatureLost(_tid, i, featuresid[i], features[i].x,
- features[i].y);
- continue;
- }
-
- if (IsFeatureValid) {
- if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
- lostDuringTrack[i] = 1;
- if (OnFeatureLost)
- OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
- continue;
- }
- }
- features[k] = features[i];
- featuresid[k] = featuresid[i];
-
- if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
-
- lostDuringTrack[i] = 0;
- k++;
- }
- countFeatures = k;
-}
-
-/*!
- 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<unsigned char> &I,
- vpColor color, unsigned int thickness)
-{
- if ((features == 0) || (I.bitmap==0) || (!initialized))
- {
- vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
- }
-
- vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
-}
-
-/*!
-
- 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(int index, int &id, float &x, float &y) const
-{
- if (index >= countFeatures)
- {
- vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
- }
-
- x = features[index].x;
- y = features[index].y;
- id = featuresid[index];
-}
-
-
-/*!
- Set the points that will be used as initial guess during the next call to track().
-
- \warning Those points will be used just one time (next track()).
-
- \param guess_pts : Reference on an array of CvPoint2D32f allocated with cvAlloc().
-*/
-void
-vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
-{
- // Save current features as previous features
- countPrevFeatures = countFeatures;
- for (int boucle=0; boucle<countFeatures;boucle++) {
- prev_featuresid[boucle] = featuresid[boucle];
- }
-
- CvPoint2D32f *swap_features = NULL;
- CV_SWAP(prev_features, *guess_pts, swap_features);
-
- CV_SWAP(features, prev_features, swap_features);
-
- flags |= CV_LKFLOW_INITIAL_GUESSES;
-
- initial_guess = true;
-}
-
-/*!
-
- Get the 'index'th previous feature image coordinates. Beware that
- getPrevFeature(i,...) may not represent the same feature before and
- after a tracking iteration (if a feature is lost, features are
- shifted in the array).
-
-*/
-void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
-{
- if (index >= countPrevFeatures)
- {
- vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
- }
-
- x = prev_features[index].x;
- y = prev_features[index].y;
- id = prev_featuresid[index];
-}
-
-/*!
-
-Add at the end of the feauture list.
-
-If there is no space left, the feature is not added (just return)
-*/
-void vpKltOpencv::addFeature(const int &id,
- const float &x, const float &y)
-{
- if (maxFeatures == countFeatures)
- {
- vpERROR_TRACE(" Cannot add the feature ");
- return;
- }
-
- CvPoint2D32f f;
- f.x = x;
- f.y = y;
- features[countFeatures] = f;
- featuresid[countFeatures] = id;
- countFeatures ++;
-}
-
-void vpKltOpencv::suppressFeature(int index)
-{
- if (index >= countFeatures)
- {
- vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
- }
-
- countFeatures --;
-
- for (int i=index ; i < countFeatures; i ++) {
- features[i] = features[i+1];
- featuresid[i] = featuresid[i+1];
- }
-}
-
-/*!
-
- Display features list.
-
- \param I : The image used as background.
-
- \param features_list : List of features
-
- \param nbFeatures : Number of features
-
- \param color : Color used to display the points.
-
- \param thickness : Thickness of the points.
-*/
-void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
- }
-}
-/*!
-
- Display features list.
-
- \param I : The image used as background.
-
- \param features_list : List of features
-
- \param nbFeatures : Number of features
-
- \param color : Color used to display the points.
-
- \param thickness : Thickness of the points.
-*/
-void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
- }
-}
-
-/*!
-
- Display features list with ids.
-
- \param I : The image used as background.
-
- \param features_list : List of features
-
- \param featuresid_list : List of ids corresponding to the features list
-
- \param nbFeatures : Number of features
-
- \param color : Color used to display the points.
-
- \param thickness : Thickness of the points
-*/
-void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
-
- char id[10];
- sprintf(id, "%ld", featuresid_list[i]);
- ip.set_u( vpMath::round( features_list[i].x + 5 ) );
- vpDisplay::displayText(I, ip, id, color);
- }
-}
-
-/*!
-
- Display features list with ids.
-
- \param I : The image used as background.
-
- \param features_list : List of features
-
- \param featuresid_list : List of ids corresponding to the features list
-
- \param nbFeatures : Number of features
-
- \param color : Color used to display the points.
-
- \param thickness : Thickness of the points
-*/
-void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color, unsigned int thickness)
-{
- vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10, color, thickness) ;
-
- char id[10];
- sprintf(id, "%ld", featuresid_list[i]);
- ip.set_u( vpMath::round( features_list[i].x + 5 ) );
- vpDisplay::displayText(I, ip, id, color);
- }
-}
-
-#endif
diff --git a/src/tracking/klt/vpKltOpencv.h b/src/tracking/klt/vpKltOpencv.h
deleted file mode 100644
index 12fc283..0000000
--- a/src/tracking/klt/vpKltOpencv.h
+++ /dev/null
@@ -1,417 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpKltOpencv.h 5148 2015-01-12 14:31:30Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
- * with opencv.
- *
- * Authors:
- * Fabien Servant
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpKltOpencv.h
-
- \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
- implemented with opencv.
-*/
-
-#ifndef vpKltOpencv_h
-#define vpKltOpencv_h
-
-#include <visp/vpConfig.h>
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408))
-
-#include <opencv2/video/tracking.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/highgui/highgui.hpp>
-
-/*!
- \class vpKltOpencv
-
- \ingroup TrackingImagePoint
-
- \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
- implemented in 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:
- vpKltOpencv();
- vpKltOpencv(const vpKltOpencv& copy);
- virtual ~vpKltOpencv();
-
- void addFeature(const float &x, const float &y);
- void addFeature(const long &id, const float &x, const float &y);
- void addFeature(const cv::Point2f &f);
-
- void display(const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::red, unsigned int thickness=1);
- static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &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;}
- void getFeature(const int &index, int &id, float &x, float &y) const;
- //! Get the list of current features.
- std::vector<cv::Point2f> getFeatures() const {return m_points[1];}
- //CvPoint2D32f* getFeatures() const {return features;}
- //! Get the unique id of each feature.
- std::vector<long> getFeaturesId() const {return m_points_id;}
- //long* getFeaturesId() const {return featuresid;}
- //! Get the free parameter of the Harris detector.
- double getHarrisFreeParameter() const {return m_harris_k;}
- //! Get the list of lost feature
- //bool *getListOfLostFeature() const { return lostDuringTrack; }
- //! Get the maximum number of features to track in the image.
- int getMaxFeatures() const {return m_maxCount;}
- //! Get the minimal Euclidean distance between detected corners during initialization.
- double getMinDistance() const {return m_minDistance;}
- //! Get the number of current features
- int getNbFeatures() const { return (int)m_points[1].size(); }
- //! Get the number of previous features.
- int getNbPrevFeatures() const { return (int)m_points[0].size(); }
- //void getPrevFeature(int index, int &id, float &x, float &y) const;
- //! Get the list of previous features
- std::vector<cv::Point2f> getPrevFeatures() const {return m_points[0];}
- //CvPoint2D32f* getPrevFeatures() const {return prev_features;}
- //! Get the list of features id
- //long* getPrevFeaturesId() const {return prev_featuresid;}
- //! Get the maximal pyramid level.
- int getPyramidLevels() const {return m_pyrMaxLevel;}
- //! Get the parameter characterizing the minimal accepted quality of image corners.
- double getQuality() const {return m_qualityLevel;}
- //! Get the window size used to refine the corner locations.
- int getWindowSize() const {return m_winSize;}
-
- void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat());
- void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts);
- void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids);
-
- vpKltOpencv & operator=(const vpKltOpencv& copy);
- void track(const cv::Mat &I);
- void setBlockSize(const int blockSize);
- void setHarrisFreeParameter(double harris_k);
- void setInitialGuess(const std::vector<cv::Point2f> &guess_pts);
- void setMaxFeatures(const int maxCount);
- void setMinDistance(double minDistance);
- void setMinEigThreshold(double minEigThreshold);
- void setPyramidLevels(const int pyrMaxLevel);
- void setQuality(double 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(const int useHarrisDetector);
- void setWindowSize(const int winSize);
- void suppressFeature(const int &index);
-
-protected:
- cv::Mat m_gray, m_prevGray;
- std::vector<cv::Point2f> m_points[2]; //!< Previous [0] and current [1] keypoint location
- std::vector<long> 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;
-
-};
-
-#elif defined(VISP_HAVE_OPENCV)
-#ifdef _CH_
-#pragma package <opencv>
-#endif
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/video/tracking.hpp>
-# include <opencv2/legacy/legacy.hpp>
-# include <opencv2/highgui/highgui.hpp>
-#else
-# ifndef _EiC
-# include <cv.h>
-# include <highgui.h>
-# include <stdio.h>
-# include <ctype.h>
-# endif
-#endif
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDebug.h>
-#include <visp/vpException.h>
-#include <visp/vpTrackingException.h>
-
-typedef int (*funccheck)(int,double,double);
-typedef void (*funcinfo)(int,int,int,double,double);
-typedef void (*funcevent)(int);
-
-/*!
- \class vpKltOpencv
-
- \ingroup TrackingImagePoint
-
- \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
- implemented in 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
-{
-private:
- int initialized; //Is the tracker ready ?
-
- int maxFeatures; //Maximum number of features to track (Default 50)
- int globalcountFeatures; //Keep over time for ID
-
-
-
- int win_size; //Size of search window for tracker (default 10)
- double quality; //Multiplier for the maxmin eigenvalue; specifies minimal accepted quality of image corners (default 0.01)
- double min_distance; //Limit, specifying minimum possible distance between returned corners; Euclidian distance is used. (default 10)
- double harris_free_parameter; //Harris detector free parameter. (default 0.04)
- int block_size; //Size of the averaging block used by the corner detector (default 3)
- int use_harris; //0 use a simple Minimum EigenValue Detector, != 0 use Harris (default 1)
- int pyramid_level; //Number of level for the tracker's gaussian pyramid data (default 3)
- int _tid; //tracker id for multiple trackers
-
- IplImage *image; //Image buffer
- IplImage *prev_image; //Image buffer for the previous iteration
- IplImage *pyramid; //Gaussian pyramid data
- IplImage *prev_pyramid; //Gaussian pyramid data for the previous iteration
- IplImage *swap_temp; //Internal
-
- int countFeatures; //Currently tracked features
- int countPrevFeatures; //Previously tracked features
-
- CvPoint2D32f *features; //List of features
- CvPoint2D32f *prev_features; //List of features for previous iteration
-
- long *featuresid; //Array of ids for features
- long *prev_featuresid; //Array of ids for previous features
-
- int flags; //Flags for tracking (internal)
-
- bool initial_guess; //Bool to precise if the next call to track() uses an initial guess
-
- bool *lostDuringTrack; // Result of the tracker for every feature : 1 = lost, 0 = still present
- char *status; //Result of the tracker for every features : 0 = lost, 1 = found
-
-
-
- //EVENT FUNCTION POINTERS
- funcevent OnInitialize;
- funcinfo OnFeatureLost;
- funcinfo OnNewFeature;
- funcinfo OnMeasureFeature;
- funccheck IsFeatureValid;
-
-private:
-
- //Internal
- void clean();
- void cleanAll();
- void reset();
-
-public:
- vpKltOpencv();
- vpKltOpencv(const vpKltOpencv& copy);
- virtual ~vpKltOpencv();
-
- void addFeature(const int &id, const float &x, const float &y);
-
- //Draw the tracked features on the given image
- void display(const vpImage<unsigned char> &I,
- vpColor color = vpColor::red, unsigned int thickness=1);
-
- //! Get the block size
- int getBlockSize() const {return block_size;}
- void getFeature(int index, int &id, float &x, float &y) const;
- //! Get the list of features
- CvPoint2D32f* getFeatures() const {return features;}
- //! Get the list of features id
- long* getFeaturesId() const {return featuresid;}
- //! Get Harris free parameter
- double getHarrisFreeParameter() const {return harris_free_parameter;}
- //! Get the list of lost feature
- bool *getListOfLostFeature() const { return lostDuringTrack; }
- //! Get Max number of features
- int getMaxFeatures() const {return maxFeatures;}
- //! Get Min Distance
- double getMinDistance() const {return min_distance;}
- //! Get the current number of features
- int getNbFeatures() const { return countFeatures; }
- //! Get the previous number of features
- int getNbPrevFeatures() const { return countPrevFeatures; }
- void getPrevFeature(int index, int &id, float &x, float &y) const;
- //! Get the list of features
- CvPoint2D32f* getPrevFeatures() const {return prev_features;}
- //! Get the list of features id
- long* getPrevFeaturesId() const {return prev_featuresid;}
- //! Get the number of pyramid levels
- int getPyramidLevels() const {return pyramid_level;}
- //! Get the quality of the tracker
- double getQuality() const {return quality;}
- //! Get Max number of features
- int getWindowSize() const {return win_size;}
-
- //Detect corners in the image. Initialize the tracker
- void initTracking(const IplImage *I, const IplImage *mask = NULL);
- void initTracking(const IplImage *I, CvPoint2D32f *pts, int size);
- void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size);
- vpKltOpencv & operator=(const vpKltOpencv& copy);
- //Track !
- void track(const IplImage *I);
-
-
- //Seters
- /*!
- 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 input : The new size of the block.
- */
- void setBlockSize(const int input) {initialized = 0; block_size=input;}
- /*!
- Set the Harris parameter (The \e k value).
-
- \warning The tracker must be re-initialised using the method initTracking().
-
- \param input : The new Harris parameter.
- */
- void setHarrisFreeParameter(double input) {initialized = 0; harris_free_parameter=input;}
- void setInitialGuess(CvPoint2D32f **guess_pts);
- /*!
- Is a feature valid (e.g. : test if not too close to borders) -> event(id_tracker, x, y)
- */
- void setIsFeatureValid(funccheck input) {IsFeatureValid = input;}
-
- /* Should be used only before initTracking */
- void setMaxFeatures(const int input);
- /*!
- Set the minimal distance between two points during the initialisation.
-
- \warning The tracker must be re-initialised using the method initTracking().
-
- \param input : The new minimal distance between two points.
- */
- void setMinDistance(double input) {initialized = 0; min_distance=input;}
-
- //Functors
-
- //Event when tracker is initialized -> event(id_tracker)
- void setOnInitialize(funcevent input) {OnInitialize = input;}
- //Event when a feature is lost -> event(id_tracker, index, uid, x, y)
- void setOnFeatureLost(funcinfo input) {OnFeatureLost = input;}
- //Event when a new feature is found -> event(id_tracker, index, uid, x, y)
- void setOnNewFeature(funcinfo input) {OnNewFeature = input;}
- //Event when a feature is found while tracking -> event(id_tracker, index, uid, x, y)
- void setOnMeasureFeature(funcinfo input) {OnMeasureFeature = input;}
- /*!
- Set the maximal pyramid level. If the level is zero, then no pyramid is
- computed for the optical flow.
-
- \warning The tracker must be re-initialised using the method initTracking().
-
- \param input : The new maximal pyramid level.
- */
- void setPyramidLevels(const int input) {initialized = 0; pyramid_level=input;}
- void setQuality(double input) {initialized = 0; quality=input;}
- void setTrackerId(int tid) {_tid = tid;}
- /*!
- Set the window size for the sub-pixel computation.
-
- \warning The tracker must be re-initialised using the method initTracking().
-
- \param input : The new number of maximum features.
- */
- void setUseHarris(const int input) {initialized = 0; use_harris=input;}
- void setWindowSize(const int input) {initialized = 0; win_size=input;}
-
- void suppressFeature(int index);
-
- //Static Functions
-public:
- static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color = vpColor::green,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color = vpColor::green,
- unsigned int thickness=1);
-
- static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color = vpColor::green, unsigned int thickness=1);
-
-};
-
-#endif
-#endif
diff --git a/src/tracking/mbt/edge/vpMbEdgeTracker.cpp b/src/tracking/mbt/edge/vpMbEdgeTracker.cpp
deleted file mode 100644
index 259a0c1..0000000
--- a/src/tracking/mbt/edge/vpMbEdgeTracker.cpp
+++ /dev/null
@@ -1,2619 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbEdgeTracker.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbEdgeTracker.cpp
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpPose.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpRobust.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpMath.h>
-#include <visp/vpException.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpMbtDistanceLine.h>
-#include <visp/vpMbtXmlParser.h>
-#include <visp/vpMbtPolygon.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-#include <limits>
-#include <string>
-#include <sstream>
-#include <float.h>
-#include <map>
-
-bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold);
-
-/*!
- Basic constructor
-*/
-vpMbEdgeTracker::vpMbEdgeTracker()
- : compute_interaction(1), lambda(1), me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0),
- nbvisiblepolygone(0), percentageGdPt(0.4), scales(1),
- Ipyramid(0), scaleLevel(0)
-{
- angleAppears = vpMath::rad(89);
- angleDisappears = vpMath::rad(89);
-
- scales[0] = true;
-
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Edge");
-#endif
-}
-
-/*!
- Basic destructor useful to deallocate the memory.
-*/
-vpMbEdgeTracker::~vpMbEdgeTracker()
-{
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy;
- vpMbtDistanceCircle *ci;
-
- for (unsigned int i = 0; i < lines.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- if (l!=NULL){
- delete l ;
- }
- l = NULL ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- if (cy!=NULL){
- delete cy ;
- }
- cy = NULL ;
- }
-
- lines[i].clear();
- cylinders[i].clear();
- }
- }
- for (unsigned int i = 0; i < circles.size(); i += 1){
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
- }
- ci = NULL ;
- }
- circles[i].clear();
- }
- cleanPyramid(Ipyramid);
-}
-
-/*!
- Set the moving edge parameters.
-
- \param p_me : an instance of vpMe containing all the desired parameters
-*/
-void
-vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
-{
- this->me = p_me;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- (*it)->setMovingEdge(&(this->me)) ;
- }
-
- vpMbtDistanceCylinder *cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- cy->setMovingEdge(&(this->me)) ;
- }
-
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- ci->setMovingEdge(&(this->me)) ;
- }
- }
- }
-}
-
-/*!
- Compute the visual servoing loop to get the pose of the feature set.
-
- \exception vpTrackingException::notEnoughPointError if the number of detected
- feature is equal to zero.
-
- \param _I : The current image.
- */
-void
-vpMbEdgeTracker::computeVVS(const vpImage<unsigned char>& _I)
-{
- double residu_1 =1e3;
- double r =1e3-1;
- vpMatrix LTL;
- vpColVector LTR;
-
- // compute the interaction matrix and its pseudo inverse
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
-
- //vpColVector w;
- vpColVector factor;
- //vpColVector error; // s-s*
- vpColVector weighted_error; // Weighted error vector wi(s-s)*
- vpVelocityTwistMatrix cVo;
-
- unsigned int iter = 0;
-
- //Nombre de moving edges
- unsigned int nbrow = 0;
- unsigned int nberrors_lines = 0;
- unsigned int nberrors_cylinders = 0;
- unsigned int nberrors_circles = 0;
-
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- nbrow += l->nbFeature;
- nberrors_lines+=l->nbFeature;
- l->initInteractionMatrixError();
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- nbrow += cy->nbFeature ;
- nberrors_cylinders += cy->nbFeature ;
- cy->initInteractionMatrixError() ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- nbrow += ci->nbFeature ;
- nberrors_circles += ci->nbFeature ;
- ci->initInteractionMatrixError() ;
- }
-
- if (nbrow==0){
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
- }
-
- vpMatrix L(nbrow,6), Lp;
-
- // compute the error vector
- m_error.resize(nbrow);
- unsigned int nerror = m_error.getRows();
- vpColVector v ;
-
- double limite = 3; //Une limite de 3 pixels
- limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
-
- //Parametre pour la premiere phase d'asservissement
- double e_prev = 0, e_cur, e_next;
- bool reloop = true;
- double count = 0;
-
- bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
-
- /*** First phase ***/
-
- while ( reloop == true && iter<10)
- {
- if(iter==0)
- {
- weighted_error.resize(nerror) ;
- m_w.resize(nerror);
- m_w = 0;
- factor.resize(nerror);
- factor = 1;
- }
-
- count = 0;
-
- unsigned int n = 0;
- reloop = false;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- l->computeInteractionMatrixError(cMo);
-
- double fac = 1;
- if (iter == 0)
- {
- for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
- int index = *itindex;
- if (l->hiddenface->isAppearing((unsigned int)index))
- {
- fac = 0.2;
- break;
- }
- if(l->closeToImageBorder(_I, 10))
- {
- fac = 0.1;
- break;
- }
- }
- }
-
- std::list<vpMeSite>::const_iterator itListLine;
- if (iter == 0 && l->meline != NULL)
- itListLine = l->meline->getMeList().begin();
-
- for (unsigned int i=0 ; i < l->nbFeature ; i++)
- {
- for (unsigned int j=0; j < 6 ; j++)
- {
- L[n+i][j] = l->L[i][j]; //On remplit la matrice d'interaction globale
- }
- m_error[n+i] = l->error[i]; //On remplit la matrice d'erreur
-
- if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
-
- m_w[n+i] = 0;
-
- if (iter == 0)
- {
- factor[n+i] = fac;
- vpMeSite site = *itListLine;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itListLine;
- }
-
- //If pour la premiere extremite des moving edges
- if (i == 0)
- {
- e_cur = l->error[0];
- if (l->nbFeature > 1)
- {
- e_next = l->error[1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- m_w[n+i] = 1/*0.5*/;
- }
- e_prev = e_cur;
- }
- else m_w[n+i] = 1;
- }
-
- //If pour la derniere extremite des moving edges
- else if(i == l->nbFeature-1)
- {
- e_cur = l->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- m_w[n+i] += 1/*0.5*/;
- }
- }
-
- else
- {
- e_cur = l->error[i];
- e_next = l->error[i+1];
- if ( fabs(e_cur - e_prev) < limite )
- {
- m_w[n+i] += 0.5;
- }
- if ( fabs(e_cur - e_next) < limite )
- {
- m_w[n+i] += 0.5;
- }
- e_prev = e_cur;
- }
- }
-
- n+= l->nbFeature ;
- }
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- cy->computeInteractionMatrixError(cMo, _I);
- double fac = 1.0;
-
- std::list<vpMeSite>::const_iterator itCyl1;
- std::list<vpMeSite>::const_iterator itCyl2;
- if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
- itCyl1 = cy->meline1->getMeList().begin();
- itCyl2 = cy->meline2->getMeList().begin();
- }
-
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
- }
- m_error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
-
- if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
-
- m_w[n+i] = 0;
-
- if (iter == 0)
- {
- factor[n+i] = fac;
- vpMeSite site;
- if(i<cy->nbFeaturel1) {
- site= *itCyl1;
- ++itCyl1;
- }
- else{
- site= *itCyl2;
- ++itCyl2;
- }
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- }
-
- //If pour la premiere extremite des moving edges
- if (i == 0)
- {
- e_cur = cy->error[0];
- if (cy->nbFeature > 1)
- {
- e_next = cy->error[1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- m_w[n+i] = 1/*0.5*/;
- }
- e_prev = e_cur;
- }
- else m_w[n+i] = 1;
- }
- if (i == cy->nbFeaturel1)
- {
- e_cur = cy->error[i];
- if (cy->nbFeaturel2 > 1)
- {
- e_next = cy->error[i+1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- m_w[n+i] = 1/*0.5*/;
- }
- e_prev = e_cur;
- }
- else m_w[n+i] = 1;
- }
-
- //If pour la derniere extremite des moving edges
- else if(i == cy->nbFeaturel1-1)
- {
- e_cur = cy->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- m_w[n+i] += 1/*0.5*/;
- }
- }
- //If pour la derniere extremite des moving edges
- else if(i == cy->nbFeature-1)
- {
- e_cur = cy->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- m_w[n+i] += 1/*0.5*/;
- }
- }
-
- else
- {
- e_cur = cy->error[i];
- e_next = cy->error[i+1];
- if ( fabs(e_cur - e_prev) < limite ){
- m_w[n+i] += 0.5;
- }
- if ( fabs(e_cur - e_next) < limite ){
- m_w[n+i] += 0.5;
- }
- e_prev = e_cur;
- }
- }
-
- n+= cy->nbFeature ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->computeInteractionMatrixError(cMo);
- double fac = 1.0;
-
- std::list<vpMeSite>::const_iterator itCir;
- if (iter == 0 && (ci->meEllipse != NULL)) {
- itCir = ci->meEllipse->getMeList().begin();
- }
-
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j]; //On remplit la matrice d'interaction globale
- }
- m_error[n+i] = ci->error[i]; //On remplit la matrice d'erreur
-
- if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
-
- m_w[n+i] = 0;
-
- if (iter == 0)
- {
- factor[n+i] = fac;
- vpMeSite site = *itCir;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itCir;
- }
-
- //If pour la premiere extremite des moving edges
- if (i == 0)
- {
- e_cur = ci->error[0];
- if (ci->nbFeature > 1)
- {
- e_next = ci->error[1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- m_w[n+i] = 1/*0.5*/;
- }
- e_prev = e_cur;
- }
- else m_w[n+i] = 1;
- }
-
- //If pour la derniere extremite des moving edges
- else if(i == ci->nbFeature-1)
- {
- e_cur = ci->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- m_w[n+i] += 1/*0.5*/;
- }
- }
-
- else
- {
- e_cur = ci->error[i];
- e_next = ci->error[i+1];
- if ( fabs(e_cur - e_prev) < limite ){
- m_w[n+i] += 0.5;
- }
- if ( fabs(e_cur - e_next) < limite ){
- m_w[n+i] += 0.5;
- }
- e_prev = e_cur;
- }
- }
-
- n+= ci->nbFeature ;
- }
-
- count = count / (double)nbrow;
- if (count < 0.85){
- reloop = true;
- }
-
- double num=0;
- double den=0;
-
- double wi ; double eri ;
- for(unsigned int i = 0; i < nerror; i++){
- wi = m_w[i]*factor[i];
- eri = m_error[i];
- num += wi*vpMath::sqr(eri);
- den += wi ;
-
- weighted_error[i] = wi*eri ;
- }
-
- if((iter==0) || compute_interaction){
- for (unsigned int i=0 ; i < nerror ; i++){
- for (unsigned int j=0 ; j < 6 ; j++){
- L[i][j] = m_w[i]*factor[i]*L[i][j] ;
- }
- }
- }
-
- // If all the 6 dof should be estimated, we check if the interaction matrix is full rank.
- // If not we remove automatically the dof that cannot be estimated
- // This is particularly useful when consering circles (rank 5) and cylinders (rank 4)
- if (isoJoIdentity_) {
- cVo.buildFrom(cMo);
-
- vpMatrix K; // kernel
- unsigned int rank = (L*cVo).kernel(K);
- if(rank == 0) {
- throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
- }
- if (rank != 6) {
- vpMatrix I; // Identity
- I.eye(6);
- oJo = I-K.AtA();
-
- isoJoIdentity_ = false;
- }
- }
-
- if(isoJoIdentity_){
- LTL = L.AtA();
- computeJTR(L, weighted_error, LTR);
- v = -0.7*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
- }
- else{
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpMatrix LVJTR;
- computeJTR(LVJ, weighted_error, LVJTR);
- v = -0.7*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- }
-
- cMo = vpExponentialMap::direct(v).inverse() * cMo;
-
- iter++;
- }
-
-// std::cout << "\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl ;
-
-
-/*** Second phase ***/
-
- vpRobust robust_lines(nberrors_lines);
- vpRobust robust_cylinders(nberrors_cylinders);
- vpRobust robust_circles(nberrors_circles);
- robust_lines.setIteration(0) ;
- robust_cylinders.setIteration(0) ;
- robust_circles.setIteration(0) ;
- iter = 0;
- vpColVector w_lines(nberrors_lines);
- vpColVector w_cylinders(nberrors_cylinders);
- vpColVector w_circles(nberrors_circles);
- vpColVector error_lines(nberrors_lines);
- vpColVector error_cylinders(nberrors_cylinders);
- vpColVector error_circles(nberrors_circles);
-
- vpHomogeneousMatrix cMoPrev;
- vpColVector W_true;
- vpMatrix L_true;
- vpMatrix LVJ_true;
-
- while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
- {
- unsigned int n = 0 ;
- unsigned int nlines = 0;
- unsigned int ncylinders = 0;
- unsigned int ncircles = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- l->computeInteractionMatrixError(cMo) ;
- for (unsigned int i=0 ; i < l->nbFeature ; i++){
- for (unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = l->L[i][j];
- m_error[n+i] = l->error[i];
- error_lines[nlines+i] = m_error[n+i];
- }
- }
- n+= l->nbFeature;
- nlines+= l->nbFeature;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- cy->computeInteractionMatrixError(cMo, _I) ;
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j];
- m_error[n+i] = cy->error[i];
- error_cylinders[ncylinders+i] = m_error[n+i];
- }
- }
-
- n+= cy->nbFeature ;
- ncylinders+= cy->nbFeature ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->computeInteractionMatrixError(cMo) ;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j];
- m_error[n+i] = ci->error[i];
- error_circles[ncircles+i] = m_error[n+i];
- }
- }
-
- n+= ci->nbFeature ;
- ncircles+= ci->nbFeature ;
- }
-
- if(iter==0)
- {
- weighted_error.resize(nerror);
- m_w.resize(nerror);
- m_w = 1;
- w_lines.resize(nberrors_lines);
- w_lines = 1;
- w_cylinders.resize(nberrors_cylinders);
- w_cylinders = 1;
- w_circles.resize(nberrors_circles);
- w_circles = 1;
-
- robust_lines.setThreshold(2/cam.get_px());
- robust_cylinders.setThreshold(2/cam.get_px());
- robust_circles.setThreshold(vpMath::sqr(2/cam.get_px()));
- if(nberrors_lines > 0)
- robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
- if(nberrors_cylinders > 0)
- robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
- if(nberrors_circles > 0)
- robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
- }
- else
- {
- robust_lines.setIteration(iter);
- robust_cylinders.setIteration(iter);
- robust_circles.setIteration(iter);
- if(nberrors_lines > 0)
- robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
- if(nberrors_cylinders > 0)
- robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
- if(nberrors_circles > 0)
- robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
- }
-
- unsigned int cpt = 0;
- while(cpt<nbrow){
- if(cpt<nberrors_lines){
- m_w[cpt] = w_lines[cpt];
- }
- else if (cpt<nberrors_lines+nberrors_cylinders){
- m_w[cpt] = w_cylinders[cpt-nberrors_lines];
- }
- else {
- m_w[cpt] = w_circles[cpt-nberrors_lines-nberrors_cylinders];
- }
- cpt++;
- }
-
- residu_1 = r;
-
- double num=0;
- double den=0;
- double wi;
- double eri;
-
- L_true = L;
- W_true = vpColVector(nerror);
-
- if(computeCovariance){
- L_true = L;
- if(!isoJoIdentity_){
- cVo.buildFrom(cMo);
- LVJ_true = (L*cVo*oJo);
- }
- }
-
- for(unsigned int i=0; i<nerror; i++){
- wi = m_w[i]*factor[i];
- W_true[i] = wi;
- eri = m_error[i];
- num += wi*vpMath::sqr(eri);
- den += wi;
-
- weighted_error[i] = wi*eri ;
- }
-
- r = sqrt(num/den); //Le critere d'arret prend en compte le poids
-
- if((iter==0)|| compute_interaction){
- for (unsigned int i=0 ; i < nerror ; i++){
- for (unsigned int j=0 ; j < 6 ; j++){
- L[i][j] = m_w[i]*factor[i]*L[i][j];
- }
- }
- }
-
- if(isoJoIdentity_){
- LTL = L.AtA();
- computeJTR(L, weighted_error, LTR);
- v = -lambda*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
- }
- else{
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpMatrix LVJTR;
- computeJTR(LVJ, weighted_error, LVJTR);
- v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- }
-
- cMoPrev = cMo;
- cMo = vpExponentialMap::direct(v).inverse() * cMo;
-
- iter++;
- }
-
- // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
- if(computeCovariance){
- vpMatrix D;
- D.diag(W_true);
-
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity_){
- computeCovarianceMatrix(cMoPrev,m_error,L_true,D);
- }
- else{
- computeCovarianceMatrix(cMoPrev,m_error,LVJ_true,D);
- }
- }
-
- unsigned int n =0 ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- {
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListLine;
- if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
-
- for (unsigned int i=0 ; i < l->nbFeature ; i++){
- wmean += m_w[n+i] ;
- vpMeSite p = *itListLine;
- if (m_w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListLine = p;
- }
-
- ++itListLine;
- }
- n+= l->nbFeature ;
-
- if (l->nbFeature!=0)
- wmean /= l->nbFeature ;
- else
- wmean = 1;
-
- l->setMeanWeight(wmean);
-
- if (wmean < 0.8)
- l->Reinit = true;
- }
- }
-
- // Same thing with cylinders as with lines
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListCyl1;
- std::list<vpMeSite>::iterator itListCyl2;
-
- if (cy->nbFeature > 0){
- itListCyl1 = cy->meline1->getMeList().begin();
- itListCyl2 = cy->meline2->getMeList().begin();
- }
-
- wmean = 0;
- for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
- wmean += m_w[n+i] ;
- vpMeSite p = *itListCyl1;
- if (m_w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCyl1 = p;
- }
-
- ++itListCyl1;
- }
-
- if (cy->nbFeaturel1!=0)
- wmean /= cy->nbFeaturel1 ;
- else
- wmean = 1;
-
- cy->setMeanWeight1(wmean);
-
- if (wmean < 0.8){
- cy->Reinit = true;
- }
-
- wmean = 0;
- for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
- wmean += m_w[n+i] ;
- vpMeSite p = *itListCyl2;
- if (m_w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCyl2 = p;
- }
-
- ++itListCyl2;
- }
-
- if (cy->nbFeaturel2!=0)
- wmean /= cy->nbFeaturel2 ;
- else
- wmean = 1;
-
- cy->setMeanWeight2(wmean);
-
- if (wmean < 0.8){
- cy->Reinit = true;
- }
-
- n+= cy->nbFeature ;
- }
-
- // Same thing with circles as with lines
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListCir;
-
- if (ci->nbFeature > 0){
- itListCir = ci->meEllipse->getMeList().begin();
- }
-
- wmean = 0;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- wmean += m_w[n+i] ;
- vpMeSite p = *itListCir;
- if (m_w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCir = p;
- }
-
- ++itListCir;
- }
-
- if (ci->nbFeature!=0)
- wmean /= ci->nbFeature ;
- else
- wmean = 1;
-
- ci->setMeanWeight(wmean);
-
- if (wmean < 0.8){
- ci->Reinit = true;
- }
-
- n+= ci->nbFeature ;
- }
-}
-
-/*!
- Check if the tracking failed.
-
- \throw vpTrackingException::fatalError if the test fails.
-*/
-void
-vpMbEdgeTracker::testTracking()
-{
- int nbExpectedPoint = 0;
- int nbGoodPoint = 0;
- int nbBadPoint = 0;
-
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- if (l->isVisible() && l->meline != NULL)
- {
- nbExpectedPoint += (int)l->meline->expecteddensity;
- for(std::list<vpMeSite>::const_iterator itme=l->meline->getMeList().begin(); itme!=l->meline->getMeList().end(); ++itme){
- vpMeSite pix = *itme;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
- }
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- if ((cy->meline1 !=NULL && cy->meline2 != NULL) && cy->isVisible())
- {
- nbExpectedPoint += (int)cy->meline1->expecteddensity;
- for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
- vpMeSite pix = *itme1;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
- }
- nbExpectedPoint += (int)cy->meline2->expecteddensity;
- for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
- vpMeSite pix = *itme2;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
- }
- }
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- if (ci->isVisible() && ci->meEllipse !=NULL)
- {
- nbExpectedPoint += ci->meEllipse->getExpectedDensity();
- for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
- vpMeSite pix = *itme;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
- }
- }
- }
-
- // Compare the number of good points with the min between the number of expected points and
- // number of points that are tracked
- int nb_min = (int)vpMath::minimum(percentageGdPt *nbExpectedPoint, percentageGdPt *(nbGoodPoint + nbBadPoint) );
- //int nb_min = std::min(val1, val2);
- if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
- std::ostringstream oss;
- oss << "Not enough moving edges ("
- << nbGoodPoint
- << ") to track the object: expected "
- << nb_min
- << ". Try to reduce the threshold="
- << percentageGdPt
- << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
- throw vpTrackingException(vpTrackingException::fatalError, oss.str());
- }
-}
-
-/*!
- Compute each state of the tracking procedure for all the feature sets.
-
- If the tracking is considered as failed an exception is thrown.
-
- \param I : The image.
- */
-void
-vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
-{
- initPyramid(I, Ipyramid);
-
-// for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
- unsigned int lvl = (unsigned int)scales.size();
- do{
- lvl--;
- if(scales[lvl]){
- vpHomogeneousMatrix cMo_1 = cMo;
- try
- {
- downScale(lvl);
-
- try
- {
- trackMovingEdge(*Ipyramid[lvl]);
- }
- catch(...)
- {
- vpTRACE("Error in moving edge tracking") ;
- throw ;
- }
-
- // initialize the vector that contains the error and the matrix that contains
- // the interaction matrix
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- if (l->isVisible()){
- l->initInteractionMatrixError() ;
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- if(cy->isVisible()) {
- cy->initInteractionMatrixError();
- }
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- if (ci->isVisible()){
- ci->initInteractionMatrixError();
- }
- }
-
- try
- {
- computeVVS(*Ipyramid[lvl]);
- }
- catch(vpException &e)
- {
- covarianceMatrix = -1;
- throw e;
- }
-
- try
- {
- testTracking();
- }
- catch(vpException &e)
- {
- throw e;
- }
-
- if (displayFeatures)
- {
- if(lvl == 0){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- if (l->isVisible()){
- l->displayMovingEdges(I);
- }
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- if(cy->isVisible()) {
- cy->displayMovingEdges(I);
- }
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- if (ci->isVisible()){
- ci->displayMovingEdges(I);
- }
- }
- }
- }
-
- try
- {
- updateMovingEdge(I);
- }
- catch(vpException &e)
- {
- throw e;
- }
-
- // Looking for new visible face
- bool newvisibleface = false ;
- visibleFace(I, cMo, newvisibleface) ;
- initMovingEdge(I,cMo) ;
-
- // Reinit the moving edge for the lines which need it.
- reinitMovingEdge(I,cMo);
- upScale(lvl);
- }
- catch(vpException &e)
- {
- if(lvl != 0){
- cMo = cMo_1;
- reInitLevel(lvl);
- upScale(lvl);
- }
- else{
- upScale(lvl);
- throw(e) ;
- }
- }
- }
- } while(lvl != 0);
-
- cleanPyramid(Ipyramid);
-}
-
-/*!
- Initialize the tracking.
-
- \param I : The image.
-*/
-void vpMbEdgeTracker::init(const vpImage<unsigned char>& I)
-{
- if(!modelInitialised){
- throw vpException(vpException::fatalError, "model not initialized");
- }
-
- bool a = false;
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre){
- if(!faces.isOgreInitialised()){
- faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
- faces.initOgre(cam);
- }
- }
-#endif
-
-
- initPyramid(I, Ipyramid);
- visibleFace(I, cMo, a);
- unsigned int i = (unsigned int)scales.size();
-
- do {
- i--;
- if(scales[i]){
- downScale(i);
- initMovingEdge(*Ipyramid[i], cMo);
- upScale(i);
- }
- } while(i != 0);
-
- cleanPyramid(Ipyramid);
-}
-
-/*!
- Set the pose to be used in entry of the next call to the track() function.
- This pose will be just used once.
-
- \warning This function has to be called after the initialization of the tracker.
-
- \param I : image corresponding to the desired pose.
- \param cdMo : Pose to affect.
-*/
-void
-vpMbEdgeTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
-{
- cMo = cdMo;
-
- if (! lines[scaleLevel].empty()) {
- lines[scaleLevel].front() ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->meline != NULL){
- delete (*it)->meline;
- (*it)->meline = NULL;
- }
- }
- }
-
- if (! cylinders[scaleLevel].empty()) {
- cylinders[scaleLevel].front() ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->meline1 != NULL){
- delete (*it)->meline1;
- (*it)->meline1 = NULL;
- }
- if((*it)->meline2 != NULL){
- delete (*it)->meline2;
- (*it)->meline2 = NULL;
- }
- }
- }
-
- if (! circles[scaleLevel].empty()) {
- circles[scaleLevel].front() ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->meEllipse != NULL){
- delete (*it)->meEllipse;
- (*it)->meEllipse = NULL;
- }
- }
- }
-
- init(I);
-}
-
-/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \param configFile : full name of the xml file.
-
- \sa loadConfigFile(const char*), vpXmlParser::cleanup()
-*/
-void
-vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
-{
- vpMbEdgeTracker::loadConfigFile(configFile.c_str());
-}
-
-/*!
- Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
-
- \param configFile : full name of the xml file.
-
- The XML configuration file has the following form:
- \code
-<?xml version="1.0"?>
-<conf>
- <ecm>
- <mask>
- <size>5</size>
- <nb_mask>180</nb_mask>
- </mask>
- <range>
- <tracking>7</tracking>
- </range>
- <contrast>
- <edge_threshold>5000</edge_threshold>
- <mu1>0.5</mu1>
- <mu2>0.5</mu2>
- </contrast>
- </ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
- <face>
- <near_clipping>0.01</near_clipping>
- <far_clipping>0.90</far_clipping>
- <fov_clipping>1</fov_clipping>
- </face>
- <camera>
- <u0>320</u0>
- <v0>240</v0>
- <px>686.24</px>
- <py>686.24</py>
- </camera>
-</conf>
- \endcode
-
- \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
-*/
-void
-vpMbEdgeTracker::loadConfigFile(const char* configFile)
-{
-#ifdef VISP_HAVE_XML2
- vpMbtXmlParser xmlp;
-
- xmlp.setCameraParameters(cam);
- xmlp.setAngleAppear(vpMath::deg(angleAppears));
- xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
- xmlp.setMovingEdge(me);
-
- try{
- std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
- xmlp.parse(configFile);
- }
- catch(...){
- throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile);
- }
-
- vpCameraParameters camera;
- vpMe meParser;
- xmlp.getCameraParameters(camera);
- xmlp.getMe(meParser);
-
- setCameraParameters(camera);
- setMovingEdge(meParser);
- angleAppears = vpMath::rad(xmlp.getAngleAppear());
- angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
-
- if(xmlp.hasNearClippingDistance())
- setNearClippingDistance(xmlp.getNearClippingDistance());
-
- if(xmlp.hasFarClippingDistance())
- setFarClippingDistance(xmlp.getFarClippingDistance());
-
- if(xmlp.getFovClipping())
- setClipping(clippingFlag | vpMbtPolygon::FOV_CLIPPING);
-
- useLodGeneral = xmlp.getLodState();
- minLineLengthThresholdGeneral = xmlp.getMinLineLengthThreshold();
- minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
-
- applyLodSettingInConfig = false;
- if(this->getNbPolygon() > 0) {
- applyLodSettingInConfig = true;
- setLod(useLodGeneral);
- setMinLineLengthThresh(minLineLengthThresholdGeneral);
- setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
- }
-
-#else
- vpTRACE("You need the libXML2 to read the config file %s", configFile);
-#endif
-}
-
-
-/*!
- Display the 3D model from a given position of the camera.
-
- \param I : The image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
-*/
-void
-vpMbEdgeTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &camera, const vpColor& col,
- const unsigned int thickness, const bool displayFullModel)
-{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- break ; //displaying model on one scale only
- }
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-/*!
- Display the 3D model from a given position of the camera.
-
- \param I : The image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
-*/
-void
-vpMbEdgeTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &camera, const vpColor& col,
- const unsigned int thickness, const bool displayFullModel)
-{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
- break ; //displaying model on one scale only
- }
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-
-/*!
- Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the lines.
-
- \param I : The image.
- \param _cMo : The pose of the camera used to initialize the moving edges.
-*/
-void
-vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
-{
- vpMbtDistanceLine *l ;
-
- lines[scaleLevel].front() ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- bool isvisible = false ;
-
- for(std::list<int>::const_iterator itindex=l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
- int index = *itindex;
- if (index ==-1) isvisible =true ;
- else
- {
- if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
- }
- }
-
- //Si la ligne n'appartient a aucune face elle est tout le temps visible
- if (l->Lindex_polygon.empty()) isvisible = true; // Not sure that this can occur
-
- if (isvisible)
- {
- l->setVisible(true) ;
- if (l->meline==NULL)
- {
-// std::cout << "init me line "<< l->getIndex() << std::endl ;
- l->initMovingEdge(I, _cMo) ;
- }
- }
- else
- {
- l->setVisible(false) ;
- if (l->meline!=NULL) delete l->meline;
- l->meline=NULL;
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
-
- bool isvisible = false ;
-
- int index = cy->index_polygon;
- if (index ==-1) isvisible =true ;
- else
- {
- if (cy->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
- }
-// vpTRACE("cyl with index %d is visible: %d", index, isvisible);
-
- if (isvisible)
- {
- cy->setVisible(true) ;
- if (cy->meline1==NULL || cy->meline2==NULL){
- cy->initMovingEdge(I, _cMo) ;
- }
- }
- else
- {
- cy->setVisible(false) ;
- if (cy->meline1!=NULL) delete cy->meline1;
- if (cy->meline2!=NULL) delete cy->meline2;
- cy->meline1=NULL;
- cy->meline2=NULL;
- }
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- bool isvisible = false ;
-
- int index = ci->index_polygon;
- if (index ==-1) isvisible =true ;
- else
- {
- if (ci->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
- }
-
- if (isvisible)
- {
- ci->setVisible(true) ;
- if (ci->meEllipse==NULL)
- {
- ci->initMovingEdge(I, _cMo) ;
- }
- }
- else
- {
- ci->setVisible(false) ;
- if (ci->meEllipse!=NULL) delete ci->meEllipse;
- ci->meEllipse=NULL;
- }
- }
-}
-
-
-/*!
- Track the moving edges in the image.
-
- \param I : the image.
-*/
-void
-vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
-{
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- if(l->isVisible() == true){
- if(l->meline == NULL){
- l->initMovingEdge(I, cMo);
- }
- l->trackMovingEdge(I, cMo) ;
- }
- }
-
- vpMbtDistanceCylinder *cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- if(cy->isVisible()) {
- if(cy->meline1 == NULL || cy->meline2 == NULL){
- cy->initMovingEdge(I, cMo);
- }
- cy->trackMovingEdge(I, cMo) ;
- }
- }
-
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- if(ci->isVisible() == true){
- if(ci->meEllipse == NULL){
- ci->initMovingEdge(I, cMo);
- }
- ci->trackMovingEdge(I, cMo) ;
- }
- }
-}
-
-
-/*!
- Update the moving edges at the end of the virtual visual servoing.
-
- \param I : the image.
-*/
-void
-vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
-{
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- l->updateMovingEdge(I, cMo) ;
- if (l->nbFeature == 0 && l->isVisible()){
- l->Reinit = true;
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- cy->updateMovingEdge(I, cMo) ;
- if((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()){
- cy->Reinit = true;
- }
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->updateMovingEdge(I, cMo) ;
- if(ci->nbFeature == 0 && ci->isVisible()){
- ci->Reinit = true;
- }
- }
-}
-
-
-/*!
- Reinitialize the lines if it is required.
-
- A line is reinitialized if the 2D line do not match enough with the projected 3D line.
-
- \param I : the image.
- \param _cMo : the pose of the used to re-initialize the moving edges
-*/
-void
-vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
-{
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- if (l->Reinit && l->isVisible())
- l->reinitMovingEdge(I, _cMo);
- }
-
- vpMbtDistanceCylinder*cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- if (cy->Reinit && cy->isVisible())
- cy->reinitMovingEdge(I, _cMo);
- }
-
- vpMbtDistanceCircle*ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- if (ci->Reinit && ci->isVisible())
- ci->reinitMovingEdge(I, _cMo);
- }
-}
-
-
-/*!
- Check if two vpPoints are similar.
-
- To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon \f$.
-
- \param P1 : The first point to compare
- \param P2 : The second point to compare
-*/
-bool
-vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2)
-{
- double dx = fabs(P1.get_oX() - P2.get_oX());
- double dy = fabs(P1.get_oY() - P2.get_oY());
- double dz = fabs(P1.get_oZ() - P2.get_oZ());
-
- if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() && dz <= std::numeric_limits<double>::epsilon())
- return true ;
- else
- return false ;
-}
-
-
-/*!
- Add a line belonging to the \f$ index \f$ the polygon to the list of lines. It is defined by its two extremities.
-
- If the line already exists, the ploygone's index is added to the list of polygon to which it belongs.
-
- \param P1 : The first extremity of the line.
- \param P2 : The second extremity of the line.
- \param polygon : The index of the polygon to which the line belongs.
- \param name : the optional name of the line
-*/
-void
-vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
-{
- //suppress line already in the model
- bool already_here = false ;
- vpMbtDistanceLine *l ;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- downScale(i);
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
- (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
- already_here = true ;
- l->Lindex_polygon.push_back(polygon);
- l->hiddenface = &faces ;
- }
- }
-
- if (!already_here){
- l = new vpMbtDistanceLine ;
-
- l->setCameraParameters(cam) ;
- l->buildFrom(P1,P2) ;
- l->Lindex_polygon.push_back(polygon);
- l->setMovingEdge(&me) ;
- l->hiddenface = &faces ;
-
- l->setIndex(nline) ;
- l->setName(name);
-
- if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
- l->getPolygon().setClipping(clippingFlag);
-
- if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
- l->getPolygon().setNearClippingDistance(distNearClip);
-
- if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
- l->getPolygon().setFarClippingDistance(distFarClip);
-
- nline +=1 ;
- lines[i].push_back(l);
- }
- upScale(i);
- }
- }
-}
-
-/*!
- Remove a line using its name.
-
- \param name : The name of the line to remove.
-*/
-void
-vpMbEdgeTracker::removeLine(const std::string& name)
-{
- vpMbtDistanceLine *l;
-
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- if (name.compare(l->getName()) == 0){
- lines[i].erase(it);
- break;
- }
- }
- }
- }
-}
-
-/*!
- Add a circle to the list of circles.
-
- \param P1 : Center of the circle.
- \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
- \param idFace : Id of the face that is associated to the circle to handle visibility test.
- \param name : the optional name of the circle.
-*/
-void
-vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace, const std::string& name)
-{
- bool already_here = false ;
- vpMbtDistanceCircle *ci ;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- downScale(i);
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
- (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
- already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
- }
- }
-
- if (!already_here){
- ci = new vpMbtDistanceCircle ;
-
- ci->setCameraParameters(cam);
- ci->buildFrom(P1, P2, P3, r);
- ci->setMovingEdge(&me);
- ci->setIndex(ncircle);
- ci->setName(name);
- ci->index_polygon = idFace;
- ci->hiddenface = &faces ;
-
-// if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
-// ci->getPolygon().setClipping(clippingFlag);
-
-// if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
-// ci->getPolygon().setNearClippingDistance(distNearClip);
-
-// if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
-// ci->getPolygon().setFarClippingDistance(distFarClip);
-
- ncircle +=1;
- circles[i].push_back(ci);
- }
- upScale(i);
- }
- }
-}
-
-/*!
- Add a cylinder to the list of cylinders.
-
- \param P1 : The first extremity of the axis.
- \param P2 : The second extremity of the axis.
- \param r : The radius of the cylinder.
- \param idFace : The index of the face.
- \param name : the optional name of the cylinder
-*/
-void
-vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string& name)
-{
- bool already_here = false ;
- vpMbtDistanceCylinder *cy ;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- downScale(i);
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
- (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
- already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
- }
- }
-
- if (!already_here){
- cy = new vpMbtDistanceCylinder ;
-
- cy->setCameraParameters(cam);
- cy->buildFrom(P1, P2, r);
- cy->setMovingEdge(&me);
- cy->setIndex(ncylinder);
- cy->setName(name);
- cy->index_polygon = idFace;
- cy->hiddenface = &faces ;
- ncylinder +=1;
- cylinders[i].push_back(cy);
- }
- upScale(i);
- }
- }
-}
-
-/*!
- Remove a cylinder by its name.
-
- \param name : The name of the cylinder to remove.
-*/
-void
-vpMbEdgeTracker::removeCylinder(const std::string& name)
-{
- vpMbtDistanceCylinder *cy;
-
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- if (name.compare(cy->getName()) == 0){
- cylinders[i].erase(it);
- break;
- }
- }
- }
- }
-}
-
-/*!
- Remove a circle by its name.
-
- \param name : The name of the circle to remove.
-*/
-void
-vpMbEdgeTracker::removeCircle(const std::string& name)
-{
- vpMbtDistanceCircle *ci;
-
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceCircle*>::iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- if (name.compare(ci->getName()) == 0){
- circles[i].erase(it);
- break;
- }
- }
- }
- }
-}
-
-/*!
- Add a polygon to the list of polygons.
-
- \param p : The polygon to add.
-*/
-void
-vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
-{
- unsigned int nbpt = p.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(p.p[i], p.p[i+1], p.getIndex()) ;
- addLine(p.p[nbpt-1], p.p[0], p.getIndex()) ;
- }
-}
-
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated since it is no more used since ViSP 2.7.0. \n
-
- Detect the visible faces in the image and says if a new one appeared.
-
- \warning If in one iteration one face appears and one disappears, then the
- function will not detect the new face.
-
- \param _cMo : The pose of the camera used to project the 3D model into the image.
- \param newvisibleline : This parameter is set to true if a new face appeared.
-*/
-void
-vpMbEdgeTracker::visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
-{
- unsigned int n ;
-
- if(!useOgre)
- n = faces.setVisible(_cMo) ;
- else{
-#ifdef VISP_HAVE_OGRE
- bool changed = false;
- n = faces.setVisibleOgre(_cMo, vpMath::rad(70), vpMath::rad(70), changed);
-#else
- n = faces.setVisible(_cMo) ;
-#endif
- }
-
-// cout << "visible face " << n << endl ;
- if (n > nbvisiblepolygone)
- {
- //cout << "une nouvelle face est visible " << endl ;
- newvisibleline = true ;
- }
- else
- newvisibleline = false ;
-
- nbvisiblepolygone= n ;
-}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- Detect the visible faces in the image and says if a new one appeared.
-
- \warning If in one iteration one face appears and one disappears, then the
- function will not detect the new face.
-
- \param _I : Image to test if a face is entirely in the image.
- \param _cMo : The pose of the camera used to project the 3D model into the image.
- \param newvisibleline : This parameter is set to true if a new face appeared.
-*/
-void
-vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> & _I,
- const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
-{
- unsigned int n ;
- bool changed = false;
-
- if(!useOgre) {
- //n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
- n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
- }
- else{
-#ifdef VISP_HAVE_OGRE
- n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
-#else
- n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
-#endif
- }
-
- if (n > nbvisiblepolygone)
- {
- //cout << "une nouvelle face est visible " << endl ;
- newvisibleline = true ;
- }
- else
- newvisibleline = false ;
-
- nbvisiblepolygone= n ;
-}
-
-/*!
- Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
-
- The id of the polygon is supposed to be set prior calling this function.
-
- This method is called from the loadModel() one to add a face of the object to track.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
-{
- unsigned int nbpt = polygon.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
- addLine(polygon.p[nbpt-1], polygon.p[0], polygon.getIndex(), polygon.getName());
- }
-}
-/*!
- Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
-
- The id of the polygon is supposed to be set prior calling this function.
-
- This method is called from the loadModel() one to add a face of the object to track.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
-{
- unsigned int nbpt = polygon.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
- }
-}
-
-/*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
-
- \param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param idFace : Index of the face associated to the circle to handle visibility test.
- \param name : The optional name of the circle.
-*/
-void
-vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace, const std::string &name)
-{
- addCircle(p1, p2, p3, radius, (int)idFace, name);
-}
-
-/*!
- Add a cylinder to track from two points on the axis (defining the length of
- the cylinder) and its radius.
-
- \param p1 : First point on the axis.
- \param p2 : Second point on the axis.
- \param radius : Radius of the cylinder.
- \param idFace : Id of the face that is associated to the cylinder to handle visibility test.
- \param name : The optional name of the cylinder.
-*/
-void
-vpMbEdgeTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
- const std::string &name)
-{
- addCylinder(p1, p2, radius, (int)idFace, name);
-}
-
-/*!
- Reset the tracker. The model is removed and the pose is set to identity.
- The tracker needs to be initialized with a new model and a new pose.
-
-*/
-void
-vpMbEdgeTracker::resetTracker()
-{
- this->cMo.setIdentity();
- vpMbtDistanceLine *l;
- vpMbtDistanceCylinder *cy;
- vpMbtDistanceCircle *ci;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- if (l!=NULL) delete l ;
- l = NULL ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- if (cy!=NULL) delete cy;
- cy = NULL;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- if (ci!=NULL) delete ci;
- ci = NULL;
- }
- lines[i].clear();
- cylinders[i].clear();
- circles[i].clear();
- }
- }
-
- faces.reset();
-
-#ifdef VISP_HAVE_OGRE
- useOgre = false;
-#endif
-
- compute_interaction=1;
- nline = 0;
- ncylinder = 0;
- lambda = 1;
- nbvisiblepolygone = 0;
- percentageGdPt = 0.4;
-
- angleAppears = vpMath::rad(89);
- angleDisappears = vpMath::rad(89);
- clippingFlag = vpMbtPolygon::NO_CLIPPING;
-
- // reinitialization of the scales.
- this->setScales(scales);
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- reInitModel(I, cad_name.c_str(), cMo_, verbose);
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- this->cMo.setIdentity();
- vpMbtDistanceLine *l;
- vpMbtDistanceCylinder *cy;
- vpMbtDistanceCircle *ci;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- if (l!=NULL) delete l ;
- l = NULL ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- cy = *it;
- if (cy!=NULL) delete cy;
- cy = NULL;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- ci = *it;
- if (ci!=NULL) delete ci;
- ci = NULL;
- }
-
- lines[i].clear();
- cylinders[i].clear();
- circles[i].clear();
- }
- }
-
- faces.reset();
-
- //compute_interaction=1;
- nline = 0;
- ncylinder = 0;
- ncircle = 0;
- //lambda = 1;
- nbvisiblepolygone = 0;
-
- loadModel(cad_name, verbose);
- initFromPose(I, cMo_);
-}
-
-/*!
- Return the number of good points (vpMeSite) tracked. A good point is a
- vpMeSite with its flag "suppress" equal to 0. Only these points are used
- during the virtual visual servoing stage.
-
- \exception vpException::dimensionError if level does not represent a used
- level.
-
- \return the number of good points.
-*/
-unsigned int
-vpMbEdgeTracker::getNbPoints(const unsigned int level) const
-{
- if((level > scales.size()) || !scales[level]){
- throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used", level);
- }
-
- unsigned int nbGoodPoints = 0;
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
- l = *it;
- if (l->isVisible() && l->meline != NULL)
- {
- for(std::list<vpMeSite>::const_iterator itme=l->meline->getMeList().begin(); itme!=l->meline->getMeList().end(); ++itme){
- if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
- }
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
- cy = *it;
- if (cy->isVisible() && (cy->meline1 != NULL || cy->meline2 != NULL))
- {
- for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
- if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
- }
- for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
- if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
- }
- }
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[level].begin(); it!=circles[level].end(); ++it){
- ci = *it;
- if (ci->isVisible() && ci->meEllipse != NULL)
- {
- for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
- if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
- }
- }
- }
-
- return nbGoodPoints;
-}
-
-/*!
- Set the scales to use to realize the tracking. The vector of boolean activates
- or not the scales to set for the object tracking. The first element of the list
- correspond to the tracking on the full image, the second element corresponds
- to the tracking on an image subsampled by two.
-
- Using multi scale tracking allows to track the object with greater moves. It
- requires the computation of a pyramid of images, but the total tracking can be
- faster than a tracking based only on the full scale. The pose is computed from
- the smallest image to the biggest. This may be dangerous if the object to
- track is small in the image, because the subsampled scale(s) will have only
- few points to compute the pose (it could result in a loss of precision).
-
- \warning This method must be used before the tracker has been initialized (
- before the call of the loadConfigFile() or loadModel() methods).
-
- \warning At least one level must be activated.
-
- \param scale : The vector describing the levels to use.
-*/
-void
-vpMbEdgeTracker::setScales(const std::vector<bool>& scale)
-{
- unsigned int nbActivatedLevels = 0;
- for (unsigned int i = 0; i < scale.size(); i += 1){
- if(scale[i]){
- nbActivatedLevels++;
- }
- }
- if((scale.size() < 1) || (nbActivatedLevels == 0)){
- vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
- this->scales.resize(0);
- this->scales.push_back(true);
- lines.resize(1);
- lines[0].clear();
- cylinders.resize(1);
- cylinders[0].clear();
- }
- else{
- this->scales = scale;
- lines.resize(scale.size());
- cylinders.resize(scale.size());
- for (unsigned int i = 0; i < lines.size(); i += 1){
- lines[i].clear();
- cylinders[i].clear();
- }
- }
-}
-
-/*!
- Set the far distance for clipping.
-
- \param dist : Far clipping value.
-*/
-void
-vpMbEdgeTracker::setFarClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING && dist <= distNearClip)
- vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
- else{
- vpMbTracker::setFarClippingDistance(dist);
- vpMbtDistanceLine *l;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- l->getPolygon().setFarClippingDistance(distFarClip);
- }
- }
- }
- }
-}
-
-/*!
- Set the near distance for clipping.
-
- \param dist : Near clipping value.
-*/
-void
-vpMbEdgeTracker::setNearClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING && dist >= distFarClip)
- vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
- else{
- vpMbTracker::setNearClippingDistance(dist);
- vpMbtDistanceLine *l;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- l->getPolygon().setNearClippingDistance(distNearClip);
- }
- }
- }
- }
-}
-
-/*!
- Specify which clipping to use.
-
- \sa vpMbtPolygonClipping
-
- \param flags : New clipping flags.
-*/
-void
-vpMbEdgeTracker::setClipping(const unsigned int &flags)
-{
- vpMbTracker::setClipping(flags);
-
- vpMbtDistanceLine *l;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- l = *it;
- l->getPolygon().setClipping(clippingFlag);
- }
- }
- }
-}
-
-/*!
- Compute the pyramid of image associated to the image in parameter. The scales
- computed are the ones corresponding to the scales attribute of the class. If
- OpenCV is detected, the functions used to computed a smoothed pyramid come
- from OpenCV, otherwise a simple subsampling (no smoothing, no interpolation)
- is realized.
-
- \warning The pyramid contains pointers to vpImage. To properly deallocate the
- pyramid. All the element but the first (which is a pointer to the input image)
- must be freed. A proper cleaning is implemented in the cleanPyramid() method.
-
- \param _I : The input image.
- \param _pyramid : The pyramid of image to build from the input image.
-*/
-void
-vpMbEdgeTracker::initPyramid(const vpImage<unsigned char>& _I, std::vector< const vpImage<unsigned char>* >& _pyramid)
-{
- _pyramid.resize(scales.size());
-
- if(scales[0]){
- _pyramid[0] = &_I;
- }
- else{
- _pyramid[0] = NULL;
- }
-
- for(unsigned int i=1; i<_pyramid.size(); i += 1){
- if(scales[i]){
- unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
- vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
- IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
- vpI0->imageData = (char*)(_I.bitmap);
- IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
- cvResize(vpI0, vpI, CV_INTER_NN);
- vpImageConvert::convert(vpI, *I);
- cvReleaseImage(&vpI);
- vpI0->imageData = NULL;
- cvReleaseImageHeader(&vpI0);
-#else
- for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
- for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
- (*I)[k][l] = _I[ii][jj];
- }
- }
-#endif
- _pyramid[i] = I;
- }
- else{
- _pyramid[i] = NULL;
- }
- }
-}
-
-/*!
- Clean the pyramid of image allocated with the initPyramid() method. The vector
- has a size equal to zero at the end of the method.
-
- \param _pyramid : The pyramid of image to clean.
-*/
-void
-vpMbEdgeTracker::cleanPyramid(std::vector< const vpImage<unsigned char>* >& _pyramid)
-{
- if(_pyramid.size() > 0){
- _pyramid[0] = NULL;
- for (unsigned int i = 1; i < _pyramid.size(); i += 1){
- if(_pyramid[i] != NULL){
- delete _pyramid[i];
- _pyramid[i] = NULL;
- }
- }
- _pyramid.resize(0);
- }
-}
-
-/*!
- Get the list of the lines tracked for the specified level. Each line contains
- the list of the vpMeSite.
-
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
-
- \param level : Level corresponding to the list to return.
- \param linesList : The list of the lines of the model.
-*/
-void
-vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
-{
- if(level > scales.size() || !scales[level]){
- std::ostringstream oss;
- oss << level;
- std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
- throw vpException(vpException::dimensionError, errorMsg);
- }
-
- linesList = lines[level];
-}
-
-
-/*!
- Get the list of the cylinders tracked for the specified level. Each cylinder
- contains the list of the vpMeSite.
-
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
-
- \param level : Level corresponding to the list to return.
- \param cylindersList : The list of the cylinders of the model.
-*/
-void
-vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
-{
- if(level > scales.size() || !scales[level]){
- std::ostringstream oss;
- oss << level;
- std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
- throw vpException(vpException::dimensionError, errorMsg);
- }
-
- cylindersList = cylinders[level];
-}
-
-
-/*!
- Get the list of the circles tracked for the specified level. Each circle
- contains the list of the vpMeSite.
-
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
-
- \param level : Level corresponding to the list to return.
- \param circlesList : The list of the circles of the model.
-*/
-void
-vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level)
-{
- if(level > scales.size() || !scales[level]){
- std::ostringstream oss;
- oss << level;
- std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
- throw vpException(vpException::dimensionError, errorMsg);
- }
-
- circlesList = circles[level];
-}
-
-/*!
- Modify the camera parameters to have them corresponding to the current scale.
- The new parameters are divided by \f$ 2^{\_scale} \f$.
-
- \param _scale : Scale to use.
-*/
-void
-vpMbEdgeTracker::downScale(const unsigned int _scale)
-{
- const double ratio = pow(2., (int)_scale);
- scaleLevel = _scale;
-
- vpMatrix K = cam.get_K();
-
- K[0][0] /= ratio;
- K[1][1] /= ratio;
- K[0][2] /= ratio;
- K[1][2] /= ratio;
-
- cam.initFromCalibrationMatrix(K);
-}
-
-/*!
- Modify the camera parameters to have them corresponding to the current scale.
- The new parameters are multiplied by \f$ 2^{\_scale} \f$.
-
- \param _scale : Scale to use.
-*/
-void
-vpMbEdgeTracker::upScale(const unsigned int _scale)
-{
- const double ratio = pow(2., (int)_scale);
- scaleLevel = 0;
-
- vpMatrix K = cam.get_K();
-
- K[0][0] *= ratio;
- K[1][1] *= ratio;
- K[0][2] *= ratio;
- K[1][2] *= ratio;
-
-
- cam.initFromCalibrationMatrix(K);
-}
-
-/*!
- Re initialize the moving edges associated to a given level. This method is
- used to re-initialize the level if the tracking failed on this level but
- succeeded on the other one.
-
- \param _lvl : The level to re-initialize.
-*/
-void
-vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
-{
- unsigned int scaleLevel_1 = scaleLevel;
- scaleLevel = _lvl;
-
- vpMbtDistanceLine *l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- l = *it;
- l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
- }
-
- vpMbtDistanceCylinder *cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- cy = *it;
- cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
- }
-
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->reinitMovingEdge(*Ipyramid[_lvl], cMo);
- }
-
- trackMovingEdge(*Ipyramid[_lvl]);
- updateMovingEdge(*Ipyramid[_lvl]);
- scaleLevel = scaleLevel_1;
-}
-
diff --git a/src/tracking/mbt/edge/vpMbEdgeTracker.h b/src/tracking/mbt/edge/vpMbEdgeTracker.h
deleted file mode 100644
index e497e6e..0000000
--- a/src/tracking/mbt/edge/vpMbEdgeTracker.h
+++ /dev/null
@@ -1,518 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbEdgeTracker.h 5217 2015-01-28 09:40:02Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbEdgeTracker.h
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-
-#ifndef vpMbEdgeTracker_HH
-#define vpMbEdgeTracker_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMbTracker.h>
-#include <visp/vpMe.h>
-#include <visp/vpMbtMeLine.h>
-#include <visp/vpMbtDistanceLine.h>
-#include <visp/vpMbtDistanceCircle.h>
-#include <visp/vpMbtDistanceCylinder.h>
-#include <visp/vpXmlParser.h>
-
-#include <iostream>
-#include <fstream>
-#include <vector>
-#include <list>
-
-#if defined(VISP_HAVE_COIN)
-//Inventor includes
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
-#include <Inventor/actions/SoGetMatrixAction.h>
-#include <Inventor/actions/SoGetPrimitiveCountAction.h>
-#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
-#endif
-
-#ifdef VISP_HAVE_OPENCV
-# if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/core/core.hpp>
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/imgproc/imgproc_c.h>
-# else
-# include <cv.h>
-# endif
-#endif
-
-/*!
- \class vpMbEdgeTracker
- \ingroup ModelBasedTracking
- \brief Make the complete tracking of an object by using its CAD model.
-
- This class allows to track an object or a scene given its 3D model. A
- video can be found in the \e http://www.irisa.fr/lagadic/visp/computer-vision.html web page. The \ref tutorial-tracking-mb is also a good starting point to use this class.
-
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
-
- The following code shows the simplest way to use the tracker.
-
-\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
- vpMbEdgeTracker tracker; // Create a model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Edge Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
- tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
-
- while(true){
- // Acquire a new image
- vpDisplay::display(I);
- tracker.track(I); // Track the object on this image
- tracker.getPose(cMo); // Get the pose
-
- tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
- vpXmlParser::cleanup();
-
- return 0;
-}
-\endcode
-
- For application with large inter-images displacement, multi-scale tracking is also possible, by setting the number of scales used and by activating (or not) them
- using a vector of booleans, as presented in the following code:
-
-\code
- ...
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
- vpCameraParameters cam;
-
- std::vector< bool > scales(3); //Three scales used
- scales.push_back(true); //First scale : active
- scales.push_back(false); //Second scale (/2) : not active
- scales.push_back(true); //Third scale (/4) : active
- tracker.setScales(scales); // Set active scales for multi-scale tracking
-
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- ...
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
- vpXmlParser::cleanup();
-\endcode
-
- The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
-
-\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
- vpMbEdgeTracker tracker; // Create a model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
- //acquire an image
- vpImageIo::read(I, "cube.pgm"); // Example of acquisition
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
- tracker.initFromPose(I, cMo); // initialize the tracker with the given pose.
-
- while(true){
- // acquire a new image
- tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
- }
-
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
- vpXmlParser::cleanup();
-
- return 0;
-}
-\endcode
-
- Finally it can be used not to track an object but just to display a model at a
- given pose:
-
-\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
- vpMbEdgeTracker tracker; // Create a model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Edge Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
-
- while(true){
- // acquire a new image
- // Get the pose using any method
- vpDisplay::display(I);
- tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
- vpXmlParser::cleanup();
-
- return 0;
-}
-\endcode
-
-*/
-
-class VISP_EXPORT vpMbEdgeTracker: virtual public vpMbTracker
-{
- protected :
-
- /*! If this flag is true, the interaction matrix
- extracted from the feature set is computed at each
- iteration in the visual servoing loop.
- */
- int compute_interaction;
- //! The gain of the virtual visual servoing stage.
- double lambda;
-
- //! The moving edges parameters.
- vpMe me;
- //! Vector of list of all the lines tracked (each line is linked to a list of moving edges). Each element of the vector is for a scale (element 0 = level 0 = no subsampling).
- std::vector< std::list< vpMbtDistanceLine*> > lines;
-
- //! Vector of the tracked circles.
- std::vector< std::list< vpMbtDistanceCircle*> > circles;
-
- //! Vector of the tracked cylinders.
- std::vector< std::list< vpMbtDistanceCylinder*> > cylinders;
-
- //! Index of the polygon to add, and total number of polygon extracted so far.
- unsigned int nline;
-
- //! Index of the circle to add, and total number of circles extracted so far.
- unsigned int ncircle;
-
- //! Index of the cylinder to add, and total number of cylinders extracted so far.
- unsigned int ncylinder;
-
- //! Number of polygon (face) currently visible.
- unsigned int nbvisiblepolygone;
-
- //! Percentage of good points over total number of points below which tracking is supposed to have failed.
- double percentageGdPt;
-
- //! Vector of scale level to use for the multi-scale tracking.
- std::vector<bool> scales;
-
- //! Pyramid of image associated to the current image. This pyramid is computed in the init() and in the track() methods.
- std::vector< const vpImage<unsigned char>* > Ipyramid;
-
- //! Current scale level used. This attribute must not be modified outside of the downScale() and upScale() methods, as it used to specify to some methods which set of distanceLine use.
- unsigned int scaleLevel;
-
-public:
-
- vpMbEdgeTracker();
- virtual ~vpMbEdgeTracker();
-
- void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
-
- /*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
-
- void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level = 0);
- void getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level = 0);
- void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level = 0);
-
- /*!
- Get the moving edge parameters.
-
- \return an instance of the moving edge parameters used by the tracker.
- */
- inline void getMovingEdge(vpMe &p_me ) const { p_me = this->me;}
- /*!
- Get the moving edge parameters.
-
- \return an instance of the moving edge parameters used by the tracker.
- */
- inline vpMe getMovingEdge() const { return this->me;}
-
- unsigned int getNbPoints(const unsigned int level=0) const;
-
- /*!
- Return the scales levels used for the tracking.
-
- \return The scales levels used for the tracking.
- */
- std::vector<bool> getScales() const {return scales;}
- /*!
- \return The threshold value between 0 and 1 over good moving edges ratio. It allows to
- decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
- moving edges should be considered as good to have a valid pose, while 0.1 means that
- 10% of the moving edge are enough to declare a pose valid.
-
- \sa setGoodMovingEdgesRatioThreshold()
- */
- inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt;}
-
- void loadConfigFile(const std::string &configFile);
- void loadConfigFile(const char* configFile);
- void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
- void resetTracker();
-
- /*!
- Set the camera parameters.
-
- \param camera : the new camera parameters
- */
- virtual void setCameraParameters(const vpCameraParameters& camera) {
- this->cam = camera;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- (*it)->setCameraParameters(cam);
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- (*it)->setCameraParameters(cam);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- (*it)->setCameraParameters(cam);
- }
- }
- }
- }
-
- virtual void setClipping(const unsigned int &flags);
-
- virtual void setFarClippingDistance(const double &dist);
-
- virtual void setNearClippingDistance(const double &dist);
-
- /*!
- Use Ogre3D for visibility tests
-
- \warning This function has to be called before the initialization of the tracker.
-
- \param v : True to use it, False otherwise
- */
- virtual void setOgreVisibilityTest(const bool &v){
- vpMbTracker::setOgreVisibilityTest(v);
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Edge");
-#endif
- }
-
- /*!
- Set the threshold value between 0 and 1 over good moving edges ratio. It allows to
- decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
- moving edges should be considered as good to have a valid pose, while 0.1 means that
- 10% of the moving edge are enough to declare a pose valid.
-
- \param threshold : Value between 0 and 1 that corresponds to the ratio of good
- moving edges that is necessary to consider that the estimated pose is valid.
- Default value is 0.4.
-
- \sa getGoodMovingEdgesRatioThreshold()
- */
- void setGoodMovingEdgesRatioThreshold(const double threshold) {percentageGdPt = threshold;}
-
- /*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain;}
-
- void setMovingEdge(const vpMe &me);
-
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
-
- void setScales(const std::vector<bool>& _scales);
-
- void track(const vpImage<unsigned char> &I);
-
-protected:
- bool samePoint(const vpPoint &P1, const vpPoint &P2);
- void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace = -1, const std::string& name = "");
- void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string& name = "");
- void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
- void addPolygon(vpMbtPolygon &p) ;
- void cleanPyramid(std::vector<const vpImage<unsigned char>* >& _pyramid);
- void computeVVS(const vpImage<unsigned char>& _I);
- void downScale(const unsigned int _scale);
- void init(const vpImage<unsigned char>& I);
- virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace=0, const std::string &name="");
- virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
- const std::string &name="");
- virtual void initFaceFromCorners(vpMbtPolygon &polygon);
- virtual void initFaceFromLines(vpMbtPolygon &polygon);
- void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo) ;
- void initPyramid(const vpImage<unsigned char>& _I, std::vector<const vpImage<unsigned char>* >& _pyramid);
- void reInitLevel(const unsigned int _lvl);
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
- void removeCircle(const std::string& name);
- void removeCylinder(const std::string& name);
- void removeLine(const std::string& name);
- void testTracking();
- void trackMovingEdge(const vpImage<unsigned char> &I) ;
- void updateMovingEdge(const vpImage<unsigned char> &I) ;
- void upScale(const unsigned int _scale);
- void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated void visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline);
-
-public:
- /*!
- \deprecated Since this function name is not explicit, use rather getGoodMovingEdgesRatioThreshold()
- that does the same.
-
- \return The threshold value between 0 and 1 that allows to decide if the tracker
- has enough valid moving edges to compute a pose.
- */
- vp_deprecated inline double getFirstThreshold() const { return percentageGdPt;}
- /*!
- \deprecated Use vpMbTracker::setDisplayFeatures() instead.
- Enable to display the points along the line 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.
-
- \param displayMe : set it to true to display the points.
- */
- vp_deprecated void setDisplayMovingEdges(const bool displayMe) {displayFeatures = displayMe;}
- /*!
- \deprecated Since this function name is not explicit, use rather setGoodMovingEdgesRatioThreshold()
- that does the same.
- Set the first threshold used to check if the tracking failed. It corresponds to the percentage
- of good point which is necessary.
-
- The condition which has to be be satisfied is the following : \f$ nbGoodPoint > threshold1 \times (nbGoodPoint + nbBadPoint)\f$.
-
- The threshold is ideally between 0 and 1.
-
- \param threshold1 : The new value of the threshold.
- */
- vp_deprecated void setFirstThreshold(const double threshold1) {percentageGdPt = threshold1;}
- #endif
-};
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtDistanceCircle.cpp b/src/tracking/mbt/edge/vpMbtDistanceCircle.cpp
deleted file mode 100644
index 60fd7ed..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceCircle.cpp
+++ /dev/null
@@ -1,430 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceCircle.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model. Circle
- * tracking.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-/*!
- \file vpMbtDistanceCircle.cpp
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-
-#include <stdlib.h>
-#include <algorithm>
-
-#include <visp/vpMbtDistanceCircle.h>
-#include <visp/vpPlane.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpPose.h>
-
-/*!
- Basic constructor
-*/
-vpMbtDistanceCircle::vpMbtDistanceCircle()
- : name(), index(0), cam(), me(NULL), wmean(1),
- featureEllipse(), meEllipse(NULL),
- circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
- L(), error(), nbFeature(0), Reinit(false),
- hiddenface(NULL), index_polygon(-1), isvisible(false)
-{
-}
-
-/*!
- Basic destructor useful to deallocate the memory.
-*/
-vpMbtDistanceCircle::~vpMbtDistanceCircle()
-{
- if (meEllipse != NULL) delete meEllipse;
- if (circle != NULL) delete circle;
- if (p1 != NULL) delete p1;
- if (p2 != NULL) delete p2;
- if (p3 != NULL) delete p3;
-}
-
-/*!
- Project the circle into the image.
-
- \param cMo : The pose of the camera used to project the circle into the image.
-*/
-void
-vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo)
-{
- circle->project(cMo) ;
-}
-
-
-/*!
- Build a vpMbtDistanceCircle thanks to its center, 3 points (including the center) with
- coordinates expressed in the object frame and defining the plane that contain
- the circle and its radius.
-
- \param _p1 : Center of the circle.
- \param _p2,_p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
-*/
-void
-vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
-{
- circle = new vpCircle ;
- p1 = new vpPoint ;
- p2 = new vpPoint ;
- p3 = new vpPoint ;
-
- // Get the points
- *p1 = _p1;
- *p2 = _p2;
- *p3 = _p3;
-
- // Get the radius
- radius = r;
-
- vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
-
- // Build our circle
- circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
-}
-
-
-/*!
- Set the moving edge parameters.
-
- \param _me : an instance of vpMe containing all the desired parameters
-*/
-void
-vpMbtDistanceCircle::setMovingEdge(vpMe *_me)
-{
- me = _me ;
- if (meEllipse != NULL)
- {
- meEllipse->setMe(me) ;
- }
-}
-
-/*!
- Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the circle.
-
- \param I : The image.
- \param cMo : The pose of the camera used to initialize the moving edges.
- \return false if an error occur, true otherwise.
-*/
-bool
-vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- // Perspective projection
- circle->changeFrame(cMo);
-
- try{
- circle->projection();
- }
- catch(...){
- std::cout<<"Problem when projecting circle\n";
- return false;
- }
-
- // Create the moving edges containers
- meEllipse = new vpMbtMeEllipse;
- meEllipse->setMe(me) ;
-
- //meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
- meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
-
- try
- {
- vpImagePoint ic;
- double mu20_p, mu11_p, mu02_p;
- vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
- meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p);
- }
- catch(...)
- {
- //vpTRACE("the circle can't be initialized");
- return false;
- }
- }
- return true;
-}
-
-/*!
- Track the moving edges in the image.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
-{
- if(isvisible){
- try
- {
- meEllipse->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meEllipse failed" << std::endl;
- meEllipse->reset();
- Reinit = true;
- }
-
- // Update the number of features
- nbFeature = (unsigned int)meEllipse->getMeList().size();
- }
-}
-
-
-/*!
- Update the moving edges internal parameters.
-
- \warning : Not implemented.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- // Perspective projection
- circle->changeFrame(cMo);
-
- try{
- circle->projection();
- }
- catch(...){std::cout<<"Problem when projecting circle\n";}
-
- try
- {
-
- vpImagePoint ic;
- double mu20_p, mu11_p, mu02_p;
- vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
- meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
- }
- catch(...)
- {
- Reinit = true;
- }
- nbFeature = (unsigned int)meEllipse->getMeList().size();
- }
-}
-
-
-/*!
- Reinitialize the circle if it is required.
-
- A circle is reinitialized if the ellipse do not match enough with the projected 3D circle.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(meEllipse!= NULL)
- delete meEllipse;
-
- meEllipse = NULL;
-
- if(initMovingEdge(I,cMo) == false)
- Reinit = true;
-
- Reinit = false;
-}
-
-
-/*!
- Display the circle. The 3D circle is projected into the image as an ellipse.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : When true, display the circle even if non visible.
- If false, display the circle only if visible.
-*/
-void
-vpMbtDistanceCircle::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel )
-{
- if(isvisible || displayFullModel){
- // Perspective projection
- circle->changeFrame(cMo);
-
- try{
- circle->projection();
- }
- catch(...){std::cout<<"Cannot project the circle";}
-
- vpImagePoint center;
- double mu20_p, mu11_p, mu02_p;
- vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
- vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
- }
-}
-
-/*!
- Display the cylinder. The 3D cylinder is projected into the image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : When true, display the circle even if non visible.
- If false, display the circle only if visible.
-*/
-void
-vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col,
- const unsigned int thickness, const bool displayFullModel)
-{
- if(isvisible || displayFullModel){
- // Perspective projection
- circle->changeFrame(cMo);
-
- try{
- circle->projection();
- }
- catch(...){std::cout<<"Cannot project the circle";}
-
- vpImagePoint center;
- double mu20_p, mu11_p, mu02_p;
- vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
- vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
- }
-}
-
-
-/*!
- Enable to display the points along the ellipse 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 (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
- \param I : The image.
-*/
-void
-vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
-{
- if (meEllipse != NULL)
- {
- meEllipse->display(I); // display the me
- if (vpDEBUG_ENABLE(3))
- vpDisplay::flush(I);
- }
-}
-
-/*!
- Initialize the size of the interaction matrix and the error vector.
-*/
-void
-vpMbtDistanceCircle::initInteractionMatrixError()
-{
- if (isvisible == true)
- {
- nbFeature = (unsigned int)meEllipse->getMeList().size();
- L.resize(nbFeature, 6);
- error.resize(nbFeature);
- }
- else
- nbFeature = 0 ;
-}
-
-/*!
- Compute the interaction matrix and the error vector corresponding to the point to ellipse algebraic distance.
-*/
-void
-vpMbtDistanceCircle::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
-{
- if (isvisible)
- {
- // Perspective projection
- circle->changeFrame(cMo) ;
- try{
- circle->projection();
- }
- catch(...){std::cout<<"Problem projection circle\n";}
-
- vpFeatureBuilder::create(featureEllipse, *circle);
-
- vpMatrix H1 = featureEllipse.interaction();
-
- vpRowVector H(5);
- double x=0, y=0;
-
- // Get the parameters of the ellipse in the image plane
- double xg = circle->p[0];
- double yg = circle->p[1];
- double mu20 = circle->p[2];
- double mu11 = circle->p[3];
- double mu02 = circle->p[4];
-
- unsigned int j = 0;
-
- for(std::list<vpMeSite>::const_iterator it=meEllipse->getMeList().begin(); it!=meEllipse->getMeList().end(); ++it){
- vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
- H[0] = 2*(mu11*(y-yg)+mu02*(xg-x));
- H[1] = 2*(mu20*(yg-y)+mu11*(x-xg));
- H[2] = vpMath::sqr(y-yg)-mu02;
- H[3] = 2*(yg*(x-xg)+y*xg+mu11-x*y);
- H[4] = vpMath::sqr(x-xg)-mu20;
-
- for (unsigned int k=0; k<6; k++)
- L[j][k] = H[0]*H1[0][k] + H[1]*H1[1][k] + H[2]*H1[2][k] + H[3]*H1[3][k] + H[4]*H1[4][k];
-
- error[j] = mu02*vpMath::sqr(x) + mu20*vpMath::sqr(y) - 2*mu11*x*y
- + 2*(mu11*yg-mu02*xg)*x + 2*(mu11*xg-mu20*yg)*y
- + mu02*vpMath::sqr(xg) + mu20*vpMath::sqr(yg) - 2*mu11*xg*yg
- + vpMath::sqr(mu11) - mu20*mu02;
-
- j++;
- }
- }
-}
-
diff --git a/src/tracking/mbt/edge/vpMbtDistanceCircle.h b/src/tracking/mbt/edge/vpMbtDistanceCircle.h
deleted file mode 100644
index 07aa075..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceCircle.h
+++ /dev/null
@@ -1,222 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceCircle.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Manage a circle used in the model-based tracker.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtDistanceCircle.h
- \brief Manage a circle used in the model-based tracker.
-*/
-
-#ifndef vpMbtDistanceCircle_HH
-#define vpMbtDistanceCircle_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMbtMeEllipse.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpCircle.h>
-#include <visp/vpMbHiddenFaces.h>
-
-/*!
- \class vpMbtDistanceCircle
-
- \brief Manage a circle used in the model-based tracker.
-
- \ingroup ModelBasedTracking
-*/
-class VISP_EXPORT vpMbtDistanceCircle
-{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- //double alpha;
- double wmean;
- vpFeatureEllipse featureEllipse ;
- //! Polygon describing the circle bbox
-// vpMbtPolygon poly;
-
- public:
- //! The moving edge containers
- vpMbtMeEllipse *meEllipse;
-
- //! The circle to track
- vpCircle *circle;
-
- //! The radius of the circle
- double radius;
-
- //! The center of the circle
- vpPoint *p1;
- //! A point on the plane containing the circle
- vpPoint *p2;
- //! An other point on the plane containing the circle
- vpPoint *p3;
-
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- unsigned int nbFeature;
- //! Indicates if the circle has to be reinitialized
- bool Reinit;
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the faces which contain the line
- int index_polygon;
- //! Indicates if the circle is visible or not
- bool isvisible;
-
- public:
- vpMbtDistanceCircle() ;
- ~vpMbtDistanceCircle() ;
-
- void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r);
-
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
-
- /*!
- Get the index of the circle.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() {return index ;}
-
- /*!
- Get the mean weight of the circle. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the circle.
- */
- inline double getMeanWeight() const {return wmean;}
-
- /*!
- Get the name of the circle.
-
- \return Return the name of the circle.
- */
- inline std::string getName() const {return name;}
-
- /*!
- Get the polygon associated to the circle.
-
- \return poly.
- */
-// inline vpMbtPolygon& getPolygon() {return poly;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Check if the circle is visible in the image or not.
-
- \return Return true if the circle is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set the index of the circle.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the circle.
-
- \param _wmean : The mean weight of the circle.
- */
- inline void setMeanWeight(const double _wmean) {this->wmean = _wmean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the circle.
-
- \param circle_name : The name of the circle.
- */
- inline void setName(const std::string& circle_name) {this->name = circle_name;}
-
- /*!
- Set the name of the circle.
-
- \param circle_name : The name of the circle.
- */
- inline void setName(const char* circle_name) {this->name = std::string(circle_name);}
-
- /*!
- Set a boolean parameter to indicates if the circle is visible in the image or not.
-
- \param _isvisible : Set to true if the circle is visible
- */
- inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
-
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- private:
- void project(const vpHomogeneousMatrix &cMo);
-} ;
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtDistanceCylinder.cpp b/src/tracking/mbt/edge/vpMbtDistanceCylinder.cpp
deleted file mode 100755
index f664e31..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceCylinder.cpp
+++ /dev/null
@@ -1,775 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceCylinder.cpp 4914 2014-10-02 13:25:47Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model. Cylinder
- * tracking.
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Bertrand Delabarre
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-/*!
- \file vpMbtDistanceCylinder.cpp
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-
-#include <visp/vpMbtDistanceCylinder.h>
-#include <visp/vpPlane.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureEllipse.h>
-#include <stdlib.h>
-#include <algorithm>
-
-#include <visp/vpPose.h>
-
-/*!
- Basic constructor
-*/
-vpMbtDistanceCylinder::vpMbtDistanceCylinder()
- : name(), index(0), cam(), me(NULL), alpha(0), wmean1(1), wmean2(1),
- featureline1(), featureline2(), meline1(NULL), meline2(NULL),
- cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
- error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
- c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
-{
-}
-
-/*!
- Basic destructor useful to deallocate the memory.
-*/
-vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
-{
-// cout << "Deleting cylinder " << index << endl ;
-
- if (p1 != NULL) delete p1 ;
- if (p2 != NULL) delete p2 ;
- if (c != NULL) delete c ;
- if (meline1 != NULL) delete meline1 ;
- if (meline2 != NULL) delete meline2 ;
- if (cercle1 != NULL) delete cercle1 ;
- if (cercle2 != NULL) delete cercle2 ;
-}
-
-/*!
- Project the cylinder and the two points corresponding to its extremities into the image.
-
- \param cMo : The pose of the camera used to project the cylinder into the image.
-*/
-void
-vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
-{
- c->project(cMo) ;
- p1->project(cMo) ;
- p2->project(cMo) ;
- cercle1->project(cMo) ;
- cercle2->project(cMo) ;
-}
-
-
-/*!
- Build a vpMbtDistanceCylinder thanks to two points corresponding to the extremities of its axis and its radius.
-
- \param _p1 : The first extremity on the axis.
- \param _p2 : The second extremity on the axis.
- \param r : Radius of the cylinder.
-*/
-void
-vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
-{
- c = new vpCylinder ;
- p1 = new vpPoint ;
- p2 = new vpPoint ;
- cercle1 = new vpCircle;
- cercle2 = new vpCircle;
-
- // Get the points
- *p1 = _p1;
- *p2 = _p2;
-
- // Get the radius
- radius = r;
-
- vpColVector ABC(3);
- vpColVector V1(3);
- vpColVector V2(3);
-
- V1[0] = _p1.get_oX();
- V1[1] = _p1.get_oY();
- V1[2] = _p1.get_oZ();
- V2[0] = _p2.get_oX();
- V2[1] = _p2.get_oY();
- V2[2] = _p2.get_oZ();
-
- // Get the axis of the cylinder
- ABC = V1-V2;
-
- // Build our extremity circles
- cercle1->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p1.get_oX(),_p1.get_oY(),_p1.get_oZ(),r);
- cercle2->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p2.get_oX(),_p2.get_oY(),_p2.get_oZ(),r);
-
- // Build our cylinder
- c->setWorldCoordinates(ABC[0],ABC[1],ABC[2],(_p1.get_oX()+_p2.get_oX())/2.0,(_p1.get_oY()+_p2.get_oY())/2.0,(_p1.get_oZ()+_p2.get_oZ())/2.0,r);
-}
-
-
-/*!
- Set the moving edge parameters.
-
- \param _me : an instance of vpMe containing all the desired parameters
-*/
-void
-vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
-{
- me = _me ;
- if (meline1 != NULL)
- {
- meline1->setMe(me) ;
- }
- if (meline2 != NULL)
- {
- meline2->setMe(me) ;
- }
-}
-
-/*!
- Computes the coordinates of the point corresponding to the intersection
- between a circle and a line from a cylinder's limbo plane.
-
- \param i : A reference to the i coordinate of the point.
- \param j : A reference to the j coordinate of the point.
- \param rho : The rho parameter resulting from the projection of the limbo line considered.
- \param theta : The theta parameter resulting from the projection of the limbo line considered.
- \param circle : A pointer to the circle we consider.
-*/
-void
-vpMbtDistanceCylinder::getCylinderLineExtremity(double &i, double &j,double rho, double theta,
- vpCircle *circle)
-{
-// This was taken from the code of art-v1. (from the artCylinder class)
- double px = cam.get_px() ;
- double py = cam.get_py() ;
- double u0 = cam.get_u0() ;
- double v0 = cam.get_v0() ;
-
- double mu11 = circle->p[3];
- double mu02 = circle->p[4];
- double mu20 = circle->p[2];
- double Xg = u0 + circle->p[0]*px;
- double Yg = v0 + circle->p[1]*py;
-
- // Find Intersection between line and ellipse in the image.
-
- // Optimised calculation for X
- double stheta = sin(theta);
- double ctheta = cos(theta);
- double sctheta = stheta*ctheta;
- double m11yg = mu11*Yg;
- double ctheta2 = vpMath::sqr(ctheta);
- double m02xg = mu02*Xg;
- double m11stheta = mu11*stheta;
- j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
- -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
- m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
- +mu02-mu02*ctheta2));
- //Optimised calculation for Y
- double rhom02 = rho*mu02;
- double sctheta2 = stheta*ctheta2;
- double ctheta3 = ctheta2*ctheta;
- i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
- +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
- +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
- (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
- mu02*ctheta2)/stheta);
-
-}
-
-
-/*!
- Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the lines.
-
- \param I : The image.
- \param cMo : The pose of the camera used to initialize the moving edges.
- \return false if an error occur, true otherwise.
-*/
-bool
-vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- // Perspective projection
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
- cercle1->changeFrame(cMo);
- cercle2->changeFrame(cMo);
- c->changeFrame(cMo);
-
- p1->projection();
- p2->projection();
- try{
- cercle1->projection();
- }
- catch(...){
- //std::cout<<"Problem when projecting circle 1\n";
- return false;
- }
- try{
- cercle2->projection();
- }
- catch(...){
- //std::cout<<"Problem when projecting circle 2\n";
- return false;
- }
- c->projection();
-
- double rho1,theta1;
- double rho2,theta2;
-
- // Create the moving edges containers
- meline1 = new vpMbtMeLine ;
- meline1->setMe(me) ;
- meline2 = new vpMbtMeLine ;
- meline2->setMe(me) ;
-
- // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
- meline1->setInitRange(0);
- meline2->setInitRange(0);
-
- // Conversion meter to pixels
- vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
-
- // Determine intersections between circles and limbos
- double i11,i12,i21,i22,j11,j12,j21,j22;
- getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
- getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
- getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
- getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
-
- // Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
-
- // update limits of the melines.
- int marge = /*10*/5; //ou 5 normalement
- if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
- if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
-
- if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
- if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
-
- // Initialize the tracking
- while (theta1 > M_PI) { theta1 -= M_PI ; }
- while (theta1 < -M_PI) { theta1 += M_PI ; }
-
- if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
- else theta1 = M_PI/2.0 - theta1;
-
- while (theta2 > M_PI) { theta2 -= M_PI ; }
- while (theta2 < -M_PI) { theta2 += M_PI ; }
-
- if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
- else theta2 = M_PI/2.0 - theta2;
-
- try
- {
- meline1->initTracking(I,ip11,ip12,rho1,theta1);
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
- return false;
- }
- try
- {
- meline2->initTracking(I,ip21,ip22,rho2,theta2);
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
- return false;
- }
- }
- return true;
-}
-
-
-
-/*!
- Track the moving edges in the image.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
-{
- if(isvisible){
- try
- {
- meline1->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meline1 failed" << std::endl;
- meline1->reset();
- Reinit = true;
- }
- try
- {
- meline2->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meline2 failed" << std::endl;
- meline2->reset();
- Reinit = true;
- }
-
- // Update the number of features
- nbFeaturel1 = (unsigned int)meline1->getMeList().size();
- nbFeaturel2 = (unsigned int)meline2->getMeList().size();
- nbFeature = nbFeaturel1 + nbFeaturel2;
- }
-}
-
-
-/*!
- Update the moving edges internal parameters.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- // Perspective projection
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
- cercle1->changeFrame(cMo);
- cercle2->changeFrame(cMo);
- c->changeFrame(cMo);
-
- p1->projection();
- p2->projection();
- try{
- cercle1->projection();
- }
- catch(...){std::cout<<"Probleme projection cercle 1\n";}
- try{
- cercle2->projection();
- }
- catch(...){std::cout<<"Probleme projection cercle 2\n";}
- c->projection();
-
- // Get the limbos
- double rho1,theta1;
- double rho2,theta2;
-
- // Conversion meter to pixels
- vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
-
- // Determine intersections between circles and limbos
- double i11,i12,i21,i22,j11,j12,j21,j22;
-
- getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
- getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
-
- getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
- getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
-
- // Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
-
- // update limits of the meline.
- int marge = /*10*/5; //ou 5 normalement
- if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
- if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
-
- if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
- if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
-
- // Initialize the tracking
- while (theta1 > M_PI) { theta1 -= M_PI ; }
- while (theta1 < -M_PI) { theta1 += M_PI ; }
-
- if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
- else theta1 = M_PI/2.0 - theta1;
-
- while (theta2 > M_PI) { theta2 -= M_PI ; }
- while (theta2 < -M_PI) { theta2 += M_PI ; }
-
- if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
- else theta2 = M_PI/2.0 - theta2;
-
- try
- {
- //meline1->updateParameters(I,rho1,theta1) ;
- meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
- }
- catch(...)
- {
- Reinit = true;
- }
- try
- {
- //meline2->updateParameters(I,rho2,theta2) ;
- meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
- }
- catch(...)
- {
- Reinit = true;
- }
-
- // Update the numbers of features
- nbFeaturel1 = (unsigned int)meline1->getMeList().size();
- nbFeaturel2 = (unsigned int)meline2->getMeList().size();
- nbFeature = nbFeaturel1 + nbFeaturel2;
- }
-}
-
-
-/*!
- Reinitialize the cylinder if it is required.
-
- A line is reinitialized if the 2D lines do not match enough with the projected 3D lines.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(meline1!= NULL)
- delete meline1;
- if(meline2!= NULL)
- delete meline2;
-
- meline1 = NULL;
- meline2 = NULL;
-
- if (initMovingEdge(I,cMo) == false)
- Reinit = true;
-
- Reinit = false;
-}
-
-
-/*!
- Display the cylinder. The 3D cylinder is projected into the image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : When true, display the circle even if non visible.
-*/
-void
-vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters&camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel)
-{
- if(isvisible || displayFullModel){
- // Perspective projection
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
- cercle1->changeFrame(cMo);
- cercle2->changeFrame(cMo);
- c->changeFrame(cMo);
-
- p1->projection();
- p2->projection();
- try{
- cercle1->projection();
- }
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
- cercle2->projection();
- }
- catch(...){std::cout<<"Problem projection circle 2";}
- c->projection();
-
- double rho1,theta1;
- double rho2,theta2;
-
- // Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
-
- // Determine intersections between circles and limbos
- double i11,i12,i21,i22,j11,j12,j21,j22;
-
- getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
- getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
-
- getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
- getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
-
- // Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
-
- // Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
- }
-}
-
-/*!
- Display the cylinder. The 3D cylinder is projected into the image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : When true, display the circle even if non visible.
-*/
-void
-vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel)
-{
- if(isvisible || displayFullModel){
- // Perspective projection
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
- cercle1->changeFrame(cMo);
- cercle2->changeFrame(cMo);
- c->changeFrame(cMo);
-
- p1->projection();
- p2->projection();
- try{
- cercle1->projection();
- }
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
- cercle2->projection();
- }
- catch(...){std::cout<<"Problem projection circle 2";}
- c->projection();
-
- double rho1,theta1;
- double rho2,theta2;
-
- // Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
-
- // Determine intersections between circles and limbos
- double i11,i12,i21,i22,j11,j12,j21,j22;
-
- getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
- getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
-
- getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
- getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
-
- // Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
-
- // Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
- }
-}
-
-
-/*!
- Enable to display the points along the lines 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 (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
- \param I : The image.
-*/
-void
-vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
-{
- if (meline1 != NULL)
- {
- meline1->display(I);
- }
- if (meline2 != NULL)
- {
- meline2->display(I);
- }
-}
-
-/*!
- Initialize the size of the interaction matrix and the error vector.
-*/
-void
-vpMbtDistanceCylinder::initInteractionMatrixError()
-{
- if (isvisible == true) {
- nbFeaturel1 = (unsigned int)meline1->getMeList().size();
- nbFeaturel2 = (unsigned int)meline2->getMeList().size();
- nbFeature = nbFeaturel1 + nbFeaturel2;
- L.resize(nbFeature, 6);
- error.resize(nbFeature);
- }
- else {
- nbFeature = 0 ;
- nbFeaturel1 = 0;
- nbFeaturel2 = 0;
- }
-}
-
-/*!
- Compute the interaction matrix and the error vector corresponding to the cylinder.
-*/
-void
-vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
-{
- if (isvisible) {
- // Perspective projection
- c->changeFrame(cMo) ;
- c->projection() ;
- cercle1->changeFrame(cMo) ;
- cercle1->changeFrame(cMo) ;
- try{
- cercle1->projection();
- }
- catch(...){std::cout<<"Problem projection circle 1\n";}
- try{
- cercle2->projection();
- }
- catch(...){std::cout<<"Problem projection circle 2\n";}
-
- bool disp = false;
- bool disp2 = false;
- if (disp || disp2) vpDisplay::flush(I);
-
- // Build the lines
- vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
- vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
-
- double rho1 = featureline1.getRho() ;
- double theta1 = featureline1.getTheta() ;
- double rho2 = featureline2.getRho() ;
- double theta2 = featureline2.getTheta() ;
-
- double co1 = cos(theta1);
- double si1 = sin(theta1);
- double co2 = cos(theta2);
- double si2 = sin(theta2);
-
- double mx = 1.0/cam.get_px() ;
- double my = 1.0/cam.get_py() ;
- double xc = cam.get_u0() ;
- double yc = cam.get_v0() ;
-
- double alpha1 ;
- vpMatrix H1 ;
- H1 = featureline1.interaction() ;
- double alpha2 ;
- vpMatrix H2 ;
- H2 = featureline2.interaction() ;
-
- double x,y ;
- vpMeSite p ;
- unsigned int j =0 ;
- for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
- x = (double)it->j;
- y = (double)it->i;
-
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
-
- alpha1 = x*si1 - y*co1;
-
- double *Lrho = H1[0] ;
- double *Ltheta = H1[1] ;
- // Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++){
- L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
- }
- error[j] = rho1 - ( x*co1 + y*si1) ;
-
- if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
-
- j++;
- }
-
- for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
- x = (double)it->j;
- y = (double)it->i;
-
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
-
- alpha2 = x*si2 - y*co2;
-
- double *Lrho = H2[0] ;
- double *Ltheta = H2[1] ;
- // Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++){
- L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
- }
- error[j] = rho2 - ( x*co2 + y*si2) ;
-
- if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
-
- j++;
- }
- }
-}
-
diff --git a/src/tracking/mbt/edge/vpMbtDistanceCylinder.h b/src/tracking/mbt/edge/vpMbtDistanceCylinder.h
deleted file mode 100755
index 0c272b9..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceCylinder.h
+++ /dev/null
@@ -1,244 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceCylinder.h 4914 2014-10-02 13:25:47Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Manage a cylinder used in the model-based tracker.
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Bertrand Delabarre
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtDistanceCylinder.h
- \brief Manage a cylinder used in the model-based tracker.
-*/
-
-#ifndef vpMbtDistanceCylinder_HH
-#define vpMbtDistanceCylinder_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMbtMeLine.h>
-#include <visp/vpLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpCylinder.h>
-#include <visp/vpCircle.h>
-#include <visp/vpMbHiddenFaces.h>
-
-/*!
- \class vpMbtDistanceCylinder
-
- \brief Manage a cylinder used in the model-based tracker.
-
- \ingroup ModelBasedTracking
-*/
-class VISP_EXPORT vpMbtDistanceCylinder
-{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- double alpha;
- double wmean1;
- double wmean2;
- vpFeatureLine featureline1 ;
- vpFeatureLine featureline2 ;
-
- public:
- //! The moving edge containers (first line of the cylinder)
- vpMbtMeLine *meline1;
- //! The moving edge containers (second line of the cylinder)
- vpMbtMeLine *meline2;
-
- //! The upper circle limiting the cylinder
- vpCircle *cercle1;
- //! The lower circle limiting the cylinder
- vpCircle *cercle2;
-
- //! The radius of the cylinder
- double radius;
-
- //! The first extremity on the axe
- vpPoint *p1;
- //! The second extremity on the axe
- vpPoint *p2;
-
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- unsigned int nbFeature;
- //! The number of moving edges on line 1
- unsigned int nbFeaturel1;
- //! The number of moving edges on line 2
- unsigned int nbFeaturel2;
- //! Indicates if the line has to be reinitialized
- bool Reinit;
- //! The cylinder
- vpCylinder *c;
-
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the face which contains the cylinder
- int index_polygon;
- //! Indicates if the cylinder is visible or not
- bool isvisible;
-
- public:
- vpMbtDistanceCylinder() ;
- ~vpMbtDistanceCylinder() ;
-
- void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r);
-
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
-
- /*!
- Get the index of the cylinder.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() {return index ;}
-
- /*!
- Get the mean weight of the first line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the first line.
- */
- inline double getMeanWeight1() const {return wmean1;}
-
- /*!
- Get the mean weight of the second line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the second line.
- */
- inline double getMeanWeight2() const {return wmean2;}
-
- /*!
- Get the name of the cylinder.
-
- \return Return the name of the cylinder
- */
- inline std::string getName() const {return name;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Check if the cylinder is visible in the image or not.
-
- \return Return true if the cylinder is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set the index of the cylinder.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the first line.
-
- \param wmean : The mean weight of the first line.
- */
- inline void setMeanWeight1(const double wmean) {this->wmean1 = wmean;}
-
- /*!
- Set the mean weight of the second line.
-
- \param wmean : The mean weight of the second line.
- */
- inline void setMeanWeight2(const double wmean) {this->wmean2 = wmean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the cylinder.
-
- \param cyl_name : The name of the cylinder.
- */
- inline void setName(const std::string& cyl_name) {this->name = cyl_name;}
-
- /*!
- Set the name of the cylinder.
-
- \param cyl_name : The name of the cylinder
- */
- inline void setName(const char* cyl_name) {this->name = std::string(cyl_name);}
-
- /*!
- Set a boolean parameter to indicates if the cylinder is visible in the image or not.
-
- \param _isvisible : Set to true if the cylinder is visible
- */
- inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- private:
- void getCylinderLineExtremity(double &i, double &j,double rho, double theta, vpCircle *circle);
- void project(const vpHomogeneousMatrix &cMo);
-} ;
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtDistanceLine.cpp b/src/tracking/mbt/edge/vpMbtDistanceLine.cpp
deleted file mode 100644
index 78adf27..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceLine.cpp
+++ /dev/null
@@ -1,634 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceLine.cpp 5136 2015-01-08 10:09:04Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-/*!
- \file vpMbtDistanceLine.cpp
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-
-#include <visp/vpMbtDistanceLine.h>
-#include <visp/vpPlane.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpFeatureBuilder.h>
-#include <stdlib.h>
-
-void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
-void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
-
-/*!
- Basic constructor
-*/
-vpMbtDistanceLine::vpMbtDistanceLine()
- : name(), index(0), cam(), me(NULL), alpha(0), wmean(1),
- featureline(), poly(), meline(NULL), line(NULL), p1(NULL), p2(NULL), L(),
- error(), nbFeature(0), Reinit(false), hiddenface(NULL), Lindex_polygon(),
- isvisible(false)
-{
-}
-
-/*!
- Basic destructor useful to deallocate the memory.
-*/
-vpMbtDistanceLine::~vpMbtDistanceLine()
-{
-// cout << "Deleting line " << index << endl ;
- if (line != NULL) delete line ;
- if (meline != NULL) delete meline ;
-
-}
-
-/*!
- Project the line and the two points corresponding to its extremities into the image.
-
- \param cMo : The pose of the camera used to project the line into the image.
-*/
-void
-vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
-{
- line->project(cMo) ;
- p1->project(cMo) ;
- p2->project(cMo) ;
-}
-
-/*!
- Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
-
- \param P : The first point to define the plane
- \param Q : The second point to define the plane
- \param R : The third point to define the plane
- \param plane : The vpPlane instance used to store the computed plane equation.
-*/
-void
-buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
-{
- vpColVector a(3);
- vpColVector b(3);
- vpColVector n(3);
- //Calculate vector corresponding to PQ
- a[0]=P.get_oX()-Q.get_oX();
- a[1]=P.get_oY()-Q.get_oY();
- a[2]=P.get_oZ()-Q.get_oZ();
-
- //Calculate vector corresponding to PR
- b[0]=P.get_oX()-R.get_oX();
- b[1]=P.get_oY()-R.get_oY();
- b[2]=P.get_oZ()-R.get_oZ();
-
- //Calculate normal vector to plane PQ x PR
- n=vpColVector::cross(a,b);
-
- //Equation of the plane is given by:
- double A = n[0];
- double B = n[1];
- double C = n[2];
- double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
-
- double norm = sqrt(A*A+B*B+C*C) ;
- plane.setA(A/norm) ;
- plane.setB(B/norm) ;
- plane.setC(C/norm) ;
- plane.setD(D/norm) ;
-}
-
-
-/*!
- Build a line thanks to 4 points.
-
- The method is the following : Two plane are computed thanks to (P1,P2,P3) and (P1,P2,P4) (see the buildPlane method). Then the line equation is computed thanks to the intersection between the two planes.
-
- \param P1 : The first point to compute the line.
- \param P2 : The second point to compute the line.
- \param P3 : The third point to compute the line.
- \param P4 : The fourth point to compute the line.
- \param L : The instance of vpLine to store the computed line equation.
-*/
-void
-buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
-{
- vpPlane plane1;
- vpPlane plane2 ;
- buildPlane(P1,P2,P3,plane1) ;
- buildPlane(P1,P2,P4,plane2) ;
-
- L.setWorldCoordinates(plane1.getA(),plane1.getB(), plane1.getC(),plane1.getD(),
- plane2.getA(),plane2.getB(), plane2.getC(),plane2.getD()) ;
-}
-
-
-/*!
- Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
-
- \param _p1 : The first extremity.
- \param _p2 : The second extremity.
-*/
-void
-vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
-{
- line = new vpLine ;
- poly.setNbPoint(2);
- poly.addPoint(0, _p1);
- poly.addPoint(1, _p2);
-
- p1 = &poly.p[0];
- p2 = &poly.p[1];
-
- vpColVector V1(3);
- vpColVector V2(3);
- vpColVector V3(3);
- vpColVector V4(3);
-
- V1[0] = p1->get_oX();
- V1[1] = p1->get_oY();
- V1[2] = p1->get_oZ();
- V2[0] = p2->get_oX();
- V2[1] = p2->get_oY();
- V2[2] = p2->get_oZ();
-
- //if((V1-V2).sumSquare()!=0)
- if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
- {
- {
- V3[0]=double(rand()%1000)/100;
- V3[1]=double(rand()%1000)/100;
- V3[2]=double(rand()%1000)/100;
-
-
- vpColVector v_tmp1,v_tmp2;
- v_tmp1 = V2-V1;
- v_tmp2 = V3-V1;
- V4=vpColVector::cross(v_tmp1,v_tmp2);
- }
-
- vpPoint P3;
- P3.setWorldCoordinates(V3[0],V3[1],V3[2]);
- vpPoint P4;
- P4.setWorldCoordinates(V4[0],V4[1],V4[2]);
- buildLine(*p1,*p2, P3,P4, *line) ;
- }
- else
- {
- vpPoint P3;
- P3.setWorldCoordinates(V1[0],V1[1],V1[2]);
- vpPoint P4;
- P4.setWorldCoordinates(V2[0],V2[1],V2[2]);
- buildLine(*p1,*p2,P3,P4,*line) ;
- }
-}
-
-
-/*!
- Set the moving edge parameters.
-
- \param _me : an instance of vpMe containing all the desired parameters
-*/
-void
-vpMbtDistanceLine::setMovingEdge(vpMe *_me)
-{
- me = _me ;
- if (meline != NULL)
- {
- meline->reset();
- meline->setMe(me) ;
- }
-}
-
-
-/*!
- Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the line.
-
- \param I : The image.
- \param cMo : The pose of the camera used to initialize the moving edges.
- \return false if an error occur, true otherwise.
-*/
-bool
-vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
-
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
- cam.computeFov(I.getWidth(), I.getHeight());
-
- poly.computeRoiClipped(cam);
-
- if(poly.roiPointsClip.size() == 2){ //Les points sont visibles.
- vpImagePoint ip1, ip2;
- double rho,theta;
- line->changeFrame(cMo);
- line->projection();
-
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
-
- //rho theta uv
- vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
-
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < -M_PI) { theta += M_PI ; }
-
- if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
- else theta = M_PI/2.0 - theta;
-
- meline = new vpMbtMeLine ;
- meline->setMe(me) ;
-
- // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
- meline->setInitRange(0);
-
- int marge = /*10*/5; //ou 5 normalement
- if (ip1.get_j()<ip2.get_j()) { meline->jmin = (int)ip1.get_j()-marge ; meline->jmax = (int)ip2.get_j()+marge ; } else{ meline->jmin = (int)ip2.get_j()-marge ; meline->jmax = (int)ip1.get_j()+marge ; }
- if (ip1.get_i()<ip2.get_i()) { meline->imin = (int)ip1.get_i()-marge ; meline->imax = (int)ip2.get_i()+marge ; } else{ meline->imin = (int)ip2.get_i()-marge ; meline->imax = (int)ip1.get_i()+marge ; }
-
- try
- {
- meline->initTracking(I,ip1,ip2,rho,theta);
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
- return false;
- }
- }
- else{
- if (meline!=NULL) delete meline;
- meline=NULL;
- isvisible = false;
- }
- }
-// trackMovingEdge(I,cMo) ;
- return true;
-}
-
-
-
-/*!
- Track the moving edges in the image.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
-{
-
- if (isvisible)
- {
-// p1->changeFrame(cMo) ;
-// p2->changeFrame(cMo) ;
-//
-// p1->projection() ;
-// p2->projection() ;
-//
-// vpImagePoint ip1, ip2;
-//
-// vpMeterPixelConversion::convertPoint(*cam,p1->get_x(),p1->get_y(),ip1) ;
-// vpMeterPixelConversion::convertPoint(*cam,p2->get_x(),p2->get_y(),ip2) ;
-//
-// int marge = /*10*/5; //ou 5 normalement
-// if (ip1.get_j()<ip2.get_j()) { meline->jmin = ip1.get_j()-marge ; meline->jmax = ip2.get_j()+marge ; }
-// else{ meline->jmin = ip2.get_j()-marge ; meline->jmax = ip1.get_j()+marge ; }
-// if (ip1.get_i()<ip2.get_i()) { meline->imin = ip1.get_i()-marge ; meline->imax = ip2.get_i()+marge ; }
-// else{ meline->imin = ip2.get_i()-marge ; meline->imax = ip1.get_i()+marge ; }
-
- try
- {
- meline->track(I) ;
- }
- catch(...)
- {
- meline->reset();
- Reinit = true;
- }
- nbFeature =(unsigned int) meline->getMeList().size();
- }
-}
-
-
-/*!
- Update the moving edges internal parameters.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(isvisible){
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
-
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
- cam.computeFov(I.getWidth(), I.getHeight());
-
- poly.computeRoiClipped(cam);
-
- if(poly.roiPointsClip.size() == 2){ //Les points sont visibles.
- vpImagePoint ip1, ip2;
- double rho,theta;
- line->changeFrame(cMo);
- line->projection();
-
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
-
- //rho theta uv
- vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
-
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < -M_PI) { theta += M_PI ; }
-
- if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
- else theta = M_PI/2.0 - theta;
-
- int marge = /*10*/5; //ou 5 normalement
- if (ip1.get_j()<ip2.get_j()) { meline->jmin = (int)ip1.get_j()-marge ; meline->jmax = (int)ip2.get_j()+marge ; } else{ meline->jmin = (int)ip2.get_j()-marge ; meline->jmax = (int)ip1.get_j()+marge ; }
- if (ip1.get_i()<ip2.get_i()) { meline->imin = (int)ip1.get_i()-marge ; meline->imax = (int)ip2.get_i()+marge ; } else{ meline->imin = (int)ip2.get_i()-marge ; meline->imax = (int)ip1.get_i()+marge ; }
-
- try
- {
- //meline->updateParameters(I,rho,theta) ;
- meline->updateParameters(I,ip1,ip2,rho,theta) ;
- }
- catch(...)
- {
- Reinit = true;
- }
- nbFeature = (unsigned int)meline->getMeList().size();
- }
- else{
- if (meline!=NULL) delete meline;
- meline=NULL;
- isvisible = false;
- }
- }
-}
-
-
-/*!
- Reinitialize the line if it is required.
-
- A line is reinitialized if the 2D line do not match enough with the projected 3D line.
-
- \param I : the image.
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
-{
- if(meline!= NULL)
- delete meline;
-
- if (initMovingEdge(I,cMo) == false)
- Reinit = true;
-
- Reinit = false;
-}
-
-
-/*!
- Display the line. The 3D line is projected into the image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the line.
- \param displayFullModel : If true, the line is displayed even if it is not visible.
-*/
-void
-vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness, const bool displayFullModel)
-{
- if(isvisible || displayFullModel){
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
-
- vpImagePoint ip1, ip2;
- vpCameraParameters c = camera;
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
-
- poly.computeRoiClipped(c);
-
- if( poly.roiPointsClip.size() == 2 &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
-
- vpDisplay::displayLine(I,ip1,ip2,col, thickness);
- }
- }
-}
-
-
-/*!
- Display the line. The 3D line is projected into the image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the line.
- \param displayFullModel : If true, the line is displayed even if it is not visible.
-*/
-void
-vpMbtDistanceLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col,
- const unsigned int thickness, const bool displayFullModel)
-{
- if(isvisible || displayFullModel){
- p1->changeFrame(cMo);
- p2->changeFrame(cMo);
-
- vpImagePoint ip1, ip2;
- vpCameraParameters c = camera;
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
-
- poly.computeRoiClipped(c);
-
- if( poly.roiPointsClip.size() == 2 &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
- ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
-
- vpDisplay::displayLine(I,ip1,ip2,col, thickness);
- }
- }
-}
-
-
-/*!
- Enable to display the points along the line 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 (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
- \param I : The image.
-*/
-void
-vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
-{
- if (meline != NULL)
- {
- meline->display(I);
- }
-}
-
-/*!
- Initialize the size of the interaction matrix and the error vector.
-*/
-void
-vpMbtDistanceLine::initInteractionMatrixError()
-{
- if (isvisible == true)
- {
- L.resize((unsigned int)meline->getMeList().size(),6) ;
- error.resize((unsigned int)meline->getMeList().size()) ;
- nbFeature = (unsigned int)meline->getMeList().size() ;
- }
- else
- nbFeature = 0 ;
-}
-
-/*!
- Compute the interaction matrix and the error vector corresponding to the line.
-*/
-void
-vpMbtDistanceLine::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
-{
-
- if (isvisible)
- {
- // feature projection
- line->changeFrame(cMo) ;
- line->projection() ;
-
- vpFeatureBuilder::create(featureline,*line) ;
-
- double rho = featureline.getRho() ;
- double theta = featureline.getTheta() ;
-
- double co = cos(theta);
- double si = sin(theta);
-
- double mx = 1.0/cam.get_px() ;
- double my = 1.0/cam.get_py() ;
- double xc = cam.get_u0() ;
- double yc = cam.get_v0() ;
-
- double alpha_ ;
- vpMatrix H ;
- H = featureline.interaction() ;
-
- double x,y ;
- vpMeSite p ;
- unsigned int j =0 ;
- for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
- x = (double)it->j ;
- y = (double)it->i ;
-
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
-
- alpha_ = x*si - y*co;
-
- double *Lrho = H[0] ;
- double *Ltheta = H[1] ;
- // Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++)
- {
- L[j][k] = (Lrho[k] + alpha_*Ltheta[k]);
- }
- error[j] = rho - ( x*co + y*si) ;
- j++;
- }
- }
-}
-
-/*!
- Test wether the line is close to the border of the image (at a given threshold)
-
- \param I : the input image (to know its size)
- \param threshold : the threshold in pixel
- \return true if the line is near the border of the image
-*/
-bool
-vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold)
-{
- if(threshold > I.getWidth() || threshold > I.getHeight()){
- return true;
- }
- if (isvisible){
-
- for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
- int i = it->i ;
- int j = it->j ;
-
- if(i < 0 || j < 0){ //out of image.
- return true;
- }
-
- if( ((unsigned int)i > (I.getHeight()- threshold) ) || (unsigned int)i < threshold ||
- ((unsigned int)j > (I.getWidth ()- threshold) ) || (unsigned int)j < threshold ) {
- return true;
- }
- }
- }
- return false;
-}
diff --git a/src/tracking/mbt/edge/vpMbtDistanceLine.h b/src/tracking/mbt/edge/vpMbtDistanceLine.h
deleted file mode 100644
index 336e27d..0000000
--- a/src/tracking/mbt/edge/vpMbtDistanceLine.h
+++ /dev/null
@@ -1,220 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceLine.h 4802 2014-07-24 09:24:43Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Manage the line of a polygon used in the model-based tracker.
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtDistanceLine.h
- \brief Manage the line of a polygon used in the model-based tracker.
-*/
-
-#ifndef vpMbtDistanceLine_HH
-#define vpMbtDistanceLine_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMbtMeLine.h>
-#include <visp/vpLine.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpFeatureLine.h>
-#include <visp/vpMbHiddenFaces.h>
-
-#include <list>
-
-/*!
- \class vpMbtDistanceLine
-
- \brief Manage the line of a polygon used in the model-based tracker.
-
- \ingroup ModelBasedTracking
-
- */
-class VISP_EXPORT vpMbtDistanceLine
-{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- double alpha;
- double wmean;
- vpFeatureLine featureline ;
- //! Polygon describing the line
- vpMbtPolygon poly;
-
- public:
- //! The moving edge container
- vpMbtMeLine *meline;
- //! The 3D line
- vpLine *line;
- //! The first extremity
- vpPoint *p1;
- //! The second extremity
- vpPoint *p2;
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- unsigned int nbFeature;
- //! Indicates if the line has to be reinitialized
- bool Reinit;
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the faces which contain the line
- std::list<int> Lindex_polygon;
- //! Indicates if the line is visible or not
- bool isvisible;
-
- public:
- vpMbtDistanceLine() ;
- ~vpMbtDistanceLine() ;
-
- void buildFrom(vpPoint &_p1, vpPoint &_p2);
-
- bool closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold);
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) const {camera = this->cam;}
-
- /*!
- Get the index of the line.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() const {return index ;}
-
- /*!
- Get the mean weight of the line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the line.
- */
- inline double getMeanWeight() const {return wmean;}
-
- /*!
- Get the name of the line.
-
- \return Return the name of the line
- */
- inline std::string getName() const {return name;}
-
- /*!
- Get the polygon associated to the line.
-
- \return poly.
- */
- inline vpMbtPolygon& getPolygon() {return poly;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Check if the line is visible in the image or not.
-
- \return Return true if the line is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set the index of the line.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the line.
-
- \param w_mean : The mean weight of the line.
- */
- inline void setMeanWeight(const double w_mean) {this->wmean = w_mean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the line.
-
- \param line_name : The name of the line.
- */
- inline void setName(const std::string line_name) {this->name = line_name;}
-
- /*!
- Set the name of the line.
-
- \param line_name : The name of the line.
- */
- inline void setName(const char* line_name) {this->name = std::string(line_name);}
-
- /*!
- Set a boolean parameter to indicates if the line is visible in the image or not.
-
- \param _isvisible : Set to true if the line is visible
- */
- inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- private:
- void project(const vpHomogeneousMatrix &cMo);
-} ;
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtMeEllipse.cpp b/src/tracking/mbt/edge/vpMbtMeEllipse.cpp
deleted file mode 100644
index 2fe646b..0000000
--- a/src/tracking/mbt/edge/vpMbtMeEllipse.cpp
+++ /dev/null
@@ -1,359 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtMeEllipse.cpp 4706 2014-03-28 07:52:00Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp/vpMbtMeEllipse.h>
-
-#include <visp/vpMe.h>
-#include <visp/vpRobust.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpImagePoint.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-
-/*!
- Basic constructor that calls the constructor of the class vpMeTracker.
-*/
-vpMbtMeEllipse::vpMbtMeEllipse()
- : iPc(), a(0.), b(0.), e(0.),
- ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
-{
-}
-
-/*!
- Copy constructor.
-*/
-vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse)
- : vpMeTracker(meellipse), iPc(), a(0.), b(0.), e(0.),
- ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
-{
- iPc = meellipse.iPc;
- a = meellipse.a;
- b = meellipse.b;
- e = meellipse.e;
-
- ce = meellipse.ce;
- se = meellipse.se;
-
- mu11 = meellipse.mu11;
- mu20 = meellipse.mu20;
- mu02 = meellipse.mu02;
-
- expecteddensity = meellipse.expecteddensity;
-}
-
-/*!
- Basic destructor.
-*/
-vpMbtMeEllipse::~vpMbtMeEllipse()
-{
- list.clear();
-}
-
-/*!
- 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.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMbtMeEllipse::sample(const vpImage<unsigned char> & I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- int height = (int)I.getHeight() ;
- int width = (int)I.getWidth() ;
-
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "In vpMbtMeEllipse::sample: " ;
- std::cout << "function called with sample step = 0" ;
- //return fatalError ;
- }
-
- double j, i;//, j11, i11;
- vpImagePoint iP11;
- j = i = 0.0 ;
-
- // Approximation of the circumference of an ellipse:
- // [Ramanujan, S., "Modular Equations and Approximations to ,"
- // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
- double t = (a-b)/(a+b);
- double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
- int nb_points_to_track = (int)(circumference / me->getSampleStep());
- double incr = 2*M_PI/nb_points_to_track;
-
- expecteddensity = 0;//nb_points_to_track;
-
- // Delete old list
- list.clear();
-
- // sample positions
- double k = 0 ;
- double iP_i, iP_j;
- for (int pt=0; pt < nb_points_to_track; pt++)
- {
- j = a *cos(k) ; // equation of an ellipse
- i = b *sin(k) ; // equation of an ellipse
-
- iP_j = iPc.get_j() + ce *j - se *i;
- iP_i = iPc.get_i() + se *j + ce *i;
-
- //vpColor col = vpColor::red ;
- //vpDisplay::displayCross(I, vpImagePoint(iP_i, iP_j), 5, col) ; //debug only
-
- // If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(iP_i), vpMath::round(iP_j), 0, height, width))
- {
- // The tangent angle to the ellipse at a site
- double theta = atan( (-mu02*iP_j + mu02*iPc.get_j() + mu11*iP_i - mu11*iPc.get_i())
- / (mu20*iP_i - mu11*iP_j + mu11*iPc.get_j() - mu20*iPc.get_i()))
- - M_PI/2;
-
- vpMeSite pix ;
- pix.init((int)iP_i, (int)iP_j, theta) ;
- pix.setDisplay(selectDisplay) ;
- pix.setState(vpMeSite::NO_SUPPRESSION);
-
- list.push_back(pix);
- expecteddensity ++;
- }
- k += incr ;
-
- }
-
- vpMeTracker::initTracking(I) ;
-}
-
-
-/*!
-
- Resample the ellipse if the number of sample is less than 90% of the
- expected value.
-
- \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
- and the parameter which indicates the number of degrees between
- two points (vpMe::sample_step).
-
- \param I : Image in which the ellipse appears.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- unsigned int n = numberOfSignal() ;
- if ((double)n<0.9*expecteddensity){
- sample(I) ;
- vpMeTracker::track(I) ;
- }
-}
-
-/*!
- Compute the oriantation angle for each vpMeSite.
-
- \note The \f$ \theta \f$ angle is useful during the tracking part.
-*/
-void
-vpMbtMeEllipse::updateTheta()
-{
- vpMeSite p_me;
- double theta;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- vpImagePoint iP;
- iP.set_i(p_me.ifloat);
- iP.set_j(p_me.jfloat);
-
- // The tangent angle to the ellipse at a site
- theta = atan( (-mu02*p_me.jfloat + mu02*iPc.get_j() + mu11*p_me.ifloat - mu11*iPc.get_i())
- / (mu20*p_me.ifloat - mu11*p_me.jfloat + mu11*iPc.get_j() - mu20*iPc.get_i()))
- - M_PI/2;
-
- p_me.alpha = theta ;
- *it = p_me;
- }
-}
-
-/*!
- Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
-*/
-void
-vpMbtMeEllipse::suppressPoints()
-{
- // Loop through list of sites to track
- for(std::list<vpMeSite>::iterator itList=list.begin(); itList!=list.end();){
- vpMeSite s = *itList;//current reference pixel
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- itList = list.erase(itList);
- else
- ++itList;
- }
-}
-
-/*!
- Display the 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.
- */
-void
-vpMbtMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
-{
- vpDisplay::displayEllipse(I, iPc, mu20, mu11, mu02, true, col);
-}
-
-void
-vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p)
-{
- iPc = ic;
- mu20 = mu20_p;
- mu11 = mu11_p;
- mu02 = mu02_p;
-
- if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
-
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
-
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- }
- else {
- a = sqrt(mu20_p);
- b = sqrt(mu02_p);
- e = 0.;
- }
-
- e = atan(e);
-
- ce = cos(e);
- se = sin(e);
-
- sample(I) ;
-
- vpMeTracker::initTracking(I) ;
-
- try{
- track(I) ;
- }
- catch(vpException &exception)
- {
- throw(exception) ;
- }
-}
-
-/*!
- Track the ellipse in the image I.
-
- \param I : Image in which the ellipse appears.
-*/
-void
-vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
-{
- try{
- vpMeTracker::track(I) ;
- }
- catch(vpException &exception)
- {
- throw(exception) ;
- }
-}
-
-void
-vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p, double mu02_p)
-{
- iPc = ic;
- mu20 = mu20_p;
- mu11 = mu11_p;
- mu02 = mu02_p;
-
- if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
-
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
-
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- }
- else {
- a = sqrt(mu20_p);
- b = sqrt(mu02_p);
- e = 0.;
- }
-
- e = atan(e);
-
- ce = cos(e);
- se = sin(e);
-
- suppressPoints();
- reSample(I);
-
- // remet a jour l'angle delta pour chaque point de la liste
- updateTheta();
-}
-
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/src/tracking/mbt/edge/vpMbtMeEllipse.h b/src/tracking/mbt/edge/vpMbtMeEllipse.h
deleted file mode 100644
index 87f11b7..0000000
--- a/src/tracking/mbt/edge/vpMbtMeEllipse.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtMeEllipse.h 4705 2014-03-27 16:24:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtMeEllipse.h
- \brief Moving edges on an ellipse
-*/
-
-#ifndef vpMbtMeEllipse_HH
-#define vpMbtMeEllipse_HH
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-
-#include <visp/vpMeTracker.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpImagePoint.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpColor.h>
-
-#include <math.h>
-#include <list>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
- \class vpMbtMeEllipse
- \ingroup TrackingImageME
-
- \brief Class that tracks an ellipse moving edges.
-
- In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
-
- \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
-
- If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
-
- The five parameters are stored in the public attribute K.
-
- An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
- major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
-
- \image html vpMbtMeEllipse.gif
- \image latex vpMbtMeEllipse.ps width=10cm
-
- It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
-
- \f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
- \f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
-
- Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
-
- \image html vpMbtMeEllipse2.gif
- \image latex vpMbtMeEllipse2.ps width=10cm
-
- The example below available in tutorial-me-ellipse-tracker.cpp and described
- in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
-
- \include tutorial-me-ellipse-tracker.cpp
-
- */
-/*
- The code below shows how to use this class.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpMbtMeEllipse.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImage<unsigned char> I;
-
- // I is the image containing the ellipse to track
-
- // Set the moving-edges tracker parameters
- vpMe me;
- me.setRange(25);
- me.setPointsToTrack(20);
- me.setThreshold(15000);
- me.setSampleStep(10);
-
- // Initialize the moving-edges ellipse tracker parameters
- vpMbtMeEllipse ellipse;
- ellipse.setMe(&me);
-
- // Initialize the tracking. You have to click on five different points belonging to the ellipse.
- ellipse.initTracking(I);
-
- while ( 1 )
- {
- // ... Here the code to read or grab the next image.
-
- // Track the ellipse.
- ellipse.track(I);
- }
- return 0;
-}
-\endcode
-
- \note It is possible to display the ellipse as an overlay. For that you
- must use the display function of the class vpMbtMeEllipse.
-*/
-
-class VISP_EXPORT vpMbtMeEllipse : public vpMeTracker
-{
-public:
- vpMbtMeEllipse() ;
- vpMbtMeEllipse(const vpMbtMeEllipse &meellipse) ;
- virtual ~vpMbtMeEllipse() ;
-
-
- void display(const vpImage<unsigned char>&I, vpColor col) ;
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
- /*!
- \return Expected number of moving edges to track along the ellipse.
- */
- int getExpectedDensity() {return (int)expecteddensity;};
-
- /*!
- Gets the 2 order central moment \f$ \mu_{11} \f$.
-
- \return the value of \f$ \mu_{11} \f$.
- */
- inline double get_mu11() const {return mu11;}
-
- /*!
- Gets the 2 order central moment \f$ \mu_{02} \f$.
-
- \return the value of \f$ \mu_{02} \f$.
- */
- inline double get_mu02() const {return mu02;}
-
- /*!
- Gets the 2 order central moment \f$ \mu_{20} \f$.
-
- \return the value of \f$ \mu_{20} \f$.
- */
- inline double get_mu20() const {return mu20;}
-
- /*!
- Gets the center of the ellipse.
- */
- inline vpImagePoint getCenter() const {return iPc; }
-
- /*!
- Gets the semiminor axis of the ellipse.
- */
- inline double getA() const {return a; }
-
- /*!
- Gets the semimajor axis of the ellipse.
- */
- inline double getB() const {return b; }
-
- /*!
- Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
- */
- inline double getE() const {return e; }
-
- /*!
- Gets the parameters a, b, e of the ellipse.
- */
- void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
-
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p) ;
-
- void track(const vpImage<unsigned char>& Im);
-
- void updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p);
-
-
-protected:
- //! The coordinates of the ellipse center.
- vpImagePoint iPc;
- //! \f$ a \f$ is the semiminor axis of the ellipse.
- double a;
- //! \f$ b \f$ is the semimajor axis of the ellipse.
- double b;
- //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
- double e;
-
-protected:
- //! Value of cos(e).
- double ce;
- //! Value of sin(e).
- double se;
-
- //! Second order central moments
- double mu11,mu20, mu02;
- //! Threshold for the robust least square.
- double thresholdWeight;
- //! Expected number of me to track along the ellipse.
- double expecteddensity;
-
-private:
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void updateTheta();
- void suppressPoints() ;
-};
-
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-#endif
diff --git a/src/tracking/mbt/edge/vpMbtMeLine.cpp b/src/tracking/mbt/edge/vpMbtMeLine.cpp
deleted file mode 100644
index 6a9c96f..0000000
--- a/src/tracking/mbt/edge/vpMbtMeLine.cpp
+++ /dev/null
@@ -1,687 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtMeLine.cpp 5216 2015-01-28 08:03:09Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
- \file vpMbtMeLine.cpp
- \brief Make the complete tracking of an object by using its CAD model.
-*/
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-#include <visp/vpMbtMeLine.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpRobust.h>
-
-//! Normalize an angle between -Pi and Pi
-static void
-normalizeAngle(double &delta)
-{
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < -M_PI) { delta += M_PI ; }
-}
-
-
-/*!
- Basic constructor that calls the constructor of the class vpMeTracker.
-*/
-vpMbtMeLine::vpMbtMeLine()
- : rho(0.), theta(0.), theta_1(M_PI/2), delta(0.), delta_1(0), sign(1),
- a(0.), b(0.), c(0.), imin(0), imax(0), jmin(0), jmax(0),
- expecteddensity(0.)
-{
-}
-
-/*!
- Basic destructor.
-*/
-vpMbtMeLine::~vpMbtMeLine()
-{
- list.clear();
-}
-
-/*!
- Initialization of the tracking. The line is defined thanks to the
- coordinates of two points corresponding to the extremities and its (\f$\rho \: \theta\f$) parameters.
-
- Remember the equation of a line : \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \param I : Image in which the line appears.
- \param ip1 : Coordinates of the first point.
- \param ip2 : Coordinates of the second point.
- \param rho_ : The \f$\rho\f$ parameter
- \param theta_ : The \f$\theta\f$ parameter
-*/
-void
-vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- double rho_, double theta_)
-{
- vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
-
- try
- {
- // 1. On fait ce qui concerne les droites (peut etre vide)
- // Points extremites
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
-
- this->rho = rho_;
- this->theta = theta_;
- theta_1 = theta_;
-
- a = cos(theta);
- b = sin(theta);
- c = -rho;
-
- delta = - theta + M_PI/2.0;
- normalizeAngle(delta);
- delta_1 = delta;
-
- sample(I) ;
- expecteddensity = (double)list.size();
-
- vpMeTracker::track(I);
- }
- catch(vpException &e)
- {
- throw e;
- }
- vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
-}
-
-
-/*!
- 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.
-*/
-void
-vpMbtMeLine::sample(const vpImage<unsigned char>& I)
-{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- double n_sample;
-
- //if (me->getSampleStep==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- throw(vpTrackingException(vpTrackingException::fatalError,
- "Function vpMbtMeLine::sample() called with moving-edges sample step = 0")) ;
- }
-
- // i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
-
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
-
- // number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
-
- double stepi = diffsi/(double)n_sample;
- double stepj = diffsj/(double)n_sample;
-
- // Choose starting point
- double is = PExt[1].ifloat;
- double js = PExt[1].jfloat;
-
- // Delete old list
- list.clear();
-
- // sample positions at i*me->getSampleStep() interval along the
- // line_p, starting at PSiteExt[0]
-
- vpImagePoint ip;
- for(int i=0; i<=vpMath::round(n_sample); i++)
- {
- // If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- vpMeSite pix ; //= list.value();
- pix.init((int)is, (int)js, delta, 0, sign) ;
-
- pix.track(I, me, false);
-
- pix.setDisplay(selectDisplay) ;
-
- if(vpDEBUG_ENABLE(3))
- {
- ip.set_i( is );
- ip.set_j( js );
- vpDisplay::displayCross(I, ip, 2, vpColor::blue);
- }
-
- list.push_back(pix);
- }
- is += stepi;
- js += stepj;
-
- }
-
- vpCDEBUG(1) << "end vpMeLine::sample() : ";
- vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl ;
-}
-
-
-/*!
- Suppress the moving which belong no more to the line.
-
- \param I : The image.
-*/
-void
-vpMbtMeLine::suppressPoints(const vpImage<unsigned char> & I)
-{
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
-
- if (fabs(sin(theta)) > 0.9) // Vertical line management
- {
- if ((s.i < imin) ||(s.i > imax))
- {
- s.setState(vpMeSite::CONSTRAST);
- }
- }
-
- else if (fabs(cos(theta)) > 0.9) // Horizontal line management
- {
- if ((s.j < jmin) || (s.j > jmax))
- {
- s.setState(vpMeSite::CONSTRAST);
- }
- }
-
- else
- {
- if ((s.i < imin) ||(s.i > imax) || (s.j < jmin) || (s.j > jmax) )
- {
- s.setState(vpMeSite::CONSTRAST);
- }
-
- }
-
- if (outOfImage(s.i, s.j, (int)(me->getRange()+me->getMaskSize()+1), (int)I.getHeight(), (int)I.getWidth()))
- {
- s.setState(vpMeSite::TOO_NEAR);
- }
-
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- it = list.erase(it);
- else
- ++it;
- }
-}
-
-
-/*!
- 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.
-*/
-void
-vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
-{
- vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
-
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- double n_sample;
-
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- throw(vpTrackingException(vpTrackingException::fatalError,
- "Function called with sample step = 0")) ;
- }
-
- // i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
-
- double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
-
- double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
- double dj = diffsj/sqrt(s) ;
-
- double length_p = sqrt(s); /*(vpMath::sqr(diffsi)+vpMath::sqr(diffsj))*/
-
- // number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
- double sample_step = (double)me->getSampleStep();
-
- vpMeSite P ;
- P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
-
- unsigned int memory_range = me->getRange() ;
- me->setRange(1);
-
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
-
- if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
- {
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
- }
- else
- if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
- }
- else
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
- }
- }
-
- P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
-
- if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
- {
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
- }
-
- else
- if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
- }
- else
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
- }
- }
-
- me->setRange(memory_range);
-
- vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
- vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
-}
-
-/*!
- Resample the line if the number of sample is less than 50% 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
-vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
-{
- unsigned int n = numberOfSignal() ;
-
- if ((double)n<0.5*expecteddensity && n > 0)
- {
- double delta_new = delta;
- delta = delta_1;
- sample(I) ;
- expecteddensity = (double)list.size();
- delta = delta_new;
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
- }
-}
-
-
-/*!
- Resample the line if the number of sample is less than 50% 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.
- \param ip1 : The first extremity of the line.
- \param ip2 : The second extremity of the line.
-*/
-void
-vpMbtMeLine::reSample(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2)
-{
- size_t n = list.size();
-
- if ((double)n<0.5*expecteddensity /*&& n > 0*/) // n is always > 0
- {
- double delta_new = delta;
- delta = delta_1;
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
- sample(I) ;
- expecteddensity = (double)list.size();
- delta = delta_new;
- vpMeTracker::track(I) ;
- }
-}
-
-/*!
- Set the alpha value of the different vpMeSite to the value of delta.
-*/
-void
-vpMbtMeLine::updateDelta()
-{
- vpMeSite p_me ;
-
- double diff = 0;
-
- //if(fabs(theta) == M_PI )
- if(std::fabs(std::fabs(theta) - M_PI) <= vpMath::maximum(std::fabs(theta), (double)M_PI)*std::numeric_limits<double>::epsilon() )
- {
- theta = 0 ;
- }
-
- diff = fabs(theta - theta_1);
- if (diff > M_PI/2.0)
- sign *= -1;
-
- theta_1 = theta;
-
- delta = - theta + M_PI/2.0;
- normalizeAngle(delta);
-
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- p_me.alpha = delta ;
- p_me.mask_sign = sign;
- *it = p_me;
- }
- delta_1 = delta;
-}
-
-/*!
- Track the line in the image I.
-
- \param I : Image in which the line appears.
- */
-void
-vpMbtMeLine::track(const vpImage<unsigned char> &I)
-{
- // 2. On appelle ce qui n'est pas specifique
- try
- {
- vpMeTracker::track(I);
- }
- catch(vpException &e)
- {
- throw e;
- }
-
- // supression des points rejetes par les ME
- // suppressPoints(I);
- // setExtremities();
-}
-
-
-/*!
- Update the moving edges parameters after the virtual visual servoing.
-
- \param I : The image.
- \param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
- \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
-*/
-void
-vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
-{
- this->rho = rho_;
- this->theta = theta_;
- a = cos(theta);
- b = sin(theta);
- c = -rho;
- // recherche de point aux extremite de la droites
- // dans le cas d'un glissement
- suppressPoints(I);
- seekExtremities(I);
- suppressPoints(I);
- setExtremities();
- //reechantillonage si necessaire
- reSample(I);
-
- // remet a jour l'angle delta pour chaque point de la liste
- updateDelta();
-}
-
-
-/*!
- Update the moving edges parameters after the virtual visual servoing.
-
- \param I : The image.
- \param ip1 : The first extremity of the line.
- \param ip2 : The second extremity of the line.
- \param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
- \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
-*/
-void
-vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2,
- double rho_, double theta_)
-{
- this->rho = rho_;
- this->theta = theta_;
- a = cos(theta);
- b = sin(theta);
- c = -rho;
- // recherche de point aux extremite de la droites
- // dans le cas d'un glissement
- suppressPoints(I);
- seekExtremities(I);
- suppressPoints(I);
- setExtremities();
- //reechantillonage si necessaire
- reSample(I,ip1,ip2);
-
- // remet a jour l'angle delta pour chaque point de la liste
- updateDelta();
-}
-
-
-/*!
- Seek in the list of available points the two extremities of the line.
-*/
-void
-vpMbtMeLine::setExtremities()
-{
- double i_min = +1e6 ;
- double j_min = +1e6;
- double i_max = -1 ;
- double j_max = -1 ;
-
- // Loop through list of sites to track
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.ifloat < i_min)
- {
- i_min = s.ifloat ;
- j_min = s.jfloat ;
- }
-
- if (s.ifloat > i_max)
- {
- i_max = s.ifloat ;
- j_max = s.jfloat ;
- }
- }
-
- if ( ! list.empty() )
- {
- PExt[0].ifloat = i_min ;
- PExt[0].jfloat = j_min ;
- PExt[1].ifloat = i_max ;
- PExt[1].jfloat = j_max ;
- }
-
- if (fabs(i_min-i_max) < 25)
- {
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.jfloat < j_min)
- {
- i_min = s.ifloat ;
- j_min = s.jfloat ;
- }
-
- if (s.jfloat > j_max)
- {
- i_max = s.ifloat ;
- j_max = s.jfloat ;
- }
- }
-
- if (! list.empty())
- {
- PExt[0].ifloat = i_min ;
- PExt[0].jfloat = j_min ;
- PExt[1].ifloat = i_max ;
- PExt[1].jfloat = j_max ;
- }
- bubbleSortJ();
- }
-
- else
- bubbleSortI();
-}
-
-
-static bool sortByI(const vpMeSite& s1, const vpMeSite& s2){
- return (s1.ifloat > s2.ifloat);
-}
-
-void
-vpMbtMeLine::bubbleSortI()
-{
-#if 0
- unsigned int nbElmt = list.size();
- for (unsigned int pass = 1; pass < nbElmt; pass++)
- {
- list.front();
- for (unsigned int i=0; i < nbElmt-pass; i++)
- {
- vpMeSite s1 = list.value() ;
- vpMeSite s2 = list.nextValue() ;
- if (s1.ifloat > s2.ifloat)
- list.swapRight();
- else
- list.next();
- }
- }
-#endif
- list.sort(sortByI);
-}
-
-
-static bool sortByJ(const vpMeSite& s1, const vpMeSite& s2){
- return (s1.jfloat > s2.jfloat);
-}
-
-void
-vpMbtMeLine::bubbleSortJ()
-{
-#if 0
- unsigned int nbElmt = list.size();
- for(unsigned int pass=1; pass < nbElmt; pass++)
- {
- list.front();
- for (unsigned int i=0; i < nbElmt-pass; i++)
- {
- vpMeSite s1 = list.value() ;
- vpMeSite s2 = list.nextValue() ;
- if (s1.jfloat > s2.jfloat)
- list.swapRight();
- else
- list.next();
- }
- }
-#endif
- list.sort(sortByJ);
-}
-
-
-void
-vpMbtMeLine::findSignal(const vpImage<unsigned char>& I, const vpMe *p_me, double *conv)
-{
- vpImagePoint itest(PExt[0].ifloat+(PExt[1].ifloat-PExt[0].ifloat)/2, PExt[0].jfloat+(PExt[1].jfloat-PExt[0].jfloat)/2);
-
- vpMeSite pix ; //= list.value();
- pix.init(itest.get_i(), itest.get_j(), delta, 0, sign);
-
- vpMeSite *list_query_pixels;
-// double convolution = 0;
- unsigned int range = p_me->getRange();
-
- list_query_pixels = pix.getQueryList(I, (int)range);
-
- vpDisplay::displayCross(I,itest,5,vpColor::cyan,3);
- vpDisplay::displayLine(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),vpImagePoint(list_query_pixels[2*range].ifloat,list_query_pixels[2*range].jfloat),vpColor::cyan);
- vpDisplay::displayCross(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),5,vpColor::orange,3);
-
- for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
- {
- conv[n] = list_query_pixels[n].convolution(I, p_me);
- }
- delete [] list_query_pixels;
-}
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtMeLine.h b/src/tracking/mbt/edge/vpMbtMeLine.h
deleted file mode 100644
index dac7d9f..0000000
--- a/src/tracking/mbt/edge/vpMbtMeLine.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtMeLine.h 4740 2014-05-26 07:15:44Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation of a line used by the model-based tracker.
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtMeLine.h
- \brief Implementation of a line used by the model-based tracker.
-*/
-
-#ifndef vpMbtMeLine_HH
-#define vpMbtMeLine_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMe.h>
-#include <visp/vpMeTracker.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/*!
- \class vpMbtMeLine
- \brief Implementation of a line used by the model-based tracker.
- \ingroup ModelBasedTracking
-
- */
-class VISP_EXPORT vpMbtMeLine : public vpMeTracker
-{
- private:
- vpMeSite PExt[2] ;
- double rho, theta, theta_1;
- double delta ,delta_1;
- int sign;
- double a,b,c;
-
- public:
- int imin, imax;
- int jmin, jmax;
- double expecteddensity;
-
- public:
- vpMbtMeLine();
- ~vpMbtMeLine();
-
- void display(const vpImage<unsigned char>& /*I*/, vpColor /*col*/) {;}
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The a coefficient of the moving edge
- */
- inline double get_a() const { return this->a;}
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The b coefficient of the moving edge
- */
- inline double get_b() const { return this->b;}
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The c coefficient of the moving edge
- */
- inline double get_c() const { return this->c;}
-
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, double theta);
-
- void track(const vpImage<unsigned char> &I);
-
- void updateParameters(const vpImage<unsigned char> &I, double rho, double theta);
- void updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2, double rho, double theta);
-
- private:
- void bubbleSortI();
- void bubbleSortJ();
- void findSignal(const vpImage<unsigned char>& I, const vpMe *me, double *conv);
- void sample(const vpImage<unsigned char>&image);
- void seekExtremities(const vpImage<unsigned char> &I);
- void setExtremities();
- void suppressPoints(const vpImage<unsigned char> &I);
- void reSample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char>&image, vpImagePoint ip1, vpImagePoint ip2);
- void updateDelta();
-} ;
-
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtXmlParser.cpp b/src/tracking/mbt/edge/vpMbtXmlParser.cpp
deleted file mode 100644
index fef8427..0000000
--- a/src/tracking/mbt/edge/vpMbtXmlParser.cpp
+++ /dev/null
@@ -1,528 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtXmlParser.cpp 5238 2015-01-30 13:52:25Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using edges).
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Aurelien Yol
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-
-#ifdef VISP_HAVE_XML2
-
-#include <iostream>
-#include <map>
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbtXmlParser.h>
-
-
-/*!
- Default constructor.
-
-*/
-vpMbtXmlParser::vpMbtXmlParser() : m_ecm(), useLod(false), minLineLengthThreshold(50.0), minPolygonAreaThreshold(2500.0)
-{
- init();
-}
-
-/*!
- Default destructor.
-*/
-vpMbtXmlParser::~vpMbtXmlParser()
-{
-}
-
-/*!
- Initialise internal variables (including the map).
-*/
-void
-vpMbtXmlParser::init()
-{
- vpMbXmlParser::init();
-
- nodeMap["ecm"] = ecm;
- nodeMap["mask"] = mask;
- nodeMap["size"] = size;
- nodeMap["nb_mask"] = nb_mask;
- nodeMap["range"] = range;
- nodeMap["tracking"] = tracking;
- nodeMap["contrast"] = contrast;
- nodeMap["edge_threshold"] = edge_threshold;
- nodeMap["mu1"] = mu1;
- nodeMap["mu2"] = mu2;
- nodeMap["sample"] = sample;
- nodeMap["step"] = step;
- nodeMap["nb_sample"] = nb_sample;
- nodeMap["lod"] = lod;
- nodeMap["use_lod"] = use_lod;
- nodeMap["min_line_length_threshold"] = min_line_length_threshold;
- nodeMap["min_polygon_area_threshold"] = min_polygon_area_threshold;
-}
-
-/*!
- Parse the file in parameters.
- This method is deprecated, use parse() instead.
-
- \param filename : File to parse.
-*/
-void
-vpMbtXmlParser::parse(const char * filename)
-{
- std::string file = filename;
- vpXmlParser::parse(file);
-}
-
-/*!
- Write info to file.
-
- \warning Useless, so not yet implemented => Throw exception.
-*/
-void
-vpMbtXmlParser::writeMainClass(xmlNodePtr /*node*/)
-{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
-}
-
-/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
- \param doc : Document to parse.
- \param node : Root node.
-*/
-void
-vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
-{
- bool camera_node = false;
- bool face_node = false;
- bool ecm_node = false;
- bool sample_node = false;
- bool lod_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
- camera_node = true;
- }break;
- case face:{
- this->read_face(doc, dataNode);
- face_node = true;
- }break;
- case ecm:{
- this->read_ecm (doc, dataNode);
- ecm_node = true;
- }break;
- case sample:{
- this->read_sample (doc, dataNode);
- sample_node = true;
- }break;
- case lod:{
- this->read_lod(doc, dataNode);
- lod_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
- }
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
- }
-
- if(!ecm_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- }
-
- if(!sample_node) {
- std::cout <<"sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
- std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
- }
-
- if(!lod_node) {
- std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
- std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
- std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
- }
-}
-
-
-/*!
- Read ecm information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the ecm information.
-*/
-void
-vpMbtXmlParser::read_ecm (xmlDocPtr doc, xmlNodePtr node)
-{
- bool mask_node = false;
- bool range_node = false;
- bool contrast_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case mask:{
- this->read_mask (doc, dataNode);
- mask_node = true;
- }break;
- case range:{
- this->read_range (doc, dataNode);
- range_node = true;
- }break;
- case contrast:{
- this->read_contrast (doc, dataNode);
- contrast_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_ecm : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!mask_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- }
-
- if(!range_node) {
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- }
-
- if(!contrast_node) {
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- }
-}
-
-/*!
- Read sample information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the sample information.
-*/
-void
-vpMbtXmlParser::read_sample (xmlDocPtr doc, xmlNodePtr node)
-{
- bool step_node = false;
- bool nb_sample_node = false;
-
- // current data values.
- double d_stp = this->m_ecm.getSampleStep();
- int d_nb_sample = this->m_ecm.getNbTotalSample();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case step:{
- d_stp = xmlReadIntChild(doc, dataNode);
- step_node = true;
- }break;
- case nb_sample:{
- d_nb_sample = xmlReadIntChild(doc, dataNode);
- nb_sample_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- this->m_ecm.setSampleStep(d_stp);
- this->m_ecm.setNbTotalSample(d_nb_sample);
-
- if(!step_node)
- std::cout <<"sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
- else
- std::cout <<"sample : sample_step : "<< this->m_ecm.getSampleStep()<<std::endl;
-
- if(!nb_sample_node)
- std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
- else
- std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<<std::endl;
-}
-
-/*!
- Read mask information for the vpMeSite.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the mask information.
-*/
-void
-vpMbtXmlParser::read_mask (xmlDocPtr doc, xmlNodePtr node)
-{
- bool size_node = false;
- bool nb_mask_node = false;
-
- // current data values.
- unsigned int d_size = this->m_ecm.getMaskSize();
- unsigned int d_nb_mask = this->m_ecm.getMaskNumber();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case size:{
- d_size = xmlReadUnsignedIntChild(doc, dataNode);
- size_node = true;
- }break;
- case nb_mask:{
- d_nb_mask = xmlReadUnsignedIntChild(doc, dataNode);
- nb_mask_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_mask : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- this->m_ecm.setMaskSize(d_size) ;
- // Check to ensure that d_nb_mask > 0
- if (! d_nb_mask)
- throw(vpException(vpException::badValue, "Model-based tracker mask size parameter should be different from zero in xml file"));
- this->m_ecm.setMaskNumber(d_nb_mask);
-
- if(!size_node)
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- else
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() <<std::endl;
-
- if(!nb_mask_node)
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- else
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() <<std::endl;
-}
-
-/*!
- Read range information for the vpMeSite.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the range information.
-*/
-void
-vpMbtXmlParser::read_range (xmlDocPtr doc, xmlNodePtr node)
-{
- bool tracking_node = false;
-
- // current data values.
- unsigned int m_range_tracking = this->m_ecm.getRange();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case tracking:{
- m_range_tracking = xmlReadUnsignedIntChild(doc, dataNode);
- tracking_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_range : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- this->m_ecm.setRange(m_range_tracking);
-
- if(!tracking_node)
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- else
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<<std::endl;
-}
-
-
-/*!
- Read the contrast information from the xml file.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the contrast information.
-*/
-void
-vpMbtXmlParser::read_contrast (xmlDocPtr doc, xmlNodePtr node)
-{
- bool edge_threshold_node = false;
- bool mu1_node = false;
- bool mu2_node = false;
-
- // current data values.
- double d_edge_threshold = this->m_ecm.getThreshold();
- double d_mu1 = this->m_ecm.getMu1();
- double d_mu2 = this->m_ecm.getMu2();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case edge_threshold:{
- d_edge_threshold = xmlReadDoubleChild(doc, dataNode);
- edge_threshold_node = true;
- }break;
- case mu1:{
- d_mu1 = xmlReadDoubleChild(doc, dataNode);
- mu1_node = true;
- }break;
- case mu2:{
- d_mu2 = xmlReadDoubleChild(doc, dataNode);
- mu2_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- this->m_ecm.setMu1(d_mu1);
- this->m_ecm.setMu2(d_mu2);
- this->m_ecm.setThreshold(d_edge_threshold);
-
- if(!edge_threshold_node)
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- else
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<std::endl;
-
- if(!mu1_node)
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- else
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<std::endl;
-
- if(!mu2_node)
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- else
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<std::endl;
-}
-
-void
-vpMbtXmlParser::read_lod (xmlDocPtr doc, xmlNodePtr node) {
- bool use_lod_node = false;
- bool min_line_length_threshold_node = false;
- bool min_polygon_area_threshold_node = false;
-
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case use_lod:
- useLod = (xmlReadIntChild(doc, dataNode) != 0);
- use_lod_node = true;
- break;
- case min_line_length_threshold:
- minLineLengthThreshold = xmlReadDoubleChild(doc, dataNode);
- min_line_length_threshold_node = true;
- break;
- case min_polygon_area_threshold:
- minPolygonAreaThreshold = xmlReadDoubleChild(doc, dataNode);
- min_polygon_area_threshold_node = true;
- break;
- default:{
-// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!use_lod_node)
- std::cout << "lod : use lod : " << useLod << " (default)" <<std::endl;
- else
- std::cout << "lod : use lod : " << useLod << std::endl;
-
- if(!min_line_length_threshold_node)
- std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<" (default)" <<std::endl;
- else
- std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<std::endl;
-
- if(!min_polygon_area_threshold_node)
- std::cout <<"lod : min polygon area threshold : " << minPolygonAreaThreshold <<" (default)" <<std::endl;
- else
- std::cout <<"lod : min polygon area threshold : " << minPolygonAreaThreshold <<std::endl;
-}
-
-#endif
-
diff --git a/src/tracking/mbt/edge/vpMbtXmlParser.h b/src/tracking/mbt/edge/vpMbtXmlParser.h
deleted file mode 100644
index 75e3dd8..0000000
--- a/src/tracking/mbt/edge/vpMbtXmlParser.h
+++ /dev/null
@@ -1,162 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtXmlParser.h 5054 2014-12-11 17:50:44Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using edges).
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- * \file vpMbtXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
-
-#ifndef vpMbtXmlParser_HH
-#define vpMbtXmlParser_HH
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbXmlParser.h>
-#include <visp/vpMe.h>
-
-/*!
- \class vpMbtXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup ModelBasedTracking
-
- Data parser for the model based tracker.
-
- */
-class VISP_EXPORT vpMbtXmlParser: virtual public vpMbXmlParser
-{
-protected:
- typedef enum{
- ecm = vpMbXmlParser::last,
- mask,
- size,
- nb_mask,
- range,
- tracking,
- contrast,
- edge_threshold,
- mu1,
- mu2,
- sample,
- step,
- nb_sample,
- lod,
- use_lod,
- min_line_length_threshold,
- min_polygon_area_threshold,
- last
- } dataToParseMb;
-
- //! Moving edges parameters.
- vpMe m_ecm;
- //! If true, the LOD is enabled, otherwise it is not
- bool useLod;
- //! Minimum line length to track a segment when LOD is enabled
- double minLineLengthThreshold;
- //! Minimum polygon area to track a face when LOD is enabled
- double minPolygonAreaThreshold;
-
-
-public:
-
- vpMbtXmlParser();
- virtual ~vpMbtXmlParser();
-
- /*!
- Get the state of LOD setting.
-
- \return True if LOD is enabled, false otherwise.
- */
- inline bool getLodState() const {
- return useLod;
- }
-
- void getMe(vpMe& _ecm) const { _ecm = this->m_ecm;}
-
- /*!
- Get the minimum line length to track a segment when LOD is enabled.
-
- \return The minimum line length.
- */
- inline double getMinLineLengthThreshold() const {
- return minLineLengthThreshold;
- }
-
- /*!
- Get the minimum polygon area to track a face when LOD is enabled.
-
- \return The minimum polygon area.
- */
- inline double getMinPolygonAreaThreshold() const {
- return minPolygonAreaThreshold;
- }
-
- void parse(const char * filename);
-
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_ecm (xmlDocPtr doc, xmlNodePtr node);
- void read_sample (xmlDocPtr doc, xmlNodePtr node);
- void read_mask (xmlDocPtr doc, xmlNodePtr node);
- void read_range (xmlDocPtr doc, xmlNodePtr node);
- void read_contrast (xmlDocPtr doc, xmlNodePtr node);
- void read_lod (xmlDocPtr doc, xmlNodePtr node);
-
- void setMovingEdge(const vpMe &_ecm){ m_ecm = _ecm; }
-
- void writeMainClass(xmlNodePtr node);
-
-protected:
- void init();
-
-};
-
-#endif
-
-#endif /* NMBTXMLPARSER_H_ */
-
-
-
diff --git a/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.cpp b/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.cpp
deleted file mode 100644
index 3799fc3..0000000
--- a/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.cpp
+++ /dev/null
@@ -1,1172 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbEdgeKltTracker.cpp 5189 2015-01-21 16:42:18Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-//#define VP_DEBUG_MODE 1 // Activate debug level 1
-
-#include <visp/vpDebug.h>
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpVelocityTwistMatrix.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-vpMbEdgeKltTracker::vpMbEdgeKltTracker()
- : compute_interaction(true), lambda(0.8), thresholdKLT(2.), thresholdMBT(2.), maxIter(200)
-{
- computeCovariance = false;
-
- vpMbKltTracker::setMaxIter(30);
-
- angleAppears = vpMath::rad(65);
- angleDisappears = vpMath::rad(75);
-
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Hybrid");
-#endif
-}
-
-/*!
- Basic constructor
-
-*/
-vpMbEdgeKltTracker::~vpMbEdgeKltTracker()
-{
-}
-
-/*!
- Initialization of the tracker using a known initial pose.
- The 3D model must first have been loaded.
-
- \param I : Input image.
-*/
-void
-vpMbEdgeKltTracker::init(const vpImage<unsigned char>& I)
-{
- vpMbKltTracker::init(I);
-
- initPyramid(I, Ipyramid);
-
- unsigned int i = (unsigned int)scales.size();
- do {
- i--;
- if(scales[i]){
- downScale(i);
- initMovingEdge(*Ipyramid[i], cMo);
- upScale(i);
- }
- } while(i != 0);
-
- cleanPyramid(Ipyramid);
-}
-
-/*!
- Set the pose to be used in entry of the next call to the track() function.
- This pose will be just used once.
-
- \warning This function has to be called after the initialization of the tracker.
-
- \param I : image corresponding to the desired pose.
- \param cdMo : Pose to affect.
-*/
-void
-vpMbEdgeKltTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
-{
- vpMbKltTracker::setPose(I, cdMo);
-
- if (! lines[scaleLevel].empty()) {
- lines[scaleLevel].front() ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->meline != NULL){
- delete (*it)->meline;
- (*it)->meline = NULL;
- }
- }
- }
-
- if (! cylinders[scaleLevel].empty()) {
- cylinders[scaleLevel].front() ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->meline1 != NULL){
- delete (*it)->meline1;
- (*it)->meline1 = NULL;
- }
- if((*it)->meline2 != NULL){
- delete (*it)->meline2;
- (*it)->meline2 = NULL;
- }
- }
- }
-
- if (! circles[scaleLevel].empty()) {
- circles[scaleLevel].front() ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->meEllipse != NULL){
- delete (*it)->meEllipse;
- (*it)->meEllipse = NULL;
- }
- }
- }
-
- initPyramid(I, Ipyramid);
-
- unsigned int i = (unsigned int)scales.size();
- do {
- i--;
- if(scales[i]){
- downScale(i);
- initMovingEdge(*Ipyramid[i], cMo);
- upScale(i);
- }
- } while(i != 0);
-
- cleanPyramid(Ipyramid);
-}
-
-/*!
- Reset the tracker. The model is removed and the pose is set to identity.
- The tracker needs to be initialized with a new model and a new pose.
-*/
-void
-vpMbEdgeKltTracker::resetTracker()
-{
- vpMbEdgeTracker::resetTracker();
- vpMbKltTracker::resetTracker();
-}
-
-unsigned int
-vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
-{
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
-
- if(lvl >= scales.size() || !scales[lvl]){
- throw vpException(vpException::dimensionError, "lvl not used.");
- }
-
- unsigned int nbrow = 0;
- for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- nbrow += l->nbFeature ;
- l->initInteractionMatrixError() ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- nbrow += cy->nbFeature ;
- cy->initInteractionMatrixError() ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- nbrow += ci->nbFeature ;
- ci->initInteractionMatrixError() ;
- }
-
- return nbrow;
-}
-
-/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \param configFile : full name of the xml file.
-
- \sa loadConfigFile(const char*), vpXmlParser::cleanup()
-*/
-void
-vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
-{
- vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
-}
-
-/*!
- Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
-
- \param configFile : full name of the xml file.
-
- The XML configuration file has the following form:
- \code
-<?xml version="1.0"?>
-<conf>
- <ecm>
- <mask>
- <size>5</size>
- <nb_mask>180</nb_mask>
- </mask>
- <range>
- <tracking>10</tracking>
- </range>
- <contrast>
- <edge_threshold>7000</edge_threshold>
- <mu1>0.5</mu1>
- <mu2>0.5</mu2>
- </contrast>
- </ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
- <camera>
- <width>640</width>
- <height>480</height>
- <u0>320</u0>
- <v0>240</v0>
- <px>686.24</px>
- <py>686.24</py>
- </camera>
- <face>
- <angle_appear>65</angle_appear>
- <angle_disappear>85</angle_disappear>
- <near_clipping>0.01</near_clipping>
- <far_clipping>0.90</far_clipping>
- <fov_clipping>1</fov_clipping>
- </face>
- <klt>
- <mask_border>10</mask_border>
- <max_features>10000</max_features>
- <window_size>5</window_size>
- <quality>0.02</quality>
- <min_distance>10</min_distance>
- <harris>0.02</harris>
- <size_block>3</size_block>
- <pyramid_lvl>3</pyramid_lvl>
- </klt>
-</conf>
- \endcode
-
- \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
-*/
-void
-vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
-{
-#ifdef VISP_HAVE_XML2
- vpMbtEdgeKltXmlParser xmlp;
-
- xmlp.setCameraParameters(cam);
- xmlp.setAngleAppear(vpMath::deg(angleAppears));
- xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
-
- xmlp.setMovingEdge(me);
-
- xmlp.setMaxFeatures(10000);
- xmlp.setWindowSize(5);
- xmlp.setQuality(0.01);
- xmlp.setMinDistance(5);
- xmlp.setHarrisParam(0.01);
- xmlp.setBlockSize(3);
- xmlp.setPyramidLevels(3);
- xmlp.setMaskBorder(maskBorder);
-
- try{
- std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
- xmlp.parse(configFile);
- }
- catch(...){
- vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
- throw vpException(vpException::ioError, "problem to parse configuration file.");
- }
-
- vpCameraParameters camera;
- xmlp.getCameraParameters(camera);
- setCameraParameters(camera);
-
- angleAppears = vpMath::rad(xmlp.getAngleAppear());
- angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
-
- if(xmlp.hasNearClippingDistance())
- setNearClippingDistance(xmlp.getNearClippingDistance());
-
- if(xmlp.hasFarClippingDistance())
- setFarClippingDistance(xmlp.getFarClippingDistance());
-
- if(xmlp.getFovClipping()){
- setClipping(vpMbEdgeTracker::clippingFlag | vpMbtPolygon::FOV_CLIPPING);
- }
-
- vpMe meParser;
- xmlp.getMe(meParser);
- vpMbEdgeTracker::setMovingEdge(meParser);
-
- tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
- tracker.setWindowSize((int)xmlp.getWindowSize());
- tracker.setQuality(xmlp.getQuality());
- tracker.setMinDistance(xmlp.getMinDistance());
- tracker.setHarrisFreeParameter(xmlp.getHarrisParam());
- tracker.setBlockSize((int)xmlp.getBlockSize());
- tracker.setPyramidLevels((int)xmlp.getPyramidLevels());
- maskBorder = xmlp.getMaskBorder();
-#else
- vpTRACE("You need the libXML2 to read the config file %s", configFile);
-#endif
-}
-
-/*!
- Realize the post tracking operations. Mostly visibility tests
-*/
-bool
-vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
- const unsigned int lvl)
-{
- bool reInit = vpMbKltTracker::postTracking(I, w_klt);
-
- postTrackingMbt(w_mbt,lvl);
-
- if (displayFeatures)
- {
- if(lvl == 0){
- vpMbtDistanceLine* l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- if (l->isVisible()){
- l->displayMovingEdges(I);
- }
- }
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- // A cylinder is always visible
- cy->displayMovingEdges(I);
- }
-
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- if (ci->isVisible()){
- ci->displayMovingEdges(I);
- }
- }
- }
- }
-
- if(reInit)
- return true;
-
- vpMbEdgeTracker::updateMovingEdge(I);
-
- vpMbEdgeTracker::initMovingEdge(I, cMo) ;
- vpMbEdgeTracker::reinitMovingEdge(I, cMo);
-
- return false;
-}
-
-/*!
- Post tracking computation. Compute the mean weight of a line and, check the
- weight associated to a site (to eventually remove an outlier) and eventually
- set a flag to re-initialize the line.
-
- \warning level parameter not yet implemented.
-
- \param w : Vector of weight associated to the residual.
- \param lvl : Optional parameter to specify the level to track.
-*/
-void
-vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
-{
- if(lvl >= scales.size() || !scales[lvl]){
- throw vpException(vpException::dimensionError, "_lvl not used.");
- }
- unsigned int n =0 ;
- vpMbtDistanceLine* l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- {
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListLine;
- if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
-
- for (unsigned int i=0 ; i < l->nbFeature ; i++){
- wmean += w[n+i] ;
- vpMeSite p = *itListLine;
- if (w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListLine = p;
- }
-
- ++itListLine;
- }
- n+= l->nbFeature ;
-
- if (l->nbFeature!=0)
- wmean /= l->nbFeature ;
- else
- wmean = 1;
-
- l->setMeanWeight(wmean);
-
- if (wmean < 0.8)
- l->Reinit = true;
- }
- }
-
- // Same thing with cylinders as with lines
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListCyl1;
- std::list<vpMeSite>::iterator itListCyl2;
- if (cy->nbFeature > 0){
- itListCyl1 = cy->meline1->getMeList().begin();
- itListCyl2 = cy->meline2->getMeList().begin();
- }
-
- wmean = 0;
- for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
- wmean += w[n+i] ;
- vpMeSite p = *itListCyl1;
- if (w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCyl1 = p;
- }
-
- ++itListCyl1;
- }
-
- if (cy->nbFeaturel1!=0)
- wmean /= cy->nbFeaturel1 ;
- else
- wmean = 1;
-
- cy->setMeanWeight1(wmean);
-
- if (wmean < 0.8){
- cy->Reinit = true;
- }
-
- wmean = 0;
- for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
- wmean += w[n+i] ;
- vpMeSite p = *itListCyl2;
- if (w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCyl2 = p;
- }
-
- ++itListCyl2;
- }
-
- if (cy->nbFeaturel2!=0)
- wmean /= cy->nbFeaturel2 ;
- else
- wmean = 1;
-
- cy->setMeanWeight2(wmean);
-
- if (wmean < 0.8){
- cy->Reinit = true;
- }
-
- n+= cy->nbFeature ;
- }
-
- // Same thing with circles as with lines
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- double wmean = 0 ;
- std::list<vpMeSite>::iterator itListCir;
-
- if (ci->nbFeature > 0){
- itListCir = ci->meEllipse->getMeList().begin();
- }
-
- wmean = 0;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- wmean += w[n+i] ;
- vpMeSite p = *itListCir;
- if (w[n+i] < 0.5){
- p.setState(vpMeSite::M_ESTIMATOR);
-
- *itListCir = p;
- }
-
- ++itListCir;
- }
-
- if (ci->nbFeature!=0)
- wmean /= ci->nbFeature ;
- else
- wmean = 1;
-
- ci->setMeanWeight(wmean);
-
- if (wmean < 0.8){
- ci->Reinit = true;
- }
-
- n+= ci->nbFeature ;
- }
-}
-
-/*!
- Realize the VVS loop for the tracking
-
- \param I : current image.
- \param nbInfos : Size of the features.
- \param w_mbt : weight vector for MBT.
- \param w_klt : weight vector for KLT.
- \param lvl : level of the pyramid.
-*/
-void
-vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
-{
- vpColVector factor;
- unsigned int nbrow = trackFirstLoop(I, factor, lvl);
-
- if(nbrow < 4 && nbInfos < 4){
- vpERROR_TRACE("\n\t\t Error-> not enough data") ;
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
- }
- else if(nbrow < 4)
- nbrow = 0;
-
- double residu = 0;
- double residu_1 = -1;
- unsigned int iter = 0;
-
- vpMatrix *L;
- vpMatrix L_mbt, L_klt; // interaction matrix
- vpColVector *R;
- vpColVector R_mbt, R_klt; // residu
- vpMatrix L_true;
- vpMatrix LVJ_true;
- //vpColVector R_true;
- vpColVector w_true;
-
- if(nbrow != 0){
- L_mbt.resize(nbrow,6);
- R_mbt.resize(nbrow);
- }
-
- if(nbInfos != 0){
- L_klt.resize(2*nbInfos,6);
- R_klt.resize(2*nbInfos);
- }
-
- //vpColVector w; // weight from MEstimator
- vpColVector v; // "speed" for VVS
- vpRobust robust_mbt(0), robust_klt(0);
- vpHomography H;
-
- vpMatrix LTL, LTR;
-
- double factorMBT = 1.0;
- double factorKLT = 1.0;
-
- //More efficient weight repartition for hybrid tracker should come soon...
-// factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
-// factorKLT = 1.0 - factorMBT;
- factorMBT = 0.35;
- factorKLT = 0.65;
-
- if (nbrow < 4)
- factorKLT = 1.;
- if (nbInfos < 4)
- factorMBT = 1.;
-
- double residuMBT = 0;
- double residuKLT = 0;
-
- vpHomogeneousMatrix cMoPrev;
-
- while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
- L = new vpMatrix();
- R = new vpColVector();
-
- if(nbrow >= 4)
- trackSecondLoop(I,L_mbt,R_mbt,cMo,lvl);
-
- if(nbInfos >= 4){
- unsigned int shift = 0;
- vpMbtDistanceKltPoints *kltpoly;
- // for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=vpMbKltTracker::kltPolygons.begin(); it!=vpMbKltTracker::kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->hasEnoughPoints()){
- vpSubColVector subR(R_klt, shift, 2*kltpoly->getCurrentNumberPoints());
- vpSubMatrix subL(L_klt, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
- kltpoly->computeHomography(ctTc0, H);
- kltpoly->computeInteractionMatrixAndResidu(subR, subL);
- shift += 2*kltpoly->getCurrentNumberPoints();
- }
- }
- }
-
- if(iter == 0){
- m_w.resize(nbrow + 2*nbInfos);
- m_w=1;
-
- if(nbrow != 0){
- w_mbt.resize(nbrow);
- w_mbt = 1;
- robust_mbt.resize(nbrow);
- }
-
- if(nbInfos != 0){
- w_klt.resize(2*nbInfos);
- w_klt = 1;
- robust_klt.resize(2*nbInfos);
- }
-
- w_true.resize(nbrow + 2*nbInfos);
- }
-
- /* robust */
- if(nbrow > 3){
- residuMBT = 0;
- for(unsigned int i = 0; i < R_mbt.getRows(); i++)
- residuMBT += fabs(R_mbt[i]);
- residuMBT /= R_mbt.getRows();
-
- robust_mbt.setIteration(iter);
- robust_mbt.setThreshold(thresholdMBT/cam.get_px());
- robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
- L->stackMatrices(L_mbt);
- R->stackMatrices(R_mbt);
- }
-
- if(nbInfos > 3){
- residuKLT = 0;
- for(unsigned int i = 0; i < R_klt.getRows(); i++)
- residuKLT += fabs(R_klt[i]);
- residuKLT /= R_klt.getRows();
-
- robust_klt.setIteration(iter);
- robust_klt.setThreshold(thresholdKLT/cam.get_px());
- robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
-
- L->stackMatrices(L_klt);
- R->stackMatrices(R_klt);
- }
-
- unsigned int cpt = 0;
- while(cpt< (nbrow+2*nbInfos)){
- if(cpt<(unsigned)nbrow){
- m_w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
- }
- else
- m_w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
- cpt++;
- }
-
- m_error = (*R);
- if(computeCovariance){
- L_true = (*L);
- if(!isoJoIdentity){
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- LVJ_true = ((*L)*cVo*oJo);
- }
- }
-
- residu_1 = residu;
- residu = 0;
- double num = 0;
- double den = 0;
- for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
- num += m_w[i]*vpMath::sqr((*R)[i]);
- den += m_w[i];
-
- w_true[i] = m_w[i];
- (*R)[i] *= m_w[i];
- if(compute_interaction){
- for (unsigned int j = 0; j < 6; j += 1){
- (*L)[i][j] *= m_w[i];
- }
- }
- }
-
- residu = sqrt(num/den);
-
- if(isoJoIdentity){
- LTL = L->AtA();
- computeJTR(*L, *R, LTR);
- v = -lambda * LTL.pseudoInverse() * LTR;
- }
- else{
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- vpMatrix LVJ = ((*L)*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpMatrix LVJTR;
- computeJTR(LVJ, *R, LVJTR);
- v = -lambda*LVJTLVJ.pseudoInverse(1e-16)*LVJTR;
- v = cVo * v;
- }
-
- cMoPrev = cMo;
- cMo = vpExponentialMap::direct(v).inverse() * cMo;
- ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
-
- iter++;
-
- delete L;
- delete R;
- }
-
- if(computeCovariance){
- vpMatrix D;
- D.diag(w_true);
-
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity){
- computeCovarianceMatrix(cMoPrev,m_error,L_true,D);
- }
- else{
- computeCovarianceMatrix(cMoPrev,m_error,LVJ_true,D);
- }
- }
-}
-
-/*!
- Realize the tracking of the object in the image.
-
- \throw vpException : if the tracking is supposed to have failed.
-
- \param I : the input image.
-*/
-void
-vpMbEdgeKltTracker::track(const vpImage<unsigned char>& I)
-{
- unsigned int nbInfos = 0;
- unsigned int nbFaceUsed = 0;
- vpColVector w_klt;
-
- try{
- vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
- }
- catch(...){}
-
- if(nbInfos >= 4)
- vpMbKltTracker::computeVVS(nbInfos, w_klt);
- else{
- nbInfos = 0;
- // std::cout << "[Warning] Unable to init with KLT" << std::endl;
- }
-
- vpMbEdgeTracker::trackMovingEdge(I);
-
- vpColVector w_mbt;
- computeVVS(I, nbInfos, w_mbt, w_klt);
-
- if(postTracking(I, w_mbt, w_klt)){
- vpMbKltTracker::reinit(I);
-
- initPyramid(I, Ipyramid);
-
- unsigned int i = (unsigned int)scales.size();
- do {
- i--;
- if(scales[i]){
- downScale(i);
- initMovingEdge(*Ipyramid[i], cMo);
- upScale(i);
- }
- } while(i != 0);
-
- cleanPyramid(Ipyramid);
- }
-}
-
-unsigned int
-vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl)
-{
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
-
- if(lvl >= scales.size() || !scales[lvl]){
- throw vpException(vpException::dimensionError, "_lvl not used.");
- }
-
- unsigned int nbrow = initMbtTracking(lvl);
-
- if (nbrow==0){
-// vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
-// throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
- return nbrow;
- }
-
- factor.resize(nbrow);
- factor = 1;
-
- unsigned int n = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- l->computeInteractionMatrixError(cMo);
-
- double fac = 1;
- for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
- int index = *itindex;
- if (l->hiddenface->isAppearing((unsigned int)index)) {
- fac = 0.2;
- break;
- }
- if(l->closeToImageBorder(I, 10)){
- fac = 0.1;
- break;
- }
- }
-
- std::list<vpMeSite>::const_iterator itListLine;
- if (l->meline != NULL)
- itListLine = l->meline->getMeList().begin();
-
- for (unsigned int i=0 ; i < l->nbFeature ; i++){
- factor[n+i] = fac;
- vpMeSite site = *itListLine;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itListLine;
- }
- n+= l->nbFeature ;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- cy->computeInteractionMatrixError(cMo, I);
- double fac = 1.0;
-
- std::list<vpMeSite>::const_iterator itCyl1;
- std::list<vpMeSite>::const_iterator itCyl2;
- if ((cy->meline1 != NULL || cy->meline2 != NULL)){
- itCyl1 = cy->meline1->getMeList().begin();
- itCyl2 = cy->meline2->getMeList().begin();
- }
-
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- factor[n+i] = fac;
- vpMeSite site;
- if(i<cy->nbFeaturel1) {
- site= *itCyl1;
- ++itCyl1;
- }
- else{
- site= *itCyl2;
- ++itCyl2;
- }
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- }
-
- n+= cy->nbFeature ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->computeInteractionMatrixError(cMo);
- double fac = 1.0;
-
- std::list<vpMeSite>::const_iterator itCir;
- if (ci->meEllipse != NULL) {
- itCir = ci->meEllipse->getMeList().begin();
- }
-
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- factor[n+i] = fac;
- vpMeSite site = *itCir;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itCir;
- }
-
- n+= ci->nbFeature ;
- }
-
- return nbrow;
-}
-
-void
-vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &error,
- vpHomogeneousMatrix& cMo_, const unsigned int lvl)
-{
- vpMbtDistanceLine* l;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
-
- unsigned int n = 0 ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- l->computeInteractionMatrixError(cMo_) ;
- for (unsigned int i=0 ; i < l->nbFeature ; i++){
- for (unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = l->L[i][j];
- error[n+i] = l->error[i];
- }
- }
- n+= l->nbFeature;
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- cy->computeInteractionMatrixError(cMo_, I) ;
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j];
- error[n+i] = cy->error[i];
- }
- }
- n+= cy->nbFeature ;
- }
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- ci = *it;
- ci->computeInteractionMatrixError(cMo) ;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j];
- error[n+i] = ci->error[i];
- }
- }
-
- n+= ci->nbFeature ;
- }
-}
-
-/*!
- Set the camera parameters
-
- \param camera : the new camera parameters
-*/
-void
-vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters& camera)
-{
- this->cam = camera;
-
- vpMbEdgeTracker::setCameraParameters(cam);
- vpMbKltTracker::setCameraParameters(cam);
-}
-
-/*!
- Initialise a new face from the coordinates given in parameter.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
-{
- vpMbEdgeTracker::initFaceFromCorners(polygon);
- vpMbKltTracker::initFaceFromCorners(polygon);
-}
-/*!
- Initialise a new face from the coordinates given in parameter.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
-{
- vpMbEdgeTracker::initFaceFromLines(polygon);
- vpMbKltTracker::initFaceFromLines(polygon);
-}
-
-/*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
-
- \param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param idFace : Id of the face associated to the circle.
- \param name : The optional name of the circle.
-*/
-void
-vpMbEdgeKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace, const std::string &name)
-{
- vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
-}
-
-/*!
- Add a cylinder to track from tow points on the axis (defining the length of
- the cylinder) and its radius.
-
- \param p1 : First point on the axis.
- \param p2 : Second point on the axis.
- \param radius : Radius of the cylinder.
- \param idFace : Id of the face associated to the cylinder.
- \param name : The optional name of the cylinder.
-*/
-void
-vpMbEdgeKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
- const std::string &name)
-{
- vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
-}
-
-/*!
- Display the 3D model at a given position using the given camera parameters
-
- \param I : The image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
-*/
-void
-vpMbEdgeKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel)
-{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- break ; //displaying model on one scale only
- }
- }
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
- }
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-/*!
- Display the 3D model at a given position using the given camera parameters
-
- \param I : The color image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
-*/
-void
-vpMbEdgeKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
- const vpColor& col , const unsigned int thickness, const bool displayFullModel)
-{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
- }
-
- break ; //displaying model on one scale only
- }
- }
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
- }
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- reInitModel(I, cad_name.c_str(), cMo_, verbose);
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- vpMbKltTracker::reInitModel(I, cad_name, cMo_, verbose);
- vpMbEdgeTracker::reInitModel(I, cad_name, cMo_, verbose);
-}
-
-#endif //VISP_HAVE_OPENCV
diff --git a/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.h b/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.h
deleted file mode 100644
index 8d694da..0000000
--- a/src/tracking/mbt/hybrid/vpMbEdgeKltTracker.h
+++ /dev/null
@@ -1,362 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbEdgeKltTracker.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMbEdgeKltTracker.h
- \brief Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
-*/
-
-#ifndef vpMbEdgeKltTracker_HH
-#define vpMbEdgeKltTracker_HH
-
-#include <visp/vpConfig.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-#include <visp/vpRobust.h>
-#include <visp/vpSubMatrix.h>
-#include <visp/vpSubColVector.h>
-#include <visp/vpExponentialMap.h>
-#include <visp/vpMbTracker.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpPoseVector.h>
-#include <visp/vpMbtEdgeKltXmlParser.h>
-#include <visp/vpMbKltTracker.h>
-
-/*!
- \class vpMbEdgeKltTracker
- \ingroup ModelBasedTracking
- \warning This class is only available if OpenCV is installed, and used.
-
- \brief Hybrid tracker based on moving-edges and keypoints tracked using KLT
- tracker.
-
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
-
- The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
-
-\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Hybrid Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
- tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
-
- while(true){
- // Acquire a new image
- vpDisplay::display(I);
- tracker.track(I); // Track the object on this image
- tracker.getPose(cMo); // Get the pose
-
- tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
-#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-
- The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
-
-\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
- //acquire an image
- vpImageIo::read(I, "cube.pgm"); // Example of acquisition
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
- tracker.initFromPose(I, cMo); // initialise the tracker with the given pose.
-
- while(true){
- // acquire a new image
- tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
- }
-
-#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-
- Finally it can be used not to track an object but just to display a model at a
- given pose:
-
-\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Hybrid Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
-
- while(true){
- // acquire a new image
- // Get the pose using any method
- vpDisplay::display(I);
- tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
-#endif
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-*/
-class VISP_EXPORT vpMbEdgeKltTracker: public vpMbKltTracker, public vpMbEdgeTracker
-{
-protected:
- //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
- bool compute_interaction;
- //! The gain of the virtual visual servoing stage.
- double lambda;
- //! The threshold used in the robust estimation of KLT.
- double thresholdKLT;
- //! The threshold used in the robust estimation of MBT.
- double thresholdMBT;
- //! The maximum iteration of the virtual visual servoing stage.
- unsigned int maxIter;
-
-public:
-
- vpMbEdgeKltTracker();
- virtual ~vpMbEdgeKltTracker();
-
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
-
- /*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
-
- /*!
- Get the maximum iteration of the virtual visual servoing stage.
-
- \return the number of iteration
- */
- virtual inline unsigned int getMaxIter() const {return maxIter;}
-
- /*!
- Get the near distance for clipping.
-
- \return Near clipping value.
- */
- virtual inline double getNearClippingDistance() const { return vpMbKltTracker::getNearClippingDistance(); }
-
- void loadConfigFile(const char* configFile);
- virtual void loadConfigFile(const std::string& configFile);
-
- void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
- void resetTracker();
-
- virtual void setCameraParameters(const vpCameraParameters& cam);
-
- /*!
- Specify which clipping to use.
-
- \sa vpMbtPolygonClipping
-
- \param flags : New clipping flags.
- */
- virtual void setClipping(const unsigned int &flags) {vpMbEdgeTracker::setClipping(flags); }
-
- /*!
- Set the far distance for clipping.
-
- \param dist : Far clipping value.
- */
- virtual void setFarClippingDistance(const double &dist) { vpMbEdgeTracker::setFarClippingDistance(dist); }
-
- /*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain; vpMbEdgeTracker::setLambda(lambda); vpMbKltTracker::setLambda(lambda);}
-
- /*!
- Set the maximum iteration of the virtual visual servoing stage.
-
- \param max : the desired number of iteration
- */
- virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
-
- /*!
- Set the near distance for clipping.
-
- \param dist : Near clipping value.
- */
- virtual void setNearClippingDistance(const double &dist) { vpMbEdgeTracker::setNearClippingDistance(dist); }
-
- /*!
- Use Ogre3D for visibility tests
-
- \warning This function has to be called before the initialization of the tracker.
-
- \param v : True to use it, False otherwise
- */
- virtual void setOgreVisibilityTest(const bool &v){
- vpMbTracker::setOgreVisibilityTest(v);
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Hybrid");
-#endif
- }
-
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
-
- virtual void testTracking(){};
- virtual void track(const vpImage<unsigned char>& I);
-
-protected:
- void computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt,
- vpColVector &w_klt, const unsigned int lvl=0);
-
- virtual void init(const vpImage<unsigned char>& I);
- virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double r, const int idFace=0,
- const std::string &name="");
- virtual void initCylinder(const vpPoint&, const vpPoint &, const double r, const int idFace,
- const std::string &name="");
- virtual void initFaceFromCorners(vpMbtPolygon &polygon);
- virtual void initFaceFromLines(vpMbtPolygon &polygon);
- unsigned int initMbtTracking(const unsigned int level=0);
-
- bool postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
- const unsigned int lvl=0);
- void postTrackingMbt(vpColVector &w, const unsigned int level=0);
-
- unsigned int trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl = 0);
- void trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &_error,
- vpHomogeneousMatrix& cMo, const unsigned int lvl=0);
-};
-
-#endif
-
-#endif //VISP_HAVE_OPENCV
diff --git a/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.cpp b/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.cpp
deleted file mode 100644
index a61e627..0000000
--- a/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.cpp
+++ /dev/null
@@ -1,198 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtEdgeKltXmlParser.cpp 4577 2014-01-10 16:19:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using edges and point features).
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-
-#ifdef VISP_HAVE_XML2
-
-#include <iostream>
-#include <map>
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbtEdgeKltXmlParser.h>
-
-
-/*!
- Default constructor.
-
-*/
-vpMbtEdgeKltXmlParser::vpMbtEdgeKltXmlParser()
-{
- init();
-}
-
-/*!
- Default destructor.
-*/
-vpMbtEdgeKltXmlParser::~vpMbtEdgeKltXmlParser()
-{
-}
-
-/*!
- Initialise internal variables (including the map).
-*/
-void
-vpMbtEdgeKltXmlParser::init()
-{
- vpMbtXmlParser::init();
- vpMbtKltXmlParser::init();
-
- nodeMap["camera"] = vpMbtEdgeKltXmlParser::camera;
- nodeMap["face"] = vpMbtEdgeKltXmlParser::face;
- nodeMap["klt"] = vpMbtEdgeKltXmlParser::klt;
- nodeMap["ecm"] = vpMbtEdgeKltXmlParser::ecm;
- nodeMap["sample"] = vpMbtEdgeKltXmlParser::sample;
-}
-
-/*!
- Parse the file in parameters.
- This method is deprecated, use parse() instead.
-
- \param filename : File to parse.
-*/
-void
-vpMbtEdgeKltXmlParser::parse(const char * filename)
-{
- std::string file = filename;
- vpXmlParser::parse(file);
-}
-
-/*!
- Write info to file.
-
- \warning Useless, so not yet implemented => Throw exception.
-*/
-void
-vpMbtEdgeKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
-{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
-}
-
-/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
- \param doc : Document to parse.
- \param node : Root node.
-*/
-void
-vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
-{
- bool camera_node = false;
- bool face_node = false;
- bool ecm_node = false;
- bool sample_node = false;
- bool klt_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case vpMbtEdgeKltXmlParser::camera:{
- this->read_camera (doc, dataNode);
- camera_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::face:{
- this->read_face(doc, dataNode);
- face_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::klt:{
- this->read_klt(doc, dataNode);
- klt_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::ecm:{
- this->read_ecm (doc, dataNode);
- ecm_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::sample:{
- this->read_sample (doc, dataNode);
- sample_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
- }
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
- }
-
- if(!klt_node) {
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
- }
-
- if(!ecm_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- }
-
- if(!sample_node) {
- std::cout <<"sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
- std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
- }
-}
-
-#endif
-
diff --git a/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.h b/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.h
deleted file mode 100644
index 06208be..0000000
--- a/src/tracking/mbt/hybrid/vpMbtEdgeKltXmlParser.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtEdgeKltXmlParser.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using edges and point features).
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- * \file vpMbtEdgeKltXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
-
-#ifndef vpMbtEdgeKltXmlParser_HH
-#define vpMbtEdgeKltXmlParser_HH
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbtXmlParser.h>
-#include <visp/vpMbtKltXmlParser.h>
-
-/*!
- \class vpMbtEdgeKltXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup ModelBasedTracking
-
- Data parser for the model based tracker.
-
- */
-class VISP_EXPORT vpMbtEdgeKltXmlParser: public vpMbtXmlParser, public vpMbtKltXmlParser
-{
-protected:
- typedef enum{
- camera,
- face,
- klt,
- ecm,
- sample
- } dataToParseMbtEdgeKlt;
-public:
-
- vpMbtEdgeKltXmlParser();
- virtual ~vpMbtEdgeKltXmlParser();
-
- void parse(const char * filename);
-
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
-
- void writeMainClass(xmlNodePtr node);
-
-protected:
- void init();
-
-};
-
-#endif
-
-#endif /* NMBTEDGEKLTXMLPARSER_H_ */
-
-
-
diff --git a/src/tracking/mbt/klt/vpMbKltTracker.cpp b/src/tracking/mbt/klt/vpMbKltTracker.cpp
deleted file mode 100644
index 10eb319..0000000
--- a/src/tracking/mbt/klt/vpMbKltTracker.cpp
+++ /dev/null
@@ -1,1199 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbKltTracker.cpp 5189 2015-01-21 16:42:18Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Model based tracker using only KLT
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpImageConvert.h>
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpTrackingException.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-vpMbKltTracker::vpMbKltTracker()
- :
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cur(),
-#else
- cur(NULL),
-#endif
- c0Mo(), compute_interaction(true),
- firstInitialisation(true), maskBorder(5), lambda(0.8), maxIter(200), threshold_outlier(0.5),
- percentGood(0.6), ctTc0(), tracker(), firstTrack(false), kltPolygons(), cylinders_disp(), circles_disp()
-{
- tracker.setTrackerId(1);
- tracker.setUseHarris(1);
- tracker.setMaxFeatures(10000);
- tracker.setWindowSize(5);
- tracker.setQuality(0.01);
- tracker.setMinDistance(5);
- tracker.setHarrisFreeParameter(0.01);
- tracker.setBlockSize(3);
- tracker.setPyramidLevels(3);
-
- angleAppears = vpMath::rad(65);
- angleDisappears = vpMath::rad(75);
-
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Klt");
-#endif
-}
-
-/*!
- Basic destructor.
-
-*/
-vpMbKltTracker::~vpMbKltTracker()
-{
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
- cvReleaseImage(&cur);
- cur = NULL;
- }
-#endif
-
- // delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
- }
- kltpoly = NULL ;
- }
- kltPolygons.clear();
-
- // delete the structures used to display cylinders and circles
- vpMbtDistanceCylinder *cy;
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders_disp.begin(); it!=cylinders_disp.end(); ++it){
- cy = *it;
- if (cy!=NULL){
- delete cy ;
- }
- cy = NULL ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
- }
- ci = NULL ;
- }
-
- cylinders_disp.clear();
- circles_disp.clear();
-}
-
-void
-vpMbKltTracker::init(const vpImage<unsigned char>& I)
-{
- if(!modelInitialised){
- throw vpException(vpException::fatalError, "model not initialized");
- }
-
- bool reInitialisation = false;
- if(!useOgre)
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
- else{
-#ifdef VISP_HAVE_OGRE
- if(!faces.isOgreInitialised()){
- faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
- faces.initOgre(cam);
- }
-
- faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
-
-#else
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
-#endif
- }
- reinit(I);
-}
-
-void
-vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
-{
- c0Mo = cMo;
- ctTc0.setIdentity();
- firstTrack = false;
-
- vpImageConvert::convert(I, cur);
-
- cam.computeFov(I.getWidth(), I.getHeight());
-
- // mask
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
-#else
- IplImage* mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
- cvZero(mask);
-#endif
- unsigned char val = 255/* - i*15*/;
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- kltpoly->polygon->computeRoiClipped(cam);
- kltpoly->updateMask(mask, val, maskBorder);
- }
- }
-
- tracker.initTracking(cur, mask);
-// tracker.track(cur); // AY: Not sure to be usefull but makes sure that the points are valid for tracking and avoid too fast reinitialisations.
-// vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << " points" << std::endl;
-
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- kltpoly->init(tracker);
- }
- }
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvReleaseImage(&mask);
-#endif
-}
-
-/*!
- Reset the tracker. The model is removed and the pose is set to identity.
- The tracker needs to be initialized with a new model and a new pose.
-*/
-void
-vpMbKltTracker::resetTracker()
-{
- cMo.setIdentity();
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
- cvReleaseImage(&cur);
- cur = NULL;
- }
-#endif
-
- // delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
- }
- kltpoly = NULL ;
- }
- kltPolygons.clear();
-
- // delete the structures used to display cylinders and circles
- vpMbtDistanceCylinder *cy;
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders_disp.begin(); it!=cylinders_disp.end(); ++it){
- cy = *it;
- if (cy!=NULL){
- delete cy ;
- }
- cy = NULL ;
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
- }
- ci = NULL ;
- }
-
- cylinders_disp.clear();
- circles_disp.clear();
-
- compute_interaction = true;
- firstInitialisation = true;
- computeCovariance = false;
- firstTrack = false;
-
- tracker.setTrackerId(1);
- tracker.setUseHarris(1);
-
- tracker.setMaxFeatures(10000);
- tracker.setWindowSize(5);
- tracker.setQuality(0.01);
- tracker.setMinDistance(5);
- tracker.setHarrisFreeParameter(0.01);
- tracker.setBlockSize(3);
- tracker.setPyramidLevels(3);
-
- angleAppears = vpMath::rad(65);
- angleDisappears = vpMath::rad(75);
-
- clippingFlag = vpMbtPolygon::NO_CLIPPING;
-
- maskBorder = 5;
- threshold_outlier = 0.5;
- percentGood = 0.7;
-
- lambda = 0.8;
- maxIter = 200;
-
- faces.reset();
-
-#ifdef VISP_HAVE_OGRE
- useOgre = false;
-#endif
-}
-
-/*!
- Get the current list of KLT points.
-
- \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
-
- \return the list of KLT points through vpKltOpencv.
-*/
-std::vector<vpImagePoint>
-vpMbKltTracker::getKltImagePoints() const
-{
- std::vector<vpImagePoint> kltPoints;
- for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
- int id;
- float x_tmp, y_tmp;
- tracker.getFeature((int)i, id, x_tmp, y_tmp);
- kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
- }
-
- return kltPoints;
-}
-
-/*!
- Get the current list of KLT points and their id.
-
- \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
-
- \return the list of KLT points and their id through vpKltOpencv.
-*/
-std::map<int, vpImagePoint>
-vpMbKltTracker::getKltImagePointsWithId() const
-{
- std::map<int, vpImagePoint> kltPoints;
- for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
- int id;
- float x_tmp, y_tmp;
- tracker.getFeature((int)i, id, x_tmp, y_tmp);
- kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
- }
-
- return kltPoints;
-}
-
-/*!
- Set the new value of the klt tracker.
-
- \param t : Klt tracker containing the new values.
-*/
-void
-vpMbKltTracker::setKltOpencv(const vpKltOpencv& t){
- tracker.setMaxFeatures(t.getMaxFeatures());
- tracker.setWindowSize(t.getWindowSize());
- tracker.setQuality(t.getQuality());
- tracker.setMinDistance(t.getMinDistance());
- tracker.setHarrisFreeParameter(t.getHarrisFreeParameter());
- tracker.setBlockSize(t.getBlockSize());
- tracker.setPyramidLevels(t.getPyramidLevels());
-}
-
-/*!
- Set the camera parameters.
-
- \param camera : the new camera parameters.
-*/
-void
-vpMbKltTracker::setCameraParameters(const vpCameraParameters& camera)
-{
-// for (unsigned int i = 0; i < faces.size(); i += 1){
-// faces[i]->setCameraParameters(camera);
-// }
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- kltpoly->setCameraParameters(camera);
- }
-
- this->cam = camera;
-}
-
-/*!
- Set the pose to be used in entry of the next call to the track() function.
- This pose will be just used once.
-
- \warning This function has to be called after the initialization of the tracker.
-
- \param I : image corresponding to the desired pose.
- \param cdMo : Pose to affect.
-*/
-void
-vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
-{
- if(firstTrack)
- {
- bool reInitialisation = false;
- if(!useOgre)
- faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
- else{
-#ifdef VISP_HAVE_OGRE
- faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
-#else
- faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
-#endif
- }
- if(reInitialisation){
- std::cout << "WARNING: Visibility changed, must reinitialize to update pose" << std::endl;
- cMo = cdMo;
- reinit(I);
- }
- else{
- vpMbtDistanceKltPoints *kltpoly;
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- std::vector<cv::Point2f> initial_pts;
- std::vector<long> initial_ids;
-#else
- unsigned int nbp = 0;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints() )
- nbp += (*it)->getCurrentNumberPoints();
- }
-
- CvPoint2D32f* initial_pts = NULL;
- initial_pts = (CvPoint2D32f*)cvAlloc(nbp*sizeof(initial_pts[0]));
- long *initial_ids = new long [nbp];
- unsigned int iter_points = 0;
-#endif
- vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
- vpHomogeneousMatrix cMcd = cdMc.inverse();
-
- vpRotationMatrix cdRc;
- vpTranslationVector cdtc;
-
- cdMc.extract(cdRc);
- cdMc.extract(cdtc);
-
- unsigned int nbCur = 0;
-
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
- kltpoly = *it;
-
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints() ) {
-
- //Get the normal to the face at the current state cMo
- vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
- plan.changeFrame(cMcd);
-
- vpColVector Nc = plan.getNormal();
- Nc.normalize();
-
- double invDc = 1.0 / plan.getD();
-
- //Create the homography
- vpMatrix cdHc;
- vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
- cdHc /= cdHc[2][2];
-
- //Create the 2D homography
- vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
-
- //Points displacement
- std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
- nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
- for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
- vpColVector cdp(3);
- cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
-
- double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
-
- if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
- cdp[0] = 0.0;
- cdp[1] = 0.0;
- throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
- }
-
- cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
- cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
-
- //Set value to the KLT tracker
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Point2f p((float)cdp[0], (float)cdp[1]);
- initial_pts.push_back(p);
- initial_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
-#else
- initial_pts[iter_points].x = (float)cdp[0];
- initial_pts[iter_points].y = (float)cdp[1];
- initial_ids[iter_points++] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
-#endif
- }
- }
- }
-
- vpImageConvert::convert(I, cur);
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- tracker.initTracking(cur,initial_pts,initial_ids); // false to keep the id of the points
-#else
- tracker.initTracking(cur, initial_pts, initial_ids, iter_points);
-
- if(initial_pts) cvFree(&initial_pts);
- initial_pts = NULL;
- delete [] initial_ids;
-#endif
-
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- kltpoly->polygon->computeRoiClipped(cam);
- kltpoly->init(tracker);
- }
- }
-
- cMo = cdMo;
- c0Mo = cMo;
- ctTc0.setIdentity();
- firstTrack = false;
- }
- }
- else{
- cMo = cdMo;
- init(I);
- }
-}
-
-/*!
- Initialise a new face from the coordinates given in parameter.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
-{
- vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
- kltPoly->setCameraParameters(cam) ;
- kltPoly->polygon = &polygon;
- kltPolygons.push_back(kltPoly);
-}
-/*!
- Initialise a new face from the coordinates given in parameter.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
-*/
-void
-vpMbKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
-{
- vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
- kltPoly->setCameraParameters(cam) ;
- kltPoly->polygon = &polygon;
- kltPolygons.push_back(kltPoly);
-}
-
-/*!
- Achieve the tracking of the KLT features and associate the features to the faces.
-
- \param I : The input image.
- \param nbInfos : Size of the features.
- \param nbFaceUsed : Number of face used for the tracking.
-*/
-void
-vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
-{
- vpImageConvert::convert(I, cur);
- tracker.track(cur);
-
- if(!firstTrack)
- firstTrack = true;
-
- nbInfos = 0;
- nbFaceUsed = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- kltpoly->computeNbDetectedCurrent(tracker);
-// faces[i]->ransac();
- if(kltpoly->hasEnoughPoints()){
- nbInfos += kltpoly->getCurrentNumberPoints();
- nbFaceUsed++;
- }
- }
- }
-}
-
-/*!
- Realize the post tracking operations. Mostly visibility tests
-*/
-bool
-vpMbKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w)
-{
- // # For a better Post Tracking, tracker should reinitialize if so faces don't have enough points but are visible.
- // # Here we are not doing it for more speed performance.
- bool reInitialisation = false;
-
- unsigned int initialNumber = 0;
- unsigned int currentNumber = 0;
- unsigned int shift = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- initialNumber += kltpoly->getInitialNumberPoint();
- if(kltpoly->hasEnoughPoints()){
- vpSubColVector sub_w(w, shift, 2*kltpoly->getCurrentNumberPoints());
- kltpoly->removeOutliers(sub_w, threshold_outlier);
- shift += 2*kltpoly->getCurrentNumberPoints();
-
- currentNumber += kltpoly->getCurrentNumberPoints();
- }
-// else{
-// reInitialisation = true;
-// break;
-// }
- }
- }
-
-// if(!reInitialisation){
- double value = percentGood * (double)initialNumber;
- if((double)currentNumber < value){
-// std::cout << "Too many point disappear : " << initialNumber << "/" << currentNumber << std::endl;
- reInitialisation = true;
- }
- else{
- if(!useOgre)
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
- else{
-#ifdef VISP_HAVE_OGRE
- faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
-#else
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
-#endif
- }
- }
-// }
-
- if(reInitialisation)
- return true;
-
- return false;
-}
-
-/*!
- Realize the VVS loop for the tracking
-
- \param nbInfos : Size of the features
- \param w : weight of the features after M-Estimation.
-*/
-void
-vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
-{
- vpMatrix L; // interaction matrix
- vpColVector R; // residu
- vpMatrix L_true; // interaction matrix
- vpMatrix LVJ_true;
- //vpColVector R_true; // residu
- vpColVector v; // "speed" for VVS
- vpHomography H;
- vpColVector w_true;
- vpRobust robust(2*nbInfos);
-
- vpMatrix LTL, LTR;
- vpHomogeneousMatrix cMoPrev;
-
- double normRes = 0;
- double normRes_1 = -1;
- unsigned int iter = 0;
-
- R.resize(2*nbInfos);
- L.resize(2*nbInfos, 6, 0);
-
- while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
-
- unsigned int shift = 0;
- vpMbtDistanceKltPoints *kltpoly;
- // for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2 &&
- kltpoly->hasEnoughPoints()){
- vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
- vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
- try{
- kltpoly->computeHomography(ctTc0, H);
- kltpoly->computeInteractionMatrixAndResidu(subR, subL);
- }catch(...){
- throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
- }
-
- shift += 2*kltpoly->getCurrentNumberPoints();
- }
- }
-
- /* robust */
- if(iter == 0){
- w_true.resize(2*nbInfos);
- w.resize(2*nbInfos);
- w = 1;
- w_true = 1;
- }
- robust.setIteration(iter);
- robust.setThreshold(2/cam.get_px());
- robust.MEstimator( vpRobust::TUKEY, R, w);
-
- m_error = R;
- if(computeCovariance){
- L_true = L;
- if(!isoJoIdentity){
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- LVJ_true = (L*cVo*oJo);
- }
- }
-
- normRes_1 = normRes;
- normRes = 0;
- for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
- w_true[i] = w[i];
- R[i] = R[i] * w[i];
- normRes += R[i];
- }
-
- if((iter == 0) || compute_interaction){
- for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
- for(unsigned int j=0; j<6; j++){
- L[i][j] *= w[i];
- }
- }
- }
-
- if(isoJoIdentity){
- LTL = L.AtA();
- computeJTR(L, R, LTR);
- v = -lambda * LTL.pseudoInverse(1e-16) * LTR;
- }
- else{
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpMatrix LVJTR;
- computeJTR(LVJ, R, LVJTR);
- v = -lambda*LVJTLVJ.pseudoInverse(1e-16)*LVJTR;
- v = cVo * v;
- }
-
- ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
- cMoPrev = cMo;
- cMo = ctTc0 * c0Mo;
-
- iter++;
- }
-
- if(computeCovariance){
- vpMatrix D;
- D.diag(w_true);
-
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity){
- computeCovarianceMatrix(cMoPrev,m_error,L_true,D);
- }
- else{
- computeCovarianceMatrix(cMoPrev,m_error,LVJ_true,D);
- }
- }
-}
-
-/*!
- Realize the tracking of the object in the image
-
- \throw vpException : if the tracking is supposed to have failed
-
- \param I : the input image
-*/
-void
-vpMbKltTracker::track(const vpImage<unsigned char>& I)
-{
- unsigned int nbInfos = 0;
- unsigned int nbFaceUsed = 0;
-
- try{
- preTracking(I, nbInfos, nbFaceUsed);
- }
- catch(vpException &e){
- throw e;
- }
-
- if(nbInfos < 4 || nbFaceUsed == 0){
- vpERROR_TRACE("\n\t\t Error-> not enough data") ;
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
- }
-
- //vpColVector w;
- computeVVS(nbInfos, m_w);
-
- if(postTracking(I, m_w))
- reinit(I);
-}
-
-/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \param configFile : full name of the xml file.
-
- \sa loadConfigFile(const char*), vpXmlParser::cleanup()
-*/
-void
-vpMbKltTracker::loadConfigFile(const std::string& configFile)
-{
- vpMbKltTracker::loadConfigFile(configFile.c_str());
-}
-
-/*!
- Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
-
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
-
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
-
- \param configFile : full name of the xml file.
-
- The XML configuration file has the following form:
- \code
-<?xml version="1.0"?>
-<conf>
- <camera>
- <width>640</width>
- <height>480</height>
- <u0>320</u0>
- <v0>240</v0>
- <px>686.24</px>
- <py>686.24</py>
- </camera>
- <face>
- <angle_appear>65</angle_appear>
- <angle_disappear>85</angle_disappear>
- <near_clipping>0.01</near_clipping>
- <far_clipping>0.90</far_clipping>
- <fov_clipping>1</fov_clipping>
- </face>
- <klt>
- <mask_border>10</mask_border>
- <max_features>10000</max_features>
- <window_size>5</window_size>
- <quality>0.02</quality>
- <min_distance>10</min_distance>
- <harris>0.02</harris>
- <size_block>3</size_block>
- <pyramid_lvl>3</pyramid_lvl>
- </klt>
-</conf>
- \endcode
-
- \sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
-*/
-void
-vpMbKltTracker::loadConfigFile(const char* configFile)
-{
-#ifdef VISP_HAVE_XML2
- vpMbtKltXmlParser xmlp;
-
- xmlp.setMaxFeatures(10000);
- xmlp.setWindowSize(5);
- xmlp.setQuality(0.01);
- xmlp.setMinDistance(5);
- xmlp.setHarrisParam(0.01);
- xmlp.setBlockSize(3);
- xmlp.setPyramidLevels(3);
- xmlp.setMaskBorder(maskBorder);
- xmlp.setAngleAppear(vpMath::deg(angleAppears));
- xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
-
- try{
- std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
- xmlp.parse(configFile);
- }
- catch(...){
- vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
- throw vpException(vpException::ioError, "problem to parse configuration file.");
- }
-
- vpCameraParameters camera;
- xmlp.getCameraParameters(camera);
- setCameraParameters(camera);
-
- tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
- tracker.setWindowSize((int)xmlp.getWindowSize());
- tracker.setQuality(xmlp.getQuality());
- tracker.setMinDistance(xmlp.getMinDistance());
- tracker.setHarrisFreeParameter(xmlp.getHarrisParam());
- tracker.setBlockSize((int)xmlp.getBlockSize());
- tracker.setPyramidLevels((int)xmlp.getPyramidLevels());
- maskBorder = xmlp.getMaskBorder();
- angleAppears = vpMath::rad(xmlp.getAngleAppear());
- angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
-
- if(xmlp.hasNearClippingDistance())
- setNearClippingDistance(xmlp.getNearClippingDistance());
-
- if(xmlp.hasFarClippingDistance())
- setFarClippingDistance(xmlp.getFarClippingDistance());
-
- if(xmlp.getFovClipping())
- setClipping(clippingFlag = clippingFlag | vpMbtPolygon::FOV_CLIPPING);
-
-#else
- vpTRACE("You need the libXML2 to read the config file %s", configFile);
-#endif
-}
-
-/*!
- Display the 3D model at a given position using the given camera parameters
-
- \param I : The image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are visible.
-*/
-void
-vpMbKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel)
-{
- vpCameraParameters c = camera;
-
- if(clippingFlag > 3) // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
-
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFullModel || kltpoly->polygon->isVisible())
- {
- kltpoly->polygon->changeFrame(cMo_);
- kltpoly->polygon->computeRoiClipped(c);
- std::vector<std::pair<vpImagePoint,unsigned int> > roi;
- kltpoly->polygon->getRoiClipped(c, roi);
-
- for (unsigned int j = 0; j < roi.size(); j += 1){
- if(((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
- vpImagePoint ip1, ip2;
- ip1 = roi[j].first;
- ip2 = roi[(j+1)%roi.size()].first;
-
- vpDisplay::displayLine (I, ip1, ip2, col, thickness);
- }
- }
- }
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
-// faces[i]->displayNormal(I);
- }
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders_disp.begin(); it!=cylinders_disp.end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness);
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-/*!
- Display the 3D model at a given position using the given camera parameters
-
- \param I : The color image.
- \param cMo_ : Pose used to project the 3D model into the image.
- \param camera : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are not visible.
-*/
-void
-vpMbKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
- const vpColor& col , const unsigned int thickness, const bool displayFullModel)
-{
- vpCameraParameters c = camera;
-
- if(clippingFlag > 3) // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
-
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFullModel || kltpoly->polygon->isVisible())
- {
- kltpoly->polygon->changeFrame(cMo_);
- kltpoly->polygon->computeRoiClipped(c);
- std::vector<std::pair<vpImagePoint,unsigned int> > roi;
- kltpoly->polygon->getRoiClipped(c, roi);
-
- for (unsigned int j = 0; j < roi.size(); j += 1){
- if(((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
- vpImagePoint ip1, ip2;
- ip1 = roi[j].first;
- ip2 = roi[(j+1)%roi.size()].first;
-
- vpDisplay::displayLine (I, ip1, ip2, col, thickness);
- }
- }
- }
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
-// faces[i]->displayNormal(I);
- }
- }
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders_disp.begin(); it!=cylinders_disp.end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness);
- }
-
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness);
- }
-
-#ifdef VISP_HAVE_OGRE
- if(useOgre)
- faces.displayOgre(cMo_);
-#endif
-}
-
-/*!
- Test the quality of the tracking.
- The tracking is supposed to fail if less than 10 points are tracked.
-
- \todo Find a efficient way to test the quality.
-
- \throw vpTrackingException::fatalError if the test fails.
-*/
-void
-vpMbKltTracker::testTracking()
-{
- unsigned int nbTotalPoints = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
- nbTotalPoints += kltpoly->getCurrentNumberPoints();
- }
- }
-
- if(nbTotalPoints < 10){
- std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
- throw vpTrackingException(vpTrackingException::fatalError,
- "test tracking failed (too few points to realize a good tracking).");
- }
-}
-
-/*!
- Add a cylinder to display (not for tracking) from two points on the axis (defining the length of
- the cylinder) and its radius.
-
- \param p1 : First point on the axis.
- \param p2 : Second point on the axis.
- \param radius : Radius of the cylinder.
- \param name : The optional name of the cylinder.
-*/
-void
-vpMbKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int /*idFace*/,
- const std::string &name)
-{
- addCylinder(p1, p2, radius, name);
-}
-
-/*!
- Add a cylinder to the list of cylinders.
-
- \param P1 : The first extremity of the axis.
- \param P2 : The second extremity of the axis.
- \param r : The radius of the cylinder.
- \param name : The optional name of the cylinder.
-*/
-void
-vpMbKltTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string &name)
-{
- bool already_here = false ;
- vpMbtDistanceCylinder *cy ;
-
-// for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders_disp.begin(); it!=cylinders_disp.end(); ++it){
-// cy = *it;
-// if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
-// (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
-// already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
-// }
-// }
-
- if (!already_here){
- cy = new vpMbtDistanceCylinder ;
-
- cy->setCameraParameters(cam);
- cy->setName(name);
- cy->buildFrom(P1, P2, r);
- cylinders_disp.push_back(cy);
- }
-}
-
-/*!
- Add a circle to display (not for tracking) from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
-
- \param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param name : The optional name of the circle.
-*/
-void
-vpMbKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int /*idFace*/, const std::string &name)
-{
- addCircle(p1, p2, p3, radius, name);
-}
-
-/*!
- Add a circle to the list of circles.
-
- \param P1 : Center of the circle.
- \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
- \param name : Name of the circle.
-*/
-void
-vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name)
-{
- bool already_here = false ;
- vpMbtDistanceCircle *ci ;
-
-// for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
-// ci = *it;
-// if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
-// (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
-// already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
-// }
-// }
-
- if (!already_here){
- ci = new vpMbtDistanceCircle ;
-
- ci->setCameraParameters(cam);
- ci->setName(name);
- ci->buildFrom(P1, P2, P3, r);
- circles_disp.push_back(ci);
- }
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- reInitModel(I, cad_name.c_str(), cMo_, verbose);
-}
-
-/*!
- Re-initialize the model used by the tracker.
-
- \param I : The image containing the object to initialize.
- \param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
-{
- this->cMo.setIdentity();
-
-#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
- cvReleaseImage(&cur);
- cur = NULL;
- }
-#endif
-
- firstInitialisation = true;
- firstTrack = false;
-
- faces.reset();
-
- loadModel(cad_name, verbose);
- initFromPose(I, cMo_);
-}
-
-#endif //VISP_HAVE_OPENCV
diff --git a/src/tracking/mbt/klt/vpMbKltTracker.h b/src/tracking/mbt/klt/vpMbKltTracker.h
deleted file mode 100644
index 4fdaa2b..0000000
--- a/src/tracking/mbt/klt/vpMbKltTracker.h
+++ /dev/null
@@ -1,422 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbKltTracker.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Model based tracker using only KLT
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-/*!
- \file vpMbKltTracker.h
- \brief Model based tracker using only KLT
-*/
-
-#ifndef vpMbKltTracker_h
-#define vpMbKltTracker_h
-
-#include <visp/vpConfig.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-#include <visp/vpMbTracker.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpMbtKltXmlParser.h>
-#include <visp/vpHomography.h>
-#include <visp/vpRobust.h>
-#include <visp/vpSubColVector.h>
-#include <visp/vpSubMatrix.h>
-#include <visp/vpExponentialMap.h>
-//#include <visp/vpMbtKltPolygon.h>
-#include <visp/vpMbtDistanceKltPoints.h>
-#include <visp/vpMbtDistanceCircle.h>
-#include <visp/vpMbtDistanceCylinder.h>
-
-/*!
- \class vpMbKltTracker
- \ingroup ModelBasedTracking
- \warning This class is only available if OpenCV is installed, and used.
-
- \brief Model based tracker using only KLT
-
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
-
- The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
-
-\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbKltTracker tracker; // Create a model based tracker via KLT points.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Klt Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // Load the 3d model in cao format. No 3rd party library is required
- tracker.initClick(I, "cube.init"); // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the cube.init file.
-
- while(true){
- // Acquire a new image
- vpDisplay::display(I);
- tracker.track(I); // Track the object on this image
- tracker.getPose(cMo); // Get the pose
-
- tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
-#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-
- The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
-
-\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
- //acquire an image
- vpImageIo::read(I, "cube.pgm"); // Example of acquisition
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
- tracker.initFromPose(I, cMo); // initialize the tracker with the given pose.
-
- while(true){
- // acquire a new image
- tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
- }
-
-#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-
- Finally it can be used not to track an object but just to display a model at a
- given pose:
-
-\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplayX.h>
-
-int main()
-{
-#if defined VISP_HAVE_OPENCV
- vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
- vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
- vpCameraParameters cam;
-
- // Acquire an image
- vpImageIo::read(I, "cube.pgm");
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
- display.init(I,100,100,"Mb Klt Tracker");
-#endif
-
-#if defined VISP_HAVE_XML2
- tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
-#endif
- tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file).
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
-
- while(true){
- // acquire a new image
- // Get the pose using any method
- vpDisplay::display(I);
- tracker.display(I, cMo, cam, vpColor::darkRed, 1, true); // Display the model at the computed pose.
- vpDisplay::flush(I);
- }
-
-#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
-
- return 0;
-#endif
-}
-\endcode
-*/
-class VISP_EXPORT vpMbKltTracker: virtual public vpMbTracker
-{
-protected:
- //! Temporary OpenCV image for fast conversion.
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat cur;
-#else
- IplImage *cur;
-#endif
- //! Initial pose.
- vpHomogeneousMatrix c0Mo;
- //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
- bool compute_interaction;
- //! Flag to specify whether the init method is called the first or not (specific calls to realize in this case).
- bool firstInitialisation;
- //! Erosion of the mask
- unsigned int maskBorder;
- //! The gain of the virtual visual servoing stage.
- double lambda;
- //! The maximum iteration of the virtual visual servoing stage.
- unsigned int maxIter;
- //! Threshold below which the weight associated to a point to consider this one as an outlier.
- double threshold_outlier;
- //! Percentage of good points, according to the initial number, that must have the tracker.
- double percentGood;
- //! The estimated displacement of the pose between the current instant and the initial position.
- vpHomogeneousMatrix ctTc0;
- //! Points tracker.
- vpKltOpencv tracker;
- //! First track() called
- bool firstTrack;
- //! Vector of the cylinders used here only to display the full model.
- std::list<vpMbtDistanceKltPoints*> kltPolygons;
- //! Vector of the cylinders used here only to display the full model.
- std::list<vpMbtDistanceCylinder*> cylinders_disp;
- //! Vector of the circles used here only to display the full model.
- std::list<vpMbtDistanceCircle*> circles_disp;
-
-public:
- vpMbKltTracker();
- virtual ~vpMbKltTracker();
-
- void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="");
- void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string &name="");
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, const vpColor& col, const unsigned int thickness=1,
- const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel = false);
-
-protected:
- virtual void init(const vpImage<unsigned char>& I);
- virtual void reinit(const vpImage<unsigned char>& I);
-
-public:
- /*! Return the address of the circle feature list. */
- std::list<vpMbtDistanceCircle*> &getFeaturesCircle() { return circles_disp; }
- /*! Return the address of the cylinder feature list. */
- std::list<vpMbtDistanceCylinder*> &getFeaturesCylinder() { return cylinders_disp; }
- /*! Return the address of the Klt feature list. */
- std::list<vpMbtDistanceKltPoints*> &getFeaturesKlt() { return kltPolygons; }
- virtual void loadConfigFile(const std::string& configFile);
- void loadConfigFile(const char* configFile);
-
- /*!
- Get the current list of KLT points.
-
- \return the list of KLT points through vpKltOpencv.
- */
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- inline std::vector<cv::Point2f> getKltPoints() {return tracker.getFeatures();}
-#else
- inline CvPoint2D32f* getKltPoints() {return tracker.getFeatures();}
-#endif
-
- std::vector<vpImagePoint> getKltImagePoints() const;
-
- std::map<int, vpImagePoint> getKltImagePointsWithId() const;
-
- /*!
- Get the klt tracker at the current state.
-
- \return klt tracker.
- */
- inline vpKltOpencv getKltOpencv() const { return tracker; }
-
- /*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
-
- /*!
- Get the erosion of the mask used on the Model faces.
-
- \return The erosion.
- */
- inline unsigned int getMaskBorder() const { return maskBorder; }
-
- /*!
- Get the maximum iteration of the virtual visual servoing stage.
-
- \return the number of iteration
- */
- virtual inline unsigned int getMaxIter() const {return maxIter;}
-
- /*!
- Get the current number of klt points.
-
- \return the number of features
- */
- inline int getNbKltPoints() const {return tracker.getNbFeatures();}
-
- /*!
- Get the threshold for the acceptation of a point.
-
- \return threshold_outlier : Threshold for the weight below which a point is rejected.
- */
- inline double getThresholdAcceptation() const { return threshold_outlier;}
-
- void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
- void resetTracker();
-
- void setCameraParameters(const vpCameraParameters& cam);
-
- void setKltOpencv(const vpKltOpencv& t);
-
- /*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain;}
-
- /*!
- Set the erosion of the mask used on the Model faces.
-
- \param e : The desired erosion.
- */
- inline void setMaskBorder(const unsigned int &e){ maskBorder = e; }
-
- /*!
- Set the maximum iteration of the virtual visual servoing stage.
-
- \param max : the desired number of iteration
- */
- virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
-
- /*!
- Use Ogre3D for visibility tests
-
- \warning This function has to be called before the initialization of the tracker.
-
- \param v : True to use it, False otherwise
- */
- virtual void setOgreVisibilityTest(const bool &v){
- vpMbTracker::setOgreVisibilityTest(v);
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Klt");
-#endif
- }
-
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
-
- /*!
- Set the threshold for the acceptation of a point.
-
- \param th : Threshold for the weight below which a point is rejected.
- */
- inline void setThresholdAcceptation(const double th) {threshold_outlier = th;}
-
- virtual void testTracking();
- virtual void track(const vpImage<unsigned char>& I);
-
-protected:
- void computeVVS(const unsigned int &nbInfos, vpColVector &w);
-
- virtual void initFaceFromCorners(vpMbtPolygon &polygon);
- virtual void initFaceFromLines(vpMbtPolygon &polygon);
- virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double, const int,
- const std::string &name="");
- virtual void initCylinder(const vpPoint&, const vpPoint &, const double, const int,
- const std::string &name="");
-
- void preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed);
- bool postTracking(const vpImage<unsigned char>& I, vpColVector &w);
-};
-
-#endif
-#endif // VISP_HAVE_OPENCV
diff --git a/src/tracking/mbt/klt/vpMbtDistanceKltPoints.cpp b/src/tracking/mbt/klt/vpMbtDistanceKltPoints.cpp
deleted file mode 100644
index 8d8a8cf..0000000
--- a/src/tracking/mbt/klt/vpMbtDistanceKltPoints.cpp
+++ /dev/null
@@ -1,555 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceKltPoints.cpp 4661 2014-02-10 19:34:58Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Klt polygon, containing points of interest.
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpMbtDistanceKltPoints.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-/*!
- Basic constructor.
-
-*/
-vpMbtDistanceKltPoints::vpMbtDistanceKltPoints()
- : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
- nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
- cam(), polygon(NULL)
-{
- initPoints = std::map<int, vpImagePoint>();
- curPoints = std::map<int, vpImagePoint>();
- curPointsInd = std::map<int, int>();
-}
-
-/*!
- Basic destructor.
-
-*/
-vpMbtDistanceKltPoints::~vpMbtDistanceKltPoints()
-{}
-
-/*!
- Initialise the face to track. All the points in the map, representing all the
- map detected in the image, are parsed in order to extract the id of the points
- that are indeed in the face.
-
- \param _tracker : ViSP OpenCV KLT Tracker.
-*/
-void
-vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
-{
- // extract ids of the points in the face
- nbPointsInit = 0;
- nbPointsCur = 0;
- initPoints = std::map<int, vpImagePoint>();
- curPoints = std::map<int, vpImagePoint>();
- curPointsInd = std::map<int, int>();
- std::vector<vpImagePoint> roi;
- polygon->getRoiClipped(cam, roi);
-
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
- int id;
- float x_tmp, y_tmp;
- _tracker.getFeature((int)i, id, x_tmp, y_tmp);
-
- if(isInside(roi, y_tmp, x_tmp)){
- initPoints[id] = vpImagePoint(y_tmp, x_tmp);
- curPoints[id] = vpImagePoint(y_tmp, x_tmp);
- curPointsInd[id] = (int)i;
- nbPointsInit++;
- nbPointsCur++;
- }
- }
-
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
-
- // initialisation of the value for the computation in SE3
- vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), polygon->getPoint(2));
-
- d0 = plan.getD();
- N = plan.getNormal();
-
- N.normalize();
- N_cur = N;
- invd0 = 1.0 / d0;
-}
-
-/*!
- compute the number of point in this instanciation of the tracker that corresponds
- to the points of the face
-
- \param _tracker : the KLT tracker
- \return the number of points that are tracked in this face and in this instanciation of the tracker
-*/
-unsigned int
-vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
-{
- int id;
- float x, y;
- nbPointsCur = 0;
- curPoints = std::map<int, vpImagePoint>();
- curPointsInd = std::map<int, int>();
-
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
- _tracker.getFeature((int)i, id, x, y);
- if(isTrackedFeature(id)){
- curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
- curPointsInd[id] = (int)i;
- nbPointsCur++;
- }
- }
-
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
-
- return nbPointsCur;
-}
-
-/*!
- Compute the interaction matrix and the residu vector for the face.
- The method assumes that these two objects are properly sized in order to be
- able to improve the speed with the use of SubCoVector and subMatrix.
-
- \warning The function preCompute must be called before the this method.
-
- \param _R : the residu vector
- \param _J : the interaction matrix
-*/
-void
-vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J)
-{
- unsigned int index_ = 0;
-
- std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- int id(iter->first);
- double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
-
- double x_cur(0), y_cur(0);
- vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
-
- vpImagePoint iP0 = initPoints[id];
- double x0(0), y0(0);
- vpPixelMeterConversion::convertPoint(cam, iP0, x0, y0);
-
- double x0_transform, y0_transform ;// equivalent x and y in the first image (reference)
- computeP_mu_t(x0, y0, x0_transform, y0_transform, H );
-
- double invZ = compute_1_over_Z(x_cur, y_cur);
-
- _J[2*index_][0] = - invZ;
- _J[2*index_][1] = 0;
- _J[2*index_][2] = x_cur * invZ;
- _J[2*index_][3] = x_cur * y_cur;
- _J[2*index_][4] = -(1+x_cur*x_cur);
- _J[2*index_][5] = y_cur;
-
- _J[2*index_+1][0] = 0;
- _J[2*index_+1][1] = - invZ;
- _J[2*index_+1][2] = y_cur * invZ;
- _J[2*index_+1][3] = (1+y_cur*y_cur);
- _J[2*index_+1][4] = - y_cur * x_cur;
- _J[2*index_+1][5] = - x_cur;
-
- _R[2*index_] = (x0_transform - x_cur);
- _R[2*index_+1] = (y0_transform - y_cur);
- index_++;
- }
-}
-
-double
-vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
-{
- double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
- double den = -(d0 - dt);
- return num/den;
-}
-
-/*!
- Compute the new coordinates of a point given by its \f$(x,y)\f$ coordinates
- after the homography.
-
- \f$ P_t = {}^{c_t}H_{c_0} . P_0 \f$
-
- \param x_in : the x coordinates of the input point
- \param y_in : the y coordinates of the input point
- \param x_out : the x coordinates of the output point
- \param y_out : the y coordinates of the output point
- \param _cHc0 : the homography used to transfer the point
-*/
-inline void
-vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& _cHc0)
-{
- double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];
-
- if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
- x_out = 0.0;
- y_out = 0.0;
- throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
- }
-
- x_out = (x_in * _cHc0[0][0] + y_in * _cHc0[0][1] + _cHc0[0][2]) / p_mu_t_2;
- y_out = (x_in * _cHc0[1][0] + y_in * _cHc0[1][1] + _cHc0[1][2]) / p_mu_t_2;
-}
-
-/*!
- compute the homography using a displacement matrix.
-
- the homography is given by:
-
- \f$ {}^cH_{c_0} = {}^cR_{c_0} + \frac{{}^cT_{c_0} . {}^tN}{d_0} \f$
-
- Several internal variables are computed (dt, cRc0_0n)
-
- \param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
- \param _cHc0 : the homography of the plane
-*/
-void
-vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
-{
- vpRotationMatrix cRc0;
- vpTranslationVector ctransc0;
-
- _cTc0.extract(cRc0);
- _cTc0.extract(ctransc0);
- vpMatrix cHc0(_cHc0);
-
-// vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
- vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
- cHc0 /= cHc0[2][2];
-
- H = cHc0;
-
-// vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
-// vpQuaternionVector RotQuat(cRc0);
-// vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
-// vpQuaternionVector partial = RotQuat * NQuat;
-// vpQuaternionVector resQuat = (partial * RotQuatConj);
-//
-// cRc0_0n = vpColVector(3);
-// cRc0_0n[0] = resQuat.x();
-// cRc0_0n[1] = resQuat.y();
-// cRc0_0n[2] = resQuat.z();
-
- cRc0_0n = cRc0*N;
-
-// vpPlane p(corners[0], corners[1], corners[2]);
-// vpColVector Ncur = p.getNormal();
-// Ncur.normalize();
- N_cur = cRc0_0n;
- dt = 0.0;
- for (unsigned int i = 0; i < 3; i += 1){
- dt += ctransc0[i] * (N_cur[i]);
- }
-}
-
-/*!
- Test whether the feature with identifier id in paramters is in the list of tracked
- features.
-
- \param _id : the id of the current feature to test
- \return true if the id is in the list of tracked feature
-*/
-bool
-vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
-{
-// std::map<int, vpImagePoint>::const_iterator iter = initPoints.begin();
-// while(iter != initPoints.end()){
-// if(iter->first == _id){
-// return true;
-// }
-// iter++;
-// }
-
- std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
- if(iter != initPoints.end())
- return true;
-
- return false;
-}
-
-/*!
- Modification of all the pixels that are in the roi to the value of _nb (
- default is 255).
-
- \param mask : the mask to update (0, not in the object, _nb otherwise).
- \param nb : Optionnal value to set to the pixels included in the face.
- \param shiftBorder : Optionnal shift for the border in pixel (sort of built-in erosion) to avoid to consider pixels near the limits of the face.
-*/
-void
-vpMbtDistanceKltPoints::updateMask(
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- cv::Mat &mask,
-#else
- IplImage* mask,
-#endif
- unsigned char nb, unsigned int shiftBorder)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- int width = mask.cols;
- int height = mask.rows;
-#else
- int width = mask->width;
- int height = mask->height;
-#endif
-
- int i_min, i_max, j_min, j_max;
- std::vector<vpImagePoint> roi;
- polygon->getRoiClipped(cam, roi);
- vpMbtPolygon::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);
-
- /* check image boundaries */
- if(i_min > height){ //underflow
- i_min = 0;
- }
- if(i_max > height){
- i_max = height;
- }
- if(j_min > width){ //underflow
- j_min = 0;
- }
- if(j_max > width){
- j_max = width;
- }
-
- double shiftBorder_d = (double) shiftBorder;
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpMbtDistanceKltPoints::isInside(roi, i_d, j_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
- mask.at<unsigned char>(i,j) = nb;
- }
- }
- else{
- if(vpMbtDistanceKltPoints::isInside(roi, i, j)){
- mask.at<unsigned char>(i,j) = nb;
- }
- }
- }
- }
-#else
- unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpMbtDistanceKltPoints::isInside(roi, i_d, j_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
- *(ptrData++) = nb;
- }
- else{
- ptrData++;
- }
- }
- else{
- if(vpMbtDistanceKltPoints::isInside(roi, i, j)){
- *(ptrData++) = nb;
- }
- else{
- ptrData++;
- }
- }
- }
- ptrData += mask->widthStep - j_max + j_min;
- }
-#endif
-}
-
-/*!
- This method removes the outliers. A point is considered as outlier when its
- associated weight is below a given threshold (threshold_outlier).
-
- \param _w : Vector containing the weight of all the tracked points.
- \param threshold_outlier : Threshold to specify wether or not a point has to be deleted.
-*/
-void
-vpMbtDistanceKltPoints::removeOutliers(const vpColVector& _w, const double &threshold_outlier)
-{
- std::map<int, vpImagePoint> tmp;
- std::map<int, int> tmp2;
- unsigned int nbSupp = 0;
- unsigned int k = 0;
-
- nbPointsCur = 0;
- std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
-// if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
- tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
- tmp2[iter->first] = curPointsInd[iter->first];
- nbPointsCur++;
- }
- else{
- nbSupp++;
- initPoints.erase(iter->first);
- }
-
- k+=2;
- }
-
- if(nbSupp != 0){
- curPoints = std::map<int, vpImagePoint>();
- curPointsInd = std::map<int, int>();
-
- curPoints = tmp;
- curPointsInd = tmp2;
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
- }
-}
-
-/*!
- Display the primitives tracked for the face.
-
- \param _I : The image where to display.
-*/
-void
-vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
-{
- std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- int id(iter->first);
- vpImagePoint iP;
- iP.set_i(static_cast<double>(iter->second.get_i()));
- iP.set_j(static_cast<double>(iter->second.get_j()));
-
- vpDisplay::displayCross(_I, iP, 10, vpColor::red);
-
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
- }
-}
-
-/*!
- Display the primitives tracked for the face.
-
- \param _I : The image where to display.
-*/
-void
-vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa>& _I)
-{
- std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- int id(iter->first);
- vpImagePoint iP;
- iP.set_i(static_cast<double>(iter->second.get_i()));
- iP.set_j(static_cast<double>(iter->second.get_j()));
-
- vpDisplay::displayCross(_I, iP, 10, vpColor::red);
-
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
- }
-}
-
-//###################################
-// Static functions
-//###################################
-
-bool
-vpMbtDistanceKltPoints::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double i_test, const double j_test, const double i, const double j)
-{
- double dx = p2.get_j() - p1.get_j();
- double dy = p2.get_i() - p1.get_i();
- double ex = j - j_test;
- double ey = i - i_test;
-
- double den = dx * ey - dy * ex;
- double t = 0, u = 0;
- //if(den != 0){
- if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
- t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
- u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
- }
- else{
- throw vpException(vpException::divideByZeroError, "denominator null");
- }
- return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
-}
-
-bool
-vpMbtDistanceKltPoints::isInside(const std::vector<vpImagePoint>& roi, const double i, const double j)
-{
- double i_test = 100000.;
- double j_test = 100000.;
- unsigned int nbInter = 0;
- bool computeAgain = true;
-
- if(computeAgain){
- computeAgain = false;
- for(unsigned int k=0; k< roi.size(); k++){
- try{
- if(vpMbtDistanceKltPoints::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
- nbInter++;
- }
- }
- catch(...){
- computeAgain = true;
- break;
- }
- }
-
- if(computeAgain){
- i_test += 100;
- j_test -= 100;
- nbInter = 0;
- }
- }
- return ((nbInter%2) == 1);
-}
-
-#endif
diff --git a/src/tracking/mbt/klt/vpMbtDistanceKltPoints.h b/src/tracking/mbt/klt/vpMbtDistanceKltPoints.h
deleted file mode 100644
index 5bcf43d..0000000
--- a/src/tracking/mbt/klt/vpMbtDistanceKltPoints.h
+++ /dev/null
@@ -1,204 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtDistanceKltPoints.h 4661 2014-02-10 19:34:58Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Klt polygon, containing points of interest.
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#ifndef vpMbtDistanceKltPoints_h
-#define vpMbtDistanceKltPoints_h
-
-#include <visp/vpConfig.h>
-
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-
-#include <map>
-
-#include <visp/vpMbtPolygon.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpPlane.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpGEMM.h>
-#include <visp/vpHomography.h>
-#include <visp/vpPlot.h>
-#include <visp/vpMbHiddenFaces.h>
-
-/*!
- \class vpMbtDistanceKltPoints
-
- \brief Implementation of a polygon of the model containing points of interest. It is used by the model-based tracker KLT, and hybrid.
-
- \warning This class is only available if OpenCV is installed, and used.
-
- \ingroup ModelBasedTracking
-*/
-class VISP_EXPORT vpMbtDistanceKltPoints
-{
-private:
- //! the homography in meter
- vpMatrix H;
- //! normal to the initial plane
- vpColVector N;
- //! current normal
- vpColVector N_cur;
- //! inverse of the distance between the plane and the camera at the initial position (speed up computation)
- double invd0;
- //! cRc0_0n (temporary variable to speed up the computation)
- vpColVector cRc0_0n;
- //! Initial points and their ID
- std::map<int, vpImagePoint> initPoints;
- //! Current points and their ID
- std::map<int, vpImagePoint> curPoints;
- //! Current points ID and their indexes
- std::map<int, int> curPointsInd;
- //! number of points detected
- unsigned int nbPointsCur;
- //! initial number of points
- unsigned int nbPointsInit;
- //! Minimal number of points to be tracked
- unsigned int minNbPoint;
- //! Boolean to know if there is enough point to be tracked
- bool enoughPoints;
- //! current camera to plane distance to speed up the computation
- double dt;
- //! distance between the plane and the camera at the initial position
- double d0;
- //! Camera parameters
- vpCameraParameters cam;
-
-public:
- //! Pointer to the polygon that define a face
- vpMbtPolygon *polygon;
-
-private:
-
- double compute_1_over_Z(const double x, const double y);
- void computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& cHc0);
- bool isTrackedFeature(const int id);
-
-public:
- vpMbtDistanceKltPoints();
- virtual ~vpMbtDistanceKltPoints();
-
- unsigned int computeNbDetectedCurrent(const vpKltOpencv& _tracker);
- void computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& cHc0);
- void computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J);
-
- void displayPrimitive(const vpImage<unsigned char>& _I);
- void displayPrimitive(const vpImage<vpRGBa>& _I);
-
- /*!
- Get the camera parameters of the face.
-
- \return cam : the camera parameters of the face.
- */
- inline vpCameraParameters& getCameraParameters(){ return cam; }
-
- inline vpColVector getCurrentNormal() const {return N_cur; }
-
- inline std::map<int, vpImagePoint>& getCurrentPoints() {return curPoints; }
-
- inline std::map<int, int>& getCurrentPointsInd() {return curPointsInd; }
-
- /*!
- Get the number of point that was belonging to the face at the initialisation
-
- \return the number of initial point.
-
- \sa getCurrentNumberPoints()
- */
- inline unsigned int getInitialNumberPoint() const { return nbPointsInit;}
- /*!
- Get the number of points detected in the last image.
-
- \warning To have the real number of points, the function computeNbDetectedCurrent()
- must be called first.
-
- \return the number of points detected in the current image.
-
- \sa getInitialNumberPoint()
- */
- inline unsigned int getCurrentNumberPoints() const {return nbPointsCur;}
-
- inline bool hasEnoughPoints() const {return enoughPoints;}
-
- void init(const vpKltOpencv& _tracker);
-
- void removeOutliers(const vpColVector& weight, const double &threshold_outlier);
-
- /*!
- Set the camera parameters
-
- \param _cam : the new camera parameters
- */
- virtual inline void setCameraParameters(const vpCameraParameters& _cam){ cam = _cam; }
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
-#else
- void updateMask(IplImage* mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
-#endif
-
- //###################
- // Deprecated Functions
- //###################
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
- /*!
- \deprecated Use rather getCurrentNumberPoints() that does the same.
-
- Get the number of points detected in the last image.
-
- \warning to have the real number of points, the function computeNbDetectedCurrent()
- must be called first.
-
- \return the number of points detected in the current image
- */
- vp_deprecated inline unsigned int getNbPointsCur() const {return nbPointsCur;}
-#endif
-
- //###################
- // Static Functions
- //###################
-private:
- static bool isInside(const std::vector<vpImagePoint>& roi, const double i, const double j);
- static bool intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double i, const double j, const double i_test, const double j_test);
-};
-
-#endif
-
-#endif // VISP_HAVE_OPENCV
diff --git a/src/tracking/mbt/klt/vpMbtKltXmlParser.cpp b/src/tracking/mbt/klt/vpMbtKltXmlParser.cpp
deleted file mode 100644
index 239389a..0000000
--- a/src/tracking/mbt/klt/vpMbtKltXmlParser.cpp
+++ /dev/null
@@ -1,284 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtKltXmlParser.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using point features).
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <iostream>
-#include <map>
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbtKltXmlParser.h>
-
-
-/*!
- Default constructor.
-
-*/
-vpMbtKltXmlParser::vpMbtKltXmlParser()
- : maskBorder(0), maxFeatures(0), winSize(0), qualityValue(0.), minDist(0.),
- harrisParam(0.), blockSize(0), pyramidLevels(0)
-{
- init();
-}
-
-/*!
- Default destructor.
-*/
-vpMbtKltXmlParser::~vpMbtKltXmlParser()
-{
-}
-
-/*!
- Initialise internal variables (including the map).
-*/
-void
-vpMbtKltXmlParser::init()
-{
- vpMbXmlParser::init();
-
- nodeMap["klt"] = klt;
- nodeMap["mask_border"] = mask_border;
- nodeMap["max_features"] = max_features;
- nodeMap["window_size"] = window_size;
- nodeMap["quality"] = quality;
- nodeMap["min_distance"] = min_distance;
- nodeMap["harris"] = harris;
- nodeMap["size_block"] = size_block;
- nodeMap["pyramid_lvl"] = pyramid_lvl;
-}
-
-/*!
- Parse the file in parameters.
- This method is deprecated, use parse() instead.
-
- \param filename : File to parse.
-*/
-void
-vpMbtKltXmlParser::parse(const char * filename)
-{
- std::string file = filename;
- vpXmlParser::parse(file);
-}
-
-/*!
- Write info to file.
-
- \warning Useless, so not yet implemented => Throw exception.
-*/
-void
-vpMbtKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
-{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
-}
-
-/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
- \param doc : Document to parse.
- \param node : Root node.
-*/
-void
-vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
-{
- bool camera_node = false;
- bool face_node = false;
- bool klt_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
- camera_node = true;
- }break;
- case face:{
- this->read_face(doc, dataNode);
- face_node = true;
- }break;
- case klt:{
- this->read_klt(doc, dataNode);
- klt_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
- }
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
- }
-
- if(!klt_node) {
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
- }
-}
-
-/*!
- Read klt information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the camera information.
-*/
-void
-vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
-{
- bool mask_border_node = false;
- bool max_features_node = false;
- bool window_size_node = false;
- bool quality_node = false;
- bool min_distance_node = false;
- bool harris_node = false;
- bool size_block_node = false;
- bool pyramid_lvl_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case mask_border:{
- maskBorder = xmlReadUnsignedIntChild(doc, dataNode);
- mask_border_node = true;
- }break;
- case max_features:{
- maxFeatures = xmlReadUnsignedIntChild(doc, dataNode);
- max_features_node = true;
- }break;
- case window_size:{
- winSize = xmlReadUnsignedIntChild(doc, dataNode);
- window_size_node = true;
- }break;
- case quality:{
- qualityValue = xmlReadDoubleChild(doc, dataNode);
- quality_node = true;
- }break;
- case min_distance:{
- minDist = xmlReadDoubleChild(doc, dataNode);
- min_distance_node = true;
- }break;
- case harris:{
- harrisParam = xmlReadDoubleChild(doc, dataNode);
- harris_node = true;
- }break;
- case size_block:{
- blockSize = xmlReadUnsignedIntChild(doc, dataNode);
- size_block_node = true;
- }break;
- case pyramid_lvl:{
- pyramidLevels = xmlReadUnsignedIntChild(doc, dataNode);
- pyramid_lvl_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!mask_border_node)
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
- else
- std::cout << "klt : Mask Border : "<< maskBorder <<std::endl;
-
- if(!max_features_node)
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
- else
- std::cout << "klt : Max Features : "<< maxFeatures <<std::endl;
-
- if(!window_size_node)
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
- else
- std::cout << "klt : Windows Size : "<< winSize <<std::endl;
-
- if(!quality_node)
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
- else
- std::cout << "klt : Quality : "<< qualityValue <<std::endl;
-
- if(!min_distance_node)
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
- else
- std::cout << "klt : Min Distance : "<< minDist <<std::endl;
-
- if(!harris_node)
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
- else
- std::cout << "klt : Harris Parameter : "<< harrisParam <<std::endl;
-
- if(!size_block_node)
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
- else
- std::cout << "klt : Block Size : "<< blockSize <<std::endl;
-
- if(!pyramid_lvl_node)
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
- else
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<std::endl;
-}
-
-#endif
-
diff --git a/src/tracking/mbt/klt/vpMbtKltXmlParser.h b/src/tracking/mbt/klt/vpMbtKltXmlParser.h
deleted file mode 100644
index 0de55c7..0000000
--- a/src/tracking/mbt/klt/vpMbtKltXmlParser.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtKltXmlParser.h 4582 2014-01-14 14:02:46Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML parameters of the Model based tracker (using point features).
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- * \file vpMbtKltXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
-*/
-#ifndef vpMbtKltXmlParser_HH
-#define vpMbtKltXmlParser_HH
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbXmlParser.h>
-
-/*!
- \class vpMbtKltXmlParser
- \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
- \ingroup ModelBasedTracking
-
- Data parser for the KLT model based tracker.
-
-*/
-class VISP_EXPORT vpMbtKltXmlParser: virtual public vpMbXmlParser
-{
-protected:
- //! Border of the mask used on Klt points
- unsigned int maskBorder;
- //! Maximum of Klt features
- unsigned int maxFeatures;
- //! Windows size
- unsigned int winSize;
- //! Quality of the Klt points
- double qualityValue;
- //! Minimum distance between klt points
- double minDist;
- //! Harris free parameters
- double harrisParam;
- //! Block size
- unsigned int blockSize;
- //! Number of pyramid levels
- unsigned int pyramidLevels;
-
- typedef enum{
- klt = vpMbXmlParser::last,
- mask_border,
- max_features,
- window_size,
- quality,
- min_distance,
- harris,
- size_block,
- pyramid_lvl,
- last
- } dataToParseMbKlt;
-
-
-public:
-
- vpMbtKltXmlParser();
- virtual ~vpMbtKltXmlParser();
-
- /*!
- Get the size of a block.
-
- \return blockSize
- */
- inline unsigned int getBlockSize() const {return blockSize;}
-
- /*!
- Get the Harris free parameter.
-
- \return harrisParam
- */
- inline double getHarrisParam() const {return harrisParam;}
-
- /*!
- Get the Border of the mask.
-
- \return faceBorder
- */
- inline unsigned int getMaskBorder() const {return maskBorder;}
-
- /*!
- Get the maximum number of features for the KLT.
-
- \return maxFeatures
- */
- inline unsigned int getMaxFeatures() const {return maxFeatures;}
-
- /*!
- Get the minimum distance between KLT points.
-
- \return minDist
- */
- inline double getMinDistance() const {return minDist;}
-
- /*!
- Get the number of pyramid levels
-
- \return pyramidLevels
- */
- inline unsigned int getPyramidLevels() const {return pyramidLevels;}
-
- /*!
- Get the quality of the KLT.
-
- \return quality
- */
- inline double getQuality() const {return qualityValue;}
-
- /*!
- Get the size of the window used in the KLT tracker.
-
- \return winSize
- */
- inline unsigned int getWindowSize() const {return winSize;}
-
- void parse(const char * filename);
-
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_klt(xmlDocPtr doc, xmlNodePtr node);
-
- /*!
- Set the size of a block.
-
- \param bs : New blockSize
- */
- inline void setBlockSize(const unsigned int &bs) {blockSize = bs;}
-
- /*!
- Set the Harris free parameter.
-
- \param hp : New harrisParam
- */
- inline void setHarrisParam(const double &hp) {harrisParam = hp;}
-
- /*!
- Set the Border of the mask.
-
- \param mb = new maskBorder
- */
- inline void setMaskBorder(const unsigned int &mb) {maskBorder = mb;}
-
- /*!
- Set the maximum number of features for the KLT.
-
- \param mF : New maxFeatures
- */
- inline void setMaxFeatures(const unsigned int &mF) {maxFeatures = mF;}
-
- /*!
- Set the minimum distance between KLT points.
-
- \param mD : New minDist
- */
- inline void setMinDistance(const double &mD) {minDist = mD;}
-
- /*!
- Set the number of pyramid levels
-
- \param pL : New pyramidLevels
- */
- inline void setPyramidLevels(const unsigned int &pL) {pyramidLevels = pL;}
-
- /*!
- Set the quality of the KLT.
-
- \param q : New quality
- */
- inline void setQuality(const double &q) {qualityValue = q;}
-
- /*!
- Set the size of the window used in the KLT tracker.
-
- \param w : New winSize
- */
- inline void setWindowSize(const unsigned int &w) {winSize = w;}
-
- void writeMainClass(xmlNodePtr node);
-
-protected:
- void init();
-
-};
-
-#endif
-
-#endif
-
-
-
diff --git a/src/tracking/mbt/vpMbHiddenFaces.h b/src/tracking/mbt/vpMbHiddenFaces.h
deleted file mode 100644
index 9bd1356..0000000
--- a/src/tracking/mbt/vpMbHiddenFaces.h
+++ /dev/null
@@ -1,746 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbTracker.h 4004 2012-11-23 17:34:44Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic model based tracker. This class declares the methods to implement in
- * order to have a model based tracker.
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-#pragma once
-
-#ifndef vpMbHiddenFaces_HH
-#define vpMbHiddenFaces_HH
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMbtPolygon.h>
-
-#ifdef VISP_HAVE_OGRE
- #include <visp/vpAROgre.h>
-#endif
-
-#include <vector>
-#include <limits>
-
-/*!
- \class vpMbHiddenFaces
-
- \brief Implementation of the polygons management for the model-based trackers.
-
- \ingroup ModelBasedTracking
-
- */
-template<class PolygonType = vpMbtPolygon>
-class vpMbHiddenFaces
-{
- private:
- //! List of polygons
- std::vector<PolygonType *> Lpol ;
- //! Number of visible polygon
- unsigned int nbVisiblePolygon;
-
-#ifdef VISP_HAVE_OGRE
- vpImage<unsigned char> ogreBackground;
- bool ogreInitialised;
- vpAROgre *ogre;
- std::vector< Ogre::ManualObject* > lOgrePolygons;
-#endif
-
- unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
- bool &changed,
- bool useOgre = false, bool testRoi = false,
- const vpImage<unsigned char> &I = vpImage<unsigned char>(),
- const vpCameraParameters &cam = vpCameraParameters()) ;
-
- public :
- vpMbHiddenFaces() ;
- ~vpMbHiddenFaces() ;
-
- void addPolygon(PolygonType *p) ;
-
- bool computeVisibility(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool testRoi,
- const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpTranslationVector &cameraPos,
- unsigned int index);
-#ifdef VISP_HAVE_OGRE
- void displayOgre(const vpHomogeneousMatrix &cMo);
-#endif
-
- /*!
- Get the list of polygons.
-
- \return Mbt Klt polygons list.
- */
- std::vector<PolygonType*>& getPolygon() {return Lpol;}
-
-#ifdef VISP_HAVE_OGRE
- void initOgre(const vpCameraParameters &cam = vpCameraParameters());
-#endif
-
- /*!
- get the number of visible polygons.
-
- \return number of visible polygons.
- */
- unsigned int getNbVisiblePolygon() const {return nbVisiblePolygon;}
-
-#ifdef VISP_HAVE_OGRE
- /*!
- Get the Ogre3D Context.
-
- \return A pointer on a vpAROgre instance.
- */
- vpAROgre* getOgreContext(){return ogre;}
-#endif
-
- bool isAppearing(const unsigned int i){ return Lpol[i]->isAppearing(); }
-
-
-#ifdef VISP_HAVE_OGRE
- /*!
- Tell whether if Ogre Context is initialised or not.
-
- \return True if it does, false otherwise.
- */
- bool isOgreInitialised() { return ogreInitialised; }
-#endif
-
- /*!
- Check if the polygon at position i in the list is visible.
-
- \param i : TPosition in the list.
-
- \return Return true if the polygon is visible.
- */
- bool isVisible(const unsigned int i){ return Lpol[i]->isVisible(); }
-
-#ifdef VISP_HAVE_OGRE
- bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
-#endif
-
- //! operator[] as modifier.
- inline PolygonType* operator[](const unsigned int i) { return Lpol[i];}
- //! operator[] as reader.
- inline const PolygonType* operator[](const unsigned int i) const { return Lpol[i];}
-
- void reset();
-
-#ifdef VISP_HAVE_OGRE
- /*!
- Set the background size (by default it is 640x480).
- The background size has to match with the size of the image that you are using for the traking.
-
- \warning This function has to be called before initOgre().
-
- \param h : Height of the background
- \param w : Width of the background
- */
- void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w) { ogreBackground = vpImage<unsigned char>(h, w, 0); }
-#endif
-
- unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed) ;
- unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
- unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
-
-#ifdef VISP_HAVE_OGRE
- unsigned int setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
- unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
-#endif
- /*!
- Get the number of polygons.
-
- \return Size of the list.
- */
- inline unsigned int size() const { return (unsigned int)Lpol.size(); }
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- //! Boolean specifying if a polygon has to be entirely in front of the camera or not.
- bool depthTest;
-
- /*!
- @name Deprecated functions
- */
- /*!
- \deprecated This method is deprecated since it is no more used since ViSP 2.7.2. \n
-
- Get the depthTest value.
-
- \return true if all the points of a polygon has to be in front of the camera, false otherwise.
- */
- vp_deprecated bool getDepthTest(){return depthTest;}
- /*!
- \deprecated This method is deprecated since it is no more used since ViSP 2.7.2. \n
-
- Set the depthTest value.
-
- \param d : New value.
- */
- vp_deprecated void setDepthTest(const bool &d){depthTest = d;}
- unsigned int setVisible(const vpHomogeneousMatrix &cMo) ;
-#endif
-} ;
-
-/*!
- Basic constructor.
-*/
-template<class PolygonType>
-vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces()
- : Lpol(), nbVisiblePolygon(0)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , depthTest(false)
-#endif
-
-{
-#ifdef VISP_HAVE_OGRE
- ogreInitialised = false;
- ogre = new vpAROgre();
- ogre->setShowConfigDialog(false);
- ogreBackground = vpImage<unsigned char>(480, 640, 0);
-#endif
-}
-
-/*!
- Basic destructor.
-*/
-template<class PolygonType>
-vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
-{
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if (Lpol[i]!=NULL){
- delete Lpol[i] ;
- }
- Lpol[i] = NULL ;
- }
- Lpol.resize(0);
-
-#ifdef VISP_HAVE_OGRE
- if(ogre != NULL){
- delete ogre;
- ogre = NULL;
- }
-
- // This is already done by calling "delete ogre"
-// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
-// if (lOgrePolygons[i]!=NULL){
-// delete lOgrePolygons[i] ;
-// }
-// lOgrePolygons[i] = NULL ;
-// }
-
- lOgrePolygons.resize(0);
-#endif
-}
-
-/*!
- Add a polygon to the list of polygons.
-
- \param p : The polygon to add.
-*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
-{
- PolygonType *p_new = new PolygonType;
- p_new->index = p->index;
- p_new->setNbPoint(p->nbpt);
- p_new->isvisible = p->isvisible;
- p_new->useLod = p->useLod;
- p_new->minLineLengthThresh = p->minLineLengthThresh;
- p_new->minPolygonAreaThresh = p->minPolygonAreaThresh;
- p_new->setName(p->name);
-
- for(unsigned int i = 0; i < p->nbpt; i++)
- p_new->p[i]= p->p[i];
- Lpol.push_back(p_new);
-}
-
-/*!
- Reset the Hidden faces (remove the list of PolygonType)
-*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::reset()
-{
- nbVisiblePolygon = 0;
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if (Lpol[i]!=NULL){
- delete Lpol[i] ;
- }
- Lpol[i] = NULL ;
- }
- Lpol.resize(0);
-
-#ifdef VISP_HAVE_OGRE
- if(ogre != NULL){
- delete ogre;
- ogre = NULL;
- }
-
- // This is already done by calling "delete ogre"
-// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
-// if (lOgrePolygons[i]!=NULL){
-// delete lOgrePolygons[i] ;
-// }
-// lOgrePolygons[i] = NULL ;
-// }
-
- lOgrePolygons.resize(0);
-
- ogreInitialised = false;
- ogre = new vpAROgre();
- ogre->setShowConfigDialog(false);
- ogreBackground = vpImage<unsigned char>(480, 640);
-#endif
-}
-
-/*!
- Compute the number of visible polygons.
-
- \param cMo : The pose of the camera
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param useOgre : True if a Ogre is used to test the visibility, False otherwise
- \param testRoi : True if a face have to be entirely in the image False otherwise
- \param I : Image used to test if a face is entirely projected in the image.
- \param cam : Camera parameters.
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool testRoi,
- const vpImage<unsigned char> &I,
- const vpCameraParameters &cam)
-{
- nbVisiblePolygon = 0;
- changed = false;
-
- vpTranslationVector cameraPos;
-
- if(useOgre){
-#ifdef VISP_HAVE_OGRE
- cMo.inverse().extract(cameraPos);
- ogre->renderOneFrame(ogreBackground, cMo);
-#else
- vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
-#endif
- }
-
- for (unsigned int i = 0; i < Lpol.size(); i++){
- if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, testRoi, I, cam, cameraPos, i))
- nbVisiblePolygon ++;
- }
- return nbVisiblePolygon;
-}
-
-/*!
- Compute the visibility of a given face index.
-
- \param cMo : The pose of the camera
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param useOgre : True if a Ogre is used to test the visibility, False otherwise
- \param testRoi : True if a face have to be entirely in the image False otherwise
- \param I : Image used to test if a face is entirely projected in the image.
- \param cam : Camera parameters.
- \param cameraPos : Position of the camera. Used only when Ogre is used as 3rd party.
- \param index : Index of the face to consider.
-
- \return Return true if the face is visible.
-*/
-template<class PolygonType>
-bool
-vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool /* testRoi */,
- const vpImage<unsigned char> & I,
- const vpCameraParameters & cam,
- const vpTranslationVector &
- #ifdef VISP_HAVE_OGRE
- cameraPos
- #endif
- ,
- unsigned int index)
-{
- unsigned int i = index;
- Lpol[i]->changeFrame(cMo);
- Lpol[i]->isappearing = false;
-
- //Commented because we need to compute visibility
- // even when dealing with line in level of detail case
- /*if(Lpol[i]->getNbPoint() <= 2)
- {
- Lpol[i]->isvisible = true;
- }
- else*/{
- if(Lpol[i]->isVisible())
- {
- bool testDisappear = false;
- unsigned int nbCornerInsidePrev = 0;
-
-// if(testRoi){
-// nbCornerInsidePrev = Lpol[i]->getNbCornerInsidePrevImage();
-// if(Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
-// testDisappear = true;
-// }
-
- if(!testDisappear){
- if(useOgre)
-#ifdef VISP_HAVE_OGRE
- testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, I)) || !isVisibleOgre(cameraPos,i));
-#else
- testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
-#endif
- else
- testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
- }
-
- // test if the face is still visible
- if(testDisappear){
-// std::cout << "Face " << i << " disappears" << std::endl;
- changed = true;
- Lpol[i]->isvisible = false;
- }
- else {
- //nbVisiblePolygon++;
- Lpol[i]->isvisible = true;
-
- if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
- changed = true;
- }
- }
- else
- {
- bool testAppear = true;
-
-// if(testRoi && Lpol[i]->getNbCornerInsideImage(I, cam) == 0)
-// testAppear = false;
-
- if(testAppear){
- if(useOgre)
-#ifdef VISP_HAVE_OGRE
- testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, I)) && isVisibleOgre(cameraPos,i));
-#else
- testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
-#endif
- else
- testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
- }
-
- if(testAppear){
-// std::cout << "Face " << i << " appears" << std::endl;
- Lpol[i]->isvisible = true;
- changed = true;
- //nbVisiblePolygon++;
- }
- else{
-// std::cout << "Problem" << std::endl;
- Lpol[i]->isvisible = false;
- }
- }
- }
- // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon << std::endl;
- return Lpol[i]->isvisible;
-}
-
-/*!
- Compute the number of visible polygons.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera.
- \param angle : Angle used to test the appearance and disappearance of a face.
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
-{
- return setVisible(I, cam, cMo, angle, angle, changed);
-}
-
-/*!
- Compute the number of visible polygons.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
-{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false,true,I,cam);
-}
-
-/*!
- Compute the number of visible polygons.
-
- \param cMo : The pose of the camera
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
-{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false);
-}
-
-#ifdef VISP_HAVE_OGRE
-/*!
- Initialise the ogre context for face visibility tests.
-
- \param cam : Camera parameters.
-*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
-{
- ogreInitialised = true;
- ogre->setCameraParameters(cam);
- ogre->init(ogreBackground, false, true);
-
- for(unsigned int n = 0 ; n < Lpol.size(); n++){
- Ogre::ManualObject* manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
-
- manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
- for(unsigned int i = 0; i < Lpol[n]->nbpt; i++){
- manual->position( (Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(), (Ogre::Real)Lpol[n]->p[i].get_oZ());
- manual->colour(1.0, 1.0, 1.0);
- manual->index(i);
- }
-
- manual->index(0);
- manual->end();
-
- ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
-
- lOgrePolygons.push_back(manual);
- }
-}
-
-/*!
- Update the display in Ogre Window.
-
- \param cMo : Pose used to display.
-*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
-{
- if(ogreInitialised && !ogre->isWindowHidden()){
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if(Lpol[i]->isVisible()){
- lOgrePolygons[i]->setVisible(true);
- }
- else
- lOgrePolygons[i]->setVisible(false);
- }
- ogre->display(ogreBackground, cMo);
- }
-}
-
-/*!
- Compute the number of visible polygons through Ogre3D.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
-{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true,true,I,cam);
-}
-
-/*!
- Compute the number of visible polygons through Ogre3D.
-
- \param cMo : The pose of the camera
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
-{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true);
-}
-
-/*!
- Test the visibility of a polygon through Ogre3D via RayCasting.
-
- \param cameraPos : Position of the camera in the 3D world.
- \param index : Index of the polygon.
-
- \return Return true if the polygon is visible, False otherwise.
-*/
-template<class PolygonType>
-bool
-vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
-{
-// std::cout << "visible" << std::endl;
- // A line is always visible
- if(Lpol[index]->getNbPoint() <= 2){
- lOgrePolygons[index]->setVisible(true);
- Lpol[index]->isvisible = true;
- return true;
- }
-
- Ogre::Vector3 camera((Ogre::Real)cameraPos[0],(Ogre::Real)cameraPos[1],(Ogre::Real)cameraPos[2]);
- if(!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())){
- lOgrePolygons[index]->setVisible(false);
- Lpol[index]->isvisible = false;
- return false;
- }
-
- //Get the center of gravity
- Ogre::Vector3 origin(0,0,0);
- for(unsigned int j = 0 ; j < Lpol[index]->getNbPoint() ; j++){
- Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(), (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
- origin += tmp;
- }
- origin /= (Ogre::Real)Lpol[index]->getNbPoint();
- Ogre::Vector3 direction = origin - camera;
-
- Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction));
- mRaySceneQuery->setSortByDistance(true);
-
- Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
- Ogre::RaySceneQueryResult::iterator it = result.begin();
-
- bool visible = false;
- double distance, distancePrev;
- if(it != result.end()){
- if(it->movable->getName().find("SimpleRenderable") != Ogre::String::npos) //Test if the ogreBackground is intersect in first
- it++;
-
- if(it != result.end()){
- distance = it->distance;
- distancePrev = distance;
- if(it->movable->getName() == Ogre::StringConverter::toString(index)){
- visible = true;
- }
- else{
- it++;
- while(!visible && it != result.end()){
- distance = it->distance;
- //if(distance == distancePrev){
- if(std::fabs(distance - distancePrev) < distance * std::numeric_limits<double>::epsilon()){
- if(it->movable->getName() == Ogre::StringConverter::toString(index)){
- visible = true;
- break;
- }
- it++;
- distancePrev = distance;
- }
- else
- break;
- }
- }
- }
- }
-
- if(visible){
- lOgrePolygons[index]->setVisible(true);
- Lpol[index]->isvisible = true;
- }
- else{
- lOgrePolygons[index]->setVisible(false);
- Lpol[index]->isvisible = false;
- }
-
- ogre->getSceneManager()->destroyQuery(mRaySceneQuery);
-
- return Lpol[index]->isvisible;
-}
-#endif //VISP_HAVE_OGRE
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated since it is no more used since ViSP 2.7.2. \n
-
- Compute the number of visible polygons.
-
- \param cMo : The pose of the camera
-
- \return Return the number of visible polygons
-*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo)
-{
- nbVisiblePolygon = 0 ;
-
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if (Lpol[i]->isVisible(cMo, depthTest)){
- nbVisiblePolygon++;
- }
- }
- return nbVisiblePolygon ;
-}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
-#endif // vpMbHiddenFaces
-
diff --git a/src/tracking/mbt/vpMbTracker.cpp b/src/tracking/mbt/vpMbTracker.cpp
deleted file mode 100755
index 6b0760c..0000000
--- a/src/tracking/mbt/vpMbTracker.cpp
+++ /dev/null
@@ -1,2445 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbTracker.cpp 5285 2015-02-09 14:32:54Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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.
- *
- * Contact visp at irisa.fr if any conditions of this licensing are
- * not clear to you.
- *
- * Description:
- * Generic model based tracker
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMbTracker.cpp
- \brief Generic model based tracker
-*/
-
-#include <iostream>
-#include <limits>
-#include <algorithm>
-#include <map>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-#include <visp/vpPoint.h>
-#include <visp/vpPose.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpColor.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpException.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpMbTracker.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpIoTools.h>
-
-#ifdef VISP_HAVE_COIN
-//Inventor includes
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
-#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
-#include <Inventor/actions/SoGetMatrixAction.h>
-#include <Inventor/actions/SoGetPrimitiveCountAction.h>
-#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLTransform.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
-#endif
-
-
-
-
-/*!
- Structure to store info about segment in CAO model files.
- */
-struct SegmentInfo {
- SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
-
- std::vector<vpPoint> extremities;
- std::string name;
- bool useLod;
- double minLineLengthThresh;
-};
-
-/*!
- Structure to store info about a polygon face represented by a vpPolygon and by a list of vpPoint
- representing the corners of the polygon face in 3D.
- */
-struct PolygonFaceInfo {
- PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
-: distanceToCamera(dist), polygon(poly), faceCorners(corners) {}
-
- bool operator<(const PolygonFaceInfo &pfi) const {
- return distanceToCamera < pfi.distanceToCamera;
- }
-
- double distanceToCamera;
- vpPolygon polygon;
- std::vector<vpPoint> faceCorners;
-};
-
-/*!
- Basic constructor.
- Set default values.
-
-*/
-vpMbTracker::vpMbTracker()
-: cam(), cMo(), oJo(6,6), isoJoIdentity(true), modelFileName(), modelInitialised(false),
- poseSavingFilename(), computeCovariance(false), covarianceMatrix(), displayFeatures(false),
- m_w(), m_error(), faces(), angleAppears( vpMath::rad(89) ), angleDisappears( vpMath::rad(89) ),
- distNearClip(0.001), distFarClip(100), clippingFlag(vpMbtPolygon::NO_CLIPPING), useOgre(false),
- nbPoints(0), nbLines(0), nbPolygonLines(0), nbPolygonPoints(0), nbCylinders(0), nbCircles(0),
- useLodGeneral(false), applyLodSettingInConfig(false), minLineLengthThresholdGeneral(50.0),
- minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames()
-{
- oJo.setIdentity();
- //Map used to parse additional information in CAO model files,
- //like name of faces or LOD setting
- mapOfParameterNames["name"] = "string";
- mapOfParameterNames["minPolygonAreaThreshold"] = "number";
- mapOfParameterNames["minLineLengthThreshold"] = "number";
- mapOfParameterNames["useLod"] = "boolean";
-}
-
-/*!
- Basic destructor that doest nothing.
-*/
-vpMbTracker::~vpMbTracker()
-{
-}
-
-/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
- \code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
- \endcode
-
- \param I : Input image
- \param initFile : File containing the points where to click
- \param displayHelp : Optionnal display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
-
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
-
-*/
-void
-vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp)
-{
- vpHomogeneousMatrix last_cMo;
- vpPoseVector init_pos;
- vpImagePoint ip;
- vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
-
- std::string ext = ".init";
- std::string str_pose = "";
- size_t pos = (unsigned int)initFile.rfind(ext);
-
- // Load the last poses from files
- std::fstream finitpos ;
- std::fstream finit ;
- char s[FILENAME_MAX];
- if(poseSavingFilename.empty()){
- if( pos == initFile.size()-ext.size() && pos != 0)
- str_pose = initFile.substr(0,pos) + ".0.pos";
- else
- str_pose = initFile + ".0.pos";
-
- finitpos.open(str_pose.c_str() ,std::ios::in) ;
- sprintf(s, "%s", str_pose.c_str());
- }else{
- finitpos.open(poseSavingFilename.c_str() ,std::ios::in) ;
- sprintf(s, "%s", poseSavingFilename.c_str());
- }
- if(finitpos.fail() ){
- std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
- last_cMo.setIdentity();
- }
- else{
- for (unsigned int i = 0; i < 6; i += 1){
- finitpos >> init_pos[i];
- }
-
- finitpos.close();
- last_cMo.buildFrom(init_pos) ;
-
- std::cout <<"last_cMo : "<<std::endl << last_cMo <<std::endl;
-
- vpDisplay::display(I);
- display(I, last_cMo, cam, vpColor::green, 1, true);
- vpDisplay::displayFrame(I, last_cMo, cam, 0.05, vpColor::green);
- vpDisplay::flush(I);
-
- std::cout << "No modification : left click " << std::endl;
- std::cout << "Modify initial pose : right click " << std::endl ;
-
- vpDisplay::displayText(I, 15, 10,
- "left click to validate, right click to modify initial pose",
- vpColor::red);
-
- vpDisplay::flush(I) ;
-
- while (!vpDisplay::getClick(I, ip, button)) ;
- }
-
-
- if (!finitpos.fail() && button == vpMouseButton::button1){
- cMo = last_cMo ;
- }
- else
- {
- vpDisplay *d_help = NULL;
-
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- vpPose pose ;
-
- pose.clearPoint() ;
-
- // file parser
- // number of points
- // X Y Z
- // X Y Z
-
- double X,Y,Z ;
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.init", initFile.c_str());
-
- std::cout << "Load 3D points from: " << s << std::endl ;
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
- std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
- }
-
- //Display window creation and initialisation
- try{
- if(displayHelp){
- std::string dispF;
- if( pos == initFile.size()-ext.size() && pos != 0)
- dispF = initFile.substr(0,pos) + ".ppm";
- else
- dispF = initFile + ".ppm";
-
- if (vpIoTools::checkFilename(dispF)) {
- std::cout << "Load image to help initialization: " << dispF << std::endl;
-#if defined VISP_HAVE_X11
- d_help = new vpDisplayX ;
-#elif defined VISP_HAVE_GDI
- d_help = new vpDisplayGDI;
-#elif defined VISP_HAVE_OPENCV
- d_help = new vpDisplayOpenCV;
-#endif
-
- vpImage<vpRGBa> Iref ;
- vpImageIo::read(Iref, dispF) ;
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
- d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
- "Where to initialize...") ;
- vpDisplay::display(Iref) ;
- vpDisplay::flush(Iref);
-#endif
- }
- }
- }
- catch(...) {
- if(d_help != NULL) {
- delete d_help;
- d_help = NULL;
- }
- }
-
- char c;
- // skip lines starting with # as comment
- finit.get(c);
- while (!finit.fail() && (c == '#')) {
- finit.ignore(256, '\n');
- finit.get(c);
- }
- finit.unget();
-
- unsigned int n ;
- finit >> n ;
- finit.ignore(256, '\n'); // skip the rest of the line
- std::cout << "Number of 3D points " << n << std::endl ;
- if (n > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
- }
-
- vpPoint *P = new vpPoint [n] ;
- for (unsigned int i=0 ; i < n ; i++){
- // skip lines starting with # as comment
- finit.get(c);
- while (!finit.fail() && (c == '#')) {
- finit.ignore(256, '\n');
- finit.get(c);
- }
- finit.unget();
-
- finit >> X ;
- finit >> Y ;
- finit >> Z ;
- finit.ignore(256, '\n'); // skip the rest of the line
-
- std::cout << "Point " << i+1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
- P[i].setWorldCoordinates(X,Y,Z) ; // (X,Y,Z)
- }
-
- finit.close();
-
-////////////////////////////////
- bool isWellInit = false;
- while(!isWellInit)
- {
-////////////////////////////////
- std::vector<vpImagePoint> mem_ip;
- for(unsigned int i=0 ; i< n ; i++)
- {
- std::ostringstream text;
- text << "Click on point " << i+1;
- vpDisplay::display(I);
- vpDisplay::displayText(I, 15, 10, text.str(), vpColor::red);
- for (unsigned int k=0; k<mem_ip.size(); k++) {
- vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2) ;
- }
- vpDisplay::flush(I) ;
-
- std::cout << "Click on point " << i+1 << " ";
- double x=0,y=0;
- vpDisplay::getClick(I, ip) ;
- mem_ip.push_back(ip);
- vpDisplay::flush(I) ;
- vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- P[i].set_x(x);
- P[i].set_y(y);
-
- std::cout << "with 2D coordinates: " << ip << std::endl;
-
- pose.addPoint(P[i]) ; // and added to the pose computation point list
- }
- vpDisplay::flush(I) ;
- vpDisplay::display(I) ;
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2){
- cMo = cMo1;
- }
- else{
- cMo = cMo2;
- }
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-
- display(I, cMo, cam, vpColor::green, 1, true);
- vpDisplay::displayText(I, 15, 10,
- "left click to validate, right click to re initialize object",
- vpColor::red);
-
- vpDisplay::flush(I) ;
-
- button = vpMouseButton::button1;
- while (!vpDisplay::getClick(I, ip, button)) ;
-
-
- if (button == vpMouseButton::button1)
- {
- isWellInit = true;
- }
- else
- {
- pose.clearPoint() ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- }
-////////////////////////////////////
- vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
-
- delete [] P;
-
- //save the pose into file
- if(poseSavingFilename.empty())
- savePose(str_pose);
- else
- savePose(poseSavingFilename);
-
- if(d_help != NULL) {
- delete d_help;
- d_help = NULL;
- }
- }
-
- std::cout <<"cMo : "<<std::endl << cMo <<std::endl;
-
- init(I);
-}
-
-/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the list points3D_list.
-
- \param I : Input image
- \param points3D_list : List of the 3D points (object frame).
- \param displayFile : Path to the image used to display the help.
-*/
-void vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile)
-{
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- vpDisplay *d_help = NULL;
-
- vpPose pose ;
- vpPoint *P = NULL; P = new vpPoint [points3D_list.size()] ;
- for (unsigned int i=0 ; i < points3D_list.size() ; i++)
- P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
-
- vpImage<vpRGBa> Iref ;
- //Display window creation and initialisation
- if(vpIoTools::checkFilename(displayFile)){
- try{
- std::cout << "Load image to help initialization: " << displayFile << std::endl;
-#if defined VISP_HAVE_X11
- d_help = new vpDisplayX ;
-#elif defined VISP_HAVE_GDI
- d_help = new vpDisplayGDI;
-#elif defined VISP_HAVE_OPENCV
- d_help = new vpDisplayOpenCV;
-#endif
-
- vpImageIo::read(Iref, displayFile) ;
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
- d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
- "Where to initialize...") ;
- vpDisplay::display(Iref) ;
- vpDisplay::flush(Iref);
-#endif
- }
- catch(...) {
- if(d_help != NULL) {
- delete d_help;
- d_help = NULL;
- }
- }
- }
-
- vpImagePoint ip;
- bool isWellInit = false;
- while(!isWellInit)
- {
- for(unsigned int i=0 ; i< points3D_list.size() ; i++)
- {
- std::cout << "Click on point " << i+1 << std::endl ;
- double x=0,y=0;
- vpDisplay::getClick(I, ip) ;
- vpDisplay::displayCross(I, ip, 5,vpColor::green) ;
- vpDisplay::flush(I) ;
- vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- P[i].set_x(x);
- P[i].set_y(y);
-
- std::cout << "Click on point " << ip << std::endl;
-
- vpDisplay::displayPoint (I, ip, vpColor::green); //display target point
- pose.addPoint(P[i]) ; // and added to the pose computation point list
- }
- vpDisplay::flush(I) ;
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2){
- cMo = cMo1;
- }
- else{
- cMo = cMo2;
- }
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-
- display(I, cMo, cam, vpColor::green, 1, true);
- vpDisplay::displayText(I, 15, 10,
- "left click to validate, right click to re initialize object",
- vpColor::red);
-
- vpDisplay::flush(I) ;
-
- vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
- while (!vpDisplay::getClick(I, ip, button)) ;
-
-
- if (button == vpMouseButton::button1)
- {
- isWellInit = true;
- }
- else
- {
- pose.clearPoint() ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- }
-
- vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
-
- delete [] P;
- if(d_help != NULL) {
- delete d_help;
- d_help = NULL;
- }
-
- init(I);
-}
-
-/*!
- Initialise the tracking by reading the 3D points (object frame) and the image points
- in initFile. The structure of this file is (without the comments):
- \code
- 4 // Number of 3D points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in meters in the object frame
- 0.01 -0.01 -0.01 // /
- 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 // /
- \endcode
-
- \param I : Input image
- \param initFile : Path to the file containing all the points.
-*/
-void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile )
-{
- char s[FILENAME_MAX];
- std::fstream finit ;
-
- std::string ext = ".init";
- size_t pos = initFile.rfind(ext);
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.init", initFile.c_str());
-
- std::cout << "filename " << s << std::endl ;
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
- std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
- }
-
- unsigned int size;
- double X, Y, Z;
- finit >> size ;
- std::cout << "number of points " << size << std::endl ;
-
- if (size > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
- }
-
- vpPoint *P = new vpPoint [size];
- vpPose pose ;
-
- for(unsigned int i=0 ; i< size ; i++)
- {
- finit >> X ;
- finit >> Y ;
- finit >> Z ;
- P[i].setWorldCoordinates(X,Y,Z) ;
- }
-
- unsigned int size2;
- double x, y;
- vpImagePoint ip;
- finit >> size2 ;
- if(size != size2)
- vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
-
- for(unsigned int i=0 ; i< size ; i++)
- {
- finit >> x;
- finit >> y;
- ip = vpImagePoint(x,y);
- vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- P[i].set_x(x);
- P[i].set_y(y);
- pose.addPoint(P[i]);
- }
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2)
- cMo = cMo1;
- else
- cMo = cMo2;
-
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-
- delete [] P;
-
- init(I);
-}
-
-/*!
- Initialise the tracking with the list of image points (points2D_list) and
- the list of corresponding 3D points (object frame) (points3D_list).
-
- \param I : Input image
- \param points2D_list : List of image points.
- \param points3D_list : List of 3D points (object frame).
-*/
-void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list,
- const std::vector<vpPoint> &points3D_list )
-{
- if(points2D_list.size() != points3D_list.size())
- vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
-
- size_t size = points3D_list.size();
- vpPoint *P = new vpPoint [size];
- vpPose pose ;
-
- for(size_t i=0 ; i< size ; i++)
- {
- P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
- double x=0,y=0;
- vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
- P[i].set_x(x);
- P[i].set_y(y);
- pose.addPoint(P[i]);
- }
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2)
- cMo = cMo1;
- else
- cMo = cMo2;
-
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-
- delete [] P;
-
- init(I);
-}
-
-/*!
- Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
- The structure of this file is (without the comments):
- \code
- // The six value of the pose vector
- 0.0000 // \
- 0.0000 // |
- 1.0000 // | Exemple of value for the pose vector where Z = 1 meter
- 0.0000 // |
- 0.0000 // |
- 0.0000 // /
- \endcode
-
- Where the three firsts lines refer to the translation and the three last to the rotation in thetaU parametrisation (see vpThetaUVector).
- \param I : Input image
- \param initFile : Path to the file containing the pose.
-*/
-void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile)
-{
- char s[FILENAME_MAX];
- std::fstream finit ;
- vpPoseVector init_pos;
-
- std::string ext = ".pos";
- size_t pos = initFile.rfind(ext);
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.pos", initFile.c_str());
-
- std::cout << "filename " << s << std::endl ;
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
- std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
- }
-
- for (unsigned int i = 0; i < 6; i += 1){
- finit >> init_pos[i];
- }
-
- cMo.buildFrom(init_pos);
- init(I);
-}
-
-/*!
- Initialise the tracking thanks to the pose.
-
- \param I : Input image
- \param cMo_ : Pose matrix.
-*/
-void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_)
-{
- this->cMo = cMo_;
- init(I);
-}
-
-/*!
- Initialise the tracking thanks to the pose vector.
-
- \param I : Input image
- \param cPo : Pose vector.
-*/
-void vpMbTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo)
-{
- vpHomogeneousMatrix _cMo(cPo);
- initFromPose(I, _cMo);
-}
-
-/*!
- Save the pose in the given filename
-
- \param filename : Path to the file used to save the pose.
-*/
-void vpMbTracker::savePose(const std::string &filename)
-{
- vpPoseVector init_pos;
- std::fstream finitpos ;
- char s[FILENAME_MAX];
-
- sprintf(s,"%s", filename.c_str());
- finitpos.open(s, std::ios::out) ;
-
- init_pos.buildFrom(cMo);
- finitpos << init_pos;
- finitpos.close();
-}
-
-
-void vpMbTracker::addPolygon(const std::vector<vpPoint>& corners, const int idFace, const std::string &polygonName,
- const bool useLod, const double minPolygonAreaThreshold, const double minLineLengthThreshold)
-{
- std::vector<vpPoint> corners_without_duplicates;
- corners_without_duplicates.push_back(corners[0]);
- for (unsigned int i=0; i < corners.size()-1; i++) {
- if (std::fabs(corners[i].get_oX() - corners[i+1].get_oX()) > std::fabs(corners[i].get_oX())*std::numeric_limits<double>::epsilon()
- || std::fabs(corners[i].get_oY() - corners[i+1].get_oY()) > std::fabs(corners[i].get_oY())*std::numeric_limits<double>::epsilon()
- || std::fabs(corners[i].get_oZ() - corners[i+1].get_oZ()) > std::fabs(corners[i].get_oZ())*std::numeric_limits<double>::epsilon()) {
- corners_without_duplicates.push_back(corners[i+1]);
- }
- }
-
- vpMbtPolygon polygon;
- polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
- polygon.setIndex((int)idFace);
- polygon.setName(polygonName);
- polygon.setLod(useLod);
-
-// //if(minPolygonAreaThreshold != -1.0) {
-// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
-// }
-//
-// //if(minLineLengthThreshold != -1.0) {
-// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinLineLengthThresh(minLineLengthThreshold);
-// }
-
- polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
- polygon.setMinLineLengthThresh(minLineLengthThreshold);
-
- for(unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
- polygon.addPoint(j, corners_without_duplicates[j]);
- }
-
- faces.addPolygon(&polygon);
-
- if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
- faces.getPolygon().back()->setClipping(clippingFlag);
-
- if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
- faces.getPolygon().back()->setNearClippingDistance(distNearClip);
-
- if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
- faces.getPolygon().back()->setFarClippingDistance(distFarClip);
-}
-
-void vpMbTracker::addPolygon(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace, const std::string &polygonName, const bool useLod, const double minPolygonAreaThreshold)
-{
- vpMbtPolygon polygon;
- polygon.setNbPoint(4);
- polygon.setName(polygonName);
- polygon.setLod(useLod);
-
-// //if(minPolygonAreaThreshold != -1.0) {
-// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
-// }
- polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
- //Non sense to set minLineLengthThreshold for circle
- //but used to be coherent when applying LOD settings for all polygons
- polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);
-
- {
- // Create the 4 points of the circle bounding box
- vpPlane plane(p1, p2, p3, vpPlane::object_frame);
-
- // Matrice de passage entre world et circle frame
- double norm_X = sqrt(vpMath::sqr(p2.get_oX()-p1.get_oX())
- + vpMath::sqr(p2.get_oY()-p1.get_oY())
- + vpMath::sqr(p2.get_oZ()-p1.get_oZ()));
- double norm_Y = sqrt(vpMath::sqr(plane.getA())
- + vpMath::sqr(plane.getB())
- + vpMath::sqr(plane.getC()));
- vpRotationMatrix wRc;
- vpColVector x(3),y(3),z(3);
- // X axis is P2-P1
- x[0] = (p2.get_oX()-p1.get_oX()) / norm_X;
- x[1] = (p2.get_oY()-p1.get_oY()) / norm_X;
- x[2] = (p2.get_oZ()-p1.get_oZ()) / norm_X;
- // Y axis is the normal of the plane
- y[0] = plane.getA() / norm_Y;
- y[1] = plane.getB() / norm_Y;
- y[2] = plane.getC() / norm_Y;
- // Z axis = X ^ Y
- z = vpColVector::crossProd(x, y);
- for (unsigned int i=0; i< 3; i++) {
- wRc[i][0] = x[i];
- wRc[i][1] = y[i];
- wRc[i][2] = z[i];
- }
-
- vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
- vpHomogeneousMatrix wMc(wtc, wRc);
-
- vpColVector c_p(4); // A point in the circle frame that is on the bbox
- c_p[0] = radius;
- c_p[1] = 0;
- c_p[2] = radius;
- c_p[3] = 1;
-
- // Matrix to rotate a point by 90 deg around Y in the circle frame
- for(unsigned int i=0; i<4; i++) {
- vpColVector w_p(4); // A point in the word frame
- vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0,vpMath::rad(90*i), 0));
- w_p = wMc * cMc_90 * c_p;
-
- vpPoint w_P;
- w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
-
- polygon.addPoint(i,w_P);
- }
- }
-
- polygon.setIndex(idFace);
- faces.addPolygon(&polygon);
-
- if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
- faces.getPolygon().back()->setClipping(clippingFlag);
-
- if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
- faces.getPolygon().back()->setNearClippingDistance(distNearClip);
-
- if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
- faces.getPolygon().back()->setFarClippingDistance(distFarClip);
-}
-
-void vpMbTracker::addPolygon(const vpPoint& p1, const vpPoint &p2, const int idFace, const std::string &polygonName,
- const bool useLod, const double minLineLengthThreshold)
-{
- // A polygon as a single line that corresponds to the revolution axis of the cylinder
- vpMbtPolygon polygon;
- polygon.setNbPoint(2);
-
- polygon.addPoint(0, p1);
- polygon.addPoint(1, p2);
-
- polygon.setIndex(idFace) ;
- polygon.setName(polygonName);
- polygon.setLod(useLod);
-
-// //if(minLineLengthThreshold != -1.0) {
-// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinLineLengthThresh(minLineLengthThreshold);
-// }
- polygon.setMinLineLengthThresh(minLineLengthThreshold);
- //Non sense to set minPolygonAreaThreshold for cylinder
- //but used to be coherent when applying LOD settings for all polygons
- polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
-
- faces.addPolygon(&polygon) ;
-
- if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
- faces.getPolygon().back()->setClipping(clippingFlag);
-
- if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
- faces.getPolygon().back()->setNearClippingDistance(distNearClip);
-
- if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
- faces.getPolygon().back()->setFarClippingDistance(distFarClip);
-}
-
-/*!
- Load a 3D model from the file in parameter. This file must either be a vrml
- file (.wrl) or a CAO file (.cao). CAO format is described in the
- loadCAOModel() method.
-
- \warning When this class is called to load a vrml model, remember that you
- have to call Call SoDD::finish() before ending the program.
- \code
-int main()
-{
- ...
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
- SoDB::finish();
-#endif
-}
- \endcode
-
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
-
- \param modelFile : the file containing the the 3D model description.
- The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbTracker::loadModel(const char *modelFile, const bool verbose)
-{
- loadModel( std::string(modelFile), verbose );
-}
-
-/*!
- Load a 3D model from the file in parameter. This file must either be a vrml
- file (.wrl) or a CAO file (.cao). CAO format is described in the
- loadCAOModel() method.
-
- \warning When this class is called to load a vrml model, remember that you
- have to call Call SoDD::finish() before ending the program.
- \code
-int main()
-{
- ...
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
- SoDB::finish();
-#endif
-}
- \endcode
-
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
-
- \param modelFile : the file containing the the 3D model description.
- The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
-*/
-void
-vpMbTracker::loadModel(const std::string& modelFile, const bool verbose)
-{
- std::string::const_iterator it;
-
- if(vpIoTools::checkFilename(modelFile)) {
- it = modelFile.end();
- if((*(it-1) == 'o' && *(it-2) == 'a' && *(it-3) == 'c' && *(it-4) == '.') ||
- (*(it-1) == 'O' && *(it-2) == 'A' && *(it-3) == 'C' && *(it-4) == '.') ){
- std::vector<std::string> vectorOfModelFilename;
- int startIdFace = 0;
- nbPoints = 0;
- nbLines = 0;
- nbPolygonLines = 0;
- nbPolygonPoints = 0;
- nbCylinders = 0;
- nbCircles = 0;
- loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true);
- }
- else if((*(it-1) == 'l' && *(it-2) == 'r' && *(it-3) == 'w' && *(it-4) == '.') ||
- (*(it-1) == 'L' && *(it-2) == 'R' && *(it-3) == 'W' && *(it-4) == '.') ){
- loadVRMLModel(modelFile);
- }
- else{
- throw vpException(vpException::ioError, "file cannot be open");
- }
- }
- else{
- throw vpException(vpException::ioError, "file cannot be open");
- }
-
- this->modelInitialised = true;
- this->modelFileName = modelFile;
-}
-
-
-/*!
- Load the 3D model of the object from a vrml file. Only LineSet and FaceSet are
- extracted from the vrml file.
-
- \warning When this class is called, remember that you have to call Call
- SoDD::finish() before ending the program.
- \code
-int main()
-{
- ...
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
- SoDB::finish();
-#endif
-}
- \endcode
-
- \warning The cylinders extracted using this method do not use the Cylinder
- keyword of vrml since vrml exporter such as Blender or AC3D consider a
- cylinder as an IndexedFaceSet. To test whether an indexedFaceSet is a cylinder
- or not, the name of the geometry is read. If the name begins with "cyl" then
- the faceset is supposed to be a cylinder. For example, the line
- \code
-geometry DEF cyl_cylinder1 IndexedFaceSet
- \endcode
- defines a cylinder named cyl_cylinder1.
-
- \throw vpException::fatalError if the file cannot be open.
-
- \param modelFile : The full name of the file containing the 3D model.
-*/
-void
-vpMbTracker::loadVRMLModel(const std::string& modelFile)
-{
-#ifdef VISP_HAVE_COIN
- SoDB::init(); // Call SoDB::finish() before ending the program.
-
- SoInput in;
- SbBool ok = in.openFile(modelFile.c_str());
- SoSeparator *sceneGraph;
- SoVRMLGroup *sceneGraphVRML2;
-
- if (!ok) {
- vpERROR_TRACE("can't open file to load model");
- throw vpException(vpException::fatalError, "can't open file to load model");
- }
-
- if(!in.isFileVRML2())
- {
- sceneGraph = SoDB::readAll(&in);
- if (sceneGraph == NULL) { /*return -1;*/ }
- sceneGraph->ref();
-
- SoToVRML2Action tovrml2;
- tovrml2.apply(sceneGraph);
-
- sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
- sceneGraphVRML2->ref();
- sceneGraph->unref();
- }
- else
- {
- sceneGraphVRML2 = SoDB::readAllVRML(&in);
- if (sceneGraphVRML2 == NULL) { /*return -1;*/ }
- sceneGraphVRML2->ref();
- }
-
- in.closeFile();
-
- vpHomogeneousMatrix transform;
- int indexFace = 0;
- extractGroup(sceneGraphVRML2, transform, indexFace);
-
- sceneGraphVRML2->unref();
-#else
- vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
- throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
-#endif
-}
-
-void vpMbTracker::removeComment(std::ifstream& fileId) {
- char c;
-
- fileId.get(c);
- while (!fileId.fail() && (c == '#')) {
- fileId.ignore(256, '\n');
- fileId.get(c);
- }
- fileId.unget();
-}
-
-std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& endLine) {
- std::map<std::string, std::string> mapOfParams;
-
- bool exit = false;
- while (!endLine.empty() && !exit) {
- exit = true;
-
- for (std::map<std::string, std::string>::const_iterator it =
- mapOfParameterNames.begin(); it != mapOfParameterNames.end();
- ++it) {
- endLine = trim(endLine);
- // std::cout << "endLine=" << endLine << std::endl;
- std::string param(it->first + "=");
-
- //Compare with a potential parameter
- if (endLine.compare(0, param.size(), param) == 0) {
- exit = false;
- endLine = endLine.substr(param.size());
-
- bool parseQuote = false;
- if (it->second == "string") {
- //Check if the string is between quotes
- if (endLine.size() > 2 && endLine[0] == '"') {
- parseQuote = true;
- endLine = endLine.substr(1);
- size_t pos = endLine.find_first_of('"');
-
- if (pos != std::string::npos) {
- mapOfParams[it->first] = endLine.substr(0, pos);
- endLine = endLine.substr(pos + 1);
- } else {
- parseQuote = false;
- }
- }
- }
-
- if (!parseQuote) {
- //Deal with space or tabulation after parameter value to substring
- // to the next sequence
- size_t pos1 = endLine.find_first_of(' ');
- size_t pos2 = endLine.find_first_of('\t');
- size_t pos = pos1 < pos2 ? pos1 : pos2;
-
- mapOfParams[it->first] = endLine.substr(0, pos);
- endLine = endLine.substr(pos + 1);
- }
- }
- }
- }
-
-// for(std::map<std::string, std::string>::const_iterator it = mapOfParams.begin(); it != mapOfParams.end(); ++it) {
-// std::cout << it->first << "=" << it->second << std::endl;
-// }
-
- return mapOfParams;
-}
-
-/*!
- Load a 3D model contained in a *.cao file.
-
- Since ViSP 2.9.1, lines starting with # character are considered as comments.
- It is also possible to add comment at the end of the lines. No specific character is requested before the comment.
- In the following example we use "//" but it could be an other character.
-
- Since ViSP 2.9.1, circles are supported.
-
- The structure of the file is :
- \code
- V1
- # Number of points describing the object
- 8
- 0.01 0.01 0.01 // point with index 0 \
- ... // ... | coordinates of the points in the object frame (in m.)
- 0.01 0.01 0.01 // point with index 7 /
- # Number of lines to track.
- 3
- 0 2 // line with index 0 \
- 1 4 // | Index of the points representing the extremities of the lines
- 1 5 // line with index 2 /
- # Number of polygon (face) to track using the line previously described
- 1
- 3 0 1 2 // Face described as follow : nbLine indexLine1 indexLine2 ... indexLineN
- # Number of polygon (face) to track using the points previously described
- 3
- 4 0 2 3 4 // Face described as follow : nbPoint IndexPoint1 IndexPoint2 ... IndexPointN
- 4 1 3 5 7
- 3 1 5 6
- # Number of cylinder
- 1
- 6 7 0.05 // Index of the limits points on the axis (used to know the 'height' of the cylinder) and radius of the cyclinder (in m.)
- # Number of circle
- 1
- 0.5 0 1 2 // radius, index center point, index 2 other points on the plane containing the circle
- \endcode
-
- \param modelFile : Full name of the main *.cao file containing the model.
- \param vectorOfModelFilename : A vector of *.cao files.
- \param startIdFace : Current Id of the face.
- \param verbose : If true, will print additional information with CAO model files which include other CAO model files.
- \param parent : This parameter is set to true when parsing a parent CAO model file, and false when parsing an included
- CAO model file.
-*/
-void
-vpMbTracker::loadCAOModel(const std::string& modelFile,
- std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
- const bool verbose, const bool parent) {
- std::ifstream fileId;
- fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
- fileId.open(modelFile.c_str(), std::ifstream::in);
- if (fileId.fail()) {
- std::cout << "cannot read CAO model file: " << modelFile << std::endl;
- throw vpException(vpException::ioError, "cannot read CAO model file");
- }
-
- if(verbose) {
- std::cout << "Model file : " << modelFile << std::endl;
- }
- vectorOfModelFilename.push_back(modelFile);
-
- try {
- char c;
- // Extraction of the version (remove empty line and commented ones (comment
- // line begin with the #)).
- //while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
- removeComment(fileId);
-
- //////////////////////////Read CAO Version (V1, V2, ...)//////////////////////////
- int caoVersion;
- fileId.get(c);
- if (c == 'V') {
- fileId >> caoVersion;
- fileId.ignore(256, '\n'); // skip the rest of the line
- } else {
- std::cout
- << "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...";
- throw vpException(vpException::badValue,
- "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...");
- }
-
- removeComment(fileId);
-
-
- //////////////////////////Read the header part if present//////////////////////////
- std::string line;
- std::string prefix = "load";
-
- fileId.get(c);
- fileId.unget();
- bool header = false;
- while(c == 'l' || c == 'L') {
- header = true;
-
- getline(fileId, line);
- if(!line.compare(0, prefix.size(), prefix)) {
-
- //Get the loaded model pathname
- std::string headerPathname = line.substr(6);
- size_t firstIndex = headerPathname.find_first_of("\")");
- headerPathname = headerPathname.substr(0, firstIndex);
-
- std::string headerPath = headerPathname;
- if(!vpIoTools::isAbsolutePathname(headerPathname)) {
- std::string parentDirectory = vpIoTools::getParent(modelFile);
- headerPath = vpIoTools::createFilePath(parentDirectory, headerPathname);
- }
-
- bool cyclic = false;
- std::string headerModelFilename = vpIoTools::getName(headerPath);
- if (!headerModelFilename.compare(vpIoTools::getName(modelFile))) {
- cyclic = true;
- }
-
- for (std::vector<std::string>::const_iterator it =
- vectorOfModelFilename.begin();
- it != vectorOfModelFilename.end() - 1 && !cyclic;
- ++it) {
- std::string loadedModelFilename = vpIoTools::getName(*it);
- if (!headerModelFilename.compare(loadedModelFilename)) {
- cyclic = true;
- }
- }
-
- if (!cyclic) {
- if (vpIoTools::checkFilename(modelFile)) {
- loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false);
- } else {
- throw vpException(vpException::ioError,
- "file cannot be open");
- }
- } else {
- std::cout << "WARNING Cyclic dependency detected with file "
- << headerPath << " declared in " << modelFile << std::endl;
- }
- } else {
- header = false;
- }
-
- removeComment(fileId);
- fileId.get(c);
- fileId.unget();
- }
-
-
- //////////////////////////Read the point declaration part//////////////////////////
- unsigned int caoNbrPoint;
- fileId >> caoNbrPoint;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbPoints += caoNbrPoint;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbrPoint << " points" << std::endl;
- }
-
- if (caoNbrPoint > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points in the CAO model.");
- }
-
- if (caoNbrPoint == 0 && !header) {
- throw vpException(vpException::badValue,
- "in vpMbTracker::loadCAOModel() -> no points are defined");
- }
- vpPoint *caoPoints = new vpPoint[caoNbrPoint];
-
- double x; // 3D coordinates
- double y;
- double z;
-
- int i; // image coordinate (used for matching)
- int j;
-
- for (unsigned int k = 0; k < caoNbrPoint; k++) {
- removeComment(fileId);
-
- fileId >> x;
- fileId >> y;
- fileId >> z;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- if (caoVersion == 2) {
- fileId >> i;
- fileId >> j;
- }
-
- caoPoints[k].setWorldCoordinates(x, y, z);
- }
-
-
- removeComment(fileId);
-
-
- //////////////////////////Read the segment declaration part//////////////////////////
- //Store in a map the potential segments to add
- std::map<std::pair<unsigned int, unsigned int>, SegmentInfo > segmentTemporaryMap;
- unsigned int caoNbrLine;
- fileId >> caoNbrLine;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbLines += caoNbrLine;
- unsigned int *caoLinePoints = NULL;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbrLine << " lines" << std::endl;
- }
-
- if (caoNbrLine > 100000) {
- delete[] caoPoints;
- throw vpException(vpException::badValue,
- "Exceed the max number of lines in the CAO model.");
- }
-
- if (caoNbrLine > 0)
- caoLinePoints = new unsigned int[2 * caoNbrLine];
-
- unsigned int index1, index2;
- //Initialization of idFace with startIdFace for dealing with recursive load in header
- int idFace = startIdFace;
-
- for (unsigned int k = 0; k < caoNbrLine; k++) {
- removeComment(fileId);
-
- fileId >> index1;
- fileId >> index2;
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
- char buffer[256];
- fileId.getline(buffer, 256);
- std::string endLine(buffer);
- std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
-
-// fileId.ignore(256, '\n'); // skip the rest of the line
-
- std::string segmentName = "";
- double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
- bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- if(mapOfParams.find("name") != mapOfParams.end()) {
- segmentName = mapOfParams["name"];
- }
- if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
- minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
- }
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
- useLod = parseBoolean(mapOfParams["useLod"]);
- }
-
- SegmentInfo segmentInfo;
- segmentInfo.name = segmentName;
- segmentInfo.useLod = useLod;
- segmentInfo.minLineLengthThresh = minLineLengthThresh;
-
- caoLinePoints[2 * k] = index1;
- caoLinePoints[2 * k + 1] = index2;
-
- if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
- std::vector<vpPoint> extremities;
- extremities.push_back(caoPoints[index1]);
- extremities.push_back(caoPoints[index2]);
- segmentInfo.extremities = extremities;
-
- std::pair<unsigned int, unsigned int> key(index1, index2);
-
- segmentTemporaryMap[key] = segmentInfo;
-// addPolygon(extremities, idFace++);
-// initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- } else {
- vpTRACE(" line %d has wrong coordinates.", k);
- }
- }
-
- removeComment(fileId);
-
-
- //////////////////////////Read the face segment declaration part//////////////////////////
- /* Load polygon from the lines extracted earlier (the first point of the line is used)*/
- //Store in a vector the indexes of the segments added in the face segment case
- std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
- unsigned int caoNbrPolygonLine;
- fileId >> caoNbrPolygonLine;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbPolygonLines += caoNbrPolygonLine;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
- }
-
- if (caoNbrPolygonLine > 100000) {
- delete[] caoPoints;
- delete[] caoLinePoints;
- throw vpException(vpException::badValue,
- "Exceed the max number of polygon lines.");
- }
-
- unsigned int index;
- for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
- removeComment(fileId);
-
- unsigned int nbLinePol;
- fileId >> nbLinePol;
- std::vector<vpPoint> corners;
- if (nbLinePol > 100000) {
- throw vpException(vpException::badValue, "Exceed the max number of lines.");
- }
-
- for (unsigned int n = 0; n < nbLinePol; n++) {
- fileId >> index;
-// if (2 * index > 2 * caoNbrLine - 1) {
- if(index >= caoNbrLine) {
- throw vpException(vpException::badValue, "Exceed the max number of lines.");
- }
- corners.push_back(caoPoints[caoLinePoints[2 * index]]);
- corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);
-
- std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
- faceSegmentKeyVector.push_back(key);
- }
-
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
- char buffer[256];
- fileId.getline(buffer, 256);
- std::string endLine(buffer);
- std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
-
-// fileId.ignore(256, '\n'); // skip the rest of the line
-
- std::string polygonName = "";
- bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
- polygonName = mapOfParams["name"];
- }
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
- minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
- }
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
- useLod = parseBoolean(mapOfParams["useLod"]);
- }
-
- addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
- // initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
- }
-
- //Add the segments which were not already added in the face segment case
- for(std::map<std::pair<unsigned int, unsigned int>, SegmentInfo >::const_iterator it =
- segmentTemporaryMap.begin(); it != segmentTemporaryMap.end(); ++it) {
- if(std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) == faceSegmentKeyVector.end()) {
- addPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
- it->second.minLineLengthThresh);
- initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- }
- }
-
- removeComment(fileId);
-
-
- //////////////////////////Read the face point declaration part//////////////////////////
- /* Extract the polygon using the point coordinates (top of the file) */
- unsigned int caoNbrPolygonPoint;
- fileId >> caoNbrPolygonPoint;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbPolygonPoints += caoNbrPolygonPoint;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbrPolygonPoint << " polygon points"
- << std::endl;
- }
-
- if (caoNbrPolygonPoint > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of polygon point.");
- }
-
- for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
- removeComment(fileId);
-
- unsigned int nbPointPol;
- fileId >> nbPointPol;
- if (nbPointPol > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
- }
- std::vector<vpPoint> corners;
- for (unsigned int n = 0; n < nbPointPol; n++) {
- fileId >> index;
- if (index > caoNbrPoint - 1) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
- }
- corners.push_back(caoPoints[index]);
- }
-
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
- char buffer[256];
- fileId.getline(buffer, 256);
- std::string endLine(buffer);
- std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
-
-// fileId.ignore(256, '\n'); // skip the rest of the line
-
- std::string polygonName = "";
- bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
- polygonName = mapOfParams["name"];
- }
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
- minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
- }
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
- useLod = parseBoolean(mapOfParams["useLod"]);
- }
-
-
- addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
- initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- }
-
- //////////////////////////Read the cylinder declaration part//////////////////////////
- unsigned int caoNbCylinder;
- try {
- removeComment(fileId);
-
- if (fileId.eof()) { // check if not at the end of the file (for old style files)
- delete[] caoPoints;
- delete[] caoLinePoints;
- return;
- }
-
- /* Extract the cylinders */
- fileId >> caoNbCylinder;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbCylinders += caoNbCylinder;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
- }
-
- if (caoNbCylinder > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of cylinders.");
- }
-
- for (unsigned int k = 0; k < caoNbCylinder; ++k) {
- removeComment(fileId);
-
- double radius;
- unsigned int indexP1, indexP2;
- fileId >> indexP1;
- fileId >> indexP2;
- fileId >> radius;
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
- char buffer[256];
- fileId.getline(buffer, 256);
- std::string endLine(buffer);
- std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
-
-// fileId.ignore(256, '\n'); // skip the rest of the line
-
- std::string polygonName = "";
- bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
- polygonName = mapOfParams["name"];
- }
- if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
- minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
- }
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
- useLod = parseBoolean(mapOfParams["useLod"]);
- }
-
- addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);
- initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idFace++, polygonName);
- }
-
- } catch (...) {
- std::cerr
- << "Cannot get the number of cylinders. Defaulting to zero."
- << std::endl;
- caoNbCylinder = 0;
- }
-
-
- //////////////////////////Read the circle declaration part//////////////////////////
- unsigned int caoNbCircle;
- try {
- removeComment(fileId);
-
- if (fileId.eof()) { // check if not at the end of the file (for old style files)
- delete[] caoPoints;
- delete[] caoLinePoints;
- return;
- }
-
- /* Extract the circles */
- fileId >> caoNbCircle;
- fileId.ignore(256, '\n'); // skip the rest of the line
-
- nbCircles += caoNbCircle;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbCircle << " circles" << std::endl;
- }
-
- if (caoNbCircle > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of cicles.");
- }
-
- for (unsigned int k = 0; k < caoNbCircle; ++k) {
- removeComment(fileId);
-
- double radius;
- unsigned int indexP1, indexP2, indexP3;
- fileId >> radius;
- fileId >> indexP1;
- fileId >> indexP2;
- fileId >> indexP3;
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
- char buffer[256];
- fileId.getline(buffer, 256);
- std::string endLine(buffer);
- std::map<std::string, std::string> mapOfParams = parseParameters(endLine);
-
-// fileId.ignore(256, '\n'); // skip the rest of the line
-
- std::string polygonName = "";
- bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
- polygonName = mapOfParams["name"];
- }
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
- minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
- }
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
- useLod = parseBoolean(mapOfParams["useLod"]);
- }
-
- addPolygon(caoPoints[indexP1], caoPoints[indexP2],
- caoPoints[indexP3], radius, idFace, polygonName, useLod, minPolygonAreaThreshold);
-
- initCircle(caoPoints[indexP1], caoPoints[indexP2],
- caoPoints[indexP3], radius, idFace++, polygonName);
- }
-
- } catch (...) {
- std::cerr << "Cannot get the number of circles. Defaulting to zero."
- << std::endl;
- caoNbCircle = 0;
- }
-
- startIdFace = idFace;
-
- delete[] caoPoints;
- delete[] caoLinePoints;
-
- if(vectorOfModelFilename.size() > 1 && parent) {
- if(verbose) {
- std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
- std::cout << "Total nb of points : " << nbPoints << std::endl;
- std::cout << "Total nb of lines : " << nbLines << std::endl;
- std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
- std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
- std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
- std::cout << "Total nb of circles : " << nbCircles << std::endl;
- } else {
- std::cout << "> " << nbPoints << " points" << std::endl;
- std::cout << "> " << nbLines << " lines" << std::endl;
- std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
- std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
- std::cout << "> " << nbCylinders << " cylinders" << std::endl;
- std::cout << "> " << nbCircles << " circles" << std::endl;
- }
- }
- } catch (...) {
- std::cerr << "Cannot read line!" << std::endl;
- throw vpException(vpException::ioError, "cannot read line");
- }
-}
-
-#ifdef VISP_HAVE_COIN
-/*!
- Extract a VRML object Group.
-
- \param sceneGraphVRML2 : Current node (either Transform, or Group node).
- \param transform : Transformation matrix for this group.
- \param idFace : Index of the face.
-*/
-void
-vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
-{
- vpHomogeneousMatrix transformCur;
- SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
- if(sceneGraphVRML2Trasnform){
- float rx, ry, rz, rw;
- sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
- vpRotationMatrix rotMat(vpQuaternionVector(rx,ry,rz,rw));
-// std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " << rw << std::endl;
-
- float tx, ty, tz;
- tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
- ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
- tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
- vpTranslationVector transVec(tx,ty,tz);
-// std::cout << "Translation: " << tx << " " << ty << " " << tz << std::endl;
-
- float sx, sy, sz;
- sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
- sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
- sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
-// std::cout << "Scale: " << sx << " " << sy << " " << sz << std::endl;
-
- for(unsigned int i = 0 ; i < 3 ; i++)
- rotMat[0][i] *= sx;
- for(unsigned int i = 0 ; i < 3 ; i++)
- rotMat[1][i] *= sy;
- for(unsigned int i = 0 ; i < 3 ; i++)
- rotMat[2][i] *= sz;
-
- transformCur = vpHomogeneousMatrix(transVec,rotMat);
- transform = transform * transformCur;
- }
-
- int nbShapes = sceneGraphVRML2->getNumChildren();
-// std::cout << sceneGraphVRML2->getTypeId().getName().getString() << std::endl;
-// std::cout << "Nb object in VRML : " << nbShapes << std::endl;
-
- SoNode * child;
-
- for (int i = 0; i < nbShapes; i++)
- {
- vpHomogeneousMatrix transform_recursive(transform);
- child = sceneGraphVRML2->getChild(i);
-
- if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
- extractGroup((SoVRMLGroup*)child, transform_recursive, idFace);
- }
-
- if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
- extractGroup((SoVRMLTransform*)child, transform_recursive, idFace);
- }
-
- if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
- SoChildList * child2list = child->getChildren();
- for (int j = 0; j < child2list->getLength(); j++)
- {
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
- {
- SoVRMLIndexedFaceSet * face_set;
- face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
- if(!strncmp(face_set->getName().getString(),"cyl",3)){
- extractCylinders(face_set, transform, idFace);
- }else{
- extractFaces(face_set, transform, idFace);
- }
- }
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
- {
- SoVRMLIndexedLineSet * line_set;
- line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
- extractLines(line_set, idFace);
- }
- }
- }
- }
-}
-
-/*!
- Extract a face of the object to track from the VMRL model. This method calls
- the initFaceFromCorners() method implemented in the child class.
-
- \param face_set : Pointer to the face in the vrml format.
- \param transform : Transformation matrix applied to the face.
- \param idFace : Face id.
-*/
-void
-vpMbTracker::extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace)
-{
- std::vector<vpPoint> corners;
- corners.resize(0);
-
-// SoMFInt32 indexList = _face_set->coordIndex;
-// int indexListSize = indexList.getNum();
- int indexListSize = face_set->coordIndex.getNum();
-
- vpColVector pointTransformed(4);
- vpPoint pt;
- SoVRMLCoordinate *coord;
-
- for (int i = 0; i < indexListSize; i++)
- {
- if (face_set->coordIndex[i] == -1)
- {
- if(corners.size() > 1)
- {
- addPolygon(corners, idFace++);
- initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- corners.resize(0);
- }
- }
- else
- {
- coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
- int index = face_set->coordIndex[i];
- pointTransformed[0]=coord->point[index].getValue()[0];
- pointTransformed[1]=coord->point[index].getValue()[1];
- pointTransformed[2]=coord->point[index].getValue()[2];
- pointTransformed[3] = 1.0;
-
- pointTransformed = transform * pointTransformed;
-
- pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
- corners.push_back(pt);
- }
- }
-}
-
-/*!
- Extract a cylinder to track from the VMRL model. This method calls
- the initCylinder() method implemented in the child class.
-
- \warning This method extract cylinder described using an indexed face set not
- a cylinder set since software such as AC3D or blender export a cylinder using
- this data type. the object name is used, if it begins with "cyl" then this
- method is called otherwise the extractFaces() is used.
-
- \param face_set : Pointer to the cylinder in the vrml format.
- \param transform : Transformation matrix applied to the cylinder.
- \param idFace : Id of the face.
-*/
-void
-vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace)
-{
- std::vector<vpPoint> corners_c1, corners_c2;//points belonging to the first circle and to the second one.
- SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
-
- unsigned int indexListSize = (unsigned int)coords->point.getNum();
-
- if(indexListSize % 2 == 1){
- std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
- throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
- }
- corners_c1.resize(indexListSize / 2);
- corners_c2.resize(indexListSize / 2);
- vpColVector pointTransformed(4);
- vpPoint pt;
-
-
- // extract all points and fill the two sets.
- for(int i=0; i<coords->point.getNum(); ++i){
- pointTransformed[0]=coords->point[i].getValue()[0];
- pointTransformed[1]=coords->point[i].getValue()[1];
- pointTransformed[2]=coords->point[i].getValue()[2];
- pointTransformed[3] = 1.0;
-
- pointTransformed = transform * pointTransformed;
-
- pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
-
- if(i < (int)corners_c1.size()){
- corners_c1[(unsigned int)i] = pt;
- }else{
- corners_c2[(unsigned int)i-corners_c1.size()] = pt;
- }
- }
-
- vpPoint p1 = getGravityCenter(corners_c1);
- vpPoint p2 = getGravityCenter(corners_c2);
-
- vpColVector dist(3);
- dist[0] = p1.get_oX() - corners_c1[0].get_oX();
- dist[1] = p1.get_oY() - corners_c1[0].get_oY();
- dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
- double radius_c1 = sqrt(dist.sumSquare());
- dist[0] = p2.get_oX() - corners_c2[0].get_oX();
- dist[1] = p2.get_oY() - corners_c2[0].get_oY();
- dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
- double radius_c2 = sqrt(dist.sumSquare());
-
- if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))){
- std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
- throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
- }
-
- addPolygon(p1, p2, idFace);
- initCylinder(p1, p2, radius_c1, idFace++);
-}
-
-/*!
- Extract a line of the object to track from the VMRL model. This method calls
- the initFaceFromCorners() method implemented in the child class.
-
- \param line_set : Pointer to the line in the vrml format.
- \param idFace : Id of the face.
-*/
-void
-vpMbTracker::extractLines(SoVRMLIndexedLineSet* line_set, int &idFace)
-{
- std::vector<vpPoint> corners;
- corners.resize(0);
-
- int indexListSize = line_set->coordIndex.getNum();
-
- SbVec3f point(0,0,0);
- vpPoint pt;
- SoVRMLCoordinate *coord;
-
- for (int i = 0; i < indexListSize; i++)
- {
- if (line_set->coordIndex[i] == -1)
- {
- if(corners.size() > 1)
- {
- addPolygon(corners, idFace++);
- initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
- corners.resize(0);
- }
- }
- else
- {
- coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
- int index = line_set->coordIndex[i];
- point[0]=coord->point[index].getValue()[0];
- point[1]=coord->point[index].getValue()[1];
- point[2]=coord->point[index].getValue()[2];
-
- pt.setWorldCoordinates(point[0],point[1],point[2]);
- corners.push_back(pt);
- }
- }
-}
-
-#endif // VISP_HAVE_COIN
-
-/*!
- Compute the center of gravity of a set of point. This is used in the cylinder
- extraction to find the center of the circles.
-
- \throw vpException::dimensionError if the set is empty.
-
- \param pts : Set of point to extract the center of gravity.
- \return Center of gravity of the set.
-*/
-vpPoint
-vpMbTracker::getGravityCenter(const std::vector<vpPoint>& pts)
-{
- if(pts.empty()){
- std::cout << "Cannot extract center of gravity of empty set." << std::endl;
- throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
- }
- double oX = 0;
- double oY = 0;
- double oZ = 0;
- vpPoint G;
-
- for(unsigned int i=0; i<pts.size(); ++i){
- oX += pts[i].get_oX();
- oY += pts[i].get_oY();
- oZ += pts[i].get_oZ();
- }
-
- G.setWorldCoordinates(oX/pts.size(), oY/pts.size(), oZ/pts.size());
- return G;
-}
-
-/*!
- Get the list of polygons faces (a vpPolygon representing the projection of the face in the image and a list of face corners
- in 3D), with the possibility to order by distance to the camera or to use the visibility check to consider if the polygon
- face must be retrieved or not.
-
- \param orderPolygons : If true, the resulting list is ordered from the nearest polygon faces to the farther.
- \param useVisibility : If true, only visible faces will be retrieved.
- \return A pair object containing the list of vpPolygon and the list of face corners.
- */
-std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
-vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility)
-{
- vpHomogeneousMatrix cMo;
- vpCameraParameters cam;
- getPose(cMo);
- getCameraParameters(cam);
-
- //Temporary variable to permit to order polygons by distance
- std::vector<vpPolygon> polygonsTmp;
- std::vector<std::vector<vpPoint> > roisPtTmp;
-
- //Pair containing the list of vpPolygon and the list of face corners
- std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
-
- for (unsigned int i = 0; i < getNbPolygon(); i++) {
- std::vector<vpImagePoint> roi;
- std::vector<vpPoint> roiPt;
- //A face has at least three points
- if (getPolygon(i)->nbpt >= 3) {
- if((useVisibility && getPolygon(i)->isvisible) || !useVisibility) {
- for (unsigned int j = 0; j < getPolygon(i)->nbpt; j++) {
- vpPoint pt(getPolygon(i)->p[j]);
- pt.project(cMo);
- double u = 0, v = 0;
- vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
- roi.push_back(vpImagePoint(v, u));
- roiPt.push_back(pt);
- }
-
- polygonsTmp.push_back(vpPolygon(roi));
- roisPtTmp.push_back(roiPt);
- }
- }
- }
-
- if(orderPolygons) {
- //Order polygons by distance (near to far)
- std::vector<PolygonFaceInfo> listOfPolygonFaces;
- for(unsigned int i = 0; i < polygonsTmp.size(); i++) {
- double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
- for(unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
- x_centroid += roisPtTmp[i][j].get_X();
- y_centroid += roisPtTmp[i][j].get_Y();
- z_centroid += roisPtTmp[i][j].get_Z();
- }
-
- x_centroid /= roisPtTmp[i].size();
- y_centroid /= roisPtTmp[i].size();
- z_centroid /= roisPtTmp[i].size();
-
- double squared_dist = x_centroid*x_centroid + y_centroid*y_centroid + z_centroid*z_centroid;
- listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
- }
-
- //Sort the list of polygon faces
- std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
-
- polygonsTmp.resize(listOfPolygonFaces.size());
- roisPtTmp.resize(listOfPolygonFaces.size());
-
- size_t cpt = 0;
- for(std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
- ++it, cpt++) {
- polygonsTmp[cpt] = it->polygon;
- roisPtTmp[cpt] = it->faceCorners;
- }
-
- pairOfPolygonFaces.first = polygonsTmp;
- pairOfPolygonFaces.second = roisPtTmp;
- } else {
- pairOfPolygonFaces.first = polygonsTmp;
- pairOfPolygonFaces.second = roisPtTmp;
- }
-
- return pairOfPolygonFaces;
-}
-
-/*!
- Use Ogre3D for visibility tests
-
- \warning This function has to be called before the initialization of the tracker.
-
- \param v : True to use it, False otherwise
-*/
-void
-vpMbTracker::setOgreVisibilityTest(const bool &v)
-{
- useOgre = v;
- if(useOgre){
-#ifndef VISP_HAVE_OGRE
- useOgre = false;
- std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
-#endif
- }
-}
-
-/*!
- Set the far distance for clipping.
-
- \param dist : Far clipping value.
-*/
-void
-vpMbTracker::setFarClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING && dist <= distNearClip)
- vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
- else{
- clippingFlag = (clippingFlag | vpMbtPolygon::FAR_CLIPPING);
- distFarClip = dist;
- for (unsigned int i = 0; i < faces.size(); i ++){
- faces[i]->setFarClippingDistance(distFarClip);
- }
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setFarClippingDistance(distFarClip);
-#endif
- }
-}
-
-/*!
- Set the flag to consider if the level of detail (LOD) is used.
-
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param name : name of the face we want to modify the LOD parameter.
-
- \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
- */
-void
-vpMbTracker::setLod(const bool useLod, const std::string &name)
-{
- for (unsigned int i = 0; i < faces.size(); i++)
- {
- if(name.empty() || faces[i]->name == name) {
- faces[i]->setLod(useLod);
- }
- }
-}
-
-/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
-
- \param minLineLengthThresh : threshold for the minimum line length in pixel.
- \param name : name of the face we want to modify the LOD threshold.
-
- \sa setLod(), setMinPolygonAreaThresh()
- */
-void
-vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
-{
- for (unsigned int i = 0; i < faces.size(); i++)
- {
- if(name.empty() || faces[i]->name == name) {
- faces[i]->setMinLineLengthThresh(minLineLengthThresh);
- }
- }
-}
-
-/*!
- Set the minimum polygon area to be considered as visible in the LOD case.
-
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param name : name of the face we want to modify the LOD threshold.
-
- \sa setLod(), setMinLineLengthThresh()
- */
-void
-vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
-{
- for (unsigned int i = 0; i < faces.size(); i++)
- {
- if(name.empty() || faces[i]->name == name) {
- faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
- }
- }
-}
-
-/*!
- Set the near distance for clipping.
-
- \param dist : Near clipping value.
-*/
-void
-vpMbTracker::setNearClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING && dist >= distFarClip)
- vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
- else{
- clippingFlag = (clippingFlag | vpMbtPolygon::NEAR_CLIPPING);
- distNearClip = dist;
- for (unsigned int i = 0; i < faces.size(); i ++){
- faces[i]->setNearClippingDistance(distNearClip);
- }
-#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setNearClippingDistance(distNearClip);
-#endif
- }
-}
-
-/*!
- Specify which clipping to use.
-
- \sa vpMbtPolygonClipping
-
- \param flags : New clipping flags.
-*/
-void
-vpMbTracker::setClipping(const unsigned int &flags)
-{
- clippingFlag = flags;
- for (unsigned int i = 0; i < faces.size(); i ++)
- faces[i]->setClipping(clippingFlag);
-}
-
-
-void
-vpMbTracker::computeCovarianceMatrix(const vpHomogeneousMatrix &cMoPrev, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
-{
- //building Lp
- vpMatrix LpInv(6,6);
- LpInv = 0;
- LpInv[0][0] = -1.0;
- LpInv[1][1] = -1.0;
- LpInv[2][2] = -1.0;
-
- vpTranslationVector ctoInit;
-
- cMoPrev.extract(ctoInit);
- vpMatrix ctoInitSkew = ctoInit.skew();
-
- vpThetaUVector thetau;
- cMoPrev.extract(thetau);
-
- vpColVector tu(3);
- for(unsigned int i = 0 ; i < 3 ; i++)
- tu[i] = thetau[i];
-
- double theta = sqrt(tu.sumSquare()) ;
-
-// vpMatrix Lthetau(3,3);
- vpMatrix LthetauInvAnalytic(3,3);
- vpMatrix I3(3,3);
- I3.setIdentity();
-// Lthetau = -I3;
- LthetauInvAnalytic = -I3;
-
- if(theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon())
- {
- // Computing [theta/2 u]_x
- vpColVector theta2u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- theta2u[i] = tu[i]/2.0 ;
- }
- vpMatrix theta2u_skew = vpColVector::skew(theta2u);
-
- vpColVector u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- u[i] = tu[i]/theta ;
- }
- vpMatrix u_skew = vpColVector::skew(u);
-
-// Lthetau += (theta2u_skew - (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
- LthetauInvAnalytic += -(vpMath::sqr(vpMath::sinc(theta/2.0)) * theta2u_skew - (1.0-vpMath::sinc(theta))*u_skew*u_skew);
- }
-
-// vpMatrix LthetauInv = Lthetau.inverseByLU();
-
- ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
-
- for(unsigned int a = 0 ; a < 3 ; a++)
- for(unsigned int b = 0 ; b < 3 ; b++)
- LpInv[a][b+3] = ctoInitSkew[a][b];
-
- for(unsigned int a = 0 ; a < 3 ; a++)
- for(unsigned int b = 0 ; b < 3 ; b++)
- LpInv[a+3][b+3] = LthetauInvAnalytic[a][b];
-
- // Building Js
- vpMatrix Js = Ls * LpInv;
-
- // building deltaP
- vpColVector deltaP = (Js).pseudoInverse(Js.getRows()*std::numeric_limits<double>::epsilon()) * deltaS;
-
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS,W);
-}
-
-/*!
- Compute \f$ J^T R \f$, with J the interaction matrix and R the vector of
- residu.
-
- \throw vpMatrixException::incorrectMatrixSizeError if the sizes of the
- matrices do not allow the computation.
-
- \warning The JTR matrix is resized.
-
- \param interaction : The interaction matrix (size Nx6).
- \param error : The residu vector (size Nx1).
- \param JTR : The resulting JTR matrix (size 6x1).
-
-*/
-void
-vpMbTracker::computeJTR(const vpMatrix& interaction, const vpColVector& error, vpMatrix& JTR)
-{
- if(interaction.getRows() != error.getRows() || interaction.getCols() != 6 ){
- throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "Incorrect matrices size in computeJTR.");
- }
-
- JTR.resize(6, 1);
- const unsigned int N = interaction.getRows();
-
- for (unsigned int i = 0; i < 6; i += 1){
- double ssum = 0;
- for (unsigned int j = 0; j < N; j += 1){
- ssum += interaction[j][i] * error[j];
- }
- JTR[i][0] = ssum;
- }
-}
-
-/*!
- Get a 1x6 vpColVector representing the estimated degrees of freedom.
- vpColVector[0] = 1 if translation on X is estimated, 0 otherwise;
- vpColVector[1] = 1 if translation on Y is estimated, 0 otherwise;
- vpColVector[2] = 1 if translation on Z is estimated, 0 otherwise;
- vpColVector[3] = 1 if rotation on X is estimated, 0 otherwise;
- vpColVector[4] = 1 if rotation on Y is estimated, 0 otherwise;
- vpColVector[5] = 1 if rotation on Z is estimated, 0 otherwise;
-
- \return 1x6 vpColVector representing the estimated degrees of freedom.
-*/
-vpColVector
-vpMbTracker::getEstimatedDoF()
-{
- vpColVector v(6);
- for(unsigned int i = 0 ; i < 6 ; i++)
- v[i] = oJo[i][i];
- return v;
-}
-
-/*!
- Set a 1x6 vpColVector representing the estimated degrees of freedom. The vector has to be this form:
- vpColVector[0] = 1 if translation on X is estimated, 0 otherwise;
- vpColVector[1] = 1 if translation on Y is estimated, 0 otherwise;
- vpColVector[2] = 1 if translation on Z is estimated, 0 otherwise;
- vpColVector[3] = 1 if rotation on X is estimated, 0 otherwise;
- vpColVector[4] = 1 if rotation on Y is estimated, 0 otherwise;
- vpColVector[5] = 1 if rotation on Z is estimated, 0 otherwise;
-
-*/
-void
-vpMbTracker::setEstimatedDoF(const vpColVector& v)
-{
- if(v.getRows() == 6)
- {
- isoJoIdentity = true;
- for(unsigned int i = 0 ; i < 6 ; i++){
- // if(v[i] != 0){
- if(std::fabs(v[i]) > std::numeric_limits<double>::epsilon()){
- oJo[i][i] = 1.0;
- }
- else{
- oJo[i][i] = 0.0;
- isoJoIdentity = false;
- }
- }
- }
-}
-
-
diff --git a/src/tracking/mbt/vpMbTracker.h b/src/tracking/mbt/vpMbTracker.h
deleted file mode 100755
index 8c35e47..0000000
--- a/src/tracking/mbt/vpMbTracker.h
+++ /dev/null
@@ -1,577 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbTracker.h 5162 2015-01-14 19:09:23Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic model based tracker. This class declares the methods to implement in
- * order to have a model based tracker.
- *
- * Authors:
- * Romain Tallonneau
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMbTracker.h
- \brief Generic model based tracker.
-*/
-#ifndef vpMbTracker_hh
-#define vpMbTracker_hh
-
-#include <vector>
-#include <string>
-#include <sstream>
-#include <fstream>
-#include <map>
-#include <algorithm>
-#include <cctype>
-#include <locale>
-#include <functional> // std::not1
-
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMbtPolygon.h>
-#include <visp/vpMbHiddenFaces.h>
-#include <visp/vpPolygon.h>
-
-#ifdef VISP_HAVE_COIN
-//Work around to avoid type redefinition int8_t with Coin
-// #if defined(_WIN32) && defined(VISP_HAVE_OGRE) && (_MSC_VER >= 1600) // Visual Studio 2010
-// #define HAVE_INT8_T 1
-// #endif
-
-//Inventor includes
-# include <Inventor/VRMLnodes/SoVRMLGroup.h>
-# include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
-# include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#endif
-
-/*!
- \class vpMbTracker
- \ingroup ModelBasedTracking
- \brief Main methods for a model-based tracker.
-
- This class provides the main methods for a model based tracker. This pure
- virtual class must be used in inheritance for a tracker that compute the
- interaction matrix and the residu vector using a defined information (edge,
- points of interest, patch, ...)
-
- This class intends to define a common basis for object tracking. This is
- realised by implementing the main functions:
- - init() : Initialisation of the tracker (it includes re-initialisation). This
- method is called at the end of the initClick() method.
- - initFaceFromCorners() : Initialisation of the lines that has to be tracked.
- - track() : Tracking on the current image
- - testTracking() : Test the tracking. This method throws exception if the
- tracking failed.
- - display() : Display the model and eventually other information.
-
-*/
-class VISP_EXPORT vpMbTracker
-{
-protected:
- //! The camera parameters.
- vpCameraParameters cam;
- //! The current pose.
- vpHomogeneousMatrix cMo;
- //! The Degrees of Freedom to estimate
- vpMatrix oJo;
- //! Boolean to know if oJo is identity (for fast computation)
- bool isoJoIdentity;
- //! The name of the file containing the model (it is used to create a file name.0.pos used to store the compute pose in the initClick method).
- std::string modelFileName;
- //! Flag used to ensure that the CAD model is loaded before the initialisation.
- bool modelInitialised;
- //! Filename used to save the initial pose computed using the initClick() method. It is also used to read a previous pose in the same method.
- std::string poseSavingFilename;
- //! Flag used to specify if the covariance matrix has to be computed or not.
- bool computeCovariance;
- //! Covariance matrix
- vpMatrix covarianceMatrix;
- //! If true, the features are displayed.
- bool displayFeatures;
- //! Weights used in the robust scheme
- vpColVector m_w;
- //! Error s-s*
- vpColVector m_error;
-
- //! Set of faces describing the object.
- vpMbHiddenFaces<vpMbtPolygon> faces;
- //! Angle used to detect a face appearance
- double angleAppears;
- //! Angle used to detect a face disappearance
- double angleDisappears;
- //! Distance for near clipping
- double distNearClip;
- //! Distance for near clipping
- double distFarClip;
- //! Flags specifying which clipping to used
- unsigned int clippingFlag;
- //! Use Ogre3d for visibility tests
- bool useOgre;
- //! Number of points in CAO model
- unsigned int nbPoints;
- //! Number of lines in CAO model
- unsigned int nbLines;
- //! Number of polygon lines in CAO model
- unsigned int nbPolygonLines;
- //! Number of polygon points in CAO model
- unsigned int nbPolygonPoints;
- //! Number of cylinders in CAO model
- unsigned int nbCylinders;
- //! Number of circles in CAO model
- unsigned int nbCircles;
- //! True if LOD mode is enabled
- bool useLodGeneral;
- //! True if the CAO model is loaded before the call to loadConfigFile, (deduced by the number of polygons)
- bool applyLodSettingInConfig;
- //! Minimum line length threshold for LOD mode (general setting)
- double minLineLengthThresholdGeneral;
- //! Minimum polygon area threshold for LOD mode (general setting)
- double minPolygonAreaThresholdGeneral;
- //! Map with [map.first]=parameter_names and [map.second]=type (string, number or boolean)
- std::map<std::string, std::string> mapOfParameterNames;
-
-public:
- vpMbTracker();
- virtual ~vpMbTracker();
-
- /*!
- Display the 3D model at a given position using the given camera parameters
- on a grey level image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param cam : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
- */
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
- /*!
- Display the 3D model at a given position using the given camera parameters
- on a color (RGBa) image.
-
- \param I : The image.
- \param cMo : Pose used to project the 3D model into the image.
- \param cam : The camera parameters.
- \param col : The desired color.
- \param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
- */
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
-
- /*! Return the angle used to test polygons appearance. */
- virtual inline double getAngleAppear() const { return angleAppears; }
-
- /*! Return the angle used to test polygons disappearance. */
- virtual inline double getAngleDisappear() const { return angleDisappears; }
-
- /*!
- Get the camera parameters.
-
- \param camera : copy of the camera parameters used by the tracker.
- */
- virtual void getCameraParameters(vpCameraParameters& camera) const { camera = this->cam;}
-
- /*!
- Get the clipping used and defined in vpMbtPolygon::vpMbtPolygonClippingType.
-
- \return Clipping flags.
- */
- virtual inline unsigned int getClipping() const { return clippingFlag; }
-
- /*!
- Get the covariance matrix.
- */
- virtual vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
- }
-
- virtual vpColVector getEstimatedDoF();
-
- /*!
- Return the error vector \f$(s-s^*)\f$ reached after the virtual visual servoing process used to estimate the pose.
-
- The following example shows how to use this function to compute the norm of the residual and the norm of the residual
- normalized by the number of features that are tracked:
- \code
- tracker.track(I);
- std::cout << "Residual: " << sqrt( (tracker.getError()).sumSquare()) << std::endl;
- std::cout << "Residual normalized: " << sqrt( (tracker.getError()).sumSquare())/tracker.getError().size() << std::endl;
- \endcode
-
- \sa getRobustWeights()
- */
- virtual vpColVector getError() {
- return m_error;
- }
-
- /*! Return a reference to the faces structure. */
- inline vpMbHiddenFaces<vpMbtPolygon>& getFaces() { return faces;}
-
- /*!
- Get the far distance for clipping.
-
- \return Far clipping value.
- */
- virtual inline double getFarClippingDistance() const { return distFarClip; }
-
- /*!
- Return the weights vector \f$w_i\f$ computed by the robust scheme.
-
- The following example shows how to use this function to compute the norm of the weighted residual
- and the norm of the weighted residual normalized by the sum of the weights associated to the
- features that are tracked:
- \code
- tracker.track(I);
- vpColVector w = tracker.getRobustWeights();
- vpColVector e = tracker.getError();
- vpColVector we(w.size());
- for(unsigned int i=0; i<w.size(); i++)
- we[i] = w[i]*e[i];
-
- std::cout << "Weighted residual: " << sqrt( (we).sumSquare() ) << std::endl;
- std::cout << "Weighted residual normalized: " << sqrt( (we).sumSquare() ) / w.sum() << std::endl;
- \endcode
-
- \sa getError()
- */
- virtual vpColVector getRobustWeights() {
- return m_w;
- }
-
- /*!
- Get the number of polygons (faces) representing the object to track.
-
- \return Number of polygons.
- */
- virtual inline unsigned int getNbPolygon() const {
- return static_cast<unsigned int>(faces.size());
- }
-
- /*!
- Get the near distance for clipping.
-
- \return Near clipping value.
- */
- virtual inline double getNearClippingDistance() const { return distNearClip; }
-
- /*!
- Return the polygon (face) "index".
-
- \exception vpException::dimensionError if index does not represent a good
- polygon.
-
- \param index : Index of the polygon to return.
- \return Pointer to the polygon index.
- */
- virtual inline vpMbtPolygon* getPolygon(const unsigned int index) {
- if(index >= static_cast<unsigned int>(faces.size()) ){
- throw vpException(vpException::dimensionError, "index out of range");
- }
-
- return faces[index];
- }
-
- virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > getPolygonFaces(const bool orderPolygons=true,
- const bool useVisibility=true);
-
- /*!
- Get the current pose between the object and the camera.
- cMo is the matrix which can be used to express
- coordinates from the object frame to camera frame.
-
- \param cMo_ : the pose
- */
- inline void getPose(vpHomogeneousMatrix& cMo_) const {cMo_ = this->cMo;}
-
- /*!
- Get the current pose between the object and the camera.
- cMo is the matrix which can be used to express
- coordinates from the object frame to camera frame.
-
- \return the current pose
- */
- inline vpHomogeneousMatrix getPose() const {return this->cMo;}
-
- /* PURE VIRTUAL METHODS */
-
- /*!
- Initialise the tracking.
-
- \param I : Input image.
- */
- virtual void init(const vpImage<unsigned char>& I)=0;
-
- // Intializer
-
- virtual void initClick( const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp = false );
- virtual void initClick( const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile = "" );
-
- virtual void initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile );
- virtual void initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list );
-
- virtual void initFromPose(const vpImage<unsigned char>& I, const std::string &initFile);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
-
- /*!
- Load a config file to parameterise the behavior of the tracker.
-
- Pure virtual method to adapt to each tracker.
-
- \param configFile : An xml config file to parse.
- */
- virtual void loadConfigFile(const std::string& configFile)=0;
-
- virtual void loadModel(const char *modelFile, const bool verbose=false);
- virtual void loadModel(const std::string &modelFile, const bool verbose=false);
-
- /*!
- Reset the tracker.
- */
- virtual void resetTracker() = 0;
-
- void savePose(const std::string &filename);
-
- /*!
- Set the angle used to test polygons appearance.
- If the angle between the normal of the polygon and the line going
- from the camera to the polygon center has a value lower than
- this parameter, the polygon is considered as appearing.
- The polygon will then be tracked.
-
- \param a : new angle in radian.
- */
- virtual inline void setAngleAppear(const double &a) { angleAppears = a; }
-
- /*!
- Set the angle used to test polygons disappearance.
- If the angle between the normal of the polygon and the line going
- from the camera to the polygon center has a value greater than
- this parameter, the polygon is considered as disappearing.
- The tracking of the polygon will then be stopped.
-
- \param a : new angle in radian.
- */
- virtual inline void setAngleDisappear(const double &a) { angleDisappears = a; }
-
- /*!
- Set the camera parameters.
-
- \param camera : the new camera parameters
- */
- virtual void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- virtual void setClipping(const unsigned int &flags);
-
- /*!
- Set if the covaraince matrix has to be computed.
-
- \param flag : True if the covariance has to be computed, false otherwise
- */
- virtual void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
-
- virtual void setEstimatedDoF(const vpColVector& v);
-
- /*!
- Enable to display the features. By features, we meant the moving edges (ME) and the klt points if used.
-
- Note that if present, the moving edges can be displayed with different colors:
- - If green : The ME is a good point.
- - If blue : The ME is removed because of a contrast problem during the tracking phase.
- - If purple : The ME is removed because of a threshold problem during the tracking phase.
- - If red : The ME is removed because it is rejected by the robust approach in the virtual visual servoing scheme.
-
- \param displayF : set it to true to display the features.
- */
- void setDisplayFeatures(const bool displayF) {displayFeatures = displayF;}
-
- virtual void setFarClippingDistance(const double &dist);
-
- virtual void setLod(const bool useLod, const std::string &name="");
-
- virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="");
-
- virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="");
-
- virtual void setNearClippingDistance(const double &dist);
-
- /*!
- Set the pose to be used in entry of the next call to the track() function.
- This pose will be just used once.
-
- \warning This function has to be called after the initialisation of the tracker.
-
- \param I : image corresponding to the desired pose.
- \param cdMo : Pose to affect.
- */
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo) = 0;
-
- /*!
- Set the filename used to save the initial pose computed using the
- initClick() method. It is also used to read a previous pose in the same method.
- If the file is not set then, the initClick() method will create a .0.pos
- file in the root directory. This directory is the path to the file given to
- the method initClick() used to know the coordinates in the object frame.
-
- \param filename : The new filename.
- */
- inline void setPoseSavingFilename(const std::string& filename){
- poseSavingFilename = filename;
- }
-
- virtual void setOgreVisibilityTest(const bool &v);
-
- /*!
- Test the quality of the tracking.
-
- \throw vpException if the test fail.
- */
- virtual void testTracking() = 0;
-
- /*!
- Track the object in the given image
-
- \param I : The current image.
- */
- virtual void track(const vpImage<unsigned char>& I)=0;
-
-protected:
- void addPolygon(const std::vector<vpPoint>& corners, const int idFace=-1, const std::string &polygonName="",
- const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0);
- void addPolygon(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=-1,
- const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0);
- void addPolygon(const vpPoint& p1, const vpPoint &p2, const int idFace=-1, const std::string &polygonName="",
- const bool useLod=false, const double minLineLengthThreshold=50);
-
- void computeCovarianceMatrix(const vpHomogeneousMatrix &cMoPrev, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W);
- void computeJTR(const vpMatrix& J, const vpColVector& R, vpMatrix& JTR);
-
-#ifdef VISP_HAVE_COIN
- virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace);
- virtual void extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace);
- virtual void extractLines(SoVRMLIndexedLineSet* line_set, int &idFace);
- virtual void extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace);
-#endif
-
- vpPoint getGravityCenter(const std::vector<vpPoint>& _pts);
-
- /*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
-
- \param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param idFace : Id of the face associated to the circle.
- \param name : Name of the circle.
- */
- virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace=0, const std::string &name="")=0;
- /*!
- Add a cylinder to track from two points on the axis (defining the length of
- the cylinder) and its radius.
-
- \param p1 : First point on the axis.
- \param p2 : Second point on the axis.
- \param radius : Radius of the cylinder.
- \param idFace : Id of the face associated to the cylinder.
- \param name : Name of the cylinder.
- */
- virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
- const std::string &name="")=0;
-
- /*!
- Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
-
- The id of the polygon is supposed to be set prior calling this function.
-
- \param polygon : The polygon describing the set of lines that has to be tracked.
- */
- virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0;
- virtual void initFaceFromLines(vpMbtPolygon &polygon)=0;
-
- virtual void loadVRMLModel(const std::string& modelFile);
- virtual void loadCAOModel(const std::string& modelFile, std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
- const bool verbose=false, const bool parent=true);
-
- void removeComment(std::ifstream& fileId);
-
- inline bool parseBoolean(std::string &input) {
- std::transform(input.begin(), input.end(), input.begin(), ::tolower);
- std::istringstream is(input);
- bool b;
- //Parse string to boolean either in the textual representation (True/False)
- //or in numeric representation (1/0)
- is >> (input.size() > 1 ? std::boolalpha : std::noboolalpha) >> b;
- return b;
- }
-
- std::map<std::string, std::string> parseParameters(std::string& endLine);
-
- inline std::string <rim(std::string &s) {
- s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
- return s;
- }
-
- inline std::string &rtrim(std::string &s) {
- s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
- return s;
- }
-
- inline std::string &trim(std::string &s) {
- return ltrim(rtrim(s));
- }
-};
-
-
-#endif
-
diff --git a/src/tracking/mbt/vpMbXmlParser.cpp b/src/tracking/mbt/vpMbXmlParser.cpp
deleted file mode 100644
index 84b450b..0000000
--- a/src/tracking/mbt/vpMbXmlParser.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbXmlParser.cpp 4577 2014-01-10 16:19:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML Parameter for Model Based Tracker.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-
-#ifdef VISP_HAVE_XML2
-
-#include <iostream>
-#include <map>
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpMbXmlParser.h>
-
-
-/*!
- Default constructor.
-
-*/
-vpMbXmlParser::vpMbXmlParser()
- : cam(), angleAppear(70), angleDisappear(80),
- hasNearClipping(false), nearClipping(false),
- hasFarClipping(false), farClipping(false), fovClipping(false)
-
-{
- init();
-}
-
-/*!
- Default destructor.
-*/
-vpMbXmlParser::~vpMbXmlParser()
-{
-}
-
-/*!
- Initialise internal variables (including the map).
-*/
-void
-vpMbXmlParser::init()
-{
- setMainTag("conf");
-
- nodeMap["conf"] = conf;
- nodeMap["face"] = face;
- nodeMap["angle_appear"] = angle_appear;
- nodeMap["angle_disappear"] = angle_disappear;
- nodeMap["near_clipping"] = near_clipping;
- nodeMap["far_clipping"] = far_clipping;
- nodeMap["fov_clipping"] = fov_clipping;
- nodeMap["camera"] = camera;
- nodeMap["height"] = height;
- nodeMap["width"] = width;
- nodeMap["u0"] = u0;
- nodeMap["v0"] = v0;
- nodeMap["px"] = px;
- nodeMap["py"] = py;
-}
-
-/*!
- Parse the file in parameters.
- This method is deprecated, use parse() instead.
-
- \param filename : File to parse.
-*/
-void
-vpMbXmlParser::parse(const char * filename)
-{
- std::string file = filename;
- vpXmlParser::parse(file);
-}
-
-/*!
- Write info to file.
-
- \warning Useless, so not yet implemented => Throw exception.
-*/
-void
-vpMbXmlParser::writeMainClass(xmlNodePtr /*node*/)
-{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
-}
-
-/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
- \param doc : Document to parse.
- \param node : Root node.
-*/
-void
-vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
-{
- bool camera_node = false;
- bool face_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
- camera_node = true;
- }break;
- case face:{
- this->read_face(doc, dataNode);
- face_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
- }
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
- }
-}
-
-/*!
- Read camera information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the camera information.
-*/
-void
-vpMbXmlParser::read_camera (xmlDocPtr doc, xmlNodePtr node)
-{
- bool u0_node = false;
- bool v0_node = false;
- bool px_node = false;
- bool py_node = false;
-
- // current data values.
- double d_u0 = this->cam.get_u0();
- double d_v0 = this->cam.get_v0();
- double d_px = this->cam.get_px();
- double d_py = this->cam.get_py();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case u0:{
- d_u0 = xmlReadDoubleChild(doc, dataNode);
- u0_node = true;
- }break;
- case v0:{
- d_v0 = xmlReadDoubleChild(doc, dataNode);
- v0_node = true;
- }break;
- case px:{
- d_px = xmlReadDoubleChild(doc, dataNode);
- px_node = true;
- }break;
- case py:{
- d_py = xmlReadDoubleChild(doc, dataNode);
- py_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- this->cam.initPersProjWithoutDistortion(d_px, d_py, d_u0, d_v0) ;
-
- if(!u0_node)
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- else
- std::cout << "camera : u0 : "<< this->cam.get_u0() <<std::endl;
-
- if(!v0_node)
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- else
- std::cout << "camera : v0 : "<< this->cam.get_v0() <<std::endl;
-
- if(!px_node)
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- else
- std::cout << "camera : px : "<< this->cam.get_px() <<std::endl;
-
- if(!py_node)
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
- else
- std::cout << "camera : py : "<< this->cam.get_py() <<std::endl;
-}
-
-/*!
- Read face information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
- \param doc : Pointer to the document.
- \param node : Pointer to the node of the camera information.
-*/
-void
-vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node)
-{
- bool angle_appear_node = false;
- bool angle_disappear_node = false;
- bool near_clipping_node = false;
- bool far_clipping_node = false;
- bool fov_clipping_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case angle_appear:{
- angleAppear = xmlReadDoubleChild(doc, dataNode);
- angle_appear_node = true;
- }break;
- case angle_disappear:{
- angleDisappear = xmlReadDoubleChild(doc, dataNode);
- angle_disappear_node = true;
- }break;
- case near_clipping:{
- nearClipping = xmlReadDoubleChild(doc, dataNode);
- near_clipping_node = true;
- hasNearClipping = true;
- }break;
- case far_clipping:{
- farClipping = xmlReadDoubleChild(doc, dataNode);
- far_clipping_node = true;
- hasFarClipping = true;
- }break;
- case fov_clipping:{
- if (xmlReadIntChild(doc, dataNode))
- fovClipping = true;
- else
- fovClipping = false;
- fov_clipping_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
- }
- }
- }
- }
-
- if(!angle_appear_node)
- std::cout << "face : Angle Appear : "<< angleAppear << " (default)" <<std::endl;
- else
- std::cout << "face : Angle Appear : "<< angleAppear <<std::endl;
-
- if(!angle_disappear_node)
- std::cout << "face : Angle Disappear : "<< angleDisappear << " (default)" <<std::endl;
- else
- std::cout << "face : Angle Disappear : "<< angleDisappear <<std::endl;
-
- if(near_clipping_node)
- std::cout << "face : Near Clipping : "<< nearClipping <<std::endl;
-
- if(far_clipping_node)
- std::cout << "face : Far Clipping : "<< farClipping <<std::endl;
-
- if(fov_clipping_node) {
- if(fovClipping)
- std::cout << "face : Fov Clipping : True" <<std::endl;
- else
- std::cout << "face : Fov Clipping : False" <<std::endl;
- }
-}
-
-#endif
-
diff --git a/src/tracking/mbt/vpMbXmlParser.h b/src/tracking/mbt/vpMbXmlParser.h
deleted file mode 100644
index 198d70b..0000000
--- a/src/tracking/mbt/vpMbXmlParser.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbXmlParser.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Load XML Parameter for Model Based Tracker.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- * \file vpMbXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
-
-#ifndef vpMbXmlParser_HH
-#define vpMbXmlParser_HH
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_HAVE_XML2
-
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-
-#include <visp/vpXmlParser.h>
-#include <visp/vpCameraParameters.h>
-
-/*!
- \class vpMbXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup ModelBasedTracking
-
- Data parser for the model based tracker.
-
- */
-class VISP_EXPORT vpMbXmlParser: public vpXmlParser
-{
-protected:
- //! Camera parameters.
- vpCameraParameters cam;
- //! Angle to determine if a face appeared
- double angleAppear;
- //! Angle to determine if a face disappeared
- double angleDisappear;
- //! Is near clipping distance specified?
- bool hasNearClipping;
- //! Near clipping distance
- double nearClipping;
- //! Is far clipping distance specified?
- bool hasFarClipping;
- //! Near clipping distance
- double farClipping;
- //! Fov Clipping
- bool fovClipping;
-
- typedef enum{
- conf,
- face,
- angle_appear,
- angle_disappear,
- near_clipping,
- far_clipping,
- fov_clipping,
- camera,
- height,
- width,
- u0,
- v0,
- px,
- py,
- last
- } dataToParseMb;
-
-
-public:
-
- vpMbXmlParser();
- virtual ~vpMbXmlParser();
-
- /*!
- Get the angle to determine if a face appeared.
-
- \return angleAppear
- */
- inline double getAngleAppear() const {return angleAppear;}
-
- /*!
- Get the angle to determine if a face disappeared.
-
- \return angleDisappear
- */
- inline double getAngleDisappear() const {return angleDisappear;}
-
- void getCameraParameters(vpCameraParameters& _cam) const { _cam = this->cam;}
-
- /*!
- Get the far clipping distance.
-
- \return farClipping
- */
- inline double getFarClippingDistance() const {return farClipping;}
-
- /*!
- Use FOV clipping
-
- \return True if yes, False otherwise.
- */
- inline bool getFovClipping() const {return fovClipping;}
-
- /*!
- Get the near clipping distance.
-
- \return nearClipping
- */
- inline double getNearClippingDistance() const {return nearClipping;}
-
- /*!
- Has Far clipping been specified?
-
- \return True if yes, False otherwise.
- */
- inline bool hasFarClippingDistance() const {return hasFarClipping;}
-
- /*!
- Has Near clipping been specified?
-
- \return True if yes, False otherwise.
- */
- inline bool hasNearClippingDistance() const {return hasNearClipping;}
-
- void parse(const char * filename);
-
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_camera (xmlDocPtr doc, xmlNodePtr node);
- void read_face(xmlDocPtr doc, xmlNodePtr node);
-
- /*!
- Set the angle to determine if a face appeared.
-
- \param aappear : New angleAppear
- */
- inline void setAngleAppear(const double &aappear) {angleAppear = aappear;}
-
- /*!
- Set the angle to determine if a face disappeared.
-
- \param adisappear : New angleDisappear
- */
- inline void setAngleDisappear(const double &adisappear) {angleDisappear = adisappear;}
-
- void setCameraParameters(const vpCameraParameters &_cam){ cam = _cam; }
-
- /*!
- Set the far clipping distance.
-
- \param fclip : New farClipping
- */
- inline void setFarClippingDistance(const double &fclip) {farClipping = fclip;}
-
- /*!
- Set the near clipping distance.
-
- \param nclip : New nearClipping
- */
- inline void setNearClippingDistance(const double &nclip) {nearClipping = nclip;}
-
- void writeMainClass(xmlNodePtr node);
-
-protected:
- void init();
-
-};
-
-#endif
-
-#endif /* NMBXMLPARSER_H_ */
-
-
-
diff --git a/src/tracking/mbt/vpMbtPolygon.cpp b/src/tracking/mbt/vpMbtPolygon.cpp
deleted file mode 100644
index fe779b3..0000000
--- a/src/tracking/mbt/vpMbtPolygon.cpp
+++ /dev/null
@@ -1,844 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtPolygon.cpp 4952 2014-11-12 14:04:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Make the complete tracking of an object by using its CAD model
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <limits.h>
-
-#include <visp/vpConfig.h>
-/*!
- \file vpMbtPolygon.cpp
- \brief Implements a polygon of the model used by the model-based tracker.
-*/
-
-#include <visp/vpMbtPolygon.h>
-#include <visp/vpPolygon.h>
-
-/*!
- Basic constructor.
-*/
-vpMbtPolygon::vpMbtPolygon()
- : index(-1), nbpt(0), nbCornersInsidePrev(0), isvisible(false), isappearing(false),
- p(NULL), roiPointsClip(), clippingFlag(vpMbtPolygon::NO_CLIPPING),
- distNearClip(0.001), distFarClip(100.),
- useLod(false), minLineLengthThresh(50.0), minPolygonAreaThresh(2500.0), name("")
-{
-}
-
-vpMbtPolygon::vpMbtPolygon(const vpMbtPolygon& mbtp)
- : index(-1), nbpt(0), nbCornersInsidePrev(0), isvisible(false), isappearing(false),
- p(NULL), roiPointsClip(), clippingFlag(vpMbtPolygon::NO_CLIPPING),
- distNearClip(0.001), distFarClip(100.),
- useLod(false), minLineLengthThresh(50.0), minPolygonAreaThresh(2500.0), name("")
-{
- *this = mbtp;
-}
-
-vpMbtPolygon& vpMbtPolygon::operator=(const vpMbtPolygon& mbtp)
-{
- index = mbtp.index;
- nbpt = mbtp.nbpt;
- nbCornersInsidePrev = mbtp.nbCornersInsidePrev;
- isvisible = mbtp.isvisible;
- isappearing = mbtp.isappearing;
- roiPointsClip = mbtp.roiPointsClip;
- clippingFlag = mbtp.clippingFlag;
- distNearClip = mbtp.distNearClip;
- distFarClip = mbtp.distFarClip;
- useLod = mbtp.useLod;
- minLineLengthThresh = mbtp.minLineLengthThresh;
- minPolygonAreaThresh = mbtp.minPolygonAreaThresh;
- name = mbtp.name;
-
- if (p) delete [] p;
- p = new vpPoint [nbpt];
- for(unsigned int i = 0; i < nbpt; i++)
- p[i] = mbtp.p[i];
-
- return (*this);
-}
-
-/*!
- Basic destructor.
-*/
-vpMbtPolygon::~vpMbtPolygon()
-{
- if (p !=NULL)
- {
- delete[] p;
- p = NULL;
- }
-}
-
-/*!
- Get a reference to a corner.
-
- \throw vpException::dimensionError if the _index is out of range.
-
- \param _index : the index of the corner
-*/
-vpPoint &
-vpMbtPolygon::getPoint(const unsigned int _index)
-{
- if(_index >= nbpt){
- throw vpException(vpException::dimensionError, "index out of range");
- }
- return p[_index];
-}
-
-/*!
- Set the number of points which are the corners of the polygon.
-
- \param nb : The number of corners.
-*/
-void
-vpMbtPolygon::setNbPoint(const unsigned int nb)
-{
- nbpt = nb ;
- if (p != NULL)
- delete[] p;
- p = new vpPoint[nb] ;
-}
-
-/*!
- Add a corner point to the list of polygon's corners.
-
- \param n : The index of the corner.
- \param P : The point to add.
-*/
-void
-vpMbtPolygon::addPoint(const unsigned int n, const vpPoint &P)
-{
- //if( p!NULL && n < nbpt )
- p[n] = P ;
-}
-
-/*!
- Project the 3D corner points into the image thanks to the pose of the camera.
-
- \param cMo : The pose of the camera.
-*/
-void
-vpMbtPolygon::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- for (unsigned int i = 0 ; i < nbpt ; i++)
- {
- p[i].changeFrame(cMo) ;
- p[i].projection() ;
- }
-}
-
-/*!
- Check if the polygon is visible in the image and if the angle between the normal
- to the face and the line vector going from the optical center to the cog of the face is below
- the given threshold.
- To do that, the polygon is projected into the image thanks to the camera pose.
-
- \param cMo : The pose of the camera.
- \param alpha : Maximum angle to detect if the face is visible (in rad).
- \param modulo : Indicates if the test should also consider faces that are not oriented
- counter clockwise. If true, the orientation of the face is without importance.
- \param cam : Camera parameters (intrinsics parameters)
- \param I : Image used to consider level of detail.
-
- \return Return true if the polygon is visible.
-*/
-bool
-vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
- const vpCameraParameters &cam, const vpImage<unsigned char> &I)
-{
- // std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
-
- changeFrame(cMo);
-
- if(nbpt <= 2) {
- //Level of detail (LOD)
- if(useLod) {
- vpCameraParameters c = cam;
- if(clippingFlag > 3) { // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
- }
- computeRoiClipped(c);
- std::vector<vpImagePoint> roiImagePoints;
- getRoiClipped(c, roiImagePoints);
-
- if (roiImagePoints.size() == 2) {
- double x1 = roiImagePoints[0].get_u();
- double y1 = roiImagePoints[0].get_v();
- double x2 = roiImagePoints[1].get_u();
- double y2 = roiImagePoints[1].get_v();
- double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
-// std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
-// vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);
-
- if (length < minLineLengthThresh) {
- isvisible = false;
- isappearing = false;
- return false;
- }
- }
- }
-
- /* a line is always visible when LOD is not used */
- isvisible = true;
- isappearing = false;
- return true ;
- }
-
- //Check visibility from normal
- //Newell's Method for calculating the normal of an arbitrary 3D polygon
- //https://www.opengl.org/wiki/Calculating_a_Surface_Normal
- vpColVector faceNormal(3);
- vpColVector currentVertex, nextVertex;
- for(unsigned int i = 0; i<nbpt; i++) {
- currentVertex = p[i].cP;
- nextVertex = p[(i+1) % nbpt].cP;
-
- faceNormal[0] += (currentVertex[1] - nextVertex[1]) * (currentVertex[2] + nextVertex[2]);
- faceNormal[1] += (currentVertex[2] - nextVertex[2]) * (currentVertex[0] + nextVertex[0]);
- faceNormal[2] += (currentVertex[0] - nextVertex[0]) * (currentVertex[1] + nextVertex[1]);
- }
- faceNormal.normalize();
-
- vpColVector e4(3) ;
- vpPoint pt;
- for (unsigned int i = 0; i < nbpt; i += 1){
- pt.set_X(pt.get_X() + p[i].get_X());
- pt.set_Y(pt.get_Y() + p[i].get_Y());
- pt.set_Z(pt.get_Z() + p[i].get_Z());
- }
- e4[0] = -pt.get_X() / (double)nbpt;
- e4[1] = -pt.get_Y() / (double)nbpt;
- e4[2] = -pt.get_Z() / (double)nbpt;
- e4.normalize();
-
- double angle = acos(vpColVector::dotProd(e4, faceNormal));
-
-// vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;
-
- if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
- isvisible = true;
- isappearing = false;
-
- if (useLod) {
- vpCameraParameters c = cam;
- if(clippingFlag > 3) { // Contains at least one FOV constraint
- c.computeFov(I.getWidth(), I.getHeight());
- }
- computeRoiClipped(c);
- std::vector<vpImagePoint> roiImagePoints;
- getRoiClipped(c, roiImagePoints);
-
- vpPolygon roiPolygon(roiImagePoints);
- double area = roiPolygon.getArea();
-// std::cout << "After normal test ; Index=" << index << " ; area=" << area << " ; clippingFlag="
-// << clippingFlag << std::endl;
- if (area < minPolygonAreaThresh) {
- isappearing = false;
- isvisible = false;
- return false;
- }
- }
-
- return true;
- }
-
- if (angle < alpha+vpMath::rad(1) ){
- isappearing = true;
- }
- else if (modulo && (M_PI - angle) < alpha+vpMath::rad(1) ){
- isappearing = true;
- }
- else {
- isappearing = false;
- }
-
- isvisible = false ;
- return false ;
-}
-
-/*!
- Compute the region of interest in the image according to the used clipping.
-
- \warning If the FOV clipping is used, camera normals have to be precomputed.
-
- \param cam : camera parameters used to compute the field of view.
-*/
-void
-vpMbtPolygon::computeRoiClipped(const vpCameraParameters &cam)
-{
- roiPointsClip.clear();
- std::vector<vpColVector> fovNormals;
- std::vector<std::pair<vpPoint,unsigned int> > roiPointsClipTemp;
- std::vector<std::pair<vpPoint,unsigned int> > roiPointsClipTemp2;
-
- if(cam.isFovComputed() && clippingFlag > 3)
- fovNormals = cam.getFovNormals();
-
- for(unsigned int i = 0 ; i < nbpt ; i++){
- p[i%nbpt].projection();
- roiPointsClipTemp.push_back(std::make_pair(p[i%nbpt],vpMbtPolygon::NO_CLIPPING));
- }
-
- if(clippingFlag != vpMbtPolygon::NO_CLIPPING)
- for(unsigned int i = 1 ; i < 64 ; i=i*2)
- {
- if(((clippingFlag & i) == i) || ((clippingFlag > vpMbtPolygon::FAR_CLIPPING) && (i==1)))
- {
- for(unsigned int j = 0 ; j < roiPointsClipTemp.size() ; j++)
- {
- vpPoint p1Clipped = roiPointsClipTemp[j].first;
- vpPoint p2Clipped = roiPointsClipTemp[(j+1)%roiPointsClipTemp.size()].first;
-
- unsigned int p2ClippedInfoBefore = roiPointsClipTemp[(j+1)%roiPointsClipTemp.size()].second;
- unsigned int p1ClippedInfo = roiPointsClipTemp[j].second;
- unsigned int p2ClippedInfo = roiPointsClipTemp[(j+1)%roiPointsClipTemp.size()].second;
-
- bool problem = true;
-
- switch(i){
- case 1:
- problem = !(vpMbtPolygon::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- i, distNearClip));
- break;
- case 2:
- problem = !(vpMbtPolygon::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- i, distFarClip));
- break;
- case 4:
- problem = !(vpMbtPolygon::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[0], vpMbtPolygon::LEFT_CLIPPING));
- break;
- case 8:
- problem = !(vpMbtPolygon::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[1], vpMbtPolygon::RIGHT_CLIPPING));
- break;
- case 16:
- problem = !(vpMbtPolygon::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[2], vpMbtPolygon::UP_CLIPPING));
- break;
- case 32:
- problem = !(vpMbtPolygon::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[3], vpMbtPolygon::DOWN_CLIPPING));
- break;
- }
-
- if(!problem)
- {
- p1Clipped.projection();
- roiPointsClipTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
-
- if(p2ClippedInfo != p2ClippedInfoBefore)
- {
- p2Clipped.projection();
- roiPointsClipTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
- }
-
- if(nbpt == 2){
- if(p2ClippedInfo == p2ClippedInfoBefore)
- {
- p2Clipped.projection();
- roiPointsClipTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
- }
- break;
- }
- }
- }
-
- roiPointsClipTemp = roiPointsClipTemp2;
- roiPointsClipTemp2.clear();
- }
- }
-
- roiPointsClip = roiPointsClipTemp;
-}
-
-/*!
- Get the clipped points according to a plane equation.
-
- \param cam : camera parameters
- \param p1 : First extremity of the line.
- \param p2 : Second extremity of the line.
- \param p1Clipped : Resulting p1.
- \param p2Clipped : Resulting p2.
- \param p1ClippedInfo : Resulting clipping flag for p1.
- \param p2ClippedInfo : Resulting clipping flag for p2.
- \param A : Param A from plane equation.
- \param B : Param B from plane equation.
- \param C : Param C from plane equation.
- \param D : Param D from plane equation.
- \param flag : flag specifying the clipping used when calling this function.
-
- \return True if the points have been clipped, False otherwise
-*/
-bool
-vpMbtPolygon::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const vpColVector &normal, const unsigned int &flag)
-{
- vpRowVector p1Vec(3);
- p1Vec[0] = p1.get_X(); p1Vec[1] = p1.get_Y(); p1Vec[2] = p1.get_Z();
- p1Vec = p1Vec.normalize();
-
- vpRowVector p2Vec(3);
- p2Vec[0] = p2.get_X(); p2Vec[1] = p2.get_Y(); p2Vec[2] = p2.get_Z();
- p2Vec = p2Vec.normalize();
-
- if((clippingFlag & flag) == flag){
- double beta1 = acos( p1Vec * normal );
- double beta2 = acos( p2Vec * normal );
-
-// std::cout << beta1 << " && " << beta2 << std::endl;
-
- // if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
- if(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
- return false;
- else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0){
- vpPoint pClipped;
- double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
- t = t / ( normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) + normal[2] * (p2.get_Z() - p1.get_Z()) );
-
- pClipped.set_X((p2.get_X() - p1.get_X())*t + p1.get_X());
- pClipped.set_Y((p2.get_Y() - p1.get_Y())*t + p1.get_Y());
- pClipped.set_Z((p2.get_Z() - p1.get_Z())*t + p1.get_Z());
-
- if(beta1 < M_PI / 2.0){
- p1ClippedInfo = p1ClippedInfo | flag;
- p1Clipped = pClipped;
- }
- else{
- p2ClippedInfo = p2ClippedInfo | flag;
- p2Clipped = pClipped;
- }
- }
- }
-
- return true;
-}
-
-bool
-vpMbtPolygon::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const unsigned int &flag, const double &distance)
-{
- // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
- // to avoid a valgrind "Source and destination overlap in memcpy" error,
- // we introduce a two temporary points.
- vpPoint p1Clipped_, p2Clipped_;
- p1Clipped_ = p1;
- p2Clipped_ = p2;
-
- p1Clipped = p1Clipped_;
- p2Clipped = p2Clipped_;
-
-
- bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
- if(flag == vpMbtPolygon::FAR_CLIPPING)
- test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
-
- bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
- if(flag == vpMbtPolygon::FAR_CLIPPING)
- test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
-
- bool test3 = (p1Clipped.get_Z() < distance);
- if(flag == vpMbtPolygon::FAR_CLIPPING)
- test3 = (p1Clipped.get_Z() > distance);
-
- if(test1)
- return false;
-
- else if(test2){
- vpPoint pClippedNear;
- double t;
- t = (p2Clipped.get_Z() - p1Clipped.get_Z());
- t = (distance - p1Clipped.get_Z()) / t;
-
- pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X())*t + p1Clipped.get_X());
- pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y())*t + p1Clipped.get_Y());
- pClippedNear.set_Z(distance);
-
- if(test3){
- p1Clipped = pClippedNear;
- if(flag == vpMbtPolygon::FAR_CLIPPING)
- p1ClippedInfo = p1ClippedInfo | vpMbtPolygon::FAR_CLIPPING;
- else
- p1ClippedInfo = p1ClippedInfo | vpMbtPolygon::NEAR_CLIPPING;
- }
- else{
- p2Clipped = pClippedNear;
- if(flag == vpMbtPolygon::FAR_CLIPPING)
- p2ClippedInfo = p2ClippedInfo | vpMbtPolygon::FAR_CLIPPING;
- else
- p2ClippedInfo = p2ClippedInfo | vpMbtPolygon::NEAR_CLIPPING;
- }
- }
-
- return true;
-}
-
-/*!
- Get the region of interest in the image.
-
- \warning Suppose that changeFrame() has already been called.
-
- \param cam : camera parameters.
-
- \return Image point corresponding to the region of interest.
-*/
-std::vector<vpImagePoint>
-vpMbtPolygon::getRoi(const vpCameraParameters &cam)
-{
- std::vector<vpImagePoint> roi;
- for (unsigned int i = 0; i < nbpt; i ++){
- vpImagePoint ip;
- vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
- roi.push_back(ip);
- }
-
- return roi;
-}
-
-/*!
- Get the region of interest in the image.
-
- \param cam : camera parameters.
- \param cMo : pose.
-
- \return Image point corresponding to the region of interest.
-*/
-std::vector<vpImagePoint>
-vpMbtPolygon::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo);
- return getRoi(cam);
-}
-
-/*!
- Get the 3D points of the clipped region of interest.
-
- \warning Suppose that changeFrame() and computeRoiClipped() have already been called.
-
- \param points : resulting points.
-*/
-void
-vpMbtPolygon::getRoiClipped(std::vector<vpPoint> &points)
-{
- for(unsigned int i = 0 ; i < roiPointsClip.size() ; i++){
- points.push_back(roiPointsClip[i].first);
- }
-}
-
-/*!
- Get the region of interest clipped in the image.
-
- \warning Suppose that changeFrame() and computeRoiClipped() have already been called.
-
- \param cam : camera parameters.
- \param roi : image point corresponding to the region of interest.
-*/
-void
-vpMbtPolygon::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
-{
- for(unsigned int i = 0 ; i < roiPointsClip.size() ; i++){
- vpImagePoint ip;
- vpMeterPixelConversion::convertPoint(cam,roiPointsClip[i].first.get_x(),roiPointsClip[i].first.get_y(),ip);
-// std::cout << "## " << ip.get_j() << " - " << ip.get_i() << std::endl;
- roi.push_back(ip);
- }
-}
-
-/*!
- Get the region of interest clipped in the image.
-
- \param cam : camera parameters.
- \param cMo : pose.
- \param roi : image point corresponding to the region of interest.
-*/
-void
-vpMbtPolygon::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi, const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo);
- computeRoiClipped(cam);
- getRoiClipped(cam, roi);
-}
-
-/*!
- Get the region of interest clipped in the image and the information to know if it's a clipped point.
-
- \warning Suppose that changeFrame() and computeRoiClipped() have already been called.
-
- \param cam : camera parameters.
- \param roi : image point corresponding to the region of interest with clipping information.
-*/
-void
-vpMbtPolygon::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi)
-{
- for(unsigned int i = 0 ; i < roiPointsClip.size() ; i++){
- vpImagePoint ip;
- roiPointsClip[i].first.projection();
- vpMeterPixelConversion::convertPoint(cam,roiPointsClip[i].first.get_x(),roiPointsClip[i].first.get_y(),ip);
- roi.push_back(std::make_pair(ip, roiPointsClip[i].second));
- }
-}
-
-/*!
- Get the region of interest clipped in the image and the information to know if it's a clipped point.
-
- \param cam : camera parameters.
- \param roi : image point corresponding to the region of interest with clipping information.
- \param cMo : pose.
-*/
-void
-vpMbtPolygon::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo);
- computeRoiClipped(cam);
- getRoiClipped(cam, roi);
-}
-
-
-
-/*!
- Static method to check the number of points of a region defined by the vector of image point that are inside the image.
-
- \param I : The image used for its size.
- \param cam : The camera parameters.
-*/
-unsigned int
-vpMbtPolygon::getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam)
-{
- unsigned int nbPolyIn = 0;
- for (unsigned int i = 0; i < nbpt; i ++){
- if(p[i].get_Z() > 0){
- vpImagePoint ip;
- vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
- if((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
- nbPolyIn++;
- }
- }
-
- nbCornersInsidePrev = nbPolyIn;
-
- return nbPolyIn;
-}
-
-//###################################
-// Static functions
-//###################################
-
-/*!
- Static method to compute the clipped points from a set of initial points.
-
- \warning When using FOV clipping and personnal camera parameters, camera normals have to be computed before (see vpCameraParameters::computeFov())
-
- \param ptIn : Input points
- \param ptOut : Output points (result of the clipping).
- \param cMo : Pose considered for the clipping.
- \param clippingFlags: Clipping flag (see vpMbtPolygon::vpMbtPolygonClippingType).
- \param cam : Camera parameters (Only used if clipping flags contain FOV clipping).
- \param znear : Near clipping distance value (Only used if clipping flags contain Near clipping).
- \param zfar : Far clipping distance value (Only used if clipping flags contain Far clipping).
-*/
-void
-vpMbtPolygon::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
- const vpCameraParameters &cam, const double &znear, const double &zfar)
-{
- ptOut.clear();
- vpMbtPolygon poly;
- poly.setNbPoint((unsigned int)ptIn.size());
- poly.setClipping(clippingFlags);
-
- if((clippingFlags & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
- poly.setNearClippingDistance(znear);
-
- if((clippingFlags & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
- poly.setFarClippingDistance(zfar);
-
- for(unsigned int i = 0; i < ptIn.size(); i++)
- poly.addPoint(i,ptIn[i]);
-
- poly.changeFrame(cMo);
- poly.computeRoiClipped(cam);
- poly.getRoiClipped(ptOut);
-}
-
-void
-vpMbtPolygon::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int & i_min, int &i_max, int &j_min, int &j_max)
-{
- // i_min_d = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- double i_min_d = (double) INT_MAX;
- double i_max_d = 0;
- double j_min_d = (double) INT_MAX;
- double j_max_d = 0;
-
- for (unsigned int i = 0; i < iroi.size(); i += 1){
- if(i_min_d > iroi[i].get_i())
- i_min_d = iroi[i].get_i();
-
- if(iroi[i].get_i() < 0)
- i_min_d = 1;
-
- if((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
- i_max_d = iroi[i].get_i();
-
- if(j_min_d > iroi[i].get_j())
- j_min_d = iroi[i].get_j();
-
- if(iroi[i].get_j() < 0)
- j_min_d = 1;//border
-
- if((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
- j_max_d = iroi[i].get_j();
- }
- i_min = static_cast<int> (i_min_d);
- i_max = static_cast<int> (i_max_d);
- j_min = static_cast<int> (j_min_d);
- j_max = static_cast<int> (j_max_d);
-}
-
-/*!
- Static method to check whether the region defined by the vector of image point
- is contained entirely in the image.
-
- \param I : The image used for its size.
- \param corners : The vector of points defining a region
-*/
-bool
-vpMbtPolygon::roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners)
-{
- double nbPolyIn = 0;
- for(unsigned int i=0; i<corners.size(); ++i){
- if((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) &&
- (corners[i].get_i() < I.getHeight()) && (corners[i].get_j() < I.getWidth())){
- nbPolyIn++;
- }
- }
-
- if(nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
- return false;
-
- return true;
-}
-
-/*!
- Set the flag to consider if the level of detail (LOD) is used or not.
- When activated, lines and faces of the 3D model are tracked if respectively their
- projected lenght and area in the image are significative enough. By significative, we mean:
- - if the lenght of the projected line in the image is greater that a threshold set by
- setMinLineLengthThresh()
- - if the area of the projected face in the image is greater that a threshold set by
- setMinPolygonAreaThresh().
-
- \param use_lod : true if level of detail must be used, false otherwise.
-
- The sample code below shows how to introduce this feature:
- \code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImageIo.h>
-
-int main()
-{
-vpImage<unsigned char> I;
-
-// Acquire an image
-vpImageIo::read(I, "my-image.pgm");
-
-std::string object = "my-object";
-vpMbEdgeTracker tracker;
-tracker.loadConfigFile( object+".xml" );
-tracker.loadModel( object+".cao" );
-
-tracker.setLod(true);
-tracker.setMinLineLengthThresh(20.);
-tracker.setMinPolygonAreaThresh(20.*20.);
-
-tracker.initClick(I, object+".init" );
-
-while (true) {
- // tracking loop
-}
-vpXmlParser::cleanup();
-
-return 0;
-}
- \endcode
-
- \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
- */
-void
-vpMbtPolygon::setLod(const bool use_lod)
-{
- this->useLod = use_lod;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated This method is deprecated since it is no more used since ViSP 2.7.2. \n
-
- Check if the polygon is visible in the image. To do that, the polygon is projected into the image thanks to the camera pose.
-
- \param cMo : The pose of the camera.
- \param depthTest : True if a face has to be entirely visible (in front of the camera). False if it can be partially visible.
-
- \return Return true if the polygon is visible.
-*/
-bool
-vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const bool &depthTest)
-{
- changeFrame(cMo) ;
-
- if(depthTest)
- for (unsigned int i = 0 ; i < nbpt ; i++){
- if(p[i].get_Z() < 0){
- isappearing = false;
- isvisible = false ;
- return false ;
- }
- }
-
- return isVisible(cMo, vpMath::rad(89));
-}
-#endif
-
-
diff --git a/src/tracking/mbt/vpMbtPolygon.h b/src/tracking/mbt/vpMbtPolygon.h
deleted file mode 100644
index 3fde8b7..0000000
--- a/src/tracking/mbt/vpMbtPolygon.h
+++ /dev/null
@@ -1,299 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMbtPolygon.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implements a polygon of the model used by the model-based tracker.
- *
- * Authors:
- * Nicolas Melchior
- * Romain Tallonneau
- * Eric Marchand
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMbtPolygon.h
- \brief Implements a polygon of the model used by the model-based tracker.
-*/
-
-#ifndef vpMbtPolygon_HH
-#define vpMbtPolygon_HH
-
-#include <visp/vpPoint.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-
-#include <vector>
-
-/*!
- \class vpMbtPolygon
-
- \brief Implementation of a polygon of the model used by the model-based tracker.
-
- \ingroup ModelBasedTracking
-
- */
-class VISP_EXPORT vpMbtPolygon
-{
-public:
- typedef enum
- {
- NO_CLIPPING = 0,
- NEAR_CLIPPING = 1,
- FAR_CLIPPING = 2,
- LEFT_CLIPPING = 4,
- RIGHT_CLIPPING = 8,
- UP_CLIPPING = 16,
- DOWN_CLIPPING = 32,
- FOV_CLIPPING = 60,
- ALL_CLIPPING = 63
- } vpMbtPolygonClippingType;
-
-public:
- //! Index of the polygon. Cannot be unsigned int because default value is -1.
- int index;
- //! Number of points used to define the polygon.
- unsigned int nbpt;
- //! Number of corners inside the image during the last call to getNbCornerInsideImage
- unsigned int nbCornersInsidePrev;
- //! flag to specify whether the face is visible or not
- bool isvisible;
- //! flag to specify whether the face is appearing or not
- bool isappearing;
- //! corners in the object frame
- vpPoint *p;
- //! Region of interest clipped
- std::vector<std::pair<vpPoint,unsigned int> > roiPointsClip;
- //! Clipping flag
- unsigned int clippingFlag;
- //! Distance for near clipping
- double distNearClip;
- //! Distance for near clipping
- double distFarClip;
- //! Flag to specify if the visibility of the polygon depends also of the current level of detail (LOD)
- bool useLod;
- //! Threshold for minimum line length in pixel to consider if the line is visible or not in LOD case
- double minLineLengthThresh;
- //! Threshold for minimum polygon area in pixel to consider if the polygon is visible or not in LOD case
- double minPolygonAreaThresh;
- //! Name of the polygon
- std::string name;
-
-private:
- bool getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const vpColVector &normal,
- const unsigned int &flag);
-
- bool getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const unsigned int &flag, const double &distance);
-
-public:
- vpMbtPolygon() ;
- vpMbtPolygon(const vpMbtPolygon& mbtp) ;
- virtual ~vpMbtPolygon() ;
-
- void addPoint(const unsigned int n, const vpPoint &P) ;
-
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
- void computeRoiClipped(const vpCameraParameters &cam = vpCameraParameters());
-
- /*!
- Get the clipping used.
-
- \sa vpMbtPolygonClipping
-
- \return Clipping flags.
- */
- inline unsigned int getClipping() const { return clippingFlag; }
-
- /*!
- Get the far distance for clipping.
-
- \return Far clipping value.
- */
- inline double getFarClippingDistance() const { return distFarClip; }
-
- /*!
- Get the index of the face.
-
- \return index : the index of the face.
- */
- inline int getIndex() const {return index ;}
-
- /*!
- Get the name of the face.
-
- \return Name of the face.
- */
- inline std::string getName() const {return name;}
-
- /*!
- Return the number of corners.
-
- \return number of corner of the face
- */
- inline unsigned int getNbPoint() const {return nbpt ;}
-
- /*!
- Return the number of corners at the previous computation.
-
- \return number of corner of the face at the previous computation
- */
- inline unsigned int getNbCornerInsidePrevImage() const { return nbCornersInsidePrev; }
-
- unsigned int getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam);
-
- /*!
- Get the near distance for clipping.
-
- \return Near clipping value.
- */
- inline double getNearClippingDistance() const { return distNearClip; }
-
- vpPoint & getPoint(const unsigned int _index);
-
- std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam);
-
- std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo);
-
- void getRoiClipped(std::vector<vpPoint> &points);
-
- void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi);
-
- void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi, const vpHomogeneousMatrix &cMo);
-
- void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi);
-
- void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo);
-
- inline bool isAppearing() const {return isappearing;}
- virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo = false,
- const vpCameraParameters &cam = vpCameraParameters(), const vpImage<unsigned char> &I = vpImage<unsigned char>());
- bool isVisible() const {return isvisible;}
-
- vpMbtPolygon& operator=(const vpMbtPolygon& mbtp) ;
-
- /*!
- Specify which clipping to use.
-
- \sa vpMbtPolygonClipping
-
- \param flags : New clipping flags.
- */
- inline void setClipping(const unsigned int &flags) { clippingFlag = flags; }
-
- /*!
- Set the far distance for clipping.
-
- \param dist : Far clipping value.
- */
- inline void setFarClippingDistance(const double &dist) { distFarClip = dist; clippingFlag = (clippingFlag | vpMbtPolygon::FAR_CLIPPING);}
-
- /*!
- Set the index of the face.
-
- \param i : the new index of the face.
- */
- virtual inline void setIndex(const int i ) { index = i ; }
-
- // Due to a doxygen warning include the sample code in the doc, we remove the inline and put the doc in the *.cpp file
- void setLod(const bool use_lod);
- /*!
- Set the threshold for the minimum line length to be considered as visible in the LOD
- (level of detail) case. This threshold is only used when setLoD() is turned on.
-
- \param min_line_length : threshold for the minimum line length in pixel. When a single line that doesn't
- belong to a face is considered by the tracker, this line is tracked only if its lenght in pixel is
- greater than \e min_line_length.
-
- \sa setLoD()
- */
- inline void setMinLineLengthThresh(const double min_line_length) {
- this->minLineLengthThresh = min_line_length;
- }
- /*!
- Set the minimum polygon area to be considered as visible in the LOD (level of detail)
- case. This threshold is only used when setLoD() is turned on.
-
- \param min_polygon_area : threshold for the minimum polygon area in pixel. When a face
- is considered by the tracker, this face is tracked only if its area in pixel is
- greater than \e min_polygon_area.
-
- \sa setLoD()
- */
- inline void setMinPolygonAreaThresh(const double min_polygon_area) {
- this->minPolygonAreaThresh = min_polygon_area;
- }
-
- /*!
- Set the name of the face.
-
- \param face_name : name of the face.
- */
- inline void setName(const std::string &face_name) {
- this->name = face_name;
- }
-
- virtual void setNbPoint(const unsigned int nb) ;
-
- /*!
- Set the near distance for clipping.
-
- \param dist : Near clipping value.
- */
- inline void setNearClippingDistance(const double &dist) { distNearClip = dist; clippingFlag = (clippingFlag | vpMbtPolygon::NEAR_CLIPPING);}
-
-public:
- static void getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo,
- const unsigned int &clippingFlags, const vpCameraParameters &cam = vpCameraParameters(),
- const double &znear = 0.001, const double &zfar = 100 );
- static void getMinMaxRoi(const std::vector<vpImagePoint> &roi, int & i_min, int &i_max, int &j_min, int &j_max);
- static bool roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners);
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
- /*!
- @name Deprecated functions
- */
- bool isVisible(const vpHomogeneousMatrix &cMo, const bool &depthTest = false) ;
-#endif
-};
-
-#endif
-
diff --git a/src/tracking/moments/vpMoment.cpp b/src/tracking/moments/vpMoment.cpp
deleted file mode 100644
index df576f6..0000000
--- a/src/tracking/moments/vpMoment.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMoment.cpp 4709 2014-03-28 17:37:12Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base for 2D moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpMoment.cpp
- \brief Base class for all 2D moments.
-*/
-
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMoment.h>
-#include <visp/vpMomentDatabase.h>
-#include <cstring>
-/*!
- Default constructor
-*/
-vpMoment::vpMoment(): object(NULL), moments(NULL), values() {}
-
-
-/*!
- Links the moment to a database of moment primitives.
- If the moment depends on other moments, these moments must be linked to the same database.
- \attention Two moments of the same class cannot be stored in the same database
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCentered.h>
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p;
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
-
- vpMomentObject obj(2);
- obj.setType(vpMomentObject::DISCRETE); // Discrete mode.
- obj.fromVector(vec_p); // Init the dense object with the polygon
-
- vpMomentDatabase db;
- vpMomentGravityCenter G; // declaration of gravity center
- vpMomentCentered mc; // mc containts centered moments
-
- G.linkTo(db); //add gravity center to database
- mc.linkTo(db); //centered moments depend on gravity, add them to the
- //database to grant access
-
- G.update(obj); // specify the object for gravity center
- mc.update(obj); // and for centered moments
-
- G.compute(); // compute the moment
- mc.compute(); //compute centered moments AFTER gravity center
-
- return 0;
-}
-
- \endcode
-
- \param data_base : database of moment primitives.
-*/
-void vpMoment::linkTo(vpMomentDatabase& data_base){
- if (strlen( name() ) >= 255) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the moment name"));
- }
-
- std::strcpy(_name,name());
- this->moments=&data_base;
-
- data_base.add(*this,_name);
-}
-
-
-/*!
- Updates the moment with the current object. This does not compute any values.
- \param moment_object : object descriptor of the current camera vision.
-*/
-void vpMoment::update(vpMomentObject& moment_object){
- this->object=&moment_object;
-}
-
-/*!
- Prints the moment contents to a stream
- \param os : a std::stream.
- \param m : a moment instance.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMoment& m){
- for(std::vector<double>::const_iterator i = m.values.begin();i!=m.values.end();i++)
- os << *i << ",";
-
- return os;
-}
-
-/*!
-Prints values of all dependent moments required to calculate a specific vpMoment.
-Not made pure to maintain compatibility
-Recommended : Types inheriting from vpMoment should implement this function
-*/
-void vpMoment::printDependencies(std::ostream& os) const{
- os << " WARNING : Falling back to base class version of printDependencies(). To prevent that, this has to be implemented in the derived classes!" << std::endl;
-}
diff --git a/src/tracking/moments/vpMoment.h b/src/tracking/moments/vpMoment.h
deleted file mode 100644
index 60bcf6e..0000000
--- a/src/tracking/moments/vpMoment.h
+++ /dev/null
@@ -1,140 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMoment.h 4708 2014-03-28 17:36:46Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base for 2D moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpMoment.h
- \brief Base class for all 2D moments.
-*/
-
-#ifndef __MOMENT_H__
-#define __MOMENT_H__
-
-#include <visp/vpConfig.h>
-
-#include <vector>
-#include <iostream>
-
-class vpMomentDatabase;
-class vpMomentObject;
-
-
-/*!
- \class vpMoment
-
- \ingroup TrackingMoments
-
- \brief This class defines shared methods/attributes for 2D moments.
-
- All moments or combination of moments in the moments module are based on this class.
- A moment uses a vpMomentObject object to access all useful information.
- Moment values are obtained by a 4-step process common for all moment types:
- - Declaration.
- \code
- vpMoment moment;
- \endcode
- - Update with object.
- \code
- moment.update(object);
- \endcode
- - Compute the moment value
- \code
- moment.compute();
- \endcode
- - Access the values:
- \code
- std::vector<double> values = moment.get();
- \endcode
-
- A moment may also be linked to a vpMomentDatabase. Moments linked to a database are able to access each others values.
- Some moments can be computed only if they are linked to a a database containing their dependencies.
- Linking to a database is done using the vpMoment::linkTo(...) method.
-
- There are no constraints about format of the array returned by vpMoment::get(); any implementation is fine.
-
- Each moment must have a string name by implementing the char* vpMoment::name() method which allows to identify the moment in the database.
- Each moment must also implement a compute method describing how to obtain its values from the object.
-
- \attention Order of moment computation DOES matter: when you compute a moment using vpMoment::compute(),
- all moment dependencies must be computed.
- We recall that implemented moments are:
- - vpMomentAlpha
- - vpMomentArea
- - vpMomentAreaNormalized
- - vpMomentBasic
- - vpMomentCentered
- - vpMomentCInvariant
- - vpMomentGravityCenter
- - vpMomentGravityCenterNormalized
-
-*/
-class VISP_EXPORT vpMoment{
- private:
- vpMomentObject* object;
- vpMomentDatabase* moments;
- char _name[255];
- protected:
- std::vector<double> values;
- /*!
- Returns the linked moment database.
- \return the moment database
- */
- inline vpMomentDatabase& getMoments() const { return *moments; }
- public:
- inline const vpMomentObject& getObject() const { return *object;}
- vpMoment();
- /*!
- Returns all values computed by the moment.
- \return vector of values
- */
- const std::vector<double>& get() const { return values;}
- void linkTo(vpMomentDatabase& moments);
- void update(vpMomentObject& object);
- virtual void compute()=0;
- virtual const char* name() const = 0;
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMoment& m);
- virtual void printDependencies(std::ostream& os) const;
-
- /*!
- Virtual destructor.
- */
- virtual ~vpMoment() {}
-};
-#endif
diff --git a/src/tracking/moments/vpMomentAlpha.cpp b/src/tracking/moments/vpMomentAlpha.cpp
deleted file mode 100644
index 8a180db..0000000
--- a/src/tracking/moments/vpMomentAlpha.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentAlpha.cpp 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Alpha moment descriptor for in-plane orientation.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentAlpha.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentCentered.h>
-#include <cmath>
-
-/*!
- Empty constructor. Initializes alpha moment as a reference alpha. A default-constructed alpha moment may be used as a reference for other alphas.
- A reference alpha is class harbouring an alpha value computed for a \f$[-\pi/2..\pi/2]\f$ portion of the circle. Not setting a reference alpha will prevent you
- from doing more than 180deg rotation with moments.
- */
-vpMomentAlpha::vpMomentAlpha() : isRef(true), symmetric(false), ref(), alphaRef(0.) {
- values.resize(1);
-}
-
-/*!
- Common constructor. Initializes alpha moment as a non-reference alpha. A default-constructed alpha moment must be used as a reference for other this alpha.
- A reference alpha is class harbouring an alpha value computed for a \f$[0..\pi]\f$ portion of the circle. Not having a reference alpha will prevent you
- from doing more than 180deg rotation with moments.
- \param ref_ : vector of 3rd order centered moments corresponding to the reference alpha in the following order: \f$\mu_{03},\mu_{12},\mu_{21},\mu_{30}\f$.
- \param alpha_ref : value of the reference alpha.
-*/
-vpMomentAlpha::vpMomentAlpha(std::vector<double>& ref_, double alpha_ref)
- : vpMoment(),isRef(false),symmetric(false),ref(ref_),alphaRef(alpha_ref)
-{
- for (std::vector<double>::iterator it = ref_.begin(); it!=ref_.end(); it++)
- if (*it<=1e-4)
- symmetric = true;
-
- values.resize(1);
-}
-
-/*!
- Compute the value of the alpha-moment.
- Depends on vpMomentCentered.
- */
-void vpMomentAlpha::compute(){
- //symmetric = symmetric | this->getObject().isSymmetric();
- bool found_moment_centered;
-
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
- found_moment_centered)));
-
- if (!found_moment_centered)
- throw vpException(vpException::notInitialized, "vpMomentCentered not found");
-
- double alpha = 0.5 * atan2(2.0 * momentCentered.get(1, 1), (momentCentered.get(2, 0) - momentCentered.get(0, 2)));
-
- unsigned int order = 4;
- std::vector<double> rotMu(4);
- std::vector<double> realMu(4);
-
- if (isRef)
- {
- alphaRef = alpha;
- }
- else
- {
- if (!symmetric)
- {
- double r11 = cos(alpha - alphaRef);
- double r12 = sin(alpha - alphaRef);
- double r21 = -sin(alpha - alphaRef);
- double r22 = cos(alpha - alphaRef);
- unsigned int idx = 0;
- for (register unsigned int c = 0; c < (order) * (order); c++)
- {
- unsigned int i = c % order;
- unsigned int j = c / order;
-
- if (i + j == 3)
- {
- double r11_k = 1.;
- for (register unsigned int k = 0; k <= i; k++)
- {
- double r12_i_k = pow(r12, (int)(i - k));
- double comb_i_k = static_cast<double> (vpMath::comb(i, k));
- for (register unsigned int l = 0; l <= j; l++)
- {
- rotMu[idx] += static_cast<double> (comb_i_k * vpMath::comb(j, l) * r11_k * pow(r21, (int)l) * r12_i_k
- * pow(r22, (int)(j - l)) * momentCentered.get(k + l, (unsigned int)(int)(i + j - k - l)));
- }
- r11_k *= r11;
- }
- realMu[idx] = momentCentered.get(i, j);
- idx++;
- }
- }
-
- double sum = 0.;
- bool signChange = true;
- for (register unsigned int i = 0; i < 4; i++)
- {
- if (std::abs(rotMu[i]) > 1e10 * std::numeric_limits<double>::epsilon() && std::abs(ref[i]) > 1e10
- * std::numeric_limits<double>::epsilon() && rotMu[i] * ref[i] > 0)
- signChange = false;
- sum += std::abs(rotMu[i] * ref[i]);
- }
-
- if (sum < 1e4 * std::numeric_limits<double>::epsilon())
- signChange = false;
- if (signChange)
- alpha = alpha + M_PI;
- }
- }
- values[0] = alpha;
-}
-
-/*!
- Prints the value of the major-axis orientation in degrees and rad
- */
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& c){
- os << (__FILE__) << std::endl;
- os << "Alpha = " << c.values[0] << "rad = " << vpMath::deg(c.values[0]) << "deg " << std::endl;
- return os;
-}
-
-/*!
-Prints the dependencies of alpha, namely centered moments mu11, mu20 ad mu02
-*/
-void vpMomentAlpha::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
- found_moment_centered)));
- if (!found_moment_centered)
- throw vpException(vpException::notInitialized, "vpMomentCentered not found");
-
- os << "mu11 = " << momentCentered.get(1, 1) << "\t";
- os << "mu20 = " << momentCentered.get(2, 0) << "\t";
- os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
-}
diff --git a/src/tracking/moments/vpMomentAlpha.h b/src/tracking/moments/vpMomentAlpha.h
deleted file mode 100644
index 4fcf356..0000000
--- a/src/tracking/moments/vpMomentAlpha.h
+++ /dev/null
@@ -1,239 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentAlpha.h 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Alpha moment descriptor for in-plane orientation.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpMomentAlpha.h
- \brief Alpha moment descriptor for in-plane orientation.
-*/
-
-#ifndef __MOMENTALPHA_H__
-#define __MOMENTALPHA_H__
-
-#include <visp/vpMoment.h>
-
-/*!
- \class vpMomentAlpha
-
- \ingroup TrackingMoments
-
- \brief This class defines the orientation of the object inside the plane parallel to the object.
-
- The value of the moment is computed in radians in a \f$ [-\pi/2 .. \pi/2] \f$ interval at most by the formula \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$.
-
- To obtain the \f$ [-\pi .. \pi] \f$ precision, you have to specify reference information. This reference information
- describes an object with 0 rad orientation or at least an orientation between \f$ -\pi/2 \f$ and \f$ \pi/2 \f$ rad.
- The nature of this information are third-order centered moments and a reference value of alpha.
- Reference centered moments are obtained with vpMomentCentered and reference alpha is obtained with vpMomentAlpha using the reference constructor.
-
- Therefore there are two modes for vpMomentAlpha and one constructor per mode:
- - Reference mode (using the empty constructor):
- The vpMomentAlpha doesn't need any additionnal information, it will compute its values from a reference object.
- - Relative mode (using non-empty constructor):
- The vpMomentAlpha is computed relative a reference alpha.
- By knowing the reference, it may distinguish in-plane rotations of \f$ \alpha \f$ from rotations of \f$ \alpha + \pi \f$.
-
- The following code demonstrates a calculation of a reference alpha and then uses this alpha to estimate the orientation
- of the same object after performing a 180 degrees rotation.
- Therefore the first and second alpha should have opposite values.
-
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentAlpha.h>
-#include <iostream>
-#include <vector>
-#include <algorithm>
-
-//generic function for printing
-void print (double i) { std::cout << i << "\t";}
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-3); p.set_y(-1); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
-
- //////////////////////////////REFERENCE VALUES////////////////////////////////
- vpMomentObject objRef(3); // Reference object. Must be of order 3
- // because we will need the 3rd order
- // centered moments
- objRef.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
- objRef.fromVector(vec_p); // Init the dense object with the polygon
-
- vpMomentDatabase dbRef; //reference database
- vpMomentGravityCenter gRef; // declaration of gravity center
- vpMomentCentered mcRef; // centered moments
- vpMomentAlpha alphaRef; //declare alpha as reference
-
- gRef.linkTo(dbRef); //add gravity center to database
- mcRef.linkTo(dbRef); //add centered moments
- alphaRef.linkTo(dbRef); //add alpha depending on centered moments
-
- dbRef.updateAll(objRef); // All of the moments must be updated, not just alpha
-
- gRef.compute(); // compute the moment
- mcRef.compute(); //compute centered moments AFTER gravity center
- alphaRef.compute(); //compute alpha AFTER centered moments.
-
- //the order of values in the vector must be as follows:
- //mu30 mu21 mu12 mu03
- std::vector<double> mu3ref(4);
- mu3ref[0] = mcRef.get(3,0);
- mu3ref[1] = mcRef.get(2,1);
- mu3ref[2] = mcRef.get(1,2);
- mu3ref[3] = mcRef.get(0,3);
-
-
- std::cout << "--- Reference object ---" << std::endl;
- std::cout << "alphaRef=" << alphaRef << std::endl << "mu3="; // print reference alpha
- std::for_each (mu3ref.begin(), mu3ref.end(),print);
- std::cout << std::endl;
-
- ////////////CURRENT VALUES (same object rotated 180deg - must be
- ////////////entered in reverse order)////////////////
- vec_p.clear();
-
- p.set_x(-3); p.set_y(1); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
- p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(-2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(1); p.set_y(-1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
-
-
- vpMomentObject obj(3); // second object. Order 3 is also required
- // because of the Alpha will compare
- // third-order centered moments to given reference.
-
- obj.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
- obj.fromVector(vec_p); // Init the dense object with the polygon
-
- vpMomentDatabase db; // database
- vpMomentGravityCenter g; // declaration of gravity center
- vpMomentCentered mc; // mc containts centered moments
- vpMomentAlpha alpha(mu3ref,alphaRef.get()); //declare alpha as relative to a reference
-
- g.linkTo(db); //add gravity center to database
- mc.linkTo(db); //add centered moments
- alpha.linkTo(db); //add alpha depending on centered moments
-
- db.updateAll(obj); // All of the moments must be updated
-
- g.compute(); // compute the moment
- mc.compute(); //compute centered moments AFTER gravity center
- alpha.compute(); //compute alpha AFTER centered moments.
-
- std::cout << "--- current object ---" << std::endl;
- std::cout << "alpha=" << alpha.get() << std::endl;
-
- return 0;
-}
-
- \endcode
-This program outputs:
-\code
---- Reference object ---
-alphaRef=0.441601
-mu3=1.80552 0.921882 0.385828 0.122449
---- current object ---
-alpha=-0.441601
-
-\endcode
-
-Shortcuts for quickly getting those references exist in vpMomentCommon.
-
-This moment depends on vpMomentCentered.
-*/
-class VISP_EXPORT vpMomentAlpha : public vpMoment {
- private:
- bool isRef;
- bool symmetric;
- std::vector<double> ref;
- double alphaRef;
- public:
-
- vpMomentAlpha();
- vpMomentAlpha(std::vector<double>& ref,double alphaRef);
-
- void compute();
- /*!
- Retrieve the orientation of the object as a single double value.
- */
- double get() const { return values[0]; }
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentAlpha";}
-
- inline bool is_ref() const
- {
- if (isRef)
- return true;
- else
- return false;
- }
-
- inline bool is_symmetric() const
- {
- if (symmetric)
- return true;
- else
- return false;
- }
-
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& v);
- void printDependencies(std::ostream& os) const;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentArea.cpp b/src/tracking/moments/vpMomentArea.cpp
deleted file mode 100644
index 371362d..0000000
--- a/src/tracking/moments/vpMomentArea.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentArea.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D area of the object
- *
- * Authors:
- * Manikandan Bakthavatchalam
- *
- *****************************************************************************/
-#include <visp/vpMomentArea.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentDatabase.h>
-#include <cmath>
-
-/*!
- Has the area \f$ a = m_{00} = \mu_{00} \f$.
- Gets the value of m00 from vpMomentCentered.
-*/
-void vpMomentArea::compute(){
- /* getObject() returns a reference to a vpMomentObject. This is public member of vpMoment */
- if(getObject().getType()==vpMomentObject::DISCRETE) {
- bool found_moment_centered;
- /* getMoments() returns a reference to a vpMomentDatabase. It is a protected member of and is 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<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- values[0] = momentCentered.get(2,0) + momentCentered.get(0,2);
- }
- else {
- values[0] = getObject().get(0,0);
- }
-}
-
-/*!
- Default constructor.
-*/
-vpMomentArea::vpMomentArea() : vpMoment(){
- values.resize(1);
-}
-
-/*!
- Outputs the moment's values to a stream.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentArea& m){
- os << (__FILE__) << std::endl;
- os << "a(m00) = " << m.values[0] << std::endl;
- return os;
-}
-
-/*!
-If the vpMomentObject type is
-1. DISCRETE(set of discrete points), uses mu20+mu02
-2. DENSE_FULL_OBJECT(from image) used mu00
-*/
-void vpMomentArea::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
-
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- if(getObject().getType()==vpMomentObject::DISCRETE)
- {
- os << "mu20 = " << momentCentered.get(2, 0) << "\t";
- os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
- }
- else
- {
- os << "mu00 = " << momentCentered.get(0, 0) << std::endl;
- }
-}
diff --git a/src/tracking/moments/vpMomentArea.h b/src/tracking/moments/vpMomentArea.h
deleted file mode 100644
index 70500c4..0000000
--- a/src/tracking/moments/vpMomentArea.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentArea.h 3530 2012-01-03 10:52:12Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Just the area m00 = mu00
- *
- * Authors:
- * Manikandan Bakthavatchalam
- *
- *****************************************************************************/
-#ifndef __MOMENTAREA_H__
-#define __MOMENTAREA_H__
-
-#include <visp/vpMoment.h>
-
-class vpMomentObject;
-class vpMomentCentered; // Required for discrete case of vpMomentObject
-
-/*!
- \class vpMomentArea
-
- \ingroup TrackingMoments
-
- \brief Class handling the surface moment.
-
-*/
-class VISP_EXPORT vpMomentArea : public vpMoment {
- public:
- vpMomentArea();
- void compute();
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentArea";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentArea& m);
- void printDependencies(std::ostream& os) const;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentAreaNormalized.cpp b/src/tracking/moments/vpMomentAreaNormalized.cpp
deleted file mode 100644
index 4b2865b..0000000
--- a/src/tracking/moments/vpMomentAreaNormalized.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentAreaNormalized.cpp 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D normalized surface moment descriptor (usually described as An)
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentDatabase.h>
-#include <cmath>
-
-
-/*!
- Computes the normalized area \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$.
- Depends on vpMomentCentered.
-*/
-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*/
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- double a;
- /* getObject() returns a reference to the vpMomentObject from which
- * the moment values are calculated. (public member of vpMoment)*/
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
-
- values[0] = desiredDepth*sqrt(desiredSurface/a);
-}
-
-/*!
- Default constructor.
- \param desired_surface : desired area \e a* when the visual servoing converges.
- \param desired_depth : desired depth \e Z* when the visual servoing converges.
-*/
-vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, double desired_depth)
- : vpMoment(),desiredSurface(desired_surface),desiredDepth(desired_depth)
-{
- values.resize(1);
-}
-
-/*!
- Outputs the moment's values to a stream.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& m){
- os << (__FILE__) << std::endl;
- os << "An = " << m.values[0] << std::endl ;
- return os;
-}
-
-/*!
-Prints dependencies namely,
-1. Depth at desired pose Z*
-2. Area moment at desired pose
- m00* if DENSE moment object, (mu20* + mu02*) if DISCRETE moment object
-3. Area moment at current pose
- m00 if DENSE moment object, (mu20 + mu02) if DISCRETE moment object
-*/
-void vpMomentAreaNormalized::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "Desired depth Z* = " << desiredDepth << std::endl;
- os << "Desired area m00* = " << desiredSurface << std::endl;
-
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered)
- throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
- os << "a = " << a << std::endl;
-}
diff --git a/src/tracking/moments/vpMomentAreaNormalized.h b/src/tracking/moments/vpMomentAreaNormalized.h
deleted file mode 100644
index 5c5e1c4..0000000
--- a/src/tracking/moments/vpMomentAreaNormalized.h
+++ /dev/null
@@ -1,156 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentAreaNormalized.h 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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).
-*/
-#ifndef __MOMENTSURFACENORMALIZED_H__
-#define __MOMENTSURFACENORMALIZED_H__
-
-#include <visp/vpMoment.h>
-
-class vpMomentObject;
-class vpMomentCentered;
-
-/*!
- \class vpMomentAreaNormalized
-
- \ingroup TrackingMoments
-
- \brief Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
-
- This moment depends on vpMomentCentered.
-
- The idea behind vpMomentAreaNormalized is described in \cite Tahri05z.
-
- During a visual servoing process, a vpMomentAreaNormalized will converge towards the desired depth when the current surface will converge to the destination surface.
- It is defined as follows: \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$ where \e a is the current surface and \e a* the destination surface.
- Consequently, the vpMomentAreaNormalized needs to have information about the desired depth \e Z* and the desired surface \e a*.
-
- \warning About the order of the object.
- The surface (refered to as \e a in the above paragraph) depends of the nature of the object.
- - In case of a continuous object (when vpMomentObject::getType() is vpMomentObject::DENSE_FULL_OBJECT or vpMomentObject::DENSE_POLYGON) \f$a=m_{00}\f$.
- - In case of a discrete object (when vpMomentObject::getType() is vpMomentObject::DISCRETE) \f$a=\mu_{20}+\mu_{02}\f$.
-
- Therefore, a vpMomentObject has to be of minimum order 2 in order to compute a vpMomentAreaNormalized moment in the discrete case
- and of minimum order 0 in continous cases.
-
- This example shows a computation in the discrete case.
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentAreaNormalized.h>
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains object points
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane of point 1
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane of point 2
- vec_p.push_back(p);
-
- //////////////////////////////REFERENCE VALUES////////////////////////////////
- vpMomentObject obj(2); // Object where all the moment defined with
- // i+j <= 2 will be computed below. Order is
- // 2 because in discrete mode, the surface
- // computation is a=mu02+mu02
-
-
- obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
- obj.fromVector(vec_p); // initialize the object with the points coordinates
-
- vpMomentDatabase db; //reference database
- vpMomentGravityCenter g; // declaration of gravity center
- vpMomentCentered mc; // centered moments
- vpMomentAreaNormalized an(2,1); //declare normalized surface with
- //destination depth of 1 meter and
- //destination surface of 2 m2
-
- g.linkTo(db); //add gravity center to database
- mc.linkTo(db); //add centered moments
- an.linkTo(db); //add alpha depending on centered moments
-
- db.updateAll(obj); // All of the moments must be updated, not just an
-
- g.compute(); // compute the moment
- mc.compute(); //compute centered moments AFTER gravity center
- an.compute(); //compute alpha AFTER centered moments.
-
- std::cout << an << std::endl;
-
- return 0;
-}
- \endcode
- This code produces the following output:
- \code
-An:1.41421
- \endcode
-*/
-class VISP_EXPORT vpMomentAreaNormalized : public vpMoment {
- private:
- double desiredSurface;
- double desiredDepth;
- public:
- vpMomentAreaNormalized(double desiredSurface, double desiredDepth);
- void compute();
- /*!
- Retrieves the desired depth \e Z* as specified in the constructor.
- */
- double getDesiredDepth() const { return desiredDepth; }
- /*!
- Retrieves the desired surface \e a* as specified in the constructor.
- */
- double getDesiredSurface() const { return desiredSurface; }
-
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentAreaNormalized";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& v);
- void printDependencies(std::ostream& os) const;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentBasic.cpp b/src/tracking/moments/vpMomentBasic.cpp
deleted file mode 100644
index 9dfb71f..0000000
--- a/src/tracking/moments/vpMomentBasic.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentBasic.cpp 4711 2014-03-28 17:41:47Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentObject.h>
-/*!
- Default constructor.
-*/
-vpMomentBasic::vpMomentBasic() : vpMoment(){
-
-}
-
-/*!
- Retrieve all moments of all orders computed. vpMomentBasic::get()[j*order+i] refers to moment \f$m_{ij}\f$.
- \return all computed moments.
-
- Same behaviour as vpMomentObject.
-*/
-const std::vector<double>& vpMomentBasic::get() const{
- return getObject().get();
-}
-
-/*!
- Gets the desired moment using indexes.
- \param i : first index of the 2D moment.
- \param j : second index of the 2D moment.
- \return \f$m_{ij}\f$ moment.
-
- Same behaviour as vpMomentObject.
-*/
-double vpMomentBasic::get(unsigned int i,unsigned int j) const{
- return getObject().get(i,j);
-}
-
-/*!
- Dummy function. Everything is already done in object.
-*/
-void vpMomentBasic::compute(){
-
-}
-
-/*!
- Outputs the moment's values to a stream.
- Same output as in vpMomentObject.
-*/
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentBasic& m){
- os << (__FILE__) << std::endl;
- vpMomentObject::printWithIndices(m.getObject(), os);
- return os;
-}
-
-/*!
-No dependencies on other vpMoments, since basic moments are computed in vpMomentObject
-Just prints the basic moments in vpMomentObject with indices
-*/
-void vpMomentBasic::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "No dependencies on other vpMoments, since basic moments are computed in vpMomentObject" << std::endl;
- vpMomentObject::printWithIndices(getObject(), os);
-}
diff --git a/src/tracking/moments/vpMomentBasic.h b/src/tracking/moments/vpMomentBasic.h
deleted file mode 100644
index 8dde861..0000000
--- a/src/tracking/moments/vpMomentBasic.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentBasic.h 4710 2014-03-28 17:39:33Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Basic moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpMomentBasic.h
- \brief Basic moment descriptor (also called m).
-*/
-
-#ifndef __MOMENTBASIC_H__
-#define __MOMENTBASIC_H__
-
-#include <visp/vpMoment.h>
-
-/*!
- \class vpMomentBasic
-
- \ingroup TrackingMoments
-
- \brief This class defines the 2D basic moment \f$m_{ij}\f$.
- This class is a wrapper for vpMomentObject wich allows to use it as a moment.
- This enables all common operations such as adding to a vpMomentDatabase.
-
- In the case of a dense object \e O, 2D basic moments are defined by:
- \f[m_{ij} = \int \int_{O} x^j y^j dx dy\f]
-
- In the case of a discrete set of \e n points, the basic moments are defined by
- \f[m_{ij} = \sum_{k=1}^{n} x_k^j y_k^j \f]
-
- The selection of a dense or discrete object is done in vpMomentObject.
- The maximum order of vpMomentBasic is the same as that of vpMomentObject (ie: \f$i+j \leq \f$ vpMomentObject::getOrder).
-
- Note that in the continuous case, the basic moment \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
- In the discrete case, it is the number of discrete points \f$n\f$.
-*/
-
-class VISP_EXPORT vpMomentBasic : public vpMoment {
- public:
- vpMomentBasic();
-
- void compute();
- const std::vector<double>& get() const;
- double get (unsigned int i,unsigned int j) const;
- /*!
- Moment name.
- */
- const char* name() const { return "vpMomentBasic";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentBasic& v);
- void printDependencies(std::ostream& os) const;
-};
-#endif
diff --git a/src/tracking/moments/vpMomentCInvariant.cpp b/src/tracking/moments/vpMomentCInvariant.cpp
deleted file mode 100644
index 20230ac..0000000
--- a/src/tracking/moments/vpMomentCInvariant.cpp
+++ /dev/null
@@ -1,278 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCInvariant.cpp 5304 2015-02-10 17:03:04Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Descriptor for various invariants used to drive space roations around X and Y axis.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentObject.h>
-
-/*!
- Default constructor.
- (option to use a different calculation mode for sx and sy)
-*/
-vpMomentCInvariant::vpMomentCInvariant(bool flg_sxsynormalization)
- : I(16),II(4),c(4),s(4), K(0.0), cn(4),sn(4), In1(0.0), flg_sxsynormalization_(flg_sxsynormalization)
-{
- values.resize(14);
-}
-
-/*!
- Computes some temporary invariants which are used internally to compute the final invariants.
- \param momentCentered : centered moments
- \param I : invariant output values
-*/
-void vpMomentCInvariant::computeI(const vpMomentCentered& momentCentered, std::vector<double>& I_val){
-
- double mu30 = momentCentered.get(3,0);
- double mu30_2 = mu30*mu30;
- double mu30_3 = mu30_2*mu30;
-
- double mu03 = momentCentered.get(0,3);
- double mu03_2 = mu03*mu03;
- double mu03_3 = mu03*mu03_2;
-
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu50 = momentCentered.get(5,0);
- double mu32 = momentCentered.get(3,2);
- double mu14 = momentCentered.get(1,4);
- double mu05 = momentCentered.get(0,5);
- double mu23 = momentCentered.get(2,3);
- double mu41 = momentCentered.get(4,1);
- double mu40 = momentCentered.get(4,0);
- double mu04 = momentCentered.get(0,4);
- double mu31 = momentCentered.get(3,1);
- double mu13 = momentCentered.get(1,3);
- double mu22 = momentCentered.get(2,2);
- double mu21 = momentCentered.get(2,1);
- double mu12 = momentCentered.get(1,2);
- double mu11 = momentCentered.get(1,1);
-
- double mu11_2 = mu11*mu11;
- double mu12_2 = mu12*mu12;
- double mu21_2 = mu21*mu21;
- double mu22_2 = mu22*mu22;
- double mu13_2 = mu13*mu13;
- double mu31_2 = mu31*mu31;
- double mu04_2 = mu04*mu04;
- double mu40_2 = mu40*mu40;
- double mu21_3 = mu21*mu21_2;
- double mu12_3 = mu12_2*mu12;
- double mu12_4 = mu12_3*mu12;
- double mu21_4 = mu21_2*mu21_2;
-
- //double kappa = mu30_2+mu03_2-3*mu21_2+6*mu21*mu03; //Used in I8 calculation but simplified with MAPLE and found it to be wrong
- double zeta = mu20-mu02;
- double zeta_2 = zeta * zeta;
- double omicron = (mu03_2+3*mu03*mu21+mu30*(mu30+3*mu12));
- double omega = mu50+2*mu32+mu14;
- double nu = mu05+2*mu23+mu41;
- double ro = mu50-2*mu32-3*mu14;
- double gamma = mu05-2*mu23-3*mu41;
-
- double delta = mu50-10*mu32+5*mu14;
- double phi = mu05-10*mu23+5*mu41;
- double omega_2 = omega*omega;
- double nu_2 = nu*nu;
- double ro_2 = ro*ro;
- double gamma_2 = gamma*gamma;
- double delta_2 = delta*delta;
- double phi_2 = phi*phi;
-
- I_val[1]=-mu20*mu02+mu11_2;
- I_val[2]=zeta_2+4*mu11_2;
- I_val[3]=(mu30-3*mu12)*(mu30-3*mu12)+(mu03-3*mu21)*(mu03-3*mu21);
- I_val[4]=(mu30+mu12)*(mu30+mu12)+(mu21+mu03)*(mu21+mu03);
- I_val[5]=-mu30_2*mu03_2+(-4*mu12_3+6*mu21*mu12*mu03)*mu30-4*mu21_3*mu03+3*mu21_2*mu12_2;
- I_val[6]=3*mu12_4+2*mu30*mu12_3+(3*mu30_2-6*mu03*mu21)*mu12_2-6*mu30*mu21*(mu21+mu03)*mu12+2*mu30_2*mu03_2+2*mu21_3*mu03+3*mu21_2*mu03_2+3*mu21_4;
- I_val[7]=(3*mu21+2*mu03)*mu12_3+3*mu30*(mu03+2*mu21)*mu12_2-3*mu21*(mu30+mu03+mu21)*(-mu30+mu03+mu21)*mu12+mu30*(-mu30_2*mu03-2*mu21_3-3*mu03*mu21_2+mu03_3);
- //I_val[8]=3*mu21_4-3*mu21_3*mu03+(3*mu03_2+kappa-6*mu12_2)*mu21_2-mu03*(-15*mu12_2+kappa)*mu21-(-3*mu12_2*mu30+(2*kappa-3*mu03_2)*mu12+kappa*mu30)*mu12;
- I_val[8] = 3*mu03*mu21_3-2*mu03_2*mu21_2+mu21_2*mu30_2+3*mu12_2*mu03*mu21-mu03*mu21*mu30_2-mu03_3*mu21+3*mu12_3*mu30-2*mu12_2*mu30_2+mu12_2*mu03_2-mu12*mu30_3-mu12*mu30*mu03_2+3*mu12*mu30*mu21_2-6*mu12*mu30*mu03*mu21;
- I_val[9]=omicron*omicron;
-
- I_val[10]=mu40*mu04-4*mu31*mu13+3*mu22_2;
- I_val[11]=3*mu13_2+2*mu31*mu13+(-3*mu40-3*mu04)*mu22-2*mu40*mu04+3*mu31_2;
- I_val[12]=3*mu04_2+(2*mu40+12*mu22)*mu04+3*mu40_2+12*mu40*mu22+16*mu31*mu13;
- I_val[13]=omega_2+nu_2;
- I_val[14]=ro_2+gamma_2;
- I_val[15]=delta_2+phi_2;
-
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
-
- c[1]=momentCentered.get(2,0)-momentCentered.get(0,2);
- s[1]=2*momentCentered.get(1,1);
- c[2]=momentCentered.get(0,3)-3*momentCentered.get(2,1);
- s[2]=momentCentered.get(3,0)-3*momentCentered.get(1,2);
- c[3]=c[1]*c[1]-s[1]*s[1];
- s[3]=2*s[1]*c[1];
-
- II[1]=c[1]*c[1]+s[1]*s[1];
- II[2]=c[2]*c[2]+s[2]*s[2];
- II[3]=momentCentered.get(2,0)+momentCentered.get(0,2);
-
- K=(II[1]*(II[3]*sqrt(std::abs(II[3]))))/sqrt(std::abs(a));
-
- /*
- * Intermediate quantities required for calculation of normalized version of Sx and Sy
- * The pij doubles below are the respective centered moment values mu_ij scaled by mu20 + mu02
- */
- double p20 = momentCentered.get(2,0)/II[3]; // II[3] is the normalization factor for the 2nd order moments
- double p11 = momentCentered.get(1,1)/II[3];
- double p02 = momentCentered.get(0,2)/II[3];
-
- double d = sqrt(std::abs(a))/(II[3]*sqrt(std::abs(II[3]))); // d is the normalization factor for 3rd order moments
- double p30 = momentCentered.get(3,0)*d;
- double p21 = momentCentered.get(2,1)*d;
- double p12 = momentCentered.get(1,2)*d;
- double p03 = momentCentered.get(0,3)*d;
-
- cn[1] = p20 - p02;
- sn[1] = 2.0*p11;
- sn[2] = p30 - 3.0*p12;
- cn[2] = p03 - 3.0*p21;
-
- cn[3] = cn[1]*cn[1]-sn[1]*sn[1];
- sn[3] = 2.0*sn[1]*cn[1];
-
- In1 = cn[1]*cn[1]+sn[1]*sn[1];
-}
-
-/*!
- Computes translation-plane-rotation-scale invariants.
- Depends on vpMomentCentered.
- All possible invariants are computed here. The selection of the invariant is done afterwards.
-*/
-void vpMomentCInvariant::compute(){
- if(getObject().getOrder()<5) throw vpException(vpException::notInitialized,"Order is not high enough for vpMomentCInvariant. Specify at least order 5.");
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered)));
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- computeI(momentCentered,I);
- double II3_2 = II[3]*II[3];
- double II3_3 = II3_2*II[3];
-
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
-
- values[0] = I[1]/I[2];
- values[1] = I[3]/I[4];
-
- values[2] = I[5]/I[6];
-
- values[3] = I[7]/I[6];
-
- values[4] = I[8]/I[6];
-
- values[5] = I[9]/I[6];
-
- values[6] = I[11]/I[10];
-
- values[7] = I[12]/I[10];
-
- values[8] = I[13]/I[15];
-
- values[9] = I[14]/I[15];
-
- if (flg_sxsynormalization_)
- calcSxSyNormalized(values[10], values[11]);
- else
- calcSxSy(values[10], values[11]);
-
- values[12] = II[1]/(II3_2); // Px
- values[13] = a*II[2]/(II3_3); // Py
-}
-
-/*!
- Sx and Sy as it was inside compute()
- */
-void vpMomentCInvariant::calcSxSy(double& sx, double& sy) const{
- sx = (c[2]*c[3]+s[2]*s[3])/K;
- sy = (s[2]*c[3]-c[2]*s[3])/K;
-}
-
-/*!
- * Sx and Sy from normalized 2nd and 3rd order moments
- * Numerically better (than in the usual Sx,Sy when K appears in the denominator)
- */
-void vpMomentCInvariant::calcSxSyNormalized(double& sx, double& sy) const{
- sx = (cn[2]*cn[3] + sn[2]*sn[3]) / In1;
- sy = (sn[2]*cn[3] - cn[2]*sn[3]) / In1;
-}
-
-/*!
- Prints the temporary invariants. Used for debug purposes only
- \param index : index of the temporary invariant
-*/
-void vpMomentCInvariant::printI(unsigned int index){
- std::cout << "I("<< index << ")=" << I[index] << std::endl;
-}
-
-/*!
- Print out all invariants that were computed
- There are 15 of them, as in [Point-based and region based.ITRO05]
- \cite Tahri05z
- */
-void vpMomentCInvariant::printInvariants(std::ostream& os) const{
- for (unsigned int i = 1; i < I.size(); ++i){ // i = 1 since vector I has been indexed from 1 in vpMomentCinvariant
- os << "I[" << i << "]=" << I[i] << std::endl;
- }
- os << std::endl;
-
-}
-
-/*!
- Outputs the moment's values to a stream.
-*/
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentCInvariant& c){
- for(unsigned int i = 0;i<c.values.size();i++){
- os << c.values[i] << "," << std::endl;
- }
- return os;
-}
-
diff --git a/src/tracking/moments/vpMomentCInvariant.h b/src/tracking/moments/vpMomentCInvariant.h
deleted file mode 100644
index c4652af..0000000
--- a/src/tracking/moments/vpMomentCInvariant.h
+++ /dev/null
@@ -1,274 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCInvariant.h 5303 2015-02-10 17:01:28Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Descriptor for various invariants used to drive space roations around X and Y axis.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpMomentCInvariant.h
- \brief Descriptor for various invariants used to drive space roations around X and Y axis.
-*/
-#ifndef __MOMENTCINVARIANT_H__
-#define __MOMENTCINVARIANT_H__
-
-#include <visp/vpMoment.h>
-#include <visp/vpMomentDatabase.h>
-
-class vpMomentCentered;
-class vpMomentBasic;
-
-/*!
- \class vpMomentCInvariant
-
- \ingroup TrackingMoments
-
- This class defines several 2D (translation+rotation+scale) invariants for both symmetric and non-symmetric objects.
- These moment-based invariants are described in the following papers \cite Chaumette04a, \cite Tahri05z.
-
- The descriptions for the invariants \f$C_1\f$ to \f$C_{10}\f$ can be found in \cite Chaumette04a and for invariants \f$P_x\f$,\f$P_y\f$,\f$S_x\f$,\f$S_y\f$ in \cite Tahri05z.
-
- These invariants are classicaly used in visual servoing to control the out-of-plane rotations.
- The C-type or P-type invariants are used for non-symmetric objects whereas the S-type invariants are used for symmetric objects.
-
- For most cases of non-symmetric objects, (\f$C_4\f$,\f$C_6\f$) or (\f$P_x\f$,\f$P_y\f$) couples are widely used to control x and y rotations.
- For symmetric objects \f$S_x\f$ and \f$S_y\f$ are the only choice.
-
- There are 14 translation+rotation+scale invariants (10 C-type, 2 P-type and 2 S-type) that can be accessed from by vpMomentCInvariant::get
- or any of the get shortcuts.
-
- The example below shows how to retrieve the \f$C_2\f$ invariant:
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentCommon.h>
-#include <iostream>
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p;
-
- p.set_x(6); p.set_y(-1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(3); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(0); p.set_y(1.2); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(-7); p.set_y(-4); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
-
- vpMomentObject obj(5); // Create an image moment object with 5 as maximum order
- obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
- obj.fromVector(vec_p);
-
- //initialisation with default values
- vpMomentCommon db(vpMomentCommon::getSurface(obj),vpMomentCommon::getMu3(obj),vpMomentCommon::getAlpha(obj),1.);
- bool success;
-
- db.updateAll(obj); // Update AND compute all moments
-
- //get C-invariant
- const vpMomentCInvariant& C = static_cast<const vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
- if(success)
- std::cout << C.get(1) << std:: endl; // print C2 invariant
- else
- std::cout << "vpMomentCInvariant not found." << std::endl;
-
- return 0;
-}
-\endcode
-
-vpMomentCInvariant depends on vpMomentCentered (see vpMomentDatabase and vpMomentCommon).
-*/
-class VISP_EXPORT vpMomentCInvariant : public vpMoment {
- private:
- std::vector<double> I;
- std::vector<double> II;
- std::vector<double> c;
- std::vector<double> s;
- double K;
- void computeI(const vpMomentCentered& momentCentered, std::vector<double>& I);
-
- /* To calculate Sx and Sy from normalized moments */
- void calcSxSy(double& sx, double& sy) const;
- void calcSxSyNormalized(double& sx, double& sy) const;
- std::vector<double> cn; // same as s above but calculated from normalized moments
- std::vector<double> sn; // same as c above but calculated from normalized moments
- double In1; // same as I1 in Sx,Sy formulae but calculated from normalized moments
- bool flg_sxsynormalization_;
-
- public:
-
- vpMomentCInvariant(bool flg_sxsynormalization = false);
-
- /*!
- Shorcut for getting the value of \f$C_1\f$.
- */
- double C1() const { return values[0]; }
- /*!
- Shorcut for getting the value of \f$C_2\f$.
- */
- double C2() const { return values[1]; }
- /*!
- Shorcut for getting the value of \f$C_3\f$.
- */
- double C3() const { return values[2]; }
- /*!
- Shorcut for getting the value of \f$C_4\f$.
- */
- double C4() const { return values[3]; }
- /*!
- Shorcut for getting the value of \f$C_5\f$.
- */
- double C5() const { return values[4]; }
- /*!
- Shorcut for getting the value of \f$C_6\f$.
- */
- double C6() const { return values[5]; }
- /*!
- Shorcut for getting the value of \f$C_7\f$.
- */
- double C7() const { return values[6]; }
- /*!
- Shorcut for getting the value of \f$C_8\f$.
- */
- double C8() const { return values[7]; }
- /*!
- Shorcut for getting the value of \f$C_9\f$.
- */
- double C9() const { return values[8]; }
- /*!
- Shorcut for getting the value of \f$C_{10}\f$.
- */
- double C10() const { return values[9]; }
-
- void compute();
-
- /*!
- Gets the desired invariant.
- \param i given index. For invariants from C1 to C10 the corresponding index is from 0 to 9. For \f$S_x\f$,\f$S_y\f$ the indexes are 10,11 and for \f$P_x\f$,\f$P_y\f$ they are 12,13.
- */
- double get(unsigned int i) const { return values[i]; }
-
- /*!
- Access to partial invariant c (see [2]).
- */
- double getC(unsigned int i) const {return c[i];}
- /*!
- Access to partial invariants. The index convention is the same as in [1].
- */
- double getI(unsigned int index) const {return I[index];}
-
- /*!
- Print the moment invariants used to obtain the actual visual features
- */
- void printInvariants(std::ostream& os) const;
-
- /*!
- Access to partial invariant I (see [2]).
- */
- double getII(unsigned int i) const {return II[i];}
- /*!
- Access to partial invariant K (see [2]).
- */
- double getK() const {return K;}
-
- /*!
- Access to partial invariant S (see [2]).
- */
- double getS(unsigned int i) const {return s[i];}
-
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentCInvariant";}
-
- /*!
- Print partial invariant.
- */
- void printI(unsigned int index);
-
- /*!
- Shorcut for getting the value of \f$P_x\f$.
- */
- double Px(){ return values[12]; }
- /*!
- Shorcut for getting the value of\f$P_y\f$.
- */
- double Py(){ return values[13]; }
-
- /*!
- Shorcut for getting the value of \f$S_x\f$.
- */
- double Sx() const { return values[10]; }
- /*!
- Shorcut for getting the value of \f$S_y\f$.
- */
- double Sy() const { return values[11]; }
-
- /*!
- * Getters for I
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getIn1() const {return In1;}
-
- /*!
- * Getter for c
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getCN(unsigned int i) const {return cn[i];}
-
- /*!
- * Getter for s
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getSN(unsigned int i) const {return sn[i];}
-
- /*!
- * To know if Sx and Sy were calculated from normalized moments or not
- */
- bool isSxSyfromNormalizedMoments() const {return flg_sxsynormalization_;};
-
- /*!
- * To get all the invariant values as a whole.
- */
- inline const std::vector<double>& getMomentVector() const { return values; }
-
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCInvariant& v);
-};
-#endif
diff --git a/src/tracking/moments/vpMomentCentered.cpp b/src/tracking/moments/vpMomentCentered.cpp
deleted file mode 100644
index 2e0a066..0000000
--- a/src/tracking/moments/vpMomentCentered.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCentered.cpp 4712 2014-03-28 17:55:43Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Centered moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <exception>
-#include <cassert>
-
-/*!
- To set the values of centred moments. Required when normalizing the moment values.
- @param i : first index of the 2D moment.
- @param j : second index of the 2D moment.
- @param value : value of the moment.
-*/
-void vpMomentCentered::set(unsigned int i, unsigned int j, double value){
- vpMomentObject mobj = getObject();
- assert(i+j<=mobj.getOrder());
- if(i+j>mobj.getOrder()) throw vpException(vpException::badValue,"You cannot set that value.");
- values[j*(mobj.getOrder()+1)+i] = value;
-}
-
-/*!
- Computes centered moments of all available orders.
- Depends on vpMomentGravityCenter.
-*/
-void vpMomentCentered::compute(){
- bool found_moment_gravity;
- values.resize((getObject().getOrder()+1)*(getObject().getOrder()+1));
-
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- unsigned int order = getObject().getOrder()+1;
- for(register unsigned int j=0;j<(order);j++){
- for(register unsigned int i=0;i<order-j;i++){
- unsigned int c = order*j+i;
- values[c]=0;
- for(register unsigned int k=0;k<=i;k++){
- double Xg_i_k = pow(-momentGravity.get()[0],(int)(i-k));
- double comb_i_k = static_cast<double>( vpMath::comb(i,k) );
- for(register unsigned int l=0;l<=j;l++){
- values[c]+= static_cast<double>( comb_i_k*vpMath::comb(j,l)
- *Xg_i_k
- *pow(-momentGravity.get()[1],(int)(j-l))*getObject().get(k,l) );
- }
- }
- }
- }
-
-}
-
-/*!
- Default constructor.
-*/
-vpMomentCentered::vpMomentCentered() : vpMoment(){
-
-}
-
-/*!
- Gets the desired moment using indexes.
- \param i : first index of the centered moment.
- \param j : second index of the centered moment.
- \return \f$\mu_{ij}\f$ moment.
-*/
-double vpMomentCentered::get(unsigned int i,unsigned int j) const {
- unsigned int order = getObject().getOrder();
- assert(i+j<=order);
- if(i+j>order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
-
- return values[j*(order+1)+i];
-}
-
-/*!
- Outputs the centered moment's values \f$\mu_{ij}\f$ to a stream presented as a matrix.
- The first line corresponds to \f$\mu_{0[0:order]}\f$, the second one to \f$\mu_{1[0:order]}\f$
- Values in table corresponding to a higher order are marked with an "x" and not computed.
-
- For example, if the maximal order is 3, the following values are provided:
-
- \code
-u00 u10 u20 u30
-u01 u11 u21 x
-u02 u12 x x
-u30 x x x
- \endcode
-
- This output will be followed by an output with indexes as produced by printWithIndices() function
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& m){
- for(unsigned int i = 0;i<m.values.size();i++){
- if(i%(m.getObject().getOrder()+1)==0)
- os << std::endl;
-
- if((i%(m.getObject().getOrder()+1)+i/(m.getObject().getOrder()+1))<m.getObject().getOrder()+1)
- os << m.values[i] ;
- else
- os << "x";
-
- os << "\t";
- }
- os << std::endl;
- m.printWithIndices(os);
- return os;
-}
-
-/*!
-Print in a readable form which looks better than output from << operator
-*/
-void
-vpMomentCentered::printWithIndices(std::ostream& os) const {
- unsigned int orderp1 = getObject().getOrder()+1;
- for(unsigned int k=0; k<orderp1; k++) {
- for(unsigned int l=0; l<orderp1-k; l++)
- {
- os << "mu[" << k << "," << l << "] = " << this->get(k,l) << "\t";
- }
- os << std::endl;
- }
- os << std::endl;
-}
-
-/*!
-Prints moments required for calculation of vpMomentCentered,
-which are
-1. Raw geometric moments (vpMomentObject) and
-2. Centre of gravity (vpMomentGravityCentered)
-*/
-void
-vpMomentCentered::printDependencies(std::ostream& os) const {
- os << (__FILE__) << std::endl;
- /*
- Retreive the raw moments
- */
- const vpMomentObject objt = getObject();
- vpMomentObject::printWithIndices(objt, os);
-
- /*
- Get xg,yg
- */
- bool found_moment_gravity;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity)
- throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- os << "Xg = " << momentGravity.getXg() << "\t" << "Yg = " << momentGravity.getYg() << std::endl;
-}
diff --git a/src/tracking/moments/vpMomentCentered.h b/src/tracking/moments/vpMomentCentered.h
deleted file mode 100644
index 0d00fb5..0000000
--- a/src/tracking/moments/vpMomentCentered.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCentered.h 4712 2014-03-28 17:55:43Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Centered moment descriptor
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#ifndef __MOMENTCENTERED_H__
-#define __MOMENTCENTERED_H__
-
-#include <visp/vpMoment.h>
-/*!
- \file vpMomentCentered.h
- \brief Centered moment descriptor (also refered to as \f$\mu_{ij}\f$).
-
-*/
-
-class vpMomentObject;
-
-/*!
- \class vpMomentCentered
-
- \ingroup TrackingMoments
-
- \brief This class defines the double-indexed centered moment descriptor \f$\mu_{ij}\f$.
-
- In the case of a dense object \e O, centered moments are defined by:
- \f[\mu_{ij}= \int \int_{O} (x_k-x_g)^j (y_k-y_g)^j\f]
-
- In the case of a discrete set of \e n points, centered moments are defined by:
- \f[\mu_{ij}= \sum_{k=1}^{n} (x_k-x_g)^j (y_k-y_g)^j\f]
-
- where \f$(x_g,y_g)\f$ are the coordinates of the center of gravity.
-
- The centered moments are computed from the object at the highest possible order.
- For example if the vpMomentObject is defined up to order 5, vpMomentCentered will be too.
-
- Values of vpMomentCentered may be accessed by one of the two vpMomentCentered::get methods.
- When using vpMomentCentered::get (), the format of the return vector is the following:
- \f$ \mu_{ij} \f$ is stored at vpMomentCentered::get ()[j* (vpMomentObject::getOrder () +1)+i]
-
- vpMomentCentered depends on vpMomentGravityCenter.
-
-*/
-class VISP_EXPORT vpMomentCentered : public vpMoment {
-public:
-
- vpMomentCentered();
-
- void compute();
- double get(unsigned int i,unsigned int j) const;
-
- inline const std::vector<double>& get() const;
- /*!
- Moment name.
- */
- inline const char* name() const {return "vpMomentCentered";}
-
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& v);
- void printWithIndices(std::ostream& os) const;
- void printDependencies(std::ostream& os) const;
-
-protected:
-
- void set(unsigned int i, unsigned int j, double value);
-
-};
-
-/*!
- Returns all centered moment values \f$\mu_{ij}\f$ with \f$i+j \leq order\f$ where order is the object's order.
-
- \return Vector of moment values. To access \f$\mu_{ij}\f$, you have to read vpMomentObject::get()[j*order+i].
-
- For example, if the maximal order is 3, the following values are provided:
- \code
-u00 u10 u20 u01 u11 u21 u02 u12 u12 u30 u03
- \endcode
- To have a better reading of the moments you can picture them as a triangular matrix:
- \code
-u00 u10 u20 u30
-u01 u11 u21 x
-u02 u12 x x
-u30 x x x
- \endcode
- The moments of the same order are on each of the matrix reverse diagonals.
- To access for example to the centered moment \f$\mu_{12}\f$, you should use this kind of code:
- \code
-vpMomentCentered mc;
-//[...]
-mc.compute();
-double mu12;
-mu12 = mc.get()[2*(obj.getOrder()+1)+1]; // i=1 and j=2
-mu12 = mc.get(1,2); // the same
- \endcode
-*/
-inline const std::vector<double>& vpMomentCentered::get() const
-{
- return vpMoment::get();
-}
-
-
-#endif
diff --git a/src/tracking/moments/vpMomentCommon.cpp b/src/tracking/moments/vpMomentCommon.cpp
deleted file mode 100644
index 9c67fcd..0000000
--- a/src/tracking/moments/vpMomentCommon.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCommon.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pre-filled moment database with all commonly used moments.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentCommon.h>
-#include <visp/vpMomentObject.h>
-
-/*!
- Default constructor. Initializes the common database with the following moments:
- basic, gravity,centered,centered+normalized,normalized gravity,normalized surface, scale-plane-rotation-translation invariant, alpha, symmetric invariant.
- \param dstSurface : destination surface. You may use vpMomentCommon::getSurface().
- \param ref : reference 3rd order moments (see vpMomentAlpha). You may use vpMomentCommon::getMu3().
- \param refAlpha : reference alpha (see vpMomentAlpha). You may use vpMomentCommon::getAlpha().
- \param dstZ : destination depth.
- \param flg_sxsyfromnormalized : flag to enable calculation of sx,sy from normalized moments.
-*/
-vpMomentCommon::vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ, bool flg_sxsyfromnormalized):
- momentBasic(),
- momentGravity(),
- momentCentered(),
- momentGravityNormalized(),
- momentSurfaceNormalized(dstSurface,dstZ),
- momentCInvariant(),
- momentAlpha(ref,refAlpha),
- momentArea()
-{
- momentCInvariant = new vpMomentCInvariant(flg_sxsyfromnormalized);
-
- momentBasic.linkTo(*this);
- momentGravity.linkTo(*this);
- momentCentered.linkTo(*this);
- momentGravityNormalized.linkTo(*this);
- momentSurfaceNormalized.linkTo(*this);
- momentCInvariant->linkTo(*this);
- momentAlpha.linkTo(*this);
- momentArea.linkTo(*this);
-}
-
-/*!
-Updates all moments in the database with the object and computes all their values.
-This is possible because this particular database knows the link between the moments it contains.
-The order of computation is as follows:
-vpMomentGravityCenter,vpMomentCentered,vpMomentAlpha,vpMomentCInvariant,vpMomentSInvariant,vpMomentAreaNormalized,vpMomentGravityCenterNormalized
-\param object : Moment object.
-
-Example of using a preconfigured database to compute one of the C-invariants:
-\code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentCommon.h>
-#include <iostream>
-
-int main()
-{
- // Define two discrete points
- vpPoint p;
- std::vector<vpPoint> vec_p; // std::vector that contains the vertices of the contour polygon
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-3); p.set_y(1); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
-
- vpMomentObject obj(5); // Object initialized up to order 5 to handle
- // all computations required by vpMomentCInvariant
- obj.setType(vpMomentObject::DENSE_POLYGON); // object is the inner part of a polygon
- obj.fromstd::vector(vec_p); // Init the discrete object with two points
-
- //initialisation with default values
- vpMomentCommon db(vpMomentCommon::getSurface(obj),vpMomentCommon::getMu3(obj),vpMomentCommon::getAlpha(obj),1.);
- bool success;
-
- db.updateAll(obj); // Update AND compute all moments
-
- //get C-invariant
- vpMomentCInvariant& C = static_cast<vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
- if(success){
- std::cout << C.get(0) << std:: std::endl;
- }else
- std::cout << "vpMomentCInvariant not found." << std::endl;
-
- return 0;
-}
-
-\endcode
-*/
-void vpMomentCommon::updateAll(vpMomentObject& object){
- try {
- vpMomentDatabase::updateAll(object);
-
- momentGravity.compute();
- momentCentered.compute();
- momentAlpha.compute();
- momentCInvariant->compute();
-
- momentSurfaceNormalized.compute();
- momentGravityNormalized.compute();
- momentArea.compute();
-
- } catch(const char* ex){
- std::cout << "exception:" << ex <<std::endl;
-
- }
-}
-
-/*!
-Gets the surface of an object
-\param object : moment object
-*/
-double vpMomentCommon::getSurface(vpMomentObject& object) {
- vpMomentDatabase moments;
-
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
-
- moments.updateAll(object);
-
- momentGravity.compute();
- momentCentered.compute();
-
- double a;
- if (object.getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = object.get(0,0);
-
- return a;
-}
-
-/*!
-Gets a reference alpha of an object.
-\param object : Moment object.
-*/
-double vpMomentCommon::getAlpha(vpMomentObject& object) {
- vpMomentDatabase moments;
-
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
- vpMomentAlpha momentAlpha; momentAlpha.linkTo(moments);
-
- moments.updateAll(object);
- momentGravity.compute();
- momentCentered.compute();
- momentAlpha.compute();
-
- return momentAlpha.get();
-}
-
-/*!
-Gets the reference 3rd order moments of an object.
-\param object : Moment object.
-*/
-std::vector<double> vpMomentCommon::getMu3(vpMomentObject& object) {
- vpMomentDatabase moments;
-
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
-
- moments.updateAll(object);
-
- momentGravity.compute();
- momentCentered.compute();
-
- std::vector<double> mu(4);
- unsigned int idx=0;
- for (unsigned int j=0; j<4; j++) {
- for (unsigned int i=0; i<4; i++) {
- if (i+j==3){
- mu[idx] = momentCentered.get(i,j);
- idx++;
- }
- }
- }
- return mu;
-}
-
-vpMomentCommon::~vpMomentCommon(){
- if (momentCInvariant)
- delete momentCInvariant;
-}
diff --git a/src/tracking/moments/vpMomentCommon.h b/src/tracking/moments/vpMomentCommon.h
deleted file mode 100644
index beb17c3..0000000
--- a/src/tracking/moments/vpMomentCommon.h
+++ /dev/null
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentCommon.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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.
-*/
-#ifndef VPCOMMONMOMENTS_H
-#define VPCOMMONMOMENTS_H
-
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentGravityCenterNormalized.h>
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentAlpha.h>
-#include <visp/vpMomentArea.h>
-
-#include <vector>
-
-class vpMomentObject;
-
-/*!
- \class vpMomentCommon
-
- \ingroup TrackingMoments
-
- \brief This class initializes and allows access to commonly used moments.
-
- It is a vpMomentDatabase filled with the following moments:
- - vpMomentBasic
- - vpMomentGravityCenter
- - vpMomentCentered
- - vpMomentGravityCenterNormalized
- - vpMomentAreaNormalized
- - vpMomentCInvariant
- - vpMomentAlpha
- - vpMomentArea
-
- There is no need to do the linkTo operations manually nor is it necessary to care about the order of moment computation.
-
- This class carries an vpMomentCommon::updateAll() method capable of updating AND computing moments from an object (see 4-step process in vpMoment).
- The moments computed by this class are classical moments used in moment-based visual servoing.
- For more information see \cite Tahri05z.
-
- To initialize this moment set the user needs to compute the following things:
- - the Mu3 value set: set of third-order centered moments computed for a reference object. (\f$\mu_{ij}$ with $i+j = 3\f$ ).
- These values allow the system to save the reference angular position and to perform planar rotations of more than 180 degrees if needed.
- - the destination depth.
- - the surface of the destination object in the end of the visual servoing process.
- - the reference alpha: angular position of the object used to obtain the Mu3 set.
-
- Shortcuts for each of these prerequisites are provided by this class except depth (methods
- vpMomentCommon::getMu3(), vpMomentCommon::getSurface(), vpMomentCommon::getAlpha()).
-
- \attention Make sure your object is at least of order 5 when using this pre-filled database.
-
-*/
-class VISP_EXPORT vpMomentCommon : public vpMomentDatabase{
-private:
- vpMomentBasic momentBasic;
- vpMomentGravityCenter momentGravity;
- vpMomentCentered momentCentered;
- vpMomentGravityCenterNormalized momentGravityNormalized;
- vpMomentAreaNormalized momentSurfaceNormalized;
- vpMomentCInvariant* momentCInvariant;
- vpMomentAlpha momentAlpha;
- vpMomentArea momentArea;
-public:
- vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ=1.0, bool flg_sxsyfromnormalized=false);
-
- static double getAlpha(vpMomentObject& object);
- static std::vector<double> getMu3(vpMomentObject& object);
- static double getSurface(vpMomentObject& object);
-
- void updateAll(vpMomentObject& object);
- ~vpMomentCommon();
-};
-#endif // VPCOMMONMOMENTS_H
diff --git a/src/tracking/moments/vpMomentDatabase.cpp b/src/tracking/moments/vpMomentDatabase.cpp
deleted file mode 100644
index f0c6f15..0000000
--- a/src/tracking/moments/vpMomentDatabase.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentDatabase.cpp 4620 2014-01-27 21:28:32Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pseudo-database used to handle dependencies between moments
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMoment.h>
-#include <typeinfo>
-#include <iostream>
-#include <visp/vpMomentObject.h>
-
-/*!
- Adds a moment to the database.
- \param moment : moment to add
- \param : name of the moment's class
-
- \attention You cannot add two moments with the same name. The rules for insersion are the same as those of std::map.
-*/
-void vpMomentDatabase::add(vpMoment& moment,const char* name){
- moments.insert(std::pair<const char*,vpMoment*>((const char*)name,&moment));
-}
-
-/*!
- Retrieves a moment from the database.
- \param type : Name of the moment's class.
- \param found : true if the moment's type exists in the database, false otherwise.
- \return Moment corresponding to \e type.
-*/
-const vpMoment& vpMomentDatabase::get(const char* type, bool& found) const {
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator it = moments.find(type);
-
- found = (it!=moments.end());
- return *(it->second);
-}
-
-/*!
- Updates the moment object for all moments in the database
- \param object : Moment object for which all the moments in the database should be updated.
-
- Sometimes, it might be useful to update the whole database when computing only one moment when this moment depends on other moments.
- The example provided in the header of this class gives an example that shows how to compute gravity center moment and the centered moment using a mass update.
-*/
-void vpMomentDatabase::updateAll(vpMomentObject& object){
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
- for(itr = moments.begin(); itr != moments.end(); itr++){
- (*itr).second->update(object);
- }
-}
-
-/*!
- Outputs all the moments values in the database to a stream.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& m){
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
- os << "{";
-
- for(itr = m.moments.begin(); itr != m.moments.end(); itr++){
- os << (*itr).first << ": [" << *((*itr).second) << "],";
- }
- os << "}";
-
- return os;
-}
diff --git a/src/tracking/moments/vpMomentDatabase.h b/src/tracking/moments/vpMomentDatabase.h
deleted file mode 100644
index 733574c..0000000
--- a/src/tracking/moments/vpMomentDatabase.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentDatabase.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pseudo-database used to handle dependencies between moments
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpMomentDatabase.h
- \brief Pseudo-database used to handle dependencies between moments.
-*/
-#ifndef __MOMENTDATABASE_H__
-#define __MOMENTDATABASE_H__
-
-#include <visp/vpImage.h>
-
-#include <map>
-#include <iostream>
-#include <cstring>
-
-class vpMoment;
-class vpMomentObject;
-
-/*!
- \class vpMomentDatabase
-
- \ingroup TrackingMoments
-
- \brief This class allows to register all vpMoments so they can access each other according to their dependencies.
-
- Sometimes, a moment needs to have access to other moment's values to be computed.
- For example vpMomentCentered needs additionnal information about the gravity center vpMomentGravityCenter in order to compute the moment's value from a vpMomentObject.
- This gravity center should be stored in a vpMomentDatabase where it can be accessed.
-
- All moments in a database can access each other freely at any time. They can also verify if a moment is present in the database or not.
- Here is a example of a dependency between two moments using a vpMomentDatabase:
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCentered.h>
-#include <iostream>
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- vpMomentObject obj(1); // Create an image moment object with 1 as
- // maximum order (sufficient for gravity center)
- obj.setType(vpMomentObject::DISCRETE); // The object is defined by
- // two discrete points
- obj.fromVector(vec_p); // Init the dense object with the polygon
-
- vpMomentDatabase db;
- vpMomentGravityCenter g; // declaration of gravity center
- vpMomentCentered mc; // mc containts centered moments
-
- g.linkTo(db); //add gravity center to database
- mc.linkTo(db); //centered moments depend on gravity, add them to the
- //database to grant access
-
- db.updateAll(obj); // All of the moments must be updated, not just mc
-
- //There is no global compute method since the order of compute calls
- //depends on the implementation
- g.compute(); // compute the moment
- mc.compute(); //compute centered moments AFTER gravity center
-
- std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment
- std::cout << "Centered moments: " << mc << std:: endl; // print centered moment
-
- return 0;
-}
- \endcode
-
- The following code outputs:
- \code
-Gravity center:
-Xg=1.5, Yg=1.5
-Centered moments:
-2 0
-0 x
- \endcode
-
- A moment is identified in the database by it's vpMoment::name method. Consequently, a database can contain at most one moment of each type.
- Often it is useful to update all moments with the same object. Shortcuts (vpMomentDatabase::updateAll) are provided for that matter.
-*/
-class VISP_EXPORT vpMomentDatabase{
- private:
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- struct cmp_str {
- bool operator()(char const *a, char const *b) const{
- return std::strcmp(a, b) < 0;
- }
- };
-#endif
- std::map<const char*,vpMoment*,cmp_str> moments;
- void add(vpMoment& moment, const char* name);
- public:
- vpMomentDatabase() : moments() {}
- virtual ~vpMomentDatabase() {}
-
- const vpMoment& get(const char* type, bool& found) const;
- /*!
- Get the first element in the database.
- May be useful in case an unnamed object is present but is the only element in the database.
- \return the first element in the database.
- */
- vpMoment& get_first(){return *(moments.begin()->second);}
-
- virtual void updateAll(vpMomentObject& object);
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& v);
- friend class vpMoment;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentGravityCenter.cpp b/src/tracking/moments/vpMomentGravityCenter.cpp
deleted file mode 100644
index 3d1e38f..0000000
--- a/src/tracking/moments/vpMomentGravityCenter.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentGravityCenter.cpp 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D Gravity Center moment descriptor (usually described by the pair Xg,Yg)
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentObject.h>
-/*!
- Computes the two gravity center coordinates commonly called \f$x_g\f$ and \f$y_g\f$.
-*/
-void vpMomentGravityCenter::compute(){
- values[0] = getObject().get(1,0)/getObject().get(0,0);
- values[1] = getObject().get(0,1)/getObject().get(0,0);
-}
-
-/*!
- Default constructor.
-*/
-vpMomentGravityCenter::vpMomentGravityCenter() : vpMoment(){
- values.resize(2);
-}
-
-/*!
- Returns a vector of the two gravity center coordinates.
- \return Coordinates in the following moment: \f$(x_g,y_g)\f$.
-*/
-const std::vector<double>& vpMomentGravityCenter::get() const{
- return values;
-}
-
-/*!
- Outputs the moment's values to a stream.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenter& m){
- os << (__FILE__) << std::endl;
- os << "(Xg,Yg) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
- return os;
-}
-
-/*!
-Prints its dependencies
-Basic moments m10, m01 and m00 from vpMomentObject
-*/
-void vpMomentGravityCenter::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "m10 = " << getObject().get(1,0) << "\t";
- os << "m00 = " << getObject().get(0,1) << "\t";
- os << "m00 = " << getObject().get(0,0) << std::endl;
-}
diff --git a/src/tracking/moments/vpMomentGravityCenter.h b/src/tracking/moments/vpMomentGravityCenter.h
deleted file mode 100644
index 8efe3c3..0000000
--- a/src/tracking/moments/vpMomentGravityCenter.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentGravityCenter.h 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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 Xg,Yg).
-*/
-#ifndef __MOMENTGRAVITYCENTER_H__
-#define __MOMENTGRAVITYCENTER_H__
-
-#include <visp/vpMoment.h>
-#include <visp/vpMomentDatabase.h>
-class vpMomentObject;
-
-/*!
- \class vpMomentGravityCenter
-
- \ingroup TrackingMoments
-
- \brief Class describing 2D gravity center moment.
-
- This moment can be computed from scratch (no need to compute any different moments before computing this).
- It gives access to both coordinates of the gravity center \f$x_g\f$ and \f$y_g\f$.
-
- These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}} \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <iostream>
-
-int main()
-{
- // Define the contour of an object by a 5 clockwise vertices on a plane
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
-
- p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
- vec_p.push_back(p);
-
- vpMomentObject obj(1); // Create an image moment object with 1 as
- // maximum order (because only m00,m01,m10
- // are needed to compute the gravity center primitive.
- obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
- obj.fromVector(vec_p); // Init the dense object with the polygon
-
- vpMomentGravityCenter g; // declaration of gravity center
- g.update(obj); // specify the object
- g.compute(); // compute the moment
-
- std::cout << "Xg=" << g.getXg() << std::endl; // access to Xg
- std::cout << "Yg=" << g.getYg() << std::endl; // access to Yg
-
- std::cout << g << std:: endl; // print gravity center
-
- return 0;
-}
-\endcode
-
-This example produces the following results:
-\code
-Xg=0.0166667
-Yg=-0.00833333
-Xg=0.0166667, Yg=-0.00833333
-\endcode
-*/
-
-class VISP_EXPORT vpMomentGravityCenter : public vpMoment {
- public:
-
- vpMomentGravityCenter();
- void compute();
- const std::vector<double>& get() const;
- /*!
- Shortcut function to retrieve \f$x_g\f$.
- \return The first gravity center coordinate.
- */
- double getXg() const { return values[0]; }
- /*!
- Shortcut function to retrieve \f$y_g\f$.
- \return The second gravity center coordinate.
- */
- double getYg() const { return values[1]; }
- /*!
- The class's string name.
- */
- const char* name() const {return "vpMomentGravityCenter";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenter& v);
- void printDependencies(std::ostream& os) const;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentGravityCenterNormalized.cpp b/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
deleted file mode 100644
index 15da646..0000000
--- a/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentGravityCenterNormalized.cpp 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentGravityCenterNormalized.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentAreaNormalized.h>
-
-/*!
- Computes normalized gravity center moment.
- Depends on vpMomentAreaNormalized and on vpMomentGravityCenter.
-*/
-void vpMomentGravityCenterNormalized::compute(){
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
-
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- double Xn = momentGravity.get()[0]*momentSurfaceNormalized.get()[0];
- double Yn = momentGravity.get()[1]*momentSurfaceNormalized.get()[0];
-
- values[0] = Xn;
- values[1] = Yn;
-
-}
-
-/*!
- Default constructor.
-*/
-vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() : vpMomentGravityCenter(){}
-
-/*!
- Outputs the moment's values to a stream.
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenterNormalized& m){
- os << (__FILE__) << std::endl;
- os << "(Xn,Yn) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
- return os;
-}
-
-/*!
-Prints the dependent moments,
-1. centre of gravity
-2. normalized area moment
-*/
-void vpMomentGravityCenterNormalized::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
-
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- os << "Xg = " << momentGravity.get()[0] << "\t" << "Yg = " << momentGravity.get()[1] << std::endl;
- os << "An = " << momentSurfaceNormalized.get()[0] << std::endl;
-}
diff --git a/src/tracking/moments/vpMomentGravityCenterNormalized.h b/src/tracking/moments/vpMomentGravityCenterNormalized.h
deleted file mode 100644
index b3a9caa..0000000
--- a/src/tracking/moments/vpMomentGravityCenterNormalized.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentGravityCenterNormalized.h 4713 2014-03-28 18:02:26Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpMomentGravityCenterNormalized.h
- \brief 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
-*/
-#ifndef __MOMENTGRAVITYNORMALIZED_H__
-#define __MOMENTGRAVITYNORMALIZED_H__
-
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-class vpMomentObject;
-
-/*!
- \class vpMomentGravityCenterNormalized
-
- \ingroup TrackingMoments
-
- \brief Class describing 2D normalized gravity center moment.
-
- Centered and normalized gravity center moment is defined as follows: \f$(x_n,y_n)\f$ where
- \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$.
-
- vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get access to \f$a_n\f$ and on vpMomentGravityCenter to get access to \f$(x_g,y_g)\f$ .
-*/
-class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter {
- public:
- vpMomentGravityCenterNormalized();
- void compute();
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentGravityCenterNormalized";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenterNormalized& v);
- void printDependencies(std::ostream& os) const;
-};
-
-#endif
diff --git a/src/tracking/moments/vpMomentObject.cpp b/src/tracking/moments/vpMomentObject.cpp
deleted file mode 100644
index 52b518d..0000000
--- a/src/tracking/moments/vpMomentObject.cpp
+++ /dev/null
@@ -1,660 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentObject.cpp 5301 2015-02-10 16:36:52Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Object input structure used by moments.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpConfig.h>
-#include <stdexcept>
-
-#include <cmath>
-#include <limits>
-
-#ifdef VISP_HAVE_OPENMP
-#include <omp.h>
-#endif
-#include <cassert>
-
-/*!
- Computes moments from a vector of points describing a polygon.
- The points must be stored in a clockwise order. Used internally.
- \param p : moment order (first index)
- \param q : moment order (second index)
- \param points : vector of points in a clockwise order
-
- \return moment value
-*/
-double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points){
- unsigned int i,k,l;
- double den,mom,s;
- double x_k;
- double x_p_k;
- double y_l;
- double y_q_l;
-
- den = static_cast<double>( (p+q+2)*(p+q+1)*vpMath::comb(p+q,p) );
-
- mom = 0.0;
- for (i=1;i<=points.size()-1;i++)
- {
- s = 0.0;
- x_k=1;
- for (k=0;k<=p;k++)
- {
- y_l=1;
- x_p_k = pow(points[i-1].get_x(), (int)(p-k));
- for (l=0;l<=q;l++)
- {
- y_q_l=pow(points[i-1].get_y(), (int)(q-l));
-
- s += static_cast<double>(
- vpMath::comb(k+l,l)
- *vpMath::comb(p+q-k-l,q-l)
- *x_k
- *x_p_k
- *y_l
- *y_q_l );
-
- y_l*=points[i].get_y();
-
- }
- x_k*=points[i].get_x();
-
- }
-
- s *= ((points[i-1].get_x())*(points[i].get_y())-(points[i].get_x())*(points[i-1].get_y()));
- mom += s;
- }
- mom /= den;
- return(mom);
-}
-
-/*!
- Caching to avoid redundant multiplications.
-
- \param cache : Lookup table that contains the order by order values. For example,
- if the order is 3, cache will contain:
- \code
- 1 x x^2
- y x*y x^2*y
- y^2 x*y^2 x^2*y^2
- \endcode
-
- \param x, y : Coordinates of a point.
-*/
-void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y){
- cache[0]=1;
-
- for(register unsigned int i=1;i<order;i++)
- cache[i]=cache[i-1]*x;
-
- for(register unsigned int j=order;j<order*order;j+=order)
- cache[j]=cache[j-order]*y;
-
- for(register unsigned int j=1;j<order;j++){
- for(register unsigned int i=1;i<order-j;i++){
- cache[j*order+i] = cache[j*order]*cache[i];
- }
- }
-}
-
-/*!
- * Manikandan.B
- * Need to cache intensity along with the coordinates for photometric moments
- */
-void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized) {
-
- cache[0]=IntensityNormalized;
-
- double invIntensityNormalized = 0.;
- if (std::fabs(IntensityNormalized)>=std::numeric_limits<double>::epsilon())
- invIntensityNormalized = 1.0/IntensityNormalized;
-
- for(register unsigned int i=1;i<order;i++)
- cache[i]=cache[i-1]*x;
-
- for(register unsigned int j=order;j<order*order;j+=order)
- cache[j]=cache[j-order]*y;
-
- for(register unsigned int j=1;j<order;j++){
- for(register unsigned int i=1;i<order-j;i++){
- cache[j*order+i] = cache[j*order]*cache[i]*invIntensityNormalized;
- }
- }
-}
-
-
-/*!
- Computes basic moments from a vector of points.
- There are two cases:
- -# Dense case specified by setType(vpMomentObject::DENSE_POLYGON):
- - The points are the vertices describing a closed contour polygon.
- - They must be stored in a clockwise order.
- - The first and the last points should be the same to close the contour.
- -# Discrete case specified by setType(vpMomentObject::DISCRETE)
- - The points will be interpreted as a discrete point cloud.
-
- \param points : Vector of points.
-
- The code below shows how to use this function to consider a dense object defined by a closed contour.
-
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-
-int main()
-{
- // Define the contour of an object by a 5 clockwise vertices on a plane
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
-
- p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
- vec_p.push_back(p);
-
- vpMomentObject obj(4); // Create an image moment object with 4 as maximum order
- obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
- obj.fromVector(vec_p); // Init the dense object with the polygon
-
- return 0;
-}
- \endcode
-
- This other example shows how to consider an object as a discrete set of four points.
-
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpPoint.h>
-
-int main()
-{
- // Define 4 discrete points on a plane
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the 4 points
-
- p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (point 1)
- vec_p.push_back(p);
- p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (point 2)
- vec_p.push_back(p);
- p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (point 3)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (point 4)
- vec_p.push_back(p);
-
- vpMomentObject obj(4); // Create an image moment object with 4 as maximum order
- obj.setType(vpMomentObject::DISCRETE); // The object is constituted by discrete points
- obj.fromVector(vec_p); // Init the dense object with the points
-
- return 0;
-}
- \endcode
-*/
-void vpMomentObject::fromVector(std::vector<vpPoint>& points){
- if(type==vpMomentObject::DENSE_POLYGON){
- if(
- std::abs(points.rbegin()->get_x()-points.begin()->get_x())>std::numeric_limits<double>::epsilon() ||
- std::abs(points.rbegin()->get_y()-points.begin()->get_y())>std::numeric_limits<double>::epsilon()
- ){
- points.resize(points.size()+1);
- points[points.size()-1] = points[0];
- }
- for(register unsigned int j=0;j<order*order;j++){
- values[j]=calc_mom_polygon(j%order,j/order,points);
- }
- }else{
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
- for(register unsigned int i=0;i<points.size();i++){
- cacheValues(cache,points[i].get_x(),points[i].get_y());
- for(register unsigned int j=0;j<order;j++){
- for(register unsigned int k=0;k<order-j;k++){
- values[j*order+k]+=cache[j*order+k];
- }
- }
- }
- }
-}
-
-/*!
- Computes basic moments from an image.
- There is no assumption made about whether the input is dense or discrete but it's more common to use vpMomentObject::DENSE_FULL_OBJECT with this method.
-
- \param image : Image to consider.
- \param threshold : Pixels with a luminance lower than this threshold will be considered.
- \param cam : Camera parameters used to convert pixels coordinates in meters in the image plane.
-
- The code below shows how to use this function.
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpImage.h>
-
-int main()
-{
- vpCameraParameters cam; // Camera parameters used for pixel to meter conversion
- vpImage<unsigned char> I(288, 384); // Image used to define the object
- // ... Initialize the image
-
- unsigned char threshold = 128; // Gray level used to define which part of the image belong to the dense object
-
- vpMomentObject obj(3); // Create an image moment object with 3 as maximum order
- obj.fromImage(I, threshold, cam); // Initialize the object from the image
-
- return 0;
-}
- \endcode
-*/
-
-void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned char threshold, const vpCameraParameters& cam){
-#ifdef VISP_HAVE_OPENMP
- #pragma omp parallel shared(threshold)
- {
- std::vector<double> curvals(order*order);
- curvals.assign(order*order,0.);
- unsigned int i_, j_;
-
- #pragma omp for nowait//automatically organize loop counter between threads
- for(int i=0;i<(int)image.getCols();i++){
- for(int j=0;j<(int)image.getRows();j++){
- i_ = static_cast<unsigned int>(i);
- j_ = static_cast<unsigned int>(j);
- if(image[j_][i_]>threshold){
- double x=0;
- double y=0;
- vpPixelMeterConversion::convertPoint(cam,i_,j_,x,y);
-
- double xval=1.;
- double yval=1.;
- for(register unsigned int k=0;k<order;k++){
- xval=1.;
- for(register unsigned int l=0;l<order-k;l++){
- curvals[(k*order+l)]+=(xval*yval);
- xval*=x;
- }
- yval*=y;
- }
- }
- }
- }
-
- #pragma omp master //only set this variable in master thread
- {
- values.assign(order*order, 0.);
- }
-
- #pragma omp barrier
-
- for(register unsigned int k=0;k<order;k++){
- for(register unsigned int l=0;l<order-k;l++){
- #pragma omp atomic
- values[k*order+l]+= curvals[k*order+l];
- }
- }
-
- }
-#else
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
- for(register unsigned int i=0;i<image.getCols();i++){
- for(register unsigned int j=0;j<image.getRows();j++){
- if(image[j][i]>threshold){
- double x=0;
- double y=0;
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
- cacheValues(cache,x,y);
- for(register unsigned int k=0;k<order;k++){
- for(register unsigned int l=0;l<order-k;l++){
- values[k*order+l]+=cache[k*order+l];
- }
- }
- }
- }
- }
-#endif
-
- //Normalisation equivalent to sampling interval/pixel size delX x delY
- double norm_factor = 1./(cam.get_px()*cam.get_py());
- for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
- *it = (*it) * norm_factor;
- }
-}
-
-/*!
- * Manikandan. B
- * Photometric moments v2
- * Intended to be used by 'vpMomentObject's of type DENSE_FULL_OBJECT
- * @param image : Grayscale image
- * @param cam : Camera parameters (to change to )
- * @param bg_type : White/Black background surrounding the image
- * @param normalize_with_pix_size : This flag if SET, the moments, after calculation are normalized w.r.t pixel size
- * available from camera parameters
- */
-void vpMomentObject::fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam,
- vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
-{
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
-
- // (x,y) - Pixel co-ordinates in metres
- double x=0;
- double y=0;
- //for indexing into cache[] and values[]
- unsigned int idx = 0;
- unsigned int kidx = 0;
-
- double intensity = 0;
- double intensity_white = 0;
-
- //double Imax = static_cast<double>(image.getMaxValue());
- double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
-
- double iscale = 1.0;
- if (flg_normalize_intensity) // This makes the image a probability density function
- iscale = 1.0/Imax;
-
- if (bg_type == vpMomentObject::WHITE) {
- /////////// WHITE BACKGROUND ///////////
- for(register unsigned int j=0;j<image.getRows();j++){
- for(register unsigned int i=0;i<image.getCols();i++){
- x = 0;
- y = 0;
- intensity = (double)(image[j][i])*iscale;
- intensity_white = 1. - intensity;
-
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
- cacheValues(cache,x,y, intensity_white); // Modify 'cache' which has x^p*y^q to x^p*y^q*(1 - I(x,y))
-
- // Copy to "values"
- for(register unsigned int k=0;k<order;k++){
- kidx = k*order;
- for(register unsigned int l=0;l<order-k;l++){
- idx = kidx+l;
- values[idx]+= cache[idx];
- }
- }
- }
- }
- }
- else {
- /////////// BLACK BACKGROUND ///////////
- for(register unsigned int j=0;j<image.getRows();j++){
- for(register unsigned int i=0;i<image.getCols();i++){
- x = 0;
- y = 0;
- intensity = (double)(image[j][i])*iscale;
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
-
- // Cache values for fast moment calculation
- cacheValues(cache,x,y, intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
-
- // Copy to moments array 'values'
- for(register unsigned int k=0;k<order;k++){
- kidx = k*order;
- for(register unsigned int l=0;l<order-k;l++){
- idx = kidx+l;
- values[idx]+= cache[idx];
- }
- }
-
- }
- }
- }
-
- if (normalize_with_pix_size){
- // Normalisation equivalent to sampling interval/pixel size delX x delY
- double norm_factor = 1./(cam.get_px()*cam.get_py());
- for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
- *it = (*it) * norm_factor;
- }
- }
-}
-
-/*!
- Does exactly the work of the default constructor as it existed in the very
- first version of vpMomentObject
- */
-void
-vpMomentObject::init(unsigned int orderinp) {
- order = orderinp + 1;
- type = vpMomentObject::DENSE_FULL_OBJECT;
- flg_normalize_intensity = true; // By default, the intensity values are normalized
- values.resize((order+1)*(order+1));
- values.assign((order+1)*(order+1),0);
-}
-
-/*!
- Helper to copy constructor
- */
-void
-vpMomentObject::init(const vpMomentObject& objin){
- order = objin.getOrder()+1;
- type = objin.getType();
- flg_normalize_intensity = objin.flg_normalize_intensity;
- values.resize(objin.values.size());
- values = objin.values;
-}
-
-/*!
- Default constructor.
- Initializes the object with the maximum used order. You cannot use higher order moments than the order of the moment object.
- The parameter specified is the highest desired included order.
- All orders up to this values will be computed. In other words, a vpMomentObject will compute all \f$ m_{ij} \f$ moments with \f$ i+j \in [0..order] \f$.
-
- \param max_order : Maximum reached order (i+j) to be used. All
- considered i+j will be of order smaller or equal than this
- parameter. For example if this parameter is 5, all moment values of
- order 0 to 5 included will be computed.
-
- Mani : outsourced the constructor work to void init (unsigned int orderinp);
-*/
-vpMomentObject::vpMomentObject(unsigned int max_order)
- : flg_normalize_intensity(true), order(max_order+1), type(vpMomentObject::DENSE_FULL_OBJECT),
- values()
-{
- init(max_order);
-}
-
-/*!
- Copy constructor
- */
-vpMomentObject::vpMomentObject(const vpMomentObject& srcobj)
- : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT),
- values()
-{
- init(srcobj);
-}
-
-/*!
- Returns all basic moment values \f$m_{ij}\f$ with \f$i \in [0:\mbox{order}]\f$ and \f$j \in [0:\mbox{order}]\f$.
-
- \return Vector of moment values. To access \f$m_{ij}\f$, you have to read vpMomentObject::get()[j*(order+1)+i].
-
- For example, if the maximal order is 3, the following values are provided:
- \code
-m00 m10 m20 m01 m11 m21 m02 m12 m12 m30 m03
- \endcode
-
- To access for example to the basic moment m12, you should use this kind of code:
- \code
-vpMomentObject obj(3);
-// ... initialise the object using fromVector() or fromImage()
-std::vector mij = obj.get();
-double m12;
-m12 = mij[2*(obj.getOrder()+1)+1]; // i=1 and j=2
- \endcode
-*/
-const std::vector<double>& vpMomentObject::get() const {
- return values;
-
-}
-
-/*!
- Returns the basic moment value \f$m_{ij}\f$ corresponding to i,j indexes
-
- \param i : First moment index, with \f$i+j \leq order\f$.
- \param j : Second moment index, with \f$i+j \leq order\f$.
-*/
-double vpMomentObject::get(unsigned int i, unsigned int j) const {
- assert(i+j<=getOrder());
- if(i+j>=order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
-
- return values[j*order+i];
-}
-
-/*!
- Sets the basic moment value \f$m_{ij}\f$ corresponding to i,j indexes
- \param i : First moment index, with \f$i+j \leq order\f$.
- \param j : Second moment index, with \f$i+j \leq order\f$.
- \param value_ij : Moment value.
-*/
-void vpMomentObject::set(unsigned int i, unsigned int j, const double& value_ij){
- assert(i+j<=getOrder());
- if(i+j>=order) throw vpException(vpException::badValue,"The requested value cannot be set, you should specify a higher order for the moment object.");
- values[j*order+i] = value_ij;
-}
-
-/*!
- Outputs the basic moment's values \f$m_{ij}\f$ to a stream presented as a matrix.
- The first line corresponds to \f$m_{0[0:order]}\f$, the second one to \f$m_{1[0:order]}\f$
- Values in table corresponding to a higher order are marked with an "x" and not computed.
-
- For example, if the maximal order is 3, the following values are provided:
-
- \code
- m00 m10 m20 m30
- m01 m11 m21 x
- m02 m12 x x
- m03 x x x
- \endcode
-
-*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& m){
- for(unsigned int i = 0;i<m.values.size();i++){
-
- if(i%(m.order)==0)
- os << std::endl;
-
- if((i%(m.order)+i/(m.order))<m.order)
- os << m.values[i] ;
- else
- os << "x";
-
- os << "\t";
-
- }
-
- return os;
-}
-
-/*!
- Outputs the raw moment values \f$m_{ij}\f$ in indexed form.
- The moment values are same as provided by the operator << which outputs x for uncalculated moments.
- */
-void
-vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os) {
- std::vector<double> moment = momobj.get();
- os << std::endl <<"Order of vpMomentObject: "<<momobj.getOrder()<<std::endl;
- // Print out values. This is same as printing using operator <<
- for(unsigned int k=0; k<=momobj.getOrder(); k++) {
- for(unsigned int l=0; l<(momobj.getOrder()+1)-k; l++){
- os << "m[" << l << "," << k << "] = " << moment[k*(momobj.getOrder()+1)+ l] << "\t";
- }
- os << std::endl;
- }
- os <<std::endl;
-}
-
-/*!
- This function returns a vpMatrix of size (order+1, order+1).
-\code
- vpMomentObject obj(8);
- obj.setType(vpMomentObject::DENSE_FULL_OBJECT);
- obj.fromImageWeighted(I, cam, vpMomentObject::BLACK); // cam should have the camera parameters
- vpMatrix Mpq = vpMomentObject::convertTovpMatrix(obj);
-\endcode
- Instead of accessing the moment m21 as obj.get(2,1), you can now do Mpq[2][1].
- This is useful when you want to use the functions available in vpMatrix.
- One use case i see now is to copy the contents of the matrix to a file or std::cout.
- For instance, like
- \code
- // Print to console
- Mpq.maplePrint(std::cout);
- // Or write to a file
- std::ofstream fileMpq("Mpq.csv");
- Mpq.maplePrint(fileMpq);
-\endcode
-
-The output can be copied and pasted to MAPLE as a matrix.
-
-\warning
-The moments that are not calculated have zeros. For instance, for a vpMomentObject of order 8,
-the moment m[7,2] is not calculated. It will have 0 by default. User discretion is advised.
-*/
-vpMatrix
-vpMomentObject::convertTovpMatrix(const vpMomentObject& momobj) {
- std::vector<double> moment = momobj.get();
- unsigned int order = momobj.getOrder();
- vpMatrix M(order+1, order+1);
- for(unsigned int k=0; k<=order; k++) {
- for(unsigned int l=0; l<(order+1)-k; l++){
- M[l][k] = moment[k*(order+1)+ l];
- }
- }
- return M;
-}
-
-/*!
- Nothing to destruct. This will allow for a polymorphic usage
- For instance,
- \code
- vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
- \endcode
- */
-vpMomentObject::~vpMomentObject(){
-// deliberate empty
-}
diff --git a/src/tracking/moments/vpMomentObject.h b/src/tracking/moments/vpMomentObject.h
deleted file mode 100644
index 7b11b82..0000000
--- a/src/tracking/moments/vpMomentObject.h
+++ /dev/null
@@ -1,299 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMomentObject.h 5300 2015-02-10 16:26:32Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Object input structure used by moments.
- *
- * Authors:
- * Filip Novotny
- * Manikandan Bakthavatchalam
- *****************************************************************************/
-/*!
- \file vpMomentObject.h
- \brief Object input structure used by moments.
-*/
-#ifndef __MOMENTOBJECT_H__
-#define __MOMENTOBJECT_H__
-
-#include <visp/vpImage.h>
-#include <visp/vpMoment.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <cstdlib>
-#include <utility>
-
-class vpCameraParameters;
-
-/*!
- \class vpMomentObject
-
- \ingroup TrackingMoments
-
- \brief Class for generic objects.
-
- It contains all basic moments often described by \f$m_{ij}\f$ of order \f$i+j\f$ going from \f$m_{00}\f$ to the order used as parameter in vpMomentObject() constructor.
- All other moments implemented in ViSP (gravity center, alpha orientation, centered moments...) use this moment object as a combination of its different values.
-
- When constructing a vpMomentObject() you need first to specify the maximum used moment order as parameter.
-
- Then there are three ways to initialize a vpMomentObject. Firstly using fromImage() you can considerer
- a dense object \e O defined by a binary image. Secondly, as described in fromVector() you can also define a dense object \e O by a closed contour.
- In these two cases, 2D basic moments are defined by:
- \f[m_{ij} = \int \int_{O} x^i y^j dx dy\f]
-
- Lastly, as presented in fromVector() you can consider a discrete set of \e n points. In that last case, the basic moments are defined by
- \f[m_{ij} = \sum_{k=1}^{n} x_k^i y_k^j \f]
-
- With setType() method you can specify the object type.
-
-
- \attention Be careful with the object order. When you specify a maximum order in the vpMomentObject::vpMomentObject constructor (see its detailed description),
- it will compute all moment orders up to the order you specified. If you want to access the values \f$ m_{ij} \f$ with the vpMomentObject::get method, you can
- do object.get()[j*(order+1)+i].
-
- A few tips about which orders to use in different situations:
- - moment based visual servoing: use vpMomentObject(6). This will compute moment values up to order 6 which will enable vpFeatureMoments up to order 5 which is the maximum order required for common moments.
- - computing gravity center: use vpMomentObject(1). You only need \f$ m_{00},m_{01},m_{10} \f$. You should compute moments up to order 1.
- - computing gravity center interaction matrix (vpFeatureMomentGravityCenter): use vpMomentObject(2). This will compute moment values till order 2 since they are needed for the interaction matrix of vpFeatureMoments of order 1.
-
-
- The following example shows how to create a moment object from 4 discrete points
- locate on a plane one meter in front of the camera. It shows also how to get the basic
- moments that are computed and how to compute other classical moments such as the gravity
- center or the centered moments.
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpPoint.h>
-
-int main()
-{
- // Define an object as 4 clockwise points on a plane (Z=0)
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the 4 points
-
- p.setWorldCoordinates(-0.2, 0.1, 0.0); // values in meters
- vec_p.push_back(p);
- p.setWorldCoordinates(+0.3, 0.1, 0.0); // values in meters
- vec_p.push_back(p);
- p.setWorldCoordinates(+0.2,-0.1, 0.0); // values in meters
- vec_p.push_back(p);
- p.setWorldCoordinates(-0.2,-0.15, 0.0); // values in meters
- vec_p.push_back(p);
-
- // These points are observed by a camera
- vpHomogeneousMatrix cMo(0, 0, 1, 0, 0, 0); // We set the camera to be 1m far the object
- // ... update cMo from an image processing
-
- // Apply the perspective projection to update the points coordinates in the camera plane
- for(unsigned int i=0; i<vec_p.size(); ++i)
- vec_p[i].project(cMo);
-
- std::cout << "Considered points: " << std::endl;
- for(unsigned int i=0; i<vec_p.size(); ++i)
- std::cout << "point " << i << ": " << vec_p[i].get_x() << ", " << vec_p[i].get_y() << std::endl;
-
- // Define an image moment object from the previous points
- vpMomentObject obj(5); // use moments up to order 5
- obj.setType(vpMomentObject::DISCRETE); // initialize the object as constituted by discrete points
- obj.fromVector(vec_p); // init the object from the points
-
- // --- Access the computed moments by querying the moment object
-
- // 1. Getting a vector of doubles
- std::vector<double> moment = obj.get();
- std::cout << std::endl << "Basic moment available (from vector of doubles) " << std::endl;
- for(unsigned int k=0; k<=obj.getOrder(); k++) {
- for(unsigned int l=0; l<(obj.getOrder()+1)-k; l++){
- std::cout << "m" << l << k << "=" << moment[k*(momobj.getOrder()+1)+ l] << "\t";
- }
- std::cout<<std::endl;
- }
-
- // 2. Print the contents of moment object directly
- std::cout << std::endl << "Basic moment available: ";
- std::cout << obj << std::endl;
-
- // 3. Directly indexing the moment object
- std::cout << std::endl << "Direct acces to some basic moments: " << std::endl;
- std::cout << "m00: " << obj.get(0, 0) << std::endl;
- std::cout << "m10: " << obj.get(1, 0) << std::endl;
- std::cout << "m01: " << obj.get(0, 1) << std::endl;
- std::cout << "m22: " << obj.get(2, 2) << std::endl;
- std::cout << "m20: " << obj.get(2, 0) << std::endl;
- std::cout << "m02: " << obj.get(0, 2) << std::endl;
-
- // Get common moments computed using basic moments
- double m00 = vpMomentCommon::getSurface(obj); // surface = m00
- double alpha = vpMomentCommon::getAlpha(obj); // orientation
- std::vector<double> mu_3 = vpMomentCommon::getMu3(obj); // centered moment up to 3rd order
-
- std::cout << std::endl << "Common moments computed using basic moments:" << std::endl;
- std::cout << "Surface: " << m00 << std::endl;
- std::cout << "Alpha: " << alpha << std::endl;
- std::cout << "Centered moments (mu03, mu12, mu21, mu30): ";
- for(unsigned int i=0; i<mu_3.size(); ++i)
- std::cout << mu_3[i] << " ";
- std::cout << std::endl;
-
- return 0;
-}
- \endcode
-
- This example produces the following results:
- \code
-Considered points:
-point 0: -0.2, 0.1
-point 1: 0.3, 0.1
-point 2: 0.2, -0.1
-point 3: -0.2, -0.15
-
-Basic moment available (from vector of doubles):
-m00=4 m10=0.1 m20=0.21 m30=0.019 m40=0.0129 m50=0.00211
-m01=-0.05 m11=0.02 m21=0.003 m31=0.0023 m41=0.00057
-m02=0.0525 m12=-0.0015 m22=0.0026 m32=9e-05
-m03=-0.002375 m13=0.000575 m23=-4.5e-05
-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
-
-Direct acces to some basic moments:
-m00: 4
-m10: 0.1
-m01: -0.05
-m22: 0.0026
-m20: 0.21
-m02: 0.0525
-
-Common moments computed using basic moments:
-Surface: 0.259375
-Alpha: 0.133296
-Centered moments (mu03, mu12, mu21, mu30): 0.003375 0.0045625 -0.00228125 -0.000421875
- \endcode
-
- Note that in the continuous case, the moment object \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
- In the discrete case, it is the number of discrete points \f$n\f$.
-*/
-class VISP_EXPORT vpMomentObject{
-public:
-
- /*!
- Type of object that will be considered.
- */
- 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) describing a polygon. It will be treated as dense. */
- DISCRETE = 2, /*!< A cloud of points. Treated as discrete. */
- } vpObjectType;
-
- /*!
- Type of camera image background.
- */
- typedef enum{
- BLACK = 0, /*! Black background */
- WHITE = 1, /*! No functionality as of now */
- } vpCameraImgBckGrndType;
-
- bool flg_normalize_intensity; // To scale the intensity of each individual pixel in the image by the maximum intensity value present in it
-
- // Constructor helpers
- void init(unsigned int orderinp);
- void init(const vpMomentObject& objin);
- // Constructors
- vpMomentObject(unsigned int order);
- vpMomentObject(const vpMomentObject& srcobj);
- /*!
- Virtual destructor to allow polymorphic usage.
- For instance,
- \code
- vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
- \endcode
- */
- virtual ~vpMomentObject();
-
- void fromImage(const vpImage<unsigned char>& image,unsigned char threshold, const vpCameraParameters& cam); // Binary version
- void fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam, vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size = true); // Photometric version
-
- void fromVector(std::vector<vpPoint>& points);
- const std::vector<double>& get() const;
- double get(unsigned int i,unsigned int j) const;
-
- /*!
- \return The type of object that is considered.
- */
- vpObjectType getType() const {return type;}
-
- /*!
- \return The maximal order. The basic moments \f$m_{ij}\f$ that will be computed
- are for \f$i+j \in [0:\mbox{order}]\f$.
- */
- unsigned int getOrder() const {return order-1;}
-
- /*!
- Specifies the type of the input data.
- \param input_type : An input type.
- */
- void setType(vpObjectType input_type){this->type=input_type;}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& v);
- /*!
- Outputs raw moments in indexed form like m[1,1] = value of moment m11
- \param momobj : A vpMomentObject
- \param os : Output stream.
- */
- static void printWithIndices(const vpMomentObject& momobj, std::ostream& os);
- /*!
- Converts the raw moments contained in vpMomentObject to a vpMatrix
- \param momobj : A vpMomentObject
- */
- static vpMatrix convertTovpMatrix(const vpMomentObject& momobj);
-
-protected:
- unsigned int order;
- vpObjectType type;
- std::vector<double> values;
- void set(unsigned int i, unsigned int j, const double& value_ij);
- void cacheValues(std::vector<double>& cache,double x, double y);
-private:
- void cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized);
- double calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points);
-
-};
-
-#endif
diff --git a/src/tracking/moving-edges/vpMe.cpp b/src/tracking/moving-edges/vpMe.cpp
deleted file mode 100644
index bdc0264..0000000
--- a/src/tracking/moving-edges/vpMe.cpp
+++ /dev/null
@@ -1,496 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMe.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- * Andrew Comport
- *
- *****************************************************************************/
-
-/*!
- \file vpMe.cpp
- \brief Moving edges
-*/
-
-
-
-#include <visp/vpMe.h>
-#include <visp/vpColVector.h>
-#include <visp/vpMath.h>
-#include <stdlib.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-
-struct point
-{
- double x ;
- double y ;
-} ;
-
-struct droite
-{
- double a ;
- double b ;
- double c ;
-} ;
-
-
-
-
-
-
-
-template <class Type>
-inline void
-permute(Type & a, Type & b)
-{
- Type t = a;
- a = b;
- b = t;
-}
-
-static droite
-droite_cartesienne(point P, point Q)
-{
- droite PQ;
-
- PQ.a = P.y-Q.y;
- PQ.b = Q.x-P.x;
- PQ.c = Q.y*P.x - Q.x*P.y;
-
- return(PQ);
-}
-
-
-
-static point
-point_intersection(droite D1, droite D2)
-{
- point I;
- double det; // determinant des 2 vect.normaux
-
- det = (D1.a*D2.b - D2.a*D1.b); // interdit D1,D2 paralleles
- I.x = (D2.c*D1.b - D1.c*D2.b)/det;
- I.y = (D1.c*D2.a - D2.c*D1.a)/det;
-
- return(I);
-}
-
-static void
-recale(point & P,
- double Xmin, double Ymin, double Xmax, double Ymax)
-{
- if(vpMath::equal(P.x,Xmin))
- P.x=Xmin; // a peu pres => exactement !
- if(vpMath::equal(P.x,Xmax))
- P.x=Xmax;
-
- if(vpMath::equal(P.y,Ymin))
- P.y=Ymin;
- if(vpMath::equal(P.y,Ymax))
- P.y=Ymax;
-}
-
-
-static void
-permute(point &A, point &B)
-{
- point C ;
-
- if (A.x>B.x) // fonction sans doute a tester...
- {
- C = A ;
- A = B ;
- B = C ;
- }
-}
-
-
-// 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
-{
- droite AB, D[4];
- D[0].a = 1; D[0].b = 0; D[0].c = -Xmin;
- D[1].a = 1; D[1].b = 0; D[1].c = -Xmax;
- D[2].a = 0; D[2].b = 1; D[2].c = -Ymin;
- D[3].a = 0; D[3].b = 1; D[3].c = -Ymax;
-
- point P[2];
- P[0]=A; P[1]=B;
- int code_P[2], // codes de P[n]
- i, bit_i, // i -> (0000100...)
- n;
-
- AB = droite_cartesienne(A,B);
-
- for ( ; ; ) // 2 sorties directes internes
- {
- // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
- // ================================================
- for(n=0; n<2; n++)
- {
- code_P[n] = 0000;
-
- if( P[n].x < Xmin )
- code_P[n] |= 1; // positionne bit0
- if( P[n].x > Xmax )
- code_P[n] |= 2; // .. bit1
- if( P[n].y < Ymin )
- code_P[n] |= 4; // .. bit2
- if( P[n].y > Ymax )
- code_P[n] |= 8; // .. bit3
- }
-
-
- // 2 CAS OU L'ON PEUT CONCLURE => sortie
- // =====================================
- if((code_P[0] | code_P[1])==0000) // Aucun bit a 1
- /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
- { if(P[0].x < P[1].x) // Rend le couple de points
- { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
- else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
- */
- {
- Ac=P[0]; Bc=P[1];
- if(vpMath::equal(Ac.x,Bc.x) && vpMath::equal(Ac.y,Bc.y))
- return(false); // AB = 1 point = invisible
- else
- return(true); // Partie de AB clippee visible!
- }
-
- if((code_P[0] & code_P[1])!=0000) // au moins 1 bit commun
- {
- return(false); // AB completement invisible!
- }
-
-
- // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
- // - clippe le point P[n] qui sort de la fen�tre (coupe Droite i)
- // - reboucle avec le nouveau couple de points
- // ================================================================
- if(code_P[0] != 0000)
- {
- n=0; // c'est P[0] qu'on clippera
- for(i=0,bit_i=1; !(code_P[0] & bit_i); i++,bit_i<<=1){;}
- }
- else
- {
- n=1; // c'est P[1] qu'on clippera
- for(i=0,bit_i=1; !(code_P[1] & bit_i); i++,bit_i<<=1){;}
- }
-
- P[n] = point_intersection(AB,D[i]); // clippe le point concerne
-
-
- // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
- // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
- // =======================================================
- recale(P[n], Xmin,Ymin,Xmax,Ymax);
-
- }
-
-}
-
-
-// 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)
-{
-
- if(Q.x < P.x) // tri le couple de points
- permute(P,Q); // selon leur abscisse x
-
-
-
- recale(P, Xmin,Ymin,Xmax,Ymax); // permet des calculs de S_relative
- recale(Q, Xmin,Ymin,Xmax,Ymax); // moins approximatifs.
-
- //if(P.x==Xmin && Q.x==Xmax)
- if((std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon()))
- return( fabs(Ymax+Ymin-P.y-Q.y) );
-
- //if( (P.y==Ymin && Q.y==Ymax) ||
- // (Q.y==Ymin && P.y==Ymax))
- if( ( (std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) )
- ||
- ( (std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) )
- return( fabs(Xmax+Xmin-P.x-Q.x) );
-
- //if( P.x==Xmin && Q.y==Ymax )
- if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
- &&
- std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon() )
- return( 1-(Ymax-P.y)*(Q.x-Xmin) );
- //if( P.x==Xmin && Q.y==Ymin )
- if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
- &&
- std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon() )
- return( 1-(P.y-Ymin)*(Q.x-Xmin) );
- //if( P.y==Ymin && Q.x==Xmax )
- if( std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()
- && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon() )
- return( 1-(Xmax-P.x)*(Q.y-Ymin) );
- //if( P.y==Ymax && Q.x==Xmax )
- if( std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()
- && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon())
- return( 1-(Xmax-P.x)*(Ymax-Q.y) );
-
-
- printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n",
- P.x,P.y,Q.x,Q.y,Xmin,Ymin,Xmax,Ymax);
- exit(-1); // DEBUG Stoppe net l'execution
-}
-
-
-static void
-calcul_masques(vpColVector &angle, // definitions des angles theta
- unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
- vpMatrix *M) // resultat M[theta](n,n)
-{
- // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)
-
- unsigned int i_theta, // indice (boucle sur les masques)
- i,j; // indices de boucle sur M(i,j)
- double X,Y, // point correspondant/centre du masque
- theta, cos_theta, sin_theta, tan_theta,
- moitie = ((double)n)/2.0; // moitie REELLE du masque
- point P1,Q1,P,Q; // clippe Droite(theta) P1,Q1 -> P,Q
- int sgn; // signe de M(i,j)
- double v; // ponderation de M(i,j)
-
- unsigned int nb_theta = angle.getRows() ;
-
- for(i_theta=0; i_theta<nb_theta; i_theta++)
- {
- theta = M_PI/180*angle[i_theta]; // indice i -> theta(i) en radians
- // angle[] dans [0,180[
- cos_theta = cos(theta); // vecteur directeur de l'ECM
- sin_theta = sin(theta); // associe au masque
-
- // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
- // =========================================================
- //if( angle[i_theta]==90 ) // => tan(theta) infinie !
- if( std::fabs(angle[i_theta]-90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.)*std::numeric_limits<double>::epsilon() ) // => tan(theta) infinie !
- {
- P1.x=0; P1.y=-(int)n;
- Q1.x=0; Q1.y= n;
- }
- else
- {
- tan_theta = sin_theta/cos_theta; // pente de la droite D(theta)
- P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
- Q1.x=n; Q1.y=tan_theta*n;
- }
-
- // CALCULE MASQUE M(theta)
- // ======================
- M[i_theta].resize(n,n); // allocation (si necessaire)
-
- for(i=0,Y=-moitie+0.5 ; i<n ; i++,Y++)
- {
- for(j=0,X=-moitie+0.5 ; j<n ; j++,X++)
- {
- // produit vectoriel dir_droite*(X,Y)
- sgn = vpMath::sign(cos_theta*Y - sin_theta*X);
-
- // Resultat = P,Q
- if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
- {
- // v dans [0,1]
- v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
- }
- else
- v=1; // PQ ne coupe pas le pixel(i,j)
-
- M[i_theta][i][j] = vpMath::round(100*sgn*v);
-
- // 2 chiffres significatifs
- // M(i,j) sans incorporer le coef a
- }
- }
- }
-
-}
-
-#endif
-
-/*!
- Initialise the array of matrices with the defined size and the number of
- matrices to create.
-
-*/
-void
-vpMe::initMask()
-{
-
- if (mask != NULL)
- delete [] mask;
-
- mask = new vpMatrix[n_mask] ;
-
- vpColVector angle(n_mask) ;
-
- unsigned int angle_pas ;
- angle_pas = 180 / n_mask ;
-
- unsigned int k =0 ;
- for (unsigned int i = 0 ; /* i < 180, */ k < n_mask ; i += angle_pas)
- angle[k++] = i ;
-
- calcul_masques(angle, mask_size, mask ) ;
-
-}
-
-
-
-void
-vpMe::print( )
-{
-
- std::cout<< std::endl ;
- std::cout<<"Moving edges settings " <<std::endl ;
- std::cout<< std::endl ;
- std::cout<<" Size of the convolution masks...."<<mask_size<<"x"<<mask_size<<" pixels"<<std::endl ;
- std::cout<<" Number of masks.................."<<n_mask<<" "<<std::endl ;
- std::cout<<" Query range +/- J................"<<range<<" pixels "<<std::endl ;
- std::cout<<" Likelihood test ratio............"<<threshold<<std::endl ;
- std::cout<<" Contrast tolerance +/-..........."<< mu1 * 100<<"% and "<<mu2 * 100<<"% "<<std::endl ;
- std::cout<<" Sample step......................"<<sample_step<<" pixels"<<std::endl ;
- std::cout<<" Strip............................"<<strip<<" pixels "<<std::endl ;
- std::cout<<" Min_Samplestep..................."<<min_samplestep<<" pixels "<<std::endl ;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- std::cout<<" Aberration......................."<<aberration<< std::endl ;
- std::cout<<" init_aberration.................."<<init_aberration<<std::endl ;
- #endif
-}
-
-vpMe::vpMe()
- : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
- range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
- n_mask(180), strip(2), mask(NULL)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , aberration(2), init_aberration(5.)
-#endif
-{
- //ntotal_sample = 0; // not sure that it is used
- //points_to_track = 500; // not sure that it is used
- anglestep = (180 / n_mask) ;
-
- initMask() ;
-}
-
-vpMe::vpMe(const vpMe &me)
- : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
- range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
- n_mask(180), strip(2), mask(NULL)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , aberration(2), init_aberration(5.)
-#endif
-{
- *this = me;
-}
-
-const
-vpMe& vpMe::operator=(const vpMe &me)
-{
- if (mask != NULL) {
- delete [] mask;
- mask = NULL ;
- }
- threshold = me.threshold ;
- mu1 = me.mu1 ;
- mu2 = me.mu2 ;
- min_samplestep = me.min_samplestep ;
- anglestep = me.anglestep ;
- mask_size = me.mask_size ;
- n_mask = me.n_mask ;
- mask_sign = me.mask_sign ;
- range = me.range ;
- sample_step = me.sample_step ;
- ntotal_sample = me.ntotal_sample;
- points_to_track = me.points_to_track;
- strip = me.strip ;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- aberration = me.aberration ;
- init_aberration = me.init_aberration;
- #endif
-
- initMask() ;
- return *this;
-}
-
-vpMe::~vpMe()
-{
- if (mask != NULL)
- {
- delete [] mask ;
- mask = NULL;
- }
-}
-
-
-void
-vpMe::setMaskNumber(const unsigned int &n)
-{
- n_mask = n ;
- anglestep = 180 / n_mask ;
- initMask() ;
-}
-
-void
-vpMe::setMaskSize(const unsigned int &s)
-{
- mask_size = s ;
- initMask() ;
-}
-
-
-
-
diff --git a/src/tracking/moving-edges/vpMe.h b/src/tracking/moving-edges/vpMe.h
deleted file mode 100644
index d51f7f4..0000000
--- a/src/tracking/moving-edges/vpMe.h
+++ /dev/null
@@ -1,346 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMe.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- * Andrew Comport
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMe.h
- \brief Moving edges
-*/
-
-// ====================================================================
-/*!
- \class vpMe
- \ingroup TrackingImageME
-
- \brief Contains predetermined masks for sites and holds moving edges
- tracking parameters.
-
-*/
-// ====================================================================
-// Andrew Comport added functionality - replaced points_to_track with
-// sample step.
-
-#ifndef vpMe_H
-#define vpMe_H
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpImage.h>
-
-
-class VISP_EXPORT vpMe
-{
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
-#else
-private:
-#endif
- double threshold ;//! Likelihood ratio threshold
- double mu1; //! Contrast continuity parameter (left boundary)
- double mu2; //! Contrast continuity parameter (right boundary)
- double min_samplestep;
- unsigned int anglestep;
- int mask_sign;
- unsigned int range; //! Seek range - on both sides of the reference pixel
- double sample_step; //! Distance between sampled points (in pixels)
- int ntotal_sample;
- int points_to_track;
- //! convolution masks' size in pixels (masks are square), \warning should not be public, use setMaskSize() and getMaskSize() instead (kept public for compatibility reasons).
- unsigned int mask_size;
- //! the number of convolution masks available for tracking ; defines resolution. \warning Should not be public, use setMaskNumber() and getMaskNumber() instead (kept public for compatibility reasons).
- unsigned int n_mask;
- //strip: defines a "security strip" such that Seek_Extremities()
- //cannot return a new extremity which is too close to the
- //frame borders which may cause Get_Sampling_Grid to refuse
- //the that extremity
- int strip;
- //int graph ;
- vpMatrix *mask ; //! Array of matrices defining the different masks (one for every angle step).
-
-public:
- vpMe() ;
- vpMe(const vpMe &me) ;
- virtual ~vpMe() ;
-
- const vpMe& operator=(const vpMe &me);
-
-
- void initMask() ;// convolution masks - offset computation
- void checkSamplestep(double &a) { if(a < min_samplestep) a = min_samplestep ; }
- void print( ) ;
-
- /*!
- Get the matrix of the mask.
-
- \return the value of mask.
- */
- inline vpMatrix* getMask() const { return mask; }
-
- /*!
- Set the number of mask applied to determine the object contour. The number of mask determines the precision of
- the normal of the edge for every sample. If precision is 2deg, then there
- are 360/2 = 180 masks.
-
- \param a : the number of mask.
- */
- void setMaskNumber(const unsigned int &a) ;
-
- /*!
- Return the number of mask applied to determine the object contour. The number of mask determines the precision of
- the normal of the edge for every sample. If precision is 2deg, then there
- are 360/2 = 180 masks.
-
- \return the current number of mask.
- */
- inline unsigned int getMaskNumber() const { return n_mask; }
-
- /*!
- Set the mask sign.
-
- \param a : new mask sign.
- */
- void setMaskSign(const int &a){ mask_sign = a ; }
-
- /*!
- Return the mask sign.
-
- \return Value of mask_sign.
- */
- inline int getMaskSign() const { return mask_sign; }
-
- /*!
- Set the mask size (in pixel) used to compute the image gradient and determine the object contour.
- The mask size defines the size of the convolution mask used to detect an edge.
-
- \param a : new mask size.
- */
- void setMaskSize(const unsigned int &a);
-
- /*!
- Return the actual mask size (in pixel) used to compute the image gradient and determine the object contour.
- The mask size defines the size of the convolution mask used to detect an edge.
-
- \return the current mask size.
- */
- inline unsigned int getMaskSize() const { return mask_size; }
-
- /*!
- Set the minimum image contrast allowed to detect a contour.
-
- \param mu_1 : new mu1.
- */
- void setMu1(const double &mu_1) { this->mu1 = mu_1 ; }
-
- /*!
- Get the minimum image contrast allowed to detect a contour.
-
- \return Value of mu1.
- */
- inline double getMu1() const { return mu1; }
-
- /*!
- Set the maximum image contrast allowed to detect a contour.
-
- \param mu_2 : new mu2.
- */
- void setMu2(const double &mu_2) { this->mu2 = mu_2 ; }
-
- /*!
- Get the maximum image contrast allowed to detect a contour.
-
- \return Value of mu2.
- */
- inline double getMu2() const { return mu2; }
-
- /*!
- Set how many discretizied points are used to track the feature.
-
- \param nb : new total number of sample.
- */
- void setNbTotalSample(const int &nb) { ntotal_sample = nb; }
-
- /*!
- Get how many discretizied points are used to track the feature.
-
- \return Value of ntotal_sample.
- */
- inline int getNbTotalSample() const { return ntotal_sample; }
-
- /*!
- Set the number of points to track.
-
- \param n : new number of points to track.
-
- \warning This method is useful only for the vpMeNurbsTracker.
- */
- void setPointsToTrack(const int &n) { points_to_track = n; }
-
- /*!
- Return the number of points to track.
-
- \return Value of points_to_track.
- */
- inline int getPointsToTrack() const { return points_to_track ; }
-
- /*!
- Set the seek range on both sides of the reference pixel.
-
- \param r : new range.
- */
- void setRange(const unsigned int &r) { range = r ; }
-
- /*!
- Return the seek range on both sides of the reference pixel.
-
- \return Value of range.
- */
- inline unsigned int getRange() const { return range; }
-
- /*!
- Set the angle step.
-
- \param a : new angle step.
- */
- void setAngleStep(const unsigned int &a) { anglestep = a ; }
-
- /*!
- Return the angle step.
-
- \return Value of anglestep.
- */
- inline unsigned int getAngleStep() const { return anglestep; }
-
- /*!
- Set the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
-
- \param min : new minimum sample step.
- */
- void setMinSampleStep(const double &min) { min_samplestep = min ; }
-
- /*!
- Get the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
-
- \return Value of min_samplestep.
- */
- inline double getMinSampleStep() const { return min_samplestep; }
-
- /*!
- Set the minimum distance in pixel between two discretized points.
-
- \param s : new sample_step.
- */
- void setSampleStep(const double &s) { sample_step = s ; }
-
- /*!
- Get the minimum distance in pixel between two discretized points.
-
- \return Value of sample_step.
- */
- inline double getSampleStep() const { return sample_step ; }
-
- /*!
- Set the number of pixels that are ignored around the image borders.
-
- \param a : new strip.
- */
- void setStrip(const int &a) { strip = a ; }
-
- /*!
- Get the number of pixels that are ignored around the image borders.
-
- \return the value of strip.
- */
- inline int getStrip() const { return strip; }
-
- /*!
- Set the likelihood threshold used to determined if the moving edge is valid or not.
-
- \param t : new threshold.
- */
- void setThreshold(const double &t) { threshold = t ; }
-
- /*!
- Return the likelihood threshold used to determined if the moving edge is valid or not.
-
- \return Value of threshold.
- */
- inline double getThreshold() const { return threshold; }
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated Attributes
- */
-public:
- double aberration;
- double init_aberration;
-
- /*!
- @name Deprecated Functions
- */
-
- /*!
- \deprecated since it wasn't used.\n \n
- Set aberration value.
-
- \param a : new value.
- */
- vp_deprecated void setAberration(const double &a) { aberration = a ; }
-
- /*!
- \deprecated since it wasn't used. \n \n
- Set initial aberration value.
-
- \param a : new value.
- */
- vp_deprecated void setInitAberration(const double &a) { init_aberration = a ; }
-
- /*!
- \deprecated Use setMinSampleStep() instead.
- */
- vp_deprecated void setMinSamplestep(const double &min) { min_samplestep = min ; } //Little mistake in the method name "step" should be "Step"
-#endif
-};
-
-
-#endif
-
-
diff --git a/src/tracking/moving-edges/vpMeEllipse.cpp b/src/tracking/moving-edges/vpMeEllipse.cpp
deleted file mode 100644
index 2ee89ea..0000000
--- a/src/tracking/moving-edges/vpMeEllipse.cpp
+++ /dev/null
@@ -1,1410 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeEllipse.cpp 4706 2014-03-28 07:52:00Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpMeEllipse.h>
-
-#include <visp/vpMe.h>
-#include <visp/vpRobust.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpImagePoint.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-
-void computeTheta(double &theta, vpColVector &K, vpImagePoint iP);
-/*!
- Computes the \f$ \theta \f$ angle which represents the angle between the tangente to the curve and the i axis.
-
- \param theta : The computed value.
- \param K : The parameters of the ellipse.
- \param iP : the point belonging th the ellipse where the angle is computed.
-*/
-void
-computeTheta(double &theta, vpColVector &K, vpImagePoint iP)
-{
-
- double i = iP.get_i();
- double j = iP.get_j();
-
- double A = 2*i+2*K[1]*j + 2*K[2] ;
- double B = 2*K[0]*j + 2*K[1]*i + 2*K[3];
-
- theta = atan2(A,B) ; //Angle between the tangente and the i axis.
-
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < 0) { theta += M_PI ; }
-}
-
-
-/*!
- 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.), mu11(0.), mu20(0.), mu02(0.),
- m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
- thresholdWeight(0.2), expecteddensity(0.), circle(false)
-{
- vpCDEBUG(1) << "begin vpMeEllipse::vpMeEllipse() " << std::endl ;
-
- // redimensionnement du vecteur de parametre
- // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
-
- K.resize(5) ;
-
- //j1 = j2 = i1 = i2 = 0 ;
- iP1.set_i(0);
- iP1.set_j(0);
- iP2.set_i(0);
- iP2.set_j(0);
-
- vpCDEBUG(1) << "end vpMeEllipse::vpMeEllipse() " << std::endl ;
-}
-
-/*!
- Copy constructor.
-*/
-vpMeEllipse::vpMeEllipse(const vpMeEllipse &meellipse)
- : vpMeTracker(meellipse), K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2*M_PI),
- ce(0.), se(0.), angle(), m00(0.), mu11(0.), mu20(0.), mu02(0.),
- m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
- thresholdWeight(0.2), expecteddensity(0.), circle(false)
-{
- K = meellipse.K;
- iPc = meellipse.iPc;
- a = meellipse.a;
- b = meellipse.b;
- e = meellipse.e;
-
- iP1 = meellipse.iP1;
- iP2 = meellipse.iP2;
- alpha1 = meellipse.alpha1;
- alpha2 = meellipse.alpha2;
- ce = meellipse.ce;
- se = meellipse.se;
-
- angle = meellipse.angle;
-
- m00 = meellipse.m00;
- mu11 = meellipse.mu11;
- mu20 = meellipse.mu20;
- mu02 = meellipse.mu02;
- m10 = meellipse.m10;
- m01 = meellipse.m01;
- m11 = meellipse.m11;
- m02 = meellipse.m02;
- m20 = meellipse.m20;
- thresholdWeight = meellipse.thresholdWeight;
-
- circle = meellipse.circle;
- expecteddensity = meellipse.expecteddensity;
-}
-
-/*!
- Basic destructor.
-*/
-vpMeEllipse::~vpMeEllipse()
-{
- vpCDEBUG(1) << "begin vpMeEllipse::~vpMeEllipse() " << std::endl ;
-
- list.clear();
- angle.clear();
-
- vpCDEBUG(1) << "end vpMeEllipse::~vpMeEllipse() " << 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.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMeEllipse::sample(const vpImage<unsigned char> & I)
-{
- vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
-
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- int height = (int)I.getHeight() ;
- int width = (int)I.getWidth() ;
-
- double n_sample;
-
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "In vpMeEllipse::sample: " ;
- std::cout << "function called with sample step = 0" ;
- //return fatalError ;
- }
-
- double j, i;//, j11, i11;
- vpImagePoint iP11;
- j = i = 0.0 ;
-
- double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
- vpColor col = vpColor::red ;
- getParameters() ;
-
- // Delete old list
- list.clear();
-
- angle.clear();
-
- // sample positions
- double k = alpha1 ;
- while (k<alpha2)
- {
-// j = a *cos(k) ; // equation of an ellipse
-// i = b *sin(k) ; // equation of an ellipse
-
- j = a *sin(k) ; // equation of an ellipse
- i = b *cos(k) ; // equation of an ellipse
-
- // (i,j) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
-// iP11.set_j( iPc.get_j() + ce *j - se *i );
-// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
-
- iP11.set_j( iPc.get_j() + ce *j + se *i );
- iP11.set_i( iPc.get_i() - se *j + ce *i );
-
- vpDisplay::displayCross(I, iP11, 5, col) ;
-
- double theta ;
- computeTheta(theta, K, iP11) ;
-
- // If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
- {
- vpMeSite pix ;
- pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
- pix.setDisplay(selectDisplay) ;
- pix.setState(vpMeSite::NO_SUPPRESSION);
-
- if(vpDEBUG_ENABLE(3))
- {
- vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
- }
- list.push_back(pix);
- angle.push_back(k);
- }
- k += incr ;
-
- }
- vpMeTracker::initTracking(I) ;
-
- n_sample = (unsigned int)list.size() ;
-
- vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
- vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
-}
-
-
-/*!
-
- Resample the ellipse if the number of sample is less than 90% of the
- expected value.
-
- \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
- and the parameter which indicates the number of degrees between
- two points (vpMe::sample_step).
-
- \param I : Image in which the ellipse appears.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMeEllipse::reSample(const vpImage<unsigned char> &I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- unsigned int n = numberOfSignal() ;
- expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
- if ((double)n<0.9*expecteddensity){
- sample(I) ;
- }
-}
-
-
-/*!
- Computes the coordinates of the ellipse center, length of the semiminor axis \f$ a \f$, the length of the semimajor axis \f$ b \f$ and,
- \f$ e \f$ which is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
-
- All those computations are made thanks to the parameters \f$ K = {K_0, ..., K_4} \f$.
-*/
-void
-vpMeEllipse::getParameters()
-{
-
- double k[6] ;
- for (unsigned int i=0 ; i < 5 ; i++)
- k[i+1] = K[i] ;
- k[0] = 1 ;
-
- double d = k[2]*k[2] - k[0]*k[1];
-
- iPc.set_i( (k[1] * k[3] - k[2] * k[4]) / d );
- iPc.set_j( (k[0] * k[4] - k[2] * k[3]) / d );
-
- double sq = sqrt(vpMath::sqr(k[1]-k[0]) + 4.0*vpMath::sqr(k[2])) ;
- if (circle ==true)
- {
- e = 0 ;
- }
- else
- {
- e = (k[1] - k[0] + sq) / (2.0*k[2]);
- e = (-1/e) ;
-
- e = atan(e) ;
- }
-
- if(e < 0.0) e += M_PI ;
-
- ce = cos(e) ;
- se = sin(e) ;
-
- double num = 2.0*(k[0]*iPc.get_i()*iPc.get_i() + 2.0*k[2]*iPc.get_j()*iPc.get_i() + k[1]*iPc.get_j()*iPc.get_j() - k[5]) ;
- double a2 = num / (k[0] + k[1] + sq ) ;
- double b2 = num / (k[0] + k[1] - sq ) ;
-
- a = sqrt( a2 ) ;
- b = sqrt( b2 ) ;
-
-}
-
-/*!
- Print the parameters \f$ K = {K_0, ..., K_4} \f$ and the coordinates of the ellipse center.
-*/
-void
-vpMeEllipse::printParameters()
-{
- std::cout << "K" << std::endl ;
- std::cout << K.t() ;
- std::cout << iPc << std::endl ;
-}
-
-/*!
- Computes the \f$ alpha \f$ angle of the two points and store them into alpha1 for the smallest and alpha2 for the biggest.
-
- \note this function is useful only during the initialization.
-
- \param pt1 : First point whose \f$ alpha \f$ angle is computed.
- \param pt2 : Second point whose \f$ alpha \f$ angle is computed.
-*/
-void
-vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
-{
-
- getParameters() ;
- double j1, i1, j11, i11;
- j1 = i1 = 0.0 ;
-
- int number_of_points = 2000 ;
- double incr = 2 * M_PI / number_of_points ; // angle increment
-
- double dmin1 = 1e6 ;
- double dmin2 = 1e6 ;
-
- double k = 0 ;
- while(k < 2*M_PI) {
-
-// j1 = a *cos(k) ; // equation of an ellipse
-// i1 = b *sin(k) ; // equation of an ellipse
-
- j1 = a *sin(k) ; // equation of an ellipse
- i1 = b *cos(k) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
-// j11 = iPc.get_j() + ce *j1 - se *i1 ;
-// i11 = iPc.get_i() -( se *j1 + ce *i1) ;
-
- j11 = iPc.get_j() + ce *j1 + se *i1 ;
- i11 = iPc.get_i() - se *j1 + ce *i1 ;
-
- double d = vpMath::sqr(pt1.get_i()-i11) + vpMath::sqr(pt1.get_j()-j11) ;
- if (d < dmin1)
- {
- dmin1 = d ;
- alpha1 = k ;
- }
- d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
- if (d < dmin2)
- {
- dmin2 = d ;
- alpha2 = k ;
- }
- k += incr ;
- }
- //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
-
- if (alpha2 < alpha1)
- alpha2 += 2 * M_PI;
- //else if (alpha2 == alpha1)
- else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits<double>::epsilon())
- alpha2 += 2 * M_PI;
-
- //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
-
-
-}
-
-
-/*!
- 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;
- double theta;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- vpImagePoint iP;
- iP.set_i(p_me.ifloat);
- iP.set_j(p_me.jfloat);
- computeTheta(theta, K, iP) ;
- p_me.alpha = theta ;
- *it = p_me;
- }
-}
-
-/*!
- Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
-*/
-void
-vpMeEllipse::suppressPoints()
-{
- // Loop through list of sites to track
- std::list<vpMeSite>::iterator itList = list.begin();
- for(std::list<double>::iterator it=angle.begin(); it!=angle.end(); ){
- vpMeSite s = *itList;//current reference pixel
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- {
- itList = list.erase(itList) ;
- it = angle.erase(it);
- }
- else
- {
- ++itList;
- ++it;
- }
- }
-}
-
-
-/*!
- Seek along the ellipse edge defined by its equation, the two extremities of
- the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.
-
- \param I : Image in which the ellipse appears.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
-
- vpImagePoint ip;
-
- unsigned int memory_range = me->getRange() ;
- me->setRange(2);
-
- double memory_mu1 = me->getMu1();
- me->setMu1(0.5);
-
- double memory_mu2 = me->getMu2();
- me->setMu2(0.5);
-
- double incr = vpMath::rad(2.0) ;
-
- if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
- {
- vpMeSite P;
- double k = alpha1;
- double i1,j1;
-
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- k -= incr;
- //while ( k < -M_PI ) { k+=2*M_PI; }
-
- i1 = b *cos(k) ; // equation of an ellipse
- j1 = a *sin(k) ; // equation of an ellipse
- P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
- P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
-
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- angle.push_back(k);
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
-
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
- }
- }
- }
- }
-
- k = alpha2;
-
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- k += incr;
- //while ( k > M_PI ) { k-=2*M_PI; }
-
- i1 = b *cos(k) ; // equation of an ellipse
- j1 = a *sin(k) ; // equation of an ellipse
- P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
- P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
-
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- angle.push_back(k);
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
-
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
- }
- }
- }
- }
- }
-
- suppressPoints() ;
-
- me->setRange(memory_range);
- me->setMu1(memory_mu1);
- me->setMu2(memory_mu2);
-}
-
-
-/*!
- Finds in the list of vpMeSite the two points with the smallest and the biggest \f$ \alpha \f$ angle value, and stores them.
-*/
-void
-vpMeEllipse::setExtremities()
-{
- double alphamin = +1e6;
- double alphamax = -1e6;
- double imin = 0;
- double jmin = 0;
- double imax = 0;
- double jmax = 0;
-
- // Loop through list of sites to track
- std::list<double>::const_iterator itAngle = angle.begin();
-
- for(std::list<vpMeSite>::const_iterator itList=list.begin(); itList!=list.end(); ++itList){
- vpMeSite s = *itList;//current reference pixel
- double alpha = *itAngle;
- if (alpha < alphamin)
- {
- alphamin = alpha;
- imin = s.ifloat ;
- jmin = s.jfloat ;
- }
-
- if (alpha > alphamax)
- {
- alphamax = alpha;
- imax = s.ifloat ;
- jmax = s.jfloat ;
- }
- ++itAngle;
- }
-
- alpha1 = alphamin;
- alpha2 = alphamax;
- iP1.set_ij(imin,jmin);
- iP2.set_ij(imax,jmax);
-}
-
-
-/*!
- 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 ellipse.
-*/
-void
-vpMeEllipse::leastSquare()
-{
- // Construction du systeme Ax=b
- //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
- // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
- unsigned int i ;
-
- vpMeSite p_me ;
-
- unsigned int iter =0 ;
- vpColVector b_(numberOfSignal()) ;
- vpRobust r(numberOfSignal()) ;
- r.setThreshold(2);
- r.setIteration(0) ;
- vpMatrix D(numberOfSignal(),numberOfSignal()) ;
- D.setIdentity() ;
- vpMatrix DA, DAmemory ;
- vpColVector DAx ;
- vpColVector w(numberOfSignal()) ;
- w =1 ;
- unsigned int nos_1 = numberOfSignal() ;
-
- if (list.size() < 3)
- {
- vpERROR_TRACE("Not enough point") ;
- throw(vpTrackingException(vpTrackingException::notEnoughPointError,
- "not enough point")) ;
- }
-
- if (circle ==false)
- {
- vpMatrix A(numberOfSignal(),5) ;
- vpColVector x(5);
-
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = vpMath::sqr(p_me.jfloat) ;
- A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
- A[k][2] = 2 * p_me.ifloat ;
- A[k][3] = 2 * p_me.jfloat ;
- A[k][4] = 1 ;
-
- b_[k] = - vpMath::sqr(p_me.ifloat) ;
- k++ ;
- }
- }
-
- while (iter < 4 )
- {
- DA = D*A ;
- vpMatrix DAp ;
-
- x = DA.pseudoInverse(1e-26) *D*b_ ;
-
- vpColVector residu(nos_1);
- residu = b_ - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
-
- k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
- k++;
- }
- iter++;
- }
-
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < thresholdWeight)
- {
- p_me.setState(vpMeSite::M_ESTIMATOR);
-
- *it = p_me;
- }
- k++ ;
- }
- }
- for(i = 0; i < 5; i ++)
- K[i] = x[i];
- }
-
- else
- {
- vpMatrix A(numberOfSignal(),3) ;
- vpColVector x(3);
-
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = 2* p_me.ifloat ;
- A[k][1] = 2 * p_me.jfloat ;
- A[k][2] = 1 ;
-
- b_[k] = - vpMath::sqr(p_me.ifloat) - vpMath::sqr(p_me.jfloat) ;
- k++ ;
- }
- }
-
- while (iter < 4 )
- {
- DA = D*A ;
- vpMatrix DAp ;
-
- x = DA.pseudoInverse(1e-26) *D*b_ ;
-
- vpColVector residu(nos_1);
- residu = b_ - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
-
- k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k];
- k++;
- }
- iter++;
- }
-
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < thresholdWeight)
- {
- p_me.setState(vpMeSite::M_ESTIMATOR);
-
- *it = p_me;
- }
- k++ ;
- }
- }
- for(i = 0; i < 3; i ++)
- K[i+2] = x[i];
- }
- getParameters() ;
-}
-
-
-/*!
- Display the 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.
- */
-void
-vpMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
-{
- vpMeEllipse::display(I,iPc,a,b,e,alpha1,alpha2,col);
-}
-
-
-/*!
- Initilization of the tracking. Ask the user to click on five points
- from the ellipse edge to track.
-
- \param I : Image in which the ellipse appears.
-*/
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I)
-{
- vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
-
- const unsigned int n=5 ;
- vpImagePoint iP[n];
-
- for (unsigned int k =0 ; k < n ; k++)
- {
- std::cout << "Click points "<< k+1 <<"/" << n ;
- std::cout << " on the ellipse in the trigonometric order" <<std::endl ;
- vpDisplay::getClick(I, iP[k], true);
- vpDisplay::displayCross(I, iP[k], 7, vpColor::red);
- vpDisplay::flush(I);
- std::cout << iP[k] << std::endl;
- }
-
- iP1 = iP[0];
- iP2 = iP[n-1];
-
- initTracking(I, n, iP) ;
-}
-
-
-/*!
- Initialization of the tracking. The ellipse is defined thanks to the
- coordinates of n points.
-
- \warning It is better to use at least five points to well estimate the K parameters.
-
- \param I : Image in which the ellipse appears.
- \param n : The number of points in the list.
- \param iP : A pointer to a list of pointsbelonging to the ellipse edge.
-*/
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- vpImagePoint *iP)
-{
- vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
-
- if (circle==false)
- {
- vpMatrix A(n,5) ;
- vpColVector b_(n) ;
- vpColVector x(5) ;
-
- // Construction du systeme Ax=b
- //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
- // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
-
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = vpMath::sqr(iP[k].get_j()) ;
- A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
- A[k][2] = 2* iP[k].get_i() ;
- A[k][3] = 2* iP[k].get_j() ;
- A[k][4] = 1 ;
-
- b_[k] = - vpMath::sqr(iP[k].get_i()) ;
- }
-
- K = A.pseudoInverse(1e-26)*b_ ;
- std::cout << K << std::endl;
- }
- else
- {
- vpMatrix A(n,3) ;
- vpColVector b_(n) ;
- vpColVector x(3) ;
-
- vpColVector Kc(3) ;
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = 2* iP[k].get_i() ;
- A[k][1] = 2* iP[k].get_j() ;
- A[k][2] = 1 ;
-
- b_[k] = - vpMath::sqr(iP[k].get_i()) - vpMath::sqr(iP[k].get_j()) ;
- }
-
- Kc = A.pseudoInverse(1e-26)*b_ ;
- K[0] = 1 ;
- K[1] = 0 ;
- K[2] = Kc[0] ;
- K[3] = Kc[1] ;
- K[4] = Kc[2] ;
-
- std::cout << K << std::endl;
- }
-
- iP1 = iP[0];
- iP2 = iP[n-1];
-
- getParameters() ;
- std::cout << "vpMeEllipse::initTracking() ellipse avant: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl;
-
- computeAngle(iP1, iP2) ;
- std::cout << "vpMeEllipse::initTracking() ellipse apres: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl;
-
- expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
-
- display(I, vpColor::green) ;
- sample(I) ;
-
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
-
- try{
- track(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
-
-}
-
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p,
- double low_alpha, double high_alpha)
-{
- iPc = ic;
- a = a_p;
- b = b_p;
- e = e_p;
- alpha1 = low_alpha;
- alpha2 = high_alpha;
-
- if (alpha2 <alpha1)
- alpha2 += 2 * M_PI;
-
- ce = cos(e);
- se = sin(e);
-
-// vpDisplay::displayEllipse(I, iPc, a, b, e, 0, vpMath::rad(360), vpColor::red, 2); // TODO remove debug only
- display(I, vpColor::green) ;
-// vpDisplay::flush(I); // TODO remove debug only
-// std::cout << "wait click" << std::endl;
-// vpDisplay::getClick(I); // TODO remove debug only
- sample(I) ;
-
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
-
- try{
- track(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
-}
-
-/*!
- Track the ellipse in the image I.
-
- \param I : Image in which the ellipse appears.
-*/
-void
-vpMeEllipse::track(const vpImage<unsigned char> &I)
-{
- vpCDEBUG(1) <<"begin vpMeEllipse::track()"<<std::endl ;
-
- static int iter =0 ;
- // 1. On fait ce qui concerne les ellipse (peut etre vide)
- {
- }
-
- //vpDisplay::display(I) ;
- // 2. On appelle ce qui n'est pas specifique
- {
-
- try{
- vpMeTracker::track(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- // std::cout << "number of signals " << numberOfSignal() << std::endl ;
- }
-
- // 3. On revient aux ellipses
- {
- // Estimation des parametres de la droite aux moindres carre
- suppressPoints() ;
- setExtremities() ;
-
-
- try{
- leastSquare() ; }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- seekExtremities(I) ;
-
- setExtremities() ;
-
- try
- {
- leastSquare() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- // suppression des points rejetes par la regression robuste
- suppressPoints() ;
- setExtremities() ;
-
- //reechantillonage si necessaire
- reSample(I) ;
-
- // remet a jour l'angle delta pour chaque point de la liste
-
- updateTheta() ;
-
- computeMoments();
-
- // Remise a jour de delta dans la liste de site me
- if (vpDEBUG_ENABLE(2))
- {
- display(I,vpColor::red) ;
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
- }
-// computeAngle(iP1, iP2) ;
-//
-// if (iter%5==0)
-// {
-// sample(I) ;
-// try{
-// leastSquare() ; }
-// catch(...)
-// {
-// vpERROR_TRACE("Error caught") ;
-// throw ;
-// }
-// computeAngle(iP1, iP2) ;
-// }
-// seekExtremities(I) ;
-//
-// vpMeTracker::display(I) ;
-// // vpDisplay::flush(I) ;
-//
-// // remet a jour l'angle theta pour chaque point de la liste
-// updateTheta() ;
-
- }
-
- iter++ ;
-
-
- vpCDEBUG(1) << "end vpMeEllipse::track()"<<std::endl ;
-
-}
-
-
-/*!
- Computes the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
-
- Computes the second central moments \f$ \mu_{20} \f$, \f$ \mu_{02} \f$ and \f$ \mu_{11} \f$
-*/
-void
-vpMeEllipse::computeMoments()
-{
- double tane = tan(-1/e);
- m00 = M_PI*a*b;
- m10 = m00*iPc.get_i();
- m01 = m00*iPc.get_j();
- m20 = m00*(a*a+b*b*tane*tane)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_i();
- m02 = m00*(a*a*tane*tane+b*b)/(4*(1+tane*tane))+m00*iPc.get_j()*iPc.get_j();
- m11 = m00*tane*(a*a-b*b)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_j();
- mu11 = m11 - iPc.get_j()*m10;
- mu02 = m02 - iPc.get_j()*m01;
- mu20 = m20 - iPc.get_i()*m10;
-}
-
-
-
-
-
-
-
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-
-/*!
- * \brief computeAngle
- */
-void
-vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1,
- int ip2, int jp2, double &_alpha2)
-{
-
- getParameters() ;
- double j1, i1, j11, i11;
- j1 = i1 = 0.0 ;
-
- int number_of_points = 2000 ;
- double incr = 2 * M_PI / number_of_points ; // angle increment
-
- double dmin1 = 1e6 ;
- double dmin2 = 1e6 ;
-
- double k = -M_PI ;
- while(k < M_PI) {
-
- j1 = a *cos(k) ; // equation of an ellipse
- i1 = b *sin(k) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- j11 = iPc.get_j() + ce *j1 - se *i1 ;
- i11 = iPc.get_i() -( se *j1 + ce *i1) ;
-
- double d = vpMath::sqr(ip1-i11) + vpMath::sqr(jp1-j11) ;
- if (d < dmin1)
- {
- dmin1 = d ;
- alpha1 = k ;
- _alpha1 = k ;
- }
- d = vpMath::sqr(ip2-i11) + vpMath::sqr(jp2-j11) ;
- if (d < dmin2)
- {
- dmin2 = d ;
- alpha2 = k ;
- _alpha2 = k ;
- }
- k += incr ;
- }
-
- if (alpha2 <alpha1) alpha2 += 2*M_PI ;
-
- vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
-
-}
-
-
-/*!
- * \brief computeAngle
- */
-void
-vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
-{
-
- double a1, a2 ;
- computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
-}
-
-
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- unsigned *i, unsigned *j)
-{
- vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
-
- if (circle==false)
- {
- vpMatrix A(n,5) ;
- vpColVector b_(n) ;
- vpColVector x(5) ;
-
- // Construction du systeme Ax=b
- //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
- // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
-
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = vpMath::sqr(j[k]) ;
- A[k][1] = 2* i[k] * j[k] ;
- A[k][2] = 2* i[k] ;
- A[k][3] = 2* j[k] ;
- A[k][4] = 1 ;
-
- b_[k] = - vpMath::sqr(i[k]) ;
- }
-
- K = A.pseudoInverse(1e-26)*b_ ;
- std::cout << K << std::endl;
- }
- else
- {
- vpMatrix A(n,3) ;
- vpColVector b_(n) ;
- vpColVector x(3) ;
-
- vpColVector Kc(3) ;
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = 2* i[k] ;
- A[k][1] = 2* j[k] ;
-
- A[k][2] = 1 ;
- b_[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
- }
-
- Kc = A.pseudoInverse(1e-26)*b_ ;
- K[0] = 1 ;
- K[1] = 0 ;
- K[2] = Kc[0] ;
- K[3] = Kc[1] ;
- K[4] = Kc[2] ;
-
- std::cout << K << std::endl;
- }
- iP1.set_i( i[0] );
- iP1.set_j( j[0] );
- iP2.set_i( i[n-1] );
- iP2.set_j( j[n-1] );
-
- getParameters() ;
- computeAngle(iP1, iP2) ;
- display(I, vpColor::green) ;
-
- sample(I) ;
-
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
-
- try{
- track(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
-
-}
-#endif // Deprecated
-
-/*!
-
- Display of the ellipse thanks to the equation parameters.
-
- \param I : The image used as background.
-
- \param center : Center of the ellipse
-
- \param A : Semiminor axis of the ellipse.
-
- \param B : Semimajor axis of the ellipse.
-
- \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
-
- \param smallalpha : Smallest \f$ alpha \f$ angle in rad.
-
- \param highalpha : Highest \f$ alpha \f$ angle in rad.
-
- \param color : Color used to display th lines.
-
- \param thickness : Thickness of the drawings.
-*/
-void vpMeEllipse::display(const vpImage<unsigned char>& 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)
-{
- double j1, i1;
- vpImagePoint iP11;
- double j2, i2;
- vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
-
- double incr = vpMath::rad(2) ; // angle increment
-
- vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
-
- double k = smallalpha ;
- while (k+incr<highalpha)
- {
- j1 = A *cos(k) ; // equation of an ellipse
- i1 = B *sin(k) ; // equation of an ellipse
-
- j2 = A *cos(k+incr) ; // equation of an ellipse
- i2 = B *sin(k+incr) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
-
- vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
-
- k += incr ;
- }
-
- j1 = A *cos(smallalpha) ; // equation of an ellipse
- i1 = B *sin(smallalpha) ; // equation of an ellipse
-
- j2 = A *cos(highalpha) ; // equation of an ellipse
- i2 = B *sin(highalpha) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
-
- vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
- vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
-}
-
-/*!
-
- Display of the ellipse thanks to the equation parameters.
-
- \param I : The image used as background.
-
- \param center : Center of the ellipse
-
- \param A : Semiminor axis of the ellipse.
-
- \param B : Semimajor axis of the ellipse.
-
- \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
-
- \param smallalpha : Smallest \f$ alpha \f$ angle in rad.
-
- \param highalpha : Highest \f$ alpha \f$ angle in rad.
-
- \param color : Color used to display th lines.
-
- \param thickness : Thickness of the drawings.
-*/
-void vpMeEllipse::display(const vpImage<vpRGBa>& 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)
-{
- double j1, i1;
- vpImagePoint iP11;
- double j2, i2;
- vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
-
- double incr = vpMath::rad(2) ; // angle increment
-
- vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
-
- double k = smallalpha ;
- while (k+incr<highalpha)
- {
- j1 = A *cos(k) ; // equation of an ellipse
- i1 = B *sin(k) ; // equation of an ellipse
-
- j2 = A *cos(k+incr) ; // equation of an ellipse
- i2 = B *sin(k+incr) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
-
- vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
-
- k += incr ;
- }
-
- j1 = A *cos(smallalpha) ; // equation of an ellipse
- i1 = B *sin(smallalpha) ; // equation of an ellipse
-
- j2 = A *cos(highalpha) ; // equation of an ellipse
- i2 = B *sin(highalpha) ; // equation of an ellipse
-
- // (i1,j1) are the coordinates on the origin centered ellipse ;
- // a rotation by "e" and a translation by (xci,jc) are done
- // to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
- // to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
-
- vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
- vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
-}
diff --git a/src/tracking/moving-edges/vpMeEllipse.h b/src/tracking/moving-edges/vpMeEllipse.h
deleted file mode 100644
index fff2701..0000000
--- a/src/tracking/moving-edges/vpMeEllipse.h
+++ /dev/null
@@ -1,393 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeEllipse.h 4705 2014-03-27 16:24:35Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMeEllipse.h
- \brief Moving edges on an ellipse
-*/
-
-#ifndef vpMeEllipse_HH
-#define vpMeEllipse_HH
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-
-#include <visp/vpMeTracker.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpImagePoint.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpColor.h>
-
-#include <math.h>
-#include <list>
-
-/*!
- \class vpMeEllipse
- \ingroup TrackingImageME
-
- \brief Class that tracks an ellipse moving edges.
-
- In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
-
- \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
-
- If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
-
- The five parameters are stored in the public attribute K.
-
- An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
- major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
-
- \image html vpMeEllipse.gif
- \image latex vpMeEllipse.ps width=10cm
-
- It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
-
- \f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
- \f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
-
- Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
-
- \image html vpMeEllipse2.gif
- \image latex vpMeEllipse2.ps width=10cm
-
- The example below available in tutorial-me-ellipse-tracker.cpp and described
- in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
-
- \include tutorial-me-ellipse-tracker.cpp
-
- */
-/*
- The code below shows how to use this class.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpMeEllipse.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImage<unsigned char> I;
-
- // I is the image containing the ellipse to track
-
- // Set the moving-edges tracker parameters
- vpMe me;
- me.setRange(25);
- me.setPointsToTrack(20);
- me.setThreshold(15000);
- me.setSampleStep(10);
-
- // Initialize the moving-edges ellipse tracker parameters
- vpMeEllipse ellipse;
- ellipse.setMe(&me);
-
- // Initialize the tracking. You have to click on five different points belonging to the ellipse.
- ellipse.initTracking(I);
-
- while ( 1 )
- {
- // ... Here the code to read or grab the next image.
-
- // Track the ellipse.
- ellipse.track(I);
- }
- return 0;
-}
-\endcode
-
- \note It is possible to display the ellipse as an overlay. For that you
- must use the display function of the class vpMeEllipse.
-*/
-
-class VISP_EXPORT vpMeEllipse : public vpMeTracker
-{
-public:
- vpMeEllipse() ;
- vpMeEllipse(const vpMeEllipse &meellipse) ;
- virtual ~vpMeEllipse() ;
-
- /*!
- \return Expected number of moving edges to track along the ellipse.
- */
- int getExpectedDensity() {return (int)expecteddensity;};
- void track(const vpImage<unsigned char>& Im);
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint* iP) ;
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p, double low_alpha, double high_alpha) ;
- void display(const vpImage<unsigned char>&I, vpColor col) ;
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
- void printParameters() ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- //@{
- void initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- unsigned *i, unsigned *j) ;
- //@}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
- /*!
- Set to true if you are sure to track a circle.
-
- \warning During all the tracking, the shape must be approximatively a circle and not an ellipse with a strong difference between the majoraxis and the minoraxis.
-
- In that case, the set of points \f$ (i,j) \f$ satisfy the implicit equation :
-
- \f[ i^2 + j^2 + 2K_2i + 2K_3j + K4 = 0 \f]
-
- Compared to the classical equation of an ellipse, \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0.
-
- \param is_circle : Set to true if you want to track a circle.
- */
- void setCircle(bool is_circle) { this->circle = is_circle ; }
-
- /*!
- Gets the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
-
- \return the value of \f$ m_{00} \f$.
- */
- inline double get_m00() const {return m00;}
-
- /*!
- Gets the 1 order raw moment \f$ m_{10} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
- \return the value of \f$ m_{10} \f$.
- */
- inline double get_m10() const {return m10;}
-
- /*!
- Gets the 1 order raw moment \f$ m_{01} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
- \return the value of \f$ m_{01} \f$.
- */
- inline double get_m01() const {return m01;}
-
- /*!
- Gets the 2 order raw moment \f$ m_{11} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
- \return the value of \f$ m_{11} \f$.
- */
- inline double get_m11() const {return m11;}
-
- /*!
- Gets the 2 order raw moment \f$ m_{20} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
- \return the value of \f$ m_{11} \f$.
- */
- inline double get_m20() const {return m20;}
-
- /*!
- Gets the 2 order raw moment \f$ m_{02} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
- \return the value of \f$ m_{11} \f$.
- */
- inline double get_m02() const {return m02;}
-
- /*!
- Gets the 2 order central moment \f$ \mu_{11} \f$.
-
- \return the value of \f$ \mu_{11} \f$.
- */
- inline double get_mu11() const {return mu11;}
-
- /*!
- Gets the 2 order central moment \f$ \mu_{02} \f$.
-
- \return the value of \f$ \mu_{02} \f$.
- */
- inline double get_mu02() const {return mu02;}
-
- /*!
- Gets the 2 order central moment \f$ \mu_{20} \f$.
-
- \return the value of \f$ \mu_{20} \f$.
- */
- inline double get_mu20() const {return mu20;}
-
- /*!
- Gets the center of the ellipse.
- */
- inline vpImagePoint getCenter() const {return iPc; }
-
- /*!
- Gets the semiminor axis of the ellipse.
- */
- inline double getA() const {return a; }
-
- /*!
- Gets the semimajor axis of the ellipse.
- */
- inline double getB() const {return b; }
-
- /*!
- Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
- */
- inline double getE() const {return e; }
-
- /*!
- Gets the equation parameters of the ellipse
- */
- void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
-
- /*!
- Gets the smallest \f$ alpha \f$ angle
- */
- inline double getSmallestAngle() { return alpha1; }
-
- /*!
- Gets the highest \f$ alpha \f$ angle
- */
- inline double getHighestAngle() { return alpha2; }
-
- /*!
- Set the new threshold for the robust estimation of the parameters of the
- ellipse equation.
- 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(const double threshold){
- if(threshold<0){
- thresholdWeight = 0;
- }else if(threshold>1){
- thresholdWeight = 1;
- }else{
- thresholdWeight = threshold;
- }
- }
-
-
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
-#else
-protected:
-#endif
- /*! Parameters of the ellipse to define the set of points that satisfy the implicit equation :
- \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
- */
- vpColVector K ;
- //! The coordinates of the ellipse center.
- vpImagePoint iPc;
- //! \f$ a \f$ is the semiminor axis of the ellipse.
- double a;
- //! \f$ b \f$ is the semimajor axis of the ellipse.
- double b;
- //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
- double e;
-
-protected:
- //! The coordinates of the point corresponding to the smallest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
- vpImagePoint iP1;
- //! The coordinates of the point corresponding to the highest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
- vpImagePoint iP2;
- //! The smallest \f$ alpha \f$ angle.
- double alpha1 ;
- //! The highest \f$ alpha \f$ angle.
- double alpha2 ;
- //! Value of cos(e).
- double ce;
- //! Value of sin(e).
- double se;
- //! Stores the value of the \f$ alpha \f$ angle for each vpMeSite.
- std::list<double> angle;
- //! Surface
- double m00;
- //! Second order central moments
- double mu11,mu20, mu02;
- //! First order raw moments
- double m10,m01;
- //! Second order raw moments
- double m11,m02,m20;
- //! Threshold for the robust least square.
- double thresholdWeight;
- //! Expected number of me to track along the ellipse.
- double expecteddensity;
-
-private:
- //! True if the ellipse to track is a circle
- bool circle ;
-
- void computeAngle(vpImagePoint pt1, vpImagePoint pt2);
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void leastSquare() ;
- void updateTheta();
- void suppressPoints() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
- void setExtremities();
- void getParameters() ;
- void computeMoments();
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- //@{
- void computeAngle(int ip1, int jp1,int ip2, int jp2) ;
- void computeAngle(int ip1, int jp1, double &alpha1,
- int ip2, int jp2, double &alpha2) ;
- //@}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
-//Static Function
-public:
- static void display(const vpImage<unsigned char>& 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);
- static void display(const vpImage<vpRGBa>& 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);
-
-};
-
-#endif
diff --git a/src/tracking/moving-edges/vpMeLine.cpp b/src/tracking/moving-edges/vpMeLine.cpp
deleted file mode 100644
index 338fea3..0000000
--- a/src/tracking/moving-edges/vpMeLine.cpp
+++ /dev/null
@@ -1,1450 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeLine.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpMeLine.cpp
- \brief Moving edges
-*/
-
-
-
-#include <visp/vpMeTracker.h>
-#include <visp/vpMe.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpRobust.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <algorithm> // std::min
-
-#define INCR_MIN 1
-
-void computeDelta(double &delta, int i1, int j1, int i2, int j2);
-
-static void
-normalizeAngle(double &delta)
-{
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < -M_PI) { delta += M_PI ; }
-}
-
-void
-computeDelta(double &delta, int i1, int j1, int i2, int j2)
-{
-
- double B = double(i1-i2) ;
- double A = double(j1-j2) ;
-
- delta = atan2(B,A) ;
- delta -= M_PI/2.0 ;
- normalizeAngle(delta) ;
-
-}
-
-static void
-project(double a, double b, double c,
- double i, double j, double &ip,double &jp)
-{
- if (fabs(a)>fabs(b))
- {
- jp = (vpMath::sqr(a)*j - a*b*i - c*b)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
- ip = (-c-b*jp)/a;
- }
- else
- {
- ip = (vpMath::sqr(b)*i-a*b*j-c*a)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
- jp = (-c-a*ip)/b;
- }
-}
-
-/*!
-
- 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.)
-
-{
- rho = meline.rho;
- theta = meline.theta;
- delta = meline.delta;
- delta_1 = meline.delta_1;
- angle = meline.angle;
- angle_1 = meline.angle_1;
- sign = meline.sign;
-
- a = meline.a;
- b = meline.b;
- c = meline.c;
- _useIntensityForRho = meline._useIntensityForRho;
- PExt[0] = meline.PExt[0];
- 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.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMeLine::sample(const vpImage<unsigned char>& I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- double n_sample;
-
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("function called with sample step = 0") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "sample step = 0")) ;
- }
-
- // i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
-
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
- if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
- throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
- // number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
-
- double stepi = diffsi/(double)n_sample;
- double stepj = diffsj/(double)n_sample;
-
- // Choose starting point
- double is = PExt[1].ifloat;
- double js = PExt[1].jfloat;
-
- // Delete old list
- list.clear();
-
- // sample positions at i*me->getSampleStep() interval along the
- // line_p, starting at PSiteExt[0]
-
- vpImagePoint ip;
- for(int i=0; i<=vpMath::round(n_sample); i++)
- {
- // If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
- {
- vpMeSite pix ; //= list.value();
- pix.init((int)is, (int)js, delta, 0, sign) ;
- pix.setDisplay(selectDisplay) ;
-
- if(vpDEBUG_ENABLE(3))
- {
- ip.set_i( is );
- ip.set_j( js );
- vpDisplay::displayCross(I, ip, 2, vpColor::blue);
- }
-
- list.push_back(pix);
- }
- is += stepi;
- js += stepj;
-
- }
-
- vpCDEBUG(1) << "end vpMeLine::sample() : ";
- 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 col : Color of the displayed line. Note that a moving edge
- that is considered as an outlier is displayed in green.
-
- */
-void
-vpMeLine::display(const vpImage<unsigned char>&I, vpColor col)
-{
- vpMeLine::display(I,PExt[0],PExt[1],list,a,b,c,col);
-}
-
-
-/*!
-
- Initilization 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<unsigned char> &I)
-{
- vpImagePoint ip1, ip2;
-
- std::cout << "Click on the line first point..." <<std::endl ;
- while (vpDisplay::getClick(I, ip1)!=true) ;
- vpDisplay::displayCross(I, ip1, 7, vpColor::red);
- vpDisplay::flush(I);
- std::cout << "Click on the line second point..." <<std::endl ;
- while (vpDisplay::getClick(I, ip2)!=true) ;
- vpDisplay::displayCross(I, ip2, 7, vpColor::red);
- vpDisplay::flush(I);
-
- try
- {
- initTracking(I, ip1, ip2) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-
-/*!
-
- 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) ;
- vpColVector x(2), x_1(2) ;
- x_1 = 0;
-
- unsigned int i ;
-
- vpRobust r(numberOfSignal()) ;
- r.setThreshold(2);
- r.setIteration(0) ;
- vpMatrix D(numberOfSignal(),numberOfSignal()) ;
- D.setIdentity() ;
- vpMatrix DA, DAmemory ;
- vpColVector DAx ;
- vpColVector w(numberOfSignal()) ;
- vpColVector B(numberOfSignal()) ;
- w =1 ;
- vpMeSite p_me ;
- unsigned int iter =0 ;
- unsigned int nos_1 = 0 ;
- double distance = 100;
-
- if (list.size() <= 2 || numberOfSignal() <= 2)
- {
- //vpERROR_TRACE("Not enough point") ;
- vpCDEBUG(1) << "Not enough point";
- throw(vpTrackingException(vpTrackingException::notEnoughPointError,
- "not enough point")) ;
- }
-
- if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
- // a i + j + c = 0
- // A = (i 1) B = (-j)
- {
- nos_1 = numberOfSignal() ;
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = p_me.ifloat ;
- A[k][1] = 1 ;
- B[k] = -p_me.jfloat ;
- k++ ;
- }
- }
-
- while (iter < 4 && distance > 0.05)
- {
- DA = D*A ;
- x = DA.pseudoInverse(1e-26) *D*B ;
-
- vpColVector residu(nos_1);
- residu = B - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
-
- k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
- k++;
- }
- iter++ ;
- distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
- x_1 = x;
- }
-
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < 0.2)
- {
- p_me.setState(vpMeSite::M_ESTIMATOR);
-
- *it = p_me;
- }
- k++ ;
- }
- }
-
- // mise a jour de l'equation de la droite
- a = x[0] ;
- b = 1 ;
- c = x[1] ;
-
- double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
- a /= s ;
- b /= s ;
- c /= s ;
- }
-
-
- else // Construction du systeme Ax=B
- // i + bj + c = 0
- // A = (j 1) B = (-i)
- {
- nos_1 = numberOfSignal() ;
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = p_me.jfloat ;
- A[k][1] = 1 ;
- B[k] = -p_me.ifloat ;
- k++ ;
- }
- }
-
- while (iter < 4 && distance > 0.05)
- {
- DA = D*A ;
- x = DA.pseudoInverse(1e-26) *D*B ;
-
- vpColVector residu(nos_1);
- residu = B - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
-
- k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
- k++;
- }
- iter++ ;
- distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
- x_1 = x;
- }
-
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < 0.2)
- {
- p_me.setState(vpMeSite::M_ESTIMATOR);
-
- *it = p_me;
- }
- k++ ;
- }
- }
- a = 1 ;
- b = x[0] ;
- c = x[1] ;
-
- double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
- a /= s ;
- b /= s ;
- c /= s ;
- }
-
- // mise a jour du delta
- delta = atan2(a,b) ;
-
- 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<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2)
-{
- vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
-
- int i1s, j1s, i2s, j2s;
-
- i1s = vpMath::round( ip1.get_i() );
- i2s = vpMath::round( ip2.get_i() );
- j1s = vpMath::round( ip1.get_j() );
- j2s = vpMath::round( ip2.get_j() );
-
- try{
-
- // 1. On fait ce qui concerne les droites (peut etre vide)
- {
- // Points extremites
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
-
- double angle_ = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;
- a = cos(angle_) ;
- b = sin(angle_) ;
-
- // Real values of a, b can have an other sign. So to get the good values
- // of a and b in order to initialise then c, we call track(I) just below
-
- computeDelta(delta,i1s,j1s,i2s,j2s) ;
- delta_1 = delta;
-
- // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);
-
- sample(I) ;
-
- }
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
- // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display
- track(I);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- 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
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
-
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- it = list.erase(it);
- else
- ++it;
- }
-}
-
-
-/*!
- Seek in the list of available points the two extremities of the line.
-*/
-void
-vpMeLine::setExtremities()
-{
- double imin = +1e6 ;
- double jmin = +1e6;
- double imax = -1 ;
- double jmax = -1 ;
-
-
- // Loop through list of sites to track
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.ifloat < imin)
- {
- imin = s.ifloat ;
- jmin = s.jfloat ;
- }
-
- if (s.ifloat > imax)
- {
- imax = s.ifloat ;
- jmax = s.jfloat ;
- }
- }
-
- PExt[0].ifloat = imin ;
- PExt[0].jfloat = jmin ;
- PExt[1].ifloat = imax ;
- PExt[1].jfloat = jmax ;
-
- if (fabs(imin-imax) < 25)
- {
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.jfloat < jmin)
- {
- imin = s.ifloat ;
- jmin = s.jfloat ;
- }
-
- if (s.jfloat > jmax)
- {
- imax = s.ifloat ;
- jmax = s.jfloat ;
- }
- }
- PExt[0].ifloat = imin ;
- PExt[0].jfloat = jmin ;
- PExt[1].ifloat = imax ;
- PExt[1].jfloat = jmax ;
- }
-}
-
-/*!
-
- 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<unsigned char> &I)
-{
- vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
-
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- double n_sample;
-
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
-
- vpERROR_TRACE("function called with sample step = 0") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "sample step = 0")) ;
- }
-
- // i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
-
- double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
-
- double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
- double dj = diffsj/sqrt(s) ;
-
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
-
- // number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
- double sample_step = (double)me->getSampleStep();
-
- vpMeSite P ;
- P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
-
- unsigned int memory_range = me->getRange() ;
- me->setRange(1);
-
- vpImagePoint ip;
-
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
-
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
-
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
- }
- }
- }
- }
-
- P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
-
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P);
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
- }
- }
- }
- }
-
- me->setRange(memory_range);
-
- vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
- 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<unsigned char> &I)
-{
- double i1,j1,i2,j2 ;
-
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
- project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
-
- // Points extremites
- PExt[0].ifloat = i1 ;
- PExt[0].jfloat = j1 ;
- PExt[1].ifloat = i2 ;
- PExt[1].jfloat = j2 ;
-
- double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
-
- unsigned int n = numberOfSignal() ;
- double expecteddensity = d / (double)me->getSampleStep();
-
- if ((double)n<0.9*expecteddensity)
- {
- double delta_new = delta;
- delta = delta_1;
- sample(I) ;
- delta = delta_new;
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::initTracking(I) ;
- }
- }
-}
-
-/*!
-
- Set the alpha value of the different vpMeSite to the value of delta.
-*/
-void
-vpMeLine::updateDelta()
-{
- vpMeSite p_me ;
-
- double angle_ = delta + M_PI/2;
- double diff = 0;
-
- while (angle_<0) angle_ += M_PI;
- while (angle_>M_PI) angle_ -= M_PI;
-
- angle_ = vpMath::round(angle_ * 180 / M_PI) ;
-
- //if(fabs(angle_) == 180 )
- if(std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon())
- {
- angle_= 0 ;
- }
-
- //std::cout << "angle theta : " << theta << std::endl ;
- diff = fabs(angle_ - angle_1);
- if (diff > 90)
- sign *= -1;
-
- angle_1 = angle_;
-
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- p_me = *it;
- p_me.alpha = delta ;
- p_me.mask_sign = sign;
- *it = p_me;
- }
- delta_1 = delta;
-}
-
-
-/*!
-
- Track the line in the image I.
-
- \param I : Image in which the line appears.
-*/
-void
-vpMeLine::track(const vpImage<unsigned char> &I)
-{
- vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;
-
- // 1. On fait ce qui concerne les droites (peut etre vide)
- {
- }
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::track(I) ;
- }
-
- // 3. On revient aux droites
- {
- // supression des points rejetes par les ME
- suppressPoints() ;
- setExtremities() ;
-
-
- // Estimation des parametres de la droite aux moindres carre
- try
- {
- leastSquare() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-
- // recherche de point aux extremite de la droites
- // dans le cas d'un glissement
- seekExtremities(I) ;
-
- setExtremities() ;
- try
- {
- leastSquare() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
- // suppression des points rejetes par la regression robuste
- suppressPoints() ;
- setExtremities() ;
-
- //reechantillonage si necessaire
- reSample(I) ;
-
- // remet a jour l'angle delta pour chaque point de la liste
-
- updateDelta() ;
-
- // Remise a jour de delta dans la liste de site me
- if (vpDEBUG_ENABLE(2))
- {
- display(I,vpColor::red) ;
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
- }
-
-
- }
-
- computeRhoTheta(I) ;
-
- vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
-}
-
-void vpMeLine::update_indices(double theta,int i,int j,int incr,int& i1,int& i2,int& j1,int& j2){
- i1 = (int)(i + cos(theta) *incr) ;
- j1 = (int)(j + sin(theta) *incr) ;
-
- i2 = (int)(i - cos(theta) *incr) ;
- 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<unsigned char>& I)
-{
- //rho = -c ;
- //theta = atan2(a,b) ;
- rho = fabs(c);
- theta = atan2(b,a) ;
-
- while (theta >= M_PI) theta -=M_PI ;
- while (theta < 0) 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) ;
- j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
-
- int end = false ;
- int incr = 10 ;
-
-
- int i1=0,i2=0,j1=0,j2=0 ;
- unsigned char v1=0,v2=0 ;
-
- int width_ = (int)I.getWidth();
- int height_ = (int)I.getHeight();
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
-
- if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
- j1<0 || j1>=width_ || j2<0 || j2>=width_){
- double rho_lim1 = fabs((double)i/cos(theta));
- double rho_lim2 = fabs((double)j/sin(theta));
-
- double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
- double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
-
- double rho_lim = std::min(rho_lim1,rho_lim2);
- double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
- incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
- if(incr<INCR_MIN){
- vpERROR_TRACE("increment is too small") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "increment is too small")) ;
- }
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
- }
-
- while (!end)
- {
- end = true;
- unsigned int i1_ = static_cast<unsigned int>(i1);
- unsigned int j1_ = static_cast<unsigned int>(j1);
- unsigned int i2_ = static_cast<unsigned int>(i2);
- unsigned int j2_ = static_cast<unsigned int>(j2);
- v1=I[i1_][j1_];
- v2=I[i2_][j2_];
- if (abs(v1-v2) < 1)
- {
-
- incr-- ;
- end = false ;
- if (incr==1)
- {
- std::cout << "In CStraightLine::GetParameters() " ;
- std::cout << " Error Tracking " << abs(v1-v2) << std::endl ;
- }
- }
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
- }
-
- if (theta >=0 && theta <= M_PI/2)
- {
- if (v2 < v1)
- {
- theta += M_PI;
- rho *= -1;
- }
- }
-
- else
- {
- double jinter;
- jinter = -c/b;
- if (v2 < v1)
- {
- theta += M_PI;
- if (jinter > 0)
- {
- rho *= -1;
- }
- }
-
- else
- {
- if (jinter < 0)
- {
- rho *= -1;
- }
- }
- }
-
- if (vpDEBUG_ENABLE(2))
- {
- vpImagePoint ip, ip1, ip2;
-
- ip.set_i( i );
- ip.set_j( j );
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
-
- vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
- vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
- }
- }
-
-}
-
-/*!
-
- 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*/
- ip1.set_i( PExt[0].ifloat );
- ip1.set_j( PExt[0].jfloat );
- ip2.set_i( PExt[1].ifloat );
- 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 denom = 0;
- double a1 = line1.a;
- double b1 = line1.b;
- double c1 = line1.c;
- double a2 = line2.a;
- double b2 = line2.b;
- double c2 = line2.c;
- double i=0, j=0;
-
- try{
-
- if (a1 > 0.1)
- {
- denom = (-(a2/a1) * b1 + b2);
-
- //if (denom == 0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
- return (false);
- }
-
- //if (denom != 0 )
- if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
- {
- j = ( (a2/a1)*c1 - c2 ) / denom;
- i = (-b1*j - c1) / a1;
- }
- }
-
- else
- {
- denom = (-(b2/b1) * a1 + a2);
-
- //if (denom == 0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
- return (false);
- }
-
- //if (denom != 0 )
- if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
- {
- i = ( (b2/b1)*c1 - c2 ) / denom;
- j = (-a1*i - c1) / b1;
- }
- }
- ip.set_i( i );
- ip.set_j( j );
-
- return (true);
- }
- catch(...)
- {
- return (false);
- }
-}
-
-/*!
- 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::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip1, ip2;
-
- if (fabs(A) < fabs(B)) {
- double i1, j1, i2, j2;
- i1 = 0;
- j1 = (-A*i1 -C) / B;
- i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
-
- }
- else {
- double i1, j1, i2, j2;
- j1 = 0;
- i1 = -(B * j1 + C) / A;
- j2 = I.getWidth() - 1.0;
- i2 = -(B * j2 + C) / A;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
- }
-
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
-
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- 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::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip1, ip2;
-
- if (fabs(A) < fabs(B)) {
- double i1, j1, i2, j2;
- i1 = 0;
- j1 = (-A*i1 -C) / B;
- i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
-
- }
- else {
- double i1, j1, i2, j2;
- j1 = 0;
- i1 = -(B * j1 + C) / A;
- j2 = I.getWidth() - 1.0;
- i2 = -(B * j2 + C) / A;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
- }
-
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
-
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- 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::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
-
- for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
- vpMeSite pix = *it;
- ip.set_i( pix.ifloat );
- ip.set_j( pix.jfloat );
-
- if (pix.getState() == vpMeSite::M_ESTIMATOR)
- vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
- else
- vpDisplay::displayCross(I, ip, 5, color,thickness);
-
- //vpDisplay::flush(I);
- }
-
- vpImagePoint ip1, ip2;
-
- if (fabs(A) < fabs(B)) {
- double i1, j1, i2, j2;
- i1 = 0;
- j1 = (-A*i1 -C) / B;
- i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
-
- }
- else {
- double i1, j1, i2, j2;
- j1 = 0;
- i1 = -(B * j1 + C) / A;
- j2 = I.getWidth() - 1.0;
- i2 = -(B * j2 + C) / A;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
- }
-
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
-
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- 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::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
-{
- vpImagePoint ip;
-
- for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
- vpMeSite pix = *it;
- ip.set_i( pix.ifloat );
- ip.set_j( pix.jfloat );
-
- if (pix.getState() == vpMeSite::M_ESTIMATOR)
- vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
- else
- vpDisplay::displayCross(I, ip, 5, color,thickness);
-
- //vpDisplay::flush(I);
- }
-
- vpImagePoint ip1, ip2;
-
- if (fabs(A) < fabs(B)) {
- double i1, j1, i2, j2;
- i1 = 0;
- j1 = (-A*i1 -C) / B;
- i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
-
- }
- else {
- double i1, j1, i2, j2;
- j1 = 0;
- i1 = -(B * j1 + C) / A;
- j2 = I.getWidth() - 1.0;
- i2 = -(B * j2 + C) / A;
-
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
- }
-
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
-
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
-}
-
diff --git a/src/tracking/moving-edges/vpMeLine.h b/src/tracking/moving-edges/vpMeLine.h
deleted file mode 100644
index 3d51add..0000000
--- a/src/tracking/moving-edges/vpMeLine.h
+++ /dev/null
@@ -1,264 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeLine.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \file vpMeLine.h
- \brief Moving edges on a line
-*/
-
-#ifndef vpMeLine_HH
-#define vpMeLine_HH
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeTracker.h>
-
-#include <math.h>
-#include <iostream>
-
-/*!
- \class vpMeLine
-
- \ingroup TrackingImageME
-
- \brief Class that tracks in an image a line moving edges.
-
- In this class the line is defined by its equation in the \f$ (i,j) =
- (line,column) \f$ image plane. Two kinds of parametrization are available to
- describe a 2D line. The first one corresponds to the following
- equation
-
- \f[ ai + bj + c = 0 \f]
-
- where \f$ i \f$ and \f$ j \f$ are the coordinates of the points
- belonging to the line. The line features are \f$ (a, b, c) \f$.
-
- The second way to write the line equation is to consider polar coordinates
- \f[ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f]
-
- where \f$ i \f$ and \f$ j \f$ are still the coordinates of the
- points belonging to the line. But now the line features are \f$
- (\rho, \theta) \f$. The computation of \f$ \rho \f$ and \f$ \theta
- \f$ is easy thanks to \f$ (a, b, c) \f$.
-
- \f[ \theta = arctan(b/a) \f]
- \f[ \rho = -c/\sqrt{a^2+b^2} \f]
-
- The value of \f$ \theta \f$ is between \f$ 0 \f$ and \f$ 2\pi
- \f$. And the value of \f$ \rho \f$ can be positive or negative. The
- conventions to find the right values of the two features are
- illustrated in the following pictures.
-
- \image html vpMeLine.gif
- \image latex vpMeLine.ps width=10cm
-
- The angle \f$\theta\f$ is computed thanks to the direction of the
- arrow. The arrow points to the side of the line which is darker.
-
- The example below available in tutorial-me-line-tracker.cpp and described
- in \ref tutorial-tracking-me shows how to use this class.
-
- \include tutorial-me-line-tracker.cpp
-*/
-
-/*
- The code below shows how to use this class.
-\code
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpMeLine.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320);
-
- // Fill the image with a black rectangle
- I = 0;
- for (int i = 100; i < 180; i ++) {
- for (int j = 120; j < 250; j ++) {
- I[i][j] = 255;
- }
- }
-
- // Set the moving-edges tracker parameters
- vpMe me;
- me.setRange(25);
- me.setThreshold(15000);
- me.setSampleStep(10);
-
- // Initialize the moving-edges line tracker parameters
- vpMeLine line;
- line.setMe(&me);
-
- // Initialize the location of the vertical line to track
- vpImagePoint ip1, ip2; // Two points belonging to the line to track
- ip1.set_i( 120 );
- ip1.set_j( 119 );
- ip2.set_i( 170 );
- ip2.set_j( 122 );
-
- line.initTracking(I, ip1, ip2);
-
- while ( 1 )
- {
- // ... Here the code to read or grab the next image.
-
- // Track the line.
- line.track(I);
- }
- return 0;
-}
-\endcode
-
- \note It is possible to display the line as an overlay. For that you
- must use the display function of the class vpMeLine.
-*/
-
-class VISP_EXPORT vpMeLine : public vpMeTracker
-{
-private:
- static void update_indices(double theta,int incr,int i,int j,int& i1,int& i2,int& j1,int& j2);
-
-protected:
- vpMeSite PExt[2] ;
-
- double rho, theta ;
- double delta ,delta_1;
- double angle, angle_1;
- int sign;
-
- //! Flag to specify wether the intensity of the image at the middle point is used to compute the sign of rho or not.
- bool _useIntensityForRho;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
-#else
-protected:
-#endif
-
- double a; //!< Parameter a of the line equation a*i + b*j + c = 0
- double b; //!< Parameter b of the line equation a*i + b*j + c = 0
- double c; //!< Parameter c of the line equation a*i + b*j + c = 0
-
-public:
- vpMeLine() ;
- vpMeLine(const vpMeLine &meline);
- virtual ~vpMeLine() ;
-
- void display(const vpImage<unsigned char>& I, vpColor col) ;
-
- void track(const vpImage<unsigned char>& Im);
-
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void leastSquare() ;
- void updateDelta();
- void setExtremities() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
- void suppressPoints() ;
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2) ;
-
- void computeRhoTheta(const vpImage<unsigned char> &I) ;
- double getRho() const ;
- double getTheta() const ;
- void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) ;
-
- /*!
- Gets the equation parameters of the line
- */
- void getEquationParam(double &A, double &B, double &C) { A = a; B = b; C = c; }
-
- /*!
- Gets parameter a of the line equation a*i + b*j + c = 0
- */
- inline double getA() const {return a; }
-
- /*!
- Gets parameter b of the line equation a*i + b*j + c = 0
- */
- inline double getB() const {return b; }
-
- /*!
- Gets parameter c of the line equation a*i + b*j + c = 0
- */
- inline double getC() const {return c; }
-
- static bool intersection(const vpMeLine &line1, const vpMeLine &line2,
- vpImagePoint &ip);
-
- /*!
- This method allows to turn off the computation of the sign of the rho
- attribute based on the intensity near the middle point of the line. This is
- usually done to distinguish between a black/white and a white/black edge but
- it may be source of problem (ex. for a servoing example) when this point can
- be occluded.
-
- \param useIntensityForRho : new value of the flag.
- */
- inline void computeRhoSignFromIntensity(const bool useIntensityForRho){
- _useIntensityForRho = useIntensityForRho;
- }
-
-//Static Functions
-public:
- static void display(const vpImage<unsigned char>& 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);
- static void display(const vpImage<vpRGBa>& 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);
-
- static void display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
-};
-
-#endif
diff --git a/src/tracking/moving-edges/vpMeNurbs.cpp b/src/tracking/moving-edges/vpMeNurbs.cpp
deleted file mode 100644
index 5685619..0000000
--- a/src/tracking/moving-edges/vpMeNurbs.cpp
+++ /dev/null
@@ -1,1397 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeNurbs.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-/*!
- \file vpMeNurbs.cpp
- \brief Moving edges
-*/
-
-#include <visp/vpMeTracker.h>
-#include <visp/vpMe.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpMeNurbs.h>
-#include <visp/vpRobust.h>
-#include <visp/vpTrackingException.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpRect.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpImageFilter.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#ifdef VISP_HAVE_OPENCV
-# if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-//# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/imgproc/imgproc_c.h>
-# else
-# include <cv.h>
-# endif
-#endif
-
-double computeDelta(double deltai, double deltaj);
-void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
- vpMe* me, double &angle, double &convlt);
-vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP);
-bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
-#endif
-
-//Compute the angle delta = arctan(deltai/deltaj)
-//and normalize it between 0 and pi
-double
-computeDelta(double deltai, double deltaj)
-{
- double delta;
- delta = atan2(deltai,deltaj) ;
- delta -= M_PI/2.0 ;
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < 0) { delta += M_PI ; }
- return(delta);
-}
-
-//Check if the image point is in the image and not to close to
-//its edge to enable the computation of a convolution whith a mask.
-static
-bool outOfImage( vpImagePoint iP , int half , int rows , int cols)
-{
- return((iP.get_i() < half + 1) || ( iP.get_i() > (rows - half - 3) )
- ||(iP.get_j() < half + 1) || (iP.get_j() > (cols - half - 3) )) ;
-}
-
-//if iP is a edge point, it computes the angle corresponding to the
-//highest convolution result. the angle is between 0 an 179.
-//The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg alpha angle)
-//and the corresponding convolution result.
-void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
- vpMe* me, double &angle, double &convlt)
-{
- int Iheight = (int)I.getHeight();
- int Iwidth = (int)I.getWidth();
- angle = 0.0;
- convlt = 0.0;
- for (int i = 0; i < 180; i++)
- {
- double conv = 0.0;
- unsigned int half;
- int index_mask;
- half = (me->getMaskSize() - 1) >> 1 ;
-
- if(outOfImage( iP , (int)half + me->getStrip() , Iheight, Iwidth))
- {
- conv = 0.0 ;
- }
- else
- {
- if (me->getAngleStep() !=0)
- index_mask = (int)(i/(double)me->getAngleStep());
- else
- throw (vpException(vpException::divideByZeroError,"angle step = 0"));
-
- unsigned int ihalf = (unsigned int)(iP.get_i()-half) ;
- unsigned int jhalf = (unsigned int)(iP.get_j()-half) ;
- unsigned int ihalfa ;
- unsigned int a ;
- unsigned int b ;
- for( a = 0 ; a < me->getMaskSize() ; a++ )
- {
- ihalfa = ihalf+a ;
- for( b = 0 ; b < me->getMaskSize() ; b++ )
- {
- conv += me->getMask()[index_mask][a][b] *
- I(ihalfa,jhalf+b) ;
- }
- }
- }
- conv = fabs(conv);
- if (conv > convlt)
- {
- convlt = conv;
- angle = vpMath::rad(i);
- angle += M_PI/2;
- while (angle > M_PI) { angle -= M_PI ; }
- while (angle < 0) { angle += M_PI ; }
- }
- }
-}
-
-
-//Find the point belonging to the edge of the sub image which respects the following hypotheses:
-//- the value of the pixel is upper than zero.
-//- the distantce between the point and iP is less than 4 pixels.
-//The function returns the nearest point of iP which respect the hypotheses
-//If no point is found the returned point is (-1,-1)
-vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP)
-{
- double dist = 1e6;
- double dist_1 = 1e6;
- vpImagePoint index(-1,-1);
- for (unsigned int i = 0; i <= Isub.getHeight(); i++)
- {
- for (unsigned int j = 0; j <= Isub.getWidth(); j++)
- {
- if(i == 0 || i == Isub.getHeight()-1 || j == 0 || j == Isub.getWidth()-1)
- {
- if (Isub(i,j) > 0)
- {
- dist = vpImagePoint::sqrDistance(vpImagePoint(iP),vpImagePoint(i,j));
- if (dist <= 16 && dist < dist_1)
- {
- dist_1 = dist;
- index.set_ij(i,j);
- }
- }
- }
- }
- }
- return index;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
-//from the center of the sub image (ie the point (15,15).
-vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
-{
- ip_edges_list->front();
- double dist;
- while (!ip_edges_list->outside())
- {
- vpImagePoint iP = ip_edges_list->value();
- dist = vpImagePoint::sqrDistance(iP,vpImagePoint(15,15));
- if (dist <= 16)
- {
- return true;
- }
- ip_edges_list->next();
- }
- return false;
-}
-#endif
-
-//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
-//from the center of the sub image (ie the point (15,15).
-bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
-{
- double dist;
- for(std::list<vpImagePoint>::const_iterator it=ip_edges_list->begin(); it!=ip_edges_list->end(); ++it){
- vpImagePoint iP = *it;
- dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15,15));
- if (dist <= 16)
- {
- return true;
- }
- }
- return false;
-}
-
-/***************************************/
-
-/*!
- 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(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false),
- cannyTh1(100.), cannyTh2(200.)
-{
- nurbs = menurbs.nurbs;
- dist = menurbs.dist;
- nbControlPoints = menurbs.nbControlPoints;
- beginPtFound = menurbs.beginPtFound;
- endPtFound = menurbs.endPtFound;
- enableCannyDetection = menurbs.enableCannyDetection;
- cannyTh1 = menurbs.cannyTh1;
- 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<unsigned char> &I)
-{
- std::list<vpImagePoint> ptList;
- vpImagePoint pt;
- vpMouseButton::vpMouseButtonType b;
-
- while (vpDisplay::getClick(I, pt, b))
- {
- if (b == vpMouseButton::button1)
- {
- //std::cout<<pt<<std::endl;
- ptList.push_back(pt);
- vpDisplay::displayCross(I,pt,10,vpColor::green);
- vpDisplay::flush(I);
- }
- if (b == vpMouseButton::button3) break;
- }
- if (ptList.size() > 3)
- initTracking(I, ptList);
- else
- throw (vpException(vpException::notInitialized,"Not enough points to initialize the Nurbs"));
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-/*!
- \deprecated You should use initTracking(const vpImage<unsigned char> &, const std::list<vpImagePoint> &) instead.
-
- 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.
-*/
-vp_deprecated void
-vpMeNurbs::initTracking(const vpImage<unsigned char> &I,
- vpList<vpImagePoint> &ptList)
-{
- std::list<vpImagePoint> listStd;
- for(ptList.front(); !ptList.outside(); ptList.next()){
- listStd.push_back(ptList.value());
- }
- nurbs.globalCurveInterp(listStd);
-
- sample(I);
-
- vpMeTracker::initTracking(I) ;
- track(I);
-}
-#endif
-
-/*!
- 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<unsigned char> &I,
- const std::list<vpImagePoint> &ptList)
-{
- nurbs.globalCurveInterp(ptList);
-
- sample(I);
-
- vpMeTracker::initTracking(I) ;
- track(I);
-}
-
-/*!
- 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.
-*/
-void
-vpMeNurbs::sample(const vpImage<unsigned char>& I)
-{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- double step = 1.0 / (double)me->getPointsToTrack();
-
- // Delete old list
- list.clear();
-
- vpImagePoint ip;
- double u = 0.0;
- vpImagePoint *pt = NULL;
- vpImagePoint pt_1(-rows,-cols);
- while (u <= 1.0)
- {
- if (pt!=NULL) delete[] pt;
- pt = nurbs.computeCurveDersPoint(u, 1);
- double delta = computeDelta(pt[1].get_i(),pt[1].get_j());
-
- // If point is in the image, add to the sample list
- if(!outOfImage(pt[0], 0, rows, cols) && vpImagePoint::sqrDistance(pt[0],pt_1) >= vpMath::sqr(me->getSampleStep()))
- {
- vpMeSite pix ; //= list.value();
- pix.init(pt[0].get_i(), pt[0].get_j(), delta) ;
- pix.setDisplay(selectDisplay) ;
-
- list.push_back(pix);
- pt_1 = pt[0];
- }
- u = u+step;
- }
- if (pt!=NULL) 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<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
-
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- {
- it = list.erase(it);
- }
- else
- ++it;
- }
-}
-
-
-/*!
- 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;
- double d = 1e6;
- double d_1 = 1e6;
- std::list<vpMeSite>::iterator it=list.begin();
-
- vpImagePoint Cu;
- vpImagePoint* der = NULL;
- double step = 0.01;
- while (u < 1 && it!=list.end())
- {
- vpMeSite s = *it;
- vpImagePoint pt(s.i,s.j);
- while (d <= d_1 && u<1)
- {
- Cu = nurbs.computeCurvePoint(u);
- d_1=d;
- d = vpImagePoint::distance(pt,Cu);
- u +=step;
- }
-
- u-=step;
- if (der != NULL) delete[] der;
- der = nurbs.computeCurveDersPoint(u, 1);
- //vpImagePoint toto(der[0].get_i(),der[0].get_j());
- //vpDisplay::displayCross(I,toto,4,vpColor::red);
-
- s.alpha = computeDelta(der[1].get_i(),der[1].get_j());
- *it = s;
- ++it;
- d = 1e6;
- d_1 = 1.5e6;
- }
- if (der != NULL) 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<unsigned char> &I)
-{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
-
- vpImagePoint* begin = NULL;
- vpImagePoint* end = NULL;
-
- begin = nurbs.computeCurveDersPoint(0.0,1);
- end = nurbs.computeCurveDersPoint(1.0,1);
-
- //Check if the two extremities are not to close to eachother.
- double d = vpImagePoint::distance(begin[0],end[0]);
- double threshold = 3*me->getSampleStep();
- double sample_step = me->getSampleStep();
- vpImagePoint pt;
- if ( d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/)
- {
- vpMeSite P ;
-
- //Init vpMeSite
- P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign) ;
- P.setDisplay(selectDisplay) ;
-
- //Set the range
- unsigned int memory_range = me->getRange() ;
- me->setRange(2);
-
- //Point at the beginning of the list
- bool beginPtAdded = false;
- vpImagePoint pt_max = begin[0];
- double angle = atan2(begin[1].get_i(),begin[1].get_j());
- double co = vpMath::abs(cos(angle));
- co = co * vpMath::sign(begin[1].get_j());
- double si = vpMath::abs(sin(angle));
- si = si * vpMath::sign(begin[1].get_i());
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - si*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - co*sample_step ; P.j = (int)P.jfloat ;
- pt.set_ij(P.ifloat,P.jfloat);
- if (vpImagePoint::distance(end[0],pt) < threshold) break;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false);
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_front(P) ;
- beginPtAdded = true;
- pt_max = pt;
- if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
- }
- }
- }
- }
-
- if (!beginPtAdded) beginPtFound++;
-
- P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
- P.setDisplay(selectDisplay);
-
- bool endPtAdded = false;
- angle = atan2(end[1].get_i(),end[1].get_j());
- co = vpMath::abs(cos(angle));
- co = co * vpMath::sign(end[1].get_j());
- si = vpMath::abs(sin(angle));
- si = si * vpMath::sign(end[1].get_i());
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + si*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + co*sample_step ; P.j = (int)P.jfloat ;
- pt.set_ij(P.ifloat,P.jfloat);
- if (vpImagePoint::distance(begin[0],pt) < threshold) break;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false);
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P) ;
- endPtAdded = true;
- if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
- }
- }
- else {
- if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
- }
- }
- }
- }
- if (!endPtAdded) endPtFound++;
- me->setRange(memory_range);
- }
- else
- {
- list.pop_front();
- }
- /*if(begin != NULL)*/ delete[] begin;
- /*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 initialize at this points.
-
- This method is practicle 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.
-*/
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
-void
-vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> &I)
-#else
-void
-vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
-#endif
-{
-#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
- vpMeSite pt = list.front();
- vpImagePoint firstPoint(pt.ifloat,pt.jfloat);
- pt = list.back();
- vpImagePoint lastPoint(pt.ifloat,pt.jfloat);
- if (beginPtFound >=3 && farFromImageEdge(I, firstPoint))
- {
- vpImagePoint *begin = NULL;
- begin = nurbs.computeCurveDersPoint(0.0,1);
- vpImage<unsigned char> Isub(32,32); //Sub image.
- vpImagePoint topLeft(begin[0].get_i()-15,begin[0].get_j()-15);
- vpRect rect(topLeft,32,32);
-
- vpDisplay::displayRectangle(I,rect,vpColor::green);
-
- vpImageTools::createSubImage(I,rect,Isub);
-
- vpImagePoint lastPtInSubIm(begin[0]);
- double u = 0.0;
- double step =0.0001;
- //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
- while (inRectangle(lastPtInSubIm,rect) && u < 1)
- {
- u += step;
- lastPtInSubIm = nurbs.computeCurvePoint(u);
- }
-
- u -= step;
- if( u > 0)
- lastPtInSubIm = nurbs.computeCurvePoint(u);
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(Isub, Ip);
-
- IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
- cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
-
- vpImageConvert::convert(dst, Isub);
-#endif
-
- vpImagePoint firstBorder(-1,-1);
-
- firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
-
- std::list<vpImagePoint> ip_edges_list;
- if (firstBorder != vpImagePoint(-1, -1))
- {
- unsigned int dir;
- double fi = firstBorder.get_i();
- double fj = firstBorder.get_j();
- double w = Isub.getWidth()-1;
- double h = Isub.getHeight()-1;
- //if (firstBorder.get_i() == 0) dir = 4;
- if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
- //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
- else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
- //else if (firstBorder.get_j() == 0) dir = 2;
- else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
- //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
- else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
- computeFreemanChainElement(Isub, firstBorder , dir);
- unsigned int firstDir = dir;
- ip_edges_list.push_back( firstBorder );
- vpImagePoint border(firstBorder);
- vpImagePoint dBorder;
- do
- {
- computeFreemanParameters(dir, dBorder);
- border = border + dBorder;
- vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
-
- ip_edges_list.push_back( border );
-
- computeFreemanChainElement(Isub, border , dir);
- } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
- }
-
- if (findCenterPoint(&ip_edges_list))
- {
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); /*++it*/){
- vpMeSite s = *it;
- vpImagePoint iP(s.ifloat,s.jfloat);
- if (inRectangle(iP,rect))
- it = list.erase(it) ;
- else
- break;
- }
-
- std::list<vpMeSite>::iterator itList=list.begin();
- double convlt;
- double delta = 0;
- int nbr = 0;
- std::list<vpMeSite> addedPt;
- for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
- vpMeSite s = *itList;
- vpImagePoint iPtemp = *itEdges + topLeft;
- vpMeSite pix;
- pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
- dist = vpMeSite::sqrDistance(s,pix);
- if (dist >= vpMath::sqr(me->getSampleStep())/*25*/)
- {
- bool exist = false;
- for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
- dist = vpMeSite::sqrDistance(pix, *itAdd);
- if (dist < vpMath::sqr(me->getSampleStep())/*25*/)
- exist = true;
- }
- if (!exist)
- {
- findAngle(I, iPtemp, me, delta, convlt);
- pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
- pix.setDisplay(selectDisplay);
- --itList;
- list.insert(itList, pix);
- ++itList;
- addedPt.push_front(pix);
- nbr++;
- }
- }
- }
-
- unsigned int memory_range = me->getRange();
- me->setRange(3);
- std::list<vpMeSite>::iterator itList2=list.begin();
- for (int j = 0; j < nbr; j++)
- {
- vpMeSite s = *itList2;
- s.track(I,me,false);
- *itList2 = s;
- ++itList2;
- }
- me->setRange(memory_range);
- }
-
- /* if (begin != NULL) */ delete[] begin;
- beginPtFound = 0;
- }
-
- if(endPtFound >= 3 && farFromImageEdge(I, lastPoint))
- {
- vpImagePoint *end = NULL;
- end = nurbs.computeCurveDersPoint(1.0,1);
-
- vpImage<unsigned char> Isub(32,32); //Sub image.
- vpImagePoint topLeft(end[0].get_i()-15,end[0].get_j()-15);
- vpRect rect(topLeft,32,32);
-
- vpDisplay::displayRectangle(I,rect,vpColor::green);
-
- vpImageTools::createSubImage(I,rect,Isub);
-
- vpImagePoint lastPtInSubIm(end[0]);
- double u = 1.0;
- double step =0.0001;
- //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
- while (inRectangle(lastPtInSubIm,rect) && u > 0)
- {
- u -= step;
- lastPtInSubIm = nurbs.computeCurvePoint(u);
- }
-
- u += step;
- if( u < 1.0)
- lastPtInSubIm = nurbs.computeCurvePoint(u);
-
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
-#else
- IplImage* Ip = NULL;
- vpImageConvert::convert(Isub, Ip);
-
- IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
- cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
-
- vpImageConvert::convert(dst, Isub);
-#endif
-
- vpImagePoint firstBorder(-1,-1);
-
- firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
-
- std::list<vpImagePoint> ip_edges_list;
- if (firstBorder != vpImagePoint(-1, -1))
- {
- unsigned int dir;
- double fi = firstBorder.get_i();
- double fj = firstBorder.get_j();
- double w = Isub.getWidth()-1;
- double h = Isub.getHeight()-1;
- //if (firstBorder.get_i() == 0) dir = 4;
- if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
- //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
- else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
- //else if (firstBorder.get_j() == 0) dir = 2;
- else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
- //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
- else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
-
-
- computeFreemanChainElement(Isub, firstBorder , dir);
- unsigned int firstDir = dir;
- ip_edges_list.push_back( firstBorder );
- vpImagePoint border(firstBorder);
- vpImagePoint dBorder;
- do
- {
- computeFreemanParameters(dir, dBorder);
- border = border + dBorder;
- vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
-
- ip_edges_list.push_back( border );
-
- computeFreemanChainElement(Isub, border , dir);
- } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
- }
-
- if (findCenterPoint(&ip_edges_list))
- {
-// list.end();
- vpMeSite s;
- while(true)//{//!list.outside())
- {
- s = list.back();//list.value() ;
- vpImagePoint iP(s.ifloat,s.jfloat);
- if (inRectangle(iP,rect))
- {
- list.erase(list.end()) ;
-// list.end();
- }
- else
- break;
- }
-
- std::list<vpMeSite>::iterator itList = list.end();
- --itList; // Move on the last element
- double convlt;
- double delta;
- int nbr = 0;
- std::list<vpMeSite> addedPt;
- for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
- s = *itList;
- vpImagePoint iPtemp = *itEdges + topLeft;
- vpMeSite pix;
- pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
- dist = vpMeSite::sqrDistance(s,pix);
- if (dist >= vpMath::sqr(me->getSampleStep()))
- {
- bool exist = false;
- for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
- dist = vpMeSite::sqrDistance(pix, *itAdd);
- if (dist < vpMath::sqr(me->getSampleStep()))
- exist = true;
- }
- if (!exist)
- {
- findAngle(I, iPtemp, me, delta, convlt);
- pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
- pix.setDisplay(selectDisplay);
- list.push_back(pix);
- addedPt.push_back(pix);
- nbr++;
- }
- }
- }
-
- unsigned int memory_range = me->getRange();
- me->setRange(3);
- std::list<vpMeSite>::iterator itList2 = list.end();
- --itList2; // Move to the last element
- for (int j = 0; j < nbr; j++)
- {
- vpMeSite me_s = *itList2;
- me_s.track(I,me,false);
- *itList2 = me_s;
- --itList2;
- }
- me->setRange(memory_range);
- }
-
- /* if (end != NULL) */ delete[] end;
- endPtFound = 0;
- }
-#else
- vpTRACE("To use the canny detection, OpenCV has to be installed.");
-#endif
-}
-
-
-/*!
- 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<unsigned char> &I)
-{
- unsigned int n = numberOfSignal();
- double nbPt = floor(dist / me->getSampleStep());
-
- if ((double)n < 0.7*nbPt)
- {
- sample(I);
- vpMeTracker::initTracking(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<unsigned char> &I)
-{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- vpImagePoint* iP = NULL;
-
- int n = (int)numberOfSignal();
-
-// list.front();
- std::list<vpMeSite>::iterator it=list.begin();
- std::list<vpMeSite>::iterator itNext=list.begin();
- ++itNext;
-
- unsigned int range_tmp = me->getRange();
- me->setRange(2);
-
- while(itNext!=list.end() && n <= me->getPointsToTrack())
- {
- vpMeSite s = *it;//current reference pixel
- vpMeSite s_next = *itNext;//current reference pixel
-
- double d = vpMeSite::sqrDistance(s,s_next);
- if(d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600)
- {
- vpImagePoint iP0(s.ifloat,s.jfloat);
- vpImagePoint iPend(s_next.ifloat,s_next.jfloat);
- vpImagePoint iP_1(s.ifloat,s.jfloat);
-
- double u = 0.0;
- double ubegin = 0.0;
- double uend = 0.0;
- double dmin1_1 = 1e6;
- double dmin2_1 = 1e6;
- while(u < 1)
- {
- u+=0.01;
- double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iP0);
- double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iPend);
-
- if (dmin1 < dmin1_1)
- {
- dmin1_1 = dmin1;
- ubegin = u;
- }
-
- if (dmin2 < dmin2_1)
- {
- dmin2_1 = dmin2;
- uend = u;
- }
- }
- u = ubegin;
-
- //if(( u != 1.0 || uend != 1.0)
- if( (std::fabs(u-1.0) > std::fabs(vpMath::maximum(u, 1.0))*std::numeric_limits<double>::epsilon())
- || (std::fabs(uend-1.0) > std::fabs(vpMath::maximum(uend, 1.0))*std::numeric_limits<double>::epsilon()))
- {
- iP = nurbs.computeCurveDersPoint(u, 1);
-
- while (vpImagePoint::sqrDistance(iP[0],iPend) > vpMath::sqr(me->getSampleStep()) && u < uend)
- {
- u+=0.01;
- /*if (iP!=NULL)*/ {
- delete[] iP;
- iP = NULL;
- }
- iP = nurbs.computeCurveDersPoint(u, 1);
- if ( vpImagePoint::sqrDistance(iP[0],iP_1) > vpMath::sqr(me->getSampleStep()) && !outOfImage(iP[0], 0, rows, cols))
- {
- double delta = computeDelta(iP[1].get_i(),iP[1].get_j());
- vpMeSite pix ; //= list.value();
- pix.init(iP[0].get_i(), iP[0].get_j(), delta) ;
- pix.setDisplay(selectDisplay) ;
- pix.track(I,me,false);
- if (pix.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.insert(it, pix);
- iP_1 = iP[0];
- }
- }
- }
- /*if (iP!=NULL)*/ {
- delete[] iP;
- iP = NULL;
- }
- }
- }
- ++it;
- ++itNext;
- }
- 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
- // Loop through list of sites to track
- list.front();
- while(!list.nextOutside())
- {
- vpMeSite s = list.value() ;//current reference pixel
- vpMeSite s_next = list.nextValue() ;//current reference pixel
-
- if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
- {
- s_next.setState(vpMeSite::TOO_NEAR);
-
- list.next();
- list.modify(s_next);
- if (!list.nextOutside()) list.next();
- }
- else
- list.next() ;
- }
-#endif
- std::list<vpMeSite>::const_iterator it=list.begin();
- std::list<vpMeSite>::iterator itNext=list.begin();
- ++itNext;
- for(;itNext!=list.end();){
- vpMeSite s = *it;//current reference pixel
- vpMeSite s_next = *itNext;//current reference pixel
-
- if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep())){
- s_next.setState(vpMeSite::TOO_NEAR);
-
- *itNext = s_next;
- ++it;
- ++itNext;
- if(itNext!=list.end()){
- ++it;
- ++itNext;
- }
- }
- else{
- ++it;
- ++itNext;
- }
- }
-}
-
-
-/*!
- Track the edge in the image I.
-
- \param I : Image in which the edge appears.
-*/
-void
-vpMeNurbs::track(const vpImage<unsigned char> &I)
-{
- //Tracking des vpMeSites
- vpMeTracker::track(I);
-
- //Suppress points which are too close to each other
- supressNearPoints();
-
- //Suppressions des points ejectes par le tracking
- suppressPoints();
-
- //Recalcule les parametres
-// nurbs.globalCurveInterp(list);
- nurbs.globalCurveApprox(list,nbControlPoints);
-
- //On resample localement
- localReSample(I);
-
- seekExtremities(I);
- if(enableCannyDetection)
- seekExtremitiesCanny(I);
-
-// nurbs.globalCurveInterp(list);
- nurbs.globalCurveApprox(list,nbControlPoints);
-
- double u = 0.0;
- vpImagePoint pt;
- vpImagePoint pt_1;
- dist = 0;
- while (u<=1.0)
- {
- pt = nurbs.computeCurvePoint(u);
- //if(u!=0)
- if(std::fabs(u) > std::numeric_limits<double>::epsilon())
- dist = dist + vpImagePoint::distance(pt,pt_1);
- pt_1 = pt;
- u=u+0.01;
- }
-
- updateDelta();
-
- 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 col : Color of the displayed line.
-
- */
-void
-vpMeNurbs::display(const vpImage<unsigned char>&I, vpColor col)
-{
- vpMeNurbs::display(I,nurbs,col);
-}
-
-
-/*!
- Considering a pixel iP compute the next element of the Freeman chain
- code.
-
- According to the gray level of pixel iP and his eight neighbors determine
- the next element of the chain in order to turn around the dot
- counterclockwise.
-
- \param I : The image we are working with.
- \param iP : The current pixel.
- \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
- with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
-
- \return false if an element cannot be found. Occurs for example with an area
- constituted by a single pixel. Return true if success.
-*/
-bool
-vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I,
- vpImagePoint &iP,
- unsigned int &element)
-{
- vpImagePoint diP;
- vpImagePoint iPtemp;
- if (hasGoodLevel( I, iP )) {
- // get the point on the right of the point passed in
- computeFreemanParameters((element + 2) %8, diP);
- iPtemp = iP + diP;
- if (hasGoodLevel( I, iPtemp )) {
- element = (element + 2) % 8; // turn right
- }
- else {
- computeFreemanParameters((element + 1) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 1) % 8; // turn diag right
- }
- else {
- computeFreemanParameters(element, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- //element = element; // keep same dir
- }
- else {
- computeFreemanParameters((element + 7) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 7) %8; // turn diag left
- }
- else {
- computeFreemanParameters((element + 6) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 6) %8 ; // turn left
- }
- else {
- computeFreemanParameters((element + 5) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 5) %8 ; // turn diag down
- }
- else {
- computeFreemanParameters((element + 4) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 4) %8 ; // turn down
- }
- else {
- computeFreemanParameters((element + 3) %8, diP);
- iPtemp = iP + diP;
-
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 3) %8 ; // turn diag right down
- }
- else {
- // No neighbor with a good level
- //
- return false;
- }
- }
- }
- }
- }
- }
- }
- }
- }
- else {
- return false;
- }
- return true;
-}
-
-
-/*!
- Check if the pixel iP is in the image and has
- a good level to belong to the edge.
-
- \param I : Image.
- \param iP : Pixel to test
-
- \return true : If the pixel iP is in the area and
- has a value greater than 0.
-
- \return false : Otherwise
-*/
-bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char>& I,
- const vpImagePoint& iP) const
-{
- if( !isInImage( I, iP ) )
- return false;
-
- if( I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0)
- {
- return true;
- }
- else
- {
- return false;
- }
-}
-
-
-/*!
- Test if a pixel is in the image. Points of the border are not considered to
- be in the image.
-
- \param I : The image.
- \param iP : An image point.
-
- \return true if the image point \e iP is in the image and false
- otherwise.
-*/
-bool vpMeNurbs::isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const
-{
- return (iP.get_i() >= 0
- && iP.get_j() >= 0
- && iP.get_i() < I.getHeight()
- && iP.get_j() < I.getWidth());
-}
-
-
-/*!
- Gives the displacement corresponding to the value of the parameter \e element.
-
- - If element = 0 diP = (0,1).
- - If element = 1 diP = (1,1).
- - If element = 2 diP = (1,0).
- - If element = 3 diP = (1,-1).
- - If element = 4 diP = (0,-1).
- - If element = 5 diP = (-1,-1).
- - If element = 6 diP = (-1,0).
- - If element = 7 diP = (-1,1).
-
- \param element : the element value(typically given by the method computeFreemanChainElement).
- \param diP : the output parameter which contains the displacement cooresponding to the value of \e element.
-*/
-void
-vpMeNurbs::computeFreemanParameters( unsigned int element, vpImagePoint &diP)
-{
- /*
- 5 6 7
- \ | /
- \|/
- 4 ------- 0
- /|\
- / | \
- 3 2 1
- */
- switch(element) {
- case 0: // go right
- diP.set_ij(0,1);
- break;
-
- case 1: // go right top
- diP.set_ij(1,1);
- break;
-
- case 2: // go top
- diP.set_ij(1,0);
- break;
-
- case 3:
- diP.set_ij(1,-1);
- break;
-
- case 4:
- diP.set_ij(0,-1);
- break;
-
- case 5:
- diP.set_ij(-1,-1);
- break;
-
- case 6:
- diP.set_ij(-1,0);
- break;
-
- case 7:
- diP.set_ij(-1,1);
- break;
- }
-}
-
-
-/*!
- Check if the point is far enough from the image edges
-
- \param I : The image.
- \param iP : An image point.
-
- \return true if the point iP is at least 20 pixels far from the image edeges.
-*/
-bool
-vpMeNurbs::farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP)
-{
- unsigned int height = I.getHeight();
- unsigned int width = I.getWidth();
- 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.
-*/
-void vpMeNurbs::display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color)
-{
- double u = 0.0;
- vpImagePoint pt;
- while (u <= 1)
- {
- pt = n.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,color);
- u+=0.01;
- }
-}
-
-/*!
-
- 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.
-*/
-void vpMeNurbs::display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color)
-{
- double u = 0.0;
- vpImagePoint pt;
- while (u <= 1)
- {
- pt = n.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,color);
- u+=0.01;
- }
-}
-
diff --git a/src/tracking/moving-edges/vpMeNurbs.h b/src/tracking/moving-edges/vpMeNurbs.h
deleted file mode 100644
index 242a99e..0000000
--- a/src/tracking/moving-edges/vpMeNurbs.h
+++ /dev/null
@@ -1,237 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeNurbs.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpMeNurbs.h
- \brief Moving edges on a form reprsented by a NURBS (Non Uniform Rational B-Spline)
-*/
-
-#ifndef vpMeNurbs_HH
-#define vpMeNurbs_HH
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpMeTracker.h>
-#include <visp/vpNurbs.h>
-
-#include <math.h>
-#include <iostream>
-#include <list>
-
-/*!
- \class vpMeNurbs
-
- \ingroup TrackingImageME
-
- \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.
-
- It is possible to have a direct access to the nurbs. It is indeed a public parameter.
-
- The code below shows how to use this class.
-\code
-#include <visp/vpImage.h>
-#include <visp/vpMeNurbs.h>
-#include <visp/vpImagePoint.h>
-
-int main()
-{
- vpImage<unsigned char> I(240, 320);
-
- // Fill the image with a black rectangle
- I = 0;
- for (int i = 100; i < 180; i ++) {
- for (int j = 0; j < 320; j ++) {
- I[i][j] = 255;
- }
- }
-
- // Set the moving-edges tracker parameters
- vpMe me;
- me.setRange(25);
- me.setPointsToTrack(20);
- me.setThreshold(15000);
- me.setSampleStep(10);
-
- // Initialize the moving-edges tracker parameters
- vpMeNurbs meNurbs;
- meNurbs.setNbControlPoints(4);
- meNurbs.setMe(&me);
-
- // Initialize the location of the edge to track (here a horizontal line
- std::list<vpImagePoint> ipList; //List of points belonginig to the edge
- ipList.push_back(vpImagePoint(110,119));
- ipList.push_back(vpImagePoint(140,119));
- ipList.push_back(vpImagePoint(160,119));
- ipList.push_back(vpImagePoint(170,119));
-
- meNurbs.initTracking(I, ipList);
-
- while ( 1 )
- {
- // ... Here the code to read or grab the next image.
-
- // Track the line.
- meNurbs.track(I);
- }
- return 0;
-}
-\endcode
-
- \note It is possible to display the nurbs as an overlay. For that you
- must use the display function of the class vpMeNurbs.
-
- \note In case of an edge which is not smooth, it can be interesting to use the
- canny detection to find the extremities. In this case, use the method
- setEnableCannyDetection to enable it. Warning : This function requires OpenCV.
-*/
-
-class VISP_EXPORT vpMeNurbs : public vpMeTracker
-{
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- public:
- #else
- private:
- #endif
- //! The Nurbs which represents the tracked edge.
- vpNurbs nurbs;
-
- private:
- //! Size of the nurbs
- double dist;
- //! Number of control points used to compute the Nurbs.
- unsigned int nbControlPoints;
- //! counter used to trigger the canny edge detection at the beginning of the Nurbs.
- int beginPtFound;
- //! counter used to trigger the canny edge detection at the end of the Nurbs.
- int endPtFound;
- //! True if the canny detection has to be used during the extremities search.
- bool enableCannyDetection;
- //! First canny threshold
- double cannyTh1;
- //! Second canny threshold
- double cannyTh2;
-
- public:
- vpMeNurbs();
- vpMeNurbs(const vpMeNurbs &menurbs);
- virtual ~vpMeNurbs();
-
- /*!
- 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(const 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.
- */
- 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;
- */
- void setCannyThreshold(const double th1, const double th2)
- {
- this->cannyTh1 = th1;
- this->cannyTh2 = th2;
- }
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I,
- const std::list<vpImagePoint> &ptList) ;
-
- void track(const vpImage<unsigned char>& Im);
-
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void updateDelta();
- void setExtremities() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
- void seekExtremitiesCanny(const vpImage<unsigned char> &I) ;
- void suppressPoints() ;
-
- void supressNearPoints();
- void localReSample(const vpImage<unsigned char> &I);
-
- /*!
- Gets the nurbs;
- */
- inline vpNurbs getNurbs() const {return nurbs; }
-
- void display(const vpImage<unsigned char>& I, vpColor col) ;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- /*!
- @name Deprecated functions
- */
- vp_deprecated void initTracking(const vpImage<unsigned char> &I,
- vpList<vpImagePoint> &ptList) ;
-#endif
-
- private:
- bool computeFreemanChainElement(const vpImage<unsigned char> &I,
- vpImagePoint &iP,
- unsigned int &element);
-
- bool hasGoodLevel(const vpImage<unsigned char>& I,
- const vpImagePoint& iP) const;
-
- bool isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const;
-
- void computeFreemanParameters( unsigned int element, vpImagePoint &diP);
-
- bool farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP);
-
-public:
- static void display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color = vpColor::green);
- static void display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color = vpColor::green);
-};
-
-#endif
diff --git a/src/tracking/moving-edges/vpMeSite.cpp b/src/tracking/moving-edges/vpMeSite.cpp
deleted file mode 100644
index 5b87a50..0000000
--- a/src/tracking/moving-edges/vpMeSite.cpp
+++ /dev/null
@@ -1,738 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeSite.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- * Andrew Comport
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMeSite.cpp
- \brief Moving edges
-*/
-
-
-
-#include <visp/vpMeSite.h>
-#include <visp/vpMe.h>
-#include <visp/vpTrackingException.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include "vpMeSite.h"
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static
-bool horsImage(int i , int j, int half, int rows, int cols)
-{
- //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
- int half_1 = half + 1;
- int half_3 = half + 3;
- //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
- return( (0 < (half_1 - i) ) || ( (i - rows + half_3) > 0 ) || ( 0 < (half_1 -j) ) || ( (j - cols + half_3) > 0 ) ) ;
-}
-#endif
-
-void
-vpMeSite::init()
-{
- // Site components
- alpha = 0.0 ;
- convlt = 0.0 ;
- weight=-1;
-
- selectDisplay = NONE ;
-
- // Pixel components
- i = 0 ;
- j = 0 ;
- v = 0 ;
- ifloat =i ;
- jfloat = j ;
- i_1 = 0 ;
- j_1 = 0 ;
-
- mask_sign = 1 ;
-
- normGradient = 0;
-
- state = NO_SUPPRESSION;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- suppress = 0;
-#endif
-}
-
-vpMeSite::vpMeSite()
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , suppress(0)
-#endif
-{
-}
-
-vpMeSite::vpMeSite(double ip, double jp)
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , suppress(0)
-#endif
-{
- i = vpMath::round(ip) ;
- j = vpMath::round(jp) ;
- ifloat = ip ;
- jfloat = jp ;
-}
-
-/*!
- Copy constructor.
-*/
-vpMeSite::vpMeSite (const vpMeSite &mesite)
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , suppress(0)
-#endif
-{
- *this = mesite;
-}
-
-// More an Update than init
-// For points in meter form (to avoid approximations)
-void
-vpMeSite::init(double ip, double jp, double alphap)
-{
- // Note: keep old value of convlt, suppress and contrast
- selectDisplay = NONE ;
-
- ifloat = ip;
- i= vpMath::round(ip);
- jfloat = jp ;
- j = vpMath::round(jp);
- alpha = alphap ;
- mask_sign =1 ;
-
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
-
-}
-
-// initialise with convolution
-void
-vpMeSite::init(double ip, double jp, double alphap, double convltp)
-{
- selectDisplay = NONE ;
- ifloat = ip ;
- i= (int)ip ;
- jfloat = jp ;
- j =(int)jp ;
- alpha = alphap ;
- convlt = convltp;
- mask_sign =1 ;
-
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
-}
-// initialise with convolution and sign
-void
-vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
-{
- selectDisplay = NONE ;
- ifloat = ip ;
- i= (int)ip ;
- jfloat = jp ;
- j =(int)jp ;
- alpha = alphap ;
- convlt = convltp;
- mask_sign = sign ;
-
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
-}
-
-vpMeSite &vpMeSite::operator=(const vpMeSite &m)
-{
- i = m.i;
- j = m.j;
- i_1 = m.i_1;
- j_1 = m.j_1;
- ifloat = m.ifloat;
- jfloat = m.jfloat;
- v = m.v;
- mask_sign = m.mask_sign;
- alpha = m.alpha;
- convlt = m.convlt;
- normGradient = m.normGradient;
- weight = m.weight;
- selectDisplay = m.selectDisplay;
- state = m.state;
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- suppress = m.suppress;
-#endif
-
- 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<unsigned char> &I, const int range)
-{
-
- int k ;
-
- int n;
- double ii , jj ;
- vpMeSite *list_query_pixels ;
- list_query_pixels = NULL ;
-
- unsigned int range_ = static_cast<unsigned int>(range);
- // Size of query list includes the point on the line
- list_query_pixels = new vpMeSite[2 * range_ + 1] ;
-
- // range : +/- the range within which the pixel's
- //correspondent will be sought
-
- double salpha = sin(alpha);
- double calpha = cos(alpha);
- n = 0 ;
- vpImagePoint ip;
-
- for(k = -range ; k <= range ; k++)
- {
- ii = (ifloat+k*salpha);
- jj = (jfloat+k*calpha);
-
- // Display
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RANGE)) {
- ip.set_i( ii );
- ip.set_j( jj );
- vpDisplay::displayCross(I, ip, 1, vpColor::yellow) ;
- }
-
- // Copy parent's convolution
- vpMeSite pel ;
- pel.init(ii, jj, alpha, convlt,mask_sign) ;
- pel.setDisplay(selectDisplay) ;// Display
-
- // Add site to the query list
- list_query_pixels[n] = pel ;
- n++ ;
- }
-
- return(list_query_pixels) ;
-}
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-// ===================================================================
-/*!
- * get the sign (according to the difference of values of the intensities of the extremities).
- * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set.
- * \param I : Image in which the sign is computed.
- * \param range : +/- the range within which the pixel's correspondent is sought
- * \post : mask_sign is computed
- */
-// ===================================================================
-void
-vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
-{
-
- int k ;
-
- double salpha = sin(alpha);
- double calpha = cos(alpha);
-
- //First extremity
- k = -range ;
- unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
- unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
-
- //Second extremity
- k = range ;
- unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
- unsigned int j2 = static_cast<unsigned int>(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 mask_sign = -1 ;
-}
-#endif
-
-// Specific function for ME
-double
-vpMeSite::convolution(const vpImage<unsigned char>&I, const vpMe *me)
-{
- int half;
- unsigned int index_mask ;
- int height_ = static_cast<int>(I.getHeight());
- int width_ = static_cast<int>(I.getWidth());
-
- double conv = 0.0 ;
- unsigned int msize = me->getMaskSize();
- half = (static_cast<int>(msize) - 1) >> 1 ;
-
- if(horsImage( i , j , half + me->getStrip() , height_, width_))
- {
- conv = 0.0 ;
- i = 0 ; j = 0 ;
- }
- else
- {
- // Calculate tangent angle from normal
- double theta = alpha+M_PI/2;
- // Move tangent angle to within 0->M_PI for a positive
- // mask index
- while (theta<0) theta += M_PI;
- while (theta>M_PI) theta -= M_PI;
-
- // Convert radians to degrees
- int thetadeg = vpMath::round(theta * 180 / M_PI) ;
-
- if(abs(thetadeg) == 180 )
- {
- thetadeg= 0 ;
- }
-
- index_mask = (unsigned int)(thetadeg/(double)me->getAngleStep());
-
- unsigned int i_ = static_cast<unsigned int>(i);
- unsigned int j_ = static_cast<unsigned int>(j);
- unsigned int half_ = static_cast<unsigned int>(half);
-
- unsigned int ihalf = i_-half_ ;
- unsigned int jhalf = j_-half_ ;
- unsigned int ihalfa ;
-
- for(unsigned int a = 0 ; a < msize ; a++ )
- {
- ihalfa = ihalf+a ;
- for(unsigned int b = 0 ; b < msize ; b++ )
- {
- conv += mask_sign* me->getMask()[index_mask][a][b] *
- // I(i-half+a,j-half+b) ;
- I(ihalfa,jhalf+b) ;
- }
- }
-
- }
-
- return(conv) ;
-}
-
-
-/*!
-
- Specific function for ME.
-
- \warning To display the moving edges graphics a call to vpDisplay::flush()
- is needed.
-
-*/
-void
-vpMeSite::track(const vpImage<unsigned char>& I,
- const vpMe *me,
- const bool test_contraste)
-{
- // vpMeSite *list_query_pixels ;
- // int max_rank =0 ;
- // int max_rank1=0 ;
- // int max_rank2 = 0;
- // double convolution = 0 ;
- // double max_convolution = 0 ;
- // double max = 0 ;
- // double contraste = 0;
- // // vpDisplay::display(I) ;
- // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
- // // vpDisplay::getClick(I) ;
- //
- // // range = +/- range of pixels within which the correspondent
- // // of the current pixel will be sought
- // int range = me->getRange() ;
- //
- // // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
- // list_query_pixels = getQueryList(I, range) ;
- //
- // double contraste_max = 1 + me->getMu2();
- // double contraste_min = 1 - me->getMu1();
- //
- // // array in which likelihood ratios will be stored
- // double *likelihood= new double[ 2 * range + 1 ] ;
- //
- // int ii_1 = i ;
- // int jj_1 = j ;
- // i_1 = i ;
- // j_1 = j ;
- // double threshold;
- // threshold = me->getThreshold();
- //
- // // std::cout <<"---------------------"<<std::endl ;
- // for(int n = 0 ; n < 2 * range + 1 ; n++)
- // {
- //
- // // convolution results
- // convolution = list_query_pixels[n].convolution(I, me) ;
- //
- // // luminance ratio of reference pixel to potential correspondent pixel
- // // the luminance must be similar, hence the ratio value should
- // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
- // if( test_contraste )
- // {
- // // Include this to eliminate temporal calculation
- // if (convlt==0)
- // {
- // std::cout << "vpMeSite::track : Division by zero convlt = 0" << std::endl ;
- // delete []list_query_pixels ;
- // delete []likelihood;
- // throw(vpTrackingException(vpTrackingException::initializationError,
- // "Division by zero")) ;
- // }
- //
- // // contraste = fabs(convolution / convlt) ;
- // contraste = convolution / convlt ;
- // // likelihood ratios
- // if((contraste > contraste_min) && (contraste < contraste_max))
- // likelihood[n] = fabs(convolution + convlt ) ;
- // else
- // likelihood[n] = 0 ;
- // }
- // else
- // likelihood[n] = fabs(2*convolution) ;
- //
- //
- // // establishment of the maximal likelihood ratios's rank
- // // in the array, the value of the likelihood ratio can now be
- // // referenced by its rank in the array
- // if (likelihood[n] > max)
- // {
- // max_convolution= convolution;
- // max = likelihood[n] ;
- // max_rank = n ;
- // max_rank2 = max_rank1;
- // max_rank1 = max_rank;
- // }
- //
- // }
- //
- // // test on the likelihood threshold if threshold==-1 then
- // // the me->threshold is selected
- //
- // vpImagePoint ip;
- //
- // // if (test_contrast)
- // if(max > threshold)
- // {
- // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- // {
- // ip.set_i( list_query_pixels[max_rank].i );
- // ip.set_j( list_query_pixels[max_rank].j );
- // vpDisplay::displayPoint(I, ip, vpColor::red);
- // }
- //
- // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by the vpMeSite of max likelihood
- // normGradient = vpMath::sqr(max_convolution);
- //
- // convlt = max_convolution;
- // i_1 = ii_1; //list_query_pixels[max_rank].i ;
- // j_1 = jj_1; //list_query_pixels[max_rank].j ;
- // delete []list_query_pixels ;
- // delete []likelihood;
- // }
- // else //none of the query sites is better than the threshold
- // {
- // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- // {
- // ip.set_i( list_query_pixels[max_rank].i );
- // ip.set_j( list_query_pixels[max_rank].j );
- // vpDisplay::displayPoint(I, ip, vpColor::green);
- // }
- // normGradient = 0 ;
- // if(max == 0)
- // suppress = 1; // contrast suppression
- // else
- // suppress = 2; // threshold suppression
- //
- // delete []list_query_pixels ;
- // delete []likelihood; // modif portage
- // }
-
- vpMeSite *list_query_pixels ;
- int max_rank =-1 ;
- // int max_rank1=-1 ;
- // int max_rank2 = -1;
- double convolution_ = 0 ;
- double max_convolution = 0 ;
- double max = 0 ;
- double contraste = 0;
- // vpDisplay::display(I) ;
- // vpERROR_TRACE("getclcik %d",me->range) ;
- // vpDisplay::getClick(I) ;
-
- // range = +/- range of pixels within which the correspondent
- // of the current pixel will be sought
- unsigned int range = me->getRange() ;
-
- // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
- list_query_pixels = getQueryList(I, (int)range) ;
-
- double contraste_max = 1 + me->getMu2();
- double contraste_min = 1 - me->getMu1();
-
- // array in which likelihood ratios will be stored
- double *likelihood= new double[ 2 * range + 1 ] ;
-
- int ii_1 = i ;
- int jj_1 = j ;
- i_1 = i ;
- j_1 = j ;
- double threshold;
- threshold = me->getThreshold() ;
- double diff = 1e6;
-
- // std::cout <<"---------------------"<<std::endl ;
- for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
- {
- // convolution results
- convolution_ = list_query_pixels[n].convolution(I, me) ;
-
- // luminance ratio of reference pixel to potential correspondent pixel
- // the luminance must be similar, hence the ratio value should
- // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
- if( test_contraste )
- {
- likelihood[n] = fabs(convolution_ + convlt );
- if (likelihood[n]> threshold)
- {
- contraste = convolution_ / convlt;
- if((contraste > contraste_min) && (contraste < contraste_max) && fabs(1-contraste) < diff)
- {
- diff = fabs(1-contraste);
- max_convolution= convolution_;
- max = likelihood[n] ;
- max_rank = (int)n ;
- // max_rank2 = max_rank1;
- // max_rank1 = max_rank;
- }
- }
- }
-
- else
- {
- likelihood[n] = fabs(2*convolution_) ;
- if (likelihood[n] > max && likelihood[n] > threshold)
- {
- max_convolution= convolution_;
- max = likelihood[n] ;
- max_rank = (int)n ;
- // max_rank2 = max_rank1;
- // max_rank1 = max_rank;
- }
- }
- }
-
- // test on the likelihood threshold if threshold==-1 then
- // the me->threshold is selected
-
- vpImagePoint ip;
-
- // if (test_contrast)
- if(max_rank >= 0)
- {
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- {
- ip.set_i( list_query_pixels[max_rank].i );
- ip.set_j( list_query_pixels[max_rank].j );
- vpDisplay::displayPoint(I, ip, vpColor::red);
- }
-
- *this = list_query_pixels[max_rank] ;//The vpMeSite2 is replaced by the vpMeSite2 of max likelihood
- normGradient = vpMath::sqr(max_convolution);
-
- convlt = max_convolution;
- i_1 = ii_1; //list_query_pixels[max_rank].i ;
- j_1 = jj_1; //list_query_pixels[max_rank].j ;
- delete []list_query_pixels ;
- delete []likelihood;
- }
- else //none of the query sites is better than the threshold
- {
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- {
- ip.set_i( list_query_pixels[0].i );
- ip.set_j( list_query_pixels[0].j );
- vpDisplay::displayPoint(I, ip, vpColor::green);
- }
- normGradient = 0 ;
- //if(contraste != 0)
- if(std::fabs(contraste) > std::numeric_limits<double>::epsilon())
- state = CONSTRAST; // contrast suppression
- else
- state = THRESHOLD; // threshold suppression
-
- delete []list_query_pixels ;
- delete []likelihood; // modif portage
- }
-}
-
-int vpMeSite::operator!=(const vpMeSite &m)
-{
- return((m.i != i) || (m.j != j)) ;
-
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS)
-{
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- return (os << "Alpha: " << vpMeS.alpha
- << " Convolution: " << vpMeS.convlt
- << " Flag: " << vpMeS.suppress
- << " Weight: " << vpMeS.weight );
-#else
- return (os << "Alpha: " << vpMeS.alpha
- << " Convolution: " << vpMeS.convlt
- << " Weight: " << vpMeS.weight );
-#endif
-}
-
-void vpMeSite::display(const vpImage<unsigned char>& I)
-{
- vpMeSite::display(I,ifloat,jfloat,state);
-}
-
-//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 (constrast 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<unsigned char>& I, const double &i, const double &j, const vpMeSiteState &state)
-{
- switch(state)
- {
- case NO_SUPPRESSION:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
- break;
-
- case CONSTRAST:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
- break;
-
- case THRESHOLD:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
- break;
-
- case M_ESTIMATOR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
- break;
-
- case TOO_NEAR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
- break;
-
- default:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
- }
-}
-
-/*!
- 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 (constrast 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<vpRGBa>& I, const double &i, const double &j, const vpMeSiteState &state)
-{
- switch(state)
- {
- case NO_SUPPRESSION:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
- break;
-
- case CONSTRAST:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
- break;
-
- case THRESHOLD:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
- break;
-
- case M_ESTIMATOR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
- break;
-
- case TOO_NEAR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
- break;
-
- default:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
- }
-}
diff --git a/src/tracking/moving-edges/vpMeSite.h b/src/tracking/moving-edges/vpMeSite.h
deleted file mode 100644
index 31c4048..0000000
--- a/src/tracking/moving-edges/vpMeSite.h
+++ /dev/null
@@ -1,263 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeSite.h 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Eric Marchand
- * Andrew Comport
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMeSite.h
- \brief Moving edges
-*/
-
-
-
-#ifndef vpMeSite_H
-#define vpMeSite_H
-
-#include <visp/vpMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpMe.h>
-
-/*!
- \class vpMeSite
- \ingroup TrackingImageME
-
- \brief Performs search in a given direction(normal) for a given
- distance(pixels) for a given 'site'. Gives the most likely site
- given the probablility from an ME mask
-
- - Bug fix: rewrote application of masks to use the temporal
- information instead of applying both temporal masks to the same
- image. ie: spatial -> spatio/temporal
-
- - Added new tracking function to choose the most similar edge
- amongst all edges found.
-
- - sample step.
- */
-class VISP_EXPORT vpMeSite
-{
-public:
- typedef enum
- {
- NONE,
- RANGE,
- RESULT,
- RANGE_RESULT
- } vpMeSiteDisplayType;
-
- typedef enum
- {
- NO_SUPPRESSION = 0,
- CONSTRAST = 1,
- THRESHOLD = 2,
- M_ESTIMATOR = 3,
- TOO_NEAR = 4,
- UNKNOW = 5
- } vpMeSiteState;
-
-public:
- int i,j ;
- int i_1, j_1 ;
- double ifloat, jfloat ;
- unsigned char v ;
- int mask_sign ;
- // Angle of tangent at site
- double alpha;
- // Convolution of Site in previous image
- double convlt ;
- // Convolution of Site in previous image
- double normGradient ;
- // Uncertainty of point given as a probability between 0 and 1
- double weight;
-
-private:
- vpMeSiteDisplayType selectDisplay ;
- vpMeSiteState state;
-
-public:
- void init() ;
- void init(double ip, double jp, double alphap) ;
- void init(double ip, double jp, double alphap, double convltp) ;
- void init(double ip, double jp, double alphap, double convltp, int sign) ;
-
- vpMeSite () ;
- vpMeSite(double ip, double jp) ;
- vpMeSite (const vpMeSite &mesite) ;
- virtual ~vpMeSite() {} ;
-
- void display(const vpImage<unsigned char>& I);
-
- double convolution(const vpImage<unsigned char>& ima, const vpMe *me) ;
-
- vpMeSite *getQueryList(const vpImage<unsigned char> &I, const int range) ;
-
- void track(const vpImage<unsigned char>& im,
- const vpMe *me,
- const bool test_contraste=true);
-
- /*!
- 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
- */
- inline double getAlpha() const { return alpha; }
-
-
- void setDisplay(vpMeSiteDisplayType select) { selectDisplay = select ; }
-
- /*!
- 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
- */
- inline int get_j() const { return j; }
-
- /*!
- 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
- */
- inline double get_jfloat() const { return jfloat; }
-
- /*!
- Set the state of the site
-
- \param flag : flag corresponding to vpMeSiteState
-
- \sa vpMeSiteState
- */
- void setState(const vpMeSiteState &flag){
- state = flag;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- suppress = (int)flag;
- #endif
- }
-
- /*!
- 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
- */
- void setWeight(const double &w) { weight = w; }
-
- /*!
- Get the weight of the site
-
- \return value of weight
- */
- inline double getWeight() const { return weight; }
-
-//Operators
- vpMeSite &operator=(const vpMeSite &m) ;
- int operator!=(const vpMeSite &m) ;
-
- friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS);
-
-//Static functions
- /*!
- Compute the distance \f$ |S1 - S2| = \sqrt{(i_1-i_2)^2+(j_1-j_2)^2} \f$
-
- \param S1 : First site
- \param S2 : Second site
-
- \return the distance between the two sites.
- */
- static double distance (const vpMeSite S1, const vpMeSite S2) {
- return(sqrt(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat)));}
-
- /*!
- Compute the distance \f$ |S1 - S2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
-
- \param S1 : First site
- \param S2 : Second site
-
- \return the distance between the two sites.
- */
- static double sqrDistance (const vpMeSite S1, const vpMeSite S2) {
- return(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat));}
-
- static void display(const vpImage<unsigned char>& I, const double &i, const double &j,
- const vpMeSiteState &state = NO_SUPPRESSION);
- static void display(const vpImage<vpRGBa>& I, const double &i, const double &j,
- const vpMeSiteState &state = NO_SUPPRESSION);
-
-//Deprecated
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
- //! Flag to indicate whether point is rejected or not
- //! 1 = contrast, 2 = threshold, 3 = M-estimator, 0 = nosupp
- int suppress;
-
- vp_deprecated void getSign(const vpImage<unsigned char> &I, const int range) ;
-#endif
-} ;
-
-#endif
diff --git a/src/tracking/moving-edges/vpMeTracker.cpp b/src/tracking/moving-edges/vpMeTracker.cpp
deleted file mode 100644
index 895f95f..0000000
--- a/src/tracking/moving-edges/vpMeTracker.cpp
+++ /dev/null
@@ -1,375 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpMeTracker.cpp 4797 2014-07-23 15:52:28Z fspindle $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Moving edges.
-*
-* Authors:
-* Andrew Comport
-*
-*****************************************************************************/
-
-/*!
-\file vpMeTracker.cpp
-\brief Contains abstract elements for a Distance to Feature type feature.
-*/
-
-
-
-#include <visp/vpMeTracker.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpColor.h>
-
-#include <visp/vpTrackingException.h>
-#include <visp/vpDebug.h>
-#include <algorithm>
-
-#define DEBUG_LEVEL1 0
-#define DEBUG_LEVEL2 0
-
-void
-vpMeTracker::init()
-{
- vpTracker::init() ;
- p.resize(2) ;
- selectDisplay = vpMeSite::NONE ;
-}
-
-vpMeTracker::vpMeTracker()
- : list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , query_range (0), display_point(false)
-#endif
-{
- init();
-}
-
-vpMeTracker::vpMeTracker(const vpMeTracker& meTracker)
- : vpTracker(meTracker),
- list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , query_range (0), display_point(false)
-#endif
-{
- init();
-
- me = meTracker.me;
- list = meTracker.list;
- nGoodElement = meTracker.nGoodElement;
- init_range = meTracker.init_range;
- selectDisplay = meTracker.selectDisplay;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- display_point = meTracker.display_point;
- query_range = meTracker.query_range;
- #endif
-}
-
-/*!
- Reset the tracker by removing all the moving edges.
- */
-void vpMeTracker::reset()
-{
- nGoodElement = 0;
- list.clear();
-}
-
-vpMeTracker::~vpMeTracker()
-{
- reset();
-}
-
-vpMeTracker&
-vpMeTracker::operator = (vpMeTracker& p_me)
-{
- list = p_me.list;
- me = p_me.me;
- selectDisplay = p_me.selectDisplay ;
-
- return *this;
-}
-
-static bool isSuppressZero(const vpMeSite& P){
- return (P.getState() == vpMeSite::NO_SUPPRESSION);
-}
-
-unsigned int
-vpMeTracker::numberOfSignal()
-{
- unsigned int number_signal=0;
-
- // Loop through all the points tracked from the contour
- number_signal = static_cast<unsigned int>(std::count_if(list.begin(), list.end(), isSuppressZero));
- return number_signal;
-}
-
-unsigned int
-vpMeTracker::totalNumberOfSignal()
-{
- return (unsigned int)list.size();
-}
-
-int
-vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
-{
- return (! ((i> half+2) &&
- (i< rows -(half+2)) &&
- (j>half+2) &&
- (j<cols-(half+2)))
- ) ;
-}
-
-int
-vpMeTracker::outOfImage(vpImagePoint iP, int half, int rows, int cols)
-{
- int i = vpMath::round(iP.get_i());
- int j = vpMath::round(iP.get_j());
- 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<unsigned char>& I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- // Must set range to 0
- unsigned int range_tmp = me->getRange();
- me->setRange(init_range);
-
- nGoodElement=0;
-
- int d = 0;
- vpImagePoint ip1, ip2;
-
- // Loop through list of sites to track
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite refp = *it;//current reference pixel
-
- d++ ;
- // If element hasn't been suppressed
- if(refp.getState() == vpMeSite::NO_SUPPRESSION)
- {
- try {
- refp.track(I,me,false);
- }
- catch(...)
- {
- // EM verifier quel signal est de sortie !!!
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- if(refp.getState() == vpMeSite::NO_SUPPRESSION) nGoodElement++;
- }
-
-
-#if (DEBUG_LEVEL2)
- {
- double a,b ;
- a = refp.i_1 - refp.i ;
- b = refp.j_1 - refp.j ;
- if(refp.getState() == vpMeSite::NO_SUPPRESSION) {
- ip1.set_i( refp.i );
- ip1.set_j( refp.j );
- ip2.set_i( refp.i+a );
- ip2.set_j( refp.j+b );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
- }
- }
-#endif
- *it = refp;
- }
-
- /*
- if (res != OK)
- {
- std::cout<< "In vpMeTracker::initTracking(): " ;
- switch (res)
- {
- case ERR_TRACKING:
- std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " << std::endl ;
- break ;
- case fatalError:
- std::cout << "vpMeTracker::initTracking:Track return fatalError" << std::endl ;
- break ;
- default:
- std::cout << "vpMeTracker::initTracking:Track return error " << res << std::endl ;
- }
- return res ;
- }
- */
-
- me->setRange(range_tmp);
-}
-
-/*!
- Track moving-edges.
-
- \param I : Image.
-
- \exception vpTrackingException::initializationError : Moving edges not initialized.
-
-*/
-void
-vpMeTracker::track(const vpImage<unsigned char>& I)
-{
- if (!me) {
- vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
- }
-
- if (list.empty())
- {
- vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
- throw(vpTrackingException(vpTrackingException::notEnoughPointError,
- "too few pixel to track")) ;
-
- }
-
- vpImagePoint ip1, ip2;
- nGoodElement=0;
- // int d =0;
- // Loop through list of sites to track
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
-
- // d++ ;
- // If element hasn't been suppressed
- if(s.getState() == vpMeSite::NO_SUPPRESSION)
- {
-
- try{
- // vpERROR_TRACE("%d",d ) ;
- // vpERROR_TRACE("range %d",me->range) ;
- s.track(I,me,true);
- }
- catch(vpTrackingException)
- {
- vpERROR_TRACE("catch exception ") ;
- s.setState(vpMeSite::THRESHOLD);
- }
-
- if(s.getState() != vpMeSite::THRESHOLD)
- {
- nGoodElement++;
-
-#if (DEBUG_LEVEL2)
- {
- double a,b ;
- a = s.i_1 - s.i ;
- b = s.j_1 - s.j ;
- if(s.getState() == vpMeSite::NO_SUPPRESSION) {
- ip1.set_i( s.i );
- ip1.set_j( s.j );
- ip2.set_i( s.i+a*5 );
- ip2.set_j( s.j+b*5 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
- }
- }
-#endif
-
- }
- *it = s;
- }
- }
-}
-
-/*!
- 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 (constrast 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<unsigned char>& I)
-{
-#if (DEBUG_LEVEL1)
- {
- std::cout <<"begin vpMeTracker::displayList() " << std::endl ;
- std::cout<<" There are "<<list.size()<< " sites in the list " << std::endl ;
- }
-#endif
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite p_me = *it;
- p_me.display(I);
- }
-}
-
-/*! Displays the status of moving edge sites
-
- \param I : The image.
- \param w : vector
- \param index_w : index
-*/
-void
-vpMeTracker::display(const vpImage<unsigned char>& I,vpColVector &w, unsigned int &index_w)
-{
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite P = *it;
-
- if(P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- P.weight = w[index_w];
- index_w++;
- }
-
- *it = P;
- }
- display(I);
-}
-
-#undef DEBUG_LEVEL1
-#undef DEBUG_LEVEL2
-
-
diff --git a/src/tracking/moving-edges/vpMeTracker.h b/src/tracking/moving-edges/vpMeTracker.h
deleted file mode 100644
index f821622..0000000
--- a/src/tracking/moving-edges/vpMeTracker.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpMeTracker.h 4797 2014-07-23 15:52:28Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Moving edges.
- *
- * Authors:
- * Andrew Comport
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \file vpMeTracker.h
- \brief Contains abstract elements for a Distance to Feature type feature.
-*/
-
-// ===================================================================
-/*!
- \class vpMeTracker
- \ingroup TrackingImageME
- \brief Contains abstract elements for a Distance to Feature type feature.
-
- 2D state = list of points, 3D state = feature
-
- */
-// ===================================================================
-
-#ifndef vpMeTracker_HH
-#define vpMeTracker_HH
-
-#include <visp/vpColVector.h>
-#include <visp/vpMeSite.h>
-#include <visp/vpMe.h>
-#include <visp/vpTracker.h>
-
-#include <math.h>
-#include <iostream>
-#include <list>
-
-class VISP_EXPORT vpMeTracker : public vpTracker
-{
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
-#else
-protected:
-#endif
- //! Tracking dependent variables/functions
- //! List of tracked moving edges points.
- std::list<vpMeSite> list ;
- //! Moving edges initialisation parameters
- vpMe *me ;
- unsigned int init_range;
- int nGoodElement;
-
-protected:
- vpMeSite::vpMeSiteDisplayType selectDisplay ;
-
-public:
- // Constructor/Destructor
- vpMeTracker() ;
- vpMeTracker(const vpMeTracker& meTracker) ;
- virtual ~vpMeTracker() ;
-
- void init() ;
- void initTracking(const vpImage<unsigned char>& I);
-
- //! Track sampled pixels.
- void track(const vpImage<unsigned char>& I);
-
- unsigned int numberOfSignal() ;
- unsigned int totalNumberOfSignal() ;
-
- virtual void display(const vpImage<unsigned char> &I, vpColor col)=0;
- virtual void display(const vpImage<unsigned char>& I);
- void display(const vpImage<unsigned char>& I, vpColVector &w, unsigned int &index_w);
-
- void setDisplay(vpMeSite::vpMeSiteDisplayType select) {
- selectDisplay = select ;
- }
-
- vpMeTracker& operator =(vpMeTracker& f);
-
- int outOfImage( int i , int j , int half , int rows , int cols) ;
- int outOfImage( vpImagePoint iP , int half , int rows , int cols) ;
-
- void reset();
-
- //!Sample pixels at a given interval
- virtual void sample(const vpImage<unsigned char> &image)=0;
-
-
- /*!
- Set the initial range.
-
- \param r : initial range.
- */
- void setInitRange(const unsigned int &r) { init_range = r; }
-
- /*!
- Return the initial range.
-
- \return Value of init_range.
- */
- inline unsigned int getInitRange() { return init_range; }
-
- /*!
- Set the moving edges initialisation parameters
-
- \param p_me : Moving Edges.
- */
- void setMe(vpMe *p_me) { this->me = p_me ; }
-
- /*!
- Return the moving edges initialisation parameters
-
- \return Moving Edges.
- */
- inline vpMe* getMe(){ return me; }
-
- /*!
- Set the list of moving edges
-
- \param l : list of Moving Edges.
- */
- void setMeList(const std::list<vpMeSite> &l) { list = l; }
-
- /*!
- Return the list of moving edges
-
- \return List of Moving Edges.
- */
- inline std::list<vpMeSite>& getMeList() { return list; }
- inline std::list<vpMeSite> getMeList() const { return list; }
-
- /*!
- Return the number of points that has not been suppressed.
-
- \return Number of good points.
- */
- inline int getNbPoints() const { return nGoodElement; }
-
-#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-public:
- int query_range;
- bool display_point;// if 1 (TRUE) displays the line that is being tracked
-#endif
-};
-
-
-#endif
-
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.cpp b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.cpp
deleted file mode 100644
index e5fed19..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSD.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpTemplateTrackerSSD.h>
-
-vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
- : vpTemplateTracker(warp), DI(), temp()
-{
- dW.resize(2,nbParam);
- G.resize(nbParam);
- H.resize(nbParam,nbParam);
- HLM.resize(nbParam,nbParam);
-
- temp.resize(nbParam);
-
- X1.resize(2);
- X2.resize(2);
- DI.resize(2);
-}
-
-double vpTemplateTrackerSSD::getCost(const vpImage<unsigned char> &I,vpColVector &tp)
-{
- double erreur=0;
- double IW,Tij;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- erreur+=((double)Tij-IW)*((double)Tij-IW);
- Nbpoint++;
- }
- }
- ratioPixelIn=(double)Nbpoint/(double)templateSize;
-
- if(Nbpoint==0)return 10e10;
- return erreur/Nbpoint;
-}
-
-
-double vpTemplateTrackerSSD::getSSD(vpImage<unsigned char> &I,vpColVector &tp)
-{
- double erreur=0;
- double IW,Tij;
- int i,j;
- double i2,j2;
- unsigned int Nbpoint=0;
-
- if(pyrInitialised)
- {
- templateSize=templateSizePyr[0];
- ptTemplate=ptTemplatePyr[0];
- }
-
- Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
-
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
- {
- Tij=ptTemplate[point].val;
- IW=I.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- erreur+=((double)Tij-IW)*((double)Tij-IW);
- Nbpoint++;
- }
- }
- if(Nbpoint==0)return 10e10;
- return erreur/Nbpoint;
-}
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.h b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.h
deleted file mode 100644
index 462066a..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSD.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSD.h 4956 2014-11-12 15:50:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerSSD.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerSSD_hh
-#define vpTemplateTrackerSSD_hh
-
-#include <math.h>
-
-#include <visp/vpTemplateTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpImageFilter.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomography.h>
-
-class VISP_EXPORT vpTemplateTrackerSSD: public vpTemplateTracker
-{
- protected:
- vpRowVector DI;
- vpRowVector temp;
-
- protected:
- double getCost(const vpImage<unsigned char> &I, vpColVector &tp);
- double getCost(const vpImage<unsigned char> &I){ return getCost(I,p); }
- virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
- virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
-
- public:
- vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp);
-
- double getSSD(vpImage<unsigned char> &I,vpColVector &tp);
- void setGain(double g){ gain=g; }
-};
-
-#endif
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.cpp b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.cpp
deleted file mode 100644
index 7865c9d..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.cpp
+++ /dev/null
@@ -1,332 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDESM.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerSSDESM.h>
-#include <visp/vpImageFilter.h>
-
-vpTemplateTrackerSSDESM::vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerSSD(warp), compoInitialised(false), HDir(), HInv(),
- HLMDir(), HLMInv(), GDir(), GInv()
-{
- useCompositionnal=false;
- useInverse=false;
-
- if(!Warp->isESMcompatible())
- std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
-
- HInv.resize(nbParam,nbParam);
- HDir.resize(nbParam,nbParam);
- HLMInv.resize(nbParam,nbParam);
- HLMDir.resize(nbParam,nbParam);
- GInv.resize(nbParam);
- GDir.resize(nbParam);
-}
-
-void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompInverse(I);
-}
-
-void vpTemplateTrackerSSDESM::initCompInverse(const vpImage<unsigned char> &/*I*/)
-{
- //std::cout<<"Initialise precomputed value of ESM with templateSize: "<< templateSize<<std::endl;
- ptTemplateCompo=new vpTemplateTrackerPointCompo[templateSize];
- int i,j;
- //direct
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- ptTemplateCompo[point].dW=new double[2*nbParam];
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
-
- }
-
- //inverse
- HInv=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
-
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
- Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- HInv[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
- }
- vpMatrix::computeHLM(HInv,lambdaDep,HLMInv);
-
-compoInitialised=true;
-}
-
-void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
-{
- double erreur=0;
- unsigned int Nbpoint=0;
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- double IW,dIWx,dIWy;
- double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- Nbpoint=0;
- erreur=0;
- dp=0;
- HDir=0;
- GDir=0;
- GInv=0;
- Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- //INVERSE
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- Nbpoint++;
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- GInv[it]+=er*ptTemplate[point].dW[it];
-
- erreur+=er*er;
-
- //DIRECT
- //dIWx=dIx.getValue(i2,j2);
- //dIWy=dIy.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
- dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;
-
- //Calcul du Hessien
- //Warp->dWarp(X1,X2,p,dW);
- Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
-
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- HDir[it][jt]+=tempt[it]*tempt[jt];
-
- for(unsigned int it=0;it<nbParam;it++)
- GDir[it]+=er*tempt[it];
- delete[] tempt;
- }
-
-
- }
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
-
- vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);
-
- try
- {
- //dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
- //dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
- //dp=HLMInv.inverseByLU()*GInv;
- dp=(HLMDir).inverseByLU()*(GDir);
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
- throw(e);
- }
-
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
- }
-
- //Warp->pRondp(p,dp,p);
- p+=dp;
- iteration++;
-
- }
- while( (iteration < iterationMax));
-
- nbIteration=iteration;
-}
-
-
-/*void vpTemplateTrackerSSDESM::InitCompInverse(vpImage<unsigned char> &I)
-{
- ptTempateCompo=new vpTemplateTrackerPointCompo[taille_template];
- int i,j;
- for(int point=0;point<taille_template;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- ptTempateCompo[point].dW=new double[2*nbParam];
- Warp->getdWdp0(i,j,ptTempateCompo[point].dW);
- }
-
-}
-
-void vpTemplateTrackerSSDESM::track(vpImage<unsigned char> &I)
-{
- double erreur=0,erreur_prec=1e38;
- int Nbpoint=0;
-
- int taillefiltre=taille_filtre_dgaussien;
- double *fg=new double[taillefiltre+1] ;
- getGaussianDerivativeKernel(fg, taillefiltre) ;
- getGradX(I, dIx,fg,taillefiltre);
- getGradY(I, dIy,fg,taillefiltre);
- delete[] fg;
-
- vpColVector dpinv(nbParam);
- double lambda=lambdaDep;
- double IW,dIWx,dIWy;
- double Tij;
- int iteration=0;
- int i,j;
- double i2,j2;
- vpTemplateTrackerPoint *pt;
- do
- {
- Nbpoint=0;
- erreur_prec=erreur;
- erreur=0;
- dp=0;
- HDir=0;
- GDir=0;
- GInv=0;
- Warp->ComputeCoeff(p);
- for(int point=0;point<taille_template;point++)
- {
- pt=&ptTemplate[point];
- i=pt->y;
- j=pt->x;
- X1[0]=j;X1[1]=i;
-
- Warp->ComputeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth())&&(i2<I.getHeight())&&(i2>0)&&(j2>0))
- {
- //INVERSE
- Tij=pt->val;
- IW=I.getPixelBI(j2,i2);
- Nbpoint++;
- double er=(Tij-IW);
- for(int it=0;it<nbParam;it++)
- GInv[it]+=er*ptTemplate[point].dW[it];
-
- erreur+=er*er;
-
- //DIRECT COMPO
- Tij=ptTemplate[point].val;
- IW=I.getPixelBI(j2,i2);
- dIWx=dIx.getPixelBI(j2,i2);
- dIWy=dIy.getPixelBI(j2,i2);
- Nbpoint++;
- Warp->dWarpCompo(X1,X2,p,ptTempateCompo[point].dW,dW);
- double *tempt=new double[nbParam];
- for(int it=0;it<nbParam;it++)
- tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
-
- for(int it=0;it<nbParam;it++)
- for(int jt=0;jt<nbParam;jt++)
- HDir[it][jt]+=tempt[it]*tempt[jt];
-
- for(int it=0;it<nbParam;it++)
- GDir[it]+=er*tempt[it];
-
- delete[] tempt;
- }
-
-
- }
- if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl;
- try
- {
- dp=(HInv+HDir).inverseByLU()*(GInv+GDir);
- }
- catch(...)
- {
- std::cout<<"probleme inversion"<<std::endl;
- break;
- }
-
- if(Compare)write_infos(p,erreur/Nbpoint);
-
- p+=Gain*dp;
- iteration++;
-
- }
- while( (iteration < IterationMax));
-
- NbIteration=iteration;
-}*/
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.h b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.h
deleted file mode 100644
index 3ab98a1..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDESM.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDESM.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerSSDESM.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerSSDESM_hh
-#define vpTemplateTrackerSSDESM_hh
-
-#include <visp/vpTemplateTrackerSSD.h>
-
-class VISP_EXPORT vpTemplateTrackerSSDESM: public vpTemplateTrackerSSD
-{
- protected:
- bool compoInitialised;
- vpMatrix HDir;
- vpMatrix HInv;
- vpMatrix HLMDir;
- vpMatrix HLMInv;
- vpColVector GDir;
- vpColVector GInv;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompInverse(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
-
- public:
- vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp);
-};
-#endif
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.cpp b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
deleted file mode 100644
index 90480f1..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
+++ /dev/null
@@ -1,213 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDForwardAdditional.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <limits> // numeric_limits
-
-#include <visp/vpTemplateTrackerSSDForwardAdditional.h>
-#include <visp/vpImageTools.h>
-
-vpTemplateTrackerSSDForwardAdditional::vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerSSD(warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
-{
- useCompositionnal=false;
-}
-
-void vpTemplateTrackerSSDForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
-{
- double erreur=0;
- unsigned int Nbpoint=0;
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- dW=0;
-
- double lambda=lambdaDep;
- double IW,dIWx,dIWy;
- double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- Nbpoint=0;
- erreur=0;
- G=0;
- H=0 ;
- Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- Nbpoint++;
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=tempt[it]*tempt[jt];
-
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=er*tempt[it];
-
- erreur+=(er*er);
- delete[] tempt;
- }
-
-
- }
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
-
- vpMatrix::computeHLM(H,lambda,HLM);
- try
- {
- dp=1.*HLM.inverseByLU()*G;
- }
- catch(vpException &e)
- {
- throw(e);
- }
-
- switch(minimizationMethod)
- {
- case vpTemplateTrackerSSDForwardAdditional::USE_LMA:
- {
- vpColVector p_test_LMA(nbParam);
- p_test_LMA=p+1.*dp;
- erreur=-getCost(I,p);
- double erreur_LMA=-getCost(I,p_test_LMA);
- if(erreur_LMA<erreur)
- {
- p=p_test_LMA;
- lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
- }
- else
- {
- lambda=(lambda*10.<1e6)?1e6:lambda*10.;
- }
- }
- break;
- case vpTemplateTrackerSSDForwardAdditional::USE_GRADIENT:
- {
- dp=gain*0.000001*G;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur,dp,alpha);
- dp=alpha*dp;
- }
- p+=1.*dp;
- break;
- }
-
- case vpTemplateTrackerSSDForwardAdditional::USE_QUASINEWTON:
- {
- double s_scal_y;
- if(iterationGlobale!=0)
- {
- vpColVector s_quasi=p-p_prec;
- vpColVector y_quasi=G-G_prec;
- s_scal_y=s_quasi.t()*y_quasi;
- //if(s_scal_y!=0)//BFGS
- // KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
- //if(s_scal_y!=0.0)//DFP
- if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) //DFP
- KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
- }
- dp=-KQuasiNewton*G;
- p_prec=p;
- G_prec=G;
- p-=1.01*dp;
- }
- break;
-
- case vpTemplateTrackerSSDForwardAdditional::USE_NEWTON:
- default:
- {
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur,dp,alpha);
- dp=alpha*dp;
- }
-
- p+=1.*dp;
- break;
- }
- }
-
- iteration++;
- iterationGlobale++;
- }
- while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
-
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
-}
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.h b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.h
deleted file mode 100644
index 4d57db0..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardAdditional.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDForwardAdditional.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerSSDForwardAdditional.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerSSDForwardAdditional_hh
-#define vpTemplateTrackerSSDForwardAdditional_hh
-
-#include <visp/vpTemplateTrackerSSD.h>
-
-/*!
- The algorithm implemented in this class is described in \cite Baker04a.
- */
-class VISP_EXPORT vpTemplateTrackerSSDForwardAdditional: public vpTemplateTrackerSSD
-{
- public:
- /*! Minimization method. */
- typedef enum {
- USE_NEWTON,
- USE_LMA,
- USE_GRADIENT,
- USE_QUASINEWTON
- } vpMinimizationTypeSSDForwardAdditional;
-
- private:
- vpMinimizationTypeSSDForwardAdditional minimizationMethod;
- //valeur pour calculer Quasi_Newton
- vpColVector p_prec;
- vpColVector G_prec;
- vpMatrix KQuasiNewton;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &/*I*/){}
- void trackNoPyr(const vpImage<unsigned char> &I);
-
- public:
- vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp);
-
- void setMinimizationMethod(vpMinimizationTypeSSDForwardAdditional method){minimizationMethod=method;}
-};
-#endif
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.cpp b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
deleted file mode 100644
index d3e605e..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDForwardCompositional.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerSSDForwardCompositional.h>
-#include <visp/vpImageFilter.h>
-
-vpTemplateTrackerSSDForwardCompositional::vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerSSD(warp), compoInitialised(false)
-{
-}
-
-void vpTemplateTrackerSSDForwardCompositional::initCompo(const vpImage<unsigned char> &/*I*/)
-{
- // std::cout<<"Initialise precomputed value of Compositionnal Direct"<<std::endl;
- int i,j;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- ptTemplate[point].dW=new double[2*nbParam];
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->getdWdp0(i,j,ptTemplate[point].dW);
-
- }
- compoInitialised=true;
-}
-
-void vpTemplateTrackerSSDForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompo(I);
-}
-
-void vpTemplateTrackerSSDForwardCompositional::trackNoPyr(const vpImage<unsigned char> &I)
-{
- if(!compoInitialised)
- std::cout<<"Compositionnal tracking no initialised\nUse InitCompo(vpImage<unsigned char> &I) function"<<std::endl;
- double erreur=0;
- unsigned int Nbpoint=0;
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- dW=0;
-
- double lambda=lambdaDep;
- double IW,dIWx,dIWy;
- double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- Nbpoint=0;
- erreur=0;
- G=0;
- H=0 ;
- Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- Nbpoint++;
- //Calcul du Hessien
- /*Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;*/
-
- Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
-
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=tempt[it]*tempt[jt];
-
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=er*tempt[it];
-
- erreur+=(er*er);
- delete[] tempt;
- }
-
-
- }
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
-
- vpMatrix::computeHLM(H,lambda,HLM);
-
- try
- {
- dp=1.*HLM.inverseByLU()*G;
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
- throw(e);
- }
-
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
- }
- Warp->pRondp(p,dp,p);
- //p+=Gain*dp;
- iteration++;
- }
- while( /*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax));
-
- nbIteration=iteration;
-}
-
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.h b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.h
deleted file mode 100644
index 76134fe..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDForwardCompositional.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDForwardCompositional.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerSSDForwardCompositional.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerSSDForwardCompositional_hh
-#define vpTemplateTrackerSSDForwardCompositional_hh
-
-#include <visp/vpTemplateTrackerSSD.h>
-
-/*!
- The algorithm implemented in this class is described in \cite Baker04a.
- */
-class VISP_EXPORT vpTemplateTrackerSSDForwardCompositional: public vpTemplateTrackerSSD
-{
- protected:
- bool compoInitialised;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompo(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
-
- public:
- vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp);
-};
-#endif
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.cpp b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
deleted file mode 100644
index bd955a5..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
+++ /dev/null
@@ -1,242 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDInverseCompositional.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp/vpImageTools.h>
-
-vpTemplateTrackerSSDInverseCompositional::vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerSSD(warp), compoInitialised(false), HInv(), HCompInverse(), useTemplateSelect(false),
- evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8)
-{
- useInverse=true;
- HInv.resize(nbParam,nbParam);
- HCompInverse.resize(nbParam,nbParam);
-}
-
-void vpTemplateTrackerSSDInverseCompositional::initCompInverse(const vpImage<unsigned char> &/*I*/)
-{
-
- H=0;
- int i,j;
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
-
- Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
- }
-
- }
- HInv=H;
- vpMatrix HLMtemp(nbParam,nbParam);
- vpMatrix::computeHLM(H,lambdaDep,HLMtemp);
-
- HCompInverse.resize(nbParam,nbParam);
- HCompInverse=HLMtemp.inverseByLU();
- //std::cout<<Hinverse<<std::endl;
- vpColVector dWtemp(nbParam);
- vpColVector HiGtemp(nbParam);
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- for(unsigned int it=0;it<nbParam;it++)
- dWtemp[it]=ptTemplate[point].dW[it];
-
- HiGtemp = -1.*HCompInverse*dWtemp;
- ptTemplate[point].HiG=new double[nbParam];
-
- for(unsigned int it=0;it<nbParam;it++)
- ptTemplate[point].HiG[it]=HiGtemp[it];
- }
- }
- compoInitialised=true;
-}
-
-void vpTemplateTrackerSSDInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompInverse(I);
-}
-
-void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
-{
- double erreur=0;
- unsigned int Nbpoint=0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
-
- vpColVector dpinv(nbParam);
- double IW;
- double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- //vpTemplateTrackerPointtest *pt;
- initPosEvalRMS(p);
-
- vpTemplateTrackerPoint *pt;
- do
- {
- Nbpoint=0;
- erreur=0;
- dp=0;
- Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- //pt=&ptTemplatetest[point];
- pt=&ptTemplate[point];
- i=pt->y;
- j=pt->x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=pt->val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- Nbpoint++;
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- dp[it]+=er*pt->HiG[it];
-
- erreur+=er*er;
- }
- }
- }
- //std::cout << "npoint: " << Nbpoint << std::endl;
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- deletePosEvalRMS();
- throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
- dp=gain*dp;
- //std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
- }
- Warp->getParamInverse(dp,dpinv);
- Warp->pRondp(p,dpinv,p);
- iteration++;
-
- computeEvalRMS(p);
- //std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
- //std::cout << "evolRMS: " << evolRMS << " threshold: " << threshold_RMS << std::endl;
- }
- while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));
-
- nbIteration=iteration;
- deletePosEvalRMS();
-}
-
-void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS(vpColVector &p_)
-{
- unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
- x_pos.resize(nb_corners);
- y_pos.resize(nb_corners);
-
- Warp->computeCoeff(p_);
- vpTemplateTrackerTriangle triangle;
-
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
- zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
- triangle.getCorner(j, X1[0], X1[1]);
-
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
- }
- }
-}
-
-void vpTemplateTrackerSSDInverseCompositional::computeEvalRMS(const vpColVector &p_)
-{
- unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
-
- Warp->computeCoeff(p_);
- evolRMS=0;
- vpTemplateTrackerTriangle triangle;
-
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
- zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
- triangle.getCorner(j, X1[0], X1[1]);
-
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
- }
- }
- evolRMS=evolRMS/nb_corners;
-}
-
-void vpTemplateTrackerSSDInverseCompositional::deletePosEvalRMS()
-{
-}
diff --git a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.h b/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.h
deleted file mode 100644
index c2c39e1..0000000
--- a/src/tracking/template-tracker/ssd/vpTemplateTrackerSSDInverseCompositional.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerSSDInverseCompositional.h 4672 2014-02-17 09:01:17Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerSSDInverseCompositional.h
- \brief
-*/
-#ifndef vpTemplateTrackerSSDInverseCompositional_hh
-#define vpTemplateTrackerSSDInverseCompositional_hh
-
-#include <vector>
-
-#include <visp/vpTemplateTrackerSSD.h>
-
-/*!
- The algorithm implemented in this class is described in \cite Baker04a.
- */
-class VISP_EXPORT vpTemplateTrackerSSDInverseCompositional: public vpTemplateTrackerSSD
-{
- protected:
- bool compoInitialised;
- vpMatrix HInv;
- vpMatrix HCompInverse;
- bool useTemplateSelect;//use only the strong gradient pixels to compute the Jabocian
- //pour eval evolRMS
- double evolRMS;
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- double threshold_RMS;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompInverse(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
- void deletePosEvalRMS();
- void computeEvalRMS(const vpColVector &p);
- void initPosEvalRMS(vpColVector &p);
-
- public:
- vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp);
-
- /*! Use only the strong gradient pixels to compute the Jabobian. By default this feature is disabled. */
- void setUseTemplateSelect(bool b) {useTemplateSelect = b;}
- void setThresholdRMS(double threshold){threshold_RMS=threshold;}
-};
-#endif
-
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.cpp b/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.cpp
deleted file mode 100644
index 7e3c336..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerBSpline.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerBSpline.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage<double> &I, double r, double t)
-{
- double res=0;
- int cr=(int)(r);
- int ct=(int)(t);
- double er=(double)r-cr;
- double et=(double)t-ct;
- int height=(int)I.getHeight();//r
- int width=(int)I.getWidth();//t
- int tr,tt;
- for(int ir=-1;ir<=2;ir++)
- {
- tr=ir+cr;
- for(int it=-1;it<=2;it++)
- {
- tt=it+ct;
- if(tr>=0 && tr <height && tt>=0 && tt <width)
- res+=Bspline4((double)ir-er)*Bspline4((double)it-et)*I[tr][tt];
- }
- }
- return res;
-}
-
-double vpTemplateTrackerBSpline::Bspline4(double diff)
-{
- //double result;
- double aDiff=vpMath::abs(diff);
- if(aDiff<1.)
- return (aDiff*aDiff*aDiff/2.-aDiff*aDiff+4./6.);
- //return (0.5*(1.-aDiff)*(1.-aDiff)*(1.-aDiff)+0.5*(1.-aDiff)*(1.-aDiff)-0.5*(1.-aDiff)+1./6.);
- else if(aDiff<2.)
- return ((2.-aDiff)*(2.-aDiff)*(2.-aDiff)/6.);
- else
- return 0;
-}
-
-#endif
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.h b/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.h
deleted file mode 100644
index 7a372d3..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerBSpline.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerBSpline.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerBSpline.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerBSpline_hh
-#define vpTemplateTrackerBSpline_hh
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-class VISP_EXPORT vpTemplateTrackerBSpline
-{
-public:
- static double Bspline4(double diff);
-
- static double getSubPixBspline4(const vpImage<double> &I, double r, double t);
-};
-#endif
-#endif
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerHeader.h b/src/tracking/template-tracker/tools/vpTemplateTrackerHeader.h
deleted file mode 100644
index 339ff92..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerHeader.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerHeader.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerHeader.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerHeader_hh
-#define vpTemplateTrackerHeader_hh
-
-#include <stdio.h>
-
-struct vpTemplateTrackerZPoint {
- int x,y;
-
- vpTemplateTrackerZPoint() : x(0), y(0) {}
-};
-struct vpTemplateTrackerDPoint {
- double x,y;
-
- vpTemplateTrackerDPoint() : x(0), y(0) {}
-};
-struct vpTemplateTrackerPoint {
- int x,y;
- double dx,dy;
- double val;
- double *dW;
- double *HiG;
-
- vpTemplateTrackerPoint() : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) {}
-};
-struct vpTemplateTrackerPointCompo {
- double *dW;
- vpTemplateTrackerPointCompo() : dW(NULL) {}
-};
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-struct vpTemplateTrackerPointSuppMIInv {
- double et;
- int ct;
- double *BtInit;
- double *Bt;
- double *dBt;
- double *d2W;
- double *d2Wx;
- double *d2Wy;
- vpTemplateTrackerPointSuppMIInv() : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL),
- d2W(NULL), d2Wx(NULL), d2Wy(NULL) {}
-};
-#endif // DOXYGEN_SHOULD_SKIP_THIS
-
-#endif
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.cpp b/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.cpp
deleted file mode 100644
index 10c6f8f..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.cpp
+++ /dev/null
@@ -1,438 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerTriangle.cpp 5060 2014-12-12 18:31:03Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerTriangle.h>
-
-/*!
- Default constructor.
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle()
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
-}
-
-/*!
- Copy constructor.
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
- *this = T;
-}
-
-/*!
- Copy operator.
- */
-vpTemplateTrackerTriangle & vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle& T)
-{
- minx_temp=T.minx_temp;
- miny_temp=T.miny_temp;
-
- l_t=T.l_t;
- h_t=T.h_t;
- C1.x=T.C1.x;
- C1.y=T.C1.y;
- C2.x=T.C2.x;
- C2.y=T.C2.y;
- C3.x=T.C3.x;
- C3.y=T.C3.y;
- //uvinv.resize(2,2);
- //uvinv=T.uvinv;
- //p_ds_uv.resize(2);
- //p_ds_uv=T.p_ds_uv;
- //ptempo.resize(2);
- //ptempo=T.ptempo;
- not_good=T.not_good;
-
- uvinv00=T.uvinv00;
- uvinv01=T.uvinv01;
- uvinv10=T.uvinv10;
- uvinv11=T.uvinv11;
-
- marge_triangle = T.marge_triangle;
- area = T.area;
-
- return (*this);
-}
-
-/*!
- Create a triangle from 3 corners.
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
-
- The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpColVector &c1,
- const vpColVector &c2,
- const vpColVector &c3)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
- init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
-}
-/*!
- Return a triangle with coordinates that are down scaled by a factor 2.
- */
-vpTemplateTrackerTriangle vpTemplateTrackerTriangle::getPyramidDown() const
-{
- vpTemplateTrackerTriangle Ttemp;
- Ttemp.init(C1.x/2.,C1.y/2.,C2.x/2.,C2.y/2.,C3.x/2.,C3.y/2.);
- return Ttemp;
-}
-
-/*!
- Create a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
- - x coordinate is along the columns
- - y coordinate is along the rows.
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1,int y1, int x2,int y2, int x3,int y3)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
- init(x1,y1,x2,y2,x3,y3);
-}
-
-/*!
- Create a triangle from 3 corners defined as image points.
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
- init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
-}
-
-/*!
- Create a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
- - x coordinate is along the columns
- - y coordinate is along the rows.
- */
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1,double y1, double x2,double y2, double x3,double y3)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
-{
- init(x1,y1,x2,y2,x3,y3);
-}
-/*!
- Initializes a triangle from 3 corners.
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
-
- The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
- */
-void vpTemplateTrackerTriangle::init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3)
-{
- init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
-}
-/*!
- Initializes a triangle from 3 corners defined as image points.
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
- */
-void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
-{
- init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
-}
-
-/*!
- Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
- - x coordinate is along the columns
- - y coordinate is along the rows.
- */
-void vpTemplateTrackerTriangle::init(int x1, int y1, int x2,int y2, int x3, int y3)
-{
- init((double)x1,(double)y1,(double)x2,(double)y2,(double)x3,(double)y3);
-}
-
-/*!
- Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
- - x coordinate is along the columns
- - y coordinate is along the rows.
- */
-void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2, double x3, double y3)
-{
- C1.x=x1;C1.y=y1;
- C2.x=x2;C2.y=y2;
- C3.x=x3;C3.y=y3;
-
- double minx,miny,maxx,maxy;
- //calcul du rectangle minimal contenant le triangle seletionne
- minx=(x1<x2)?x1:x2;
- miny=(y1<y2)?y1:y2;
- minx=(minx<x3)?minx:x3;
- miny=(miny<y3)?miny:y3;
- maxx=(x1>x2)?x1:x2;
- maxy=(y1>y2)?y1:y2;
- maxx=(maxx>x3)?maxx:x3;
- maxy=(maxy>y3)?maxy:y3;
-
- vpColVector u;
- vpColVector v;
- u.resize(2);
- v.resize(2);
- vpMatrix uv(2,2);
- vpMatrix uvinv(2,2);
-
- u[0]=C2.x-C1.x;
- u[1]=C2.y-C1.y;
-
- v[0]=C3.x-C1.x;
- v[1]=C3.y-C1.y;
-
- uv[0][0]=u[0];uv[1][0]=v[0];
- uv[0][1]=u[1];uv[1][1]=v[1];
- try
- {
- uvinv=uv.inverseByLU();
- not_good=false;
- }
- catch(...)
- {
- not_good=true;
- std::cout<<"Triangle vide"<<std::endl;
-
- }
- uvinv00=uvinv[0][0];
- uvinv01=uvinv[0][1];
- uvinv10=uvinv[1][0];
- uvinv11=uvinv[1][1];
-
- l_t=maxx-minx;
- h_t=maxy-miny;
- minx_temp=minx;
- miny_temp=miny;
-
- marge_triangle = 0.00001;
- area = 0.5 * fabs(uv.det());
-}
-
-//marge ajoutee a zone pour que sommet soit pris en compte
-
-/*!
- Indicates if a point with coordinates (i,j) is in the triangle.
- \param i : Coordinate along the rows.
- \param j : Coordinate along the columns.
- */
-bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
-{
- if(not_good)
- return false;
-
- /*ptempo[0]=j-C1.x;
- ptempo[1]=i-C1.y;
-
- p_ds_uv=ptempo*uvinv;
- return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
-
- double ptempo0=j-C1.x;
- double ptempo1=i-C1.y;
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
- return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
-}
-
-/*!
- Indicates if a point with coordinates (i,j) is in the triangle.
- \param i : Coordinate along the rows.
- \param j : Coordinate along the columns.
- */
-bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
-{
- if(not_good)
- return false;
- /*ptempo[0]=j-C1.x;
- ptempo[1]=i-C1.y;
-
- p_ds_uv=ptempo*uvinv;
- return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
- double ptempo0=j-C1.x;
- double ptempo1=i-C1.y;
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
- return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
-}
-
-/*!
- Indicates if an image point is in the triangle.
- \param ip : Image point to consider.
- */
-bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const
-{
- return inTriangle(ip.get_i(), ip.get_j());
-}
-/*!
- Returns the coordinates of the triangle corners as an image point.
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
- */
-void vpTemplateTrackerTriangle::getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const
-{
- c1.set_uv(C1.x, C1.y);
- c2.set_uv(C2.x, C2.y);
- c3.set_uv(C3.x, C3.y);
-}
-
-/*!
- Returns the coordinates of the triangle corners as a 3 dimension vector of image points.
- \param c : 3 dimension vector of image points that correspond to the triangle corners.
- */
-void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
-{
- c.resize(3);
- c[0].set_uv(C1.x, C1.y);
- c[1].set_uv(C2.x, C2.y);
- c[2].set_uv(C3.x, C3.y);
-}
-
-/*!
- Returns the coordinates of the triangle corners as a 2 dimension vector (x,y).
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
- */
-void vpTemplateTrackerTriangle::getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const
-{
- c1=getCorner1();
- c2=getCorner2();
- c3=getCorner3();
-}
-
-/*!
- Returns the coordinates of the triangle first corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
- */
-vpColVector vpTemplateTrackerTriangle::getCorner1() const
-{
- vpColVector c(2);
- c[0]=C1.x;
- c[1]=C1.y;
-
- return c;
-}
-/*!
- Returns the coordinates of the triangle second corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
- */
-vpColVector vpTemplateTrackerTriangle::getCorner2() const
-{
- vpColVector c(2);
- c[0]=C2.x;
- c[1]=C2.y;
- return c;
-}
-
-/*!
- Returns the coordinates of the triangle third corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
- */
-vpColVector vpTemplateTrackerTriangle::getCorner3() const
-{
- vpColVector c(2);
- c[0]=C3.x;
- c[1]=C3.y;
- return c;
-}
-
-/*!
- Get the size of the triangle bounding box.
- \param w : Bounding box width.
- \param h : Bounding box height.
- */
-void vpTemplateTrackerTriangle::getSize(double &w,double &h) const
-{
- w=l_t;
- h=h_t;
-}
-/*!
- Get the size of the triangle bounding box.
- \param w : Bounding box width.
- \param h : Bounding box height.
- */
-void vpTemplateTrackerTriangle::getSize(int &w,int &h) const
-{
- w=(int)l_t+1;
- h=(int)h_t+1;
-}
-
-/*!
- \return The minimal x coordinate (along the columns of the image) of the points that are in the triangle.
- \sa getMaxx()
- */
-double vpTemplateTrackerTriangle::getMinx() const
-{
- return minx_temp-1;
-}
-/*!
- \return The minimal y coordinate (along the rows of the image) of the points that are in the triangle.
- \sa getMaxy()
- */
-double vpTemplateTrackerTriangle::getMiny() const
-{
- return miny_temp-1;
-}
-/*!
- \return The maximal x coordinate (along the columns of the image) of the points that are in the triangle.
- \sa getMinx()
- */
-double vpTemplateTrackerTriangle::getMaxx() const
-{
- return minx_temp+l_t+1;
-}
-/*!
- \return The maximal y coordinate (along the rows of the image) of the points that are in the triangle.
- \sa getMaxx()
- */
-double vpTemplateTrackerTriangle::getMaxy() const
-{
- return miny_temp+h_t+1;
-}
-
-
-
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.h b/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.h
deleted file mode 100644
index e4324d5..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerTriangle.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerTriangle.h 4781 2014-07-15 13:03:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerTriangle.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerTriangle_hh
-#define vpTemplateTrackerTriangle_hh
-
-#include <assert.h>
-#include <vector>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpMath.h>
-#include <visp/vpColVector.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpTemplateTrackerHeader.h>
-
-class VISP_EXPORT vpTemplateTrackerTriangle
-{
- protected:
- double minx_temp;
- double miny_temp;
- vpTemplateTrackerDPoint C1; //! Corner 1
- vpTemplateTrackerDPoint C2; //! Corner 2
- vpTemplateTrackerDPoint C3; //! Corner 3
-
- double l_t;
- double h_t;
-
- bool not_good;
- double uvinv00;
- double uvinv01;
- double uvinv10;
- double uvinv11;
- double marge_triangle;
- double area;
-
- private:
- vpColVector getCorner1() const;
- vpColVector getCorner2() const;
- vpColVector getCorner3() const;
-
- public:
- vpTemplateTrackerTriangle();
- vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T);
- vpTemplateTrackerTriangle(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3);
- vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
- vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
- vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3);
-
- /*!
- Return the area of the triangle.
-
- \return The area of the triangle.
- */
- inline double getArea() const{
- return this->area;
- }
-
- vpTemplateTrackerTriangle getPyramidDown() const;
- void getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const;
- void getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const;
- void getCorners(std::vector<vpImagePoint> &c) const;
-
- /*!
- Returns the coordinates of a triangle corner.
- \param i : Allowed values are 0, 1 or 2.
- \return
- - if i = 0, return corner 1 coordinates,
- - if i = 1, return corner 2 coordinates,
- - if i = 2, return corner 3 coordinates.
-
- The coordinates are returned as a 2 dimension vector (x,y).
- */
- vpColVector getCorner(unsigned int i) const {
- assert(i<3);
- if(i==0) return getCorner1();
- else if(i==1) return getCorner2();
- else /*if(i==2)*/ return getCorner3();
- };
- /*!
- Returns the coordinates of a triangle corner.
- \param i : Allowed values are 0, 1 or 2.
- - if i = 0, return corner 1 coordinates,
- - if i = 1, return corner 2 coordinates,
- - if i = 2, return corner 3 coordinates.
- \param x,y : The coordinates of the corner.
- */
- void getCorner(unsigned int i, double &x, double &y) const {
- assert(i<3);
- if(i==0) {
- x = C1.x;
- y = C1.y;
- }
- else if(i==1) {
- x = C2.x;
- y = C2.y;
- }
- else /*if(i==2)*/ {
- x = C3.x;
- y = C3.y;
- }
- };
-
- double getMaxx() const;
- double getMaxy() const;
- double getMinx() const;
- double getMiny() const;
-
- void getSize(double &w, double &h) const;
- void getSize(int &w, int &h) const;
-
- void init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3);
- void init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
- void init(int x1,int y1, int x2,int y2, int x3,int y3);
- void init(double x1,double y1, double x2,double y2, double x3,double y3);
- bool inTriangle(const vpImagePoint &ip) const;
- bool inTriangle(const int &i, const int &j) const;
- bool inTriangle(const double &i,const double &j) const;
-
- vpTemplateTrackerTriangle & operator=(const vpTemplateTrackerTriangle& T);
-};
-#endif
-
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerZone.cpp b/src/tracking/template-tracker/tools/vpTemplateTrackerZone.cpp
deleted file mode 100644
index 573786a..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerZone.cpp
+++ /dev/null
@@ -1,619 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZone.cpp 5119 2015-01-05 10:02:46Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <limits> // numeric_limits
-
-#include <visp/vpConfig.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020300
-#include <opencv2/imgproc/imgproc.hpp>
-#endif
-
-#include <visp/vpTemplateTrackerZone.h>
-
-
-/*!
- Default constructor.
- */
-vpTemplateTrackerZone::vpTemplateTrackerZone()
- : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
-{
-}
-
-/*!
- Copy constructor.
- */
-vpTemplateTrackerZone::vpTemplateTrackerZone(const vpTemplateTrackerZone &z)
- : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
-{
- *this = z;
-}
-
-/*!
- Remove all the triangles that define the zone.
- */
-void vpTemplateTrackerZone::clear()
-{
- min_x=-1;
- min_y=-1;
- max_x=-1;
- max_y=-1;
-
- Zone.clear();
-}
-
-/*!
- Copy operator.
- */
-vpTemplateTrackerZone & vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z)
-{
- clear();
-
- this->copy(z);
- return (*this);
-}
-
-/*!
- Initialize a zone in image \e I using mouse click.
-
- \param I : Image used to select the zone.
- \param delaunay : Flag used to enable Delaunay triangulation.
- - If true, from the image points selected by the user, a Delaunay triangulation is performed
- to initialize the zone.
- - A left click select a image point;
- - A right click select the last image point and ends the initialisation stage.
- In that case at least 3 points need to be selected by the user.
- - If false, the user select directly points as successive triangle corners.
- Three successive points define a triangle. It is not mandatory
- that triangles have one edge in common; they can define a discontinued area.
- - A left click select a triangle corner;
- - A right click select the last triangle corner and ends the initialisation stage.
- The number of points that are selected by the user should be a multiple of 3.
- For example, to select a zone as two triangles, the user has to left click
- five times and finish the selection on the sixth corner with a right click.
-
- */
-void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool delaunay)
-{
- Zone.clear();
-
- std::vector<vpImagePoint> vip;
-
- bool end = false;
-
- do {
- vpImagePoint p;
- vpMouseButton::vpMouseButtonType button;
- if (vpDisplay::getClick(I, p, button, false) ) {
- vip.push_back(p);
-
- vpDisplay::displayCross(I, p, 7, vpColor::red);
-
- if (vip.size() > 1) {
- if (delaunay) {
- // Draw a line between the 2 last points
- vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
- }
- else {
- if(vip.size() % 3 ==2)
- // draw line between point 2-1
- vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
- else if(vip.size() % 3 ==0) {
- // draw line between point 3-2
- vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
- // draw line between point 3-1
- vpDisplay::displayLine(I, p, vip[vip.size()-3], vpColor::blue, 3);
- }
-
- }
- }
-
- if (button == vpMouseButton::button3)
- end = true;
- }
-
- vpTime::wait(20);
- vpDisplay::flush(I);
- } while(!end);
-
- initFromPoints(I, vip, delaunay);
-}
-
-/*!
-
- Initialize the zone using a vector of image points.
-
- \param I : Image to process.
- \param vip : Vector of image points used as initialization.
- \param delaunay :
- - If true, a Delaunay triangulation is perfomed on the vector of image points. This functionality
- is only available if ViSP is build with OpenCV >2.3 third-party.
- - If false, the vector of image points describe triangles. Its size is then a multiple of 3.
- */
-void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &vip, bool delaunay)
-{
- if (delaunay) {
- if(vip.size() == 3) {
- initFromPoints(I, vip, false);
- }
- else if(vip.size() == 4) {
- std::vector<vpImagePoint> vip_delaunay;
- vip_delaunay.push_back(vip[0]);
- vip_delaunay.push_back(vip[1]);
- vip_delaunay.push_back(vip[2]);
- vip_delaunay.push_back(vip[2]);
- vip_delaunay.push_back(vip[3]);
- vip_delaunay.push_back(vip[0]);
- initFromPoints(I, vip_delaunay, false);
- }
- else {
-#if VISP_HAVE_OPENCV_VERSION >= 0x020300
- // Init Delaunay
- cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
- for(size_t i=0; i< vip.size(); i++) {
- cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
- //std::cout << "Click point: " << vip[i] << std::endl;
- subdiv.insert(fp);
- }
-
- // Compute Delaunay triangulation
- std::vector<cv::Vec6f> triangleList;
- subdiv.getTriangleList(triangleList);
-
- // Keep only the Delaunay points that are inside the area
- vpRect rect(0, 0, I.getWidth(), I.getHeight());
-
- std::vector<vpImagePoint> vip_delaunay;
- for( size_t i = 0; i < triangleList.size(); i++ ) {
- cv::Vec6f t = triangleList[i];
- std::vector<vpImagePoint> p(3);
-
- p[0].set_uv(t[0], t[1]);
- p[1].set_uv(t[2], t[3]);
- p[2].set_uv(t[4], t[5]);
-
- if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
- vip_delaunay.push_back(p[0]);
- vip_delaunay.push_back(p[1]);
- vip_delaunay.push_back(p[2]);
- }
- }
-
- initFromPoints(I, vip_delaunay, false);
-#else
- throw vpException(vpException::functionNotImplementedError,"Delaunay triangulation is not available!");
-#endif
- }
- }
- else {
- Zone.clear();
- for(unsigned int i=0; i<vip.size(); i+=3) {
- vpTemplateTrackerTriangle triangle(vip[i], vip[i+1], vip[i+2]);
- add(triangle);
-
-// vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green, 1);
-// vpDisplay::displayLine(I, vip[i+1], vip[i+2], vpColor::green, 1);
-// vpDisplay::displayLine(I, vip[i+2], vip[i], vpColor::green,1);
-// vpDisplay::flush(I) ;
-
- // Update the bounding box
- if((triangle.getMinx()<min_x)||(min_x==-1))
- min_x=(int)triangle.getMinx();
- if((triangle.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)triangle.getMaxx();
- if((triangle.getMiny()<min_y)||(min_y==-1))
- min_y=(int)triangle.getMiny();
- if((triangle.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)triangle.getMaxy();
- }
- }
-}
-
-/*!
- Add a triangle to the zone and update the bounding box.
- \param t : Triangle to introduce in the zone.
- */
-void vpTemplateTrackerZone::add(const vpTemplateTrackerTriangle &t)
-{
- Zone.push_back(t);
-
- // Update the bounding box
- if((t.getMinx()<min_x)||(min_x==-1))
- min_x=(int)t.getMinx();
- if((t.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)t.getMaxx();
- if((t.getMiny()<min_y)||(min_y==-1))
- min_y=(int)t.getMiny();
- if((t.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)t.getMaxy();
-}
-
-/*!
- Test if a pixel with coordinates (i,j) is in the zone..
- \param i, j : Coordinates of the pixel to test.
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
- */
-bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
-{
- std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- return true;
- }
- return false;
-}
-
-/*!
- Test if a pixel with coordinates (i,j) is in the zone..
- \param i, j : Coordinates of the pixel to test.
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
- */
-bool vpTemplateTrackerZone::inZone(const double &i,const double &j) const
-{
- std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- return true;
- }
- return false;
-}
-
-/*!
- Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
- the triangle that contains the pixel.
- \param i, j : Coordinates of the pixel to test.
- \param id_triangle : Index of the triangle that contains the pixel (i,j).
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
- */
-bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_triangle) const
-{
- unsigned int id=0;
- std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- {
- id_triangle=id;
- return true;
- }
- id++;
- }
- return false;
-}
-
-/*!
- Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
- the triangle that contains the pixel.
- \param i, j : Coordinates of the pixel to test.
- \param id_triangle : Index of the triangle that contains the pixel (i,j).
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
- */
-bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int &id_triangle) const
-{
- unsigned int id=0;
- std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- {
- id_triangle=id;
- return true;
- }
- id++;
- }
- return false;
-}
-
-/*!
- A zone is defined by a set of triangles. This function returns the ith triangle.
- \param i : Index of the triangle to return.
- \param T : The triangle corresponding to index i.
- \return true if the triangle with index i was found, false otherwise.
-
- The following sample code shows how to use this function:
- \code
- vpTemplateTrackerZone zone;
- ...
- for (unsigned int i=0; i < zone.getNbTriangle(); i++) {
- vpTemplateTrackerTriangle triangle;
- zone.getTriangle(i, triangle);
- }
- \endcode
- */
-void vpTemplateTrackerZone::getTriangle(unsigned int i,vpTemplateTrackerTriangle &T) const
-{
- if (i > getNbTriangle()-1)
- throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
-
- T = Zone[i];
-}
-/*!
- A zone is defined by a set of triangles. This function returns the ith triangle.
- \param i : Index of the triangle to return.
- \return The triangle corresponding to index i.
-
- The following sample code shows how to use this function:
- \code
- vpTemplateTrackerZone zone;
- ...
- for (unsigned int i=0; i < zone.getNbTriangle(); i++) {
- vpTemplateTrackerTriangle triangle = zone.getTriangle(i);
- }
- \endcode
- */
-vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) const
-{
- if (i > getNbTriangle()-1)
- throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
-
- return Zone[i];
-}
-/*!
- Return the position of the center of gravity of the zone.
- \exception vpException::divideByZeroError: The size of the zone is null.
- */
-vpImagePoint vpTemplateTrackerZone::getCenter() const
-{
- double xc=0;
- double yc=0;
- int cpt=0;
- for(int i=min_y;i<max_y;i++)
- for(int j=min_x;j<max_x;j++)
- if(inZone(i,j))
- {
- xc+=j;
- yc+=i;
- cpt ++;
- }
- if(! cpt) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot compute the zone center: size = 0")) ;
- }
- xc=xc/cpt;
- yc=yc/cpt;
- vpImagePoint ip;
- ip.set_uv(xc, yc);
- return ip;
-}
-
-/*!
- \return The maximal x coordinate (along the columns of the image) of the points that are in the zone.
- \sa getMinx(), getBoundingBox()
- */
-int vpTemplateTrackerZone::getMaxx() const
-{
- return max_x;
-}
-/*!
- \return The maximal y coordinate (along the rows of the image) of the points that are in the zone.
- \sa getMiny(), getBoundingBox()
- */
-int vpTemplateTrackerZone::getMaxy() const
-{
- return max_y;
-}
-/*!
- \return The minimal x coordinate (along the columns of the image) of the points that are in the zone.
- \sa getMaxx(), getBoundingBox()
- */
-int vpTemplateTrackerZone::getMinx() const
-{
- return min_x;
-}
-/*!
- \return The minimal y coordinate (along the rows of the image) of the points that are in the zone.
- \sa getMaxy(), getBoundingBox()
- */
-int vpTemplateTrackerZone::getMiny() const
-{
- return min_y;
-}
-
-/*!
- Return a rectangle that defines the bounding box of the zone.
- \sa getMinx(), getMiny(), getMaxx(), getMaxy()
- */
-vpRect vpTemplateTrackerZone::getBoundingBox() const
-{
- vpRect bbox;
- bbox.setTopLeft(vpImagePoint(min_y, min_x));
- bbox.setBottomRight(vpImagePoint(max_y, max_x));
- return bbox;
-}
-
-/*!
- If a display device is associated to image \c I, display in overlay the triangles that define the zone.
- \param I : Image.
- \param col : Color used to display the triangles.
- \param thickness : Thickness of the triangle lines.
- */
-void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
-{
- std::vector<vpImagePoint> ip;
- for (unsigned int i=0; i < Zone.size(); i++) {
- vpTemplateTrackerTriangle triangle;
- Zone[i].getCorners(ip);
- vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
- vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
- vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
- }
-}
-
-/*!
- If a display device is associated to image \c I, display in overlay the triangles that define the zone.
- \param I : Image.
- \param col : Color used to display the triangles.
- \param thickness : Thickness of the triangle lines.
- */
-void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
-{
- std::vector<vpImagePoint> ip;
- for (unsigned int i=0; i < Zone.size(); i++) {
- vpTemplateTrackerTriangle triangle;
- Zone[i].getCorners(ip);
- vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
- vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
- vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
- }
-}
-
-/*!
- Destructor.
- */
-vpTemplateTrackerZone::~vpTemplateTrackerZone()
-{
- clear();
-}
-
-/*!
- Modify all the pixels inside a triangle with a given gray level.
- \param I: Output image.
- \param id: Triangle id. This value should be less than the number
- of triangles used to define the zone and available using getNbTriangle().
- \param gray_level: Color used to fill the triangle with.
- */
-void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char>& I, unsigned int id,unsigned char gray_level)
-{
- assert(id < getNbTriangle());
- vpTemplateTrackerTriangle triangle;
- getTriangle(id, triangle);
- for (int i=0 ; i < (int) I.getHeight() ; i++)
- {
- for (int j=0 ; j < (int) I.getWidth() ; j++)
- {
- if(triangle.inTriangle(i,j))
- {
- I[i][j]=gray_level;
- }
- }
- }
-}
-
-/*!
- Return a zone with triangles that are down scaled by a factor 2.
- */
-vpTemplateTrackerZone vpTemplateTrackerZone::getPyramidDown() const
-{
- vpTemplateTrackerZone tempZone;
- vpTemplateTrackerTriangle Ttemp;
- vpTemplateTrackerTriangle TtempDown;
- for(unsigned int i=0;i<getNbTriangle();i++)
- {
- getTriangle(i,Ttemp);
- TtempDown=Ttemp.getPyramidDown();
- tempZone.add(TtempDown);
- }
- return tempZone;
-}
-
-/*!
- Copy all the triangles that define zone \c Z in the current zone (*this) and
- update the zone bounding box.
- \param z : Zone with a set of triangles provided as input.
- */
-void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone& z)
-{
- vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<z.getNbTriangle();i++)
- {
- z.getTriangle(i, triangle);
- add(triangle);
- // Update the bounding box
- if((triangle.getMinx()<min_x)||(min_x==-1))
- min_x=(int)triangle.getMinx();
- if((triangle.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)triangle.getMaxx();
- if((triangle.getMiny()<min_y)||(min_y==-1))
- min_y=(int)triangle.getMiny();
- if((triangle.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)triangle.getMaxy();
- }
-}
-
-/*!
- Return the position of the center of gravity in a given area.
- \param borne_x : Right coordinate of the area to consider.
- \param borne_y : Bottom coordinate of the area to consider.
- \exception vpException::divideByZeroError: The size of the zone is null.
- */
-
-vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
-{
- int cpt_pt=0;
- double x_center=0,y_center=0;
- for(int j=0;j<borne_x;j++)
- for(int i=0;i<borne_y;i++)
- if(inZone(i,j))
- {
- x_center+=j;
- y_center+=i;
- cpt_pt++;
- }
-
- if(! cpt_pt) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot compute the zone center: size = 0")) ;
- }
-
- x_center=x_center/cpt_pt;
- y_center=y_center/cpt_pt;
- vpImagePoint center;
- center.set_uv(x_center, y_center);
- return center;
-}
-
-/*!
- Return the area of the template zone.
- */
-double vpTemplateTrackerZone::getArea() const
-{
- double area = 0;
- vpTemplateTrackerTriangle triangle;
- for (unsigned int i=0; i < getNbTriangle(); i++) {
- getTriangle(i, triangle);
- area += triangle.getArea();
- }
- return area;
-}
-
diff --git a/src/tracking/template-tracker/tools/vpTemplateTrackerZone.h b/src/tracking/template-tracker/tools/vpTemplateTrackerZone.h
deleted file mode 100644
index 324eb93..0000000
--- a/src/tracking/template-tracker/tools/vpTemplateTrackerZone.h
+++ /dev/null
@@ -1,119 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZone.h 4781 2014-07-15 13:03:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#ifndef vpTemplateTrackerZone_hh
-#define vpTemplateTrackerZone_hh
-
-#include <vector>
-
-#include <visp/vpDisplay.h>
-#include <visp/vpException.h>
-#include <visp/vpImage.h>
-#include <visp/vpTemplateTrackerTriangle.h>
-#include <visp/vpTemplateTrackerHeader.h>
-#include <visp/vpRect.h>
-
-/*!
- A zone is defined by a set of triangles defined as vpTemplateTrackerTriangle.
-
- A zone can be initialized either by user interaction using mouse click in a display device
- throw initClick(), or by a list of points throw initFromPoints().
- */
-class VISP_EXPORT vpTemplateTrackerZone
-{
- protected:
- std::vector<vpTemplateTrackerTriangle> Zone; //!< Vector of triangles that defines the zone.
- int min_x; //!< Bounding box parameter
- int min_y; //!< Bounding box parameter
- int max_x; //!< Bounding box parameter
- int max_y; //!< Bounding box parameter
-
- public:
- vpTemplateTrackerZone();
- vpTemplateTrackerZone(const vpTemplateTrackerZone &z);
- ~vpTemplateTrackerZone();
-
- //add a triangle to the zone
- void add(const vpTemplateTrackerTriangle &t);
- void clear();
- void copy(const vpTemplateTrackerZone& z);
-
- //display the area on an image
- void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
- void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
-
- //colorie le tieme triangle
- void fillTriangle(vpImage<unsigned char>& I, unsigned int id, unsigned char gray_level);
-
- double getArea() const;
- vpImagePoint getCenter() const;
- vpImagePoint getCenter(int borne_x, int borne_y) const;
- //get bounds of the area
- int getMaxx() const;
- int getMaxy() const;
- int getMinx() const;
- int getMiny() const;
- vpRect getBoundingBox() const;
-
- /*! Return the number of triangles that define the zone. \sa getTriangle() */
- unsigned int getNbTriangle() const { return (unsigned int)Zone.size(); }
- vpTemplateTrackerZone getPyramidDown() const;
- //renvoie le ieme triangle de la zone
- void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const;
- vpTemplateTrackerTriangle getTriangle(unsigned int i) const;
-
- //create an area by clicking on an image
- void initClick(const vpImage<unsigned char>& I, bool delaunay = false);
- //create an area with a pointer of integer that describes a series of triangles:
- // *pt= t0.S1.x,t0.S1.y,t0.S2.x,t0.S2.y,t0.S3.x,t0.S3.y, t1.S1.x ...
- void initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &ip, bool delaunay = false);
-
- //check if a point is in the area
- bool inZone(const int &i,const int &j) const;
- bool inZone(const double &i,const double &j) const;
- //check if a point is in the area and return the corresponding triangle id_triangle where the point is.
- bool inZone(const int &i,const int &j, unsigned int &id_triangle) const;
- bool inZone(const double &i,const double &j, unsigned int &id_triangle) const;
-
- vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z);
-};
-#endif
-
diff --git a/src/tracking/template-tracker/vpTemplateTracker.cpp b/src/tracking/template-tracker/vpTemplateTracker.cpp
deleted file mode 100644
index 8e85adc..0000000
--- a/src/tracking/template-tracker/vpTemplateTracker.cpp
+++ /dev/null
@@ -1,981 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTracker.cpp 5062 2014-12-15 13:43:33Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpTemplateTracker.h>
-#include <visp/vpTemplateTrackerBSpline.h>
-
-vpTemplateTracker::vpTemplateTracker(vpTemplateTrackerWarp *_warp)
- : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
- ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
- ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false),
- templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
- ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
- pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(),
- HLMdesireInverse(), HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40),
- costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
- taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0),
- lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0),
- useCompositionnal(true), useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(),
- dW(), BI(), dIx(), dIy(), zoneRef_()
-{
- nbParam = Warp->getNbParam() ;
- p.resize(nbParam);
- dp.resize(nbParam);
-
- fgG=new double[(taillef+1)/2] ;
- vpImageFilter::getGaussianKernel(fgG, taillef) ;
-
- fgdG=new double[(taillef+1)/2] ;
- vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
-}
-
-void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill)
-{
- taillef=new_taill;
- if (fgG) delete[] fgG;
- fgG=new double[taillef] ;
- vpImageFilter::getGaussianKernel(fgG, taillef) ;
-
- if (fgdG) delete[] fgdG;
- fgdG=new double[taillef] ;
- vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
-}
-
-void vpTemplateTracker::initTracking(const vpImage<unsigned char>& I, vpTemplateTrackerZone &zone)
-{
- // std::cout<<"\tInitialise reference..."<<std::endl;
- zoneTracked=&zone;
-
- int largeur_im=(int)I.getWidth();
- int hauteur_im=(int)I.getHeight();
-
- unsigned int NbPointDsZone=0;
- //double xtotal=0,ytotal=0;
- int mod_fi,mod_fj;
- mod_fi=mod_i;mod_fj=mod_i;
-
- for(int i=0;i<hauteur_im;i+=mod_fi) {
- for(int j=0;j<largeur_im;j+=mod_fj) {
- if(zone.inZone(i,j)) {
- NbPointDsZone++;
- //xtotal+=j;
- //ytotal+=i;
- }
- }
- }
-
- //Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
-
- templateSize=NbPointDsZone;
- ptTemplate = new vpTemplateTrackerPoint[templateSize];ptTemplateInit=true;
- ptTemplateSelect = new bool[templateSize];ptTemplateSelectInit=true;
-
- Hdesire.resize(nbParam,nbParam);
- HLMdesire.resize(nbParam,nbParam);
-
- vpTemplateTrackerPoint pt;
- //vpTemplateTrackerZPoint ptZ;
- vpImage<double> GaussI ;
- vpImageFilter::filter(I, GaussI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- unsigned int cpt_point=0;
- templateSelectSize=0;
- for(int i=0;i<hauteur_im;i+=mod_i)
- {
- //for(int j=minx_t;j<maxx_t;j++)
- for(int j=0;j<largeur_im;j+=mod_j)
- {
-// if(i%mod_i ==0 && j%mod_j ==0)
- if(zone.inZone(i,j))
- {
- pt.x=j;
- pt.y=i;
-
- pt.dx=dIx[i][j];
- pt.dy=dIy[i][j];
-
- if(pt.dx*pt.dx+pt.dy*pt.dy>thresholdGradient)
- {
- ptTemplateSelect[cpt_point]=true;
- templateSelectSize++;
- }
- else
- ptTemplateSelect[cpt_point]=false;
- //ptTemplate_select[cpt_point]=true;
-
- /*if(blur)
- pt.val=GaussI[i][j];
- else
- pt.val=I[i][j];*/
- pt.val=vpTemplateTrackerBSpline::getSubPixBspline4(GaussI,i,j);
- //ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
-
- ptTemplate[cpt_point]=pt;
- cpt_point++;
- }
- }
- }
-
- // std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
- // std::cout<<"utilisation de "<<taille_template_select<<"/"<<cpt_point<<" = "<<100.*taille_template_select/cpt_point<<"% pour calcul derivees"<<std::endl;
-
- templateSize=cpt_point;
- GaussI.destroy();
- // std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
-}
-
-vpTemplateTracker::~vpTemplateTracker()
-{
- // vpTRACE("destruction tracker");
- delete[] fgG;
- delete[] fgdG;
-
- resetTracker();
-}
-
-/*!
- Reset the tracker by freeing the memory allocated by the template tracker during the initialization.
- */
-void vpTemplateTracker::resetTracker()
-{
- // reset the tracker parameters
- p = 0;
-
- // vpTRACE("resetTracking");
- if(pyrInitialised)
- {
- if(ptTemplatePyr){
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
- if(ptTemplatePyr[i]){
- for(unsigned int point=0;point<templateSizePyr[i];point++)
- {
- delete[] ptTemplatePyr[i][point].dW;
- delete[] ptTemplatePyr[i][point].HiG;
- }
- delete[] ptTemplatePyr[i];
- }
- }
- delete[] ptTemplatePyr;
- ptTemplatePyr = NULL;
- }
-
- if (ptTemplateCompoPyr) {
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
- if (ptTemplateCompoPyr[i]) {
- for(unsigned int point=0;point<templateSizePyr[i];point++) {
- delete[] ptTemplateCompoPyr[i][point].dW;
- }
- delete[] ptTemplateCompoPyr[i];
- }
- }
- delete[] ptTemplateCompoPyr;
- ptTemplateCompoPyr = NULL;
- }
-
- if (ptTemplateSuppPyr) {
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
- if (ptTemplateSuppPyr[i]) {
- for(unsigned int point=0;point<templateSizePyr[i];point++) {
- delete[] ptTemplateSuppPyr[i][point].Bt;
- delete[] ptTemplateSuppPyr[i][point].BtInit;
- delete[] ptTemplateSuppPyr[i][point].dBt;
- delete[] ptTemplateSuppPyr[i][point].d2W;
- delete[] ptTemplateSuppPyr[i][point].d2Wx;
- delete[] ptTemplateSuppPyr[i][point].d2Wy;
- }
- delete[] ptTemplateSuppPyr[i];
- }
- }
- delete[] ptTemplateSuppPyr;
- ptTemplateSuppPyr = NULL;
- }
-
- if(ptTemplateSelectPyr){
- for(unsigned int i=0;i<nbLvlPyr;i++){
- if(ptTemplateSelectPyr[i])
- delete[] ptTemplateSelectPyr[i];
- }
- delete[] ptTemplateSelectPyr;
- ptTemplateSelectPyr = NULL;
- }
-
- if(templateSizePyr){
- delete[] templateSizePyr;
- templateSizePyr = NULL;
- }
-
- if(HdesirePyr) {
- delete[] HdesirePyr;
- HdesirePyr = NULL;
- }
-
- if(HLMdesirePyr){
- delete[] HLMdesirePyr;
- HLMdesirePyr = NULL;
- }
-
- if(HLMdesireInversePyr){
- delete[] HLMdesireInversePyr;
- HLMdesireInversePyr = NULL;
- }
-
- if(zoneTrackedPyr){
- delete[] zoneTrackedPyr;
- zoneTrackedPyr = NULL;
- }
-
- if(pyr_IDes){
- delete[] pyr_IDes;
- pyr_IDes = NULL;
- }
- }
- else
- {
- if(ptTemplateInit)
- {
- for(unsigned int point=0;point<templateSize;point++)
- {
- delete[] ptTemplate[point].dW;
- delete[] ptTemplate[point].HiG;
- }
- delete[] ptTemplate;
- ptTemplate = NULL;
- ptTemplateInit = false;
- }
- if (ptTemplateCompo) {
- for(unsigned int point=0;point<templateSize;point++)
- {
- delete[] ptTemplateCompo[point].dW;
- }
- delete[] ptTemplateCompo;
- ptTemplateCompo = NULL;
- }
- if (ptTemplateSupp) {
- for(unsigned int point=0;point<templateSize;point++)
- {
- delete[] ptTemplateSupp[point].Bt;
- delete[] ptTemplateSupp[point].BtInit;
- delete[] ptTemplateSupp[point].dBt;
- delete[] ptTemplateSupp[point].d2W;
- delete[] ptTemplateSupp[point].d2Wx;
- delete[] ptTemplateSupp[point].d2Wy;
- }
- delete[] ptTemplateSupp;
- ptTemplateSupp = NULL;
- }
- if(ptTemplateSelectInit) {
- if (ptTemplateSelect) {
- delete[] ptTemplateSelect; ptTemplateSelect = NULL;
- }
- }
- }
-}
-
-/*!
- Display the warped reference template in an image.
-
- \param I: Image in which the warped zone has to be displayed.
- \param col: Color used to draw the triangle edges.
- \param thickness: Thickness of the lines.
-
- The following code shows how to use display capabilities:
- \code
-#include <visp/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp/vpTemplateTrackerWarpHomography.h>
-
-int main()
-{
- vpImage<unsigned char> I;
- vpTemplateTrackerWarpHomography warp;
- vpTemplateTrackerSSDInverseCompositional tracker(&warp);
- vpTemplateTrackerZone zoneRef, zoneWarped;
-
- // Display the warped zone
- tracker.display(I, vpColor::red);
-
- // Display the reference zone
- zoneRef = tracker.getZoneRef();
- zoneRef.display(I, vpColor::green);
-
- // Display the warped zone
- vpColVector p = tracker.getp();
- warp.warpZone(zoneRef, p, zoneWarped);
- zoneWarped.display(I, vpColor::blue);
-}
- \endcode
- */
-void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor& col, const unsigned int thickness)
-{
- if (I.display) { // Only if a display is associated to the image
- vpTemplateTrackerZone zoneWarped;
- Warp->warpZone(*zoneTracked, p, zoneWarped);
- zoneWarped.display(I, col, thickness);
- }
-}
-
-/*!
- Display the warped reference template in an image.
-
- \param I: Image in which the warped zone has to be displayed.
- \param col: Color used to draw the triangle edges.
- \param thickness: Thickness of the lines.
-
- The following code shows how to use display capabilities:
- \code
-#include <visp/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp/vpTemplateTrackerWarpHomography.h>
-
-int main()
-{
- vpImage<vpRGBa> I;
- vpTemplateTrackerWarpHomography warp;
- vpTemplateTrackerSSDInverseCompositional tracker(&warp);
- vpTemplateTrackerZone zoneRef, zoneWarped;
-
- // Display the warped zone
- tracker.display(I, vpColor::red);
-
- // Display the reference zone
- zoneRef = tracker.getZoneRef();
- zoneRef.display(I, vpColor::green);
-
- // Display the warped zone
- vpColVector p = tracker.getp();
- warp.warpZone(zoneRef, p, zoneWarped);
- zoneWarped.display(I, vpColor::blue);
-}
- \endcode
- */
-void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor& col, const unsigned int thickness)
-{
- if (I.display) { // Only if a display is associated to the image
- vpTemplateTrackerZone zoneWarped;
- Warp->warpZone(*zoneTracked, p, zoneWarped);
- zoneWarped.display(I, col, thickness);
- }
-}
-
-void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha)
-{
- vpColVector **ptp;
- ptp=new vpColVector*[4];
- vpColVector p0(nbParam);
- p0=tp;
-
- //valeur necessaire si conditionnel
- vpColVector dpt(Warp->getNbParam());
- vpColVector adpt(Warp->getNbParam());
-
- vpColVector p1(nbParam);
- if(useCompositionnal)
- {
- if(useInverse)
- Warp->getParamInverse(direction,dpt);
- else
- dpt=direction;
- Warp->pRondp(tp,dpt,p1);
- }
- else
- {
- p1=tp+direction;
- }
-
- vpColVector p2(nbParam);
- if(useCompositionnal)
- {
- adpt=alpha*direction;
- if(useInverse)
- Warp->getParamInverse(adpt,dpt);
- else
- dpt=adpt;
- Warp->pRondp(tp,dpt,p2);
- }
- else
- {
- p2=tp+alpha*direction;
- }
- vpColVector p3(nbParam);
- ptp[0]=&p0;
- ptp[1]=&p1;
- ptp[2]=&p2;
- ptp[3]=&p3;
-
-
- double *Cost=new double[4];
- //Cost[0]=getCost(I,p0);
- Cost[0]=tMI;
- Cost[1]=getCost(I,p1);
- Cost[2]=getCost(I,p2);
-
-
- double *talpha=new double[4];
- talpha[0]=0;
- talpha[1]=1.;
- talpha[2]=alpha;
-
- //for(int i=0;i<3;i++)
- // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] = "<<Cost[i]<<std::endl;
-
- //Utilise trois estimï¿œes de paraboles succesive ...
- //A changer pour rendre adaptable
- for(unsigned int opt=0;opt<nbIterBrent;opt++)
- {
- //double a=talpha[0];
- //double b=talpha[1];
- //double c=talpha[2];
- //double Cost0=Cost[0];
- //double Cost1=Cost[1];
- //double Cost2=Cost[2];
-
- vpMatrix A(3,3);
- for(unsigned int i=0;i<3;i++){A[i][0]=talpha[i]*talpha[i];A[i][1]=talpha[i];A[i][2]=1.;}
- //std::cout<<"A="<<A<<std::endl;
- vpColVector B(3);for(unsigned int i=0;i<3;i++)B[i]=Cost[i];
- vpColVector parabol(3);parabol=(A.t()*A).inverseByLU()*A.t()*B;
- //vpColVector parabol(3);parabol=A.pseudoInverse()*B;
-
- //si convexe
- if(parabol[0]>0)
- {
- talpha[3]=-0.5*parabol[1]/parabol[0];
- //std::cout<<"parabol = "<<parabol<<std::endl;
- //std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
- }
- else//si concave
- {
- int tindic_x_min=0;
- int tindic_x_max=0;
- for(int i=1;i<3;i++)
- {
- if(talpha[i]<talpha[tindic_x_min])
- tindic_x_min=i;
- if(talpha[i]>talpha[tindic_x_max])
- tindic_x_max=i;
- }
-
- if(Cost[tindic_x_max]<Cost[tindic_x_min])
- {
- //talpha[3]=talpha[tindic_x_max]+1.;
- talpha[3]=talpha[tindic_x_max]+1.;
- /*if(talpha[tindic_x_min]>talpha[tindic_x_max])
- talpha[3]=talpha[tindic_x_min]+1.;
- else
- talpha[3]=talpha[tindic_x_min]-1.;*/
- }
- else
- {
- //talpha[3]=talpha[tindic_x_min]-1.;
- talpha[3]=talpha[tindic_x_min]-1.;
- /*if(talpha[tindic_x_min]<talpha[tindic_x_max])
- talpha[3]=talpha[tindic_x_max]+1.;
- else
- talpha[3]=talpha[tindic_x_max]-1.;*/
- }
- //std::cout<<"concave talpha="<<talpha[3]<<std::endl;
- }
- //std::cout<<"talpha="<<talpha[3]<<std::endl;
- int indic_x_min=0;
- int indic_x_max=0;
- for(int i=1;i<3;i++)
- {
- if(talpha[i]<talpha[indic_x_min])
- indic_x_min=i;
- if(talpha[i]>talpha[indic_x_max])
- indic_x_max=i;
- }
- //std::cout<<"talpha = "<<talpha[3]<<std::endl;
- if(talpha[3]>talpha[indic_x_max])
- if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+4.;
- if(talpha[3]<talpha[indic_x_min])
- if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-4.;
-
- /*if(((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0))==0)
- {Cost[3]=1000;break;}
-
- //calcul du gain correspondant au minimum de la parabole estimï¿œe
- talpha[3]=b-0.5*((b-a)*(b-a)*(Cost1-Cost2)-(b-c)*(b-c)*(Cost1-Cost0))/((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0));
- int indic_x_min=0;
- int indic_x_max=0;
- for(int i=1;i<3;i++)
- {
- if(talpha[i]<talpha[indic_x_min])
- indic_x_min=i;
- if(talpha[i]>talpha[indic_x_max])
- indic_x_max=i;
- }
- std::cout<<"talpha = "<<talpha[3]<<std::endl;
- if(talpha[3]>talpha[indic_x_max])
- if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+alpha;
- if(talpha[3]<talpha[indic_x_min])
- if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-alpha;*/
-
- //p3=tp-talpha[3]*direction;
- if(useCompositionnal)
- {
- adpt=talpha[3]*direction;
- if(useInverse)
- Warp->getParamInverse(adpt,dpt);
- else
- dpt=adpt;
- Warp->pRondp(tp,dpt,p3);
- }
- else
- {
- p3=tp+talpha[3]*direction;
- }
-
- Cost[3]=getCost(I,p3);
- //std::cout<<"new cost="<<Cost[3]<<std::endl;
-
- int indice_f_max=0;
- for(int i=1;i<4;i++)
- if(Cost[i]>Cost[indice_f_max])
- indice_f_max=i;
- if(indice_f_max!=3)
- {
- *ptp[indice_f_max]=*ptp[3];
- Cost[indice_f_max]=Cost[3];
- talpha[indice_f_max]=talpha[3];
- }
- else
- break;
- }
-
- int indice_f_min=0;
- for(int i=0;i<4;i++)
- if(Cost[i]<Cost[indice_f_min])
- indice_f_min=i;
-
- alpha=talpha[indice_f_min];
-
- if(alpha<1)alpha=1.;
-
- delete[] ptp;
- delete[] Cost;
- delete[] talpha;
-}
-
-
-/*!
- \param nbLvl : Number of levels in the pyramid.
- \param l0 : Pyramid level where the tracking is stopped. The level with the highest resolution is 0.
- */
-void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0)
-{
- // vpTRACE("init_pyramidal");
- nbLvlPyr=nbLvl;
- l0Pyr=l0;
-
- zoneTrackedPyr=new vpTemplateTrackerZone[nbLvlPyr];
- pyr_IDes=new vpImage<unsigned char>[nbLvlPyr];
- ptTemplatePyr=new vpTemplateTrackerPoint*[nbLvlPyr];
- ptTemplateSelectPyr=new bool*[nbLvlPyr];
- ptTemplateSuppPyr=new vpTemplateTrackerPointSuppMIInv*[nbLvlPyr];
- ptTemplateCompoPyr=new vpTemplateTrackerPointCompo*[nbLvlPyr];
- for(unsigned int i=0; i< nbLvlPyr; i++) {
- ptTemplatePyr[i] = NULL;
- ptTemplateSuppPyr[i] = NULL;
- ptTemplateSelectPyr[i] = NULL;
- ptTemplateCompoPyr[i] = NULL;
- }
- templateSizePyr=new unsigned int[nbLvlPyr];
- HdesirePyr=new vpMatrix[nbLvlPyr];
- HLMdesirePyr=new vpMatrix[nbLvlPyr];
- HLMdesireInversePyr=new vpMatrix[nbLvlPyr];
-
- pyrInitialised=true;
- // vpTRACE("fin init_pyramidal");
-
-}
-void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone)
-{
- // vpTRACE("initTrackingPyr");
- zoneTrackedPyr[0].copy(zone);
- //vpTRACE("fin copy zone");
-
- pyr_IDes[0]=I;
- initTracking(pyr_IDes[0],zoneTrackedPyr[0]);
- ptTemplatePyr[0]=ptTemplate;
- ptTemplateSelectPyr[0]=ptTemplateSelect;
- templateSizePyr[0]=templateSize;
-
- //creationpyramide de zones et images desiree
- if(nbLvlPyr>1)
- {
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- zoneTrackedPyr[i]=zoneTrackedPyr[i-1].getPyramidDown();
- vpImageFilter::getGaussPyramidal(pyr_IDes[i-1],pyr_IDes[i]);
-
- initTracking(pyr_IDes[i],zoneTrackedPyr[i]);
- ptTemplatePyr[i]=ptTemplate;
- ptTemplateSelectPyr[i]=ptTemplateSelect;
- templateSizePyr[i]=templateSize;
- //reste probleme avec le Hessien
- }
- }
- /*for(int i=0;i<nbLvlPyr;i++)
- {
- vpColVector ptemp(8);ptemp=0;
- zoneTracked=&zoneTrackedPyr[i];
- init_pos_evalRMS(ptemp);
- }*/
- zoneTracked=&zoneTrackedPyr[0];
-
- // vpTRACE("fin initTrackingPyr");
-
-}
-
-/*!
- Select the reference template in image \e I using mouse click.
-
- \param I: Image containing the reference template.
- \param delaunay: Flag used to enable Delaunay triangulation.
- - If true, from the image points selected by the user, a Delaunay triangulation is performed
- to initialize the reference template.
- - A left click select a image point;
- - A right click select the last image point and ends the initialisation stage.
- - If false, the user select directly points as successive triangle corners.
- The size of \e v_ip vector should be a multiple of 3. It is not mandatory
- that triangles have one edge in common; they can define a discontinued area.
- - A left click select a triangle corner;
- - A right click select the last triangle corner and ends the initialisation stage.
- For example, to select the reference template as two triangles, the user has to left click
- five times and finish the selection on the sixth corner with a right click.
-
- */
-void vpTemplateTracker::initClick(const vpImage<unsigned char> &I, bool delaunay)
-{
- zoneRef_.initClick(I, delaunay);
-
- if (nbLvlPyr > 1) {
- initPyramidal(nbLvlPyr, l0Pyr);
- initTrackingPyr(I, zoneRef_);
- initHessienDesiredPyr(I);
- }
- else {
- initTracking(I, zoneRef_);
- initHessienDesired(I);
-// trackNoPyr(I);
- }
-}
-
-/*!
- Initialize the reference template from a vector of points.
-
- \param I: Image containing the reference template.
- \param v_ip: Vector of image points defining the reference template.
- \param delaunay:
- - If true, from the image points defining the reference template enable Delaunay triangulation.
- - If false, the vector of image points define the reference template as a list of triangles.
- The size of \e v_ip vector should be a multiple of 3.
- */
-void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay)
-{
- zoneRef_.initFromPoints(I, v_ip, delaunay);
-
- if (nbLvlPyr > 1) {
- initPyramidal(nbLvlPyr, l0Pyr);
- initTrackingPyr(I, zoneRef_);
- initHessienDesiredPyr(I);
- }
- else {
- initTracking(I, zoneRef_);
- initHessienDesired(I);
- //trackNoPyr(I);
- }
-}
-
-/*!
- Initialize the reference template from a vector of points.
-
- \param I: Image containing the reference template.
- \param zone: The zone that describes the reference template.
- */
-void vpTemplateTracker::initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone)
-{
- zoneRef_ = zone;
-
- if (nbLvlPyr > 1) {
- initPyramidal(nbLvlPyr, l0Pyr);
- initTrackingPyr(I, zoneRef_);
- initHessienDesiredPyr(I);
- }
- else {
- initTracking(I, zoneRef_);
- initHessienDesired(I);
-// trackNoPyr(I);
- }
-}
-
-void vpTemplateTracker::initCompInversePyr(const vpImage<unsigned char> &I)
-{
- vpTRACE("initCompInversePyr");
- templateSize=templateSizePyr[0];
- //ptTemplateSupp=ptTemplateSuppPyr[0];
- //ptTemplateCompo=ptTemplateCompoPyr[0];
- ptTemplate=ptTemplatePyr[0];
- ptTemplateSelect=ptTemplateSelectPyr[0];
- initCompInversePyr(I);
- ptTemplateSuppPyr[0]=ptTemplateSupp;
- ptTemplateCompoPyr[0]=ptTemplateCompo;
-
-
- if(nbLvlPyr>1)
- {
- vpImage<unsigned char> Itemp=I;
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- vpImageFilter::getGaussPyramidal(Itemp,Itemp);
-
- templateSize=templateSizePyr[i];
- ptTemplate=ptTemplatePyr[i];
- ptTemplateSelect=ptTemplateSelectPyr[i];
- initCompInversePyr(Itemp);
- ptTemplateSuppPyr[i]=ptTemplateSupp;
- ptTemplateCompoPyr[i]=ptTemplateCompo;
- }
- }
- // vpTRACE("fin initCompInversePyr");
-
-}
-void vpTemplateTracker::initHessienDesiredPyr(const vpImage<unsigned char> &I)
-{
- // vpTRACE("initHessienDesiredPyr");
-
- templateSize=templateSizePyr[0];
- //ptTemplateSupp=ptTemplateSuppPyr[0];
- //ptTemplateCompo=ptTemplateCompoPyr[0];
- ptTemplate=ptTemplatePyr[0];
- ptTemplateSelect=ptTemplateSelectPyr[0];
-// ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
- try{
- initHessienDesired(I);
- ptTemplateSuppPyr[0]=ptTemplateSupp;
- ptTemplateCompoPyr[0]=ptTemplateCompo;
- HdesirePyr[0]=Hdesire;
- HLMdesirePyr[0]=HLMdesire;
- HLMdesireInversePyr[0]=HLMdesireInverse;
- }
- catch(vpException &e){
- ptTemplateSuppPyr[0]=ptTemplateSupp;
- ptTemplateCompoPyr[0]=ptTemplateCompo;
- HdesirePyr[0]=Hdesire;
- HLMdesirePyr[0]=HLMdesire;
- HLMdesireInversePyr[0]=HLMdesireInverse;
- throw(e);
- }
-
- if(nbLvlPyr>1)
- {
- vpImage<unsigned char> Itemp;Itemp=I;
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- vpImageFilter::getGaussPyramidal(Itemp,Itemp);
-
- templateSize=templateSizePyr[i];
- ptTemplate=ptTemplatePyr[i];
- ptTemplateSelect=ptTemplateSelectPyr[i];
- //ptTemplateSupp=ptTemplateSuppPyr[i];
- //ptTemplateCompo=ptTemplateCompoPyr[i];
- try{
- initHessienDesired(Itemp);
- ptTemplateSuppPyr[i]=ptTemplateSupp;
- ptTemplateCompoPyr[i]=ptTemplateCompo;
- HdesirePyr[i]=Hdesire;
- HLMdesirePyr[i]=HLMdesire;
- HLMdesireInversePyr[i]=HLMdesireInverse;
- }
- catch(vpException &e){
- ptTemplateSuppPyr[i]=ptTemplateSupp;
- ptTemplateCompoPyr[i]=ptTemplateCompo;
- HdesirePyr[i]=Hdesire;
- HLMdesirePyr[i]=HLMdesire;
- HLMdesireInversePyr[i]=HLMdesireInverse;
- throw(e);
- }
- }
- }
- // vpTRACE("fin initHessienDesiredPyr");
-}
-
-/*!
- Track the template on image \e I.
- \param I: Image to process.
- */
-void vpTemplateTracker::track(const vpImage<unsigned char> &I)
-{
- if (nbLvlPyr > 1)
- trackPyr(I);
- else
- trackNoPyr(I);
-}
-
-void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
-{
- //vpTRACE("trackPyr");
- vpImage<unsigned char> *pyr_I;
-// pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
- pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
- pyr_I[0]=I;
-
- try
- {
- vpColVector ptemp(nbParam);
- if(nbLvlPyr>1)
- {
- // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
- // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
-
- // p_sauv[0]=p;
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
- //test getParamPyramidDown
- /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
- vpColVector vX_test2(2);
- Warp->computeCoeff(p);
- Warp->computeDenom(vX_test,p);
- Warp->warpX(vX_test,vX_test2,p);
- std::cout<<"p = "<<p.t()<<std::endl;*/
- //std::cout<<"get p down"<<std::endl;
- Warp->getParamPyramidDown(p,ptemp);
- p=ptemp;
- zoneTracked=&zoneTrackedPyr[i];
-
- // p_sauv[i]=p;
- /*std::cout<<"p_down = "<<p.t()<<std::endl;
-
- vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
- vpColVector vX_testd2(2);
- Warp->computeCoeff(p);
- Warp->computeDenom(vX_testd,p);
- Warp->warpX(vX_testd,vX_testd2,p);
- std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
-
- }
-
- for(int i=(int)nbLvlPyr-1;i>=0;i--)
- {
- if(i>=(int)l0Pyr)
- {
- templateSize=templateSizePyr[i];
- ptTemplate=ptTemplatePyr[i];
- ptTemplateSelect=ptTemplateSelectPyr[i];
- ptTemplateSupp=ptTemplateSuppPyr[i];
- ptTemplateCompo=ptTemplateCompoPyr[i];
- H=HdesirePyr[i];
- HLM=HLMdesirePyr[i];
- HLMdesireInverse=HLMdesireInversePyr[i];
- // zoneTracked=&zoneTrackedPyr[i];
- trackRobust(pyr_I[i]);
- }
- //std::cout<<"get p up"<<std::endl;
- // ptemp=p_sauv[i-1];
- if (i > 0) {
- Warp->getParamPyramidUp(p,ptemp);
- p=ptemp;
- zoneTracked=&zoneTrackedPyr[i-1];
- }
- }
- #if 0
- if(l0Pyr==0)
- {
- templateSize=templateSizePyr[0];
- ptTemplate=ptTemplatePyr[0];
- ptTemplateSelect=ptTemplateSelectPyr[0];
- ptTemplateSupp=ptTemplateSuppPyr[0];
- ptTemplateCompo=ptTemplateCompoPyr[0];
- H=HdesirePyr[0];
- HLM=HLMdesirePyr[0];
- HLMdesireInverse=HLMdesireInversePyr[0];
- zoneTracked=&zoneTrackedPyr[0];
- trackRobust(pyr_I[0]);
- }
-
- if (l0Pyr > 0) {
- // for (int l=(int)l0Pyr; l >=0; l--) {
- // Warp->getParamPyramidUp(p,ptemp);
- // p=ptemp;
- // }
- zoneTracked=&zoneTrackedPyr[0];
- }
- #endif
- // delete [] p_sauv;
- }
- else
- {
- //std::cout<<"reviens a tracker de base"<<std::endl;
- trackRobust(I);
- }
- delete[] pyr_I;
- }
- catch(vpException &e){
- delete[] pyr_I;
- throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
- }
-}
-
-void vpTemplateTracker::trackRobust(const vpImage<unsigned char> &I)
-{
- if(costFunctionVerification)
- {
- vpColVector p_pre_estimation;p_pre_estimation=p;
- getGaussianBluredImage(I);
- double pre_fcost=getCost(I,p);
-
- trackNoPyr(I);
-
- //std::cout<<"fct avant : "<<pre_fcost<<std::endl;
- double post_fcost=getCost(I,p);
- //std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
- if(pre_fcost<post_fcost)
- p=p_pre_estimation;
- }
- else
- trackNoPyr(I);
-}
diff --git a/src/tracking/template-tracker/vpTemplateTracker.h b/src/tracking/template-tracker/vpTemplateTracker.h
deleted file mode 100644
index cac7730..0000000
--- a/src/tracking/template-tracker/vpTemplateTracker.h
+++ /dev/null
@@ -1,234 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTracker.h 5113 2015-01-05 08:00:50Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTracker.h
- \brief
-*/
-
-#ifndef vpTemplateTracker_hh
-#define vpTemplateTracker_hh
-
-#include <math.h>
-
-#include <visp/vpTemplateTrackerHeader.h>
-#include <visp/vpTemplateTrackerZone.h>
-#include <visp/vpTemplateTrackerWarp.h>
-#include <visp/vpImageFilter.h>
-
-class VISP_EXPORT vpTemplateTracker
-{
- protected:
- //traitement pyramidal
- unsigned int nbLvlPyr; // If = 1, disable pyramidal usage
- unsigned int l0Pyr;
- bool pyrInitialised;
-
- vpTemplateTrackerPoint *ptTemplate;
- vpTemplateTrackerPoint **ptTemplatePyr;
- bool ptTemplateInit;
- unsigned int templateSize;
- unsigned int *templateSizePyr;
- bool *ptTemplateSelect;
- bool **ptTemplateSelectPyr;
- bool ptTemplateSelectInit;
- unsigned int templateSelectSize;
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; //pour inverse et compo
- vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; //pour inverse et compo
- #endif
-
- vpTemplateTrackerPointCompo *ptTemplateCompo; //pour ESM
- vpTemplateTrackerPointCompo **ptTemplateCompoPyr; //pour ESM
- vpTemplateTrackerZone *zoneTracked;
- vpTemplateTrackerZone *zoneTrackedPyr;
-
- vpImage<unsigned char> *pyr_IDes;
-
- vpMatrix H;
- vpMatrix Hdesire;
- vpMatrix *HdesirePyr;
- vpMatrix HLM;
- vpMatrix HLMdesire;
- vpMatrix *HLMdesirePyr;
- vpMatrix HLMdesireInverse;
- vpMatrix *HLMdesireInversePyr;
- vpColVector G;
-
- double gain;
- double thresholdGradient;
- bool costFunctionVerification;
- bool blur;
- bool useBrent;
- unsigned int nbIterBrent;
- unsigned int taillef;
- double *fgG;
- double *fgdG;
- double ratioPixelIn;
- int mod_i;
- int mod_j;//variable de sampling de zone de reference
- unsigned int nbParam ;
- double lambdaDep ;
- unsigned int iterationMax ;
- //pour BFGS
- unsigned int iterationGlobale;
- //diverge is set to true if there is no more point in the tracked area
- bool diverge;
- unsigned int nbIteration;
- bool useCompositionnal;
- bool useInverse;
-
- vpTemplateTrackerWarp *Warp;
- //Parametre de deplacement
- vpColVector p;
- vpColVector dp;
-
- //temporary values for warping
- vpColVector X1;
- vpColVector X2;
- //temporary derivative matrix
- vpMatrix dW;
-
- vpImage<double> BI;
- vpImage<double> dIx ;
- vpImage<double> dIy ;
- vpTemplateTrackerZone zoneRef_; // Reference zone
-
- public:
- vpTemplateTracker(vpTemplateTrackerWarp *_warp);
- virtual ~vpTemplateTracker();
-
- void display(const vpImage<unsigned char> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
- void display(const vpImage<vpRGBa> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
-
- bool getDiverge() const {return diverge;}
- vpColVector getdp(){ return dp; }
- vpColVector getG() const { return G; }
- vpMatrix getH() const { return H; }
- unsigned int getNbParam() const { return nbParam ; }
- unsigned int getNbIteration() const { return nbIteration; }
- vpColVector getp() const { return p;}
- double getRatioPixelIn() const {return ratioPixelIn;}
-
- /*!
-
- \return The pointer to the warper.
- */
- vpTemplateTrackerWarp *getWarp() const {return Warp;}
-
- /*!
- Return the reference template zone.
- */
- vpTemplateTrackerZone getZoneRef() const { return zoneRef_; }
-
- void initClick(const vpImage<unsigned char> &I, bool delaunay=false);
- void initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false);
- void initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone);
-
- void resetTracker();
-
- void setBlur(bool b){blur = b;}
- void setCostFunctionVerification(bool b){costFunctionVerification = b;}
- void setGain(double g){gain=g;}
- void setGaussianFilterSize(unsigned int new_taill);
- void setHDes(vpMatrix &tH){ Hdesire=tH; vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire); HLMdesireInverse = HLMdesire.inverseByLU();}
- /*!
- Set the maximum number of iteration of the estimation scheme.
- \param n : Maximum number of iterations to stop the estimation scheme. A typical value is arround 100.
- */
- void setIterationMax(const unsigned int &n) { iterationMax = n ; }
- /*!
- Set the convergence gain used in the estimation scheme.
- \param l : Gain. A typical value is 0.001.
- */
- void setLambda(double l) { lambdaDep = l ; }
- void setNbIterBrent(const unsigned int &b){nbIterBrent=b;}
- void setp(const vpColVector &tp){ p=tp; diverge=false; iterationGlobale=0; }
- /*!
- Set the number of pyramid levels used in the multi-resolution scheme.
- If \e nlevels > 1, the tracker uses a pyramidal approach.
-
- \param nlevels : Number of pyramid levels. Algorithm starts at level nlevels-1.
- \param level_to_stop : Last level of the pyramid that will be considered. Lowest level is zero.
- */
- void setPyramidal(unsigned int nlevels=2, unsigned int level_to_stop=1) {
- nbLvlPyr = nlevels;
- l0Pyr = level_to_stop;
- if(l0Pyr >= nlevels){
- std::cout << "Warning: level_to_stop: " << level_to_stop << " higher than level_to_start: " << nlevels-1 << " (nlevels-1)" <<std::endl;
- std::cout << "Level to stop put to: " << nlevels-1 << std::endl;
- l0Pyr = nlevels-1;
- }
- }
- /*!
- Set the pixel sampling parameters along the rows and the columns.
- \param sample_i : Sampling factor along the rows.
- If 1 all the lines are considered. If 2, consider one line over two.
-
- \param sample_j : Sampling factor along the columns.
- If 1 all the columns are considered. If 2, consider one column over two.
- */
- void setSampling(int sample_i,int sample_j){mod_i=sample_i; mod_j=sample_j;}
- void setThresholdGradient(double threshold){thresholdGradient=threshold;}
- /*! By default Brent usage is disabled. */
- void setUseBrent(bool b){useBrent = b;}
-
- void track(const vpImage<unsigned char> &I);
- void trackRobust(const vpImage<unsigned char> &I);
-
- protected:
-
- void computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha);
- virtual double getCost(const vpImage<unsigned char> &I, vpColVector &tp) = 0;
- void getGaussianBluredImage(const vpImage<unsigned char> &I){ vpImageFilter::filter(I, BI,fgG,taillef); }
- void initCompInverse(const vpImage<unsigned char> &I);
- virtual void initCompInversePyr(const vpImage<unsigned char> &I);
- virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
- virtual void initHessienDesiredPyr(const vpImage<unsigned char> &I);
- virtual void initPyramidal(unsigned int nbLvl,unsigned int l0);
- void initTracking(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
- virtual void initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
- virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
- virtual void trackPyr(const vpImage<unsigned char> &I);
-};
-#endif
-
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.cpp
deleted file mode 100644
index 23333ef..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.cpp
+++ /dev/null
@@ -1,167 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarp.cpp 4782 2014-07-15 13:04:19Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarp.h>
-
-void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out)
-{
- if (p.size() < 2) {
- vpCTRACE << "Bad template tracker warp parameters dimension. Should never occur. " << std::endl;
- throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
- }
- vpColVector S1(2),S2(2),S3(2);
- vpColVector rS1(2),rS2(2),rS3(2);
- in.getCorners(S1,S2,S3);
- computeDenom(S1,p);
- warpX(S1,rS1,p);
- computeDenom(S2,p);
- warpX(S2,rS2,p);
- computeDenom(S3,p);
- warpX(S3,rS3,p);
- out.init(rS1,rS2,rS3);
-}
-void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out)
-{
- vpTemplateTrackerTriangle TR,TT;
- out.clear();
- for(unsigned int i=0;i<in.getNbTriangle();i++)
- {
- in.getTriangle(i,TR);
- warpTriangle(TR,p,TT);
- out.add(TT);
- }
-}
-
-double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
-{
- unsigned int nb_corners = Z.getNbTriangle() * 3;
- computeCoeff(p);
- vpColVector X1(2),X2(2);
-
- double res=0;
- vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<Z.getNbTriangle();i++)
- {
- Z.getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
- triangle.getCorner(j, X1[0], X1[1]);
-
- computeDenom(X1,p);
- warpX(X1,X2,p);
- res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
- }
- }
-
- return res/nb_corners;
-}
-
-void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
-{
- computeCoeff(p);
- vpColVector X1(2),X2(2);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,X2,p);
- u[i]=X2[0];
- v[i]=X2[1];
- //std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
- }
-}
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
-{
- vpMatrix dW_(2,nbParam);
- vpMatrix dX(2,1);
- vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
- vpMatrix G(nbParam,1);
-
- int cpt=0;
- vpColVector X1(2);
- vpColVector fX1(2);
- vpColVector X2(2);
- double erreur=0;
- double erreur_prec;
- double lambda=0.01;
- do
- {
- erreur_prec=erreur;
- H=0;
- G=0;
- erreur=0;
- computeCoeff(p);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,fX1,p);
- dWarp(X1,fX1,p,dW_);
- H+=dW_.AtA();
-
- X2[0]=u[i];
- X2[1]=v[i];
-
- dX=X2-fX1;
- G+=dW_.t()*dX;
-
- erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
-
- }
-
- vpMatrix::computeHLM(H, lambda, HLM);
- try{
- p+=HLM.inverseByLU()*G;
- }
- catch(vpException &e) {
- //std::cout<<"Cannot inverse the matrix by LU " << std::endl;
- throw(e);
- }
- cpt++;
- }
- while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
- //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
-}
-#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.h
deleted file mode 100644
index 74f5273..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarp.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarp.h 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarp.h
- \brief
-*/
-
-
-#ifndef vpTemplateTrackerWarp_hh
-#define vpTemplateTrackerWarp_hh
-
-#include <visp/vpDisplay.h>
-#include <visp/vpTemplateTrackerHeader.h>
-#include <visp/vpTemplateTrackerTriangle.h>
-#include <visp/vpTemplateTrackerZone.h>
-#include <visp/vpTrackingException.h>
-
-class VISP_EXPORT vpTemplateTrackerWarp
-{
- protected:
- double denom;
- vpMatrix dW;
- unsigned int nbParam ;
-
- public:
- //constructor;
- vpTemplateTrackerWarp() : denom(1.), dW(), nbParam(0) {}
- virtual ~vpTemplateTrackerWarp(){}
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- virtual void computeCoeff(const vpColVector &p)=0;
- virtual void computeDenom(vpColVector &vX, const vpColVector &ParamM)=0;
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- virtual void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW) = 0;
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- virtual void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW) = 0;
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
- #endif
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- virtual void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW) = 0;
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- virtual void getdWdp0(const int &i,const int &j,double *dIdW) = 0;
-
- /*!
- Compute the distance between a zone and its associated warped zone.
-
- \param Z : Zone to consider.
- \param p : Parameters of the warping function.
- */
- double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z,const vpColVector &p);
-
- /*!
- Get the number of parameters of the warping function.
-
- \return Number of parameters.
- */
- unsigned int getNbParam() const {return nbParam;}
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- virtual void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const = 0;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- virtual void getParamPyramidDown(const vpColVector &p,vpColVector &pdown) =0;
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- virtual void getParamPyramidUp(const vpColVector &p,vpColVector &pup) =0;
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- virtual bool isESMcompatible() const =0;
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- virtual void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const = 0;
-
- /*!
- Set the number of parameters of the warping function.
-
- \param nb : New number of parameters.
- */
- void setNbParam(unsigned int nb){nbParam=nb;dW.resize(2,nbParam);}
-
- /*!
- Warp a list of points.
-
- \param ut0 : List of u coordinates of the points.
- \param vt0 : List of v coordinates of the points.
- \param nb_pt : Number of points to consider.
- \param p : Parameters of the warp.
- \param u : Resulting u coordinates.
- \param v : resulting v coordinates.
- */
- void warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warp.
- */
- virtual void warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM) = 0;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- virtual void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- virtual void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
-
- /*!
- Warp a triangle and store the result in a new zone.
-
- \param in : Triangle to warp.
- \param p : Parameters of the warping function. These parameters are estimated by the template
- tracker and returned using vpTemplateTracker::getp().
- \param out : Resulting triangle.
- */
- void warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out);
-
- /*!
- Warp a zone and store the result in a new zone.
-
- \param in : Zone to warp.
- \param p : Parameters of the warping function. These parameters are estimated by the template
- tracker and returned using vpTemplateTracker::getp().
- \param out : Resulting zone.
- */
- void warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out);
-
-};
-
-#endif
-
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.cpp
deleted file mode 100644
index 078f13c..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpAffine.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarpAffine.h>
-
-
-vpTemplateTrackerWarpAffine::vpTemplateTrackerWarpAffine()
-{
- nbParam = 6 ;
- dW.resize(2,nbParam);
-}
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpAffine::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
-{
- pdown=p;
- pdown[4]=p[4]/2.;
- pdown[5]=p[5]/2.;
-}
-
-void vpTemplateTrackerWarpAffine::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
-{
- pup=p;
- pup[4]=p[4]*2.;
- pup[5]=p[5]*2.;
-}
-/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpAffine::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
-{
- dIdW[0]=j*dx;
- dIdW[1]=j*dy;
- dIdW[2]=i*dx;
- dIdW[3]=i*dy;
- dIdW[4]=dx;
- dIdW[5]=dy;
-}
-/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpAffine::getdWdp0(const int &i,const int &j,double *dIdW)
-{
- dIdW[0]=j;
- dIdW[1]=0;
- dIdW[2]=i;
- dIdW[3]=0;
- dIdW[4]=1.;
- dIdW[5]=0;
-
- dIdW[6]=0;
- dIdW[7]=j;
- dIdW[8]=0;
- dIdW[9]=i;
- dIdW[10]=0;
- dIdW[11]=1.;
-}
-
-void vpTemplateTrackerWarpAffine::warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM)
-{
- j2=(1+ParamM[0])*j+ParamM[2]*i+ParamM[4];
- i2=ParamM[1]*j+(1+ParamM[3])*i+ParamM[5];
-}
-
-
-void vpTemplateTrackerWarpAffine::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- vXres[0]=(1.0+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
- vXres[1]=ParamM[1]*vX[0]+(1.0+ParamM[3])*vX[1]+ParamM[5];
-}
-
-void vpTemplateTrackerWarpAffine::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,vpMatrix &dW_)
-{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=j;dW_[0][2]=i;dW_[0][4]=1;
- dW_[1][1]=j;dW_[1][3]=i;dW_[1][5]=1;
-}
-
-/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpAffine::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
- const double *dwdp0, vpMatrix &dW_)
-{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=(1.+ParamM[0])*dwdp0[i]+ParamM[2]*dwdp0[i+nbParam];
- dW_[1][i]=ParamM[1]*dwdp0[i]+(1.+ParamM[3])*dwdp0[i+nbParam];
- }
-}
-
-void vpTemplateTrackerWarpAffine::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- vXres[0]=(1+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
- vXres[1]=ParamM[1]*vX[0]+(1+ParamM[3])*vX[1]+ParamM[5];
-}
-void vpTemplateTrackerWarpAffine::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- vpColVector Trans(2);
- vpMatrix MWrap(2,2);
- Trans[0]=ParamM[4];Trans[1]=ParamM[5];
- MWrap[0][0]=1+ParamM[0];MWrap[0][1]=ParamM[2];
- MWrap[1][0]=ParamM[1];MWrap[1][1]=1+ParamM[3];
-
- vpMatrix MWrapInv(2,2);MWrapInv=MWrap.inverseByLU();
- vpColVector TransInv(2);TransInv=-1*MWrapInv*Trans;
-
- ParamMinv[0]=MWrapInv[0][0]-1;ParamMinv[2]=MWrapInv[0][1];
- ParamMinv[1]=MWrapInv[1][0];ParamMinv[3]=MWrapInv[1][1]-1;
- ParamMinv[4]=TransInv[0];ParamMinv[5]=TransInv[1];
-}
-
-void vpTemplateTrackerWarpAffine::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
-{
- vpColVector Trans1(2);
- vpMatrix MWrap1(2,2);
- Trans1[0]=p1[4];Trans1[1]=p1[5];
- MWrap1[0][0]=1+p1[0];MWrap1[0][1]=p1[2];
- MWrap1[1][0]=p1[1];MWrap1[1][1]=1+p1[3];
-
- vpColVector Trans2(2);
- vpMatrix MWrap2(2,2);
- Trans2[0]=p2[4];Trans2[1]=p2[5];
- MWrap2[0][0]=1+p2[0];MWrap2[0][1]=p2[2];
- MWrap2[1][0]=p2[1];MWrap2[1][1]=1+p2[3];
-
- vpColVector TransRes(2);
- vpMatrix MWrapRes(2,2);
- TransRes=MWrap1*Trans2+Trans1;
- MWrapRes=MWrap1*MWrap2;
-
- pres[0]=MWrapRes[0][0]-1;pres[2]=MWrapRes[0][1];
- pres[1]=MWrapRes[1][0];pres[3]=MWrapRes[1][1]-1;
- pres[4]=TransRes[0];pres[5]=TransRes[1];
-}
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.h
deleted file mode 100644
index 0c60a29..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpAffine.h
+++ /dev/null
@@ -1,175 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpAffine.h 4607 2014-01-21 16:02:11Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarpAffine.h
- \brief Affine warping function: w(X)=AX+b with: A=[[1+p0, p2], [p1, 1+p3]] and b= [p4, p5]]
-*/
-
-
-#ifndef vpTemplateTrackerWarpAffine_hh
-#define vpTemplateTrackerWarpAffine_hh
-
-#include <visp/vpTemplateTrackerWarp.h>
-
-
-class VISP_EXPORT vpTemplateTrackerWarpAffine: public vpTemplateTrackerWarp
-{
- public:
- //constructor;
- vpTemplateTrackerWarpAffine();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the columns) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-};
-#endif
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.cpp
deleted file mode 100644
index 46ad0f1..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.cpp
+++ /dev/null
@@ -1,221 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpHomography.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarpHomography.h>
-#include <visp/vpTrackingException.h>
-
-vpTemplateTrackerWarpHomography::vpTemplateTrackerWarpHomography()
-{
- nbParam = 8 ;
- dW.resize(2,nbParam);
-}
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpHomography::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
-{
- pdown=p;
- pdown[2]=p[2]*2.;
- pdown[5]=p[5]*2.;
- pdown[6]=p[6]/2.;
- pdown[7]=p[7]/2.;
-}
-
-void vpTemplateTrackerWarpHomography::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
-{
- pup=p;
- pup[2]=p[2]/2.;
- pup[5]=p[5]/2.;
- pup[6]=p[6]*2.;
- pup[7]=p[7]*2.;
-}
-
-/*calcul de di*dw(x,p0)/dp */
-void vpTemplateTrackerWarpHomography::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
-{
- dIdW[0]=j*dx;
- dIdW[1]=j*dy;
- dIdW[2]=-j*j*dx-i*j*dy;
- dIdW[3]=i*dx;
- dIdW[4]=i*dy;
- dIdW[5]=-i*j*dx-i*i*dy;
- dIdW[6]=dx;
- dIdW[7]=dy;
-}
-/*calcul de dw(x,p0)/dp */
-void vpTemplateTrackerWarpHomography::getdWdp0(const int &i,const int &j,double *dIdW)
-{
- dIdW[0]=j;
- dIdW[1]=0;
- dIdW[2]=-j*j;
- dIdW[3]=i;
- dIdW[4]=0;
- dIdW[5]=-i*j;
- dIdW[6]=1.;
- dIdW[7]=0;
-
- dIdW[8]=0;
- dIdW[9]=j;
- dIdW[10]=-i*j;
- dIdW[11]=0;
- dIdW[12]=i;
- dIdW[13]=-i*i;
- dIdW[14]=0;
- dIdW[15]=1.;
-}
-void vpTemplateTrackerWarpHomography::computeDenom(vpColVector &vX, const vpColVector &ParamM)
-{
- denom=(1./(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1.));
-}
-
-void vpTemplateTrackerWarpHomography::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
-{
- j2=((1.+ParamM[0])*j+ParamM[3]*i+ParamM[6])*denom;
- i2=(ParamM[1]*j+(1.+ParamM[4])*i+ParamM[7])*denom;
-}
-
-
-void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- //if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
- if((denom)>0)// FS optimisation
- {
- vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
- vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
- }
- else
- throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
-}
-
-void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
-{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=j*denom;
- dW_[0][2]=-j*X2[0]*denom;
- dW_[0][3]=i*denom;
- dW_[0][5]=-i*X2[0]*denom;
- dW_[0][6]=denom;
-
- dW_[1][1]=j*denom;
- dW_[1][2]=-j*X2[1]*denom;
- dW_[1][4]=i*denom;
- dW_[1][5]=-i*X2[1]*denom;
- dW_[1][7]=denom;
-}
-
-/*compute dw=dw/dx*dw/dp */
-void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &ParamM,
- const double *dwdp0,vpMatrix &dW_)
-{
- double dwdx0,dwdx1;
- double dwdy0,dwdy1;
-
- dwdx0=((1.+ParamM[0])-X2[0]*ParamM[2])*denom;
- dwdx1=(ParamM[1]-X2[1]*ParamM[2])*denom;
- dwdy0=(ParamM[3]-X2[0]*ParamM[5])*denom;
- dwdy1=((1.+ParamM[4])-X2[1]*ParamM[5])*denom;
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=dwdx0*dwdp0[i]+dwdy0*dwdp0[i+nbParam];
- dW_[1][i]=dwdx1*dwdp0[i]+dwdy1*dwdp0[i+nbParam];
- }
-}
-
-void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
-
- if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)<0)//si dans le plan image reel
- {
- vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
- vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
- }
- else
- throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
-}
-void vpTemplateTrackerWarpHomography::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- vpHomography H = getHomography(ParamM);
- vpHomography Hinv = H.inverse();
- getParam(Hinv, ParamMinv);
-}
-
-
-vpHomography vpTemplateTrackerWarpHomography::getHomography(const vpColVector &ParamM) const
-{
- vpHomography H;
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- {
- if(i+3*j!=8)
- {
- H[i][j]=ParamM[i+3*j];
- if(i==j)H[i][j]++;
- }
- else
- H[i][j]=1.;
- }
-
- return H;
-}
-void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H,vpColVector &par) const
-{
- par=0;
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- {
- if(i+3*j!=8)
- {
- par[i+3*j]=H[i][j]/H[2][2];
- if(i==j)par[i+3*j]--;
- }
- }
-}
-
-
-
-void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
-{
- vpHomography H1 = getHomography(p1);
- vpHomography H2 = getHomography(p2);
- vpHomography H = H1*H2;
- getParam(H,pres);
-}
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.h
deleted file mode 100644
index 58201ab..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomography.h
+++ /dev/null
@@ -1,200 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpHomography.h 4607 2014-01-21 16:02:11Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarpHomography.h
- \brief warping function of an homography: the homography is directly defined by the diplacement parameter: H=[[1+p0, p3, p6], [p1, 1+p4, p7], [p2, p5, 1]]
-*/
-
-
-#ifndef vpTemplateTrackerWarpHomography_hh
-#define vpTemplateTrackerWarpHomography_hh
-
-#include <visp/vpTemplateTrackerWarp.h>
-#include <visp/vpHomography.h>
-
-
-class VISP_EXPORT vpTemplateTrackerWarpHomography: public vpTemplateTrackerWarp
-{
- public:
- //constructor;
- vpTemplateTrackerWarpHomography();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- #endif
-
- /*!
- Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
-
- \param vX : Point to consider
- \param ParamM : parameters of the warping function.
- */
- void computeDenom(vpColVector &vX, const vpColVector &ParamM);
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Return the homography defined by the given parameters
-
- \param ParamM : Parameters of the homography.
-
- \return An Homography via vpHomography.
- */
- vpHomography getHomography(const vpColVector &ParamM) const;
-
- /*!
- Compute the parameters of the homography warping function according to the given vpHomography
-
- \param H : Homography used to compute the parameters.
- \param par : Resulting warping function parameters.
- */
- void getParam(const vpHomography &H,vpColVector &par) const;
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-};
-#endif
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.cpp
deleted file mode 100644
index 507fec2..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.cpp
+++ /dev/null
@@ -1,402 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpHomographySL3.cpp 4783 2014-07-15 13:07:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarpHomographySL3.h>
-
-//findWarp special a SL3 car methode additionnelle ne marche pas (la derivee n est calculable qu en p=0)
-// => resout le probleme de maniere compositionnelle
-void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0,const double *vt0,
- const double *u, const double *v,int nb_pt,vpColVector& p)
-{
- //std::cout<<"findWarp OVERLOADE"<<std::endl;
- vpColVector dp(nbParam);
- vpMatrix dW_(2,nbParam);
- vpMatrix dX(2,1);
- vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
- vpMatrix G_(nbParam,1);
-
- //vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
- double **dW_ddp0=new double*[(unsigned int)nb_pt];
- for(int i=0;i<nb_pt;i++)
- {
- //dW_ddp0[i].resize(2,nbParam);
- dW_ddp0[i]=new double[2*nbParam];
- //getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
- //std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
- getdWdp0(v[i],u[i],dW_ddp0[i]);
- }
-
- int cpt=0;
- vpColVector X1(2);
- vpColVector fX1(2);
- vpColVector X2(2);
- double erreur=0;
- double erreur_prec;
- double lambda=0.00001;
- do
- {
- erreur_prec=erreur;
- H=0;
- G_=0;
- erreur=0;
- computeCoeff(p);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,fX1,p);
- //dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
- //dWarp(X1,fX1,p,dW);
- for(unsigned int ip=0;ip<nbParam;ip++)
- {
- dW_[0][ip]=dW_ddp0[i][ip];
- dW_[1][ip]=dW_ddp0[i][ip+nbParam];
- }
-
- H+=dW_.AtA();
-
- X2[0]=u[i];
- X2[1]=v[i];
-
- dX=X2-fX1;
- G_+=dW_.t()*dX;
-
- erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
- }
-
- vpMatrix::computeHLM(H, lambda, HLM);
- try{
- dp=HLM.inverseByLU()*G_;
- }
- catch(vpException &e){
- //std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
- throw(e);
- }
- pRondp(p,dp,p);
-
- cpt++;
- // std::cout<<"erreur ="<<erreur<<std::endl;
- }
- //while((cpt<1500));
- while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
-
- //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
- for(int i=0;i<nb_pt;i++)
- delete[] dW_ddp0[i];
- delete[] dW_ddp0;
-
-}
-
-vpTemplateTrackerWarpHomographySL3::vpTemplateTrackerWarpHomographySL3()
- : G(), dGx(), A()
-{
- nbParam = 8 ;
- G.resize(3,3);
- dGx.resize(3,nbParam);
-
- A.resize(8);
- for(unsigned int i=0;i<8;i++)
- {
- A[i].resize(3,3);
- A[i]=0;
- }
- A[0][0][2]=1;
- A[1][1][2]=1;
- A[2][0][1]=1;
- A[3][1][0]=1;
- A[4][0][0]=1;
- A[4][1][1]=-1;
- A[5][1][1]=-1;
- A[5][2][2]=1;
- A[6][2][0]=1;
- A[7][2][1]=1;
-}
-
-vpTemplateTrackerWarpHomographySL3::~vpTemplateTrackerWarpHomographySL3()
-{
-}
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-//a refaire de facon analytique
-void vpTemplateTrackerWarpHomographySL3::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
-{
- double *u,*v;u=new double[4];v=new double[4];
- //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
- u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
- double *u2,*v2;u2=new double[4];v2=new double[4];
- warp(u,v,4,p,u2,v2);
- //p=0;findWarp(u,v,u2,v2,4,p);
- for(int i=0;i<4;i++)
- {
- u[i]=u[i]/2.;
- v[i]=v[i]/2.;
- u2[i]=u2[i]/2.;
- v2[i]=v2[i]/2.;
- //std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
- }
- pdown=p;
- findWarp(u,v,u2,v2,4,pdown);
- delete[] u;
- delete[] v;
- delete[] u2;
- delete[] v2;
-}
-
-void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
-{
- double *u,*v;u=new double[4];v=new double[4];
- //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
- u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
- //u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
- double *u2,*v2;u2=new double[4];v2=new double[4];
-
- //pup=p;
-
- /*vpColVector ptest=pup;
- warp(u,v,4,ptest,u2,v2);
- for(int i=0;i<4;i++)
- std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
-
- warp(u,v,4,p,u2,v2);
- //p=0;findWarp(u,v,u2,v2,4,p);
-
-
- for(int i=0;i<4;i++)
- {
- u[i]=u[i]*2.;
- v[i]=v[i]*2.;
- u2[i]=u2[i]*2.;
- v2[i]=v2[i]*2.;
- /*std::cout<<"#########################################################################################"<<std::endl;
- std::cout<<"#########################################################################################"<<std::endl;
- std::cout<<"#########################################################################################"<<std::endl;
- std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
- }
- findWarp(u,v,u2,v2,4,pup);
-
- delete[] u;
- delete[] v;
- delete[] u2;
- delete[] v2;
-}
-
-void vpTemplateTrackerWarpHomographySL3::computeDenom(vpColVector &vX, const vpColVector &/*ParamM*/)
-{
- denom=vX[0]*G[2][0]+vX[1]*G[2][1]+G[2][2];
-}
-
-void vpTemplateTrackerWarpHomographySL3::computeCoeff(const vpColVector &p)
-{
- vpMatrix pA(3,3);
- pA[0][0]=p[4];
- pA[0][1]=p[2];
- pA[0][2]=p[0];
-
- pA[1][0]=p[3];
- pA[1][1]=-p[4]-p[5];
- pA[1][2]=p[1];
-
- pA[2][0]=p[6];
- pA[2][1]=p[7];
- pA[2][2]=p[5];
-
- G=pA.expm();
-}
-
-
-void vpTemplateTrackerWarpHomographySL3::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &/*ParamM*/)
-{
- double i=vX[1],j=vX[0];
- vXres[0]=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
- vXres[1]=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
-}
-void vpTemplateTrackerWarpHomographySL3::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &/*ParamM*/)
-{
- j2=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
- i2=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
-}
-
-vpHomography vpTemplateTrackerWarpHomographySL3::getHomography() const
-{
- vpHomography H;
- for (unsigned int i=0; i<3; i++)
- for (unsigned int j=0; j<3; j++)
- H[i][j] = G[i][j];
- return H;
-}
-
-void vpTemplateTrackerWarpHomographySL3::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
-{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1./denom;dhdx[1][1]=1./denom;dhdx[0][2]=-X2[0]/(denom);dhdx[1][2]=-X2[1]/(denom);
- dGx=0;
- for(unsigned int i=0;i<3;i++)
- {
- dGx[i][0]=G[i][0];
- dGx[i][1]=G[i][1];
- dGx[i][2]=G[i][0]*X1[1];
- dGx[i][3]=G[i][1]*X1[0];
- dGx[i][4]=G[i][0]*X1[0]-G[i][1]*X1[1];
- dGx[i][5]=G[i][2]-G[i][1]*X1[1];
- dGx[i][6]=G[i][2]*X1[0];
- dGx[i][7]=G[i][2]*X1[1];
- }
- dW_=dhdx*dGx;
-
-}
-
-/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
-{
- vpMatrix dhdx(1,3);
- dhdx=0;
- dhdx[0][0]=dx;dhdx[0][1]=dy;dhdx[0][2]=-j*dx-i*dy;
- G.setIdentity();
-
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
- }
-
- for(unsigned int par=0;par<nbParam;par++)
- {
- double res=0;
- for(unsigned int par2=0;par2<3;par2++)
- res+=dhdx[0][par2]*dGx[par2][par];
- dIdW[par]=res;
- }
-
-}
-/*calcul de dw(x,p0)/dp
-*/
-
-void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i,const int &j,double *dIdW)
-{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
- G.setIdentity();
-
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
- }
- vpMatrix dIdW_temp(2,nbParam);
- dIdW_temp=dhdx*dGx;
-
- for(unsigned int par=0;par<nbParam;par++)
- {
- dIdW[par]=dIdW_temp[0][par];
- dIdW[par+nbParam]=dIdW_temp[1][par];
- }
-
-}
-void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i,const double &j,double *dIdW)
-{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
- G.setIdentity();
-
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
- }
- vpMatrix dIdW_temp(2,nbParam);
- dIdW_temp=dhdx*dGx;
-
- for(unsigned int par=0;par<nbParam;par++)
- {
- dIdW[par]=dIdW_temp[0][par];
- dIdW[par+nbParam]=dIdW_temp[1][par];
- }
-
-}
-/*compute dw=dw/dx*dw/dp
-*/
-
-void vpTemplateTrackerWarpHomographySL3::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &/*ParamM*/,
- const double *dwdp0,vpMatrix &dW_)
-{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=denom*((G[0][0]-X2[0]*G[2][0])*dwdp0[i]+(G[0][1]-X2[0]*G[2][1])*dwdp0[i+nbParam]);
- dW_[1][i]=denom*((G[1][0]-X2[1]*G[2][0])*dwdp0[i]+(G[1][1]-X2[1]*G[2][1])*dwdp0[i+nbParam]);
- }
-}
-
-void vpTemplateTrackerWarpHomographySL3::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- ParamMinv=-ParamM;
-}
-void vpTemplateTrackerWarpHomographySL3::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
-{
- //vrai que si commutatif ...
- pres=p1+p2;
-}
-
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.h
deleted file mode 100644
index 43adff0..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpHomographySL3.h
+++ /dev/null
@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpHomographySL3.h 4669 2014-02-16 16:25:57Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarpHomographySL3.h
- \brief warping function of an homography: the homography is defined on the sl3 lie algebra H=exp(Sum(p[i]* A_i)) A_i is the basis of the SL3 Algebra
-*/
-
-
-#ifndef vpTemplateTrackerWarpHomographySL3_hh
-#define vpTemplateTrackerWarpHomographySL3_hh
-
-#include <vector>
-
-#include <visp/vpTemplateTrackerWarp.h>
-#include <visp/vpHomography.h>
-
-class VISP_EXPORT vpTemplateTrackerWarpHomographySL3: public vpTemplateTrackerWarp
-{
- protected:
- vpMatrix G;
- vpMatrix dGx;
- std::vector<vpMatrix> A;
-
- public:
- //constructor;
- vpTemplateTrackerWarpHomographySL3();
- ~vpTemplateTrackerWarpHomographySL3();
-
- /*!
- Compute the exponential of the homography matrix defined by the given parameters
-
- \param p : Parameters of the SL3 homography warping function.
- */
- void computeCoeff(const vpColVector &p);
-
- /*!
- Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
-
- \param vX : Point to consider
- \param ParamM : parameters of the warping function.
- */
- void computeDenom(vpColVector &vX, const vpColVector &ParamM);
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Find the displacement/warping function parameters from a list of points.
-
- \param ut0 : Original u coordinates.
- \param vt0 : Original v coordinates.
- \param u : Warped u coordinates.
- \param v : Warped v coordinates.
- \param nb_pt : Number of points.
- \param p : Resulting warping function parameters.
- */
- void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const double &i, const double &j,double *dIdW);
-
- /*!
- Return the homography defined by the warping function
-
- \return An Homography via vpHomography.
- */
- vpHomography getHomography() const;
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return true;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void warpXInv(const vpColVector &/*vX*/,vpColVector &/*vXres*/,const vpColVector &/*ParamM*/) {}
- #endif
-};
-#endif
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.cpp
deleted file mode 100644
index 6913eff..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.cpp
+++ /dev/null
@@ -1,188 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpSRT.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarpSRT.h>
-
-
-vpTemplateTrackerWarpSRT::vpTemplateTrackerWarpSRT()
-{
- nbParam = 4 ;
- dW.resize(2,nbParam);
-}
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpSRT::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
-{
- pdown=p;
- pdown[2]=p[2]/2.;
- pdown[3]=p[3]/2.;
-}
-
-void vpTemplateTrackerWarpSRT::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
-{
- pup=p;
- pup[2]=p[2]*2.;
- pup[3]=p[3]*2.;
-}
-/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpSRT::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
-{
- // std::cout << "getdW0" << std::endl;
- dIdW[0]=j*dx + i*dy;
- dIdW[1]=-i*dx + j*dy;
- dIdW[2]=dx;
- dIdW[3]=dy;
-}
-/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpSRT::getdWdp0(const int &i,const int &j,double *dIdW)
-{
- dIdW[0]=j;
- dIdW[1]=-i;
- dIdW[2]=1.;
- dIdW[3]=0;
-
- dIdW[4]=i;
- dIdW[5]=j;
- dIdW[6]=0;
- dIdW[7]=1.;
-}
-
-void vpTemplateTrackerWarpSRT::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
-{
- j2=((1.0+ParamM[0])*cos(ParamM[1])*j) - ((1.0+ParamM[0])*sin(ParamM[1])*i) + ParamM[2];
- i2=((1.0+ParamM[0])*sin(ParamM[1])*j) + ((1.0+ParamM[0])*cos(ParamM[1])*i) + ParamM[3];
-}
-
-
-void vpTemplateTrackerWarpSRT::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
- vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
-}
-
-void vpTemplateTrackerWarpSRT::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &ParamM,vpMatrix &dW_)
-{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=cos(ParamM[1])*j - sin(ParamM[1])*i;
- dW_[0][1]=(-(1.0+ParamM[0])*sin(ParamM[1])*j) - ((1.0+ParamM[0])*cos(ParamM[1])*i);
- dW_[0][2]=1;
-
- dW_[1][0]=sin(ParamM[1])*j + cos(ParamM[1])*i;
- dW_[1][1]=(1.0+ParamM[0])*cos(ParamM[1])*j - (1.0+ParamM[0])*sin(ParamM[1])*i;
- dW_[1][3]=1;
-}
-
-/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpSRT::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
- const double *dwdp0,vpMatrix &dW_)
-{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=((1.+ParamM[0])*cos(ParamM[1])*dwdp0[i]) - ((1.0+ParamM[0])*sin(ParamM[1])*dwdp0[i+nbParam]);
- dW_[1][i]=((1.+ParamM[0])*sin(ParamM[1])*dwdp0[i]) + ((1.0+ParamM[0])*cos(ParamM[1])*dwdp0[i+nbParam]);
- }
-}
-
-void vpTemplateTrackerWarpSRT::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- // std::cout << "warpXspe" << std::endl;
- vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
- vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
-}
-
-void vpTemplateTrackerWarpSRT::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- vpColVector Trans(2);
- vpMatrix MWrap(2,2);
- Trans[0]=ParamM[2];
- Trans[1]=ParamM[3];
- MWrap[0][0]=cos(ParamM[1]);
- MWrap[0][1]=-sin(ParamM[1]);
- MWrap[1][0]=sin(ParamM[1]);
- MWrap[1][1]=cos(ParamM[1]);
-
- vpMatrix MWrapInv(2,2);
- MWrapInv=MWrap.transpose();
- vpColVector TransInv(2);
- TransInv=(-1.0/(1.0+ParamM[0]))*MWrapInv*Trans;
-
- ParamMinv[0]=1.0/(1.0+ParamM[0]) - 1.0;
- ParamMinv[1]= atan2(MWrapInv[1][0],MWrapInv[1][1]);
- ParamMinv[2]=TransInv[0];
- ParamMinv[3]=TransInv[1];
-}
-
-void vpTemplateTrackerWarpSRT::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
-{
- vpColVector Trans1(2);
- vpMatrix MWrap1(2,2);
- Trans1[0]=p1[2];Trans1[1]=p1[3];
-
- MWrap1[0][0]=cos(p1[1]);
- MWrap1[0][1]=-sin(p1[1]);
- MWrap1[1][0]=sin(p1[1]);
- MWrap1[1][1]=cos(p1[1]);
-
- vpColVector Trans2(2);
- vpMatrix MWrap2(2,2);
- Trans2[0]=p2[2];Trans2[1]=p2[3];
-
- MWrap2[0][0]=cos(p2[1]);
- MWrap2[0][1]=-sin(p2[1]);
- MWrap2[1][0]=sin(p2[1]);
- MWrap2[1][1]=cos(p2[1]);
-
- vpColVector TransRes(2);
- vpMatrix MWrapRes(2,2);
- TransRes=(1.0+p1[0])*MWrap1*Trans2+Trans1;
- MWrapRes=MWrap1*MWrap2;
-
- pres[0]=(1.0+p1[0])*(1.0+p2[0]) - 1.0;
- pres[1]=atan2(MWrapRes[1][0],MWrapRes[1][1]);
-
- pres[2]=TransRes[0];
- pres[3]=TransRes[1];
-}
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.h
deleted file mode 100644
index 08f7d50..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpSRT.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpSRT.h 4607 2014-01-21 16:02:11Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarpSRT.h
- \brief sRt warping function
-*/
-
-
-#ifndef vpTemplateTrackerWarpSRT_hh
-#define vpTemplateTrackerWarpSRT_hh
-
-#include <visp/vpTemplateTrackerWarp.h>
-
-class VISP_EXPORT vpTemplateTrackerWarpSRT: public vpTemplateTrackerWarp
-{
- public:
- //constructor;
- vpTemplateTrackerWarpSRT();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-};
-#endif
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.cpp b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.cpp
deleted file mode 100644
index d61f8fc..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpTranslation.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerWarpTranslation.h>
-
-vpTemplateTrackerWarpTranslation::vpTemplateTrackerWarpTranslation()
-{
- nbParam = 2 ;
- dW.resize(2,nbParam);
-}
-
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpTranslation::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
-{
- pdown[0]=p[0]/2.;
- pdown[1]=p[1]/2.;
-}
-
-void vpTemplateTrackerWarpTranslation::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
-{
- pup[0]=p[0]*2.;
- pup[1]=p[1]*2.;
-}
-
-/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpTranslation::getdW0(const int &/*i*/,const int &/*j*/,const double &dy,const double &dx,double *dIdW)
-{
- dIdW[0]=dx;
- dIdW[1]=dy;
-}
-/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpTranslation::getdWdp0(const int &/*i*/,const int &/*j*/,double *dIdW)
-{
- dIdW[0]=1.;
- dIdW[1]=0;
-
- dIdW[2]=0;
- dIdW[3]=1.;
-}
-
-void vpTemplateTrackerWarpTranslation::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
-{
- j2=j+ParamM[0];
- i2=i+ParamM[1];
-}
-
-
-void vpTemplateTrackerWarpTranslation::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- vXres[0]=vX[0]+ParamM[0];
- vXres[1]=vX[1]+ParamM[1];
-}
-
-void vpTemplateTrackerWarpTranslation::dWarp(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
- vpMatrix &dW_)
-{
- dW_[0][0]=1;dW_[0][1]=0;
- dW_[1][0]=0;dW_[1][1]=1;
-}
-
-/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpTranslation::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
- const double *dwdp0,vpMatrix &dW_)
-{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=dwdp0[i];
- dW_[1][i]=dwdp0[i+nbParam];
- }
-}
-
-void vpTemplateTrackerWarpTranslation::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
-{
- vXres[0]=vX[0]+ParamM[0];
- vXres[1]=vX[1]+ParamM[1];
-}
-void vpTemplateTrackerWarpTranslation::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- ParamMinv[0]=-ParamM[0];
- ParamMinv[1]=-ParamM[1];
-}
-
-void vpTemplateTrackerWarpTranslation::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
-{
- pres[0]=p1[0]+p2[0];
- pres[1]=p1[1]+p2[1];
-}
diff --git a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.h b/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.h
deleted file mode 100644
index 61a705f..0000000
--- a/src/tracking/template-tracker/warp/vpTemplateTrackerWarpTranslation.h
+++ /dev/null
@@ -1,175 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerWarpTranslation.h 4607 2014-01-21 16:02:11Z ayol $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerWarpTranslation.h
- \brief Translation warping function: w(X)=X+b with: b= [p0, p1]]
-*/
-
-
-#ifndef vpTemplateTrackerWarpTranslation_hh
-#define vpTemplateTrackerWarpTranslation_hh
-
-#include <visp/vpTemplateTrackerWarp.h>
-
-
-class VISP_EXPORT vpTemplateTrackerWarpTranslation: public vpTemplateTrackerWarp
-{
- public:
- //constructor;
- vpTemplateTrackerWarpTranslation();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warp.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const ;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return true;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-};
-#endif
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.cpp b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.cpp
deleted file mode 100644
index 1b14af2..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCC.cpp 4682 2014-02-24 07:56:27Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerZNCC.h>
-
-vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
- : vpTemplateTracker(warp), DI(), temp()
-{
- dW.resize(2,nbParam);
- G.resize(nbParam);
- H.resize(nbParam,nbParam);
- HLM.resize(nbParam,nbParam);
-
- temp.resize(nbParam);
-
- X1.resize(2);
- X2.resize(2);
- DI.resize(2);
-}
-
-
-double vpTemplateTrackerZNCC::getCost(const vpImage<unsigned char> &I, vpColVector &tp)
-{
- double IW,Tij;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- Warp->computeCoeff(tp);
-
- double moyTij=0;
- double moyIW=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
-
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- moyTij+=Tij;
- moyIW+=IW;
- Nbpoint++;
- }
- }
- ratioPixelIn=(double)Nbpoint/(double)templateSize;
- if(! Nbpoint) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot get cost: size = 0")) ;
- }
-
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
-
- double nom=0,denom=0;
- double var1=0,var2=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
-
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- nom+=(Tij-moyTij)*(IW-moyIW);
- denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
- var1+=(IW-moyIW)*(IW-moyIW);
- var2+=(Tij-moyTij)*(Tij-moyTij);
-
- Nbpoint++;
- }
- }
- // if(Nbpoint==0)return 10e10; // cannot occur
- //return -nom/sqrt(denom);
- return -nom/sqrt(var1*var2);
-}
-
-
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.h b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.h
deleted file mode 100644
index 39e45fa..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCC.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCC.h 4956 2014-11-12 15:50:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerZNCC.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerZNCC_hh
-#define vpTemplateTrackerZNCC_hh
-
-#include <math.h>
-
-#include <visp/vpTemplateTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpImageFilter.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomography.h>
-
-#define APPROX_NCC
-
-class VISP_EXPORT vpTemplateTrackerZNCC: public vpTemplateTracker
-{
- protected:
- vpRowVector DI;
- vpRowVector temp;
-
- protected:
- double getCost(const vpImage<unsigned char> &I, vpColVector &tp) ;
- double getCost(const vpImage<unsigned char> &I) {vpColVector tp; return getCost(I,tp);}
- virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
- virtual void trackNoPyr(const vpImage<unsigned char> &I)=0;
-
- public:
- vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp);
-
- void setGain(double _gain){gain=_gain;}
-};
-#endif
-
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
deleted file mode 100644
index 98ab0ed..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCCForwardAdditional.cpp 4783 2014-07-15 13:07:52Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <visp/vpTemplateTrackerZNCCForwardAdditional.h>
-#include <visp/vpImageFilter.h>
-
-vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp):vpTemplateTrackerZNCC(warp)
-{
- useCompositionnal=false;
-}
-
-void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
-{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- vpImage<double> dIxx,dIxy,dIyx,dIyy;
- vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
- vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
-
- vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
- vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
-
- Warp->computeCoeff(p);
- double IW,dIWx,dIWy;
- double Tij;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- double moyTij=0;
- double moyIW=0;
- double denom=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
-
- Nbpoint++;
- moyTij+=Tij;
- moyIW+=IW;
- }
- }
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
- Hdesire=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
-
-
- double prod=(Tij-moyTij);
-
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- Hdesire[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
- /*Hdesire[0][0]+=prod*d_Ixx;
- Hdesire[1][0]+=prod*d_Ixy;
- Hdesire[0][1]+=prod*d_Ixy;
- Hdesire[1][1]+=prod*d_Iyy;*/
-
- denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
- delete[] tempt;
- }
-
-
- }
-
- Hdesire=Hdesire/sqrt(denom);
- vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
-
-}
-
-void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
-{
- double erreur=0;
- int Nbpoint=0;
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- /*vpImage<double> dIxx,dIxy,dIyx,dIyy;
- getGradX(dIx, dIxx, fgdG,taillef);
- getGradY(dIx, dIxy, fgdG,taillef);
-
- getGradX(dIy, dIyx, fgdG,taillef);
- getGradY(dIy, dIyy, fgdG,taillef);*/
-
- dW=0;
-
- //double lambda=lambdaDep;
- double IW,dIWx,dIWy;
- double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- Nbpoint=0;
- erreur=0;
- G=0;
- H=0 ;
- Warp->computeCoeff(p);
- double moyTij=0;
- double moyIW=0;
- double denom=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
-
- Nbpoint++;
- moyTij+=Tij;
- moyIW+=IW;
- }
- }
-
- if(! Nbpoint) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot track the template: no point")) ;
- }
-
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
- //vpMatrix d2Wx(nbParam,nbParam);
- //vpMatrix d2Wy(nbParam,nbParam);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
- else
- IW=BI.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
-
-
- double prod=(Tij-moyTij);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=prod*tempt[it];
-
- /* Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
- for(int it=0;it<nbParam;it++)
- for(int jt=0;jt<nbParam;jt++)
- H[it][jt]+=prod*(d2Wx[it][jt]*dIWx+d2Wx[it][jt]*dIWy);*/
- /*double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(int it=0;it<nbParam;it++)
- for(int jt=0;jt<nbParam;jt++)
- H[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));*/
- /*H[0][0]+=prod*d_Ixx;
- H[1][0]+=prod*d_Ixy;
- H[0][1]+=prod*d_Ixy;
- H[1][1]+=prod*d_Iyy;*/
-
- double er=(Tij-IW);
- erreur+=(er*er);
- denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
- delete[] tempt;
- }
-
-
- }
- /*std::cout<<"G="<<G<<std::endl;
- std::cout<<"H="<<H<<std::endl;
- std::cout<<" denom="<<denom<<std::endl;*/
- G=G/sqrt(denom);
- //std::cout<<G<<std::endl;
- H=H/sqrt(denom);
-
- //if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl; // cannot occur
-
- try
- {
- //vpMatrix::computeHLM(H,lambda,HLM);
- //dp=1.*HLM.inverseByLU()*G;
- dp=1.*HLMdesireInverse*G;
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
- throw(e);
- }
-
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
- }
- p-=dp;
- iteration++;
- }
- while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
-
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
-}
-
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.h b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.h
deleted file mode 100644
index 5da1c7e..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCForwardAdditional.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCCForwardAdditional.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerZNCCForwardAdditional.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerZNCCForwardAdditional_hh
-#define vpTemplateTrackerZNCCForwardAdditional_hh
-
-
-#include <visp/vpTemplateTrackerZNCC.h>
-
-/*!
- The algorithm implemented in this class is described in \cite Irani98a.
- */
-class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional: public vpTemplateTrackerZNCC
-{
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
-
- public:
- vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp);
-};
-#endif
-
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
deleted file mode 100644
index 8d9cdca..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
+++ /dev/null
@@ -1,439 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCCInverseCompositional.cpp 5264 2015-02-04 13:49:55Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-#include <limits> // numeric_limits
-
-#include <visp/vpTemplateTrackerZNCCInverseCompositional.h>
-#include <visp/vpImageFilter.h>
-
-vpTemplateTrackerZNCCInverseCompositional::vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerZNCC(warp), compoInitialised(false),
- evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8), moydIrefdp()
-{
- useInverse=true;
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
-{
- //std::cout<<"Initialise precomputed value of Compositionnal Inverse"<<std::endl;
- int i,j;
-
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
-
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
-
- double dx=ptTemplate[point].dx;
- double dy=ptTemplate[point].dy;
- //std::cout<<ptTemplate[point].dx<<","<<ptTemplate[point].dy<<std::endl;
-
- Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
-
- }
- //vpTRACE("fin Comp Inverse");
- compoInitialised=true;
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompInverse(I);
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- vpImage<double> dIxx,dIxy,dIyx,dIyy;
- vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
- vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
-
- vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
- vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
-
- Warp->computeCoeff(p);
- double Ic,dIcx=0.,dIcy=0.;
- double Iref;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- double moyIref=0;
- double moyIc=0;
- double denom=0;
- moydIrefdp.resize(nbParam); moydIrefdp=0;
- vpMatrix moyd2Iref(nbParam,nbParam);moyd2Iref=0;
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
- else
- Ic=BI.getValue(i2,j2);
-
- Nbpoint++;
- moyIref+=Iref;
- moyIc+=Ic;
-
- for(unsigned int it=0;it<nbParam;it++)
- moydIrefdp[it]+=ptTemplate[point].dW[it];
-
-
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- {
- moyd2Iref[it][jt] +=(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
- }
-
- delete[] tempt;
-
-
- }
- }
-
- moyIref=moyIref/Nbpoint;
- moydIrefdp=moydIrefdp/Nbpoint;
- moyd2Iref=moyd2Iref/Nbpoint;
- moyIc=moyIc/Nbpoint;
- Hdesire=0;
- double covarIref=0,covarIc=0;
- double sIcIref=0;
- vpColVector sIcdIref(nbParam);sIcdIref=0;
- vpMatrix sIcd2Iref(nbParam,nbParam);sIcd2Iref=0;
- vpMatrix sdIrefdIref(nbParam,nbParam);sdIrefdIref=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
- else
- Ic=BI.getValue(i2,j2);
-
- dIcx=dIx.getValue(i2,j2);
- dIcy=dIy.getValue(i2,j2);
-
- Warp->dWarp(X1,X2,p,dW);
-
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
-
- double prodIc=(Ic-moyIc);
-
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- {
- sIcd2Iref[it][jt] +=prodIc*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy)-moyd2Iref[it][jt]);
- sdIrefdIref[it][jt] +=(ptTemplate[point].dW[it]-moydIrefdp[it])*(ptTemplate[point].dW[jt]-moydIrefdp[jt]);
- }
-
-
- delete[] tempt;
-
- for(unsigned int it=0;it<nbParam;it++)
- sIcdIref[it]+=prodIc*(ptTemplate[point].dW[it]-moydIrefdp[it]);
-
- covarIref+=(Iref-moyIref)*(Iref-moyIref);
- covarIc+=(Ic-moyIc)*(Ic-moyIc);
- sIcIref+=(Iref-moyIref)*(Ic-moyIc);
- }
-
-
- }
- covarIref=sqrt(covarIref);
- covarIc=sqrt(covarIc);
-
- denom=covarIref*covarIc;
-
- double NCC=sIcIref/denom;
- //std::cout<<"NCC = "<<NCC<<std::endl;
- vpColVector dcovarIref(nbParam);dcovarIref=-sIcdIref/covarIref;
-
- vpColVector dNCC(nbParam);dNCC=(sIcdIref/denom-NCC*dcovarIref/covarIref);
- vpMatrix d2covarIref(nbParam,nbParam);
- d2covarIref=-(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/covarIref;
-#ifdef APPROX_NCC
- Hdesire=sIcd2Iref/denom;
-#else
- Hdesire=(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/denom;
-#endif
- vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
-{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
-
- double erreur=0;
- unsigned int Nbpoint=0;
- vpColVector dpinv(nbParam);
- double Ic;
- double Iref;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- initPosEvalRMS(p);
- do
- {
- Nbpoint=0;
- erreur=0;
- G=0;
- Warp->computeCoeff(p);
- double moyIref=0;
- double moyIc=0;
- double denom=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
- else
- Ic=BI.getValue(i2,j2);
-
- Nbpoint++;
- moyIref+=Iref;
- moyIc+=Ic;
- }
-
-
- }
- if(Nbpoint > 0)
- {
- moyIref=moyIref/Nbpoint;
- moyIc=moyIc/Nbpoint;
- double sIcIref=0;
- double covarIref=0,covarIc=0;
- vpColVector sIcdIref(nbParam);sIcdIref=0;
- vpColVector sIrefdIref(nbParam);sIrefdIref=0;
-
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
- else
- Ic=BI.getValue(i2,j2);
-
-
- double prod=(Ic-moyIc);
- for(unsigned int it=0;it<nbParam;it++)
- sIcdIref[it]+=prod*(ptTemplate[point].dW[it]-moydIrefdp[it]);
- for(unsigned int it=0;it<nbParam;it++)
- sIrefdIref[it]+=(Iref-moyIref)*(ptTemplate[point].dW[it]-moydIrefdp[it]);
-
- double er=(Iref-Ic);
- erreur+=(er*er);
- //denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
- covarIref+=(Iref-moyIref)*(Iref-moyIref);
- covarIc+=(Ic-moyIc)*(Ic-moyIc);
- sIcIref+=(Iref-moyIref)*(Ic-moyIc);
- }
-
-
- }
- covarIref=sqrt(covarIref);
- covarIc=sqrt(covarIc);
- denom=covarIref*covarIc;
-
- //if(denom==0.0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
- diverge=true;
- }
- else
- {
- double NCC=sIcIref/denom;
- vpColVector dcovarIref(nbParam);dcovarIref=sIrefdIref/covarIref;
- G=1.*(sIcdIref/denom-NCC*dcovarIref/covarIref);
-
-
- try
- {
- dp=-1.*HLMdesireInverse*G;
- }
- catch(...)
- {
- std::cout<<"probleme inversion"<<std::endl;
- break;
- }
-
- Warp->getParamInverse(dp,dpinv);
- Warp->pRondp(p,dpinv,p);
-
- computeEvalRMS(p);
- }
- }
- else
- diverge=true;
-
- iteration++;
- }
- while( (!diverge &&(evolRMS>threshold_RMS) && (iteration < iterationMax)));
-
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
-
- deletePosEvalRMS();
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::initPosEvalRMS(vpColVector &p_)
-{
- unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
- x_pos.resize(nb_corners);
- y_pos.resize(nb_corners);
-
- Warp->computeCoeff(p);
- vpTemplateTrackerTriangle triangle;
-
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
- zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
- triangle.getCorner(j, X1[0], X1[1]);
-
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
- }
- }
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::computeEvalRMS(const vpColVector &p_)
-{
- unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
-
- Warp->computeCoeff(p_);
- evolRMS=0;
- vpTemplateTrackerTriangle triangle;
-
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
- zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
- triangle.getCorner(j, X1[0], X1[1]);
-
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
- }
- }
- evolRMS=evolRMS/nb_corners;
-
-}
-
-void vpTemplateTrackerZNCCInverseCompositional::deletePosEvalRMS()
-{
-}
diff --git a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.h b/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.h
deleted file mode 100644
index 814b237..0000000
--- a/src/tracking/template-tracker/zncc/vpTemplateTrackerZNCCInverseCompositional.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpTemplateTrackerZNCCInverseCompositional.h 4669 2014-02-16 16:25:57Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Template tracker.
- *
- * Authors:
- * Amaury Dame
- * Aurelien Yol
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \file vpTemplateTrackerZNCCInverseCompositional.h
- \brief
-*/
-
-#ifndef vpTemplateTrackerZNCCInverseCompositional_hh
-#define vpTemplateTrackerZNCCInverseCompositional_hh
-
-#include <vector>
-
-#include <visp/vpTemplateTrackerZNCC.h>
-
-/*!
- The algorithm implemented in this class is described in \cite Irani98a.
- */
-class VISP_EXPORT vpTemplateTrackerZNCCInverseCompositional: public vpTemplateTrackerZNCC
-{
- protected:
- bool compoInitialised;
- //pour eval evolRMS
- double evolRMS;
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- double threshold_RMS;
- vpColVector moydIrefdp;
-
- protected:
- void initCompInverse(const vpImage<unsigned char> &I);
- void initHessienDesired(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
- void deletePosEvalRMS();
- void computeEvalRMS(const vpColVector &p);
- void initPosEvalRMS(vpColVector &p);
-
- public:
- vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp);
-
- void setThresholdRMS(double threshold){threshold_RMS=threshold;}
-};
-#endif
-
diff --git a/src/video/vpFFMPEG.cpp b/src/video/vpFFMPEG.cpp
deleted file mode 100644
index 238238a..0000000
--- a/src/video/vpFFMPEG.cpp
+++ /dev/null
@@ -1,935 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class that manages the FFMPEG library.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpFFMPEG.cpp
- \brief Class that manages the FFMPEG library
-*/
-
-#include <stdio.h>
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFFMPEG.h>
-#include <visp/vpImageConvert.h>
-
-#ifdef VISP_HAVE_FFMPEG
-
-extern "C"
-{
-//#include <libavcodec/avcodec.h>
-#include <libavformat/avformat.h>
-#include <libswscale/swscale.h>
-}
-
-/*!
- Basic constructor.
-*/
-vpFFMPEG::vpFFMPEG()
- : width(-1), height(-1), frameNumber(0), pFormatCtx(NULL), pCodecCtx(NULL),
- pCodec(NULL), pFrame(NULL), pFrameRGB(NULL), pFrameGRAY(NULL), packet(NULL),
- img_convert_ctx(NULL), videoStream(0), numBytes(0), buffer(NULL), index(),
- streamWasOpen(false), streamWasInitialized(false), color_type(COLORED),
- f(NULL), outbuf(NULL), picture_buf(NULL), outbuf_size(0), out_size(0),
- bit_rate(500000), encoderWasOpened(false),
- framerate_stream(-1), framerate_encoder(25)
-{
- packet = new AVPacket;
-}
-
-/*!
- Basic destructor.
-*/
-vpFFMPEG::~vpFFMPEG()
-{
- closeStream();
- delete packet;
-}
-
-/*!
- Allocates and initializes the parameters depending on the video and the desired color type.
- One the stream is opened, it is possible to get the video encoding framerate getFramerate(),
- and the dimension of the images using getWidth() and getHeight().
-
- \param filename : Path to the video which has to be read.
- \param colortype : Desired color map used to open the video.
- The parameter can take two values : COLORED and GRAY_SCALED.
-
- \return It returns true if the paramters could be initialized. Else it returns false.
-*/
-bool vpFFMPEG::openStream(const char *filename, vpFFMPEGColorType colortype)
-{
- this->color_type = colortype;
-
- av_register_all();
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,0,0) // libavformat 52.84.0
- if (av_open_input_file (&pFormatCtx, filename, NULL, 0, NULL) != 0)
-#else
- if (avformat_open_input (&pFormatCtx, filename, NULL, NULL) != 0) // libavformat 53.4.0
-#endif
- {
- vpTRACE("Couldn't open file ");
- return false;
- }
-
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,21,0) // libavformat 53.21.0
- if (av_find_stream_info (pFormatCtx) < 0)
-#else
- if (avformat_find_stream_info (pFormatCtx, NULL) < 0)
-#endif
- return false;
-
- videoStream = 0;
- bool found_codec = false;
-
- /*
- * Detect streams types
- */
- for (unsigned int i = 0; i < pFormatCtx->nb_streams; i++)
- {
-#if LIBAVUTIL_VERSION_INT < AV_VERSION_INT(51,0,0)
- if(pFormatCtx->streams[i]->codec->codec_type==CODEC_TYPE_VIDEO) // avutil 50.33.0
-#else
- if(pFormatCtx->streams[i]->codec->codec_type==AVMEDIA_TYPE_VIDEO) // avutil 51.9.1
-#endif
- {
- videoStream = i;
- //std::cout << "rate: " << pFormatCtx->streams[i]->r_frame_rate.num << " " << pFormatCtx->streams[i]->r_frame_rate.den << std::endl;
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(55,12,0)
- framerate_stream = pFormatCtx->streams[i]->r_frame_rate.num;
- framerate_stream /= pFormatCtx->streams[i]->r_frame_rate.den;
-#else
- framerate_stream = pFormatCtx->streams[i]->avg_frame_rate.num;
- framerate_stream /= pFormatCtx->streams[i]->avg_frame_rate.den;
-#endif
- found_codec= true;
- break;
- }
- }
-
- if (found_codec)
- {
- pCodecCtx = pFormatCtx->streams[videoStream]->codec;
- pCodec = avcodec_find_decoder(pCodecCtx->codec_id);
-
- if (pCodec == NULL)
- {
- vpTRACE("unsuported codec");
- return false; // Codec not found
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
- if (avcodec_open (pCodecCtx, pCodec) < 0)
-#else
- if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0)
-#endif
- {
- vpTRACE("Could not open codec");
- return false; // Could not open codec
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrame = avcodec_alloc_frame();
-#else
- pFrame = av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (color_type == vpFFMPEG::COLORED)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrameRGB=avcodec_alloc_frame();
-#else
- pFrameRGB=av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (pFrameRGB == NULL)
- return false;
-
- numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
- }
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrameGRAY=avcodec_alloc_frame();
-#else
- pFrameGRAY=av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (pFrameGRAY == NULL)
- return false;
-
- numBytes = avpicture_get_size (PIX_FMT_GRAY8,pCodecCtx->width,pCodecCtx->height);
- }
-
- /*
- * Determine required buffer size and allocate buffer
- */
- width = pCodecCtx->width ;
- height = pCodecCtx->height ;
- buffer = (uint8_t *) malloc ((unsigned int)(sizeof (uint8_t)) * (unsigned int)numBytes);
- }
- else
- {
- vpTRACE("Didn't find a video stream");
- return false;
- }
-
- if (color_type == vpFFMPEG::COLORED)
- avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- avpicture_fill((AVPicture *)pFrameGRAY, buffer, PIX_FMT_GRAY8, pCodecCtx->width, pCodecCtx->height);
-
- streamWasOpen = true;
-
- return true;
-}
-
-/*!
- This method initializes the conversion parameters.
-
- It browses the video and lists all the frame. It sets the number of frame in the video.
-
- \returns It returns true if the method was executed without any problem. Else it returns false.
-*/
-bool vpFFMPEG::initStream()
-{
- if (color_type == vpFFMPEG::COLORED)
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_RGB24, SWS_BICUBIC, NULL, NULL, NULL);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_GRAY8, SWS_BICUBIC, NULL, NULL, NULL);
-
- int ret = av_seek_frame(pFormatCtx, (int)videoStream, 0, AVSEEK_FLAG_ANY) ;
- if (ret < 0 )
- {
- vpTRACE("Error rewinding stream for full indexing") ;
- return false ;
- }
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frame_no = 0 ;
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- ret = avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- ret = avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (ret < 0 )
- {
- vpTRACE("Unable to decode video picture");
- }
- index.push_back(packet->dts);
- frame_no++ ;
- }
- }
- }
-
- frameNumber = index.size();
- av_free_packet(packet);
-
- streamWasInitialized = true;
-
- return true;
-}
-
-
-/*!
- Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
-
- \param I : The vpImage used to stored the video's frame.
- \param frame : The index of the frame which has to be read.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::getFrame(vpImage<vpRGBa> &I, unsigned int frame)
-{
-
- if (frame < frameNumber && streamWasInitialized== true)
- {
- int64_t targetPts = index[frame];
- av_seek_frame(pFormatCtx, (int)videoStream,targetPts, AVSEEK_FLAG_ANY);
- }
- else
- {
- vpTRACE("Couldn't get a frame");
- return false;
- }
-
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
-
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- Gets the Next frame in the video.
-
- \param I : The vpImage used to stored the video's frame.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::acquire(vpImage<vpRGBa> &I)
-{
- int frameFinished ;
-
- if (streamWasInitialized == false)
- {
- vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
- return false;
- }
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
- av_free_packet(packet);
- return true;
-}
-
-/*!
- Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
-
- \param I : The vpImage used to stored the video's frame.
- \param frame : The index of the frame which has to be read.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::getFrame(vpImage<unsigned char> &I, unsigned int frame)
-{
-
- if (frame < frameNumber && streamWasInitialized== true)
- {
- int64_t targetPts = index[frame];
- av_seek_frame(pFormatCtx,(int)videoStream,targetPts, AVSEEK_FLAG_ANY);
- }
- else
- {
- vpTRACE("Couldn't get a frame");
- return false;
- }
-
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
-
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- Gets the Next frame in the video.
-
- \param I : The vpImage used to stored the video's frame.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::acquire(vpImage<unsigned char> &I)
-{
- int frameFinished ;
-
- if (streamWasInitialized == false)
- {
- vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
- return false;
- }
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- This method enable to fill the vpImage bitmap thanks to the selected frame.
-
- \throw vpException::dimensionError if either the height or the width
- associated to the class is negative.
-
- \param I : the image to fill.
-*/
-void vpFFMPEG::copyBitmap(vpImage<vpRGBa> &I)
-{
- if(height < 0 || width < 0){
- throw vpException(vpException::dimensionError, "width or height negative.");
- }
- I.resize((unsigned int)height, (unsigned int)width);
-
- unsigned char* line;
- unsigned char* beginOutput = (unsigned char*)I.bitmap;
- unsigned char* output = NULL;
-
- if (color_type == COLORED)
- {
- unsigned char* input = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line);
- *(output++) = *(line+1);
- *(output++) = *(line+2);
- *(output++) = 0;
-
- line+=3;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-
- else if (color_type == GRAY_SCALED)
- {
- unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
- int widthStep = pFrameGRAY->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);;
-
- line++;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-}
-
-/*!
- This method enable to fill the vpImage bitmap thanks to the selected frame.
-
- \throw vpException::dimensionError if either the height or the width
- associated to the class is negative.
-
- \param I : the image to fill.
-*/
-void vpFFMPEG::copyBitmap(vpImage<unsigned char> &I)
-{
- if(height < 0 || width < 0){
- throw vpException(vpException::dimensionError, "width or height negative.");
- }
- I.resize((unsigned int)height, (unsigned int)width);
-
- unsigned char* line;
- unsigned char* beginOutput = (unsigned char*)I.bitmap;
- unsigned char* output = NULL;
-
- if (color_type == GRAY_SCALED)
- {
- unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
- int widthStep = pFrameGRAY->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + width * i;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line);
-
- line++;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-
- if (color_type == COLORED)
- {
- unsigned char* input = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
- for (int i = 0 ; i < height ; i++)
- {
- vpImageConvert::RGBToGrey(input + i*widthStep, beginOutput + i*width, (unsigned int)width, 1, false);
- }
- }
-}
-
-/*!
- Deallocates all the FFMPEG parameters.
-*/
-void vpFFMPEG::closeStream()
-{
- if (streamWasOpen)
- {
- if (buffer != NULL) {
- free(buffer);
- buffer = NULL;
- }
-
- if (color_type == vpFFMPEG::COLORED)
- av_free(pFrameRGB);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- av_free(pFrameGRAY);
-
- // Free the YUV frame
- av_free(pFrame);
-
- // Close the codec
- if (pCodecCtx) avcodec_close(pCodecCtx);
-
- // Close the video file
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,17,0) // libavformat 53.17.0
- av_close_input_file(pFormatCtx);
-#else
- avformat_close_input(&pFormatCtx);
-#endif
- }
- streamWasOpen = false;
-
- if (encoderWasOpened)
- {
- if(f!=NULL) endWrite();
-
- if(buffer!=NULL) delete[] buffer;
-
- if(outbuf != NULL) delete[] outbuf;
-
- if(picture_buf != NULL) delete[] picture_buf;
-
- av_free(pFrameRGB);
- av_free(pFrame);
- if (pCodecCtx) avcodec_close(pCodecCtx);
- }
-
- encoderWasOpened = false;
-
- if(streamWasInitialized || encoderWasOpened){
- sws_freeContext (img_convert_ctx);
- }
- streamWasInitialized = false;
-}
-
-/*!
- Allocates and initializes the parameters depending on the video to write.
-
- \param filename : Path to the video which has to be writen.
- \param w,h : Width and height of the image which will be saved.
- \param codec : Type of codec used to encode the video.
-
- By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
- AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
- AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
-
- Of course to use the codec it must be installed on your computer.
-
- \return It returns true if the paramters could be initialized. Else it returns false.
-*/
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
-bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, CodecID codec)
-#else
-bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, AVCodecID codec)
-#endif
-{
- av_register_all();
-
- /* find the mpeg1 video encoder */
- pCodec = avcodec_find_encoder(codec);
- if (pCodec == NULL) {
- fprintf(stderr, "codec not found\n");
- return false;
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,5,0) // libavcodec 53.5.0
- pCodecCtx = avcodec_alloc_context();
-#else
- pCodecCtx = avcodec_alloc_context3(NULL);
-#endif
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrame = avcodec_alloc_frame();
- pFrameRGB = avcodec_alloc_frame();
-#else
- pFrame = av_frame_alloc(); // libavcodec 55.34.1
- pFrameRGB = av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- /* put sample parameters */
- pCodecCtx->bit_rate = (int)bit_rate;
- /* resolution must be a multiple of two */
- pCodecCtx->width = (int)w;
- pCodecCtx->height = (int)h;
- this->width = (int)w;
- this->height = (int)h;
- /* frames per second */
- pCodecCtx->time_base.num = 1;
- pCodecCtx->time_base.den = framerate_encoder;
- pCodecCtx->gop_size = 10; /* emit one intra frame every ten frames */
- pCodecCtx->max_b_frames=1;
- pCodecCtx->pix_fmt = PIX_FMT_YUV420P;
-
- /* open it */
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
- if (avcodec_open (pCodecCtx, pCodec) < 0) {
-#else
- if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0) {
-#endif
- fprintf(stderr, "could not open codec\n");
- return false;
- }
-
- /* the codec gives us the frame size, in samples */
-
- f = fopen(filename, "wb");
- if (!f) {
- fprintf(stderr, "could not open %s\n", filename);
- return false;
- }
-
- outbuf_size = 100000;
- outbuf = new uint8_t[outbuf_size];
-
- numBytes = avpicture_get_size (PIX_FMT_YUV420P,pCodecCtx->width,pCodecCtx->height);
- picture_buf = new uint8_t[numBytes];
- avpicture_fill((AVPicture *)pFrame, picture_buf, PIX_FMT_YUV420P, pCodecCtx->width, pCodecCtx->height);
-
- numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
- buffer = new uint8_t[numBytes];
- avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
-
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, PIX_FMT_RGB24, pCodecCtx->width,pCodecCtx->height,PIX_FMT_YUV420P, SWS_BICUBIC, NULL, NULL, NULL);
-
- encoderWasOpened = true;
-
- return true;
-}
-
-
-/*!
- Saves the image I as frame of the video.
-
- \param I : the image to save.
-
- \return It returns true if the image could be saved.
-*/
-bool vpFFMPEG::saveFrame(vpImage<vpRGBa> &I)
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- writeBitmap(I);
- sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
-
- int got_output;
- int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
- fflush(stdout);
- return true;
-}
-
-
-/*!
- Saves the image I as frame of the video.
-
- \param I : the image to save.
-
- \return It returns true if the image could be saved.
-*/
-bool vpFFMPEG::saveFrame(vpImage<unsigned char> &I)
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- writeBitmap(I);
- sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
-
- int got_output;
- int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
-
- fflush(stdout);
- return true;
-}
-
-/*!
- Ends the writing of the video and close the file.
-
- \return It returns true if the file was closed without problem
-*/
-bool vpFFMPEG::endWrite()
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- int ret = 1;
- while (ret != 0)
- {
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- ret = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, NULL);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
- int got_output;
- ret = avcodec_encode_video2(pCodecCtx, &pkt, NULL, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
- }
-
- /*The end of a mpeg file*/
- outbuf[0] = 0x00;
- outbuf[1] = 0x00;
- outbuf[2] = 0x01;
- outbuf[3] = 0xb7;
- fwrite(outbuf, 1, 4, f);
- fclose(f);
- f = NULL;
- return true;
-}
-
-/*!
- This method enables to fill the frame bitmap thanks to the vpImage bitmap.
-*/
-void vpFFMPEG::writeBitmap(vpImage<vpRGBa> &I)
-{
- unsigned char* beginInput = (unsigned char*)I.bitmap;
- unsigned char* input = NULL;
- unsigned char* output = NULL;
- unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
-
- for(int i=0 ; i < height ; i++)
- {
- input = beginInput + 4 * i * width;
- output = beginOutput + i * widthStep;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(input);
- *(output++) = *(input+1);
- *(output++) = *(input+2);
-
- input+=4;
- }
- }
-}
-
-
-/*!
- This method enables to fill the frame bitmap thanks to the vpImage bitmap.
-*/
-void vpFFMPEG::writeBitmap(vpImage<unsigned char> &I)
-{
- unsigned char* beginInput = (unsigned char*)I.bitmap;
- unsigned char* input = NULL;
- unsigned char* output = NULL;
- unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
-
- for(int i=0 ; i < height ; i++)
- {
- input = beginInput + i * width;
- output = beginOutput + i * widthStep;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(input);
- *(output++) = *(input);
- *(output++) = *(input);
-
- input++;
- }
- }
-}
-
-#endif
diff --git a/src/video/vpFFMPEG.h b/src/video/vpFFMPEG.h
deleted file mode 100644
index 85b0986..0000000
--- a/src/video/vpFFMPEG.h
+++ /dev/null
@@ -1,272 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Class that manages the FFMPEG library.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpFFMPEG.h
- \brief Class that manages the FFMPEG library
-*/
-
-#ifndef vpFFMPEG_H
-#define vpFFMPEG_H
-
-#include <visp/vpImageIo.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <vector>
-
-#ifdef VISP_HAVE_FFMPEG
-
-// Fix for the following compilation error:
-// libavutil/common.h:154: error: UINT64_C was not declared in this scope
-// libavutil/common.h is no more autosufficient for C++ program because
-// stdint.h defines UINT64_C only for C program and not C++ program
-#ifdef __cplusplus
-# ifndef __STDC_CONSTANT_MACROS
-# define __STDC_CONSTANT_MACROS
-# endif
-# ifdef _STDINT_H
-# undef _STDINT_H
-# endif
-// On OS X
-# ifdef _STDINT_H_
-# undef _STDINT_H_
-# endif
-# include <stdint.h>
-
-# ifndef INT64_C
-# define INT64_C(c) (c ## LL)
-# endif
-# ifndef UINT64_C
-# define UINT64_C(c) (c ## ULL)
-# endif
-#endif
-// end fix
-
-
-extern "C"
-{
-#include <libavcodec/avcodec.h> // requested for CodecID enum
-//#include <avformat.h>
-//#include <swscale.h>
-}
-
-struct AVFormatContext;
-struct AVCodecContext;
-struct AVCodec;
-struct AVFrame;
-struct AVFrame;
-struct AVFrame;
-struct AVPacket;
-struct SwsContext;
-
-/*!
- \class vpFFMPEG
- \ingroup Video
-
- \brief This class interfaces the FFmpeg library to enable video stream reading or writing.
-
- Here an example which explains how to use the class to read a video stream.
- \code
-#include <visp/vpFFMPEG.h>
-
-int main ()
-{
-#ifdef VISP_HAVE_FFMPEG
- vpImage<vpRGBa> I; // The image to stores the frames
- vpFFMPEG ffmpeg;
- // Initialization
- ffmpeg.openStream("video.mpeg", vpFFMPEG::COLORED);
- ffmpeg.initStream();
- std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
- std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
- // Video reading
- int frameIndex = 0;
- ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
-#endif
-}
- \endcode
-
- If you want to open the video as a gray scaled video, you can use the following example.
- \code
-#include <visp/vpFFMPEG.h>
-
-int main ()
-{
-#ifdef VISP_HAVE_FFMPEG
- vpImage<unsigned char> I; // The image to stores the frames
- vpFFMPEG ffmpeg;
- // Initialization
- ffmpeg.openStream("video.mpeg", vpFFMPEG::GRAY_SCALED);
- ffmpeg.initStream();
- std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
- std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
- // Video reading
- int frameIndex = 0;
- ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
-#endif
-}
- \endcode
-*/
-class VISP_EXPORT vpFFMPEG
-{
- public:
- typedef enum
- {
- COLORED,
- GRAY_SCALED,
- }vpFFMPEGColorType;
-
- private:
- //! Video's height and width
- int width, height;
- //! Number of frame in the video.
- unsigned long frameNumber;
- //! FFMPEG variables
- AVFormatContext *pFormatCtx;
- AVCodecContext *pCodecCtx;
- AVCodec *pCodec;
- AVFrame *pFrame;
- AVFrame *pFrameRGB;
- AVFrame *pFrameGRAY;
- AVPacket *packet;
- SwsContext *img_convert_ctx ;
- unsigned int videoStream;
- int numBytes ;
- uint8_t * buffer ;
- std::vector<int64_t> index;
- //! Indicates if the openStream method was executed
- bool streamWasOpen;
- //! Indicates if the initStream method was executed
- bool streamWasInitialized;
- //! Indicates the video's color output.
- vpFFMPEGColorType color_type;
-
- //!The file which is the video file
- FILE *f;
- //!Buffers
- uint8_t *outbuf, *picture_buf;
- //!Buffer size
- int outbuf_size;
- //!Size of the data to write in the file
- int out_size;
- //!Bit rate of the video to write
- unsigned int bit_rate;
- //!Indicates if the openEncoder method was executed
- bool encoderWasOpened;
- double framerate_stream; // input stream
- int framerate_encoder; // output stream
-
- public:
- vpFFMPEG();
- ~vpFFMPEG();
-
- bool acquire(vpImage<vpRGBa> &I);
- bool acquire(vpImage<unsigned char> &I);
-
- void closeStream();
-
- bool endWrite();
-
- bool getFrame(vpImage<vpRGBa> &I, unsigned int frameNumber);
- bool getFrame(vpImage<unsigned char> &I, unsigned int frameNumber);
- /*!
- Gets the video's frame number.
-
- \return The value of the video's frame number.
- */
- inline unsigned long getFrameNumber() const {return frameNumber;}
- /*!
- Return the framerate used to encode the video. The video stream need
- to be opened before calling this function.
-
- \exception vpException::ioError : If the video stream was not opened priviously.
- */
- inline double getFramerate() const {
- if (streamWasOpen)
- return framerate_stream;
- throw vpException(vpException::ioError, "Video stream not opened.");
- }
- /*!
- Gets the video's height.
-
- \return The value of the video's height.
- */
- inline int getHeight() const {return height;}
- /*!
- Gets the video's width.
-
- \return The value of the video's width.
- */
- inline int getWidth() const {return width;}
-
- bool initStream();
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- bool openEncoder(const char *filename, unsigned int width, unsigned int height, CodecID codec = CODEC_ID_MPEG1VIDEO);
-#else
- bool openEncoder(const char *filename, unsigned int width, unsigned int height, AVCodecID codec = AV_CODEC_ID_MPEG1VIDEO);
-#endif
- bool openStream(const char *filename,vpFFMPEGColorType color_type);
-
- bool saveFrame(vpImage<vpRGBa> &I);
- bool saveFrame(vpImage<unsigned char> &I);
- /*!
- Sets the bit rate of the video when encoding.
-
- \param rate : the expected bit rate.
- */
- inline void setBitRate(const unsigned int rate) {this->bit_rate = rate;}
- /*!
- Sets the framerate of the video when encoding.
-
- \param framerate : the expected framerate.
- */
- inline void setFramerate(const int framerate) {framerate_encoder = framerate;}
-
- private:
- void copyBitmap(vpImage<vpRGBa> &I);
- void copyBitmap(vpImage<unsigned char> &I);
- void writeBitmap(vpImage<vpRGBa> &I);
- void writeBitmap(vpImage<unsigned char> &I);
-};
-#endif
-#endif
diff --git a/src/video/vpVideoReader.cpp b/src/video/vpVideoReader.cpp
deleted file mode 100644
index 11aef78..0000000
--- a/src/video/vpVideoReader.cpp
+++ /dev/null
@@ -1,765 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Read videos and image sequences.
-*
-* Authors:
-* Nicolas Melchior
-* Fabien Spindler
-*
-*****************************************************************************/
-
-/*!
-\file vpVideoReader.cpp
-\brief Read videos and image sequences
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpVideoReader.h>
-
-#include <iostream>
-#include <fstream>
-#include <limits> // numeric_limits
-
-/*!
-Basic constructor.
-*/
-vpVideoReader::vpVideoReader()
- : imSequence(NULL),
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg(NULL),
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture(), frame(),
-#endif
- formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
- firstFrame(0), lastFrame(0), firstFrameIndexIsSet(false), lastFrameIndexIsSet(false)
-{
-}
-
-/*!
-Basic destructor.
-*/
-vpVideoReader::~vpVideoReader()
-{
- if (imSequence != NULL)
- {
- delete imSequence;
- }
-#ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- delete ffmpeg;
- }
-#endif
-}
-
-
-/*!
-It enables to set the path and the name of the file(s) which as/have to be read.
-
-If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
-
-If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-
-\param filename : Path to a video file or file name template of a image sequence.
-*/
-void vpVideoReader::setFileName(const char *filename)
-{
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
- }
-
- if (strlen( filename ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the file name"));
- }
-
- strcpy(this->fileName,filename);
-
- formatType = getFormat(fileName);
-
- if (formatType == FORMAT_UNKNOWN) {
- throw(vpException(vpException::badValue, "Filename extension not supported"));
- }
-
- initFileName = true;
-}
-
-/*!
-It enables to set the path and the name of the file(s) which as/have to be read.
-
-If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
-
-If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-
-\param filename : Path to a video file or file name template of a image sequence.
-*/
-void vpVideoReader::setFileName(const std::string &filename)
-{
- setFileName(filename.c_str());
-}
-
-/*!
-Sets all the parameters needed to read the video or the image sequence.
-
-Grab the first frame and stores it in the image \f$ I \f$.
-
-\param I : The image where the frame is stored.
-*/
-void vpVideoReader::open(vpImage< vpRGBa > &I)
-{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (isImageExtensionSupported())
- {
- imSequence = new vpDiskGrabber;
- imSequence->setGenericName(fileName);
- if (firstFrameIndexIsSet)
- imSequence->setImageNumber(firstFrame);
- }
- else if (isVideoExtensionSupported())
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- if(!ffmpeg->openStream(fileName, vpFFMPEG::COLORED))
- throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
- ffmpeg->initStream();
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture.open(fileName);
-
- if(!capture.isOpened())
- {
- throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
- }
-#else
- //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
-#endif
- }
- else if (formatType == FORMAT_UNKNOWN)
- {
- //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
- throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
- }
-
- findFirstFrameIndex();
- frameCount = firstFrame;
- if(!getFrame(I, firstFrame))
- {
- //vpERROR_TRACE("Could not read the video first frame");
- throw (vpException(vpException::ioError ,"Could not read the video first frame"));
- }
-
- height = I.getHeight();
- width = I.getWidth();
-
- isOpen = true;
- findLastFrameIndex();
-}
-
-
-/*!
-Sets all the parameters needed to read the video or the image sequence.
-
-Grab the first frame and stores it in the image \f$ I \f$.
-
-\param I : The image where the frame is stored.
-*/
-void vpVideoReader::open(vpImage<unsigned char> &I)
-{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (isImageExtensionSupported())
- {
- imSequence = new vpDiskGrabber;
- imSequence->setGenericName(fileName);
- if (firstFrameIndexIsSet)
- imSequence->setImageNumber(firstFrame);
- }
- else if (isVideoExtensionSupported())
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- if (!ffmpeg->openStream(fileName, vpFFMPEG::GRAY_SCALED))
- throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
- ffmpeg->initStream();
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture.open(fileName);
-
- if(!capture.isOpened())
- {
- throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
- }
-#else
- //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv >= 2.1.0 3rd party libraries."));
-#endif
- }
- else if (formatType == FORMAT_UNKNOWN)
- {
- //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
- throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
- }
-
- findFirstFrameIndex();
- frameCount = firstFrame;
- if(!getFrame(I,firstFrame))
- {
- //vpERROR_TRACE("Could not read the video first frame");
- throw (vpException(vpException::ioError ,"Could not read the video first frame"));
- }
-
- height = I.getHeight();
- width = I.getWidth();
-
- isOpen = true;
- findLastFrameIndex();
-}
-
-
-/*!
-Grabs the current (k) image in the stack of frames and increments the frame counter
-in order to grab the next image (k+1) during the next use of the method. If open()
-was not called previously, this method opens the video reader.
-
-This method enables to use the class as frame grabber.
-
-\param I : The image where the frame is stored.
-*/
-void vpVideoReader::acquire(vpImage< vpRGBa > &I)
-{
- if (!isOpen) {
- open(I);
- }
-
- //getFrame(I,frameCount);
- if (imSequence != NULL)
- {
- imSequence->acquire(I);
- frameCount++; // next index
- }
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg !=NULL)
- {
- ffmpeg->acquire(I);
- frameCount++; // next index
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else
- {
- capture >> frame;
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
-#else
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
-#endif
-
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
- }
-#endif
-}
-
-
-/*!
-Grabs the kth image in the stack of frames and increments the frame counter in order to grab the next image (k+1) during the next use of the method.
-
-This method enables to use the class as frame grabber.
-
-\param I : The image where the frame is stored.
-*/
-void vpVideoReader::acquire(vpImage< unsigned char > &I)
-{
- if (!isOpen) {
- open(I);
- }
-
- if (imSequence != NULL)
- {
- imSequence->acquire(I);
- frameCount++; // next index
- }
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL)
- {
- ffmpeg->acquire(I);
- frameCount++; // next index
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else
- {
- capture >> frame;
-#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
-#else
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
-#endif
-
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
- }
-#endif
-}
-
-
-/*!
-Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
-
-\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
-But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
-one after one.
-
-\param I : The vpImage used to stored the frame.
-\param frame_index : The index of the frame which has to be read.
-
-\return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpVideoReader::getFrame(vpImage<vpRGBa> &I, long frame_index)
-{
- if (imSequence != NULL)
- {
- try
- {
- imSequence->acquire(I, frame_index);
- frameCount = frame_index + 1; // next index
- }
- catch(...)
- {
- vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
- return false;
- }
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
- return false;
- }
- frameCount = frame_index + 1; // next index
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
- return false;
- }
-
- capture >> frame;
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
- return false;
- }
-
- capture >> frame;
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
-#endif
- }
-
- return true;
-}
-
-
-/*!
-Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
-
-\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
-But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
-one after one.
-
-\param I : The vpImage used to stored the frame.
-\param frame_index : The index of the frame which has to be read.
-
-\return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpVideoReader::getFrame(vpImage<unsigned char> &I, long frame_index)
-{
- if (imSequence != NULL)
- {
- try
- {
- imSequence->acquire(I, frame_index);
- frameCount = frame_index + 1;
- }
- catch(...)
- {
- vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
- return false;
- }
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
- return false;
- }
- frameCount = frame_index + 1; // next index
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
- return false;
- }
- capture >> frame;
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); // next index
- return false;
- }
- capture >> frame;
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES);
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
- else
- vpImageConvert::convert(frame, I);
-#endif
- }
-
- return true;
-}
-
-
-/*!
-Gets the format of the file(s) which has/have to be read.
-
-\return Returns the format.
-*/
-vpVideoReader::vpVideoFormatType
- vpVideoReader::getFormat(const char *filename)
-{
- std::string sfilename(filename);
-
- std::string ext = vpVideoReader::getExtension(sfilename);
-
- if (ext.compare(".PGM") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".pgm") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".PPM") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".ppm") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".JPG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".JPEG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpeg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".PNG") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".png") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".TIFF") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".tiff") == 0)
- return FORMAT_TIFF;
- else if (ext.compare(".BMP") == 0)
- return FORMAT_BMP;
- else if (ext.compare(".bmp") == 0)
- return FORMAT_BMP;
- else if (ext.compare(".DIB") == 0)
- return FORMAT_DIB;
- else if (ext.compare(".dib") == 0)
- return FORMAT_DIB;
- else if (ext.compare(".PBM") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".PBM") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".SR") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".sr") == 0)
- return FORMAT_PBM;
- else if (ext.compare(".RAS") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".ras") == 0)
- return FORMAT_RASTER;
- else if (ext.compare(".JP2") == 0)
- return FORMAT_JPEG2000;
- else if (ext.compare(".jp2") == 0)
- return FORMAT_JPEG2000;
- else if (ext.compare(".AVI") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".avi") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".MPEG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpeg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPEG4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mpeg4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MP4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mp4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MOV") == 0)
- return FORMAT_MOV;
- else if (ext.compare(".mov") == 0)
- return FORMAT_MOV;
- else if (ext.compare(".OGV") == 0)
- return FORMAT_OGV;
- else if (ext.compare(".ogv") == 0)
- return FORMAT_OGV;
- else if (ext.compare(".WMV") == 0)
- return FORMAT_WMV;
- else if (ext.compare(".wmv") == 0)
- return FORMAT_WMV;
- else if (ext.compare(".FLV") == 0)
- return FORMAT_FLV;
- else if (ext.compare(".flv") == 0)
- return FORMAT_FLV;
- else if (ext.compare(".MKV") == 0)
- return FORMAT_MKV;
- else if (ext.compare(".mkv") == 0)
- return FORMAT_MKV;
- else
- return FORMAT_UNKNOWN;
-}
-
-// return the extension of the file including the dot
-std::string vpVideoReader::getExtension(const std::string &filename)
-{
- // extract the extension
- size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
- return ext;
-}
-
-
-/*!
-Get the last frame index (update the lastFrame attribute).
-*/
-void
- vpVideoReader::findLastFrameIndex()
-{
- if (!isOpen)
- {
- vpERROR_TRACE("Use the open method before");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
- }
-
- if (imSequence != NULL)
- {
- if (! lastFrameIndexIsSet) {
- char name[FILENAME_MAX];
- int image_number = firstFrame;
- bool failed;
- do
- {
- std::fstream file;
- sprintf(name,fileName,image_number) ;
- file.open(name, std::ios::in);
- failed = file.fail();
- if (!failed) {
- file.close();
- image_number++;
- }
- }while(!failed);
-
- lastFrame = image_number;
- }
- }
-
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL) {
- if (! lastFrameIndexIsSet) {
- lastFrame = (long)(ffmpeg->getFrameNumber());
- }
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- else if (! lastFrameIndexIsSet)
- {
- lastFrame = (long) capture.get(cv::CAP_PROP_FRAME_COUNT);
- if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 3.0.0
- {
- //std::cout << "Warning: Problem with cv::CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
- lastFrame = 100000; // Set lastFrame to an arbitrary value
- }
- lastFrame--; //Last frame index = total frame count - 1
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else if (! lastFrameIndexIsSet)
- {
- lastFrame = (long) capture.get(CV_CAP_PROP_FRAME_COUNT);
- if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 2.4.10
- {
- //std::cout << "Warning: Problem with CV_CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
- lastFrame = 100000; // Set lastFrame to an arbitrary value
- }
- lastFrame--; //Last frame index = total frame count - 1
- }
-#endif
-}
-/*!
-Get the first frame index (update the firstFrame attribute).
-*/
-void
- vpVideoReader::findFirstFrameIndex()
-{
- if (imSequence != NULL)
- {
- if (! firstFrameIndexIsSet) {
- char name[FILENAME_MAX];
- int image_number = 0;
- bool failed;
- do {
- std::fstream file;
- sprintf(name, fileName, image_number) ;
- file.open(name, std::ios::in);
- failed = file.fail();
- if (!failed) file.close();
- image_number++;
- } while(failed);
-
- firstFrame = image_number - 1;
- imSequence->setImageNumber(firstFrame);
- }
- }
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL) {
- if (! firstFrameIndexIsSet) {
- firstFrame = (long)(0);
- }
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else if (! firstFrameIndexIsSet)
- {
- firstFrame = (long) (0);
- }
-#endif
-}
-
-/*!
-Return the framerate in Hz used to encode the video stream.
-
-If the video is a sequence of images, return -1.
-*/
-double vpVideoReader::getFramerate()
-{
- double framerate = -1.;
-
-#ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- framerate = ffmpeg->getFramerate();
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- framerate = capture.get(cv::CAP_PROP_FPS);
- // if(framerate == 0)
- if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("Problem with cv::CAP_PROP_FPS") ;
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- framerate = capture.get(CV_CAP_PROP_FPS);
- // if(framerate == 0)
- if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("Problem with CV_CAP_PROP_FPS") ;
- }
-#endif
- return framerate;
-}
-
-/*!
-Return true if the image file extension is supported, false otherwise.
-*/
-bool vpVideoReader::isImageExtensionSupported()
-{
- return (formatType == FORMAT_PGM ||
- formatType == FORMAT_PPM ||
- formatType == FORMAT_JPEG ||
- formatType == FORMAT_PNG ||
- formatType == FORMAT_TIFF ||
- formatType == FORMAT_BMP ||
- formatType == FORMAT_DIB ||
- formatType == FORMAT_PBM ||
- formatType == FORMAT_RASTER ||
- formatType == FORMAT_JPEG2000);
-}
-
-/*!
-Return true if the video file extension is supported, false otherwise.
-*/
-bool vpVideoReader::isVideoExtensionSupported()
-{
- return (formatType == FORMAT_AVI ||
- formatType == FORMAT_MPEG ||
- formatType == FORMAT_MPEG4 ||
- formatType == FORMAT_MOV ||
- formatType == FORMAT_OGV ||
- formatType == FORMAT_WMV ||
- formatType == FORMAT_FLV ||
- formatType == FORMAT_MKV);
-}
diff --git a/src/video/vpVideoReader.h b/src/video/vpVideoReader.h
deleted file mode 100644
index 7bd511d..0000000
--- a/src/video/vpVideoReader.h
+++ /dev/null
@@ -1,307 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read videos and sequences of images .
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpVideoReader.h
- \brief Read videos and image sequences
-*/
-
-#ifndef vpVideoReader_H
-#define vpVideoReader_H
-
-#include <string>
-
-#include <visp/vpDiskGrabber.h>
-#include <visp/vpFFMPEG.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020200
-#include "opencv2/highgui/highgui.hpp"
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020000
-#include "opencv/highgui.h"
-#endif
-
-/*!
- \class vpVideoReader
-
- \ingroup Video
-
- \brief Class that enables to manipulate easily a video file or a sequence of
- images. As it inherits from the vpFrameGrabber Class, it can be used like an
- other frame grabber class.
-
- The following example available in tutorial-video-reader.cpp shows how this
- class is really easy to use. It enables to read a video file named video.mpeg.
- \include tutorial-video-reader.cpp
-
- As shown in the next example, this class allows also to access to a specific
- frame. But be careful, for video files, the getFrame() method is not precise
- and returns the nearest intra key frame from the expected frame. You can use
- the getFrame() method to position the reader in the video and then use the
- acquire() method to get the following frames one by one.
- \code
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpVideoReader.h>
-
-int main()
-{
-#ifdef VISP_HAVE_FFMPEG
- vpImage<vpRGBa> I;
-
- vpVideoReader reader;
-
- // Initialize the reader.
- reader.setFileName("video.mpeg");
- reader.open(I);
-
- // Read the nearest key frame from the 3th frame
- reader.getFrame(I,2);
-
- // After positionning the video reader use acquire to read the video frame by frame
- reader.acquire(I);
-
- return 0;
-#endif
-}
- \endcode
-
- The other following example explains how to use the class to read a
- sequence of images. The images are stored in the folder "./image" and are
- named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ... As explained
- in setFirstFrameIndex() and setLastFrameIndex() it is also possible to set the
- first and last image numbers to read a portion of the sequence. If these two
- functions are not used, first and last image numbers are set automatically to
- match the first and image images of the sequence.
-
- \code
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpVideoReader.h>
-
-int main()
-{
- vpImage<vpRGBa> I;
-
- vpVideoReader reader;
-
- // Initialize the reader.
- reader.setFileName("./image/image%04d.jpeg");
- reader.setFirstFrameIndex(10);
- reader.setLastFrameIndex(20);
- reader.open(I);
-
- while (! reader.end() )
- reader.acquire(I);
-
- return 0;
-}
- \endcode
-
- Note that it is also possible to access to a specific frame using getFrame().
- \code
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpVideoReader.h>
-
-int main()
-{
- vpImage<vpRGBa> I;
-
- vpVideoReader reader;
-
- // Initialize the reader.
- reader.setFileName("./image/image%04d.jpeg");
- reader.open(I);
-
- // Read the 3th frame
- reader.getFrame(I,2);
-
- return 0;
-}
- \endcode
-*/
-
-class VISP_EXPORT vpVideoReader : public vpFrameGrabber
-{
- private:
- //!To read sequences of images
- vpDiskGrabber *imSequence;
-#ifdef VISP_HAVE_FFMPEG
- //!To read video files
- vpFFMPEG *ffmpeg;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- //!To read video files with OpenCV
- cv::VideoCapture capture;
- cv::Mat frame;
-#endif
- //!Types of available formats
- typedef enum
- {
- FORMAT_PGM,
- FORMAT_PPM,
- FORMAT_JPEG,
- FORMAT_PNG,
- // Formats supported by opencv
- FORMAT_TIFF,
- FORMAT_BMP,
- FORMAT_DIB,
- FORMAT_PBM,
- FORMAT_RASTER,
- FORMAT_JPEG2000,
- // Video format
- FORMAT_AVI,
- FORMAT_MPEG,
- FORMAT_MPEG4,
- FORMAT_MOV,
- FORMAT_OGV,
- FORMAT_WMV,
- FORMAT_FLV,
- FORMAT_MKV,
- FORMAT_UNKNOWN
- } vpVideoFormatType;
-
- //!Video's format which has to be read
- vpVideoFormatType formatType;
-
- //!Path to the video
- char fileName[FILENAME_MAX];
- //!Indicates if the path to the video is set.
- bool initFileName;
- //!Indicates if the video is "open".
- bool isOpen;
- //!Count the frame number when the class is used as a grabber.
- long frameCount; // Index of the next image
- //!The first frame index
- long firstFrame;
- //!The last frame index
- long lastFrame;
- bool firstFrameIndexIsSet;
- bool lastFrameIndexIsSet;
-
- public:
- vpVideoReader();
- ~vpVideoReader();
-
- void acquire(vpImage< vpRGBa > &I);
- void acquire(vpImage< unsigned char > &I);
- void close(){;}
-
- /*!
- \return true if the end of the sequence is reached.
- */
- inline bool end() {
- if (frameCount > lastFrame )
- return true;
- return false;
- }
- bool getFrame(vpImage<vpRGBa> &I, long frame);
- bool getFrame(vpImage<unsigned char> &I, long frame);
- double getFramerate();
-
- /*!
- Get the frame index of the next image. This index is updated at each call of the
- acquire method. It can be used to detect the end of a file (comparison
- with getLastFrameIndex()).
-
- \return Returns the current frame index.
- */
- inline long getFrameIndex() const { return frameCount;}
-
- /*!
- Gets the first frame index.
-
- \return Returns the first frame index.
- */
- inline long getFirstFrameIndex() const {return firstFrame;}
- /*!
- Gets the last frame index.
-
- \return Returns the last frame index.
- */
- inline long getLastFrameIndex() const {return lastFrame;}
- void open (vpImage< vpRGBa > &I);
- void open (vpImage< unsigned char > &I);
-
- /*!
- Reset the frame counter and sets it to the first image index.
-
- By default the first frame index is set to 0.
-
- This method is useful if you use the class like a frame grabber (ie with theacquire method).
- */
- inline void resetFrameCounter() {frameCount = firstFrame;}
- void setFileName(const char *filename);
- void setFileName(const std::string &filename);
- /*!
- Enables to set the first frame index if you want to use the class like a grabber (ie with the
- acquire method).
-
- \param first_frame : The first frame index.
-
- \sa setLastFrameIndex()
- */
- inline void setFirstFrameIndex(const long first_frame) {
- this->firstFrameIndexIsSet = true;
- this->firstFrame = first_frame;
- }
- /*!
- Enables to set the last frame index.
-
- \param last_frame : The last frame index.
-
- \sa setFirstFrameIndex()
- */
- inline void setLastFrameIndex(const long last_frame) {
- this->lastFrameIndexIsSet = true;
- this->lastFrame = last_frame;
- }
-
- private:
- vpVideoFormatType getFormat(const char *filename);
- static std::string getExtension(const std::string &filename);
- void findFirstFrameIndex();
- void findLastFrameIndex();
- bool isImageExtensionSupported();
- bool isVideoExtensionSupported();
-};
-
-#endif
diff --git a/src/video/vpVideoWriter.cpp b/src/video/vpVideoWriter.cpp
deleted file mode 100755
index 82720a1..0000000
--- a/src/video/vpVideoWriter.cpp
+++ /dev/null
@@ -1,435 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Write image sequences.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpVideoWriter.cpp
- \brief Write image sequences.
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpVideoWriter.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020200
-# include <opencv2/imgproc/imgproc.hpp>
-#endif
-
-
-/*!
- Basic constructor.
-*/
-vpVideoWriter::vpVideoWriter() :
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg(NULL),
-# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- codec(CODEC_ID_MPEG1VIDEO),
-# else
- codec(AV_CODEC_ID_MPEG1VIDEO),
-# endif
- bit_rate(500000),
- framerate(25),
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- writer(), fourcc(0), framerate(0.),
-#endif
- formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
- firstFrame(0), width(0), height(0)
-{
- initFileName = false;
- firstFrame = 0;
- frameCount = 0;
- isOpen = false;
- width = height = 0;
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = NULL;
-# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- codec = CODEC_ID_MPEG1VIDEO;
-# else
- codec = AV_CODEC_ID_MPEG1VIDEO;
-# endif
- bit_rate = 500000;
- framerate = 25;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- framerate = 25.0;
- fourcc = cv::VideoWriter::fourcc('P','I','M','1');
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- framerate = 25.0;
- fourcc = CV_FOURCC('P','I','M','1'); // default is a MPEG-1 codec
-#endif
-}
-
-
-/*!
- Basic destructor.
-*/
-vpVideoWriter::~vpVideoWriter()
-{
- #ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- delete ffmpeg;
- #endif
-}
-
-
-/*!
- It enables to set the path and the name of the files which will be saved.
-
- If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-
- \param filename : filename template of an image sequence.
-*/
-void vpVideoWriter::setFileName(const char *filename)
-{
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
- }
-
- if (strlen( filename ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the file name"));
- }
-
- strcpy(this->fileName,filename);
-
- formatType = getFormat(fileName);
-
- if (formatType == FORMAT_UNKNOWN) {
- throw(vpException(vpException::badValue, "Filename extension not supported"));
- }
-
- initFileName = true;
-}
-
-/*!
- It enables to set the path and the name of the files which will be saved.
-
- If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-
- \param filename : filename template of an image sequence.
-*/
-void vpVideoWriter::setFileName(const std::string &filename)
-{
- setFileName(filename.c_str());
-}
-
-/*!
- Sets all the parameters needed to write the video or the image sequence.
-
- \param I : One image with the right dimensions.
-*/
-void vpVideoWriter::open(vpImage< vpRGBa > &I)
-{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (formatType == FORMAT_PGM ||
- formatType == FORMAT_PPM ||
- formatType == FORMAT_JPEG ||
- formatType == FORMAT_PNG)
- {
- width = I.getWidth();
- height = I.getHeight();
- }
- else if (formatType == FORMAT_AVI ||
- formatType == FORMAT_MPEG ||
- formatType == FORMAT_MPEG4 ||
- formatType == FORMAT_MOV)
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- ffmpeg->setFramerate(framerate);
- ffmpeg->setBitRate(bit_rate);
- if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
- throw (vpException(vpException::ioError ,"Could not open encode the video with ffmpeg"));
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- writer = cv::VideoWriter(fileName, fourcc, framerate, cv::Size((int)I.getWidth(), (int)I.getHeight()));
-
- if(!writer.isOpened())
- {
- //vpERROR_TRACE("Could not open encode the video with opencv");
- throw (vpException(vpException::fatalError , "Could not open encode the video with opencv"));
- }
-#else
- //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
-#endif
- }
-
- frameCount = firstFrame;
-
- isOpen = true;
-}
-
-
-/*!
- Sets all the parameters needed to write the video or the image sequence.
-
- \param I : One image with the right dimensions.
-*/
-void vpVideoWriter::open(vpImage< unsigned char > &I)
-{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (formatType == FORMAT_PGM ||
- formatType == FORMAT_PPM ||
- formatType == FORMAT_JPEG ||
- formatType == FORMAT_PNG)
- {
- width = I.getWidth();
- height = I.getHeight();
- }
- else if (formatType == FORMAT_AVI ||
- formatType == FORMAT_MPEG ||
- formatType == FORMAT_MPEG4 ||
- formatType == FORMAT_MOV)
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- ffmpeg->setFramerate(framerate);
- ffmpeg->setBitRate(bit_rate);
- if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
- throw (vpException(vpException::ioError ,"Could not encode the video with ffmpeg"));
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- writer = cv::VideoWriter(fileName, fourcc, framerate, cv::Size((int)I.getWidth(), (int)I.getHeight()));
-
- if(!writer.isOpened())
- {
- vpERROR_TRACE("Could not encode the video with opencv");
- throw (vpException(vpException::ioError ,"Could not encode the video with opencv"));
- }
-#else
- //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
-#endif
- }
-
- frameCount = firstFrame;
-
- isOpen = true;
-}
-
-
-/*!
- Saves the image as a frame of the video or as an image belonging to the image sequence.
-
- Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
-
- \param I : The image which has to be saved
-*/
-void vpVideoWriter::saveFrame (vpImage< vpRGBa > &I)
-{
- if (!isOpen)
- {
- vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
- }
-
-
- if (formatType == FORMAT_PGM ||
- formatType == FORMAT_PPM ||
- formatType == FORMAT_JPEG ||
- formatType == FORMAT_PNG)
- {
- char name[FILENAME_MAX];
-
- sprintf(name,fileName,frameCount);
-
- vpImageIo::write(I, name);
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg->saveFrame(I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- cv::Mat matFrame;
- vpImageConvert::convert(I, matFrame);
- writer << matFrame;
-#endif
- }
-
- frameCount++;
-}
-
-
-/*!
- Saves the image as a frame of the video or as an image belonging to the image sequence.
-
- Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
-
- \param I : The image which has to be saved
-*/
-void vpVideoWriter::saveFrame (vpImage< unsigned char > &I)
-{
- if (!isOpen)
- {
- vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
- }
-
- if (formatType == FORMAT_PGM ||
- formatType == FORMAT_PPM ||
- formatType == FORMAT_JPEG ||
- formatType == FORMAT_PNG)
- {
- char name[FILENAME_MAX];
-
- sprintf(name,fileName,frameCount);
-
- vpImageIo::write(I, name);
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg->saveFrame(I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- cv::Mat matFrame, rgbMatFrame;
- vpImageConvert::convert(I, matFrame);
- cv::cvtColor(matFrame, rgbMatFrame, cv::COLOR_GRAY2BGR);
- writer << rgbMatFrame;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- cv::Mat matFrame, rgbMatFrame;
- vpImageConvert::convert(I, matFrame);
- cv::cvtColor(matFrame, rgbMatFrame, CV_GRAY2BGR);
- writer << rgbMatFrame;
-#endif
- }
-
- frameCount++;
-}
-
-
-/*!
- Deallocates parameters use to write the video or the image sequence.
-*/
-void vpVideoWriter::close()
-{
- if (!isOpen)
- {
- vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
- }
- #ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- ffmpeg->endWrite();
- }
- #endif
-}
-
-
-/*!
- Gets the format of the file(s) which has/have to be written.
-
- \return Returns the format.
-*/
-vpVideoWriter::vpVideoFormatType
-vpVideoWriter::getFormat(const char *filename)
-{
- std::string sfilename(filename);
-
- std::string ext = vpVideoWriter::getExtension(sfilename);
-
- if (ext.compare(".PGM") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".pgm") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".PPM") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".ppm") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".JPG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".JPEG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpeg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".PNG") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".png") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".AVI") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".avi") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".MPEG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpeg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPEG4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mpeg4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MP4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mp4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MOV") == 0)
- return FORMAT_MOV;
- else if (ext.compare(".mov") == 0)
- return FORMAT_MOV;
- else
- return FORMAT_UNKNOWN;
-}
-
-// return the extension of the file including the dot
-std::string vpVideoWriter::getExtension(const std::string &filename)
-{
- // extract the extension
- size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
- return ext;
-}
diff --git a/src/video/vpVideoWriter.h b/src/video/vpVideoWriter.h
deleted file mode 100755
index bac0aa2..0000000
--- a/src/video/vpVideoWriter.h
+++ /dev/null
@@ -1,298 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpImagePoint.h 2359 2009-11-24 15:09:25Z nmelchio $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Write videos and sequences of images.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-/*!
- \file vpVideoWriter.h
- \brief Write videos and sequences of images.
-*/
-
-#ifndef vpVideoWriter_H
-#define vpVideoWriter_H
-
-#include <string>
-
-#include <visp/vpImageIo.h>
-#include <visp/vpFFMPEG.h>
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020200
-# include <opencv2/highgui/highgui.hpp>
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020000
-# include <opencv/highgui.h>
-#endif
-
-
-/*!
- \class vpVideoWriter
-
- \ingroup Video
-
- \brief Class that enables to write easily a video file or a sequence of images.
-
- The following example available in tutorial-video-recorder.cpp shows how this
- class can be used to record a video from a camera by default in an mpeg file.
- \include tutorial-video-recorder.cpp
-
- The following example shows also how this class can be used to write an image sequence.
- The images are stored in the folder "./image" and are named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ...
-
- \code
- #include <visp/vpConfig.h>
- #include <visp/vpVideoWriter.h>
-
- int main()
- {
- vpImage<vpRGBa> I;
-
- vpVideoWriter writer;
-
- //Initialize the writer.
- writer.setFileName("./image/image%04d.jpeg");
-
- writer.open(I);
-
- for ( ; ; )
- {
- //Here the code to capture or create an image and stores it in I.
-
- //Save the image
- writer.saveFrame(I);
- }
-
- writer.close();
-
- return 0;
- }
- \endcode
-
- The other following example explains how to use the class to write directly an mpeg file.
-
- \code
-#include <visp/vpVideoWriter.h>
-
-int main()
-{
- vpImage<vpRGBa> I;
-
- vpVideoWriter writer;
-
- // Set up the framerate to 30Hz. Default is 25Hz.
- writer.setFramerate(30);
-
-#ifdef VISP_HAVE_FFMPEG
- // Set up the bit rate
- writer.setBitRate(1000000);
- // Set up the codec to use
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- writer.setCodec(CODEC_ID_MPEG2VIDEO);
-#else
- writer.setCodec(AV_CODEC_ID_MPEG2VIDEO);
-#endif
-#elif defined VISP_HAVE_OPENCV
- writer.setCodec( CV_FOURCC('P','I','M','1') );
-#endif
- writer.setFileName("./test.mpeg");
-
- writer.open(I);
-
- for ( ; ; )
- {
- // Here the code to capture or create an image and store it in I.
-
- // Save the image
- writer.saveFrame(I);
- }
-
- writer.close();
-
- return 0;
-}
- \endcode
-*/
-
-class VISP_EXPORT vpVideoWriter
-{
- private:
-#ifdef VISP_HAVE_FFMPEG
- //!To read video files
- vpFFMPEG *ffmpeg;
- //!The codec to use
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- CodecID codec;
-#else
- AVCodecID codec;
-#endif
- //!The bite rate
- unsigned int bit_rate;
- int framerate;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- cv::VideoWriter writer;
- int fourcc;
- double framerate;
-#endif
- //!Types of available formats
- typedef enum
- {
- FORMAT_PGM,
- FORMAT_PPM,
- FORMAT_JPEG,
- FORMAT_PNG,
- FORMAT_AVI,
- FORMAT_MPEG,
- FORMAT_MPEG4,
- FORMAT_MOV,
- FORMAT_UNKNOWN
- } vpVideoFormatType;
-
- //!Video's format which has to be writen
- vpVideoFormatType formatType;
-
- //!Path to the image sequence
- char fileName[FILENAME_MAX];
-
- //!Indicates if the path to the image sequence is set.
- bool initFileName;
-
- //!Indicates if the video is "open".
- bool isOpen;
-
- //!Count the frame number.
- unsigned int frameCount;
-
- //!The first frame index.
- unsigned int firstFrame;
-
- //!Size of the frame
- unsigned int width;
- unsigned int height;
-
- public:
- vpVideoWriter();
- ~vpVideoWriter();
-
- void close();
-
- /*!
- Gets the current frame index.
-
- \return Returns the current frame index.
- */
- inline unsigned int getCurrentFrameIndex() const {return frameCount;}
-
- void open (vpImage< vpRGBa > &I);
- void open (vpImage< unsigned char > &I);
- /*!
- Reset the frame counter and sets it to the first image index.
-
- By default the first frame index is set to 0.
- */
- inline void resetFrameCounter() {frameCount = firstFrame;}
-
- void saveFrame (vpImage< vpRGBa > &I);
- void saveFrame (vpImage< unsigned char > &I);
-
-#ifdef VISP_HAVE_FFMPEG
- /*!
- Sets the bit rate of the video when encoding.
-
- \param bitrate : the expected bit rate.
-
- By default the bit rate is set to 500 000.
- */
- inline void setBitRate(const unsigned int bitrate) {this->bit_rate = bitrate;}
-
- /*!
- Sets the codec used to encode the video.
-
- \param codec_id : the expected codec.
-
- By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
- AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
- AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
-
- Of course to use the codec it must be installed on your computer.
- */
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- inline void setCodec(const CodecID codec_id) {this->codec = codec_id;}
-#else
- inline void setCodec(const AVCodecID codec_id) {this->codec = codec_id;}
-#endif
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- inline void setCodec(const int fourcc_codec) {this->fourcc = fourcc_codec;}
-#endif
-
- void setFileName(const char *filename);
- void setFileName(const std::string &filename);
- /*!
- Enables to set the first frame index.
-
- \param first_frame : The first frame index.
- */
- inline void setFirstFrameIndex(const unsigned int first_frame) {this->firstFrame = first_frame;}
-#ifdef VISP_HAVE_FFMPEG
- /*!
- Sets the framerate in Hz of the video when encoding.
-
- \param frame_rate : the expected framerate.
-
- By default the framerate is set to 25Hz.
- */
- inline void setFramerate(const int frame_rate) {
- this->framerate = frame_rate;
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- /*!
- Sets the framerate in Hz of the video when encoding.
-
- \param frame_rate : the expected framerate.
-
- By default the framerate is set to 25Hz.
- */
- inline void setFramerate(const double frame_rate) {
- this->framerate = frame_rate;
- }
-#endif
-
- private:
- vpVideoFormatType getFormat(const char *filename);
- static std::string getExtension(const std::string &filename);
-};
-
-#endif
diff --git a/src/visual-feature/vpBasicFeature.cpp b/src/visual-feature/vpBasicFeature.cpp
deleted file mode 100644
index 4029b5f..0000000
--- a/src/visual-feature/vpBasicFeature.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBasicFeature.cpp 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature.
- *
- * Authors:
- * Eric Marchand
- * Nicolas Mansard
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpBasicFeature.h>
-
-const unsigned int vpBasicFeature::FEATURE_LINE [32] =
- {
- (unsigned int)(1 << 0), (unsigned int)(1 << 1),
- (unsigned int)(1 << 2), (unsigned int)(1 << 3),
- (unsigned int)(1 << 4), (unsigned int)(1 << 5),
- (unsigned int)(1 << 6), (unsigned int)(1 << 7),
- (unsigned int)(1 << 8), (unsigned int)(1 << 9),
- (unsigned int)(1 << 10), (unsigned int)(1 << 11),
- (unsigned int)(1 << 12), (unsigned int)(1 << 13),
- (unsigned int)(1 << 14), (unsigned int)(1 << 15),
- (unsigned int)(1 << 16), (unsigned int)(1 << 17),
- (unsigned int)(1 << 18), (unsigned int)(1 << 19),
- (unsigned int)(1 << 20), (unsigned int)(1 << 21),
- (unsigned int)(1 << 22), (unsigned int)(1 << 23),
- (unsigned int)(1 << 24), (unsigned int)(1 << 25),
- (unsigned int)(1 << 26), (unsigned int)(1 << 27),
- (unsigned int)(1 << 28), (unsigned int)(1 << 29),
- (unsigned int)(1 << 30), (unsigned int)(1 << 31)
- };
-const unsigned int vpBasicFeature::FEATURE_ALL = 0xffff;
-
-/*!
- \file vpBasicFeature.cpp
- \brief Class that defines what is a visual feature.
-*/
-/*!
- Default constructor.
-*/
-vpBasicFeature::vpBasicFeature()
- : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user)
-{
-}
-
-/*!
- Destructor that free allocated memory.
-*/
-vpBasicFeature::~vpBasicFeature()
-{
- if (flags != NULL) {
- delete [] flags;
- flags = NULL;
- }
-}
-
-/*!
- Copy constructor.
-*/
-vpBasicFeature::vpBasicFeature(const vpBasicFeature &f)
- : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user)
-{
- *this = f;
-}
-
-/*!
- Copy operator.
-*/
-vpBasicFeature &vpBasicFeature::operator=(const vpBasicFeature &f)
-{
- s = f.s;
- dim_s = f.dim_s;
- nbParameters = f.nbParameters;
- deallocate = f.deallocate;
- if (flags)
- delete [] flags;
- flags = new bool [nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = f.flags[i];
-
- return (*this);
-}
-
-//! Get the feature vector dimension.
-unsigned int
-vpBasicFeature::getDimension(unsigned int select) const
-{
- unsigned int dim = 0 ;
- 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 ;
- }
- return dim ;
-}
-
-//! Get the feature vector \f$\bf s\f$.
-vpColVector
- vpBasicFeature::get_s(const unsigned int select) const
-{
- vpColVector state(0),stateLine(1);
- // if s is higher than the possible selections (photometry), send back the whole vector
- if(dim_s > 31)
- return s;
-
- for(unsigned int i=0;i<dim_s;++i)
- {
- if(FEATURE_LINE[i] & select)
- {
- stateLine[0] = s[i];
- state.stackMatrices(stateLine);
- }
- }
- return state ;
-}
-
-void vpBasicFeature::resetFlags()
-{
- if (flags != NULL)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = false;
- }
-}
-
-//! Set feature flags to true to prevent warning when re-computing the interaction matrix without having updated the feature.
-void vpBasicFeature::setFlags()
-{
- if (flags != NULL)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = true;
- }
-}
-
-//! Compute the error between two visual features from a subset of the possible features.
-vpColVector vpBasicFeature::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0),eLine(1);
- if (dim_s <= 31)
- {
- for(unsigned int i=0;i<dim_s;++i){
- if(FEATURE_LINE[i] & select)
- {
- eLine[0] = s[i] - s_star[i];
- e.stackMatrices(eLine);
- //std::cout << "dim_s <= 31"<<std::endl;
- }
- }
- }
- else
- {
- e.resize(dim_s);
- vpColVector sd = s_star.get_s();
- e = s - sd;
- }
-
- return e ;
-}
-
-/*
- * Local variables:
- * c-basic-offset: 4
- * End:
- */
diff --git a/src/visual-feature/vpBasicFeature.h b/src/visual-feature/vpBasicFeature.h
deleted file mode 100644
index 4f4d055..0000000
--- a/src/visual-feature/vpBasicFeature.h
+++ /dev/null
@@ -1,173 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpBasicFeature.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature.
- *
- * Authors:
- * Eric Marchand
- * Nicolas Mansard
- *
- *****************************************************************************/
-
-
-
-#ifndef vpBasicFeature_H
-#define vpBasicFeature_H
-
-/*!
- \file vpBasicFeature.h
- \brief class that defines what is a visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-
-// Display Issue
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-
-//Color / image / display
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-
-// #define FEATURE_ALL 0xff
-
-// #define FEATURE_LINE1 0x1
-// #define FEATURE_LINE2 0x2
-// #define FEATURE_LINE3 0x4
-// #define FEATURE_LINE4 0x8
-// #define FEATURE_LINE5 0x10
-// #define FEATURE_LINE6 0x20
-// #define FEATURE_LINE7 0x40
-// #define FEATURE_LINE8 0x80
-
-
-/*!
- \class vpBasicFeature
- \brief class that defines what is a visual feature
-*/
-class VISP_EXPORT vpBasicFeature
-{
-public: // Public constantes
- static const unsigned int FEATURE_LINE [32];
-
- static const unsigned int FEATURE_ALL;
-
-protected:
- //! State of the visual feature.
- vpColVector s ;
- //! Dimension of the visual feature.
- unsigned int dim_s ;
- //int featureLine[8] ;
- //! Ensure that all the parameters needed to compute the iteraction matrix are set.
- bool *flags;
- //! Number of parameters needed to compute the interaction matrix.
- unsigned int nbParameters;
-
-public:
- /*! Return the dimension of the feature vector \f$\bf s\f$. */
- unsigned int dimension_s() { return dim_s ; }
-
-public:
-
- virtual void init() = 0 ;
-
- vpBasicFeature() ;
- vpBasicFeature(const vpBasicFeature &f) ;
- vpBasicFeature &operator=(const vpBasicFeature &f) ;
- virtual ~vpBasicFeature();
- //! Return element \e i in the state vector (usage : x = s[i] )
- virtual inline double operator[](const unsigned int i) const { return s[i]; }
-
- //! Select all the features.
- static unsigned int selectAll() { return FEATURE_ALL ; }
-
- // Get the feature vector.
- vpColVector get_s(unsigned int select=FEATURE_ALL) const;
-
- // Get the feature vector dimension.
- unsigned int getDimension(const unsigned int select=FEATURE_ALL) const;
- //! Compute the interaction matrix from a subset of the possible features.
- virtual vpMatrix interaction(const unsigned int select = FEATURE_ALL) = 0;
- virtual vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select= FEATURE_ALL);
- //! Print the name of the feature.
- virtual void print(const unsigned int select= FEATURE_ALL) const = 0 ;
-
- virtual vpBasicFeature *duplicate() const = 0 ;
-
-public:
- virtual void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const = 0;
- virtual void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const = 0;
-
- void setFlags();
-
-protected:
- void resetFlags();
- // memory issue (used by the vpServo class)
-public:
-
- /*!
- \enum vpBasicFeatureDeallocatorType
- Indicates who should deallocate the feature.
-
- */
- typedef enum
- {
- user,
- vpServo
- } vpBasicFeatureDeallocatorType;
-
-protected:
- vpBasicFeatureDeallocatorType deallocate ;
-public:
- void setDeallocate(vpBasicFeatureDeallocatorType d) { deallocate = d ; }
- vpBasicFeatureDeallocatorType getDeallocate() { return deallocate ; }
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureDepth.cpp b/src/visual-feature/vpFeatureDepth.cpp
deleted file mode 100644
index e8bc72c..0000000
--- a/src/visual-feature/vpFeatureDepth.cpp
+++ /dev/null
@@ -1,510 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureDepth.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point visual feature.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureDepth.cpp
- \brief Class that defines 2D point visual feature
-*/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureDepth.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-
-
-/*
-
-
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities ar useful but not mandatory
-
-
-
-
-
-*/
-
-/*!
- Initialize the memory space requested for 3D depth visual feature.
-*/
-void
-vpFeatureDepth::init()
-{
- //feature dimension
- dim_s = 1 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
- x = y = 0.;
- Z = 1.;
-}
-
-
-/*!
- Default constructor that build a visual feature.
-*/
-vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
-{
- init() ;
-}
-
-
-/*!
- Set the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
-
- \param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
-*/
-void
-vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
-{
- s[0] = LogZoverZstar ;
-}
-
-
-/*!
- Get the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
-
- \return The value of \f$ log(\frac{Z}{Z^*}) \f$.
-*/
-double
-vpFeatureDepth::get_LogZoverZstar() const
-{
- return s[0] ;
-}
-
-
-/*!
- Set the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
-
- \param x_ : \f$ x \f$ value to set.
-*/
-void
-vpFeatureDepth::set_x(const double x_)
-{
- this->x = x_ ;
- flags[0] = true;
-}
-
-
-/*!
- Get the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
-
- \return The value of \f$ x \f$.
-*/
-double
-vpFeatureDepth::get_x() const
-{
- return x ;
-}
-
-
-/*!
- Set the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
-
- \param y_ : \f$ y \f$ value to set.
-*/
-void
-vpFeatureDepth::set_y(const double y_)
-{
- this->y = y_ ;
- flags[1] = true;
-}
-
-
-/*!
- Get the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
-
- \return The value of \f$ y \f$.
-*/
-double
-vpFeatureDepth::get_y() const
-{
- return y ;
-}
-
-/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \param Z_ : \f$ Z \f$ value to set.
-*/
-void
-vpFeatureDepth::set_Z(const double Z_)
-{
- this->Z = Z_ ;
- flags[2] = true;
-}
-
-
-/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \return The value of \f$ Z \f$.
-*/
-double
-vpFeatureDepth::get_Z() const
-{
- return Z ;
-}
-
-
-/*!
- Set the value of \f$ x \f$, \f$ y \f$, \f$ Z \f$ and \f$ log(\frac{Z}{Z^*}) \f$. \f$ x \f$ and \f$ y \f$ represent the coordinates of the point in the camera frame. \f$ Z \f$ is the 3D coordinate representing the depth. \f$ log(\frac{Z}{Z^*}) \f$ represents the logarithm of the current depth relative to the desired depth.
-
- \param x_ : \f$ x \f$ value to set.
- \param y_ : \f$ y \f$ value to set.
- \param Z_ : \f$ Z \f$ value to set.
- \param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
-*/
-void
-vpFeatureDepth::set_xyZLogZoverZstar(const double x_,
- const double y_,
- const double Z_,
- const double LogZoverZstar)
-{
- set_x(x_) ;
- set_y(y_) ;
- set_Z(Z_) ;
- set_LogZoverZstar(LogZoverZstar) ;
- for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
- Compute and return the interaction matrix \f$ L \f$. The computation is made thanks to the values of the point coordinates \f$ x \f$ and \f$ y \f$ and the depth \f$ Z \f$.
-
- \f[ L =
- \left[\begin{array}{cccccc}
- 0 & 0 & -1/Z & -y & x & 0
- \end{array}\right]\f]
-
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
-
- \return The interaction matrix computed from the point feature.
-
- The code below shows how to compute the interaction matrix associated to the visual feature \f$ s = log(\frac{Z}{Z^*}) \f$.
- \code
- // Creation of the current feature s
- vpFeatureDepth s;
- s.buildFrom(0, 0, 5, log(5/1)); //The current depth is 5 metters and the desired is 1 metter.
-
- vpMatrix L_x = s.interaction();
- \endcode
-*/
-vpMatrix
-vpFeatureDepth::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but z was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- L.resize(1,6) ;
-
- double x_ = get_x();
- double y_ = get_y();
- double Z_ = get_Z();
-
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z_) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- if (FEATURE_ALL & select)
- {
- L = 0;
- L[0][0] = 0;
- L[0][1] = 0;
- L[0][2] = -1/Z_;
- L[0][3] = -y_;
- L[0][4] = x_;
- L[0][5] = 0;
- }
-
- return L ;
-}
-
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- Since this visual feature \f$ s \f$ represent the current depth relative to the desired depth, the desired visual
- feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
- the current visual feature \f$ s \f$.
-
- \param s_star : Desired visual visual feature that should be equal to zero.
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- \exception vpFeatureException::badInitializationError : If the
- desired visual feature \f$ s^* \f$ is not equal to zero.
-
- The code below shows how to use this method:
-
- \code
- // Creation of the current feature s
- vpFeatureDepth s;
-
- // Creation of the desired feature s^*. By default this feature is
- // initialized to zero
- vpFeatureDepth s_star;
- s_star.setLogZoverZstar(0)
-
- // Compute the interaction matrix for the ThetaU_z feature
- vpMatrix L_z = s.interaction();
-
- // Compute the error vector (s-s*) for the ThetaU_z feature
- s.error(s_star);
- \endcode
-*/
-vpColVector
-vpFeatureDepth::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
-
- if (fabs(s_star.get_s().sumSquare()) > 1e-6)
- {
- vpERROR_TRACE("s* should be zero ! ") ;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "s* should be zero !")) ;
- }
-
- vpColVector e(1) ;
- if(FEATURE_ALL & select)
- {
- e[0] = s[0];
- }
-
- return e ;
-}
-
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
-
- \code
- vpFeatureDepth s; // Current visual feature s
-
- // Creation of the current feature s
- s.buildFrom(0, 0, 5, log(5/1));
-
- s.print(); // print all the 2 components of the feature
- \endcode
-*/
-void
-vpFeatureDepth::print(const unsigned int select ) const
-{
- if (FEATURE_ALL & select)
- {
- std::cout <<"Point: x=" << get_x() ;
- std::cout <<" Point: y=" << get_y() ;
- std::cout <<" Point: Z=" << get_Z() ;
-
- std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;
-
- std::cout <<std::endl ;
- }
-}
-
-
-/*!
- Build a 3D depth visual feature from the point coordinates \f$ x \f$ and \f$ y \f$ given in the camera frame, \f$ Z \f$ which describes the depth and \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
-
- \param x_ : The \f$ x \f$ parameter.
- \param y_ : The \f$ y \f$ parameter.
- \param Z_ : The \f$ Z \f$ parameter.
- \param LogZoverZstar : The \f$ log(\frac{Z}{Z^*}) \f$ parameter.
-*/
-void
-vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
-{
-
- s[0] = LogZoverZstar;
-
- this->x = x_ ;
- this->y = y_ ;
- this->Z = Z_ ;
-
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-
-}
-
-
-
-/*!
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureDepth s;
- s_star = s.duplicate(); // s_star is now a vpFeatureDepth
- \endcode
-
-*/
-vpFeatureDepth *vpFeatureDepth::duplicate() const
-{
- vpFeatureDepth *feature = new vpFeatureDepth;
- return feature ;
-}
-
-/*!
-
- Not implemented.
-
-*/
-void
-vpFeatureDepth::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-/*!
-
- Not implemented.
-
- */
-void
-vpFeatureDepth::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureDepth.h b/src/visual-feature/vpFeatureDepth.h
deleted file mode 100644
index 7528ea0..0000000
--- a/src/visual-feature/vpFeatureDepth.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureDepth.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point visual feature.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureDepth_H
-#define vpFeatureDepth_H
-
-/*!
- \file vpFeatureDepth.h
- \brief Class that defines 3D point visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeatureDepth
- \ingroup VsFeature3
-
- \brief Class that defines a 3D point visual feature \f$ s\f$ which
- is composed by one parameters that is \f$ log( \frac{Z}{Z^*}) \f$
- that defines the current depth relative to the desired depth. Here
- \f$ Z \f$ represents the current depth and \f$ Z^* \f$ the desired
- depth.
-
- In this class \f$ x \f$ and \f$ y \f$ are the 2D coordinates in the
- camera frame and are given in meter. \f$ x \f$, \f$ y \f$ and \f$ Z
- \f$ are needed during the computation of the interaction matrix \f$
- L \f$.
-
- The visual features can be set easily thanks to the buildFrom() method.
-
- As the visual feature \f$ s \f$ represents the current depth
- relative to the desired depth, the desired visual feature \f$ s^*
- \f$ is set to zero. Once the value of the visual feature is set, the
- interaction() method allows to compute the interaction matrix \f$ L
- \f$ associated to the visual feature, while the error() method
- computes the error vector \f$(s - s^*)\f$ between the current visual
- feature and the desired one which is here set to zero.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 3D depth feature \f$ log( \frac{Z}{Z^*}) \f$ that
- corresponds to the current depth relative to the desired depth. To
- control six degrees of freedom, at least five other features must be
- considered. First we create a current (\f$s\f$) 3D depth
- feature. Then we set the task to use the interaction matrix
- associated to the current feature \f$L_s\f$. And finally we compute
- the camera velocity \f$v=-\lambda \; L_s^+ \; (s-s^*)\f$. The
- current feature \f$s\f$ is updated in the while() loop.
-
- \code
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpFeatureDepth s; //The current point feature.
- //Set the current parameters x, y, Z and the desired depth Zs
- double x; //You have to compute the value of x.
- double y; //You have to compute the value of y.
- double Z; //You have to compute the value of Z.
- double Zs; //You have to define the desired depth Zs.
- //Set the point feature thanks to the current parameters.
- s.buildfrom(x, y, Z, log(Z/Zs));
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the desired visual features sd
- task.setInteractionMatrixType(vpServo::CURRENT);
-
- // Add the 3D depth feature to the task
- task.addFeature(s); // s* is here considered as zero
-
- // Control loop
- for ( ; ; ) {
- // The new parameters x, y and Z must be computed here.
-
- // Update the current point visual feature
- s.buildfrom(x, y, Z, log(Z/Zs));
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
- return 0;
-}
- \endcode
-
- If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
- build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-
-int main()
-{
- vpFeatureDepth s; //The current point feature.
- //Set the current parameters x, y, Z and the desired depth Zs
- double x; //You have to compute the value of x.
- double y; //You have to compute the value of y.
- double Z; //You have to compute the value of Z.
- double Zs; //You have to define the desired depth Zs.
- //Set the point feature thanks to the current parameters.
- s.buildfrom(x, y, Z, log(Z/Zs));
-
- // Compute the interaction matrix L_s for the current point feature
- vpMatrix L = s.interaction();
-
- // Compute the error vector (s-s*) for the point feature with s* considered as 0.
- vpColVector s_star(1); //the dimension is 1.
- s_star(1) = 0; //The value of s* is 0.
- s.error(s_star);
-}
- \endcode
-*/
-
-
-class VISP_EXPORT vpFeatureDepth : public vpBasicFeature
-{
-
-private:
- //! The \f$ x \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
- double x;
- //! The \f$ y \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
- double y;
- //! The \f$ Z \f$ 3D coordinate of the point in the camera frame (required to compute the interaction matrix)
- double Z;
-
-public:
- void init() ;
- vpFeatureDepth() ;
- //! Destructor.
- virtual ~vpFeatureDepth() {}
-
- /*
- section Set coordinates
- */
-
- void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar) ;
-
- void set_x(const double x) ;
-
- void set_y(const double y) ;
-
- void set_Z(const double Z) ;
-
- void set_LogZoverZstar(const double LogZoverZstar);
-
- void set_xyZLogZoverZstar(const double x, const double y, const double Z, const double logZZs) ;
-
- double get_x() const ;
-
- double get_y() const ;
-
- double get_Z() const ;
-
- double get_LogZoverZstar() const ;
-
-
- /*
- vpBasicFeature method instantiation
- */
-
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
-
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- vpFeatureDepth *duplicate() const ;
-
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureDisplay.cpp b/src/visual-feature/vpFeatureDisplay.cpp
deleted file mode 100644
index 83fdd19..0000000
--- a/src/visual-feature/vpFeatureDisplay.cpp
+++ /dev/null
@@ -1,429 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureDisplay.cpp 4773 2014-07-10 17:09:23Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpFeatureDisplay.h>
-
-// Meter/pixel conversion
-#include <visp/vpMeterPixelConversion.h>
-
-// display
-#include <visp/vpDisplay.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-#include <visp/vpImagePoint.h>
-
-
-
-/*!
- \param x,y : Point coordinates in meters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
-
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try{
- vpImagePoint ip; // pixel coordinates in float
- vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
-
- vpDisplay::displayCross(I, ip, 15, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-/*!
- \param rho, theta : Line parameters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness )
-{
-
-
- try{
- // x cos(theta) + y sin(theta) - rho = 0
-
- double rhop,thetap ;
- vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
-
- // u cos(thetap) + v sin(thetap) - rhop = 0
-
- double co = cos(thetap) ;
- double si = sin(thetap) ;
- double c = -rhop ;
-
- // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
- // double u1,v1,u2,v2 ;
-
- double a = si ;
- double b = co ;
- vpImagePoint ip1, ip2;
-
- if (fabs(a) < fabs(b)) {
- ip1.set_ij(0, (-c)/b);
- double h = I.getHeight() - 1;
- ip2.set_ij(h, (-c - a*h)/b);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- else {
- ip1.set_ij((-c)/a, 0);
- double w = I.getWidth()-1;
- ip2.set_ij((-c - b*w)/a, w);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-/*!
- \param rho1, theta1 : Cylinder limb1 parameters.
- \param rho2, theta2 : Cylinder limb2 parameters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayCylinder(double rho1,double theta1,
- double rho2,double theta2,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- displayLine(rho1, theta1, cam, I, color, thickness) ;
- displayLine(rho2, theta2, cam, I, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-/*!
- \param x, y, mu20, mu11, mu02 : Ellipse parameters where:
- - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
- along the horizontal and vertical axis in the image.
- - \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-
- \sa vpDisplay::displayEllipse()
-*/
-void vpFeatureDisplay::displayEllipse(double x,double y,
- double mu20, double mu11, double mu02,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try {
- unsigned int number_of_points = 45 ;
- const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
- unsigned int i = 0 ;
-
- double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
- double a, b, e ;
-
- //if (fabs(mu11)<1e-6) e =0 ;
- if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
- e = 0 ;
- a = sqrt(mu20);
- b = sqrt(mu02);
- }
- else {
- e = (mu02-mu20+s)/(2*mu11) ;
- a = sqrt( (mu02+mu20+s)/2.0) ;
- b = sqrt( (mu02+mu20-s)/2.0) ;
- }
-
- double e1 = atan(e) ;
-
- double k = 0.0 ;
-
- double ce = cos(e1) ;
- double se = sin(e1) ;
-
- double x2 = 0;
- double y2 =0;
- vpImagePoint ip1, ip2;
-
- for( i = 0; i < number_of_points+2 ; i++)
- {
- double x1 = a *cos(k) ; // equation of an ellipse
- double y1 = b *sin(k) ; // equation of an ellipse
- double x11 = x + ce *x1 - se *y1 ;
- double y11 = y + se *x1 + ce *y1 ;
-
- vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
-
- if (i > 1) {
- ip2.set_u( x2 );
- ip2.set_v( y2 );
-
- vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
- }
-
- ip2 = ip1;
- y2 = y1 ;
- x2 = x1 ;
- k += incr ;
- } // end for loop
- }
- catch(vpException &e)
- {
- throw(e);
- }
-}
-
-/*!
- \param x, y : Point coordinates in meters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
-
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try{
- vpImagePoint ip; // pixel coordinates in float
- vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
-
- vpDisplay::displayCross(I, ip, 15, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
-}
-
-/*!
- \param rho, theta : Line parameters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness )
-{
-
-
- try{
- // x cos(theta) + y sin(theta) - rho = 0
-
- double rhop,thetap ;
- vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
-
- // u cos(thetap) + v sin(thetap) - rhop = 0
-
- double co = cos(thetap) ;
- double si = sin(thetap) ;
- double c = -rhop ;
-
- // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
- // double u1,v1,u2,v2 ;
-
- double a = si ;
- double b = co ;
- vpImagePoint ip1, ip2;
-
- if (fabs(a) < fabs(b)) {
- ip1.set_ij(0, (-c)/b);
- double h = I.getHeight() - 1;
- ip2.set_ij(h, (-c - a*h)/b);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- else {
- ip1.set_ij((-c)/a, 0);
- double w = I.getWidth()-1;
- ip2.set_ij((-c - b*w)/a, w);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-/*!
- \param rho1, theta1 : Cylinder limb1 parameters.
- \param rho2, theta2 : Cylinder limb2 parameters.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-*/
-void vpFeatureDisplay::displayCylinder(double rho1, double theta1,
- double rho2, double theta2,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try
- {
- displayLine(rho1, theta1, cam, I, color, thickness) ;
- displayLine(rho2, theta2, cam, I, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
- \param x, y, mu20, mu11, mu02 : Ellipse parameters where:
- - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
- along the horizontal and vertical axis in the image.
- - \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
- \param cam : Camera intrinsic parameters.
- \param I : Image.
- \param color : Color to use to display the feature
- \param thickness : Thickness of the feature representation.
-
- \sa vpDisplay::displayEllipse()
-*/
-void vpFeatureDisplay::displayEllipse(double x, double y,
- double mu20, double mu11, double mu02,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness)
-{
- try {
- unsigned int number_of_points = 45 ;
- const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
- unsigned int i = 0 ;
-
- double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
- double a, b, e ;
-
- //if (fabs(mu11)<1e-6) e =0 ;
- if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
- e = 0 ;
- a = sqrt(mu20);
- b = sqrt(mu02);
- }
- else {
- e = (mu02-mu20+s)/(2*mu11) ;
- a = sqrt( (mu02+mu20+s)/2.0) ;
- b = sqrt( (mu02+mu20-s)/2.0) ;
- }
-
- double e1 = atan(e) ;
-
- double k = 0.0 ;
-
- double ce = cos(e1) ;
- double se = sin(e1) ;
-
- double x2 = 0;
- double y2 =0;
- vpImagePoint ip1, ip2;
-
- for( i = 0; i < number_of_points+2 ; i++)
- {
- double x1 = a *cos(k) ; // equation of an ellipse
- double y1 = b *sin(k) ; // equation of an ellipse
- double x11 = x + ce *x1 - se *y1 ;
- double y11 = y + se *x1 + ce *y1 ;
-
- vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
-
- if (i > 1) {
- ip2.set_u( x2 );
- ip2.set_v( y2 );
-
- vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
- }
-
- ip2 = ip1;
- y2 = y1 ;
- x2 = x1 ;
- k += incr ;
- } // end for loop
- }
- catch(vpException &e)
- {
- throw(e);
- }
-}
diff --git a/src/visual-feature/vpFeatureDisplay.h b/src/visual-feature/vpFeatureDisplay.h
deleted file mode 100644
index 9547c4d..0000000
--- a/src/visual-feature/vpFeatureDisplay.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureDisplay.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Interface with the image for feature display.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureDisplay_H
-#define vpFeatureDisplay_H
-
-/*!
- \file vpFeatureDisplay.h
- \brief interface with the image for feature display
-*/
-
-//Color / image / display
-#include <visp/vpImage.h>
-#include <visp/vpColor.h>
-#include <visp/vpRGBa.h>
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-
-/*!
- \class vpFeatureDisplay
-
- \ingroup VsFeatureDisplay
- \brief Interface with the image for feature display.
-*/
-class VISP_EXPORT vpFeatureDisplay
-{
-
-public:
- static void displayCylinder(double rho1,double theta1,
- double rho2,double theta2,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayCylinder(double rho1,double theta1,
- double rho2,double theta2,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
-
- static void displayEllipse(double x,double y,
- double mu20, double mu11, double m02,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
-
- static void displayEllipse(double x,double y,
- double mu20, double mu11, double m02,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
-
- static void displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
-
- static void displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureEllipse.cpp b/src/visual-feature/vpFeatureEllipse.cpp
deleted file mode 100644
index 8c864e1..0000000
--- a/src/visual-feature/vpFeatureEllipse.cpp
+++ /dev/null
@@ -1,489 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureEllipse.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D ellipse visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureEllipse.cpp
- \brief Class that defines 2D ellipse visual feature
-*/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureEllipse.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-
-
-#include <visp/vpFeatureDisplay.h>
-
-
-/*
-
-
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities ar useful but not mandatory
-
-
-
-
-
-*/
-
-void
-vpFeatureEllipse::init()
-{
- //feature dimension
- dim_s = 5 ;
- nbParameters = 8;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default depth values
- A = B = 0;
- C =1 ;
-
-}
-
-vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0)
-{
- init() ;
-}
-
-
-
-//! compute the interaction matrix from a subset a the possible features
-vpMatrix
-vpFeatureEllipse::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but mu20 was not set yet");
- break;
- case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but mu11 was not set yet");
- break;
- case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but mu02 was not set yet");
- break;
- case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
- break;
- case 6:
- vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
- break;
- case 7:
- vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- double xc = s[0] ;
- double yc = s[1] ;
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
-
- //eq 39
- double Z = 1/(A*xc + B*yc + C) ;
-
-
-
- if (vpFeatureEllipse::selectX() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
-
- H[0][0] = -1/Z;
- H[0][1] = 0 ;
- H[0][2] = xc/Z + A*mu20 + B*mu11;
- H[0][3] = xc*yc + mu11;
- H[0][4] = -1-vpMath::sqr(xc)-mu20;
- H[0][5] = yc;
-
-
- L = vpMatrix::stackMatrices(L,H) ;
- }
-
- if (vpFeatureEllipse::selectY() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
-
- H[0][0] = 0 ;
- H[0][1] = -1/Z;
- H[0][2] = yc/Z + A*mu11 + B*mu02;
- H[0][3] = 1+vpMath::sqr(yc)+mu02;
- H[0][4] = -xc*yc - mu11;
- H[0][5] = -xc;
-
- L = vpMatrix::stackMatrices(L,H) ;
- }
-
- if (vpFeatureEllipse::selectMu20() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
- H[0][0] = -2*(A*mu20+B*mu11);
- H[0][1] = 0 ;
- H[0][2] = 2*((1/Z+A*xc)*mu20+B*xc*mu11) ;
- H[0][3] = 2*(yc*mu20+xc*mu11);
- H[0][4] = -4*mu20*xc;
- H[0][5] = 2*mu11;
-
- L = vpMatrix::stackMatrices(L,H) ;
- }
-
- if (vpFeatureEllipse::selectMu11() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
- H[0][0] = -A*mu11-B*mu02;
- H[0][1] = -A*mu20-B*mu11;
- H[0][2] = A*yc*mu20+(3/Z-C)*mu11+B*xc*mu02;
- H[0][3] = 3*yc*mu11+xc*mu02;
- H[0][4] = -yc*mu20-3*xc*mu11;
- H[0][5] = mu02-mu20;
-
- L = vpMatrix::stackMatrices(L,H) ;
- }
-
- if (vpFeatureEllipse::selectMu02() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
- H[0][0] = 0 ;
- H[0][1] = -2*(A*mu11+B*mu02);
- H[0][2] = 2*((1/Z+B*yc)*mu02+A*yc*mu11);
- H[0][3] = 4*yc*mu02;
- H[0][4] = -2*(yc*mu11 +xc*mu02) ;
- H[0][5] = -2*mu11 ;
- L = vpMatrix::stackMatrices(L,H) ;
- }
-
-
- return L ;
-}
-
-//! compute the error between two visual features from a subset
-//! a the possible features
-vpColVector
-vpFeatureEllipse::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeatureEllipse::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeatureEllipse::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- if (vpFeatureEllipse::selectMu20() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[2] - s_star[2] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeatureEllipse::selectMu11() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[3] - s_star[3] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- if (vpFeatureEllipse::selectMu02() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[4] - s_star[4] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matrix related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
-
- return e ;
-
-}
-
-
-void
-vpFeatureEllipse::print(const unsigned int select ) const
-{
-
- std::cout <<"Ellipse: " << std::endl ;
- if (vpFeatureEllipse::selectX() & select )
- std::cout << " x=" << s[0] <<std::endl ;;
- if (vpFeatureEllipse::selectY() & select )
- std::cout << " y=" << s[1] <<std::endl ;
- if (vpFeatureEllipse::selectMu20() & select )
- std::cout << " mu20=" << s[2] <<std::endl ;
- if (vpFeatureEllipse::selectMu11() & select )
- std::cout << " mu11=" << s[3] <<std::endl ;
- if (vpFeatureEllipse::selectMu02() & select )
- std::cout << " mu02=" << s[4] <<std::endl ;
- std::cout << "A = "<<A <<" B = "<<B <<" C = "<<C << std::endl ;
-}
-
-
-void
-vpFeatureEllipse::buildFrom(const double x, const double y,
- const double mu20, const double mu11,
- const double mu02)
-{
-
- s[0] = x ;
- s[1] = y ;
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
-
- for( int i = 0; i < 5; i++) flags[i] = true;
-
-}
-
-void
-vpFeatureEllipse::buildFrom(const double x, const double y,
- const double mu20, const double mu11,
- const double mu02,
- const double a, const double b, const double c)
-{
-
- s[0] = x ;
- s[1] = y ;
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
-
- this->A = a ;
- this->B = b ;
- this->C = c ;
-
- for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-void
-vpFeatureEllipse::set_x(const double x)
-{
- s[0] = x ;
- flags[0] = true;
-}
-
-void
-vpFeatureEllipse::set_y(const double y)
-{
- s[1] = y ;
- flags[1] = true;
-}
-
-void
-vpFeatureEllipse::set_xy(const double x,const double y)
-{
- s[0] = x ;
- s[1] = y ;
- for( int i = 0; i < 2; i++) flags[i] = true;
-}
-
-void
-vpFeatureEllipse::setABC(const double a, const double b, const double c)
-{
- this->A = a ;
- this->B = b ;
- this->C = c ;
- for( unsigned int i = 5; i < nbParameters; i++) flags[i] = true;
-}
-
-
-void
-vpFeatureEllipse::setMu(const double mu20, const double mu11,
- const double mu02)
-{
-
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
- for( int i = 2; i < 5; i++) flags[i] = true;
-
-}
-
-
-
-/*!
-
- Display ellipse feature.
-
- \param cam : Camera parameters.
- \param I : Image on which features have to be displayed.
- \param color : Color used to display the feature.
- \param thickness : Thickness of the feature representation.
-*/
-void
-vpFeatureEllipse::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness ) const
-{
- try{
- double x = s[0] ;
- double y = s[1] ;
-
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
-
- vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
- cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Display ellipse feature.
-
- \param cam : Camera parameters.
- \param I : Color image on which features have to be displayed.
- \param color : Color used to display the feature.
- \param thickness : Thickness of the feature representation.
-*/
-void
-vpFeatureEllipse::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness ) const
-{
- try{
- double x = s[0] ;
- double y = s[1] ;
-
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
-
- vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
- cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-//! for memory issue (used by the vpServo class only)
-vpFeatureEllipse *vpFeatureEllipse::duplicate() const
-{
- vpFeatureEllipse *feature = new vpFeatureEllipse ;
- return feature ;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureEllipse.h b/src/visual-feature/vpFeatureEllipse.h
deleted file mode 100644
index aea4547..0000000
--- a/src/visual-feature/vpFeatureEllipse.h
+++ /dev/null
@@ -1,156 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureEllipse.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D ellipse visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpFeatureEllipse_H
-#define vpFeatureEllipse_H
-
-/*!
- \file vpFeatureEllipse.h
- \brief Class that defines 2D ellipse visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeatureEllipse
- \ingroup VsFeature2
- \brief Class that defines 2D ellipse visual feature.
-*/
-class VISP_EXPORT vpFeatureEllipse : public vpBasicFeature
-{
- /*
- attributes and members directly related to the vpBasicFeature needs
- other functionalities are useful but not mandatory
- */
-private:
- //! FeatureEllipse depth (required to compute the interaction matrix)
- //! default Z = 1m
- double A,B,C ;
-
-
-public:
- //! Default initialization.
- void init() ;
- //! Default constructor.
- vpFeatureEllipse() ;
- //! Destructor.
- virtual ~vpFeatureEllipse() { }
-
-public:
- /*!
- \section Set coordinates
- */
- //! basic constructor
- vpFeatureEllipse(const double x, const double y,
- const double mu20, const double mu11, const double mu02);
-
- // void buildFrom(const vpEllipse &p) ;
- void buildFrom(const double x, const double y,
- const double mu20, const double mu11, const double mu02) ;
- void buildFrom(const double x, const double y,
- const double mu20, const double mu11, const double mu02,
- const double A, const double B, const double C) ;
- void set_x(const double x);
- void set_y(const double y);
- void set_xy(const double x,const double y);
- void setABC(const double A, const double B, const double C) ;
- void setMu(const double mu20, const double mu11, const double mu02) ;
-
- double get_x() const {return s[0];}
- double get_y() const {return s[1];}
- double getMu20() const {return s[2];}
- double getMu11() const {return s[3];}
- double getMu02() const {return s[4];}
-
-public:
- /*!
- vpBasicFeature method instantiation
- */
-
- // feature selection
- inline static unsigned int selectX() { return FEATURE_LINE[0] ; }
- inline static unsigned int selectY() { return FEATURE_LINE[1] ; }
- inline static unsigned int selectMu20() { return FEATURE_LINE[2] ; }
- inline static unsigned int selectMu11() { return FEATURE_LINE[3] ; }
- inline static unsigned int selectMu02() { return FEATURE_LINE[4] ; }
-
- //! compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- //! compute the error between two visual features from a subset
- //! a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- //! compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
- //! print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- //! feature duplication
- vpFeatureEllipse *duplicate() const ;
-
-
-public:
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureException.h b/src/visual-feature/vpFeatureException.h
deleted file mode 100644
index f89311d..0000000
--- a/src/visual-feature/vpFeatureException.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureException.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Exception that can be emited by the vpFeature class and its derivates.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef __vpFeatureException_H
-#define __vpFeatureException_H
-
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-
-/* \file vpFeatureException.h
- \brief error that can be emited by the vpFeature class and its derivates
- */
-/* Classes standards. */
-
-#include <visp/vpConfig.h>
-#include <visp/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-/*!
- \class vpFeatureException
- \ingroup Exception
- \brief Error that can be emited by the vpBasicFeature class and its derivates.
- */
-class VISP_EXPORT vpFeatureException : public vpException
-{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpFeature member
- */
- enum errorFeatureCodeEnum
- {
- //! feature list or desired feature list is empty
- badErrorVectorError,
- sizeMismatchError,
- notInitializedError,
- badInitializationError
- } ;
-
- public:
- vpFeatureException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpFeatureException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpFeatureException (const int id)
- : vpException(id){ ; }
-
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureLine.cpp b/src/visual-feature/vpFeatureLine.cpp
deleted file mode 100644
index 77d348d..0000000
--- a/src/visual-feature/vpFeatureLine.cpp
+++ /dev/null
@@ -1,566 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureLine.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D line visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeatureLine.cpp
- \brief Class that defines 2D line visual feature
-*/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureLine.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// simple math function (round)
-#include <visp/vpMath.h>
-
-// Display Issue
-
-// Meter/pixel conversion
-#include <visp/vpCameraParameters.h>
-
-//Color / image / display
-#include <visp/vpColor.h>
-#include <visp/vpImage.h>
-
-
-
-#include <visp/vpFeatureDisplay.h>
-
-
-/*
-
-
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities ar useful but not mandatory
-
-
-
-
-
-*/
-
-/*!
- Initialize the memory space requested for 2D line visual feature.
-*/
-void
-vpFeatureLine::init()
-{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 6;
-
- // memory allocation
- // x cos(theta) + y sin(theta) - rho = 0
- // s[0] = rho
- // s[1] = theta
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- A = B = C = D = 0.0 ;
-}
-
-
-/*!
- Default constructor that build a visual feature.
-*/
-vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0)
-{
- init() ;
-}
-
-
-/*!
- Sets the values of \f$ \rho \f$ and \f$ \theta \f$ which represent the parameters of the 2D line feature.
-
- \param rho : \f$ \rho \f$ value to set.
- \param theta : \f$ \theta \f$ value to set.
-*/
-void
-vpFeatureLine::setRhoTheta(const double rho, const double theta)
-{
- s[0] = rho ;
- s[1] = theta ;
- for( int i = 0; i < 2; i++) flags[i] = true;
-}
-
-
-/*!
- Sets the values of A, B, C and D which represent the parameters used to describe the equation of a plan in the camera frame.
- \f[ AX + BY + CZ + D = 0 \f]
- Those parameters are needed to compute the interaction matrix associated to a visual feature. Normally, two plans are needed to describe a line (the intersection of those two plans). But to compute the interaction matrix only one plan equation is required. The only one restrictions is that the value of D must not be equal to zero !
-
- \param A_ : A value to set.
- \param B_ : B value to set.
- \param C_ : C value to set.
- \param D_ : D value to set.
-*/
-void
-vpFeatureLine::setABCD(const double A_, const double B_,
- const double C_, const double D_)
-{
- this->A = A_ ;
- this->B = B_ ;
- this->C = C_ ;
- this->D = D_ ;
- for(unsigned int i = 2; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L \f$. The computation
- is made thanks to the values of the line feature \f$ \rho \f$ and
- \f$ \theta \f$ and the equation of a plan to which the line belongs.
-
- \f[ L = \left[\begin{array}{c}L_{\rho} \\ L_{\theta}\end{array}\right] =
- \left[\begin{array}{cccccc}
- \lambda_{\rho}cos(\theta) & \lambda_{\rho}sin(\theta) & -\lambda_{\rho}\rho & (1+\rho^2)sin(\theta) & -(1+\rho^2)cos(\theta) & 0 \\
- \lambda_{\theta}cos(\theta) & \lambda_{\theta}sin(\theta) & -\lambda_{\theta}\rho & -\rho cos(\theta) & -\rho sin(\theta) & -1
- \end{array}\right]\f]
-
- Where :
- \f[ \lambda_{\rho} = (A \; \rho \; cos(\theta) + B \; \rho \; sin(\theta) + C) / D \f]
- \f[ \lambda_{\theta} = (A \; sin(\theta) - B \; cos(\theta)) / D \f]
-
- \param select : Selection of a subset of the possible line features.
- - To compute the interaction matrix for all the two line features
- use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
- interaction matrix is \f$ [2 \times 6] \f$
- - To compute the interaction matrix for only one of the line
- component feature (\f$ \rho, \theta \f$) use one of the
- corresponding function selectRho() or selectTheta(). In that case
- the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
-
- \return The interaction matrix computed from the line features.
-
- The code below shows how to compute the interaction matrix
- associated to the visual feature \f$ s = \theta \f$.
- \code
- // Creation of the current feature s
- vpFeatureLine s;
- s.buildFrom(0, 0, 0, 0, 1, -1);
-
- vpMatrix L_theta = s.interaction( vpFeatureLine::selectTheta() );
- \endcode
-
- The code below shows how to compute the interaction matrix
- associated to the visual feature \f$ s = (\rho, \theta) \f$.
-
- \code
- // Creation of the current feature s
- vpFeatureLine s;
- s.buildFrom(0, 0, 0, 0, 1, -1);
-
- vpMatrix L_theta = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-*/
-vpMatrix
-vpFeatureLine::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
- break;
- case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
- break;
- case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
- break;
- case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but D was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
- double rho = s[0] ;
- double theta = s[1] ;
-
-
- double co = cos(theta);
- double si = sin(theta);
-
- if (fabs(D) < 1e-6)
- {
- vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f",D) ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Incorrect plane coordinates D")) ;
- }
-
- double lambda_theta =( A*si - B*co) /D;
- double lambda_rho = (C + rho*A*co + rho*B*si)/D;
-
- if (vpFeatureLine::selectRho() & select )
- {
- vpMatrix Lrho(1,6) ;
-
-
- Lrho[0][0]= co*lambda_rho;
- Lrho[0][1]= si*lambda_rho;
- Lrho[0][2]= -rho*lambda_rho;
- Lrho[0][3]= si*(1.0 + rho*rho);
- Lrho[0][4]= -co*(1.0 + rho*rho);
- Lrho[0][5]= 0.0;
-
- L = vpMatrix::stackMatrices(L,Lrho) ;
- }
-
- if (vpFeatureLine::selectTheta() & select )
- {
- vpMatrix Ltheta(1,6) ;
-
- Ltheta[0][0] = co*lambda_theta;
- Ltheta[0][1] = si*lambda_theta;
- Ltheta[0][2] = -rho*lambda_theta;
- Ltheta[0][3] = -rho*co;
- Ltheta[0][4] = -rho*si;
- Ltheta[0][5] = -1.0;
-
- L = vpMatrix::stackMatrices(L,Ltheta) ;
- }
- return L ;
-}
-
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- \param s_star : Desired visual feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible line features.
- - To compute the error for all the two line features use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2
- dimension column vector.
- - To compute the error for only one of the line component feature
- (\f$ \rho, \theta \f$) use one of the corresponding function
- selectRho() or selectTheta(). In that case the error vector is a 1
- dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- The code below shows how to use this method to manipulate the \f$
- \theta \f$ subset:
-
- \code
- // Creation of the current feature s
- vpFeatureLine s;
- s.buildFrom(0, 0, 0, 0, 1, -1);
-
- // Creation of the desired feature s*
- vpFeatureLine s_star;
- s_star.buildFrom(0, 0, 0, 0, 1, -5);
-
- // Compute the interaction matrix for the theta feature
- vpMatrix L_theta = s.interaction( vpFeatureLine::selectTheta() );
-
- // Compute the error vector (s-s*) for the Theta feature
- s.error(s_star, vpFeatureLine::selectTheta());
- \endcode
-*/
-vpColVector
-vpFeatureLine::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeatureLine::selectRho() & select )
- {
- vpColVector erho(1) ;
- erho[0] = s[0] - s_star[0] ;
-
-
-
- e = vpMatrix::stackMatrices(e,erho) ;
- }
-
- if (vpFeatureLine::selectTheta() & select )
- {
-
- double err = s[1] - s_star[1] ;
- while (err < -M_PI) err += 2*M_PI ;
- while (err > M_PI) err -= 2*M_PI ;
-
- vpColVector etheta(1) ;
- etheta[0] = err ;
- e = vpMatrix::stackMatrices(e,etheta) ;
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matric related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
-
- return e ;
-
-}
-
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible line features.
- - To print all the two line features use vpBasicFeature::FEATURE_ALL.
- - To print only one of the line component feature (\f$ \rho, \theta \f$)
- use one of the corresponding function selectRho() or selectTheta().
-
- \code
- vpFeatureLine s; // Current visual feature s
-
- // Creation of the current feature s
- s.buildFrom(0, 0);
-
- s.print(); // print all the 2 components of the feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeatureLine::selectRho()); // print only the rho component
- \endcode
-*/
-
-void
-vpFeatureLine::print(const unsigned int select ) const
-{
-
- std::cout <<"Line:\t " << A <<"X+" << B <<"Y+" << C <<"Z +" << D <<"=0" <<std::endl ;;
- if (vpFeatureLine::selectRho() & select )
- std::cout << " \trho=" << s[0] ;
- if (vpFeatureLine::selectTheta() & select )
- std::cout << " \ttheta=" << s[1] ;
- std::cout <<std::endl ;
-}
-
-
-/*!
-
- Build a 2D line visual feature from the line equation parameters \f$
- \rho \f$ and \f$ \theta \f$ given in the image plan.
-
- \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f]
-
- See the vpFeatureLine class description for more details about \f$
- \rho \f$ and \f$ \theta \f$.
-
- \param rho : The \f$ \rho \f$ parameter.
- \param theta : The \f$ \theta \f$ parameter.
-
-*/
-void
-vpFeatureLine::buildFrom(const double rho, const double theta)
-{
- s[0] = rho ;
- s[1] = theta ;
- for( int i = 0; i < 2; i++) flags[i] = true;
-}
-
-
-/*!
-
- Build a 2D line visual feature from the line equation parameters \f$
- \rho \f$ and \f$ \theta \f$ given in the image plan. The
- parameters A, B, C and D which describe the equation of a plan to
- which the line belongs, are set in the same time.
-
- \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f]
- \f[ AX + BY + CZ + D = 0 \f]
-
- See the vpFeatureLine class description for more details about \f$
- \rho \f$ and \f$ \theta \f$.
-
- The A, B, C, D parameters are needed to compute the interaction
- matrix associated to a visual feature. Normally, two plans are
- needed to describe a line (the intersection of those two plans). But
- to compute the interaction matrix only one plan equation is
- required. The only one restrictions is that the value of D must not
- be equal to zero !
-
- \param rho : The \f$ \rho \f$ parameter.
- \param theta : The \f$ \theta \f$ parameter.
- \param A_ : A parameter of the plan equation.
- \param B_ : B parameter of the plan equation.
- \param C_ : C parameter of the plan equation.
- \param D_ : D parameter of the plan equation.
-
-*/
-void vpFeatureLine::buildFrom(const double rho, const double theta,
- const double A_, const double B_,
- const double C_, const double D_)
-{
- s[0] = rho ;
- s[1] = theta ;
- this->A = A_ ;
- this->B = B_ ;
- this->C = C_ ;
- this->D = D_ ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureLine s;
- s_star = s.duplicate(); // s_star is now a vpFeatureLine
- \endcode
-
-*/
-vpFeatureLine *vpFeatureLine::duplicate() const
-{
- vpFeatureLine *feature = new vpFeatureLine ;
- return feature ;
-}
-
-
-
-/*!
-
- Display line feature.
-
- \param cam : Camera parameters.
- \param I : Image on which features have to be displayed.
- \param color : Color used to display the feature.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeatureLine::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double rho,theta ;
- rho = getRho() ;
- theta = getTheta() ;
-
- vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Display line feature.
-
- \param cam : Camera parameters.
- \param I : Color image on which features have to be displayed.
- \param color : Color used to display the feature.
- \param thickness : Thickness of the feature representation.
-
- */
-void
-vpFeatureLine::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double rho,theta ;
- rho = getRho() ;
- theta = getTheta() ;
-
- vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureLine.h b/src/visual-feature/vpFeatureLine.h
deleted file mode 100644
index 5bb47d2..0000000
--- a/src/visual-feature/vpFeatureLine.h
+++ /dev/null
@@ -1,313 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureLine.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D line visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureLine_H
-#define vpFeatureLine_H
-
-/*!
- \file vpFeatureLine.h
- \brief Class that defines 2D line visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeatureLine
- \ingroup VsFeature2
-
- \brief Class that defines a 2D line visual feature \f$ s\f$ which is
- composed by two parameters that are \f$ \rho \f$ and \f$ \theta \f$,
- the polar coordinates of a line.
-
- In this class, the equation of the line in the image plane is given by :
- \f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f] Here
- \f$ x \f$ and \f$ y \f$ are the coordinates of a point belonging to
- the line and they are given in meter. The following image shows the
- meanings of the distance \f$\rho\f$ and the angle \f$\theta\f$.
-
- \image html vpFeatureLine.gif
- \image latex vpFeatureLine.ps width=10cm
-
- You have to note that the \f$ \theta \f$ angle has its value between
- \f$ -\pi \f$ and \f$ \pi \f$ and that the \f$ \rho \f$ distance can
- be positive or negative. The conventions are illustrated by the
- image above.
-
- The visual features can be set easily from an instance of the
- classes vpLine, vpMeLine or vpCylinder. For more precision see the
- class vpFeatureBuilder.
-
- Once the values of the visual features are set, the interaction()
- method allows to compute the interaction matrix \f$ L \f$ associated
- to the visual feature, while the error() method computes the error
- vector \f$(s - s^*)\f$ between the current visual feature and the
- desired one.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 2D line feature \f$(\rho,\theta)\f$ that correspond to
- the 2D equation of a line in the image plan. To control six
- degrees of freedom, at least four other features must be considered
- like two other line features for example. First we create a current
- (\f$s\f$) 2D line feature. Then we set the task to use the
- interaction matrix associated to the current feature \f$L_s\f$. And
- finally we compute the camera velocity \f$v=-\lambda \; L_s^+ \;
- (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
- loop.
-
- \code
-#include <visp/vpFeatureLine.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpFeatureLine sd; //The desired line feature.
- //Sets the desired features rho and theta
- double rhod = 0;
- double thetad = 0;
- //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
- //The line described by the features belongs to this plan.
- //Normally two plans are needed to describe a line. But to compute the interaction matrix only
- //one equation of the two plans is needed.
- //Notes that the Dd value must not be equal to zero !
- double Ad = 0;
- double Bd = 0;
- double Cd = 1;
- double Dd = -1;
- //Set the line feature thanks to the desired parameters.
- sd.buildfrom(rhod, thetad, Ad,Bd, Cd, Dd);
-
- vpFeatureLine s; //The current line feature.
- //Sets the current features rho and theta
- double rho; //You have to compute the value of rho.
- double theta; //You have to compute the value of theta.
- //Set the line feature thanks to the current parameters.
- s.buildfrom(rho, theta);
- //In this case the parameters A, B, C, D are not needed because the interaction matrix is computed
- //with the desired visual feature.
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the desired visual features sd
- task.setInteractionMatrixType(vpServo::DESIRED);
-
- // Add the 2D line feature to the task
- task.addFeature(s, sd);
-
- // Control loop
- for ( ; ; ) {
- // The new parameters rho and theta must be computed here.
-
- // Update the current line visual feature
- s.buildfrom(rho, theta);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
- return 0;
-}
- \endcode
-
- If you want to build your own control law, this other example shows how to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D line visual feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeatureLine.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpFeatureLine sd; //The desired line feature.
- //Sets the desired features rho and theta
- double rhod = 0;
- double thetad = 0;
- //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
- double Ad = 0;
- double Bd = 0;
- double Cd = 1;
- double Dd = -1;
- //Set the line feature thanks to the desired parameters.
- sd.buildfrom(rhod, thetad, Ad,Bd, Cd, Dd);
-
- vpFeatureLine s; //The current line feature.
- //Sets the current features rho and theta
- double rho; //You have to compute the value of rho.
- double theta; //You have to compute the value of theta.
- //Sets the parameters which describe the equation of a plan in the camera frame : AX+BY+CZ+D=0.
- double A; //You have to compute the value of A.
- double B; //You have to compute the value of B.
- double C; //You have to compute the value of C.
- double D; //You have to compute the value of D. D must not be equal to zero !
- //Set the line feature thanks to the current parameters.
- s.buildfrom(rho, theta, A, B, C, D);
-
- // Compute the interaction matrix L_s for the current line feature
- vpMatrix L = s.interaction();
- // You can also compute the interaction matrix L_s for the desired line feature
- // The corresponding line of code is : vpMatrix L = sd.interaction();
-
- // Compute the error vector (s-sd) for the line feature
- s.error(s_star);
-}
- \endcode
-
-*/
-class VISP_EXPORT vpFeatureLine : public vpBasicFeature
-{
- /*!
- attributes and members directly related to the vpBasicFeature needs
- other functionalities ar useful but not mandatory
- */
-private:
- //! FeatureLine depth (required to compute the interaction matrix)
- //! equation of a plane
- double A,B,C,D ;
-
-public:
- void init() ;
- vpFeatureLine() ;
- //! Destructor.
- virtual ~vpFeatureLine() {}
-
-public:
-
- void setRhoTheta(const double rho, const double theta) ;
- void setABCD(const double A, const double B,
- const double C, const double D) ;
-
- // void buildFrom(const vpLine &l) ;
- // void buildFrom(const vpCylinder &c, const int l) ;
- void buildFrom(const double rho, const double theta) ;
- void buildFrom(const double rho, const double theta,
- const double A, const double B,
- const double C, const double D) ;
-
-
- /*!
- Return the \f$ \rho \f$ subset value of the visual feature \f$ s \f$.
- */
- double getRho() const { return s[0] ; }
-
- /*!
- Return the \f$ \theta \f$ subset value of the visual feature \f$ s \f$.
- */
- double getTheta() const { return s[1] ; }
-
-
-public:
-
- /*!
-
- Function used to select the \f$ \rho \f$ subset of the line visual feature.
-
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \rho \f$.
-
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureLine s;
- vpServo task;
- ...
- // Add the (rho) subset features from the 2D line
- task.addFeature(s, vpFeatureLine::selectRho());
- \endcode
- */
- inline static unsigned int selectRho() { return FEATURE_LINE[0] ; }
-
- /*!
-
- Function used to select the \f$ \theta \f$ subset of the line visual feature.
-
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \theta \f$.
-
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureLine s;
- vpServo task;
- ...
- // Add the (rho) subset features from the 2D line
- task.addFeature(s, vpFeatureLine::selectTheta());
- \endcode
- */
- inline static unsigned int selectTheta() { return FEATURE_LINE[1] ; }
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
-
- //vpColVector error(const int select = FEATURE_ALL) ;
-
- void print(const unsigned int select= FEATURE_ALL) const ;
-
- vpFeatureLine *duplicate() const ;
-
-
-public:
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureLuminance.cpp b/src/visual-feature/vpFeatureLuminance.cpp
deleted file mode 100755
index b40dbe5..0000000
--- a/src/visual-feature/vpFeatureLuminance.cpp
+++ /dev/null
@@ -1,386 +0,0 @@
-
-
-#include <visp/vpMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpImageFilter.h>
-#include <visp/vpException.h>
-
-#include <visp/vpFeatureLuminance.h>
-
-
-/*!
- \file vpFeatureLuminance.cpp
- \brief Class that defines the image luminance visual feature
-
- For more details see \cite Collewet08c.
-*/
-
-
-
-/*!
- Initialize the memory space requested for vpFeatureLuminance visual feature.
-*/
-void
-vpFeatureLuminance::init()
-{
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value Z (1 meters)
- Z = 1;
-
- firstTimeIn =0 ;
-
- nbr = nbc = 0;
-}
-
-
-void
-vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
-{
- init() ;
-
- nbr = _nbr ;
- nbc = _nbc ;
-
- if((nbr < 2*bord) || (nbc < 2*bord)){
- throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
- }
-
- // number of feature = nb column x nb lines in the images
- dim_s = (nbr-2*bord)*(nbc-2*bord) ;
-
- s.resize(dim_s) ;
-
- if (pixInfo != NULL)
- delete [] pixInfo;
-
- pixInfo = new vpLuminance[dim_s] ;
-
- Z = _Z ;
-}
-
-/*!
- Default constructor that build a visual feature.
-*/
-vpFeatureLuminance::vpFeatureLuminance()
- : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
-{
- nbParameters = 1;
- dim_s = 0 ;
- flags = NULL;
-
- init() ;
-}
-
-/*!
- Copy constructor.
- */
-vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance& f)
- : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
-{
- *this = f;
-}
-
-/*!
- Copy operator.
- */
-vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance& f)
-{
- Z = f.Z;
- nbr = f.nbr;
- nbc = f.nbc;
- bord = f.bord;
- firstTimeIn = f.firstTimeIn;
- cam = f.cam;
- if (pixInfo)
- delete [] pixInfo;
- pixInfo = new vpLuminance[dim_s] ;
- for(unsigned int i=0; i< dim_s; i++)
- pixInfo[i] = f.pixInfo[i];
- return (*this);
-}
-
-/*!
- Destructor that free allocated memory.
-*/
-vpFeatureLuminance::~vpFeatureLuminance()
-{
- if (pixInfo != NULL) delete [] pixInfo ;
-}
-
-/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \param Z_ : \f$ Z \f$ value to set.
-*/
-void
-vpFeatureLuminance::set_Z(const double Z_)
-{
- this->Z = Z_ ;
- flags[0] = true;
-}
-
-
-/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \return The value of \f$ Z \f$.
-*/
-double
-vpFeatureLuminance::get_Z() const
-{
- return Z ;
-}
-
-
-void
-vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam)
-{
- cam = _cam ;
-}
-
-
-/*!
-
- Build a luminance feature directly from the image
-*/
-
-void
-vpFeatureLuminance::buildFrom(vpImage<unsigned char> &I)
-{
- unsigned int l = 0;
- double Ix,Iy ;
-
- double px = cam.get_px() ;
- double py = cam.get_py() ;
-
-
- if (firstTimeIn==0)
- {
- firstTimeIn=1 ;
- l =0 ;
- for (unsigned int i=bord; i < nbr-bord ; i++)
- {
- // cout << i << endl ;
- for (unsigned int j = bord ; j < nbc-bord; j++)
- { double x=0,y=0;
- vpPixelMeterConversion::convertPoint(cam,
- j,i,
- x,y) ;
-
- pixInfo[l].x = x;
- pixInfo[l].y = y;
-
- pixInfo[l].Z = Z ;
-
- l++;
- }
- }
- }
-
- l= 0 ;
- for (unsigned int i=bord; i < nbr-bord ; i++)
- {
- // cout << i << endl ;
- for (unsigned int j = bord ; j < nbc-bord; j++)
- {
- // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
- Ix = px * vpImageFilter::derivativeFilterX(I,i,j) ;
- Iy = py * vpImageFilter::derivativeFilterY(I,i,j) ;
-
- // Calcul de Z
-
- pixInfo[l].I = I[i][j] ;
- s[l] = I[i][j] ;
- pixInfo[l].Ix = Ix;
- pixInfo[l].Iy = Iy;
-
- l++;
- }
- }
-
-}
-
-
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L_I \f$. The computation is made
- thanks to the values of the luminance features \f$ I \f$
-*/
-void
-vpFeatureLuminance::interaction(vpMatrix &L)
-{
- double x,y,Ix,Iy,Zinv;
-
- L.resize(dim_s,6) ;
-
- for(unsigned int m = 0; m< L.getRows(); m++)
- {
- Ix = pixInfo[m].Ix;
- Iy = pixInfo[m].Iy;
-
- x = pixInfo[m].x ;
- y = pixInfo[m].y ;
- Zinv = 1 / pixInfo[m].Z;
-
- {
- L[m][0] = Ix * Zinv;
- L[m][1] = Iy * Zinv;
- L[m][2] = -(x*Ix+y*Iy)*Zinv;
- L[m][3] = -Ix*x*y-(1+y*y)*Iy;
- L[m][4] = (1+x*x)*Ix + Iy*x*y;
- L[m][5] = Iy*x-Ix*y;
- }
- }
-}
-
-/*!
- Compute and return the interaction matrix \f$ L_I \f$. The computation is made
- thanks to the values of the luminance features \f$ I \f$
-*/
-vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
-{
- /* static */ vpMatrix L ; // warning C4640: 'L' : construction of local static object is not thread-safe
- interaction(L) ;
- return L ;
-}
-
-
-/*!
- Compute the error \f$ (I-I^*)\f$ between the current and the desired
-
- \param s_star : Desired visual feature.
- \param e : Error between the current and the desired features.
-
-*/
-void
-vpFeatureLuminance::error(const vpBasicFeature &s_star,
- vpColVector &e)
-{
- e.resize(dim_s) ;
-
- for (unsigned int i =0 ; i < dim_s ; i++)
- {
- e[i] = s[i] - s_star[i] ;
- }
-}
-
-
-
-/*!
- Compute the error \f$ (I-I^*)\f$ between the current and the desired
-
- \param s_star : Desired visual feature.
- \param select : Not used.
-
-*/
-vpColVector
-vpFeatureLuminance::error(const vpBasicFeature &s_star,
- const unsigned int /* select */)
-{
- /* static */ vpColVector e ; // warning C4640: 'e' : construction of local static object is not thread-safe
-
- error(s_star, e) ;
-
- return e ;
-
-}
-
-
-
-
-/*!
-
- Not implemented.
-
- */
-void
-vpFeatureLuminance::print(const unsigned int /* select */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
- }
-
-
-
-/*!
-
- Not implemented.
-
- */
-void
-vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
- const vpImage<unsigned char> & /* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*!
-
- Not implemented.
-
- */
-void
-vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
- const vpImage<vpRGBa> & /* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-
-/*!
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureLuminance s;
- s_star = s.duplicate(); // s_star is now a vpFeatureLuminance
- \endcode
-
-*/
-vpFeatureLuminance *vpFeatureLuminance::duplicate() const
-{
- vpFeatureLuminance *feature = new vpFeatureLuminance ;
- return feature ;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureLuminance.h b/src/visual-feature/vpFeatureLuminance.h
deleted file mode 100755
index e5958ab..0000000
--- a/src/visual-feature/vpFeatureLuminance.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureLuminance.h 5098 2014-12-20 16:32:06Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Luninance based feature .
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-#ifndef vpFeatureLuminance_h
-#define vpFeatureLuminance_h
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpImage.h>
-
-
-/*!
- \file vpFeatureLuminance.h
- \brief Class that defines the image luminance visual feature
-
- For more details see \cite Collewet08c.
-*/
-
-
-/*!
- \class vpLuminance
- \brief Class that defines the luminance and gradient of a point
-
- \sa vpFeatureLuminance
-*/
-
-
-class VISP_EXPORT vpLuminance
-{
- public:
- double x, y; // point coordinates (in meter)
- double I ; // pixel intensity
- double Ix,Iy ; // pixel gradient
- double Z; // pixel depth
-
-};
-
-
-/*!
- \class vpFeatureLuminance
- \brief Class that defines the image luminance visual feature
-
- For more details see \cite Collewet08c.
-*/
-
-class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature
-{
- protected:
- //! FeaturePoint depth (required to compute the interaction matrix)
- //! default Z = 1m
- double Z ;
-
- //! Number of rows.
- unsigned int nbr ;
- //! Number of column.
- unsigned int nbc ;
- //! Border size.
- unsigned int bord ;
-
- //! Store the image (as a vector with intensity and gradient I, Ix, Iy)
- vpLuminance *pixInfo ;
- int firstTimeIn ;
-
- public:
- void buildFrom(vpImage<unsigned char> &I) ;
-
-public:
-
- void init() ;
- void init(unsigned int _nbr, unsigned int _nbc, double _Z) ;
-
- vpFeatureLuminance() ;
- vpFeatureLuminance(const vpFeatureLuminance& f) ;
- vpFeatureLuminance &operator=(const vpFeatureLuminance& f) ;
-
- //! Destructor.
- virtual ~vpFeatureLuminance() ;
-
- public:
- vpCameraParameters cam ;
- void setCameraParameters(vpCameraParameters &_cam) ;
- /*
- section Set/get Z
- */
-
-
- void set_Z(const double Z) ;
- double get_Z() const ;
-
-
- /*
- vpBasicFeature method instantiation
- */
-
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void interaction(vpMatrix &L);
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- void error(const vpBasicFeature &s_star,
- vpColVector &e) ;
-
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- vpFeatureLuminance *duplicate() const ;
-
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
-
- //! Compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-} ;
-
-
-
-#endif
diff --git a/src/visual-feature/vpFeatureMoment.cpp b/src/visual-feature/vpFeatureMoment.cpp
deleted file mode 100644
index 9b92dee..0000000
--- a/src/visual-feature/vpFeatureMoment.cpp
+++ /dev/null
@@ -1,276 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMoment.cpp 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Base for all moment features
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpMoment.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMath.h>
-
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-#include <visp/vpDebug.h>
-#include <vector>
-
-class vpBasicFeature;
-
-/*!
- Initialize common parameters for moment features.
-*/
-void vpFeatureMoment::init (){
- //feature dimension
- /*
- * The dimension of the visual feature is set according to the size of the vpMoment associated to it.
- * This partly explains why vpFeatureMomentBasic cannot be used directly as a visual feature.
- */
- if(this->moment!=NULL)
- dim_s = (unsigned int)this->moment->get().size();
- else
- dim_s = 0;
-
- nbParameters = 1;
-
- // memory allocation
- s.resize(dim_s) ;
- for(unsigned int i=0;i<dim_s;i++)
- s[i] = 0;
-
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = false;
-}
-
-
-/*!
- Feature's dimension according to selection.
-*/
-int vpFeatureMoment::getDimension (unsigned int select) const{
- int dim=0;
-
- for(unsigned int i=0;i<dim_s;++i)
- if(vpBasicFeature::FEATURE_LINE[i] & select)
- dim++;
-
- return dim;
-}
-
-
-/*!
- Outputs the content of the feature: it's corresponding selected moments.
-*/
-void vpFeatureMoment::print (unsigned int select) const{
- for(unsigned int i=0;i<dim_s;++i){
- if(vpBasicFeature::FEATURE_LINE[i] & select){
- std::cout << s[i] << ",";
- }
- }
-
- std::cout << std::endl;
-}
-
-/*!
- Not implemented since visual representation of a moment doesn't often make sense.
-*/
-void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
- const vpColor &color, unsigned int thickness) const
-{
- //visual representation of a moment doesn't often make sense
- (void)cam;
- (void)I;
- (void)color;
- (void)thickness;
-}
-
-/*!
- Not implemented since visual representation of a moment doesn't often make sense.
-*/
-void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
- const vpColor &color, unsigned int thickness) const
-{
- (void)cam;
- (void)I;
- (void)color;
- (void)thickness;
-}
-
-/*!
- Updates the interaction matrices with the image plane the camera is facing. The plane must be in the format: \f$ \frac{1}{Z}=Ax+By+C \f$ .
- The moment primitives MUST be updated before calling this function.
-
- This method also computes the interaction matrix. Therefore, you must call vpFeatureMoment::update before calling vpFeatureMoment::interaction.
-
- \attention The behaviour of this method is not the same as vpMoment::update which only acknowledges the new object. This method also computes the interaction matrices.
-
- \param A_ : A coefficient of the plane.
- \param B_ : B coefficient of the plane.
- \param C_ : C coefficient of the plane.
-*/
-void vpFeatureMoment::update (double A_, double B_, double C_){
- this->A = A_;
- this->B = B_;
- this->C = C_;
-
- if(moment==NULL){
- bool found;
- this->moment = &(moments.get(momentName(),found));
- if(!found) throw vpException(vpException::notInitialized,"Moment not found for feature");
- }
- nbParameters = 1;
- if(this->moment!=NULL){
- dim_s = (unsigned int)this->moment->get().size();
-
- s.resize(dim_s);
-
- for(unsigned int i=0;i<dim_s;i++)
- s[i] = this->moment->get()[i];
-
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = false;
- }else
- dim_s = 0;
-
- compute_interaction();
-}
-
-/*!
- Retrieves the interaction matrix. No computation is done.
-
- \param select : Feature selector.
-
- \return The corresponding interaction matrix.
-
- There is no rule about the format of the feature selector. It may be
- different for different features. For example, for
- vpFeatureMomentBasic or vpFeatureMomentCentered features, select may
- refer to the \f$ (i,j) \f$ couple in the \f$ j \times order + i \f$
- format, but for vpFeatureMomentCInvariant the selector allows to
- select couples \f$ (i,j,k,l...) \f$ in the following format: 1 << i
- + 1 << j + 1 << k + 1 << l.
-*/
-vpMatrix vpFeatureMoment::interaction (unsigned int select){
- vpMatrix L(0,0);
-
- for(unsigned int i=0;i<dim_s;++i){
- if(vpBasicFeature::FEATURE_LINE[i] & select){
- L.stackMatrices(interaction_matrices[i]);
- }
- }
-
- return L;
-}
-
-/*! Duplicates the feature into a vpGenericFeature harbouring the
- same properties. The resulting feature is of vpMomentGenericFeature
- type. While it still can compute interaction matrices and has acces
- to it's moment primitive, it has lost all precise information about
- its precise type and therefore cannot be used in a feature database.
-
- \return The corresponding feature.
-*/
-vpBasicFeature* vpFeatureMoment::duplicate () const
-{
- vpFeatureMoment* feat = new vpMomentGenericFeature(moments,A,B,C,featureMomentsDataBase,moment);
- feat->interaction_matrices = interaction_matrices;
- feat->dim_s = dim_s;
- feat->nbParameters = nbParameters;
- // memory allocation
- feat->s.resize(dim_s) ;
- for(unsigned int i=0;i<dim_s;i++)
- feat->s[i] = this->s[i];
-
- feat->flags = new bool[(unsigned int)nbParameters];
- for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
- feat->flags[i] = flags[i];
-
- return feat;
-}
-
-/*!
- Links the feature to the feature's database. NB: The feature's database is different from the moment's database.
- \param featureMoments : database in which the moment features are stored.
-
-*/
-void vpFeatureMoment::linkTo(vpFeatureMomentDatabase& featureMoments){
- if (strlen( name() ) >= 255) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the moment name"));
- }
-
- std::strcpy(_name,name());
- this->featureMomentsDataBase=&featureMoments;
-
- featureMoments.add(*this,_name);
-}
-
-
-void vpFeatureMoment::compute_interaction (){
-
-}
-
-vpFeatureMoment::~vpFeatureMoment (){
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM) {
- /*
- A const_cast is forced here since interaction() defined in vpBasicFeature() is not const
- But introducing const in vpBasicFeature() can break a lot of client code
- */
- vpMatrix Lcomplete((unsigned int)featM.getDimension(), 6); // 6 corresponds to 6velocities in standard interaction matrix
- Lcomplete = const_cast<vpFeatureMoment&>(featM).interaction(vpBasicFeature::FEATURE_ALL);
- Lcomplete.matlabPrint(os);
- return os;
-}
-
-/*!
-Interface function to display the moments and other interaction matrices
-on which a particular vpFeatureMoment is dependent upon
-Not made pure to maintain compatibility
-Recommended : Types inheriting from vpFeatureMoment should implement this function
-*/
-void
-vpFeatureMoment::printDependencies(std::ostream& os) const{
- os << " WARNING : Falling back to base class version of printDependencies() in vpFeatureMoment. To prevent that, this has to be implemented in the derived classes!" << std::endl;
-}
diff --git a/src/visual-feature/vpFeatureMoment.h b/src/visual-feature/vpFeatureMoment.h
deleted file mode 100644
index fcf9bbe..0000000
--- a/src/visual-feature/vpFeatureMoment.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/****************************************************************************
-*
-* $Id: vpFeatureMoment.h 4714 2014-03-28 18:16:13Z mbakthav $
-*
-* This file is part of the ViSP software.
-* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-*
-* This software is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* ("GPL") version 2 as published by the Free Software Foundation.
-* 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-*
-* This software was developed at:
-* INRIA Rennes - Bretagne Atlantique
-* Campus Universitaire de Beaulieu
-* 35042 Rennes Cedex
-* France
-* http://www.irisa.fr/lagadic
-*
-* If you have questions regarding the use of this file, please contact
-* INRIA at visp at 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:
-* Base for all moment features
-*
-* Authors:
-* Filip Novotny
-*
-*****************************************************************************/
-/*!
-\file vpFeatureMoment.h
-\brief Base class for moment features.
-
-Handles common system operations like selection, duplication. Functionality is computed in derived classes.
-*/
-#ifndef __FEATUREMOMENT_H__
-#define __FEATUREMOMENT_H__
-
-#include <visp/vpConfig.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpException.h>
-#include <vector>
-
-class vpMomentObject;
-class vpMomentDatabase;
-class vpFeatureMomentDatabase;
-class vpMoment;
-
-/*!
-\class vpFeatureMoment
-
-\ingroup VsFeature2
-
-\brief This class defines shared system methods/attributes for 2D moment features but no functional code.
-It is used to compute interaction matrices for moment features.
-
-While vpMoment-type classes do only compute moment values and can by used for almost anything, vpFeatureMoment-type classes
-are specifically designed for visual servoing. More importantly, a vpFeatureMoment is used to compute the interaction matrix associated to it's moment primitive.
-
-This class is virtual and cannot be used directly. It defines the following characteristics common to all moment features:
-- Plane orientation parameters (A,B,C):
-Each camera frame corresponds to a physical planar object contained in a plane. This plane's equation has the following form:
-\f$ A \times x+B \times y + C = \frac{1}{Z} \f$.
-These parameters can be updated anytime.
-- Get corresponding moment primitive: for example a vpFeatureMomentCInvariant will provide access to a vpMomentCInvariant instance.
-- Provide access to a feature database (vpFeatureMomentDatabase).
-- All interaction matrices (different from vpBasicFeature::interaction which selects the required interaction matrix).
-
-Like vpMoment, vpFeatureMoment provides a vpFeatureMoment::update() method. But unlike vpMoment::update() which only acknowledges the new object,
-the vpFeatureMoment::update() acknowledges the new plane parameters AND computes the interaction matrices associated with the feature.
-
-A vpFeatureMoment will be often part of a vpFeatureMomentDatabase in the same way a vpMoment is part of a vpMomentDatabase. This database is specified inside the
-vpFeatureMoment::vpFeatureMoment() constructor.
-As a result, a vpFeatureMoment will be able to access other vpFeatureMoments through this database.
-
-A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
-is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
-
-Note that you can use vpFeatureMoment to do visual servoing but it is not it's only purpose. You may compute your interaction matrices with vpFeatureMoment::update()
-and use them for any purpose.
-
-\attention - A vpFeatureMoment is not responsible for updating the moment primitives it depends on. Make sure your vpMoments are all up to date before computing an interaction matrix using vpFeatureMoment.
-
-\attention - Be careful with orders. Often, computing a feature of order n requires vpMoment primitives of order n+1.
-Make sure to check the documentation of the specialised vpFeatureMoment classes when deciding to which order you want to initialize the object.
-An object of order 6 should be sufficient for all classic implementations of vpFeatureMoment.
-
-Here is an example of how to use a vpFeatureMoment (in this case vpFeatureMomentBasic).
-\code
-#include <visp/vpPoint.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpFeatureMomentBasic.h>
-
-int main()
-{
-vpPoint p;
-std::vector<vpPoint> vec_p; // vector that contains the vertices
-
-p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
-vec_p.push_back(p);
-p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
-vec_p.push_back(p);
-
-//////////////////////////////REFERENCE VALUES////////////////////////////////
-vpMomentObject obj(3); // Init object of order 3 because we need
-// vpFeatureMomentBasic of order 2 which
-// implies third-order moment primitives
-obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
-obj.fromVector(vec_p);
-
-vpMomentDatabase mdb; //database for moment primitives. This will
-//only contain the basic moment.
-vpMomentBasic bm; //basic moment (this particular moment is nothing
-//more than a shortcut to the vpMomentObject)
-bm.linkTo(mdb); //add basic moment to moment database
-
-vpFeatureMomentBasic fmb(mdb,0,0,1,NULL);
-
-//update and compute the vpMoment BEFORE doing any operations with
-//vpFeatureMoment
-bm.update(obj);
-bm.compute();
-
-fmb.update(0,0,1); //update the vpFeatureMoment with a plane
-//configuration
-std::cout << fmb.interaction(1,1) << std::endl;
-
-return 0;
-}
-\endcode
-*/
-class VISP_EXPORT vpFeatureMoment : public vpBasicFeature{
-protected:
- const vpMoment* moment;
- const vpMoment& getMoment() const {return *moment;}
- vpMomentDatabase& moments;
- vpFeatureMomentDatabase* featureMomentsDataBase;
- std::vector<vpMatrix> interaction_matrices;
-
- double A;
- double B;
- double C;
- char _name[255];
-
-public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane, feature database and matrix size.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database
- \param nbmatrices : If you want to create a new vpFeatureMoment implementation, your feature will often have a matrix size of n lines. You can specify the number of lines by this parameter.
- */
- vpFeatureMoment(vpMomentDatabase& data_base,double A_=0.0, double B_=0.0, double C_=0.0,
- vpFeatureMomentDatabase* featureMoments=NULL,unsigned int nbmatrices=1)
- : moment(NULL),
- moments(data_base),
- featureMomentsDataBase(featureMoments),
- interaction_matrices(nbmatrices),
- A(A_),B(B_),C(C_) {}
- virtual ~vpFeatureMoment();
-
- virtual void compute_interaction (void);
- vpBasicFeature* duplicate () const;
- void display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
- void display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
-
- int getDimension (unsigned int select=FEATURE_ALL) const;
- void init (void);
- vpMatrix interaction (const unsigned int select=FEATURE_ALL) ;
- void linkTo(vpFeatureMomentDatabase& featureMoments);
-
- /*!
- Name of the moment corresponding to the feature. This allows to locate the moment
- associated with the feature in the provided database.
- */
- virtual const char* momentName() const = 0;
- /*!
- Name of the feature used to locate it in the database of features.
- */
- virtual const char* name() const = 0;
- void print (const unsigned int select=FEATURE_ALL) const ;
-
- void update (double A, double B, double C);
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- vpFeatureMoment& operator=(const vpFeatureMoment &){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
-
- friend VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM);
- virtual void printDependencies(std::ostream& os) const;
-};
-
-/*!
-\class vpMomentGenericFeature
-
-\ingroup VsFeature2
-
-\brief This class defines a generic feature used for moment feature duplication.
-
-A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
-is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
-The reason for this is that a vpMomentGenericFeature can refer to anything therefore it has no specific name.
-
-Duplication is mostly used internally in ViSP.
-
-*/
-class VISP_EXPORT vpMomentGenericFeature : public vpFeatureMoment{
-public:
- vpMomentGenericFeature(vpMomentDatabase& data_base,double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments, const vpMoment* p_moment)
- : vpFeatureMoment(data_base,A_,B_,C_,featureMoments){this->moment = p_moment;}
- /*!
- No specific moment name.
- */
- const char* momentName() const { return NULL;}
- /*!
- No specific feature name.
- */
- virtual const char* name() const { return NULL;}
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentAlpha.cpp b/src/visual-feature/vpFeatureMomentAlpha.cpp
deleted file mode 100644
index 9645e49..0000000
--- a/src/visual-feature/vpFeatureMomentAlpha.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for alpha moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpFeatureMomentAlpha.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentDatabase.h>
-
-#include <vector>
-#include <limits>
-
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-
-/*!
- Computes interaction matrix for alpha moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
-*/
-void vpFeatureMomentAlpha::compute_interaction(){
- bool found_moment_centered;
- bool found_FeatureMoment_centered;
-
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- double multiplier = -1./(momentCentered.get(2,0)*momentCentered.get(2,0)-2*momentCentered.get(0,2)*momentCentered.get(2,0)+4*momentCentered.get(1,1)*momentCentered.get(1,1)+momentCentered.get(0,2)*momentCentered.get(0,2));
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[0] = multiplier*(momentCentered.get(1,1)*featureMomentCentered.interaction(2,0)+(momentCentered.get(0,2)-momentCentered.get(2,0))*featureMomentCentered.interaction(1,1)-momentCentered.get(1,1)*featureMomentCentered.interaction(0,2));
-}
-
-#else
-
-/*!
- Computes interaction matrix for alpha moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpMomentGravityCenter
-*/
-void vpFeatureMomentAlpha::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentObject& momentObject = moment->getObject();
-
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- double mu11 = momentCentered.get(1,1);
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu12 = momentCentered.get(1,2);
- double mu21 = momentCentered.get(2,1);
- double mu03 = momentCentered.get(0,3);
- double mu30 = momentCentered.get(3,0);
-
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double Avx,Avy,Avz,Awx,Awy;
- double beta,gamma;
-
- if (momentObject.getType()==vpMomentObject::DISCRETE){
- beta = 4;
- gamma = 2;
- } else {
- beta = 5;
- gamma = 1;
- }
-
- double d = (mu20-mu02)*(mu20-mu02) + 4 * mu11*mu11;
- double DA = mu20+mu02;
- double DA_2 = DA*DA;
- double mu11_2 = mu11*mu11;
-
- Avx = mu11*DA*A/d+(DA*mu02+(0.5)*d-(0.5)*DA_2)*B/d;
- Avy = (DA*mu02-(0.5)*d-(.5)*DA_2)*A/d-B*mu11*DA/d;
-
- Awx = (beta*(mu12*(mu20-mu02)+mu11*(mu03-mu21))+gamma*Xg*(mu02*(mu20-mu02)-2*mu11_2)+gamma*Yg*mu11*(mu20+mu02))/d;
- Awy = (beta*(mu21*(mu02-mu20)+mu11*(mu30-mu12))+gamma*Xg*mu11*(mu20+mu02)+gamma*Yg*(mu20*(mu02-mu20)-2*mu11_2))/d;
-
- Avz = B*Awx-A*Awy;
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Avx;
- interaction_matrices[0][0][VY] = Avy;
- interaction_matrices[0][0][VZ] = Avz;
-
- interaction_matrices[0][0][WX] = Awx;
- interaction_matrices[0][0][WY] = Awy;
- interaction_matrices[0][0][WZ] = -1.;
-}
-
-vpColVector vpFeatureMomentAlpha::error (const vpBasicFeature &s_star, const unsigned int /* select */){
- vpColVector e(0) ;
- double err = s[0] - s_star[0] ;
-
- if (err < -M_PI) err += 2*M_PI ;
- if (err > M_PI) err -= 2*M_PI ;
-
- vpColVector ecv(1) ;
- ecv[0] = err ;
- e = vpMatrix::stackMatrices(e,ecv) ;
-
- return e;
-}
-#endif
diff --git a/src/visual-feature/vpFeatureMomentAlpha.h b/src/visual-feature/vpFeatureMomentAlpha.h
deleted file mode 100644
index 40faae3..0000000
--- a/src/visual-feature/vpFeatureMomentAlpha.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentAlpha.h
- \brief Implementation of the interaction matrix computation for vpMomentAlpha.
-*/
-
-#ifndef __FEATUREMOMENTALPHA_H__
-#define __FEATUREMOMENTALPHA_H__
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpColVector.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-
-
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentAlpha
-
- \ingroup VsFeature2
-
- \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$. Computes the interaction matrix associated with vpMomentAlpha.
-
- The interaction matrix for the feature can be deduced from \cite Tahri05z.
-
- This class allows to compute the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
-
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
-
- This feature is often used in moment-based visual servoing to control the planar rotation parameter.
-
- Minimum vpMomentObject order needed to compute this feature: 4.
-
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered.
-*/
-class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAlpha(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(moments,A,B,C,featureMoments,1)
- {}
-
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAlpha";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAlpha";}
-};
-#else
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentAlpha
-
- \ingroup VsFeature2
-
- \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$: computes the interaction matrix associated with vpMomentAlpha.
-
- This class computes the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
-
- The interaction matrix for the feature has the following form:
- \f[{
- \left[ \begin {array}{c} {\frac {\mu_{{1,1}}{\it DA}\,A}{d}}+{\frac {
- \left( {\it DA}\,\mu_{{0,2}}+1/2\,d-1/2\,{{\it DA}}^{2} \right) B}{d}
-}\\ \noalign{\medskip}{\frac { \left( {\it DA}\,\mu_{{0,2}}-1/2\,d-1/2
-\,{{\it DA}}^{2} \right) A}{d}}-{\frac {B\mu_{{1,1}}{\it DA}}{d}}
-\\ \noalign{\medskip}Bw_{{x}}-Aw_{{y}}\\ \noalign{\medskip}{\frac {
-\beta\, \left( \mu_{{1,2}} \left( \mu_{{2,0}}-\mu_{{0,2}} \right) +\mu
-_{{1,1}} \left( \mu_{{0,3}}-\mu_{{2,1}} \right) \right) +\gamma\,x_{{
-g}} \left( \mu_{{0,2}} \left( \mu_{{2,0}}-\mu_{{0,2}} \right) -2\,{\mu
-_{{1,1}}}^{2} \right) +\gamma\,y_{{g}}\mu_{{1,1}} \left( \mu_{{2,0}}+
-\mu_{{0,2}} \right) }{d}}\\ \noalign{\medskip}{\frac {\beta\, \left(
-\mu_{{2,1}} \left( \mu_{{0,2}}-\mu_{{2,0}} \right) +\mu_{{1,1}}
- \left( \mu_{{3,0}}-\mu_{{1,2}} \right) \right) +\gamma\,x_{{g}}\mu_{
-{1,1}} \left( \mu_{{2,0}}+\mu_{{0,2}} \right) +\gamma\,y_{{g}} \left(
-\mu_{{2,0}} \left( \mu_{{0,2}}-\mu_{{2,0}} \right) -2\,{\mu_{{1,1}}}^{
-2} \right) }{d}}\\ \noalign{\medskip}-1\end {array} \right]
-}^t
-\f]
-with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} = DA^2+4{\mu_{1,1}}^2\f$.
-
- - In the discrete case:
- \f$beta = 4\f$,\f$gamma = 2\f$.
- - In the dense case:
- \f$beta = 5\f$,\f$gamma = 1\f$.
-
-
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
-
- This feature is often used in moment-based visual servoing to control the planar rotation parameter.
-
- Minimum vpMomentObject order needed to compute this feature: 4.
-
- This feature depends on:
- - vpMomentCentered
- - vpMomentGravityCenter
-*/
-class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAlpha(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1)
- {}
-
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentAlpha";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentAlpha";}
-
- vpColVector error (const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL);
-};
-#endif
-#endif
diff --git a/src/visual-feature/vpFeatureMomentArea.cpp b/src/visual-feature/vpFeatureMomentArea.cpp
deleted file mode 100644
index 175f922..0000000
--- a/src/visual-feature/vpFeatureMomentArea.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentArea.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation of vpFeatureMomentArea associated to vpMomentArea
- *
- * Authors:
- * Manikandan Bakthavatchalam
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-#include <vector>
-#include <limits>
-#include <cassert>
-#include <limits> // numeric_limits
-
-#include <visp/vpFeatureMomentArea.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentArea.h>
-#include <visp/vpFeatureMomentDatabase.h>
-
-/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentGravityCenter
- - vpMomentArea
-*/
-void vpFeatureMomentArea::compute_interaction(){
-
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
-
- // Retreive the moment object associated with this feature
- vpMomentObject mobj = moment->getObject();
- if (mobj.getType()==vpMomentObject::DISCRETE) {
- /*
- * The interaction matrix for the discrete case is zero
- * since the feature m00 is constant.
- * Refer thesis of Omar Tahri 2005 [Section 3.4.22]
- */
- interaction_matrices[0][0][0] = 0.;
- interaction_matrices[0][0][1] = 0.;
- interaction_matrices[0][0][2] = 0.;
- interaction_matrices[0][0][3] = 0.;
- interaction_matrices[0][0][4] = 0.;
- interaction_matrices[0][0][5] = 0.;
- }
- else {
- // Get Xg and Yg
- bool found_xgyg;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_xgyg));
- if (!found_xgyg) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- bool found_m00;
- const vpMomentArea& areamoment = static_cast<const vpMomentArea&>(moments.get("vpMomentArea", found_m00));
- if (!found_m00) throw vpException(vpException::notInitialized,"vpMomentArea not found");
-
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double a = areamoment.get()[0]; // Area scalar
-
- assert(std::fabs(a-mobj.get(0,0)) < a*std::numeric_limits<double>::epsilon());
-
- interaction_matrices[0][0][0] = -a*A;
- interaction_matrices[0][0][1] = -a*B;
- interaction_matrices[0][0][2] = (3*a)*(A*Xg+B*Yg)+(2*C*a);
- interaction_matrices[0][0][3] = 3*a*Yg;
- interaction_matrices[0][0][4] = -3*a*Xg;
- interaction_matrices[0][0][5] = 0.;
- }
-}
diff --git a/src/visual-feature/vpFeatureMomentArea.h b/src/visual-feature/vpFeatureMomentArea.h
deleted file mode 100644
index 1d95aac..0000000
--- a/src/visual-feature/vpFeatureMomentArea.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentArea.h 3317 2011-09-06 14:14:47Z mbakthav $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Definition of vpFeatureMomentArea associated to vpMomentArea
- *
- * Authors:
- * Manikandan Bakthavatchalam
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentArea.h
- \brief Implementation of the interaction matrix computation for vpMomentArea.
-*/
-#ifndef __FEATUREMOMENTAREA_H__
-#define __FEATUREMOMENTAREA_H__
-#include <visp/vpFeatureMoment.h>
-
-class vpMomentDatabase;
-
-/*!
- \class vpFeatureMomentArea
-
- \ingroup VsFeature2
-
- \brief Surface moment feature. Computes the interaction matrix associated with vpMomentArea.
-
-*/
-
-class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentArea(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
-
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentArea";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentArea";}
-
-};
-#endif
diff --git a/src/visual-feature/vpFeatureMomentAreaNormalized.cpp b/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
deleted file mode 100644
index 5150af6..0000000
--- a/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-#include <visp/vpConfig.h>
-
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpFeatureMomentAreaNormalized.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentDatabase.h>
-
-
-/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentAreaNormalized
- - vpFeatureMomentBasic
-*/
-void vpFeatureMomentAreaNormalized::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_surface_normalized;
- bool found_FeatureMoment_centered;
-
- bool found_featuremoment_basic;
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
- double normalized_multiplier;
- double a;
- vpMatrix La;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2);
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
-
- normalized_multiplier = (-momentSurfaceNormalized.getDesiredDepth()/(2*a))*sqrt(momentSurfaceNormalized.getDesiredSurface()/a);
- interaction_matrices[0] = normalized_multiplier*La;
-
-}
-
-#else
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMomentAreaNormalized.h>
-
-/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpMomentAreaNormalized
- - vpMomentGravityCenter
-*/
-void vpFeatureMomentAreaNormalized::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_surface_normalized;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
-
- if (!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if (!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if (!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double An=momentSurfaceNormalized.get()[0];
-
- double Xn = An*Xg;
- double Yn = An*Yg;
-
- double Anvx,Anvy,Anvz,Anwx,Anwy;
-
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- double a = momentCentered.get(2,0)+momentCentered.get(0,2);
-
- double e01 = momentCentered.get(0,1)/a;
- double e10 = momentCentered.get(1,0)/a;
- double e11 = momentCentered.get(1,1)/a;
- double e02 = momentCentered.get(0,2)/a;
- double e20 = momentCentered.get(2,0)/a;
- double e12 = momentCentered.get(1,2)/a;
- double e21 = momentCentered.get(2,1)/a;
- double e03 = momentCentered.get(0,3)/a;
- double e30 = momentCentered.get(3,0)/a;
-
- Anvx = An*A*e20+An*B*e11;
- Anvy = An*A*e11+An*B*e02;
-
- Anwx = (n02*e01+n11*e10-e03-e21)*An-Xn*e11+(-1-e02)*Yn;
- Anwy = (e12+e30-n11*e01-n20*e10)*An+(2-e02)*Xn+Yn*e11;
-
- Anvz = -An*C+B*Anwx-A*Anwy;
-
- } else {
- Anvx = A*An/2.;
- Anvy = B*An/2.;
- Anvz = -An*C-(3./2.)*A*Xn-(3./2.)*B*Yn;
-
- Anwx = -(3./2.)*Yn;
- Anwy = (3./2.)*Xn;
- }
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Anvx;
- interaction_matrices[0][0][VY] = Anvy;
- interaction_matrices[0][0][VZ] = Anvz;
-
- interaction_matrices[0][0][WX] = Anwx;
- interaction_matrices[0][0][WY] = Anwy;
- interaction_matrices[0][0][WZ] = 0.;
-}
-
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentAreaNormalized.h b/src/visual-feature/vpFeatureMomentAreaNormalized.h
deleted file mode 100644
index 7004fe3..0000000
--- a/src/visual-feature/vpFeatureMomentAreaNormalized.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentAreaNormalized.h
- \brief Implementation of the interaction matrix computation for vpMomentAreaNormalized.
-*/
-#ifndef __FEATUREMOMENTAREANORMALIZED_H__
-#define __FEATUREMOMENTAREANORMALIZED_H__
-#include <visp/vpFeatureMoment.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-class vpMomentDatabase;
-
-/*!
- \class vpFeatureMomentAreaNormalized
-
- \ingroup VsFeature2
-
- \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
-
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z.
-
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ a_n \f$.
-
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
-
- This feature is often used in moment-based visual servoing to control the depth parameter.
-
- Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
-
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentAreaNormalized
- - vpFeatureMomentBasic
-
-*/
-class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAreaNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,1){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAreaNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAreaNormalized";}
-
-};
-
-#else
-class vpMomentDatabase;
-
-/*!
- \class vpFeatureMomentAreaNormalized
-
- \ingroup VsFeature2
-
- \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
-
- The interaction matrix for the moment has the following form:
- - In the discrete case:
- \f[
- L_{a_n} =
-{
- \left[ \begin {array}{c} a_{{n}}Ae_{{2,0}}+a_{{n}}Be_{{1,1}}
-\\ \noalign{\medskip}a_{{n}}Ae_{{1,1}}+a_{{n}}Be_{{0,2}}
-\\ \noalign{\medskip}-a_{{n}}C+Bw_{{x}}-Aw_{{y}}\\ \noalign{\medskip}-
- \left( e_{{2,0}}+2\,e_{{0,2}} \right) y_{{g}}-e_{{2,1}}-x_{{g}}e_{{1,
-1}}+\eta_{{1,1}}e_{{1,0}}-e_{{0,3}}+\eta_{{0,2}}e_{{0,1}}
-\\ \noalign{\medskip} \left( 2\,e_{{2,0}}+e_{{0,2}} \right) x_{{g}}+e_
-{{3,0}}+y_{{g}}e_{{1,1}}-\eta_{{2,0}}e_{{1,0}}+e_{{1,2}}-\eta_{{1,1}}e
-_{{0,1}}\\ \noalign{\medskip}0\end {array} \right]
-
-}^t
-
- \f]
- - In the dense case:
- \f[
- L_{a_n} =
-{
- \left[ \begin {array}{c} 1/2\,a_{{n}}A\\ \noalign{\medskip}1/2\,a_{{n
-}}B\\ \noalign{\medskip}-a_{{n}}C-3/2\,Ax_{{n}}-3/2\,By_{{n}}
-\\ \noalign{\medskip}-3/2\,y_{{n}}\\ \noalign{\medskip}3/2\,x_{{n}}
-\\ \noalign{\medskip}0\end {array} \right]
-
-}^t
-
- \f]
-with:
- - \f$e_{i,j}=\frac{\mu_{i,j}}{NA}\f$
- - \f$NA=\mu_{2,0}+\mu_{0,2}\f$
- - \f$\eta\f$ is the centered and normalized moment.
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ a_n \f$.
-
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
-
- This feature is often used in moment-based visual servoing to control the depth parameter.
-
- Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
-
- This feature depends on:
- - vpMomentCentered
- - vpMomentAreaNormalized
- - vpMomentGravityCenter
-
-*/
-class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAreaNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAreaNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAreaNormalized";}
-
-};
-#endif
-#endif
diff --git a/src/visual-feature/vpFeatureMomentBasic.cpp b/src/visual-feature/vpFeatureMomentBasic.cpp
deleted file mode 100644
index 08ef40e..0000000
--- a/src/visual-feature/vpFeatureMomentBasic.cpp
+++ /dev/null
@@ -1,156 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <vector>
-#include <limits>
-/*!
- Default constructor.
- \param data_base : Database of moment primitives.
- \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param featureMoments : Database of features.
-*/
-vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments), order(0)
-{
-}
-
-/*!
- Computes interaction matrix for basic moment. Called internally.
- The moment primitives must be computed before calling this.
-*/
-void vpFeatureMomentBasic::compute_interaction(){
- int delta;
- const vpMomentObject& m = moment->getObject();
- order = m.getOrder()+1;
- interaction_matrices.resize(order*order);
- for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end();i++)
- i->resize(1,6);
- if (m.getType()==vpMomentObject::DISCRETE){
- delta=0;
- } else {
- delta=1;
- }
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- //i=0;j=0
- interaction_matrices[0][0][VX] = -delta*A*m.get(0, 0);
- interaction_matrices[0][0][VY] = -delta*B*m.get(0, 0);
- interaction_matrices[0][0][VZ] = 3*delta*(A*m.get(1, 0)+B*m.get(0, 1)+C*m.get(0, 0))-delta*C*m.get(0, 0);
-
- interaction_matrices[0][0][WX] = 3*delta*m.get(0, 1);
- interaction_matrices[0][0][WY] = -3*delta*m.get(1, 0);
- interaction_matrices[0][0][WZ] = 0;
-
- // int i=0;
- for(int j=1;j<(int)order-1;j++){
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
-
- interaction_matrices[j_*order][0][VX] = -delta*A*m.get(0, j_);
- interaction_matrices[j_*order][0][VY] = -j*(A*m.get(1,jm1_)+B*m.get(0,j_)+C*m.get(0,jm1_))-delta*B*m.get(0,j_);
- interaction_matrices[j_*order][0][VZ] = (j+3*delta)*(A*m.get(1,j_)+B*m.get(0,jp1_)+C*m.get(0,j_))-delta*C*m.get(0,j_);
-
- interaction_matrices[j_*order][0][WX] = (j+3*delta)*m.get(0,jp1_)+j*m.get(0,jm1_);
- interaction_matrices[j_*order][0][WY] = -(j+3*delta)*m.get(1,j_);
- interaction_matrices[j_*order][0][WZ] = -j*m.get(1,jm1_);
- }
-
- //int j=0;
- for(int i=1;i<(int)order-1;i++){
- unsigned int i_ = (unsigned int) i;
- unsigned int im1_ = i_ - 1;
- unsigned int ip1_ = i_ + 1;
-
- interaction_matrices[i_][0][VX] = -i*(A*m.get(i_, 0)+B*m.get(im1_, 1)+C*m.get(im1_, 0))-delta*A*m.get(i_, 0);
- interaction_matrices[i_][0][VY] = -delta*B*m.get(i_, 0);
- interaction_matrices[i_][0][VZ] = (i+3*delta)*(A*m.get(ip1_, 0)+B*m.get(i_, 1)+C*m.get(i_, 0))-delta*C*m.get(i_, 0);
-
- interaction_matrices[i_][0][WX] = (i+3*delta)*m.get(i_, 1);
- interaction_matrices[i_][0][WY] = -(i+3*delta)*m.get(ip1_, 0)-i*m.get(im1_, 0);
- interaction_matrices[i_][0][WZ] = i*m.get(im1_, 1);
- }
-
- for(int j=1; j<(int)order-1; j++){
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
-
- for(int i=1; i<(int)order-j-1; i++){
- unsigned int i_ = (unsigned int) i;
- unsigned int im1_ = i_ - 1;
- unsigned int ip1_ = i_ + 1;
-
- interaction_matrices[j_*order+i_][0][VX] = -i*(A*m.get(i_, j_)+B*m.get(im1_, jp1_)+C*m.get(im1_,j_))-delta*A*m.get(i_, j_);
- interaction_matrices[j_*order+i_][0][VY] = -j*(A*m.get(ip1_, jm1_)+B*m.get(i_, j_)+C*m.get(i_,jm1_))-delta*B*m.get(i_, j_);
- interaction_matrices[j_*order+i_][0][VZ] = (i+j+3*delta)*(A*m.get(ip1_, j_)+B*m.get(i_,jp1_)+C*m.get(i_, j_))-delta*C*m.get(i_,j_);
-
- interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*m.get(i_, jp1_)+j*m.get(i_, jm1_);
- interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*m.get(ip1_, j_)-i*m.get(im1_, j_);
- interaction_matrices[j_*order+i_][0][WZ] = i*m.get(im1_,jp1_)-j*m.get(ip1_, jm1_);
- }
- }
-}
-
-/*!
-Interaction matrix corresponding to \f$ m_{ij} \f$ moment.
-\param select_one : first index (i).
-\param select_two : second index (j).
-\return Interaction matrix \f$ L_{m_{ij}} \f$ corresponding to the moment.
-*/
-vpMatrix vpFeatureMomentBasic::interaction (unsigned int select_one,unsigned int select_two) const {
- if(select_one+select_two>moment->getObject().getOrder())
- throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
- return interaction_matrices[select_two*order+select_one];
-}
diff --git a/src/visual-feature/vpFeatureMomentBasic.h b/src/visual-feature/vpFeatureMomentBasic.h
deleted file mode 100644
index 64df62c..0000000
--- a/src/visual-feature/vpFeatureMomentBasic.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentBasic.h
- \brief Implementation of the interaction matrix computation for vpMomentBasic.
-*/
-
-#ifndef __FEATUREMOMENTBASIC_H__
-#define __FEATUREMOMENTBASIC_H__
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpMomentObject.h>
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentBasic
-
- \ingroup VsFeature2
-
- \brief Functionality computation for basic moment feature. Computes the interaction matrix associated with vpMomentBasic.
-
- The interaction matrix for the basic moment feature is defined in \cite Tahri05z, equation (13).
- This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
- The interaction matrix \f$ L_{m_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ m_{ij} \f$ obtained by vpMomentBasic::get (i,j).
- vpFeatureMomentBasic computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
- \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
- For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
- \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
-
- You can see an example of vpFeatureMomentBasic by looking at the documentation of the vpFeatureMoment class.
-
- This feature depends on:
- - vpMomentBasic
-
-*/
-class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment{
-protected:
- unsigned int order;
- public:
- vpFeatureMomentBasic(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
- void compute_interaction();
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /* Add function due to pure virtual definition in vpBasicFeature.h */
- vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
-
- vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentBasic";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentBasic";}
-
-};
-#endif
diff --git a/src/visual-feature/vpFeatureMomentCInvariant.cpp b/src/visual-feature/vpFeatureMomentCInvariant.cpp
deleted file mode 100644
index 10aa1ec..0000000
--- a/src/visual-feature/vpFeatureMomentCInvariant.cpp
+++ /dev/null
@@ -1,424 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-#include <visp/vpMomentObject.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <visp/vpFeatureMomentCInvariant.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentBasic.h>
-
-#include <visp/vpFeatureMomentDatabase.h>
-
-#include <vector>
-#include <limits>
-
-
-/*!
- Computes interaction matrix for space-scale-rotation invariants. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentCInvariant
- - vpFeatureMomentBasic
-*/
-void vpFeatureMomentCInvariant::compute_interaction(){
- std::vector<vpMatrix> LI(16);
- bool found_moment_centered;
- bool found_moment_cinvariant;
- bool found_FeatureMoment_centered;
- bool found_featuremoment_basic;
-
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- vpMatrix zeros(1,6);
- for(int i=0;i<6;i++) zeros[0][i]=0;
-
-
- LI[1]=-featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) - momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) + 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
-
-
- LI[2]=2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) * (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) + 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
-
-
- LI[3]=2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) + 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) * (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
-
-
- LI[4]=2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) + 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) * (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
-
- LI[5]=-2*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)-4*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-12*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interac [...]
-
-
- LI[6]=6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+2*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)-12*moment [...]
-
-
- LI[7] = -3*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-2*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(3,0)-3*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+3*mome [...]
-
- LI[8]=-2 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * featureMomentCentered.interaction(3, 0) + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) - 6 * featureMomentCentered.interaction(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) * [...]
-
- LI[9] = 4*pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(3,0)+18*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+12*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*m [...]
-
-
- LI[10]=featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) - 4 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) - 4 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) + 6 * momentCentered.get(2, 2) * featureMomentCentered.interaction(2, 2);
-
- LI[11]=-3 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) - 3 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) - 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) - 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(3, 1) * featureMomentCentered.interaction(3, 1) + 2 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 2 * momentCentered.get(3, 1) * featureMome [...]
-
-
- LI[12]=6 * momentCentered.get(4, 0) * featureMomentCentered.interaction(4, 0) + 12 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) + 12 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) + 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 16 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 16 * momentCentered.get(3, 1) * featureM [...]
-
-
- LI[13]=2 * (momentCentered.get(5, 0) + 2 * momentCentered.get(3, 2) + momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) + 2 * featureMomentCentered.interaction(3, 2) + featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) + 2 * momentCentered.get(2, 3) + momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) + 2 * featureMomentCentered.interaction(2, 3) + featureMomentCentered.interaction(4, 1));
-
- LI[14]=2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) - 3 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) - 3 * featureMomentCentered.interaction(4, 1));
-
- LI[15]=2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) + 5 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) + 5 * featureMomentCentered.interaction(4, 1));
-
- double s3 = momentCInvariant.getS(3);
- double s2 = momentCInvariant.getS(2);
- double c3 = momentCInvariant.getC(3);
- double c2 = momentCInvariant.getC(2);
- double I1 = momentCInvariant.getII(1);
- double I2 = momentCInvariant.getII(2);
- double I3 = momentCInvariant.getII(3);
-
- vpMatrix Lc2 = featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1);
- vpMatrix Ls2 = featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2);
- vpMatrix Lc3 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) - 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
- vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1,1) * (momentCentered.get(2,0) - momentCentered.get(0,2)) + 4 * momentCentered.get(1,1) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2));
- vpMatrix LI1 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) + 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
- vpMatrix LI2 = 2 * (momentCentered.get(0,3) - 3 * momentCentered.get(2,1)) * (featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1)) + 2 * (momentCentered.get(3,0) - 3 * momentCentered.get(1,2)) * (featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2));
- vpMatrix LI3 = featureMomentCentered.interaction(2,0) + featureMomentCentered.interaction(0,2);
-
-
- vpMatrix La(1,6);
- double a;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
- interaction_matrices.resize(14);
-
-
- interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
- interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
-
- interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
-
- interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
-
- interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
-
- interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
-
-
- interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
-
- interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
-
- interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
-
- interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
-
-
- interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
- interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
-}
-
-#else
-#include <visp/vpFeatureMomentCInvariant.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentBasic.h>
-
-#include <visp/vpFeatureMomentDatabase.h>
-
-#include <vector>
-#include <iostream>
-#include <limits>
-#include <cmath>
-
-/*!
- Computes interaction matrix for space-scale-rotation invariants. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentCInvariant
- - vpFeatureMomentBasic
-*/
-void vpFeatureMomentCInvariant::compute_interaction(){
-
- //std::vector<vpMatrix> LI(16);
- LI.resize(16); // LI made class member
-
- bool found_moment_centered;
- bool found_moment_cinvariant;
- bool found_FeatureMoment_centered;
- bool found_featuremoment_basic;
-
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
-
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- vpMatrix zeros(1,6);
- for(int i=0;i<6;i++) zeros[0][i]=0;
-
- double mu30 = momentCentered.get(3,0);
- double mu30_2 = mu30*mu30;
- double mu30_3 = mu30_2*mu30;
-
- double mu03 = momentCentered.get(0,3);
- double mu03_2 = mu03*mu03;
- double mu03_3 = mu03*mu03_2;
-
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu50 = momentCentered.get(5,0);
- double mu32 = momentCentered.get(3,2);
- double mu14 = momentCentered.get(1,4);
- double mu05 = momentCentered.get(0,5);
- double mu23 = momentCentered.get(2,3);
- double mu41 = momentCentered.get(4,1);
- double mu40 = momentCentered.get(4,0);
- double mu04 = momentCentered.get(0,4);
- double mu31 = momentCentered.get(3,1);
- double mu13 = momentCentered.get(1,3);
- double mu22 = momentCentered.get(2,2);
- double mu21 = momentCentered.get(2,1);
- double mu12 = momentCentered.get(1,2);
- double mu11 = momentCentered.get(1,1);
-
- double mu12_2 = mu12*mu12;
- double mu21_2 = mu21*mu21;
- double mu21_3 = mu21*mu21_2;
- double mu12_3 = mu12_2*mu12;
-
- vpMatrix Lmu20 = featureMomentCentered.interaction(2,0);
- vpMatrix Lmu02 = featureMomentCentered.interaction(0,2);
- vpMatrix Lmu11 = featureMomentCentered.interaction(1,1);
- vpMatrix Lmu12 = featureMomentCentered.interaction(1,2);
- vpMatrix Lmu03 = featureMomentCentered.interaction(0,3);
- vpMatrix Lmu21 = featureMomentCentered.interaction(2,1);
- vpMatrix Lmu30 = featureMomentCentered.interaction(3,0);
- vpMatrix Lmu40 = featureMomentCentered.interaction(4,0);
- vpMatrix Lmu04 = featureMomentCentered.interaction(0,4);
- vpMatrix Lmu31 = featureMomentCentered.interaction(3,1);
- vpMatrix Lmu13 = featureMomentCentered.interaction(1,3);
- vpMatrix Lmu22 = featureMomentCentered.interaction(2,2);
- vpMatrix Lmu05 = featureMomentCentered.interaction(0,5);
- vpMatrix Lmu14 = featureMomentCentered.interaction(1,4);
- vpMatrix Lmu23 = featureMomentCentered.interaction(2,3);
- vpMatrix Lmu32 = featureMomentCentered.interaction(3,2);
- vpMatrix Lmu41 = featureMomentCentered.interaction(4,1);
- vpMatrix Lmu50 = featureMomentCentered.interaction(5,0);
-
- LI[1]= -Lmu20*mu02-mu20*Lmu02+2*mu11*Lmu11;
-
- LI[2]= (-2*mu20+2*mu02)*Lmu02+8*mu11*Lmu11+(2*mu20-2*mu02)*Lmu20;
-
- LI[3]= (-6*mu21+2*mu03)*Lmu03+(-6*mu30+18*mu12)*Lmu12+(18*mu21-6*mu03)*Lmu21+(2*mu30-6*mu12)*Lmu30;
-
- LI[4]= (2*mu21+2*mu03)*Lmu03+(2*mu30+2*mu12)*Lmu12+(2*mu21+2*mu03)*Lmu21+(2*mu30+2*mu12)*Lmu30;
-
- LI[5]= (-2*mu30_2*mu03+6*mu30*mu21*mu12-4*mu21_3)*Lmu03+(6*mu30*mu21*mu03-12*mu30*mu12_2+6*mu21_2*mu12)*Lmu12+(6*mu30*mu12*mu03-12*mu21_2*mu03+6*mu21*mu12_2)*Lmu21+(-2*mu30*mu03_2-4*mu12_3+6*mu21*mu12*mu03)*Lmu30;
-
- LI[6]= (-6*mu30*mu21*mu12-6*mu21*mu12_2+6*mu21_2*mu03+2*mu21_3+4*mu30_2*mu03)*Lmu03+(-6*mu30*mu21_2-6*mu30*mu21*mu03+12*mu12_3+6*mu30_2*mu12-12*mu21*mu12*mu03+6*mu30*mu12_2)*Lmu12+(6*mu21*mu03_2+6*mu21_2*mu03-6*mu30*mu12*mu03+12*mu21_3-12*mu30*mu21*mu12-6*mu12_2*mu03)*Lmu21+(6*mu30*mu12_2+2*mu12_3+4*mu30*mu03_2-6*mu21_2*mu12-6*mu21*mu12*mu03)*Lmu30;
-
- LI[7] = (-6*mu21_2*mu12+3*mu30*mu03_2-mu30_3-3*mu30*mu21_2-6*mu21*mu12*mu03+3*mu30*mu12_2+2*mu12_3)*Lmu03+(-3*mu21*mu03_2+12*mu30*mu21*mu12+6*mu30*mu12*mu03+3*mu30_2*mu21+9*mu21*mu12_2-6*mu21_2*mu03-3*mu21_3+6*mu12_2*mu03)*Lmu12+(3*mu30_2*mu12-9*mu21_2*mu12-12*mu21*mu12*mu03-6*mu30*mu21*mu03-6*mu30*mu21_2+6*mu30*mu12_2+3*mu12_3-3*mu12*mu03_2)*Lmu21+(6*mu21*mu12_2+6*mu30*mu21*mu12-3*mu30_2*mu03+3*mu12_2*mu03-3*mu21_2*mu03-2*mu21_3+mu03_3)*Lmu30;
-
- LI[8]= (6*mu21_3-2*mu30*mu12*mu03+2*mu12_2*mu03+3*mu21*mu12_2-6*mu30*mu21*mu12-mu30_2*mu21-4*mu21_2*mu03-3*mu21*mu03_2)*Lmu03+(2*mu12*mu03_2-4*mu30_2*mu12+9*mu30*mu12_2-mu30*mu03_2-6*mu30*mu21*mu03+3*mu30*mu21_2+6*mu21*mu12*mu03-mu30_3)*Lmu12+(18*mu21_2*mu03+6*mu30*mu21*mu12-4*mu21*mu03_2-mu03_3-mu30_2*mu03-6*mu30*mu12*mu03+3*mu12_2*mu03+2*mu30_2*mu21)*Lmu21+(-6*mu21*mu12*mu03-4*mu30*mu12_2-2*mu30*mu21*mu03+2*mu30*mu21_2+3*mu12_3+3*mu21_2*mu12-3*mu30_2*mu12-mu12*mu03_2)*Lmu30;
-
- LI[9] = (2*(2*mu03+3*mu21))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu03+6*mu30*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu12+6*mu03*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu21+(2*(2*mu30+3*mu12))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu30;
-
- LI[10]= Lmu40*mu04+mu40*Lmu04-4*Lmu31*mu13-4*mu31*Lmu13+6*mu22*Lmu22;
-
- LI[11]= (-2*mu40-3*mu22)*Lmu04+(2*mu31+6*mu13)*Lmu13+(-3*mu04-3*mu40)*Lmu22+(2*mu13+6*mu31)*Lmu31+(-3*mu22-2*mu04)*Lmu40;
-
- LI[12]= (2*mu40+12*mu22+6*mu04)*Lmu04+16*mu31*Lmu13+(12*mu40+12*mu04)*Lmu22+16*Lmu31*mu13+(6*mu40+12*mu22+2*mu04)*Lmu40;
-
- LI[13]=(2*mu05+4*mu23+2*mu41)*Lmu05+(2*mu50+4*mu32+2*mu14)*Lmu14+(4*mu05+8*mu23+4*mu41)*Lmu23+(4*mu50+8*mu32+4*mu14)*Lmu32+(2*mu05+4*mu23+2*mu41)*Lmu41+(2*mu50+4*mu32+2*mu14)*Lmu50 ;
-
- LI[14]= (2*mu05-4*mu23-6*mu41)*Lmu05+(-6*mu50+12*mu32+18*mu14)*Lmu14+(-4*mu05+8*mu23+12*mu41)*Lmu23+(-4*mu50+8*mu32+12*mu14)*Lmu32+(-6*mu05+12*mu23+18*mu41)*Lmu41+(2*mu50-4*mu32-6*mu14)*Lmu50;
-
- LI[15]= (2*mu05-20*mu23+10*mu41)*Lmu05+(10*mu50-100*mu32+50*mu14)*Lmu14+(-20*mu05+200*mu23-100*mu41)*Lmu23+(-20*mu50+200*mu32-100*mu14)*Lmu32+(10*mu05-100*mu23+50*mu41)*Lmu41+(2*mu50-20*mu32+10*mu14)*Lmu50;
-
- double s3 = momentCInvariant.getS(3);
- double s2 = momentCInvariant.getS(2);
- double c3 = momentCInvariant.getC(3);
- double c2 = momentCInvariant.getC(2);
-
- double I1 = momentCInvariant.getII(1);
- double I2 = momentCInvariant.getII(2);
- double I3 = momentCInvariant.getII(3);
-
- vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
- double mu20__mu02 = mu20 - mu02;
- vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
- vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
- vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02) - 8. * mu11 * Lmu11;
- vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
- vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
- vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
- vpMatrix LI3 = Lmu20 + Lmu02;
-
- vpMatrix La(1,6);
- double a;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
-
- interaction_matrices.resize(14);
-
- /*
- momentCInvariant.printInvariants(std::cout);
- printLsofInvariants(std::cout);
- */
-
- interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
-
- interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
-
- interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
-
- interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
-
- interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
-
- interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
-
- interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
-
- interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
-
- interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
-
- interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
-
- interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
-
- interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
-
- /*
- std::cout << (*this);
- vpTRACE("Done.");
- std::exit(-1);
- */
-}
-
-/*!
- Print out all invariants that were computed
- There are 15 of them, as in [Point-based and region based.ITRO05]
- \cite Tahri05z
- */
-void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream& os) const{
- for (unsigned int i = 1; i < 15; ++i){
- os << "LI[" << i << "] = ";
- LI[i].matlabPrint(os);
- os << std::endl;
- }
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMomentCInvariant& featcinv)
-{
- //Print L for c1 .. c10
- for (unsigned int i = 0; i < 10; ++i){
- os << "L_c[" << i << "] = ";
- featcinv.interaction_matrices[i].matlabPrint(os);
- os << std::endl;
- }
-
- // sx, sy
- os << "L_sx = ";
- featcinv.interaction_matrices[10].matlabPrint(os);
- os << std::endl;
- os << "L_sy = ";
- featcinv.interaction_matrices[11].matlabPrint(os);
- os << std::endl;
- // Px,Py
- os << "L_Px = ";
- featcinv.interaction_matrices[12].matlabPrint(os);
- os << std::endl;
- os << "L_Py = ";
- featcinv.interaction_matrices[13].matlabPrint(os);
- os << std::endl;
-
- return os;
-
-}
-#endif
diff --git a/src/visual-feature/vpFeatureMomentCInvariant.h b/src/visual-feature/vpFeatureMomentCInvariant.h
deleted file mode 100644
index a9039cd..0000000
--- a/src/visual-feature/vpFeatureMomentCInvariant.h
+++ /dev/null
@@ -1,294 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentCInvariant.h
- \brief Implementation of the interaction matrix computation for vpMomentCInvariant.
-*/
-#ifndef __FEATUREMOMENTCINVARIANT_H__
-#define __FEATUREMOMENTCINVARIANT_H__
-#include <visp/vpFeatureMoment.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-/*!
- \class vpFeatureMomentCInvariant
-
- \ingroup VsFeature2
-
- \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
-
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
- To do so, one must derive them and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
- The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
- For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
- Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
- Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
-
- These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
-
- Be careful about the nature of your object when selecting the right features.
- Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
-
- Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
-
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentCInvariant
- - vpFeatureMomentBasic
-
- An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
-
-*/
-class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentCInvariant(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(moments,A,B,C,featureMoments,16){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCInvariant";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCInvariant";}
-
- /*!
- Shortcut selector for \f$C_1\f$.
- */
- static unsigned int selectC1(){ return 1 << 0; }
- /*!
- Shortcut selector for \f$C_2\f$.
- */
- static unsigned int selectC2(){ return 1 << 1; }
- /*!
- Shortcut selector for \f$C_3\f$.
- */
- static unsigned int selectC3(){ return 1 << 2; }
- /*!
- Shortcut selector for \f$C_4\f$.
- */
- static unsigned int selectC4(){ return 1 << 3; }
- /*!
- Shortcut selector for \f$C_5\f$.
- */
- static unsigned int selectC5(){ return 1 << 4; }
- /*!
- Shortcut selector for \f$C_6\f$.
- */
- static unsigned int selectC6(){ return 1 << 5; }
- /*!
- Shortcut selector for \f$C_7\f$.
- */
- static unsigned int selectC7(){ return 1 << 6; }
- /*!
- Shortcut selector for \f$C_8\f$.
- */
- static unsigned int selectC8(){ return 1 << 7; }
- /*!
- Shortcut selector for \f$C_9\f$.
- */
- static unsigned int selectC9(){ return 1 << 8; }
- /*!
- Shortcut selector for \f$C_{10}\f$.
- */
- static unsigned int selectC10(){ return 1 << 9; }
- /*!
- Shortcut selector for \f$S_x\f$.
- */
- static unsigned int selectSx(){ return 1 << 10; }
- /*!
- Shortcut selector for \f$S_y\f$.
- */
- static unsigned int selectSy(){ return 1 << 11; }
- /*!
- Shortcut selector for \f$P_x\f$.
- */
- static unsigned int selectPx(){ return 1 << 12; }
- /*!
- Shortcut selector for \f$P_y\f$.
- */
- static unsigned int selectPy(){ return 1 << 13; }
-
-};
-
-#else
-class vpMomentDatabase;
-
-/*!
- \class vpFeatureMomentCInvariant
-
- \ingroup VsFeature2
-
- \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
-
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
- To do so, one must derive them and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
- The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
- For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
- Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
- Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
-
- These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
-
- Be careful about the nature of your object when selecting the right features.
- Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
-
- Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
-
- This feature depends on:
- - vpMomentCentered
- - vpFeatureMomentCentered
- - vpMomentCInvariant
- - vpFeatureMomentBasic
-
- An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
-
-*/
-class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
- private:
- std::vector<vpMatrix> LI;
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentCInvariant(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,16){LI.resize(16);}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCInvariant";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCInvariant";}
-
- /*!
- Shortcut selector for \f$C_1\f$.
- */
- static unsigned int selectC1(){ return 1 << 0; }
- /*!
- Shortcut selector for \f$C_2\f$.
- */
- static unsigned int selectC2(){ return 1 << 1; }
- /*!
- Shortcut selector for \f$C_3\f$.
- */
- static unsigned int selectC3(){ return 1 << 2; }
- /*!
- Shortcut selector for \f$C_4\f$.
- */
- static unsigned int selectC4(){ return 1 << 3; }
- /*!
- Shortcut selector for \f$C_5\f$.
- */
- static unsigned int selectC5(){ return 1 << 4; }
- /*!
- Shortcut selector for \f$C_6\f$.
- */
- static unsigned int selectC6(){ return 1 << 5; }
- /*!
- Shortcut selector for \f$C_7\f$.
- */
- static unsigned int selectC7(){ return 1 << 6; }
- /*!
- Shortcut selector for \f$C_8\f$.
- */
- static unsigned int selectC8(){ return 1 << 7; }
- /*!
- Shortcut selector for \f$C_9\f$.
- */
- static unsigned int selectC9(){ return 1 << 8; }
- /*!
- Shortcut selector for \f$C_{10}\f$.
- */
- static unsigned int selectC10(){ return 1 << 9; }
- /*!
- Shortcut selector for \f$S_x\f$.
- */
- static unsigned int selectSx(){ return 1 << 10; }
- /*!
- Shortcut selector for \f$S_y\f$.
- */
- static unsigned int selectSy(){ return 1 << 11; }
- /*!
- Shortcut selector for \f$P_x\f$.
- */
- static unsigned int selectPx(){ return 1 << 12; }
- /*!
- Shortcut selector for \f$P_y\f$.
- */
- static unsigned int selectPy(){ return 1 << 13; }
-
- /*!
- Print all the interaction matrices of the moment invariants
- */
- void printLsofInvariants(std::ostream& os) const;
-
- /*!
- Print all the interaction matrices of visual features
- */
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCInvariant& featcinv);
-
-};
-#endif
-#endif
diff --git a/src/visual-feature/vpFeatureMomentCentered.cpp b/src/visual-feature/vpFeatureMomentCentered.cpp
deleted file mode 100644
index e518fe2..0000000
--- a/src/visual-feature/vpFeatureMomentCentered.cpp
+++ /dev/null
@@ -1,314 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- * Manikandan Bakthavatchalam
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentObject.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-
-
-/*!
- Default constructor
- \param moments_ : Database of moment primitives.
- \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param featureMoments : Database of features.
-*/
-vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase& moments_,
- double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments)
- : vpFeatureMoment(moments_, A_, B_, C_, featureMoments), order(0)
-{
-}
-
-/*!
-Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
-\param select_one : first index (i)
-\param select_two : second index (j)
-\return Interaction matrix corresponding to the moment
-*/
-vpMatrix vpFeatureMomentCentered::interaction (unsigned int select_one,unsigned int select_two) const {
- if(select_one+select_two>moment->getObject().getOrder())
- throw vpException(vpException::badValue,
- "The requested value has not been computed, you should specify a higher order.");
- return interaction_matrices[select_two*order+select_one];
-}
-
-/*!
- * Core function for the interaction matrix computation for moment m_pq
- * Given its dependent moment and interaction matrices, computes the interaction matrix of centred moments
- */
-vpMatrix
-vpFeatureMomentCentered::compute_Lmu_pq(const unsigned int& p, const unsigned int& q, const double& xg, const double& yg,
- const vpMatrix& L_xg, const vpMatrix& L_yg,
- const vpMomentBasic& m, const vpFeatureMomentBasic& feature_moment_m) const
-{
- // term1, term2 and Lterm3 (matrix) will be repeatedly computed inside the innermost loop
- double term1 = 0.0;
- double term2 = 0.0;
- vpMatrix Lterm3(1,6);
-
- double pcombk = 0.0;
- double qcombl = 0.0;
- double pcombkqcombl = 0.0;
-
- double mkl = 0.0;
- vpMatrix L_mkl;
-
- int pmk = 0; // p-k
- int qml = 0; // q-l
- double minus1pow = 0.; // (-1)^(p+q-k-l)
- double pintom = 0.;
-
- for (unsigned int k = 0; k <=p; ++k)
- {
- pmk = (int)p-(int)k;
- pcombk = static_cast<double>(vpMath::comb(p,k));
- for (unsigned int l = 0; l <= q; ++l)
- {
- qml = (int)q - (int)l;
- qcombl = static_cast<double>(vpMath::comb(q,l));
- minus1pow = pow((double)-1, (double)(pmk + qml));
- pcombkqcombl = pcombk * qcombl;
- mkl = m.get(k, l);
- pintom = pcombkqcombl * mkl;
- L_mkl = feature_moment_m.interaction(k, l);
- if(pmk>0)
- term1 += pintom * pmk * pow(xg, pmk-1) * pow(yg, qml) * minus1pow;
- if(qml>0)
- term2 += pintom * qml * pow(xg, pmk) * pow(yg, qml-1) * minus1pow;
- Lterm3 += pcombkqcombl * pow(xg, pmk) * pow(yg, qml) * L_mkl * minus1pow;
- }
- }
-
- // L_xg and L_yg stay constant with respect to the above loops. Lterm3 is summed over
- vpMatrix L_mupq = L_xg*term1 + L_yg*term2 + Lterm3;
- return L_mupq;
-}
-
-/*!
- Interface to the interaction matrix computation for centered moments. Called internally.
- Calls compute_Lmu_pq() for main computation moments (upto order-1)
-Dependencies:
- Moment classes
- - vpMomentBasic
- Interaction matrix classes
- - vpMomentGravityCenter
- - vpFeatureMomentBasic
- - vpFeatureMomentGravityCenter
-*/
-void vpFeatureMomentCentered::compute_interaction() {
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
- const vpMomentObject& momentObject = moment->getObject();
- order = momentObject.getOrder()+1;
- interaction_matrices.resize(order*order);
- for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end(); ++i)
- i->resize(1,6);
-
- bool found_moment_gravity;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- double xg = momentGravity.get()[0];
- double yg = momentGravity.get()[1];
-
- bool found_feature_gravity_center;
- vpFeatureMomentGravityCenter& featureMomentGravityCenter= (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_feature_gravity_center)));
- if(!found_feature_gravity_center) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
- vpMatrix Lxg = featureMomentGravityCenter.interaction(1<<0);
- vpMatrix Lyg = featureMomentGravityCenter.interaction(1<<1);
-
- bool found_moment_basic;
- const vpMomentBasic& momentbasic = static_cast<const vpMomentBasic&>(moments.get("vpMomentBasic",found_moment_basic));
- if(!found_moment_basic) throw vpException(vpException::notInitialized,"vpMomentBasic not found");
-
- bool found_featuremoment_basic;
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
- // Calls the main compute_Lmu_pq function for moments upto order-1
- for(int i=0;i<(int)order-1;i++){
- for(int j=0;j<(int)order-1-i;j++){
- interaction_matrices[(unsigned int)j*order+(unsigned int)i] = compute_Lmu_pq(i, j, xg, yg, Lxg, Lyg, momentbasic, featureMomentBasic);
- }
- }
-#else // #ifdef VISP_MOMENTS_COMBINE_MATRICES
- bool found_moment_centered;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- int delta;
- int epsilon;
- const vpMomentObject& momentObject = moment->getObject();
- order = momentObject.getOrder()+1;
- interaction_matrices.resize(order*order);
- for (std::vector< vpMatrix >::iterator i=interaction_matrices.begin(); i!=interaction_matrices.end(); ++i)
- i->resize(1,6);
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- delta=0;
- epsilon=1;
- } else {
- delta=1;
- epsilon=4;
- }
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
- double mu00 = momentCentered.get(0,0);
-
- unsigned int VX = 0;
- unsigned int VY = 1;
- unsigned int VZ = 2;
- unsigned int WX = 3;
- unsigned int WY = 4;
- unsigned int WZ = 5;
-
- interaction_matrices[0][0][VX] = -(delta)*A*mu00;
- interaction_matrices[0][0][VY] = -(delta)*B*mu00;
-
- // Since mu10=0 and mu01=0
- // interaction_matrices[0][0][WX] = (3*delta)*MU(0,1)+(3*delta)*Yg*mu00;
- // interaction_matrices[0][0][WY] = -(3*delta)*MU(1,0)-(3*delta)*Xg*mu00;
- // we get the simplification:
- interaction_matrices[0][0][WX] = (3*delta)*Yg*mu00;
- interaction_matrices[0][0][WY] = -(3*delta)*Xg*mu00;
- interaction_matrices[0][0][VZ] = -A*interaction_matrices[0][0][WY]+B*interaction_matrices[0][0][WX]+(2*delta)*C*mu00;
- interaction_matrices[0][0][WZ] = 0.;
-
- for (int i=1; i<(int)order-1; i++){
- unsigned int i_ = (unsigned int) i;
- unsigned int im1_ = i_ - 1;
- unsigned int ip1_ = i_ + 1;
-
- double mu_im10 = momentCentered.get(im1_,0);
- double mu_ip10 = momentCentered.get(ip1_,0);
- double mu_im11 = momentCentered.get(im1_,1);
- double mu_i0 = momentCentered.get(i_,0);
- double mu_i1 = momentCentered.get(i_,1);
-
- interaction_matrices[i_][0][VX] = -(i+delta)*A*mu_i0-(i*B*mu_im11);
- interaction_matrices[i_][0][VY] = -(delta)*B*mu_i0;
-
- interaction_matrices[i_][0][WX] = (i+3*delta)*mu_i1+(i+3*delta)*Yg*mu_i0+i*Xg*mu_im11-i*epsilon*n11*mu_im10;
- interaction_matrices[i_][0][WY] = -(i+3*delta)*mu_ip10-(2*i+3*delta)*Xg*mu_i0+i*epsilon*n20*mu_im10;
- interaction_matrices[i_][0][VZ] = -A*interaction_matrices[i_][0][WY]+B*interaction_matrices[i_][0][WX]+(i+2*delta)*C*mu_i0;
- interaction_matrices[i_][0][WZ] = i*mu_im11;
- }
-
- for(int j=1;j<(int)order-1;j++){
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
-
- double mu_0jm1 = momentCentered.get(0,jm1_);
- double mu_0jp1 = momentCentered.get(0,jp1_);
- double mu_1jm1 = momentCentered.get(1,jm1_);
- double mu_0j = momentCentered.get(0,j_);
- double mu_1j = momentCentered.get(1,j_);
-
- interaction_matrices[j_*order][0][VX] = -(delta)*A*mu_0j;
- interaction_matrices[j_*order][0][VY] = -j*A*mu_1jm1-(j+delta)*B*mu_0j;
-
- interaction_matrices[j_*order][0][WX] = (j+3*delta)*mu_0jp1+(2*j+3*delta)*Yg*mu_0j-j*epsilon*n02*mu_0jm1;
- interaction_matrices[j_*order][0][WY] = -(j+3*delta)*mu_1j-(j+3*delta)*Xg*mu_0j-j*Yg*mu_1jm1+j*epsilon*n11*mu_0jm1;
- interaction_matrices[j_*order][0][VZ] = -A*interaction_matrices[j_*order][0][WY]+B*interaction_matrices[j_*order][0][WX]+(j+2*delta)*C*mu_0j;
- interaction_matrices[j_*order][0][WZ] = -j*mu_1jm1;
- }
-
- for(int j=1; j<(int)order-1; j++) {
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
- for(int i=1; i<(int)order-j-1; i++) {
- unsigned int i_ = (unsigned int) i;
- unsigned int im1_ = i_ - 1;
- unsigned int ip1_ = i_ + 1;
-
- double mu_ijm1 = momentCentered.get(i_,jm1_);
- double mu_ij = momentCentered.get(i_,j_);
- double mu_ijp1 = momentCentered.get(i_,jp1_);
- double mu_im1j = momentCentered.get(im1_,j_);
- double mu_im1jp1 = momentCentered.get(im1_,jp1_);
- double mu_ip1jm1 = momentCentered.get(ip1_,jm1_);
- double mu_ip1j = momentCentered.get(ip1_,j_);
-
- interaction_matrices[j_*order+i_][0][VX] = -(i+delta)*A*mu_ij-i*B*mu_im1jp1;
- interaction_matrices[j_*order+i_][0][VY] = -j*A*mu_ip1jm1-(j+delta)*B*mu_ij;
-
- interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*mu_ijp1+(i+2*j+3*delta)*Yg*mu_ij
- +i*Xg*mu_im1jp1-i*epsilon*n11*mu_im1j-j*epsilon*n02*mu_ijm1;
- interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*mu_ip1j-(2*i+j+3*delta)*Xg*mu_ij
- -j*Yg*mu_ip1jm1+i*epsilon*n20*mu_im1j+j*epsilon*n11*mu_ijm1;
- interaction_matrices[j_*order+i_][0][VZ] = -A*interaction_matrices[j_*order+i_][0][WY]+B*interaction_matrices[j_*order+i_][0][WX]+(i+j+2*delta)*C*mu_ij;
- interaction_matrices[j_*order+i_][0][WZ] = i*mu_im1jp1-j*mu_ip1jm1;
- }
- }
-#endif // #ifdef VISP_MOMENTS_COMBINE_MATRICES
-}
-
-
-std::ostream& operator<<(std::ostream & os, const vpFeatureMomentCentered& mu){
- vpTRACE(" << Ls - CENTRED MOMENTS >>");
- unsigned int order_m_1 = (unsigned int)(mu.order - 1);
- for(unsigned int i=0; i<order_m_1; i++){
- for(unsigned int j=0; j<order_m_1-i; j++){
- os << "L_mu[" << i << "," << j << "] = ";
- mu.interaction(i,j).matlabPrint(os);
- }
- }
- return os;
-}
-
diff --git a/src/visual-feature/vpFeatureMomentCentered.h b/src/visual-feature/vpFeatureMomentCentered.h
deleted file mode 100644
index bfb13ab..0000000
--- a/src/visual-feature/vpFeatureMomentCentered.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- * Manikandan Bakthavatchalam
- *****************************************************************************/
-/*!
- \file vpFeatureMomentCentered.h
- \brief Implementation of the interaction matrix computation for vpMomentCentered.
-*/
-#ifndef __FEATUREMOMENTCENTERED_H__
-#define __FEATUREMOMENTCENTERED_H__
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpFeatureMomentBasic.h>
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentCentered
-
- \ingroup VsFeature2
-
- \brief Functionality computation for centered moment feature. Computes the interaction matrix associated with vpMomentCentered.
-
- The interaction matrix for the feature is defined in \cite Tahri05z, equation (17).
- This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
- The interaction matrix \f$ L_{\mu_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ \mu_{ij} \f$ obtained by vpFeatureMomentCentered::get (i,j).
-
- vpFeatureMomentCentered computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
- \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
- For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
- \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
-
- This feature depends on:
- - vpFeatureMomentBasic
- - vpFeatureMomentGravityCenter
- - vpMomentGravityCenter
-*/
-class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment{
-
-protected:
-
- unsigned int order;
- /*!
- Core computation of interaction matrix for moment m_pq
- */
- vpMatrix
- compute_Lmu_pq(const unsigned int& p, const unsigned int& q, const double& xg, const double& yg,
- const vpMatrix& L_xg, const vpMatrix& L_yg,
- const vpMomentBasic& m, const vpFeatureMomentBasic& feature_moment_m) const;
-
- public:
- vpFeatureMomentCentered(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
- void compute_interaction();
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /* Add function due to pure virtual definition in vpBasicFeature.h */
- vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
-#endif
- /*!
- Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
- \param select_one : first index (i)
- \param select_two : second index (j)
- \return Interaction matrix corresponding to the moment
- */
- vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
-
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCentered";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCentered";}
-
-friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCentered& v);
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentCommon.cpp b/src/visual-feature/vpFeatureMomentCommon.cpp
deleted file mode 100644
index 7b4bf31..0000000
--- a/src/visual-feature/vpFeatureMomentCommon.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentCommon.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pre-filled pseudo-database used to handle dependencies between common moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpMomentDatabase.h>
-
-/*!
- Constructor which initializes and links all common features in the database
- \param moments : database for moment primitives
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
-*/
-vpFeatureMomentCommon::vpFeatureMomentCommon(vpMomentDatabase& moments,double A,double B,double C) :
- featureGravity(moments,A,B,C),
- featureGravityNormalized(moments,A,B,C),
- featureAn(moments,A,B,C),
- featureCInvariant(moments,A,B,C),
- featureAlpha(moments,A,B,C),
- featureCentered(moments,A,B,C),
- featureMomentBasic(moments,A,B,C),
- feature_moment_area(moments,A,B,C)
-
-{
- featureGravity.linkTo(*this);
- featureGravityNormalized.linkTo(*this);
- featureAn.linkTo(*this);
- featureCInvariant.linkTo(*this);
- featureAlpha.linkTo(*this);
- featureMomentBasic.linkTo(*this);
- featureCentered.linkTo(*this);
- feature_moment_area.linkTo(*this);
-}
-
-/*!
- Update all moment features in the database with plane coefficients
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
-*/
-void vpFeatureMomentCommon::updateAll(double A,double B,double C){
- featureMomentBasic.update(A,B,C);
- featureGravity.update(A,B,C);
- featureCentered.update(A,B,C);
- featureAn.update(A,B,C);
- featureGravityNormalized.update(A,B,C);
- featureCInvariant.update(A,B,C);
- featureAlpha.update(A,B,C);
- feature_moment_area.update(A,B,C);
-}
-
diff --git a/src/visual-feature/vpFeatureMomentCommon.h b/src/visual-feature/vpFeatureMomentCommon.h
deleted file mode 100644
index 4bb96ae..0000000
--- a/src/visual-feature/vpFeatureMomentCommon.h
+++ /dev/null
@@ -1,271 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentCommon.h 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pre-filled pseudo-database used to handle dependencies between common moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpFeatureMomentCommon.h
- \brief Pre-filled pseudo-database used to handle dependencies between common moment features.
-*/
-
-#ifndef __FEATUREMOMENTCOMMON_H__
-#define __FEATUREMOMENTCOMMON_H__
-#include <visp/vpFeatureMomentAlpha.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpFeatureMomentGravityCenterNormalized.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentCInvariant.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpFeatureMomentAreaNormalized.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMomentArea.h>
-
-class vpMomentDatabase;
-class vpServo;
-/*!
- \class vpFeatureMomentCommon
-
- \ingroup VsFeature2
-
- \brief This class allows to access common vpFeatureMoments in a pre-filled database.
-
- It is a vpMomentDatabase filled with the following moments:
- - vpFeatureMomentGravityCenter
- - vpFeatureMomentGravityCenterNormalized
- - vpFeatureMomentAreaNormalized
- - vpFeatureMomentCInvariant
- - vpFeatureMomentAlpha
- - vpFeatureMomentCentered
- - vpFeatureMomentBasic
-
-
- There is no need to do the linkTo operations manually nor is it necessary to care about the order of feature computation.
-
- This class has an vpMomentCommon::updateAll method capable of updating the plane parameters AND computing interaction matrices inside the features.
-
- The moment features computed by this class are classical moments
- features used in moment-based visual servoing. For more
- information see \cite Tahri05z.
-
- To initialize this feature set, the user needs to supply a vpMomentDatabase containing at least the contents of vpMomentCommon.
-
- The features can be retrieved like from a normal vpFeatureMomentDatabase. However, some shortcuts to retrieve the features are provided.
-
- \attention Make sure your object is at least of order 6 when using this pre-filled database.
-
- The following code demonstrates the construction of a 6x6 interaction matrix as described in [1].
- \code
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpPoint.h>
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpServo.h>
-#include <iostream>
-#include <vector>
-
-int main()
-{
- // Define source polygon
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
-
- p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
- p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
- vec_p.push_back(p);
-
-
- vpMomentObject src(6); // Create a source moment object with 6 as maximum order
- src.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
- src.fromVector(vec_p); // Init the dense object with the source polygon
- vec_p.clear();
-
- //Define destination polygon. This is the source polygon translated
- //of 0.1 on x-axis
- p.set_x(-0.1); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(+0.4); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
- p.set_x(+0.3); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3)
- vec_p.push_back(p);
- p.set_x(-0.1); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4)
- vec_p.push_back(p);
- p.set_x(-0.1); p.set_y(0.1); // close the contour (vertex 5 = vertex 1)
- vec_p.push_back(p);
-
- vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order
- dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon
- dst.fromVector(vec_p); // Init the dense object with the destination
- // polygon
-
-
- //init classic moment primitives (for source)
- vpMomentCommon mdb_src(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.);
- //Init classic features
- vpFeatureMomentCommon fmdb_src(mdb_src);
-
- ////init classic moment primitives (for destination)
- vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.);
- //Init classic features
- vpFeatureMomentCommon fmdb_dst(mdb_dst);
-
- //update+compute moment primitives from object (for source)
- mdb_src.updateAll(src);
- //update+compute features (+interaction matrices) from plane
- fmdb_src.updateAll(0.,0.,1.);
-
- //update+compute moment primitives from object (for destination)
- mdb_dst.updateAll(dst);
- //update+compute features (+interaction matrices) from plane
- fmdb_dst.updateAll(0.,0.,1.);
-
- //define visual servoing task
- vpServo task;
- task.setServo(vpServo::EYEINHAND_CAMERA);
- task.setInteractionMatrixType(vpServo::CURRENT);
-
- //Add all classic features to the task
- //In this example, source and destination features are translated by 0.1
- //will produce a movement of 0.1 on x-axis.
- task.addFeature(fmdb_src.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
- task.addFeature(fmdb_src.getFeatureAn(),fmdb_dst.getFeatureAn());
- //the object is NOT symmetric
- //select C4 and C6
- task.addFeature(fmdb_src.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),(1 << 3) | (1 << 5));
- task.addFeature(fmdb_src.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
-
- task.setLambda(1) ;
- vpColVector v = task.computeControlLaw() ;
-
- task.print();
-
- task.kill();
-
- return 0;
-}
- \endcode
-This code produces the following output:
-\code
-Visual servoing task:
-Type of control law
-Eye-in-hand configuration
-Control in the camera frame
-List of visual features : s
-0.0166667,-0.00833333,
-1,
--0.312148,0.0249916,
--1.43449,
-List of desired visual features : s*
-0.116667,-0.00833333,
-1,
--0.312148,0.0249916,
--1.43449,
-Interaction Matrix Ls
--1 0 -6.938893904e-18 0.007291666667 -1.06875 -0.008333333333
-0 -1 3.469446952e-18 1.0171875 -0.007291666667 -0.01666666667
-0 0 -1 0.0125 0.025 0
-0 0 -4.585529113e-15 -0.2983860943 0.5832596643 -4.376751552e-16
-0 0 -3.58244462e-15 0.08633028234 -0.2484618767 3.63421192e-16
-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
-
-\endcode
-*/
-class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase{
-private:
- vpFeatureMomentGravityCenter featureGravity;
- vpFeatureMomentGravityCenterNormalized featureGravityNormalized;
- vpFeatureMomentAreaNormalized featureAn;
- vpFeatureMomentCInvariant featureCInvariant;
- vpFeatureMomentAlpha featureAlpha;
- vpFeatureMomentCentered featureCentered;
- vpFeatureMomentBasic featureMomentBasic;
- vpFeatureMomentArea feature_moment_area;
-
-public:
- vpFeatureMomentCommon(vpMomentDatabase& moments,double A=0.0,double B=0.0,double C=1.0);
- void updateAll(double A,double B,double C);
- /*!
- Returns alpha.
- */
- vpFeatureMomentAlpha& getFeatureAlpha(){ return featureAlpha;}
-
- /*!
- Returns normalized surface.
- */
- vpFeatureMomentAreaNormalized& getFeatureAn(){ return featureAn;}
- /*!
- Returns basic moment.
- */
- vpFeatureMomentBasic& getFeatureMomentBasic(){ return featureMomentBasic;}
- /*!
- Returns centered moments.
- */
- vpFeatureMomentCentered& getFeatureCentered(){ return featureCentered;}
-
- /*!
- Returns non-symmetric invariants.
- */
- vpFeatureMomentCInvariant& getFeatureCInvariant(){ return featureCInvariant;}
- /*!
- Returns normalized gravity center.
- */
- vpFeatureMomentGravityCenterNormalized& getFeatureGravityNormalized(){ return featureGravityNormalized;}
- /*!
- Returns the area
- */
- vpFeatureMomentArea& getFeatureArea(){ return feature_moment_area;}
- /*!
- Returns gravity center
- */
- vpFeatureMomentGravityCenter& getFeatureGravityCenter(){ return featureGravity;}
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentDatabase.cpp b/src/visual-feature/vpFeatureMomentDatabase.cpp
deleted file mode 100644
index be39434..0000000
--- a/src/visual-feature/vpFeatureMomentDatabase.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentDatabase.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pseudo-database used to handle dependencies between moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMoment.h>
-#include <typeinfo>
-#include <iostream>
-#include <visp/vpConfig.h>
-
-/*!
- Add a moment and it's corresponding name to the database
- \param featureMoment : database for moment features
- \param name : the feature's name, usually the string naming it's class. Each name must be unique
-*/
-void vpFeatureMomentDatabase::add(vpFeatureMoment& featureMoment,char* name){
- featureMomentsDataBase.insert(std::pair<const char*,vpFeatureMoment*>((const char*)name,&featureMoment));
-}
-
-/*!
- Retrieves a moment feature from the database
- \param type : the name of the feature, the one specified when using add
- \param found : true if the type string is found inside the database, false otherwise
-
- \return the moment feature corresponding to the type string
-*/
-vpFeatureMoment& vpFeatureMomentDatabase::get(const char* type, bool& found){
- std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator it = featureMomentsDataBase.find(type);
-
- found = (it!=featureMomentsDataBase.end());
- return *(it->second);
-}
-
-/*!
- Update all moment features in the database with plane coefficients
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
-*/
-void vpFeatureMomentDatabase::updateAll(double A, double B, double C)
-{
- std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
-#ifdef VISP_HAVE_OPENMP
- std::vector<vpFeatureMoment*> values;
- values.reserve(featureMomentsDataBase.size());
- for(itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); itr++){
- values.push_back((*itr).second);
- }
- unsigned int i_;
- #pragma omp parallel for shared(A,B,C)
- for(int i=0;i<(int)values.size();i++){
- i_ = static_cast<unsigned int>(i);
- values[i_]->update(A,B,C);
- }
-#else
- for(itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); itr++){
- (*itr).second->update(A,B,C);
- }
-#endif
-}
-
-/*
-std::ostream & operator<<(std::ostream & os, const vpFeatureMomentDatabase& m){
- std::map<const char*,vpMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
- os << "{";
-
- for(itr = m.featureMoments.begin(); itr != m.featureMoments.end(); itr++){
- os << (*itr).first << ": [" << *((*itr).second) << "],";
- }
- os << "}";
-
- return os;
-}*/
diff --git a/src/visual-feature/vpFeatureMomentDatabase.h b/src/visual-feature/vpFeatureMomentDatabase.h
deleted file mode 100644
index f7457c5..0000000
--- a/src/visual-feature/vpFeatureMomentDatabase.h
+++ /dev/null
@@ -1,180 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentDatabase.h 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Pseudo-database used to handle dependencies between moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-/*!
- \file vpFeatureMomentDatabase.h
- \brief Pseudo-database used to handle dependencies between moment features.
-*/
-
-#ifndef __FEATUREMOMENTDATABASE_H__
-#define __FEATUREMOMENTDATABASE_H__
-
-#include <visp/vpConfig.h>
-#include <map>
-#include <iostream>
-#include <cstring>
-
-class vpFeatureMoment;
-class vpMomentObject;
-/*!
- \class vpFeatureMomentDatabase
-
- \ingroup VsFeature2
-
- \brief This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
-
- Like moments (implemented in vpMoment... classes), a vpFeatureMoment needs to have access to other vpFeatureMoment's values to be computed.
- In most cases, a vpFeatureMoment needs both: vpMoments and vpFeatureMoments which explains the two databases (see vpFeatureMoment::vpFeatureMoment).
- For example vpFeatureMomentAlpha needs additionnal information about centered moments vpMomentCentered AND their interaction matrices obtained
- by vpFeatureMomentCentered in order to compute the moment's value from a vpMomentObject.
- Like the vpMomentCentered is stored in a vpMomentDatabase, the vpFeatureMomentCentered should be stored in a vpFeatureMomentDatabase.
-
- All moment features in a database can access each other freely at any time. They can also verify if a moment feature is present in the database or not.
- This code illustrates the use of both databases to handle dependencies between moment primitives and moment features:
- \code
-#include <visp/vpPoint.h>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentCInvariant.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-
-#include <visp/vpFeatureMomentCInvariant.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpFeatureMomentCentered.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <iostream>
-#include <vector>
-
-int main()
-{
- try{
-
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
-
- //////////////////////////////REFERENCE VALUES////////////////////////////////
- vpMomentObject obj(6); // Init object of order 6 because we are
- // computing C-invariants
- obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
- obj.fromVector(vec_p);
-
-
- vpMomentDatabase mdb; //database for moment primitives. This will
- //only contain the basic moment.
- vpMomentCentered mc; //Centered moment
- vpMomentBasic bm; //Basic moment
- vpMomentGravityCenter gc; //gravity center
- vpMomentCInvariant ci; //C-type invariant
-
- bm.linkTo(mdb); //add basic moment to moment database
- mc.linkTo(mdb); //add centered moment to moment database
- gc.linkTo(mdb); //add gravity center to moment database
- ci.linkTo(mdb); //add C-invariant to moment database
-
- vpFeatureMomentDatabase fmdb; //feature moment database to store
- //feature dependencies
-
- //Declare and link moments to database
- vpFeatureMomentBasic fmb(mdb,0.,0.,1.,&fmdb); fmb.linkTo(fmdb);
- vpFeatureMomentCentered fmc(mdb,0.,0.,1.,&fmdb); fmc.linkTo(fmdb);
- vpFeatureMomentCInvariant fci(mdb,0.,0.,1.,&fmdb); fci.linkTo(fmdb);
-
-
- //update the whole moment database
- mdb.updateAll(obj);
-
- //Compute moments in the correct order with the object
- bm.compute();
- gc.compute();
- mc.compute();
- ci.compute();
-
- //update the whole feature moment database with a plane
- fmb.update(0.,0.,1.);
- fmc.update(0.,0.,1.);
- fci.update(0.,0.,1.);
-
- std::cout << fci.interaction(vpFeatureMomentCInvariant::selectC1()) << std::endl;
- }catch(vpException e){
- std::cout << e.getMessage() << std::endl;
- }
-
- return 0;
-}
-\endcode
-*/
-class VISP_EXPORT vpFeatureMomentDatabase{
- private:
- struct cmp_str {
- 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<const char*,vpFeatureMoment*,cmp_str> featureMomentsDataBase;
- void add(vpFeatureMoment& featureMoment,char* name);
- public:
- /*!
- Default constructor.
- */
- vpFeatureMomentDatabase() : featureMomentsDataBase() {}
- /*!
- Virtual destructor that does nothing.
- */
- virtual ~vpFeatureMomentDatabase() {}
- virtual void updateAll(double A=0.0, double B=0.0, double C=1.0);
-
- vpFeatureMoment& get(const char* type, bool& found);
-
- friend VISP_EXPORT std::ostream & operator<<(std::ostream& os, const vpFeatureMomentDatabase& m);
- friend class vpFeatureMoment;
-};
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentGravityCenter.cpp b/src/visual-feature/vpFeatureMomentGravityCenter.cpp
deleted file mode 100644
index 425d757..0000000
--- a/src/visual-feature/vpFeatureMomentGravityCenter.cpp
+++ /dev/null
@@ -1,163 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <visp/vpFeatureMomentDatabase.h>
-
-
-/*!
- Computes interaction matrix for gravity center moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpFeatureMomentBasic
-
- Minimum vpMomentObject order needed to compute this feature: 2.
-*/
-void vpFeatureMomentGravityCenter::compute_interaction(){
- bool found_featuremoment_basic;
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
- const vpMomentObject& momentObject = moment->getObject();
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
- interaction_matrices[0] = featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) - momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
- interaction_matrices[1] = featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) - momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
-
-
-}
-
-#else
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-#include <visp/vpMomentObject.h>
-
-
-/*!
- Computes interaction matrix for gravity center moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpMomentGravityCenter
-
- Minimum vpMomentObject order needed to compute this feature: 2.
-*/
-void vpFeatureMomentGravityCenter::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
-
-
- const vpMomentObject& momentObject = moment->getObject();
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
- int epsilon;
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- epsilon=1;
- } else {
- epsilon=4;
- }
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
- double Zg_inv = A*Xg+B*Yg+C;
- double Xgvz = Xg*Zg_inv+A*epsilon*n20+B*epsilon*n11;
- double Ygvz = Yg*Zg_inv+A*epsilon*n11+B*epsilon*n02;
- double Xgwx = Xg*Yg+epsilon*n11;
- double Ygwy = -Xgwx;
- double Xgwy = -(1+Xg*Xg+epsilon*n20);
- double Ygwx = 1+Yg*Yg+epsilon*n02;
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = -Zg_inv;
- interaction_matrices[0][0][VY] = 0;
- interaction_matrices[0][0][VZ] = Xgvz;
-
- interaction_matrices[0][0][WX] = Xgwx;
- interaction_matrices[0][0][WY] = Xgwy;
- interaction_matrices[0][0][WZ] = Yg;
-
- interaction_matrices[1][0][VX] = 0;
- interaction_matrices[1][0][VY] = -Zg_inv;
- interaction_matrices[1][0][VZ] = Ygvz;
-
- interaction_matrices[1][0][WX] = Ygwx;
- interaction_matrices[1][0][WY] = Ygwy;
- interaction_matrices[1][0][WZ] = -Xg;
-
-}
-
-#endif
diff --git a/src/visual-feature/vpFeatureMomentGravityCenter.h b/src/visual-feature/vpFeatureMomentGravityCenter.h
deleted file mode 100644
index e79af85..0000000
--- a/src/visual-feature/vpFeatureMomentGravityCenter.h
+++ /dev/null
@@ -1,243 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentGravityCenter.h
- \brief Implementation of the interaction matrix computation for vpMomentGravityCenter.
-*/
-#ifndef __FEATUREMOMENTGRAVITYCENTER_H__
-#define __FEATUREMOMENTGRAVITYCENTER_H__
-#include <visp/vpFeatureMoment.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentGravityCenter
-
- \ingroup VsFeature2
-
- \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
-
- The interaction matrix for the is defined in \cite Tahri05z, equation (16).
- It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
- The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
- The following code demonstrates a selection of \f$ L_{y_{g}} \f$:
-
- \code
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentBasic.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpPoint.h>
-#include <visp/vpFeatureMoment.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-#include <visp/vpFeatureMomentBasic.h>
-#include <iostream>
-#include <vector>
-
-int main()
-{
- vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices
-
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
- vec_p.push_back(p);
-
- //////////////////////////////REFERENCE VALUES////////////////////////////////
- vpMomentObject obj(2); // Init object of order 2 because we need
- // vpFeatureMomentBasic of order 1 (for vpFeatureMomentGravityCenter) which
- // implies third-order moment primitives
- obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object
- obj.fromVector(vec_p);
-
-
- vpMomentDatabase mdb; //database for moment primitives. This will
- //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
-
- bm.linkTo(mdb); //add basic moment to moment database
- gc.linkTo(mdb); //add gravity center to moment database
-
- vpFeatureMomentDatabase fmdb; //feature moment database to store
- //feature dependencies
-
- //Declare and link moments to database
- vpFeatureMomentBasic fmb(mdb,0.,0.,1.,&fmdb); fmb.linkTo(fmdb);
- vpFeatureMomentGravityCenter fgc(mdb,0.,0.,1.,&fmdb); fgc.linkTo(fmdb);
-
- //update and compute the vpMomentBasic before computing vpMomentGravityCenter
- bm.update(obj);
- bm.compute();
- //update and compute the vpMomentGravityCenter before computing vpFeatureMomentBasic
- gc.update(obj);
- gc.compute();
-
- fmb.update(0.,0.,1.); //update the vpFeatureMoment with a plane
- //configuration and compute interaction matrix
-
- fgc.update(0.,0.,1.); //update the plane configuration for gravity
- //center feature and compute it's associated matrix.
-
- std::cout << fgc.interaction(1 << 1) << std::endl;
-
- return 0;
-}
-
- \endcode
-
- This code produces the following output:
- \code
-0 -1 1.5 3.5 -2.5 -1.5
- \endcode
-
- You can also use the shortcut selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
-
- \code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
- \endcode
- This feature depends on:
- - vpFeatureMomentBasic
-
- Minimum vpMomentObject order needed to compute this feature: 2.
-*/
-class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
- */
- vpFeatureMomentGravityCenter(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentGravityCenter";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentGravityCenter";}
-
- /*!
- Shortcut selector for \f$x_g\f$.
- */
- static unsigned int selectXg(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_g\f$.
- */
- static unsigned int selectYg(){ return 1 << 1; }
-
-};
-
-#else
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentGravityCenter
-
- \ingroup VsFeature2
-
- \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
-
- The interaction matrix for the is defined in \cite Tahri05z, equation (16).
- It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
- The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
-
- You can use the selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
-
- \code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
- \endcode
- This feature depends on:
- - vpMomentCentered
- - vpMomentGravityCenter
-
- Minimum vpMomentObject order needed to compute this feature: 2.
-*/
-class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
- */
- vpFeatureMomentGravityCenter(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentGravityCenter";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentGravityCenter";}
-
- /*!
- Shortcut selector for \f$x_g\f$.
- */
- static unsigned int selectXg(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_g\f$.
- */
- static unsigned int selectYg(){ return 1 << 1; }
-
-};
-
-#endif
-#endif
diff --git a/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp b/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
deleted file mode 100644
index 88b5905..0000000
--- a/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
+++ /dev/null
@@ -1,221 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Brngne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpMomentGravityCenter.h>
-#include <visp/vpFeatureMomentGravityCenter.h>
-#include <visp/vpFeatureMomentAreaNormalized.h>
-#include <visp/vpFeatureMomentGravityCenterNormalized.h>
-#include <visp/vpFeatureMomentDatabase.h>
-
-/*!
- Computes interaction matrix for centered and normalized moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpFeatureMomentGravityCenter
- - vpMomentGravityCenter
- - vpMomentAreaNormalized
- - vpFeatureMomentAreaNormalized
-*/
-void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
-
- bool found_featuremoment_gravity;
- bool found_featuremoment_surfacenormalized;
-
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- vpFeatureMomentGravityCenter& featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_featuremoment_gravity)));
- vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized&>(featureMomentsDataBase->get("vpFeatureMomentAreaNormalized",found_featuremoment_surfacenormalized)));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- if(!found_featuremoment_gravity) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
- if(!found_featuremoment_surfacenormalized) throw vpException(vpException::notInitialized,"vpFeatureMomentAreaNormalized not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
- interaction_matrices[0] = momentGravity.get()[0]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(1);
- interaction_matrices[1] = momentGravity.get()[1]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(2);
-}
-
-#else
-
-#include <vector>
-#include <limits>
-
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentCentered.h>
-#include <visp/vpFeatureMomentDatabase.h>
-#include <visp/vpFeatureMomentGravityCenterNormalized.h>
-#include <visp/vpMomentAreaNormalized.h>
-#include <visp/vpMomentGravityCenter.h>
-
-
-/*!
- Computes interaction matrix for centered and normalized moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
- - vpMomentCentered
- - vpMomentAreaNormalized
- - vpMomentGravityCenter
-*/
-void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
-
- bool found_moment_surface_normalized;
- bool found_moment_gravity;
- bool found_moment_centered;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- const vpMomentObject& momentObject = moment->getObject();
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
-
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double n10 = momentCentered.get(1,0)/momentObject.get(0,0);
- double n01 = momentCentered.get(0,1)/momentObject.get(0,0);
- double n03 = momentCentered.get(0,3)/momentObject.get(0,0);
-
-
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double An=momentSurfaceNormalized.get()[0];
-
- double Xn = An*Xg;
- double Yn = An*Yg;
-
-
- double Xnvx,Xnvy,Xnvz,Xnwx,Xnwy;
- double Ynvx,Ynvy,Ynvz,Ynwx,Ynwy;
-
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- double a = momentCentered.get(2,0)+momentCentered.get(0,2);
-
- double e11 = momentCentered.get(1,1)/a;
- double e12 = momentCentered.get(1,2)/a;
- double e21 = momentCentered.get(2,1)/a;
- double e30 = momentCentered.get(3,0)/a;
-
- double NA = n20+n02;
-
-
- Xnvx = B*Xn*e11-Yn*B-An*C-n02*A*Xn/NA;
- Xnvy = A*Xn*e11+n02*B*Xn/NA;
-
- Xnwx = An*e11*NA+Yn*n10-Xn*Xg*e11+Xn*n01+Xn*n10*e11-Xn*e21+(-Xn*n03+(Xn*n01-Yn*Xg)*n02)/NA;
- Xnwy = -An*NA+Xn*e12+Xn*Xg-An+e11*Xg*Yn-Xn*n01*e11-2*Xn*n10+Xn*e30+n02*An+(-Xn*Xg+Xn*n10)*n02/NA;
-
-
-
- Ynvx = (Yn-n02*Yn/NA)*A+Yn*e11*B;
- Ynvy = (-Xn+e11*Yn)*A+(-Yn+n02*Yn/NA)*B-An*C;
-
- Ynwx = n02*An+Yn*n10*e11-e11*Xg*Yn+An-Yn*e21+Yn*n01+(-Yn*n03+(Yn*n01-Yn*Yg)*n02)/NA;
- Ynwy = -An*e11*NA+Yn*e11*Yg-Yn*n01*e11+Yn*Xg+Yn*e12+Yn*e30-Xn*n01-2*Yn*n10+(Yn*n10-Yn*Xg)*n02/NA;
-
-
- }else{
- Xnvx = -An*C-A*Xn-Yn*B;
- Xnvy = (0.5)*B*Xn;
-
- Xnwx = (0.5*(8.*n10-Xg))*Yn+4.*An*n11+4.*n01*Xn;
- Xnwy = (0.5*(-2.-8.*n20))*An+(0.5)*Xn*(-8.*n10+Xg);
-
- Ynvx = (0.5)*A*Yn;
- Ynvy = -(0.5)*B*Yn-C*An-A*Xn;
-
- Ynwx = (0.5)*Yn*(8.*n01-Yg)+(.5*(2.+8.*n02))*An;
- Ynwy = (0.5*(-8.*n10+Xg))*Yn-4.*An*n11-4.*n01*Xn;
-
- }
-
- Ynvz = -A*Ynwy+(-An+Ynwx)*B;
- Xnvz = -A*An-A*Xnwy+B*Xnwx;
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Xnvx;
- interaction_matrices[0][0][VY] = Xnvy;
- interaction_matrices[0][0][VZ] = Xnvz;
-
- interaction_matrices[0][0][WX] = Xnwx;
- interaction_matrices[0][0][WY] = Xnwy;
- interaction_matrices[0][0][WZ] = Yn;
-
- interaction_matrices[1][0][VX] = Ynvx;
- interaction_matrices[1][0][VY] = Ynvy;
- interaction_matrices[1][0][VZ] = Ynvz;
-
- interaction_matrices[1][0][WX] = Ynwx;
- interaction_matrices[1][0][WY] = Ynwy;
- interaction_matrices[1][0][WZ] = -Xn;
-
-}
-#endif
diff --git a/src/visual-feature/vpFeatureMomentGravityCenterNormalized.h b/src/visual-feature/vpFeatureMomentGravityCenterNormalized.h
deleted file mode 100644
index f6a657e..0000000
--- a/src/visual-feature/vpFeatureMomentGravityCenterNormalized.h
+++ /dev/null
@@ -1,255 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureMomentImpl.h 3317 2011-09-06 14:14:47Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Implementation for all supported moment features.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-/*!
- \file vpFeatureMomentGravityCenterNormalized.h
- \brief Implementation of the interaction matrix computation for vpMomentGravityCenterNormalized.
-*/
-#ifndef __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
-#define __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
-#include <visp/vpFeatureMoment.h>
-#ifdef VISP_MOMENTS_COMBINE_MATRICES
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentGravityCenterNormalized
-
- \ingroup VsFeature2
-
- \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
-
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equation (19).
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrices for \f$ (x_n,y_n) \f$.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
- The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
- You can use these shortcut selectors as follows:
-
- \code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
- \endcode
-
- The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
-
- This feature is often used in moment-based visual servoing to control the planar translation parameters.
-
- Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
-
- This feature depends on:
- - vpFeatureMomentGravityCenter
- - vpMomentGravityCenter
- - vpMomentAreaNormalized
- - vpFeatureMomentAreaNormalized
-
-*/
-class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentGravityCenterNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
-
- /*!
- Shortcut selector for \f$x_n\f$.
- */
- static unsigned int selectXn(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_n\f$.
- */
- static unsigned int selectYn(){ return 1 << 1; }
-
-};
-
-#else
-class vpMomentDatabase;
-/*!
- \class vpFeatureMomentGravityCenterNormalized
-
- \ingroup VsFeature2
-
- \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
-
- It computes the interaction matrices for \f$ (x_n,y_n) \f$.
- The interaction matrix for the moment feature has the following expression:
- - In the discrete case:
- \f[
- L_{x_n} =
-{
- \left[ \begin {array}{c} -Ax_{{n}}\theta+ \left( x_{{n}}e_{{1,1}}-y_{
-{n}} \right) B-a_{{n}}C\\ \noalign{\medskip}Ax_{{n}}e_{{1,1}}+Bx_{{n}}
-\theta\\ \noalign{\medskip} \left( -a_{{n}}-w_{{y}} \right) A+Bw_{{x}}
-\\ \noalign{\medskip}a_{{n}}e_{{1,1}}{\it NA}+ \left( \eta_{{1,0}}e_{{
-1,1}}+\eta_{{0,1}}-e_{{2,1}}-x_{{g}}e_{{1,1}}+\eta_{{0,1}}\theta
- \right) x_{{n}}+ \left( \eta_{{1,0}}-x_{{g}}\theta \right) y_{{n}}-{
-\frac {x_{{n}}\eta_{{0,3}}}{{\it NA}}}\\ \noalign{\medskip} \left( -1+
-\theta \right) a_{{n}}{\it NA}+ \left( e_{{1,2}}+x_{{g}}-\eta_{{0,1}}e
-_{{1,1}}-2\,\eta_{{1,0}}+e_{{3,0}}+ \left( -x_{{g}}+\eta_{{1,0}}
- \right) \theta \right) x_{{n}}+e_{{1,1}}x_{{g}}y_{{n}}-a_{{n}}
-\\ \noalign{\medskip}y_{{n}}\end {array} \right]
-}^t
-
- L_{y_n} =
-
-
-{
- \left[ \begin {array}{c} \left( 1-\theta \right) y_{{n}}A+y_{{n}}e_{
-{1,1}}B\\ \noalign{\medskip} \left( -x_{{n}}+y_{{n}}e_{{1,1}} \right)
-A+ \left( -1+\theta \right) y_{{n}}B-a_{{n}}C\\ \noalign{\medskip}-Aw_
-{{y}}+ \left( -a_{{n}}+w_{{x}} \right) B\\ \noalign{\medskip}\theta\,a
-_{{n}}{\it NA}+ \left( -e_{{2,1}}+\eta_{{1,0}}e_{{1,1}}+\eta_{{0,1}}-x
-_{{g}}e_{{1,1}}+ \left( \eta_{{0,1}}-y_{{g}} \right) \theta \right) y_
-{{n}}+a_{{n}}-{\frac {y_{{n}}\eta_{{0,3}}}{{\it NA}}}
-\\ \noalign{\medskip}-a_{{n}}e_{{1,1}}{\it NA}-x_{{n}}\eta_{{0,1}}+
- \left( e_{{1,2}}+y_{{g}}e_{{1,1}}-\eta_{{0,1}}e_{{1,1}}+x_{{g}}+e_{{3
-,0}}-2\,\eta_{{1,0}}+ \left( -x_{{g}}+\eta_{{1,0}} \right) \theta
- \right) y_{{n}}\\ \noalign{\medskip}-x_{{n}}\end {array} \right]
-
-}^t
- \f]
- - In the dense case:
- \f[
- L_{x_n} =
-{
-\left[ \begin {array}{c} -a_{{n}}C-1/2\,Ax_{{n}}-By_{{n}}
-\\ \noalign{\medskip}1/2\,Bx_{{n}}\\ \noalign{\medskip} \left( -a_{{n}
-}-w_{{y}} \right) A+Bw_{{x}}\\ \noalign{\medskip} \left( 4\,\eta_{{1,0
-}}-1/2\,x_{{g}} \right) y_{{n}}+4\,a_{{n}}\eta_{{1,1}}+4\,x_{{n}}\eta_
-{{0,1}}\\ \noalign{\medskip} \left( -4\,\eta_{{1,0}}+1/2\,x_{{g}}
- \right) x_{{n}}+ \left( -1-4\,\eta_{{2,0}} \right) a_{{n}}
-\\ \noalign{\medskip}y_{{n}}\end {array} \right]
-
-
-}^t
-
- L_{y_n} =
-{
- \left[ \begin {array}{c} 1/2\,Ay_{{n}}\\ \noalign{\medskip}-1/2\,By_{
-{n}}-a_{{n}}C-Ax_{{n}}\\ \noalign{\medskip}-Aw_{{y}}+ \left( -a_{{n}}+
-w_{{x}} \right) B\\ \noalign{\medskip}4\,\theta\,a_{{n}}{\it NA}+
- \left( 4\,\eta_{{0,1}}-1/2\,y_{{g}} \right) y_{{n}}+a_{{n}}
-\\ \noalign{\medskip} \left( -4\,\eta_{{1,0}}+1/2\,x_{{g}} \right) y_{
-{n}}-4\,a_{{n}}\eta_{{1,1}}-4\,x_{{n}}\eta_{{0,1}}
-\\ \noalign{\medskip}-x_{{n}}\end {array} \right]
-
-}^t
- \f]
-with:
- - \f$e_{i,j}=\frac{\mu_{i,j}}{NA}\f$
- - \f$NA=\mu_{2,0}+\mu_{0,2}\f$
- - \f$\theta=\frac{\eta_{0,2}}{NA}\f$
- - \f$\eta\f$ is the centered and normalized moment.
-
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
- The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
- You can use these shortcut selectors as follows:
-
- \code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
- \endcode
-
- The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
-
- This feature is often used in moment-based visual servoing to control the planar translation parameters.
-
- Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
-
- This feature depends on:
- - vpFeatureMomentGravityCenter
- - vpMomentGravityCenter
- - vpMomentAreaNormalized
- - vpFeatureMomentAreaNormalized
-
-*/
-class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentGravityCenterNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
-
- /*!
- Shortcut selector for \f$x_n\f$.
- */
- static unsigned int selectXn(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_n\f$.
- */
- static unsigned int selectYn(){ return 1 << 1; }
-
-};
-#endif
-#endif
diff --git a/src/visual-feature/vpFeaturePoint.cpp b/src/visual-feature/vpFeaturePoint.cpp
deleted file mode 100644
index 0e216da..0000000
--- a/src/visual-feature/vpFeaturePoint.cpp
+++ /dev/null
@@ -1,557 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePoint.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeaturePoint.cpp
- \brief Class that defines 2D point visual feature
-*/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeaturePoint.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-
-
-/*
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities ar useful but not mandatory
-
-*/
-
-/*!
- Initialize the memory space requested for 2D point visual feature.
-*/
-void
-vpFeaturePoint::init()
-{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value Z (1 meters)
- Z = 1;
-}
-
-/*!
- Default constructor that build a visual feature.
-*/
-vpFeaturePoint::vpFeaturePoint() : Z(1.)
-{
- init() ;
-}
-
-
-/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \param Z_ : \f$ Z \f$ value to set.
-*/
-void
-vpFeaturePoint::set_Z(const double Z_)
-{
- this->Z = Z_ ;
- flags[2] = true;
-}
-
-
-/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
-
- \return The value of \f$ Z \f$.
-*/
-double
-vpFeaturePoint::get_Z() const
-{
- return Z ;
-}
-
-
-/*!
-
- Set the value of \f$ x \f$ which represents the x coordinate of the point
- in image plan. It is one parameter of the visual feature \f$ s \f$.
-
- \param x : \f$ x \f$ value to set.
-*/
-void
-vpFeaturePoint::set_x(const double x)
-{
- s[0] = x ;
- flags[0] = true;
-}
-
-
-/*!
- Get the value of \f$ x \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
-
- \return The value of \f$ x \f$.
-*/
-double
-vpFeaturePoint::get_x() const
-{
- return s[0] ;
-}
-
-
-/*!
- Set the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
-
- \param y : \f$ y \f$ value to set.
-*/
-void
-vpFeaturePoint::set_y(const double y)
-{
- s[1] = y ;
- flags[1] = true;
-}
-
-
-/*!
- Get the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
-
- \return The value of \f$ y \f$.
-*/
-double
-vpFeaturePoint::get_y() const
-{
- return s[1] ;
-}
-
-
-/*!
-
- Set the value of \f$ x \f$, \f$ y \f$ and \f$ Z \f$. \f$ x \f$ and \f$ y \f$
- represent the coordinates of the point in the image plan and are the
- parameters of the visual feature \f$ s \f$. \f$ Z \f$ is the 3D coordinate in the camera frame
- representing the depth.
-
- \param x_ : \f$ x \f$ value to set.
- \param y_ : \f$ y \f$ value to set.
- \param Z_ : \f$ Z \f$ value to set.
-*/
-void
-vpFeaturePoint::set_xyZ(const double x_,
- const double y_,
- const double Z_)
-{
- set_x(x_) ;
- set_y(y_) ;
- set_Z(Z_) ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L \f$. The computation is made
- thanks to the values of the point features \f$ x \f$ and \f$ y \f$ and the
- depth \f$ Z \f$.
-
- \f[ L = \left[\begin{array}{c}L_{x} \\ L_{y}\end{array}\right] =
- \left[\begin{array}{cccccc}
- -1/Z & 0 & x/Z & xy & -(1+x^2) & y \\
- 0 & -1/Z & y/Z & 1+y^2 & -xy & -x
- \end{array}\right]\f]
-
- \param select : Selection of a subset of the possible point features.
- - To compute the interaction matrix for all the two point features use
- vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
- matrix is \f$ [2 \times 6] \f$
- - To compute the interaction matrix for only one of the point component
- feature (\f$ x, y \f$) use one of the corresponding function selectX() or
- selectY(). In that case the returned interaction matrix is \f$ [1 \times 6]
- \f$ dimension.
-
- \return The interaction matrix computed from the point features.
-
- The code below shows how to compute the interaction matrix associated to the
- visual feature \f$ s = x \f$.
- \code
- // Creation of the current feature s
- vpFeaturePoint s;
- s.buildFrom(0, 0, 1);
-
- vpMatrix L_x = s.interaction( vpFeaturePoint::selectX() );
- \endcode
-
- The code below shows how to compute the interaction matrix associated to the
- visual feature \f$ s = (x, y) \f$.
- \code
- // Creation of the current feature s
- vpFeaturePoint s;
- s.buildFrom(0, 0, 1);
-
- vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-*/
-vpMatrix
-vpFeaturePoint::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- double x_ = get_x() ;
- double y_ = get_y() ;
- double Z_ = get_Z() ;
-
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z_) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- if (vpFeaturePoint::selectX() & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
-
- Lx[0][0] = -1/Z_ ;
- Lx[0][1] = 0 ;
- Lx[0][2] = x_/Z_ ;
- Lx[0][3] = x_*y_ ;
- Lx[0][4] = -(1+x_*x_) ;
- Lx[0][5] = y_ ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeaturePoint::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
-
- Ly[0][0] = 0 ;
- Ly[0][1] = -1/Z_ ;
- Ly[0][2] = y_/Z ;
- Ly[0][3] = 1+y_*y_ ;
- Ly[0][4] = -x_*y_ ;
- Ly[0][5] = -x_ ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
- return L ;
-}
-
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- \param s_star : Desired visual feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible point features.
- - To compute the error for all the two point features use vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2 dimension column vector.
- - To compute the error for only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY(). In that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired visual feature.
-
- The code below shows how to use this method to manipulate the \f$ x \f$ subset:
- \code
- // Creation of the current feature s
- vpFeaturePoint s;
- s.buildFrom(0, 0, 1);
-
- // Creation of the desired feature s*
- vpFeaturePoint s_star;
- s_star.buildFrom(1, 1, 1);
-
- // Compute the interaction matrix for the x feature
- vpMatrix L_x = s.interaction( vpFeaturePoint::selectX() );
-
- // Compute the error vector (s-s*) for the x feature
- s.error(s_star, vpFeaturePoint::selectX());
- \endcode
-*/
-vpColVector
-vpFeaturePoint::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeaturePoint::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeaturePoint::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matrix related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
-
- return e ;
-
-}
-
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible point features.
- - To print all the two point features use vpBasicFeature::FEATURE_ALL.
- - To print only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY().
-
- \code
- vpFeaturePoint s; // Current visual feature s
-
- // Creation of the current feature s
- s.buildFrom(0, 0, 1);
-
- s.print(); // print all the 2 components of the feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeaturePoint::selectX()); // print only the x component
- \endcode
-*/
-void
-vpFeaturePoint::print(const unsigned int select ) const
-{
-
- std::cout <<"Point: Z=" << get_Z() ;
- if (vpFeaturePoint::selectX() & select )
- std::cout << " x=" << get_x() ;
- if (vpFeaturePoint::selectY() & select )
- std::cout << " y=" << get_y() ;
- std::cout <<std::endl ;
-}
-
-
-/*!
- Build a 2D point visual feature from the point coordinates in the image plan \f$ x \f$ and \f$ y \f$. The parameter Z which describes the depth, is set in the same time.
-
- See the vpFeaturePoint class description for more details about \f$ x \f$ and \f$ y \f$.
-
- \param x_ : The \f$ x \f$ parameter.
- \param y_ : The \f$ y \f$ parameter.
- \param Z_ : The \f$ Z \f$ parameter.
-*/
-void
-vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_)
-{
-
- s[0] = x_ ;
- s[1] = y_ ;
-
- this->Z = Z_ ;
-
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z_) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-
-}
-
-
-/*!
-
- Display point feature.
-
- \param cam : Camera parameters.
- \param I : Image.
- \param color : Color to use for the display.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeaturePoint::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Display point feature.
-
- \param cam : Camera parameters.
- \param I : color Image.
- \param color : Color to use for the display.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeaturePoint::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*!
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeaturePoint s;
- s_star = s.duplicate(); // s_star is now a vpFeaturePoint
- \endcode
-
-*/
-vpFeaturePoint *vpFeaturePoint::duplicate() const
-{
- vpFeaturePoint *feature = new vpFeaturePoint ;
- return feature ;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeaturePoint.h b/src/visual-feature/vpFeaturePoint.h
deleted file mode 100644
index 932f08a..0000000
--- a/src/visual-feature/vpFeaturePoint.h
+++ /dev/null
@@ -1,306 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePoint.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point visual feature.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#ifndef vpFeaturePoint_H
-#define vpFeaturePoint_H
-
-/*!
- \file vpFeaturePoint.h
- \brief Class that defines 2D point visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpPoint.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeaturePoint
- \ingroup VsFeature2
-
- \brief Class that defines a 2D point visual feature \f$ s\f$ which
- is composed by two parameters that are the cartesian coordinates \f$
- x \f$ and \f$ y \f$.
-
- In this class \f$ x \f$ and \f$ y \f$ are the 2D coordinates in the
- image plane and are given in meter. \f$ Z \f$ which is the 3D
- coordinate representing the depth is also a parameter of the
- point. It is needed during the computation of the interaction matrix
- \f$ L \f$.
-
- The visual features can be set easily from an instance of the
- classes vpPoint, vpDot or vpDot2. For more precision see the
- vpFeatureBuilder class.
-
- Once the values of the visual features are set, the interaction()
- method allows to compute the interaction matrix \f$ L \f$ associated
- to the visual feature, while the error() method computes the error
- vector \f$(s - s^*)\f$ between the current visual feature and the
- desired one.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 2D point feature \f$(x,y)\f$ that correspond to the 2D
- coordinates of a point in the image plane. To control six degrees
- of freedom, at least four other features must be considered like two
- other point features for example. First we create a current
- (\f$s\f$) 2D point feature. Then we set the task to use the
- interaction matrix associated to the current feature \f$L_s\f$. And
- finally we compute the camera velocity \f$v=-\lambda \; L_s^+ \;
- (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
- loop.
-
- \code
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpFeaturePoint sd; //The desired point feature.
- //Set the desired features x and y
- double xd = 0;
- double yd = 0;
- //Set the depth of the point in the camera frame.
- double Zd = 1;
- //Set the point feature thanks to the desired parameters.
- sd.buildFrom(xd, yd, Zd);
-
- vpFeaturePoint s; //The current point feature.
- //Set the current features x and y
- double x; //You have to compute the value of x.
- double y; //You have to compute the value of y.
- double Z; //You have to compute the value of Z.
- //Set the point feature thanks to the current parameters.
- s.buildFrom(x, y, Z);
- //In this case the parameter Z is not necessary because the interaction matrix is computed
- //with the desired visual feature.
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the desired visual features sd
- task.setInteractionMatrixType(vpServo::DESIRED);
-
- // Add the 2D point feature to the task
- task.addFeature(s, sd);
-
- // Control loop
- for ( ; ; ) {
- // The new parameters x and y must be computed here.
-
- // Update the current point visual feature
- s.buildFrom(x, y, Z);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
- return 0;
-}
- \endcode
-
- If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
- build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpFeaturePoint sd; //The desired point feature.
- //Set the desired features x and y
- double xd = 0;
- double yd = 0;
- //Set the depth of the point in the camera frame.
- double Zd = 1;
- //Set the point feature thanks to the desired parameters.
- sd.buildFrom(xd, yd, Zd);
-
- vpFeaturePoint s; //The current point feature.
- //Set the current features x and y
- double x; //You have to compute the value of x.
- double y; //You have to compute the value of y.
- double Z; //You have to compute the value of Z.
- //Set the point feature thanks to the current parameters.
- s.buildFrom(x, y, Z);
-
- // Compute the interaction matrix L_s for the current point feature
- vpMatrix L = s.interaction();
- // You can also compute the interaction matrix L_s for the desired point feature
- // The corresponding line of code is : vpMatrix L = sd.interaction();
-
- // Compute the error vector (s-sd) for the point feature
- s.error(s_star);
-}
- \endcode
-
- An other fully explained example is given in the \ref tutorial-ibvs.
-
-*/
-
-
-class VISP_EXPORT vpFeaturePoint : public vpBasicFeature
-{
-private:
- //! FeaturePoint depth (required to compute the interaction matrix)
- //! default Z = 1m
- double Z ;
-
-public:
-
- void init() ;
-
- vpFeaturePoint() ;
- //! Destructor.
- virtual ~vpFeaturePoint() {}
-
-
- /*
- section Set coordinates
- */
-
- void buildFrom(const double x, const double y, const double Z) ;
-
- void set_x(const double x) ;
-
- void set_y(const double y) ;
-
- void set_Z(const double Z) ;
-
- void set_xyZ(const double x, const double y, const double Z) ;
-
- double get_x() const ;
-
- double get_y() const ;
-
- double get_Z() const ;
-
-
- /*
- vpBasicFeature method instantiation
- */
-
- // feature selection
- /*!
-
- Function used to select the \f$ x \f$ subset of the point visual feature.
-
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ x \f$.
-
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePoint s;
- vpServo task;
- ...
- // Add the (x) subset features from the 2D point
- task.addFeature(s, vpFeaturePoint::selectX());
- \endcode
- */
- inline static unsigned int selectX() { return FEATURE_LINE[0] ; }
-
- /*!
-
- Function used to select the \f$ y \f$ subset of the point visual feature.
-
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ y \f$.
-
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePoint s;
- vpServo task;
- ...
- // Add the (y) subset features from the 2D point
- task.addFeature(s, vpFeaturePoint::selectY());
- \endcode
- */
- inline static unsigned int selectY() { return FEATURE_LINE[1] ; }
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
-
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- vpFeaturePoint *duplicate() const ;
-
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-
- /*!
- @name Deprecated functions
- */
- typedef enum
- {
- X = 1, // x coordinates
- Y = 2 // y coordinates
- } vpFeaturePointType;
- //! Compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeaturePoint3D.cpp b/src/visual-feature/vpFeaturePoint3D.cpp
deleted file mode 100644
index 3cea622..0000000
--- a/src/visual-feature/vpFeaturePoint3D.cpp
+++ /dev/null
@@ -1,631 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePoint3D.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 3D point visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeaturePoint3D.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-
-
-
-/*
-
- attributes and members directly related to the vpBasicFeature needs
- other functionalities are useful but not mandatory
-
-*/
-
-/*!
-
- Initialise the memory space requested for a 3D point visual
- feature.
-
- By default this feature is initialized to \f${\bf X} = (0, 0, 1)\f$.
-*/
-void
-vpFeaturePoint3D::init()
-{
- //feature dimension
- dim_s = 3 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value XYZ
- s[0] = 0;
- s[1] = 0;
- s[2] = 1;
-
-}
-
-/*!
-
- Default constructor that build a 3D point visual feature and
- initialize it to \f${\bf X} = (0, 0, 1)\f$.
-
-*/
-vpFeaturePoint3D::vpFeaturePoint3D()
-{
- init() ;
-}
-
-
-/*!
-
- Initialise the \f$X\f$ coordinate in the camera frame of the 3D Point
- visual feature \f${\bf X} = (X,Y,Z)\f$.
-
- \param X : \f$X\f$ coordinate of the visual feature.
- \sa get_X()
-
-*/
-void
-vpFeaturePoint3D::set_X(const double X)
-{
- s[0] = X ;
- flags[0] = true;
-}
-
-/*!
-
- Initialise the \f$Y\f$ coordinate in the camera frame of the 3D Point
- visual feature \f${\bf X} = (X,Y,Z)\f$.
-
- \param Y : \f$Y\f$ coordinate of the visual feature.
- \sa get_Y()
-
-*/
-void
-vpFeaturePoint3D::set_Y(const double Y)
-{
- s[1] = Y ;
- flags[1] = true;
-}
-
-/*!
-
- Initialise the \f$Z\f$ coordinate in the camera frame of the 3D Point
- visual feature \f${\bf X} = (X,Y,Z)\f$.
-
- \param Z : \f$Z\f$ coordinate or depth of the visual feature.
- \sa get_Z()
-
-*/
-void
-vpFeaturePoint3D::set_Z(const double Z)
-{
- s[2] = Z ;
- flags[2] = true;
-}
-
-/*!
- Initialize the 3D point coordinates.
-
- \param X,Y,Z : \f$(X,Y,Z)\f$ coordinates in the camera frame of the
- 3D point visual feature.
-
- \sa set_X(), set_Y(), set_Z()
-*/
-void
-vpFeaturePoint3D::set_XYZ(const double X,
- const double Y,
- const double Z)
-{
- set_X(X) ;
- set_Y(Y) ;
- set_Z(Z) ;
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-//! Return the \f$X\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_X() const
-{
- return s[0] ;
-}
-
-//! Return the \f$Y\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_Y() const
-{
- return s[1] ;
-}
-
-//! Return the \f$Z\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_Z() const
-{
- return s[2] ;
-}
-
-/*!
- Compute and return the interaction matrix \f$ L \f$ associated to a subset
- of the possible 3D point features \f$(X,Y,Z)\f$ that
- represent the 3D point coordinates expressed in the camera frame.
-
- \f[
- L = \left[
- \begin{array}{rrrrrr}
- -1 & 0 & 0 & 0 & -Z & Y \\
- 0 & -1 & 0 & Z & 0 & -X \\
- 0 & 0 & -1 & -Y & X & 0 \\
- \end{array}
- \right]
- \f]
-
-
- \param select : Selection of a subset of the possible 3D point coordinate
- features.
- - To compute the interaction matrix for all the three
- subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
- that case the dimension of the interaction matrix is \f$ [3 \times
- 6] \f$
- - To compute the interaction matrix for only one of the
- subset (\f$X, Y,Z\f$) use
- one of the corresponding function selectX(), selectY() or
- selectZ(). In that case the returned interaction matrix is \f$ [1
- \times 6] \f$ dimension.
-
- \return The interaction matrix computed from the 3D point coordinate
- features.
-
- The code below shows how to compute the interaction matrix
- associated to the visual feature \f$s = X \f$.
-
- \code
- vpPoint point;
- ...
- // Creation of the current feature s
- vpFeaturePoint3D s;
- s.buildFrom(point);
-
- vpMatrix L_X = s.interaction( vpFeaturePoint3D::selectX() );
- \endcode
-
- The code below shows how to compute the interaction matrix
- associated to the \f$s = (X,Y) \f$
- subset visual feature:
-
- \code
- vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY() );
- \endcode
-
- L_XY is here now a 2 by 6 matrix. The first line corresponds to
- the \f$ X \f$ visual feature while the second one to the \f$
- Y \f$ visual feature.
-
- It is also possible to build the interaction matrix from all the
- 3D point coordinates by:
-
- \code
- vpMatrix L_XYZ = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-
- In that case, L_XYZ is a 3 by 6 interaction matrix where the last
- line corresponds to the \f$ Z \f$ visual feature.
-
-*/
-vpMatrix
-vpFeaturePoint3D::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- double X = get_X() ;
- double Y = get_Y() ;
- double Z = get_Z() ;
-
- if (vpFeaturePoint3D::selectX() & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
-
- Lx[0][0] = -1 ;
- Lx[0][1] = 0 ;
- Lx[0][2] = 0 ;
- Lx[0][3] = 0 ;
- Lx[0][4] = -Z ;
- Lx[0][5] = Y ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeaturePoint3D::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
-
- Ly[0][0] = 0 ;
- Ly[0][1] = -1 ;
- Ly[0][2] = 0 ;
- Ly[0][3] = Z ;
- Ly[0][4] = 0 ;
- Ly[0][5] = -X ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
- if (vpFeaturePoint3D::selectZ() & select )
- {
- vpMatrix Lz(1,6) ; Lz = 0;
-
- Lz[0][0] = 0 ;
- Lz[0][1] = 0 ;
- Lz[0][2] = -1 ;
- Lz[0][3] = -Y ;
- Lz[0][4] = X ;
- Lz[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Lz) ;
- }
- return L ;
-}
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- \param s_star : Desired 3D point visual feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible 3D point coordinate features.
- - To compute the error for all the three coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
- dimension column vector.
- - To compute the error for only one of the coordinate
- feature \f$(X,Y, or Z)\f$ use one of the
- corresponding function selectX(), selectY() or selectZ(). In
- that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- The code below shows how to use this method to manipulate the \f$
- Z \f$ subset:
-
- \code
- // Creation of the current feature s
- vpFeaturePoint3D s;
- s.set_Z(0.8); // Initialization of the current Z feature
-
- // Creation of the desired feature s*.
- vpFeatureTranslation s_star;
- s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter
-
- // Compute the interaction matrix for the Z coordinate feature
- vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );
-
- // Compute the error vector (s-s*) for the Z feature
- s.error(s_star, vpFeaturePoint3D::selectZ());
- \endcode
-
- To manipulate the subset features \f$s=(Y, Z)\f$,
- the code becomes:
- \code
- // Compute the interaction matrix for the Y, Z feature coordinates
- vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );
-
- // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
- vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
- \endcode
-
-*/
-vpColVector
-vpFeaturePoint3D::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeaturePoint3D::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeaturePoint3D::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- if (vpFeaturePoint3D::selectZ() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2] - s_star[2] ;
- e = vpMatrix::stackMatrices(e,ez) ;
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matrix related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
- return e ;
-
-}
-
-/*!
-
- Build a 3D point visual feature from the camera frame coordinates
- \f$(X,Y,Z)\f$ of a point.
-
- \param p : A point with camera frame coordinates \f${^c}P=(X,Y,Z)\f$
- up to date (see vpPoint class).
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is negative. That means that the 3D point is
- behind the camera which is not possible.
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is null. That means that the 3D point is
- on the camera which is not possible.
-*/
-void
-vpFeaturePoint3D::buildFrom(const vpPoint &p)
-{
-
- // cP is expressed in homogeneous coordinates
- // we devide by the fourth coordinate
- s[0] = p.cP[0]/p.cP[3] ;
- s[1] = p.cP[1]/p.cP[3] ;
- s[2] = p.cP[2]/p.cP[3] ;
-
- double Z = s[2] ;
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-
-}
-
-/*!
-
- Build a 3D point visual feature from the camera frame coordinates
- \f$(X,Y,Z)\f$ of a point.
-
- \param X,Y,Z : Camera frame coordinates \f$(X,Y,Z)\f$ of a 3D point.
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is negative. That means that the 3D point is
- on the camera which is not possible.
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is null. That means that the 3D point is
- on the camera which is not possible.
-
-*/
-void
-vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
-{
-
- s[0] = X ;
- s[1] = Y ;
- s[2] = Z ;
-
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-
-}
-
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible 3D point
- feature coordinates.
- - To print all the three coordinates used as features use
- vpBasicFeature::FEATURE_ALL.
- - To print only one of the coordinate
- feature \f$(X,Y,Z)\f$ use one of the
- corresponding function selectX(), selectX() or selectZ().
-
- \code
- vpPoint point;
-
- // Creation of the current feature s
- vpFeaturePoint3D s;
- s.buildFrom(point);
-
- s.print(); // print all the 3 components of the translation feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeaturePoint3D::selectZ()); // print only the Z component
- \endcode
-*/
-void
-vpFeaturePoint3D::print(const unsigned int select ) const
-{
-
- std::cout <<"Point3D: " ;
- if (vpFeaturePoint3D::selectX() & select )
- std::cout << " X=" << get_X() ;
- if (vpFeaturePoint3D::selectY() & select )
- std::cout << " Y=" << get_Y() ;
- if (vpFeaturePoint3D::selectZ() & select )
- std::cout << " Z=" << get_Z() ;
- std::cout <<std::endl ;
-}
-
-
-/*!
-
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeaturePoint3D s;
- s_star = s.duplicate(); // s_star is now a vpFeaturePoint3D
- \endcode
-
-*/
-vpFeaturePoint3D *vpFeaturePoint3D::duplicate() const
-{
- vpFeaturePoint3D *feature = new vpFeaturePoint3D ;
- return feature ;
-}
-
-/*!
-
- Not implemented.
-*/
-void
-vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*!
-
- Not implemented.
- */
-void
-vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeaturePoint3D.h b/src/visual-feature/vpFeaturePoint3D.h
deleted file mode 100644
index b398d37..0000000
--- a/src/visual-feature/vpFeaturePoint3D.h
+++ /dev/null
@@ -1,366 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePoint3D.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 3D point visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeaturePoint3d_H
-#define vpFeaturePoint3d_H
-
-/*!
- \file vpFeaturePoint3D.h
- \brief class that defines the 3D point visual feature.
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpPoint.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-/*!
- \class vpFeaturePoint3D
- \ingroup VsFeature3
- \brief Class that defines the 3D point visual feature.
-
- A 3D point visual feature corresponds to a 3D point with \f$
- {\bf X} = (X,Y,Z)\f$ coordinates in the camera frame.
-
- This class is intended to manipulate the 3D point visual feature
- \f$ s = (X,Y,Z) \f$. The interaction matrix related to \f$ s \f$ is given by:
- \f[
- L = \left[
- \begin{array}{rrrrrr}
- -1 & 0 & 0 & 0 & -Z & Y \\
- 0 & -1 & 0 & Z & 0 & -X \\
- 0 & 0 & -1 & -Y & X & 0 \\
- \end{array}
- \right]
- \f]
-
- Two ways are allowed to initialize the feature.
-
- - The first way by setting the feature values \f$(X,Y,Z)\f$ using
- vpFeaturePoint3D member fonctions like set_X(), set_Y(), set_Z(),
- or also buildFrom().
-
- - The second by using the feature builder functionalities to
- initialize the feature from a point structure like
- vpFeatureBuilder::create (vpFeaturePoint3D &, const vpPoint &).
-
- The interaction() method allows to compute the interaction matrix
- \f$ L\f$ associated to the 3D point visual feature, while the
- error() method computes the error vector \f$ (s - s^*)\f$ between the
- current visual feature and the desired one.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 3D point feature \f$(X,Y,Z)\f$ that correspond to the
- 3D point coordinates in the camera frame. To control six degrees of
- freedom, at least three other features must be considered like
- vpFeatureThetaU visual features. First we create a current (\f$s\f$)
- and desired (\f$s^*\f$) 3D point feature, set the task to use the
- interaction matrix associated to the desired feature \f$L_{s^*}\f$
- and than compute the camera velocity \f$v=-\lambda \; {L_{s^*}}^+ \;
- (s-s^*)\f$. The current feature \f$s\f$ is updated in the while()
- loop while \f$s^*\f$ is set to \f$Z^*=1\f$.
-
- \code
-#include <iostream>
-#include <visp/vpFeaturePoint3D.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- // Set the 3D point coordinates in the object frame: oP
- vpPoint point;
- point.setWorldCoordinates(0.1, -0.1, 0);
-
- vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
- cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
- // ... cMo need here to be computed from a pose estimation
-
- point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
-
- // Creation of the current feature s
- vpFeaturePoint3D s;
- s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame: s=(X,Y,Z)
- s.print();
-
- // Creation of the desired feature s*.
- vpFeaturePoint3D s_star;
- s_star.buildFrom(0, 0, 1); // Z*=1 meter
- s_star.print();
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the desired visual features s*
- task.setInteractionMatrixType(vpServo::DESIRED);
- // Set the constant gain
- double lambda = 0.8;
- task.setLambda(lambda);
-
- // Add the 3D point feature to the task
- task.addFeature(s, s_star);
-
- // Control loop
- for ( ; ; ) {
- // ... cMo need here to be estimated from for example a pose estimation.
- point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
-
- // Update the current 3D point visual feature
- s.buildFrom(point);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
-}
- \endcode
-
- If you want to deal only with the \f$(X,Y)\f$ subset feature from the 3D
- point feature, you have just to modify the addFeature() call in
- the previous example by the following line. In that case, the dimension
- of \f$s\f$ is two.
-
- \code
- // Add the (X,Y) subset feature from the 3D point visual feature to the task
- task.addFeature(s, s_star, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
- \endcode
-
- If you want to build your own control law, this other example shows
- how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
- point visual feature, compute the corresponding error
- vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
-
- \code
-#include <iostream>
-#include <visp/vpFeaturePoint3D.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- // Set the 3D point coordinates in the object frame: oP
- vpPoint point;
- point.setWorldCoordinates(0.1, -0.1, 0);
-
- vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
- cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
- // ... cMo need here to be computed from a pose estimation
-
- point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
-
- // Creation of the current feature s
- vpFeaturePoint3D s;
- s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame
- s.print();
-
- // Creation of the desired feature s*.
- vpFeaturePoint3D s_star;
- s_star.buildFrom(0, 0, 1); // Z*=1 meter
- s_star.print();
-
- // Compute the L_s interaction matrix associated to the current feature
- vpMatrix L = s.interaction();
- std::cout << "L: " << L << std::endl;
-
- // Compute the error vector (s-s*) for the 3D point feature
- vpColVector e = s.error(s_star); // e = (s-s*)
-
- std::cout << "e: " << e << std::endl;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpFeaturePoint3D : public vpBasicFeature
-
-{
-
-public:
- // basic construction
- void init() ;
- // basic constructor
- vpFeaturePoint3D() ;
- //! Destructor. Does nothing.
- virtual ~vpFeaturePoint3D() {}
-
- /*
- /section Set coordinates
- */
-
- // build feature from a point (vpPoint)
- void buildFrom(const vpPoint &p) ;
- // set the point XY and Z-coordinates
- void buildFrom(const double X, const double Y, const double Z) ;
-
- // set the point X-coordinates
- void set_X(const double X) ;
- // set the point Y-coordinates
- void set_Y(const double Y) ;
- // set the point depth (camera frame)
- void set_Z(const double Z) ;
- // set the point XY and Z-coordinates
- void set_XYZ(const double X, const double Y, const double Z) ;
-
- // get the point X-coordinates
- double get_X() const ;
- // get the point Y-coordinates
- double get_Y() const ;
- // get the point depth (camera frame)
- double get_Z() const ;
-
-
- /*
- vpBasicFeature method instantiation
- */
-
- /*!
-
- Function used to select the \f$ X\f$ subset coordinate of the 3D point
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ X\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePoint3D p;
- vpServo task;
- ...
- // Add the (X,Y) subset coordinates features from a 3D point to the task
- task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
- \endcode
-
- \sa selectY(), selectZ()
-
- */
- inline static unsigned int selectX() { return FEATURE_LINE[0] ; }
-
- /*!
-
- Function used to select the \f$ Y\f$ subset coordinate of the 3D point
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ Y\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePoint3D p;
- vpServo task;
- ...
- // Add the (X,Y) subset coordinates features from a 3D point to the task
- task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
- \endcode
-
- \sa selectX(), selectZ()
-
- */
- inline static unsigned int selectY() { return FEATURE_LINE[1] ; }
-
- /*!
-
- Function used to select the \f$ Z\f$ subset coordinate of the 3D point
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ Z\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePoint3D p;
- vpServo task;
- ...
- // Add the (Z) subset coordinate feature from a 3D point to the task
- task.addFeature(p, vpFeaturePoint3D::selectZ());
- \endcode
-
- \sa selectX(), selectY()
-
- */
- inline static unsigned int selectZ() { return FEATURE_LINE[2] ; }
- // compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- // compute the error between two visual features from a subset
- // a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- // print the name of the feature
- void print(const unsigned int select=FEATURE_ALL ) const ;
-
- // feature duplication
- vpFeaturePoint3D *duplicate() const ;
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeaturePointPolar.cpp b/src/visual-feature/vpFeaturePointPolar.cpp
deleted file mode 100644
index d6775f9..0000000
--- a/src/visual-feature/vpFeaturePointPolar.cpp
+++ /dev/null
@@ -1,646 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePointPolar.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point visual feature.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file vpFeaturePointPolar.cpp
- \brief Class that defines a 2D point visual feature with polar coordinates..
-*/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeaturePointPolar.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-
-
-/*
-
- attributes and members directly related to the vpBasicFeature needs
- other functionalities ar useful but not mandatory
-
-*/
-
-/*!
-
- Initialise the memory space requested for a 2D point visual
- feature with polar coordinates.
-
- By default this feature is initialized to \f$(\rho, \theta) = (0,
- 0)\f$. The 3D depth of the point requested in the interaction
- matrix (see interaction()) is initialized to \f$Z=1\f$.
-*/
-void
-vpFeaturePointPolar::init()
-{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value Z (1 meters)
- Z = 1;
-
-}
-
-/*!
-
- Default constructor that build a 2D point visual feature with
- polar coordinates and initialize it to \f$(\rho, \theta) = (0,
- 0)\f$.
-
- The 3D depth of the point requested in the interaction
- matrix (see interaction()) is initialized to \f$Z=1\f$.
-
-*/
-vpFeaturePointPolar::vpFeaturePointPolar() : Z(1.)
-{
- init() ;
-}
-
-
-/*!
- Set the image point \f$\rho\f$ polar coordinate.
-
- \sa set_theta()
-*/
-void
-vpFeaturePointPolar::set_rho(const double rho)
-{
- s[0] = rho ;
- flags[0] = true;
-}
-/*!
- Set the image point \f$\theta\f$ polar coordinate.
-
- \sa set_rho()
-*/
-void
-vpFeaturePointPolar::set_theta(const double theta)
-{
- s[1] = theta ;
- flags[1] = true;
-}
-
-/*!
- Set the 3D point depth in the camera frame.
-
-*/
-void
-vpFeaturePointPolar::set_Z(const double Z_)
-{
- this->Z = Z_ ;
- flags[2] = true;
-}
-
-/*!
- Initialize the image point visual feature with polar coordinates.
-
- \param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
- the image point.
-
- \param Z_ : 3D depth of the point in the camera frame.
-
- \sa set_rho(), set_theta(), set_Z()
-*/
-void
-vpFeaturePointPolar::set_rhoThetaZ(const double rho,
- const double theta,
- const double Z_)
-{
- set_rho(rho) ;
- set_theta(theta) ;
- set_Z(Z_) ;
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-/*!
- Get the image point \f$\rho\f$ polar coordinate.
-
- \sa get_theta()
-*/
-double
-vpFeaturePointPolar::get_rho() const
-{
- return s[0] ;
-}
-
-/*!
- Get the image point \f$\theta\f$ polar coordinate.
-
- \sa get_rho()
-*/
-double
-vpFeaturePointPolar::get_theta() const
-{
- return s[1] ;
-}
-/*!
- Get the 3D point depth in the camera frame.
-
-*/
-double
-vpFeaturePointPolar::get_Z() const
-{
- return this->Z ;
-}
-
-
-
-/*!
- Compute and return the interaction matrix \f$ L \f$ associated to a
- subset of the possible 2D image point features with polar
- coordinates \f$(\rho,\theta)\f$.
-
- \f[
- L = \left[
- \begin{array}{l}
- L_{\rho} \\
- \; \\
- L_{\theta}\\
- \end{array}
- \right]
- =
- \left[
- \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]
-
- where \f$Z\f$ is the 3D depth of the considered point.
-
- \param select : Selection of a subset of the possible polar
- point coordinate features.
- - To compute the interaction matrix for all the two
- subset features \f$(\rho,\theta)\f$ use vpBasicFeature::FEATURE_ALL. In
- that case the dimension of the interaction matrix is \f$ [2 \times
- 6] \f$
- - To compute the interaction matrix for only one of the subset
- (\f$\rho,\theta\f$) use one of the corresponding function
- selectRho() or selectTheta(). In that case the returned
- interaction matrix is \f$ [1 \times 6] \f$ dimension.
-
- \return The interaction matrix computed from the 2D point
- polar coordinate features.
-
- \exception vpFeatureException::badInitializationError : If the point
- is behind the camera \f$(Z < 0)\f$, or if the 3D depth is null \f$(Z
- = 0)\f$, or if the \f$\rho\f$ polar coordinate of the point is null.
-
- The code below shows how to compute the interaction matrix associated to
- the visual feature \f$s = (\rho,\theta)\f$.
- \code
- vpFeaturePointPolar s;
- double rho = 0.3;
- double theta = M_PI;
- double Z = 1;
- // Creation of the current feature s
- s.buildFrom(rho, theta, Z);
- // Build the interaction matrix L_s
- vpMatrix L = s.interaction();
- \endcode
-
- The interaction matrix could also be build by:
- \code
- vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-
- In both cases, L is a 2 by 6 matrix. The first line corresponds to
- the \f$\rho\f$ visual feature while the second one to the
- \f$\theta\f$ visual feature.
-
- It is also possible to build the interaction matrix associated to
- one of the possible features. The code below shows how to consider
- only the \f$\theta\f$ component.
-
- \code
- vpMatrix L_theta = s.interaction( vpFeaturePointPolar::selectTheta() );
- \endcode
-
- In that case, L_theta is a 1 by 6 matrix.
-*/
-vpMatrix
-vpFeaturePointPolar::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- double rho = get_rho() ;
- double theta = get_theta() ;
- double Z_ = get_Z() ;
-
- double c_ = cos(theta);
- double s_ = sin(theta);
-
- double rho2 = rho*rho;
-
- if (fabs(rho) < 1e-6) {
- vpERROR_TRACE("rho polar coordinate of the point is null") ;
- std::cout <<"rho = " << rho << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "rho polar coordinate of the point is null")) ;
- }
-
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z_) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- if (vpFeaturePointPolar::selectRho() & select )
- {
- vpMatrix Lrho(1,6) ; Lrho = 0;
-
- Lrho[0][0] = -c_/Z_ ;
- Lrho[0][1] = -s_/Z_ ;
- Lrho[0][2] = rho/Z_ ;
- Lrho[0][3] = (1+rho2)*s_ ;
- Lrho[0][4] = -(1+rho2)*c_ ;
- Lrho[0][5] = 0 ;
-
-// printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
-// std::cout << "Lrho: " << Lrho << std::endl;
-
- L = vpMatrix::stackMatrices(L,Lrho) ;
- }
-
- if (vpFeaturePointPolar::selectTheta() & select )
- {
- vpMatrix Ltheta(1,6) ; Ltheta = 0;
-
- Ltheta[0][0] = s_/(rho*Z_) ;
- Ltheta[0][1] = -c_/(rho*Z_) ;
- Ltheta[0][2] = 0 ;
- Ltheta[0][3] = c_/rho ;
- Ltheta[0][4] = s_/rho ;
- Ltheta[0][5] = -1 ;
-
-// printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
-// std::cout << "Ltheta: " << Ltheta << std::endl;
- L = vpMatrix::stackMatrices(L,Ltheta) ;
- }
- return L ;
-}
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- For the angular component \f$\theta\f$, we define the error as
- \f$\theta \ominus \theta^*\f$, where \f$\ominus\f$ is modulo \f$2\pi\f$
- substraction.
-
- \param s_star : Desired 2D image point visual feature with
- polar coordinates.
-
- \param select : The error can be computed for a selection of a
- subset of the possible 2D point polar coordinate features.
- - To compute the error for all the three coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
- dimension column vector.
- - To compute the error for only one of the polar coordinate
- feature \f$(\rho,\theta)\f$ use one of the
- corresponding function selectRho() or selectTheta(). In
- that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- The code below shows how to use this method to manipulate the
- \f$\rho\f$ component.
-
- \code
- vpFeaturePointPolar s;
- // Creation of the current feature s
- s.buildFrom(0.2, ..., 1); // rho and Z need to be set
- // Build the interaction matrix L associated to rho component
- vpMatrix L_rho = s.interaction( vpFeaturePointPolar::selectRho() );
-
- vpFeaturePointPolar s_star;
- // Creation of the desired feature s*
- s_star.buildFrom(0.45, ..., 1.2); // rho and Z need to be set
-
- // Compute the error vector (s-s*) for the rho feature
- vpColVector e = s.error(s_star, vpFeaturePointPolar::selectRho());
- \endcode
-*/
-vpColVector
-vpFeaturePointPolar::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeaturePointPolar::selectRho() & select )
- {
- vpColVector erho(1) ;
- erho[0] = s[0] - s_star[0] ;
-
- e = vpMatrix::stackMatrices(e,erho) ;
- }
-
- if (vpFeaturePointPolar::selectTheta() & select )
- {
-
- // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
- double err = s[1] - s_star[1] ;
-
- // printf("Error: %f ", err );
- while (err < -M_PI) err += 2*M_PI ;
- while (err > M_PI) err -= 2*M_PI ;
- // printf(" modif %f \n", err );
-
- vpColVector etheta(1) ;
- etheta[0] = err ;
- e = vpMatrix::stackMatrices(e,etheta) ;
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matrix related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
-
- return e ;
-
-}
-
-
-/*!
- Print to stdout the values of the current visual feature.
-
- \param select : Selection of a subset of the possible 2D image point
- feature coordinates.
- - To print all the two polar coordinates \f$(\rho,\theta)\f$ used as
- features use vpBasicFeature::FEATURE_ALL.
- - To print only one of the polar coordinate
- feature \f$(\rho,\theta)\f$ use one of the
- corresponding function selectRho() or selectTheta().
-
- \code
- // Creation of the current feature s
- vpFeaturePointPolar s;
- s.buildFrom(0.1, M_PI_2, 1.3);
-
- s.print(); // print all the 2 components of the image point feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeaturePointPolar::selectRho()); // print only the rho component
- \endcode
-*/
-void
-vpFeaturePointPolar::print(const unsigned int select ) const
-{
-
- std::cout <<"Point: Z=" << get_Z() ;
- if (vpFeaturePointPolar::selectRho() & select )
- std::cout << " rho=" << get_rho() ;
- if (vpFeaturePointPolar::selectTheta() & select )
- std::cout << " theta=" << get_theta() ;
- std::cout <<std::endl ;
-}
-
-
-/*!
-
- Build a 2D image point visual feature with polar coordinates.
-
- \param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
- the image point.
-
- \param Z_ : 3D depth of the point in the camera frame.
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is negative. That means that the 3D point is
- behind the camera which is not possible.
-
- \exception vpFeatureException::badInitializationError: If the depth
- (\f$Z\f$ coordinate) is null. That means that the 3D point is
- on the camera which is not possible.
-*/
-void
-vpFeaturePointPolar::buildFrom(const double rho, const double theta,
- const double Z_)
-{
-
- s[0] = rho ;
- s[1] = theta ;
-
- this->Z = Z_ ;
-
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
-
- if (fabs(Z) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
- }
-
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
-
- Display image point feature.
-
- \param cam : Camera parameters.
- \param I : Image.
- \param color : Color to use for the display
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeaturePointPolar::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try {
- double rho,theta;
- rho = get_rho();
- theta = get_theta();
-
- double x,y;
- x = rho*cos(theta);
- y = rho*sin(theta);
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Display image point feature.
-
- \param cam : Camera parameters.
- \param I : color Image.
- \param color : Color to use for the display
- \param thickness : Thickness of the feature representation.
-
- */
-void
-vpFeaturePointPolar::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try {
- double rho,theta;
- rho = get_rho();
- theta = get_theta();
-
- double x,y;
- x = rho*cos(theta);
- y = rho*sin(theta);
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
-
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-/*!
-
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeaturePointPolar s;
- s_star = s.duplicate(); // s_star is now a vpFeaturePointPolar
- \endcode
-
-*/
-vpFeaturePointPolar *vpFeaturePointPolar::duplicate() const
-{
- vpFeaturePointPolar *feature = new vpFeaturePointPolar;
- return feature ;
-}
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeaturePointPolar.h b/src/visual-feature/vpFeaturePointPolar.h
deleted file mode 100644
index dc3e811..0000000
--- a/src/visual-feature/vpFeaturePointPolar.h
+++ /dev/null
@@ -1,394 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeaturePointPolar.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D point with polar coordinates visual feature.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeaturePointPolar_H
-#define vpFeaturePointPolar_H
-
-/*!
- \file vpFeaturePointPolar.h
- \brief Class that defines a 2D point visual feature with polar coordinates.
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpPoint.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeaturePointPolar
- \ingroup VsFeature2
-
- \brief Class that defines 2D image point visual feature with
- polar coordinates \f$(\rho,\theta)\f$ described in \cite Corke09a.
-
- Let us denote \f$(\rho,\theta)\f$ the polar coordinates of an image
- point, with \f$\rho\f$ the radius of the feature point with respect
- to the optical center and \f$\theta\f$ the angle. From cartesian
- coordinates \f$(x,y)\f$ of a image point, polar coordinates are
- obtained by:
-
- \f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
-
- From polar coordinates, cartesian coordinates of the feature point
- can be obtained by:
-
- \f[x = \rho \cos\theta \hbox{,}\; \; y = \rho \sin\theta\f]
-
- This class is intended to manipulate the 2D image point visual
- feature in polar coordinates \f$ s = (\rho, \theta) \f$. The
- interaction matrix related to \f$ s \f$ is given by:
-
- \f[
- L = \left[
- \begin{array}{l}
- L_{\rho} \\
- \; \\
- L_{\theta}\\
- \end{array}
- \right]
- =
- \left[
- \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]
-
- where \f$Z\f$ is the 3D depth of the considered point in the camera frame.
-
- Two ways are allowed to initialize the feature.
-
- - The first way by setting the feature values \f$(\rho,\theta,Z)\f$
- using vpFeaturePointPolar members like set_rho(), set_theta(),
- set_Z(), or set_rhoThetaZ(), or also buildFrom().
-
- - The second way by using the feature builder functionalities to
- initialize the feature from a dot tracker, like
- vpFeatureBuilder::create (vpFeaturePointPolar &, const
- vpCameraParameters &, const vpDot &) or vpFeatureBuilder::create
- (vpFeaturePointPolar &, const vpCameraParameters &, const vpDot2
- &). Be aware, that in that case only \f$(\rho,\theta)\f$ are
- initialized. You may also initialize the 3D depth \f$Z\f$. It is
- also possible to initialize the feature from a point structure,
- like vpFeatureBuilder::create(vpFeaturePointPolar &, const vpPoint
- &) or vpFeatureBuilder::create(vpFeaturePointPolar &, const
- vpCameraParameters &, const vpCameraParameters &, const vpPoint
- &). In that case all the feature parameters \f$(\rho,\theta,Z)\f$
- would be initialized.
-
- The interaction() method allows to compute the interaction matrix
- \f$L\f$ associated to the visual feature, while the error() method
- computes the error vector \f$(s - s^*)\f$ between the current visual
- feature and the desired one.
-
- The code below shows how to create a eye-in hand visual servoing
- task using four 2D point features with polar coordinates. First we
- create four current features \f$s\f$ (p var name in the code) and
- four desired \f$s^*\f$ (pd var name in the code) point features with
- polar coordinates, set the task to use the interaction matrix
- associated to the current feature \f$L_{s}\f$ and than compute the
- camera velocity \f$v=-\lambda \; {L_{s}}^+ \; (s-s^*)\f$. The
- current feature \f$s\f$ is updated in the while() loop, while
- \f$s^*\f$ is initialized at the beginning.
-
- \code
-#include <visp/vpPoint.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeaturePointPolar.h>
-#include <visp/vpServo.h>
-
-int main()
-{
-
- // Create 4 points to specify the object of interest
- vpPoint point[4];
-
- // Set the 3D point coordinates in the object frame: oP
- point[0].setWorldCoordinates(-0.1, -0.1, 0);
- point[1].setWorldCoordinates( 0.1, -0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
-
- // Initialize the desired pose between the camera and the object frame
- vpHomogeneousMatrix cMod;
- cMod.buildFrom(0, 0, 1, 0, 0, 0);
-
- // Compute the desired position of the point
- for (int i = 0 ; i < 4 ; i++) {
- // Compute the 3D point coordinates in the camera frame cP = cMod * oP
- point[i].changeFrame(cMod);
- // Compute the perspective projection to set (x,y)
- point[i].projection();
- }
-
- // Create 4 desired visual features as 2D points with polar coordinates
- vpFeaturePointPolar pd[4];
- // Initialize the desired visual feature from the desired point positions
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i], point[i]);
-
- // Initialize the current pose between the camera and the object frame
- vpHomogeneousMatrix cMo;
- cMo.buildFrom(0, 0, 1.2, 0, 0, M_PI);
- // ... cMo need here to be computed from a pose estimation
-
- for (int i = 0 ; i < 4 ; i++) {
- // Compute the 3D point coordinates in the camera frame cP = cMo * oP
- point[i].changeFrame(cMo);
- // Compute the perspective projection to set (x,y)
- point[i].projection();
- }
- // Create 4 current visual features as 2D points with polar coordinates
- vpFeaturePointPolar p[4];
- // Initialize the current visual feature from the current point positions
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i], point[i]);
-
- // Visual servo task initialization
- vpServo task;
- // - Camera is monted on the robot end-effector and velocities are
- // computed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // - Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
- // - Set the contant gain to 1
- task.setLambda(1);
- // - Add current and desired features
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i], pd[i]);
-
- // Control loop
- for ( ; ; ) {
- // ... cMo need here to be estimated from for example a pose estimation.
- // Computes the point coordinates in the camera frame and its 2D
- // coordinates in the image plane
- for (int i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
-
- // Update the current 2D point visual feature with polar coordinates
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i], point[i]);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
-
- task.kill();
-}
- \endcode
-
- If you want to deal only with the \f$\rho\f$ subset feature from the 2D
- point feature set, you have just to modify the addFeature() call in the
- previous example by the following line. In that case, the dimension of
- \f$s\f$ is four.
-
- \code
- // Add the rho subset feature from the 2D point polar coordinates visual features
- task.addFeature(p[i], pd[i], vpFeaturePointPolar::selectRho());
- \endcode
-
- If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature with polar coordinates, compute the corresponding error vector
- \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeaturePointPolar.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- // Creation of the current feature s
- vpFeaturePointPolar s;
- // Initialize the current feature
- s.buildFrom(0.1, M_PI, 1); // rho=0.1m, theta=pi, Z=1m
-
- // Creation of the desired feature s
- vpFeaturePointPolar s_star;
- // Initialize the desired feature
- s.buildFrom(0.15, 0, 0.8); // rho=0.15m, theta=0, Z=0.8m
-
- // Compute the interaction matrix L_s for the current feature
- vpMatrix L = s.interaction();
-
- // Compute the error vector (s-s*) for the point feature with polar coordinates
- s.error(s_star);
-
- return 0;
-}
- \endcode
-
-*/
-class VISP_EXPORT vpFeaturePointPolar : public vpBasicFeature
-{
-private:
- //! FeaturePoint depth (required to compute the interaction matrix)
- //! default Z = 1m
- double Z ;
-
-public:
- // basic construction
- void init() ;
- // basic constructor
- vpFeaturePointPolar() ;
- //! Destructor. Does nothing.
- virtual ~vpFeaturePointPolar() { }
-
- void buildFrom(const double rho, const double theta, const double Z) ;
-
- /*
- Set coordinates
- */
-
- // set the point rho-coordinates
- void set_rho(const double rho) ;
- // set the point theta-coordinates
- void set_theta(const double theta) ;
- // set the point depth (camera frame)
- void set_Z(const double Z) ;
- // set the point rho, theta polar coordinates and Z coordinate
- void set_rhoThetaZ(const double rho, const double theta, const double Z) ;
-
- // get the point rho-coordinates
- double get_rho() const ;
- // get the point theta-coordinates
- double get_theta() const ;
- // get the point depth (camera frame)
- double get_Z() const ;
-
- /*
- vpBasicFeature method instantiation
- */
-
- /*!
-
- Function used to select the \f$\rho\f$ subset polar
- coordinate of the image point visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$\rho\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePointPolar p;
- vpServo task;
- ...
- // Add only the rho subset coordinate feature from an image point to the task
- task.addFeature(p, vpFeaturePointPolar::selectRho());
- \endcode
-
- \sa selectTheta()
- */
- inline static unsigned int selectRho() { return FEATURE_LINE[0] ; }
-
-
- /*!
-
- Function used to select the \f$\theta\f$ subset polar
- coordinate of the image point visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$\theta\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeaturePointPolar p;
- vpServo task;
- ...
- // Add only the theta subset coordinate feature from an image point to the task
- task.addFeature(p, vpFeaturePointPolar::selectTheta());
- \endcode
-
- \sa selectRho()
- */
- inline static unsigned int selectTheta() { return FEATURE_LINE[1] ; }
- // compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- // compute the error between two visual features from a subset
- // a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- // print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- // feature duplication
- vpFeaturePointPolar *duplicate() const ;
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
- /*!
- @name Deprecated functions
- */
- //! compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-
-} ;
-
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureSegment.cpp b/src/visual-feature/vpFeatureSegment.cpp
deleted file mode 100644
index 7ba11dd..0000000
--- a/src/visual-feature/vpFeatureSegment.cpp
+++ /dev/null
@@ -1,608 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureThetaU.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Segment visual feature.
- *
- * Authors:
- * Filip Novotny
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureSegment.h>
-#include <visp/vpImagePoint.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpMath.h>
-#include <visp/vpDisplay.h>
-#include <cmath>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpFeatureSegment.cpp
- \brief class that defines the vpFeatureSegment visual feature
-*/
-
-/*!
-
- Initialise the memory space requested for segment visual
- feature.
-
-*/
-void
-vpFeatureSegment::init()
-{
- //feature dimension
- dim_s = 4 ;
- nbParameters = 6;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-}
-
-/*!
- Default constructor that builds an empty segment visual feature.
-
- \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
- If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
-*/
-vpFeatureSegment::vpFeatureSegment(bool normalized)
- : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
-{
- init();
-}
-
-/*!
- Compute and return the interaction matrix \f$ L \f$ associated to a
- subset of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
- or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
-
- The interaction matrix of the non normalized feature set is of the following form:
- \f[
- {\bf L} = \left[
- \begin{array}{c}
- L_{x_c} \\
- L_{y_c} \\
- L_{l} \\
- L_{\alpha}
- \end{array}
- \right] =
- \left[
- \begin{array}{cccccc}
- -\lambda_2 & 0 & \lambda_2 x_c - \lambda_1 l \frac{\cos \alpha}{4} &
- x_c y_c + l^2 \frac{\cos \alpha \sin \alpha}{4} &
- -(1 + {x_{c}}^{2} + l^2 \frac{\cos^2\alpha}{4}) &
- y_c \\
- 0 & -\lambda_2 & \lambda_2 y_c - \lambda_1 l \frac{\sin \alpha}{4} &
- 1 + {y_{c}}^{2} + l^2 \frac{\sin^2 \alpha}{4} &
- -x_c y_c-l^2 \frac{\cos \alpha \sin \alpha}{4} &
- -x_c \\
- \lambda_1 \cos \alpha & \lambda_1 \sin \alpha &
- \lambda_2 l - \lambda_1 (x_c \cos \alpha + y_c \sin \alpha) &
- l (x_c \cos \alpha \sin \alpha + y_c (1 + \sin^2 \alpha)) &
- -l (x_c (1 + \cos^2 \alpha)+y_c \cos \alpha \sin \alpha) &
- 0 \\
- -\lambda_1 \frac{\sin \alpha}{l} & \lambda_1 \frac{\cos \alpha}{l} &
- \lambda_1 \frac{x_c \sin \alpha - y_c \cos \alpha}{l} &
- -x_c \sin^2 \alpha + y_c \cos \alpha \sin \alpha &
- x_c \cos \alpha \sin \alpha - y_c \cos^2 \alpha &
- -1
- \end{array}
- \right]
- \f]
-
- with \f$ \lambda_1 = \frac{Z_1 - Z_2}{Z_1 Z_2}\f$ and \f$ \lambda_2 = \frac{Z_1 + Z_2}{2 Z_1 Z_2}\f$
- where \f$Z_i\f$ are the depths of the points.
-
-
- \param select : Selection of a subset of the possible segment features.
- - To compute the interaction matrix for all the four
- subset features \f$(x_c \f$, \f$ y_c \f$, \f$ l \f$, \f$ \alpha)\f$ or
- \f$(x_n \f$, \f$ y_n \f$, \f$ l_n \f$, \f$ \alpha)\f$ use vpBasicFeature::FEATURE_ALL. In
- that case the dimension of the interaction matrix is \f$ [4 \times 6] \f$.
- - To compute the interaction matrix for only one of the subset
- use one of the following functions:
- selectXc(), selectYc(), selectL(), selectAlpha(). In that case, the returned
- interaction matrix is of dimension \f$ [1 \times 6] \f$ .
-
- \return The interaction matrix computed from the segment features.
-
- The code below shows how to compute the interaction matrix associated to
- the visual feature \f${\bf s} = (x_c, y_c, l, \alpha)\f$.
- \code
-#include <visp/vpFeatureSegment.h>
-#include <visp/vpPoint.h>
-
-int main()
-{
- // Define two 3D points in the object frame
- vpPoint p1, p2;
- p1.setWorldCoordinates(.1, .1, 0.);
- p2.setWorldCoordinates(.3, .2, 0.);
-
- // Define the camera pose wrt the object
- vpHomogeneousMatrix cMo (0, 0, 1, 0, 0, 0); // Z=1 meter
- // Compute the coordinates of the points in the camera frame
- p1.changeFrame(cMo);
- p2.changeFrame(cMo);
- // Compute the coordinates of the points in the image plane by perspective projection
- p1.project();
- p2.project();
-
- // Build the segment visual feature
- vpFeatureSegment s;
- s.buildFrom(p1.get_x(), p1.get_y(), p1.get_Z(), p2.get_x(), p2.get_y(), p2.get_Z());
-
- // Compute the interaction matrix
- vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
-}
- \endcode
-
- In this case, L is a 4 by 6 matrix.
-
- It is also possible to build the interaction matrix associated to
- one of the possible features. The code below shows how to modify the previous code to consider as
- visual feature \f$s = (l, \alpha)\f$.
-
- \code
- vpMatrix L = s.interaction( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() );
- \endcode
-
- In that case, L is a 2 by 6 matrix.
-*/
-vpMatrix
-vpFeatureSegment::interaction( const unsigned int select )
-{
-
- vpMatrix L ;
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but xc was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Yc was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but l was not set yet");
- break;
- case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
- break;
- case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but Z1 was not set yet");
- break;
- case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but Z2 was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- }
-
- //This version is a simplification
- double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
- double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
-
- if (normalized_)
- {
- // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
- double xn = xc_;
- double yn = yc_;
- double ln = l_;
- double lambda = -lambda1 * ln;
- double Zn_inv = lambda2 * ln;
- double lc = cos_a_ / ln;
- double ls = sin_a_ / ln;
- double xnalpha = xn*cos_a_+yn*sin_a_;
- double lnc = cos_a_ * ln;
- double lns = sin_a_ * ln;
-
- if (vpFeatureSegment::selectXc() & select ){
- vpMatrix Lxn(1,6);
- Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
- Lxn[0][1] = lambda * xn * sin_a_ ;
- Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
- Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
- Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
- Lxn[0][5] = yn ;
- L = vpMatrix::stackMatrices(L, Lxn) ;
- }
-
- if (vpFeatureSegment::selectYc() & select ){
- vpMatrix Lyn(1,6);
- Lyn[0][0] = lambda*yn*cos_a_ ;
- Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
- Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
- Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
- Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
- Lyn[0][5] = -xn ;
- L = vpMatrix::stackMatrices(L, Lyn) ;
- }
-
- if (vpFeatureSegment::selectL() & select ){
- vpMatrix Lln(1,6);
- Lln[0][0] = lambda * lnc ;
- Lln[0][1] = lambda * lns ;
- Lln[0][2] = -(Zn_inv + lambda*xnalpha);
- Lln[0][3] = -yn-xnalpha*sin_a_ ;
- Lln[0][4] = xn + xnalpha*cos_a_ ;
- Lln[0][5] = 0 ;
- L = vpMatrix::stackMatrices(L, Lln) ;
- }
- if (vpFeatureSegment::selectAlpha() & select ){
- // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
- vpMatrix Lalpha(1,6);
- Lalpha[0][0] = -lambda1*sin_a_*l_ ;
- Lalpha[0][1] = lambda1*cos_a_*l_ ;
- Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_);
- Lalpha[0][3] = (-xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_)/l_;
- Lalpha[0][4] = (xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_)/l_ ;
- Lalpha[0][5] = -1 ;
- L = vpMatrix::stackMatrices(L,Lalpha) ;
- }
- }
- else
- {
- if (vpFeatureSegment::selectXc() & select ){
- vpMatrix Lxc(1,6);
- Lxc[0][0] = -lambda2 ;
- Lxc[0][1] = 0. ;
- Lxc[0][2] = lambda2*xc_ - lambda1*l_*cos_a_/4.;
- Lxc[0][3] = xc_*yc_ + l_*l_*cos_a_*sin_a_/4. ;
- Lxc[0][4] = -(1+xc_*xc_+l_*l_*cos_a_*cos_a_/4.) ;
- Lxc[0][5] = yc_ ;
- L = vpMatrix::stackMatrices(L,Lxc) ;
- }
-
- if (vpFeatureSegment::selectYc() & select ){
- vpMatrix Lyc(1,6);
- Lyc[0][0] = 0. ;
- Lyc[0][1] = -lambda2 ;
- Lyc[0][2] = lambda2*yc_ - lambda1*l_*sin_a_/4.;
- Lyc[0][3] = 1+yc_*yc_+l_*l_*sin_a_*sin_a_/4. ;
- Lyc[0][4] = -xc_*yc_-l_*l_*cos_a_*sin_a_/4. ;
- Lyc[0][5] = -xc_ ;
- L = vpMatrix::stackMatrices(L,Lyc) ;
- }
-
- if (vpFeatureSegment::selectL() & select ){
- vpMatrix Ll(1,6);
- Ll[0][0] = lambda1*cos_a_ ;
- Ll[0][1] = lambda1*sin_a_ ;
- Ll[0][2] = lambda2*l_-lambda1*(xc_*cos_a_+yc_*sin_a_);
- Ll[0][3] = l_*(xc_*cos_a_*sin_a_ + yc_*(1+sin_a_*sin_a_)) ;
- Ll[0][4] = -l_*(xc_*(1+cos_a_*cos_a_)+yc_*cos_a_*sin_a_) ;
- Ll[0][5] = 0 ;
- L = vpMatrix::stackMatrices(L,Ll) ;
- }
- if (vpFeatureSegment::selectAlpha() & select ){
- vpMatrix Lalpha(1,6);
- Lalpha[0][0] = -lambda1*sin_a_/l_ ;
- Lalpha[0][1] = lambda1*cos_a_/l_ ;
- Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_)/l_;
- Lalpha[0][3] = -xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_;
- Lalpha[0][4] = xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_ ;
- Lalpha[0][5] = -1 ;
- L = vpMatrix::stackMatrices(L,Lalpha) ;
- }
- }
-
- return L ;
-}
-
-/*!
- Computes the error between the current and the desired visual features from a subset
- of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
- or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
-
- For the angular component \f$\alpha\f$, we define the error as
- \f$\alpha \ominus \alpha^*\f$, where \f$\ominus\f$ is modulo \f$2\pi\f$
- substraction.
-
- \param s_star : Desired 2D segment feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible segment features.
- - To compute the error for all the four parameters use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 4
- dimension column vector.
- - To compute the error for only one subfeature of
- \f${\bf s} = (x_c, y_c, l, \alpha)\f$ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ feature set
- use one of the following functions: selectXc(), selectYc(), selectL(), selectAlpha().
-
- \return The error between the current and the desired
- visual feature.
-
-*/
-vpColVector
-vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select )
-{
- vpColVector e(0) ;
-
- if (vpFeatureSegment::selectXc() & select ){
- vpColVector exc(1) ;
- exc[0] = xc_-s_star[0];
- e = vpMatrix::stackMatrices(e,exc) ;
- }
-
- if (vpFeatureSegment::selectYc() & select ){
- vpColVector eyc(1) ;
- eyc[0] = yc_ - s_star[1];
- e = vpMatrix::stackMatrices(e,eyc) ;
- }
-
- if (vpFeatureSegment::selectL() & select ){
- vpColVector eL(1) ;
- eL[0] = l_ - s_star[2];
- e = vpMatrix::stackMatrices(e,eL) ;
- }
-
- if (vpFeatureSegment::selectAlpha() & select ){
- vpColVector eAlpha(1) ;
- eAlpha[0] = alpha_ - s_star[3];
- while (eAlpha[0] < -M_PI) eAlpha[0] += 2*M_PI ;
- while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
- e = vpMatrix::stackMatrices(e,eAlpha) ;
- }
- return e ;
-}
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible segement features (\f$ x_c \f$,\f$ y_c \f$,\f$ l \f$,\f$ \alpha \f$).
-
- \code
- s.print();
- \endcode
-
- produces the following output:
-
- \code
- vpFeatureSegment: (xc = -0.255634; yc = -0.13311; l = 0.105005; alpha = 92.1305 deg)
- \endcode
-
- while
- \code
- s.print( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() );
- \endcode
-
- produces the following output:
-
- \code
- vpFeatureSegment: (l = 0.105005; alpha = 92.1305 deg)
- \endcode
-*/
-void
-vpFeatureSegment::print( const unsigned int select ) const
-{
- std::cout <<"vpFeatureSegment: (";
- if (vpFeatureSegment::selectXc() & select ) {
- if (normalized_)
- std::cout << "xn = ";
- else
- std::cout << "xc = ";
- std::cout << s[0] << "; ";
- }
- if (vpFeatureSegment::selectYc() & select ) {
- if (normalized_)
- std::cout << "yn = ";
- else
- std::cout << "yc = ";
- std::cout << s[1] << "; ";
- }
- if (vpFeatureSegment::selectL() & select ) {
- if (normalized_)
- std::cout << "ln = ";
- else
- std::cout << "l = ";
- std::cout << s[2] << "; ";
- }
- if (vpFeatureSegment::selectAlpha() & select ) {
- std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
- }
- std::cout << ")" << std::endl;
-}
-
-/*!
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureSegment s;
- s_star = s.duplicate(); // s_star is now a vpFeatureSegment
- \endcode
-
-*/
-vpFeatureSegment *vpFeatureSegment::duplicate() const
-{
- vpFeatureSegment *feature;
-
- feature = new vpFeatureSegment(*this) ;
- return feature ;
-}
-
-/*!
-
- Displays a segment representing the feature on a greyscale image.
- The two limiting points are displayed in cyan and yellow.
-
- \param cam : Camera parameters.
- \param I : Image.
- \param color : Color to use for the segment.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeatureSegment::display(const vpCameraParameters &cam ,
- const vpImage<unsigned char> & I,
- const vpColor &color,
- unsigned int thickness ) const
-{
- double l, x, y;
- if (normalized_) {
- l = 1./l_;
- x = xc_ * l;
- y = yc_ * l;
- }
- else {
- l = l_;
- x = xc_;
- y = yc_;
- }
-
- double x1 = x - (l/2.)*cos_a_;
- double x2 = x + (l/2.)*cos_a_;
-
- double y1 = y - (l/2.)*sin_a_;
- double y2 = y + (l/2.)*sin_a_;
- vpImagePoint ip1,ip2;
-
- vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
- vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- vpDisplay::displayCircle(I, ip1, 5, color, true);
- vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
-}
-
-/*!
-
- Displays a segment representing the feature on a RGBa image.
- The two limiting points are displayed in cyan and yellow.
-
- \param cam : Camera parameters.
- \param I : Image.
- \param color : Color to use for the segment.
- \param thickness : Thickness of the feature representation.
-*/
-void
-vpFeatureSegment::display(const vpCameraParameters & cam ,
- const vpImage<vpRGBa> & I,
- const vpColor &color,
- unsigned int thickness ) const
-{
- double l, x, y;
- if (normalized_) {
- l = 1./l_;
- x = xc_ * l;
- y = yc_ * l;
- }
- else {
- l = l_;
- x = xc_;
- y = yc_;
- }
-
- double x1 = x - (l/2.)*cos_a_;
- double x2 = x + (l/2.)*cos_a_;
-
- double y1 = y - (l/2.)*sin_a_;
- double y2 = y + (l/2.)*sin_a_;
- vpImagePoint ip1,ip2;
-
- vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
- vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
- vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
-}
-
-/*!
-
- Build a segment visual feature from two points and their Z coordinates.
-
- \param x1, y1 : coordinates of the first point in the image plane.
- \param Z1 : depth of the first point in the camera frame.
-
- \param x2, y2 : coordinates of the second point in the image plane.
- \param Z2 : depth of the second point in the camera frame.
-
- Depending on the feature set that is considered, the features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
- or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ are computed from the two points using the
- following formulae:
- \f[ x_c = \frac{x_1 + x_2}{2} \f]
- \f[ y_c = \frac{y_1 + y_2}{2} \f]
- \f[ l = \sqrt{{x_1 - x_2}^2 + {y_1 - y_2}^2} \f]
- \f[ \alpha = arctan(\frac{y_1 - y_2}{x_1 - x_2}) \f]
-*/
-void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
- const double x2, const double y2, const double Z2)
-{
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
- double x_c = (x1+x2)/2.;
- double y_c = (y1+y2)/2.;
- double alpha = atan2(y1-y2,x1-x2);
-
- if (normalized_) {
- setXc( x_c / l );
- setYc( y_c / l );
- setL( 1/l );
- setAlpha( alpha );
-
- setZ1(Z1);
- setZ2(Z2);
- }
- else {
- setXc( x_c );
- setYc( y_c );
- setL( l );
- setAlpha( alpha );
-
- setZ1(Z1);
- setZ2(Z2);
- }
-}
-
-
diff --git a/src/visual-feature/vpFeatureSegment.h b/src/visual-feature/vpFeatureSegment.h
deleted file mode 100644
index 5463d2a..0000000
--- a/src/visual-feature/vpFeatureSegment.h
+++ /dev/null
@@ -1,400 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureThetaU.h 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Segment visual feature.
- *
- * Authors:
- * Filip Novotny
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureSegment_H
-#define vpFeatureSegment_H
-
-/*!
- \file vpFeatureSegment.h
- \brief class that defines the Segment visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpPoint.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpFeatureException.h>
-
-/*!
- \class vpFeatureSegment
- \ingroup VsFeature2
-
- \brief Class that defines a 2D segment visual features.
- This class allow to consider two sets of visual features:
- - the non normalised features \f${\bf s} = (x_c, y_c, l, \alpha)\f$ where \f$(x_c,y_c)\f$
- are the coordinates of the segment center, \f$ l \f$ the segment length
- and \f$ \alpha \f$ the orientation of the segment with respect to the \f$ x \f$ axis.
- - or the normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ with \f$x_n = x_c/l\f$,
- \f$y_n = y_c/l\f$ and \f$l_n = 1/l\f$.
-
-
-
- The selection of the feature set is done either during construction using vpFeatureSegment(bool),
- or by setNormalized(bool).
-
-*/
-class VISP_EXPORT vpFeatureSegment : public vpBasicFeature
-{
-public:
- //empty constructor
- vpFeatureSegment(bool normalized=false);
-
- //! Destructor. Does nothing.
- ~vpFeatureSegment() {}
- // change values of the segment
- void buildFrom(const double x1, const double y1, const double Z1, const double x2, const double y2, const double Z2);
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- //! Feature duplication.
- vpFeatureSegment *duplicate() const ;
- // compute the error between two visual features from a subset
- // a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
-
- /*!
- Get the x coordinate of the segment center in the image plane.
-
- \return If normalized features are used, return \f$ x_n = x_c / l \f$. Otherwise return \f$ x_c \f$.
- */
- inline double getXc() const { return s[0] ; }
-
- /*!
- Get the y coordinate of the segment center in the image plane.
-
- \return If normalized features are used, return \f$ y_n = y_c / l \f$. Otherwise return \f$ y_c \f$.
- */
- inline double getYc() const { return s[1] ; }
-
- /*!
- Get the length of the segment.
-
- \return If normalized features are used, return \f$ l_n = 1 / l \f$. Otherwise return \f$ l \f$.
-
- */
- inline double getL() const { return s[2] ; }
-
- /*!
- Get the value of \f$ \alpha \f$ which represents the orientation of the segment.
-
- \return The value of \f$ \alpha \f$.
- */
- inline double getAlpha() const { return s[3] ;}
-
- /*!
- Get the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment first point.
-
- \return The value of the depth \f$ Z_1 \f$.
- */
- inline double getZ1() const { return Z1_ ;}
-
- /*!
- Get the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment second point.
-
- \return The value of the depth \f$ Z_2 \f$.
- */
- inline double getZ2() const { return Z2_ ;}
-
- // Basic construction.
- void init() ;
-
- // compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
-
- void print(const unsigned int select= FEATURE_ALL) const ;
-
- /*!
- Indicates if the normalized features are considered.
- */
- bool isNormalized() { return normalized_; };
-
- /*!
-
- Function used to select the \f$x_c\f$ or \f$x_n\f$ subfeature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$x_c\f$ or \f$x_n\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureSegment s, s_star; // Current and desired visual feature
- vpServo task;
- ...
- // Add only the xc subset feature from a segment to the task
- task.addFeature(s, s_star, vpFeatureSegment::selectXc());
- \endcode
-
- \sa selectYc(), selectL(), selectAlpha()
- */
- inline static unsigned int selectXc() { return FEATURE_LINE[0] ; }
-
- /*!
-
- Function used to select the \f$y_c\f$ or \f$y_n\f$ subfeature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$y_c\f$ or \f$y_n\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureSegment s, s_star; // Current and desired visual feature
- vpServo task;
- ...
- // Add only the yc subset feature from a segment to the task
- task.addFeature(s, s_star, vpFeatureSegment::selectYc());
- \endcode
-
- \sa selectXc(), selectL(), selectAlpha()
- */
-
- inline static unsigned int selectYc() { return FEATURE_LINE[1] ; }
-
- /*!
-
- Function used to select the \f$l\f$ or \f$l_n\f$ subfeature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$l\f$ or \f$l_n\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureSegment s, s_star; // Current and desired visual feature
- vpServo task;
- ...
- // Add only the l subset feature from a segment to the task
- task.addFeature(s, s_star, vpFeatureSegment::selectL());
- \endcode
-
- \sa selectXc(), selectYc(), selectAlpha()
- */
-
- inline static unsigned int selectL() { return FEATURE_LINE[2] ; }
-
- /*!
-
- Function used to select the \f$\alpha\f$ subfeature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$\alpha\f$ feature.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureSegment s, s_star; // Current and desired visual feature
- vpServo task;
- ...
- // Add only the alpha subset feature from a segment to the task
- task.addFeature(s, s_star, vpFeatureSegment::selectAlpha());
- \endcode
-
- \sa selectXc(), selectYc(), selectL()
- */
-
- inline static unsigned int selectAlpha() { return FEATURE_LINE[3] ; }
-
- /*!
- Set the king of feature to consider.
- \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
- If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
- */
- void setNormalized(bool normalized) { normalized_ = normalized; };
- /*!
-
- Set the value of the x coordinate of the segment center
- in the image plane. It is one parameter of the visual feature \f$ s \f$.
-
- \param val : Value to set, that is either equal to \f$ x_n = x_c/l \f$ when normalized features
- are considered, or equal to \f$ x_c \f$ otherwise.
- */
- inline void setXc(const double val){
- s[0] = xc_ = val;
- flags[0] = true;
- }
- /*!
-
- Set the value of the y coordinate of the segment center
- in the image plane. It is one parameter of the visual feature \f$ s \f$.
-
- \param val : Value to set, that is either equal to \f$ y_n = y_c/l \f$ when normalized features
- are considered, or equal to \f$ y_c \f$ otherwise.
- */
- inline void setYc(const double val){
- s[1] = yc_ = val;
- flags[1] = true;
- }
- /*!
-
- Set the value of the segment length in the image plane. It is one parameter of the visual feature \f$ s \f$.
-
- \param val : Value to set, that is either equal to \f$l_n= 1/l \f$ when normalized features
- are considered, or equal to \f$ l \f$ otherwise.
- */
- inline void setL(const double val){
- s[2] = l_ = val;
- flags[2] = true;
- }
- /*!
-
- Set the value of \f$ \alpha \f$ which represents the orientation of the segment
- in the image plane. It is one parameter of the visual feature \f$ s \f$.
-
- \param val : \f$ \alpha \f$ value to set.
- */
- inline void setAlpha(const double val){
- s[3] = alpha_ = val;
- cos_a_ = cos(val);
- sin_a_ = sin(val);
- flags[3] = true;
- }
-
- /*!
-
- Set the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment first point.
-
- This value is requested to compute the interaction matrix.
-
- \param val : \f$ Z_1 \f$ value to set.
-
- \exception vpFeatureException::badInitializationError : If Z1 is behind the camera or equal to zero.
- */
- inline void setZ1(const double val)
- {
- Z1_ = val;
-
- if (Z1_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z1 = " << Z1_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z1 is behind the camera ")) ;
- }
-
- if (fabs(Z1_) < 1e-6)
- {
- vpERROR_TRACE("Point Z1 coordinates is null ") ;
- std::cout <<"Z1 = " << Z1_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z1 coordinates is null")) ;
- }
-
- flags[4] = true;
- }
-
- /*!
-
- Set the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment second point.
-
- This value is requested to compute the interaction matrix.
-
- \param val : \f$ Z_2 \f$ value to set.
-
- \exception vpFeatureException::badInitializationError : If Z2 is behind the camera or equal to zero.
- */
- inline void setZ2(const double val)
- {
- Z2_ = val;
-
- if (Z2_ < 0)
- {
- vpERROR_TRACE("Point Z2 is behind the camera ") ;
- std::cout <<"Z2 = " << Z2_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z2 is behind the camera ")) ;
- }
-
- if (fabs(Z2_) < 1e-6)
- {
- vpERROR_TRACE("Point Z2 coordinates is null ") ;
- std::cout <<"Z2 = " << Z2_ << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z2 coordinates is null")) ;
- }
-
- flags[5] = true;
- }
-
-
-private:
- double xc_;
- double yc_;
- double l_;
- double alpha_;
- double Z1_;
- double Z2_;
- double cos_a_;
- double sin_a_;
- bool normalized_;
-} ;
-
-#endif
-
diff --git a/src/visual-feature/vpFeatureThetaU.cpp b/src/visual-feature/vpFeatureThetaU.cpp
deleted file mode 100644
index cfee61e..0000000
--- a/src/visual-feature/vpFeatureThetaU.cpp
+++ /dev/null
@@ -1,705 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureThetaU.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * ThetaU visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpMath.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpFeatureThetaU.cpp
- \brief class that defines the ThetaU visual feature
-*/
-/*
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities are useful but not mandatory
-
-*/
-
-/*!
-
- Initialise the memory space requested for 3D \f$ \theta u \f$ visual
- feature.
-*/
-void
-vpFeatureThetaU::init()
-{
- //feature dimension
- dim_s = 3 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-}
-
-/*!
- Default constructor that build a visual feature and initialize it to zero.
-
- \param r [in] : The rotation representation of the \f$ \theta u\f$
- visual feature.
-
-*/
-vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
-{
- //vpTRACE("0x%x", this);
- init() ;
-
- // kind of rotation representation
- rotation = r;
-}
-
-/*!
-
- Constructor that build a 3D visual feature from a \f$ \theta u \f$
- vector that represent the rotation \f$ R \f$ the camera has to
- achieve.
-
- \param tu [in] : Rotation that the camera has to achieve in \f$
- \theta u \f$ angle/axis representation. Depending on the rotation
- representation type
- (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
- construct this object, the parameter \e tu represents either the
- rotation that the camera has to achieve to move from the desired
- camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
- that the camera has to achieve to move from the current camera frame
- to the desired one (\f$ ^{c}R_{c^*}\f$).
-
- \param r [in] : The rotation representation of \f$ \theta u \f$.
-*/
-
-vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu,
- vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
-{
- init() ;
-
- buildFrom(tu) ;
-}
-
-/*!
-
- Constructor that build a 3D \f$ \theta u \f$ visual feature from a
- rotation matrix \f$ R \f$ that represent the rotation that
- the camera has to achieve.
-
- \param R [in] : Rotation that the camera has to achieve. Depending
- on r this rotation represent either the rotation that the camera has
- to achieve to move from the desired camera frame to the current one
- (\f$ ^{c^*}R_c\f$), or the rotation that the camera has to achieve to
- move from the current camera frame to the desired one (\f$
- ^{c}R_{c^*}\f$).
-
- \param r [in] : The rotation representation of R.
-
-*/
-
-vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
- vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
-{
- init() ;
-
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
-}
-
-/*!
-
- Constructor that build a 3D \f$ \theta u \f$ visual feature from an
- homogeneous matrix \f$ M \f$ that represent the displacement that
- the camera has to achieve.
-
- \param M [in] : Homogeneous transformation that describe the
- movement that the camera has to achieve. Only the rotational part of
- this homogeneous transformation is taken into
- consideration. Depending on r the rotation represent either the
- rotation that the camera has to achieve to move from the desired
- camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
- that the camera has to achieve to move from the current camera frame
- to the desired one (\f$ ^{c}R_{c^*}\f$).
-
- \param r [in] : The rotation representation of M.
-
-
-*/
-vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
- vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
-{
- init() ;
- vpRotationMatrix R ;
- M.extract(R) ;
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
-}
-
-/*!
-
- Build a 3D visual feature from a \f$ \theta u \f$ vector that
- represent the rotation \f$ R \f$ the camera has to achieve.
-
- \param tu [in] : Rotation that the camera has to achieve in \f$
- \theta u \f$ angle/axis representation. Depending on the rotation
- representation type
- (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
- construct this object, the parameter \e tu represents either the
- rotation that the camera has to achieve to move from the desired
- camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
- that the camera has to achieve to move from the current camera frame
- to the desired one (\f$ ^{c}R_{c^*}\f$).
-
-*/
-void
-vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
-{
- s[0] = tu[0] ;
- s[1] = tu[1] ;
- s[2] = tu[2] ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-/*!
- Build a 3D \f$ \theta u \f$ visual feature from a
- rotation matrix \f$ R \f$ that represent the rotation that
- the camera has to achieve.
-
- \param R [in] : Rotation that the camera has to achieve. Depending
- on the rotation representation type
- (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
- construct this object, the parameter \e R represents either the
- rotation that the camera has to achieve to move from the desired
- camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
- that the camera has to achieve to move from the current camera frame
- to the desired one (\f$ ^{c}R_{c^*}\f$).
-
-*/
-void
-vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
-{
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
-}
-
-/*!
- Build a 3D \f$ \theta u \f$ visual feature from an
- homogeneous matrix \f$ M \f$ that represent the displacement that
- the camera has to achieve.
-
- \param M [in] : Homogeneous transformation that describe the
- movement that the camera has to achieve. Only the rotational part of
- this homogeneous transformation is taken into consideration.
- Depending on the rotation representation type
- (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
- construct this object, the parameter \e M represents either the
- rotation that the camera has to achieve to move from the desired
- camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
- that the camera has to achieve to move from the current camera frame
- to the desired one (\f$ ^{c}R_{c^*}\f$).
-
-
-*/
-void
-vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
-{
- vpRotationMatrix R ;
- M.extract(R) ;
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
-}
-
-/*!
-
- Initialise the \f$\theta u_x \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param tu_x : \f$\theta u_x \f$ subset value to initialize.
- \sa get_TUz()
-*/
-void vpFeatureThetaU::set_TUx(const double tu_x)
-{
- s[0] = tu_x ;
- flags[0] = true;
-}
-/*!
-
- Initialise the \f$\theta u_y \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param tu_y : \f$\theta u_y \f$ subset value to initialize.
- \sa get_TUy()
-*/
-void vpFeatureThetaU::set_TUy(const double tu_y)
-{
- s[1] = tu_y ;
- flags[1] = true;
-}
-/*!
-
- Initialise the \f$\theta u_z \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param tu_z : \f$\theta u_z \f$ subset value to initialize.
- \sa get_TUz()
-*/
-void
-vpFeatureThetaU::set_TUz(const double tu_z)
-{
- s[2] = tu_z ;
- flags[2] = true;
-}
-
-/*!
-
- Return the \f$\theta u_x \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double vpFeatureThetaU::get_TUx() const
-{
- return s[0] ;
-}
-
-/*!
- Return the \f$\theta u_y \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double vpFeatureThetaU::get_TUy() const
-{
- return s[1] ;
-}
-
-
-/*!
- Return the \f$\theta u_z \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double
-vpFeatureThetaU::get_TUz() const
-{
- return s[2] ;
-}
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L \f$ from
- a subset (\f$ \theta u_x, \theta u_y, \theta u_z\f$) of the possible
- \f$ \theta u \f$ features that represent the 3D rotation
- \f$^{c^*}R_c\f$ or \f$^{c}R_{c^*}\f$, with
-
- \f[ L = [ 0_3 \; L_{\theta u}] \f]
-
- See the vpFeatureThetaU class description for the equations of
- \f$L_{\theta u}\f$.
-
- \param select : Selection of a subset of the possible \f$ \theta u \f$
- features.
- - To compute the interaction matrix for all the three \f$ \theta u \f$
- features use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
- interaction matrix is \f$ [3 \times 6] \f$
- - To compute the interaction matrix for only one of the \f$ \theta u \f$
- component feature (\f$\theta u_x, \theta u_y, \theta u_z\f$) use one of the
- corresponding function selectTUx(), selectTUy() or selectTUz(). In
- that case the returned interaction matrix is \f$ [1 \times 6] \f$
- dimension.
-
- \return The interaction matrix computed from the \f$ \theta u \f$
- features that represent either the rotation \f$^{c^*}R_c\f$ or the
- rotation \f$^{c}R_{c^*}\f$.
-
- The code below shows how to compute the interaction matrix
- associated to the visual feature \f$s = \theta u_x \f$.
-
- \code
- vpRotationMatrix cdMc;
-
- // Creation of the current feature s
- vpFeatureThetaU s(vpFeatureThetaU::cdRc);
- s.buildFrom(cdMc);
-
- vpMatrix L_x = s.interaction( vpFeatureThetaU::selectTUx() );
- \endcode
-
- The code below shows how to compute the interaction matrix
- associated to the \f$s = (\theta u_x, \theta u_y) \f$
- subset visual feature:
-
- \code
- vpMatrix L_xy = s.interaction( vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy() );
- \endcode
-
- L_xy is here now a 2 by 6 matrix. The first line corresponds to
- the \f$ \theta u_x \f$ visual feature while the second one to the \f$
- \theta u_y \f$ visual feature.
-
- It is also possible to build the interaction matrix from all the \f$
- \theta u \f$ components by:
-
- \code
- vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-
- In that case, L_xyz is a 3 by 6 interaction matrix where the last
- line corresponds to the \f$ \theta u_z \f$ visual feature.
-
-*/
-vpMatrix
-vpFeatureThetaU::interaction(const unsigned int select)
-{
-
- vpMatrix L ;
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
- break;
- case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
- vpColVector u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- u[i] = s[i]/2.0 ;
- }
-
- vpMatrix Lw(3,3) ;
- Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
-
- vpMatrix U2(3,3) ;
- U2.setIdentity() ;
-
- double theta = sqrt(s.sumSquare()) ;
- if (theta >= 1e-6) {
- for (unsigned int i=0 ; i < 3 ; i++)
- u[i] = s[i]/theta ;
-
- vpMatrix skew_u ;
- skew_u = vpColVector::skew(u) ;
- U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
- }
-
- if (rotation == cdRc) {
- Lw += U2;
- }
- else {
- Lw -= U2;
- }
-
- //This version is a simplification
- if (vpFeatureThetaU::selectTUx() & select )
- {
- vpMatrix Lx(1,6) ;
-
- Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
-
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeatureThetaU::selectTUy() & select )
- {
- vpMatrix Ly(1,6) ;
-
- Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
-
- if (vpFeatureThetaU::selectTUz() & select )
- {
- vpMatrix Lz(1,6) ;
-
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
-
- L = vpMatrix::stackMatrices(L,Lz) ;
- }
-
- return L ;
-}
-
-/*!
-
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- Since this visual feature \f$ s \f$ represent either the rotation
- from the desired camera frame to the current camera frame
- \f$^{c^*}R_{c} \f$, or the rotation from the current camera frame to
- the desired camera frame \f$^{c}R_{c^*} \f$, the desired visual
- feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
- the current visual feature \f$ s \f$.
-
- \param s_star : Desired visual visual feature that should be equal to zero.
-
- \param select : The error can be computed for a selection of a
- subset of the possible \f$ \theta u \f$ features.
- - To compute the error for all the three \f$ \theta u \f$ features use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
- dimension column vector.
- - To compute the error for only one of the \f$ \theta u \f$ component
- feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
- corresponding function selectTUx(), selectTUy() or selectTUz(). In
- that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- \exception vpFeatureException::badInitializationError : If the
- desired visual feature \f$ s^* \f$ is not equal to zero.
-
- The code below shows how to use this method to manipulate the
- \f$\theta u_z \f$ subset:
-
- \code
- // Creation of the current feature s
- vpFeatureThetaU s(vpFeatureThetaU::cdRc);
- s.set_TUz(0.3);
-
- // Creation of the desired feature s^*. By default this feature is
- // initialized to zero
- vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
-
- // Compute the interaction matrix for the ThetaU_z feature
- vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
-
- // Compute the error vector (s-s*) for the ThetaU_z feature
- s.error(s_star, vpFeatureThetaU::selectTUz());
- \endcode
-
- To manipulate the subset features \f$s=(\theta u_y, \theta u_z)\f$,
- the code becomes:
- \code
- // Compute the interaction matrix for the ThetaU_y, ThetaU_z features
- vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );
-
- // Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
- vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
- \endcode
-
-*/
-vpColVector
-vpFeatureThetaU::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
-
- if (fabs(s_star.get_s().sumSquare()) > 1e-6)
- {
- vpERROR_TRACE("s* should be zero ! ") ;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "s* should be zero !")) ;
- }
-
- vpColVector e(0) ;
-
-
- if (vpFeatureThetaU::selectTUx() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] ;
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeatureThetaU::selectTUy() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- if (vpFeatureThetaU::selectTUz() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2] ;
- e = vpMatrix::stackMatrices(e,ez) ;
- }
- return e ;
-}
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible \f$ \theta u
- \f$ features.
- - To print all the three \f$ \theta u \f$ features use
- vpBasicFeature::FEATURE_ALL.
- - To print only one of the \f$ \theta u \f$ component
- feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
- corresponding function selectTUx(), selectTUy() or selectTUz().
-
- \code
- vpThetaUVector tu; // Current visual feature s
- tu[0] = 0.1;
- tu[1] = 0.2;
- tu[2] = 0.3;
-
- // Creation of the current feature s
- vpFeatureThetaU s(vpFeatureThetaU::cdRc);
- s.buildFrom(tu);
-
- s.print(); // print all the 3 components of the feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeatureThetaU::selectTUz()); // print only the ThetaU_z component
- \endcode
-*/
-void
-vpFeatureThetaU::print(const unsigned int select) const
-{
- std::cout <<"ThetaU: ";
- if (vpFeatureThetaU::selectTUx() & select ) {
- std::cout << s[0] << " ";
- }
- if (vpFeatureThetaU::selectTUy() & select ) {
- std::cout << s[1] << " ";
- }
- if (vpFeatureThetaU::selectTUz() & select ) {
- std::cout << s[2] << " ";
- }
- std::cout << std::endl;
-}
-
-/*!
-
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureThetaU s;
- s_star = s.duplicate(); // s_star is now a vpFeatureThetaU
- \endcode
-
-*/
-vpFeatureThetaU *vpFeatureThetaU::duplicate() const
-{
- vpFeatureThetaU *feature;
- if (rotation == cdRc)
- feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
- else //if (rotation == cRcd
- feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
-
- return feature ;
-}
-
-/*!
-
- Not implemented.
-
-*/
-void
-vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-/*!
-
- Not implemented.
-
- */
-void
-vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureThetaU.h b/src/visual-feature/vpFeatureThetaU.h
deleted file mode 100644
index aa4a8b9..0000000
--- a/src/visual-feature/vpFeatureThetaU.h
+++ /dev/null
@@ -1,401 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureThetaU.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * ThetaU visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureThetaU_H
-#define vpFeatureThetaU_H
-
-/*!
- \file vpFeatureThetaU.h
- \brief class that defines the ThetaU visual feature
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpBasicFeature.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeatureThetaU
- \ingroup VsFeature3
-
- \brief Class that defines a 3D visual feature \f$ s\f$ from a \f$ \theta
- u \f$ axis/angle parametrization that represent the rotation between
- to frames.
-
- Let us denote \f$ \theta u = (\theta u_x, \theta u_y, \theta u_z)\f$ .
-
- It is convenient to consider two coordinate frames: the current
- camera frame \f$ {\cal{F}}_c \f$ and the desired camera frame \f$
- {\cal{F}}_{c^*} \f$.
-
- Let \f$^{c^*}R_c \f$ be the rotation matrix that gives the
- orientation of the current camera frame relative to the desired camera
- frame. Let \f$ \theta u_{^{c^*}R_c} \f$ to corresponding axis/angle
- representation of this rotation.
-
- Furthermore, let \f$^{c}R_{c^*} \f$ the rotation matrix that gives the
- orientation of the desired camera frame relative to the current
- camera frame. Let \f$ \theta u_{^{c}R_{c^*}} \f$ to corresponding
- axis/angle representation of this rotation.
-
- This class can be used to manipulate two kind of visual features:
-
- - \f$ s = \theta u_{^{c^*}R_c} \f$ if the orientation of current
- camera frame relative to the desired frame has to be
- considered. The desired visual feature \f$ s^* \f$ is equal to
- zero. The corresponding error is than equal to \f$ e=(s-s^*) =
- \theta u_{^{c^*}R_c} \f$. In this case, the interaction matrix
- related to \f$ s \f$ is given by \f[ L = \left[ \begin{array}{cc}
- 0_3 & L_{\theta u} \end{array} \right] \f] with \f[
- L_{\theta u} = I_3 + \frac{\theta}{2} \; [u]_\times +
- \left(1 - \frac{sinc \theta}{sinc^2 \frac{\theta}{2}}\right)
- [u]^2_\times \f] where \f$ 0_3 \f$ is a \f$ 3 \times 3 \f$ nul
- matrix, \f$ I_3 \f$ is the \f$3 \times 3\f$ identity matrix, and
- for more readability \f$ \theta \f$ and \f$ u \f$ respectively the
- angle and the axis coordinates of the \f$ \theta u_{^{c^*}R_c} \f$
- representation.
-
- - \f$ s = \theta u_{^{c}R_{c^*}} \f$ if it is more the orientation
- of the desired camera frame relative to the current frame that has
- to be considered. The desired visual feature \f$ s^* \f$ is equal
- to zero. The corresponding error is than equal to \f$e=(s-s^*) =
- \theta u_{^{c}R_{c^*}} \f$. In this case, the interaction matrix
- related to \f$ s \f$ is given by \f[ L = \left[ \begin{array}{cc}
- 0_3 & L_{\theta u} \end{array} \right] \f] with \f[
- L_{\theta u} = -I_3 + \frac{\theta}{2} \; [u]_\times
- - \left(1 - \frac{sinc \theta}{sinc^2 \frac{\theta}{2}}\right)
- [u]^2_\times \f] where \f$ 0_3 \f$ is a \f$ 3 \times 3 \f$ nul
- matrix, \f$ I_3 \f$ is the \f$3 \times 3\f$ identity matrix, and
- for more readability \f$ \theta \f$ and \f$ u \f$ respectively the
- angle and the axis coordinates of the \f$ \theta u_{^{c}R_{c^*}}
- \f$ representation.
-
- The kind of visual feature is to set during the construction of the
- vpFeatureThetaU() object by using the selector
- vpFeatureThetaU::vpFeatureThetaURotationRepresentationType.
-
- To initialize the feature \f$(\theta u_x, \theta u_y, \theta u_z)\f$
- you may use vpFeatureThetaU member fonctions like set_TUx(),
- set_TUy(), set_TUz(), or also buildFrom() fonctions.
-
- Depending on the choice of the visual feature representation, the
- interaction() method allows to compute the interaction matrix \f$
- L \f$ associated to the visual feature, while the error()
- method computes the error vector \f$(s - s^*)\f$ between the current
- visual feature and the desired one.
-
- To know more on the \f$ \theta u \f$ axis/angle representation for a
- 3D rotation see the vpThetaUVector class.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 3D \f$\theta u\f$ feature \f$(\theta u_x,\theta u_y,
- \theta u_z)\f$ that correspond to the 3D rotation between the
- current camera frame and the desired camera frame. To control six
- degrees of freedom, at least three other features must be considered
- like vpFeatureTranslation visual features. First we create a current
- (\f$s\f$) 3D \f$\theta u\f$ feature, than set the
- task to use the interaction matrix associated to the current feature
- \f$L_s\f$ and than compute the camera velocity \f$v=-\lambda \;
- L_s^+ \; (s-s^*)\f$. The current feature \f$s\f$ is updated in the
- while() loop while \f$s^*\f$ is considered as zero.
-
- \code
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpHomogeneousMatrix cMcd;
- // ... cMcd need here to be initialized from for example a pose estimation.
-
- // Creation of the current feature s that correspond to the rotation
- // in angle/axis parametrization between the current camera frame
- // and the desired camera frame
- vpFeatureThetaU s(vpFeatureThetaU::cRcd);
- s.buildFrom(cMcd); // Initialization of the feature
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
-
- // Add the 3D ThetaU feature to the task
- task.addFeature(s); // s* is here considered as zero
-
- // Control loop
- for ( ; ; ) {
- // ... cMcd need here to be initialized from for example a pose estimation.
-
- // Update the current ThetaU visual feature
- s.buildFrom(cMcd);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
-}
- \endcode
-
- If you want to deal only with the \f$(\theta u_x,\theta u_y)\f$ subset
- feature from the 3D \f$\theta u\f$ , you have just to modify the
- addFeature() call in the previous example by the following line. In
- that case, the dimension of \f$s\f$ is two.
-
- \code
- // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
- // rotation to the task
- task.addFeature(s, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
- \endcode
-
- If you want to build your own control law, this other example shows
- how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
- \f$\theta u\f$ visual feature, compute the corresponding error
- vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpHomogeneousMatrix cdMc;
- // ... cdMc need here to be initialized from for example a pose estimation.
-
- // Creation of the current feature s
- vpFeatureThetaU s(vpFeatureThetaU::cdRc);
- s.buildFrom(cdMc); // Initialization of the feature
-
- // Creation of the desired feature s*. By default this feature is
- // initialized to zero
- vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
-
- // Compute the interaction matrix L_s for the current ThetaU feature
- vpMatrix L = s.interaction();
-
- // Compute the error vector (s-s*) for the ThetaU feature
- s.error(s_star);
-}
- \endcode
-
-
-*/
-class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature
-{
-public:
- typedef enum
- {
- TUx = 1, /*!< Select the subset \f$ \theta u_x \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- TUy = 2, /*!< Select the subset \f$ \theta u_y \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- TUz = 4 /*!< Select the subset \f$ \theta u_z \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- } vpFeatureThetaUType;
- typedef enum
- {
- cdRc, /*!< Selector used to manipulate the visual feature \f$ s
- = \theta u_{^{c^*}R_c} \f$. This visual feature
- represent the orientation of the current camera frame
- relative to the desired camera frame. */
- cRcd /*!< Selector used to manipulate the visual feature \f$ s = \theta
- u_{^{c}R_{c^*}} \f$. This visual feature
- represent the orientation of the desired camera frame
- relative to the current camera frame. */
- } vpFeatureThetaURotationRepresentationType;
- /*
- attributes and members directly related to the vpBasicFeature needs
- other functionalities are useful but not mandatory
- */
-
-public:
- // Basic construction.
- void init() ;
- // Basic constructor.
- vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpThetaUVector &tu,
- vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpRotationMatrix &R,
- vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpHomogeneousMatrix &M,
- vpFeatureThetaURotationRepresentationType r) ;
- void buildFrom(vpThetaUVector &tu) ;
- // build from a rotation matrix
- void buildFrom(const vpRotationMatrix &R) ;
- // build from an homogeneous matrix
- void buildFrom(const vpHomogeneousMatrix &M) ;
-
- //! Destructor. Does nothing.
- virtual ~vpFeatureThetaU() {}
-
-public:
-
-
- void set_TUx(const double tu_x) ;
- void set_TUy(const double tu_y) ;
- void set_TUz(const double tu_z) ;
-
- double get_TUx() const ;
- double get_TUy() const ;
- double get_TUz() const ;
-
-
-public:
- /*
- vpBasicFeature method instantiation
- */
-
- /*!
-
- Function used to select the \f$ \theta u_x\f$ subset of the \f$
- \theta u \f$ visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ \theta u_x\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureThetaU tu;
- vpServo task;
- ...
- // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
- // rotation to the task
- task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
- \endcode
-
- \sa selectTUy(), selectTUz()
- */
- inline static unsigned int selectTUx() { return FEATURE_LINE[0] ; }
- /*!
-
- Function used to select the \f$ \theta u_y\f$ subset of the \f$
- \theta u \f$ visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ \theta u_y\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureThetaU tu;
- vpServo task;
- ...
- // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
- // rotation to the task
- task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
- \endcode
-
- \sa selectTUx(), selectTUz()
- */
- inline static unsigned int selectTUy() { return FEATURE_LINE[1] ; }
- /*!
-
- Function used to select the \f$ \theta u_z\f$ subset of the \f$
- \theta u \f$ visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ \theta u_z\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- \code
- vpFeatureThetaU tu;
- vpServo task;
- ...
- // Add the (ThetaU_z) subset feature from the 3D ThetaU
- // rotation to the task
- task.addFeature(tu, vpFeatureThetaU::selectTUz());
- \endcode
-
- \sa selectTUx(), selectTUy()
- */
- inline static unsigned int selectTUz() { return FEATURE_LINE[2] ; }
- // compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- // compute the error between two visual features from a subset
- // a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- void print(const unsigned int select= FEATURE_ALL) const ;
-
- //! Feature duplication.
- vpFeatureThetaU *duplicate() const ;
-
-public:
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
- private:
- vpFeatureThetaURotationRepresentationType rotation;
-} ;
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureTranslation.cpp b/src/visual-feature/vpFeatureTranslation.cpp
deleted file mode 100644
index 7b7031b..0000000
--- a/src/visual-feature/vpFeatureTranslation.cpp
+++ /dev/null
@@ -1,617 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureTranslation.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 3D translation visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureTranslation.h>
-
-#include <visp/vpMath.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-/*!
- \file vpFeatureTranslation.cpp
- \brief class that defines 3D translation visual feature
-*/
-/*
-
-attributes and members directly related to the vpBasicFeature needs
-other functionalities are useful but not mandatory
-
-*/
-
-/*!
-
- Initialise the memory space requested for 3D translation visual
- feature.
-*/
-void
-vpFeatureTranslation::init()
-{
- //feature dimension
- dim_s = 3 ;
- nbParameters = 1;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
-}
-
-/*!
- Default constructor that build a visual feature and initialize it to zero.
-
- \param r : Type of considered 3D translation feature.
-
-*/
-vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r)
- : f2Mf1(), translation(r)
-{
- init() ;
-}
-
-
-/*!
-
- Constructor that build a 3D visual feature from an homogeneous
- matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
-
- \param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
- move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
- \param r : type of feature. It can be vpFeature::cdMc or vpFeature::cMo.
-
-*/
-vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1_, vpFeatureTranslationRepresentationType r)
- : f2Mf1(), translation(r)
-{
- init() ;
-
- buildFrom(f2Mf1_) ;
-}
-
-/*!
- Build a 3D translation visual feature from an homogeneous
- matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
-
- \param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
- move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
-*/
-void
-vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_)
-{
- this->f2Mf1 = f2Mf1_ ;
- s[0] = f2Mf1[0][3] ;
- s[1] = f2Mf1[1][3] ;
- s[2] = f2Mf1[2][3] ;
-
- flags[0] = true;
-}
-
-/*!
-
- Initialise the \f$t_x \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param t_x : \f$t_x \f$ subset value to initialize.
- \sa get_Tx()
-
-*/
-void vpFeatureTranslation::set_Tx(const double t_x)
-{
- s[0] = t_x ;
-}
-/*!
-
- Initialise the \f$t_y \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param t_y : \f$t_y \f$ subset value to initialize.
- \sa get_Ty()
-
-*/
-void vpFeatureTranslation::set_Ty(const double t_y)
-{
- s[1] = t_y ;
-}
-/*!
-
- Initialise the \f$t_z \f$ subset value of the 3D
- visual feature \f$ s\f$.
-
- \param t_z : \f$t_z \f$ subset value to initialize.
- \sa get_Tz()
-
-*/
-void
-vpFeatureTranslation::set_Tz(const double t_z)
-{
- s[2] = t_z ;
-}
-
-/*!
- Return the \f$t_x \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double vpFeatureTranslation::get_Tx() const
-{
- return s[0] ;
-}
-
-
-/*!
- Return the \f$t_y \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double vpFeatureTranslation::get_Ty() const
-{
- return s[1] ;
-}
-
-
-/*!
- Return the \f$t_z \f$ subset value of the visual feature
- \f$s\f$.
-
-*/
-double
-vpFeatureTranslation::get_Tz() const
-{
- return s[2] ;
-}
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L \f$ from a subset
- \f$(t_x, t_y, t_z)\f$ of the possible translation features that
- represent the 3D transformation \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$.
-
- As it exists three different features, the computation of the
- interaction matrix is diferent for each one.
-
- - With the feature type cdMc:
-
- \f[ L = [ ^{c^*}R_c \;\; 0_3] \f]
-
- where \f$^{c^*}R_c\f$ is the rotation the camera has to achieve to
- move from the desired camera frame to the current camera frame.
-
- - With the feature type cMcd:
-
- \f[ L = [ -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
-
- where \f$^{c}R_{c^*}\f$ is the rotation the camera has to achieve to
- move from the current camera frame to the desired camera frame.
-
- - With the feature type cMo:
-
- \f[ L = [ -I_3 \;\; [^{c}t_o]_\times] \f]
-
- where \f$^{c}t_o \f$ is the position of
- the object frame relative to the current camera frame.
-
- \param select : Selection of a subset of the possible translation
- features.
- - To compute the interaction matrix for all the three translation
- subset features \f$(t_x,t_y,t_y)\f$ use vpBasicFeature::FEATURE_ALL. In
- that case the dimension of the interaction matrix is \f$ [3 \times
- 6] \f$
- - To compute the interaction matrix for only one of the translation
- subset (\f$t_x, t_y, t_z\f$) use
- one of the corresponding function selectTx(), selectTy() or
- selectTz(). In that case the returned interaction matrix is \f$ [1
- \times 6] \f$ dimension.
-
- \return The interaction matrix computed from the translation
- features.
-
- The code below shows how to compute the interaction matrix
- associated to the visual feature \f$s = t_x \f$ using the cdMc feature type.
-
- \code
- vpHomogeneousMatrix cdMc;
- ...
- // Creation of the current feature s
- vpFeatureTranslation s(vpFeatureTranslation::cdMc);
- s.buildFrom(cdMc);
-
- vpMatrix L_x = s.interaction( vpFeatureTranslation::selectTx() );
- \endcode
-
- The code below shows how to compute the interaction matrix
- associated to the \f$s = (t_x, t_y) \f$
- subset visual feature:
-
- \code
- vpMatrix L_xy = s.interaction( vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy() );
- \endcode
-
- L_xy is here now a 2 by 6 matrix. The first line corresponds to
- the \f$ t_x \f$ visual feature while the second one to the \f$
- t_y \f$ visual feature.
-
- It is also possible to build the interaction matrix from all the
- translation components by:
-
- \code
- vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
- \endcode
-
- In that case, L_xyz is a 3 by 6 interaction matrix where the last
- line corresponds to the \f$ t_z \f$ visual feature.
-
-*/
-vpMatrix
-vpFeatureTranslation::interaction(const unsigned int select)
-{
-
- vpMatrix L ;
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user) {
- for (unsigned int i = 0; i < nbParameters; i++) {
- if (flags[i] == false) {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- if (translation == cdMc) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
-
- for (int i=0 ; i < 3 ; i++)
- Lx[0][i] = f2Mf1[0][i] ;
- Lx[0][3] = 0 ; Lx[0][4] = 0 ; Lx[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
-
- for (int i=0 ; i < 3 ; i++)
- Ly[0][i] = f2Mf1[1][i] ;
- Ly[0][3] = 0 ; Ly[0][4] = 0 ; Ly[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
-
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
-
- for (int i=0 ; i < 3 ; i++)
- Lz[0][i] = f2Mf1[2][i] ;
- Lz[0][3] = 0 ; Lz[0][4] = 0 ; Lz[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Lz) ;
- }
- }
- if (translation == cMcd) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
- Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
- Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
- Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
-
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
- Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Lz) ;
- }
- }
-
- if (translation == cMo) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
- Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
- Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
- Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
-
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
- Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
-
- L = vpMatrix::stackMatrices(L,Lz) ;
- }
- }
-
- return L ;
-}
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- - With the feature type cdMc:
- Since this visual feature \f$ s \f$ represent the 3D translation from the desired
- camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
- visual feature \f$ s^* \f$ should be zero. Thus, the error is here
- equal to the current visual feature \f$ s \f$.
-
- - With the feature type cMo:
- In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.
-
- \param s_star : Desired visual feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible translation features.
- - To compute the error for all the three translation vector coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
- dimension column vector.
- - To compute the error for only one of the translation vector coordinate
- feature \f$(t_x, t_y, t_z)\f$ use one of the
- corresponding function selectTx(), selectTy() or selectTz(). In
- that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- \exception vpFeatureException::badInitializationError : If the
- desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.
-
- The code below shows how to use this method to manipulate the \f$
- t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.
-
- \code
- // Creation of the current feature s
- vpFeatureTranslation s(vpFeatureTranslation::cdMc);
- s.set_TUz(0.3); // Initialization of the feature
-
- // Creation of the desired feature s*. By default this feature is
- // initialized to zero
- vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
-
- // Compute the interaction matrix for the t_z translation feature
- vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );
-
- // Compute the error vector (s-s*) for the t_z feature
- s.error(s_star, vpFeatureTranslation::selectTz());
- \endcode
-
- To manipulate the subset features \f$s=(t_y, t_z)\f$,
- the code becomes:
- \code
- // Compute the interaction matrix for the t_y, t_z features
- vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz() );
-
- // Compute the error vector e = (s-s*) for the t_y, t_z feature
- vpColVector e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz());
- \endcode
-
-*/
-vpColVector
-vpFeatureTranslation::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- if(translation == cdMc || translation == cMcd)
- {
- if (s_star.get_s().sumSquare() > 1e-6)
- {
- vpERROR_TRACE("s* should be zero ! ") ;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "s* should be zero !")) ;
- }
- }
-
-
- if (vpFeatureTranslation::selectTx() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0]-s_star[0] ;
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeatureTranslation::selectTy() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1]-s_star[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
-
- if (vpFeatureTranslation::selectTz() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2]-s_star[2] ;
- e = vpMatrix::stackMatrices(e,ez) ;
- }
-
- return e ;
-}
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible translation features.
- - To print all the three translation vector coordinates used as features use
- vpBasicFeature::FEATURE_ALL.
- - To print only one of the translation coordinate
- feature \f$(t_x, t_y, t_z)\f$ use one of the
- corresponding function selectTx(), selectTy() or selectTz().
-
- \code
- vpHomogeneousMatrix cdMc; // Homogenous transformation between the desired camera frame and the current camera frame.
-
- // Creation of the current feature s
- vpFeatureTranslation s(vpFeatureTranslation::cdMc);
- s.buildFrom(cdMc);
-
- s.print(); // print all the 3 components of the translation feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpFeatureTranslation::selectTz()); // print only the t_z component
- \endcode
-*/
-void
-vpFeatureTranslation::print(const unsigned int select) const
-{
- std::cout <<"Translation 3D: ";
- if (vpFeatureTranslation::selectTx() & select ) {
- std::cout << s[0] << " ";
- }
- if (vpFeatureTranslation::selectTy() & select ) {
- std::cout << s[1] << " ";
- }
- if (vpFeatureTranslation::selectTz() & select ) {
- std::cout << s[2] << " ";
- }
- std::cout << std::endl;
-}
-
-
-/*!
-
- Create an object with the same type.
-
- \code
- vpBasicFeature *s_star;
- vpFeatureTranslation s(vpFeatureTranslation::cdMc); //or vpFeatureTranslation s(vpFeatureTranslation::cMo);
- s_star = s.duplicate(); // s_star is now a vpFeatureTranslation
- \endcode
-
-*/
-vpFeatureTranslation *vpFeatureTranslation::duplicate() const
-{
- vpFeatureTranslation *feature = NULL;
- if (translation == cdMc)
- feature = new vpFeatureTranslation(cdMc) ;
- if (translation == cMo)
- feature = new vpFeatureTranslation(cMo) ;
- if (translation == cMcd)
- feature = new vpFeatureTranslation(cMcd) ;
- return feature ;
-}
-
-
-/*!
-
- Not implemented.
-
-*/
-void
-vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-/*!
-
- Not implemented.
-
-*/
-void
-vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureTranslation.h b/src/visual-feature/vpFeatureTranslation.h
deleted file mode 100644
index 4ecacd4..0000000
--- a/src/visual-feature/vpFeatureTranslation.h
+++ /dev/null
@@ -1,494 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureTranslation.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 3D translation visual feature.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureTranslation_H
-#define vpFeatureTranslation_H
-
-/*!
- \file vpFeatureTranslation.h
- \brief class that defines the translation visual feature.
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-/*!
- \class vpFeatureTranslation
- \ingroup VsFeature3
-
- \brief Class that defines the translation visual feature
- \f$s=(t_x,t_y,t_z)\f$.
-
- It is convenient to consider two coordinate frames noted here \f$ {\cal{F}}_1 \f$ and \f$
- {\cal{F}}_{2} \f$.
-
- Let \f$^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ be the homogeneous matrix that gives the
- orientation and the translation of the frame \f$ {\cal{F}}_1 \f$ with respect to the frame \f$ {\cal{F}}_2 \f$.
-
- \f[
- ^{{\cal{F}}_2}M_{{\cal{F}}_1} = \left(\begin{array}{cc}
- ^{{\cal{F}}_2}R_{{\cal{F}}_1} & ^{{\cal{F}}_2}t_{{\cal{F}}_1} \\
- {\bf 0}_{1\times 3} & 1
- \end{array}
- \right)
- \f]
-
- with \f$^{{\cal{F}}_2}R_{{\cal{F}}_1} \f$ the rotation matrix that gives the orientation
- of the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$ and
- \f$^{{\cal{F}}_2}t_{{\cal{F}}_1} \f$ the translation vector that gives the position of
- the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$. To
- know more about homogeneous matrices see vpHomogeneousMatrix
- documentation.
-
- This class can be used to manipulate three kind of visual features:
-
- - This class can be used to manipulate the translation visual feature
- \f$s= ^{c^*}t_c\f$ which gives the position of
- the current camera frame relative to the desired camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
- visual feature \f$ s^* \f$ is equal to zero. The corresponding error
- is than equal to \f$ e=(s-s^*) = ^{c^*}t_c \f$. In this case, the
- interaction matrix related to \f$ s \f$ is given by \f[ L = [
- ^{c^*}R_c \;\; 0_3] \f]
-
- - This class can also be used to manipulate the translation visual feature
- \f$s= ^{c}t_{c^*}\f$ which gives the position of
- the desired camera frame relative to the current camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
- visual feature \f$ s^* \f$ is equal to zero. The corresponding error
- is than equal to \f$ e=(s-s^*) = ^{c}t_{c^*} \f$. In this case, the
- interaction matrix related to \f$ s \f$ is given by \f[ L = [
- -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
-
- - Actually, this class can also be used to manipulate the
- translation visual feature \f$s= ^{c}t_o\f$ which gives the position
- of the object frame relative to the current camera frame. It is
- composed by the three components \f$(t_x,t_y,t_z)\f$ too. The
- desired visual feature \f$ s^* \f$ is the translation visual feature
- \f$s^*= ^{c^*}t_o\f$ which gives the position of the object frame
- relative to the desired camera frame. The corresponding error is
- than equal to \f$ e=(s-s^*) = ^{c}t_o - ^{c^*}t_o \f$. In this case,
- the interaction matrix related to \f$ s \f$ is given by \f[ L = [
- -I_3 \;\; [^{c}t_o]_\times] \f]
-
- To initialize the feature \f$(t_x, t_y, t_z)\f$ you may use member
- fonctions like set_Tx(), set_Ty(), set_Tz(), or also buildFrom()
- fonctions.
-
- The interaction() method allows to compute the interaction matrix
- \f$ L\f$ associated to the translation visual feature, while the
- error() method computes the error vector \f$(s - s^*)\f$ between the
- current visual feature and the desired one.
-
- The code below shows how to create a eye-in hand visual servoing
- task using a 3D translation feature \f$(t_x,t_y,t_z)\f$ that
- correspond to the 3D translation between the desired camera frame
- and the current camera frame. To control six degrees of freedom, at
- least three other features must be considered like vpFeatureThetaU
- visual features. First we create a current (\f$s\f$) and desired
- (\f$s^*\f$) 3D translation feature, set the task to use the
- interaction matrix associated to the current feature \f$L_s\f$ and
- than compute the camera velocity \f$v=-\lambda \; L_s^+ \;
- (s-s^*)\f$. The current feature \f$s\f$ is updated in the while() loop
- while \f$s^*\f$ is set to zero.
-
- \code
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpHomogeneousMatrix cdMc;
- // ... cdMc need here to be initialized from for example a pose estimation.
-
- // Creation of the current visual feature s
- vpFeatureTranslation s(vpFeatureTranslation::cdMc);
- s.buildFrom(cdMc); // Initialization of the current feature s=(tx,ty,tz)
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
- // Set the constant gain
- double lambda = 0.8;
- task.setLambda(lambda);
-
- // Add the 3D translation feature to the task
- task.addFeature(s); // s* is here considered as zero
-
- // Control loop
- for ( ; ; ) {
- // ... cdMc need here to be initialized from for example a pose estimation.
-
- // Update the current 3D translation visual feature
- s.buildFrom(cdMc);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
-}
- \endcode
-
- If you want to deal only with the \f$(t_x,t_y)\f$ subset feature from the 3D
- translation, you have just to modify the addFeature() call in
- the previous example by the following line. In that case, the dimension
- of \f$s\f$ is two.
-
- \code
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(s, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- If you want to build your own control law, this other example shows
- how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
- translation visual feature, compute the corresponding error
- vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
-
- \code
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpMatrix.h>
-
-int main()
-{
- vpHomogeneousMatrix cdMc;
- // ... cdMc need here to be initialized from for example a pose estimation.
-
- // Creation of the current feature s
- vpFeatureTranslation s(vpFeatureTranslation::cdMc);
- s.buildFrom(cdMc); // Initialization of the feature
-
- // Creation of the desired feature s*. By default this feature is
- // initialized to zero
- vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
-
- // Compute the interaction matrix for the translation feature
- vpMatrix L = s.interaction();
-
- // Compute the error vector (s-s*) for the translation feature
- vpColVector e = s.error(s_star); // e = (s-s*)
-}
- \endcode
-
- The code below shows how to create an eye-in hand visual servoing
- task using a 3D translation feature \f$(t_x,t_y,t_z)\f$ that
- correspond to the 3D translation between the current camera frame
- and the object frame. Like with the previous examples, to
- control six degrees of freedom, at least three other features must be
- considered like vpFeatureThetaU visual features. The way to initialize
- the visual features is quite the same as before. The difference is that
- the cMo method must be precised and the desired feature is note
- necessary equal to zero.
-
- \code
-#include <visp/vpFeatureTranslation.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- vpHomogeneousMatrix cdMo;
- // ... cdMo need here to be initialized from for example a pose estimation.
-
- // Creation of the desired visual feature s*
- vpFeatureTranslation s_star(vpFeatureTranslation::cMo);
- s_star.buildFrom(cdMo); // Initialization of the desired feature s*=(tx*,ty*,tz*)
-
- vpHomogeneousMatrix cMo;
- // ... cMo need here to be computed.
-
- // Creation of the current visual feature s
- vpFeatureTranslation s(vpFeatureTranslation::cMo);
- s.buildFrom(cMo); // Initialization of the current feature s=(tx,ty,tz)
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
- // Set the constant gain
- double lambda = 0.8;
- task.setLambda(lambda);
-
- // Add the 3D translation feature to the task
- task.addFeature(s, s_star); // s* is here considered as zero
-
- // Control loop
- for ( ; ; ) {
- // ... cMo need here to be computed from for example a pose estimation.
-
- // Update the current 3D translation visual feature
- s.buildFrom(cMo);
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
-}
- \endcode
-
-*/
-class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature
-{
-public:
-
- /*!
- \enum vpFeatureTranslationRepresentationType
- Kind of implemented 3D translation feature.
- */
- typedef enum {
- /*! Selector used to manipulate the visual feature \f$s=
- ^{c^*}t_c\f$ which gives the position of the current camera frame
- relative to the desired camera frame.*/
- cdMc,
- /*! Selector used to manipulate the visual feature \f$s=
- ^{c}t_{c^*}\f$ which gives the position of the desired camera frame
- relative to the current camera frame.*/
- cMcd,
- /*! Selector used to manipulate the visual feature \f$s=
- ^{c}t_o\f$ which gives the position of the object frame relative to
- the current camera frame. */
- cMo
- } vpFeatureTranslationRepresentationType;
-
- // basic construction
- void init() ;
- // basic constructor
- vpFeatureTranslation(vpFeatureTranslationRepresentationType r) ;
- // constructor : build from an homogeneous matrix
- // cdMc is the displacement that the camera has to realize
- vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, vpFeatureTranslationRepresentationType r) ;
- //! Destructor. Does nothing.
- virtual ~vpFeatureTranslation() {}
-
- // build from an homogeneous matrix
- // cdMc is the displacement that the camera has to realize
- void buildFrom(const vpHomogeneousMatrix &f2Mf1) ;
-
- void set_Tx(const double t_x) ;
- void set_Ty(const double t_y) ;
- void set_Tz(const double t_z) ;
-
- double get_Tx() const ;
- double get_Ty() const ;
- double get_Tz() const ;
-
-
- // feature selection
- /*!
-
- Function used to select the \f$ t_x\f$ subset of the translation
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ t_x\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- - With the feature type cdMc:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cdMc);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- - With the feature type cMcd:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMcd);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- - With the feature type cMo:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMo);
- vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- \sa selectTy(), selectTz()
-
- */
- inline static unsigned int selectTx() { return FEATURE_LINE[0] ; }
- /*!
-
- Function used to select the \f$ t_y\f$ subset of the translation
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ t_y\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- - With the feature type cdMc:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cdMc);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- - With the feature type cMcd:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMcd);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- - With the feature type cMo:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMo);
- vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
- vpServo task;
- ...
- // Add the (tx,ty) subset features from 3D translation to the task
- task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
- \endcode
-
- \sa selectTx(), selectTz()
- */
- inline static unsigned int selectTy() { return FEATURE_LINE[1] ; }
- /*!
-
- Function used to select the \f$ t_z\f$ subset of the translation
- visual feature.
-
- This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$ t_z\f$.
-
- See the interaction() method for an usage example.
-
- This function is also useful in the vpServo class to indicate that
- a subset of the visual feature is to use in the control law:
-
- - With the feature type cdMc:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cdMc);
- vpServo task;
- ...
- // Add the (tz) subset feature from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTz());
- \endcode
-
- - With the feature type cMcd:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMcd);
- vpServo task;
- ...
- // Add the (tz) subset feature from 3D translation to the task
- task.addFeature(t, vpFeatureTranslation::selectTz());
- \endcode
-
- - With the feature type cMo:
- \code
- vpFeatureTranslation t(vpFeatureTranslation::cMo);
- vpFeatureTranslation t_star(vpFeatureTranslation::cMo);
- vpServo task;
- ...
- // Add the (tz) subset feature from 3D translation to the task
- task.addFeature(t, t_star, vpFeatureTranslation::selectTz());
- \endcode
-
- \sa selectTx(), selectTy()
- */
- inline static unsigned int selectTz() { return FEATURE_LINE[2] ; }
- // compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- // compute the error between two visual features from a subset
- // a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- // print the name of the feature
- void print(const unsigned int select= FEATURE_ALL) const ;
-
-
- //! Feature duplication
- vpFeatureTranslation *duplicate() const ;
-
-
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-private:
- //! displacement that the camera has to realize
- vpHomogeneousMatrix f2Mf1;
- vpFeatureTranslationRepresentationType translation;
-} ;
-
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpFeatureVanishingPoint.cpp b/src/visual-feature/vpFeatureVanishingPoint.cpp
deleted file mode 100644
index c3cd4c4..0000000
--- a/src/visual-feature/vpFeatureVanishingPoint.cpp
+++ /dev/null
@@ -1,320 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureVanishingPoint.cpp 4632 2014-02-03 17:06:40Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
- *
- * Authors:
- * Odile Bourquardez
- *
- *****************************************************************************/
-
-
-/*! \file vpFeatureVanishingPoint.cpp
- \brief Class that defines 2D vanishing
- point visual feature (Z coordinate in 3D space is infinity)
-*/
-#include <visp/vpBasicFeature.h>
-#include <visp/vpFeatureVanishingPoint.h>
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-// math
-#include <visp/vpMath.h>
-
-#include <visp/vpFeatureDisplay.h>
-
-void
-vpFeatureVanishingPoint::init()
-{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 2;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //Z not required (infinity)
- //set_Z(1) ;
-
-}
-vpFeatureVanishingPoint::vpFeatureVanishingPoint()
-{
- init() ;
-}
-
-
-//! set the point x-coordinates
-void
-vpFeatureVanishingPoint::set_x(const double _x)
-{
- s[0] = _x ;
- flags[0] = true;
-}
-//! get the point x-coordinates
-double
-vpFeatureVanishingPoint::get_x() const
-{
- return s[0] ;
-}
-
-//! set the point y-coordinates
-void
-vpFeatureVanishingPoint::set_y(const double _y)
-{
- s[1] = _y ;
- flags[1] = true;
-}
-//! get the point y-coordinates
-double
-vpFeatureVanishingPoint::get_y() const
-{
- return s[1] ;
-}
-
-
-//! set the point xy coordinates
-void
-vpFeatureVanishingPoint::set_xy(const double _x,
- const double _y)
-{
- set_x(_x) ;
- set_y(_y) ;
-}
-
-
-//! compute the interaction matrix from a subset of the possible features
-vpMatrix
-vpFeatureVanishingPoint::interaction(const unsigned int select)
-{
- vpMatrix L ;
-
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
- case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
- case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
- default:
- vpTRACE("Problem during the reading of the variable flags");
- }
- }
- }
- resetFlags();
- }
-
- double x = get_x() ;
- double y = get_y() ;
-
- if (vpFeatureVanishingPoint::selectX() & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
-
- Lx[0][0] = 0. ;
- Lx[0][1] = 0. ;
- Lx[0][2] = 0. ;
- Lx[0][3] = x*y ;
- Lx[0][4] = -(1+x*x) ;
- Lx[0][5] = y ;
-
- L = vpMatrix::stackMatrices(L,Lx) ;
- }
-
- if (vpFeatureVanishingPoint::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
-
- Ly[0][0] = 0 ;
- Ly[0][1] = 0. ;
- Ly[0][2] = 0. ;
- Ly[0][3] = 1+y*y ;
- Ly[0][4] = -x*y ;
- Ly[0][5] = -x ;
-
- L = vpMatrix::stackMatrices(L,Ly) ;
- }
- return L ;
-}
-
-
-/*! compute the error between two visual features from a subset of the possible
- features
- */
-vpColVector
-vpFeatureVanishingPoint::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- vpColVector e(0) ;
-
- try{
- if (vpFeatureVanishingPoint::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- if (vpFeatureVanishingPoint::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpMatrix::stackMatrices(e,ey) ;
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matrix related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- return e ;
-}
-
-
-
-void
-vpFeatureVanishingPoint::print(const unsigned int select ) const
-{
-
- std::cout <<"Point: " <<std::endl;
- if (vpFeatureVanishingPoint::selectX() & select )
- std::cout << " x=" << get_x() ;
- if (vpFeatureVanishingPoint::selectY() & select )
- std::cout << " y=" << get_y() ;
- std::cout <<std::endl ;
-}
-
-
-void
-vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
-{
- s[0] = _x ;
- s[1] = _y ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
-}
-
-
-/*!
-
- Display vanishing point feature.
-
- \param cam : Camera parameters.
- \param I : Image.
- \param color : Color to use for the display.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-/*!
-
- Display vanishing point feature.
-
- \param cam : Camera parameters.
- \param I : color Image.
- \param color : Color to use for the display.
- \param thickness : Thickness of the feature representation.
-
-*/
-void
-vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
-{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
-
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-}
-
-
-/*! for memory issue (used by the vpServo class only)
- */
-vpFeatureVanishingPoint *vpFeatureVanishingPoint::duplicate() const
-{
- vpFeatureVanishingPoint *feature = new vpFeatureVanishingPoint ;
- return feature ;
-}
diff --git a/src/visual-feature/vpFeatureVanishingPoint.h b/src/visual-feature/vpFeatureVanishingPoint.h
deleted file mode 100644
index d3c0169..0000000
--- a/src/visual-feature/vpFeatureVanishingPoint.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpFeatureVanishingPoint.h 5237 2015-01-30 13:52:04Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
- *
- * Authors:
- * Odile Bourquardez
- *
- *****************************************************************************/
-
-
-#ifndef vpFeatureVanishingPoint_H
-#define vpFeatureVanishingPoint_H
-
-
-
-/*!
- \file vpFeatureVanishingPoint.h \brief Class that defines 2D vanishing
- point visual feature (Z coordinate in 3D space is infinity)
-*/
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpPoint.h>
-
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpRGBa.h>
-
-
-
-/*!
- \class vpFeatureVanishingPoint
- \ingroup VsFeature2
-
- \brief Class that defines 2D vanishing point visual feature (Z
- coordinate in 3D space is infinity).
-*/
-class VISP_EXPORT vpFeatureVanishingPoint : public vpBasicFeature
-{
-public:
- typedef enum
- {
- X = 1, // x coordinates
- Y = 2 // y coordinates
- } vpFeatureVanishingPointType;
-
- /*
- attributes and members directly related to the vpBasicFeature needs
- other functionalities ar useful but not mandatory
- */
- //no Z required
-
-public:
- //! Default initialization.
- void init() ;
- //! Default constructor.
- vpFeatureVanishingPoint() ;
- //! Destructor.
- virtual ~vpFeatureVanishingPoint() {}
-
-
-public:
- /*
- /section Set coordinates
- */
-
-
- //! set the point x-coordinates
- void set_x(const double _x) ;
- //! get the point x-coordinates
- double get_x() const ;
- //! set the point y-coordinates
- void set_y(const double _y) ;
- //! get the point y-coordinates
- double get_y() const ;
- //! set the point xy coordinates
- void set_xy(const double _x, const double _y) ;
- // void buildFrom(const vpPoint &p) ;
- void buildFrom(const double _x, const double _y) ;
-
-
-
- public:
- /*
- vpBasicFeature method instantiation
- */
- // feature selection
- inline static unsigned int selectX() { return FEATURE_LINE[0] ; }
- inline static unsigned int selectY() { return FEATURE_LINE[1] ; }
-
-
- //! compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- //! compute the error between two visual features from a subset
- //! a the possible features
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- //! compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
- //! print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- //! feature duplication
- vpFeatureVanishingPoint *duplicate() const ;
-
-public:
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const;
-
-} ;
-
-
-
-#endif
diff --git a/src/visual-feature/vpGenericFeature.cpp b/src/visual-feature/vpGenericFeature.cpp
deleted file mode 100644
index 0144fb5..0000000
--- a/src/visual-feature/vpGenericFeature.cpp
+++ /dev/null
@@ -1,777 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpGenericFeature.cpp 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic feature (used to create new feature not implemented in ViSP).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-#include <visp/vpGenericFeature.h>
-
-
-// Exception
-#include <visp/vpException.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpFeatureException.h>
-
-// Debug trace
-#include <visp/vpDebug.h>
-
-
-
-/*!
- \file vpGenericFeature.cpp
- \brief class that defines what is a generic feature (used to create new
- feature not implemented in ViSP2
-*/
-
-vpGenericFeature::~vpGenericFeature()
-{
-
-}
-
-void vpGenericFeature::init()
-{
- s = 0 ;
-}
-
-
-/*!
-
- Default constructor. You are not allowed to use this
- constructor. Please use the vpGenericFeature::vpGenericFeature(int
- _dim) constructor.
-
- \exception vpException::cannotUseConstructorError : If the use call
- this constructor.
-
-*/
-vpGenericFeature::vpGenericFeature()
- : L(), err(), errorStatus(errorNotInitalized)
-{
- /*
- vpERROR_TRACE("You are not allow to use this constructor ") ;
- vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
- "constructor") ;
- vpERROR_TRACE("And provide the dimension of the visual feature ") ;
- throw(vpException(vpException::cannotUseConstructorError,
- "You are not allow to use this constructor ")) ;
- */
-}
-
-
-/*!
- Constructor of the class you have to use. The feature table is initilialized with the good dimension.
-
- \param dimension_gen_s : Dimension of the generic feature. It corresponds to the number of features you want to create.
-*/
-vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s)
- : L(), err(), errorStatus(errorNotInitalized)
-{
- this->dim_s = dimension_gen_s ;
- s.resize(dimension_gen_s) ;
-}
-
-/*!
-
- Set the error vector \f$(s-s*)\f$.
-
- \param error_vector : Error vector \f$(s-s*)\f$.
-
- \exception vpFeatureException::sizeMismatchError : If the size of
- the error vector is bad.
-*/
-void
-vpGenericFeature::setError(const vpColVector &error_vector)
-{
- if (error_vector.getRows() != dim_s)
- {
- vpERROR_TRACE("size mismatch between error dimension"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between error dimension"
- "and feature dimension"));
-
- }
- errorStatus = errorInitialized ;
- err = error_vector ;
-}
-
-
-/*!
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features.
-
- \exception if errorHasBeenInitialized is true (that is if
- vpGenericFeature::setError have been used) then s_star is useless. In that
- since the error HAS TO BE recomputed at each iteration
- errorHasBeenInitialized is set to errHasToBeUpdated if
- vpGenericFeature::serError is not used in the loop then an exception is
- thrown
-
- obviously if vpGenericFeature::setError is not used then s_star is considered
- and this warning is meaningless.
-
- \param s_star : Desired visual feature.
-
- \param select : The error can be computed for a selection of a
- subset of the possible features.
- - To compute the error for all the features use
- vpBasicFeature::FEATURE_ALL. In that case the error vector column
- vector whose dimension is equal to the number of features.
- - To compute the error for only one of the component feature you
- have to say which one you want to take into account. If it is the
- first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
- the second one set select to vpBasicFeature::FEATURE_LINE[1], and
- so on. In that case the error vector is a 1 dimension column
- vector.
- - To compute the error for only two of the component feature you
- have to say which ones you want to take into account. If it is the
- first one and the second one set select to
- vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
- that case the error vector is a 2 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature.
-
- The code below shows how to use this method to manipulate the two
- visual features over three:
-
- \code
- // Creation of the current feature s
- vpGenericFeature s(3);
- s.set_s(0, 0, 0);
-
- // Creation of the desired feature s*
- vpGenericFeature s_star(3);
- s_star.set_s(1, 1, 1);
-
- // Here you have to compute the interaction matrix L
- s.setInteractionMatrix(L);
-
- // Compute the error vector (s-s*) for the two first features
- s.error(s_star, vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
- \endcode
-*/
-vpColVector
-vpGenericFeature::error(const vpBasicFeature &s_star,
- const unsigned int select)
-{
- if (s_star.get_s().getRows() != dim_s)
- {
- vpERROR_TRACE("size mismatch between s* dimension "
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between s* dimension "
- "and feature dimension"));
-
- }
-
- vpColVector e(0) ;
-
- try
- {
- if (errorStatus == errorHasToBeUpdated)
- {
- vpERROR_TRACE("Error has no been updated since last iteration"
- "you should have used vpGenericFeature::setError"
- "in you visual servoing loop") ;
- throw(vpFeatureException(vpFeatureException::badErrorVectorError,
- "Error has no been updated since last iteration"));
- }
- else
- if (errorStatus == errorInitialized)
- {
- vpDEBUG_TRACE(25,"Error init: e=e.");
- errorStatus = errorHasToBeUpdated ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = err[i] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
- }
- else
- {
- vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
-
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[i] - s_star[i] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matric related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- return e ;
-
-}
-
-
-
-/*!
-
- Compute the error \f$ (s-s^*)\f$ between the current and the desired
- visual features from a subset of the possible features. But in this
- case the desired feature is considered as set to 0.
-
- \param select : The error can be computed for a selection of a
- subset of the possible features.
- - To compute the error for all the features use
- vpBasicFeature::FEATURE_ALL. In that case the error vector column
- vector whose dimension is equal to the number of features.
- - To compute the error for only one of the component feature you
- have to say which one you want to take into account. If it is the
- first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
- the second one set select to vpBasicFeature::FEATURE_LINE[1], and
- so on. In that case the error vector is a 1 dimension column
- vector.
- - To compute the error for only two of the component feature you
- have to say which ones you want to take into account. If it is the
- first one and the second one set select to
- vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
- that case the error vector is a 2 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired
- visual feature which is automatically set to zero.
-
- The code below shows how to use this method to manipulate the two
- visual features over three:
- \code
- // Creation of the current feature s
- vpGenericFeature s(3);
- s.set_s(0, 0, 0);
-
- // Here you have to compute the interaction matrix L
- s.setInteractionMatrix(L);
-
- // Compute the error vector (s-s*) for the two first features
- s.error(vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
- \endcode
-*/
-vpColVector
-vpGenericFeature::error( const unsigned int select)
-{
- vpColVector e(0) ;
-
- try
- {
- if (errorStatus == errorHasToBeUpdated)
- {
- vpERROR_TRACE("Error has no been updated since last iteration"
- "you should have used vpGenericFeature::setError"
- "in you visual servoing loop") ;
- throw(vpFeatureException(vpFeatureException::badErrorVectorError,
- "Error has no been updated since last iteration"));
- }
- else
- if (errorStatus == errorInitialized)
- {
- errorStatus = errorHasToBeUpdated ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = err[i] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
- }
- else
- {
-
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = s[i] ;
-
- e = vpMatrix::stackMatrices(e,ex) ;
- }
-
- }
- }
- catch(vpMatrixException me)
- {
- vpERROR_TRACE("caught a Matric related error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
- catch(vpException me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
- }
-
- return e ;
-
-}
-
-
-/*!
-
- Compute and return the interaction matrix \f$ L \f$ for the whole
- features or a part of them.
-
- \param select : Selection of a subset of the possible features.
- - To compute the interaction matrix for all the features use
- vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
- matrix is \f$ [number of features \times 6] \f$
- - To compute the interaction matrix for only one of the component
- feature you have to say which one you want to take into
- account. If it is the first one set select to
- vpBasicFeature::FEATURE_LINE[0], if it is the second one set
- select to vpBasicFeature::FEATURE_LINE[1], and so on. In that case
- the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
- - To compute the interaction matrix for only two of the component
- features you have to say which ones you want to take into
- account. If it is the first one and the second one set select to
- vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
- that case the returned interaction matrix is \f$ [2 \times 6] \f$
- dimension.
-
- \return The interaction matrix computed from the features.
-
- The code below shows how to compute the interaction matrix associated to the
- first visual feature.
- \code
- // Creation of the current feature s
- vpGenericFeature s(3);
- s.set_s(0, 0, 0);
-
- // Here you have to compute the interaction matrix L for all the three features
- s.setInteractionMatrix(L);
-
- vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0] );
- \endcode
-
- The code below shows how to compute the interaction matrix associated to two
- visual features over three.
- \code
- // Creation of the current feature s
- vpGenericFeature s(3);
- s.set_s(0, 0, 0);
-
- // Here you have to compute the interaction matrix L
- s.setInteractionMatrix(L);
-
- vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0]|vpBasicFeature::FEATURE_LINE[1] );
- \endcode
-*/
-vpMatrix
-vpGenericFeature::interaction(const unsigned int select)
-{
- if (L.getRows() == 0)
- {
- std::cout << "interaction matrix " << L << std::endl ;
- vpERROR_TRACE("Interaction has not been initialized");
- std::cout << "A possible reason (may be) is that you have set" << std::endl ;
- std::cout << "the interaction matrix for s and compute a control " << std::endl ;
- std::cout << "with Ls=s* (default) or vice versa" << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::notInitializedError,
- "size mismatch between s* dimension "
- "and feature dimension"));
-
- }
-
- vpMatrix Ls ;
-
- Ls.resize(0,6) ;
-
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
-
- for (int j=0 ; j < 6 ; j++)
- Lx[0][j] = L[i][j] ;
-
- Ls = vpMatrix::stackMatrices(Ls,Lx) ;
- }
-
- return Ls ;
-}
-
-
-/*!
- \brief set the value of the interaction matrix.
-
- \param L_ : The matrix corresponding to the interaction matrix you computed.
-
- \exception an exception is thrown if the number of row of the interaction
- matrix is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
-{
- if (L_.getRows() != dim_s)
- {
- std::cout << L_.getRows() <<" " << dim_s << std::endl ;;
- vpERROR_TRACE("size mismatch between interaction matrix size "
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between interaction matrix size "
- "and feature dimension"));
- }
-
- this->L = L_ ;
-}
-
-/*!
- \brief set the value of all the features.
-
- \param s_vector : It is a vector containing the value of the visual features.
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::set_s(const vpColVector &s_vector)
-{
-
- if (s_vector.getRows() != dim_s)
- {
- vpERROR_TRACE("size mismatch between s dimension"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between s dimension"
- "and feature dimension"));
- }
- this->s = s_vector ;
-}
-
-
-/*!
- \brief get the value of all the features.
-
- \param s_vector : It is a vector which will contain the value of the visual features.
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::get_s(vpColVector &s_vector) const
-{
- if (s_vector.getRows() != dim_s)
- {
- vpERROR_TRACE("size mismatch between s dimension"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between s dimension"
- "and feature dimension"));
- }
- s_vector = this->s ;
-}
-
-
-/*!
- \brief set the value of three features if the number of feature is equal to 3.
-
- \param s0 : value of the first visual feature
-
- \param s1 : value of the second visual feature
-
- \param s2 : value of the third visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::set_s(const double s0, const double s1, const double s2)
-{
-
- if (3 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
-}
-
-
-/*!
- \brief get the value of three features if the number of feature is equal to 3.
-
- \param s0 : value of the first visual feature
-
- \param s1 : value of the second visual feature
-
- \param s2 : value of the third visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
-{
-
- if (3 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
-}
-
-
-/*!
- \brief set the value of two features if the number of feature is equal to 2.
-
- \param s0 : value of the first visual feature
-
- \param s1 : value of the second visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::set_s(const double s0, const double s1)
-{
-
- if (2 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s[0] = s0 ; s[1] = s1 ;
-}
-
-
-/*!
- \brief get the value of two features if the number of feature is equal to 2.
-
- \param s0 : value of the first visual feature
-
- \param s1 : value of the second visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::get_s(double &s0, double &s1) const
-{
-
- if (2 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s0 = s[0] ; s1 = s[1] ;
-}
-
-
-/*!
- \brief set the value of one feature if the number of feature is equal to 1.
-
- \param s0 : value of the visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::set_s(const double s0)
-{
-
- if (1 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s[0] = s0 ;
-}
-
-
-/*!
- \brief get the value of one feature if the number of feature is equal to 1.
-
- \param s0 : value of the visual feature
-
- \exception an exception is thrown if the number of row of the vector s
- is different from the dimension of the visual feature as specified
- in the constructor
-*/
-void
-vpGenericFeature::get_s(double &s0) const
-{
-
- if (1 != dim_s)
- {
- vpERROR_TRACE("size mismatch between number of parameters"
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between number of parameters"
- "and feature dimension"));
-
- }
- s0 = s[0];
-}
-
-
-/*!
- Print to stdout the values of the current visual feature \f$ s \f$.
-
- \param select : Selection of a subset of the possible features.
- - To print all the features use vpBasicFeature::FEATURE_ALL.
- - To print only one of the component features you have to say which
- one you want to take into account. If it is the first one set
- select to vpBasicFeature::FEATURE_LINE[0], if it is the second one
- set select to vpBasicFeature::FEATURE_LINE[1], and so on.
-
- \code
- vpGenericFeature s; // Current visual feature s
-
- // Creation of the current feature s
- s.set_s(0, 0, 0);
-
- s.print(); // print all components of the feature
- s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
- s.print(vpBasicFeature::FEATURE_LINE[0]); // print only the first component
- \endcode
-*/
-void
-vpGenericFeature::print(const unsigned int select) const
-{
-
- std::cout <<"Generic Feature: " ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- std::cout << " s["<<i << "]=" << s[i] ;
- }
-
- std::cout <<std::endl ;
-}
-
-vpGenericFeature *vpGenericFeature::duplicate() const
-{
- vpGenericFeature *feature= new vpGenericFeature(dim_s) ;
-
- vpTRACE("dims = %d",dim_s) ;
- return feature ;
-}
-
-/*!
- Not implemented.
-*/
-void
-vpGenericFeature::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-/*!
- Not implemented.
- */
-void
-vpGenericFeature::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
-{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
-}
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/src/visual-feature/vpGenericFeature.h b/src/visual-feature/vpGenericFeature.h
deleted file mode 100644
index ffbccad..0000000
--- a/src/visual-feature/vpGenericFeature.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/****************************************************************************
- *
- * $Id: vpGenericFeature.h 4649 2014-02-07 14:57:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Generic feature (used to create new feature not implemented in ViSP).
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-#ifndef vpGenericFeature_hh
-#define vpGenericFeature_hh
-
-/*!
- \file vpGenericFeature.h
- \brief class that defines what is a generic feature (used to create new
- feature not implemented in ViSP2
- */
-
-#include <visp/vpMatrix.h>
-#include <visp/vpBasicFeature.h>
-#include <visp/vpRGBa.h>
-
-#include <math.h>
-
-/*!
- \class vpGenericFeature
- \ingroup VsFeatureGeneric
-
- \brief Class that enables to define a feature or a set of features which are not implemented in ViSP as a specific class. It is indeed possible to create its own features, to use the corresponding interaction matrix, and to compute an error between the current and the desired feature. Moreover the created features can be mixed with features already implemented.
-
- The following example shows how to use the vpGenericFeature class to create and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The interaction matrix corresponding to this feature is \f[ L = \left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f].
- \code
-#include <visp/vpGenericFeature.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- //First we have to define the desired feature log(Z*) corresponding to the desired point.
- double xd = 0; //The x coordinate of the desired point.
- double yd = 0; //The y coordinate of the desired point.
- double Zd = 1; //The depth of the desired point.
- vpGenericFeature logZd(1); //The dimension of the feature is 1.
- logZd.set_s( log(Zd) );
-
- //Then we have to define the current feature log(Z) corresponding to the current point.
- double x = 1; //The x coordinate of the current point.
- double y = 1; //The y coordinate of the current point.
- double Z = 2; //The depth of the current point.
- vpGenericFeature logZ(1); //The dimension of the feature is 1.
- logZ.set_s( log(Z) );
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the current visual features sd
- task.setInteractionMatrixType(vpServo::CURRENT);
-
- // Add the point feature to the task
- task.addFeature(logZ, logZd);
-
- // Control loop
- for ( ; ; ) {
- // The new parameters x, y and Z must be computed here.
-
- // Update the current point visual feature
- logZ.set_s( log(Z) ) ;
-
- // We have to compute the interaction matrix corresponding to the feature.
- vpMatrix LlogZ(1,6) ;
- LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
- LlogZ[0][2] = -1/Z;
- LlogZ[0][3] = -y;
- LlogZ[0][4] = x;
- logZ.setInteractionMatrix(LlogZ) ;
-
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
- return 0;
-}
- \endcode
-
-The second example shows how to create and use a feature whose specificity is to have a desired feature fixed to zero. It is the case for the feature \f$ log( \frac{Z}{Z^*}) \f$.
-
- \code
-#include <visp/vpGenericFeature.h>
-#include <visp/vpServo.h>
-
-int main()
-{
- vpServo task; // Visual servoing task
-
- //First we have to define the desired feature log(Z*) corresponding to the desired point.
- double xd = 0; //The x coordinate of the desired point.
- double yd = 0; //The y coordinate of the desired point.
- double Zd = 1; //The depth of the desired point.
-
- //Then we have to define the current feature log(Z) corresponding to the current point.
- double x = 1; //The x coordinate of the current point.
- double y = 1; //The y coordinate of the current point.
- double Z = 2; //The depth of the current point.
- vpGenericFeature logZ(1); //The dimension of the feature is 1.
- logZ.set_s( log(Z/Zd) );
-
- // Set eye-in-hand control law.
- // The computed velocities will be expressed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
- // Interaction matrix is computed with the current visual features sd
- task.setInteractionMatrixType(vpServo::CURRENT);
-
- // Add the point feature to the task
- task.addFeature(logZ);
-
- // Control loop
- for ( ; ; ) {
- // The new parameters x, y and Z must be computed here.
-
- // Update the current point visual feature
- logZ.set_s( log(Z/Zd) ) ;
-
- // We have to compute the interaction matrix corresponding to the feature.
- vpMatrix LlogZ(1,6) ;
- LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
- LlogZ[0][2] = -1/Z;
- LlogZ[0][3] = -y;
- LlogZ[0][4] = x;
- logZ.setInteractionMatrix(LlogZ) ;
-
-
- // compute the control law
- vpColVector v = task.computeControlLaw(); // camera velocity
- }
- return 0;
-}
- \endcode
-
-If the feature needs to be use with other features, the example servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it.
- */
-class VISP_EXPORT vpGenericFeature : public vpBasicFeature
-{
-private:
- vpGenericFeature() ;
-public:
- void init() ;
- vpGenericFeature(unsigned int dim) ;
- virtual ~vpGenericFeature() ;
-public:
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
-
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-
- void print(const unsigned int select = FEATURE_ALL ) const ;
-
- vpGenericFeature *duplicate() const ;
-
-private:
- typedef enum
- {
- errorNotInitalized,
- errorInitialized,
- errorHasToBeUpdated
- } vpGenericFeatureErrorType;
-
- vpMatrix L ;
- vpColVector err ;
- vpGenericFeatureErrorType errorStatus ;
-
-public:
- void setInteractionMatrix(const vpMatrix &L) ;
- vpMatrix getInteractionMatrix() const { return L ; }
- void setError(const vpColVector &error_vector) ;
- void set_s(const vpColVector &s) ;
- void set_s(const double s0) ;
- void set_s(const double s0, const double s1) ;
- void set_s(const double s0, const double s1, const double s2) ;
-
- void get_s(vpColVector &s) const;
- void get_s(double &s0) const;
- void get_s(double &s0, double &s1) const;
- void get_s(double &s0, double &s1, double &s2) const;
-
-public:
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
-
-
-} ;
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
deleted file mode 100644
index a208460..0000000
--- a/test/CMakeLists.txt
+++ /dev/null
@@ -1,64 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4574 2014-01-09 08:48:51Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- camera
- device
- feature
- homography
- image
- key-point
- math
- network
- pose
- servo-afma4
- servo-afma6
- servo-viper
- tools
- tracking
- visual-feature
-)
-
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
diff --git a/test/camera/CMakeLists.txt b/test/camera/CMakeLists.txt
deleted file mode 100644
index 3c36476..0000000
--- a/test/camera/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testCameraParametersConversion.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/camera/testCameraParametersConversion.cpp b/test/camera/testCameraParametersConversion.cpp
deleted file mode 100644
index f8a70f9..0000000
--- a/test/camera/testCameraParametersConversion.cpp
+++ /dev/null
@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- * $Id: testCameraParametersConversion.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Performs various tests on the vpPixelMeterConversion and
- * vpPixelMeterConversion class.
- *
- * Authors:
- * Anthony saunier
- *
- *****************************************************************************/
-
-
-
-/*!
- \file testCameraParametersConversion.cpp
-
- Performs various tests on the vpPixelMeterConversion and
- vpPixelMeterConversion class.
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-#include <visp/vpMath.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpMeterPixelConversion.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpMath.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
- Performs various tests on the vpPixelMeterConversion and\n\
- vpPixelMeterConversion class.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpCameraParameters cam;
- double px,py,u0,v0;
- px = 1657.429131;
- py = 1658.818598;
- u0 = 322.2437833;
- v0 = 230.8012737;
- vpCameraParameters camDist;
- double px_dist,py_dist,u0_dist,v0_dist,kud_dist,kdu_dist;
- px_dist = 1624.824731;
- py_dist = 1625.263641;
- u0_dist = 324.0923411;
- v0_dist = 245.2421388;
- kud_dist = -0.1741532338;
- kdu_dist = 0.1771165148;
-
- cam.initPersProjWithoutDistortion(px,py,u0,v0);
- camDist.initPersProjWithDistortion(px_dist,py_dist,u0_dist,v0_dist,
- kud_dist, kdu_dist);
-
- double u1 = 320;
- double v1 = 240;
- double x1 = 0, y1 = 0;
- double u2 = 0, v2 = 0;
- vpPixelMeterConversion::convertPoint(cam,u1,v1,x1,y1);
- vpMeterPixelConversion::convertPoint(cam,x1,y1,u2,v2);
- if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
- vpTRACE("Error in convertPoint without distortion:\n"
- "u1 = %f, u2 = %f\n"
- "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
- return -1;
- }
- vpTRACE("convertPoint without distortion :\n"
- "u1 - u2 = %.20f\n"
- "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
-
- vpPixelMeterConversion::convertPoint(camDist,u1,v1,x1,y1);
- vpMeterPixelConversion::convertPoint(camDist,x1,y1,u2,v2);
- if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
- vpTRACE("Error in convertPoint with distortion :\n"
- "u1 = %f, u2 = %f\n"
- "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
- return -1;
- }
- vpTRACE("convertPoint with distortion :\n"
- "u1 - u2 = %.20f\n"
- "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/device/CMakeLists.txt b/test/device/CMakeLists.txt
deleted file mode 100644
index f61ebd2..0000000
--- a/test/device/CMakeLists.txt
+++ /dev/null
@@ -1,51 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 3057 2011-02-11 13:17:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- framegrabber
- display
-)
-
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
diff --git a/test/device/display/CMakeLists.txt b/test/device/display/CMakeLists.txt
deleted file mode 100644
index abb24d2..0000000
--- a/test/device/display/CMakeLists.txt
+++ /dev/null
@@ -1,81 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testClick.cpp
- testDisplayRoi.cpp
- testDisplays.cpp
- testMouseEvent.cpp
- testVideoDevice.cpp
- testVideoDeviceDual.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
-
-add_test(testClick testClick -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(testDisplayRoi testDisplayRoi -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(testDisplays testDisplays -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(testMouseEvent testMouseEvent -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(testVideoDevice testVideoDevice -c ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(testVideoDeviceDual testVideoDeviceDual -c ${OPTION_TO_DESACTIVE_DISPLAY})
diff --git a/test/device/display/testClick.cpp b/test/device/display/testClick.cpp
deleted file mode 100644
index ed3d5c1..0000000
--- a/test/device/display/testClick.cpp
+++ /dev/null
@@ -1,443 +0,0 @@
-/****************************************************************************
- *
- * $Id: testClick.cpp 5230 2015-01-30 06:55:59Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for mouse click manipulations.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <iostream>
-#include <string>
-
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-/*!
- \example testClick.cpp
-
- \brief Mouse click manipulations.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:hlt:dc"
-
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
-
-void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display );
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param dtype : Type of video device.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype)
-{
- fprintf(stdout, "\n\
-Test click functionnalities in video devices or display.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] \n\
- [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
-", name);
-
- std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
- }
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -t <type of video device> \"%s\"\n\
- String specifying the video device to use.\n\
- Possible values:\n\
- \"X11\": only on UNIX platforms,\n\
- \"GTK\": on all plaforms,\n\
- \"GDI\": only on Windows platform (Graphics Device Interface),\n\
- \"D3D\": only on Windows platform (Direct3D).\n\
- \"CV\" : (OpenCV).\n\
-\n\
- -l\n\
- Print the list of video-devices available and exit.\n\
-\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), display.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param dtype : Type of video device.
- \param list : Set as true,list the available video-devices.
- \param click_allowed : Enable/disable mouse click.
- \param display : Set as true, activates the image display. This is
- the default configuration. When set to false, the display is
- disabled. This can be useful for automatic tests using crontab
- under Unix or using the task manager under Windows.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- std::string sDisplayType;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
- // Parse the display type option
- if (sDisplayType.compare("X11") == 0) {
- dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
- dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
- dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
- dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
- dtype = vpCV;
- }
-
- break;
- case 'h': usage(argv[0], NULL, ipath, dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
-
- default:
- usage(argv[0], optarg_, ipath, dtype); return false; break;
- }
- }
-
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, dtype);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- bool opt_list = false; // To print the list of video devices
- vpDisplayType opt_dtype; // Type of display to use
- std::string ipath;
- std::string filename;
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Default display is one available
-#if defined VISP_HAVE_GTK
- opt_dtype = vpGTK;
-#elif defined VISP_HAVE_X11
- opt_dtype = vpX11;
-#elif defined VISP_HAVE_GDI
- opt_dtype = vpGDI;
-#elif defined VISP_HAVE_D3D9
- opt_dtype = vpD3D;
-#elif defined VISP_HAVE_OPENCV
- opt_dtype = vpCV;
-#endif
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Print the list of video-devices available
- if (opt_list) {
- unsigned nbDevices = 0;
- std::cout << "List of video-devices available: \n";
-#if defined VISP_HAVE_GTK
- std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_X11
- std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_GDI
- std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_D3D9
- std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_OPENCV
- std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
- if (!nbDevices) {
- std::cout << " No display is available\n";
- }
- return (0);
- }
-
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- // Create a grey level image
- vpImage<unsigned char> I ;
-
- // Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(I, filename) ;
-
- // Create a display for the image
- vpDisplay *display = NULL;
-
- switch(opt_dtype) {
- case vpX11:
- std::cout << "Requested X11 display functionnalities..." << std::endl;
-#if defined VISP_HAVE_X11
- display = new vpDisplayX;
-#else
- std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGTK:
- std::cout << "Requested GTK display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GTK
- display = new vpDisplayGTK;
-#else
- std::cout << " Sorry, GTK video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGDI:
- std::cout << "Requested GDI display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GDI
- display = new vpDisplayGDI;
-#else
- std::cout << " Sorry, GDI video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpD3D:
- std::cout << "Requested D3D display functionnalities..." << std::endl;
-#if defined VISP_HAVE_D3D9
- display = new vpDisplayD3D;
-#else
- std::cout << " Sorry, D3D video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpCV:
- std::cout << "Requested OpenCV display functionnalities..." << std::endl;
-#if defined(VISP_HAVE_OPENCV)
- display = new vpDisplayOpenCV;
-#else
- std::cout << " Sorry, OpenCV video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- }
-
- if (opt_display){
-
- // We open a window using either X11 or GTK or GDI.
- // Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- if ( opt_click_allowed ){
- std::cout << "Click on a pixel to get his coordinates...\n";
- vpImagePoint ip;
- vpMouseButton::vpMouseButtonType button;
- vpDisplay::getClick(I, ip, button);
- std::cout << " You click down on pixel (" << ip <<") ";
- switch(button) {
- case vpMouseButton::button1: std::cout << "with left button.\n"; break;
- case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
- case vpMouseButton::button3: std::cout << "with right button.\n"; break;
- case vpMouseButton::none: break;
- }
- vpDisplay::getClickUp(I, ip, button);
- std::cout << " You click up on pixel (" << ip <<") ";
- switch(button) {
- case vpMouseButton::button1: std::cout << "with left button.\n"; break;
- case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
- case vpMouseButton::button3: std::cout << "with right button.\n"; break;
- case vpMouseButton::none: break;
- }
- vpDisplay::getPointerPosition(I,ip);
- std::cout << " Pointer poisition : " << ip << std::endl;
- std::cout << "A click to exit...\n";
- vpDisplay::getClick(I) ;
- }
- }
- delete display;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
- exit(-1);
- }
-}
-
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
-
-#endif
diff --git a/test/device/display/testDisplayRoi.cpp b/test/device/display/testDisplayRoi.cpp
deleted file mode 100644
index 0e2ec19..0000000
--- a/test/device/display/testDisplayRoi.cpp
+++ /dev/null
@@ -1,209 +0,0 @@
-/****************************************************************************
- *
- * $Id: testDisplayRoi.cpp 4776 2014-07-11 10:11:02Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image roi display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testDisplayRoi.cpp
-
- Test display of an image roi.
-
-*/
-
-#include <stdlib.h>
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpRect.h>
-
-// List of allowed command line options
-#define GETOPTARGS "cdh"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
-
- */
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Read an image on the disk, display it using X11, display some\n\
-features (line, circle, caracters) in overlay and finaly write \n\
-the image and the overlayed features in an image on the disk.\n\
-\n\
-SYNOPSIS\n\
- %s [-c] [-d] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -c\n\
- Disable the mouse click. Useful to automate the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Disable the image display. This can be useful \n\
- for automatic tests using crontab under Unix or \n\
- using the task manager under Windows.\n\
-\n\
- -h\n\
- Print the help.\n\n");
-
- if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
- }
-
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param click_allowed : Enable/disable mouse click.
- \param display : Set as true, activates the image display. This is
- the default configuration. When set to false, the display is
- disabled. This can be useful for automatic tests using crontab
- under Unix or using the task manager under Windows.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int main(int argc, const char ** argv)
-{
-#ifdef VISP_HAVE_DISPLAY
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- if (opt_display) {
-
- vpImage<unsigned char> I(480,640,255);
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX d;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK d;
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI d;
-#elif defined(VISP_HAVE_D3D9)
- vpDisplayD3D d;
-#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV d;
-#endif
- d.init(I);
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- I = 0;
-
- vpRect roi(I.getWidth()/4, I.getHeight()/4, I.getWidth()/2, I.getHeight()/2);
- vpDisplay::displayROI(I, roi);
- vpDisplay::flush(I);
- if (opt_click_allowed) {
- std::cout << "A click in the image to continue..." << std::endl;
- vpDisplay::getClick(I);
- }
- vpDisplay::close(I);
-
- vpImage<vpRGBa> C(480,640,vpRGBa(255,0,0,0));
-
- //vpDisplayX d;
- d.init(C);
- vpDisplay::display(C);
- vpDisplay::flush(C);
-
- C = vpRGBa(0,255,0,0);
-
- vpDisplay::displayROI(C, roi);
- vpDisplay::flush(C);
- if (opt_click_allowed) {
- std::cout << "A click in the image to exit..." << std::endl;
- vpDisplay::getClick(C);
- }
- }
-#else
- (void)argc;
- (void)argv;
-#endif
- return 0;
-}
diff --git a/test/device/display/testDisplays.cpp b/test/device/display/testDisplays.cpp
deleted file mode 100644
index 8d904cb..0000000
--- a/test/device/display/testDisplays.cpp
+++ /dev/null
@@ -1,408 +0,0 @@
-/****************************************************************************
- *
- * $Id: testDisplays.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image display.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <iostream>
-#include <string>
-
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpRect.h>
-
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-/*!
- \example testDisplays.cpp
-
- \brief Test all the displays. Draws several shapes.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "hldc"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display);
-void draw(vpImage<vpRGBa> &I);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
-
- */
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test video devices or display.\n\
-\n\
-SYNOPSIS\n\
- %s [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
-", name);
-
- std::string display;
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -l\n\
- Print the list of video-devices available and exit.\n\
-\n\
- -h\n\
- Print the help.\n\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param list : To get the list of available display.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- std::string sDisplayType;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'l': list = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
-
- default:
- usage(argv[0], optarg_); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-void draw(vpImage<vpRGBa> &I)
-{
- vpImagePoint iP1, iP2;
- unsigned int w,h;
-
- iP1.set_i(20);
- iP1.set_j(10);
- iP2.set_i(20);
- iP2.set_j(30);
- vpDisplay::displayArrow (I, iP1, iP2, vpColor::green, 4, 2, 3);
-
- iP1.set_i(20);
- iP1.set_j(60);
- vpDisplay::displayText (I, iP1, "Test...", vpColor::black);
-
- iP1.set_i(80);
- iP1.set_j(220);
- iP2.set_i(80);
- iP2.set_j(480);
- vpDisplay::displayCircle (I, iP1, 30, vpColor::red, false, 3);
- vpDisplay::displayCircle (I, iP2, 30, vpColor::red, true, 3);
-
- iP1.set_i(20);
- iP1.set_j(220);
- vpDisplay::displayCross (I, iP1, 5, vpColor::blue, 1);
-
- iP1.set_i(140);
- iP1.set_j(10);
- iP2.set_i(140);
- iP2.set_j(50);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
-
- iP1.set_i(120);
- iP1.set_j(180);
- iP2.set_i(160);
- iP2.set_j(250);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
-
- iP1.set_i(160);
- iP1.set_j(280);
- iP2.set_i(120);
- iP2.set_j(340);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
-
- //static void displayFrame (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, vpColor color)
-
- iP1.set_i(140);
- iP1.set_j(80);
- iP2.set_i(140);
- iP2.set_j(150);
- vpDisplay::displayLine (I, iP1, iP2, vpColor::orange, 3);
-
- iP1.set_i(140);
- iP1.set_j(400);
- vpDisplay::displayPoint (I, iP1, vpColor::red);
-
- iP1.set_i(350);
- iP1.set_j(20);
- w = 60;
- h = 50;
- vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, false, 3);
-
- iP1.set_i(350);
- iP1.set_j(110);
- vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, true, 3);
-
- iP1.set_i(350);
- iP1.set_j(200);
- iP2.set_i(400);
- iP2.set_j(260);
- vpDisplay::displayRectangle (I, iP1, iP2, vpColor::orange, false, 3);
-
- iP1.set_i(350);
- iP1.set_j(290);
- iP2.set_i(400);
- iP2.set_j(350);
- vpRect rectangle(iP1, iP2);
- vpDisplay::displayRectangle (I, rectangle, vpColor::yellow, false, 3);
-
- iP1.set_i(380);
- iP1.set_j(400);
- vpDisplay::displayRectangle (I, iP1, 45, w, h, vpColor::green, 3);
-}
-
-int
-main(int argc, const char ** argv)
-{
- try{
- bool opt_list = false; // To print the list of video devices
- bool opt_click_allowed = true;
- bool opt_display = true;
-
-
- // Read the command line options
- if (getOptions(argc, argv, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Print the list of video-devices available
- if (opt_list) {
- unsigned nbDevices = 0;
- std::cout << "List of video-devices available: \n";
-#if defined VISP_HAVE_GTK
- std::cout << " GTK\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_X11
- std::cout << " X11\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_GDI
- std::cout << " GDI\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_D3D9
- std::cout << " D3D\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_OPENCV
- std::cout << " OpenCV\n";
- nbDevices ++;
-#endif
- if (!nbDevices) {
- std::cout << " No display is available\n";
- }
- return (0);
- }
-
- // Create a grey level image
- vpImage<unsigned char> I;
- // Create a color image
- vpImage<vpRGBa> Irgba;
-
- // Create a color image for each display.
- vpImage<vpRGBa> Ix;
- vpImage<vpRGBa> Igtk;
- vpImage<vpRGBa> Icv;
- vpImage<vpRGBa> Igdi;
- vpImage<vpRGBa> Id3d;
-
-#if defined VISP_HAVE_X11
- vpDisplayX *displayX = NULL;
- displayX = new vpDisplayX;
- Ix.init(480, 640, 255);
- if (opt_display)
- {
- displayX->init(Ix, 100, 100,"Display X11") ;
- vpDisplay::display(Ix) ;
- draw(Ix);
- vpDisplay::flush(Ix);
- if (opt_click_allowed)
- vpDisplay::getClick(Ix);
- }
-#endif
-
-#if defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV *displayCv = NULL;
- displayCv = new vpDisplayOpenCV;
- Icv.init(480, 640, 255);
- if (opt_display)
- {
- displayCv->init(Icv, 100, 100,"Display OpenCV") ;
- vpDisplay::display(Icv) ;
- draw(Icv);
- vpDisplay::flush(Icv);
- if (opt_click_allowed)
- vpDisplay::getClick(Icv);
- }
-#endif
-
-#if defined VISP_HAVE_GTK
- vpDisplayGTK *displayGtk = NULL;
- displayGtk = new vpDisplayGTK;
- Igtk.init(480, 640, 255);
- if (opt_display)
- {
- displayGtk->init(Igtk, 100, 100,"Display GTK") ;
- vpDisplay::display(Igtk) ;
- draw(Igtk);
- vpDisplay::flush(Igtk);
- if (opt_click_allowed)
- vpDisplay::getClick(Igtk);
- }
-#endif
-
-#if defined VISP_HAVE_GDI
- vpDisplayGDI *displayGdi = NULL;
- displayGdi = new vpDisplayGDI;
- Igdi.init(480, 640, 255);
- if (opt_display)
- {
- displayGdi->init(Igdi, 100, 100,"Display GDI") ;
- vpDisplay::display(Igdi) ;
- draw(Igdi);
- vpDisplay::flush(Igdi);
- if (opt_click_allowed)
- vpDisplay::getClick(Igdi);
- }
-#endif
-
-#if defined VISP_HAVE_D3D9
- vpDisplayD3D *displayD3d = NULL;
- displayD3d = new vpDisplayD3D;
- Id3d.init(480, 640, 255);
- if (opt_display)
- {
- displayD3d->init(Id3d, 100, 100,"Display Direct 3D") ;
- vpDisplay::display(Id3d) ;
- draw(Id3d);
- vpDisplay::flush(Id3d);
- if (opt_click_allowed)
- vpDisplay::getClick(Id3d);
- }
-#endif
-
-
-
-#if defined VISP_HAVE_X11
- delete displayX;
-#endif
-
-#if defined VISP_HAVE_GTK
- delete displayGtk;
-#endif
-
-#if defined(VISP_HAVE_OPENCV)
- delete displayCv;
-#endif
-
-#if defined VISP_HAVE_GDI
- delete displayGdi;
-#endif
-
-#if defined VISP_HAVE_D3D9
- delete displayD3d;
-#endif
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
- exit(-1);
- }
-}
-
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
-
-#endif
diff --git a/test/device/display/testMouseEvent.cpp b/test/device/display/testMouseEvent.cpp
deleted file mode 100644
index 52c4282..0000000
--- a/test/device/display/testMouseEvent.cpp
+++ /dev/null
@@ -1,593 +0,0 @@
-/****************************************************************************
- *
- * $Id: testMouseEvent.cpp 5230 2015-01-30 06:55:59Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read an image sequence from the disk and display it.
- *
- * Authors:
- * Fabien Spindler
- * Anthony Saunier
- *
- *****************************************************************************/
-/*!
- \file testMouseEvent.cpp
-
- \brief Read an image sequence from the disk and display it.
-
- The sequence is made of separate images. Each image corresponds to a
- PGM file.
-
-*/
-
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
-#include <iomanip>
-
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpMouseButton.h>
-
-#include <visp/vpTime.h>
-
-/*!
- \example testMouseEvent.cpp
-
- Read an image sequence from the disk and display it.
-
- The sequence is made of separate images. Each image corresponds to a
- PGM file.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "cdi:lp:ht:f:n:s:w"
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
-} vpDisplayType;
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string ppath,
- unsigned first, unsigned nimages, unsigned step, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath,
- unsigned &first, unsigned &nimages, unsigned &step,
- vpDisplayType &dtype, bool &list, bool &display, bool &click, bool &wait);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath : Input image path.
- \param ppath : Personal image path.
- \param first : First image.
- \param nimages : Number of images to manipulate.
- \param step : Step between two images.
- \param dtype : Type of video device.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string ppath,
- unsigned first, unsigned nimages, unsigned step, vpDisplayType &dtype)
-{
- fprintf(stdout, "\n\
-Read an image sequence from the disk and display it.\n\
-The sequence is made of separate images. Each image corresponds\n\
-to a PGM file.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <test image path>] [-p <personal image path>]\n\
- [-f <first image>] [-n <number of images>] [-s <step>] \n\
- [-t <type of video device>] [-l] [-w] [-c] [-d] [-h]\n \
- ", name);
-
- std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- }
-
- fprintf(stdout, "\n\
- OPTIONS: Default\n\
- -i <test image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
- images. These images come from ViSP-images-x.y.z.tar.gz\n\
- available on the ViSP website.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
- \n\
- -p <personal image path> %s\n\
- Specify a personal sequence containing images \n\
- to process.\n\
- By image sequence, we mean one file per image.\n\
- The following image file formats PNM (PGM P5, PPM P6)\n\
- are supported. The format is selected by analysing \n\
- the filename extension.\n\
- Example : \"/Temp/ViSP-images/cube/image.%%04d.pgm\"\n\
- %%04d is for the image numbering.\n\
- \n\
- -f <first image> %u\n\
- First image number of the sequence.\n\
- \n\
- -n <number of images> %u\n\
- Number of images to load from the sequence.\n\
- \n\
- -s <step> %u\n\
- Step between two images.\n\
-\n\
- -t <type of video device> \"%s\"\n\
- String specifying the video device to use.\n\
- Possible values:\n\
- \"X11\": only on UNIX platforms,\n\
- \"GTK\": on all plaforms,\n\
- \"GDI\": only on Windows platform (Graphics Device Interface),\n\
- \"D3D\": only on Windows platform (Direct3D).\n\
-\n\
- -l\n\
- Print the list of video-devices available and exit.\n\
-\n\
- -c\n\
- Disable mouse click.\n\
-\n\
- -d\n\
- Disable the image display. This can be useful \n\
- for automatic tests using crontab under Unix or \n\
- using the task manager under Windows.\n\
-\n\
- -w\n\
- Wait for a mouse click between two images.\n\
- If the image display is disabled (using -d)\n\
- this option is without effect.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(),ppath.c_str(), first, nimages, step, display.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath : Input image path.
- \param ppath : Personal image path.
- \param first : First image.
- \param nimages : Number of images to display.
- \param step : Step between two images.
- \param dtype : Type of video device.
- \param list : Set as true,list the available video-devices.
- \param click : Set as true, activates the mouse click.
- \param display : Set as true, activates the image display. This is
- the default configuration. When set to false, the display is
- disabled. This can be useful for automatic tests using crontab
- under Unix or using the task manager under Windows.
-
- \param wait : Boolean for waiting a mouse click between two images.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath,
- unsigned &first, unsigned &nimages, unsigned &step,
- vpDisplayType &dtype, bool &list, bool &display, bool &click, bool &wait)
-{
- const char *optarg_;
- int c;
- std::string sDisplayType;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': click = false; break;
- case 'd': display = false; break;
- case 't': sDisplayType = optarg_;
- // Parse the display type option
- if (sDisplayType.compare("X11") == 0) {
- dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
- dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
- dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
- dtype = vpD3D;
- }
-
- break;
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'w': wait = true; break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
- return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, ppath, first, nimages, step, dtype);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- std::string env_ipath;
- std::string opt_ipath;
- std::string ipath;
- std::string opt_ppath;
- std::string dirname;
- std::string filename;
- unsigned opt_first = 30;
- unsigned opt_nimages = 10;
- unsigned opt_step = 1;
- vpDisplayType opt_dtype; // Type of display to use
- bool opt_list = false; // To print the list of video devices
- bool opt_display = true;
- bool opt_click = true;
- bool opt_click_blocking = false;
-
- // Default display is one available
-#if defined VISP_HAVE_GTK
- opt_dtype = vpGTK;
-#elif defined VISP_HAVE_X11
- opt_dtype = vpX11;
-#elif defined VISP_HAVE_GDI
- opt_dtype = vpGDI;
-#elif defined VISP_HAVE_D3D9
- opt_dtype = vpD3D;
-#endif
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_dtype, opt_list, opt_display, opt_click,
- opt_click_blocking) == false) {
- exit (-1);
- }
- // Print the list of video-devices available
- if (opt_list) {
- unsigned nbDevices = 0;
- std::cout << "List of video-devices available: \n";
-#if defined VISP_HAVE_GTK
- std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_X11
- std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_GDI
- std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_D3D9
- std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
-#endif
- if (!nbDevices) {
- std::cout << " No display is available\n";
- }
- return (0);
- }
-
- if ( ! opt_display )
- opt_click_blocking = false; // turn off the waiting
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
- usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step,opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
- << " use personal images." << std::endl
- << std::endl;
-
- exit(-1);
- }
-
- // 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<unsigned char> I ;
-
- unsigned iter = opt_first;
- std::ostringstream s;
- char cfilename[FILENAME_MAX];
-
- if (opt_ppath.empty()){
-
-
- // Warning :
- // the image sequence is not provided with the ViSP package
- // therefore the program will return you an error :
- // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
- // ViSP-images/cube/image.0001.pgm
- // !! vpDotExample.cpp: main(#95) :Error while reading the image
- // terminate called after throwing an instance of 'vpImageException'
- //
- // The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
- // in the download section. It is named "ViSP-images.tar.gz"
-
- // Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
-
- // Build the name of the image file
-
- s.setf(std::ios::right, std::ios::adjustfield);
- s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
- filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
-
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
- filename = cfilename;
- }
- // Read the PGM image named "filename" on the disk, and put the
- // bitmap into the image structure I. I is initialized to the
- // correct size
- //
- // exception readPGM may throw various exception if, for example,
- // the file does not exist, or if the memory cannot be allocated
- try{
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot read " << filename << std::endl;
- std::cerr << " Check your -i " << ipath << " option, " << std::endl
- << " or your -p " << opt_ppath << " option " <<std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable"
- << std::endl;
- exit(-1);
- }
- // Create a display for the image
- vpDisplay *display = NULL;
-
- switch(opt_dtype) {
- case vpX11:
- std::cout << "Requested X11 display functionnalities..." << std::endl;
-#if defined VISP_HAVE_X11
- display = new vpDisplayX;
-#else
- std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGTK:
- std::cout << "Requested GTK display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GTK
- display = new vpDisplayGTK;
-#else
- std::cout << " Sorry, GTK video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGDI:
- std::cout << "Requested GDI display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GDI
- display = new vpDisplayGDI;
-#else
- std::cout << " Sorry, GDI video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpD3D:
- std::cout << "Requested D3D display functionnalities..." << std::endl;
-#if defined VISP_HAVE_D3D9
- display = new vpDisplayD3D;
-#else
- std::cout << " Sorry, D3D video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- }
-
- if (opt_display) {
- try {
- // We open a window using either X11 or GTK or GDI.
- // Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
- delete display;
- exit(-1);
- }
- }
-
- // double tms_1 = vpTime::measureTimeMs() ;
- unsigned niter=0 ;
- // this is the loop over the image sequence
- while (iter < opt_first + opt_nimages*opt_step) {
- try {
- double tms = vpTime::measureTimeMs() ;
-
- // set the new image name
-
- if (opt_ppath.empty()){
- s.str("");
- s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
- filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
- filename = cfilename;
- }
-
- std::cout << "read : " << filename << std::endl;
- // read the image
- vpImageIo::read(I, filename);
- if (opt_display) {
- // Display the image
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
-
- if (opt_click_blocking) {
- std::cout << "A click in the image to continue..." << std::endl;
- }
- vpImagePoint ip;
- vpMouseButton::vpMouseButtonType button;
-
- if (opt_click) {
- bool pressed = vpDisplay::getClick(I, ip, button, opt_click_blocking);
- if (pressed) {
- switch (button) {
- case vpMouseButton::button1:
- std::cout << "Left button was pressed." << std::endl;
- break;
- case vpMouseButton::button2:
- std::cout << "Middle button was pressed." << std::endl;
- break;
- case vpMouseButton::button3:
- std::cout << "Right button was pressed. Bye. " << std::endl;
- delete display;
- return 0; break;
- case vpMouseButton::none: break;
- }
- }
- }
-
- vpTime::wait(tms, 1000);
- }
-
- else {
- // Synchronise the loop to 40 ms
- vpTime::wait(tms, 40) ;
- }
- niter++ ;
- }
- catch(...) {
- delete display;
- exit(-1) ;
- }
- iter += opt_step ;
- }
- delete display;
- // double tms_2 = vpTime::measureTimeMs() ;
- // double tms_total = tms_2 - tms_1 ;
- // std::cout << "Total Time : "<< tms_total<<std::endl;
-
-}
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have X11 or GTK display functionalities...");
-}
-
-#endif
diff --git a/test/device/display/testVideoDevice.cpp b/test/device/display/testVideoDevice.cpp
deleted file mode 100644
index badda35..0000000
--- a/test/device/display/testVideoDevice.cpp
+++ /dev/null
@@ -1,485 +0,0 @@
-/****************************************************************************
- *
- * $Id: testVideoDevice.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image display.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <iostream>
-#include <string>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-
-/*!
- \example testVideoDevice.cpp
-
- \brief Image display manipulations.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:hlt:dc"
-
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
-
-void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display );
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param dtype : Type of video device.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype)
-{
- fprintf(stdout, "\n\
-Test video devices or display.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] \n\
- [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
-", name);
-
- std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
- }
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -t <type of video device> \"%s\"\n\
- String specifying the video device to use.\n\
- Possible values:\n\
- \"X11\": only on UNIX platforms,\n\
- \"GTK\": on all plaforms,\n\
- \"GDI\": only on Windows platform (Graphics Device Interface),\n\
- \"D3D\": only on Windows platform (Direct3D).\n\
- \"CV\" : (OpenCV).\n\
-\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -l\n\
- Print the list of video-devices available and exit.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), display.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param dtype : Type of display.
- \param list : To get the list of available display.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display )
-{
- const char *optarg_;
- int c;
- std::string sDisplayType;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
- // Parse the display type option
- if (sDisplayType.compare("X11") == 0) {
- dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
- dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
- dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
- dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
- dtype = vpCV;
- }
-
- break;
- case 'h': usage(argv[0], NULL, ipath,dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
-
- default:
- usage(argv[0], optarg_, ipath,dtype); return false; break;
- }
- }
-
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, dtype);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try{
- std::string env_ipath;
- std::string opt_ipath;
- bool opt_list = false; // To print the list of video devices
- vpDisplayType opt_dtype; // Type of display to use
- std::string ipath;
- std::string filename;
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Default display is one available
-#if defined VISP_HAVE_GTK
- opt_dtype = vpGTK;
-#elif defined VISP_HAVE_X11
- opt_dtype = vpX11;
-#elif defined VISP_HAVE_GDI
- opt_dtype = vpGDI;
-#elif defined VISP_HAVE_D3D9
- opt_dtype = vpD3D;
-#elif defined VISP_HAVE_OPENCV
- opt_dtype = vpCV;
-#endif
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Print the list of video-devices available
- if (opt_list) {
- unsigned nbDevices = 0;
- std::cout << "List of video-devices available: \n";
-#if defined VISP_HAVE_GTK
- std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_X11
- std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_GDI
- std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_D3D9
- std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_OPENCV
- std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
- if (!nbDevices) {
- std::cout << " No display is available\n";
- }
- return (0);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- // Create a grey level image
- vpImage<unsigned char> I ;
- // Create a color image
- vpImage<vpRGBa> Irgba ;
-
- // Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(I, filename) ;
-
- // Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Irgba, filename) ;
-
-
- // Create a display for the image
- vpDisplay *display = NULL;
-
- switch(opt_dtype) {
- case vpX11:
- std::cout << "Requested X11 display functionnalities..." << std::endl;
-#if defined VISP_HAVE_X11
- display = new vpDisplayX;
-#else
- std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGTK:
- std::cout << "Requested GTK display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GTK
- display = new vpDisplayGTK;
-#else
- std::cout << " Sorry, GTK video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGDI:
- std::cout << "Requested GDI display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GDI
- display = new vpDisplayGDI;
-#else
- std::cout << " Sorry, GDI video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpD3D:
- std::cout << "Requested D3D display functionnalities..." << std::endl;
-#if defined VISP_HAVE_D3D9
- display = new vpDisplayD3D;
-#else
- std::cout << " Sorry, D3D video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpCV:
- std::cout << "Requested OpenCV display functionnalities..." << std::endl;
-#if defined(VISP_HAVE_OPENCV)
- display = new vpDisplayOpenCV;
-#else
- std::cout << " Sorry, OpenCV video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- }
- if (opt_display){
-
- // We open a window using either X11 or GTK or GDI or D3D.
- // Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(I) ;
-
- display->close(I);
-
- // We open a window using either X11 or GTK or GDI or D3D
- // but using anothe function who doesn't take title.
- // Its size is automatically defined by the image (I) size
-
- display->init(I, 100, 100);
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(I) ;
-
- display->close(I);
-
- // We open a window using either X11 or GTK or GDI or D3D.
- // Its size is automatically defined by the image (I) size
-
- display->init(Irgba, 100, 100,"Color display...");
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(Irgba) ;
- //Flush the display
- vpDisplay::flush(Irgba) ;
-
- std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(Irgba) ;
-
- display->close(Irgba);
-
- // We open a window using either X11 or GTK or GDI or D3D
- // but using anothe function who doesn't take title.
- // Its size is automatically defined by the image (I) size
-
- display->init(Irgba, 100, 100);
-
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(Irgba) ;
- //Flush the display
- vpDisplay::flush(Irgba) ;
-
- std::cout << "A click to exit...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(Irgba) ;
- }
- delete display;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
- exit(-1);
- }
-}
-
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
-
-#endif
-
diff --git a/test/device/display/testVideoDeviceDual.cpp b/test/device/display/testVideoDeviceDual.cpp
deleted file mode 100644
index b0c765c..0000000
--- a/test/device/display/testVideoDeviceDual.cpp
+++ /dev/null
@@ -1,360 +0,0 @@
-/****************************************************************************
- *
- * $Id: testVideoDeviceDual.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image display.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpParseArgv.h>
-#include <stdlib.h>
-#include <iostream>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
-
-/*!
- \example testVideoDeviceDual.cpp
-
- \brief Example showing how to open two video devices.
-
-*/
-
-
-// List of allowed command line options
-#define GETOPTARGS "hlt:dc"
-
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
-
-void usage(const char *name, const char *badparam, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param dtype : Type of video device.
-
- */
-void usage(const char *name, const char *badparam, vpDisplayType &dtype)
-{
- fprintf(stdout, "\n\
-Test to open video devices or display.\n\
-\n\
-SYNOPSIS\n\
- %s [-t <type of video device>] [-l] [-c] [-d] [-h]\n\
-", name);
-
- std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
- }
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -t <type of video device> \"%s\"\n\
- String specifying the video device to use.\n\
- Possible values:\n\
- \"X11\": only on UNIX platforms,\n\
- \"GTK\": on all plaforms,\n\
- \"GDI\": only on Windows platform (Graphics Device Interface),\n\
- \"D3D\": only on Windows platform (Direct3D).\n\
- \"CV\" : (OpenCV).\n\
-\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -l\n\
- Print the list of video-devices available and exit.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- display.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param dtype : Type of display.
- \param list : To get the list of available display.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv,
- vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- std::string sDisplayType;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
- // Parse the display type option
- if (sDisplayType.compare("X11") == 0) {
- dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
- dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
- dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
- dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
- dtype = vpCV;
- }
-
- break;
- case 'h': usage(argv[0], NULL, dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
-
- default:
- usage(argv[0], optarg_, dtype); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, dtype);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int main(int argc, const char ** argv)
-{
- try {
- bool opt_list = false; // To print the list of video devices
- vpDisplayType opt_dtype; // Type of display to use
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Default display is one available
-#if defined VISP_HAVE_GTK
- opt_dtype = vpGTK;
-#elif defined VISP_HAVE_X11
- opt_dtype = vpX11;
-#elif defined VISP_HAVE_GDI
- opt_dtype = vpGDI;
-#elif defined VISP_HAVE_D3D9
- opt_dtype = vpD3D;
-#elif defined VISP_HAVE_OPENCV
- opt_dtype = vpCV;
-#endif
-
- // Read the command line options
- if (getOptions(argc, argv, opt_dtype, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Print the list of video-devices available
- if (opt_list) {
- unsigned nbDevices = 0;
- std::cout << "List of video-devices available: \n";
-#if defined VISP_HAVE_GTK
- std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_X11
- std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_GDI
- std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_D3D9
- std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
-#endif
-#if defined VISP_HAVE_OPENCV
- std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
- if (!nbDevices) {
- std::cout << " No display is available\n";
- }
- return (0);
- }
-
- // Create 2 images
- vpImage<unsigned char> I1(240, 320), I2(240, 320);
- I1 = 128;
- I2 = 255;
-
- // Create 2 display
- vpDisplay *d1 = NULL, *d2 = NULL;
-
- // Initialize the displays
- switch(opt_dtype) {
- case vpX11:
- std::cout << "Requested X11 display functionnalities..." << std::endl;
-#if defined VISP_HAVE_X11
- d1 = new vpDisplayX;
- d2 = new vpDisplayX;
-#else
- std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGTK:
- std::cout << "Requested GTK display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GTK
- d1 = new vpDisplayGTK;
- d2 = new vpDisplayGTK;
-#else
- std::cout << " Sorry, GTK video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpGDI:
- std::cout << "Requested GDI display functionnalities..." << std::endl;
-#if defined VISP_HAVE_GDI
- d1 = new vpDisplayGDI;
- d2 = new vpDisplayGDI;
-#else
- std::cout << " Sorry, GDI video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpD3D:
- std::cout << "Requested D3D display functionnalities..." << std::endl;
-#if defined VISP_HAVE_D3D9
- d1 = new vpDisplayD3D;
- d2 = new vpDisplayD3D;
-#else
- std::cout << " Sorry, D3D video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- case vpCV:
- std::cout << "Requested OpenCV display functionnalities..." << std::endl;
-#if defined(VISP_HAVE_OPENCV)
- d1 = new vpDisplayOpenCV;
- d2 = new vpDisplayOpenCV;
-#else
- std::cout << " Sorry, OpenCV video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
- return 0;
-#endif
- break;
- }
-
- if (opt_display){
- int winx1 = 100, winy1 = 200;
- d1->init(I1, winx1, winy1, "Display 1");
-
- int winx2 = winx1+10+(int)I1.getWidth(), winy2 = winy1;
- d2->init(I2, winx2, winy2, "Display 2");
-
- vpDisplay::display(I1);
- vpDisplay::display(I2);
-
- vpDisplay::flush(I1);
- vpDisplay::flush(I2);
- }
-
- std::cout << "A click in display 1 to exit..." << std::endl;
- if ( opt_click_allowed )
- vpDisplay::getClick(I1);
-
- delete d1;
- delete d2;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
-
-#endif
diff --git a/test/device/framegrabber/CMakeLists.txt b/test/device/framegrabber/CMakeLists.txt
deleted file mode 100644
index 9a7a6db..0000000
--- a/test/device/framegrabber/CMakeLists.txt
+++ /dev/null
@@ -1,64 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- test1394TwoResetBus.cpp
- test1394TwoGrabber.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/device/framegrabber/test1394TwoGrabber.cpp b/test/device/framegrabber/test1394TwoGrabber.cpp
deleted file mode 100644
index 77ef541..0000000
--- a/test/device/framegrabber/test1394TwoGrabber.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-
-/****************************************************************************
- *
- * $Id: test1394TwoGrabber.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file test1394TwoGrabber.cpp
-
- \brief Aquire images using libdc1394-2.x library.
-
-*/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <iostream>
-#include <string>
-
-#if defined(VISP_HAVE_DC1394_2)
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-/*!
- \example test1394TwoGrabber.cpp
-
-
-
-*/
-int
-main()
-{
- try {
- std::cout << "IEEE1394 test in progress..." << std::endl;
-
- // Get the user name
- std::string username;
- vpIoTools::getUserName(username);
- std::string outputpath = "/tmp/" + username;
- vpIoTools::makeDirectory(outputpath);
-
- // Creation of an empty image container
- vpImage<unsigned char> I;
-
- // Creation of a framegrabber
- vp1394TwoGrabber g;
- uint64_t guid;
-
- // Get the number of cameras connected on the bus
- unsigned int ncameras; // Number of cameras on the bus
- ncameras = g.getNumCameras();
- for (unsigned int i=0; i < ncameras; i++) {
- g.setCamera(i);
- g.getGuid(guid);
- std::cout << "Detected camera with GUID 0x"
- << std::hex << guid << std::endl;
- }
- // If more than one camera connected, use the last one
- if (ncameras > 1) {
- g.setCamera(ncameras -1);
- g.getGuid(guid);
- std::cout << "Use camera with GUID 0x" << std::hex << guid << std::endl;
- g.setCamera(0); // to be sure that the setCamera() in the next line with guid as parameter works
- g.setCamera(guid);
- }
-
- for (int i=0;i<10;i++) g.acquire(I);
- g.close();
- std::string filename = outputpath + "/imagetest1.pgm";
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename);
-
- std::cout << "New connection..."<< std::endl;
- g.open(I);
- g.close();
-
- std::cout << "New connection..."<< std::endl;
- g.open(I);
- //g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_800x600_YUV422);
- g.acquire(I);
- g.close();
- filename = outputpath + "/imagetest2.pgm";
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename);
- }
- catch (...) {
- vpCERROR << "Failure: exit" << std::endl;
- }
-}
-#else
-int
-main()
-{
- vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
- "You should install libdc1394-2 to use this binary.") ;
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/test/device/framegrabber/test1394TwoResetBus.cpp b/test/device/framegrabber/test1394TwoResetBus.cpp
deleted file mode 100644
index 14839c0..0000000
--- a/test/device/framegrabber/test1394TwoResetBus.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- * $Id: test1394TwoResetBus.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Firewire cameras video capture.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file test1394TwoResetBus.cpp
-
- \brief Resets the IEEE1394 bus using libdc1394-2.x library.
-
-*/
-
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <iostream>
-
-#if defined(VISP_HAVE_DC1394_2)
-
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-
-
-/*!
- \example test1394TwoResetBus.cpp
-
- Resets the IEEE1394 bus which first camera is attached to. Resetting
- the bus is "rude" to other devices because it causes them to
- re-enumerate on the bus and may cause a temporary disruption in
- their current activities. Thus, use it sparingly. Its primary use
- is if a program shuts down uncleanly and needs to free leftover ISO
- channels or bandwidth. A bus reset will free those things as a side
- effect.
-
-*/
-int
-main()
-{
- try {
- std::cout << "IEEE1394 bus reset in progress..." << std::endl;
- vp1394TwoGrabber g;
- g.resetBus(); // Reset the bus attached to the first camera found
- std::cout << "IEEE1394 bus was reset." << std::endl;
-
- vpImage<unsigned char> I;
- g.acquire(I);
- // std::cout << "write /tmp/test.pgm" << std::endl;
- // vpImageIo::write(I, "/tmp/test.pgm");
- }
- catch (...) {
- vpCERROR << "Failure: exit" << std::endl;
- }
-}
-#else
-int
-main()
-{
- vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
- "You should install libdc1394-2 to use this binary.") ;
-}
-
-#endif
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/test/feature/CMakeLists.txt b/test/feature/CMakeLists.txt
deleted file mode 100644
index 556fde3..0000000
--- a/test/feature/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testPoint.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/feature/testPoint.cpp b/test/feature/testPoint.cpp
deleted file mode 100644
index a16f39c..0000000
--- a/test/feature/testPoint.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/****************************************************************************
- *
- * $Id: testPoint.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Performs various tests on the point class.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-
-/*!
- \file testPoint.cpp
- \brief Performs various tests on the the point class.
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPoint.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpFeatureException.h>
-#include <visp/vpDebug.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Performs various tests on the point class.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
-
- vpPoint point ;
- vpTRACE("set point coordinates in the world frame ") ;
- point.setWorldCoordinates(0,0,0) ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the projection ") ;
- point.track(cMo) ;
-
- vpTRACE("coordinates in the world frame ") ;
- std::cout << point.oP.t() << std::endl ;
- vpTRACE("coordinates in the camera frame ") ;
- std::cout << point.cP.t() << std::endl ;
-
- vpTRACE("2D coordinates ") ;
- std::cout<< point.get_x() << " " << point.get_y() << std::endl ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the interaction matrix ") ;
-
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ;
-
- vpMatrix L ;
- L = p.interaction() ;
- std::cout << L << std::endl ;
-
- vpTRACE("test the interaction matrix select") ;
- vpTRACE("\t only X") ;
- L = p.interaction(vpFeaturePoint::selectX()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t only Y") ;
- L = p.interaction(vpFeaturePoint::selectY()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t X & Y") ;
- L = p.interaction(vpFeaturePoint::selectX() |
- vpFeaturePoint::selectY()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t selectAll") ;
- L = p.interaction(vpFeaturePoint::selectAll() ) ;
- std::cout << L << std::endl ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the error ") ;
-
- try{
- vpFeaturePoint pd ;
- pd.set_x(0) ;
- pd.set_y(0) ;
-
- pd.print() ; std::cout << std::endl ;
- vpColVector e ;
- e = p.error(pd) ;
- std::cout << e << std::endl ;
-
- vpTRACE("test the interaction matrix select") ;
- vpTRACE("\t only X") ;
- e = p.error(pd,vpFeaturePoint::selectX()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t only Y") ;
- e = p.error(pd,vpFeaturePoint::selectY()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t X & Y") ;
- e = p.error(pd,vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t selectAll") ;
- e = p.error(pd,vpFeaturePoint::selectAll() ) ;
- std::cout << e << std::endl ;
- }
- catch(vpFeatureException me){ std::cout << me << std::endl ; }
- catch(vpException me){ std::cout << me << std::endl ; }
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the dimension") ;
- unsigned int dim ;
- dim = p.getDimension() ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("test the dimension with select") ;
- vpTRACE("\t only X") ;
- dim = p.getDimension(vpFeaturePoint::selectX()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t only Y") ;
- dim = p.getDimension(vpFeaturePoint::selectY()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t X & Y") ;
- dim = p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t selectAll") ;
- dim = p.getDimension(vpFeaturePoint::selectAll() ) ;
- std::cout << "Dimension = " << dim << std::endl ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/homography/CMakeLists.txt b/test/homography/CMakeLists.txt
deleted file mode 100644
index 7382083..0000000
--- a/test/homography/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testDisplacement.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/homography/testDisplacement.cpp b/test/homography/testDisplacement.cpp
deleted file mode 100644
index 1f9c5ab..0000000
--- a/test/homography/testDisplacement.cpp
+++ /dev/null
@@ -1,237 +0,0 @@
-/****************************************************************************
- *
- * $Id: testDisplacement.cpp 4833 2014-08-28 14:21:46Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tests transformation within various representations of rotation.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-
-/*!
- \file testDisplacement.cpp
- \brief Tests transformation within various representations of rotation
-*/
-
-/*!
- \example testDisplacement.cpp
-
-
-*/
-
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomography.h>
-#include <visp/vpDebug.h>
-#include <visp/vpThetaUVector.h>
-#include <visp/vpParseArgv.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Tests transformation within various representations of rotation.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- {
- vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
-
- std::cout << "Initialization " <<std::endl ;
- // std::cout << tu << std::endl ;
-
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- vpRotationMatrix R(tu) ;
-
- // pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
-
- std::cout << "M" <<std::endl <<M << std::endl ;
- vpPlane p(0,0,1,1) ;
-
- vpHomography H(M,p) ;
-
- std::cout << "H" <<std::endl <<H << std::endl ;
-
- vpColVector n ;
- vpTranslationVector T ;
-
- H.computeDisplacement(R,T,n) ;
-
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
- }
- std::cout <<"------------------------------------------------------" << std::endl ;
-
- {
- vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
-
- std::cout << "Initialization " << std::endl ;
- // std::cout << tu << std::endl ;
-
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- vpRotationMatrix R(tu) ;
-
- // pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
-
- M[0][3] = 0.21 ;
- M[1][3] = 0.31 ;
- M[2][3] = 0.5 ;
-
- std::cout << "M" << std::endl << M << std::endl ;
- vpPlane p(0,0,1,1) ;
-
- vpHomography H(M,p) ;
-
- std::cout << "H" << std::endl << H << std::endl ;
-
- vpColVector n ;
- vpTranslationVector T ;
-
- H.computeDisplacement(R,T,n) ;
-
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
- }
-
- std::cout <<"------------------------------------------------------" << std::endl ;
- {
- vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45)) ;
-
- vpRotationMatrix R(tu) ;
-
- // pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
-
- M[0][3] = 0.21 ;
- M[1][3] = -0.31 ;
- M[2][3] = 0.5 ;
-
- std::cout << "M" << std::endl << M << std::endl ;
- vpPlane p(0.4,-0.5,0.5,1) ;
-
- vpHomography H(M,p) ;
-
- std::cout << "H" << std::endl << H << std::endl ;
-
- vpColVector n ;
- vpTranslationVector T ;
- H.computeDisplacement(R,T,n) ;
-
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
-
- vpPlane p1(n[0],n[1],n[2],1.0) ;
- H.buildFrom(R,T,p1) ;
- std::cout << "H" << std::endl << H << std::endl ;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/image/CMakeLists.txt b/test/image/CMakeLists.txt
deleted file mode 100644
index 3ac6c10..0000000
--- a/test/image/CMakeLists.txt
+++ /dev/null
@@ -1,81 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testConversion.cpp
- testCreateSubImage.cpp
- testImagePoint.cpp
- testIoPGM.cpp
- testIoPPM.cpp
- testUndistortImage.cpp
- testReadImage.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-# Add test
-# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
-# environment variable to the ViSP test sequences location.
-# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
-add_test(testConversion testConversion)
-add_test(testCreateSubImage testCreateSubImage)
-add_test(testImagePoint testImagePoint)
-add_test(testIoPGM testIoPGM)
-add_test(testIoPPM testIoPPM)
-add_test(testReadImage testReadImage)
diff --git a/test/image/testConversion.cpp b/test/image/testConversion.cpp
deleted file mode 100644
index 4fe6cfd..0000000
--- a/test/image/testConversion.cpp
+++ /dev/null
@@ -1,564 +0,0 @@
-/****************************************************************************
- *
- * $Id: testConversion.cpp 5023 2014-12-03 16:07:48Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image conversions.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <stdlib.h>
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <visp/vpTime.h>
-
-
-/*!
- \example testConversion.cpp
-
- \brief Manipulation of image conversions.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
-{
- fprintf(stdout, "\n\
-Test image conversions.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-o <output image path>]\n\
- [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -o <output image path> %s\n\
- Set image output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- Klimt_grey.pgm and Klimt_color.ppm output images\n\
- are written.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_opath;
- std::string ipath;
- std::string opath;
- std::string filename;
- std::string username;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Set the default output path
-#if defined(_WIN32)
- opt_opath = "C:/temp";
-#else
- opt_opath = "/tmp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- opath = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << opath << std::endl;
- std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (opt_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- //
- // Here starts really the test
- //
-
- vpImage<unsigned char> Ig ; // Grey image
- vpImage<vpRGBa> Ic ; // Color image
-
- //-------------------- .pgm -> .ppm
- vpTRACE("Convert a grey image (.pgm) to a color image (.ppm)");
- // Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ig, filename) ;
- // Create a color image from the grey
- vpImageConvert::convert(Ig, Ic);
- filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
- vpCTRACE << "Write " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
-
- //-------------------- .ppm -> .pgm
- vpTRACE("Convert a color image (.ppm) to a grey image (.pgm)");
- // Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
- // Create a grey image from the color
- vpImageConvert::convert(Ic, Ig);
- filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
- vpCTRACE << "Write " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
-
- //-------------------- YUV -> RGB
- unsigned char y=187, u=10, v=30;
- unsigned char r, g, b;
-
- // Convert a YUV pixel value to a RGB value
- vpImageConvert::YUVToRGB(y, u, v, r, g, b);
- vpTRACE("y(%d) u(%d) v(%d) = r(%d) g(%d) b(%d)", y, u, v, r, g, b);
-
-#ifdef VISP_HAVE_OPENCV
-#if VISP_HAVE_OPENCV_VERSION < 0x020408
- double t0 = vpTime::measureTimeMs();
- /////////////////////////
- // Convert a IplImage to a vpImage<vpRGBa>
- ////////////////////////
- IplImage* image = NULL; /*!< The image read / acquired */
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- vpCTRACE << "Reading the color image with opencv: "<< std::endl
- << filename << std::endl;
- if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) {
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpImageConvert::convert(image, Ic);
- filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.ppm");
- /* Save the the current image */
- vpImageIo::write(Ic, filename) ;
-
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
-
- /* Read the pgm image */
-
- vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
- << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
- if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpImageConvert::convert(image, Ic);
- filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.ppm");
- /* Save the the current image */
- vpImageIo::write(Ic, filename) ;
-
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ///////////////////////////
- // Convert a IplImage to a vpImage<unsigned char>
- ////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- vpCTRACE << "Reading the color image with opencv: "<< std::endl
- << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
- if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) {
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpImageConvert::convert(image, Ig);
- filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.pgm");
- /* Save the the current image */
- vpImageIo::write(Ig, filename) ;
-
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
-
- /* Read the pgm image */
-
- vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
- << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
- if((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
-
- return (-1);
- }
- vpImageConvert::convert(image, Ig);
- filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.pgm");
- /* Save the the current image */
- vpImageIo::write(Ig, filename) ;
-
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ////////////////////////////////////
- // Convert a vpImage<vpRGBa> to a IplImage
- ////////////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- // Load a color image from the disk
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
- vpImageConvert::convert(Ic, image);
- filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cv.ppm");
- /* Save the the current image */
- vpCTRACE << "Write " << filename << std::endl;
- if((cvSaveImage(filename.c_str(), image)) == 0) {
- vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
- return (-1);
- }
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ////////////////////////////////////////
- // Convert a IplImage to a vpImage<unsigned char>
- ////////////////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
-
- /* Read the grey image */
-
- // Load a color image from the disk
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ig, filename) ;
- vpImageConvert::convert(Ig, image);
- filename = vpIoTools::createFilePath(opath, "Klimt_ipl_grey_cv.pgm");
- /* Save the the current image */
-
- vpCTRACE << "Write " << filename << std::endl;
- if((cvSaveImage(filename.c_str(), image)) == 0) {
- vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
- return (-1);
- }
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- if(image!=NULL) cvReleaseImage( &image );
- double t1 = vpTime::measureTimeMs();
- std::cout << "Conversion c interface : " << t1 - t0 << " ms" << std::endl;
-#endif
-
- /* ------------------------------------------------------------------------ */
- /* conversion for the new c++ interface */
- /* ------------------------------------------------------------------------ */
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020100
- double t2 = vpTime::measureTimeMs();
- /////////////////////////
- // Convert a cv::Mat to a vpImage<vpRGBa>
- ////////////////////////
- cv::Mat imageMat;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpCTRACE << "Reading the color image with c++ interface of opencv: "<< std::endl
- << filename << std::endl;
- imageMat = cv::imread(filename, 1);// force to a three channel color image.
- if(imageMat.data == NULL){
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return -1;
- }
- vpImageConvert::convert(imageMat, Ic);
- filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.ppm");
- /* Save the the current image */
- vpImageIo::write(Ic, filename) ;
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- /* Read the pgm image */
-
- vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
- << filename << std::endl;
- imageMat = cv::imread(filename, 0);// forced to grayscale.
- if(imageMat.data == NULL) {
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpImageConvert::convert(imageMat, Ic);
- filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.ppm");
- /* Save the the current image */
- vpImageIo::write(Ic, filename) ;
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ///////////////////////////
- // Convert a cv::Mat to a vpImage<unsigned char>
- ////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- vpCTRACE << "Reading the color image with opencv: "<< std::endl
- << filename << std::endl;
- imageMat = cv::imread(filename, 1);// force to a three channel color image.
- if(imageMat.data == NULL){
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return -1;
- }
- vpImageConvert::convert(imageMat, Ig);
- filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.pgm");
- /* Save the the current image */
- vpImageIo::write(Ig, filename) ;
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
-
- /* Read the pgm image */
-
- vpCTRACE << "Reading the greyscale image with opencv: "<< std::endl
- << filename << std::endl;
- imageMat = cv::imread(filename, 0);
- if(imageMat.data == NULL){
- vpCTRACE<<"Cannot read image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpImageConvert::convert(imageMat, Ig);
- filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.pgm");
- /* Save the the current image */
- vpImageIo::write(Ig, filename) ;
-
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ////////////////////////////////////
- // Convert a vpImage<vpRGBa> to a cv::Mat
- ////////////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- // Load a color image from the disk
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
- vpImageConvert::convert(Ic, imageMat);
- filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cvMat.ppm");
- /* Save the the current image */
- vpCTRACE << "Write " << filename << std::endl;
- if(!cv::imwrite(filename, imageMat)){
- vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- ////////////////////////////////////////
- // Convert a IplImage to a vpImage<unsigned char>
- ////////////////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
-
- /* Read the grey image */
-
- // Load a color image from the disk
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ig, filename);
- vpImageConvert::convert(Ig, imageMat);
- filename = vpIoTools::createFilePath(opath, "Klimt_ipl_grey_cvMat.pgm");
- /* Save the the current image */
-
- vpCTRACE << "Write " << filename << std::endl;
- if(!cv::imwrite(filename, imageMat)){
- vpCTRACE<<"Cannot write image: "<< std::endl << filename << std::endl;
- return (-1);
- }
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
- double t3 = vpTime::measureTimeMs();
- std::cout << "Conversion c++ interface : " << t3 - t2 << " ms" << std::endl;
-#endif
-#endif
-
- ////////////////////////////////////
- // Split a vpImage<vpRGBa> to vpImage<unsigned char>
- ////////////////////////////////////
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-
- /* Read the color image */
-
- // Load a color image from the disk
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
- vpImage<unsigned char> R,G,B,a;
- vpImageConvert::split(Ic, &R,NULL,&B);
- double begintime = vpTime::measureTimeMs();
- for(int i=0; i<1000;i++){
- vpImageConvert::split(Ic, &R,NULL,&B);
- }
- double endtime = vpTime::measureTimeMs();
-
- std::cout<<"Time for 1000 split (ms): "<< endtime - begintime <<std::endl;
-
- filename = vpIoTools::createFilePath(opath, "Klimt_RChannel.pgm");
- /* Save the the current image */
- vpCTRACE << "Write " << filename << std::endl;
- vpImageIo::write(R, filename) ;
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
-
- filename = vpIoTools::createFilePath(opath, "Klimt_BChannel.pgm");
- /* Save the the current image */
- vpCTRACE << "Write " << filename << std::endl;
- vpImageIo::write(B, filename) ;
- vpCTRACE<< "Convert result in "<<std::endl<< filename << std::endl;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/image/testCreateSubImage.cpp b/test/image/testCreateSubImage.cpp
deleted file mode 100644
index 9f33448..0000000
--- a/test/image/testCreateSubImage.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-/****************************************************************************
- *
- * $Id: testCreateSubImage.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for sub-image extraction.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpRect.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-
-/*!
- \example testCreateSubImage.cpp
-
- \brief Create a sub-image from an image by cropping a rectangular area.
-
- Read an image from the disk, crop a rectangular area and save the
- cropped image on the disk.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
-{
- fprintf(stdout, "\n\
-Read an image from the disk (Klimt.pgm), crop a rectangular area\n\
-and save the cropped image on the disk (Klimt_cropped.pgm).\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-o <output image path>]\n\
- [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- image.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -o <output image path> %s\n\
- Set image output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- Klimt_cropped.pgm output image is written.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_opath;
- std::string ipath;
- std::string opath;
- std::string filename;
- std::string username;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Set the default output path
-#if defined(_WIN32)
- opt_opath = "C:/temp";
-#else
- opt_opath = "/tmp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- opath = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << opath << std::endl;
- std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (opt_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- //
- // Here starts really the test
- //
- vpImage<unsigned char> I; // Input image
- vpImage<unsigned char> C; // Cropped output image
-
- // Read the input grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
-
- // Specify the cropping area
- vpRect crop;
- crop.setLeft(-10.2);
- crop.setTop(10.0);
- crop.setWidth(I.getWidth());
- crop.setHeight(20.0);
-
- // Create the cropped image
- vpImageTools::createSubImage(I, crop, C);
-
- // Write the cropped image on the disk
- filename = vpIoTools::createFilePath(opath, "Klimt_crop.pgm");
- std::cout << "Write cropped image: " << filename << std::endl;
- vpImageIo::write(C, filename) ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/image/testImagePoint.cpp b/test/image/testImagePoint.cpp
deleted file mode 100644
index d21b640..0000000
--- a/test/image/testImagePoint.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- * $Id: testImagePoint.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for vpImagePoint class.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-/*!
- \example testImagePoint.cpp
-
- \brief Test vpImagePoint functionalities.
-
-*/
-
-#include <visp/vpImagePoint.h>
-#include <iostream>
-
-int main()
-{
- vpImagePoint ip1, ip2, ip3;
-
- ip1.set_u(-11.1);
- ip1.set_v(10);
-
- ip2.set_j(-11.1);
- ip2.set_i(10);
-
- ip3.set_j(11.10001);
- ip3.set_i(10.1);
-
- std::cout << "We define ip1 with coordinates: "
- << ip1 << std::endl;
-
-
- std::cout << "We define ip2 with coordinates: "
- << ip2 << std::endl;
-
- std::cout << "We define ip3 with coordinates: "
- << ip3 << std::endl;
-
- if (ip1 == ip2) {
- std::cout << "ip1 == ip2" << std::endl;
- }
- else {
- std::cout << "ip1 != ip2 (bad result)" << std::endl;
- return -1;
- }
-
- if (ip1 != ip2) {
- std::cout << "ip1 != ip2 (bad result)" << std::endl;
- return -1;
- }
- else {
- std::cout << "ip1 == ip2" << std::endl;
- }
-
- if (ip1 == ip3) {
- std::cout << "ip1 == ip3 (bad result)" << std::endl;
- return -1;
- }
- else {
- std::cout << "ip1 != ip3" << std::endl;
- }
-
- if (ip1 != ip3) {
- std::cout << "ip1 != ip3" << std::endl;
- }
- else {
- std::cout << "ip1 == ip3 (bad result)" << std::endl;
- return -1;
- }
-
-
- return 0;
-}
diff --git a/test/image/testIoPGM.cpp b/test/image/testIoPGM.cpp
deleted file mode 100644
index 74a9205..0000000
--- a/test/image/testIoPGM.cpp
+++ /dev/null
@@ -1,273 +0,0 @@
-/****************************************************************************
- *
- * $Id: testIoPGM.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read and write PGM images on the disk.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-/*!
- \example testIoPGM.cpp
-
- \brief Read and write PGM images on the disk. Also test exceptions.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
-{
- fprintf(stdout, "\n\
-Read and write PGM images on the disk. Also test exceptions.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-o <output image path>]\n\
- [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- image.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -o <output image path> %s\n\
- Set image output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- Klimt_grey.pgm output image is written.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_opath;
- std::string ipath;
- std::string opath;
- std::string filename;
- std::string username;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Set the default output path
-#if defined(_WIN32)
- opt_opath = "C:/temp";
-#else
- opt_opath = "/tmp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- opath = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << opath << std::endl;
- std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- //
- // Here starts really the test
- //
-
- // Create a grey level image
- vpImage<unsigned char> I ;
-
- // Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename);
- // Write the content of the image on the disk
- filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
-
- try {
- // Try to load a non existing image (test for exceptions)
- // Load a non existing grey image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.pgm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(vpException e) {
- std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
- }
-
- try {
- // Try to write an image to a non existing directory
- filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.pgm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
- }
- catch(vpException e) {
- std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/image/testIoPPM.cpp b/test/image/testIoPPM.cpp
deleted file mode 100644
index 35ea8be..0000000
--- a/test/image/testIoPPM.cpp
+++ /dev/null
@@ -1,314 +0,0 @@
-/****************************************************************************
- *
- * $Id: testIoPPM.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read and write PGM images on the disk.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-/*!
- \example testIoPPM.cpp
-
- \brief Read and write PPM images on the disk. Also test exceptions.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
-{
- fprintf(stdout, "\n\
-Read and write PPM images on the disk. Also test exceptions.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-o <output image path>]\n\
- [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -o <output image path> %s\n\
- Set image output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- Klimt_grey.ppm and Klimt_color.ppm output image\n\
- are written.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_opath;
- std::string ipath;
- std::string opath;
- std::string filename;
- std::string username;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Set the default output path
-#if defined(_WIN32)
- opt_opath = "C:/temp";
-#else
- opt_opath = "/tmp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- opath = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << opath << std::endl;
- std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- //
- // Here starts really the test
- //
-
- /////////////////////////////////////////////////////////////////////
- // Create a grey level image
- vpImage<unsigned char> I ;
-
- // Load a color image from the disk and convert it to a grey level one
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename);
- // Write the content of the image on the disk
- filename = vpIoTools::createFilePath(opath, "Klimt_grey.ppm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
-
- // Try to load a non existing image (test for exceptions)
- try
- {
- // Load a non existing grey image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(vpImageException e)
- {
- vpERROR_TRACE("at main level");
- std::cout << e << std::endl ;
- }
-
- // Try to write an image to a non existing directory
- try {
- filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
- }
- catch(vpException e) {
- std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
- }
-
- /////////////////////////////////////////////////////////////////////
- // Create a color image
- vpImage<vpRGBa> Irgba ;
-
- // Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(Irgba, filename);
- // Write the content of the color image on the disk
- filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(Irgba, filename) ;
-
- try {
- // Try to load a non existing image (test for exceptions)
- // Load a non existing color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(Irgba, filename) ;
- }
- catch(vpException e) {
- std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
- }
-
- try {
- // Try to write a color image to a non existing directory
- filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
- std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(Irgba, filename) ;
- }
- catch(vpException e) {
- std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/image/testReadImage.cpp b/test/image/testReadImage.cpp
deleted file mode 100644
index 68cae29..0000000
--- a/test/image/testReadImage.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/****************************************************************************
- *
- * $Id: testReadImage.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Read images on the disk.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-
-/*!
- \example testReadImage.cpp
-
- \brief Read images on the disk.
-
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "i:p:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string ppath);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Personal image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string ppath)
-{
- fprintf(stdout, "\n\
-Read images on the disk.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-p <personal image path>]\n\
- [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm,\n\
- .ppm, .jpeg and .png images.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -p <personal image path> %s\n\
- Set personal image path.\n\
- From this path read the image \"%s\".\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), ppath.c_str(), ppath.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param ppath : Personal image path.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, ppath); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, ppath); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, ppath);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_ppath;
- std::string ipath;
- std::string ppath;
- std::string filename;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_ppath.empty())
- ppath = opt_ppath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
-
- //
- // Here starts really the test
- //
-
- /////////////////////////////////////////////////////////////////////
- // Create a grey level image
- //vpImage<vpRGBa> I;
- vpImage<unsigned char> I;
- vpImage<vpRGBa> Irgb;
-
- if (opt_ppath.empty())
- {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(I,filename);
- printf("Read ppm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I,filename);
- printf("Read pgm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
- vpImageIo::read(I,filename);
- printf("Read jpeg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpg");
- vpImageIo::read(I,filename);
- printf("Read jpg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
- vpImageIo::read(I,filename);
- printf("Read png ok\n");
-
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgb,filename);
- printf("Read ppm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(Irgb,filename);
- printf("Read pgm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
- vpImageIo::read(Irgb,filename);
- printf("Read jpeg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpg");
- vpImageIo::read(Irgb,filename);
- printf("Read jpg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
- vpImageIo::read(Irgb,filename);
- printf("Read png ok\n");
- }
- else
- {
- filename = opt_ppath;
- vpImageIo::read(I,filename);
- vpTRACE("image read without problem");
- }
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
diff --git a/test/image/testUndistortImage.cpp b/test/image/testUndistortImage.cpp
deleted file mode 100644
index a29ce28..0000000
--- a/test/image/testUndistortImage.cpp
+++ /dev/null
@@ -1,291 +0,0 @@
-/****************************************************************************
- *
- * $Id: testUndistortImage.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for image undistortion.
- *
- * Authors:
- * Anthony Saunier
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpImage.h>
-#include <visp/vpRGBa.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpDebug.h>
-#include <visp/vpTime.h>
-#include <stdlib.h>
-/*!
- \example testUndistortImage.cpp
-
- \brief Undistort an image.
-
- Read an image from the disk, undistort it and save the
- undistorted image on the disk.
-
- */
-
-// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-//#define COLOR
-#define BW
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
-/*
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
-
- */
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
-{
- fprintf(stdout, "\n\
-Read an image from the disk, undistort it \n\
-and save the undistorted image on the disk.\n\
-(Klimt_undistorted.pgm).\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-o <output image path>]\n\
- [-h]\n\
- ", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- image.\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
- \n\
- -o <output image path> %s\n\
- Set image output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- Klimt_undistorted.pgm output image is written.\n\
-\n\
- -h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param opath : Output image path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-
- */
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-int main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string opt_opath;
- std::string ipath;
- std::string opath;
- std::string filename;
- std::string username;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
- // Set the default output path
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- opt_opath = "/tmp";
-#elif defined(_WIN32)
- opt_opath = "C:\\temp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- opath = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << opath << std::endl;
- std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (opt_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
- //
- // Here starts really the test
- //
-#if defined BW
- vpImage<unsigned char> I; // Input image
- vpImage<unsigned char> U; // undistorted output image
-#elif defined COLOR
- vpImage<vpRGBa> I; // Input image
- vpImage<vpRGBa> U; // undistorted output image
-#endif
- vpCameraParameters cam;
- cam.initPersProjWithDistortion(600,600,192,144,-0.17,0.17);
- // Read the input grey image from the disk
-#if defined BW
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm") ;
-#elif defined COLOR
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
-#endif
- std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
-
- std::cout << "Undistortion in process... " << std::endl;
- vpImageTools::undistort(I, cam, U);
-
- double begintime = vpTime::measureTimeMs();
-
- // For the test, to have a significant time measure we repeat the
- // undistortion 100 times
- for(unsigned int i=0;i<100;i++)
- // Create the undistorted image
- vpImageTools::undistort(I, cam, U);
-
- double endtime = vpTime::measureTimeMs();
-
- std::cout<<"Time for 100 undistortion (ms): "<< endtime - begintime
- << std::endl;
-
- // Write the undistorted image on the disk
-#if defined BW
- filename = vpIoTools::path( vpIoTools::createFilePath(opath, "Klimt_undistorted.pgm") );
-#elif defined COLOR
- filename = vpIoTools::path( vpIoTools::createFilePath(opath, "Klimt_undistorted.ppm") );
-#endif
- std::cout << "Write undistorted image: " << filename << std::endl;
- vpImageIo::write(U, filename) ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/key-point/CMakeLists.txt b/test/key-point/CMakeLists.txt
deleted file mode 100644
index 6f8bf19..0000000
--- a/test/key-point/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testSurfKeyPoint.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-# Add test
-add_test(testSurfKeyPoint testSurfKeyPoint -c ${OPTION_TO_DESACTIVE_DISPLAY})
diff --git a/test/key-point/testSurfKeyPoint.cpp b/test/key-point/testSurfKeyPoint.cpp
deleted file mode 100644
index 502112a..0000000
--- a/test/key-point/testSurfKeyPoint.cpp
+++ /dev/null
@@ -1,386 +0,0 @@
-/****************************************************************************
- *
- * $Id: testSurfKeyPoint.cpp 5202 2015-01-24 09:29:06Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test auto detection of dots.
- *
- * Authors:
- * Nicolas Melchior
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
-#include <iomanip>
-#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) // Require opencv >= 1.1.0 < 3.0.0
-
-#include <visp/vpKeyPointSurf.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-
-
-#include <visp/vpCameraParameters.h>
-
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-/*!
- \example testSurfKeyPoint.cpp
-
- \brief Test matching points thanks to the surf key points.
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "cdi:h"
-
-void usage(const char *name, const char *badparam, std::string ipath);
-bool getOptions(int argc, const char **argv, std::string &ipath,
- bool &click_allowed, bool &display);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
-
-*/
-void usage(const char *name, const char *badparam, std::string ipath)
-{
- fprintf(stdout, "\n\
-Test dot tracking.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-c] [-d] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read image \n\
- \"ViSP-images/ellipse/ellipse.pgm\"\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -h\n\
- Print the help.\n",
- ipath.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
-}
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath : Input image path.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath,
- bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string ipath;
- std::string dirname;
- std::string filenameRef;
- std::string filenameCur;
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
-
- // 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<unsigned char> Iref ;
- vpImage<unsigned char> Icur ;
-
- // Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
-
- // Build the name of the image file
- filenameRef = vpIoTools::createFilePath(dirname, "image.0000.pgm");
- filenameCur = vpIoTools::createFilePath(dirname, "image.0079.pgm");
-
- // Read the PGM image named "filename" on the disk, and put the
- // bitmap into the image structure I. I is initialized to the
- // correct size
- //
- // exception readPGM may throw various exception if, for example,
- // the file does not exist, or if the memory cannot be allocated
- try{
- vpCTRACE << "Load: " << filenameRef << std::endl;
-
- vpImageIo::read(Iref, filenameRef) ;
-
- vpCTRACE << "Load: " << filenameCur << std::endl;
-
- vpImageIo::read(Icur, filenameCur) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot read " << filenameRef << "or" << filenameCur <<std::endl;
- std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
- exit(-1);
- }
-
- // We open a window using either X11, GTK or GDI.
-#if defined VISP_HAVE_X11
- vpDisplayX display[2];
-#elif defined VISP_HAVE_GTK
- vpDisplayGTK display[2];
-#elif defined VISP_HAVE_GDI
- vpDisplayGDI display[2];
-#endif
-
- if (opt_display) {
- // Display size is automatically defined by the image (I) size
- display[0].init(Iref, 100, 100, "Reference image") ;
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(Iref) ;
- //Flush the display
- vpDisplay::flush(Iref) ;
- }
-
- vpKeyPointSurf surf;
- unsigned int nbrRef;
-
- if (opt_click_allowed && opt_display)
- {
- std::cout << "Select a part of the image where the reference points will be computed. This part is a rectangle." << std::endl;
- std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Iref, corners[i]);
- }
-
- vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::red);
- vpDisplay::flush(Iref);
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
-
- //Computes the reference points
- nbrRef = surf.buildReference(Iref, corners[0], height, width);
- }
-
- else
- {
- nbrRef = surf.buildReference(Iref);
- }
-
- if(nbrRef < 1)
- {
- vpTRACE("No reference point");
- exit(-1);
- }
-
-
- unsigned int nbrPair;
- if (opt_display) {
- display[1].init(Icur, (int)(100+Iref.getWidth()), 100, "Current image") ;
- // display variable.
- vpDisplay::display(Icur) ;
- //Flush the display
- vpDisplay::flush(Icur) ;
- }
-
- if (opt_click_allowed && opt_display)
- {
- std::cout << "Select a part of the current image where the reference will be search. This part is a rectangle." << std::endl;
- std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
- vpDisplay::getClick(Icur, corners[i]);
- }
- vpDisplay::displayRectangle(Icur, corners[0], corners[1], vpColor::green);
- vpDisplay::flush(Icur);
- unsigned int height, width;
- height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
- width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
-
- //Computes the reference points
- nbrPair = surf.matchPoint(Icur, corners[0], height, width);
- }
-
- else
- {
- nbrPair = surf.matchPoint(Icur);
- }
-
- if(nbrPair < 1)
- {
- vpTRACE("No point matched");
- }
-
- if (opt_display)
- {
- surf.display(Iref, Icur, 7);
- vpDisplay::flush(Iref) ;
- vpDisplay::flush(Icur) ;
- if (opt_click_allowed)
- {
- std::cout << "A click on the reference image to exit..." << std::endl;
- vpDisplay::getClick(Iref);
- }
- }
- return (0);
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return (1);
- }
-}
-#else
-int
-main()
-{
-#if ( ! (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) )
- vpERROR_TRACE("You do not have X11, GTK or GDI display functionalities...");
-#else
- vpERROR_TRACE("You do not have 1.1.0 <= OpenCV < 2.4.8 that contains opencv_nonfree component...");
-#endif
-}
-
-#endif
diff --git a/test/math/CMakeLists.txt b/test/math/CMakeLists.txt
deleted file mode 100644
index 0ad8f84..0000000
--- a/test/math/CMakeLists.txt
+++ /dev/null
@@ -1,75 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testColvector.cpp
- testKalmanAcceleration.cpp
- testKalmanVelocity.cpp
- testMatrix.cpp
- testMatrixException.cpp
- testMatrixInverse.cpp
- testRobust.cpp
- testRotation.cpp
- testSvd.cpp
- testTwistMatrix.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/math/testColvector.cpp b/test/math/testColvector.cpp
deleted file mode 100644
index 64ccecf..0000000
--- a/test/math/testColvector.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/****************************************************************************
- *
- * $Id: testColvector.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test some vpColVector functionalities.
- *
- * Authors:
- * Eric Marchand
- *
- *****************************************************************************/
-
-/*!
- \example testColvector.cpp
-
- Test some vpColVector functionalities.
-*/
-
-#include <visp/vpColVector.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test some vpColVector functionalities.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpColVector V(4) ;
- V = 1.0;
-
- vpTRACE("------------------------");
- vpTRACE("call std::cout << V;");
- std::cout << V << std::endl;
-
- vpTRACE("------------------------");
- vpTRACE("call V.normalize();");
- V.normalize();
-
- vpTRACE("------------------------");
- vpTRACE("call std::cout << V;");
- std::cout << V << std::endl;
- return (0);
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return (1);
- }
-}
diff --git a/test/math/testKalmanAcceleration.cpp b/test/math/testKalmanAcceleration.cpp
deleted file mode 100644
index e3ed84c..0000000
--- a/test/math/testKalmanAcceleration.cpp
+++ /dev/null
@@ -1,122 +0,0 @@
-/****************************************************************************
- *
- * $Id: testKalmanAcceleration.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tests some vpLinearKalmanFilterInstantiation functionalities.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testKalmanAcceleration.cpp
-
- \brief Test some vpLinearKalmanFilterInstantiation functionalities
- with constant acceleration state model.
-*/
-
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-#include <iostream>
-#include <fstream>
-
-int
-main()
-{
- try {
- unsigned int nsignal = 1; // Number of signal to filter
- unsigned int niter = 100;
-
- std::string filename = "/tmp/log.dat";
- std::ofstream flog(filename.c_str());
-
- vpLinearKalmanFilterInstantiation kalman;
-
- vpLinearKalmanFilterInstantiation::vpStateModel model;
- model = vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel;
- kalman.setStateModel(model);
-
- unsigned int size_state_vector = kalman.getStateSize()*nsignal;
- unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
-
- vpColVector sigma_measure(size_measure_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++)
- sigma_measure = 0.0001;
- vpColVector sigma_state(size_state_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[3*signal] = 0.; // not used
- sigma_state[3*signal+1] = 0.000001;
- sigma_state[3*signal+2] = 0.000001;
- }
-
- vpColVector velocity_measure(size_measure_vector);
-
- double rho = 0.9; // correlation
- double dt = 0.2; // sampling period
-
- for (unsigned int signal=0; signal < nsignal; signal ++)
- velocity_measure[signal] = 3+2*signal;
-
- kalman.verbose(false);
- kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
-
-
- for (unsigned int iter=0; iter <= niter; iter++) {
- std::cout << "-------- iter " << iter << " ------------" << std::endl;
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- velocity_measure[signal] = 3+2*signal
- + 0.3*sin(vpMath::rad(360./niter*iter));
- }
- std::cout << "measure : " << velocity_measure.t() << std::endl;
-
- flog << velocity_measure.t();
-
- // kalman.prediction();
- kalman.filter(velocity_measure);
- flog << kalman.Xest.t();
- flog << kalman.Xpre.t();
-
- std::cout << "Xest: " << kalman.Xest.t() << std::endl;
- std::cout << "Xpre: " << kalman.Xpre.t() << std::endl;
-
- flog << std::endl;
- }
-
- flog.close();
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 0;
- }
-}
diff --git a/test/math/testKalmanVelocity.cpp b/test/math/testKalmanVelocity.cpp
deleted file mode 100644
index 25f5b7c..0000000
--- a/test/math/testKalmanVelocity.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-/****************************************************************************
- *
- * $Id: testKalmanVelocity.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tests some vpLinearKalmanFilterInstantiation functionalities.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testKalmanVelocity.cpp
-
- \brief Test some vpLinearKalmanFilterInstantiation functionalities
- with constant velocity state model.
-*/
-
-#include <visp/vpLinearKalmanFilterInstantiation.h>
-#include <iostream>
-#include <fstream>
-
-typedef enum {
- Position, // Considered measures are the succesive positions of the target
- Velocity // Considered measures are the succesive velocities of the target
-} vpMeasureType;
-
-int
-main()
-{
- try {
- unsigned int nsignal = 2; // Number of signal to filter
- unsigned int niter = 200;
- unsigned int size_state_vector = 2*nsignal;
- unsigned int size_measure_vector = 1*nsignal;
- //vpMeasureType measure_t = Velocity;
- vpMeasureType measure_t = Position;
-
- std::string filename = "/tmp/log.dat";
- std::ofstream flog(filename.c_str());
-
- vpLinearKalmanFilterInstantiation kalman;
-
- vpColVector sigma_measure(size_measure_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++)
- sigma_measure = 0.000001;
- vpColVector sigma_state(size_state_vector);
-
- switch (measure_t) {
- case Velocity:
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[2*signal] = 0.; // not used
- sigma_state[2*signal+1] = 0.000001;
- }
- break;
- case Position:
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[2*signal] = 0.000001;
- sigma_state[2*signal+1] = 0; // not used
- }
- break;
- }
-
- vpColVector measure(size_measure_vector);
-
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- measure[signal] = 3+2*signal;
- }
-
- kalman.verbose(true);
-
- vpLinearKalmanFilterInstantiation::vpStateModel model;
- double dt = 0.04; // Sampling period
- double rho = 0.5;
- double dummy = 0; // non used parameter
- switch (measure_t) {
- case Velocity:
- model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
- kalman.setStateModel(model);
- kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
- break;
- case Position:
- model = vpLinearKalmanFilterInstantiation::stateConstVel_MeasurePos;
- kalman.setStateModel(model);
- kalman.initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
- break;
- }
-
- for (unsigned int iter=0; iter <= niter; iter++) {
- std::cout << "-------- iter " << iter << " ------------" << std::endl;
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- measure[signal] = 3+2*signal + 0.3*sin(vpMath::rad(360./niter*iter));
- }
- std::cout << "measure : " << measure.t() << std::endl;
-
- flog << measure.t();
-
- // kalman.prediction();
- kalman.filter(measure);
- flog << kalman.Xest.t() << std::endl;
-
- std::cout << "Xest: " << kalman.Xest.t() << std::endl;
- }
-
- flog.close();
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/math/testMatrix.cpp b/test/math/testMatrix.cpp
deleted file mode 100644
index 02aad08..0000000
--- a/test/math/testMatrix.cpp
+++ /dev/null
@@ -1,286 +0,0 @@
-/****************************************************************************
- *
- * $Id: testMatrix.cpp 5242 2015-01-30 18:22:41Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test some vpMatrix functionalities.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testMatrix.cpp
-
- Test some vpMatrix functionalities.
-*/
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpGEMM.h>
-
-
-#include <stdlib.h>
-#include <stdio.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test some vpMatrix functionalities.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
- {
- vpMatrix M(4,5);
- int val = 0;
- for(unsigned int i=0; i<M.getRows(); i++) {
- for(unsigned int j=0; j<M.getCols(); j++) {
- M[i][j] = val++;
- }
- }
- std::cout <<"M ";
- M.print (std::cout, 4);
-
- vpMatrix N;
- N.init(M, 0, 1, 2, 3);
- std::cout <<"N ";
- N.print (std::cout, 4);
- }
- {
-
- std::cout << "------------------------" << std::endl;
- std::cout << "--- TEST PRETTY PRINT---" << std::endl;
- std::cout << "------------------------" << std::endl;
- vpMatrix M ;
- M.eye(4);
-
- std::cout << "call std::cout << M;" << std::endl;
- std::cout << M << std::endl;
-
- std::cout << "call M.print (std::cout, 4);" << std::endl;
- M.print (std::cout, 4);
-
- std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
- M.eye(3);
- M[1][0]=1.235;
- M[1][1]=12.345;
- M[1][2]=.12345;
- std::cout << "call std::cout << M;" << std::endl;
- std::cout << M;
- std::cout << "call M.print (std::cout, 6);" << std::endl;
- M.print (std::cout, 6);
- std::cout << std::endl;
-
- std::cout << "------------------------" << std::endl;
- M[0][0]=-1.235;
- M[1][0]=-12.235;
-
- std::cout << "call std::cout << M;" << std::endl;
- std::cout << M << std::endl;
-
- std::cout << "call M.print (std::cout, 10);" << std::endl;
- M.print (std::cout, 10);
- std::cout << std::endl;
-
- std::cout << "call M.print (std::cout, 2);" << std::endl;
- M.print (std::cout, 2);
- std::cout << std::endl;
-
- std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
- M.eye(3);
- M[0][2]=-0.0000000876;
- std::cout << "call std::cout << M;" << std::endl;
- std::cout << M << std::endl;
-
- std::cout << "call M.print (std::cout, 4);" << std::endl;
- M.print (std::cout, 4);
- std::cout << std::endl;
- std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
- M.print (std::cout, 10, "M");
- std::cout << std::endl;
- std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
- M.print (std::cout, 20, "M");
- std::cout << std::endl;
-
-
- std::cout << "------------------------" << std::endl;
- std::cout << "--- TEST RESIZE --------" << std::endl;
- std::cout << "------------------------" << std::endl;
- std::cout << "5x5" << std::endl;
- M.resize(5,5,false);
- std::cout << M << std::endl;
- std::cout << "3x2" << std::endl;
- M.resize(3,2,false);
- std::cout << M << std::endl;
- std::cout << "2x2" << std::endl;
- M.resize(2,2,false);
- std::cout << M << std::endl;
- std::cout << "------------------------" << std::endl;
-
- vpVelocityTwistMatrix vMe;
- vpMatrix A(1,6),B;
-
- A=1.0;
- //vMe=1.0;
- B=A*vMe;
-
- std::cout << "------------------------" << std::endl;
- std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
- std::cout << "------------------------" << std::endl;
- vpRowVector r(3);
- r[0] = 2;
- r[1] = 3;
- r[2] = 4;
-
- vpColVector c(3);
- c[0] = 1;
- c[1] = 2;
- c[2] = -1;
-
- double rc = r * c;
-
- r.print(std::cout, 2, "r");
- c.print(std::cout, 2, "c");
- std::cout << "r * c = " << rc << std::endl;
-
- std::cout << "------------------------" << std::endl;
- std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
- std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
- M.eye(3);
-
- M[1][0] = 1.5;
- M[2][0] = 2.3;
-
- vpRowVector rM = r * M;
-
- r.print(std::cout, 2, "r");
- M.print(std::cout, 10, "M");
- std::cout << "r * M = " << rM << std::endl;
-
- std::cout << "------------------------" << std::endl;
- std::cout << "--- TEST vpGEMM " << std::endl;
- std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
- M.eye(3);
- vpMatrix N(3, 3);
- N[0][0] = 2;
- N[1][0] = 1.2;
- N[1][2] = 0.6;
- N[2][2] = 0.25;
-
- vpMatrix C(3, 3);
- C.eye(3);
-
- vpMatrix D;
-
- //realise the operation D = 2 * M^T * N + 3 C
- vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
- std::cout << D << std::endl;
- return 0;
- }
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/math/testMatrixException.cpp b/test/math/testMatrixException.cpp
deleted file mode 100644
index d9b8b88..0000000
--- a/test/math/testMatrixException.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-/****************************************************************************
- *
- * $Id: testMatrixException.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test matrix exceptions.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testMatrixException.cpp
- \brief Tests matrix exception
-*/
-
-#include <visp/vpMath.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpMatrixException.h>
-#include <visp/vpDebug.h>
-
-#include <visp/vpParseArgv.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test some vpMatrix functionalities.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpMatrix M ;
- vpMatrix M1(2,3) ;
- vpMatrix M2(3,3) ;
- vpMatrix M3(2,2) ;
-
- vpTRACE("test matrix size in multiply") ;
-
- try
- {
- M = M1*M3 ;
- }
- catch (vpMatrixException me)
- {
- std::cout << me << std::endl ;
- }
-
-
- vpTRACE("test matrix size in addition") ;
-
- try
- {
- M = M1+M3 ;
- }
- catch (vpMatrixException me)
- {
- std::cout << me << std::endl ;
- }
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/math/testMatrixInverse.cpp b/test/math/testMatrixInverse.cpp
deleted file mode 100644
index 6051083..0000000
--- a/test/math/testMatrixInverse.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/****************************************************************************
- *
- * $Id: testSvd.cpp 3857 2012-07-25 11:47:30Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test various inversions.
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-
-
-/*!
- \example testMatrixInverse.cpp
- \brief Test various matrix inversions.
-*/
-
-
-
-#include <visp/vpTime.h>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpParseArgv.h>
-#include <vector>
-#include <stdlib.h>
-#include <stdio.h>
-#include <fstream>
-#include <iostream>
-#include <cmath>
-// List of allowed command line options
-#define GETOPTARGS "n:i:pf:r:c:vh"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv,
- unsigned int& nb_matrices, unsigned int& nb_iterations,
- bool& use_plot_file, std::string& plotfile,
- unsigned int& nbrows, unsigned int& nbcols, bool& verbose);
-void writeTime(const char *name, double time);
-vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
-
- */
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test matrix inversions\n\
-using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
-Outputs a comparison of these methods.\n\
-\n\
-SYNOPSIS\n\
- %s [-n <number of matrices>] [-f <plot filename>]\n\
- [-r <number of rows>] [-c <number of columns>]\n\
- [-i <number of iterations>] [-p] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -n <number of matrices> \n\
- Number of matrices inverted during each test loop.\n\
-\n\
- -i <number of iterations> \n\
- Number of iterations of the test.\n\
-\n\
- -f <plot filename> \n\
- Set output path for plot output.\n\
- The plot logs the times of \n\
- the different inversion methods: \n\
- QR,LU,Cholesky and Pseudo-inverse.\n\
-\n\
- -r <number of rows>\n\
- Number of rows of the automatically generated matrices \n\
- we test on.\n\
-\n\
- -c <number of columns>\n\
- Number of colums of the automatically generated matrices \n\
- we test on.\n\
-\n\
- -p \n\
- Plot into filename in the gnuplot format. \n\
- If this option is used, tests results will be logged \n\
- into a filename specified with -f.\n\
-\n\
- -h\n\
- Print the help.\n\n");
-
- if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
- }
-
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv,
- unsigned int& nb_matrices, unsigned int& nb_iterations,
- bool& use_plot_file, std::string& plotfile,
- unsigned int& nbrows, unsigned int& nbcols, bool& verbose)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
- case 'n':
- nb_matrices = (unsigned int)atoi(optarg_);
- break;
- case 'i':
- nb_iterations = (unsigned int)atoi(optarg_);
- break;
- case 'f':
- plotfile = optarg_;
- use_plot_file = true;
- break;
- case 'p':
- use_plot_file = true;
- break;
- case 'r':
- nbrows = (unsigned int)atoi(optarg_);
- break;
- case 'c':
- nbcols = (unsigned int)atoi(optarg_);
- break;
- case 'v':
- verbose = true;
- break;
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-void writeTime(const char *name, double time)
-{
- std::cout << name << " time=" << time << " ms." << std::endl;
-}
-
-vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols)
-{
- vpMatrix A;
- A.resize(nbrows,nbcols);
-
- for (unsigned int i=0 ; i < A.getRows() ; i++)
- for (unsigned int j=0 ; j < A.getCols() ; j++)
- A[i][j] = (double)rand()/(double)RAND_MAX;
- return A;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
-#ifdef VISP_HAVE_LAPACK
- try {
- unsigned int nb_matrices=1000;
- unsigned int nb_iterations=10;
- unsigned int nb_rows = 6;
- unsigned int nb_cols = 6;
- bool verbose = false;
- std::string plotfile("plot.txt");
- bool use_plot_file=false;
- std::ofstream of;
-
- double t, qr_time, lu_time,pi_time,chol_time;
- // Read the command line options
- if (getOptions(argc, argv, nb_matrices,nb_iterations,use_plot_file,plotfile,nb_rows,nb_cols,verbose) == false) {
- exit (-1);
- }
-
- if(use_plot_file){
- of.open(plotfile.c_str());
- }
-
- for(unsigned int iter=0;iter<nb_iterations;iter++){
- std::vector<vpMatrix> benchQR;
- std::vector<vpMatrix> benchLU;
- std::vector<vpMatrix> benchCholesky;
- std::vector<vpMatrix> benchPseudoInverse;
- if(verbose)
- std::cout << "********* generating matrices for iteration " << iter << "." << std::endl;
- for(unsigned int i=0;i<nb_matrices;i++){
- vpMatrix cur;
- double det=0.;
- //don't put singular matrices in the benchmark
- for(cur=makeRandomMatrix(nb_rows,nb_cols);std::abs(det=cur.AtA().det())<.01;cur = makeRandomMatrix(nb_rows,nb_cols))
- if(verbose){
- std::cout << "Generated random matrix A*tA=" << std::endl << cur.AtA() << std::endl;
- std::cout << "generated random matrix not invertibleL: det="<<det<< ". Retrying..." << std::endl;
- }
- benchCholesky.push_back(cur);
- benchQR.push_back(cur);
- benchLU.push_back(cur);
- benchPseudoInverse.push_back(cur);
- }
-
- if(verbose)
- std::cout << "\t Inverting " << benchCholesky[0].AtA().getRows() << "x" << benchCholesky[0].AtA().getCols() << " matrix using cholesky decomposition." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchCholesky[i]=benchCholesky[i].AtA().inverseByCholesky()*benchCholesky[i].transpose();
- }
- chol_time = vpTime::measureTimeMs() - t ;
-
- if(verbose)
- std::cout << "\t Inverting " << benchLU[0].AtA().getRows() << "x" << benchLU[0].AtA().getCols() << " matrix using LU decomposition." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++)
- benchLU[i] = benchLU[i].AtA().inverseByLU()*benchLU[i].transpose();
- lu_time = vpTime::measureTimeMs() -t ;
-
- if(verbose)
- std::cout << "\t Inverting " << benchQR[0].AtA().getRows() << "x" << benchQR[0].AtA().getCols() << " matrix using QR decomposition." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchQR[i]=benchQR[i].AtA().inverseByQR()*benchQR[i].transpose();
- }
- qr_time = vpTime::measureTimeMs() - t ;
-
- if(verbose)
- std::cout << "\t Inverting " << benchPseudoInverse[0].AtA().getRows() << "x" << benchPseudoInverse[0].AtA().getCols() << " matrix while computing pseudo-inverse." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchPseudoInverse[i]=benchPseudoInverse[i].pseudoInverse();
- }
- pi_time = vpTime::measureTimeMs() - t ;
-
- double avg_err_lu_qr=0.;
- double avg_err_lu_pi=0.;
- double avg_err_lu_chol=0.;
- double avg_err_qr_pi=0.;
- double avg_err_qr_chol=0.;
- double avg_err_pi_chol=0.;
-
- for(unsigned int i=0;i<nb_matrices;i++){
- avg_err_lu_qr+= (benchQR[i]-benchLU[i]).euclideanNorm();
- avg_err_lu_pi+= (benchPseudoInverse[i]-benchLU[i]).euclideanNorm();
- avg_err_qr_pi+= (benchPseudoInverse[i]-benchQR[i]).euclideanNorm();
- avg_err_qr_chol+= (benchCholesky[i]-benchQR[i]).euclideanNorm();
- avg_err_lu_chol+= (benchCholesky[i]-benchLU[i]).euclideanNorm();
- avg_err_pi_chol+= (benchCholesky[i]-benchPseudoInverse[i]).euclideanNorm();
- }
-
- avg_err_lu_qr/=nb_matrices;
- avg_err_lu_pi/=nb_matrices;
- avg_err_qr_pi/=nb_matrices;
-
- if(use_plot_file){
- of << iter << "\t" << lu_time << "\t" << qr_time << "\t" << pi_time << "\t" << chol_time << "\t" << avg_err_lu_qr << "\t" << avg_err_qr_pi << "\t" << avg_err_lu_pi << "\t" << avg_err_qr_chol << "\t" << avg_err_lu_chol << "\t" << avg_err_pi_chol << std::endl;
- }
- if(verbose || !use_plot_file){
- writeTime("LU",lu_time);
- writeTime("QR",qr_time);
- writeTime("Pseudo-inverse",pi_time);
- writeTime("Cholesky",chol_time);
- }
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-
-#else
- (void)argc;
- (void)argv;
- std::cout << "You don't have lapack installed" << std::endl;
- return 0;
-#endif
-}
-
diff --git a/test/math/testRobust.cpp b/test/math/testRobust.cpp
deleted file mode 100644
index 5bf3cd4..0000000
--- a/test/math/testRobust.cpp
+++ /dev/null
@@ -1,220 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobust.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test some vpMath functionalities.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobust.cpp
-
- Test some vpMath functionalities.
-*/
-
-
-#include <visp/vpRobust.h>
-#include <string>
-#include <fstream>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
-// List of allowed command line options
-#define GETOPTARGS "ho:"
-
-void usage(const char *name, const char *badparam, std::string ofilename);
-bool getOptions(int argc, const char **argv, std::string &ofilename);
-
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ofilename : Output filename.
-*/
-void usage(const char *name, const char *badparam, std::string ofilename)
-{
- fprintf(stdout, "\n\
-Test some vpMath functionalities. Compute weights and print\n\
-them in an output file.\n\
-\n\
-Using gnuplot the content of the output file can be printed by:\n\
-set style data line\n\
-set ylabel \"weight\"\n\
-set yr [0:1.19]\n\
-set xlabel \"Normalized residuals\"\n\
-plot '%s' title \"Tukey Estimator\" lw 2, 1 title \"Least-Squares\" lw 2\n\
-\n\
-\n\
-SYNOPSIS\n\
- %s [-o <output filename>] [-h]\n", ofilename.c_str(), name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -o <output filename> %s\n\
- Name and path of the file containing computed \n\
- weights.\n\
-\n\
- -h\n\
- Print the help.\n",
- ofilename.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ofilename : Output filename.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ofilename)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'o': ofilename = optarg_; break;
- case 'h': usage(argv[0], NULL, ofilename); return false; break;
-
- default:
- usage(argv[0], optarg_, ofilename);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ofilename);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string ofilename;
- std::string username;
-
- // Set the default output filename
-#if defined(_WIN32)
- ofilename = "C:/temp";
-#else
- ofilename = "/tmp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Append to the output filename string, the login name of the user
- ofilename = ofilename + "/" + username;
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(ofilename) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(ofilename);
- }
- catch (...) {
- usage(argv[0], NULL, ofilename);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << ofilename << std::endl;
- std::cerr << " Check your -o " << ofilename << " option " << std::endl;
- exit(-1);
- }
- }
-
- // Append to the output filename string, the name of the file
- ofilename = ofilename + "/w.dat";
-
- // Read the command line options
- if (getOptions(argc, argv, ofilename) == false) {
- exit (-1);
- }
-
- double sig = 1 ;
-
- double w ;
- std::ofstream f;
- std::cout << "Create file: " << ofilename << std::endl;
- f.open(ofilename.c_str());
- if (f.fail()) {
- usage(argv[0], NULL, ofilename);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create the file: " << ofilename << std::endl;
- std::cerr << " Check your -o " << ofilename << " option " << std::endl;
- exit(-1);
-
- }
- double x = -10 ;
- while (x<10)
- {
- if (fabs(x/sig)<=(4.6851))
- {
- w = vpMath::sqr(1-vpMath::sqr(x/(sig*4.6851)));
- }
- else
- {
- w = 0;
- }
- f << x <<" "<<w <<std::endl ;
- x+= 0.01 ;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/math/testRotation.cpp b/test/math/testRotation.cpp
deleted file mode 100644
index d87369f..0000000
--- a/test/math/testRotation.cpp
+++ /dev/null
@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRotation.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tests transformation from various representations of rotation.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \file testRotation.cpp
- \brief Tests transformation within various representations of rotation.
-*/
-
-
-#include <visp/vpMath.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpQuaternionVector.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <cassert>
-#include <limits>
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Tests transformation within various representations of rotation.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
- vpRotationMatrix R;
- for(int i=-10;i<10;i++){
- for(int j=-10;j<10;j++){
- vpThetaUVector tu(vpMath::rad(90+i), vpMath::rad(170+j), vpMath::rad(45)) ;
- tu.buildFrom(vpRotationMatrix(tu)); //put some coherence into rotation convention
-
- std::cout << "Initialization " <<std::endl ;
-
- double theta;
- vpColVector u;
- tu.extract(theta, u);
-
- std::cout << "theta=" << vpMath::deg(theta) << std::endl ;
- std::cout << "u=" << u << std::endl ;
-
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(tu) ;
-
- std::cout << "Matrix R" ;
- if (R.isARotationMatrix()==1) std::cout <<" is a rotation matrix " << std::endl ;
- else std::cout <<" is not a rotation matrix " << std::endl ;
-
- std::cout << R << std::endl ;
-
- std::cout << "From vpRotationMatrix to vpQuaternionVector " << std::endl ;
- vpQuaternionVector q(R);
- std::cout << q <<std::endl ;
-
- R.buildFrom(q);
- std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl ;
-
- std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl ;
- vpRxyzVector RxyzBuildFromR(R) ;
- std::cout << RxyzBuildFromR <<std::endl ;
-
-
- std::cout << "From vpRxyzVector to vpThetaUVector " << std::endl ;
- std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl ;
- std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl ;
-
-
- vpThetaUVector tuBuildFromEu ;
- tuBuildFromEu.buildFrom(R) ;
-
- std::cout << std::endl ;
- std::cout << "result : should equivalent to the first one " << std::endl ;
-
-
- double theta2;
- vpColVector u2;
-
- tuBuildFromEu.extract(theta2, u2);
- std::cout << "theta=" << vpMath::deg(theta2) << std::endl ;
- std::cout << "u=" << u2 << std::endl ;
-
- assert(vpMath::abs(theta2-theta)<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[0]-u2[0])<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[1]-u2[1])<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[2]-u2[2])<std::numeric_limits<double>::epsilon()*1e10);
- }
- vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
- std::cout << "Initialization vpRzyzVector " <<std::endl ;
- std::cout << rzyz << std::endl ;
- std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(rzyz) ;
- std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl ;
- vpRzyzVector rzyz_final ;
- rzyz_final.buildFrom(R) ;
- std::cout << rzyz_final << std::endl ;
-
-
- vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
- std::cout << "Initialization vpRzyxVector " <<std::endl ;
- std::cout << rzyx << std::endl ;
- std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(rzyx) ;
- std::cout << R << std::endl ;
- std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl ;
- vpRzyxVector rzyx_final ;
- rzyx_final.buildFrom(R) ;
- std::cout << rzyx_final << std::endl ;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/math/testSvd.cpp b/test/math/testSvd.cpp
deleted file mode 100644
index 5963321..0000000
--- a/test/math/testSvd.cpp
+++ /dev/null
@@ -1,270 +0,0 @@
-/****************************************************************************
- *
- * $Id: testSvd.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test various svd decompositions.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-/*!
- \example testSvd.cpp
- \brief Test various svd decompositions.
-*/
-
-
-
-#include <visp/vpTime.h>
-
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpParseArgv.h>
-#include <vector>
-#include <algorithm>
-#include <stdlib.h>
-#include <stdio.h>
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-bool testSvdOpenCvGSLCoherence(double epsilon);
-#ifdef VISP_HAVE_GSL
-bool testRandom(double epsilon);
-#endif
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Test various svd decompositions.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-#define abs(x) ((x) < 0 ? - (x) : (x))
-#ifdef VISP_HAVE_GSL
-
-bool testRandom(double epsilon)
-{
- vpMatrix L0(6,6);
- vpMatrix L1(6,6);
-
- for (unsigned int i=0 ; i < L0.getRows() ; i++)
- for (unsigned int j=0 ; j < L0.getCols() ; j++)
- L1[i][j] = L0[i][j] = (double)rand()/(double)RAND_MAX;
-
- vpColVector W0(L0.getCols()) ;
- vpMatrix V0(L0.getCols(), L0.getCols()) ;
- vpColVector W1(L1.getCols()) ;
- vpMatrix V1(L1.getCols(), L1.getCols()) ;
-
- L0.svdNr(W0,V0);
- L1.svdGsl(W1,V1);
-
- vpColVector _W0 = vpColVector::sort(W0);
- vpColVector _W1 = vpColVector::sort(W1);
-
- vpColVector diff = _W0-_W1;
- double error=-1.0;
-
- for(unsigned int i=0;i<6;i++)
- error=std::max(abs(diff[i]),error);
-
- return error<epsilon;
-
-}
-
-#endif
-
-bool testSvdOpenCvGSLCoherence(double epsilon)
-{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && defined (VISP_HAVE_GSL) // Require opencv >= 2.1.1
- vpMatrix A;
- vpMatrix vA;
- vpColVector wA;
- vpMatrix B;
- vpMatrix vB;
- vpColVector wB;
- A.resize(6,6);
- B.resize(6,6);
- vA.resize(6,6);
- vB.resize(6,6);
- wA.resize(6);
- wB.resize(6);
-
- for (unsigned int i=0 ; i < A.getRows() ; i++)
- for (unsigned int j=0 ; j < A.getCols() ; j++)
- B[i][j] = A[i][j] = (double)rand()/(double)RAND_MAX;
-
- A.svdOpenCV(wA,vA);
- B.svdGsl(wB,vB);
-
- bool error=false;
- for (unsigned int i=0 ; i < A.getRows() ; i++){
- error = error | (abs(wA[i]-wB[i])>epsilon);
- }
-
- return !error;
-#else
- (void)epsilon;
- return true;
-#endif
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpMatrix L(60000,6), Ls ;
- for (unsigned int i=0 ; i < L.getRows() ; i++)
- for (unsigned int j=0 ; j < L.getCols() ; j++)
- L[i][j] = 2*i+j + cos((double)(i+j))+((double)(i)) ;
- // std::cout << L << std::endl ;
- Ls = L ;
- std::cout << "--------------------------------------"<<std::endl ;
-
- vpColVector W(L.getCols()) ;
- vpMatrix V(L.getCols(), L.getCols()) ;
-
- double t = vpTime::measureTimeMs() ;
- L.svdNr(W,V) ;
- t = vpTime::measureTimeMs() -t ;
-
- std::cout <<"svdNr Numerical recipes \n time " <<t << std::endl;
- std::cout << W.t() ;
- std::cout << "--------------------------------------"<<std::endl ;
-
-
-#ifdef VISP_HAVE_GSL
- L = Ls ;
- t = vpTime::measureTimeMs() ;
- L.svdGsl(W,V) ;
- t = vpTime::measureTimeMs() -t ;
- std::cout <<"svdGsl_mod \n time " <<t << std::endl;
- std::cout << W.t() ;
-
- std::cout << "--------------------------------------"<<std::endl ;
- std::cout << "TESTING RANDOM MATRICES:" ;
-
- bool ret = true;
- for(unsigned int i=0;i<2000;i++)
- ret = ret & testRandom(0.00001);
- if(ret)
- std:: cout << "Success"<< std:: endl;
- else
- std:: cout << "Fail"<< std:: endl;
-
- std::cout << "--------------------------------------"<<std::endl ;
-#endif
-
- std::cout << "--------------------------------------"<<std::endl ;
- std::cout << "TESTING OPENCV-GSL coherence:" ;
-
- bool ret2 = true;
- for(unsigned int i=0;i<1;i++)
- ret2 = ret2 & testSvdOpenCvGSLCoherence(0.00001);
- if(ret2)
- std:: cout << "Success"<< std:: endl;
- else
- std:: cout << "Fail"<< std:: endl;
-
- std::cout << "--------------------------------------"<<std::endl ;
-
- L = Ls ;
- t = vpTime::measureTimeMs() ;
- L.svdFlake(W,V) ;
- t = vpTime::measureTimeMs() -t ;
- std::cout <<"svdFlake\n time " <<t << std::endl;
- std::cout << W.t() ;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/math/testTwistMatrix.cpp b/test/math/testTwistMatrix.cpp
deleted file mode 100644
index f0cd2db..0000000
--- a/test/math/testTwistMatrix.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-/****************************************************************************
- *
- * $Id: testTwistMatrix.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Tests some vpMatrix functionalities.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testTwistMatrix.cpp
-
- \brief Test some vpMatrix functionalities.
-*/
-
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpDebug.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-#include <visp/vpMath.h>
-#include <visp/vpMatrix.h>
-#include <visp/vpColVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpVelocityTwistMatrix.h>
-#include <visp/vpParseArgv.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Tests some vpMatrix functionalities.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpTRACE("--------------------------");
- vpTRACE("--- TEST vpVelocityTwistMatrix ---");
- vpTRACE("--------------------------");
-
- // Set the translation
- vpTranslationVector cte;
- cte[0] = 1.;
- cte[1] = 0.5;
- cte[2] = -1.;
-
- // Set the rotation
- vpRxyzVector cre;
- cre[0] = M_PI/2.;
- cre[1] = -M_PI/2.;
- cre[2] = -M_PI/4.;
-
- // Build rotation matrix
- vpRotationMatrix cRe(cre);
-
- // Build the twist matrix
- vpVelocityTwistMatrix cVe(cte, cRe);
-
- vpTRACE("cVe twist matrix:");
- cVe.print (std::cout, 6);
-
-
- // Set a speed skew
- vpColVector ev(6);
-
- ev[0] = 1.;
- ev[1] = 0.1;
- ev[2] = -0.5;
- ev[3] = M_PI/180.;
- ev[4] = M_PI/18.;
- ev[5] = M_PI/10.;
-
- vpTRACE("ev colvector:");
- ev.print (std::cout, 6);
-
- // Set a speed skew
- vpColVector cv;
-
- cv = cVe * ev;
-
- vpTRACE("cv = cVe * ev:");
- cv.print (std::cout, 6);
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/network/CMakeLists.txt b/test/network/CMakeLists.txt
deleted file mode 100644
index 26f45ab..0000000
--- a/test/network/CMakeLists.txt
+++ /dev/null
@@ -1,64 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Aurelien Yol
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testClient.cpp
- testServer.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/network/testClient.cpp b/test/network/testClient.cpp
deleted file mode 100644
index bdf7eb6..0000000
--- a/test/network/testClient.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-/****************************************************************************
- *
- * $Id: testClient.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for TCP client/Server.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \example testClient.cpp
-
- Example of a TCP client/server.
-*/
-
-#include <iostream>
-#include <visp/vpClient.h>
-
-int main()
-{
- try {
- std::string servername = "localhost";
- unsigned int port = 35000;
-
- vpClient client;
- client.connectToHostname(servername, port);
- //client.connectToIP("127.0.0.1",port);
-
- int val = 0;
-
- while(1)
- {
- if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
- std::cout << "Error while sending" << std::endl;
- else
- std::cout << "Sending : " << val << std::endl;
-
- if(client.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
- else
- std::cout << "Received : " << val << std::endl;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/network/testServer.cpp b/test/network/testServer.cpp
deleted file mode 100644
index e71f13b..0000000
--- a/test/network/testServer.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- * $Id: testServer.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for TCP client/Server.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-/*!
- \example testServer.cpp
-
- Example of a TCP client/server.
-*/
-
-#include <iostream>
-#include <visp/vpServer.h>
-
-int main()
-{
- try {
- int port = 35000;
- vpServer serv(port); //Launch the server on localhost
- serv.start();
-
- bool run = true;
- int val;
-
- while(run){
- serv.checkForConnections();
-
- if(serv.getNumberOfClients() > 0)
- {
- if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
- else
- std::cout << "Received : " << val << std::endl;
-
- val = val+1;
- if(serv.send(&val) != sizeof(int)) //Sending the new value to the first client
- std::cout << "Error while sending" << std::endl;
- else
- std::cout << "Sending : " << val << std::endl;
- }
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/pose/CMakeLists.txt b/test/pose/CMakeLists.txt
deleted file mode 100644
index 6456016..0000000
--- a/test/pose/CMakeLists.txt
+++ /dev/null
@@ -1,69 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testPose.cpp
- testPoseRansac.cpp
- testFindMatch.cpp
- testPoseFeatures.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/pose/testFindMatch.cpp b/test/pose/testFindMatch.cpp
deleted file mode 100644
index 62223fb..0000000
--- a/test/pose/testFindMatch.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/****************************************************************************
- *
- * $Id: testFindMatch.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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 <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-#define L 0.1
-
-
-/*!
- \example testFindMatch.cpp
-
- Find Matches using Ransac.
-
-*/
-
-int
-main()
-{
- try {
- std::cout << "Find Matches using Ransac" << std::endl;
- unsigned int nb3D = 5;
- unsigned int nb2D = 5;
- std::vector<vpPoint> P(nb3D);
- std::vector<vpPoint> p(nb2D);
-
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,L, 0 ) ;
- P[4].setWorldCoordinates(-0,L/2., L ) ;
-
- vpHomogeneousMatrix cMo_ref(0, 0.2, 1, vpMath::rad(3), vpMath::rad(-2), vpMath::rad(10)) ;
-
- for(unsigned int i=0 ; i < nb3D ; i++)
- {
- vpPoint pt = P[i];
- pt.project(cMo_ref);
- p[i].set_x(pt.get_x());
- p[i].set_y(pt.get_y());
- }
-
- unsigned int ninliers ;
- std::vector<vpPoint> inliers;
- double threshold = 1e-6;
- unsigned int nbInlierToReachConsensus = nb3D;
-
- vpHomogeneousMatrix cMo ;
-
- vpPose::findMatch(p,P,nbInlierToReachConsensus,threshold,ninliers,inliers,cMo);
-
- std::cout << "Inliers: " << std::endl;
- for (unsigned int i = 0; i < inliers.size() ; i++)
- {
- inliers[i].print() ;
- std::cout << std::endl;
- }
-
- std::cout << "cMo :\n" << vpPoseVector(cMo).t() << std::endl << std::endl;
-
- vpPoseVector pose_ref = vpPoseVector(cMo_ref);
- vpPoseVector pose_est = vpPoseVector(cMo);
-
- std::cout << std::endl;
- std::cout << "reference cMo :\n" << pose_ref.t() << std::endl << std::endl;
- std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
-
- int test_fail = 0;
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
- test_fail = 1;
- }
-
- std::cout << "Matching is " << (test_fail ? "badly" : "well") << " performed" << std::endl;
-
- return test_fail;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/pose/testPose.cpp b/test/pose/testPose.cpp
deleted file mode 100644
index 00b3922..0000000
--- a/test/pose/testPose.cpp
+++ /dev/null
@@ -1,280 +0,0 @@
-/****************************************************************************
- *
- * $Id: testPose.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Compute the pose of a 3D object using the Dementhon, Lagrange and
- * Non-Linear approach.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-#include <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRxyzVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpDebug.h>
-#include <visp/vpParseArgv.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-#define L 0.035
-
-/*!
- \example testPose.cpp
-
- Compute the pose of a 3D object using the Dementhon, Lagrange and
- Non-Linear approach.
-
-*/
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
-int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
- const std::string &legend);
-
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Compute the pose of a 3D object using the Dementhon, Lagrange and\n\
-Non-Linear approach.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-// print the resulting estimated pose
-void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
-{
- vpPoseVector cpo = vpPoseVector(cMo);
-
- std::cout << std::endl << legend << "\n "
- << "tx = " << cpo[0] << "\n "
- << "ty = " << cpo[1] << "\n "
- << "tz = " << cpo[2] << "\n "
- << "tux = vpMath::rad(" << vpMath::deg(cpo[3]) << ")\n "
- << "tuy = vpMath::rad(" << vpMath::deg(cpo[4]) << ")\n "
- << "tuz = vpMath::rad(" << vpMath::deg(cpo[5]) << ")\n"
- << std::endl;
-}
-
-// test if pose is well estimated
-int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
- const std::string &legend)
-{
- vpPoseVector pose_ref = vpPoseVector(cMo_ref);
- vpPoseVector pose_est = vpPoseVector(cMo_est);
-
- int fail = 0;
-
- // Test done on the 3D pose
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
- fail = 1;
- }
-
- std::cout << "Based on 3D parameters " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
-
- // Test done on the residual
- double r = pose.computeResidual(cMo_est);
- if (pose.listP.size() < 4) {
- fail = 1;
- std::cout << "Not enough point" << std::endl;
- return fail;
- }
- r = sqrt(r)/pose.listP.size();
- //std::cout << "Residual on each point (meter): " << r << std::endl;
- fail = (r > 0.1) ? 1 : 0;
- std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
- return fail;
-}
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- vpPoint P[5] ; // Point to be tracked
- vpPose pose ;
- pose.clearPoint() ;
-
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-2*L, 3*L, 0 ) ;
- P[4].setWorldCoordinates(-L,L, 0.01 ) ;
- //P[3].setWorldCoordinates(-L,L, 0 ) ;
-
- int test_fail = 0, fail = 0;
- vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0,vpMath::rad(10));
- vpHomogeneousMatrix cMo_ref(cpo_ref) ;
- vpHomogeneousMatrix cMo ; // will contain the estimated pose
-
- for(int i=0 ; i < 5 ; i++) {
- P[i].project(cMo_ref) ;
- //P[i].print();
- pose.addPoint(P[i]) ; // and added to the pose computation class
- }
-
- // Let's go ...
- print_pose(cMo_ref, std::string("Reference pose")); // print the reference pose
-
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Lagrange"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
- test_fail |= fail;
-
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Dementhon"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
- test_fail |= fail;
-
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.setRansacNbInliersToReachConsensus(4);
- pose.setRansacThreshold(0.01);
- pose.computePose(vpPose::RANSAC, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Ransac"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
- test_fail |= fail;
-
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE_LOWE, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Lagrange than Lowe"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than Lowe");
- test_fail |= fail;
-
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON_LOWE, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Dementhon than Lowe"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than Lowe");
- test_fail |= fail;
-
- // Now Virtual Visual servoing
-
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by VVS"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
- test_fail |= fail;
-
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Dementhon than by VVS"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than by VVS");
- test_fail |= fail;
-
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
-
- print_pose(cMo, std::string("Pose estimated by Lagrange than by VVS"));
- fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than by VVS");
- test_fail |= fail;
-
- std::cout << "\nGlobal pose estimation test " << (test_fail ? "fail" : "is ok") << std::endl;
-
- return test_fail;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/pose/testPoseFeatures.cpp b/test/pose/testPoseFeatures.cpp
deleted file mode 100644
index 19d2a59..0000000
--- a/test/pose/testPoseFeatures.cpp
+++ /dev/null
@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- * $Id: testPoseFeatures.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Compute the pose from visual features by virtual visual servoing.
- *
- * Authors:
- * Aurelien Yol
- *
- *****************************************************************************/
-
-#include <visp/vpConfig.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPoint.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPoseFeatures.h>
-#include <visp/vpPose.h>
-#include <iostream>
-#include <vector>
-#include <visp/vpPose.h>
-#include <limits>
-
-/*!
- \example testPoseFeatures.cpp
-
- Compute the pose from different features.
-
-*/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
-class vp_createPointClass{
-public:
- int value;
-
- vp_createPointClass() : value(0){}
-
- int vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
- value += 1;
- vpFeatureBuilder::create(fp,v);
- return value;
- }
-};
-
-void vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
- vpFeatureBuilder::create(fp,v);
-}
-
-void vp_createLine(vpFeatureLine &fp,const vpLine &v){
- vpFeatureBuilder::create(fp,v);
-}
-#endif
-#endif
-
-int main()
-{
- try {
- vpImage<unsigned char> I(600,600);
-
- vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
- vpPoseVector pose_ref = vpPoseVector(cMo_ref);
-
- std::cout << "Reference pose used to create the visual features : " << std::endl;
- std::cout << pose_ref.t() << std::endl;
-
- vpPoseFeatures pose;
-
- vpPoint pts[6];
-
- double val = 0.25;
- double val2 = 0.0;
-
- //2D Point Feature
- pts[0].setWorldCoordinates(0.0,-val,val2);
- pts[1].setWorldCoordinates(0.0,val,val2);
- pts[2].setWorldCoordinates(-val,val,val2);
-
- //Segment Feature
- pts[3].setWorldCoordinates(-val,-val/2.0,val2);
- pts[4].setWorldCoordinates(val,val/2.0,val2);
-
- //3D point Feature
- pts[5].setWorldCoordinates(0.0,0.0,-1.5);
-
- //Line Feature
- vpLine line;
- line.setWorldCoordinates(0.0,1.0,0.0,.0,
- 0.0,0.0,1.0,0.0);
-
- //Vanishing Point Feature
- vpLine l1;
- l1.setWorldCoordinates(0.0,1.0,0.2,0.0,
- 1.0,0.0,0.0,-0.25);
-
- vpLine l2;
- l2.setWorldCoordinates(0.0,1.0,0.2,0.0,
- -1.0,0.0,0.0,-0.25);
-
- //Ellipse Feature
- vpCircle circle;
- circle.setWorldCoordinates(0.0, 0.0, 1.0 , 0.0, 0.0, 0.0, 0.25);
-
- pts[0].project(cMo_ref);
- pts[1].project(cMo_ref);
- pts[2].project(cMo_ref);
-
- pts[3].project(cMo_ref);
- pts[4].project(cMo_ref);
-
- pts[5].project(cMo_ref);
-
- line.project(cMo_ref);
-
- l1.project(cMo_ref);
- l2.project(cMo_ref);
-
- circle.project(cMo_ref);
-
- pose.addFeaturePoint(pts[0]);
- // pose.addFeaturePoint(pts[1]);
- pose.addFeaturePoint(pts[2]);
-
- pose.addFeaturePoint3D(pts[5]);
-
- pose.addFeatureVanishingPoint(l1,l2);
-
- // pose.addFeatureSegment(pts[3],pts[4]);
- //
- // pose.addFeatureLine(line);
-
- pose.addFeatureEllipse(circle);
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- vpFeaturePoint fp;
- vpFeatureLine fl;
- vpFeatureSegment fs;
- void (*ptr)(vpFeatureSegment&, vpPoint&, vpPoint&) = &vpFeatureBuilder::create;
- vp_createPointClass cpClass;
- int (vp_createPointClass::*ptrClass)(vpFeaturePoint&, const vpPoint&) = &vp_createPointClass::vp_createPoint;
- pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
- pose.addSpecificFeature(&vp_createLine, fl, line);
- pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
-#endif
-
- pose.setVerbose(true);
- pose.setLambda(0.6);
- pose.setVVSIterMax(200);
- pose.setCovarianceComputation(true);
-
- vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
- vpPoseVector pose_est = vpPoseVector(cMo_est);
- std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
- std::cout << pose_est.t() << std::endl;
-
- pose.computePose(cMo_est);
- // pose.computePose(cMo_est, vpPoseFeatures::ROBUST_VIRTUAL_VS);
-
-
- std::cout << "\nEstimated pose from visual features : " << std::endl;
- pose_est.buildFrom(cMo_est);
- std::cout << pose_est.t() << std::endl;
-
- std::cout << "\nResulting covariance (Diag): " << std::endl;
- vpMatrix covariance = pose.getCovarianceMatrix();
- std::cout << covariance[0][0] << " " << covariance[1][1] << " "
- << covariance[2][2] << " " << covariance[3][3] << " "
- << covariance[4][4] << " " << covariance[5][5] << " " << std::endl;
-
- int test_fail = 0;
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
- test_fail = 1;
- }
-
- std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
-
- return test_fail;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/pose/testPoseRansac.cpp b/test/pose/testPoseRansac.cpp
deleted file mode 100644
index 70e8acc..0000000
--- a/test/pose/testPoseRansac.cpp
+++ /dev/null
@@ -1,140 +0,0 @@
-/****************************************************************************
- *
- * $Id: testPoseRansac.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * 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 <visp/vpPose.h>
-#include <visp/vpPoint.h>
-#include <visp/vpMath.h>
-#include <visp/vpHomogeneousMatrix.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-
-#define L 0.1
-
-
-/*!
- \example testPoseRansac.cpp
-
- Compute the pose of a 3D object using the Ransac method.
-
-*/
-
-int
-main()
-{
- try {
- std::cout << "Pose computation with matched points" << std::endl;
- int size = 8;
- vpPoint *P = new vpPoint [size] ; // Point to be tracked
-
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,L, 0 ) ;
-
- double L2 = L*3.0;
- P[4].setWorldCoordinates(0,-L2, 0 ) ;
- P[5].setWorldCoordinates(L2,0, 0 ) ;
- P[6].setWorldCoordinates(0,L2, 0 ) ;
- P[7].setWorldCoordinates(-L2,0, 0 ) ;
-
- // P[4].setWorldCoordinates(-0,0, L ) ;
-
- vpHomogeneousMatrix cMo_ref(0, 0.2, 1, 0, 0, 0) ;
- for(int i=0 ; i < size ; i++)
- {
- P[i].project(cMo_ref) ;
- P[i].print() ;
- std::cout << std::endl;
- }
-
- //Introduce an error
- double error = 0.01;
- P[3].set_y(P[3].get_y() + 2*error);
- P[6].set_x(P[6].get_x() + error);
-
- vpPose pose;
- for(int i=0 ; i < size ; i++)
- pose.addPoint(P[i]);
-
- unsigned int nbInlierToReachConsensus = (unsigned int)(75.0 * (double)size / 100.0);
- double threshold = 0.001;
-
- pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
- pose.setRansacThreshold(threshold);
-
- vpHomogeneousMatrix cMo ;
- //vpPose::ransac(lp,lP, 5, 1e-6, ninliers, lPi, cMo) ;
- pose.computePose(vpPose::RANSAC, cMo);
-
- std::vector<vpPoint> inliers = pose.getRansacInliers();
-
- std::cout << "Inliers: " << std::endl;
- for (unsigned int i = 0; i < inliers.size() ; i++)
- {
- inliers[i].print() ;
- std::cout << std::endl;
- }
-
- vpPoseVector pose_ref = vpPoseVector(cMo_ref);
- vpPoseVector pose_est = vpPoseVector(cMo);
-
- std::cout << std::endl;
- std::cout << "reference cMo :\n" << pose_ref.t() << std::endl << std::endl;
- std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
-
- int test_fail = 0;
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
- test_fail = 1;
- }
-
- std::cout << "Pose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
- delete [] P;
- return test_fail;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/servo-afma4/CMakeLists.txt b/test/servo-afma4/CMakeLists.txt
deleted file mode 100644
index abcac6c..0000000
--- a/test/servo-afma4/CMakeLists.txt
+++ /dev/null
@@ -1,67 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testAfma4.cpp
- testRobotAfma4.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/servo-afma4/testAfma4.cpp b/test/servo-afma4/testAfma4.cpp
deleted file mode 100644
index 03583ed..0000000
--- a/test/servo-afma4/testAfma4.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- * $Id: testAfma4.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testAfma4.cpp
-
- Example of a real robot control, the Afma4 robot (cartesian robot, with 6
- degrees of freedom).
-*/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpAfma4.h>
-#include <visp/vpDebug.h>
-#include <visp/vpCameraParameters.h>
-
-#include <iostream>
-
-
-int main()
-{
- try {
-
- std::cout << "a test for vpAfma4 class..." << std::endl;
-
- vpAfma4 afma4;
-
- std::cout << afma4 << std::endl;
-
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/servo-afma4/testRobotAfma4.cpp b/test/servo-afma4/testRobotAfma4.cpp
deleted file mode 100644
index 96a26ff..0000000
--- a/test/servo-afma4/testRobotAfma4.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobotAfma4.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 4 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobotAfma4.cpp
-
- Example of a real robot control, the Afma4 robot (cylindrical robot, with 4
- degrees of freedom).
-*/
-
-#include <visp/vpRobotAfma4.h>
-#include <visp/vpDebug.h>
-#include <visp/vpCameraParameters.h>
-
-#include <iostream>
-
-
-#ifdef VISP_HAVE_AFMA4
-
-int main()
-{
- try {
-
- std::cout << "a test for vpRobotAfma4 class..." << std::endl;
-
- vpRobotAfma4 afma4;
-
- std::cout << afma4 << std::endl;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
-#else
-int main()
-{
- std::cout << "The real Afma4 robot controller is not available." << std::endl;
- return 0;
-}
-
-#endif
diff --git a/test/servo-afma6/CMakeLists.txt b/test/servo-afma6/CMakeLists.txt
deleted file mode 100644
index d0a19dc..0000000
--- a/test/servo-afma6/CMakeLists.txt
+++ /dev/null
@@ -1,73 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testAfma6.cpp
- testRobotAfma6.cpp
- testRobotAfma6Pose.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-# Add test
-add_test(testAfma6 testAfma6)
-add_test(testRobotAfma6 testRobotAfma6)
-
-# testRobotAfma6Pose cannot be run automaticlly, since human interaction is
-# required to click on the dots for the pose computation
-#add_test(testRobotAfma6Pose testRobotAfma6Pose)
diff --git a/test/servo-afma6/testAfma6.cpp b/test/servo-afma6/testAfma6.cpp
deleted file mode 100644
index 0665786..0000000
--- a/test/servo-afma6/testAfma6.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- * $Id: testAfma6.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testAfma6.cpp
-
- Example of a real robot control, the Afma6 robot (cartesian robot, with 6
- degrees of freedom).
-*/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpAfma6.h>
-#include <visp/vpDebug.h>
-#include <visp/vpCameraParameters.h>
-
-#include <iostream>
-
-
-int main()
-{
- try {
-
- std::cout << "a test for vpAfma6 class..." << std::endl;
-
- vpAfma6 afma6;
- vpCameraParameters cam;
-
- std::cout << "-- Default settings for Afma6 ---"
- << std::endl;
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP);
-
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the CCMOP tool with distortion ------"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the gripper tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_GRIPPER);
-
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the gripper tool with distortion ------"
- << std::endl;
- afma6.init( vpAfma6::TOOL_GRIPPER,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/servo-afma6/testRobotAfma6.cpp b/test/servo-afma6/testRobotAfma6.cpp
deleted file mode 100644
index 9ac60a2..0000000
--- a/test/servo-afma6/testRobotAfma6.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobotAfma6.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobotAfma6.cpp
-
- Example of a real robot control, the Afma6 robot (cartesian robot, with 6
- degrees of freedom).
-*/
-
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpDebug.h>
-#include <visp/vpCameraParameters.h>
-
-#include <iostream>
-
-
-#ifdef VISP_HAVE_AFMA6
-
-int main()
-{
- try {
-
- std::cout << "a test for vpRobotAfma6 class..." << std::endl;
-
- vpRobotAfma6 afma6;
- vpCameraParameters cam;
-
- std::cout << "-- Default settings for Afma6 ---"
- << std::endl;
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP);
-
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to CCMOP tool with distortion ------"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the gripper tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_GRIPPER);
-
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to gripper tool with distortion ------"
- << std::endl;
- afma6.init( vpAfma6::TOOL_GRIPPER,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << afma6 << std::endl;
- afma6.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
-#else
-int main()
-{
- std::cout << "The real Afma6 robot controller is not available." << std::endl;
- return 0;
-}
-
-#endif
diff --git a/test/servo-afma6/testRobotAfma6Pose.cpp b/test/servo-afma6/testRobotAfma6Pose.cpp
deleted file mode 100644
index 038212a..0000000
--- a/test/servo-afma6/testRobotAfma6Pose.cpp
+++ /dev/null
@@ -1,240 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobotAfma6Pose.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobotAfma6Pose.cpp
-
- Example of robot pose usage.
-
- Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
- rMc from the robot position.
-
-*/
-
-
-
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpRobotAfma6.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpPoint.h>
-#include <visp/vpDot.h>
-#include <visp/vpPose.h>
-#include <visp/vpDebug.h>
-#include <iostream>
-#if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394_2)
-
-int main()
-{
- try {
- // Create an image B&W container
- vpImage<unsigned char> I;
-
- // Create a firewire grabber based on libdc1394-2.x
- vp1394TwoGrabber g;
-
- // Grab an image from the firewire camera
- g.acquire(I);
-
- // Create an image viewer for the image
-#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
-#endif
-
- // Display the image
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- // Define a squared target
- // The target is made of 4 planar points (square dim = 0.077m)
- double sdim = 0.077; // square width and height
- vpPoint target[4] ;
- // Set the point world coordinates (x,y,z) in the object frame
- // o ----> x
- // |
- // |
- // \/
- // y
- target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
- target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
- target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
- target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
-
- // Image processing to extract the 2D coordinates in sub-pixels of the 4
- // points from the image acquired by the camera
- // Creation of 4 trackers
- vpDot dot[4];
- vpImagePoint cog;
- for (int i=0; i < 4; i ++) {
- dot[i].setGraphics(true); // to display the tracking results
- std::cout << "Click on dot " << i << std::endl;
- dot[i].initTracking( I );
- // The tracker computes the sub-pixels coordinates in the image
- // i ----> u
- // |
- // |
- // \/
- // v
- std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
- // Flush the tracking results in the viewer
- vpDisplay::flush(I) ;
- }
-
- // Create an intrinsic camera parameters structure
- vpCameraParameters cam;
-
- // Create a robot access
- vpRobotAfma6 robot;
-
- // Load the end-effector to camera frame transformation obtained
- // using a camera intrinsic model with distortion
- robot.init(vpAfma6::TOOL_CCMOP,
- vpCameraParameters::perspectiveProjWithDistortion);
-
- // Get the intrinsic camera parameters associated to the image
- robot.getCameraParameters(cam, I);
-
- // Using the camera parameters, compute the perspective projection (transform
- // the dot sub-pixel coordinates into coordinates in the camera frame in
- // meter)
- for (int i=0; i < 4; i ++) {
- double x=0, y=0 ; // coordinates of the dots in the camera frame
- // c ----> x
- // |
- // |
- // \/
- // y
- //pixel to meter conversion
- cog = dot[i].getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- target[i].set_x(x) ;
- target[i].set_y(y) ;
- }
-
- // From now, in target[i], we have the 3D coordinates of a point in the
- // object frame, and their correspondances in the camera frame. We can now
- // compute the pose cMo between the camera and the object.
- vpPose pose;
- // Add the 4 points to compute the pose
- for (int i=0; i < 4; i ++) {
- pose.addPoint(target[i]) ;
- }
- // Create an homogeneous matrix for the camera to object transformation
- // computed just bellow
- vpHomogeneousMatrix cMo;
- vpRotationMatrix R;
- vpRxyzVector r;
- // Compute the pose: initialisation is done by Lagrange method, and the final
- // pose is computed by the more accurate Virtual Visual Servoing method.
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
-
-
- std::cout << "Pose cMo: " << std::endl << cMo;
- cMo.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- // Get the robot position in the reference frame
- vpHomogeneousMatrix rMc;
- vpColVector p; // position x,y,z,rx,ry,rz
- robot.getPosition(vpRobotAfma6::REFERENCE_FRAME, p);
- std::cout << "Robot pose in reference frame: " << p << std::endl;
- vpTranslationVector t;
- t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
- r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
- R.buildFrom(r);
- rMc.buildFrom(t, R);
- std::cout << "Pose rMc: " << std::endl << rMc;
- rMc.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- robot.getPosition(vpRobotAfma6::ARTICULAR_FRAME, p);
- std::cout << "Robot pose in articular: " << p << std::endl;
-
- robot.get_fMc(p, rMc);
- std::cout << "Pose rMc from MGD: " << std::endl << rMc;
- rMc.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- vpHomogeneousMatrix rMo;
- rMo = rMc * cMo;
- std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
- rMo.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
-#else
-int main()
-{
- std::cout << "Sorry, test not valid. You should have an Afma6 robot..."
- << std::endl;
- return 0;
-}
-
-#endif
diff --git a/test/servo-viper/CMakeLists.txt b/test/servo-viper/CMakeLists.txt
deleted file mode 100644
index 13f8ba5..0000000
--- a/test/servo-viper/CMakeLists.txt
+++ /dev/null
@@ -1,73 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-SET (SOURCE
- testViper850.cpp
- testRobotViper850.cpp
- testRobotViper850Pose.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-# Add test
-add_test(testViper850 testViper850)
-add_test(testRobotViper850 testRobotViper850)
-
-# testRobotViper850Pose cannot be run automaticlly, since human interaction is
-# required to click on the dots for the pose computation
-#add_test(testRobotViper850Pose testRobotViper850Pose.cpp)
diff --git a/test/servo-viper/testRobotViper850.cpp b/test/servo-viper/testRobotViper850.cpp
deleted file mode 100644
index cad8251..0000000
--- a/test/servo-viper/testRobotViper850.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobotViper850.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobotViper850.cpp
-
- Example of a real robot control, the Viper850 robot (arm, with 6
- degrees of freedom).
-*/
-
-
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpDebug.h>
-#include <visp/vpCameraParameters.h>
-#include <iostream>
-#ifdef VISP_HAVE_VIPER850
-
-int main()
-{
- try {
-
- std::cout << "a test for vpRobotViper850 class..." << std::endl;
-
- vpRobotViper850 viper850;
- vpCameraParameters cam;
-
- std::cout << "-- Default settings for Viper850 ---"
- << std::endl;
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
- << std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
-
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
- << std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
-
- std::cout << "-- Current joint position:" << std::endl;
- vpColVector q;
- viper850.getPosition(vpRobot::ARTICULAR_FRAME, q);
- std::cout << " " << q.t() << std::endl;
-
- std::cout << "-- Current fMe:" << std::endl;
-
- vpHomogeneousMatrix fMe;
- viper850.get_fMe(q, fMe);
- vpTranslationVector t;
- fMe.extract(t);
- vpRotationMatrix R;
- fMe.extract(R);
- vpRzyzVector rzyz;
- rzyz.buildFrom(R);
-
- std::cout << "fMe:" << std::endl
- << "\tt: " << t.t() << std::endl
- << "\trzyz (deg): " << vpMath::deg(rzyz[0])
- << " " << vpMath::deg(rzyz[1])
- << " " << vpMath::deg(rzyz[2]) << std::endl;
-
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
-#else
-int main()
-{
- std::cout << "The real Viper850 robot controller is not available." << std::endl;
- return 0;
-}
-
-#endif
diff --git a/test/servo-viper/testRobotViper850Pose.cpp b/test/servo-viper/testRobotViper850Pose.cpp
deleted file mode 100644
index 9748bb8..0000000
--- a/test/servo-viper/testRobotViper850Pose.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/****************************************************************************
- *
- * $Id: testRobotViper850Pose.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Afma 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testRobotViper850Pose.cpp
-
- Example of robot pose usage.
-
- Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
- rMc from the robot position.
-
-*/
-
-
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpImage.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpRobotViper850.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpPoint.h>
-#include <visp/vpDot.h>
-#include <visp/vpPose.h>
-#include <visp/vpDebug.h>
-#include <iostream>
-#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2)
-
-int main()
-{
- try {
- // Create an image B&W container
- vpImage<unsigned char> I;
-
- // Create a firewire grabber based on libdc1394-2.x
- bool reset = false;
- vp1394TwoGrabber g(reset);
-
- // Grab an image from the firewire camera
- g.acquire(I);
-
- // Create an image viewer for the image
-#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
-#endif
-
- // Display the image
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- // Define a squared target
- // The target is made of 4 planar points (square dim = 0.077m)
- double sdim = 0.077; // square width and height
- vpPoint target[4] ;
- // Set the point world coordinates (x,y,z) in the object frame
- // o ----> x
- // |
- // |
- // \/
- // y
- target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
- target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
- target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
- target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
-
- // Image processing to extract the 2D coordinates in sub-pixels of the 4
- // points from the image acquired by the camera
- // Creation of 4 trackers
- vpDot dot[4];
- vpImagePoint cog;
- for (int i=0; i < 4; i ++) {
- dot[i].setGraphics(true); // to display the tracking results
- std::cout << "Click on dot " << i << std::endl;
- dot[i].initTracking( I );
- // The tracker computes the sub-pixels coordinates in the image
- // i ----> u
- // |
- // |
- // \/
- // v
- std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
- // Flush the tracking results in the viewer
- vpDisplay::flush(I) ;
- }
-
- // Create an intrinsic camera parameters structure
- vpCameraParameters cam;
-
- // Create a robot access
- vpRobotViper850 robot;
-
- // Load the end-effector to camera frame transformation obtained
- // using a camera intrinsic model with distortion
- robot.init(vpViper850::defaultTool,
- vpCameraParameters::perspectiveProjWithDistortion);
-
- // Get the intrinsic camera parameters associated to the image
- robot.getCameraParameters(cam, I);
-
- // Using the camera parameters, compute the perspective projection (transform
- // the dot sub-pixel coordinates into coordinates in the camera frame in
- // meter)
- for (int i=0; i < 4; i ++) {
- double x=0, y=0 ; // coordinates of the dots in the camera frame
- // c ----> x
- // |
- // |
- // \/
- // y
- //pixel to meter conversion
- cog = dot[i].getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- target[i].set_x(x) ;
- target[i].set_y(y) ;
- }
-
- // From now, in target[i], we have the 3D coordinates of a point in the
- // object frame, and their correspondances in the camera frame. We can now
- // compute the pose cMo between the camera and the object.
- vpPose pose;
- // Add the 4 points to compute the pose
- for (int i=0; i < 4; i ++) {
- pose.addPoint(target[i]) ;
- }
- // Create an homogeneous matrix for the camera to object transformation
- // computed just bellow
- vpHomogeneousMatrix cMo;
- vpRotationMatrix R;
- vpRxyzVector r;
- // Compute the pose: initialisation is done by Lagrange method, and the final
- // pose is computed by the more accurate Virtual Visual Servoing method.
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
-
-
- std::cout << "Pose cMo: " << std::endl << cMo;
- cMo.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- // Get the robot position in the reference frame
- vpHomogeneousMatrix rMc;
- vpColVector p; // position x,y,z,rx,ry,rz
- robot.getPosition(vpRobotViper850::REFERENCE_FRAME, p);
- std::cout << "Robot pose in reference frame: " << p << std::endl;
- vpTranslationVector t;
- t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
- r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
- R.buildFrom(r);
- rMc.buildFrom(t, R);
- std::cout << "Pose rMc: " << std::endl << rMc;
- rMc.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, p);
- std::cout << "Robot pose in articular: " << p << std::endl;
-
- robot.get_fMc(p, rMc);
- std::cout << "Pose rMc from MGD: " << std::endl << rMc;
- rMc.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- vpHomogeneousMatrix rMo;
- rMo = rMc * cMo;
- std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
- rMo.extract(R);
- r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
-
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- }
- return 0;
-}
-#else
-int main()
-{
- std::cout << "Sorry, test not valid. You should have an Viper850 robot..."
- << std::endl;
- return 0;
-}
-
-#endif
diff --git a/test/servo-viper/testViper850.cpp b/test/servo-viper/testViper850.cpp
deleted file mode 100644
index 20c35ab..0000000
--- a/test/servo-viper/testViper850.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/****************************************************************************
- *
- * $Id: testViper850.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test for Viper850 6 dof robot.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \example testViper850.cpp
-
- Example of a real robot control, the Viper robot (arm robot, with 6
- degrees of freedom).
-*/
-
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpViper850.h>
-#include <visp/vpDebug.h>
-
-#include <iostream>
-
-int main()
-{
- try {
-
- std::cout << "a test for vpViper850 class..." << std::endl;
-
- vpViper850 viper850;
- vpCameraParameters cam;
-
- std::cout << "-- Default settings for Viper 850 ---"
- << std::endl;
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
- << std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
-
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
- << std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
- std::cout << viper850 << std::endl;
- viper850.getCameraParameters(cam, 640, 480);
- std::cout << cam << std::endl;
-
- vpColVector qmotor(6);
-#if 1
- qmotor[0] = vpMath::rad(30);
- qmotor[1] = vpMath::rad(-100);
- qmotor[2] = vpMath::rad(180);
- qmotor[3] = vpMath::rad(20);
- qmotor[4] = vpMath::rad(90);
- qmotor[5] = vpMath::rad(13.37);
-#else
- qmotor[0] = vpMath::rad(0);
- qmotor[1] = vpMath::rad(0-90);
- qmotor[2] = vpMath::rad(0);
- qmotor[3] = vpMath::rad(0);
- qmotor[4] = vpMath::rad(0);
- qmotor[5] = vpMath::rad(0);
-#endif
- vpHomogeneousMatrix fMe;
- viper850.get_fMe(qmotor, fMe);
-
- vpTranslationVector t;
- fMe.extract(t);
- vpRotationMatrix R;
- fMe.extract(R);
- vpRzyzVector r;
- r.buildFrom(R);
-
- std::cout << "fMe:" << std::endl
- << "\tt: " << t.t() << std::endl
- << "\trzyz (rad): " << r.t() << std::endl
- << "\trzyz (deg): " << vpMath::deg(r[0])
- << " " << vpMath::deg(r[1])
- << " " << vpMath::deg(r[2]) << std::endl;
-
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
diff --git a/test/tools/CMakeLists.txt b/test/tools/CMakeLists.txt
deleted file mode 100644
index b1437df..0000000
--- a/test/tools/CMakeLists.txt
+++ /dev/null
@@ -1,54 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 5255 2015-02-03 14:52:43Z strinh $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# Set SRC_SUBDIRS variable to all the sub directories we want to parse during
-# the build process.
-#
-# If you add/remove a directory, modify here
-SET (SRC_SUBDIRS
- convert
- io
- geometry
- time
- xml
-)
-
-# Build process propagation in the sub directories
-SUBDIRS(${SRC_SUBDIRS})
diff --git a/test/tools/convert/CMakeLists.txt b/test/tools/convert/CMakeLists.txt
deleted file mode 100644
index 4a27cab..0000000
--- a/test/tools/convert/CMakeLists.txt
+++ /dev/null
@@ -1,68 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testConvert.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-
diff --git a/test/tools/convert/testConvert.cpp b/test/tools/convert/testConvert.cpp
deleted file mode 100644
index 92d8b85..0000000
--- a/test/tools/convert/testConvert.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/****************************************************************************
- *
- * $Id: testIoTools.cpp 5210 2015-01-26 10:51:11Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test functions in vpIoTools.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-
-/*!
-
- \example testConvert.cpp
-
- \brief Test functions in Convert.
-
-*/
-
-#include <iostream> // std::cout
-#include <limits> // std::numeric_limits
-#include <visp/vpConfig.h>
-#include <visp/vpConvert.h>
-
-
-bool areSame(double a, double b) {
- return fabs(a - b) < std::numeric_limits<double>::epsilon();
-}
-
-void testConvertFromImagePointToPoint2f() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- vpImagePoint imPt1(12.5f, .85f);
- vpImagePoint imPt2(-44.26f, 125.11f);
- vpImagePoint imPt3(0.0f, -1.756e-10f);
-
- cv::Point2f pt1, pt2, pt3;
- vpConvert::convertToOpenCV(imPt1, pt1);
- vpConvert::convertToOpenCV(imPt2, pt2);
- vpConvert::convertToOpenCV(imPt3, pt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
-
- std::vector<vpImagePoint> listOfImPts(3);
- listOfImPts[0] = imPt1;
- listOfImPts[1] = imPt2;
- listOfImPts[2] = imPt3;
-
- std::vector<cv::Point2f> listOfPts;
- vpConvert::convertToOpenCV(listOfImPts, listOfPts);
-
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromImagePointToPoint2f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromPoint2fToImagePoint() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- vpImagePoint imPt1, imPt2, imPt3;
-
- cv::Point2f pt1(12.5f, .85f), pt2(-44.26f, 125.11f), pt3(0.0f, -1.756e-10f);
- vpConvert::convertFromOpenCV(pt1, imPt1);
- vpConvert::convertFromOpenCV(pt2, imPt2);
- vpConvert::convertFromOpenCV(pt3, imPt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
-
- std::vector<vpImagePoint> listOfImPts;
-
- std::vector<cv::Point2f> listOfPts(3);
- listOfPts[0] = pt1;
- listOfPts[1] = pt2;
- listOfPts[2] = pt3;
-
- vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
-
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromPoint2fToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromImagePointToPoint2d() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- vpImagePoint imPt1(12.5, .85);
- vpImagePoint imPt2(-44.26, 125.11);
- vpImagePoint imPt3(0, -1.756e-10);
-
- cv::Point2d pt1, pt2, pt3;
- vpConvert::convertToOpenCV(imPt1, pt1);
- vpConvert::convertToOpenCV(imPt2, pt2);
- vpConvert::convertToOpenCV(imPt3, pt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
-
- std::vector<vpImagePoint> listOfImPts(3);
- listOfImPts[0] = imPt1;
- listOfImPts[1] = imPt2;
- listOfImPts[2] = imPt3;
-
- std::vector<cv::Point2d> listOfPts;
- vpConvert::convertToOpenCV(listOfImPts, listOfPts);
-
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromImagePointToPoint2d=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromPoint2dToImagePoint() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- vpImagePoint imPt1, imPt2, imPt3;
-
- cv::Point2d pt1(12.5, .85), pt2(-44.26, 125.11), pt3(0, -1.756e-10);
- vpConvert::convertFromOpenCV(pt1, imPt1);
- vpConvert::convertFromOpenCV(pt2, imPt2);
- vpConvert::convertFromOpenCV(pt3, imPt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
-
- std::vector<vpImagePoint> listOfImPts;
-
- std::vector<cv::Point2d> listOfPts(3);
- listOfPts[0] = pt1;
- listOfPts[1] = pt2;
- listOfPts[2] = pt3;
-
- vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
-
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromPoint2dToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromKeyPointToImagePoint() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- cv::KeyPoint kp1(12.5f, .85f, 0), kp2(-44.26f, 125.11f, 0), kp3(0.0f, -1.756e-10f, 0);
- vpImagePoint imPt1, imPt2, imPt3;
-
- vpConvert::convertFromOpenCV(kp1, imPt1);
- vpConvert::convertFromOpenCV(kp2, imPt2);
- vpConvert::convertFromOpenCV(kp3, imPt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), kp1.pt.x) && areSame(imPt1.get_v(), kp1.pt.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), kp2.pt.x) && areSame(imPt2.get_v(), kp2.pt.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), kp3.pt.x) && areSame(imPt3.get_v(), kp3.pt.y)) nbOk++; else nbNOk++;
-
- std::vector<cv::KeyPoint> listOfKeyPoints(3);
- listOfKeyPoints[0] = kp1;
- listOfKeyPoints[1] = kp2;
- listOfKeyPoints[2] = kp3;
-
- std::vector<vpImagePoint> listOfImPts;
- vpConvert::convertFromOpenCV(listOfKeyPoints, listOfImPts);
-
- if(listOfImPts.size() == listOfKeyPoints.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfKeyPoints[i].pt.x) && areSame(listOfImPts[i].get_v(), listOfKeyPoints[i].pt.y)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromKeyPointToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromPoint3fToPoint() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- cv::Point3f pt1(12.5f, .85f, 110.0f), pt2(-44.26f, 125.11f, -98e2f), pt3(0.0f, -1.756e-10f, 0.00015f);
- vpPoint point1, point2, point3;
-
- vpConvert::convertFromOpenCV(pt1, point1);
- vpConvert::convertFromOpenCV(pt2, point2);
- vpConvert::convertFromOpenCV(pt3, point3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
-
- std::vector<cv::Point3f> listOfPoints3f(3);
- listOfPoints3f[0] = pt1;
- listOfPoints3f[1] = pt2;
- listOfPoints3f[2] = pt3;
-
- std::vector<vpPoint> listOfPts;
- vpConvert::convertFromOpenCV(listOfPoints3f, listOfPts);
-
- if(listOfPoints3f.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromPoint3fToPoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-void testConvertFromPointToPoint3f() {
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- cv::Point3f pt1, pt2, pt3;
- vpPoint point1, point2, point3;
- point1.set_oX(12.5f);
- point1.set_oY(.85f);
- point1.set_oZ(110.0f);
-
- point2.set_oX(-44.26f);
- point2.set_oY(125.11f);
- point2.set_oZ(-98e2f);
-
- point3.set_oX(0.0f);
- point3.set_oY(-1.756e-10f);
- point3.set_oZ(0.00015f);
-
- vpConvert::convertToOpenCV(point1, pt1);
- vpConvert::convertToOpenCV(point2, pt2);
- vpConvert::convertToOpenCV(point3, pt3);
-
- int nbOk = 0, nbNOk = 0;
- if(areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
-
- std::vector<cv::Point3f> listOfPoints3f;
- std::vector<vpPoint> listOfPts(3);
- listOfPts[0] = point1;
- listOfPts[1] = point2;
- listOfPts[2] = point3;
-
- vpConvert::convertToOpenCV(listOfPts, listOfPoints3f);
-
- if(listOfPoints3f.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
- }
- } else {
- nbNOk += 3;
- }
-
- std::cout << "testConvertFromPointToPoint3f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl;
-#endif
-}
-
-int main() {
- testConvertFromImagePointToPoint2f();
- testConvertFromPoint2fToImagePoint();
- testConvertFromImagePointToPoint2d();
- testConvertFromPoint2dToImagePoint();
-
- testConvertFromKeyPointToImagePoint();
- testConvertFromPoint3fToPoint();
- testConvertFromPointToPoint3f();
- return 0;
-}
diff --git a/test/tools/geometry/CMakeLists.txt b/test/tools/geometry/CMakeLists.txt
deleted file mode 100644
index bc22f00..0000000
--- a/test/tools/geometry/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 2897 2010-11-04 11:17:58Z rtallonn $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testPolygon.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary} -c ${OPTION_TO_DESACTIVE_DISPLAY})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/tools/geometry/testPolygon.cpp b/test/tools/geometry/testPolygon.cpp
deleted file mode 100755
index 3a9f7c7..0000000
--- a/test/tools/geometry/testPolygon.cpp
+++ /dev/null
@@ -1,255 +0,0 @@
-/****************************************************************************
- *
- * $Id: testPolygon.cpp 5004 2014-11-24 08:24:18Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Example which test the polygon.
- *
- * Author:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-
-#include <visp/vpConfig.h>
-#include <visp/vpPolygon.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpImagePoint.h>
-
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-
-
-#include <math.h>
-
-#include <iostream>
-#include <string>
-#include <vector>
-
-//! List of allowed command line options
-#define GETOPTARGS "cdh"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, bool& opt_display, bool& opt_click);
-
-/*!
-
-Print the program options.
-
-\param name : Program name.
-\param badparam : Bad parameter name.
-
- */
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-test the generic 2D polygons.\n\
-\n\
-SYNOPSIS\n\
- %s [-c] [-d] [-h]\n \
-", name);
-
- fprintf(stdout, "\n\
-OPTIONS: \n\
- -c \n\
- Disable mouse click.\n\
-\n\
- -d \n\
- Turn off display.\n\
-\n\
- -h\n\
- Print the help.\n\n");
-
- if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
- }
-}
-
-/*!
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param opt_display : optionnal flag to turn off the display.
- \param opt_click : activates the mouse click.
- \return false if the program has to be stopped, true otherwise.
-*/
-bool getOptions(int argc, const char **argv, bool& opt_display, bool& opt_click)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': opt_click = false; break;
- case 'd': opt_display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-
-/* -------------------------------------------------------------------------- */
-/* MAIN FUNCTION */
-/* -------------------------------------------------------------------------- */
-
-int
-main(int argc, const char** argv)
-{
- try {
- bool opt_display = true;
- bool opt_click = true;
- vpImage<unsigned char> I(480, 640, 255);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_display, opt_click) == false) {
- return (-1);
- }
-
- std::vector <vpImagePoint> vec1;
- vec1.push_back(vpImagePoint(200, 200));
- vec1.push_back(vpImagePoint(200, 400));
- vec1.push_back(vpImagePoint(320, 400));
- vec1.push_back(vpImagePoint(380, 300));
- vec1.push_back(vpImagePoint(280, 280));
- vpPolygon p1;
- p1.buildFrom(vec1);
-
- std::vector <vpImagePoint> vec2;
- vec2.push_back(vpImagePoint(20, 20));
- vec2.push_back(vpImagePoint(100, 20));
- vec2.push_back(vpImagePoint(100, 100));
- vec2.push_back(vpImagePoint(20, 100));
- vpPolygon p2(vec2);
-
-
- std::vector <vpImagePoint> vec3;
- vpPolygon p3(vec3);
-
-#if defined VISP_HAVE_X11
- vpDisplayX display;
-#elif defined VISP_HAVE_GTK
- vpDisplayGTK display;
-#elif defined VISP_HAVE_GDI
- vpDisplayGDI display;
-#else
- opt_display = false;
-#endif
-
- std::cout << " Polygon 1 : " << std::endl;
- std::cout << " area : " << p1.getArea() << std::endl;
- std::cout << " center : " << p1.getCenter() << std::endl << std::endl;
-
- std::cout << " Polygon 2 : " << std::endl;
- std::cout << " area : " << p2.getArea() << std::endl;
- std::cout << " center : " << p2.getCenter() << std::endl << std::endl;
-
- std::cout << " Polygon 3 : " << std::endl;
- std::cout << " area : " << p3.getArea() << std::endl;
- std::cout << " center : " << p3.getCenter() << std::endl;
-
-
- if(opt_display){
-#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || (defined VISP_HAVE_GDI)
- display.init(I, 10, 10, "Test vpPolygon");
-#endif
- vpDisplay::display(I);
- p1.display(I, vpColor::green, 1);
- vpDisplay::displayCross(I, p1.getCenter(), 5, vpColor::green);
- p2.display(I, vpColor::red, 1);
- vpDisplay::displayCross(I, p2.getCenter(), 5, vpColor::red);
- p3.display(I, vpColor::blue, 1);
- vpDisplay::displayCross(I, p3.getCenter(), 5, vpColor::lightBlue);
- vpDisplay::displayText(I, vpImagePoint(10, 10), "Click to finish", vpColor::red);
- vpDisplay::flush(I);
-
- if (opt_click)
- vpDisplay::getClick(I);
-
-
- vpDisplay::display(I);
- vpDisplay::displayText(I, vpImagePoint(10, 10), "Left click to add a point", vpColor::red);
- vpDisplay::displayText(I, vpImagePoint(20, 10), "Right click to build the polygon", vpColor::red);
- vpDisplay::flush(I);
- if (opt_click) {
- vpPolygon p4;
- p4.initClick(I);
- p4.display(I, vpColor::green, 1);
- std::cout << std::endl;
- std::cout << " Polygon 4 : " << std::endl;
- std::cout << " area : " << p4.getArea() << std::endl;
- std::cout << " center : " << p4.getCenter() << std::endl;
- std::cout << "Click to continue." << std::endl;
- vpDisplay::flush(I);
- vpDisplay::getClick(I);
-
- vpRect bbox = p4.getBoundingBox();
- for(unsigned int i= (unsigned int)floor(bbox.getTop()); i<(unsigned int)ceil(bbox.getBottom()); ++i){
- for(unsigned int j=(unsigned int)floor(bbox.getLeft()); j<(unsigned int)ceil(bbox.getRight()); ++j){
- if(p4.isInside(vpImagePoint(i, j))){
- vpDisplay::displayPoint(I, vpImagePoint(i, j), vpColor::orange);
- }
- }
- }
- vpDisplay::flush(I);
- std::cout << "Click to finish." << std::endl;
-
- vpDisplay::getClick(I);
- }
- }
-
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-
diff --git a/test/tools/io/CMakeLists.txt b/test/tools/io/CMakeLists.txt
deleted file mode 100644
index 6d36807..0000000
--- a/test/tools/io/CMakeLists.txt
+++ /dev/null
@@ -1,68 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testIoTools.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-
diff --git a/test/tools/io/testIoTools.cpp b/test/tools/io/testIoTools.cpp
deleted file mode 100644
index 871127c..0000000
--- a/test/tools/io/testIoTools.cpp
+++ /dev/null
@@ -1,446 +0,0 @@
-/****************************************************************************
- *
- * $Id: testIoTools.cpp 5210 2015-01-26 10:51:11Z strinh $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test functions in vpIoTools.
- *
- * Authors:
- * Souriya Trinh
- *
- *****************************************************************************/
-
-/*!
-
- \example testIoTools.cpp
-
- \brief Test functions in IoTools.
-
-*/
-
-#include <stdio.h>
-#include <string.h>
-#include <iostream>
-#include <visp/vpIoTools.h>
-
-
-int
-main(int argc, const char ** argv)
-{
- const char c = vpIoTools::separator;
- if(c == '\\')
- {
- std::cout << "The directory separator character is '" << c << "' (Windows platform)." << std::endl;
- }
- else {
- std::cout << "The directory separator character is '" << c << "' (Unix like platform)." << std::endl;
- }
-
-
- std::string pathname = "";
-#if defined(_WIN32)
- pathname = "C:\\Program Files (x86)\\Java\\jre7";
-#else
- pathname = "/usr/bin/java";
-#endif
-
- std::cout << "Parent of " << pathname << " is " << vpIoTools::getParent(pathname) << std::endl;
- std::cout << "Name of " << pathname << " is " << vpIoTools::getName(pathname) << std::endl;
-
-
- if(argc == 3 && std::string(argv[1]) == std::string("-i"))
- {
- std::cout << "Parent of " << argv[2] << " is " << vpIoTools::getParent(argv[2]) << std::endl;
- std::cout << "Name of " << argv[2] << " is " << vpIoTools::getName(argv[2]) << std::endl;
- }
-
- std::string windowsPathnameStyle = "\\usr\\bin\\java";
- std::cout << "Parent of " << windowsPathnameStyle << " is " << vpIoTools::getParent(windowsPathnameStyle) << std::endl;
- std::cout << "Name of " << windowsPathnameStyle << " is " << vpIoTools::getName(windowsPathnameStyle) << std::endl;
-
- std::string parent = "/usr/toto/", child = "\\blabla\\java";
- std::cout << "parent=" << vpIoTools::path(parent) << " ; child=" << vpIoTools::path(child) << std::endl;
- std::cout << "Create file path from parent=" << parent << " and child=" << child << " is "
- << vpIoTools::createFilePath(parent, child) << std::endl;
-
- std::string expandPath = "~/Documents/fictional directory/fictional file";
- std::cout << "Path for " << expandPath << " is " << vpIoTools::path(expandPath) << std::endl;
-
- std::cout << "Test get name with an empty pathname=" << vpIoTools::getName("") << std::endl;
- std::cout << "Get parent with an empty pathname=" << vpIoTools::getParent("") << std::endl;
- std::cout << "Get parent with a filename=" << vpIoTools::getParent("my_file.txt") << std::endl;
- expandPath = "~/Documents/fictional dir/fictional file.txt";
- std::cout << "Get name with a unix expand pathname " << expandPath << "=" << vpIoTools::getName(expandPath) << std::endl;
- std::cout << "Get parent with a unix expand pathname " << expandPath << "=" << vpIoTools::getParent(expandPath) << std::endl;
-
-
- pathname = "c:/dir";
- std::cout << "pathname=" << vpIoTools::splitDrive(pathname).first << " ; " << vpIoTools::splitDrive(pathname).second << std::endl;
-
- std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
- pathname = "c:/dir/fictional directory/fictional file.txt";
- std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
- pathname = "/home/user/Documents/fictional directory/fictional file.txt";
- std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
- pathname = "~/Documents/fictional directory/fictional file.txt";
- std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
- pathname = "fictional directory/fictional file.txt";
- std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
-
- //Test vpIoTools::splitDrive
- unsigned int nbFail = 0, nbOk = 0;
-#if defined(_WIN32)
- if(strcmp(vpIoTools::splitDrive("c:\\foo\\bar").first.c_str(), "c:") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").first << " should be=c:" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("c:\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").second << " should be=\\foo\\bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("c:/foo/bar").first.c_str(), "c:") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").first << " should be=c:" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("c:/foo/bar").second.c_str(), "/foo/bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").second << " should be=/foo/bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "\\\\conky\\mountpoint") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first << " should be=\\\\conky\\mountpoint" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").second << " should be=\\foo\\bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("//conky/mountpoint/foo/bar").first.c_str(), "//conky/mountpoint") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("//conky/mountpoint/foo/bar").first << " should be=//conky/mountpoint" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("//conky/mountpoint/foo/bar").second.c_str(), "/foo/bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("//conky/mountpoint/foo/bar").second << " should be=/foo/bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").first << " should be=" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").second.c_str(),
- "\\\\\\conky\\mountpoint\\foo\\bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\\\conky\\mountpoint\\foo\\bar").second << " should be=\\\\\\conky\\mountpoint\\foo\\bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").first.c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("///conky/mountpoint/foo/bar").first << " should be=" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second.c_str(), "///conky/mountpoint/foo/bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second << " should be=///conky/mountpoint/foo/bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").first.c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").first << " should be=" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").second.c_str(),
- "\\\\conky\\\\mountpoint\\foo\\bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("\\\\conky\\\\mountpoint\\foo\\bar").second << " should be=\\\\conky\\\\mountpoint\\foo\\bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").first.c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("//conky//mountpoint/foo/bar").first << " should be=" << std::endl;
- }
- if(strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second.c_str(), "//conky//mountpoint/foo/bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second << " should be=//conky//mountpoint/foo/bar" << std::endl;
- }
-
- std::cout << "Test vpIoTools::splitDrive (Win32) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
-#endif
-
-
- //Test vpIoTools::getFileExtension
-#if defined(_WIN32)
- nbFail = 0;
- nbOk = 0;
-
- if(strcmp(vpIoTools::getFileExtension("foo.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("/foo/foo.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("/foo/foo.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension(".ext").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension(".ext") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("\\foo.ext\\foo").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("\\foo.ext\\foo") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("foo.ext\\").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext\\") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("foo.bar.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("xx/foo.bar.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("xx/foo.bar.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("xx\\foo.bar.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("xx\\foo.bar.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("c:a/b\\c.d").c_str(), ".d") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("c:a/b\\c.d") << " should be=.d" << std::endl;
- }
-
- std::cout << "Test vpIoTools::getFileExtension (WIN32 platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
-#else
- nbFail = 0;
- nbOk = 0;
-
- if(strcmp(vpIoTools::getFileExtension("foo.bar").c_str(), ".bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar") << " should be=.bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("foo.boo.bar").c_str(), ".bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.bar") << " should be=.bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("foo.boo.biff.bar").c_str(), ".bar") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.biff.bar") << " should be=.bar" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension(".csh.rc").c_str(), ".rc") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension(".csh.rc") << " should be=.rc" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("nodots").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("nodots") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension(".cshrc").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension(".cshrc") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("...manydots").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("...manydots.ext").c_str(), ".ext") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots.ext") << " should be=.ext" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension(".").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension(".") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("..").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("..") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("........").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("........") << " should be=" << std::endl;
- }
-
- if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
- nbOk++;
- }
- else {
- nbFail++;
- std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl;
- }
-
- std::cout << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
-#endif
-
-
- std::cout << std::endl << "End" << std::endl;
-
- return 0;
-}
diff --git a/test/tools/time/CMakeLists.txt b/test/tools/time/CMakeLists.txt
deleted file mode 100644
index 380fb69..0000000
--- a/test/tools/time/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testTime.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/tools/time/testTime.cpp b/test/tools/time/testTime.cpp
deleted file mode 100644
index 259d9e6..0000000
--- a/test/tools/time/testTime.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-/****************************************************************************
- *
- * $Id: testTime.cpp 4658 2014-02-09 09:50:14Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Time management.
- *
- * Authors:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
-
- \example testTime.cpp
-
- \brief Time management.
-
-*/
-#include <visp/vpConfig.h>
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
-#elif defined(_WIN32)
-# include <windows.h>
-# include <mmsystem.h>
-# include <winbase.h>
-#endif
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
-#include <time.h>
-#include <visp/vpTime.h>
-#include <visp/vpParseArgv.h>
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv);
-
-// List of allowed command line options
-#define GETOPTARGS "h"
-
-/*!
-
- Print the program options.
-
-*/
-void usage(const char *name, const char *badparam)
-{
- fprintf(stdout, "\n\
-Time management.\n\
-\n\
-SYNOPSIS\n\
- %s [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -h\n\
- Print the help.\n");
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-}
-/*!
-
- Set the program options.
-
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
-
- default:
- usage(argv[0], optarg_);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- // Read the command line options
- if (getOptions(argc, argv) == false) {
- exit (-1);
- }
-
- double v = 0;
-
- double t0 = vpTime::measureTimeMs();
- for (int i =0 ; i < 100000; i ++)
- for (int j =0 ; j < 100; j ++)
- v = i * 2 / 3. + j;
- std::cout << "Computed dummy value: " << v << std::endl;
-
- double t1 = vpTime::measureTimeMs();
- vpTime::wait(t1, 40);
-
- double t2 = vpTime::measureTimeMs();
-
- // Sleep 10ms
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep(10*1000);
-#elif defined(_WIN32)
- Sleep(10);
-#endif
-
- double t3 = vpTime::measureTimeMs();
-
- // Sleep 2ms
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep(2*1000);
-#elif defined(_WIN32)
- Sleep(2);
-#endif
- double t4 = vpTime::measureTimeMs();
-
- vpTime::wait(t4, 19);
-
- double t5 = vpTime::measureTimeMs();
-
- vpTime::wait(5);
-
- double t6 = vpTime::measureTimeMs();
-
- vpTime::wait(21);
-
- double t7 = vpTime::measureTimeMs();
-
- vpTime::wait(2);
-
- double t8 = vpTime::measureTimeMs();
-
- std::cout << "t1-t0: computation time: " << t1 - t0 << std::endl;
- std::cout << "t2-t1: wait(t1, 40 ms): " << t2 - t1 << std::endl;
- std::cout << "t3-t2: sleep(10 ms): " << t3 - t2 << std::endl;
- std::cout << "t4-t3: sleep(2 ms): " << t4 - t3 << std::endl;
- std::cout << "t5-t4: wait(t, 19 ms): " << t5 - t4 << std::endl;
- std::cout << "t6-t5: wait(5 ms): " << t6 - t5 << std::endl;
- std::cout << "t7-t6: wait(21 ms): " << t7 - t6 << std::endl;
- std::cout << "t8-t7: wait(2 ms): " << t8 - t7 << std::endl;
-
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/tools/xml/CMakeLists.txt b/test/tools/xml/CMakeLists.txt
deleted file mode 100644
index 1738304..0000000
--- a/test/tools/xml/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testXmlParser.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/tools/xml/testXmlParser.cpp b/test/tools/xml/testXmlParser.cpp
deleted file mode 100755
index d0f0740..0000000
--- a/test/tools/xml/testXmlParser.cpp
+++ /dev/null
@@ -1,388 +0,0 @@
-/****************************************************************************
- *
- * $Id: testXmlParser.cpp 5126 2015-01-05 22:07:11Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Example which describes how to use the xml parser class.
- *
- * Author:
- * Romain Tallonneau
- *
- *****************************************************************************/
-
-
-/*!
- \example testXmlParser.cpp
-
- XML parser example.
-
- This example contains the declaration of a class used to read and write data
- in a xml file like:
- \code
- <config>
- <range>5.5</range>
- <step>7</step>
- <size_filter>3</size_filter>
- <name>Object</name>
- </config>
- \endcode
-
-*/
-
-
-#include <visp/vpConfig.h>
-
-#include <iostream>
-#if defined(VISP_HAVE_XML2)
-
-#include <visp/vpXmlParser.h>
-#include <visp/vpDebug.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpParseArgv.h>
-
-#include <string>
-
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-/* -------------------------------------------------------------------------- */
-/* CLASS EXAMPLE */
-/* -------------------------------------------------------------------------- */
-
-/*!
- \class vpExampleDataParser
- \brief Class example used to show how to implement a xml parser based on the
- vpXmlParser
-*/
-class vpExampleDataParser: public vpXmlParser
-{
-protected:
- double m_range;
- int m_step;
- int m_size_filter;
- std::string m_name;
-
- typedef enum{
- config,
- range,
- step,
- size_filter,
- name
- }dataToParse;
-
-
-public:
- vpExampleDataParser();
- virtual ~vpExampleDataParser();
-
- // Data accessors.
- double getRange() const {return m_range;}
- int getStep() const {return m_step;}
- int getSizeFilter() const {return m_size_filter;}
- std::string getName() const {return m_name;}
-
- void setRange(const double _range) {m_range = _range;}
- void setStep(const int _step) {m_step = _step;}
- void setSizeFilter(const int _size_filter) {m_size_filter = _size_filter;}
- void setName(const std::string& _name) { m_name = _name;}
-
-protected:
- virtual void readMainClass (xmlDocPtr doc, xmlNodePtr node);
- virtual void writeMainClass (xmlNodePtr node);
-};
-
-/*!
- Constructor.
- Initialise the map according to the data to parse, and initialise data to
- default values.
-
-*/
-vpExampleDataParser::vpExampleDataParser()
- : m_range(0.), m_step(0), m_size_filter(0), m_name("")
-{
- nodeMap["config"] = config;
- nodeMap["range"] = range;
- nodeMap["step"] = step;
- nodeMap["size_filter"] = size_filter;
- nodeMap["name"] = name;
-}
-
-/*!
- Destructor.
-
-*/
-vpExampleDataParser::~vpExampleDataParser()
-{
-
-}
-
-/*!
- Read the main class. This method corresponds to the parsing of the main
- document (which contains the whole data in the class). At this point, the
- document exists and is open.
-
- \param doc : Pointer to the document to parse.
- \param node : Pointer to the root node of the document.
-*/
-void
-vpExampleDataParser::readMainClass (xmlDocPtr doc, xmlNodePtr node)
-{
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case range:
- this->m_range = xmlReadDoubleChild(doc, dataNode);
- break;
- case step:
- this->m_step = xmlReadIntChild(doc, dataNode);
- break;
- case size_filter:
- this->m_size_filter = xmlReadIntChild(doc, dataNode);
- break;
- case name:{
- this->m_name = xmlReadStringChild(doc, dataNode);
- }break;
- default:
- vpTRACE("unknown tag in readConfigNode : %d, %s", iter_data->second, (iter_data->first).c_str());
- break;
- }
- }
- }
- }
-}
-
-/*!
- Write the data in the file.
- The file has already been opened or created in the save() method. And the root
- node (corresponding to the main tag) has already been writen.
-
- \param node : Pointer to the root node.
-*/
-void
-vpExampleDataParser::writeMainClass(xmlNodePtr node)
-{
- xmlWriteDoubleChild(node, (const char*)"range", m_range);
- xmlWriteIntChild(node, (const char*)"step", m_step);
- xmlWriteIntChild(node, (const char*)"size_filter", m_size_filter);
- xmlWriteCharChild(node, (const char*)"name", m_name.c_str());
-}
-
-
-#endif // doxygen
-
-/* -------------------------------------------------------------------------- */
-/* COMMAND LINE OPTIONS */
-/* -------------------------------------------------------------------------- */
-
-// List of allowed command line options
-#define GETOPTARGS "o:h"
-
-void usage(const char *name, const char *badparam, const std::string& opath, const std::string& user);
-bool getOptions(int argc, const char **argv, std::string &opath, const std::string& user);
-
-/*!
-
-Print the program options.
-
-\param name : Program name.
-\param badparam : Bad parameter name.
-\param opath : Output image path.
-\param user : Username.
-
- */
-void usage(const char *name, const char *badparam, const std::string& opath, const std::string& user)
-{
- fprintf(stdout, "\n\
-Write and read data in a xml file.\n\
- \n\
-SYNOPSIS\n\
- %s [-o <output image path>] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -o <output data path> %s\n\
- Set data output path.\n\
- From this directory, creates the \"%s\"\n\
- subdirectory depending on the username, where \n\
- dataTestXml.xml file is written.\n\
- \n\
- -h\n\
- Print the help.\n\n", opath.c_str(), user.c_str());
-
- if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
- }
-}
-
-/*!
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param opath : Output data path.
- \param user : Username.
- \return false if the program has to be stopped, true otherwise.
-*/
-bool getOptions(int argc, const char **argv, std::string &opath, const std::string& user)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, opath, user); return false; break;
-
- default:
- usage(argv[0], optarg_, opath, user); return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, opath, user);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-
-/* -------------------------------------------------------------------------- */
-/* MAIN FUNCTION */
-/* -------------------------------------------------------------------------- */
-
-int main(int argc, const char** argv)
-{
- try {
- std::string opt_opath;
- std::string opath;
- std::string filename;
- std::string username;
-
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << " testXmlParser.cpp" <<std::endl << std::endl ;
- std::cout << " writing and readind data using a xml parser" << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
-
- // Set the default output path
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- opt_opath = "/tmp";
-#elif defined(_WIN32)
- opt_opath = "C:\\temp";
-#endif
-
- // Get the user login name
- vpIoTools::getUserName(username);
-
- // Read the command line options
- if (getOptions(argc, argv, opt_opath, username) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_opath.empty())
- opath = opt_opath;
-
- // Append to the output path string, the login name of the user
- std::string dirname = vpIoTools::createFilePath(opath, username);
-
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(dirname) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(dirname);
- }
- catch (...) {
- usage(argv[0], NULL, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot create " << dirname << std::endl;
- std::cerr << " Check your -o " << opath << " option " << std::endl;
- exit(-1);
- }
- }
-
- filename = dirname + vpIoTools::path("/") + "dataTestXml.xml";
-
- // Write data using a parser.
- {
- vpExampleDataParser parser1;
-
- // Acquire data from measurments or tests.
- parser1.setRange(3.5);
- parser1.setStep(2);
- parser1.setSizeFilter(5);
- parser1.setName("cube");
-
- std::cout << "Write data to " << filename << std::endl;
- parser1.save(filename);
- }
-
- // Read data using another parser.
- {
- vpExampleDataParser parser2;
-
- parser2.parse(filename);
-
- std::cout << "Read from " << filename << std::endl ;
- std::cout << "Range : " << parser2.getRange() << std::endl;
- std::cout << "Step : " << parser2.getStep() << std::endl;
- std::cout << "Filter size : " << parser2.getSizeFilter() << std::endl;
- std::cout << "name : " << parser2.getName() << std::endl;
- }
-
- // Clean up memory allocated by the xml library
- vpXmlParser::cleanup();
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-#else
-
-int main()
-{
- std::cout << "Xml parser requires libxml2." << std::endl;
- return 0;
-}
-#endif
diff --git a/test/tracking/CMakeLists.txt b/test/tracking/CMakeLists.txt
deleted file mode 100644
index 9d1bec4..0000000
--- a/test/tracking/CMakeLists.txt
+++ /dev/null
@@ -1,66 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testTrackDot.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- # Add test
- add_test(${binary} ${binary} -c ${OPTION_TO_DESACTIVE_DISPLAY})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
diff --git a/test/tracking/testTrackDot.cpp b/test/tracking/testTrackDot.cpp
deleted file mode 100644
index 2af1586..0000000
--- a/test/tracking/testTrackDot.cpp
+++ /dev/null
@@ -1,319 +0,0 @@
-/****************************************************************************
- *
- * $Id: testTrackDot.cpp 4814 2014-07-31 11:38:39Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Test auto detection of dots.
- *
- * Authors:
- * Eric Marchand
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-
-#include <visp/vpDebug.h>
-#include <visp/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
-#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDot2.h>
-#include <visp/vpFeatureEllipse.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpIoTools.h>
-
-/*!
- \example testTrackDot.cpp
-
- \brief Test dot tracking on an image sequence by using vpDot.
-*/
-
-// List of allowed command line options
-#define GETOPTARGS "cdi:h"
-
-bool getOptions(int argc, const char **argv, std::string &ipath,
- bool &click_allowed, bool &display);
-
-void usage(const char *name, const char *badparam, std::string ipath);
-/*!
-
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
-
-*/
-void usage(const char *name, const char *badparam, std::string ipath)
-{
- fprintf(stdout, "\n\
-Test dot tracking.\n\
-\n\
-SYNOPSIS\n\
- %s [-i <input image path>] [-c] [-d] [-h]\n", name);
-
- fprintf(stdout, "\n\
-OPTIONS: Default\n\
- -i <input image path> %s\n\
- Set image input path.\n\
- From this path read image \n\
- \"ViSP-images/ellipse/ellipse.pgm\"\n\
- Setting the VISP_INPUT_IMAGE_PATH environment\n\
- variable produces the same behaviour than using\n\
- this option.\n\
-\n\
- -c\n\
- Disable the mouse click. Useful to automaze the \n\
- execution of this program without humain intervention.\n\
-\n\
- -d \n\
- Turn off the display.\n\
-\n\
- -h\n\
- Print the help.\n",
- ipath.c_str());
-
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
-}
-/*!
-
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath : Input image path.
- \param click_allowed : Mouse click activation.
- \param display : Display activation.
- \return false if the program has to be stopped, true otherwise.
-
-*/
-bool getOptions(int argc, const char **argv, std::string &ipath,
- bool &click_allowed, bool &display)
-{
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
-
- default:
- usage(argv[0], optarg_, ipath);
- return false; break;
- }
- }
-
- if ((c == 1) || (c == -1)) {
- // standalone param or error
- usage(argv[0], NULL, ipath);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
- return false;
- }
-
- return true;
-}
-
-
-int
-main(int argc, const char ** argv)
-{
- try {
- std::string env_ipath;
- std::string opt_ipath;
- std::string ipath;
- std::string dirname;
- std::string filename;
- bool opt_click_allowed = true;
- bool opt_display = true;
-
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
- env_ipath = vpIoTools::getViSPImagesDataPath();
-
- // Set the default input path
- if (! env_ipath.empty())
- ipath = env_ipath;
-
-
- // Read the command line options
- if (getOptions(argc, argv, opt_ipath,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
-
- // Get the option values
- if (!opt_ipath.empty())
- ipath = opt_ipath;
-
- // Compare ipath and env_ipath. If they differ, we take into account
- // the input path comming from the command line option
- if (!opt_ipath.empty() && !env_ipath.empty()) {
- if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
- std::cout << " Since -i <visp image path=" << ipath << "> "
- << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
- << " we skip the environment variable." << std::endl;
- }
- }
-
- // Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
- usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
- << " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
- exit(-1);
- }
-
-
- // 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<unsigned char> I ;
-
- // Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/ellipse");
-
- // Build the name of the image file
- filename = vpIoTools::createFilePath(dirname, "ellipse.pgm");
-
- // Read the PGM image named "filename" on the disk, and put the
- // bitmap into the image structure I. I is initialized to the
- // correct size
- //
- // exception readPGM may throw various exception if, for example,
- // the file does not exist, or if the memory cannot be allocated
- try{
- vpCTRACE << "Load: " << filename << std::endl;
-
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot read " << filename << std::endl;
- std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
- exit(-1);
- }
-
- // We open a window using either X11, GTK or GDI.
-#if defined VISP_HAVE_X11
- vpDisplayX display;
-#elif defined VISP_HAVE_GTK
- vpDisplayGTK display;
-#elif defined VISP_HAVE_GDI
- vpDisplayGDI display;
-#endif
-
- if (opt_display) {
- // Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
- // Display the image
- // The image class has a member that specify a pointer toward
- // the display that has been initialized in the display declaration
- // therefore is is no longuer necessary to make a reference to the
- // display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- }
-
- vpDot2 dot ;
- std::cout << "debut 1\n";
- //dot.setMaxDotSize(0.50); // dot max size = 50% of the image size
- vpImagePoint ip;
- ip.set_i( 140 );
- ip.set_j( 140 );
- dot.initTracking(I, ip);
- if (opt_display) {
- dot.setGraphics(true) ;
- }
- else {
- dot.setGraphics(false) ;
- }
- dot.setComputeMoments(true);
- dot.track(I) ;
-
- vpFeatureEllipse e ;
-
- vpCameraParameters cam ;
- vpFeatureBuilder::create(e,cam,dot) ;
- if (opt_display) {
- e.display(cam, I, vpColor::red) ;
- vpDisplay::flush(I);
- if (opt_click_allowed) {
- std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
- }
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-#else
-int
-main()
-{
- vpERROR_TRACE("You do not have X11, GTK or GDI display functionalities...");
-}
-
-#endif
diff --git a/test/visual-feature/CMakeLists.txt b/test/visual-feature/CMakeLists.txt
deleted file mode 100755
index 203793e..0000000
--- a/test/visual-feature/CMakeLists.txt
+++ /dev/null
@@ -1,70 +0,0 @@
-#############################################################################
-#
-# $Id: CMakeLists.txt 4961 2014-11-14 13:06:26Z fspindle $
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
-#
-# This software was developed at:
-# INRIA Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-# http://www.irisa.fr/lagadic
-#
-# If you have questions regarding the use of this file, please contact
-# INRIA at visp at 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:
-# ViSP overall configuration file.
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# SOURCE variable corresponds to the list of all the sources to build binaries.
-# The generate binary comes by removing the .cpp extension to
-# the source name.
-#
-# If you want to add/remove a source, modify here
-set(SOURCE
- testFeature.cpp
- testFeatureMoment.cpp
- testFeatureSegment.cpp
-)
-
-# rule for binary build
-foreach(source ${SOURCE})
- # Compute the name of the binary to create
- get_filename_component(binary ${source} NAME_WE)
-
- # From source compile the binary and add link rules
- add_executable(${binary} ${source})
- target_link_libraries(${binary} ${VISP_INTERN_LIBRARY} ${VISP_EXTERN_LIBRARIES})
-
- add_dependencies(visp_tests ${binary})
- if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${binary} PROPERTIES FOLDER "tests")
- endif()
-endforeach()
-
-add_test(testFeature testFeature)
-add_test(testFeatureMoment testFeatureMoment)
-add_test(testFeatureSegment testFeatureSegment ${OPTION_TO_DESACTIVE_DISPLAY} -normalized 0)
-add_test(testFeatureSegment-norm testFeatureSegment ${OPTION_TO_DESACTIVE_DISPLAY} -normalized 1)
diff --git a/test/visual-feature/testFeature.cpp b/test/visual-feature/testFeature.cpp
deleted file mode 100644
index cd8a2cc..0000000
--- a/test/visual-feature/testFeature.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- * $Id: testFeature.cpp 4574 2014-01-09 08:48:51Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature manipulation.
- *
- * Author:
- * Fabien Spindler
- *
- *****************************************************************************/
-
-
-#include <visp/vpColVector.h>
-#include <visp/vpTranslationVector.h>
-#include <visp/vpRotationMatrix.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpGenericFeature.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureThetaU.h>
-#include <visp/vpDot2.h>
-#include <visp/vpCameraParameters.h>
-
-#include <iostream>
-
-/*!
-
- \example testFeature.cpp
-
- Shows how to build a task with a \f$ \theta u \f$ visual feature.
-
-*/
-int main()
-{
- try {
- for (int i=0; i < 3; i++) {
- vpServo task ;
-
- // Creation od a Theta U vector that represent the rotation
- // between the desired camera frame and the current one.
- vpThetaUVector tu_cdRc; // Current visual feature s
- tu_cdRc[0] =0.1;
- tu_cdRc[1] =0.2;
- tu_cdRc[2] =0.3;
-
- // Creation of the current feature s
- vpFeatureThetaU s(vpFeatureThetaU::cdRc);
- s.buildFrom(tu_cdRc);
- s.print();
- task.addFeature(s); // Add current ThetaU feature
-
- // Creation of the desired feature s^*
- vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); // init to zero
-
- // Compute the interaction matrix for the ThetaU_z feature
- vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
- // Compute the error vector (s-s^*) for the ThetaU_z feature
- s.error(s_star, vpFeatureThetaU::selectTUz());
-
- // A call to kill() is requested here to destroy properly the current
- // and desired feature lists.
- task.kill();
-
- std::cout << "End, call vpServo destructors..." << std::endl;
- }
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/test/visual-feature/testFeatureMoment.cpp b/test/visual-feature/testFeatureMoment.cpp
deleted file mode 100644
index cfd2ada..0000000
--- a/test/visual-feature/testFeatureMoment.cpp
+++ /dev/null
@@ -1,226 +0,0 @@
-/****************************************************************************
- *
- * $Id: testFeatureMoment.cpp 3323 2011-09-13 15:23:56Z fnovotny $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Example of visual servoing with moments using a polygon as object container
- *
- * Authors:
- * Filip Novotny
- *
- *****************************************************************************/
-#include <visp/vpMomentObject.h>
-#include <visp/vpMomentDatabase.h>
-#include <visp/vpMomentCommon.h>
-#include <visp/vpFeatureMomentCommon.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpServo.h>
-#include <visp/vpDebug.h>
-#include <visp/vpPlane.h>
-
-#include <limits>
-#include <iostream>
-
-//initialize scene in the interface
-void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst);
-
-vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst); //launch the test
-void planeToABC(const vpPlane& pl, double& A,double& B, double& C);
-int test(double x,double y,double z,double alpha);
-
-//Compute a set of parallel positions and check if the matrix is in the right form;
-int main()
-{
- try {
- int sum=0;
- for(double i=-0.2;i<0.2;i+=0.1){
- for(double j=-0.2;j<0.2;j+=0.1){
- for(double k=-vpMath::rad(30);k<vpMath::rad(30);k+=vpMath::rad(10)){
- for(double l=0.5;l<1.5;l+=0.1){
- sum+=test(i,j,l,k);
- }
- }
-
- }
- }
- if(sum<0) return -1;
- else return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
-
-int test(double x,double y,double z,double alpha){
- //intial pose
- vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
- //Desired pose
- vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
-
- //source and destination objects for moment manipulation
- vpMomentObject src(6);
- vpMomentObject dst(6);
-
- //init and run the simulation
- initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
-
- vpMatrix mat = execute(cMo, cdMo, src, dst);
-
- if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- return 0;
-}
-
-void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst)
-{
- std::vector<vpPoint> src_pts;
- std::vector<vpPoint> dst_pts;
-
- double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
- double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
- int nbpoints = 4;
-
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
- p.track(cMo) ;
- src_pts.push_back(p);
- }
-
- src.setType(vpMomentObject::DENSE_POLYGON);
- src.fromVector(src_pts);
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p;
- p.setWorldCoordinates(x[i],y[i],0.0);
- p.track(cdMo) ;
- dst_pts.push_back(p);
- }
- dst.setType(vpMomentObject::DENSE_POLYGON);
- dst.fromVector(dst_pts);
-}
-
-
-vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst)
-{
- vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired
-
- vpServo task;
- task.setServo(vpServo::EYEINHAND_CAMERA);
- //A,B,C parameters of source and destination plane
- double A; double B; double C;
- double Ad; double Bd; double Cd;
- //init main object: using moments up to order 6
-
- //Initializing values from regular plane (with ax+by+cz=d convention)
- vpPlane pl;
- pl.setABCD(0,0,1.0,0);
- pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
-
- pl.setABCD(0,0,1.0,0);
- pl.changeFrame(cdMo);
- planeToABC(pl,Ad,Bd,Cd);
-
- //extracting initial position (actually we only care about Zdst)
- vpTranslationVector vec;
- cdMo.extract(vec);
-
- ///////////////////////////// initializing moments and features /////////////////////////////////
- //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
- vpMomentCommon moments (vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
- vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
- //same thing with common features
- vpFeatureMomentCommon featureMoments(moments);
- vpFeatureMomentCommon featureMomentsDes(momentsDes);
-
- moments.updateAll(src);
- momentsDes.updateAll(dst);
-
- featureMoments.updateAll(A,B,C);
- featureMomentsDes.updateAll(Ad,Bd,Cd);
-
- //setup the interaction type
- task.setInteractionMatrixType(interaction_type) ;
- //////////////////////////////////add useful features to task//////////////////////////////
- task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
- task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
- //the moments are different in case of a symmetric object
- task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
- task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
-
- task.setLambda(0.4) ;
-
- task.computeControlLaw();
- vpMatrix mat = task.computeInteractionMatrix();
- task.kill();
- return mat;
-}
-
-
-void planeToABC(const vpPlane& pl, double& A,double& B, double& C)
-{
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
-}
-
diff --git a/test/visual-feature/testFeatureSegment.cpp b/test/visual-feature/testFeatureSegment.cpp
deleted file mode 100644
index 855035a..0000000
--- a/test/visual-feature/testFeatureSegment.cpp
+++ /dev/null
@@ -1,263 +0,0 @@
-/****************************************************************************
- *
- * $Id: testFeature.cpp 3530 2012-01-03 10:52:12Z fspindle $
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
- * This software was developed at:
- * INRIA Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- * http://www.irisa.fr/lagadic
- *
- * If you have questions regarding the use of this file, please contact
- * INRIA at visp at 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:
- * Visual feature manipulation (segment).
- *
- * Author:
- * Filip Novotny
- *
- *****************************************************************************/
-
-#include <fstream>
-#include <iostream>
-#include <vector>
-#include <numeric>
-
-#include <visp/vpConfig.h>
-#include <visp/vpCameraParameters.h>
-#include <visp/vpDisplay.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureSegment.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpImage.h>
-#include <visp/vpMath.h>
-#include <visp/vpParseArgv.h>
-#include <visp/vpPlot.h>
-#include <visp/vpPoint.h>
-#include <visp/vpRobotCamera.h>
-#include <visp/vpServo.h> //visual servoing task
-
-/*!
-
- \example testFeatureSegment.cpp
-
- Shows how to build a task with a segment visual feature.
-
-*/
-int main(int argc, const char **argv)
-{
- try {
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- int opt_no_display = 0;
- int opt_curves = 1;
-#endif
- int opt_normalized = 1;
-
- // Parse the command line to set the variables
- vpParseArgv::vpArgvInfo argTable[] =
- {
- #if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- {"-d", vpParseArgv::ARGV_CONSTANT, 0, (char *) &opt_no_display,
- "Disable display and graphics viewer."},
- #endif
- {"-normalized", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &opt_normalized,
- "1 to use normalized features, 0 for non normalized."},
- {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
- "Print the help."},
- {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
- } ;
-
- // Read the command line options
- if(vpParseArgv::parse(&argc, argv, argTable,
- vpParseArgv::ARGV_NO_LEFTOVERS |
- vpParseArgv::ARGV_NO_ABBREV |
- vpParseArgv::ARGV_NO_DEFAULTS)) {
- return (false);
- }
-
- std::cout << "Used options: " << std::endl;
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- opt_curves = (opt_no_display == 0) ? 1 : 0;
- std::cout << " - no display: " << opt_no_display << std::endl;
- std::cout << " - curves : " << opt_curves << std::endl;
-#endif
- std::cout << " - normalized: " << opt_normalized << std::endl;
-
- vpCameraParameters cam(640.,480.,320.,240.);
-
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
- vpDisplay *display = NULL;
- if (!opt_no_display) {
-#if defined(VISP_HAVE_X11)
- display = new vpDisplayX;
-#elif defined VISP_HAVE_GDI
- display = new vpDisplayGDI;
-#endif
- }
-#endif
- vpImage<unsigned char> I(480,640,0);
-
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- if (!opt_no_display)
- display->init(I);
-#endif
-
- vpHomogeneousMatrix cMo (-0.5, 0.5, 4., vpMath::rad(10), vpMath::rad(20), vpMath::rad(90));
- vpHomogeneousMatrix cdMo(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
-
- vpPoint P[4]; // 4 points in the object frame
- P[0].setWorldCoordinates( .1, .1, 0.);
- P[1].setWorldCoordinates(-.1, .1, 0.);
- P[2].setWorldCoordinates(-.1, -.1, 0.);
- P[3].setWorldCoordinates( .1, -.1, 0.);
-
- vpPoint Pd[4]; // 4 points in the desired camera frame
- for (int i=0; i<4; i++) {
- Pd[i] = P[i];
- Pd[i].project(cdMo);
- }
- vpPoint Pc[4]; // 4 points in the current camera frame
- for (int i=0; i<4; i++) {
- Pc[i] = P[i];
- Pc[i].project(cMo);
- }
-
- vpFeatureSegment seg_cur[2], seg_des[2]; // Current and desired features
- for (int i=0; i <2; i++)
- {
- if (opt_normalized) {
- seg_cur[i].setNormalized(true);
- seg_des[i].setNormalized(true);
- }
- else {
- seg_cur[i].setNormalized(false);
- seg_des[i].setNormalized(false);
- }
- vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
- vpFeatureBuilder::create(seg_des[i], Pd[i*2], Pd[i*2+1]);
- seg_cur[i].print();
- seg_des[i].print();
- }
-
- //define visual servoing task
- vpServo task;
- task.setServo(vpServo::EYEINHAND_CAMERA);
- task.setInteractionMatrixType(vpServo::CURRENT);
- task.setLambda(1) ;
-
- for (int i=0; i <2; i++)
- task.addFeature(seg_cur[i], seg_des[i]);
-
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
- if (!opt_no_display) {
- vpDisplay::display(I);
- for (int i=0; i <2; i++) {
- seg_cur[i].display(cam, I, vpColor::red);
- seg_des[i].display(cam, I, vpColor::green);
- vpDisplay::flush(I);
- }
- }
-#endif
-
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- vpPlot *graph = NULL;
- if (opt_curves)
- {
- //Create a window (700 by 700) at position (100, 200) with two graphics
- graph = new vpPlot(2, 500, 500, 700, 10, "Curves...");
-
- //The first graphic contains 3 curve and the second graphic contains 3 curves
- graph->initGraph(0,6);
- graph->initGraph(1,8);
- // graph->setTitle(0, "Velocities");
- // graph->setTitle(1, "Error s-s*");
- }
-#endif
-
- //param robot
- vpRobotCamera robot ;
- float sampling_time = 0.010f ; // Sampling period in seconds
- robot.setSamplingTime(sampling_time) ;
- robot.setPosition(cMo) ;
- int iter=0;
-
- do{
- double t = vpTime::measureTimeMs();
- robot.getPosition(cMo);
- for (int i=0; i <4; i++)
- Pc[i].project(cMo);
-
- for (int i=0; i <2; i++)
- vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
-
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
- if (!opt_no_display) {
- vpDisplay::display(I);
- for (int i=0; i <2; i++) {
- seg_cur[i].display(cam, I, vpColor::red);
- seg_des[i].display(cam, I, vpColor::green);
- vpDisplay::flush(I);
- }
- }
-#endif
-
- vpColVector v = task.computeControlLaw();
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- if (opt_curves)
- {
- graph->plot(0, iter, v); // plot velocities applied to the robot
- graph->plot(1, iter, task.getError()); // plot error vector
- }
-#endif
-
- vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
- iter ++;
-
- } while(( task.getError() ).sumSquare() > 0.0005);
-
- // A call to kill() is requested here to destroy properly the current
- // and desired feature lists.
- task.kill();
-
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- if (graph != NULL)
- delete graph;
-#endif
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- if (!opt_no_display && display != NULL)
- delete display;
-#endif
-
- std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
- return 0;
- }
- catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
-}
diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt
index cf3e0ce..4fc11c1 100644
--- a/tutorial/CMakeLists.txt
+++ b/tutorial/CMakeLists.txt
@@ -2,6 +2,8 @@ project(ViSP-tutorial)
cmake_minimum_required(VERSION 2.6)
+find_package(VISP REQUIRED)
+
if(MSVC)
if(NOT VISP_SHARED)
foreach(flag_var
@@ -23,22 +25,23 @@ if(MSVC)
endif()
endif()
-add_subdirectory(computer-vision)
-add_subdirectory(grabber)
-add_subdirectory(detection/barcode)
-add_subdirectory(detection/face)
-add_subdirectory(detection/matching)
-add_subdirectory(detection/object)
-add_subdirectory(image)
-add_subdirectory(robot/pioneer)
-add_subdirectory(simulator/image)
-add_subdirectory(trace)
-#add_subdirectory(tracking/barcode)
-add_subdirectory(tracking/blob)
-add_subdirectory(tracking/keypoint)
-add_subdirectory(tracking/model-based/edges)
-add_subdirectory(tracking/model-based/hybrid)
-add_subdirectory(tracking/model-based/keypoint)
-add_subdirectory(tracking/template-tracker)
-add_subdirectory(tracking/moving-edges)
-add_subdirectory(visual-servo/ibvs)
+visp_add_subdirectory(bridge/opencv REQUIRED_DEPS visp_core visp_io)
+visp_add_subdirectory(computer-vision REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui)
+visp_add_subdirectory(grabber REQUIRED_DEPS visp_core visp_sensor visp_io visp_gui)
+visp_add_subdirectory(detection/barcode REQUIRED_DEPS visp_core visp_detection visp_io visp_gui)
+visp_add_subdirectory(detection/face REQUIRED_DEPS visp_core visp_detection visp_io visp_gui)
+visp_add_subdirectory(detection/matching REQUIRED_DEPS visp_core visp_vision visp_io visp_gui)
+visp_add_subdirectory(detection/object REQUIRED_DEPS visp_core visp_vision visp_mbt visp_io visp_gui)
+visp_add_subdirectory(image REQUIRED_DEPS visp_core visp_io visp_gui)
+visp_add_subdirectory(robot/pioneer REQUIRED_DEPS visp_core visp_robot visp_vs visp_gui)
+visp_add_subdirectory(simulator/image REQUIRED_DEPS visp_core visp_robot visp_io visp_gui)
+visp_add_subdirectory(trace REQUIRED_DEPS visp_core)
+visp_add_subdirectory(tracking/blob REQUIRED_DEPS visp_core visp_blob visp_io visp_gui)
+visp_add_subdirectory(tracking/keypoint REQUIRED_DEPS visp_core visp_vision visp_klt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/generic REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/edges REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/hybrid REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/keypoint REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
+visp_add_subdirectory(tracking/template-tracker REQUIRED_DEPS visp_core visp_tt visp_io visp_gui)
+visp_add_subdirectory(tracking/moving-edges REQUIRED_DEPS visp_core visp_me visp_io visp_gui)
+visp_add_subdirectory(visual-servo/ibvs REQUIRED_DEPS visp_core visp_robot visp_vs visp_blob visp_vision visp_io visp_gui)
diff --git a/tutorial/bridge/opencv/CMakeLists.txt b/tutorial/bridge/opencv/CMakeLists.txt
new file mode 100644
index 0000000..8332b24
--- /dev/null
+++ b/tutorial/bridge/opencv/CMakeLists.txt
@@ -0,0 +1,22 @@
+project(tutorial-bridge-opencv)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io)
+
+set(tutorial_cpp
+ tutorial-bridge-opencv.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/chessboard.pgm" )
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-bridge-opencv.cpp ${data})
+endforeach()
diff --git a/tutorial/bridge/opencv/chessboard.pgm b/tutorial/bridge/opencv/chessboard.pgm
new file mode 100644
index 0000000..3f4231e
--- /dev/null
+++ b/tutorial/bridge/opencv/chessboard.pgm
@@ -0,0 +1,4 @@
+P5
+640 480
+255
+MORSUW[^fkrwz|~�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
WXYZ\`fjrw}�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
]]^^_cimtx~�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
eedcdhot|}��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
hijjknsw|}~������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
%7J`q������������������������������������������������������r_L9#
$7Ocw�������������������������������������������������������~rhb\]^`cgjkmosvxz~����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������˽����������մ�bBlnquxyzzyy{|}}}}~~}}~~~~~��������������������������������������������������������������������������� [...]
%7J`q������������������������������������������������������q^K8#
%7Ncv������������������������������������������������������oaTLEEFHKMPQUX[^_aegjknrvz|~��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Ȼ���������ؿ�nKlnquwy{{yz{|}}}}}}}}���������������������������������������������������������������������������������������vbP?, [...]
*<Odu������������������������������������������������������q^E5"
%7Nbv������������������������������������������������������lU at -!
!"!#&((*-/3469<?ABHIKMPRTUXZ]`eikmquz{{�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Ѽ���������լ�Wlosuvxz|{|}~~~~}���~��������������������������������������������������������������������������������������raO;' [...]
$7Pcu������������������������������������������������������kT>*
!"#%&'(/02468::CDFIKNPQSVZ^^aly����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ǯ��������ַ�^mptvwxz|{|}}~~}}~~~~�������������������������������������������������������������������� [...]
$7Pcu������������������������������������������������������kT>*
"#%%)*+-/1338:>AACNZo���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������˳��������ڿ�gnquwxy{}{{|}~~}}}}}~~~~~�������������������������������������������������������������������� [...]
,>Qfw������������������������������������������������������iYE4!
$7Pcu������������������������������������������������������lU?*
##&)()3>Xs��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ѹ���������ʪtprvxxy{}z{|}}}}|}|||}}~~���������������������������������������������������������������������� [...]
. at Shx�����������������������������������������������������~gWD3
$7Pcu������������������������������������������������������lU?+
%/Kg��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ֿ���������Է�psvxxy{|z{||}}||}||{|}~���������������������������������������������������������������������� [...]
0CUjz�����������������������������������������������������}fUC2
$7Pcu������������������������������������������������������lU at +
%Da������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������psvwwxz{zz{|||||}}|{|~������������������������������������������������������������������������ [...]
2EWl{�����������������������������������������������������}fUB1
$7Pcu������������������������������������������������������mV@, ">[�������������������������������������������������ξ�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������ʴ���������˝pruvvwxzyz{||||{~}||}������������������������������������������������������������������������� [...]
$7Pcu������������������������������������������������������mV@,
"<Y{������������������������������������������������������|~�����������������������������������������������������������������������������������������������������������������������������������������������������������������������к���������Ҫortvvvwyyz{||||{~}|}������������������������������������������������������������������������� [...]
$4H^r�����������������������������������������������������~fT=-
$8Qdv������������������������������������������������������qZC.
#9Ty������������������������������������������������˪�x_RKFHKNRWWZ^djptw{}���������������������������������������������������������������������������������������������������������������������������������������������������� [...]
&6J`t������������������������������������������������������}eS=-$8Qdv������������������������������������������������������qZC. #8Tx������������������������������������������������ɦ�mR>5..0148;=AEJORT[]agmrvy~��������������������������������������������������������������������������������������������������������������������������������������������� [...]
(9Mbv������������������������������������������������������{cR=-%8Qev������������������������������������������������������r[D. "7Rv������������������������������������������������ɥ�gJ6+!
"%&(+.135:<@FKQUW[_cglqw{�������������������������������������������������������������������������������������������������������������������������������������� [...]
%9Rew������������������������������������������������������t\E/ 4Or�������������������������������������������������pR;-
!#$%')-037<?CGMRV[adjr�����������������������������������������������������������������������������������������������������������������������������������˷���������pqrtuvvvyz{||||{||}~������������������������������������������������������������������������� [...]
&:Rfw������������������������������������������������������t]E/! 3Mp������������������������������������������������Ѯ�nP;,
!"$(*+.269<@CMVk������������������������������������������������������������������������������������������������������������������������������������������oprsuuuuyz{||||{||}~�������������������������������������������������������������������������� [...]
&:Rfx������������������������������������������������������u]E/!
1Ln������������������������������������������������ү�pQ>.
#
!$%&'*,2;Rt��������������������������������������������������������������������������������������������������������������������������������������������oprstuuuyz{||||{||}~������������������������������������������������������������������������� [...]
&:Sfx������������������������������������������������������u]F/!
1Kn������������������������������������������������Բ�uW@/
"$ *Bf����������������������������������������������������������������������������������������������������������������������������������������pqstuvvvxyz{|}}}z{}�������������������������������������������������������������������������� [...]
!2DWk�������������������������������������������������������x`M;+%9Rfx������������������������������������������������������u^I2!,Gj������������������������������������������������ӳ�vXB1!
$(:Z����������������������������������������������������������������������������������������������������������������������������������ʹ��������pqstuvvvxyz{|}}|z{|~ [...]
!3EXl�������������������������������������������������������w`M;+%9Rfx������������������������������������������������������w`K4",Fj������������������������������������������������ն�y[C2"
!%6W~���������������������������������������������������������������������������������������������������������������������������������϶��������pqstuvvvxyz{||||{{{{} [...]
"4GYn�������������������������������������������������������w_L:*&:Rfx������������������������������������������������������xaL5#+Eh������������������������������������������������غ�}^E4"
"2Qx���������������������������������������������������������������������������������������������������������������������������������Һ��������pqstuvvvyz{|||||{zyy{ [...]
"5I[p�������������������������������������������������������w_L9)&:Sgy������������������������������������������������������ybM6$*Dg������������������������������������������������ٽ��`G6$
.Lr�������������������������������������������������������������������������������������������������������������������������������������������pqstuvvvyz{|||{{{zy [...]
';Uj~�������������������������������������������������������s[B/
-Ik�������������������������������������������������ɣ�jM6(
!
4Sz�����������������������������������������ÛxaH:4.+,-/0000/.--./.-2112442154<X������������������������������������������������������������ttuvvwxxyxxwwxxy{|}������������������������������������������������������������������������� [...]
';Ti}���������������������������������������������������ú��t]D1
+Hm�������������������������������������������������ӭ�mP8)
0Kq�����������������������������������������̤y`O>32.--,,--....//000//01123313:Ns�����������������������������������������������������Ŷ����ttuvwwxxyxxwwxxy|}~�������������������������������������������������������������������������������������������raN<(
[...]
&8Nat��������������������������������������������������������nVC/!
';Ti}���������������������������������������������������û��u]D2 *Eh�������������������������������������������������ԯ�pS;+
/Io�����������������������������������������ЪdR at 52.--,,--....///00//011233348Ik�����������������������������������������������������ɹ����tuuvwxxxyyxxxxyy}~�������������������������������������������������������������������������������������������sbL [...]
';Ti}���������������������������������������������������Ž��w_E2 (Ac�������������������������������������������������ֲ�tV>.
-Ej�����������������������������������������ֲ�jVC62.--,,--.....///0//011233645B`�����������������������������������������������������ξ����uuvvwxyyzyxxxxyz~��������������������������������������������������������������������������������������������tdL [...]
';Ti}�������������������������������������������������������|cH4"%@d�������������������������������������������������ۻ�{\B1
':\��������������������������������������������u[G80.--,,--.---...////0112335219Ls�����������������������������������������������������IJ���vvvwxyyzzzyyyyzz~�������������������������������������������������������������������������������������������|jZH7% [...]
';Ti}����������������������������������������������������ñ�~dI5##?c��������������������������������������������������~_E3!
!
%5V|�����������������������������������������ǟz_J:0.--,,--.,---....//0112334117Ef�����������������������������������������������������ȵ���vvwwxyzz{zzyyzz{}~������������������������������������������������������������������������������ [...]
1FZf��������������������������������������������������������yo`TJ at 6/10//02354568:;==@BDEEFHJJKMOPQRRTVY[\^acdeegikmnpsqkfec`gefhe``caa`^][ZYYXWUSQPONMLJIGFFCCB@>=<;:9875422/-7Qn��������������������������������������������������ǧ�cM5&!
.Mo�����������������������������������������ұ�jO>4-)*+)*-+++,,------..///22036/8Rz����������������������������������������������������϶�stwyz{zzyyz{}~�����������������������������������seT?- [...]
#0BXj~�������������������������������������������������������mZA. 1F^q����������������������������������������������������������gR?+
0If����������������������������������������������������������zrmpsrsvy{|~���������������������������������ŵ��n\G<80-,,+*)((%%$#""!!!!! !!! !
"
%:[~������������������������������������������gL>4+ [...]
$2D[m��������������������������������������������������������lX?- 2G_r����������������������������������������������������������iS>*
/Ge���������������������������������������������������ȿ���}ri[RTXVW[^`bcdgkopqsvy}���������������������������~pcRLI@?>=<:987321/-,*)('&%$#"!!!!!! !
!
!4Vy�����������������������������������������ӫ�kO at 6,* [...]
%4F]o��������������������������������������������������������iU=+
2G`s����������������������������������������������������������iS?*,Ec�����������������������������������������������������ª�xeWD79=;;@ACEFGILOSTVX[_bcdfimpsttyyy{~������������upj__`WXWVTSQPOKJHEB?=<876420/.+++*))((#$!"% "4Qt�����������������������������������������֯�nRB7-) [...]
%4G^q��������������������������������������������������������gS;) 2G`s����������������������������������������������������������hS?,
)B`�����������������������������������������������������д�w_D/ "&#$)&(+,,-/1779;>ADFGILPSUVW]\]^afjmnpxxt|~qrkmmjotlrqpnljihdc`]YVTRONLJGECB>==;:988./,-0)+<Nq�����������������������������������������ر�qUC7-) [...]
'6I`r�������������������������������������������������������|eQ:(
"4H`r����������������������������������������������������������hS@,
&D^z����������������������������������������������������ٹ�{[D-
""$%'()*-/147;=>@ACEHJLMQSVY[^be^fmqx�����������~{xuspljliea^[WTUSPNNLJHDB>;;>BE]y�����������������������������������������ڼ�q\D8-)(&'*)((((()),,,,,,,,--./01127Ks�����������������������������������������������������stwz{{zyxwxz~������������������������������������~qbQ<'
)9Kbu�������������������������������������������������������{dQ9'
"4I`r����������������������������������������������������������hS@,
&C]y����������������������������������������������������ٺ�{\D-
!#$&(('(*,.01268:<=@DGR_nz�����������������������~|yurqnkjigeba^YURRSTl�������������������������������������������ĝw`I=1,+)*-**)))*+++++++,,,-../01225Fl�����������������������������������������������������tuwz{{zzxwxz~������������������������������������|o`N:% [...]
"4I`s����������������������������������������������������������hS@,
%B\x����������������������������������������������������ٻ�}^E.!
!#$%%'*-AVq������������ÿ����������������������}zvqmkiiw�������������������������������������������ɥ}cJ=0+)'(+((((()*+**++,,--..//01123 at a�����������������������������������������������������uvxyz{zzxwxz~������������������������������������{l\K7# [...]
#5Ias����������������������������������������������������������iT@,
#@Zv����������������������������������������������������ڼ�`G0"
"4Os��������������������¿�������������������������������������������������������������������ʧcJ>1+)&'**)))**+,*++,--..--..//002:V�����������������������������������������������������wxxyzz{{xwxz~������������������������������������yiXG4! [...]
#5Jas����������������������������������������������������������iT at +!=Xt����������������������������������������������������ڽ��cH1#
-Kq����������������������������������¿�����������������������������������������������������ǥ~cNB61/,+-.-,,,,--***++,,,,----...36Lz����������������������������������������������������xyyyzz{{xwxz~������������������������������������wfUC1 [...]
$5Jbt����������������������������������������������������������iT at + ;Vr����������������������������������������������������۾��eJ2$
+Gl���������������������������������������������������~zyz}��������������������������������Ӿ�}fQG=:84120/.-,+,,,,++++++-------.43Dm����������������������������������������������������zyyyzz{{xwxz~������������������������������������udRA/
[...]
$6Jbt����������������������������������������������������������iT at + :Tq����������������������������������������������������ۿ��gK3%
)Bd���������������������������������������������ʷ���uhc[]_behjkrtx|���������������������ƾ��|l\SKJHC@?<;9765443322100/////////31>c����������������������������������������������������zzyyyz{{xwxz~������������������������������������ucQ?.
[...]
$6Jbt����������������������������������������������������������jT at +
9Sp����������������������������������������������������ۿ��hK4%
(>^����������������������������������������������˪�w^MF?@BEHKMNUVZ]`cefprtx{����������������{qld^]\WRQPOMJHFFE<;;98766221111112/:\����������������������������������������������������yzz{{zzyz{}������������������������������������}p`P=) [...]
%7Kbt����������������������������������������������������������iT@,
7Pn�����������������������������������������������������§�iO5$
"9]����������������������������������������������Ҳ�sV?2/-+,/3558:<@DHJLSTW[_cfhjlpu{���������|xrpprrolledb_]ZXWSPKHGFDC@><<<<:887?X����������������������������������������������������yzz{{zzyz{}������������������������������������}p_O<( [...]
%7Kct����������������������������������������������������������iT@,
6Om�����������������������������������������������������è�jP6%
!7Y}���������������������������������������������Դ�tW?1(%#"$&''()*,/12368:=ADFHOQTX]begkpuvuvy{����������}zwtqpmjeb`_\ZURNKIGC at B@E[����������������������������������������������������zz{{{{zzz|}������������������������������������|n]L:& [...]
%7Lcu����������������������������������������������������������iT@,
5Nl�����������������������������������������������������ĩ�kQ7%
4Ux���������������������������������������������ع�vY at 1)%! !!!!!!!!$$&(*,./579<@CEGLPTW\gw��������������������~|yvsqniea]XTWRSe����������������������������������������������������zz{||{zz{|~�������������������������������������zlZI7$ [...]
%8Mdv����������������������������������������������������������iT@,
4Mj�����������������������������������������������������Ŭ�lS9&
1Ps���������������������������������������������ܿ�y\B1+'#! !! #""! "##$&&'(((*,.01289:>EVq�����������������������������~zvrrljw����������������������������������������������������z{||||{z{|~�������������������������������������yjWF4" [...]
&8Mew����������������������������������������������������������iT@,
3Lh�����������������������������������������������������Ʈ�mU:'
/Ln����������������������������������������������Š}`F3'$
"""""###!!"##$%%)'(+2Fj�������������������¿��������������������������������������������������������������������{{||||{{|}�������������������������������������xhUC1 [...]
&9Nfx����������������������������������������������������������iT@,
2Jg�����������������������������������������������������ǰ�nW<(
-Hj����������������������������������������������ʥ�dI5'$
!
!!!!!! """"#####!"&-Ah���������������������������¾������������������������������������������������������������{||}}||{|}�������������������������������������wgR at .
[...]
&9Nfy����������������������������������������������������������iT@,
2Ie�����������������������������������������������������Ȳ�oY=(
)Cd�����������������������������������������������hM7*'#!!""! !!""##""!!! $$$$$$$$$"#'+<d�����������������������������������������������������������������������������������������{||}}||{|}�������������������������������������vfQ?-
[...]
&9Ogy����������������������������������������������������������iT@,
1Ie�����������������������������������������������������ɳ�pZ>(
'@a����������������������������������������������Ϫ�jO9*&"
""!!! !!!!! # !$%2X�������������������������������������������������������������¾�������������������������|{{{{|}~{}������������������������������������}sbM;* [...]
'<Rhx����������������������������������������������������������iT@,
,Hj�����������������������������������������������������ȯ�v]@*
#:^����������������������������������������������Ե�oQ>.& !#! !!!!!!!! %# $.Oz����������������������������������������������������������������������������������������||{{||}~|~�������������������������������������|raL:) [...]
'<Rhy����������������������������������������������������������iT@,
,Hi�����������������������������������������������������ʱ�v]@+ !8[}���������������������������������������������շ�qS at .& !" !!!!!!!! %# $-Mw����������������������������������������������������������������������������������������||{{|}~~~�������������������������������������|p_J8' [...]
(=Siz����������������������������������������������������������iT@,
+Gh����������������������������������������������������ʹ�v^A+ 5Wx���������������������������������������������ػ�uVB0'!!" !!!!!!!!
$" $,Ir����������������������������������������������������������������������������������������}||||}~��������������������������������������{m]G5% [...]
(>Tj{����������������������������������������������������������iT@,
*Fg~����������������������������������������������������з�w_B,!
2Tt���������������������������������������������ۿ�zZE1(!!!
$" #*Ej����������������������������������������������������������������������������������������}}||}}~���������������������������������������zkZD2# [...]
)?Ul|����������������������������������������������������������iT@,
)Ef}����������������������������������������������������Ѻ�y`C-!
0Qq����������������������������������������������á~]H3)!
#! #(@c����������������������������������������������������������������������������������������}}}}}~��������������������������������������xhWB/ [...]
*@Vm~����������������������������������������������������������iT@,
(Dd{����������������������������������������������������ѻ�zaD-"
.Mm����������������������������������������������ƥ�_I5+"
"! #&;\����������������������������������������������������������������������������������������~}}}}~�~�������������������������������������xgU?- [...]
*@Wn����������������������������������������������������������iT@,
(Cdz����������������������������������������������������л�|aD."+Ii����������������������������������������������ȧ�aJ6+"
" #%7V}���������������������������������������������������������������������������������������~}}}}~�}�������������������������������������wfT>,
[...]
+AWn����������������������������������������������������������iT@,
'Bcz����������������������������������������������������λ�}bE."(Ff����������������������������������������������ɨ�aJ7,"
" #$5Sy�������������������������������������������������������������������������������������� ������~��������������������������������������udS<) [...]
.BXo�����������������������������������������������������������jUA,
'Bcz����������������������������������������������������ϻ�~bK0 (Bb����������������������������������������������ͦ�gL8," !
"%"!#"0Lp��������������������������������������������������������������������������������������怀��������������������������������������������scQ;( [...]
.CYp�����������������������������������������������������������jUA,
&Bcz����������������������������������������������������м�~cL1!(Ba����������������������������������������������Χ�hM9-" !
!$! "!.Im�������������������������������������������������������������������������������������������������������������������������������}paO8& [...]
/DZr�����������������������������������������������������������kVA-
&Aby����������������������������������������������������Ӿ�dM2"'A`����������������������������������������������Ϫ�jN:-" !
# ! ,Eh���������������������������������������������������������������������������������������������������������������������������������|o_L5$ [...]
5Ph~���������������������������������������������������������������rVB*
"9Vr��������������������������������������������������������á�iQ4#&<\������������������������������������������������ͱ�hP8("
":X~������������������������������������������������������������������������߭�������������������������������������hT<+ [...]
4Of}���������������������������������������������������������������rVB*
!8Tp��������������������������������������������������������á�iQ5#$9Y|�����������������������������������������������β�hQ8("
7Tz������������������������������������������������������������������������ݬ�������������������������������������hS<+ [...]
4Me}���������������������������������������������������������������rVB*
!7To��������������������������������������������������������á�iQ5#"7Vz�����������������������������������������������ϲ�hP7'!
4Qx������������������������������������������������������������������������ܭ�������������������������������������hT;* [...]
2J_s����������������������������������������������������������¿���nTA,
2Le~����������������������������������������������������������hR7&
(Hi�����������������������������������������������̯�iSA,
!,Fl������������������������������������������������������������������������ݭ�������������������������������������hS:+
[...]
,C]y����������������������������������������������������������ŭ�w_E1
4Tl����������������������������������������������������˪�qS:*
4Qx��������������������������������������������jK:(
1V��������������������Ĵ��������������������������������������������������������������������������������������������������������� [...]
,C]y����������������������������������������������������������Ư�y`F2
3Tl����������������������������������������������������˫�rT;*
4Pw�������������������������������������������خ�kL;(
0U~��������������������Ŷ��������������������������������������������������������������������������������������������������������� [...]
,C]y����������������������������������������������������������ȱ�{bG2
3Sl����������������������������������������������������ͭ�sU<+
3Ov�������������������������������������������۱�mN<) 0T}��������������������ɼ��������������������������������������������������������������������������������������������������������� [...]
,C]y����������������������������������������������������������ɲ�|bG2
3Sl����������������������������������������������������ϯ�uW=,
1Mt�������������������������������������������ݶ�pQ>+!
/Q{������������������������������������������������������������������������������������������������������������������������������ [...]
,C]y����������������������������������������������������������ʳ�}cG1
2Rl����������������������������������������������������ѱ�wY?-
0Kr�������������������������������������������໙sS@,!
.Oy������������������������º���Ǭ�������������������������������������������������������������������������������������������������� [...]
,C]y����������������������������������������������������������̶��eH2
2Rm����������������������������������������������������ҳ�yZ at .
.Jo����������������������������������������������vVB-"
-Mw���������������������������������������������������������������������������������������������������������������������������� [...]
,C]y�����������������������������������������������������������hK5
1Qm����������������������������������������������������Դ�z\A/
-Hn��������������������������������������������ÞxXC.#
,Ku�����������������������������Ӯ��������������������������������������������������������������������������������������ÿ��������� [...]
,C]y����������������������������������������������������������ҽ��kM7
1Qm����������������������������������������������������յ�{\B/
,Hm��������������������������������������������şyYD/#
,Jt�����������������������������֩������������������������������������������������������������������������������������������������ [...]
,C[r���������������������������������������������������������������}eI4
+Cd}�����������������������������������������������������������¨�mS=&%;^|����������������������������������������������������ϴ�qZ=(
!#$&'(-./13466=>?ADGIJLR]kx����������������|zxvuspmjhgfca_^[WTPONLJHGFPc}��������������������������������������������������������������������ǯ������������ [...]
+BZq���������������������������������������������������������������}dI4 *Bb|�����������������������������������������������������������¨�mR=&$;]z����������������������������������������������������ǫ�nZB0(
!#$%&)*+-/13389:=?ACDIJLNPRSTZ[^aegiinrx|�}|}ztrsrnjigda^\[YXVTQOMLIGEDB@<977643112@\��������������������������������������������������������������������������������� [...]
+BYp���������������������������������������������������������������|dI4 *Bb{�����������������������������������������������������������¨�lR=&$<]y���������������������������������������������������˷��k]I<824568:<>?FGIKNPRSUVY[^acdfgikmpqrvx|����������}unfe`YVXWSQPNLIGFE>=;964321/-,+)%"$$$" !"6W���������������������������������������������������������������������������������� [...]
)@Wo�����������������������������������������������������������ƿ��{bH5!
+Dd|�����������������������������������������������������������Ī�nW<*"<Wy�������������������������������������������������������pc\VPLOOPRUXZ\]^`cfhjkopsvy|~���������������������}l_XNECB?=>98631/..*)('%$#"
3Q|�����������������������������������������������������������������¿�����������������������������������������������������������������������������������������������������rZA) [...]
+AXo�����������������������������������������������������������ǿ��zaG3
*B_v�����������������������������������������������������������ê�nW<+%=Wx����������������������������þ�����������������������xoihihgklnqtvxyz{}���������������������������������xbQB8/-,)'(%$"
2P{������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
*@]s�����������������������������������������������������������ë�nW=,!*CZw���������������������������������������������������|uommqw~��������������������������������������������˳�wZE3)
1Oy���������������������������������������������������������������Ȼ�������������������������������������������������������������������������������������������������������oXA+
[...]
*A^u�����������������������������������������������������������ª�nW<,
!#$##%'*((*+,3;O`u�����������������������������������|xwwutsrrqqomjgd`afnx�����������������������������������������������|\E1&
/Lw���������������������������������������������������������������Ķ�������������������������������������������������������������������������������������������������������oX@*
[...]
*@Wn����������������������������������������������������������ÿ���ybI6!'?_w����������������������������������������������������������μ��mX;-
!!!""###'(()*+,,,-./123468;;;<>@EBCFHHMS^fr{���������������~~~ywutsspnomjhgeb_`_][YWVUTSRQNJIJU`q������������������������������������������������ĥ�`I/$
,Jt���������������������������������������������������������������ö�������������� [...]
&=\t���������������������������������������������������������Ƽ���kZB6,)(')+**+,-/1244556778<<=?@BCCCDEGIKLMMORSSTWY^[\acbdhnnquutuzwwvuspnmmkjhfeeeb`]\\[YWTQOMLKHFHGFCA>=<;9997348IZt������������������������������������������������ɩ�aI0$
*Hr���������������������������������������������������������������ƻ�������������������������������������������������������������������������������������������������������lYE2& [...]
3Oo��������������������������������������������������������϶�~bH0
-Op������������������������������������������������ݺ�uU=)
7\�����������������������������������������������������������������������������������������������������������zdN7
#3H]s�������������������������������������������������������������n[H3&
[...]
3Oo��������������������������������������������������������ѷ��dK3!.Oq�������������������������������������������������vV=)
6Z������������������������������������������������������������������������������������������������������������hM9#!2Iax�������������������������������������������������������������s^H1# [...]
.F`|������������������������������������������������������������Ƕ��iR7$
0Lm��������������������������������������������������������Կ�}dF.!+Gl�������������������������������������������������ĥ�_F- .Nt����������������������������������������������������������������������������������������������������ü�����kS>& [...]
.F`|������������������������������������������������������������ȷ��kS8%
0Kl��������������������������������������������������������Կ�}eF.!*Ej�������������������������������������������������Ŧ�_H.
-Lr���������������������������������������������������������������������������������������������������� [...]
.F`}������������������������������������������������������������ȸ��kT9%
/Jk��������������������������������������������������������Կ�~gH0"(Cg�������������������������������������������������ǧ�`I/!
,Jo������������������������������������������������������������������Ŀ���������������������������������������oV@' [...]
.F`}������������������������������������������������������������ȸ��kT:&
/Jk��������������������������������������������������������Կ��iK2"'Ae�������������������������������������������������ɪ�bK0!
*Gk������������������������������������������������������������������ſ����������������������������������¼���pWA( [...]
.Fa}������������������������������������������������������������ȷ��kU:&
/Ij��������������������������������������������������������ӿ��kM3#&@d�������������������������������������������������ˬ�dK0!
(Ei������������������������������������������������������������������������������������������������������ø���qZD* [...]
/E^z������������������������������������������������������������ʼ��lT>%0Hf������������������������������������������������������������hN5!%=`�������������������������������������������������Ы�gL1"
$?f������������������������������������������������������������������������������������������������������ø���r[D* [...]
/D]z������������������������������������������������������������̾��nT>&0Hf���������������������������������������������������������¨�jP7#%<_�������������������������������������������������Ь�hM2" $>e������������������������������������������������������������������������������������������������������Ĺ���u\E+
[...]
.D]z����������������������������������������������������������������pU?&0Hf���������������������������������������������������������Ī�kR8$$;^�������������������������������������������������Ѯ�iN2# #<b������������������������������������������������������������������������������������������������������Ż���w]F,
[...]
-C\z����������������������������������������������������������������pV@'0Hf���������������������������������������������������������Ī�kR9$#:\~������������������������������������������������Ӱ�jN3# ":_�������������������������������������������������������������������������������������������������� [...]
,B\z����������������������������������������������������������������pXB)0Hf���������������������������������������������������������é�kQ8$"9Z|������������������������������������������������Գ�kO4$ !8[�������������������������������������������������������������������������������������������������� [...]
+A[z����������������������������������������������������������������qYC*0Hf���������������������������������������������������������ê�kR8$!7Yz������������������������������������������������ֵ�lP5% 6X~������������������������������������������������������������������������������������������������� [...]
*@[z�������������������������������������������������������������ë�sZD*
0Hf���������������������������������������������������������ū�mS:&!6Wx�������������������������������������������������mQ6%! 4V|�������������������������������������������������������������������������������������������������� [...]
*?Zz�������������������������������������������������������������ĭ�t[D+
0Hf���������������������������������������������������������ǭ�nU;'
6Vw������������������������������������������������ظ�mQ6&! 3Tz�������������������������������������������������������������������������������������������������� [...]
#>Wr��������������������������������������������������������������ä�lT7#
#7Ux���������������������������������������������������������Ȩ�jM2!%Be�������������������������������������������������ɠ}[E,!
! #'*-047;@EKOSX[^fnw���z~wstslge^[WSQNIF>=:741/.&%$"!
2Ms���������������������Dz��������������������������������������Ǻ��sS9($/A_x�����������������������������������������������������������ҽ��kN8" [...]
#7Uw���������������������������������������������������������ɩ�kN3"%Bd�������������������������������������������������Ĝz`J0#
#&(*,/36;>CFIMQUZ^dinrwz}�����yoh_YXUOJHC@<:852/*)'%"
0Kq���������������������ȳ��������������������������������������ʽ��sT9)$/A_x�����������������������������������������������������������Ծ��mO9# [...]
\ No newline at end of file
diff --git a/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp
new file mode 100644
index 0000000..be16f98
--- /dev/null
+++ b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp
@@ -0,0 +1,53 @@
+//! \example tutorial-bridge-opencv.cpp
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020300
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#endif
+
+int main()
+{
+#if VISP_HAVE_OPENCV_VERSION >= 0x020300
+ //! [Set ViSP camera parameters]
+ double u0 = 326.6;
+ double v0 = 215.0;
+ double px = 582.7;
+ double py = 580.6;
+ double kud = -0.3372;
+ double kdu = 0.4021;
+ vpCameraParameters cam(px, py, u0, v0, kud, kdu);
+ //! [Set ViSP camera parameters]
+
+ //! [Set OpenCV camera parameters]
+ cv::Mat K = (cv::Mat_<double>(3,3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
+ cv::Mat D = (cv::Mat_<double>(4,1) << cam.get_kud(), 0, 0, 0);
+ //! [Set OpenCV camera parameters]
+
+ //! [Load ViSP image]
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "chessboard.pgm");
+ //! [Load ViSP image]
+
+ //! [Convert ViSP 2 OpenCV image]
+ cv::Mat image;
+ vpImageConvert::convert(I, image);
+ //! [Convert ViSP 2 OpenCV image]
+
+ //! [Undistort OpenCV image]
+ cv::Mat imageUndistorted;
+ cv::undistort(image, imageUndistorted, K, D);
+ //! [Undistort OpenCV image]
+
+ //! [Convert OpenCV 2 ViSP image]
+ vpImage<unsigned char> IUndistorted;
+ vpImageConvert::convert(imageUndistorted, IUndistorted);
+ //! [Convert OpenCV 2 ViSP image]
+
+ //! [Save image]
+ vpImageIo::write(IUndistorted, "chessboard-undistorted.pgm");
+ //! [Save image]
+#endif
+}
diff --git a/tutorial/computer-vision/CMakeLists.txt b/tutorial/computer-vision/CMakeLists.txt
index a8aed1f..bde9d4c 100644
--- a/tutorial/computer-vision/CMakeLists.txt
+++ b/tutorial/computer-vision/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-computer-vision)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_blob visp_vision visp_io visp_gui)
set(tutorial_cpp
tutorial-homography-from-points.cpp
diff --git a/tutorial/computer-vision/tutorial-homography-from-points.cpp b/tutorial/computer-vision/tutorial-homography-from-points.cpp
index e4aae51..6da57bf 100644
--- a/tutorial/computer-vision/tutorial-homography-from-points.cpp
+++ b/tutorial/computer-vision/tutorial-homography-from-points.cpp
@@ -1,19 +1,19 @@
//! \example tutorial-homography-from-points.cpp
//! [Include]
-#include <visp/vpHomography.h>
+#include <visp3/vision/vpHomography.h>
//! [Include]
-#include <visp/vpMeterPixelConversion.h>
+#include <visp3/core/vpMeterPixelConversion.h>
int main()
{
//! [Set 3D points]
double L = 0.1;
- std::vector<vpPoint> oP(4);
- oP[0].setWorldCoordinates( -L,-L, 0);
- oP[1].setWorldCoordinates(2*L,-L, 0);
- oP[2].setWorldCoordinates( L, 3*L, 0);
- oP[3].setWorldCoordinates( -L, 4*L, 0);
+ std::vector<vpPoint> oP;
+ oP.push_back( vpPoint( -L,-L, 0) );
+ oP.push_back( vpPoint(2*L,-L, 0) );
+ oP.push_back( vpPoint( L, 3*L, 0) );
+ oP.push_back( vpPoint( -L, 4*L, 0) );
//! [Set 3D points]
//! [Simulation]
diff --git a/tutorial/computer-vision/tutorial-pose-from-points-image.cpp b/tutorial/computer-vision/tutorial-pose-from-points-image.cpp
index a414087..a09ba98 100644
--- a/tutorial/computer-vision/tutorial-pose-from-points-image.cpp
+++ b/tutorial/computer-vision/tutorial-pose-from-points-image.cpp
@@ -1,11 +1,11 @@
/*! \example tutorial-pose-from-points-image.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpPose.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpPose.h>
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo);
@@ -56,11 +56,11 @@ int main()
dot[1].initTracking(I, vpImagePoint(203, 366));
dot[2].initTracking(I, vpImagePoint(313, 402));
dot[3].initTracking(I, vpImagePoint(304, 133));
- std::vector<vpPoint> point(4);
- point[0].setWorldCoordinates(-0.06, -0.06, 0);
- point[1].setWorldCoordinates( 0.06, -0.06, 0);
- point[2].setWorldCoordinates( 0.06, 0.06, 0);
- point[3].setWorldCoordinates(-0.06, 0.06, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.06, -0.06, 0) );
+ point.push_back( vpPoint( 0.06, -0.06, 0) );
+ point.push_back( vpPoint( 0.06, 0.06, 0) );
+ point.push_back( vpPoint(-0.06, 0.06, 0) );
vpHomogeneousMatrix cMo;
bool init = true;
diff --git a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
index a9af0cf..dc8fffc 100644
--- a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
+++ b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
@@ -1,12 +1,15 @@
/*! \example tutorial-pose-from-points-tracking.cpp */
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpPose.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#endif
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpPose.h>
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo);
@@ -62,10 +65,10 @@ void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
int main()
{
-#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
try { vpImage<unsigned char> I;
-#if defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g;
g.open(I);
#elif defined(VISP_HAVE_CMU1394)
@@ -89,12 +92,12 @@ int main()
vpHomogeneousMatrix cMo;
std::vector<vpDot2> dot(4);
- std::vector<vpPoint> point(4);
+ std::vector<vpPoint> point;
double L = 0.06;
- point[0].setWorldCoordinates(-L, -L, 0);
- point[1].setWorldCoordinates( L, -L, 0);
- point[2].setWorldCoordinates( L, L, 0);
- point[3].setWorldCoordinates(-L, L, 0);
+ point.push_back( vpPoint(-L, -L, 0) );
+ point.push_back( vpPoint( L, -L, 0) );
+ point.push_back( vpPoint( L, L, 0) );
+ point.push_back( vpPoint(-L, L, 0) );
bool init = true;
#if defined(VISP_HAVE_X11)
@@ -107,7 +110,7 @@ int main()
while(1){
// Image Acquisition
-#if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394)
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
g >> frame;
diff --git a/tutorial/detection/barcode/CMakeLists.txt b/tutorial/detection/barcode/CMakeLists.txt
index add4cc6..13a85d6 100644
--- a/tutorial/detection/barcode/CMakeLists.txt
+++ b/tutorial/detection/barcode/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-barcode)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_detection visp_io visp_gui)
set(tutorial_cpp
tutorial-barcode-detector.cpp
diff --git a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
index 60297d0..bb74a8d 100644
--- a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
+++ b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
@@ -1,11 +1,14 @@
//! \example tutorial-barcode-detector-live.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageConvert.h>
-#include <visp/vpDetectorDataMatrixCode.h>
-#include <visp/vpDetectorQRCode.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/detection/vpDetectorDataMatrixCode.h>
+#include <visp3/detection/vpDetectorQRCode.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
int main(int argc, const char** argv)
{
diff --git a/tutorial/detection/barcode/tutorial-barcode-detector.cpp b/tutorial/detection/barcode/tutorial-barcode-detector.cpp
index 6d9523f..df10870 100644
--- a/tutorial/detection/barcode/tutorial-barcode-detector.cpp
+++ b/tutorial/detection/barcode/tutorial-barcode-detector.cpp
@@ -1,12 +1,12 @@
//! \example tutorial-barcode-detector.cpp
//! [Include]
-#include <visp/vpDetectorDataMatrixCode.h>
-#include <visp/vpDetectorQRCode.h>
+#include <visp3/detection/vpDetectorDataMatrixCode.h>
+#include <visp3/detection/vpDetectorQRCode.h>
//! [Include]
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageIo.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
int main(int argc, const char** argv)
{
diff --git a/tutorial/detection/face/CMakeLists.txt b/tutorial/detection/face/CMakeLists.txt
index 258eaf1..7284297 100644
--- a/tutorial/detection/face/CMakeLists.txt
+++ b/tutorial/detection/face/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-detection-face)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_detection visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/detection/face/tutorial-face-detector-live.cpp b/tutorial/detection/face/tutorial-face-detector-live.cpp
index e50b4c3..735b3b2 100644
--- a/tutorial/detection/face/tutorial-face-detector-live.cpp
+++ b/tutorial/detection/face/tutorial-face-detector-live.cpp
@@ -1,9 +1,12 @@
//! \example tutorial-face-detector-live.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDetectorFace.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/detection/vpDetectorFace.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
int main(int argc, const char* argv[])
{
diff --git a/tutorial/detection/face/tutorial-face-detector.cpp b/tutorial/detection/face/tutorial-face-detector.cpp
index 9a6927b..aa694cd 100644
--- a/tutorial/detection/face/tutorial-face-detector.cpp
+++ b/tutorial/detection/face/tutorial-face-detector.cpp
@@ -1,11 +1,11 @@
//! \example tutorial-face-detector.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
//! [Include]
-#include <visp/vpDetectorFace.h>
+#include <visp3/detection/vpDetectorFace.h>
//! [Include]
-#include <visp/vpVideoReader.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, const char* argv[])
{
diff --git a/tutorial/detection/matching/CMakeLists.txt b/tutorial/detection/matching/CMakeLists.txt
index cb29651..b70aca6 100644
--- a/tutorial/detection/matching/CMakeLists.txt
+++ b/tutorial/detection/matching/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-matching-keypoint)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_vision visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
index b5f4b81..dc680bc 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-matching-keypoint-SIFT.cpp
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpImageIo.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpImageIo.h>
//! [Include]
-#include <visp/vpKeyPoint.h>
+#include <visp3/vision/vpKeyPoint.h>
//! [Include]
int main() {
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
index 6aa46d5..42d0974 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-matching-keypoint-homography.cpp
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpHomography.h>
-#include <visp/vpKeyPoint.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, const char **argv)
{
@@ -83,16 +83,11 @@ int main(int argc, const char **argv)
//! [Allocation]
for (unsigned int i = 0; i < nbMatch; i++) {
- vpImagePoint matched_ref, matched_cur;
- keypoint.getMatchedPoints(i, matched_ref, matched_cur);
+ keypoint.getMatchedPoints(i, iPref[i], iPcur[i]);
//! [Pixel conversion]
- vpPixelMeterConversion::convertPoint(cam, matched_ref, mPref_x[i], mPref_y[i]);
- vpPixelMeterConversion::convertPoint(cam, matched_cur, mPcur_x[i], mPcur_y[i]);
+ vpPixelMeterConversion::convertPoint(cam, iPref[i], mPref_x[i], mPref_y[i]);
+ vpPixelMeterConversion::convertPoint(cam, iPcur[i], mPcur_x[i], mPcur_y[i]);
//! [Pixel conversion]
-
- // Store the image coordinates in pixel of the matched points
- iPref[i] = matched_ref;
- iPcur[i] = matched_cur;
}
//! [Homography estimation]
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint.cpp b/tutorial/detection/matching/tutorial-matching-keypoint.cpp
index a70938f..5e84a81 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-matching-keypoint.cpp
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpImageIo.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/io/vpImageIo.h>
//! [Include]
-#include <visp/vpKeyPoint.h>
+#include <visp3/vision/vpKeyPoint.h>
//! [Include]
int main() {
diff --git a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
index 243e582..234f15e 100755
--- a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
+++ b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
@@ -1,8 +1,8 @@
//! \example tutorial-matching-surf-deprecated.cpp
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
-#include <visp/vpKeyPointSurf.h>
+#include <visp3/vision/vpKeyPointSurf.h>
//! [Include]
int main()
diff --git a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
index d7e17a6..0e31e8e 100644
--- a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
+++ b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-matching-surf-homography-deprecated.cpp
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpHomography.h>
-#include <visp/vpKeyPointSurf.h>
-#include <visp/vpPixelMeterConversion.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/vision/vpKeyPointSurf.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, const char **argv)
{
diff --git a/tutorial/detection/object/CMakeLists.txt b/tutorial/detection/object/CMakeLists.txt
index b6d8a2e..e353bc1 100644
--- a/tutorial/detection/object/CMakeLists.txt
+++ b/tutorial/detection/object/CMakeLists.txt
@@ -2,11 +2,13 @@ project(tutorial-detection-object)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_vision visp_mbt visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
- tutorial-detection-object-mbt.cpp)
+ tutorial-detection-object-mbt.cpp
+ tutorial-detection-object-mbt2.cpp
+)
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/detection-config.xml")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/detection-config-SIFT.xml")
@@ -16,6 +18,15 @@ list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.wrl")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.init")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.mpg")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.xml")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.cao")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.wrl")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.init")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.mpeg")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube0001.png")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube0150.png")
+list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube0200.png")
+
foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(COMMAND visp_add_dependency)
@@ -27,3 +38,7 @@ endforeach()
foreach(data ${tutorial_data})
visp_copy_data(tutorial-detection-object-mbt.cpp ${data})
endforeach()
+
+foreach(data2 ${tutorial_data2})
+ visp_copy_data(tutorial-detection-object-mbt2.cpp ${data2})
+endforeach()
diff --git a/tutorial/detection/object/cube.cao b/tutorial/detection/object/cube.cao
new file mode 100644
index 0000000..4408839
--- /dev/null
+++ b/tutorial/detection/object/cube.cao
@@ -0,0 +1,19 @@
+V1
+8
+ 0.000 0.000 0.000
+-0.084 0.000 0.000
+-0.084 0.084 0.000
+ 0.000 0.084 0.000
+ 0.000 0.000 0.084
+-0.084 0.000 0.084
+-0.084 0.084 0.084
+ 0.000 0.084 0.084
+0
+0
+6
+4 0 4 5 1
+4 1 5 6 2
+4 6 7 3 2
+4 3 7 4 0
+4 0 1 2 3
+4 7 6 5 4
diff --git a/tutorial/detection/object/cube.init b/tutorial/detection/object/cube.init
new file mode 100644
index 0000000..a43f558
--- /dev/null
+++ b/tutorial/detection/object/cube.init
@@ -0,0 +1,6 @@
+4
+ 0.000 0.000 0.000
+ 0.000 0.000 0.084
+-0.084 0.000 0.084
+-0.084 0.084 0.084
+
diff --git a/tutorial/detection/object/cube.mpeg b/tutorial/detection/object/cube.mpeg
new file mode 100644
index 0000000..a081b02
Binary files /dev/null and b/tutorial/detection/object/cube.mpeg differ
diff --git a/tutorial/detection/object/cube.wrl b/tutorial/detection/object/cube.wrl
new file mode 100644
index 0000000..759f76c
--- /dev/null
+++ b/tutorial/detection/object/cube.wrl
@@ -0,0 +1,34 @@
+#VRML V2.0 utf8
+
+DEF fst_0 Group {
+children [
+
+# Object "cube"
+Shape {
+
+geometry DEF cube IndexedFaceSet {
+
+coord Coordinate {
+point [
+ 0.000 0.000 0.000,
+-0.084 0.000 0.000,
+-0.084 0.084 0.000,
+ 0.000 0.084 0.000,
+ 0.000 0.000 0.084,
+-0.084 0.000 0.084,
+-0.084 0.084 0.084,
+ 0.000 0.084 0.084 ]
+}
+
+coordIndex [
+ 0,4,5,1,-1,
+ 1,5,6,2,-1,
+ 6,7,3,2,-1,
+ 3,7,4,0,-1,
+ 0,1,2,3,-1,
+ 7,6,5,4,-1]}
+}
+
+
+]
+}
diff --git a/tutorial/detection/object/cube.xml b/tutorial/detection/object/cube.xml
new file mode 100644
index 0000000..8c9eccb
--- /dev/null
+++ b/tutorial/detection/object/cube.xml
@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>7</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>5000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ </ecm>
+ <sample>
+ <step>4</step>
+ <nb_sample>250</nb_sample>
+ </sample>
+ <camera>
+ <width>640</width>
+ <height>480</height>
+ <u0>338.7036994</u0>
+ <v0>234.5083345</v0>
+ <px>547.7367575</px>
+ <py>542.0744058</py>
+ </camera>
+ <face>
+ <angle_appear>89</angle_appear>
+ <angle_disappear>89</angle_disappear>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>10</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>10000</max_features>
+ <window_size>5</window_size>
+ <quality>0.01</quality>
+ <min_distance>5</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+</conf>
+
diff --git a/tutorial/detection/object/cube0001.png b/tutorial/detection/object/cube0001.png
new file mode 100644
index 0000000..378dae7
Binary files /dev/null and b/tutorial/detection/object/cube0001.png differ
diff --git a/tutorial/detection/object/cube0150.png b/tutorial/detection/object/cube0150.png
new file mode 100644
index 0000000..80dd4bb
Binary files /dev/null and b/tutorial/detection/object/cube0150.png differ
diff --git a/tutorial/detection/object/cube0200.png b/tutorial/detection/object/cube0200.png
new file mode 100644
index 0000000..a128cc3
Binary files /dev/null and b/tutorial/detection/object/cube0200.png differ
diff --git a/tutorial/detection/object/tutorial-detection-object-mbt.cpp b/tutorial/detection/object/tutorial-detection-object-mbt.cpp
index c05e3b8..92c9fdc 100644
--- a/tutorial/detection/object/tutorial-detection-object-mbt.cpp
+++ b/tutorial/detection/object/tutorial-detection-object-mbt.cpp
@@ -1,15 +1,16 @@
//! \example tutorial-detection-object-mbt.cpp
-#include <visp/vpConfig.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpKeyPoint.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpIoTools.h>
int main(int argc, char ** argv) {
-#if defined(VISP_HAVE_OPENCV) && ((VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG))
+#if defined(VISP_HAVE_OPENCV) && ((VISP_HAVE_OPENCV_VERSION >= 0x020400) || defined(VISP_HAVE_FFMPEG))
//! [MBT code]
try {
std::string videoname = "teabox.mpg";
@@ -127,7 +128,6 @@ int main(int argc, char ** argv) {
keypoint_learning.setMatcher(matcherName);
//! [Keypoint code config]
}
- //! [Keypoint declaration and initialization]
//! [Keypoints reference detection]
std::vector<cv::KeyPoint> trainKeyPoints;
@@ -224,7 +224,7 @@ int main(int argc, char ** argv) {
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
}
diff --git a/tutorial/detection/object/tutorial-detection-object-mbt2.cpp b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp
new file mode 100644
index 0000000..c83eca3
--- /dev/null
+++ b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp
@@ -0,0 +1,335 @@
+//! \example tutorial-detection-object-mbt2.cpp
+#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/vision/vpKeyPoint.h>
+#include <visp3/core/vpIoTools.h>
+
+
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020400)
+void learnCube(const vpImage<unsigned char> &I, vpMbEdgeTracker &tracker, vpKeyPoint &keypoint_learning, int id) {
+ //! [Keypoints reference detection]
+ std::vector<cv::KeyPoint> trainKeyPoints;
+ double elapsedTime;
+ keypoint_learning.detect(I, trainKeyPoints, elapsedTime);
+ //! [Keypoints reference detection]
+
+ //! [Keypoints selection on faces]
+ std::vector<vpPolygon> polygons;
+ std::vector<std::vector<vpPoint> > roisPt;
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
+ polygons = pair.first;
+ roisPt = pair.second;
+
+ std::vector<cv::Point3f> points3f;
+ vpHomogeneousMatrix cMo;
+ tracker.getPose(cMo);
+ vpCameraParameters cam;
+ tracker.getCameraParameters(cam);
+ vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
+ //! [Keypoints selection on faces]
+
+ //! [Keypoints build reference]
+ keypoint_learning.buildReference(I, trainKeyPoints, points3f, true, id);
+ //! [Keypoints build reference]
+
+ //! [Display reference keypoints]
+ for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
+ vpDisplay::displayCross(I, (int) it->pt.y, (int) it->pt.x, 4, vpColor::red);
+ }
+ //! [Display reference keypoints]
+}
+#endif
+
+int main(int argc, char ** argv) {
+#if defined(VISP_HAVE_OPENCV) && ((VISP_HAVE_OPENCV_VERSION >= 0x020400) || defined(VISP_HAVE_FFMPEG))
+ //! [MBT code]
+ try {
+ std::string videoname = "cube.mpeg";
+
+ for (int i=0; i<argc; i++) {
+ if (std::string(argv[i]) == "--name")
+ videoname = std::string(argv[i+1]);
+ else if (std::string(argv[i]) == "--help") {
+ std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
+ return 0;
+ }
+ }
+ std::string parentname = vpIoTools::getParent(videoname);
+ std::string objectname = vpIoTools::getNameWE(videoname);
+
+ if(! parentname.empty())
+ objectname = parentname + "/" + objectname;
+
+ std::cout << "Video name: " << videoname << std::endl;
+ std::cout << "Tracker requested config files: " << objectname
+ << ".[init,"
+#ifdef VISP_HAVE_XML2
+ << "xml,"
+#endif
+ << "cao or wrl]" << std::endl;
+ std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
+
+ vpImage<unsigned char> I;
+ vpHomogeneousMatrix cMo;
+ vpCameraParameters cam;
+
+ vpMbEdgeTracker tracker;
+ bool usexml = false;
+#ifdef VISP_HAVE_XML2
+ if(vpIoTools::checkFilename(objectname + ".xml")) {
+ tracker.loadConfigFile(objectname + ".xml");
+ tracker.getCameraParameters(cam);
+ usexml = true;
+ }
+#endif
+ if (! usexml) {
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(7);
+ me.setThreshold(5000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ me.setNbTotalSample(250);
+ tracker.setMovingEdge(me);
+ cam.initPersProjWithoutDistortion(547, 542, 339, 235);
+ tracker.setCameraParameters(cam);
+ tracker.setAngleAppear( vpMath::rad(89) );
+ tracker.setAngleDisappear( vpMath::rad(89) );
+ tracker.setNearClippingDistance(0.01);
+ tracker.setFarClippingDistance(10.0);
+ tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
+ }
+
+ tracker.setOgreVisibilityTest(false);
+ if(vpIoTools::checkFilename(objectname + ".cao"))
+ tracker.loadModel(objectname + ".cao");
+ else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ tracker.loadModel(objectname + ".wrl");
+ tracker.setDisplayFeatures(true);
+ //! [MBT code]
+
+ //! [Keypoint declaration]
+ vpKeyPoint keypoint_learning("ORB", "ORB", "BruteForce-Hamming");
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ keypoint_learning.setDetectorParameter("ORB", "nLevels", 1);
+#else
+ cv::Ptr<cv::ORB> orb_learning = keypoint_learning.getDetector("ORB").dynamicCast<cv::ORB>();
+ if(orb_learning != NULL) {
+ orb_learning->setNLevels(1);
+ }
+#endif
+ //! [Keypoint declaration]
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX display;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI display;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV display;
+#else
+ std::cout << "No image viewer is available..." << std::endl;
+ return 0;
+#endif
+
+ /*
+ * Start the part of the code dedicated to object learning from 3 images
+ */
+ std::string imageName[] = {"cube0001.png", "cube0150.png", "cube0200.png"};
+ vpHomogeneousMatrix initPoseTab[] = {
+ vpHomogeneousMatrix(0.02143385294, 0.1098083886, 0.5127439561, 2.087159614, 1.141775176, -0.4701291124),
+ vpHomogeneousMatrix(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828),
+ vpHomogeneousMatrix(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025)};
+ for(int i = 0; i < 3; i++) {
+ vpImageIo::read(I, imageName[i]);
+ if (i==0) {
+ display.init(I, 10, 10);
+ }
+ std::stringstream title;
+ title << "Learning cube on image: " << imageName[i];
+ vpDisplay::setTitle(I, title.str().c_str());
+
+ vpDisplay::display(I);
+
+ //! [Set tracker pose]
+ tracker.setPose(I, initPoseTab[i]);
+ //! [Set tracker pose]
+
+ //! [Refine pose]
+ tracker.track(I);
+ //! [Refine pose]
+
+ //! [Display tracker pose]
+ tracker.getPose(cMo);
+ tracker.display(I, cMo, cam, vpColor::red);
+ //! [Display tracker pose]
+
+ //! [Learn cube call]
+ learnCube(I, tracker, keypoint_learning, i);
+ //! [Learn cube call]
+
+ vpDisplay::displayText(I, 10, 10, "Learning step: keypoints are detected on visible cube faces", vpColor::red);
+ if(i < 2) {
+ vpDisplay::displayText(I, 30, 10, "Click to continue the learning...", vpColor::red);
+ } else {
+ vpDisplay::displayText(I, 30, 10, "Click to continue with the detection...", vpColor::red);
+ }
+
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I, true);
+ }
+
+ //! [Save learning data]
+ keypoint_learning.saveLearningData("cube_learning_data.bin", true);
+ //! [Save learning data]
+
+ /*
+ * Start the part of the code dedicated to detection and localization
+ */
+ //! [Init keypoint detection]
+ vpKeyPoint keypoint_detection("ORB", "ORB", "BruteForce-Hamming");
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
+ keypoint_detection.setDetectorParameter("ORB", "nLevels", 1);
+#else
+ cv::Ptr<cv::ORB> orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
+ orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
+ if(orb_detector != NULL) {
+ orb_detector->setNLevels(1);
+ }
+#endif
+ //! [Init keypoint detection]
+
+ //! [Load teabox learning data]
+ keypoint_detection.loadLearningData("cube_learning_data.bin", true);
+ //! [Load teabox learning data]
+
+ //! [Create image matching]
+ vpImage<unsigned char> IMatching;
+ keypoint_detection.createImageMatching(I, IMatching);
+ //! [Create image matching]
+
+ vpVideoReader g;
+ g.setFileName(videoname);
+ g.open(I);
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display2;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display2;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display2;
+#else
+ vpDisplayOpenCV display2;
+#endif
+ display2.init(IMatching, 50, 50, "Display matching between learned and current images");
+ vpDisplay::setTitle(I, "Cube detection and localization");
+
+ double error;
+ bool click_done = false;
+
+ while(! g.end()) {
+ g.acquire(I);
+ vpDisplay::display(I);
+
+ //! [Insert image matching]
+ keypoint_detection.insertImageMatching(I, IMatching);
+ //! [Insert image matching]
+
+ vpDisplay::display(IMatching);
+ vpDisplay::displayText(I, 10, 10, "Detection and localization in process...", vpColor::red);
+
+ double elapsedTime;
+ //! [Matching and pose estimation]
+ if(keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
+ //! [Matching and pose estimation]
+
+ //! [Tracker set pose]
+ tracker.setPose(I, cMo);
+ //! [Tracker set pose]
+
+ //! [Display]
+ tracker.display(I, cMo, cam, vpColor::red, 2);
+ vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
+ //! [Display]
+
+ keypoint_detection.displayMatching(I, IMatching);
+
+ //! [Get RANSAC inliers outliers]
+ std::vector<vpImagePoint> ransacInliers = keypoint_detection.getRansacInliers();
+ std::vector<vpImagePoint> ransacOutliers = keypoint_detection.getRansacOutliers();
+ //! [Get RANSAC inliers outliers]
+
+ //! [Display RANSAC inliers]
+ for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
+ vpDisplay::displayCircle(I, *it, 4, vpColor::green);
+ vpImagePoint imPt(*it);
+ imPt.set_u(imPt.get_u() + I.getWidth());
+ imPt.set_v(imPt.get_v() + I.getHeight());
+ vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::green);
+ }
+ //! [Display RANSAC inliers]
+
+ //! [Display RANSAC outliers]
+ for(std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
+ vpDisplay::displayCircle(I, *it, 4, vpColor::red);
+ vpImagePoint imPt(*it);
+ imPt.set_u(imPt.get_u() + I.getWidth());
+ imPt.set_v(imPt.get_v() + I.getHeight());
+ vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::red);
+ }
+ //! [Display RANSAC outliers]
+
+ //! [Display image matching]
+ keypoint_detection.displayMatching(I, IMatching);
+ //! [Display image matching]
+
+ //! [Display model image matching]
+ vpCameraParameters cam2;
+ cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(),
+ cam.get_u0() + I.getWidth(), cam.get_v0() + I.getHeight());
+ tracker.setCameraParameters(cam2);
+ tracker.setPose(IMatching, cMo);
+ tracker.display(IMatching, cMo, cam2, vpColor::red, 2);
+ vpDisplay::displayFrame(IMatching, cMo, cam2, 0.05, vpColor::none, 3);
+ //! [Display model image matching]
+ }
+
+ vpDisplay::flush(I);
+ vpDisplay::displayText(IMatching, 30, 10, "A click to exit.", vpColor::red);
+ vpDisplay::flush(IMatching);
+ if (vpDisplay::getClick(I, false)) {
+ click_done = true;
+ break;
+ }
+ if (vpDisplay::getClick(IMatching, false)) {
+ click_done = true;
+ break;
+ }
+ }
+
+ if (! click_done)
+ vpDisplay::getClick(IMatching);
+
+#ifdef VISP_HAVE_XML2
+ vpXmlParser::cleanup();
+#endif
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+ }
+ catch(vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl;
+#endif
+
+ return 0;
+}
diff --git a/tutorial/grabber/CMakeLists.txt b/tutorial/grabber/CMakeLists.txt
index bf7b788..83d45a0 100644
--- a/tutorial/grabber/CMakeLists.txt
+++ b/tutorial/grabber/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-grabber)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_sensor visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/grabber/tutorial-grabber-1394-writer.cpp b/tutorial/grabber/tutorial-grabber-1394-writer.cpp
index d764956..cc02717 100644
--- a/tutorial/grabber/tutorial-grabber-1394-writer.cpp
+++ b/tutorial/grabber/tutorial-grabber-1394-writer.cpp
@@ -1,12 +1,12 @@
/*! \example tutorial-grabber-1394-writer.cpp */
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
-#include <visp/vpVideoWriter.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/io/vpVideoWriter.h>
int main(int argc, char **)
{
-#ifdef VISP_HAVE_DC1394_2
+#ifdef VISP_HAVE_DC1394
try {
bool save = false;
if(argc == 2) {
diff --git a/tutorial/grabber/tutorial-grabber-1394.cpp b/tutorial/grabber/tutorial-grabber-1394.cpp
index ea41cfc..bb17fc7 100644
--- a/tutorial/grabber/tutorial-grabber-1394.cpp
+++ b/tutorial/grabber/tutorial-grabber-1394.cpp
@@ -1,11 +1,11 @@
//! \example tutorial-grabber-1394.cpp
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
int main()
{
-#ifdef VISP_HAVE_DC1394_2
+#ifdef VISP_HAVE_DC1394
try {
vpImage<unsigned char> I; // Create a gray level image container
bool reset = true; // Enable bus reset during construction (default)
diff --git a/tutorial/grabber/tutorial-grabber-CMU1394.cpp b/tutorial/grabber/tutorial-grabber-CMU1394.cpp
index a7b527d..7082977 100644
--- a/tutorial/grabber/tutorial-grabber-CMU1394.cpp
+++ b/tutorial/grabber/tutorial-grabber-CMU1394.cpp
@@ -1,7 +1,7 @@
/*! \example tutorial-grabber-CMU1394.cpp */
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpImage.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpImage.h>
int main()
{
diff --git a/tutorial/grabber/tutorial-grabber-opencv-bis.cpp b/tutorial/grabber/tutorial-grabber-opencv-bis.cpp
index aed7af6..eb5d1b7 100644
--- a/tutorial/grabber/tutorial-grabber-opencv-bis.cpp
+++ b/tutorial/grabber/tutorial-grabber-opencv-bis.cpp
@@ -1,7 +1,7 @@
/*! \example tutorial-grabber-opencv-bis.cpp */
#include <stdlib.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImageConvert.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImageConvert.h>
// usage: binary <device name>
// device name: 0 is the default to dial with the first camera,
diff --git a/tutorial/grabber/tutorial-grabber-opencv.cpp b/tutorial/grabber/tutorial-grabber-opencv.cpp
index 2586e24..1debe79 100644
--- a/tutorial/grabber/tutorial-grabber-opencv.cpp
+++ b/tutorial/grabber/tutorial-grabber-opencv.cpp
@@ -1,6 +1,6 @@
/*! \example tutorial-grabber-opencv.cpp */
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpOpenCVGrabber.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
int main()
{
diff --git a/tutorial/grabber/tutorial-grabber-v4l2.cpp b/tutorial/grabber/tutorial-grabber-v4l2.cpp
index ff9087b..ee7de4a 100644
--- a/tutorial/grabber/tutorial-grabber-v4l2.cpp
+++ b/tutorial/grabber/tutorial-grabber-v4l2.cpp
@@ -1,7 +1,7 @@
/*! \example tutorial-grabber-v4l2.cpp */
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImage.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
int main()
{
diff --git a/tutorial/grabber/tutorial-video-reader.cpp b/tutorial/grabber/tutorial-video-reader.cpp
index cb7dd46..bd982d7 100644
--- a/tutorial/grabber/tutorial-video-reader.cpp
+++ b/tutorial/grabber/tutorial-video-reader.cpp
@@ -1,10 +1,10 @@
//! \example tutorial-video-reader.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
//! [Include]
-#include <visp/vpTime.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
/*!
diff --git a/tutorial/grabber/tutorial-video-recorder.cpp b/tutorial/grabber/tutorial-video-recorder.cpp
index b00d4d7..c6cd4a1 100644
--- a/tutorial/grabber/tutorial-video-recorder.cpp
+++ b/tutorial/grabber/tutorial-video-recorder.cpp
@@ -1,11 +1,11 @@
/*! \example tutorial-video-recorder.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpTime.h>
-#include <visp/vpVideoWriter.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpVideoWriter.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
/*!
This example allows to record a video from a camera.
diff --git a/tutorial/image/CMakeLists.txt b/tutorial/image/CMakeLists.txt
index 3f72d7f..7067db4 100644
--- a/tutorial/image/CMakeLists.txt
+++ b/tutorial/image/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-image)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_io visp_gui)
set(tutorial_cpp
tutorial-image-converter.cpp
@@ -13,11 +13,11 @@ set(tutorial_cpp
tutorial-viewer.cpp
tutorial-image-filter.cpp)
-list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/lena.bmp" )
-list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/lena.ppm" )
-list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/lena.pgm" )
-list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/lena.jpeg" )
-list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/lena.png" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/monkey.bmp" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/monkey.ppm" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/monkey.pgm" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/monkey.jpeg" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/monkey.png" )
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/chessboard.pgm" )
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/camera.xml" )
diff --git a/tutorial/image/monkey.bmp b/tutorial/image/monkey.bmp
new file mode 100644
index 0000000..8159f97
Binary files /dev/null and b/tutorial/image/monkey.bmp differ
diff --git a/tutorial/image/monkey.jpeg b/tutorial/image/monkey.jpeg
new file mode 100644
index 0000000..71ac7ea
Binary files /dev/null and b/tutorial/image/monkey.jpeg differ
diff --git a/tutorial/image/monkey.pgm b/tutorial/image/monkey.pgm
new file mode 100644
index 0000000..d14c682
--- /dev/null
+++ b/tutorial/image/monkey.pgm
@@ -0,0 +1,298 @@
+P5
+256 256
+255
+
<24 9'4 <222&&<2SESS2UE
*2 S4
h$ 4&4
'K<?dC42d^YYRF6
AOO?A
'D
?%'?dC
).
d68r^86C0$C)6KnAC+6RW6& )@T0
03,W$WW))6ccW6;OR"E"4=IPCKTEErR,&C864.
6)4;&406%286ERC&6
;yCcWKEcK+8F^.D626%;$
-mk]MRIW�mh6)K~]@SKU]W]:SRA=Ciii2
', 4D$
&
2v&9<,&&ESS&2SE2C&EU&42444
U?'?44%
6TTnYC-YYR[48&446?022R0
4?')?+%"IRAO2AE4WE&& )0&+ %,;&&%@8)
%6&WO@)
8)O'"Od3OWEC@&=E) 6"%&&C,444)
4WW@@ 4 "2O&.OWVEOO
PcCT$8RRT+8R2FDF4,48==mmF46I=Woh]RDh]IIKC{m?K4UW6.-][C !!')
]*<<E9
S�]<&
2{S222292D2EE44WR4DS:?mF?mIRW4WFRWR&&%C)4 0
RC)
Ad4A.)A% IFTR8+&2@)%
)4)
"
&"6
0@ % ?O&
%8@^OBOR48ERR6")O466
,.
+ && 4%
6"&)E)6E36ccdcr=8rT? $RK+48R IO^d[YmnmYCRKY==kkA]M%:<:@4IK-Si[U64.-=-4ES4&
,!4
/$&&S1E*V9&EV<EE&22�E4&
4h4 S?24SkdS,'
DR4WYA=)R0RcOR; .%
@& ?6,C�C)[+8.Cdd6EEA0
&)
+&))) &4%%0
% 46& &
8OO
@ ) &) "OO8) C
%)
&'
@R=;OvVWdEWR$ ,C0KW2 $Rdmmn.8TdY&6RkmR<UK[]:K<'C[444:CM)4R?,%UW]{[4'9
ES$
*V22
&&49<9&2*2V<C4VSSVE&EE2&42'4UU649'424[? ?446-ddRA?0+d[d=
%@)
A@&&)0A&8AR<
0 ;Wc64;=&R3 +C& C.46% %&0%%2&
+40"
40
&,&2
& E&
"OR888=.=LPcWETc+TCOC.nI6F6m[=M$"8d6'YOM~6:[{`Uh-<MWD4RFRUhW8C.,hhiv{I!C4
1
*VV2&*ESSEEVE2 222]S4ESS{VE24ESR4SRk%
''2'4S4
DRd84282;TI&4W@&&@&A4%"4?+46$C4 [...]
+
)&&2&
)$
&%
4
&
%
)64"
46
"T@)
'0.E4RR68.=Kcc;8cW WcE,;6=m08A$"2TAT2-6<?R=+D]PK`AU~o]h~hDUihYRIU2 at OOI9*:"
<*&,9SE<VE<<]C22&
ESS?RSSE222Rh~URhR4
R??42R4')
RR??+%
)82 &,%0 at C4220%2O+&6,66&0
& )&
+
0
@B))3R0&
O8 )
0
,O3%6,,%&8)@6,=EKWBd=cB",cdCO4,"468)R0
K,)?$IRdO),9C=<FOK&2.:~k[{{iUDChkD6D4%,49C''2S'''
&<
9h2E2<&Sh2&
4E{SCSSV2
2ShS2S{SR2
?RRRD2U?2)2E4DAW@ @;C64&,=@OC0 2+
&
2&
&
+
+
+
+4 at 3
%
&0&@)
33R^R0'RdW6
66 46&466C.)664 at 8;.&;Rccc=cWP;KRTW at 646 .CAD+28"[RYY=2!-MRDYkk-)88U[[i at 9{hKUhhDCR'44UUh~[,9VE*
22 &
E,*<E244SSS&'4SVRCSSS4&S{4&<4UR'R~h?4)44F?D)2A)%%OyROO686;644 +
+
+%
"
%2
+
+
+
%
+
&
))"@@BORcR03 at dR=8" 3
.36)& ,8))=TREWKOO.E6KccR=WRPWVWdR6OR6,
%!4!
OadP.6<hmO).WhMCR8RhiS$
:[kahhhkWCDU9KU]S
E2 &4E&<2 <E<&R[]]S2ESESSECSRE4CS{{44URUhh4444
'F4DTCA8TccTOO464
6"&2R
")&0
)&
,EO.
+
"
&3
+
'&3:3)E^RVOyyRCRcR6)@E6. 4" )88 at WrrcRBOcWW?KVWcdV@;R86.,OR6?;)
'
"08?E ?KW&"IdIM]~YY`[U,':UCD]hMU]UiI9'$
CC4DR?C2&
SU49S24CESE22VS2SC44S24S4SSR'44
4SS!-
?)' dT0
C4&,&4 )"
&0$+4.&!4
+@%%
&)468),=,46
63)346
,83)%&
R at R^R:pydyRyTcv~O006
"@W at EcdRWRcc=WVWWE,;W,40;3. 66.,3&,22;6$C, AI,%<4","$
-.!k�R86MoWD.4S4444:),2hMCC?,*4!
4244'
'&2
&S2<�iEV]C&E{V2&E{]SSSSSRCS24CC?RDCSWK&''4
4
4'2
AW
0)68&46E0 at WE4" . )
+ 6B==B&84;)' )6)&
RR)4)R8 at 63@:@RLcvOydRyydROcmR4OWc&,R==;6RWRB=WWd6 at B@B=KcE at 6&604 )@R466& &44A&TE$"R+ , &-Wk4 4<SiU6DWFYkh:'4hm444DSUC4KUY4,9C4E42&
&<V9<VI4<S[US]]S22SUEE2ES{RCRRChRC'4U4 2S2
+&0T4?C= "4R. at 8OE.68W8&);
.&
+)6;E$".$6W8&).8BB6 at 3 )RO)
+;ER8BRdBORddO~cvycR4C at 0
&&%
+
KR6,
RBTB)EEO8"
"&ER8046
, +, 4R
!6.=II
IY-
,%@[hdkmddkWDYYYaUhh{[[{h]M=&9C4
*<2&&',2$&*&
Si4SC22ESSES2C&4{USS4&4RCRD
4444'
&O2%8&
%6O at +;$)
0& 8C& ,KO&
,"""PR6KC=$RBEmRyR,R=0). at vO@@R^RdvvRyddRcR40
%
+"63. &
) 2
2C "
$686.
-4
<):U2-MkkkhCFRWdYhk~aM[�{WIFO4' '4 2* <V$
C4&'4
''S{SS&&22&4&44&&S44&'%4C'
&-
C6
%44A )
$)6
&6%&& &)
&4
+"..EB$==WyRPW�PTW&
)RR at B)@OERWV=.WVRP..
%)
+
+
&&
+
0
% !4600
,-:MM"2'
"FkhhkhYkRCUSM-UW[UYFDS9'
&
'&&<&/ 'U'CSE*'4{{[S?EC2&24ShR4244URS4'4'
'%4+
))&)0&
&
&
"&")'.EEB8WRdcRB3&
@WO8'@B8PER",RB8 &
+
"
+
+
+
&
+
&)!,:D4''4)%6=<i~~~{hW]{F)'Rh[hYU[iS44
'-&2&2&&4
hC&<S?9U[{KE{US]S2ESUUhS?
&2 '2&')
%%
+
%
+ &&&
+
+
+4C,;EE)6===cORO&& "
&03ERBBR3 at 0&
+ &2&
2622
,
4MUK4'44DWWUh~hi{S[{[K4A?U[K4*,>ESE4")FR2&EE44i{E&2U$&i{[RU{SSSE&&S{{hhR4<2
4?)44
'
)
+
")8")8)))ccR3
))
). ) ,4
%
+%% "
-D)
!,PkWhD:][K~]ISIKDC9[hdFA6 *]SI4
-442
&VV]i9*]]9'4 <]ES[U[?C444D{�S&44
'
4'
-44
+
+
+
.'@8E8)
0
0&
+
0'")&
+
+
+
$
%?
"+!%6.<D?]UDUACC%EU?4KhRF+'44!E]S'%CD44S4
&&&&EVV2]{IW4&SS<E2SCCR4CUE2
'6D44'9
&
'
&4)"
%
3B8,@
+
+
)"4)66OFRMKD4U44ID4[]h{��k2)F6!2SS4
& 2C4)<E&&*<<EE*[SE]4'ESSS]S42[{{hS&
'2kh+!
' "4'
+
,&
+
44
+
+
+
+
%
8I++C[[dn~d4-:[[hkM[[UC6Oo~t~~kd8'6SU4&
4E4Dh?!<EVV&E< $$&&9S{SC4iSE*ES?C[US4' '4:KC'!-).
"
C,
+
+
+
+
+
+
2
$+6F`OY<&
,IUmk`F88Rd`{~~~M8KaMDDD?444CD4
2/222
<V<
4Sih{h{i2
4SSUDC)'9CE*&&
+
+
+
+
+$////IV[[[I];//
+
++)+8.4C4&,ADMYYn[^F]~{kWRDC-
)DYdRD?'
& 2[VECC4)&ESE]�SEE* E[hUSR:4[2"
")"".
+
+
+
2+C0& )$;;;<[]]]]]]66]66'@
+
6FA6;E[KRD6.)'CDW~m~kYU4-4]OUUUUCC4'44<=WRD4CC
2<*VVV2[i[]]S/ES9D4<S{R4,CS:FdMD4%
+
".
$
+
+
+
+
+
+
+
2&
%&
2";A,O0
+8/66]II6<;;6]]]]6];&O3
&
+
?8FKIOO`k?))6C<:A`~]:,4IUhi{[[[]�[U6)
"<]U)
&2 2
9]S<&[SEVV<&9SihC4499Ck?449::CC.!
+
!.
+
+
+
+
+
+
$"66666
+
+%& &C0&
112"A424&
+32;[];]]6
/6IVI]I/?R
&A0%
% $++"Md`kF2-KU:SU6-1CD=h~~~hhKCUhhiE" U4
&* E**4*'
CES]S )hh9
*EE9C{h[SUhSC4 -.,"$"
+
+%
6666LLL]66"66L666.")
46
+
%%
+
&12
)8)%
O<I]]I]6&4D6]]V;;6E^%&)A8%
+ 2I4+&++6DFYYO2)4Fm{W4!-=]kk{hF.",CURUF<<<=WkWRU42*2*&2]EEE*42CU49<4<I{{C222,4SUIShhWUW4 '-4,4-
+8
+
0466]6]L6
6666666:
482
+
+
+"&2<2"2)%
+
+O<II]]V;
:K
6]]]];;2y0
24+8)6A+ % &"
48$.6,)%EOaaI8)"4464:CD68Rk~~~~R)!!C[{{R4'A`mM:-::CSU4U <9$<E9ES]{<99DK]i�{S2,E{{{~{~U44D6446'
+
+
+@)6]L6]L6
,"666LL at 8
0,2
+
+
&%2&.6C40
O9[[I]]V/
6]]6]]I4^
682IT;?4
&
$+8?Y$+;I=A6")DDRRR`k`YFMYK[]6!DSK<RkkWM6&4
!4KWMEE&/<E9*<1<E>,:UD9IE]�{{US]SDSkkUk`hhFDUD+!"&"
+
+
+
+O';666L6
M
666LL at B& )4)
"@3
<)2& @4;I];]]];6]]]66];U^ 8ROnOF4%& &%+,,,6 )+"<Y^64?Od[RAFDFK<I]M.&FC42CE]SDWiSC'
!!CD4)"&</ES]h&24DI][SUESVVS{�hkhh~WS4:[UWD<<)-+"
,+
R6O)6]66L]6
666L;OO
+%;2?''
/
%&Y6
%0/6]]]]]]]]]]]]]"6[/OB =PRaRI22&&
) & E=)=8?="
0FYk~aYYkkYMWW4'44,94,4:41S]?C{SUKK<CC'
$$*]]{S V<
1&9E]]S?S]i�VCC4UWWYYkkM?6+".6
%4
+
+2$ %&!0)666]]]]6
6L866L;P'
226+-
+
?I%
A26]]]]]]]]]]]6]66;4R F?OYd[C?&) , + ,$Cn=+++ $<YO="CD,-Ak`O6RSSC444D4%"
Sh{�S4-C4 -444&4C&1$<<E]< V{D 4*4S[hUUS[S[U9:UU]UK2DW<&80)0.8$I8
"
%&+A;FA&
00&6]]LL]]LLLLLL66666K%2= '
&
2F?
0VV]]]]]][I66]];/*R
"6.`[dTC;64&&.$I, +IC+8T+&-Yn^[[F''-!
,C<=DUhhR?h{kR4?dkkUD4!*II442
*ES $<1&,&.)$ISC9,C[[]ihhkhCKWk~~kU[M6Y]M]OD==F.$8=$%%
06CI6=?A0
0
66]LL]LL]]LLL6L661.
8+)!
!64
0/6]]]]I]V]]]66&A3
DI=cOTRC at 4)$$ +I4+;CF+$
, <m�nA))44F64C9IU[{[D?C~kR4R`MR6&,>S:*'&
9*&&<E'*<**
E9,%9[S9 &,&II[{S[SK[{W[]]W<2MYMDA..?I8K,"% ++
)),4CTF6ADD
06L]]]6L]]]]66]6;E
*82=
)D6'
,*M
/<;]]Vs]]]6/'03
2YarR[R.4) ,
)$,W+C6+8 %CMC:4+)4DFRRDRCW[Uh]S4SDCSKRY82+
'MhS4CE2
'
*V*
$2VS'EV]E</* 'RWKCS[[[[<-DD<:9[I<FWkF6=A[O0$46C $$2 , &2CKPRd2
66]]L]66]L]]661
+%
&
%3&RY2
$*[
/<6I]I6/&&3
0
OIOOnRK42+& + 8IE=88MA.4Y[OF<RkhkkkWFFWYR{{hD?h{hWYWUD4
.42FdWURSS4<<22&
<9*1S]V9$* 4Ei[]D-4K[]{~YMC249C6-<6&Fn040,,I;" 4)43E at CII=%+
+
6666]][]]]66%
>
+)+
*'
238[C%
A]%
%@;IaMR^F8&)&$ 468I$EF8=$IamA)6`h`MU[WhWWWUR)?~hRRddURRkkhU4'-hmFFDDhU<4,*/<<2& 9% 9]]M]]99<2CCUDhSE9!P~~`FYkh="=46.O=?P[[O8"&C&
6%366 at WIF8
'
6666];""
$
68&
:!
2%"-I4&&
+�v>
OO
4AAa`[R2+$&&2$C4$+A+
%+OndYFFMWFIIAFRUF.)?Rh~RdddRRRD4-,-S{h4,)
%?4
'V<&229'EE92!!4]]VEEE9CUhU[[WSSE<*1SihIOK:,K&FM.6OYC8$288I44 ,4&4TRR?-)4
1$,448%
&9,
I&0&0
@&
4O6
IA;<I?RK4+"% 8O44 %48EIR[666FRICdmmdRUSRYmR?ARddn~~hIR44W~i4 !))-42' 4$/9<&&<E9SK4*EO]>9[~~~{{UC2E]E$:O% ,>@hh:!!4FA0
CY.8)++))
&06COI06M2
0
4:%
+ &;C8
:40
)0
?C4<,?c=^C.%$ &
+"I 6FI4, CIIIF=FARRdmmYdYdkh?AR~�~m~h[644[]C'"D`kR4C9*
,EV$&<V2**&E9 *I]V>9:O[ihU[{~U9**9hU4,44?UhW<-DR.=0,[I.4I$
2I 4CC60R??+44%
00-)
R= AR
$"133
2A
+EART at 0A)0
)
3333C)2^R=OIdnP;+4
8R,%, A4) ";IYYICAI[n��mY`~~~kamkkWU[][h{hK424<W`YII4*
2<&<
&<E
&&S:!%KI9**&
&C[h{�{UUUK:RhIWhkU4.==R$%YmkA+CI"&;)C&&;0&0008AA?%00
%
+;Ec at 26444?
"1%
1
%$2$
42C;4,4 at O
0R at O33RORR@@)
0TTI??=?PnC4
&
- ,+ % .I++,+CnnmT^n�dRDMk~~h~~dYdR4DUkh]??44CCU[S4'!4CC,&&4&<&
EES< <EK{i]E<<4ES{]]iV[]MMKM{~�{[{]KM=?=08Iaa$
4&+4&4&
2 at 4ARC
%44
%
),6=WPn����nC
+
23"
%&0$
"
2CT; &4C)
%)0& C[rTI=?[TTI)
+ "6A= +
+WO8 )[Ra~ndRIA-)Sh]kWMOFRW4CUS{~kddRR24ISE,':KI?44?4$
&&49I>ISS4, 'Ci{U,'4S4S{[U[S<,,499Eh�iiSK-&%).&,8M.02A6+4,+C%
%%%0AR4%
%?A)
)8)O33@@OO6&6, WWIEc[I6+
1%""":"1'A?+
&%FWI8 %46C&
&6C^nTF80&cPOK
&$$4C,&CIY2;A8F[YYR+)ARDWk~[UDFYmWDD?DRWW??RdaWKSSC,4%'4US?44
49ESIEUIUC441IK{ihWKUhhhKKSK!4UhDCk~~hR.DMRF84,I8??A.
&&K&
&%&&C2)'%8I4%
&%&
%$$,)0;04[I %&
$
""/3""""1'3384%@A
C0;EIKIE4+.+ %%0++4+,+?wPKRIOaaC80)344 8$RC +$)8&%&.mmdOIRd[AFWk~~~YkR?<YWk~mYRR???444CS4C[WUWMUSS4$
&'2E[[E9CI99:@[]]SV2
'CUi{{�{4'4USSi�hhC''4ADDOO0";dK6&&+CI8% 40600 %42.)) &AA%%0&%%%0%%&&&"
,TC4+&&
11
">P'/1A13CO6)
00
2 ,8K4KII^P;@4C+$.8IKPO=[kwwP=.8
),&,+ 2A4+2F44IO+'6D<.6FR=akm`][Tdwmmmk~hRRdY~~{~~dddYY`h4'4R?2 2<<< *9S]]E99'4US{hC94CCOIEIC4USU]~k]KCIC?2:M<D&FY82I8?0=0;;,&36&4&%
8+6
&&AA4&+ACOCCC8.;08=, .. at 8&&
''1
@&%""""'
'8PO000
.&.00?a`k[O3;Pk�cUrckk=0!
&)),+
%++62
484 .&0I[K)+:R=ARFRIRFDD44Rk`~�o~ndRRh{~{~kdR?RDUC4'DR?444'&9&&2<EE<,,[SKMDRIUi{hU[RCFRRW][]O:-EUh{hDD-)-6--?3"<==;0%$;82,),%%&
,$!2$4?22A2
+%4% +3".08==;O="&00&06
231
&3%/I;"1 at 1 KO600%00
&
)$$.2C3=AOPPUPcU.
"0)
&2=8)=I++8[ndRK$.?8)+6<~��mdRA2)4U[WFUkmhR?6I[C,E?Di{D
!4C<[hC2
%EM&&<>MVS,!$I~�k�dW{[CDDCSii[DMM::4'4KWUK
?<<6kF88I?O?+)))4$;64& %
+
%48D=IcP=30&OPCCC^C?P?&0$)
63&
+&1"'3U':1;.:1%
?W606
)&33"&66&+2
&$ 3&
"
34)03 ,4 "0RR, "PO?OR,
$<Uk`M=?6D]UF<hhkkdk~~dR4<?Wkk?2i{kUUCD'24444CR?4*9<* S[9$$$''!<]OSIKD[{hC66DWICD4"-KUR]UhC4A?2-F<$8IP.$&C3 8=,446)
+
).=?3=O?CC=383A.A20
""
)03
+;8&3%"'"I_:"":.6::
0PB6O)&
033 at 8.)6.. &$ &"
)&360 )
)+6+%,&+ "$%4)
=8,&)+=UkYOWD8A4'4MWk~~YRRRhhdA4Mk~~kkmRF2F`hdD4??F%4
&244<$*E9<24,>E[K),:,'4UUi]]S%
!444:D]]YRIRACIY=A0
+";=&E?A;T;)44&
+
&&03&2.22"&$
! $,& ,4)
+
+,0?'"
1"@U:1:.".:11EW@@0
.:VVVVdddkdVVEE3'
,)) +=+&+&8 6D)&M[CWRIFDDRF??%4?DDCI[k~~dRd~R?4Dh~~m~nCRDUWMY]UhU4,'
92E]S< 44-9E[SIC49]SSCICUUi]]]94RD44UUFU]KYddMMDY]aF"+6+F8? "A<$;C84)
".&
+
+
+$?8&23
:tY at .;/":P'
0KW4
.EdoooouuusxxuosudoVVE3
+
)
+8.I8)&8E +"RA"68)$82T[RYT+28AdRIAFWk]hkYdRARh~U??Si{~ddRUD44FRDC4IC6
/$<<9SE4M9K[]k[]]<49E4Chk~khR<<d~YF464'44?Rh`K2=<?MFI" 48=;$,%
&$4)&
+
'
,O0'1
1_::>;;;IY12
;O&
'Lkppxuuyyysy|xyxyuuyuukk^E
"
,40+$ 6M& . 4AA)64 8?FIICTOOTd��ndA?R[k�d?CA?Yh{kYY~{kRDS44 )CRd?4?A'
*/
VV]]E-%9S]K]OICC<DU{[S]O494CU{�{{W:C??DRRWD-",4.&<3+P88$0.&
2)" ,C8,
+
"''LV^p^:.
?K?1 at .
3>UUY@::I:Y12$)
8^pp^suyxyyyy||||xx|xukkkkVLEL:8'
E4$
-6""2$6P=R8))%+A8CdddddRdn�mkRRRARmm~m~kkdk~~kkWKRRhUWWhh[SUU4444'&
<]ES[IC9E1-2E9' !19Vi[9& %9CD{{KCKU{~{hhh[9?CD&&),
..&2? &
8
+
+
+
.'",LdvyuppRK\>@P"
"'PfY;.;>;::A2
^xxs||||||||��||sxypV^LLE:L:08')36A,)
%)"<8 )&)IRE64IYmdRTAYd~��ddcWYYdk~~dddm~mkdkdRR{~DA4,4CSR44
22$$Ei,99:
'4CR:C,&%
4SSUhC'.CWh{{~hKI2KUUDKDWC4'!-
&=&&..+)
+
+
+
+
+
.EVVVVd^dVLEEVV^^my�|^'
.;C?@P@
'k@:";.6;1P@
+
^px|���������|�ym^RB860&
&)66,
)& AD)%4;6;0AWR4)-RDYYWW~~~dnm~d[R~�~kdYI[mRDWFR?44C4!'CR4
&*2&&I:4C4)4S]~{[C4!,K]i�i:4-9C:Ci�i[I&!<F~iD<C[K-M%%8 )=<"4;%
+
+
..EEEddodkoookkpkkkk^^u��yE.?C3 at U1"
3y@:";6L::YY:
'L||�|���������pRR@@340000$
)%%")"-'
-YT8IIARYdOW`akdDDWkmddmkRFARYk~~~�dRRR{~R%'D48IRD44& ES]9,,:-%IOOS[oiv]E*24SSEE at K[i[KUU2%
6)D[iK]h]]A?"
+688$&I0%
"%&
+
'..Vddkoodouyypppxxx^p^^yd=
,?31P@'
@y3"">R>;IIYY"
.m||���������ymRRERVVVVVVVEE:
)""& FT ."-??FDOAKRRT8DkRMRD')+RYRFRd�~dFWY]{~khhR~kh{hU4,
'?FUICCSC4 */VS9$*O*$:C]{h<<hhiI2!4]]9 4[ok~i<4CKDUM3CW<&=`k8CF-
"&
+"
." %
+
+
+
'Edkoouuuxxpxpppx|xxxu^^P
"\?APN"
3w3.MLML.:NYU)
B|��������ycy^^^kkssooooodddVE:8"
"
")3) 84'4.)F$+RRA $4FDYF?4448RWkRRW~~mRRkhUS?RhR26FRdR=6W~kR4&4&
4&2,2,@:
9>9>E]]S9RUSKIS[]]<<CR]]@CWhhkR-DDC2<`WmA")6D3
4
,
&
+
.LEddouxxyuup||||||||mWA'
&C3Y_ at 1 3n@>LQL6"":PUA!
$^������yyy^^yyypxuuuuoooddokVE:.' ""
)6"
), )CR+ )`kO2+ 6WYDMdd[YdhkRRWkR?FhhU:?RRR?24244'"!IShD
'4444S 9EE1!9K9>[U4,9>IE:*&C[][[UhYWhE24Kk�{h[-&
%.3"FY]3<A)-!4&+
)0"
..:33 ..EVkouxxy|x||u|���|VC:
3 at __N1 "@fNMQQQ6>:I:NYA
0m������~y^yy||xx|yyyuuoododsk^L''.""" ++ 0%
2")D-++"2+6)4C46WhM6IAAWhhhU444'2?FA?RRhRU?A?ADUWW[S:4
24
99&$&*1:KE at 119:F:><E**O]>VppM[]E*,:<[[[i]I?3
!M`]M<<K<MD"
!)
..EEVVE:." ":^pp||||||||||||ymd3'
3:f_N1" "1ZVUMQQQL.:INY@@
0v�����yRmyyy�||y|yyuuuoooodd^:83)% %4A4446+6F6.DOFR)4]kY??2:URIhkRRddFdn~~~~~hkR%)CKR4'%4C4''ES *</$$&*1$& *EE*4OU)',&,E,!
4I{{iM:@14[P:<W<.FAM:&CA
EEVddddVR')$:^pxx||||�|||�yP:."
'U_NN1".1 at fZQQQ>>>.1@@YP4
By����yRyy~�|�||yyyyuuoodd^E84)44
%A%"R?
&%64)$284=WkkI?62DFRU{kddddndm~~~~RRhURRRhkhWkYWC%'4S?2!%94:2!!
$$4<E>M`{KARWkhK [...]
+"%
+
Eddddddd^^L""8ppu|������|���y\@.'Y_ZLA " .. at _VQMQL.6>:NNI;@
0y����ymy�yyy��||x|yyyuyuddVO0+%4,.'::.'
"&2)6 !8$+R8+TdmdWD)26FdWh~h{khm�~YkdRdhhRFdkRRFW~~R4DC?R4C4
&CO9 ,UO9 at O]I1,2<hPIUOO<E]]]/%!4[iU at KKIC2&DKM<2IM$"&OA-&
'EdVdddoooV^R:3B^yu������vPN."
"1YdULU6"">>I_VOMQQ>>>IIINI:$6|���yyy�|�|���||||xyyuskE8 at RO@@B:EEVE:3)
&))&0
4F
K`=2OICRWY?8)2?FYkkRRhhRkYFA?hhRR~hRARRdddhhhRR4
4?SCSK4**,K at 11E9$%>EEUMCC at 9S[iM?M<AFWM2!
2 at 9@-.--".=<2W-
+
.ddddooousps^R:Rpy�����|���vC at A"">IdYUUM. "1 at dZOOJML>L>IYNZI/'K||yy�yy�|���|�||||yyxsd@@3OR^V^dksdkVV:'
)
&&&6C8 4,
'DU)!4F6=W~mRTO[dmkRCRR~~RCARh~~hdRTdRdd???4%
44%2SkdR!"&
%!&'-9IE]M<9KI>>>O>@,:1@[[[C-4KWA=.-]K-6F`-'"2:I%
'33"'Ldoooosssss^^^R^������|~c31>6'1Y_YYL> 6..:NYRQQQM8L>INZZI6$"=m|yyy�~�������|�|�|xxVO at LL^^^k^sxpypk^E3.
6)&%&
)%-6?)%)2624`mYYYYYddddRRRRRRdhWYh~{hR04'CSS44444RR??4CD-OC,%,-1999:2CC$*,'*>>119EV]SI-!! [...]
+.)4.,;) %"
.Vdd^sp^R:Edouysx|y|yuyudyv��������yaAM8.IYZYUMQ>88"1A:VYZLOQQQMLUIZZIA1'Cyu������������yy^R^^pkoodousuuuskVR3&&&)))@6;;E=484226+& ;4RddWTRFRR?22?RRRAdd~~~RRRRRddRR?444!'UkhC44
&<E>>
%*O]C@@I][OUK:S at 9!
%9IOUC..AR<&DU,$
-<"8;OI;+ ,8
Edooospy^:.Vdosysx|yxyummy~��������yULM..>ZYYQQM68..M>1:NUQURQQQMMOIZYOA:"Cp||����������ysRspupsssusuuusudVB836B@) )@8438)8O4.AA00)),6644Adnr�dndAAAA2?Rdm�mdRRddRd~~~WkR?4???DhhRC4CS&4&&,I2!4[E%
!:CII1$$>iM214M::A`ki[U4,==MY3"9"& [...]
+
Vduupk^m^^:Eosspxx|y�yyuyvv��������UMM.6>YZYUOJ8.88>QAIdYUQQOOQQRUQQUIII"Cuu�~��������yyR^ys|x|yuuyuuuudR:8RRBR:00O8@@))6ERRW8)6C6 at CRR.)KOYndOdRTRRR[[d��ndRAA2FF`YRF??266M]h{S422&29,4,:2KU@%!%4E$$*<,/>2%2-&&.F`h]i{[9DA%&4,!&'<<&.?=IIY82+
VVuyppppyp^:Vskkxxx|y�yyyv�������yUL8.8UYYZQ>QJM868JRQZUQQQOOORQAOUUUZN"Puu�����������mR^ypxssssuuuuusV:38R^^RRR0
688=88;O64EWRcycR6++22A4Kdd[ddIddYdAYRdnndYRFDD`kkdR?FYDCCS4424442 %1$.:::*-'",,*1<$$%<K=DCUKC]]-,,4&"&)M@,);[[K8&<64+
EdkyxpppupyEEdsx|yx|~y~~v~�������U>L .ZdUUQJ8L868>LQUQURQQRJOORQUVYZZ at 1Cmu��~�������mysyxxxsxysuuuudE.'R^^^dV^@3&)3))",@=E@ AEOEcrWOCAIY[TITddRRdmkFR���dd~nmmYhhhWRR64-4FDSCS?4C4&,$%-K:1$!&2CKEVVE1$ ,I>@91:IUYhKK=MMDC488 [...]
& %C&!'% 9VV9!4::UO12,*9 at RA4:A[[WDKUY]]WKdYMUK=& 8I8$0043+&
+:kkooyp||us^VVssuy||||~~y�yv�����~y`>>LMYYZURQRMLMM>>OUVUQQLQQOQOUVUZZZNA at d|��~y������ymm|||y||yuuy|ys^E3L^^kx||sdV:)33)6=4,FC==446IRR4RI4;I[dCFRRaRTRWWRRY~~YYTRFOKhkhU4.DhR9:CSh?&' :2$%%IS1,%,>1 -!"C]i[E99$-UID2<@[O,!)86.<F=M=DKR<.A=.$0C?08K?$ .kkkpkyppuy^^R^px||||�~yyv������c:L8LJNYUQUQ>>L>8>UZZVUUQQQQQOQRQUVIYZA>uy|�������vyy||||�||yyyyusVEL^^u|xxxuoVV388"";O46));B at R[RR6)6=OdddRMkRCAOk~dWh~kkI?FOYYMUR=26CU4,:CDKUhSURC4,&,""49%
1<IP-&8R at 9,,4[iIOI1,<<AKi[I99<-",4-! [...]
:EL.Edkkkpx|y�|yy^Epu|y||pyypu~~�����y at MORUZZUQUMJ8LQRUUZVUURQRRQQQQRUZYUZZIVy|��������y|���|��||yy||^^^xppy|xxxyuuudV3.RRORO66).OWO at OE66Tn^TTT[?28[w�^FF`F=66:[hU?DUUMD4C94SC)-6-44'4RCF?R "!
1:&%%@OS1!.6K]I1*,':@1/1<W]UM=<6<88=dwmO& Kn?)
,...:L^:Ldkksxxx|��yym^^||�||y|uyv����y at MOQZZUURMM88QQQQUUUQQQQRRQMQQRQYYZZNAVyu��������|�����||||||y|s^uupu|yuyyyuooV:0438 at ROR@))6IWdn[EECT^ccnndTC;8IndRdY[IF=DFRYhWC6-:C?4<?4?CRhRD444'&!!4.!"
$%',/%
)]K* ""-P[@1',%::<FCA``M at 2 [...]
'.VVkL:kksokxxyy�|y^^^||~y|yyv~���r at MQQUZUUQRQM>LQQQUQUUQURQQOOOOJOYVZNN:Vuu��������y�����||||��ys^^pu||||uououodd:33:33 at 38@@BRRRWc[c@@@OEKWdYdRRCRhFM`Yd^EYmmmkRIdd[R?YIAYRFR?)2'4%4S4!+"! ,>>2!&%1,2&.F<)IOM<9,!&=mo`]2-6DDUI1-2'!0F<
+
'LdkkL.Lokuxuxy|||y^R^uy�~||y|ymuyv~�yrIJROUZUURQJQQQQRQUQQUUUROOQRQMOOUVNZI:Vu�������y�����y||||||p^pxy|�|yyusuuoodRRRE330,,686)@WdOO^ccWcT at C[��rnrndd68FFTRIAFRF=<Ra�~ddhAAddT?46?44'2C211%%
%,K2:&"!%,:-,I<&&&CYAC8=RkM<M..YRK<,)--<.
EkokV3Eksyxxxyyyyy^^pm��x|muuuvy��rIMRZUUZOQQOJJMOOUUUVVSQQRQRRQQMMUIVVS1Vy��������y����yy�|�||x^p|||||yx|ysuuooVR^^R at 0%4,4;3)
&6=TnccTTdcWdY[mIRdTd[dmdYRRIIYF44<FWkmYRWWTRRA2%44242,!-!
!!%%!$$&@2&,@@1<<.36DM12=6<Rh]<F`Y`OAKI..
+FM:&.VkkdE:ukkpxxxx��~yyy^m�~y|uyu~��nUJOOQVUUQRJGJMOQQZYVVUQROOOOQORROUZUS1V�����������||�|�||ympy||||yyyyuuuodV^sVV33
&0OO@@ ,6 at TTOOOCTnnnnnYTTRKC;n��wm[RTRRFY[T[FD,4<FYd22
2??4&&,,!%!&9 at 1").A3<]]]][KAACKMi[@<FOK]iK<MCK2 [...]
+"'".VkykE:Vkkuxx|ux��ydymyy��u|uu�u~��rIQRQQUZURNONOORJUYYZUZUQRROQOQQORUUQQ1Vy������yy����||��umpmpu|||yyyyyuuuoVOsssVV3006
&0OdO60EI^rrcWTRnddd[cTOA6<FAPOMWIKC;IkmF,-4A8244''44&'' ,21:229C@@1!!2&'+)AMOii2--%!2:1%&M]i[P<:K@@468COYD)$"+0'Edyuyk:Vkkks|yu|yym^^uy~~�yu�~~rIRQQQSYYROOOJJJRUYVZZZURQROOOOORRQUUQ1V�������yy���|���uupmmu�|||xyyyuuudVsdVodd3 at 6&&
&0?@&6)4?dndOOOOcc^mnddAMOM]UKR.6F<8)&FF2-<<DA244444
244$ 11%2&!K``M<IO2!-&"!)6.<K][KA2& [...]
' 'Euy|yk:Eoppyxxuu||pdyuy�~�|uu|�~yu~�rZQQUUUVZQOJJJJOOUQUYYYURQQQOQOQOOUUSL1V������y����|���|upvy|||yyyuuuudVssoddOO0&&0&A@@0+0&4;T^K@@COO~ndddYdYFAM]h{]?.+)<D<I8IDK:Ddd??S?%&4-
&2
-[A.<1%<i]3%-KD:&2 at DMOOMO]U at I263&-$,"$+"
+
'EyuyupV:oypupxxx||yymmm���|uxv�zURQUQUUQONJJJJOROUQVYZUQQQOOOOOORUOQAGV������|y��������ummyyy|||yy|yuuodssssodd00
%0RWOA&;IT^ndWE=dc~��cORRAA.AMhkOI[K<DYW]W=%!KD444
4%%11&::-&%1<-:K at 2&!!3<M?W[KCMSI=.:=<U]K<.!9% 8=02+&
'Edouy||dEdxxssxyxuy|xummmm�����p|||~v�jIORUQQRRQMJ8JQRRROOQUQUQRQOOOOOORQQQI1d�����yy���|�������mpmm�|��||yyxyyudsVsssssdO0
%0 at ROROR?88IIECC4FRd~m�nO?Rd``Y<?K;;PmY8.6F?)!,<DDR??424441,OO1
,IP1I at -C2
<POKihi2-6R]M<K]K<%,,-08$,.8C
'.Eou|||^EEsxsxuxxxy|umvy~v��~����~yrURQOUROQMMQJJJRQQUUUUUURRRQOOQQOQQUUGGV�����m����|�����uyyy�|�|||yy|yuuod^^ssssdVV030000AAA04TOC?ARC&4ITRTdcddddRRMkhdY=8TTRI6606MMWC)!4R4211%%%%:92 at 1.<:%!@I at 1:]WD4&..&"&2!!6`M&
+<R<)?6<+..Voyyy|pVVussssxy||~ym~�y~����|uy�jUMRQOROOMQQJ8QQOUSUVVZVQRRORQOQQRQOSAGV�����yy�������||vvu���|||yyyyusoV^sssssdVV3333 at 330ORAACA2%MmI44CRRTdmnnTCddk`WkYTKCCdk]F4FD?4?4CS444
1-$%"21,!'=M[<1,I>%KiS:=::&.-22&D]3%.^mO<88)
+
EEdoyy||uVEdoossxxu|�v~vy�~�~||�|yykUOUUUVQOMQ8JMOJQUYYVYdYZZRQQQOQQQUUYG1v����yy���������yy~yy|�||||yyysuuodV^sssssdVV at 0@RO at 300@OOA0%%+2?4 at CRdnddOAACn~n`aYROIARRU?)!',6RR?4444444%%
*$% %!%:DI&2&%%.M][K`]KYMDD2<D<
6I=M<Y=
'.EddyyyypVEdduxpx|puy���yy~y~v��|uy~kUMUQUQQNNMOMJORQQYdYVVddZUQQRQQORQUUG'v����mu����|������ymvvy��||y||yuudVmpsssussV at 3)0 at OO@30OTO at 40800A?TnOFICRTWKI`k`YI4.?F?RYW?
)6?R?4%'4S424
24
",1:9,
%-,CS,!.:14:-,<<DiPYM<IK [...]
+
+
'EEduyyy|u^EEkupkx||p||~�ymv~~������y�yyyLOVUUURONOMMRQQQUYdYYYUQRRRRQQOQRUZYG>y���uy����������yyymmy����|||yyyyyd:^sxyyuudV00
+TcO4RR468OOCCRW?AATT?+)4YndmnT[TAFAFRddRA&
42444'4
444!'C&"FF4::
+)%$$%.IIE1$%!DWW]kooM<MI<M6YMAYOOM!'EEkkyyyyupVEdkxyxuu||~�|v~�����|�||vvfUUVUUQROOOOOJOQSUVdddYZUURQQOOQRRRVYG>y���uy���y������yyymuyy����|||yyyysVVksyysudVO0&
%2RR)%0406OWWTTTTddTnmWC8RdR?20<?)+8dmTC+"4D?44?4424
04))&1&!,C:,',% : %%!%K[%!!9-:W`M==F:2I`UO at MY<<
+.EEddoooyxy^:Ekkupx|y�y��������||y�fQQZUQQOOOMNORQQQUVyfd]dZUQRQQMRRRROQAVv��ymy�y||y�����y�ym|����|��|yyxxsVVkpssysudVO0
00000&6?TOATnnTRRKCOYmR[mmdF0,-=<Ym`?2+"'DDR?4%?R44444,K,'92!
&%%'"%C]], !..MRdYdMMM2!::4)2F:-
EEEddooyuyp^:Epsp|upu|��|m~������||||yy_UUZUQQRNOOMNNQQQV]yf]ddZZUQRQQQRQUQLA>v��my�y|uu�����yvm������|||||xy^V^sppyyysdd00%02&
&06O at OT[CORmmYTAddTCIK[adnWKK86=--UWD4''!)R`YA44?444444
!,E" C2-
!22AA:,9:'
6Fk`hiP [...]
+
+'VEEdduyyxxxp^:^pu|||���uy���������yy_QQUQQRROONNNOQQUYYdf]YZZZUUQQQLQQRUUG@���uy|�����yvmy����||�||||xy^^|xssyyusdV at 0%&%A400ACR40AdROTTRIA8?AC=YW6)%&4A;=CMF6<R22)'%
!'
44?4044
$
%,1%,C:II:%CUC4-"-:<8]][I<-.=M0!%
+
+.ddEVdoyy|||ypL:ppx|�||�����yv�������||uvydQQQQUUQOMMONOOQQ]Y]]]YdZsUQQQLQQQQOUS>���my�������ymmy���||||||||ysk^^|x|xsuuuoO0%
%%&% 6@?R?42&%)AR^I+.FRIOF2%+28."
-6?A4
''4'&4R?44444
!!44-% ,&'%99--""!9:1 %$%"9[4<4&"-).IF",<
EkdEEduyy|�y^V:xxp||����~�yy~������y||ymjVQQUUQQOOMMOMNOLSGG]]]]dYZZRQQLQQ]QUQQ at v�umu�������vyy���|||||yyysLmm|||yyyuysO0%
%
%0%
%AId at 042%%&4C6AIRCAIIRCAA86)')20AA+
'
!44')?44444442%&!,!!
.<2-,!'-,!+OW<-,:9:44-<Y[ [...]
+
EkdEEduy|||�|m^:Vskx||���y~~v�������|uumyVUQQUQQROOOOMOOLSVVG]]SZZUZUMLQQQQRQOAV�vmmy�u�������ymuy�y|�|||||y|oV:^^|||yyyu|sV30
&%
%0)%2+&Adc^WECanTARYmdTCRdTRR26AFRdTTR)%
'
')4
444@%%--,442$%
4[>1$$!-&<K,,,4"")6TP?M]]<. $6=
!
dkdVEdpx||�|||^:Expxy��~�~~vv�����~�|uyjYVUQUQQRNNNMMOOLSVVVff]YUQUOQQQQQQRQUS>v�mmy�y�������vyuy��|||||y|oEE^py|||xyyys:&
4ROdOO at WTRd�dITTddOFFC;CTRTRRA?2%%24
%4+''
44?O001//,,2<<,
-D,
)!!4[]?8;,"! [...]
4[UD.%!9-&&&.W<<
+!-%$EdddEEpkp|||�u^RBLp||~���~���~vv������uuy�YZQUUUQOONNNJGLSSVffyfVUQUQQOOOOOOORQA@��muuuuy������vvyy��|��||||x|pL^^pp||||yuspkO340%
6I[[[TROTCA?24WmYRAA88840.Y[F??2+)))4F?4)%
4004444?41AA2&&&&
-:-%!,! $//
"6[K-'4-,"$MhK,!$."
+
VdooEEokk|||�|^L:Epyu��~�����yv������uuymj]UUQQUQOONNNGGGLSVVf�fVYVUUROQQQOOORQLVyy^muuy��������yy�������||||pVER^p|||||xodsk3
%0&
2CC64CRnWOTTC8)6FFC48AF2?DdY2' "+??6%%
)%
?4%''04?3A at 2223@2&*1
*/
)$
-<,4,<: &6A:,4
$""EdoV..^kyyy|�mLV:^u|�������~vm������uuymj]dZZVUQONMNNNLLSGVfffffYZUQOOOMRQOQOOJ>�ymuuyy�������vvy�����y�|u||ukLL^^~|xyyxoodV0
%0 at 0 6I[6&&04?24TWndAA8FYdR26FF64?F6266FR?24
)4)4'44
%4R`PM3-2-2 at 9%%
!
A]D
%'
9:
[...]
$
,
EdodE.dduyy||m^LLLp|�~�����~�yy������yyuyjfdZUUUQONNMNNGGSVVVf�ffYZUOQORQRONORQJV�yVuyyy�����yvvy������|uy|pkLmmp��||yyyyskR0
%&
%0A8++
%%04A2?ACRCCTO8?ARYYWA))??66)42)
!'
4044%%4403&&&==2<:&'%
1$
'''
+
+..&!
2-:"$"
+
.dppE'Eddoy||||^mLL^y��������yvv�������yyj]SUUUURQNMNMOQOSSGfffffdZUZROQROQNNOLQ>�vmuyyyy�����vyy�������yuu|pLLmxy�|||yusooV30
%8R=+%
%44 at OYmT48ETCRWT0)'222?A6?D<)
'6)'''%%'%%'%4)'?
"<K.)
-U1
&:4%
-%"
!-
+
.dpkE':dooy|||uuLLLVpp�������ymvy������yy�YLSYVQQONNNOOOQSSGVffffYZUQRQRQRQQOOLLVyvmyyy������vvyy�������y||kLmmm��|x|xsossV3&%&6,&=IR8C4+0 at I8[kTCTdA%)AA+ %'44)'4 ' ''4%4?422222??00
%)6,0AK2
$
%4,!
)-D@'
+
.dk^k..Edooy|||uVLVL^u��������yyv������yyvjSOUdVQRNNMONNNOS]VVffffVUUQQQROQQOOOQQ>�vmyyyy����vvm����������||uLLVmv����|xssssR3
0026.<ddOA444+%4.&%2RA&%%
;CA;+46
!)
'2?4)')6244%4002?+4)++<88=22
!
%''6<"
+
2
.EkppE.Edoy|||mm^LLu���������ymv�����yyvjVOSUURNNONOONNQS]VGVfffdVUQQRRRRNROOLQ>yumyyyy�����vy�����������|upLVpu~�����xyysdE)
00 ,82+)4I0242
+224+AARRF6+ )IIR6'?4%2+46RI4!%)%
%
442ROA4+%1%%$
)COF-&
'9$
&%
',!'RI<8
"
+
dkupkE.ddyu||�umV^Lp�������yumv������yv\SJNQQRMNNNQONNOSSS]ffff_GUUQORRROOOGSGf�ymyyy�����vv�������������pLLVV|����|yxxxs at 0
66022RA?2446IRIOFdRD464
)A,D)''
!'')
2?%
%%2?424%@@'$!
!!
!4' 6:1-
'%! %
+
+
+
ddkp^E
Vddkyu|yum^mm^u������yyyv�����yyy\SGOUQOMNJNQOOLLSS]V]VffYVUQQORQRONNGLG\�dmyy������yvv�����������pLVLm�~����|xxxxd3
?FR40CdTACCC8IC6A8A24")22%%
)42%)'
"26+%%42%)?2+)1123-3=M-'
$ '4D%
--42 ,
+
EkpkkE.Edoyyx|�upmd^p������yyymm�����yyvfQQQQQNMNMNNNOJLG]fVVfffVUUQRROOOONOJLS\�mmyy������vy������������uLLm^u����|||xssV@
&&
%
)406F?2&+%4++<RI86222262')4
6WR)
&844&2%%%+&'@11%&32&!% "
+
%4,
*
"!
EkkodE..dkdo||||pdv^Lu������y�um�����yyvfOQUQOONNNOOJNOLS]]fffffVUUORRORQOOOGLL\�ymyy�����vVv�����������pL^mVy�|y||x|xssV3)
0
%4+
%44
%)%%'4)C?4'%!442)26+)''!"
)84224
)&&-&
.&"-&
$
):EdopukE.Vdddo��|mdvv^my������yydv����ymfSGLRONNNMRONJOJL]VfffffVQOOQORRUOOOOLL\vyuyy�����Vyy����������yuLVmVm��|p|||||s^@3&60?22
%444444FDR44D?6)+22RR2+"
'
'''
)"
-==AM326-&&!*$
!
"
EkuupuL'.ddok��yvddmmy������yymv����yy\SOOONONOOOMNJOJLGfVf�ffVUQQOQORROOOOLAjymuyy����yVdy���������yymL^dmu���||x||||sV:3
2&&%2?%
4????????4?%
'2?4444A42"
6C2
'4,'"!
'
&&-:AF==D<2=A4,'-%$** !$,&
+""
+
EkyxuuL..ddkpu|�yvmm^y~����yyymd�����yy\LJOONNNOMOMJJLL]VfVfffVdUQOROOOROOOOLAj�myyy����dVyy����������uLV^Lmuy���|xyy|ysVVE)
%&&&0 at C22%4?%A2%%%2?4R6'')YkR2%$+ '.'
'2),2)'"
)-)
'
-2ACO=3:&"%!!!"%
+
".&
+
+
+
do|kppE.Edkyu|yyuddmmy������yumm�����yy\LNQOMMNOOOMGLLLGffffyfVVSUQOOQOOONNOJQj�my�����yvuy�����������mL^d^yy��|uy||yVER:
&&""3C3&&%0
%)&4%%???44?ART646-MM?2-+)%%")?A?)%
%?22'
'%%11-&&<OOM:&%.2<'
!
+
+
+
kppkuu^EEokppuyyydvmVuy������yumy����yyjGOONNNNMNNMMQQLGVfVfff]NNQQOOORROOOGLGjymyy�����vyy����������uLVvVu��uuxy||yuVEV3&
0&%
0%
&%%44%4?%'
)2+).6D2
%
)-'
+68%
%2)
'%
KA:21>@OOA---.<3.)'<
,$
".-
+Ekkkuu^L.dkykuyy��vvdpuy��y���vumv����yyjGNONNNMMMMMMOGLGVVVVfff_NUROOOOOOONLSGvymyy����ydy����������y^VydVy���|pyxy|uoV^V0
&&
%2A%
22%4
%
%4)
).)!
??4%)&
'4
[K@:@IPAPkP>>>.<=I?M=!!
+Eddkkpp^.Vooku|����vdV^y��yv���vmv�����yVGMNNMNMMMNMMNG>GGfVfff�yfYQROOOOOJGLLVvymuy���vVyy����y�����uLvdVmy���|xyyyy|uV:VV)
%0%
%&+2A&%??4&
%24'
)%
))))&
'2"%%%+
%
OPOOI@<@AAAM]P:2-DK1,,68-&!% & &
+
+
+'dddyuyV.Ekuuu����mdm^uy�yymv�v^dy����ydNNMMNNNMMMNJNJQGGVfffVfyfdUROOOOOGGGGV�Vmy�����Vuyv���y����ymVdmVv�����|yyyxxyEVkE3
)4%
%
'&
+'
4'!
)-4"
4F?)
?F2%4+
&TO%
'%
I@>IK<@2-=PM]P<119:1 &:::@@:D, &&9
-! !
+
VddyuuL..Lppuy����vVm^py�yyyvvyumy�����dVQOMMNNMMMMJGGLGVffff���yfYQORNOOOOOJfvmmuyv���duy���������ymVvdd����||yx|yydEssR0
%%
060%%2
46''
!4D4
&!
46))
2!
%%
%
MOI>OPAKMP[OACIK:<AKK@@:<-"4-'!*&
!)
+
EVdkpumL.Epuuy��y�ddyVVy�yyy�ymmdd�����v_QONMMNONNMJJLLGVVfff���fdYQOOOOOMMOJ\~mmvy���ydy����������pVVdv������|pkx|ukEEsV3%&044402%04+%
4?''
",''
)))
'
4)2)
'%
MV^]M@@@]`M`]]K at 2FW]A:&%4 !!
$ "
+.Vdy|yL.E^ppu���vyyvmmuy�yyyyyudV��y~�y\QOMNNNNNNMNMGLGGVVf��f��d]QRQOOOOGOQjvmuyv���ymyyv�������yVVVVv�����|xykyxykE^sV3&)640%%0%&&04%
.%
)4'22
%22%
%%%424%
%%
]pp]MOOOO[`OKI1!$:C=-
$,%
**% '4&
"
EokkuyV.Eppmmy��yvyydmd����yyypmVy���yv\QOMMNNNNMONOLLGVVf�yfVf�fYRRRONNOOGNv�dmy����mmyy���y����uLVvv������|uuyook^EusV
0 at 00))&2864?2 %C6"))?A2%226'
))%%%%
%
%
]p]]]^VOM>UP>>>>11 at KKD@:1:,,*41*/
)
+EdkuuymL.kpmmy��vvyvyuuyv��vyvmmLV���~yjYOOMNRNNOOLLLSGVVVyffVfff]QQRRMMOOLVv�muy����vyyv�������LVdV�������up|||kk^VuVR
%&
?O2&,6=CAA02 82
2" %)2% '
%
)C0%% )"
"
%^]]]vv]]OM>M at E>MOPP@>I at A@I at E@@9*&
+
.dddpyuV.Epppv����vVVVmyy���v�mu^m�����dNONNNNMNONGGLLGVfffVff_VdYUQQRMMOOLV�vmy����ydvyv�������uVLVyv�����|ypx|yukELdV:
&OA440&&2442+
4?)6%%2%''
'4)%?244!
)22
>Oivvi]]]O>UP>MOO[OMOKI at 99D@III*
ddVpy�V.Lkp^v����yvVmmy����vyyymdy���ydVQNNMNNNOMGGGGGVfffVfVV_]YZQUQJOJGG\�vmy����v^yy��y�����V^Vmy������u|uu|yydL^kV
%%
20
%282)
)
))%
%%0)2%4%%%))'
EOOOOOi]]]@1PKOOOEO]IO[IOOOOSO at 1*
+
Edddy�VL.Lpuy������VVVv�������yyVd����vfQNMNOOOQMGG8GGGVV\fVVfdUQQRORGLLLGV�vmy����Vuy��������u^Vdu������||uy|upE^|^:
))&&
%+24$
+%)2%FA)4?4%'44%%2+%
%
2
M^^O at MM]i]M>O`[]OOi[O]]]]OOSKOIF'
+
.dddd�yV.Vpu������vvVVy�������vydV�����\YLGNROOOMMJOQLGGfVVVfVYSUQONORUOLVmvmyy����Vuy���v����mLVmy������u||y|y|dE^^^3
%
"""" %)644''
02'%%
)?+
>OOMM]]^MMOP[oOv]iiiiOOOK[P[KC8
+
+
+
EEdky�V..puuv����y�VVm����y��yyVV���~�j]SONOOOMMJMLLSGVVVVV>VVVVQOONOOOLf��vy����vLyy�������yLVdyv������u||||ykL^m^:
%
00
&
&
))
)%
'
'
'%
$C2
%)
+42
IE]OOM^]Oii^]it[[i]i]O[iK[UC,
+
+
EdokmvdLEkuu������yVVm��������vmVy��yvyVVQOOOOMMJNJJGVGGVVVVNGVZOMNNOGOGj��vvy���VLyy�������uVVdv�������yu|||yLE^kV:
&00%
"+0)'24
)2 2+')"
)%
%%&,%
<;<^M^pv]v^M]^oiii]i]OI at C9,
+
+
+
+
E
.V^kpy�VLLpuy������VLVy����y��vmVy����yfYQOOOONMJGGLGGGVVVVVVQQMONNNNGJGy�ymy����VVy��������mVmmv������y|||uVLmss:
%0%
!
"<A242)46 %4",+"
+&& %' %&
;II^^]p]]OvvPiio[O@@1:1D-
+
+
E.
EddkyyvLEpuy������VLVu�������yyVy����vfVQOORJNNJJOJLGGGG at VVVQQOONNJMGL>��vyy���yLmy�������vLmvd�������y|||dELpk^3
&C&
%'%2)-!'
% )
&%)
"
%
MIMM]vvvvv]POiO[PO111*,<'!
+
+
+
..
.Ed^^yvLEVky������vVVm���yy���yud��y�y\VONOONNMJJGGLLGGVV_VVQQOONJNOOGf��dyv���uLuyv������mVdvv������y|||||uEL^poE
&%
)460)AC+
'%
))!%%")'!
%&
%
!'MM]]p]ivvPPOit]@OM at M>1*!
+
.
.ddkpyvvLLpy������VvVLv���yy�yvvVy���vjNQOMONJMJJGJLGGVV_dNGVQROMMNOJQj��dyy���uLy��������dmVdy�����||||||ydLpppp.
0
%
6+
%0) )
+' 2%%%%"'
%22
"
!
%!
I^^^ppi[OM at OM]OMMiOO[K$*,$* &,&
+
+
E.
EdkpummVEppu�����vVVmVyy�����vdVy���vcVVQNOOGMMNJ881GGGfVGQVROONMNGGGj�yvuv���VVy��������VVdVdv���u||upyk:LuukV:
&&
42?
%%
2??4
+'
44%
)
IM^p]^^^^]M^PpMM]iPP at 1$/1///1<'%,'
+
+
+EE
Edkpuyv^LVpmv���yvVVVVddy�y��vmVv���vVVSLGNNGJMJG8LGNVVVV]QQONNNJGGJGy�vmvv��vVpy�������vVVVVmv���uu|||yL:v|xsV
+
%
%0
22
%+"4D?)
%))
!
!'
")"'')"
!
[V]p^^p]]ppivP]O>/1>K>/<<<$ $'9$
+
+
+
+
.V''dkkuyvV:Vppd�����yVLmyVVVdy�yvdV���vyfGOJONJJJJGJGGVGGLALQQNNMJNJGGG��dmyy��VLmv�������uVLVmy���y|uu||yk:LyskkE)
&)&0002%%C)
4+
"
))
%)
%
)
p~~p^v]pvvM^p]^M</>/>iPM>1 $$ 1
+
+
+
+
VE
:dkku�v.:puu����dVLdm��dVVvdyvVVy��vyjNLOONJJJJJLLAGGGGGLQQONMNNJGGV��ddvvy�VLVvyv�y���pLVVd����yppy||yL.^yxsk3)
&@O42'2%
%%
+A% ")%
+2A+)
ppp]vvvvvpv]v[OO]M>>M]]PK2$1*
+
+
+
+
.E
'Vkpu�yV:Eumv�y�ddVVmdy�yvdVVmVVyy��vyNOOJMMMMMJJLLGVVGSLOOONMMGGGLf�vdvv��VLVv�vvv�yvVVLLVv����upku||u.BmyssV3
&46)4 at 4%0
"%)
"%+&"%
!
^V]vpvvv^vMP at P^M^VMMMM>ESM2,--
+
+
E.EkkuyyVEE^puvy�yvVVVu�����vvyvVyyyvyjGOOORJJMGGJAGGVVGLLOOOOJNJGGS��dVv��yV.Vy�vdv��v^L.VVy���yppuppkE.^�xuuE
&)&
) 06&&&);4&
&%"
")!"
%
"%
"
!!&
%
V^pp^]^MP^MMMM^MV[^[II>/*@]MaK
+
+++
+
.Edkpy�dLEpmVvd��vVLLd������dVvddyyyyj]OJJJJNGG8LGLGGVf]LOROONGGGGG�vVd���VVLVvvv����vV.VVv����uppppyL'Vypkk:
%)840&00 %2)).'%
")'"'
" %++%"$
!
ppp]]pp]^]]^M^^^p^^M>@>;<>,%:<
,E
.ddpuy�VEkumV�yyvVVL^v������vvVVdyyjjYJJOJJJJJGJLAGVVVSQQQQQGOGGG]fvVy���V..yvv�v���yV'VVm���yp^kd^uL.vyukkE.
%% % %
) 2+%
")"
+) "4+ "$
!
^^I^^p^^^p^^^^[s^MI>1/<<//$,I-
+
+
+%&
+
.E
EoppuyV.d^^VdyvVVLVuv��y��yvvmVVy�y\IJJJJ8JJGJJAGVVVVGLQRRRGGGGLffyv����VLVyvvmv���uV'0Vv��yupp^VuL.V�ukkV:
%
26AT+ 26) ""!.+
^M^II^^^^]O^pp^^MMMO1II//*&$$$$,:-
+
'V.
VkpumBEpmvvyvV'VVdv���yv�vvmVVjyyVJJJJJJJJJGJLAGV]]]NQUQRQOOGJVjjd���vB.Lyvyv���yVL''mv��upmkkpp.LukkkV:'
$",08===,8),6&
+
+422)!+2-+
"2"
^^^^^]M^^^^M^P^OM]MIII/</$& *</$ /,:+
+
+
+
+
0&
.dL
EopuumB:dpmmvdVV0BVdy���yyy�vVVv�yYNJJJJ8MJJGJA]GVGNNUQQOQQQLOLGfVy���d'.mv��y���uL' BVv�ypupkkuL..V^LpkE::
)@;"$ 640,00=KI&
+))2$
)"
'%'4)"64'
^]]vpp^pp^P^MP]MM>1I>M>><2*<<//&$ 2+
+
+
+0
EE''E^ppmV:Ekpum�VV'VLmd��y�VdyvVVyv�NOJJG8GJMNJOGSGNVNNVQQRRJOJOLGfj����V'Ldv��v��vmB 'By��vupppppL.LLLkpV.'
"'
".6,
+
$;K8
""$!'
")%"".+82)"
pp^^^M^^^p^p^p^MMIIM<1@@</</*/$//$*-)
'EE
EdkVLmB.kumudVVB0LVvvyVdVdddVBVyfYOJ>JJJJMJQOQGGYNdYQQRRQROQJGVfj����V'Vv�ydvyvyVL''Vy�ypppkkkL.L^VLud:E:
.)
+
4%
,=0%
''
))+))%""%.-')'
"
^pp^^^^^p^pp^YMMI;;////1<<</<;>>*&-.,8&
'E.
.d^.LuL'Ekm^VVVV.0VVVVVddVvyy=V�fIAQJJJJMJNJOLQGNNYYUQQRUQOULGQfy���y.'Vyvvdvdvv^.'BVyyppkkkkVL.L^LEVE.L^:
3.
% CT40)%!'"!'!
"!"'))"!)4)
^p~^p^^]pvp^^M^^<;/$/<>/*<9E>M>,"+8 &
+
+
.E:'ELEL^L'.LkLLVuV0'LVVVdvyvvVVV\_YVUOJMJJJNNJOLQGNV]NUROQORQOJGQfyyyVV 'Vvmvv�v�mL' 'VmmpkpppLE.Lm^^EV'.EV'3'
& .;CIA!
)"")
) )'
''
ppppp^ppp^^:M]^M<///<<>>>]]>:,$
+&,;8%
EV'.Edo^V.
.ELLmyVV0BLdydyvvdddP at NIYGJJJMJMNNOQVGYVYVUUQQOQQQOJJLVjyVVd '^vmmdvvdLB .mmpkkpmLE''V�pLE'
E^V
'33
+
&" ));4$ ))68%
+"
!
%"
""4+
!p]ppvvpvYM^M^^^I;/$/<;IM>MO>KU,%
+
+))))44&$+0
.^E
EdkpmV ..^yuVBB'Lv�yvvdd�vV3NUIL>JJNMMNNJQSG]]VQQUQQQQORJJJLV�yVmV".mmvmdvvmB" Lm^kLLumLE.V�ypL.
.EE:''.
+%% );. "%%$ -"
'",;+$)%$"' !!
pppvvvvpMPMM^MI//1I[III>>IM>O>*
+
+
EE'.Vdyu^''VLVyV'0 Edd���vvd�V at VAJ>OOONNRMNOOSNVVVUQQQQQQOOJJOJVjyvvL .Vy�vv�mm.'' BLVVLLp^L..VypL.'EL:.L:
"
&)&"
"
)%!
!2)2,+"++")-))%
p�~ppppp^^^p^:/;;[^[[;I;/;//I1&
+
+
+
'''VL':VuuL0'.LLVV' .mv�vyvVyvjVNQQJOONNOOOJOLQQQGYQQQQQQUOONOQJQky�yP'Luy����uL'0 '.BL..LVuE'LyuLE
E.
:'33'
"'
"
),
8,$%"!").++)"++
! )'" !
~~p^ppp^[^^^^;;;[[[IIII/>>>I/&
+
+
E
.B'
Ekp^B''.LVVL''.dv��vVVyvj_GLOMJOOOOOOOOQQQYdYSOQQQQQOQOOQJLVf�v0Ldv���ymL0 '..LLELVup.'LymL.':E:'::3::3
'""
""
68
%
8I++)0) ! ))"
""'
!")-' s^sp~sss^^^[:6L[]];I^I11OMI</*
+
E:..:'.dVEL.'.E.Vm^. :VVvvyVdy�kIQOOOONNNOONMNOQQ]]SQQOOORQQQQQQOLVfyV" BVv���uL''LVVLLLLLuV'.Vp^.EpV:
B:':BE''
")
&3)
")
%
$)"").)
!'!)8)"!!' !!&)"!ssssss^^^^^I;L][^I^^^:M^MMI<<
+
+
+ &&%
..EV.
VL:LL'VpLmmVB'0LvdVVdrkVUOORJOJMNOONNOOQSQQUQQQROQQQQUQOLLSfjB.Vv��yymL' 'LumpLpuukE.VLLL'.^^E''L''3''""'
3'.)'@CE8&
!"
+.)" !!"0.)"!"-!!&)sss~s^^^^p^[^^pp^pp^^MIMIMpV/
+
+
&))
'
:VkpL
EE.LE'EppuyvB .VVVVyvyjVVJOQOONJMJONNNOQQSVVVQRQQQUQQUQROOLSV\0 Vdy�vvmd:LVmmLLpuuL'EVLL'
'.pp:
'8':..''
))" '" "
"
+&+& "
"
"2"
'$)"""!"!"%'&"!
,ssps^pppp~s^^^vvvvp^^MIM^spI
&88$$$++000
EE^^L
::EE'.^puyyB'LL^vyv��yVAOROQONOOONNNNOQQQSSQQOQURQOQQQQOOLSVV Vdyy�yv�m'''VumLELyyuE'LmL..dkE
'
.'33'
)))
&$$ $ "
"""!))$)!
",-!&)!!)s^sp~~~~~p^M^ppp~vpp^[;[[[I<
$ &0KjcK;&$
.':Ekm:":'..EL^mVB 'LVdvvy�_SQOJOOOOJOOOOOOLQQUYVQQQUQOORQOOQQQLLGfVy��v�yVmV'".vmL:LyuL..umB.LpL.
''
'.:''"
)6 "$ "! ;,%+ "
!!!"+!'&&!"-)%!'p~~~~~~~pp^^pppvsp~^^^OY^MM/$
&&
+83-...2?).aO<680$$
'
:k^B
'B'
..LV. 'Vdvy�yfYSQOOQOONOOOOOOOQSSU]SUQQQQQQQUQOROOQQSf�y�yyVdVdL''VyyVLVV^L.'LmL'
.ppE'
..''3
:.
':3
)"
& "$+.""""$
&&""')%
"-'!p~�~�p~ppV^[pp�p~~~s[^^p]ii]PPA@@%%
+%
+$O6.$
'
.E^:
B'
''.''LVvvyfYSQQOQRQQOOOOOOOOOQSQUSUSQQUQQOQQQQROQOOLYfjyyvyVVVL'LVVV.VVL.".LV''
.LpL.:LB''
'''
'""&
.&
.. )6+.68""&$)!""!"!&'--"!'%&&~��~p�ps^pspp~p~�ps[s^^^^M;>///"
0;,,?!
&&88+++"")8E=E. ''E:.
':'
''ELmLVddyy_YOJOOLSQOOONOOOOOOOQSUSQQUQQQQQQQUQQQORQOLQVy�y�vvvdL 'VLL''L.BL'"''L^L.''...'''
3
".6 &.$,&+).,
+"
"!"" "&+)"!"!%"!!)'!!
,:Vp�p�p~p~~p~~�~~�~s^p^^p^I11$:-
"0
)K^8" .=K8&
""'
'EL'...''.LuudvdyffYQONJOOQQOOMMNOOOOOQOQSQSSUQQQQQVUQQUROQOOQLLQ]f���yvdV''LVVL.LV0LV'''L..'EL''.RV8"
'' =,)&),,,
&"
"+)'"&&!"!")!
'&s~p���~p~~~��pp~v~sp~p~^I//%<AMA=?IPKKWWE=;,,)IPCKCOW[K?0?=-
'
'::
'E.
'.L.BVc\\fVVQQOMMOOOOONNOMMOOOOOOQLQVSVVSSQQOQQQQQQQQQQOQLQOQS]fVVVVmV.VmVpL^L' 'LL.''.L''''.B.'0KKKKC=""
&6;0&&,,)% "!
!%"""")
"&&$"&-"%")"-",s~�~�~��~~����~��~Z~Zpp^M11$/
88;[Kcrrcc^^crn[RI.
"
')'
''
'P>>GGNVISLOOOOOOOOOONOROOOOQQQQLOQSQSYQQQQQQQQQQQQQOOONOOOOOOL]VffyVVLmVm^LLL''LpL.''L.''''''""0PP8)
6))&&",=K6 ,&& "$!&
%" """'"!"--+"'!%)-)+)-ss~~s~pp~�~~����~~�~~s[^:IP`A! / !%
6T[^[[nc^T^dTK8,
3)....,."
'"
'" >@AAALOQQQQQOOOQQQQOOORONNQQQQQQQQOQSSSSQQUQQQQQQQQQOOOOQQOOOOOLLS]fVVVmV^p^LL..^VL.'BBB
"''..''' E^@')."
).&,66.0IK+)+,+% "
%&--""
'-'.+)&spps~~~~~�~~p~p�~���p~s^;/>@M-,<1$!
+
+;WnTd[ddP^nTIPTWK;K0$&
'683386'
''
:. '>>AAJLQAQUQOOOOOQONNNNNNOQOOQOQQQLLSSSSQUQQQQUQOOQROOOOROOQQOLLGLLSVVfVVVLLL^''LL''.L.'"''BB8BPVRBR83
WP"
",0;E;;??8800C^KIKI;0$ ).))"
!""--"""-)'--+~~~~~�����ss���������Zs[;:>:1%*2, ,0crcnnnmn^^^c^mcPF.$
)8"'.:B''8>ALLAVSQQQQQOQOQORNNNOOOOOQQQQQOSSQQQVQQUQQQQUUOOQQOOOOQQQUQQQQQOORQQIN>VVLLL''L.'LV.LV0''''
''''33'
'0K;).0KE=;40&&%% $;;)"$;=08) )" %""
!$&.)"-.-.'&)-~~~~���~ss����������pss^^I;/>1!-8+
+
+ 0Innnd[dny�wwYA.=I0)
..' '1AJQGQYYYQQQQQQQOOQOOONOOOOOQQQQQLQQQSQSQUVQQSQUQOQQOOOORQUQQQQQQQOOOOQQLAV>CBL..LV''...'.'...:.'
'33
"WdP=RPPPPPPP^K==604,0?8
$&&,0)$
!
!!"$+66)").--%&+~p~~��~s���s��������Z~^^^MM//M/$$
$06AIOa^^ndccOFOFK[aOM6&
)'3''''''.8ALGQNNYGYVVSUUQQQQQOOOOOOQQQUQQQLOQSSQVQQQQQQOQQQQQQROORQQUQQQUQQQQQQQOOQAG30VELLB'L.''..LB..'.B'.'':33
'8R8$&B.& $,08;CKPPW;4;;$%
!
)$""!$..8&&+--.,".��~��~ss�s��������� [...]
+'.''' ''.AJ>OOJOOQQAAUUZYYY]]]]]SSSSQQLLQQQQSSYYQSQSSSLSQQOQOOQQQYYYYddddddd]YYZZVZUUOROG>>'B.VBBBBB.:'.LB''.:.83.''383'"''..E.6=@66.,0;;K;8)" K^cK0.,4$$
"%)""""..66.-&-.--ss~~~ss��s������~~~ssZ][[I:>@12<!%O<,&19]]O]tkomwOO^^^WTOC8
.B8686.3'' ' 8JJJJOOJJ>L>>AQOUV]dddf]]]]]SSSQQQQQSYSSYSQUSLLSQUQQOQQQYdfdfy_dddYYZYYZUZZUUURQ81>'''..0''8LB:'.0B8'.BLB:833'
""""".'3E,)&,;64,+ $0?I?&$?c^T8+!"!!+&&$ $.<=.+"-..&Zspp~~~s�������s~~~U][ss[^@11114.
CTao]` [...]
+$'''''.8JNNNNNOOQLVVQOJ8
.QUSSS]]]]]]f]VffV]f]]Vf]ffd]]YZUZUUL"
8>OOUUQRQUUQOG8A1@>'>>'.VB.'.8.3'="""=WP080.8.38=6=.3B6;@@606;2.."&."""$8K?.$;;80.+.++)).+..).<D66<2<v�d~��ppss������~~�~s�ssIIM;/ $ 9E4&"Io`kM2<AMazmn��ncncT? &0+
+'.'=&''1>JONOMNNQOQU]VQQQQ8 8QQZNS]YS]V]]ffff]]ddfdd]]]YYZUUU>.
.8LI1AJOOUQQQUUUQOOOOA'>''V'0BV8.'8.
8".Pc=;==BB=0=R=)@6.4@@A44C408+.)""%"%$,?CK .0,,..+""&00.....FF66<<���~p��Zs���sssss~s~��s~:/II//$CI-!'-<`I[[PPacmmw��wk`OO=0?PP?+ 8E0.'1A8>OOOMOOQQQUSUVYYYUQ8
>OZUSSSQS]]]]ff]]]f]dd]YVYZZZUO>
'AIVGQVUQOQQUQRQQQQROOOJA1>0.LB'0B.'""'))88))
&K^=0880.@==")=RRC at 4A424C40.$&""+))&$$?K$ &&++)"$$08..=88FFF<6<�v~pZ~�v����ssssss��~ssp^<>/<</
I9:4
KOIKMO`ww`r��n`ORIOPrr^P.8B at 6"
18AJNNNNOOOOQQQQU]YSQSS8
>OUSLLSSSS]]ffffd]]]]VZVVYZZUM
.AGGSVSSVYVSVUQUQQUQRROOORA1 0V'0CVP=0"'"")"'0.R8"&=
"=ERcWEOEWTWWOCA4CE8.&!++4.'"
0K& $$4,$0.$&.0.00=I[FF=6=v~vp~p~������ss]ss�ssss[^^I;II$
* '%
=P:Ko~PK]Po�wi]mmrnrzycTW@,
''1>JONOOOOOOOQQQQQQQQQSSS8
>ISSSSLSS]]]]]d]]]YVUZUZZUUR >GSSSQSVSVVUVUUQUOOQQQQOONOA1 C>'' "'CP=
''"'...88B=B.""
,P$")6BPE;4;EWTccWRC;8;0.-)++)"&=$$$+06860....668=RR<?<<�v~Z��v�����ssss��ssss]ss^MIM;
--0kOS</KIO at AnoO1<`acwrr^P^A ""'G18L8OGGOOOLLQLLLLOSSY]SQL8 ASYZQOQQQU]]Y]YdYVZUUZZUU> 8LSSYVUSVVUVUUVQUQQOQQQORONG8L'> 0PP8."'WP8.'".8."""BEB8',K0.)..8=WE=46446Kc^II8.,2-+"!")&0$$$&.??0,..$.38=FMF66<vpp��dv���s~ss�s�sssssss][I<;/
">>1-1]><E/2M[azwoP]UKcr [...]
+
'0C'1G8GLGGNOOLLLLQLLLSSSS]SLQG8 8AUUOOQQOQQZVVVVUUUUZZZUO8 8QSSUVUUQQUVUUVQUQQRNRROOOMGOJG>'0'8cP,
"PB)"' '00'0PPE80"K$ )..=REEOEC;;A;;=KKKK;04."""""""$ $&,.C?2&0+.68=FDFD<Dvpvvv�v��~~~s���sss]ss]sI^^;;@/>>
P$ME at Moonw�t]ioanwmUn[3&=PPc?$
;;$' A8LLGGJOOLLSLLLSSS]]]SQLLLJG >QUOQORQOZUUQUUUUUUZZZO888OQQQSUUQQQQUUQQQLLOOQOQQRNNNGLG'''0'0PKE '=W) 0' 0PPWccB3.68ER=8BTE=CEKCE?Tc^PTE0"")$"$+$,+$4+2?8....0<<8FFD86vpvpvv����s~ss�s~�]sssssIp^:>/$$/$<@9S>MPiiPOPPiowwoMOOcOTPPn^;;,")PP&
'1A8LJLOOOOQSSSSSSY]]]SSLLLOGJ8.AIIQQQOZUUQUUUUZZYYZZ>8GJQQQUQUQQQOQOQQOQOOOOQQQQONNOLLA1'000'".=CP&
"'.PB8
"
0rrP=KKEPWR=EOWWOEEKP^T[KKKPPI;0&++""$$0;$000=80.008=03=AMF=pvd����~s~~~�s���ssZsss[I^^:>I/*/
,K9SM/IOO`12PPU``PPPOaPCCIrc=?0.W;
'1AGOOGOOOOLLSS]S]YS]SQSSLSQQGG88AGIUUUZZUUUUQUVYNZZ>GJQQSQUQQQQQOOOOOQOOOOOQQOQOOOROOQ11 000V'' .PC)P00IC$"08"
+
Pr;B;66ORP=EEEW==KPCIKPI8&.0CPTC0,+$,,K8.&.86000066=6=FFRFv��v��~~~~~sssssss]s[s^LIsI;I/$$*A>EE/1V]UO1<PMtoPOPPac[ccn=&&=&=c0 0"& '8JGOOOOLLSLS]]]SS]SSSSSSLQQJJ881NNVVIZVUUUUQUYVVNOJJOQQQOQQQQLOOOOOOOONLOOQQOOOOROOQAG1>30=000^0
.E
=P0 0=& ;rPPEEWcKWEEKKWE;KI=6==<-&""&=CKI0.$0I0..860360<<===F=RA~vv�����~~~sssssss]sss[^p^I;6/>1$1],9>>>OoM;//AP>MM>OPazO^c3&=WPWr,"0K." ''1A8GOOOQLLQSSS]QSSSSSSSLSSSSOJJ81NGIVUZQQQQUUZVILJOJQQQQQQQLLLLLLOOOOOOQOQQOOROOOJJQA1@>= 00.$=C
PK'.P;&P; &0C=PPTPEWcPKcTORIC6.===I==8.&"
"$0;=4,;.+.08=C6008?8=F=F<v�����v�s~ssss]s~ss[ss]^~[MI;"$/$$KO<9/$/PO>[>9P]O]^]iPrU^cPP^WPyr?;c^K&
+ 0''18OLOLQLQLSLSSS]SSLLQSLQLLLOGJ88ANNGUIISQUQUNGAJJQQQ]SQSSQSQLQQQLOOOOOONONNOOONOOJAA1'3=0""..0$V'
K="=)
0C"" 0^P000KWc^Wc^PcTTIRI?=FI?=?F=884&$,+$$&,.=;46A=8=0=8=F=FFFM��t�pp~sp~sZssp~ssssss[p~^^^;/"$]>*$//@[Mi]>OM>M]]pPwzc�r^PPrrj0PC??;
>'1 18GLOLQLLSLLLQSLLLSQQLLQOLOLOJ81GN]GV]YYYGNGQGOLQQSVSQSLQQOQQOOOOOOOOOORQOOOOOOQVA11'& . '0="8$?P 'K
'PP^=,^^WI,[^T^^^crrdARRRRIRIKPKKP=C;;;8.$"$+,=ITI=88F88F=FOIFFItt�v~pppp~ps~sss~sZ~~s[^p[;IMI//3]V</;<]M<>MO;>I]IMMMwzzjccrrccPPPCK?
'''VGGGOLQQLQLOOOLOLOLOQLQQOQQQLGOANSGV]]VVV]]VJNQLQQQQQSVQLLQQLQLLLLQLLOOQOOONOOOUYGA@>0$ 0&.0=&
=c0P;6"
00?^r?.&Pc^;8^Kcc^P[^nccc^^T^[R=;=80,0,&4,)"$$&;,0CPCC=88A=R[IFF=t�vppYpv~�~~p~ppp^ppps[[[[^^[^;1Op]E/1[</"MM/<s^V^o�w�zjcc^crP?0cP^
C? 0U_AL8OOOOOOOQOOGLLLOOOGOLQQQLQLLQGYYYYYYd]]YVQJOLLSSSLQQSQLQSSQLQLSLOUQOOOOMOOOROLAG>33& B,&=&
+
PP$
PyyjP&
CC3CcP$KzKIC^ICI[d[dcrcrcyc^^P^K?=8&&+&.+"$+.08.00IYOA8?8ORIFO=t�vpp~~vd�pvpp~~^^ppp~[[sss[^I;% ]iO1<K]**2iV/IV]pt�w��aPa\crj0PcrP.
Kc0'0V^@'1AGLQOLOOOOOJOOOOOOLOLLQQQLLLLSNVSVVYVVSYYGLLLQSVSSSSQSQSQQSSSQLLOOQQNMOMOOONOOI@'U\
' 0B".P0
&^P0cy^c0 3\PCP^^$$cr[I?^[[^P[^dnTI[nc[II??KP8..&+.+&"$+8C2606C[[?F8IOO=I=�vpvpvp�pppp��pv^p^^pp[[[~s^^;/%12pM9E at i>/</MI<;V~v{���zU3PUCzzPPCPj? Kyc&PjP&
'1AGLLLLOLOONGOOOGLLLOLQLLLQQLQQJJQUVYYVVUVQLQLQQQQQSSQSSQLQLQQQQQUUQONNNNNMNNOAN_''0P"".0 [...]
dC?\>@Pjc?=cP.,Pcc^^[c?[ccnnd`Odn[T??KK?=<..++,.-0+.8I.486?=I[R?FIFA=FvYppp~vpp~p�v��~vpP^ps^][^s[[;1"1 at MI1>KOE<I>9/IV^ppo��ookUOCcyjP0PP=r�[Tc3"0''1AJ8LLOOOOOOOOJOOOOGOLLLLOOOGJJJQQUUQUQQOOOQLQOOOOSQQQQOQSQQQOOQQQOONNONMOOOJLAG\
jV
,.& 0K=?
P"PP3CC''0?PK0cC46PnncIT[Rnn[Odand[[TnYA4&&-.&...++..&0K68A==8=?^IIFIC=Iivppvvvpps�vv�vpv]]ps^s[[pZ[;M;/1I<1/>OIO<<A1IpV^]w�tyoUU1'P�yPPPCyr��n0.^^=
?0
'G1AAGGOONNOGOONOMOGGOLOOONONJJNNQQQQQQQOOOQOOOOLQQSQQQOLQQQQQQOOOOONNOOONOOJJAA'>C&jC$," "j^P
"0C jy^\\P3&?C=rWPKT^PcRc[OOcnnnndm`]AA2,1&-.)....&.008T688<=FI8ITIIRCAIYpYvvvppppppvv�vpipp[ss[[[[;IM11;MI/<1 at SM<Ai]^Vppt�oyPAPUOacj\AC?c��ync^rcE$.B&;>'1 at GLGJOOOQOONONJMOLOOGOOONJJJJMMUUUUUUQOJLLQOQOSSQQQQQQQLQQQQQQOOOONNOONMOOOLA1'
C0
&r0".. "'crP^0$=\cj\ykCP3
+
0PrKErK^^cnRma[[O[an``WIC at 1,&%-<=.&..&$.&0K66=A=F==F?IIYT=?vvvvvppvppivvp�~vv]s^[ss[[;I^"";^]I</1U>/1M]OM]v�o^oP"C1\UcUaa33Crycc^r��nC3P=E0 3'1AA8LGGYYOONNNJJNOOONJOJNONONJOJQUUVULGOLLLQQOQOOQQQSQQOOQOOOQOOONOOQOMOSQJLAV0 P
&j0".=B0"'0PjVrP&3jy\\jj=??"
+0rrCcPcr^n^dndOYamomWWUIC:1::363)&&.&"$.8C06<==F=8AFFFPYFCvvivpv�pppvv�vpp�vp^[[ss[^^^;//;VOI>/9O/11oM>MvtiMMU;1U:`w3Ow33Pry&0cy��yrcc^P.&K3'@V at VQLNSONJJJJJOOOMNJJOOJNJNMGGJLVZVULOGLOOOOONNOOQQQQOQQQQQQQOOOOONONOQVYQL1'P00C0=r0000 Pcrr>>C\\yjycKC&0r^^KKrzrnncnaY][[````]IK@<:.-.-"
""$..;626688F=AAAF?RRIRiivvv��piov�vpp�{pss^[[[[s[^[;/I[MII;I at 1>[hM11PP;MPP;@@'PU3PkAPjr>Cy�yr�rryP =c&
0>3>@1NSJGJJJNNJNONOMNMMONNNMMOJJAVYYUOGOLOOMNOONOONOOONOOQOOQOOOONNNNNOOOUVN'
CC&P0CCc&. [...]
=0PP^yP^cP[n^Yc[Oa``ioiPoo]C@@@@2:KKM<<<66..&&&.=86=IIC008=AIECCE~okv�ooooio�vpPv]v^pvpPppps^[^MIII]><M11K];"6<;`to>@^Pwk�wMP3r�za\cP
&Ky�y^�zr�rOzzrznCP3PU@>@UV18GV8>NJLVGAVAGAAVNGVNLJL]SSJOOQOMOUZMOOOQYOOLVYGYLJ@:C>
3P\nOP3
P\P�\z\U��kU\z�z>
&CPcCP0PP0PWPc^WKPPO[OY`AM`MAA at IP`o[@@2@@:2AFF=82...-+..<6.8==80888=IKC;Eo{{vooooPoovvvv]p]OppPPvpp[^^>;IIMvPMM;1<O////APoi;POAktoPU1C�wP\nrCPyyn^czj3cyca���wcccAc33CUUV11 at VANG8GG8VGGGLVVGNNA>GLNQGGLYLQOGOSOOGJJYQOJLN__I1 at U3\3
Pj\kac3Ozw�zkz@ [...]
& jC&KPP0^P0?cKOKPYO`OYM211*121 at AP`@:199::22<=..0<&&&.68=+6FI=8408FIC;0Cvto~owo`ovvvpPPhpPM^]^Ovp^M^II<IMOiOM>11<O;/II/>`1MPMtPkzoPUkjjzUCcn`cyj^C30rc;rc=y���nczccaP^czcP1'U>\_A1fA at j@VA8\V1VAA>8>NQGQOYLQLGGYQOLGVSNLQGGUf_:'U3Ck &rPU\kkU\yzykz_ at f\�3 jUyc
0
K�0P^PPKP03^c?<<<=AYMM at 99/>@@@PYPM11112--2&..302.--.88808ACC8=;8C?;88C]ktwmmohiviiiPiv]OP]vPvpp]pVI;;MOPOM^111A>II<I;]@$P:MiawoPoPakckA>zUPrc^^C?Pc^C�3P�k�zrz�z�arjzrUP'3\Uj@>@\@\f__1 at fV>N:NA8AVGNN8NVQLJAVGJJAVAYAJA1'\k3>0U3zc0ckUzzzkU\zykty@>\�\
PPPyc?
PrrycPKP00=cOFFIOOYYMMII[K@:@U`@M@*%:9----...-.66.0A;88A;CIC;668?88;8``ommo`ov_iii]iv[iPOP^Pvvp]^I;]P>@M]]>1>P/<<<<M`"/`OMPztiM]U\aw`3PzOP^cP?& Prnyc3z�^�y�rz��crjz�P\3C\j\3\U\Cj\U\>>\\\\@UA11_G1V1 at kNA1A_NLA.VA_NNG1"UyOc3UCPz\cwkk�yy\\Uy�k_U@\kjPPc =nP
&yrrKWEPPPC^rOIO[daYa``M][][MYO?AO<2.2-<3..-.6..80.;868888IK;4;8;88=?O`adok~ov]i]`]ii[[@]]iiippV^IMM>>MOOM>/IO</<<MPO/>[M2PoUM at UUnUP>C�kU3an=.
Ky��rPjrra�r����zzjz�zjU>jUj>C\jPcj>jP>^jcPj@\@>@k@@f@@y_ at 1@_VGN>VN at _P@@'>zj[CUz\�jr�ztUtYkyU\yyU@>>VjyCc? PPK)=cCPKEPE=KKPynT[[^daa`aak]`MMOA=<PMD=<6...&2860.6&2880;8;;;IE484C;;;C`am[koooa`o``ho[KAOPPP^ppp[^;1M1O]OI1MOE>>>MM;2MIO@'OooP1PAao33cwoP1aOU3?Pycr�yr�cCczz����zzj��k�>>jcyC\jjCcP?jC?cz\PrPyP\PzU^zU@�kU@>k\1@>>>>zU_>>Pj�kaCjkykk��yUy_Y�k\ [...]
& KjPn&$$crKR=0,P;c^PPOOFA=cw]MMMAMA@:21A at 22@32=3&&&602-&-&-660880+8;;;4;CK40,.8E;Oo`P[POao`iPMOOAI]~iP]pM^V^>AAP[]]OO@:>]]]M1<>MMO/12.:C33U3UPPP at Ocww`PUOcy��ykzP.[3&&
P�czaCky__ at Gky_@kkjV at kf@U\zjy\\Pj\jjz�k���kkjz_zYky\kU\@\ykykNN_kwtyN_�yyfk\wUk\'C�\3n\
Pc^?, cP==.,P?PP[[PP=AAMwaMDA<2:<@2:M1 at C:-&=@.."&-..-$),38.8=0+00;;;6;R;6,0;C=Po[[PIYao`PU[aRRMiiovi]MppM at AWM]]PP]I1O][]11>I<O://23'P3AUU at MU33kkwoPP`jar�r��a3OP3
&C�jac>'P�kwtYt_@@fkk@@yN:@UjU\\\\\\jz�\Uy�ykykz_kVU�Uy at k_Uy_yfNAV_ffyN1y��UUU\zcU>&Cj 3k
j^r;&?K====K^^a[[FM`?=MnwWMMK<<<KAM[::@12&=3-<&&&..+-)&-0.=C0+80;;;08C=00.,,6`a[IC[a```i]`[OM]i]]POPpvMO:A]K]v]O]KPOO]O1/I1>M</"
3>U?kU:"CPPUaayP`kkoUn�rw�P?c?0Pjy�z[3?&j��yt_yYAU�\_V__NANV\V>VV\@@\\zV_y�_fk_yY_N_f_yNtNYfNytdANV_y_N1 [...]
CP=;,KK=C.P^^^YOMOIOFMOamo][KA:<3<9:::,,<2&&&&"&..0."&.88.0..0000008;6800000K^adaMOY[PRO=FOKP]oKKOPii]M<O?KP[MM]@OP:I@>>@O>/1/2aaa3%/13U:1&2UPoo]Uwon�`rz��rrn^jryrjOcCcCPCCayf@@U111UNIJAJAAGNGIVINVIVQGffYLdILAIAYdI_QN_I_IAY__IA1:kt\@
\�yz3
&
=\
&P\?j^K=0 &^$B=Ec?T=3^aPcOOODFFFMOOMOK@:@IKD:<:<<3<2-%
"&..+-&$-.6=.60&..880,0884,,080^naOF?ddaOKMFMM[]``POOOi]MFMMYWPPDMM@[K2AK@>O>/1$%=UA3&%@2OP/1APUMt]awo`zrnzP�zzncr�rrcrja>c3aCkU\UUY>1 at G@1YUAO>>>NAIAQNNUUVUtfGAYUJAIANNI_IN_ItIAI_iI.11Pk\> >�� [...]
$ccK8<=6=KW^IIPORYY[[^T[WTT?0KK^[?FORO=<==3C==<<:@%$%-"A2%132>1/@`UP>iYoyw��tPyz���tUnzz�zzwwykwkUUtkk_UN^@UUPY\_UUkYU_:U_:@kYP_UYYY_f_U_U:@f_UkUUyUk at AaP&3cccI\CP^jnz�oUnj��wkKK]UM2 at Kok@1IOMd1$$1,-:D4,'&44.)'-""!!"!.)"&!.006AK8.&
"
$
WC8ARD8CKEKICT^IIR[T^^KWWEC0=PTKOOO??8==??O==6<::*3A-&-2%$&1/1;AMPM>i]tw�oowwz���wow�z����zzwkzyokkktkfoY_f\YVkt_k_YtU:U@@@f at _kUt_U_U__Y_Y_ktUyftyUkka\UUCkjzakncnzww�z``k���o[IOiPM@<@`K112I`14*$9,,-4::,,-66-'4-!'!.-"
0&&8 [...]
EC=CTC;EECIIPKIIFRPTTPKKKKE;KWTKYF=0?80=F=?==6<:2%
-2,<D- at 2 2/1/AOO:1Po�wotowww���it�zz�����w�y�kykkkykfUykfyfkyyyy_NtU@\P at Mf_o_fkf_t_U_Nff_yytyyy\Uyyck`U\Uwzzwzoowwow�tUanw�t[OE]@]P::M[O>11WE<$ ,,%'',',,44-)'%'
))"""
0++.F0 """
,8," TT=EEEEIE=?RK;;KITOCPTKKPTKTWRTIF??==?=F=====<:21!22-<-!2@$$A/"1A>>>$At�io�o~tw�yoo��������ywz���kwzywkktfkkt\kzyky_tfffk__Ykk_ktyt_____oyyyyytttt_tkk`kUUokzzzzwto��ow�yoowm��i]OM]IK[<<:O@/1K<* &%$ %%
!,%!!)!!$&!"$0&&& [...]
+
&
$$%&,&&&&.&&.-$&.&.&&.&&.&02.&3&.&&..&&&&&.
&&&&& 2&' &.&&..3.33&&3.31...&.&&&.&&%.&&%%&%&%&%&&1%%%)%'
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/tutorial/image/monkey.png b/tutorial/image/monkey.png
new file mode 100644
index 0000000..81bd2dc
Binary files /dev/null and b/tutorial/image/monkey.png differ
diff --git a/tutorial/image/monkey.ppm b/tutorial/image/monkey.ppm
new file mode 100644
index 0000000..b725d13
--- /dev/null
+++ b/tutorial/image/monkey.ppm
@@ -0,0 +1,4 @@
+P6
+256 256
+255
+oQ/UL-fZ8D:+UL-gg9oQ/oQ/��;fZ8UL-UL-y�<y�Jgg9��IhuIy�Jgg9gg9GF-��;y�<y�<y�<lr9lr9��;y�<��W��I��W��Wy�<��h��IZe;wv;y�<gg9��Wo�VJU:Ze;gg9��xfjg86+WW8o�Vlr9o�VZe;huI��g��gx�k�����Yo�Vj�h��������������u��xx�hVZVQjJz�yv��v��x�kz�yIWGQjJKeWQjJIWGJU:huI��hZe;IWGJU:x�k[tXhuIx�k���z��QjJhvXgy�QjJ���i��w�w������w�wx�hz��px�fjgz��ZwvOKNi��������z�yz��:GFGH8hxgi��v�����i��YuhWifZwvj�����]�qQjJKeW=eeKeW]�qZ��gyw���IYVfjg������ZwvZwvi�����������i��w��v��v��WiwVZVy��Wiwi�xw��������z��������y��y�����v�� [...]
\ No newline at end of file
diff --git a/tutorial/image/tutorial-image-converter.cpp b/tutorial/image/tutorial-image-converter.cpp
index 76f55aa..6b9c9ee 100644
--- a/tutorial/image/tutorial-image-converter.cpp
+++ b/tutorial/image/tutorial-image-converter.cpp
@@ -1,6 +1,6 @@
/*! \example tutorial-image-converter.cpp */
-#include <visp/vpImageIo.h>
-#include <visp/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageConvert.h>
int main()
{
@@ -8,16 +8,16 @@ int main()
try {
cv::Mat A;
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- A = cv::imread("lena.bmp", cv::IMREAD_GRAYSCALE);
+ A = cv::imread("monkey.bmp", cv::IMREAD_GRAYSCALE);
#else
- A = cv::imread("lena.bmp", CV_LOAD_IMAGE_GRAYSCALE);
+ A = cv::imread("monkey.bmp", CV_LOAD_IMAGE_GRAYSCALE);
#endif
vpImage<unsigned char> I;
vpImageConvert::convert(A, I);
-# ifdef VISP_HAVE_LIBPNG
- vpImageIo::write(I, "lena.png"); // Gray
+# ifdef VISP_HAVE_PNG
+ vpImageIo::write(I, "monkey.png"); // Gray
# endif
}
catch(vpException e) {
diff --git a/tutorial/image/tutorial-image-filter.cpp b/tutorial/image/tutorial-image-filter.cpp
index c224206..8e01aeb 100644
--- a/tutorial/image/tutorial-image-filter.cpp
+++ b/tutorial/image/tutorial-image-filter.cpp
@@ -1,12 +1,12 @@
//! \example tutorial-image-filter.cpp
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageFilter.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageFilter.h>
void display(vpImage<unsigned char> &I, const std::string &title);
void display(vpImage<double> &D, const std::string &title);
diff --git a/tutorial/image/tutorial-image-manipulation.cpp b/tutorial/image/tutorial-image-manipulation.cpp
index fe230a7..6f8c459 100644
--- a/tutorial/image/tutorial-image-manipulation.cpp
+++ b/tutorial/image/tutorial-image-manipulation.cpp
@@ -1,5 +1,5 @@
/*! \example tutorial-image-manipulation.cpp */
-#include <visp/vpImage.h>
+#include <visp3/core/vpImage.h>
int main()
{
diff --git a/tutorial/image/tutorial-image-reader.cpp b/tutorial/image/tutorial-image-reader.cpp
index 109b806..4c72a3c 100644
--- a/tutorial/image/tutorial-image-reader.cpp
+++ b/tutorial/image/tutorial-image-reader.cpp
@@ -1,12 +1,12 @@
/*! \example tutorial-image-reader.cpp */
-#include <visp/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
try {
vpImage<vpRGBa> I;
- vpImageIo::read(I, "lena.jpeg");
- vpImageIo::write(I, "lena.png");
+ vpImageIo::read(I, "monkey.jpeg");
+ vpImageIo::write(I, "monkey.png");
}
catch(vpException e) {
std::cout << e.getMessage() << std::endl;
diff --git a/tutorial/image/tutorial-image-viewer.cpp b/tutorial/image/tutorial-image-viewer.cpp
index a3a6dfa..3f2bea0 100644
--- a/tutorial/image/tutorial-image-viewer.cpp
+++ b/tutorial/image/tutorial-image-viewer.cpp
@@ -1,15 +1,15 @@
/*! \example tutorial-image-viewer.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImagePoint.h>
int main()
{
try {
vpImage<vpRGBa> I;
- vpImageIo::read(I, "lena.ppm");
+ vpImageIo::read(I, "monkey.ppm");
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
@@ -20,7 +20,7 @@ int main()
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
- vpDisplay::setTitle(I, "Lena");
+ vpDisplay::setTitle(I, "Monkey");
vpDisplay::display(I);
vpDisplay::displayRectangle(I, vpImagePoint(90,90), 70, 90, vpColor::red, false, 2);
@@ -30,8 +30,8 @@ int main()
vpDisplay::getImage(I, O);
try {
- vpImageIo::write(I, "lena-out.jpg");
- vpImageIo::write(O, "lena-out-with-overlay.jpg");
+ vpImageIo::write(I, "monkey-out.jpg");
+ vpImageIo::write(O, "monkey-out-with-overlay.jpg");
}
catch(...) {
std::cout << "Cannot write the image: unsupported format..." << std::endl;
diff --git a/tutorial/image/tutorial-undistort.cpp b/tutorial/image/tutorial-undistort.cpp
index b26460f..dbce3be 100644
--- a/tutorial/image/tutorial-undistort.cpp
+++ b/tutorial/image/tutorial-undistort.cpp
@@ -1,7 +1,7 @@
//! \example tutorial-undistort.cpp
-#include <visp/vpImageIo.h>
-#include <visp/vpImageTools.h>
-#include <visp/vpXmlParserCamera.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpXmlParserCamera.h>
int main()
{
diff --git a/tutorial/image/tutorial-viewer.cpp b/tutorial/image/tutorial-viewer.cpp
index cfd8439..2ab5efd 100644
--- a/tutorial/image/tutorial-viewer.cpp
+++ b/tutorial/image/tutorial-viewer.cpp
@@ -1,13 +1,13 @@
//! \example tutorial-viewer.cpp
//! [Include display]
-#include <visp/vpDisplayD3D.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
//! [Include display]
//! [Include io]
-#include <visp/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
//! [Include io]
int main(int argc, char** argv)
@@ -35,12 +35,12 @@ int main(int argc, char** argv)
//! [vpDisplay construction]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV d(I);
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK d(I);
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI d(I);
#elif defined(VISP_HAVE_D3D9)
vpDisplayD3d d(I);
#else
diff --git a/tutorial/robot/pioneer/CMakeLists.txt b/tutorial/robot/pioneer/CMakeLists.txt
index 1de807f..8a18330 100644
--- a/tutorial/robot/pioneer/CMakeLists.txt
+++ b/tutorial/robot/pioneer/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-robot)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_robot visp_vs visp_gui)
set(tutorial_cpp
tutorial-pioneer-robot.cpp
diff --git a/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp b/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
index 40c1008..ba97712 100644
--- a/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
+++ b/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
@@ -1,6 +1,6 @@
/*! \example tutorial-pioneer-robot.cpp */
#include <iostream>
-#include <visp/vpRobotPioneer.h>
+#include <visp3/robot/vpRobotPioneer.h>
int main()
{
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
index 19a028e..5418082 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
@@ -17,14 +17,14 @@
*/
#include <iostream>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPlot.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorPioneer.h>
-#include <visp/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -46,8 +46,7 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ vpPoint point(0,0,0);
point.track(cMo);
vpServo task;
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
index e2d7371..d540573 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
@@ -17,14 +17,14 @@
*/
#include <iostream>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPlot.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorPioneer.h>
-#include <visp/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -46,8 +46,7 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ vpPoint point(0,0,0);
point.track(cMo);
vpServo task;
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
index a3c587b..9e31ea7 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
@@ -18,14 +18,14 @@
*/
#include <iostream>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPlot.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorPioneerPan.h>
-#include <visp/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorPioneerPan.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -54,8 +54,7 @@ int main()
wMo = wMc * cMo;
// Define the target
- vpPoint point;
- point.setWorldCoordinates(0,0,0); // Coordinates in the object frame
+ vpPoint point(0,0,0); // Coordinates in the object frame
point.track(cMo);
vpServo task;
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
index 6484c9a..32ea393 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
@@ -17,14 +17,14 @@
*/
#include <iostream>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpFeatureDepth.h>
-#include <visp/vpFeaturePoint.h>
-#include <visp/vpHomogeneousMatrix.h>
-#include <visp/vpPlot.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorPioneer.h>
-#include <visp/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -46,8 +46,7 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ vpPoint point(0,0,0);
point.track(cMo);
vpServo task;
diff --git a/tutorial/simulator/image/CMakeLists.txt b/tutorial/simulator/image/CMakeLists.txt
index 82c7927..e2d6f97 100644
--- a/tutorial/simulator/image/CMakeLists.txt
+++ b/tutorial/simulator/image/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-simulation-image)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_robot visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/simulator/image/tutorial-image-simulator.cpp b/tutorial/simulator/image/tutorial-image-simulator.cpp
index 589a6a5..58230c1 100644
--- a/tutorial/simulator/image/tutorial-image-simulator.cpp
+++ b/tutorial/simulator/image/tutorial-image-simulator.cpp
@@ -1,10 +1,10 @@
//! \example tutorial-image-simulator.cpp
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpImageIo.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpImageIo.h>
//! [Include]
-#include <visp/vpImageSimulator.h>
+#include <visp3/robot/vpImageSimulator.h>
//! [Include]
int main()
diff --git a/tutorial/trace/CMakeLists.txt b/tutorial/trace/CMakeLists.txt
index 8a22082..87774e1 100644
--- a/tutorial/trace/CMakeLists.txt
+++ b/tutorial/trace/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-trace)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/trace/tutorial-trace.cpp b/tutorial/trace/tutorial-trace.cpp
index 737e285..dfbe5fd 100644
--- a/tutorial/trace/tutorial-trace.cpp
+++ b/tutorial/trace/tutorial-trace.cpp
@@ -3,7 +3,7 @@
//#define VP_DEBUG // Activate the debug mode
#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-#include <visp/vpDebug.h>
+#include <visp3/core/vpDebug.h>
int main()
{
diff --git a/tutorial/tracking/blob/CMakeLists.txt b/tutorial/tracking/blob/CMakeLists.txt
index e2c7149..961a442 100644
--- a/tutorial/tracking/blob/CMakeLists.txt
+++ b/tutorial/tracking/blob/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-blob)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_blob visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp b/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
index 92e608a..48c4e6b 100644
--- a/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-blob-auto-tracker.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
-#include <visp/vpImageIo.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
diff --git a/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp b/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
index f893e66..4fb2b75 100644
--- a/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
@@ -1,17 +1,20 @@
//! \example tutorial-blob-tracker-live-firewire.cpp
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#endif
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
int main()
{
-#if (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
vpImage<unsigned char> I; // Create a gray level image container
-#if defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g(false);
g.open(I);
#elif defined(VISP_HAVE_CMU1394)
@@ -49,7 +52,7 @@ int main()
while(1) {
try {
-#if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394)
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
g >> frame;
diff --git a/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp b/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
index 166682c..cae17dc 100644
--- a/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
@@ -1,10 +1,13 @@
//! \example tutorial-blob-tracker-live-v4l2.cpp
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayGTK.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDot2.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/blob/vpDot2.h>
int main()
{
diff --git a/tutorial/tracking/keypoint/CMakeLists.txt b/tutorial/tracking/keypoint/CMakeLists.txt
index 007ac47..f420008 100644
--- a/tutorial/tracking/keypoint/CMakeLists.txt
+++ b/tutorial/tracking/keypoint/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-keypoint)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_vision visp_klt visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
index b6f824c..05ff239 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
@@ -1,9 +1,12 @@
/*! \example tutorial-klt-tracker-live-v4l2.cpp */
-#include <visp/vpImageConvert.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
-#include <visp/vpV4l2Grabber.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, const char *argv[])
{
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
index 7013c0c..aac1655 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
@@ -1,8 +1,8 @@
//! \example tutorial-klt-tracker-with-reinit.cpp
-#include <visp/vpImageConvert.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
int main()
{
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
index 2dbe100..cb2a8c9 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
@@ -1,9 +1,9 @@
//! \example tutorial-klt-tracker.cpp
//! [Include]
-#include <visp/vpImageConvert.h>
-#include <visp/vpKltOpencv.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
int main(int argc, const char *argv[])
diff --git a/tutorial/tracking/model-based/edges/CMakeLists.txt b/tutorial/tracking/model-based/edges/CMakeLists.txt
index fbad7e2..b08bdab 100644
--- a/tutorial/tracking/model-based/edges/CMakeLists.txt
+++ b/tutorial/tracking/model-based/edges/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-mb-edges)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_mbt visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/model-based/edges/teabox.xml b/tutorial/tracking/model-based/edges/teabox.xml
index 3869704..8995130 100644
--- a/tutorial/tracking/model-based/edges/teabox.xml
+++ b/tutorial/tracking/model-based/edges/teabox.xml
@@ -13,11 +13,10 @@
<mu1>0.5</mu1>
<mu2>0.5</mu2>
</contrast>
+ <sample>
+ <step>4</step>
+ </sample>
</ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
<camera>
<u0>325.66776</u0>
<v0>243.69727</v0>
diff --git a/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp b/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
index 6d1794f..b405654 100644
--- a/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
+++ b/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
@@ -1,13 +1,13 @@
//! \example tutorial-mb-edge-tracker.cpp
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
//! [Include]
-#include <visp/vpMbEdgeTracker.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
//! [Include]
-#include <visp/vpVideoReader.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, char** argv)
{
@@ -85,7 +85,6 @@ int main(int argc, char** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
tracker.setMovingEdge(me);
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
@@ -104,6 +103,7 @@ int main(int argc, char** argv)
}
//! [Set ogre]
tracker.setOgreVisibilityTest(false);
+ tracker.setOgreShowConfigDialog(false);
//! [Set ogre]
//! [Load cao]
if(vpIoTools::checkFilename(objectname + ".cao"))
@@ -144,14 +144,19 @@ int main(int argc, char** argv)
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
//! [Cleanup]
}
catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
}
+#ifdef VISP_HAVE_OGRE
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ }
+#endif
#else
(void)argc;
(void)argv;
diff --git a/tutorial/tracking/model-based/generic/CMakeLists.txt b/tutorial/tracking/model-based/generic/CMakeLists.txt
new file mode 100644
index 0000000..0786938
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/CMakeLists.txt
@@ -0,0 +1,29 @@
+project(tutorial-tracking-mb)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_mbt visp_io visp_gui)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-mb-tracker.cpp
+ tutorial-mb-tracker-full.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.mpg" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.cao" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.init" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.ppm" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.wrl" )
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.xml" )
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-mb-tracker.cpp ${data})
+endforeach()
diff --git a/tutorial/tracking/model-based/generic/teabox.cao b/tutorial/tracking/model-based/generic/teabox.cao
new file mode 100644
index 0000000..83c1fdf
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/teabox.cao
@@ -0,0 +1,27 @@
+V1
+# 3D Points
+8 # Number of points
+0 0 0 # Point 0: X Y Z
+0 0 -0.08
+0.165 0 -0.08
+0.165 0 0
+0.165 0.068 0
+0.165 0.068 -0.08
+0 0.068 -0.08
+0 0.068 0 # Point 7
+# 3D Lines
+0 # Number of lines
+# Faces from 3D lines
+0 # Number of faces
+# Faces from 3D points
+6 # Number of faces
+4 0 1 2 3 # Face 0: [number of points] [index of the 3D points]...
+4 1 6 5 2
+4 4 5 6 7
+4 0 3 4 7
+4 5 4 3 2
+4 0 7 6 1 # Face 5
+# 3D cylinders
+0 # Number of cylinders
+# 3D circles
+0 # Number of circles
diff --git a/tutorial/tracking/model-based/generic/teabox.init b/tutorial/tracking/model-based/generic/teabox.init
new file mode 100644
index 0000000..f109800
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/teabox.init
@@ -0,0 +1,5 @@
+4 # Number of points
+0 0 0 # Point 0
+0.165 0 0 # Point 3
+0.165 0 -0.08 # Point 2
+0.165 0.068 -0.08 # Point 5
diff --git a/tutorial/tracking/model-based/generic/teabox.mpg b/tutorial/tracking/model-based/generic/teabox.mpg
new file mode 100644
index 0000000..50fb8ad
Binary files /dev/null and b/tutorial/tracking/model-based/generic/teabox.mpg differ
diff --git a/tutorial/tracking/model-based/generic/teabox.ppm b/tutorial/tracking/model-based/generic/teabox.ppm
new file mode 100644
index 0000000..b20630a
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/teabox.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+GGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGNNNGGGNNNNNNGGGNNNNNNNNNNNNNNNNNNNNNNNNNNNVVVNNNNNNVVVVVVVVVVVVVVVVVVVVVVVV^^^VVV^^^^^^^^^VVV^^^^^^^^^^^^^^^NNNNNNNNNNNNNNNNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVNNNVVVVVVVVVVVVVVV^^^VVV^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ [...]
\ No newline at end of file
diff --git a/tutorial/tracking/model-based/generic/teabox.wrl b/tutorial/tracking/model-based/generic/teabox.wrl
new file mode 100644
index 0000000..a452ea0
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/teabox.wrl
@@ -0,0 +1,33 @@
+#VRML V2.0 utf8
+
+DEF fst_0 Group {
+children [
+
+# Object "teabox"
+Shape {
+
+geometry DEF cube IndexedFaceSet {
+
+coord Coordinate {
+point [
+0 0 0 ,
+0 0 -0.08,
+0.165 0 -0.08,
+0.165 0 0 ,
+0.165 0.068 0 ,
+0.165 0.068 -0.08,
+0 0.068 -0.08,
+0 0.068 0 ]
+}
+
+coordIndex [
+ 0,1,2,3,-1,
+ 1,6,5,2,-1,
+ 4,5,6,7,-1,
+ 0,3,4,7,-1,
+ 5,4,3,2,-1,
+ 0,7,6,1,-1]}
+}
+
+]
+}
diff --git a/tutorial/tracking/model-based/generic/teabox.xml b/tutorial/tracking/model-based/generic/teabox.xml
new file mode 100644
index 0000000..5efcd38
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/teabox.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>8</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ <camera>
+ <u0>325.66776</u0>
+ <v0>243.69727</v0>
+ <px>839.21470</px>
+ <py>839.44555</py>
+ </camera>
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ <near_clipping>0.1</near_clipping>
+ <far_clipping>100</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+</conf>
+
diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp
new file mode 100644
index 0000000..8434c33
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp
@@ -0,0 +1,213 @@
+//! \example tutorial-mb-tracker-full.cpp
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+//! [Include]
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
+//! [Include]
+#include <visp3/io/vpVideoReader.h>
+
+int main(int argc, char** argv)
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+
+ try {
+ std::string opt_videoname = "teabox.mpg";
+ int opt_tracker = 0;
+
+ for (int i=0; i<argc; i++) {
+ if (std::string(argv[i]) == "--name")
+ opt_videoname = std::string(argv[i+1]);
+ if (std::string(argv[i]) == "--tracker")
+ opt_tracker = atoi(argv[i+1]);
+ else if (std::string(argv[i]) == "--help") {
+ std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n" << std::endl;
+ return 0;
+ }
+ }
+ std::string parentname = vpIoTools::getParent(opt_videoname);
+ std::string objectname = vpIoTools::getNameWE(opt_videoname);
+
+ if(! parentname.empty())
+ objectname = parentname + "/" + objectname;
+
+ std::cout << "Video name: " << opt_videoname << std::endl;
+ std::cout << "Tracker requested config files: " << objectname
+ << ".[init,"
+#ifdef VISP_HAVE_XML2
+ << "xml,"
+#endif
+ << "cao or wrl]" << std::endl;
+ std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
+
+ //! [Image]
+ vpImage<unsigned char> I;
+ vpCameraParameters cam;
+ //! [Image]
+ //! [cMo]
+ vpHomogeneousMatrix cMo;
+ //! [cMo]
+
+ vpVideoReader g;
+ g.setFileName(opt_videoname);
+ g.open(I);
+
+ vpDisplay *display = NULL;
+#if defined(VISP_HAVE_X11)
+ display = new vpDisplayX;
+#elif defined(VISP_HAVE_GDI)
+ display = new vpDisplayGDI;
+#else
+ display = new vpDisplayOpenCV;
+#endif
+ display->init(I, 100, 100, "Model-based tracker");
+
+ //! [Constructor]
+ vpMbTracker *tracker;
+ if (opt_tracker == 0)
+ tracker = new vpMbEdgeTracker;
+#ifdef VISP_HAVE_MODULE_KLT
+ else if (opt_tracker == 1)
+ tracker = new vpMbKltTracker;
+ else
+ tracker = new vpMbEdgeKltTracker;
+#else
+ else {
+ std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ return 0;
+ }
+#endif
+ //! [Constructor]
+
+ bool usexml = false;
+ //! [Load xml]
+#ifdef VISP_HAVE_XML2
+ if(vpIoTools::checkFilename(objectname + ".xml")) {
+ tracker->loadConfigFile(objectname + ".xml");
+ usexml = true;
+ }
+#endif
+ //! [Load xml]
+
+ if (! usexml) {
+ //! [Set parameters]
+ if (opt_tracker == 0 || opt_tracker == 2) {
+ //! [Set moving-edges parameters]
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ dynamic_cast<vpMbEdgeTracker*>(tracker)->setMovingEdge(me);
+ //! [Set moving-edges parameters]
+ }
+
+#ifdef VISP_HAVE_MODULE_KLT
+ if (opt_tracker == 1 || opt_tracker == 2) {
+ //! [Set klt parameters]
+ vpKltOpencv klt_settings;
+ klt_settings.setMaxFeatures(300);
+ klt_settings.setWindowSize(5);
+ klt_settings.setQuality(0.015);
+ klt_settings.setMinDistance(8);
+ klt_settings.setHarrisFreeParameter(0.01);
+ klt_settings.setBlockSize(3);
+ klt_settings.setPyramidLevels(3);
+ dynamic_cast<vpMbKltTracker*>(tracker)->setKltOpencv(klt_settings);
+ dynamic_cast<vpMbKltTracker*>(tracker)->setMaskBorder(5);
+ //! [Set klt parameters]
+ }
+#endif
+
+ //! [Set camera parameters]
+ cam.initPersProjWithoutDistortion(839.21470, 839.44555, 325.66776, 243.69727);
+ tracker->setCameraParameters(cam);
+ //! [Set camera parameters]
+
+ //! [Set angles]
+ tracker->setAngleAppear( vpMath::rad(70) );
+ tracker->setAngleDisappear( vpMath::rad(80) );
+ //! [Set angles]
+ //! [Set clipping distance]
+ tracker->setNearClippingDistance(0.1);
+ tracker->setFarClippingDistance(100.0);
+ //! [Set clipping distance]
+ //! [Set clipping fov]
+ tracker->setClipping(tracker->getClipping() | vpMbtPolygon::FOV_CLIPPING);
+ //! [Set clipping fov]
+
+ //! [Set ogre]
+ tracker->setOgreVisibilityTest(false);
+ tracker->setOgreShowConfigDialog(false);
+ //! [Set ogre]
+
+ //! [Set parameters]
+ }
+
+ //! [Load cao]
+ if(vpIoTools::checkFilename(objectname + ".cao"))
+ tracker->loadModel(objectname + ".cao");
+ //! [Load cao]
+ //! [Load wrl]
+ else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ tracker->loadModel(objectname + ".wrl");
+ //! [Load wrl]
+ //! [Set display]
+ tracker->setDisplayFeatures(true);
+ //! [Set display]
+ //! [Init]
+ tracker->initClick(I, objectname + ".init", true);
+ //! [Init]
+
+ while(! g.end()){
+ g.acquire(I);
+ vpDisplay::display(I);
+ //! [Track]
+ tracker->track(I);
+ //! [Track]
+ //! [Get pose]
+ tracker->getPose(cMo);
+ //! [Get pose]
+ //! [Display]
+ tracker->getCameraParameters(cam);
+ tracker->display(I, cMo, cam, vpColor::red, 2, true);
+ //! [Display]
+ vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
+ vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red);
+ vpDisplay::flush(I);
+
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+ vpDisplay::getClick(I);
+ //! [Cleanup]
+#ifdef VISP_HAVE_XML2
+ vpXmlParser::cleanup();
+#endif
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+ delete display;
+ delete tracker;
+ //! [Cleanup]
+ }
+ catch(vpException e) {
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
+ }
+#ifdef VISP_HAVE_OGRE
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ }
+#endif
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
+#endif
+}
diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp
new file mode 100644
index 0000000..7f88a34
--- /dev/null
+++ b/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp
@@ -0,0 +1,159 @@
+//! \example tutorial-mb-tracker.cpp
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+//! [Include]
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
+//! [Include]
+#include <visp3/io/vpVideoReader.h>
+
+int main(int argc, char** argv)
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+
+ try {
+ std::string opt_videoname = "teabox.mpg";
+ int opt_tracker = 0;
+
+ for (int i=0; i<argc; i++) {
+ if (std::string(argv[i]) == "--name")
+ opt_videoname = std::string(argv[i+1]);
+ if (std::string(argv[i]) == "--tracker")
+ opt_tracker = atoi(argv[i+1]);
+ else if (std::string(argv[i]) == "--help") {
+ std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n" << std::endl;
+ return 0;
+ }
+ }
+ std::string parentname = vpIoTools::getParent(opt_videoname);
+ std::string objectname = vpIoTools::getNameWE(opt_videoname);
+
+ if(! parentname.empty())
+ objectname = parentname + "/" + objectname;
+
+ std::cout << "Video name: " << opt_videoname << std::endl;
+ std::cout << "Tracker requested config files: " << objectname
+ << ".[init, cao]" << std::endl;
+ std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
+
+ //! [Image]
+ vpImage<unsigned char> I;
+ vpCameraParameters cam;
+ //! [Image]
+ //! [cMo]
+ vpHomogeneousMatrix cMo;
+ //! [cMo]
+
+ vpVideoReader g;
+ g.setFileName(opt_videoname);
+ g.open(I);
+
+ vpDisplay *display = NULL;
+#if defined(VISP_HAVE_X11)
+ display = new vpDisplayX;
+#elif defined(VISP_HAVE_GDI)
+ display = new vpDisplayGDI;
+#else
+ display = new vpDisplayOpenCV;
+#endif
+ display->init(I, 100, 100, "Model-based tracker");
+
+ //! [Constructor]
+ vpMbTracker *tracker;
+ if (opt_tracker == 0)
+ tracker = new vpMbEdgeTracker;
+#ifdef VISP_HAVE_MODULE_KLT
+ else if (opt_tracker == 1)
+ tracker = new vpMbKltTracker;
+ else
+ tracker = new vpMbEdgeKltTracker;
+#else
+ else {
+ std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ return 0;
+ }
+#endif
+ //! [Constructor]
+
+ //! [Set parameters]
+ if (opt_tracker == 0 || opt_tracker == 2) {
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ dynamic_cast<vpMbEdgeTracker*>(tracker)->setMovingEdge(me);
+ }
+
+#ifdef VISP_HAVE_MODULE_KLT
+ if (opt_tracker == 1 || opt_tracker == 2) {
+ vpKltOpencv klt_settings;
+ klt_settings.setMaxFeatures(300);
+ klt_settings.setWindowSize(5);
+ klt_settings.setQuality(0.015);
+ klt_settings.setMinDistance(8);
+ klt_settings.setHarrisFreeParameter(0.01);
+ klt_settings.setBlockSize(3);
+ klt_settings.setPyramidLevels(3);
+ dynamic_cast<vpMbKltTracker*>(tracker)->setKltOpencv(klt_settings);
+ dynamic_cast<vpMbKltTracker*>(tracker)->setMaskBorder(5);
+ }
+#endif
+
+ //! [Set camera parameters]
+ cam.initPersProjWithoutDistortion(839, 839, 325, 243);
+ //! [Set camera parameters]
+ tracker->setCameraParameters(cam);
+ //! [Set parameters]
+
+ //! [Load cao]
+ tracker->loadModel(objectname + ".cao");
+ //! [Load cao]
+ //! [Set display]
+ tracker->setDisplayFeatures(true);
+ //! [Set display]
+ //! [Init]
+ tracker->initClick(I, objectname + ".init", true);
+ //! [Init]
+
+ while(! g.end()){
+ g.acquire(I);
+ vpDisplay::display(I);
+ //! [Track]
+ tracker->track(I);
+ //! [Track]
+ //! [Get pose]
+ tracker->getPose(cMo);
+ //! [Get pose]
+ //! [Display]
+ tracker->getCameraParameters(cam);
+ tracker->display(I, cMo, cam, vpColor::red, 2, true);
+ //! [Display]
+ vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
+ vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red);
+ vpDisplay::flush(I);
+
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+ vpDisplay::getClick(I);
+ //! [Cleanup]
+ delete display;
+ delete tracker;
+ //! [Cleanup]
+ }
+ catch(vpException e) {
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
+ }
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
+#endif
+}
diff --git a/tutorial/tracking/model-based/hybrid/CMakeLists.txt b/tutorial/tracking/model-based/hybrid/CMakeLists.txt
index c126946..67428b9 100644
--- a/tutorial/tracking/model-based/hybrid/CMakeLists.txt
+++ b/tutorial/tracking/model-based/hybrid/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-mb-hybrid)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_mbt visp_klt visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/model-based/hybrid/teabox.xml b/tutorial/tracking/model-based/hybrid/teabox.xml
index 41a1afb..5efcd38 100644
--- a/tutorial/tracking/model-based/hybrid/teabox.xml
+++ b/tutorial/tracking/model-based/hybrid/teabox.xml
@@ -13,11 +13,10 @@
<mu1>0.5</mu1>
<mu2>0.5</mu2>
</contrast>
+ <sample>
+ <step>4</step>
+ </sample>
</ecm>
- <sample>
- <step>4</step>
- <nb_sample>250</nb_sample>
- </sample>
<klt>
<mask_border>5</mask_border>
<max_features>300</max_features>
diff --git a/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp b/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
index 3bdd6ed..641cad1 100644
--- a/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
+++ b/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
@@ -1,11 +1,11 @@
/*! \example tutorial-mb-hybrid-tracker.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, char** argv)
{
@@ -72,9 +72,7 @@ int main(int argc, char** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- me.setNbTotalSample(250);
tracker.setMovingEdge(me);
- tracker.setMaskBorder(5);
vpKltOpencv klt_settings;
klt_settings.setMaxFeatures(300);
klt_settings.setWindowSize(5);
@@ -84,6 +82,7 @@ int main(int argc, char** argv)
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
tracker.setKltOpencv(klt_settings);
+ tracker.setMaskBorder(5);
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
tracker.setAngleAppear( vpMath::rad(70) );
@@ -93,6 +92,7 @@ int main(int argc, char** argv)
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
}
tracker.setOgreVisibilityTest(true);
+ tracker.setOgreShowConfigDialog(false);
tracker.loadModel(objectname + ".cao");
tracker.setDisplayFeatures(true);
tracker.initClick(I, objectname + ".init", true);
@@ -116,13 +116,18 @@ int main(int argc, char** argv)
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
}
catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
}
+#ifdef VISP_HAVE_OGRE
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ }
+#endif
#else
(void)argc;
(void)argv;
diff --git a/tutorial/tracking/model-based/keypoint/CMakeLists.txt b/tutorial/tracking/model-based/keypoint/CMakeLists.txt
index 9723f38..e58383e 100644
--- a/tutorial/tracking/model-based/keypoint/CMakeLists.txt
+++ b/tutorial/tracking/model-based/keypoint/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-mb-keypoint)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_mbt visp_klt visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp b/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
index b2620b6..55e6ce8 100644
--- a/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
+++ b/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
@@ -1,11 +1,11 @@
/*! \example tutorial-mb-klt-tracker.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpIoTools.h>
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/mbt/vpMbKltTracker.h>
+#include <visp3/io/vpVideoReader.h>
int main(int argc, char** argv)
{
@@ -89,6 +89,7 @@ int main(int argc, char** argv)
//! [Set parameters]
}
tracker.setOgreVisibilityTest(true);
+ tracker.setOgreShowConfigDialog(false);
tracker.loadModel(objectname + "-triangle.cao");
tracker.setDisplayFeatures(true);
tracker.initClick(I, objectname + ".init", true);
@@ -112,13 +113,18 @@ int main(int argc, char** argv)
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
-#if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
}
catch(vpException e) {
- std::cout << "Catch an exception: " << e << std::endl;
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
}
+#ifdef VISP_HAVE_OGRE
+ catch(Ogre::Exception e) {
+ std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
+ }
+#endif
#else
(void)argc;
(void)argv;
diff --git a/tutorial/tracking/moving-edges/CMakeLists.txt b/tutorial/tracking/moving-edges/CMakeLists.txt
index 590ef19..b9d4ea9 100644
--- a/tutorial/tracking/moving-edges/CMakeLists.txt
+++ b/tutorial/tracking/moving-edges/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-me)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_me visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp b/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
index 38d5bc1..22dce75 100644
--- a/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
+++ b/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
@@ -1,27 +1,42 @@
/*! \example tutorial-me-ellipse-tracker.cpp */
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpMeEllipse.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/me/vpMeEllipse.h>
int main()
{
-#if (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
try {
vpImage<unsigned char> I;
-#if defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g(false);
#elif defined(VISP_HAVE_CMU1394)
vp1394CMUGrabber g;
#elif defined(VISP_HAVE_V4L2)
vpV4l2Grabber g;
+#elif defined(VISP_HAVE_OPENCV)
+ cv::VideoCapture g(0); // open the default camera
+ if(!g.isOpened()) { // check if we succeeded
+ std::cout << "Failed to open the camera" << std::endl;
+ return -1;
+ }
+ cv::Mat frame;
#endif
- g.open(I);
+
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
+#elif defined(VISP_HAVE_OPENCV)
+ g >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
+#endif
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 0, 0, "Camera view");
@@ -47,7 +62,12 @@ int main()
ellipse.initTracking(I);
while(1) {
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
+#elif defined(VISP_HAVE_OPENCV)
+ g >> frame;
+ vpImageConvert::convert(frame, I);
+#endif
vpDisplay::display(I);
ellipse.track(I);
ellipse.display(I, vpColor::red);
diff --git a/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp b/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
index 5c6657e..783192b 100644
--- a/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
+++ b/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
@@ -1,24 +1,36 @@
/*! \example tutorial-me-line-tracker.cpp */
-#include <visp/vp1394CMUGrabber.h>
-#include <visp/vp1394TwoGrabber.h>
-#include <visp/vpV4l2Grabber.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpMeLine.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_SENSOR
+#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/me/vpMeLine.h>
int main()
{
-#if (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2))
try {
vpImage<unsigned char> I;
-#if defined(VISP_HAVE_DC1394_2)
+#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g(false);
#elif defined(VISP_HAVE_CMU1394)
vp1394CMUGrabber g;
#elif defined(VISP_HAVE_V4L2)
vpV4l2Grabber g;
+#elif defined(VISP_HAVE_OPENCV)
+ cv::VideoCapture g(0); // open the default camera
+ if(!g.isOpened()) { // check if we succeeded
+ std::cout << "Failed to open the camera" << std::endl;
+ return -1;
+ }
+ cv::Mat frame;
+ g >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
#endif
g.open(I);
g.acquire(I);
@@ -47,7 +59,12 @@ int main()
line.initTracking(I);
while(1) {
+#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
+#elif defined(VISP_HAVE_OPENCV)
+ g >> frame;
+ vpImageConvert::convert(frame, I);
+#endif
vpDisplay::display(I);
line.track(I);
line.display(I, vpColor::red);
diff --git a/tutorial/tracking/template-tracker/CMakeLists.txt b/tutorial/tracking/template-tracker/CMakeLists.txt
index d2b6957..68f470a 100644
--- a/tutorial/tracking/template-tracker/CMakeLists.txt
+++ b/tutorial/tracking/template-tracker/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-tracking-template)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_tt visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp b/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
index 921e562..63d64ed 100644
--- a/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
+++ b/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
@@ -1,11 +1,11 @@
/*! \example tutorial-template-tracker.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpVideoReader.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
-#include <visp/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp/vpTemplateTrackerWarpHomography.h>
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
//! [Include]
int main(int argc, char** argv)
diff --git a/tutorial/visual-servo/ibvs/CMakeLists.txt b/tutorial/visual-servo/ibvs/CMakeLists.txt
index 06edb2c..3a71092 100644
--- a/tutorial/visual-servo/ibvs/CMakeLists.txt
+++ b/tutorial/visual-servo/ibvs/CMakeLists.txt
@@ -2,7 +2,7 @@ project(tutorial-visual-servo-ibvs)
cmake_minimum_required(VERSION 2.6)
-find_package(VISP REQUIRED)
+find_package(VISP REQUIRED visp_core visp_robot visp_vs visp_blob visp_vision visp_io visp_gui)
# set the list of source files
set(tutorial_cpp
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
index ccae30d..1f29b19 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
@@ -1,12 +1,12 @@
/*! \example tutorial-ibvs-4pts-display.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServoDisplay.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/vs/vpServoDisplay.h>
void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
@@ -36,11 +36,11 @@ int main()
vpHomogeneousMatrix cMo(0.15, -0.1, 1.,
vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1, 0.1, 0) );
+ point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
index 09ac68f..0359ce9 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
@@ -1,13 +1,13 @@
/*! \example tutorial-ibvs-4pts-image-tracking.cpp */
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpImageSimulator.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot);
@@ -90,11 +90,11 @@ int main()
vpImage<unsigned char> I(480, 640, 255);
vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1, 0.1, 0) );
+ point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
index 2253fb2..8ce1213 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
@@ -1,13 +1,17 @@
/*! \example tutorial-ibvs-4pts-ogre-tracking.cpp */
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpAROgre.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpPose.h>
-#include <visp/vpServo.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_AR
+#include <visp3/ar/vpAROgre.h>
+#endif
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/blob/vpDot2.h>
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot, unsigned int thickness);
#if defined(VISP_HAVE_OGRE)
@@ -58,11 +62,11 @@ int main()
vpCameraParameters cam(840, 840, background.getWidth()/2, background.getHeight()/2);
// Define the target as 4 points
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1, 0.1, 0) );
+ point.push_back( vpPoint(-0.1, 0.1, 0) );
// Our object
// A simulator with the camera parameters defined above,
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
index 0328d7a..37f3b35 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
@@ -1,9 +1,11 @@
/*! \example tutorial-ibvs-4pts-ogre.cpp */
-
-#include <visp/vpAROgre.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_MODULE_AR
+#include <visp3/ar/vpAROgre.h>
+#endif
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
int main()
{
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
index beef4d0..30ba49f 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpPlot.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/gui/vpPlot.h>
int main()
{
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
index d3d0a7b..7789b61 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter-gain-adaptive.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpPlot.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/gui/vpPlot.h>
int main()
{
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
index df01839..fad7a47 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpPlot.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/gui/vpPlot.h>
int main()
{
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
index 5378468..ecd0689 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
@@ -1,13 +1,13 @@
/*! \example tutorial-ibvs-4pts-wireframe-camera.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpProjectionDisplay.h>
-#include <visp/vpServoDisplay.h>
-#include <visp/vpWireFrameSimulator.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
index 016b07c..8039778 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
@@ -1,10 +1,10 @@
/*! \example tutorial-ibvs-4pts-wireframe-robot-afma6.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorAfma6.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
@@ -60,11 +60,11 @@ int main()
*/
vpHomogeneousMatrix wMo(0, 0, 1., 0, 0, 0);
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1, 0.1, 0) );
+ point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
index adb8ade..3cfd6ab 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
@@ -1,10 +1,10 @@
/*! \example tutorial-ibvs-4pts-wireframe-robot-viper.cpp */
-#include <visp/vpDisplayGDI.h>
-#include <visp/vpDisplayOpenCV.h>
-#include <visp/vpDisplayX.h>
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorViper850.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorViper850.h>
void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
@@ -60,11 +60,11 @@ int main()
vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
- point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ std::vector<vpPoint> point;
+ point.push_back( vpPoint(-0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1,-0.1, 0) );
+ point.push_back( vpPoint( 0.1, 0.1, 0) );
+ point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
index 1d676a7..b041086 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
@@ -1,7 +1,7 @@
/*! \example tutorial-ibvs-4pts.cpp */
-#include <visp/vpFeatureBuilder.h>
-#include <visp/vpServo.h>
-#include <visp/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/robot/vpSimulatorCamera.h>
int main()
{
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/visp.git
More information about the debian-science-commits
mailing list